[發明專利]一種鉆錨機器人鉆臂軌跡規劃方法、系統及電子設備

在審

• 申請號: 202211526865.2
• 申請日: 2022-11-30
• 公開（公告）號: CN116276955A
• 公開（公告）日: 2023-06-23
• 發明（設計）人: 張旭輝;黃夢瑤;楊文娟;雷孟宇;李彬;田浩
• 申請（專利權）人: 西安科技大學;渭南陜煤啟辰科技有限公司
• 主分類號: B25J9/16
• 分類號: B25J9/16;B25J11/00;E21D20/00
• 代理公司: 北京高沃律師事務所 11569
• 代理人: 褚海英

1.一種鉆錨機器人鉆臂軌跡規劃方法，其特征在于，所述鉆錨機器人鉆臂軌跡規劃方法包括：

獲取鉆錨機器人的三維工作區域模型、工作起始點和鉆孔點；

采用快速擴展隨機樹算法確定所述鉆錨機器人的初始鉆臂路徑；所述初始鉆臂路徑為所述鉆錨機器人的鉆臂，在所述三維工作區域模型中，從所述工作起始點至所述鉆孔點的路徑；

采用連接法去除所述鉆錨機器人的初始鉆臂路徑中的冗余點，以得到第一優化路徑；

采用二階貝塞爾曲線對所述第一優化路徑進行擬合，以得到所述鉆錨機器人的最終鉆臂路徑。

2.根據權利要求1所述的鉆錨機器人鉆臂軌跡規劃方法，其特征在于，所述三維工作區域模型中包括障礙物區域和自由區域；

所述采用快速擴展隨機樹算法確定所述鉆錨機器人的初始鉆臂路徑，具體包括：

以所述工作起始點為起始點，以所述鉆孔點為目標點，并將所述工作起始點作為隨機樹初次擴展的根節點；

針對隨機樹的每次擴展，在所述自由區域內確定隨機節點，根據所述隨機節點確定所述隨機樹中的近距離節點；所述近距離節點為所述隨機樹在當前次擴展中與所述隨機節點的距離最小的根節點；

在所述近距離節點與所述隨機節點的連線上隨機確定待用節點；所述待用節點與所述近距離節點之間的距離為一個預設步長；

判斷所述近距離節點與所述隨機節點的連線是否與所述障礙物區域相交，以得到第一結果；

若所述第一結果表示相交，則舍棄所述待用節點，并返回在所述自由區域內確定隨機節點，根據所述隨機節點確定所述隨機樹中的近距離節點的步驟；

若所述第一結果表示未相交，則將所述待用節點作為新增根節點加入所述隨機樹，并在所述新增根節點與所述目標點之間的距離大于或等于一個預設步長時進入下一次擴展，直至擴展的次數達到預設擴展次數或者所述新增根節點與所述目標點之間的距離小于一個預設步長時，停止擴展；

循環遍歷所述隨機樹的所有根節點，與所述目標點連接以得到所述鉆錨機器人的初始鉆臂路徑。

3.根據權利要求2所述的鉆錨機器人鉆臂軌跡規劃方法，其特征在于，根據所述隨機節點確定所述隨機樹中的近距離節點，具體包括：

基于NEAREST函數利用歐氏距離，選擇所述隨機樹在當前次擴展中與所述隨機節點的距離最小的根節點。

4.根據權利要求1所述的鉆錨機器人鉆臂軌跡規劃方法，其特征在于，采用連接法去除所述鉆錨機器人的初始鉆臂路徑中的冗余點，以得到第一優化路徑，具體包括：

將所述初始鉆臂路徑劃分為多個路段；每個路段包括路段起點和路段終點；

將所述工作起始點作為優化起始點，將所述優化起始點與第i個路段的路段終點連接；i≥1；

判斷所述優化起始點與所述第i個路段的路段終點的連線之間是否存在障礙物，以得到第二結果；

若所述第二結果表示存在障礙物，則將所述優化起始點與第i個路段的路段起點的連線標記為優化路段，并將所述優化起始點更新為第i個路段的路段起點，返回將所述優化起始點與第i個路段的路段終點連接的步驟；

若所述第二結果表示不存在障礙物，判斷所述第i個路段的路段終點是否為鉆孔點，以得到第三結果；

若所述第三結果表示否，則將所述第i個路段更新為第i+1個路段，返回將所述工作起始點與第i個路段的路段終點連接的步驟；

若所述第三結果表示是，則將所述優化起始點與第i個路段的路段終點的連線標記為優化路段；

提取所有所述優化路段，以確定第一優化路徑。

5.根據權利要求1所述的鉆錨機器人鉆臂軌跡規劃方法，其特征在于，所述二階貝塞爾曲線的計算公式為：

B(t)＝(1-t)²*P₀+2t(1-t)*P₁+t²*P₂?0≤t≤1

其中，P₀表示擬合曲線起始點，P₁表示擬合曲線控制點，P₂表示擬合曲線結束點，t為系數，B(t)表示以點P₀為起始點、點P₂為結束點的擬合曲線。