激光雷達運動畸變去除方法

激光雷達運動畸變產生的原因

在機器人運動過程中，每個激光點都在不同的基準位姿上產生。
激光掃描時伴隨着機器人的運動，每個角度的激光數據都不是瞬時獲得的，當激光雷達掃描的頻率比較低的時候，機器人運動帶來的激光幀的運動畸變是不能被忽略的。例如掃描頻率是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.計算