基于無(wú)線傳感器網(wǎng)絡(luò)的未知環(huán)境下移動(dòng)機(jī)器人實(shí)時(shí)路徑規(guī)劃

摘 要：利用無(wú)線傳感器網(wǎng)絡(luò)技術(shù)、多傳感器信息融合技術(shù)、基于占用網(wǎng)格的概率地圖構(gòu)建和基于群集智能的微粒群仿生學(xué)算法，采用三層控制結(jié)構(gòu)的規(guī)劃策略，提出了一種新的在未知環(huán)境下移動(dòng)機(jī)器人在線實(shí)時(shí)路徑規(guī)劃方法。最后設(shè)計(jì)并構(gòu)造出了實(shí)際的無(wú)線傳感器網(wǎng)絡(luò)，驗(yàn)證了算法的有效性、準(zhǔn)確性和實(shí)時(shí)性。 

關(guān)鍵詞：無(wú)線傳感器網(wǎng)絡(luò)； 實(shí)時(shí)路徑規(guī)劃； 多傳感器融合； 微粒群算法； 群集智能； 概率地圖構(gòu)建

中圖分類號(hào)：TP24 文獻(xiàn)標(biāo)志碼：A

文章編號(hào)：1001-3695(2008)07-2029-04



Realtime path planning based on wireless sensor network

for mobile robots under unknown environment

XUE Han1，TAO Yi1，2，MA Hongxu1

(1.College of Electromechanical Engineering Automation， National University of Defense Technology， Changsha 410073， China；2.Wuhan Ordnance N.C.O. Academy of PLA，Wuhan 430075， China)

Abstract:Utilizing the advantages of wireless sensor network，multisensor fusion for building occupancy grid map using probabilistic sensor model and particle swarm algorithm using bionic swarm intelligence， an online realtime path planning algorithm for mobile nodes under unknown dynamic environment was proposed through three level planning control strategy. A practical implementation had been carried out to demonstrate the enhanced efficiency and accuracy of the proposed algorithm.

Key words:wireless sensor network(WSN); realtime path planning; multisensor fusion; particle swarm algorithm; swarm intelligence; probabilistic map building



無(wú)線傳感器網(wǎng)絡(luò)技術(shù)是將現(xiàn)代無(wú)線通信技術(shù)、微型傳感器技術(shù)和網(wǎng)絡(luò)技術(shù)有機(jī)地融合為一體的新型網(wǎng)絡(luò)技術(shù)，在國(guó)防、環(huán)境監(jiān)督、家庭自動(dòng)化、運(yùn)輸和其他許多領(lǐng)域具有廣闊的應(yīng)用前景和極高的應(yīng)用價(jià)值^[1]。

根據(jù)機(jī)器人對(duì)環(huán)境信息掌握的程度，移動(dòng)機(jī)器人路徑規(guī)劃可分為以下幾類: a)已知環(huán)境下對(duì)靜態(tài)障礙物的路徑規(guī)劃;b)已知環(huán)境下對(duì)動(dòng)態(tài)障礙物的路徑規(guī)劃;c)未知環(huán)境下對(duì)靜態(tài)障礙物的路徑規(guī)劃;d)未知環(huán)境下對(duì)動(dòng)態(tài)障礙物的路徑規(guī)劃。前兩種是基于先驗(yàn)已知環(huán)境模型完全信息的離線一次性全局規(guī)劃，找出從起始點(diǎn)到目標(biāo)的符合一定性能的可行或最優(yōu)路徑，涉及世界模型的表達(dá)和搜尋策略；然而適應(yīng)范圍相對(duì)有限、實(shí)時(shí)性差，不能處理環(huán)境改變或動(dòng)態(tài)障礙。后兩種是基于未知或部分未知的環(huán)境，通過(guò)傳感器實(shí)時(shí)探測(cè)障礙物的尺寸、形狀和位置，經(jīng)過(guò)多次局部規(guī)劃來(lái)得到可行的安全路徑；然而無(wú)法保證路徑的最優(yōu)，有時(shí)反應(yīng)速度不快，易陷入環(huán)境死區(qū)，對(duì)規(guī)劃系統(tǒng)品質(zhì)的要求較高。

作為一種新興的演化計(jì)算技術(shù)， 群集智能成為近年來(lái)人工智能領(lǐng)域新的研究熱點(diǎn)課題，吸引了大量國(guó)內(nèi)外學(xué)者。群集智能與人工生命特別是進(jìn)化策略和遺傳算法有特殊聯(lián)系， 已完成的理論和應(yīng)用研究證明，群集智能方法是一種能夠有效解決大多數(shù)全局隨機(jī)優(yōu)化問(wèn)題的新方法，已在許多領(lǐng)域得到應(yīng)用并取得較好效果，對(duì)傳統(tǒng)結(jié)構(gòu)優(yōu)化問(wèn)題提供了復(fù)雜的分布式解決方案。本文利用群集智能算法的自治性和突變性描述了基于群集智能計(jì)算的微粒群算法，很好地解決了路徑優(yōu)化問(wèn)題。 本文針對(duì)上述全局規(guī)劃和局部規(guī)劃的缺陷，將兩者有效結(jié)合，提出了一種基于WSN的在線實(shí)時(shí)路徑規(guī)劃新方法，不僅為移動(dòng)機(jī)器人的運(yùn)動(dòng)規(guī)劃提供了一種新的選擇，而且極大地推動(dòng)了WSN與移動(dòng)機(jī)器人的協(xié)作。

1 WSN與移動(dòng)機(jī)器人

WSN是由布置在監(jiān)測(cè)區(qū)域內(nèi)大量的廉價(jià)微型傳感器節(jié)點(diǎn)組成，通過(guò)無(wú)線通信方式形成的一個(gè)多跳的自組織的網(wǎng)絡(luò)系統(tǒng)，其目的是協(xié)作地感知、采集和處理網(wǎng)絡(luò)覆蓋區(qū)域中感知對(duì)象的信息，并發(fā)送給觀察者，如圖1所示。

但WSN數(shù)據(jù)處理和執(zhí)行力卻非常有限^[2]，一個(gè)顯著制約是受到嚴(yán)格的能量限制。移動(dòng)機(jī)器人能自動(dòng)部署和校準(zhǔn)傳感器，對(duì)傳感器的錯(cuò)誤和不合理配置進(jìn)行檢測(cè)和處理，對(duì)硬件或環(huán)境條件的變化作出動(dòng)態(tài)響應(yīng)，使分布式的無(wú)線傳感器網(wǎng)絡(luò)系統(tǒng)能在更長(zhǎng)的時(shí)間內(nèi)良好地持續(xù)運(yùn)作，維護(hù)系統(tǒng)整體健壯性。移動(dòng)機(jī)器人作為靈活的行動(dòng)執(zhí)行者，能給傳感器重新?lián)Q電池或者使用電路連接來(lái)充電，極大增加了無(wú)線節(jié)點(diǎn)的適應(yīng)性和機(jī)動(dòng)性，能被允許自由配置到周圍缺少充電基礎(chǔ)設(shè)施的環(huán)境。另外，移動(dòng)機(jī)器人有著強(qiáng)大的計(jì)算能力和移動(dòng)性，但其感知能力的局限性限制了其智能的發(fā)展。WSN不僅可以為移動(dòng)機(jī)器人提供對(duì)全局的實(shí)時(shí)感知能力，對(duì)環(huán)境進(jìn)行連續(xù)的、大范圍的監(jiān)測(cè)，還可以作為通信和計(jì)算的媒介，提高路徑最優(yōu)的能力。因此，集成了機(jī)器人的無(wú)線傳感器網(wǎng)絡(luò)系統(tǒng)，既增強(qiáng)移動(dòng)機(jī)器人的感知能力，也提高了傳感器網(wǎng)絡(luò)對(duì)環(huán)境的控制力。

2 問(wèn)題描述與建模

2．1 基于網(wǎng)格的地圖構(gòu)建

網(wǎng)格地圖是把機(jī)器人的工作環(huán)境離散化為正則網(wǎng)格，網(wǎng)格單元存儲(chǔ)該環(huán)境部分是否被占用的信度，稱為占用網(wǎng)格(occupancy grid)。占用網(wǎng)格在大規(guī)模的環(huán)境中容易構(gòu)建和維護(hù)。

設(shè)待測(cè)繪的未知區(qū)域D被分割為一系列網(wǎng)格D={（xi，yi）∈R2|i∈Γ}。其中Γ為網(wǎng)格索引的有限集合。每個(gè)(xi，yi)∈D代表的長(zhǎng)方形區(qū)域[xi，xi+1]×[yi，yi+1]R2是傳感信號(hào)能夠穿過(guò)的空域或者是使傳感信號(hào)反射的被占用域。圖2示意了一個(gè)長(zhǎng)方形區(qū)域D。其中被障礙物占用的網(wǎng)格用深色表示。

概率地圖由許多網(wǎng)格構(gòu)成，使用空間占用技術(shù)，輸入占用網(wǎng)格的值反映了每個(gè)網(wǎng)格被占用的情況和在環(huán)境的相應(yīng)區(qū)域中障礙物的存在情況。機(jī)器人通過(guò)傳感器獲得的環(huán)境信息和自定位信息使用融合算法建立占用地圖。地圖中每個(gè)網(wǎng)格的不同灰度表示不同的網(wǎng)格占用模糊隸屬度，網(wǎng)格顏色越深表示該網(wǎng)格被占用的信度越高。創(chuàng)建的不確定性概率地圖較好地反映了真實(shí)環(huán)境。

定義mO(Si)和mE(Si)分別為網(wǎng)格i被占用和為空的均值，滿足下式：

2．2 傳感器模型

根據(jù)車載傳感器的類型和能力，傳感器模型是從環(huán)境狀態(tài)到測(cè)量讀數(shù)的函數(shù)^[3]。環(huán)境主要由障礙物的配置情況和機(jī)器人的位置和方向構(gòu)成。對(duì)于占用度的表示法，需要用概率傳感器模型將不同的可能度量與環(huán)境配置關(guān)聯(lián)起來(lái)。當(dāng)測(cè)量從傳感器到障礙物的讀數(shù)時(shí)，由于諸如噪聲干擾或校正錯(cuò)誤等各種各樣的原因，傳感器的讀數(shù)不一定能真實(shí)反映真正的距離，需要概率傳感器模型將這些不確定性反映成讀數(shù)。圖3為傳感器模型。

3 路徑規(guī)劃算法

3．1 算法框架

本文采用一種新的三級(jí)結(jié)構(gòu)實(shí)現(xiàn)機(jī)器人的路徑規(guī)劃過(guò)程，在不同的層次機(jī)器人采用不同的信息進(jìn)行路徑規(guī)劃決策。決策層根據(jù)給定的外部命令進(jìn)行任務(wù)規(guī)劃，宏觀上把握機(jī)器人的整體運(yùn)行方向；中間層接收決策層傳來(lái)的命令，實(shí)時(shí)處理，為決策層提供相應(yīng)的細(xì)節(jié)處理過(guò)程，產(chǎn)生一系列可供底層執(zhí)行的動(dòng)作序列，并提供死鎖、有界、平均執(zhí)行時(shí)間等性能分析；底層最終執(zhí)行所要求完成的路徑規(guī)劃任務(wù)，對(duì)執(zhí)行結(jié)果進(jìn)行性能評(píng)價(jià)，并將評(píng)價(jià)結(jié)果逐級(jí)向上反映，起到學(xué)習(xí)的作用。

本算法采用基于多傳感器信息融合的概率地圖全局規(guī)劃，采用仿生智能微粒群算法的局部規(guī)劃。由于每次規(guī)劃都引入了全局概率地圖信息作為判據(jù)，避免了陷入局部死點(diǎn)的問(wèn)題；又由于利用已知的全局信息，采用了更為合理的路徑選擇策略，可以對(duì)路徑進(jìn)行優(yōu)化， 削減不必要的路徑段，能有效地利用局部規(guī)劃時(shí)間從而得出相對(duì)更好的路徑，而且還能及時(shí)處理所遇到的隨機(jī)障礙信息，從而提高機(jī)器人整體規(guī)劃性能。

3．2 基于多傳感器融合的地圖更新

移動(dòng)機(jī)器人在動(dòng)態(tài)環(huán)境中進(jìn)行路徑規(guī)劃所需的環(huán)境信息都是從傳感器收集得來(lái)。由于不確定誤差、環(huán)境影響、通信時(shí)延和傳輸錯(cuò)誤等，單傳感器難以保證輸入信息的準(zhǔn)確性和可靠性，很難對(duì)工作環(huán)境進(jìn)行精確的描述。而多傳感器所獲得的網(wǎng)格占用信息具有冗余性、互補(bǔ)性、實(shí)時(shí)性和低代價(jià)性，使機(jī)器人能夠在不斷感知環(huán)境的過(guò)程中更新地圖，可以有效地減弱傳感器誤差、剔除錯(cuò)誤讀數(shù)，快速準(zhǔn)確地并行分析現(xiàn)場(chǎng)環(huán)境。

移動(dòng)機(jī)器人利用無(wú)線傳感器網(wǎng)絡(luò)，實(shí)時(shí)探測(cè)收集局部環(huán)境信息，并根據(jù)新的環(huán)境信息頻繁地重新規(guī)劃和調(diào)整路徑。移動(dòng)機(jī)器人需維護(hù)環(huán)境的全局地圖，機(jī)器人根據(jù)當(dāng)前的地圖規(guī)劃出路徑；然后沿該路徑前進(jìn)一段時(shí)間，利用這段時(shí)間內(nèi)收集到的新信息更新全局地圖；再用更新過(guò)的全局地圖重新規(guī)劃或調(diào)整現(xiàn)有的路徑， 如此循環(huán)直至到達(dá)目標(biāo)位置。

貝葉斯理論^[4]、卡爾曼濾波^[5]、DempsterShafer理論^[6]和模糊邏輯算法^[7]是四種常用的應(yīng)用于移動(dòng)機(jī)器人路徑規(guī)劃的多傳感器信息融合方法。本文使用貝葉斯理論進(jìn)行信息融合，根據(jù)傳感器與被測(cè)量網(wǎng)格的距離，使用貝葉斯條件概率確定更新的占用概率。

3．3 局部最優(yōu)搜索

機(jī)器人需要根據(jù)全局地圖中各個(gè)節(jié)點(diǎn)的連通性搜索出從起始節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的最短路徑。這樣，機(jī)器人在全局環(huán)境中的最優(yōu)路徑規(guī)劃問(wèn)題就轉(zhuǎn)換為全局地圖中的最短路徑搜索問(wèn)題，可采用成熟的圖論算法求得最短路徑，如Dijkstra、Floryd、A算法、可視圖法、自由空間法、柵格法、拓?fù)浞ǖ取１疚牟捎没谌杭悄艿牡南伻悍律惴ǎ撍惴ê?jiǎn)單、迅速、高效，并且具有較強(qiáng)的魯棒性。

微粒群算法（PSO）^[8]是由美國(guó)Kennedy博士和Eberhart博士于1995年開(kāi)發(fā)的一種演化計(jì)算技術(shù)，基本思想是受到早期對(duì)鳥(niǎo)類群體行為研究結(jié)果的啟發(fā)，利用和改進(jìn)了生物學(xué)家的生物群體模型，以使微粒能夠飛向解空間并在最優(yōu)解處降落。優(yōu)化問(wèn)題的解是搜索空間中微粒。各微粒都有一個(gè)由被優(yōu)化的函數(shù)決定的適應(yīng)值以及速度決定飛翔的方向和距離，微粒群追隨當(dāng)前的最優(yōu)微粒在解空間中搜索。其應(yīng)用領(lǐng)域已擴(kuò)展到多目標(biāo)優(yōu)化、數(shù)據(jù)分類、數(shù)據(jù)聚類、模式識(shí)別、路由計(jì)算、生物系統(tǒng)建模、流程規(guī)劃、信號(hào)處理、機(jī)器人控制、決策支持以及仿真和系統(tǒng)辨識(shí)等方面。

與大多數(shù)基于梯度應(yīng)用優(yōu)化算法不同，PSO是一種概率搜索算法。 雖然概率搜索算法通常要采用較多評(píng)價(jià)函數(shù)，但與梯度方法及傳統(tǒng)的演化算法相比，其優(yōu)點(diǎn)還是顯著的：a)魯棒性好。由于無(wú)集中控制約束，不會(huì)因個(gè)別個(gè)體的故障影響整個(gè)問(wèn)題的求解。b)具備分布式的特征。可方便應(yīng)用分布式算法模型以及利用多處理器并行計(jì)算。c)應(yīng)用面廣。對(duì)問(wèn)題定義的連續(xù)性無(wú)特殊要求。d)以非直接的信息交流方式確保了系統(tǒng)的擴(kuò)展性。e)算法實(shí)現(xiàn)簡(jiǎn)單。

基于微粒群的局部最優(yōu)路徑搜索實(shí)現(xiàn)步驟如下：

a)設(shè)置初始參數(shù)，如微粒群的隨機(jī)位置坐標(biāo)和速度的初始值。

b)計(jì)算每個(gè)微粒的適應(yīng)函數(shù)值，適應(yīng)度函數(shù)的設(shè)計(jì)如下：

其中：α、β、χj是正實(shí)數(shù)，反映了路徑規(guī)劃中各參量的重要性；(Z)是懲罰函數(shù)；r是懲罰的程度。

算法通過(guò)適應(yīng)度來(lái)確定微粒當(dāng)前位置的優(yōu)劣，路經(jīng)越優(yōu)，則適應(yīng)度越高。

c)個(gè)體最佳位置更新。若Ji＞Jibest，則Jibest=Ji，Pi=Xi。Pi為各微粒經(jīng)歷過(guò)的最好位置。

d)全局最佳位置更新。若Ji＞Jgbest，則Jgbest=Ji，Pg=Xi。Pg為群體所有微粒經(jīng)歷的最好位置。

e)更新微粒速度和位置，根據(jù)如下的公式：

其中：r1、r2是[0，1]的隨機(jī)數(shù);學(xué)習(xí)因子c1、c2為非負(fù)常數(shù)，決定pi、pg的影響程度。

由式(15)可以看出，微粒的移動(dòng)由三部分組成：微粒原有速度vi(t)；微粒當(dāng)前位置與其經(jīng)歷的最佳位置的偏差pi-xi(t)以及微粒當(dāng)前位置和整個(gè)微粒群所經(jīng)歷的最佳位置的偏差pg-xi(t)。權(quán)重系數(shù)w、c1、c2決定三者的相對(duì)重要性。

f)判斷結(jié)束條件。如果已達(dá)到最大的迭代步數(shù)或最優(yōu)的目標(biāo)條件，則返回當(dāng)前最佳微粒的位置坐標(biāo)作為路徑優(yōu)化結(jié)果，算法結(jié)束；否則返回b)，繼續(xù)下一循環(huán)的優(yōu)化求解。

4 實(shí)驗(yàn)結(jié)果

算法在實(shí)際的移動(dòng)節(jié)點(diǎn)上實(shí)施，最高層的主要構(gòu)件是 MICAz傳感器節(jié)點(diǎn)，由MCU、ATmega128L和無(wú)線通信芯片 CC2420構(gòu)成；傳感器板可以附加連接到MICAz。最高層被指派讀取傳感數(shù)據(jù)并與其他節(jié)點(diǎn)通信，通過(guò)串口控制底層。移動(dòng)節(jié)點(diǎn)使用該路徑規(guī)劃方法實(shí)現(xiàn)目標(biāo)。所構(gòu)建的傳感器網(wǎng)絡(luò)能夠長(zhǎng)時(shí)間持續(xù)工作。

所用的傳感器節(jié)點(diǎn)的規(guī)格如表1所示。

圖4是三個(gè)移動(dòng)機(jī)器人探索區(qū)域D的實(shí)例，繪制了不同時(shí)刻占用網(wǎng)格的快照。初始時(shí)刻網(wǎng)格的狀態(tài)都是未知的，呈現(xiàn)自然灰色;當(dāng)移動(dòng)機(jī)器人向四周探索時(shí)，逐漸探索出網(wǎng)格的占用信息；當(dāng)網(wǎng)格被探測(cè)到是空的或有障礙物，其顏色變成相應(yīng)白色或黑色。

路徑的起點(diǎn)和目標(biāo)點(diǎn)分別為圖5中的左下角和右上角，算法仿真成功以后在上述基于WSN的移動(dòng)機(jī)器人上進(jìn)行實(shí)驗(yàn)，得到了很好的效果。從實(shí)驗(yàn)中的移動(dòng)軌跡結(jié)果可見(jiàn)，移動(dòng)機(jī)器人的行為表現(xiàn)出比較好的一致性、連續(xù)性和穩(wěn)定性;在復(fù)雜的障礙未知情況，該算法能規(guī)劃出有效路徑且基本為最優(yōu)化路徑。機(jī)器人行走的路徑如圖5中的灰度格所示。

對(duì)單傳感器和基于多傳感器信息融合方法測(cè)量準(zhǔn)確度進(jìn)行了對(duì)比實(shí)驗(yàn)，圖6為兩種方法距離誤差比較的實(shí)驗(yàn)結(jié)果。圖中的橫坐標(biāo)表示機(jī)器人傳感器測(cè)量的距離d，縱坐標(biāo)表示測(cè)量的距離誤差。從圖中可以看出，采用基于多傳感器信息融合比單傳感器的準(zhǔn)確度要高很多。圖7顯示了算法的分析時(shí)間，可以看出，分析時(shí)間隨著障礙物數(shù)量的增加而延長(zhǎng)。計(jì)算機(jī)配置環(huán)境是 Pentium 4 1．4 GHz CPU。在地圖中隨機(jī)放置障礙物，不同數(shù)量的障礙物情況下的分析時(shí)間都測(cè)試了一百次來(lái)計(jì)算平均值。地圖的大小不影響障礙物，這在很大程度上提高了效率，路徑規(guī)劃系統(tǒng)能夠快速地得到路徑的最優(yōu)解。

5 結(jié)束語(yǔ)

本文提出了一種新的基于無(wú)線傳感器網(wǎng)絡(luò)的移動(dòng)機(jī)器人實(shí)時(shí)在線路徑規(guī)劃方法。將全局規(guī)劃和局部規(guī)劃兩者有效相結(jié)合，采用三層控制結(jié)構(gòu)的規(guī)劃策略，使用多傳感器信息融合構(gòu)建全局概率地圖，基于微粒群的群集仿生智能算法避障，適合機(jī)器人在未知環(huán)境下的實(shí)時(shí)要求，提高了規(guī)劃的整體性能。實(shí)驗(yàn)結(jié)果證實(shí)了該算法具有較好的有效性、實(shí)時(shí)性和適應(yīng)性， 為移動(dòng)機(jī)器人實(shí)時(shí)在線路徑規(guī)劃提供了新途徑。

參考文獻(xiàn)：

［1］GRACANIN D.A servicecentric model for wireless sensor networks[J].IEEE Journal on Selected Areas in Communications，2005，23(6)：11591166.

［2］AKYILDIZ I F，SU W J，SANKARASUBRAMANIAM Y，et al.Wireless sensor networks: a survey[J].Computer Networks，2002，38(4):393-422.

［3］PATHIRANA R N，BULUSU N，SAVKIN A V，et al.Node localization using mobile robots in disconnected sensor networks[J].IEEE Trans on Mobile Computing，2005，4(3):285-296.

［4］ELFES A.Using occupancy grids for mobile robot perception and navigation[J].Computer，1989，22(6):249-265. 

［5］GAO J B，HARRIS C J.Some remarks on Kalman filters for the multisensor fusion[J].Information Fusion，2002，3(3):191-200.

［6］YAGER R R.On the dempstershafer framework and new combination rules[J].Information Sciences，1987，41(2):93137.

［7］RUSSO F，RAMPONI G.Fuzzy methods for multi sensor data fusion[J].IEEE Trans on Instrumentation and Measurement，1994，43(2): 288-289.

［8］KENNEDY J，EBERHART R.Particle swarm optimization[C]//Proc of IEEE International Conference on Neural Networks.1995:39-43.

注：“本文中所涉及到的圖表、注解、公式等內(nèi)容請(qǐng)以PDF格式閱讀原文。”