专利名称：一种冗余空间机械臂在轨抓捕的轨迹规划方法
技术领域：
本发明涉及一种冗余空间机械臂在轨抓捕的轨迹规划方法，属于航天器姿态控制领域。
背景技术：
空间机械臂在轨抓捕操作中，为了节省燃料，不对空间操控平台的位置加以控制，只对空间操控平台的姿态进行稳定控制，属于姿态受控的自由飞行机器人，因此在系统模型的变量中可以加以任意控制的只有机械臂的各个关节角。轨迹规划的基本要求即是以期望的末端作用器的位置和姿态指向为目标，来通过一定的算法确定出各个关节角的角位置、角速度和角加速度轨迹，从而由控制器实现机械臂的抓捕操作。这里冗余是指，机械臂具有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表示试探总次数，Tfflax表示可接受最大试探次数；无碰撞路径搜索算法步骤如下I)初始化,令m = LT = I,选取i,设置Tmax ；2)判断m是否等于pN+1 ；若m = ρΝ+l，则输出无碰撞的关节运动序列，并结束搜索算法；否则利用碰撞检测算法，判断机械臂第m步执行子运动i是否与障碍物碰撞；如果不碰撞，机械臂执行予运动i，并做记录，执行步骤3);如果碰撞，执行步骤4)；3) m = m+1, i = F ⑴，T = T+1，执行步骤 6)；4)判断第m步是否已经试探了所有可能的子运动i，若是，执行步骤5);否则i =F(i)，T = T+I，执行步骤 6)；
5)跳回第m-Ι步，令m = m_l ;判断第m步是否已经试探了所有可能的子运动，若是重新执行步骤5);否则令
权利要求
1.一种冗余空间机械臂在轨抓捕的轨迹规划方法；所述冗余空间机械臂安装在空间操控平台上，机械臂末端安装末端作用器；空间操控平台装有视觉测量敏感器和星载计算机；冗余空间机械臂在轨抓捕的对象是空间非合作目标；其特征在于，将空间机械臂的抓捕操作分为粗略抓捕段和精细抓捕段， 粗略抓捕段的轨迹规划方法包括如下步骤 (1.1)由粗略抓捕段末端作用器的期望位置和姿态，利用无碰撞目标构型求解算法求得无碰撞的目标构型； (1. 2)根据机械臂当前构型和目标构型，利用无碰撞路径搜索算法，求得无碰撞的构型空间中的路径； (1. 3)利用路径优化算法，对无碰撞的构型空间中的路径进行优化获得不与障碍物相碰的机械臂的各个关节角的运动序列； 精细抓捕段的轨迹规划方法包括如下步骤 (2.1)由期望的抓捕目标的末端作用器位置/姿态和当前末端作用器的位置/姿态预先设计出末端作用器位置/姿态的运动轨迹，速度轨迹和加速度轨迹； (2. 2)根据视觉测量敏感器测量得到的目标相对运动对所述末端作用器位置/姿态的运动轨迹，速度轨迹和加速度轨迹进行补偿获得补偿后的末端作用器位置/姿态的运动轨迹，速度轨迹和加速度轨迹； (2. 3)根据补偿后的末端作用器位置/姿态的运动轨迹，速度轨迹和加速度轨迹采用基于运动学的伪逆算法获得机械臂的各个关节角的角速度轨迹和角加速度轨迹。
2.根据权利要求1所述的一种冗余空间机械臂在轨抓捕的轨迹规划方法，其特征在于所述步骤(1. D中，无碰撞目标构型求解算法如下 首先根据目标构型求解算法计算目标构型； 目标构型求解算法的输入为机械臂当前的各个关节角，以及抓捕目标所需的末端作用器的位置和姿态，目标构型求解算法利用数值积分方法求解机械臂逆运动学方程得到抓捕目标所需的各个关节角，即目标构型； 然后利用碰撞检测算法判断计算得到的目标构型是否与障碍物相碰，如果与障碍物相碰，则对输入至目标构型求解算法的机械臂当前的关节角进行调整，按照目标构型求解算法重新进行计算，直至得到无碰撞的目标构型。
3.根据权利要求1所述的一种冗余空间机械臂在轨抓捕的轨迹规划方法，其特征在于所述步骤(1.2)中无碰撞路径搜索算法将每个关节角从当前构型到目标构型的总转角都平均分成N份，将每一个关节角转过每份转角定义为一个子运动，从而形成了 pN个子运动，其中P为关节数目； 记第i个关节角的子运动为子运动i，i = 1，2，...，p，无碰撞路径搜索算法将pN个子运动以合适的顺序排列，形成一个PN步的运动序列； 设定序号生成函数F(i)，该函数由当前子运动序号i生成下一步要试探的子运动序号；F(i)为递增、递减或随机生成函数； 用m表示步号，T表示试探总次数，Tfflax表示可接受最大试探次数； 无碰撞路径搜索算法步骤如下 I)初始化，令m = LT= I,选取i,设置Tmax ；2)判断m是否等于pN+1； 若m = ρΝ+l，则输出无碰撞的关节运动序列，并结束搜索算法；否则利用碰撞检测算法，判断机械臂第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-Ι步，令m= m-l ;判断第m步是否已经试探了所有可能的子运动，若是重新执行步骤5);否则令i = F(i (m))，i (m)指第m步原来采用的子运动序号，T = T+1，执行步骤6)； 6)判断T是否等于！^， 若T = Tmax，则输出算法搜索失败并结束搜索算法，否则执行步骤2)； 若算法搜索失败，则采用不同的试探规律F (i)或增大路径分割常数N重新执行无碰撞路径搜索算法。
4.根据权利要求1所述的一种冗余空间机械臂在轨抓捕的轨迹规划方法，其特征在于所述步骤(1. 3)中路径优化算法方法如下将无碰撞路径搜索算法搜索到的无碰撞的机械臂的各个关节角的运动序列中相邻的两个子运动合并成一个子运动，然后采用碰撞检测算法检测合并后的子运动是否与障碍物相碰；如果不相碰，则将这两个运动合并；若相碰则取消合并；重复上述过程，直至得到的运动序列中相邻的子运动无法进一步合并。
5.根据权利要求2至4任一权利要求所述的一种冗余空间机械臂在轨抓捕的轨迹规划方法，其特征在于所述碰撞检测算法方法如下将机械臂简化为一些连接的线段，然后进行空间上的离散化，离散化为机械臂上的一系列的点，把障碍物分成几部分，每一部分找到一个凸多面体的包络，然后用这些凸多面体近似代替障碍物本身；记某个凸多面体的第i个面的中心点SPi，第i个面的外法线矢量为A，记待检测的点为Pt，则点Pt位于这个凸多面体的充分必要条件是对于凸多面体的所有的面都有>O;若存在待检测点在障碍物内部，则认为机械臂与障碍物相碰，否则认为机械臂与障碍物不会发生碰撞。
6.根据权利要求1所述的一种冗余空间机械臂在轨抓捕的轨迹规划方法，其特征在于所述步骤(2.3)中的机械臂的各个关节角的角速度轨迹公式为
7.根据权利要求1所述的一种冗余空间机械臂在轨抓捕的轨迹规划方法，其特征在于所述精细抓捕段的轨迹规划方法还包括如下步骤利用机械臂的冗余自由度，采用空转使机械臂远离奇异构型。
8.根据权利要求7所述的一种冗余空间机械臂在轨抓捕的轨迹规划方法，其特征在于 带空转的机械臂的各个关节角的角速度轨迹为
全文摘要
一种冗余空间机械臂在轨抓捕的轨迹规划方法，通过将空间机械臂的抓捕操作分为粗略抓捕段和精细抓捕段，将问题进行了简化。针对不同阶段的突出问题，采用了不同的规划算法。在粗略抓捕段采用基于构型空间的轨迹搜索算法，该算法基于构型空间中的试探；在精细抓捕段采用基于运动学的伪逆算法，通过对目标相对运动的补偿保证最终的精确抓捕。
文档编号B25J9/16GK103009389SQ201210520049
公开日2013年4月3日 申请日期2012年11月30日 优先权日2012年11月30日
发明者汤亮, 何英姿, 王悦, 王大轶, 魏春岭 申请人:北京控制工程研究所