路径跟踪算法之模型预测控制(MPC)跟踪

路径跟踪算法之模型预测控制(MPC)跟踪

1.MPC简介

模型预测控制（以下简称 MPC）是一种依赖于系统模型进行数学优化的复杂控制器。它利用优化算法计算有限时间范围内一系列的控制输入序列，并优化该序列，但控制器仅执行序列中的第一组控制输入，然后再次重复该循环。MPC 主要分为 3 个关键步骤：模型预测、滚动优化、反馈校正。

2.MPC在轨迹跟踪过程中的应用

在轨迹跟踪应用领域，通常 MPC 建模可根据机器人的控制方式选择基于运动学运动状态方程建模或者基于动力学运动状态方程建模。前者是根据车辆转向的几何学角度关系和速度位置关系来建立描述车辆的运动的预测模型，一般只适用于低速运动场景；后者是对被控对象进行综合受力分析，从受力平衡的角度建模，一般应用在高速运动场景，如汽车无人驾驶。本文基于双轮差速运动学模型，推导实现差速运动的MPC轨迹跟踪控制。 双轮差速运动学线性 MPC 轨迹跟踪实现的基本思路是状态方程–线性化–离散化–预测方程–约束线性化–非线性目标函数转为二次规划–求解最优问题。其中线性化可采用泰勒级数展开忽略高次项的方式，离散化可采用后向差分法，这两个部分都有一定的模型精度丢失。

3.推导过程

3.1双轮差速运动学建模
首先，以双轮中心点作为参考点建立双轮差速运动学状态方程：

其中状态量为[x,y,z]T,控制量为:[v,w]T。
式 3-1 为非线性方程，角度和位置存在耦合关系，为了便于逻辑计算与后续
结果解算，在期望位置点采用一级泰勒展开进行近似线性化处理：

其中:

式 3-2 减去式 3-3 后得到：

其中：

线性化完成之后，形成连续的线性运动学模型，为了在控制周期内编程实现，再采用欧拉前项差分法将模型离散化：

变换得：

整理得离散化的线性表达式：

3.2.预测模型
为预测下时刻与当前时刻的状态关系与趋势，将当前状态量与上一时刻的控制量进行组合，作为新的状态量输入，形成预测方程。

根据式 3-6。凑式成的线性齐次的矩阵表达式：

根据离散化的预测方程，结合预测时域和控制时域，结合式 3-10 列项如下：

根据规律，写成向量形式如下：

为了再分解出状态量和控制量，可以令：

其中：

n为状态量个数。
则系数矩阵可以作如下更新：

3.3.目标函数及约束
目标函数的选取基于两个优化原则：1.机器人的位姿尽量靠近参考路径，以实现跟踪的目的；2.机器人的控制量变化率尽量小，以保证速度变化的流畅性。基于此原则构造代价函数如下：

其中,Q 和 R 为权重系数。ρε2为松弛因子参数，一方面可以保证方程有解，
一方面也为了便于凑式计算。
将目标函数转换成二次规划的形式，便于求解。将预测方程带入代价函数，
矩阵方程凑式成标准的二次规划形式，有：

约束则有：

需要根据求解需要，将约束关系凑成标准的约束形式，主要包括不等式约束、等式约束和上下界约束。

4.matlab 仿真

4.1 参考轨迹生成

%% 生成直线轨迹
pos_num = 500; % 目标轨迹点的数量
sampling_period = 0.05; % 采样周期50ms
reference_pos_data = zeros(pos_num,3); % 用于存储目标轨迹点上的车辆状态；车辆共有3个状态变量：X轴坐标、Y轴坐标、航向角
for i = 1:1:pos_num
    %reference_pos_data(i,1) = (i-1)*sampling_period; % 设置目标轨迹点的X轴坐标
    reference_pos_data(i,2) = 2;
    reference_pos_data(i,2) = cos(reference_pos_data(i,1)/5)*reference_pos_data(i,1)/4;%曲线; % 设置目标轨迹点的Y轴坐标
    if i>1              
        reference_pos_data(i-1,3) = atan((reference_pos_data(i,2) - reference_pos_data(i-1,2))/(reference_pos_data(i,1) - reference_pos_data(i-1,1))); % 设置目标轨迹点的航向角
    end
    if i == pos_num
        reference_pos_data(i,3) = reference_pos_data(i-1,3);
    end
end
reference_velocity(1) = 0.15;%期望线速度1m/s
reference_velocity(2) = 0;%期望角速度0m/s

4.2 预测模型

%二次规划标准式 (X'*(1/2 *H) *X +f*X 最小值
H_cell = cell(2,2);
H_cell{1,1} = THETA'*Q*THETA + R;
H_cell{1,2} = zeros(control_length *control_num,1);
H_cell{2,1} = zeros(1,control_length *control_num);
H_cell{2,2} = relaxation_factor;
H = cell2mat(H_cell);
H = (1/2)*H;

f_cell = cell(1,2);
f_cell{1,1} = 2*(PHI*kesi)'*Q*THETA;
f_cell{1,2} = 0;
f = cell2mat(f_cell);

4.3 求解

options = optimset('Algorithm','interior-point-convex');
[u_result,fval,exitflag,~] = quadprog(H,f',A_cons,b_cons,[],[],lb,ub,[],options);

control_variable(1) = last_controlerror(1) + u_result(1);%当前时刻的控制量偏差 = 上一时刻的控制量偏差 + 计算得到的偏差增量值
control_variable(2) = last_controlerror(2) + u_result(2);%当前时刻的控制增量 = 上一时刻的控制增量 + 计算得到的偏差增量值

4.4 仿真效果

5.参考资料及链接：
[1].B 站：《模型预测控制 MPC 轨迹跟踪》（手稿+视频）
[2].CSDN：《无人驾驶运动学模型——线性时变模型预测控制的思路推演过程》
[3].CSDN：《Apollo 代码学习(六)—模型预测控制(MPC)》

仿真代码链接：
mpc跟踪仿真代码链接