基于Pure Pursuit算法的预瞄点确定方法
【技术领域】
[0001] 本发明涉及一种基于Pure Pursuit算法的预目苗点确定方法，属于智能车辆路径跟 踪相关领域。 技术背景
[0002] 为了提高交通运输效率和交通安全，世界各国对智能交通系统和安全驾驶等问题 的研宄投入了大量的人力、物力和财力。美国DARPA自2004年以来，连续三年举办了三届 智能车挑战赛活动，进一步促进了智能车领域和智能车技术的发展，2012年谷歌公司发布 了一款无人驾驶智能车，并预计在2018年正式上市。除了研宄机构、科技公司，许多汽车制 造商也都纷纷投入到智能车技术的研宄中，奥迪公司的Q7轿车上使用了 Hella公司的车道 变更告警系统、沃尔沃公司推出的激光防撞系统等。2015年4月，由汽车配件商德尔福研发 的一辆无人驾驶汽车完成了横跨美国之旅，再次说明近年来无人驾驶智能车技术的飞速发 展。"中国智能车未来挑战赛"是由我国国家自然科学基金委员会举办的无人驾驶智能车比 赛，自2009年开始，我国已成功举办了六届"中国智能车未来挑战赛"。
[0003] 无人驾驶智能车的控制主要包括横向控制和纵向控制两部分。其中横向控制主要 指对车辆转向系统的控制。路径跟踪是智能车横向控制的一个主要应用。路径跟踪是指在 已经得到一条期望路径的前提下，智能车根据大地坐标系中的位置信息，按照一定的控制 策略，使智能车的实际行驶路径可以与规划路径达到一致。目前被广泛应用在无人驾驶智 能车路径跟踪的算法基本上都是通过调节实际路径与规划路径之间存在的偏差以实现对 目标路径进行跟踪，如方位误差、距离误差等。其中Pure Pursuit算法、斯坦利算法、环形 预瞄算法是几类常见的路径跟踪算法。
【发明内容】
[0004] 本发明的目的在于提出一种确定规划路径上预瞄点的方法，根据车速和道路， 确定预目苗点，并利用Pure Pursuit算法实现车辆的路径跟踪，相较于传统的基于Pure Pursuit算法确定预目苗点，该算法可准确、快速地确定规划路径上的预目苗点，并对前方道路 进行提前反应，确保车辆在直道和弯道行驶过程中的平稳性，更好地实现智能车的路径跟 踪，同时该算法也可降低规划路径数据点，减少了算法在程序实现过程中数据量，降低了程 序运算时间。
[0005] Pure Pursuit算法的原理为以规划路径上的某一个点为预目苗点，通过控制车辆转 向角，使车辆可以沿着一条经过该预瞄点的圆弧行驶，最终实现路径跟踪。因此，预瞄点的 确定是该算法实现的重要环节。本发明提出了一种根据几何特性确定预瞄点的方法，采用 该方法可以降低对规划路径数据点的要求、减少数据计算量、提高算法的准确性。
[0006] -种基于Pure Pursuit算法的预目苗点确定方法，包括以下步骤：
[0007] 步骤I :GPS位置信息坐标系转换。
[0008] GPS输出数据经度、炜度、高度坐标值，并不适合于平面运动的车辆导航。采用通用 横轴墨卡托坐标系（Universal Transverse Mercator，UTM)将经炜度转化到平面坐标系。
[0009] 步骤2 :根据车辆参数和当前车辆行驶速度确定预瞄距离 [0010] 根据人类驾驶车辆的经验，车辆以不同车速行驶在道路上时，驾驶员的预瞄距离 也随之改变，在低速行驶时，驾驶员预瞄前方道路的距离较近，车速增大，驾驶员的预瞄距 离也随之增大。
[0011] 步骤3 :判断车辆与规划路径位置
[0012] 步骤4:确定预目苗点
[0013] 步骤5:道路判断
[0014] 道路判断主要用于判断前方道路的状况，即车辆行驶在直线道路上还是行驶在弯 道上。对于不同道路状况，预瞄点的筛选与略有不同。
[0015] 步骤6:算法的实现
[0016] 得到预瞄点和预瞄距离后，根据Pure Pursuit算法可计算出转向角控制量，为了 提高车辆在直线或弯道环境中车辆行驶的平稳性，制定了更改比例增益的控制策略。
[0017] 具体实施过程如下，
[0018] 步骤I :GPS位置信息坐标系换算
[0019] GPS接收机接收到的GPS信号ASCII码语句，包括GPGGA、GPGSA、GPGSV、GPRMC ; PGRME、PGRMM、PGRMT (GARMIN定义的语句）。因此得到的位置信息属于空间大地坐标系，需 要转换为UTM直角坐标系。
[0020] UTM坐标系是将地球划分成60个投影带，以180°经线为起始线，向东每个6°划 分为一个纵带，每个纵带中部的经线被命名为中央经线，并定义中央经线坐标值为500000， 在南炜80°至北炜84°范围内，分别从赤道开始，每隔8°划分一个横带。
[0021] 空间大地坐标系转换为UTM坐标系，其中r = 6378. 137km，r为地球半径；kQ = 0. 9996, kQ为坐标系转化系数；ec = 0. 0818192, ec为偏心率；（λ，φ)为经炜度坐标，λ为 经度，iP为炜度；（e，η)为UTM坐标，e为方向东，η为方向北；Φ表示Φ的弧度值。因此可 得到如下公式：
[0030] 步骤2 :根据车辆参数和当前车辆行驶速度确定预瞄距离
[0031] 预瞄距离是无人驾驶智能车实现路径跟踪的一个重要参数，预瞄距离与车速存在 一定的数学关系，即通过车辆行驶速度确定预瞄距离的公式为：
[0032] Id= AV 2+BV+C (3)
[0033] 其中，V为车辆行驶速度，A、B、C为常数项。
[0034] AV2表示车辆制动距离。其中，A可表示为：
[0035] A = l/2amax
[0036] a_为车辆最大制动加速度。
[0037] BV表示车辆遇到异常情况进行反应的车辆行驶距离。
[0038] C表示车辆最小转弯半径。
[0039] 步骤3 :判断车辆与规划路径位置
[0040] 基于Pure Pursuit算法的智能车路径跟踪实际上是以预瞄距离为半径，以当前车 体位置为圆心做圆，确定预目苗点，进而通过Pure Pursui t算法得出转向角控制量，实现路径 跟踪。因此，以车体位置为圆心，预瞄距离为半径画出的圆与期望路径存在三种位置关系： 相交、相切和相离。
[0041] Id为预瞄距离，α为跟踪航向角度偏差，（cx，Cy)为车辆当前位置，（G x，Gy)为期望 路径上的预瞄点。
[0042] 期望路径为一条已知的路径，根据期望路径上相邻两点坐标可得到直线方程的斜 率K和常数项B，因此可得到预瞄点（Gx，Gy)之间的关系为：
[0043] Gy=KGx+B (4)
[0044] 根据两点间距离公式可得到：
[0045] Id2= (Cx-Gx)2+(Cy-Gy) 2 (5)
[0046] 将公式⑷代入公式（5)并整理：
[0052] 贝丨J :
[0053] aGx2+bGx+c = 0 (6)
[0054] 根据一元二次方程求根公式可对公式进行求解，并判断期望路径与预瞄范围之间 的关系：
[0055] 当b2_4ac>0时，规划路径与圆相交。
[0056] 当b2_4ac = 0时，规划路径与圆相切。
[0057] 当b2_4ac〈0时，规划路径与圆相离。
[0058] 步骤4:确定预目苗点
[0059] 当规划路径与圆相交时，一元二次方程存在两个解集（xl，yl)、（x2，y2)，其中一个 解为路径跟踪算法的预瞄点，另一个解为车辆已行驶过的区域，为无用点，因此要需根据方 程解与规划路径上点的关系，选择可用的预瞄点，删除无用点。当规划路径与圆相交时，方 程两个解与期望路径上连续的两个点（xk，yk)、（xk+l，yk+l)存在12种位置关系。其中， ldl、ld2 分别为点（xl，yl)到（xk, yk)，（xk+1, yk+Ι)的距离，ld3、ld4 分别为点（x2, y2) 到（xk，yk)，（xk+l，yk+l)的距离，ld5 为（xk，yk)，（xk+l，yk+l)两点间的距离。
[0060] 当（xl，yl)、（x2, y2)在点（xk，yk)x 下侧，且点（xl，yl)位于（x2, y2)下侧。此 时（x2, y2)为预瞄点，（x2, y2)为（Gx，Gy)，可根据如下公式进行判定：
[0062]当（xl，yl)、（x2，y2)在点（xk，yk)下侧，且点（xl，yl)位于（x2，y2)上侧。此时 (xl，yl)为预瞄点，（xl，yl)为（Gx，Gy)，可根据如下公式进行判定：
[0064]当（xl，yl)在点（xk, yk)下侧，（x2, y2)位于（xk, yk)，（xk+1, yk+Ι)两点间。此 时（x2, y2)为预瞄点，即（x2, y2)为（Gx，Gy)，可根据如下公式进行判定：
[0066]当（xl，yl)在点（xk, yk)下侧，（x2, y2)位于（xk+1, yk+Ι)上侧。此时（x2, y2) 为预瞄点，即（x2，y2)为（Gx，Gy)，可根据，如下公式进行判定：
[0068]当（xl，yl)位于（xk，yk)、（xk+l，yk+l)两点间，且（x2, y2)位于（xk，yk)下侧。 此时（xl，yl)为预瞄点，即（x2, y2)为（Gx，Gy)，可根据如下公式进行判定：
[0070]当（xl，yl)、（x2, y2)位于（xk, yk)、（xk+1, yk+Ι)两点间，且（xl，yl)位于（x2， y2)下侧。此时（x2，y2)为预瞄点，即（x2，y2)为（Gx，Gy)，可根据如下公式进行判定：
[0072]当（xl，yl)