一、系统概述
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里程计原理和公式推倒
未完待续...文章来源:https://www.toymoban.com/news/detail-413853.html
知乎链接:知乎 - 有问题,就会有答案文章来源地址https://www.toymoban.com/news/detail-413853.html
到了这里,关于IMU惯性里程计解算(附代码实现)的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!