劉 志,陳 超

(江蘇科技大學 機械工程學院,鎮(zhèn)江 212003)

一般來說大多數(shù)盲人和視力障礙者使用的導盲工具基本為功能簡單的手杖,當今社會發(fā)展迅速,周圍的環(huán)境變得越來越復雜,簡單的手杖已經(jīng)不能適應變化如此之快的環(huán)境.以導盲犬為代表的生物導盲方式受高訓練成本、長訓練周期和壽命等影響始終無法普及.由于智能化機械越來越成熟,性能優(yōu)異的導盲機器人是解決盲人生活困難問題的最優(yōu)解．

在導盲機器人自定位與自動導引研究方面,文獻[1]利用超聲波和紅外傳感器結(jié)合模糊PID控制理論實現(xiàn)導盲機器人的尋跡導航功能,但是缺乏實用性,只能以局部環(huán)境避障作為工作主體．文獻[2]采用天線射頻識別(radio frequency identifcation,RFID)技術(shù)完成導盲機器人自主定位,但該方法需要在房間內(nèi)序言布置好射頻標簽等硬件．文獻[3]利用Kinect深度相機進行定位和地圖構(gòu)建,實現(xiàn)導盲機器人的蔽障和路徑規(guī)劃,但其構(gòu)建的地圖是直接將Kinect獲取的三維模型進行平面投影所得,與真實環(huán)境相差較大．可見傳統(tǒng)的導盲機器人難以實現(xiàn)對復雜環(huán)境精確而有效的認知,且無法完成全局定位和導航,在人機交互方面上也存在較為明顯的不足．在數(shù)據(jù)融合地圖構(gòu)建(simultaneous localization and mapping，SLAM)方面國內(nèi)的相關(guān)研究還很少,文獻[4]使用Kinect和激光雷達進行數(shù)據(jù)融合,優(yōu)化所得地圖;文獻[5]對飛行時間技術(shù)(time of flight,TOF)相機和Kinect進行融合,獲得更精細的點云,構(gòu)建更準確的三維地圖;文獻[6]使用聲納和激光雷達進行數(shù)據(jù)融合,使得機器人在低能見度下進行SLAM．

為解決上述研究中存在的不足,針對導盲機器人的功能特點,提出將激光雷達和深度傳感器進行數(shù)據(jù)融合的構(gòu)建導盲機器人SLAM方法．首先通過對比現(xiàn)階段在機器人操作系統(tǒng)(robot operating system，ROS)中常用的二維SLAM方法來建立導盲機器人激光雷達的二維地圖；然后通過Kinect建立環(huán)境的三維地圖,結(jié)合Kinect生成的三維環(huán)境模型地圖數(shù)據(jù)信息豐富但數(shù)據(jù)精度欠缺的特點和激光雷達構(gòu)建的二維地圖進行拼接,彌補激光雷達構(gòu)建二維地圖數(shù)據(jù)精確但相對來說數(shù)據(jù)信息較少的缺點,得到融合二者優(yōu)點的三維空間障礙物模型地圖；最后以Turtlebot2為基礎(chǔ)搭建實驗平臺,在真實環(huán)境中對所提方法進行實驗驗證和比較．

1 激光雷達SLAM

SLAM方法可大致分為兩大類：基于濾波器的方法,以及基于圖優(yōu)化的方法[7]．目前ROS系統(tǒng)中有5種常用的SLAM構(gòu)建方法：HectorSLAM、Gmapping、KartoSLAM、CoreSLAM 和LagoSLAM．通過對這5種方法的優(yōu)缺點和精確的定量比較,定義一個通用的準則,選取一種合適導盲機器人使用的方法．

1.1 ROS系統(tǒng)中2D SLAM方式對比

HectorSLAM[8]對掃描的頻率有較高的要求,當掃描頻率較低時無法取得良好的結(jié)果,該方法不需要里程計的數(shù)據(jù),適用于飛行器和在地形起伏較大的地面行駛的機器人．HectorSLAM是結(jié)合已經(jīng)構(gòu)建的地圖對激光束點陣進行優(yōu)化,以此來預測激光點在實際地圖中的位置以及占據(jù)柵格的概率,從而更新地圖．Gmapping[9]采用基于RBPF (Rao-Blackwellized particle filter)粒子濾波的Fast SLAM 2.0算法．其利用擴展卡爾曼濾波將當前機器人的環(huán)境觀測信息融入粒子濾波器的提議分布設計之中,使得粒子集中地分布于高觀測似然區(qū)域,自適應重采樣技術(shù)引入減少了粒子耗散問題,計算粒子分布的時候不單單僅依靠機器人的運動,同時將當前觀測考慮進去,減少了機器人位置在粒子濾波步驟中的不確定性．KartoSLAM[10]和LagoSLAM[11]是基于圖優(yōu)化的方法,CoreSLAM[12]是為了簡單和容易理解最小化性能損失而設計的一種SLAM算法．

1.2 仿真環(huán)境對比試驗

在仿真環(huán)境stdr-simulator仿真器中選取一個簡單的樓層地圖作為實驗環(huán)境,地圖總大小為20 m×15 m，共有6個房間,每個房間大小為5 m×4.5 m,加載機器人半徑為0.15 m,在機器人中部安裝URG-04LX激光雷達,掃描半徑為4 m,掃描范圍為270°,模擬地圖和生成地圖如圖1．

圖1 仿真環(huán)境下機器人構(gòu)建的地圖Fig.1 SLAM in simulation environment

結(jié)果表明,在仿真環(huán)境下,HectorSLAM、Gmapping和KartoSLAM表現(xiàn)較好,其中Gmapping最為精確,CoreSLAM和LagoSLAM存在較為明顯的誤差．

2 三維環(huán)境模型建立

文中基于Kinect傳感器的實時三維環(huán)境構(gòu)建地圖，先提取彩色圖像中的特征點,計算其在深度點云空間中所對應的三維坐標;而后進行彩色圖像配準,利用配準中得到的運動結(jié)構(gòu)信息,進行深度圖像的拼接;最后形成同一坐標系中的三維環(huán)境地圖．

2.1 圖像預處理

2.1.1 特征點提取

文中選擇點特征作為視覺路標,選用FAST角點檢測方法,FAST[13]角點檢測利用像素點某個鄰域內(nèi)相鄰像素點灰度值的差異值的大小來決定該點是否為角點．該方法具有旋轉(zhuǎn)不變性,以及較快的速度和較多的檢測數(shù)量,能支持實時系統(tǒng)．

2.1.2 特征點匹配

采用的特征點匹配算法是歸一化互相關(guān)匹配(normalized cross correlation,NCC),原理是計算相鄰幀上角點鄰域內(nèi)所有像素點灰度值的相關(guān)性,利用相關(guān)系數(shù)來進行圖像匹配[13](圖2)．在圖像n中以角點A為中心取一個大小為(2l+1)×(2k+1)的鄰域T,在待配準圖片m中以B點在圖像n中的坐標為中心選取一個大小為(2H+1)×(2W+1)的搜索域S,然后對鄰域T和搜索域S進行歸一化互相關(guān)計算,將鄰域T疊放在搜索域S上平移．若搜索域中S中的某個角點B大于設定的閾值,則待配準圖片m中的B與圖像n中的A相匹配,認定是空間中的同一點．

圖2 NCC算法示意Fig.2 Schematic diagram of NCC algorithm

2.2 三維點云匹配

在獲取可靠的特征點對后,需要對三維點云進行匹配,使用迭代最近點(iterative closest points, ICP)算法，對于兩個來自不同坐標系的三維數(shù)據(jù)點集,找出兩個點集之間的空間變換， 對它們進行空間匹配．文中采用FAST角點檢測和NCC角點匹配是為了減小點集元素的數(shù)目,使用k-d tree來尋找特征點在參考點云中的最近點,不用遍歷三維點云中的每一個數(shù)據(jù),減少算法的時間代價．ICP三維點云匹配流程如圖3．

圖3 ICP算法Fig.3 ICP algorithm

3 三維模型與二維地圖融合

激光雷達構(gòu)建的二維SLAM地圖在精度、速度和魯棒性方面都有很大的優(yōu)勢,但是二維的激光雷達只能獲取某一個平面內(nèi)的環(huán)境信息,無法獲取立體的環(huán)境信息,將Kinect建立的三維環(huán)境模型與激光雷達建立的二維地圖進行拼接融合,既保留了激光雷達的高精度等優(yōu)點,又增加了三維環(huán)境的認知．

圖像拼接方法的具體步驟如下:首先使用坎尼邊緣檢測法對地圖進行邊緣檢測,對于三維模型地圖需選取某一層來進行;然后使用擴展相位相關(guān)法直接計算出兩幅地圖的平移、旋轉(zhuǎn)和尺度變化參數(shù),使用這些參數(shù)來拼接兩幅地圖內(nèi)的圖像邊緣;地圖的融合使用漸入漸出法,即在拼接地圖中的重疊區(qū)域,由第一幅地圖慢慢過渡到第二幅,從而保持拼接地圖在視覺上的一致性．

3.1 坎尼邊緣檢測法

坎尼(Canny)算子是一種最優(yōu)的邊緣檢測算子,廣泛應用在數(shù)字圖像處理領(lǐng)域[14]．其檢測邊緣的標準是:低誤判率;高定位精度;抑制虛假邊緣，盡可能地把圖像中的邊緣點誤認為是非邊緣點,同時準確地將圖像的邊緣點定位在灰度差異最大的像素點上，并盡量抑制圖像中虛假邊緣的產(chǎn)生．

坎尼邊緣檢測流程如下:

(1) 由于大多圖像數(shù)據(jù)中含有噪聲,噪聲點與附近像素點梯度幅值差異大,容易將其無檢測視為邊緣點,所以利用高斯濾波器對要處理的圖像進行卷積濾波,減小噪聲對梯度計算的影響．高斯濾波器如下:

(1)

(2) 使用一階差分算子分別算出水平方向和垂直方向的梯度幅值分量,進而得到圖像的梯度的幅值M和方向θ：

(2)

θ=arctan(Gy/Gx)

(3)

(3) 非極大值抑制．對梯度幅值圖像M(i,j)上每個像素和梯度方向上兩個相鄰像素進行插值計算,獲得該像素的梯度幅值,如果該像素的梯度幅值是大的,則把該像素視為可能的邊緣點,認為是非邊緣點,梯度幅值圖像M(i,j)經(jīng)過非極大值抑制得到圖像NMS(i,j)．

(4) 雙閾值檢測和邊緣連接．遍歷圖像NMS(i,j),分別使用高閾值Th和低閾值Tl進行閾值化獲得邊緣圖像E1和E2,E1是強邊緣點,可能出現(xiàn)間斷,E2是弱邊緣點．跟蹤E1中的邊緣,如果邊緣遇到端點,則在圖像E2相對位置的8鄰域內(nèi)尋找邊緣點來連接E1中的間斷,經(jīng)過持續(xù)的邊緣跟蹤,把圖像E1中邊緣的間斷連接起來．

3.2 基于邊緣擴展相位相關(guān)法

將對數(shù)極坐標變換與相位相關(guān)法相結(jié)合,形成了擴展相位相關(guān)法,目的是解決同時存在相對平移、旋轉(zhuǎn)和尺度變化的兩幅圖像的配準問題[14]．

假設兩幅圖像信號f1(x,y)和f2(x,y)滿足:

(4)

即f2(x,y)相對f1(x,y)沿正時針方向旋轉(zhuǎn)了θ0角度,尺度變化了α倍,平移了(x0,y0),要根據(jù)已知圖像f1(x,y)和f2(x,y)求出參數(shù)角度θ0、尺度因子α和平移量(x0,y0)，使用擴展相位相關(guān)法:

(1) 對f1(x,y)和f2(x,y)分別取傅里葉變換得F1(μ,υ)和F2(μ,υ),應滿足:

(5)

(2) 對F1(μ,υ)和F2(μ,υ)取模得M1(μ,υ)和M2(μ,υ),則有:

(6)

(3) 對M1(μ,υ)和M2(μ,υ)進行對數(shù)極坐標變換,即把這兩幅圖像從笛卡爾系中變換成對數(shù)極坐標系．

對M1(μ,υ)和M2(μ,υ)做極坐標變換得M1(ρ,θ)和M2(ρ,θ),由式(6)得:

M2(ρ,θ)=α2M1(αρ,θ+θ0)

(7)

式中：ρ為極徑,θ為極角．對式(7)沿ρ軸取對數(shù)得:

M2(lgρ,θ)=α2M1(lgρ+lgα,θ+θ0)

(8)

令γ=lgρ,γ0=lgα,則式(8)變?yōu)?

M2(γ,θ)=α2M1(γ+γ0,θ+θ0)

(9)

式中：γ為對數(shù)極徑.式(9)表示尺度因子和旋轉(zhuǎn)角度在對數(shù)極坐標系中表現(xiàn)為M1(γ,θ)和M2(γ,θ)的相對平移量．

(4) 相位相關(guān)法求解尺度因子α和平移量(x0,y0)．

將M1(γ,θ)和M2(γ,θ)做傅里葉變換得Fm1(μ,υ)和Fm2(μ,υ),則其歸一化互功率譜為:

(10)

將C(μ,υ)進行傅里葉反變換得:

c(γ,θ)=δ(γ-γ0,θ-θ0)

(11)

(5) 對圖像f1(x,y)沿正時針方向旋轉(zhuǎn),并尺度變化α倍得到相對于f2(x,y)僅存在(x0,y0)平移的圖像,再次使用步驟(4)就可以求解平移量(x0,y0)．

4 實驗

為了驗證文中所述的方法能合理有效地應用于導盲機器人,以Turtlebot2為基礎(chǔ)搭建實驗平臺,主要包括:Yujin Kobuki移動底座、Kinect攝像機、HOKUYO激光雷達和筆記本電腦以及其他輔件,如圖6．

圖6 實驗平臺Fig.6 Experiment platform

4.1 實際環(huán)境中SLAM算法對比

真實場景中的實驗選擇在有4條走廊的辦公樓,其中最長的走廊長度為45 m,寬度為2 m,用手柄控制移動機器人采集走廊環(huán)境信息,并使用不同方式構(gòu)建地圖,所得地圖如圖7．

經(jīng)過實驗發(fā)現(xiàn)其精度由激光雷達的掃描頻率決定,低掃描頻率下誤差較大,所以HectorSLAM只適用于高性能的激光雷達．Gmapping在所有試驗中表現(xiàn)出最好的精確性和魯棒性,通過里程計和掃描匹配的結(jié)合減少了粒子數(shù)量,并且能長時間保持精度,錯誤率和CPU使用率都較低．

圖7 真實環(huán)境下機器人構(gòu)建的地圖Fig.7 SLAM in actual environment

KartoSLAM和LagoSLAM都是基于圖優(yōu)化的SLAM方法,KartoSLAM提供精確的地圖較低的CPU負載,而LagoSLAM生成地圖誤差和CPU負載較高,原因在于兩者使用的節(jié)點配置和優(yōu)化方式不一樣．CoreSLAM在整個試驗中結(jié)果最差,主要是因為它沒有回環(huán)檢測環(huán)節(jié),CoreSLAM設計的初衷是在長走廊中進行SLAM,使用簡單的粒子濾波環(huán)節(jié)來匹配雷達數(shù)據(jù)來構(gòu)建地圖,由圖7,它在較細的長廊中表現(xiàn)差強人意,但是其他地方的位置錯誤估計較多．5種方法在構(gòu)建地圖時的CPU使用率如表1,測量時間為360 s．

表1 不同SLAM方法的CPU使用率Table 1 CPU Load of the 2D SLAM approaches %

經(jīng)過在仿真環(huán)境和真實環(huán)境中的對比,Gmapping方法在各方面表現(xiàn)較好,速度快,魯棒性好,因此選用Gmapping作為導盲機器人的二維地圖構(gòu)建方法．

4.2 三維環(huán)境模型建立

Kinect的三維SLAM地圖建立在一個房間里進行,機器人在一個矩形房間里環(huán)繞一周,采集環(huán)境信息,當某一幀的特征點數(shù)量太小,則舍棄該幀,當機器人的移動小于5 cm或者轉(zhuǎn)角小于2°,也舍棄這些幀的數(shù)據(jù)．Kinect建立的三維模型和渲染效果如圖8(a)和(b),(c)為去除地面特征的效果．

圖8 三維模型獲取及渲染效果Fig.8 3D model

4.3 三維模型與二維地圖拼接

在同一個房間內(nèi)使用Kinect建立三維SLAM地圖,再使用激光雷達建立二維SLAM地圖,然后進行三維模型和二維地圖的拼接,后期將整個流程集成到一個功能包中．該房間大小為5 m×7 m,當機器人環(huán)繞場地一周后,同時構(gòu)建二維和三維地圖,并完成拼接,第一幀的結(jié)果如圖9(a);整個房間的構(gòu)圖和拼接結(jié)果如圖9(b);去除點云信息后保留的二維地圖如圖9(c),可見即使是在障礙物較多、環(huán)境較為復雜的房間內(nèi),依然可以獲得較為理想的結(jié)果．由于Kinect精度以及導盲機器人速度控制等不可避免的問題,匹配遺漏的現(xiàn)象還會存在,想取得良好的實驗結(jié)果,需保持機器人均勻低速運行．

圖9 地圖拼接Fig.9 Map merging

5 結(jié)論

(1) 仿真環(huán)境和實際環(huán)境中對幾種ROS系統(tǒng)下的SLAM算法進行對比,分析各自的圖像質(zhì)量、運行速度、魯棒性和CUP使用率,總結(jié)得出Gmapping算法最適合室內(nèi)導盲機器人使用．

(2) 利用深度傳感器Kinect采集的信息,采用FAST角點檢測提取穩(wěn)定特征點,并使用歸一化互相關(guān)匹配法進行特征點匹配,再使用ICP算法對獲取的深度圖像進行精確配準,得到三維環(huán)境地圖,圖中包含環(huán)境內(nèi)的三維深度信息,但是在相應速度和精度上略有不足．

(3) 提出了一種基于激光雷達和Kinect信息融合的導盲機器人SLAM方法,通過綜合激光傳感器和深度圖象傳感器Kinect在采集數(shù)據(jù)和精度方面的優(yōu)缺點,使用邊緣擴展相位相關(guān)法,對Kinect生成的三維SLAM地圖和激光雷達構(gòu)建的二維SLAM地圖進行拼接,得到融合二者優(yōu)點的三維空間障礙物模型地圖．在真實環(huán)境中進行實驗,發(fā)現(xiàn)融合激光和深度信息的三維空間障礙物模型地圖能足夠全面和精確的認知環(huán)境,機器人因此能更好地完成導盲任務．

(4) 采用分布式ROS機器人操作系統(tǒng),使得整個機器人具有良好的實時性,文中激光掃描匹配和深度特征的提取與匹配很好的解決了機器人在構(gòu)建地圖的過程中遇到移動物體的干擾,所得地圖包含了室內(nèi)二維和三維的信息,并且所得地圖可以直接用于后續(xù)的機器人定位和導盲,配合相應的蔽障和路徑規(guī)劃算法可以讓導盲機器人自主移動,進而可以引導盲人安全行走．

參考文獻(References)

[ 1 ] 張志美, 程立英, 趙以恒, 等. 基于模糊PID控制算法的導盲機器人研究[J]. 沈陽師范大學學報(自然科學版), 2015, 33(1): 81-85

ZHANG Zhimei, CHEN Liying, ZHAO Yiheng, et al. Fuzzy-PID arithmetic for blind guiding robots [J].Journal of Shenyang Normal University(Natural Science Edition),2015, 33(1):81-85.(in Chinese)

[ 2 ] 陳超，唐堅，靳祖光.基于RFID技術(shù)導盲機器人室內(nèi)路徑規(guī)劃的研究[J].江蘇科技大學學報(自然科學版)，2013，27(1)：60-63.DOI：10.3969/j.issn.1673-4807.2013.01.014.

CHEN Chao, TANG Jian, JIN Zuguang. Indoor path planning for seeing eyes robot based on RFID[J].Journal of Jiangsu University of Science and Technology(Natural Science Edition),2013，27(1)：60-63.DOI：10.3969/j.issn.1673-4807.2013.01.014.(in Chinese)

[ 3 ] 張?zhí)?陳超. 突破環(huán)境限制的導盲方法[J]. 中國圖象圖形學報,2015, 20(11):1545-1551.

ZHENG Tan,CHEN Chao.Guiding the blind to overcome environment limitation [J].Journal of Image and Graphics,2015, 20(11):1545-1551. (in Chinese)

[ 4 ] YEON A S A, KAMARUDIN K, VISVANATHAN R, et al. Feasibility analysis of 2D-SLAM using combination of kinect and laser scanner[J]. Jurnal Teknologi, 2015, 76(12).

[ 5 ] VIEJO D, GARCIA-RODRIGUEZ J, CAZORLA M. Combining visual features and growing neural gas networks for robotic 3D SLAM[J]. Information Sciences, 2014, 276: 174-185. DOI:10.1016/j.ins.2014.02.053.

[ 6 ] SANTOS J M, COUCEIRO M S, PORTUGAL D, et al. A sensor fusion layer to cope with reduced visibility in SLAM[J]. Journal of Intelligent & Robotic Systems, 2015, 80(3): 401-422. DOI:10.1007/s10846-015-0180-8.

[ 7 ] 梁明杰,閔華清,羅榮華. 基于圖優(yōu)化的同時定位與地圖創(chuàng)建綜述[J]. 機器人,2013, 35(4):500-512.

LIANG Mjingjie,MIN Huaqing,LUO Ronghua.Graph-based SLAM: A Survey [J]. Robots, 2013, 35(4):500-512. (in Chinese)

[ 8 ] KOHLBRECHER S, STRYK O V, MEYER J, et al. A flexible and scalable SLAM system with full 3D motion estimation[C]//IEEE International Symposium on Safety, Security, and Rescue Robotics. IEEE, 2011:155-160.

[ 9 ] SANTOS J M, PORTUGAL D, ROCHA R P. An evaluation of 2D SLAM techniques available in robot operating system[J]. 2013 IEEE International Symposium on Safety, Security, and Rescue Robotics (SSRR), 2013. DOI:10.1109/ssrr.2013.6719348.

[10] DHIMAN N K, DEODHARE D, KHEMANI D. Where am I? Creating spatial awareness in unmanned ground robots using SLAM: A survey[J]. Sadhana, 2015, 40(5): 1385-1433. DOI:10.1007/s12046-015-0402-6.

[11] SANTOS J M, PORTUGAL D, ROCHA R P. An evaluation of 2D SLAM techniques available in robot operating system[J]. 2013 IEEE International Symposium on Safety, Security, and Rescue Robotics (SSRR), 2013. DOI:10.1109/ssrr.2013.6719348.

[12] VERKEENKO M S. Development of an algorithm for fast corner points detection[J]. Journal of Computer and Systems Sciences International, 2014, 53(3): 392-401. DOI:10.1134/s1064230714030162.

[13] SHEN X, BAO W. The remote sensing image matching algorithm based on the normalized cross-correlation and SIFT[J]. Journal of the Indian Society of Remote Sensing, 2013, 42(2): 417-422. DOI:10.1007/s12524-013-0323-y.

[14] 曾俊. 圖像邊緣檢測技術(shù)及其應用研究[D].華中科技大學,2011:17-38.