vio-视觉与IMU融合实现里程计（第二章）——作业

最新推荐文章于 2025-03-25 19:57:59 发布

思考之路

最新推荐文章于 2025-03-25 19:57:59 发布

阅读量642

点赞数

分类专栏： vio 文章标签：自动驾驶 matlab 计算机视觉

本文链接：https://blog.youkuaiyun.com/weixin_46417419/article/details/124604064

版权

vio 专栏收录该内容

10 篇文章

订阅专栏

本文介绍了如何使用imu_utils和kalibr_allan工具在ROS环境下生成并分析IMU数据的Allan方差标定曲线。通过修改代码参数，实现了静态IMU数据的生成，然后使用imu_utils的matlab脚本进行数据处理和曲线绘制。此外，还对比了欧拉积分与中值积分在IMU数据仿真中的效果。最后，探讨了从已知轨迹生成IMU数据的方法。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

$\sum\limits_{i=0}^{n} (_{i}^{n}) P_i(1 - t)^{n-i}t^i$

\int_{-N}^{N}
在这里插入图片描述

1, 设置IMU仿真代码中的不同的参数，生成Allen方差标定曲线。
课后习题中给出的代码中非ros的代码是用来处理第2题，显示离散时间下指定轨迹的角速度和加速度数据仿真，而ros的代码是用来生成静态的imu数据，用于allan方差标定曲线的，可以使用的工具有：imu_utils和kalibr_allan两种。

对于ROS：专门生成静止IMU数据，用于allan方差标定
1.1 使用imu_utils完成allan标定步骤

ros下编译
执行，生成imu.bag
rosbag play -r 500 imgimu_utils.bag回放
用imu_utils进行接收和分析
用imu_utils下的scripts/下的matlab脚本化allan曲线

详细步骤：使用imu_utils工具生成allen方差标定曲线
第一步：创建工作空间，用系统命令创建工作空间目录，然后运行ROS的工作空间初始化命令即可完成创建过程

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
catkin_imit_workspace

第二步：下载code_utils并编译

cd ~/catkin_ws/src
git clone git clone https://github.com/gaowenliang/code_utils
cd ~/catkin_ws
catkin_make

第三步：下载标定工具imu_utils并编译

cd ~/catkin_ws/src
git clone https://github.com/gaowenliang/imu_utils.git
cd ~/catkin_ws
catkin_make

注意：code_utils和imu_utils的下载编译顺序

第四步：在高博给出的vio_data_simulation-ros_version/src下的genera_alldata.cpp中修改需要存储的imu.bag的路径

const std::string bag_path ="./imu.bag";

第五步:在工作空间目录下进行编译，生成vio_data_simulation_node节点
在这里插入图片描述
第六步：在ubuntu中打开一个终端，输入roscore命令运行ROS的节点管理器——ROS Master，这个ROS必须运行的管理器节点，然后再打开一个新的终端启用rosrun启动生成的该节点

roscore

souce devel/setup.bash
rosrun vio_data_simulation vio_data_simulation_node

roscore启用后的日志信息：
在这里插入图片描述
rosrun运行后，开始生成imu.bag数据，生成的bag包数据的在代码修改后的存放路径下

第七步：将launch文件复写一个
在catkin_ws/src/imu_utils/launch目录下，复写my.launch，将其中的imu_topic修改为和代码中一样的topic,此时发布的话题是/imu

 ros::Publisher IMU_pub = n.advertise<sensor_msgs::Imu>("/imu", 20);

<launch>
    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
        <param name="imu_topic" type="string" value= "/imu"/>
        <param name="imu_name" type="string" value= "mytest"/>
        <param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
        <param name="max_time_min" type="int" value= "120"/>
        <param name="max_cluster" type="int" value= "100"/>
    </node>
</launch>

修改完成后在～/catkin_ws目录下，先source devel/setup/bash,然后在～/catkin_ws/src目录下，roslaunch imu_utils my.launch
第八步：回放数据包，并让imu_utils进行接收和分析

rosbag play -r 200 ../imu.bag

在这里插入图片描述
回放完成后会在imu_utils/data目录下生成txt的数据

此是就可以看到imu_utils/scripts脚本下的依赖于txt数据的draw_allan.m文件，使用matlab进行可视化显示

1.2 使用kalibr_allan完成allan标定（推荐）

ros下编译
执行，生成imu.bag
用kalibr_allan的bagconver把bag转成mat文件，见readme
用kalibr_allan的m脚本对mat文件进行分析，（需要改m文件中的mat文件路径）
用kalibr_allan的m脚本画allan曲线(修改m文件中的result文件路径）
m脚本运行需要matlab，安装耗时。

第一步：下载kalibr_allan工具，并编译

cd ~/catkin_ws/src
git clone https://github.com/rpng/kalibr_allan
cd ..
catkin_make

在这里插入图片描述
第二步：使用kalibr_allan的bagconver工具把bag包转换成mat文件
拷贝上述生成的imu_bag包到kilbr_allan的data文件夹中，将.bag文件转换成.mat文件：

cp imu.bag catkin_ws/src/kalibr_allan/data/
rosrun bagconvert bagconvert catkin_ws/src/kalibr_allan/data/imu.bag  imu.mat

在这里插入图片描述
第三步：生成Allan曲线
修改～/catkin_ws/src/kalibr_allan/matlab文件夹下的SCRIPT_process_results.m中.mat文件路径，然后运行SCRIPT_allan_matparallel.m即可在data目录下生成results_20220326T173409.mat文件。

2，将IMU仿真代码中的欧拉积分替换成中值积分
对非ros：生成运动imu数据

编译
执行bin/data_gen, 生成数据
执行python_tools/draw_trajectory.py画出轨迹
换成中值积分，重复上面步骤

    for(int i = 1; i < imudata.size(); ++i)
    {
        MotionData imupose_pre = imudata[i-1];
        MotionData imupose_now = imudata[i];
        MotionData imupose_mean = imudata[i];
        imupose_mean.imu_gyro = (imupose_pre.imu_gyro + imupose_now.imu_gyro) / 2.0;

        Eigen::Quaterniond dq;
        Eigen::Vector3d dtheta_half = (imupose_mean.imu_gyro) * dt / 2.0;
        dq.w() = 1;
        dq.x() = dtheta_half.x();
        dq.y() = dtheta_half.y();
        dq.z() = dtheta_half.z();

        //
        Eigen::Vector3d acc_w = Qwb * (imupose_pre.imu_acc) + gw;
        Qwb = Qwb * dq;
        Eigen::Vector3d acc_w1 = Qwb * (imupose_now.imu_acc) + gw;
        acc_w = (acc_w + acc_w1) / 2.0;
        Vw = Vw  + acc_w  * dt;
        Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;

        //按着imu postion, imu quaternion , cam postion, cam quaternion 的格式存储，由于没有cam，所以imu存了两次
        save_points<<imupose_mean.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;