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

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

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

MPC控制算法

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

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

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

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

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

非线性模型离散化
zk+1=zk+zk˙(zk,uk)dtz_{k+1}=z_{k} + \dot{z_k}(z_k, u_k)dtzk+1=zk+zk˙(zk,uk)dt
在zˉ,uˉ\bar{z},\bar{u}zˉ,uˉ位置进行泰勒展开
zk+1=zk+(zk˙(zˉ,uˉ)+∂zk˙∂z(zk−zˉ)+∂zk˙∂u(uk−uˉ))dtz_{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}))dtzk+1=zk+(zk˙(zˉ,uˉ)+∂z∂zk˙(zk−zˉ)+∂u∂zk˙(uk−uˉ))dt
zk+1=(I+dt∂zk˙∂z)zk+dt∂zk˙∂uuk+(zk˙(zˉ,uˉ)−∂zk˙∂zzˉ−∂zk˙∂uuˉ)dtz_{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})dtzk+1=(I+dt∂z∂zk˙)zk+dt∂u∂zk˙uk+(zk˙(zˉ,uˉ)−∂z∂zk˙zˉ−∂u∂zk˙uˉ)dt
因此有：
A=(I+dt∂zk˙∂z)A=(I+dt\frac{\partial \dot{z_k}}{\partial z})A=(I+dt∂z∂zk˙)
B=dt∂zk˙∂uB=dt\frac{\partial \dot{z_k}}{\partial u}B=dt∂u∂zk˙
C=(zk˙(zˉ,uˉ)−∂zk˙∂zzˉ−∂zk˙∂uuˉ)dtC=(\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})dtC=(zk˙(zˉ,uˉ)−∂z∂zk˙zˉ−∂u∂zk˙uˉ)dt
对于导数∂zk˙∂z,∂zk˙∂u\frac{\partial \dot{z_k}}{\partial z},\frac{\partial \dot{z_k}}{\partial u}∂z∂zk˙,∂u∂zk˙,则需要根据具体模型得到。

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

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

问题求解

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

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

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

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

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

车辆运动学

自行车模型

状态向量
z=[x,y,v,ϕ]Tz = [x,y,v,\phi]^T z=[x,y,v,ϕ]T

其中，x,y,ϕx,y,\phix,y,ϕ为车辆在世界坐标系下的位置及角度。vvv为车辆当前的速度。
控制量
u=[a,δ]Tu = [a, \delta]^T u=[a,δ]T

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

自行车模型如下图所示：

根据几何关系有
vx=vcos⁡(θ)v_x=v \cos(\theta)vx=vcos(θ)vy=vsin⁡(θ)v_y=v \sin(\theta)vy=vsin(θ)
R=Ltan⁡(δ)R = \frac{L}{\tan(\delta)}R=tan(δ)L
且有
ω=vR=vtan⁡δL\omega=\frac{v}{R}=\frac{v \tan{\delta}}{L}ω=Rv=Lvtanδ

状态向量的导数
z˙=[vcos⁡(ϕ)vsin⁡(ϕ)avtan⁡δL]\dot{z} = \left[\begin{matrix} v \cos(\phi) \\ v \sin(\phi)\\ a \\ \frac{v \tan{\delta}}{L} \end{matrix}\right] z˙=⎣⎢⎢⎡vcos(ϕ)vsin(ϕ)aLvtanδ⎦⎥⎥⎤

因此有：
∂z˙∂z=[00cos(ϕ)−vsin⁡(ϕ)00sin(ϕ)vcos⁡(ϕ)000000tan⁡δL0]\frac{\partial \dot{z}}{\partial z}=\left[\begin{matrix} 0 & 0 & cos(\phi) & -v \sin(\phi) \\ 0 & 0 & sin(\phi) & v \cos(\phi)\\ 0 & 0 & 0 & 0\\ 0 & 0 & \frac{\tan{\delta}}{L} & 0 \end{matrix}\right]∂z∂z˙=⎣⎢⎢⎡00000000cos(ϕ)sin(ϕ)0Ltanδ−vsin(ϕ)vcos(ϕ)00⎦⎥⎥⎤

∂z˙∂u=[0000100vLcos⁡2(δ)]\frac{\partial \dot{z}}{\partial u}=\left[\begin{matrix} 0 & 0 \\ 0 & 0 \\ 1 & 0 \\ 0 & \frac{v}{L \cos^2(\delta)} \end{matrix}\right]∂u∂z˙=⎣⎢⎢⎡0010000Lcos2(δ)v⎦⎥⎥⎤

最终得到线性模型：
zk+1=A∗zk+B∗uk+Cz_{k+1} = A * z_{k} + B * u_k + C zk+1=A∗zk+B∗uk+C

A=(I+dt∂zk˙∂z)=[10cos(ϕ)dt−vsin⁡(ϕ)dt01sin(ϕ)dtvcos⁡(ϕ)dt001000tan⁡δLdt1]A = (I+dt\frac{\partial \dot{z_k}}{\partial z})=\left[\begin{matrix} 1 & 0 & cos(\phi)dt & -v \sin(\phi) dt\\ 0 & 1 & sin(\phi)dt & v \cos(\phi) dt\\ 0 & 0 & 1 & 0\\ 0 & 0 & \frac{\tan{\delta}}{L} dt & 1 \end{matrix}\right] A=(I+dt∂z∂zk˙)=⎣⎢⎢⎡10000100cos(ϕ)dtsin(ϕ)dt1Ltanδdt−vsin(ϕ)dtvcos(ϕ)dt01⎦⎥⎥⎤
B=dt∂zk˙∂u=[0000dt00vLcos⁡2(δ)dt]B=dt\frac{\partial \dot{z_k}}{\partial u}=\left[\begin{matrix} 0 & 0 \\ 0 & 0 \\ dt & 0 \\ 0 & \frac{v}{L \cos^2(\delta)} dt \end{matrix}\right] B=dt∂u∂zk˙=⎣⎢⎢⎡00dt0000Lcos2(δ)vdt⎦⎥⎥⎤
C=(zk˙(zˉ,uˉ)−∂zk˙∂zzˉ−∂zk˙∂uuˉ)dt=[vsin⁡(ϕ)ϕdt−vcos⁡(ϕ)ϕdt0−vδLcos⁡2(δ)dt]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{matrix} v \sin(\phi)\phi dt\\ -v \cos(\phi)\phi dt \\ 0 \\ -\frac{v\delta}{L\cos^2(\delta)} dt \end{matrix}\right]C=(zk˙(zˉ,uˉ)−∂z∂zk˙zˉ−∂u∂zk˙uˉ)dt=⎣⎢⎢⎡vsin(ϕ)ϕdt−vcos(ϕ)ϕdt0−Lcos2(δ)vδdt⎦⎥⎥⎤

差速模型

状态向量
z=[x,y,ϕ]Tz = [x,y,\phi]^T z=[x,y,ϕ]T

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

控制量
u=[vr,vl]Tu = [v_r, v_l]^T u=[vr,vl]T
分别为左轮速度vlv_lvl和右轮速度vrv_rvr。

状态向量的导数

z˙=[vr+vl2cos⁡(ϕ)vr+vl2sin⁡(ϕ)vr−vlD]\dot{z} = \left[\begin{matrix} \frac{v_r + v_l}{2}\cos(\phi) \\ \frac{v_r + v_l}{2} \sin(\phi)\\ \frac{v_r - v_l}{D} \end{matrix}\right] z˙=⎣⎡2vr+vlcos(ϕ)2vr+vlsin(ϕ)Dvr−vl⎦⎤

因此有：
∂z˙∂z=[00−vr+vl2sin⁡(ϕ)00vr+vl2cos⁡(ϕ)000]\frac{\partial \dot{z}}{\partial z}=\left[\begin{matrix} 0 & 0 & -\frac{v_r + v_l}{2} \sin(\phi) \\ 0 & 0 & \frac{v_r + v_l}{2} \cos(\phi) \\ 0 & 0 & 0 \\ \end{matrix}\right]∂z∂z˙=⎣⎡000000−2vr+vlsin(ϕ)2vr+vlcos(ϕ)0⎦⎤

∂z˙∂u=[cos⁡(ϕ)2cos⁡(ϕ)2sin⁡(ϕ)2sin⁡(ϕ)21D−1D]\frac{\partial \dot{z}}{\partial u}=\left[\begin{matrix} \frac{\cos(\phi)}{2} & \frac{\cos(\phi)}{2} \\ \frac{\sin(\phi)}{2} & \frac{\sin(\phi)}{2} \\ \frac{1}{D} & -\frac{1}{D} \\ \end{matrix}\right]∂u∂z˙=⎣⎡2cos(ϕ)2sin(ϕ)D12cos(ϕ)2sin(ϕ)−D1⎦⎤

最终得到线性模型：
zk+1=A∗zk+B∗uk+Cz_{k+1} = A * z_{k} + B * u_k + C zk+1=A∗zk+B∗uk+C

A=(I+dt∂zk˙∂z)=[10−vr+vl2sin⁡(ϕ)dt01vr+vl2cos⁡(ϕ)dt001]A = (I+dt\frac{\partial \dot{z_k}}{\partial z})=\left[\begin{matrix} 1 & 0 & -\frac{v_r + v_l}{2} \sin(\phi) dt \\ 0 & 1 & \frac{v_r + v_l}{2} \cos(\phi) dt\\ 0 & 0 & 1 \\ \end{matrix}\right] A=(I+dt∂z∂zk˙)=⎣⎡100010−2vr+vlsin(ϕ)dt2vr+vlcos(ϕ)dt1⎦⎤
B=dt∂zk˙∂u=[cos⁡(ϕ)2dtcos⁡(ϕ)2dtsin⁡(ϕ)2dtsin⁡(ϕ)2dt1Ddt−1Ddt]B=dt\frac{\partial \dot{z_k}}{\partial u}=\left[\begin{matrix} \frac{\cos(\phi)}{2} dt & \frac{\cos(\phi)}{2} dt \\ \frac{\sin(\phi)}{2} dt & \frac{\sin(\phi)}{2} dt \\ \frac{1}{D} dt & -\frac{1}{D} dt \\ \end{matrix}\right] B=dt∂u∂zk˙=⎣⎡2cos(ϕ)dt2sin(ϕ)dtD1dt2cos(ϕ)dt2sin(ϕ)dt−D1dt⎦⎤
C=(zk˙(zˉ,uˉ)−∂zk˙∂zzˉ−∂zk˙∂uuˉ)dt=[vr+vl2sin⁡(ϕ)ϕdt−vr+vl2cos⁡(ϕ)ϕdt0]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{matrix} \frac{v_r + v_l}{2}\sin(\phi)\phi dt\\ -\frac{v_r + v_l}{2} \cos(\phi)\phi dt \\ 0 \end{matrix}\right]C=(zk˙(zˉ,uˉ)−∂z∂zk˙zˉ−∂u∂zk˙uˉ)dt=⎣⎡2vr+vlsin(ϕ)ϕdt−2vr+vlcos(ϕ)ϕdt0⎦⎤

算法流程

将控制时间以dtdtdt的间隔进行离散成t0−t3t_0-t_3t0−t3四个时刻。根据上一次优化得到的控制量u0−u2u_0-u_2u0−u2能够根据非线性模型预测得到四个时刻的状态即x0−x3x_0-x_3x0−x3。并且由参考轨迹给出参考位置xref,0−3x_{ref,0-3}xref,0−3

目标函数：

控制量尽可能小
cost1=∥u0∥2+∥u1∥2+∥u2∥2cost_1 = \|u_0\|^2+\|u_1\|^2+\|u_2\|^2cost1=∥u0∥2+∥u1∥2+∥u2∥2
控制量的变化尽可能小
cost2=∥u1−u0∥2+∥u2−u1∥2cost_2 = \|u_1 - u_0\|^2+\|u_2-u_1\|^2cost2=∥u1−u0∥2+∥u2−u1∥2
中间状态均尽可能接近目标状态
cost3=∥xref,0−x0∥+∥xref,1−x1∥+∥xref,2−x2∥cost_3=\|x_{ref,0} - x_0\|+\|x_{ref,1} - x_1\|+\|x_{ref,2} - x_2\|cost3=∥xref,0−x0∥+∥xref,1−x1∥+∥xref,2−x2∥
结束状态尽可能接近目标状态（权重更大）
cost4=∥xref,3−x3∥2cost_4=\|x_{ref,3} - x_3\|^2cost4=∥xref,3−x3∥2

最终目标为加权和：
cost=w1⋅cost1+w2⋅cost2+w3⋅cost3+w4⋅cost4cost = w_1 \cdot cost_1 + w_2 \cdot cost_2 + w_3 \cdot cost_3 + w_4 \cdot cost_4 cost=w1⋅cost1+w2⋅cost2+w3⋅cost3+w4⋅cost4

约束条件：

满足线性模型 xt+1=Axt+But+Cx_{t+1}=A x_t + B u_t + Cxt+1=Axt+But+C
x1=A0x0+B0u0+C0x2=A1x1+B1u1+C1x3=A2x2+B2u2+C2x_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 x1=A0x0+B0u0+C0x2=A1x1+B1u1+C1x3=A2x2+B2u2+C2
其中，A0−2,B0−2,C0−2A_{0-2},B_{0-2},C_{0-2}A0−2,B0−2,C0−2分别为模型在xˉ0−2\bar{x}_{0-2}xˉ0−2处，分别进行泰勒展开计算得到。而 xˉ0−2\bar{x}_{0-2}xˉ0−2则为根据上一次优化结果预测系统在t0−2t_{0-2}t0−2时刻的状态。
控制量的变化小于最大允许值 ∣ut+1−ut∣<maxu⋅dt|u_{t+1}-u_t|<max_{u}\cdot dt∣ut+1−ut∣<maxu⋅dt
初始状态始终保持不变 x0=xlast,0x_0=x_{last,0}x0=xlast,0
其他
1. 最大、最小速度约束
2. 最大控制量约束

求解：

cost=w1⋅cost1+w2⋅cost2+w3⋅cost3+w4⋅cost4=w1⋅(∥u0∥2+∥u1∥2+∥u2∥2)+w2⋅(∥u1−u0∥2+∥u2−u1∥2)+w3⋅(∥xref,0−x0∥2+∥xref,1−x1∥2+∥xref,2−x2∥2)+w4⋅∥xref,3−x3∥2\begin{aligned} cost = & w_1 \cdot cost_1 + w_2 \cdot cost_2 + w_3 \cdot cost_3 + w_4 \cdot cost_4 \\ =&w_1 \cdot (\|u_0\|^2+\|u_1\|^2+\|u_2\|^2) + \\ &w_2 \cdot (\|u_1 - u_0\|^2+\|u_2-u_1\|^2) + \\ &w_3 \cdot (\|x_{ref,0} - x_0\|^2+\|x_{ref,1} - x_1\|^2+\|x_{ref,2} - x_2\|^2) + \\ & w_4 \cdot \|x_{ref,3} - x_3\|^2 \end{aligned}cost==w1⋅cost1+w2⋅cost2+w3⋅cost3+w4⋅cost4w1⋅(∥u0∥2+∥u1∥2+∥u2∥2)+w2⋅(∥u1−u0∥2+∥u2−u1∥2)+w3⋅(∥xref,0−x0∥2+∥xref,1−x1∥2+∥xref,2−x2∥2)+w4⋅∥xref,3−x3∥2

其中前两项为[u0,u1,u2]T[u_0,u_1,u_2]^T[u0,u1,u2]T的二次函数，后面两项中有：
x0=xlast,0=constx1=A0x0+B0u0+C0=B0u0+C0′x2=A1x1+B1u1+C1=A1(B0u0+C0′)+B1u1+C1=A1B0u0+B1u1+C1′x3=A2x2+B2u2+C2=A2(A1B0u0+B1u1+C1′)+B2u2+C2=A2A1B0u1+A2B1u0+B2u2+C2′\begin{aligned} x_0 &= x_{last,0} = const \\ x_1 &= A_0 x_0 + B_0 u_0 +C_0 = B_0u_0 + C^{'}_0 \\ x_2 &= A_1 x_1 + B_1 u_1 +C_1 = A_1 (B_0u_0 + C^{'}_0) + B_1 u_1 +C_1 \\ &= A_1 B_0 u_0 +B_1 u_1 + C^{'}_1\\ 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) + B_2 u_2 +C_2 \\ &= A_2A_1B_0 u_1 + A_2 B_1 u_0 + B_2 u2 + C^{'}_2 \end{aligned}x0x1x2x3=xlast,0=const=A0x0+B0u0+C0=B0u0+C0′=A1x1+B1u1+C1=A1(B0u0+C0′)+B1u1+C1=A1B0u0+B1u1+C1′=A2x2+B2u2+C2=A2(A1B0u0+B1u1+C1′)+B2u2+C2=A2A1B0u1+A2B1u0+B2u2+C2′

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