基于粒子群优化算法的绳驱动连续体机器人轨迹规划

王会肖 郑天江 沈雯钧 甄存合 方灶军 张苏英

摘 要：为提高绳驱动连续体机器人运动的平滑性和稳定性，在关节空间和笛卡尔空间研究了基于样条函数和粒子群算法的轨迹规划问题。首先，采用双参数局部指数积公式建立连续体机器人的运动学模型;其次，根据牛顿-拉夫森迭代方法进行逆运动学求解;最后，基于自适应惯性权重的粒子群时间最优化算法结合五次B样条函数，分别实现了连续体机器人在关节空间和笛卡尔空间的轨迹规划。仿真结果表明：在相同的条件下，两种方法均可得到连续体机器人末端的连续轨迹，速度均小于10 mm/s，加速度均小于20 mm/s2;基于关节空间规划出的关节位移、速度、加速度曲线更为平滑，关节空间规划用时9.219 3 s，笛卡尔空间规划用时10.604 6 s。基于粒子群优化算法的绳驱动连续体机器人轨迹规划研究，提高了连续体机器人的运动性能，可为绳驱动连续体机器人的位姿规划提供参考。

关键词：机器人控制;连续体机器人;轨迹规划;笛卡尔空间;关节空间;粒子群优化算法

中图分类号：TP242   文献标识码：A   doi：10.7535/hbkd.2020yx05004

Abstract：In order to improve the smoothness and stability of the motion of the cable-driven continuum robot， the trajectory planning methods based on spline function and particle swarm optimization algorithm were proposed for the cable-driven continuum robot in its joint space and Cartesian space respectively. Firstly， the kinematic model was established by applying the local product-of-exponential（POE） formula with two parameters. Secondly， the inverse kinematics was solved by Newton Raphson iterative method. Finally， the particle swarm time optimization algorithm based on adaptive inertia weight combing with the quintic B-spline function was used to realize the trajectory planning of the continuum robot in joint space and Cartesian space respectively. The simulation results show that continuous trajectories can be obtained both in joint space and Cartesian space under the same conditions， the obtained velocities are less than 10 mm/s， and the accelerations are less than 20 mm/s2.The joint displacement， velocity and acceleration curves are smoother in joint space， which takes 9.219 3 s， while it takes 10.604 6 s in Cartisian space. The research on trajectory planning of a cable-driven continuum robot based on particle swarm optimization algorithm improves the kinematic performance of the continuum robot and provides references for pose planning of the cable-driven continuum robots.

Keywords：robot control; continuum robot; trajectory planning; Cartesian space; joint space; particle swarm optimization（PSO） algorithm

剛性机械臂虽然广泛用于较多场景，但随着机器人技术的进步，其工作环境变得越来越复杂，对安全性和柔顺性的要求也越来越高，很难再采用传统的刚性机械臂进行作业。与传统刚性机械臂相比，连续体机器人具有结构轻量化、柔顺性高、环境适应性好等优点，可被广泛应用在狭窄空间的探索检测、医疗服务机器人以及抢险救援等领域[1-3]。

连续体机器人是一类仿照生物、可实现连续变形特征的机器人。目前，连续体机器人的主要驱动方式有形状记忆合金[4]、导电聚合物[5]、气压驱动[6]、绳索驱动[7-9]等。现有研究中，针对连续体的研究较多。例如：HANNAN等[10]研究了由2组对抗绳驱动的类象鼻连续体操作臂;耿仕能等[11]开发了以超镍钛合金为柔性支撑骨架的绳驱动可变刚度连续操作臂;隋立明等[12]设计了一种基于气动软体驱动器的爬行机器人，用来模仿环节动物的纵向肌与环肌功能等。连续体机器人通常采用柔性骨架材料，由于大部分材料具有各向同性，导致其在无需变形的方向上也会产生一定的变形，降低了控制精度，因此，现有连续体机器人的研究主要集中在新型连续体机器人机构的设计方面，很少涉及轨迹规划方面的研究。

刚性机器人和移动机器人轨迹规划的方法较多。文献[13]和文献[14]分别基于三次和五次B样条曲线，实现了两种机器人的工作要求;文献[15]和文献[16]分别利用遗传算法和粒子群算法优化给定任务;王俊刚等[17]提出一种迭代步长顺序查找法匹配机械臂的关键点与空间离散路径曲线，由于缺乏准确的形状传感器，使得该方法在实际操作时较为困难;TANG等[18]提出了预测查找和插值算法来匹配路径，但需要建立较为庞大的数据库，规划出来的路径连续性不佳。由于连续体机器人往往像刚性机器人那样，具有大的速度、加速度以及频繁切换方向等特征，因此，在进行轨迹规划时，其性能容易受最大速度、加速度以及时间等参数的约束，如何将这些参数综合考虑是轨迹规划的关键。

本文以一种能够实现拉/压、扭转刚度大而弯曲刚度小的柔性骨架作为绳驱动机器人的支撑骨架，设计了能够实现连续变形的8自由度绳驱动连续体机器人，在兼顾关节速度和关节加速度的约束下，提出了一种基于五次B样条的自适应惯性权重粒子群优化算法，实现了在约束条件下的连续体机器人最优化轨迹规划，分别得到了绳驱动连续体机器人在关节空间和笛卡尔空间的连续轨迹，提高了连续体机器人的运动性能。

1 8自由度连续体机器人

本文研究的繩驱动连续体机器人由4个连续体机器人关节模块串联组成，每个关节模块均由基座、动平台、柔性骨架和4根绳索组成。柔性骨架连接在动平台和基座的中间，采用具备拉/压、扭转刚度大而弯曲刚度小的液压软管;动平台由绳索驱动，可实现2个自由度的弯曲运动;4根驱动绳索在动平台和基座上的连接点相隔90°。由4个关节模块组成连续体机器人，可实现8自由度弯曲运动。绳驱动连续体机器人样机如图1所示。

2 运动学建模

2.1 正运动学模型

从图4 d）可以看出，α的加速度曲线有一部分存在波动大的情况，可能会导致关节的抖动。由于绳驱动连续体机器人是冗余的机械臂，在进行运动学逆解运算时会存在多解情况，所以在笛卡尔空间求得末端位姿后，求解关节角时会因为关节限制而出现关节加速度不连续的情况。为避免在笛卡尔空间规划产生的这些问题，采用同样的末端轨迹点，在关节空间对连续体机器人进行轨迹规划。经运动学逆解求得各关节对应的关节角如表2所示。同样以4个关节的旋转角α为例，仿真结果如图5所示。

为了能更好地对比两种空间规划的结果，采用相同的限制条件。初始时间间隔hi=3 s，边界条件v1=0，v2=0，a1=0，a2=0;约束条件Vmax=10 mm/s，Amax=20 mm/s2;骨架长度L=120 mm。

由图5可以看出，关节空间规划得到的末端轨迹与笛卡尔空间规划得到的末端轨迹几乎一致，而关节空间得到的4个关节旋转角α的位移、速度、加速度曲线均平滑且连续，解决了在笛卡尔空间规划时出现的加速度不连续的问题，总运行时间由原来的12 s缩短为9.219 3 s，关节速度小于10 mm/s，关节加速度小于20 mm/s2，都在限制范围内，说明此方法适合应用于绳驱动连续体机器人。

5 结 语

1）本文提出了一种基于五次B样条函数和自适应惯性权重粒子群算法相结合的绳驱动连续体机器人轨迹规划研究方法。利用粒子群优化算法，在关节速度、关节加速度约束条件下实现了对连续体机器人时间最优规划轨迹;利用五次B样条算法，保证了轨迹规划的连续性。

2）仿真结果表明，两种空间下的规划方法均可应用于绳驱动连续体机器人的轨迹规划，速度均小于10 mm/s，加速度均小于20 mm/s2，都在给定的限制范围内。

3）在关节空间规划时，经过粒子群优化后关节轨迹运行时间从12 s降低至9.219 3 s;在笛卡尔空间规划时，规划时间从12 s减少至10.604 6 s。

4）在笛卡尔空间进行轨迹规划时，出现了加速度突变情况，可能会导致机器人臂末端的抖动;而在关节空间直接进行规划轨迹时，可以有效解决这一现象，得到更加平滑连续的关节位移、速度、加速度曲线。

5）本文仅考虑了绳驱动机器人位置的轨迹规划，未来尚需解决对位置和姿态同时进行规划的难题，以适应更为复杂的作业要求。

参考文献/References：

[1] ANSCOMBE R，BRYANT A，BUCKINGHAM R，et al. Snake-arm robots： A new approach to aircraft assembly[J].SAE Technical Paper，2008，doi：10.4271/2007-01-3870.

[2] BOGUE R. Robots in the nuclear industry： A review of technologies and applications[J]. Industrial Robot： An International Journal， 2011， 38（2）： 113-118.

[3] WOLF A， CHOSET H H， BROWN B H， et al. Design and control of a mobile hyper-redundant urban search and rescue robot[J].Advanced Robotics， 2005， 19（3）： 221-248.

[4] 邹娇. 基于形状记忆合金的软体爬行机器人结构设计及性能研究[D]. 镇江：江苏大学，2019.

ZOU Jiao. Design and Performance of SMA-Based Soft Crawling Robot[D]. Zhenjiang： Jiangsu University，2019.

[5] SHEN Q， TRABIA S， STALBAUM T， et al. A multiple-shape memory polymer-metal composite actuator capable of programmable control， creating complex 3D motion of bending， twisting， and oscillation[J]. Scientific Reports，2016，doi： 10.1038/srep24462.

[6] 龚道雄，何睿，于建均，等.一种气动肌肉拮抗驱动机器人关节的类人运动控制方法[J]. 机器人，2019，41（6）：803-812.

GONG Daoxiong， HE Rui， YU Jianjun，et al. A human-like motion control method for robotic joint actuated by antagonistic pneumatic muscles[J]. Robot， 2019， 41（6）： 803-812.

[7] 孙定阳，沈浩，郭朝，等. 绳驱动柔性上肢外骨骼机器人设计与控制[J]. 机器人，2019，41（6）： 834-841.

SUN Dingyang， SHEN Hao， GUO Zhao，et al. Design and control of the cable driven compliant upper limb exoskeleton robot[J]. Robot，2019，41（6）： 834-841.

[8] 王从浩.绳索驱动的仿人机器人下肢设计与研究[D]. 合肥：合肥工业大学，2018.

WANG Conghao. Design and Research of the Lower Limbs of Humanoid Robot with Rope Drive[D].Hefei： Hefei University of Techno-logy，2018.

[9] 方旭. 基于绳驱动的机械臂创新设计与研究[D]. 青岛：中国海洋大学，2014.

FANG Xu. Innovation Design of the Cable-driven Manipulator[D]. Qingdao： Ocean University of China，2014.

[10] HANNAN M W， WALKER I D. Analysis and experiments with an elephant's trunk robot[J]. Advanced Robotics，2001，15（8）： 847-858.

[11] 耿仕能， 王友渔， 陈丽莎，等. 变刚度连续型机械臂设计与控制[J]. 宇航学报，2018， 39（12）： 1391-1400.

GENG Shineng， WANG Youyu， CHEN Lisha， et al. Design and control of a continuum arm with variable stiffness[J]. Journal of Astronautics，2018， 39（12）： 1391-1400.

[12] 隋立明， 劉亭羽， 席作岩. 气动软体爬行机器人驱动方式的分析与实验[J].液压与气动，2018（11）： 99-103.

SUI Liming， LIU Tingyu，XI Zuoyan. Analysis and experiment of actuating method for pneumatic soft crawling robot[J]. Chinese Hydraulics & Pneumatics，2018（11）： 99-103.

[13] 郑天江，李俊杰，陈庆盈，等.基于三次B样条的移动机器人实时轨迹规划研究[J].制造业自动化，2017，39（5）：4-7.

ZHENG Tianjiang， LI Junjie， CHEN Qingying，et al. A study of robot motion controller based on pure network[J].Manufacturing Automation，2017，39（5）：4-7.

[14] 马国庆， 刘丽， 梁嵬，等. 工业机器人抓取作业轨迹规划研究[J].制造业自动化， 2020，42（4）： 79-82.

MA Guoqing， LIU Li， LIANG Wei， et al.Research on trajectory planning of industrial robots[J]. Manufacturing Automation，2020，42（4）： 79-82.

[15] 郑先鹏，王雷.面向作业车间调度问题的遗传算法改进[J].河北科技大学学报，2019，40（6）：496-502.

ZHENG Xianpeng， WANG Lei. Improved genetic algorithm for job shop scheduling[J].Journal of Hebei University of Science and Technology， 2019，40（6）：496-502.

[16] 甄然，司超，吴学礼，等.基于改进粒子群算法的飞行器冲突解脱方法研究[J].河北科技大学学报，2016，37（5）：491-496.

ZHEN Ran， SI Chao， WU Xueli，et al.Study of the aircraft conflict resolution method based on modified particle swarm optimization [J]. Journal of Hebei University of Science and Technology，2016，37（5）：491-496.

[17] 王俊刚， 汤磊， 谷国迎，等. 超冗余度机械臂跟随末端轨迹运动算法及其性能分析[J].机械工程学报，2018， 54（3）： 18-25.

WANG Jungang，TANG Lei，GU Guoying，et al.Tip-following path planning and its performance analysis for hyper-redundant manipulators[J].Journal of Mechanical Engineering， 2018，54（3）： 18-25.

[18] TANG L， ZHU L M， ZHU X Y， et al. Confined spaces path following for cable-driven snake robots with prediction lookup and interpolation algorithms[J]. Science China Technological Sciences， 2019， 63（2）：255-264.

[19] 于靖軍. 机器人机构学的数学基础[M]. 北京： 机械工业出版社，2008.

[20] 王宪，杨国梁，张方生，等.基于牛顿-拉夫逊迭代法的6自由度机器人逆解算法[J].传感器与微系统，2010，29（10）：116-118.

WANG Xian，YANG Guoliang， ZHANG Fangsheng， et al. An inverse kinematics algorithm of the general 6-DOF robot based on Newton-Raphson ineration[J]. Transducer and Microsystem Technologies，2010， 29（10）： 116-118.

[21] ZHANG Z，YANG G，YEO S H，et al. Design optimization of a cable-driven two-DOF joint module with a flexible backbone[J]. IEEE/ASME International Conference Advanced Intelligent Mechatronics， 2010（6）：385-390.

[22] 江鸿怀，金晓怡，邢亚飞，等. 基于粒子群优化算法的五自由度机械臂轨迹规划[J].机械设计与研究，2020， 36（1）： 107-110.

JIANG Honghuai， JIN Xiaoyi， XING Yafei， et al.Five-degree-of-freedom manipulator trajectory planning based on PSO particle algorithm[J].Machine Design & Research， 2020， 36（1）： 107-110.