基于智能空間的變電站機器人復合全局定位系統(tǒng)設計?

梁林勛，楊俊杰，樓志斌

（1．上海電力學院 電子與信息工程學院，上海200090；2．上?？茖W院，上海201203）

0 引 言

隨著智能電網(wǎng)［1］及機器人技術的發(fā)展，變電站作為整個電網(wǎng)體系的重要一環(huán)，為確保變電站實現(xiàn)無人值守、安全穩(wěn)定的運行，變電站巡檢機器人成為了各國研究的熱點。其中，精確的自主定位與導航是巡檢機器人實現(xiàn)一切服務的前提，通常，機器人定位問題可分為兩種：全局定位［2］與位置跟蹤。全局定位是指機器人在缺乏有效的初始位姿先驗知識前提下，通過不確定的傳感器觀測來確定位姿的過程，是后續(xù)位置跟蹤的基礎，主要用于處理機器人啟動時位置初始化及移動過程中機器人綁架等問題。

卡爾曼濾波［3］（Kalman Filter， KF）和基于蒙特卡洛的粒子濾波［4］（Particle Filter， PF）是目前廣泛使用的全局定位方法。文獻［5］采用基于卡爾曼濾波的組合定位導航算法，通過DR推算與LMS高精度激光雷達組合濾波，提高了強電磁環(huán)境下變電站機器人定位系統(tǒng)的穩(wěn)定性與準確性；文獻［6］提出了一種與遺傳算法結合的自適應進化粒子濾波算法，通過引入自適應控制參數(shù)控制重采樣次數(shù)，有效提升了定位精度；文獻［7］提出了一種聯(lián)合蒙特卡羅與改進粒子濾波的IUPF-MCL算法，該方法通過迭代Sigma點KF濾波來設計粒子濾波提議分布以改善濾波退化問題，最后利用MCL算法實現(xiàn)機器人精確定位。相較于卡爾曼濾波，粒子濾波更適合處理復雜的非線性非高斯問題，因而在機器人全局定位與地圖構建中廣泛使用，但在變電站這樣的室外大范圍環(huán)境下，全局粒子濾波定位通常又存在以下問題：（1）由于環(huán)境較大，全局定位通常需要較多的粒子數(shù)，而粒子數(shù)與存儲空間和計算復雜度密切相關；（2）若地圖過大且粒子數(shù)有限情況下，粒子稀疏會造成定位精度的下降甚至失??；（3）隨著算法迭代，不可避免的會產(chǎn)生粒子退化問題［8］，而重采樣方法的引入又會導致粒子多樣性的喪失，從而加劇算法發(fā)散的可能。

針對以上問題，本文將智能空間［9］與機器人技術相結合，提出一種智能空間下基于ZigBee指紋定位與PSO-PF的復合全局定位方法。智能空間是一個嵌入了計算、信息設備和多模態(tài)傳感器的空間系統(tǒng)，通過設備間的交互與分布式計算，統(tǒng)一管理空間中的各種資源，為用戶提供方便的服務，與機器人結合的思想是將感知、執(zhí)行器件分布地安裝在空間相應位置，從而能夠?qū)崿F(xiàn)對空間的全面感知并為機器人提供更完備的信息，降低機器人對于本體傳感器的要求。因此，該方法首先通過UPnP（Universal Plug and Play）技術將機器人接入智能空間，利用智能空間中的BP神經(jīng)網(wǎng)絡進行ZigBee指紋初定位，然后機器人結合ZigBee定位結果，在誤差范圍進行粒子撒點，將全局定位問題轉化為局部搜索問題，利用粒子群優(yōu)化算法將最新觀測信息引入粒子濾波采樣分布中，改善樣本后驗概率分布，從而完成定位過程。實驗結果表明，該方法可有效提高變電站機器人全局定位精度，改善算法性能并縮短迭代時間，具有一定可行性。

1 機器人全局定位系統(tǒng)框架

如圖1所示，機器人復合全局定位系統(tǒng)主要由Zig-Bee空間感知單元、智能空間調(diào)度系統(tǒng)和ROS變電站機器人組成。其中，空間感知單元主要負責各節(jié)點RSSI強度的采集并通過Wifi網(wǎng)絡轉發(fā)給智能空間調(diào)度系統(tǒng)；智能空間調(diào)度系統(tǒng)負責全局系統(tǒng)管理，協(xié)調(diào)網(wǎng)絡通信以及利用神經(jīng)網(wǎng)絡模塊進行初步位置估計；機器人平臺接收到粗略定位結果后初始化粒子撒點范圍，結合觀測信息進行粒子濾波完成定位過程。

圖1 機器人定位系統(tǒng)框架Fig．1 Framework of robot localization system

1．1 空間感知模塊

ZigBee空間感知單元主要在智能空間中進行各參考節(jié)點 RSSI強度采集、預處理、發(fā)送等任務，由CC2530模塊和控制器組成。其中，CC2530模塊將掃描到的節(jié)點及其信號強度通過RS232發(fā)送給控制器；控制器負責感知模塊初始化以及信息預處理，并通過接入智能空間的Wifi網(wǎng)絡將信息上傳到神經(jīng)網(wǎng)絡處理模塊，完成ZigBee指紋初定位過程。

1．2 智能空間系統(tǒng)

智能空間調(diào)度系統(tǒng)主要由ZigBee指紋數(shù)據(jù)庫、神經(jīng)網(wǎng)絡模塊以及調(diào)度系統(tǒng)組成，以BP神經(jīng)網(wǎng)絡為核心，采用建立的ZigBee指紋庫進行訓練，通過對接收到的指紋信息并行處理得到初步的位置估計，同時負責設備與信息流管理，響應ROS機器人接入請求并將指紋定位結果通過initialpose消息發(fā)送回機器人本體，初始化位姿估計并完成全局定位。

1．3 機器人平臺

本文采用自主開發(fā)的ROS機器人平臺，ROS［10］（Robot Operating System）是一種開源的機器人操作系統(tǒng)，它提供部分標準系統(tǒng)級服務，以其較好的硬件抽象及常用庫集成的特點，近年來受到了廣泛的關注。

如圖2所示，在ROS機器人中，全局定位系統(tǒng)主要由以下幾部分組成，其中，UPnP為機器人設備抽象節(jié)點，它通過UPnP協(xié)議和XML文件將機器人描述成具有統(tǒng)一接口的抽象設備，并將設備和服務描述文檔廣播出去，當機器人接入智能空間網(wǎng)絡時，智能空間調(diào)度模塊即可以發(fā)現(xiàn)并調(diào)用相應服務，從而實現(xiàn)與智能空間的零配置與松耦合；Odom和Laserscan節(jié)點分別負責里程計和激光數(shù)據(jù)的采集和發(fā)布，通過ROS標準通信框架，實現(xiàn)模塊間數(shù)據(jù)共享；map_server提供地圖服務，負責加載已生成的環(huán)境地圖并發(fā)布map數(shù)據(jù)；Location節(jié)點通過initialpose主題初始化粒子撒點范圍，融合里程計、激光觀測以及地圖數(shù)據(jù)通過粒子群優(yōu)化的粒子濾波進行位姿估計，然后發(fā)布estimatePose主題，通過調(diào)用move_base完成系統(tǒng)定位與導航。

圖2 機器人軟件體系描述Fig．2 Software architecture description of robot

2 復合全局定位算法設計

基于智能空間ZigBee指紋與PSO-PF的復合全局定位算法主要分為兩步：首先，采集智能空間中ZigBee指紋，利用建立的BP神經(jīng)網(wǎng)絡進行粗略估計；然后在誤差范圍內(nèi)初始化粒子撒點范圍，利用粒子群優(yōu)化的粒子濾波融合傳感器觀測以及地圖數(shù)據(jù)進一步得到機器人的精確位姿。

2．1 智能空間ZigBee指紋定位

ZigBee是基于IEEE802．15．4標準的低功耗局域網(wǎng)協(xié)議，由于ZigBee特有的優(yōu)點，在電力等工業(yè)領域得到了廣泛應用，其定位方法多種多樣，以TOF、RSSI和位置指紋法最為常見。RSSI定位［11］是依據(jù)節(jié)點接收到的信號強度（RSSI），計算信號傳播過程中的能量損耗，通過信號衰減理論模型轉化為傳播距離；指紋定位［12］是基于與位置相關的RSSI指紋，通過與建立的指紋庫匹配得到盲節(jié)點的位置，一般分為兩個階段：離線階段和在線階段。離線階段根據(jù)環(huán)境大小，選取一組位置作為采樣點，通過采集參考節(jié)點的RSSI指紋建立空間指紋庫；在線階段通過采集的各參考節(jié)點信號強度與指紋庫匹配，從而估計出盲節(jié)點位置。BP神經(jīng)網(wǎng)絡［13］是一種誤差逆向傳播的多層前饋神經(jīng)網(wǎng)絡，一般由輸入層、隱含層（至少一層）和輸出層組成，理論上，一個3層的BP網(wǎng)絡可以逼近任意非線性系統(tǒng)。由于位置指紋法簡單易行，同時BP網(wǎng)絡具有較好的容錯性和泛化自學習能力，因此指紋定位的離線學習及在線匹配過程可以采用BP網(wǎng)絡來實現(xiàn)。

假設BP網(wǎng)絡輸入數(shù)據(jù)為各參考節(jié)點RSSI強度，輸出為對應采樣點坐標，則輸入和期望輸出向量為：

其中，xi和oj分別為此時第i個參考節(jié)點RSSI強度和坐標向量第j個分量，m、n分別為參考節(jié)點總數(shù)和坐標分量個數(shù)。

BP網(wǎng)絡具體訓練過程如下：

Step1：初始階段，初始化BP網(wǎng)絡各神經(jīng)元間權重及閾值，并將輸入輸出向量歸一化。

Step2：正向傳播過程，假設訓練過程中第k組樣本輸入為X（k），節(jié)點i和節(jié)點j之間權重為wij，節(jié)點j閾值為θi，則隱含層與輸出層節(jié)點j狀態(tài)為：

其中，f（·）為激活函數(shù)，以Sigmoid函數(shù)為例：

Step3：誤差反向修正，誤差e以實際輸出yi與期望輸出oi之間的均方差表示：

若學習誤差e小于給定的閾值ε，訓練結束；否則根據(jù)梯度下降原則反向調(diào)節(jié)各層節(jié)點權重及閾值進行誤差修正：

其中，k為學習次數(shù)，η為學習因子。然后重新輸入樣本訓練，直至BP網(wǎng)絡誤差小于給定的閾值，算法結束。

2．2 PSO優(yōu)化的粒子濾波定位

上述基于BP網(wǎng)絡定位的結果具有較大誤差，一般難以滿足機器人定位要求，因此需要在此基礎上融合激光觀測、里程計及地圖信息進一步確定機器人的精確位置。

粒 子 群 算 法［14］（Particle Swarm Optimization，PSO）是一種基于群智能的仿生優(yōu)化算法，其基本思想是通過群體中大量簡單個體的協(xié)作與信息共享來尋找最優(yōu)解。假設粒子為D維空間中一批隨機分布的點，每個PSO粒子都具有兩個初始參數(shù)：速度與位置。算法實質(zhì)是通過空間中大量粒子的飛行，不斷搜索群體中個體極值和全局極值，然后追隨全局極值以速度與位置公式更新粒子下一時刻運動狀態(tài)，并逐漸使整個群體逼近于最優(yōu)目標的。與粒子濾波相比，二者具有相似的機理，因此可以將兩種算法融合，通過粒子群算法良好的全局尋優(yōu)能力，使粒子濾波粒子集在權重更新前更逼近于真實分布，從而使得粒子權重普遍增大，降低了粒子間“貧富”差距，使濾波過程中有效的粒子數(shù)目增多，同時在重采樣過程中減少了由于權重低而舍棄的粒子數(shù)目，從而改善了樣本貧化現(xiàn)象，進一步提高了全局定位算法的穩(wěn)定性與魯棒性。

基于PSO優(yōu)化的粒子濾波定位過程如下：

（1）初始狀態(tài)。

根據(jù)上述指紋定位結果，在誤差范圍內(nèi)隨機撒點以模擬真實的后驗概率分布，將常規(guī)全局濾波問題轉化為局部濾波問題，降低粒子搜索代價。

（2）轉移階段。

粒子濾波預測過程中，每一個粒子都表示一個機器人可能的狀態(tài)，機器人下一時刻位姿預測可以通過運動模型獲得：

其中，xit-1為t-1時刻第i個粒子狀態(tài)，ut-1為t-1時刻控制輸入量。

（3）粒子速度-位置更新。

對預測后的粒子狀態(tài)使用速度與位置公式進行更新：

其中，c1、c2為學習因子，Rand為對角矩陣，其對角線元素為符合高斯分布的隨機數(shù)，pp和pg分別表示機器人位置預測的個體極值與全局最優(yōu)解。

（4）適應度評價。

引入適應度函數(shù)F對（3）中的粒子進行評價：

其中，RK為測量噪聲協(xié)方差矩陣，ZtN和ZtP分別為路標實際觀測和狀態(tài)預測值。

設定參考閾值σ，若F遠大于σ，則表示粒子集分布在真實狀態(tài)附近，可以停止優(yōu)化；若F＜σ，則表明粒子適應度普遍較低，整體狀態(tài)不符合真實概率分布，應不斷使用式（9）、式（10）更新粒子位置與速度，驅(qū)動粒子集向高似然區(qū)域運動。

（5）決策階段。

結合最新路標觀測及地圖數(shù)據(jù)，計算粒子權重并歸一化：

（6）重采樣。

為了解決粒子退化問題，粒子集權重歸一化后，以權重最大的粒子作為機器人最終位置估計；并依據(jù)權重大小選取較大的粒子復制添加到新的粒子集中，然后返回（2）進行下一階段的粒子濾波。

3 實驗及結果分析

本文以實驗室自主開發(fā)的ROS變電站機器人為平臺，在布有ZigBee節(jié)點的室內(nèi)模擬變電站環(huán)境進行實驗。如圖3所示，搭載的傳感器主要有2 000線增量式光電編碼器，ZigBee CC2530模塊以及RoboPeak團隊開發(fā)的低成本2D RPLidar A2M8激光雷達；軟件平臺采用基于Ubuntu 16．04的ROS Kinetic系統(tǒng)；室內(nèi)環(huán)境大小約為 40×16m，地圖分辨率 0．05。

圖3 ROS機器人移動平臺Fig．3 Robot mobile platform based on ROS

為了驗證本文算法的有效性，本文以常規(guī)粒子濾波全局定位為參照進行比較。圖4為兩種算法對比情況，圖4（a）、圖4（b）分別為常規(guī) PF全局定位與本文算法的初始粒子集分布狀態(tài)，圖中可以看出，由于本算法ZigBee初定位結果的引入，將全局定位問題轉化為了局部定位問題，粒子撒點范圍大大縮小；圖4（c）、圖4（d）分別為兩種算法迭代50次后最終位置估計情況，相較而言，本算法位置估計更接近于真實位置，具體誤差隨迭代次數(shù)變化曲線如圖5所示。

圖5（a）、圖5（b）分別為常規(guī) PF定位與本算法定位誤差隨迭代次數(shù)的變化曲線，可以看出最終本算法定位誤差約為0．5 m，優(yōu)于常規(guī)PF算法的1．6 m。

圖4 定位結果對比Fig．4 Comparison of localization results

圖5 定位誤差對比Fig．5 Comparison of localization error

為了進一步考察本算法在算法性能方面的改善情況，本文分別在初始粒子數(shù)目不同情況下，對兩種算法進行了檢驗。圖6（a）為常規(guī)PF算法定位誤差隨粒子數(shù)目變化曲線，圖6（b）為本文算法定位誤差與粒子數(shù)目關系曲線，圖中可以看出，粒子數(shù)目的多少是影響兩種算法定位精度的關鍵因素，在本文實驗環(huán)境下，本算法在粒子數(shù)目為800時精度已達到較高狀態(tài)，全局定位精度約為0．5 m；而常規(guī)PF算法在粒子數(shù)目為2 000時約為1．5 m，進一步說明了本算法可以有效減少迭代所需的粒子數(shù)，只需較少的粒子就可以達到較高的精度。

同時當粒子數(shù)均為2 000時，對兩種算法各迭代50次，所需時間如圖7所示，結合圖4、圖5，可以看出本算法由于ZigBee定位結果的引入，將全局粒子搜索問題轉化為局部問題，可以有效縮短迭代時間，加快算法運行速度。因此，通過以上與常規(guī)粒子濾波方法的對比說明，本文設計的算法可以有效的提高初始全局定位精度并改善算法性能，縮短迭代時間，具有一定可行性。

圖6 定位誤差-粒子數(shù)目關系Fig．6 Relation between localization error and particle number

圖7 迭代時間對比Fig．7 Comparison of iteration time

4 結束語

精確的定位導航是實現(xiàn)機器人自主移動的先決條件，本文針對巡檢機器人在缺乏有效的初始位姿先驗知識情況下全局定位的問題，結合變電站特點，提出了一種智能空間與ROS機器人相結合的復合全局定位系統(tǒng)。該方法首先在智能空間中利用 BP網(wǎng)絡進行ZigBee指紋初定位，在此基礎上結合激光觀測、里程計以及歷史地圖數(shù)據(jù)采用PSO優(yōu)化的粒子濾波精確定位，從而完成系統(tǒng)定位與導航。

實驗結果表明，該方法結合智能空間特點，增強了機器人定位系統(tǒng)的準確性與穩(wěn)定性，同時改善算法性能，提高了變電站機器人自主運行能力。目前采用單一機器人進行實驗，如何利用智能空間優(yōu)勢，并實現(xiàn)變電站多機器人協(xié)同定位，這將是需要深入研究的工作。