基于滑模控制的自动泊车系统路径跟踪研究

姜立标 杨 杰

(华南理工大学机械与汽车工程学院, 广州 510640)

0 引言

随着科技的不断发展，汽车智能化已经是当前必然趋势。车辆作为一个复杂的非线性系统，随着国内外学者的不断研究，许多规划方法和新型的控制方法被应用到泊车运动控制中[1-10]。

现有研究中，路径规划方法大多使用圆弧直线组合路径，泊车路径曲率不连续，没有考虑车辆转向能力。轨迹跟踪控制器同时控制车速和方向盘转角，一旦车速控制误差较大会直接影响轨迹跟踪效果。在对设计的路径规划方法和控制器进行仿真分析时，简化的车辆运动学模型会造成实际控制器控制效果不佳。

本文以某SUV为研究对象，结合国内外自动泊车研究现状，对路径规划方法中常见的曲率不连续问题进行研究，以解决规划路径曲率不连续、路径规划初始条件严格的问题，并提出基于B样条曲线的路径规划方法和基于趋近律的非时间参考终端滑模路径跟踪控制方法。

1 车辆运动学模型

假设车辆泊车过程为低速运动过程，忽略车轮侧向滑动,车辆在行驶中不出现侧向力，基于阿克曼转向原理建立车辆运动学模型如图1所示。

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

图中，θ为车辆车身与坐标系X轴的夹角，φ为等效前轮转角，l为车辆轴距，vf为车辆前轴中心点的速度，(xf,yf)为车辆前轮轴中心坐标，(xr,yr)为车辆后轮轴中心坐标。

为方便计算，将四轮车简化为虚拟的两轮车，如图2所示。简化后前后轮转角的关系式为

cotα+cotβ=2cotφ

(1)

式中α——外轮转角β——内轮转角

图2 等效前轮转角示意图Fig.2 Equivalent front wheel angle diagram

由实际条件可知，在满足阿克曼转向原理条件下，车辆方向盘转角和等效前轮转角之间存在近似线性的转换关系

φ≈kφ

(2)

由目标车辆的实际测量结果可得k为16.2。

由假设条件可知，车辆运动学模型本身符合非完整性约束条件,即

(3)

由图1可知

(4)

式(4)对时间求导得

(5)

将式(5)代入式(3),可得

(6)

又由图1可得

(7)

将式(7)代入式(6)可得

(8)

联立式(5)、(7)、(8)得

(9)

为简化表达式，使用向量q=(v,ω)T表示车辆的线速度和角速度，其中，v为后轴中心点的速度，其方向垂直于后轴轴线。同时令

(10)

所以，对于前轮转向的车辆，以后轴中点为参考点，其运动学方程为

(11)

车辆位姿可由后轮中点在坐标系中的位置以及车辆航向角来表示，可以描述为p=(x,y,θ)T。则车辆的运动学方程可以表示为

(12)

假设车辆最大等效前轮转角为φmax，后轴中点最小的转弯半径为ρmin，ρmin=tanφmax/l，则对应的角速度为ω≤v/ρmin。

2 基于B样条的泊车路径规划

常规泊车情况指车身方向与道路方向平行，与路基或是车库方向没有夹角。在实际泊车过程中，受驾驶经验以及个人主观因素的影响，驾驶员难以将车身方向调整至与道路方向完全一致，车身总会与道路方向存在一定夹角，倾斜车辆初始位姿更为常见，所以本文以倾斜车辆初始位姿为研究工况。

2.1 B样条曲线

在实际工程应用中，曲线拟合可分为两种：①曲线通过所有的样本点，如三次参数样条曲线和抛物样条曲线等。②曲线不一定通过所有样本点，只是较好地逼近样本点。在两种曲线中，考虑到局部修改的便捷性，方便设计者交互操作，第1种曲线不能满足要求，第2种曲线多用于产品外形设计。因此Bezier提出了一种新的参数曲线表示方法，但Bezier曲线在实际应用中缺乏灵活性、控制性差、不易修改。为了克服上述问题，Shoenbery定义了B样条曲线，其表达式为

(13)

式中Pi——目标曲线控制点

Ni,k(t)——B样条基函数，是分段多项式

DeBoor和Cox定义的B样条曲线基函数递推表达式为

(14)

由式(14)可知，四阶样条曲线函数在其节点定义区间二阶可导，能满足泊车路径中曲率连续以及曲率变化率连续的要求，因此在满足方向盘转角连续变化的约束下，为减少计算量，使用四阶均匀B样条曲线作为目标路径的规划基础。根据样条曲线基函数递推公式，可求出目标曲线表达式，并将对应的曲线表达式写成矩阵形式

(15)

将式(15)中控制点分解为横坐标分量和纵坐标分量，即

(16)

2.2 运动学约束分析

(1)最大曲率约束

设计的路径应该满足车辆的等效偏转角约束，其最大等效前轮偏转角应该小于车辆允许的最大值，即

(17)

(2)速度约束

为避免车辆滑移等现象，泊车过程中的车速应该满足低速运动的假设条件要求，即

|v|max≤|vmax|

(18)

(3)角速度约束

若参数超出实际车辆执行能力，使后续路径跟踪效果极差。设计路径与等效偏转角速度相关时参数应该满足其角速度约束条件，即

|ω|≤ωmax

(19)

以控制点为变量，由B样条曲线表达式求解路径曲线，将泊车结束时车身与车位的夹角，即车身偏转角最小化作为优化目标，结合车辆运动学约束、车辆机械结构约束以及环境约束等建立非线性约束方程。

3 轨迹跟踪控制器设计

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

现有轨迹跟踪算法大多将期望轨迹视为时间函数，在时间域中求解控制律。这种控制方法常将车速作为控制量之一。但是在车辆行驶过程中，车速受到驾驶员、车辆状态以及行驶环境等多方面的影响，如果因为特殊情况造成车速控制不佳，使得基于时间参考的控制律失去继续跟踪期望轨迹的能力，最终将造成泊车失败。产生上述问题是因为时间相对于路径跟踪问题过于“自由”，与系统运行状态无关，从而导致系统的规划和控制不协调[11]。因此，基于非时间参考的控制思想被提出，通过引入其他非时间参考量代替时间参考量，提高系统在不确定环境中的路径跟踪能力。

基于非时间参考的路径跟踪控制律需要定义一个非时间参考的标量f，要求f为随时间单调递增函数，即

f=F(t)

(20)

(21)

设计的泊车路径是一条与时间无关的空间曲线，不包含与时间相关的变量。车辆跟踪目标路径的过程可以描述为：对任意给定的几何路径S(x,y,θ)=0，寻找反馈控制律，对于任意给定的ζ>0，存在ft，当f>ft时，使|(xr,yr,θr)-(x,y,θ)|≤ζ成立，其中(xr,yr,θr)是参考的目标路径坐标和车身偏转角。

在车辆倒车状态下，在建立的坐标系中选择车辆实际路径在X轴方向上的投影作为非时间参考量，即F=-x(t)，则车辆跟踪目标路径误差如图3所示。由图3可得

(22)

则车辆路径跟踪误差函数可以表示为

(23)

图3 路径跟踪误差Fig.3 Path tracking error

令pe=(ye,θe)，则其导数可表示为

(24)

假设车辆以稳定的车速运行，并且对目标路径进行跟踪，目标是在特定的有界等效前轮转角输入φ控制下，使得在任一的初始误差下，系统能在该控制下pe=(ye,θe)有界并且不断趋向于0，即

(25)

3.2 滑模控制器设计

为方便表示，令系统参考状态量为

(26)

则系统状态量导数可以表示为

(27)

将式(8)代入式(27)，可得

(28)

假设等效前轮转角为小角度输入，φ≈tanφ，令

(29)

则系统误差状态方程表示为

(30)

式中f(x)、b(x)——非线性函数

u——系统输入

在普通滑模控制中，通常选择一个线性的滑动超平面

(31)

使得系统到达滑动模态后，各误差渐近收敛为0，并且渐近收敛的速度可以通过选择滑动平面参数矩阵任意调节，但状态跟踪误差不会在有限时间内收敛为0。

为了提高控制器效果，提出终端滑模控制策略，通过引入非线性弧面，使得滑模面上跟踪误差能够在有限时间内收敛到0。通过设计一种动态非线性滑模面方程实现终端滑模控制，切换函数为

(32)

其中p>q，p和q为正奇数，β>0。

基于滑模控制和终端滑模控制的非时间参考路径跟踪控制律都具备跟踪目标路径的能力，但对车身偏转角状态控制存在强烈的抖振现象，需提高趋近品质。结合采用趋近律的方法对趋近运动轨迹进行限制，改善趋近运动的动态品质。

根据趋近律滑模控制方法，采用一种新型趋近律滑模控制，令滑模面的导数为

(33)

其中

(34)

式中0<δ<1，η>0，ε>0 ，δ为fal(s,η,δ)在原点附近正负对称线性段的区间长度，且fal(s,η,δ)为非连续函数。当|s|≤δ时，即s接近0时，式(33)第1项可以使得系统趋近速度变大，使得系统状态在有限时间之内快速到达滑动面，同时在滑动面附近fal(s,η,δ)是光滑连续的，控制输入的高频震动现象被有效削弱。而且式(33)的第2项也是光滑连续函数，当|s|>δ时，能保证系统状态以较大的速度趋近滑动面并且起到平滑和限幅的作用，联立式(29)、(30)、(33)、(34)可得

(35)

定义Lyapunov函数

(36)

对式(36)求导并代入式(35)可得

(37)

其中γ>0，因此可得

(38)

图4 路径规划仿真结果Fig.4 Path planning simulation results

4 仿真验证

在Matlab中输入车辆的尺寸参数(表1)、车位信息以及初始位姿信息， 添加等式约束条件函数和不等式约束条件函数，在满足约束条件下，令终止车身偏转角最小为目标对控制点坐标进行优化求解，则结果就是满足约束条件的B样条曲线路径的控制点坐标。

表1 车辆相关参数Tab.1 Vehicle related parameters

4.1 路径规划

结合实际泊车经验，当车身倾斜程度过大时，驾驶员会自行调整方向盘使得车身偏转角在一个合理的范围之内，所以θ0不会太大，因此假设|θ0|∈(0°,5°]。查阅对应车型的技术手册，可知最大允许曲率为0.256 4 m-1，最大允许角速度0.593 4 rad/s。

采用尺寸为6.4 m×2.0 m的车位作为目标车位，车辆起始位姿为x0=6.80 m，y0=1.80 m，θ0=0.087 rad。

由图4a B样条泊车路径可知，规划的曲线很好地连接了车辆起始点和预设终点，并且车辆在沿着规划路径行驶过程中，车辆轮廓没有与道路边缘、车库边界等发生碰撞，实现了车辆避障的要求。

由图4b车身偏转角可知，初始位置车身存在0.087 rad的初始偏转角，但随着规划的泊车路径，车辆在预设的泊车终点实现车身偏转角为0 rad，满足泊车的位姿要求，最大车身偏转角为0.777 6 rad。

由图4c泊车路径曲率可知，在起始点处，由于车身有一定的偏角，规划的路径利用了该偏角，路径起始点存在一定的曲率，该处的曲率与车身起始的偏转角有关，路径曲率最大值为0.236 8 m-1，小于最大允许曲率0.256 4 m-1，满足车辆转向约束条件。

由图4d前轮转向角速度可知，在起始位置处前轮需要迅速地满足设计曲率的要求，因此需要较大的角速度，在整个泊车过程中，最大角速度为0.528 1 rad/s，小于最大允许角速度0.593 4 rad/s，满足角速度约束。由上述仿真结果可知，此次规划的路径能实现车辆单步平行泊车。

4.2 路径跟踪

为验证基于趋近律的终端滑模路径跟踪控制器的跟踪效果，在Simulink上搭建路径跟踪仿真实验。设定跟踪路径方程为y=16sin(0.05x)+5，车辆初始位姿参数为(0，0，0.02)，车速为1 m/s，3种路径跟踪控制器仿真结果如图5所示。

图5 3种控制算法路径跟踪效果对比Fig.5 Comparison of path tracking effects of three control algorithms

由图5可知，终端滑模控制与非时间参考滑模控制相比，效果有所提升，在横坐标约为-26 m处成功地跟踪到参考路径，车辆在转弯较为剧烈地方的纵坐标跟踪误差减少；基于趋近率的终端滑模控制与基于终端滑模控制效果相近，但车辆在转弯较为剧烈处纵坐标跟踪效果更优，能更好地跟踪到参考路径。

由图6可知，终端滑模控制与非时间参考滑模控制相比，纵坐标跟踪最小误差为-0.063 m，最大误差约为-0.147 m，平均误差为-0.105 0 m，系统的稳定平均误差减少0.157 5 m，控制效果有明显提升；基于趋近率的终端滑模控制与基于终端滑模控制效果相比，稳定误差较小，最小误差约为-0.069 m，最大误差约为-0.144 m，平均误差约为-0.106 5 m。

图6 纵坐标跟踪误差效果对比Fig.6 Comparison of vertical ordinate tracking error

由图7可知，终端滑模控制与非时间参考滑模控制相比，车身偏转角跟踪误差最小误差约为0 rad，最大误差约为-0.044 rad，平均误差约为-0.022 rad，约为-1.26°，控制效果相当，仍存在较强的抖振现象；基于趋近率的终端滑模控制与基于终端滑模控制效果相比，车身偏转角跟踪最小误差约为-0.017 7 rad，最大误差约为-0.023 2 rad，稳定误差约为-0.020 5 rad，约为-1.174°，误差范围与前者相当。但系统车身偏转角误差的抖振现象明显减弱，控制效果更优。

图7 车身偏转角跟踪误差对比Fig.7 Comparison of body deflection angle tracking error

实际泊车过程中，车辆一般从静止开始运动到达目标车速，存在速度波动情况，为验证设计控制器的适用性，设计车速从0 m/s加速到目标车速并且其过程中存在速度波动，对该情况进行仿真。目标路径方程为y=16sin(0.05x)+5,车辆初始位姿参数为(0，0，0.02)，仿真时间为200 s。

图8 不同车速路径跟踪仿真结果Fig.8 Tracking simulation results of different vehicle speeds

由图8可知，在不同目标速度并且存在波动的情况下，基于趋近律的终端滑模路径跟踪控制器能跟踪到目标路径，不同速度下跟踪情况相似，跟踪效果受到速度影响较小，与车辆位置有较大关系，说明速度变化剧烈的情况下，本文设计的控制器仍具有较好的跟踪效果。

对于相同的参考路径，3种路径跟踪控制器都能控制车辆运动模型跟踪到目标路径。滑模控制结构简单，但控制效果一般，系统相对稳定时，会存在较大的稳定误差，并且车身偏转角有较强的抖振现象；相比前者，终端滑模控制的控制效果有所改善，稳定误差降低，但是车身偏转角的抖振现象强烈，甚至还有所加强；本文设计的基于趋近律的终端滑模控制器不仅降低了稳定误差，而且车身偏转角误差的抖振现象也明显减弱，控制效果更优。

图10 Simulink/CarSim联合仿真结果Fig.10 Co-simulation result of Simulink/CarSim

4.3 Simulink/CarSim联合仿真

在Simulink和CarSim中建立联合仿真模型，包括路径规划、跟踪控制、CarSim车辆模型S函数，仿真车速采用PID控制，不是本文重点，故不再赘述。联合仿真流程如图9所示。

图9 联合仿真流程图Fig.9 Flow chart of co-simulation

结合日常泊车工况，设定期望车速为-1 m/s，x0=6.80 m，y0=1.80 m，θ0=0.087 rad。路径规划与跟踪仿真结果如图10所示。

由图10a可知，在初始车身偏转角不平行于车库方向，车身偏转角为5°的情况下，基于趋近律的终端滑模控制器能够根据给出的参考路径，控制目标车辆的方向盘转角，使得车辆从起始位置到达目标位置。在路径起始阶段，车辆起始位姿与路径起始处坐标以及该处切线方向一致，车辆能较好地跟踪到参考路径，当车辆停稳后，车辆与目标位置的纵坐标误差为0.027 8 m。

由图10b可知，车辆能够跟踪到参考路径下的参考车身偏转角，泊车结束时误差为0.005 rad。

由图10c、10d可知，在控制器控制下，当车辆到达预设位置后实现停车，不随时间的变化发生移动。

由图10e可知，在控制器控制下，纵坐标正向误差最大达到0.031 m，负向最大误差为-0.028 m，在此过程中，存在抖振现象，并且当泊车过程结束时，存在误差0.027 8 m。

由图10f可知，跟踪过程中存在抖振现象，在x=5.72 m处，车身偏转角正向误差最大，达到0.045 rad，x=1.68 m处，车身偏转角负向误差最大，达到-0.038 rad，当泊车结束时，存在0.005 rad的偏转角误差。

由图11可知，在CarSim仿真结果后处理部分中的动画显示中，控制的目标车辆能够从初始位置较好地沿着参考路径进入目标车位，从整体的轮廓图可以看出，车辆与周围的车辆没有发生碰撞，在停车时没有与后车发生碰撞，所以设计的参考路径是合理的。

图11 CarSim仿真动画显示Fig.11 Simulation animation of CarSim

5 结论

(1)结合B样条曲线方法建立了非线性泊车路径优化函数，并针对倾斜泊车起始位姿进行路径规划Simulink仿真，验证了设计的路径规划方法具有较好的效果。

(2)结合非时间参考路径跟踪控制方法和终端滑模控制方法的优点，提出了基于趋近律的终端滑模路径跟踪控制方法。通过跟踪目标正弦路径的仿真实验对比，验证了设计的控制器具有较好的路径跟踪控制效果。

(3)建立了路径规划和基于趋近律的终端滑模路径跟踪控制器Simulink模型以及目标车型的CarSim整车模型，联合仿真结果表明，本文所提算法路径规划结果满足车辆泊车避障和运动约束要求，所设计路径跟踪控制器控制能较好地跟踪参考路径，快速安全进入车位。