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

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

0 引言

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

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

1 AGV激光SLAM算法框架

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

圖1 激光SLAM算法流程圖

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

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

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

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

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

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

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

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

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

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

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

(1)

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

(2)

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

(3)

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

圖3 二元正態(tài)分布?xì)w一化概率圖

3.2 離散尋優(yōu)

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

圖4 搜索窗口范圍和角度

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

(4)

其中，res是所建柵格地圖的分辨率，max是激光雷達(dá)掃描的最遠(yuǎn)點(diǎn)距離。

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

(5)

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

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

圖5 多分辨率搜索

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

(6)

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

(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ù)避免了效率低下的浮點(diǎn)型運(yùn)算，提高了地圖更新的效率。那么未知概率0.5對(duì)應(yīng)的是27=128,大于該值的柵格認(rèn)為自由，小于該值的柵格認(rèn)為占據(jù)。在建圖前，只需計(jì)算一次0～1的浮點(diǎn)概率對(duì)應(yīng)的0～255整形值，后續(xù)不用計(jì)算只需查表即可。

自由概率地圖數(shù)據(jù)包含了整個(gè)地圖的自由概率值free_value、用柵格數(shù)表示的地圖寬width和高h(yuǎn)eight、世界坐標(biāo)原點(diǎn)在地圖坐標(biāo)系中的坐標(biāo)origin、地圖柵格的分辨率resolution。

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

(8)

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

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

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

(9)

測(cè)量自由更新：

(10)

更新后的自由概率：

(11)

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

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

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

圖6 激光點(diǎn)概率插值

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

(12)

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

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

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

(13)

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

(14)

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

(15)

(16)

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

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

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

(17)

其中，pi(x)表示機(jī)器人在位姿x(px,py,pθ)處，激光雷達(dá)掃描點(diǎn)S(sx,sy)對(duì)應(yīng)的柵格坐標(biāo)。

(18)

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

對(duì)Δx求導(dǎo)：

(19)

令H和fd為：

(20)

(21)

整理，化簡(jiǎn)可得Δx:

Δx=H-1×fd

(22)

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

(23)

(24)

(25)

迭代求解Δx，當(dāng)步進(jìn)值小于設(shè)定閾值，迭代停止。

6 匹配和建圖實(shí)驗(yàn)

6.1 實(shí)驗(yàn)設(shè)備和參數(shù)

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

(a) 實(shí)驗(yàn)平臺(tái) (b) 實(shí)驗(yàn)室環(huán)境圖7 實(shí)驗(yàn)平臺(tái)和環(huán)境

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

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

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

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

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

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

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

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

圖8 WIN10-VS平臺(tái)下SLAM算法效果

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

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

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

表3 算法的定位精度測(cè)量

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

7 結(jié)束語(yǔ)

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