本發(fā)明屬于智能駕駛及其控制技術(shù)領(lǐng)域，特別涉及一種半結(jié)構(gòu)化環(huán)境中智能車輛自主換道超車運動規(guī)劃方法。

背景技術(shù)：

超車換道行為在日常車輛駕駛過程中頻繁發(fā)生，駕駛員為駛達目的地經(jīng)常使用該駕駛技能，同時它也成為日常交通中非常危險的行為策略。駕駛員需掌握熟練的技術(shù)和技巧，并遵守交通法規(guī)，防止意外和事故發(fā)生。隨行車速度的提高，超車時換道碰撞有明顯的上升趨勢。超車引發(fā)的事故不算多，由換道碰撞引起的死亡率不高，約占交通事故死亡人數(shù)的0.5％，但后果都較嚴(yán)重，而且造成嚴(yán)重的交通延時，大大降低了公路的運輸效能。因此研究更加安全的智能車輛換道超車方法在我國具有很大的現(xiàn)實意義。借助一系列智能感知等手段，自動駕駛的智能型汽車可大大提升車輛的主動安全性能，糾正駕駛員錯誤的駕駛行為，在未來智能化交通發(fā)展的前景下，智能型汽車將最終徹底消滅交通事故

真實交通環(huán)境中駕駛員需要的超車換道信息90％來源與視覺，所以當(dāng)前智能車輛超車換道實驗信息來源于毫米波雷達、激光雷達、相機和gps等高精度傳感器。近幾十年來，國內(nèi)外關(guān)于超車換道問題的相關(guān)研究主要分為兩類：即基于理論模型仿真分析和超車換道控制研究。模型仿真分析是嘗試通過模型建立、分析和仿真找出最優(yōu)的安全超車軌跡；而超車換道控制研究則基于實驗方法研究找出適合智能車的最優(yōu)控制算法。

在智能駕駛及其決策與規(guī)劃領(lǐng)域，對超車換道的實現(xiàn)提出了較高的要求。特別是在城郊這種半結(jié)構(gòu)化道路中，車道線時有時無，給智能車輛超車換道增加了難度。為此，需研究一種在城郊道路中基于信息融合的智能車超車換道規(guī)劃方法，使得該方法能夠合理的切換跟蹤gps點和車道線信息，實施更新?lián)Q道點或回道點來更好地躲避動靜態(tài)障礙物，更加安全的完成行駛?cè)蝿?wù)。

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

本發(fā)明提出了一種半結(jié)構(gòu)化環(huán)境中智能車輛自主換道超車運動規(guī)劃方法，其目的在于，為了解決現(xiàn)有理論和設(shè)計上存在超車換道、車輛回道不流暢的不足，通過采用跟蹤gps與車道線融合的超車換道、車輛回道路徑規(guī)劃方法，提高了智能車輛換道和回道的容錯性和魯棒性；在換道和回道過程中實時更新變道點，使車輛可以更好地避開動態(tài)、靜態(tài)障礙物，順利得到達目標(biāo)車道。

一種半結(jié)構(gòu)化環(huán)境中智能車輛自主換道超車運動規(guī)劃方法，包括以下步驟：

步驟一，建立車輛導(dǎo)航坐標(biāo)系，根據(jù)上一時刻車輛行駛狀態(tài)，并結(jié)合當(dāng)前車道上的前方障礙物情況，確定車輛所處的當(dāng)前行駛狀態(tài)；

所述車輛行駛狀態(tài)包括：直行、換道、超車或回道；

步驟二，檢測當(dāng)前道路環(huán)境，若符合向左換道條件，實施向左換道，確定換道速度，并采用基于gps與車道線融合的換道路徑規(guī)劃方法規(guī)劃超車換道軌跡；

步驟三，基于實時車速獲取超車換道最優(yōu)預(yù)瞄距離，從而確定超車換道預(yù)瞄點坐標(biāo)，基于超車換道預(yù)瞄點坐標(biāo)采用考慮車輛運動學(xué)方程的三次多項式方法實時計算并更新超車換道軌跡；

步驟四，向左換道結(jié)束后，依據(jù)設(shè)定超車速度和時間進行超車；

步驟五，超車結(jié)束后檢測當(dāng)前車道情況，若符合向右回道條件，實施向右回道，確定回道速度，并采用基于gps與車道線融合的換道路徑規(guī)劃方法規(guī)劃超車回道軌跡；

步驟六，基于實時車速獲取超車回道最優(yōu)預(yù)瞄距離，從而確定超車回道預(yù)瞄點坐標(biāo)，基于超車回道預(yù)瞄點坐標(biāo)采用考慮車輛運動學(xué)方程的三次多項式方法實時計算并更新超車回道軌跡；

步驟七，向右回道結(jié)束后，按照設(shè)定的直行速度開始直行。

進一步地，采用基于gps與車道線融合的換道路徑規(guī)劃方法規(guī)劃變道軌跡是指：

以gps為準(zhǔn)校驗車道線是否被正確識別，當(dāng)車道線中線與gps點橫向距離小于0.8m時，車道線被正確識別，以車道線為基準(zhǔn)規(guī)劃變道軌跡；否則，以gps為基準(zhǔn)規(guī)劃變道軌跡；

進行超車換道時，若以車道線為基準(zhǔn)進行換道，將當(dāng)前車道內(nèi)左車道線沿y軸正方向平移半個車道寬度作為全局期望路徑點；若以gps點為基準(zhǔn)進行換道，將gps點沿y軸正方向平移一個車道寬度得到的點作為全局期望路徑點；

進行超車回道時，若以車道線為基準(zhǔn)進行回道，將當(dāng)前車道內(nèi)右車道線沿y軸反方向平移半個車道寬度作為全局期望路徑點；若以gps點為基準(zhǔn)進行回道，將gps點沿y軸反方向平移一個車道寬度得到的點作為全局期望路徑點。

進一步地，所述變道點按照以下步驟獲?。?/p>

步驟1：根據(jù)車輛的當(dāng)前車速算出最優(yōu)預(yù)瞄距離la：

式中，lamin、vchange、kla為常數(shù)，其中l(wèi)amin＝6.55，vchange＝2.28，kla＝287；v表示車輛當(dāng)前速率；lamin表示智能車輛遇緊急情況時安全做出反應(yīng)所需要的最小距離，即最小轉(zhuǎn)彎半徑；

步驟2：分別計算車輛前方5個點到車輛所處位置(x0,y0)的距離di：

每個點的坐標(biāo)為(xi,yi)，i＝1,2,3,4,5，其中，第1個點為車輛當(dāng)前位置在偏移后的全局期望路徑上所對應(yīng)的點，后續(xù)四個點以第1個點為起始點每隔5米確定；

步驟3：分別計算di線段與x軸的航向角delta_di：delta_di＝arctan[(yi-y0)/(xi-x0)]；

步驟4：分別計算每個點在車輛行駛方向的橫向距離m_later(i)：

m_later(i)＝di×sin(yawvehicle-delta_di)

其中，yawvehicle表示車輛當(dāng)前航向角；

步驟5：計算每個點對應(yīng)的曲率半徑ri：ri＝di²/(2×m_later(i))；

步驟6：在ri中選擇大于等于la，且與la誤差最小的ri所對應(yīng)的點為預(yù)瞄點，即變道點(xf，yf)。

進一步地，采用考慮車輛運動學(xué)方程的三次多項式方法實時計算得到的變道軌跡方程為y(x)＝a0+a1x+a2x²+a3x³；

其中，a0＝a1＝0，yoffset＝y(tǒng)fcosθ+xfsinθ；

yoffset為車輛當(dāng)前位置和期望全局路徑在變道點處切線之間的橫向距離，θ表示變道點處期望航向角。

期望軌跡每50ms重新刷新一次，實時滾動優(yōu)化，既可以消除橫向的位置偏差，也能保證行車路線安全可靠。同時，由于控制對象的慣性與滯后性，設(shè)計控制器控制周期為100ms。對于橫向控制器而言，每100ms讀取一次期望給定軌跡(更新a0～a3軌跡系數(shù))，給客車轉(zhuǎn)向系統(tǒng)足夠的時間完成在控制周期內(nèi)的軌跡跟蹤。而控制器的采樣時間為100ms，快速采樣計算能有效消除外部干擾。

進一步地，所述車輛導(dǎo)航坐標(biāo)系是指坐標(biāo)原點為車頭中心位置，車輛正前方為x軸，車輛正左方為y軸，車輛正上方為z軸。

進一步地，所述向左換道條件是指車輛不在緩沖區(qū)、彎道及匝道；前方車輛速度小于本車車速；本車道左側(cè)存在同向車道；左側(cè)車道中安全距離內(nèi)不存在障礙物；

所述向右回道條件是指本車道右側(cè)存在同向車道；右側(cè)車道中安全距離內(nèi)不存在障礙物。

車輛當(dāng)前車道前方的障礙物用毫米波雷達檢測，側(cè)方如左右車道障礙物用激光雷達檢測；

前方車輛速度采用毫米波雷達檢測；

采用毫米波雷達檢測前方障礙，并對障礙物信息按照以下處理過程進行排序，獲得排序：

對于低速動態(tài)障礙物的濾除，采用速度門限的方法，通過對動態(tài)障礙物速度信息的比較，高于門限值的動態(tài)障礙物將得以保存，低于門限值的障礙物將被濾除，完成低速障礙物的濾除之后，需要對障礙物的進行排序并編號，編號的依據(jù)是每一組障礙物當(dāng)中所包含的距離信息，按照距離由近到遠，依次進行排序。

有益效果

本發(fā)明提供了一種半結(jié)構(gòu)化環(huán)境中智能車輛自主換道超車運動規(guī)劃方法，包括以下步驟：步驟一，建立車輛導(dǎo)航坐標(biāo)系，根據(jù)上一時刻車輛行駛狀態(tài)，并結(jié)合當(dāng)前車道上的前方障礙物情況，確定車輛所處的當(dāng)前行駛狀態(tài)；步驟二，實施向左換道，確定換道速度，并采用基于gps與車道線融合的換道路徑規(guī)劃方法規(guī)劃超車換道軌跡；步驟三，確定超車換道預(yù)瞄點坐標(biāo)，更新超車換道軌跡；步驟四，向左換道結(jié)束后，依據(jù)設(shè)定超車速度和時間進行超車；步驟五，實施向右回道，確定回道速度，并采用基于gps與車道線融合的換道路徑規(guī)劃方法規(guī)劃超車回道軌跡；步驟六，確定超車回道預(yù)瞄點坐標(biāo)，基更新超車回道軌跡；步驟七，向右回道結(jié)束后，按照設(shè)定的直行速度開始直行。

其優(yōu)點具體體現(xiàn)在以下幾點：

1.采用基于gps與車道線融合的換道和回道路徑規(guī)劃方法，在丟失車道線信息或者gps點信息其中的一種時，智能車輛仍然能夠安全的進行超車操作，具有更好的容錯性與魯棒性。

2.動態(tài)更新?lián)Q道點和回道點并生成軌跡，使車輛能夠更好的躲避動靜態(tài)障礙物，使整個超車過程更加安全；

3.改善了智能車輛在整個超車階段的軌跡生成方法與速度分配方法，有效的克服了換道或回道時橫向速度和加速度過大的問題，提高了乘車的舒適性。

附圖說明

圖1為本發(fā)明所述方法的流程圖；

圖2為車輛坐標(biāo)系；

圖3為采用三次多項式生成軌跡示意圖；

圖4為實施例換道效果圖。

具體實施方式

以下將結(jié)合附圖和具體實施例對本發(fā)明作進一步詳細說明。

本實施例采用長12m，寬2.5m大客車改裝的智能車，裝有激光雷達，毫米波雷達，攝像頭以及gps/imu系統(tǒng)，在標(biāo)準(zhǔn)雙向六車道的道路展開智能車輛的超車實驗。

參見圖1所示的流程圖，一種半結(jié)構(gòu)化環(huán)境中智能車輛自主換道超車運動規(guī)劃方法，規(guī)劃周期為50ms，包括以下步驟：

步驟一，建立車輛導(dǎo)航坐標(biāo)系，根據(jù)上一時刻車輛行駛狀態(tài)，并結(jié)合當(dāng)前車道上的前方障礙物情況，確定車輛所處的當(dāng)前行駛狀態(tài)；

所述車輛行駛狀態(tài)包括：直行、換道、超車或回道；

步驟二，檢測當(dāng)前道路環(huán)境，若符合向左換道條件，實施向左換道，確定換道速度，并采用基于gps與車道線融合的換道路徑規(guī)劃方法規(guī)劃超車換道軌跡；

步驟三，基于實時車速獲取超車換道最優(yōu)預(yù)瞄距離，從而確定超車換道預(yù)瞄點坐標(biāo)，基于超車換道預(yù)瞄點坐標(biāo)采用考慮車輛運動學(xué)方程的三次多項式方法實時計算并更新超車換道軌跡；

步驟四，向左換道結(jié)束后，依據(jù)設(shè)定超車速度和時間進行超車；

步驟五，超車結(jié)束后檢測當(dāng)前車道情況，若符合向右回道條件，實施向右回道，確定回道速度，并采用基于gps與車道線融合的換道路徑規(guī)劃方法規(guī)劃超車回道軌跡；

步驟六，基于實時車速獲取超車回道最優(yōu)預(yù)瞄距離，從而確定超車回道預(yù)瞄點坐標(biāo)，基于超車回道預(yù)瞄點坐標(biāo)采用考慮車輛運動學(xué)方程的三次多項式方法實時計算并更新超車回道軌跡；

步驟七，向右回道結(jié)束后，按照設(shè)定的直行速度開始直行。

采用毫米波雷達檢測前方障礙，并對障礙物信息按照以下處理過程進行排序，獲得排序：

對于低速動態(tài)障礙物的濾除，采用速度門限的方法，通過對動態(tài)障礙物速度信息的比較，高于門限值的動態(tài)障礙物將得以保存，低于門限值的障礙物將被濾除，完成低速障礙物的濾除之后，需要對障礙物的進行排序并編號，編號的依據(jù)是每一組障礙物當(dāng)中所包含的距離信息，按照距離由近到遠，依次進行排序。

所述的步驟一具體包括以下步驟：

(1.1)由于客車車長為12m，車頭與車尾在轉(zhuǎn)彎時存在較大的角度誤差，gps/imu接收器安裝在車頭位置，有效減少車輛轉(zhuǎn)向誤差及gps反饋誤差。為了在換道和回道階段平移車道線或gps點，定義車輛坐標(biāo)系，其中坐標(biāo)原點為車頭中心位置，車輛正前方為x軸，車輛正左方為y軸，車輛正上方為z軸，滿足右手定則，參見圖2。

(1.2)毫米波雷達檢測當(dāng)前車道中車輛前方是否存在障礙物，如果不存在，則車輛進行直行操作，目標(biāo)速度為vd＝5.4m/s；如果存在，則需考慮障礙物與本車之間的距離來進行下一步判斷，距離不同則智能車輛進行的操作不同。當(dāng)毫米波雷達檢測到存在障礙物，且兩車之間距離大于最小換道距離s1時(最小換道距離s1為：s1＝4v+17.5m，其中v為車輛當(dāng)前速度，單位m/s)，考慮換道；當(dāng)毫米波雷達檢測到存在障礙物，且兩車之間距離小于最小換道距離s1，且大于安全距離s2時(安全距離s2為：s2＝2.5v+12m，其中v為車輛當(dāng)前速度，單位m/s)，車輛減速直行；當(dāng)毫米波雷達檢測到存在障礙物，且兩車之間距離小于安全距離s2時，車輛緊急停車。

(1.3)根據(jù)上一時刻車輛狀態(tài)確定車輛當(dāng)前狀態(tài)：直行、換道、超車或者回道，然后分別進入不同的步驟中開始執(zhí)行相應(yīng)的操作。上一時刻車輛狀態(tài)為直行時，車輛當(dāng)前狀態(tài)為直行并轉(zhuǎn)步驟二執(zhí)行；上一時刻車輛狀態(tài)為換道時，車輛當(dāng)前狀態(tài)為換道并轉(zhuǎn)步驟三執(zhí)行；上一時刻車輛狀態(tài)為超車時，車輛當(dāng)前狀態(tài)為超車并轉(zhuǎn)步驟四執(zhí)行；上一時刻車輛狀態(tài)為回道時，車輛當(dāng)前狀態(tài)為回道并轉(zhuǎn)步驟六執(zhí)行。

所述步驟二具體包括以下步驟：

(2.1)當(dāng)激光雷達等感知檢測到當(dāng)前環(huán)境符合換道條件后，實施向左換道；否則，繼續(xù)直行。具體換道的條件包括以下四項：車輛不在緩沖區(qū)、彎道、匝道等特殊路段；毫米波雷達檢測到前方車輛速度小于本車車速；本車道左側(cè)存在同向車道；激光雷達檢測到左側(cè)車道中換道安全距離s3內(nèi)不存在障礙物(依據(jù)經(jīng)驗換道安全距離s3定義為：s3＝4v+30m，其中v為車輛當(dāng)前速度，單位m/s)。

(2.2)換道速度：為保證換道安全以及舒適性，換道階段速度應(yīng)低于直行階段速度，因此，應(yīng)適當(dāng)減速，令目標(biāo)速度vd＝2.7m/s。

(2.3)采用基于gps與車道線融合的換道路徑規(guī)劃方法：以gps為準(zhǔn)校驗車道線是否正確識別，當(dāng)車道線中線與gps點橫向距離小于0.8m時，車道正確識別，以車道線為基準(zhǔn)規(guī)劃換道軌跡；否則，以gps為基準(zhǔn)規(guī)劃換道軌跡。

所述步驟三具體包括以下步驟：

(3.1)若以車道線為基準(zhǔn)進行換道，將當(dāng)前車道內(nèi)左車道線沿y軸正方向平移半個車道寬度作為全局期望路徑點；若以gps點為基準(zhǔn)進行換道，將gps點沿y軸正方向平移一個車道寬度得到的點作為全局期望路徑點。其中，車道寬度為2m。

為使換道軌跡具有連續(xù)性，動態(tài)更新?lián)Q道點，每隔50ms刷新一次，具體計算步驟如下：

步驟3.1.1：為根據(jù)車輛的當(dāng)前車速算出最優(yōu)預(yù)瞄距離la，并根據(jù)最優(yōu)預(yù)瞄距離la及車輛前方五個路徑點確定換道點。

式中，lamin、vchange、kla為常數(shù)，其中l(wèi)amin＝6.55，vchange＝2.28，kla＝287。v表示車輛當(dāng)前速率。lamin表示智能車輛遇緊急情況時安全做出反應(yīng)所需要的最小距離，即最小轉(zhuǎn)彎半徑。

步驟3.1.2：取車輛前方5個點坐標(biāo)，點的選取方法如下：選取車輛當(dāng)前位置在偏移后的全局期望路徑上所對應(yīng)的點作為第1個點，，然后每隔5米確定剩余4個點。

分別計算車輛前方5個點坐標(biāo)(xi,yi)，i＝1,2,3,4,5到車輛所處位置(x0,y0)的距離di：

步驟3.1.3：分別計算di線段與x軸的航向角delta_di：

delta_di＝arctan[(yi-y0)/(xi-x0)]

步驟3.1.4：分別計算每個點在車輛行駛方向的橫向距離m_later(i)：

m_later(i)＝di×sin(yawvehicle-delta_di)

步驟3.1.5：分別算出曲率半徑ri：

ri＝di²/(2×m_later(i))

步驟3.1.6：在ri中選擇大于等于la，且與la誤差最小的ri所對應(yīng)的點為預(yù)瞄點，也即換道點(xf，yf)。

(3.2)采用考慮車輛運動學(xué)方程的三次多項式方法，生成換道軌跡，參見圖3，期望軌跡每50ms重新刷新一次，實時滾動優(yōu)化，既可以消除橫向的位置偏差，也能保證行車路線安全可靠。同時，由于控制對象的慣性與滯后性，設(shè)計控制器控制周期為100ms。對于橫向控制器而言，每100ms讀取一次基于三次多項式生成的期望軌跡，以此更新軌跡系數(shù)a0～a3軌跡系數(shù))，給客車轉(zhuǎn)向系統(tǒng)足夠的時間完成在控制周期內(nèi)的軌跡跟蹤。而控制器的采樣時間為100ms，快速采樣計算能有效消除外部干擾；本實例中的換道效果圖如圖4所示。

設(shè)換道軌跡方程為：

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

其中，a0＝a1＝0，yoffset＝y(tǒng)fcosθ+xfsinθ；

其中，yoffset為車輛當(dāng)前位置和期望全局路徑在換道點處切線之間的橫向距離，θ表示換道點處期望航向角。

所述步驟四具體包括以下步驟：

(4.1)向左換道結(jié)束后，開始超車操作；否則，繼續(xù)向左換道。向左換道結(jié)束條件如下：車輛當(dāng)前位置與期望軌跡橫向偏差小于設(shè)定值，設(shè)定值為0.2m。

(4.2)確定超車速度并判定車回道時刻：為保證能夠?qū)崿F(xiàn)超車任務(wù)，超車階段速度應(yīng)高于換道階段速度，因此應(yīng)適當(dāng)加速，令目標(biāo)速度vd＝5.4m/s。并使加速保持一段時間，令加速時間t＝100s。

所述步驟五具體包括以下步驟：

(5.1)超車結(jié)束條件：超車階段時間到達設(shè)定值，即100s。

(5.2)超車結(jié)束并滿足向右回道條件后，實施向右回道，若不滿足向右回道條件，則車輛在當(dāng)前車道直行直到滿足向右回道條件。向右回道條件包括以下兩項：本車道右側(cè)存在同向車道；激光雷達檢測到右側(cè)車道中回道安全距離s4內(nèi)不存在障礙物(依據(jù)經(jīng)驗安全距離s4定義為：s2＝4v+30m，其中v為車輛當(dāng)前速度，單位m/s)。

(5.3)確定回道速度：為確保安全以及舒適性，回道速度應(yīng)低于超車階段速度，因此適當(dāng)減速，令目標(biāo)速度vd＝2.7m/s。

(5.4)采用基于gps與車道線融合的回道路徑規(guī)劃方法：以gps為準(zhǔn)校驗車道線是否正確識別，當(dāng)車道線中線與gps點橫向距離小于0.8m時，車道正確識別，以車道線為基準(zhǔn)規(guī)劃回道軌跡；否則，以gps為基準(zhǔn)規(guī)劃回道軌跡。

所述步驟六具體包括以下步驟：

(6.1)若以車道線為基準(zhǔn)進行回道，將當(dāng)前車道內(nèi)右車道線沿y軸反方向平移半個車道寬度作為全局期望路徑點；若以gps點為基準(zhǔn)進行回道，將gps點沿y軸反方向平移一個車道寬度得到的點作為全局期望路徑點。其中，車道寬度為2m。

為使回道軌跡具有連續(xù)性，動態(tài)更新回道點，每隔50ms刷新一次，具體計算步驟如下：

步驟6.1.1：根據(jù)車輛的當(dāng)前車速算出最優(yōu)預(yù)瞄距離la，并根據(jù)最優(yōu)預(yù)瞄距離la及車輛前方五個路徑點確定回道點。

式中，lamin、vchange、k1a為常數(shù)，其中l(wèi)amin＝6.55，vchange＝2.28，k1a＝287。v表示車輛當(dāng)前速率。lamin表示智能車輛遇緊急情況時安全做出反應(yīng)所需要的最小距離，即最小轉(zhuǎn)彎半徑。

步驟6.1.2：取車輛前方5個點坐標(biāo)，點的選取方法如下：選取車輛當(dāng)前位置在偏移后的全局期望路徑上所對應(yīng)的點，作為第1個點，然后每隔5米確定剩余4個點。

分別計算車輛前方5個點坐標(biāo)(xi,yi)，i＝1,2,3,4,5到車輛所處位置(x0,y0)的距離di：

步驟6.1.3：分別計算di線段與x軸的航向角delta_di：

delta_di＝arctan[(yi-y0)/(xi-x0)]

步驟6.1.4：分別計算每個點在車輛行駛方向的橫向距離m_later(i)：

m_later(i)＝di×sin(yawvehicle-delta_di)

步驟6.1.5：分別算出曲率半徑ri：

ri＝di²/(2×m_later(i))

步驟6.1.6：在ri中選擇大于等于lala，且與lala誤差最小的ri所對應(yīng)的點為預(yù)瞄點，也即回道點(xf′，yf′)。

(6.2)采用考慮車輛運動學(xué)方程的三次多項式方法，生成回道軌跡，期望軌跡每50ms重新刷新一次，實時滾動優(yōu)化，既可以消除橫向的位置偏差，也能保證行車路線安全可靠。同時，由于控制對象的慣性與滯后性，設(shè)計控制器控制周期為100ms。對于橫向控制器而言，每100ms讀取一次基于三次多項式生成的期望軌跡，以此更新軌跡系數(shù)a0′～a3′，給客車轉(zhuǎn)向系統(tǒng)足夠的時間完成在控制周期內(nèi)的軌跡跟蹤。而控制器的采樣時間為100ms，快速采樣計算能有效消除外部干擾。

設(shè)回道軌跡方程為：

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

其中，a0＝a1＝0，yoffset＝y(tǒng)f'cosθ'+xf'sinθ'；

其中，y′offset為車輛當(dāng)前位置和期望全局路徑在回道點處切線之間的橫向距離，θ′表示回道點處期望航向角。

所述步驟七具體包括以下步驟：

(7.1)向右回道結(jié)束后開始直行；否則，繼續(xù)向右回道。向右回道結(jié)束的條件：車輛當(dāng)前位置與期望軌跡橫向偏差小于設(shè)定值，設(shè)定值為0.2m。

(7.2)確定直行速度：回道時為保證安全以及舒適性，車輛以較低速度行駛，為此回道結(jié)束進入直行階段后，車輛應(yīng)適當(dāng)加速，令目標(biāo)速度vd＝5.4m/s。

當(dāng)前周期結(jié)束后，在新的周期內(nèi)根據(jù)條件判斷選擇性執(zhí)行上述步驟，從而實現(xiàn)在城郊道路中基于信息融合的智能車超車換道。

以上實施例僅用以說明本發(fā)明的技術(shù)方案而非對其限制，盡管參照上述實施例對本發(fā)明進行了詳細的說明，所屬領(lǐng)域的普通技術(shù)人員應(yīng)當(dāng)理解：依然可以對本發(fā)明的具體實施方式進行修改或者等同替換，而未脫離本發(fā)明精神和范圍的任何修改或者等同替換，其均應(yīng)涵蓋在本發(fā)明的權(quán)利要求范圍當(dāng)中。