利用IMU进行激光点云运动畸变校正

利用IMU进行激光点云运动畸变校正

1.思想原理

在激光SLAM定位与建图过程中，当激光雷达运动得比较慢没有比较大幅度的旋转时，可以不用考虑点云的运动畸变，在帧间匹配时依然能用ICP配准估计出雷达的位姿。但是在激光雷达运动幅度比较大时，就不得不考虑激光点云的运动畸变，可以采用IMU或者编码器提供的短时间位姿估计信息去除点云的运动畸变。在这里我采用的是IMU对点云的运动畸变进行去除，IMU输出的加速度信息积分很发散，如果是更差的IMU设备，更用不了加速度信息，在这里之考虑了利用IMU的角速度信息。在1帧的点云时间内，对所有的IMU的角速度进行积分得到IMU的旋转角度，再转换到激光雷达上，对雷达的姿态进行姿态四元数球面线性插值将对应点云转换到末状态的姿态上实现去点云运动畸变去除。

2.操作流程

使用的设备：

• 激光雷达：Velodyne VLP16 ，频率为10HZ，机械旋转式，16线，在垂直方向为$(-15^{\circ },15^{\circ })$，每条线之间夹角为$2^{\circ }$，在水平面上有1800个激光点，平均分辨率$0.2^{\circ }$。
• IMU: D435i，频率为200HZ。

具体流程：

1. 对IMU进行角速度积分：角速度积分参照了VINS-Mono的预积分流程，激光雷达的频率远低于IMU频率。在一个点云数据的一个周期内（100ms），IMU大约会产生$200÷10=20$个，根据公式：
   $$\begin{gathered} {\alpha }_t=\frac{{\omega }_{t_0}+{\omega }_t}{2}\cdot dt \\ dt=t-t_0 \\ \alpha =\sum _{i=0}^{n-1}{\alpha }_i \end{gathered}$$
   其中${\alpha }_t$为$t$时刻相对于前一时刻$t_0$的角度变化，${\omega }_{t_0},{\omega }_t$分别为对应时刻的角速度，$n$个IMU数据。这样子不断积分累加迭代即可得到一个周期内IMU旋转角度。

   在这里，采用的是四元数的计算方式。四元数$q=\left[\begin{array}{c}cos(\frac{\theta }{2})\\ sin(\frac{\theta }{2})\cdot r\end{array}\right]=\left[\begin{array}{c}w\\ x\\ y\\ z\end{array}\right]$，对于相邻的IMU数据$\frac{\theta }{2}$都趋近于0，可用等价无穷小
   $$\begin{gathered} \underset{狗\to 0}{\lim }\frac{sin(狗)}{狗}=1 \\ \underset{狗\to 0}{\lim }cos(狗)=1 \end{gathered}$$
   则有：
   $$\begin{gathered} q_t=q_{t_0}\cdot q_{dt} \\ {q}_{dt}=(1,\frac{{\alpha }_x}{2},\frac{{\alpha }_y}{2},\frac{{\alpha }_z}{2}) \end{gathered}$$
   其中为$q_t$时刻的姿态四元数，$q_{t_0}$为前一时刻姿态四元数，$q_{dt}$为该时间段内四元数变化值，${\alpha }_x$, ${\alpha }_y$ , ${\alpha }_z$为对应的XYZ三轴角度变化.

   附上部分ROS+Eigen的代码

   std::vector<sensor_msgs::Imu::ConstPtr> imu_msg_vector;
   std::mutex mutex_lock;
   
   // IMU消息回调存储
   void imu_cb(const sensor_msgs::Imu::ConstPtr &imu_msg)
   {
       mutex_lock.lock();
       imu_msg_vector.push_back(imu_msg);
       mutex_lock.unlock();
   }
   
   // IMU角度积分
   Eigen::Quaterniond imu_q = Eigen::Quaterniond::Identity(); // R
   Eigen::Vector3d angular_velocity_1;
   angular_velocity_1 << imu_msg_v[0]->angular_velocity.x, imu_msg_v[0]->angular_velocity.y, imu_msg_v[0]->angular_velocity.z;
   double lastest_time = imu_msg_v[0]->header.stamp.toSec();
   for (int i = 1; i < imu_msg_v.size(); i++)
   {
       double t = imu_msg_v[i]->header.stamp.toSec();
       double dt = t - lastest_time;
       lastest_time = t;
       Eigen::Vector3d angular_velocity_2;
       angular_velocity_2 << imu_msg_v[i]->angular_velocity.x, imu_msg_v[i]->angular_velocity.y, imu_msg_v[i]->angular_velocity.z;
       Eigen::Vector3d aver_angular_vel = (angular_velocity_1 + angular_velocity_2) / 2.0;
       imu_q = imu_q * Eigen::Quaterniond(1, 0.5 * aver_angular_vel(0) * dt, 0.5 * aver_angular_vel(1) * dt, 0.5 * aver_angular_vel(2) * dt);
   }
   

2. IMU->雷达的姿态变换：以上测量出IMU的姿态角变化，需要映射到激光雷达坐标系上。实现需要知道激光雷达与IMU的外参变换变换矩阵（不用平移向量，但一定要旋转矩阵），$R_{lidar}^{IMU}$为从lidar到IMU的旋转矩阵（需要事先标定）。在lidar与IMU一起运动过程中，可以认为$R_{lidar}^{IMU}$是不变的。则有关系：
   $$R_{lida{r}_0}^{lida{r}_1}\cdot R_{lidar}^{IMU}=R_{lidar}^{IMU}\cdot R_{IM{U}_0}^{IM{U}_1}$$
   其中$R_{lida{r}_0}^{lida{r}_1}$为雷达始状态到末状态姿态变换，$R_{IMU_0}^{IMU_1} $为IMU始状态到末状态姿态变换（也就是上面积分出来IMU姿态角四元数$q$）。将该公式同时右乘逆，可得：
   $$R_{lida{r}_0}^{lida{r}_1}=R_{lidar}^{IMU}\cdot R_{IM{U}_0}^{IM{U}_1}\cdot {R_{lidar}^{IMU}}^{-1}$$
   Eigen代码：

   Eigen::Matrix3d lidar_imu_R << -1,     -1.22465e-16,            0,
             1.22465e-16,          -1,            0,
              0,            0,           1;     // lidar->IMU 外参数旋转矩阵
   Eigen::Matrix3d lidar_R;
   lidar_R = lidar_imu_R * (imu_q.toRotationMatrix()) * lidar_imu_R.inverse();
   

   效果如下：
   

3. 对雷达的姿态四元数进行球面线性插值转换：在1个周期（100ms），雷达在俯视图上按照顺时针旋转，水平角度如下：
   

   在一个水平面360度，分成1800份，先计算每个点云所对应的角度${\theta }_i$:
   $$\begin{gathered} {\theta }_i=arccos\left(\frac{x_i}{\sqrt{x_i^2+y_i^2}}\right),if{y}_i<0 \\ {\theta }_i=2\pi -arccos\left(\frac{x_i}{\sqrt{x_i^2+y_i^2}}\right),if{y}_i>0 \end{gathered}$$
   再进行插值，注意是将当前时刻的点云转换到末状态雷达姿态上：
   $$q_{slerp}=Slerp\left(q_0,q_1;t\right)=q_0{\left(q_0^{-1}{q}_1\right)}^t=q_1{\left(q_1^{-1}{q}_0\right)}^{1-t}={\left(q_0{q}_1^{-1}\right)}^{1-t}{q}_1={\left(q_1{q}_0^{-1}\right)}^t{q}_0$$
   其中$q_0$为起始四元数，$q_1$为末状态四元数，$t$为插值的步长，$t\in [0,1]$。

   将四元数$q_{slerp}$转化成旋转矩阵$R_{slerp}$形式接着就可以利用坐标转换公式：
   $$P_1=\left[\begin{array}{c}{x}_1\\ y_1\\ z_1\\ 1\end{array}\right]=T_{slerp}\cdot P_0=\left[\begin{array}{cc}{R}_{slerp}& 0\\ 0& 1\end{array}\right]\cdot \left[\begin{array}{c}{x}_0\\ y_0\\ z_0\\ 1\end{array}\right]$$
   其中$P_0$为原始点云坐标，$P_1$为转化后的点云坐标。

   附上Eigen部分代码：

   pcl::PointCloud<pcl::PointXYZI> cloud_undistorted;
   pcl::copyPointCloud(cloud_in, cloud_undistorted);
   
   for (int i = 0; i < cloud_undistorted.points.size(); i++)
   {
       double horizon_angle; // 化为角度为单位
       if (cloud_rgb.points[i].y < 0)
       {
           double theta = acos(cloud_undistorted.points[i].x / sqrt(cloud_undistorted.points[i].x * cloud_undistorted.points[i].x + cloud_undistorted.points[i].y * cloud_undistorted.points[i].y));
           horizon_angle = theta * 180.0 / M_PI;
       }
       else
       {
           double theta = acos(cloud_undistorted.points[i].x / sqrt(cloud_undistorted.points[i].x * cloud_undistorted.points[i].x + cloud_undistorted.points[i].y * cloud_undistorted.points[i].y));
           horizon_angle = (2 * M_PI - theta) * 180 / M_PI;
       }
       int id = int(horizon_angle / 360.0 * 1800);
       // std::cout << "horizon_angle: " << horizon_angle << ", id: " << id << std::endl;
   
       Eigen::Quaterniond q_slerp = lidar_q0.slerp((1.0 - horizon_angle / 360.0), lidar_q);
       Eigen::Matrix3d R_slerp = q_slerp.toRotationMatrix();
       Eigen::Matrix4d T_lidar_slerp;
       T_lidar_slerp << R_slerp(0, 0), R_slerp(0, 1), R_slerp(0, 2), 0,
           R_slerp(1, 0), R_slerp(1, 1), R_slerp(1, 2), 0,
           R_slerp(2, 0), R_slerp(2, 1), R_slerp(2, 2), 0,
           0, 0, 0, 1;
       Eigen::Vector4d Pi, Pj;
       Pi << cloud_undistorted.points[i].x, cloud_undistorted.points[i].y, cloud_undistorted.points[i].z, 1;
       Pj = T_lidar_slerp.inverse() * Pi;
       cloud_undistorted.points[i].x = Pj(0);
       cloud_undistorted.points[i].y = Pj(1);
       cloud_undistorted.points[i].z = Pj(2);
   }
   

4. 效果：

   单帧点云处理效果：
   
   在图中白色点云为原始数据，绿色为去畸变点云效果。这样子看不不出来实际效果，接着有个建图例子来说明。

   室外的走廊环境：
   
   红色为手持激光雷达行进方向。采用A-LOAM进行建图，未去畸变点云建图：
   
   去畸变后点云在有大旋转大环境范围下建图依然稳如老狗，效果如下：