摘要

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

主要貢獻(xiàn)

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

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

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

A. 系統(tǒng)

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

傳感器為中心的滑動(dòng)地圖包括下面幾個(gè)步驟：

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

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

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

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

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

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

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

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

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）。

實(shí)驗(yàn)分析

結(jié)論

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

審核編輯 ：李倩