【VIO】练习1 IMU标定

1.获得ROS_bag

如果手头有IMU可以自己录制bag文件，本篇介绍如何在ROS环境中使用 WHEELTEC N100 惯导模块。

（1）配置固定串口设备

​ 1）修改串口号

​ 从网上下载 CP21xxCustomizationUtility 软件，通过 CP21xxCustomizationUtility 这个 windows 上的软件修改并固定。程序链接：https://download.csdn.net/download/ASUNAchan/86728154?spm=1001.2014.3001.5503

​ 2）设备创建别名

​ 外设对应的串口名一般都是会变化的，为了避免手动选择，这里可以通过给 USB 设备创建别名的方式解决。

​ 1 命名一个wheeltec_udev.sh 脚本文件：

echo  'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60",ATTRS{serial}=="0003", MODE:="0777", GROUP:="dialout", SYMLINK+="fdilink_ahrs"' >/etc/udev/rules.d/fdilink_ahrs.rules

service udev reload
sleep 2
service udev restart

​ 其中 ATTRS{serial}==“0003” 就是刚从0001改为的0003。

​ 2 给这个脚本文件赋权：

sudo chmod 777 wheeltec_udev.sh

​ 3 执行脚本：

sudo sh wheeltec_udev.sh

（2）配置ROS驱动功能包

​ 驱动包地址：https://github.com/SHUNLU-1/fdilink_ahrs.git

1）创建ROS工作空间

mkdir -p 自定义空间名称/src

​ 将下载好的驱动包文件放在src文件夹下

cd 自定义空间名称
catkin_make

​ 如果没有安装串口驱动，则安装：

sudo apt install ros-noetic-serial

2）运行

​ 1 启动ROS

roscore

​ 2 运行

cd 工作空间
source ./devel/setup.bash
roslaunch fdilink_ahrs ahrs_driver.launch

​ 3 查看话题

rostopic list

​ 通过 rostopic echo /imu 打印imu消息

​ 4 录制

​ 录制imu话题 (一般为两个小时左右)

rosbag record /imu -o imu.bag

2.使用imu_utils标定

（1）安装imu_utils

​ 网上资料很多，此处就不赘述了。基本上是创建工作空间，将包放在src文件夹下编译。

（2）标定

​ 1）查看imu.bag包

rosbag info imu.bag

录制的数据

path:        imu.bag
version:     2.0
duration:    2hr 3:40s (7420s)
start:       Sep 29 2022 15:54:05.05 (1664438045.05)
end:         Sep 29 2022 17:57:45.10 (1664445465.10)
size:        268.3 MB
messages:    742014
compression: none [347/347 chunks]
types:       sensor_msgs/Imu [6a62c6daae103f4ff57a132d6f95cec2]
topics:      /imu   742014 msgs    : sensor_msgs/Imu

​ 2）launch文件

<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= "mems"/>
        <param name="data_save_path" type="string" value= "$(find imu_utils)/data2/"/>
        <param name="max_time_min" type="int" value= "120"/>
        <param name="max_cluster" type="int" value= "100"/>
    </node>
</launch>

​ 3）生成Allan方差数据

cd 工作空间
source ./devel/setup.bash
roslaunch imu_utils mems.launch

​ 打开一个窗口，倍速播放imu.bag数据

rosbag play -r 200 imu.bag

​ 在data文件夹下生成一些数据，其中mems_imu_param.yaml文件：

%YAML:1.0
---
type: IMU
name: mems
Gyr:
   unit: " rad/s"
   avg-axis:
      gyr_n: 8.4255797310257684e-03
      gyr_w: 8.0220535819774673e-05
   x-axis:
      gyr_n: 3.3386166058607726e-03
      gyr_w: 4.9852324036276215e-05
   y-axis:
      gyr_n: 6.0109360367037205e-03
      gyr_w: 4.2140284844438258e-05
   z-axis:
      gyr_n: 1.5927186550512812e-02
      gyr_w: 1.4866899857860955e-04
Acc:
   unit: " m/s^2"
   avg-axis:
      acc_n: 2.5570191303919226e-02
      acc_w: 2.4360505792146779e-03
   x-axis:
      acc_n: 5.3263758480915149e-02
      acc_w: 5.3969183475373857e-03
   y-axis:
      acc_n: 1.1062720096106503e-02
      acc_w: 7.2464052789635480e-04
   z-axis:
      acc_n: 1.2384095334736032e-02
      acc_w: 1.1865928622102927e-03

​ 4）绘制Allan方差图

​ 修改 /scripts/draw_allan.m中的文件路径

clear 
close all

dt = dlmread('../data2/data_mems_gyr_t.txt');         
data_x = dlmread('../data2/data_mems_gyr_x.txt'); 
data_y= dlmread('../data2/data_mems_gyr_y.txt'); 
data_z = dlmread('../data2/data_mems_gyr_z.txt'); 
data_draw=[data_x data_y data_z] ;

data_sim_x= dlmread('../data2/data_mems_sim_gyr_x.txt'); 
data_sim_y= dlmread('../data2/data_mems_sim_gyr_y.txt'); 
data_sim_z= dlmread('../data2/data_mems_sim_gyr_z.txt'); 
data_sim_draw=[data_sim_x data_sim_y data_sim_z] ;


figure
loglog(dt, data_draw , 'o');
% loglog(dt, data_sim_draw , '-');
xlabel('time:sec');                
ylabel('Sigma:deg/h');             
% legend('x','y','z');      
grid on;                           
hold on;                           
loglog(dt, data_sim_draw , '-');

matlab不易安装无法使用的话可以选择开源软件Octave替代

运行draw_allan.m：

（3）结果分析

%YAML:1.0
---
type: IMU
name: mems
Gyr:
   unit: " rad/s"
   avg-axis:
      gyr_n: 8.4255797310257684e-03
      gyr_w: 8.0220535819774673e-05

Acc:
   unit: " m/s^2"
   avg-axis:
      acc_n: 2.5570191303919226e-02
      acc_w: 2.4360505792146779e-03

​ 通过查资料，WHEELTEC N100的采样频率应该为400Hz，$ \Delta t = \frac{1}{400} = 0.0025$

​ 高斯白噪声：
$$\sigma ={\sigma }_d\sqrt{\Delta t}$$
​ bias随机游走：
$${\sigma }_b={\sigma }_{bd}\sqrt{\Delta t}$$

高斯白噪声	标定结果	标定结果连续化
陀螺仪	8.4255797310257684e-03	0.00042127898655128844
加速度	2.5570191303919226e-02	0.0012785095651959614

陀螺仪随机游走	标定结果	标定结果连续化
陀螺仪	8.0220535819774673e-05	4.011026790988734e-06
加速度计	2.4360505792146779e-03	0.0001218025289607339

3.使用kalibr_allan标定

需要事先安装matlab。。。就不用它来标定了

后记：标定过程也是自己摸索的，而且生成的allan图感觉误差有点大的样子，欢迎指正错误。