冗余绳索驱动蛇形机器人的轨迹规划研究

摘 要：传统机械臂无法进入狭窄封闭的空间作业，冗余绳索驱动机器人有很好的灵活性，因此在狭小封闭空间应用潜力巨大。针对冗余绳索驱动机械臂在狭窄空间作业时的避障问题，提出一套轨迹规划算法。该算法利用搜索算法得到离散模式下的初始规划路径，等间隔法选取初始规划路径的关键点，基于三次B样条曲线，将关键点生成可行路径。通过仿真实验，验证了所提出算法的可行性和有效性。

关键词：绳索驱动；蛇形机器人；轨迹规划

中图分类号：TP242.2" 文献标志码：B" 文章编号：1671-5276（2024）05-0251-04

Research on Trajectory Planning of Snake Robot Driven by Redundant Rope

Abstract：Redundant rope-driven robot， flexible and different to traditional manipulator which is incabable of entering the narrow and closed space for operation， has potentiality for application in a narrow and closed space. To cope with the obstacle avoidance trouble of redundant cable-driven manipulator operating in a narrow space， a set of trajectory planning algorithm is proposed， which implements the search algorithm to obtain the initial planning path in discrete mode， selects the key points of the initial planning path by the equal interval method， and generates a feasible path from the key points based on the cubic B-spline curve. The simulation experiments verify the feasibility and effectiveness of the proposed algorithm.

Keywords：rope drive；snake robot；trajectory planning

0 引言

冗余绳索驱动机器人具有轻量化、机电分离等特点，在狭小空间中有着很大优势。路径规划是实现机器人控制的关键技术之一［1］。由于冗余自由度的存在，冗余绳索驱动机器人的路径规划变得困难。目前对冗余机械臂的路径规划问题已经取得了一些成果，求解路径规划的方法主要包括人工势场法［2-3］和人工智能启发式搜索算法，如A*算法［4］、Dijkstra算法、遗传算法［5-6］和蚁群算法。KHATIB等［7］提出了一种利用人工势场法满足机械臂内部关节约束。宫金良等［8］利用Dijkstra算法对校园送餐路线进行路径规划，创建了节点邻接集合方法，缩短了最小路径的搜索域，提高了路径规划效率。罗德林等［9］提出一种基于人工势场和蚁群算法相结合的路径规划算法，提高了最优路径的搜索效率。DONG等［10］利用机械臂运动的傅里叶变换概念，得到冗余机械臂单个连杆的工作空间密度，求解路径规划问题，可生成平滑且距离短的路径，多障碍物避障十分有效。王建滨等［11］提出了一种对超冗余度机械臂路径安全优化算法，结合安全性优化的数学模型，利用Dijkstra算法进行求解，极大地提高了路径的安全性。张伟民等［12］为了保证路径的可行性，利用三次B样条曲线对改进的RRT*算法搜索到的路径进行平滑处理，使曲线无过大折角且光滑连续。

以上算法主要是在给定起点、终点和障碍物的坐标之后，得到一种离散模式下的路径，这种路径并不完全满足蛇形机器人的任务需求。因此，本文提出一种基于Dijkstra搜素算法和三次B样条曲线相结合的冗余绳索驱动机器人路径规划算法，并对算法开展了仿真实验。

1 二维轨迹规划算法研究

1.1 基于Dijkstra的初始路径生成

Dijkstra算法可以寻找最短路径。根据蛇形机器人的实际应用场景，利用Dijkstra算法生成起点位置到终点位置最短的初始路径，如图1所示。生成的路径为折线图，该路径可用于离散体机器人，而蛇形机器人由多个刚性连杆串联，运动时需要协调关节限制和整体运动。该路径转折点曲率过大可能会超过关节运动限制，蛇形机器人无法沿该路径运动。

1.2 基于三次B样条的路径规划

基于Dijkstra算法生成的初始路径，利用等步长的方式对关键点进行选取。

1）关键点选取

基于Dijkstra算法生成的初始路径，从该路径上初始点开始，利用等步长的方式选取点，若终点没有包含在内，需将终点添加到选取点集合，如图2所示。该点集合为生成光滑路径的关键点。

2）三次B样条曲线控制点求解

在图形学计算中，Bezier曲线和B样条曲线都是利用所有数据点生成插值曲线的方法。结合二者各自的优缺点，经过对比，本文选择B样条曲线插值方法，如下式所示。

式中：Pi为三次B样条曲线的控制点；Bi，k（u）为B样条函数的基函数。

三次B样条曲线的逆向计算过程：给定的关键点ki（i=1，2，…，n）的坐标数据，反求出相应的控制点pi（i=0，1，…，n+1）。三次B样条曲线的关键点与控制点的关系如下式所示。

3）三次B样条曲线正向运算

三次B样条曲线的正向计算：由三次B样条曲线的逆向计算求解出控制点的p0，p1，p2，…，pn，pn+1，第i段三次B样条函数曲线取4个控制点pi－1，pi，pi+1，pi+2带入下式。

Pi（t）=F0，3（t）pi－1+F1，3（t）pi+F2，3（t）pi+1+F3，3（t）pi+2 （i=1，2，3，…，n）（3）

将以上n段三次B样条曲线拼接起来，即得到光滑且符合要求的路径。

1.3 轨迹规划

本文采用笛卡儿空间轨迹规划，根据应用场景，联合表示路径的曲线方程和圆方程进行求解。取其中横坐标大于圆心的交点作为下一时刻圆的圆心，各圆心的坐标是关节中心的坐标，如下式所示。

式中：y=f（x）为路径的曲线函数；（xi，yi） 为当前圆心的坐标。

将n连杆单自由度蛇形机器人轨迹规划算法分为3个步骤：

1） 计算运动周期为T；

2） 设置2n+1个辅助圆心的初始位置；

3） 更新下一时刻的2n+1个圆心的坐标。

以6连杆平面蛇形机器人为研究对象，将A定义为关节，B定义为连杆，C定义为末端。连杆数n=6，需计算13个辅助圆心O－6，O－5，…，O5，O6的坐标，其中6个辅助圆心O－6，O－5，O－4，…，O－1的初始坐标如表1所示，剩余的圆心按照轨迹规划算法计算。蛇形机器人构型如图3所示。

初始时刻t=0s、第一周期时刻t=ti（0≤ti≤T）和特定时刻t=6T时蛇形机器人各关节中心与圆心的对应关系及其相对位置关系如图4所示。

2 仿真试验与分析

为了验证提出的Dijkstra算法和三次B样条曲线的冗余绳索驱动蛇形机器人路径规划算法，以平面6自由度蛇形机器人为例，利用Matlab搭建仿真模型，进行仿真试验。

假设有一条已经规划好的路径，规划路径可以用以下函数表示：

以6连杆蛇形机器人为例，设置圆的半径为50mm，连杆数为6，需计算13个辅助圆的圆心。已知基座的进给速度为10mm/s，轨迹规划的时间小于30s。t=0s时，圆心O－6，O－5，O－4，…，O－1的横坐标如表2所示，纵坐标初始位置均为0。

剩余的辅助圆心坐标可以根据以下公式进行计算：

辅助圆心O1，O2，O3，…，O6计算后的初始坐标如表3所示。

为了满足轨迹规划的精度要求，对实际位置和期望位置的误差进行了分析，结果如表4所示。机器人各关节中心位置的误差都小于10－10，可满足机器人轨迹规划的精度要求。

给定初始坐标，按上述路径规划方法设置相关参数，t=0s起始，t=30s结束，进行仿真实验。如图5所示为蛇形臂机器人分别在2s、15s、18s和30s时的仿真状态，其中黑色线段为机器人的连杆，绿色点为机器人的关节旋转中心，黑色点为机器人的末端执行器，红色曲线为规划的路径（本刊为黑白印刷，疑问之处请咨询作者）。

根据机器人在仿真过程中关节变量的值（表5），机器人的所有关节变量都在其旋转角度限制范围内，表明了所提出的轨迹算法的有效性。

为验证本文提出方法的有效性，首先，对本文提出的轨迹规划方法的精度进行分析，实际位置和期望位置的误差都小于10－10，可满足机器人轨迹规划的精度要求。然后，按上述路径规划方法设置相关参数，进行了仿真实验，实验结果表明机器人可按照规划的路径进行运动。最后，对仿真过程中的关节变量值进行分析，以判断机器人是否运动超限。结果表明：在机器人运动过程中所有关节变量都在其旋转角度限制范围内，均在机器人的可达工作空间。综上表明所提出的轨迹规划方法可实现冗余绳索驱动机器人轨迹的有效规划，且精度较高。

3 结语

本文提出了一种基于Dijkstra算法和三次B样条曲线的冗余绳索驱动蛇形机器人路径规划算法。该方法首先基于Dijkstra算法生成初始路径，然后以等间隔的方式在初始路径上选取一系列关键点，最后基于三次B样条曲线算法利用获取的关键点生成光滑无碰撞路径。仿真试验结果验证了所提出的冗余绳索驱动蛇形机器人路径规划算法的有效性。

参考文献：

［1］ GONALVES V M，FRAISSE P，CROSNIER A，et al. Parsimonious kinematic control of highly redundant robots［J］. IEEE Robotics and Automation Letters，2016，1（1）：65-72.

［2］ 曹博，毕树生，郑晶翔，等. 改进人工势场法的冗余机械臂避障算法［J］. 哈尔滨工业大学学报，2019，51（7）：184-191.

［3］ 霍凤财，任伟建，刘东辉. 基于改进的人工势场法的路径规划方法研究［J］. 自动化技术与应用，2016，35（3）：63-67.

［4］ 汪首坤，邸智，王军政，等. 基于A*改进算法的机械臂避障路径规划［J］. 北京理工大学学报，2011，31（11）：1302-1306.

［5］ ABU-DAKKA F J，VALERO F J，SUER J L，et al. A direct approach to solving trajectory planning problems using genetic algorithms with dynamics considerations in complex environments［J］. Robotica，2015，33（3）：669-683.

［6］ TIAN L， COLLINS C. An effective robot trajectory planning method using a genetic algorithm［J］. Mechatronics，2004，14（5）：455-470：

［7］ KHATIB O. Real-time obstacle avoidance for manipulators and mobile robots［C］//Proceedings of 1985 IEEE International Conference on Robotics and Automation. St. Louis，MO，USA： IEEE，2003：500-505.

［8］ 宫金良，牛作硕，张彦斐. 基于局域降维Dijkstra算法的校园送餐机器人多目标路径规划［J］. 山东理工大学学报（自然科学版），2021，35（4）：75-80.

［9］ 罗德林，吴顺祥. 基于势场蚁群算法的机器人路径规划［J］. 系统工程与电子技术，2010，32（6）：1277-1280.

［10］ DONG H，DU Z J. Obstacle avoidance path planning of planar redundant manipulators using workspace density［J］. International Journal of Advanced Robotic Systems，2015，12（2）：1-10.

［11］ 王建滨，马培荪，徐军，等. 超冗余度机械臂的路径安全性优化研究［J］. 上海交通大学学报，2003，37（5）：710-714.

［12］ 张伟民，付仕雄. 基于改进RRT*算法的移动机器人路径规划［J］. 华中科技大学学报（自然科学版），2021，49（1）：31-36.