[发明专利]基于PurePursuit算法的预瞄点确定方法

摘要

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

主权项

基于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；因此得到的位置信息属于空间大地坐标系，需要转换为UTM直角坐标系；UTM坐标系是将地球划分成60个投影带，以180°经线为起始线，向东每个6°划分为一个纵带，每个纵带中部的经线被命名为中央经线，并定义中央经线坐标值为500000，在南纬80°至北纬84°范围内，分别从赤道开始，每隔8°划分一个横带；空间大地坐标系转换为UTM坐标系，其中r＝6378.137km，r为地球半径；k0＝0.9996，k0为坐标系转化系数；ec＝0.0818192，ec为偏心率；为经纬度坐标，λ为经度，λref为参考经度，即所在区域中央子午线的经度，为纬度；(e，n)为UTM坐标，e为方向东，n为方向北；Ф表示的弧度值；因此可得到如下公式：e=k0rv(Φ)[A+(1-Ψ+C)A36+(5-18Ψ+Ψ2)A5120]---(1)]]>n=k0r(s(Φ)+v(Φ)tanΦ[A22+(5-Ψ+9C+4C2)A424+(61-58Ψ+Ψ2)A6720])---(2)]]>式中：v(Φ)=11-ec2sin2Φ]]>s(Φ)=(1-ec24-3ec464-5ec6256)Φ-(3ec28+3ec432+45ec61024)sin2Φ+(15ec4256+45ec61024)sin4Φ-35ec63072sin4Φ]]>A＝(λ‑λref)cosФC=ec21-ec2cos2Φ]]>Ψ＝tan2Ф步骤2：根据车辆参数和当前车辆行驶速度确定预瞄距离预瞄距离是无人驾驶智能车实现路径跟踪的一个重要参数，预瞄距离与车速存在一定的数学关系，即通过车辆行驶速度确定预瞄距离的公式为：ld＝AV2+BV+C                     (3)其中，V为车辆行驶速度，A、B、C为常数项；AV2表示车辆制动距离；其中，A可表示为：A＝1/2amaxamax为车辆最大制动加速度；BV表示车辆遇到异常情况进行反应的车辆行驶距离；C表示车辆最小转弯半径；步骤3：判断车辆与规划路径位置基于Pure Pursuit算法的智能车路径跟踪实际上是以预瞄距离为半径，以当前车体位置为圆心做圆，确定预瞄点，进而通过Pure Pursuit算法得出转向角控制量，实现路径跟踪；因此，以车体位置为圆心，预瞄距离为半径画出的圆与期望路径存在三种位置关系：相交、相切和相离；ld为预瞄距离，α为跟踪航向角度偏差，(Cx，Cy)为车辆当前位置，(Gx，Gy)为期望路径上的预瞄点；期望路径为一条已知的路径，根据期望路径上相邻两点坐标可得到直线方程的斜率K和常数项B，因此可得到预瞄点(Gx，Gy)之间的关系为：Gy＝KGx+B               (4)根据两点间距离公式可得到：ld2＝(Cx‑Gx)2+(Cy‑Gy)2        (5)将公式(4)代入公式(5)并整理：(1+K2)Gx2+2(K·B‑Cx‑K·Cy)Gx+Cx2+Cy2+B2‑2Cy·B‑ld2＝0令：a＝1+K2b＝2(K·B‑Cx‑K·Cy)c＝Cx2+Cy2+B2‑2Cy·B‑ld2则：aGx2+bGx+c＝0                       (6)根据一元二次方程求根公式可对公式进行求解，并判断期望路径与预瞄范围之间的关系：当b2‑4ac>0时，规划路径与圆相交；当b2‑4ac＝0时，规划路径与圆相切；当b2‑4ac<0时，规划路径与圆相离；步骤4：确定预瞄点当规划路径与圆相交时，一元二次方程存在两个解集(x1，y1)、(x2，y2)，其中一个解为路径跟踪算法的预瞄点，另一个解为车辆已行驶过的区域，为无用点，因此要需根据方程解与规划路径上点的关系，选择可用的预瞄点，删除无用点；当规划路径与圆相交时，方程两个解与期望路径上连续的两个点(xk,yk)、(xk+1,yk+1)存在12种位置关系，即公式7到公式18所描述的12种状态，为了便于表述，用a,b,c,d,e,f,g,h,i,j,k,l表示；其中，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)下侧，且点(x1，y1)位于(x2，y2)下侧；此时(x2，y2)为预瞄点，(x2，y2)为(Gx，Gy)，可根据如下公式进行判定:ld1+ld2>ld5ld3+ld4>ld5ld1>ld3ld1ld2<1ld3ld4<1---(7)]]>当(x1，y1)、(x2，y2)在点(xk,yk)下侧，且点(x1，y1)位于(x2，y2)上侧；此时(x1，y1)为预瞄点，(x1，y1)为(Gx，Gy)，可根据如下公式进行判定：ld1+ld2>ld5ld3+ld4>ld5ld1<ld3ld1ld2<1ld3ld4<1---(8)]]>当(x1，y1)在点(xk,yk)下侧，(x2，y2)位于(xk,yk)，(xk+1,yk+1)两点间；此时(x2，y2)为预瞄点，即(x2，y2)为(Gx，Gy)，可根据如下公式进行判定:ld1+ld2>ld5ld3+ld4=ld5ld1ld2<1---(9)]]>当(x1，y1)在点(xk,yk)下侧，(x2，y2)位于(xk+1,yk+1)上侧；此时(x2，y2)为预瞄点，即(x2，y2)为(Gx，Gy)，可根据，如下公式进行判定:ld1+ld2>ld5ld3+ld4>ld5ld1ld2<1ld3ld4>1---(10)]]>当(x1，y1)位于(xk,yk)、(xk+1,yk+1)两点间，且(x2，y2)位于(xk,yk)下侧；此时(x1，y1)为预瞄点，即(x2，y2)为(Gx，Gy)，可根据如下公式进行判定：ld1+ld2=ld5ld3+ld4>ld5ld3ld4<1---(11)]]>当(x1，y1)、(x2，y2)位于(xk,yk)、(xk+1,yk+1)两点间，且(x1，y1)位于(x2，y2)下侧；此时(x2，y2)为预瞄点，即(x2，y2)为(Gx，Gy)，可根据如下公式进行判定:ld1+ld2=ld5ld3+ld4=ld5ld1<ld3---(12)]]>当(x1，y1)、(x2，y2)位于(xk,yk)、(xk+1,yk+1)两点间，且(x1，y1)位于(x2，y2)上侧；此时(x1，y1)为预瞄点，即(x1，y1)为(Gx，Gy)，可根据如下公式进行判定:ld1+ld2=ld5ld3+ld4=ld5ld1>ld3---(13)]]>当(x1，y1)位于(xk,yk)、(xk+1,yk+1)两点间，且(x2，y2)位于(xk+1,yk+1)上侧；此时(x2，y2)为预瞄点，即(x2，y2)为(Gx，Gy)，可根据如下公式进行判定:ld1+ld2=ld5ld3+ld4>ld5ld3ld4>1---(14)]]>当(x1，y1)位于(xk+1,yk+1)上侧，且(x2，y2)位于(xk,yk)下侧；此时(x1，y1)为预瞄点，(x1，y1)为(Gx，Gy)，可根据如下公式进行判定:ld1+ld2>ld5ld3+ld4>ld5ld1ld2>1ld3ld4<1---(15)]]>当(x1，y1)位于(xk+1,yk+1)上侧，且(x2，y2)位于(xk,yk)、(xk+1,yk+1)两点间；此时(x1，y1)为预瞄点，(x1，y1)为(Gx，Gy)，可根据如下公式进行判定:ld1+ld2>ld5ld3+ld4=ld5ld1ld2>1---(16)]]>当(x1，y1)、(x2，y2)在点(xk+1,yk+1)上侧，且点(x1，y1)位于(x2，y2)下侧；此时(x2，y2)为预瞄点，即(x2，y2)为(Gx，Gy)，可根据如下公式进行判定:ld1+ld2>ld5ld3+ld4>ld5ld1<ld3ld1ld2>1ld3ld4>1---(17)]]>当(x1，y1)、(x2，y2)在点(xk+1,yk+1)上侧，且点(x1，y1)位于(x2，y2)上侧；此时(x1，y1)为预瞄点，即(x1，y1)为(Gx，Gy)，可根据如下公式进行判定:ld1+ld2>ld5ld3+ld4>ld5ld1>ld3ld1ld2>1ld3ld4>1---(18)]]>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路径跟踪算法的原理中，该算法是以车辆后轴为切点，即(Cx,Cy)，车辆纵轴为切线画出一条圆弧，该圆弧经过规划路径上的预瞄点(Gx,Gy)，预瞄点与车辆后轴间的连线ld为预瞄距离，α为车辆航向角与预瞄点的夹角，L为车辆轴距，Pure Pursuit路径跟踪算法就是根据几何特性，计算出一个可以使车辆行驶轨迹驶过预瞄点的前轮偏角控制量；根据正弦定理可到如下公式：ldsin(2α)=Rsin(π2-α)]]>ld2sin(α)cos(α)=Rcos(α)]]>ldsin(α)=2R---(19)]]>公式(19)也可表示为：κ=2sin(α)ld---(20)]]>其中κ为圆弧的曲率，根据简化的阿克曼车辆模型，前轮偏角可用如下公式表示：δ＝tan‑1(κL)                      (21)根据公式(20)、(21)，可以得到纯追踪算法的控制量表达式：δ(t)=tan-1(2Lsin(α(t))ld)---(22)]]>根据步骤1、2、3、4、5可得到预瞄点、预瞄距离和误差角α，将这些变量代入公式(22)可得到前轮偏角控制量；最终的前轮偏角控制量采用比例控制，为了确保车辆在行驶过程中的稳定性，所以在直线道路行驶和弯道行驶时，采用了不同的比例增益Kp对前轮偏角进行控制；δ(t)=Kptan-1(2Lsin(α(t))ld)---(23)]]>本方法定义k1为(xk,yk)、(xk+1,yk+1)所在直线的斜率，k2为(xk+1,yk+1)、(xk+2,yk+2)所在直线的斜率，当k1与k2夹角小于5°时为车辆行驶在直线道路中，当k1与k2夹角大于这一角度时，视为车辆驶入弯道；因此定义：δ(t)=0.5tan-1(2Lsin(α(t))ld)(|tan-1(k1-k2)|≤5)δ(t)=|tan-1(k1-k2)|+37.585tan-1(2Lsin(α(t))ld)(|tan-1(k1-k2)|>5).]]>