本文详细介绍了激光雷达运动畸变的产生原因及其去除方法，主要涉及时间同步、数据对齐、分段线性近似与线性插值等步骤。通过里程计辅助，实现了低帧率激光雷达数据的畸变校正，从而提高数据准确性。内容包括坐标变换的数学原理及C++核心代码解析。
目录
6.3 激光雷达坐标系==》里程计坐标系与6.4 里程计坐标系==》基准雷达坐标系
1、激光雷达运动畸变说明
如果激光雷达的扫描帧率为5Hz，一帧数据的首尾时间差200ms，若机器人以0.5m/s的速度向x方向走，扫描前面的墙面，那么200ms后尾部的测量距离和首部的测量距离在x方向上相差10cm，如下图所示：
绿色箭头表示激光雷达旋转方向，黑色箭头表示激光雷达运动方向。显然，我们可以看出每个激光束的距离数据都是在不同位姿下采集而来，如橙色、灰色、蓝色点位姿所示；然而，一般激光雷达驱动封装数据时，却默认一帧激光雷达数据的所有激光束是在同一位姿下、瞬时获得的，也就是所有激光束都是从橙色点获得的数据，这样实际使用的数据和真实数据有明显差距，如10cm。所以，对于低帧率的激光雷达，运动畸变明显是个需要被解决的问题。
2、去除运动畸变的原理
原理：在一帧激光雷达数据中，为每个激光束的原点都找到一个近似的里程计位姿，并且认为该激光束是在该里程计位姿上进行数据采集的，得到这些基础数据后，进行系列的坐标变换，把所有的激光点都变换到第一束激光原点的坐标系下，然后再封装数据，这样就能极大地减小畸变。
3、如何使用里程计辅助去除运动畸变
3.1 大致流程
- 用CPU读取激光雷达数据，同时单片机上传里程计数据，两者进行时间同步、数据对齐；
- 分段线性近似、线性插值，找到①激光束原点近似的里程计位姿和②近似的激光雷达坐标系角度；
- 将所有激光束从近似的激光雷达坐标系变换到里程计坐标系上（激光雷达坐标系==》里程计坐标系）；
- 最后将所有激光束从里程计坐标系变换到基准坐标系、封装数据（里程计坐标系==》基准坐标系）；
3.2 时间同步、数据对齐
若一帧激光数据的t_s、t_e（开始、结束）时刻在缓存的里程计数据里没有找到准确对应的位姿，则需要使用waitForTransform()函数来等待tf的坐标系转换线程，直到得到你想要的时间点的坐标转换数据，代码中实现方式如下：
tf_->waitForTransform(“odom”, “laser_link”, dt, ros::Duration(0.5))
ros::Duration(0.5) ： 为 waitForTransform() 函数 的结束条件：最多等待 0.5 秒，如果提前得到了坐标的转换信息，直接结束等待。
3.3 分段线性近似、线性插值
这里先假设机器人在一帧激光数据采集过程中是匀加速、匀速、匀减速运动中一种或者多种混合都行。机器人的位姿是关于时间t的分段函数，如下图所示；这里只给出了机器人匀减速运动的解释，其他运动方式都是一样的处理方式。
上图中，p_s和p_e是一帧激光数据的激光雷达坐标系原点对应的里程计开始和结束位姿，这里按照分段时间把一帧里程计进行分段处理，得到如①、②区域代表的不同分段。
然后对每个分段进行线性插值，如图①区域橙色虚线所示；然后再进行线性插值，如①区域绿色线；如此往复，对一帧数据中的每个分段都进行线性近似、线性插值。这样我们就获得了每个激光束的原点对应的里程计位姿（激光雷达坐标系在里程计坐标系下的位姿）。
3.4 坐标转换流程
激光雷达坐标系—>轮式里程计坐标系—>基准坐标系
在上面的插值完成后，在近似位姿的激光雷达坐标系（绿色坐标系）下将每个激光点变换到里程计坐标系（蓝色坐标系）下，再将每个激光点的数据从里程计坐标系下变换到基准坐标系下（绿色A坐标系）。
这里将分段①（一帧数据的第一个分段）进行线性插值和坐标变换的过程放大化了，方便理解，如上图所示。
odom蓝色坐标系：机器人里程计坐标系；
绿色坐标系：运动过程中的激光雷达坐标系，若干；
A点绿色坐标系：采集第一束激光数据时刻的激光雷达坐标系，这也当作基准坐标系；
B点绿色坐标系：该分段中采集某一个激光束BD时刻的激光雷达坐标系。
A、C两点：近似线性分段的起始和结束点，A、C坐标系和坐标是从tf树获得，分别是激光雷达坐标系、以及坐标系的里程计位姿。
AC橙色虚直线：对AC弧线运动过程的线性近似，这里是放大后的效果图。
D点：激光束BD的终点。
B点蓝色坐标系：和里程计坐标方向一致、相互平行的坐标系。
激光雷达驱动实际采集到的数据：如B点绿色坐标系下的激光束BD的数据（距离和角度），该数据存储在/scan中，但存储数据时认为该数据是在A点绿色坐标系下采集的，所以存在运动畸变。
很明显，我们要将D点从B点绿色坐标系变换到A点绿色坐标系，如从黑色BD到绿色AD，此时我们就获得了畸变情况下在基准坐标下的D点的数据。于是就完成了一个激光点的去运动畸变过程，其他同理。
这里实现的C++代码流程是：
（1）把该插值B对应的激光点D的距离和角度，在近似的激光雷达坐标系（如绿色B坐标系）的XY轴计算出分量；
（2）通过旋转矩阵，将绿色B坐标系下的激光点D旋转到蓝色B坐标系，在通过平移将蓝色B坐标系下的激光点D平移到里程计坐标系下，经过计算得出激光点D在里程计坐标系下的位姿；
（3）同样，通过旋转+平移，将里程计坐标系下的D点变换到基准坐标系（绿色A坐标系）下，这样就得到激光点D在绿色A坐标系下的XY轴分量。
（4）根据XY轴分量根据sqrt(dx2+dy2)算出激光点距离，atan2(dy,dx)算出角度，对于所有激光束都是按照上述步骤处理。
4、坐标变换涉及的数学知识
从B点绿色坐标系将激光点D变换到odom坐标系；
注意这里是B坐标系，坐标系的里程计位姿为x_t和y_t，这是经过计算就转换回了odom坐标系。 手写推导如下，注意看其中的边角关系：
从odom坐标系将激光点D变换到该帧数据的基准坐标系（A点绿色坐标系）：
这里重新将里程计坐标系下的D点位姿转换回A基准坐标系下的表示l_x和l_y，需要注意的是使用的是上面刚计算得到的x_1和y_1，这里假设A基准坐标系的里程计位姿为x_0和y_0，而上面的分段插值B坐标系为x_t和y_t，注意变量下标，手推关系如下：
5、低帧率激光雷达运动畸变去除程序流程
6、核心代码
我们按照上面的步骤进行代码解析：
- 用CPU读取激光雷达数据，同时单片机上传里程计数据，两者进行时间同步、数据对齐；
- 分段线性近似、线性插值，找到①激光束原点近似的里程计位姿和②近似的激光雷达坐标系角度；
- 将所有激光束从近似的激光雷达坐标系变换到里程计坐标系上（激光雷达坐标系==》里程计坐标系）；
- 最后将所有激光束从里程计坐标系变换到基准坐标系、封装数据（里程计坐标系==》基准坐标系）；
这里的lidar_undistortion.cpp文件中的LidarMotionCalibrator::lidarCalibration(std::vector<double>& ranges, std::vector<double>& angles, ros::Time startTime, ros::Time endTime, tf::TransformListener* tf_)中传入的参数全部来自my_slam_gmapping.cpp的laserCalback(const sensor_msgs::LaserScan::ConstPtr& scan)函数，接着往下看：
6.1 时间同步，数据对齐
根据tf_变换缓存数据中，根据帧起始线束时间start_time，得到的帧起始线束时的雷达位姿（激光雷达坐标系基准坐标系到里程计坐标系的变换），代码如下：
// 激光雷达运动畸变去除函数
void LidarMotionCalibrator::lidarCalibration(std::vector<double>& ranges, std::vector<double>& angles,
ros::Time startTime, ros::Time endTime, tf::TransformListener* tf_)
{ // tf_是激光雷达坐标系基准坐标系到里程计坐标系的变换
// 激光束的数量
int beamNumber = ranges.size();
// 分段时间间隔，单位us
int interpolation_time_duration = 5 * 1000; // 单位us, 5ms
tf::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，转化时间单位为us startTime.toSec()单位是s，先转换为ms，再转换为us，接着就是ns
double end_time = endTime.toSec() * 1000 * 1000;
double time_inc = (end_time - start_time) / beamNumber; // 每相邻两束激光数据的时间间隔，单位us
// 得到start_time时刻，laser_link在里程计坐标下的位姿，存放到 frame_start_pose，这是后面进行 点云去畸变的 基准坐标
if (!getLaserPose(frame_start_pose, ros::Time(start_time/1000000.0), tf_))
{
ROS_WARN("Not Start Pose, Can not Calib");
return ;
}这里看一下getLaserPose()函数，该函数的作用是从tf_缓存数据中，寻找laser_link对应时间戳的里程计位姿：
// 从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_); // 这里只是初始化，B绿色坐标系到odom蓝色坐标系的转换关系odom_pose此时为单位阵
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;
}
// 转换一个 带时间戳的位姿tmp 到 目标坐标系odom_name_的位姿，变换关系输出到 odom_pose
tf_->transformPose(odom_name_, tmp, odom_pose); // 这里相当于得到了B绿色坐标系到odom蓝色坐标系的转换 odom_pose
}
catch (const std::exception& e)
{
std::cerr << e.what() << '\n';
}
return true;
}6.2 分段线性近似，线性插值
① 先计算时间，一直进行分段时间点计算mid_time（分段终点时间），如果分段终点时间减去分段起始时间大于分段时间，那么开始进行线性插值；
② 根据分段终点时间mid_time，再次调用getLaserPose()函数得到当前mid_time时间的激光雷达里程计位姿x_t和y_t；
③根据当前符合插值的线束数序号i，计算需要插值的个数。
④对本分段的激光点进行运动畸变的去除；
⑤更新时间、当前分段起始线束序号和当前线束起始位姿。
// 分段个数计数
int cnt = 0;
// 当前插值的段的起始坐标
int start_index = 0;
// 默认基准坐标系就是第一个位姿的坐标系，A坐标系的里程计位姿 这是后面进行 点云去畸变的 基准坐标
frame_base_pose = frame_start_pose;
for (int i=0; i<beamNumber; i ++)
{
// 按照分割时间分段，分割时间大小为interpolation_time_duration
double 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_pose
if (!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;
// 对本分段的激光点进行运动畸变的去除
lidarMotionCalibration(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; // 将上一分段的结束位姿，传递为下一分段的开始位姿
}
}6.3 激光雷达坐标系==》里程计坐标系与6.4 里程计坐标系==》基准雷达坐标系
这两步都在LidarMotionCalibrator::lidarMotionCalibration中，具体代码如下：
①首先获得基准雷达坐标系A（里程计下的描述）、分段起始雷达坐标系B和分段结束雷达坐标系C的位姿，代码如下：
// 根据传入参数，对任意一个分段进行插值
void LidarMotionCalibrator::lidarMotionCalibration(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); // eg. beam_number=5-1=4 , beam_step = 1.0/(4-1) = 1/3, 即1到5之间插2、3、4 三个数
// 该分段中，在里程计坐标系下，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位姿的起始位姿、结束位姿，以及该帧激光数据在里程计坐标系下基准坐标系的位姿 其实为了得到里程计坐标系下的（x，y，0）坐标
tf::Vector3 start_pos = frame_start_pose.getOrigin(); start_pos.setZ(0); // B坐标系
tf::Vector3 end_pos = frame_end_pose.getOrigin(); end_pos.setZ(0); // C坐标系
tf::Vector3 base_pos = frame_base_pose.getOrigin(); base_pos.setZ(0); // 因为是平面，所以这里Z轴数据设置为0 // A坐标系②创建临时变量：
// 临时变量
double mid_angle;
tf::Vector3 mid_pos; // 线性插值点
tf::Vector3 mid_point; // [l_x,l_y]^T和[x_1，x_1]^T的暂存变量
double lidar_angle, lidar_dist; // 去畸变修正后的A基准坐标系下的距离和角度③求出B雷达坐标系下D点的表示lx和ly，将其转换到B坐标系的x_t和y_t坐标系下，得到里程计下的描述x_1和y_1，代码如下：
// beam_number为该分段中需要插值的激光束的个数
for (int i=0; i < beam_number; i ++)
{
/******** 注意：这里其实分两部分进行插值：1.角度（偏航角）插值 ； 2. 位姿（位置）插值 *******/
// 得到第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,laser_y;
laser_x = lidar_dist * cos(lidar_angle); // 求出lx
laser_y = lidar_dist * sin(lidar_angle); // 求出ly
// 该分段中的该激光点，变换的里程计坐标系下的坐标，（这里用插值的激光雷达坐标系近似上面哪个真实存在的激光雷达坐标系，因为知道数值，可以进行计算）
double odom_x,odom_y;
double cos_ , sin_;
cos_ = cos(mid_angle);
sin_ = sin(mid_angle);
odom_x = laser_x * cos_ - laser_y * sin_ + mid_pos.x(); // 求出x_t，y_t雷达位姿里程计坐标系下的激光点位置D的位置 x1和y1
odom_y = laser_x * sin_ + laser_y * cos_ + mid_pos.y();
mid_point.setValue(odom_x, odom_y, 0); // [x_1, y1]^T④ 得到基准坐标系A的里程计位姿x_0和y_0，将B点得到的里程计坐标系下的x_1和y_1转换到A基准坐标系下，得到上面的[l_x, l_y]^T：
//在里程计坐标系下，基准坐标系的参数 也就是A坐标系x_0和y_0的激光雷达里程计位姿坐标系
double x0,y0,a0,s,c;
x0 = base_pos.x();
y0 = base_pos.y();
a0 = base_angle_r;
s = sin(a0);
c = cos(a0);
// 把该激光点都从里程计坐标系下，变换到基准坐标系下， 也就得到了B激光雷达坐标系下D激光点位置在A基准坐标系下的位置l_x, l_y
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); // [l_x, l_y]^T⑤计算A基准坐标系下，去畸变后的修正距离和角度：
// 然后计算该激光点以起始坐标为起点的 dist angle 重新计算出A基准坐标系下D点的距离和角度
double 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;
}
}
}参考文章：
1. https://blog.csdn.net/zhao_ke_xue/article/details/105734833 详解2D激光雷达运动畸变去除
扫一扫
97
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
