改進(jìn)Informed-RRT*算法的柑橘采摘機(jī)械臂運(yùn)動(dòng)路徑規(guī)劃

劉 頓，王 毅

(重慶理工大學(xué) 機(jī)械工程學(xué)院， 重慶 400054)

近年來，隨著工業(yè)機(jī)器人的快速發(fā)展和我國鄉(xiāng)村振興戰(zhàn)略推進(jìn)，利用機(jī)器人提高農(nóng)業(yè)生產(chǎn)效率成為一個(gè)新的研究方向，果蔬產(chǎn)品的收獲采摘是農(nóng)業(yè)生產(chǎn)的重要組成部分，占到其生產(chǎn)過程作業(yè)量的40%，利用機(jī)器人來完成果蔬采摘收獲是農(nóng)業(yè)自動(dòng)化、智能化發(fā)展的重點(diǎn)研發(fā)領(lǐng)域[1-4]。機(jī)器人在采摘空間合理的運(yùn)動(dòng)路徑是保證采摘成功率和縮短采摘時(shí)間的關(guān)鍵因素。在自然環(huán)境中，果蔬生長位置隨機(jī)，大多被枝干、枝葉等障礙物包圍，在實(shí)際采摘中帶有末端執(zhí)行器的機(jī)械臂很難通過一次規(guī)劃來完成采摘，目前大多機(jī)器人都需要多次規(guī)劃路徑并滿足合理的采摘姿態(tài)才能保證采摘順利進(jìn)行。2017年，Lehnert等[5]研制了6軸機(jī)械臂與帶有吸盤切割刀片的末端執(zhí)行器組合的甜椒主動(dòng)收獲機(jī)器人，該型收獲機(jī)器人在室外實(shí)驗(yàn)的成功率為46%～58%，并且成功收獲果實(shí)的機(jī)械臂路徑規(guī)劃嘗試次數(shù)都在2次以上。2019年，Xiong等[6]設(shè)計(jì)了帶有障礙物分離算法的草莓自動(dòng)采摘機(jī)器人，該機(jī)器人在嘗試采摘草莓時(shí)，最多含有5段行程，包括機(jī)械臂前移、采摘預(yù)備、采摘控制、花梗剪切位置調(diào)整、返回，采摘成功率在50%～90%之間波動(dòng)。2019年，王毅[7]提出了一種基于預(yù)采摘點(diǎn)的采摘方法來提高采摘成功率，機(jī)械臂分為兩段運(yùn)動(dòng)，先運(yùn)動(dòng)到中間點(diǎn)再調(diào)整姿態(tài)完成采摘，室外單果采摘成功率為87.5%。

運(yùn)動(dòng)路徑的最優(yōu)性也很關(guān)鍵，這樣不僅可以節(jié)約采摘時(shí)間，提高采摘效率，還可以減少機(jī)械臂的運(yùn)動(dòng)幅度，減小與外界發(fā)生碰撞的可能，這就對(duì)機(jī)械臂的路徑規(guī)劃算法提出了新的要求。基于采樣的路徑規(guī)劃算法，基于采樣的快速擴(kuò)展隨機(jī)樹算法(rapid-exploration random tree,RRT)，以增量方法在工作空間中隨機(jī)采樣，能快速有效地探索高維空間，在機(jī)械臂運(yùn)動(dòng)規(guī)劃中運(yùn)用廣泛。但是該算法存在收斂速度慢，生成路徑非最優(yōu)、不平滑的問題。Karaman等[8]提出RRT*，通過重新選取父節(jié)點(diǎn)和重新布線找到最優(yōu)路徑，但是當(dāng)環(huán)境較為復(fù)雜，需要節(jié)點(diǎn)較多時(shí)，算法的內(nèi)存消耗和計(jì)算時(shí)間會(huì)成倍增長。Gammell等[9]提出Informed-RRT*，通過生成超橢球子集然后對(duì)齊集中采樣，讓搜索更加集中，對(duì)規(guī)劃問題的維度和域的依賴性更小，能夠更快更好地找到不同的路徑;但是該算法的生成超橢圓子集的大小嚴(yán)重依賴初始路徑的優(yōu)劣，并且當(dāng)超橢圓子集體大于規(guī)劃問題的范圍時(shí)，該算法將不再適用[10]。

路徑規(guī)劃算法在采摘機(jī)械臂的運(yùn)用方面,Ademovic等[11]在RRT基礎(chǔ)上，利用障礙點(diǎn)擴(kuò)展體積信息，提出了構(gòu)型空間中機(jī)械臂搜尋無碰撞路徑的方法，提高了機(jī)械臂高維空間的探索能力。2019年，馬冀桐等[12]針對(duì)采摘機(jī)械臂需要深入樹冠內(nèi)部進(jìn)行采摘的問題，提出了一種基于構(gòu)型空間先驗(yàn)知識(shí)引導(dǎo)點(diǎn)的RRT改進(jìn)算法，通過增加引導(dǎo)點(diǎn)提高了機(jī)械臂通過樹冠內(nèi)眾多枝干構(gòu)成的窄通道的能力。2021年，張勤等[13]在RRT算法基礎(chǔ)上，引入啟發(fā)式采樣和節(jié)點(diǎn)拒絕策略，改善了路徑成本，提升了收斂速度。有研究報(bào)道，當(dāng)前大多數(shù)機(jī)器人系統(tǒng)執(zhí)行軌跡要比規(guī)劃路徑花費(fèi)更多時(shí)間[14]。但是目前很少有采摘機(jī)械臂規(guī)劃算法以減少采摘機(jī)械臂規(guī)劃和運(yùn)動(dòng)次數(shù)來提高采摘效率，減少采摘耗時(shí)的報(bào)道。

本文針對(duì)目前采摘機(jī)械臂完成采摘存在多次行程規(guī)劃效率低、耗時(shí)長、路徑非最優(yōu)等問題，在漸進(jìn)最優(yōu)算法Informed-RRT*基礎(chǔ)上，結(jié)合采摘環(huán)境與采摘?jiǎng)幼鳎跈C(jī)械臂構(gòu)型空間中定義了預(yù)采摘點(diǎn)，提出了四顆隨機(jī)樹同時(shí)探索方法，同時(shí)加入自適應(yīng)步長函數(shù)和啟發(fā)式的節(jié)點(diǎn)優(yōu)化策略。可一次性規(guī)劃出滿足采摘的機(jī)械臂運(yùn)動(dòng)路徑，并且保證了采摘路徑質(zhì)量，降低路徑規(guī)劃的時(shí)間。

1 算法提出

1.1 基于預(yù)采摘引導(dǎo)點(diǎn)的四顆隨機(jī)樹并行探索

為了保證采摘順利進(jìn)行，預(yù)采摘點(diǎn)的設(shè)置至關(guān)重要。王燕[15]采用兩段擺線運(yùn)動(dòng)組合的軌跡規(guī)劃方法使機(jī)械手的末端執(zhí)行器在采摘果實(shí)時(shí)始終保持為水平狀態(tài)，機(jī)械臂保持水平采摘運(yùn)動(dòng)也會(huì)縮小機(jī)械臂的采摘距離。因此，本文設(shè)置的預(yù)采摘點(diǎn)位置為與果實(shí)水平且相距X=25 cm的位置，如圖1所示。

圖1 水平預(yù)采摘位置設(shè)置

同時(shí)將采摘引導(dǎo)點(diǎn)納入到規(guī)劃算法中，在構(gòu)型空間探索中，以預(yù)采摘點(diǎn)為第3點(diǎn)實(shí)現(xiàn)四顆樹同時(shí)擴(kuò)展。假設(shè)在構(gòu)型空間中，果實(shí)目標(biāo)點(diǎn)的坐標(biāo)為(x，y，z)，則預(yù)采摘點(diǎn)的坐標(biāo)為(x1，y1，z1)，如圖2所示。Informed-RRT*算法采用原始的RRT算法生成一條初始路徑Cbest，而初始路徑的好壞直接影響該算法效率。原始的RRT算法存在的一些缺陷，雙向搜索是一種常用的改進(jìn)策略，已經(jīng)被證明可以明顯提高基于采樣的運(yùn)動(dòng)規(guī)劃算法的成功率和收斂速度。并且為了加快隨機(jī)到達(dá)目標(biāo)點(diǎn)的速度，在隨機(jī)樹的每次生長過程中，加入隨機(jī)概率P(0.0-1.0)的函數(shù)ChoseNode來選擇生長方向是目標(biāo)點(diǎn)還是隨機(jī)點(diǎn)。規(guī)劃器在雙樹隨機(jī)采樣擴(kuò)展的同時(shí)，以P概率向終點(diǎn)目標(biāo)運(yùn)動(dòng)，如圖3所示。

圖2 構(gòu)型空間中預(yù)采摘點(diǎn)位置

圖3 加入概率P函數(shù)的雙樹探索

函數(shù):ChoseNode(qnew, P)1 if Rand≤P2 qrand=qgoal else3 qrand=Rand

1.2 自適應(yīng)動(dòng)態(tài)步長

原始的Informed-RRT*算法中，在對(duì)未知空間探索時(shí)步長是不變的，這種定值步長在探索無障礙空間時(shí)會(huì)影響隨機(jī)樹的擴(kuò)展速度。本文在算法中加入了動(dòng)態(tài)步長的調(diào)節(jié)函數(shù)AdustableSteer，引入調(diào)節(jié)系數(shù)k，具體為：隨機(jī)樹以初始步長ε向目標(biāo)探索，在擴(kuò)展新節(jié)點(diǎn)時(shí)，如在節(jié)點(diǎn)處沒有發(fā)生碰撞且未與另一顆樹連接，則在下一次擴(kuò)展中，步長為ε=ε+kε，依次疊加。步長越大，算法擴(kuò)展速度就越強(qiáng)，收斂速度也越快，但是在環(huán)境復(fù)雜、障礙物較多時(shí)，機(jī)械臂的局部路徑也會(huì)變長，與環(huán)境發(fā)生碰撞的可能性也會(huì)增加，因此在遇到障礙物碰撞之后，步長變?yōu)樵疾介L，調(diào)節(jié)系數(shù)由正變負(fù)，依次疊加，直到脫離局部路徑。

1.3 啟發(fā)式節(jié)點(diǎn)優(yōu)化策略

在 Informed-RRT*算法的路徑優(yōu)化過程采用RRT*類似的方法，即對(duì)潛在最優(yōu)父節(jié)點(diǎn)的搜索對(duì)象是由路徑規(guī)劃過程中構(gòu)成的節(jié)點(diǎn)數(shù)。潛在的最優(yōu)父節(jié)點(diǎn)的數(shù)量只有少數(shù)幾個(gè)，而檢測的過程中需要檢測所有節(jié)點(diǎn)，存在檢測多余節(jié)點(diǎn)而造成路徑規(guī)劃效率低的問題。

改進(jìn)算法在Informed-RRT*基礎(chǔ)上，加入啟發(fā)式的節(jié)點(diǎn)優(yōu)化策略NodeOptimazation函數(shù)，在路徑優(yōu)化時(shí)，以更少的迭代次數(shù)和更少的節(jié)點(diǎn)來優(yōu)化路徑，提高效率。在節(jié)點(diǎn)qnew擴(kuò)展前，計(jì)算出該節(jié)點(diǎn)到起始點(diǎn)路徑的代價(jià)α，再通過先驗(yàn)知識(shí)得到該節(jié)點(diǎn)到目標(biāo)點(diǎn)的估計(jì)代價(jià)β。如果條件成立即加入該點(diǎn)的估計(jì)代價(jià)小于當(dāng)前路徑代價(jià)，那么該節(jié)點(diǎn)就有可能改善當(dāng)前路徑，從而將該點(diǎn)納入擴(kuò)展樹中。

αstar-to-q+βq-to-goal≤Cbest

(1)

式中:αstar-to-q為擴(kuò)展節(jié)點(diǎn)qnew到起始點(diǎn)路徑的代價(jià);βq-to-goal為qnew到目標(biāo)點(diǎn)的直線距離;Cbest為當(dāng)前最低的路徑代價(jià)。

1.4 采摘算法的實(shí)現(xiàn)

圖4 算法節(jié)點(diǎn)擴(kuò)展過程示意圖

2 實(shí)驗(yàn)驗(yàn)證與結(jié)果分析

2.1 二維環(huán)境仿真實(shí)驗(yàn)與討論

在Matlab軟件中進(jìn)行二維算法模擬,設(shè)置了3種模擬環(huán)境：無障礙環(huán)境、簡單障礙環(huán)境以及二維采摘模擬環(huán)境。簡單障礙在目標(biāo)點(diǎn)之間分布3個(gè)固定障礙物；二維采摘模擬環(huán)境是對(duì)實(shí)際采摘環(huán)境進(jìn)行二維映射，果實(shí)目標(biāo)點(diǎn)被枝干等障礙物包圍。起始點(diǎn)為(130，130)，目標(biāo)點(diǎn)為(710，520)。二維仿真實(shí)驗(yàn)中，概率P設(shè)為0.1，步長設(shè)置為20，動(dòng)態(tài)步長調(diào)節(jié)系數(shù)K為0.1，最大步長設(shè)置為30，搜索半徑為120，最大迭代次數(shù)為1 000。仿真環(huán)境的硬件信息為：CPU為i7-6700,主頻3.4 GHz，顯卡為GTX1060，24 G內(nèi)存，1 TB硬盤，操作系統(tǒng)為Windows 10專業(yè)版。

基于上述的3種模擬環(huán)境分別進(jìn)行50次模擬實(shí)驗(yàn)，比較Informed-RRT*與本文提出的改進(jìn)算法的路徑規(guī)劃，驗(yàn)證算法的有效性。仿真過程如圖4所示，紅色為起始點(diǎn)，綠色為目標(biāo)點(diǎn)，紫色為預(yù)采摘點(diǎn)，黑色為障礙物；綠色曲線為節(jié)點(diǎn)重新布線，藍(lán)色曲線為規(guī)劃算法生成的初始的路徑，黑色曲線為從算法優(yōu)化更新后的路徑，黃色曲線為最終生成的路徑。

比較圖5、6算法路徑規(guī)劃圖，可以看出2種算法都具有明顯的漸進(jìn)最優(yōu)性，改進(jìn)Inform-RRT*算法在進(jìn)行路徑規(guī)劃時(shí)，隨機(jī)擴(kuò)展的節(jié)點(diǎn)和綠色曲線明顯減少，最終的路徑成本也更小，表明改進(jìn)算法提高了節(jié)點(diǎn)的利用率，減少了路徑探索的盲目性，提高了路徑規(guī)劃的效率，降低了路徑成本。

記錄二維模擬環(huán)境中20次重復(fù)實(shí)驗(yàn)的相關(guān)數(shù)據(jù)，如表1所示。由于Informed-RRT*無法一次規(guī)劃出兩段路徑，因此在同一張地圖下，進(jìn)行2次原始Informed-RRT*算法仿真，將2次的算法規(guī)劃相關(guān)數(shù)據(jù)相加。相較于原始Informed-RRT*算法，改進(jìn)的算法在初始路徑和最終路徑都有明顯的改進(jìn)，在初始路徑階段加入了四樹探索、P概率采樣以及動(dòng)態(tài)步長，大大加速了路徑探索的速度；在生成初始路徑階段，改進(jìn)后的算法路徑使成本減少了12%，時(shí)間縮短了78%，節(jié)點(diǎn)數(shù)縮短了24%，平均迭代次數(shù)下降了60%。通過節(jié)點(diǎn)優(yōu)化策略剔除了不能改善當(dāng)前路徑的節(jié)點(diǎn)，大大提高了路徑優(yōu)化的效率。在路徑優(yōu)化階段，改進(jìn)后的算法路徑使成本減少了6%，時(shí)間縮短了88%，節(jié)點(diǎn)數(shù)縮短了42%，平均迭代次數(shù)下降了64%。

圖5 改進(jìn)Informed-RRT*算法路徑規(guī)劃

圖6 原始Informed-RRT*算法路徑規(guī)劃

表1 算法二維仿真結(jié)果數(shù)據(jù)

2.2 三維采摘仿真實(shí)驗(yàn)與結(jié)果分析

2.2.1 實(shí)驗(yàn)設(shè)計(jì)

利用本團(tuán)隊(duì)研發(fā)的在Linux系統(tǒng)下基于ROS的采摘機(jī)器人平臺(tái)[5]驗(yàn)證改進(jìn)算法，該機(jī)器人由機(jī)械臂、視覺系統(tǒng)及移動(dòng)底盤等構(gòu)成。機(jī)械臂為北京遨博公司生產(chǎn)的協(xié)作機(jī)械臂aubo_i5，機(jī)械臂的采摘范圍是0～2 m，如圖7所示。

為了更好地驗(yàn)證改進(jìn)算法在采摘環(huán)境下的性能，在windows下安裝的Ubuntu 虛擬機(jī)，分配4GB的內(nèi)存大小，GPU為GTX1060，搭建基于ROS的柑橘采摘機(jī)器人仿真控制平臺(tái)，如圖7所示。

1.末端執(zhí)行器; 2.aubo機(jī)械臂; 3.雙目攝像頭; 4.履帶底盤; 5.機(jī)器人外殼; 6.果實(shí)盛放口

為了驗(yàn)證本文所提出的軌跡規(guī)劃算法是否能在高維環(huán)境中實(shí)現(xiàn)采摘路徑的一次性規(guī)劃。利用ROS機(jī)器人系統(tǒng)中的moveit插件和Rivz虛擬仿真環(huán)境進(jìn)行了仿真實(shí)驗(yàn)。在Rviz環(huán)境中，為減小碰撞檢測的計(jì)算量，對(duì)柑橘采摘機(jī)器人模型進(jìn)行簡化，使用簡單的長方體包圍盒代替復(fù)雜的底座三維形狀，如圖8所示，在ROS系統(tǒng)的rviz仿真環(huán)境中以綠色圓柱體外包絡(luò)線表示枝干，橘黃色的球體表示柑橘，重建了4棵柑橘樹，進(jìn)行了仿真環(huán)境下的柑橘單果采摘避障規(guī)劃實(shí)驗(yàn)。將設(shè)計(jì)的改進(jìn)算法添加到moveit的開源運(yùn)動(dòng)規(guī)劃庫OMPL中，并與RRI-connect、RRT*、Informed-RRT*這3種基于采樣的路徑規(guī)劃算法進(jìn)行比較。實(shí)驗(yàn)中使用其他算法時(shí)，采用2次規(guī)劃兩段路徑來實(shí)現(xiàn)采摘。采摘姿態(tài)均采用水平姿態(tài)采摘。采摘機(jī)械臂的初始姿態(tài)均為(0，282，234,0,0,0)，如圖9所示。在不同的采摘環(huán)境下，每種算法進(jìn)行20次重復(fù)采摘規(guī)劃實(shí)驗(yàn)。改進(jìn)算法的為黃色路徑，其他算法路徑為藍(lán)色，如圖10所示。

圖8 機(jī)器人本體模型 圖9 機(jī)械臂的初始位姿

圖10 路徑規(guī)劃仿真結(jié)果

2.2.2實(shí)驗(yàn)過程和分析

仿真實(shí)驗(yàn)結(jié)果表明：改進(jìn)后的算法可以實(shí)現(xiàn)復(fù)雜采摘環(huán)境的避障和采摘。由表2數(shù)據(jù)可以看出，基于預(yù)采摘點(diǎn)的改進(jìn)算法顯著提高了在復(fù)雜障礙采摘環(huán)境下采摘路徑規(guī)劃的規(guī)劃時(shí)間和成功率，在不同的采摘環(huán)境下,改進(jìn)算法的平均規(guī)劃時(shí)間約為0.81 s，規(guī)劃成功率約為96%。原始的Informed-RRT*算法的路徑比RRT-connect和RRT*算法有明顯改善。相較于使用Informed-RRT*算法來采摘規(guī)劃，改進(jìn)算法規(guī)劃時(shí)間減少了約46%，規(guī)劃成功率提高約11%，路徑成本優(yōu)化了約16%。

表2 三維仿真采摘環(huán)境下路徑 規(guī)劃算法數(shù)據(jù)比較

續(xù)表(表2)

3 結(jié)論

針對(duì)目前采摘機(jī)械臂完成采摘存在多次行程規(guī)劃效率低、路徑成本高、優(yōu)化不足的問題，提出了基于Informed-RRT*改進(jìn)的規(guī)劃算法。仿真實(shí)驗(yàn)表明：基于Informed-RRT*改進(jìn)的采摘算法通過在構(gòu)型空間內(nèi)加入預(yù)采摘引導(dǎo)點(diǎn)可實(shí)現(xiàn)一次就規(guī)劃出滿足采摘需求的機(jī)械臂運(yùn)動(dòng)路徑，提高了機(jī)械臂采摘效率。

基于Informed-RRT*改進(jìn)的采摘算法通過引入預(yù)采摘點(diǎn)，在初始點(diǎn)與預(yù)采摘點(diǎn)和預(yù)采摘點(diǎn)與目標(biāo)點(diǎn)之間四樹并行擴(kuò)展；用P概率采樣取代隨機(jī)采樣改善采樣的盲目性；通過動(dòng)態(tài)步長生成新的節(jié)點(diǎn)，提高了算法探索未知空間的速度和靈活性；節(jié)約了采摘路徑規(guī)劃時(shí)間。并且加入了啟發(fā)性的節(jié)點(diǎn)優(yōu)化策略，提高了算法路徑漸進(jìn)優(yōu)化效率。實(shí)驗(yàn)表明：相較于原始的Informed-RRT*算法，規(guī)劃耗時(shí)減少46%，規(guī)劃成功率提高了11%。