本發(fā)明屬于汽車主動安全與輔助駕駛領(lǐng)域，特別是涉及一種基于車車協(xié)同下無人駕駛車輛的換道軌跡規(guī)劃方法。

背景技術(shù)：

在無人駕駛環(huán)境下，車輛對周圍交通狀態(tài)的感知和車輛實時軌跡控制是無人駕駛車輛安全高效運行的重要因素。其中軌跡控制就是通過軌跡規(guī)劃得到平滑連續(xù)的曲率軌跡，使之滿足車輛的運動特性和安全要求。現(xiàn)有的軌跡規(guī)劃是對機器人研究領(lǐng)域的路徑規(guī)劃方法的拓展，一類是全局軌跡方法，提出尋找連接起點到目標點的全局軌跡，由于該類方法需要對行駛環(huán)境變化進行全面分析，耗時很長且不利于處理突發(fā)情況，因此難以在無人駕駛車輛的軌跡規(guī)劃中實現(xiàn)。另一類局部軌跡方法是在全局軌跡的引導下，依靠環(huán)境感知信息生成實時軌跡，可廣泛應(yīng)用于無人駕駛車輛的軌跡規(guī)劃中。

無人駕駛車輛的軌跡規(guī)劃包括車輛跟馳、換道和超車幾種情況。其中跟馳軌跡規(guī)劃方法的研究在傳統(tǒng)成熟的跟馳模型下已蓬勃發(fā)展，部分相關(guān)技術(shù)已得到應(yīng)用，超車行為可以看作兩次換道行為，因此車輛換道軌跡規(guī)劃是無人駕駛車輛軌跡規(guī)劃研究的重點內(nèi)容之一?，F(xiàn)常用于無人駕駛車輛的換道軌跡規(guī)劃方法有貝塞爾曲線、樣條曲線和多項式曲線。貝塞爾曲線生成的換道軌跡具有連續(xù)的曲率半徑，但是需要選取控制點，只適用于靜態(tài)規(guī)劃，難以實現(xiàn)換道中的實時避碰行為。樣條曲線可以對圓弧換道軌跡、正弦換道軌跡進行再規(guī)劃，克服原有曲線曲率突變、不連續(xù)等缺點，但會導致無法實現(xiàn)實時控制。多項式曲線計算速度快，曲率連續(xù)便于實時控制，通過三次微分可得出舒適度方程。

在無人駕駛環(huán)境下，車輛大多處于高速行駛狀態(tài)，且車速穩(wěn)定，車輛之間的跟馳距離很小，給無人駕駛車輛的換道帶來困難。雖然超車換道等非轉(zhuǎn)向換道會減少甚至避免，但道路出入口的強制換道和城市路段的轉(zhuǎn)向換道是不可避免的，所以本發(fā)明立足于車車協(xié)同的思想，依靠車輛之間的信息交流，簡化并實現(xiàn)無人駕駛環(huán)境下車輛安全舒適換道。

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

針對現(xiàn)有技術(shù)的不足，本發(fā)明提供了一種基于車車協(xié)同下無人駕駛車輛的換道軌跡規(guī)劃方法，以換道協(xié)同策略和五次多項式的軌跡規(guī)劃方法為基礎(chǔ)，以車輛運動學和舒適度為控制條件，建立目標車道后車不同協(xié)同程度下的主車換道軌跡優(yōu)化模型和換道軌跡評價費用模型，可以規(guī)劃設(shè)計出無人駕駛環(huán)境下車輛安全換道的最優(yōu)軌跡。

為了實現(xiàn)上述技術(shù)目的，本發(fā)明采用如下技術(shù)方案：

一種基于車車協(xié)同下無人駕駛車輛的換道軌跡規(guī)劃方法，其特征在于，具體包括以下步驟：

步驟一：根據(jù)安全避碰原則建立車車協(xié)同換道準則：

主車換道前發(fā)出換道請求，周邊車輛接收到主車發(fā)出的換道請求信號后是否接受換道請求的準則為：

(101)當前車道后車、目標車道前車若保持當前車速勻速行駛不會與主車相碰撞，則接受主車的換道請求，并保持當前車速行駛，直至主車換道成功，若會與主車相撞，拒絕主車的換道請求；

(102)當前車道前車、目標車道后車若保持當前車速勻速行駛不會與主車相碰撞，則接受主車的換道請求，并保持當前速度勻速行駛，直至主車換道成功；若會與主車相碰撞，判斷與其后車的位置關(guān)系和協(xié)同程度的要求，在可接受的協(xié)同程度下加速或減速，并判斷協(xié)同行為后是否與主車發(fā)生碰撞，若無碰撞，接受換道請求，并在預定協(xié)同程度下加速或減速協(xié)同，直至主車換道成功，若協(xié)同行為后仍與主車碰撞，拒絕主車的換道請求；

步驟二：建立主車、當前車道前車、目標車道后車的行駛軌跡模型：

以主車換道前所在的位置為原點，換道前車輛行駛的方向為x軸，垂直于行駛方向為y軸，建立平面直角坐標系，建立主車、目標車道前后車的行駛軌跡模型；主車換道運動軌跡方程，運動軌跡方程包括：

(201)采用五次多項式建立主車m換道的行駛軌跡：

其中，xm(t)和ym(t)分別表示m的縱向位移和橫向位移；ai,i∈{0,1,2,3,4,5}和bi,i∈{0,1,2,3,4,5}分別是是m縱向位移和橫向位移的參數(shù)；t表示時間。根據(jù)換道起始狀態(tài)和換道結(jié)束狀態(tài)求解五次多項式；

(202)當前車道前車lo的行駛軌跡：

(203)目標車道后車fd的行駛軌跡：

其中，分別表示m與lo、fd之間換道前的車頭間距；分別表示lo、fd的初始行駛速度；w表示車輛fd到m的橫向距離；表示fd的減速度；表示fd通過減速達到可接受速度的時間，當fd勻速行駛時，

步驟三：選取矩形車輛模型，根據(jù)安全避碰原則建立車車協(xié)同安全換道條件的計算模型：

(301)主車m與當前車道前車lo的安全換道的條件：

(302)主車m與目標車道后車fd的安全換道的條件：

其中，分別表示lo的車長與車寬，與分別表示fd的車長與車寬；是車輛沿質(zhì)心旋轉(zhuǎn)的半徑；α為車輛的偏轉(zhuǎn)角，為主車m的矩形模型頂點與水平方向的夾角；

步驟四：根據(jù)車輛動力學和乘客舒適度兩方面的因素對主車安全換道模型中的參數(shù)進行約束，給定測試參數(shù)，計算得到換道軌跡集，為車車協(xié)同下無人駕駛車輛的換道軌跡：

(401)動力學約束條件：

轉(zhuǎn)向角：δα為車輛單位時間δt內(nèi)變化的轉(zhuǎn)向角，δαmax為車輛設(shè)置的在不影響駕駛安全性能下，單位時間δt內(nèi)變化的最大轉(zhuǎn)向角；為m在t時刻的橫向速度，為m在t時刻的縱向速度；

曲率：k為曲率，r為轉(zhuǎn)彎半徑，zm為主車的軸距，kmax為不影響駕駛安全性能的最大曲率；

速度：0<vx,m(t)<vx,max，vx,m(t)為m縱向的行駛速度，vx,max為路段上限制的最大縱向行駛速度；

位置：0<ym(t)<w，ym(t)為m的橫向位移，w為車道的寬度；

(402)乘客舒適度約束條件，采用橫縱向的加速度和加加速度來判斷：

其中，ax和jx分別為縱向的加速度和加加速度；ay和jy分別為橫向的加速度和加加速度；ax,max和jx,max為考慮舒適度而允許的最大縱向加速度和加加速度；ay,max和jy,max為考慮舒適度而允許的最大橫向加速度和加加速度；

步驟五：綜合人的舒適程度、換道的縱向位移和換道時間三個因素建立評價換道軌跡影響程度的費用函數(shù)模型，計算得到最優(yōu)的換道軌跡，為車車協(xié)同下無人駕駛車輛的換道最優(yōu)軌跡：

(501)目標車道后車fd勻速協(xié)同時的費用函數(shù)：

其中，w1、w2、w3是權(quán)重系數(shù)，其和為1；max(j)、max(xc)和max(tc)分別為可行換道軌跡集中的舒適度函數(shù)、縱向換道距離、換道時間的最大值；其中人的舒適度計算函數(shù)為

(502)目標車道后車fd減速協(xié)同時的費用函數(shù)

其中，λ1、λ2、λ3、λ4是權(quán)重系數(shù)，其和為1；xfd,slow是換道時間tc內(nèi)fd減速協(xié)同的距離，xfd,con是換道時間tc內(nèi)如果fd保持勻速行駛的距離；

步驟六：由費用函數(shù)計算得出的主車最優(yōu)換道軌跡的最終狀態(tài)，根據(jù)主車換道前的初始狀態(tài)和最終狀態(tài)求解公式(1)得出最優(yōu)換道軌跡方程。

本發(fā)明的有益效果在于：

本發(fā)明提出的一種基于車車協(xié)同下無人駕駛車輛的換道軌跡規(guī)劃方法；首先由五次多項式生成無人駕駛車輛換道軌跡，根據(jù)安全避碰原則，建立矩形車輛模型條件下的安全換道的邊界條件模型，然后通過滿足車輛動力學和乘客舒適度的要求作為控制條件，求解得到無人駕駛車輛的實際換道軌跡范圍，最后建立軌跡評價費用函數(shù)，得到最優(yōu)換道軌跡，運用本發(fā)明方法能夠完成無人駕駛環(huán)境下的車輛安全換道，同時滿足車輛換道舒適性以及動力學的要求。

附圖說明

圖1為本發(fā)明一種基于車車協(xié)同下無人駕駛車輛的換道軌跡規(guī)劃方法中主車與周圍其他車輛的相對位置示意圖。

圖2為本發(fā)明一種基于車車協(xié)同下無人駕駛車輛的換道軌跡規(guī)劃方法中車車協(xié)同的過程示意圖。

圖3(a)為本發(fā)明一種基于車車協(xié)同下無人駕駛車輛的換道軌跡規(guī)劃方法中車輛m與lo的潛在碰撞形式示意圖。

圖3(b)為本發(fā)明一種基于車車協(xié)同下無人駕駛車輛的換道軌跡規(guī)劃方法中車輛m與fd的潛在碰撞形式示意圖。

圖4為本發(fā)明一種基于車車協(xié)同下無人駕駛車輛的換道軌跡規(guī)劃方法中4種車輛模型。

圖5為本發(fā)明一種基于車車協(xié)同下無人駕駛車輛的換道軌跡規(guī)劃方法中車輛碰撞關(guān)系示意圖。

具體實施方式

如圖1所示，主車m在換道過程中，周圍車輛主要有當前車道前車lo，當前車道后車fo，目標車道前車ld和目標車道后車lo。一種基于車車協(xié)同下無人駕駛車輛的換道軌跡規(guī)劃方法包括以下步驟：

第一步，主車換道前發(fā)出換道請求，建立周邊車輛接收到主車發(fā)出的換道請求信號后是否接受換道請求的準則：

當前車道后車fo、目標車道前車ld若保持當前車速勻速行駛不會與主車相碰撞，則接受主車的換道請求，并保持當前車速行駛，直至主車換道成功，若會與主車相撞，拒絕主車的換道請求。

當前車道前車lo、目標車道后車fd若保持當前車速勻速行駛不會與主車相碰撞，則接受主車的換道請求，并保持當前速度勻速行駛，直至主車換道成功；若會與主車相碰撞，需判斷與其后車的位置關(guān)系和協(xié)同程度的要求，在可接受的協(xié)同程度下加速或減速，并判斷協(xié)同行為后是否與主車發(fā)生碰撞，若無碰撞，接受換道請求，并在預定協(xié)同程度下加速或減速協(xié)同，直至主車換道成功，若協(xié)同行為后仍與主車碰撞，拒絕主車的換道請求。當前車道前車、目標車道后車與主車的車車協(xié)同過程見圖2。

根據(jù)車車協(xié)同策略，換道車輛之間的避碰發(fā)生在主車與當前車道前車和目標車道后車之間。

第二步，建立主車m、當前車道前車lo、目標車道后車lo的行駛軌跡模型。以m換道前所在的位置為原點，換道前車輛行駛的方向為x軸，垂直于行駛方向為y軸，建立平面直角坐標系。

考慮到多項式曲線計算速度快，曲率連續(xù)便于實時控制，通過三次微分可得出舒適度方程，采用五次多項式建立主車m換道的行駛軌跡：

其中，xm(t)和ym(t)分別表示m的縱向位移和橫向位移；ai,i∈{0,1,2,3,4,5}和bi,i∈{0,1,2,3,4,5}分別是是m縱向位移和橫向位移的參數(shù)；t表示時間。根據(jù)換道起始狀態(tài)和換道結(jié)束狀態(tài)求解五次多項式。

起始狀態(tài)：

結(jié)束狀態(tài)：

其中，x0和y0表示m在起始狀態(tài)的橫向和縱向位移，vx，0和vy，0表示m在起始狀態(tài)的橫向和縱向速度，ax，0和ay，0表示m在起始狀態(tài)的橫向和縱向加速度；xc和yc表示換道結(jié)束時的橫向和縱向位移，vx，c和vy，c表示m在換道結(jié)束時的橫向和縱向速度，αx，c和ay，c表示m在換道結(jié)束時的橫向和縱向加速度，tc為換道時間。

當前車道前車lo的行駛軌跡：

目標車道后車fd的行駛軌跡:

其中，分別表示m與lo、fd之間換道前的車頭間距；分別表示lo、fd的初始行駛速度；w表示車輛fd到m的橫向距離，文中記為車道的寬度；表示fd的減速度；表示fd通過減速達到可接受速度的時間。

本發(fā)明給出車輛碰撞形式，如圖3(a)、圖3(b)?？紤]到各種車輛模型的優(yōu)缺點，為實現(xiàn)車輛在換道過程中避碰的要求，采用了矩形作為車輛模型，如圖4；給定的車輛碰撞關(guān)系如圖5。

第三步，建立車輛成功安全換道條件如下：

若為當前車道前車協(xié)同換道，選用lo與m安全換道的條件進行軌跡計算：

若為目標車道后車協(xié)同換道，選用fd與m安全換道的條件進行軌跡計算：

其中，分別表示lo的車長與車寬，與分別表示fd的車長與車寬；是車輛沿質(zhì)心旋轉(zhuǎn)的角度；α為車輛的偏轉(zhuǎn)角，為主車m的矩形模型頂點與水平方向的夾角；

第四步，根據(jù)車輛動力學和乘客舒適度兩方面的因素對主車安全換道模型中的參數(shù)進行約束，給定測試參數(shù)，計算得到換道軌跡集：

運動學要求：

轉(zhuǎn)向角：δα為車輛單位時間δt內(nèi)變化的轉(zhuǎn)向角，δαmax為車輛設(shè)置的在不影響駕駛安全性能下，單位時間δt內(nèi)變化的最大轉(zhuǎn)向角；為m在t時刻的橫向速度，為m在t時刻的縱向速度；

曲率：k為曲率，r為轉(zhuǎn)彎半徑，zm為主車的軸距，kmax為不影響駕駛安全性能的最大曲率；

速度：0<vx,m(t)<vx,max，vx,m(t)為m縱向的行駛速度，vx,max為路段上限制的最大縱向行駛速度；

位置：0<ym(t)<w，ym(t)為m的橫向位移，w為車道的寬度；

舒適度要求：

其中，ax和jx分別為縱向的加速度和加加速度；ay和jy分別為橫向的加速度和加加速度；ax,max和jx,max為考慮舒適度而允許的最大縱向加速度和加加速度；ay,max和jy,max為考慮舒適度而允許的最大橫向加速度和加加速度；

第五步，綜合人的舒適程度、換道的縱向位移和換道時間三個因素建立評價換道軌跡影響程度的費用函數(shù)模型，計算得到最優(yōu)的換道軌跡，為車車協(xié)同下無人駕駛車輛的換道最優(yōu)軌跡：

目標車道后車fd勻速協(xié)同時的費用函數(shù)：

其中，w1、w2、w3是權(quán)重系數(shù)，其和為1；max(j)、max(xc)和max(tc)分別為可行換道軌跡集中的舒適度函數(shù)、縱向換道距離、換道時間的最大值。其中人的舒適度計算函數(shù)為

目標車道后車fd減速協(xié)同時的費用函數(shù)：

其中，λ1、λ2、λ3、λ4是權(quán)重系數(shù)，其和為1；xfd,slow是換道時間tc內(nèi)fd減速協(xié)同的距離，xfd,con是換道時間tc內(nèi)如果fd保持勻速行駛的距離。

由費用函數(shù)計算得出的主車最優(yōu)換道軌跡的最終狀態(tài)，根據(jù)主車換道前的初始狀態(tài)和最終狀態(tài)求解公式(1)得出最優(yōu)換道軌跡方程。

本實施例選取后車勻速協(xié)同換道和減速協(xié)同換道兩種情況進行分析。給定測試參數(shù)如下，所有車輛車長均為4.5m，車寬均為1.8m，車道寬度均為3.5m；m的起始速度vx，0為30km/h，換道到目標車道的速度vx，c為40km/h，lo的速度30km/h；m與lo的車頭間距為20m；換道時間設(shè)為1-12s；車輛動力學和舒適的約束條件：轉(zhuǎn)向角δα∈(0，20°)；曲率k∈(0，1)；vx，m(t)∈(0，60)；ym(t)∈(0，3.5)；ax，ay的絕對值均小于10；jx，jy的絕對值均小于10。測試以下幾種情況下主車換道的安全軌跡：

目標車道后車勻速協(xié)同換道時：給定fd的速度40km/h，fd與m的車頭間距為20m。帶入公式(3)得到fd的行駛軌跡，再將其帶入公式(5)，根據(jù)公式(5)及約束條件可計算生成符合要求的換道軌跡，見表1。

表1目標車道后車勻速協(xié)同換道時滿足約束條件的主車安全軌跡集

再根據(jù)公式(6)，得到xc＝68.05m，yc＝3.51m，tc＝7.0s時對應(yīng)的軌跡為最優(yōu)軌跡，帶入到公式(1)的起始、最終狀態(tài)中，擬合求解最優(yōu)換道軌跡方程為：

對該軌跡進行動態(tài)仿真，主車可以實現(xiàn)無碰安全換道，且換道的曲線較為平緩，滿足無人駕駛環(huán)境下乘客的舒適度要求。

目標車道后車減速協(xié)同換道時：給定測試參數(shù)fd協(xié)同前速度為48km/h，協(xié)同速度為40km/h，fd與m的車頭間距為20m。帶入公式(3)得到fd的行駛軌跡，再將其帶入公式(5)，根據(jù)公式(5)及約束條件可計算生成符合要求的換道軌跡，見表2。

表2目標車道后車減速協(xié)同換道時滿足約束條件的主車安全軌跡集

再根據(jù)公式(7)，得到xc＝59.31m，yc＝3.50m，tc＝6.10s時對應(yīng)的軌跡為最優(yōu)軌跡，帶入到公式(1)的起始、最終狀態(tài)中，擬合求解最優(yōu)換道軌跡方程為：

對該軌跡進行動態(tài)仿真，主車可以實現(xiàn)無碰安全換道，且換道的曲線較為平緩，滿足無人駕駛環(huán)境下乘客的舒適度要求。