视觉-惯导多传感器融合（1）IMU ROS

ROS从入门到精通系列（十六）-- IMU in ROS_Techblog of HaoWANG-CSDN博客1. IMU简介请移步本人的另一篇博客文章：2. ROS IMU msgs官方文档：http://docs.ros.org/api/sensor_msgs/html/msg/Imu.htmlIMU 消息类型为ROS中的一种标准传感器消息类型，其定义已经包含在sensor_msgs/Imu.msg文件夹中。在发布IM消息时需要将该消息类型的头文件包含在内。消息详细...https://haowang.blog.csdn.net/article/details/104961725

1. IMU通识简介

1. IMU简介

请移步本人的另一篇博客文章，详细介绍了IMU工作原理和卡尔曼滤波方法：Arduino学习笔记（12） -- MPU6050与卡尔曼滤波算法实践_Techblog of HaoWANG-CSDN博客_arduino kalmanhttps://blog.csdn.net/hhaowang/article/details/88846468
ROS传感器之IMU简介 - 知乎一.概念IMU，全称inertial measurement unit，即惯性测量单元，主要用于测量自身位姿，位姿包含位置和姿态。二.组成最为基本的IMU包含两个器件，加速计和陀螺仪。三.数学模型通常使用一个三维笛卡尔坐标系来表示…https://zhuanlan.zhihu.com/p/136151969原文如下：

一.概念

IMU，全称inertial measurement unit，即惯性测量单元，主要用于测量自身位姿，位姿包含位置和姿态。

二.组成

最为基本的IMU包含两个器件，加速计和陀螺仪。

三.数学模型

通常使用一个三维笛卡尔坐标系来表示IMU，以microstrain 3DM-GX2为例，如图：

加速计测量三轴上的加速度，陀螺仪测量绕三轴的角速度，通过这两种传感器数据，可以计算出IMU自身的位姿变化，即当前时刻相对于上一时刻的姿态变化。

四.IMU与AHRS

AHRS，全称Attitude and Heading Reference System，即航姿参考系统，其组件为加速计、陀螺仪和磁力计，可以看到AHRS比IMU多一个磁力计，磁力计可以测量磁场的方向和强度。AHRS与IMU都可以测量位姿，但是其参考坐标系不同，IMU参考坐标系是其上一时刻的位姿，AHRS的参考坐标系一般是导航坐标系，例如东北天或者北东地坐标系。AHRS可以测量静止状态时的绝对姿态，IMU测量相对位姿。市面上有些IMU实际是集成了AHRS，模式可配置选择。

五. IMU与GPS

在自动驾驶定位算法中，IMU+GPS被称为黄金搭档，其主要原因是这两种器件互补性比较好，其数据有以下特征：

1. 频率互补，GPS频率一般为10Hz，IMU频率一般为100Hz。

2. 数据干扰互补，GPS数据易受环境影响，例如高楼、隧道等；IMU不易受外界干扰。

3. 数据误差互补，如上述第六点所述，IMU存在数据漂移，GPS每次测量都是独立的，即与上次测量无关，所以不存在误差累计。

4. IMU测量的是相对位置，GPS测量的是绝对位置。

六. 数据漂移

我们使用IMU器件最终想要获取的是位姿数据，包含位置和姿态，而实际IMU直接测量的数据是加速度和角速度，需要对角速度进行一次积分计算角度，也就是姿态；对加速度进行二次积分获取距离，也就是位置。由于种种原因，所有的传感器测量数据，都有测量误差，而IMU是通过积分实现间接测量值，且其参考数据是上时刻数据，这就造成其误差也会随着时间传递下去，时间越久，其误差越大，这一特性成为数据漂移，所以IMU一般需要与其它传感器配合使用。

七. ROS中IMU数据表示

执行一下指令：
rosmsg show sensor_msgs/Imu
执行效果如下图：

header项同前文GPS(链接“ROS传感器之GPS简介”)，linear_acceleration表示线加速度，linear_acceleration_covariance表示线加速度协方差，angular_velocity表示角速度，angular_velocity_covariance角速度协方差，orientation表示姿态，使用四元数表示，orientation_covariance表示姿态协方差。

orientation是由linear_acceleration和angular_velocity计算而得，但并不是所有IMU设备都直接提供orientation，如果没有提供，将orientation各项置为0，orientation_covariance各项置为-1.这里协方差表示各个数据的误差，一般由器件厂商给出。

八. 小结

本篇介绍了IMU的概念，基本组件，数据特性，以及在ROS中如何表示IMU数据，下一篇，以microstrain 3DM-GX2为例，介绍如何获取，解析以及发布IMU数据。

2. IMU & ROS

3. 编译及使用IMU

ROS包支持的环境版本：

ubuntu 16.04, ROS kinetic

ubuntu 18.04, ROS Melodic

ubuntu 20.04, ROS Noetic

如果使用其它ROS版本，请用户自己探索安装相关环境。

1. 安装 ros imu　功能依赖包

如果你使用的是 ubuntu 16.04, ROS kinetic:
  sudo apt-get install ros-kinetic-imu-tools ros-kinetic-rviz-imu-pluginsudo apt-get install python-visual
如果你使用的是 ubuntu 18.04, ROS Melodic:
    ```sudo apt-get install ros-melodic-imu-tools ros-melodic-rviz-imu-plugin```
如果你使用的是 ubuntu 20.04, ROS Noetic:
    ```sudo apt-get install ros-noetic-imu-tools ros-noetic-rviz-imu-plugin```
2. 下载并编译 handsfree_ros_imu 驱动包，如果没有安装 git 工具，请使用 `sudo apt-get install -y git`命令，通过终端安装。

源码地址GitHub - HANDS-FREE/handsfree_ros_imuContribute to HANDS-FREE/handsfree_ros_imu development by creating an account on GitHub.https://github.com/HANDS-FREE/handsfree_ros_imu
    ```mkdir -p  ~/handsfree/handsfree_ros_ws/src/cd ~/handsfree/handsfree_ros_ws/src/git clone https://gitee.com/HANDS-FREE/handsfree_ros_imu.gitcd ~/handsfree/handsfree_ros_ws/catkin_makecd ~/handsfree/handsfree_ros_ws/src/handsfree_ros_imu/scripts/sudo chmod 777 *.py```
3. 将 setup.sh 写入 .bashrc 文件中
    ```echo "source ~/handsfree/handsfree_ros_ws/devel/setup.bash" >> ~/.bashrcsource ~/.bashrc```
4. 插上 USB 设备（连接 IMU 的 USB，检查电脑能否识别到 ttyUSB0，检测到 ttyUSB0 后，给 ttyUSB0 赋权限
    ```ls /dev/ttyUSB0sudo chmod 777 /dev/ttyUSB0```
陀螺仪和加速计的发布话题：/handsfree/imu

磁力计的发布话题：/handsfree/mag

1. 通过 USB 连接线将 IMU 和电脑连接。

2. 运行 ROS 驱动可视化程序，打开 rviz 。
    roslaunch handsfree_ros_imu rviz_and_imu.launch imu_type:=a9（改成你的imu的型号，可选项：a9，b9，b6）
3. 运行脚本获取 IMU 欧拉角格式数据：Roll（翻滚），Pitch（俯仰），Yaw（偏航）
    ```rosrun rosrun handsfree_ros_imu get_imu_rpy.py```
![通过 python 脚本来打印 ros imu 的 Roll（翻滚），Pitch（俯仰），Yaw（偏航）角度](./img/doc/3.jpg)

4. 打开通过 python 脚本编写的 3D 可视化（此功能只适用于 ubuntu 16.04　ROS kinetic 环境）
    ```roslaunch handsfree_ros_imu display_and_imu.launch imu_type:=a9（改成你的imu的型号，可选项：a9，b9，b6）```
此时，转动手中的 IMU，就会看到 rviz 上面的模型的变化了。

4. 测试及可视化

1. rviz_and_imu.launch


<!--imu 和 rviz 同时打开--><launch><!-- imu型号，默认 a9 --><arg name="imu_type" default="a9" doc="model type [a9, b9, b6 ,d6]"/><!-- imu 对应 python 文件 --><node pkg="handsfree_ros_imu" type="hfi_$(arg imu_type)_ros.py" name="imu" output="screen"></node><!-- 加载 rviz --><node name="rviz" pkg="rviz" type="rviz" args="-d $(find handsfree_ros_imu)/rviz/handsfree_ros_imu.rviz"></node></launch>

2. 打开PlotJuggler绘制IMU数据图像

5.关于 imu 的一些相关使用

1、里程计和 IMU 融合

通过机器人运动学解算可以把测量到的电机运动数据转换成里程计数据，电机里程计通过扩展卡尔曼滤波融合 IMU 偏航(yaw)角的数据后，可以把机器人里程计的精度大大提升，称之为惯性融合里程计。

里程计的权重信息可以通过打滑碰撞检测模块来确定,通过检测电机的电流和编码器测量的电机速度以及 IMU 数据，来判断是否发生打滑和碰撞。若发生这些情况则意味着里程计会发生瞬间漂移，通过降低里程计的权重可以大概率的保障定位导航系统的可靠性。

2、视觉 SLAM 和 IMU 融合

视觉 SLAM 容易受图像遮挡，光照变化,运动物体干扰弱纹理场景等影响，单目视觉无法测量尺度，深度信息往往无法直接获取，仅通过单张图像无法获得像素的深度信息，需要通过三角测量的方法来估计地图点的深度。此外单目纯旋转运动无法估计，快速运动时易丢失。

视觉与 IMU 融合之后会弥补各自的劣势，可利用视觉定位信息来估计 IMU 的零偏，减少IMU由零偏导致的发散和累积误差。IMU 可以为视觉提供快速运动时的定位，IMU 可以提供尺度信息，避免单目无法测尺度

3、GPS 和 IMU 融合

目前的 GPS 有很多时候是精度不够准确或者无效的，例如在隧道中经常因为信号不好无法使用，或者在市中心的高楼里 GPS 信号容易被折射反射。这个时候 IMU 就可以增强 GPS 的导航能力。例如，在车道线识别模块失效时，利用失效之前感知到的道路信息和 IMU 对汽车航迹的推演，仍然能够让汽车继续在车道内行驶。

此外，GPS 更新频率过低（仅有 10Hz）不足以提供足够实时的位置更新，IMU 的更新频率可以达到 100Hz 或者更高完全能弥补 GPS 所欠缺的实时性。GPS/IMU 组合系统通过高达 100Hz 频率的全球定位和惯性更新数据，可以帮助自动驾驶完成定位。通过整合 GPS 与 IMU，汽车可以实现既准确又足够实时的位置更新。

IMU 连同板载 ADAS 和卫星定位输入，提供精确的车辆位置和航向画面，同时抑制正常驾驶产生的冲击和振动。

4、其他

IMU 设计的应用领域广泛，在军航空航天、航海、军事、医疗、娱乐、机器人、车辆等领域都有这重要的作用。

参考：贡献文章 · HandsFree Tutorialhttps://wiki.hfreetech.org/contribute.html