故障航天器在轨维修、失效卫星拯救、废弃卫星减缓回收等已成为航天技术发展面对和待解决的现实问题，无人空间机器人是解决这些问题的关键技术之一^[1-3]。由于故障航天器一般不具备专门的合作机构，且往往处于自旋或翻滚状态，机械臂在轨空间操作往往涉及与自由漂浮目标的物理接触，抓捕风险很大。对于这类非合作的故障航天器，在抓捕前精确获取其运动状态、形状和惯量比等信息，是保证目标被成功捕获的重要前提^[4-6]。

对于空间失控目标，目前主要采用非接触式的测量技术获取目标相对服务航天器的相对位姿信息。Terui，Kamimura和Nishida利用立体视觉测量目标点云数据，并采用基于已知目标三维模型的迭代最近点(Iterative closest point，ICP)算法获取相对位姿估计^[7]。Neptec公司和加拿大太空局早期开发了激光相机系统(Laser camera system，LCS)系统^[8-10]，但由于其近距分辨率较低，后期在此基础上，结合了三角测量和飞行时间原理，开发了能够同时兼顾高精度近距离和远距离观测要求的三角测量雷达传感器(Triangulation-LIDAR sensor，TriDAR)系统^[11], 以测量系统获得的相对位姿作为观测输入，通常采用卡尔曼滤波原理估计目标的线速度、角速度、惯量比等参数。Thienel和Queen等人研究了一种非线性滤波估计方法，以满足哈伯望远镜机械臂在轨服务的测量要求^[12]。Lichter和Dubowsky研究了基于序列图像的动态目标运动状态、几何形状和惯性参数的估计方法^[13]。Aghili和Parsa研究了翻滚目标运动状态和参数的自适应卡尔曼滤波估计方法^[14]。Zhang等人研究了翻滚航天器的相对姿态和位置估计方法^[15]。Yuan等人研究了基于常值状态滤波器的翻滚航天器惯性估计和姿态预测方法^[16]。Shtark和Gurfil研究了基于立体视觉的非合作目标相对位置和速度的测量估计方法^[17]。Pesce，Lavagna和Bevilacqua研究了基于立体视觉的非合作目标姿态、运动和惯量比的测量估计方法^[18]。

针对失效航天器的非接触式姿态运动感知与估计问题，本文研究了基于序列图像的非合作目标的运动测量与状态估计方法。首先，由于故障航天器姿态失效后，主要是绕其最大惯量轴的自旋或章动运动，给出了基于目标3个非共线特征点的立体视觉相对姿态测量方法；其次，推导了基于姿态四元素的扩展卡尔曼滤波估计方法，实现了目标角速度和惯量比的估计；再次，利用数学仿真验证了所给出的卡尔曼滤波估计算法在不同目标运动情况下的收敛性；最后，利用机械臂模拟目标的自旋运动，利用双目相机采集目标运动图像，试验结果验证了所给出的运动测量与状态估计方法可以连续的给出目标角速度的精确估计。

1 基于立体视觉的目标运动测量与状态估计

立体视觉测量系统由安装在服务航天器上的两部相机组成，设相机平行放置，构成平行式双目立体视觉模型，如图 1所示。左相机光心处为相机坐标系o_cx_cy_cz_c，右相机位于左相机x轴上，基线长度为b，两台相机内参一致，焦距为f，像元大小为μ，主点为u₀和v₀。对于目标航天器上的某一特征点P₁，其在左相机图像像素坐标系o_lu_lv_l和右相机图像像素坐标系o_ru_rv_r的投影点为p_l¹和p_r¹。那么，根据透视投影的几何原理，得第i个特征点P_i在相机参考系中的坐标为

$ \begin{array}{*{20}{c}} {{P_{ix}} = \frac{{{P_{iz}}\mu \left( {p_{{\rm{l}}x}^i - {u_0}} \right)}}{f}}\\ {{P_{iy}} = \frac{{{P_{iz}}\mu \left( {p_{{\rm{l}}y}^i - {v_0}} \right)}}{f}}\\ {{P_{iz}} = \frac{{fb}}{{\mu \left( {p_{{\rm{r}}x}^i - p_{{\rm{l}}x}^i} \right)}}} \end{array} $

(1)

当同时观测到目标上的3个非共线的特征点P₁，P₂，P₃时，可以建立目标坐标系。定义目标坐标系的原点o_T为P₁，x_T轴方向为矢量$ \overrightarrow {{\mathit{\boldsymbol{P}}_1}{\mathit{\boldsymbol{P}}_2}} $，其单位矢量为

$ {\mathit{\boldsymbol{r}}_x} = \frac{{\overrightarrow {{\mathit{\boldsymbol{P}}_1}{\mathit{\boldsymbol{P}}_2}} }}{{\left\| {\overrightarrow {{\mathit{\boldsymbol{P}}_1}{\mathit{\boldsymbol{P}}_2}} } \right\|}} $

(2)

定义z_T轴为矢量$\overrightarrow {{\mathit{\boldsymbol{P}}_1}{\mathit{\boldsymbol{P}}_2}} $和矢量$ \overrightarrow {{\mathit{\boldsymbol{P}}_1}{\mathit{\boldsymbol{P}}_3}} $所在平面的法线方向，其单位矢量为

$ {\mathit{\boldsymbol{r}}_z} = \frac{{\overrightarrow {{\mathit{\boldsymbol{P}}_1}{\mathit{\boldsymbol{P}}_2}} \times \overrightarrow {{\mathit{\boldsymbol{P}}_1}{\mathit{\boldsymbol{P}}_3}} }}{{\left\| {\overrightarrow {{\mathit{\boldsymbol{P}}_1}{\mathit{\boldsymbol{P}}_2}} \times \overrightarrow {{\mathit{\boldsymbol{P}}_1}{\mathit{\boldsymbol{P}}_3}} } \right\|}} $

(3)

根据右手定则确定y_T轴单位矢量为

$ {\mathit{\boldsymbol{r}}_y} = {\mathit{\boldsymbol{r}}_z} \times {\mathit{\boldsymbol{r}}_x} $

(4)

那么，相机坐标系o_cx_vy_cz_c到目标坐标系o_Tx_Ty_Tz_T的姿态变换矩阵为

$ \mathit{\boldsymbol{R}} = {\left[ {\begin{array}{*{20}{c}} {{\mathit{\boldsymbol{r}}_x}}&{{\mathit{\boldsymbol{r}}_y}}&{{\mathit{\boldsymbol{r}}_z}} \end{array}} \right]^{\rm{T}}} $

(5)

对于翻滚航天器，为避免奇异性，通常采用四元素描述航天器姿态，定义由坐标系A到坐标系B的四元素为q，其由转轴的单位矢量e和绕此轴的转角ϕ组成

$ \mathit{\boldsymbol{q}} = \left[ \begin{array}{l} \mathit{\boldsymbol{e}}\sin \frac{\varphi }{2}\\ \cos \frac{\varphi }{2} \end{array} \right] = \left[ {\begin{array}{*{20}{c}} {{\mathit{\boldsymbol{q}}_v}}\\ {{q_0}} \end{array}} \right] $

(6)

式中，下标v和0分别代表四元素的矢量和标量部分。

由四元素描述的由坐标系A到坐标系B的转换矩阵为

$ \mathit{\boldsymbol{R}}\left( \mathit{\boldsymbol{q}} \right) = \left( {q_0^2 - \mathit{\boldsymbol{q}}_v^{\rm{T}}{\mathit{\boldsymbol{q}}_v}} \right){\mathit{\boldsymbol{I}}_3} + 2{\mathit{\boldsymbol{q}}_v}\mathit{\boldsymbol{q}}_v^{\rm{T}} - 2{q_0}\mathit{\boldsymbol{S}}\left( {{\mathit{\boldsymbol{q}}_v}} \right) $

(7)

式中，I₃为3×3单位矩阵，S(a)为斜对称矩阵

$ \mathit{\boldsymbol{S}}\left( a \right) = \left[ {\begin{array}{*{20}{c}} 0&{ - {a_3}}&{{a_2}}\\ {{a_3}}&0&{ - {a_1}}\\ { - {a_2}}&{{a_1}}&0 \end{array}} \right] $

(8)

定义四元素乘积算子⊗为

$ \left[ {\mathit{\boldsymbol{q}} \otimes } \right] = \left[ {\begin{array}{*{20}{c}} { - \mathit{\boldsymbol{S}}\left( {{\mathit{\boldsymbol{q}}_v}} \right) + {q_0}{I_3}}&{{\mathit{\boldsymbol{q}}_v}}\\ { - \mathit{\boldsymbol{q}}_v^{\rm{T}}}&{{q_0}} \end{array}} \right] $

(9)

四元素的逆为q^－1=[－q_v^T  q₀]^T，满足

$ \mathit{\boldsymbol{q}} \otimes {\mathit{\boldsymbol{q}}^{ - 1}} = {\mathit{\boldsymbol{q}}^{ - 1}} \otimes \mathit{\boldsymbol{q}} = {\left[ {\begin{array}{*{20}{c}} 0&0&0&1 \end{array}} \right]^{\rm{T}}} $

(10)

对于机动前四元素q₁，机动四元素q′和机动后四元素q₂，关系如下

$ \begin{array}{l} {\mathit{\boldsymbol{q}}_2} = \mathit{\boldsymbol{q'}} \otimes {\mathit{\boldsymbol{q}}_1}\\ \mathit{\boldsymbol{q'}} = {\mathit{\boldsymbol{q}}_2} \otimes \mathit{\boldsymbol{q}}_1^{ - 1} \end{array} $

(11)

目标航天器的姿态运动学方程为

$ \mathit{\boldsymbol{\dot q}} = \frac{1}{2}\mathit{\boldsymbol{\bar \omega }} \otimes \mathit{\boldsymbol{q}} $

(12)

式中，$ \mathit{\boldsymbol{\overline \omega }} = {\left[ {{\mathit{\boldsymbol{\omega }}^{\rm{T}}}\;\;0} \right]^{\rm{T}}}$。令目标航天器的主惯量为I_xx、I_yy、I_zz，定义惯量比

$ {l_x} = \frac{{{I_{yy}} - {I_{zz}}}}{{{I_{xx}}}},{l_y} = \frac{{{I_{zz}} - {I_{xx}}}}{{{I_{yy}}}},{l_z} = \frac{{{I_{xx}} - {I_{yy}}}}{{{I_{zz}}}} $

(13)

根据欧拉方程，目标航天器自由运动的姿态动力学方程为

$ \mathit{\boldsymbol{\dot \omega = }}\psi \left( \mathit{\boldsymbol{\omega }} \right) = \left[ {\begin{array}{*{20}{c}} {{l_x}{\omega _y}{\omega _z}}\\ {{l_y}{\omega _x}{\omega _z}}\\ {{l_z}{\omega _x}{\omega _y}} \end{array}} \right] $

(14)

定义误差四元素为

$ \delta \mathit{\boldsymbol{q}} = \mathit{\boldsymbol{q}} \otimes {{\mathit{\boldsymbol{\hat q}}}^{ - 1}} $

(15)

式中，上标“^ ”代表估计值。线性化方程(12)和方程(14)，得

$ \delta {{\mathit{\boldsymbol{\dot q}}}_v} \approx - \mathit{\boldsymbol{\omega }} \times \delta {\mathit{\boldsymbol{q}}_v} + \frac{1}{2}\delta \mathit{\boldsymbol{\omega }} $

(16)

$ \begin{array}{l} \delta {{\mathit{\boldsymbol{\dot q}}}_0} \approx 0\\ \delta \mathit{\boldsymbol{\dot \omega }} = \mathit{\boldsymbol{M}}\left( {\mathit{\boldsymbol{\hat \omega }},\mathit{\boldsymbol{\hat l}}} \right)\delta \mathit{\boldsymbol{\omega }} + \mathit{\boldsymbol{N}}\left( {\mathit{\boldsymbol{\hat \omega }}} \right)\delta \mathit{\boldsymbol{l}} \end{array} $

(17)

式中$ \mathit{\boldsymbol{l = }}{\left[ {{l_x}\;\;\;{l_y}\;\;\;{l_z}} \right]^{\rm{T}}}$

$ \mathit{\boldsymbol{M}}\left( {\mathit{\boldsymbol{\omega }},\mathit{\boldsymbol{l}}} \right) = \frac{{\partial \mathit{\boldsymbol{\psi }}}}{{\partial \mathit{\boldsymbol{\omega }}}} = \left[ {\begin{array}{*{20}{c}} 0&{{l_x}{\omega _z}}&{{l_x}{\omega _y}}\\ {{l_y}{\omega _z}}&0&{{l_y}{\omega _x}}\\ {{l_z}{\omega _y}}&{{l_z}{\omega _x}}&0 \end{array}} \right] $

(18)

$ \mathit{\boldsymbol{N}}\left( \mathit{\boldsymbol{\omega }} \right) = \frac{{\partial \mathit{\boldsymbol{\psi }}}}{{\partial \mathit{\boldsymbol{l}}}} = \left[ {\begin{array}{*{20}{c}} {{\omega _y}{\omega _z}}&0&0\\ 0&{{\omega _x}{\omega _z}}&0\\ 0&0&{{\omega _x}{\omega _y}} \end{array}} \right] $

(19)

由方程(5)，可求解目标航天器相对于服务航天器的姿态四元素q_r为

$ {\mathit{\boldsymbol{q}}_{{\rm{r}}v}} = \frac{1}{{4{\mathit{\boldsymbol{q}}_{{\rm{r}}0}}}}\left[ {\begin{array}{*{20}{c}} {\mathit{\boldsymbol{R}}\left( {2,3} \right) - \mathit{\boldsymbol{R}}\left( {3,2} \right)}\\ {\mathit{\boldsymbol{R}}\left( {3,1} \right) - \mathit{\boldsymbol{R}}\left( {1,3} \right)}\\ {\mathit{\boldsymbol{R}}\left( {1,2} \right) - \mathit{\boldsymbol{R}}\left( {2,1} \right)} \end{array}} \right] $

(20)

$ {\mathit{\boldsymbol{q}}_{{\rm{r}}0}} = \frac{1}{2}{\left( {{\rm{tr}}\left( \mathit{\boldsymbol{R}} \right) + 1} \right)^{\frac{1}{2}}} $

式中，tr(R)为矩阵R的迹。已知服务航天器相对于惯性系的姿态四元素q_s，可得目标航天器相对于惯性系的姿态四元素观测值q_m为

$ {\mathit{\boldsymbol{q}}_m} = {{\mathit{\boldsymbol{q'}}}_{\rm{r}}} \otimes {\mathit{\boldsymbol{q}}_s} $

(21)

定义扩展卡尔曼估计器的状态向量为

$ \mathit{\boldsymbol{x}} = {\left[ {\begin{array}{*{20}{c}} {\mathit{\boldsymbol{q}}_v^{\rm{T}}}&{{\mathit{\boldsymbol{\omega }}^{\rm{T}}}}&{{\mathit{\boldsymbol{l}}^{\rm{T}}}} \end{array}} \right]^{\rm{T}}} $

(22)

则线性化系统的离散化的状态方程和观测方程为

$ \delta {\mathit{\boldsymbol{x}}_{k + 1}} = {\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_k}\delta {\mathit{\boldsymbol{x}}_k} + {\mathit{\boldsymbol{\varepsilon }}_k} $

(23)

$ {\mathit{\boldsymbol{z}}_k} = \mathit{\boldsymbol{H}}\delta {\mathit{\boldsymbol{x}}_k} + {\mathit{\boldsymbol{\upsilon }}_k} $

(24)

式中，ε_k为过程噪声，υ_k为观测噪声，Φ_k为状态转移矩阵，H为观测矩阵。ε_k和υ_k是均值为零、方差阵各为Q和R的不相关白噪声。

$ {\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_k} = {{\rm{e}}^{\mathit{\boldsymbol{A}}\Delta T}} \approx {\mathit{\boldsymbol{I}}_9} + \mathit{\boldsymbol{A}}\Delta T $

(25)

$ \mathit{\boldsymbol{A}} = \left[ {\begin{array}{*{20}{c}} { - \mathit{\boldsymbol{S}}\left( {\mathit{\boldsymbol{\hat \omega }}} \right)}&{\frac{1}{2}{\mathit{\boldsymbol{I}}_3}}&{{{\bf{0}}_{3 \times 3}}}\\ {{{\bf{0}}_{3 \times 3}}}&{\mathit{\boldsymbol{M}}\left( {\mathit{\boldsymbol{\hat \omega }},\mathit{\boldsymbol{\hat l}}} \right)}&{\mathit{\boldsymbol{N}}\left( {\mathit{\boldsymbol{\hat \omega }}} \right)}\\ {{{\bf{0}}_{3 \times 3}}}&{{{\bf{0}}_{3 \times 3}}}&{{{\bf{0}}_{3 \times 3}}} \end{array}} \right] $

(26)

$ \mathit{\boldsymbol{H}} = \left[ {\begin{array}{*{20}{c}} {{\mathit{\boldsymbol{I}}_3}}&{{{\bf{0}}_{3 \times 6}}} \end{array}} \right] $

(27)

式中，ΔT为离散化时间间隔。

基于目标航天器姿态运动学方程(12)和动力学方程(14)，以及所推导的离散系统模型(23), (24)，根据扩展卡尔曼滤波理论，建立扩展卡尔曼估计器的步骤如下:

(1) 初始化

$ {{\mathit{\boldsymbol{\hat x}}}_0}\left( + \right) = \mathit{\boldsymbol{x}}\left( {{t_0}} \right),{\mathit{\boldsymbol{P}}_0}\left( + \right) = \mathit{\boldsymbol{P}}\left( {{t_0}} \right) $

(28)

(2) 状态一步预测

$ {{\mathit{\boldsymbol{\hat q}}}_k}\left( - \right) = {{\mathit{\boldsymbol{\hat q}}}_{k - 1}}\left( + \right) + \frac{1}{2}{{\mathit{\boldsymbol{\hat \omega }}}_{k - 1}}\left( + \right) \otimes {{\mathit{\boldsymbol{\hat q}}}_{k - 1}}\left( + \right)\Delta T $

(29)

$ {{\mathit{\boldsymbol{\hat q}}}_k}\left( - \right) = \frac{{{{\mathit{\boldsymbol{\hat q}}}_k}\left( - \right)}}{{\left\| {{{\mathit{\boldsymbol{\hat q}}}_k}\left( - \right)} \right\|}} $

(30)

$ {{\mathit{\boldsymbol{\hat \omega }}}_k}\left( - \right) = {{\mathit{\boldsymbol{\hat \omega }}}_{k - 1}}\left( + \right) + \mathit{\boldsymbol{\psi }}\left( {{{\mathit{\boldsymbol{\hat \omega }}}_{k - 1}}\left( + \right)} \right)\Delta T $

(31)

(3) 协方差矩阵预测

$ {\mathit{\boldsymbol{P}}_k}\left( - \right) = {\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_{k - 1}}{\mathit{\boldsymbol{P}}_{k - 1}}\left( + \right)\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_{k - 1}^{\rm{T}} + \mathit{\boldsymbol{Q}} $

(32)

(4) Kalman滤波增益

$ {K_k} = {\mathit{\boldsymbol{P}}_k}\left( - \right)\mathit{\boldsymbol{H}}_k^{\rm{T}}{\left( {{\mathit{\boldsymbol{H}}_k}{\mathit{\boldsymbol{P}}_k}\left( - \right)\mathit{\boldsymbol{H}}_k^{\rm{T}} + R} \right)^{ - 1}} $

(33)

(5) 状态更新

$ \begin{array}{l} \delta {{\mathit{\boldsymbol{\hat x}}}_k}\left( + \right) = {\mathit{\boldsymbol{K}}_k}\left( {{{\mathit{\boldsymbol{\hat q}}}_{k0}}\left( - \right){\mathit{\boldsymbol{q}}_{mv}} - {\mathit{\boldsymbol{q}}_{m0}}{{\mathit{\boldsymbol{\hat q}}}_{kv}}\left( - \right) - } \right.\\ \;\;\;\;\;\;\;\;\left. {{{\mathit{\boldsymbol{\hat q}}}_{kv}}\left( - \right) \times {\mathit{\boldsymbol{q}}_{mv}}} \right) \end{array} $

(34)

$ \delta {{\mathit{\boldsymbol{\hat q}}}_k}\left( + \right) = {\left[ {\delta \mathit{\boldsymbol{\hat q}}_{kv}^{\rm{T}}\left( + \right),\sqrt {1 - \delta \mathit{\boldsymbol{\hat q}}_{kv}^{\rm{T}}\left( + \right)\delta {{\mathit{\boldsymbol{\hat q}}}_{kv}}\left( + \right)} } \right]^{\rm{T}}} $

(35)

$ {{\mathit{\boldsymbol{\hat q}}}_k}\left( + \right) = \delta {{\mathit{\boldsymbol{\hat q}}}_k}\left( + \right) \otimes {{\mathit{\boldsymbol{\hat q}}}_k}\left( - \right) $

(36)

$ {{\mathit{\boldsymbol{\hat \omega }}}_k}\left( + \right) = {{\mathit{\boldsymbol{\hat \omega }}}_k}\left( - \right) + \delta {{\mathit{\boldsymbol{\hat \omega }}}_k}\left( + \right) $

(37)

$ {{\mathit{\boldsymbol{\hat l}}}_k}\left( + \right) = {{\mathit{\boldsymbol{\hat l}}}_k}\left( - \right) + \delta {{\mathit{\boldsymbol{\hat l}}}_k}\left( + \right) $

(38)

2 仿真验证

采用数学仿真验证卡尔曼滤波估计算法的收敛性。假设卫星主惯量为I_xx=10 300 kg·m², I_yy=5 390 kg·m², I_zz=9 190 kg·m²，对应的惯量比为l_x=-0.368 9，l_y=-0.205 9，l_z=0.534 3。假设目标角速度量测误差为0.01 rad，采样周期为0.1 s。目标初始姿态为q₀=[0 0 0 1]^T。目标初始姿态估计值为$ \mathit{\boldsymbol{\hat q}}\left( {{t_0}} \right) = {\left[ {0\;0\;0\;1} \right]^{\rm{T}}}$，初始角速度估计值为$ \mathit{\boldsymbol{\hat \omega }}\left( {{t_0}} \right) = {\left[ {0\;0\;0} \right]^{\rm{T}}}$，初始惯量比估计值为$ \mathit{\boldsymbol{\hat l}}\left( {{t_0}} \right) = {\left[ {0.01\;0.02\;0.05} \right]^{\rm{T}}}$。

设置目标初始角速度依次为ω₀=[5 5 5]^T °/s，ω₀=[20 5 5]^T°/s，ω₀=[30 1 1]^T°/s，即目标分别处于翻滚状态、章动状态和近似自旋状态，仿真结果如图 2—4所示。可以看出:(1)由于目标角速度的可观性较好，在3种工况中，其收敛性和估计精度一致性较好；(2)由于目标惯量比在动力学方程中为常值，其可观性受到目标运动角速度的影响，在3种工况中，其估计精度较好，但收敛速度受目标初始角速度的影响较大。随着x轴自旋角速度的提高，x轴的惯量比l_x的收敛速度逐渐变慢，y轴和z轴的惯量比l_y和l_z的收敛速度逐渐变快，其原因可以从方程(14)中得到，随着自旋角速度的增加，相比较而言，章动角速度ω_y和ω_z变小，l_xω_yω_z变为小量，l_x对模型的贡献值变小，当章动角速度l_y和l_z为零时，方程退化为单轴自旋运动，对惯量比的估计失去意义；(3)此滤波方法基于目标三轴转动运动方程推导得到，对惯量比的估计主要适用于三轴翻滚运动或具有一定章动效应的自旋运动。

3 试验验证

搭建地面试验系统，用于验证立体视觉相对姿态测量算法和目标滤波估计算法的有效性。试验系统包括双目相机、UR5机械臂和一个模拟目标。仿真工况如表 1所示，利用机械臂关节5以近似10°间隔将目标放置在不同的自旋轴方向，利用机械臂关节6驱动目标以60 °/s的角速度进行逆时针自旋运动，采用双目相机以33帧/s的帧频对目标进行图像采集。

以工况1为例，采用图像处理方法，完成目标特征点的提取，具体步骤为：(1)目标图像采集；(2)图像二值化和连通域搜索；(3)边缘检测；(4)直线检测；(5)直线交点计算。经过图像处理后获取目标观测面的四边形的4个顶点，如图 5所示。进一步，利用式(2)—(4)，计算得到相机系到目标系的姿态变换矩阵，最终获得对应的姿态四元素，如图 6(a)所示。以测量得到的目标相对姿态四元素作为滤波估计算法的观测量，采用所给出的目标滤波估计算法进行运动估计，计算结果如图 6(b, c)所示。可以看出，经过10 s的收敛时间，目标姿态四元素的估计值与测量值的曲线基本吻合，目标角速度估计收敛值约为[7, -6.5, -59.5] °/s，大小为60.2 °/s，与目标的驱动角速度基本吻合。同时注意到，目标角速度估计值在x轴和y轴收敛到一定的常值，这是由于目标与机械臂的末关节无法做到同轴安装，且观测面不是存平面，因此，以目标观测面4个顶点建立的参考系的z轴与机械臂关节驱动轴存在着一定的夹角误差，也就造成了目标角速度估计在x轴和y轴存在一定的角速度。

最后，3种工况的试验统计结果如表 2所示，可以看出，在目标自旋轴偏转10°和20°条件下，只要能够稳定的提取目标上的特征点，仍然可以稳定地获取目标的自选轴、旋转角速度信息，自选轴方向误差优于2°，角速度误差优于1 °/s，可以较好地满足服务航天器对目标近距离的全局观测需求。

4 结束语

故障航天器运动感知与估计是在轨维修、维护的重要前提。针对自旋或章动目标，本文给出了基于序列图像的非合作目标运动测量与状态估计方法。首先，采用非接触式的立体视觉实现了运动目标的相对姿态测量；其次，以相对姿态四元素作为观测值，采用扩展卡尔曼滤波器实现了目标运动角速度和惯量比参数的估计。最后，给出了数学仿真和物理试验，仿真结果说明了所给出的卡尔曼滤波估计算法具有很好的收敛性，特别是当目标具有一定的章动效应时，可以实现三轴惯量比参数的估计；试验结果验证了所给出的运动测量与状态估计方法可以连续的给出运动目标的相对姿态测量，并经过短暂的收敛时间，给出目标角速度的精确估计。