IMU参数辨识及标定

一、标定参数分析

        标定的本质是参数辨识。首先明确哪些参数可辨识，其次弄清怎样辨识。

        参数包括陀螺仪和加速度计各自的零偏、标度因数、安装误差。

• IMU需要标定的参数主要是确定性误差和随机误差，确定性误差主要标定bias，scale和misalignment，随机误差主要标定noise和random walk，kalibr_allan 是用于求取随机误差的开源工具。

可以中英对照 

1.参数项

1) 零偏

        这个比较好理解，就是输出比输入多了一个常值误差。

        需要注意的是，通过Allan方差分析，得到了器件的量化噪声、角度随机游走、角速率随机游走、零偏不稳定性噪声、速率斜坡，仔细看，这些都是对零偏质量的分析，也可以直观的理解为零偏的波动和漂移程度，这里面并没有分析零偏本身的大小，而这个才是我们标定里要去估计的那个常值误差。

                加速度计的零偏在这里表示为

        陀螺仪的零偏在这里表示为

2) 标度因数误差

        也叫刻度因数误差。假设器件输出的是标准单位角速度(rad/s)，那么输出和输入的比就是1。如果不是，就得需要标定，修正这个比例。

        加速度计的标度因数这里表示如下：

        陀螺仪的标度因数这里表示为：

1. 修改配置文件

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

3) 安装误差

        这里面b坐标系是正交的imu坐标系，g坐标系的三个轴是分别对应三个陀螺仪。由于加工工艺原因，陀螺仪的三个轴并不正交，和我们导航中使用的正交轴不重合。我们需要仔细想一想，这个安装误差怎么在陀螺输出中体现出来的，因为我们标定时只能采集到陀螺的输出，而无法直接去测量安装误差。理论上，在陀螺坐标轴和b系重合的情况下，我们沿b系某一个坐标轴旋转，那么其他两个轴是不会有角速度输出的，而有了安装误差以后，便有了输出，据此，我们就可以建立输出和误差之间的关系了。以图中一项误差为例，Sgxy表示的就是y轴的单位输入，在x陀螺上由安装误差造成的输出。由此，我们可以把所有的安装误差都成矩阵形式，即:

        加速度计的安装误差原理和它一样，直接给出公式。

        这样一共有12项安装误差参数。有的时候，可以简化为9项，具体什么情况下简化，以及怎样简化，我们会在本文的后面讲。

2）启动launch文件

source devel/setup.bash

roslaunch imu_utils xxxx.launch

2. 误差模型

        通过上面的参数分析，我们已经可以很容易地写出误差模型了。

        陀螺仪：

        其中W是陀螺输出，ω是各坐标轴真实输入。该公式的展开形式为

        同理，可以得到加速度计的展开形式为：

二、利用imu_utils 标定IMU参数

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

1. 下载imu_utils和code_utils

imu_utils下载地址为：GitHub - gaowenliang/imu_utils: A ROS package tool to analyze the IMU performance.
code_utils下载地址为：GitHub - gaowenliang/code_utils: my code utils

注意：

1、全局安装ceres库，code_imu依赖ceres；

2、不要同时把imu_utils和code_utils一起放到src下进行编译。由于imu_utils依赖code_utils，所以先把code_utils放在工作空间的src下面，进行编译。然后再将imu_utils放到src下面，再编译。

2. 安装依赖并编译

sudo apt-get install libdw-dev

 2.1 编译code_utils

2.2 安装Ceres

Ubuntu20.04安装Ceres1.14.0 - 知乎

2.3 构建工作空间编译code_utils

问题：

sumpixel_test.cpp文件

问题1：修改#include "backward.hpp"为 #include“code_utils/backward.hpp”

问题2：

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

问题3：

解决：V_MINMAX 改为 NORM_MINMAX

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

3 imu_utils编译

文件结构：

        将 ROS 包imu_utils放入code_utils的工作区src下面进行编译使用catkin_make;进行构建

        CMakeLists.txt文件下

        代码修改如下：

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

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

4.1 播放rosbag

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

4.2 运行标定文件

source ./devel/setup.bash

roslaunch imu_utils gh5a5s1.launch

注意：等rosbag播放两秒左右在启动launch文件，因为在IMU前几分钟录制的数据误差比较大。

4.3 标定结果

        数据包播放结束之后，在/imu_utils/data/这个文件夹下会出现一系列的参数文件，如上图所示。

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

        虽然我们得到了标定结果，但这个标定结果并不是我们最终的结果。现在得到的结果的单位是rad/s和m/s^2，噪声是个能量概念或者说功率概念，我们还要把标定得到的参数归一化到每单位sqrt(hz)尺度下,即/aqrt（Hz）。

        加速度allan方差图

        加速度每个轴的allan方差

        陀螺的allan方差

        陀螺每个轴的allan方差

有时候播放完数据，程序没有任何反映。试试300rete 播放。

三、 利用kalibr_allan标定IMU随机误差

        该方法需要安装Matlab，才能编译kalibr_allan。

1. 下载kalibr_allan并编译

        问题：

        在/kalibr_allan-master/bagconvert/cmake目录下找到FindMatlab.cmake文件，更改matlab的地址。

find_program(MATLAB_EXE_PATH matlab

    PATHS /usr/local/MATLAB/R2018a/bin)

        把 /usr/local/MATLAB/R2018a/bin更换成自己matlab的地址

        修改之后，删除build和dev文件夹，再次catkin_make

2. 生成mat文件

        rosrun bagconvert bagconvert bag名字 topic名字

3. matlab计算IMU误差

        利用/kalibr_allan/matlab/SCRIPT_allan_matparallel.m生成误差文件。

生成误差的.m文件：

注意：修改路径,最好改为绝对路径

/kalibr_allan/matlab/SCRIPT_allan_matparallel.m

4. 绘制allan方差曲线

4.1 运行/kalibr_allan/matlab/SCRIPT_process_results.m文件

修改路径mat_path的路径修改为步骤3中得到的.mat文件

4.2 标定结果

accelerometer_noise_density = 0.00102011

accelerometer_random_walk   = 0.00003924

gyroscope_noise_density     = 0.00001248

gyroscope_random_walk       = NaN