基于概率推斷的語(yǔ)義SLAM數(shù)據(jù)關(guān)聯(lián)方法

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

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

0 引言

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

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

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

1 語(yǔ)義SLAM模型

1.1 語(yǔ)義視覺(jué)SLAM

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

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

(1)

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

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

(2)

(3)

(4)

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

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

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

(5)

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

dt,k～DP(α)

(6)

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

(7)

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

(8)

(9)

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

2.1 算法整體流程

針對(duì)語(yǔ)義路標(biāo)數(shù)據(jù)關(guān)聯(lián)中離散路標(biāo)估計(jì)和連續(xù)位姿優(yōu)化問(wèn)題，本文通過(guò)改進(jìn)概率推斷方法實(shí)現(xiàn)機(jī)器人語(yǔ)義路標(biāo)的數(shù)據(jù)關(guān)聯(lián)。根據(jù)狄利克雷先驗(yàn)和觀測(cè)似然得到目標(biāo)后驗(yàn)概率分布，在關(guān)聯(lián)時(shí)利用吉布斯方法進(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)算法

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

(10)

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

(11)

di,k=AML(Pi)。

(12)

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

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

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

1 begin

3 for每個(gè)吉布斯采樣步長(zhǎng)T

4 for 每個(gè)語(yǔ)義觀測(cè)，執(zhí)行

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

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

7 end for

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

9 end for。

3 實(shí)驗(yàn)

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

3.1 仿真實(shí)驗(yàn)及分析

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

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

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

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

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

3.2 KITTI數(shù)據(jù)集實(shí)驗(yàn)及分析

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

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

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

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

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

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

圖7 本文算法、DP和ORB SLAM2算法運(yùn)行軌跡

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

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