基于Pure Pursuit算法的智能车路径跟踪

段建民，杨 晨，石 慧

（北京工业大学城市交通学院，北京 100124）

基于Pure Pursuit算法的智能车路径跟踪

段建民，杨 晨，石 慧

（北京工业大学城市交通学院，北京 100124）

为了实现智能车路径跟踪，根据电子式助力转向系统的工作原理，对北京工业大学智能车BJUT-IV的转向系统进行设计.根据车辆参数和车速确定预瞄距离，并通过车辆与规划路径间存在的位置关系确定预瞄点，采用Pure Pursuit算法计算出前轮偏角控制量.建立车辆横向运动学模型，并在Matlab上进行了路径跟踪仿真实验.最后，与GPS相融合，根据GPS提供的车体位置信息，在校园规定线路对路径跟踪算法进行验证.实验结果表明:该算法跟踪偏差较小且前轮摆动幅度适中，确保了车辆在行驶过程中的稳定性.

智能车；电子式助力转向系统；横向控制；路径跟踪

智能车辆的横向控制是指在无人驾驶环境下对智能车转向系统的自动控制，主要目的是实现跟踪道路，使车辆始终沿着期望路径行驶［1-2］.

道路跟踪系统根据使用传感器的不同可分为预瞄式和非预瞄式参考系统［2-3］.预瞄式路径跟踪是指在智能车行驶过程中模拟人类驾驶习惯，通过比较车体当前位置与期望路径间的偏差，对转向系统进行控制，从而使车辆沿着期望轨迹行驶.

本文以北京工业大学智能车BJUT-IV为实验平台，根据电子式助理转向系统的工作原理，对智能车的转向系统进行改装；根据车辆与规划路径之间存在的几何位置关系，确定期望轨迹上的预瞄点，采用Pure Pursuit跟踪算法，实现对期望路径的跟踪.通过建立车辆横向系统运动学数学模型，在Matlab上进行了路径跟踪仿真实验；结合GPS、里程计等传感器提供的位置、速度信息，在实际校园环境中完成对期望道路的跟踪.

1 转向系统的设计

1.1硬件构成

智能车的横向控制主要是对智能车转向的控制，因此，转向系统在接收到控制指令后，需要准确、快速地完成转向动作，实现转弯、躲避障碍物等任务.

BJUT-IV转向控制系统以电子式电动助力转向系统（electric power steering，EPS）为基础进行改装设计，主要包括增量式光电编码器、转角传感器、永磁同步电机（额定功率240 W）、转向柱和控制器等硬件，如图1所示.

1.2转向系统的控制策略

转向控制系统中控制器、转向电机、霍尔元件和光电编码器构成了一个双闭环控制系统，即电流环和位置环.电流环的输入为位置环的输出，通过霍尔元件可检测到控制器输出给转向电机的电流值，电流环给定输入和反馈值进行比较，经控制器调节输出给电机；位置环的输入是上层控制器发送的电机期望位置，光电编码器可实时检测转向电机的位置反馈给控制器，通过比较期望值与反馈值，底层控制器对电机进行控制.转向控制系统框图如图2所示.

底层控制器采用PID控制，对控制器进行调试，分别得到PID控制器电流环和位置环的参数如下:

1.3转向系统实验分析

转向系统设计完成后，对转向电机位置进行了检测，其结果如图3所示.

图3中2条曲线分别为参考位置和电机实际转动脉冲.可知，电机参考位置为3转，电机实际转动为2.976转，误差为0.8％，对实际应用无明显影响.

2 基于Pure Pursuit算法的横向控制

2.1Pure Pursuit算法原理

Pure Pursuit算法，被广泛应用在机器人的路径跟踪上.该算法以智能车后轴为切点，车辆纵向车身为切线，通过控制前轮偏角δ，使车辆可以沿着一条经过预瞄点的圆弧行驶［4］，如图4所示.

其中:（Gx，Gy）为规划路径上的预瞄点；（Cx，Cy）为车辆当前位置；ld为预瞄距离，即后轮与预瞄点的距离；α为车与预瞄点的夹角.根据正弦定理可到

式（1）也可表示为

式中κ为圆弧曲率.根据简化的阿克曼车辆模型，前轮偏角表示为

根据式（2）（3）可以得到Pure Pursuit算法的控制量表达式

2.2预瞄距离的确定

预瞄距离是无人驾驶智能车实现路径跟踪的一个重要参数，预瞄距离与车速存在一定的数学关系，根据文献［5］，可得到预瞄距离计算式

式中:V为车辆行驶速度；A、B、C为常数项.

式（5）第1项为常数A与速度V的平方的乘积，表示车辆制动距离.其中，A可表示为

amax为车辆最大制动加速度，经试验测量，智能车最大制动加速度为3 m/s2.

式（5）第2项为常数B与速度V的乘积，表示车辆遇到异常情况进行反应的车辆行驶距离，令B 为0.2.

C表示车辆最小转弯半径.根据BJUT-IV智能车整车参数，最小转弯半径为5 m，前轮偏角最大值为23°.

因此，预瞄距离式（5）可表示为

根据式（6）可得到预瞄距离与车辆速度之间函数关系图，如图5所示.

2.3预瞄点的确定

基于Pure Pursuit算法的智能车路径跟踪实际上是以预瞄距离为半径，以当前车体位置为圆心做圆，该圆与规划路径存在如图6～8所示的3种位置关系:相交、相切和相离.根据这3种位置关系，可以确定预瞄点，进而通过Pure Pursuit算法得出前轮偏角控制量，实现路径跟踪.

当圆与期望路径相交时，可根据其几何特性筛选合适预瞄点；由于中国车道宽度标准采用波良科夫模型［6］，常见道路宽度在3.5 m左右，而根据式（6）计算出的最小预瞄距离为5 m.因此，存在如图7、8位置关系的情况只可能发生在车辆驶入弯道时.因此，可根据智能车航向角与期望路径间的夹角α进行相应的控制:当α＜0时，车辆向期望路径方向行驶，无须对转角进行控制，当行驶至图6状态时，可采用Pure Pursuit算法进行控制；当α≥0时，车辆背离期望路径，须对前轮偏角进行修正，使车辆向期望路径方向行驶，当行驶至图6状态时，采用Pure Pursuit算法进行控制.

3 仿真

在仿真实验中，给出车体初始位置和航向角，根据车辆横向运动学模型计算车辆下一时刻的车体位置和航向角等信息，模拟GPS返回给智能车的车体信息.

根据文献［4］可计算出下一时刻车体位置和航向角

式中:x，y为车体位置；θ为航向角；δ为前轮偏角；v为车速；k为当前时刻；k+1为下一时刻.

如图9所示，其中黑色虚线为规划路径，红线代表车辆以2 m/s的速度行驶时车辆行驶轨迹，蓝线为车辆7 m/s的速度行驶时车辆行驶轨迹.图10分别为对应2 m/s和7 m/s车速时，前轮偏角控制量随时间变化的曲线.从仿真结果可以看出，该跟踪算法在不同车速条件下都可以实现对期望路径的良好跟踪，跟踪轨迹基本吻合.这是因为，随着车速的增大，预瞄距离也随之增大，使车辆可以提前对前方道路进行处理，但是随着速度的增大，前轮偏角变化率也随之增大，这对于车辆在实际入弯时的稳定性存在一定影响，因此在实际应用中，当车辆驶入弯道时，应将车速降低到一定的数值，确保车辆入弯的平稳性.

4 GPS数据处理

全球定位系统（global positioning system，GPS），具有迅速、准确、可靠等优点，可以全天候地为车辆提供导航、定位信息［7］.

北京工业大学智能车BJUT-IV采用NovAtel公司生产的FlexPak-G2接收机和GPS-702-GG接收天线，以及PACIFIC CREST公司PDL4535数传电台.采用载波相位差分技术，又称RTK技术（real time kinematic）将定位精度达到厘米级，以实现智能车在运动过程中的实时定位功能.

接收到的GPS信号为美国国家海洋电子协会（National Marine Electronics Association）制定的NMEA0183版本3.00的ASCII码语句，包括GPGGA、GPGSA、GPGSV、GPRMC（NMEA标准语句）；PGRME、PGRMM、PGRMT（GARMIN定义的语句）［8］.因此得到的位置信息属于空间大地坐标系，需要转换为空间直角坐标系.在相同基准下，空间大地坐标系转换为空间直角坐标系方法为［9］

GPS除了可以提供车辆实时位置信息，也可通过计算当前时刻与上一时刻两点的斜率计算出车辆行驶的航向角，计算公式为

5 软件设计及实验结果

采用UDP网络传输协议，将GPS数据处理上位机得到的车体位置和航向角等信息传输给BJUTIV决策上位机.决策上位机根据这些信息，结合规划路径，计算出位于规划路径上的预瞄点，利用Pure Pursuit算法得出期望的前轮偏角控制量，之后经控制器得到最终下发给转向电机的位置控制信息.路径跟踪算法软件设计流程如图11所示.

软件界面如图12所示，主要包括串口参数设置区域、车体信息实时采集显示区域、人工控制区域、自动控制区域、数据保存区域、车辆实时地图.

本文实验内容为在规定校园道路环境下，实现智能车的路径跟踪.智能车BJUT-IV根据车上安装的里程计和GPS返回的车体位置信息计算出行驶里程和瞬时车速，根据车辆当前位置和航向角，利用Pure Pursuit算法计算前轮偏角控制量，使车辆的实际轨迹按照规划路径行驶，实验结果如图13、14所示.其中图13为车辆实际行驶轨迹与期望路径对比图，图14为车辆前轮偏角随时间的变化曲线.

实验证明，智能车BJUT-IV的转向系统可以快速、准确地对车辆的前轮偏角进行控制.同时，采用在不同车速下调整预瞄距离的Pure Pursuit算法，很好地实现了在北京工业大学规定校园路径中的路径跟踪实验:在直线道路上最大偏差为0.5 m，前轮偏角最大摆动角度为5.7°，减小了车辆在直线道路上的摆动，弯道时最大偏差约1.6 m，前轮偏角最大摆动角度为17°.

6 结论

1）根据电子式助力转向系统的结构和工作原理，对北京工业大学智能车BJUT-IV的转向系统进行了设计并应用，经测试，该转向系统的误差为0.8％，可以准确、快速地对前轮偏角进行控制.

2）根据车辆参数和车速确定预瞄距离，通过车辆与规划路径间存在的位置关系确定预瞄点，然后基于Pure Pursuit算法，实现了在Matlab上的路径跟踪仿真，仿真结果表明该算法可以在不同车速条件中对规划路径进行跟踪，跟踪效果良好.

3）在规定校园道路环境中，根据GPS返回的车体位置和航向角等信息，实现了车辆的路径跟踪实验，跟踪偏差较小且前轮摆动幅度适中，确保了车辆在行驶过程中的稳定性. for intelligent vehicl［M］.Berlin:Springer，2007.

［2］赵熙俊，陈慧岩.智能车辆路径跟踪横向控制方法的研究［J］.汽车工程，2011（5）:382-387. ZHAO X J，CHEN H Y.A study on lateral control method for the path tracking of intelligent vehicles［J］.Automotive Engineering，2011（5）:382-387.（in Chinese）

［3］HERNANDEZ J I，CHEN-YUAN K.Steering control of automated vehicles using absolute positioning GPS and magnetic markers［J］.IEEE Transactions on Vehicular Technology，2003，52（1）:150-161.

［4］SNIDER J M.Automatic steering methods for autonomous automobilepath tracking，CMU［D］.Pittsburgh:Carnegie Mellon University，2009.

［5］刘凯.无人驾驶车辆体系结构与定位导航技术研究［D］.北京:北京理工大学，2010. LIU K.Study of architecture system and navigation technology of unmanned grand vehicle［D］.Beijing: Beijing Institute of Technology，2010.（in Chinese）

［6］李军，李俭全，李剑.城市道路车道宽度计算型修正研究［J］.西部交通科技，2010（8）:7-11. LI J，LI J Q，LI J.Research on municipal road width calculation modeling［J］.Western China Communications Science＆amp;Technology，2010（8）:7-11.（in Chinese）

［7］游胜玉.GPS_DR车辆组合定位技术的研究［D］.南昌:江西师范大学，2007. YOU S Y.Research on the technologies of GPS/DR integrated positioning for vehicle［D］.Nanchang:Jiangxi Normal University，2007.（in Chinese）

［8］夏州.GPS车辆导航中的数据处理与地图匹配研究［D］.北京:北京交通大学，2009. XIA Z.Research of GPS data proeessing and map matching in vehicle navigation system［D］.Beijing: Beijing Jiaotong University，2009.（in Chinese）

［9］王雷杰.陀螺仪-全球定位的汽车组合导航算法的研究［D］.长春:吉林大学，2004. WANG L J.Research on vehicle navigation algorithm integrating gyroscope with global positioning system［D］. Changchun:Jilin University，2004.（in Chinese）

（责任编辑 杨开英）

［1］LI L，FEIYUE W.Advanced motion control and sensing

Path Tracking Based on Pure Pursuit Algorithm for Intelligent Vehicles

DUAN Jianmin，YANG Chen，SHI Hui
（College of Metropolitan Transportation，Beijing University of Technology，Beijing 100124，China）

To achieve intelligent vehicle path tracking，an electric power steering（EPS）system as the foundation was modified and mounted on the steering system of Beijing University of Technology Intelligent Vehicle（BJUT-IV）.According to parameters of the vehicle and the speed，the look-ahead distance was obtained.According to position and planning path of the vehicle，the preview point was selected.Using the Pure Pursuit algorithm，the vehicle lateral kinematics model and simulation on the Matlab were established.The intelligent vehicle control system PC software was designed.In the experiment，according to the location information provided by GPS，the unmanned experiment in the campus road was completed.Experimental results show that the algorithm tracking error is small，and the swing magnitude of the front is moderate to ensure the stability of the vehicle during driving.

intelligent vehicle；electric power steering（EPS）；lateral control；path tracking

TP 271.4

A

0254-0037（2016）09-1301-06

10.11936/bjutxb2015060065

2015-06-25

北京市属高等学校人才强教计划资助项目（038000543115025）

段建民（1959—），男，教授，主要从事车辆环境信息融合、道路识别、自动驾驶技术、嵌入式汽车电子控制技术、网络化监控技术和智能化仪器仪表方面的研究，E-mail:jmduan@bjut.edu.cn