[發(fā)明專利]適應(yīng)未知動態(tài)空間的快速路徑規(guī)劃方法、裝置及存儲介質(zhì)

有效

• 申請?zhí)枺?/td>: 202010794708.4
• 申請日: 2020-08-10
• 公開（公告）號: CN111964682B
• 公開（公告）日: 2022-03-08
• 發(fā)明（設(shè)計）人: 羅冠辰;張鋮林;王鵬濤;王明杰
• 申請（專利權(quán)）人: 北京軒宇空間科技有限公司
• 主分類號: G01C21/20
• 分類號: G01C21/20;G05D1/02
• 代理公司: 成都誠中致達(dá)專利代理有限公司 51280
• 代理人: 曹宇杰

1.一種適應(yīng)未知動態(tài)空間的快速路徑規(guī)劃方法，其特征在于，包括步驟：

根據(jù)載具的外形尺寸約束，在柵格地圖基礎(chǔ)上建立拓展地圖；

根據(jù)拓展地圖獲取不可通行區(qū)域的凸頂點(diǎn)；

以凸頂點(diǎn)為節(jié)點(diǎn)，自適應(yīng)選擇路徑規(guī)劃獲取粗路徑；

根據(jù)局部環(huán)境特點(diǎn)，與軌跡跟隨控制相結(jié)合，對粗路徑進(jìn)行修正，快速形成平滑、最優(yōu)的局部路徑；

其中，根據(jù)拓展地圖獲取不可通行區(qū)域的凸頂點(diǎn)，包括步驟：

遍歷拓展地圖中的不可通行柵格點(diǎn)i；

不可通行柵格點(diǎn)i周圍所有的（3ⁿ-1）個柵格點(diǎn)，組成(3ⁿ-1)/2個柵格點(diǎn)對(i_j, i_-j)，其中n表示柵格地圖的維數(shù)取四舍五入后的值，點(diǎn)i_-j表示點(diǎn)i_j關(guān)于點(diǎn)i的對稱點(diǎn)；

判斷i_j和i_-j是否可通行，若對于不可通行柵格點(diǎn)i，所有(3ⁿ-1)/2個柵格點(diǎn)對(i_j, i_-j)每對都滿足至多有一個點(diǎn)是不可通行柵格點(diǎn)，將點(diǎn)i作為凸頂點(diǎn)。

2.根據(jù)權(quán)利要求1所述的適應(yīng)未知動態(tài)空間的快速路徑規(guī)劃方法，其特征在于，在柵格地圖基礎(chǔ)上建立拓展地圖，包括步驟：

遍歷柵格地圖中的不可通行柵格點(diǎn)；

將柵格地圖中不可通行柵格點(diǎn)周圍所有的半徑為r內(nèi)柵格點(diǎn)都標(biāo)記為不可通行柵格點(diǎn)，其中r表示載具轉(zhuǎn)彎半徑與單位柵格長度的比值向上取整。

3.根據(jù)權(quán)利要求1所述的適應(yīng)未知動態(tài)空間的快速路徑規(guī)劃方法，其特征在于，柵格地圖為2維的平面投影圖，或3維的空間占有圖，或2.5維的平面投影+地形跟隨標(biāo)高地圖。

4.根據(jù)權(quán)利要求3所述的適應(yīng)未知動態(tài)空間的快速路徑規(guī)劃方法，其特征在于，柵格地圖為可修整的柵格地圖，所述修整是指通過地圖修整算法進(jìn)行新地圖的修改或局部地圖的疊加，或通過人為選定方式添加、改變或刪除地圖中的通行區(qū)域、未知區(qū)域、不可通行區(qū)域。

5.根據(jù)權(quán)利要求1所述的適應(yīng)未知動態(tài)空間的快速路徑規(guī)劃方法，其特征在于，若需要判別的不可通行柵格點(diǎn)在拓展地圖邊界，則將柵格點(diǎn)對中超出邊界的點(diǎn)視為通行點(diǎn)。

6.根據(jù)權(quán)利要求1所述的適應(yīng)未知動態(tài)空間的快速路徑規(guī)劃方法，其特征在于，以凸頂點(diǎn)為節(jié)點(diǎn)，自適應(yīng)選擇路徑規(guī)劃獲取粗路徑時：

路徑規(guī)劃算法根據(jù)局部地圖大小、全局地圖大小、兩種地圖大小之比，自適應(yīng)選擇全局或局部規(guī)劃進(jìn)行實(shí)時在線規(guī)劃；

對于備選的每種路徑規(guī)劃算法，代價函數(shù)采用廣義距離和/或其他指標(biāo)的單項(xiàng)指標(biāo)或多項(xiàng)加權(quán)指標(biāo)進(jìn)行優(yōu)化；

所述廣義距離，在2維或3維地圖中是指歐氏直線距離；在2.5維平面投影+地形跟隨標(biāo)高地圖中是指平面距離復(fù)合高度爬升指標(biāo)；

所述其他指標(biāo)，根據(jù)所載荷傳感器的不同選擇不同的記錄參數(shù)進(jìn)行綜合，包括時間、能量消耗、威脅暴露概率、交通流量、任務(wù)完成評價中的一項(xiàng)指標(biāo)或多項(xiàng)指標(biāo)加權(quán)。

7.根據(jù)權(quán)利要求1所述的適應(yīng)未知動態(tài)空間的快速路徑規(guī)劃方法，其特征在于，對粗路徑進(jìn)行修正，以路徑跟隨控制作為內(nèi)回路引入局部路徑修正優(yōu)化，快速修正得到局部路徑，并形成最優(yōu)控制：通過引入人工勢場，以當(dāng)前目標(biāo)粗路徑點(diǎn)求取引力、以閾值范圍內(nèi)特定點(diǎn)求取斥力；以局部引力和斥力、載具轉(zhuǎn)彎半徑、速度限制作為約束，采用最優(yōu)控制直接解算載具運(yùn)動指令。