本发明属于空间机器人协调路径规划方法，涉及一种基于随机采样的双臂空间机器人协调路径规划方法。
背景技术：
随着在轨维修、空间装配等任务需求的与日俱增，空间机器人技术被验证是执行特殊恶劣空间环境下任务最有效的手段之一。典型实例有“工程试验卫星(engineeringtestsatellitevii,ets-vii)”和“轨道快车(orbitexpress,oe)”。空间机器人在轨服务中最具挑战性的工作之一是其抓捕过程中的运动规划问题。过去的文献中已提出过多种空间机器人运动规划方法。基于零作用空间，yoshida等人提出了无反作用机械臂，解决了运动学非冗余臂和冗余臂时间损失和机械臂速度限制的问题。基于零反作用空间的概念，wang等提出了一种考虑任务优先级的运动学冗余机械臂路径规划方法，并基于bézier曲线提出抓捕自旋卫星后的最优消旋与路径规划策略。luo等提出了一种利用傅里叶级数对空间机械臂关节运动轨迹进行参数化的规划方法。xu等利用自由漂浮空间机器人非完整特性提出了一种点到点的路径规划方法，同时调节基座姿态和末端的位姿。nakamura最早在机器人学中引入了任务优先级的概念，基于二次规划框架，仅考虑两个优先级水平，解决了冗余机械臂关节轨迹优化问题。随着越来越多的操作任务需要在非结构化环境下进行，因此非结构化环境下的机器人自主导航控制技术成为亟需。
近年来，基于随机采样的规划方法在机器人领域中兴起，它适合于解决高自由度机器人在复杂环境中的规划问题，而且对带有微分约束的规划问题也具有较强的解决能力。例如，rrt算法，其本质上是单一查询的规划方法，其基本算法的主要思想是：由起始位形点出发构造搜索树，而后再从整个位形空间中随机选择一个位形点，找出与该点在搜索树中最近的位形点，以此不断的产生新位形状态，直到到达目标，搜索树构造结束。由于在搜索树构造过程中可以充分考虑了多种微分约束(如非完整约束、运动动力学约束等)，因而获得的规划结果合理性较强。
技术实现要素：
要解决的技术问题
为了避免现有技术的不足之处，本发明提出一种基于随机采样的双臂空间机器人协调路径规划方法，基于rrt*算法提出一种复杂空间环境下双臂空间机器人的协调路径规划方法。
技术方案
一种基于随机采样的双臂空间机器人协调路径规划方法，包括了双臂空间机器人的运动学模型建立；通过改进rrt*算法，设计双臂空间机器人的初始路径；利用四次样条曲线拟合由改进rrt*算法生成的初始路径，得到适合于机器人执行的可行平滑路径；其特征在于步骤如下：
步骤1、建立双臂空间机器人的运动学模型：
双臂空间机器人系统由基座和2个n自由度的机械臂构成，共计2n 1个刚体，且系统不受任何外力作用；
根据泊松原理，双臂空间机器人的运动学模型为：
其中，上标a和b分别表示变量属于机械臂a和b，下标b和e分别表示变量属于基座base和末端执行器end-effector；
系统初始线/角动量m0＝[p0l0]t为：
其中，
步骤2、利用rrt*算法设计双臂空间机器人的初始路径：
为避免两机械臂在操作过程中发生相互碰撞，对rrt*算法进行改进，在不同的采样空间中分别进行各末端执行器的快速搜索路径规划，得到其初始路径的时间序列路径点(t0,xe,0),(t1,xe,1),…,(tn,xe,n)，具体步骤为：
1.fork＝1tokdo
2.随机在不同的任务空间中对双末端分别采样
3.在原树结构中搜索新生采样点附近的点
4.进行碰撞检测，生成新的树节点
5.判断是否有更少代价的航迹，若有则替代原航迹
6.returnpath；
在原始rrt*算法基础上，对双臂各自采样空间预留0.1m宽度的欧氏距离，通过在不同的任务空间中分别进行各末端执行器的路径搜索，即对双末端施加具有边界范围的笛卡尔空间约束；
注意，此空间约束不固定，其随末端执行器的运动而连续变化，进而保证整个规划执行过程中双臂空间机器人不发生自身碰撞，包括两个末端执行器不发生碰撞，以及双臂各连杆不发生自身碰撞；
步骤3、采用四次样条曲线平滑双臂空间机器人的初始路径：
由rrt*算法得到的初始路径十分不平滑，不能直接施加于空间机器人，为保证机器人路径的三阶导数连续，采用四次样条曲线对得到的rrt*算法得到的初始路径进行平滑处理：
首先，给定由rrt*算法规划得到的初始时间序列路径点如下
(t0,xe,0),(t1,xe,1),…,(tn,xe,n)
其中，xe,i表示机器人末端执行器在时间ti的期望位置；
然后，利用四次样条曲线平滑地拟合这些路径点，其基本形式表达如下
xe,i(t)＝ai bi(t-ti) ci(t-ti)2 di(t-ti)3 ei(t-ti)4
其中，ai,bi,ci,di,ei分别是四次样条曲线的参数，存在唯一的样条参数集合，使得拟合的路径通过各个路径点，且在各个路径点处满足速度连续，加速度连续等条件。针对机器人运动而言，还需要保持加加速度连续，以防机械臂产生抖振现象；
因此，至少满足以下度、加速度、加加速度微分约束条件：
通过上述微分约束条件，构建关于四次样条参数的线性方程组，进而求解样条参数ai,bi,ci,di,ei，得到各时间段内的连续路径；
最后，机器人的路径由各段四次样条曲线级联而成，表达如下
根据步骤1中的机器人运动学模型，通过逆运动学求解，即得到机械臂各关节运动轨迹
有益效果
本发明提出的一种基于随机采样的双臂空间机器人协调路径规划方法，借助rrt*算法分别规划双臂末端执行器在笛卡尔空间中的初始可行路径，并利用四次样条曲线对rrt*算法生成的初始路径进行平滑处理，通过满足样条曲线的一阶、二阶及三阶微分连续约束和机械臂末端执行器的初末速度、加速度约束，得到适合于机器人执行的平滑路径。最后以实例验证本发明提出的路径规划方法的有效性。
本发明的有益效果是：提出了一种空间环境下利用rrt*进行双臂空间机器人协调路径规划的框架。利用rrt*算法在非结构化环境中快速生成末端的安全可行的初始路径，并利用四次样条曲线，平滑了rrt*生成的初始路径，保证机器人以良好的状态执行规划的路径，且考虑了机械臂的物理极限约束。仿真结果说明了所提协调路径规划方法的有效性。所提算法对于机械臂运动规划具有重要意义，特别是针对失效卫星的抓捕路径规划。
附图说明
图1：自由漂浮双臂空间机器人系统示意图
图2：样条rrt*路径规划示意图
图3：规划的末端执行器的路径
(a)a末端执行器的路径；(b)b末端执行器的路径；
(c)a末端执行器的速度；(d)b末端执行器的速度
图4：机械臂各连杆之间的相对距离
具体实施方式
现结合实施例、附图对本发明作进一步描述：
该发明的实施主要包括以下三个步骤：
步骤一、双臂空间机器人运动学建模。
双臂空间机器人系统由基座和2个n自由度的机械臂构成，共计2n 1个刚体，且系统不受任何外力作用。根据泊松公式，空间机器人的运动学方程表示如下，
其中，上标a和b分别表示变量属于机械臂a和b。
根据拉格朗日原理，系统的动力学方程表示如下，
式中：
式中：m0＝[p0l0]t为系统的初始线动量、角动量。由于矩阵hb不可逆，基座的运动如下
将式(4)代入式(1)，那么机器人末端执行器的运动表达如下
式中：jg称之为系统的广义雅克比矩阵，jd表示由于系统初始动量m0引起的耦合矩阵。
步骤二、基于rrt*的协调路径规划。
基于随机采样原理的快速搜索随机树方法具有概率完备性、计算量小、计算难度低等特点，非常适合空间自主航天器完成复杂空间的近距离操作服务任务。此外，它对解决高维状态规划问题有着较强的适应性，这一能力是目前很多规划算法所不具备的，因此它非常适合空间机器人高维运动状态规划的实际情况。鉴于rrt*算法不仅加快了原rrt算法的收敛速度，还增强了算法的最优性能，因此本专利选取rrt*作为空间机器人路径规划的主要方法。
将rrt*算法直接应用于双臂空间机器人的末端执行器在任务空间下的路径规划，双末端执行器在规划的过程中由于随机采样会发生碰撞，因此本专利针对原有rrt*算法进行改进，在不同的采样空间中分别进行各末端执行器的快速搜索路径规划，保证整个规划执行过程中双臂空间机器人能够完全不发生自身碰撞，包括两个末端执行器不发生碰撞，以及双臂各连杆不发自身碰撞，进而完成空间机器人的协调路径规划，改进的协调rrt*算法流程如表2所示。
注意，由上述rrt*算法得到的末端执行器的路径十分不平滑，不能直接施加于空间机器人，需要对得到的初始路径进行平滑处理。常见的平滑处理技巧分为两类，一类是基于优化方法，例如，共轭梯度法、凸优化方法等。但此类方法比较耗时，因此本专利采用第二类方法：曲线拟合技巧。
步骤三、四次样条曲线平滑路径。
样条曲线被认为是平滑机器人路径的最有效的手段之一，在机器人规划中也得到了很多应用，包括关节规划和末端规划。实际应用中最常见的样条曲线是三次样条曲线，然而这里采用四次样条曲线，本研究表明，针对末端的路径规划至少需要用到四次及以上的样条曲线，才能保证满足一些必要的约束条件，具体分析后面会详细说明。
首先，回顾样条曲线的基本知识。n 1个时间位置相关的路径点如下
(t0,x0),(t1,x1),…,(tn,xn)(6)
其中，xi表示机器人末端执行器在时间ti的期望位置。
给定这些路径点，四次样条曲线可以平滑的衔接这些路径点，且保证在曲线衔接处满足平滑条件，例如一阶微分约束、二阶微分约束等。四次条件曲线的基本形式表达如下
xi(t)＝ai bi(t-ti) ci(t-ti)2 di(t-ti)3 ei(t-ti)4(7)
其中，ai,bi,ci,di,ei分别是四次样条曲线的参数，最终的路径由若干段四次样条曲线合成。
给定路径点，存在唯一的样条参数集合，使得规划的路径通过各个路径点，且在各个路径点处满足速度连续，加速度连续等条件。针对机器人运动而言，还需要保持加加速度连续，以防机械臂产生抖振现象。因此，至少满足以下微分约束条件
给定一个末端执行器的4个期望路径点，那么需要级联3段四次样条曲线，每段样条曲线包含5个参数，共计15个未知参数。通过保证各个路径点位置约束(6个约束)；中间路径点还需满足速度、加速度、加加速度连续约束(6个约束)，以保证机器人能够平滑地执行全部路径；此外，本专利对末端的初末速度、初始加速度(即对初末路径点的速度、初始加速度)进行限制(3个约束)。由上述15个约束构建方程组，可以求解15个未知样条参数。若采样三次样条曲线，无论取多少分段，无法保证同时满足所提的约束条件，因此至少需要四次样条及以上。构建的约束方程组具体表达如下
其中，h0,h1,h2是仅与时间相关的常量，取决于时间序列t0,t1,t2,t3。通过上述方程组即可求解样条参数，进而得到机器人末端执行器的轨迹。基于rrt*算法，生成若干路径点，再由四次样条曲线拟合这些路径点，即可得到平滑可行的机器人路径。
表1为所用机器人的运动学和动力学符号说明：
表1所用运动学和动力学符号
表2为改进rrt*算法流程：
表2协调rrt*算法流程
为验证所提协调路径规划方法的有效性，采用运动学冗余机械臂抓捕空间目标，表3给出了本文所用双臂空间机器人的运动学和动力学参数，两机械臂参数相同。
表3空间机器人运动学和动力学参数
机械臂末端执行器的初始位姿分别为
终止位姿分别为
图3给出了由规划的末端执行器的路径，以及路径的执行速度。可以看出，规划的末端路径较为平滑，且速度保持在机器人末端执行速度的最大限制范围(0.1m/s)内，且路径的初始速度、加速度和终止速度都为零，符合所设计的情况，由此说明了规划路径的有效性。此外，机械臂各连杆在执行过程中亦未发生碰撞，各部件均保持在安全距离(di＝0.1m)以外，如图4所示。
1.一种基于随机采样的双臂空间机器人协调路径规划方法，包括了双臂空间机器人的运动学模型建立；通过改进rrt*算法，设计双臂空间机器人的初始路径；利用四次样条曲线拟合由改进rrt*算法生成的初始路径，得到适合于机器人执行的可行平滑路径；其特征在于步骤如下：
步骤1、建立双臂空间机器人的运动学模型：
双臂空间机器人系统由基座和2个n自由度的机械臂构成，共计2n 1个刚体，且系统不受任何外力作用；
根据泊松原理，双臂空间机器人的运动学模型为：
其中，上标a和b分别表示变量属于机械臂a和b，下标b和e分别表示变量属于基座base和末端执行器end-effector；
系统初始线/角动量m0＝[p0l0]t为：
其中，
步骤2、利用rrt*算法设计双臂空间机器人的初始路径：
对rrt*算法进行改进，在不同的采样空间中分别进行各末端执行器的快速搜索路径规划，得到其初始路径的时间序列路径点(t0,xe,0),(t1,xe,1),…,(tn,xe,n)，具体步骤为：
1.fork＝1tokdo
2.随机在不同的任务空间中对双末端分别采样
3.在原树结构中搜索新生采样点附近的点
4.进行碰撞检测，生成新的树节点
5.判断是否有更少代价的航迹，若有则替代原航迹
6.returnpath；
在原始rrt*算法基础上，对双臂各自采样空间预留0.1m宽度的欧氏距离，对双末端施加具有边界范围的笛卡尔空间约束；
步骤3、采用四次样条曲线平滑双臂空间机器人的初始路径：
采用四次样条曲线对得到的rrt*算法得到的初始路径进行平滑处理：
首先，给定由rrt*算法规划得到的初始时间序列路径点如下
(t0,xe,0),(t1,xe,1),…,(tn,xe,n)
其中，xe,i表示机器人末端执行器在时间ti的期望位置；
然后，利用四次样条曲线平滑地拟合这些路径点，其基本形式表达如下
xe,i(t)＝ai bi(t-ti) ci(t-ti)2 di(t-ti)3 ei(t-ti)4
其中，ai,bi,ci,di,ei分别是四次样条曲线的参数；
所述速度、加速度、加加速度微分约束条件：
通过上述微分约束条件，构建关于四次样条参数的线性方程组，进而求解样条参数ai,bi,ci,di,ei，得到各时间段内的连续路径；
最后，机器人的路径由各段四次样条曲线级联而成，表达如下
根据步骤1中的机器人运动学模型，通过逆运动学求解，即得到机械臂各关节运动轨迹
