激光雷達數學模型及運動畸變去除

黃 越，郁漢琪，陸宇豪

（南京工程學院 機械工程學院，南京 211100）

激光雷達作為SLAM系統的主要傳感器，影響定位準確度與地圖構建效果。

1 激光雷達傳感器介紹

1.1 激光雷達常用測距原理

對于市面上的主流激光雷達，主要是用于環境探測、地圖構建，按技術路線可分為：三角測距激光雷達，TOF激光雷達。

TOF測距是通過測量光的飛行時間來計算距離。根據發射激光信號的不同形式，TOF測距法可分為脈沖測距和相位測距[1]。脈沖激光測距基本原理是激光器發射激光脈沖，計時器記錄發射時間T1；接收器接收被反射的激光，計時器記錄接收時間T2。兩個時間的間隔△T即為激光的“飛行時間”。光速c是一定的，因此距離可由速度與時間計算得到。相位測距是將發射的連續的激光進行幅度調制（調制光的光強隨時間做周期性變化）[2]，被反射后，激光產生相位變化，根據相位差來間接測量時間和距離（曲線測距）[3]。

三角測距根據攝像頭的光斑成像位置，利用相似三角形的幾何關系計算距離。雙目視覺、結構光測距等，都可歸納為三角測距原理。激光器發射激光，激光照射到物體表面后發生反射；基線另一端的CCD相機接收反射信號，并記錄入射光與反射光的夾角。依照光學路徑，不同距離的物體會顯示在CCD上的不同位置。因此，在已知激光源和CCD間距離的情況下，根據相似三角形的幾何關系[4]，就可以推導出激光源與物體表面的距離。

1.2 激光雷達數學模型介紹

1）光束模型

激光雷達的光束模型需要第t時刻機器人的位姿xt和地圖m，來獲得第t時刻激光數據，包含K個激光束測量的距離值zt。在非結構化環境中機器人位姿微小的變化就會造成期望值巨大的改變，從而導致得分進行突變，因此光束模型很少有應用場景。

2）似然場模型

似然場模型是對圖像進行高斯平滑，在任何環境中期望值對于位姿都是平滑的。似然場得分的技術不需要經過光線追蹤，而是通過高斯分布計算出一個得分換成查表的形式。似然場模型從程序一開始就是在進行離線計算，所以對一幀激光來說，只需要查360次表，同時適合結構化環境和非結構化環境中[5]。

2 激光雷達運動畸變

2.1 運動畸變產生的原因

運動畸變是由于激光雷達在采集數據過程中，隨著載體運動，導致一幀激光數據中的激光點不是在同一時刻采集的，即不同激光點的坐標不一致。在靜止場景中，即激光雷達載體本身靜止且環境中沒有運動物體，那么采集到的一幀點云沒有畸變。在運動場景中，即激光雷達載體運動或者環境中存在運動物體，采集到的點云數據有畸變現象。采集一幀數據起始點的雷達原點位置和采集一幀數據終止點時的雷達原點位置出現了偏移，三維環境信息出現了畸變。

由于雷達計算激光點坐標時，都是以接收到激光束時刻的雷達自身坐標系為基礎的，所以載體運動過程中，每一列激光點的基準坐標系都是不一樣的，但是他們在同一幀點云里，希望能統一在同一個坐標系下，所以需要知道每次采集時的坐標系相對于初始時刻坐標系的轉換關系。

2.2 運動畸變的去除

點云的畸變是由于一幀數據的采集過程中，雷達載體進行運動導致，因而需要先計算采集過程中雷達的運動，然后在每幀中根據每個點的相對時間進行補償這個運動量，包括旋轉和平移的補償。

去除激光雷達運動畸變的方法有包括ICP（Iterative Closest Point）、VICP（ Velocity Estimation Iterative Closest Point）的純估計方法和傳感器的輔助方法。ICP方法沒有考慮到激光的運動畸變，以及當前激光數據錯誤問題；VICP法忽略了低頻率激光雷達掃描對小車勻速運動的假設不成立，以及數據預處理和狀態估計過程耦合的問題。針對ICP和VICP方法的缺點，主要介紹傳感器的輔助方法以及提出輪式方法與匹配方法進行融合從而去除運動畸變。

3 傳感器輔助方法

傳感器輔助方法包括慣性測量單元（IMU）輔助方法及輪式里程計輔助方法。慣性測量單元輔助方法是直接測量機器人的角速度和線加速度，具有較高的角速度測量精度[6]，但測量頻率極高，一般在1kHz～8kHz，并且線加速度精度太差，二次積分在局部的精度依然很差。輪式里程計直接測量機器人的位移和角度，具有較高的局部角度測量精度[7]，與IMU輔助方法相比，更新速度更高，一般在 100kHz～ 200kHz。

輪式里程計方法的原理是求解當前幀激光數據中每一個激光點對應的機器人位姿，根據求解的位姿把所有激光點轉換到同一坐標系下，重新封裝成一幀激光數據發布出去。

已知當前幀激光起始時間ts，結束時間te，兩個激光束之間的時間間隔為Δt。輪式里程輔助算法步驟如下：

1）求解 ：ts，te時刻的位姿ps，pe

里程計隊列正好和激光數據同步，假設第i和第j個數據的時刻分布為ts，te：

在ts時刻沒有對應的里程計位姿，則進行線性插值。設在l，k時刻有位姿，且l＜s＜k，則 ：

2）二次插值

在一幀激光數據之間，認為機器人做勻加速運動。機器人的位姿是關于時間t的二次函數，設：tm=(ts+te)/2，且l＜m＜k，則 ：

已知：ps，pm，pe，則可以插值一條二次曲線：

3）二次曲線的近似

用分段線性函數對二次函數進行近似，分段數大于3時近似誤差可忽略不計。在ts和te時間段內一共取m個位姿{ps,ps+1,…,ps+m-2,pe}，位姿通過線性插值獲取，在這m個位姿之間，進行線性插值：

設ps和ps+1之間有n個位姿{ps,ps1,…,ps(n-2),ps+1}，假設si在l和k之間，則：

4）坐標系統轉換與激光數據發布

一幀激光數據K個激光點，每個激光點對應的位姿{p1,p2,…,pk}，通過上述的方法插值得到。xi為轉換之前的坐標，為轉換之后的坐標，則：

把轉換后的坐標轉換為激光數據發布出去：

4 輪式方法與匹配方法的融合

4.1 融合方法思想

本文提出了將輪式里程計方法與ICP方法進行融合，其主要思想如下：

1）用里程計方法進行矯正，去除絕大部分的運動畸變。

2）認為里程計存在誤差，但是誤差值是線性分布的。

3）用ICP方法進行匹配，匹配的結果作為正確值，得到里程計的誤差值。

4）把誤差值均攤到每一個點上，重新進行激光點位置修正。

5）再一次進行ICP迭代，直到收斂為止。

4.2 實驗結果分析

4.2.1 激光雷達性能參數

試驗使用的激光雷達選用思嵐RPLIDAR A2型號，其性能參數見表1。

表1 思嵐RPLIDAR A2性能參數Table 1 RPLIDAR A2 performance parameters

4.2.2 激光雷達去畸實驗

為了驗證激光雷達運動畸變去除算法對同步定位與地圖構建（Simultaneous Localization And Mapping，SLAM）系統在實際環境中的影響[8]，在機器人系統ROS平臺中采用兩種算法構建環境地圖，在可視化界面RVIZ中顯示環境地圖。設置智能移動底盤移動的線速度為0.5 m/s。

兩種算法所建地圖如圖1、圖2所示。

從圖中可以明顯看出，當移動底盤發生運動時，激光雷達所掃描的點云數據發生了畸變現象。比較圖1與圖2可以看出，采用輪式里程計與ICP融合的方法去除激光雷達產生運動畸變的效果更好，所建地圖精度更高。通過比較兩種算法的建圖效果，提出的輪式里程計與ICP融合方法提高了系統構建環境地圖的精度。

圖1 輔助方法的建圖效果Fig.1 Mapping effect of the auxiliary method

圖2 輪式里程計和ICP融合方法的建圖效果Fig.2 Mapping effect of wheel odometer and ICP fusion method

5 結論

本文介紹了激光雷達的測距原理以及數學模型，并根據載體運動時激光雷達采集數據過程中產生的運動畸變，詳述了傳感器輔助方法去除激光雷達運動畸變的原理。在此基礎上，針對輪式里程計輔助方法的不足，提出了輪式里程計和ICP 融合的方法。當智能底盤在室內移動時，分別使用兩種算法進行構建環境地圖。實驗結果表明，輪式里程計和 ICP 融合的方法所構建的地圖精度更高。