開放式環境下變電站機器人SLAM算法研究

任　杰，宋建濤，李功燕,

(1. 中國科學院大學,北京 100049；2. 中國科學院微電子研究所,北京 100029；3.中國科學院物聯網研究發展中心，江蘇 無錫 214135)

0　引言

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

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

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

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

1　基本算法介紹

1.1　GMAPPING算法

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

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

(4) 進行運動更新。結合里程計模型更新粒子位置。

(5) 進行觀測更新。用觀測模型進行更新，并結合運動更新后粒子位置、上一時刻粒子權值計算當前時刻粒子權值。

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

(9) End for

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

運動更新以及觀測更新等的計算過程本文不展開論述，具體請參照參考文獻[10]。

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

1.2　AMCL算法

AMCL算法也是基于粒子濾波的一種算法，它將SLAM問題中的“建圖”與“定位”拆分，使用第一類方法構建的地圖作為先驗知識，只需將激光數據與原地圖匹配進行“定位”運算，在靜態環境下大大提高了計算效率，減少了資源消耗，達到了實時性與穩定性的要求。同時，因為有了地圖作為先驗知識，所有的路徑規劃均可以地圖為基準進行規劃，有效避免了GMAPPING算法的弊端。

AMCL算法與GMAPPING算法的步驟大致相同，不同點在于：

(1) 因為AMCL不用進行地圖構建，所需計算量大大減小，所以在進行粒子濾波時可以大量增加粒子數量來提高精度(通?？梢赃_到幾百或幾千)。

(2) 因為粒子數目大量增加，小范圍內粒子位置可基本覆蓋正確位置，在進行當前時刻位置計算時可以不用計算高斯分布，直接用最佳粒子位置代替機器人位置。

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

2　局部建圖算法

局部建圖算法的核心思想是：在地圖的靜態區域仍使用原有AMCL算法，機器人行駛至地圖發生變化的區域時使用GMAPPING算法進行局部實時建圖，并將新建地圖與原地圖進行重合，機器人駛出后恢復使用原地圖。它融合了GMAPPING算法與AMCL算法的優點，既實現了較為方便的路徑規劃，又滿足了實時建圖的需求。

2.1　算法分析

局部建圖算法的關鍵點在于如何實現新建地圖與原地圖的重合。新建地圖后，機器人所獲得的自身坐標是相對于新圖的坐標，只有將這個坐標轉換到舊圖坐標上才能保證之前設定的路徑規劃等功能不受影響。

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

Δσ=sinr

(1)

即偏離1°誤差就可以達到1.74%，對于50 m誤差可以達到87 cm，這是實際應用中無法接受的。

該算法的主要誤差分為兩類，一類是算法本身的誤差，包括模型誤差、噪聲誤差、傳感器導致的誤差等，通常這些誤差較??；另一類是切換后變換關系導致的誤差，根據之前敘述，該算法對角度的敏感度非常高，雖然采取了一定的方法去解決，但是仍會出現不可避免的誤差。第二類誤差的量與機器人行駛距離成正比，是線性誤差。

2.2　算法步驟

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

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

(2) 選擇切換點，經過切換點后進入學習階段。

(3) 學習階段開啟GMAPPING建圖，但不輸出機器人位置，同時不關閉AMCL算法，將AMCL算法得到的機器人位置作為輸入給GMAPPING。用LS方法計算兩種坐標間的轉換關系，得到關聯式。

(4) 關閉AMCL算法，同時開啟GMAPPING輸出，利用關聯式將新輸出坐標轉換到舊地圖上。

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

3　實驗檢驗

根據第2節介紹，該算法的第一類誤差較小，通常為幾厘米，第二類誤差呈線性，在行駛距離較大時誤差較大。因此，在行駛距離較大時，該算法的整體誤差大致呈線性。可以通過測量機器人行駛一定距離后偏離正常位置的距離來驗證算法的可靠性。

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

該實驗場地因為道路兩旁綠化帶生長，現場環境與先驗地圖有較大偏差，機器人在正常巡檢過程中時常出現無法找到正確位置的情況，圖1是10次實驗中，機器人偏離正常位置的最大距離D。其中方塊代表使用AMCL算法，圓代表使用沒有學習階段的局部建圖算法。誤差達到1 m表示機器人丟失自身位置。

圖1　10次實驗機器人偏離正常位置最大距離

由圖1可以看出，在使用AMCL算法的情況下有7次機器人完全丟失位置，3次沒有丟失位置的情況也都出現了較大偏差。而使用局部建圖算法10次都完成了50 m的路程，但是在不搭建學習區域的情況下，最大誤差可以達到80 cm，難以滿足要求?？梢钥闯觯植拷▓D算法能夠滿足適應變化環境的要求，但在精度方面仍需提升。

圖2是采用不同的精度提升策略得到的行駛距離與誤差關系圖。X軸表示的是機器人行駛距離L，Y軸表示的是10次實驗下的平均偏差D。方塊代表的是不使用任何策略；圓點代表使用學習區域，但不用LS算法處理，而是計算相應坐標間的變換關系然后求平均；三角代表使用LS方法求解關聯式。

圖2　機器人行駛距離與平均偏差圖

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

4　結論

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

[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]. 制造業自動化, 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，FOX 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] 趙景堂,杜國明,李秀海.基于總體最小二乘法的二維坐標轉換方法[J].黑龍江工程學院學報,2015(1):21-22.