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

(1.哈爾濱工業(yè)大學(xué)蕪湖機器人產(chǎn)業(yè)技術(shù)研究院，安徽 蕪湖 241000； 2.蕪湖哈特機器人產(chǎn)業(yè)技術(shù)研究院有限公司，安徽 蕪湖 241000)

0 引言

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

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

1 AGV激光SLAM算法框架

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

圖1 激光SLAM算法流程圖

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

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

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

2 激光雷達數(shù)據(jù)預(yù)處理

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

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

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

3 二元正態(tài)分布化及離散尋優(yōu)

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

3.1 二元正態(tài)分布化激光點云

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

(1)

可得該柵格的協(xié)方差矩陣：

(2)

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

(3)

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

圖3 二元正態(tài)分布歸一化概率圖

3.2 離散尋優(yōu)

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

圖4 搜索窗口范圍和角度

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

(4)

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

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

(5)

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

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

圖5 多分辨率搜索

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

(6)

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

(7)

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

4 自由概率地圖

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

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

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

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

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

(8)

設(shè)定柵格測量占據(jù)概率為pocc，對應(yīng)優(yōu)勢概率為oddsocc。

設(shè)定柵格測量自由概率為pfree，對應(yīng)優(yōu)勢概率為oddsfree。

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

(9)

測量自由更新：

(10)

更新后的自由概率：

(11)

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

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

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

圖6 激光點概率插值

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

(12)

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

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

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

(13)

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

(14)

計算地圖概率相對于柵格坐標的導(dǎo)數(shù)?M/?p：

(15)

(16)

5 基于雙概率殘差的非線性優(yōu)化

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

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

(17)

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

(18)

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

對Δx求導(dǎo)：

(19)

令H和fd為：

(20)

(21)

整理，化簡可得Δx:

Δx=H-1×fd

(22)

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

(23)

(24)

(25)

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

6 匹配和建圖實驗

6.1 實驗設(shè)備和參數(shù)

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

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

6.2 基于離散尋優(yōu)的多分辨率搜索實驗

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

表1 二元正態(tài)分布匹配測試

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

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

表2 二元正態(tài)分布匹配精度測試

6.3 基于非線性優(yōu)化的精度測試

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

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

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

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

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

表3 算法的定位精度測量

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

7 結(jié)束語

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