[發(fā)明專利]一種基于lanelet框架的全局路徑規(guī)劃切割方法和系統(tǒng)

有效

• 申請(qǐng)?zhí)枺?/td>: 202110307663.8
• 申請(qǐng)日: 2021-03-23
• 公開（公告）號(hào): CN113063423B
• 公開（公告）日: 2023-01-10
• 發(fā)明（設(shè)計(jì)）人: 夏燦銘;吳元清;何子俊;盧泳康
• 申請(qǐng)（專利權(quán)）人: 廣東工業(yè)大學(xué)
• 主分類號(hào): G01C21/20
• 分類號(hào): G01C21/20;G01C21/34;G05D1/00
• 代理公司: 廣州粵高專利商標(biāo)代理有限公司 44102
• 代理人: 林麗明

1.一種基于lanelet框架的全局路徑規(guī)劃切割方法，其特征在于，包括以下步驟：

S1：基于lanelet框架下，將地圖轉(zhuǎn)換為由一段一段的lanelet拼接而成的地圖；

S2：根據(jù)定義的起點(diǎn)、終點(diǎn)和道路的行駛方向，利用Dijkstra算法進(jìn)行路徑搜索，得到初步的路徑；

S3：使起點(diǎn)和終點(diǎn)所在的lanelet的中心線上隨機(jī)分布若干個(gè)路徑點(diǎn)，且每一段lanelet的第一個(gè)路徑點(diǎn)和最后一個(gè)路徑點(diǎn)均在lanelet的邊界處；

S4：獲取距離起點(diǎn)和終點(diǎn)最近的路徑點(diǎn)；

S5：按照行駛方向，將起點(diǎn)后面的路徑點(diǎn)和終點(diǎn)前面的路徑點(diǎn)刪除，剩下的路徑點(diǎn)組成起點(diǎn)和終點(diǎn)所在的lanelet的路徑規(guī)劃；

S6：綜合S2得到的初步路徑和S5得到的路徑規(guī)劃，得到最終的路徑規(guī)劃；

步驟S4中，若最近的路徑點(diǎn)p2不是lanelet的第一個(gè)路徑點(diǎn)和最后一個(gè)路徑點(diǎn)，則還需要考慮前后兩個(gè)路徑點(diǎn)，總共連著三個(gè)路徑點(diǎn)，按順序設(shè)為p1,p2,p3：

計(jì)算起點(diǎn)或終點(diǎn)距離p1與p2的中垂線l1的距離d2、起點(diǎn)或終點(diǎn)距離p2與p3的中垂線l2的距離d1、路徑點(diǎn)p2到達(dá)l2的距離k1；

當(dāng)滿足以下條件時(shí)：

則判斷為起點(diǎn)或終點(diǎn)在最近路徑點(diǎn)p2的前方；

當(dāng)滿足以下條件時(shí)：

則判斷為起點(diǎn)或終點(diǎn)在最近路徑點(diǎn)p2的后方。

2.根據(jù)權(quán)利要求1所述的基于lanelet框架的全局路徑規(guī)劃切割方法，其特征在于，所述利用Dijkstra算法進(jìn)行路徑搜索，得到初步的路徑，具體為：

S2.1：對(duì)每一段lanelet進(jìn)行編號(hào)，將lanelet轉(zhuǎn)化為圖的搜索格式，原來每段Lanelet上的中心點(diǎn)之間的距離確認(rèn)邊權(quán)值，距離越長(zhǎng)，相鄰圖標(biāo)的邊權(quán)值越大；

S2.2：將搜索圖上的起點(diǎn)所在的初始點(diǎn)看作一個(gè)集合S，其它點(diǎn)看作另一個(gè)集合；

S2.3：根據(jù)初始點(diǎn)，求出其它點(diǎn)到初始點(diǎn)的距離d[i]；

S2.4：選取最小的d[i]，記為d[x]，并將此d[i]邊對(duì)應(yīng)的點(diǎn)，記為x，加入集合S；

S2.5：再根據(jù)x，更新跟x相鄰點(diǎn)y的d[y]值；

S2.6：重復(fù)S2.4至S2.5步驟，直到終點(diǎn)所在的點(diǎn)也加入了集合，此時(shí)集合S即為初步的路徑。

3.根據(jù)權(quán)利要求2所述的基于lanelet框架的全局路徑規(guī)劃切割方法，其特征在于，步驟S2.3中其它點(diǎn)到初始點(diǎn)的距離d[i]，具體定義為：

若其它點(diǎn)與初始點(diǎn)相鄰，則d[i]為邊權(quán)值；若不相鄰，則d[i]為無限大。

4.根據(jù)權(quán)利要求3所述的基于lanelet框架的全局路徑規(guī)劃切割方法，其特征在于，步驟S2.5中更新跟x相鄰點(diǎn)y的d[y]值，具體為：

d[y]＝min{d[y],d[x]+w[x][y]}

式中，w[x][y]為x,y兩點(diǎn)之間的邊權(quán)值。

5.根據(jù)權(quán)利要求4所述的基于lanelet框架的全局路徑規(guī)劃切割方法，其特征在于，步驟S4中根據(jù)歐式距離的大小判斷距離起點(diǎn)和終點(diǎn)最近的路徑點(diǎn)，令起點(diǎn)坐標(biāo)為(X,Y)，路徑點(diǎn)坐標(biāo)為(a,b)，則距離公式有

遍歷lanelet中的每個(gè)路徑點(diǎn)，獲取離起點(diǎn)和終點(diǎn)最近的路徑點(diǎn)。

6.根據(jù)權(quán)利要求1所述的基于lanelet框架的全局路徑規(guī)劃切割方法，其特征在于，步驟S5中，當(dāng)最近的路徑點(diǎn)p2不是lanelet的第一個(gè)路徑點(diǎn)和最后一個(gè)路徑點(diǎn)且判斷起點(diǎn)或終點(diǎn)在最近路徑點(diǎn)p2的前方，則將最近點(diǎn)及其之前的路徑點(diǎn)都刪除，將起點(diǎn)作為第一個(gè)路徑點(diǎn)，把終點(diǎn)之后的路徑點(diǎn)都刪除，不包含最近路徑點(diǎn)，將終點(diǎn)作為最后一個(gè)路徑點(diǎn)；

當(dāng)最近的路徑點(diǎn)p2是第一個(gè)路徑點(diǎn)而不是最后一個(gè)路徑點(diǎn)的時(shí)候,對(duì)于起點(diǎn)，直接刪除第一個(gè)路徑點(diǎn)，并將起點(diǎn)作為第一個(gè)點(diǎn)；對(duì)于終點(diǎn)，刪除在終點(diǎn)之后的路徑點(diǎn)，不包含最近路徑點(diǎn)，并將終點(diǎn)作為最后一個(gè)點(diǎn)；

當(dāng)最近的路徑點(diǎn)p2不是第一個(gè)路徑點(diǎn)而是最后一個(gè)路徑點(diǎn)的時(shí)候,對(duì)于起點(diǎn)，直接刪除在起點(diǎn)之前的路徑點(diǎn)，不包含最近路徑點(diǎn)，并將起點(diǎn)作為第一個(gè)點(diǎn)；對(duì)于終點(diǎn)，直接刪除最后一個(gè)路徑點(diǎn)，并將終點(diǎn)作為最后一個(gè)點(diǎn)。