IMU参数辨识及标定
一、标定参数分析
标定的本质是参数辨识。首先明确哪些参数可辨识,其次弄清怎样辨识。
参数包括陀螺仪和加速度计各自的零偏、标度因数、安装误差。
- IMU需要标定的参数主要是确定性误差和随机误差,确定性误差主要标定bias,scale和misalignment,随机误差主要标定noise和random walk,
kalibr_allan
是用于求取随机误差的开源工具。
可以中英对照
1.参数项
1) 零偏
这个比较好理解,就是输出比输入多了一个常值误差。
需要注意的是,通过Allan方差分析,得到了器件的量化噪声、角度随机游走、角速率随机游走、零偏不稳定性噪声、速率斜坡,仔细看,这些都是对零偏质量的分析,也可以直观的理解为零偏的波动和漂移程度,这里面并没有分析零偏本身的大小,而这个才是我们标定里要去估计的那个常值误差。
加速度计的零偏在这里表示为
陀螺仪的零偏在这里表示为
2) 标度因数误差
也叫刻度因数误差。假设器件输出的是标准单位角速度(rad/s),那么输出和输入的比就是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