摘要

本文提出了一種三維激光雷達SLAM，命名為Ground-SLAM，該算法利用了多層環境下的地面的約束去減少由激光雷達測量偏差引起位姿漂移。該算法是基于位姿圖優化框架開發。在前端，提出了一種基于傳感器為中心的滑動地圖（sensor-centric sliding map）的新型的激光點云里程計（LO）來進行運動估計。Sensor-centric sliding map可以有效的根據誤差傳遞模型過濾掉過期的特征。在每個關鍵幀中，滑動的地圖（sliding map）被保存為一個局部地圖。在局部地圖中，算法會提取臨近的平面并以最近點（Closest Point）參數化形式保存。然后，在不同關鍵幀中被觀測到的平面被聯系起來，地面的約束被放入圖優化框架中。最終，回環檢測，利用非線性方式整體優化形成一個全局優化地圖。實驗結果展示了該算法擁有很高的精確度。

主要貢獻

我們工作的主要貢獻有：

提出一種以傳感器為中心的滑動地圖（sensor-centric sliding map）維護方法， 用基于觀察的方法可以有效地消除冗余特征的同時保持基本的特征 。

提出了一種局部地面關聯對應方法，將連續關鍵幀的地面平面地標關聯起來。

提出一種 3D LiDAR SLAM 算法，它利用室內多層環境中的地面約束來減小由 LiDAR 測量偏差引起的漂移。

算法流程

在對如何表達無限平面的參數的問題中，文中提到Hesse Form （HF）過度參數化用一個3D向量來表示2DOF且在最小二乘優化中容易遭遇奇異矩陣的問題。球坐標在俯仰角是正負90度的時候會出現表達模糊。四元數來表達平面，他們的聯系并不清晰。 本文用CP（Closest Point）的方法，通過定義在平面上到給點坐標系原點的最近點來表示無限平面。通過利用HF作為中間傳遞參數的方式，在利用CP可以得到：

A. 系統

激光點云的去畸變利用了IMU混合輪速計形成的運動估計（EKF）的結果。去畸變后的點云利用了point-to-plane ICP算法，進行連續掃描的點云的配準得到相對運動。本文采用傳感中心化的滑動地圖的方法來克服激光雷達點云的稀松性和增加LO的精度。以傳感器為中心的滑動圖在每個關鍵幀記錄一個局部地圖，并且用最小二乘的方式提取地平面。 當地平面的約束被放入圖優化框架中。姿態漂移，尤其是由 LiDAR 測量偏差引起的豎直方向的漂移被壓縮，減小。增加了LO的軌跡的精度。然后通過進行閉環檢測，殘差被最小化得到全局的優化地圖。B. 激光雷達里程計與傳感器為中心的滑動地圖 我們的 LiDAR 里程計方法主要包括兩個 部分：scan-to-map 點云配準框架和一個以傳感器為中心的滑動地圖方法。

傳感器為中心的滑動地圖包括下面幾個步驟：

首先，配準后的的新掃描點云與滑動地圖點云通過檢查預定義的距離來度量，例如歐幾里得距離，馬氏距離， 等等

然后，選擇關聯的滑動地圖點，并且重置這些點云的協方差矩陣為與他關聯的新點云的協方差矩陣，這個通常稱為觀察誤差。

如果滑動地圖中被關聯的點的協方差過大，它將被過濾。在實踐中，會求取協方差矩陣的跡來跟設定值作比較。

最后，配準的點云中沒有被關聯到的點會被添加到滑動地圖中作為新的觀察。

C. 地平面的提取 為了利用幾何地面約束，我們引入了一個合理的假設，假設地面靠近車輛的點可以建模為無限平面。在關鍵幀中，地面靠近車輛運動路徑的點首先被分割。然后，初始無限平面 CP參數用RANSAC來估計。 RANSAC 估計的初始平面參數可能噪音很大。因此，我們制定了一個加權最小二乘 （7） 和 （8） ，并最小化地面點和 CP 之間的點到面距離。

上面這三個狀態都與加速度計和gyro的bias有關系，所以bias也被當做待優化的項。

D. 地平面關聯估計

由于在此模型中地面被建模為平面地標，在不同位置提取的地平面需要互相關聯。圖 4 顯示了一個室內結構化多層地面估計。假設有幾個順序 關鍵幀 Fi， 其中i = 1， 。.. ， 。每個關鍵幀都有一個估計的姿態由LO，和觀察到的接地平面與估計參數及其不確定性Σ提供，則剩下的問題是確定這些觀察到的平面之間的關系 。 我們通過觀測CP參數變化的方法來觀察局部平面的相關性估計。

E.聯合位姿圖優化 當觀察到的地平面之間的對應關系已確定之后，地面觀測約束會被融合到位姿圖優化框架中，如圖5（b）所示。使用（3）和（4），我們可以 將幀 Fj 中的地平面觀測轉換為全局坐標。如（19）所示，是殘差的表示。殘差rij的雅可比矩陣表示為（20）、（21）和（22）。

實驗分析

結論

在本文中，我們提出了一種地面約束LiDAR SLAM算法命名為Ground-SLAM。如果在室外環境中，Ground-SLAM可以正常工作，只要地面大致平坦。Ground-SLAM的主要缺點是戶外場景的適應性并不好。我們計劃將語義信息放入LiDAR SLAM以提高準確性和魯棒性。

審核編輯 ：李倩