使用非线性 MPC 在 MATLAB Simulink 中的仿真过程中构建贝塞尔曲线

问题描述

目前我正在做模型来模拟 Simulink 中 2 辆车的超车机动，对于路径跟随，我在 Simulink 中使用内置的非线性 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,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,80);
   pts =  [kron((1-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,2)-1)
       yaw = [yaw,i)))];
   end 
   pts = [pts,yaw'];
    
   counter = counter + 1;
end
bezierPoints = pts(:,counter);  
end

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

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

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

谢谢！

解决方法

暂无找到可以解决该程序问题的有效方法，小编努力寻找整理中！

如果你已经找到好的解决方法，欢迎将解决方案带上本链接一起发送给小编。

小编邮箱:dio#foxmail.com (将#修改为@）