CN106166749A - 一种空间移动多臂机器人的移动轨迹规划方法 - Google Patents

Abstract

一种空间移动多臂机器人的移动轨迹规划方法，包含以下步骤：首先根据空间移动多臂机器人具体位置转移任务要求，确定第一机械臂两端所需到达的位置以及第二机械臂两端所需到达的位置；进而基于五次多项式规划方法解算出第一机械臂两端的移动轨迹以及第二机械臂两端的移动轨迹；然后根据机械臂运动学方程解算出第一机械臂各个关节的移动轨迹以及第二机械臂各个关节的移动轨迹，从而确定空间移动多臂机器人的移动轨迹；最后设计碰撞检测算法，用于检验机器人移动过程中机械臂与机器人平台之间、机械臂与目标航天器之间以及机械臂与机械臂之间的碰撞安全问题。

Description

一种空间移动多臂机器人的移动轨迹规划方法

技术领域

本发明涉及一种空间移动多臂机器人的移动轨迹规划方法，属于机器人轨迹规划技术领域。

背景技术

随着航天事业的发展，航天器规模日益增大，类似于大型空间站、大型高轨通讯卫星的航天器得到广泛使用，这些航天器的尺寸通常可达到数十米甚至是百米量级。对此类航天器的在轨组装、维护、维修、部件更换等任务要求更强的空间在轨服务能力。

目前的在轨维修通常依靠宇航员出舱完成，这增大了任务的复杂性和危险性，提高了任务成本。因此研究具备在轨服务能力的空间机器人成为各个航天大国的研究热点，例如日本的ETS-VII、美国的轨道快车等任务验证了空间机器人在轨捕获卫星并进行维护的关键技术。(Oda,M."Space robot experiments on NASDA's ETS-VII satellite-preliminary overview of the experiment results."Robotics&Automation.proceedings.IEEE International Conference on 2(1999):1390-1395vol.2.Motaghedi,Pejmun."On-orbit performance of the Orbital ExpressCapture System."Proceedings of SPIE-The International Society for OpticalEngineering 6958(2008):69580E-69580E-12.)

但是现有的机器人通常仅带有一部机械臂，维护的对象为小型的合作或非合作目标。当面对大型航天器时，此类空间机器人的应用受到限制：

1.仅带有一部机械臂的空间机器人在对大型航天器维护时，需要空间机器人与目标航天器间精确的的相对位置和姿态控制，以保证仅有的机械臂完成更换部件等工作；

2.大型航天器的维护任务通常比较复杂，很可能需要进行多处维修；因此仅带有一部机械臂的空间机器人在完成在某点的维修操作后，必须通过自身携带的轨道控制发动机实现位置转移。由于空间机器人与目标航天器的相对距离很小，且后者表面形状复杂，在空间机器人位置转移过程中，容易与目标航天器发生碰撞。

因此提出基于带有多个机械臂的空间机器人在大型航天器表面完成在轨服 务任务，为保证其在航天器表面位置转移过程中的可控性和安全性，采用多个机械臂依次捕获航天器表面的不同位置，实现位置转移。在该移动阶段的控制中，系统轨迹规划是首要解决的难题。

发明内容

本发明的目的在于：克服现有技术的不足，提供了一种空间移动多臂机器人的移动轨迹规划方法，解决了空间移动多臂机器人在大型航天器表面完成在轨服务任务时，多个机械臂的移动轨迹规划问题。

本发明的技术解决方案：

一种空间移动多臂机器人的移动轨迹规划方法，包含以下步骤：

(1)根据空间移动多臂机器人具体位置转移任务要求，确定第一机械臂两端所需到达的位置以及第二机械臂两端所需到达的位置；

(2)根据步骤(1)中确定的第一机械臂两端所需到达的位置以及第二机械臂两端所需到达的位置，解算出第一机械臂两端的位移、速度和加速度的时间历程以及第二机械臂两端的位移、速度和加速度的时间历程；

(3)规划第一机械臂的运动，根据机械臂运动学方程以及步骤(2)中确定的第一机械臂两端的移动轨迹，解算出第一机械臂各个关节角位移、速度和加速度的时间历程；

(4)规划第二机械臂的运动，根据机械臂运动学方程以及步骤(2)中确定的第二机械臂两端的移动轨迹，解算出第二机械臂各个关节角位移、速度和加速度的时间历程；

(5)根据步骤(3)中确定的第一机械臂各个关节角位移以及步骤(4)中确定的第二机械臂各个关节角位移，确定空间移动多臂机器人的移动轨迹；

(6)根据所述空间移动多臂机器人的移动轨迹，判断两个机械臂之间、机械臂与机器人平台之间、机械臂与目标航天器之间是否发生碰撞，若均未发生碰撞，则空间移动多臂机器人的移动轨迹安全，否则表示移动轨迹不安全，返回步骤(1)重新规划在多臂移动过程中第一机械臂两端所需到达的位置以及第二机械臂两端所需到达的位置。

本发明与现有技术相比的有益效果为：

(1)本发明提出的空间移动多臂机器人的轨迹规划方法，提出了空间移动 多臂机器人在大型航天器表面完成在轨服务任务时，解决了空间移动多臂机器人在航天器表面移动过程的轨迹规划问题：首先，针对连接目标航天器和空间机器人本体的机械臂，规划其各个关节角，实现空间机器人本体期望的运动；其次，针对移动过程中仅连接空间机器人本体的机械臂，规划其上的关节角，实现在机器人本体大范围平动的情况下，精确到达期望的移动捕获点。通过这样的处理，将复杂移动过程简单化，便于使用各种先进的关节角求解算法。

(2)本发明的空间移动多臂机器人的轨迹规划方法，基于机械臂、机器人平台和目标航天器的几何学关系，设计了碰撞检测方法，将复杂的类似于机器人这种多体系统的碰撞问题，转化为简单的点、线、面关系判断，简化了计算流程。并且，提出的碰撞检测方法能够推广至所有空间机器人，保障空间机器人在轨操作过程的安全性。

附图说明

图1为空间移动多臂机器人移动轨迹规划流程图；

图2为空间移动多臂机器人某一步移动过程图，其中，图2(a)为空间移动多臂机器人初始状态，图2(b)为空间移动多臂机器人终止状态；

图3为空间移动多臂机器人及机械臂构型图，其中，图3(a)为空间移动多臂机器人构型图，图3(b)为机械臂构型图；

图4为第二机械臂末端移动轨迹图；

图5为机器人平台移动轨迹图；

图6为第一机械臂各关节的移动轨迹图；

图7为第二机械臂各关节的移动轨迹图；

图8为机械臂与机器人平台碰撞检测模型；

图9为特殊情况下机械臂与机器人平台碰撞检测模型，其中，图9(a)为机械臂和机器人平台在机器人平台x-y截面的投影图，图9(b)为机械臂和机器人平台在机器人平台y-z截面的投影图；

图10为机械臂与目标航天器碰撞检测模型；

图11为两个机械臂之间碰撞检测模型；

图12为机械臂碰撞示意图。

具体实施方式

本发明提供了一种空间移动多臂机器人的移动轨迹规划方法，能够有效地 规划出空间移动多臂机器人在大型航天器表面移动过程的轨迹，并保障空间移动多臂机器人在轨操作过程的安全性，而且本发明所提出的轨迹规划方法能够应用于今后所有的空间移动多臂机器人任务设计，所提的方案切实可行，便于推广；本发明提出的碰撞检测同样适用于任意构型、执行任意任务的空间机器人，因此适用范围很广，具有很大的市场竞争力和先发优势，弥补了相关领域实用性方案和理论研究内容的空白，具有很强的工程实用和理论指导意义。

下面以某一步移动为例，说明本发明所提出的空间移动多臂机器人的移动轨迹规划方法的具体步骤。如图2(a)所示，任务假定第一机械臂末端已捕获目标航天器，第二机械臂末端未捕获目标航天器，期望其到达目标航天器上某一指定位置，如图2(b)所示。移动过程分为两部分，分别由第一机械臂和第二机械臂完成。对于第一机械臂，其末端与目标航天器固定，另一端与机器人平台固定，通过臂上各关节的运动带动机器人平台平动，使机器人平台到达期望位置；对于第二机械臂，其固定于机器人平台的一端随机器人平台运动，同时通过臂上各关节的运动使其末端到达目标航天器上期望位置，相当于完成一个动基座的单臂机器人的定点抓捕操作。所述移动轨迹规划方法，首先根据第一机械臂两端所需到达的位置以及第二机械臂两端所需到达的位置，基于五次多项式规划方法解算出第一机械臂两端的移动轨迹以及第二机械臂两端的移动轨迹；进而根据机械臂运动学方程解算出第一机械臂各个关节的移动轨迹以及第二机械臂各个关节的移动轨迹，从而确定空间移动多臂机器人的移动轨迹；最后根据所述空间移动多臂机器人的移动轨迹，判断两个机械臂之间、机械臂与机器人平台之间、机械臂与目标航天器之间是否发生碰撞，若均未发生碰撞，则空间移动多臂机器人的移动轨迹安全，否则表示移动轨迹不安全，需重新规划在多臂移动过程中第一机械臂两端所需到达的位置以及第二机械臂两端所需到达的位置。

如图1所示，本发明提供的一种空间移动多臂机器人的移动轨迹规划方法，包含以下步骤：

(1)根据空间移动多臂机器人具体位置转移任务要求，确定第一机械臂两端所需到达的位置以及第二机械臂两端所需到达的位置；所述第一机械臂是指同时连接目标航天器和机器人平台的机械臂，第二机械臂是指仅连接机器人平 台的机械臂。所述机器人平台是连接机械臂和惯性空间的中心体结构，其体坐标系原点位于机器人平台质心O，x，y与z轴为机器人平台惯性主轴，且构成右手坐标系。所述第一机械臂和第二机械臂均为七自由度机械臂，即均包含七个关节和七节机械臂，每节机械臂相对其内接体均只有一个转动自由度。第j机械臂系统中的第i节机械臂体坐标系原点位于该节机械臂与其内接体的铰链处，坐标系各轴与第i节机械臂固联，j＝1,2，i＝1,2,..,7。在以下各步骤内容中，变量的右上标“j”表示有关第j机械臂系统的相关变量，变量的右下标“i”表示有关第i节机械臂的相关变量。

(2)根据步骤(1)中确定的第一机械臂两端所需到达的位置以及第二机械臂两端所需到达的位置，通过五次多项式规划方法解算出第一机械臂两端的位移、速度和加速度的时间历程以及第二机械臂两端的位移、速度和加速度的时间历程，具体为：

其中，r_w表示第一机械臂或第二机械臂某一端的平动位移，表示第一机械臂或第二机械臂某一端的速度,表示第一机械臂或第二机械臂某一端的加速度,r_w0表示第一机械臂或第二机械臂某一端的初始位置,r_wf表示第一机械臂或第二机械臂某一端所需到达的位置，w＝t1时对应第一机械臂末端，w＝t2时对应第二机械臂末端，w＝b时对应第一机械臂或第二机械臂连接机器人平台的一端，t_f是总移动时间，t∈[0，t_f]。

(3)规划第一机械臂的运动，根据机械臂运动学方程以及步骤(2)中确定的第一机械臂两端的移动轨迹，解算出第一机械臂各个关节角位移、速度和加速度的时间历程，具体为：

(3.1)结合机械臂的运动学关系，写出第一机械臂末端位置，机器人平台位置与各关节旋转角度之间的等式关系

其中，r_t1表示第一机械臂末端相对机器人平台质心的位置，r_b表示机器人平台质心的平动位移，r_b1表示第一机械臂的安装位置，r_i表示第i节机械臂体坐标系中坐标原点相对该节末端的矢径，i＝1,2,..,7；是根据第一机械臂各关节转角得到的第i节机械臂体坐标系与机器人平台坐标系之间的转换矩阵；

对(3.1)中的等式关系求导，得到第一机械臂末端速度与其他部位间的相对关系

其中，和为第一机械臂末端和机器人平台的速度，机器人平台的速度即为机械臂另一端的速度，r_i7表示在第i节机械臂坐标系中第i节机械臂坐标系原点到机械臂末端的矢径，Γ_i表示第i节关节的旋转方向，表示第一机械臂的第i个关节角速度；

(3.2)为保证第一机械臂末端垂直指向目标航天器表面，需通过改变第一机械臂各关节旋转角度以调整第一机械臂末端姿态。结合机械臂的运动学方程，写出第一机械臂末端姿态与各关节旋转角度之间的等式关系

其中，是根据第一机械臂各关节转角得到的第i节机械臂体坐标系与第7节机械臂体坐标系之间的转换矩阵；ω_t1是第一机械臂末端姿态角速度，且 通过五次多项式规划得到，具体为：

σ_m1和n_e1由第一机械臂末端初始姿态与期望姿态之间的姿态误差四元数得出

(3.3)将步骤(3.1)中得到的第一机械臂末端速度与其他部位间的相对关系与步骤(3.2)中得到的第一机械臂末端姿态与各关节旋转角度之间的等式关系联立，通过广义雅可比矩阵J描述空间机器人平台质心和第一机械臂末端运动速度与关节速度之间的关系，写出第一机械臂的运动学方程

其中在移动过程中，第一机械臂的末端与目标航天器固定，仅通过第一机械臂上各关节的运动带动机器人平台平动，则只需考虑机器人平台位置与第一机械臂各关节的运动学关系，即ω_t1＝0_3×1，

(3.4)对步骤(3.3)中得到的第一机械臂的运动学方程利用逆运动学反解，得到第一机器臂各关节的移动轨迹，即得到第一机械臂各个关节角位移、速度和加速度的时间历程，具体为：

其中J⁺为广义雅可比矩阵J的Moore-Pseudo广义逆，记为J⁺＝J^T(JJ^T)^-1； 和分别是和J⁺的一阶导数。

(4)规划第二机械臂的运动，根据机械臂运动学方程以及步骤(2)中确定的第二机械臂两端的移动轨迹，解算出第二机械臂各个关节角位移、速度和加速度的时间历程，具体为：

(4.1)结合机械臂的运动学关系，写出第二机械臂末端位置，机器人平台位置与各关节旋转角度之间的等式关系

其中，r_t2表示第二机械臂末端相对机器人平台质心的位置，r_b表示机器人平台质心的平动位移，r_b2表示第二机械臂的安装位置，r_i表示第i节机械臂体坐标系中坐标原点相对该节末端的矢径，i＝1,2,..,7；是根据第二机械臂各关节转角得到的第i节机械臂体坐标系与机器人平台坐标系之间的转换矩阵；

对(4.1)中的等式关系求导，得到第二机械臂末端速度与其他部位间的相对关系

其中，和为第二机械臂末端和机器人平台的速度，机器人平台的速度即为机械臂另一端的速度，r_i7表示在第i节机械臂坐标系中第i节机械臂坐标系原点到机械臂末端的矢径，Γ_i表示第i节关节的旋转方向，表示第二机械臂的 第i个关节角速度；

(4.2)为保证第二机械臂末端垂直指向目标航天器表面，需通过改变第二机械臂各关节旋转角度以调整第二机械臂末端姿态。结合机械臂的运动学方程，写出第二机械臂末端姿态与各关节旋转角度之间的等式关系

其中，是根据第二机械臂各关节转角得到的第i节机械臂体坐标系与第7节机械臂体坐标系之间的转换矩阵；ω_t2是第二机械臂末端姿态角速度，且 通过五次多项式规划得到，具体为：

σ_m2和n_e2由第二机械臂末端初始姿态与期望姿态之间的姿态误差四元数得出

(4.3)将步骤(4.1)中得到的第二机械臂末端速度与其他部位间的相对关系与步骤(4.2)中得到的第二机械臂末端姿态与各关节旋转角度之间的等式关系联立，通过广义雅可比矩阵J描述空间机器人平台质心和第二机械臂末端运动速度与关节速度之间的关系，写出第二机械臂的运动学方程

其中在移动过程中，第二机械臂仅连接机器人平台而末端自由，因而第二机械臂要完成的是一个动基座的单臂机器人的定点抓捕任务，需同时考虑第二机械臂末端位置，机器人平台位置与第二机械臂各关节的运动学关系，即

(4.4)对步骤(4.3)中得到的第二机械臂的运动学方程利用逆运动学反解，得到第二机器臂各关节的移动轨迹，即得到第二机械臂各个关节角位移、速度和加速度的时间历程，具体为：

其中J⁺为广义雅可比矩阵J的Moore-Pseudo广义逆，记为J⁺＝J^T(JJ^T)^-1； 和分别是和J⁺的一阶导数。

(5)根据步骤(3)中确定的第一机械臂各个关节角位移以及步骤(4)中确定的第二机械臂各个关节角位移，确定空间移动多臂机器人的移动轨迹，即各节机械臂的移动轨迹，具体为：

其中，依次为第一机械臂的七节机械臂的移动轨迹，变量的右上标表示有关第一机械臂系统的相关变量，变量的右下标表示有关第i节机械臂的相关变量，i＝1,2,...,7；依次为第二机械臂的七节机械臂的移动轨迹，变量的右上标表示有关第二机械臂系统的相关变量，变量的右下标表示有关第i节机械臂的相关变量，i＝1,2,...,7。

(6)根据所述空间移动多臂机器人的移动轨迹，判断两个机械臂之间、机械臂与机器人平台之间、机械臂与目标航天器之间是否发生碰撞，具体为：

(6.1)机械臂与机器人平台碰撞检测

机械臂与机器人平台之间的碰撞问题和各节机械臂与机器人平台轴线的几何关系有关，由此将三维空间的碰撞检查问题转化为各节机械臂在机器人平台二维横截面上的投影与横截面中心之间的最小距离问题。

将各节机械臂和机器人平台投影到机器人平台y-z截面，若机器人平台y-z截面中心O到各节机械臂的最小距离OP大于最小安全距离a，则保证机械臂与机器人平台不碰撞，具体为：

记第j机械臂的第i节机械臂两端和在y-z截面的坐标为和中间变量变量的右上标“j”表示有关第j机械臂系统的相关变量，变量的右下标“i-1”和“i”表示有关第i节机械臂两端的相关变量；

若0≤λ₁≤1，则O到的垂足在线段上，线段上距离O最近的点P为该垂足点，那么当时，不发生碰撞；当时，发生碰撞；

若λ₁>1，则O到的垂足不在线段上，线段上距离O最近的点P为那么当时，不发生碰撞；时，发生碰撞；

若λ₁<0，则O到的垂足不在线段上，线段上距离O最近的点P为那么当不发生碰撞；则发生碰撞；

(6.2)机械臂与目标航天器碰撞检测

机械臂与目标航天器之间的碰撞问题和各节机械臂与目标航天器轴线的几何关系有关，由此将三维空间的碰撞检查问题转化为各节机械臂在目标航天器二维横截面上的投影与横截面中心之间的最小距离问题。

将各节机械臂和目标航天器投影到目标航天器x-y截面，若目标航天器x-y截面中心O′到各节机械臂的最小距离O′Q大于最小安全距离r，则保证机械臂与目标航天器不碰撞，具体为：

记第j机械臂的第i节机械臂两端r_i-1,j和在x-y截面的坐标为和中间变量变量的右上标“j”表示有关第j机械臂系统的相关变量，变量的右下标“i-1”和“i”表示有关第i节机械臂两端的相关变量；

若0≤λ₂≤1，则O′到的垂足在线段上，线段上距离O′最近的点Q为该垂足点，那么当时，不发生碰撞；当时，发生碰撞；

若λ₂>1，则O′到的垂足不在线段上，线段上距离O′最近的点Q为那么当时，不发生碰撞；当时，发生碰撞；

若λ₂<0，则O′到的垂足不在线段上，线段上距离O′最近的点Q为那么当时，不发生碰撞；当 时，发生碰撞；

(6.3)两个机械臂之间碰撞检测

任意两节机械臂之间的最小距离L大于0，则保证机械臂之间不碰撞，具体为：

记中间变量u、v：

其中变量的右上标“j₁”表示有关第j₁机械臂系统的相关变量，“j₂”表示有关第j₂机械臂系统的相关变量，变量的右下标“i-1”和“i”表示有关第i节机械臂两端的相关变量，“k-1”和“k”表示有关第k节机械臂两端的相关变量，j₁＝1,2，j₂＝1,2，i＝1,2,..,7，k＝1,2,..,7，且j₁＝j₂和i＝k不同时成立。

当且仅当两连杆所在直线的公垂线的两个垂足均在连杆上且两个垂足之间的距离为零，即0≤u≤1、0≤v≤1、时，两节机械臂发生碰撞，否则不发生碰撞。

若均未发生碰撞，则空间移动多臂机器人的移动轨迹安全，否则表示移动轨迹不安全，返回步骤(1)重新规划在多臂移动过程中第一机械臂两端所需到达的位置以及第二机械臂两端所需到达的位置。

下面结合附图对本发明的工作过程和工作原理做进一步解释。

所述带有三部机械臂的空间移动多臂机器人如图3(a)所示，机械臂构型如图3(b)所示。三部机械臂中，第一机械臂和第二机械臂用于移动任务，第 三机械臂在空间机器人移动到目标位置后执行在轨操作任务。图2表示任务中180～240s阶段，如图2(a)所示，该阶段初始时第一机械臂末端已捕获目标航天器，第二机械臂末端未捕获目标航天器，期望终止时第二机械臂末端到达目标航天器上某一指定位置，如图2(b)所示。移动过程中，第一机械臂的末端与目标航天器固定不动，七关节可动并带动机器人平台发生平动；同时第二机械臂从待命状态开始运动，在60s内完成对目标点的捕获；而第三机械臂保持原始状态。

(1)根据空间移动多臂机器人具体位置转移任务要求，确定第一机械臂末端位置不变，第二机械臂末端从位置(2.25,-1.95,-1.1)m到达位置(1.85,-1,1.2)m，连接第一机械臂和第二机械臂另一端的机器人平台从位置(0,0,0)到达位置(0,0,1)m；

(2)根据步骤(1)中确定的第一机械臂两端所需到达的位置以及第二机械臂两端所需到达的位置，通过五次多项式规划方法解算出第一机械臂两端的位移、速度和加速度的时间历程以及第二机械臂两端的位移、速度和加速度的时间历程，具体为：

第一机械臂末端位置不变，第二机械臂末端移动轨迹如图4所示，连接第一机械臂和第二机械臂另一端的机器人平台移动轨迹如图5所示。图4和图5说明五次多项式规划的移动轨迹平滑、连贯，满足操作要求。

(3)规划第一机械臂的运动，根据机械臂运动学方程以及步骤(2)中确定的第一机械臂两端的移动轨迹，解算出第一机械臂各个关节角位移、速度和加速度的时间历程，具体为：

将第一机械臂末端速度与其他部位间的相对关系与第一机械臂末端姿态与各关节旋转角度之间的等式关系联立，通过广义雅可比矩阵J描述空间机器人平台质心和第一机械臂末端运动速度与关节速度之间的关系，写出第一机械臂 的运动学方程

利用逆运动学反解，得到第一机器臂各关节的移动轨迹，即得到第一机械臂各个关节角位移、速度和加速度的时间历程，具体为：

第一机器臂各关节的移动轨迹如图6所示。图6说明所提方法能够实现连接目标航天器和机器人平台的机械臂的轨迹规划，该过程中第一机械臂各关节的角运动轨迹连续而平稳，能够在机器人平台发生大范围平动同时保证机械臂移动轨迹平滑、连贯，满足操作要求。

(4)规划第二机械臂的运动，根据机械臂运动学方程以及步骤(2)中确定的第二机械臂两端的移动轨迹，解算出第二机械臂各个关节角位移、速度和加速度的时间历程，具体为：

将第二机械臂末端速度与其他部位间的相对关系与第二机械臂末端姿态与各关节旋转角度之间的等式关系联立，通过广义雅可比矩阵J描述空间机器人平台质心和第二机械臂末端运动速度与关节速度之间的关系，写出第二机械臂的运动学方程

利用逆运动学反解，得到第二机器臂各关节的移动轨迹，即得到第二机械臂各个关节角位移、速度和加速度的时间历程，具体为：

第二机器臂各关节的移动轨迹如图7所示。图7说明所提方法能够实现仅连接机器人平台的机械臂抓捕目标航天器上期望位置点的轨迹规划，该过程中第二机械臂各关节的角运动轨迹连续而平稳，能够在机器人平台发生大范围平动同时保证机械臂末端能够精确到达期望位置，并且移动轨迹平滑、连贯，满足操作要求。

(5)根据步骤(3)中确定的第一机械臂各个关节角位移以及步骤(4)中 确定的第二机械臂各个关节角位移，确定空间移动多臂机器人的移动轨迹，即各节机械臂的移动轨迹，具体为：

(6)根据所述空间移动多臂机器人的移动轨迹，判断两个机械臂之间、机械臂与机器人平台之间、机械臂与目标航天器之间是否发生碰撞，若均未发生碰撞，则空间移动多臂机器人的移动轨迹安全，否则表示移动轨迹不安全，返回步骤(1)重新规划在多臂移动过程中第一机械臂两端所需到达的位置以及第二机械臂两端所需到达的位置。具体为：

(6.1)机械臂与机器人平台碰撞检测

根据空间机器人的简化模型，机器人平台是一个边长为a＝0.8m，高为h＝1.2m的六棱柱，由于分析的是各连杆与机器人平台轴线的位置关系，采用一个圆柱包络体作为近似模型，将各连杆与机器人平台投影到y-z截面，要求机器人平台y-z截面中心O到各节机械臂的最小距离OP大于六边形边长a，如图8所示。

若O到的垂足在线段上，线段上距离O最近的点P为该垂足点，那么当时，不发生碰撞；当 时，发生碰撞；

若O到的垂足P不在线段上，线段上距离O最近的点P为那么当时，不发生碰撞；时，发生 碰撞；

若O到的垂足P不在线段上，线段上距离O最近的点P为那么当不发生碰撞；则发生碰撞；

另外，由于机器人平台是个有限长度的模型，机械臂构型可能出现如图9(a)和(b)中所示情况。这时只要该连杆上位于机器人平台边缘附近的点D与机器人平台轴线的距离大于asin60°，就能保证机械臂与机器人平台不碰撞。

(6.2)机械臂与目标航天器碰撞检测

目标航天器简化模型是一个“圆柱”结构，将各连杆与目标航天器投影到x-y截面，要求目标航天器x-y截面中心O′到各节机械臂的最小距离O′Q大于圆柱半径r＝2.5m，如图10所示。

若O′到的垂足在线段上，线段上距离O′最近的点Q为该垂足点，那么当时，不发生碰撞；当 时，发生碰撞；

若O′到的垂足不在线段上，线段上距离O′最近的点Q为那么当时，不发生碰撞；当时，发生碰撞；

若O′到的垂足不在线段上，线段上距离O′最近的点Q为那么当时，不发生碰撞；当时，发生碰撞；

(6.3)两个机械臂之间碰撞检测

如图11所示，当且仅当两连杆所在直线的公垂线的两个垂足均在连杆上且两个垂足之间的距离为零，即0≤u≤1、0≤v≤1、 时，两节机械臂发生碰撞，否则不发生碰撞。

为了说明碰撞检测算法的结果，将每次检测结果用matlab软件中fprintf('有碰撞\n')或fprintf('无碰撞\n')语句输出，并采用计数方式统计机械臂与机器人平台之间，机械臂与目标航天器之间以及机械臂与机械臂之间的碰撞情况。

在对所规划的单步移动过程进行碰撞检测之前，首先给出一组特殊算例，以验证所设计的碰撞检测算法合理有效。

显而易见，由于机器人与目标航天器在z轴方向距离过近，两个机械臂都插入到目标航天器中。如图12所示，第一机械臂的杆3～7和第二机械臂的杆4～7都与目标航天器发生碰撞，而机械臂与自身机器人平台之间，以及机械臂与机械臂之间无碰撞。表格1碰撞情况输出结果给出了详细的碰撞情况，

表格2给出了碰撞次数统计结果，表明检测结果与实际情况一致。

表格1碰撞情况输出结果

表格2碰撞统计结果

确定了该方法的有效性，进一步通过这种碰撞检测对所规划的单步移动过程的安全性进行验证。表格1碰撞情况输出结果给出了详细的碰撞情况，表格4给出了碰撞次数统计结果。显然，统计结果都为零，证明了机械臂与机器人平台之间，机械臂与目标航天器之间以及机械臂与机械臂之间均无碰撞发生，即说明所规划的轨迹是安全的。

表格3单步移动过程碰撞情况输出结果

表格4单步移动过程碰撞统计结果

本发明保护范围不仅局限于本实施例，本实施例用于解释本发明，凡与本发明在相同原理和构思条件下的变更或修改均在本发明公开的保护范围之内。

Claims (8)

1.一种空间移动多臂机器人的移动轨迹规划方法，其特征在于包含以下步骤：

(1)根据空间移动多臂机器人具体位置转移任务要求，确定第一机械臂两端所需到达的位置以及第二机械臂两端所需到达的位置；

(2)根据步骤(1)中确定的第一机械臂两端所需到达的位置以及第二机械臂两端所需到达的位置，解算出第一机械臂两端的位移、速度和加速度的时间历程以及第二机械臂两端的位移、速度和加速度的时间历程；

(3)规划第一机械臂的运动，根据机械臂运动学方程以及步骤(2)中确定的第一机械臂两端的移动轨迹，解算出第一机械臂各个关节角位移、速度和加速度的时间历程；

(4)规划第二机械臂的运动，根据机械臂运动学方程以及步骤(2)中确定的第二机械臂两端的移动轨迹，解算出第二机械臂各个关节角位移、速度和加速度的时间历程；

(5)根据步骤(3)中确定的第一机械臂各个关节角位移以及步骤(4)中确定的第二机械臂各个关节角位移，确定空间移动多臂机器人的移动轨迹；

(6)根据所述空间移动多臂机器人的移动轨迹，判断两个机械臂之间、机械臂与机器人平台之间、机械臂与目标航天器之间是否发生碰撞，若均未发生碰撞，则空间移动多臂机器人的移动轨迹安全，否则表示移动轨迹不安全，返回步骤(1)重新规划在多臂移动过程中第一机械臂两端所需到达的位置以及第二机械臂两端所需到达的位置。

2.根据权利要求1所述的一种空间移动多臂机器人的移动轨迹规划方法，其特征在于：所述第一机械臂是指同时连接目标航天器和机器人平台的机械臂，第二机械臂是指仅连接机器人平台的机械臂。

3.根据权利要求1或2所述的一种空间移动多臂机器人的移动轨迹规划方法，其特征在于：所述第一机械臂和第二机械臂均为七自由度机械臂，即均包含七个关节和七节机械臂。

4.根据权利要求1所述的一种空间移动多臂机器人的移动轨迹规划方法，其特征在于：所述步骤(2)根据第一机械臂两端所需到达的位置以及第二机械臂两端所需到达的位置，解算出第一机械臂两端的位移、速度和加速度的时间历程以及第二机械臂两端的位移、速度和加速度的时间历程，具体为：

$r_w\left(t\right)=r_{w0}\left(t\right)+\frac{6(r_{wf}-r_{w0})}{{t_f}^5}{t}^5-\frac{15(r_{wf}-r_{w0})}{{t_f}^4}{t}^4+\frac{10(r_{wf}-r_{w0})}{{t_f}^3}{t}^3$

${\stackrel{\&CenterDot;}{r}}_w\left(t\right)=\frac{30(r_{wf}-r_{w0})}{{t_f}^5}{t}^4-\frac{60(r_{wf}-r_{w0})}{{t_f}^4}{t}^3+\frac{30(r_{wf}-r_{w0})}{{t_f}^3}{t}^2$

${\stackrel{\&CenterDot;\&CenterDot;}{r}}_w\left(t\right)=\frac{120(r_{wf}-r_{w0})}{{t_f}^5}{t}^3-\frac{180(r_{wf}-r_{w0})}{{t_f}^4}{t}^2+\frac{60(r_{wf}-r_{w0})}{{t_f}^3}t$

其中，r_w表示第一机械臂或第二机械臂某一端的平动位移，表示第一机械臂或第二机械臂某一端的速度,表示第一机械臂或第二机械臂某一端的加速度,r_w0表示第一机械臂或第二机械臂某一端的初始位置,r_wf表示第一机械臂或第二机械臂某一端所需到达的位置，w＝t1时对应第一机械臂末端，w＝t2时对应第二机械臂末端，w＝b时对应第一机械臂或第二机械臂连接机器人平台的一端，t_f是总移动时间，t∈[0，t_f]。

5.根据权利要求1所述的一种空间移动多臂机器人的移动轨迹规划方法，其特征在于：所述步骤(3)解算出第一机械臂各个关节角位移、速度和加速度的时间历程，具体为：

(5.1)结合机械臂的运动学关系，写出第一机械臂末端位置，机器人平台位置与各关节旋转角度之间的等式关系

$r_{t1}\left(t\right)=r_b\left(t\right)+r_{b1}+\underset{i=1}{\overset{7}{\&Sigma;}}{A}_{bi}\left({\mathrm{\&theta;}}_i^1\right)r_i$

其中，r_t1表示第一机械臂末端相对机器人平台质心的位置，r_b表示机器人平台质心的平动位移，r_b1表示第一机械臂的安装位置，r_i表示第i节机械臂体坐标系中坐标原点相对该节末端的矢径，i＝1,2,..,7；是根据第一机械臂各关节转角得到的第i节机械臂体坐标系与机器人平台坐标系之间的转换矩阵；

对(5.1)中的等式关系求导，得到第一机械臂末端速度与其他部位间的相对关系

${\stackrel{\&CenterDot;}{r}}_{t1}\left(t\right)={\stackrel{\&CenterDot;}{r}}_b\left(t\right)-\underset{i=1}{\overset{7}{\&Sigma;}}{A}_{bi}\left({\mathrm{\&theta;}}_i^1\right){\tilde{r}}_{i7}{\mathrm{\&Gamma;}}_i{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_i^1$

其中，和为第一机械臂末端和机器人平台的速度，机器人平台的速度即为机械臂另一端的速度，r_i7表示在第i节机械臂坐标系中第i节机械臂坐标系原点到机械臂末端的矢径，Γ_i表示第i节关节的旋转方向，表示第一机械臂的第i个关节角速度；

(5.2)结合机械臂的运动学方程，写出第一机械臂末端姿态与各关节旋转角度之间的等式关系

${\mathrm{\&omega;}}_{t1}=\underset{i=1}{\overset{7}{\&Sigma;}}{A}_{7i}\left({\mathrm{\&theta;}}_i^1\right){\mathrm{\&Gamma;}}_i{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_i^1$

其中，是根据第一机械臂各关节转角得到的第i节机械臂体坐标系与第7节机械臂体坐标系之间的转换矩阵；ω_t1是第一机械臂末端姿态角速度，且 通过五次多项式规划得到，具体为：

${\stackrel{\&CenterDot;}{\mathrm{\&sigma;}}}_1\left(t\right)=\frac{30{\mathrm{\&sigma;}}_{m1}}{{t_f}^5}{t}^4-\frac{60{\mathrm{\&sigma;}}_{m1}}{{t_f}^4}{t}^3+\frac{30{\mathrm{\&sigma;}}_{m1}}{{t_f}^3}{t}^2$

σ_m1和n_e1由第一机械臂末端初始姿态与期望姿态之间的姿态误差四元数得出

${\mathrm{\&sigma;}}_{m1}=2arccos\left(q_{e0}^1\right),n_{e1}=\frac{1}{sin\frac{{\mathrm{\&sigma;}}_{m1}}{2}}{\stackrel{\&OverBar;}{q}}_e^1;$

(5.3)将步骤(5.1)中得到的第一机械臂末端速度与其他部位间的相对关系与步骤(5.2)中得到的第一机械臂末端姿态与各关节旋转角度之间的等式关系联立，通过广义雅可比矩阵J描述空间机器人平台质心和第一机械臂末端运动速度与关节速度之间的关系，写出第一机械臂的运动学方程

${\stackrel{\&CenterDot;}{X}}_{E1}=J{\stackrel{\&CenterDot;}{\mathrm{\&eta;}}}_1$

其中

(5.4)对步骤(5.3)中得到的第一机械臂的运动学方程利用逆运动学反解，得到第一机器臂各关节的移动轨迹，即得到第一机械臂各个关节角位移、速度和加速度的时间历程，具体为：

${\mathrm{\&eta;}}_1=\&Integral;{\stackrel{\&CenterDot;}{\mathrm{\&eta;}}}_{1}dt;$

${\stackrel{\&CenterDot;}{\mathrm{\&eta;}}}_1=J^{+}{\stackrel{\&CenterDot;}{X}}_{E1};$

${\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&eta;}}}_1=J^{+}{\stackrel{\&CenterDot;\&CenterDot;}{X}}_{E1}+{\stackrel{\&CenterDot;}{J}}^{+}{\stackrel{\&CenterDot;}{X}}_{E1};$

其中J⁺为广义雅可比矩阵J的Moore-Pseudo广义逆，记为J⁺＝J^T(JJ^T)^-1；和分别是和J⁺的一阶导数。

6.根据权利要求1所述的一种空间移动多臂机器人的移动轨迹规划方法，其特征在于：所述步骤(4)解算出第二机械臂各个关节角位移、速度和加速度的时间历程，具体为：

(6.1)结合机械臂的运动学关系，写出第二机械臂末端位置，机器人平台位置与各关节旋转角度之间的等式关系

$r_{t2}\left(t\right)=r_b\left(t\right)+r_{b2}+\underset{i=1}{\overset{7}{\&Sigma;}}{A}_{bi}\left({\mathrm{\&theta;}}_i^2\right)r_i$

其中，r_t2表示第二机械臂末端相对机器人平台质心的位置，r_b表示机器人平台质心的平动位移，r_b2表示第二机械臂的安装位置，r_i表示第i节机械臂体坐标系中坐标原点相对该节末端的矢径，i＝1,2,..,7；是根据第二机械臂各关节转角得到的第i节机械臂体坐标系与机器人平台坐标系之间的转换矩阵；

对(6.1)中的等式关系求导，得到第二机械臂末端速度与其他部位间的相对关系

${\stackrel{\&CenterDot;}{r}}_{t2}\left(t\right)={\stackrel{\&CenterDot;}{r}}_b\left(t\right)-\underset{i=1}{\overset{7}{\&Sigma;}}{A}_{bi}\left({\mathrm{\&theta;}}_i^2\right){\tilde{r}}_{i7}{\mathrm{\&Gamma;}}_i{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_i^2$

其中，和为第二机械臂末端和机器人平台的速度，机器人平台的速度即为机械臂另一端的速度，r_i7表示在第i节机械臂坐标系中第i节机械臂坐标系原点到机械臂末端的矢径，Γ_i表示第i节关节的旋转方向，表示第二机械臂的第i个关节角速度；

(6.2)结合机械臂的运动学方程，写出第二机械臂末端姿态与各关节旋转角度之间的等式关系

${\mathrm{\&omega;}}_{t2}=\underset{i=1}{\overset{7}{\&Sigma;}}{A}_{7i}\left({\mathrm{\&theta;}}_i^2\right){\mathrm{\&Gamma;}}_i{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_i^2$

其中，是根据第二机械臂各关节转角得到的第i节机械臂体坐标系与第7节机械臂体坐标系之间的转换矩阵；ω_t2是第二机械臂末端姿态角速度，且 通过五次多项式规划得到，具体为：

${\stackrel{\&CenterDot;}{\mathrm{\&sigma;}}}_2\left(t\right)=\frac{30{\mathrm{\&sigma;}}_{m2}}{{t_f}^5}{t}^4-\frac{60{\mathrm{\&sigma;}}_{m2}}{{t_f}^4}{t}^3+\frac{30{\mathrm{\&sigma;}}_{m2}}{{t_f}^3}{t}^2$

σ_m2和n_e2由第二机械臂末端初始姿态与期望姿态之间的姿态误差四元数得出

${\mathrm{\&sigma;}}_{m2}=2\arccos \left(q_{e0}^2\right),n_{e2}=\frac{1}{sin\frac{{\mathrm{\&sigma;}}_{m2}}{2}}{\stackrel{\&OverBar;}{q}}_e^2$

(6.3)将步骤(6.1)中得到的第二机械臂末端速度与其他部位间的相对关系与步骤(6.2)中得到的第二机械臂末端姿态与各关节旋转角度之间的等式关系联立，通过广义雅可比矩阵J描述空间机器人平台质心和第二机械臂末端运动速度与关节速度之间的关系，写出第二机械臂的运动学方程

${\stackrel{\&CenterDot;}{X}}_{E2}=J{\stackrel{\&CenterDot;}{\mathrm{\&eta;}}}_2$

其中

(6.4)对步骤(6.3)中得到的第二机械臂的运动学方程利用逆运动学反解，得到第二机器臂各关节的移动轨迹，即得到第二机械臂各个关节角位移、速度和加速度的时间历程，具体为：

${\mathrm{\&eta;}}_2=\&Integral;{\stackrel{\&CenterDot;}{\mathrm{\&eta;}}}_{2}dt$

${\stackrel{\&CenterDot;}{\mathrm{\&eta;}}}_2=J^{+}{\stackrel{\&CenterDot;}{X}}_{E2}$

${\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&eta;}}}_2=J^{+}{\stackrel{\&CenterDot;\&CenterDot;}{X}}_{E2}+{\stackrel{\&CenterDot;}{J}}^{+}{\stackrel{\&CenterDot;}{X}}_{E2}$

其中J⁺为广义雅可比矩阵J的Moore-Pseudo广义逆，记为J⁺＝J^T(JJ^T)^-1；和分别是和J⁺的一阶导数。

7.根据权利要求1所述的一种空间移动多臂机器人的移动轨迹规划方法，其特征在于：所述步骤(5)根据第一机械臂各个关节角位移以及第二机械臂各个关节角位移确定空间移动多臂机器人的移动轨迹，即各节机械臂的移动轨迹，具体为：

$\begin{array}{cc}{r}_1^1\left(t\right)=r_b\left(t\right)+r_{b1}+A_{b1}\left({\mathrm{\&theta;}}_1^1\right)r_1& r_1^2\left(t\right)=r_b\left(t\right)+r_{b2}+A_{b1}\left({\mathrm{\&theta;}}_1^2\right)r_1\end{array}$

$\begin{array}{cc}{r}_2^1\left(t\right)=r_b\left(t\right)+r_{b1}+\underset{i=1}{\overset{2}{\&Sigma;}}{A}_{bi}\left({\mathrm{\&theta;}}_i^1\right)r_i& r_2^2\left(t\right)=r_b\left(t\right)+r_{b2}+\underset{i=1}{\overset{2}{\&Sigma;}}{A}_{bi}\left({\mathrm{\&theta;}}_i^2\right)r_i\end{array}$

$\begin{array}{cc}{r}_3^1\left(t\right)=r_b\left(t\right)+r_{b1}+\underset{i=1}{\overset{3}{\&Sigma;}}{A}_{bi}\left({\mathrm{\&theta;}}_i^1\right)r_i& r_3^2\left(t\right)=r_b\left(t\right)+r_{b2}+\underset{i=1}{\overset{3}{\&Sigma;}}{A}_{bi}\left({\mathrm{\&theta;}}_i^2\right)r_i\end{array}$

$r_4^1\left(t\right)=r_b\left(t\right)+r_{b1}+\underset{i=1}{\overset{4}{\&Sigma;}}{A}_{bi}\left({\mathrm{\&theta;}}_i^1\right)r_i,r_4^2\left(t\right)=r_b\left(t\right)+r_{b2}+\underset{i=1}{\overset{4}{\&Sigma;}}{A}_{bi}\left({\mathrm{\&theta;}}_i^2\right)r_i;$

$\begin{array}{cc}{r}_5^1\left(t\right)=r_b\left(t\right)+r_{b1}+\underset{i=1}{\overset{5}{\&Sigma;}}{A}_{bi}\left({\mathrm{\&theta;}}_i^1\right)r_i& r_5^2\left(t\right)=r_b\left(t\right)+r_{b2}+\underset{i=1}{\overset{5}{\&Sigma;}}{A}_{bi}\left({\mathrm{\&theta;}}_i^2\right)r_i\end{array}$

$\begin{array}{cc}{r}_6^1\left(t\right)=r_b\left(t\right)+r_{b1}+\underset{i=1}{\overset{6}{\&Sigma;}}{A}_{bi}\left({\mathrm{\&theta;}}_i^1\right)r_i& r_6^2\left(t\right)=r_b\left(t\right)+r_{b2}+\underset{i=1}{\overset{6}{\&Sigma;}}{A}_{bi}\left({\mathrm{\&theta;}}_i^2\right)r_i\end{array}$

$\begin{array}{cc}{r}_7^1\left(t\right)=r_b\left(t\right)+r_{b1}+\underset{i=1}{\overset{7}{\&Sigma;}}{A}_{bi}\left({\mathrm{\&theta;}}_i^1\right)r_i& r_7^2\left(t\right)=r_b\left(t\right)+r_{b2}+\underset{i=1}{\overset{7}{\&Sigma;}}{A}_{bi}\left({\mathrm{\&theta;}}_i^2\right)r_i\end{array}$

其中，依次为第一机械臂的七节机械臂的移动轨迹，依次为第二机械臂的七节机械臂的移动轨迹。

8.根据权利要求1所述的一种空间移动多臂机器人的移动轨迹规划方法，其特征在于：所述步骤(6)根据所述空间移动多臂机器人的移动轨迹，判断两个机械臂之间、机械臂与机器人平台之间、机械臂与目标航天器之间是否发生碰撞，具体为：

(8.1)机械臂与机器人平台碰撞检测

将各节机械臂和机器人平台投影到机器人平台y-z截面，若机器人平台y-z截面中心O到各节机械臂的最小距离OP大于最小安全距离a，则保证机械臂与机器人平台不碰撞，具体为：

记第j机械臂的第i节机械臂两端和在y-z截面的坐标为和j＝1,2，i＝1,2,..,7；中间变量

若0≤λ₁≤1，不发生碰撞；则发生碰撞；

若λ₁>1，不发生碰撞；则发生碰撞；

若λ₁<0，不发生碰撞；则发生碰撞；

(8.2)机械臂与目标航天器碰撞检测

将各节机械臂和目标航天器投影到目标航天器x-y截面，若目标航天器x-y截面中心O′到各节机械臂的最小距离O′Q大于最小安全距离r，则保证机械臂与目标航天器不碰撞，具体为：

记第j机械臂的第i节机械臂两端r_i-1，j和在x-y截面的坐标为和j＝1，2，i＝1，2，..，7；中间变量

若0≤λ₂≤1，不发生碰撞；则发生碰撞；

若λ₂＞1，不发生碰撞；则发生碰撞；

若λ₂＜0，不发生碰撞；则发生碰撞；

(8.3)两个机械臂之间碰撞检测

任意两节机械臂之间的最小距离L大于0，则保证机械臂之间不碰撞，具体为：

记中间变量u、v：

$u=\frac{{(r_i^{j_1}-r_{i-1}^{j_1})}^T(r_k^{j_2}-r_{k-1}^{j_2}){(r_k^{j_2}-r_{k-1}^{j_2})}^T(r_{i-1}^{j_1}-r_{k-1}^{j_2})-\left|\right|r_k^{j_2}-r_{k-1}^{j_2}|{|}^2{(r_i^{j_1}-r_{i-1}^{j_1})}^T(r_{i-1}^{j_1}-r_{k-1}^{j_2})}{\left|\right|r_i^{j_1}-r_{i-1}^{j_1}\left|{|}^2\right||r_k^{j_2}-r_{k-1}^{j_2}|{|}^2-{\&lsqb;{(r_i^{j_1}-r_{i-1}^{j_1})}^T(r_k^{j_2}-r_{k-1}^{j_2})\&rsqb;}^2}$

$v=\frac{{(r_k^{j_2}-r_{k-1}^{j_2})}^T(r_{i-1}^{j_1}-r_{k-1}^{j_2})\left|\right|r_i^{j_1}-r_{i-1}^{j_1}|{|}^2-{(r_i^{j_1}-r_{i-1}^{j_1})}^T(r_{i-1}^{j_1}-r_{k-1}^{j_2}){(r_k^{j_2}-r_{k-1}^{j_2})}^T(r_i^{j_1}-r_{i-1}^{j_1})}{\left|\right|r_i^{j_1}-r_{i-1}^{j_1}\left|{|}^2\right||r_k^{j_2}-r_{k-1}^{j_2}|{|}^2-{\&lsqb;{(r_i^{j_1}-r_{i-1}^{j_1})}^T(r_k^{j_2}-r_{k-1}^{j_2})\&rsqb;}^2}$

其中j₁＝1，2，j₂＝1，2，i＝1，2，..，7，k＝1，2，..，7，且j₁＝j₂和i＝k不同时成立。

当且仅当0≤u≤1、0≤v≤1、时，两节机械臂发生碰撞，否则不发生碰撞。