一種基于多規(guī)則約束的SLAM概率物體關(guān)聯(lián)方法研究?

張建博，陶 慶?，袁 亮,2

(1.新疆大學(xué) 機(jī)械工程學(xué)院，新疆 烏魯木齊 830017；2.北京化工大學(xué) 信息科學(xué)與技術(shù)學(xué)院，北京 100029)

0 引言

同時(shí)定位與地圖構(gòu)建（Simultaneous Localization And Mapping,SLAM）是未知環(huán)境下移動(dòng)機(jī)器人根據(jù)自身攜帶的傳感器進(jìn)行自主定位和環(huán)境地圖構(gòu)建，近年國(guó)內(nèi)外對(duì)此進(jìn)行了大量研究[1?3].語(yǔ)義SLAM在機(jī)器人的人機(jī)交互、任務(wù)級(jí)導(dǎo)航以及路標(biāo)級(jí)定位方面起著至關(guān)重要的作用，而其最關(guān)鍵的一環(huán)是相機(jī)連續(xù)幀中觀測(cè)的語(yǔ)義信息與真實(shí)物理世界中物體的關(guān)聯(lián)問題，由于觀測(cè)噪聲以及假陽(yáng)性問題會(huì)造成物體關(guān)聯(lián)的模糊，從而嚴(yán)重影響SLAM的定位精度.經(jīng)典視覺SLAM分為前端跟蹤和后端優(yōu)化，目前的物體關(guān)聯(lián)方法分為基于前端和基于后端兩種方法.Bowman等人基于高斯假設(shè)，采用期望最大化（Expectation-Maximization,EM）算法進(jìn)行迭代優(yōu)化[4].而Doherty等人將物體關(guān)聯(lián)看作是非高斯推斷問題，并在因子圖的基礎(chǔ)上增加語(yǔ)義因子進(jìn)行優(yōu)化[5?6].他們都在SLAM的后端優(yōu)化部分對(duì)物體約束進(jìn)行了聯(lián)合優(yōu)化，但在大規(guī)模SLAM中，計(jì)算成本將會(huì)非常高，且仍然很難解決物體關(guān)聯(lián)模糊的問題.

Yang等人在線將物體建模為立體框，利用物體檢測(cè)框中檢測(cè)的共視特征點(diǎn)的數(shù)量進(jìn)行物體關(guān)聯(lián)[7].Wu等人結(jié)合文獻(xiàn)[7-8]中對(duì)物體的表示，并采用一種集成的物體關(guān)聯(lián)策略[9].上述工作在大部分情況下是輕量和魯棒的，但由于本質(zhì)是基于特征點(diǎn)的關(guān)聯(lián)，而在特征點(diǎn)較少或物體間有相互遮擋的環(huán)境下很容易發(fā)生關(guān)聯(lián)模糊.Ran等人提出了多假設(shè)狄利克雷過程的非參方法進(jìn)行物體關(guān)聯(lián)[10].Zhang等人提出用非參概率模型進(jìn)行物體關(guān)聯(lián)[11]，但仍未考慮多種物體類別以及假陽(yáng)性觀測(cè)的問題.

針對(duì)上述問題，本文提出一種基于多規(guī)則約束的概率物體關(guān)聯(lián)方法.該方法結(jié)合德羅內(nèi)克函數(shù)進(jìn)行多物體的語(yǔ)義類別約束，增加了可關(guān)聯(lián)物體的語(yǔ)義種類，同時(shí)分別對(duì)物體的歐式距離、尺寸、朝向以及共視特征點(diǎn)數(shù)量進(jìn)行概率建模.然后選擇最大關(guān)聯(lián)概率進(jìn)行觀測(cè)與路標(biāo)的物體關(guān)聯(lián)，減少由于單一關(guān)聯(lián)規(guī)則造成的關(guān)聯(lián)模糊問題.最后將概率物體關(guān)聯(lián)引入語(yǔ)義SLAM系統(tǒng)，并在后端進(jìn)行聯(lián)合優(yōu)化，從而提高SLAM的定位精度.

1 語(yǔ)義SLAM物體關(guān)聯(lián)

1.1 語(yǔ)義SLAM

語(yǔ)義SLAM可以描述為根據(jù)傳感器觀測(cè)的物體信息推斷移動(dòng)機(jī)器人的位姿以及周圍環(huán)境中語(yǔ)義物體的位置.相比傳統(tǒng)基于特征點(diǎn)、線或面的SLAM系統(tǒng)，語(yǔ)義SLAM能夠根據(jù)物體的多種屬性提供更準(zhǔn)確的物體關(guān)聯(lián)從而提高定位精度.在未知環(huán)境中，搭載的傳感器隨著機(jī)器人的運(yùn)動(dòng)，觀測(cè)到一系列路標(biāo)Z(0:M)={z0,z1,···,zm}，最終的目標(biāo)是推斷機(jī)器人位姿X(0:T)={x0,x1,···,xt}和物體的位置L(0:N)={l0,l1,···,ln}，而在觀測(cè)zi與路標(biāo)lj之間的對(duì)應(yīng)關(guān)系D就是物體關(guān)聯(lián)要解決的問題.所以SLAM本質(zhì)上可以轉(zhuǎn)換為如下概率推斷問題：

1.2 多規(guī)則約束

基于視覺的圖像包含豐富的語(yǔ)義信息，其中包括物體類別屬性、大小尺寸以及朝向.這些屬性在物體關(guān)聯(lián)中可以起到至關(guān)重要的作用，而傳統(tǒng)物體關(guān)聯(lián)都未曾考慮，這在具有物體遮擋等場(chǎng)景下會(huì)出現(xiàn)關(guān)聯(lián)模糊問題，從而影響視覺SLAM的定位精度.本文利用物體的多種屬性規(guī)則建模，對(duì)物體關(guān)聯(lián)進(jìn)行約束.

采用文獻(xiàn)[7]的3D立體框表示物體，每個(gè)物體由9個(gè)參數(shù)組成，O={T,d}，其中T=[R,t]∈SE(3)表示物體的6自由度位姿，d∈R3表示立體框的三維長(zhǎng)度.本文建立五種規(guī)則進(jìn)行魯棒的物體關(guān)聯(lián)，分別為物體框的語(yǔ)義類別規(guī)則、歐氏距離規(guī)則、大小尺寸規(guī)則、朝向角度規(guī)則和包含特征點(diǎn)規(guī)則.

物體框的語(yǔ)義類別規(guī)則規(guī)定只有相同的類別觀測(cè)才能進(jìn)行物體關(guān)聯(lián)，采用Kronecker delta函數(shù)建模：

式中：pc表示物體的觀測(cè)Oi關(guān)聯(lián)到路標(biāo)lj上的概率，本文采用YOLOv3[12]網(wǎng)絡(luò)進(jìn)行2D物體框的檢測(cè)，pr表示檢測(cè)的置信度.當(dāng)觀測(cè)類別與物體類別相同時(shí)，δij=1，語(yǔ)義關(guān)聯(lián)概率取決于物體觀測(cè)的置信度.而觀測(cè)類別與物體類別不是同類物體時(shí)，δij=0、pc=0，觀測(cè)不與該路標(biāo)進(jìn)行關(guān)聯(lián).

檢測(cè)與路標(biāo)的3D物體框的歐氏距離、大小尺寸以及朝向角度規(guī)則具有相似的規(guī)律，即二者的差值越小代表物體關(guān)聯(lián)的概率越高.首先定義誤差項(xiàng)：

式中：e?表示觀測(cè)與路標(biāo)的誤差.D?、S?以及A?分別表示物體的位置、大小和朝向角度.相應(yīng)的概率可以建模為負(fù)指數(shù)函數(shù)：

式中：P?分別表示距離概率Pd、尺寸概率Ps和朝向概率Pa；U是尺度因子.此概率模型為單調(diào)遞減的，即觀測(cè)與路標(biāo)差值越小，物體關(guān)聯(lián)的概率越大.

最后一項(xiàng)規(guī)則是包含特征點(diǎn)的屬性，由于每個(gè)物體上都有對(duì)應(yīng)特征點(diǎn)，傳統(tǒng)物體關(guān)聯(lián)方法就是根據(jù)觀測(cè)與路標(biāo)物體上包含的共視特征點(diǎn)數(shù)量進(jìn)行判斷[7].本文利用邏輯回歸模型對(duì)此規(guī)則進(jìn)行建模：

式中：a、b分別為形狀因子和閾值.這一規(guī)則的特征主要體現(xiàn)為：當(dāng)共視特征點(diǎn)個(gè)數(shù)x接近閾值b，關(guān)聯(lián)概率近似接近線性；當(dāng)個(gè)數(shù)少于拐點(diǎn)值，則關(guān)聯(lián)概率接近0，而當(dāng)大于穩(wěn)定值時(shí)對(duì)應(yīng)概率接近1，即保留關(guān)聯(lián)的模糊性而讓其它規(guī)則進(jìn)行判斷.

總的概率物體關(guān)聯(lián)模型如下：

由此獲得當(dāng)前觀測(cè)與各路標(biāo)之間的關(guān)聯(lián)概率，根據(jù)最大似然估計(jì)（Maximum Likelihood Estimation,MLE），選擇最大關(guān)聯(lián)概率進(jìn)行關(guān)聯(lián).

2 實(shí)驗(yàn)驗(yàn)證

本文對(duì)提出的方法在室外公共數(shù)據(jù)集KITTI[13]上進(jìn)行了對(duì)比驗(yàn)證.對(duì)比了ORB-SLAM2[14]和CubeSLAM[7]，根據(jù)單目視覺SLAM系統(tǒng)，我們?cè)O(shè)定輸入為環(huán)境的圖像信息，并保持前端跟蹤、后端優(yōu)化與CubeSLAM一致，將物體關(guān)聯(lián)部分的算法作為唯一的變量，用以驗(yàn)證物體關(guān)聯(lián)算法，從而使SLAM的定位精度可以反映物體關(guān)聯(lián)的性能.

實(shí)驗(yàn)平臺(tái)是Intel i5-7500 CPU、3.4 GHz、NVIDIA GeForce GT 1030顯卡.為驗(yàn)證大范圍場(chǎng)景下物體關(guān)聯(lián)的性能，在實(shí)驗(yàn)中選擇公共數(shù)據(jù)集KITTI進(jìn)行驗(yàn)證實(shí)驗(yàn)，KITTI數(shù)據(jù)集包含了室外場(chǎng)景下的城鎮(zhèn)和鄉(xiāng)村、長(zhǎng)距離和短距離等豐富的場(chǎng)景，可以較好地驗(yàn)證系統(tǒng)的泛化性.此外，由于序列01、03和04僅包含極少的物體目標(biāo)，無(wú)法用來(lái)驗(yàn)證本文所提的物體關(guān)聯(lián)方法，故采用其它8個(gè)序列進(jìn)行驗(yàn)證.基于前人工作[7]，也基于水平面假設(shè)，故假設(shè)檢測(cè)物體垂直且相接于地面.

2.1 定量實(shí)驗(yàn)

實(shí)驗(yàn)計(jì)算了軌跡的均方根誤差（Root Mean Squared Error,RMSE）.對(duì)比了ORB-SLAM2[14]、CubeSLAM[7]和MHDP[10]，其中CubeSLAM和本文方法都是基于單目ORB-SLAM2系統(tǒng)，由于增加了物體約束，從而證明了物體級(jí)語(yǔ)義SLAM的精度高于傳統(tǒng)基于特征點(diǎn)的SLAM方法.在實(shí)驗(yàn)中，對(duì)四種算法都關(guān)閉了回環(huán)檢測(cè)功能，以驗(yàn)證僅依靠物體級(jí)定位的精度.

表1展示了KITTI數(shù)據(jù)集中序列00、02、05、06、07、08、09和10上的均方根誤差.其中CubeSLAM和MHDP的結(jié)果來(lái)源于文獻(xiàn)[7]和文獻(xiàn)[10].由表1可知，語(yǔ)義SLAM相比傳統(tǒng)的基于特征點(diǎn)的SLAM定位精度提升了76.53%.這是由于物體級(jí)的語(yǔ)義信息具有更加魯棒的特性，為SLAM提供了更穩(wěn)定的約束，從而提高了定位精度.與CubeSLAM相比，由于本文算法加入了多規(guī)則約束的物體關(guān)聯(lián)方法，減少了物體關(guān)聯(lián)模糊的問題，所以定位精度提高了51.12%.與同類的概率物體關(guān)聯(lián)方法MHDP相比（其采用距離作為關(guān)聯(lián)規(guī)則），本文定位精度提高了49.52%.上述結(jié)果間接證明了物體關(guān)聯(lián)結(jié)果對(duì)語(yǔ)義SLAM具有顯著的影響.

表1 均方根誤差

2.2 定性實(shí)驗(yàn)

圖1～圖4展示了本文方法的定性結(jié)果，各圖中左圖展示的是軌跡在x-z軸上的軌跡（y軸垂直于地面），中圖和右圖展示了連續(xù)圖像幀在x軸、y軸以及z軸上的位移誤差和旋轉(zhuǎn)誤差.從KITTI數(shù)據(jù)集的軌跡誤差、平移誤差以及旋轉(zhuǎn)誤差上可以看出，本文方法誤差更小、曲線更接近真實(shí)值.而沒有回環(huán)檢測(cè)的ORB-SLAM2具有大的累計(jì)漂移，語(yǔ)義SLAM中通過物體特征的約束，減少了累計(jì)誤差.但CubeSLAM的物體關(guān)聯(lián)方法在室外大范圍場(chǎng)景下魯棒性較差，而基于改進(jìn)的多規(guī)則約束的方法提高了物體關(guān)聯(lián)的準(zhǔn)確率，從而更接近真實(shí)值.

2.3 消融實(shí)驗(yàn)

針對(duì)本文提出的多規(guī)則約束模型，通過對(duì)比MHDP[10]并進(jìn)行消融實(shí)驗(yàn)來(lái)驗(yàn)證不同規(guī)則對(duì)于整體系統(tǒng)的影響.表2展示了KITTI數(shù)據(jù)集中四個(gè)序列的消融實(shí)驗(yàn)結(jié)果，選擇兩個(gè)城鎮(zhèn)道路（00和07序列）以及兩個(gè)鄉(xiāng)村道路（09和10序列）作為代表，其中字母“S”“D”“O”和“P”分別表示語(yǔ)義類別、歐氏距離、尺寸朝向（合為一類）、共視特征點(diǎn)四種規(guī)則，“+”表示多規(guī)則的組合，即本文方法也可以看作“S+D+O+P”.由表2可知，“S+D+O”與“O+P”對(duì)定位精度的提升有更為明顯的影響，分別提高了49.42%和49.82%，從而反映出物體關(guān)聯(lián)結(jié)果更好.

表2 消融實(shí)驗(yàn)

通過消融實(shí)驗(yàn)可以得出如下結(jié)果：（1）多規(guī)則約束的效果高于較少或單規(guī)則約束的效果；（2）從單種規(guī)則的影響力上看，歐式距離規(guī)則“D”最高，包含特征點(diǎn)規(guī)則“P”次之，尺寸朝向規(guī)則“O”第三，最后是語(yǔ)義類別規(guī)則“S”；（3）各規(guī)則之間會(huì)產(chǎn)生相互影響，即會(huì)出現(xiàn)不滿足第一條結(jié)果的特殊情況，如“D”規(guī)則，造成這種問題的原因是由于“D”的約束性更強(qiáng)，應(yīng)該分配不同的權(quán)重，而權(quán)重的比例又來(lái)自最終的結(jié)果，即各規(guī)則之間無(wú)法獲得自適應(yīng)的權(quán)重.目前僅采用最簡(jiǎn)單的平均權(quán)重的方法，經(jīng)過大量的實(shí)驗(yàn)表明，雖然伴有少量的隨機(jī)性，但整體SLAM系統(tǒng)基本符合上述第一條和第二條結(jié)果.

3 結(jié)論

本文提出一種多規(guī)則約束的模型用于語(yǔ)義SLAM的物體關(guān)聯(lián)，通過物體的語(yǔ)義類別、大小尺寸、朝向角度以及共視特征點(diǎn)的數(shù)量約束物體關(guān)聯(lián)，克服了由于單一策略引起的物體關(guān)聯(lián)模糊問題，以提高語(yǔ)義SLAM的定位精度.通過在真實(shí)公共數(shù)據(jù)集上與ORB-SLAM2和CubeSLAM的對(duì)比，驗(yàn)證了語(yǔ)義SLAM定位精度高于傳統(tǒng)SLAM方法，同時(shí)也證明本文提出的基于多規(guī)則約束的物體關(guān)聯(lián)的準(zhǔn)確性，能夠有效提高物體級(jí)語(yǔ)義SLAM的定位精度.在未來(lái)的工作中，將考慮多規(guī)則之間的自適應(yīng)權(quán)重分配問題，以實(shí)現(xiàn)更魯棒和更高精度的物體關(guān)聯(lián).