陳超 徐軍 張偉偉

摘? 要： 在移動機器人的同步定位與建圖（SLAM）研究中，使用單一的Kinect傳感器或激光傳感器時，出現(xiàn)建圖精度低、信息不完整、回環(huán)檢測易出錯、可靠性差等問題，為此提出一種方案，將這兩種傳感器進行融合，利用融合之后的數(shù)據(jù)來創(chuàng)建環(huán)境地圖。該方法首先將Kinect采集的深度圖轉(zhuǎn)換成偽激光數(shù)據(jù)后，與激光傳感器的數(shù)據(jù)進行融合，來構(gòu)建環(huán)境的局部柵格地圖，并利用融合后的數(shù)據(jù)進行激光回環(huán)檢測。當(dāng)檢測到有回環(huán)時，再利用Kinect的RGB圖的豐富的視覺信息進行視覺回環(huán)檢測。通過雙回環(huán)檢測來過濾掉錯誤的回環(huán)信息，提高全局地圖的精度和完整性。最后在機器人操作系統(tǒng)上進行試驗，結(jié)果表明該方案能顯著提高建圖的精度和完整性。

關(guān)鍵詞： 同步定位建圖; 移動機器人; 多傳感器融合; 環(huán)境地圖; 回環(huán)檢測; 系統(tǒng)試驗

中圖分類號： TN830?34; TP242.6? ? ? ? ? ? ? ? 文獻標(biāo)識碼： A? ? ? ? ? ? ? ? ? ? ? 文章編號： 1004?373X（2020）14?0164?06

Mobile robot simultaneous localization and mapping based on multi?sensor fusion

CHEN Chao， XU Jun， ZHANG Weiwei

（College of Mechanical Engineering， Jiangsu University of Science and Technology， Zhenjiang 212003， China）

Abstract： In the research of mobile robot simultaneous localization and mapping （SLAM）， there are some problems such as low accuracy of mapping， incomplete information， error loop closure and poor reliability when using a single Kinect sensor or laser sensor. A scheme is proposed to fuse these two sensors， and the environment map is constructed by means of the fusion data. In this method， the depth map acquired by means of the Kinect is converted into the pseudo laser data， which is fused with the data of the laser sensor， to create a local raster map of the environment and conduct the laser loopback detection with the fused data. When a loopback is detected， the visual loop detection will be performed by using the visual information of the Kinect′s RGB image. The double loop closure detection is used to filter out the wrong loop closure and improve the accuracy and integrity of the global map. The experiments on the robot operating system show that the method can significantly improve the accuracy and integrity of the map.

Keywords： simultaneous localization and mapping; mobile robot; multi?sensor fusion; environment map; loopback detection; system testing

0? 引? 言

隨著SLAM研究的不斷發(fā)展，各種傳感器由于其自身的特點被廣泛地應(yīng)用于不同的場景。其中以激光傳感器的研究和應(yīng)用最為成熟。近年來隨著計算機視覺技術(shù)的發(fā)展，視覺傳感器在SLAM研究中也得到了越來越廣泛的應(yīng)用。當(dāng)這兩種傳感器單獨使用時，創(chuàng)建的地圖精度和完整性難以滿足實際的要求。因此，采用多傳感器融合，充分利用這兩種傳感器的優(yōu)勢，獲取環(huán)境的冗余信息，提高系統(tǒng)的魯棒性和準(zhǔn)確度，是當(dāng)前SLAM研究的重要方向之一。

激光傳感器能非常準(zhǔn)確、快速地獲取環(huán)境信息，計算復(fù)雜度低，能夠很好地滿足實時性要求，被廣泛應(yīng)用于傳統(tǒng)的2D柵格地圖的創(chuàng)建之中[1?2]。但是由于激光所獲取環(huán)境的信息量極少，因此非常容易在回環(huán)上出錯。而回環(huán)一旦出錯，將會導(dǎo)致整個建圖失敗。因此，Wolfgang Hess等人提出對激光傳感器采集的數(shù)據(jù)進行處理之后，再進行回環(huán)檢測，以此提高回環(huán)的成功率[3]。但是當(dāng)環(huán)境中存在大量幾何形狀相似的物體時，該方法仍然容易出錯。

視覺傳感器主要有單目相機、雙目相機及深度相機。由于相機輕便價格便宜，而且能夠獲取豐富的環(huán)境信息，現(xiàn)在已經(jīng)被廣泛應(yīng)用到SLAM研究中。如Raul Mur?Artal的ORB?SLAM2[4]能夠分別使用單雙目及深度相機來創(chuàng)建環(huán)境的稀疏點云地圖。Felix Endres等人提出基于Kinect的RGB?D?SLAM[5]充分利用了深度相機的信息來創(chuàng)建環(huán)境地圖。但是視覺傳感器易受光照的影響，且精度低，計算復(fù)雜度高，實時性差。

針對單一傳感器的這些固有的缺陷和不足，文獻[6?9]提出了各種方案來對激光和視覺傳感器進行融合。融合之后，利用兩種傳感器的冗余信息來創(chuàng)建環(huán)境地圖。其結(jié)果表明使用多傳感器融合，能取得比單一傳感器更好的效果。但是，以上的各種融合方案中并未充分利用這兩種傳感器的有效信息。如：有的只使用了Kinect的深度信息，而舍棄了RGB信息;有的只使用Kinect的RGB圖來對深度圖的黑洞區(qū)域進行局部補充，而未充分利用RGB圖的其余信息[10]。

針對以上情況，本文提出一種新的方案來融合激光和Kinect傳感器進行環(huán)境地圖創(chuàng)建。該方案首先通過Kinect來獲取環(huán)境的深度圖和RGB圖，將深度圖轉(zhuǎn)換成激光格式的數(shù)據(jù)，與激光傳感器采集的數(shù)據(jù)進行融合，構(gòu)建環(huán)境的二維柵格地圖。然后在回環(huán)部分先利用融合后激光格式的數(shù)據(jù)來進行回環(huán)預(yù)檢。當(dāng)激光數(shù)據(jù)檢測到有回環(huán)存在時，會發(fā)送一個信號給視覺部分，然后利用豐富的視覺信息來進行視覺回環(huán)檢測，以確定是否存在回環(huán)。通過使用雙回環(huán)檢測來提高建圖的精度和完整性。最后通過實驗來對該方案的可行性和效果進行驗證。

1? SLAM問題描述

移動機器人SLAM是一個關(guān)于狀態(tài)估計的問題。機器人在不斷運動過程中，通過自身所攜帶的傳感器來獲取環(huán)境的觀測數(shù)據(jù)，然后通過觀測的數(shù)據(jù)來估計自身的位姿并不斷地迭代下去如圖1所示。由于傳感器通常是在一定的時間間隔內(nèi)采集數(shù)據(jù)，所以結(jié)果是把一段連續(xù)時間內(nèi)的運動變成了離散的各時刻的運動[11]。將其抽象成數(shù)學(xué)模型，總結(jié)如下：

[xk+1=f（xk，uk，wk）] （1）

[zk，j=g（xk，yj，vk，j）] （2）

用[xi]表示機器人在[i]時刻的位置，于是移動機器人在各時刻的位置就表示為[x1，x2，…，xi，…，xn]，它們一起組成機器人的運動軌跡。[yi]為移動機器人上相機所觀測到的第[i]個環(huán)境路標(biāo)，于是[y1，y2…，yi，…yN]構(gòu)成了環(huán)境的地圖。[zk，j]為相機在[xk]處觀測到路標(biāo)[yj]的觀測數(shù)據(jù)。[uk]為輸入控制數(shù)據(jù)，[wk]為位置噪聲，[vk，j]為觀測噪聲。

式（1）為運動模型，式（2）為觀測模型。函數(shù)[f，g]在不同的傳感器中具體有不同的形式，根據(jù)傳感器的不同，有著不同的參數(shù)化形式。這兩個方程描述了最基本的SLAM問題：已知控制輸入量[u]，及觀測的數(shù)據(jù)[z]時，如何求機器人的位置[x]，并構(gòu)建環(huán)境的地圖[y]。這樣就把SLAM問題建模成一個狀態(tài)估計問題，即如何通過帶有噪聲的測量數(shù)據(jù)，來估計機器人的狀態(tài)變量。

2? 多傳感器融合

2.1? 傳感器模型

激光傳感器如圖2所示。其型號為Hokuyo UST?10LX，測量精度為±4 mm，檢測視角為270°，有效測量距離可達8 m。

視覺傳感器為Kinect V1，如圖3所示。它是微軟公司開發(fā)的一款深度相機，能同時獲取環(huán)境的深度信息和彩色信息。其在水平方向視角為57°，豎直方向視角為43°，檢測范圍為0.8～4 m，幀率為30 f/s。

本文針對這兩種傳感器的優(yōu)缺點，對其進行融合。同時使用激光傳感器和Kinect傳感器，來獲得大的視角、豐富的環(huán)境信息和準(zhǔn)確的測量數(shù)據(jù)，如圖4所示。

2.2? 融合方案

將激光傳感器與Kinect傳感器采集的數(shù)據(jù)進行融合，來創(chuàng)建環(huán)境的二維柵格地圖。其融合方案見圖5。

2.3? Kinect深度圖轉(zhuǎn)換成激光數(shù)據(jù)

在開源機器人操作系統(tǒng)ROS（Robot Operating System）上，提供的用來創(chuàng)建環(huán)境二維柵格地圖的SLAM算法，只能接收一種激光掃描類型數(shù)據(jù)作為輸入。因此，Kinect傳感器所采集的數(shù)據(jù)必須先轉(zhuǎn)換成激光類型的數(shù)據(jù)，也稱作偽激光數(shù)據(jù)。然后再和激光傳感器所采集的數(shù)據(jù)融合成為一種數(shù)據(jù)，才能輸入到系統(tǒng)中進行處理、建圖[12]。Kinect采集的RGB圖和對應(yīng)的深度圖如圖6所示，其中深度圖的分辨率為320×240。然后將Kinect的深度圖轉(zhuǎn)換成激光數(shù)據(jù)的格式。在轉(zhuǎn)換之前需要先將深度圖進行壓縮處理，選取每列中距離成像平面最近的點為：

[ri=min（ri1，ri2，…，rij，…，ri240）] （3）

以[ri] 作為偽激光所檢測到距離Kinect最近的障礙物。從而實現(xiàn)既保留障礙物的高度信息，又有效地減少了數(shù)據(jù)量的大小。

2.4? 柵格地圖的創(chuàng)建與更新

Kinect和激光傳感器融合后的數(shù)據(jù)，以及創(chuàng)建的柵格地圖主要有三種狀態(tài)：空、占據(jù)、未知，每一種狀態(tài)都有對應(yīng)的區(qū)域。由于傳感器本身存在誤差，其測量并不準(zhǔn)確，常常帶有一定的噪聲。因此，只能計算傳感器前方給定位置處，存在障礙物的概率是多少。對于這種不確定性的環(huán)境，通過引入貝葉斯模型，用概率來對其進行描述，并用柵格地圖來表示。柵格地圖將環(huán)境劃分成一系列的網(wǎng)格，每個網(wǎng)格的取值在0～1之間。不同的取值代表著每個網(wǎng)格的狀態(tài)，0表示網(wǎng)格為空，1表示網(wǎng)格被占據(jù)，0.5表示網(wǎng)格狀態(tài)未知[13]。

貝葉斯公式被用于更新柵格地圖的狀態(tài)，提高建圖的精度。用[p（mij）]表示[（i，j）]位置處存在障礙物的概率。對于一個柵格點，[p（m=0）]表示該柵格為空，[p（m=1）]表示該柵格被占據(jù)，且

[p（m=0）=1-p（m=1）] （4）

將兩者的比值作為該點的狀態(tài)：

[Odd（m）=p（m=1）p（m=0）] （5）

對于一個網(wǎng)格點，現(xiàn)在有一個新的測量值[z∈{0，1}]，需要更新該點的狀態(tài)。未更新之前的狀態(tài)為[Odd（m）]，更新之后的狀態(tài)為：

[Odd（m|z）=p（m=1|z）p（m=0|z）] （6）

表示在有新的觀測值[z]之后，[m]的概率。然后根據(jù)貝葉斯公式得到測量之后的后驗概率為：

[p（m|z）=p（z|m）p（m）p（z）] （7）

代入到更新狀態(tài)方程中有：

[Odd（m|z）=p（m=1|z）p（m=0|z）=p（z|m=1）p（m=1）p（z）p（z|m=0）p（m=0）p（z）? ? ? ? ? ? ?=p（z|m=1）p（m=1）p（z|m=0）p（m=0）=p（z|m=1）p（z|m=0）Odd（m）] （8）

對式（8）兩邊同時取對數(shù)，得：

[log[Odd（m|z）]=logp（z|m=1）p（z|m=0）+log[Odd（m）]] （9）

式（9）中含有新的測量值的項只有[logp（z|m=1）p（z|m=0）]，將其記為[logmeans]，即：

[logmeans=logp（z|m=1）p（z|m=0）] （10）

式中[logmeans]只有兩種狀態(tài)，分別為：

[logfree=logp（z=0|m=1）p（z=0|m=0）] （11）

[logoccu=logp（z=1|m=1）p（z=1|m=0）] （12）

用[S+]來表示[log[Odd（m|z）]]，指通過測量值，更新后的狀態(tài);用[S-]來表示[log[Odd（m）]]，指更新前的狀態(tài)。則式（9）可以表示為：

[S+=logmeans+S-] （13）

即更新后的狀態(tài)值為測量的狀態(tài)值加上未更新前的狀態(tài)值。在沒有任何測量值的初始狀態(tài)下，柵格的狀態(tài)為：

[p（m=0）=p（m=1）=0.5] （14）

此時，

[S0=log[Odd（m）]=logp（m=1）p（m=0）? ? ?=log0.50.5=0] （15）

經(jīng)過上面的建模和轉(zhuǎn)化后，柵格地圖中任意一點的狀態(tài)都可以通過傳感器的測量值，進行加減來更新地圖的狀態(tài)。一個點的狀態(tài)值越大，就越有可能表示它是被占據(jù)的狀態(tài);值越小，就越有可能表示它是空的狀態(tài)。

2.5? 激光回環(huán)檢測

激光回環(huán)檢測主要有三種方法：

1） scan?to?scan，即當(dāng)前掃描數(shù)據(jù)幀與歷史掃描幀間的匹配。

2） scan?to?map，即當(dāng)前數(shù)據(jù)與歷史局部地圖之間的匹配，以確認回環(huán)。

3） map?to?map，即將當(dāng)前連續(xù)幀激光數(shù)據(jù)先構(gòu)建成子地圖，再和之前的子地圖進行匹配。

方法1）由于信息量太少，常常出現(xiàn)匹配錯誤;方法2）的匹配精度較高，且匹配速到快，實時性好;方法3）由于需要建立大量的子地圖，來進行相互匹配，雖然匹配準(zhǔn)確性高，但是計算量大，實時性差，且對硬件要求高。因此，本文的激光回環(huán)檢測將采用方法2），即scan?to?map的方式。

由于激光只能掃描到其安裝平面上的信息，在該平面上下的障礙物都會被其忽略。因此，激光掃描能獲取的環(huán)境信息極少。在回環(huán)檢測時，常常由于信息的不足而導(dǎo)致回環(huán)失敗。同時，激光只能獲取障礙物的幾何信息，而不能獲取障礙物表面的紋理信息。因此，當(dāng)環(huán)境中存在大量與幾何外形相似的障礙物時，激光將難以僅僅根據(jù)環(huán)境物體的形狀來判斷是否存在回環(huán)。

而在Kinect的深度圖轉(zhuǎn)換的偽激光數(shù)據(jù)中，包含障礙物的高度信息，其信息更加豐富。先用融合后的激光類型數(shù)據(jù)來構(gòu)建環(huán)境的子地圖，再利用以后時刻的融合數(shù)據(jù)與子地圖進行匹配，根據(jù)環(huán)境的幾何特征來初步判斷是否存在可能的回環(huán)，從而提高激光回環(huán)的成功率。當(dāng)激光檢測到有回環(huán)存在時，將會向視覺回環(huán)發(fā)出一個信號，來啟動視覺回環(huán)進行檢測。

2.6? 視覺回環(huán)檢測

視覺傳感器獲取的信息十分豐富，使用視覺進行回環(huán)檢測的成功率要遠遠高于激光的回環(huán)。因此，利用Kinect的RGB圖中含有豐富的環(huán)境信息，首先從RGB圖中選取關(guān)鍵幀，再從這些關(guān)鍵幀中提取特征點。本文提取的是ORB特征點[14]，如圖7所示。通過這些關(guān)鍵幀間的特征點進行回環(huán)檢測，來驗證激光所檢測到的回環(huán)是否存在，從而提高建圖的準(zhǔn)確性和全局一致性。

由于直接進行特征點的匹配會比較耗時，當(dāng)光照變化時，特征的描述也可能不穩(wěn)定，所以，為了減少系統(tǒng)的運算量，需要在選取的關(guān)鍵幀之間設(shè)定合理的間隔。同時，采用詞袋模型，即Bag?of?Words（Bow），通過離線訓(xùn)練來構(gòu)建環(huán)境的字典，加快ORB特征的匹配速度[15]。

在這里Kinect的視覺信息，即RGB圖，只是用來建立詞袋，用于視覺回環(huán)檢測使用，并不用于建立包含視覺信息的三維點云地圖。這樣能夠有效地避免視覺SLAM創(chuàng)建地圖時，需要進行的高復(fù)雜度計算，提高了系統(tǒng)的實時性和運行效率。

最后用回環(huán)檢測的信息來對柵格地圖進行校正、優(yōu)化，并創(chuàng)建出全局一致性的地圖。

3? 實? 驗

3.1? 實驗平臺及環(huán)境

本文采用的實驗平臺是改裝后的Turtlebot 2移動機器人，其搭載了微軟的Kinect V1傳感器、Hokuyo激光傳感器和一臺筆記本電腦，如圖8所示。電腦采用的是Ubuntu 14.04 LTS系統(tǒng)， 并在ROS indigo上運行開源SLAM算法Gmapping。

實驗環(huán)境如圖9所示，其為人工搭建室內(nèi)場景。場地尺寸大小為5 m×2.5 m。在實驗場地中設(shè)置如圖9中所示的障礙物，其中1號障礙物的高度高于激光雷達的安裝平面;2號、3號障礙物高度低于激光雷達的安裝平面，且2號、3號障礙物的幾何外形有著明顯的區(qū)別，便于激光回環(huán)檢測時的準(zhǔn)確識別。通過這些障礙物的設(shè)置來對本文所提出的融合方案的可行性和效果進行驗證與分析。

3.2? 實驗結(jié)果

首先只使用激光傳感器來對環(huán)境進行建圖，效果如圖10所示。從圖中可以看出，在激光所創(chuàng)建的二維柵格地圖中，比激光傳感器安裝位置低的障礙物2、障礙物3在圖中都未顯示;而比其安裝位置高的障礙物1則出現(xiàn)在地圖中。表明激光只能檢測到其安裝平面上的障礙物。

只使用Kinect傳感器所創(chuàng)建的環(huán)境地圖，如圖11所示，從圖中可以看出，Kinect傳感器能夠?qū)h(huán)境中，激光傳感器檢測不到的小障礙物2、障礙物3都檢測出來。這主要是由于Kinect傳感器在豎直方向上有著43°的視角，讓其檢測范圍不只限于水平面上。但是可以看出，Kinect傳感器所創(chuàng)建的柵格地圖的精度，明顯低于激光傳感器，存在很大誤差。這主要是因為，Kinect傳感器在水平方向上只有57°的視角。在小視角的情況下，Kinect傳感器將不能進行準(zhǔn)確地掃描匹配。同時，Kinect傳感器的分辨率也低于激光傳感器，從而導(dǎo)致Kinect傳感器所創(chuàng)建的二維柵格地圖，精度低于激光傳感器。

最后再同時使用激光傳感器和Kinect傳感器來構(gòu)建環(huán)境的地圖，其效果如圖12所示。從圖中可以看出，之前被激光所忽略掉的障礙物，被檢測出來，并且，創(chuàng)建地圖的精度和完整性相比于單一的傳感器，有很大的改善。

4? 結(jié)? 語

本文主要的改進在于，一是將Kinect的深度圖與激光的掃描數(shù)據(jù)進行充分融合后，來創(chuàng)建局部柵格地圖，使得地圖具有高度方向信息;二是利用融合后的激光類型數(shù)據(jù)，進行激光回環(huán)檢測，來快速預(yù)判是否存在回環(huán);三是充分利用Kinect的RGB信息來進行視覺回環(huán)檢測，對激光回環(huán)的結(jié)果進行確定，去除錯誤的回環(huán)信息。實驗結(jié)果證明，這些改進相對于單一傳感器的使用，能夠顯著地提高全局地圖的精度和完整性。

本文的雙回環(huán)檢測部分，是由激光回環(huán)的結(jié)果來啟動視覺的回環(huán)檢測。不能根據(jù)某一環(huán)境的特征，動態(tài)地使用對應(yīng)的信息進行回環(huán)檢測。因此，下一步主要改進方向是讓回環(huán)部分根據(jù)環(huán)境的不同，而采用對應(yīng)的檢測方式。如：當(dāng)環(huán)境中各處有明顯的幾何外形差異，且缺少紋理特征時，只使用激光來進行回環(huán)檢測;當(dāng)環(huán)境中存在大量的幾何外形相似，且有明顯的紋理特征時，只使用視覺來進行回環(huán)檢測。通過這種方式，來加速回環(huán)檢測，提高回環(huán)效率。

參考文獻

[1] LIANG X， CHEN H Y， LI Y J， et al. Visual laser?SLAM in large?scale indoor environments [C]// IEEE International Conference on Robotics & Biomimetics. Qingdao： IEEE， 2017： 134?137.

[2] WU Y， DING Z. Research on laser navigation mapping and path planning of tracked mobile robot based on hector SLAM [C]// International Conference on Intelligent Informatics & Biomedical Sciences. Bangkok： IEEE， 2018： 210?215.

[3] HESS W， KOHLER D， RAPP H， et al. Real?time loop closure in 2D LIDAR SLAM [C]// IEEE International Conference on Robotics and Automation. Stockholm： IEEE， 2016： 101?106.

[4] MUR?ARTAL R， TARDOS J D. ORB?SLAM2： an open?source SLAM system for monocular， stereo and RGB?D cameras [J]. IEEE transactions on robotics， 2017， 33（5）： 1255?1262.

[5] ENDRES F， HESS J， STURM J， et al. 3D mapping with an RGB?D camera [J]. IEEE transactions on robotics， 2017， 30（1）： 177?187.

[6] LIU Z X， XIE C X， XIE M， et al. Mobile robot positioning method based on multi?sensor information fusion laser SLAM [J]. Cluster computing， 2018， 22（2）： 55?61.

[7] KAMARUDIN K， MAMDUH S M， YEON A S A. Improving performance of 2D SLAM methods by complementing Kinect with laser scanner [C]// IEEE International Symposium on Robotics & Intelligent Sensors. Langkawi： IEEE， 2015： 47?61.

[8] YEON A S A， KAMARUDIN K， VISVANATHAN R， et al. Feasibility analysis of 2D?SLAM using combination of kinect and laser scanner [J]. Sciences & engineering， 2015， 76（12）： 9?15.

[9] TUBMAN R， POTGIETER J， ARIF K M. Efficient robotic SLAM by fusion of RatSLAM and RGBD?SLAM [C]// 2016 23rd International Conference on Mechatronics and Machine Vision in Practice. Nanjing： IEEE， 2016： 47?53.

[10] 公維思，周紹磊，吳修振，等.基于改進FAST特征檢測的ORB?SLAM方法[J].現(xiàn)代電子技術(shù)，2018，41（6）：53?56.

[11] 高翔，張濤.視覺SLAM十四講：從理論到實踐[M].北京：電子工業(yè)出版社，2017.

[12] CHEN M X， YANG S W， YI X D， et al. Real?time 3D mapping using a 2D laser scanner and IMU?aided visual SLAM [C]// IEEE International Conference on Real?time Computing & Robotics. Okinawa： IEEE， 2018： 106?118.

[13] FILATOV A， FILATOV A， KRINKIN K， et al. 2D SLAM quality evaluation methods [C]// Processing of the 21th Conference of Fruct Association. Petersburg： IEEE， 2017： 54?67.

[14] FANG W K， ZHANG Y J， YU B， et al. FPGA?based ORB feature extraction for real?time visual SLAM [C]// 2017 International Conference on Field Programmable Technology. Melbourne： IEEE， 2018： 32?41.

[15] RONGBO H B， WU W， TING H， et al. Indoor robot localization and 3D dense mapping based on ORB?SLAM [J]. Journal of computer applications， 2017（5）： 1439?1444.