【論文筆記】高效、稀疏的三維雷射SLAM ： A Novel 3D LiDAR SLAM Based on Directed Geometry Point and Sparse Frame

中國科學院自動化研究所

基于scan2map的三維雷射slam魯棒性好, 深度資訊精确, 是相關領域的研究熱點。但是，為了達到滿意的定位精度，還需要建構一個大的稠密地圖，這導緻scan2map的比對非常耗時。

為了解決這個問題，本文提出了一種基于有向幾何點（DGP）和稀疏幀的新型3D雷射SLAM（包括閉環檢測以及位姿圖優化）。前者用于在空間次元上提供點的稀疏分布，後者用于在時間序列中産生幀的稀疏分布。該政策提升了三維雷射SLAM的效率和裡程計位姿估計的精度。為了補償稀疏幀導緻的定位和建圖的精度損失，本文使用point propagation方法來提升有向點的品質和scan2map的比對精度。

論文方法 DGP-SLAM

有向點的表示：（x,y,z,i,j,k）, 坐标 + 機關方向向量

從雷射雷達的一幀中提取：

• directed edge point set L t L_t Lt​, 點雲中的點的機關方向向量代表所在直線的方向
• directed planar point set P t P_t Pt​，點雲的機關方向向量代表點所在平面的法向量。

問題描述:

給定幀 C t C_t Ct​（t=0,1,2，…）中的三維雷射雷達點雲，提取有向邊點和有 向平面點集 L t L_t Lt​和 P t P_t Pt​，估計位姿變換 T k α C t T_{k_αC_t} Tkα​Ct​​，并通過scan2map來保留fusion frames的有向點集 L x y L^y_x Lxy​和 P x y P^y_x Pxy​。在此基礎上，利用關鍵幀上的有向點集進行閉環檢測，并在關鍵幀之間執行全局位姿圖優化。

處理流程概述

DGP-SLAM 包括DGP extraction, scan-to-map matching 以及loop closure 三個部分。首先對新的一幀進行有向點提取，然後根據有向點在局部圖中通過疊代資料關聯和位姿優化進行scan2map比對。接着依次執行幀選取和點傳播來更新局部地圖。最後閉環檢測子產品結合關鍵幀的有向點集以及幀間相對位姿來檢測閉環以及優化全局位姿圖。

算法輸出該新的幀的全局姿态和一個周遊區域的全局地圖。

一、DGP Extraction

有向點是從原始點雲資料中生成的新的點，其坐标是點雲鄰域的中心，方向向量是所在邊的朝向或所在面的法向量。

首先将地面點雲分離出來作為平面點，

地面點提取方法：《fast segmentation of 3Dpoint clouds: A paradigm on LiDAR data for autonomous vehicle appli-cations》

然後在非地面點雲中提取edge points和planar points，提取方法與loam相同

在此基礎上，對每個未周遊的邊或平面點，通過KD樹的半徑搜尋得到相鄰點u1，u2，···um，并計算領域的中心 c u c_u cu​和方向向量 d ^ u \hat d_u d^u​，其中方向向量對應于矩陣 ∑ U = 1 m ∑ i = 1 m ( u i − c u ) ( u i − c u ) \sum_U=\frac1m\sum^m_{i=1}(u_i−c_u)(u_i−c_u) ∑U​=m1​∑i=1m​(ui​−cu​)(ui​−cu​)的最小或最大特征值的機關特征向量，取決于是平面點還是邊緣點。

然後将（ c u c_u cu​, d ^ u \hat d_u d^u​）添加到Lt或者Pt中，并将u1，u2，···um标記為周遊。

注意這裡的有向幾何點的數量遠小于幾何點的數量，因為需要多個幾何點才能生成一個有向幾何點。

二、Scan-to-Map Matching

這一步是DGP_slam的核心所在。

1. Frame Selection

定義了三種幀：fusion frame, key frame, non-fusion frame

該步驟的目的就是決定目前幀屬于哪一種幀。

假設目前幀 c t c_t ct​ 之前的最近的fusion frame是 K α β K_{\alpha}^β Kαβ​,如果二者之間的位姿距離 ∥ t W K α β − t W C t ∥ \left\|t_{W K_{\alpha}^{\beta}}-t_{W C_{t}}\right\| ∥∥∥​tWKαβ​​−tWCt​​∥∥∥​大于門檻值，則将 c t c_t ct​ 标記為fusion frame K α β + 1 K_{\alpha}^{β+1} Kαβ+1​。否則将其标記為新的關鍵幀 K α + 1 K_{\alpha+1} Kα+1​

2. Pose Estimation

該子產品通過基于有向點的疊代資料關聯和姿态優化 ，求解目前幀 c t c_t ct​到最近的關鍵幀 K α K_{\alpha} Kα​之間的相對位姿 T K α C t T_{K_{\alpha} C_{t}} TKα​Ct​​。

1. 首先，将最近的N個fusion frames中的有向點集投影到與目前幀最近的關鍵幀 K α K_{\alpha} Kα​中，進而建構局部地圖。
2. 其次,疊代地進行資料關聯和位姿優化（優化的位姿是關鍵幀到目前幀的相對位姿），每一步的初始化值為：
   • T K α C 0 = T K α C 1 = I T_{K_{\alpha} C_{0}}=T_{K_{\alpha} C_{1}}=I TKα​C0​​=TKα​C1​​=I
   • T K α C t = T K α C t − 1 ( T K α C t − 2 ) − 1 T K α C t − 1 , t ≥ 2 T_{K_{\alpha} C_{t}}=T_{K_{\alpha} C_{t-1}}\left(T_{K_{\alpha} C_{t-2}}\right)^{-1} T_{K_{\alpha} C_{t-1}}, t \geq 2 TKα​Ct​​=TKα​Ct−1​​(TKα​Ct−2​​)−1TKα​Ct−1​​,t≥2

3. 初始化完成後，目前幀 C t C_t Ct​中的有向點集與局部圖之間的資料進行關聯：

   C t C_t Ct​中的有向邊點和有向平面點根據位姿變換 T K α C t T_{K_{\alpha} C_{t}} TKα​Ct​​投影到關鍵幀 K α K_{\alpha} Kα​中，然後通過以下三個嚴格标準來确定局部地圖中的候選關聯點：

   1. point-point distance。該定向幾何點的投影位置與候選點的投影位置之間的距離小于r1
   2. point-line or point-plane distance。有向幾何點投影到候選點所在的直線或平面的距離小于D1
   3. direction difference。投影的有向幾何點的方向與候選點方向之間的夾角小于θ1

具有最小點直線或點面距離的候選點被認為是最佳關聯.

假設對于目前幀 c t c_t ct​中的有向邊點 l i t l_i^t lit​和有向面點 p i t p_i^t pit​，其對應的最佳關聯分别為fusion frame K a b K_a^b Kab​中的有向邊點 l j a , b l_j^{a,b} lja,b​,和fusion frame K c d K_c^d Kcd​中的有向面點 p k c , d p_k^{c,d} pkc,d​，則需要将這兩個最佳關聯資料從fusion frame投影到關鍵幀 K α K_{\alpha} Kα​中：

都投影在了關鍵幀中，就可以計算誤差然後對相對位姿進行優化了：

将優化的目标函數轉換到歐氏空間得到：

使用LM算法進行優化。

重複上述資料關聯和姿勢優化的疊代過程NI（3）次，以獲得最終的姿勢估計結果

3. Point Propagation

根據上一步獲得的相對位姿，重新進行資料關聯，隻不過點線、點面、朝向距離的門檻值更小了（更嚴格），保證關聯的資料對一定是正确的。

• 如果 c t c_t ct​被識别為fusion frame, 則将 c t c_t ct​中的關聯資料點的有向點加入到其有向點集合中，然後執行前向傳播，即 用 之前的fusion frame中的有向點 更新 c t c_t ct​中的、關聯到的對應有向點，然後将之前的fusion frame中的有向點從之前的fusion frame中删除。
• 否則， 執行反向傳播：即 用 c t c_t ct​中的、關聯到的有向點 更新 之前的fusion frame中的對應有向點。

那麼具體的，如何進行有向點的更新呢？

論文提出了增量更新的機制，即對點集U 和點集V 的和矩陣（sum matrixs）、和向量（sum vectors）進行更新：

Φ U = ∑ i = 1 m u i u i T ,   τ U = ∑ i = 1 m u i \Phi_U=\sum^m_{i=1}u_iu_i^T , \ \tau_U=\sum^m_{i=1}u_i ΦU​=∑i=1m​ui​uiT​, τU​=∑i=1m​ui​

Φ V = ∑ i = 1 n v i v i T ,   τ V = ∑ i = 1 n v i \Phi_V=\sum^n_{i=1}v_iv_i^T , \ \tau_V=\sum^n_{i=1}v_i ΦV​=∑i=1n​vi​viT​, τV​=∑i=1n​vi​

經過變換矩陣T=[R,t]的變換後：

Φ V ′ = R Φ V R ⊤ + R τ V t ⊤ + t τ V ⊤ R ⊤ + t t ⊤ τ V ′ = R τ V + t \begin{aligned} \Phi_{V}^{\prime} &=R \Phi_{V} R^{\top}+R \tau_{V} t^{\top}+t \tau_{V}^{\top} R^{\top}+t t^{\top} \\ \tau_{V}^{\prime} &=R \tau_{V}+t \end{aligned} ΦV′​τV′​​=RΦV​R⊤+RτV​t⊤+tτV⊤​R⊤+tt⊤=RτV​+t​

根據變換後的和矩陣、和向量對原來的和矩陣、和向量進行更新：

Φ U ← Φ U + Φ V ′ τ U ← τ U + τ V ′ m ← m + n \Phi_{U} \leftarrow \Phi_{U}+\Phi_{V}^{\prime} \tau_{U} \leftarrow \tau_{U}+\tau_{V}^{\prime} m \leftarrow m+n ΦU​←ΦU​+ΦV′​τU​←τU​+τV′​m←m+n

根據更新後的和矩陣以及和向量，計算出提取DGP時用到的矩陣:

Σ U ∪ V = 1 m Φ U − 1 m 2 τ U τ U ⊤ \Sigma_{U \cup V}=\frac{1}{m} \Phi_{U}-\frac{1}{m^{2}} \tau_{U} \tau_{U}^{\top} ΣU∪V​=m1​ΦU​−m21​τU​τU⊤​

有了這個矩陣，重新執行一次DGP Extraction得到更新後的點

三、Loop Closure

該步驟在關鍵幀上執行閉環檢測以及位姿圖優化，以分開的線程執行。

一旦目前幀被識别為新的關鍵幀，則擷取最近的關鍵幀 K α K_{\alpha} Kα​的有向點集，執行閉環檢測：

1. 首先，在關鍵幀 K 0 K_{0} K0​ 到 K α − S K_{\alpha-S} Kα−S​之間，選擇與 K α K_{\alpha} Kα​空間上最接近的閉環候關鍵選幀。S是為了避免閉環候選幀與 K α K_{\alpha} Kα​太過接近。
2. 若選擇到的閉環候選關鍵幀為%K_i%，則分别建構 K α K_{\alpha} Kα​、 K i K_i Ki​附近的局部地圖，然後執行scan matching。這裡scan matching 使用的是ICP，而不是之前的資料關聯，因為兩個局部地圖可能相距較遠。

3. 若scan matching的得分大于門檻值，則視為檢測到了回環。則将 K α K_{\alpha} Kα​、 K i K_i Ki​之間的相對位姿加入到位姿圖中作為閉環系數，同時目前幀與 K α K_{\alpha} Kα​之間的相對位姿 T K α K α + 1 T_{K_{\alpha} K_{\alpha+1}} TKα​Kα+1​​也被加入到位姿圖作為裡程計系數。

   使用論文《ISAM2: Incremental smoothing and mapping using the bayes tree》的方法進行位姿圖優化，得到新的位姿： T ^ K x K x + 1 ( 0 ≤ x ≤ α ) \hat{T}_{K_{x} K_{x+1}}(0 \leq x \leq \alpha) T^Kx​Kx+1​​(0≤x≤α)，以及包含了有向邊點和有向面點的全局一緻的有向幾何地圖。

實驗結果

消失實驗

kitti dataset

Newer College dataset

速度對比

地圖規模對比

可以看出，由于利用DGP特征融合幀的稀疏性，本方法的地圖尺寸明顯小于其他方法，也為高效的scan2map比對奠定了基礎。