一種基于全局位置估計誤差的路標探索策略

馬健，俞揚

(南京大學計算機軟件新技術國家重點實驗室，江蘇南京 210023)

由于自主移動機器人在日常生活服務[1]、危險環境作業[2]、太空探索[3]等領域有廣泛的應用，對機器人的自主行走和環境感知的研究吸引了越來越多的研究者關注。路標的發現及其位置的精確估計自主移動機器人在環境中的定位起到關鍵作用。當路標的位置信息已知時，通過對視線范圍內路標的鑒別和距離的估計，機器人即可計算出自己的所在位置。因此如何有效地在未知環境中探索和估計路標的位置是自主移動機器人研究的重要問題。

在完全未知環境中自主探索和估計路標的方法主要基于同時定位與地圖構建(simultaneous localization and mapping，SLAM)技術。SLAM最先由Self和Cheeseman[4]提出，它迭代的通過路標信息修正自身位置，同時通過自身位置的估計來修正路標信息，使得在機器人的移動過程中能夠逐漸提高其自身位置和路標的估計精度。由于其重要的理論與應用價值，同時定位與地圖構建(SLAM)被認為是自主移動機器人實現真正自主運動的關鍵，是智能移動機器人進入人類日常生活的必要基礎，也成為近年來機器人和人工智能領域中一個非?；钴S的研究熱點[5-7]。

在以往的研究中，移動機器人在未知環境中游走，并在移動過程中使用SLAM技術對路標進行估計[8]。常用的游走方式包括隨機游走或啟發式的探索策略，目標是最大限度地、盡快地引導機器人向未探測區域運動，盡量避免在已探測區域內運動，從而盡快減少未探測的區域。然而，基于SLAM的路標估計方法需要機器人重復探測已探測過的環境來提高對自身位置和路標位置的估計精度[9]，過快移向未探測區域將使得已探測路標的精度過低，導致較大的定位誤差。

本文提出一種以全局定位誤差最小化為指導的探索策略，進一步提高路標位置的估計精度。本文中還將全局定位信息引入SLAM的位置估計中，有效地校正當移動變化過大時(例如速度發生變化或轉彎時)SLAM估計的誤差。在模擬環境中的初步實驗驗證了本文方法的有效性。

1 路徑規劃相關工作

1.1 同時定位與地圖構建相關算法

同時定位與地圖構建的一個重要支撐技術是概率統計學。以貝葉斯濾波器為基本原理，研究人員先后提出了擴展卡爾曼濾波的SLAM算法(EKFSLAM)[7]期望值最大化的SLAM算法(EMEKF)[8]粒子濾波和擴展卡爾曼濾波的SLAM算法(FastSLAM)[9]等。EKF-SLAM 算法是用高斯分布表示后驗概率的貝葉斯濾波[10]。該算法作了3個近似。首先，運動模型近似為線性的，在環境為靜態的條件下，地圖特征狀態不變，僅有機器人狀態發生變化。根據擴展卡爾曼濾波狀態方程線性的要求，機器人的非線性運動模型近似為一個帶有高斯噪聲的線性函數;其次，觀測模型近似為線性的。最后，初始的不確定量近似為高斯分布。EKF-SLAM方法已被廣泛地研究和應用。Dissanayake等[11]通過在路邊架設雷達點路標并人工駕駛室外移動汽車，驗證了卡爾曼濾波在同時定位與地圖構建方面的有效性。Castellanos等[12]利用擴展卡爾曼濾波研究室內環境的同時定位與地圖構建。悉尼大學的Williams等[13]將它應用于水下環境。

EM-SLAM算法是一種由最大似然估計法發展而來的統計算法，由Thrun等[14]提出。應用EM算法進行地圖構建的主要觀點是當機器人路徑已知時，確定地圖是相對簡單的;同樣，對于一張給定的地圖，確定機器人位姿的概率估計也相對簡單。求解時交替執行2個步驟，分別稱為期望步驟(expectation step，E步驟)和最大值化步驟(maximization step，M步驟).在E步驟中，根據當前最大可能性地圖估計各個時間點機器人位置的概率估計，在M步驟中，根據在E步驟所計算得到的位置估計最大可能性地圖。通過不斷重復，機器人位置估計和地圖估計的精確性不斷提高。

針對EKF-SLAM方法在單峰高斯假設下難以解決數據對應問題、無法構建環行環境地圖的缺陷，以及EM-SLAM方法需要多次遍歷數據集、難以遞增地構建環境地圖的缺陷，Hahnel等[15]提出了基于粒子濾波和卡爾曼濾波的混合SLAM方法，也稱為FastSLAM。FastSLAM算法的基本思想是通過樣本集合表示概率密度。每一個樣本是關于狀態真值的一個離散猜測，一組樣本描述出概率分布的代表性特性。這種基于樣本的表示方法使得粒子濾波可以表示各種概率密度，適用于在線的、非線性、非高斯分布的實時估計。FastSLAM面向路標特征地圖，將機器人位姿估計和地圖特征估計相分離，用粒子樣本描述機器人路徑的概率分布，每一個粒子對應于一個可能的機器人路徑，每個粒子中維護著一張地圖，以該粒子所對應的機器人路徑為條件，利用擴展卡爾曼濾波估計地圖中各個特征的高斯分布。通過將粒子濾波和擴展卡爾曼濾波的有機結合，FastSLAM極大地減少了粒子需求數[10]。

1.2 室內未知環境的路徑規劃

室內未知環境的路徑規劃算法可分為3類:1)隨機遍歷策略，如迂回往復式、內外螺旋式[18];2)沿邊學習加局部路徑規劃方法，主要應用算法為細胞分解法，應用的局部路徑規劃方法有人工勢場法、完全應激式算法、模糊邏輯算法[17];3)漫步式探測路徑規劃[19]，機器人根據自身傳感器探測周圍環境信息，并在可視區域根據一定的算法生成局部運動的規劃目標，將機器人的路徑規劃歸結為逐步尋找下一步最佳視點的遞歸。采用此種策略的完全遍歷規劃方法有強化學習方法、基于隨機樹結構的方法、生物激勵的神經網絡方法、探測路徑規劃[13]。

隨機遍歷策略有迂回往復式、內外螺旋式、隨機轉向式、時間轉向式、模板模型法、隨機局部遍歷的方法。這些方法的共同點是:1)不采用現在通用的效益函數;2)規劃算法簡單，方便低成本軟硬件設計實現?？偟膩碚f，在不采用效益評價函數，如工作時間、能量損耗、重復覆蓋率等的前提下，可以達到長時間上的大范圍覆蓋未知環境。簡單的隨機遍歷策略廣泛應用于清潔機器人，如科沃斯地寶系列機器人。迂回往復法也經常作為其他算法的底層策略，或算法判斷后所采取的方法[16]。

沿邊學習加局部路徑規劃方法中機器人在沿邊學習的過程中可以獲取室內未知環境的全局信息進行建模，而后采用全局視角和局部路徑規劃相結合的方法，遍歷整個室內的未知環境。其思想是機器人首先沿著未知室內環境邊沿行走一周，然后選擇某個位置向四周觀察，確定合適的探測區域前進，并以此循環下去，典型的算法有MTCP[17]算法。進行局部路徑規劃時，機器人可以建立虛擬子目標，主要應用算法為細胞分解法，該方法在機器人進行沿邊學習之后，算法將環境分解為多個細胞，每個細胞設置一個基點，機器人走遍了基點則完成了遍歷。人工勢場法、完全應激式算法、模糊邏輯算法以及啟發式函數也可以作為路徑評價的判斷標準。

雖然沿邊學習方法可以獲得部分環境信息，但是由于一方面此方法需要對環境進行建模，另一方面傳感器信息不完全，因此該方法有局限性。而漫步式路徑規劃算法則可以避免這些缺點，其方法有強化學習法、基于隨機樹結構的方法、基于生物激勵的神經網絡方法和探測路徑規劃方法。

對于探測路徑規劃方法，其規劃序列為首先選擇一組下一步觀測視點的候選位置，然后通過效用函數評估這些候選位置，選擇使效用函數最大化的位置作為下一個觀測視點。探測路徑規劃主要包括2個方面:候選視點的生成和評價。在機器人進行探測時，首先應當考慮的是信息收益和路徑成本。

候選點的生成算法一般為前沿(Frontier)理論、隨機生成候選點理論以及這2種方法的混合算法。前沿理論基本思想是將下一步探測視點放置在已探測環境與未探測環境的交界線上，從而期望獲得最大化信息收益。隨機生成的方法是在傳感器掃描區域的圓或者圓環上以隨機的方式生成一定數量的候選視點。對候選視點的評價中，信息收益是一項重要指標，一般有2種方法進行衡量:一種是直接衡量傳感器探測空間的未知區域的幾何信息法;另一種是采用熵的概率信息法[16]。此外，路徑成本也是效益評價函數里的常見參數，如何衡量定位不確定性，成為了目前研究的熱點。

2 本文方法

2.1 基于路標的全局位置估計探索策略

本文提出的室內未知環境機器人路徑規劃方法屬于漫步式探測路徑規劃，其目的是最小化全局定位誤差。機器人從室內某一初始位置出發后先隨機游走直至發現一定數量的路標，而后選擇下一步的探測路徑。本文的思想是首先選擇一組下一步觀測視點的候選位置，然后通過效益評價函數評估這些候選位置，選擇使效益評價函數最大化的位置作為下一個觀測位置。由于在整個可達空間中搜索下一個最佳探測規劃位置意味著巨大的計算量，為減少計算量，一般對候選探測位置作一定的約束以提高計算效率。因此，探測規劃研究的討論主要集中在2個方面，一個是候選位置的生成，另一個是候選位置的評估。

本文方法將以當前自主移動機器人所在位置為圓心、r為半徑的圓上隨機生成的N個可達空間內的位置作為候選視點，而后分別預估自主移動機器人在這N個候選位置對應的N條路徑上可能觀測到的新路標的個數及位置、新路標的個數與該條路徑預估新探測的環境面積的比例、已經探測的路標個數、已經探測的環境面積的比例。新探測的路標的位置則用隨機的方法進行估計。候選位置的評價采用效用函數對候選視點進行量化評估，該方法考慮了全局定位誤差最小化，并對較大轉彎角度進行懲罰，以使機器人能夠在降低全局定位誤差的同時較為平滑地游走。該效用函數為

式(1)的目標函數表示全局定位誤差，其中i表示需要估計位置的路標，m表示環境中需要估計位置的路標的個數，α表示機器人一步轉彎的角度，est_again_location(i)表示結合全局位置估計和卡爾曼濾波的SLAM算法得到的第i個路標的估計坐標，est_location(i)表示上次得到的第i個路標的估計坐標，這2個函數的具體估計方法見3.2小節。punish(α)表示轉彎角度的懲罰項，本文簡單地用機器人轉彎時的角度的弧度制大小表示。

機器人一步優化路徑規劃的算法流程如下:

1)以預測的機器人所在位置為圓心、R為半徑的圓上隨機找N個在面板范圍內的點;

2)分別預估機器人在這N條路徑上會觀測到的新路標的個數及位置;

3)通過模擬機器人在這N條路徑上游走，分別計算每條路徑對應的效用函數值;

4)選取效用函數值最小路徑為下一步探測路徑。

2.2 全局位置估計和卡爾曼濾波的SLAM算法

在傳統的SLAM問題的相關的算法中，SLAM是一個濾波問題，是根據系統的初始狀態和從0到t時刻的觀測信息與控制信息估計系統的當前狀態?？柭鼮V波算法就屬于這種思想，卡爾曼濾波器已經被廣泛認為是實現SLAM的一種基本方法。然而，在實際情況中，當移動機器人移動變化過大時(例如速度發生變化或轉彎時)，使用傳統的SLAM相關算法，機器人的位置和真實的位置的誤差會比較大，機器人轉彎越急誤差可能越大?；诖吮疚奶岢鼋Y合全局位置估計和卡爾曼濾波的SLAM算法，由于全局定位信息更少地依賴歷史運動軌跡，與卡爾曼濾波等傳統結合使用不僅可以減小機器人游走時的估計誤差，而且可以有效降低機器人在轉彎時估計的誤差。

不失一般性，本文假設機器人所在的真實物理位置為 (xi，yi)，i∈ {1，2，...，n}，選取的已觀測到的路標的真實位置為(xti，yti)，路標的估計位置為(xsi，ysi)，假設需要估計的機器人的位置為(xs，ys)，優化的目標函數為

記機器人與第i個路標的真實距離為

這樣，本文有n個這樣的式子，分別將第i個式子減去第n個式子得到n－1個如下的等式:

這是一個最小二乘問題，記A為n－1個式(3)的右邊部分，即n－1個[2(xsi－xsn)2(ysi－ysn)]，則A為(n－1)×2的矩陣。記x為所求的[xsys]T，B為n－1個式(4)的左邊部分，即B為(n－1)×1的矩陣。需要估計的機器人的位置的最小二乘的最后結果 x'= [xs'，ys']T=(ATA)－1ATB 。在機器人每一步行走過程中，本文用上述方法得出的估計位置和卡爾曼濾波得出的估計位置的平均值作為機器人最終的估計位置。

3 仿真實驗

3.1 實驗設置

本文對2種規劃方法做了對比仿真，實驗隨機選擇候選點運動和一步最優規劃算法。機器人運動模型為本實驗室全方位移動機器人模型，假設其可以感知半徑10 m范圍內的路標.環境大小為80 m×80 m的正方形，其中隨機分布了30個路標，游走前機器人位于坐標(30 m，20 m)處，與x軸正方向成45o夾角，2種游走策略下自主移動機器人運行步數都是1 000步。對于本文的方法，在環境中均勻選取100個點，以估計全局誤差。

3.2 實驗結果

圖1給出了不同探索策略的機器人從相同位置和角度出發在相等的運行步數下生成的運動路徑。

圖1 運動軌跡與探索的路標結果對比Fig.1 The comparison of trajectory and road signs

其中，略大的點表示環境中的路標，虛線表示自主移動機器人真實的游走路線，實線表示自主移動機器人分別使用隨機游走策略和本文的方法的游走路線，略小的點表示估計的路標的位置，圓圈表示算法預計的路標的估計誤差。實驗結果表明:1)使用本文的游走策略探測到的路標更多，本文的游走策略未探測的路標有6個，而使用隨機游走后未探測到的路標有13個;2)在觀測到的路標上，使用隨機游走策略的最大誤差更大;3)使用本文策略后自主移動機器人游走的路線更平滑，在機器人位置的估計問題上，本文采用新算法后位置估計的誤差有較為顯著的減小。

3.2.1 全局路標定位誤差比較

圖2給出了自主移動機器人2種游走策略下的全局位置估計誤差的比較結果。其中橫坐標表示的是機器人游走的步數，縱坐標表示2種游走策略對應的在每一步的全局位置估計誤差。由于機器人游走開始時會隨機游走一段至發現一定數量的路標，所以橫坐標不是從0開始，而是從發現一定數量的路標的時刻180開始的。機器人隨機選擇候選點的全局誤差的均值和方差分別是1.821 2和1.062 4，而機器人采用基于路標的全局位置估計探索策略后的全局誤差的均值和方差分別是 1.532 0和0.943 2。

圖2 2種游走策略下的全局位置估計誤差比較結果Fig.2 The global position estimation error under two kinds of migration strategy

3.2.2 自主移動機器人自身定位誤差比較

圖3給出了卡爾曼濾波的SLAM算法和結合全局位置估計、卡爾曼濾波的SLAM算法在機器人使用本文的游走策略時每一步的機器人定位誤差的比較結果。其中橫坐標表示的是機器人游走的步數，縱坐標表示每種算法在每一步對機器人自身位置估計誤差的平均值。從圖中可以看出在機器人游走初期，機器人探測到的路標不多，基于卡爾曼濾波的SLAM算法產生的誤差和本文的方法的位置估計誤差差別不大。同時，全局位置估計的誤差變化較大，從圖中可以看出，自主移動機器人在180～500步之間位置估計誤差抖動較大。但隨著機器人游走步數的增大，機器人探測到的路標增多，本文方法產生的誤差明顯減小。圖中誤差圖有峰值出現，出現的原因是由于機器人轉彎時的誤差比平時大，從圖中可以看出本文方法在機器人轉彎時的誤差比卡爾曼濾波的SLAM算法產生的誤差明顯小?？柭鼮V波的SLAM算法產生的誤差的均值和方差分別為1.046 7和0.842 7，結合全局位置估計和卡爾曼濾波的SLAM算法產生的誤差的均值和方差分別為0.791 0 和0.409 1。

圖3 2種SLAM算法的機器人定位誤差比較Fig.3 The robot positioning error under two kinds of SLAM algorithm

3.2.3 2種游走策略下移動機器人耗時的比較

由于移動機器人采用基于路標的全局位置估計探索策略后，每次轉彎等動作時需要較多的時間規劃下一步的游走路徑，所以相對于隨機游走，移動機器人游走的耗時增加。實驗表明移動機器人采用路標的全局位置估計探索策略時耗時為237.588 2 s，而移動機器人隨機游走時耗時為81.959 9 s。

實驗結果表明，相對于卡爾曼濾波的SLAM算法，采用結合全局位置估計和卡爾曼濾波的SLAM算法后機器人自身位置估計的精度有明顯提高，但耗時相對增加。改變初始條件，多次實驗，每次的誤差數據不盡相同，但得到的結論相同。

4 結束語

采用基于路標的全局位置估計探索策略后，相對于機器人隨機游走，機器人的全局定位誤差有了較為明顯的減小。結合全局位置估計和卡爾曼濾波的SLAM算法相對于基于卡爾曼濾波的SLAM算法，可以有效地減小機器人位置的估計誤差。更多的實驗分析以及機器人多步優化路徑規劃問題等，值得進一步研究。

[1]DURRANT-WHYTE H F.An autonomous guided vehicle for cargo handling applications[J].International Journal of Robotics Research，1996，15(5):407-440.

[2]MONTEMERLO M，PINEAU J，ROY N，et al.Experiences with a mobile robotic guide for the elderly[C]//Proceedings of the 18th National Conference on Artificial Intelligence.Edmonton，Canada.2002:587-592.

[3]HUNTSBERGER T，AGHAZARIAN H，CHENG Y，et al.Rover autonomy for long range navigation and science data acquisition on planetary surfaces[C]//Proceedings of IEEE International Conference on Roboticsand Automation.Washington，DC，2002:3161-3168.

[4]SMITH R，SELF M，Cheeseman P.Estimating uncertain spatial relationships in robotics[M].Autonomous Robot Vehicles，1990:167-193.

[5]LI C，HUANG Y，KANG Y，et al.Monocular SLAM using verticalstraightlineswith inverse-depth representation[C]//Proceedings of 7th IEEE World Congress on Intelligent Control and Automation.Chongqing，China，2008:3015-3020.

[6]THRUN S，BURGARD W，FOX D.A probabilistic approach to concurrent mapping and localization for mobile robots[J].Autonomous Robots，1998，5(3/4):253-271.

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

[8]張恒，樊曉平.移動機器人同步定位與地圖構建過程中的軌跡規劃研究[J].機器人，2006，28(3):285-290.ZHANG Heng，FAN Xiaoping.Mobile robot trajectory planning in simultaneous localization and mapping problem[J].Robot，2006，28(3):285-290.

[9]NEWMAN P.On the structure and solution of the simultaneous localisation and map building problem[D].Sydney:University of Sydney，1999:1-5.

[10]熊蓉.室內未知環境線段特征地圖構建[D].杭州:浙江大學，2009:10-23.XIONG Rong.Line feature map building for unknown indoor environments[D].Hangzhou:Zhejiang University，2009:10-23.

[11]DISSANAYAKE M W M G，NEWMAN P，CLARK S，et al.A solution to the simultaneous localization and map building(SLAM)problem[J].IEEE Transactions on Robotics and Automation，2001，17(3):229-241.

[12]CASTELLANOS J A，MONTIEL J M M，NEIRA J，et al.The SPmap:A probabilistic framework for simultaneous localization and map building[J].IEEE Transactions on Robotics and Automation，1999，15(5):948-952.

[13]WILLIAMS S，DISSANAYAKE G，DURRANT-WHYTE H F.Towards terrain-aided navigation for underwater robotics[J].Advanced Robotics，2001，15(5):533-549.

[14]THRUN S.Probabilistic algorithms in robotics[J].AI Magazine，2000，21(4):93-109.

[15]HAHNEL D，BURGARD W，FOX D，et al.An efficient FastSLAM algorithm for generating maps of large-scale cyclic environments from raw laser range measurements[C]//Proceedings of 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems.Las Vegas，NV，2003:206-211.

[16]李一波，張慶濤.室內未知環境遍歷路徑規劃算法綜

述[J].計算機科學，2012，39(11A):334-338.

LI Yibo，ZHANG Qingtao.Review of coverage path planning arithmetic in unknown indoor environment[J].Computer Science，2012，39(11A):334-338.

[17]ULRICH I，MONDADA F，NICOUD J D.Autonomous vacuum cleaner[J].Robotics and Autonomous Systems，1997，19(3):233-245.

[18]OH J S，PARK J B，CHOI Y H.Complete coverage navigation of clean robot based on triangular cell map[J].IEEE Transactions on Industrial Electronics，2004，51(3):718-726.

[19]JEBARI I，BAZEILLE S，FILLIAT D.Informatics in control，automation and robotics[M].Berlin:Springer，2012:224-237.