基于三節(jié)臂最優(yōu)路徑的智能挖掘機軌跡規(guī)劃研究

羅映　張學(xué)強　王成濤　勾俊賀　張振

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

關(guān)鍵詞：機器人操作系統(tǒng) 機械臂 運動規(guī)劃

1 引言

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

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

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

2 規(guī)劃算法：RRT和RRT-connect

2.1 RRT和RRT-connect算法原理

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

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

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

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

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

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

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

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

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

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

4 總結(jié)

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

基金項目：2022年度省重點扶持區(qū)域引進急需緊缺人才項目“工程機械智能網(wǎng)聯(lián)控制平臺研究及產(chǎn)業(yè)化”。

參考文獻：

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

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

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

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

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

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