摘要

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

主要貢獻(xiàn)

我們工作的主要貢獻(xiàn)有：

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

提出了一種局部地面關(guān)聯(lián)對應(yīng)方法，將連續(xù)關(guān)鍵幀的地面平面地標(biāo)關(guān)聯(lián)起來。

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

算法流程

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

A. 系統(tǒng)

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

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

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

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

如果滑動地圖中被關(guān)聯(lián)的點的協(xié)方差過大，它將被過濾。在實踐中，會求取協(xié)方差矩陣的跡來跟設(shè)定值作比較。

最后，配準(zhǔn)的點云中沒有被關(guān)聯(lián)到的點會被添加到滑動地圖中作為新的觀察。

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

上面這三個狀態(tài)都與加速度計和gyro的bias有關(guān)系，所以bias也被當(dāng)做待優(yōu)化的項。

D. 地平面關(guān)聯(lián)估計

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

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

實驗分析

結(jié)論

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

審核編輯 ：李倩