广告
基于关键帧的使用非线性优化的视觉-惯导SLAM
2018-12-24 21:37:32· 来源：同济智能汽车研究所 作者：Leutenegger.Stefan
主要作者：Leutenegger.Stefan本文由同济智能汽车研究所编译，转载请在文首注明出处。摘要：视觉和惯性传感器融合因其感知方式的互补性质在机器人技术中已经变得
主要作者：Leutenegger.Stefan
本文由同济智能汽车研究所编译，转载请在文首注明出处。
摘要：视觉和惯性传感器融合因其感知方式的互补性质在机器人技术中已经变得流行起来。虽然目前大多数融合策略依赖于滤波方案，但视觉机器人社区最近已经转向基于非线性优化方法的同步定位与建图（SLAM），并发现这种方法在性能方面具有显着优势和计算复杂性。遵循这一趋势，我们提出一种新的方法，将视觉测量与来自惯性测量单元（IMU）的读数在SLAM中紧耦合。IMU误差项以完全概率的方式与特征重投影误差紧耦合，优化联合非线性代价函数。我们使用“关键帧”的概念将旧状态部分边缘化，以维持有限变量的优化窗口，从而确保系统实时运行。与松耦合的视觉惯性算法相比，我们的实验证实了紧耦合在精度和鲁棒性方面更有优势。
I. 引言
融合视觉和惯性传感器长期以来一直是解决常见机器人任务，如运动估计、视觉测距和SLAM的流行手段。图像数据中丰富的场景特性，和在IMU中陀螺仪和加速度计的精确短期估计特性被认为彼此互补，可用于无人机[6,20]和无人车中[14] ]的导航。 此外，大多数智能手机中这些传感器的普及，引起了对视觉惯性SLAM的研究的极大兴趣。
历史上已经通过滤波方式处理了视觉惯性姿态估计问题，IMU测量被用于预测传播而关键点检测用于更新。Mourikis和Roumeliotis [14]提出了一种基于EKF的单目视觉的实时融合方法，Jones和Soatto [8]在长距离的室外轨迹上提供单目视觉惯性滤波估计结果，其中包括IMU到相机标定和闭环检测。这两个方法表现出令人印象深刻的低于0.5％的距离误差。Kelly和Sukhatme[9]提供了相机标定和基于滤波的vision-IMU融合可观性的研究。偏航角和绝对位置的全局不可观性，和相对于初始参考姿态的不确定性，是视觉惯性估计的本质问题；这对依赖于线性化的滤波方法提出了挑战。
文献[18]表明，相比滤波方法，基于优化方法的纯视觉SLAM在相同的计算条件下提供了更好的精度。因此，保持稀疏的关键帧和关联的地标的图优化方法，已经非常受欢迎。
文献中的视觉-惯性传感器融合方法遵循两种思路。在松耦合系统中，例如文献[10]，陀螺仪数据和相对偏航测量作为独立的IMU观测结合到立体视觉中进行优化。 Weiss等人[20]仅用视觉姿态估计作为IMU传播的EKF的更新。在文献[15,7]中，相对立体姿态估计被集成到包含惯性项和绝对GPS测量的因子图中。这种方法降低了系统的复杂性，但忽略了不同传感器的内部状态之间的关联。相反，紧耦合方法联合估计所有传感器状态。作为滤波方案的替代，Dong-Si和Mourikis [2]提出了一种固定滞后平滑器，其中在固定窗口中维持连续的机器人姿势和相关状态估计，超出范围后边缘化状态[19。类似的方法，也在文献[16]中使用。
出于视觉惯性SLAM系统的精度和鲁棒性，我们提倡紧耦合方法，以最大限度地利用传感器特性，同时使用非线性优化方法而非滤波方法，以减少由于线性化造成的误差。我们的方法灵感来自文献[17]，其中建议在批量优化的SLAM中加入IMU误差项（仅在初始化时）。 我们的方法与[2]中提出的固定滞后平滑器密切相关，因为它在单个代价函数中结合惯性误差项和重新投影误差项，并且通过边缘化旧帧以限制系统复杂性。
我们的工作主要有三个贡献：
- 我们在慢速运动或完全静止时，采用关键帧进行无漂移估计：而不是使用时间上连续的姿势的优化窗口，我们保持关键帧在时间间隔上随机分布，以保持视觉约束的同时仍然考虑IMU项。我们的关键帧的相对不确定性方程让我们可以在不表示全局姿态不确定性的情况下建立位姿图，这是从RSLAM[13]得到的灵感。
- 我们用完全概率的方式推导了IMU误差项，包括相关的连续图像帧间对应的信息矩阵，而没有以IMU数据频率引入精确状态。
- 在系统层面，我们开发了精确实时SLAM方案的硬件和算法，包括鲁棒关键点匹配和通过惯性测量项进行外点剔除。
在本文的其余部分，在II-B中，我们在batch visual SLAM中引入了惯性误差项，随后在II-C中简单描述了我们的实时立体声图像处理和关键帧选择，和II-D中的边缘化形式。 最后，在III中我们展示了立体视觉和IMU传感器在室内和室外获得的结果。
II. 紧耦合的视觉-IMU融合
在视觉SLAM中，非线性优化通过最小化在相机帧中观察到地标的重投影误差来找到相机姿势和地标位置。图1表示相应的图优化拓扑结构：其将测量显示为具有方框的边，估计的量作为节点。一旦引入惯性测量，它们不仅在连续姿势之间产生约束，而且在加速度计和陀螺仪的速度和IMU误差估计之间产生状态增量的约束。 在本节中，我们提出了将惯性测量结合到视觉SLAM中的方法。
图1：在视觉SLAM（左）和视觉惯导SLAM（右）中涉及的状态变量图模型
图2：硬件中涉及的坐标系：两个相机分别代表一个坐标系
FCi，IMU数据在FS坐标系中获得，FS坐标系通过世界坐标系FW估计获得。
A.符号和定义
- 符号：我们在整个过程中采用以下符号：FA表示参考帧A，其中向量被写为pA或pBCA，其中B和C分别为起点和终点。帧间的变换由齐次变换矩阵TAB表示，TAB表示从FA到FB的点坐标变换 ，其旋转矩阵部分写为CAB。我们采用Barfoot等人介绍的符号[1]：四元数乘法qAC=qAB⊗qBC，引入左手侧复合运算符（.）+和右手侧运算符（.）⊕，使得qAC = qAB+qBC=qBC⊕qAB。
- 坐标系：使用图2中描绘的立体相机IMU。在被跟踪物体内部，相对于世界坐标系FW，我们区分相机坐标系FC和IMU传感器坐标系FS。
- 状态：要估计的变量包括图像时间xkR和地标xcL处的机器人状态。xR表示在惯性帧pWSW中的机器人位置，qWS表示旋转四元数，vWSW表示世界坐标系下的速度，以及陀螺仪的偏置bg和加速度计的偏置ba。于是xR被写为：
此外，我们使用分割成姿态状态和速度/偏置状态。 地标坐标以齐次形式表示，如[3]中所示，以便允许接近和非常远的地标的无缝集成。
我们在流形上的切空间g中的扰动采用组运算符，指数映射exp和对数映射log。我们使用最小坐标表示法。双射映射Φ从最小坐标变换到切线空间：
具体来说，我们使用轴角扰动旋转，可通过指数转换成其等效四元数δq：
因此，使用算子⊗，我们获得最小机器人误差状态向量：
我们使用姿态误差状态和速度/偏置的误差状态。我们将齐次地图坐标视为具有最小扰动的四元数δβ，因此：
B.具有惯性条件的批视觉SLAM
我们尝试公式化视觉惯性定位和建图问题，将它表示成包含（加权）重投影误差er和IMU的误差项es的代价函数J(x)的联合优化：
其中i是组件的相机索引，k表示相机帧索引，j表示地标索引。在第k帧和第i个相机中可见的标志的索引被写为集合J(i,k)。此外，W表示相应地标测量的信息矩阵。
纯视觉SLAM需要在优化期间保持固定的6自由度（DoF），即绝对姿势。组合的视觉惯性问题仅具有4个自由度，因为重力使得两个旋转自由度可观察到。我们想要固定围绕重力方向（世界z轴）的偏航角，以及第一帧的位置。因此，除了将位置变化设置为零，我们还假设：
在下面，我们将列出重投影误差公式。之后，给出了IMU运动学与偏置模型，基于此我们得到IMU误差项。
1）重投影误差公式
我们使用标准的重投影误差公式：
这里，hi(·)表示相机投影模型，zi,j,k表示图像坐标系下特征点坐标。
2）IMU运动学
在地球旋转的测量效应小于陀螺仪精度的假设下，我们可以将IMU运动学与动态偏置模型结合起来：
其中都是不相关的零均值高斯白噪声过程。与随机游走的陀螺偏差相反，我们使用时间常数τ>0来将加速度计偏置建模成有界随机游走。矩阵Ω由估计的角速率形成，用陀螺仪测量数据：
线性化误差动力学采取形式：
其中G是直接导出和：
（.）×表示与矢量关联的斜对称叉乘矩阵。
注意到可以采用在经典EKF滤波中用于计算均值和协方差的相同方式。实际实现中，需要这些方程的离散化，其中索引p表示第p个IMU测量。出于计算复杂性考虑，我们选择使用简单的Euler-Forward方法在时间差Δt上积分。类似地，我们获得离散形式的误差状态传播矩阵：
协方差传播方程：
其中包含各个过程的噪声。
3）IMU测量误差项的公式
图3示出了在时间步骤k和k+1进行的相机测量的测量速率的差异，较快的IMU测量通常不与相机数据同步。
图3：IMU和相机的采样的不同频率：一个IMU测量项使用了图像帧间的所有陀螺仪和加速度数据。
我们需要将IMU误差项表示为步骤k和k+1处的机器人状态的函数。因此，我们需要假设在相机测量在k和k + 1的给定机器人状态下的条件概率密度f为正态分布：
考虑包含状态传播的协方差矩阵，IMU预测误差项现在可写为：
上述公式基于先验的状态的预测和实际状态之间的差值，除了旋转状态量，其中我们使用简单的最小二乘误差。
接下来，在应用误差传播定律时，得到相关的信息矩阵：
Jacobian矩阵可以直接获得但有非零解，因为旋转误差一般是非零的。
最后，在优化过程中，k到k+1帧状态的Jacobian矩阵需要被计算。回溯前人的方法，IMU误差项通过迭代方式来计算（。因此应用链式法则计算相对于第k帧状态的微分：
C.关键点匹配和关键帧选择
我们的过程pipeline采用本地化的多尺度SSE优化的Harris角点检测结合BRISK描述子提取[12]。检测器通过抑制具有较弱权重的角点来使图像中的关键点均匀分布，因为它们在到较强角点的小距离处被检测到。描述子沿着重力方向被提取（投影到图像中），其由于与IMU紧耦合而可观的。
最初，关键点被立体三角化并插入到局部映射中。我们对所有地图地标执行暴力匹配;通过使用通过IMU积分获得的（不确定）姿态预测，通过在图像坐标中应用卡方检验简单地执行外点剔除。没有RANSAC步骤，这是紧耦合IMU的另一个优点。对于后续优化，保持相机帧的有界集合，即具有在该时刻拍摄的相关图像的姿态；在这些图像中可见的所有地标都保存在本地地图中。如图4所示，我们区分两种类型的帧：我们引入包括当前帧的S个最近帧的时间窗口;并且我们使用可能在过去远的N个关键帧。对于关键帧选择，我们使用一个简单的启发式：如果匹配点跨越的图像区域与所有检测到的点跨越的区域之间的比率低于50到60％，则帧被标记为关键帧。
图4：图像帧被保留用以匹配和后续的优化
D.部分边缘化
非线性时间约束可以驻留在的有界优化窗口中是不明显的，窗口中包含可能在时间上任意间隔开的关键帧。下文中我们首先提供边缘化的数学基础，即消除非线性优化中的状态，并将其应用于视觉惯性SLAM。
1）非线性优化边际化的数学公式
高斯-牛顿方程组由所有的误差项，雅可比和信息构成：形式Hδx = b。 让我们考虑要被边缘化的一组状态，与误差项和剩余状态集合相关的所有状态的集合。 由于条件独立性，我们可以简化边缘化步骤，只将其应用于子问题：
Schur-Complement运算的得到：
文献[18]中的方程描述了边缘化的单一步骤。在我们基于关键帧的方法中，必须重复应用边缘化步骤，并将结果信息作为先验，因为我们的状态估计会持续改变。因此，我们固定x0周围的线性化点，x0为初始边缘化时x的值。换句话说x组成是：
这个通用的公式允许我们将先验信息应用到任何状态变量-包括单位四元数。引入Δx项：
现在我们可以表示高斯-牛顿系统为：
在该形式中，右侧变为：
边缘化节点包括无限远（或足够接近无限远）的地标，或来自单个姿势的仅在一个摄像机中可见的地标，因此与那些地标相关联的Hessian矩阵块有可能不是满秩矩阵。因此，我们采用伪逆的形式。
上述公式为边缘化xμ以及剩余状态xλ的状态引入了固定线性化点。这也将被用作所有未来线性化的参考点。接着，我们可以去除消耗的非线性项，并将边缘化的和作为加数，以构建整个高斯-牛顿系统。对最小二乘误差的贡献可以写为：
2）边缘化应用于基于关键帧的视觉惯性SLAM
最初边缘化误差项由前N + 1帧构成，如图5中以图形方式可视化.N个第一帧将全部被解释为关键帧，并且边缘化步骤包括消除相应的速度和偏置状态。
图5：图模型展示了在最初N+1帧时的初始边缘化过程
当将新帧插入到优化窗口中时，我们应用边缘化操作。在时间窗口中最旧的帧不是关键帧的情况下，我们将丢弃其所有的界标测量，然后将其与最旧的速度和偏置状态一起边缘化。图6示出了该过程。下降的地标测量是次优的。
图6：图模型中有N=3个关键帧和一个IMU临时节点，一个普通帧从滑动窗口中被丢弃。
然而，系统需要保持矩阵稀疏以快速求解。具有关键帧的视觉SLAM并行处理，丢弃具有其地标测量的整个帧。
在是关键帧的情况下，简单地丢弃所有关键点测量的信息丢失将更显着：在共同界标观测中编码的最早的两个关键帧之间的所有相对姿势信息将丢失。因此，我们另外将在k1帧中可见但在最近的关键帧中不可见的地标边缘化。图7描绘了该过程。于是矩阵的稀疏性再次得以保留。
图7：xTc-S被判断为关键帧的边缘化过程：最老的关键帧xTk1被舍弃。
III. 结论
本文提出了一种将惯性测量与基于关键帧的视觉SLAM紧耦合的方法。非线性优化中的误差项组合由特征点检测和IMU误差得到，因此取代了对任何调谐参数的需要。使用文章提出的方法，我们可以得到重力方向的全局一致性和在IMU运动学模型下更鲁棒的外点剔除。同时，保留了基于关键帧非线性优化的所有优点，例如静止姿态保持。使用立体相机和IMU传感器获得的结果证明了所提出的框架的可以实时操作，同时对比纯视觉SLAM或松散耦合方法表现出更好的精度和鲁棒性。
参考文献
[1] T. Barfoot, J. R. Forbes, and P. T.Furgale. Pose estimation using linearized rotations and quaternion algebra.Acta Astronautica, 68(12):101 – 112, 2011.
[2] T-C. Dong-Si and A. I. Mourikis. Motiontracking with fixed-lag smoothing: Algorithm and consistency analysis. InProceedings of the IEEE International Conference on Robotics and Automation(ICRA), 2011.
[3] P. T. Furgale. Extensions to the VisualOdometry Pipeline for the Exploration of Planetary Surfaces. PhD thesis,University of Toronto, 2011.
[4] P. T.Furgale, J. Rehder, and R. Siegwart. Unified temporaland spatial calibration for multi-sensor systems. In Proc. of the IEEE/RSJInternational Conference on Intelligent Robots and Systems (IROS), 2013. Toappear.
[5] A. Geiger, P. Lenz, and R. Urtasun. Arewe ready for autonomous driving? the KITTI vision benchmark suite. In Proc. ofthe IEEE Conference on Computer Vision and Pattern Recognition (CVPR), 2012.
[6] J. A. Hesch, D. G. Kottas, S. L.Bowman, and S. I. Roumeliotis. Towards consistent vision-aided inertialnavigation. In Proc. of the Int’l Workshop on the Algorithmic Foundations ofRobotics (WAFR), 2012.
[7] V. Indelman, S. Williams, M. Kaess, andF. Dellaert. Factor graph based incremental smoothing in inertial navigationsystems. In Information Fusion (FUSION), International Conference on, 2012.
[8] E. S. Jones and S. Soatto.Visual-inertial navigation, mapping and localization: A scalable real-timecausal approach. International Journal of Robotics Research (IJRR),30(4):407–430, 2011.
[9] J. Kelly and G. S. Sukhatme.Visual-inertial sensor fusion: Localization, mapping and sensor-to-sensorselfcalibration. International Journal of Robotics Research (IJRR),30(1):56–79, 2011.
[10] K. Konolige, M. Agrawal, and J. Sola.Large-scale visual odometry for rough terrain. In Robotics Research, pages201–212. Springer, 2011.
[11] R. Kummerle, G. Grisetti, H. Strasdat,K. Konolige, ¨ and W. Burgard. g 2o: A general framework for graphoptimization. In Proceedings of the IEEE International Conference on Roboticsand Automation (ICRA), 2011.
[12] S.Leutenegger, M. Chli, and R.Y. Siegwart. BRISK: Binaryrobust invariant scalable keypoints. In Proceedings of the IEEE InternationalConference on Computer Vision (ICCV), 2011.
[13] C. Mei, G. Sibley, M. Cummins, P. M.Newman, and I. D. Reid. Rslam: A system for large-scale mapping inconstant-time using stereo. International Journal of Computer Vision, pages198–214, 2011.
[14] A. I. Mourikis and S. I. Roumeliotis.A multistate constraint Kalman filter for vision-aided inertial navigation. InProceedings of the IEEE International Conference on Robotics and Automation(ICRA), 2007.
[15] A. Ranganathan, M. Kaess, and F.Dellaert. Fast 3d pose estimation with out-of-sequence measurements. In Proc.of the IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS), 2007.
[16] G. Sibley, L. Matthies, and G.Sukhatme. Sliding window filter with application to planetary landing. Journalof Field Robotics, 27(5):587–608, 2010.
[17] D. Sterlow and S. Singh. Motionestimation from image and intertial measurements. International Journal ofRobotics Research (IJRR), 23(12):1157–1195, 2004.
[18] H. Strasdat, J. M. M. Montiel, and A.J. Davison. Realtime monocular SLAM: Why filter? In Proceedings of the IEEEInternational Conference on Robotics and Automation (ICRA), 2010.
[19] B. Triggs, P. Mclauchlan, R. Hartley,and A. Fitzgibbon. Bundle adjustment – a modern synthesis. In VisionAlgorithms: Theory and Practice, September, 1999, pages 298–372.Springer-Verlag, 1999.
[20] S. Weiss,M.W. Achtelik, S. Lynen, M. Chli, and R. Siegwart. Real-timeonboard visual-inertial state estimation and self-calibration of MAVs inunknown environments. In Proc. of the IEEE International Conference on Roboticsand Automation (ICRA), 2012.
广告 
广告
编辑推荐
最新资讯
-
Plus为自动驾驶卡车功能添加了H.E.L.P.警报
2024-12-23 17:18
-
美国能源部发布最新版氢计划
2024-12-23 17:16
-
系统级封装（SiP）在新能源汽车领域的应用
2024-12-23 08:51
-
车载通信框架 --- 智能汽车车载通信架构浅
2024-12-23 08:40
-
全国首例！武汉车网智联公司完成智能网联测
2024-12-23 08:39
广告
广告
