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

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

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

1 引言

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

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

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

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

2.1 RRT和RRT-connect算法原理

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

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

2.2 RRT和RRT-connect算法模擬對(duì)比分析

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

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

3 基于兩種算法的挖掘機(jī)臂運(yùn)動(dòng)規(guī)劃對(duì)比分析

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

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

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

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

4 總結(jié)

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

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

參考文獻(xiàn)：

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

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

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

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

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

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