一种融合外部姿态信息的SLAM算法

李醒飞，李婧娴，郭凤志

(天津大学 精密测试技术及仪器国家重点实验室，天津 300072)

近年来，SLAM 技术由于其环境感知和自我定位能力，成为了自主导航领域的研究热点[1]。降低估计误差、提升系统稳定性一直是SLAM技术发展的方向。目前，SLAM 技术已从基于概率和统计方法[2]，发展至基于非线性优化的图优化方法。在苛刻条件下，SLAM 系统的累计误差成为了限制定位和建图精度的关键因素，严重影响了SLAM 系统的稳定性。尽管视觉已经摆脱了轮速计较为严重的漂移问题[3]，但机器人在长期运行过程中，视觉位姿和地图的不确定度增大，误差逐步累积依然会影响系统的稳定性。

对此，研究学者倾向于采用多传感融合的方案加以解决。如2018 年香港科技大学秦通等人实现了视觉与IMU 的紧耦合位姿估计[4]，夏琳琳等人则在融合IMU 基础上又结合了光流-线特征方法[5]，另外张福斌等人避免了IMU 二次积分的距离误差，转而融合里程计与陀螺仪降低累积误差[6]，王立玲等人则额外融合了IMU 和关节编码器实现机器人的自主定位[7]。P.Gakne 等人设计了基于GNSS 与空天相机紧耦合的位姿估计算法，定位精度有明显提升[8]。此外，还有学者利用地磁传感器获得实时测量的地磁序列修正SLAM 的姿态漂移[9]。但这些方法受制于使用时间或者环境，无法在特殊场景下校正累积误差。

为在严苛条件下能够降低累积误差，SLAM 系统需要找到准确可靠且不产生累计误差的传感器或传感系统。在导航和定位领域，寻北仪作为直接准确测量地理真实姿态的设备，已被广泛应用于导弹发射等军事领域以及采矿、测绘和天文学等民用领域[10]。故在本文中使用自研捷联式寻北仪与靶标构成外部测姿系统，为SLAM 系统在严苛条件下提供地理真实姿态信息作为姿态约束，从而有效降低SLAM 系统的累积误差。

本文针对地下、室内等特殊应用场景，提出了一种融合外部姿态信息的SLAM 算法。相机在采集过程中通过标定靶获得外部姿态信息，在解算图像帧的相对位姿不确定度后，通过稀疏的外部姿态信息和密集的图像帧建立了融合姿态信息的姿态图和全局优化模型，在特殊应用场景下依然能够有效降低SLAM 系统的累计误差，提高SLAM 系统的稳定性，对机器人定位和建图具有重要意义。

1 框架搭建

为降低SLAM 系统的累计误差，在原有框架中加入外部姿态信息，以连续三帧相机位姿和两个路标点为例，可构建如图1 所示的贝叶斯网络。由图1 可知，除原本的起始点和相机约束外，测姿系统的观测量z5和z6为相机位姿添加了额外约束关系，通过约束关系中的姿态信息可对相机位姿和路标点位置进行优化，以减小姿态和位置的漂移。

图1 贝叶斯网络Fig.1 Bayesian network

在实际应用场景下，测姿系统的观测量较图像观测量显著稀疏。此外，为保证姿态的测量与相机运动过程不冲突，测姿系统和相机相互独立。由于测姿系统能便捷地获取地理真实姿态信息，相机可在需要校正时拍摄测姿系统的靶标，通过视觉进行位姿传递，获取相机姿态。x 为相机位姿，p 为路标点位置，z 为观测值，其中z0为固定的起始点，z1～z4为相机观测量，z5和z6为测姿系统观测量。

2 融合外部姿态信息的SLAM 算法

2.1 协方差估计算法

相对姿态的协方差矩阵描述了相对姿态的不确定度大小，外部姿态信息相比于图像关键帧十分稀疏，两者比值可达到两个数量级，但前者给出的是世界坐标系下不含累积误差的三维姿态，后者则是通过相对位姿的迭代获取世界坐标系下六维姿态。由于前者不含累积误差，得到的相机相对姿态的协方差行列式远小于后者姿态的协方差行列式，这为优化方程的参数设计提供了参考。另一方面，对于后者不同关键帧间的相对姿态的协方差矩阵也各不相同，因此，为能够根据不确定度进行准确的姿态图优化，需要估计姿态图优化中各个边的协方差。

1）相机连续帧的协方差估计

在前段的跟踪环节，非线性优化问题的量测方程可写为含有高斯噪声的形式：

式中，x 是相机位姿， f （x ）是量测方程，y 是被观测量， N （0,Σ ）表示高斯噪声， y 具有无偏高斯噪声，噪声协方差矩阵为半正定矩阵Σ 。因此，在观测量为y 的条件下，x 的最大似然估计为：

式(2)实际是Mahalanobis 距离的平方[11]。当f （x）在 x*处的雅克比矩阵 J（ x*）是列满秩时，相机位姿的协方差矩阵为：

2）组合位姿协方差估计

在关键帧非线性优化模型中，需要考虑两关键帧之间的相对位姿的不确定度，进而获得用于姿态图优化的信息矩阵。本文通过递推关键帧间的相机位姿来计算得到关键帧间的相对位姿不确定度估计。首先，给出连续两帧相机姿态，分别为：

因此，当3D-2D 匹配点数目较多时，协方差矩阵Σ1和Σ2的行列式较小，对于连续两帧的组合相机位姿，可以直接保留二阶项用于估计组合相机位姿的协方差矩阵：

对于连续多次的相机位姿组合问题，推导可得边（m, n ）的相对位姿不确定度：

2.2 姿态图算法

1）姿态图优化

考虑到常用的位姿图优化需要六自由度的信息，由于地理姿态信息不含对相机位置的约束，因此推导融合地理真实姿态的姿态图优化算法。首先，定义相对姿态李群表达形式如下：

将其表达为李代数形式：

误差表达式为：

对式(12)添加左扰动，可得：

根据恒等式：

继续整理可得：

式中：

式(16)和式(17)即为姿态图优化的雅克比矩阵子块，将其带入高斯牛顿方程可迭代求解相机的姿态。在获得用于姿态优化的雅克比矩阵子块后，给出误差方程表达式：

式中，雅克比矩阵 J（ x ）为：

式中，M 为图像共视关键帧所组成的边的个数；边的集合为MΘ ，m 为序号下标；N 为测姿帧所组成的边的个数；边的集合为NΘ ，n 为序号下标。且满足：

式中，（ i ,j ）∈ΘM以及（ p ,q ）∈ΘN，下面给出误差项的协方差矩阵：

式中，oΣ 为对角分块矩阵。通过非线性优化方法可迭代求解关键帧姿态。

通过融合地理真实姿态的姿态图优化算法，不仅能够在运算中避免计算大量路标点，提升优化效率，还能够有效避免路标点本身所具有的累积误差对于相机位姿估计和路标点位置的影响，从而正确利用地理真实姿态准确校正各个关键帧的相机位姿和路标点位置，避免直接进行全局优化而导致相机位姿和路标点位置收敛于局部最小值。此外，进一步考虑到姿态图优化后的相机位姿和路标点的位置还存在一定的偏差，需要在下一步进行全局优化以获得更加准确的估计结果。

2）全局优化

在全局优化中，相机位姿和路标点位置同时作为优化变量，路标点重投影误差和测姿帧误差表达式分别为：

由此，可以将姿态图优化后的相机位姿和路标点位置作为初始值，利用雅克比矩阵求解高斯牛顿方程，实现相机位姿和路标点位置的全局优化。

3仿真分析及试验结果

3.1 仿真分析

为验证算法的有效性，本文利用标准数据集KITTI中编号为00的数据集中部分数据进行仿真分析，该数据集提供了真实位姿，仿真中提取部分关键帧的真实姿态作为寻北系统所检测到的地理真实姿态信息。

图2中，X 轴为数据集序列号，Y 轴为融合前后的绝对轨迹误差（AbsolutePosition Error,APE）。可以看出，当相机运动到拐弯处，即关键帧序号为35帧之后时，误差变化剧烈，尽管在(40,50)阶段APE在减少，实际上是由于姿态发生了严重的漂移而导致相机估计轨迹与实际轨迹在三维空间形成了较大的相交角度，因此在接近区域误差减小，但此后误差逐渐增大；在融合优化后，如蓝色曲线所示，相机的位置误差显著减少。

图2 融合优化前后的绝对位置误差Fig.2 Absolute position error beforeand after fusion optimization

此外，相对位置误差能够有效体现相邻关键帧位置估计误差的变化情况，图3给出了相同序列上关键帧相对位置误差和相对位姿协方差行列式对数值。其中，蓝色圆圈标记了相对位置误差较大的点，绿色圆圈标记了相对位置协方差较大的曲线段，可以看出两者基本呈负相关，即协方差大的位置，相对位姿误差较大，证明了协方差估计的合理性。通过在姿态图中融入协方差进行优化，能够正确设置图优化中边的权重，实现累积误差的有效消除。

图3 关键帧相对位置误差和相对位姿协方差行列式对数值Fig.3 Relative position error of key frameand relative pose covariancedeterminant log value

仿真分析表明，在没有回环检测环节参与的情况下，经过融合优化后相机位姿的绝对轨迹误差下降至优化前的30%以下。此外，根据实际测量需求可以降低阈值，以获得更好的优化效果。在下一节中，将该算法应用于经典框架ORB-SLAM2以分析算法性能。

3.2 试验结果

3.2.1 室内试验

在室内试验中，将本文算法搭载于ORB-SLAM2，外部测姿系统如图4所示，由转台、光纤陀螺仪和加速度计构成，航向精度在30s内达到0.3°，水平精度在30s内达到0.05°。通过靶标进行姿态传递，相机能够较为准确的获得地理真实姿态。

图4 测姿系统Fig.4 Attitudemeasurement syste m

本试验中，具有寻北信息的图像帧共有5帧。考虑到相机绝对位置在当前实验条件下不易获得，本文利用实验场地处于空间内一个平面这一限制条件分析算法的效果。融合优化前后相机在y 轴方向上的轨迹误差值如表1所示。

从表1可知，融合后各项误差均显著降低，其中均值误差仅为融合前的67.6%，最大值为融合前的68.2%。最后，将y 轴误差呈现在相机运动的xz 平面内，如图5所示。

表1 融合前后相机在y轴上的轨迹误差（单位：米）Tab.1 The trajectory error of thecamera onthe y-axis b efore andafter the fusion(Unit:m)

图5中，右侧色带从蓝色过渡到红色表示轨迹在y 轴的误差逐渐变大，在靠近起点同时也是回环处的误差相对较小，轨迹显示为蓝色，这是由于开始运动时累积误差较小，同时回环检测可以有效降低误差。同时，从两幅曲线图也可以发现，在距离起点较远的位置处，融合前的轨迹误差达到了整个估计值误差中的最大值，这是由于回环检测对距离回环距离较远的位置校正能力十分有限。在通过融合算法后，距离校正点最远的位置曲线颜色从红色变为了黄绿色，同时整个曲线的颜色向蓝色偏移，即全局误差整体显著降低，在一定程度上验证了算法对于减小累积误差的有效性。为进一步准确地评估融合优化前后的误差情况，下一节将融合视觉和地理真实姿态的ORB-SLAM2 算法应用在标准数据集KITTI 上进行实验。

图5 运动轨迹xz 平面的y轴误差Fig.5 The y-axiserror of the xz planeof the trajectory

3.2.2 基于标准数据集KITTI的试验

本节利用KITTI 数据集进行实验，该数据集是国际上最大的自动驾驶场景下计算机视觉算法评测数据集，部分数据集还提供了相机的真实位姿，本实验所需要的地理真实姿态信息来自数据集所提供的真实位姿。为体现在稀疏寻北帧下的优化效果并保证均匀性，每200个图像关键帧添加一次寻北关键帧，以进行融合优化。此外，为验证算法在不同情况下的性能，本实验选取KITTI 数据集中具有多次回环的00号数据集和不存在回环的01号数据集。

1）具有多次回环的00号数据集实验

在本实验中，共添加了6帧寻北关键帧，利用轨迹真实值对融合前后轨迹的估计值进行评定，融合前后误差值如表2所示。

表2 融合前后误差Tab.2 Error before andafter fusion

根据表2，融合后的轨迹误差显著下降，均值下降为融合前的62.9%，标准差下降为53.6%；融合后旋转角误差同样显著下降，均值下降为融合前的55.9%，除标准差微弱上升外，其余各项误差均显著下降。此外，将相机的轨迹误差呈现在运动轨迹的xz平面中，如图6所示，彩色线为估计轨迹，虚线为真实轨迹。

如图6(a)所示，在距离回环检测位置较远的位置，回环检测的校正能力有限，导致误差较大，因此呈现红色；如图6(b)所示，在经过融合优化后，全局的轨迹误差明显降低，误差最严重的区域也从红色变为了黄绿色。综上，对于00号数据集，融合后轨迹误差和旋转角误差明显降低，证明了优化算法的有效性。

图6 运动轨迹xz 平面的轨迹误差Fig.6 The y-axiserror of the xz planeof the trajectory

2）无回环的01号数据集实验

上述实验所利用的KITTI 的00号数据集具有多次回环，为进一步验证算法的综合性能，对KITTI 的01号数据集进行测试，该数据集并未存在回环，因此可以很好检测在没有回环情况下融合算法的性能，其中寻北关键帧共有7帧，融合前后误差值如表3所示。

表3 融合前后误差Tab.3 Error beforeand after fusion

根据表3，融合后的轨迹误差有所下降，最大值下降为融合前的79.7%，标准差下降为80.9%；融合后旋转角误差显著下降，均值仅为融合前的8.8%，标准差仅为16.7%。考虑到旋转角误差下降明显，进一步分析融合算法对姿态角误差的校正情况，姿态角变化情况如图7所示，X轴为数据集序列号，绿线为融合前角度，蓝线为融合后角度，虚线为真实角度。

图7 姿态角Fig.7 Attitude angle

从整体和局部姿态角曲线可知，蓝色曲线与虚线基本重合，而它们与绿色曲线有一定偏离，这代表融合后的俯仰角、翻滚角和偏航角与它们的真实值基本相同，而融合前角度则与真实值有一定差别。因此，融合优化算法显著提升了相机旋转角的估计精度，能够有效降低相机的姿态误差。此外，将相机的轨迹误差呈现在运动轨迹的xz 平面中，如图8所示，彩色线为估计轨迹，虚线为真实轨迹。

图8 运动轨迹xz平面的轨迹误差图Fig.8 The trajectory error of the xz planeof the motion trajectory

如图8(a)所示，在融合前，轨迹末端（右下角所示）明显偏离于轨迹真实值，而如图8(b)所示，在经过融合优化后，轨迹曲线中代表误差最大的深红色变为了橙色，全局的轨迹误差有效降低。综上，对于不存在回环的01 号数据集，融合后轨迹误差在一定程度上被减小，姿态角误差几乎被消除，证明了融合优化算法在数据集不存在回环情况下对于减小累积误差的有效性。

4 结 论

本文设计了融合外部姿态信息的SLAM 算法，以消除系统的累积误差。为有效利用真实位姿信号减小累积误差，设计了融合视觉和地理真实姿态的SLAM算法架构。通过标定靶传递外部姿态信息，推导了姿态组合后的不确定度表达式，并提出了姿态图和全局优化的算法，获得了校正累积误差后的全局最优轨迹和地图。实验验证了融合视觉和地理真实姿态对于累计误差消除的有效性。

考虑在室内建模、地下救援、灾后重建等场景下，常规传感器易受到环境的制约，无法稳定的消除SLAM 系统中的累积误差，导致系统在长时间工作过程中定位和建图精度严重下降，甚至失效。本文提出的视觉和外部姿态测量系统融合方案，则能在这些严苛环境下依然为SLAM系统提供真实准确的外部姿态信息，同时通过算法解决了外部姿态信息相比于图像帧过于稀疏而带来的优化难题，对SLAM 的应用领域拓展具有重要意义。

此外，本文所使用的测姿系统体积较大，不能灵活移动，会在一定程度上限制SLAM 系统的应用，且室内实验未能提供用于评估误差的完整的相机运动轨迹。未来计划选型其他更加便捷的测姿系统，并利用激光跟踪仪等设备获取相机的真实轨迹，开展进一步验证工作。