基于视觉的含章动小天体运动状态估算方法

程楚楚 高林涛 魏承

(哈尔滨工业大学，哈尔滨 150001)

随着人类空间活动日益频繁，在轨航天器不断增多，空间失效航天器以及空间碎片带来的问题引起了各国的广泛重视，它们不但占用了许多空间资源，还威胁着其他航天器的安全。然而，无论是对故障航天器进行维修，还是对太空垃圾进行清理，首先要解决的关键问题即空间目标的位姿测量问题。近些年来，利用视觉进行位姿测量的方法得到了广泛关注和实践。文献[1]提出了使用计算机视觉从二维图像获取物体三维信息的可能性，由此基于计算机视觉的发展涌现出了许多新方法。文献[2]提出了一种新的匹配方法——半稠密匹配法，这种方法解决了稀疏重建信息少和稠密重建点云信息多等问题。文献[3]在开放式图形库(OpenGL)环境下，基于双目视觉系统建立非合作目标位姿测量仿真系统。文献[4]提出了一种基于尺度不变特征变换(Scale-Invariant Feature Transform，SIFT)算法的改进立体匹配方法，该方法较大地提高了匹配准确率和效率。文献[5]提出了基于特征光流信息的非合作目标位姿估计方法，利用光流法构建了光流信息与相对位姿之间的数学模型，并进行了实验仿真。文献[6]提出了一种基于单目图像序列目标重建结果的非合作目标相对位姿测量方法，利用观测到的图像序列计算目标三维坐标，再基于点云数据建立递推深度模型，最后利用卡尔曼滤波算法实现了目标位姿估计，得到了较为精确的位姿结果。文献[7]利用立体序列观测影像实现了非合作目标位姿估计，并通过仿真实验验证该方法的可行性。文献[8]提出了基于单目悬停相机实现空间非合作目标三维重建，将SIFT算法与k-维二叉树(k-d)相结合获取目标位姿信息。目前还可以通过多种传感器实现非合作目标的位姿测量，例如基于扫描式激光雷达测量、无扫描三维激光成像测量[9]、基于多传感器融合的位姿测量方法[10]等。视觉测量技术可同时对目标进行成像监视和位姿测量，并且具有精度高、设备简易、成本低等特点。但目前使用的视觉测量方法具有一定的局限性，为了更好地解决针对小天体的位姿估计问题，由此引入在移动机器人领域中得到广泛应用的同步定位与建图(SLAM)方法，它是指运动物体根据传感器的信息，一边计算自身位置，一边构建环境地图的过程。本文重点研究基于视觉SLAM的相对位姿测量，提出一种旋转小天体相对位姿确定方法。

本文对特征提取后的小天体进行视觉SLAM，对得到的相机轨迹进行空间圆拟合，可确定小天体的相对位置及转速。针对小天体可能出现的章动问题，本文也给出了含章动小天体的转速测量方法并进行了仿真验证。由于提取的小天体特征具有一般性，该方法可以为卫星姿态的观测和估计以及其他在轨航天器的位姿测量提供参考。

1 ORB特征提取及匹配

图像的特征是图像上最具代表性的一些点，这些点包含了图像表述的大部分信息。即使旋转、缩放，甚至调整图像的亮度，这些点仍然稳定地存在，不会丢失。找出这些点，就相当于确定了这张图像，它们可以用来做匹配、识别等等有意义的工作。特征点由关键点和描述子两部分组成。特征点有很多类，如SIFT、基于加速分割测试的特征(Features from Accelerated Segment Test，FAST)、加速稳健特征(Speeded Up Robust Features，SURF)、快速特征点提取和描述算法(Oriented FAST and Rotated BRIEF，ORB)，考虑到实时性和性能，本文选用ORB特征。

ORB特征由Ethan Rublee等人[11]在2011年提出，关键点提取采用FAST算法，特征描述采用二进制稳健独立基本特征(Binary Robust Independent Elementary Features，BRIEF)描述子。ORB算法运行速度是SURF的10倍，是SIFT的100倍。

1.1 FAST算法

FAST算法检测关键点的核心思想是找出与周围像素不同的点作为关键点。FAST算法认为：若给定阈值t，当两个像素点的灰度值之差的绝对值大于t，则这两点不同，反之这两个点比较相似。判断一个点是否为FAST特征点，具体算法如下。

(1)计算该点灰度值Ip，设定阈值t。

(2)考虑该像素周围16个点，如图1所示。

(3)如果这16个点中有连续的n个点都和P点不同，那么它就是一个特征点，一般设定n为12。

针对第(3)步可以提出一个优化，仅仅检查位置1、9、5和13这4个位置的像素。如果是一个角点，那么上述4个像素点中至少有3个应该和中心点相同。如果都不满足，那么不可能是一个角点。这样可以快速排除大量非特征点的像素点。

图1 FAST特征点示意图Fig.1 Schematic diagram of FAST feature points

1.2 BRIEF描述子

BRIEF描述子计算出来的是一个二进制串的特征描述符，它是在一个特征点的邻域内，选择n对像素点pi、qi(i=1,2,…,n)。然后比较每个点对的灰度值的大小。如果I(pi)>I(qi)，则生成二进制串中的1，否则为0。所有的点对都进行比较，则生成长度为n的二进制串。一般n取128、256或512，默认为256。为了增加特征描述符的抗噪性，算法首先需要对图像进行高斯平滑处理。

特征点S×S的区域内选取点对的方法，采用p和q符合(0,S2/25)的高斯分布的策略。

1.3 ORB特征提取

ORB特征采用具有方向性的FAST特征，并采用具有旋转不变性的BRIEF特征描述子。FAST和BRIEF都是非常快速的特征计算方法，因此ORB具有非同一般的性能优势。OpenCV是一个基于BSD许可发行的跨平台计算机视觉和机器学习软件库，可以运行在Linux、Windows、Android和Mac OS操作系统上。它由一系列C函数和少量C++类构成，同时提供了Python、Ruby等语言的接口，实现了图像处理和计算机视觉方面的很多通用算法。提取ORB特征点可直接用OpenCV直接提取，匹配ORB特征点的过程如下(见图2)。

从提取的两张相邻图片中所有的描述子中找到两两距离最近的配对，可用欧氏距离进行量度。但实际上有些特征点可能并没有在两张图中同时出现，因此误匹配的情况还是很多的，所以，匹配好后还要进一步筛选。筛选策略是当描述子之间的距离大于所有配对的描述子的最小距离的二倍时，认为匹配有误。

图2 ORB特征提取Fig.2 ORB feature extraction

2 小天体旋转轴确定

SLAM中环境是静止的，相机是相对运动的，而小天体相对位姿测量假定相机是静止的，目标是相对运动的，因此二者可以等效。

(1)对采集图像进行SIFT特征提取，建立图像集之间的特征点匹配关系，并基于视觉SLAM对目标天体运动状态进行测量。

(2)在远距离视觉观测中，完成对小天体轨道位置的视角测量，并估计小天体的位置运动状态。

(3)在中近距离视觉观测中，完成小天体姿态运动状态的测量与辨识。

2.1 坐标系

OCXCYCZC坐标系定义为相机坐标系，OC0XC0YC0ZC0坐标系为固连在小天体上的，第一帧的时候与相机坐标系重合，OBXBYBZB坐标系为本体坐标系，且过目标质心。根据相对运动原理，假定小天体静止，相机绕目标转动，即OC0XC0YC0ZC0坐标系与OBXBYBZB坐标系静止，OCXCYCZC坐标系运动。理想情况下OCXCYCZC坐标系原点的运动轨迹为圆弧。该空间圆弧所在平面的法向量即为小天体自旋轴的平行向量，且自旋轴必经过此圆弧的圆心(见图3)。

图3 坐标系Fig.3 Coordinate system

2.2 空间平面法向量

对n个时刻OCXCYCZC坐标系原点进行空间平面拟合，由于所有观测点必在同一平面上，所以首先需对实测点进行平面拟合。任意空间平面方程可以表示为

ax+by+cz-1=0

(1)

式中：a、b、c为不全为0的常数，x、y、z分别为空间直角坐标系3个轴上的投影。

将n个观测点的三维坐标代入式(1)可得

A·X-l=0

(2)

根据最小二乘法则VTPV=min可知(权阵P为单位矩阵，V为目标矩阵)，拟合平面的法向量的方向系数为

X′=(ATA)-1ATl

(3)

2.3 空间圆圆心

文献[12]提出一种实用的空间圆形拟合检测新方法，根据空间圆中任意两条弦所对应的中垂面与空间圆所处的平面必然相交且交点即为圆心这一空间圆特性，利用空间向量按照最小二乘法推导出圆心计算方程,按照附有条件的间接平差求解圆心坐标，进而反算出空间圆半径。

(x2-x1,y2-y1,z2-z1)·

(4)

可简化为

Δx12·x0+Δy12·y0+Δz12·z0-l1=0

(5)

由空间球体中垂面方程的相关性，n个观测点坐标可以列出n-1个线性无关的中垂面方程，可得误差方程为

(6)

式(6)化简为

V=B·Y-L

(7)

认定圆心必在拟合的空间平面上，依此作为限制条件，按照附有条件的间接平差进行计算，限制条件为式(8)，联立得式(9)，推导可得圆心的最小二乘解。

限制条件为

C·Y-Wx=0

(8)

联立得到方程为

(9)

此时权阵P为单位阵。

式中：Ks为限制条件的联系数向量。

得出最小二乘解为

(10)

2.4 章动初始条件设置

(11)

式中：Ix、Iy、Iz为相对于直角坐标系三轴的转动惯量，ωx、ωy、ωz为三轴下的角速度，Tx、Ty、Tz为三轴方向上的力矩。

假设本体系坐标轴为主惯量轴，对于关于YB轴对称刚体，转动惯量存在如下关系

(12)

式中：It为引进变量。

代入式(11)，考虑空间自由旋转刚体，化简得到

(13)

(14)

式(14)进一步化简得到

(15)

即

(16)

式中：t表示时间。

根据式(16)得出：对称刚体自由转动状态下，存在绕对称轴恒定的自转角速度以及垂直对称轴平面内的大小恒定，方向周期性变化的平面角速度分量。据此，刚体自由旋转角动量矢量在本体系下可表示为

(17)

自转轴与角动量矢量夹角即为章动角，设为α，则

(18)

如图4所示，初始条件下四元数可由章动角表示为

(19)

图4 初始姿态示意图Fig.4 Schematic diagram of the initial pose

由本体系下角速度与欧拉四元数之间关系，可知

(20)

(21)

根据图4，得到初始条件

(22)

代入式(21)，可得四元数导数初始条件设置方法。

3 仿真分析验证

3.1 无章动小天体

选取小天体模型(见图5)，分别选取Y和与XYZ三轴呈相同角度的w轴为旋转轴，根据四元数乘法公式

(23)

式中：q0、q1、q2、q3，r0、r1、r2、r3为欧拉四元数。

图5 小天体模型Fig.5 Model of small celestial body

使用MBDyn软件[14]进行动力学仿真，利用paraview软件对二维和三维数据进行分析和可视化。假设小天体做自旋运动，且无章动，使用MBDyn软件与paraview软件进行联合仿真，设定原始转轴方向，取时间步长为0.002 s，每10步取一帧由paraview保存为视频，对视频提取关键帧进行ORB-SLAM得到位姿四元数并进行计算绘图。如图6为旋转拟合空间圆，如表1为计算得到旋转拟合空间圆圆心和旋转拟合空间圆平面法向量。

图6 拟合空间圆Fig.6 Fitted spatial circle

表1 拟合空间圆圆心和平面法向量Table 1 Center of the fitted space circle and normal vector of the plane

由ORB-SLAM2关键帧姿态四元数得到相邻帧转角与帧数关系，其中以Y轴为旋转轴每隔0.5 s取一帧，由其直线斜率拟合可得转速为7.065 (°)/s，如图7(a)所示；以w轴为旋转轴每隔1 s取一帧，由其图像斜率拟合可得转速为6.781 (°)/s，如图7(b)所示。

图7 小天体转速Fig.7 Rotational speed of small celestial body

3.2 含章动小天体

设置小天体对称主轴惯量IYB=150，IXB=IZB=100，章动角设置为α=π/12，绕自转轴自转速度为ω=π/12，使用MBDyn软件与paraview软件进行联合仿真，设定原始转轴方向，取时间步长为0.01 s，每10步取一帧由paraview保存为视频，对视频提取关键帧进行ORB-SLAM得到位姿四元数并进行计算绘图，如图8为拟合相机轨迹。

由ORB-SLAM2关键帧姿态四元数得到相邻帧转角与帧数关系，其中以Y轴为旋转轴每隔1 s取一帧，由其图像斜率可得转速为14.972(°)/s，误差为0.028 (°)/s，如图9所示。

图8 拟合相机轨迹Fig.8 Fitted trajectory of camera

图9 小天体转速Fig.9 Rotation speed of small celestial body

验证结果表明：基于视觉SLAM对目标天体进行三维重构与分析，对得到的相机轨迹进行空间圆拟合，可确定小天体的相对位置及转速，对于章动空间目标也具有适用性。

4 结束语

本文设计了一种小天体目标在轨位姿测量方法，实现了基于视觉SLAM的目标天体观测处理，获得了在轨小天体的位姿信息。针对空间小天体目标的章动特性，设计了一种章动小天体的观测测速方法，误差控制在0.1(°)/s以内。本文为航天器在轨运行以及空间垃圾处理等任务提供了一种有效的基于视觉的目标位姿测量方法，通过视觉同步定位与建图，获得了空间目标的旋转轴和旋转角速度，对章动目标也具有适用性，可为后续的捕获和抓取任务提供可靠依据。