loam中imu消除重力加速度的数学推导

LOAM源码解读之IMU重力补偿
原创 已于 2022-11-04 15:13:03 修改 · 8.6k 阅读 · 65 ·
CC 4.0 BY-SA版权
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
文章标签：

#slam

于 2020-12-21 09:12:44 首次发布
本文深入解析LOAM激光雷达建图系统中IMU重力加速度补偿算法，通过欧拉角到旋转矩阵的转换，计算并消除IMU因重力产生的加速度偏差。

最近在看loam的源码发现里面有一段关于imu消除重力加速度的源码，刚开始看不明白后来终于搞清楚了，欢迎大家批评指正。

要理解这个问题首先得明白欧拉角到旋转矩阵的变换。。先上图

此图描述的是先绕X，再绕Y，再绕Z的欧拉角到旋转矩阵R的变换，请注意R是正交矩阵，即R乘R的转置为 E。
由于imu受重力的影响会挤压下表面从而产生一个向上的加速度，因此重力加速度的坐标在原世界坐标系下的坐标为[0,0,9.8],则根据坐标变换可知，[0,0,9.8]=R*[x,y,z],则重力加速度在imu坐标系下的坐标[x,y,z]为R的转置乘于[0,0,9.8]。

代进去乘得到[x,y,z]=[-9.8sin,9.8sincos,9.8cos*cos],符号不知道怎么打出来，大家自己算下，很简单。。。，再用总的imu加速度坐标相减即可。

loam 中imu消除重力的部分：
在这里插入图片描述

易子岚
关注
VIO学习笔记（二）—— IMU 传感器
梁小憨憨的博客
03-25 5446
学习资料是深蓝学院的《从零开始手写VIO》课程，对课程做一些记录，方便自己以后查询，如有错误还请斧正。由于习惯性心算公式，所以为了加深理解，文章公式采用手写的形式。 IMU 传感器旋转运动学线速度与角速度旋转坐标系下的运动学IMU 测量模型及运动模型MEMS 加速度计工作原理陀螺工作原理音叉振动陀螺陀螺仪的 G-sensitivityIMU 误差模型误差分类确定性误差BiasScaleNonort...
slam学习笔记七----IMU传感器
YOULANSHENGMENG的博客
04-02 4841
一，IMU 惯性测量单元 惯性测量单元（IMU）包含了三个单轴的加速度计和三个单轴的陀螺仪，加速度计检测物体在载体坐标系统独立三轴的加速度信号，而陀螺仪检测载体相对于导航坐标系的角速度信号，测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。在导航中用着很重要的应用价值。 IMU可获得载体的姿态、速度和位移等信息，被广泛用于汽车、机器人领域，也被用于需要用姿态进行精密位移推算的场合，如潜艇、飞机等惯性导航设备中。 IMU模块IMU的定义及功能慣性测量单元被...
(学习笔记)关于imu如何消除重力带来的影响，通过roll, pitch角计算
qq_41898018的博客
10-23 4345
1.我们首先假设车子平稳的在路上行走，然后他在上一个角度为pitch的坡，此时roll角为0，如下图所示 如上图所示，此时的重力分量g可以分解为两个分量，一个为g1，垂直于x，一个是x上的分量 2.此时假如坡度发生变化，倾斜了roll角，如下图所示 3.此时的g1产生了两个分量，其中一个是z轴，其中一个是y轴，公式如下 4.由第一张图可得g1 = g*cos(pitch) ，于是可以得如下公式 ...
基于重力传感器和陀螺仪(IMU)的方向检测
03-14
国外大牛写的，基于重力传感器和陀螺仪来检测方向和动作，里面有不少关于算法的细节描述，值得参考！
LOAM中消除IMU重力影响解析
weixin_44492854的博客
10-24 7313
在LOAM的处理中，IMU数据甫一进入系统第一步是对重力分量的去除。LOAM中对重力影响的去除有以下公式： float accX = vec_imuData[pointer].accy - sin(imuRoll[pointer]) * cos(imuPitch[pointer]) * 9.81; float accY = vec_imuData[pointer].accz - cos(imuRoll[pointer]) * cos(imuPitch[pointer]) * 9.81; float accZ
关于LOAM中IMU去除重力加速度的个人理解
qq_44305240的博客
09-10 1357
LOAM中IMU消除重力加速度影响
关于LOAM系列SLAM算法中IMU的去重力和坐标系变换
WillWinston的博客
08-30 4405
一、坐标系 LOAM系列中，IMU的坐标系和载体坐标系是对齐的，都是X向前，Y向左，Z向上的右手坐标系。 二、IMU去除重力 IMU的安装坐标系朝向和载体坐标系朝向是一致的。IMU得到的角度是IMU当前姿态向载体坐标系对齐的过程，即旋转顺序为：x(roll)–>y(pitch)–>z(yaw)；或者说是载体坐标系向IMU坐标系对齐的过程，即旋转顺序为：z(yaw)–>y(pitch)–>x(roll)。如果旋转有正负的话，应该为第二种。 重力加速度在当前IMU坐标系下表示为：[xIG
LeGo-LOAM源码篇:源码分析(2)
slamml
10-28 2141
featureAssociation.cpp，主要内容包含去除运动畸变、计算曲率、特征提取、特征匹配、位姿更新。
LeGO-LOAM 源码阅读笔记(featureAssociation.cpp)
wykxwyc的博客
08-03 7468
LeGO-LOAM是一种在LOAM之上进行改进的激光雷达建图方法，建图效果比LOAM要好，但是建图较为稀疏，计算量也更小了。 本文原地址：wykxwyc的博客 github注释后LeGO-LOAM源码：LeGO-LOAM_NOTED 关于代码的详细理解，建议阅读： 1.地图优化代码理解 2.图像重投影代码理解 3.特征关联代码理解 4.LeGO-LOAM中的数学公式推导 以上博客会随时更新，如果...
高翔【自动驾驶与机器人中的SLAM技术】学习笔记（十）高翔书中的细节：参考链接；卫星导航；ESKF
xiaoyaolangwj的博客
09-27 1985
呕心沥血，理解整理ESKF
MATLAB实现手机重力加速度去除
01-06
做这个的来由是一哥们想要通过手机的加速度和方位角数据，分离出手机在无重力情况下的加速度（类似于NS健身环的功能）。 那怎么实现呢：首先我们有的数据是手机的三个方位角度α，β，γ以及xyz方向上的加速度a_x，a_y，a_z。这个问题的关键在于怎么找出重力加速度，并把它去掉，那我们可以先把手机的方位还原为初始状态，即α，β，γ均为0，通过使用欧拉角变换公式： 通过欧拉角变换后，重力加速度被聚集到z分量上，直接减去一个g即可剩余的加速度即是无重力情况下的加速度。以下是基于MATLAB的方法： close all; clear all, clc a_init = [ 0; 5; 0];% 输入加速度
MPU6050 去除重力分量算法
05-26
用matlab仿真实现去除MPU6050三轴加速度中的重力加速度分量
Gait-Tracking-With-x-IMU-master.rar_Gait Tracking_IMU_去除重力
09-14
由于加速度计采到的数据，会受到重力轴的影响，本代码主要是去除重力。
SLAM文献之-FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman
qq_36812406的博客
03-03 1225
Fast-LIO（Fast LiDAR-Inertial Odometry）是一种高效的激光雷达-惯性里程计（LIO）算法，专为实时处理高频激光雷达数据而设计。由于激光雷达扫描频率较低（通常10-20Hz），而IMU频率较高（100-1000Hz），Fast-LIO利用反向传播技术对激光点云进行运动畸变补偿。Fast-LIO通过紧耦合IMU与激光雷达数据，结合iEKF和反向传播技术，在高速、高动态场景下实现了实时、高精度的状态估计。的更新通过误差状态卡尔曼滤波（ESKF）实现，避免直接处理四元数的协方差。
Logo-loam中imu消除重力影响
看看就好，IT小白
10-17 867
Logo-loam中imu消除重力影响代码解释
基于ROS2的消除重力加速度对IMU加速度影响，动态获取当前重力加速度。
To be a better man
08-26 2864
IMU静止时，与IMU所在平面会有一个向上的支持力，IMU就会测量到一个角速度，由于IMU与地面不是完全垂直，存在一定角度的误差，因此重力的反作用力也就是支持力会按照夹角分配到IMU各个轴上，我们就是将与重力坐标系转到IMU坐标系，将IMU坐标系得到的值减去重力在每个轴上的分量，就能消除重力对加速度的影响。因为当前IMU在斜坡上，理论加速度应该为(0,0,0) ，但实际上为（4.874595 ， -0.502782 ， 8.432.378）。如果IMU与水平面平行，理论上的值应该是(0,0,9)
将MPU6050的重力加速度从加速度计的数据中分离用以计算三维线性加速度
godmial的博客
08-13 2014
将MPU6050的重力加速度从加速度计的数据中分离
将姿态的航向角，俯仰角和滚动角，与x,y,z轴的加速度结合，将姿态变化导致的x，y加速度变化去掉的惯导程序
2301_79311842的博客
08-14 686
然而，完全去除由姿态变化引起的加速度影响在现实中是复杂的，因为它依赖于准确的姿态估计（通常通过陀螺仪、加速度计和/或磁力计的组合数据获得）。这里，我将提供一个简化的C程序示例，该程序假设你已经有了一个姿态估计（以四元数形式），并尝试从加速度计读数中去除重力加速度的影响。在处理姿态和加速度数据时，一个常见的需求是从加速度计读数中去除由于设备姿态变化（如旋转）引起的加速度分量，只保留线性加速度（即与重力无关的加速度）。首先，我们需要定义一些基本的数学函数和结构体来处理四元数和向量运算。
LeGO-LOAM+IMU定位
最新发布
07-11
LeGO-LOAM是一种轻量级且地面优化的SLAM框架，其设计初衷是为了在嵌入式系统上实现实时六自由度姿态估计。IMU（惯性测量单元）作为提供短时间高频率运动数据的重要传感器，在LeGO-LOAM中发挥了关键作用[^4]。 ### IMU在LeGO-LOAM中的集成 1. **IMU数据的作用** IMU提供了加速度和角速度信息，能够在短时间内快速响应机器人的动态变化，尤其是在激光雷达数据可能受到干扰或稀疏的情况下，IMU数据能够补充机器人姿态估计的信息缺失。在LeGO-LOAM中，IMU数据主要用于辅助激光雷达进行姿态估计，并帮助提高定位的鲁棒性和精度[^2]。 2. **IMU与激光雷达数据融合机制** LeGO-LOAM采用了一个两步优化策略，将IMU数据与激光雷达提取的特征点（如边缘和平面特征）结合[^1]。具体而言： - 在第一阶段，利用IMU提供的短期高精度运动预测作为初始位姿估计。 - 在第二阶段，使用激光雷达提取的特征点与地图进行匹配，进一步优化该位姿估计。 - 这种方式有效地降低了纯激光雷达方法中可能出现的漂移问题，并提升了整体系统的稳定性[^3]。 3. **误差补偿与优化** IMU存在固有的偏差（bias）和噪声问题，这些误差会随着时间累积影响定位精度。为此，LeGO-LOAM通过因子图优化的方式对IMU的偏差进行了在线估计和修正。具体来说，IMU预积分因子被引入到因子图中，与其他因子（如激光雷达里程因子、GPS因子等）一起参与联合优化，从而实现对IMU偏差的实时校正[^3]。 4. **地平面分割与IMU辅助定位** LeGO-LOAM的一个显著特点是其对地面车辆的优化。在点云处理过程中，首先会对点云进行地平面分割，去除地面点后，保留的非地面点用于特征提取。这一过程减少了不必要的计算开销，并提高了特征提取的准确性。在此基础上，IMU数据用于提供初始姿态估计，进一步增强了地面车辆在复杂地形环境下的定位能力。 5. **实验验证与性能对比** 实验表明，LeGO-LOAM在结合IMU数据后，相比仅依赖激光雷达的LOAM方法，在减少计算成本的同时保持了较高的定位精度。特别是在KITTI数据集上的测试结果显示，加入IMU后，LeGO-LOAM在长距离运行中表现出更小的姿态估计误差，且在回环检测的支持下，能够有效消除累计误差[^2]。 ### 示例代码片段 以下是一个简单的示例代码片段，展示了如何在ROS环境中订阅IMU数据并将其用于LeGO-LOAM的定位流程： ```cpp #include <ros/ros.h> #include <sensor_msgs/Imu.h> #include <nav_msgs/Odometry.h> #include <tf/transform_broadcaster.h> class ImuHandler { public: ImuHandler() : nh_("~") { imu_sub_ = nh_.subscribe("imu_topic", 10, &ImuHandler::imuCallback, this); odom_pub_ = nh_.advertise<nav_msgs::Odometry>("odom", 10); } private: void imuCallback(const sensor_msgs::ImuConstPtr& msg) { // 提取IMU数据中的线加速度和角速度 double ax = msg->linear_acceleration.x; double ay = msg->linear_acceleration.y; double az = msg->linear_acceleration.z; double gx = msg->angular_velocity.x; double gy = msg->angular_velocity.y; double gz = msg->angular_velocity.z; // 使用IMU数据更新当前位姿估计 updatePose(ax, ay, az, gx, gy, gz); // 发布当前估计的位姿 publishOdometry(); } void updatePose(double ax, double ay, double az, double gx, double gy, double gz) { // 简单的积分模型，实际应用中应使用更复杂的滤波器或因子图优化 // 此处仅为示例，展示如何用IMU数据更新位姿 static ros::Time last_time = ros::Time::now(); ros::Time current_time = ros::Time::now(); double dt = (current_time - last_time).toSec(); // 更新角度 roll_ += gx * dt; pitch_ += gy * dt; yaw_ += gz * dt; // 更新位置（简化为匀速模型） x_ += ax * dt * dt / 2.0; y_ += ay * dt * dt / 2.0; z_ += az * dt * dt / 2.0; last_time = current_time; } void publishOdometry() { nav_msgs::Odometry odom; odom.header.stamp = ros::Time::now(); odom.header.frame_id = "odom"; odom.child_frame_id = "base_link"; // 设置位置 odom.pose.pose.position.x = x_; odom.pose.pose.position.y = y_; odom.pose.pose.position.z = z_; // 设置方向（从欧拉角转换为四元数） tf::Quaternion q = tf::createQuaternionFromRPY(roll_, pitch_, yaw_); odom.pose.pose.orientation.x = q.x(); odom.pose.pose.orientation.y = q.y(); odom.pose.pose.orientation.z = q.z(); odom.pose.pose.orientation.w = q.w(); // 设置速度（此处设为零） odom.twist.twist.linear.x = 0.0; odom.twist.twist.linear.y = 0.0; odom.twist.twist.angular.z = 0.0; odom_pub_.publish(odom); } ros::NodeHandle nh_; ros::Subscriber imu_sub_; ros::Publisher odom_pub_; double x_ = 0.0, y_ = 0.0, z_ = 0.0; double roll_ = 0.0, pitch_ = 0.0, yaw_ = 0.0; }; int main(int argc, char** argv) { ros::init(argc, argv, "imu_handler"); ImuHandler handler; ros::spin(); return 0; } ``` ###
评论 27
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
红包 添加红包
表情包 插入表情
表情包 代码片
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值