MPC（4）基于运动学模型的轨迹跟踪控制器设计

有了之前的三篇知识基础，这一篇主要是工程实践。

无人车辆在惯性坐标系中，车辆必须从一个给定的初始状态出发，然后通过某种控制器控制车辆尽量按照规划好的轨迹行驶。如下图所示：

如图中所示，期望轨迹是一条几何曲线$f(x_t(t))$，自变量$x_t$是时间$t$的函数。
为了进一步深入研究，做出了以下假设：

• 环境感知部分无问题，规划出的路径为可行驶区域
• 定位部分可以精确获得车辆的位置信息，包括车辆后轴中点的坐标信息，偏航角
• 可以获取到车辆底层信息，车辆前轮的转角、车身速度等相关信息

换句话说，就是轨迹跟踪是在行驶环境以及车辆内部状态信息完全已知的情况下进行的。

模型的选取

由上一篇文章可知，目前主要使用的三种车辆模型，由于不考虑车辆高速行驶的情况，本文采用第二种车辆运动学模型。
$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\phi }\end{array}\right]=\left[\begin{array}{c}\cos \phi \\ \sin \phi \\ \tan \delta /l\end{array}\right]\ast v\text{\hspace{1em}(1)}$$
式中，$(x,y)$为车辆后轴中心的坐标，
$\phi$为车辆的偏航角，
$\delta$为车辆前轮转角，
$v$为车辆后轴中心速度，
$l$为车辆前后轴之间的距离。

系统化为一般形式，输入为$u(v,\delta )$和状态量$\chi (x,y,\phi )$，此时有
$$\dot{\chi }=f(\chi ,u)\text{\hspace{1em}(2)}$$
对于给定的参考轨迹，可以用参考车辆的运动轨迹描述,用$r$代表参考量，参考轨迹可以转化为：
$$\dot{{\chi }_r}=f({\chi }_r,u_r)\text{\hspace{1em}(3)}$$
式中，${\chi }_r=[x_r{y}_r{\phi }_r{]}^T$,
$u_r=[v_r{\delta }_r]$。

将式(2)用泰勒展开并忽略高阶项，有：
$$\begin{gathered} \dot{\chi }=f({\chi }_r,u_r)+\frac{\alpha f(\chi ,u)}{\alpha \chi }{|}_{\genfrac{}{}{0ex}{}{\chi ={\chi }_r}{u=u_r}}\ast (\chi -{\chi }_r) \\ +\frac{\alpha f(\chi ,u)}{\alpha u}{|}_{\genfrac{}{}{0ex}{}{\chi ={\chi }_r}{u=u_r}}\ast (u-u_r)\text{\hspace{1em}(4)} \end{gathered}$$

式(4)与式(3)相减，可以得到
$$\begin{gathered} \dot{\tilde{\chi }}=\left[\begin{array}{c}\dot{x}-{\dot{x}}_r\\ \dot{y}-{\dot{y}}_r\\ \dot{\phi }-{\dot{\phi }}_r\end{array}\right] \\ =\left[\begin{array}{ccccc}0& & 0& & -v_r\ast sin{\phi }_r\\ 0& & 0& & v_r\ast cos{\phi }_r\\ 0& & 0& & 0\end{array}\right]\left[\begin{array}{c}x-x_r\\ y-y_r\\ \phi -{\phi }_r\end{array}\right]+\left[\begin{array}{ccc}cos{\phi }_r& & 0\\ sin{\phi }_r& & 0\\ \frac{tan{\delta }_r}{l}& & \frac{v_r}{l\ast co{s}^2{\delta }_r}\end{array}\right]\left[\begin{array}{c}v-v_r\\ \delta -{\delta }_r\end{array}\right]\text{\hspace{1em}(5)} \end{gathered}$$
式(5)即线性化的无人驾驶车辆误差模型。
工程中，可以参考的一般只有规划好的路点坐标而已，与上述的误差模型还是有区别的。

为了便于控制器的设计，对式(5)离散化处理后，得到
$$\tilde{\chi }(k+1)=A_{k,t}\ast \tilde{\chi }(k)+B_{k,t}\ast \tilde{u}(k)\text{\hspace{1em}(6)}$$
式中，

$A_{k,t}=\left[\begin{array}{ccccc}1& & 0& & -v_r\ast sin{\phi }_r\ast T\\ 0& & 1& & v_r\ast cos{\phi }_r\ast T\\ 0& & 0& & 1\end{array}\right]$，
$B_{k,t}=\left[\begin{array}{ccc}cos{\phi }_r\ast T& & 0\\ sin{\phi }_r\ast T& & 0\\ \frac{tan{\delta }_f\ast T}{l}& & \frac{v_r\ast T}{l\ast co{s}^2{\delta }_r}\end{array}\right]$，T为采样时间。

误差模型的线性化

首先，需要将式(6)转化为线性误差模型，令
$$\xi (k|t)=\left[\begin{array}{c}\tilde{\chi }(k|t)\\ \tilde{u}(k-1|t)\end{array}\right]\text{\hspace{1em}(7)}$$
得到新的空间状态表达式：
$$\begin{gathered} \xi (k+1|t)={\tilde{A}}_{k,t}\ast \xi (k|t)+{\tilde{B}}_{k,t}\ast \Delta U(k|t) \\ \eta (k|t)={\tilde{C}}_{k,t}\ast \xi (k|t)\text{\hspace{1em}(8)} \end{gathered}$$
式中，
$${\tilde{A}}_{k,t}=\left[\begin{array}{ccc}{A}_{k,t}& & B_{k,t}\\ 0_{m\ast n}& & I_m\end{array}\right],{\tilde{B}}_{k,t}=\left[\begin{array}{c}{B}_{k,t}\\ I_m\end{array}\right]$$

$n$为状态量维度，
$m$为控制量维度。

为了简化计算，做出如下假设
$$\begin{gathered} A_{k,t}=A_{t,t},k=1,\dots ,t+N-1 \\ {B}_{k,t}=B_{t,t},k=1,\dots ,t+N-1\text{\hspace{1em}(9)} \end{gathered}$$
推导后，得到系统的预测输出表达式：
$$Y(t)={\Psi }_t\ast \xi (t|t)+\Theta \ast \Delta U(t)\text{\hspace{1em}(10)}$$
式中，
$$Y(t)=\left[\begin{array}{c}\eta (t+1|t)\\ \eta (t+2|t)\\ \dots \\ \eta (t+N_c|t)\\ \dots \\ \eta (t+N_p|t)\end{array}\right]$$，
$${\Psi }_t=\left[\begin{array}{c}{\tilde{C}}_{t,t}\ast {\tilde{A}}_{t,t}\\ {\tilde{C}}_{t,t}\ast {\tilde{A}}_{t,t}^2\\ \dots \\ {\tilde{C}}_{t,t}\ast {\tilde{A}}_{t,t}^{N_c}\\ \dots \\ {\tilde{C}}_{t,t}\ast {\tilde{A}}_{t,t}^{N_p}\end{array}\right]$$，
$${\Theta }_t=\left[\begin{array}{ccccccc}{\tilde{C}}_{t,t}{\tilde{B}}_{t,t}& & 0& & 0& & 0\\ {\tilde{C}}_{t,t}{\tilde{A}}_{t,t}{\tilde{B}}_{t,t}& & {\tilde{C}}_{t,t}{\tilde{B}}_{t,t}& & 0& & 0\\ \cdots & & \cdots & & \ddots & & \cdots \\ {\tilde{C}}_{t,t}{\tilde{A}}_{t,t}^{N_c-1}{\tilde{B}}_{t,t}& & {\tilde{C}}_{t,t}{\tilde{A}}_{t,t}^{N_c-2}{\tilde{B}}_{t,t}& & \cdots & & {\tilde{C}}_{t,t}{\tilde{B}}_{t,t}\\ {\tilde{C}}_{t,t}{\tilde{A}}_{t,t}^{N_c}{\tilde{B}}_{t,t}& & {\tilde{C}}_{t,t}{\tilde{A}}_{t,t}^{N_c-1}{\tilde{B}}_{t,t}& & \cdots & & {\tilde{C}}_{t,t}{\tilde{A}}_{t,t}{\tilde{B}}_{t,t}\\ ⋮& & ⋮& & \ddots & & ⋮\\ {\tilde{C}}_{t,t}{\tilde{A}}_{t,t}^{N_p-1}{\tilde{B}}_{t,t}& & {\tilde{C}}_{t,t}{\tilde{A}}_{t,t}^{N_p-2}{\tilde{B}}_{t,t}& & \cdots & & {\tilde{C}}_{t,t}{\tilde{A}}_{t,t}^{N_p-N_c-1}{\tilde{B}}_{t,t}\end{array}\right]$$
$$\Delta U(t)=\left[\begin{array}{c}\Delta u(t|t)\\ \Delta u(t+1|t)\\ \cdots \\ \Delta u(t+N_c|t)\end{array}\right]$$
。

目标函数设计

目标函数要能够保证无人驾驶车辆快速且平稳地追踪期望轨迹。以下介绍基于无人驾驶车辆线性误差模型预测车辆未来时刻的输出。
一般采用如下形式的目标函数：
$$J(k)=\sum _{i=1}^{N_p}{\Vert \begin{array}{c}\eta (k+i|t)-{\eta }_{ref}(k+i|t)\end{array}\Vert }_Q^2+\sum _{i=1}^{N_c-1}{\Vert \begin{array}{c}\Delta U(k+i|t)\end{array}\Vert }_R^2+\rho \ast {\epsilon }^2\text{\hspace{1em}(11)}$$
式中，$N_p$为预测时域，
$N_c$为控制时域，
$Q,R,\rho$为权重系数，
$\epsilon$为松弛因子，加入松弛因子是为了防止求解过程出现无可行解的情况，
$\eta (k|t)$为系统状态量，
$\Delta U(k|t)$为系统控制量。
第一项表示控制系统对期望轨迹的跟踪能力，第二项表示控制系统控制量的约束情况。

约束条件设计

实际过程中，受到实际控制过程中，不同控制器之间的控制量的可控制范围不同，大部分情况下控制量可以表达为如下形式：
$$\begin{gathered} u_{min}(t+k)\le u_{t+k}\le u_{max}(t+k) \\ k=0,1,\dots ,N_c-1\text{\hspace{1em}(12)} \end{gathered}$$
控制增量约束表达式：
$$\begin{gathered} \Delta u_{min}(t+k)\le \Delta u_{t+k}\le \Delta u_{max}(t+k) \\ k=0,1,\dots ,N_c-1\text{\hspace{1em}(13)} \end{gathered}$$

将式(12)，(13)转换为以下形式：
$$U_{min}\le A\ast \Delta U_t+U_t\le U_{max}$$
式中，$U_{min}，U_{max}$分别为控制时域内的控制量最小值、最大值集合。
将目标函数转化为标准二次型并结合约束条件，可以解决如下形式的优化问题：
$$J(\xi (t),u(t-1),\Delta U(t))=[\Delta U(t{)}^T,\epsilon {]}^T\ast H_t\ast [\Delta U(t{)}^T,\epsilon ]+G_t\ast [\Delta U(t{)}^T,\epsilon ]\text{\hspace{1em}(14)}$$
式中，
$$H_t=\left[\begin{array}{ccc}{\Theta }_t^T\ast Q\ast {\Theta }_t+R& & 0\\ 0& & \rho \end{array}\right]$$，
$$G_t=\left[\begin{array}{ccc}2\ast e_t^T\ast Q\ast \Theta & & 0\end{array}\right]$$，
$e_t$为预测时域内的跟踪误差。
每个空置周期内对式(14)求解后，得到了一组控制时域内的一系列控制输入增量：
$$\Delta U_t^{\ast }=[\Delta u_t^{\ast },\Delta u_{t+1}^{\ast },\dots ,\Delta u_{t+N_c-1}^{\ast }{]}^T\text{\hspace{1em}(15)}$$
取控制序列的第一个控制增量与上一时刻的控制量作用于系统，
$$u(t)=u(t-1)+\Delta u_t^{\ast }\text{\hspace{1em}(16)}$$
进入下一周期后，重复上述的计算，即可实现对车辆的轨迹跟踪控制。