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

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

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