基于先验子图检测的失效航天器SLAM方法

康国华，马云，乔思元，郭玉洁，金晨迪

南京航空航天大学微小卫星研究中心，南京210016

失效航天器的位姿估计技术是当前在轨服务的研究热点。失效航天器属于非合作目标，通常没有先验的信息、可供辨识或捕获的机构;没有姿态控制能力，在重力梯度、光压等空间摄动力作用下处于翻滚状态;因碰撞等故障，在周围通常伴有碎片等干扰。在这种空间场景下，服务航天器需要估计出失效航天器的相对位姿信息，对环境进行探测和建模，从而为后期的导航、控制等在轨服务提供信息，具有重要的研究意义［1］。

上述情形可以简单描述为在未知环境中，任务主体自身处于运动的同时，开展一系列导航任务。这类难题，在地面移动机器人和无人机领域已经获得广泛而深入的研究，它可以采用同步定位与环境构建(Simultaneous Localization and Mapping，SLAM)技术来解决［2-5］，即利用传感器不断地采集外界环境数据，在移动中通过数据处理实现对自身位置的估计，并实时更新环境地图。

近年来，国内外开始有学者把SLAM方法应用到空间和水下未知运动目标的位姿观测和估计问题，做了许多值得借鉴的工作。Augenstein等［6］在研究中针对目标的六自由度姿态和三维构型问题采用了一种递归估计算法，也称作基于FastSLAM的启发式姿态估计与重构算法。该方法不需要目标的先验信息，仅仅利用单目相机对图像处理和提取特征，和扩展卡尔曼滤波(Extended Kalman Filter，EKF)算法相比，采用的粒子滤波算法的测量更新效率更高，并且不受单峰概率的限制。在此之后Augenstein等［7］又提出一种实时的帧间姿态估计方法，它结合贝叶斯算法和测量反演方法，估计出了移动机器人平台和翻滚目标之间的相对姿态，并进行地图的构建。Schnitzer等［8］在研究中模拟了一种未知非合作的目标物体，在对SLAM提取的稀疏点云采用随机一致性采样(Random Sample Consensus，RANSAC)处理后，对非合作目标的三维构型进行了重建，为后期的视觉导航处理任务提供先验信息。Cho等［9］利用视觉对目标物图像的特征进行检测，在目标丢失的情况下，提出了一种鲁棒性强的特征配准算法，并基于因子图的概率图模型进行平滑估计，计算出位置变化和六自由度的状态。Tweddle［10-11］在研究中分析了空间非合作目标的特点，阐述非合作目标本身的线速度、角速度、惯性主轴、质心等信息对近距离捕获任务具有重要意义，并且针对典型SLAM算法不适用于高速旋转物体问题，提出一种解决围绕任意轴旋转的非合作目标的SLAM算法。该方法依据牛顿第二定律和欧拉转动方程，对运动物体未来的状态进行预测，把这些信息作为因子，再采用增量式平滑的优化方法对位姿进行估计，最后通过试验评估算法的收敛性和精度。Thomas等［12］研究了一种单目SLAM算法，在更新相机的位置和姿态的同时，对目标的三维特征进行跟踪，用于轨道上卫星的近距离自主捕获任务。郝刚涛等［13］将视觉SLAM的滤波模型应用到失效非合作航天器的位姿量测，融合无损卡尔曼滤波和粒子滤波算法对目标的位姿进行估计，仿真结果表明该算法实现了较优的精度和速度。

上述研究相对集中在视觉SLAM方面，且设置的应用场景相对简单，然而太空环境中光照变化剧烈，导致视觉SLAM算法的适用范围受到限制，并且失效航天器通常处于自由翻滚运动，其周围充斥着太空垃圾或碎片等障碍物的干扰。考虑上述诸多因素的空间应用场景下的SLAM算法的研究相对较少，值得进一步的研究。本文选用激光雷达作为传感器，研究相对复杂的应用场景下的图优化SLAM。

1 图优化SLAM和先验子图检测

1．1 通用的图优化SLAM方法

SLAM问题，也就是通过服务航天器搭载传感器，在移动过程中，对服务航天器的位置姿态进行实时估计，同时绘制出符合任务需求的环境地图。

当服务航天器在未知环境中移动时，它的轨迹可以由离散的变量 x1，x2，…，xk来描述，运动传感器(惯性测量单元)的数据称为输入值，用u1，u2，…，uk表示，传感器的噪声用 ωk表示，运动方程可以表示为:

服务航天器在位置xk对环境中路标(包括失效航天器、碎片等)lj的观测量用zk，j表示，观测数据的噪声用vk，j表示，观测方程可以表示为:

把SLAM问题建模成状态估计问题，从上述联立的方程组中估计出状态变量。状态估计问题的求解有很多种方法。目前主流的图优化方法在解决状态估计问题时具有优越的性能［14］。本文采用图优化方法，并用因子图进行描述［15］。

通用的图优化SLAM框架如图1所示。

图1 通用的图优化SLAM框架Fig.1 General SLAM scheme of graph-based optimization

通用图优化SLAM框架的主要组成部分如下:

1)传感器数据。激光雷达采集空间环境信息生成点云，惯性传感器对服务航天器自身运动信息进行测量。

2)顺序数据关联。对激光雷达扫描到的相邻帧进行配准，结合惯性传感器的数据，估计出服务航天器在相邻时刻的位姿变换关系。

3)回环检测。回环检测判断服务航天器是否到达过先前的位置，它可以通过多个节点之间的约束来判断，通常是一种强约束。假如检测到该约束信息，可以用于位姿优化。

4)位姿图优化。后端接受不同时刻估计出的服务航天器的位姿，以及回环检测的约束信息，找到平衡所有约束的最优位姿配置解，从而得到全局一致的轨迹。

5)构建地图。用合适的地图来表示空间环境模型信息。

将通用的图优化SLAM应用到空间失效航天器的应用场景中。本文选取空间长期失效航天器作为服务对象，服务星在绕飞阶段进行同步定位与地图构建，估计相对位姿，为后期的导航、规划和近距离操控等任务提供有效信息。

1．2 先验子图检测

图优化SLAM要求应用场景相对于参考坐标系处于静止状态。而空间场景中待观测的一部分路标(失效航天器)处于缓慢运动状态，导致位姿估计在长时间运行后产生累积误差的问题。为了解决该问题，从失效目标航天器的自身运动特点着手，引入先验子图检测法。

根据对失效目标航天器运动特性的分析，它通常围绕惯量主轴处于缓慢旋转的运动状态［16］。存在能量耗散的航天器，当航天器章动时，内部可活动质点的运动随着能量的损耗，趋向于最小动能状态;由于角动量守恒，航天器的动能不可能全部消失，在最小动能状态下，航天器运动不再引起能量损耗，将围绕某一主惯量轴稳定自旋。因此，空间长期失效航天器经过长年累月的能量损耗，其稳定状态是围绕最大惯量主轴缓慢自旋，或具有很小的章动角。

为简化问题，考虑处于缓慢自旋的失效航天器。这类失效航天器的运动处于周期性变化。假定服务航天器携带的激光雷达在初始时刻t0观测到的失效航天器的姿态如图2所示，然后失效航天器缓慢自旋一周后，在特定时刻tk的姿态和初始时刻t0的姿态保持一致，这两个时刻失效航天器和空间环境碎片的位姿保持不变，即处于相对静止状态，服务航天器在时刻tk和初始时刻t0相当于对静态场景进行观测，通过对这两个时刻观测到的点云进行配准，可以获得服务航天器在不同时刻的位姿之间的约束关系。

图2 先验子图检测示意Fig.2 Sketch of prior submap detecting

先验子图是指具有先验信息的关键帧。将初始时刻t0激光雷达观测到的点云记为先验关键帧，当服务航天器携带激光雷达的移动过程中，把观测到的每个关键帧和先验关键帧进行点云配准，如果满足配准要求，则确定激光雷达在不同时刻的位姿关系，相应地，通过坐标变换获得服务航天器之间的位姿约束信息。

可以将检测出来的位姿关系添加到位姿图，记为不连续节点之间的约束因子。由于这一过程利用了初始时刻的先验关键帧以及失效航天器的运动特点，将这种方法称为先验子图检测法。

2 基于先验子图检测改进的图优化SLAM算法

2．1 总体框架

基于先验子图检测改进的图优化SLAM框架，如图3所示，其中，先验子图检测是框架的重要环节。

图3 基于先验子图检测改进的图优化SLAM框架Fig.3 Improved SLAM scheme of graph-based optimization based on prior submap detecting

先验子图检测，和回环检测的目的类似，利用不连续的位姿节点之间的约束关系，对位姿图进行调整。由于长期失效航天器处于缓慢自旋，它的运动状态呈现出周期性重复的先验特点，根据这一特点，采用先验子图检测算法获得位姿节点间的约束信息用于图优化。

2．2 前端位姿图构建

位姿图构建过程如图4所示。

通过将传感器获得的点云联系起来，从而在位姿节点间构成约束关系。由于激光雷达是固定在服务航天器上，当空间环境相对于参考坐标系近似于静止时，两帧激光数据之间的相对位姿变换，就对应位姿图中的边，从而构建位姿联系。

具体步骤如下。

(1)特征提取

前端的任务是建立激光雷达的位姿图，它根据相邻帧的点云来估计出粗略的激光雷达运动，给后端提供较为不错的初始值。

图4 位姿图构建过程Fig.4 Procedure of constructing pose graph

特征是指在激光雷达运动之中保持稳定的点，它通常表现为地图中的路标。

通过激光雷达扫描到的点云，估计出法线向量，然后对近邻点进行查找和判断，提取出边界点作为特征点。

当给定向量空间M中点集P={p1，p2，…，pn}与待搜索点q∈M，那么q的相近元素N(q，P)∈P。搜索查询点近邻点，统计其法线向量，找到边界点。

(2)相邻帧的运动估计

采用迭代最近点(Iterative Closest Point，ICP)方法求取相邻帧的位置和姿态变换。

假设相邻帧的边界特征点构成的子集为P ，Q，相应的点为 pi，qi，则有 P={p1，p2，…，pn}和 Q={q1，q2，…，qn}。把 ICP 方法计算相邻帧的旋转分量R以及平移分量t这个问题转换为求解一个最小二乘问题，

式中:minf(R，t)为误差大小;ωi为每一对的匹配点在整体中所占的权重。

ICP问题可以通过非线性优化的方法求解，目标函数可以表示为:

在计算过程中，通过不断迭代直到函数值取得极小值。

(3)构建位姿图

位姿图中的节点代表的是激光雷达的位姿，连接节点的边则代表激光雷达从前一时刻到下一时刻进行的运动变换，由一连串变换序列绘制服务航天器的位姿图。

2．3 后端位姿优化

基于先验子图检测的后端位姿优化如图5所示。

(1)全因子图模型

令服务航天器在时刻ti的位姿用xi表示，服务航天器在当前时刻k的状态集用Xk表示，

图5 基于先验子图检测的后端优化过程Fig.5 Procedure of backend optimization based on prior submap detecting

对联合概率密度函数因式分解，则联合分布函数为:

式中:操作符d(·)定义为消耗的代价函数。

对于高斯噪声分布，通用的因子可以表示为:

若因子表示的是量测模型，那么

式中:h(·)为量测函数，通过它可以从给定的状态预估出传感器的量测值。

根据式(5)～(9)可知，状态变量的最优估计也就是让下式最小化:

根据不同的量测模型，对本文用到的因子进行描述。全因子图模型如图6所示。

1)Prior因子。对于特定的状态变量有先验信息ν，于是Prior因子为:

2)IMU因子。服务航天器的状态变量x可以用连续的非线性微分方程表示:

图6 全因子图模型Fig.6 Factor graph model

式中:fb，ωb是惯性测量单元在本体系下测量的力和角加速度。，式(12)对应的离散方程为:

根据式(13)，可以通过状态xk和传感器测量值预测出下个时刻的状态x，而预测值和实k+1际的xk+1存在的差值体现为IMU因子的误差函数:

3)Lidar因子。激光雷达的量测方程可以表示为:

式中:路标l是全局参考系的坐标;旋转矩阵R和平移向量t表示激光雷达坐标系和全局参考系的转换关系。Lidar因子为:

4)约束因子。变量节点包括服务航天器状态和路标节点。在不连续的变量节点之间存在约束关系，可以用于优化。假定在初始时刻t1和时刻tk存在约束关系，量测方程可以表示为:

式中:hConstr为约束节点之间的量测函数;nConstr为量测噪声。约束因子可以表示为:

在图6中，约束因子的连线用虚线表示，约束因子fConstr包括先验子图约束因子fSubmap和闭环因子fLoop两类。

(2)先验子图检测方法

本文采用RANSAC算法来实现先验子图检测方法。RANSAC算法通过迭代的方式来估计数据中的模型参数。估算的数据集合包括有效点(Inliers)和无效点(Outliners)，其中有效点处于所需的数学模型附近，异常点可能是一些环境干扰、测量误差等产生的。

本文采用的是多点RANSAC算法。具体过程如下:

1)在初始先验关键帧中，选择若干参数点d(特征点)，失效目标航天器m，周围环境n，总共m+n参数点;将待匹配帧的相关联的参数点，通过平移与旋转计算出对应的d'，再把d'和d相对比，若误差小于一定的阈值，视为有效参数点。

2)将所有对应参数点进行上述的运算，如果有效参数点的数量满足指定的要求，那么判断两帧匹配成功，并且上一步中计算出的平移向量和旋转矩阵有效。

3)若2)中未达到足够有效点，视为无效。

在RANSAC算法中，如果当前帧和先验关键帧的点云配准成功，计算出的旋转矩阵和平移向量表示了激光雷达和失效航天器等环境信息之间的相对位姿关系，然后计算出服务航天器在当前时刻和初始时刻的位姿之间的约束信息。这一信息可以作为节点之间的具有较强约束的边，用于局部位姿图的调整和优化。

(3)图优化

通过前端顺序数据关联构建出位姿图之后，依据先验子图检测得到局部节点的约束信息，回环检测获得环中大范围内节点的约束关系，就可以对位姿图进行优化。

对于因子图，因子节点处于待优化的位姿节点之间，包含了位姿节点之间的约束关系，它们来自于运动方程和观测方程。例如，顺序数据关联，可以用惯性测量单元因子来描述相邻时刻位姿之间的约束关系;先验子图的检测，获得不同的局部节点的制约关系;回环检测则获得环中更多节点的制约关系。对因子图的优化，就是调整各个变量的值，使得它们的因子乘积最大化，可以采取增量平滑与地图构建方法(incremental Smoothing And Mapping， iSAM) 来 优 化［17］。

SLAM数据关联和位姿优化的伪代码如下:

1 Factor_Graph←InitializationNodesFactors()

2 for keyframe KP_k do

3 Features(k)←ExtractFeatures(KP_k)

4 {Rk，Tk} ← ICP(Features(k)，Features(k－1))

5 Curr_Pose← NewPoseNodeAndFactor(Rk，Tk)

6 Factor_Graph←Curr_Pose

7 {R，T} ← {R，T，Rk，Tk}

8 end for

9 {R，T} ← iSAM_Smooth(R，T)

10 for keyframe KP_k do

11 {Rs_k0，Ts_k0，Flags_k} ← SubmapDetect_RANSAC(KP_k，KP_0)

12 if IsPriorSubmap(Flags_k)then

13 Factor_Graph←NewContraintFactorAndNode(Curr_Pose，Rs_k0，Ts_k0)

14 end if

15 {Rloop_k0，Tloop_k0，Flagloop_k} ← LoopDetect(KP_k，KP_0)

16 if IsLoop(Flagloop_k)then

17 Factor_Graph←NewContraintFactorAndNode(Curr_Pose，Rloop_k0，Tloop_k0)

18 end if

19 Factor_Graph←iSAM_Update(Factor_Graph)20 end for

3 仿真结果与分析

试验系统环境采用的是机器人操作系统(Robot Operating System，ROS)，版本为 Indigo，以及仿真开发环境MATLAB 2015b。

仿真试验过程主要为:

1)仿真环境搭建。通过ROS搭建系统环境，同时创建任务节点，ROS主机和MATLAB节点通过局域网连接并通信;利用Gazebo搭建空间环境模型，激光雷达扫描模型生成点云数据，模拟惯性测量单元的数据，并通过ROS以话题的方式发布。

2)核心部分。SLAM算法，ROS主机订阅话题获取传感器数据，进行位姿图构建、先验子图检测、回环检测和后端位姿图优化等操作。

3)位姿轨迹的显示。ROS主机将位姿图(服务航天器的轨迹)数据发布出来，MATLAB节点订阅该数据并进行结果显示和分析。

在绕飞阶段服务航天器对失效航天器进行环境建模，SLAM算法应用场景如图7所示，包含有失效卫星模型和碎片障碍物。

图7 SLAM算法应用场景Fig.7 Application scenario of SLAM algorithm

仿真参数设定如下:

1)失效航天器本体的尺寸，长、宽、高分别为220cm、180cm、240cm，太阳翼的长、宽分别为600cm、160cm，太阳翼的三角支架和翼板相接的边为160cm，高为130cm。

2)失效航天器绕着最大惯量主轴处于缓慢自旋状态，旋转角速率为5(°)/s。

3)失效航天器周围伴随若干碎片障碍物，障碍物在参考坐标系下保持相对静止。

4)在参考坐标系下，初始时刻的服务航天器位于(0，0，0)，失效航天器位于(25，0，0)。

5)服务航天器在失效航天器同一轨道平面内绕飞，绕飞半径25m，理想轨迹如图7所示。

SLAM算法的仿真轨迹如图8所示，绿色点划线表示通用SLAM算法估计出的轨迹，紫色实线表示基于先验子图检测改进后的SLAM算法估计出的轨迹。

图8 SLAM算法的轨迹比较Fig.8 Trajectories comparison of SLAM algorithms

由图8可知，通用的图优化SLAM算法的位姿和轨迹真值相比偏差较大，它是由动态场景下长时间的位姿估计产生的累积误差造成的，仅靠回环检测对位姿优化的效果并不显著;而基于先验子图检测改进后的SLAM算法，通过先验子图检测获得局部的位姿约束信息，对位姿进行后端优化，进一步减少累积误差，改善位姿估计精度。

对比基于先验子图检测改进的SLAM算法和通用的图优化SLAM算法，两种SLAM算法的位置误差比较如图9(a)所示，姿态误差比较如图9(b)所示。从图中可以看出，未改进的通用SLAM算法由于累积误差的影响导致位置和姿态误差较大，基于先验子图检测改进后的SLAM算法，位置和姿态估计精度均有提高，并且位置的改善效果更为显著。

图9 SLAM算法误差比较Fig.9 Error comparison of SLAM algorithms

基于先验子图检测改进后的SLAM算法的位置误差曲线如图10(a)所示，姿态角误差曲线如图10(b)所示，从中可以看出，位置精度优于0．08m，姿态角精度优于 0．6°。

基于先验子图检测改进后的SLAM算法的误差值如表1所示。由表可知，改进后算法的位姿估计误差小，精度较高。

图优化SLAM算法作为星上算法，以空间长期失效航天器为对象。在实际的应用场景中，还需要考虑失效航天器翻滚速度、传感器的测量范围、分辨率、遮挡等因素的约束。还可以根据失效航天器的运动学特性，估计运动变化之间的动力学关系，将动力学信息添加到SLAM因子图，从而将SLAM方法应用到更广泛真实的运动场景。

图10 基于先验子图改进后的SLAM算法误差曲线Fig.10 Deviation curve of improved SLAM algorithm based on prior submap detecting

表1 改进后算法估计误差Table 1 Comparison of improved algorithm for estimation deviation

改进图优化SLAM算法，在应用场景中，提高了精度，位置精度优于0．08 m，姿态角精度优于0．6°，可以满足相对导航任务需求，为后续导航、控制等在轨任务提供重要信息。

4 结束语

本文开展了失效航天器的激光雷达SLAM技术研究。通过对空间长期失效航天器的特点进行分析，在通用图优化SLAM基础上，给出一种基于先验子图检测改进的SLAM方法。仿真试验表明，本文提出的方法获得的航天器相对位姿精度较高，位置精度优于0．08 m，姿态角精度优于0．6°，可满足相对导航任务需求。