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

最新推荐文章于 2023-07-26 15:45:02 发布
最新推荐文章于 2023-07-26 15:45:02 发布
阅读量1.4k 收藏 8
点赞数
分类专栏： 深蓝-从零开始写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
关注
  • 0
    点赞
  • 踩
  • 8
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
DSO/LDSO 等直接法相关资料整理
weixin_50578602的博客
03-14 491
直接法视觉里程计/SLAM的学习门槛较特征法高，我将DSO/LDSO 等直接法相关资料尽量按作者分类整理如下，毕竟沿着一个人的思路学习跨度不会太大；此外，将一些涉及的其他知识也一并整理，方便查阅。
从零开始手写VIO课件
07-06
VIO 知识面广，公式繁多复杂，初学...为此，贺一家与高博在深蓝学院联合推出了『从零开始手写VIO』课程。通过这门课程，我们将详细讲解VIO的实现细节。更重要的是，课程将手写后端作为重点。 从零手写VIO课程知识点整理
深蓝学院从零开始手写VIO（一）——IMU模型
weixin_43991178的博客
02-28 7051
深蓝学院从零开始手写VIO（一）——IMU模型旋转运动学 声明：本专栏文章为深蓝学院《从零开始手写VIO》课程个人学习笔记，更多学习资源请咨询深蓝学院相关课程。 旋转运动学 ...
惯性导航学习笔记——惯性技术基础知识
abanchao的博客
08-22 1928
多传感器融合定位中最重要的传感器是IMU，试验平台上搭载了轮趣的N100、北云科技的A1和华测组合导航接收机CGI 610。各产品的IMU精度不同，那么各产品的精度是否存在虚标或者因为产品个体差异存在的偏差呢？精度差异的体现又是什么呢？那就自己来进行学习和对比分析！
陀螺仪那点事
linan101的博客
11-09 1万+
首先从问题说起，因为我最近在和陀螺仪厂商讨论陀螺仪对我们真正使用的影响。所以我先从陀螺仪的指标说起，基本知识会放在后面。 首先介绍一下零偏的的概念。 零偏 零偏：称为零位漂移或零位偏移或零偏稳定性，也可简称零漂或漂移率，英文中称为drift 或bias drift。零偏应理解为陀螺仪的输出信号围绕其均值的起伏或波动，习惯上用标准差（σ）或均方根（RMS ）表示，一般折算为等效输入角速率(°/ h) 。在角速度输入为零时，陀螺仪的输出是一条复合白噪声信号缓慢变化的曲线，曲线的峰-峰值就是零偏值（drift ）
利用 imu_utils 标定 imu
er_dan_love的博客
04-23 5778
前言 记录利用 imu_utils 标定 imu，最近在使用imu做导航，要对imu进行标定，使用标定得到的加速度的噪声和加速度随机游走噪声以及角速度的噪声和角速度的随机游走噪声。 一、安装 imu_utils 1、安装依赖项 sudo apt-get install libdw-dev 2、安装ceres 参考高博slam十四讲，安装ceres。 3、编译 code_utils cd ~/imu_calib_ws/src git clone https:...
手写VIO第一章作业答案.pdf
03-04
手写VIO第一章优秀答案：第一题是VIO文献阅读进行总结，第二题是四元数和李代数的更新，第三题是求导。
从零开始手写vio-第6节：视觉前端.pdf
09-08
vio 视觉前端 视觉前端
从零开始手写vio-第5节滑动窗口算法实践.pdf
09-08
从零开始手写vio- 第5节滑动窗口第5节滑动窗口算法实践
从零手写VIO学习记录——系列一：【VIO基础知识，四元数与李代数】（学习记录 | 作业 | 代码）
01-06
本系列主要根据深蓝学院《视觉SLAM进阶：从零开始手写VIO》。博客主要以本人在课程学习过程中的作业和修改的代码为主。如需详细视频及PPT，请联系深蓝学院相关人员。 代码已上传github: ...
从零手写VIO-实现更可靠的定位方案
3D视觉工坊
05-25 830
自主导航是移动机器人的核心功能，而这一功能是建立在移动机器人定位技术之上。目前很多移动机器人都具备了IMU，GPS、相机等多种模态的传感器，虽然单传感器定位能初步实现定位，但是单传感器定位在不同的应用场景中存在很多问题。如：在穿越隧道或者信号不好的区域，IMU容易发生退化无法工作；在快速运动时，相机功能无法连续获得清晰的图像；当图像发生变化时，相机无法得知是自身产生了运动...
从零开始手写 VIO
热门推荐
知行合一
05-13 1万+
前言 最近和高博合作推出了一个关于 VIO 的课程，借此博客推荐下。这个课程的图优化后端是我们自己写的，仅依赖 Eigen, 实现后系统的精度和 ceres 以及 g2o 不相上下。 个人感觉这个课程还是能学到不少东西，特别是网上关于 SLAM 后端的资料比较少。 下面是一些关于这个课程的介绍： 这两年视觉 SLAM 相当火了，作为面试官常常感叹现在的小伙伴比我们那会厉害多了，简历中一般都会说熟悉...
IMU噪声标定——加速度计和陀螺仪的白噪声和零偏不稳定性
W_Tortoise的博客
05-06 1万+
首先，安装依赖： sudo apt-get install libdw-dev 作者在README.md中的编译步骤为： download required code_utils; put the ROS package imu_utils and code_utils into your workspace, usually named catkin_ws; cd to your work...
【学习总结】学习惯导模块
larrydong的博客
07-08 3854
本文仅用于记录自己在项目中选择惯导模块中，查到的一些资料。 1. 陀螺的类型 MEMS惯导、光纤惯导、激光惯导。 MEMS指“微机电系统”，相对来说精度、价格较低；据说5w左右就算是不错的 光纤、激光，为光学陀螺，利用萨格奈克效应。激光的价格和精度高于光纤。价格一般10w+ 一些厂商[1]： 2. 关于GPS 失锁：由于遮挡等原因，造成无法接受GPS信号。 如果可以收到卫星信号，可以采用组合导航的方法，运行精度会高很多。 如果只有在启动时能够收到卫星信号，也能对纯IMU的漂移进行一定的处理，也比纯IMU精
随机游走笔记（一）
天道酬勤
07-26 3808
这就是随机游走的中心极限定理的体现：随着步数的增加，随机游走的位置分布趋近于正态分布。为了演示随机游走的中心极限定理，我们可以生成多个随机游走路径，并观察随着步数的增加，随机游走路径的位置分布是否趋近于正态分布。随机游走是一个非常有用的工具，可以帮助我们理解复杂的随机过程和现象，并在各种学科和领域中提供实用的建模和分析方法。对称性：在一维的离散随机游走中，如果步长的概率分布在左右移动时是对称的，那么称为对称随机游走。：随机游走的中心极限定理告诉我们，随着步数的增加，随机游走的位置将趋于正态分布。
手写VIO第一讲习题:第二题, 更新方式验证
VINO6520的博客
06-13 1381
#include<iostream> #include<cmath> #include"/usr/include/eigen3/Eigen/Core" //包含几何模块 #include"/usr/include/eigen3/Eigen/Geometry" #include "/usr/local/include/sophus/so3.h" using namespac...
从VINS-MONO到VIO学习（一）
AsJHJYHJYG的博客
06-30 409
VINS
imu噪声的建模理解
weixin_42681311的博客
08-03 2054
imu噪声模型
手写VIO学习总结（一）
蒋成的博客
11-02 2852
文章目录1.简介作业1.1作业1.2作业1.2 1.简介 贺一家和高博合作推出了一个关于 VIO 的课程，此课程主要为VIO学习者提供学习资源。VIO不仅应用在无人车、机器人，还应用于手机 AR 等领域。可以说，VIO 已成为 vSLAM 算法工程师的必备技能。课程介绍如下： 本博客主要记录学习课程的笔记和课程的作业。 作业1.1 阅读VIO 相关综述文献如a，回答以下问题： • 视觉与IMU ...
从零开始手写vio 笔记
最新发布
12-16
从零开始手写vio 笔记很简单。首先，准备一本笔记本和一支笔。然后，找一个安静舒适的地方，准备开始记录。 开始写之前，先留出每一页的标题和日期，这样可以更好地组织和查找笔记。然后，根据需要，可以使用不同的颜色和标记方式来突出重点或者归类信息。 在记录过程中，要尽量简明扼要地表达观点和信息，避免冗长的文字。可以使用简单的图表或者符号来帮助理解和记忆。 另外，在整理和复习笔记的时候，可以添加一些额外的说明和补充，以便更全面地理解和掌握知识。 最后，及时整理和归档笔记，保持笔记的整洁和有序。这样可以方便日后的查找和回顾。 总的来说，从零开始手写vio 笔记并不难，只要有一颗认真和耐心的心，就可以轻松记录并掌握所需的知识。

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明：

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

余额充值