[發明專利]大面積環境中的改進型快速搜索隨機樹路徑規劃方法

有效

• 申請號: 202011179457.5
• 申請日: 2020-10-29
• 公開（公告）號: CN112393739B
• 公開（公告）日: 2022-07-29
• 發明（設計）人: 黃杰;尚偉偉;董翔宇;朱俊;吳永恒;劉鑫;汪太平;楊波;柴宏博;陳源;崇寶辰;張飛
• 申請（專利權）人: 國網安徽省電力有限公司檢修分公司
• 主分類號: G01C21/34
• 分類號: G01C21/34
• 代理公司: 北京潤平知識產權代理有限公司 11283
• 代理人: 肖冰濱;劉兵

1.一種大面積環境中的改進型快速搜索隨機樹路徑規劃方法，其特征在于，所述方法包括：

確定整個規劃空間的可通行區域和障礙物區域；

以起始點為圓心、所述起始點與目標點的距離的一半為半徑作圓弧；

以預設的弧度間隔在所述圓弧上隨機選取采樣點；

判斷選取的采樣點是否在所述障礙物區域；

在判斷選取的采樣點在所述障礙物區域的情況下，再次以預設的弧度間隔在所述圓弧上隨機選取采樣點，并執行所述方法的相應步驟，直到判斷選取的采樣點不在所述障礙物區域；

在判斷選取的采樣點不在所述障礙物區域的情況下，在所述起始點和選取的采樣點建立第一棵搜索樹，在選取的采樣點和所述起始點之間建立第二棵搜索樹，在選取的采樣點和所述目標點之間建立第三棵搜索樹，在所述目標點和選取的采樣點之間建立第四棵搜索樹；

判斷所述第一棵搜索樹的擴展點和所述第二棵搜索樹的擴展點之間的距離是否小于或等于預設的閾值距離；

在判斷所述第一棵搜索樹的擴展點和所述第二棵搜索樹的擴展點之間的距離小于預設的閾值距離的情況下，將所述第一棵搜索樹當前的路徑和所述第二棵搜索樹當前的路徑組合以得到所述起始點和選取的采樣點之間的路徑；

判斷所述第三棵搜索樹的擴展點和所述第四棵搜索樹的擴展點之間的距離是否小于所述閾值距離；

在判斷所述第三棵搜索樹的擴展點和所述第四棵搜索樹的擴展點之間的距離小于所述閾值距離的情況下，將所述第三棵搜索樹當前的路徑和所述第四棵搜索樹當前的路徑組合以得到選取的采樣點和所述目標點之間的路徑；

將所述起始點和選取的采樣點之間的路徑和選取的采樣點和所述目標點之間的路徑組合以得到所述起始點和所述目標點之間的路徑；

所述以預設的弧度間隔在所述圓弧上隨機選取采樣點具體包括：

首個采樣點為所述起始點和所述目標點的連線中點；

所述第一棵搜索樹、所述第二棵搜索樹、所述第三棵搜索樹以及所述第四棵搜索樹具體包括：

在規劃空間內隨機選擇一個采樣點；

判斷選擇的采樣點是否處于所述障礙物區域內；

在判斷選擇的采樣點處于障礙物區域內的情況下，再次在規劃空間內隨機選擇一個采樣點，直到判斷選擇的采樣點處于所述障礙物區域外；

在判斷選擇的采樣點處于所述障礙物區域外的情況下，確定在搜索樹中與選擇的采樣點距離最小的最近點；

根據公式(1)確定所述搜索樹當前的擴展點，

X_new＝X_nearest+r·e_r+k·e_g， (1)

其中，X_new表示所述擴展點的坐標，X_nearest表示所述最近點的坐標，e_r為從所述最近點到選擇的采樣點方向上的單位向量，e_g為從所述最近點到所述搜索樹對應的終點方向上的單位向量，r、k分別為對應的權重值；

計算最近點X_nearest和擴展點X_new之間的路徑的代價值以作為第一代價值；

以擴展點X_new為圓心，預設值r_i為半徑建立擴展點X_new的父節點集合；

從所述父節點集合中隨機選擇一個潛在父節點；

計算選取的所述潛在父節點和擴展點X_new之間的路徑的代價值以作為第二代價值；

選擇所述第一代價值和所述第二代價值中的較小者對應的節點進行碰撞檢測；

在所述碰撞檢測通過的情況下，將所述較小者作為所述第一代價值；

判斷所述父節點集合中是否還存在未被選擇的所述潛在父節點；

在判斷所述父節點集合中存在未被選擇的所述潛在父節點的情況下，再次從所述父節點集合中隨機選擇一個潛在父節點，并執行所述方法的相應步驟；

在判斷所述父節點集合中不存在未被選擇的所述潛在父節點的情況下，將當前的所述第一代價值對應的節點與擴展點X_new的路徑加入所述搜索樹中，再次從規劃空間內隨機選擇一個采樣點，并執行所述方法的相應步驟。