采用单目视觉的非合作目标星状态估计

郭新程，孟中杰，黄攀峰

(1. 西北工业大学航天学院智能机器人研究中心，西安 710072；2. 西北工业大学航天飞行动力学技术国家级重点实验室，西安 710072)

0 引 言

航天器相对导航技术可以应用在各种空间任务场景，例如编队飞行、在轨服务、失效卫星和空间碎片清除、彗星和小行星探索等，具有重要的研究意义。根据任务场景和目标合作程度的不同，采用不同的方案和传感器进行相对导航[1]。对于合作目标和模型已知的非合作目标，相对状态估计问题已经得到了很好的解决。然而，空间物体大多属于模型未知的非合作目标，即不能与主动航天器进行信息交互，未安装光学标志器且几何模型未知。对于此类目标，由于缺乏目标的运动、表面结构和惯性参数等信息，相对状态估计问题变得尤为复杂。

实际上，空间未知非合作目标的相对状态估计问题和地面机器人的同时定位与建图(Simult-aneously localization and mapping, SLAM)问题类似，即在估计目标相对位姿的同时，恢复目标的表面结构，对于翻滚运动的目标，还需要估计目标质心，惯量主轴等状态。对此，国内外学者主要采用光学敏感器作为测量传感器并设计相应的估计方法对非合作目标的相对状态进行估计。采用的光学敏感器主要包括主动式的激光雷达和被动式的相机及由多个相机组成的立体视觉测量系统等。文献[2]利用一组协同工作的三维传感器(假设这些传感器均匀分布在目标周围)来估计目标的相对位姿和表面结构。文献[3-5]等利用立体视觉，并设计相应的滤波或者平滑算法对非合作目标相对状态进行估计。但是应该注意到平台卫星对传感器的限制，一方面要考虑平台星的载荷、电气和计算能力，另一方面也要考虑卫星系统的设计和开发成本。尤其是针对小卫星，如立方星参与的在轨服务任务，单目相机无疑是具有很大优势。和激光雷达相比，单目相机结构简单，质量小，功耗和经济成本低；和立体视觉系统相比，单目相机工作范围更远，并且不受搭载平台大小的限制。实际上，在目前的立方星等微小卫星平台上，受体积、质量、功耗等限制，仅适合搭载单目相机。

当然，使用单目相机进行目标星状态估计也存在一些缺点，例如：被动式的测量对光照条件敏感；无法直接获得关于目标三维测量等。特别是后者限制了单目相机的应用，因此一些研究通过引入额外信息来解决。文献[6]将单目相机和距离传感器获得的测量数据融合，通过UKF估计目标状态；文献[7]虽然只使用了单目相机，但是假设了目标的几何模型是已知的。对于仅用单目相机对未知非合作目标进行相对位姿估计的文献较少。文献[8]提出了一种纯单目视觉SLAM算法，使用特征点的图像投影作为观测，并设计滤波算法进行相对状态的估计。但是该文献假设目标以恒定角速度旋转，因此无法估计目标的转动惯量信息。文献[9]使用特征点视线方向作为观测量，通过EKF算法估计出目标的状态。为了说明滤波器对位置相关状态的可观性，一方面使用基于Lie导数的可观性秩条件法进行分析；另一方面，采用文献[10]的方法，在假设目标姿态相关状态已知的条件下，证明了相机偏置安装时位置相关状态是可观的。此外，对于自由翻滚运动的目标，还需要估计目标的转动惯量。考虑到在缺乏激励的条件下无法获得转动惯量的绝对值，通常的处理方式是将转动惯量的比值作为参数一起进行估计。文献[5,9,11]采用了不同的转动惯量参数化方式。

利用直接估计[12]的方法(即以目标表面特征点的投影作为观测量，设计滤波算法估计目标相对状态)可以很好地处理非合作未知目标的状态估计问题，文献[3-4,9,12]都是利用这种方法。直接估计的方法依赖于对目标特征的跟踪能力，并且由于待估计状态包含每个特征的视线或三维位置矢量，因此计算量随着特征点数量的增加而增加。而对于单目相机而言，很难直接得到未知非合作目标的相对位置和姿态，因此使用直接估计的方式就成了首选。对此，本文针对立方星等小卫星逼近观测非合作目标星任务，使用单目相机，以目标特征点的投影像素位置作为观测量，通过EKF估计未知非合作目标的相对位置、相对线速度、相对姿态、角速度、转动惯量比和特征点位置。最后，以简化的ENVISAT卫星模型为目标，通过数值仿真校验所提出估计方法的有效性。

1 追踪星/目标星相对运动模型

1.1 坐标系定义

为建立追踪星与目标星之间的相对运动模型，首先定义以下坐标系。

地心惯性坐标系I：原点位于地球质心，z轴指向地球北极，x轴指向春分点，y轴由右手法则确定。

LVLH坐标系H：坐标系原点位于追踪星质心，x轴由地心指向追踪星质心，z轴垂直于轨道面且与轨道角动量方向一致，y轴由右手法则确定。

追踪星本体坐标系A：坐标原点固定在追踪星质心上，坐标轴与追踪星惯量主轴重合。

目标本体坐标系B：坐标原点固定在目标质心上，坐标轴与目标的惯量主轴重合，规定最小惯量主轴为x轴，最大惯量主轴为z轴。

相机坐标系C：坐标原点固定在相机光心上，z轴沿相机光轴方向，x轴y轴平行于成像平面。

为简化模型，假设LVLH坐标系和追踪星本体坐标系重合，相机坐标系和追踪星本体坐标系各坐标轴平行。

1.2 相对位置运动模型

(1)

(2)

(3)

(4)

1.3 相对姿态运动模型

与四元数q对应的旋转矩阵：

(5)

(6)

定义四元数运算：

(7)

定义追踪星和目标星的角速度在各自本体坐标系下的表示分别为ωc,ωt，将两者的相对速度表示在目标本体系下可得：

(8)

相对旋转的运动学方程为：

(9)

相对旋转的动力学方程为：

(10)

式中：Jc,Jt分别为追踪星和目标的转动惯量矩阵，Mc,Mt分别为追踪星和目标所受的外力矩。

假设目标做自由翻滚运动，则目标所受外力矩Mt=0。一方面使用式(10)估计相对旋转角速度比较复杂;另一方面，假设追踪星角速度是可以获得的。因此，本文使用目标星的姿态动力学方程进行估计。目标的姿态动力学方程为：

(11)

由于目标的转动惯量矩阵未知，因此需要对转动惯量进行参数化处理。采用类似于文献[5]的参数化方式，定义转动惯量比：

(12)

式中：k1=-ln(Jxx/Jzz),k2=-ln(Jyy/Jzz)，代入式(11)得：

(13)

式中：

考虑到目标星为刚体，短期内质量及其分布不发生变化，其转动惯量比为常量，应满足：

(14)

2 目标星观测方程

如图1所示，在追踪星上安装有固定的单目相机，单目相机在追踪星本体坐标系下的位置坐标为d=[dx,dy,dz]T。相机成像采用小孔成像模型，不考虑畸变，单目相机内参矩阵为：

(15)

(16)

(17)

(18)

将式(17)代入式(18)即可得到观测方程，其形式为：

(19)

3 基于单目视觉的相对状态估计

在本文第1、第2节中分别描述了非合作目标星与追踪星的相对运动方程和状态观测方程，本节设计了通过EKF估计目标相对姿态、角速度、转动惯量比、相对质心位置、相对质心速度及特征点位置的方法。

状态选择为：

(20)

(21)

根据式(3)、(9)、(13)、(14)和(21)可得系统状态方程：

(22)

式(22)描述的是连续非线性系统，需要先转换为离散模型，之后再按照标准的扩展卡尔曼滤波公式进行估计。

推导离散模型为：

X(k+1)=X(k)+f(X(k))Δt+

(23)

由矩阵Ak可以得到系统状态转移矩阵的一阶近似φ(k)=I15+3N+AkΔt, Δt为滤波周期。过程噪声矩阵近似为：

(24)

测量模型的雅克比矩阵可以通过对观测方程式(19)求导得到，

(25)

该雅克比矩阵为2N×(15+3N)维，具体形式参见文献[13]。

使用扩展卡尔曼滤波对式(23)和式(19)所表示的过程模型和测量模型进行状态估计。注意由于直接用了四元数进行姿态估计，因此姿态四元数每一步预测和更新后都需要单位化处理。

具体算法流程如图2所示。详细的相对状态估计流程为：

步骤3.获得观测Zk后根据观测模型和状态预测值及预测误差方差阵计算观测雅克比矩阵Hk、增益矩阵Kk和预测残差rk。

4 仿真校验

本节通过非合作目标星与追踪星相对运动的动力学模型模拟两航天器的相对运动轨迹。以目标星表面的固有特征为观测特征点，根据特征点的运动和小孔成像模型产生特征点的图像投影像素坐标，并加入测量噪声，作为系统观测输入。

以失效的近地轨道地球观测卫星Envisat为目标星，其轨道半长轴a=7143 km，真近点角速率约为n=0.001 rad/s。在本文仿真中，为便于选择特征点，将Envisat卫星主体部分简化为一个12 m×4 m×4 m的长方体，其余部分如太阳能电池板和雷达暂不考虑。目标主惯量矩阵设为Jt=diag(16960,120570,124970)。选择其几何顶点作为观测特征点，特征点在非合作目标上的位置分布如表1所示。

表1 特征点的位置分布Table 1 The distribution of feature points’ position

仿真时间设计为200 s，仿真步长为0.1 s，仿真初始条件设置如下：

初始协方差:

相对位置估计误差为：

(26)

相对速度估计误差为：

(27)

相对角速度估计误差为：

(28)

k1,k2估计误差为：

(29)

eθ=2arccos(qe4)

(30)

式中：qe4为姿态误差四元数的标量部分。

目标上特征点的位置估计误差为：

(31)

(32)

仿真结果如图3～图9所示。

图3、图4分别为目标相对姿态和角速度估计值与仿真真值之间的误差曲线。图3中，将姿态误差四元数转换成轴角后估计误差在2°以内。图4中，目标角速度估计误差在0.3°/s以内。图5、图6分别为目标质心相对位置和相对速度的误差变化曲线。相对位置估计误差70 s后收敛在0.1 m以内,120 s后收敛在0.05 m附近,相对速度估计误差30 s 后收敛在0.0005 m/s以内。结果表明了本方法对目标星相对位姿估计的有效性。

图7、图8分别为目标转动惯量比k1，k2的误差曲线，最终稳定时k1误差上界为0.005，k2误差上界为0.001，这说明本方法可以很好地估计出目标星的转动惯量比。

图9为所有特征点在目标本体系上的位置估计误差曲线，100 s后误差曲线稳定在0.25 m左右，特征点平均估计误差小于0.04 m，说明了本方法可以很好地估计目标表面特征点位置。

综上可知，本文设计的基于单目视觉的目标星状态估计方法能够有效地实现对未知非合作目标星相对状态估计，估计方法收敛时间较短，估计精度较高，满足近距离相对导航任务的要求。

5 结 论

针对空间未知非合作目标星的相对状态估计问题，本文以微小卫星搭载单目相机，设计了一种基于单目视觉的目标星状态估计方法，以目标星的固有特征成像为输入，设计了一种基于EKF的目标星相对姿态、角速度、两自由度转动惯量比、相对质心位置、相对线速度和特征点位置的估计方法，并通过数值仿真校验了该方法的有效性。但是，值得注意的是，目标星固有特征的提取和跟踪是本文的状态估计方法的前提，空间光照条件的变化会影响相机成像精度，从而使得观测噪声统计特性发生变化，因此有必要在下一步工作中研究适合不同统计特性噪声的自适应滤波算法，进一步提高状态估计精度。