从零开始手写VIO 第二章 IMU传感器

最新推荐文章于 2023-10-24 14:54:02 发布
最新推荐文章于 2023-10-24 14:54:02 发布
阅读量1.5k 收藏 9
点赞数 1
分类专栏： 深蓝-从零开始写VIO 文章标签： 计算机视觉 人工智能 c++
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/weixin_41281151/article/details/123276780
版权

第二章 IMU传感器

课程代码：

https://github.com/kahowang/Visual_Internal_Odometry/tree/main/%E7%AC%AC%E4%BA%8C%E7%AB%A0%20IMU%E4%BC%A0%E6%84%9F%E5%99%A8/homework_chapter2

参考博客：

深蓝学院《从零开始手写VIO》作业2

使用imu_utils工具生成IMU的Allan方差标定曲线

惯导笔记 - 传感器误差分析

1.生成仿真数据集，标定Allan方差

1.1.惯性器件的噪声分析

多传感器融合定位笔记(任乾)：

信号噪声的组成;

1)量化噪声

​ 一切量化操作所固有的噪声,是数字传感器必然出现的噪声;
​ 产生原因:通过AD采集把连续时间信号采集成离散信号的过程中,精度会损失,精度损失的大小和AD转换的步长有关,步长越小,量化噪声越小。

2)角度随机游走

​ 宽带角速率白噪声:陀螺输出角速率是含噪声的,该噪声中的白噪声成分;
​ 产生原因:计算姿态的本质是对角速率做积分,这必然会对噪声也做了积分。白噪声的积分并不是白噪声,而是一个马尔可夫过程,即当前时刻的误差是在上一时刻误差的基础上累加一个随机白噪声得到的。
角度误差中所含的马尔可夫性质的误差,称为角度随机游走。

3)角速率随机游走

​ 与角度随机游走类似,角速率误差中所含的马尔可夫性质的误差,称为角速率随机游走。而这个马尔可夫性质的误差是由宽带角加速率白噪声累积的结果。

4)零偏不稳定性噪声

​ 零偏:即常说的bias,一般不是一个固定参数,而是在一定范围内缓慢随机飘移。
​ 零偏不稳定性:零偏随时间缓慢变化,其变化值无法预估,需要假定一个概率区间描述它有多大的可能性落在这个区间内。时间越长,区间越大。

5)速率斜坡

​ 该误差是趋势性误差,而不是随机误差。
​ 随机误差,是指你无法用确定性模型去拟合并消除它,最多只能用概率模型去描述它,这样得到的预测结果也是概
率性质的。
​ 趋势性误差,是可以直接拟合消除的,在陀螺里产生这种误差最常见的原因是温度引起零位变化,可以通过温补来
消除。

6)零偏重复性

​ 多次启动时,零偏不相等,因此会有一个重复性误差。在实际使用中,需要每次上电都重新估计一次。
​ Allan方差分析时,不包含对零偏重复性的分析。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-g2pqoQpl-1646376743662)(/home/lory/Visual_Internal_Odometry/第二章 IMU传感器/pic/allen1.png)]

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-zC8J28kj-1646376743663)(pic/allen2.png)]

VIO 笔记(贺一家)

误差分类：

​ 加速度计和陀螺仪的误差可分为：确定性误差，随机误差。

​ 确定性误差可以事先标定确认，包括：bias，scale …

​ 随机误差通常假设噪声服从高斯分布，包括：高斯白噪声，bias随机游走

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-tArlRJw2-1646376743664)(pic/allen3.png)]

1.2 生成 ROS的imu数据集合

mkdir vio_sim_ws/src 
#将vio_data_simulation-ros_version  放进src中
catkin_make 
rosrun vio_data_simulation vio_data_simulation_node    # 生成 imu.bag 数据集

rqt_bag  info  imu.bag  # 查看当前数据包的内容
path:        imu.bag
version:     2.0
duration:    3hr 59:59s (14399s)
start:       Feb 24 2022 13:04:10.04 (1645679050.04)
end:         Feb 24 2022 17:04:10.04 (1645693450.04)
size:        1.0 GB
messages:    2880001
compression: none [1344/1344 chunks]
types:       sensor_msgs/Imu [6a62c6daae103f4ff57a132d6f95cec2]
topics:      imu   2880001 msgs    : sensor_msgs/Imu

1.3 imu_utils 完成allan 标定

1.3 imu_utils 完成allan 标定

编译，安装 imu_utils 工具包，主要参考网址：

LIO-SAM运行自己数据包遇到的问题解决–SLAM不学无数术小问题

用imu_utils标定IMU，之后用于kalibr中相机和IMU的联合标定

使用imu_utils工具生成IMU的Allan方差标定曲线

注意：code_utils 和 imu_utils 有先后顺序，不能放在一起编译

FILE : imu_utils_ws/src/imu_utils/launch 新建 sim_imu.launch

<launch>
    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
        <param name="imu_topic" type="string" value= "/imu"/>				<!--imu ROS话题  /imu -->
        <param name="imu_name" type="string" value= "sim_imu"/>
        <param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>	<!--保存表标定完毕后数据路径-->
        <param name="max_time_min" type="int" value= "230"/>		<!--读取数据的最大时间-->
        <param name="max_cluster" type="int" value= "100"/>
    </node>
</launch>

roslaunch imu_utils sim_imu.launch
rosbag  play  imu.bag  -r  500    # 以500倍速播放

标定结果： FILE : imu_utils_ws/src/imu_utils/data

注意： sim_imu 预设置的噪声 单位为 gyro : rad/(s * sqrt(hz)) acc : m/(s^2*sqrt(hz)) ， 而imu_utils 预设的的噪声单位为 gyro: rad/s acc: m/s^2。统一单位时，需要将imu_utils 的结果都除以 sqrt(hz) , sim_imu 的 采样频率为200hz , 所以 imu_utils 的结果都需要除以 sqrt(200) = 14.14

仿真imu数据，预设的imu_noise 单位

仿真imu数据，预设的imu_noise 单位

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-cJwTyIFK-1646376743664)(pic/phd_he_imu_nose_uint.png)]

imu_utils 标定后的单位

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-CmNK9HiJ-1646376743665)(pic/imu_utils_imu_noise_uint.png)]

误差类型真值 rad/(s * sqrt(hz)) m/(s^2*sqrt(hz))imu_utils rad/s m/s^2imu_utils 单位对齐后 rad/(s * sqrt(hz)) m/(s^2*sqrt(hz))
gyro white noise0.0150.211655310.014968
gyro bias random walk0.000050.000950790.0000672
acc white noise0.0190.234412960.016578
acc bias random walk0.00050.003491270.00024691

使用matlab 描绘gyro acc 的allan方差曲线

FILE: imu_utils_ws/src/imu_utils/scripts/draw_allan.m

gyro_allan 曲线

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-Zhv0gnhk-1646376743665)(pic/allan_gyro_matlab.png)]

acc_allan 曲线

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-TCZYbDYH-1646376743666)(pic/allan_acc_matlab.png)]

2.生成运动imu数据，使用中值法、欧拉法进行惯性积分

这里使用贺博的vio_data_simulation 代码进行生成轨迹

2.1 编译及运行

cd  vio_data_simulation
mkdir build 
cd buid 
cmake ..
make  -j				# 编译

cd .. 
cd bin
./data_gen				#运行后即会在当前目录下生成 imu 轨迹数据集

cd ..
cd  python_tool
python  draw_trajcory.py   #  绘制轨迹曲线

2.2 欧拉法进行imu的惯性解算

FILE : vio_data_simulation/src/imu.cpp

贺博原版代码，以进行欧拉法的惯性解算，公式如下所示

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-n5mMK4kz-1646376743666)(pic/euler.png)]

对应代码如下所示： testImu_euler()

//读取生成的imu数据并用imu动力学模型对数据进行计算，最后保存imu积分以后的轨迹，
//用来验证数据以及模型的有效性。
void IMU::testImu_euler(std::string src, std::string dist)
{
    std::vector<MotionData>imudata;
    LoadPose(src,imudata);

    std::ofstream save_points;
    save_points.open(dist);

    double dt = param_.imu_timestep;
    Eigen::Vector3d Pwb = init_twb_;              // position :    from  imu measurements
    Eigen::Quaterniond Qwb(init_Rwb_);            // quaterniond:  from imu measurements
    Eigen::Vector3d acc_w_last ;
    Eigen::Vector3d Vw = init_velocity_;          // velocity  :   from imu measurements
    Eigen::Vector3d gw(0,0,-9.81);    // ENU frame
    Eigen::Vector3d temp_a;
    Eigen::Vector3d theta;
    for (int i = 1; i < imudata.size(); ++i) {

        MotionData imupose = imudata[i];

        //delta_q = [1 , 1/2 * thetax , 1/2 * theta_y, 1/2 * theta_z]
        Eigen::Quaterniond dq;
        Eigen::Vector3d dtheta_half =  imupose.imu_gyro * dt /2.0;
        dq.w() = 1;
        dq.x() = dtheta_half.x();
        dq.y() = dtheta_half.y();
        dq.z() = dtheta_half.z();
        dq.normalize();
        
        /// imu 动力学模型 欧拉积分
        Eigen::Vector3d acc_w = Qwb * (imupose.imu_acc) + gw;  // aw = Rwb * ( acc_body - acc_bias ) + gw
        Qwb = Qwb * dq;
        Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
        Vw = Vw + acc_w * dt;
                
        //　按着imu postion, imu quaternion , cam postion, cam quaternion 的格式存储，由于没有cam，所以imu存了两次
        save_points<<imupose.timestamp<<" "
                   <<Qwb.w()<<" "
                   <<Qwb.x()<<" "
                   <<Qwb.y()<<" "
                   <<Qwb.z()<<" "
                   <<Pwb(0)<<" "
                   <<Pwb(1)<<" "
                   <<Pwb(2)<<" "
                   <<Qwb.w()<<" "
                   <<Qwb.x()<<" "
                   <<Qwb.y()<<" "
                   <<Qwb.z()<<" "
                   <<Pwb(0)<<" "
                   <<Pwb(1)<<" "
                   <<Pwb(2)<<" "
                   <<std::endl;
    }
    std::cout<<"test　end"<<std::endl;
}

2.3 中值法进行imu的惯性解算

FILE : vio_data_simulation/src/imu.cpp

中值法对比欧拉法，在获取 imu 的accel 和 gyro 信息时进行均值处理，更加接近真实值

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-DR6hs95D-1646376743667)(pic/middle.png)]

核心修改代码为 获取accel 和 gyro 部分

Eigen::Vector3d  gyro_middle  = (imupose_last.imu_gyro +  imupose.imu_gyro) / 2.0;  //  中值积分后的gyro
Eigen::Vector3d dtheta_half =  gyro_middle  * dt /2.0;

Eigen::Vector3d acc_w =  (Qwb_last * (imupose_last.imu_acc) + gw  + Qwb *  (imupose.imu_acc) + gw) / 2.0  ;

完整代码如下所示 ： testImu_middle()

//读取生成的imu数据并用imu动力学模型对数据进行计算，最后保存imu积分以后的轨迹，
//用来验证数据以及模型的有效性。   中值积分法
void IMU::testImu_middle(std::string src, std::string dist)
{
    std::vector<MotionData>imudata;
    LoadPose(src,imudata);

    std::ofstream save_points;
    save_points.open(dist);

    double dt = param_.imu_timestep;
    Eigen::Vector3d Pwb = init_twb_;              // position :    from  imu measurements
    Eigen::Quaterniond Qwb(init_Rwb_);            // quaterniond:  from imu measurements
    Eigen::Quaterniond Qwb_last(init_Rwb_);    //  记录上一时刻的姿态
    Eigen::Vector3d acc_w_last ;
    Eigen::Vector3d Vw = init_velocity_;          // velocity  :   from imu measurements
    Eigen::Vector3d gw(0,0,-9.81);    // ENU frame
    Eigen::Vector3d temp_a;
    Eigen::Vector3d theta;
    for (int i = 1; i < imudata.size(); ++i) {

        MotionData imupose = imudata[i];
        MotionData imupose_last = imudata[i-1];
        
        /// 中值积分
        Eigen::Quaterniond dq;
        Eigen::Vector3d  gyro_middle  = (imupose_last.imu_gyro +  imupose.imu_gyro) / 2.0;  //  中值积分后的gyro
        Eigen::Vector3d dtheta_half =  gyro_middle  * dt /2.0;
        dq.w() = 1;
        dq.x() = dtheta_half.x();
        dq.y() = dtheta_half.y();
        dq.z() = dtheta_half.z();
        dq.normalize();

        Eigen::Vector3d acc_w =  (Qwb_last * (imupose_last.imu_acc) + gw  + Qwb *  (imupose.imu_acc) + gw) / 2.0  ; 
        Qwb_last =  Qwb; 
        Qwb = Qwb * dq ;
        Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
        Vw = Vw + acc_w * dt;
        
        //　按着imu postion, imu quaternion , cam postion, cam quaternion 的格式存储，由于没有cam，所以imu存了两次
        save_points<<imupose.timestamp<<" "
                   <<Qwb.w()<<" "
                   <<Qwb.x()<<" "
                   <<Qwb.y()<<" "
                   <<Qwb.z()<<" "
                   <<Pwb(0)<<" "
                   <<Pwb(1)<<" "
                   <<Pwb(2)<<" "
                   <<Qwb.w()<<" "
                   <<Qwb.x()<<" "
                   <<Qwb.y()<<" "
                   <<Qwb.z()<<" "
                   <<Pwb(0)<<" "
                   <<Pwb(1)<<" "
                   <<Pwb(2)<<" "
                   <<std::endl;

    }
    std::cout<<"test　end"<<std::endl;
}

2.4 欧拉法 与 中值法 轨迹对比

通过下图的轨迹曲线对比，可看出使用middle 中值法进行imu 惯性积分得到的轨迹与真实值更加接近。

blue: GroundTruth orange: imu _int

Euler 欧拉法惯性积分Middle 中值法惯性积分
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-PPtRjZ34-1646376743668)(pic/euler_trajectory.png)][外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-zjKUitS9-1646376743669)(pic/middle_trajectory.png)]

​ edited by kaho 2022.2.24

KaHoWong
关注
专栏目录
从零开始手写VIO课件逻辑
weixin_44186910的博客
02-16 1220
第一讲 概述与课程介绍 1-9页说的是课程的概要：第四页说了这门课主要讲什么 • IMU 的工作原理和噪声方程(第二讲) • 视觉与 IMU 紧耦合的基础理论(第三讲) • 从零开始实现 VIO 紧耦合优化器（仅基于 Eigen）(四讲之后) 10-20页说的是VIO的概述：第十一至十四页说了什么是VIO，什么是IMU(组成、精度、使用场景等)，和相机有什么不同，二者的优劣对比；第十五页说了这两...
从零开始手写VIO课件
07-06
VIO 知识面广，公式繁多复杂，初学者入门时间长且不容易掌握，半途而废者居多，这也导致熟练掌握 VIO 的 SLAMer并不多，企业相关人才需求缺口较大。为此，贺一家与高博在深蓝学院联合推出了『从零开始手写VIO』课程。通过这门课程，我们将详细讲解VIO的实现细节。更重要的是，课程将手写后端作为重点。 从零手写VIO课程知识点整理
从零开始手写VIO第一章作业（含详细推导过程）
cuifeng1993的博客
07-08 4591
这里写自定义目录标题前言·与同主题博文的不同之处1.VIO 文献阅读2.四元数和李代数更新2.1理论依据2.2代码实现3.其他导数3.1第一题3.2第二题 前言·与同主题博文的不同之处 贺一家、高翔、崔华坤老师在深蓝学院开设了vSLAM进阶课程《从零开始手写VIO》，课程相当火爆，已经有数篇关于第一章作业的优秀博文。不过小编觉得还可以从两个方面进一步优化这个主题的博文。首先，第二道编程题和第三道公式推导题中涉及到的理论依据和参考资料可以明确给出，给出公式推导每一行对应的理论依据，具体到哪一本书的哪一个公式，
从零开始手写 VIO
热门推荐
知行合一
05-13 1万+
前言 最近和高博合作推出了一个关于 VIO 的课程，借此博客推荐下。这个课程的图优化后端是我们自己写的，仅依赖 Eigen, 实现后系统的精度和 ceres 以及 g2o 不相上下。 个人感觉这个课程还是能学到不少东西，特别是网上关于 SLAM 后端的资料比较少。 下面是一些关于这个课程的介绍： 这两年视觉 SLAM 相当火了，作为面试官常常感叹现在的小伙伴比我们那会厉害多了，简历中一般都会说熟悉...
从零手写VIO(1)
weixin_45559915的博客
06-15 458
从零手写VIO(1)
1.从零开始手写 VIO-学习准备
算法攻城狮
10-24 977
目录 1.VIO概述 2.VIO分类 3.为什么要使用紧耦合 4.本课程要探讨的问题 1.VIO概述 以视觉与 IMU 融合实现里程计 IMU(惯性测量单元)： 典型 6 轴 IMU 以较高频率（≥ 100Hz）返回被测量物体的角速度 与加速度 受自身温度、零偏、振动等因素干扰，积分得到的平移和旋转容 易漂移 视觉： 以图像形式记录数据，频率较低（15 - 60Hz 居多）...
手写VIO第一章作业答案.pdf
03-04
综上所述，VIO结合视觉传感器和IMU的优势，提出了一系列的融合方案，这些方案不仅在学术界引起了广泛关注，也在工业界得到了广泛应用。随着深度学习技术的发展，VIO系统正朝着更高精度和更强鲁棒性迈进。同时，...
从零手写VIO学习记录——系列一：【VIO基础知识，四元数与李代数】（学习记录 | 作业 | 代码）
01-06
本系列主要根据深蓝学院《视觉SLAM进阶：从零开始手写VIO》。博客主要以本人在课程学习过程中的作业和修改的代码为主。如需详细视频及PPT，请联系深蓝学院相关人员。 代码已上传github: ...
视觉SLAM进阶：从零开始手写VIO（第二章）
m0_49114997的博客
10-24 137
（2）至于终端弹出的no handles...由于没学过python并不知如何解决，但是不影响运行。在vio_data_simulation/bin下生成了data_gen可知性文件。在vio_data_simulation目录下有python_tool文件夹。（1）matplotlib是python语言中用来绘图的工具；1..直接在终端打开vio_data_simulation。执行python3 draw_points.py。运行之后生成.txt文件，用于绘图。2.标准外部编译运行。
从零手写VIO（一）——初识VIO
图像处理、SLAM、PCL、OpenCV爱好者
06-16 2716
从零手写VIO第一讲作业 前面几个综述问题大部分 完全参考的大佬的博客https://blog.csdn.net/learning_tortosie/article/details/102408738 公式推导部分略有不同 1. 视觉与IMU融合之后有何优势？ 要说视觉与IMU融合之后有何优势，首先要明白视觉与IMU各自的局限： （1）IMU虽然可以测得加速度和角速度，但这些量都存在明显的漂移，使得积分两次得到的位姿数据非常不可靠。好比说，我们将IMU放在桌上不动，用它的读数积分得到的位姿也会漂出十万八千里
从零开始手写VIO第五章作业（含理论说明及编程思维过程）
cuifeng1993的博客
08-09 2616
文章目录前言·与同主题博文的不同1.完成单目 Bundle Adjustment (BA) 求解器 problem.cc 中的部分代码1.1完成 Problem::MakeHessian() 中信息矩阵 H 的计算1.2完成 Problem::SolveLinearSystem() 中 SLAM 问题的求解1.2.1理论及代码1.2.2仿真结果及分析2.完成滑动窗口算法测试函数2.1完成 Problem::TestMarginalize() 中的代码，并通过测试3.总结论文4.在代码中给第一帧和第二帧添加
深蓝学院从零开始手写VIO（一）——IMU模型
weixin_43991178的博客
02-28 9328
深蓝学院从零开始手写VIO（一）——IMU模型旋转运动学 声明：本专栏文章为深蓝学院《从零开始手写VIO》课程个人学习笔记，更多学习资源请咨询深蓝学院相关课程。 旋转运动学 ...
从零开始手写vio-ch3作业
joun772的博客
11-27 341
1 1 2 修改的代码如下： 对于函数y=ax2+bx+c 来说 它的残差特别简单，它的雅克比页特变简单 error=ax2+bx+c-y (这里面的a,b,c是待优化的变量 ，y是观测值，x是数据点) J=[x2 , x, 1] // 计算曲线模型误差 virtual void ComputeResidual() override { Vec3 abc = verticies_[0]->Parameters(); // 估计的参数 residua.
3.从零开始手写 VIO-习题
算法攻城狮
10-25 314
后续加
从零开始手写VIO第二章作业
waihekor的博客
02-25 929
从零开始手写VIO第二章作业
从零开始手写VIO第三章作业（含关键点细节及思维过程）
cuifeng1993的博客
07-26 2832
文章目录前言·与同主题博文的不同1.设置 IMU 仿真代码中的不同的参数，生成 Allen 方差标定曲线1.1环境搭建 前言·与同主题博文的不同 延续该系列第一篇博文的风格，本博文站在之前同主题优秀博文的肩膀上的同时，尽量补充对于新手朋友来说比较重要的细节，如文件所在路径截图，前后步骤之间的联系等。另外还会给出一些小编认为很重要但是前人博文中忽略或者存在争议的点。 1.设置 IMU 仿真代码中的不同的参数，生成 Allen 方差标定曲线 课程给了两个标定工具，imu_utils和kalibr_allan，相
深蓝学院《从零开始手写VIO》作业一
weixin_44580210的博客
01-08 1900
深蓝学院《从零开始手写VIO》作业一深蓝学院《从零开始手写VIO》作业一1. VIO文献阅读1.1 视觉与IMU进行融合之后有何优势？1.2 有哪些常见的视觉+IMU融合方案？有没有工业界应用的例子？1.3 在学术界，VIO研究有哪些新进展？有没有将学习方法用到VIO中的例子？2. 四元数和李代数更新3. 其他导数 深蓝学院《从零开始手写VIO》作业一 1. VIO文献阅读 阅读VIO相关综述文献...
从零开始手写vio 笔记
最新发布
12-16
从零开始手写vio 笔记很简单。首先，准备一本笔记本和一支笔。然后，找一个安静舒适的地方，准备开始记录。 开始写之前，先留出每一页的标题和日期，这样可以更好地组织和查找笔记。然后，根据需要，可以使用不同的...
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明：

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

余额充值