轨迹跟踪—线性 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=v\sin (\theta )$$
$$R=\frac{L}{\tan (\delta )}$$
且有
$$\omega =\frac{v}{R}=\frac{v\tan \delta }{L}$$

状态向量的导数
$$\dot{z}=\left[\begin{array}{c}v\cos (\varphi )\\ v\sin (\varphi )\\ a\\ \frac{v\tan \delta }{L}\end{array}\right]$$

因此有：
$$\frac{\partial \dot{z}}{\partial z}=\left[\begin{array}{cccc}0& 0& cos(\varphi )& -v\sin (\varphi )\\ 0& 0& sin(\varphi )& v\cos (\varphi )\\ 0& 0& 0& 0\\ 0& 0& \frac{\tan \delta }{L}& 0\end{array}\right]$$

$$\frac{\partial \dot{z}}{\partial u}=\left[\begin{array}{cc}0& 0\\ 0& 0\\ 1& 0\\ 0& \frac{v}{L{\cos }^2(\delta )}\end{array}\right]$$

最终得到线性模型：
$$z_{k+1}=A\ast z_k+B\ast u_k+C$$

$$A=(I+dt\frac{\partial \dot{z_k}}{\partial z})=\left[\begin{array}{cccc}1& 0& cos(\varphi )dt& -v\sin (\varphi )dt\\ 0& 1& sin(\varphi )dt& v\cos (\varphi )dt\\ 0& 0& 1& 0\\ 0& 0& \frac{\tan \delta }{L}dt& 1\end{array}\right]$$
$$B=dt\frac{\partial \dot{z_k}}{\partial u}=\left[\begin{array}{cc}0& 0\\ 0& 0\\ dt& 0\\ 0& \frac{v}{L{\cos }^2(\delta )}dt\end{array}\right]$$
$$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=\left[\begin{array}{c}v\sin (\varphi )\varphi dt\\ -v\cos (\varphi )\varphi dt\\ 0\\ -\frac{v\delta }{L{\cos }^2(\delta )}dt\end{array}\right]$$

差速模型

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

其中，$x,y,\varphi$为车辆在世界坐标系下的位置及角度。

控制量
$$u=[v_r,v_l{]}^T$$
分别为左轮速度$v_l$和右轮速度$v_r$。

状态向量的导数

$$\dot{z}=\left[\begin{array}{c}\frac{v_r+v_l}{2}\cos (\varphi )\\ \frac{v_r+v_l}{2}\sin (\varphi )\\ \frac{v_r-v_l}{D}\end{array}\right]$$

因此有：
$$\frac{\partial \dot{z}}{\partial z}=\left[\begin{array}{ccc}0& 0& -\frac{v_r+v_l}{2}\sin (\varphi )\\ 0& 0& \frac{v_r+v_l}{2}\cos (\varphi )\\ 0& 0& 0\end{array}\right]$$

$$\frac{\partial \dot{z}}{\partial u}=\left[\begin{array}{cc}\frac{\cos (\varphi )}{2}& \frac{\cos (\varphi )}{2}\\ \frac{\sin (\varphi )}{2}& \frac{\sin (\varphi )}{2}\\ \frac{1}{D}& -\frac{1}{D}\end{array}\right]$$

最终得到线性模型：
$$z_{k+1}=A\ast z_k+B\ast u_k+C$$

$$A=(I+dt\frac{\partial \dot{z_k}}{\partial z})=\left[\begin{array}{ccc}1& 0& -\frac{v_r+v_l}{2}\sin (\varphi )dt\\ 0& 1& \frac{v_r+v_l}{2}\cos (\varphi )dt\\ 0& 0& 1\end{array}\right]$$
$$B=dt\frac{\partial \dot{z_k}}{\partial u}=\left[\begin{array}{cc}\frac{\cos (\varphi )}{2}dt& \frac{\cos (\varphi )}{2}dt\\ \frac{\sin (\varphi )}{2}dt& \frac{\sin (\varphi )}{2}dt\\ \frac{1}{D}dt& -\frac{1}{D}dt\end{array}\right]$$
$$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=\left[\begin{array}{c}\frac{v_r+v_l}{2}\sin (\varphi )\varphi dt\\ -\frac{v_r+v_l}{2}\cos (\varphi )\varphi dt\\ 0\end{array}\right]$$

算法流程

将控制时间以$dt$的间隔进行离散成$t_0-t_3$四个时刻。根据上一次优化得到的控制量$u_0-u_2$能够根据非线性模型预测得到四个时刻的状态即$x_0-x_3$。并且由参考轨迹给出参考位置$x_{ref,0-3}$

目标函数：

1. 控制量尽可能小
   $$cos{t}_1=\Vert u_0{\Vert }^2+\Vert u_1{\Vert }^2+\Vert u_2{\Vert }^2$$

2. 控制量的变化尽可能小
   $$cos{t}_2=\Vert u_1-u_0{\Vert }^2+\Vert u_2-u_1{\Vert }^2$$

3. 中间状态均尽可能接近目标状态
   $$cos{t}_3=\Vert x_{ref,0}-x_0\Vert +\Vert x_{ref,1}-x_1\Vert +\Vert x_{ref,2}-x_2\Vert$$

4. 结束状态尽可能接近目标状态（权重更大）
   $$cos{t}_4=\Vert x_{ref,3}-x_3{\Vert }^2$$

最终目标为加权和：
$$cost=w_1\cdot cos{t}_1+w_2\cdot cos{t}_2+w_3\cdot cos{t}_3+w_4\cdot cos{t}_4$$

约束条件：

1. 满足线性模型 $x_{t+1}=A{x}_t+B{u}_t+C$
   $$\begin{gathered} x_1=A_0{x}_0+B_0{u}_0+C_0 \\ {x}_2=A_1{x}_1+B_1{u}_1+C_1 \\ {x}_3=A_2{x}_2+B_2{u}_2+C_2 \end{gathered}$$
   其中，$A_{0-2},B_{0-2},C_{0-2}$分别为模型在${\bar{x}}_{0-2}$处，分别进行泰勒展开计算得到。而 ${\bar{x}}_{0-2}$则为根据上一次优化结果预测系统在$t_{0-2}$时刻的状态。
2. 控制量的变化小于最大允许值 $|u_{t+1}-u_t|<ma{x}_u\cdot dt$
3. 初始状态始终保持不变 $x_0=x_{last,0}$
4. 其他
   1. 最大、最小速度约束
   2. 最大控制量约束

求解：

$$\begin{array}{rl}cost=& w_1\cdot cos{t}_1+w_2\cdot cos{t}_2+w_3\cdot cos{t}_3+w_4\cdot cos{t}_4\\ =& w_1\cdot (\Vert u_0{\Vert }^2+\Vert u_1{\Vert }^2+\Vert u_2{\Vert }^2)+\\ & w_2\cdot (\Vert u_1-u_0{\Vert }^2+\Vert u_2-u_1{\Vert }^2)+\\ & w_3\cdot (\Vert x_{ref,0}-x_0{\Vert }^2+\Vert x_{ref,1}-x_1{\Vert }^2+\Vert x_{ref,2}-x_2{\Vert }^2)+\\ & w_4\cdot \Vert x_{ref,3}-x_3{\Vert }^2\end{array}$$

其中前两项为$[u_0,u_1,u_2{]}^T$的二次函数，后面两项中有：
$$\begin{array}{rl}{x}_0& =x_{last,0}=const\\ x_1& =A_0{x}_0+B_0{u}_0+C_0=B_0{u}_0+C_0^{{}^{\prime }}\\ x_2& =A_1{x}_1+B_1{u}_1+C_1=A_1(B_0{u}_0+C_0^{{}^{\prime }})+B_1{u}_1+C_1\\ & =A_1{B}_0{u}_0+B_1{u}_1+C_1^{{}^{\prime }}\\ x_3& =A_2{x}_2+B_2{u}_2+C_2=A_2(A_1{B}_0{u}_0+B_1{u}_1+C_1^{{}^{\prime }})+B_2{u}_2+C_2\\ & =A_2{A}_1{B}_0{u}_1+A_2{B}_1{u}_0+B_{2}u2+C_2^{{}^{\prime }}\end{array}$$

因此后面两项均同样为$[u_0,u_1,u_2{]}^T$的二次函数，因此代价函数整体都是二次的，可直接采用二次规划算法进行求解。