open_vins（三）：imu静止初始化

最新推荐文章于 2025-11-10 22:10:11 发布

原创

最新推荐文章于 2025-11-10 22:10:11 发布 · 6.6k 阅读

53 ·

CC 4.0 BY-SA版权

本文介绍了一种基于静态条件下的IMU初始化方法，通过检测加速度计的方差来判断设备是否处于静止状态，并据此估计重力方向及传感器偏置。

初始化是指在系统启动阶段，需要估计重力方向(gravity direction)、加速度计以及陀螺仪biases (accelerometer and gyroscope biases)、初始速度(initial velocity)、初始位置(initial position)等。

OpenVINS系统中，只估计了重力方向、加速度计/陀螺仪biases。
初始化函数位于：ov_msckf/src/core/VioManager.cpp 中
Manager::try_to_initialize()

一．静止初始化原理

静止初始化是指在视频序列开始阶段，需要有一段静止状态。
系统利用静止状态的IMU数据来估计重力方向、加速度计陀螺仪biases。

静止初始化需要检测imu的跳变过程：
其中包含两个状态：静止状态、跳变状态
在这里插入图片描述

通过设定一个阈值［_imu_excite_threshold］,可以判断imu是否得到足够的激励，从得到运动激励前的一段静止状态下的imu测量值，根据静止状态测量值，估算出imu初始化参数。

主要步骤：

将imu数据按固定时间的窗口(0.5s - 2s)划分
求最新的窗口w1 中imu加速度的方差 a_var，判断是否出现＂跳变＂
若a_var1 < imu激励阈值，则说明窗口w1是一段静止状态，返回false;
若a_var1 >= imu激励阈值，则说明窗口w1出现跳变，下步4 -> 对第二近的窗口w2进行判断
若a_var2 < imu激励阈值，则使用w2窗口的ｉｍｕ数据进行系统初始化;
计算窗口w2的平均加速度与角速度以及方差.
静止初始化求出重力方向(gravity direction)、初始速度(initial velocity)、初始位置(initial position)、加速度计以及陀螺仪biases (accelerometer and gyroscope biases)

代码框图如下：
在这里插入图片描述

二．理论公式

step１：计算初始旋转矩阵

窗口w2的加速度计平均值： am2_avg
窗口w2的陀螺仪平均值： wm2_avg

因为，在世界系下，静止时的加速度测量值为（0,0,g）
将窗口w2的am2_avg除以其模长，得到平均加速度的方向（单位向量），即为世界系ｚ轴的方向在imu坐标系上的投影：
在这里插入图片描述
确定z轴方向后，通过施密特正交化构建单位坐标系。

假设，e1为imu系的xi轴对应的单位方向向量：
在这里插入图片描述
将xi轴投影到 zw-xw平面，求出xw轴在imu坐标系下的方向向量：

y轴方向由x轴和z轴叉乘得到:

通过上述步骤，得到了从世界系w到imu坐标系的初始旋转 Rwi=[x y z]

step2：计算陀螺仪bias：
静止时刻的陀螺仪理想测量值（角速度）应该为0，因此陀螺仪的bias即窗口w2陀螺仪数据的平均值 [wm2_avg]
在这里插入图片描述
step3：计算加速度计bias:
静止时的加速度计bias为加速度平均值与实际重力加速度的差：

g为世界系下的重力加速度，Ｒ为第一步求出来的旋转矩阵，将重力矢量从世界系投影到imu坐标系下。

三．相关代码

在 feed_measurement_stereo／feed_measurement_monocular 过程中调用初始化函数try_to_initialize()

bool VioManager::try_to_initialize() {
   
   
    // Returns from our initializer
    double time0;
    Eigen::Matrix<double, 4, 1> q_GtoI0;
    Eigen::Matrix<double, 3, 1> b_w0, v_I0inG, b_a0, p_I0inG;
    // Try to initialize the system
    // We will wait for a jerk if we do not have the zero velocity update enabled
    // Otherwise we can initialize right away as the zero velocity will handle the stationary case
    bool wait_for_jerk = (updaterZUPT == nullptr);
    bool success = initializer->initialize_with_imu(time0, q_GtoI0, b_w0, v_I0inG, b_a0, p_I0inG, wait_for_jerk);
    // Return if it failed
    if (!success) {
   
   
        return false;
    }
    // Make big vector (q,p,v,bg,ba), and update our state
    // Note: start from zero position, as this is what our covariance is based off of
    Eigen::Matrix<double,16,1> imu_val;
    imu_val.block(0,0,4,1) = q_GtoI0;
    imu_val.block(4,0,3,1) << 0,0,0;
    imu_val.block(7,0,3,1) = v_I0inG;
    imu_val.block(10,0,3,1) = b_w0;
    imu_val.block(13,0,3,1) = b_a0;

    state->_imu->set_value(imu_val)