不確定采摘環境下改進RRT算法的機械臂路徑規劃研究

摘要：

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

關鍵詞：果蔬采摘；機械臂；快速擴展隨機樹；動態避障；目標導向；動態檢測；路徑規劃

中圖分類號：S23； TP241

文獻標識碼：A

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

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

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

第一作者、通訊作者：李曉娟，女，1987年生，烏魯木齊人，博士，副教授，碩導；研究方向農牧特種機器人。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 引言

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

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

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

1 RRT算法研究及改進

1.1 RRT算法原理

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

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

1.2 改進RRT算法

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

1.2.1 目標導向策略

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

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

（1）

xrand=

xgoalif P＞Prand

Sampleelse

式中：

l——擴展步長；

xnew——新擴展節點；

xnear——上一擴展節點；

xrand——隨機擴展節點；

‖xrand-xnear‖——

xrand與xnear之間的歐氏距離。

1.2.2 動態檢測機制

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

2 仿真試驗與驗證

2.1 目標導向策略路徑規劃仿真

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

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

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

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

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

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

2.2 動態檢測路徑規劃仿真

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

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

3 結論

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

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

參 考 文 獻

［1］ 劉繼展. 溫室采摘機器人技術研究進展分析［J］. 農業機械學報， 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］ 張文翔， 張兵園， 貢宇， 等. 果蔬采摘機器人機械臂研究現狀與展望［J］. 中國農機化學報， 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］ 張潔， 李艷文. 果蔬采摘機器人的研究現狀、問題及對策［J］. 機械設計， 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］ 張勤， 劉豐溥， 蔣先平， 等. 番茄串收機械臂運動規劃方法與試驗［J］. 農業工程學報， 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］ 林韓熙， 向丹， 歐陽劍， 等. 移動機器人路徑規劃算法的研究綜述［J］. 計算機工程與應用， 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*算法的采摘機器人路徑規劃與跟蹤控制［J］. 中國農機化學報， 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］ 李廷珍， 招啟軍， 張夏陽， 等. 基于改進人工勢場法的無人直升機三維航跡規劃［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］ 王碩， 段蓉凱， 廖與禾. 機器人路徑規劃中快速擴展隨機樹算法的改進研究［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*算法的柑橘采摘機械臂運動路徑規劃［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*的果蔬采摘機械臂運動路徑規劃［J］. 農業機械學報， 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］ 馬宇豪， 梁雁冰. 一種基于六次多項式軌跡規劃的機械臂避障算法［J］. 西北工業大學學報， 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算法的獼猴桃采摘機器人全局路徑規劃［J］. 農業機械學報， 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］ 陳秋蓮， 蔣環宇， 鄭以君. 機器人路徑規劃的快速擴展隨機樹算法綜述［J］. 計算機工程與應用， 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算法的機械臂多場景運動規劃［J］. 農業機械學報， 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.