CN103009389B - 一种冗余空间机械臂在轨抓捕的轨迹规划方法 - Google Patents

Abstract

一种冗余空间机械臂在轨抓捕的无碰撞无奇异轨迹规划方法，通过将空间机械臂的抓捕操作分为粗略抓捕段和精细抓捕段，将问题进行了简化。针对不同阶段的突出问题，采用了不同的规划算法。在粗略抓捕段采用基于构型空间的轨迹搜索算法，该算法基于构型空间中的试探；在精细抓捕段采用基于运动学的伪逆算法，通过对目标相对运动的补偿保证最终的精确抓捕。

Description

一种冗余空间机械臂在轨抓捕的轨迹规划方法

技术领域

本发明涉及一种冗余空间机械臂在轨抓捕的轨迹规划方法，属于航天器姿态控制领域。

背景技术

空间机械臂在轨抓捕操作中，为了节省燃料，不对空间操控平台的位置加以控制，只对空间操控平台的姿态进行稳定控制，属于姿态受控的自由飞行机器人，因此在***模型的变量中可以加以任意控制的只有机械臂的各个关节角。轨迹规划的基本要求即是以期望的末端作用器的位置和姿态指向为目标，来通过一定的算法确定出各个关节角的角位置、角速度和角加速度轨迹，从而由控制器实现机械臂的抓捕操作。这里冗余是指，机械臂具有7个自由度，除了能够使得末端作用器到达指定的位置姿态之外，还具有多余的自由度用于规避与障碍物的碰撞，同时可以采用多个机械臂协同操控，使得灵活更换末端作用器、稳固安全可靠地抓捕非合作目标。

机械臂在抓捕操作中如果与障碍物发生碰撞，则空间操控平台受到未知的反作用力的作用，对整个***带来未知干扰力和干扰力矩，使平台出现无法预料的运动，破坏平台的姿态稳定和机械臂的运动稳定，不仅会导致抓捕操作的失败，还可能对空间操控平台造成不可预测的损害。因此，规避碰撞是空间机械臂抓捕操作轨迹规划中必须考虑的因素。此外，机械臂的奇异问题和目标相对运动的补偿问题也是轨迹规划中必须考虑的问题。

机械臂的轨迹规划算法主要有伪逆算法、人工势场法和基于构型空间的自由空间法。伪逆算法在求解中易出现奇异。人工势场法存在局部最小值问题，且对于复杂问题，计算量过大，较适用于移动机器人的轨迹规划。构型空间中，构型点的坐标为机械臂的各关节角。由运动学关系将作用器目标位姿和障碍物由工作空间映射到构型空间，得到目标构型点和构型空间障碍。无碰撞轨迹规划即要在构型空间中搜索出连接初始和目标构型点且不经过障碍的路径。目前存在的方法都需求解构型空间，对于多自由度机械臂计算量很大，构型空间障碍只能通过离散化得到，为提高精度需占用大量内存。这导致现存的算法无法应用于计算资源紧张的空间机械臂，难以应用于航天任务。

发明内容

本发明所要解决的技术问题是：克服现有技术的不足，提供一种无碰撞无奇异的冗余空间机械臂在轨抓捕的轨迹规划方法，该方法工程实施简单，能够节省计算资源。

本发明的技术方案是：

一种冗余空间机械臂在轨抓捕的轨迹规划方法；所述冗余空间机械臂安装在空间操控平台上，机械臂末端安装末端作用器；空间操控平台装有视觉测量敏感器和星载计算机；冗余空间机械臂在轨抓捕的对象是空间非合作目标；将空间机械臂的抓捕操作分为粗略抓捕段和精细抓捕段，

粗略抓捕段的轨迹规划方法包括如下步骤：

(1.1)由粗略抓捕段末端作用器的期望位置和姿态，利用无碰撞目标构型求解算法求得无碰撞的目标构型；

(1.2)根据机械臂当前构型和目标构型，利用无碰撞路径搜索算法，求得无碰撞的构型空间中的路径；

(1.3)利用路径优化算法，对无碰撞的构型空间中的路径进行优化获得不与障碍物相碰的机械臂的各个关节角的运动序列；

精细抓捕段的轨迹规划方法包括如下步骤：

(2.1)由期望的抓捕目标的末端作用器位置/姿态和当前末端作用器的位置/姿态预先设计出末端作用器位置/姿态的运动轨迹，速度轨迹和加速度轨迹；

(2.2)根据视觉测量敏感器测量得到的目标相对运动对所述末端作用器位置/姿态的运动轨迹，速度轨迹和加速度轨迹进行补偿获得补偿后的末端作用器位置/姿态的运动轨迹，速度轨迹和加速度轨迹；

(2.3)根据补偿后的末端作用器位置/姿态的运动轨迹，速度轨迹和加速度轨迹采用基于运动学的伪逆算法获得机械臂的各个关节角的角速度轨迹和角加速度轨迹。

所述步骤(1.1)中，无碰撞目标构型求解算法如下：

首先根据目标构型求解算法计算目标构型；

目标构型求解算法的输入为机械臂当前的各个关节角，以及抓捕目标所需的末端作用器的位置和姿态，目标构型求解算法利用数值积分方法求解机械臂逆运动学方程得到抓捕目标所需的各个关节角，即目标构型；

然后利用碰撞检测算法判断计算得到的目标构型是否与障碍物相碰，如果与障碍物相碰，则对输入至目标构型求解算法的机械臂当前的关节角进行调整，按照目标构型求解算法重新进行计算，直至得到无碰撞的目标构型。

所述步骤(1.2)中无碰撞路径搜索算法将每个关节角从当前构型到目标构型的总转角都平均分成N份，将每一个关节角转过每份转角定义为一个子运动，从而形成了pN个子运动，其中p为关节数目；

记第i个关节角的子运动为子运动i，i＝1，2，...，P，无碰撞路径搜索算法将pN个子运动以合适的顺序排列，形成一个pN步的运动序列；

设定序号生成函数F(i)，该函数由当前子运动序号i生成下一步要试探的子运动序号；F(i)为递增、递减或随机生成函数；

用m表示步号，T表示试探总次数，T_max表示可接受最大试探次数；

无碰撞路径搜索算法步骤如下：

1)初始化，令m＝1，T＝1，选取i，设置T_max；

2)判断m是否等于pN+1；

若m＝pN+1，则输出无碰撞的关节运动序列，并结束搜索算法；否则利用碰撞检测算法，判断机械臂第m步执行子运动i是否与障碍物碰撞；如果不碰撞，机械臂执行予运动i，并做记录，执行步骤3)；如果碰撞，执行步骤4)；

3)m＝m+1，i＝F(i)，T＝T+1，执行步骤6)；

4)判断第m步是否已经试探了所有可能的子运动i，若是，执行步骤5)；否则i＝F(i)，T＝T+I，执行步骤6)；

5)跳回第m-1步，令m＝m-1；判断第m步是否已经试探了所有可能的子运动，若是重新执行步骤5)；否则令i＝F(i(m))，i(m)指第m步原来采用的子运动序号，T＝T+1，执行步骤6)；

6)判断T是否等于T_max，

若T＝T_max，则输出算法搜索失败并结束搜索算法，否则执行步骤2)；

若算法搜索失败，则采用不同的试探规律F(i)或增大路径分割常数N重新执行无碰撞路径搜索算法。

所述步骤(1.3)中路径优化算法方法如下：将无碰撞路径搜索算法搜索到的无碰撞的机械臂的各个关节角的运动序列中相邻的两个子运动合并成一个子运动，然后采用碰撞检测算法检测合并后的子运动是否与障碍物相碰；如果不相碰，则将这两个运动合并；若相碰则取消合并；重复上述过程，直至得到的运动序列中相邻的子运动无法进一步合并。

所述碰撞检测算法方法如下：将机械臂简化为一些连接的线段，然后进行空间上的离散化，离散化为机械臂上的一系列的点，把障碍物分成几部分，每一部分找到一个凸多面体的包络，然后用这些凸多面体近似代替障碍物本身；记某个凸多面体的第i个面的中心点为P_i，第i个面的外法线矢量为记待检测的点为P_T，则点P_T位于这个凸多面体的充分必要条件是：对于凸多面体的所有的面都有若存在待检测点在障碍物内部，则认为机械臂与障碍物相碰，否则认为机械臂与障碍物不会发生碰撞。

所述步骤(2.3)中的机械臂的各个关节角的角速度轨迹公式为：

${\stackrel{\·}{q}}_r\left(t\right)=J^{+}[{\stackrel{\·}{X}}_{\mathrm{Er}}\left(t\right)+\mathrm{\δ}\·(X_{\mathrm{Er}}\left(t\right)-X_E\left(t\right))]$

其中，X_Er(t)、和为补偿后的末端作用器位置/姿态的运动轨迹和速度轨迹，X_E(t)为机械臂实际运动过程中末端作用器的位置/姿态；δ·(X_Er(t)-X_E(t))为加入的比例调节项，其中δ＞0；J⁺＝J^T(JJ^T)^-1为雅克比矩阵J的伪逆；

所述机械臂的各个关节角的关节角加速度轨迹为：

${\stackrel{\·\·}{q}}_r\left(t\right)=J^{+}[{\stackrel{\·\·}{X}}_{\mathrm{Er}}\left(t\right)+\mathrm{\δ}\·({\stackrel{\·}{X}}_{\mathrm{Er}}\left(t\right)-{\stackrel{\·}{X}}_E\left(t\right))]+{\stackrel{\·}{J}}^{+}[{\stackrel{\·}{X}}_{\mathrm{Er}}\left(t\right)+\mathrm{\δ}\·(X_{\mathrm{Er}}\left(t\right)-X_E\left(t\right))]$

其中，为补偿后的末端作用器位置/姿态的加速度轨迹；为雅克比矩阵J的伪逆的时间导数。

所述精细抓捕段的轨迹规划方法还包括如下步骤：利用机械臂的冗余自由度，采用空转使机械臂远离奇异构型。

带空转的机械臂的各个关节角的角速度轨迹为

${{\stackrel{\·}{q}}_r}^{\′}={\stackrel{\·}{q}}_r\left(t\right)+P_{J}y,$

P_J＝(I-J^T(JJ^T)^-1J)为雅可比矩阵J的正交投影矩阵；列向量y＝αv；ξ(q)为奇异度量：ξ(q)＝det(JJ^T)；标量参数α选取为：上标“.”代表求导运算；上标“T”代表求转置运算；上标“-1”代表求逆运算；代表标量ξ对矢量q求梯度。

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

(1)本发明是一种全新的针对空间冗余机械臂在轨抓捕操作的无碰撞无奇异轨迹规划算法。通过将空间机械臂的抓捕操作分为粗略抓捕段和精细抓捕段，将问题进行了简化。针对不同阶段的突出问题，采用了不同的规划算法。在粗略抓捕段采用基于构型空间的轨迹搜索算法，该算法基于构型空间中的试探；在精细抓捕段采用基于运动学的伪逆算法，通过对目标相对运动的补偿保证最终的精确抓捕。

(2)该方法在粗略抓捕段通过基于构型空间的轨迹搜索算法，实现了对障碍物的规避。构型空间是以机械臂各个关节角为坐标的一个高维的抽象空间。这种基于在构型空间中试探的轨迹搜索算法，不需要求解构型空间，通过碰撞检测试探就可以在构型空间中找出无碰撞路径，与现有技术相比，节约了计算资源和内存，适用于计算资源紧张的空间机械臂的轨迹的快速规划，为航天工程应用提供了一种简单有效的方法。通过轨迹优化算法，得到的最终轨迹较为平滑，易于工程实现。

(3)该方法在精细抓捕段通过加入目标相对运动补偿的基于运动学的伪逆算法，可以通过空间操控平台上的视觉敏感器的测量信息，获得目标的相对运动，将目标的相对运动引入到伪逆算法中，实现对目标相对运动的补偿。此外，在伪逆算法中，加入比例调节项，对轨迹的跟随误差进行补偿，增加***的抗干扰能力和鲁棒性，保证了在精细抓捕段，机械臂对目标的准确抓捕。

(4)本发明中所涉及的方法工程实施简单，推广应用前景良好。该方法不仅满足了空间机械臂无碰撞无奇异轨迹规划的基本要求，而且算法简单易于实施，节约计算时间，节省计算资源和内存，特别适用于计算资源紧张的空间机械臂的快速轨迹规划。该方法尤其适用于空间机械臂对非合作目标的快速抓捕，也适用于月球车上机械臂的无碰撞轨迹规划。

附图说明

图1为本发明空间操控平台抓捕空间目标示意图；

图2为本发明无碰撞无奇异的机械臂轨迹规划算法的流程示意图；

图3为本发明中无碰撞路径搜索算法的流程示意图；

图4为本发明中7自由度机械臂的初始构型和转轴正方向；

图5为本发明中算法平滑前的无碰撞路径；

图6为本发明中算法平滑前的关节角速度；其中图6(a)中曲线11、12、13分别是第1、2、3关节的角速度变化曲线；图6(b)中曲线14、15、16分别是第4、5、6关节的角速度变化曲线；

图7为本发明中算法平滑后的无碰撞路径；

图8为本发明中算法平滑后的关节角速度；其中图8(a)中曲线21、22、23分别是第1、2、3关节的角速度变化曲线；图8(b)中曲线24、25、26分别是第4、5、6关节的角速度变化曲线。

具体实施方式

本发明提供了一种冗余空间机械臂在轨抓捕的无碰撞无奇异轨迹规划方法，如图1所示，方法所依托的设备包括空间操控平台1以及安装在空间操控平台上的机械臂***3；空间操控平台装有视觉测量敏感器和星载计算机；机械臂末端安装有末端作用器；机械臂抓捕的对象是空间非合作目标2。对于空间操作平台一般可指带有机械臂的在轨可控卫星，空间非合作目标一般指空间不可控卫星等。机械臂***3包括n部机械臂，每部机械臂包含p个关节4，从空间操控平台开始依次记为第1关节，第2关节......第p关节，第p关节上安装末端作用器5。本发明提供的即为机械臂***3抓捕目标的无碰撞无奇异的轨迹规划方法。

例如机械臂***3可以包括3个机械臂，每个机械臂具有7个自由度。

下面结合附图2及实例对本发明做详细说明。如图2所示将空间机械臂的整个抓捕操作分为粗略抓捕段和精细抓捕段。在粗略抓捕段将空间机械臂的轨迹规划问题简化为固定基座机械臂的轨迹规划问题，采用基于构型空间的轨迹搜索算法，将末端作用器移动到接近抓捕目标的位置和姿态的状态，与最终抓捕目标的位置和姿态的差值和粗略抓捕段由目标相对运动引起的误差一并由精细抓捕段加以修正。而精细抓捕段操作量很小，目标相对运动不明显，同时采用基于运动学的伪逆算法进行轨迹规划，在算法中对目标的相对运动进行实时补偿，最终实现准确抓捕。

一、粗略抓捕段的轨迹规划

具体包括如下步骤：

(1.1)由粗略抓捕段末端作用器的期望位置和姿态，利用无碰撞目标构型求解算法求得无碰撞的目标构型；

粗略抓捕段末端作用器的期望位置和姿态为粗略抓捕段结束时期望达到的位置和姿态，也就是抓捕目标所需的末端作用器的位置和姿态。

所述无碰撞目标构型求解算法首先根据目标构型求解算法计算目标构型；

目标构型求解算法的输入为机械臂当前的各个关节角，即当前机械臂构型，以及抓捕目标所需的末端作用器的位置和姿态，输出为抓捕目标所需的各个关节角，即目标构型。采用的方法为利用数值积分方法(如4阶龙格-库塔方法)求解机械臂逆运动学方程

$\stackrel{\·}{q}\left(t\right)=J^{+}\left(q\right){\stackrel{\·}{X}}_E\left(t\right).$

逆运动学方程积分的初始关节角为机械臂当前的各个关节角，积分的末端值即为算法的输出：抓捕目标所需的各个关节角，即目标构型。在方程中，X_E为机械臂末端作用器位置/姿态矢量；为由当前末端作用器的位置和姿态、抓捕目标所需的末端作用器的位置和姿态以及抓捕时间设计的机械臂末端作用器位置/姿态矢量X_E的运动规律，可以是关于时间的多项式函数或正弦函数；q为机械臂关节角列向量；为机械臂关节角速度列向量；J⁺(q)＝J^T(JJ^T)^-1为雅克比矩阵J的广义逆；上标“.”代表求导运算；上标“T”代表转置运算；上标“-1”代表求逆运算。

然后利用碰撞检测算法判断计算得到的目标构型是否与障碍物相碰，如果积分得到的目标构型与障碍物相碰，则可以对目标构型求解算法的输入：机械臂当前的关节角进行任意的调整(如改变第一个关节角的角度)，按照目标构型求解算法重新进行计算，获得满足抓捕目标所需的末端作用器位置和姿态的目标构型，直至得到无碰撞的目标构型。上述过程只是对输入至目标构型求解算法的机械臂当前的关节角进行调整，机械臂当前的关节角并不发生变化。

(1.2)根据机械臂当前构型和目标构型，利用无碰撞路径搜索算法，求得无碰撞的构型空间中的路径；

如图3所示，无碰撞路径搜索算法思路如下：算法输入为当前机械臂的构型(起始构型)，机械臂要到达的目标构型以及障碍物的参数，输出为不与障碍物相碰的机械臂的各个关节角的运动序列。令正整数N为路径分割常数，将每个关节角从当前构型到目标构型的总转角都平均分成N份，将每一个关节角转过每份转角定义为一个子运动，就形成了pN个子运动，其中p为关节数目。那么只要N足够大，将路径分割得足够细，就可以用这pN个子运动按合适的顺序拼接成一个无碰撞折线路径。

记第i个关节角的子运动为子运动i，i＝1，2，...，p，搜索算法的任务是通过不断试探确定这pN个子运动的顺序。即将总路径分成pN步，将pN个子运动以合适的顺序排列，形成一个pN步的运动序列。

设定序号生成函数F(i)，该函数由当前子运动序号i生成下一步要试探的子运动序号，F(i)可以根据需要选取，如递增、递减或随机生成。用m表示步号，T表示试探总次数，T_max表示可接受最大试探次数，算法如下：

1)初始化，m＝1，T＝1，选取i，设置T_max；

2)若m＝pN+1，算法成功结束，输出结果，否则利用碰撞检测算法，判断机械臂第m步执行子运动i是否与障碍物碰撞。如果不碰撞，机械臂执行子运动i(这是在计算机中的模拟)，并做记录，执行步骤3)；如果碰撞，执行步骤4)；

3)m＝m+1，i＝F(i)，T＝T+1，执行步骤6)；

4)判断第m步是否已经试探了所有可能的子运动，若是，执行步骤5)；否则i＝F()，T＝T+1，执行步骤6)；

5)跳回第m-1步，令m＝m-1。判断第m步是否已经试探了所有可能的子运动，若是重新执行步骤5)；否则令i＝F(i(m))，i(m)指第m步原来采用的子运动序号，T＝T+1，执行步骤6)；

6)若T＝T_max，算法失败结束，否则执行2。

若该算法搜索失败，则可以采用不同的试探规律F(i)或增大路径分割常数N重新搜索。试探规律F(i)可以有多种，若采用随机试探，每次试探的结果都会不同。试探规律的多样性，保证了搜索算法的有效性。增大路径分割常数N则可以增大搜索的成功率，但是这是以增加计算量为代价的。

(1.3)利用路径优化算法，对无碰撞的构型空间中的路径进行优化获得不与障碍物相碰的机械臂的各个关节角的运动序列。

算法输入为无碰撞路径搜索算法得到的不与障碍物相碰的机械臂的各个关节角的运动序列，输出为优化后的不与障碍物相碰的机械臂的各个关节角的运动序列。尝试将无碰撞路径搜索算法搜索到的无碰撞的机械臂的各个关节角的运动序列中相邻的两个子运动合并成一个子运动，将之前按时间先后顺序顺次进行的两个子运动同时进行，然后检测合并后的子运动是否与障碍物相碰。如果不相碰，则将这两个子运动合并；若相碰则取消合并。不断尝试，直至得到的运动序列中相邻的子运动无法进一步合并。

所述碰撞检测算法可以检测机械臂的一个运动过程或某一个构型是否与障碍物相碰。

碰撞检测算法方法如下：算法的输入为待检测的机械臂的某一个运动过程，即各个关节角随时间的运动规律。首先将机械臂的运动过程进行时间上的离散化，离散为处于一系列构型的机械臂；在各个构型下，将机械臂简化为一些连接的线段，然后进行空间上的离散化，离散化为机械臂上的一系列的点，将问题简化为检测点是否在障碍物内部；把障碍物分成几部分，每一部分找到一个凸多面体的包络，然后用这些凸多面体近似代替障碍物本身。记某个凸多面体的第i个面的中心点为P_i，第i个面的外法线矢量为记待检测的点为P_T，则点P_T位于这个凸多面体的充分必要条件是：对于凸多面体的所有的面都有若存在待检测点在障碍物内部，则认为机械臂与障碍物相碰，否则认为机械臂与障碍物不会发生碰撞。

二、精细抓捕段的轨迹规划

精细抓捕段的轨迹规划方法包括如下步骤：

(2.1)由期望的抓捕目标的末端作用器位置/姿态和当前末端作用器的位置/姿态预先设计出末端作用器位置/姿态的运动轨迹，速度轨迹和加速度轨迹；

在粗略抓捕段结束之后，利用空间操作平台上的视觉敏感器对目标上抓捕点的位置/姿态进行测量获得期望的抓捕目标的末端作用器的位置/姿态，并与当前末端作用器的位置/姿态做差，然后设计出一条连接期望值和当前值的末端作用器位置/姿态运动轨迹，速度轨迹和加速度轨迹。

预先设计出的末端作用器位置/姿态的速度轨迹和加速度轨迹在操作的起点和终点处为0。

(2.2)根据视觉测量敏感器测量得到的目标相对运动对所述末端作用器位置/姿态的运动轨迹，速度轨迹和加速度轨迹进行补偿获得补偿后的末端作用器位置/姿态的运动轨迹，速度轨迹和加速度轨迹；从而实现对目标的相对运动的实时补偿；

(2.3)根据补偿后的末端作用器位置/姿态的运动轨迹，速度轨迹和加速度轨迹采用基于运动学的伪逆算法获得机械臂的各个关节角的角速度轨迹和角加速度轨迹；从而能够使末端作用器的位置和姿态跟随预先设计的运动轨迹，速度轨迹和加速度轨迹的机械臂。

机械臂的各个关节角的角速度轨迹公式为：

${\stackrel{\·}{q}}_r\left(t\right)=J^{+}[{\stackrel{\·}{X}}_{\mathrm{Er}}\left(t\right)+\mathrm{\δ}\·(X_{\mathrm{Er}}\left(t\right)-X_E\left(t\right))]$

其中，X_Er(t)、和为补偿后的末端作用器位置/姿态的运动轨迹和速度轨迹，X_E(t)为机械臂实际运动过程中末端作用器的位置/姿态；δ·(X_Er(t)-X_E(t))为加入的比例调节项，可以对实际运动与预先设计的轨迹的跟随误差进行补偿，增加***的抗干扰能力和鲁棒性，其中δ＞0；J⁺＝J^T(JJ^T)^-1为雅克比矩阵J的伪逆；

上标“·”代表求导运算；上标“T”代表转置运算；上标“-1”代表求逆运算。

关节角加速度轨迹为：

${\stackrel{\·\·}{q}}_r\left(t\right)=J^{+}[{\stackrel{\·\·}{X}}_{\mathrm{Er}}\left(t\right)+\mathrm{\δ}\·({\stackrel{\·}{X}}_{\mathrm{Er}}\left(t\right)-{\stackrel{\·}{X}}_E\left(t\right))]+{\stackrel{\·}{J}}^{+}[{\stackrel{\·}{X}}_{\mathrm{Er}}\left(t\right)+\mathrm{\δ}\·(X_{\mathrm{Er}}\left(t\right)-X_E\left(t\right))]$

其中，为补偿后的末端作用器位置/姿态的加速度轨迹；为雅克比矩阵J的伪逆的时间导数。

(2.4)利用机械臂的冗余自由度，采用空转使机械臂远离奇异构型。

利用机械臂的冗余自由度，采用空转使机械臂远离奇异构型，最终的关节角速度为

${\stackrel{\·}{q}}_r={\stackrel{\·}{q}}_{\mathrm{rmp}}+P_{J}y,$

其中为步骤(203)中得到的逆运动学的基础解；P_J＝(I-J^T(JJ^T)^-1J)为雅可比矩阵J的正交投影矩阵；列向量y＝αv；ξ(q)为奇异度量：ξ(q)＝det(JJ^T)；标量参数α选取为：上标“·”代表求导运算；上标“T”代表求转置运算；上标“-1”代表求逆运算；代表标量ξ对矢量q求梯度。

实施例1

实施例1主要用于验证粗略抓捕段的算法的有效性。

(1)设计了一个7自由度机械臂。机械臂的几何参数如表1所示，其中第1节臂安装点在基座坐标系中表示。各个关节角为0时的初始构型以及各个转角的正方向见图4，图中黑色线段代表了机械臂，黑色圆圈代表连接各节机械臂的关节，关节旁的箭头表示各个关节转角的正方向。此时基座坐标系和各节臂的体坐标系重合，见图4中的坐标系oxyz。图4中，6为机械臂的基座安装点，7为机械臂的末端作用器，即第7节臂。下面以该机械臂为例验证粗略抓捕段的算法的有效性。

表1各节臂的安装点位置参数

(2)设机械臂的初始构型为q_S，各关节角为(-1.5707，3.1414，1，0，0，0，0)rad。以下参数在基座坐标系中描述：期望的粗略抓捕段结束时末端作用器端点位置为(2.0，0.3，0.5)m；期望的粗略抓捕段结束时末端作用器轴线方向余弦为(0.4699，-0.1987，0.8601)；障碍物为一个棱边与坐标轴平行的长方体，坐标范围为χ：1.2～1.7m，y：0～0.5m，z：0～0.5m。注意，目标位姿对末端作用器沿轴线的旋转没有约束，且第7节臂轴线与第7关节角转轴平行，所以第7关节角不会发生运动，实质上是一个6自由度机械臂满足5自由度的末端作用器位姿要求。

(3)首先由无碰撞的目标构型求解算法，求解出一个满足末端作用器位姿要求且不与障碍物相碰的目标构型qE，各关节角为(-0.0567，4.6127，1.4569，-2.1691，0.9561，-0.3024，0)rad。

(4)接着由无碰撞路径搜索算法搜索出一条不与障碍物相碰的路径，路径分隔常数N＝10，第7关节角转角没有变化，实质上是6维空间的搜索问题，共分成6N＝60个子运动，函数F(i)采用递增试探。经过64次试探，找到一个无碰撞的运动序列：1，2，3，4，5，6，1，2，3，4，5，6，1，2，3，4，5，6，1，2，3，4，5，6，1，2，3，4，5，6，1，2，3，4，5，6，1，2，3，4，5，6，2，3，4，5，6，2，3，4，5，6，2，3，4，5，6，1，1，1。可以看到序列的前面部分与试探顺序相同，没有进行调整，在序列的后面算法对试探顺序进行调整以避开障碍。这里得到的无碰撞路径并不是最终结果，但为了说明路径优化算法的必要性，我们在这里给出机械臂在未优化的无碰撞运动序列上的运动过程和关节角的运动曲线。得到机械臂的关节角运动序列之后，还需要给出机械臂完成序列中每一步所需的关节角的运动规律。设定每一段子运动运动时间为4s，关节角运动规律采用关于时间的多项式，保证每个子运动开始和结束时，关节角速度和角加速度为0。机械臂的运动过程见图5，图中每隔一段时间记录一次机械臂的位置，图中8为机械臂初始构型，9为粗抓捕段结束时的目标构型，6为机械臂的基座安装点，10为障碍物。如图5所示，机械臂从底部绕过障碍物到达了目标构型。图6给出了各个关节角的角速度曲线。从图6可以看出，关节角速度从总体来看是不够平滑的。因此对该路径进行优化是必要的。

(5)下面采用路径平滑算法对上面得到的折线路径进行平滑。结果发现原有的60个子运动组成的序列只需分割成2步即可避开障碍物，第1步由初始构型运动至中间构型，第2步由中间构型至目标构型，各构型见表2所示。得到每一步的起始构型和结束构型之后，还需要给出机械臂完成每一步所需的关节角的运动规律。设定每一步运动时间为20s，关节角运动规律采用关于时间的多项式，保证每一步开始和结束时，关节角速度和角加速度为0。机械臂的运动过程见图7所示，图中每隔一段时间记录一次机械臂的位置，图中8为机械臂初始构型，9为粗抓捕段结束时的目标构型，6为机械臂的基座安装点，10为障碍物，11为机械臂的中间构型。机械臂第一步由初始构型8运动至中间构型11，第二步由中间构型11运动至目标构型9。由图7可见，机械臂从底部绕过障碍物到达了目标构型，机械臂在这两步运动过程中，都没有与障碍物相碰。图8给出了各个关节角的角速度曲线。从图8可以看出，与图6相比，机械臂各个关节角的运动规律非常平滑，易于实现，并且操作时间大大减小，便于对非合作目标的快速抓捕。

表2平滑后路径上的三个关键构型(rad)

实施例2

实施例2给出了冗余空间机械臂整个抓捕过程的控制方法。

(1)将实施例1中的机械臂安装在一个空间操作平台上，对一个非合作目标进行抓捕。首先由抓捕任务要求和敏感器测量，确定非合作目标上的抓捕点，计算出抓捕目标时所需要的末端作用器的位置和姿态。

(2)将整个抓捕过程划分为粗略抓捕段和精细抓捕段。粗略抓捕段的起始状态即为机械臂的当前构型，将粗略抓捕段的结束状态设定为机械臂的末端作用器位于抓捕点上方一定距离处(比如20cm)。

(3)由上一步所设定的粗略抓捕段结束时末端作用器的位置和姿态，由无碰撞目标构型求解算法，求出无碰撞的目标构型。由机械臂的当前构型和目标构型，由无碰撞路径搜索算法和路径优化算法求得平滑的无碰撞的关节角运动序列。根据抓捕任务的时间要求，设定无碰撞运动序列上每一步的运动时间(比如每一步耗时20s)。然后根据设定的运动时间，设计关节角运动规律为时间的多项式函数，保证在每一步开始和结束时，关节角速度和角加速度为0。随后，空间操作平台的控制***，采用一定的控制方法(比如PID控制器)，驱动机械臂关节处的电机，跟随给定的关节角运动规律，从而实现粗略抓捕段的目标，将机械臂的末端作用器无碰撞地移动至抓捕点上方一定距离处。

(4)粗略抓捕段结束之后，由空间操作平台上的敏感器，测量得到此时的抓捕点的位置，设计出末端作用器位置和姿态的运动轨迹，速度轨迹和加速度轨迹，采用基于运动学的伪逆算法，实时解算出机械臂关节角的位置，速度和加速度信息，由空间操作台的控制***，采用一定的控制方法(比如PID控制器)，驱动机械臂关节处的电机，跟随实时解算出的关节角运动规律。同时，由空间操作平台上的敏感器，测量目标上的抓捕点相对于空间操作平台的运动，在伪逆算法中对目标的相对运动加以补偿，从而实现最终的精确抓捕，即机械臂将末端作用器精确移动到目标上的抓捕点处。

本具体实施方式部分针对7自由度的冗余机械臂目标抓捕实现了无碰撞无奇异的轨迹规划，满足工程要求，易于工程应用，具有良好的推广前景。

本发明说明书中未作详细描述的内容属本领域技术人员的公知技术。

Claims (5)

1.一种冗余空间机械臂在轨抓捕的轨迹规划方法；所述冗余空间机械臂安装在空间操控平台上，机械臂末端安装末端作用器；空间操控平台装有视觉测量敏感器和星载计算机；冗余空间机械臂在轨抓捕的对象是空间非合作目标；其特征在于，将空间机械臂的抓捕操作分为粗略抓捕段和精细抓捕段，

粗略抓捕段的轨迹规划方法包括如下步骤：

(1.1)由粗略抓捕段末端作用器的期望位置和姿态，利用无碰撞目标构型求解算法求得无碰撞的目标构型；

无碰撞目标构型求解算法如下：

首先根据目标构型求解算法计算目标构型；目标构型求解算法的输入为机械臂当前的各个关节角，以及抓捕目标所需的末端作用器的位置和姿态，目标构型求解算法利用数值积分方法求解机械臂逆运动学方程得到抓捕目标所需的各个关节角，即目标构型；机械臂逆运动学方程为逆运动学方程积分的初始关节角为机械臂当前的各个关节角，积分的末端值为抓捕目标所需的各个关节角；X_E为机械臂末端作用器位置/姿态矢量；为由当前末端作用器的位置和姿态、抓捕目标所需的末端作用器的位置和姿态以及抓捕时间设计的机械臂末端作用器位置/姿态矢量X_E的运动规律，是关于时间的多项式函数；q为机械臂关节角列向量；为机械臂关节角速度列向量；J⁺(q)为雅克比矩阵J的广义逆；

然后利用碰撞检测算法判断计算得到的目标构型是否与障碍物相碰，如果与障碍物相碰，则对输入至目标构型求解算法的机械臂当前的关节角进行调整，按照目标构型求解算法重新进行计算，直至得到无碰撞的目标构型；

(1.2)根据机械臂当前构型和目标构型，利用无碰撞路径搜索算法，求得无碰撞的构型空间中的路径；

(1.3)利用路径优化算法，对无碰撞的构型空间中的路径进行优化获得不与障碍物相碰的机械臂的各个关节角的运动序列；

精细抓捕段的轨迹规划方法包括如下步骤：

(2.1)利用空间操作平台上的视觉敏感器对目标上抓捕点的位置/姿态进行测量获得期望的抓捕目标的末端作用器的位置/姿态，并与当前末端作用器的位置/姿态做差，然后设计出一条连接期望值和当前值的末端作用器位置/姿态运动轨迹，速度轨迹和加速度轨迹，设计的速度轨迹和加速度轨迹在操作的起点和终点处为0；

(2.2)根据视觉测量敏感器测量得到的目标相对运动对所述末端作用器位置/姿态的运动轨迹，速度轨迹和加速度轨迹进行补偿获得补偿后的末端作用器位置/姿态的运动轨迹，速度轨迹和加速度轨迹；

(2.3)根据补偿后的末端作用器位置/姿态的运动轨迹，速度轨迹和加速度轨迹采用基于运动学的伪逆算法获得机械臂的各个关节角的角速度轨迹和角加速度轨迹；

(2.4)利用机械臂的冗余自由度，采用空转使机械臂远离奇异构型；

带空转的机械臂的各个关节角的角速度轨迹为

${{\stackrel{.}{q}}_r}^{\′}={\stackrel{.}{q}}_r\left(t\right)+P_{J}y,$

P_J＝(I-J^T(JJ^T)^-1J)为雅可比矩阵J的正交投影矩阵；列向量y＝αv；ξ(q)为奇异度量：ξ(q)＝det(JJ^T)；标量参数α选取为：上标“·”代表求导运算；上标“T”代表求转置运算；上标“-1”代表求逆运算；代表标量ξ对矢量q求梯度。

2.根据权利要求1所述的一种冗余空间机械臂在轨抓捕的轨迹规划方法，其特征在于：所述步骤(1.2)中无碰撞路径搜索算法将每个关节角从当前构型到目标构型的总转角都平均分成N份，将每一个关节角转过每份转角定义为一个子运动，从而形成了pN个子运动，其中p为关节数目；

记第i个关节角的子运动为子运动i，i＝1，2，...，p，无碰撞路径搜索算法将pN个子运动以合适的顺序排列，形成一个pN步的运动序列；

设定序号生成函数F(i)，该函数由当前子运动序号i生成下一步要试探的子运动序号；F(i)为递增、递减或随机生成函数；

用m表示步号，T表示试探总次数，T_max表示可接受最大试探次数；

无碰撞路径搜索算法步骤如下：

1)初始化，令m＝1，T＝1，选取i，设置T_max；

2)判断m是否等于pN+1；

若m＝pN+1，则输出无碰撞的关节运动序列，并结束搜索算法；否则利用碰撞检测算法，判断机械臂第m步执行子运动i是否与障碍物碰撞；如果不碰撞，机械臂执行子运动i，并做记录，执行步骤3)；如果碰撞，执行步骤4)；

3)m＝m+1，i＝F(i)，T＝T+1，执行步骤6)；

4)判断第m步是否已经试探了所有可能的子运动i，若是，执行步骤5)；否则i＝F(i)，T＝T+1，执行步骤6)；

5)跳回第m-1步，令m＝m-1；判断第m步是否已经试探了所有可能的子运动，若是重新执行步骤5)；否则令i＝F(i(m))，i(m)指第m步原来采用的子运动序号，T＝T+1，执行步骤6)；

6)判断T是否等于T_max，

若T＝T_max，则输出算法搜索失败并结束搜索算法，否则执行步骤2)；

若算法搜索失败，则采用不同的试探规律F(i)或增大路径分割常数N重新执行无碰撞路径搜索算法。

3.根据权利要求1所述的一种冗余空间机械臂在轨抓捕的轨迹规划方法，其特征在于：所述步骤(1.3)中路径优化算法方法如下：将无碰撞路径搜索算法搜索到的无碰撞的机械臂的各个关节角的运动序列中相邻的两个子运动合并成一个子运动，然后采用碰撞检测算法检测合并后的子运动是否与障碍物相碰；如果不相碰，则将这两个运动合并；若相碰则取消合并；重复上述过程，直至得到的运动序列中相邻的子运动无法进一步合并。

4.根据权利要求2至3任一权利要求所述的一种冗余空间机械臂在轨抓捕的轨迹规划方法，其特征在于：所述碰撞检测算法方法如下：将机械臂简化为一些连接的线段，然后进行空间上的离散化，离散化为机械臂上的一系列的点，把障碍物分成几部分，每一部分找到一个凸多面体的包络，然后用这些凸多面体近似代替障碍物本身；记某个凸多面体的第i个面的中心点为P_i，第i个面的外法线矢量为记待检测的点为P_T，则点P_T位于这个凸多面体的充分必要条件是：对于凸多面体的所有的面都有若存在待检测点在障碍物内部，则认为机械臂与障碍物相碰，否则认为机械臂与障碍物不会发生碰撞。

5.根据权利要求1所述的一种冗余空间机械臂在轨抓捕的轨迹规划方法，其特征在于：所述步骤(2.3)中的机械臂的各个关节角的角速度轨迹公式为：

${\stackrel{.}{q}}_r\left(t\right)=J^{+}[{\stackrel{.}{X}}_{\mathrm{Er}}\left(t\right)+\mathrm{\δ}\·(X_{\mathrm{Er}}\left(t\right)-X_E\left(t\right))]$

其中，X_Er(t)、和为补偿后的末端作用器位置/姿态的运动轨迹和速度轨迹，X_E(t)为机械臂实际运动过程中末端作用器的位置/姿态；δ·(X_Er(t)-X_E(t))为加入的比例调节项，其中δ＞0；J⁺＝J^T(JJ^T)^-1为雅克比矩阵J的伪逆；

所述机械臂的各个关节角的关节角加速度轨迹为：

${\stackrel{..}{q}}_r\left(t\right)=J^{+}[{\stackrel{..}{X}}_{\mathrm{Er}}\left(t\right)+\mathrm{\δ}\·({\stackrel{.}{X}}_{\mathrm{Er}}\left(t\right)-{\stackrel{.}{X}}_E\left(t\right))]+{\stackrel{.}{J}}^{+}[{\stackrel{.}{X}}_{\mathrm{Er}}\left(t\right)+\mathrm{\δ}\·(X_{\mathrm{Er}}\left(t\right)-X_E\left(t\right))]$

其中，为补偿后的末端作用器位置/姿态的加速度轨迹；为雅克比矩阵J的伪逆的时间导数。