CN109606753A - 一种空间双臂机器人协同抓捕目标的控制方法 - Google Patents

Abstract

本发明公开了一种空间双臂机器人抓捕目标的协同控制方法，包括：建立空间双臂机器人基座和机器人的独立动力学模型；根据空间机器人基座零距离停靠控制方法，控制两个飞行器的相对位置和视线指向；机械臂基于视觉伺服的协同规划与控制方法使两个机械臂同时到达目标；空间机器人的动力学与运动学耦合补偿方法，利用相对导航信息估计基座与目标相对运动的耦合，补偿机械臂的运动，同时利用机械臂的状态估算对平台的反作用力，作为前馈补偿基座控制。本发明针对空间双臂机器人抓捕问题，提供一种抓捕翻滚目标的协同控制方法，保证双臂对目标抓捕的时间一致性，再通过基座和两个机械臂的互相补偿，达到高精度抓捕的目的。

Description

一种空间双臂机器人协同抓捕目标的控制方法

技术领域

本发明属于空间机器人技术领域，尤其涉及一种空间双臂机器人协同抓捕目标的控制方法。

背景技术

随着空间技术的发展，飞行器的能力越来越强，在轨维护和服务也逐渐成为发 展的主流方向。另一方面，大量失效卫星由于没有及时离轨而成为空间碎片，占据 这宝贵的空间轨道资源。

空间机器人，指携带操作机械臂的空间服务飞行器，对目标的抓捕主要分为两类：一是针对非合作目标进行抓捕，这类目标没有视觉测量的标志器和适应机械臂 抓捕工具的适配器，由于失控、燃料泄漏、空间摄动力的影响，通常目标可能具有 自旋、章动等运动特性，此类抓捕难度较大；二是针对合作目标进行抓捕，由于事 先设计可以配置的标志器和适配器，并且目标星具有可控性，能保持较好的姿态稳 定度，此类抓捕难度相对较小。

研究如何利用空间机器人抓捕稳定或翻滚的航天器是在轨服务技术的关键问题。由于目标的运动特性复杂，在抓捕过程中需要克服两个飞行器之间的相对速度 和角速度差造成的动量交换，减小对机械臂的冲击力，可以采用两套或多套机械臂 协同控制的方式实施抓捕，发挥多机械臂组合的能力。

发明内容

本发明针对空间双臂机器人抓捕问题，提供一种抓捕翻滚目标的协同控制方法，保 证双臂对目标抓捕的时间一致性，再通过基座和两个机械臂的互相补偿，达到高精度抓捕的目的。

为了达到上述目的，本发明公开了以下技术方案予以实现，一种空间双臂机器人协 同抓捕目标的控制方法，包括以下步骤：

步骤1，建立空间双臂机器人动力学控制方程；

步骤2，空间机器人基座零距离停靠控制方法；

步骤3，机械臂采用基于视觉伺服的协同规划与控制方法使两个机械臂同时到达目 标抓捕位置；

步骤4，利用相对导航信息估计基座与目标相对运动的耦合，补偿机械臂的运动，同时利用机械臂的状态估算对平台的反作用力，作为前馈补偿基座控制。

进一步，在上述空间双臂机器人协同抓捕控制方法中，所述步骤1，空间双臂机械臂动力学控制方程，包含两部分：空间双臂机器人基座动力学和机械臂动力学方程。 其中基座动力学方程，经过适当的简化，，忽略高阶耦合项，得到如下：

J为空间机器人系统的惯量矩阵，M为空间机器人系统的质量，T_c为基座姿控系 统产生的控制力矩，T_d1和T_d2分别为两套机械臂运动对基座的耦合作用力矩。F_c为基座 轨控系统产生的位置控制力，F_d1和F_d2分别为两套机械臂运动对基座的耦合作用力矩。 机械臂动力学方程，为机械臂关节运动与关节力和力矩的动力学方程：

其中，H为机械臂的惯性矩阵，为机械臂的非线性项，τ为机械臂各个关 节的力矩。

在上述空间双臂机器人协同抓捕控制方法中，所述步骤2，空间机器人基座零距离停靠控制方法，包含两部分：基座位置相对位置控制律和基座相对姿态控制律。

相对位置控制律如下：

其中，F_cx,F_cy,F_cz分别为三轴控制力矩，K_opx,K_opy,K_opz和K_odx,K_ody,K_odz为三轴为轨道控制PD参数，X₀为基座相对目标悬停的标称距离，ΔX，ΔY，ΔZ为基座质心与目 标的相对位置。

相对姿态控制采用：

其中，T_cx,T_cy,T_cz分别为三轴控制力矩，K_apx,K_apy,K_apz和K_adx,K_ady,K_adz为三轴姿态控制PD参数，γ₀为基座与目标的相对滚动角期望值，Δγ为基座与目标的相对滚动角测量值，Δθ，Δψ为基座与目标相对视线的高低角和方位角。

在上述空间双臂机器人协同抓捕控制方法中，所述步骤3，机械臂采用基于视觉伺服的协同规划与接近控制方法，包含两部分：基于视觉伺服的协同规划和基于视觉伺服 的接近控制方法。

步骤3.1，基于视觉伺服的协同规划，采用基于驱动函数的路径规划，利用驱动函数D(λ)参数化期望的相对运动。驱动函数D(λ)是归一化时间λ的函数，进而得到时间 序列下的相对希望位姿转移矩阵：

其中为初始时刻机械臂末端到目标抓捕点相对位姿的齐次矩阵，驱动函数D(λ) 采用梯形规划，根据最大速度约束和时间约束得到梯形曲线的参数，两套机械臂采用相 同的驱动函数，从而保证不同初始位姿下到达目标的时间一致性。

对于姿态和位置分别进行计算。考虑到相对姿态的欧拉角可能存在奇异的情况，采用四元数描述法来进行计算：

其中，q⁰为机械臂末端与目标的初始相对姿态四元数，q¹为目标姿态四元数。q(t)和分别为当前期望的姿态四元数和四元数速度。和分别为归一化驱动函数位 置和速度。

同理可以得到期望的相对位置轨迹，p(t)和

根据q(t)、和p(t)、可以得到期望的相对位姿转移矩阵和速度的时间序列：

需要说明的是，对于两套机械臂，机械臂末端相对目标的初始位姿是不同的，采用相同的驱动函数和相同计算步骤分别计算相对位姿转移矩阵时间序列期望速度 时间序列v_ref。

步骤3.2，基于视觉伺服的接近控制方法，采用基于方向余弦阵的相对姿态控制律和相 对位置控制方法。

其中ΔR为3×3的矩阵，表示机械臂末端轨迹与期望轨迹的误差，为的左上三角矩阵，为相对测量系统计算的机械臂末端到目标的相对姿态矩阵。

对于姿态跟踪控制，

ΔAx＝K_Apx·(ΔR(3,2)-ΔR(2,3))

ΔAy＝K_Apy·(ΔR(1,3)-ΔR(3,1))

ΔAz＝K_Apz·(ΔR(2,1)-ΔR(1,2))

对于位置跟踪控制，

Δpx＝K_px·(Δr_x)

Δpy＝K_py·(Δr_y)

Δpz＝K_pz·(Δr_z)

其中r_m是相对测量系统计算的机械臂末端到目标的相对位置，是的有第 四列位置向量。

将姿态和位置控制量合并为6×1的矩阵，即

Δx＝[Δpx Δpy Δpz ΔAx ΔAy ΔAz]^T

进一步计算关节控制增量

其中ΔT为控制周期，J_a为机械臂雅克比矩阵，v_ref为期望速度，v_forword为速度的 前馈将在下一个步骤中计算。

基于式(2)，进一步得到

q_d＝Δq+q_t

其中q_t为当前的关节角度，q_d机械臂关节的运动位置指令。

在上述空间双臂机器人协同抓捕控制方法中，所述步骤4，空间机器人的动力学与运动学耦合补偿方法，包含两部分：一是利用机械臂的状态估算对平台的反作用力，作 为前馈补偿基座控制；二是利用相对导航信息估计基座与目标相对运动的耦合，补偿机 械臂的运动。具体方法如下：

步骤4.1，计算利用机械臂的动力学计算机械臂对基座的反作用力和力矩，作为基座的控制力和力矩的前馈补偿量。

步骤4.2，计算基座运动对机械臂的运动学补偿。主要考虑基座到机械臂的位置级和速 度级耦合模型，作为机械臂运动补偿的前馈。

目标抓捕点到机械臂末端的相对速度计算公式如下：

式中，R_sbj为机械臂安装系到基座本体系的坐标转换矩阵。R_st为目标本体系到基座本体的坐标转换矩阵。ω_s、ω_t分别为基座、目标本体系相对惯性系的角速度；ω_j为机 械臂安装坐标系相对惯性系的角速度。r_st为目标抓捕中心在基座本体系的位置矢量；r_s0为机械臂安装位置在基座本体系的位置矢量；r为目标抓被捕点在机械臂末端坐标系的 位置矢量；r_t1为目标抓捕点相对抓捕中心在在目标本体系的位置矢量。

步骤4.3，将步骤4.1计算的动力学前馈补偿量带入式(1)，形成对基座的最终控制量；将步骤4.2计算的运动学补偿量代入式(2)，形成对机械臂控制的最终控制量。

与现有技术相比，本发明具有以下优点：

本发明解决了空间双臂机器人协同抓捕问题，提出了一种协同控制的抓捕方法，包 括建立空间双臂机器人和失效旋转目标的动力学和运动学方程；提出了空间机器人基座 零距离逼近与停靠控制方法；采用了机械臂基于视觉伺服的协同规划与控制方法使两个 机械臂同时到达目标；利用相对导航信息估计基座与目标相对运动的耦合，补偿机械臂的运动，同时利用机械臂的状态估算对平台的反作用力，作为前馈补偿基座控制；本发 明提出的协同控制抓捕方法使得整个抓捕操作过程中，双臂互补干扰独立控制，而且抓 捕精度也比较高。

附图说明

图1是本发明实施例中所述的空间双臂机器人示意图。

图2是本发明实施例采用的控制原理图。

图3是本发明实施例中所述的基于驱动函数的机械臂规划方法示意图。

图4是本发明实施例中所述的驱动函数梯形规划曲线示意图。

图5是本发明实施例中所述的机械臂对动态目标跟踪控制原理图。

图6是本发明实施例中所述的空间机器人协同抓捕控制效果仿真图(相对位置控制结 果)。

图7是本发明实施例中所述的空间机器人协同抓捕控制效果仿真图(相对姿态控制结果)。

具体实施方式

以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照 本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保护范围之内。本发明说明书中未作详细描述的内容属于本领域专业技术人员的公知技术。

为了更加清晰的阐述本发明的技术方案，以下通过实施例对本发明的技术方案进行 说明。本发明针对空间双臂机器人抓捕问题，提供一种抓捕翻滚目标的协同控制方法，保证双臂对目标抓捕的时间一致性，再通过基座和两个机械臂的互相补偿，达到高精度 抓捕的目的。

本实施例中，空间翻滚目标处于沿最大惯量轴自旋状态。参照图1，空间双臂机器人由一个飞行器基座和两套7自由度机械臂组成，机械臂末端各装有一套抓捕工具，抓 捕工具针对翻滚目标的对接环实施实施抓捕。

参照图2，本实施例中，待捕获的翻滚目标与空间双臂机器人的相对位姿由绝对姿态与相对位姿测量导航系统解算得到，机械臂抓捕工具到目标对接环的相对位姿由基于手眼市局的目标相对位姿测量解算得到，机械臂的测量信息包括各个关节的角度数据， 这些作为本技术方案的所属的控制方法的输入信息。

通过本实施例所述的控制方法，空间双臂机器人系统相对翻滚目标起旋控制，并保 持相对旋转角速度在一定的范围内。本实施例中，所属空间双臂机器人抓捕目标的协同控制方法，包括：

步骤1，建立机械臂与基座的动力学模型。

步骤1.1，空间双臂机器人基座动力学方程，得到如下：

J为空间机器人系统的惯量矩阵，M为空间机器人系统的质量，T_c为基座姿控系 统产生的控制力矩，T_d1和T_d2分别为两套机械臂运动对基座的耦合作用力矩。F_c为基座 轨控系统产生的位置控制力，F_d1和F_d2分别为两套机械臂运动对基座的耦合作用力矩。

步骤1.2，本实施例中，机械臂动力学方程，如下：

其中，H为机械臂的惯性矩阵，为机械臂的非线性项，τ为机械臂各个关 节的力矩。

步骤2，采用空间机器人基座零距离停靠控制方法，保持基座与目标的相对悬停在一个标称值。

步骤2.1基座位置相对位置控制律和基座相对姿态控制律。

相对位置控制律如下：

其中，F_cx,F_cy,F_cz分别为三轴控制力矩，K_opx,K_opy,K_opz和K_odx,K_ody,K_odz为三轴为轨道控制PD参数，X₀为基座相对目标悬停的标称距离，ΔX，ΔY，ΔZ为基座质心与目 标的相对位置。

相对姿态控制采用

其中，T_cx,T_cy,T_cz分别为三轴控制力矩，K_apx,K_apy,K_apz和K_adx,K_ady,K_adz为三轴姿态控制 PD参数，γ₀为基座与目标的相对滚动角期望值，Δγ为基座与目标的相对滚动角测量值， Δθ，Δψ为基座与目标相对视线的高低角和方位角。

步骤3，机械臂采用基于视觉伺服的协同规划与接近控制方法。

步骤3.1，基于视觉伺服的协同规划。采用基于驱动函数的路径规划，利用驱动函数D(λ)参数化期望的相对运动。参照图3两套机械臂采用相同的驱动函数。驱动函数 D(λ)是归一化时间λ的函数，进而得到时间序列下的相对希望位姿转移矩阵：

其中为初始时刻机械臂末端到目标抓捕点相对位姿的齐次矩阵，驱动函数D(λ)采用梯形规划，根据最大速度约束和时间约束得到梯形曲线的参数，图4为本实施例所 述梯形规划的曲线。

本实施例中，对于姿态和位置分别进行计算。考虑到相对姿态的欧拉角可能存 在奇异的情况，采用四元数描述法来进行计算：

其中，q⁰为机械臂末端与目标的初始相对姿态四元数，q¹为目标姿态四元数。q(t)和 分别为当前期望的姿态四元数和四元数速度。和分别为归一化驱动函数位置和速度。

同理可以得到期望的相对位置轨迹，p(t)和

根据q(t)、和p(t)、可以得到期望的相对位姿转移矩阵和速度的时间序列：

对于两套机械臂利用式(5)～(8)，分别计算相对位姿转移矩阵时间序列期望 速度时间序列v_ref。

步骤3.2，基于视觉伺服的接近控制方法，采用基于方向余弦阵的相对姿态控制律和相 对位置控制方法。

其中ΔR为3×3的矩阵，表示机械臂末端轨迹与期望轨迹的误差，为的左 上三角矩阵，为相对测量系统计算的机械臂末端到目标的相对姿态矩阵。

对于姿态跟踪控制，

A_ex＝(ΔR(3,2)-ΔR(2,3))

A_ey＝(ΔR(1,3)-ΔR(3,1))

A_ez＝(ΔR(2,1)-ΔR(1,2))

其中为A_ex的差分项，其它同理。

对于位置跟踪控制，

其中为r_ex的差分项，其它同理。

将姿态和位置控制量合并为6×1的矩阵，即

Δx＝[Δpx Δpy Δpz ΔAx ΔAy ΔAz]^T

进一步计算关节控制增量

其中ΔT为控制周期，为机械臂雅克比矩阵的广义逆，v_ref为期望速度，v_forword为速 度的前馈量，将在下一个步骤中计算。

基于式(9)，进一步得到

q_d＝Δq+q_t

其中q_t为当前的关节角度，q_d机械臂关节的运动位置指令。

步骤4，计算间机器人的动力学与运动学耦合补偿。

步骤4.1，计算机械臂对基座的反作用力，作为基座的控制力矩前馈补偿量。

整个算法由两部分组成：

首先向外递推计算各连杆的速度和加速度，由牛顿-欧拉公式算出各连杆的惯性力f和 力矩n(i:0->n-1)，n为关节数量：

然后，向内递推计算各连杆相互作用的力和力矩，以及关节驱动力或力矩(i:-n>1)：

其中，ⁱf_ci：杆件i所受合外力在坐标系{i}中的表示；

ⁱf_i：杆件i-1对杆件i所施加的力在坐标系{i}中的表示；

ⁱf_i+1：杆件i对杆件i+1所施加的力在坐标系{i}中的表示；

ⁱn_ci：杆件i所受合外力矩在坐标系{i}中的表示；

ⁱn_i：杆件i+1对杆件i所施加的力矩在坐标系{i}中的表示；

ⁱn_i+1：杆件i对杆件i+1所施加的力矩在坐标系{i}中的表示；

^ci+1I_i+1：第i+1根杆件相对质心的惯性张量；

ω₀＝ω_b (基座角速度与角加速度，若基座固定，ω₀＝0)

ⁿ⁺¹f_n+1＝F_e ⁿ⁺¹n_n+1＝M_e(若机械臂在自由空间无接触力，ⁿ⁺¹f_n+1＝ⁿ⁺¹n_n+1＝0) 根据上述计算步骤，得到式(4)中

步骤4.2，计算基座运动对机械臂的运动学补偿。主要考虑基座到机械臂的位置级和速 度级耦合模型，作为机械臂运动补偿的前馈。

目标抓捕点到机械臂末端的相对速度计算公式如下：

式中，R_sbj为机械臂安装系到基座本体系的坐标转换矩阵。R_st为目标本体系到基座本体 的坐标转换矩阵。ω_s、ω_t分别为基座、目标本体系相对惯性系的角速度；ω_j为机械臂安装坐标系相对惯性系的角速度。r_st为目标抓捕中心在基座本体系的位置矢量；r_s0为 机械臂安装位置在基座本体系的位置矢量；r为目标抓被捕点在机械臂末端坐标系的位 置矢量；r_t1为目标抓捕点相对抓捕中心在在目标本体系的位置矢量。

步骤4.3，将步骤4.1计算的动力学前馈补偿量带入式(1)，形成对基座的最终控制量； 将步骤4.2计算的运动学补偿量代入式(3)，形成对机械臂控制的最终控制量。

下面给出一组仿真算例，以空间双臂机器人抓捕目标的协同控制方法。假设大型空 间碎片处于GEO轨道，以三轴角速度[5,1,1]°/s为初始速度作自由翻滚运动。空间机 器人相对目标悬停距离5m。机械臂规划总时间为40s，归一化速度为0.0375，梯形规划 时间t₁,t₂,t₃分别为13.3s，13.4s,13.3s。从第60s开始实施抓捕控制。机械臂抓捕目标 的相对位置和相对姿态控制结果图见图6、图7。

上述仅为本发明的具体实施例，该实施例只为更清楚的说明本发明所用，而并非对 本发明的限定，任何本领域的技术人员能思之的变化，都应落在保护范围内。

Claims (6)

1.一种空间双臂机器人抓捕协同目标的控制方法，其特征在于，包括以下步骤：

1)建立空间双臂机器人基座和机器人的独立动力学模型；

2)根据空间机器人基座零距离停靠控制方法，控制两个飞行器相对位置和视线指向；

3)机械臂基于视觉伺服的协同规划与控制方法使两个机械臂同时到达目标；

4)空间机器人的动力学与运动学耦合补偿方法，利用相对导航信息估计基座与目标相对运动的耦合，补偿机械臂的运动，同时利用机械臂的状态估算对平台的反作用力，作为前馈补偿基座控制。

2.根据权利要求1所述的一种空间双臂机器人协同抓捕目标的控制方法，其特征在于，所述步骤1)的具体方法如下：基座动力学方程，忽略高阶耦合项，如下：

其中，J为空间机器人系统的惯量矩阵，M为空间机器人系统的质量，T_c为基座姿控系统产生的控制力矩，T_d1和T_d2分别为两套机械臂运动对基座的耦合作用力矩；

F_c为基座轨控系统产生的位置控制力，F_d1和F_d2分别为两套机械臂运动对基座的耦合作用力矩；

机械臂动力学方程，为机械臂关节运动与关节力和力矩的动力学方程：

其中，H为机械臂的惯性矩阵，为机械臂的非线性项，τ为机械臂各个关节的力矩。

3.根据权利要求2所述的一种空间双臂机器人协同抓捕目标的控制方法，其特征在于，所述步骤2)的具体方法包含基座位置相对位置控制律和基座相对姿态控制律两部分，相对位置控制律如下：

其中，F_cx,F_cy,F_cz分别为三轴控制力矩，K_opx,K_opy,K_opz和K_odx,K_ody,K_odz为三轴为轨道控制PD参数，X₀为基座相对目标悬停的标称距离，ΔX，ΔY，ΔZ为基座质心与目标的相对位置；

相对姿态控制采用：

其中，T_cx,T_cy,T_cz分别为三轴控制力矩，K_apx,K_apy,K_apz和K_adx,K_ady,K_adz为三轴姿态控制PD参数，γ₀为基座与目标的相对滚动角期望值，Δγ为基座与目标的相对滚动角测量值，Δθ，Δψ为基座与目标相对视线的高低角和方位角。

4.根据权利要求3所述的一种空间双臂机器人协同抓捕目标的控制方法，其特征在于，所述步骤3)采用基于驱动函数的路径规划，利用驱动变换D(λ)参数化期望的相对运动，驱动变换D(λ)是归一化时间λ的函数，进而得到时间序列下的相对希望位姿转移矩阵

其中，驱动函数采用梯形规划，根据最大速度约束和时间约束得到梯形曲线的参数；

利用驱动函数同时规划两套机械臂的运动期望路径，进而实现基于视觉的接近控制；对于姿态和位置分别进行计算；考虑到相对姿态的欧拉角可能存在奇异的情况，采用四元数描述法来进行计算：

其中，q⁰为机械臂末端与目标的初始相对姿态四元数，q¹为目标姿态四元数；q(t)和分别为当前期望的姿态四元数和四元数速度；和分别为归一化驱动函数位置和速度；同理可以得到期望的相对位置轨迹，p(t)和

根据q(t)、和p(t)、可以得到期望的相对位姿转移矩阵和速度的时间序列：

5.根据权利要求4所述的一种空间双臂机器人协同抓捕目标的控制方法，其特征在于，采用基于方向余弦阵的相对姿态控制律和相对位置控制方法，具体方法如下：

其中ΔR为3×3的矩阵，表示机械臂末端轨迹与期望轨迹的误差，为的左上三角矩阵，为相对测量系统计算的机械臂末端到目标的相对姿态矩阵；

其中ΔR为3×3的矩阵，表示机械臂末端轨迹与期望轨迹的误差，为的左上三角矩阵，为相对测量系统计算的机械臂末端到目标的相对姿态矩阵；

对于姿态跟踪控制，

A_ex＝(ΔR(3,2)-ΔR(2,3))

A_ey＝(ΔR(1,3)-ΔR(3,1))

A_ez＝(ΔR(2,1)-ΔR(1,2))

其中为A_ex的差分项，其它同理；

对于位置跟踪控制，

其中，r_m是相对测量系统计算的机械臂末端到目标的相对位置，是的有第四列位置向量；

其中为r_ex的差分项，其它同理；

将姿态和位置控制量合并为6×1的矩阵，即Δx＝[Δpx Δpy Δpz ΔAx ΔAy ΔAz]^T

进一步计算关节控制增量

其中，ΔT为控制周期，J_a为机械臂雅克比矩阵，v_ref为期望速度，v_forword为速度的前馈将在下一个步骤中计算；基于式(2)，进一步得到

q_d＝Δq+q_t

其中，q_t为当前的关节角度，q_d机械臂关节的运动位置指令。

6.根据权利要求5所述的一种空间双臂机器人协同抓捕目标的控制方法，其特征在于，所述步骤4)空间机器人的动力学与运动学耦合补偿方法，包含两部分：一是利用机械臂的状态估算对平台的反作用力，作为前馈补偿基座控制；二是利用相对导航信息估计基座与目标相对运动的耦合，补偿机械臂的运动；

步骤4.1，计算利用机械臂的动力学计算机械臂对基座的反作用力和力矩，作为基座的控制力和力矩的前馈补偿量；

步骤4.2，计算基座运动对机械臂的运动学补偿；主要考虑基座到机械臂的位置级和速度级耦合模型，作为机械臂运动补偿的前馈；

目标抓捕点到机械臂末端的相对速度计算公式如下：

式中，R_sbj为机械臂安装系到基座本体系的坐标转换矩阵；R_st为目标本体系到基座本体的坐标转换矩阵。ω_s、ω_t分别为基座、目标本体系相对惯性系的角速度；ω_j为机械臂安装坐标系相对惯性系的角速度；r_st为目标抓捕中心在基座本体系的位置矢量；r_s0为机械臂安装位置在基座本体系的位置矢量；r为目标抓被捕点在机械臂末端坐标系的位置矢量；r_t1为目标抓捕点相对抓捕中心在在目标本体系的位置矢量；

步骤4.3，将步骤4.1计算的动力学前馈补偿量带入式(1)，形成对基座的最终控制量；将步骤4.2计算的运动学补偿量(3)代入式(2)，形成对机械臂控制的最终控制量。