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

1.思想原理

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

2.操作流程

使用的设备：

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

具体流程：

对IMU进行角速度积分：角速度积分参照了VINS-Mono的预积分流程，激光雷达的频率远低于IMU频率。在一个点云数据的一个周期内（100ms），IMU大约会产生200÷10=20200{\div} 10 = 20200÷10=20个，根据公式：
αt=ωt0+ωt2⋅dtdt=t−t0α=∑i=0n−1αi\alpha_{t} = \frac{\omega_{t_0} + \omega_{t}}{2}\cdot dt \newline dt = t - t_0 \\ \alpha = \sum_{i =0}^{n-1} \alpha_i αt=2ωt0+ωt⋅dtdt=t−t0α=i=0∑n−1αi
其中αt\alpha_{t}αt为ttt时刻相对于前一时刻t0t_0t0的角度变化，ωt0,ωt\omega_{t_0} , \omega_{t}ωt0,ωt分别为对应时刻的角速度，nnn个IMU数据。这样子不断积分累加迭代即可得到一个周期内IMU旋转角度。

在这里，采用的是四元数的计算方式。四元数q=[cos(θ2)sin(θ2)⋅r]=[wxyz]q=\begin{bmatrix} cos(\frac{\theta}{2} ) \\ sin(\frac{\theta}{2})\cdot r \end{bmatrix} = \begin{bmatrix} w \\ x \\ y \\ z \end{bmatrix}q=[cos(2θ)sin(2θ)⋅r]=⎣⎢⎢⎡wxyz⎦⎥⎥⎤，对于相邻的IMU数据θ2\frac{\theta}{2}2θ都趋近于0，可用等价无穷小
lim⁡狗→0sin(狗)狗=1lim⁡狗→0cos(狗)=1\lim_{狗 \to 0} \frac{sin(狗)}{狗} = 1 \\ \lim_{狗 \to 0} cos(狗) = 1 狗→0lim狗sin(狗)=1狗→0limcos(狗)=1
则有：
qt=qt0⋅qdtqdt=(1,αx2,αy2,αz2)q_t = q_{t_0}\cdot q_{dt} \\ q_{dt} = (1, \frac{\alpha_x}{2} , \frac{\alpha_y}{2}, \frac{\alpha_z}{2}) qt=qt0⋅qdtqdt=(1,2αx,2αy,2αz)
其中为qtq_tqt时刻的姿态四元数，qt0q_{t_0}qt0为前一时刻姿态四元数，qdtq_{dt}qdt为该时间段内四元数变化值，αx\alpha_xαx, αy\alpha_yαy , αz\alpha_zα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);
}
```
IMU->雷达的姿态变换：以上测量出IMU的姿态角变化，需要映射到激光雷达坐标系上。实现需要知道激光雷达与IMU的外参变换变换矩阵（不用平移向量，但一定要旋转矩阵），RlidarIMUR_{lidar}^{IMU}RlidarIMU为从lidar到IMU的旋转矩阵（需要事先标定）。在lidar与IMU一起运动过程中，可以认为RlidarIMUR_{lidar}^{IMU}RlidarIMU是不变的。则有关系：
Rlidar0lidar1⋅RlidarIMU=RlidarIMU⋅RIMU0IMU1R_{lidar_0}^{lidar_1} \cdot R_{lidar}^{IMU} = R_{lidar}^{IMU} \cdot R_{IMU_0}^{IMU_1} Rlidar0lidar1⋅RlidarIMU=RlidarIMU⋅RIMU0IMU1
其中Rlidar0lidar1R_{lidar_0}^{lidar_1}Rlidar0lidar1为雷达始状态到末状态姿态变换，$R_{IMU_0}^{IMU_1} 为IMU始状态到末状态姿态变换（也就是上面积分出来IMU姿态角四元数为IMU始状态到末状态姿态变换（也就是上面积分出来IMU姿态角四元数为IMU始状态到末状态姿态变换（也就是上面积分出来IMU姿态角四元数q$）。将该公式同时右乘逆，可得：
Rlidar0lidar1=RlidarIMU⋅RIMU0IMU1⋅RlidarIMU−1R_{lidar_0}^{lidar_1} = R_{lidar}^{IMU} \cdot R_{IMU_0}^{IMU_1} \cdot {R_{lidar}^{IMU}}^{-1} Rlidar0lidar1=RlidarIMU⋅RIMU0IMU1⋅RlidarIMU−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();
```
效果如下：
对雷达的姿态四元数进行球面线性插值转换：在1个周期（100ms），雷达在俯视图上按照顺时针旋转，水平角度如下：

在一个水平面360度，分成1800份，先计算每个点云所对应的角度θi\theta_iθi:
θi=arccos(xixi2+yi2),ifyi<0θi=2π−arccos(xixi2+yi2),ifyi>0\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 θi=arccos(xi2+yi2xi),if yi<0θi=2π−arccos(xi2+yi2xi),if yi>0
再进行插值，注意是将当前时刻的点云转换到末状态雷达姿态上：
qslerp=Slerp(q0,q1;t)=q0(q0−1q1)t=q1(q1−1q0)1−t=(q0q1−1)1−tq1=(q1q0−1)tq0q_{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} qslerp=Slerp(q0,q1;t)=q0(q0−1q1)t=q1(q1−1q0)1−t=(q0q1−1)1−tq1=(q1q0−1)tq0
其中q0q_0q0为起始四元数，q1q_1q1为末状态四元数，ttt为插值的步长，t∈[0,1]t\in [0,1]t∈[0,1]。

将四元数qslerpq_{slerp}qslerp转化成旋转矩阵RslerpR_{slerp}Rslerp形式接着就可以利用坐标转换公式：
P1=[x1y1z11]=Tslerp⋅P0=[Rslerp001]⋅[x0y0z01]P_1 = \begin{bmatrix} x_1 \\ y_1 \\ z_1 \\ 1 \end{bmatrix} = T_{slerp} \cdot P_0 = \begin{bmatrix} R_{slerp} & \boldsymbol 0 \\ \boldsymbol 0 & 1 \end{bmatrix} \cdot \begin{bmatrix} x_0 \\ y_0 \\ z_0 \\ 1 \end{bmatrix} P1=⎣⎢⎢⎡x1y1z11⎦⎥⎥⎤=Tslerp⋅P0=[Rslerp001]⋅⎣⎢⎢⎡x0y0z01⎦⎥⎥⎤
其中P0P_0P0为原始点云坐标，P1P_1P1为转化后的点云坐标。

附上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);
}
```
效果：

单帧点云处理效果：

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

室外的走廊环境：

红色为手持激光雷达行进方向。采用A-LOAM进行建图，未去畸变点云建图：

去畸变后点云在有大旋转大环境范围下建图依然稳如老狗，效果如下：

利用IMU进行激光点云运动畸变校正相关推荐

Cartographer中对激光雷达运动畸变的处理方法分析
任务动机:梳理cartographer处理激光雷达运动畸变的原理,并针对特殊数据特性的雷达数据做相应适配,进而提升建图效果. 任务描述:查阅cartographer源码中激光雷达运动畸变的处理流程,对 ...
详解2D激光雷达运动畸变去除
2D激光雷达运动畸变去除 1.激光雷达运动畸变的说明如果是扫描频率5Hz的激光雷达,一帧数据的首尾时间差200ms,若机器人以0.5m/s的速度向x方向走扫描前面的墙面,那么200ms后尾部的测量距 ...
激光SLAM第三章作业去除运动畸变
·# 激光SLAM第三章作业去除运动畸变一.原理 1.由于曲线可以通过分段来近似二.题目介绍 1.题目描述题目描述 1.本次的作业为实现一个里程计去除激光雷达运动畸变的模块 2.本次的作业里面有 ...
在ROS下利用OpenCV的Mat类，将激光点云展开为深度图像(从零开始，超详细)
激光雷达3D目标检测任务需要将地面滤除,滤除地面的方法多种多样:基于深度学习.基于栅格.基于平面拟合.基于条件随机场.基于深度图像等等.关于将点云转化为深度图像,PCL库中有相关函数,但使用起来有一定 ...
激光点云系列之一：详解激光雷达点云数据的处理过程
交流群 | 进"传感器群/滑板底盘群"请加微信号:xsh041388 交流群 | 进"汽车基础软件群"请加微信号:Faye_chloe 备注信息:群名称 + 真 ...
基于全景图像与激光点云配准的彩色点云生成算法（2014年文章）
标题:The algorithm to generate color point-cloud with the registration between panoramic imageand lase ...
激光雷达传感器以及运动畸变去除
<深蓝学院-激光SLAM>课程一丶激光雷达传感器介绍 1.三角测距其中假设右边为一个激光发射器,左边为摄像头接受右方反射的光. 其中已知 L = L 1 + L 2 L=L_1+L_2 ...
自动驾驶视觉融合-相机校准与激光点云投影
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者:william 链接:https://zhuanlan.zhihu.com/p/13626375 ...
基于三维激光点云的目标识别与跟踪研究
基于三维激光点云的目标识别与跟踪研究人工智能技术与咨询来源:<汽车工程> ,作者徐国艳等［摘要］针对无人车环境感知中的障碍物检测问题,设计了一套基于车载激光雷达的目标识别与跟踪方法 ...
利用深度图建立三维点云地图笔记
前言:这几天在独立地研究对RGBD图像序列,建立其三维点云地图.这是我研究生期间,毕业论文中的一点小工作.由于我并没有借鉴像RTAB-MAP等SLAM方法,所以本文仅仅能够帮助学习和理解是三维建图 ...

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

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

1.思想原理

2.操作流程

利用IMU进行激光点云运动畸变校正相关推荐

最新文章

热门文章