IMU惯性里程计解算(附代码实现)

这篇具有很好参考价值的文章主要介绍了IMU惯性里程计解算(附代码实现)。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

一、系统概述

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

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

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

在ROS消息中的格式为:

IMU惯性里程计解算(附代码实现)

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

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

在ROS消息中的格式为:

IMU惯性里程计解算(附代码实现)

​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);

对应公式如下:

IMU惯性里程计解算(附代码实现)

 

IMU惯性里程计解算(附代码实现)

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惯性里程计解算(附代码实现)

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

未完待续...

知乎链接:知乎 - 有问题,就会有答案文章来源地址https://www.toymoban.com/news/detail-413853.html

到了这里,关于IMU惯性里程计解算(附代码实现)的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • 视觉里程计学习笔记

    一、相机 1、双目相机的 D、P、K、R矩阵 是相机标定时得到的一组重要参数,它们分别代表以下含义: D矩阵 : 畸变矫正参数矩阵 。它包含了 相机图像坐标系下的径向畸变和切向畸变系数,用于将畸变像素坐标转化为无畸变的像素坐标 。对每个相机而言都有一个D矩阵。 P矩

    2024年02月03日
    浏览(40)
  • 基于ROS发布里程计信息

    参考文档: navigationTutorialsRobotSetupOdom 参考博客: (1)ROS机器人里程计模型 (2)ROS里程计消息nav_msgs/Odometry的可视化方法 1 常用坐标系系统模型 世界坐标系是描述机器人全局信息的坐标系;机器人坐标系是描述机器人自身信息的坐标系;传感器坐标系是描述传感器信息的坐

    2024年02月08日
    浏览(44)
  • 差速驱动机器人的车轮里程计模型

            车轮测程法是指使用旋转编码器(即连接到车轮电机以测量旋转的传感器)的测程法(即估计运动和位置)。这是轮式机器人和自动驾驶汽车定位的有用技术。         在本文中,我们将通过探索差速驱动机器人的

    2024年02月15日
    浏览(47)
  • 经典文献阅读之--VoxelMap(体素激光里程计)

    作为激光里程计,常用的方法一般是特征点法或者体素法,最近Mars实验室发表了一篇文章《Efficient and Probabilistic Adaptive Voxel Mapping for Accurate Online LiDAR Odometry》,同时还开源了代码在Github上。文中为雷达里程计提出了一种高效的概率自适应体素建图方法。地图是体素的集合,

    2024年02月16日
    浏览(41)
  • 激光雷达数据和里程计数据的时间同步方法汇总

    激光雷达(LIDAR)和里程计数据的时间同步,可以采用多种方法,每种方法都有其适用场景和优势。以下是一些常用方法的汇总: 简单遍历同步 : 对两个数据流进行遍历,寻找时间戳最接近的数据对。 适用于数据量小的场景,实现简单。 双缓冲队列 : 为里程计和激光雷达

    2024年01月20日
    浏览(38)
  • 机器人运动学——轮速里程计(SLAM)

    目录 一、机器人运动学分析 二、阿克曼小车  2.1运动学分析   想要让机器人运动, 除了提供目标速度还不够, 需要将机器人的目标速度转换每个电机实际的目标速度, 最终根据电机的目标速度对电机的控制实现对机器人的控制。 机器人的目标速度转换成电机的目标速度这

    2024年02月16日
    浏览(41)
  • 【机器人模拟-02】 模拟移动机器人设置里程计

            在本教程中,我将向您展示如何设置移动机器人的测程。本教程是“机器人模拟”指南中的第二个教程。测量位移是仿真中的重要内容,设置测程的官方教程在此页面上,但我将逐步引导您完成整个过程。         您可以在此处获取此项目的完整代码。让我们

    2024年02月16日
    浏览(48)
  • 视觉SLAM14讲笔记-第7讲-视觉里程计2

    直接法的引出 直接法是视觉里程计另一个主要分支,它与特征点法有很大的不同。 使用特征点法估计相机运动时,我们把特征点看作固定在三维空间的不动点。根据它们在相机中的投影位置,通过 最小化重投影误差 来优化相机运动。 相对地,在直接法中,我们并不需要知

    2024年02月09日
    浏览(37)
  • 自动驾驶地面车辆的雷达里程计:方法与数据集综述

    Nader J. Abu-Alrub, Nathir A. Rawashdeh, Senior Member, IEEE         摘要: 雷达里程计在过去十年中受到越来越多的关注。它是在不利条件下进行机器人状态估计的最佳解决方案之一,其他内部感知和外部感知传感器可能在这些条件下无法胜任。雷达被广泛采用,对天气和光照条件具

    2024年02月15日
    浏览(48)
  • SLAM和里程计评估工具——evo使用方法全解

            本帖的主要内容是整理evo的使用方法及各种命令,不含安装步骤及过程,还未安装的请移步其他博主。         evo目前支持的公开数据集格式有: TUM、KITTI、EuRoC 以及 ROS bagfile 。如果使用的数据集格式为这些中的某一种,那么无须额外的数据格式处理,就可以

    2024年02月08日
    浏览(47)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包