激光雷达运动畸变去除方法

本文针对二维激光雷达产生的运动畸变进行了分析，如有错误请指正！

一、激光雷达运动畸变产生的原因

在机器人运动过程中，每个激光点都在不同的基准位姿上产生（就是在不同的时刻发出激光时机器人的位置是不同的（由于机器人自身的运动），这也是运动畸变产生的根源所在）。
激光扫描时伴随着机器人的运动，每个角度的激光数据都不是瞬时获得的，当激光雷达扫描的频率比较低的时候，机器人运动带来的激光帧的运动误差是不能被忽略的。
当扫描频率是5Hz的激光雷达，一帧数据的收尾时间差是200ms，如果机器人以0.5m/s的速度沿着x方向行走并扫描前面的墙体，那么200ms后尾部的测量距离和首部的测量距离在x方向上就差10cm。
综上：激光雷达产生畸变的原因有三：

• 激光点数据不是瞬时获得
• 激光测量时伴随着机器人的运动
• 激光帧率较低时，机器人的运动不能忽略（目前 国产的激光雷达 旋转的频率大约在是10Hz ~~100ms）
  
  蓝色是产生运动畸变之后轨迹。

二、为什么需要解决这个问题？

一般激光雷达驱动封装一帧数据时， 默认一帧激光数据的所有激光点是在同一个时刻和同一个位姿下采集的，在低频率扫描的激光雷达应用中（5-10Hz），该问题是不可忽视的。当频率比较高时，我们默认激光数据是在同一个时刻同一个位姿下采集的，可忽视该问题。

三、去除运动畸变的原理

去除激光雷达运动畸变的原理是把一帧激光雷达数据的每个激光点对应的激光雷达坐标转换到不同时刻的机器人里程计上（近似对应的里程计的位置，达到尽可能去除畸变的目的）

四、运动去畸变的方法

1、纯估计方法（ICP / VICP）

• 《1》ICP(Iterative Cloest Point)方法、
  目的：ICP方法是最通用的用来求解两个点云集合转换关系的方法。简单的理解：就是匹配两幅点云，通过ICP算法实现，我们需要求解点云配准的误差最小即可。
  
  已知对应点的求解方法（解析解）：
  当我们知道了两幅点云的点对时（也就是两幅点云的对应点），我们会有解析解（不需要最小二乘去求解）：
  数学描述：
  

未知对应点的求解方法：

ICP算法的缺点：
没有考虑激光的运动畸变，因此得到的激光数据是错误的（计算的时候假设是正确的），从而造成点云匹配发生错误，通过不断的迭代，虽然能够收敛，能够求解，但是存在误差。

• 《2》VICP方法介绍

VICP算法考虑了激光的运动畸变，也就是考虑了机器人的运动和速度的存在。

• ICP算法的变种
• 考虑了机器人的运动
• 匀速运动（假设是匀速运动）
• 进行匹配的同时估计机器人的速度
  
  
  矫正后的结果
  
  VICP 方法的缺点：（1）低频率情况下（5Hz）,匀速运动假设不成立 （2）数据预处理和状态估计过程耦合
  如何解决上述问题呢？
  我们尽可能的反应运动情况，实现预处理和状态估计的解耦，我们提出了传感器辅助的方法。

2、传感器辅助的方法（odom，IMU）

传感器辅助的方法就是极高的位姿更新频率（200Hz），可以比较准确的反应运动情况，以及较高精度的局部位姿估计和状态估计完全解耦

《1》惯性测量单元IMU

• 直接测量角速度和线加速度
• 具有较高的角速度测量精度（比较准确）
• 测量频率极高(1kHz~8kHz)
• 线加速度精度太差，二次积分在局部的精度依然很差（这一点我们就可以否定他在移动机器人上面的使用，主要原因是精度不高）

《2》轮式里程计odom

• 直接测量机器人的位移和角度
• 具有较高的局部角度测量精度
• 具有较高的局部位置测量精度
• 更新速度较高(100Hz~200Hz)

如何使用odom去除运动畸变？




COde

#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/LaserScan.h>
#include <iostream>
#include <dirent.h>
#include <fstream>
#include <iostream>
#include <string>
//如果使用调试模式，可视化点云，需要安装PCL
#define debug_ 0

#if debug_
#include <pcl-1.7/pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl-1.7/pcl/visualization/cloud_viewer.h>
pcl::visualization::CloudViewer g_PointCloudView("PointCloud View");//初始化一个pcl窗口
#endif

class LidarMotionCalibrator
{
   
public:
    //构造函数，初始化tf_、订阅者、回调函数ScanCallBack
    LidarMotionCalibrator(tf::TransformListener* tf)
    {
   
        tf_ = tf;
        scan_sub_ = nh_.subscribe(scan_sub_name_, 10, &LidarMotionCalibrator::ScanCallBack, this);
        scan_pub_ = nh_.advertise<sensor_msgs::LaserScan>(scan_pub_name_, 1000);
    }
    //析构函数，释放tf_
    ~LidarMotionCalibrator()
    {
   
        if(tf_!=NULL)
            delete tf_;
    }
    // 拿到原始的激光数据来进行处理
    void ScanCallBack(const sensor_msgs::LaserScanConstPtr& scan_msg)
    {
   
        //转换到矫正需要的数据
        ros::Time startTime, endTime;
        //一帧scan数据到来首先得出，开始结束的时间戳、数据的size
        startTime = scan_msg->header.stamp;
        sensor_msgs::LaserScan laserScanMsg = *scan_msg;

        //得到最终点的时间
        int beamNum = laserScanMsg.ranges.size();
        endTime = startTime + ros::Duration(laserScanMsg.time_increment * beamNum);

        // 将数据复制出来
        std::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();
        //转换为pcl::pointcloud for visuailization
        //数据矫正前、封装打算点云可视化、红色
        visual_cloud_scan(ranges,angles,255,0,0);
        #endif
        //进行矫正
        Lidar_Calibration(ranges,angles,startTime,endTime,tf_);
        //数据矫正后、封装打算点云可视化、绿色
        //转换为pcl::pointcloud for visuailization
        #if debug_
        visual_cloud_scan(ranges,angles,0,255,0);
        #endif
        //发布矫正后的scan
        //ROS_INFO("scan_time:%f",ros::Duration(laserScanMsg.time_increment * beamNum).toSec());
        scan_cal_pub(ranges,startTime,ros::Duration