0 引　言

作为无人驾驶系统的一部分，泊车系统的出现能够有效地解决城市道路停车难的问题。目前，在泊车过程中，主要包括车位探测、路径规划与轨迹跟踪^[1-2]。轨迹跟踪是自动泊车的最后一个环节，也是研究重点之一。轨迹跟踪根据控制器规划和实时反馈车辆状态，能够使车辆稳定、准确地跟踪参考轨迹。由于车辆具有欠驱动的非完整约束系统，所以设计合适的控制器完成车辆泊车轨迹跟踪是一个值得研究的问题。

模型预测控制(model predictive control, MPC)具有前馈-反馈的结构，可以处理带有约束的非完整系统^[3]。杨博等人^[4]通过多项式进行路径规划，加入了防碰撞和防侧翻约束条件，利用传统MPC设计出避障路径跟踪控制器，提高了避障换道过程中的安全性。PAOLO F等人^[5-6]通过建立三自由度的车辆非线性模型，再使用MPC方案，控制效果较好。胡远志等人^[7]针对泊车场景使用了MPC方法，同时加入动态障碍物，对车辆停车避障进行研究，增加了泊车安全性。顾青等人^[8]对自动泊车路径跟踪采用非线性MPC方法，比起MPC跟踪精度和稳定性都将能有所保证，在实时性方面性能有所降低，但在可接受范围内。

传统MPC控制方案应用到车辆泊车轨迹跟踪时，第一，没有考虑增量约束，这会影响轨迹跟踪的稳定性和实时性。第二，在泊车转向过程中存在转向角突变的情况，这会导致轨迹跟踪精度降低。

针对传统MPC控制方案存在的问题，本文设计了一种基于增量MPC轨迹跟踪控制方案。首先建立车辆二自由度运动学模型及约束，将状态量和控制量改写为状态增量和控制增量形式，设计增量MPC控制器，提高了轨迹跟踪实时性和稳定性；其次，利用卡尔曼滤波算法对前轮转角进行优化处理，提高了轨迹跟踪的精度。最后通过搭建Matlab和Carsim联合仿真环境验证了在平行泊车和垂直泊车两种工况下所设计控制器和转向角优化策略的有效性。

1 车辆运动学模型及约束

车辆运动学模型如图1所示，在泊车过程中车辆属于低速运动，可以忽略轮胎侧滑对轨迹跟踪控制的影响^[9]。因此选择车辆运动学模型设计轨迹跟踪控制器。

由于车速较低时车辆后轴横向速度可以近似为零，将车辆后轴中心设置为车辆轨迹点，车辆运动学模型表示为：

式中：x、y——以后轮车轴为基点的横、纵坐标；

v——当前车辆速度；

l——前后车轮轴距；

$\varphi $——横摆角，车体径向轴与x轴正向间的夹角；

$\delta $——前轮转角；

$({X_r},{Y_r})$——车辆后轴中心在惯性坐标系下的坐标。

本文以车辆参数以D级车Sendan为原型，整车参数如表1所示。

泊车过程中，约束条件主要考虑的是车辆前轮偏转角及其增量约束和横向速度及其增量约束^[10]。前轮转向角约束$\delta $设置为$ \pm $39°，推导出前轮转向角增量$\Delta \delta $为$ \pm $1.35°。由于泊车过程车辆行驶速度较低，将车辆速度设置为$ \pm $5 $ {\text{m}}/{\text{s}} $，则将$\Delta v$为速度增量设置为$ \pm $0.1 $ {\text{m}}/{\text{s}} $。

2 轨迹跟踪控制器 2.1 控制系统整体框架

在泊车轨迹跟踪控制器框图如图2所示，增量MPC控制器根据参考轨迹和车辆当前状态计算出前轮转角和车速，再经过卡尔曼滤波算法对车辆前轮转角进行优化处理，最后将优化后的转向角输入到Carsim车辆模型中进行控制。Carsim车辆模型再将计算后的车辆状态反馈到增量MPC控制器中进行处理，如此往复循环，从而完成车辆的泊车轨迹跟踪控制。

2.2 增量模型预测控制器设计

式(1)中所建立的车辆运动学模型是非线性的，为设计合适的增量模型预测控制器，将非线性车辆运动学模型线性化再转化成二次规划问题求解。

首先将式(1)看作一个控制量为$ {\boldsymbol{u}} = {[v,\delta ]^{\text{T}}} $、状态量为${\boldsymbol{ \chi}} = {[x,y,\varphi ]^{\text{T}}} $的系统，对于给定的参考轨迹，用r代表参考量，用t表示某一时刻，则式(1)改写为下式：

其中${{\boldsymbol{u}}_r}(t) = {\left[ {{v_r},{\delta _r}} \right]^{\text{T}}}$，${{\boldsymbol{\chi}} _r}(t) = {\left[ {{x_r},{y_y},{\delta _r}} \right]^{\text{T}}}$。

当对车辆施加控制时${\boldsymbol{u}}(t) = {{\boldsymbol{u}}_0}$，得到一条实际状态轨迹点$\dot {\boldsymbol{\chi }}(t)$，且满足下式：

对式(3)在轨迹点$ ({\chi _0},0) $处一阶近似泰勒展开并忽略高阶项，保留一阶项^[11]。得到：

将式(5)与式(4)相减，同时，得到新方程：

采用前向欧拉法对式(6)进行离散化处理，故离散的状态空间方程为:

式(7)中：

T为采样时间。由式(7)可知，输出$ {\boldsymbol{y}}\left( k \right) $的线性依赖于$\tilde {\boldsymbol{\chi}} (k)$，则

为了将该数学模型用于增量MPC控制器^[10]。定义控制增量$ \Delta {\boldsymbol{u}}(k) = \tilde {\boldsymbol{u}}(k) - \tilde {\boldsymbol{u}}(k - 1) $，状态增量$\Delta {\boldsymbol{\chi}} (k) = \tilde {\boldsymbol{\chi}} (k) - \tilde {\boldsymbol{\chi}} (k - 1)$，则将式(7)和式(8)改为增量状态可将方程如下：

简写为：

将最新的测量值作为初始条件，设定预测时域为$ \mathop N\nolimits_p $，控制时域$ \mathop N\nolimits_c $，且$ \mathop N\nolimits_c \leqslant \mathop N\nolimits_p $。基于式(10)预测未来$\mathop N\nolimits_p $个时刻的系统状态，将预测时域内的系统输出写成：

同时预测时刻内的控制增量也写成如下形式：

根据模型预测控制原理^[12]。通过推导得到系统在第k时刻的预测输出方程：

式(13)中，$ \boldsymbol{\psi}_k=[\tilde{\boldsymbol{C}}\tilde{\boldsymbol{A}}(k),\tilde{\boldsymbol{C}}\tilde{\boldsymbol{A}}(k)^2,\cdots,\tilde{\boldsymbol{C}}\tilde{\boldsymbol{A}}(k)^{N_p}]^{\text{T}} $，

控制器要保证车辆能快速平稳地跟踪上参考轨迹，在设计轨迹跟踪控制器时，采用如下的目标函数进行求解：

式中：$ {\boldsymbol{Q}} $——输出权重矩阵；

${\boldsymbol{R}}$——控制增量权重矩阵；

$ \rho $——松弛因子权重；

$\varepsilon $——松弛因子。

将式(14)改写为矩阵向量的形式为：

将式(12)带入式(15)得到：

式(16)中，预测时域内的跟踪误差为${\boldsymbol{E}}(k) = {\boldsymbol{\psi}} (k)\xi (k)$，权重矩阵分别为$ \tilde {\boldsymbol{Q}} = {{\boldsymbol{I}}_{{N_P}}}{\boldsymbol{Q}} $，$\tilde {\boldsymbol{R}} ={{\boldsymbol{I}}_{Nc}}{\boldsymbol{R}}$。

将目标函数转为为标准二次型为：

其中

那么增量MPC的每一步求解就可以转换成带约束的二次规划问题：

在式(18)中，$ {{\boldsymbol{U}}_t} = \left[ {\begin{array}{*{20}{c}} {\tilde {\boldsymbol{u}}(k - 1)}& \cdots &{\tilde {\boldsymbol{u}}(k - 1)} \end{array}} \right]_{{N_c} \times 1}^{\text{T}} $，$ \Delta {{\boldsymbol{U}}_t} $为控制增量约束上下限集合，${{\boldsymbol{Y}}_n}$为软约束输出，可以通过松弛因子调整其范围，${{\boldsymbol{A}}_I}$为单位下三角矩阵。在每一轮控制周期中，求解式(18)可以得到一系列控制增量序列：

将该序列中的首个元素作为实际控制输入增量作用于车辆模型，同时重新计算车辆状态，重述上述过程，实现对车辆的轨迹跟踪控制。

2.3 转向角优化策略

在将车辆运动学模型线性化过程中，由于忽略了泰勒公式的高阶部分，这使得增量MPC的跟踪精度有所下降。同时为了解决车辆角速度存在突变的情况，需要计算合适的转向角。因此通过设计卡尔曼滤波算法对转向角进行优化校正，以提高控制器稳定性^[13-14]。

设计离散的状态观测方程分别如下：

式中：${\boldsymbol{x}}(k)$和${\boldsymbol{y}}(k)$、${\boldsymbol{u}}(k)$——$k$时刻的状态变量和输出变量、控制输入量；

${\boldsymbol{f}}({\boldsymbol{x}}(k),{\boldsymbol{u}}(k))$和${\boldsymbol{g}}({\boldsymbol{x}}(k),{\boldsymbol{u}}(k))$——当前车辆状态函数；

${\boldsymbol{w}}(k)$和${\boldsymbol{v}}(k)$——系统噪声和测量噪声；

$ {{\boldsymbol{A}}_k} $——状态矩阵；

$ {{\boldsymbol{B}}_k} $——控制矩阵；

${{\boldsymbol{H}}_k}$——状态矩阵。

二者协方差矩阵分别为${\boldsymbol{Q}}$、${\boldsymbol{R}}$。

卡尔曼滤波算法分为预测转向角部分和测量更新转向角部分：

预测部分是通过$k$时刻的${\boldsymbol{x}}(k)$预测下一时刻的${\boldsymbol{x}}(k + 1|k)$：

通过式(21)可知，由于系统噪声存在，求解出预测误差协方差矩阵：

测量更新部分通过卡尔曼增益矩阵${\boldsymbol{K}}$对${{\boldsymbol{x}}_{k + 1|k}}$进行修正得到下一时刻估计值${{\boldsymbol{x}}_{k + 1}}$：

最后通过更新下一时刻协方差矩阵表示估计误差以用于下一次计算：

通过式(22)~(26)在每个时刻进行计算，对转向角进行了优化处理。定义车辆输出结果的横向误差和航向误差如图3所示，作为评判跟踪控制效果的标准^[8]。

横向误差${e_d}$是$ \mathit{{M}{M}}_0 $的长度，航向误差${e_h}$为车辆航向和参考轨迹在$ \mathit{{M}}_0 $点处的航向之差。

3 仿真与分析 3.1 车辆及控制器参数设置

通过Simuilink和Carsim进行联合仿真，在Simulink中搭建增量MPC控制器和加入卡尔曼滤波算法，在Carsim中搭建仿真车辆模型。仿真软件为Matlab2018a，搭载仿真环境的处理器为AMD Ryzen74800U，内存为16GB。

分别设计A、B两种控制器对泊车轨迹进行仿真分析。控制器A用增量MPC控制器进行轨迹跟踪，控制器B则采用MPC控制器，两个控制器参数如表2所示：

3.2 平行泊车工况

设置车位参数，车库长6.4 m，车位宽2 m，在Carsim中搭建泊车环境，仿真模型如图4所示。

图4中可以看出，在Carsim仿真中车辆模型能够完成平行泊车。Simulink输出轨迹对比，如图5所示。

对图5结果分析，控制器A下的轨迹与参考轨迹大致相同，说明控制器A能够使车辆按照规划的轨迹行驶。而在控制器B下的轨迹跟踪，由于将非线性模型线性化，省略了泰勒公式的高阶部分，随着误差的累积，会出现控制偏差。

图6展示了两个控制器的横向误差和航向误差，在图6(a)中，控制器A下的轨迹跟踪最大横向误差为0.016 m，控制器B最大横向误差为0.079 m。在图6(b)中，控制器A最大航向误差为0.15°，而控制器B最大航向误差为1.83°。控制器A整体航向误差波动小，比控制器B效果更好。

图7展示了两种控制器的在平行泊车工况下经过卡尔曼滤波后转向角输出结果。由图6得出，车辆转向角经卡尔曼滤波之后有明显的优化，更符合真实物理情况。通过对比图7(a)，控制器A转向角优化前缓冲时间为0.57 s，优化后为1.521 s，适当延长的缓冲时间，能够提高车辆在轨迹跟踪过程的稳定性，安全性和舒适性。

图8展示位两个控制器的在泊车过程中每个控制周期的计算时间，控制器A的平均运算时间为0.019 s，而控制器B的平均运算时间为0.021 s。

表3为平行泊车轨迹跟踪性能对比表，能够看出，在平行泊车工况下，控制器A的性能指标比控制器B的更加稳定，改善效果明显。

3.3 垂直泊车工况

在垂直泊车中，通过设置车位长为5 m，车位宽为2.6 m。在Carsim中搭建仿真泊车环境如图9所示。

图9中可以看出，在Carsim仿真中车辆模型能够完成垂直泊车，Matlab/Simulink输出轨迹如图10所示。

对图10输出结果分析，当车辆进行垂直泊车时，两种控制器的跟踪精度相近，都能够按照期望的参考轨迹行驶。在控制器A基本上可以准确完成轨迹的跟踪，同时精度较高。而控制器B的控制精度则略有下降。

图11展示为垂直泊车工况下的两种误差结果。在图11(a)中，控制器A的最大横向误差为0.01 m，最大航向误差为0.742°，在图11(b)中，控制器B的最大横向误差为0.08 m，最大航向误差为1.194°。

图12显示的车辆前轮转角在两个控制器作用下的对比输出，和平行泊车工况相似，控制器A比控制器B更加平稳，同时前轮转角突变的情况也得到良好的改善。控制器A优化后的缓冲时间为1.04 s，控制器B优化后的缓存时间为3.05 s，控制器A相比与控制器B在转向过程中响应更快。

图13展示了两个控制器的每个控制周期的计算时间，控制器A平均运算时间为0.009 s，而控制器B的平均运算时间为0.012 s，可以看出垂直泊车工况下控制器A相对于控制器B能减少部分运算时间，同时稳定性也得到良好的控制。

表4为垂直泊车轨迹跟踪性能对比表，可以看出，在控制器A在垂直泊车工况下的各方面性能都比控制器B的跟踪精度更高。结合表3，说明控制器A在两种泊车工况下，控制效果相比与控制器B更好，验证了增量MPC控制方案的可靠性。

4 结束语

本文针对传统MPC控制方案在泊车轨迹跟踪中出现的问题，设计了一种基于增量MPC的控制方案。首先，建立车辆二自由度运动学模型并将其线性化得到车辆轨迹跟踪的线性误差模型，再根据MPC原理设计增量MPC控制器；其次，在此基础之上，增设转向角优化策略。最后通过仿真实验表明，增量MPC控制方案相比传统MPC控制方案，降低了横向误差和航向误差，提高了实时性。同时转向角优化策略在转向过程中优化了前轮转角曲线突变的情况，提高轨迹跟踪精度和鲁棒性。

文中所设计的增量MPC控制方案适用于跟踪精度较低、实时性较差的泊车场景。在之后的研究中，将考虑车辆外部扰动等问题所带来的影响，在此基础上进一步完善本文所提出的控制方案，以提高轨迹跟踪控制性能。