開放式環(huán)境下變電站機(jī)器人SLAM算法研究

任　杰，宋建濤，李功燕,

(1. 中國科學(xué)院大學(xué),北京 100049；2. 中國科學(xué)院微電子研究所,北京 100029；3.中國科學(xué)院物聯(lián)網(wǎng)研究發(fā)展中心，江蘇 無錫 214135)

0　引言

為了保證巡檢的可靠性與安全性，變電站對于自主巡檢機(jī)器人的需求日益增大。同時(shí)定位與建圖(Simultaneous Location and Mapping，SLAM)最先是由Smith和Cheeseman提出的，由于其重要的理論研究以及實(shí)際應(yīng)用價(jià)值，被認(rèn)為是實(shí)現(xiàn)機(jī)器人自我定位及導(dǎo)航的關(guān)鍵，因此也成為了該領(lǐng)域的研究熱點(diǎn)[1]。對于SLAM問題的求解，經(jīng)歷了從卡爾曼濾波，到擴(kuò)展卡爾曼濾波等延伸算法，最后到粒子濾波的過程[2-6]。卡爾曼濾波因其自身特性，無法有效處理非高斯、非線性情況，而粒子濾波則無此限制。所以，近年來，粒子濾波已經(jīng)成為了求解SLAM問題的主要方法。

移動機(jī)器人SLAM問題的求解總共分為兩部分，一部分叫做運(yùn)動更新，是根據(jù)里程計(jì)的數(shù)據(jù)進(jìn)行更新；另一部分叫做觀測更新，是根據(jù)傳感器數(shù)據(jù)對運(yùn)動更新進(jìn)行矯正，求解最佳位置。根據(jù)傳感器類型的不同(激光或者攝像頭)，也分為激光SLAM和視覺SLAM兩個方向。其中激光SLAM因?yàn)槠錅y量精確、穩(wěn)定性高、算法成熟，已逐漸應(yīng)用到變電站巡檢等各個領(lǐng)域[7]。

搭載激光傳感器的變電站巡檢機(jī)器人在粒子濾波的基礎(chǔ)上，發(fā)展出兩類比較成熟的算法。第一類是以柵格地圖創(chuàng)建(Grid Mapping, GMAPPING)為代表的SLAM算法[8]。此類算法同步性高，將機(jī)器人定位與地圖構(gòu)建同時(shí)進(jìn)行，但在實(shí)際應(yīng)用中往往因?yàn)橛?jì)算量過大，難以滿足實(shí)時(shí)性要求，同時(shí)因?yàn)槁窂揭?guī)劃等問題難以投入使用。第二類算法是以增強(qiáng)蒙特卡洛定位(Augmented Monte Carlo Localization，AMCL)算法為代表的地圖匹配算法[9]。該算法將SLAM問題中的“建圖”與“定位”拆分，使用第一類方法構(gòu)建的地圖作為先驗(yàn)知識，只需將激光數(shù)據(jù)與原地圖匹配進(jìn)行“定位”運(yùn)算，在靜態(tài)環(huán)境下大大提高了計(jì)算效率，減少了資源消耗，達(dá)到了實(shí)時(shí)性與穩(wěn)定性的要求。但由于過于依賴先驗(yàn)地圖知識，一旦地圖發(fā)生變化，算法的可靠性將大大降低。

變電站一般為開放式的室外環(huán)境。變電站機(jī)器人的巡檢既需要滿足實(shí)時(shí)性的要求，又要機(jī)器人在動態(tài)環(huán)境下仍能保持穩(wěn)定。本文結(jié)合上述兩種算法的優(yōu)缺點(diǎn)，將兩種算法融合，提出了一種在原有地圖上局部實(shí)時(shí)建圖的算法。該算法的核心思想是：在地圖的靜態(tài)區(qū)域仍使用原有AMCL算法，機(jī)器人行駛至地圖發(fā)生變化的區(qū)域時(shí)使用GMAPPING算法進(jìn)行局部實(shí)時(shí)建圖，并將新建地圖與原地圖進(jìn)行重合，機(jī)器人駛出后恢復(fù)使用原地圖。本文選取地圖發(fā)生變化的變電站作為實(shí)驗(yàn)場地，發(fā)現(xiàn)使用原有AMCL算法會出現(xiàn)較大偏差甚至完全丟失位置，而使用局部建圖算法則可較好地完成巡檢。同時(shí)，針對局部建圖算法中地圖重合和變換關(guān)系選取等問題，采用多種策略進(jìn)行比較，最終選定了最小二乘(Least Square，LS)方法計(jì)算變換關(guān)系。根據(jù)實(shí)際測試情況可以看出，局部建圖算法融合了兩種現(xiàn)有算法的優(yōu)點(diǎn)，能夠較好地解決開放式環(huán)境下地圖發(fā)生變化的問題，是一種有效而實(shí)用的方法。

1　基本算法介紹

1.1　GMAPPING算法

GMAPPING算法是一種基于2D激光傳感器的算法，其主要作用是構(gòu)建2D地圖。該算法的核心思想是：先隨機(jī)給定機(jī)器人在當(dāng)前環(huán)境中的初始坐標(biāo)，然后以初始坐標(biāo)為起點(diǎn)開始建圖，該部分建圖完成后，機(jī)器人移動，將上一時(shí)刻新建完成的地圖與機(jī)器人采集的激光數(shù)據(jù)進(jìn)行匹配，從而確定機(jī)器人位置，再根據(jù)激光相對于機(jī)器人的位置完成建圖。顯然，這是一個遞推過程。GMAPPING算法的主要步驟[10]如下：

(1) 輸入t-1時(shí)刻的里程計(jì)信息ut-1，粒子集合St-1，以及t時(shí)刻的觀測值zt。

(4) 進(jìn)行運(yùn)動更新。結(jié)合里程計(jì)模型更新粒子位置。

(5) 進(jìn)行觀測更新。用觀測模型進(jìn)行更新，并結(jié)合運(yùn)動更新后粒子位置、上一時(shí)刻粒子權(quán)值計(jì)算當(dāng)前時(shí)刻粒子權(quán)值。

(7) 根據(jù)觀測信息以及計(jì)算的最佳粒子位置，更新地圖。

(9) End for

(10) 從St中進(jìn)行重采樣。

運(yùn)動更新以及觀測更新等的計(jì)算過程本文不展開論述，具體請參照參考文獻(xiàn)[10]。

GMAPPING算法的弊端在于，一方面，每個粒子都需要單獨(dú)維護(hù)一幅地圖，計(jì)算量較大，在大環(huán)境下難以滿足實(shí)時(shí)性的需求；另一方面，二維激光所提供的信息僅僅是與激光等高的一個平面，對于路徑、路面等信息完全無法估計(jì)，造成了路徑規(guī)劃時(shí)的困難。因此，雖然GMAPPING算法也自帶定位功能，但在實(shí)際應(yīng)用中往往只使用該算法來建圖，將得到的地圖作為先驗(yàn)知識使用AMCL算法進(jìn)行定位。

1.2　AMCL算法

AMCL算法也是基于粒子濾波的一種算法，它將SLAM問題中的“建圖”與“定位”拆分，使用第一類方法構(gòu)建的地圖作為先驗(yàn)知識，只需將激光數(shù)據(jù)與原地圖匹配進(jìn)行“定位”運(yùn)算，在靜態(tài)環(huán)境下大大提高了計(jì)算效率，減少了資源消耗，達(dá)到了實(shí)時(shí)性與穩(wěn)定性的要求。同時(shí)，因?yàn)橛辛说貓D作為先驗(yàn)知識，所有的路徑規(guī)劃均可以地圖為基準(zhǔn)進(jìn)行規(guī)劃，有效避免了GMAPPING算法的弊端。

AMCL算法與GMAPPING算法的步驟大致相同，不同點(diǎn)在于：

(1) 因?yàn)锳MCL不用進(jìn)行地圖構(gòu)建，所需計(jì)算量大大減小，所以在進(jìn)行粒子濾波時(shí)可以大量增加粒子數(shù)量來提高精度(通?？梢赃_(dá)到幾百或幾千)。

(2) 因?yàn)榱Ｗ訑?shù)目大量增加，小范圍內(nèi)粒子位置可基本覆蓋正確位置，在進(jìn)行當(dāng)前時(shí)刻位置計(jì)算時(shí)可以不用計(jì)算高斯分布，直接用最佳粒子位置代替機(jī)器人位置。

AMCL算法的弊端在于，需要地圖作為先驗(yàn)知識，一旦地圖發(fā)生變化，算法的可靠性將大大降低。所以，在變電站這種開放式的大環(huán)境下，長時(shí)間使用AMCL算法仍會出現(xiàn)一些問題。因此，提出了局部建圖算法。

2　局部建圖算法

局部建圖算法的核心思想是：在地圖的靜態(tài)區(qū)域仍使用原有AMCL算法，機(jī)器人行駛至地圖發(fā)生變化的區(qū)域時(shí)使用GMAPPING算法進(jìn)行局部實(shí)時(shí)建圖，并將新建地圖與原地圖進(jìn)行重合，機(jī)器人駛出后恢復(fù)使用原地圖。它融合了GMAPPING算法與AMCL算法的優(yōu)點(diǎn)，既實(shí)現(xiàn)了較為方便的路徑規(guī)劃，又滿足了實(shí)時(shí)建圖的需求。

2.1　算法分析

局部建圖算法的關(guān)鍵點(diǎn)在于如何實(shí)現(xiàn)新建地圖與原地圖的重合。新建地圖后，機(jī)器人所獲得的自身坐標(biāo)是相對于新圖的坐標(biāo)，只有將這個坐標(biāo)轉(zhuǎn)換到舊圖坐標(biāo)上才能保證之前設(shè)定的路徑規(guī)劃等功能不受影響。

但是，另一方面，由于激光傳感器誤差、算法本身誤差等原因，“切換點(diǎn)”的坐標(biāo)不一定是完全準(zhǔn)確的，尤其是此時(shí)的機(jī)器人朝向R(單位為°)，一旦與真實(shí)朝向有一個偏差角r，那么新建地圖與原有地圖也將有一個r角度的偏差。這個偏差帶來的誤差可以用式(1)表示：

Δσ=sinr

(1)

即偏離1°誤差就可以達(dá)到1.74%，對于50 m誤差可以達(dá)到87 cm，這是實(shí)際應(yīng)用中無法接受的。

該算法的主要誤差分為兩類，一類是算法本身的誤差，包括模型誤差、噪聲誤差、傳感器導(dǎo)致的誤差等，通常這些誤差較?。涣硪活愂乔袚Q后變換關(guān)系導(dǎo)致的誤差，根據(jù)之前敘述，該算法對角度的敏感度非常高，雖然采取了一定的方法去解決，但是仍會出現(xiàn)不可避免的誤差。第二類誤差的量與機(jī)器人行駛距離成正比，是線性誤差。

2.2　算法步驟

局部建圖算法的主要步驟如下：

(1) 在地圖無變化區(qū)域使用原有AMCL算法。

(2) 選擇切換點(diǎn)，經(jīng)過切換點(diǎn)后進(jìn)入學(xué)習(xí)階段。

(3) 學(xué)習(xí)階段開啟GMAPPING建圖，但不輸出機(jī)器人位置，同時(shí)不關(guān)閉AMCL算法，將AMCL算法得到的機(jī)器人位置作為輸入給GMAPPING。用LS方法計(jì)算兩種坐標(biāo)間的轉(zhuǎn)換關(guān)系，得到關(guān)聯(lián)式。

(4) 關(guān)閉AMCL算法，同時(shí)開啟GMAPPING輸出，利用關(guān)聯(lián)式將新輸出坐標(biāo)轉(zhuǎn)換到舊地圖上。

(5) 駛出地圖變化區(qū)域，將得到的坐標(biāo)作為初始位置送給AMCL算法，同時(shí)關(guān)閉建圖與GMAPPING的位置輸出。

3　實(shí)驗(yàn)檢驗(yàn)

根據(jù)第2節(jié)介紹，該算法的第一類誤差較小，通常為幾厘米，第二類誤差呈線性，在行駛距離較大時(shí)誤差較大。因此，在行駛距離較大時(shí)，該算法的整體誤差大致呈線性?？梢酝ㄟ^測量機(jī)器人行駛一定距離后偏離正常位置的距離來驗(yàn)證算法的可靠性。

本文使用杭州申昊科技股份有限公司生產(chǎn)的SHIR-3000X型機(jī)器人，搭載西科公司生產(chǎn)的LMS511型激光。該機(jī)器人已經(jīng)在浙江、湖北等地的多個變電站中投入使用。實(shí)驗(yàn)場地是浙江金華明珠變電站，該實(shí)驗(yàn)場地早在建設(shè)的過程中就對機(jī)器人應(yīng)處的正確位置做出了標(biāo)定。選擇的實(shí)驗(yàn)路段總長度為50 m、寬度為2 m。

該實(shí)驗(yàn)場地因?yàn)榈缆穬膳跃G化帶生長，現(xiàn)場環(huán)境與先驗(yàn)地圖有較大偏差，機(jī)器人在正常巡檢過程中時(shí)常出現(xiàn)無法找到正確位置的情況，圖1是10次實(shí)驗(yàn)中，機(jī)器人偏離正常位置的最大距離D。其中方塊代表使用AMCL算法，圓代表使用沒有學(xué)習(xí)階段的局部建圖算法。誤差達(dá)到1 m表示機(jī)器人丟失自身位置。

圖1　10次實(shí)驗(yàn)機(jī)器人偏離正常位置最大距離

由圖1可以看出，在使用AMCL算法的情況下有7次機(jī)器人完全丟失位置，3次沒有丟失位置的情況也都出現(xiàn)了較大偏差。而使用局部建圖算法10次都完成了50 m的路程，但是在不搭建學(xué)習(xí)區(qū)域的情況下，最大誤差可以達(dá)到80 cm，難以滿足要求。可以看出，局部建圖算法能夠滿足適應(yīng)變化環(huán)境的要求，但在精度方面仍需提升。

圖2是采用不同的精度提升策略得到的行駛距離與誤差關(guān)系圖。X軸表示的是機(jī)器人行駛距離L，Y軸表示的是10次實(shí)驗(yàn)下的平均偏差D。方塊代表的是不使用任何策略；圓點(diǎn)代表使用學(xué)習(xí)區(qū)域，但不用LS算法處理，而是計(jì)算相應(yīng)坐標(biāo)間的變換關(guān)系然后求平均；三角代表使用LS方法求解關(guān)聯(lián)式。

圖2　機(jī)器人行駛距離與平均偏差圖

由圖2可以看出，10次實(shí)驗(yàn)下的偏差呈近似的線性分布，采用兩種策略均能極大改善起始點(diǎn)選擇帶來的偏差，其中使用LS方法可以把誤差控制在20 cm內(nèi)，能夠滿足現(xiàn)場使用需求。根據(jù)實(shí)驗(yàn)，最終選擇使用LS方法求取關(guān)聯(lián)式來提高算法精度。

4　結(jié)論

本文針對傳統(tǒng)二維激光SLAM算法實(shí)時(shí)性差、對先驗(yàn)地圖過于依賴的弊端，提出了一種能夠在開放式環(huán)境下使用的局部建圖算法。該算法在地圖無變化區(qū)域使用原有算法，在地圖變化部分實(shí)時(shí)局部建圖，并與原地圖重合。通過與傳統(tǒng)AMCL算法的比較，證明了該算法能夠較好地解決現(xiàn)場環(huán)境發(fā)生變化與先驗(yàn)地圖產(chǎn)生偏差的情況。同時(shí)通過實(shí)驗(yàn)驗(yàn)證，證明了使用LS方法建立關(guān)聯(lián)式，能夠有效地解決算法精度問題。但該算法的誤差與機(jī)器人行駛距離成正比，在長距離中只能進(jìn)行分段使用。分段使用過程中，如何在進(jìn)出時(shí)快速收斂自身位置是以后的研究方向。

[1] WILLIAMS S B. Constrained initialization of the simulataneous localization and mapping algorithm[J].International Journal of Robotics Research,2003，22(7/8):541-564.

[2] WELCH G, BISHOP G. An introduction to the Kalman filter[M]. University of North Carolina at Chapel Hill, 2006.

[3] SHOUDONG H, GAMINI D. Convergence and consistency analysis for extended Kalman filter based SLAM[J]. IEEE Transactions on Robotics,2007(5):1036-1049.

[4] ADAMS M, ZHANG S, XIE L. Particle filter based outdoor robot localization using natural features extracted from laser scanners[C]// IEEE International Conference on Robotics and Automation, 2004:1493-1498.

[5] LUBIN C, BAIQING H, AN L. Transformed unscented Kalman filter[J]. IEEE Transactions on Automatic Control, 2013(1):252-257.

[6] DOUCET A, DE FREITAS N, MURPHY K, et al. Rao-Blackwellised particle filtering for dynamic Bayesian networks[C]//Proceedings of the Sixteenth Conference on Uncertainty in Artificial Intelligence. Morgan Kaufmann Publishers Inc., 2000: 176-183.

[7] 李紅梅, 王濱海, 廖文龍,等. 基于地圖匹配的變電站巡檢機(jī)器人激光導(dǎo)航系統(tǒng)設(shè)計(jì)[J]. 制造業(yè)自動化, 2014(1):52-56.

[8] GRISETTI G，STACHNISS C， BURGARD W.Improving Grid-based SLAM with Rao-Blackwellized particle filters by adaptive proposals and selective resampling[C]//Proceedings of the 2005 IEEE International Conference on Robotics and Automation,2005:2432-2437.

[9] DELLAERT F，F(xiàn)OX D，BURGARD W，et al，Monte Carlo localization for mobile robots[C]//Proceedings 1999 IEEE International Conference on Robotics and Automation (Cat. No.99CH36288C), 1999:1322-1328.

[10] GRISETTI G，STACHNISS C， BURGARD W.Improved techniques for grid mapping with Rao-Blackwellized particle filters[J].IEEE Transactions on Robotics, 2007(1):34-46.

[11] 趙景堂,杜國明,李秀海.基于總體最小二乘法的二維坐標(biāo)轉(zhuǎn)換方法[J].黑龍江工程學(xué)院學(xué)報(bào),2015(1):21-22.