自动泊车路径纯跟踪算法应用研究

齐志权，吴世南，邹波，王浩，任宏斌

（1.北京理工大学 机械与车辆学院，北京 100081；2.重庆铁马工业集团有限公司，重庆 400050）

智能化是汽车行业发展的一个重要方向，智能车辆典型体系结构主要包括环境感知、行为决策两个部分[1]，其中行为决策包含路径规划、轨迹规划以及车辆控制等子系统.智能车辆控制系统包括纵向控制和横向控制，横向控制指路径跟踪，即在保证舒适性和安全性的情况下，通过自动转向控制车辆沿着参考轨迹行驶[2].路径跟踪方法有基于模型的控制方法[3-4]、基于几何的方法以及基于机器学习[5]的方法等.几何方法只利用参考路径和车辆之间的几何关系，例如纯跟踪法和Stanley 法.其中，纯跟踪算法自提出以来，因其良好的鲁棒性、易实现且与人类驾驶员行为类似的特点得到了广泛应用.它通过计算车辆从当前位置到目标点的曲率来控制转向，即使在存在较大的横向偏差和参考路径曲率不连续的情况下也能达到很好的跟踪效果.以圆与直线，Hybrid A*为基础的泊车路径规划算法得到的轨迹大多是曲率不连续的，因此纯跟踪算法在泊车轨迹跟踪中应用广泛.算法难点在于选择恰当的预瞄距离[6]，过短的预瞄距离会导致路径点跳变,控制量离散化进而引起控制振荡；过长的预瞄距离使得跟踪精度下降，在大曲率工况下会出现切圆现象.

国内外学者在改进纯跟踪算法方面做了大量研究.PARK 等[7]将PI 控制应用到侧向运动控制中，提出了自适应纯跟踪算法，在输出期望方向盘转角时考虑了车辆与参考路径间的横向偏移量，该方法在曲线路径的跟踪上取得了良好的效果.WANG[8]在纯跟踪算法中提出了一种基于优化前瞻距离的纯跟踪算法，在直线路径、正弦路径、抛物线路径以及换道路径中验证了算法有效性.YU 等[9]为了解决无人驾驶公交车的路径跟踪问题，提出了一种基于前轴参考的模糊纯跟踪控制的路径跟踪控制器，将纯跟踪算法的参考点选为前轴中心，改进算法在实车实验中取得了良好效果.AHN 等[10]在选取预瞄点的过程中综合考虑了车辆和参考路径之间的相对位置及方向关系，使用该方法车辆可以快速收敛到参考路径上并且减少了“切角”现象.WANG 等[11]采用两个模糊控制器同时调整纯跟踪算法的车速和预瞄距离，以减少跟踪误差.SUKHI 等[12]提出了一种贪心算法，为参考轨迹上的路点分配最佳预瞄距离.WANG等[13]将GPS 数据与纯跟踪算法融合，提出基于二次多项式函数的动态预瞄距离纯跟踪算法，以减小横向误差.然而，上述改进算法均没有考虑大曲率路径工况下，纯跟踪算法控制量输出振荡问题.自动泊车系统规划模块生成的参考路径曲率较大，跟踪精度要求较高，选择较小预瞄距离时方向盘抖动问题愈发明显，需要对纯跟踪算法进行必要的改进.

为平衡跟踪精度和控制平顺性能，文中针对智能车辆自动泊车低速大曲率工况下的路径跟踪控制问题，对纯跟踪算法进行了改进，算法经修正处理后可以最小程度地选取预瞄距离.通过轨迹跟踪误差以及方向盘转角控制平顺性两个指标与传统纯跟踪算法进行了对比.

1 纯跟踪算法

1.1 以后轴中心为原点的车辆运动学模型

实现智能车辆路径的准确跟踪控制，首先要对车辆的运动建立准确的数学模型，模型对车辆运动的描述越准确，则对车辆的跟踪控制效果就越好.同时，建立的数学模型除了真实反映车辆特性外，也应该尽可能的简单易用.在自动泊车低速大曲率路径工况下，由于车轮侧偏特性影响较小，自行车车辆运动学模型可以满足算法设计需要.

自行车运动学模型的建立基于如下假设：

①不考虑车辆在垂直方向(Z轴方向)的运动，即假设车辆的运动是一个二维平面上的运动；②假设车辆同轴左右侧轮胎在任意时刻都拥有相同的转角和转速，车辆的左右轮胎运动可以合并描述；③忽略前后轴载荷转移；④假设车身和悬架系统是刚性系统；⑤假设车辆前轮转向；⑥车辆在低速行驶状态下，侧向加速度小，此时可认为轮胎侧偏角近似为0，即车轮没有侧向滑移.

如图1 所示为自行车运动学模型示意图.图中，O′点 是车辆的瞬时转动中心；β为 质心侧偏角；ψ为车身横摆角；δ为前轮转角；v为车辆质心处速度；R为转弯半径；a,b分别为质心到前轴以及后轴的距离.

分析图中几何关系，在三角形O′GA和O′GB中，由正弦定理可得到如下关系：

1.2 纯跟踪算法原理

如图2 所示为纯跟踪算法的几何描述.以车辆后轴位置为圆心，预瞄距离ld为半径做圆，与前方规划路径的交点为预瞄点 (gx,gy).根据自行车模型，以车辆后轴中心为切点，车辆纵向车身为切线，通过控制前轮转角，使车辆可以沿着一条经过预瞄点的圆弧行驶，实现路径跟踪控制.

图2 纯跟踪算法几何描述Fig.2 Geometric description of pure pursuit algorithm

根据几何关系可以计算得到预期路径的曲率大小，具体计算式为

式中：α为车辆当前航向与预期路径圆弧之间的弦切角大小；R为预期路径圆弧半径.

整理可得：

可以看出，纯跟踪算法实际上是基于横向偏差的比例控制器，增益受预瞄距离影响很大，预瞄距离越大，增益越小.当选取小的预瞄距离时，因存在超调量且路径曲率变化，车辆的频繁转向会引起控制振荡，舒适性变差.当选取大的预瞄距离时，车辆的振荡减小，但是控制系统忽略了一部分参考路径的信息，导致跟踪精度下降.

2 纯跟踪算法在自动泊车路径跟踪中的应用

本节针对智能车辆泊车工况应用场景，分析纯跟踪算法在应用过程中需要注意的问题.算法实现如图3 所示.

图3 纯跟踪算法实现流程图Fig.3 Flow chart of pure pursuit algorithm

以垂直车位自动泊车路径跟踪为例，泊车路径由路径规划模块给出，车辆实际行驶轨迹通过GPS设备采集.实验设备及场景如图4 所示，图4(a)为GPS基站及垂直车位图，图4(b)为实验车辆及GNSS 天线安装位置，图4(c)为GPS 移动站.实验用车具体参数如下：整车长4.59 m，整车宽1.88 m，轴距2.75 m，转弯半径5 m，方向盘传动比18.06.

图4 实验设备及场景Fig.4 Experimental equipment and scenes

路径规划模块输出的参考路径如图5 所示，最小半径为6 m，生成的一系列参考路点集合为Pref={p0,p1,p2···}，其中pi={xi,yi}，相邻参考路点之间的欧氏距离约为0.2 m.根据公式(10)曲率 κ 与半径R的关系, 结合车辆最小转弯半径限制，半径6 m 是在保证泊车任务顺利完成的前提下，实验车辆可以达到的最大曲率.

图5 垂直泊车路径规划（三步泊车）Fig.5 Path planning for vertical parking (three steps)

图6 垂直泊车路径纯跟踪控制结果Fig.6 Path tracking with pure pursuit for vertical parking

图7 纯跟踪最大横向误差Fig.7 Pure pursuit maximum lateral error

图8 垂直泊车路径纯跟踪方向盘控制输出Fig.8 Steering wheel angle output of pure pursuit for vertical parking

3 带记忆参数的纯跟踪方向盘转角修正

纯跟踪算法的每一个控制周期都会输出前轮的期望转角，通过车辆线控转向或者电动助力转向系统控制对应的方向盘转角，在自动泊车大曲率路径的跟踪控制中，方向盘的高频抖动不仅会影响到驾驶体验，对车辆线控底盘系统也会带来不利影响.文中在保证跟踪精度的前提下，为消除纯跟踪算法带来的方向盘抖动，基于自动泊车低速、方向盘转角变化率小等特性，引入控制记忆参数，对方向盘转角控制指令进行修正.具体算法的处理过程主要有3 个步骤:

①记忆低通滤波.除第一个控制周期的方向盘转角直接输出外，此后的每一个控制周期的方向盘转角控制值都与上一控制周期实际控制值进行加权，得到有效低通滤波值，具体计算公式为

式中：θoutput(t-1)为 上一控制周期输出的方向盘转角，θe(t)为当前控制周期根据式(7)及方向盘传动比计算得出的期望方向盘转角，θpro(t)为当前控制周期待处理的方向盘转角，β为根据实际工况选取的权重系数.β越大，方向盘转角指令输出越平稳，但是会造成灵敏度降低.

②期望方向盘转角变化记忆参数计算.通过计算 θpro(t)和 θoutput(t-1)的 差 值 θdif，并 与 设 定 的 阈 值 进 行 比较，当 θdif超过阈值时，转角变化记忆参数增加，具体更新过程为

式中：Mleft，Mright分别为左(右)转信号记忆参数，初值为0；steer_space 为设定的转向阈值.

③更新期望方向盘转角控制输出.为了避免传统纯跟踪算法在路径跟踪时频繁转向的情况，只有当期望方向盘转角变化记忆参数超过一定范围，且期望方向盘转角与上一控制周期的变化差值大于一定步长时，才会更新转向控制输出，具体更新方法如下:

当Mleft，Mright小于等于设定的数量控制阈值control_space 时，仍将上一个控制周期输出的方向盘转角 θoutput(t-1)作为当前控制输出.

当Mleft，Mright超过设定的阈值control_space 时，若θdif的绝对值超过设定的步长控制阈值step_space，则将 θpro(t)作为当前时刻转向控制输出；否则，仍将上一控制周期的方向盘控制转角 θoutput(t-1)作为当前时刻转向控制输出.算法流程如图9 所示.

图9 纯跟踪方向盘转角修正算法流程图Fig.9 Flow chart of corrected pure pursuit algorithm for steering wheel angle

4 算法实车实验验证

4.1 参数选择

以上述垂直泊车工况为例进行实车验证，如图5所示，车辆初始位置为(-0.08，4)，垂直泊车位位置为(-5.6，0)，整个泊车规划过程分为三步，泊车过程中最高车速限定为2.5 km/h.经实车实验标定，主要车辆参数和控制参数如表1 所示.同样分别选取预瞄距离ld为1 m，2 m，3 m，将传统纯跟踪算法与文中修正后的纯跟踪算法在跟踪精度以及方向盘转角控制输出稳定性进行对比分析.

表1 参数选择Tab.1 Parameters value used

4.2 跟踪精度分析

图10 为修正纯跟踪算法轨迹图，从图中可以看出，随着预瞄距离减小，跟踪精度能得到提高.图11为修正纯跟踪算法最大横向误差图，当预瞄距离为1 米时，轨迹的横向误差最大为0.054 7 m，与传统纯跟踪算法相同；当预瞄距离为2 m 时，修正算法的轨迹最大横向误差与传统纯跟踪算法几乎相同；当预瞄距离为3 m 时，轨迹的最大横向误差有所减少，可以达到0.156 2 m.由此可见，文中提出的修正纯跟踪算法在低速大曲率路径跟踪工况下，可以保证甚至提高跟踪精度.

图10 垂直泊车路径修正纯跟踪控制结果Fig.10 Path tracking with corrected pure pursuit for vertical parking

图11 修正算法最大横向误差Fig.11 Correction algorithm maximum lateral error

4.3 控制输出稳定性分析

图12 为修正纯跟踪算法方向盘转角控制输出结果，与图8 对比可以看出，在预瞄距离分别为1 m，2 m，3 m 时，采用修正纯跟踪算法，方向盘转角的抖动问题均得到很好控制，同时跟踪精度没有降低.原因在于改进算法在输出方向盘转角时通过记忆低通滤波和设定记忆参数使输出慢于输入，产生迟滞，通过记忆因子的衰减作用滤掉了较大的高频波动.图13 为预瞄距离为1 m 时纯跟踪算法修正前后输出方向盘转角的对比结果，可以看出修正的纯跟踪算法可以大幅度提高小预瞄距离下纯跟踪算法方向盘转角输出的稳定性，使控制更为平顺，避免控制振荡问题.

图12 修正纯跟踪算法方向盘控制输出Fig.12 Steering wheel angle of corrected pure pursuit.

图13 方向盘转角输出对比Fig.13 Comparation of steering wheel angle

5 结 论

针对传统纯跟踪算法在自动泊车大曲率路径跟踪过程中，跟踪精度和方向盘转角控制输出平顺性相互制约的问题，文中在原始纯跟踪算法的基础上，利用自动泊车低速、方向盘转角变化率小的特性，通过引入控制记忆参数，对方向盘转角控制输出进行修正.实验验证表明，该方法可以应用在自动泊车轨迹跟踪系统中，在保证跟踪精度的同时，大幅度减少方向盘抖动，提高跟踪控制的平顺性.值得指出的是，该方法同样适用于其他低速大曲率路径跟踪工况.