本發(fā)明涉及自動(dòng)駕駛，尤其涉及一種軌跡規(guī)劃方法。

背景技術(shù)：

1、隨著自動(dòng)駕駛技術(shù)的發(fā)展，車輛在各種復(fù)雜環(huán)境中的自主導(dǎo)航能力變得尤為重要，尤其在狹窄道路中，車輛的通行面臨著更高的挑戰(zhàn)，而路徑規(guī)劃和局部避障技術(shù)則是確保車輛安全行駛的關(guān)鍵技術(shù)之一。現(xiàn)有的路徑規(guī)劃算法，如混合a*算法，已經(jīng)能夠在大多數(shù)情況下提供有效的路徑規(guī)劃。然而，當(dāng)車輛在狹窄道路或復(fù)雜環(huán)境中被卡住時(shí)，會(huì)導(dǎo)致路徑規(guī)劃失敗，車輛無法脫困，因此現(xiàn)有的軌跡規(guī)劃技術(shù)仍存在一定的局限性，亟需一種新的軌跡規(guī)劃方法，提高車輛在狹窄道路和復(fù)雜環(huán)境中的避障和脫困能力。

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

1、本發(fā)明提供一種軌跡規(guī)劃方法，旨在解決現(xiàn)有技術(shù)中的缺陷，實(shí)現(xiàn)有效避免目前路徑規(guī)劃算法在窄道中被障礙物卡住后無法脫困的情形，提高軌跡規(guī)劃成功率。

2、為達(dá)到上述目的，本發(fā)明所采取的技術(shù)方案為：

3、本發(fā)明提供一種軌跡規(guī)劃方法，包括：

4、步驟1、確定窄道場景中邊界、障礙物和車輛位姿，建立窄道通行模型，根據(jù)所述車輛位姿建立軌跡規(guī)劃的局部坐標(biāo)系；

5、步驟2、在所述窄道通行模型中確定目標(biāo)參考線及目標(biāo)點(diǎn)；

6、步驟3、根據(jù)所述目標(biāo)點(diǎn)，基于可通行區(qū)域進(jìn)行混合a*軌跡規(guī)劃，同時(shí)計(jì)算并記錄車輛行駛過程中的軌跡，并判斷所述軌跡規(guī)劃是否失敗，是則進(jìn)入下一步，否則重復(fù)執(zhí)行本步驟；

7、步驟4、讀取記錄的歷史行車軌跡，進(jìn)行循跡追蹤回退，返回步驟2。

8、具體地，所述步驟2包括：

9、步驟201、建立車輛前方視圖，確定窄道邊界和障礙物分布；

10、步驟202、判斷窄道內(nèi)是否存在障礙物，是則以所述障礙物的中心點(diǎn)為圓心建立所述障礙物的外接圓作為第一輔助圓，并將所述障礙物對(duì)向的窄道的角點(diǎn)作為等同障礙物，并以所述等同障礙物的圓心、預(yù)設(shè)半徑建立第二輔助圓，否則將所述窄道兩側(cè)的角點(diǎn)作為等同障礙物，并以所述等同障礙物的圓心、預(yù)設(shè)半徑分別建立第一輔助圓、第二輔助圓；

11、步驟203、根據(jù)所述第一輔助圓、第二輔助圓的相對(duì)位置，確定關(guān)鍵可通過點(diǎn)及第一輔助可通過點(diǎn)、第二輔助可通過點(diǎn)；

12、步驟204、根據(jù)所述關(guān)鍵可通過點(diǎn)及第一輔助可通過點(diǎn)、第二輔助可通過點(diǎn)確定目標(biāo)圓弧，所述目標(biāo)圓弧即為目標(biāo)參考線，所述目標(biāo)圓弧上的點(diǎn)即為目標(biāo)點(diǎn)，同時(shí)獲取所述關(guān)鍵可通過點(diǎn)的切線，并將所述切線的角度作為所述目標(biāo)點(diǎn)的偏航角。

13、具體地，所述步驟203包括：若所述第一輔助圓、第二輔助圓的相對(duì)位置為相切，則按照第一預(yù)設(shè)規(guī)則確定關(guān)鍵可通過點(diǎn)以及第一輔助可通過點(diǎn)、第二輔助可通過點(diǎn)，所述第一預(yù)設(shè)規(guī)則為：

14、步驟a1、將相切點(diǎn)作為關(guān)鍵可通過點(diǎn)；

15、步驟a2、連接所述關(guān)鍵可通過點(diǎn)、第一圓心、第二圓心，生成第一輔助線；

16、所述第一圓心為第一輔助圓的圓心，所述第二圓心為第二輔助圓的圓心；

17、步驟a3、經(jīng)過所述關(guān)鍵可通過點(diǎn)，生成所述第一輔助線的垂線，記為第一垂線；

18、步驟a4、以所述關(guān)鍵可通過點(diǎn)為中心，在所述第一垂線上根據(jù)預(yù)設(shè)長度選取兩個(gè)端點(diǎn)作為初始第一輔助可通過點(diǎn)、初始第二輔助可通過點(diǎn)；

19、步驟a5、分別以所述第一輔助可通過點(diǎn)、初始第二輔助可通過點(diǎn)為中心，生成待選窗口區(qū)域；

20、步驟a6、在所述待選窗口區(qū)域中根據(jù)障礙物分布計(jì)算距離代價(jià)值，將具有最小距離代價(jià)的點(diǎn)作為第一輔助可通過點(diǎn)、第二輔助可通過點(diǎn)。

21、具體地，所述步驟203包括：若所述第一輔助圓、第二輔助圓的相對(duì)位置為相離，則按照第二預(yù)設(shè)規(guī)則確定關(guān)鍵可通過點(diǎn)以及第一輔助可通過點(diǎn)、第二輔助可通過點(diǎn)，所述第二預(yù)設(shè)規(guī)則為：

22、步驟b1、連接第一圓心、第二圓心，生成第二輔助線；

23、步驟b2、將所述第二輔助線位于兩圓之間的線段的中點(diǎn)作為關(guān)鍵可通過點(diǎn)；

24、步驟b3、經(jīng)過所述關(guān)鍵可通過點(diǎn)，生成所述第二輔助線的垂線，記為第二垂線；

25、步驟b4、以所述關(guān)鍵可通過點(diǎn)為中心，在所述第二垂線上根據(jù)預(yù)設(shè)長度選取兩個(gè)端點(diǎn)作為初始第一輔助可通過點(diǎn)、初始第二輔助可通過點(diǎn)；

26、步驟b5、分別以所述第一輔助可通過點(diǎn)、初始第二輔助可通過點(diǎn)為中心，生成待選窗口區(qū)域；

27、步驟b6、在所述待選窗口區(qū)域中根據(jù)障礙物分布計(jì)算距離代價(jià)值，將具有最小距離代價(jià)的點(diǎn)作為第一輔助可通過點(diǎn)、第二輔助可通過點(diǎn)。

28、具體地，所述步驟203包括：若所述第一輔助圓、第二輔助圓的相對(duì)位置為相交，則按照第三預(yù)設(shè)規(guī)則確定關(guān)鍵可通過點(diǎn)以及第一輔助可通過點(diǎn)、第二輔助可通過點(diǎn)，所述第三預(yù)設(shè)規(guī)則為：

29、步驟c1、將第一輔助圓、第二輔助圓的交點(diǎn)作為第一初始輔助可通過點(diǎn)、第二初始輔助可通過點(diǎn)；

30、步驟c2、連接第一輔助圓、第二輔助圓的交點(diǎn)，生成第三輔助線；

31、步驟c3、將所述第三輔助線的中點(diǎn)作為關(guān)鍵可通過點(diǎn)；

32、步驟c4、分別以所述第一輔助可通過點(diǎn)、初始第二輔助可通過點(diǎn)為中心，生成待選窗口區(qū)域；

33、步驟c5、在所述待選窗口區(qū)域中根據(jù)障礙物分布計(jì)算距離代價(jià)值，將具有最小距離代價(jià)的點(diǎn)作為第一輔助可通過點(diǎn)、第二輔助可通過點(diǎn)。

34、具體地，所述基于可通行區(qū)域進(jìn)行混合a*軌跡規(guī)劃包括：

35、步驟d1、獲取車輛當(dāng)前位置作為當(dāng)前節(jié)點(diǎn)，并擴(kuò)展周圍節(jié)點(diǎn)，得到若干個(gè)待選節(jié)點(diǎn)；

36、步驟d2、對(duì)所述待選節(jié)點(diǎn)進(jìn)行代價(jià)值計(jì)算，選擇代價(jià)值最小的節(jié)點(diǎn)作為下一個(gè)節(jié)點(diǎn)；

37、步驟d3、根據(jù)所有所述節(jié)點(diǎn)、目標(biāo)點(diǎn)，利用dubins曲線通過混合a*算法進(jìn)行路徑擴(kuò)展，獲得行車軌跡。

38、具體地，所述計(jì)算并記錄車輛行駛過程中的軌跡包括：

39、步驟e1、根據(jù)第一預(yù)設(shè)公式計(jì)算偏航角變化量；

40、步驟e2、根據(jù)所述偏航角變化量判斷車輛的行駛類型，若為直線行駛，則按照第二預(yù)設(shè)公式計(jì)算當(dāng)前時(shí)刻的車輛后軸的坐標(biāo)、偏航角，若為曲線行駛，則第三預(yù)設(shè)公式計(jì)算當(dāng)前時(shí)刻的車輛后軸的坐標(biāo)、偏航角；

41、步驟e3、記錄所有時(shí)刻車輛后軸的坐標(biāo)、偏航角。

42、具體地，所述第一預(yù)設(shè)公式為：

43、

44、其中，δc表示車輛行駛距離，δδ表示車輛前輪轉(zhuǎn)角變化量，laxies表示軸距。具體地，所述第二預(yù)設(shè)公式為：

45、x(i+1)＝x(i)+λδccos(θ(i))

46、y(i+1)＝y(tǒng)(i)+λδcsin(θ(i))

47、θ(i+1)＝θ(i)

48、其中，x(i)、y(i)表示上一時(shí)刻車輛后軸的坐標(biāo)，x(i+1)、y(i+1)表示當(dāng)前時(shí)刻車輛后軸的坐標(biāo)；θ(i)表示上一時(shí)刻的偏航角，θ(i+1)表示當(dāng)前時(shí)刻的偏航角；δc表示車輛行駛距離；λ表示車輛行駛方向，λ＝1表示前進(jìn)，λ＝-1表示后退；laxies表示軸距。

49、具體地，所述第三預(yù)設(shè)公式為：

50、

51、

52、θ(i+1)＝θ(i)+δθ

53、其中，x(i)、y(i)表示上一時(shí)刻車輛后軸的坐標(biāo)，x(i+1)、y(i+1)表示當(dāng)前時(shí)刻車輛后軸的坐標(biāo)；θ(i)表示上一時(shí)刻的偏航角，θ(i+1)表示當(dāng)前時(shí)刻的偏航角；δθ表示偏航角變化量；δδ表示車輛前輪轉(zhuǎn)角變化量；λ表示車輛行駛方向，λ＝1表示前進(jìn)，λ＝-1表示后退；laxies表示軸距。

54、本發(fā)明的有益效果在于：本發(fā)明通過獲取可通行區(qū)域信息，計(jì)算通過窄道的目標(biāo)點(diǎn)，擴(kuò)展軌跡節(jié)點(diǎn)，計(jì)算dubins曲線并實(shí)時(shí)記錄車輛軌跡，當(dāng)路徑規(guī)劃失敗后結(jié)合則結(jié)合循跡追蹤算法跟蹤歷史軌跡回退預(yù)設(shè)距離，增大避障空間，再重復(fù)上述步驟，直到找到dubins曲線/目標(biāo)點(diǎn)，成功規(guī)劃路線，有效避免了目前路徑規(guī)劃算法在窄道中被障礙物卡住后無法脫困的情形。