基于网格搜索算法的6-RUS并联机器人时间最优轨迹规划

刘栋财 董广宇 杜玉红 李文鹏

摘要：

針对6-RUS并联喷涂机器人再现轨迹不平滑、轨迹规划效率低等问题，提出了基于优化贝塞尔曲线节点位置的6-RUS并联机器人时间最优轨迹规划方法。首先，将预处理的轨迹离散化为网格点，更新节点参数并优化贝塞尔曲线弧长，进一步拟合小线段路径获取最优几何路径；然后，计算不同粗网格点对应的最佳速度以及求解时间，选择合适的粗网格点，进一步以较小步长密化网格点间路径，迭代求解正反向最大速度，搜索路径的最佳速度曲线，获取6-RUS并联机器人的最佳运行时间。最后，在自研的6-RUS并联机器人平台上进行实验。结果表明，在相同示教轨迹条件下，基于所提的改进贝塞尔曲线算法得到的路径长为8.12 m，优于传统贝塞尔曲线算法以及G2CBS算法的结果；同时将改进的时间最优轨迹规划算法（TOPP）用于优化后的示教路径，所提算法的最优速度曲线的求解时间为416.4 ms，与TOPP-RA算法的最优速度曲线的求解时间相比缩短了244.7 ms，而且该算法下最优轨迹规划时间也优于TOPP-RA算法，该方法提高了最佳速度的求解速率，缩短了6-RUS并联机器人轨迹再现时间，提高了工作效率。

关键词：6-RUS并联机器人；改进的时间最优轨迹规划；贝塞尔曲线；网格搜索

中图分类号：TH112

DOI：10.3969/j.issn.1004-132X.2023.13.008

Time-optimal Trajectory Planning of 6-RUS Parallel Robots Based on Grid Search Algorithm

LIU Dongcai1，2 DONG Guangyu1，2 DU Yuhong1，2 LI Wenpeng1，2

1.School of Mechanical Engineering，Tiangong University，Tianjin，300387

2.Key Laboratory of Advanced Mechatronics Equipment Technology，Tianjin，300387

Abstract： Aiming at the problems of uneven reproducing trajectory and low trajectory planning efficiency of 6-RUS parallel painting robots， a trajectory planning method of optimal time of 6-RUS parallel painting robots was proposed based on optimizing the position of Bézier curve nodes. Firstly， the preprocessed trajectory was discretized to grid points， the node parameters were updated and the arc length of Bézier curve was optimized， and the path of small line segment was further fitted to obtain the optimal geometric path. Then， the optimal velocity corresponding to different coarse grid points and the solution time were calculated， and the appropriate coarse grid points were selected. The paths between grid points were further densified by small steps， and the forward and backward maximum velocity were iteratively solved. The maximum feasible velocity curve of the path was searched to obtain the optimal running time of the 6-RUS parallel robots. Finally， the experiments were carried out on a self-developed 6-RUS parallel robot platform. The results show that under the same teaching trajectory， the path length of the improved Bessel curve algorithm herein is as 8.12 m， which is better than that of traditional Bézier curve and G2CBS（G2 continuous cubic Bézier spiral）aigorithm. Meanwhile， the improved time-optimal path parameterization algorithm herein was used for the optimized teaching path. The solution time of the optimal velocity curve of the algorithm herein is as 416.4 ms， which is 244.7 ms less than that of TOPP-RA algorithm. Moreover， the time-optimal trajectory planning time of the algorithm herein is also better than that of TOPP-RA algorithm. The method improves the solving speed of the optimal velocity， shortens the track reproduction time of 6-RUS parallel robots， and improves the working efficiency.

Key words： 6-RUS parallel robot； improved time-optimal path parameterization； Bézier curve； grid search

收稿日期：2022-08-08

基金项目：

天津市科技计划（21YFFCYS00080）

0 引言

时间最优轨迹规划是机器人运动控制的重要组成部分，在最优化算法下，机器人以最短时间从起始点运动到终点，且运动轨迹平滑。目前机器人喷涂示教再现领域，并联机器人运动学正解复杂且要求运动精度高，导致计算成本极高，在给定复杂的再现轨迹信息情况下，机器人不能满足笛卡儿空间下的实时规划的最优速度，从而无法达到时间最优的运动，同时存在再现轨迹不平滑、路径较长等问题，影响喷涂质量与效率。

在给定约束下，寻求最优时间规划策略是机器人领域长期关注的问题。BOBROW等［1］、SHIN等［2］提出了时间最优轨迹规划（time-optimal path parameterization，TOPP）算法，寻求最短时间完成轨迹或任务，将机器人动力学模型表示为轨迹参数的函数，并将关节力矩的约束转化为轨迹参数约束，建立了机器人的时间最优问题。目前求解时间最优问题有三种方法。一是基于庞特里亚金最大化（Pontryagins maximum）原理的数值积分法［3-4］，在平面（s，s·）下以最大速度和最小速度依次积分求得最优轨迹，求解速度相比凸优化更快，但存在积分后产物，导致路径存在奇异点、机器抖动，同时该方法编程相对困难。PHAM［5］提出了一个通用的、快速的、健壮的TOPP算法，解决了动态奇异的关键问题。二是动态规划法。OBERHERBER等［6］提出了一种机械手的最优轨迹路径动态规划算法。近年来CONSTANTINESCU等［7］、KASERER等［8］将转矩等约束引入动态规划算法［6］进行改进，但约束数量的增大导致计算时间成倍增加。三是凸优化法［9-10］，将时间优化问题转化为凸优化问题，然后利用凸优化包提高求解效率。PHAM等［11］提出了基于可达性分析的时间最优轨迹规划（time-optimal path parameterization based on reachability analysis，TOPP-RA）算法，通过求解小线性规划（ linear programming，LP）递归地计算路径上离散位置的可达集和可控集，但TOPP方法针对小线段、方向变化大且轨迹不平滑的路径，优化效果不明显，同时该方法求解速度相对慢，仍不满足实际应用的需求。

本文根据某企业提供给定喷涂轨迹再现的场景，探讨改进贝塞爾曲线算法，优化机器人示教轨迹的几何路径，基于改进TOPP的时间最优的示教轨迹方法，解决并联机器人进行轨迹再现效率低、轨迹不平滑等问题，从而获得6-RUS并联机器人喷涂的最优时间，并与已有的TOPP-RA算法进行对比分析，在自研实验平台上验证方法的有效性。

1 喷涂工作站

根据某小型企业不同的汽车外观零部件人工拖动喷涂和轨迹再现自动喷涂的要求，笔者设计了6-RUS并联机器人喷涂工作站，如图1所示。

工作站内机器人本体采用6-RUS并联机构，相对6-PUS、6-PSS并联机器人具有工作空间较大、稳定性强（结构对称）、结构载荷小（驱动副固连基座）等优点。下平台为动平台，动平台实现3个方向的平动和3个方向的转动；上平台为静平台。由6个结构相同的驱动分支RUU连接2个平台，机构简图见图2、图3。图3中，A1～A6 分别是6个转动副位置，B1～B6为驱动杆与从动杆之间的万向节，D1～D6为动平台上的6个万向节。

4 基于网格搜索的6-RUS并联机器人喷涂最优轨迹规划实验

4.1 实验平台

搭建实验平台如图9所示。将预处理的示教轨迹文件录入上位机，首先通过改进的贝塞尔曲线对示教轨迹进行平滑与优化，然后通过改进的TOPP算法求解给定示教路径的最佳速度曲线，与TOPP-RA算法对比，评估6-RUS并联机器人在给定路径下的运行时间。

4.2 示教轨迹平滑与优化实验

为了验证优化贝塞尔曲线的弧长最优，给定6-RUS并联机器人的工作空间、运动约束，截取一段预处理后的示教轨迹进行实验，每组实验重复进行50次。目前G2CBS［15］算法从运动学约束获得约束曲率极限，在节点处使用路径修剪和平滑，有较好的平滑和优化路径效果。将本文算法与文献［15］以及3阶贝塞尔曲线算法下的实验结果进行对比分析，得到轨迹输出结果见图10。100次实验的路径弧长平均值对比见表1。

根据图10可知，在工作空间与运动约束下，G2CBS算法和本文算法均对原始示教路径进行了平滑与优化，但本文算法相对于G2CBS算法得到的轨迹曲线更加平滑，G2CBS算法侧重于对节

点处进行优化，本文算法以节点为参数，迭代更新参数已达到曲线整体优化的效果。根据表1可知，在相同约束下，本文算法相对于G2CBS算法以及3阶贝塞尔曲线下6-RUS并联机器人的示教轨迹更短，更加适合用于路径时间最优化的算法研究。

4.3 基于网格搜索的6-RUS并联机器人轨迹参数化实验

选取上述已优化的示教轨迹路径进行时间参数化实验，给定6-RUS并联机器人的关节、TCP的速度、加速度、加加速度、电机性能以及关节力矩约束条件，以不同的步长设置初代网格点M为50、100、200、500，分别进行时间参数化（即路径离散化），计算满足约束的每个网格点的最佳速度，消除冗余计算量，即给定示教轨迹信息，使用基于Seidel算法［16］的自定义LP求解器求解TOPP的LPs问题，获得不同网格点Mi下轨迹的关节速度、加速度、TCP最佳速度曲线，如图11～图14所示，生成最优轨迹时间对比如表2所示。

根据图11a、图12a、图13a、图14a可知，同一路径离散化成不同数量网格点，6-RUS并联机器人的6个关节电机旋转角度基本相同，网格点在100之内，关节速度曲线的曲率相对小，网格点在200、500时，速度曲线的曲率变大，即关节速度变快。由图11b、图12b可以看出，由于网格点的划分稀疏，计算的步长相对较大，导致约束不全，使得关节加速度曲线不能快速达到理想值，同时曲线呈线性化且抖动大，0～2 ms处关节1与2的加速度直接下降，表明此处网格点的约束缺失导致加速度未呈现理想状态；由图13b、图14b可知，随着网格点的增加，加速度曲线可快速达到理想值，曲线变得更加平滑，同时加速度变化的节点的抖动、尖点也相对减少。由图11c、图12c、图13c、图14c可知，网格点由50到200的情况下，机器人TCP末端的速度曲线更加平滑，同时正反向计算的速度曲线更加健壮，TCP最佳速度曲线对正反向速度曲线跟随性更好，表明随着网格点的增加，路径的约束会不断健全，同时正反向计算的速度会更加精确，因此可以获取更多最佳可行速度的上下限约束数值。但是网格点由200到500的情况下，TCP末端的最佳速度曲线并没有变化，仅仅关节加速度曲线变得更加平滑。考虑机器人运行成本，本文进一步分析不同网格点下，网格划分以及求解正反向速度的时间对机器人运行时间的影响，具体如表2所示。

根据表2可知，随着网格点的增加，轨迹参数化的时间基本不变，但正反向速度约束下的最优曲线求解时间显著增加，根据图11c、图12c、图13c、图14c可知，网格点从200增加到500并没有对速度产生明显的优化，因此考虑计算机的算力以及时间成本，可选用网格点200作为初代最佳速度搜索的基准，进一步进行迭代最佳速度优化。

4.4 基于网格搜索的6-RUS并联机器人轨迹规划时间优化实验

根据实际喷涂工况需求，拖动6-RUS并联机器人沿工件移动并采集路径信息，本文采用改进贝塞尔曲线的节点位置对原始路径进行优化，获取最优的几何路径，如图15所示。采用本文TOPP算法对机器人轨迹再现时间最优化处理，与TOPP-RA算法对比，分析6-RUS并联机器人的关节角速度、加速度的变化曲线如图16、图17所示，机器人的TCP最佳速度曲线如图18所示。

由图15可知，基于改进的贝塞尔曲线节点位置算法的几何路径相对于原始路径更加平滑，同时路径的长度相对减少，有利于机器人轨迹再现时间的整体优化。

根据图16可知，基于TOPP-RA算法的关节运行曲线出现波浪式前进，本文算法下的各关节运行速度曲线相对于TOPP-RA的速度曲线均更加平滑，同时曲线几乎没有波动。由图17可知，由于TOPP-RA算法的约束的冗余性，造成每个网格点的过度约束，导致6-RUS并联机器人出现频繁的加减速，使得加速度曲線的持续波动。而本文的算法采用初始粗搜索的方法，消除了冗余约束，同时保留了可控约束的完整性，从而得到图17b所示更加平滑的加速度曲线，优化了加减速时间，同时提高了效率。

由图18可知，TOPP-RA算法与本文算法下的TCP的最佳速度曲线大体相同，但是由图18a的点划线框可看到，在速度上限约束条件改变时，TOPP-RA算法下的最佳速度的求解较慢，导致最佳速度的应激变化较慢，即对速度上限的跟随性不好，从而造成6-RUS并联机器人的运行速度降低；机器人运行时间在500～850 ms、2000～2500 ms区间时，由于6-RUS并联机器人关节加速度频繁升降（图17a），导致TCP的速度曲线出现抖动，影响机器人的运行速度，而本文算法消除了加减速带来的影响，TCP的速度曲线均更加平滑。

在相同的几何路径下，TOPP-RA算法的运行时间为2986 ms，本文算法的运行时间为2440 ms。为了对比TOPP-RA算法与本文算法的最佳可行速度求解时间，本文进一步分析最佳速度求解时间与机器人运行时间，以初代搜索的200个网格点为例，如表3所示。

由表3可知，在相同的示教路径下，基于本文算法的6-RUS并联机器人运行时间相对于基于TOPP-RA算法的运行时间少506 ms；TOPP-RA算法的最佳速度曲线求解时间为661.1 ms，本文算法在初代粗搜索时，仅用21.4 ms即获取初代网格点下的机器人的可行速度；进一步对网格段迭代计算速度曲线，用时395 ms，即本文算法最佳速度曲线的计算时间为416.4 ms，比TOPP-RA的时间少244.7 ms。

由此可知，本文算法的最佳速度求解时间相对于TOPP-RA算法的最佳速度求解时间更优。同时由于本文的速度与加速度的持续稳定性，优化了最优轨迹规划时间，在实际应用过程，采用本文算法的6-RUS并联机器人的运行时间相对较短，提高了工作效率。

5 结论

（1）针对6-RUS并联机器人示教轨迹再现应用场景，提出了一种基于优化贝塞尔曲线节点位置的改进时间最优轨迹规划方法。

（2）本文通过优化贝塞尔曲线节点参数，建立弧长最小化的目标函数，同时给定曲线相关的约束，利用模拟退火优化求解器计算每个节点的弧长最小值，最后拟合得到最优的示教轨迹，为进一步进行示教轨迹的时间优化提供基础。

（3）提出了一种改进的时间最优轨迹方法，通过求解初代网格点下的机器人最佳速度简化冗余约束计算，以及迭代密化求解整体路径的最佳可行速度，提高了机器人最佳速度的计算效率，同时优化了机器人运行速度与加速度抖动问题，缩短了6-RUS并联机器人的最优轨迹规划时间。

（4）实验结果表明，相同的示教轨迹下，基于优化贝塞尔曲线节点位置算法得出的路径长度为8.12 m，小于3阶贝塞尔曲线和G2CBS算法下的路径长度；在已优化轨迹下，基于本文算法的最佳速度搜索时间为416.4 ms，优于TOPP-RA算法的244.7 ms，基于本文算法的最优轨迹规划时间也优于TOPP-RA算法的最优轨迹规划时间，可在最短时间内达到预期速度并进入稳态，提高了机器人运行效率。该方法为并联机器人示教轨迹时间最优规划提供了新的思路。

参考文献：

［1］ BOBROW J， DUBOWSKY S， GIBSON J. Time-optimal Control of Robotic Manipulators along Specified Paths［J］. The International Journal of Robotics Research， 1985， 4（3）：3-17.

［2］ SHIN K， MCKAY N. Selection of Near-minimum Time Geometric Paths for Robotic Manipulators［J］. IEEE Transactions on Automatic Control， 1986， 31（6）：501-511.

［3］ PHAM Q C. Characterizing and Addressing Dynamic Singularities in the Time-optimal Path Parameterization Algorithm［C］∥2013 IEEE/RSJ International Conference on Intelligent Robots and Systems. Tokyo， 2013：2357-2363.

［4］ KUNZ T， STILMAN M. Time-optimal Trajectory Generation for Path Following with Bounded Acceleration and Velocity［M］∥AGARWAL P， KUMAR S， RYDE J， et al. Robotics：Science and Systems Ⅷ. Cambridge：MIT Press， 2013：209-216.

［5］ PHAM Q C. A General， Fast， and Robust Implementation of the Time-optimal Path Parameterization Algorithm［J］. IEEE Transactions on Robotics， 2014， 30（6）：1533-1540.

［6］ OBERHERBER M， GATTRINGER H， MLLER A. Successive Dynamic Programming and Subsequent Spline Optimization for Smooth Time Optimal Robot Path Tracking［J］. Mechanical Sciences， 2015， 6（2）：245-254.

［7］ CONSTANTINESCU D， CROFT E A. Smooth and Time-optimal Trajectory Planning for Industrial Manipulators along Specified Paths［J］. Journal of Robotic Systems， 2000， 17（5）：233-249.

［8］ KASERER D， GATTRINGER H， MULLER A. Nearly Optimal Path Following with Jerk and Torque Rate Limits Using Dynamic Programming［J］. IEEE Transactions on Robotics， 2019， 35（2）：521-528.

［9］ VERSCHEURE D， DEMEULENAERE B， SWEVERS J， et al. Time-optimal Trajectory Planning for Robots：a Convex Optimization Approach［J］. IEEE Transactions on Automatic Control， 2009， 54（10）：2318-2327.

［10］ HAUSER K. Fast Interpolation and Time-optimization with Contact［J］. The International Journal of Robotics Research， 2014， 33（9）：1231-1250.

［11］ PHAM H， PHAM Q C. A New Approach to Time-optimal Path Parameterization Based on Reachability Analysis［J］. IEEE Transactions on Robotics， 2018， 34（3）：645-659.

［12］ HOBBYJ D. Smooth， Easy to Compute Interpolating Splines［J］. Discrete & Computational Geometry， 1986， 1（2）：123-140.

［13］ CLAPHAM C， NICHOLSON J， NICHOLSON J R. The Concise Oxford Dictionary of Mathematics［M］. Oxford：Oxford University Press， 2014.

［14］ KIRKPATRICK S， GELATT JR C D， VECCHI M P. Optimization by Simulated Annealing［J］. Science， 1983， 220（4598）：671-680.

［15］ YANGK， JUNG D， SUKKARIEH S. Continuous Curvature Path-smoothing Algorithm Using Cubic Bezier Spiral Curves for Non-holonomic Robots［J］. Advanced Robotics， 2013， 27（4）：247-258.

［16］ SEIDEL R. Small-dimensional Linear Programming and Convex Hulls Made Easy［J］. Discrete & Computational Geometry， 1991， 6：423-434.

（編辑 王旻玥）

作者简介：

刘栋财，男，1999年生，硕士研究生。研究方向为并联机器人柔顺控制。E-mail：Liudongcai_119591@163.com。

杜玉红（通信作者），女，1974年生，教授、博士研究生导师。研究方向为机器人技术与智能控制及图像处理，工业机器人控制。发表论文20余篇。E-mail：dyh202@163.com。