[發明專利]一種基于SDF地圖的無人機三維路徑快速規劃方法

有效

• 申請號: 202011385621.8
• 申請日: 2020-12-01
• 公開（公告）號: CN112327931B
• 公開（公告）日: 2022-11-29
• 發明（設計）人: 趙滿超;王志敏;佘換林
• 申請（專利權）人: 天津基點科技有限公司
• 主分類號: G05D1/10
• 分類號: G05D1/10;G01C21/20
• 代理公司: 天津創展知識產權代理事務所(普通合伙) 12261
• 代理人: 趙曉輝

1.一種基于SDF地圖的無人機三維路徑快速規劃方法，其特征在于，具體步驟如下：

步驟一，建立待擴展列表和已擴展列表，將起始點加入待擴展列表中；

步驟二，判斷待擴展列表是否為空，若是則結束，若否則進入步驟三；

步驟三，取出待擴展列表中評估函數f值最小的點p，判斷p點是否為目標點，若是則結束，若否則進入步驟四；

步驟四，以歐幾里得符號距離場體素大小的單倍步長，判斷點p與其父節點之間的路徑是否存在碰撞，若否則將當前起始點設置為點p的父節點，并將點p設置為新的起始點，進入快速探索；若是則需判斷點p的父節點集合是否為空，若是則將點p加入已擴展列表并跳到步驟二，若否則以父節點集合中剩余的父節點中評估函數f值最小的父節點為路徑碰撞檢測的參考點，再次進入步驟四；

快速探索的具體步驟為：

步驟A，將點p拷貝一份命名為p2；

步驟B，以歐幾里得符號距離場體素大小的n倍步長對p2的全部鄰居節點進行快速探索找出p2的f值最小的鄰居節點，以歐幾里得符號距離場體素大小的單倍步長，判斷該鄰居節點與p2之間的路徑是否存在碰撞，若是則進入步驟五，若否則進入步驟C；

步驟C，判斷該鄰居節點是否在已擴展集合中，若是則進入步驟D，若否則將其加入待擴展列表并進入步驟G；

步驟D，判斷該鄰居節點的其他父節點是否全碰撞，若是則進入步驟E，若否則進入步驟F；

步驟E，將p2加入該鄰居節點的父節點集合當中，重新計算該鄰居節點的f值，并將該鄰居節點加入待擴展列表中，進入步驟F；

步驟F，判斷該鄰居節點當前計算的g值是否小于其原有g值，若是則將該鄰居節點的g值和f值更新為當前計算的g值和f值，并將其父節點更新為p2，之后進入步驟G，若否則直接進入步驟G；

步驟G，將該鄰居節點作為新的p2，進入步驟B；

步驟五，以歐幾里得符號距離場體素大小的n倍步長對當前起始點的全部未擴展的鄰居節點進行擴展，n∈[2,10]，n為整數，對當前起始點周圍未擴展的鄰居節點進行碰撞檢測，將未碰撞的鄰居節點加入待擴展列表，之后跳到步驟二；

碰撞檢測的具體方法為：判斷當前起始點的未擴展鄰居節點集合是否為空，若是則直接跳到步驟二，若否則取出一個未擴展鄰居節點并對其進行碰撞檢測，若通過檢測則將其加入待擴展集合后重新判斷未擴展鄰居節點集合是否為空，若未通過碰撞檢測則直接重新判斷未擴展鄰居節點集合是否為空。

2.根據權利要求1所述的基于SDF地圖的無人機三維路徑快速規劃方法，其特征在于，步驟五中的n＝4。

3.根據權利要求1-2中任意一項所述的基于SDF地圖的無人機三維路徑快速規劃方法，其特征在于，評估函數f＝g+lambda*h，其中，g為從起始點開始到當前節點的實際代價，h為當前節點到目標點的啟發值，lambda為權重系數，步驟三中lambda＝20，快速探索的步驟B中lambda＝1，快速探索的步驟E中lambda＝20。

4.根據權利要求3所述的基于SDF地圖的無人機三維路徑快速規劃方法，其特征在于，g＝g_value+distance1，其中g_value為當前節點的父節點的g值，distance1為當前節點的父節點到當前節點的歐幾里得距離，h＝1.0001*distance2，distance2為當前節點到終點的歐幾里得距離。