2D激光雷达运动畸变矫正

参考了他人博客，对其中的坐标系转换计算进行更细化的理解，以及归纳了一些函数流程。

1、激光雷达运动畸变的原因

如上图所示，系1和系2是同一帧激光数据的laser frame。
如果雷达扫描频率较低，如10Hz，那么一帧激光点云的第1束和最后1束光束会相差100ms，robot在这段时间内会运动，导致一帧内laser frame已经从系1变化到系2.
在同一帧点云里，我们会默认laser frame是不动的，即认为laser扫描到P2时，laser frame是在系1处，但实际上laser frame已经是在系2处。
所以问题就出现了：P2的扫描数据实际range是绿线，但是我们误解成了蓝线range，而且在其他模块使用时，也是默认用的蓝线range数据。因此我们需要把绿线range矫正为蓝线range。

2、数据矫正思路

我们知道系1和系2在全局坐标（这里是odom frame）中的位姿，而根据系2和绿线range数据，可以知道P2在全局坐标中的位置；那么可以把P2在系1中的位置求出来，就是蓝线range。

2.1 时间与坐标系

ros里使用tf的waitForTransform()函数来获取一帧激光数据的t_s、t_e（开始、结束）时刻对应的laser frame。

2.2 分段线性插值

一帧数据里的每个光束对应的laser frame并不知道，所以需要由一帧激光数据的t_s、t_e时刻对应的laser frame来插值出每一个小时刻的laser frame。又由于机器人在一帧激光数据采集过程中是匀加速、匀速、匀减速运动中一种或者多种混合的运动状态，所以需要分段线性差值，尽量使插值贴近实际。
上图中，p_s和 p_e是一帧激光数据的激光雷达坐标系原点对应的里程计开始和结束位姿，这里先按照分段时间把一帧里程计进行分段处理，得到如①、②区域代表的不同分段。

然后对每个分段进行线性近似，如①区域橙色虚线所示；然后再进行线性插值，如①区域绿色线；如此往复，对一帧数据中的每个分段都进行线性近似、线性插值。这样就我们就获得了每个激光束的原点对应的里程计位姿（激光雷达坐标系在里程计坐标系下的位姿）。

2.3 坐标转换流程

激光雷达坐标系—>轮式里程计坐标系—>基准坐标系

3、坐标变换涉及的数学知识

3.1 从参考laser frame转到odom frame

将参考laser frame坐标系的P点转换到odom坐标系。
转换关系为：Po=Rol∗Pl+tolP_o =R_{ol}*P_l+t_{ol}Po=Rol∗Pl+tol
上式中：

PoP_oPo：P点在odom frame中的表示
PlP_lPl：P点在laser frame中的表示
RolR_{ol}Rol：laser frame旋转到与odom frame平行，在laser frame旋转的角度（以laser frame为度量）
tolt_{ol}tol：laser frame原点移动到odom frame原点，在odom frame移动的距离（以odom frame为度量）

这里也可以转换成齐次矩阵，计算更方便。
laser frame在odom frame中的表示，通过ros tf或者插值获得。

3.2 从odom frame转到基准base frame

将每一帧点云的第一束光束的laser frame成为base frame基准坐标系，这一帧里其他的参考laser frame数据都对齐到base frame。
转换关系为：Pb=Rbo∗Po+tboP_b =R_{bo}*P_o+t_{bo}Pb=Rbo∗Po+tbo
这里我们不知道tbot_{bo}tbo，即以base frame为度量，odom frame原点移动到base frame原点的距离；但是知道tobt_{ob}tob，需要先求出两者的关系。
（1）求tbot_{bo}tbo
以Po=Rob∗Pb+tobP_o =R_{ob}*P_b+t_{ob}Po=Rob∗Pb+tob来说，令Po=[0,0]TP_o=[0,0]^TPo=[0,0]T，表示P点在O坐标系下的描述是[0,0]T[0,0]^T[0,0]T，即P是O坐标系的原点。那么求出P点在B坐标系下的描述PbP_bPb，就是O坐标系原点在B坐标系下的位置，相当于位移tbot_{bo}tbo。
0=Rob∗Pb+tob0 =R_{ob}*P_b+t_{ob}0=Rob∗Pb+tob

Rob∗Pb=−tobR_{ob}*P_b=-t_{ob}Rob∗Pb=−tob

Pb=−Rob−1∗tobP_b=-R^{-1}_{ob}*t_{ob}Pb=−Rob−1∗tob

所以tbo=−Rob−1∗tobt_{bo}=-R^{-1}_{ob}*t_{ob}tbo=−Rob−1∗tob

(2)求PbP_bPb
Pb=Rbo∗Po+tbo=Rbo∗Po−Rob−1∗tob=Rbo∗Po−Rbo∗tobP_b =R_{bo}*P_o+t_{bo}=R_{bo}*P_o-R^{-1}_{ob}*t_{ob}=R_{bo}*P_o-R_{bo}*t_{ob}Pb=Rbo∗Po+tbo=Rbo∗Po−Rob−1∗tob=Rbo∗Po−Rbo∗tob
其中：

Rbo=Rob−1R_{bo}=R^{-1}_{ob}Rbo=Rob−1
Po=[x1,y1]TP_o=[x_1,y_1]^TPo=[x1,y1]T
tob=[x0,y0]Tt_{ob}=[x_0,y_0]^Ttob=[x0,y0]T

最终：

4、C++代码流程

4.1 流程

每一帧点云开始t_s时，雷达在odom frame的位姿p_s作为基准坐标系base frame。
以t_s开始，每隔一段时间线性分段一次，用于线性插值。
在间隔时间里处理线性分段，全部对齐到基准base frame。
下面开始都是分段插值处理:
处理线性分段函数需要的形参：1）base frame；2）以每一分段开始时laser的位姿p_s与间隔时间计算，获得分段结束时laser的位姿p_e；3）每一分段开始时点云的序号startIndex；4）每一分段要插值的个数（以分段里点云的个数为准）；5）点云原始数据range，angle。
对位置和角度进行线性插值。
以插值后的位姿作为每一个点云的参考laser frame。
每个点云在其参考laser frame上分解x和y，得该点云在laser frame的坐标。
在7的结果上，从参考laser frame用坐标系转换，得点云在odom frame下的坐标。
再坐标系转换，得点云在基准base frame下的坐标。

综上：

计算每个点云在其参考 laser frame的坐标；
转换为在odom frame的坐标；
最后转换为base frame的坐标。

问：为什么不直接计算参考laser frame到 base frame的转换？
答：因为线性分段了，无法获得每个参考laser frame到base frame的转换关系，比如第二个分段里的参考laser frame与base frame的关系并不是线性的；索性以odom frame为跳转。

4.2 参考代码

// scan回调函数
void LidarMotionCalibrator::ScanCallBack(const sensor_msgs::LaserScanConstPtr &scan_msg)
{ros::Time startTime, endTime;//一帧scan的时间戳就代表一帧数据的开始时间startTime = scan_msg->header.stamp;sensor_msgs::LaserScan laserScanMsg = *scan_msg;int beamNum = laserScanMsg.ranges.size();//根据激光时间分割和激光束个数的乘机+startTime得到endTime（最后一束激光束的时间）endTime = startTime + ros::Duration(laserScanMsg.time_increment * beamNum);//拷贝数据到angles,rangesstd::vector<double> angles, ranges;for (int i = 0; i < beamNum; i++){double lidar_dist = laserScanMsg.ranges[i]; //单位米double lidar_angle =laserScanMsg.angle_min + laserScanMsg.angle_increment * i; //单位弧度ranges.push_back(lidar_dist);angles.push_back(lidar_angle);}#if debug_visual_cloud_.clear();//数据矫正前、封装激光点云可视化、红色visual_cloud_scan(ranges, angles, 255, 0, 0);
#endif//激光雷达运动畸变去除函数Lidar_Calibration(ranges, angles, startTime, endTime, tf_);#if debug_//数据矫正后、封装打算点云可视化、绿色visual_cloud_scan(ranges, angles, 0, 255, 0);g_PointCloudView.showCloud(visual_cloud_.makeShared());
#endif
}
//激光雷达运动畸变去除函数
void LidarMotionCalibrator::Lidar_Calibration(std::vector<double> &ranges,std::vector<double> &angles,ros::Time startTime,ros::Time endTime,tf::TransformListener *tf_)
{//激光束的数量int beamNumber = ranges.size();//分段时间间隔，单位usint interpolation_time_duration = 5 * 1000; //单位ustf::Stamped<tf::Pose> frame_base_pose; //基准坐标系原点位姿tf::Stamped<tf::Pose> frame_start_pose;tf::Stamped<tf::Pose> frame_mid_pose;double start_time =startTime.toSec() * 1000 * 1000; //*1000*1000转化时间单位为usdouble end_time = endTime.toSec() * 1000 * 1000;double time_inc = (end_time - start_time) /beamNumber; //每相邻两束激光数据的时间间隔，单位us//得到start_time时刻，laser_link在里程计坐标下的位姿，存放到frame_start_poseif (!getLaserPose(frame_start_pose, ros::Time(start_time / 1000000.0), tf_)){ROS_WARN("Not Start Pose,Can not Calib");return;}//分段个数计数int cnt = 0;//当前插值的段的起始坐标int start_index = 0;//默认基准坐标系就是第一个位姿的坐标系frame_base_pose = frame_start_pose;for (int i = 0; i < beamNumber; i++){//按照分割时间分段，分割时间大小为interpolation_time_durationdouble mid_time = start_time + time_inc * (i - start_index);//这里的mid_time、start_time多次重复利用if (mid_time - start_time > interpolation_time_duration ||(i == beamNumber - 1)){cnt++;//得到临时结束点的laser_link在里程计坐标系下的位姿，存放到frame_mid_poseif (!getLaserPose(frame_mid_pose, ros::Time(mid_time / 1000000.0), tf_)){ROS_ERROR("Mid %d Pose Error", cnt);return;}//计算该分段需要插值的个数int interp_count = i + 1 - start_index;//对本分段的激光点进行运动畸变的去除Lidar_MotionCalibration(frame_base_pose,  //对于一帧激光雷达数据，传入参数基准坐标系是不变的frame_start_pose, //每一次的传入，都代表新分段的开始位姿，第一个分段，根据时间戳，在tf树上获得，其他分段都为上一段的结束点传递frame_mid_pose,   //每一次的传入，都代表新分段的结束位姿，根据时间戳，在tf树上获得ranges,           //引用对象，需要被修改的距离数组angles,           //引用对象，需要被修改的角度数组start_index,      //每一次的传入，都代表新分段的开始序号interp_count);    //每一次的传入，都代表该新分段需要线性插值的个数//更新时间start_time = mid_time;start_index = i;frame_start_pose =frame_mid_pose; //将上一分段的结束位姿，传递为下一分段的开始位姿}}
}
//从tf缓存数据中，寻找laser_link对应时间戳的里程计位姿
bool LidarMotionCalibrator::getLaserPose(tf::Stamped<tf::Pose> &odom_pose,ros::Time dt,tf::TransformListener *tf_)
{//初始化odom_pose.setIdentity();//定义一个 tf::Stamped 对象，构造函数的入口参数（const T& input, const//ros::Time& timestamp, const std::string & frame_id）tf::Stamped<tf::Pose> tmp(odom_pose, dt, scan_frame_name_);try{//阻塞直到可能进行转换或超时，解决时间不同步问题if (!tf_->waitForTransform(odom_name_, scan_frame_name_, dt,ros::Duration(0.5))) // 0.15s 的时间可以修改{ROS_ERROR("LidarMotion-Can not Wait Transform()");return false;}//转换一个带时间戳的位姿到目标坐标系odom_name_的位姿输出到odom_posetf_->transformPose(odom_name_, tmp, odom_pose);}catch (const std::exception &e){std::cerr << e.what() << '\n';}return true;
}
//根据传入参数，对任意一个分段进行插值
void LidarMotionCalibrator::Lidar_MotionCalibration(tf::Stamped<tf::Pose> frame_base_pose,tf::Stamped<tf::Pose> frame_start_pose,tf::Stamped<tf::Pose> frame_end_pose, std::vector<double> &ranges,std::vector<double> &angles, int startIndex, int &beam_number)
{// beam_step插值函数所用的步长double beam_step = 1.0 / (beam_number - 1);//该分段中，在里程计坐标系下，laser_link位姿的起始角度 和 结束角度，四元数表示tf::Quaternion start_angle_q = frame_start_pose.getRotation();tf::Quaternion end_angle_q = frame_end_pose.getRotation();//该分段中，在里程计坐标系下，laser_link位姿的起始角度、该帧激光数据在里程计坐标系下基准坐标系位姿的角度，弧度表示double start_angle_r = tf::getYaw(start_angle_q);double base_angle_r = tf::getYaw(frame_base_pose.getRotation());//该分段中，在里程计坐标系下，laser_link位姿的起始位姿、结束位姿，以及该帧激光数据在里程计坐标系下基准坐标系的位姿tf::Vector3 start_pos = frame_start_pose.getOrigin();start_pos.setZ(0);tf::Vector3 end_pos = frame_end_pose.getOrigin();end_pos.setZ(0);tf::Vector3 base_pos = frame_base_pose.getOrigin();base_pos.setZ(0);//临时变量double mid_angle;tf::Vector3 mid_pos;tf::Vector3 mid_point;double lidar_angle, lidar_dist;// beam_number为该分段中需要插值的激光束的个数for (int i = 0; i < beam_number; i++){//得到第i个激光束的角度插值，线性插值需要步长、起始和结束数据,与该激光点坐标系和里程计坐标系的夹角mid_angle = tf::getYaw(start_angle_q.slerp(end_angle_q, beam_step * i)); // slerp（）角度线性插值函数//得到第i个激光束的近似的里程计位姿线性插值mid_pos =start_pos.lerp(end_pos, beam_step * i); // lerp（），位姿线性插值函数//如果激光束距离不等于无穷,则需要进行矫正,说明量程范围内有物体if (std::isinf(ranges[startIndex + i]) == false){//取出该分段中该束激光距离和角度lidar_dist = ranges[startIndex + i];lidar_angle = angles[startIndex + i];//在当前帧的激光雷达坐标系下，该激光点的坐标//（真实的、实际存在的、但不知道具体数值）double laser_x = lidar_dist * cos(lidar_angle);double laser_y = lidar_dist * sin(lidar_angle);//该分段中的该激光点，变换的里程计坐标系下的坐标，（这里用插值的激光雷达坐标系近似上面哪个真实存在的激光雷达坐标系，因为知道数值，可以进行计算）double odom_x =laser_x * cos(mid_angle) - laser_y * sin(mid_angle) + mid_pos.x();double odom_y =laser_x * sin(mid_angle) + laser_y * cos(mid_angle) + mid_pos.y();mid_point.setValue(odom_x, odom_y, 0); //该点在odom frame下的位置//在里程计坐标系下，基准坐标系的参数double x0, y0, a0, s, c;x0 = base_pos.x();y0 = base_pos.y();a0 = base_angle_r;s = sin(a0);c = cos(a0);/** 把odom转换到base为 [  c s -x0*c - y0*s;*                    -s c  x0*s - y0*c;*                     0 0  1          ]*///这个矩阵是转换矩阵，即旋转矩阵R+平移向量t//把该激光点都从里程计坐标系下，变换的基准坐标系下double tmp_x, tmp_y; //该点在基准坐标系下的位置tmp_x = mid_point.x() * c + mid_point.y() * s - x0 * c - y0 * s;tmp_y = -mid_point.x() * s + mid_point.y() * c + x0 * s - y0 * c;mid_point.setValue(tmp_x, tmp_y, 0);//然后计算该激光点以起始坐标为起点的 dist angledouble dx, dy;dx = (mid_point.x());dy = (mid_point.y());lidar_dist = sqrt(dx * dx + dy * dy);lidar_angle = atan2(dy, dx);//激光雷达被矫正，替换原数据ranges[startIndex + i] = lidar_dist;angles[startIndex + i] = lidar_angle;}//如果等于无穷,则随便计算一下角度else{double tmp_angle;lidar_angle = angles[startIndex + i];//里程计坐标系的角度tmp_angle = mid_angle + lidar_angle;tmp_angle = tfNormalizeAngle(tmp_angle);//如果数据非法//则只需要设置角度就可以了。把角度换算成start_pos坐标系内的角度lidar_angle = tfNormalizeAngle(tmp_angle - start_angle_r);angles[startIndex + i] = lidar_angle;}}
}

参考：

深蓝学院课程-激光雷达运动畸变矫正
详解2D激光雷达运动畸变去除