【Carsim Simulink自动驾驶仿真】基于MPC的轨迹跟踪控制
如果对Carsim的基础使用还不了解，可以参考：【Carsim Simulink自动驾驶仿真】基于MPC的速度控制
如果对MPC算法原理不清楚，可以参考：如何理解MPC模型预测控制理论
项目介绍：
教程为北理工的无人驾驶车辆模型预测控制第2版。所用的仿真软件为Carsim2020.0和MatlabR2021a。使用MPC控制思想对车辆进行轨迹跟踪控制，并给出仿真结果。
整整弄了两天，踩了无数的坑，所以篇幅比较大，如果还有什么其他问题，欢迎一起讨论。
从网上下载下来的代码运行不了，所以本文的代码是经过修改和调试的。
基于MPC的轨迹跟踪控制
- 效果图
-
- 直线跟踪
-
- 3m/s
- 5m/s
- 10m/s
- 圆跟踪
-
- 3m/s
- 5m/s
- 10m/s
- MATLAB框架搭建
- 代码的数学遗漏问题与修改
-
- 运动学模型少L
- 改写之后的目标函数线性项少系数
- 代码中的其他问题及讨论
-
- 输入维度不匹配的情况
- 索引超出数组范围
-
- 1.变量的范围
- 2.换一个求解器
- 3.初始值全部设为0
- 4.对于kesi(4)和kesi(5)的赋值 （这个是我的问题）
-
- 一些理解
- 参数的选取
- 速度问题
- 附录：全代码(修改过后的)
效果图
因为参数经过了调整，所以效果上和书上不一样。
直线跟踪
图像为：路径，速度，误差和前轮偏角
前轮偏角的局部放大图为：
3m/s
5m/s
10m/s
速度的具体图像为：
圆跟踪
图像为：路径，速度和前轮偏角(误差不知道该在Carsim中怎么画，知道的大佬请私信，呜呜呜)：
轨迹放大之后：
前轮偏角为：
前轮偏角放大之后：
3m/s
速度为：
5m/s
速度为：
10m/s
速度为：
MATLAB框架搭建
书中给出的MATLAB框架如下：
因为对于这种方法输出需要再另外处理。所以我这里直接将速度乘3.6，角度也在程序中直接处理了。对应的框架图为：
代码的数学遗漏问题与修改
书中的代码和下载下来的代码不一样，所以还是检查一下这两个地方，
运动学模型少L
在书中，推导运动学模型的时候，最后一个数字分母是有一个l的：
但是在代码中，这个地方少了，需要加上。
改写之后的目标函数线性项少系数
这里的2在书中没有少，但是下载下来的代码中没有这个系数。如果没有的可以加一下。
代码中的其他问题及讨论
输入维度不匹配的情况
看一下S-Function的输入是3个还是5个，输出是2个还是直接给出5个，对应的代码中也应该和其一样。具体在S-Function中是初始化的第一个函数中的：
sizes.NumOutputs =5;%[speed,steering]
sizes.NumInputs =3;
索引超出数组范围
这个是我的主要问题，之所以报错，是因为求解器求解不出来优化问题，方法的话我找了一些，然后自己也试了一些：
1.变量的范围
这里有两种情况：
- 第一种情况是检查变量范围
delta_umin=[-0.05;-0.0082;];如果当中的-0.05是0.05，将其加一个负号。 - 第二种情况其实扩大变量的范围可以求解，但是这里不建议，如果你在扩大范围之后可以求解，那么建议还是将其变回原来的数，更改其他值。
2.换一个求解器
使用interior-point-convex，而不是active-set
%options = optimset('Algorithm','active-set');
options = optimset('Algorithm','interior-point-convex');
3.初始值全部设为0
检查初始化函数mdlInitializeSizes中的x0和U是否都为零。注意，这里的U表示的就是控制量与参考控制的差值。
function [sys,x0,str,ts] = mdlInitializeSizes
%找到以下两个值
x0 = [0;0;0];
U=[0;0];
4.对于kesi(4)和kesi(5)的赋值 （这个是我的问题）
这个我认为争议比较大，但是改了之后确实没有报错了。
先说一下，按照理论：
kesi(4)和kesi(5)就是这里的控制变量 u ~ \tilde{u} u~。而这个 u ~ \tilde{u} u~根据定义是：
是当前速度与参考速度的插值，所以这里我们应该将Carsim中的5个变量全部输入到S-Funciton中，然后进行如下操作：
U(1) = u(4)/3.6 - vd1;
steer_SW = u(5)*pi/180;
steering_angle = steer_SW/18;
U(2) = steering_angle - vd2;
kesi(4)=U(1); % vel-vel_ref
kesi(5)=U(2); % steer_angle - steering_ref
之前我从来没有怀疑过这里，直到我看到书中的代码和网上下载的不一样的时候，我就尝试了一下书上的代码，即将前四行全部注释掉：
% U(1) = u(4)/3.6 - vd1;
% steer_SW = u(5)*pi/180;
% steering_angle = steer_SW/18;
% U(2) = steering_angle - vd2;
kesi(4)=U(1); % vel-vel_ref
kesi(5)=U(2); % steer_angle - steering_ref
这样改之后我的代码就不会出现问题了。
一些理解
首先我们要清楚，kesi(4,5)是控制减去参考控制，求解器解出来的X是前后两次的控制差值。
我们先看能跑对的程序，大致过程可以简化成设置值，求解，给出输出三步：
U = [0,0]
for 循环：
#第一步：设置kesi
kesi(4)=U(1); % vel-vel_ref
kesi(5)=U(2); % steer_angle - steering_ref
#第二步：求解
[X,~,~]=quadprog(H,f,A_cons,b_cons,[],[],lb,ub,[],options);
#第三步：给出输出
u_dot(1)=X(1);
u_dot(2)=X(2);
U(1)=kesi(4)+u_dot(1);%用于存储上一个时刻的控制量
U(2)=kesi(5)+u_dot(2);
u_real(1) = U(1) + vd1;
u_real(2) = U(2) + vd2;
这里列一个表格：
| 时间 | 求解之前的U，也就是上一步的U | kesi | u_dot | 求解之后的U | 输出 |
|---|---|---|---|---|---|
| k-1 | u k − 1 − u k − 1 r e f u_{k-1}-u_{k-1}^{ref} uk−1−uk−1ref | u k − 1 − u k − 1 r e f u_{k-1}-u_{k-1}^{ref} uk−1−uk−1ref | u k − u k r e f − u k − 1 + u k − 1 r e f u_k-u_{k}^{ref}-u_{k-1}+u_{k-1}^{ref} uk−ukref−uk−1+uk−1ref | u k − 1 − u k − 1 r e f + u k − u k r e f − u k − 1 + u k − 1 r e f = u k − u k r e f u_{k-1}-u_{k-1}^{ref}+u_k-u_{k}^{ref}-u_{k-1}+u_{k-1}^{ref}=u_k-u_{k}^{ref} uk−1−uk−1ref+uk−ukref−uk−1+uk−1ref=uk−ukref | u_k |
| 之后就可以征程循环。注意这里的初始值设置为[0,0]是合理的。 |
我出错的程序：
U = [0,0]
for 循环：
#第一步：设置kesi
U(1) = u(4)/3.6 - vd1;
steer_SW = u(5)*pi/180;
steering_angle = steer_SW/18;
U(2) = steering_angle - vd2;
kesi(4)=U(1); % vel-vel_ref
kesi(5)=U(2); % steer_angle - steering_ref
#第二步：求解
[X,~,~]=quadprog(H,f,A_cons,b_cons,[],[],lb,ub,[],options);
#第三步：给出输出
u_dot(1)=X(1);
u_dot(2)=X(2);
U(1)=kesi(4)+u_dot(1);%用于存储上一个时刻的控制量
U(2)=kesi(5)+u_dot(2);
u_real(1) = U(1) + vd1;
u_real(2) = U(2) + vd2;
建立同样的表格，这里唯一不一样的是，之前的输入其实是记忆到了程序里面，这里的输入变成了从程序中获取：
| 时间 | 求解之前的U，也就是上一步的U | kesi | u_dot | 求解之后的U | 输出 |
|---|---|---|---|---|---|
| k-1 | u k − 1 i n p u t − u k − 1 r e f u_{k-1}^{input}-u_{k-1}^{ref} uk−1input−uk−1ref | u k − 1 i n p u t − u k − 1 r e f u_{k-1}^{input}-u_{k-1}^{ref} uk−1input−uk−1ref | u k − u k r e f − u k − 1 i n p u t + u k − 1 r e f u_k-u_{k}^{ref}-u_{k-1}^{input}+u_{k-1}^{ref} uk−ukref−uk−1input+uk−1ref | u k − u k r e f u_k-u_{k}^{ref} uk−ukref | u_k |
| k | u k i n p u t − u k r e f u_{k}^{input}-u_{k}^{ref} ukinput−ukref | ||||
| 但是按照道理来说，这个和上一个表格是一样的，这里不一样的是速度输入之后需要进行变换，输出的时候也要进行变换，这变换之后会存在的误差比较大。还有一种可能，这里其实输入进来的是速度和方向盘转角，这个方向盘转角和轮胎转角之间有一个系数，这个系数这里设置的18，但是具体设置多少，也没有具体的深究。 | |||||
| 总之我个人感觉可能是上一时刻的输出 u k u_k uk和这一时刻的输入 u k i n p u t u_k^{input} ukinput可能不一样。希望各位大佬也给出一些解释。 |
参数的选取
参数这里有4个，预测步长，控制步长，Q和R。这里Q和R都是对角阵，表格中的数值是对角阵中的数，而不是Q和R的实际值。
这里我的参数为：
| 预测步长( N p N_p Np) | 控制步长(N_c) | Q | R |
|---|---|---|---|
| 100 | 30 | 1 | 100 |
我先调整的Q和R，当轨迹不贴合的时候，调大Q,当速度收敛太慢，震荡太久的时候调整R。根据我的经验Q取1就好，之后就根据速度图像的震荡和你的评判标准进行选择就好。
对于预测步长和控制步长，预测的越久，说明这个智能体越有远见，超调就会相对小一点，个人感觉控制步长太多不是什么好的事情，因为本身希望汽车进行相对较少的控制。
速度问题
这里有一个小问题，书中的速度在初始时刻是没有减速的：
而在我做的过程中，速度在一开始都有减少：
这里我找到了问题，是两个前轮的速度在一开始不一样，但是按照道理，输入都是一样的，出现这样的差异不知道怎么解决。
附录：全代码(修改过后的)
function [sys,x0,str,ts] = MPCtracking(t,x,u,flag)
%控制量为速度和前轮偏角，模型是用运动学模型
switch flag
case 0
[sys,x0,str,ts]=mdlInitializeSizes;
case 2
sys = mdlUpdates(t,x,u);
case 3
sys = mdlOutputs(t,x,u);
case {1,4,9}
sys = [];
otherwise
error(['unhandled flag =', num2str(flag)])
end
%End of MPCtracking
%==============================================================
% 初始化
%==============================================================
function [sys,x0,str,ts] = mdlInitializeSizes
sizes = simsizes;
sizes.NumContStates =0;
sizes.NumDiscStates =3;
sizes.NumOutputs =5;%[speed,steering]
sizes.NumInputs =3;
sizes.DirFeedthrough =1;
sizes.NumSampleTimes =1;
sys = simsizes(sizes);
x0 = [0;0;0];
global U;%存储目前的控制变量[vel,delta]
U=[0;0];
str = [];
ts = [0.05,0]; %采样时间：[period,offset]
%End of mdlInitializeSizes
%==============================================================
% 更新离散状态
%==============================================================
function sys = mdlUpdates(t,x,u)
sys = x;
%End of mdlUpdate.
%==============================================================
% 计算输出，核心输出
%==============================================================
function sys = mdlOutputs(t,x,u)
global a b u_dot;
global U;
global kesi;
tic
Nx = 3;%状态量的个数
Nu = 2;%控制量的个数
Np = 100;%预测步长
Nc = 30;%控制步长
Row = 10; %松弛因子
yaw_angle =u(3)*pi/180;%CarSim输出的Yaw angle为角度，角度转换为弧度
%==================================================
%最后调参
%==================================================
u_bound1 = 0.2;
u_bound2 = 0.436;
delta1 = 0.05;
delta2 = 0.0082;
Q_a = 1;
R_a = 100;
%==================================================
%构建路径
%==================================================
% %直线路径
% vd1 = 10;%ref_velocity
% vd2 = 0;%ref_steering
% r(1)=vd1*t; %ref_x
% r(2)=5;%ref_y
% r(3)=0;%ref_heading_angle
% %半径为35m的圆形轨迹, 圆心为(0, 35), 速度为3m/s
% r(1)=25*sin(0.12*t);
% r(2)=25+10-25*cos(0.12*t);
% r(3)=0.12*t;
% vd1=3;
% vd2=0.104;
%半径为25m的圆形轨迹, 圆心为(0, 35), 速度为10m/s
r(1)=25*sin(0.4*t);
r(2)=25+10-25*cos(0.4*t);
r(3)=0.4*t;
vd1=10;
vd2=0.104;
% %半径为25m的圆形轨迹, 圆心为(0, 35), 速度为5m/s
% r(1)=25*sin(0.2*t);
% r(2)=25+10-25*cos(0.2*t);
% r(3)=0.2*t;
% vd1=5;
% vd2=0.104;
%==================================================
%构建结束
%==================================================
%添加转化后的自变量kesi
kesi=zeros(Nx+Nu,1);
kesi(1) = u(1)-r(1);%u(1)==X(1),x_offset
kesi(2) = u(2)-r(2);%u(2)==X(2),y_offset
heading_offset = yaw_angle - r(3); %u(3)==X(3),heading_angle_offset
if (heading_offset < -pi)
heading_offset = heading_offset + 2*pi;
end
if (heading_offset > pi)
heading_offset = heading_offset - 2*pi;
end
kesi(3)=heading_offset;
% U(1) = u(4)/3.6 - vd1;
% steer_SW = u(5)*pi/180;
% steering_angle = steer_SW/18;
% U(2) = steering_angle - vd2;
kesi(4)=U(1); % vel-vel_ref
kesi(5)=U(2); % steer_angle - steering_ref
T = 0.05;
L = 2.6;%轮间距
%矩阵初始化
t_d = r(3);
%系数
Q = Q_a*eye(Nx*Np,Nx*Np);
% Q(Nx*Np,Nx*Np) = Q_a*0.01;
R = R_a*eye(Nu*Nc);
%运动学方程
u_dot = zeros(Nx,1);%%%%这里应该没有x
a = [1 0 -vd1*sin(t_d)*T;
0 1 vd1*cos(t_d)*T;
0 0 1;];
b = [cos(t_d)*T 0;
sin(t_d)*T 0;
tan(vd2)*T/L vd1*T/(L*(cos(vd2))^2);];
%构建新的状态空间
A_cell=cell(2,2);
B_cell=cell(2,1);
A_cell{1,1}=a;
A_cell{1,2}=b;
A_cell{2,1}=zeros(Nu,Nx);
A_cell{2,2}=eye(Nu);
B_cell{1,1}=b;
B_cell{2,1}=eye(Nu);
A=cell2mat(A_cell);
B=cell2mat(B_cell);
C=[ 1 0 0 0 0;
0 1 0 0 0;
0 0 1 0 0];
PHI_cell=cell(Np,1);
THETA_cell=cell(Np,Nc);
for j=1:1:Np
PHI_cell{j,1}=C*A^j;
for k=1:1:Nc
if k<=j
THETA_cell{j,k}=C*A^(j-k)*B;
else
THETA_cell{j,k}=zeros(Nx,Nu);
end
end
end
PHI=cell2mat(PHI_cell);%size(PHI)=[Nx*Np Nx+Nu]
THETA=cell2mat(THETA_cell);%size(THETA)=[Nx*Np Nu*(Nc+1)]
H_cell=cell(2,2);
H_cell{1,1}=THETA'*Q*THETA+R;
H_cell{1,2}=zeros(Nu*Nc,1);
H_cell{2,1}=zeros(1,Nu*Nc);
H_cell{2,2}=Row;
H=cell2mat(H_cell);
% H=(H+H')/2;
error=PHI*kesi;
f_cell=cell(1,2);
f_cell{1,1} = 2*(error'*Q*THETA);
f_cell{1,2} = 0;
f=cell2mat(f_cell);
%% 以下为约束生成区域
%不等式约束
A_t=zeros(Nc,Nc);%见falcone论文 P181
for p=1:1:Nc
for q=1:1:Nc
if q<=p
A_t(p,q)=1;
else
A_t(p,q)=0;
end
end
end
A_I=kron(A_t,eye(Nu));%对应于falcone论文约束处理的矩阵A,求克罗内克积
Ut=kron(ones(Nc,1), U);%
umin=[-u_bound1; -u_bound2];%[min_vel, min_steer]维数与控制变量的个数相同
umax=[u_bound1; u_bound2]; %[max_vel, max_steer],%0.436rad = 25deg
delta_umin = [-delta1; -delta2]; % 0.0082rad = 0.47deg
delta_umax = [delta1; delta2];
Umin=kron(ones(Nc,1),umin);
Umax=kron(ones(Nc,1),umax);
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);%（求解方程）状态量不等式约束的取值
% 状态量约束
delta_Umin = kron(ones(Nc,1),delta_umin);
delta_Umax = kron(ones(Nc,1),delta_umax);
lb = [delta_Umin; 0];%（求解方程）状态量下界
ub = [delta_Umax; 10];%（求解方程）状态量上界
%尝试初始值
%x0 = zeros(Nc*Nu+1,1);
%% 开始求解过程
%options = optimset('Algorithm','active-set');
options = optimset('Algorithm','interior-point-convex');
warning off all % close the warnings during computation
[X,~,~]=quadprog(H,f,A_cons,b_cons,[],[],lb,ub,[],options);
%% 计算输出
u_dot(1)=X(1);
u_dot(2)=X(2);
U(1)=kesi(4)+u_dot(1);%用于存储上一个时刻的控制量
U(2)=kesi(5)+u_dot(2);
u_real(1) = U(1) + vd1;
u_real(2) = U(2) + vd2;
sys= [u_real(1)*3.6; u_real(2)*180/pi; u_real(2)*180/pi;0;0]; % vel, steering, x, y
toc
% End of mdlOutputs.