轨迹跟踪—线性 MPC 控制算法

轨迹跟踪—线性 MPC 控制算法

在自动驾驶和无人车领域的路径规划常常被分为运动轨迹生成与车辆路径跟踪控制两个部分。传统的基于几何条件的跟踪算法如pure-pursuit由于没有考虑车辆运动学导致控制精度和稳定性等方面存在不足，因此目前大部分采用线性MPC控制算法，从而平衡控制精度与计算量。

本文首先介绍通用的MPC控制流程，对这一基于优化的控制理论有一定的认知。然后对常见的差速运动模型和自行车模型进行运动学分析，接着针对轨迹跟踪问题介绍如何将通用MPC算法应用至轨迹跟踪问题。最后基于开源代码对整体代码流程进行进一步的理解。

MPC控制算法

状态量$z$：系统的状态（也可认为是系统输出量），通常可以构成一个空间，如车辆状态$(x,y,\theta {)}^T\in SE2$

控制量$u$：系统的输入量，不同的系统模型有不同的控制参数

系统模型：描述系统输入与输出的精确关系，通常是非线性的关系。

采用增量的形式对系统模型进行表达：
$$z_{k+1}=f(z_k)+g(u_k)$$

线性系统模型：系统模型的线性化结果，用于近似求解，具有速度快、不会陷入局部极值的优势。
$$z_{k+1}=A\ast z_k+B\ast u_k+C$$
其中，矩阵$A,B$的求解过程如下：

1. 非线性模型离散化
   $$z_{k+1}=z_k+\dot{z_k}(z_k,u_k)dt$$
2. 在$\bar{z},\bar{u}$位置进行泰勒展开
   $$z_{k+1}=z_k+(\dot{z_k}(\bar{z},\bar{u})+\frac{\partial \dot{z_k}}{\partial z}(z_k-\bar{z})+\frac{\partial \dot{z_k}}{\partial u}(u_k-\bar{u}))dt$$
   $$z_{k+1}=(I+dt\frac{\partial \dot{z_k}}{\partial z})z_k+dt\frac{\partial \dot{z_k}}{\partial u}{u}_k+(\dot{z_k}(\bar{z},\bar{u})-\frac{\partial \dot{z_k}}{\partial z}\bar{z}-\frac{\partial \dot{z_k}}{\partial u}\bar{u})dt$$
   因此有：
   $$A=(I+dt\frac{\partial \dot{z_k}}{\partial z})$$
   $$B=dt\frac{\partial \dot{z_k}}{\partial u}$$
   $$C=(\dot{z_k}(\bar{z},\bar{u})-\frac{\partial \dot{z_k}}{\partial z}\bar{z}-\frac{\partial \dot{z_k}}{\partial u}\bar{u})dt$$
3. 对于导数$\frac{\partial \dot{z_k}}{\partial z},\frac{\partial \dot{z_k}}{\partial u}$,则需要根据具体模型得到。

目标函数：系统的期望状态，即当前控制系统最优的、理想的目标，采用最小化或最大化某个函数表达。
例如，对于轨迹跟踪问题：
1）跟踪的误差尽可能小
2）加速度尽量小，保证运动平滑
3）…

约束项：系统状态的先验约束（硬约束，一定不能违背的部分）
例如，
1）初始状态不变
2）满足线性模型
3）…

问题求解

问题：在满足约束条件的前提下，计算得到最优的控制量$u$，使得目标函数最小。

具体来说就是将问题转换为二次规划问题即，

1. 目标函数用状态量表达 + 线性模型表达成一个二次函数形式。
2. 约束用状态量线性函数表达

由于模型被线性化，因此得到的目标函数总是一个多维二次函数，而约束条件为线性的。因此本问题可以直接采用二次规划的方法进行求解。具体可参考凸优化、数值优化等书籍。

具体的例子将在后续进行分析。

车辆运动学

自行车模型

状态向量
$$z=[x,y,v,\varphi {]}^T$$

其中，$x,y,\varphi$为车辆在世界坐标系下的位置及角度。$v$为车辆当前的速度。
控制量
$$u=[a,\delta {]}^T$$

其中，$a$为车辆加速度，而$\delta$为前轮转向角度

自行车模型如下图所示：

根据几何关系有
$$v_x=v\cos (\theta )$$$$v_y=$$