葛艷茹，張國偉，沈宏雙，孫溫和

(上海電力學(xué)院 自動(dòng)化工程學(xué)院，上海 200090)

基于激光測(cè)距儀全局匹配掃描的SLAM算法研究

葛艷茹，張國偉，沈宏雙，孫溫和

(上海電力學(xué)院 自動(dòng)化工程學(xué)院，上海 200090)

針對(duì)傳統(tǒng)的SLAM(Simultaneous Localization and Mapping)算法構(gòu)建地圖時(shí)容易受環(huán)境因素和外界條件的的影響，在非線性系統(tǒng)狀態(tài)下誤差修正能力不足，且當(dāng)機(jī)器人位姿都處于未知狀態(tài)時(shí)，移動(dòng)機(jī)器人位姿獲取不精確，地圖構(gòu)建SLAM技術(shù)特征量的獲取比較繁瑣、不準(zhǔn)確等問題;以電力巡檢機(jī)器人為平臺(tái)，研究了基于全局匹配的掃描算法,摒棄傳統(tǒng)的柵格地圖模型的插值方法，采用雙線性濾波的插值方法，保證子?xùn)鸥駟卧木_性，估算柵格占用函數(shù)的概率和導(dǎo)數(shù);最后采用此算法解決了SLAM地圖構(gòu)建的問題，并分別在室內(nèi)室外環(huán)境進(jìn)行實(shí)驗(yàn);實(shí)驗(yàn)結(jié)果表明：基于激光測(cè)距儀的全局匹配掃描的SALM算法，在室內(nèi)室外兩種不同環(huán)境下，不受復(fù)雜背景的影響，準(zhǔn)確地進(jìn)行機(jī)器人位姿定位，以及環(huán)境地圖的構(gòu)建。

激光測(cè)距儀；全局匹配掃描算法；同步定位及地圖構(gòu)建；巡檢機(jī)器人；位姿定位

0 引言

地面自主移動(dòng)機(jī)器人是智能機(jī)器人領(lǐng)域的一個(gè)重要研究方向[1]，機(jī)器人的自主移動(dòng)成了當(dāng)下一大熱點(diǎn)。隨著移動(dòng)機(jī)器人的作業(yè)環(huán)境變的越來越復(fù)雜和非結(jié)構(gòu)化，作為實(shí)現(xiàn)移動(dòng)機(jī)器人自主運(yùn)動(dòng)的關(guān)鍵技術(shù)之一，SLAM技術(shù)已成為越來越重要的基礎(chǔ)性難題，引起了眾多學(xué)者紛紛對(duì)其進(jìn)行研究。文獻(xiàn)[2]基于改進(jìn)粒子濾波的SLAM算法研究，針對(duì)粒子退化和粒子耗盡提出了兩種不同的粒子改進(jìn)算法，提高了算法的精確性，但是此算法需大量采樣粒子，工作量大，且頻繁采樣，會(huì)造成粒子的耗盡。文獻(xiàn)[3]基于CEKF的SLAM算法研究與分析，對(duì)CEKF算法進(jìn)行了研究，事實(shí)證明，機(jī)器人的運(yùn)動(dòng)速度和變化率會(huì)產(chǎn)生較大的定位誤差，并不可取。文獻(xiàn)[4]基于迭代平方根CKF的SLAM算法,提出了一種迭代平方根的CKF的改進(jìn)算法，利用最新的觀測(cè)信息，降低CKF的估計(jì)誤差，但是此改進(jìn)算法容易受環(huán)境因素的影響，地圖構(gòu)建的精度還有所欠缺。文獻(xiàn)[5]基于匹配的SLAM的機(jī)器人定位系統(tǒng)研究，采用掃描匹配的方法對(duì)機(jī)器人定位系統(tǒng)進(jìn)行分析，但是需要對(duì)每一次掃描進(jìn)行處理，大大增加了計(jì)算量。所以，本文采用基于激光測(cè)距儀的全局匹配掃描算法進(jìn)行SLAM研究，旨在準(zhǔn)確實(shí)現(xiàn)機(jī)器人的位姿定位，提高地圖構(gòu)建的準(zhǔn)確性，最后在電力巡檢機(jī)器人的平臺(tái)上對(duì)此算法進(jìn)行實(shí)驗(yàn)驗(yàn)證，用以驗(yàn)證算法的可行性。

1 激光測(cè)距全局匹配掃描的SLAM算法

全局匹配掃描的SLAM算法是一種掃描匹配算法，將當(dāng)前的掃描與全局地圖匹配，通過計(jì)算剛體變換解來獲得機(jī)器人當(dāng)前位姿，包括地圖描述，掃描匹配，多分辨率地圖3個(gè)部分。

1.1 地圖描述

傳統(tǒng)的占用柵格地圖模型無法通過插值和微分的方法進(jìn)行直接計(jì)算，無法實(shí)現(xiàn)要求的精確度，論文摒棄此種插值方法，采用雙線性濾波的插值方法，既能保證子?xùn)鸥駟卧木_性，又能估算柵格占用函數(shù)的概率和導(dǎo)數(shù)。首先給出一個(gè)連續(xù)的坐標(biāo)點(diǎn)，通過公式(1)，公式(2)，公式(3)計(jì)算出柵格地圖的組成單元的占用值M(Pm)。

通過x-y軸坐標(biāo)系進(jìn)行線性插值得到:

(1)

其微分可以近似表示為：

(2)

(3)

1.2 掃描匹配

掃描匹配過程是當(dāng)前激光掃描與參考激光掃描進(jìn)行匹配,或者與機(jī)器人當(dāng)前的地圖進(jìn)行匹配?，F(xiàn)代的激光掃描傳感器具有精度高和高頻率的特點(diǎn)。因此，設(shè)法將激光掃描高效匹配就有可能獲得很精確的結(jié)果。論文所采用的方法是將激光束的終點(diǎn)集和目前已知的地圖盡可能地匹配。通過這種方法，沒有必要再尋找激光束終點(diǎn)之間的數(shù)據(jù)關(guān)聯(lián)。

論文采用式(4)求解ξ=(px,py,ψ)T，即:

(4)

通過這種變換從而最有效地將當(dāng)前激光掃描和現(xiàn)有地圖相匹配。其中，Si(ξ)是世界坐標(biāo)下機(jī)器人坐標(biāo)ξ的函數(shù)，激光終點(diǎn)Si=(Si,x,Si,y)T是環(huán)境特征在世界坐標(biāo)下的坐標(biāo)，函數(shù)M(Si(ξ))返回坐標(biāo)點(diǎn)Si(ξ)被占用的概率值，給定某一初始的ξ，可以通過式(5)估計(jì)Δξ的值:

(5)

解出Δξ需要使用高斯—牛頓等式解決如下化簡問題：

(6)

通過等式(6)我們可以得到:

(7)

R=Var{ξ}=σ2·H-1

(8)

σ是一個(gè)尺度因素，它和激光掃描器的屬性有關(guān)。

1.3 多分辨率地圖

論文采用多分辨率地圖方式來解決基于梯度算法的方法可能會(huì)陷入局部極小值的問題，類似于計(jì)算機(jī)視覺中的圖像金字塔方法，選擇性的使用多層占有柵格地圖，這些柵格地圖的分辨率逐層減半。內(nèi)存中存放了不同的地圖，通過對(duì)激光掃描匹配得到的位姿估算值對(duì)這些地圖進(jìn)行實(shí)時(shí)更新。這一過程不僅保證了地圖的連續(xù)性，同時(shí)又減小了向下采樣帶來的代價(jià)消耗。激光匹配過程開始用分辨率最低的那張地圖，得到近似的位姿估算值，被當(dāng)做下一次位姿估計(jì)的初始值。該方法會(huì)產(chǎn)生一種有益的伴隨作用，可以立即得到粗糙的圖像，進(jìn)而可以用于路徑規(guī)劃和機(jī)器人導(dǎo)航等應(yīng)用。

2 巡檢機(jī)器人SLAM實(shí)驗(yàn)

2.1 實(shí)驗(yàn)流程

論文采用柵格地圖作為輸出地圖，初始化地圖尺寸和初始分辨率，將激光數(shù)據(jù)導(dǎo)入論文提出的全局匹配算法，實(shí)現(xiàn)地圖的實(shí)時(shí)構(gòu)建與機(jī)器人的定位，具體流程如圖1所示。

圖1 基于全局匹配的SLAM算法實(shí)現(xiàn)流程圖

2.2 實(shí)驗(yàn)平臺(tái)

本次機(jī)器人定位和地圖構(gòu)建是以電力巡檢機(jī)器人為平臺(tái)，此平臺(tái)包括硬件和軟件平臺(tái)。

硬件平臺(tái)：激光測(cè)距儀，云臺(tái)攝像機(jī)，控制主板等。如圖2所示。

軟件平臺(tái)：Window系統(tǒng)下用vc2013版本開發(fā)工具開發(fā)，并用到機(jī)器人領(lǐng)域的MRPT(Mobile Robot Programming Toolkit)工具。

2.3 SLAM實(shí)驗(yàn)結(jié)果

論文分別對(duì)全局匹配算法進(jìn)行了室內(nèi)和室外環(huán)境實(shí)驗(yàn)，以便觀察環(huán)境的改變對(duì)機(jī)器人位姿確定和地圖構(gòu)建產(chǎn)生的影響。首先通過遙控方式使電力巡檢機(jī)器人運(yùn)動(dòng)一定范圍，同時(shí)打開激光測(cè)距程序?qū)崟r(shí)采集數(shù)據(jù)，并將數(shù)據(jù)導(dǎo)入論文的全局匹配算法，進(jìn)行同步定位與構(gòu)圖。

室內(nèi)環(huán)境：室內(nèi)環(huán)境如圖3所示，根據(jù)室內(nèi)環(huán)境設(shè)置輸出柵格地圖尺寸為100 m×25 m，分辨率為0.1 m×0.1 m。通過遙控的方式指引電力巡檢機(jī)器人平臺(tái)在室內(nèi)運(yùn)動(dòng)，采集數(shù)據(jù)進(jìn)行定位與構(gòu)圖。

圖2 巡檢機(jī)器人平臺(tái) 圖3 室內(nèi)環(huán)境

室內(nèi)構(gòu)圖結(jié)果：通過MRPT編程工具箱GUI(圖形用戶界面)顯示構(gòu)建地圖結(jié)果如圖4所示。

圖4 室內(nèi)環(huán)境地圖構(gòu)建

圖4中的閉合曲線表示機(jī)器人實(shí)際運(yùn)行路徑，其他點(diǎn)集表示室內(nèi)地圖特征。通過該圖可以清晰的看出室內(nèi)的特征，可見，全局匹配掃描算法的SLAM地圖構(gòu)建在室內(nèi)環(huán)境準(zhǔn)確的定位了機(jī)器人位姿，準(zhǔn)確的構(gòu)建出所處室內(nèi)環(huán)境地圖。

室外環(huán)境：設(shè)置輸出柵格地圖尺寸100 m×100 m，分辨率為0.2 m×0.2 m。通過遙控方式控制電力巡檢機(jī)器人平臺(tái)走出一條折現(xiàn)路徑，并采集數(shù)據(jù)進(jìn)行定位與構(gòu)圖。

圖5 室外環(huán)境

室外構(gòu)圖結(jié)果：通過MRPT編程工具箱GUI(圖形用戶界面)顯示構(gòu)建地圖結(jié)果如圖6所示。

圖6 室外柵格地圖

圖6中曲線a表示機(jī)器人實(shí)際運(yùn)行路徑，其他點(diǎn)集表示當(dāng)前室外環(huán)境特征。將室外場景和機(jī)器人構(gòu)建的地圖對(duì)比可知，該圖清晰地反映了室外折角的特征。由圖(4)和(5)不難發(fā)現(xiàn)，無論是在室外還是室內(nèi)環(huán)境中，全局匹配的掃描算法都能準(zhǔn)確地獲取機(jī)器人位姿，準(zhǔn)確地構(gòu)建當(dāng)前環(huán)境地圖。

3 結(jié)束語

本文研究了基于激光測(cè)距儀全局匹配的的SLAM解決方案，并針對(duì)室內(nèi)室外兩種環(huán)境下，利用巡檢機(jī)器人平臺(tái)，對(duì)全局匹配算法進(jìn)行了實(shí)驗(yàn)驗(yàn)證。實(shí)驗(yàn)結(jié)果表明：室內(nèi)室外兩種不同的環(huán)境，都能夠準(zhǔn)確的定位機(jī)器人的當(dāng)前位姿，構(gòu)建的地圖都清晰的反映了當(dāng)前的環(huán)境，可見，環(huán)境的復(fù)雜程度并不會(huì)對(duì)全局匹配的SLAM研究造成很大的影響。以后將在論文的基礎(chǔ)上，繼續(xù)研究機(jī)器人的自主導(dǎo)航工作。

[1] 張會(huì)麗. 地面自主移動(dòng)機(jī)器人路徑規(guī)劃算法研究[D]. 杭州：浙江大學(xué)，2013.

[2] 周旭. 基于改進(jìn)粒子濾波的SLAM算法研究[D]. 南京:南京理工大學(xué), 2014.

[3] 雷碧波. 基于CEKF的SLAM算法研究與分析[J]. 工業(yè)控制計(jì)算機(jī), 2015, 28(10):14-16.

[4] 高 偉, 張 亞, 孫 騫,等. 基于迭代平方根CKF的SLAM算法[J]. 哈爾濱工業(yè)大學(xué)學(xué)報(bào), 2014, 46(12):120-124.

[5] 杜釗君, 吳懷宇. 基于激光測(cè)距與雙目視覺信息融合的移動(dòng)機(jī)器人SLAM研究[J]. 計(jì)算機(jī)測(cè)量與控制, 2013.21(1):180-183.

[6] 魏紹清. 基于EKF-SLAM算法的室內(nèi)移動(dòng)機(jī)器人一致性研究[D]. 石家莊：河北科技大學(xué)，2013.

[7] Cooper A J. A comparison of data association techniques for Simultaneous Localization and Mapping [J]. Massachusetts Institute of Technology, 2006.

[8] Zhang Z. Iterative point matching for registration of free-form curves and surfaces[J]. International Journal of Computer Vision, 1994, 13(2): 119-152.

[9] 張 琦. 移動(dòng)機(jī)器人的路徑規(guī)劃與定位技術(shù)研究[D]. 哈爾濱：哈爾濱工業(yè)大學(xué)，2014.

[10] 曲麗萍. 移動(dòng)機(jī)器人同步定位與地圖構(gòu)建關(guān)鍵技術(shù)的研究[D]. 哈爾濱：哈爾濱工程大學(xué)，2013，28(10):14-16.

SLAM Research of Global Matching Scanning Based on Laser Range Finders

Ge Yanru，Zhang Guowei，Shen Hongshuang，Sun Wenhe

(College of Automation Engineering, Shanghai University of Electric Power，Shanghai 200090，China)

Targeting to tackle those problems like traditional SLAM(Simultaneous Localization and Mapping) algorithm to construct a map easily affected by environmental factors and external conditions, in the condition of nonlinear system error correction ability is insufficient, and during environment map and robot pose are unkown state, mobile robot get inaccurate, map building SLAM technology for comparison of the characteristics of complicated and inaccurate. Inspection robot for power as a platform, based on matching scanning algorithm are studied, and instead of traditional grid map model interpolation method, using bilinear filtering interpolation method, ensure the accuracy of grid unit estimate the probability and the derivative of grid takes up function. Finally, adopt the algorithm solved the problem of the SLAM map building, and respectively in the indoor and outdoor environment for experiments. The experimental results show that the SLAM algorithm of global matching scanning based on laser rangefinder, the indoor and outdoor, two different from the complex background, the influence of robot positioning accurately, as well as the environment map building.

laser range finder; global matching scanning algorithm; SLAM; inspection robot; posture positioning

2016-07-08；

2016-07-25。

上海市電站自動(dòng)化技術(shù)重點(diǎn)實(shí)驗(yàn)室(13DZ2273800)。

葛艷茹(1991-)，女，碩士研究生，主要從事手勢(shì)識(shí)別在機(jī)器人上的應(yīng)用方向的研究。

張國偉(1970-)，男,副教授,碩士生導(dǎo)師,主要從事信息檢測(cè)方向的研究。

1671-4598(2016)12-0198-02

10.16526/j.cnki.11-4762/tp.2016.12.056

TP242

A