不確定采摘環(huán)境下改進RRT算法的機械臂路徑規(guī)劃研究

摘要：

由于果蔬采摘環(huán)境的不確定性和復雜性，機械臂在復雜環(huán)境中完成采摘，其路徑規(guī)劃需考慮實時避障。為實現(xiàn)采摘機械臂在不確定環(huán)境下安全采摘，提出一種改進RRT的動態(tài)避障算法，以提升機械臂在不確定采摘環(huán)境的適應(yīng)性。針對基本快速擴展隨機樹算法 （Rapidly-exploring Random Trees，RRT） 在動態(tài)環(huán)境下迭代時間長、路徑長、適應(yīng)性差等問題，在RRT算法的基礎(chǔ)上，引入目標導向策略，把終點以一定概率作為隨機采樣點的采樣方向，提高算法的迭代效率；引入動態(tài)檢測機制，對已完成規(guī)劃的初始路徑進行實時檢測，使算法適應(yīng)動態(tài)變化的環(huán)境。通過仿真分析改進RRT算法，結(jié)果表明：改進RRT算法的路徑減少16%，迭代時間縮短86.5%；同時，動態(tài)檢測機制使算法適應(yīng)動態(tài)環(huán)境。

關(guān)鍵詞：果蔬采摘；機械臂；快速擴展隨機樹；動態(tài)避障；目標導向；動態(tài)檢測；路徑規(guī)劃

中圖分類號：S23； TP241

文獻標識碼：A

文章編號：2095-5553 （2024） 04-0193-06

收稿日期：2023年3月6日" 修回日期：2023年7月12日

基金項目：國家自然科學基金（52265003）；機械制造系統(tǒng)工程國家重點實驗室開放課題基金項目（sklms2022023）；新疆維吾爾自治區(qū)創(chuàng)新團隊機器人及智能裝備技術(shù)科技創(chuàng)新團隊（2022D14002）

第一作者、通訊作者：李曉娟，女，1987年生，烏魯木齊人，博士，副教授，碩導；研究方向農(nóng)牧特種機器人。E-mail： lxj_xj903@163.com

Research on path planning of robotic arm with improved" RRT algorithm

in uncertain environment for harvesting

Li Xiaojuan， Chen Tao， Han Ruichun， Liu Jianxuan

（College of Mechanical Engineering， Xinjiang University， Urumqi， 830000， China）

Abstract：

Due to the uncertainty and complexity of the harvesting environment for fruits and vegetables， the manipulator needs to consider real-time obstacle avoidance in completing harvesting tasks in complex environments. In order to achieve safe harvesting of manipulator in uncertain environments， an improved dynamic obstacle avoidance algorithm based on the rapidly-exploring random trees （RRT） algorithm is proposed to enhance the adaptability of manipulator in uncertain harvesting environments. In order" to address the issues of long iteration time， long path length， and poor adaptability in dynamic environments of the basic RRT algorithm， this study first introduces a target-oriented strategy to increase the iteration efficiency of the algorithm by randomly sampling points with a certain probability towards the endpoint. Secondly， a dynamic detection mechanism is introduced to dynamically detect the initial path that has been planned， making the algorithm adaptable to changes in the environment. Simulation analysis shows that the improved RRT algorithm reduces path length by 16% and shortens iteration time by 86.5% compared to the basic RRT algorithm. Furthermore， the dynamic detection mechanism allows the algorithm to adapt to dynamic environments.

Keywords：

pick fruits and vegetables; robotic arm; rapidly-exploring random trees; dynamic obstacle avoidance; goal orientation; dynamic detection; path planning

0 引言

果蔬采摘作為農(nóng)業(yè)生產(chǎn)中最耗時耗力的一個環(huán)節(jié)［1］，隨著城鎮(zhèn)化進程的加快，大量農(nóng)業(yè)勞動力流失，并受到人口老齡化加劇的影響［2］，傳統(tǒng)的依靠大量勞動力的果蔬采摘模式已經(jīng)不能滿足未來農(nóng)業(yè)發(fā)展的需求［3］。隨著機器人技術(shù)的迅速發(fā)展及鄉(xiāng)村振興戰(zhàn)略的持續(xù)推進，為提高農(nóng)業(yè)生產(chǎn)效率，利用機械臂實現(xiàn)無人化果蔬高效、無損采摘成為一個重要的研究方向［4］，而采摘機械臂路徑規(guī)劃是實現(xiàn)精確采摘的重要前提。

常見的路徑規(guī)劃算法有：Dijkstra［5］、A*［6］、人工勢場［7］等算法，這些算法都需對空間進行網(wǎng)格化處理，在高維空間中會因計算的提升導致搜索性能嚴重下降。為解決在高維環(huán)境中機械臂路徑規(guī)劃問題，文獻［8，9］提出了快速搜索隨機樹（Rapidly-exploring Random Trees，RRT）算法，有效解決了高維空間路徑規(guī)劃問題。針對基本RRT算法存在隨機性強、冗余節(jié)點多、搜索效率低等不足［10］，國內(nèi)外的學者進行了研究，并取得了大量成果。例如，為提高搜索速度、效率，文獻［11］提出雙向搜索樹RRT_Connect，從初始點和目標點兩棵隨機樹同時進行搜索；為保證路徑的最優(yōu)，文獻［12］提出了RRT*算法，通過代價函數(shù)選取擴展節(jié)點鄰域內(nèi)最小代價的方式，改進父節(jié)點的選擇，每次搜索都重新選擇樹上的節(jié)點連接；為提高收斂速度，文獻［13］提出漸近最優(yōu)Informed RRT*算法，對RRT*的采樣過程進行優(yōu)化，采用一個橢圓采樣方式來代替全局均勻采樣，減少冗余的分支，提高了搜索效率；文獻［14］提出批量處理Batch Informed Trees（BIT*）算法，BIT*對采樣空間進行多批次多樣本的采樣，提高了采樣的效率進而提高了搜索效率。同時，國內(nèi)的學者針對具體的工作環(huán)境提出了具有目的性的改進算法，例如，文獻［15］針對荔枝采摘環(huán)境，提出了改進RRT的荔枝采摘機械臂運動規(guī)劃算法，在RRT算法中引入目標重力概念，以加快路徑搜索速度，并采用遺傳算法對生成的路徑進行優(yōu)化，然后用一種路徑平滑方法對路徑進行平滑處理，極大地縮短了路徑長度。文獻［16］針對柑橘的采摘環(huán)境，提出了基于Informed-RRT*改進的柑橘采摘機械臂運動規(guī)劃算法，在起始點和目標點之間選取第三點作為預采摘引導點，并在算法中引入了啟發(fā)性的節(jié)點采樣策略，提高了最優(yōu)路徑的收斂速度。文獻［17］針對多設(shè)施農(nóng)業(yè)果蔬采摘，提出了柯西目標引力雙向RRT*算法，通過柯西分布的方法進行啟發(fā)式采樣，降低采樣的盲目性，引入目標引力，提高搜索速度，引入節(jié)點拒絕策略，提高計算效率。

綜合國內(nèi)外目前的研究現(xiàn)狀來看，雖然大量的學者們對機械臂的路徑規(guī)劃都進行了廣泛的研究，但都未考慮到規(guī)劃完成后環(huán)境中障礙物位置信息的變化對已完成規(guī)劃路徑的影響，這可能會導致機械臂在運動過程中發(fā)生碰撞［18］。因此，本文提出一種改進的動態(tài)RRT路徑規(guī)劃算法，該算法在采樣過程中結(jié)合目標導向策略，將隨機采樣點以一定概率把終點作為隨機采樣方向，減少冗余迭代節(jié)點的產(chǎn)生，提高算法的迭代效率。當算法完成迭代時引入動態(tài)檢測機制，實時檢測已完成規(guī)劃的路徑是否有阻礙，滿足機械臂適應(yīng)不確定的動態(tài)采摘環(huán)境的要求。

1 RRT算法研究及改進

1.1 RRT算法原理

RRT算法是基于一顆隨機采樣并擴展成一顆有無碰撞路徑的樹，其擴展本質(zhì)是與樹生長類似［19， 20］。以起點作為樹的樹根，通過對狀態(tài)空間進行隨機采樣并經(jīng)過碰撞檢測后，若無碰撞則將采樣點加入樹中，以此往復進行樹擴展，當隨機采樣點與終點重合或采樣點距離終點值小于設(shè)定的閾值時，完成擴展并找到規(guī)劃的路徑，其擴展示意圖如圖1所示。

在非結(jié)構(gòu)化的采摘環(huán)境中，由于初始信息可能是不完整或者采摘環(huán)境本身就存在動態(tài)障礙物，當障礙物對已規(guī)劃完成的路徑發(fā)生碰撞時，將導致已規(guī)劃完成的初始路徑無效，如圖2所示，若算法未及時重新進行路徑規(guī)劃，將會導致機械臂在路徑追蹤過程中發(fā)生碰撞甚至是采摘設(shè)備的損壞。

1.2 改進RRT算法

為使采摘機械臂在動態(tài)環(huán)境的采摘過程安全，針對基本RRT算法在動態(tài)環(huán)境中的不足進行如下改進。1）針對隨機性強的缺點，引入目標導向策略，對采樣點進行目標導向，減少冗余節(jié)點的產(chǎn)生。2）針對RRT算法在動態(tài)環(huán)境中的不足，引入動態(tài)檢測機制，檢測已規(guī)劃完成路徑情況，使算法適應(yīng)不確定的動態(tài)環(huán)境。改進RRT算法流程圖如圖3所示。

1.2.1 目標導向策略

基本RRT算法在工作區(qū)域內(nèi)采樣具有隨機性，在擴展過程中方向不明確，具有一定的盲目性［21］。因此，在基本RRT算法中引入目標導向策略，在隨機采樣過程中以一定概率P進行目標采樣進行擴展，其余進行隨機采樣，有效地提高采樣方向的導向性，提高了迭代效率。其目標導向策略如式（1）所示。

xnew=xnear+lxrand-xnear‖xrand-xnear‖

（1）

xrand=

xgoalif P＞Prand

Sampleelse

式中：

l——擴展步長；

xnew——新擴展節(jié)點；

xnear——上一擴展節(jié)點；

xrand——隨機擴展節(jié)點；

‖xrand-xnear‖——

xrand與xnear之間的歐氏距離。

1.2.2 動態(tài)檢測機制

采摘機械臂路徑規(guī)劃的目的是在已知環(huán)境中規(guī)劃出一條無碰撞的路徑［22］，為軌跡追蹤提供依據(jù)，路徑規(guī)劃的結(jié)果嚴重影響跟蹤控制的精度。在采摘環(huán)境中，隨著機械臂的運動，可以及時的更新已知環(huán)境，但可能會在已規(guī)劃完成的路徑出現(xiàn)新障礙物，從而導致軌跡追蹤失效，在這種情況下就需重新規(guī)劃路徑。對重新規(guī)劃路徑，一般有兩種處理方法：一種是放棄前期所有的路徑規(guī)劃，從起點重新開始規(guī)劃，這種方法使在運動中的機械臂受到影響且搜索過程耗時，尤其在復雜環(huán)境中；另外一種是保留原有無障礙物的路徑，在障礙物前端處繼續(xù)向目標搜索，進行新的路徑規(guī)劃，這種方法不需要從起點開始搜索，可以根據(jù)障礙物的位置及時調(diào)整規(guī)劃的路徑，從而保證運動的穩(wěn)定性，非常適合采摘環(huán)境的路徑規(guī)劃。因此，本文選用第二種方法，其擴展示意如圖4所示。

2 仿真試驗與驗證

2.1 目標導向策略路徑規(guī)劃仿真

仿真試驗電腦配置為：Windows11系統(tǒng)，處理器Intel i5-11400H，CPU主頻2.5 GHz，顯卡RTX3050Ti，機帶RAM 16 GB。

為了驗證本文算法在搜索中目標導向性，將在無障礙與有障礙二維仿真環(huán)境下對基本RRT算法及本文改進算法進行對比驗證。為模擬采摘環(huán)境中障礙物位置的不確定及簡化仿真環(huán)境，本文將障礙物設(shè)置為隨機的圓形。設(shè)定地圖大小為［20，20］，搜索步長為0.5，最大迭代次數(shù)為5 000次，閾值為0.5，目標導向概率為50%，設(shè)置起點為［0，0］，目標點為［15，12］。由于算法在采樣過程有一定隨機性，為減少試驗誤差，所以試驗次數(shù)定為30。對不同算法在不同地圖中進行算法仿真與驗證，如圖5所示。

從圖5中可以看出，未經(jīng)過改進的RRT算法出現(xiàn)了較多分支，這不僅會產(chǎn)生冗余節(jié)點，使迭代次數(shù)增加，還會導致規(guī)劃時間長、路徑長。在引入目標導向策略后，采樣目標性得到了極大的改善。

本文采用30組仿真來減少采樣隨機性試驗的誤差，在仿真中均成功找到規(guī)劃路徑。并根據(jù)仿真試驗數(shù)據(jù)繪出搜索時間與路徑長度對比圖，如圖6所示。

從圖6中可以看出，無論是在無障礙地圖中還是在有障礙地圖中，本文改進算法的時間整體優(yōu)于基本RRT算法，在路徑長度方面也整體優(yōu)于基本RRT算法，具體算法仿真數(shù)據(jù)如表1所示。

由表1可知基本RRT算法在有無障礙的兩種環(huán)境中平均路徑長度為25.1 mm，而本文算法在有無障礙的兩種環(huán)境中平均路徑長度為21.1 mm，路徑長度減少了16%；同時，基本RRT算法在有無障礙的兩種環(huán)境中平均迭代時間為30 s，而本文算法在有無障礙的兩種環(huán)境中兩種環(huán)境中平均迭代時間為4.05 s，迭代時間縮短了86.5%。因此，本文改進算法的迭代速度更快，可以更快地規(guī)劃出更短無碰撞路徑。

2.2 動態(tài)檢測路徑規(guī)劃仿真

對完成規(guī)劃的路徑進行動態(tài)檢測，以確保路徑的可行性，若規(guī)劃完成的路徑有新的障礙物出現(xiàn)，則在新障礙物前方進行重新路徑規(guī)劃，其思路主要可以分為三個步驟：第一步是檢測初始路徑。為了提高路徑的重復使用率，應(yīng)對初始路徑進行動態(tài)檢測。第二步是碰撞檢測。當新增障礙物與初始路徑發(fā)成碰撞時，應(yīng)立即進行重新路徑規(guī)劃。第三步是重新規(guī)劃路徑。刪除障礙物后的路徑，同時利用障礙物之前的初始路徑進行重新搜索，規(guī)劃出新路徑。具體規(guī)劃過程如圖7所示。

通過動態(tài)RRT算法擴展示意圖，可以看出動態(tài)檢測機制對已規(guī)劃完成的路徑進行實時檢測，當有障礙物阻礙初始路徑時，立刻進行新的路徑規(guī)劃，以保證RRT算法適應(yīng)動態(tài)變化的環(huán)境，提高算法的實時性。

3 結(jié)論

1） 針對RRT算法存在隨機性強、搜索效率等不足，引入了目標導向策略，通過一定概率進行采樣，在一定程度上降低算法搜索的隨機性，保證算法向目標點快速搜索，提高搜索效率；同時為使機械臂適應(yīng)動態(tài)采摘環(huán)境，引入動態(tài)檢測機制，實時檢測動態(tài)環(huán)境，能有效地避開動態(tài)障礙物，增強算法動態(tài)環(huán)境的適應(yīng)性，提高算法的實時性。

2） 根據(jù)試驗結(jié)果，與RRT算法相比，改進算法規(guī)劃路徑長度減少了16%。改進算法規(guī)劃迭代的時間縮短了86.5%。改進算法能夠適應(yīng)不確定的運動環(huán)境，為機械臂在不確定環(huán)境中采摘提供依據(jù)。

參 考 文 獻

［1］ 劉繼展. 溫室采摘機器人技術(shù)研究進展分析［J］. 農(nóng)業(yè)機械學報， 2017， 48（12）： 1-18.

Liu Jizhan. Research progress analysis of robotic harvesting technologies in greenhouse ［J］. Transactions of the Chinese Society for Agricultural Machinery， 2017， 48（12）： 1-18.

［2］ 張文翔， 張兵園， 貢宇， 等. 果蔬采摘機器人機械臂研究現(xiàn)狀與展望［J］. 中國農(nóng)機化學報， 2022， 43（9）： 232-237， 244.

Zhang Wenxiang， Zhang Bingyuan， Gong Yu， et al. Research status and prospect of fruit and vegetable picking robot manipulator ［J］. Journal of Chinese Agricultural Mechanization， 2022， 43（9）： 232-237， 244.

［3］ 張潔， 李艷文. 果蔬采摘機器人的研究現(xiàn)狀、問題及對策［J］. 機械設(shè)計， 2010， 27（6）： 1-5.

Zhang Jie， Li Yanwen. Research situation， problems and solutions of fruit-vegetable picking robots ［J］. Journal of Machine Design， 2010， 27（6）： 1-5.

［4］ 張勤， 劉豐溥， 蔣先平， 等. 番茄串收機械臂運動規(guī)劃方法與試驗［J］. 農(nóng)業(yè)工程學報， 2021， 37（9）： 149-156.

Zhang Qin， Liu Fengpu， Jiang Xianping， et al. Motion planning method and experiments of tomato bunch harvesting manipulator ［J］. Transactions of the Chinese Society of Agricultural Engineering， 2021， 37（9）： 149-156.

［5］ 林韓熙， 向丹， 歐陽劍， 等. 移動機器人路徑規(guī)劃算法的研究綜述［J］. 計算機工程與應(yīng)用， 2021， 57（18）： 38-48.

Lin Hanxi， Xiang Dan， Ouyang Jian， et al. Review of path planning algorithms for mobile robots ［J］. Computer Engineering and Applications， 2021， 57（18）： 38-48.

［6］ 代玉梅， 張瑞玲， 馬黎. 改進A*算法的采摘機器人路徑規(guī)劃與跟蹤控制［J］. 中國農(nóng)機化學報， 2022， 43（3）： 138-145.

Dai Yumei， Zhang Ruiling， Ma Li. Path planning and tracking control of picking robot based on improved A* algorithm ［J］. Journal of Chinese Agricultural Mechanization， 2022， 43（3）： 138-145.

［7］ 李廷珍， 招啟軍， 張夏陽， 等. 基于改進人工勢場法的無人直升機三維航跡規(guī)劃［J］. 飛行力學， 2022， 40（1）： 69-75.

Li Tingzhen， Zhao Qijun， Zhang Xiayang， et al. Three-dimensional path planning of unmanned helicopter based on improved artificial potential field method ［J］. Flight Dynamics， 2022， 40（1）： 69-75

［8］ LaValle S M. Rapidly-exploring random trees： A new tool for path planning ［J］. Research Report， 1999.

［9］ LaValle S M， Kuffner Jr J J. Randomized kinodynamic planning ［J］. International Journal of Robotics amp; Research， 2001， 20（5）： 378-400.

［10］ 王碩， 段蓉凱， 廖與禾. 機器人路徑規(guī)劃中快速擴展隨機樹算法的改進研究［J］. 西安交通大學學報， 2022， 56（7）： 1-8.

Wang Shuo， Duan Rongkai， Liao Yuhe. Research on improvement of rapidly exploring random tree in algorithm robot path planning ［J］. Journal of Xi’an Jiaotong University， 2022， 56（7）： 1-8.

［11］ Kuffner J J， Lavalle S M. RRT-connect： An efficient approach to single-query path planning ［C］. Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings （Cat. No. 00CH37065）. IEEE， 2000， 2： 995-1001.

［12］ Karaman S， Walter M R， Perez A， et al. Anytime motion planning using the RRT ［C］. 2011 IEEE International Conference on Robotics and Automation， IEEE， 2011： 1478-1483.

［13］ Gammell J D， Srinivasa S S， Barfoot T D. Informed RRT* optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic ［C］. WIProc of IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway， NJ： IEEE Press， 2014： 2997-3004.

［14］ Gammell J D， Barfoot T D， Srinivasa S S. Batch informed trees （BIT*）： Informed asymptotically optimal anytime search ［J］. International Journal of Robotics Research， 2017， 39（5）： 543-567.

［15］ Cao X， Zou X， Jia C， et al. RRT-based path planning for an intelligent litchi-picking manipulator ［J］. Computers and Electronics in Agriculture， 2019， 156： 105-118.

［16］ 劉頓， 王毅. 改進Informed-RRT*算法的柑橘采摘機械臂運動路徑規(guī)劃［J］. 重慶理工大學學報（自然科學）， 2021， 35（11）： 158-165.

Liu Dun， Wang Yi. Motion path planning of citrus picking robot arm based on improved Informed-RRT* algorithm ［J］. Journal of Chongqing University of Technology （Natural Science）， 2021， 35（11）： 158-165.

［17］ 張勤， 樂曉亮， 李彬， 等. 基于CTB-RRT*的果蔬采摘機械臂運動路徑規(guī)劃［J］. 農(nóng)業(yè)機械學報， 2021， 52（10）： 129-136.

Zhang Qin， Yue Xiaoliang， Li Bin， et al. Motion planning of picking manipulator based on CTB-RRT* algorithm ［J］. Transactions of the Chinese Society for Agricultural Machinery， 2021， 52（10）： 129-136.

［18］ 馬宇豪， 梁雁冰. 一種基于六次多項式軌跡規(guī)劃的機械臂避障算法［J］. 西北工業(yè)大學學報， 2020， 38（2）： 392-400.

Ma Yuhao， Liang Yanbing. An obstacle avoidance algorithm for manipulators based on six-order polynomial trajectory planning ［J］. Journal of Northwestern Polytechnical University， 2020， 38（2）： 392-400.

［19］ 崔永杰， 王寅初， 何智， 等. 基于改進RRT算法的獼猴桃采摘機器人全局路徑規(guī)劃［J］. 農(nóng)業(yè)機械學報， 2022， 53（6）： 151-158.

Cui Yongjie， Wang Yinchu， He Zhi， et al. Global path planning of kiwifruit harvesting robot based on improved RRT algorithm ［J］. Transactions of the Chinese Society for Agricultural Machinery， 2022， 53（6）： 151-158.

［20］ 陳秋蓮， 蔣環(huán)宇， 鄭以君. 機器人路徑規(guī)劃的快速擴展隨機樹算法綜述［J］. 計算機工程與應(yīng)用， 2019， 55（16）： 10-17.

Chen Qiulian， Jiang Huanyu， Zheng Yijun. Summary of rapidly-exploring random tree algorithm in robot path planning ［J］. Computer Engineering and Applications， 2019， 55（16）： 10-17.

［21］ Li B， Chen B. An adaptive rapidly-exploring random tree ［J］. IEEE/CAA Journal of Automatica Sinica， 2021， 9（2）： 283-294.

［22］ 王懷震， 高明， 王建華， 等. 基于改進RRT*-Connect算法的機械臂多場景運動規(guī)劃［J］. 農(nóng)業(yè)機械學報， 2022， 53（4）： 432-440.

Wang Huaizhen， Gao Ming， Wang Jianhua， et al. Multi-scene fast motion planning of manipulator based on improved RRT*-Connect algorithm ［J］. Transactions of the Chinese Society for Agricultural Machinery， 2022， 53（4）： 432-440.