目录
前言
一、yalmip简介
二、车辆模型
1.车辆运动学模型
2.离散化
3.线性化
三、MPC优化问题定义
四、Matlab代码
五、结果
总结
前言
在无人驾驶的运动控制中，模型预测控制（MPC）算法得到了广泛使用，龚建伟《无人驾驶车辆模型预测控制》一书对MPC算法进行了细致的讲解，并提供了源代码，非常值得参考和学习。但书中提及各矩阵（模型矩阵、约束矩阵）的计算推导非常复杂，短时间内难以理解，代码更改起来也容易报错。而采用yalmip工具可以直接添加约束和成本函数，在很大程度上简化代码，利于初学者对应理解MPC公式与代码，修改起来也较为容易。
一、yalmip简介
yalmip是由Lofberg开发的一种免费的优化求解工具。它是一个建模工具，甚至可以称为一种“语言”，通过这种“语言”来描述模型，然后再调用其他求解器（如quadprog、gurobi、fmincon等）来求解模型。其最大特色在于集成许多外部的优化求解器，形成一种统一的建模求解语言，提供了Matlab的调用API，减少学习者学习成本。
具体可以参考（原文链接：https://blog.csdn.net/s83625981/article/details/80076478），安装和学习使用yalmip。
二、车辆模型
1.车辆运动学模型
2.离散化
3.线性化
这里使用针对状态轨迹的线性化方法（《无人驾驶车辆模型预测控制》(第二版)第五章代码所使用的方法），与第三、四章的存在参考系统的性线化方法略有不同，本质上区别不大，具体可以参考《无人驾驶车辆模型预测控制》(第一版)的介绍。
若使用较复杂的模型，可借助jacobian函数求解雅可比矩阵A,B
1
2
3
4
5
6
7
8
9syms x y phi delta v L T %x：横坐标；y：纵坐标；phi:航向角；delta：前轮偏角； %v：速度；L：轴距；T：离散时间 kesi=[v*cos(phi);v*sin(phi);v*tan(delta)/L]*T+[x;y;phi];%离散化方程 X=[x,y,phi];%状态量 u=[v,delta];%控制量 A=jacobian(kesi,X) B=jacobian(kesi,u)
三、MPC优化问题定义
程序目标是对轨迹进行跟踪，设计成本函数第一项：状态量与参考轨迹误差的平方，第二项：控制量的平方。约束依次为初始状态约束，车辆运动学模型，控制量约束，控制增量约束。优化问题如下所示：
四、Matlab代码
本文编写的代码主要为了对标《无人驾驶车辆模型预测控制》（第二版）第四章的代码，主体参照S函数形式编写，可结合Carsim使用。以下主要介绍yalmip编写的MPC计算函数：
1.函数输入
A,B：模型（系统）矩阵；Q,R：权重矩阵；N：控制步长；kesi：当前状态量和控制量；state_k1：下一时刻状态量；umin,umax,delta_min,delta_max：控制量和控制增量约束矩阵；Ref： 参考轨迹；MPC_solver：求解器。
1function [ x, uPred ] = MPC_yalmip( A, B, Q, R, N, kesi, state_k1, umin, umax, delta_umin, delta_umax, Ref, MPC_solver)
2.定义变量
采用yalmip语言定义预测域的状态变量和控制变量，大小为3*(N+1)和2*N。
1
2x=sdpvar(size(A,2),N+1);%维度：[x;y;phi]*(N+1) u=sdpvar(size(B,2),N); %维度：[v;delta]*N
3.约束
约束部分参照第三部分依次设置：初始条件，动力学模型约束，控制量约束，控制增量约束。动力学模型约束参照之前线性化的等式来设定，需要事先计算ξ(k+1)和A、B矩阵。
采用yalmip语言，可以通过for循环、==、<=设定约束，极为便利，不需要去推导庞大的约束矩阵。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15Constraints = [ x(:,1)==kesi(1:3)]; %初始条件 for i = 1:N Constraints=[Constraints; %运动学模型约束 x(:,i+1) == A*(x(:,i)-kesi(1:3)) + B*(u(:,i)-kesi(4:5))+state_k1;%非线性 umin<=u(:,i)<=umax;%控制量约束 ]; end for i = 1:N-1 Constraints=[Constraints; %控制增量约束 delta_umin<=u(:,1)-kesi(4:5)<=delta_umax; delta_umin<=u(:,i)-u(:,i+1) <=delta_umax; ]; end
4.成本函数
同样通过for循环设置成本函数。
1
2
3
4
5
6
7Cost=0; for i=1:N Cost=Cost+(x(:,i+1)-Ref(:,i))'*Q*(x(:,i+1)-Ref(:,i)); end for i=1:N Cost=Cost+u(:,i)'*R*u(:,i); end
5.求解
使用yalmip语言调用求解器进行求解，最后输出所需的控制量。
1
2
3options = sdpsettings('verbose',1,'solver', MPC_solver); Problem = optimize(Constraints,Cost,options); uPred = double( u(:,1));
完整MPC求解函数如下：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34function [ x, uPred ] = MPC_yalmip( A, B, Q, R, N, kesi, state_k1, umin, umax, delta_umin, delta_umax, Ref, MPC_solver) yalmip('clear');%清理内存 % ===== 定义变量 ===== x=sdpvar(size(A,2),N+1);%维度：[x;y;phi]*(N+1) u=sdpvar(size(B,2),N); %维度：[v;delta]*N % ===== 设置约束 ===== Constraints = [ x(:,1)==kesi(1:3)]; %初始条件 for i = 1:N Constraints=[Constraints; %运动学模型约束 x(:,i+1) == A*(x(:,i)-kesi(1:3)) + B*(u(:,i)-kesi(4:5))+state_k1;%非线性 umin<=u(:,i)<=umax;%控制量约束 ]; end for i = 1:N-1 Constraints=[Constraints; %控制增量约束 delta_umin<=u(:,1)-kesi(4:5)<=delta_umax; delta_umin<=u(:,i)-u(:,i+1) <=delta_umax; ]; end % ===== 定义成本函数 ===== Cost=0; for i=1:N Cost=Cost+(x(:,i+1)-Ref(:,i))'*Q*(x(:,i+1)-Ref(:,i)); end for i=1:N Cost=Cost+u(:,i)'*R*u(:,i); end % ===== 求解 ===== options = sdpsettings('verbose',1,'solver', MPC_solver); Problem = optimize(Constraints,Cost,options); uPred = double( u(:,1)); end
6.完整代码
完整代码是对圆形轨迹进行跟踪，对被控车辆部分进行了简化，安装yalmip后可直接运行。
主函数部分先设定了车辆初始状态，然后调用子函数mdlOutputs（对应S函数的mdlOutputs部分）进行轨迹跟踪，mdlOutputs内部计算各矩阵之后，调用MPC_yalmip函数进行优化求解。mdlOutputs的输出会输入给被控车辆（这里使用离散化模型代替Carsim车辆），之后通过输入更新车辆状态，最后绘制x,y,phi的跟踪结果。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143clear;clc; %% 主函数 % ===== 参数 ===== global U T T=0.05;%离散时间 L=2.6;%轴距 T_all=30;%总时间 % ===== 初始化 ===== x0=0; y0=10;%10 phi0=0; v0=5; delta0=0; U=[v0;delta0]; % ===== 运行模型 ===== kesi_cell=cell(1,T_all/T+1);%状态量和控制量矩阵 i=1; kesi_cell{i}=[x0;y0;phi0;v0;delta0]; for t=0:T:T_all state_t=kesi_cell{i}(1:3); x=state_t(1);%横坐标 y=state_t(2);%纵坐标 phi=state_t(3);%航向角 sys = mdlOutputs(t,1,state_t);%MPC计算 v=sys(1); %车速 delta=sys(2); %前轮转角 %被控车辆：以离散化的运动学模型代替 i=i+1; state_k=[v*cos(phi);v*sin(phi);v*tan(delta)/L]*T+[x;y;phi]; kesi_cell{i}=[state_k;sys];%存储状态量和控制量 fprintf('时间：%.2fsn',t); end kesi=cell2mat(kesi_cell);%状态量矩阵[x;y;phi] %% ===== 绘图 ===== figure; subplot(1,2,1) hold on; plot(kesi(1,:),kesi(2,:),'-*');%绘制x,y坐标 tt=0:T:40; x_ref=25*sin(0.2*tt); y_ref=25+10-25*cos(0.2*tt); plot(x_ref,y_ref,'r'); xlabel('x/m'); ylabel('y/m'); legend('实际轨迹','参考轨迹') subplot(1,2,2); hold on; plot(0:T:T_all+T,kesi(3,:),'-*'); phi_ref=0.2*tt; plot(tt,phi_ref,'r'); xlabel('t/s'); ylabel('phi'); legend('实际轨迹','参考轨迹') %% ===== 子函数 ===== %% 模型计算输出 function sys = mdlOutputs(t,~,u) % ===== 车辆参数及输入 ===== global U T %控制量 离散时间 N=25;%控制步长 L=2.6;%轴距 %当前状态量：S函数输入为u x=u(1);%横坐标 y=u(2);%纵坐标 phi=u(3);%航向角 %控制量 v=U(1); %车速 delta=U(2); %前轮转角 kesi=[x,y,phi,v,delta]'; %下一时刻状态量 state_k1=[v*cos(phi);v*sin(phi);v*tan(delta)/L]*T+[x;y;phi]; % ===== 参考轨迹 ===== %圆形轨迹 Q=diag([100 100 10])*1; R=diag([1 1])*10;%控制增量(Nu,Nu) Ref_cell=cell(1,N); for p=1:1:N x_ref=25*sin(0.2*(T*p+t)); y_ref=25+10-25*cos(0.2*(T*p+t)); phi_ref=0.2*(T*p+t); % v_ref=5; % delta_ref=0.104; Ref_cell{1,p}=[x_ref;y_ref;phi_ref]; end Ref=cell2mat(Ref_cell); % ===== 求解器 ===== MPC_solver = 'quadprog'; % 'gurobi' or 'quadprog'. % ===== 系统矩阵A,B ===== A=[ 1, 0, -T*v*sin(phi); 0, 1, T*v*cos(phi); 0, 0, 1]; B=[ T*cos(phi), 0; T*sin(phi), 0; 0, T*v/cos(delta)^2 /L ]; % ===== 构建约束矩阵 ===== %控制量约束 umin=[4.8;-0.436];%-0.2<v-vd<0.2;-25°<delta<0.25° umax=[5.2; 0.436]; % 控制增量约束 delta_umin = [-0.05;-0.0082];%-0.05<Δv<0.05;-0.47°<Δdelta<0.47° delta_umax = [ 0.05; 0.0082]; % ===== MPC ===== [~, uPred ] = MPC_yalmip( A, B, Q, R, N, kesi, state_k1, umin, umax, delta_umin, delta_umax, Ref, MPC_solver); % ===== 计算输出 ===== U(1)=uPred(1);%存储控制量 U(2)=uPred(2); sys= uPred; %输出 end %% MPC function [ x, uPred ] = MPC_yalmip( A, B, Q, R, N, kesi, state_k1, umin, umax, delta_umin, delta_umax, Ref, MPC_solver) yalmip('clear');%清理内存 % ===== 定义变量 ===== x=sdpvar(size(A,2),N+1);%维度：[x;y;phi]*(N+1) u=sdpvar(size(B,2),N); %维度：[v;delta]*N % ===== 设置约束 ===== Constraints = [ x(:,1)==kesi(1:3)]; %初始条件 for i = 1:N Constraints=[Constraints; %运动学模型约束 x(:,i+1) == A*(x(:,i)-kesi(1:3)) + B*(u(:,i)-kesi(4:5))+state_k1;%非线性 umin<=u(:,i)<=umax;%控制量约束 ]; end for i = 1:N-1 Constraints=[Constraints; %控制增量约束 delta_umin<=u(:,1)-kesi(4:5)<=delta_umax; delta_umin<=u(:,i)-u(:,i+1) <=delta_umax; ]; end % ===== 定义成本函数 ===== Cost=0; for i=1:N Cost=Cost+(x(:,i+1)-Ref(:,i))'*Q*(x(:,i+1)-Ref(:,i)); end for i=1:N Cost=Cost+u(:,i)'*R*u(:,i); end % ===== 求解 ===== options = sdpsettings('verbose',1,'solver', MPC_solver); Problem = optimize(Constraints,Cost,options); uPred = double( u(:,1)); end
五、结果
以下为MPC对圆形轨迹跟踪的结果，x,y坐标、航向角的实际轨迹和参考轨迹对比
第一组：初始位置(0,10)
参数设置：x0=0; y0=10; N=25;
第二组：初始位置(0,0)
参数设置：x0=0; y0=0; N=80;
（ 第二组初始跟踪效果比较一般，有待改善。）
总结
1.采用yalmip编写无人驾驶车辆MPC代码，可以简化庞大的模型矩阵以及约束矩阵的推导计算过程。代码与对应的优化问题更便于理解，更换求解器和设置约束非常便利，修改时不易报错。
2.采用yalmip工具的优势在于简化代码，相应的代价则是降低了计算效率。与直接使用求解器(如quadprog)求解相比，计算效率仅为1/10左右。
3.所编写的代码对标《无人驾驶车辆模型预测控制》（第二版）第四章的代码，mdlOutputs函数可替换S函数相应的mdlOutputs，适当修改可以实现Carsim和Simulink联合仿真。调试代码需要简单学习yalmip语言编程。
最后
以上就是乐观爆米花最近收集整理的关于Matlab/yalmip工具编写自动驾驶模型预测控制(MPC)代码前言一、yalmip简介二、车辆模型1.车辆运动学模型2.离散化三、MPC优化问题定义 四、Matlab代码五、结果总结的全部内容，更多相关Matlab/yalmip工具编写自动驾驶模型预测控制(MPC)代码前言一、yalmip简介二、车辆模型1.车辆运动学模型2.离散化三、MPC优化问题定义 内容请搜索靠谱客的其他文章。
发表评论 取消回复