本發(fā)明屬于無人駕駛及路徑規(guī)劃技術(shù)領(lǐng)域，特別涉及一種基于車道線與gps跟隨的智能駕駛局部軌跡容錯規(guī)劃方法。

背景技術(shù)：

智能駕駛車輛是能通過車載傳感器系統(tǒng)感知道路環(huán)境，自動規(guī)劃行車路線并控制車輛到達預(yù)定目標的智能汽車。智能駕駛技術(shù)中的路徑軌跡規(guī)劃是智能駕駛領(lǐng)域的關(guān)鍵技術(shù)之一。在半結(jié)構(gòu)化環(huán)境下的智能駕駛過程中，易遇到gps/imu干擾不穩(wěn)定、車道線不清晰或不連續(xù)或錯誤識別車道線等復(fù)雜突發(fā)情況。該局部軌跡規(guī)劃為非特定場景下的動態(tài)規(guī)劃，要求準確完成車道跟隨，且需對異常突發(fā)情況快速響應(yīng)。

目前，國內(nèi)外學(xué)者圍繞智能駕駛車輛局部路徑軌跡規(guī)劃方法開展了大量研究工作，主要包括基于啟發(fā)式搜索、智能仿生、行為規(guī)劃以及再勵學(xué)習(xí)等路徑規(guī)劃方法，在全局路徑規(guī)劃方面取得了良好的效果。但并未考慮如半結(jié)構(gòu)化道路下車道不連續(xù)和車道錯誤識別或gps/imu定位失真等因數(shù)據(jù)不穩(wěn)定對規(guī)劃系統(tǒng)帶來的難適應(yīng)性。如何利用現(xiàn)有數(shù)據(jù)信息提高車道跟隨的穩(wěn)定性和實時性，是值得深入研究的實際問題。

在半結(jié)構(gòu)化環(huán)境下，對智能駕駛局部軌跡規(guī)劃的連續(xù)性、誤數(shù)據(jù)的高容錯性提出了較高的要求。半結(jié)構(gòu)化環(huán)境下的導(dǎo)航過程存在突發(fā)異常，包括車道不連續(xù)和車道錯誤識別，或gps/imu定位失真等。該情況下的局部路徑軌跡規(guī)劃需要具備對異常問題的高容錯性和穩(wěn)定性。為此，需研究一種基于車道線和gps跟隨的智能駕駛局部軌跡容錯規(guī)劃方法，使得該方法具有高容錯性和高穩(wěn)定性，提升智能車輛路徑軌跡規(guī)劃的實時性和魯棒性。

技術(shù)實現(xiàn)要素：

本發(fā)明為了克服上述現(xiàn)有技術(shù)的不足，提供了一種基于車道線與gps跟隨的智能駕駛局部軌跡容錯規(guī)劃方法，通過對多傳感器數(shù)據(jù)的有效甄別，設(shè)計容錯偏差算法，充分考慮各種數(shù)據(jù)異常情況下的軌跡規(guī)劃方法，實現(xiàn)智能駕駛車道跟隨，簡化局部規(guī)劃系統(tǒng)的設(shè)計，提升魯棒性和穩(wěn)定性。

一種基于車道線與gps跟隨的智能駕駛局部軌跡容錯規(guī)劃方法，包括以下步驟：

步驟一：初始化車輛駕駛跟隨模式，建立智能駕駛車輛坐標系，并設(shè)定期望路徑；

步驟二：根據(jù)gps信息對gps數(shù)據(jù)進行有效性判別；

步驟三：獲取智能車輛上攝像頭實時采集的車輛所在車道的車道線的識別狀態(tài)；

所述車輛所在車道的車道線包括左、右兩條車道線；

步驟四：根據(jù)車道線識別狀態(tài)計算容錯誤差；

步驟五：根據(jù)當前gps數(shù)據(jù)有效性、車道線識別狀態(tài)和容錯誤差，更新車輛駕駛跟隨模式；

步驟六：基于更新后的車輛駕駛跟隨模式進行局部路徑與軌跡規(guī)劃。

進一步地，所述步驟一的具體過程如下：

步驟1a：初始化車輛駕駛跟隨模式mfollow為gps跟隨模式mgps，即mfollow＝mgps；

步驟1b：設(shè)定車輛坐標系，坐標原點為車頭中心位置，車輛正前方為x軸，車輛正左方為y軸，車輛正上方為z軸；

gps/imu接收器安裝在智能駕駛車輛車頭位置，有效減少車輛轉(zhuǎn)向誤差及gps反饋誤差；因此定義車輛坐標系；

步驟1c：設(shè)定期望路徑，包括基于gps的期望路徑pgps和基于車道線的期望路徑plane，每50ms重新刷新一次；

其中，pgps來自地圖全局規(guī)劃提供的基于gps車道中心路徑，plane從智能駕駛車輛攝像頭檢測圖像中提取的智能車輛當前行駛車道線，所述智能車輛當前行駛車道線為車輛攝像頭采集的圖像中的左側(cè)車道線planeleft和右側(cè)車道線planeright的中值線；

給定基于三次多項式的車輛期望路徑表達形式：

p(x)＝a3x³+a2x²+a1x+a0

其中，x為車輛坐標系下x軸位置坐標，p為期望路徑，針對不同情景共分為兩種類型：基于gps的期望路徑pgps和基于車道線的期望路徑plane。

進一步地，所述根據(jù)gps信息對gps數(shù)據(jù)進行有效性判別的具體過程如下：

步驟2a：計算t與t-1時刻的gps位置誤差errorgps：errorgps＝pgps(x＝0|t)-pgps(x＝0|t-1)

步驟2b：對gps數(shù)據(jù)有效性validgps進行判別：

其中，pgps(x＝0|t)表示智能車輛在t時刻，在車輛坐標系下x軸位置坐標為0時的位置值，errormove為t與t-1時刻下gps在局部坐標系下因智能車輛移動產(chǎn)生的運動誤差，errormove＝a1v+b1，a1,b1為由經(jīng)驗試湊得到的常數(shù),，a取值為0～0.5，b取值為-1～+1；v為智能車輛行駛速度。

進一步地，所述智能車輛上攝像頭實時采集的車輛所在車道的車道線的識別狀態(tài)的獲取過程如下：

步驟3a：計算期望路徑下車輛行駛車道中心線分別基于gps和車道線的y軸位置ycentergps、ycenterlane，其中，ycenterlane包括車輛行駛車道中心線分別基于左車道線和右車道線的y軸位置ycenterleft和ycenterright：

其中，x1,x2,x3為三個不同位置點橫坐標，取x1＝5,x2＝10,x3＝15；

步驟3b：根據(jù)車道線的追蹤幀數(shù)ftrackleft、ftrackright來判別左、右車道線的有效性validleft、validright；

左、右兩車道線的原始有效性為validorileft、validoriright，隨車道線50ms更新一次，有效為true，無效為false；

當ftrackleft≤fthreshold時，左車道線判別為無效validleft＝false；當ftrackright≤fthreshold，右車道線判別為無效validright＝false，車道線須至少被追蹤fthreshold幀，fthreshold的取值為不小于30的整數(shù)；

步驟3c：根據(jù)期望路徑下車輛行駛車道中心線在y軸上的位置ycentergps與ycenterleft、ycenterright之間的距離誤差errorlaneleft、errorlaneright和中心偏差閾值errorcenterthreshold來判別車道線的有效性validleft、validright：

其中，wreal為真實車道寬度；

若errorlaneleft≤errorcenterthreshold，則左車道線判別為無效validleft＝false；若errorlaneright≤errorcenterthreshold，則右車道線判別為無效validright＝false；

步驟3d：比較檢測出的車道寬度與真實車道寬度來判別車道線的有效性：

w＝y(tǒng)centerleft-ycenterright

errorlanewidth＝w-wreal

其中，w為檢測出的車道寬度，wreal為真實車道寬度，errorlanewidth表示車道寬度誤差；

若errorlanewidth≤errorwidththreshold，errorwidththreshold表示車道寬度誤差閾值，則左右車道線均判別無效validleft＝false、validright＝false；

步驟3e：根據(jù)判別后的validleft、validright確定車道線的識別狀態(tài)statuslane：

若validleft、validright均為false，則statuslane＝nonelanedetected；

若validleft、validright均為true，則statuslane＝doublelanedetected；

若validleft、validright僅有一個為true，則statuslane＝singlelanedetected；

其中，nonelanedetected表示未正確識別車道線，doublelanedetected表示正確識別雙車道線，singlelanedetected表示正確識別單車道線。

進一步地，所述根據(jù)車道線識別狀態(tài)計算容錯誤差的具體過程如下：

步驟4a：根據(jù)車道線識別狀態(tài)statuslane，在期望路徑的更新周期內(nèi)分別計算單車道線持續(xù)時長、丟失左車道線持續(xù)時長以及丟失右車道線持續(xù)時長：

當正確識別雙車道線時，三種時長均置0；當正確識別單車道線時，對應(yīng)無效車道線的丟失時長加1，對應(yīng)有效車道線的丟失時長置0；當未正確識別車道線時，三種時長均置inf；

步驟4b：計算當前容錯偏差erroroffsetcurrent：

errorgpslane＝(ycenterleft+ycenterright)/2-ycentergps

erroroffsetcurrent＝λerrorgpslane+errorcalibration

其中，errorgpslane為采集的車道線中心線與gps位置之間偏差；λ為由經(jīng)驗試湊得到的偏差比例系數(shù)；

步驟4c：計算不同跟隨模式下的真實容錯偏差erroroffset：

其中，mgradient表示漸變模式，mgps表示gps跟隨模式，mlane表示車道線跟隨模式；

漸變模式是指從車道線跟隨模式逐漸變化到gps跟隨模式的中間過渡模式，在車道線跟隨模式下，不斷地在每個周期內(nèi)逐步減小當前期望路徑的容錯偏差，直至容錯偏差為0，使得當前期望路徑完全變?yōu)間ps跟隨模式下的期望路徑；

所述容錯偏差是指gps獲取的路徑數(shù)據(jù)與車道線之間的誤差。

進一步地，所述更新車輛駕駛跟隨模式的具體過程如下：

當gps數(shù)據(jù)有效時，即validgps＝true：

(1)若當前跟隨模式mfollow為gps跟隨模式mgps，則按以下表達式更新跟隨模式：

(2)若當前跟隨模式mfollow為車道線跟隨模式mlane，則按以下表達式更新跟隨模式：

case1：若左右車道線均判別無效，則更新為漸變模式，即mfollow＝mgradient；

case2：若單車道線識別有效，則比較單車道線的丟失持續(xù)時長和單車道線持續(xù)時長，如果丟失時長大于車道保留時長，則更新跟隨模式為漸變模式；

(3)若當前跟隨模式mfollow為漸變模式mgradient，則按以下表達式更新跟隨模式：

其中，case3：如果容錯偏差不為0且雙車道線均判別有效，則更新為車道線跟隨模式，

當gps數(shù)據(jù)無效時，即validgps＝false，且validorileft＝true或validoriright＝true，則更新跟隨模式為車道線跟隨模式并置零容錯偏差，即mfollow＝mlane,erroroffset＝0；若validgps＝false，且validorileft＝false、validoriright＝false，則停止局部規(guī)劃并警示報錯。

進一步地，基于更新后的車輛駕駛跟隨模式進行局部路徑與軌跡規(guī)劃的過程如下：

步驟6a：根據(jù)跟隨模式確定路徑，路徑p的表述為：

p＝pfollowmode+erroroffset

其中，valuedecrease為每間隔50ms，容錯偏差erroroffset的減小值，取值為0.005-0.008；

步驟6b：根據(jù)路徑p規(guī)劃基于三次多項式的運動軌跡：

(3)由當前車輛速度確定軌跡終點坐標(xf,yf,θf)：

(4)計算基于三次多項式的最優(yōu)軌跡ptrajectory：

其中，p'(xf)為p(xf)的導(dǎo)數(shù)，p(x)＝a3x³+a2x²+a1x+a0，yoffset為終點偏移量，即原點到終點切線方向的距離，a3,a2,a1,a0為軌跡ptrajectory的系數(shù)。

有益效果

本發(fā)明提供了一種基于車道線與gps跟隨的智能駕駛局部軌跡容錯規(guī)劃方法，其步驟包括：首先初始化跟隨模式并建立智能駕駛車輛坐標系；其次根據(jù)gps、車道線信息進行g(shù)ps數(shù)據(jù)和車道線識別狀態(tài)的甄別；然后根據(jù)甄別后的識別狀態(tài)計算容錯偏差，并更新跟隨模式；最后基于新跟隨模式進行局部路徑、軌跡規(guī)劃；

該規(guī)劃方法相對于現(xiàn)有技術(shù)具有以下幾點優(yōu)點：

1.數(shù)據(jù)有效性判別的過程中包括對gps、車道線的甄別算法，有效提高了系統(tǒng)對數(shù)據(jù)異常的甄別能力。

2.基于各數(shù)據(jù)狀態(tài)設(shè)計了容錯偏差，并對其進行實時動態(tài)的更新，簡化系統(tǒng)復(fù)雜度，易于實際應(yīng)用，提高了數(shù)據(jù)處理的魯棒性。

3.針對gps、車道線等多傳感數(shù)據(jù)對跟隨模式進行實時狀態(tài)轉(zhuǎn)移，簡化各模式之間的關(guān)系，實現(xiàn)多跟隨狀態(tài)之間的連續(xù)、平滑控制，提高了智能駕駛車輛的舒適性和穩(wěn)定性。

附圖說明

圖1為局部軌跡容錯規(guī)劃的跟隨模式轉(zhuǎn)移鏈圖；

圖2為實驗場地航拍圖；

圖3為局部軌跡容錯規(guī)劃方法流程圖；

圖4為實驗車輛坐標系示意圖。

具體實施方式

下面將結(jié)合附圖和實施例對本發(fā)明做進一步地說明。

本實施例采用長12m，寬2.5m大客車改裝的智能駕駛車輛，裝有激光雷達、毫米波雷達、攝像頭以及gps/imu系統(tǒng)，在標準直線道路展開局部軌跡容錯規(guī)劃實驗，如圖2所示，本實施例的實驗場地航拍圖。

如圖1所示，本發(fā)明用于局部軌跡容錯規(guī)劃的跟隨模式轉(zhuǎn)移鏈圖。

如圖3所示局部軌跡容錯規(guī)劃方法流程圖，一種基于車道線與gps跟隨的智能駕駛局部軌跡容錯規(guī)劃方法，局部規(guī)劃周期50ms，每規(guī)劃周期具體包括以下步驟：

一種基于車道線與gps跟隨的智能駕駛局部軌跡容錯規(guī)劃方法，包括以下步驟：

步驟一：初始化車輛駕駛跟隨模式，建立智能駕駛車輛坐標系，并設(shè)定期望路徑；

步驟二：根據(jù)gps信息對gps數(shù)據(jù)進行有效性判別；

步驟三：獲取智能車輛上攝像頭實時采集的車輛所在車道的車道線的識別狀態(tài)；

所述車輛所在車道的車道線包括左、右兩條車道線；

步驟四：根據(jù)車道線識別狀態(tài)計算容錯誤差；

步驟五：根據(jù)當前gps數(shù)據(jù)有效性、車道線識別狀態(tài)和容錯誤差，更新車輛駕駛跟隨模式；

步驟六：基于更新后的車輛駕駛跟隨模式進行局部路徑與軌跡規(guī)劃。

所述步驟一的具體過程如下：

步驟1a：初始化車輛駕駛跟隨模式mfollow為gps跟隨模式mgps，即mfollow＝mgps；

步驟1b：設(shè)定車輛坐標系，坐標原點為車頭中心位置，車輛正前方為x軸，車輛正左方為y軸，車輛正上方為z軸；

gps/imu接收器安裝在智能駕駛車輛車頭位置，有效減少車輛轉(zhuǎn)向誤差及gps反饋誤差；因此定義車輛坐標系，如圖4所示；

步驟1c：設(shè)定期望路徑，包括基于gps的期望路徑pgps和基于車道線的期望路徑plane，每50ms重新刷新一次；

其中，pgps來自地圖全局規(guī)劃提供的基于gps車道中心路徑，plane從智能駕駛車輛攝像頭檢測圖像中提取的智能車輛當前行駛車道線，所述智能車輛當前行駛車道線為車輛攝像頭采集的圖像中的左側(cè)車道線planeleft和右側(cè)車道線planeright的中值線；

給定基于三次多項式的車輛期望路徑表達形式：

p(x)＝a3x³+a2x²+a1x+a0

其中，x為車輛坐標系下x軸位置坐標，p為期望路徑，針對不同情景共分為兩種類型：基于gps的期望路徑pgps和基于車道線的期望路徑plane。

所述根據(jù)gps信息對gps數(shù)據(jù)進行有效性判別的具體過程如下：

步驟2a：計算t與t-1時刻的gps位置誤差errorgps：errorgps＝pgps(x＝0|t)-pgps(x＝0|t-1)

步驟2b：對gps數(shù)據(jù)有效性validgps進行判別：

其中，pgps(x＝0|t)表示智能車輛在t時刻，在車輛坐標系下x軸位置坐標為0時的位置值，errormove為t與t-1時刻下gps在局部坐標系下因智能車輛移動產(chǎn)生的運動誤差，errormove＝a1v+b1，a1,b1為由經(jīng)驗試湊得到的常數(shù),，a取值為0～0.5，b取值為-1～+1；v為智能車輛行駛速度。

所述智能車輛上攝像頭實時采集的車輛所在車道的車道線的識別狀態(tài)的獲取過程如下：

步驟3a：計算期望路徑下車輛行駛車道中心線分別基于gps和車道線的y軸位置ycentergps、ycenterlane，其中，ycenterlane包括車輛行駛車道中心線分別基于左車道線和右車道線的y軸位置ycenterleft和ycenterright：

其中，x1,x2,x3為三個不同位置點橫坐標，取x1＝5,x2＝10,x3＝15；

步驟3b：根據(jù)車道線的追蹤幀數(shù)ftrackleft、ftrackright來判別左、右車道線的有效性validleft、validright；

左、右兩車道線的原始有效性為validorileft、validoriright，隨車道線50ms更新一次，有效為true，無效為false；

當ftrackleft≤fthreshold時，左車道線判別為無效validleft＝false；當ftrackright≤fthreshold，右車道線判別為無效validright＝false，車道線須至少被追蹤fthreshold幀，fthreshold的取值為不小于30的整數(shù)；

步驟3c：根據(jù)期望路徑下車輛行駛車道中心線在y軸上的位置ycentergps與ycenterleft、ycenterright之間的距離誤差errorlaneleft、errorlaneright和中心偏差閾值errorcenterthreshold來判別車道線的有效性validleft、validright：

其中，wreal為真實車道寬度；

若errorlaneleft≤errorcenterthreshold，則左車道線判別為無效validleft＝false；若errorlaneright≤errorcenterthreshold，則右車道線判別為無效validright＝false；

步驟3d：比較檢測出的車道寬度與真實車道寬度來判別車道線的有效性：

w＝y(tǒng)centerleft-ycenterright

errorlanewidth＝w-wreal

其中，w為檢測出的車道寬度，wreal為真實車道寬度，errorlanewidth表示車道寬度誤差；

若errorlanewidth≤errorwidththreshold，errorwidththreshold表示車道寬度誤差閾值，則左右車道線均判別無效validleft＝false、validright＝false；

步驟3e：根據(jù)判別后的validleft、validright確定車道線的識別狀態(tài)statuslane：

若validleft、validright均為false，則statuslane＝nonelanedetected；

若validleft、validright均為true，則statuslane＝doublelanedetected；

若validleft、validright僅有一個為true，則statuslane＝singlelanedetected；

其中，nonelanedetected表示未正確識別車道線，doublelanedetected表示正確識別雙車道線，singlelanedetected表示正確識別單車道線。

所述根據(jù)車道線識別狀態(tài)計算容錯誤差的具體過程如下：

步驟4a：根據(jù)車道線識別狀態(tài)statuslane，在期望路徑的更新周期內(nèi)分別計算單車道線持續(xù)時長、丟失左車道線持續(xù)時長以及丟失右車道線持續(xù)時長：

當正確識別雙車道線時，三種時長均置0；當正確識別單車道線時，對應(yīng)無效車道線的丟失時長加1，對應(yīng)有效車道線的丟失時長置0；當未正確識別車道線時，三種時長均置inf；

步驟4b：計算當前容錯偏差erroroffsetcurrent：

errorgpslane＝(ycenterleft+ycenterright)/2-ycentergps

erroroffsetcurrent＝λerrorgpslane+errorcalibration

其中，errorgpslane為采集的車道線中心線與gps位置之間偏差；λ為由經(jīng)驗試湊得到的偏差比例系數(shù)；

步驟4c：計算不同跟隨模式下的真實容錯偏差erroroffset：

其中，mgradient表示漸變模式，mgps表示gps跟隨模式，mlane表示車道線跟隨模式；

漸變模式是指從車道線跟隨模式逐漸變化到gps跟隨模式的中間過渡模式，在車道線跟隨模式下，不斷地在每個周期內(nèi)逐步減小當前期望路徑的容錯偏差，直至容錯偏差為0，使得當前期望路徑完全變?yōu)間ps跟隨模式下的期望路徑；

所述容錯偏差是指gps獲取的路徑數(shù)據(jù)與車道線之間的誤差。

所述更新車輛駕駛跟隨模式的具體過程如下：

當gps數(shù)據(jù)有效時，即validgps＝true：

(1)若當前跟隨模式mfollow為gps跟隨模式mgps，則按以下表達式更新跟隨模式：

(2)若當前跟隨模式mfollow為車道線跟隨模式mlane，則按以下表達式更新跟隨模式：

case1：若左右車道線均判別無效，則更新為漸變模式，即mfollow＝mgradient；

case2：若單車道線識別有效，則比較單車道線的丟失持續(xù)時長和單車道線持續(xù)時長，如果丟失時長大于車道保留時長，則更新跟隨模式為漸變模式；

(3)若當前跟隨模式mfollow為漸變模式mgradient，則按以下表達式更新跟隨模式：

其中，case3：如果容錯偏差不為0且雙車道線均判別有效，則更新為車道線跟隨模式，

當gps數(shù)據(jù)無效時，即validgps＝false，且validorileft＝true或validoriright＝true，則更新跟隨模式為車道線跟隨模式并置零容錯偏差，即mfollow＝mlane,erroroffset＝0；若validgps＝false，且validorileft＝false、validoriright＝false，則停止局部規(guī)劃并警示報錯。

基于更新后的車輛駕駛跟隨模式進行局部路徑與軌跡規(guī)劃的過程如下：

步驟6a：根據(jù)跟隨模式確定路徑，路徑p的表述為：

p＝pfollowmode+erroroffset

其中，valuedecrease為每間隔50ms，容錯偏差erroroffset的減小值，取值為0.005-0.008；

步驟6b：根據(jù)路徑p規(guī)劃基于三次多項式的運動軌跡：

(5)由當前車輛速度確定軌跡終點坐標(xf,yf,θf)：

(6)計算基于三次多項式的最優(yōu)軌跡ptrajectory：

其中，p'(xf)為p(xf)的導(dǎo)數(shù)，p(x)＝a3x³+a2x²+a1x+a0，yoffset為終點偏移量，即原點到終點切線方向的距離，a3,a2,a1,a0為軌跡ptrajectory的系數(shù)。

最后，將規(guī)劃軌跡傳入跟蹤控制系統(tǒng)，當前規(guī)劃周期結(jié)束后，在新的規(guī)劃周期內(nèi)重復(fù)上述6個步驟，依次循環(huán)實現(xiàn)基于gps與車道線的局部容錯路徑規(guī)劃。

以上所述僅是本發(fā)明的優(yōu)選實施方式，本發(fā)明的保護范圍并不僅限于上述實施例，凡屬于本發(fā)明思路下的技術(shù)方案均屬于本發(fā)明的保護范圍。應(yīng)當指出，對于本技術(shù)領(lǐng)域的普通技術(shù)人員來說，在不脫離本發(fā)明原理前提下的若干改進和潤飾，這些改進和潤飾也應(yīng)視為本發(fā)明的保護范圍。