基于改进RRT的筒子纱搬运机械臂路径规划研究

李文杰 赵地 杜玉红 邴志刚

摘 要：针对筒子纱搬运过程中存在纱线架、放置平台、其他筒子纱等多障碍物的环境下，传统的目标偏置RRT容易陷入局部最优的问题，提出一种改进的RRT（rapidly-exploring random tree）算法。通过几何包络法建立碰撞检测模型，并添加距离系数g，使机械臂末端与障碍物保持安全距离；提出随机动作选择策略，使树每一次扩展前对扩展动作进行选择；提出弱化目标偏置策略，根据Xgoal相对于Xnear和Xrand连线的位置，对扩展方向进行偏置；根据垂距限值法和三次样条插值对初始路径处理，得到一条平滑路径。通过与RRT算法、M-RRT算法、添加引力系数的RRT算法和改进的RRT*算法仿真对比，路径长度方面最大下降了23.3％，时间方面最大下降了82.5%，并始终与障碍物保持50 mm以上的距离。结果表明该算法提升了机械臂搬运的效率和安全性。

关键词：机械臂； 快速扩展随机树； 路径规划； 目标偏置； 筒子纱

中图分类号：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 引言

如今，機械臂在纺织行业发挥的作用越来越大，代替工人工作的场景逐渐增多，不仅降低了工人的工作强度，同时提高了过程中的精度和速度。筒子纱的替换由手工搬运发展为机械臂搬运，整个运行过程存在较多干涉碰撞问题，例如存在筒子纱、放置平台、纱线架等障碍物，为了提高效率且要求机械臂工作时间短、精度高、无碰撞等，搬运筒子纱环境下的避障路径规划成为目前研究热点和难点。

随着计算机技术发展，许多学者对路径规划做了深入研究［1～3］，其中为有效解决机械臂的避障路径规划问题，快速扩展随机树算法和人工势场算法被引入机械臂控制领域并得到了充分的研究和发展。Khitab［4］在1985年提出人工势场法，规划出的路径质量高，但在复杂的环境中，极易陷入局部最优且伴随振荡。Lavalle［5］在1998年提出快速扩展随机树算法，是一种基于随机采样的算法，在多障碍物环境下有优秀的表现，但搜索时的随机性强、效率低。很多学者对快速扩展随机树算法进行了改进。Urmson等人［6］在2003年提出了一种具有节点偏置概率的hRRT算法，这种算法运用了启发式的搜索计算节点偏置概率，对于符合条件的节点使其偏向目标扩展。Kalisiak等人［7］在2006年提出RRT-blossom算法，通过回归约束函数产生新的节点后使得随机树降低重复区域探索的概率。Karaman等人［8］在2010年提出一种通过对父亲节点重新选择并进行剪枝优化的算法，但效率过低。Nasir等人［9］在2013年提出了RRT*-smart算法，在RRT*的基础上处理完冗余点后，将靠近障碍物的节点设定为信标点，定义偏置率B，在周围偏置采样。Jordan等人［10］在2013年提出基于启发式技术改进的B-RRT*，通过降低节点扩展过程的计算量，达到快速收敛的效果。Adiyatov等人［11］在2013年对RRT*算法的智能化采样以及路径代价方面进行了改进，提出RRT*Smart算法，一定程度上提高了算法的收敛速度。Tahir等人［12］在2018年通过容许启发形式产生新的节点，提出了bidirectional RRT*。刘恩海等人［13］在2019年引入引力系数，扩展时受引力的影响。李季等人［14］在2020年引入权重系数，增加了朝目标位置扩展的分量。李扬等人［15］在2021年提出目标点按一定概率作为采样点的算法。张建冬等人［16］在2021年提出了添加引力权重系数的RRT算法，引入引力权重系数对扩展方向偏置，使快速向目标方向生长。文献［13～16］中提出RRT的改进，集中于加强对目标点导向，使算法能快速朝目标点前进，但带来的问题是在复杂障碍物环境下，特别是目标点附近存在较多障碍物时，随机树在某处陷入局部最优，因此需进一步研究。张振等人［17］在2022年提出基于约束采样的RRT，通过对重复性区域减少采样和对采样区域进行动态大小调整，提高了算法的收敛速度和节点利用率，虽然减少了重复采样区域探索，但对有效节点的发现不利，在复杂障碍物环境下，不利于快速找到有效路径。张婷婷等人［18］在2022年提出M-RRT，在新节点扩展中引入系数，在系数的作用下生成向目标点扩展的节点。张兰勇等人［19］在2023年提出针对水下机器人集群控制问题，将目标偏置函数加入RRT*的算法。文献［18，19］中学者通过对采样点的约束使算法能朝着目标快速前进，其中文献［19］对RRT*加入目标偏置函数，RRT*含有重选父节点和重新布线两步操作，加入目标偏置会出现明显的时间过长问题。

针对上述问题，本文提出一种基于改进RRT算法的机械臂避障路径规划方法。添加距离系数使机械臂末端与障碍物保持一定的距离；在扩展过程中通过对动作的随机选择，即保留随机性，又依据Xgoal相对于Xnear的位置，对扩展方向进行偏置；通过垂距限值法和三次样条插值处理初始路径获得平滑的搬运轨迹，并通过仿真验证，证明了改进算法的有效性。

1 筒子纱搬运机械臂运动学模型

1.1 基于改进的D-H法建立连杆坐标系

改进的D-H法［20，21］常用于建立多关节机器人的数学模型，是一种对连杆坐标系的描述。机械臂是多个关节连接起来的连杆模型，连杆的功能在于连接前后两端的关节，连杆的特性主要由其轴线位置确定。改进的D-H法用四个参数描述相邻坐标系之间的变化，即连杆长度a、连杆扭转角α、连杆偏移量d和关节角θ。本文筒子纱搬运机械臂研究对象为EFORT的ER20-1700六自由度机械臂，机械臂工作环境及工作流程如图1所示。工作流程为：从A点取新筒子纱，放到平台B上，从纱线架C处抓取用完的筒子纱放到放置平台D处，即可完成一次空筒子纱的拆卸和替换的准备工作，之后机械臂将平台B处新筒子纱抓取到纱线架上即可完成筒子纱替换工作。机械臂D-H参数如表1所示。

3 仿真实验及结果分析

仿真实验基于MATLAB R2021b平台，计算机配置为Windows 10操作系统，处理器为Intel Core i7-10870H，运行内存为16 GB。为验证所提算法的有效性，对文献［5］提出的RRT算法、M-RRT算法文献［16］提出的添加引力系数的RRT算法、文献［19］提出的改进的RRT*算法和本文改进的RRT算法进行仿真对比实验。图10为搬运工作环境示意图，表2为环境参数。

参数如下：最大迭代次数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和其他算法的仿真数据如表3所示。

由表3可知，改进的RRT算法在时间和路径长度方面都优于其他四种算法，在相同的最大迭代条件下，以更少的扩展节点和更高的节点利用率完成筒子纱搬運任务。时间分别下降了69.1%、60.7%、55.7%、79.1%，扩展节点分别下降了66.1%、58.2%、53.8%、66.7%，扩展节点个数的下降，表示算法以更少的扩展点、更短的时间寻找到有效路径，提升了工作效率。为进一步验证算法的可靠性和有效性，分别对五种算法进行30次仿真实验。五种算法的路径长度仿真结果如图13所示，仿真的平均数据如表4所示。

由图13和表4可知，改进的RRT在路径规划过程中，路径长度相对于其他四种算法有了不同程度的下降，最低点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%，平均扩展节点数分别下降了64.7%、56.9%、49.5%、65.1%。经此验证，改进的算法能在1 s左右完成路径规划任务，相较其他四种算法，能以更短时间完成筒子纱搬运的工作。

初始路径存在较多冗余点，不利于机械臂的搬运工作，产生抖动，加大能耗，关节变化不平滑，因此对路径进一步处理，路径处理前后关节变化对比如图15所示。为验证平滑后关节波动程度，以关节3为例进行分析。由图16可知，处理后，初始路径上的冗余点被剔除，关节变化更流畅，波动更小，极差下降了76.9％，减小了机械臂的抖动，保证工作各个环节的平稳作业。

机械臂末端与各个障碍物之间的距离变化如图17所示。由图17圆圈位置可知，最低点为第2 800个路径点，距离障碍物7的距离为80.3 mm。经此验证，改进的RRT算法能使机械臂在空间内安全搬运。

综合上述仿真实验可知，改进的RRT算法能在0.5～1.5 s内完成路径规划任务，规划出一条平滑且与障碍物保持50 mm以上距离的路径，适合筒子纱搬運机械臂工作，提高了算法的收敛速度和机械臂的搬运效率。

4 结束语

为实现在含有多个筒子纱、放置平台、纱线架等多障碍物环境中高效工作，快速规划出有效路径，本文提出一种改进的算法：通过几何包络法建立碰撞检测模型，在碰撞检测中添加距离系数g，保证了机械臂末端与障碍物保持50 mm以上的距离；提出随机动作选择策略和弱化目标偏置策略，提高算法搜索效率，解决了在单一扩展方式下带来的盲目性和局部最优问题；通过垂距限值法修剪和三次样条插值处理，得到一条光滑路径，更适合机械臂的搬运工作；通过与RRT算法、M-RRT算法、添加引力系数的RRT算法、改进的RRT*算法仿真对比，在时间方面最大下降82.5%，在路径长度方面最大下降23.3%，验证了改进RRT算法的有效性和可靠性。但该研究仍有不足，仅对节点扩展方向进行了处理，未考虑步长大小对整体的影响，对此应进行下一步的研究完善。

参考文献：

［1］张贝， 闵华松， 张新明. 改进海洋捕食者算法和插值平滑的机器人路径规划［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］仲健宁， 向国菲， 佃松宜. 针对包含狭窄通道复杂环境的高效RRT*路径规划算法［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］司徒华杰， 雷海波， 庄春刚. 动态环境下基于人工势场引导的RRT路径规划算法［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路径规划算法［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］李季， 史晨发， 邵磊， 等. 基于改进RRT算法的6-DOF机器人路径规划［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样条的机械臂运动规划方法［J］. 计算机集成制造系统， 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算法的移动机械臂路径规划［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的机械臂运动规划［J］. 计算机集成制造系统， 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的关节机械臂路径规划［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］张兰勇， 韩宇. 基于改进的RRT*算法的AUV集群路径规划研究［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-），男，山西吕梁人，硕士研究生，主要研究方向为机械臂的路径规划；赵地（1987-），男（通信作者），天津人，讲师，博士研究生，主要研究方向为移动机器人路径规划（zhaodi@tiangong.edu.cn）；杜玉红（1974-），女，教授，博导，博士，主要研究方向为图像处理及模式识别；邴志刚（1972-），男，教授，硕导，博士，主要研究方向为海陆空机器人、数字孪生仿真．