自动泊车路径纯跟踪算法应用研究_齐志权.pdf

自动泊车路径纯跟踪算法应用研究齐志权1，吴世南1，邹波2，王浩1，任宏斌1（1.北京理工大学 机械与车辆学院，北京100081；2.重庆铁马工业集团有限公司，重庆400050）摘 要：为实现自动泊车系统的路径跟踪功能，解决纯跟踪算法由于预瞄距离选取不当导致的方向盘抖动、跟踪精度下降的问题，文中提出了一种改进的纯跟踪路径跟踪算法，该方法在传统纯跟踪算法的计算结果基础上设计了一种带记忆参数的方向盘转角修正算法.实车实验表明，该方法适用于自动泊车系统，可以在适用工况下选取较小的预瞄距离，在保证跟踪精度的同时解决了纯跟踪算法在小预瞄距离下的方向盘抖动问题.关键词：路径跟踪；自动泊车；纯跟踪；预瞄距离中图分类号：U469.79 文献标志码：A 文章编号：1001-0645(2023)05-0510-07DOI：10.15918/j.tbit1001-0645.2022.143Application of Pure Pursuit Algorithm for Automatic Parking SystemQI Zhiquan1，WU Shinan1，ZOU Bo2，WANG Hao1，REN Hongbin1（1.School of Mechanical Engineering,Beijing Institute of Technology,Beijing 100081,China;2.Chongqing Tiema Industries Group Co.,LTD.,Chongqing 400050,China）Abstract：In order to realize the path tracking function of the automatic parking system and solve the problemsof steering wheel shaking and tracking accuracy decrease due to select look-ahead distance improperly for thepure pursuit algorithm,an improved pure pursuit algorithm was proposed in this paper.Based on the calculationresults of the traditional pure pursuit algorithm,a steering wheel angle correction algorithm with memory para-meters was designed.The real vehicle experiments show that the method is suitable for automatic parking sys-tem,and a small look-ahead distance can be selected under applicable working conditions,and the steering wheelshaking problem can be solved for pure pursuit algorithm under small look-ahead distance,while ensuring thetracking accuracy.Key words：path tracking；automatic parking；pure pursuit；look-ahead distance 智能化是汽车行业发展的一个重要方向，智能车辆典型体系结构主要包括环境感知、行为决策两个部分1，其中行为决策包含路径规划、轨迹规划以及车辆控制等子系统.智能车辆控制系统包括纵向控制和横向控制，横向控制指路径跟踪，即在保证舒适性和安全性的情况下，通过自动转向控制车辆沿着参考轨迹行驶2.路径跟踪方法有基于模型的控制方法3 4、基于几何的方法以及基于机器学习5的方法等.几何方法只利用参考路径和车辆之间的几何关系，例如纯跟踪法和 Stanley 法.其中，纯跟踪算法自提出以来，因其良好的鲁棒性、易实现且与人类驾驶员行为类似的特点得到了广泛应用.它通过计算车辆从当前位置到目标点的曲率来控制转向，即使在存在较大的横向偏差和参考路径曲率不连续的情况下也能达到很好的跟踪效果.以圆与直线，HybridA*为基础的泊车路径规划算法得到的轨迹大多是曲率不连续的，因此纯跟踪算法在泊车轨迹跟踪中应用广泛.算法难点在于选择恰当的预瞄距离6，过短的预瞄距离会导致路径点跳变,控制量离散化进而引起控制振荡；过长的预瞄距离使得跟踪精度下降，在大曲率工况下会出现切圆现象.国内外学者在改进纯跟踪算法方面做了大量研 收稿日期：2022 06 14基金项目：国家自然科学基金资助项目(52002025)作者简介：齐志权（1976），男，副教授，硕士生导师，E-mail：.第 43 卷第 5 期北 京 理 工 大 学 学 报Vol.43No.52023 年 5 月Transactions of Beijing Institute of TechnologyMay 2023究.PARK 等7将 PI 控制应用到侧向运动控制中，提出了自适应纯跟踪算法，在输出期望方向盘转角时考虑了车辆与参考路径间的横向偏移量，该方法在曲线路径的跟踪上取得了良好的效果.WANG 8在纯跟踪算法中提出了一种基于优化前瞻距离的纯跟踪算法，在直线路径、正弦路径、抛物线路径以及换道路径中验证了算法有效性.YU 等9为了解决无人驾驶公交车的路径跟踪问题，提出了一种基于前轴参考的模糊纯跟踪控制的路径跟踪控制器，将纯跟踪算法的参考点选为前轴中心，改进算法在实车实验中取得了良好效果.AHN 等 10在选取预瞄点的过程中综合考虑了车辆和参考路径之间的相对位置及方向关系，使用该方法车辆可以快速收敛到参考路径上并且减少了“切角”现象.WANG 等11采用两个模糊控制器同时调整纯跟踪算法的车速和预瞄距离，以减少跟踪误差.SUKHI 等12提出了一种贪心算法，为参考轨迹上的路点分配最佳预瞄距离.WANG等13将 GPS 数据与纯跟踪算法融合，提出基于二次多项式函数的动态预瞄距离纯跟踪算法，以减小横向误差.然而，上述改进算法均没有考虑大曲率路径工况下，纯跟踪算法控制量输出振荡问题.自动泊车系统规划模块生成的参考路径曲率较大，跟踪精度要求较高，选择较小预瞄距离时方向盘抖动问题愈发明显，需要对纯跟踪算法进行必要的改进.为平衡跟踪精度和控制平顺性能，文中针对智能车辆自动泊车低速大曲率工况下的路径跟踪控制问题，对纯跟踪算法进行了改进，算法经修正处理后可以最小程度地选取预瞄距离.通过轨迹跟踪误差以及方向盘转角控制平顺性两个指标与传统纯跟踪算法进行了对比.1 纯跟踪算法 1.1 以后轴中心为原点的车辆运动学模型实现智能车辆路径的准确跟踪控制，首先要对车辆的运动建立准确的数学模型，模型对车辆运动的描述越准确，则对车辆的跟踪控制效果就越好.同时，建立的数学模型除了真实反映车辆特性外，也应该尽可能的简单易用.在自动泊车低速大曲率路径工况下，由于车轮侧偏特性影响较小，自行车车辆运动学模型可以满足算法设计需要.自行车运动学模型的建立基于如下假设：不考虑车辆在垂直方向(Z 轴方向)的运动，即假设车辆的运动是一个二维平面上的运动；假设车辆同轴左右侧轮胎在任意时刻都拥有相同的转角和转速，车辆的左右轮胎运动可以合并描述；忽略前后轴载荷转移；假设车身和悬架系统是刚性系统；假设车辆前轮转向；车辆在低速行驶状态下，侧向加速度小，此时可认为轮胎侧偏角近似为 0，即车轮没有侧向滑移.OvRa,b如图 1 所示为自行车运动学模型示意图.图中，点是车辆的瞬时转动中心；为质心侧偏角；为车身横摆角；为前轮转角；为车辆质心处速度；为转弯半径；分别为质心到前轴以及后轴的距离.YOOXARaGBbv/2图 1 自行车运动学模型Fig.1 Bicycle kinematics model 分析图中几何关系，在三角形 OGA 和 OGB 中，由正弦定理可得到如下关系：sin()a=sin(/2)R（1）sinb=sin(/2)R（2）结合以上 2 式，可以得到tancos=a+bR（3）=0L车辆低速行驶时，根据前文假设，认为车辆不会发生侧向滑动，因此质心侧偏角，为车辆轴距,可以得到前轮转角与转弯半径的关系:tan=LR（4）1.2 纯跟踪算法原理ld(gx,gy)如图 2 所示为纯跟踪算法的几何描述.以车辆后轴位置为圆心，预瞄距离 为半径做圆，与前方规划路径的交点为预瞄点.根据自行车模型，以车辆后轴中心为切点，车辆纵向车身为切线，通过控制前轮转角，使车辆可以沿着一条经过预瞄点的圆弧行驶，实现路径跟踪控制.根据几何关系可以计算得到预期路径的曲率大小，具体计算式为第 5 期齐志权等：自动泊车路径纯跟踪算法应用研究511ldsin(2)=Rsin(2)（5）R式中：为车辆当前航向与预期路径圆弧之间的弦切角大小；为预期路径圆弧半径.跟踪路径预瞄路径圆弧RReld(gx,gy)2ld图 2 纯跟踪算法几何描述Fig.2 Geometric description of pure pursuit algorithm 整理可得：R=ld2sin（6）根据基于 Ackerman 转向车辆的简单几何自行车模型，可以得到纯跟踪算法的前轮转角控制律为(t)=tan1(2Lsin(t)ld)（7）因此，在预瞄距离给定的情况下，对于每一个都可以计算出前轮等效转角，根据方向盘传动比得到对应的方向盘转角，作为控制输入完成路径跟踪.eld如图 2 所示，定义预瞄点与车辆之间的横向偏差为eld=ldsin（8）考虑小角度假设，则：(t)=tan1(2Lld2eld)（9）可以看出，纯跟踪算法实际上是基于横向偏差的比例控制器，增益受预瞄距离影响很大，预瞄距离越大，增益越小.当选取小的预瞄距离时，因存在超调量且路径曲率变化，车辆的频繁转向会引起控制振荡，舒适性变差.当选取大的预瞄距离时，车辆的振荡减小，但是控制系统忽略了一部分参考路径的信息，导致跟踪精度下降.2 纯跟踪算法在自动泊车路径跟踪中的应用本节针对智能车辆泊车工况应用场景，分析纯跟踪算法在应用过程中需要注意的问题.算法实现如图 3 所示.开始预瞄距离设定搜索距离车辆最近路径点的索引n计算路径点i到车辆的距离d计算i和i1路径点线段与以车辆位置为圆心，预瞄距离为半径的圆的交点，即预瞄点计算由预瞄点、后轴中心和车身航向角确定的圆的半径或曲率计算前轮转角控制律，式(7)i=ni=i+1否是d预瞄距离图 3 纯跟踪算法实现流程图Fig.3 Flow chart of pure pursuit algorithm 以垂直车位自动泊车路径跟踪为例，泊车路径由路径规划模块给出，车辆实际行驶轨迹通过 GPS设备采集.实验设备及场景如图 4 所示，图 4(a)为 GPS(a)GPS 基站(b)GNSS 天线(c)GPS 移动站图 4 实验设备及场景Fig.4 Experimental equipment and scenes512北 京 理 工 大 学 学 报第 43 卷基站及垂直车位图，图 4(b)为实验车辆及 GNSS 天线安装位置，图 4(c)为 GPS 移动站.实验用车具体参数如下：整车长 4.59 m，整车宽 1.88 m，轴距 2.75 m，转弯半径 5 m，方向盘传动比 18.06.Pref=p0,p1,p2pi=xi,yiR路径规划模块输出的参考路径如图 5 所示，最小半径为 6 m，生成的一系列参考路点集合为，其中，相邻参考路点之间的欧氏距离约为 0.2 m.根据公式(10)曲率 与半径的关系,结合车辆最小转弯半径限制，半径 6 m 是在保证泊车任务顺利完成的前提下，实验车辆可以达到的最大曲率.=1R（10）Preal=w0,w1,w2wi=xi,yiPrealwi pipi+1通过选取不同的预瞄距离（分别为 1 m，2 m，3 m），基于纯跟踪算法的实车实验轨迹如图 6 所示.车辆实际行驶轨迹，其中，由GPS 设备采集得到.遍历集合中的每一点，找到对应的参考路径向量，计算实际轨迹点到参考路径向量的垂直距离得到横向误差.泊车过程中最大横向误差如图 7 所示.从图中可以看出，预瞄距离越大，则跟踪误差越大；当预瞄距离为 3 米时，轨迹的最大跟踪误差 0.18 m，不能满足泊车系统控制要求；当预瞄距离为 1 m 时，轨迹的跟踪误差可减小到0.054 7 m，但是输出的方向盘转角控制量出现了严重抖动问题，如图 8 所示.事实上，由于无人车的参考路径是由路径规划模块生成的一系列离散的路点构成，在路径跟踪的过程中，纯跟踪算法搜索的预瞄点在规划路径上移动，由于路径的分段线性特征，使得由预瞄点、车辆后轴中心和车身航向角确定的预瞄路径圆弧的曲率会出现分段跳变，在大曲率路径工况下，该问题更为突出，这会导致输出控制量的高频抖动，因而也限制了纯跟踪算法在自动泊车控制系统中的应用.6543210 x/m0.500.51.01.52.02.53.03.54.04.5y/m规划路径1 m2 m3 m图 6 垂直泊车路径纯跟踪控制结果Fig.6 Path tracking with pure pursuit for vertical parking 0.054 70.101 50.181 m2 m3 m预瞄距离00.020.040.060.080.100.120.140.160.180.20最大横向误差/m图 7 纯跟踪最大横向误差Fig.7 Pure pursuit maximum lateral error 02004006008001 0001 200t/(0.02 s)50040030020010001002003004005001 m2 m3 m控制转角/(）图 8 垂直泊车路径纯跟踪方向盘控制输出Fig.8 Steering wheel angle output of pure pursuit for vertical parking 3 带记忆参数的纯跟踪方向盘转角修正纯跟踪算法的每一个控制周期都会输出前轮的期望转角，通过车辆线控转向或者电动助力转向系统控制对应的方向盘转角，在自动泊车大曲率路径 第一步第二步第三步x/my/m终点起点4.54.03.53.02.52.01.51.00.500.56543210图 5 垂直泊车路径规划（三步泊车）Fig.5 Path planning for vertical parking(three steps)第 5 期齐志权等：自动泊车路径纯跟踪算法应用研究513的跟踪控制中，方向盘的高频抖动不仅会影响到驾驶体验，对车辆线控底盘系统也会带来不利影响.文中在保证跟踪精度的前提下，为消除纯跟踪算法带来的方向盘抖动，基于自动泊车低速、方向盘转角变化率小等特性，引入控制记忆参数，对方向盘转角控制指令进行修正.具体算法的处理过程主要有 3 个步骤:记忆低通滤波.除第一个控制周期的方向盘转角直接输出外，此后的每一个控制周期的方向盘转角控制值都与上一控制周期实际控制值进行加权，得到有效低通滤波值，具体计算公式为pro(t)=output(t1)+e(t)(1)t=2,3,4,n（11）output(t1)e(t)pro(t)式中：为上一控制周期输出的方向盘转角，为当前控制周期根据式(7)及方向盘传动比计算得出的期望方向盘转角，为当前控制周期待处理的方向盘转角，为根据实际工况选取的权重系数.越大，方向盘转角指令输出越平稳，但是会造成灵敏度降低.pro(t)output(t1)difdif期望方向盘转角变化记忆参数计算.通过计算和的差值，并与设定的阈值进行比较，当超过阈值时，转角变化记忆参数增加，具体更新过程为dif=pro(t)output(t1)（12）Mleft+1difsteer_spaceMright+1dif steer_spaceMleft=0,Mright=0else（13）式中：Mleft，Mright分别为左(右)转信号记忆参数，初值为 0；steer_space 为设定的转向阈值.更新期望方向盘转角控制输出.为了避免传统纯跟踪算法在路径跟踪时频繁转向的情况，只有当期望方向盘转角变化记忆参数超过一定范围，且期望方向盘转角与上一控制周期的变化差值大于一定步长时，才会更新转向控制输出，具体更新方法如下:output(t1)当 Mleft，Mright小于等于设定的数量控制阈值 con-trol_space 时，仍将上一个控制周期输出的方向盘转角作为当前控制输出.difpro(t)output(t1)当 Mleft，Mright超过设定的阈值 control_space 时，若的绝对值超过设定的步长控制阈值 step_space，则将作为当前时刻转向控制输出；否则，仍将上一控制周期的方向盘控制转角作为当前时刻转向控制输出.算法流程如图 9 所示.转角差值大于控制步长开始参数初始化低通滤波转向记忆参数大于阈值计算期望方向盘转角输出当前计算转角输出上一周期控制转角是否到达终点结束是是是否否否图 9 纯跟踪方向盘转角修正算法流程图Fig.9 Flow chart of corrected pure pursuit algorithm for steering wheelangle 4 算法实车实验验证 4.1 参数选择ld以上述垂直泊车工况为例进行实车验证，如图 5所示，车辆初始位置为(0.08，4)，垂直泊车位位置为(5.6，0)，整个泊车规划过程分为三步，泊车过程中最高车速限定为 2.5 km/h.经实车实验标定，主要车辆参数和控制参数如表 1 所示.同样分别选取预瞄距离 为 1 m，2 m，3 m，将传统纯跟踪算法与文中修正后的纯跟踪算法在跟踪精度以及方向盘转角控制输出稳定性进行对比分析.表 1 参数选择Tab.1 Parameters value used参数值参数值轴距2.75 m0.5采样间隔0.02 ssteer_space2.0step_space5.0control_space10 4.2 跟踪精度分析图 10 为修正纯跟踪算法轨迹图，从图中可以看出，随着预瞄距离减小，跟踪精度能得到提高.图 11为修正纯跟踪算法最大横向误差图，当预瞄距离为1 米时，轨迹的横向误差最大为 0.054 7 m，与传统纯跟踪算法相同；当预瞄距离为 2 m 时，修正算法的轨迹最大横向误差与传统纯跟踪算法几乎相同；当预瞄距离为 3 m 时，轨迹的最大横向误差有所减少，可514北 京 理 工 大 学 学 报第 43 卷以达到 0.156 2 m.由此可见，文中提出的修正纯跟踪算法在低速大曲率路径跟踪工况下，可以保证甚至提高跟踪精度.6543210 x/m0.500.51.01.52.02.53.03.54.04.5y/m规划路径1 m2 m3 m图 10 垂直泊车路径修正纯跟踪控制结果Fig.10 Path tracking with corrected pure pursuit for vertical parking 0.054 70.109 30.156 21 m2 m3 m预瞄距离00.020.040.060.080.100.120.140.160.180.20最大横向误差/m图 11 修正算法最大横向误差Fig.11 Correction algorithm maximum lateral error 4.3 控制输出稳定性分析图 12 为修正纯跟踪算法方向盘转角控制输出结果，与图 8 对比可以看出，在预瞄距离分别为 1 m，2 m，3 m 时，采用修正纯跟踪算法，方向盘转角的抖动问题均得到很好控制，同时跟踪精度没有降低.原因在于改进算法在输出方向盘转角时通过记忆低通滤波和设定记忆参数使输出慢于输入，产生迟滞，通过记忆因子的衰减作用滤掉了较大的高频波动.图 13 为预瞄距离为 1 m 时纯跟踪算法修正前后输出方向盘转角的对比结果，可以看出修正的纯跟踪算法可以大幅度提高小预瞄距离下纯跟踪算法方向盘转角输出的稳定性，使控制更为平顺，避免控制振荡问题.020040060080010001200t/(0.02 s)5004003002001000100200300400500Steer angle/()1 m2 m3 m图 12 修正纯跟踪算法方向盘控制输出Fig.12 Steering wheel angle of corrected pure pursuit.02004006008001000 1200t/(0.02 s)5004003002001000100200300400500控制转角/(）控制转角/(）控制转角/(）修正算法纯跟踪算法(a)全程方向盘转角输出(b)局部方向盘转角输出对比1(c)局部方向盘转角输出对比221t/(0.02 s)t/(0.02 s)0501001502002501000100200300400500修正算法纯跟踪算法380400420440修正算法纯跟踪算法图 13 方向盘转角输出对比Fig.13 Comparation of steering wheel angle 5 结论针对传统纯跟踪算法在自动泊车大曲率路径跟踪过程中，跟踪精度和方向盘转角控制输出平顺性相互制约的问题，文中在原始纯跟踪算法的基础上，利用自动泊车低速、方向盘转角变化率小的特性，通过引入控制记忆参数，对方向盘转角控制输出进行修正.实验验证表明，该方法可以应用在自动泊车轨迹跟踪系统中，在保证跟踪精度的同时，大幅度减少方向盘抖动，提高跟踪控制的平顺性.值得指出的是，该方法同样适用于其他低速大曲率路径跟踪工况.第 5 期齐志权等：自动泊车路径纯跟踪算法应用研究515参考文献：BADUE C,GUIDOLINI R,CARNEIRO R V,et al.Self-driving cars:A surveyJ.Expert Systems With Applications,2021,165:113816.1 113816.27.1 KHODAYARI A,GHAFFARI A,AMELI S,et al.A historicalreview on lateral and longitudinal control of autonomousvehicle motionsC/International Conference on Mechanicaland Electrical Technology (ICMET 2010),Singapore:IEEE,2010:421 429.2 刘凯,王威,龚建伟,等.越野地形下智能车辆的动力学建模与轨迹跟踪J.北京理工大学学报,2019,39(9):933 937.LIU Kai,WANG Wei,GONG Jianwei,et al.Dynamicmodeling and trajectory tracking of intelligent vehicles in off-road terrainJ.Transactions of Beijing Institute of Technology,2019,39(9):933 937.(in Chinese)3 陈特,徐兴,蔡英凤,等.基于状态估计的无人车前轮转角与横摆稳定协调控制J.北京理工大学学报,2021,41(10):1050 1057.CHEN Te,XU Xing,CAI Yingfeng,et al.Coordinated controlof front-wheel steering angle and yaw stability for unmannedground vehicle based on state estimationJ.Transactions ofBeijing Institute of Technology,2021,41(10):1050 1057.(inChinese)4 龚建伟,龚乘,林云龙,等.智能车辆规划与控制策略学习方法综述J.北京理工大学学报,2022,42(7):665 674.GONG Jianwei,GONG Cheng,LIN Yunlong,et al.Review onmachine learning methods for motion planning and controlpolicy of intelligent vehiclesJ.Transactions of BeijingInstitute of Technology,2022,42(7):665 674.(in Chinese)5 COULTER R C.Implementation of the pure pursuit path 6 tracking algorithmR.Pittsburgh:Carnegie Mellon UniversityPittsburgh PA Robotics Institute,CMU-RI-TR-92-01,1992.PARK M W,LEE S W,HAN W Y.Development of lateralcontrol system for autonomous vehicle based on adaptive purepursuit algorithmC/International Conference on Control,Seoul,Korea:IEEE,2014:1443 1447.7 WANG R,LI Y,FAN J,et al.A novel pure pursuit algorithmfor autonomous vehicles based on salp swarm algorithm andvelocity controllerJ.IEEE Access,2020,8:166525 166540.8 YU L,YAN X,KUANG Z,et al.Driverless bus path trackingbased on fuzzy pure pursuit control with a front axlereferenceJ.Applied Sciences,2019,10(1):230 249.9 AHN J,SHIN S,KIM M,et al.Accurate path tracking byadjusting look-ahead point in pure pursuit methodJ.International Journal of Automotive Technology,2021,22(1):119 129.10 WANG H,CHEN X,CHEN Y,et al.Trajectory tracking andspeed control of cleaning vehicle based on improved purepursuit algorithmC/2019 Chinese Control Conference(CCC),Guangzhou,China:IEEE,2019:4348 4353.11 SUKHIL V,BEHL M.Adaptive lookahead pure-pursuit forautonomous racingJ.arXiv Preprint,2021,arXiv:2111.08873.12 WANG W J,HSU T M,WU T S.The improved pure pursuitalgorithm for autonomous driving advanced systemC/2017IEEE 10th International Workshop on ComputationalIntelligence and Applications (IWCIA),Hiroshima,Japan:IEEE,2017:33 38.13（责任编辑：孙竹凤）516北 京 理 工 大 学 学 报第 43 卷