空间失效慢旋卫星视觉特征跟踪与位姿测量

2021-03-27刘宗明牟金震张硕杜宣曹姝清张宇

航空学报 2021年1期

刘宗明，牟金震，张硕，杜宣，曹姝清，张宇,*

1. 上海航天控制技术研究所, 上海 201109 2. 上海市空间智能控制技术重点实验室, 上海 201109 3. 上海航天技术研究院, 上海 201109

针对诸如空间碎片、失效卫星和小行星等非合作目标的相对测量问题，其中一种解决思路是利用视觉传感器直接对目标进行在线三维重建。随着机器人技术的快速发展，同步定位与制图(SLAM)[1]技术可以有益地借鉴到针对非合作目标的测量任务中来。SLAM技术已在机器人领域得到了广泛的应用，它可以帮助机器人在一个未知的环境中计算出自己的运动轨迹，同时可以重建周围环境的地图。在大多数应用中，机器人运行的环境被认为是静止不动的，但是如果动态场景中存在的是刚体目标的话，SLAM也是可以处理的。因而，完全可以将SLAM技术应用到空间非合作目标的测量中来。

Sonnenburg等[2]提出了一种基于特征和EKF(Extended Kalman Filter)滤波器的SLAM算法，该算法聚焦于相对导航滤波器结构设计，特征点集假设利用双目相机已经完成了提取，并且作为输入提供给滤波器；在滤波器中建立的平移运动方程是基于Hill相对运动模型的，该模型假设目标飞行器和追踪飞行器之间的相对轨道是圆形的，但是滤波器的状态量中不包含目标的惯量信息；考虑了两种不同的绝对转动动力学情况，在这两种情况中都忽略了目标飞行器和追踪飞行器在LVLH(Local Vertical Local Horizontal)坐标系下的指向差异；同时，提出了利用EKF进行单目相机特征提取和跟踪的滤波器框架。

Augenstein和Rock[3]提出了一种基于单目视觉SLAM的非合作目标位姿跟踪和外形重建算法，利用了Rao-Blackwellized粒子滤波器架构计算相对旋转和平移动力学；假设目标以恒定角速度进行旋转运动，并且为了给滤波器提供一个先验估计，同时需要假设初始位姿已经获得；由于算法只能得到目标的离散三维点云图，所以无法对惯性矩进行估计；对算法进行了数学仿真，并且利用水下绳系目标进行了试验验证。

Schnitzer等[4]融合了基于EKF滤波的SLAM算法和基于RANSAC(RANdom SAmple Consensus)的表面重建算法对非合作目标进行建模，利用SURF(Speeded-Up Robust Features)算子[5]描述目标表面的特征点，基于立体视觉原理计算目标特征点的位置信息；利用小型接近操作模拟器在实验室内对算法性能进行了验证，试验结果表明尽管存在噪声干扰，同时目标三维点云的分布是稀疏的、不均匀的，该算法仍然能够得到较好的表面重建结果，但完成一次计算需要耗时几秒钟，实时性难以保证。

Tweddle[6]在其博士毕业论文中在Kaess等[7]工作的基础上提出了一种绕其惯性主轴旋转的非合作目标SLAM测量方法，该算法采用的平滑滤波器相较于常规的卡尔曼滤波器而言具有更好的收敛性，但是带来的问题同样也是计算负担较重、耗时较长；该算法假设追踪飞行器静止不动，模型方程中不考虑外部干扰力矩对相对运动动力学的影响；Tweddle[6]利用球形卫星模型对算法进行了试验验证，基于双目视觉完成三维重建，结合滤波器可以实现对完整的相对状态量的估计，包括目标线速度和角速度、质心位置和惯性主轴等。

Segal等[8]提出了一种基于多iEKF(iterated Extended Kalman Filter)滤波器和最大后验概率的惯性张量辨识算法，采用双目视觉的方式获取目标特征点的三维位置信息，滤波器方程中基于希尔方程建立相对运动模型，同时考虑了旋转和平移运动的动力学耦合[9]，通过数学仿真对算法性能进行了评估；在仿真中假设通过双目视觉相机可以稳定地获得均匀分布于目标表面的10个特征点，双目相机的基线为1 m，最远测量距离大于100 m，高斯噪声为10-5rad，平均测量精度为厘米级。

Conway等[10]提出了一种微小型目标接近过程的导航策略，采用了RGB-D(RGB-Deepth)相机，采样帧频为30 fps，可以同时获得图像信息和深度信息；特征点提取和匹配采用的是ORB(Oriented fast and Rotated Brief)描述子，通过二维图像特征点及其对应深度值的匹配可以得到特征点的三维信息；进行了数学仿真，并利用微软公司的Kinect进行了测试试验。

Pesce等[11]提出了一种相对运动估计和惯性参数重建的iEKF方法，文献[11]中的滤波器结构与文献[6]相似，但是文献[11]采用的平滑方法提高了计算效率；由于文献[11]并未考虑外部干扰力矩对相对旋转动力学模型的影响，因为只能求得惯量比，但是可以通过一个离线的图像处理过程获得目标完整的三维重建模型；与文献[8]类似，Pesce等[11]利用简化的立体视模型，通过数学仿真对算法性能进行了验证，并且对离线三维重建算法进行了初步的测试评估。

从现有文献的分析来看，目前针对非合作旋转目标的测量方案主要还是集中在基于特征的滤波器设计方面，利用滤波算法对目标状态进行预测，常用的滤波方法在一个较小的运动尺度和较低的运动速度范围内，通过持续地维护和升级摄像机状态向量和特征点向量进行测量，可以表现出较好的跟踪效果。但是其计算精度受限于所构建的状态模型和测量模型的精度，模型的非线性和相机的测量噪声等会导致测量误差放大，甚至发散。计算速度也受到状态向量维数的严重制约，即使是通过减少特征数来保持计算效率，也很难避免高维的状态向量导致的“维数灾难”问题[12]。

为了克服以上问题，本文构建了目标特征数据库，将当前帧与特征数据库进行比较，完成特征匹配，采用关键帧共视图对特征点进行搜索跟踪。基于位姿图优化完成对目标位姿的局部和全局最优化求解。在李代数空间下建立位姿向量微分扰动方程，获得测量值与估计值的残差目标函数，基于贝叶斯法则最大后验概率和李群与李代数的对指变换法则，求取位姿向量最优解，以期解决李群空间下由于位姿变换矩阵不可加性而无法优化的问题，提高连续测量过程中系统的测量精度。

1 失效慢旋卫星视觉特征跟踪

1.1 构建目标特征点数据库

空间的光照条件较为复杂，视觉系统采集到的目标航天器图像会发生部分灰度退化或者几何退化，如图像模糊、图像对比度变化和图像灰度退化等。主要的干扰因素如下：

1) 目标表面反射不均匀。由于空间目标表面大都包覆有高反射系数的热控多层保温材料，太阳光、目标表面和相机在一定的角度范围内会出现目标成像效果局部过亮或过暗的现象，导致特征点无法有效提取。

2) 杂散光干扰。相机镜头安装在航天器舱外，所以会受到光的照射，阳光、地气光、月光、恒星光和行星光等天体照射舱体外各种部件导致光反射，形成大量的杂散光干扰。

特征点提取的快速性、描述子的尺度不变性和旋转不变性是实现非合作旋转目标实时测量的基础保证。为了保证特征的不变特性，需要在多个尺度空间下对图像特征点进行提取，并且赋予旋转不变特性的描述子，将高维的原始图像转化为低维的描述向量。目标特征子的提取速度和描述子的复杂程度将直接影响测量系统的计算效率和匹配效果，选取的图像特征表达在提高计算速度的同时，也要保证在后续图像匹配时的正确率，这两点同时满足才能保证特征检测的精度与速度。为保证特征匹配的尺度不变性，使用尺度因子将当前帧图像转换到8个尺度空间上进行FAST(Features from Accelerated Segment Test)角点提取。为了确保在图像核心目标区域角点的均匀分布，对每个尺度的图像进行网格划分，将其等分为若干份，在每个网格中至少提取5个角点。最后对提取到的FAST角点进行Brief(Binary robust independent elementary features)描述子和方向的计算，共同组成一个ORB特征点。

在针对非合作目标的测量过程中，目标模型的具体尺寸是无法预先得知的，目标表面的纹理特征就更无法提前获取了。当前时刻位姿取决于前一时刻的位姿和前一时刻与当前时刻的位姿变换的乘积，该流程虽然计算简单，运算量少，但是如果当前帧中特征点缺失，容易造成跟踪失败。并且当前一帧的位姿估计误差较大时，还会出现显著的漂移，造成误差累积，直至发散。因而考虑构建目标的二维和三维特征数据库，关注的重点变为当前帧与特征数据库的准确跟踪匹配和特征数据库中特征点的优化问题。假设只考虑前后两图像帧之间运动关系，设前一帧为参考帧，用ref表示，当前帧用cur表示，以参考帧为基准，计算当前帧与其的运动关系。假设参考帧和当前帧相对世界坐标系的变换矩阵分别为Trw和Tcw，两帧之间的变换矩阵为Tcr，则它们之间的关系可表示为

(1)

在图1中，在t0～t1时刻，以t0为参考，求t1时刻的运动；然后在t1～t2时刻，又以t1时刻为参考帧，考虑t1～t2时刻间的运动关系，如此往复。如果当前帧或参考帧中纹理稀少、光照变化和遮挡等原因导致特征点缺失，容易造成跟踪失败。并且，当参考帧的位姿估计误差较大时，还会出现显著的漂移，造成误差累积，直至发散。因此，考虑构建目标的二维和三维特征数据库，将当前帧与特征数据库进行比较，如图2所示。问题转变为相机在每一个t时刻采集到的图像与之前时刻重建的特征数据库进行准确匹配和特征数据库中特征点的优化问题。在一个不断更新的特征点数据库中，只要特征点正确，即使过程帧出现了差错，仍然有可能求出后续图像帧的正确位置。特征数据库又分为局部和全局两种，局部特征数据库描述了当前帧附近的特征信息，可以提高当前位姿的计算效率。全局特征数据库则记录了系统运行以来，根据一定规则选择保留的特征点，主要用于闭环检测。

目标数据库的特征点是指可靠的目标三维点，每个数据库特征点存包含的信息包括：

1) 其在世界坐标系下的三维坐标。

2) 平均视线方向(由该点指向所有共有这个点的关键帧光心的平均法向量)。

3) ORB特征算子[13]和描述子[14]。

4) 根据ORB尺度因子(1.2)确定的最大和最小可视范围。

其中，关键帧是指从连续采集的图像中选出来的帧，每个关键帧包括：

1) 相机姿态矩阵。

2) 相机的内参矩阵。

3) 本帧所有的ORB特征点。

为保证特征库点云数量控制在一定范围内，不至于过于膨胀，当有更合适点出现时，需要将其加入到特征库中；当出现冗余或相似点云时，需要将其从特征库中剔除。

1.2 利用特征数据库进行特征点跟踪

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

(2)

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

(3)

将估计的特征点与实际提取的ORB特征点进行匹配，如果当前帧与上一帧没有足够的匹配点，通过在上一帧预估的位置附近扩大一定的搜索范围继续搜索。当找到对应的匹配点后，利用它们对相机的预估位姿进行基于捆集调整优化处理，便可以得到当前帧较为准确的位姿变换。为了提高测量的可靠性和精度，在得到了一个相机的位姿估计和一个初始的特征匹配集的基础上，便可以将特征数据库中的三维点投影到当前帧中搜索更多的匹配点对。为了限制计算的复杂度，只投影局部三维特征数据库，此局部特征数据库包括一个关键帧集合K1，此集合与当前帧有公共的三维特征点；还包括一个关键帧集合K2，此集合与K1相邻，具有共视图；最后还包括一个参考关键帧Kref，其中，Kref∈K1，Kref与当前帧有最多的共视点[15]。它们之间的关系如图3所示。其中，Fcur表示当前帧；K11、K12和K13构成集合K1；K13为参考关键帧Kref；K21、K22和K23构成集合K2。在测量过程中，关键帧和特征点数据集不应该一直无限增大，这样不仅是对搜索的运算量还是存储空间而言都是巨大的压力。在局部建图过程中有一个冗余关键帧和特征点剔除机制，因此，在跟踪过程中要实时确定该点是否为可用特征点；在跟踪的最后一步是确定当前帧是否为关键帧，这样可以使得对相机运动的追踪变得更加稳定。每一个在K1和K2集合中可以看到的三维点在当前帧中按照以下规则进行搜索：

1) 计算三维点在当前帧中的投影点，如果其超出了预先设置的图像边界，将该点剔除，不将其用于特征接力。

2) 计算当前视轴(当前帧法向量)和该三维点的平均视线(该三维点指向所属的所有关键帧光心的平均法向量)的夹角，如果大于60°则将该点剔除，不将其用于特征接力。

3) 计算该三维点到当前帧所对应的光心距，如果超出其对应的尺度不变域范围则将该点剔除，不将其用于特征接力。

4) 计算当前帧尺度。

在尺度范围内，将估计的特征点与实际提取的ORB特征点进行匹配，如果当前帧与上一帧没有足够的匹配点，则通过在上一帧预估的位置附近扩大一定的搜索范围继续搜索。当找到对应的匹配点后，则将其与当前三维点关联起来，据此，可以将当前帧与特征数据库中的公共点对应起来，完成特征接力。利用当前帧中的特征点对相机的位姿进行基于捆集调整优化处理，便可以得到当前帧较为准确的位姿变换。

2 李代数空间下的位姿图优化

2.1 图优化概念

假设状态变量x=[x1,x2,…,xn]T，其中xk表示在k时刻相机的位姿节点。两节点之间的观测方程为

zij=h(xi,xj)+vij

(4)

式中：h为测量函数；vij为观测噪声，由于各种误差因素的存在，产生了测量残差：

eij(xi,xj)=zij-h(xi,xj)

(5)

假设以xk为优化变量，对于一个具有n条边的图，其每条边的优化目标函数为

(6)

式中：Ωij为信息矩阵，是协方差阵的逆，是一个对称矩阵。所有边的优化目标函数为

(7)

最终的优化目标误差函数可以写成：

(8)

(9)

(10)

(11)

如果考虑所有边，则式(11)可改写为

HΔx=-b

(12)

式中:H为信息矩阵;b为标量系数。

系统的解就是在初始值上叠加此增量：

(13)

GN迭代就是不断重复式(13)直至收敛，LM迭代则引入了一个松弛因子λ控制迭代速度：

(H+λI)Δx=-b

(14)

由于GN迭代有可能出现雅可比矩阵乘积奇异或者病态的情况，导致增量的稳定性差，算法因此将会发散，LM迭代在一定程度上修正了这个问题，因而采用LM迭代。

2.2 位姿图优化

在2.1节中定义的目标误差函数可能会没有意义，因为加法操作对位姿变换矩阵而言是不封闭的，即两个变换矩阵的和并不是变换矩阵。而且，在构建优化问题时，旋转矩阵作为待计算的优化变量，必须是一个行列式为1的正交矩阵，这样会引入额外的约束条件，使优化问题变得更复杂，因而不便于在李群空间下直接对其进行优化操作。笔者先在李代数空间进行加法与求导操作，再将计算结果转换为李群[19]，便可完成对测量误差函数的优化求解。

在刚体变换过程中定义三维欧氏群为

SE(3)=

(15)

式中：g为位姿变换矩阵；SO(3)为三维旋转群。

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

se(3)=

(16)

式中：ξ为一个六维向量；上标∧代表求反对称阵运算；ω为旋转角速度；v为速度；so(3)为三维旋转李代数。

李代数对应的六维向量为ξ=[v，ω]T∈6，可以得到三维欧氏李代数到三维欧氏李群的指数映射：

(17)

式中：eξ为李代数到李群的指数映射；eω∧为旋转角速度的指数映射；V为变换系数,可表示为

(18)

式中：α为三维向量ω的方向；θ为三维向量ω的模。

由于ω是一个三维向量，由以上分析可知，相应的对数运算可以将李群映射到李代数：

(19)

式中：上标∨表示从矩阵到向量的运算。

在位姿图优化中，节点表示相机的位姿，用ξ1,ξ2,…,ξn表述；边表示两个位姿节点之间相对运动状态的估计。假设两个位姿节点在三维欧氏李代数se(3)上的表示为ξi和ξj，它们之间的运动估计为ξij，则其对应关系为

(20)

如果在三维欧氏李群SE(3)上的表示为Ti、Tj和位姿i、j间的变换矩阵Tij，则其对应关系为

(21)

构建误差函数：

(22)

利用李代数扰动模型将误差函数eij分别对ξi和ξj各加一个左扰动δξi和δξj，式(20)变为

(23)

根据SE(3)上的伴随性质[20]：

eξ∧T=Te(Ad(T-1)ξ)∧

(24)

式中：

(25)

(26)

根据李代数上的求导法则可以得到关于位姿Ti和Tj的雅可比矩阵，其中：

(27)

式中：Jr为雅可比矩阵。

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

(28)

3 测量实验与结果

实验环境为超近距离相对运动实验室，如图6所示；所用运动装置为高精度三轴转台，如图7所示。对目标星的模拟采用了1∶1的真实模型，表面贴有热控多层反光材料，同时，采用高亮度LED光源模拟入射的太阳光。模型分别固定于转台上，以一定的角速度绕竖直轴匀速运动。由于目标的转动，相机成像的亮度值发生了明显变化，甚至出现曝光饱和现象。实验中先后进行了基于前后图像帧信息的EKF滤波算法、无位姿优化和有位姿优化的测量算法，从实验结果来看，所提算法能较好地适应光照条件变化，具有较强的鲁棒性。

3.1 基于前后图像帧信息的EKF滤波算法验证

目标以5 (°)/s的角速度绕垂直地面轴做自旋运动，每当视觉相机检测到新的特征时，都将其添加到状态方程中，组成全状态模型[22]：

(29)

图8为连续测量过程中保存的图像帧，图9为相机等效的运行轨迹。图9可以反映出目标是在绕其固定轴做旋转运动，但是轨迹图并不完整，只绘制出了3/4的圆周运动。究其原因在于模型的非线性和相机的测量噪声等因素影响，导致测量误差放大，无法收敛，通过前后帧估计得到的特征点位置和位姿变换矩阵无法继续完成运动状态的跟踪测量。由于测量结果无尺度因子，因而图中未标出具体量纲。随着特征点数量的增多，状态向量的维度越来越大，数据更新的速率明显降低，如图10所示。

3.2 基于特征数据库的无位姿优化测量算法验证

目标以12 (°)/s的角速度绕垂直地面轴做自旋运动，从图11可以看出，在目标卫星旋转过程中，尽管光照强度发生了比较明显的变化，但是仍然能够对提取出的特征点进行比较好的跟踪。从图12可以看出，随着时间的延长，累积误差愈来愈大，三维点云逐步发散，无法较好地绘制出目标的三维形态。图13和图14分别为无位姿优化时针对以12 (°)/s旋转的常规立方卫星大模型的位姿测试曲线。从图13中可以看出自旋轴旋转角在-180°～180°范围内变化，与自旋轴垂直的两轴角度随着测量时间的延长并未趋于收敛。由于目标模型固定安装在三轴转台，其三轴位置不随测量时间的延长发生明显变化，为了更直观地反映相机测量效果，在相机对目标完成旋转一周的测量后，其等效运动轨迹应该是个理想的圆形，因而采用绘制相机等效运动的方式说明非线性优化对位置测量的重要性。从图15中也可以看出随着时间的延长，位置曲线所构成的圆的半径在逐渐扩大，呈发散状态。

3.3 基于特征数据库的有位姿优化测量算法验证

图16为包含位姿优化的测量算法三维重建效果图，稀疏点云较好地重建了目标的三维形态，与实际试验情况相符。图17和图18分别为有闭环检测时针对以12 (°)/s旋转的常规立方卫星大模型的位姿测试曲线。从图中可以看出，测量曲线明显连续光滑了很多，而且位置姿态是收敛状态。图17中自旋转角在-180°～180°范围内变化，之所以出现明显的阶跃现象，一方面是由于画图采用的是关键帧位姿，而关键帧相对来说比较稀疏；另一方面是由于过程中包含有位姿闭环优化算法，当目标旋转回初始位置附近时，进行了位姿的闭环检测和位姿优化，使原本趋于发散的位姿收敛了回来。