ORB_SLAM2视觉惯性紧耦合定位技术路线与代码详解2——IMU初始化

本文详细介绍了视觉惯性初始化的过程,包括陀螺仪偏置标定、尺度恢复和重力加速度预估、加速度计偏置标定及尺度重力加速度优化等内容。通过对这些步骤的理解,可以更好地实现视觉和惯性数据的融合。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

3.IMU初始化(视觉惯性联合初始化)
终于来到视觉惯性初始化阶段了,这段是视觉和惯性数据融合的第一步,是一段松耦合过程。理论上是将准确的视觉数据(但缺少尺度)和快速的IMU数据(需要重力加速度又存在零偏误差)相结合。具体介绍在VINS的博客中也同样说过,这部分关于最小二乘、尺度收敛等问题请参考我的博客:视觉SLAM常见的QR分解SVD分解等矩阵分解方式求解满秩和亏秩最小二乘问题(最全的方法分析总结)
https://blog.youkuaiyun.com/wangshuailpp/article/details/80209863
(1)陀螺仪偏置标定(零偏)
这一部分比较简单,直接联立N-1个相机做旋转矩阵的最小二乘即可,然后通过高斯牛顿方法即可得到零偏bg。需要注意一点,当求出零偏后将其代入预积分公式会重新计算一遍预积分值,使预积分数值更加准确。
这里写图片描述
(2)尺度恢复和重力加速度预估
首先建立预估状态向量X=[s,gw],其中s是尺度,gw是世界坐标系下的重力加速度也是第一个相机坐标系下的重力加速度。ORB_SLAM2中世界坐标选取的是第一个相机对应的坐标(VINS则不同),这样做会存在一个问题,因为第一个相机可能自身存在一定的旋转倾斜导致整个世界坐标看起来是歪着的,画轨迹的时候有一种倾斜的即视感,所以我觉得还是尽量固定好z方向,使轨迹没有横滚和俯仰。这里使用了三个关键帧联立视觉和IMU预积分数据构建一个AX=B的最小二乘超定方程,至少需要四个关键帧,采用奇异值分解求最小二乘问题,速度较慢但精度高。
这里写图片描述
这里写图片描述
(3)加速度计偏置标定和尺度重力加速度优化
上面计算过程没有考虑到加速度计偏置的影响,使得重力加速度和加速度计偏置难以区分,很有可能会导致系统病态性问题,文中提出了重力加速度的大小G,假设其是一个不变值,优化重力加速度的方向。

这里写图片描述
这里写图片描述
最后优化的状态量变成了X=[s,deta_θ,ba],也就是说我们只优化重力加速度的方向,每次优化得到一个deta_θ都会代入到论文中的公式(17)计算出最新的gw然后继续优化,直到变量收敛。

评论 10
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值