融合語(yǔ)義激光與地標(biāo)信息的SLAM技術(shù)研究

楊 爽，曾 碧，何煒婷

廣東工業(yè)大學(xué) 計(jì)算機(jī)學(xué)院，廣州 510006

1 引言

重定位是指由于外界因素的影響，移動(dòng)機(jī)器人會(huì)丟失其實(shí)際位置，需要重新定位以修正其錯(cuò)誤位姿，依賴即時(shí)定位與地圖構(gòu)建（SLAM）技術(shù)中定位算法的自適應(yīng)能力。目前在SLAM 的實(shí)際應(yīng)用中包含多種定位算法。比如Hanten[1]和Wang 等人[2]提出的自適應(yīng)蒙特卡羅定位算法，對(duì)傳統(tǒng)基于粒子濾波原理的蒙特卡羅定位算法[3]耗時(shí)較長(zhǎng)、應(yīng)用環(huán)境單一等缺陷進(jìn)行改進(jìn)，提高了它們?cè)诙ㄎ贿^(guò)程中的自適應(yīng)能力。但其對(duì)里程計(jì)（記錄速度、位移等運(yùn)動(dòng)量的相關(guān)方法）信息依賴程度過(guò)高，觀測(cè)靈敏度也不夠。在變化頻繁的動(dòng)態(tài)環(huán)境中，碰到位置漂移，機(jī)器人定位被劫持這些意外情況時(shí)，該類算法的容錯(cuò)性并不高，定位仍存在問(wèn)題。文獻(xiàn)[4-5]則是針對(duì)SLAM中的掃描匹配過(guò)程進(jìn)行了改進(jìn)，不僅減少了觀測(cè)匹配和位姿估計(jì)的時(shí)間，還降低了算法對(duì)環(huán)境特征的依賴，但由于其基礎(chǔ)算法本身易受環(huán)境因素的影響，在特征稀少的動(dòng)態(tài)場(chǎng)景中，重定位效果依然不好。

目前基于深度圖像信息的視覺(jué)SLAM 方法發(fā)展迅速，見(jiàn)文獻(xiàn)[6]。本文依賴地標(biāo)物體準(zhǔn)確的位置信息來(lái)進(jìn)行重定位，而在視覺(jué)SLAM算法[7]的研究應(yīng)用中，已經(jīng)有很多學(xué)者提出了把地標(biāo)作為參照物，用來(lái)修正機(jī)器人位姿的算法，如主流的ORB-SLAM[8]和LSD-SLAM[9]算法。除此之外，F(xiàn)rintrop等人在文獻(xiàn)[10]中提出的位姿圖優(yōu)化算法，以及地標(biāo)的自動(dòng)識(shí)別與配準(zhǔn)方法[11]，都是在區(qū)域內(nèi)人為使用（設(shè)定）少量的地標(biāo)，豐富該區(qū)域的特征信息進(jìn)行圖優(yōu)化，提高匹配對(duì)比的準(zhǔn)確性，達(dá)到優(yōu)化定位精度的目的。因?yàn)閷?duì)于這類算法，觀測(cè)信息越可靠，位姿估計(jì)也就更加準(zhǔn)確。

這些利用地標(biāo)信息的視覺(jué)SLAM 算法均能通過(guò)后端優(yōu)化來(lái)重新修正錯(cuò)誤位姿。一些方法[12-14]在此基礎(chǔ)上引入了IMU傳感器，進(jìn)行數(shù)據(jù)融合，把視覺(jué)和慣性測(cè)量信息緊密耦合在一起，就能進(jìn)一步提高定位精度，縮小偏差。但從長(zhǎng)遠(yuǎn)的趨勢(shì)上來(lái)看，機(jī)器人的位姿漂移現(xiàn)象仍然存在。所以張玉龍等人基于此在文獻(xiàn)[15]中提出了一種閉環(huán)檢測(cè)算法，用于修正位姿漂移，重新找回初始定位。Kasyanov等人也提出了一種KBV（IKey frame Based on Visual Inertia）算法[16]，在整個(gè)SLAM過(guò)程中，采用閉環(huán)檢測(cè)方法，在過(guò)去的關(guān)鍵幀中找到與當(dāng)前關(guān)鍵幀的最佳匹配，以此來(lái)優(yōu)化機(jī)器人的運(yùn)動(dòng)軌跡，不僅如此，在機(jī)器人丟失位置之后，也能通過(guò)這種關(guān)鍵幀匹配方法重新定位。該算法的重定位性能良好，但由于使用的視覺(jué)傳感器在獲取深度和角度信息時(shí)，易受光線、觀測(cè)方位等多方面因素影響，位姿修正結(jié)果往往不如人意。

2018 年，Zhang 等人[17]專門為環(huán)境信息中的地標(biāo)提出了一種概念——設(shè)立及識(shí)別自然地標(biāo)，并建立自然地標(biāo)庫(kù)。通過(guò)對(duì)地標(biāo)圖像進(jìn)行特征檢測(cè)與匹配，完成對(duì)自然地標(biāo)的準(zhǔn)確識(shí)別，從而達(dá)到提高機(jī)器人定位精度的目的。

機(jī)器人在室內(nèi)環(huán)境中導(dǎo)航，基于多傳感器數(shù)據(jù)融合的SLAM 技術(shù)[18]是目前較為熱門的研究方向。激光信息測(cè)距十分準(zhǔn)確，而視覺(jué)傳感器獲取的圖像信息則更為豐富，為了結(jié)合兩種數(shù)據(jù)的特性，本實(shí)驗(yàn)室曹軍等人在文獻(xiàn)[19]提出了一種將離散的激光點(diǎn)聚類成激光簇的方法（本文將該算法簡(jiǎn)稱為C-Clustering），用于加強(qiáng)環(huán)境特征，縮小了重定位的檢測(cè)范圍，在文獻(xiàn)[20]中提出了一種視覺(jué)信息與激光數(shù)據(jù)融合的方法（本文將該算法簡(jiǎn)稱為C-Fusion）。通過(guò)這兩種方法可以給激光數(shù)據(jù)賦予語(yǔ)義標(biāo)簽，因而本文利用此研究基礎(chǔ)，在SLAM 過(guò)程中，基于語(yǔ)義激光數(shù)據(jù)和地標(biāo)物體提出了一種重定位方法（Relocation based Semantic Laser and Landmark Information，RLSLALI），該方法借助激光感知與視覺(jué)識(shí)別融合后的語(yǔ)義激光信息，結(jié)合地圖環(huán)境中的地標(biāo)物體搭建地標(biāo)數(shù)據(jù)庫(kù)，建立位姿轉(zhuǎn)化模型，修正機(jī)器人錯(cuò)誤定位。在室內(nèi)動(dòng)態(tài)場(chǎng)景中，其定位健壯性高，可在標(biāo)準(zhǔn)硬件設(shè)備上正常運(yùn)行且不易受環(huán)境等因素影響。

2 一種基于語(yǔ)義激光與地標(biāo)信息的機(jī)器人重定位算法

人類在日常行車過(guò)程中，即使有全球定位工具，在經(jīng)過(guò)一些復(fù)雜、偏僻的路段時(shí)，依然容易迷失，往往需要借助路邊的指示牌或者標(biāo)志性建筑物來(lái)確認(rèn)自身所處位置。由此，本文提出一種利用語(yǔ)義激光及地標(biāo)信息進(jìn)行重定位的方法，用激光傳感器獲取距離、位置等定量信息，而分類、名稱等語(yǔ)義信息則使用視覺(jué)傳感器得到。這樣既避免了由于激光數(shù)據(jù)太相似導(dǎo)致的誤識(shí)別與誤匹配，又能減少光線、角度等影響視覺(jué)檢測(cè)的因素所帶來(lái)的數(shù)據(jù)誤差，適合室內(nèi)動(dòng)態(tài)環(huán)境。算法整體結(jié)構(gòu)見(jiàn)圖1。

圖1 重定位算法結(jié)構(gòu)圖

2.1 搭建地標(biāo)數(shù)據(jù)庫(kù)系統(tǒng)

2.1.1 云地標(biāo)庫(kù)的建立

本文之所以提出建立地標(biāo)數(shù)據(jù)庫(kù)，是因?yàn)椴⒉皇撬械奈矬w都能設(shè)為地標(biāo)，要依賴相關(guān)權(quán)限進(jìn)行授權(quán)。而云地標(biāo)庫(kù)就相當(dāng)于授權(quán)者、各機(jī)器人在不同的環(huán)境中，均可通過(guò)查找匹配云地標(biāo)庫(kù)中的數(shù)據(jù)內(nèi)容，初定當(dāng)前環(huán)境中的候選地標(biāo)，所以也可將云地標(biāo)庫(kù)稱作候選地標(biāo)庫(kù)。在構(gòu)建云地標(biāo)庫(kù)的過(guò)程中，最為關(guān)鍵的部分就是篩選條件的設(shè)定，用于判斷物體是否有成為地標(biāo)的資格，設(shè)定原則如下：

（1）物體的圖像特征要明顯，如色彩鮮艷、樣式豐富等，或自身就具有可辨識(shí)的標(biāo)志。

（2）物體形狀盡量扁平化，就能保證在多個(gè)角度識(shí)別到地標(biāo)時(shí)，也不會(huì)存在明顯的位置偏差；如果是形狀或位置會(huì)變化的物體，則拒絕，以此保證基礎(chǔ)數(shù)據(jù)的準(zhǔn)確性。

基于此，在本文的實(shí)驗(yàn)測(cè)試中，在云地標(biāo)庫(kù)中錄入了兩大類物體，一種是類似于高速公路上指示牌，一種是貼在墻壁上的壁畫(huà)。可將本文地標(biāo)數(shù)據(jù)庫(kù)系統(tǒng)表示為圖2。

圖2 地標(biāo)數(shù)據(jù)庫(kù)系統(tǒng)

2.1.2 有效地標(biāo)庫(kù)的建立

在建圖過(guò)程中，通過(guò)查找云地標(biāo)庫(kù)中的數(shù)據(jù)內(nèi)容，可在識(shí)別出的物體中篩選出候選地標(biāo)。但這樣還不夠，除云地標(biāo)庫(kù)外，還需建立多個(gè)有效地標(biāo)庫(kù)，見(jiàn)圖2。那是因?yàn)橥ㄟ^(guò)云地標(biāo)庫(kù)篩選出的候選地標(biāo)，很難達(dá)到算法的實(shí)際要求，需要針對(duì)每個(gè)運(yùn)行場(chǎng)景，單獨(dú)成庫(kù)，利用各候選地標(biāo)的相對(duì)位置關(guān)系進(jìn)行判斷，篩選出在當(dāng)前環(huán)境下可用的地標(biāo)物體。

相對(duì)位置關(guān)系的約束條件設(shè)置為：

（1）地標(biāo)的位置盡量挑選在不常訪問(wèn)的地方（靠墻或者角落等）。

（2）如果一個(gè)地圖環(huán)境中存在多個(gè)地標(biāo)，則各個(gè)地標(biāo)的位置不能太過(guò)稠密，要分散選擇，盡量做到地圖環(huán)境的全覆蓋。

這樣便可初步建立有效地標(biāo)庫(kù)，再結(jié)合2.2.1 小節(jié)融合視覺(jué)信息與激光數(shù)據(jù)的方法，獲取每個(gè)地標(biāo)物體在地圖上的位置坐標(biāo)，就能順利完成有效地標(biāo)庫(kù)的搭建，詳見(jiàn)2.2.2小節(jié)。

2.2 創(chuàng)建地標(biāo)名稱與位置坐標(biāo)的關(guān)聯(lián)映射表

建圖過(guò)程中，本文算法在融合激光感知與視覺(jué)識(shí)別的基礎(chǔ)上，根據(jù)地標(biāo)庫(kù)系統(tǒng)的約束條件，篩選并確定當(dāng)前環(huán)境中的可用地標(biāo)；在有效地標(biāo)庫(kù)中創(chuàng)建地標(biāo)名稱與全局位置坐標(biāo)的關(guān)聯(lián)映射表。對(duì)應(yīng)流程見(jiàn)圖3。

圖3 創(chuàng)建映射表流程

2.2.1 融合視覺(jué)信息與激光數(shù)據(jù)

首先通過(guò)C-Clustering 算法對(duì)激光數(shù)據(jù)進(jìn)行聚類，可得到每個(gè)激光掃描點(diǎn)的聚類標(biāo)簽，對(duì)應(yīng)每個(gè)掃描點(diǎn)的類別信息為{c1,c2,…,cn} ，由于激光雷達(dá)的工作原理是在極坐標(biāo)系下每隔一定角度（角度分辨率）θ發(fā)射和接收激光信息，獲得測(cè)量距離d，假設(shè)所使用的激光傳感器每幀測(cè)量數(shù)據(jù)包括了n個(gè)掃描點(diǎn)，則激光雷達(dá)數(shù)據(jù)可用公式（1）表示，L中每個(gè)元素 (θi,di)表示第i個(gè)激光掃描點(diǎn)在激光雷達(dá)極坐標(biāo)系下的角度和距離。

通常激光掃描點(diǎn)的信息可以轉(zhuǎn)換到直角坐標(biāo)系（2）下表示，D中每個(gè)元素表示第i個(gè)激光掃描點(diǎn)的笛卡爾坐標(biāo)，其中l(wèi) 代表local，是指以激光雷達(dá)中心為原點(diǎn)構(gòu)建的局部坐標(biāo)系。

接著根據(jù)C-Fusion 算法給聚類激光與物體語(yǔ)義進(jìn)行融合，賦予每個(gè)激光掃描點(diǎn)語(yǔ)義標(biāo)簽。同一類的激光數(shù)據(jù)可聚類成一個(gè)激光簇，對(duì)于每個(gè)激光簇j，將對(duì)應(yīng)物體的標(biāo)簽及標(biāo)簽概率賦予激光簇內(nèi)每個(gè)激光掃描點(diǎn)，所以激光掃描點(diǎn)可能存在多個(gè)標(biāo)簽及標(biāo)簽概率。最終聯(lián)合可得式（3）：

其中，Sj={(li,p′ij):p′ij ＞0}。li表示第i個(gè)物體的標(biāo)簽，p′ij表示第j個(gè)激光簇屬于視覺(jué)檢測(cè)結(jié)果中第i個(gè)物體的概率。

在此過(guò)程中，利用YOLOv3算法獲取地標(biāo)等物體的語(yǔ)義信息。通常情況下，當(dāng)運(yùn)行環(huán)境中存在較多符合云地標(biāo)庫(kù)約束的物體時(shí)，僅通過(guò)coco公開(kāi)數(shù)據(jù)集預(yù)訓(xùn)練好的模型便可完成物體的識(shí)別分類，實(shí)現(xiàn)成本相對(duì)較低。但是當(dāng)環(huán)境中的物體較少且具有特殊性，或者重復(fù)物體過(guò)多時(shí)，信息的獲取難度加大，此時(shí)視覺(jué)檢測(cè)模型的性能并不能滿足本文算法的需求。為了保證機(jī)器人在運(yùn)行過(guò)程中，能夠穩(wěn)定獲取準(zhǔn)確的基礎(chǔ)數(shù)據(jù)，首先要確保視覺(jué)識(shí)別的有效性。所以就需要針對(duì)當(dāng)前運(yùn)行環(huán)境，進(jìn)一步訓(xùn)練調(diào)優(yōu)視覺(jué)檢測(cè)模型。該部分的前期工作大致可分為三步。第一步是要添加當(dāng)前環(huán)境對(duì)應(yīng)的目標(biāo)檢測(cè)數(shù)據(jù)集，并對(duì)它們進(jìn)行數(shù)據(jù)增強(qiáng)。這一步尤其重要，首先在距離遠(yuǎn)近、背景簡(jiǎn)單與復(fù)雜、亮度高低這幾種條件下對(duì)環(huán)境中的物體實(shí)拍400（50×2×2×2）樣張圖片。緊接著使用工具程序?qū)@400張圖片進(jìn)行隨機(jī)類型（增強(qiáng)類型有調(diào)節(jié)飽和度、亮度、對(duì)比度、左右翻轉(zhuǎn)、旋轉(zhuǎn)0～30°等）的數(shù)據(jù)增強(qiáng)。對(duì)整個(gè)樣張集反復(fù)增強(qiáng)5次，得到包含2 000張圖片（400×5）的有效數(shù)據(jù)集，通過(guò)這些數(shù)據(jù)的訓(xùn)練，就能降低識(shí)別過(guò)程中隨機(jī)性的影響；第二步是結(jié)合實(shí)際環(huán)境中的物體，在視覺(jué)檢測(cè)模型中重新設(shè)定分類（以本文實(shí)驗(yàn)為例，雖然有多個(gè)指示牌，但是本文將其分為指示牌1 類、指示牌2 類等）；第三步則是要利用k-means函數(shù)根據(jù)第一步得到的數(shù)據(jù)集計(jì)算出9種類型的初始候選框尺寸，以此提高識(shí)別準(zhǔn)確率。然后自行確定檢測(cè)模型中幾個(gè)基本參數(shù)的初始值（學(xué)習(xí)率、衰退率等）。基于此，便可利用該有效數(shù)據(jù)集，繼續(xù)訓(xùn)練模型，達(dá)到算法適應(yīng)特殊環(huán)境的目的。而激光數(shù)據(jù)的獲取相對(duì)簡(jiǎn)單，通過(guò)激光雷達(dá)掃描周圍環(huán)境，并獲取環(huán)境中各物體的位置信息?；诖?，經(jīng)過(guò)以上方法，即可完成視覺(jué)檢測(cè)結(jié)果與激光雷達(dá)信息的融合，產(chǎn)生語(yǔ)義激光數(shù)據(jù)。語(yǔ)義激光數(shù)據(jù)Z中每個(gè)元素不僅包含原始激光掃描點(diǎn)位置信息，還包含聚類標(biāo)簽和物體標(biāo)簽及其對(duì)應(yīng)的標(biāo)簽概率，模擬運(yùn)行過(guò)程見(jiàn)圖4。

圖4 賦予地標(biāo)語(yǔ)義標(biāo)簽示意圖

2.2.2 設(shè)置有效地標(biāo)

采用以上融合方法可得到場(chǎng)景中物體的語(yǔ)義激光數(shù)據(jù)，因此本文利用該方法設(shè)置有效地標(biāo)（既能識(shí)別出地標(biāo)的分類，又能記錄其對(duì)應(yīng)的位置信息）。這樣機(jī)器人在后續(xù)的導(dǎo)航過(guò)程中，就可依賴準(zhǔn)確的地標(biāo)信息來(lái)修正自身位姿偏差。在2.2.1 小節(jié)的融合過(guò)程中，通過(guò)標(biāo)定參數(shù)使相機(jī)和激光雷達(dá)的探測(cè)范圍一致。激光聚類，可使識(shí)別出的每一個(gè)物體都對(duì)應(yīng)一束激光簇，一束激光簇中又包含多個(gè)激光數(shù)據(jù)點(diǎn)，對(duì)應(yīng)多個(gè)位置坐標(biāo)，而同一個(gè)物體又無(wú)法用多個(gè)坐標(biāo)定位，所以C-Fusion算法中有個(gè)特定的約束，選取激光簇中心激光點(diǎn)記錄的坐標(biāo)代表對(duì)應(yīng)物體的位置，視覺(jué)識(shí)別出的物體分類信息也就可以看做是其中心激光點(diǎn)的語(yǔ)義標(biāo)簽。所以利用式（3）和SLAM 技術(shù)中g(shù)mapping 建圖算法將各物體相對(duì)激光雷達(dá)的局部坐標(biāo)轉(zhuǎn)換成當(dāng)前地圖的全局坐標(biāo)。同時(shí)獲取物體的語(yǔ)義標(biāo)簽及標(biāo)簽概率，將每個(gè)物體的標(biāo)簽結(jié)合它的標(biāo)簽概率與2.1節(jié)建立的云地標(biāo)數(shù)據(jù)庫(kù)中的數(shù)據(jù)內(nèi)容匹配對(duì)比出候選地標(biāo)，再根據(jù)各候選地標(biāo)在運(yùn)行場(chǎng)景中的位置約束，自適應(yīng)地確定當(dāng)前環(huán)境中最終可使用的地標(biāo)物體，并存入有效地標(biāo)庫(kù)，作為用于修正錯(cuò)誤定位的有效地標(biāo)。所以在位姿轉(zhuǎn)換模型中，可將每個(gè)地標(biāo)物體看作一個(gè)點(diǎn)，一個(gè)語(yǔ)義名稱對(duì)應(yīng)一個(gè)坐標(biāo)點(diǎn)，聯(lián)合可推得下式：

其中，g 代表global，是指以世界地圖中心為原點(diǎn)構(gòu)建的全局坐標(biāo)系；mm表示當(dāng)前環(huán)境包含有效地標(biāo)的數(shù)量，表示第mm個(gè)地標(biāo)在全局笛卡爾坐標(biāo)系下的橫坐標(biāo)，表示其縱坐標(biāo)，Kmm表示第mm個(gè)地標(biāo)的語(yǔ)義名稱。

由此，可在有效地標(biāo)數(shù)據(jù)庫(kù)中通過(guò)式（4）創(chuàng)建地標(biāo)名稱和地標(biāo)全局位置坐標(biāo)的關(guān)聯(lián)映射表，見(jiàn)表1。

表1 地標(biāo)名稱與位置坐標(biāo)的關(guān)聯(lián)映射表

在后續(xù)修正定位的過(guò)程中，與此部分一致，機(jī)器人可能會(huì)從不同視角、方位觀測(cè)到地標(biāo)物體，但不論哪種情況，只要能識(shí)別出地標(biāo)的語(yǔ)音名稱即可，具體進(jìn)行位姿推導(dǎo)時(shí)，是將每個(gè)地標(biāo)物體當(dāng)作地圖上的一個(gè)點(diǎn)，獲取其全局位置和局部位置，不受機(jī)器人觀測(cè)位置和觀測(cè)角度的影響，滿足實(shí)際情形中可能會(huì)遇到的各類情況，詳見(jiàn)2.4節(jié)。

這部分的研究工作是為了在當(dāng)前構(gòu)建的地圖上設(shè)置地標(biāo)物體，搭建有效地標(biāo)庫(kù)，并給這些地標(biāo)賦予語(yǔ)義名稱，同時(shí)記錄其（其中心激光點(diǎn)）在地圖上的全局坐標(biāo)，創(chuàng)建地標(biāo)名稱-位置坐標(biāo)的關(guān)聯(lián)映射表，為位姿推導(dǎo)模型提供數(shù)據(jù)支持。傳統(tǒng)方法中，通常是人為設(shè)定地標(biāo)物體，不僅需要花費(fèi)大量的人力、時(shí)間成本去完成設(shè)置，還需要在算法中預(yù)先輸入地標(biāo)位置的定值、可變性差，前期準(zhǔn)備工作繁瑣。而通過(guò)本文方法搭建地標(biāo)庫(kù)，僅需要針對(duì)運(yùn)行環(huán)境，完成視覺(jué)檢測(cè)模型訓(xùn)練調(diào)優(yōu)的前期工作，機(jī)器人便可根據(jù)各類條件自主設(shè)定地標(biāo)，并將相關(guān)信息在算法中動(dòng)態(tài)更新，即使地標(biāo)發(fā)生改變，也能通過(guò)重新建圖自主更新地標(biāo)庫(kù)。不僅如此，所有使用者還能為云地標(biāo)庫(kù)提供負(fù)反饋，提交庫(kù)中缺少的地標(biāo)種類，讓本文算法自適應(yīng)各類實(shí)際運(yùn)行環(huán)境。

2.3 獲取各類數(shù)據(jù)

2.3.1 獲取地標(biāo)物體的全局坐標(biāo)

由上可知，完成地圖與有效地標(biāo)庫(kù)的建立工作后，利用式（4）創(chuàng)建的映射表可以直接得到地標(biāo)物體在地圖上的全局坐標(biāo)，但是在映射表中可能存在多個(gè)地標(biāo)物體，不能盲目地選擇一個(gè)用于求解，必須要讓機(jī)器人認(rèn)清周圍是哪個(gè)地標(biāo)，所以必須通過(guò)地標(biāo)名稱來(lái)篩選查找。

模擬人類用眼睛分辨物體信息，本文基于視覺(jué)傳感器，采用目前主流的深度學(xué)習(xí)方法進(jìn)行視覺(jué)識(shí)別，得到被識(shí)別物體的語(yǔ)義信息。比較各種物體檢測(cè)算法，發(fā)現(xiàn)它們?cè)趯?shí)時(shí)性和準(zhǔn)確率這兩方面不能同時(shí)滿足本文需求。由于機(jī)器人在導(dǎo)航時(shí)處于運(yùn)動(dòng)狀態(tài)，對(duì)運(yùn)算實(shí)時(shí)性要求較高，所以在此優(yōu)先考慮了YOLOv3-Tiny 檢測(cè)模型，經(jīng)過(guò)實(shí)驗(yàn)驗(yàn)證，在運(yùn)動(dòng)過(guò)程中，YOLOv3-Tiny雖然識(shí)別物體速度能達(dá)到本文需求，但正確率較低，特別是在復(fù)雜環(huán)境下識(shí)別較遠(yuǎn)位置的地標(biāo)時(shí)，檢測(cè)效果很不理想。

針對(duì)以上問(wèn)題，本文在YOLOv3-Tiny物體檢測(cè)模型研究的基礎(chǔ)上提出了Acc-YOLOv3（Accurate YOLOv3）算法。首先，由于YOLOv3-Tiny只有13×13特征尺寸圖（對(duì)應(yīng)候選框?yàn)椋?16×90），（156×198），（373×326））和26×26 特征尺寸圖（對(duì)應(yīng)候選框?yàn)椋?0×61），（62×45），（59×119））兩種特征圖尺寸。所以在其基礎(chǔ)上加上52×52特征尺寸圖（對(duì)應(yīng)候選框?yàn)椋?0，13），（16，30），（33，23）），來(lái)增加遠(yuǎn)距離小物體識(shí)別的正確率。原理見(jiàn)圖5，特征圖尺寸越小，對(duì)應(yīng)候選框越大，識(shí)別的是較大目標(biāo)；特征尺寸圖越大，對(duì)應(yīng)候選框越小，識(shí)別的是較小目標(biāo)。例如：圖5（a）與（b）是較小的特征尺寸3×3 和對(duì)應(yīng)較大的候選框；圖5（c）與（d）是較大的特征尺寸6×6 和對(duì)應(yīng)較小的候選框。所以，加上52×52 特征尺寸圖（對(duì)應(yīng)候選框?yàn)椋?0，13），（16，30），（33，23）），可以提高小目標(biāo)物體的識(shí)別正確率。

圖5 候選框選擇原理示意圖

加入了52×52 特征尺寸圖后，檢測(cè)速度基本不變，但準(zhǔn)確率并沒(méi)有達(dá)到期望效果，反而還有些許下降。分析整體架構(gòu)可知，其提取特征的基礎(chǔ)網(wǎng)絡(luò)層數(shù)太少太淺（一共15層），導(dǎo)致用來(lái)提取52×52特征尺寸圖候選框的網(wǎng)絡(luò)層太靠近輸入端，即候選框中采用的52×52特征尺寸圖所在的網(wǎng)絡(luò)層太淺（第7 層），提取的細(xì)粒度還不夠，特征還未被提取完就進(jìn)行識(shí)別，導(dǎo)致準(zhǔn)確率下降。

所以，參考Darknet19，進(jìn)一步把YOLO3-Tiny 的基礎(chǔ)網(wǎng)絡(luò)在其基礎(chǔ)上分別增加一層52×52和兩層26×26尺寸的卷積層，見(jiàn)圖6，加強(qiáng)特征提取。

圖6 增加卷積層示例

經(jīng)過(guò)實(shí)驗(yàn)測(cè)試，識(shí)別準(zhǔn)確率提升到85%以上，識(shí)別響應(yīng)時(shí)間基本沒(méi)受影響，只增加了0.01 s左右，效果可達(dá)到本文需求。

基于此，可獲取當(dāng)前運(yùn)行環(huán)境的各物體語(yǔ)義，再通過(guò)語(yǔ)義信息查找匹配有效地標(biāo)庫(kù)，則可得到地標(biāo)物體在世界地圖上的全局坐標(biāo)。

2.3.2 獲取地標(biāo)物體的局部坐標(biāo)

為保證數(shù)據(jù)來(lái)源的準(zhǔn)確性，本文RLSLALI 算法使用激光雷達(dá)獲取地標(biāo)的局部坐標(biāo)，避免視覺(jué)傳感器受光線等環(huán)境因素的影響產(chǎn)生偏差。將激光雷達(dá)與機(jī)器人的安裝位置統(tǒng)一，并且在算法中確定了它們的相對(duì)關(guān)系之后，激光雷達(dá)所建立的局部坐標(biāo)系就與機(jī)器人的基坐標(biāo)系保持一致。

以本文實(shí)驗(yàn)所用的rplidar-a2 激光雷達(dá)為例，雷達(dá)掃描激光數(shù)據(jù)點(diǎn)間隔設(shè)置為0.5°（分辨率），掃描范圍設(shè)置為正前方的180°，可掃描361個(gè)數(shù)據(jù)點(diǎn)。而雷達(dá)的測(cè)量數(shù)據(jù)點(diǎn)是按角度順序排列的，可知第i個(gè)地標(biāo)物體激光簇中心點(diǎn)對(duì)應(yīng)的第j個(gè)數(shù)據(jù)點(diǎn)在以雷達(dá)為中心的極坐標(biāo)系中的角度θj為：

見(jiàn)圖7，假定Ri是機(jī)器人附近的第i個(gè)地標(biāo)，對(duì)應(yīng)的第j個(gè)激光點(diǎn)，根據(jù)激光雷達(dá)測(cè)距及式（1）可知第i個(gè)地標(biāo)物體在局部坐標(biāo)系下的笛卡爾坐標(biāo)為：

圖7 以機(jī)器人為原點(diǎn)的局部坐標(biāo)系及其位置關(guān)系

2.3.3 獲取機(jī)器人的偏轉(zhuǎn)角

IMU（慣性測(cè)量單元）是一種記錄加速度、角速度等運(yùn)動(dòng)量信息的工具，其中包含的陀螺儀可以記錄機(jī)器人運(yùn)動(dòng)的轉(zhuǎn)動(dòng)速度及航向角，但是其值很容易受其他因素的影響，不同環(huán)境下的同一個(gè)陀螺儀測(cè)量值都相差甚大，所以必須針對(duì)當(dāng)前使用場(chǎng)景，對(duì)所使用的IMU進(jìn)行校正標(biāo)定，使其適配環(huán)境。標(biāo)定完成后，IMU 計(jì)算出來(lái)機(jī)器人偏轉(zhuǎn)角精度高，且健壯性好，因此本文采用IMU作為一種數(shù)據(jù)源來(lái)獲取機(jī)器人的偏轉(zhuǎn)角α。

2.4 創(chuàng)建位姿推導(dǎo)模型

2.4.1 構(gòu)建全局坐標(biāo)與局部坐標(biāo)的轉(zhuǎn)換關(guān)系

導(dǎo)航過(guò)程中，當(dāng)機(jī)器人位姿估計(jì)產(chǎn)生偏差時(shí)，本文RLSLALI算法的理念是以準(zhǔn)確的地標(biāo)信息推算機(jī)器人的實(shí)時(shí)位置，所以必須要在世界坐標(biāo)系中，建立地標(biāo)全局位置、地標(biāo)局部位置及機(jī)器人實(shí)時(shí)位置相互轉(zhuǎn)換的數(shù)學(xué)模型，圖8給出了全局坐標(biāo)系與局部坐標(biāo)系的相對(duì)關(guān)系以及其中的位置坐標(biāo)轉(zhuǎn)換。圖8 中O為世界地圖（全局坐標(biāo)系）的原點(diǎn)。O′為機(jī)器人的實(shí)時(shí)位置，以其為原點(diǎn)構(gòu)建一個(gè)局部坐標(biāo)系。其中位置關(guān)系轉(zhuǎn)換的計(jì)算信息如圖8中標(biāo)注，α為機(jī)器人相對(duì)全局坐標(biāo)系的偏轉(zhuǎn)角，機(jī)器人的實(shí)時(shí)位置坐標(biāo)設(shè)為(x0,y0)，通過(guò)轉(zhuǎn)換關(guān)系可推得：

其中，lx1,lx2,ly1,ly2可根據(jù)地標(biāo)R的局部坐標(biāo)以及角度α推導(dǎo)而得，可將轉(zhuǎn)換關(guān)系式整理為：

其中，地標(biāo)相對(duì)于全局坐標(biāo)系的位置坐標(biāo)，可根據(jù)視覺(jué)識(shí)別結(jié)果查找地標(biāo)名稱-位置坐標(biāo)映射表得到，地標(biāo)相對(duì)于局部坐標(biāo)系的位置坐標(biāo)，可根據(jù)式（6）、（7）推得。

圖8 相對(duì)坐標(biāo)系及各位置坐標(biāo)轉(zhuǎn)換模型

2.4.2 修正位姿偏差完成重定位

由2.4.1 小節(jié)中的式（10）及式（11）理論上可推算機(jī)器人當(dāng)前位姿，圖8 給出的是第一象限的坐標(biāo)轉(zhuǎn)換關(guān)系。但是由于實(shí)際情況的多樣性，比如考慮到機(jī)器人及地標(biāo)物體不可能只處于坐標(biāo)系的第一象限，機(jī)器人有可能運(yùn)動(dòng)到全局坐標(biāo)系的其他象限，地標(biāo)物體也有可能在機(jī)器人局部坐標(biāo)系的其他象限，所以分析各類場(chǎng)景中的不同情況，可排列組合出64種情況，對(duì)每種情況進(jìn)行推導(dǎo)驗(yàn)算，并完成多組實(shí)驗(yàn)測(cè)試與分析，可將機(jī)器人實(shí)際位置的推導(dǎo)式總結(jié)為：

通過(guò)本文提出的算法，根據(jù)多類傳感器數(shù)據(jù)及地標(biāo)物體信息，可以快速準(zhǔn)確地推算出機(jī)器人的實(shí)時(shí)位置坐標(biāo)(x0,y0)及它的航向角α，從而修正其位姿偏差，完成重定位。機(jī)器人修正其錯(cuò)誤定位的過(guò)程可見(jiàn)示意圖9。

綜上，本文算法只需要輸入視覺(jué)信息與激光數(shù)據(jù)，就能借助地標(biāo)物體，完成正確位姿的推導(dǎo)，修正其錯(cuò)誤定位。偽代碼如下：

圖9 機(jī)器人利用地標(biāo)修正位姿示意圖

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

3.1 實(shí)驗(yàn)平臺(tái)及實(shí)驗(yàn)環(huán)境的搭建

為了驗(yàn)證本文RLSLALI 算法的有效性及創(chuàng)新性，搭建了相應(yīng)的實(shí)驗(yàn)平臺(tái)及實(shí)驗(yàn)環(huán)境，見(jiàn)圖10。本文的實(shí)驗(yàn)平臺(tái)是由PC、相機(jī)、激光雷達(dá)、IMU 和移動(dòng)底座這幾部分組成。采用ROS 機(jī)制下的通信方式進(jìn)行數(shù)據(jù)傳輸，主要算法由PC決策運(yùn)行。

圖10 實(shí)驗(yàn)平臺(tái)及實(shí)驗(yàn)環(huán)境

其中相機(jī)為kinect v1，分辨率：640×480；PC的CPU為i7-8750H，顯卡為GeForce GTX 1060；移動(dòng)底座為Kobuki；IMU為L(zhǎng)3G4200D型號(hào)的3軸數(shù)字陀螺儀；激光雷達(dá)為RPLIDAR A2，測(cè)距分辨率可達(dá)0.5 mm。

為了更好地證明RLSLALI 算法的先進(jìn)性，驗(yàn)證其更好的定位效果，本文設(shè)置了一個(gè)室內(nèi)動(dòng)態(tài)場(chǎng)景作為實(shí)驗(yàn)環(huán)境：8×7 m2大小，多種障礙物隨機(jī)擺放，并且在運(yùn)行過(guò)程中有動(dòng)態(tài)人流變化；同時(shí)也會(huì)人為改變室內(nèi)光照強(qiáng)度和環(huán)境物體擺放的位置等等。

3.2 驗(yàn)證算法的定位性能

本文首先要對(duì)比文獻(xiàn)[2]、文獻(xiàn)[8]與文獻(xiàn)[16]中算法的定位性能，驗(yàn)證RLSLALI 算法是否具有有效性與先進(jìn)性，所以設(shè)計(jì)機(jī)器人在當(dāng)前環(huán)境自主導(dǎo)航6 h，重復(fù)航行軌跡12 次，測(cè)量72 個(gè)誤差值（每5 min 記錄一次誤差值），利用機(jī)器人在地圖上顯示的位置(x,y)與其實(shí)際位置 (x′,y′) 的歐式距離表示其誤差，在6 h的運(yùn)行過(guò)程中，各算法的定位誤差隨時(shí)間的變化情況見(jiàn)圖11。計(jì)算誤差值的均值，用來(lái)說(shuō)明算法修正定位的精度；計(jì)算誤差值的方差，用來(lái)說(shuō)明算法在場(chǎng)景中各個(gè)位置和各時(shí)間段修正定位的穩(wěn)定性，得到表2的對(duì)比數(shù)據(jù)。圖12為各類算法定位性能對(duì)比。

3.3 “綁架機(jī)器人”后的算法重定位效果對(duì)比

為了進(jìn)一步評(píng)估本文算法在3.2節(jié)中的實(shí)驗(yàn)結(jié)果是否準(zhǔn)確可靠，本節(jié)將通過(guò)算法的重定位效果對(duì)其分析驗(yàn)證。在當(dāng)前已知的地圖環(huán)境中，人為“劫持”機(jī)器人一段距離，觀察機(jī)器人在地圖中能否定位到新的位置，及時(shí)更新自己的實(shí)時(shí)位姿。所以本文就人為模擬了“機(jī)器人綁架”情況，在SLAM 過(guò)程中，把機(jī)器人騰空提起，直接搬離至另一個(gè)位置，對(duì)比RLSLALI 算法與其他算法的重定位效果。

圖11 運(yùn)行過(guò)程中各算法的定位誤差變化

表2 各類算法定位性能對(duì)比表

圖12 各類算法定位性能對(duì)比

圖13 及圖14 是環(huán)境特征較為明顯時(shí)文獻(xiàn)[2]中AMCL定位算法與本文RLSLALI算法的重定位效果對(duì)比，其中黑色和灰色所圍成的部分是當(dāng)前環(huán)境的地圖表示，黑色圓圈O表示的是當(dāng)前時(shí)刻機(jī)器人在地圖上顯示的位姿，也就是誤判位姿，而綠邊白色圓圈N 表示的是機(jī)器人在當(dāng)前環(huán)境的實(shí)際位姿。圖13是文獻(xiàn)[2]中AMCL算法的重定位效果，其中的圖13（a）代表t時(shí)刻人為把機(jī)器人從黑圓O 位置搬離到白圓位置，圖13（b）代表t+1 時(shí)刻機(jī)器人的位姿更新?tīng)顟B(tài)，在地圖上觀察到機(jī)器人還是在原來(lái)位置的周圍，并未進(jìn)行明顯的位姿更新，而圖13（c）則是t+2 時(shí)刻機(jī)器人左右各旋轉(zhuǎn)一圈后的位姿狀態(tài)，機(jī)器人在地圖上重定位到白圓位置N 周圍，也就是其實(shí)際位姿。而圖14是本文RLSLALI算法的重定位效果，其中的圖14（a）表示的是t時(shí)刻將機(jī)器人從當(dāng)前位置O 強(qiáng)行搬離到一個(gè)新的位置N，圖14（b）則代表在t+1 時(shí)刻時(shí)，機(jī)器人已經(jīng)在地圖上重定位到白圓N（實(shí)際位置）的附近。

圖13 環(huán)境多特征情況下AMCL算法的重定位效果

圖14 環(huán)境多特征情況下RLSLALI算法的重定位效果

進(jìn)行多組實(shí)驗(yàn)，結(jié)果表明，對(duì)比本文RLSLALI 算法，傳統(tǒng)的AMCL定位算法不僅耗時(shí)較長(zhǎng)，要求較高（需要機(jī)器人的其他觀測(cè)行為），成功率也不理想，在本文實(shí)驗(yàn)測(cè)試中僅為72%，而這還是在搬離到特征較為明顯的位置時(shí)所產(chǎn)生的結(jié)果，如果是將機(jī)器人搬離到一個(gè)環(huán)境特征低的位置（比如墻角，周圍墻壁環(huán)境類似等），傳統(tǒng)的AMCL定位算法就會(huì)幾乎無(wú)法完成重定位，而本文算法依然可以利用地標(biāo)物體實(shí)時(shí)修正位姿，不受其影響，實(shí)驗(yàn)測(cè)試效果見(jiàn)圖15 及圖16，AMCL 算法無(wú)法將機(jī)器人從黑圓位置O更新到白圓位置N，而使用本文RLSLALI算法卻能有效重定位。

圖15 環(huán)境低特征情況下AMCL算法的重定位效果

圖16 環(huán)境低特征情況下RLSLALI算法的重定位效果

不僅如此，還要將本文RLSLALI算法與視覺(jué)SLAM算法[8]的重定位效果進(jìn)行對(duì)比，見(jiàn)圖17及圖18。

圖17 ORB-SLAM算法的重定位效果

其中圖17表示視覺(jué)SLAM算法的重定位效果，圖17（a）代表SLAM過(guò)程中算法正在獲取當(dāng)前環(huán)境地圖特征點(diǎn)，圖17（b）是將機(jī)器人從藍(lán)邊倒三角位置O搬離到了綠邊倒三角N 的位置，而圖17（c）則是下一個(gè)時(shí)刻機(jī)器人在地圖上的進(jìn)行位置更新，重新定位到綠邊倒三角N位置的附近，并形成一定的位姿軌跡，準(zhǔn)確性較高。圖18則與之前的幾組實(shí)驗(yàn)相似，表示的是本文RLSLALI 算法將機(jī)器人在地圖上黑圓O 的位置順利更新到綠邊白圓N位置（實(shí)際位置）的周圍。

圖18 RLSLALI算法的重定位效果

3.4 物體檢測(cè)模型的識(shí)別效果

由于本文算法中所用的地標(biāo)全局坐標(biāo)是通過(guò)視覺(jué)識(shí)別地標(biāo)，查找匹配關(guān)聯(lián)映射表來(lái)獲取的，所以為了適配本文重定位算法對(duì)實(shí)時(shí)性和準(zhǔn)確率的需求，提出了Acc-YOLOv3 算法。為了驗(yàn)證本文Acc-YOLOv3 識(shí)別算法的可行性，分別在亮光條件（300以上lux）和暗光條件（200 以下lux）對(duì)應(yīng)的遠(yuǎn)近距離（1.5 m 與3.5 m）進(jìn)行實(shí)驗(yàn)驗(yàn)證，識(shí)別效果見(jiàn)圖19與圖20。

圖19 近距離物體檢測(cè)效果圖

圖20 遠(yuǎn)距離物體檢測(cè)效果圖

圖19 中的（a）是模擬暗光條件下（200 lux 以下）近距離對(duì)地標(biāo)物體（模擬指示牌）的識(shí)別結(jié)果；（b）是模擬亮光條件下（300 lux以上）近距離對(duì)地標(biāo)物體（模擬指示牌）的識(shí)別結(jié)果。將其框住并賦予語(yǔ)義標(biāo)簽。

圖20 中的（a）是模擬暗光條件下（200 lux 以下）遠(yuǎn)距離對(duì)地標(biāo)物體（模擬指示牌）的識(shí)別結(jié)果；（b）是模擬亮光條件下（300 lux以上）遠(yuǎn)距離對(duì)地標(biāo)物體（模擬指示牌）的識(shí)別結(jié)果。將其框住并賦予語(yǔ)義標(biāo)簽。

通過(guò)與其他識(shí)別算法在當(dāng)前實(shí)驗(yàn)場(chǎng)景中實(shí)驗(yàn)效果對(duì)比，可以得到表3的實(shí)驗(yàn)數(shù)據(jù)，性能對(duì)比見(jiàn)圖21。

表3 各類物體檢測(cè)算法實(shí)驗(yàn)結(jié)果對(duì)比表

圖21 各識(shí)別算法的性能對(duì)比

對(duì)比實(shí)驗(yàn)結(jié)果可知，雖然本文Acc-YOLOv3算法不能在檢測(cè)速度和準(zhǔn)確率上同時(shí)做到最佳，但卻可以在只多消耗一點(diǎn)時(shí)間的情況下大幅度提升檢測(cè)的準(zhǔn)確率，從而適配本文RLSLALI算法的運(yùn)行需求。

4 結(jié)論和展望

本文提出的重定位算法是在當(dāng)前主流SLAM 算法的基礎(chǔ)上，對(duì)它們的定位效果加以優(yōu)化和改進(jìn)。本文RLSLALI 算法利用視覺(jué)傳感器模擬人的眼睛，用來(lái)識(shí)別機(jī)器人周圍物體及地標(biāo)；利用激光和IMU 來(lái)獲取位置、距離、角度等準(zhǔn)確度要求較高的信息。巧妙地結(jié)合激光測(cè)距的準(zhǔn)確性與視覺(jué)信息的豐富性，用來(lái)構(gòu)建本文創(chuàng)新的地標(biāo)數(shù)據(jù)庫(kù)與位姿推導(dǎo)模型。使機(jī)器人能夠根據(jù)環(huán)境中的地標(biāo)信息，自主地完成重定位。從實(shí)驗(yàn)結(jié)果對(duì)比來(lái)看，本文算法取得了較好的重定位效果，算法修正每一次的錯(cuò)誤定位之后，總的定位偏差均值約為10.2 cm，方差約為10.1 cm，可見(jiàn)本文提出的基于語(yǔ)義激光與地標(biāo)信息的機(jī)器人重定位算法具有先進(jìn)性、有效性以及健壯性。

本文算法不僅是對(duì)目前SLAM 技術(shù)中定位方法的改進(jìn)與補(bǔ)充，也是對(duì)某些人為設(shè)置地標(biāo)優(yōu)化定位精度算法的加強(qiáng)與優(yōu)化。一般情況下，通過(guò)本文算法搭建的地標(biāo)數(shù)據(jù)庫(kù)系統(tǒng)，能夠讓機(jī)器人在運(yùn)行環(huán)境中自主篩選和設(shè)置有效地標(biāo)，無(wú)需為設(shè)置地標(biāo)做大量的前期繁雜工作，減少了人為干預(yù)步驟。當(dāng)然，本文算法也存在待改進(jìn)的地方，就是過(guò)于依賴自然地標(biāo)，如果環(huán)境中符合地標(biāo)條件的物體太少甚至沒(méi)有的話，就必須通過(guò)增加數(shù)據(jù)集對(duì)模型繼續(xù)訓(xùn)練調(diào)優(yōu)和設(shè)置人為地標(biāo)來(lái)實(shí)現(xiàn)算法，這也是本文作者以后繼續(xù)研究的一大重點(diǎn)。

致謝 感謝本實(shí)驗(yàn)室曹軍的前期研究工作，給本文RLSLALI算法研究提供了前期數(shù)據(jù)支持，在他的研究基礎(chǔ)上，本文順利地將地標(biāo)概念引入后端重定位算法中。