梁林勛，楊俊杰，樓志斌

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

0 引 言

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

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

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

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

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

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

1．1 空間感知模塊

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

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

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

1．3 機(jī)器人平臺(tái)

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

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

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

2 復(fù)合全局定位算法設(shè)計(jì)

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

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

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

假設(shè)BP網(wǎng)絡(luò)輸入數(shù)據(jù)為各參考節(jié)點(diǎn)RSSI強(qiáng)度，輸出為對(duì)應(yīng)采樣點(diǎn)坐標(biāo)，則輸入和期望輸出向量為：

其中，xi和oj分別為此時(shí)第i個(gè)參考節(jié)點(diǎn)RSSI強(qiáng)度和坐標(biāo)向量第j個(gè)分量，m、n分別為參考節(jié)點(diǎn)總數(shù)和坐標(biāo)分量個(gè)數(shù)。

BP網(wǎng)絡(luò)具體訓(xùn)練過(guò)程如下：

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

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

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

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

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

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

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

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

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

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

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

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

（2）轉(zhuǎn)移階段。

粒子濾波預(yù)測(cè)過(guò)程中，每一個(gè)粒子都表示一個(gè)機(jī)器人可能的狀態(tài)，機(jī)器人下一時(shí)刻位姿預(yù)測(cè)可以通過(guò)運(yùn)動(dòng)模型獲得：

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

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

對(duì)預(yù)測(cè)后的粒子狀態(tài)使用速度與位置公式進(jìn)行更新：

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

（4）適應(yīng)度評(píng)價(jià)。

引入適應(yīng)度函數(shù)F對(duì)（3）中的粒子進(jìn)行評(píng)價(jià)：

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

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

（5）決策階段。

結(jié)合最新路標(biāo)觀測(cè)及地圖數(shù)據(jù)，計(jì)算粒子權(quán)重并歸一化：

（6）重采樣。

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

3 實(shí)驗(yàn)及結(jié)果分析

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

圖3 ROS機(jī)器人移動(dòng)平臺(tái)Fig．3 Robot mobile platform based on ROS

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

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

圖4 定位結(jié)果對(duì)比Fig．4 Comparison of localization results

圖5 定位誤差對(duì)比Fig．5 Comparison of localization error

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

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

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

圖7 迭代時(shí)間對(duì)比Fig．7 Comparison of iteration time

4 結(jié)束語(yǔ)

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

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