基于三節臂最優路徑的智能挖掘機軌跡規劃研究

羅映　張學強　王成濤　勾俊賀　張振

摘 要：以智能挖掘機三節臂的快速、安全運動規劃為研究目的，建立了挖掘機三維幾何模型，并在機器人操作系統（ROS）平臺中進行運動配置，建立運動規劃分析模型。模擬對比分析了快速擴展隨機樹算法（RRT）和雙向擴展平衡的結構型雙樹算法（RRT-connect）的路徑搜索能力，并量化了具體參數指標。進一步分別采用RRT和RRT-connect算法完成具體復雜約束環境下挖掘機臂的運動規劃，結果表明，RRT-connect算法，搜索能力強，更適用于復雜環境下挖掘機臂的運動規劃計算求解。

關鍵詞：機器人操作系統 機械臂 運動規劃

1 引言

挖掘機是工程機械的最主要機種，其功能典型，在各類在基礎工程建設、搶險救災等任務中發揮著不可替代的作用。目前，人工智能已進入2.0時代，人工智能技術與各細分產業融合是時代和行業發展的必然。在這一時代背景下，為應對精度、效率等方面越來越高的作業要求，適應危險、惡劣的作業環境，智能化、自主化已成為挖掘機未來主要發展趨勢[1]。

依據研究技術的應用與面向功能的實現，智能挖掘機的研究可以分為三個階段：基于傳感器輔助的自動控制、基于軌跡規劃的自動挖掘、面向現場工況的自主作業[2]。基于傳感器輔助的自動控制相關技術研究是目前工程機械廠商進行改造升級應用的主要技術，其相關技術相對較為筒單且成熟，可以滿足確定工況下筒單自動化需求，輔助操作人員完成指定任務，但其應用場景與功能不易更迭。相對于第一階段的自動控制，基于軌跡規劃的自動挖掘階段的研究重點在于更加復雜的控制算法與工作裝置的軌跡規劃，一定程度上實現挖掘機器人的柔性控制，能夠初步實現相對筒單的作業環節的自主控制，如自動裝載、直溝挖掘等任務。面向現場工況的自主作業階段的研究重點在于非確定環境下的自主作業，要求挖掘機具有感知復雜工況進行實時建模的能力，并據此自主規劃作業流程。在施工過程中，自主完成任務分解、軌跡規劃、驅動控制、自主避障等工作，并檢測作業效果，最終實現開放環境中，多機協調合作完成工程任務。

在智能挖掘機械研究領域，挖掘機械臂的運動規劃是一項關鍵技術，是保證挖掘機高效、準確自主作業的重要前提。研究中，基于對挖掘機臂工作裝置驅動空間、關節空間和位姿空間的運動關系分析，在ROS環境下，建立了挖掘機三維模型，完成對機械臂模型的運動學配置，利用Moveit功能包，采用快速擴展隨機樹算法（RRT）和雙向擴展平衡的結構型雙樹（RRT-connect）路徑規劃算法進行挖掘機臂挖掘作業的的運動規劃仿真，對比分析兩種運動規劃算法的規劃能力。

2 規劃算法：RRT和RRT-connect

2.1 RRT和RRT-connect算法原理

RRT算法（Rapidly-exploring Random Tree）是基于采樣的運動規劃算法，是Steven M. LaValle和James J. Kuffner Jr.提出的一種通過隨機構建空間填充樹（Space Filling Tree）實現對非凸高維空間快速搜索的算法。RRT 算法是一種單樹型的路徑規劃算法，其基本思想是通過隨機采樣和逐步擴展樹的方式，快速探索配置空間（configuration space）。RRT 算法優勢是搜索速度快，但生成的路徑可能不是最優路徑。如圖1中左圖所示，RRT算法將起點作為樹的唯一節點，在搜索空間中以一定概率選擇一個目標點作為采樣點。從樹中選擇距離采樣點最近的節點，計算該節點到采樣點的路徑。將計算得到的路徑上的下一個點添加到樹中，形成一個新的節點。同時，檢測新節點與環境中的障礙物是否發生碰撞。如果發生碰撞，則舍棄該節點。如果新節點沒有碰撞，將其與樹中距離最近的節點進行連接。重復隨機采樣，直到找到一條路徑連接起點和目標點。最后從樹的末端節點向根節點回溯，得到從起點到目標點的最優路徑[3]。

RRT-Connect（Rapidly Exploring Random Tree Connect）是RRT算法的一種變體，用于解決從起點到目標點的路徑規劃問題。如圖1中右圖所示，與傳統的RRT算法不同，RRT-Connect算法通過在兩個樹之間建立連接來搜索路徑，此算法可以檢查兩個樹的最新節點是否可以連接。如果兩個樹的最新節點不發生碰撞且路徑之間沒有障礙物，則連接這兩個節點。如果連接成功，交換兩個樹的角色。即以第一個樹為目標樹，第二個樹為起始樹。當兩個樹連接的節點之間的距離小于某個閾值時，路徑被認為可行[4-5]。

2.2 RRT和RRT-connect算法模擬對比分析

建立50cm×30cm的仿真環境，設立若干個障礙，由于每一次運行結果存在差異，分別對兩種算法運行60次，并計算路徑規劃時間、路徑節點數量和節點利用率，列于表1，圖2顯示了其中一組計算結果對比。

根據仿真結果可以很直觀的觀察到，傳統的RRT算法規劃時間約為RRT-connect算法的4倍。同時，與RRT-connect算法相比，RRT算法采樣帶點數量多，存在盲目搜索的問題，最優路徑曲率過大，存在這大量拐點，因此在對機械臂進行運動規劃時，容易造成機械臂抖動；相比之下，RRT-connect算法在規劃時間和采樣點數量上有明顯優勢，但其規劃路徑平滑性稍差，并且節點的利用效率略低于RRT算法。

3 基于兩種算法的挖掘機臂運動規劃對比分析

基于三維建模軟件Solidworks建立挖掘機的三維實體模型，并設定正確的基準軸和坐標系。完成三維實體建模后，在ROS系統中設置各個連接關節和相應的父連桿、子連桿，并定義各個關節的轉動角度范圍[6]，如圖3所示，并建立挖掘機的三維模型，如圖4所示。

為了對比分析RRT和RRT-connect算法在挖掘機臂運動規劃中的計算性能和穩定性，在rviz中搭建三維復雜環境，設置多個障礙物，規劃時間設置為三秒，不采用循環規劃。圖5為所搭建簡單環境的正視圖（a）和俯視圖（b），綠色柱體為障礙物，圖中1代表初始位姿，2代表終點位姿。

基于搭建的環境，在相同初始條件下，分別采用RRT算法及RRT-connect，對挖掘機臂進行運動規劃計算求解，計算運動規劃完成時間、節點總數、節點利用率、最優路徑長度等參數。計算結果如圖6所示，性能指標均值計算結果列于表2。

可以看出，復雜環境條件下，RRT與RRT-connect算法計算結果存在差異。為了定量分析兩種算法的計算性能，避免隨機誤差，在簡單環境下進行60次仿真實驗，得到計算結果均值，列于表3。根據參數指標計算結果， RRT算法在60次實驗中與障礙物發生了3次碰撞，功率降為95%。相比之下，RRT-connect 算法具有100%成功率。兩種算法的最優路徑平均節點數分別為148、106，平均規劃時間分別為1.17s和0.90s。相比之下，RRT-connect 算法具更高的成功率、更快的計算時間和更高的可靠性、更高的節點利用率。綜合各指標可知，在復雜環境下，與RRT算法相比，RRT-connect計算性能優勢顯著。

4 總結

利用三維建模軟件，建立了挖掘機臂的三維幾何模型。基于ROS平臺，生成XML格式的URDF 文件，并利用moveit功能包完成對挖掘機臂的運動配置。預設挖掘機臂運動規劃的六個主要位姿，在仿真環境添加障礙物模型，使挖掘機機械臂處于一定復雜的環境下。分別采用 RRT 算法和RRT-connect算法，實現對挖掘機臂的避障軌跡規劃計算求解，并對規劃成功率、路徑平均節點數、規劃時間、平均節點利用率等指標進行定量對比分析。結果說明，RRT-connect在挖掘機機械臂自動避障的安全運動規劃中綜合性能較為優異。

基金項目：2022年度省重點扶持區域引進急需緊缺人才項目“工程機械智能網聯控制平臺研究及產業化”。

參考文獻：

[1]張良壯. 智能挖掘機器人軌跡規劃與任務決策研究[D]. 浙江大學，2019.

[2]李運華， 范茹軍，楊麗曼. 智能化挖掘機的研究現狀與發展趨勢[J].機械工程學報，2020，56（13）：165-178.

[3]丁健. 基于ROS 的深海機械臂運動規劃研究[D]. 上海海洋大學，2022.

[4]智晉寧，賈旭峰，劉超，謝虎，史青錄. 平整工況下三節臂挖掘機時間最優運動規劃[J]. 中國農機化學報， 2022， 43（11）：146-154.

[5]唐家朋. 無人操縱挖掘機軌跡規劃與控制[D]. 浙江大學， 2021.

[6]朱萌，孟婥，張豪，孫以澤. 基于ROS的6自由度機械臂運動軌跡規劃[J]. 組合機床與自動化加工技術， 2021， 000（004）：1-3，9.