一种空间非合作翻滚目标的大容差捕获方法及系统

摘要

本发明公开了一种空间非合作翻滚目标的大容差捕获方法，包括以下步骤：空间机器人系统获取空间翻滚目标局部位置的视觉信息，并选取可行抓捕区域；所述系统判断各机械臂的末端位姿是否满足抓捕条件；若不满足抓捕条件，所述系统测量所述目标的位姿并预测目标的运动，控制参考机械臂的运动以及组合机械臂的协同运动，进而控制所述机械臂对期望抓捕区域进行位姿跟踪与抓捕；若同时满足参考机械臂的位姿跟踪条件以及组合机械臂的协同约束条件时，则系统实现对翻滚目标的抓捕锁紧。本发明还公开一种空间非合作翻滚目标的大容差捕获系统，其可实现对非合作翻滚目标的安全、快速捕获，具有潜在的巨大经济效益，广泛应用于空间机器人技术领域。

权利要求

1.一种空间非合作翻滚目标的大容差捕获方法，其特征在于，包括以下步骤：双臂空间机器人系统获取空间翻滚目标局部位置的视觉信息，并选取可行抓捕区域；

所述系统判断参考机械臂的末端位姿是否满足抓捕条件；

若不满足抓捕条件，所述系统测量所述目标的位姿并预测目标的运动，控制参考机械臂的运动以及组合机械臂的协同运动，进而控制机械臂对期望抓捕区域进行位姿跟踪与抓捕；

若同时满足参考机械臂的位姿跟踪条件以及组合机械臂的协同约束条件时，则系统实现对翻滚目标的抓捕锁紧；

所述方法还包括通过机械臂末端工具与抓捕区域形成面接触，从而实现对目标卫星的抓捕；

满足抓捕锁紧的条件为：所述参考机械臂末端工具位于期望抓捕区域，且所述末端工具的法向量与期望抓捕面垂直。

2.根据权利要求1所述的空间非合作翻滚目标的大容差捕获方法，其特征在于：所述抓捕条件还包括：所述各机械臂的末端中心点与抓捕点的位置容差Ep满足以下边界约束条件，Ep：{0≤xe-xc≤a；0≤ye-yc≤a；ze-zc＝0}；

其中{xe,ye,ze}和{xc,yc,zc}分别代表各个抓捕面上任意点以及抓捕面几何中心点的位置；a为正方形抓捕区域的边长。

3.根据权利要求2所述的空间非合作翻滚目标的大容差捕获方法，其特征在于，所述抓捕条件还包括：所述各机械臂末端工具的姿态与抓捕点的姿态容差Er满足如下约束：Er：{(α 0 0)；α∈[-π,π]}；

其中α表示采用Z-Y-X欧拉角表示的末端抓捕工具与抓捕点在Z轴方向上的姿态偏差。

4.根据权利要求3所述的空间非合作翻滚目标的大容差捕获方法，其特征在于：当不满足抓捕条件时，所述系统通过计算得到表示机械臂末端法向量与抓捕面法向量指向偏差的转轴和转角，进而得到机械臂末端的期望角速度及速度。

5.根据权利要求4所述的空间非合作翻滚目标的大容差捕获方法，其特征在于：所述系统通过相对雅克比矩阵控制两机械臂的各关节协同运动。

6.一种空间非合作翻滚目标的大容差捕获系统，其特征在于，用于执行如权利要求1所述的一种空间非合作翻滚目标的大容差捕获方法，包括：第一模块，用于执行步骤双臂空间机器人系统获取空间翻滚目标局部位置的视觉信息，并选取可行抓捕区域；

第二模块，用于执行步骤所述系统判断参考机械臂的末端位姿是否满足抓捕条件；

第三模块，用于执行步骤若不满足抓捕条件，所述系统测量所述目标的位姿并预测目标的运动，控制参考机械臂的运动以及组合机械臂的协同运动，进而控制机械臂对期望抓捕区域进行位姿跟踪与抓捕；

第四模块，用于执行步骤若同时满足参考机械臂的位姿跟踪条件以及组合机械臂的协同约束条件时，则系统实现对翻滚目标的抓捕锁紧。

说明书

一种空间非合作翻滚目标的大容差捕获方法及系统

技术领域

[0001] 本发明涉及空间机器人技术领域，具体为一种空间非合作翻滚目标的双臂大容差捕获方法及系统。

背景技术

[0002] 伴随着人类对空间资源的不断探索与利用，由故障失效卫星、反卫星试验、各种爆炸、飞船的腐蚀以及碎片等各种原因造成的大量空间垃圾，也使得空间资源逐步呈现出短缺的现状，同时对空间安全造成严重威胁。针对失效航天器或轨道垃圾清理的问题，各主要航天机构已开展了一些项目的研究，空间机器人在轨操控技术是当前解决空间垃圾最为行之有效的方法。总结目前空间机器人的抓捕方式主要分为通过机械臂精确抓捕的刚性连接捕获和通过飞网、飞爪等捕获工具的柔性连接捕获。其中精确抓捕适用于合作目标或相对运动较小的目标抓捕，而飞网、飞爪的抓捕适用范围较广，但是抓捕后很难开展对目标的维修、检测以及再利用。

[0003] 为了便于开展捕获后维修以及再利用等操作，很多学者考虑接触式机械臂刚性连接捕获技术。研究内容主要包括：捕获前的规划与控制过程中机械臂与平台的耦合、抓捕过程中机械臂与非合作目标的接触碰撞以及捕获后组合体的消旋与稳定控制等。目标的翻滚运动特性，对于捕获过程的控制策略、控制方法，均有很大的影响。在机械臂捕获理论和方法的研究方面，大部分的研究多是针对合作目标，或简单自旋目标的。对于同时具有自旋、章动的非合作航天器的捕获，尚未形成系统的研究，相关的理论和方法尚不完善。因此该技术有必要进行改进。

发明内容

[0004] 为了解决上述技术问题，本发明的目的是提供一种空间非合作翻滚目标的大容差捕获方法及系统。

[0005] 本发明所采用的技术方案是：

[0006] 本发明提供一种空间非合作翻滚目标的大容差捕获方法，包括以下步骤：

[0007] 双臂空间机器人系统获取空间翻滚目标局部位置的视觉信息，并选取可行抓捕区域；

[0008] 所述系统判断参考机械臂的末端位姿是否满足抓捕条件；

[0009] 若不满足抓捕条件，所述系统测量所述目标的位姿并预测目标的运动，控制参考机械臂的运动以及组合机械臂的协同运动，进而控制机械臂对期望抓捕区域进行位姿跟踪与抓捕；

[0010] 若同时满足参考机械臂的位姿跟踪条件以及组合机械臂的协同约束条件时，则系统实现对翻滚目标的抓捕锁紧。

[0011] 作为该技术方案的改进，所述方法还包括通过机械臂末端工具与抓捕区域形成面接触，从而实现对目标卫星的抓捕。

[0012] 作为该技术方案的改进，满足抓捕锁紧的条件为：所述参考机械臂末端工具位于期望抓捕区域，且所述末端工具的法向量与期望抓捕面垂直。

[0013] 作为该技术方案的改进，所述抓捕条件还包括：所述各机械臂的末端中心点与抓捕点的位置容差Ep满足以下边界约束条件，

[0014] Ep：{0≤xe-xc≤a；0≤ye-yc≤a；ze-zc＝0}；

[0015] 其中{x,y,z}和{xc,yc,zc}分别代表各个抓捕面上任意点以及抓捕面几何中心点的位置；a为正方形抓捕区域的边长。

[0016] 进一步地，所述抓捕条件还包括：所述各机械臂末端工具的姿态与抓捕点的姿态容差Er满足如下约束：

[0017] Er：{(α 0 0)；α∈[-π,π]}；

[0018] 其中α表示采用Z-Y-X欧拉角表示的末端抓捕工具与抓捕点在Z轴方向上的姿态偏差。

[0019] 进一步地，当不满足抓捕条件时，所述系统通过计算得到表示机械臂末端法向量与抓捕面法向量指向偏差的转轴和转角，进而得到机械臂末端的期望角速度及速度。

[0020] 进一步地，所述系统通过相对雅克比矩阵控制两机械臂的各关节协同运动。

[0021] 另一方面，本发明还提供一种空间非合作翻滚目标的大容差捕获系统，包括：

[0022] 第一模块，用于执行步骤双臂空间机器人系统获取空间翻滚目标局部位置的视觉信息，并选取可行抓捕区域；

[0023] 第二模块，用于执行步骤所述系统判断参考机械臂的末端位姿是否满足抓捕条件；

[0024] 第三模块，用于执行步骤若不满足抓捕条件，所述系统测量所述目标的位姿并预测目标的运动，控制参考机械臂的运动以及组合机械臂的协同运动，进而控制机械臂对期望抓捕区域进行位姿跟踪与抓捕；

[0025] 第四模块，用于执行步骤若同时满足参考机械臂的位姿跟踪条件以及组合机械臂的协同约束条件时，则系统实现对翻滚目标的抓捕锁紧。

[0026] 本发明的有益效果是：本发明提供的一种空间非合作翻滚目标的大容差捕获方法及系统，针对空间非合作翻滚目标卫星，提出了基于抓捕区域的大容差双臂协同捕获方法，通过控制机械臂末端运动到目标抓捕面的一个范围内(不是固定点)，即可控制机械臂对目标卫星进行抓捕锁紧，增强了抓捕算法的适应性和快速性。

[0027] 该协调捕获方法具有如下两大优势：其一是采用区域抓捕的概念，抓捕的约束条件更少，抓捕容差更大；其二是只需一个目标点的视觉测量信息便可以实现双臂之间的协同抓捕控制，算法效率以及实时性更高。本方案可以应用于空间在轨维修维护以及轨道垃圾清理等任务，实现对非合作翻滚目标的安全、快速捕获，具有潜在的巨大经济效益。

附图说明

[0028] 下面结合附图对本发明的具体实施方式作进一步说明：

[0029] 图1是本发明第一实施例的基于双臂的区域抓捕策略示意图；

[0030] 图2是本发明第二实施例的翻滚目标受力示意图；

[0031] 图3是本发明第三实施例的翻滚目标抓捕区域示意图；

[0032] 图4是本发明第四实施例的区域环抱抓捕示意图；

[0033] 图5是本发明第五实施例的组合机械臂协同操作的相对关系示意图；

[0034] 图6是本发明第六实施例的双臂大容差协调捕获控制流程示意图。

具体实施方式

[0035] 需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。

[0036] 本发明提供一种空间非合作翻滚目标的大容差捕获方法，包括以下步骤：

[0037] 双臂空间机器人系统获取空间翻滚目标局部位置的视觉信息，并选取可行抓捕区域；

[0038] 所述系统判断参考机械臂的末端位姿是否满足抓捕条件；

[0039] 若不满足抓捕条件，所述系统测量所述目标的位姿并预测目标的运动，控制参考机械臂的运动以及组合机械臂的协同运动，进而控制机械臂对期望抓捕区域进行位姿跟踪与抓捕；

[0040] 若同时满足参考机械臂的位姿跟踪条件以及组合机械臂的协同约束条件时，则系统实现对翻滚目标的抓捕锁紧。

[0041] 作为该技术方案的改进，所述方法还包括通过机械臂末端工具与抓捕区域形成面接触，从而实现对目标卫星的抓捕。

[0042] 作为该技术方案的改进，满足抓捕锁紧的条件为：所述参考机械臂末端工具位于期望抓捕区域，且所述末端工具的法向量与期望抓捕面垂直。

[0043] 作为该技术方案的改进，所述抓捕条件还包括：所述各机械臂的末端中心点与抓捕点的位置容差Ep满足以下边界约束条件，

[0044] Ep：{0≤xe-xc≤a；0≤ye-yc≤a；ze-zc＝0}；

[0045] 其中{x,y,z}和{xc,yc,zc}分别代表各个抓捕面上任意点以及抓捕面几何中心点的位置；a为正方形抓捕区域的边长。

[0046] 进一步地，所述抓捕条件还包括：所述各机械臂末端工具的姿态与抓捕点的姿态容差Er满足如下约束：

[0047] Er：{(α 0 0)；α∈[-π,π]}；

[0048] 其中α表示采用Z-Y-X欧拉角表示的末端抓捕工具与抓捕点在Z轴方向上的姿态偏差。

[0049] 进一步地，当不满足抓捕条件时，所述系统通过计算得到表示机械臂末端法向量与抓捕面法向量指向偏差的转轴和转角，进而得到机械臂末端的期望角速度及速度。

[0050] 进一步地，所述系统通过相对雅克比矩阵控制两机械臂的各关节协同运动。

[0051] 另一方面，本发明还提供一种空间非合作翻滚目标的大容差捕获系统，包括：

[0052] 第一模块，用于执行步骤双臂空间机器人系统获取空间翻滚目标局部位置的视觉信息，并选取可行抓捕区域；

[0053] 第二模块，用于执行步骤所述系统判断参考机械臂的末端位姿是否满足抓捕条件；

[0054] 第三模块，用于执行步骤若不满足抓捕条件，所述系统测量所述目标的位姿并预测目标的运动，控制参考机械臂的运动以及组合机械臂的协同运动，进而控制机械臂对期望抓捕区域进行位姿跟踪与抓捕；

[0055] 第四模块，用于执行步骤若同时满足参考机械臂的位姿跟踪条件以及组合机械臂的协同约束条件时，则系统实现对翻滚目标的抓捕锁紧。

[0056] 其具体通过如下步骤实现：

[0057] 步骤1：空间机器人系统沿着翻滚目标动量轴的方向逼近至最佳抓捕点(机械臂的灵巧工作空间)，此时平台起旋对翻滚目标沿最大惯量轴的自旋运动进行跟随，进而控制空间机器人对残余相对运动的翻滚目标进行抓捕；

[0058] 步骤2：通过空间机器人平台的全局视觉测量系统以及机械臂的手眼视觉相机获取空间翻滚目标局部位置的视觉信息，对可行抓捕区域进行定义以及选取；

[0059] 步骤3：判断参考机械臂的末端位姿是否满足抓捕条件，亦即末端位置是否满足期望抓捕区域以及末端姿态是否垂直于期望抓捕面；

[0060] 步骤4：若不满足抓捕条件，通过对目标位姿的测量以及目标运动的预测，控制参考机械臂对期望抓捕区域进行位姿跟踪接近；

[0061] 步骤5：规划组合机械臂的协同运动，通过相对雅克比矩阵实现组合机械臂对翻滚目标的协同跟踪接近；

[0062] 步骤6：若同时满足参考机械臂的位姿跟踪条件以及组合机械臂的协同约束条件时，控制空间机器人实现对翻滚目标的抓捕锁紧。

[0063] 其中步骤2中与以往固定抓捕点的捕获锁紧方式不同，定义可行抓捕区域的概念。区域环抱抓捕实现自旋失效卫星的抓捕稳定，关键在于保持机械臂末端工具与抓捕面之间的面面接触，通过保持一定的正压力与摩擦力使失效卫星达到消旋、稳定状态。

[0064] 步骤3中基于区域抓捕方法，满足抓捕锁紧的条件为：参考机械臂末端工具位于期望抓捕区域，并且末端工具的法向量与期望抓捕面垂直。区域抓捕方法的抓捕锁紧条件可以容许较大的位姿偏差。

[0065] 首先考虑位置容差Ep，机械臂的末端中心点与抓捕点的位置偏差满足一号抓捕区域的边界约束条件，亦即：

[0066] Ep＝{0≤xe-xc≤a；0≤ye-yc≤a；ze-zc＝0}；

[0067] 同时考虑姿态容差Er，只需要保证机械臂末段工具的法向量垂直于抓捕面，因此机械臂末端工具的姿态与抓捕点的姿态偏差(采用ZYX欧拉角表示)应满足如下约束：

[0068] Er＝{(θ 0 0)；θ∈[-π,π]}；

[0069] 步骤4中参考机械臂对期望抓捕区域进行位姿跟踪接近，需要获取机械臂末端与期望抓捕点之间的位姿偏差[△pe △ψe]T。其中机械臂末端与抓捕点的位置偏差可以直接通过视觉相机测量得到，而姿态偏差则简化为末端抓捕工具的法向量与抓捕面的法向量之间的夹角。

[0070] 机械臂末端与抓捕面法向量的指向偏差可以通过转轴l和转角θ表示，其中转轴l和转角θ分别通过下式求解：

[0071]

[0072] 进而得到表示机械臂末端与抓捕面法向量的指向偏差的转轴l和转角θ，由此得到机械臂末端的姿态偏差即△ψe为：

[0073] △ψe＝lθ

[0074] 步骤5中将空间机器人系统等效为组合机械臂，通过相对雅克比矩阵控制其协同运动。相对雅克比矩阵可以将双臂末端不同的操作任务等效为以其中一个机械臂为参考的组合机械臂的操作任务。

[0075] 根据双臂协同操作时的相对位置关系，可以得到相对位置约束方程如下：

[0076] 1p2+1R22p3＝1p4+1R44p3；

[0077] 2p3＝2R1(1p4+1R44p3-1p2)；

[0078]

[0079] 同理，得到双臂协同操作时的相对姿态约束方程如下：

[0080] 1R22R3＝1R44R3；

[0081]

[0082] 对速度微分方程进行整理和化简，可以得到双臂协同操作的相对雅克比矩阵JR如下所示：

[0083] JR＝[-2ψ3 2Ω1JA 2Ω4JB]。

[0084] 参照图1-2，是本发明一实施例的基于双臂的区域抓捕策略示意图。通过对非合作翻滚抓捕对象以及抓捕方案的分析，提出一种与以往机械臂针对固定抓捕点的抓捕方式不同的区域环抱抓捕方案。区域环抱抓捕通过机械臂末端工具(柔性环节)与抓捕点附近的区域形成面接触，从而实现对目标卫星的抓捕稳定，参照图1所示，基于双臂的区域抓捕主要通过机械臂末端工具与抓捕面之间形成的压力以及摩擦力实现抓捕、稳定，最终空间机器人以及目标卫星之间形成弱连接闭链约束关系。

[0085] 参照,3，目标失效卫星安装有太阳翼的两侧面最为规则、障碍物较少，因此选择目标卫星安装有太阳翼的两侧面为环抱抓捕的目标抓捕面。由于区域环抱抓捕方法不需要对某一特定抓捕点进行抓捕，因此当机械臂末端在特定的抓捕区域且满足抓捕条件时，即可以控制机械臂实现对自旋目标卫星的抓捕稳定。翻滚目标抓捕面上的抓捕区域如图3所示，表1给出了各个抓捕区域的定义。

[0086] 表1

[0087]

[0088] 区域环抱抓捕实现自旋失效卫星的抓捕稳定，关键在于保持机械臂末端工具与抓捕面之间的面接触，通过保持一定的正压力与摩擦力使失效卫星达到消旋、稳定状态。不失一般性，以一号抓捕区域为例，对区域抓捕方法的容差进行非分析。

[0089] 首先考虑位置容差Ep，机械臂的末端中心点与抓捕点的位置偏差满足一号抓捕区域的边界约束条件，亦即：

[0090] Ep：{0≤xe-xc≤a；0≤ye-yc≤a；ze-zc＝0}  (1)

[0091] 同时考虑姿态容差Er，只需要保证机械臂末段工具的法向量垂直于抓捕面，因此机械臂末端工具的姿态与抓捕点的姿态偏差采用ZYX欧拉角表示)应满足如下约束：

[0092] Er：{(α 0 0)；α∈[-π,π]}  (2)

[0093] 当已知目标卫星抓捕对象的视觉测量信息时，可以采用基于视觉信息的运动控制。基于视觉信息的运动控制是利用机械臂手眼相机的相对位姿测量值(手柄坐标系相对于机器人末端坐标系的位置和姿态)，实时规划空间机器人的运动，直至最终达到目标抓捕区域。主要包括如下步骤：手眼相机测量、位姿偏差计算、目标运动的预测、空间机器人末端运动速度规划、空间机器人避奇异的路径规划等。首先，根据手眼测量数据判断相对位姿偏差ep和eo是否满足期望的抓捕区域阈值εp和εo(即捕获区域①②③④)，若小于，则闭合手爪、捕获目标；反之，则根据相对位姿偏差，实时估计目标的运动状态，并将估计的结果反馈到机械臂末端速度的规划中，以保证机械臂末端时刻朝最近的方向(直线)趋近目标，机械臂末端能自主跟踪目标的运动，直到最后捕获目标。规划出末端运动速度后，解算出关节的期望角速度，实现机械臂对目标的跟踪捕获。整个过程一直持续到机械臂到达期望的抓捕区域为止，上述跟踪抓捕控制流程主要采用的控制算法如下：

[0094] 1)位姿偏差测量算法

[0095] 采用上述跟踪抓捕算法，需要提供目标相对于机械臂末端(用坐标系∑E表示)的相对位置和姿态，分别表示为△pe和△ψe，即：

[0096] △pe＝pt-pe＝[X,Y,Z]T  (3)

[0097] △ψe＝[α,β,γ]T  (4)

[0098] 其中，姿态采用z-y-x欧拉角表示。

[0099] 大容差区域抓捕的示意图如图4所示，其中ZC为抓捕面的法向量，ZE为机械臂末端抓捕工具的法向量。当机械臂末端在抓捕面上的投影位于抓捕区域内且机械臂末端抓捕工具的法向量与抓捕面的法向量夹角为零时，抓捕条件满足，可以进行对自旋目标卫星的抓捕。

[0100] 为了进行机械臂末端的期望运动规划，需要获取机械臂末端与期望抓捕点之间的位姿偏差[△pe△ψe]T。其中机械臂末端与抓捕点的位置偏差可以直接通过视觉相机测量得到，而姿态偏差则简化为末端抓捕工具的法向量与抓捕面的法向量之间的夹角。

[0101] 机械臂末端与抓捕面法向量的指向偏差可以通过转轴l和转角θ表示，其中转轴l和转角θ分别通过下式求解：

[0102]

[0103] l＝zE×zC  (6)

[0104] 根据视觉相机测量，由式(5)和(6)计算得到表示机械臂末端与抓捕面法向量的指向偏差的转轴l和转角θ，由此得到机械臂末端的姿态偏差即△ψe为：

[0105] △ψe＝lθ  (7)

[0106] 2)空间机器人末端运动规划

[0107] 基于上述的估计结果，期望的末端运动速度按下式进行规划：

[0108]

[0109] 上式中，Kp、Kv分别为规划的比例、微分参数； 为估计的目标运动速度。

[0110] 在实际运用中，一般不允许机械臂末端的运动速度过大，因而，在选择K时要小心，同时最好对速度的上限作规定(增益矩阵设为单位阵，同时设定末端速度上限为vem＝60mm/s，角速度上限为6°/s)。而且，需要保证机械臂运动过程中启动段的平稳性，因此，对末端速度做下述处理：

[0111]

[0112]

[0113] 其中，ts为设定的平滑启动时间，如ts＝5s。这样可以确保机械臂在0-ts内平滑启动到合成速度最大值为vem(方向矢量仍然是ved)；而当启动后的抓捕过程中，机械臂末端速度≥vem时，将其限制在vem(方向矢量仍然是ved)。这样的处理对于实际工程实现非常重要。

[0114] 3)速度级逆运动学求解

[0115] 速度级微分运动学方程实际为6×n个方程组，若n＝6，则J是方阵，当J满秩时，机械臂关节角速度可按下式计算：

[0116]

[0117] 当n>6(本实施例采用冗余机械臂对象，n＝7，J为6×7的Jacobian矩阵)时，方程的已知量(机械臂末端速度，6个元素)个数大于未知量(机械臂关节角速度，7个)个数，若J的秩为6，即J为非奇异，方程(11)有无穷多解；若J的秩小于6，方程(11)无有效解，相应的臂型称为奇异臂型。

[0118] 对于冗余机械臂，式(11)的解由特解和齐次解组成，可表示为：

[0119]

[0120] 式中，r为任意常数， 为满足式(11)的任意特解， 为齐次解，满足下式：

[0121]

[0122] 一般情况下，可按下式计算方程(11)的最小范数解：

[0123]

[0124] 其中，J+为J的Moore-Penrose广义逆(或称为伪逆)，按下式计算：

[0125] J+＝JT(JJT)-1  (15)

[0126] 参照图5，双臂协同抓捕非合作翻滚目标任务中采用相对雅克比的概念，其中参考机械臂根据局部视觉测量信息实现对目标抓捕点的跟踪，而后通过相对雅克比矩阵控制组合机械臂实现对目标的环抱锁紧。相对雅克比矩阵通过将双臂末端不同的操作任务等效为相对于其中一个机械臂的操作任务，将双臂协同控制问题简化为单臂的控制，简化了控制的实现。参照图5所示，根据双臂协同操作时的相对位置关系，可以得到相对位置约束方程如下：

[0127] 1p2+1R22p3＝1p4+1R44p3  (16)

[0128] 2p3＝2R1(1p4+1R44p3-1p2)  (17)

[0129]

[0130] 同理，根据双臂协同操作时的相对姿态关系，可以得到相对姿态约束方程如下：

[0131] 1R22R3＝1R44R3  (19)

[0132]

[0133] 对速度微分方程进行整理和化简，可以得到双臂协同操作的相对雅克比矩阵JR如下所示：

[0134] JR＝[-2ψ3 2Ω1JA 2Ω4JB]  (21)

[0135] 其中：

[0136] 其中，JA(Θ)、JB(Θ)∈R6×n为机械臂的Jacobian矩阵，是关节角的函数，它建立了机械臂关节角速度与末端运动速度之间的关系。机械臂的Jacobian矩阵J(Θ)可写成如下形式：

[0137] J(Θ)＝[J1,J2,…,Jn]  (22)

[0138] 假设ξi表示第i个关节的旋转轴、pi表示第i个关节的位置(连杆坐标系的原点)，则Jacobian矩阵的第i列可按下式计算：

[0139]

[0140] RefJ＝[RefJ1 RefJ2 … RefJn]  (24)

[0141] 参照图6，定义双臂空间机器人其中一个机械臂作为参考机械臂，而整个空间机器人系统又可以看作为以参考机械臂末端为基座的组合机械臂。因此协同捕获流程可以描述为：参考机械臂主要进行目标抓捕点的位姿测量，为组合机械臂协同抓捕提供参考；同时根据抓捕与消旋需求，通过相对雅克比矩阵控制组合机械臂运动；当参考机械臂的末端位姿满足期望的抓捕区域条件时，控制组合机械臂进行协同捕获。协调捕获流程如图6所示。

[0142] 该协调捕获方法具有如下两点优势：其一是采用区域抓捕的概念，抓捕的约束条件更少，抓捕容差更大；其二是只需一个目标点的视觉测量信息便可以实现双臂之间的协同抓捕控制，算法效率以及实时性更高。

[0143] 以上是对本发明的较佳实施进行了具体说明，但本发明创造并不限于所述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可做出种种的等同变形或替换，这些等同的变形或替换均包含在本申请权利要求所限定的范围内。