基于改進RRT的筒子紗搬運機械臂路徑規(guī)劃研究

李文杰 趙地 杜玉紅 邴志剛

摘 要：針對筒子紗搬運過程中存在紗線架、放置平臺、其他筒子紗等多障礙物的環(huán)境下，傳統(tǒng)的目標偏置RRT容易陷入局部最優(yōu)的問題，提出一種改進的RRT（rapidly-exploring random tree）算法。通過幾何包絡法建立碰撞檢測模型，并添加距離系數(shù)g，使機械臂末端與障礙物保持安全距離；提出隨機動作選擇策略，使樹每一次擴展前對擴展動作進行選擇；提出弱化目標偏置策略，根據(jù)Xgoal相對于Xnear和Xrand連線的位置，對擴展方向進行偏置；根據(jù)垂距限值法和三次樣條插值對初始路徑處理，得到一條平滑路徑。通過與RRT算法、M-RRT算法、添加引力系數(shù)的RRT算法和改進的RRT*算法仿真對比，路徑長度方面最大下降了23.3％，時間方面最大下降了82.5%，并始終與障礙物保持50 mm以上的距離。結果表明該算法提升了機械臂搬運的效率和安全性。

關鍵詞：機械臂； 快速擴展隨機樹； 路徑規(guī)劃； 目標偏置； 筒子紗

中圖分類號：TP242?? 文獻標志碼：A

文章編號：1001-3695（2023）09-016-2663-06

doi：10.19734/j.issn.1001-3695.2023.02.0017

Research on path planning of package yarn handling

manipulator based on improved RRT

Li Wenjie1a，1b， Zhao Di1a，1b， Du Yuhong1a，1b， Bing Zhigang2

（1. a.College of Mechanical Engineering， b.Tianjin Key Laboratory of Modern Electromechanical Equipment， Tianjin University of Technology， Tianjin 300387，China; 2.School of Automation & Electrical Engineering， Tianjin Vocational & Technical Normal University， Tianjin 300350，China）

Abstract：This paper proposed an improved RRT algorithm to solve the problem that the traditional target offset RRT was easy to fall into local optimum in the environment where there were many obstacles such as yarn racks， placing platforms and other packages during the package handling process. It established the collision detection model by geometric envelope method， and added the distance coefficient g to keep a safe distance between the end of the manipulator and the obstacle. It proposed a random action selection strategy to select the expansion action before each expansion of the tree， and proposed a weakening target offset strategy to offset the extension direction according to the position of Xgoal relative to the Xnear and Xrand line. According to the vertical distance limit method and cubic spline interpolation，it optimized the initial path， and finally obtained a smooth path. Through simulation comparison with RRT algorithm， M-RRT algorithm， RRT algorithm with added gravity coefficient and improved RRT* algorithm， the maximum decrease in path length was 23.3%， the maximum decrease in time was 82.5%， and the distance from obstacles was always more than 50 mm. The results show that the algorithm improves the efficiency and safety of the manipulator handling.

Key words：mechanical ARM; rapidly-exploring random tree; path planning; goal bias; bobbin yarn

0 引言

如今，機械臂在紡織行業(yè)發(fā)揮的作用越來越大，代替工人工作的場景逐漸增多，不僅降低了工人的工作強度，同時提高了過程中的精度和速度。筒子紗的替換由手工搬運發(fā)展為機械臂搬運，整個運行過程存在較多干涉碰撞問題，例如存在筒子紗、放置平臺、紗線架等障礙物，為了提高效率且要求機械臂工作時間短、精度高、無碰撞等，搬運筒子紗環(huán)境下的避障路徑規(guī)劃成為目前研究熱點和難點。

隨著計算機技術發(fā)展，許多學者對路徑規(guī)劃做了深入研究［1～3］，其中為有效解決機械臂的避障路徑規(guī)劃問題，快速擴展隨機樹算法和人工勢場算法被引入機械臂控制領域并得到了充分的研究和發(fā)展。Khitab［4］在1985年提出人工勢場法，規(guī)劃出的路徑質量高，但在復雜的環(huán)境中，極易陷入局部最優(yōu)且伴隨振蕩。Lavalle［5］在1998年提出快速擴展隨機樹算法，是一種基于隨機采樣的算法，在多障礙物環(huán)境下有優(yōu)秀的表現(xiàn)，但搜索時的隨機性強、效率低。很多學者對快速擴展隨機樹算法進行了改進。Urmson等人［6］在2003年提出了一種具有節(jié)點偏置概率的hRRT算法，這種算法運用了啟發(fā)式的搜索計算節(jié)點偏置概率，對于符合條件的節(jié)點使其偏向目標擴展。Kalisiak等人［7］在2006年提出RRT-blossom算法，通過回歸約束函數(shù)產(chǎn)生新的節(jié)點后使得隨機樹降低重復區(qū)域探索的概率。Karaman等人［8］在2010年提出一種通過對父親節(jié)點重新選擇并進行剪枝優(yōu)化的算法，但效率過低。Nasir等人［9］在2013年提出了RRT*-smart算法，在RRT*的基礎上處理完冗余點后，將靠近障礙物的節(jié)點設定為信標點，定義偏置率B，在周圍偏置采樣。Jordan等人［10］在2013年提出基于啟發(fā)式技術改進的B-RRT*，通過降低節(jié)點擴展過程的計算量，達到快速收斂的效果。Adiyatov等人［11］在2013年對RRT*算法的智能化采樣以及路徑代價方面進行了改進，提出RRT*Smart算法，一定程度上提高了算法的收斂速度。Tahir等人［12］在2018年通過容許啟發(fā)形式產(chǎn)生新的節(jié)點，提出了bidirectional RRT*。劉恩海等人［13］在2019年引入引力系數(shù)，擴展時受引力的影響。李季等人［14］在2020年引入權重系數(shù)，增加了朝目標位置擴展的分量。李揚等人［15］在2021年提出目標點按一定概率作為采樣點的算法。張建冬等人［16］在2021年提出了添加引力權重系數(shù)的RRT算法，引入引力權重系數(shù)對擴展方向偏置，使快速向目標方向生長。文獻［13～16］中提出RRT的改進，集中于加強對目標點導向，使算法能快速朝目標點前進，但帶來的問題是在復雜障礙物環(huán)境下，特別是目標點附近存在較多障礙物時，隨機樹在某處陷入局部最優(yōu)，因此需進一步研究。張振等人［17］在2022年提出基于約束采樣的RRT，通過對重復性區(qū)域減少采樣和對采樣區(qū)域進行動態(tài)大小調整，提高了算法的收斂速度和節(jié)點利用率，雖然減少了重復采樣區(qū)域探索，但對有效節(jié)點的發(fā)現(xiàn)不利，在復雜障礙物環(huán)境下，不利于快速找到有效路徑。張婷婷等人［18］在2022年提出M-RRT，在新節(jié)點擴展中引入系數(shù)，在系數(shù)的作用下生成向目標點擴展的節(jié)點。張?zhí)m勇等人［19］在2023年提出針對水下機器人集群控制問題，將目標偏置函數(shù)加入RRT*的算法。文獻［18，19］中學者通過對采樣點的約束使算法能朝著目標快速前進，其中文獻［19］對RRT*加入目標偏置函數(shù)，RRT*含有重選父節(jié)點和重新布線兩步操作，加入目標偏置會出現(xiàn)明顯的時間過長問題。

針對上述問題，本文提出一種基于改進RRT算法的機械臂避障路徑規(guī)劃方法。添加距離系數(shù)使機械臂末端與障礙物保持一定的距離；在擴展過程中通過對動作的隨機選擇，即保留隨機性，又依據(jù)Xgoal相對于Xnear的位置，對擴展方向進行偏置；通過垂距限值法和三次樣條插值處理初始路徑獲得平滑的搬運軌跡，并通過仿真驗證，證明了改進算法的有效性。

1 筒子紗搬運機械臂運動學模型

1.1 基于改進的D-H法建立連桿坐標系

改進的D-H法［20，21］常用于建立多關節(jié)機器人的數(shù)學模型，是一種對連桿坐標系的描述。機械臂是多個關節(jié)連接起來的連桿模型，連桿的功能在于連接前后兩端的關節(jié)，連桿的特性主要由其軸線位置確定。改進的D-H法用四個參數(shù)描述相鄰坐標系之間的變化，即連桿長度a、連桿扭轉角α、連桿偏移量d和關節(jié)角θ。本文筒子紗搬運機械臂研究對象為EFORT的ER20-1700六自由度機械臂，機械臂工作環(huán)境及工作流程如圖1所示。工作流程為：從A點取新筒子紗，放到平臺B上，從紗線架C處抓取用完的筒子紗放到放置平臺D處，即可完成一次空筒子紗的拆卸和替換的準備工作，之后機械臂將平臺B處新筒子紗抓取到紗線架上即可完成筒子紗替換工作。機械臂D-H參數(shù)如表1所示。

3 仿真實驗及結果分析

仿真實驗基于MATLAB R2021b平臺，計算機配置為Windows 10操作系統(tǒng)，處理器為Intel Core i7-10870H，運行內存為16 GB。為驗證所提算法的有效性，對文獻［5］提出的RRT算法、M-RRT算法文獻［16］提出的添加引力系數(shù)的RRT算法、文獻［19］提出的改進的RRT*算法和本文改進的RRT算法進行仿真對比實驗。圖10為搬運工作環(huán)境示意圖，表2為環(huán)境參數(shù)。

參數(shù)如下：最大迭代次數(shù)10 000，g為50，步長15，μ1、μ2為2、6，不同ε取值下的尋路時長熱力圖如圖11所示，在0.1～0.3內具有更短的時長，且當L≤D/10時，ε=0.1，當L＞D/10，ε=0.2時具有最短的時間。一次仿真結果如圖12所示，改進RRT和其他算法的仿真數(shù)據(jù)如表3所示。

由表3可知，改進的RRT算法在時間和路徑長度方面都優(yōu)于其他四種算法，在相同的最大迭代條件下，以更少的擴展節(jié)點和更高的節(jié)點利用率完成筒子紗搬運任務。時間分別下降了69.1%、60.7%、55.7%、79.1%，擴展節(jié)點分別下降了66.1%、58.2%、53.8%、66.7%，擴展節(jié)點個數(shù)的下降，表示算法以更少的擴展點、更短的時間尋找到有效路徑，提升了工作效率。為進一步驗證算法的可靠性和有效性，分別對五種算法進行30次仿真實驗。五種算法的路徑長度仿真結果如圖13所示，仿真的平均數(shù)據(jù)如表4所示。

由圖13和表4可知，改進的RRT在路徑規(guī)劃過程中，路徑長度相對于其他四種算法有了不同程度的下降，最低點6 174.1 mm，最高點6 352.6 mm，平均路徑長度6 284.4 mm，相對于其他四種算法平均路徑長度分別下降了4.4%、9.8%、23.3%、4.5%。因此，改進的RRT算法能以較短的路徑完成任務。

路徑時間的仿真結果如圖14所示。由圖14和表4可知，改進的RRT算法相對于其他四種算法在路徑時間上有了明顯的下降，最短時間0.86 s，最長時間1.27 s，平均時間1.03 s，時間方面分別下降了69.7%、61.5%、58.3%、82.5%，平均擴展節(jié)點數(shù)分別下降了64.7%、56.9%、49.5%、65.1%。經(jīng)此驗證，改進的算法能在1 s左右完成路徑規(guī)劃任務，相較其他四種算法，能以更短時間完成筒子紗搬運的工作。

初始路徑存在較多冗余點，不利于機械臂的搬運工作，產(chǎn)生抖動，加大能耗，關節(jié)變化不平滑，因此對路徑進一步處理，路徑處理前后關節(jié)變化對比如圖15所示。為驗證平滑后關節(jié)波動程度，以關節(jié)3為例進行分析。由圖16可知，處理后，初始路徑上的冗余點被剔除，關節(jié)變化更流暢，波動更小，極差下降了76.9％，減小了機械臂的抖動，保證工作各個環(huán)節(jié)的平穩(wěn)作業(yè)。

機械臂末端與各個障礙物之間的距離變化如圖17所示。由圖17圓圈位置可知，最低點為第2 800個路徑點，距離障礙物7的距離為80.3 mm。經(jīng)此驗證，改進的RRT算法能使機械臂在空間內安全搬運。

綜合上述仿真實驗可知，改進的RRT算法能在0.5～1.5 s內完成路徑規(guī)劃任務，規(guī)劃出一條平滑且與障礙物保持50 mm以上距離的路徑，適合筒子紗搬運機械臂工作，提高了算法的收斂速度和機械臂的搬運效率。

4 結束語

為實現(xiàn)在含有多個筒子紗、放置平臺、紗線架等多障礙物環(huán)境中高效工作，快速規(guī)劃出有效路徑，本文提出一種改進的算法：通過幾何包絡法建立碰撞檢測模型，在碰撞檢測中添加距離系數(shù)g，保證了機械臂末端與障礙物保持50 mm以上的距離；提出隨機動作選擇策略和弱化目標偏置策略，提高算法搜索效率，解決了在單一擴展方式下帶來的盲目性和局部最優(yōu)問題；通過垂距限值法修剪和三次樣條插值處理，得到一條光滑路徑，更適合機械臂的搬運工作；通過與RRT算法、M-RRT算法、添加引力系數(shù)的RRT算法、改進的RRT*算法仿真對比，在時間方面最大下降82.5%，在路徑長度方面最大下降23.3%，驗證了改進RRT算法的有效性和可靠性。但該研究仍有不足，僅對節(jié)點擴展方向進行了處理，未考慮步長大小對整體的影響，對此應進行下一步的研究完善。

參考文獻：

［1］張貝， 閔華松， 張新明. 改進海洋捕食者算法和插值平滑的機器人路徑規(guī)劃［J］. 計算機應用研究， 2023，40（7）：2082-2089. （Zhang Bei， Min Huasong， Zhang Xinming. Robot path planning with improved marine predator algorithm and interpolation smoothing［J］. Application Research of Computers， 2023，40（7）：2082-2089.）

［2］仲健寧， 向國菲， 佃松宜. 針對包含狹窄通道復雜環(huán)境的高效RRT*路徑規(guī)劃算法［J］. 計算機應用研究， 2021，38（8）： 2308-2314. （Zhong Jianning， Xiang Guofei， Tian Songyi. Efficient RRT* path planning algorithm for complex environment including narrow channels［J］. Application Research of Computers， 2021，38（8）： 2308-2314.）

［3］司徒華杰， 雷海波， 莊春剛. 動態(tài)環(huán)境下基于人工勢場引導的RRT路徑規(guī)劃算法［J］. 計算機應用研究， 2021，38（3）：714-717， 724. （Situ Huajie， Lei Haibo， Zhuang Chungang. RRT path planning algorithm based on artificial potential field guidance in dynamic environment［J］. Application Research of Computers， 2021，38（3）： 714-717，724.）

［4］Khatib O. Real-time obstacle avoidance for manipulators and mobile robots［C］//Proc of IEEE International Conference on Robotics and Automation. Piscataway， NJ： IEEE Press， 1985： 500-505.

［5］Lavalle S M. Rapidly-exploring random tress： a new tool for path planning［R］.［S.l.］：Iowa State University， 1998. ［6］Urmson C，Simmons R. Approaches for heuristically biasing RRT growth［C］//Proc of IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway，NJ： IEEE Press，2003：1178-1183.

［7］Kalisiak M， Panne M. RRT-blossom： RRT with a local flood-fill behavior［C］//Proc of IEEE International Conference on Robotics and Automation. Piscataway， NJ： IEEE Press， 2006：1237-1242.

［8］Karaman S， Frazzoli E. Incremental sampling-based algorithms for optimal motion planning［J］. The International Journal of Robo-tics Research， 2011，30（7）： 846-894.

［9］Nasir J， Islam F， Malik U， et al. RRT*-SMART： a rapid convergence implementation of RRT［J］. International Journal of Advanced Robotic Systems， 2013，10（4）.

［10］Jordan M， Perez A. Optimal bidirectional rapidly-exploring random trees， MIT-CSAIL-TR-2013-021［R］.［S.l.］：CSAIL MIT， 2013.

［11］Adiyatov O， Varol H A. Rapidly-exploring random tree based memory efficient motion planning［C］//Proc of IEEE International Conference on Mechatronics and Automation. Piscataway， NJ： IEEE Press， 2013： 354-359.

［12］Tahir Z， Qureshi A H， Ayaz Y， et al. Potentially guided bidirectionalized RRT* for fast optimal path planning in cluttered environments［J］. Robotics and Autonomous Systems， 2018，108： 13-27.

［13］劉恩海， 高文斌， 孔瑞平， 等. 改進的RRT路徑規(guī)劃算法［J］. 計算機工程與設計， 2019， 40（8）： 2253-2258. （Liu Enhai， Gao Wenbin， Kong Ruiping， et al. Improved RRT path planning algorithm［J］. Computer Engineering and Design， 2019，40（8）： 2253-2258.）

［14］李季， 史晨發(fā)， 邵磊， 等. 基于改進RRT算法的6-DOF機器人路徑規(guī)劃［J］. 計算機應用與軟件， 2020，37（9）： 221-226. （Li Ji， Shi Chenfa， Shao Lei， et al. 6-DOF robot path planning based on improved RRT algorithm［J］. Computer Applications and Software， 2020，37（9）： 221-226.）

［15］李揚， 張蕾， 李鵬飛， 等. 基于改進RRT結合B樣條的機械臂運動規(guī)劃方法［J］. 計算機集成制造系統(tǒng)， 2023， 29（1）：254-263. （Li Yang， Zhang Lei， Li Pengfei， et al. Manipulator motion planning method based on improved RRT and B-spline［J］. Computer Integrated Manufacturing System， 2023，29（1）： 254-263.）

［16］張建冬， 王東， 馬立東， 等. 基于改進RRT算法的移動機械臂路徑規(guī)劃［J］. 電子測量技術， 2021，44（23）： 48-53. （Zhang Jiandong， Wang Dong， Ma Lidong， et al. Path planning of mobile mani-pulator based on improved RRT algorithm［J］. Electronic Measurement Technology， 2021，44（23）： 48-53.）

［17］張振， 李新宇， 董昊臻， 等. 基于約束采樣RRT的機械臂運動規(guī)劃［J］. 計算機集成制造系統(tǒng)， 2022，28（6）： 1616-1626. （Zhang Zhen， Li Xinyu， Dong Haozhen， et al. Manipulator motion planning based on constraint sampling RRT［J］. Computer Integrated Ma-nufacturing System， 2022，28（6）： 1616-1626.）

［18］張婷婷， 柳林燕， 汪惠芬. 基于改進RRT的關節(jié)機械臂路徑規(guī)劃［J］. 組合機床與自動化加工技術， 2022（5）：11-14. （Zhang Tingting， Liu Linyan， Wang Huifen. Path planning of joint manipulator based on improved RRT［J］. Modular Machine Tools and Automatic Processing Technology， 2022（5）： 11-14.）

［19］張?zhí)m勇， 韓宇. 基于改進的RRT*算法的AUV集群路徑規(guī)劃研究［J］. 中國艦船研究， 2022， 18（1）： 43-51. （Zhang Lanyong， Han Yu. Research on AUV cluster path planning based on improved RRT* algorithm［J］. China Ship Research， 2022，18（1）： 43-51.）

［20］Chen Kai， Zhan Kai， Yang Xiaocong， et al. Accuracy improvement method of a 3D laser scanner based on the DH model［J］. Shock and Vibration， 2021，2021： 1-9.

［21］Guo Fayong， Cai Hao， Ceccarelli M， et al. Enhanced DH： an improved convention for establishing a robot link coordinate system fixed on the joint［J］. Industrial Robot： the International Journal of Robotics Research and Application， 2020，47（2）： 197-205.

［22］Shou Huahao， Hu Liangchen， Fang Shiaofen. Progressive iterative approximation of non-uniform cubic B-spline curves and surfaces via successive over-relaxation iteration［J］. Mathematics， 2022，10（20）： 3766.

收稿日期：2023-02-03；修回日期：2023-03-24? 基金項目：天津市科技計劃資助項目（20YDTPJC00740）；江蘇省重點實驗室開放基金資助項目（HGAMTL-1811）

作者簡介：李文杰（1998-），男，山西呂梁人，碩士研究生，主要研究方向為機械臂的路徑規(guī)劃；趙地（1987-），男（通信作者），天津人，講師，博士研究生，主要研究方向為移動機器人路徑規(guī)劃（zhaodi@tiangong.edu.cn）；杜玉紅（1974-），女，教授，博導，博士，主要研究方向為圖像處理及模式識別；邴志剛（1972-），男，教授，碩導，博士，主要研究方向為海陸空機器人、數(shù)字孿生仿真．