无人驾驶轨迹跟踪仿真——线性时变模型预测代码详解
对照推导的公式，对代码进行一一详细注解，方便学习代码的同学。该代码为龚建伟《无人驾驶车辆模型预测控制》中第3章3.3.3的例子。
1 公式推导
这一部分的推导过程，比我上一篇文章的推导较简单一些，主要表现在三个方面：第1是控制量由前轮转角δ变成了角速度ω，第2是没有对状态量和控制量进行组合，第3是代价函数没有添加松弛变量。
具体的推导过程，我放上了我的手稿。
推导公式中的矩阵形式，以及最终的二次规划形式在后边的代码中都有所体现，我会在代码部分作出解释。
2 代码部分
代码共分为了5部分。
2.1 参考轨迹生成
参考轨迹为Y=2的一条直线
N=100; %参考轨迹点的数量
T=0.05; %采样周期
Xout=zeros(N,3); %生成N行3列的0矩阵，为参考点矩阵
Tout=zeros(N,1); %生成N行1列的0矩阵
for k=1:1:N
Xout(k,1)=k*T; %第1列表示x值
Xout(k,2)=2; %第2列表示y值
Xout(k,3)=0; %第3列表示φ值
Tout(k,1)=(k-1)*T; %为时刻矩阵
end
2.2 常量参数赋值
Nx=3; %状态量个数
Nu =2; %控制量个数
Tsim =20; %文中注解仿真时间，我认为是预测时域Np
X0 = [0 0 pi/3]; %初始位置的状态
[Nr,Nc] = size(Xout); %取Xout矩阵的行数和列数分别赋值给Nr和Nc
L = 1; %车辆轴距
Rr = 1; %轮胎直径
w = 1; %车轮转速
vd1 = Rr*w; %参考系统的纵向速度
vd2 = 0; %参考系统的角速度
2.3 定义矩阵
x_real=zeros(Nr,Nc); %状态量矩阵
x_piao=zeros(Nr,Nc); %状态量误差矩阵
u_real=zeros(Nr,2); %控制量矩阵
u_piao=zeros(Nr,2); %控制量误差矩阵
x_real(1,:)=X0; %把初始状态赋值给状态量第一行
x_piao(1,:)=x_real(1,:)-Xout(1,:); %计算第一个状态量误差
X_PIAO=zeros(Nr,Nx*Tsim);
XXX=zeros(Nr,Nx*Tsim); %用于保持每个时刻预测的所有状态值
q=[1 0 0;0 1 0;0 0 0.5]; %状态量的权重系数
Q_cell=cell(Tsim,Tsim);
for i=1:1:Tsim
for j=1:1:Tsim
if i==j
Q_cell{i,j}=q;
else
Q_cell{i,j}=zeros(Nx,Nx);
end
end
end
Q=cell2mat(Q_cell); %将多个矩阵合并成一个矩阵，得到权重系数矩阵Q
R=0.1*eye(Nu*Tsim,Nu*Tsim); %权重系数矩阵R
2.4 MPC的预测和优化
for i=1:1:Nr
t_d =Xout(i,3); %取航向角φ
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;
0 T;]; %控制量系数矩阵
A_cell=cell(Tsim,1);
B_cell=cell(Tsim,Tsim);
for j=1:1:Tsim
A_cell{j,1}=a^j; %A_cell为 ψ 矩阵的分量
for k=1:1:Tsim
if k<=j
B_cell{j,k}=(a^(j-k))*b; %B_cell为 θ 矩阵的分量
else
B_cell{j,k}=zeros(Nx,Nu);
end
end
end
A=cell2mat(A_cell); %合并A_cell生成 ψ 矩阵
B=cell2mat(B_cell); %合并B_cell生成 θ 矩阵
H=2*(B'*Q*B+R); %二次规划中的H
f=(2*x_piao(i,:)*A'*Q*B)'; %二次规划中的f
%% 约束条件
A_cons=[]; %不等式约束的系数矩阵
b_cons=[]; %不等式约束的值
lb=[-1;-1]; %自变量约束下界
ub=[1;1]; %自变量约束上界
tic %用来保存当前时间
[X,fval(i,1),exitflag(i,1),output(i,1)]=quadprog(H,f,A_cons,b_cons,[],[],lb,ub); %%二次规划函数求解
toc %用来记录程序完成时间
X_PIAO(i,:)=(A*x_piao(i,:)'+B*X)'; %输出矩阵Y
%% 输出每个时刻预测的所有的状态量
%% i表示时刻k，预测时域值为20，则当时刻k=80开始后，预测到的时刻要超100了，所以之后都取100时刻的值。
if i+j<Nr
for j=1:1:Tsim
XXX(i,1+3*(j-1))=X_PIAO(i,1+3*(j-1))+Xout(i+j,1);
XXX(i,2+3*(j-1))=X_PIAO(i,2+3*(j-1))+Xout(i+j,2);
XXX(i,3+3*(j-1))=X_PIAO(i,3+3*(j-1))+Xout(i+j,3);
end
else
for j=1:1:Tsim
XXX(i,1+3*(j-1))=X_PIAO(i,1+3*(j-1))+Xout(Nr,1);
XXX(i,2+3*(j-1))=X_PIAO(i,2+3*(j-1))+Xout(Nr,2);
XXX(i,3+3*(j-1))=X_PIAO(i,3+3*(j-1))+Xout(Nr,3);
end
end
u_piao(i,1)=X(1,1); %将二次规划的第一个解的x值赋值给控制量矩阵当前时刻x值
u_piao(i,2)=X(2,1); %将二次规划的第一个解的y值赋值给控制量矩阵当前时刻y值
Tvec=[0:0.05:4];
X00=x_real(i,:); %将当前时刻的状态量赋值给X00
vd11=vd1+u_piao(i,1); %当前时刻的纵向速度
vd22=vd2+u_piao(i,2); %当前时刻的角速度
%% 基于控制量计算下一时刻的状态量
XOUT=dsolve('Dx-vd11*cos(z)=0','Dy-vd11*sin(z)=0','Dz-vd22=0','x(0)=X00(1)','y(0)=X00(2)','z(0)=X00(3)');
t=T;
%% 记录下一时刻的状态量
x_real(i+1,1)=eval(XOUT.x);
x_real(i+1,2)=eval(XOUT.y);
x_real(i+1,3)=eval(XOUT.z);
if(i<Nr)
x_piao(i+1,:)=x_real(i+1,:)-Xout(i+1,:); %计算下一时刻的误差
end
u_real(i,1)=vd1+u_piao(i,1);
u_real(i,2)=vd2+u_piao(i,2); %当前时刻的控制量矩阵
figure(1);
plot(Xout(1:Nr,1),Xout(1:Nr,2)); %作图参考轨迹点x-y
hold on;
plot(x_real(i,1),x_real(i,2),'r*'); %作图状态量x-y
title('跟踪结果对比');
xlabel('横向位置X');
axis([-1 5 -1 3]);
ylabel('纵向位置Y');
hold on;
for k=1:1:Tsim
X(i,k+1)=XXX(i,1+3*(k-1)); %每个时刻的预测值x
Y(i,k+1)=XXX(i,2+3*(k-1)); %每个时刻的预测值y
end
X(i,1)=x_real(i,1);
Y(i,1)=x_real(i,2);
plot(X(i,:),Y(i,:),'y') %所有时刻的预测值x-y
hold on;
end
注：代码每一个x_piao的分量在形式上是与推导中是转置的关系，所以在二次规划表达式中 f 的表达式可以看出来。如图1所示：
2.5 绘图
figure(2)
subplot(3,1,1);
plot(Tout(1:Nr),Xout(1:Nr,1),'k--'); %时刻-参考点 x 值
hold on;
plot(Tout(1:Nr),x_real(1:Nr,1),'k'); %时刻-状态量 x 值
%grid on;
%title('状态量-横向坐标X对比');
xlabel('采样时间T');
ylabel('横向位置X')
subplot(3,1,2);
plot(Tout(1:Nr),Xout(1:Nr,2),'k--'); %时刻-参考点 y 值
hold on;
plot(Tout(1:Nr),x_real(1:Nr,2),'k'); %时刻-状态量 y 值
%grid on;
%title('状态量-横向坐标Y对比');
xlabel('采样时间T');
ylabel('纵向位置Y')
subplot(3,1,3);
plot(Tout(1:Nr),Xout(1:Nr,3),'k--'); %时刻-参考点 φ 值
hold on;
plot(Tout(1:Nr),x_real(1:Nr,3),'k'); %时刻-状态量 φ 值
%grid on;
hold on;
%title('状态量-\theta对比');
xlabel('采样时间T');
ylabel('\theta')
figure(3)
subplot(2,1,1);
plot(Tout(1:Nr),u_real(1:Nr,1),'k'); %时刻-控制量 v 值
%grid on;
%title('控制量-纵向速度v对比');
xlabel('采样时间T');
ylabel('纵向速度')
subplot(2,1,2)
plot(Tout(1:Nr),u_real(1:Nr,2),'k'); %时刻-控制量 w 值
%grid on;
%title('控制量-角加速度对比');
xlabel('采样时间T');
ylabel('角加速度')
figure(4)
subplot(3,1,1);
plot(Tout(1:Nr),x_piao(1:Nr,1),'k'); %时刻-控制量误差 x 值
%grid on;
xlabel('采样时间T');
ylabel('e(x)');
subplot(3,1,2);
plot(Tout(1:Nr),x_piao(1:Nr,2),'k'); %时刻-控制量误差 y 值
%grid on;
xlabel('采样时间T');
ylabel('e(y)');
subplot(3,1,3);
plot(Tout(1:Nr),x_piao(1:Nr,3),'k'); %时刻-控制量误差 φ 值
%grid on;
xlabel('采样时间T');
ylabel('e(\theta)');
所有图如下所示：
图2
图3
图4
本文主要对文中的代码做了较为详细的注释，并针对代码，先对公式进行了推导，其中公式中的矩阵和代码中是一一对应的，在学习代码时，需要仔细对照，希望对各位学习MPC仿真的同学有用，谢谢~
参考1：龚建伟《无人驾驶车辆模型预测控制》
评论（0）
您还未登录，请登录后发表或查看评论