【3D激光SLAM(三)】Velodyne激光SLAM学习之激光雷达+IMU内参标定

一、标定所使用设备

激光雷达：velodyne16线激光雷达

IMU：来自Pixhawk2.4.8飞控

二、安装与编译标定程序

需要先后安装两个代码报包：code_utils，imu_utils。

code_utils下载链接：

https://github.com/gaowenliang/code_utils

imu_utils下载链接：

https://github.com/gaowenliang/imu_utils

imu_utils 依赖 code_utils，故先catkin_make code_utils, 再将imu_utils放入编译。

编译可能报错解决：

编译code_utils 报错， fatal error: backward.hpp: No such file or directory

解决办法： 在code_utils文件夹下面找到sumpixel_test.cpp，修改#include “backward.hpp” 为 #include“code_utils/backward.hpp”即可编译成功。

三、录制标定数据与运行标定程序

3.1 新建launch文件

首先模仿imu_utils的launch文件下的sbg.launch，新建一个launch包：

touch px4.launch

然后写入到px4.launch：

可以把data文件夹修改为你自己创建的文件夹，但是你要先创建文件夹，系统不好根据launch文件自动创建你写的文件夹，否则就没有标定的结果文件产生。

录制imu数据包，将IMU静置，放置时间要稍大于中的录制时间max_time_min，即大于50分钟。频率为IMU的采样频率。

3.2 录制话题数据包

启动激光雷达和PX4飞控的mavros

注意：后面我打算将标定结果用在Lio-SAM算法上，该算法需要9轴IMU的支持，而且建议IMU频率要大于200Hz。在运行roslaunch mavros px4.launch后，我们可以看到有两个关于IMU的话题：

/mavros/imu/data #有九轴IMU数据
/mavros/imu/data_raw #仅仅有六轴IMU数据

我们选择订阅/mavros/imu/data来获取IMU数据。

对于IMU频率的问题，飞控默认的频率在50Hz左右，为了修改PX4飞控的IMU频率，我们需要取出飞控的SD卡，在SD卡的根目录下创建/etc/extras.txt ，写入

mavlink stream -d /dev/ttyACM0 -s ATTITUDE_QUATERNION -r 200
mavlink stream -d /dev/ttyACM0 -s HIGHRES_IMU -r 200

然后查看IMU频率：rostopic hz /mavros/imu/data_raw ，确认飞控传输的imu频率在200hz左右（我的飞控实际只有180Hz左右）。

准备工作做完后我们录制话题包：

rosbag record -O imu_calibration /mavros/imu/data

其中imu_calibration是bag包的名字，可以更改，录的包在当前终端目下，/mavros/imu/data是发布的IMU topic。（-O是大写字母O）

启动后等待50分钟，我听别人说得两个小时，下次再试试两小时。

3.3 标定IMU

进入刚刚录制rosbag包的工作目录，回放数据包：

rosbag play -r 200 imu_calibration.bag

标定结束后在imu_catkin_ws/src/imu_utils/data中生成许多文件，px4_imu_param.yaml就是我们想要的结果，展示如下：

%YAML:1.0
---
type: IMU
name: px4
Gyr:
   unit: " rad/s"
   avg-axis:
      gyr_n: 1.8592840863593844e-03
      gyr_w: 1.6445652273008798e-05
   x-axis:
      gyr_n: 1.7265861147301490e-03
      gyr_w: 1.0477803819087294e-05
   y-axis:
      gyr_n: 2.4483430772409402e-03
      gyr_w: 2.9022081903475343e-05
   z-axis:
      gyr_n: 1.4029230671070640e-03
      gyr_w: 9.8370710964637526e-06
Acc:
   unit: " m/s^2"
   avg-axis:
      acc_n: 1.1964613285076815e-02
      acc_w: 3.7731070649168168e-04
   x-axis:
      acc_n: 1.1679101305555653e-02
      acc_w: 2.7475459986410993e-04
   y-axis:
      acc_n: 1.1444259530376470e-02
      acc_w: 2.4021266404565691e-04
   z-axis:
      acc_n: 1.2770479019298321e-02
      acc_w: 6.1696485556527825e-04

然后替换LIO-SAM中config文件夹的配置yaml文件中这四个参数，第五个参数需要连续采样Z轴重力加速度做均值。

（~Noise对应的是~_n, BiasN对应的是~_w, 我们只用avg-axis下的数据，下图只是示意，请忽略数值）