非合作慢旋目标即时状态位姿确定

牟金震，温凯瑞，刘宗明

(1.上海航天控制技术研究所，上海 201109； 2.上海市空间智能控制技术重点实验室，上海 201109； 3.南京航空航天大学，南京 210016)

0 引言

慢旋运动是在轨失效卫星普遍具有的一种运动状态，其绕惯性主轴以某一固定的角速度保持匀速旋转运动。由于空间光照条件的复杂性，在空间开展在轨维修等操控任务时，需要对其旋转信息进行快速、准确的测量[1-3]。对空间失效卫星等非合作目标进行在轨操控，采用的测量方案是视觉测量方法[4-5]，目前已经开展了相关地面验证试验，是近距离段主要的测量手段。张世杰等[6]假设被测目标几何模型已知，设计了一种非合作光标位姿测量方法。文献[7-8]采用的方法一致，首先获得被测目标的3D模型，通过点云模板匹配的方法实现非合作目标的相对位姿测量。但空间大多数失效卫星以及垃圾碎片无法提供准确的几何模型，因此文献[7-8]提出的方法具有一定的局限性。Civera等[9]在扩展卡尔曼滤波(Extended Kalman Filter，EKF)-同步定位与地图构建(Simultaneous Locali-zation and mapping，SLAM)的基础上，利用逆深度参数处理方法估计特征的尺度。Augenstein等[10]提出了一种用于非合作目标姿态跟踪和重建的单目视觉SLAM算法。Schnitzer等[11]将文献[9]中EKF-SLAM与随机抽样一致(Random Sample Consensus，RANSAC)算法相结合，生成目标的模型，该模型可用于进一步细化估计位姿。Segal等[12]采用立体视觉系统跟踪目标飞行器上的特征点，建立模型，采用EKF的方法估计特征点位置、目标旋转角度、以四元数表示的目标旋转状态等。文献[13]基于Hill方程的相对运动模型，考虑了非合作目标水平运动和旋转运动之间的动力学耦合。Cho等[14]根据航天器相对距离的不同，采用了Shi-Tomas和加速稳健特征(Speeded Up Robust Features，SURF)两种算法，并提出了角点检测的鲁棒性。文献[15]在文献[14]的SURF算法的基础上，提出了对象请求代理(Object Request Broker，ORB)角点检测算法，解决了特征检测的旋转不变性。文献[16]在ORB-SLAM的基础上，分析了非合作旋转目标的闭环检测性能。以上文献[9-16]针对非合作目标相对位姿测量提出了多种解决方法,但关于复杂光照条件下非合作旋转目标测量的研究较少，仍处于起步阶段。

针对旋转目标的测量按照工作流程来说可以分为3个阶段，第1个阶段为远距离旋转特性跟踪测量阶段，该阶段的主要任务为悬停观测，追踪星在定点位置长时间连续观测目标星运动特性，在此过程中相机准确估计目标的旋转角速度，为后续的同步起旋提供测量输入；第2个阶段为起旋过程跟踪测量阶段，该阶段的主要任务为起旋同步，追踪星加速旋转到与目标星自旋角速度稳定同步，在此过程中相机稳定跟踪并提供相对角度和角速度信息；第3个阶段为靠拢捕获阶段，该阶段的主要任务为稳定测量，当追踪星与目标星之间的旋转相对稳定后，追踪星逐渐逼近目标星，在此过程中相机准确提供相对位置和相对姿态信息。本文重点解决第1个阶段——悬停观测段，即相机如何在复杂的光照条件下快速、准确地跟踪目标特征点，并且准确估计目标的自旋角速度信息。

1 非合作慢旋目标特位姿初始化

ORB[15]是一种快速特征点提取和描述的算法，分为特征点提取和特征点描述两部分。ORB通过构造图像金字塔，在每层金字塔采用Fast算法提取特征点。检测候选特征点周围一圈的像素值，如果候选点周围领域内有足够多的像素点与该候选点的灰度值差别够大，则认为该候选点为一个特征点。ORB通过灰度质心法计算特征点半径为r的圆形邻域内灰度质心位置，并寻找特征点的主方向。为了解决旋转的不变性，以特征点p为中心，取一个s×s大小的邻域。在特征域内随机选取2×N个点对(x,y)，然后对2×N点分别做高斯平滑，定义测试规则τ，比较N对像素点的灰度值

(1)

(2)

针对平面场景和非平面场景，本文分别采用单应矩阵和基础矩阵求解位姿变换，根据对称转移误差方程和距离阈值概率分布来统计内点的概率得分，如果选取的两帧满足约束条件，则初始化成功；否则，放弃这两帧并重新进行。

根据对称转移误差方程和距离阈值概率分布来统计内点的概率得分

(3)

根据RANSAC原则[16]，比较当前得分和历史得分，保留最高得分并记录相应的参数。用SH表示单应矩阵的得分，SF表示基础矩阵的得分。如果SH/(SH+SF)>0.4，那么选择单应矩阵求解位姿变换，反之选择基础矩阵。在完成得分概率统计后，针对所选用的模型求解阵R和平移向量t：1)根据单应矩阵分解R和t；2)根据基础矩阵分解R和t。

图1 2颗不同轴指向旋转卫星模型的位姿初始化Fig.1 Pose initialization of two rotating satellite models with different axes

2 非合作慢旋目标位姿优化

2.1 特征点跟踪

根据上一帧和匀速运动模型估计的当前帧位姿变换为

(4)

假设相机的内参为K，上一帧的三维特征点集为Piw，其在当前帧中的投影为

(5)

将估计的特征点与实际提取的ORB特征点进行匹配，如果当前帧与上一帧没有足够的匹配点，通过在上一帧预估的位置附近扩大一定的搜索范围继续搜索。当找到对应的匹配点后，利用它们对相机的预估位姿进行基于捆集调整的优化处理[17]，便可以得到当前帧准确的位姿变换。为了提高测量的可靠性和精度，在得到一个相机的位姿估计和一个初始的特征匹配集的基础上，将特征数据库中的三维点投影到当前帧中以搜索更多的匹配点对。

2.2 位姿优化

在本节中，引入李群和李代数，在李代数空间进行加法与求导操作，再将计算结果转换为李群，便可完成对测量误差函数的优化求解[18]。

对于旋转矩阵R(t)，等式(6)成立

R(t)RT(t)=I

(6)

对式(6)两边求导可得

(7)

式(7)是一个反对称矩阵，因此，存在一个三维向量ω(t)，满足

(8)

其中，∧表示从一个向量到反对称阵，式(8)两边同时右乘R(t)，可得

(9)

式(9)是一个微分方程，对旋转矩阵求导只需要左乘一个反对称矩阵ω(t)∧即可。设t0=0，且此时旋转矩阵R(t0)=I，在t0附近ω(t0)保持为常数ω(t0)=ω0，则有

(10)

对其进行积分可得

(11)

=I+α∧sinθ+(α∧)2(1-cosθ)

(12)

(α∧)2

(13)

在刚体变换过程中，除了旋转外还有平移变换，定义三维欧氏群SE(3)为

SE(3)=

(14)

其中，t为平移矩阵。对刚体变换矩阵g求导并乘以g-1可得

(15)

存在一个反对称矩阵ω(t)∧和一个三维向量v(t)，满足

(16)

(17)

因而，式(15)可以改写为

(18)

在此拓展了∧ 表示意义，为从一个向量到矩阵，对于三维向量来说，其表示一个到反对称阵的运算；对于高于三维的向量来说，其表示一个从向量到矩阵的运算，所以

(19)

(20)

定义三维欧氏群对应的李代数为

(21)

(22)

其中

(23)

由以上分析可知，相应的对数运算可以将李群映射到李代数。

ω=ln(R)∨

(24)

v=V-1t

(25)

假设2个位姿节点在se(3)上的表示为ξi和ξj，它们之间的运动估计为ξij，则其对应关系如下所示

(26)

式中，∨表示从矩阵到向量的运算。如果在SE(3)上的表示为Ti、Tj和ΔTij，则其对应关系为

(27)

构建误差函数

(28)

利用李代数扰动模型对误差函数求导，分别给ξi和ξj一个左扰动δξi和δξj，式(28)变为

(29)

根据SE(3) 上的伴随性质

eξ∧t=te(Ad(t-1)ξ)∧

(30)

其中

(31)

(32)

由式(32)可以得到，关于位姿Ti和Tj的雅克比矩阵，其中

(33)

(34)

在此，对雅克比矩阵进行近似，可以得到

(35)

根据图优化，假设C为所有边的集合，那么基于李代数空间位姿图优化的目标函数可记为

(36)

其中，Ω是信息矩阵。

3 实验分析

在进行地面仿真实验时，对目标星的模拟采用了两款模型，其中一款为常规卫星，表面贴有热控多层反光材料；另一款为嫦娥卫星，带有太阳能帆板。模型分别固定于转台上，其中常规卫星目标模型以 10(°)/s的角速度绕竖直轴匀速运动，嫦娥卫星模型以3(°)/s的角速度绕竖直轴匀速运动，相机光轴方向与目标旋转轴方向垂直。同时，采用高亮度LED光源，模拟复杂光照条件。由于目标的转动，相机成像的亮度值发生了明显变化，甚至出现曝光饱和现象，但仍然能快速稳定地跟踪旋转目标，具有较好的鲁棒性。

图2和图6所示分别为针对嫦娥卫星和常规卫星旋转运动过程中采集的图像帧，从中抽取关键帧为代表，图中的绿色小方形对应的是提取的特征点。从图2和图6可以看出，当被测卫星出现局部过饱的情况下，依然能稳定提取特征点。图3和图8所示为相机的等效运动轨迹，其中蓝色方框表示关键帧。图7所示为在局部跟踪丢失特征点后，根据已有的地图估计的相机位姿。图4和图9所示分别为目标以3(°)/s和10(°)/s的角速度自旋运动时的测量曲线，从图中可以看出，在目标自旋过程中能稳定跟踪被测卫星。图5和图10中上半部分的点直线和实直线分别表示实际的旋转角和估计的旋转角；下半部分表示实际旋转角与估计旋转角之间的残差，当目标以3(°)/s和10(°)/s自旋运动时，测量稳定后平均角速度误差分别为0.02°和0.1°左右。

图2 嫦娥卫星模型不同角度提取特征点(3(°)/s)Fig.2 Chang’e satellite model extracted feature from different perspectives

图3 等效的相机位姿轨迹 Fig.3 Equivalent camera pose trajectory

图4 实际运动曲线(3(°)/s)Fig.4 Actual motion curve(3(°)/s)

图5 拟合曲线及测量误差(3(°)/s) Fig.5 Fitting curve and measurement error(3(°)/s)

图6 常规卫星不同角度特征提取(10(°)/s)Fig.6 General satellite model extracted feature from different perspectives(10(°)/s)

图7 局部跟踪丢失情况下姿态轨迹Fig.7 Pose trajectory in case of local tracking loss

图8 优化后的等效运动姿态轨迹Fig.8 The optimized equivalent motion posture

图9 实际曲线(10(°)/s)Fig.9 Actual motion curve(10(°)/s)

图10 拟合曲线及残差(10(°)/s) Fig.10 Fitting curve and measurement error(10(°)/s)

4 总结

针对复杂光照条件下非合作慢旋目标位姿测量问题，本文通过ORB对特征点赋予了尺度特性和旋转不变特性，使目标点同时具备了提取快速性和匹配稳定性的特点。当相机前后两帧成像视差较大时，如果特征点在同一个平面上，则通过计算单应性矩阵来估算位姿；如果特征点不在同一平面上，则通过计算基础矩阵来估算位姿，基于概率统计模型求解初始化位姿。为了对位姿结果进行优化处理,通过计算李代数中测量值与估计值的残差，利用李代数求导扰动模型进行求解，再将计算结果转换为李群，实现了对测量误差函数的优化求解。