基于多点序列预瞄的自动驾驶汽车路径跟踪算法研究*

赵凯1 朱愿2 冯明月2 苏致远1 郭宏达1 （ 1.陆军军事交通学院，天津 300161；2.军事交通运输研究所，天津 300161）

【摘要】针对自动驾驶汽车自主行驶问题，提出了一种基于预瞄信息的路径跟踪算法。以GPS轨迹点序列作为目标路径，建立车辆—路径相对运动关系模型，使用实时差分GPS数据确定车辆位置。通过预瞄点序列，计算路径的预瞄偏差角和路径弯曲度。根据路径弯曲度确定行驶速度，实现纵向控制；通过 Pure Pursuit算法将预瞄偏差角转换成前轮转角的控制量，实现横向控制。试验结果表明，提出的路径跟踪方法在纵向、横向控制和跟踪平稳性方面都具有良好的效果。主题词：自动驾驶汽车 预瞄信息 路径弯曲度 路径跟踪TP242 A 10.19620/j.cnki.1000-3703.20180269中图分类号： 文献标识码： DOI:

Research on Path Tracking Algorithm of Autonomous Vehicles Based on Multi-Point Sequence Preview

Zhao Kai1, Zhu Yuan2, Feng Mingyue2, Su Zhiyuan1, Guo Hongda1 1. Army Military Transportation University, Tianjin 300161; 2. Institute of Military Transportation, Tianjin 300161） （ Abstract A path tracking algorithm based on preview information was proposed for autonomous driving of self【 】driving cars. Using the GPS trajectory point sequence as the target path, a vehicle- path relative motion relation model was established, and real- time differential GPS data was used to determine the vehicle position. Through the preview point sequence, the path's preview deviation angle and path curvature were calculated. The travel speed was determined according to the path curvature, and the longitudinal control was realized; the preview deviation angle was converted into the control amount of the front wheel rotation angle by the Pure Pursuit algorithm to realize the lateral control. The experimental results show that the proposed path tracking method has a good effect in longitudinal control, lateral control and tracking stability.

Key words: Autonomous vehicle, Preview information, Path curvature, Path tracking 1 前言

移动机器人是复杂的非线性系统，且运行于不同的复杂工况下，其路径跟踪问题一直是研究的重点和难点[1- 4]。移动机器人所跟踪的路径大致可分为显式Explicit） Implicit）

（ 和隐式（ 两种，相对于隐式路径，显式路径的基本思想是只需从传感器数据中提取特定路径的基本信息，可大幅降低处理的复杂性，因此多采用显式路径作为目标跟踪路径[5]。为提高移动机器人的路Maier D GPS径跟踪能力， 等[6]提出了一种改进的 传感器GPS

模型来生成城市区域的 轨迹地图，并使用全地形 Takai R机器人检验了模型的合理性。 等[7]将实时动态GPS（Real- time Kinematic GPS，RTK- GPS）

差分 与惯性Inertial Measurement Unit，IMU）

测量单元（ 结合，用于无人履带式拖拉机的巡线跟踪。

PID在控制策略的选择上，除传统的 控制外，考虑当前车辆姿态和跟随路径之间的几何关系亦可提供更[4]

简单的跟踪策略[8]。文献 中，谢德胜等人采用标准卡RTK- GPS尔曼滤波器对受干扰时的 数据进行滤波，用PD最优估计值作为机器人的真实位置，使用角度偏差0.67 m控制器输出转向，将横向偏差控制在 以内，但其2 m/s Zhang Wenyu

移动速度仅为 。 等[8]人采用粒度支持

Granular Support Vector Machine，GSVR）向量回归（ 的方Pure Pursuit

法提高了 算法的适应性和动态性能，使得1.2 m/s

自主机器人以 的速度跟踪路径时，直线追踪最0.088 m

大横向偏差小于 。

自动驾驶汽车作为轮式移动机器人的一种，同样存在着路径跟踪这一难题。对此，本文提出了一种基GPS于预瞄信息的路径跟踪算法。车辆接收实时差分数据，确定其在全局坐标系中的位置，根据当前位姿及其与路径间的相对运动关系建立预瞄点序列搜索模型，使用多点预瞄的策略预测路径的曲率变化，并按一定的规则控制车速。另外，将预瞄偏差角作为控制器Pure Pursuit

的输入，通过 算法计算出前轮转角控制量，用以控制车辆的转向。最后，通过试验平台验证了跟踪算法的有效性。

2 多点序列预瞄信息的获取

路径预瞄信息包括横向位置偏差、预瞄偏差角、路径弯曲度等。对这些信息加以利用能够使车辆更准确地跟踪路径[9- 10]。为了获取路径的预瞄信息，首先需建立车辆和路径的相对运动关系模型。

2.1 车辆—路径相对运动关系模型

经简化和抽象，建立的车辆—路径相对运动关系模

1

型如图 所示。其中： EON为全局（大地）坐标系， E轴正方向为正东方向， N轴正方向为正北方向；以车辆后XO′轴中点为坐标原点建立局部（车身）坐标系 Y，规定X轴的正方向为车辆前进方向；为前轮转角；为航向角， δ θ E即车辆前进方向与 轴的夹角；为前进速度； v L为轴距。 GPS

采集到的 轨迹实则是一系列有序的经纬度坐WGS- 84

标，将其由 坐标系转换到平面直角坐标系，再i= 1）将轨迹点序列的起点作为坐标原点（ 建立全局（大GPS

地）坐标系， 轨迹点在全局坐标系下的坐标表示为

( E(i),N(i))(i= 1,2,…, n- 1, n)， GPS i为 轨迹点序号。自动驾驶汽车所跟踪的目标路径以上述点序列形式给出。设

( ge,gn)，车辆的后轴中点在全局坐标系下的位置坐标为则车辆的运动学模型可表示为：

2.2 预瞄偏差角与路径弯曲度

在人工驾驶车辆过程中，驾驶员的眼睛对前方路径不断进行预瞄，根据前方路径的相关信息决定车辆的转弯方向、转角及车速，使车辆尽量逼近前方的路径。对于自动驾驶汽车的路径跟踪控制问题，参考人工驾驶行为，引入路径弯曲度和预瞄偏差角[11]的概念。假设自动驾驶汽车运动方向与连接预瞄跟踪点和α [9]，当前位置点的连线的夹角为 ，参考文献 定义该夹Preview-deviation-yaw） [9]角为预瞄偏差角（ 。根据文献的分析可知，路径跟踪的横向控制问题可以转换成预瞄偏差角的跟踪问题。对于目标路径预瞄跟踪点处曲率变化的描述，本文提出多点预瞄的预瞄策略。除在目标路径上选取一个预瞄点作为跟踪点外，其余预瞄点仅用来描述路径的曲率变化，获取前方路径的弯曲度。首先建立预瞄点搜索2

算法模型如图 所示，以自动驾驶汽车为参考对象，将GPS XO′

轨迹点序列转化到局部坐标系 Y中，则轨迹( X(i),Y(i))点序列在局部（车身）坐标系下的坐标表示为

( i= 1,2,…, n- 1, n)：

é X( i) ù é E( i) g ù cosθ sinθ -

= - sinθ cosθ · e （ 2）

( ê - ú

Y i) N( i) g n 2

图 中，为预瞄距离； ρ ed为横向位置偏差，即当前位GPS Pj(j= 1,2,…, n- 1, n)

置与 轨迹之间的距离偏差； 为在GPS

目标路径即 轨迹点序列上获取的预瞄点序列。搜索预瞄跟踪点P1的步骤为： a. 2） GPS

由式（ 将 轨迹点转化到局部坐标系下； b. GPS

在描述目标路径的 轨迹点序列中找出与车辆当前位置距离最近的点，作为本次搜索的起始点； c. GPS从起始点开始，沿车身前进方向，在 点序列中依次寻找一个点满足：

X( p1) - ρ][

X( p1 + 1)- ρ]≤ 0 3） （式中， p1为满足式（ 3） GPS

的点在 轨迹点序列中的序号。