https://www.zhihu.com/video/1577313722581811200
作者：自動駕駛人 | 原文出處： 公眾號【自動駕駛專欄】
雷射雷達建圖演算法
建圖演算法比裏程計演算法以更低的頻率執行，並且每次sweep僅呼叫一次。在第次sweep結束的時候，雷射雷達裏程計生成了去畸變的點雲，並且同時計算得到一個位姿變換，它為雷射雷達在時間段內的運動。建圖演算法將點雲與世界座標系{}下地圖進行匹配和配準，如下圖所示。
為了解釋這一過程，定義為地圖中的點雲，它隨著每一次sweep而累積，並且記為第次sweep結束的時候雷射雷達相對於地圖的位姿。在獲取雷射雷達裏程計演算法輸出的位姿後，建圖演算法透過到這段時間內的sweep和來計算，並且將點雲投影到世界座標系{}下，記為。下一步，建圖演算法透過最佳化雷射雷達位姿來匹配和地圖點雲。特征點提取方式和裏程計演算法相同，只是需要使用10倍數量的特征點。為了尋找特征點的匹配關系，使用邊長為10公尺的立方體區域來儲存地圖中的點雲，將與有交集的立方體中的點雲提取出來並且儲存在3D的KD樹中。建圖演算法在特征點周圍的一定區域內尋找中的點，記為找到的周圍點集。對於一個邊緣特征點，僅需要儲存中邊緣線上的點；對於一個平面特征點，僅需要儲存中平面小塊上的點。接著，計算的共變異數矩陣，以及的特征值和特征向量，分別記為和。如果分布在一條邊緣線上，則的一個特征值遠比另外兩個特征值大，並且最大特征值在中關聯的特征向量表示該邊緣線的方向。另一方面，如果分布在一個平面小塊上，則包含兩個較大的特征值和第三個遠小的特征值，並且最小特征值在中關聯的特征向量表示該平面小塊的法向量。邊緣線或者平面小塊的位置穿過的幾何中心。為了計算每個特征點與其對應關系的距離，選擇邊緣線上的兩個點和平面小塊上的三個點，距離的計算公式與雷射雷達裏程計演算法相同。與裏程計演算法不同的是，中所有點的時間戳相同。非線性最佳化問題再次透過Levenberg-Marquardt方法進行魯棒擬合求解，使用求解得到的位姿將更新到地圖上。為了使得點雲均勻分布，地圖點雲使用邊長為5cm的立方體的體素網格濾波器進行下采樣。
位姿變換融合如上圖所示。藍色區域表示雷射雷達建圖演算法輸出的位姿變換，它每一個sweep輸出一次。橘黃色區域表示雷射雷達裏程計演算法輸出的位姿變換，它以10Hz的頻率執行。雷射雷達相對於地圖的位姿是這兩種位姿變換的組合，其頻率與雷射雷達裏程計相同。
實驗
在本實驗中，處理雷射雷達數據的演算法執行在2.5GHz主頻和6Gib記憶體的電腦上，電腦裏的Linux系統安裝了機器人作業系統（ROS）。本文提出的方法需要兩個核執行，裏程計和建圖程式分別執行在這兩個分開的核上，該軟體程式碼和數據集是公開的。
A.室內和室外實驗
本文提出的方法在室內和室外環境下測試。在室內實驗中，雷射雷達和電池以及膝上型電腦一起放置在一輛手推車上，手推車由一個人推著走。下圖中的圖(a)和圖(c)展示了兩個有代表性的室內環境（狹窄長走廊和大廳）建立的地圖，圖(b)和圖(d)分別展示了相同場景所拍攝的兩張照片。在室外實驗中，雷射雷達被安裝在地面車輛的前方。圖(e)和圖(g)展示了植被道路和旁邊有兩排樹的果園環境下建立的地圖，圖(f)和圖(h)分別展示了相同場景所拍攝的照片。在所有的測試中，雷射雷達以0.5m/s的速度移動。
為了評估地圖的局部精度，在相同環境下采集第二組雷射雷達點雲。雷射雷達在數據采集的過程中保持固定並且放置在每個環境中的一些不同位置處。將之前采集的點雲數據和這第二組點雲數據使用點到平面的ICP方法進行匹配和比較。在匹配完成以後，前一組點雲和第二組點雲對應平面小塊的距離被認為是匹配誤差。下圖展示了誤差分布密度，它表明室內環境下的匹配誤差要小於室外環境下的匹配誤差。這個結果是合理的，因為自然環境下的特征匹配不如人造場景下的特征匹配那麽精確。
除此之外，本文對運動估計的累積漂移進行測試，測試環境選擇包含閉環的走廊作為室內測試環境，這使得實驗能夠在相同位置開始和結束，起始和結束位置之間由運動估計產生的偏差代表了運動估計漂移的大小。對於室外環境，本文選擇果園作為測試環境，攜帶雷射雷達的地面車輛上安裝了一個高精度的GPS/INS裝置用於獲取運動軌跡的真值。測量得到的真值和估計值之間的偏差相對於行駛距離作為相對精度衡量，如下表所示。一般而言，室內測試的相對精度大約為1%，室外測試的相對精度大約為2.5%。
B.IMU輔助
本實驗給雷射雷達安裝了Xsens MTi-10 IMU來處理較大速度改變的情況。與之前不同的是，點雲在發送給本文提出的方法之前，先進行以下兩步預處理：1）有了IMU(陀螺儀)提供的旋轉之後，在一次sweep中接收到的點雲被旋轉對齊到這次sweep的初始旋轉姿態；2）有了IMU（加速度計）提供的加速度測量後，點雲的運動畸變可以被部份消除，就好像雷射雷達在這次sweep中恒速運動那樣。經過這兩步點雲預處理後，將點雲發送給雷射雷達裏程計和建圖程式進行處理。上述第一步預處理過程中，IMU旋轉是用卡爾曼濾波器融合陀螺儀的角速度以及加速度計的讀數來獲得的。下圖圖(a)展示了一個實驗結果，該實驗中由人手持雷射雷達並且在樓梯上移動。當計算圖中紅色軌跡曲線的時候，使用IMU提供的旋轉，本文提出的方法只用來估計平移量，在5分鐘的數據采集過程中角度漂移超過25°。下圖中綠色曲線僅依賴於本文提出的方法，不依賴於IMU。下圖中藍色曲線透過IMU數據進行預處理，然後再用本文提出的方法估計運動，透過實驗發現綠色和藍色曲線存在微小差別。下圖圖(b)展示了藍色曲線對應的地圖，圖(c)比較了圖(b)中黃色矩形框內的兩個地圖的近檢視。圖(c)中上下兩幅圖分別對應於藍色和綠色曲線，仔細比較後發現上面那幅圖中構建的輪廓更清晰（說明運動估計精度高）。
下表比較了使用和沒有使用IMU進行運動估計的相對誤差。雷射雷達由一個步速為0.5m/s的人手持移動，並且雷射雷達上下移動的振幅大約為0.5m，該實驗中真值透過卷尺手動測量。在所有4個實驗中，本文提出的方法加上IMU輔助，獲得了最高的精度，然而只使用IMU提供的旋轉則精度最低。實驗結果表明IMU在消除非線性運動方面非常有效，本文提出的方法可以處理線性運動（因此兩者結合精度非常高）。
C.KITTI數據集測試
本研究也使用KITTI裏程計數據集來驗證所提出的方法，該數據集由一輛在結構化道路行駛的乘用車（下圖圖(a)）上安裝的傳感器來采集。該車輛裝載了一個360°的Velodyne雷射雷達，彩色/單色雙目相機和用於獲取真值的高精度GPS/INS。雷射雷達幀率為10Hz，本文提出的方法使用它來估計裏程計。由於篇幅限制，這裏就不展示結果了，然而鼓勵讀者從數據集網站上去檢視本文的實驗結果。
KITTI數據集主要覆蓋了三種型別的環境：帶有建築物的市區，場景中帶有植被的小路和道路寬闊且周圍環境相對幹凈的高速公路。上圖圖(b)顯示了一個雷射雷達點雲範例以及對應的市區環境的照片。數據集包含的整體行駛距離為39.2公裏，在上傳車輛運動軌跡之後，數據集伺服器會自動計算精度和排名。本文提出的方法按照數據集伺服器評估後在所有方法中排名第一，這些方法不考慮傳感器的形式，其中包括目前最好的雙目視覺裏程計。本方法的平均位置誤差為行駛距離的0.88%，這是透過在100公尺、200公尺、...、800公尺長度的軌跡段來生成的。
總結和展望
使用來自旋轉雷射雷達的點雲進行運動估計和建圖是很困難的，因為該問題包含了運動恢復以及雷射雷達點雲的運動畸變校正。本文提出的方法將該問題劃分為兩個並列的演算法進行求解：雷射雷達裏程計演算法以高頻粗略地估計速度，同時雷射建圖演算法以低頻構建精細的地圖，這兩種演算法的組合能夠即時地進行運動估計和建圖。進一步，本文提出的方法能夠利用雷射掃描模式和點雲分布的優勢進行特征提取，並且特征匹配在裏程計演算法中具有較快的計算速度，在建圖演算法中有足夠的精度。最後，本方法在室內、室外以及KITTI裏程計數據集上進行了測試和驗證。 由於當前的方法沒有辨識回環，未來工作中可以引入回環來校正運動估計漂移，同時也可以透過卡爾曼濾波器融合本方法估計的位姿和IMU數據來進一步減少運動估計漂移。
