CN104015191B - 基于基座卫星角速度的空间机械臂工具坐标下的运动补偿方法 - Google Patents

Abstract

基于基座卫星角速度的空间机械臂工具坐标下的运动补偿方法，涉及一种空间机械臂工具坐标下的运动补偿方法。本发明通过简单的机械臂逆运动学计算，得到规划的机械臂各关节角速度，解决漂浮基座下空间机械臂的运动规划问题。计算空间机器人期望的末端线速度和角速度；利用卫星基座的陀螺获得基座角速度；计算卫星基座的线速度；计算基座雅克比矩阵；计算空间机械臂雅克比矩阵；利用基座速度对空间机器人期望的末端速度在工具坐标下进行补偿，计算空间机械臂期望运动的关节速度。本发明考虑了机械臂与卫星基座的耦合运动，在同样的控制律下可以使空间机械臂更快地到达目标点，有利于空间机器人快速规划，可用于空间机器人的在轨维护及操作。

Description

基于基座卫星角速度的空间机械臂工具坐标下的运动补偿方法

技术领域

本发明涉及一种空间机械臂工具坐标下的运动补偿方法，涉及漂浮基座下空间机械臂运动规划领域。

背景技术

随着空间科学技术的发展，人类的活动空间在不断地向太空扩展，日本、加拿大、欧空局、美国等国家和机构大力发展空间机器人技术，并相继发射试验卫星进行了相关试验。空间机器人一般由机器人载体(航天器或卫星)和上面搭载的机械臂构成。空间机器人执行空间任务时，通常基座处于自由漂浮状态，基座载体与机械臂之间存在动力学耦合。机械臂的运动会改变载体卫星的位置和姿态，而载体卫星位置姿态的改变也会反过来影响空间机械臂末端的定位，同时也会放大空间机械臂末端操作器的定位误差，不利于空间机械臂的高精度控制。因此，无法直接将固定基座机械臂的规划方法直接运用于漂浮基座下空间机械臂的运动规划。

对空间机械臂的运动规划，是空间机器人完成任务的首要条件。在微重力环境下，漂浮基座下的空间机器人的线动量与角动量守恒。一些学者根据空间机器人基座与连杆的关系提出虚拟机械臂的概念，建立了一个无质量的运动链，其基座为虚地。虚拟机械臂仅仅体现了线动量守恒，不能给出机械臂关节空间与工作空间的速度变化关系。另有学者根据线动量守恒和角动量守恒，提出了关于工作空间和关节空间运动速度的广义雅克比矩阵，该矩阵与空间机器人的几何参数、质量和转动惯量有关。在完成空间任务时，卫星本体燃料消耗会引起质量和惯量发生变化，无法实时获得；广义雅克比矩阵计算繁琐、求逆计算量比较大；星载计算机计算能力有限，都使得广义雅克比矩阵很难用于实际工程。因此，提出一种漂浮基座下简单实用的空间机械臂运动计算方法，是非常迫切和必要的。

发明内容

本发明提供一种基于基座卫星角速度的空间机械臂工具坐标下的运动补偿方法，通过简单的机械臂逆运动学计算，得到规划的机械臂各关节角速度，解决漂浮基座下空间机械臂的运动规划问题。

本发明方法利用卫星基座的陀螺获得的基座角速度，并计算的基座线速度，对漂浮基座下的机械臂末端期望速度进行运动补偿，将机械臂与卫星的动力学耦合规划转化为机械臂的运动学规划，通过简单的机械臂逆运动学计算，得到规划的机械臂各关节角速度。基于基座卫星角速度的机械臂工具坐标下的运动补偿方法由以下步骤完成：

步骤一、根据当前位姿与目标的位姿，计算空间机器人期望的末端线速度v_e和角速度ω_e；

步骤二、利用卫星基座的陀螺获得基座角速度ω₀；

步骤三、计算卫星基座的线速度v₀；

步骤四、计算基座雅克比矩阵J_b；

步骤五、计算空间机械臂雅克比矩阵J_m；

步骤六、利用基座速度对空间机器人期望的末端速度在工具坐标下进行补偿，计算空间机械臂期望运动的关节速度

步骤一所述的计算空间机器人期望的末端线速度v_e和角速度ω_e的公式为：

其中v_e和ω_e分别为空间机器人期望的末端线速度和角速度，k_v和k_w分别为末端线速度和角速度的调节比例系数，D_e为目标位置到空间机器人末端操作器的距离，η和分别为空间机器人旋转到目标姿态的欧拉转轴和转角，若目标位置相对于空间机器人当前姿态用四元数法表示为Q＝[q₁q₂q₃q₄]^T，则η和的计算公式为：

步骤三所述的计算卫星基座的线速度v₀的过程为：在空间环境自由漂浮情况下，空间机器人初始线动量为零根据空间机器人线动量守恒可得到

$P=\left[\begin{array}{cc}ME& M{\tilde{r}}_{0g}^T\end{array}\right]\left[\begin{array}{c}{v}_0\\ {\mathrm{\ω}}_0\end{array}\right]+J_{Tw}\stackrel{\·}{\mathrm{\Θ}}=O---\left(3\right)$

其中P是空间机器人系统线动量，是空间机器人机械臂的关节角速度向量，v₀，ω₀分别为卫星基座的线速度和角速度；M为空间机器人总质量，E为单位阵，r_0g为空间机器人质心到载体卫星质心的距离，是r_0g的协对称矩阵的转置；J_Tw为雅克比矩阵，表示为

$J_{Tw}=\underset{i=1}{\overset{n}{\Σ}}\left(m_i{J}_{Ti}\right)---\left(4\right)$

n为空间机器人机械臂的自由度，m_i为空间机器人机械臂的第i个臂杆质量，J_Ti利用式(5)计算为：

J_Ti＝[k₁×(r_i-p₁),k₂×(r_i-p₂),...,k_i×(r_i-p_i),0,...,0](5)

其中k_i为空间机器人第i个关节的转动方向，r_i和p_i分别是空间机器人第i个臂杆质心和末端位置，r_i-p_i为空间机器人第i个臂杆质心到第i个臂杆末端的距离；根据公式(12)，卫星基座的线速度v₀表示为：

$v_0=\frac{-J_{Tw}\stackrel{\·}{\mathrm{\Θ}}-M{\tilde{r}}_{0g}^T{\mathrm{\ω}}_0}{M}---\left(6\right).$

步骤四所述的计算基座雅克比矩阵J_b的过程为：

$J_b=\left(\begin{array}{cc}E& -{\tilde{p}}_{0e}\\ O& E\end{array}\right)---\left(7\right)$

其中p_0e为空间机器人末端位置，是p_0e的斜对称矩阵，E为单位阵。

步骤五所述的计算空间机械臂雅克比矩阵J_m的过程为：

$J_m=\left[\begin{array}{ccccc}{k}_1\×(p_e-p_1)& \mathrm{...}& k_i\×(p_e-p_i)& \mathrm{...}& k_n\×(p_e-p_n)\\ k_1& \mathrm{...}& k_i& \mathrm{...}& k_n\end{array}\right]---\left(8\right)$

其中k_i为空间机器人第i个关节的转动方向，p_e是空间机器人末端位置，p_i是空间机器人第i个臂杆末端位置，p_e-p_i空间机器人末端位置到空间机器人第i个臂杆末端的距离。

步骤六所述的利用基座速度对空间机器人期望的末端速度在工具坐标下进行补偿，计算空间机械臂期望运动的关节速度，其过程为：

利用基座卫星速度对空间机械臂的末端速度在工具坐标下补偿，并进行机械臂逆运动学计算，获得空间机械臂期望运动的关节速度；机械臂期望关节速度可用下式计算：

$\stackrel{\·}{\mathrm{\Θ}}={J_m}^{-1}\left(\left[\begin{array}{c}{v}_e\\ {\mathrm{\ω}}_e\end{array}\right]-J_b\left[\begin{array}{c}{v}_0\\ {\mathrm{\ω}}_0\end{array}\right]\right)---\left(9\right).$

本发明具有以下有益效果：本发明用一种简单的运动补偿方法解决了漂浮基座下空间机械臂的运动规划问题。利用卫星基座的陀螺测得的基座角速度及计算的基座线速度，通过对机械臂末端的期望运动速度在机械臂工具坐标下进行补偿，可将漂浮基座下复杂的机械臂与卫星含有动力学的耦合运动规划，转化为简单的机械臂运动学规划，通过机械臂本身的逆运动学计算，即可实现漂浮基座下机械臂的运动规划。

本发明的优点是避免建立复杂的空间机器人漂浮基座下的动力学模型，避免求解复杂的广义雅克比矩阵及其求逆计算，减小了计算量，便于在工程中实现漂浮基座下空间机器人的任务操作；由于考虑了机械臂与卫星基座的耦合运动，在同样的控制律下可以使空间机械臂更快地到达目标点，有利于空间机器人快速规划，可用于空间机器人的在轨维护及操作。

附图说明

图1为空间机器人系统各坐标系示意图；

图2为基于基座卫星速度的空间机器人运动补偿示意图；

图3为基于基座卫星速度的空间机器人运动补偿流程图；

图4为空间机械臂经补偿后从当前点到目标点的末端轨迹规划曲线，图4中：图4a为空间机器人目标位置距机械臂末端的距离随时间变化曲线，图4b为空间机器人目标姿态相对于空间机器人末端姿态欧拉角随时间变化曲线。

图5为空间机械臂未经补偿从当前点到目标点的末端轨迹规划曲线，图5中：图5a为空间机器人目标位置距机械臂末端的距离随时间变化曲线，图5b为空间机器人目标姿态相对于空间机器人末端姿态欧拉角随时间变化曲线。

具体实施方式

具体实施方式一：结合图1、图2和图3说明本实施方式，本实施方式由以下步骤完成：步骤一、根据空间机器人当前末端位姿与目标的位姿，计算空间机器人期望的末端线速度v_e和角速度ω_e；步骤二、利用卫星基座的陀螺获得基座角速度ω₀；步骤三、计算卫星基座的线速度v₀；步骤四、计算基座雅克比矩阵J_b；步骤五、计算空间机械臂雅克比矩阵J_m；步骤六、利用基座速度对空间机器人期望的末端速度在工具坐标下进行补偿，计算空间机械臂期望运动的关节速度

具体实施方式二：本实施方式与具体实施方式一的不同点是：本实施方式在步骤一所述的计算空间机器人期望的末端线速度和角速度的公式为：

其中v_e和ω_e分别为空间机器人期望的末端线速度和角速度，k_v和k_w分别为末端线速度和角速度的调节比例系数，D_e为目标位置到空间机器人末端操作器的距离，η和分别为空间机器人旋转到目标姿态的欧拉转轴和转角，若目标位置相对于空间机器人当前姿态用四元数法表示为Q＝[q₁q₂q₃q₄]^T，则η和的计算公式为：

具体实施方式三：本实施方式与具体实施方式一或二的不同点是：本实施方式在步骤三所述的计算卫星基座的线速度为：在空间环境自由漂浮情况下，空间机器人初始线动量为零，根据空间机器人线动量守恒，可以得到

$P=\left[\begin{array}{cc}ME& M{\tilde{r}}_{0g}^T\end{array}\right]\left[\begin{array}{c}{v}_0\\ {\mathrm{\ω}}_0\end{array}\right]+J_{Tw}\stackrel{\·}{\mathrm{\Θ}}=O---\left(12\right)$

其中P是空间机器人系统线动量，是空间机器人机械臂的关节角速度向量，v₀，ω₀分别为卫星基座的线速度和角速度。M为空间机器人总质量，E为单位阵，r_0g为空间机器人质心到载体卫星质心的距离，是r_0g的协对称矩阵的转置。J_Tw为雅克比矩阵，表示为

$J_{Tw}=\underset{i=1}{\overset{n}{\Σ}}\left(m_i{J}_{Ti}\right)---\left(13\right)$

n为空间机器人机械臂的自由度，m_i为空间机器人机械臂的第i个臂杆质量，J_Ti可以计算为：

J_Ti＝[k₁×(r_i-p₁),k₂×(r_i-p₂),...,k_i×(r_i-p_i),0,...,0](14)

其中k_i为空间机器人第i个关节的转动方向，r_i和p_i分别是空间机器人第i个臂杆质心和末端位置，r_i-p_i为空间机器人第i个臂杆质心到第i个臂杆末端的距离。根据公式(12)，卫星基座的线速度v₀可表示为：

$v_0=\frac{-J_{Tw}\stackrel{\·}{\mathrm{\Θ}}-M{\tilde{r}}_{0g}^T{\mathrm{\ω}}_0}{M}---\left(15\right)$

具体实施方式四：本实施方式与具体实施方式三的不同点是：本实施方式在步骤四所述的计算基座雅克比矩阵J_b为：

$J_b=\left(\begin{array}{cc}E& -{\tilde{p}}_{0e}\\ O& E\end{array}\right)---\left(16\right)$

其中p_0e为空间机器人末端位置，是p_0e的斜对称矩阵，E为单位阵。

具体实施方式五：结合图1说明本实施方式，本实施方式与具体实施方式四的不同点是：本实施方式在步骤五所述的计算空间机械臂雅克比矩阵J_m为：

$J_m=\left[\begin{array}{ccccc}{k}_1\×(p_e-p_1)& \mathrm{...}& k_i\×(p_e-p_i)& \mathrm{...}& k_n\×(p_e-p_n)\\ k_1& \mathrm{...}& k_i& \mathrm{...}& k_n\end{array}\right]---\left(17\right)$

其中k_i为空间机器人第i个关节的转动方向，p_e是空间机器人末端位置，p_i是空间机器人第i个臂杆末端位置，p_e-p_i空间机器人末端位置到空间机器人第i个臂杆末端的距离。

具体实施方式六：结合图2说明本实施方式，本实施方式与具体实施方式五的不同点是：本实施方式在步骤六所述的利用基座速度对空间机器人期望的末端速度在工具坐标下进行补偿，计算空间机械臂期望运动的关节速度，其过程为：

漂浮基座下空间机器人，由于机械臂运动与卫星基座间存在耦合，机械臂的运动会导致卫星基座运动，需要用基座卫星速度对空间机械臂的末端速度在工具坐标下补偿，并进行机械臂逆运动学计算，可得空间机械臂期望运动的关节速度。机械臂期望关节速度可用下式计算：

$\stackrel{\·}{\mathrm{\Θ}}={J_m}^{-1}\left(\left[\begin{array}{c}{v}_e\\ {\mathrm{\ω}}_e\end{array}\right]-J_b\left[\begin{array}{c}{v}_0\\ {\mathrm{\ω}}_0\end{array}\right]\right)---\left(18\right)$

通过上述步骤，利用卫星基座陀螺测得的基座角速度及计算的基座线速度，对机械臂末端的期望运动速度在机械臂工具坐标下进行补偿，并进行机械臂逆运动学计算，即可简单、快速求解出补偿后机械臂期望运动的关节速度。

实施例

结合图4和图5说明本实施例，空间机器人由六自由度机械臂和卫星组成，本实施例是在漂浮卫星基座下，使机械臂末端从当前位姿到达目标位姿。机械臂的运动规划是利用基座卫星角速度，在机械臂末端工具坐标下对其运动进行补偿，其步骤为：

步骤一、通过ProE建模计算出空间机器人的运动学和动力学参数，机械臂为六自由度。

空间机器人基座卫星基座参数如表1所示：

表1.空间机器人卫星基座参数

空间机器人机械臂参数如表2所示：

表2.空间机器人机械臂参数

空间机械臂末端距离目标位置的距离为P_e0＝[70mm40mm290mm]^T；目标姿态相对于械臂末端初始姿态的用欧拉角表示为Ψ_e0＝[0.52rad1.57rad2.62rad]^T，空间机器人机械臂的初始关节角为θ₀＝[0°-128°94°0°-210°-15°]^T。

步骤二、基于空间机器人的目标位置和目标姿态，根据公式(1)计算空间机器人期望的末端线速度v_e和角速度ω_e，用于空间机械臂末端运动的补偿。

步骤三、计算基座角速度ω₀。由于实际应用中，空间机器人基座角速度由卫星陀螺提供，本例中基座角速度按照空间机器人角动量守恒进行计算。

步骤四、根据空间机器人基座角速度和空间机器人动力学、运动学参数，利用公式(6)计算卫星基座的线速度v₀，用于空间机械臂的运动补偿。

步骤五、根据空间机器人运动学，按照公式(7)计算基座雅克比矩阵J_b。

步骤六、根据空间机器人运动学，按照公式(8)计算空间机械臂雅克比矩阵J_m。

步骤七、根据前述各步骤的计算结果，利用基座速度对空间机器人期望的末端速度在工具坐标下进行补偿，基于公式(9)计算空间机械臂期望运动的关节速度将所得到的关节速度用于空间机器人机械臂关节控制。

所设计的基于基座卫星角速度的空间机械臂工具坐标下的运动补偿方法的实际效果如图4、图5所示。图4为空间机械臂经补偿后从当前点到目标点的末端轨迹规划曲线，图4a为空间机器人目标位置距机械臂末端的距离随时间变化曲线，图4b为空间机器人目标姿态相对于空间机器人末端姿态欧拉角随时间变化曲线。从图4可以看出，由于空间机械臂的规划在工具坐标下经过运动补偿，考虑了漂浮基座下机械臂运动导致的卫星基座耦合运动，机械臂末端从当前点到目标点位置用了15秒，到达目标姿态用了9秒。图5为空间机械臂未经补偿从当前点到目标点的末端轨迹规划曲线，图5a为空间机器人目标位置距机械臂末端的距离随时间变化曲线，图5b为空间机器人目标姿态相对于空间机器人末端姿态欧拉角随时间变化曲线。从图5中可以看出，由于空间机械臂的规划未考虑机械臂与基座的耦合运动，机械臂末端运动未经过补偿，机械臂末端从当前点到目标点位置用了18秒，到达目标姿态用了15秒。

从图4和图5对比可看出，按照所设计的基于基座卫星角速度的空间机械臂工具坐标下的运动补偿方法，能够使空间机械臂末端平滑地达到目标位置和姿态，完成漂浮基座下空间机械臂的运动规划；并且相比于没有进行运动补偿的空间机器人，由于考虑了基座的耦合运动，在同样的控制律下，到达目标点的收敛时间短，有利于机器人快速规划，使空间机器人更快的到达目标位置。

从实施例可以看出，该方法利用卫星基座陀螺测量的角速度，对机械臂末端的期望运动速度在工具坐标下进行补偿，通过简单的逆运动学计算，即可实现漂浮基座下空间机械臂的运动规划，避免了建立复杂的空间机器人漂浮基座动力学模型，避免求解复杂的广义雅克比矩阵及其求逆计算，减小了计算量；在同样的控制律下，使空间机械臂可以更快地到达目标点，有利于空间机器人快速规划，可以满足工程系统的实际需要。

Claims (5)

1.一种基于基座卫星角速度的空间机械臂工具坐标下的运动补偿方法，其特征在于：所述方法的实现过程为：

步骤一、根据空间机器人当前末端位姿与目标的位姿，计算空间机器人期望的末端线速度v_e和角速度ω_e；

步骤一所述的计算空间机器人期望的末端线速度v_e和角速度ω_e的公式为：

其中v_e和ω_e分别为空间机器人期望的末端线速度和角速度，k_v和k_w分别为末端线速度和角速度的调节比例系数，D_e为目标位置到空间机器人末端操作器的距离，η和分别为空间机器人旋转到目标姿态的欧拉转轴和转角，若目标位置相对于空间机器人当前姿态用四元数法表示为Q＝[q₁q₂q₃q₄]^T，则η和的计算公式为：

步骤二、利用卫星基座的陀螺获得基座角速度ω₀；

步骤三、计算卫星基座的线速度v₀；

步骤四、计算基座雅克比矩阵J_b；

步骤五、计算空间机械臂雅克比矩阵J_m；

步骤六、利用基座速度对空间机器人期望的末端速度在工具坐标下进行补偿，计算空间机械臂期望运动的关节速度

2.根据权利要求1所述的基于基座卫星角速度的空间机械臂工具坐标下的运动补偿方法，其特征在于：

步骤三所述的计算卫星基座的线速度v₀的过程为：在空间环境自由漂浮情况下，空间机器人初始线动量为零，根据空间机器人线动量守恒，可得到

$P=\left[\begin{array}{cc}ME& M{\tilde{r}}_{0g}^T\end{array}\right]\left[\begin{array}{c}{v}_0\\ {\mathrm{\ω}}_0\end{array}\right]+J_{Tw}\stackrel{\·}{\mathrm{\Θ}}=O---\left(3\right)$

其中P是空间机器人系统线动量，是空间机械臂期望运动的关节速度，v₀，ω₀分别为卫星基座的线速度和角速度；M为空间机器人总质量，E为单位阵，r_0g为空间机器人质心到载体卫星质心的距离，是r_0g的斜对称矩阵的转置；J_Tw为雅克比矩阵，表示为

$J_{Tw}=\underset{i=1}{\overset{n}{\Σ}}\left(m_i{J}_{Ti}\right)---\left(4\right)$

n为空间机器人机械臂的自由度，m_i为空间机器人机械臂的第i个臂杆质量，J_Ti利用式(5)计算为：

J_Ti＝[k₁×(r_i-p₁),k₂×(r_i-p₂),...,k_i×(r_i-p_i),0,...,0](5)

其中k_i为空间机器人第i个关节的转动方向，r_i和p_i分别是空间机器人第i个臂杆质心和末端位置，r_i-p_i为空间机器人第i个臂杆质心到第i个臂杆末端的距离；根据公式(3)，卫星基座的线速度v₀表示为：

$v_0=\frac{-J_{Tw}\stackrel{\·}{\mathrm{\Θ}}-M{\tilde{r}}_{0g}^T{\mathrm{\ω}}_0}{M}---\left(6\right).$

3.根据权利要求2所述的基于基座卫星角速度的空间机械臂工具坐标下的运动补偿方法，其特征在于：

步骤四所述的计算基座雅克比矩阵J_b的过程为：

$J_b=\left(\begin{array}{cc}E& -{\tilde{p}}_{0e}\\ O& E\end{array}\right)---\left(7\right)$

其中p_0e为空间机器人末端位置，是p_0e的斜对称矩阵，E为单位阵。

4.根据权利要求3所述的基于基座卫星角速度的空间机械臂工具坐标下的运动补偿方法，其特征在于：

步骤五所述的计算空间机械臂雅克比矩阵J_m的过程为：

$J_m=\left[\begin{array}{ccccc}{k}_1\×(p_e-p_1)& ...& k_i\×(p_e-p_i)& ...& k_n\×(p_e-p_n)\\ k_1& ...& k_i& ...& k_n\end{array}\right]---\left(8\right)$

其中k_i为空间机器人第i个关节的转动方向，p_e是空间机器人末端位置，p_i是空间机器人第i个臂杆末端位置，p_e-p_i空间机器人末端位置到空间机器人第i个臂杆末端的距离。

5.根据权利要求1、2、3或4所述的基于基座卫星角速度的空间机械臂工具坐标下的运动补偿方法，其特征在于：

步骤六所述的利用基座速度对空间机器人期望的末端速度在工具坐标下进行补偿，计算空间机械臂期望运动的关节速度，其过程为：

利用基座卫星速度对空间机械臂的末端速度在工具坐标下补偿，并进行机械臂逆运动学计算，获得空间机械臂期望运动的关节速度；空间机械臂期望运动的关节速度可用下式计算：

$\stackrel{\·}{\mathrm{\Θ}}={J_m}^{-1}\left(\left[\begin{array}{c}{v}_e\\ {\mathrm{\ω}}_e\end{array}\right]-J_b\left[\begin{array}{c}{v}_0\\ {\mathrm{\ω}}_0\end{array}\right]\right)---\left(9\right).$