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

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

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

去除运动畸变的原理

去除激光雷达运动畸变的原理是把一帧激光雷达数据的每个激光点对应的激光雷达坐标转换到不同时刻的机器人里程计上

去除运动畸变的方法

纯估计方法

纯估计方法：
ICP（迭代最近邻匹配 iterative closest point）——点对点匹配
ICP算法的基本原理是：分别在带匹配的目标点云P和原点云Q中，按照一定的约束条件，找到最邻近点（pi，qi），然后计算出最优匹配参数R和T，使得误差函数最小，误差函数如下：
(https://img-blog.csdnimg.cn/20200628143900198.jpg#pic_center)
其中n为最邻近点对的个数，pi为目标点云P中的一点，qi为原点云Q中与pi对应的最近点，R为旋转矩阵，t为平移向量。

ICP算法步骤：
（1）在目标点云P中取点集pi∈P；
（2）找出源点云Q中的对应点集qi∈Q，使得||qi-pi||=min；
（3）计算旋转矩阵R和平移矩阵t，使得误差函数最小；
（4）对pi使用上一步求得的旋转矩阵R和平移矩阵t进行旋转和平移变换，的到新的对应点集pi’={pi’=Rpi+t,pi∈P}；
（5）计算pi’与对应点集qi的平均距离；
（6）如果d小于某一给定的阈值或者大于预设的最大迭代次数，则停止迭代计算。
否则返回第2步，直到满足收敛条件为止。

ICP算法关键点：
（1）原始点集的采集
均匀采样、随机采样和法矢采样
（2）确定对应点集
点到点、点到投影、点到面
（3）计算变化矩阵
四元数法、SVD奇异值分解法

VICP（速度估计ICP——velocity estimation ICP）即ICP算法的变种算法，考虑了机器人的运动为匀速运动，进行匹配的时候同时估计机器人的速度。

里程计辅助

里程计辅助方法是直接测量机器人的卫衣和角度，具有较高的局部角度测量精度和局部位置测量精度，直接用CPU读取激光雷达数据，同时单片机上传里程计数据，两者进行时间同步，在CPU上统一进行运动畸变去除。
在选取里程计辅助的里程计的时候要综合考虑里程计的测量精度，所以在IMU和轮式里程计二者之间就会选择轮式里程计，因为其可以直接测量机器人的位移和角度、具有较高的局部角度测量精度、具有较高的局部位置测量精度、更新速度较高（100Hz～200Hz）

辅助流程：
·一直当前激光帧的起始时间t1
·两个激光束之间的时间间隔t
·里程计数据按照时间顺序存储在一个队列中
·求解当前帧激光数据中的每一个激光点对应的里程计数据（机器人位姿）
·根据求解的位姿把所有的激光点转换到同一坐标系下
·重新封装成一帧激光数据发布出去

具体过程：
一、获取激光雷达数据
1.获取scan的起始、结束时间ts，te
2.转化为rangle、angle
3.开始矫正（Lidar_calibration）
4.发布数据（scan_cal_pub)

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(laserScanMsg.time_increment * beamNum).toSec());
        //进行显示
        #if debug_
        g_PointCloudView.showCloud(visual_cloud_.makeShared());
        #endif
    }

二、矫正
1.在te，ts之间定义分段时间
2.获取激光雷达在odom中的位姿（getLaserPose）
3.取出分段数据（在Lidar_calibration已经分好，然后调用Lidar_MotionCalibration）
4.开始去运动畸变（Lidar_motionCalibration）
5.更新时间
6.重复步骤3，直到所有分段结束

bool getLaserPose(tf::Stamped<tf::Pose> &odom_pose,
                      ros::Time dt,
                      tf::TransformListener * tf_)
    {
        odom_pose.setIdentity();
 
        tf::Stamped < tf::Pose > robot_pose;
        robot_pose.setIdentity();
        robot_pose.frame_id_ = scan_frame_name_;//这里是laser_link
        robot_pose.stamp_ = dt;                 //设置为ros::Time()表示返回最近的转换关系
 
        // get the global pose of the robot
        try
        {   //解决时间不同步问题
            if(!tf_->waitForTransform(odom_name_, scan_frame_name_, dt, ros::Duration(0.5)))             // 函数说明见后
            {
                ROS_ERROR("LidarMotion-Can not Wait Transform()");
                return false;
            }
            tf_->transformPose(odom_name_, robot_pose, odom_pose);//说明见后
        }
        catch (tf::LookupException& ex)
        {
            ROS_ERROR("LidarMotion: No Transform available Error looking up robot pose: %s\n", ex.what());
            return false;
        }
        catch (tf::ConnectivityException& ex)
        {
            ROS_ERROR("LidarMotion: Connectivity Error looking up looking up robot pose: %s\n", ex.what());
            return false;
        }
        catch (tf::ExtrapolationException& ex)
        {
            ROS_ERROR("LidarMotion: Extrapolation Error looking up looking up robot pose: %s\n", ex.what());
            return false;
        }
 
        return true;
    }

三、去运动畸变
1.得到起始位姿、结束位姿
2.计算出角度差值与线性插值
3.得到这帧激光的角度、距离
4.求激光坐标下的坐标
5.转换到odom坐标系下的坐标
6.计算