基于二元正態分布匹配和非線性優化的激光SLAM研究

陳智君，郝 奇，伍永健，鄭 亮

(1.哈爾濱工業大學蕪湖機器人產業技術研究院，安徽 蕪湖 241000； 2.蕪湖哈特機器人產業技術研究院有限公司，安徽 蕪湖 241000)

0 引言

目前市場上的AGV大多數是紅外引導、磁條導航和二維碼導航，這些有著非自主移動、路徑固定、安裝費時和成本高的缺點。激光SLAM和傳統定位方法不同，激光雷達采集到的環境信息是一幀幀離散的、具有角度和距離信息的點，被稱為激光幀。激光SLAM系統需要對相鄰時刻的激光幀進行幀間匹配或與地圖進行匹配，計算AGV的實時位姿，并使用實時位姿和激光幀對概率地圖進行更新得到導航地圖，來實現后續的路徑規劃和自主導航[1-2]。

現有最強大的激光SLAM算法主要是基于非線性優化的Hector和基于圖優化的Cartographer。Hector是基于多分辨率地圖的非線性優化，其算法在多分辨率地圖中容易陷入局部極值，而且多圖層的更新也耗大量時間[3-4]。Cartographer前端匹配暴力搜索候選項，耗費大量時間，而且最優位姿僅基于概率地圖的優化使得魯棒性不足[5]。對于現有方法中存在的問題，本研究提出了基于二元正態分布匹配和非線性優化的激光SLAM算法，實現了一個實時激光SLAM算法，并在AGV平臺上進行了測試。

1 AGV激光SLAM算法框架

該算法主要運行有三個線程，分別是雷達數據預處理，激光幀匹配和地圖更新和顯示線程，具體框架和流程如圖1所示。

圖1 激光SLAM算法流程圖

第一個是雷達數據預處理線程，用于從激光雷達獲取激光幀，并對激光幀進行濾波處理，包括去除離群點和降采樣。

第二個線程是激光幀匹配線程。首先，第一幀雷達激光幀需要初始化自由概率地圖；其次，將t-1時刻激光幀以一定柵格尺度進行二元正態分布化，對落入激光點超過一定數量的柵格計算其二元正態分布參數。然后，由t時刻激光幀在t-1時刻激光幀位姿態的相關性區間進行投影和概率打分，通過離散尋優，找到離散點集最優解；之后，由二元正態分布概率密度函數和自由概率殘差構建非線性最小二乘目標函數，使用高斯牛頓法迭代求解最優位姿。

第三個線程是地圖更新與顯示線程。用于將匹配線程得到的最優位姿投影到概率地圖，進行地圖更新，最終轉化為導航地圖并進行顯示。

2 激光雷達數據預處理

如圖2a所示，激光掃描到物體的邊緣會發生拖尾，這些無效點影響匹配的穩定性，因此在匹配前使用半徑濾波器去除這些離群點。半徑濾波器遍歷每個點，計算該點到臨近點的距離，如果距離大于預設值，認定為離群點，然后進行剔除，剔除后如圖2b所示。

圖2b是濾波后的圖，但是仍然有很多點，SLAM地圖柵格尺寸為5 cm，同一個柵格內點數非常多，極大影響匹配和更新效率。因此還需要進行體素濾波，在同一個柵格內只保留一個點，在保證點云微觀特征的基礎上降低點云的密度，提高匹配效率，濾波后如圖2c所示。

(a) 原始激光幀 (b) 剔除離群點 (c) 體素降采樣圖2 激光點云濾波圖

3 二元正態分布化及離散尋優

該方法的目的是初步計算AGV在t-1時刻到t時刻的位姿增量，就是計算兩幀激光幀點云的旋轉平移變換。該方法是離散位姿集通過二元正態分布概率函數打分找到一個最優位姿，給最后的非線性優化提供一個良好的位姿初值。

3.1 二元正態分布化激光點云

將t-1時刻濾波后的激光幀點云柵格化，計算每個柵格內激光點集的位置均值:

(1)

可得該柵格的協方差矩陣：

(2)

令pi(x)=Pi-mean，∑=covMatrix。那么可得二元正態分布匹配的歸一化概率F(x):

(3)

其函數分布如圖3所示，然后將t時刻激光點云s以一定位姿投影到t-1時刻的二元正態分布化點云地圖上，那么函數計算的點云歸一化概率得分越高，兩點云重合度越高，認為匹配成功。

圖3 二元正態分布歸一化概率圖

3.2 離散尋優

已知室內AGV的工作最大線速度linear_v和最大角速度ang_v，再已知雷達頻率為10 Hz，那么在t-1時刻到t時刻，AGV單向移動距離不超過0.1*linear_v，旋轉角度不超過0.1*ang_v，那么離散尋優的窗口范圍應該是以t-1時刻機器人的位姿POSEt-1為中心，以0.2*linear_v寬度的方形窗口搜素位置，以0.2*ang_v角度范圍搜索姿態角，搜索范圍如圖4所示。

圖4 搜索窗口范圍和角度

已知柵格分辨率下，雷達旋轉角度步進的最高分辨率是激光掃描的最遠點旋轉一個地圖柵格尺寸的角度，那么由余弦定理可得：

(4)

其中，res是所建柵格地圖的分辨率，max是激光雷達掃描的最遠點距離。

如果暴力搜索所有位姿，那么可得匹配位姿候選項數目:

(5)

假設AGV雷達一幀最遠點距離為30 m，所建地圖分辨率為0.05 m，機器人最大速度1 m/s，旋轉最大速度180 °/s。如上公式可得角度候選項數目達400，位置候選項25，總候選的離散位姿數目達到10 000，匹配會耗費大量時間。

由于匹配方法使用二元正態分布打分，在最佳匹配角度上得分最高，離真實角度越遠，二元正態分布打分就越低。因此在找最優角度時，使用如圖5方法，在每一個柵格內先使用較大的搜索角度步進值ang_step，插值尋找最優角度angle_n，然后縮小搜索范圍，同時縮小角度步進值。最后一層使用公式(4)得到的最小分辨率角度遍歷，得到最優角度。

圖5 多分辨率搜索

概率地圖使用雙三次插值計算概率，再考慮到激光點的跳動偏差，角度步進的最低分辨率可跨過多個地圖柵格，在此設為grid_a。那么由公式(4)可得:

(6)

設定每層縮小搜索范圍為shrr倍，角度步進值縮小shra倍,那么總角度候選項為：

(7)

此方法在保證精度和穩定性的前提下，匹配次數大大減少，提高了匹配速度。

4 自由概率地圖

4.1 自由概率地圖表示與更新

概率地圖的柵格有三種狀態：未知，占據和自由。常用概率柵格地圖未知概率為0.5，大于0.5表示柵格為占據狀態，小于0.5表示為自由狀態[8-9]。

本方法使用uint8的整形值(0～255)作為柵格的自由值，使用整形數據避免了效率低下的浮點型運算，提高了地圖更新的效率。那么未知概率0.5對應的是27=128,大于該值的柵格認為自由，小于該值的柵格認為占據。在建圖前，只需計算一次0～1的浮點概率對應的0～255整形值，后續不用計算只需查表即可。

自由概率地圖數據包含了整個地圖的自由概率值free_value、用柵格數表示的地圖寬width和高height、世界坐標原點在地圖坐標系中的坐標origin、地圖柵格的分辨率resolution。

地圖的更新方法如下，首先計算地圖柵格優勢概率：

(8)

設定柵格測量占據概率為pocc，對應優勢概率為oddsocc。

設定柵格測量自由概率為pfree，對應優勢概率為oddsfree。

那么任意一次地圖更新，柵格優勢概率計算公式如下，其中測量占據更新：

(9)

測量自由更新：

(10)

更新后的自由概率：

(11)

本方法通過如上公式預計算256個整形值更新后的自由值，在進行激光幀匹配和更新時只需要查表即可。

4.2 基于雙三次插值的自由概率分布

占用柵格地圖是離散的，不能求導，直接取柵格概率還會限制精度。因此對于任一激光點打在概率地圖中的概率要進行雙三次插值[10-12]。如圖6所示，將單個柵格概率看作是周圍16個柵格連續概率分布的一個樣本，此時在激光點位置取的概率更精準，也能對位姿求導，方便后期的位姿優化。

圖6 激光點概率插值

雙三次插值函數ω(p)如下所示：

(12)

其中設定p的浮點坐標為(r,c)，向下取整坐標為(row,col)，取小數部分坐標為(u,v)。

每行柵格的列坐標和行坐標分別為(1+v,v, 1-v,2-v)和(1+u,u,1-u,2-u)。

代入雙三次插值函數ω(p)，分別得到4個列權重ki和4個行權重kj。

(13)

每個柵格概率的權重為kij=ki×kj，由公式(14)得到p點插值概率M：

(14)

計算地圖概率相對于柵格坐標的導數?M/?p：

(15)

(16)

5 基于雙概率殘差的非線性優化

前面兩節找到的是離散最優位姿，給如下的非線性優化方法提供初值，來迭代找到真實最優位姿。非線性優化方法很多，本方法選用較實用的高斯牛頓GN方法去優化，迭代速度較快，全局收斂較為精準穩定[13-14]。具體計算過程如下：

首先確定非線性最小二乘目標函數，如下所示，包含二元正態分布概率函數和概率地圖殘差項：

(17)

其中，pi(x)表示機器人在位姿x(px,py,pθ)處，激光雷達掃描點S(sx,sy)對應的柵格坐標。

(18)

目標函數在位姿x處進行一階泰勒展開：其中，JF是二元正態分布函數F的雅各比矩陣；JM是雙三次插值概率地圖函數M的雅各比矩陣。

對Δx求導：

(19)

令H和fd為：

(20)

(21)

整理，化簡可得Δx:

Δx=H-1×fd

(22)

其中，H和fd中的雅各比矩陣JF和JM如下：

(23)

(24)

(25)

迭代求解Δx，當步進值小于設定閾值，迭代停止。

6 匹配和建圖實驗

6.1 實驗設備和參數

本算法的實驗運行環境是WIN10-VS2015，CPU為i5-4200M2.5GHZ。團隊使用德國倍加福R2000激光雷達，其掃描范圍360°，掃描半徑50 m，在權衡AGV工作速度和精度的前提下，雷達選用頻率10 Hz，采樣率8400的參數進行。實驗平臺和環境如圖7所示。

(a) 實驗平臺 (b) 實驗室環境圖7 實驗平臺和環境

6.2 基于離散尋優的多分辨率搜索實驗

由于位姿候選項的位置分布是基于周圍柵格的整形柵格坐標位置，AGV室內導航所需地圖分辨率為5 cm，誤差不會這么大，最佳匹配都是固定在一個柵格內部的不同角度，因此測試的是二元正態分布匹配的1000次角度標準差和時間均值，實驗結果如表1所示。

表1 二元正態分布匹配測試

由表1可知，暴力搜索的候選項數目為7007，多分辨率搜索3層的候選項總數為1519，約為暴力搜索的1/5。由于生成不同分辨率候選項要重新構建類對象，也要耗時間，因此最終匹配耗時約為暴力匹配的1/2，但也明顯提高了匹配速度。

表2是暴力搜索和多分辨率搜索原地匹配1000次的精度測試輸出結果，可以看出角度最大偏差和標準差基本一致，多分辨率搜索的時間明顯減少。

表2 二元正態分布匹配精度測試

6.3 基于非線性優化的精度測試

如圖8所示，SLAM算法是在WIN10-VS平臺下實驗效果圖，左邊是SLAM的同步定位和建圖界面，右邊的終端顯示了包含離散優化和非線性優化的匹配過程。圖9右側終端其中一次顯示離散優化用時20.051 m，輸出位姿(7.083,3.056,0.003 165)。非線性優化使用離散位姿作為初值，優化后位姿為(7.078,3.060,0.003 069)，用時11.573 ms，結合其余數據處理時間，總用時45.455 ms。匹配時間在雷達10 Hz頻率范圍內，因此本算法可實時進行建圖和定位，建圖過程如圖9所示，能看到地圖概率更新由暗到亮的過程，越亮表示空間越自由。

圖8 WIN10-VS平臺下SLAM算法效果

(a) 位姿1 (b) 位姿2

(c) 位姿3 (d) 位姿4圖9 建圖過程的地圖更新

如表3所示，對本算法和另外兩種算法進行匹配1000實驗，程序均不使用序列化加速，算法測試平臺保持一致。結果表明本算法精度相比Hector有明顯提升，和Cartographer精度基本一致，但是本方法的平均耗時約47.9 ms，相比后兩者有明顯提升。

表3 算法的定位精度測量

本算法的位置最大偏差為7.072 11 mm，標準差σ為2.329 87 mm，精度高于主流的激光SLAM算法[3,4]。3σ的偏差為6.989 6 mm，都在1 cm內，滿足AGV的工業精度要求。

7 結束語

本方法首先從激光雷達獲取激光幀，對激光幀進行濾波處理，由第一幀激光幀初始化自由概率地圖；其次將t-1時刻激光幀進行二元正態分布化，由t時刻激光幀在t-1時刻激光幀的相關區間進行投影和概率打分，找到離散最優解；之后由二元正態分布概率密度函數和雙三次插值的自由概率殘差構建目標代價函數，在離散最優解位置進行高斯牛頓迭代優化，得到最優位姿；最后將t時刻激光幀投影到地圖中的最優解位姿，進行地圖更新。實驗測試結果表明，該算法匹配位姿精度高，建圖效果好。在10 Hz激光雷達頻率下，匹配更新只耗時約50 ms,3σ的偏差在7 mm左右，能滿足AGV工業精度和實時性要求，該方法對AGV在實際工業環境的應用具有重要意義。