IMU初始化：重力与偏置估计
文章简介
本文是MSCKF VIO系列文章的第二篇，详细介绍IMU初始化过程。初始化是VIO系统启动的关键步骤，直接影响后续估计的精度和稳定性。
目录
文章目录
- IMU初始化：重力与偏置估计
-
- 文章简介
- 目录
- [1. 初始化的重要性](#1. 初始化的重要性)
-
- [1.1 为什么需要初始化](#1.1 为什么需要初始化)
- [1.2 初始化的目标](#1.2 初始化的目标)
- [2. 算法原理](#2. 算法原理)
-
- [2.1 IMU测量模型](#2.1 IMU测量模型)
- [2.2 静止条件下的简化](#2.2 静止条件下的简化)
- [2.3 重力对齐原理](#2.3 重力对齐原理)
- [3. 代码实现](#3. 代码实现)
-
- [3.1 数据收集](#3.1 数据收集)
- [3.2 初始化核心函数](#3.2 初始化核心函数)
- [3.3 详细步骤解析](#3.3 详细步骤解析)
- [4. 数学推导](#4. 数学推导)
-
- [4.1 重力对齐公式](#4.1 重力对齐公式)
- [4.2 初始化精度分析](#4.2 初始化精度分析)
- [5. 参数配置](#5. 参数配置)
-
- [5.1 相关参数](#5.1 相关参数)
- [5.2 建议值](#5.2 建议值)
- [6. 注意事项](#6. 注意事项)
-
- [6.1 初始化条件](#6.1 初始化条件)
- [6.2 常见问题](#6.2 常见问题)
- [6.3 改进建议](#6.3 改进建议)
- [7. 验证方法](#7. 验证方法)
-
- [7.1 重力大小检查](#7.1 重力大小检查)
- [7.2 偏置大小检查](#7.2 偏置大小检查)
- [7.3 姿态合理性检查](#7.3 姿态合理性检查)
- [8. 总结](#8. 总结)
- 下一篇预告
- 参考文献
1. 初始化的重要性
1.1 为什么需要初始化
IMU传感器存在以下需要校准的误差源：
- 陀螺仪偏置：静止时输出非零
- 加速度计偏置：测量值偏离真实值
- 重力方向：需要确定世界坐标系中重力的方向
1.2 初始化的目标
- 估计重力向量 g \mathbf{g} g 在IMU坐标系中的方向
- 估计陀螺仪初始偏置 b g \mathbf{b}_g bg
- 确定初始姿态 R W I \mathbf{R}_{WI} RWI
2. 算法原理
2.1 IMU测量模型
陀螺仪测量模型 ：
ω m = ω t r u e + b g + n g \boldsymbol{\omega}m = \boldsymbol{\omega}{true} + \mathbf{b}_g + \mathbf{n}_g ωm=ωtrue+bg+ng
加速度计测量模型 ：
a m = R W I T ( a t r u e − g ) + b a + n a \mathbf{a}m = \mathbf{R}{WI}^T (\mathbf{a}_{true} - \mathbf{g}) + \mathbf{b}_a + \mathbf{n}_a am=RWIT(atrue−g)+ba+na
其中：
- ω m , a m \boldsymbol{\omega}_m, \mathbf{a}_m ωm,am: 传感器测量值
- ω t r u e , a t r u e \boldsymbol{\omega}{true}, \mathbf{a}{true} ωtrue,atrue: 真实值
- b g , b a \mathbf{b}_g, \mathbf{b}_a bg,ba: 偏置
- n g , n a \mathbf{n}_g, \mathbf{n}_a ng,na: 测量噪声
- R W I \mathbf{R}_{WI} RWI: 世界到IMU的旋转
- g \mathbf{g} g: 重力向量
2.2 静止条件下的简化
在初始化期间，假设IMU保持静止：
ω t r u e = 0 , a t r u e = 0 \boldsymbol{\omega}{true} = \mathbf{0}, \quad \mathbf{a}{true} = \mathbf{0} ωtrue=0,atrue=0
因此：
ω m = b g + n g \boldsymbol{\omega}_m = \mathbf{b}_g + \mathbf{n}_g ωm=bg+ng
a m = − R W I T g + b a + n a \mathbf{a}m = -\mathbf{R}{WI}^T \mathbf{g} + \mathbf{b}_a + \mathbf{n}_a am=−RWITg+ba+na
2.3 重力对齐原理
通过测量值估计重力方向：
a ˉ m = 1 N ∑ i = 1 N a m , i ≈ − R W I T g \bar{\mathbf{a}}m = \frac{1}{N} \sum{i=1}^{N} \mathbf{a}{m,i} \approx -\mathbf{R}{WI}^T \mathbf{g} aˉm=N1i=1∑Nam,i≈−RWITg
初始姿态可以通过重力对齐获得：
R W I 0 = FromTwoVectors ( a ˉ m , − g r e f ) \mathbf{R}_{WI}^0 = \text{FromTwoVectors}(\bar{\mathbf{a}}m, -\mathbf{g}{ref}) RWI0=FromTwoVectors(aˉm,−gref)
3. 代码实现
3.1 数据收集
cpp
// src/msckf_vio.cpp:229-246
void MsckfVio::imuCallback(const sensor_msgs::ImuConstPtr& msg) {
// 将IMU消息缓存到缓冲区
imu_msg_buffer.push_back(*msg);
// 等待收集足够的数据（200帧）
if (!is_gravity_set) {
if (imu_msg_buffer.size() < 200) return;
// 执行初始化
initializeGravityAndBias();
is_gravity_set = true;
}
}关键点：
- 使用200帧IMU数据进行初始化（约2秒@100Hz）
- 数据量足够大可以有效降低噪声影响
3.2 初始化核心函数
cpp
// src/msckf_vio.cpp:248-284
void MsckfVio::initializeGravityAndBias() {
// 1. 累加角速度和线性加速度
Vector3d sum_angular_vel = Vector3d::Zero();
Vector3d sum_linear_acc = Vector3d::Zero();
for (const auto& imu_msg : imu_msg_buffer) {
Vector3d angular_vel = Vector3d::Zero();
Vector3d linear_acc = Vector3d::Zero();
tf::vectorMsgToEigen(imu_msg.angular_velocity, angular_vel);
tf::vectorMsgToEigen(imu_msg.linear_acceleration, linear_acc);
sum_angular_vel += angular_vel;
sum_linear_acc += linear_acc;
}
// 2. 估计陀螺仪偏置（静止时角速度应为0）
state_server.imu_state.gyro_bias =
sum_angular_vel / imu_msg_buffer.size();
// 3. 计算加速度计平均值（包含重力信息）
Vector3d gravity_imu = sum_linear_acc / imu_msg_buffer.size();
// 4. 设置重力向量大小
double gravity_norm = gravity_imu.norm();
IMUState::gravity = Vector3d(0.0, 0.0, -gravity_norm);
// 5. 估计初始姿态（重力对齐）
Quaterniond q0_i_w = Quaterniond::FromTwoVectors(
gravity_imu, -IMUState::gravity);
state_server.imu_state.orientation =
rotationToQuaternion(q0_i_w.toRotationMatrix().transpose());
}3.3 详细步骤解析
步骤1：数据平均
cpp
Vector3d avg_angular_vel = sum_angular_vel / imu_msg_buffer.size();
Vector3d avg_linear_acc = sum_linear_acc / imu_msg_buffer.size();数学原理 ：
ω ˉ = 1 N ∑ i = 1 N ω i \bar{\boldsymbol{\omega}} = \frac{1}{N} \sum_{i=1}^{N} \boldsymbol{\omega}_i ωˉ=N1i=1∑Nωi
a ˉ = 1 N ∑ i = 1 N a i \bar{\mathbf{a}} = \frac{1}{N} \sum_{i=1}^{N} \mathbf{a}_i aˉ=N1i=1∑Nai
通过平均可以有效降低高斯噪声的影响，噪声方差降低为原来的 1 N \frac{1}{N} N1。
步骤2：陀螺仪偏置估计
cpp
state_server.imu_state.gyro_bias = avg_angular_vel;原理 ：
在静止状态下，真实角速度为0：
b ^ g = ω ˉ m ≈ b g \hat{\mathbf{b}}_g = \bar{\boldsymbol{\omega}}_m \approx \mathbf{b}_g b^g=ωˉm≈bg
步骤3：重力向量估计
cpp
Vector3d gravity_imu = avg_linear_acc;
double gravity_norm = gravity_imu.norm();
IMUState::gravity = Vector3d(0.0, 0.0, -gravity_norm);原理 ：
静止时加速度计测量值主要包含重力信息：
a ˉ m ≈ − R W I T g \bar{\mathbf{a}}m \approx -\mathbf{R}{WI}^T \mathbf{g} aˉm≈−RWITg
步骤4：姿态估计
cpp
Quaterniond q0_i_w = Quaterniond::FromTwoVectors(
gravity_imu, -IMUState::gravity);
state_server.imu_state.orientation =
rotationToQuaternion(q0_i_w.toRotationMatrix().transpose());数学原理：
我们需要找到旋转 R W I \mathbf{R}_{WI} RWI 使得：
R W I T g = − a ˉ i m u \mathbf{R}{WI}^T \mathbf{g} = -\bar{\mathbf{a}}{imu} RWITg=−aˉimu
等价于：
R W I ( − a ˉ i m u ) = g \mathbf{R}{WI} (-\bar{\mathbf{a}}{imu}) = \mathbf{g} RWI(−aˉimu)=g
FromTwoVectors 函数计算从向量 a ˉ i m u \bar{\mathbf{a}}_{imu} aˉimu 到 − g -\mathbf{g} −g 的旋转。
4. 数学推导
4.1 重力对齐公式
给定两个向量 v 1 \mathbf{v}_1 v1 和 v 2 \mathbf{v}_2 v2，求旋转 R \mathbf{R} R 使得 R v 1 = v 2 \mathbf{R} \mathbf{v}_1 = \mathbf{v}_2 Rv1=v2。
方法1：使用四元数
q = v 1 × v 2 1 + v 1 ⋅ v 2 \mathbf{q} = \begin{bmatrix} \mathbf{v}_1 \times \mathbf{v}_2 \\ 1 + \mathbf{v}_1 \cdot \mathbf{v}_2 \end{bmatrix} q=v1×v21+v1⋅v2
归一化后转换为旋转矩阵。
方法2：使用轴角
旋转角度：
θ = arccos ( v 1 ⋅ v 2 ) \theta = \arccos(\mathbf{v}_1 \cdot \mathbf{v}_2) θ=arccos(v1⋅v2)
旋转轴：
k = v 1 × v 2 ∥ v 1 × v 2 ∥ \mathbf{k} = \frac{\mathbf{v}_1 \times \mathbf{v}_2}{\|\mathbf{v}_1 \times \mathbf{v}_2\|} k=∥v1×v2∥v1×v2
旋转矩阵：
R = I + sin θ k × + ( 1 − cos θ ) k × 2 \mathbf{R} = \mathbf{I} + \sin\theta \\mathbf{k}\times + (1 - \cos\theta) \\mathbf{k}\times^2 R=I+sinθk×+(1−cosθ)k×2
4.2 初始化精度分析
假设加速度计测量噪声为高斯白噪声 σ a \sigma_a σa，则：
- 单次测量精度： σ a \sigma_a σa
- N次测量平均后精度： σ a / N \sigma_a / \sqrt{N} σa/N
- 姿态估计精度： ≈ σ a / ( g N ) \approx \sigma_a / (g \sqrt{N}) ≈σa/(gN )
数值示例：
对于 σ a = 0.01 m/s 2 \sigma_a = 0.01 \text{ m/s}^2 σa=0.01 m/s2, N = 200 N = 200 N=200, g = 9.81 m/s 2 g = 9.81 \text{ m/s}^2 g=9.81 m/s2：
σ θ ≈ 0.01 9.81 × 200 ≈ 0.0007 rad ≈ 0.04 ∘ \sigma_\theta \approx \frac{0.01}{9.81 \times \sqrt{200}} \approx 0.0007 \text{ rad} \approx 0.04^\circ σθ≈9.81×200 0.01≈0.0007 rad≈0.04∘
5. 参数配置
5.1 相关参数
| 参数 | 位置 | 默认值 | 说明 |
|---|---|---|---|
imu_msg_buffer.size() < 200 |
imuCallback | 200 | 初始化所需的IMU帧数 |
5.2 建议值
| 场景 | 帧数 | 时间 | 说明 |
|---|---|---|---|
| 静止初始化 | 200 | 2秒 | 100Hz IMU |
| 快速初始化 | 100 | 1秒 | 精度略低 |
| 高精度初始化 | 500 | 5秒 | 适合高噪声环境 |
6. 注意事项
6.1 初始化条件
重要：初始化期间IMU必须保持静止！
- 静止要求：初始化期间IMU必须保持静止
- 水平放置：建议初始时IMU水平放置（z轴朝上）
- 无振动：避免在有持续振动的环境下初始化
6.2 常见问题
问题1：初始化后姿态漂移
- 原因：陀螺仪偏置估计不准确
- 解决：增加初始化帧数或使用更精确的偏置估计方法
问题2：重力方向错误
- 原因：初始化时有外部加速度
- 解决：确保初始化期间完全静止
问题3：初始化失败检测
cpp
// 添加初始化检查
double gravity_norm = gravity_imu.norm();
if (gravity_norm < 8.0 || gravity_norm > 11.0) {
ROS_WARN("Gravity magnitude out of range: %f", gravity_norm);
// 可能初始化失败
}6.3 改进建议
- 使用磁力计辅助：提供航向角初始化
- 运动检测：自动检测静止状态
- 在线偏置估计：使用滤波器持续估计偏置
7. 验证方法
7.1 重力大小检查
cpp
double gravity_norm = IMUState::gravity.norm();
assert(gravity_norm > 9.0 && gravity_norm < 10.0);7.2 偏置大小检查
cpp
double gyro_bias_norm = state_server.imu_state.gyro_bias.norm();
assert(gyro_bias_norm < 0.1); // 偏置不应太大7.3 姿态合理性检查
cpp
Matrix3d R = quaternionToRotation(state_server.imu_state.orientation);
Vector3d gravity_est = R * IMUState::gravity;
// 应该与加速度计平均值方向一致8. 总结
IMU初始化是VIO系统的关键步骤，通过本文的介绍，读者应该了解：
- 初始化的基本原理和数学推导
- 代码实现的详细过程
- 参数配置和调优方法
- 常见问题和解决方案
下一篇预告
下一篇文章将详细介绍状态传播过程，包括四阶龙格库塔积分方法。
参考文献
-
Woodman, O. J. (2007). An introduction to inertial navigation. University of Cambridge, Cambridge, UK.
-
Titterton, D., & Weston, J. L. (2004). Strapdown inertial navigation technology (Vol. 17). IET.
本文为原创文章，转载请注明出处。欢迎点赞、收藏、评论交流！