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

相关文章

使用Sentinel自定义返回和实现区分来源方式

《使用Sentinel自定义返回和实现区分来源方式》:本文主要介绍使用Sentinel自定义返回和实现区分来源方式,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录Sentinel自定义返回和实现区分来源1. 自定义错误返回2. 实现区分来源总结Sentinel自定

Java实现时间与字符串互相转换详解

《Java实现时间与字符串互相转换详解》这篇文章主要为大家详细介绍了Java中实现时间与字符串互相转换的相关方法,文中的示例代码讲解详细,感兴趣的小伙伴可以跟随小编一起学习一下... 目录一、日期格式化为字符串(一)使用预定义格式(二)自定义格式二、字符串解析为日期(一)解析ISO格式字符串(二)解析自定义

opencv图像处理之指纹验证的实现

《opencv图像处理之指纹验证的实现》本文主要介绍了opencv图像处理之指纹验证的实现,文中通过示例代码介绍的非常详细,对大家的学习或者工作具有一定的参考学习价值,需要的朋友们下面随着小编来一起学... 目录一、简介二、具体案例实现1. 图像显示函数2. 指纹验证函数3. 主函数4、运行结果三、总结一、

Springboot处理跨域的实现方式(附Demo)

《Springboot处理跨域的实现方式(附Demo)》:本文主要介绍Springboot处理跨域的实现方式(附Demo),具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不... 目录Springboot处理跨域的方式1. 基本知识2. @CrossOrigin3. 全局跨域设置4.

Spring Boot 3.4.3 基于 Spring WebFlux 实现 SSE 功能(代码示例)

《SpringBoot3.4.3基于SpringWebFlux实现SSE功能(代码示例)》SpringBoot3.4.3结合SpringWebFlux实现SSE功能,为实时数据推送提供... 目录1. SSE 简介1.1 什么是 SSE?1.2 SSE 的优点1.3 适用场景2. Spring WebFlu

基于SpringBoot实现文件秒传功能

《基于SpringBoot实现文件秒传功能》在开发Web应用时,文件上传是一个常见需求,然而,当用户需要上传大文件或相同文件多次时,会造成带宽浪费和服务器存储冗余,此时可以使用文件秒传技术通过识别重复... 目录前言文件秒传原理代码实现1. 创建项目基础结构2. 创建上传存储代码3. 创建Result类4.

SpringBoot日志配置SLF4J和Logback的方法实现

《SpringBoot日志配置SLF4J和Logback的方法实现》日志记录是不可或缺的一部分,本文主要介绍了SpringBoot日志配置SLF4J和Logback的方法实现,文中通过示例代码介绍的非... 目录一、前言二、案例一:初识日志三、案例二:使用Lombok输出日志四、案例三:配置Logback一

java之Objects.nonNull用法代码解读

《java之Objects.nonNull用法代码解读》:本文主要介绍java之Objects.nonNull用法代码,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐... 目录Java之Objects.nonwww.chinasem.cnNull用法代码Objects.nonN

Python如何使用__slots__实现节省内存和性能优化

《Python如何使用__slots__实现节省内存和性能优化》你有想过,一个小小的__slots__能让你的Python类内存消耗直接减半吗,没错,今天咱们要聊的就是这个让人眼前一亮的技巧,感兴趣的... 目录背景:内存吃得满满的类__slots__:你的内存管理小助手举个大概的例子:看看效果如何?1.

Python+PyQt5实现多屏幕协同播放功能

《Python+PyQt5实现多屏幕协同播放功能》在现代会议展示、数字广告、展览展示等场景中,多屏幕协同播放已成为刚需,下面我们就来看看如何利用Python和PyQt5开发一套功能强大的跨屏播控系统吧... 目录一、项目概述:突破传统播放限制二、核心技术解析2.1 多屏管理机制2.2 播放引擎设计2.3 专