目录

前言

一、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

syms 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：求解器。

function [ 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。

    x=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循环、==、<=设定约束，极为便利，不需要去推导庞大的约束矩阵。

 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;%控制量约束];endfor 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循环设置成本函数。

    Cost=0;for i=1:NCost=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语言调用求解器进行求解，最后输出所需的控制量。

    options = sdpsettings('verbose',1,'solver', MPC_solver);Problem = optimize(Constraints,Cost,options);uPred = double( u(:,1));

整个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;%控制量约束];endfor 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:NCost=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的跟踪结果。

clear;clc;
%% 主函数
% ===== 参数 =====global U TT=0.05;%离散时间L=2.6;%轴距T_all=30;%总时间
% ===== 初始化 =====x0=0;y0=10;%10phi0=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('时间：%.2fs\n',t);endkesi=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:Nx_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];endRef=cell2mat(Ref_cell);
% ===== 求解器 =====MPC_solver = 'gurobi'; % '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;%控制量约束];endfor 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:NCost=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.所编写的代码对标《无人驾驶车辆模型预测控制》（第二版）第四章的代码，mdlOutputs函数可替换S函数相应的mdlOutputs，适当修改可以实现Carsim和Simulink联合仿真。使用此代码需要学习yalmip语言，并大概了解《无人驾驶车辆模型预测控制》书中的代码。

3.采用yalmip工具的优势在于简化代码，相应的代价则是降低了计算效率。与直接使用求解器(如quadprog)相比，计算效率仅为1/10左右。

4.个人认为在编写复杂MPC代码时，使用yalmip工具能节省大量时间，减少代码出错的概率。此外，yalmip也便于随时更换求解器和设置非线性约束。

Matlab/yalmip工具编写自动驾驶模型预测控制(MPC)代码相关推荐

自动驾驶——模型预测控制（MPC）理解与实践
当时在做路径跟踪.路径规划时,使用了MPC,通过项目的应用,对于MPC建立了一定的认识,但是一段时间过去后,认知又渐渐模糊了,当时学习过程中也是看了许多人的blog及代码才弄清楚,这里试图从理论到实践 ...
基于扩展卡尔曼滤波EKF和模型预测控制MPC，自动泊车场景建模开发
基于扩展卡尔曼滤波EKF和模型预测控制MPC,自动泊车场景建模开发,文复现. MATLAB 基于扩展卡尔曼滤波EKF和模型预测控制MPC,自动泊车场景建模开发,文复现. MATLAB(工程项目线上支持 ...
Matlab：基于Matlab通过GUI实现自动驾驶的车牌智能识别
Matlab:基于Matlab通过GUI实现自动驾驶的车牌智能识别目录车牌图像数据集视频动态演示核心代码相关文章 Matlab:基于Matlab通过GUI实现自动驾驶的车牌智能识别 Matl ...
地铁自动驾驶模型，地铁列车牵引系统整车模型。
完整的列车模型包括目标曲线的优化,控制层进行跟随目标曲线,经神经网络控制和模糊控制给出推荐牵引力,牵引传动系统控制牵引电机输出牵引转矩,输入列车自动驾驶模型进行牵引计算.其中生成优化曲线通过遗传算法进 ...
无人车系统（十一）：轨迹跟踪模型预测控制(MPC)原理与python实现【40行代码】
前面介绍的PID,pure pursuit方法,Stanley方法都只是利用当前的系统误差来设计控制器.人们对这些控制器的设计过程中都利用了构建模型对无人车未来状态的估计(或者说利用模型估计未来的运动 ...
模型预测控制_模型预测控制(MPC)算法之一MAC算法
引言随着自动驾驶技术以及机器人控制技术的不断发展及逐渐火热,模型预测控制(MPC)算法作为一种先进的控制算法,其应用范围与领域得到了进一步拓展与延伸.目前提出的模型预测控制算法主要有基于非参数模型的 ...
【附C++源代码】模型预测控制(MPC)公式推导以及算法实现，Model Predictive control介绍
2022年的第一篇博客,首先祝大家新年快乐! 提示:本篇博客主要集中在对MPC的理解以及应用.这篇博客可以作为你对MPC控制器深入研究的一个开始,起到抛砖引玉,带你快速了解其原理的作用. 这篇博客将介 ...
基于模型预测控制(MPC)的车道保持控制实现方法
车辆保持的目的是通过检测到车辆与道路中心线的横向偏差和横摆角偏差来控制车辆的方向盘的转角,最终使车辆行驶在道路中心线上. MATLAB 2018b中有一个关于车道保持的案例,本次设计模型控制算法部分与 ...
Apollo代码学习(六)—模型预测控制(MPC)_follow轻尘的博客-CSDN博客_mpc代码
Apollo代码学习(六)-模型预测控制(MPC)_follow轻尘的博客-CSDN博客_mpc代码
基于模型预测控制(MPC)的悬架系统仿真分析
目录前言 1.悬架系统 2.基于MPC的悬架系统仿真分析 2.1 simulink模型 2.2仿真结果 2.2.1 随机C级路面 2.2.2 正弦路面 2.3 结论 3 总结前言模型预测控制是无 ...

Matlab/yalmip工具编写自动驾驶模型预测控制(MPC)代码

前言