首页 未命名 正文

基于关键帧使用非线性优化的视觉-惯导SLAM|厚势汽车

来源:同济智能汽车研究所编译 作者:Leutenegger.Stefan 编辑:小小 入群:加微信号 autoHS,入厚势汽车科技群与行业专家讨论更多自动驾驶行业信息 摘要:视觉和惯性传感器的融合,因其感知方式的互补性在机器人技术中已经变得流行起来。虽然目前大多数融合策略依赖于滤...
来源:同济智能汽车研究所编译 作者:Leutenegger.Stefan 编辑:小小 入群:加微信号 autoHS,入厚势汽车科技群与行业专家讨论更多自动驾驶行业信息 摘要:视觉和惯性传感器的融合,因其感知方式的互补性在机器人技术中已经变得流行起来。虽然目前大多数融合策略依赖于滤波方案,但视觉机器人社区最近已经转向基于非线性优化方法的同步定位与建图(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=qABqBC,引入左手侧复合运算符(.)+和右手侧运算符(.),使得 qAC = qAB+qBC=qBCqAB。 坐标系:使用图 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. -END- 文章精选 ---厚势汽车科技周报--- 特斯拉停止关闭实体店-威马融资30亿-蔚来亏损近百亿股价跌三成-恒大EV亏损17亿-海南禁售燃油车|厚势EV周报0311 大众将投资福特AV估值40亿美元-自动驾驶融资屡破纪录-加州DMV发布AV报告-滴滴裁员3千-中国支持外资AV|厚势AV周报02.18 苹果AV裁员2百-华为发布5G芯片-宝马奔驰合资酒杯-格力布局车联网-摩拜单车更名-宝马奔驰合资酒杯|厚势AV周报01.28 ---往期谷歌Waymo和其他自动驾驶文章--- 全球自动驾驶盘点一:发展现状大扫描,何时真正实现产业化? 全球自动驾驶盘点二:谷歌Waymo的整车传感器配置方案 全球自动驾驶盘点三:特斯拉的整车传感器配置方案 谷歌Waymo自动驾驶到底怎么样?我们分析了它的10个真实道路视频 厚 势 汽 车 为您对接资本和产业 新能源汽车 自动驾驶 车联网 联系邮箱 sasa@ihoushi.com

相关推荐

  • 暂无相关文章