0

目前我正在做模型来模拟 Simulink 中 2 辆汽车的超车机动，对于路径跟踪，我在 Simulink 中使用内置的非线性 MPC。它获取参考点作为输入。

非线性 MPC

为了构建参考点，我开发了 MATLAB 函数。这是代码：

function bezierPoints = buildBezier(egoX, egoY, egoV, targetX, targetY, targetV)

bezierPoints = zeros(3,1);
pts = [];
yaw = [0];
timeGap = (abs(egoX - targetX))/(egoV - targetV);

counter = 0;

if (egoX - targetX) < -1.5
   %counter = 1;

   P0 = [egoX ; egoY];
   P1 = P0;
   P2 = [egoX + 2 ; egoY];
   P3 = P2;

   t = linspace(0,1,80);
   yaw = linspace(0,0,80);
   pts = [kron((1-t).^3,P0) + kron(3*(1-t).^2.*t,P1) + kron(3*(1-t).*t.^2,P2) + kron(t.^3,P3), yaw'];
    
   counter = counter + 1;

elseif (egoX - targetX) > -1.5 && (egoX - targetX) <= 1
   %counter = 1;

   finalX = egoX + egoV*timeGap;
   intermediateX = (egoX + finalX)/2;
   P0 = [egoX ; egoY];
   P1 = [intermediateX ; -0.5];
   P2 = [intermediateX ; 0.5];
   P3 = [finalX ; 0.5];

   t = linspace(0,1,80);
   pts =  [kron((1-t).^3,P0) + kron(3*(1-t).^2.*t,P1) + kron(3*(1-t).*t.^2,P2) + kron(t.^3,P3)];
   for i=1:(size(pts,2)-1)
       yaw = [ yaw , atan((pts(2,i+1)-pts(2,i))/(pts(1,i+1)-pts(1,i)))];
   end
   pts = [pts, yaw'];

   counter = counter + 1;

elseif (egoX - targetX) > 1
   %counter = 1;

   finalX = egoX + egoV*timeGap;
   intermediateX = (egoX + finalX)/2;
   P0 = [egoX ; egoY];
   P1 = [intermediateX ; 0.5];
   P2 = [intermediateX ; -0.5];
   P3 = [finalX ; -0.5];

   t = linspace(0,1,80);
   pts =  [kron((1-t).^3,P0) + kron(3*(1-t).^2.*t,P1) + kron(3*(1-t).*t.^2,P2) + kron(t.^3,P3)];
   for i=1:(size(pts,2)-1)
       yaw = [yaw , atan((pts(2,i+1)-pts(2,i))/(pts(1,i+1)-pts(1,i)))];
   end 
   pts = [pts, yaw'];
    
   counter = counter + 1;
end
bezierPoints = pts(:,counter);  
end

我知道代码和系统本身存在一些问题非常“愚蠢”，但现在的主要问题是非线性 MPC 无法获得可变大小的信号（MATLAB 说它是可变大小的，但实际上大小总是相同的，因为我定义了这条线的点数t = linspace(0,1,80);）。Simulink 显示以下错误。

Simulink 中的错误

我尝试了几件事。我使变量bezierPoints保持不变，我尝试了从这里构建贝塞尔曲线的其他方法：如何将二次贝塞尔曲线代码转换为三次贝塞尔曲线？. 在上面的代码中，我使用了这里的方法：Bézier Curves and Kronecker's Tensor Product。我将变量的赋值移到bezierPoints函数的末尾（我认为可能由于向量大小不同，多个赋值会导致此错误发生）。但没有任何效果。

所以现在我将这个问题与我用于构建贝塞尔曲线的方法联系起来。有没有什么方法可以在模拟过程中将这条曲线构建为固定大小的数组？在此功能之前，我在模拟之前预定义了曲线，并使用带有时间参考的块“From Workspace”进行模拟，但现在我需要在模拟期间根据两辆车的位置和速度来构建它。

谢谢！

4

0 回答 0