Ubuntu20.04编译并运行imu_utils，并且标定IMU

一、编译并运行imu_utils
二、IMU数据采集
- 1 IMU型号
- 2 运行步骤
三、播放bag文件，进行离线标定IMU（使用imu_utils计算allan方差）
- 1 修改配置文件
- 2 标定过程

前言：imu_utils是用于求取IMU随机误差的开源工具。

一、编译并运行imu_utils

1 安装依赖项

sudo apt-get install libdw-dev

2 下载imu_utils和code_utils

imu_utils下载地址为：https://github.com/gaowenliang/imu_utils
code_utils下载地址为：https://github.com/gaowenliang/code_utils

注意：
1、全局安装ceres库，code_imu依赖ceres；
2、不要同时把imu_utils和code_utils一起放到src下进行编译。由于imu_utils依赖code_utils，所以先把code_utils放在工作空间的src下面，进行编译。然后再将imu_utils放到src下面，再编译。

2.1 code_utils编译

编译时出现的问题，解决如下：

2.1.1 CMakeLists.txt文件下

代码修改如下：

修改set(CMAKE_CXX_FLAGS "-std=c++11")为set(CMAKE_CXX_FLAGS "-std=c++14")

2.1.2 sumpixel_test.cpp文件下

代码修改如下：

问题1：

修改#include "backward.hpp"为 #include “code_utils/backward.hpp”

问题2：
在这里插入图片描述
修改：

添加头文件：#include"opencv2/imgcodecs/legacy/constants_c.h"

问题3：
在这里插入图片描述
修改：

CV_MINMAX 改为 NORM_MINMAX

2.1.3 mat_io_test.cpp文件下

代码修改如下：

问题：
在这里插入图片描述

opencv4.x以上，有些宏，API名字改了，需要改为新的：
CV_LOAD_IMAGE_UNCHANGED 改为 cv::IMREAD_UNCHANGED
CV_LOAD_IMAGE_GRAYSCALE 改为 cv::IMREAD_GRAYSCALE
CV_LOAD_IMAGE_COLOR 改为 cv::IMREAD_COLOR
CV_LOAD_IMAGE_ANYDEPTH 改为 cv::IMREAD_ANYDEPTH

修改：

CV_LOAD_IMAGE_UNCHANGED 改为 cv::IMREAD_UNCHANGED

2.2 imu_utils编译

code_utils编译成功以后，再把imu_utils放到工作空间的src下面，进行编译

编译时出现的问题，解决如下：

2.2.1 CMakeLists.txt文件下

代码修改如下：

修改set(CMAKE_CXX_FLAGS "-std=c++11")为set(CMAKE_CXX_FLAGS "-std=c++14")

2.2.2 imu_an.cpp文件下

代码修改如下：

问题：
在这里插入图片描述

修改：

添加头文件：#include <fstream>

二、IMU数据采集

可以先在室内录制2小时imu数据（bag包格式），前提是全程保持imu静止不动，然后播放bag包用imu_utils来做离线标定

1 IMU型号

IMU 采用的WHEELTEC IMU N100
在这里插入图片描述

2 运行步骤

在运行程序之前，需要先在ubuntu系统下安装串口驱动。安装驱动的方法如下：新打开一个命
令窗口，输入以下命令即可。

sudo apt install ros-noetic-serial

以上命令中的noetic是所装ROS的版本，如果所安装的ROS版本不是noetic，则需要把命令中的noetic改为自己装的ROS版本

连接硬件设备
查找对应串口的标号，在命令窗口输入以下命令即可查找对应串口的标号。

ls -l /dev/ttyUSB* //查找串口标号

输入后的结果如下图：
在这里插入图片描述

根据以上查找到的串口标号修改采集程序的配置文件。

在/home/cupido/imuread_ws/src/2.ROS_SDK/fdilink_ahrs_ROS1/fdilink_ahrs/launch中找到ahrs_data.launch打开该文件，如下图所示：
在这里插入图片描述

可以根据上面查到的串口标号修改上图对应的launch文件的串口配置参数，即添加：

<param name="port"  value="/dev/ttyUSB0"/>

运行采集程序

只需在命令窗口运行以下命令即可完成数据读取。

在根目录下：

首先运行打开串口权限，运行以下命令即可：
sudo chmod 666 /dev/ttyUSB* // 若硬件设备断电，则需要重新运行该命令

然后编译串口程序：
catkin_make

接着运行 source 命令：
source devel/setup.bash

然后运行采集程序：
roslaunch fdilink_ahrs ahrs_data.launch

此时就会发布以下几个话题:
在这里插入图片描述

通过指令查看IMU数据：
在终端输入以下指令：

rostopic echo /imu/data

IMU输出数据如下：
在这里插入图片描述

借助rviz将IMU可视化：
ADD话题，增加imu信息；参照配置文件，修改坐标系换成gyro_link。rviz效果如下：
在这里插入图片描述

如果无法输出数据，查看波特率是否太高了，这里为改成：921600 / 2 = 460800

最后保存数据，指令如下：

//             指定话题      将要录制的bag的文件名
rosbag record /imu/data -O imu.bag		// 让IMU静止不动两个小时，录制IMU的bag

最好是IMU上电10分钟之后在开始录制，上电瞬间开始录制会有较大误差。

终端显示如下：
在这里插入图片描述

查看文件夹，会看到imu.bag.active这个文件，就是正在录制的bag，静止不动，录制两个小时以上，录制完毕后Ctrl+C停止即可。此时，会有imu.bag文件，就是录制好的bag文件。

参考大佬：SLAM硬件搭建（激光雷达+IMU+相机+移动底盘）、https://gitee.com/xfbug/fdilink_ros_demo

三、播放bag文件，进行离线标定IMU（使用imu_utils计算allan方差）

IMU需要标定的参数主要是确定性误差（系统误差）和随机误差。误差和分析
确定性误差主要标定bias（零偏误差），scale（刻度系数误差，标度因数）和misalignment（安装误差，小量）；
随机误差主要标定bias instability（零偏不稳定性误差：会使陀螺仪和加速度计的零偏随时间慢慢变化，逐渐偏离开机时校准的零偏误差，同时还会造成一部分随机行走误差的效果；另一种说法是它表示在一段时间内零偏bias的变化量）和random walk（陀螺仪与加速度计的随机游走误差，又叫：传感器的高斯白噪声，详细解释为：陀螺仪测量的角速度，加速度计测量的加速度的噪声是高斯白噪声）。
上面的说法可能不太准确，看下面：

【注】：
1、一般文献中的 IMU 随机误差模型是高斯白噪声和维纳过程的叠加，IMU 的随机误差的参数可以通过艾伦方法测量。在卡尔曼滤波器中，我们需要使用 IMU 的误差模型来计算状态变量的方差随时间的变化。
2、下面第3点，归为随机误差里的bias，另外还有random walk，这是两类噪声。
3、针对零偏bias（又称：零偏的重复性），每次IMU上电会产生较大的误差，这个一般在自己的代码中，用融合的方法（在滤波或者融合的过程中）去实时的估这个bias值。另外需要额外指出的是：此时的总bias = 零偏重复性产生的 bias值+ bias instability（零偏不稳定性误差，不确定性零偏）。
【后注】：
1、针对确定性误差（bias（零偏误差），scale（刻度系数误差，标度因数）和misalignment（安装误差，小量）），在陀螺出厂前会进行调零操作，避免陀螺输出参数存在此误差对后续算法精度产生影响。
2、理论上，采数据时的陀螺只存在bias instability（零偏不稳定性误差，不确定性零偏）和 random walk（陀螺仪与加速度计的随机游走误差），这两个参数在组合导航中都是要实时估计的。
参考大佬：
1、4.1 IMU误差模型
2、VIO标定IMU随机误差：Allan方差法
3、IMU标定（二）随机误差的标定
4、IMU标定（三）确定误差的标定

1 修改配置文件

进入之前编译好的imu_utils文件夹下fdilink.launch文件（自己添加的）查看相关设置：

<launch>
    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
        <param name="imu_topic" type="string" value= "/imu/data"/>	<!--IMU话题名-->
        <param name="imu_name" type="string" value= "fdilink"/>	 <!--相应名字-->
        <param name="data_save_path" type="string" value= "$(find imu_utils)/data/fdilink/"/>   <!--存放数据的文件路径可以根据自己需要修改-->
        <param name="max_time_min" type="int" value= "120"/>		<!--标定的时长 120为120min-->
        <param name="max_cluster" type="int" value= "100"/>
    </node>
</launch>

这里有一个max_time_min表示使用bag数据的最大时长，单位是分钟，默认是120分钟，程序会在最大时间截断读取数据。

2 标定过程

在标定文件根目录下，依次输入以下指令：

catkin_make
source devel/setup.bash
roslaunch imu_utils fdilink.launch

之后，在所录制的IMU数据集bag包目录下，播放录制好的IMU的bag包：

rosbag play -r 200 imu.bag		// 200 倍速播放rosbag

程序进入bag读取，并计算allan方。当bag包加速回放完毕后，执行launch的窗口仍然会显示wait for imu data.，等待一段时间计算，计算完毕后会显示计算结果。

标定过程图片如下：
陀螺仪：
在这里插入图片描述
加速度计：

数据包播放结束之后，在/home/cupido/imu_ws/src/imu_utils/data/fdilink/（这是我修改后的文件路径）这个文件夹下会出现一系列的参数文件，如下图所示：

在这里插入图片描述

打开fdilink_imu_param.yaml这个文件，会看到计算出来的噪声和随机游走的系数值，如下所示：

%YAML:1.0
---
type: IMU
name: fdilink
Gyr:
   unit: " rad/s"
   avg-axis:
      gyr_n: 2.8226930135388296e-03		# 陀螺仪误差模型高斯白噪声
      gyr_w: 1.5638351283029004e-04		# 陀螺仪bias
   x-axis:
      gyr_n: 1.5253117163846117e-03
      gyr_w: 1.7174322527202657e-05
   y-axis:
      gyr_n: 1.6839509060303619e-03
      gyr_w: 2.7759881586750127e-05
   z-axis:
      gyr_n: 5.2588164182015163e-03
      gyr_w: 4.2421633437691737e-04
Acc:
   unit: " m/s^2"
   avg-axis:
      acc_n: 1.1448093263922482e-02		# 加速度计误差模型高斯白噪声
      acc_w: 4.0629665781519094e-04		# 加速度计bias
   x-axis:
      acc_n: 1.1073225312699064e-02
      acc_w: 4.1640077903725196e-04
   y-axis:
      acc_n: 7.7222501823143138e-03
      acc_w: 5.4834967948903443e-04
   z-axis:
      acc_n: 1.5548804296754069e-02
      acc_w: 2.5413951491928631e-04

至此，IMU的内参标定和记录结束。

参考大佬：
1、用imu_utils标定IMU，之后用于kalibr中相机和IMU的联合标定
2、VIO 中 IMU 的标定流程 (1/3) - imu_utils 使用备忘
3、使用imu_utils工具标定imu的内参