无人驾驶车辆模型预测控制第五章实际行驶轨迹为直线问题解决

无人驾驶车辆模型预测控制第五章实际行驶轨迹为直线问题解决

看了大约两个下午的S函数代码，感觉大体上是没有什么问题的

注意仿真时间的设置与CarSim中一致

T_all=30;%总的仿真时间，主要功能是防止计算期望轨迹越界

预测时域、控制时域、仿真步长等参数设置不恰当会报索引超出数组元素的数目

T=0.05;%仿真步长

Np=20;%预测步长
Nc=5;%控制步长
Row=1000;%松弛因子权重

将控制量与控制量增量的约束放宽

% umin=-0.1744;%维数与控制变量的个数相同
% umax=0.1744;
% delta_umin=-0.0148*0.4;
% delta_umax=0.0148*0.4;
umin=-0.5;%维数与控制变量的个数相同
umax=0.5;
delta_umin=-0.05;
delta_umax=0.05;
Umin=kron(ones(Nc,1),umin);
Umax=kron(ones(Nc,1),umax);

取消输出量约束

%输出量约束
% ycmax=[0.21;5];
% ycmin=[-0.3;-3];
% Ycmax=kron(ones(Np,1),ycmax);
% Ycmin=kron(ones(Np,1),ycmin);

%结合控制量约束和输出量约束
% A_cons_cell={A_I zeros(Nu*Nc,1);-A_I zeros(Nu*Nc,1);THETA zeros(Ny*Np,1);-THETA zeros(Ny*Np,1)};
% b_cons_cell={Umax-Ut;-Umin+Ut;Ycmax-PSI*kesi-GAMMA*PHI;-Ycmin+PSI*kesi+GAMMA*PHI};
A_cons_cell={
    
    A_I zeros(Nu*Nc,1);-A_I zeros(Nu*Nc,1)};
b_cons_cell={
    
    Umax-Ut;-Umin+Ut};
A_cons=cell2mat(A_cons_cell);%（求解方程）状态量不等式约束增益矩阵，转换为绝对值的取值范围
b_cons=cell2mat(b_cons_cell);%（求解方程）状态量不等式约束的取值

下面是最关键的部分，原模型输入给CarSim四个车轮的转角，但MPC的输出量只是单轨模型的前轮转角，同时输入给左前轮和右前轮，显然是不合理的

更改调整后如下

编写绘制参考轨迹与实际轨迹的函数

shape=2.4;%参数名称，用于参考轨迹生成
dx1=25;dx2=21.95;%没有任何实际意义，只是参数名称
dy1=4.05;dy2=5.7;%没有任何实际意义，只是参数名称
Xs1=27.19;Xs2=56.46;%参数名称
X_phi=1:1:220;%这个点的区间是根据纵向速度（x_dot）来定的，如果速度为10m/s则区间=10*0.1=1
z1=shape/dx1*(X_phi-Xs1)-shape/2;
z2=shape/dx2*(X_phi-Xs2)-shape/2;
Y_ref=dy1/2.*(1+tanh(z1))-dy2/2.*(1+tanh(z2));
figure(1);
plot(X_phi, Y_ref,'r--','LineWidth',2);
hold on;
plot(y.signals.values(:,6),y.signals.values(:,5),'g','LineWidth',2);
legend('参考轨迹', '实际轨迹');
hold off;

仿真结果如下