陳淵博，袁 亮，2，周德勤，于海群，何 麗

(1.新疆大學(xué)，烏魯木齊 830000； 2.北京化工大學(xué)，北京 100000； 3.北京煜邦電力技術(shù)股份有限公司，北京 100000)

0 引言

精確的定位和建圖是機(jī)器人自主執(zhí)行可靠導(dǎo)航任務(wù)的基礎(chǔ)[1]。傳統(tǒng)視覺SLAM(Simultaneous Localization and Mapping)在幾何特征不明顯或環(huán)境變化顯著情況下易匹配錯誤?；谡Z義信息的SLAM方法借助語義路標(biāo)實現(xiàn)穩(wěn)定的匹配和地圖構(gòu)建，近年來得到廣泛研究[2-4]。

圖像的采集是連續(xù)的，而語義路標(biāo)的提取是離散的，語義數(shù)據(jù)關(guān)聯(lián)實現(xiàn)環(huán)境一致性[5]。語義數(shù)據(jù)關(guān)聯(lián)的研究多基于低噪聲假設(shè)或靜態(tài)擾動，而真實場景中光照、陰影和運動物體的干擾會產(chǎn)生顯著的觀測噪聲和里程計噪聲，將導(dǎo)致對語義路標(biāo)的數(shù)據(jù)關(guān)聯(lián)產(chǎn)生較大的關(guān)聯(lián)錯誤，干擾機(jī)器人定位并使構(gòu)建的地圖失效[6-8]。

在較大噪聲干擾和語義路標(biāo)數(shù)量未知時，本文提出一種改進(jìn)的非參數(shù)推斷方法，使用YOLO檢測獲取語義信息[9]，結(jié)合MCMC (Markov Chain Monte Carlo)方法和狄利克雷過程模型進(jìn)行語義路標(biāo)的數(shù)據(jù)關(guān)聯(lián)，關(guān)聯(lián)結(jié)果幫助SLAM構(gòu)建環(huán)境的語義地圖[10]。針對大噪聲擾動下關(guān)聯(lián)模糊問題，該算法使用吉布斯方法對后驗概率分布迭代采樣，利用隨機(jī)近似推斷優(yōu)化語義路標(biāo)間的關(guān)聯(lián)約束，降低語義路標(biāo)間關(guān)聯(lián)模糊性。本文實驗在仿真和KITTI數(shù)據(jù)集上進(jìn)行[11]，結(jié)果表明，該方法有效提高了語義路標(biāo)關(guān)聯(lián)的準(zhǔn)確性，并實現(xiàn)了機(jī)器人的準(zhǔn)確定位。

1 語義SLAM模型

1.1 語義視覺SLAM

與幾何SLAM模型相比，語義視覺SLAM利用相機(jī)獲取的語義信息進(jìn)行位姿估計與優(yōu)化，相機(jī)的位姿信息用x表示，則集合XT表示機(jī)器人在全部時間步長T的軌跡，相機(jī)在t時間的運動方程為

xt=f(xt-1,ut)+eoeo～N(0,Q)

(1)

式中：ut為運動測量；eo為運動噪聲；Q為相機(jī)姿態(tài)噪聲矩陣。當(dāng)相機(jī)位姿為x時，觀測到語義路標(biāo)測量L和特征點測量y，em為觀測噪聲，R為觀測噪聲的協(xié)方差矩陣，則相機(jī)在t時刻的觀測方程為

zt=h(xt,yt,Lt)+emem～N(0,R)。

(2)

(3)

(4)

當(dāng)語義路標(biāo)數(shù)據(jù)關(guān)聯(lián)隨時間累加，相機(jī)的姿態(tài)和語義路標(biāo)位姿不斷更新。新的語義路標(biāo)被檢測時，多個類別和外觀相似的語義路標(biāo)受噪聲擾動會產(chǎn)生匹配誤差，造成數(shù)據(jù)關(guān)聯(lián)錯誤，使相機(jī)姿態(tài)、語義路標(biāo)姿態(tài)估計產(chǎn)生累積誤差，因此，提高語義路標(biāo)數(shù)據(jù)關(guān)聯(lián)的準(zhǔn)確性能夠有效提高機(jī)器人的定位精度。

1.2 語義數(shù)據(jù)關(guān)聯(lián)

機(jī)器人在環(huán)境中運動時，路標(biāo)類別和數(shù)量先驗未知，文獻(xiàn)[8]提出利用狄利克雷過程(DP)模型進(jìn)行非參數(shù)推斷。假定機(jī)器人在t時刻觀測到M個語義路標(biāo)，每個語義路標(biāo)測量Lt,k與語義路標(biāo)i關(guān)聯(lián)的先驗概率為

(5)

式中：mi為語義路標(biāo)i的已有關(guān)聯(lián)對象數(shù)；狄利克雷先驗α表示將觀測對象作為新語義路標(biāo)的概率參數(shù)。則語義路標(biāo)的先驗關(guān)聯(lián)概率服從狄利克雷分布，有

dt,k～DP(α)

(6)

在語義信息的獲取中，獲取的語義信息不是絕對可靠的，通過類別概率分布減小關(guān)聯(lián)誤差。對于語義觀測Lt,k，類別分布模型用π表示，類別分布的狄利克雷先驗參數(shù)為β，有語義路標(biāo)類別分布模型

(7)

πLt,k～Dir(βLt,k)。

(8)

(9)

2 語義數(shù)據(jù)關(guān)聯(lián)算法

2.1 算法整體流程

針對語義路標(biāo)數(shù)據(jù)關(guān)聯(lián)中離散路標(biāo)估計和連續(xù)位姿優(yōu)化問題，本文通過改進(jìn)概率推斷方法實現(xiàn)機(jī)器人語義路標(biāo)的數(shù)據(jù)關(guān)聯(lián)。根據(jù)狄利克雷先驗和觀測似然得到目標(biāo)后驗概率分布，在關(guān)聯(lián)時利用吉布斯方法進(jìn)行迭代優(yōu)化，得到最優(yōu)關(guān)聯(lián)目標(biāo)并優(yōu)化機(jī)器人位姿。概率推斷數(shù)據(jù)關(guān)聯(lián)整體算法流程如圖1所示。

圖1 整體算法流程

2.2 概率推斷數(shù)據(jù)關(guān)聯(lián)算法

語義路標(biāo)在低噪聲假設(shè)中的里程計測量和物體觀測誤差小，語義路標(biāo)關(guān)聯(lián)的后驗概率分布中存在唯一的最大似然目標(biāo)。真實環(huán)境中，里程計和圖像觀測噪聲擾動大，遮擋、重疊和光照變化使存在多個語義對象的后驗概率相近，關(guān)聯(lián)的模糊性增加。針對這一問題，本文提出利用吉布斯采樣原理對后驗概率分布進(jìn)行迭代處理，該算法通過構(gòu)造滿足遍歷理論的馬爾可夫鏈，對狀態(tài)空間中的變量x進(jìn)行隨機(jī)采樣，樣本分布隨累計趨于平滑，得到平穩(wěn)的目標(biāo)分布，樣本函數(shù)的均值趨于函數(shù)的期望值，即

(10)

式中,MC(p)表示滿足馬爾可夫原理的馬爾可夫鏈。語義路標(biāo)的概率數(shù)據(jù)關(guān)聯(lián)針對多目標(biāo)變量，吉布斯采樣方法通過固定其他維度信息對單維對象構(gòu)造馬爾可夫鏈，利用多次迭代得到逼近真實后驗概率中的最優(yōu)特征值，采樣原理為

(11)

di,k=AML(Pi)。

(12)

通過吉布斯方法進(jìn)行迭代采樣，優(yōu)先關(guān)聯(lián)模糊度小的語義對象，利用語義目標(biāo)關(guān)聯(lián)信息增加約束，降低模糊目標(biāo)錯誤關(guān)聯(lián)概率，得到趨近目標(biāo)的聯(lián)合分布和相對準(zhǔn)確的關(guān)聯(lián)，具體概率數(shù)據(jù)關(guān)聯(lián)算法如下。

Input：相機(jī)位姿X，相機(jī)觀測Z

Output：關(guān)聯(lián)狀態(tài)D，語義路標(biāo)數(shù)量M和語義路標(biāo)測量L(Lc,Lp)

1 begin

3 for每個吉布斯采樣步長T

4 for 每個語義觀測，執(zhí)行

5 式(9)得到后驗概率分布Pi

6 執(zhí)行式(12)得到近似關(guān)聯(lián)對象

7 end for

8 執(zhí)行式(4)和式(5)更新關(guān)聯(lián)狀態(tài)D和語義路標(biāo)測量L

9 end for。

3 實驗

本文實驗在PowerEdge R640上進(jìn)行，處理器為Intel?Xeon?Silver 4110 CPU @2.1 GHz，內(nèi)存為16GiB DDR4 RDIMM，實驗操作系統(tǒng)為Ubuntu 16.04。仿真數(shù)據(jù)集實驗中，使用文獻(xiàn)[8]中構(gòu)建的數(shù)據(jù)集并構(gòu)建新的具有高模糊度的數(shù)據(jù)集。在KITTI數(shù)據(jù)集實驗中，將提出的SLAM算法與DP算法、ORB SLAM2算法在道路場景下進(jìn)行實驗驗證。

3.1 仿真實驗及分析

仿真數(shù)據(jù)集包含15個屬于5類語義類別的路標(biāo)，初始1098個語義路標(biāo)隨機(jī)分布在二維平面內(nèi)。假定噪聲服從高斯分布，觀測噪聲為m～N(0,0.12)，里程計噪聲為o～N(0,0.022)時，本文算法和DP算法的數(shù)據(jù)關(guān)聯(lián)結(jié)果如圖2和圖3所示。圖2(e)表示環(huán)境真實位姿(Ground Truth,GT)，該實驗條件下提出的算法經(jīng)過3次迭代能夠收斂到15個語義路標(biāo)，如圖2(d)所示。DP方法平均需要4次，收斂結(jié)果如圖3(e)所示。通過圖2和圖3迭代結(jié)果和原始數(shù)據(jù)比較可得，本文算法在低噪聲擾動下有準(zhǔn)確的關(guān)聯(lián)和優(yōu)化結(jié)果。

圖2 本文算法仿真數(shù)據(jù)集數(shù)據(jù)關(guān)聯(lián)結(jié)果

圖3 DP算法仿真數(shù)據(jù)集數(shù)據(jù)關(guān)聯(lián)結(jié)果

為評估所提算法在相似場景下的性能，構(gòu)建含有多個相似語義路標(biāo)的數(shù)據(jù)集。此數(shù)據(jù)集包含15個屬于同一類別的語義路標(biāo)，相機(jī)軌跡和語義路標(biāo)位姿的真實分布如圖4(a)所示，圖4(b)為觀測噪聲m～N(0,0.12)和里程計噪聲o～N(0,0.022)情況下的初始觀測數(shù)據(jù)，包含937個語義路標(biāo)獲取。該數(shù)據(jù)集實驗中，本文算法能在兩次迭代后收斂到15個語義路標(biāo)，如圖4(d)所示。

圖4 本文算法在創(chuàng)建的數(shù)據(jù)集下的數(shù)據(jù)關(guān)聯(lián)結(jié)果

為驗證提出算法的魯棒性，設(shè)定多組里程計噪聲和觀測噪聲進(jìn)行對比實驗，9次迭代的結(jié)果如圖5所示。藍(lán)色實線是本文算法的迭代結(jié)果，紅色和黃色分別表示DP和HDP(Hierarchical Dirichlet Process)算法[13]得到的結(jié)果，綠色虛線表示語義路標(biāo)真實數(shù)量。

結(jié)果表明，提出的算法能更快地實現(xiàn)穩(wěn)定收斂。綜上所述，本文算法在SLAM語義數(shù)據(jù)關(guān)聯(lián)中具有較高的準(zhǔn)確性和魯棒性，在不同的噪聲擾動下關(guān)聯(lián)準(zhǔn)確性高。從仿真數(shù)據(jù)集實驗的關(guān)聯(lián)結(jié)果圖可以看出，語義路標(biāo)在數(shù)據(jù)關(guān)聯(lián)過程中優(yōu)化位姿，并幫助機(jī)器人提高定位精度。

3.2 KITTI數(shù)據(jù)集實驗及分析

在真實場景實驗中，使用KITTI中的單目數(shù)據(jù)進(jìn)行驗證。所選道路場景包含足夠的語義物體，場景中出現(xiàn)的車輛作為進(jìn)行數(shù)據(jù)關(guān)聯(lián)的語義路標(biāo)，使用YOLO檢測提取語義路標(biāo)信息，語義路標(biāo)檢測結(jié)果如圖6所示。

圖6 語義路標(biāo)檢測結(jié)果

KITTI數(shù)據(jù)集運行軌跡均方根誤差(RMSE)對比如表1所示。對比算法為基于DP算法的語義SLAM算法和基于幾何特征的ORB SLAM2算法，實驗結(jié)果使用絕對軌跡誤差(Absolute Trajectory Error，ATE)評估。其中,RMSE越小，說明運行的軌跡越接近真實軌跡，即機(jī)器人定位精度越高。

表1 KITTI數(shù)據(jù)集運行軌跡誤差對比Table 1 Comparison of trajectory errors in KITTI dataset

從表1結(jié)果可知，本文算法的均方根誤差在不同序列數(shù)據(jù)集上均有所提升，表明本文算法具有更高的軌跡一致性和定位精度。

在序列00，05，07和10中，存在足夠語義路標(biāo)實現(xiàn)數(shù)據(jù)關(guān)聯(lián)，準(zhǔn)確的關(guān)聯(lián)幫助機(jī)器人優(yōu)化定位。從表1可得，本文算法在序列00，05，07和10上對機(jī)器人運動估計的精度分別提高了10.77%，21.30%，10.53%和6.07%。軌跡估計結(jié)果如圖7所示。以上實驗結(jié)果表明本文算法的運行結(jié)果與真實軌跡一致性高，本文語義數(shù)據(jù)關(guān)聯(lián)方法消除了相機(jī)位姿偏差，提高了機(jī)器人定位精度。

圖7 本文算法、DP和ORB SLAM2算法運行軌跡

4 結(jié)束語

針對噪聲擾動下語義SLAM數(shù)據(jù)關(guān)聯(lián)模糊性問題，采用隨機(jī)近似推斷方法降低關(guān)聯(lián)的不確定性。本文提出的語義SLAM方法結(jié)合了Dirichlet方法和吉布斯采樣關(guān)聯(lián)語義路標(biāo)。靜態(tài)仿真環(huán)境中，該方法在不同擾動下能準(zhǔn)確關(guān)聯(lián)語義路標(biāo)。在KITTI數(shù)據(jù)集實驗中，該方法得到的關(guān)聯(lián)結(jié)果提高了機(jī)器人和語義路標(biāo)的定位精度。隨著語義信息的增加，語義約束條件增多，數(shù)據(jù)關(guān)聯(lián)的模糊性降低。當(dāng)機(jī)器人在真實場景中運動時，環(huán)境中包含多類移動語義對象，下一步工作將針對多類運動語義路標(biāo)提取展開，構(gòu)建用于導(dǎo)航的高精度語義地圖。