[發明專利]一種基于改進EM算法的局部路徑規劃方法、介質及設備

在審

• 申請號: 202110756851.9
• 申請日: 2021-07-05
• 公開（公告）號: CN113654556A
• 公開（公告）日: 2021-11-16
• 發明（設計）人: 王璐
• 申請（專利權）人: 的盧技術有限公司
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C21/34
• 代理公司: 南京蘇高專利商標事務所(普通合伙) 32204
• 代理人: 柏尚春

1.一種基于改進EM算法的局部路徑規劃方法，其特征在于，包括以下步驟：

(1)輸入參考線、障礙物檢測結果、障礙物軌跡預測結果及車輛狀態；

(2)根據障礙物生成Voronoi圖并進行點采樣；

(3)根據所有采樣點生成路徑并進行代價計算；

(4)選取代價最小的路徑作為最優路徑；

(5)根據參考線及車輛運動學約束獲取速度上限；

(6)根據車輛速度上限及障礙物生成速度并進行代價計算；

(7)選取代價最小的速度作為最優速度；

(8)根據約束條件和優化目標進行速度優化；

(9)將路徑和速度進行合成，生成軌跡。

2.根據權利要求1所述的方法，其特征在于，所述步驟(2)具體為：

(2.1)根據障礙物生成生成Voronoi圖；

(2.2)對步驟(2.1)中的Voronoi圖進行點采樣。

3.根據權利要求2所述的方法，其特征在于，所述步驟(2.1)具體為：

(2.1.1)通過將障礙物邊界形狀分割成許多小線段，可將障礙物邊界近似為大量點，這樣就獲取了表示多邊形障礙物的一組離散點；

(2.1.2)對得到的離散點進行Delaunay三角剖分，采用增量法，逐個添加頂點到圖中，并采用Quad-Edge表示三角形的邊；

(2.1.3)對得到的Delaunay三角剖分中的每個三角形的每條邊計算通道寬度；

(2.1.4)生成Voronoi圖。

4.根據權利要求2所述的方法，其特征在于，所述步驟(2.2)具體為：

(2.2.1)縱向點采樣根據車輛速度和最小采樣距離取點；

(2.2.2)橫向點采樣根據Voronoi邊的通道寬度，取通道寬度的中心點作為橫向采樣點。

5.根據權利要求3所述的方法，其特征在于，所述步驟(2.1.3)具體為：

(2.1.3.1)獲取與該邊相關聯的左邊三角形的所有的頂點，計算該三角形的外接圓的圓心，作為Voronoi邊的起始頂點；

(2.1.3.2)獲取與該邊相關聯的右邊三角形的所有頂點，計算該三角形的外接圓的圓心，作為Voronoi邊的終止頂；

(2.1.3.3)假設該Voronoi邊的起始頂點和終止頂點位于障礙物內部，就刪除該Voronoi邊，并繼續搜索Delaunay三角剖分中的其他邊；否則轉入步驟④，判斷Voronoi邊表示的通道寬度；

(2.1.3.4)選取右邊的三角形，在右三角形中，如果三個點位于不同的障礙物的邊上，則取三角形的第一個點和第二個點的連線長度Wr作為通道寬度，否則轉入步驟(2.1.3.5)；

(2.1.3.5)在右三角形中，如果有兩個點位于同一個障礙物的邊上，則計算三角形中不在同一個障礙物的點到對邊的垂直距離，取該距離為通道寬度Wr；

(2.1.3.6)左邊的三角形按照右邊的三角形執行相同的操作，得到通道寬度Wl，取W＝min(Wl,Wr)作為該Voronoi邊表示的通道的寬度。

6.根據權利要求1所述的方法，其特征在于，步驟(8)中所述約束條件具體為：

起始點函數值約束f(0)＝0；等式約束；

起始點速度約束f'(0)＝v0＝初始速度；等式約束；

單調性約束，即隨時間t增大，s＝f(t)一定是不變或增大(單調遞增)，因為車會停下或前進，不后退；不等式約束；

分界點處各階導數連續平滑性約束，這里用了0～3階導數；等式約束；

采樣時刻t對應的自車s坐標范圍約束；不等式約束；

采樣時刻t對應的自車速度范圍約束；不等式約束；

采樣時刻t對應的自車加速度范圍約束；不等式約束。

7.根據權利要求1所述的方法，其特征在于，步驟(8)中所述優化目標包括加速度、加加速度、參考線參考速度、決策速度、路徑參考速度、正則項。

8.根據權利要求1所述的方法，其特征在于，步驟(8)中采用的優化方法為qpOASES庫優化。