CN104960520A - 基于Pure Pursuit算法的预瞄点确定方法 - Google Patents

Abstract

基于Pure Pursuit算法的预瞄点确定方法，首先将GPS提供的空间大地坐标信息转换为UTM直角坐标信息，并将该信息传送给决策上位机；然后根据车辆当前行驶车速确定预瞄距离；以GPS提供的车辆当前位置为圆心，预瞄距离为半径画圆，根据该圆与期望路径的位置关系，确定预瞄点；利用Pure Pursuit算法，计算出智能车转角控制量，从而实现车辆对规划路径的跟踪。本发明针对无人驾驶智能车在规定路径上的跟踪实验，通过判断规划路径与预瞄圆的位置关系，确定预瞄点，筛选算法新颖，且计算量小、运算效率高、易于计算机编程实现，提高了算法的准确性和可靠性。

Description

基于Pure Pursuit算法的预瞄点确定方法

技术领域

本发明涉及一种基于Pure Pursuit算法的预瞄点确定方法，属于智能车辆路径跟踪相关领域。

技术背景

为了提高交通运输效率和交通安全，世界各国对智能交通系统和安全驾驶等问题的研究投入了大量的人力、物力和财力。美国DARPA自2004年以来，连续三年举办了三届智能车挑战赛活动，进一步促进了智能车领域和智能车技术的发展，2012年谷歌公司发布了一款无人驾驶智能车，并预计在2018年正式上市。除了研究机构、科技公司，许多汽车制造商也都纷纷投入到智能车技术的研究中，奥迪公司的Q7轿车上使用了Hella公司的车道变更告警系统、沃尔沃公司推出的激光防撞系统等。2015年4月，由汽车配件商德尔福研发的一辆无人驾驶汽车完成了横跨美国之旅，再次说明近年来无人驾驶智能车技术的飞速发展。“中国智能车未来挑战赛”是由我国国家自然科学基金委员会举办的无人驾驶智能车比赛，自2009年开始，我国已成功举办了六届“中国智能车未来挑战赛”。

无人驾驶智能车的控制主要包括横向控制和纵向控制两部分。其中横向控制主要指对车辆转向系统的控制。路径跟踪是智能车横向控制的一个主要应用。路径跟踪是指在已经得到一条期望路径的前提下，智能车根据大地坐标系中的位置信息，按照一定的控制策略，使智能车的实际行驶路径可以与规划路径达到一致。目前被广泛应用在无人驾驶智能车路径跟踪的算法基本上都是通过调节实际路径与规划路径之间存在的偏差以实现对目标路径进行跟踪，如方位误差、距离误差等。其中Pure Pursuit算法、斯坦利算法、环形预瞄算法是几类常见的路径跟踪算法。

发明内容

本发明的目的在于提出一种确定规划路径上预瞄点的方法，根据车速和道路，确定预瞄点，并利用Pure Pursuit算法实现车辆的路径跟踪，相较于传统的基于Pure Pursuit算法确定预瞄点，该算法可准确、快速地确定规划路径上的预瞄点，并对前方道路进行提前反应，确保车辆在直道和弯道行驶过程中的平稳性，更好地实现智能车的路径跟踪，同时该算法也可降低规划路径数据点，减少了算法在程序实现过程中数据量，降低了程序运算时间。

Pure Pursuit算法的原理为以规划路径上的某一个点为预瞄点，通过控制车辆转向角，使车辆可以沿着一条经过该预瞄点的圆弧行驶，最终实现路径跟踪。因此，预瞄点的确定是该算法实现的重要环节。本发明提出了一种根据几何特性确定预瞄点的方法，采用该方法可以降低对规划路径数据点的要求、减少数据计算量、提高算法的准确性。

一种基于Pure Pursuit算法的预瞄点确定方法，包括以下步骤：

步骤1：GPS位置信息坐标系转换。

GPS输出数据经度、纬度、高度坐标值，并不适合于平面运动的车辆导航。采用通用横轴墨卡托坐标系(Universal Transverse Mercator，UTM)将经纬度转化到平面坐标系。

步骤2：根据车辆参数和当前车辆行驶速度确定预瞄距离

根据人类驾驶车辆的经验，车辆以不同车速行驶在道路上时，驾驶员的预瞄距离也随之改变，在低速行驶时，驾驶员预瞄前方道路的距离较近，车速增大，驾驶员的预瞄距离也随之增大。

步骤3：判断车辆与规划路径位置

步骤4：确定预瞄点

步骤5：道路判断

道路判断主要用于判断前方道路的状况，即车辆行驶在直线道路上还是行驶在弯道上。对于不同道路状况，预瞄点的筛选与略有不同。

步骤6：算法的实现

得到预瞄点和预瞄距离后，根据Pure Pursuit算法可计算出转向角控制量，为了提高车辆在直线或弯道环境中车辆行驶的平稳性，制定了更改比例增益的控制策略。

具体实施过程如下，

步骤1：GPS位置信息坐标系换算

GPS接收机接收到的GPS信号ASCII码语句,包括GPGGA、GPGSA、GPGSV、GPRMC；PGRME、PGRMM、PGRMT(GARMIN定义的语句)。因此得到的位置信息属于空间大地坐标系，需要转换为UTM直角坐标系。

UTM坐标系是将地球划分成60个投影带，以180°经线为起始线，向东每个6°划分为一个纵带，每个纵带中部的经线被命名为中央经线，并定义中央经线坐标值为500000，在南纬80°至北纬84°范围内，分别从赤道开始，每隔8°划分一个横带。

空间大地坐标系转换为UTM坐标系，其中r＝6378.137km，r为地球半径；k₀＝0.9996，k₀为坐标系转化系数；ec＝0.0818192，ec为偏心率；为经纬度坐标，λ为经度，为纬度；(e，n)为UTM坐标，e为方向东，n为方向北；Ф表示的弧度值。因此可得到如下公式：

$e=k_{0}rv\left(\mathrm{\Φ}\right)\[A+(1-\mathrm{\Ψ}+C)\frac{A^3}{6}+(5-18\mathrm{\Ψ}+{\mathrm{\Ψ}}^2)\frac{A^5}{120}\]---\left(1\right)$

$n=k_{0}r\left(s\right(\mathrm{\Φ})+v(\mathrm{\Φ})tan\mathrm{\Φ}\[\frac{A^2}{2}+(5-\mathrm{\Ψ}+9C+4C^2)\frac{A^4}{24}+(61-58\mathrm{\Ψ}+{\mathrm{\Ψ}}^2)\frac{A^6}{720}\])---\left(2\right)$

式中：

$v\left(\mathrm{\Φ}\right)=\frac{1}{\sqrt{1-{\mathrm{ec}}^2{\sin }^2\mathrm{\Φ}}}$

$\begin{array}{c}s\left(\mathrm{\Φ}\right)=(1-\frac{{\mathrm{ec}}^2}{4}-\frac{3{\mathrm{ec}}^4}{64}-\frac{5{\mathrm{ec}}^6}{256})\mathrm{\Φ}-(\frac{3{\mathrm{ec}}^2}{8}+\frac{3{\mathrm{ec}}^4}{32}+\frac{45{\mathrm{ec}}^6}{1024})\sin 2\mathrm{\Φ}\\ +(\frac{15{\mathrm{ec}}^4}{256}+\frac{45{\mathrm{ec}}^6}{1024})\sin 4\mathrm{\Φ}-\frac{35{\mathrm{ec}}^6}{3072}\sin 4\mathrm{\Φ}\end{array}$

A＝(λ-λ_ref)cosФ

$C=\frac{{\mathrm{ec}}^2}{1-{\mathrm{ec}}^2}{\cos }^2\mathrm{\Φ}$

Ψ＝tan²Ф

步骤2：根据车辆参数和当前车辆行驶速度确定预瞄距离

预瞄距离是无人驾驶智能车实现路径跟踪的一个重要参数，预瞄距离与车速存在一定的数学关系，即通过车辆行驶速度确定预瞄距离的公式为：

l_d＝AV²+BV+C  (3)

其中，V为车辆行驶速度，A、B、C为常数项。

AV²表示车辆制动距离。其中，A可表示为：

A＝1/2a_max

a_max为车辆最大制动加速度。

BV表示车辆遇到异常情况进行反应的车辆行驶距离。

C表示车辆最小转弯半径。

步骤3：判断车辆与规划路径位置

基于Pure Pursuit算法的智能车路径跟踪实际上是以预瞄距离为半径，以当前车体位置为圆心做圆，确定预瞄点，进而通过Pure Pursuit算法得出转向角控制量，实现路径跟踪。因此，以车体位置为圆心，预瞄距离为半径画出的圆与期望路径存在三种位置关系：相交、相切和相离。

l_d为预瞄距离，α为跟踪航向角度偏差，(C_x，C_y)为车辆当前位置，(G_x，G_y)为期望路径上的预瞄点。

期望路径为一条已知的路径，根据期望路径上相邻两点坐标可得到直线方程的斜率K和常数项B，因此可得到预瞄点(G_x，G_y)之间的关系为：

G_y＝KG_x+B  (4)

根据两点间距离公式可得到：

l_d ²＝(C_x-G_x)²+(C_y-G_y)²  (5)

将公式(4)代入公式(5)并整理：

(1+K²)G_x ²+2(K·B-C_x-K·C_y)G_x+C_x ²+C_y ²+B²-2C_y·B-l_d ²＝0

令：

a＝1+K²

b＝2(K·B-C_x-K·C_y)

c＝C_x ²+C_y ²+B²-2C_y·B-l_d ²

则：

aG_x ²+bG_x+c＝0  (6)

根据一元二次方程求根公式可对公式进行求解，并判断期望路径与预瞄范围之间的关系：

当b²-4ac>0时，规划路径与圆相交。

当b²-4ac＝0时，规划路径与圆相切。

当b²-4ac<0时，规划路径与圆相离。

步骤4：确定预瞄点

当规划路径与圆相交时，一元二次方程存在两个解集(x1，y1)、(x2，y2)，其中一个解为路径跟踪算法的预瞄点，另一个解为车辆已行驶过的区域，为无用点，因此要需根据方程解与规划路径上点的关系，选择可用的预瞄点，删除无用点。当规划路径与圆相交时，方程两个解与期望路径上连续的两个点(xk,yk)、(xk+1,yk+1)存在12种位置关系。其中，ld1、ld2分别为点(x1，y1)到(xk,yk)，(xk+1,yk+1)的距离，ld3、ld4分别为点(x2，y2)到(xk,yk)，(xk+1,yk+1)的距离，ld5为(xk,yk)，(xk+1,yk+1)两点间的距离。

当(x1，y1)、(x2，y2)在点(xk,yk)x下侧，且点(x1，y1)位于(x2，y2)下侧。此时(x2，y2)为预瞄点，(x2，y2)为(G_x，G_y)，可根据如下公式进行判定:

$\left\{\begin{array}{c}ld1+ld2>ld5\\ ld3+ld4>ld5\\ ld1>ld3\\ \frac{ld1}{ld2}<1\\ \frac{ld3}{ld4}<1\end{array}\right.---\left(7\right)$

当(x1，y1)、(x2，y2)在点(xk,yk)下侧，且点(x1，y1)位于(x2，y2)上侧。此时(x1，y1)为预瞄点，(x1，y1)为(G_x，G_y)，可根据如下公式进行判定：

$\left\{\begin{array}{c}ld1+ld2>ld5\\ ld3+ld4>ld5\\ ld1<ld3\\ \frac{ld1}{ld2}<1\\ \frac{ld3}{ld4}<1\end{array}\right.---\left(8\right)$

当(x1，y1)在点(xk,yk)下侧，(x2，y2)位于(xk,yk)，(xk+1,yk+1)两点间。此时(x2，y2)为预瞄点，即(x2，y2)为(G_x，G_y)，可根据如下公式进行判定:

$\left\{\begin{array}{c}ld1+ld2>ld5\\ ld3+ld4=ld5\\ \frac{ld1}{ld2}<1\end{array}\right.---\left(9\right)$

当(x1，y1)在点(xk,yk)下侧，(x2，y2)位于(xk+1,yk+1)上侧。此时(x2，y2)为预瞄点，即(x2，y2)为(G_x，G_y)，可根据，如下公式进行判定:

$\left\{\begin{array}{c}ld1+ld2>ld5\\ ld3+ld4>ld5\\ \frac{ld1}{ld2}<1\\ \frac{ld3}{ld4}>1\end{array}\right.---\left(10\right)$

当(x1，y1)位于(xk,yk)、(xk+1,yk+1)两点间，且(x2，y2)位于(xk,yk)下侧。此时(x1，y1)为预瞄点，即(x2，y2)为(G_x，G_y)，可根据如下公式进行判定：

$\left\{\begin{array}{c}ld1+ld2=ld5\\ ld3+ld4>ld5\\ \frac{ld3}{ld4}<1\end{array}\right.---\left(11\right)$

当(x1，y1)、(x2，y2)位于(xk,yk)、(xk+1,yk+1)两点间，且(x1，y1)位于(x2，y2)下侧。此时(x2，y2)为预瞄点，即(x2，y2)为(G_x，G_y)，可根据如下公式进行判定:

$\left\{\begin{array}{c}ld1+ld2=ld5\\ ld3+ld4=ld5\\ ld1<ld3\end{array}\right.---\left(12\right)$

当(x1，y1)、(x2，y2)位于(xk,yk)、(xk+1,yk+1)两点间，且(x1，y1)位于(x2，y2)上侧。此时(x1，y1)为预瞄点，即(x1，y1)为(G_x，G_y)，可根据如下公式进行判定:

$\left\{\begin{array}{c}ld1+ld2=ld5\\ ld3+ld4=ld5\\ ld1>ld3\end{array}\right.---\left(13\right)$

当(x1，y1)位于(xk,yk)、(xk+1,yk+1)两点间，且(x2，y2)位于(xk+1,yk+1)上侧。此时(x2，y2)为预瞄点，即(x2，y2)为(G_x，G_y)，可根据如下公式进行判定:

$\left\{\begin{array}{c}ld1+ld2=ld5\\ ld3+ld4>ld5\\ \frac{ld3}{ld4}>1\end{array}\right.---\left(14\right)$

当(x1，y1)位于(xk+1,yk+1)上侧，且(x2，y2)位于(xk,yk)下侧。此时(x1，y1)为预瞄点，(x1，y1)为(G_x，G_y)，可根据如下公式进行判定:

$\left\{\begin{array}{c}ld1+ld2>ld5\\ ld3+ld4>ld5\\ \frac{ld1}{ld2}>1\\ \frac{ld3}{ld4}<1\end{array}\right.---\left(15\right)$

当(x1，y1)位于(xk+1,yk+1)上侧，且(x2，y2)位于(xk,yk)、(xk+1,yk+1)两点间。此时(x1，y1)为预瞄点，(x1，y1)为(G_x，G_y)，可根据如下公式进行判定:

$\left\{\begin{array}{c}ld1+ld2>ld5\\ ld3+ld4=\mathrm{ld}5\\ \frac{ld1}{ld2}>1\end{array}\right.---\left(16\right)$

当(x1，y1)、(x2，y2)在点(xk+1,yk+1)上侧，且点(x1，y1)位于(x2，y2)下侧。此时(x2，y2)为预瞄点，即(x2，y2)为(G_x，G_y)，可根据如下公式进行判定:

$\left\{\begin{array}{c}ld1+ld2>ld5\\ ld3+ld4>ld5\\ ld1<ld3\\ \frac{ld1}{ld2}>1\\ \frac{ld3}{ld4}>1\end{array}\right.---\left(17\right)$

当(x1，y1)、(x2，y2)在点(xk+1,yk+1)上侧，且点(x1，y1)位于(x2，y2)上侧。此时(x1，y1)为预瞄点，即(x1，y1)为(G_x，G_y)，可根据如下公式进行判定:

$\left\{\begin{array}{c}ld1+ld2>ld5\\ ld3+ld4>ld5\\ ld1>ld3\\ \frac{ld1}{ld2}>1\\ \frac{ld3}{ld4}>1\end{array}\right.---\left(18\right)$

a、b两种情况一般发生在车辆进行路径跟踪的初始时刻，由于车辆初始位置为随机的，有可能在规划路径后方。当发生d、h、i、j、k、l六种情况时，选取的预瞄点位于(xk+1,yk+1)右侧，即预瞄点位于点(xk+1,yk+1)、(xk+2,yk+2)间，车辆在执行完本次预瞄动作后，在下一次的预瞄点确定过程中将在(xk+1,yk+1)、(xk+2,yk+2)依据如上的方法确定新的预瞄点。

当期望路径与圆相离或相切时。此时可根据航向角与期望路径斜率角的差值α对转向角进行控制，当α<0时，车辆向期望路径方向行驶，无需对转角进行控制，可采用Pure Pursuit算法进行控制；当α≥0时，车辆背离期望路径，需对对前轮偏角进行修正，使车辆向期望路径方向行驶，采用Pure Pursuit算法进行控制，根据实验经验，当车辆背离期望路径行驶时，方向盘控制量设置为100°。

步骤5：道路预测

考虑到车辆在入弯时，相比于直线道路，以车体位置为圆心，预瞄距离为半径的圆与规划路径存在多个交点，因此需对车辆前方道路进行预测。根据比较规划路径上连续三个点(xk,yk)、(xk+1,yk+1)、(xk+2,yk+2)斜率的变化，确定车辆是否开始驶入弯道，其中k1为(xk,yk)、(xk+1,yk+1)所在直线的斜率，k2为(xk+1,yk+1)、(xk+2,yk+2)所在直线的斜率。当k1＝k2时，证明车辆行驶在直线道路上，此时应用步骤4的方法得到相应的预瞄点；当k1≠k2时，车辆将驶入弯道，此时存在两个预瞄点，分别为直线(xk,yk)、(xk+1,yk+1)上的预瞄点和直线(xk+1,yk+1)、(xk+2,yk+2)上的预瞄点，面对这种情况，正确预瞄点应选取直线(xk+1,yk+1)、(xk+2,yk+2)上的预瞄点为有效预瞄点。这种筛选预瞄点的方法可以使车辆提前对变化的车道做出反应，防止出现车辆转弯时转向角过小，而无法顺利驶入弯道的情况。

步骤6：算法的实现

确定预瞄点后，利用Pure Pursuit路径跟踪算法，计算智能车转向角控制量，使车辆可以对规划路径进行跟踪。Pure Pursuit路径跟踪算法的原理中，该算法是以车辆后轴为切点，即(C_x,C_y)，车辆纵轴为切线画出一条圆弧，该圆弧经过规划路径上的预瞄点(G_x,G_y)，预瞄点与车辆后轴间的连线l_d为预瞄距离，α为车辆航向角与预瞄点的夹角，Pure Pursuit路径跟踪算法就是根据几何特性，计算出一个可以使车辆行驶轨迹驶过预瞄点的前轮偏角控制量。

根据正弦定理可到如下公式：

$\frac{l_d}{sin\left(2\mathrm{\&alpha;}\right)}=\frac{R}{sin(\frac{\mathrm{\&pi;}}{2}-\mathrm{\&alpha;})}$

$\frac{l_d}{2sin\left(\mathrm{\&alpha;}\right)cos\left(\mathrm{\&alpha;}\right)}=\frac{R}{cos\left(\mathrm{\&alpha;}\right)}$

$\frac{l_d}{sin\left(\mathrm{\&alpha;}\right)}=2R---\left(19\right)$

公式(19)也可表示为：

$\mathrm{\&kappa;}=\frac{2sin\left(\mathrm{\&alpha;}\right)}{l_d}---\left(20\right)$

其中κ为圆弧的曲率，根据简化的阿克曼车辆模型，前轮偏角可用如下公式表示：

δ＝tan^-1(κL)  (21)

根据公式(20)、(21)，可以得到纯追踪算法的控制量表达式：

$\mathrm{\&delta;}\left(t\right)={\tan }^{-1}\left(\frac{2Lsin\left(\mathrm{\&alpha;}\right(t\left)\right)}{l_d}\right)---\left(22\right)$

根据步骤1、2、3、4、5可得到预瞄点、预瞄距离和误差角α，将这些变量代入公式(22)可得到前轮偏角控制量。最终的前轮偏角控制量采用比例控制，为了确保车辆在行驶过程中的稳定性，所以在直线道路行驶和弯道行驶时，采用了不同的比例增益。

$\mathrm{\&delta;}\left(t\right)=K_p{\tan }^{-1}\left(\frac{2Lsin\left(\mathrm{\&alpha;}\right(t\left)\right)}{l_d}\right)---\left(23\right)$

本发明定义k1为(xk,yk)、(xk+1,yk+1)所在直线的斜率，k2为(xk+1,yk+1)、(xk+2,yk+2)所在直线的斜率，当k1与k2夹角小于5°时为车辆行驶在直线道路中，当k1与k2夹角大于这一角度时，视为车辆驶入弯道。因此定义：

$\left\{\begin{array}{cc}\mathrm{\&delta;}\left(t\right)=0.5{\tan }^{-1}\left(\frac{2Lsin\left(\mathrm{\&alpha;}\right(t\left)\right)}{l_d}\right)& \left(\right|{\tan }^{-1}(k1-k2)|\&le;5)\\ \mathrm{\&delta;}\left(t\right)=\frac{\left|{\tan }^{-1}(k1-k2)\right|+37.5}{85}{\tan }^{-1}\left(\frac{2Lsin\left(\mathrm{\&alpha;}\right(t\left)\right)}{l_d}\right)& \left(\right|{\tan }^{-1}(k1-k2)|>5)\end{array}\right.$

附图说明

图1为本发明所涉及的方法流程图

图2GPS坐标系换算直角标系示意图；

图3预瞄距离与速度关系

图4位置关系

图5预瞄点确定方法

图6Pure Pursuit算法几何图

图7实验结果

具体实施方式

下面结合附图和实施步骤对本发明做进一步说明。

本发明所述方法的流程图如图1所示，包括以下步骤：

步骤1：GPS位置信息坐标系换算

GPS接收机接收到的GPS信号为美国国家海洋电子协会(National MarineElectronics Association)制定的NMEA0183版本3.00的ASCII码语句,包括GPGGA、GPGSA、GPGSV、GPRMC(NMEA标准语句)；PGRME、PGRMM、PGRMT(GARMIN定义的语句)。因此得到的位置信息属于空间大地坐标系，需要转换为UTM直角坐标系。

UTM坐标系是将地球划分成60个投影带，以180°经线为起始线，向东每个6°划分为一个纵带，每个纵带中部的经线被命名为中央经线，并定义中央经线坐标值为500000，在南纬80°至北纬84°范围内，分别从赤道开始，每隔8°划分一个横带。如图2所示

空间大地坐标系转换为UTM坐标系，其中r＝6378.137km，k₀＝0.9996，ec＝0.0818192，为经纬度坐标，(e，n)为UTM坐标，可得到如下公式：

$e=k_{0}rv\left(\mathrm{\&Phi;}\right)\&lsqb;A+(1-\mathrm{\&Psi;}+C)\frac{A^3}{6}+(5-18\mathrm{\&Psi;}+{\mathrm{\&Psi;}}^2)\frac{A^5}{120}\&rsqb;---\left(1\right)$

$n=k_{0}r\left(s\right(\mathrm{\&Phi;})+v(\mathrm{\&Phi;})tan\mathrm{\&Phi;}\&lsqb;\frac{A^2}{2}+(5-\mathrm{\&Psi;}+9C+4C^2)\frac{A^4}{24}+(61-58\mathrm{\&Psi;}+{\mathrm{\&Psi;}}^2)\frac{A^6}{720}\&rsqb;)---\left(2\right)$

式中：

$v\left(\mathrm{\&Phi;}\right)=\frac{1}{\sqrt{1-{\mathrm{ec}}^2{\sin }^2\mathrm{\&Phi;}}}$

$\begin{array}{c}s\left(\mathrm{\&Phi;}\right)=(1-\frac{{\mathrm{ec}}^2}{4}-\frac{3{\mathrm{ec}}^4}{64}-\frac{5{\mathrm{ec}}^6}{256})\mathrm{\&Phi;}-(\frac{3{\mathrm{ec}}^2}{8}+\frac{3{\mathrm{ec}}^4}{32}+\frac{45{\mathrm{ec}}^6}{1024})\sin 2\mathrm{\&Phi;}\\ +(\frac{15{\mathrm{ec}}^4}{256}+\frac{45{\mathrm{ec}}^6}{1024})\sin 4\mathrm{\&Phi;}-\frac{35{\mathrm{ec}}^6}{3072}\sin 4\mathrm{\&Phi;}\end{array}$

A＝(λ-λ_ref)cosФ

$C=\frac{{\mathrm{ec}}^2}{1-{\mathrm{ec}}^2}{\cos }^2\mathrm{\&Phi;}$

Ψ＝tan²Ф

步骤2：根据车辆参数和当前车辆行驶速度确定预瞄距离

预瞄距离是无人驾驶智能车实现路径跟踪的一个重要参数，预瞄距离与车速存在一定的数学关系，即通过车辆行驶速度确定预瞄距离的公式为：

l_d＝AV²+BV+C  (3)

其中，V为车辆行驶速度，A、B、C为常数项。

AV²表示车辆制动距离。其中，A可表示为：

A＝1/2a_max

a_max为车辆最大制动加速度，经试验测量，车辆最大制动加速度为3m/s²。

BV表示车辆遇到异常情况进行反应的车辆行驶距离，令B为0.2。

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

所以预瞄距离公式可表示为：

$l_d=\frac{1}{6}{V}^2+\frac{1}{5}V+5---\left(4\right)$

根据公式(4)可得到预瞄距离与车辆速度之间函数关系图，如图3。

步骤3：判断车辆与规划路径位置

基于Pure Pursuit算法的智能车路径跟踪实际上是以预瞄距离为半径，以当前车体位置为圆心做圆，确定预瞄点，进而通过Pure Pursuit算法得出转向角控制量，实现路径跟踪。因此，以车体位置为圆心，预瞄距离为半径画出的圆与期望路径存在如图4所示的三种位置关系：相交、相切和相离。

如图4(a)所示，l_d为预瞄距离，α为跟踪航向角度偏差，(C_x，C_y)为车辆当前位置，(G_x，G_y)为期望路径上的预瞄点。

期望路径为一条已知的路径，根据期望路径上相邻两点坐标可得到直线方程的斜率K和常数项B，因此可得到预瞄点(G_x，G_y)之间的关系为：

G_y＝KG_x+B  (5)

根据两点间距离公式可得到：

l_d ²＝(C_x-G_x)²+(C_y-G_y)²  (6)

将公式(5)代入公式(6)并整理：

(1+K²)G_x ²+2(K·B-C_x-K·C_y)G_x+C_x ²+C_y ²+B²-2C_y·B-l_d ²＝0

令：

a＝1+K²

b＝2(K·B-C_x-K·C_y)

c＝C_x ²+C_y ²+B²-2C_y·B-l_d ²

则：

aG_x ²+bG_x+c＝0  (7)

根据一元二次方程求根公式可对公式进行求解，并判断期望路径与预瞄范围之间的关系：

当b²-4ac>0时，规划路径与圆相交，如图4(a)所示。

当b²-4ac＝0时，规划路径与圆相切，如图4(b)所示。

当b²-4ac<0时，规划路径与圆相离，如图4(c)所示。

步骤4：确定预瞄点

当规划路径与圆相交时，一元二次方程存在两个解集(x1，y1)、(x2，y2)，其中一个解为路径跟踪算法的预瞄点，另一个解为车辆已行驶过的区域，为无用点，因此要需根据方程解与规划路径上点的关系，选择可用的预瞄点，删除无用点。当规划路径与圆相交时，方程两个解与期望路径上连续的两个点(xk,yk)、(xk+1,yk+1)存在12种位置关系，如图5。其中，ld1、ld2分别为点(x1，y1)到(xk,yk)，(xk+1,yk+1)的距离，ld3、ld4分别为点(x2，y2)到(xk,yk)，(xk+1,yk+1)的距离，ld5为(xk,yk)，(xk+1,yk+1)两点间的距离。

如图5(a)所示，当(x1，y1)、(x2，y2)在点(xk,yk)x下侧，且点(x1，y1)位于(x2，y2)下侧。此时(x2，y2)为预瞄点，(x2，y2)为(G_x，G_y)，可根据如下公式进行判定:

$\left\{\begin{array}{c}ld1+dl2>ld5\\ ld3+dl4>ld5\\ ld1>ld3\\ \frac{ld1}{ld2}<1\\ \frac{ld3}{ld4}<1\end{array}\right.---\left(8\right)$

如图5(b)所示，当(x1，y1)、(x2，y2)在点(xk,yk)下侧，且点(x1，y1)位于(x2，y2)上侧。此时(x1，y1)为预瞄点，(x1，y1)为(G_x，G_y)，可根据如下公式进行判定:

$\left\{\begin{array}{c}ld1+ld2>ld5\\ ld3+ld4>ld5\\ ld1<ld3\\ \frac{ld1}{ld2}<1\\ \frac{ld3}{ld4}<1\end{array}\right.---\left(9\right)$

当(x1，y1)在点(xk,yk)下侧，(x2，y2)位于(xk,yk)，(xk+1,yk+1)两点间，如图5(c)所示。此时(x2，y2)为预瞄点，即(x2，y2)为(G_x，G_y)，可根据如下公式进行判定:

$\left\{\begin{array}{c}ld1+ld2>ld5\\ ld3+ld4=ld5\\ \frac{ld1}{ld2}<1\end{array}\right.---\left(10\right)$

如图5(d)所示，当(x1，y1)在点(xk,yk)下侧，(x2，y2)位于(xk+1,yk+1)上侧。此时(x2，y2)为预瞄点，即(x2，y2)为(G_x，G_y)，可根据，如下公式进行判定:

$\left\{\begin{array}{c}ld1+ld2>ld5\\ ld3+ld4>ld5\\ \frac{ld1}{ld2}<1\\ \frac{ld3}{dl4}>1\end{array}\right.---\left(11\right)$

如图5(e)所示，当(x1，y1)位于(xk,yk)、(xk+1,yk+1)两点间，且(x2，y2)位于(xk,yk)下侧。此时(x1，y1)为预瞄点，即(x2，y2)为(G_x，G_y)，可根据如下公式进行判定:

$\left\{\begin{array}{c}ld1+ld2=ld5\\ ld3+ld4>ld5\\ \frac{ld3}{ld4}<1\end{array}\right.---\left(12\right)$

如图5(f)所示，当(x1，y1)、(x2，y2)位于(xk,yk)、(xk+1,yk+1)两点间，且(x1，y1)位于(x2，y2)下侧。此时(x2，y2)为预瞄点，即(x2，y2)为(G_x，G_y)，可根据如下公式进行判定:

$\left\{\begin{array}{c}ld1+ld2=ld5\\ ld3+ld4=ld5\\ ld1<ld3\end{array}\right.---\left(13\right)$

如图5(g)所示，当(x1，y1)、(x2，y2)位于(xk,yk)、(xk+1,yk+1)两点间，且(x1，y1)位于(x2，y2)上侧。此时(x1，y1)为预瞄点，即(x1，y1)为(G_x，G_y)，可根据如下公式进行判定:

$\left\{\begin{array}{c}ld1+ld2=ld5\\ ld3+ld4=ld5\\ ld1>ld3\end{array}\right.---\left(14\right)$

如图5(h)所示，当(x1，y1)位于(xk,yk)、(xk+1,yk+1)两点间，且(x2，y2)位于(xk+1,yk+1)上侧。此时(x2，y2)为预瞄点，即(x2，y2)为(G_x，G_y)，可根据如下公式进行判定:

$\left\{\begin{array}{c}ld1+ld2=ld5\\ ld3+ld4>ld5\\ \frac{ld3}{ld4}>1\end{array}\right.---\left(15\right)$

如图5(i)所示，当(x1，y1)位于(xk+1,yk+1)上侧，且(x2，y2)位于(xk,yk)下侧。此时(x1，y1)为预瞄点，(x1，y1)为(G_x，G_y)，可根据如下公式进行判定:

$\left\{\begin{array}{c}ld1+ld2>ld5\\ ld3+ld4>ld5\\ \frac{ld1}{ld2}>1\\ \frac{ld3}{ld4}<1\end{array}\right.---\left(16\right)$

如图5(j)所示，当(x1，y1)位于(xk+1,yk+1)上侧，且(x2，y2)位于(xk,yk)、(xk+1,yk+1)两点间。此时(x1，y1)为预瞄点，(x1，y1)为(G_x，G_y)，可根据如下公式进行判定:

$\left\{\begin{array}{c}ld1+ld2>ld5\\ ld3+ld4=ld5\\ ld1>1\end{array}\right.---\left(17\right)$

如图5(k)所示，当(x1，y1)、(x2，y2)在点(xk+1,yk+1)上侧，且点(x1，y1)位于(x2，y2)下侧。此时(x2，y2)为预瞄点，即(x2，y2)为(G_x，G_y)，可根据如下公式进行判定:

$\left\{\begin{array}{c}ld1+ld2>ld5\\ ld3+ld4>ld5\\ ld1<ld3\\ \frac{ld1}{ld2}>1\\ \frac{ld3}{ld4}>1\end{array}\right.---\left(18\right)$

如图5(l)所示，当(x1，y1)、(x2，y2)在点(xk+1,yk+1)上侧，且点(x1，y1)位于(x2，y2)上侧。此时(x1，y1)为预瞄点，即(x1，y1)为(G_x，G_y)，可根据如下公式进行判定:

$\left\{\begin{array}{c}ld1+ld2>ld5\\ ld3+ld4>ld5\\ ld1>ld3\\ \frac{ld1}{ld2}>1\\ \frac{ld3}{ld4}>1\end{array}\right.---\left(19\right)$

图5中a、b两种情况一般发生在车辆进行路径跟踪的初始时刻，由于车辆初始位置为随机的，有可能在规划路径后方。当发生图5中d、h、i、j、k、l六种情况时，选取的预瞄点位于(xk+1,yk+1)右侧，即预瞄点位于点(xk+1,yk+1)、(xk+2,yk+2)间，车辆在执行完本次预瞄动作后，在下一次的预瞄点确定过程中将在(xk+1,yk+1)、(xk+2,yk+2)依据如上的方法确定新的预瞄点。

当期望路径与圆相离或相切时，如图4(b)、图4(c)所示。此时可根据航向角与期望路径斜率角的差值α对转向角进行控制，当α<0时，车辆向期望路径方向行驶，无需对转角进行控制，当行驶至图4(a)状态时，可采用Pure Pursuit算法进行控制；当α≥0时，车辆背离期望路径，需对对前轮偏角进行修正，使车辆向期望路径方向行驶，当行驶至图4(a)状态时，采用Pure Pursuit算法进行控制，根据实验经验，当车辆背离期望路径行驶时，方向盘控制量设置为100°。

步骤5：道路预测

考虑到车辆在入弯时可能存在如图4(a)所示情况，相比于直线道路，以车体位置为圆心，预瞄距离为半径的圆与规划路径存在多个交点，因此需对车辆前方道路进行预测。根据比较规划路径上连续三个点(xk,yk)、(xk+1,yk+1)、(xk+2,yk+2)斜率的变化，确定车辆是否开始驶入弯道，其中k1为(xk,yk)、(xk+1,yk+1)所在直线的斜率，k2为(xk+1,yk+1)、(xk+2,yk+2)所在直线的斜率。当k1＝k2时，证明车辆行驶在直线道路上，此时应用步骤4的方法得到相应的预瞄点；当k1≠k2时，车辆将驶入弯道，此时存在两个预瞄点，分别为直线(xk,yk)、(xk+1,yk+1)上的预瞄点和直线(xk+1,yk+1)、(xk+2,yk+2)上的预瞄点，面对这种情况，正确预瞄点应选取直线(xk+1,yk+1)、(xk+2,yk+2)上的预瞄点为有效预瞄点。这种筛选预瞄点的方法可以使车辆提前对变化的车道做出反应，防止出现车辆转弯时转向角过小，而无法顺利驶入弯道的情况。。

步骤6：算法的实现

确定预瞄点后，利用Pure Pursuit路径跟踪算法，计算智能车转向角控制量，使车辆可以对规划路径进行跟踪。Pure Pursuit路径跟踪算法的原理如图6所示，该算法是以车辆后轴为切点，即(C_x,C_y)，车辆纵轴为切线画出一条圆弧，该圆弧经过规划路径上的预瞄点(G_x,G_y)，预瞄点与车辆后轴间的连线l_d为预瞄距离，α为车辆航向角与预瞄点的夹角，Pure Pursuit路径跟踪算法就是根据几何特性，计算出一个可以使车辆行驶轨迹驶过预瞄点的前轮偏角控制量。

根据正弦定理可到如下公式：

$\frac{l_d}{sin\left(2\mathrm{\&alpha;}\right)}=\frac{R}{\sin (\frac{\mathrm{\&pi;}}{2}-\mathrm{\&alpha;})}$

$\frac{l_d}{2sin\left(\mathrm{\&alpha;}\right)cos\left(\mathrm{\&alpha;}\right)}=\frac{R}{cos\left(\mathrm{\&alpha;}\right)}$

$\frac{l_d}{sin\left(\mathrm{\&alpha;}\right)}=2R---\left(20\right)$

公式(20)也可表示为：

$\mathrm{\&kappa;}\frac{2sin\left(\mathrm{\&alpha;}\right)}{l_d}---\left(21\right)$

其中κ为圆弧的曲率，根据简化的阿克曼车辆模型，前轮偏角可用如下公式表示：

δ＝tan^-1(κL)  (22)

根据公式(21)、(22)，可以得到纯追踪算法的控制量表达式：

$\mathrm{\&delta;}\left(t\right)={\tan }^{-1}\left(\frac{2Lsin\left(\mathrm{\&alpha;}\right(t\left)\right)}{l_d}\right)---\left(23\right)$

根据步骤1、2、3、4、5可得到预瞄点、预瞄距离和误差角α，将这些变量代入公式(23)可得到前轮偏角控制量。最终的前轮偏角控制量采用比例控制，为了确保车辆在行驶过程中的稳定性，所以在直线道路行驶和弯道行驶时，采用了不同的比例增益。

$\mathrm{\&delta;}\left(t\right)=K_p{\tan }^{-1}\left(\frac{2Lsin\left(\mathrm{\&alpha;}\right(t\left)\right)}{l_d}\right)---\left(24\right)$

本发明定义k1为(xk,yk)、(xk+1,yk+1)所在直线的斜率，k2为(xk+1,yk+1)、(xk+2,yk+2)所在直线的斜率，当k1与k2夹角小于5°时为车辆行驶在直线道路中，当k1与k2夹角大于这一角度时，视为车辆驶入弯道。因此定义：

$\left\{\begin{array}{cc}\mathrm{\&delta;}\left(t\right)=0.5{\tan }^{-1}\left(\frac{2L\sin \left(\mathrm{\&alpha;}\right(t\left)\right)}{l_d}\right)& \left(\right|{\tan }^{-1}(k1-k2)|\&le;5)\\ \mathrm{\&delta;}\left(t\right)=\frac{\left|{\tan }^{-1}(k1-k2)\right|+37.5}{85}{\tan }^{-1}\left(\frac{2L\sin \left(\mathrm{\&alpha;}\right(t\left)\right)}{l_d}\right)& \left(\right|{\tan }^{-1}(k1-k2)|>5)\end{array}\right.$

Claims (1)

1.基于Pure Pursuit算法的预瞄点确定方法，其特征在于：

包括以下步骤：

步骤1：GPS位置信息坐标系转换；

GPS输出数据经度、纬度、高度坐标值，并不适合于平面运动的车辆导航；采用通用横轴墨卡托坐标系即UTM，将经纬度转化到平面坐标系；

步骤2：根据车辆参数和当前车辆行驶速度确定预瞄距离

根据人类驾驶车辆的经验，车辆以不同车速行驶在道路上时，驾驶员的预瞄距离也随之改变，在低速行驶时，驾驶员预瞄前方道路的距离较近，车速增大，驾驶员的预瞄距离也随之增大；

步骤3：判断车辆与规划路径位置

步骤4：确定预瞄点

步骤5：道路判断

道路判断主要用于判断前方道路的状况，即车辆行驶在直线道路上还是行驶在弯道上；对于不同道路状况，预瞄点的筛选与略有不同；

步骤6：算法的实现

得到预瞄点和预瞄距离后，根据Pure Pursuit算法可计算出转向角控制量，为了提高车辆在直线或弯道环境中车辆行驶的平稳性，制定了更改比例增益的控制策略；

具体实施过程如下，

步骤1：GPS位置信息坐标系换算

GPS接收机接收到的GPS信号ASCII码语句,包括GPGGA、GPGSA、GPGSV、GPRMC；PGRME、PGRMM、PGRMT(GARMIN定义的语句)；因此得到的位置信息属于空间大地坐标系，需要转换为UTM直角坐标系；

UTM坐标系是将地球划分成60个投影带，以180°经线为起始线，向东每个6°划分为一个纵带，每个纵带中部的经线被命名为中央经线，并定义中央经线坐标值为500000，在南纬80°至北纬84°范围内，分别从赤道开始，每隔8°划分一个横带；

空间大地坐标系转换为UTM坐标系，其中r＝6378.137km，r为地球半径；k₀＝0.9996，k₀为坐标系转化系数；ec＝0.0818192，ec为偏心率；为经纬度坐标，λ为经度，为纬度；(e，n)为UTM坐标，e为方向东，n为方向北；Ф表示的弧度值；因此可得到如下公式：

$e=k_{0}rv\left(\mathrm{\&Phi;}\right)\&lsqb;A+(1-\mathrm{\&Psi;}+C)\frac{A^3}{6}+(5-18\mathrm{\&Psi;}+{\mathrm{\&Psi;}}^2)\frac{A^5}{120}\&rsqb;---\left(1\right)$

$n=k_{0}r(s\left(\mathrm{\&Phi;}\right)+v\left(\mathrm{\&Phi;}\right)tan\mathrm{\&Phi;}\&lsqb;\frac{A^2}{2}+(5-\mathrm{\&Psi;}+9C+4C^2)\frac{A^4}{24}+(61-58\mathrm{\&Psi;}+{\mathrm{\&Psi;}}^2)\frac{A^6}{720}\&rsqb;)---\left(2\right)$ 式中：

$v\left(\mathrm{\&Phi;}\right)=\frac{1}{\sqrt{1-{\mathrm{ec}}^2{\sin }^2\mathrm{\&Phi;}}}$

$\begin{array}{c}s\left(\mathrm{\&Phi;}\right)=\left(1-\frac{{\mathrm{ec}}^2}{4}-\frac{3{\mathrm{ec}}^4}{64}-\frac{5{\mathrm{ec}}^6}{256}\right)\mathrm{\&Phi;}-\left(\frac{3{\mathrm{ec}}^2}{8}+\frac{3{\mathrm{ec}}^4}{32}+\frac{45{\mathrm{ec}}^6}{1024}\right)\sin 2\mathrm{\&Phi;}\\ +\left(\frac{15{\mathrm{ec}}^4}{256}+\frac{45{\mathrm{ec}}^6}{1024}\right)\sin 4\mathrm{\&Phi;}-\frac{35{\mathrm{ec}}^6}{3072}\sin 4\mathrm{\&Phi;}\end{array}$

A＝(λ-λ_ref)cosФ

$C=\frac{{\mathrm{ec}}^2}{1-{\mathrm{ec}}^2}{\cos }^2\mathrm{\&Phi;}$

Ψ＝tan²Ф

步骤2：根据车辆参数和当前车辆行驶速度确定预瞄距离

预瞄距离是无人驾驶智能车实现路径跟踪的一个重要参数，预瞄距离与车速存在一定的数学关系，即通过车辆行驶速度确定预瞄距离的公式为：

l_d＝AV²+BV+C   (3)

其中，V为车辆行驶速度，A、B、C为常数项；

AV²表示车辆制动距离；其中，A可表示为：

A＝1/2a_max

a_max为车辆最大制动加速度；

BV表示车辆遇到异常情况进行反应的车辆行驶距离；

C表示车辆最小转弯半径；

步骤3：判断车辆与规划路径位置

基于Pure Pursuit算法的智能车路径跟踪实际上是以预瞄距离为半径，以当前车体位置为圆心做圆，确定预瞄点，进而通过Pure Pursuit算法得出转向角控制量，实现路径跟踪；因此，以车体位置为圆心，预瞄距离为半径画出的圆与期望路径存在三种位置关系：相交、相切和相离；

l_d为预瞄距离，α为跟踪航向角度偏差，(C_x，C_y)为车辆当前位置，(G_x，G_y)为期望路径上的预瞄点；

期望路径为一条已知的路径，根据期望路径上相邻两点坐标可得到直线方程的斜率K和常数项B，因此可得到预瞄点(G_x，G_y)之间的关系为：

G_y＝KG_x+B   (4)

根据两点间距离公式可得到：

l_d ²＝(C_x-G_x)²+(C_y-G_y)²   (5)

将公式(4)代入公式(5)并整理：

(1+K²)G_x ²+2(K·B-C_x-K·C_y)G_x+C_x ²+C_y ²+B²-2C_y·B-l_d ²＝0

令：

a＝1+K²

b＝2(K·B-C_x-K·C_y)

c＝C_x ²+C_y ²+B²-2C_y·B-l_d ²

则：

aG_x ²+bG_x+c＝0   (6)

根据一元二次方程求根公式可对公式进行求解，并判断期望路径与预瞄范围之间的关系：

当b²-4ac>0时，规划路径与圆相交；

当b²-4ac＝0时，规划路径与圆相切；

当b²-4ac<0时，规划路径与圆相离；

步骤4：确定预瞄点

当规划路径与圆相交时，一元二次方程存在两个解集(x1，y1)、(x2，y2)，其中一个解为路径跟踪算法的预瞄点，另一个解为车辆已行驶过的区域，为无用点，因此要需根据方程解与规划路径上点的关系，选择可用的预瞄点，删除无用点；当规划路径与圆相交时，方程两个解与期望路径上连续的两个点(xk,yk)、(xk+1,yk+1)存在12种位置关系；其中，ld1、ld2分别为点(x1，y1)到(xk,yk)，(xk+1,yk+1)的距离，ld3、ld4分别为点(x2，y2)到(xk,yk)，(xk+1,yk+1)的距离，ld5为(xk,yk)，(xk+1,yk+1)两点间的距离；

当(x1，y1)、(x2，y2)在点(xk,yk)x下侧，且点(x1，y1)位于(x2，y2)下侧；此时(x2，y2)为预瞄点，(x2，y2)为(G_x，G_y)，可根据如下公式进行判定:

$\left\{\begin{array}{c}ld1+ld2>ld5\\ ld3+ld4>ld5\\ ld1>ld3\\ \frac{ld1}{ld2}<1\\ \frac{ld3}{ld4}<1\end{array}\right.---\left(7\right)$

当(x1，y1)、(x2，y2)在点(xk,yk)下侧，且点(x1，y1)位于(x2，y2)上侧；此时(x1，y1)为预瞄点，(x1，y1)为(G_x，G_y)，可根据如下公式进行判定：

$\left\{\begin{array}{c}ld1+ld2>ld5\\ ld3+ld4>ld5\\ ld1<ld3\\ \frac{ld1}{ld2}<1\\ \frac{ld3}{ld4}<1\end{array}\right.---\left(8\right)$

当(x1，y1)在点(xk,yk)下侧，(x2，y2)位于(xk,yk)，(xk+1,yk+1)两点间；此时(x2，y2)为预瞄点，即(x2，y2)为(G_x，G_y)，可根据如下公式进行判定:

$\left\{\begin{array}{c}ld1+ld2>ld5\\ ld3+ld4=ld5\\ \frac{ld1}{ld2}<1\end{array}\right.---\left(9\right)$

当(x1，y1)在点(xk,yk)下侧，(x2，y2)位于(xk+1,yk+1)上侧；此时(x2，y2)为预瞄点，即(x2，y2)为(G_x，G_y)，可根据，如下公式进行判定:

$\left\{\begin{array}{c}ld1+ld2>ld5\\ ld3+ld4>ld5\\ \frac{ld1}{ld2}<1\\ \frac{ld3}{ld4}>1\end{array}\right.---\left(10\right)$

当(x1，y1)位于(xk,yk)、(xk+1,yk+1)两点间，且(x2，y2)位于(xk,yk)下侧；此时(x1，y1)为预瞄点，即(x2，y2)为(G_x，G_y)，可根据如下公式进行判定：

$\left\{\begin{array}{c}ld1+ld2=ld5\\ ld3+ld4>ld5\\ \frac{ld3}{ld4}<1\end{array}\right.---\left(11\right)$

当(x1，y1)、(x2，y2)位于(xk,yk)、(xk+1,yk+1)两点间，且(x1，y1)位于(x2，y2)下侧；此时(x2，y2)为预瞄点，即(x2，y2)为(G_x，G_y)，可根据如下公式进行判定:

$\left\{\begin{array}{c}ld1+ld2=ld5\\ ld3+ld4=ld5\\ ld1<ld3\end{array}\right.---\left(12\right)$

当(x1，y1)、(x2，y2)位于(xk,yk)、(xk+1,yk+1)两点间，且(x1，y1)位于(x2，y2)上侧；此时(x1，y1)为预瞄点，即(x1，y1)为(G_x，G_y)，可根据如下公式进行判定:

$\left\{\begin{array}{c}ld1+ld2=ld5\\ ld3+ld4=ld5\\ ld1>ld3\end{array}\right.---\left(13\right)$

当(x1，y1)位于(xk,yk)、(xk+1,yk+1)两点间，且(x2，y2)位于(xk+1,yk+1)上侧；此时(x2，y2)为预瞄点，即(x2，y2)为(G_x，G_y)，可根据如下公式进行判定:

$\left\{\begin{array}{c}ld1+ld2=ld5\\ ld3+ld4>ld5\\ \frac{ld3}{ld4}>1\end{array}\right.---\left(14\right)$

当(x1，y1)位于(xk+1,yk+1)上侧，且(x2，y2)位于(xk,yk)下侧；此时(x1，y1)为预瞄点，(x1，y1)为(G_x，G_y)，可根据如下公式进行判定:

$\left\{\begin{array}{c}ld1+ld2>ld5\\ ld3+ld4>ld5\\ \frac{ld1}{ld2}>1\\ \frac{ld3}{ld4}<1\end{array}\right.---\left(15\right)$

当(x1，y1)位于(xk+1,yk+1)上侧，且(x2，y2)位于(xk,yk)、(xk+1,yk+1)两点间；此时(x1，y1)为预瞄点，(x1，y1)为(G_x，G_y)，可根据如下公式进行判定:

$\left\{\begin{array}{c}ld1+ld2>ld5\\ ld3+ld4=ld5\\ \frac{ld1}{ld2}>1\end{array}\right.---\left(16\right)$

当(x1，y1)、(x2，y2)在点(xk+1,yk+1)上侧，且点(x1，y1)位于(x2，y2)下侧；此时(x2，y2)为预瞄点，即(x2，y2)为(G_x，G_y)，可根据如下公式进行判定:

$\left\{\begin{array}{c}ld1+ld2>ld5\\ ld3+ld4>ld5\\ ld1<ld3\\ \frac{ld1}{ld2}>1\\ \frac{ld3}{ld4}>1\end{array}\right.---\left(17\right)$

当(x1，y1)、(x2，y2)在点(xk+1,yk+1)上侧，且点(x1，y1)位于(x2，y2)上侧；此时(x1，y1)为预瞄点，即(x1，y1)为(G_x，G_y)，可根据如下公式进行判定:

$\left\{\begin{array}{c}ld1+ld2>ld5\\ ld3+ld4>ld5\\ ld1>ld3\\ \frac{ld1}{ld2}>1\\ \frac{ld3}{ld4}>1\end{array}\right.---\left(18\right)$

a、b两种情况一般发生在车辆进行路径跟踪的初始时刻，由于车辆初始位置为随机的，有可能在规划路径后方；当发生d、h、i、j、k、l六种情况时，选取的预瞄点位于(xk+1,yk+1)右侧，即预瞄点位于点(xk+1,yk+1)、(xk+2,yk+2)间，车辆在执行完本次预瞄动作后，在下一次的预瞄点确定过程中将在(xk+1,yk+1)、(xk+2,yk+2)依据如上的方法确定新的预瞄点；

当期望路径与圆相离或相切时；此时可根据航向角与期望路径斜率角的差值α对转向角进行控制，当α<0时，车辆向期望路径方向行驶，无需对转角进行控制，可采用Pure Pursuit算法进行控制；当α≥0时，车辆背离期望路径，需对对前轮偏角进行修正，使车辆向期望路径方向行驶，采用Pure Pursuit算法进行控制，根据实验经验，当车辆背离期望路径行驶时，方向盘控制量设置为100°；

步骤5：道路预测

考虑到车辆在入弯时，相比于直线道路，以车体位置为圆心，预瞄距离为半径的圆与规划路径存在多个交点，因此需对车辆前方道路进行预测；根据比较规划路径上连续三个点(xk,yk)、(xk+1,yk+1)、(xk+2,yk+2)斜率的变化，确定车辆是否开始驶入弯道，其中k1为(xk,yk)、(xk+1,yk+1)所在直线的斜率，k2为(xk+1,yk+1)、(xk+2,yk+2)所在直线的斜率；当k1＝k2时，证明车辆行驶在直线道路上，此时应用步骤4的方法得到相应的预瞄点；当k1≠k2时，车辆将驶入弯道，此时存在两个预瞄点，分别为直线(xk,yk)、(xk+1,yk+1)上的预瞄点和直线(xk+1,yk+1)、(xk+2,yk+2)上的预瞄点，面对这种情况，正确预瞄点应选取直线(xk+1,yk+1)、(xk+2,yk+2)上的预瞄点为有效预瞄点；这种筛选预瞄点的方法可以使车辆提前对变化的车道做出反应，防止出现车辆转弯时转向角过小，而无法顺利驶入弯道的情况；

步骤6：算法的实现

确定预瞄点后，利用Pure Pursuit路径跟踪算法，计算智能车转向角控制量，使车辆可以对规划路径进行跟踪；Pure Pursuit路径跟踪算法的原理中，该算法是以车辆后轴为切点，即(C_x,C_y)，车辆纵轴为切线画出一条圆弧，该圆弧经过规划路径上的预瞄点(G_x,G_y)，预瞄点与车辆后轴间的连线l_d为预瞄距离，α为车辆航向角与预瞄点的夹角，Pure Pursuit路径跟踪算法就是根据几何特性，计算出一个可以使车辆行驶轨迹驶过预瞄点的前轮偏角控制量；

根据正弦定理可到如下公式：

$\frac{l_d}{sin\left(2\mathrm{\&alpha;}\right)}=\frac{R}{sin(\frac{\mathrm{\&pi;}}{2}-\mathrm{\&alpha;})}$

$\frac{l_d}{2sin\left(\mathrm{\&alpha;}\right)cos\left(\mathrm{\&alpha;}\right)}=\frac{R}{cos\left(\mathrm{\&alpha;}\right)}$

$\frac{l_d}{sin\left(\mathrm{\&alpha;}\right)}=2R---\left(19\right)$

公式(19)也可表示为：

$\mathrm{\&kappa;}=\frac{2sin\left(\mathrm{\&alpha;}\right)}{l_d}---\left(20\right)$

其中κ为圆弧的曲率，根据简化的阿克曼车辆模型，前轮偏角可用如下公式表示：

δ＝tan^-1(κL)   (21)

根据公式(20)、(21)，可以得到纯追踪算法的控制量表达式：

$\mathrm{\&delta;}\left(t\right)={\tan }^{-1}\left(\frac{2Lsin\left(\mathrm{\&alpha;}\right(t))}{l_d}\right)---\left(22\right)$

根据步骤1、2、3、4、5可得到预瞄点、预瞄距离和误差角α，将这些变量代入公式(22)可得到前轮偏角控制量；最终的前轮偏角控制量采用比例控制，为了确保车辆在行驶过程中的稳定性，所以在直线道路行驶和弯道行驶时，采用了不同的比例增益；

$\mathrm{\&delta;}\left(t\right)=K_p{\tan }^{-1}\left(\frac{2Lsin\left(\mathrm{\&alpha;}\right(t))}{l_d}\right)---\left(23\right)$

本方法定义k1为(xk,yk)、(xk+1,yk+1)所在直线的斜率，k2为(xk+1,yk+1)、(xk+2,yk+2)所在直线的斜率，当k1与k2夹角小于5°时为车辆行驶在直线道路中，当k1与k2夹角大于这一角度时，视为车辆驶入弯道；因此定义：

$\left\{\begin{array}{cc}\mathrm{\&delta;}\left(t\right)=0.5{\tan }^{-1}\left(\frac{2L\sin \left(\mathrm{\&alpha;}\left(t\right)\right)}{l_d}\right)& \left(\left|{\tan }^{-1}\left(k1-k2\right)\right|\&le;5\right)\\ \mathrm{\&delta;}\left(t\right)=\frac{\left|{\tan }^{-1}\left(k1-k2\right)\right|+37.5}{85}{\tan }^{-1}\left(\frac{2L\sin \left(\mathrm{\&alpha;}\left(t\right)\right)}{l_d}\right)& \left(\left|{\tan }^{-1}\left(k1-k2\right)\right|>5\right)\end{array}\right.$