IMU内参标定和IMU与激光联合标定实验

系列文章目录

---

本实验主要是LIO-SAM定位实验之前的预备实验，主要是为了标定IMU的bias和高斯白噪声，还有IMU与激光雷达坐标系之间的联合标定。

前言

传感器标定是指通过校准传感器以准确地测量和估计物体或环境的参数。在SLAM中，常见的传感器包括摄像头、激光雷达、惯性测量单元（IMU）等。传感器标定的目的是消除传感器本身的误差和偏移，以确保获取到的数据能够准确反映实际情况，从而提高SLAM系统的精度和鲁棒性。
传感器标定对SLAM系统的作用主要体现在以下几个方面：

1. 坐标系对齐：不同传感器可能使用不同的坐标系进行测量。通过标定可以将不同传感器的测量结果统一到同一坐标系中，以便在SLAM过程中进行一致的地图构建和定位。
2. 校正传感器误差：传感器由于生产和使用过程中可能存在固有误差和漂移等问题，标定可以通过确定和校正这些误差，提高传感器的测量精度和准确性。
3. 估计传感器参数：标定可以帮助估计传感器的内部参数和外部参数，如相机的焦距、畸变系数，激光雷达的旋转中心等。这些参数对于精确地重建环境和定位机器人至关重要。
4. 数据关联和融合：SLAM系统通常使用多个传感器进行数据采集，标定可以帮助将不同传感器获取的数据进行关联和融合，以获得更全面、准确的环境信息。

---

一、IMU内参标定

使用imu_utils工具标定imu的内参

1、下载安装imu_utils

下载code_utils https://github.com/gaowenliang/code_utils
下载imu_utils https://github.com/gaowenliang/imu_utils
从上面的地址下载两个包，注意imu_utils 依赖 code_utils所以先把code_utils放在工作空间的src下面编译然后再将imu_utils放到src下编译

2、录制IMU数据包

首先，启动使用的IMU的驱动，我使用的是LPMS-IG1这个九轴IMU，因此进入工作空间运行节点

cd catkin_ws
source devel/setup.bash
rosrun openzen_sensor openzen_sensor_node

然后使用rosbag录制IMU数据

rosbag record -O imu.bag /imu/data

这里的/imu/data为IMU数据的话题
录制过程全程保持imu静止不动，imu上电运行十分钟后开始录制，录制两个小时
使用Ctrl+c结束录制，此时就获取到了imu数据录制包imu.bag

3、使用imu_utils工具标定IMU

在 imu_utils/src/imu_utils/launch/下找到xsense.launch,修改imu话题名和time，两小时就设为120

启动imu_utils

//在imu_utils工作空间下
source  devel/setup.bash
roslaunch imu_utils xsense.launch 

启动imu_utils后，播放录制的数据包

//在放imu包的文件夹下右键新开终端，以两百倍速播放imu包
rosbag play -r200  imu.bag

此时需要静静等待较长时间

播包结束时才会显示标定完成，标定结果在data文件夹下会生成一堆文件我们需要的是第一个标定结束，标定结果存在imu_utils/src/imu_utils/data/下xsense_imu_param.yaml

二、IMU与LIDAR联合标定

吸取之前联合标定实验结果很差的教训，此次实验做了如下改进：

1. 将激光雷达安装位置降低
2. 解决了移动机器人在移动过程中由于前后支撑轮高度与驱动轮不同导致的来回晃动问题。
3. 在室外大场景环境下录制数据包

联合标定lidar和imu的外参，使用瑞士苏黎世联邦理工大学自动驾驶实验室开发的lidar_align工具标定雷达和imu外参

1.下载lidar_align

使用git下载lidar_align，如果不能科学上网，也可以直接从github上下载后

git clone https://github.com/ethz-asl/lidar_align.git

下载完成编译该包

2.修改lidar_align部分内容

改写imu接口（lidar_align原来不是用于标定lidar与 imu的这里需要改写imu接口）

打开lidar_align/src/lidar_align/src/loader.cpp在开头添加个头文件，注释掉odom部分，添加一部分

//在loader.cpp文件的开头添加头文件
 
#include <sensor_msgs/Imu.h>
//找到odom部分注释掉，如下
/*  types.push_back(std::string("geometry_msgs/TransformStamped"));
  rosbag::View view(bag, rosbag::TypeQuery(types));
  size_t tform_num = 0;
  for (const rosbag::MessageInstance& m : view) {
    std::cout << " Loading transform: \e[1m" << tform_num++
              << "\e[0m from ros bag" << '\r' << std::flush;
    geometry_msgs::TransformStamped transform_msg =
        *(m.instantiate<geometry_msgs::TransformStamped>());
    Timestamp stamp = transform_msg.header.stamp.sec * 1000000ll +
                      transform_msg.header.stamp.nsec / 1000ll;
    Transform T(Transform::Translation(transform_msg.transform.translation.x,
                                       transform_msg.transform.translation.y,
                                       transform_msg.transform.translation.z),
                Transform::Rotation(transform_msg.transform.rotation.w,
                                    transform_msg.transform.rotation.x,
                                    transform_msg.transform.rotation.y,
                                    transform_msg.transform.rotation.z));
    odom->addTransformData(stamp, T);
  }
*/
 
//在注释的位置粘贴如下代码
 
	types.push_back(std::string("sensor_msgs/Imu"));
	rosbag::View view(bag, rosbag::TypeQuery(types));
	size_t imu_num = 0;
	double shiftX=0,shiftY=0,shiftZ=0,velX=0,velY=0,velZ=0;
	ros::Time time;
	double timeDiff,lastShiftX,lastShiftY,lastShiftZ;
	for (const rosbag::MessageInstance& m : view){
	  std::cout <<"Loading imu: \e[1m"<< imu_num++<<"\e[0m from ros bag"<<'\r'<< std::flush;
 
	  sensor_msgs::Imu imu=*(m.instantiate<sensor_msgs::Imu>());
 
	  Timestamp stamp = imu.header.stamp.sec * 1000000ll +imu.header.stamp.nsec / 1000ll;
	  if(imu_num==1){
		 time=imu.header.stamp;
			 Transform T(Transform::Translation(0,0,0),Transform::Rotation(1,0,0,0));
		 odom->addTransformData(stamp, T);
	 }
	 else{
		 timeDiff=(imu.header.stamp-time).toSec();
		 time=imu.header.stamp;
		 velX=velX+imu.linear_acceleration.x*timeDiff;
		 velY=velX+imu.linear_acceleration.y*timeDiff;
		 velZ=velZ+(imu.linear_acceleration.z-9.801)*timeDiff;
 
		 lastShiftX=shiftX;
		 lastShiftY=shiftY;
		 lastShiftZ=shiftZ;
		 shiftX=lastShiftX+velX*timeDiff+imu.linear_acceleration.x*timeDiff*timeDiff/2;
		 shiftY=lastShiftY+velY*timeDiff+imu.linear_acceleration.y*timeDiff*timeDiff/2;
		 shiftZ=lastShiftZ+velZ*timeDiff+(imu.linear_acceleration.z-9.801)*timeDiff*timeDiff/2;
 
		 Transform T(Transform::Translation(shiftX,shiftY,shiftZ),
				Transform::Rotation(imu.orientation.w,
						 imu.orientation.x,
						 imu.orientation.y,
						 imu.orientation.z));
		 odom->addTransformData(stamp, T);
	 }
	}

因为修改了源码所以需要在工作空间lidar_align/下重新catkin_make 编译一次

3、录制IMU和LIDAR数据包

给lidar和imu上电运行，在home/下新建文件夹命名testbag，进入testbag右键新开终端
**注意：**需要在室外开阔环境下控制移动机器人直行和转弯，注意转弯幅度不要过大，大概录制2分钟左右的数据。

// rosbag record -大写欧（小写o你的包名会自动加时间日期） 包名 要录的话题1 要录的话题2 
rosbag record -O lidarimu.bag /imu/data /rslidar_points
//两分钟左右ctrl+c结束录制，包会自动保存在打开终端的testbag文件夹

4、使用lidar_align标定录制的数据包

如下图所示，修改lidar_align.launch文件（把录的包路径和名称添加进去）

启动lidar_align.launch文件进行标定

source devel/setup.bash 
roslaunch lidar_align lidar_align.launch

5、根据标定结果修改LIO-SAM中的参数

标定结果保存在 lidar_align/src/lidar_align/results下以包名命名的一个txt文件一个ply文件

---

总结