一种改进RRT结合四次样条的协调路径规划方法1)

2020-08-11罗建军王明明高登巍

力学学报 2020年4期

余敏罗建军王明明高登巍

(西北工业大学深圳研究院，广东深圳 518057)

(西北工业大学航天学院，西安 710072)

引言

随着在轨维修、空间装配等任务需求的与日俱增，空间机器人技术被验证是执行特殊恶劣空间环境下任务最有效的手段之一.典型实例有“工程试验卫星(engineering test satellite VII,ETS-VII)”[1]和“轨道快车(orbit express,OE)”[2].空间机器人在轨服务任务中最具挑战性的工作之一是其抓捕过程中的运动规划问题[3-7].

过去的文献中已提出过多种空间机器人运动规划方法.基于零作用空间，Yoshida 等[8]提出了无反作用机械臂，解决了运动学非冗余臂和冗余臂考虑时间最少和机械臂速度限制的规划问题.基于零反作用空间的概念，Wang 等[9-13]提出了一种考虑任务优先级的运动学冗余机械臂路径规划方法，并基于Bézier 曲线提出抓捕自旋卫星后的最优消旋与路径规划策略.罗建军等[14]提出了一种利用傅里叶级数对空间机械臂关节运动轨迹进行参数化的规划方法.Xu 等[15]利用自由漂浮空间机器人非完整特性提出了一种点到点的路径规划方法，同时调节基座姿态和末端的位姿.Nakamura 等[16]最早在机器人学中引入了任务优先级的概念，基于二次规划框架，仅考虑两个优先级水平，解决了冗余机械臂关节轨迹优化问题.戈新生等[17]基于Legendre 伪谱法研究自由漂浮空间机器人非完整路径规划的最优控制问题.程靖等[18]分析了双臂空间机器人捕获卫星的力学，并研究其镇定控制问题.Salehian 等[19]研究了双臂机器人的协调操作机制与自碰撞规避策略.杨冬等[20]研究了冗余双臂机器人的实时协调避碰方法.方健[21]研究双臂机器人的在线运动规划与协调操作方法.

近年来，非结构化环境下的机器人自主导航控制技术成为亟需.基于随机采样的规划方法在机器人领域中兴起，它适合于解决高自由度机器人在复杂环境中的规划问题，而且对带有微分约束的规划问题也具有较强的解决能力.例如，RRT 算法[22]，其本质上是单一查询的规划方法，其基本算法的主要思想是：由起始位形点出发构造搜索树，而后再从整个位形空间中随机选择一个位形点，找出与该点在搜索树中最近的位形点，以此不断的产生新位形状态，直到到达目标，搜索树构造结束.由于在搜索树构造过程中可以考虑多种微分约束(如非完整约束、运动学及动力学约束等)，因而获得的规划结果合理性较强.Vahrenkamp 等[23]基于RRT 算法，提出一种适用于地面双臂机器人的抓捕和运动规划策略.James 等[24]将RRT 方法应用于双臂空间机器人抓捕前阶段的路径规划过程中.然而，基于RRT 算法的运动规划方法产生的初始轨迹不平滑，不能直接应用于机器人运动规划过程中.常见的平滑处理技巧有优化方法和样条曲线拟合技巧[25]等，然而采用优化方法较为耗时.Kolter 等[26]利用三次样条曲线设计四足机器人的在任务空间下的运动轨迹.Vaz 等[27]结合三次样条和半无限规划方法进行机器人的轨迹规划.基于三次非均匀B 样条曲线，赵皓等[28]和赖永林等[29]分别进行6DOF 机械臂和工业机器人的轨迹规划.上述研究表明，样条函数是最有效的平滑处理技巧之一，且在机器人运动规划中得到广泛的成功应用.

本文主要研究空间双臂机器人的协调路径规划问题，基于RRT*算法分别规划双臂的末端执行器在笛卡尔空间中的初始可行路径，并利用四次样条曲线对由RRT*算法生成的初始路径进行平滑处理，通过满足样条曲线的一阶、二阶及三阶微分连续约束，保证机械臂末端执行器的初末速度、加速度约束条件，得到适合于机器人执行的平滑路径.本工作采用RRT*和四次样条曲线方法，研究了空间多臂机器人的快速路径规划问题，考虑双臂规划过程中的协调性操作和路径平滑处理，以期为空间多臂机器人协调运动规划问题以及多智能体协调运动规划问题提供参考数据.

1 自由漂浮双臂空间机器人建模

如图1 所示，双臂空间机器人系统由1 个基座和2 个n自由度的机械臂构成，共计2n+1 个刚体，且系统不受任何外力作用(本文研究中n=7).

图1 自由漂浮双臂空间机器人系统示意图Fig.1 Sketch of a dual-arm free-floating space robot

空间机器人建模方面的现有研究很成熟，根据泊松公式[30]，空间机器人的运动学方程表示如下

其中，上标a和b分别表示变量属于机械臂a和b.下标b 和e 分别表示变量属于基座(base)和末端执行器(end-effector).分别为基座和末端执行器的线速度、角速度合矢量，为各机械臂关节角速度.若无特别说明，文中矢量(除姿态相关矢量)选择向惯性坐标系投影，其他矢量已在表1 中列举.

表1 本文所用运动学和动力学符号Table 1 Symbols used in this paper

根据第二类拉格朗日方程[31]，空间机器人的动力学方程表示如下

式中，M0=[P0L0]T为系统的初始线动量、角动量.由于矩阵Hb可逆，可以将基座的运动方程改写如下

将式(2)代入式(1)，那么末端执行器的运动表达如下

式中，Jg为系统的广义雅克比矩阵，Jd为系统初始动量M0引起的耦合矩阵.

2 协调RRT*结合四次样条的路径规划

2.1 RRT*协调路径规划

基于随机采样原理的快速搜索随机树方法具有概率完备性、计算量小、计算难度低等特点，非常适合空间自主航天器完成复杂空间的近距离操作服务任务.此外，它对解决高维状态规划问题有着较强的适应性，这一能力是目前很多规划算法所不具备的，因此它非常适合空间机器人高维运动状态规划的实际情况.鉴于RRT*算法不仅加快了原RRT 算法的收敛速度，还增强了算法的最优性能，因此本文选取RRT*作为空间机器人路径规划的主要方法，这里简单给出RRT*算法的基本流程，关于RRT*算法的细则可以参考文献[32].

本文研究中，利用RRT*规划算法生成机械臂末端执行器在笛卡尔空间下的期望路径，具体工作流程如下：

(1)将系统的初始状态(末端执行器的位置信息)作为树的第一个顶点；

(2)当无障碍环境时在状态空间中随机生成一个采样点；

(3)在原树与该采样点最近点处延伸固定步长(初始时即为顶点)；

(4)对采样点周围的点进行检测与优化，若有更短的航迹，则用新节点替代旧节点；

(5)考虑障碍物等运动学约束，进行碰撞检测，以保证生成路径的安全性；

(6)经过有限次迭代优化求解，直至达到目标状态.

通过以上步骤，可以在非结构环境中快速搜索到一条无碰撞的从初始状态到期望状态的优化运动路径.

然而，将RRT*算法直接应用于双臂空间机器人的末端执行器在任务空间下的路径规划，双末端执行器在规划的过程中由于随机采样极有可能会发生碰撞.因此本文针对原有RRT*算法进行改进，提出一种适用于多臂路径规划的协调RRT*算法，改进的RRT*算法同样适用于其它多智能体系统的协调路径规划问题.

基于原有RRT*算法框架，在设置双臂的采样空间时，本文对双臂各自采样空间预留了约0.1 m 宽度的欧氏距离，通过在不同的采样空间中分别进行各末端执行器的路径搜索，这相当于对双末端施加了具有一定边界范围的笛卡尔空间约束.值得注意的是，此空间约束不是固定的，应随着末端执行器的运动而连续变化.此空间约束可以保证整个规划执行过程中双臂空间机器人不发生自身碰撞，包括两个末端执行器不发生碰撞，以及双臂各连杆不发生自身碰撞.改进的协调RRT*路径规划算法流程如下：

(1)Fork=1 toKdo;

(2)随机在不同的任务空间中对双末端分别采样;

(3)在原树结构中搜索新生采样点附近的点;

(4)进行碰撞检测，生成新的树节点;

(5)判断是否有更少代价的航迹，若有则替代原航迹;

(6)Return Tree.

注意，由上述RRT*算法得到的末端初始路径十分不平滑，不能直接施加于空间机器人，需要对得到的初始路径进行平滑处理.常见的平滑处理技巧分为两类，一类是基于优化方法，例如，共轭梯度法、凸优化方法等.但此类方法比较耗时，因此本文采用第二类方法：曲线拟合技巧.

2.2 四次样条平滑

样条曲线被认为是平滑机器人路径的最有效的手段之一，在机器人规划中也得到了很多应用，包括关节规划和末端规划.实际应用中最常见的样条曲线是三次样条曲线，本文研究表明，为了满足路径的三阶导数连续条件，针对末端的路径规划至少需要用到四次及以上的样条曲线.

首先，回顾样条曲线的基本知识.假设有n+1 个时间位置相关的路径点如下

其中，xi表示机器人末端执行器在时间ti的期望位置.

给定这些路径点，四次样条曲线可以平滑地衔接这些路径点，且保证在曲线衔接处满足一阶、二阶微分连续约束等.四次条件曲线的基本形式表达如下

其中，ai,bi,ci,di,ei分别是四次样条曲线的参数.最终的路径由若干段四次样条曲线级联而成.给定路径点，存在唯一的样条参数集合，使得规划的路径通过各个路径点，且在各个路径点处满足特定的速度连续，加速度连续等约束条件.针对机器人运动而言，还需要保持加加速度连续，以防机械臂产生抖振现象.

如图2 所示，给定一系列末端执行器的期望路径点(本文研究中，这些路径点由RRT*算法给出)，假设i=3，那么则有4 个路径点，即需要级联3 段四次样条曲线，共计15 个未知参数.

图2 四次样条曲线示意图Fig.2 Sketch of quartic splines

通过保证各个路径点位置约束(6 个约束)；中间路径点还需满足速度、加速度、加加速度连续约束(6 个约束)，以保证机器人能够平滑地执行全部路径；此外，本文对末端的初始速度、加速度和终止速度施加限制(3 个约束).由上述15 个约束构建方程组，可以求解15 个未知的样条参数.构建的约束方程组具体表达如下

其中，h0,h1,h2是仅与时间相关的常量，取决于时间序列t0,t1,t2,t3.通过上述方程组即可求解样条参数，进而得到机器人末端的运动轨迹.

综上所述，自由漂浮双臂空间机器人的路径规划问题可描述为以下形式

式中，目标函数f(xe)为由RRT*算法给出的路径点的位置函数，保证路径点位置与样条曲线中的位置匹配.规划过程中，还需要满足末端执行器的初末状态约束，以及机械臂关节物理极限约束.

基于RRT*算法，在非结构化环境中生成若干可行路径点，再由四次样条曲线拟合这些路径点，即可得到平滑可行的机器人路径，本文所提的路径规划算法，效果如图3 所示.其中，红色曲线是原始RRT*算法生成的路径，蓝色曲线是由RRT*结合样条设计生成的路径，可以明显看出所提算法生成的路径不仅平滑，而且极大地保留了原RRT*生成路径的最优性.此外，图4 验证了所提路径规划算法的协调性，基于分离的采样空间，有效地避免了可能发生的碰撞.

图3 RRT*结合四次样条路径规划示意图Fig.3 Sketch of spline-RRT*algorithm

图4 协调RRT*路径规划示意图Fig.4 Sketch of coordinated RRT*algorithm

3 仿真结果

为验证所提协调路径规划方法的有效性，本章给出了采用运动学冗余双机械臂抓捕空间目标的演示实例，如图5 所示.表2 给出了本文所用双臂空间机器人的运动学和动力学参数，两机械臂参数相同.

假设基座的初始位姿是0，末端执行器的初始位姿分别为

终止位姿分别为

图5 空间机器人抓捕目标示意图Fig.5 Sketch of capturing task by space robot

表2 空间机器人运动学和动力学参数Table 2 Kinematica and dynamic parameters of space robot

注意，本文所提的基于RRT*路径规划方法只用于末端执行器的位置规划，对于其姿态规划，仍采用前期工作中[33]的用到的Bézier 曲线.基于五阶Bézier 曲线，规划末端执行器在笛卡尔空间下的姿态路径x(t)，描述如下

其中，多项式bj,m(τ)是m阶Bernstein 基本多项式，是二项式系数.Pij是给定构建Bézier 曲线的控制点.

给定末端的初末位姿，考虑机械臂的速度以及加速度限制条件，可以计算出规划路径的执行时间

图6 给出了由规划的末端执行器的路径，以及路径的执行速度.可以看出，规划的末端路径较为平滑，且速度保持在机器人末端执行速度的最大限制范围(0.1 m/s)内，且路径的初始速度、加速度和终止速度都为零，符合所设计的情况，由此说明了规划路径的有效性.

图6 规划的末端执行器的路径及其速度Fig.6 Planned path and velocity for end-effectors

图7 给出了机械臂执行所规划路径过程中，机械臂关节角、关节角速度以及关节角加速度的变化曲线.可以看出，各机械臂的关节角、关节角速度以及关节角加速度变化平滑，且速度和加速度大小均被限制在给定范围内，分别是0.1 rad/s 和0.1 rad/s2，且关节角速度和关节角加速度初始为零，符合实际情况.

图8 给出了机械臂末端执行器的实际跟踪误差曲线.可以看出，整个执行过程中，末端执行器的实际跟踪误差保持在10−4量级，由此说明，末端能够跟踪上规划的路径，证明了所设计的规划算法的正确性.此外，机械臂各连杆在执行过程中亦未发生碰撞，各部件均保持在安全距离(di=0.1 m)以外，如图9 所示.

图7 各机械臂关节变化曲线Fig.7 Joint angle,velocity and acceleration

图8 末端执行器的跟踪误差Fig.8 Tracking error of end-effectors

图9 机械臂各连杆之间的相对距离Fig.9 The minimum distance between robot links

4 结论

考虑到安全性和最优性因素，本文提出了一种双臂空间机器人协调路径规划方法.基于RRT*算法在非结构化环境中快速生成末端执行器安全可行的初始路径，并利用四次样条曲线，平滑了由RRT*生成的初始路径，保证机器人以良好的状态执行规划的路径，同时考虑了机械臂末端以及关节的物理极限约束.仿真结果说明了所提协调路径规划方法的有效性.

附录A 双臂空间机器人运动学建模

双臂空间机器人各连杆质心和末端执行器的位置矢量可以表示为