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

2023-11-01 08:00

本文主要是介绍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里程计原理和公式推倒

未完待续...

知乎链接:知乎 - 有问题,就会有答案

这篇关于IMU惯性里程计解算(附代码实现)的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/321618

相关文章

Python Transformers库(NLP处理库)案例代码讲解

《PythonTransformers库(NLP处理库)案例代码讲解》本文介绍transformers库的全面讲解,包含基础知识、高级用法、案例代码及学习路径,内容经过组织,适合不同阶段的学习者,对... 目录一、基础知识1. Transformers 库简介2. 安装与环境配置3. 快速上手示例二、核心模

python实现svg图片转换为png和gif

《python实现svg图片转换为png和gif》这篇文章主要为大家详细介绍了python如何实现将svg图片格式转换为png和gif,文中的示例代码讲解详细,感兴趣的小伙伴可以跟随小编一起学习一下... 目录python实现svg图片转换为png和gifpython实现图片格式之间的相互转换延展:基于Py

Python利用ElementTree实现快速解析XML文件

《Python利用ElementTree实现快速解析XML文件》ElementTree是Python标准库的一部分,而且是Python标准库中用于解析和操作XML数据的模块,下面小编就来和大家详细讲讲... 目录一、XML文件解析到底有多重要二、ElementTree快速入门1. 加载XML的两种方式2.

Java的栈与队列实现代码解析

《Java的栈与队列实现代码解析》栈是常见的线性数据结构,栈的特点是以先进后出的形式,后进先出,先进后出,分为栈底和栈顶,栈应用于内存的分配,表达式求值,存储临时的数据和方法的调用等,本文给大家介绍J... 目录栈的概念(Stack)栈的实现代码队列(Queue)模拟实现队列(双链表实现)循环队列(循环数组

C++如何通过Qt反射机制实现数据类序列化

《C++如何通过Qt反射机制实现数据类序列化》在C++工程中经常需要使用数据类,并对数据类进行存储、打印、调试等操作,所以本文就来聊聊C++如何通过Qt反射机制实现数据类序列化吧... 目录设计预期设计思路代码实现使用方法在 C++ 工程中经常需要使用数据类,并对数据类进行存储、打印、调试等操作。由于数据类

Python实现图片分割的多种方法总结

《Python实现图片分割的多种方法总结》图片分割是图像处理中的一个重要任务,它的目标是将图像划分为多个区域或者对象,本文为大家整理了一些常用的分割方法,大家可以根据需求自行选择... 目录1. 基于传统图像处理的分割方法(1) 使用固定阈值分割图片(2) 自适应阈值分割(3) 使用图像边缘检测分割(4)

Android实现在线预览office文档的示例详解

《Android实现在线预览office文档的示例详解》在移动端展示在线Office文档(如Word、Excel、PPT)是一项常见需求,这篇文章为大家重点介绍了两种方案的实现方法,希望对大家有一定的... 目录一、项目概述二、相关技术知识三、实现思路3.1 方案一:WebView + Office Onl

C# foreach 循环中获取索引的实现方式

《C#foreach循环中获取索引的实现方式》:本文主要介绍C#foreach循环中获取索引的实现方式,本文给大家介绍的非常详细,对大家的学习或工作具有一定的参考借鉴价值,需要的朋友参考下吧... 目录一、手动维护索引变量二、LINQ Select + 元组解构三、扩展方法封装索引四、使用 for 循环替代

Spring Security+JWT如何实现前后端分离权限控制

《SpringSecurity+JWT如何实现前后端分离权限控制》本篇将手把手教你用SpringSecurity+JWT搭建一套完整的登录认证与权限控制体系,具有很好的参考价值,希望对大家... 目录Spring Security+JWT实现前后端分离权限控制实战一、为什么要用 JWT?二、JWT 基本结构

Java实现优雅日期处理的方案详解

《Java实现优雅日期处理的方案详解》在我们的日常工作中,需要经常处理各种格式,各种类似的的日期或者时间,下面我们就来看看如何使用java处理这样的日期问题吧,感兴趣的小伙伴可以跟随小编一起学习一下... 目录前言一、日期的坑1.1 日期格式化陷阱1.2 时区转换二、优雅方案的进阶之路2.1 线程安全重构2