IMU惯性里程计解算（附代码实现）

一、系统概述

IMU是机器人常用的传感器之一，IMU对机器人的定位功能实现非常重要，其优点在于是内源传感器对外部环境变化不明显，输出频率高，缺点在于存在累积误差。本文主要记录一下在机器人定位中对IMU的使用和对惯性导航里程计的理解和实现。

本文代码主要依赖于ROS相关库实现，源代码见：GitHub - Abin1258/imu_to_odom: imu odometry

1.系统输入：IMU传感器测量数据：线性加速度、角速度

在ROS消息中的格式为：

ps：1.要注意观察不同IMU传感器的单位不同，有的传感器加速度单位是重力加速度的倍数，有的传感器是米每秒的平方，本文所用传感器的单位是

2.系统输出：Odometry消息：位置、姿态、线速度、角速度和每个量的协方差矩阵

在ROS消息中的格式为：

​3.解算模型：

3.1初始化

3.1.1 0时刻位姿初始化

一般设置0时刻为里程计起点，即位姿速度全为0，代码上如下：

odom.header.frame_id = "odom"; odom.child_frame_id = "base_link"; Eigen::Vector3d zero(0, 0, 0); point.pos = zero; point.orien = Eigen::Matrix3d::Identity(); point.v = zero; point.w = zero; firstT = true;

3.1.2 重力初始化

只有IMU一个传感器，所以直接用了第一帧数据（假设当前载体处于静止状态）的加速度作为重力加速度项，代码如下：

gravity[0] = msg.x; gravity[1] = msg.y; gravity[2] = msg.z;

3.2 求解位姿

初始化完成后，先求解位姿，因为求解位置的时候需要使用位姿结果将IMU坐标系下的加速度转化到全局坐标系下的加速度，求解位姿的方法有很多，在下面的章节陆续补充，本文代码实现的是用旋转矩阵表示的方法求解的位姿，代码如下：

point.w << msg.x, msg.y, msg.z; //基于旋转矩阵表示方法 Eigen::Matrix3d B; B << 0, -msg.z * deltaT, msg.y * deltaT, msg.z * deltaT, 0, -msg.x * deltaT, -msg.y * deltaT, msg.x * deltaT, 0; //欧拉法 double sigma = std::sqrt(std::pow(msg.x, 2) + std::pow(msg.y, 2) + std::pow(msg.z, 2)) * deltaT; //罗德里格斯公式 point.orien = point.orien * (Eigen::Matrix3d::Identity() + (std::sin(sigma) / sigma) * B - ((1 - std::cos(sigma)) / std::pow(sigma, 2)) * B * B);

对应公式如下：

 

3.3 求解线速度和位置

求解完位姿后求解位置就较为简单，两个积分公式即可

  Eigen::Vector3d acc_l(msg.x, msg.y, msg.z);//imu坐标系下的加速度
  Eigen::Vector3d acc_g = point.orien * acc_l;//转化到里程计坐标系下的加速度
  point.v = point.v + deltaT * (acc_g - gravity);//积分得到速度
  point.pos = point.pos + deltaT * point.v;//积分得到位置

4.实现效果

求解完成后只需要发布里程计消息，即可在RVIZ中观测，实际效果如果没有其他传感器观测矫正误差，10秒中左右累积误差已经达到米级

二、IMU里程计原理和公式推倒

未完待续...

知乎链接：知乎 - 有问题，就会有答案