失控或者非合作的目标,其运动状态无法通过自身的传感器直接获取,需要根据外部设备提供的测量信息进行解算或者估计而间接得到。目前常用的测量设备仅能提供相对位姿信息,无法直接解算出相对速度和加速度信息,此时需要基于已有测量信息采用估计算法得到目标运动状态量的估计值。目标运动预测的思路为:首先根据经验和先验知识建立目标运动的状态模型,设定状态量x(t);然后分别通过测量和状态递推的方式获取运动目标运动状态y(t)和预测模型输出 (t);计算y(t)和 (t)的误差Err,根据误差对估计运动状态进行实时修正,得到当前运动状态估计值后,再根据状态方程对未来时刻的运动状态进行预测,目标运动预测思想如图3所示。

图3

Fig.3

	Figure Option ViewDownloadNew Window
	图3 运动预测示意图Fig.3 Motion prediction schematically

四元素对姿态角的表示范围非常大,不存在奇异问题,算法全局稳定,且在手眼视觉测量下,可以直接根据相对姿态矩阵反求四元数,因此本文采用四元素来描述自旋目标的姿态。

将姿态四元素及目标角速度定义为系统的状态量,即:

X=[ q, ω]^T(6)

定义系统的状态量测量为:

Z= q^T=[ q₁, q₂, q₃, q₄]^T(7)

式中: q=[ q₁, q₂, q₃, q₄]^T为航天器的姿态四元素; ω=[ ω _x, ω _y, ω _z]^T为目标相对星体坐标系的角速度分量。

由航天器动力学方程很容易获得四元素与角速度间的关系:

ω=2

(8)

式中: Q为由四元素分量组成的四维正交旋转矩阵,表达式为:

Q=

(9)

则四元素的导数 可表示为:

= Q ^-1 ω(10)

将式(10)展开为矩阵形式为:

=

(11)

由自旋目标动力学方程(1)可得:

(12)

由式(11)和式(12)可知,系统运动方程存在非线性关系,其方程可表示为:

(13)

式中:δ _k和υ _k分别为扰动噪声和测量噪声,假定均为零均值高斯噪声,方差分别为 R和 Q; f( X _k)由式(11)和式(12)组成;h( X _k)= CX _k,其中:

C=

(14)

根据自旋目标的运动状态和量测方程,采用适当的预测算法可以获得目标相对于测量的下一时刻运动状态量:姿态四元素和角速度。

常用的预测和估计方法有最小二乘法和卡尔曼滤波法。最小二乘法是依据目标点的历史信息,通过预测目标的运动趋势,得到目标的预测值,但其依赖于过去的运动信息,计算中需要较大的存储容量。卡尔曼滤波算法模型简单,并且计算量较小。其在递推的过程中,只需考虑前一个时刻的信息,而不依赖于所有以前的信息,因此能够满足较高的实时性要求。无损卡尔曼滤波通过无损变换,能够将非线性模型展开至泰勒三阶式,且不需将非线性系统线性化,因此可以得到对非线性系统更好的估计结果,是目标非线性系统较为常用的方法。根据自旋目标运动状态方程和量测方程的特点,本文采用无损卡尔曼滤波对自旋目标的运动进行预测,以获取目标的预测速度。3 自旋目标自主捕获路径规划方法

空间机器人动力学建模和控制方面的重要研究成果见文献[12-13],典型的单臂空间机器人由 n自由度机械臂和作为其基座的航天器平台组成,其微分运动学方程可写成:

= Jb + Jm

(15)

式中: v_e、 ω_e分别为基座质心处的线速度和角速度; Θ∈ R ⁿ为关节角( n为机械臂自由度数); J_b为与基座运动相关的雅可比矩阵; J_m为与机械臂运动相关的雅可比矩阵。

利用自由漂浮空间机器人动量守恒方程可得基座速度和角速度与关节角速度之间的关系为:

=- Hbm = Jbm

(16)

式中: J_bm为基座-机械臂雅可比矩阵。

将式(16)代入式(15),得到自由漂浮模式下空间机器人系统的运动学方程为:

= Jb + Jm = Jg

(17)

式中: J_g为空间机器人的广义雅可比矩阵,它是基座姿态、机械臂关节角以及各刚体质量、惯量的函数。

自旋目标简化模型如图4所示,以远地点发动机喷嘴为捕获对象,在远地点发动机内定义一个捕获点,并在捕获点处建立坐标系,作为自主捕获操作的目标坐标系。 t时刻,目标如实线部分所示, t₂时刻,目标如虚线所示。抓捕时,要求机器人末端和抓捕手柄间相对速度和相对位姿误差足够小。因此自旋目标的捕获问题等同于机械臂末端追踪捕获点的问题。当机械臂末端工具坐标系与捕获点坐标系间的误差足够小时,认为目标捕获成功。

图4

Fig.4

	Figure Option ViewDownloadNew Window
	图4 自旋目标运动示意图Fig.4 Movement schematically of spin target

基于上述的估计结果,期望的末端运动速度按式(18)进行规划:

=K +

(18)

式中: v_ed、 ω_ed和 v_h、 ω_h分别为机械臂末端的期望速度、角速度和目标的绝对速度、角速度在坐标系 åE中的表示; K为比例系数; Δ O为姿态指向误差(即捕获点坐标系相对于机械臂末端坐标系的姿态),其计算公式为:

Δ O=( ne ×nh +oe ×oh +ae ×ah)=

(19)

式中: 、 分别为åE和 åH的姿态变换矩阵(相对于惯性系); ^E A _H为通过手眼相机测出的捕获点相对于机械臂末端坐标系的姿态矩阵。

根据相对运动原理,目标的绝对运动速度为:

(20)

式中: ^E 、 ^E ω_h分别为目标星的捕获点相对于机械臂末端的线速度和角速度在末端坐标系中的表示,即:

= +

(21)

根据式(20),目标的运动速度为:

(22)

根据自由漂浮空间机器人的微分运动学方程,有:

=( E Jg)

(23)

式中: ^E J_g为机械臂的末端广义雅可比矩阵; 为机械臂的当前角速度。

将式(23)代入式(22),有:

= + ( E Jg)

(24)

最后,根据式(24)和(18),可按式(25)规划机械臂末端运动速度:

=K + + ( E Jg)

(25)

在实际运用中,一般不允许机械臂末端的运动速度过大,因此,在选择 K时要根据机械臂末端运动约束进行。