自动泊车系统路径规划与跟踪控制方法研究

黄江 魏德奎 秦良艳 李攀 杜军

（1.重庆理工大学，重庆 400054；2.重庆邮电大学，重庆 400065）

主题词：自动泊车 滑模控制 路径规划 五次多项式 路径跟踪

1 前言

目前，自动泊车系统的研究内容主要包括路径规划、路径跟踪、车位探测与定位3个方向。针对泊车路径规划问题：文献[1]、文献[2]利用2段相切的圆弧进行路径规划，但规划路径存在曲率不连续的问题；文献[3]指出，车速和转向盘转角角速度恒定时，车辆行驶轨迹将会固定，并在此基础上对双圆弧轨迹进行改进，提出了基于双匀速轨迹的平行泊车路径规划方法，解决了传统的双圆弧泊车路径曲率不连续的问题；文献[4]～文献[6]对泊车避障环境进行分析，建立了约束方程，并采用B样条曲线进行路径规划设计，所得路径满足曲率连续要求，但是约束条件和路径曲线参数较多，求解过程较为复杂；文献[7]为避免自动泊车过程中进入泊车死区，设计了满足安全驶入车位的泊车前进水平线，并以此作为起始区域进行泊车规划，保证了泊车的安全性，但没有考虑曲率连续性要求的问题。针对泊车路径跟踪问题：文献[8]～文献[10]采用模糊控制的方法进行泊车控制；文献[11]基于非时间参考系路径跟踪控制设计了非时间参考的定点跟踪控制方法，提高了泊车的精确度；文献[12]利用二次型最优控制方法进行了泊车控制器设计，完成了对泊车轨迹的有效跟踪。

自动泊车过程要求泊车轨迹曲率连续、泊车控制器控制精度高，以实现泊车过程的连贯与安全。本文以逆向行驶的方法进行泊车路径的初步规划，并利用五次多项式对路径进行平滑处理，同时，为了提高平行泊车控制器的控制精度，以滑模控制为基础，结合非时间参考控制方法，提出一种基于非时间参考的滑模控制路径跟踪方法，以实现泊车系统高精度的路径跟踪效果。

2 车辆运动学模型

汽车的外形尺寸直接影响泊车过程中避障的成功率，为了在泊车避障路径规划过程中留下一定裕度的同时简化汽车运动学分析，以矩形代替车辆外形，其长度和宽度分别为汽车外形尺寸的最大长度和最大宽度，如图1所示。通常，泊车过程中汽车的速度维持在较低水平，车辆不会出现明显的侧倾或俯仰运动，因此简化模型不会对分析结果产生较大影响。

图1 车辆外形简化模型

图1中，A、B、C、D为汽车外形顶点，车辆参数如表1所示。

表1 车辆外形参数

由于后轮为非转向轮，故可选取汽车后轴中心点为参考点，以其运动轨迹表示汽车的行驶轨迹，以车位左后方顶点d为原点，以平行车位方向为X轴，以车位宽度方向为Y轴建立坐标系，建立运动学模型：

式中，x(t)、y(t)分别为汽车后轴中心点横、纵坐标；φ(t)为汽车航向角；δ(t)为前轮等效转角；v(t)为车速。

3 平行泊车路径规划算法

3.1 平行泊车初步路径规划

汽车从泊车起始点O1泊车至终点O4的过程如图2所示，可分为3个阶段：首先假定车辆初始姿态平行于车位方向，目标车辆从初始位置O1点处以R1为半径、M1为圆心行驶θ角度后到达O2点；接着沿直线段O2O3行驶至O3点；最后从O3点处以R2为半径、M2点为圆心行驶θ角度后到达O4点，泊车完成。

图2 泊车过程

为方便计算，并减小可泊车车位长度，本文将R2和R1设定为车辆后轴中点的最小转弯半径Rmin，Rmin由转向运动学关系得到：

利用逆向行驶的方法推算泊车起始点的范围，根据避障条件确定O4、O3坐标，只需调整O2O3段的长度，便可求得O1的坐标。

可得O4点坐标为：

式中，Lsafe1为汽车泊车终止状态与后方障碍物之间的安全距离，根据参考文献[3]，本文取Lsafe1=0.2 m。

如果车辆能够从O4沿轨迹O3O4安全驶出车位，则车辆端点B与车位端点a不能发生碰撞，即当B点绕M2转动至B1点时，B1与a点间的距离应大于安全距离Lsafe2，本文取Lsafe2=0.2 m，如图3所示。

图3 B点避障分析

根据几何关系可推算得到最小车位长度Lmin为：

当B点绕M2转动θ角度至B1点时，后轴中心同样从O4转动θ角度至O3：

Lmin和θ可根据车辆参数求出，从而得出O3点的坐标：

分析可知，O3点处车辆航向角φ=θ，O2O3段轨迹斜率为tanθ。O1点的选取范围线段与O2O3平行，故其斜率同样为tanθ。

O1和O2点下限位置点的选取应保证车辆沿O1O2行驶时，车身右侧不与车位顶点a发生碰撞。如图4所示，当汽车沿直线段O2O3行驶至汽车后轴延长线与车身右侧的交点I点到达图3中B1点位置时再转弯行驶即可。这种处理方法求得的O2点虽然不是O2点纵坐标最小值的位置，但是简化了运算分析量，故以该位置作为O2点的纵坐标下限值位置点，则此状态下的O1点即为泊车起始区域的下限坐标值所在点。

图4 I点避障分析

可推算O1点下限坐标为：

式中，O1minX和O1minY分别为O1点下限位置点的横、纵坐标。

如图5所示，汽车沿轨迹O1O2行驶时，需要保证车身左侧端点A不与道路左侧发生碰撞，即轨迹AA1的纵坐标的最大值应小于道路宽度Lroad。

O1点的纵坐标O1y需满足：

由此可得O1点纵坐标的上限值O1maxY为：

图5 A点避障分析

根据O1点选取范围段的斜率和式（7）推算得到O1点上限值横坐标O1maxX为：

则O1点的选取范围为：

式中，O1X、O1Y分别为O1点横、纵坐标，且O1X∈[O1minX,O1maxX]。

分析可知，若车辆的初始姿态平行于X轴，则车辆在O1点的选取范围线段右方时，均可倒车行驶至曲线O1maxO1min上的点后再进入泊车轨迹。所以实际泊车起始点的选取范围应为3条曲线所包含的区域：

实际泊车起始点的选取范围如图6所示。

图6 泊车起始点可选区域

3.2 路径平滑处理

采用逆向行驶方法得到的泊车路径存在曲率不连续的问题，故需进行平滑处理。传统的路径平滑处理方式是采用反正切函数和Sigmoid函数，不能满足轨迹起点零曲率或小曲率的要求，而B样条曲线处理方式的求解参数数量较多，计算量较大，本文采用的五次多项式路径平滑处理方法在满足轨迹起点零曲率或小曲率要求的同时，表达式更简洁、求解参数数量更少、计算量更小。为提高路径曲线的平滑度，需要适量增加最小车位长度，同时泊车起始点行驶范围需向右平移一定距离得到新的起始点坐标O′1(X1,Y1)。本文设定最小车位长度为6.8 m，泊车起始点向右移动1 m。假设泊车路径表达式为：

式中，K1～K6为五次多项式各项系数。

泊车起始状态与泊车终止状态的车辆姿态应与X轴方向平行，所以泊车轨迹方程在泊车起始点与终点处的导数为0，可建立如下约束方程：

为了使车辆初始状态前轮等效转角为0，车辆在起点处泊车轨迹的二阶导数需为0，则：

为避免泊车过程中车身与车位顶点a发生碰撞，车辆行驶轨迹需要经过O3点。而为了使得路径更加平滑，泊车轨迹曲率K应满足：

为满足轨迹的曲率约束要求，需要使O3点纵坐标减小LO3。根据多次仿真测试，当LO3=0.05 m时，可得到满足曲率约束条件和避障条件的泊车轨迹方程，调整后的点坐标为(X3,Y3)。由此可得到约束方程：

4 路径跟踪控制器设计

4.1 非时间参考系路径跟踪系统误差分析

非时间参考路径跟踪控制方法最早应用于机器人路径跟踪系统[13]，通过引入非时间参考量代替时间参考量，解决传统路径跟踪方法中将期望轨迹视为时间函数的问题。

非时间参考量需要满足随时间单调递增的要求，故本文选取非时间参考量f=-x(t)，求导可得：

因为泊车时的速度为负，泊车过程中车身航向角φ∈[-0.5π,0.5π]，所以ḟ＞0 ，满足非时间参考量随时间变化单调递增的要求。非时间参考的车辆运动学方程可表示为：

设目标路径函数为yr=F(xr)，若汽车能够无误差地跟踪，当车辆后轴中心点横坐标为xr时，相应的纵坐标为yr、航向角为φr、前轮等效转角为δr。根据轨迹函数的一次导数和二次导数，结合式（22）可得：

在已知目标路径函数的前提下，可通过式（23）和式（24）计算得到横坐标为xr下所对应的车辆理想状态参数。

选取车辆路径跟踪误差为：

式中，y、φ为汽车后轴中心点横坐标为x时所对应的汽车实际状态参数，xe、ye和φe分别为纵向误差、横向误差和航向角误差。

则以非时间参考量f=-x(t)参考的路径跟踪偏差为：

4.2 滑模变结构路径跟踪控制器设计

滑模变结构控制是自动控制系统设计方法中重要的研究分支，因算法简单、鲁棒性好、适应性强、实用性广而被广泛运用。滑模变结构控制的核心是设计出合适的切换函数和变结构控制律，使系统状态轨迹线能快速到达设计的切换面进行滑动模态运动[14]。

设定系统的参考状态量为：

对上式求导：

设计滑模切换函数s为：

其中k1＞0，对上式求导可得：

定义Lyapunov函数为：

采用指数趋近律来设计滑模变结构控制器，以改善趋近运动的动态品质，取：

式中，k3＞0；k2＞0；sgn(s)为符号函数。

可得到：

当且仅当s=0时等号成立，根据连续系统滑动模态的存在性和可达性[14]可知，系统的滑动模态存在且可达，即系统s在趋近律式（32）作用下可到达平衡点s=0。

结合式（30）和式（32），可设计泊车系统转向控制量，即前轮等效转角为：

由于指数趋近律设计的滑模控制器中使用了符号函数sgn(s)，系统会出现不连续的开关特性，使得滑模变结构系统在滑动模态下存在高频的振动现象，从而导致路径跟踪过程中控制器会出现较大波动，继而影响泊车控制的精确度。为抑制振动，本文采取饱和函数sat(s/Δ)来代替符号函数sgn(s)，以保证系统的输入在切换面附近光滑连续：

式中，Δ为边界层厚度。

对控制器参数k1、k2、k3和Δ进行调试修改，得到合理的参数取值，便能得到较好的控制效果。

5 仿真分析

5.1 路径规划仿真

在MATLAB中输入车辆参数，且假定道路宽度为Lroad=4 m，采用逆向行驶的方法得到初步规划的泊车起始点选取范围和主要路径点坐标。然后将泊车起始点向右平移1 m后得到新的起始点选择区域为：

式中，X∈[8.443 8,10.775 8]。

分别选择泊车起始点上限值(O1maxX+1,O1maxY)、下限值(O1minX+1,O1minY)和中间点(0.5(O1minX+O1maxX)+1,0.5(O1maxY+O1minY))进行求解计算，得到如图7所示的3个起始点对应行驶轨迹。分别对3条规划轨迹进行求导和求曲率计算，结果分别如图8和图9所示。

图7 路径规划仿真结果

图8 规划路径导数

图9 规划泊车路径曲率

由图7～图9可知，求解出的3条规划轨迹都能够顺利连接泊车起始点和终点，未与道路边界和车位边界发生碰撞，轨迹曲线的导数均为0，曲率均在-0.231 6～0.231 6 m-1范围内，泊车起始点处曲率均为0，满足避障条件、泊车姿态条件、最大曲率约束条件和初始位置等效前轮转角的要求。

5.2 路径跟踪控制器仿真

为了验证结合非时间参考系设计的滑模路径跟踪控制器在泊车系统路径跟踪控制应用中的有效性，本文利用MATLAB/Simulink搭建了汽车运动学模型和路径跟踪控制器，选择前文的轨迹3作为参考轨迹，设定车辆初始姿态平行于X轴，车速为-1 m/s，选定控制器参数k1=25、k2=1.1、k3=0.01、Δ=0.5进行仿真验证，结果如图10～图12所示。

图10 速度恒定时路径跟踪仿真结果

图11 速度恒定时路径跟踪效果

由图10可知，利用本文设计的控制器进行泊车控制，当泊车速度恒定时，车辆能够顺利进入车位且未与四周障碍物发生碰撞，满足安全性要求。

由图11可知，控制器进行平行泊车路径跟踪时，后轴中心点的运动轨迹和车辆的航向角与规划轨迹基本重合，表明车辆能够顺利跟踪目标路径和目标姿态。

图12 速度恒定时路径跟踪误差

由图12可知，泊车路径跟踪精度较高，泊车终点处的航向角为-0.004 6 rad，可近似认为车身姿态平行于X轴，满足泊车结束时姿态平行于车位方向的要求。

在实际泊车控制中，车辆速度难以维持在一固定值，但通常在一较低的数值附近波动。为验证泊车控制器在速度波动的情况下的有效性，本文选取速度v=(0.1sin(5t)-1) m/s进行仿真验证，仍选择轨迹3为参考轨迹。车辆初始姿态平行于X轴，仿真结果如图13～图15所示。

图13 速度波动时路径跟踪仿真结果

图14 速度波动时路径跟踪效果

图15 速度波动时路径跟踪误差

由图13～图15可知，当车辆泊车速度波动时，控制器仍能够使车辆安全地完成泊车车辆轨迹和姿态仍具有较好的跟踪效果。由图15可知，泊车终点处的车身航向角为-0.004 7 rad，满足泊车终点姿态要求。虽然误差曲线因为车速的变化在跟踪过程中出现了明显波动，但跟踪过程中误差均较小，可认为跟踪精度较高。

综上所述，当车速恒定和波动时，控制器均能准确地跟踪目标轨迹，具有较强的适应性。

6 结束语

本文首先利用逆向行驶的方式，推算出泊车起始点的选择范围，并利用五次多项式对初步规划路径进行平滑处理，以满足泊车系统对轨迹曲率连续的要求，并结合非时间参考系路径跟踪方法和滑模变结构控制设计了基于平行泊车的自动泊车系统路径跟踪控制器，仿真结果表明，控制器具有较好的跟踪效果，当速度波动时仍能较好地跟踪规划轨迹。