基于Pure Pursuit算法的預(yù)瞄點確定方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種基于Pure Pursuit算法的預(yù)目苗點確定方法，屬于智能車輛路徑跟 蹤相關(guān)領(lǐng)域。 技術(shù)背景
[0002] 為了提高交通運輸效率和交通安全，世界各國對智能交通系統(tǒng)和安全駕駛等問題 的研宄投入了大量的人力、物力和財力。美國DARPA自2004年以來，連續(xù)三年舉辦了三屆 智能車挑戰(zhàn)賽活動，進一步促進了智能車領(lǐng)域和智能車技術(shù)的發(fā)展，2012年谷歌公司發(fā)布 了一款無人駕駛智能車，并預(yù)計在2018年正式上市。除了研宄機構(gòu)、科技公司，許多汽車制 造商也都紛紛投入到智能車技術(shù)的研宄中，奧迪公司的Q7轎車上使用了 Hella公司的車道 變更告警系統(tǒng)、沃爾沃公司推出的激光防撞系統(tǒng)等。2015年4月，由汽車配件商德爾福研發(fā) 的一輛無人駕駛汽車完成了橫跨美國之旅，再次說明近年來無人駕駛智能車技術(shù)的飛速發(fā) 展。"中國智能車未來挑戰(zhàn)賽"是由我國國家自然科學(xué)基金委員會舉辦的無人駕駛智能車比 賽，自2009年開始，我國已成功舉辦了六屆"中國智能車未來挑戰(zhàn)賽"。
[0003] 無人駕駛智能車的控制主要包括橫向控制和縱向控制兩部分。其中橫向控制主要 指對車輛轉(zhuǎn)向系統(tǒng)的控制。路徑跟蹤是智能車橫向控制的一個主要應(yīng)用。路徑跟蹤是指在 已經(jīng)得到一條期望路徑的前提下，智能車根據(jù)大地坐標系中的位置信息，按照一定的控制 策略，使智能車的實際行駛路徑可以與規(guī)劃路徑達到一致。目前被廣泛應(yīng)用在無人駕駛智 能車路徑跟蹤的算法基本上都是通過調(diào)節(jié)實際路徑與規(guī)劃路徑之間存在的偏差以實現(xiàn)對 目標路徑進行跟蹤，如方位誤差、距離誤差等。其中Pure Pursuit算法、斯坦利算法、環(huán)形 預(yù)瞄算法是幾類常見的路徑跟蹤算法。

【發(fā)明內(nèi)容】

[0004] 本發(fā)明的目的在于提出一種確定規(guī)劃路徑上預(yù)瞄點的方法，根據(jù)車速和道路， 確定預(yù)目苗點，并利用Pure Pursuit算法實現(xiàn)車輛的路徑跟蹤，相較于傳統(tǒng)的基于Pure Pursuit算法確定預(yù)目苗點，該算法可準確、快速地確定規(guī)劃路徑上的預(yù)目苗點，并對前方道路 進行提前反應(yīng)，確保車輛在直道和彎道行駛過程中的平穩(wěn)性，更好地實現(xiàn)智能車的路徑跟 蹤，同時該算法也可降低規(guī)劃路徑數(shù)據(jù)點，減少了算法在程序?qū)崿F(xiàn)過程中數(shù)據(jù)量，降低了程 序運算時間。
[0005] Pure Pursuit算法的原理為以規(guī)劃路徑上的某一個點為預(yù)目苗點，通過控制車輛轉(zhuǎn) 向角，使車輛可以沿著一條經(jīng)過該預(yù)瞄點的圓弧行駛，最終實現(xiàn)路徑跟蹤。因此，預(yù)瞄點的 確定是該算法實現(xiàn)的重要環(huán)節(jié)。本發(fā)明提出了一種根據(jù)幾何特性確定預(yù)瞄點的方法，采用 該方法可以降低對規(guī)劃路徑數(shù)據(jù)點的要求、減少數(shù)據(jù)計算量、提高算法的準確性。
[0006] -種基于Pure Pursuit算法的預(yù)目苗點確定方法，包括以下步驟：
[0007] 步驟I :GPS位置信息坐標系轉(zhuǎn)換。
[0008] GPS輸出數(shù)據(jù)經(jīng)度、煒度、高度坐標值，并不適合于平面運動的車輛導(dǎo)航。采用通用 橫軸墨卡托坐標系（Universal Transverse Mercator，UTM)將經(jīng)煒度轉(zhuǎn)化到平面坐標系。
[0009] 步驟2 :根據(jù)車輛參數(shù)和當(dāng)前車輛行駛速度確定預(yù)瞄距離 [0010] 根據(jù)人類駕駛車輛的經(jīng)驗，車輛以不同車速行駛在道路上時，駕駛員的預(yù)瞄距離 也隨之改變，在低速行駛時，駕駛員預(yù)瞄前方道路的距離較近，車速增大，駕駛員的預(yù)瞄距 離也隨之增大。
[0011] 步驟3 :判斷車輛與規(guī)劃路徑位置
[0012] 步驟4:確定預(yù)目苗點
[0013] 步驟5:道路判斷
[0014] 道路判斷主要用于判斷前方道路的狀況，即車輛行駛在直線道路上還是行駛在彎 道上。對于不同道路狀況，預(yù)瞄點的篩選與略有不同。
[0015] 步驟6:算法的實現(xiàn)
[0016] 得到預(yù)瞄點和預(yù)瞄距離后，根據(jù)Pure Pursuit算法可計算出轉(zhuǎn)向角控制量，為了 提高車輛在直線或彎道環(huán)境中車輛行駛的平穩(wěn)性，制定了更改比例增益的控制策略。
[0017] 具體實施過程如下，
[0018] 步驟I :GPS位置信息坐標系換算
[0019] GPS接收機接收到的GPS信號ASCII碼語句，包括GPGGA、GPGSA、GPGSV、GPRMC ; PGRME、PGRMM、PGRMT (GARMIN定義的語句）。因此得到的位置信息屬于空間大地坐標系，需 要轉(zhuǎn)換為UTM直角坐標系。
[0020] UTM坐標系是將地球劃分成60個投影帶，以180°經(jīng)線為起始線，向東每個6°劃 分為一個縱帶，每個縱帶中部的經(jīng)線被命名為中央經(jīng)線，并定義中央經(jīng)線坐標值為500000， 在南煒80°至北煒84°范圍內(nèi)，分別從赤道開始，每隔8°劃分一個橫帶。
[0021] 空間大地坐標系轉(zhuǎn)換為UTM坐標系，其中r = 6378. 137km，r為地球半徑；kQ = 0. 9996, kQ為坐標系轉(zhuǎn)化系數(shù)；ec = 0. 0818192, ec為偏心率；（λ，φ)為經(jīng)煒度坐標，λ為 經(jīng)度，iP為煒度；（e，η)為UTM坐標，e為方向東，η為方向北；Φ表示Φ的弧度值。因此可 得到如下公式：
[0030] 步驟2 :根據(jù)車輛參數(shù)和當(dāng)前車輛行駛速度確定預(yù)瞄距離
[0031] 預(yù)瞄距離是無人駕駛智能車實現(xiàn)路徑跟蹤的一個重要參數(shù)，預(yù)瞄距離與車速存在 一定的數(shù)學(xué)關(guān)系，即通過車輛行駛速度確定預(yù)瞄距離的公式為：
[0032] Id= AV 2+BV+C (3)
[0033] 其中，V為車輛行駛速度，A、B、C為常數(shù)項。
[0034] AV2表示車輛制動距離。其中，A可表示為：
[0035] A = l/2amax
[0036] a_為車輛最大制動加速度。
[0037] BV表示車輛遇到異常情況進行反應(yīng)的車輛行駛距離。
[0038] C表示車輛最小轉(zhuǎn)彎半徑。
[0039] 步驟3 :判斷車輛與規(guī)劃路徑位置
[0040] 基于Pure Pursuit算法的智能車路徑跟蹤實際上是以預(yù)瞄距離為半徑，以當(dāng)前車 體位置為圓心做圓，確定預(yù)目苗點，進而通過Pure Pursui t算法得出轉(zhuǎn)向角控制量，實現(xiàn)路徑 跟蹤。因此，以車體位置為圓心，預(yù)瞄距離為半徑畫出的圓與期望路徑存在三種位置關(guān)系： 相交、相切和相離。
[0041] Id為預(yù)瞄距離，α為跟蹤航向角度偏差，（cx，Cy)為車輛當(dāng)前位置，（G x，Gy)為期望 路徑上的預(yù)瞄點。
[0042] 期望路徑為一條已知的路徑，根據(jù)期望路徑上相鄰兩點坐標可得到直線方程的斜 率K和常數(shù)項B，因此可得到預(yù)瞄點（Gx，Gy)之間的關(guān)系為：
[0043] Gy=KGx+B (4)
[0044] 根據(jù)兩點間距離公式可得到：
[0045] Id2= (Cx-Gx)2+(Cy-Gy) 2 (5)
[0046] 將公式⑷代入公式（5)并整理：
[0052] 貝丨J :
[0053] aGx2+bGx+c = 0 (6)
[0054] 根據(jù)一元二次方程求根公式可對公式進行求解，并判斷期望路徑與預(yù)瞄范圍之間 的關(guān)系：
[0055] 當(dāng)b2_4ac>0時，規(guī)劃路徑與圓相交。
[0056] 當(dāng)b2_4ac = 0時，規(guī)劃路徑與圓相切。
[0057] 當(dāng)b2_4ac〈0時，規(guī)劃路徑與圓相離。
[0058] 步驟4:確定預(yù)目苗點
[0059] 當(dāng)規(guī)劃路徑與圓相交時，一元二次方程存在兩個解集（xl，yl)、（x2，y2)，其中一個 解為路徑跟蹤算法的預(yù)瞄點，另一個解為車輛已行駛過的區(qū)域，為無用點，因此要需根據(jù)方 程解與規(guī)劃路徑上點的關(guān)系，選擇可用的預(yù)瞄點，刪除無用點。當(dāng)規(guī)劃路徑與圓相交時，方 程兩個解與期望路徑上連續(xù)的兩個點（xk，yk)、（xk+l，yk+l)存在12種位置關(guān)系。其中， 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] 當(dāng)（xl，yl)、（x2, y2)在點（xk，yk)x 下側(cè)，且點（xl，yl)位于（x2, y2)下側(cè)。此 時（x2, y2)為預(yù)瞄點，（x2, y2)為（Gx，Gy)，可根據(jù)如下公式進行判定：
[0062]當(dāng)（xl，yl)、（x2，y2)在點（xk，yk)下側(cè)，且點（xl，yl)位于（x2，y2)上側(cè)。此時 (xl，yl)為預(yù)瞄點，（xl，yl)為（Gx，Gy)，可根據(jù)如下公式進行判定：
[0064]當(dāng)（xl，yl)在點（xk, yk)下側(cè)，（x2, y2)位于（xk, yk)，（xk+1, yk+Ι)兩點間。此 時（x2, y2)為預(yù)瞄點，即（x2, y2)為（Gx，Gy)，可根據(jù)如下公式進行判定：
[0066]當(dāng)（xl，yl)在點（xk, yk)下側(cè)，（x2, y2)位于（xk+1, yk+Ι)上側(cè)。此時（x2, y2) 為預(yù)瞄點，即（x2，y2)為（Gx，Gy)，可根據(jù)，如下公式進行判定：
[0068]當(dāng)（xl，yl)位于（xk，yk)、（xk+l，yk+l)兩點間，且（x2, y2)位于（xk，yk)下側(cè)。 此時（xl，yl)為預(yù)瞄點，即（x2, y2)為（Gx，Gy)，可根據(jù)如下公式進行判定：
[0070]當(dāng)（xl，yl)、（x2, y2)位于（xk, yk)、（xk+1, yk+Ι)兩點間，且（xl，yl)位于（x2， y2)下側(cè)。此時（x2，y2)為預(yù)瞄點，即（x2，y2)為（Gx，Gy)，可根據(jù)如下公式進行判定：
[0072]當(dāng)（xl，yl)