使用EKF融合odometry及imu数据

article/2025/7/22 14:38:58

      整理资料发现早前学习robot_pose_ekf的笔记,大抵是一些原理基础的东西加一些自己的理解,可能有不太正确的地方。当时做工程遇到的情况为机器人在一些如光滑的地面上打滑的情形,期望使用EKF利用imu对odom数据进行校正。就结果来看,机器人旋转性能得到改善,前进方向性能没有改善,符合程序原理。若需要提高前进方向的性能,可以考虑加入VO或gps模块。实际工程中应注意odom数据进行协方差矩阵初始化的问题,否则程序会报错。

      一、扩展卡尔曼滤波(EKF)原理

      当前状态的概率的概率分布是关于上一状态和将要执行的控制量的二元函数,再叠加一个高斯噪声,测量值同样是关于当前状态的函数叠加高斯噪声。系统可为非线性系统。

                                                                     

      对g(x_(t-1),u_t )和h(x_t )泰勒展开,将其线性化,只取一次项为一阶EKF滤波。

                                                  

      g(xt-1,ut )在上一状态估计的最优值μt-1处取一阶导数,h(xt)在当前时刻预测值μ ̅t处取一阶导数,得到Gt和Ht。
      结合通用的Kalman滤波公式如下:
      预测过程:

                                                                          

      更新过程:

                                                                    

      二、卡尔曼滤波的多传感器融合

      多传感器系统模型模型如下:

                                                                  

      和传统的卡尔曼滤波系统模型相比,多传感器系统的观测方程有多个,每个传感器的测量量都可以不同。
      单观测模型系统:

                                                    

      卡尔曼滤波的核心为卡尔曼系数K的选取。对于多传感器系统,y(k)-Cx ̅(k)无法直接计算。
      多传感器信息融合:
      状态预测方程依然为:

                                                                    

      第一个传感器的观测方程更新后得到系统的状态量x(k)及系统协方差矩阵Σ(k)。将二者作为下一个传感器更新过程的系统预测状态量x ̅(k)和系统预测协方差矩阵Σ ̅(k)进行状态更新。将最后一个传感器更新后得到的系统的状态量x(k)及系统协方差矩阵Σ(k)作为融合后输出,并将二者用于预测过程进行下一时刻的迭代。
      扩展卡尔曼滤波工具包robot_pose_ekf

      基于里程计系统建模:

      系统状态量:

      系统输入:

      系统方程:

                       

                                       

      过程激励协方差矩阵根据实际情况设定。同时在初始时刻,系统状态及系统协方差矩阵需要进行初始化。
      观测系统模型:
      通过topic信息分别得到轮式里程计(ODOMETRY)、IMU、视觉里程计(VO)、GPS各观测值和观测协方差矩阵,运用多传感器信息融合的原理对系统状态进行更新。
      其中,ODOMETRY的观测量为[x,y,z,pitch,roll,raw]^T,观测矩阵为:
[1 0 0 0 0 0;0 1 0 0 0 0;0 0 0 0 0 0;0 0 0 0 0 0;0 0 0 0 0 0;0 0 0 0 0 0 1]
      IMU的观测量为:[pitch,roll,raw]^T,观测矩阵为:
[0 0 0 1 0 0;0 0 0 0 1 0;0 0 0 0 0 1]
      VO的观测量为[x,y,z,pitch,roll,raw]^T,观测矩阵为:
[1 0 0 0 0 0;0 1 0 0 0 0;0 0 1 0 0 0;0 0 0 1 0 0;0 0 0 0 1 0;0 0 0 0 0 0 1]
      GPS的观测量为[x,y,z]^T,观测矩阵为:

[1 0 0 0 0 0;0 1 0 0 0 0;0 0 1 0 0 0]

      三、程序流程(默认系统模型及各测量模型已建立完毕):

      ①开始监听ODOMETRY、IMU、VO及GPS信息;

      ②构建扩展卡尔曼滤波器,具体可参考:https://blog.csdn.net/zhxue_11/article/details/83822625,扩展卡尔曼滤波器的非线性模型为上文提到的里程计模型;

      ③开始更新,起始只引入系统噪声;

      ④获取ODOMETRY信息作为观测量及观测协方差矩阵,进行状态更新,得到更新后的系统状态及系统协方差矩阵;

      ⑤获取IMU信息作为观测量及观测协方差矩阵,进行状态更新,得到更新后的系统状态及系统协方差矩阵;

      ⑥获取VO信息作为观测量及观测协方差矩阵,进行状态更新,得到更新后的系统状态及系统协方差矩阵;

      ⑦获取GPS信息作为观测量及观测协方差矩阵,进行状态更新,得到更新后的系统状态及系统协方差矩阵;

      ⑧发布更新后的状态量及协方差矩阵作为融合后的信息odom_combined。

      四、robot_pose_ekf包使用方法

      1、参数调整(odom_estimation_node.cpp中):

           ①设定系统过程激励协方差矩阵;

           ②初始化设定系统协方差矩阵;

       2、robot_pose_ekf.launch文件的设置:

           ①<param name="base_footprint_frame" value="base_footprint"/>将”base_footprint”替换为自身机器人坐标系;

           ②<param name="freq" value="30.0"/>EKF发布的频率为30Hz;

           ③<param name="sensor_timeout" value="1.0"/>传感器超时设置为1s;

  ④<param name="odom_used" value="true"/>启动轮式例程计信息作为观测量,”imu_used”,”vo_used”,”gps_used”同理;

           ⑤<remap from="odom" to="pr2_base_odometry/odom" />将”pr2_base_odometry/odom”替换为自身发布的odom的topic名称,该映射实现对自身机器人信息的监听,”imu”,”vo”,”gps”同理,注意其对应声明分别为“imu_data”、“vo”、“gps”。

      五、robot_pose_ekf官方文档

      http://wiki.ros.org/robot_pose_ekf?distro=kinetic

      http://wiki.ros.org/robot_pose_ekf/Tutorials/AddingGpsSensor


http://chatgpt.dhexx.cn/article/uNRVt9BL.shtml

相关文章

【论文阅读】LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain

LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain 文章摘要I. 简介II. 硬件系统III. 轻量级激光雷达里程计和建图A. 系统概况B. 分割C. 特征提取D. 激光里程计E. 激光建图 IV. 实验V. 总结与讨论 本文主要是对LeGO-LOAM论文的一些…

ros移动机器人,激光雷达里程计rf2o_laser_odometry的使用与分析

目录 1.问题分析 2.激光雷达里程计 2.1 代码下载 2.2 使用方法 (1) ERRO&#xff1a;“base_link” passed to lookupTransform argument source_frame does not exist. (2)"Waiting for laser_scans...." (3)无法发布odom->base_footprint的tf信息 2.3 导…

Super odometry:以IMU为核心的激光雷达视觉惯性融合框架(ICRA2021)

Super Odometry: IMU-centric LiDAR-Visual-Inertial Estimator for Challenging Environments 作者&#xff1a;Shibo Zhao&#xff0c;Hengrui Zhang, Peng Wang, Lucas Nogueira&#xff0c;Sebastian Scherer 论文地址&#xff1a;https://arxiv.org/pdf/2104.14938.pdf 视…

svo: semi-direct visual odometry 论文解析

SVO 从名字来看&#xff0c;是半直接视觉里程计&#xff0c;所谓半直接是指通过对图像中的特征点图像块进行直接匹配来获取相机位姿&#xff0c;而不像直接匹配法那样对整个图像使用直接匹配。整幅图像的直接匹配法常见于RGBD传感器&#xff0c;因为RGBD传感器能获取整幅图像的…

LIMO: Lidar-Monocular Visual Odometry

IROS2018的论文 本文提出了LIMO SLAM框架&#xff0c;主要工作是融合了LiDAR和Monocular。 https://github.com/johannes-graeter/limo 前端 特征 本文使用的是viso2特征&#xff0c;它能 non-maimum suppression, outlier rejection 和 subpixel refinement. 30-40ms能提…

Visual Odometry技术 (Of VSLAM)

目录 什么是SLAM 经典视觉SLAM框架 Visual Odometry Lucas–Kanade光流 Acknowledgements and References 什么是SLAM SLAM是Simultaneous localization and mapping缩写&#xff0c;意为“同步定位与建图”1。它是指搭载了特定传感器的主体&#xff0c;如机器人或者无人…

ROS 位置姿态Odometry仿真模拟(gmapping)

使用单纯的雷达数据进行 2D 建立图像的时候&#xff0c;建图的效果和雷达的性能也有关系&#xff0c;雷达的扫描频率会随着小车的移动速度增大而变化&#xff0c;通过更改扫描的频率来更新栅格地图的完整性。 点击DEMO-LINK 所需要的环境 ubuntu 18.04 ROS Melodic desktop fu…

rospy Odometry天坑小计

rospy Odometry天坑小计 在使用 python 自己搭建 ros 控制小车的上位机接口时&#xff0c;遇到了一个非常奇怪的问题&#xff0c;发布里程计的时候&#xff0c;一旦里程计得到订阅&#xff0c;发布者就会报 ndarray 属性找不到错误&#xff0c;乍一看着实让人脑瓜疼&#xff0…

Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析(一)

Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析&#xff08;一&#xff09; 1. LiDAR inertial odometry and mapping简介2. Tightly coupled LiDAR inertial odometry2.1 LiDAR-IMU odometry overview2.2 IMU and pre-integration2.3 De-skewing and feature e…

SLAM——入门到放弃:ROS里程计(odometry)

ROS gmapping导航包&#xff0c;要求有2 个 输入&#xff0c;一个是激光数据&#xff0c;另一个就是里程计信息。 里程计又包含2 个方面的信息&#xff1a; 位姿&#xff08;位置和转角&#xff09;&#xff0c;即&#xff08;x,y,θ&#xff09;是速度&#xff08;前进速度和…

Ros Odometry获取机器人位置-python 代码打印

纪念一下&#xff0c;辛苦了一天多&#xff0c;终于找出来了&#xff0c;经过网上的资料反向推出结构。 博主需要通过机器人的实时位置来进行计算。从网上查阅得知nav_msgs/Odometry消息中存储机器人的位置和速度估计等。 下面展示一下操作流程&#xff1a; 1.首先通过 ros…

ROS里程计消息nav_msgs/Odometry的可视化方法

ROS中里程计的消息类型为nav_msgs/Odometry&#xff0c;该消息类型具有以下结构&#xff1a; 可以看到&#xff0c;里程计消息中的pose包含了位置pose.position和姿态pose.orientation 在ROS中&#xff0c;有一种常用消息类型为nav_msgs/Path&#xff0c;可视化的方法为&#…

基于rf2o_laser_odometry纯激光里程计的gmapping建图

ROS环境&#xff1a;ubuntu16.04 & ROS kinetic激光雷达&#xff1a;EAI-X4 or RPlidar-A1激光里程计&#xff1a;rf2o_laser_odometry建图&#xff1a;gmapping 对于很多刚入门的同学&#xff0c;购买一台带有高精度轮式里程计的ROS小车经济上往往不允许。但是大多数同学…

nav_msgs/Odometry消息的发布和tf变换

一。ROS使用tf来决定机器人的位置和静态地图中的传感器数据&#xff0c;但是tf中没有机器人的速度信息&#xff0c;所以导航功能包要求机器人 能够通过里程计信息源发布包含速度信息的里程计nav_msgs/Odometry 消息。 本篇将介绍nav_msgs/Odometry消息&#xff0c;并且通过代码…

【视觉SLAM】DM-VIO: Delayed Marginalization Visual-Inertial Odometry

L. v. Stumberg and D. Cremers, “DM-VIO: Delayed Marginalization Visual-Inertial Odometry,” in IEEE Robotics and Automation Letters, vol. 7, no. 2, pp. 1408-1415, April 2022, doi: 10.1109/LRA.2021.3140129. 论文阅读方法&#xff1a;Title&#xff0c;Abstract…

DSO(Direct Sparse Odometry)

DSO&#xff08;Direct Sparse Odometry&#xff09; 文章目录 1. 简述2. 概述3. 框架流程3.1 代码框架与数据表示3.2 VO流程 4. DSO详细介绍4.1 残差的构成与雅可比4.2 滑动窗口的维护与边缘化4.3 零空间&#xff0c;FEJ4.4 其他零散的模块和算法 5. 光度标定6. 评述7. 资料与…

SVO(SVO: fast semi-direct monocular visual odometry)

SVO2系列之深度滤波DepthFiltersvo_noteSVO&#xff08;SVO: fast semi-direct monocular visual odometry&#xff09;SVO 半直接视觉里程计【DepthFilter】深度滤波器【svopro】代码梳理 SVO&#xff08;SVO: fast semi-direct monocular visual odometry&#xff09;翻译 1、…

航迹推演

​ 做机器人底层程序的时候&#xff0c;经常用到航迹推演&#xff08;Odometry&#xff09;&#xff0c;无论是定位导航还是普通的方向控制。航迹推演中除了对机器人位姿进行估计&#xff0c;另一个很重要的关系是移动机器人前进速度、转向角速度与左轮速度、右轮速度之间的转换…

航迹推演(Odometry)

做机器人底层程序的时候&#xff0c;经常用到航迹推演&#xff08;Odometry&#xff09;&#xff0c;无论是定位导航还是普通的方向控制。航迹推演中除了对机器人位姿进行估计&#xff0c;另一个很重要的关系是移动机器人前进速度、转向角速度与左轮速度、右轮速度之间的转换。…

大数据基础编程+实验

本文仅用于分析和记录在校期间学习大数据分析课程的一点心得体会。 1、Ubuntu系统的安装和使用 本文采用Ubuntu16.04系统&#xff0c;安装系统省略&#xff0c;选择镜像后一直点击下一步 1.1 进入系统后&#xff0c;调整输入法&#xff0c;将输入法切换至中英文切换 1.2为…