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

有效

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

本發明提供一種基于SDF地圖的無人機三維路徑快速規劃方法，具體步驟如下：將起始點加入待擴展列表中；判斷待擴展列表是否為空；取出待擴展列表中評估函數f值最小的點p；以歐幾里得符號距離場體素大小的單倍步長，判斷點p與其父節點之間的路徑是否存在碰撞若否則將當前起始點設置為點p的父節點，并將點p設置為新的起始點；以n倍步長對鄰居節點進行擴展。本發明方法與傳統的A星算法相比，在保證了逼近最優解的同時擁有更少的規劃時間，加速效果明顯，使得即使是全局路徑規劃算法也達到了實時的規劃速度。

技術領域

本發明涉及無人機三維路徑快速規劃方法技術領域，尤其涉及一種基于SDF地圖的無人機三維路徑快速規劃方法。

背景技術

目前，三維路徑規劃有兩種地圖，一種是柵格地圖，一種是歐幾里德符號距離場地圖(SDF地圖)，而實際測試中，顯示歐幾里得符號距離場的相關庫實現獲取目標點到障礙物的距離從而判斷是否與障礙物碰撞是比較耗時的(例如voxblox)，而且無論是柵格地圖還是歐幾里得符號距離場，都存在分辨率這樣一個概念。要測試一段路徑是否發生碰撞需要根據分辨率，將這條路徑上的所有以分辨率作為劃分依據的的點都進行碰撞檢測，所以在實現傳統的全局路徑規劃算法(例如A星算法)時，發現由于碰撞檢測和分辨率之間密不可分的關系，使得傳統的A星算法在這樣的路徑規劃任務中運算速度較慢。

發明內容

本發明旨在解決現有技術的不足，而提供一種基于SDF地圖的無人機三維路徑快速規劃方法。

本發明為實現上述目的，采用以下技術方案：

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

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

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

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

步驟四，以歐幾里得符號距離場體素大小的單倍步長，判斷點p與其父節點之間的路徑是否存在碰撞，若是則將點p加入已擴展列表，并跳到步驟二，若否則將當前起始點設置為點p的父節點，并將點p設置為新的起始點，進入步驟五；

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

進一步的，從步驟四跳到步驟二之前，需判斷點p的父節點集合是否為空，若是則跳到步驟二，若否則以父節點集合中剩余的父節點中評估函數f值最小的父節點為路徑碰撞檢測的參考點，再次進行碰撞檢測，直到父節點集合為空。

進一步的，步驟五中的n＝4。

進一步的，評估函數f＝g+lambda*h，其中，g為從起始點開始到當前節點的實際代價，h為當前節點到目標點的啟發值，lambda為權重系數，步驟三中lambda＝20。

進一步的，g＝g_value+distance1，其中g_value為當前節點的父節點的g值，distance1為當前節點的父節點到當前節點的歐幾里得距離，h＝1.0001*distance2。distance2為當前節點到終點的歐幾里得距離。

進一步的，步驟五進行之前，以歐幾里得符號距離場體素大小的n倍步長對待擴展集合中的節點進行快速探索，快速探索的具體步驟如下：

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

步驟B，找出p2的f值最小的鄰居節點，公式中lambda＝1，以歐幾里得符號距離場體素大小的單倍步長，判斷點該鄰居節點與p2之間的路徑是否存在碰撞，若是則進入步驟五，若否則進入步驟C；