一种空间非合作目标相对位姿紧耦合估计方法

刘 柯，汪 玲，刘寒寒，张 翔

(1.南京航空航天大学 电子信息工程学院，南京 211106; 2. 南京理工大学 机械工程学院，南京 210094)

0 引言

随着空间技术的发展，频繁的航天任务导致对在轨服务航天器实现复杂任务的需求日益增加，例如对目标的绕飞观测和逼近停靠，对失效卫星和空间碎片进行清理，还有维修故障航天器等，这些任务都需要服务航天器近距离准确得到目标的相对位置与姿态信息[1-4]。根据空间目标能否反馈有用的合作信息，如能直接进行通信或已安装光学标志等，可以分为合作目标和非合作目标[5-6]。针对空间合作目标的近距离相对位姿测量技术，国内外学者已经进行了大量研究，提出的许多方法都已成功在轨验证[7-10]。而空间非合作目标由于其缺乏先验信息，相对位姿测量问题变得相当具有挑战性，而且在已编目的在轨目标中，又有大量被归类为非合作目标，所以空间非合作目标的相对位姿测量问题越来越受到国内外学者的重视。

目前空间非合作目标的相对位姿获取方法主要有：光学成像测量与激光雷达测量[11]，具体传感器的选择必须考虑任务航天器上的可用资源，例如功率、质量、体积和计算资源等，另外还必须考虑到具体的任务设想和整体的费用。相比激光雷达测量技术，基于光学成像测量的位姿估计技术因其具有低成本、高精度等优势，已成为航天器近距离导航中获取目标相对位姿的重要途径。

此外小型卫星由于研发快、成本低等优势，在航天活动中越来越受到青睐，应用领域不断拓展。在卫星小型化发展的过程中，对星载传感器功率、质量、体积等的要求也越来越苛刻。单目相机有功耗低、质量轻、体积小、结构简单等优势，恰恰满足了小型卫星对星载传感器的要求，是小型卫星近距离导航中获取目标相对位姿的有效途径，所以基于单目相机的空间非合作目标位姿估计技术越来越受到国内外学者的重视。文献[6]基于单目相机估计了非合作航天器的相对位姿，但假设目标航天器特征点模型已知。文献[12]利用移动机器人中同步定位与地图构建(SLAM，simultaneous localization and mapping)方法和运动中结构恢复(SFM，structure from motion)方法来解决空间未知非合作目标的相对位姿估计问题，该文献提出了一种实时估计未知翻滚目标帧间位姿的SLAM/SFM算法，只使用单个相机，该算法融合了贝叶斯估计方法和测量反演法来估计相对位姿参数，可用于检查和维修故障或损坏的卫星，但该方法假定目标初始位姿已知。文献[13]针对此方法中的相对位姿估计部分进行改进，提出联合无迹卡尔曼滤波(UKF，unscented kalman filter)和粒子滤波 (PF，particle filter)来估计目标的位姿参数，但该方法计算量大，实时性较差。

由于单目相机的局限性，无法测得空间非合作目标深度，由此一些学者提出利用单目视觉与激光测距仪混合的系统来估计空间非合作目标的位姿参数。文献[14]基于单目手眼相机和激光测距仪， 提出了一种尺寸未知的空间目标的位姿测量算法，但该方法只适用于矩形目标或者自带矩形标志点的目标。文献[15]提出了一种基于可见光相机与激光测距仪的空间碎片在轨识别与定位方法，由单目相机提供测角信息，以优化后的测角信息驱动激光测距仪完成测距，之后结合两个传感器的测量信息，采用扩展卡尔曼滤波算法获得较高精度的目标位置信息，该方法适用于完全未知的空间非合作目标，但该方法只求得了目标的位置信息，而没有解算目标的姿态信息。

本文针对空间任务中服务航天器实时估计空间未知非合作目标的相对位置和姿态这一难题，提出了一种融合单目相机与激光测距仪的相对位姿估计方法。采用单目相机获取目标序列图像，在初始化时构建了真实尺度下的世界坐标，在连续位姿估计时以紧耦合的形式融合相机数据与激光测距仪数据来优化相对位姿，得到了较高精度的相对位姿信息。最后使用Blender软件生成空间非合作目标序列图像，验证了本文算法能稳健地得到较高精度的空间非合作目标的相对位姿，且拥有较好的实时性。

1 系统基本知识

1.1 基本假设

在相对位姿估计中涉及两个空间物体，服务航天器和空间非合作目标。空间非合作目标作为一个未知目标，其外形、质量、质心位置和转动惯量等所有参数均是未知的。服务航天器作为主动航天器具有姿态控制与轨道控制能力。服务航天器上装备一部单目相机和一个激光测距仪作为测量传感器，假设相机视场中只出现目标或图像背景中的地球已被滤除。考虑到我们感兴趣的是服务航天器与空间非合作目标间的相对位姿问题，若相机视场中只存在非合作目标，则可将非合作目标的运动等效看作为服务航天器的运动，比如非合作目标靠近服务航天器的运动可等效为服务航天器远离非合作目标的运动，这种运动转换在定义完坐标系之后进行详述。

1.2 坐标系定义

图1给出了各个坐标系的定义，设oC-xC-yC-zC为相机坐标系，其中oC为摄像机光心，也是针孔模型中的针孔，zC轴穿过相机光心，与成像平面垂直指向相机前方，xC向右，yC向下。设oB-xB-yB-zB为目标本体坐标系，O-X-Y为物理成像坐标系，o-u-v为像素坐标系。物理成像平面与相机光心的距离为相机焦距f，激光测距仪激光发射点L距相机光心的距离为基线长度d，发射方向与相机坐标系zC轴平行，记激光测距仪照在目标上的点为激光测距点。非合作目标上的空间点P，小孔成像后落在物理成像平面上的成像点为p。ow-xw-yw-zw为世界坐标系，在此坐标系下估计服务航天器与空间非合作目标的相对位姿。相机坐标系与目标本体坐标系会随相机与目标的运动而不停变动，图1是初始时刻时，相机坐标系与世界坐标系重合，目标本体坐标系与世界坐标系坐标指向相同。

图1 坐标系定义

1.3 运动转换

测量空间非合作目标的相对位姿时，在剔除背景干扰的情况下，可以将空间非合作目标的运动全部转换成相机的运动，如图2所示，目标向相机移动to等同于相机向目标移动-to：

图2 运动转换示意图

除了图2这种简单的平移运动，非合作目标的旋转可以转化为相机的旋转加平移运动，例如非合作目标自旋一周，可以转换为相机将镜头一直面向目标绕飞一周，有了这些转换情况，就可以构建一个只含有任务航天器的单目相机和空间非合作目标的系统：

在初始化时确定目标在世界坐标系下的位姿，然后将后面所有的目标运动都转换为相机的运动，即目标在世界坐标系下的位姿始终不发生改变，这样根据目标在相机图像中的变化，就可以求得世界坐标系下相机自身的运动叠加转换的相机运动，之后根据目标在世界坐标系下的位姿就能得到相机与目标的相对位姿。

后文中再出现的相机运动，都是世界坐标系下相机自身的运动与转换的相机运动叠加后的运动，当目标位姿在初始化中固定后，再求得相机在世界坐标系下的位姿相当于求得了相机与目标之间的相对位姿。

2 相对位姿初始化

选取初始两帧图像与第一帧时刻激光测距仪数据，对服务航天器与空间非合作目标的相对位姿进行初始化。图3为初始化流程图，下面对其中的关键步骤做介绍。

图3 位姿测量初始化流程图

2.1 特征点检测与匹配

考虑到空间环境的光照环境复杂，非合作目标运动的不确定性，本文在特征检测时选取加速鲁棒特征(SURF， speeded up robust features)[16]作为目标特征点，该特征点具有良好的旋转和尺度不变性，且在光照变化时仍能保持较好的检测效果。

后续在恢复尺度时使用KLT(kanade-lucas-tomasi)光流法跟踪求解激光测距点的伪测量点，在求解相对位姿时，使用暴力匹配的基础上，增加FLANN算法[17]进行筛选，使匹配更为精确。

2.2 激光测距点伪测量值的计算与匹配

由于激光测距点可能没有直接在像平面成像，或者成像了也无法确定像平面哪个点是激光测距点，所以其在第二帧图像上的投影点坐标无法像其他特征点一样直接观测得到。而是根据两帧中的其他特征点的观测值利用光流法计算出激光测距点在第二帧的伪观测点。

针孔模型的像为倒像，而我们实际在使用相机时，看到的一般是正着的像，因为相机内自身的软件会翻转输出的图像，所以将图1中成像平面对称挪到相机光心前方，如图4所示。

图4 带对称虚拟成像平面的投影模型

激光测距点为非合作目标上空间点PL，激光测距仪距光心距离为d，读数为l，则在初始状态，空间点PL的世界坐标为[x,d,l]T，成像点PL的世界坐标为[X,Y,f]T，则有：

(1)

激光测距仪发射方向与相机坐标系zc轴平行，落在ycoczc平面上，所以x为0，则点PL在物理成像坐标系中的坐标[X,Y]T为：

(2)

在求解非合作目标相对位姿问题时，我们能得到的观测值为特征点的像素坐标，根据点在物理成像坐标系中的坐标求得该点在第一帧的像素坐标系中坐标为：

(3)

其中:dx、dv为单位距离内像素的个数，当焦距f单位为米时，1/dx和1/dv的单位为像素/米，[cx,cv]T是图片中心点像素坐标，fx、fv单位为像素，这些都是相机内参，写成矩阵形式为：

(4)

式中,l为激光测距仪读数，矩阵K就是相机内参矩阵，通过相机标定获得，向量PL为激光测距点PL坐标。

以像素坐标[u1,v1]T为中心，以200 pixels×200 picels的矩形为感兴趣区域检测匹配上的特征点，假设匹配到n对特征点，它们在第一帧图像中的像素坐标为[u1,i,v1,i]T，使用KLT光流法跟踪得到它们在第二帧图像中的像素坐标为[u2,i,v2,i]T，其中i=1,2,3,...,n，则激光测距点投影在第二帧图像中像素坐标[ux2,v2]T为：

(5)

然后将第一帧图像中的像素点[u1,v1]T与第二帧图像中的像素点[u2,v2]T加入2.1中使用特征点描述子进行暴力匹配的匹配特征对p1,i-p2,i中，一起进行后续操作。

2.3 估计归一化相机位姿与特征点空间坐标

以旋转矩阵R和平移向量t描述第二帧相机坐标系相对于第一帧相机坐标系的位姿关系，在初始化时，旋转矩阵R和平移向量t也是第二帧相机相对于世界坐标系的位姿。记非合作目标上一空间点世界坐标为P，该点投影在两帧图像中的像素坐标分别为p1，p2，则有：

(6)

式中，z1和z2分别为该特征点在两帧相机坐标系下深度，根据目标空间点与两帧图像对应的相机光心共面的对极几何关系，可以得到下面的对极约束：

(7)

其中:t∧表示位移向量t对应的反对称矩阵，记本质矩阵E=t∧R。可利用五点法[18]来估计E，然后通过奇异值分解恢复出旋转矩阵R和带尺度的平移向量ts，在实际操作中，通常将ts进行归一化，即不论前面两张图片对应的相机运动中的平移量为多少，都认为相机平移了距离1，并以此为基准计算特征点坐标和更新后续的相对位姿。

然后根据三角测量原理求得两个深度z1和z2，再根据式(6)，就得到了该特征点空间坐标P，根据此方法可求得个特征点的带尺度的空间坐标Psi，(i=1,2,…,n)。此时求得的R是我们最终要得到的旋转矩阵，但是向量ts与坐标Psi的尺度都是模糊的。

将微胶囊分为3份相同质量的样品和硅胶放在一起,置于4、25和40 ℃的条件下避光贮藏,于0、10、20、30 d后,测定其精油残留量,观察精油的释放规律,以此可验证预期寿命的准确性。若初始精油量为Mo,精油残留量为M,产品保留率公式如下:

2.4 相机位姿与特征点空间坐标的尺度恢复

(8)

此时已知匹配特征点的像素坐标和对应的世界坐标，可利用EPnP(efficient perspective-n-point)算法[19]求解世界坐标系下的相机外参，即旋转矩阵R和平移向量t。

此时第i个特征点的初始三维坐标为Pi，相机的第二帧初始外参数为[Rt]，选取激光测距点为非合作目标本体坐标系原点，且目标本体坐标系的坐标轴方向与第一帧相机坐标系平行，则相机在第一帧与目标初始相对姿态用ZYX旋转的欧拉角表示为[0,0,0]T，初始相对位置为[0,d,l]T。

3 连续相对位姿估计

3.1 构建紧耦合形式的联合代价函数

(9)

上式中的RkPi+tk和TkPi中，Pi分别是非齐次坐标和齐次坐标。在第k帧可使用EPnP算法求解相机外参Tk，然后使用三角测量更新特征点三维坐标，但是该方法只使用当前帧的信息，随时间推移存在估计漂移，累积误差会越来越大，且没有充分利用激光测距仪数据。为了解决估计漂移问题，充分利用激光测距仪数据和所有帧数据，本文在上述求解的相机外参和特征点三维坐标基础上，基于光束法平差(BA，bundle adjustment)优化算法，根据特征点和激光测距点的估计值与观测值构建紧耦合形式的联合代价函数，然后调整相机位姿最小化该代价函数，调整结束就得到优化后的相机位姿。

记自变量x=[1,...,m]T为待优化的变量，其中k为第k帧时相机外参对应的李代数。记为第i个特征点在第k帧图像中的观测值，为其估计值，pi为第i个特征点世界坐标，只考虑目标特征点时，有以下代价函数：

(10)

只考虑激光测距仪时，有以下代价函数：

(11)

(12)

因为特征点数量n远大于1，所以在代价函数式(12)中，激光测距仪信息相对于图像信息权重占比极小，可以忽略不计，使得式(12)解算的结果同式(10)基本一致。为充分利用激光测距仪数据，考虑增加激光测距仪数据权重，优化后构建最终的联合代价函数为：

(13)

3.2 联合代价函数求解

对于式(13)，需要求解自变量x=[1,...,m]T，使得该代价函数最小。可以从EPnP算法给定的初值开始，不断地寻找自变量x沿代价函数梯度下降方向的增量Δx，使得f(x+Δx)更小，直到某个时刻自变量x的增量Δx非常小，无法再使函数下降，此时算法收敛，得到优化后的自变量x，具体数学推导过程如下。

因为代价函数中有关激光测距仪的观测误差与无关，则可将代价函数变成以下形式：

(14)

(15)

(16)

然后可继续将代价函数简化为：

(17)

求上式关于Δx的导数，并令其为0，J为雅克比矩阵，可构造关于Δx的增量方程为：

JTJΔx=-JTe

(18)

不断迭代求解该增量方程直到Δx足够小，无法再使函数下降，则得到了优化后的相机外参。

4 实验结果与分析

为了验证本文算法的有效性，利用Blender软件建立空间非合作目标的三维模型，并生成该模型投影在单目相机中的序列图像。

假设空间非合作目标是一个失效卫星，该卫星主体是一个高1.88 m，宽1.14 m的八棱柱，两个太阳能电池板平行于主体棱柱的轴线在主体两侧展开。设置光源为无限远处点光源，光线方向为[1,1,0]T，来模拟空间中太阳光照。任务航天器的单目相机焦距f=0.069 8 m，视场角为28.91°×28.91°，图像分辨率为800 pixels×600 pixels。在世界坐标系下任务航天器的初始位置为[0,0,0]Tm，初始姿态为[0,0,0]T度，空间非合作目标的初始位置为[0,0.1,24.43]Tm，初始姿态为[0,0,0]T度。

令服务航天器相对空间非合作目标做绕飞观测运动，飞行半径为25 m，相对姿态以ω=[0,-1,0]T°/S的角速度绕世界坐标中的Y轴旋转，相对线速度为0.436 3 m/s，图5给出了该相对运动的示意图。服务航天器的相机采样速率为1帧/秒，共采集100帧序列图像，并在采样图像中添加高斯噪声。假设激光测距仪采样速率与相机采样速率相同且同步，激光测距仪输出的测量值为真实距离值加上高斯噪声。图6为利用Blender生成的部分图像，由第一帧开始，从上到下，从左到右依次间隔30帧。

图5 相机与目标间相对运动

图6 空间非合作目标仿真图像

选择前两帧图像使用本文提出的方法初始化，图7给出了初始化时图像的特征检测与匹配结果，将前两帧灰度图重合，红色圆圈是第一帧上检测到的SURF特征点，绿色加号是第二帧上检测到的SURF特征点，可见匹配效果很好，没有误匹配，两帧之间特征点移动极小。

图7 特征点检测与匹配

为充分验证初始化算法的有效性和鲁棒性，初始化仿真100次，结果如表1、图8和图9所示。分析可见本文提出的初始化方法的精度较高，相对位置估计的均方根误差不超过0.02 m，最大误差不超过0.07 m，相对姿态估计的均方根误差不超过0.03°，最大误差不超过0.10°。

表1 初始化时相对位姿估计误差

图8 初始化时相对位置估计误差

图9 初始化时相对姿态估计误差

在初始化之后继续用后续图像序列进行连续位姿估计，为充分验证算法有效性和鲁棒性，连续进行5次仿真实验，实验结果如表2、图10和图11所示。分析可见本文提出的连续位姿估计算法的精度较高，相对位置估计的均方根误差不超过0.12 m，最大误差不超过0.50 m，相对姿态估计的均方根误差不超过0.29°，最大误差不超过1.02°。

表2 连续测量时相对位姿估计误差

图10 连续相对位置估计误差

图11 连续相对姿态估计误差

以上的结果能表明本文提出算法能很好地估计服务航天器与空间非合作目标间的相对位姿，此外，为了评估算法的实时性，平均100次初始化用时和平均100帧连续位姿估计用时，可得初始化平均用时为0.07 s，连续位姿估计时平均每帧图像用时为0.12 s，这说明了该算法能满足实时要求。

5 结束语

针对空间任务中服务航天器实时估计空间非合作目标的相对位置和姿态这一难题，提出了一种融合单目相机和激光测距仪的空间非合作目标相对位姿紧耦合估计方法。该算法分为初始化和连续位姿估计两部分，在初始化部分恢复了所有参数的真实尺度，构建了真实尺度下的各个坐标系；在连续位姿估计部分，以紧耦合的形式融合相机数据与激光测距仪数据来优化相对位姿，即激光测距仪的信息不仅用于实时优化特征点三维坐标，还与图像信息一起构建目标函数来优化全局位姿，解决估计漂移问题。最后使用Blender软件仿真了任务航天器以25 m的半径相对空间非合作目标做绕飞观测的图像，以此对算法进行验证。结果显示初始化时相对位置和姿态估计的均方根误差分别不超过0.02 m和0.03°，连续位姿估计时相对位置和姿态估计的均方根误差分别不超过0.12 m和0.29°。初始化平均用时为0.07 s，连续位姿估计时平均每帧图像用时为0.12 s。表明该算法估计精度与实时性较高，理论上可以为自主交会、空间态势感知、空间碎片清理等空间任务提供所需的空间非合作目标位姿信息。