本發(fā)明涉及自動(dòng)駕駛，更具體的說(shuō)，涉及一種基于相對(duì)定位信息的局部軌跡規(guī)劃方法。

背景技術(shù)：

1、高級(jí)駕駛輔助系統(tǒng)(advanced?driving?assistance?system)是利用安裝在車上的各式各樣傳感器(毫米波雷達(dá)、激光雷達(dá)、單\雙目攝像頭以及衛(wèi)星導(dǎo)航)，在汽車行駛過(guò)程中隨時(shí)來(lái)感應(yīng)周圍的環(huán)境，收集數(shù)據(jù)，進(jìn)行靜態(tài)、動(dòng)態(tài)物體的辨識(shí)、偵測(cè)與追蹤，并結(jié)合導(dǎo)航地圖數(shù)據(jù)，進(jìn)行系統(tǒng)的運(yùn)算與分析，從而預(yù)先讓駕駛者察覺(jué)到可能發(fā)生的危險(xiǎn)，有效增加汽車駕駛的舒適性和安全性。

2、高級(jí)駕駛輔助系統(tǒng)因成本限制，往往僅能通過(guò)識(shí)別車道線獲取局部相對(duì)定位信息。基于相對(duì)定位信息的軌跡規(guī)劃通常在車輛坐標(biāo)系(車輛直角坐標(biāo)系)下進(jìn)行，通常只能適應(yīng)直線工況。圖1揭示了基于車輛坐標(biāo)系的網(wǎng)格采樣規(guī)劃效果示意圖，如圖1所示，在大曲率參考線工況下，傳統(tǒng)基于車輛坐標(biāo)系的規(guī)劃方法其規(guī)劃路徑可能超出可行駛區(qū)域。

3、基于相對(duì)定位信息在車輛坐標(biāo)系下進(jìn)行規(guī)劃的具體方法，例如，于宏嘯、段建民的文獻(xiàn)《關(guān)于車道線圖像識(shí)別軌跡規(guī)劃跟蹤仿真》中有所描述。

4、選擇車輛坐標(biāo)系下的若干個(gè)終端狀態(tài)并進(jìn)行多項(xiàng)式擬合獲得路徑集，通過(guò)目標(biāo)函數(shù)獲取最優(yōu)路徑，但車輛坐標(biāo)系下終端狀態(tài)值的采樣不適用于曲率較大的工況。

5、在曲線工況下，出于駕駛安全性和舒適性考慮，目前亟需一種基于相對(duì)定位信息的局部軌跡規(guī)劃方法，滿足曲線工況下的最優(yōu)路徑規(guī)劃。

技術(shù)實(shí)現(xiàn)思路

1、本發(fā)明的目的是提供一種基于相對(duì)定位信息的局部軌跡規(guī)劃方法，解決現(xiàn)有技術(shù)的軌跡規(guī)劃不適用于曲線工況的問(wèn)題。

2、為了實(shí)現(xiàn)上述目的，本發(fā)明提供了一種基于相對(duì)定位信息的局部軌跡規(guī)劃方法，包括以下步驟：

3、識(shí)別車道線獲取參考線的相對(duì)定位信息；

4、基于相對(duì)定位信息進(jìn)行軌跡規(guī)劃；

5、其中，所述軌跡規(guī)劃包括路徑規(guī)劃和速度規(guī)劃；

6、所述路徑規(guī)劃，采用車輛曲線坐標(biāo)系獲得無(wú)碰撞最優(yōu)路徑，所述車輛曲線坐標(biāo)系，是描述規(guī)劃路徑點(diǎn)與參考線相對(duì)位置的坐標(biāo)系；

7、所述速度規(guī)劃，采用自適應(yīng)最大規(guī)劃速度的梯形速度規(guī)劃獲得規(guī)劃速度曲線。

8、在一實(shí)施例中，所述路徑規(guī)劃，進(jìn)一步包括：

9、將參考線的相對(duì)定位信息從車輛坐標(biāo)系轉(zhuǎn)換至車輛曲線坐標(biāo)系；

10、對(duì)定位信息進(jìn)行網(wǎng)格采樣，生成車輛曲線坐標(biāo)系下的有限路徑集；

11、將車輛曲線坐標(biāo)系下的有限路徑集進(jìn)行坐標(biāo)轉(zhuǎn)換，生成車輛坐標(biāo)系下的候選路徑集；

12、對(duì)候選路徑進(jìn)行篩選，獲取無(wú)碰撞最優(yōu)路徑。

13、在一實(shí)施例中，所述車輛曲線坐標(biāo)系與車輛坐標(biāo)系之間的轉(zhuǎn)換公式，進(jìn)一步包括以下表達(dá)式：

14、

15、其中，xx為車輛坐標(biāo)系中規(guī)劃路徑點(diǎn)的橫坐標(biāo)；

16、yx為車輛坐標(biāo)系中規(guī)劃路徑點(diǎn)的縱坐標(biāo)；

17、θx為車輛坐標(biāo)系中規(guī)劃路徑點(diǎn)的航向角；

18、xr為車輛坐標(biāo)系中規(guī)劃路徑點(diǎn)投影在參考線上的橫坐標(biāo)；

19、yr為車輛坐標(biāo)系中規(guī)劃路徑點(diǎn)投影在參考線上的縱坐標(biāo)；

20、θr為投影點(diǎn)的朝向；

21、kr為投影點(diǎn)的曲率；

22、d為車輛曲線坐標(biāo)系下的橫坐標(biāo)；

23、為車輛曲線坐標(biāo)系下橫坐標(biāo)d對(duì)縱坐標(biāo)s的導(dǎo)數(shù)。

24、在一實(shí)施例中，所述對(duì)定位信息進(jìn)行網(wǎng)格采樣，生成車輛曲線坐標(biāo)系下的有限路徑集，進(jìn)一步包括：

25、對(duì)一個(gè)規(guī)劃周期內(nèi)縱坐標(biāo)方向上的采樣距離s1和路徑方程的終點(diǎn)狀態(tài)進(jìn)行采樣并限制取值范圍。

26、在一實(shí)施例中，所述對(duì)候選路徑進(jìn)行篩選，獲取無(wú)碰撞最優(yōu)路徑，進(jìn)一步包括：

27、構(gòu)建評(píng)價(jià)函數(shù)計(jì)算候選路徑的評(píng)價(jià)函數(shù)值，并根據(jù)評(píng)價(jià)函數(shù)值對(duì)候選路徑集進(jìn)行排序并篩選，獲取無(wú)碰撞最優(yōu)路徑。

28、在一實(shí)施例中，所述評(píng)價(jià)函數(shù)，對(duì)應(yīng)的表達(dá)式如下：

29、

30、其中，kj、kd和ks為常數(shù)系數(shù)；

31、kjjs表示優(yōu)先選取jerk值低的路徑；

32、表示優(yōu)先選取路徑終點(diǎn)偏離參考線小的路徑；

33、kss1表示優(yōu)先選擇較短路徑。

34、在一實(shí)施例中，將評(píng)價(jià)函數(shù)值最低的候選路徑為最優(yōu)路徑，對(duì)最優(yōu)路徑進(jìn)行碰撞檢測(cè)：

35、如果檢測(cè)通過(guò)，則最優(yōu)路徑為無(wú)碰撞最優(yōu)路徑；

36、如果檢測(cè)不通過(guò)，則對(duì)評(píng)價(jià)函數(shù)值排序次低的候選路徑進(jìn)行檢測(cè)，以此類推直到篩選出通過(guò)檢測(cè)的候選路徑，作為無(wú)碰撞最優(yōu)路徑。

37、在一實(shí)施例中，所述碰撞檢測(cè)方法采用軸向包圍盒檢測(cè)方法與方向包圍盒檢測(cè)方法。

38、在一實(shí)施例中，在進(jìn)行碰撞檢測(cè)時(shí)，將每一個(gè)路徑點(diǎn)根據(jù)車輛尺寸進(jìn)行膨化處理，得到規(guī)劃點(diǎn)矩形，采用障礙物矩形表示障礙物；

39、將規(guī)劃點(diǎn)矩形與障礙物矩形的長(zhǎng)和寬分別增加安全冗余長(zhǎng)度lsafe，再進(jìn)行碰撞檢測(cè)。

40、在一實(shí)施例中，所述路徑規(guī)劃，進(jìn)一步包括：

41、基于車輛當(dāng)前行駛速度與規(guī)劃周期，確定一個(gè)規(guī)劃周期內(nèi)車輛行駛距離，對(duì)新規(guī)劃周期內(nèi)的初始狀態(tài)進(jìn)行更新。

42、在一實(shí)施例中，所述速度規(guī)劃，進(jìn)一步包括：

43、計(jì)算最大規(guī)劃速度；

44、采用改進(jìn)梯形速度規(guī)劃算法，得到梯形速度曲線；

45、采用梯度下降算法，對(duì)梯形速度曲線進(jìn)行平滑處理，得到平滑速度曲線。

46、在一實(shí)施例中，所述計(jì)算最大規(guī)劃速度，進(jìn)一步包括以下步驟：

47、以無(wú)碰撞最優(yōu)路徑終點(diǎn)的曲率和橫向偏移，計(jì)算虛擬最大速度；

48、確定目標(biāo)點(diǎn)的縱向坐標(biāo)值；

49、根據(jù)無(wú)碰撞最優(yōu)路徑，得到目標(biāo)點(diǎn)曲率和橫向偏差；

50、根據(jù)目標(biāo)點(diǎn)參數(shù)，計(jì)算最大規(guī)劃速度。

51、在一實(shí)施例中，所述采用改進(jìn)梯形速度規(guī)劃算法，得到梯形速度曲線，進(jìn)一步包括以下步驟：

52、將規(guī)劃路徑分為加速段sa、勻速段sm、減速段sd、停止段和跟車段se；

53、計(jì)算虛擬中間速度vv；

54、根據(jù)當(dāng)前速度vc、最大規(guī)劃速度vm、目標(biāo)速度vt以及虛擬中間速度vv之間的大小關(guān)系，得出路徑長(zhǎng)度為s的路徑點(diǎn)所對(duì)應(yīng)的速度，得到梯形速度曲線。

55、在一實(shí)施例中，采用梯度下降算法對(duì)速度曲線進(jìn)行平滑處理，對(duì)應(yīng)的優(yōu)化目標(biāo)函數(shù)為：

56、

57、其中，α、β為常數(shù)系數(shù)；

58、vi、vi0為路徑點(diǎn)i迭代優(yōu)化后的速度和優(yōu)化前的初始速度且δvi＝vi-vi-1；

59、n為可執(zhí)行路徑離散后的路徑點(diǎn)個(gè)數(shù)。

60、本發(fā)明提供一種基于相對(duì)定位信息的局部軌跡規(guī)劃方法，能夠在具有彎道的局部相對(duì)定位場(chǎng)景下平穩(wěn)糾偏、避障和跟車行駛，增加算法的適用場(chǎng)景，提升駕駛的舒適性和安全性。

技術(shù)特征：

1.一種基于相對(duì)定位信息的局部軌跡規(guī)劃方法，其特征在于，包括以下步驟：

2.根據(jù)權(quán)利要求1所述的基于相對(duì)定位信息的局部軌跡規(guī)劃方法，其特征在于，所述路徑規(guī)劃，進(jìn)一步包括：

3.根據(jù)權(quán)利要求2所述的基于相對(duì)定位信息的局部軌跡規(guī)劃方法，其特征在于，所述車輛曲線坐標(biāo)系與車輛坐標(biāo)系之間的轉(zhuǎn)換公式，進(jìn)一步包括以下表達(dá)式：

4.根據(jù)權(quán)利要求2所述的基于相對(duì)定位信息的局部軌跡規(guī)劃方法，其特征在于，所述對(duì)定位信息進(jìn)行網(wǎng)格采樣，生成車輛曲線坐標(biāo)系下的有限路徑集，進(jìn)一步包括：

5.根據(jù)權(quán)利要求2所述的基于相對(duì)定位信息的局部軌跡規(guī)劃方法，其特征在于，所述對(duì)候選路徑進(jìn)行篩選，獲取無(wú)碰撞最優(yōu)路徑，進(jìn)一步包括：

6.根據(jù)權(quán)利要求5所述的基于相對(duì)定位信息的局部軌跡規(guī)劃方法，其特征在于，所述評(píng)價(jià)函數(shù)，對(duì)應(yīng)的表達(dá)式如下：

7.根據(jù)權(quán)利要求5所述的基于相對(duì)定位信息的局部軌跡規(guī)劃方法，其特征在于，將評(píng)價(jià)函數(shù)值最低的候選路徑為最優(yōu)路徑，對(duì)最優(yōu)路徑進(jìn)行碰撞檢測(cè)：

8.根據(jù)權(quán)利要求7所述的基于相對(duì)定位信息的局部軌跡規(guī)劃方法，其特征在于，所述碰撞檢測(cè)方法采用軸向包圍盒檢測(cè)方法與方向包圍盒檢測(cè)方法。

9.根據(jù)權(quán)利要求7所述的基于相對(duì)定位信息的局部軌跡規(guī)劃方法，其特征在于，在進(jìn)行碰撞檢測(cè)時(shí)，將每一個(gè)路徑點(diǎn)根據(jù)車輛尺寸進(jìn)行膨化處理，得到規(guī)劃點(diǎn)矩形，采用障礙物矩形表示障礙物；

10.根據(jù)權(quán)利要求2所述的基于相對(duì)定位信息的局部軌跡規(guī)劃方法，其特征在于，所述路徑規(guī)劃，進(jìn)一步包括：

11.根據(jù)權(quán)利要求1所述的基于相對(duì)定位信息的局部軌跡規(guī)劃方法，其特征在于，所述速度規(guī)劃，進(jìn)一步包括：

12.根據(jù)權(quán)利要求11所述的基于相對(duì)定位信息的局部軌跡規(guī)劃方法，其特征在于，所述計(jì)算最大規(guī)劃速度，進(jìn)一步包括以下步驟：

13.根據(jù)權(quán)利要求11所述的基于相對(duì)定位信息的局部軌跡規(guī)劃方法，其特征在于，所述采用改進(jìn)梯形速度規(guī)劃算法，得到梯形速度曲線，進(jìn)一步包括以下步驟：

14.根據(jù)權(quán)利要求11所述的基于相對(duì)定位信息的局部軌跡規(guī)劃方法，其特征在于，采用梯度下降算法對(duì)速度曲線進(jìn)行平滑處理，對(duì)應(yīng)的優(yōu)化目標(biāo)函數(shù)為：

技術(shù)總結(jié)
本發(fā)明涉及自動(dòng)駕駛技術(shù)領(lǐng)域，更具體的說(shuō)，涉及一種基于相對(duì)定位信息的局部軌跡規(guī)劃方法。本發(fā)明提出的基于相對(duì)定位信息的局部軌跡規(guī)劃方法，包括以下步驟：識(shí)別車道線獲取參考線的相對(duì)定位信息；基于相對(duì)定位信息進(jìn)行軌跡規(guī)劃；其中，所述軌跡規(guī)劃包括路徑規(guī)劃和速度規(guī)劃；所述路徑規(guī)劃，采用車輛曲線坐標(biāo)系，獲得無(wú)碰撞最優(yōu)路徑，所述車輛曲線坐標(biāo)系，是描述規(guī)劃路徑點(diǎn)與參考線相對(duì)位置的坐標(biāo)系；所述速度規(guī)劃，采用自適應(yīng)最大規(guī)劃速度的梯形速度規(guī)劃算法，獲得規(guī)劃速度曲線。本發(fā)明能夠在具有彎道的局部相對(duì)定位場(chǎng)景下平穩(wěn)糾偏、避障和跟車行駛，增加算法的適用場(chǎng)景，提升駕駛的舒適性和安全性。

技術(shù)研發(fā)人員：羅佳祥,袁希文,黃強(qiáng),張新銳,黃瑞鵬,黃治坤
受保護(hù)的技術(shù)使用者：中車株洲電力機(jī)車研究所有限公司
技術(shù)研發(fā)日：
技術(shù)公布日：2024/10/21