基于MPC的自动泊车控制方法

杨述斌，刘 寒，蒋宗霖

（武汉工程大学 电气信息学院 智能机器人湖北省重点实验室，武汉 430205）

随着人们生活水平的提高，汽车成为人们出行重要的交通工具，城市汽车数量的增长使得停车成为人们日常面对的首要难题，越来越窄的停车区域，缺少经验的新手司机，也是造成停车事故频发的主要原因，因此，开发自动泊车系统成为国内外学者不断研究的目标[1]。

自动泊车控制由轨迹规划和轨迹跟踪控制来实现，随着许多规划方法和新型的控制方法被应用到泊车系统中[2]，如何将汽车这种复杂的非线性系统转变为满足功能要求的精确模型成为越来越多学者追求的目标。文献[3]中使用改进型Fuzzy-PID控制器对速度进行控制，能够将幅值控制在一定的范围内，完成泊车过程，但未考虑到实际操作中驾驶员和乘客舒适性问题；文献[4]中对车辆控制分为了横向控制和纵向控制，都是使用PID控制器对误差量进行修正，这样不基于车辆运动学和动力学模型的考虑在进行仿真时往往会导致在有阶跃干扰量出现时无法及时作出响应[5]。

本文用多段式泊车曲线作为参考轨迹，设计了模型预测控制器，加入干扰量作为外部扰动，使用仿真软件CarSim对所设计的轨迹跟踪控制器进行仿真。

1 车辆运动学建模

由于自动泊车具有速度低的特点，汽车运动学特性远大于动力学特性，此时可以忽略汽车侧向力，满足阿克曼转向原理，车辆后轴中心点的运动轨迹可以表征车辆的横向运动特性，前轴作为转向轴可以反映侧向运动特性，以后轴中心坐标和前轴等效偏转角度为对象，在大地坐标系上的运动模型如图1所示。

图1 车辆运动学模型Fig.1 Vehicle kinematics model

图中，（Xr，Yr）和（Xf，Yf）代表车辆后轴轴心和前轴轴心的坐标，φ代表车体的横摆角，δ代表前轮偏角，Vr代表车辆后轴中心速度，Vf代表车辆前轴中心速度，L代表轴距。此处假设转向过程中车辆质心侧偏角保持不变，即车辆瞬时转向半径与道路曲率半径相同，且车辆作为刚体，在大地坐标系下，泊车过程的车辆运动学方程式形式如下：

2 基于三阶反正切函数的泊车路径规划

自动泊车轨迹曲线与反正切函数曲线拟合度高，且泊车轨迹非线性，二次多项式不贴切泊车轨迹，而四次及高次多项式使计算量复杂，因此本次实验选择三次多项式和反正切函数来确定参考轨迹，并加入一个非线性扰动来更好地描述泊车过程中的不可确定性，设计的平行泊车轨迹模型如下[6]：

式中：x，y 为车辆停车轨迹的坐标；a1，a2，a3，a4，a5，a6为未知的常数，通过逼近泊车过程所有样本点，来确定泊车轨迹，从而确定常数的值。

3 基于误差模型的预测控制算法

针对如何有效实现自动泊车系统路径跟踪控制问题，本文提出了一种基于误差模型的预测控制方法，根据车辆运动学模型，系统可以看做一个输入为 u（v ω）和状态量为 X（x y φ）的控制系统。 对于给定的参考轨迹，由车辆的运动轨迹描述，插值后每一个点都满足运动学方程，在参考轨迹点采用泰勒级数展开并忽略高阶项，得到：

将式（4）与一般形式相减，可以得到误差模型方程式：

为了能够将该模型应用于模型预测控制器，对上式进行离散化处理：

由式（5）设计目标函数，保证车辆泊车能够平稳准确地追踪期望轨迹，加入对系统状态量的偏差和控制量的优化，采用如下形式的目标函数：

式中：Np为预测时域；Nc为控制时域；ρ为权重系数；ε为松弛因子。本模型目标函数中用控制增量取代控制量并且加入松弛因子，这样不仅能对控制量进行直接的限制，也能防止执行过程中出现没有可行解的情况，在目标函数中，需要计算未来一段时间系统的输出[7]。

考虑该系统的控制量约束和控制增量约束：

本文约束条件由横纵向跟踪能力测试得到，纵向速度约束考虑对车辆输入 5 m/s、7.5 m/s、10 m/s的阶跃期望速度，达到期望速度后再以泊车场景制动力0.8 MPa进行制动，记录加减速度时间，测试结果如表1所示，横向跟踪能力实验考虑泊车过程车辆前轮从左极限位置连续转向至右极限位置，转向最大角度为25°，转向时间为5 s。

表1 自动泊车纵向跟踪能力测试Tab.1 Automatic parking longitudinal tracking ability test

目前对于自动泊车速度控制还要考虑驾驶员和乘客的舒适性，一般要求加速度大小不应超过2 m/s2，因此为了使跟踪控制更为平稳，本文设计约束条件为

将目标函数转化为标准二次型形式并结合约束条件，解决以下优化问题

4 控制效果仿真

根据上文推导的车辆运动学模型，按照方程在SimuLink里搭建运动学模型，CarSim中车辆的基本参数设置：车型为CS B-Class-Hatchback，轴距L＝2.7 m，最大前轮转角为34°，并设置泊车场景，CarSim车辆模型S函数，结合日常泊车工况，设定期望车速为5 m/s，仿真结果如图2所示。

图2 CarSim自动泊车仿真动画Fig.2 CarSim automatic parking simulation animation

由图3可知，控制的目标车辆能够从初始位置沿着参考路径进入目标车位，车辆与周围的车辆没有发生碰撞，在停车时没有与后车发生碰撞，因此设计的参考路径是合理的。

自动泊车轨迹跟踪中一个重要的性能指标就是驾驶员的舒适性，在控制系统中，主要由加速度大小决定，根据IOS-2631可以知道加速度与舒适性的关系如表2所示。

表2 加速度大小与舒适性关系Tab.2 Relationship between acceleration and comfort

本文设计的模型预测控制器通过求解最优解控制，并在约束条件中设置每次控制序列范围，在整个过程中加速度幅值范围保持在 0.2～0.6m/s2，如图 3 所示，文献[3]中采用改进型Fuzzy-PID对纵向速度进行控制，能够使加速度幅值保持在±1 m/s2，对比可知本文设计的控制器能够将加速度峰值控制在更舒适的范围内，同时也可看出幅值震荡控制效果比文献[3]要好。

图3 泊车过程加速度曲线Fig.3 Parking process acceleration curve

自动泊车过程中，路径中会出现石子、坑洼等不可控因素，而车辆自带的雷达与摄像头不能完全监控到这些因素，因此考虑干扰量在泊车系统中是有实用性的，泊车分为纵向速度与横向转向角控制，干扰量主要对转向角起一定的影响，本次设计实验，在转向角最大时加入大小约为一个数量级的干扰量，结果如图4～图9所示。

图4 纯路径跟踪侧滚波形图Fig.4 Pure path tracking roll waveform

图5 纯路径跟踪横向速度图Fig.5 Pure path tracking lateral velocity map

图6 纯路径偏航角图Fig.6 Pure path yaw angle diagram

图7 模型预测侧滚波形图Fig.7 Model prediction roll waveform

图8 模型预测横向速度图Fig.8 Model predicts lateral velocity map

从图中可以看出MPC对泊车过程中转角速度有改善作用，纯路径规划在加减速度拐角时会产生较大的波动，而且在有干扰量的时候速度会突变[8]，在后续整个过程中难以恢复，而带有MPC控制器的系统在出现干扰量时MPC会通过求解最优控制序列来减小干扰量对整个系统的偏差，对于速度控制，和车辆侧倾度可以明显看出带有MPC控制器的系统能够较好地避免干扰量对整个泊车系统造成的偏差，在泊车情景中，干扰量会比较小，因此MPC对于车辆控制性能更优。

图9 模型预测偏航角图Fig.9 Model predicting yaw angle map

5 结语

本文主要介绍了基于模型预测控制的自动泊车控制方法，以汽车运动学模型为基础，建立利用模型预测控制器的泊车路径跟踪控制系统，分析了此系统在舒适性上和PID控制器的优点，得出本方法对驾驶员和乘客舒适性更高的结论，分别对纯路经跟踪和带有模型预测控制的路径跟踪进行泊车路径仿真研究，在转向角最大时加入干扰量，对比两种方法的抗干扰能力，得出本方法抗干扰能力更优的结论。