本發(fā)明屬于汽車軌跡規(guī)劃，具體涉及到一種基于多層可通行空間的軌跡規(guī)劃方法。

背景技術(shù)：

1、自動駕駛作為推動港口發(fā)展的關(guān)鍵，其應(yīng)用和普及能顯著提高港口運營效率并降低成本。然而，港口眾多的狹窄場景，如過鎖扭站、過換電站也給自動駕駛軌跡規(guī)劃帶來了諸多挑戰(zhàn)。

2、對于障礙物，傳統(tǒng)的軌跡規(guī)劃只考慮障礙物的二維平面信息，而忽略了z軸高度信息。具體來說，將三維立體直接投影到二維平面。由于這種方案損失了障礙物的高度信息，因此投影時會保守處理，得到所有高度上最大的二維投影面積，這會造成通行空間的浪費，使得一些極限狹窄場景無法通過。

3、以車輛穿過兩側(cè)鎖扭站的場景進(jìn)行說明，通常兩側(cè)鎖扭站之間的寬度從高到低是不一致的，下半部分較窄(3200mm)，上半部分較寬(3700mm)，整車的寬度也不是統(tǒng)一的，如承裝平臺最寬，底盤次之，集裝箱最窄。按傳統(tǒng)方法，通行空間、車輛寬度按同一寬度處理，出于安全考慮，通行寬度取最窄處(3200mm)，車輛寬度按最寬處取(3500mm)，顯然車輛寬度大于通行寬度，最終得出了車輛無法通行的結(jié)論，而沒有對障礙物、車輛做精細(xì)化的考慮，忽略了不同高度的寬度不一致這個關(guān)健信息，在實際上存在通行空間的情況下得出了無法通行的錯誤結(jié)論。

4、除了鎖扭站外，港口典型的窄道場景還包括：換電站、兩側(cè)低矮柵欄等，這些都是自動駕駛車輛在港口作業(yè)過程中必不可少的通行環(huán)節(jié)。因此，順利、高效通過窄道空間對自動駕駛車輛，特別是寬體自動駕駛車輛，具有重要意義，有利于提升全場景通過性。

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

1、針對上述問題，本發(fā)明的主要目的在于設(shè)計一種基于多層可通行空間的軌跡規(guī)劃方法，融合障礙物三維信息的多層可行域解析，得到精細(xì)化的軌跡，解決狹窄場景下自動駕駛的通行問題。

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

3、一種基于多層可通行空間的軌跡規(guī)劃方法，包括如下步驟：

4、步驟1：將車輛與障礙物進(jìn)行分層，并使用線條表征障礙物的外輪廓，得到障礙物集合；

5、步驟2：針對多層可通行域解析，計算車輛的可行駛范圍，包括如下過程：

6、步驟2.1：根據(jù)地圖參考線進(jìn)行采樣，得到采樣序列；

7、步驟2.2：計算每個采樣點到兩側(cè)線條的距離；

8、步驟2.3：遍歷完采樣序列的所有采樣點，得到每一層的可行域；

9、步驟3：建立軌跡規(guī)劃模型，并求解優(yōu)化軌跡。

10、作為本發(fā)明進(jìn)一步的描述，所述步驟1中，每個線條line包括的信息為：編號，線條起點及終點的x、y、z軸坐標(biāo)信息，線條起點及終點在frenet坐標(biāo)下的sl信息，線條的繞行方向。

11、作為本發(fā)明進(jìn)一步的描述，根據(jù)車輛的外部結(jié)構(gòu)從低到高進(jìn)行高度分層，障礙物與車輛的高度分層一致；

12、障礙物每層包含若干線條，即layer0＝l{ine?0l,ine?l1i,ne?2...}，障礙物的線條組成障礙物集合mdb＝{layer?0l,ayer?l1a,yer?2…}。

13、作為本發(fā)明進(jìn)一步的描述，所述步驟2.1中，沿著參考線按一定分辨率delta_s＝0.2m采樣，得到采樣序列s＝{s0,s1,s2...}。

14、作為本發(fā)明進(jìn)一步的描述，所述步驟2.3中，綜合每一層的可行域，得到車輛的最終可行域，最終可行域包括硬邊界、軟邊界，表達(dá)式為：

15、barrier_set＝{s0,l_lower0,l_upper0,s1,l_lower1,l_upper1...}；

16、soft_set＝{s0,l_lower0,l_upper0,s1,l_lower1,l_upper1...}；

17、其中，上述barrier為硬邊界，soft為軟邊界，下標(biāo)的數(shù)字代表采樣點的排序，l_lower0,l_upper0為第0個采樣點的可行域的右邊界和左邊界；

18、對于任意的采樣點，通過對其s進(jìn)行插值，可得到對應(yīng)s的軟邊界與硬邊界。

19、作為本發(fā)明進(jìn)一步的描述，所述步驟3中，建立軌跡規(guī)劃模型包括如下步驟：

20、步驟3.1：根據(jù)車輛運動學(xué)特性，通過狀態(tài)量x＝[x,y,theta,phif,phir]，構(gòu)建狀態(tài)轉(zhuǎn)移方程，表達(dá)式為：

21、

22、其中，x,y表示車體中心坐標(biāo)，theta表示車體朝向，phif,phir分別表示車體前后輪轉(zhuǎn)角；phi＝atan((tan(phif)+tan(phir))/2)，表示車輛實際行駛方向與車頭朝向的角度，kappa＝(tan(phif)-tan(phir))/wheelbase，表示由車輛前后輪轉(zhuǎn)角計算得到的曲率，wheelbase表示車輛軸距；

23、步驟3.2：設(shè)計代價函數(shù)；

24、根據(jù)參考線上的軌跡點對應(yīng)信息(xr,yr,thetar)，得到在frenet下的橫向偏移為：l＝-sin(thetar)*(x-xr)+cos(thetar)*(y-yr)；

25、其中，xr,yr表示車體中心坐標(biāo)的參考值，thetar表示車體朝向的參考值；

26、偏離參考線代價：0.5*ref_weight*l*l+0.5*theta_weight*(theta-thetar)*(theta-thetar)；

27、其中，weight表示對應(yīng)變量的權(quán)重；ref_weight表示為參考線權(quán)重，theta_weight表示為車體朝向權(quán)重；

28、曲率：

29、0.5*kappa_weight*kappa*kappa；

30、其中，kappa_weight表示為曲率權(quán)重；

31、曲率變化率：

32、0.5*dphif_weight*dphif*dphif+dphir_weight*dphir*dphir；

33、其中，dphif_weight表示為前輪轉(zhuǎn)角變化率的權(quán)重，dphir_weight表示為后輪轉(zhuǎn)角變化率的權(quán)重，dphif表示為前輪轉(zhuǎn)角變化率，dphir表示為后輪轉(zhuǎn)角變化率；

34、步驟3.3：約束設(shè)計；

35、l_lower≤l≤l_upper；

36、sl_lower≤sl≤sl_upper；

37、phif_lower≤phif≤phif_upper；

38、phir_lower≤phir≤phir_upper；

39、dphif_lower≤dphif≤dphif_upper；

40、dphir_lower≤dphir≤dphir_upper；

41、其中，l_lower、l_upper分別表示采樣點可行域的右邊界和左邊界，sl、sl_lower、sl_upper分別表示采樣點可行域軟邊界、軟邊下界、軟邊上界，phif_lower、phif_upper分別表示車體前輪轉(zhuǎn)角的下界和上界，phir_lower、phir_upper分別表示車體后輪轉(zhuǎn)角的右邊界和左邊界，dphif_lower、dphif_upper分別表示前輪轉(zhuǎn)角的下界和上界，dphir_lower、dphir_upper分別表示前輪轉(zhuǎn)角變化率的下界和上界。

42、作為本發(fā)明進(jìn)一步的描述，所述步驟3中，通過求解器求解軌跡規(guī)劃模型，得到可通行空間的軌跡。

43、相對于現(xiàn)有技術(shù)，本發(fā)明的技術(shù)效果為：

44、本發(fā)明提供了一種基于多層可通行空間的軌跡規(guī)劃方法，通過將障礙物與車輛進(jìn)行分層，并對分層求解后的可行域信息進(jìn)行融合，得到最終的可通行空間，結(jié)合使用mpc進(jìn)行規(guī)劃問題建模、求解，得到規(guī)劃軌跡；該方法融合三維信息的障礙物表征方法，實現(xiàn)可通行空間的分層精細(xì)化建模；對帶有高度信息的多層障礙物進(jìn)行分層通行空間解析，得到精細(xì)化的多層可通行域，提升狹窄場景下的通過性。