機(jī)器人定位方法
【專利摘要】本公開了一種機(jī)器人定位方法，待定位的機(jī)器人基于WiFi接入無線局域網(wǎng)，包括以下步驟：基于WiFi指紋定位確定機(jī)器人當(dāng)前所在的估計(jì)位置；調(diào)用所述估計(jì)位置的地圖并在該地圖約束的估計(jì)位置撒點(diǎn)，采用粒子濾波算法得到機(jī)器人的精確位置。依據(jù)本發(fā)明的機(jī)器人定位方法定位精度較高，且所需要的迭代時(shí)間較少。
【專利說明】
機(jī)器人定位方法
技術(shù)領(lǐng)域
[0001 ]本發(fā)明涉及一種機(jī)器人定位方法。
【背景技術(shù)】
[0002] 隨著科學(xué)技術(shù)的發(fā)展以及人們生活水平的提高，例如服務(wù)機(jī)器人已經(jīng)逐漸進(jìn)入人 們的生活，為人類提供各種人性化服務(wù)。相對(duì)于其他類型的機(jī)器人，服務(wù)機(jī)器人的工作環(huán)境 比較復(fù)雜，因此在復(fù)雜環(huán)境中的自主與智能定位是服務(wù)機(jī)器人為人類提供高質(zhì)量服務(wù)的前 提。
[0003] 目前服務(wù)機(jī)器人定位問題主要分為兩種:位置跟蹤與全局定位。其中，全局定位是 指機(jī)器人在初始位姿沒有任何先驗(yàn)知識(shí)的情況下，通過傳感器感知信息確定位姿。全局定 位主要用來解決機(jī)器人位置初始化、機(jī)器人綁架以及機(jī)器人移動(dòng)過程中的迷途與位置校準(zhǔn) 等問題。
[0004] 常用的服務(wù)機(jī)器人全局定位方法包括馬爾可夫定位、卡爾曼濾波和基于蒙特卡洛 算法的粒子濾波方法等?；诿商乜逅惴ǖ牧Ｗ訛V波算法能夠很好地處理非線性非高斯 問題，因此該算法被廣泛應(yīng)用于服務(wù)機(jī)器人定位和地圖構(gòu)建問題中。
[0005] 文獻(xiàn)"面向擁擠環(huán)境的移動(dòng)機(jī)器人改進(jìn)粒子濾波定位"（《機(jī)器人》，2012,34(5): 596-603,王勇，陳衛(wèi)東，王景川，等）提出了一種基于可定位性的改進(jìn)粒子濾波定位算法來 解決擁擠環(huán)境下的移動(dòng)機(jī)器人定位問題。該方法通過判斷激光器數(shù)據(jù)和里程計(jì)數(shù)據(jù)的可信 度，來修正它們?cè)诹Ｗ佣ㄎ恢械谋戎?，有效地解決了移動(dòng)機(jī)器人在擁擠環(huán)境中的定位問題。
[0006] 文獻(xiàn)"平方根容積Rao-Blackwi 11 ised粒子濾波SLAM算法"（《自動(dòng)化學(xué)報(bào)》，2014， 40(2): 357-367 ?宋宇，李慶玲，康軼非，等)提出了一種平方根容積FastSLAM(Simultaneous Localization and Mapping)算法，有效地提高了粒子濾波SLAM算法的精度和性能。
[0007] 文獻(xiàn)"利于kinect傳感器和測(cè)距進(jìn)行移動(dòng)機(jī)器人定位"（Ganganath N, Leung H.Mobile robot localization using odometry and kinect sensor[C]//IEEE International Conference on Emerging Signal Processing Applications .Piscataway,USA: IEEE,2012:91-94?)提出一種基于粒子濾波器和擴(kuò)展卡爾 曼濾波器的低成本定位系統(tǒng)。該系統(tǒng)在預(yù)測(cè)階段使用里程計(jì)信息估計(jì)位姿，而在更新階段 利用Kinect和人工地標(biāo)校準(zhǔn)機(jī)器人位姿。
[0008] 由于家庭和辦公環(huán)境具有較高的復(fù)雜性和動(dòng)態(tài)性，在此類環(huán)境下全局粒子濾波定 位算法存在以下問題：1)由于地圖較大，所需粒子數(shù)多，全局粒子撒點(diǎn)計(jì)算時(shí)間較長(zhǎng);2)當(dāng) 采樣粒子接近機(jī)器人真實(shí)位置時(shí)才能保證粒子濾波定位算法的成功率，但在地圖過大且粒 子數(shù)目有限的情況下，采樣粒子無法覆蓋機(jī)器人真實(shí)位置，造成定位精度的下降；3)在結(jié)構(gòu) 相似性強(qiáng)的環(huán)境中，全局定位成功率不高。
[0009] 針對(duì)此問題，文獻(xiàn)"基于單目視覺的移動(dòng)機(jī)器人全局定位"（機(jī)器人，2007,29(2): 140-144,178.厲茂海，洪炳镕，羅榮華，等)利用單目視覺創(chuàng)建環(huán)境地圖并通過特征匹配實(shí) 現(xiàn)全局定位;但圖像處理較為復(fù)雜，單目視覺系統(tǒng)增加了機(jī)器人的成本和計(jì)算負(fù)擔(dān)。文獻(xiàn) "基于粒子濾波器的移動(dòng)機(jī)器人定位和地圖創(chuàng)建研究進(jìn)展"（機(jī)器人，2007，29(3): 281-289， 297.余洪山，王耀南)介紹了一些使用粒子濾波器進(jìn)行機(jī)器人地圖創(chuàng)建與定位的方法，但在 復(fù)雜環(huán)境下由于場(chǎng)景結(jié)構(gòu)的相似性，單一的粒子濾波定位在機(jī)器人位置無任何先驗(yàn)知識(shí)的 條件下容易造成定位失敗。

【發(fā)明內(nèi)容】

[0010] 本發(fā)明的目的在于提供一種在獲得較高定位精度的條件下，大大縮減計(jì)算迭代時(shí) 間的機(jī)器人定位方法。
[0011] 本發(fā)明采用以下技術(shù)方案：
[0012] 一種機(jī)器人定位方法，待定位的機(jī)器人基于WiFi接入無線局域網(wǎng)，包括以下步驟： [00 13]基于WiFi指紋定位確定機(jī)器人當(dāng)前所在的估計(jì)位置；
[0014]調(diào)用所述估計(jì)位置的地圖并在該地圖約束的估計(jì)位置撒點(diǎn)，采用粒子濾波算法得 到機(jī)器人的精確位置。
[0015] 上述機(jī)器人定位方法，可選地，所述估計(jì)位置為機(jī)器人估計(jì)點(diǎn)位和估計(jì)點(diǎn)位誤差 所約束。
[0016] 可選地，所述粒子濾波算法為基于蒙特卡洛算法的粒子濾波算法。
[0017] 可選地，1)所述粒子濾波算法以前一時(shí)刻基于粒子分布所采樣的用于表征機(jī)器人 狀態(tài)的離散數(shù)據(jù)及機(jī)器人在相應(yīng)機(jī)器人狀態(tài)時(shí)的概率作為先驗(yàn)概率密度分布的采樣集；
[0018] 2)更新：以機(jī)器人所搭載激光測(cè)距傳感器的信息修正所述采樣集，用以更新采樣 集中樣本的權(quán)重值，得到估計(jì)位置的后驗(yàn)數(shù)據(jù)，進(jìn)而增加權(quán)重較大的粒子的數(shù)目，減少權(quán)重 較小的粒子的數(shù)目，產(chǎn)生在估計(jì)位置的粒子的重新分布，而產(chǎn)生新的采樣集；
[0019] 迭代預(yù)定次數(shù)的步驟2)，輸出權(quán)重最大的粒子所適配的機(jī)器人位置估計(jì)作為機(jī)器 人位置的精確值；
[0020] 其中，所述地圖為匹配所述激光測(cè)距傳感器的激光地圖。
[0021] 可選地，基于機(jī)器人地圖和里程計(jì)信息所匹配的運(yùn)動(dòng)學(xué)模型產(chǎn)生采樣集中的樣 本，該樣本即所述離散數(shù)據(jù)和相應(yīng)的概率。
[0022] 可選地，更新步驟中的增加權(quán)重較大的粒子的數(shù)目，減少權(quán)重較小的粒子的數(shù)目 的方法是：
[0023] 基于樣本的當(dāng)前權(quán)重隨機(jī)出預(yù)定數(shù)目的樣本。
[0024] 可選地于，在更新步驟中，在更新采樣集中樣本的權(quán)重值前，對(duì)樣本的權(quán)重進(jìn)行歸 一化處理。
[0025] 可選地，基于激光測(cè)距傳感器更新采樣集中樣本的權(quán)重值是以在采樣集條件下基 于激光測(cè)距傳感器采樣得到的機(jī)器人狀態(tài)的發(fā)生概率。
[0026] 可選地，WiFi指紋定位為基于BP神經(jīng)網(wǎng)絡(luò)的WiFi指紋定位。
[0027]可選地，支持BP神經(jīng)網(wǎng)絡(luò)的AP點(diǎn)在室內(nèi)的分布包括對(duì)室內(nèi)機(jī)器人移動(dòng)區(qū)域熱點(diǎn)的 中心分布和在移動(dòng)區(qū)域外圍的邊緣分布；
[0028]其中，中心分布是在一個(gè)矩形空間內(nèi)的一個(gè)對(duì)角線上的若干AP陣列，邊緣分布則 是在與AP陣列相交叉的方向上相異的兩端的各一個(gè)AP。
[0029]依據(jù)本發(fā)明，在沒有任何初始位姿先驗(yàn)知識(shí)的情況下，通過WiFi進(jìn)行機(jī)器人的粗 定位，在粗定位確定的機(jī)器人所處的空間內(nèi)，再利用粒子濾波算法進(jìn)行機(jī)器人的精確定位。 由于相對(duì)于全局定位，粗定位所約束的空間相對(duì)較小，需要的粒子數(shù)相對(duì)較少，減少了計(jì)算 量，且基于粒子濾波的算法迭代的次數(shù)也會(huì)減少，縮短了迭代時(shí)間，或者在迭代次數(shù)相同的 情況下，定位精度相對(duì)較高。
【附圖說明】
[0030]圖1為服務(wù)機(jī)器人全局定位系統(tǒng)原理框圖。
[0031 ]圖2為服務(wù)機(jī)器人服務(wù)描述結(jié)構(gòu)圖。
[0032]圖3為實(shí)驗(yàn)室中AP點(diǎn)的分布圖。
[0033]圖4為神經(jīng)網(wǎng)絡(luò)訓(xùn)練誤差曲線圖。
[0034]圖5(a)為已知的GL0BAL-PF全局定位采樣集分布(實(shí)際采樣圖的灰度圖）。
[0035]圖5(b)為依據(jù)本發(fā)明的WIFI-PF全局定位采樣集分布(實(shí)際采樣圖的灰度圖）。 [0036]圖5(c)為已知的GL0BAL-PF全局定位結(jié)果(實(shí)際采樣圖的灰度圖）。
[0037]圖5(d)為依據(jù)本法名的WIFI-PF全局定位結(jié)果(實(shí)際采樣圖的灰度圖）。
[0038]圖6為誤差隨迭代次數(shù)變化曲線：圖6(a)為已知的GL0BAL-PF誤差曲線；圖6(b)為 依據(jù)本發(fā)明的WIFI-PF誤差曲線。
[0039] 圖7為已知的GL0BAL-PF全局定位、WiFi定位與依據(jù)本發(fā)明的WiFi-PF全局定位的 平均誤差對(duì)比圖。
[0040] 圖8為已知的GL0BAL-PF全局定位與依據(jù)本發(fā)明的WiFi-PF全局定位迭代時(shí)間對(duì)比 圖。
[0041 ]圖中：1.實(shí)驗(yàn)室，2. AP，3.實(shí)際位置，4.估計(jì)位置。
【具體實(shí)施方式】
[0042] 以智能空間下基于WiFi(Wireless Fidelity，是基于IEEE 802. lib標(biāo)準(zhǔn)的無線局 域網(wǎng)）指紋定位與粒子濾波的服務(wù)機(jī)器人復(fù)合全局定位方法為例詳述本發(fā)明的原理，使其 目的、原理、手段更加清晰。
[0043]如圖1所示，全局定位系統(tǒng)由WiFi信息采集模塊、智能空間調(diào)度系統(tǒng)和服務(wù)機(jī)器人 本體3部分組成。系統(tǒng)工作流程描述為:首先通過UPnP(Universal Plug and Play，通用即 插即用)基于WiFi接入無線局域網(wǎng)的服務(wù)機(jī)器人向智能空間調(diào)度系統(tǒng)發(fā)送請(qǐng)求消息，智能 空間調(diào)度系統(tǒng)接到請(qǐng)求后，響應(yīng)該請(qǐng)求，向服務(wù)機(jī)器人上的WiFi采集模塊發(fā)送控制信息，從 而，服務(wù)機(jī)器人上的WiFi采集模塊將掃描到的AP(Wireless Access Point，無線訪問接入 點(diǎn)）的RSSI(Received Signal Strength Indication，接收的信號(hào)強(qiáng)度指示)信號(hào)強(qiáng)度通過 TCP協(xié)議發(fā)送給神經(jīng)網(wǎng)絡(luò)模塊，從而得到機(jī)器人估計(jì)位置，智能空間調(diào)度系統(tǒng)調(diào)用服務(wù)機(jī)器 人的roscontrol服務(wù)中的SetPose方法，向amcl節(jié)點(diǎn)發(fā)送initialpose消息設(shè)定初始估計(jì) 點(diǎn)，之后進(jìn)行粒子濾波定位得到機(jī)器人精確位置。
[0044]在一些實(shí)現(xiàn)中，單獨(dú)的WiFi指紋定位即可獲得較高的定位精度，然而，其所依賴的 是成本比較高的路由器，而在家庭環(huán)境中，普遍使用成本較低，精度較差，且容易受干擾、信 號(hào)強(qiáng)度不穩(wěn)定的普通路由器，因此，服務(wù)機(jī)器人在室內(nèi)的定位，尤其是家用服務(wù)機(jī)器人的定 位精度在純粹的依賴家用普通路由器的條件下是比較低的。
[0045] 此外，對(duì)于基于WiFi的指紋定位往往需要嚴(yán)格的時(shí)間同步和專用的硬件支持，技 術(shù)實(shí)現(xiàn)難度比較高，且由于成本比較高，而難以在家庭中應(yīng)用。
[0046] 在一些實(shí)施例中，WiFi采集系統(tǒng)由3個(gè)模塊構(gòu)成:CC3000模塊，STM32控制器和RM04 模塊。CC3000模塊掃描智能空間中的AP點(diǎn)并得到其信號(hào)強(qiáng)度，將信號(hào)強(qiáng)度數(shù)據(jù)通過SPI總線 傳輸給STM32控制器。STM32控制器負(fù)責(zé)CC3000模塊與RM04模塊的初始化，從CC3000模塊中 接收強(qiáng)度信息，并將強(qiáng)度信息通過串口發(fā)送到RM04模塊。RM04為串口轉(zhuǎn)WiFi模塊，由該WiFi 模塊負(fù)責(zé)接入智能空間WiFi網(wǎng)絡(luò)中，將從串口接收到的AP強(qiáng)度數(shù)據(jù)發(fā)送給神經(jīng)網(wǎng)絡(luò)模塊， 從而實(shí)現(xiàn)WiFi指紋定位。
[0047]關(guān)于智能空間調(diào)度系統(tǒng)：
[0048] 智能空間調(diào)度系統(tǒng)主要進(jìn)行WiFi指紋定位以及R0S(Route Operation System，軟 件路由）服務(wù)機(jī)器人的消息獲取和服務(wù)執(zhí)行。其中神經(jīng)網(wǎng)絡(luò)模塊通過智能空間WiFi數(shù)據(jù)庫 中的數(shù)據(jù)，采用BP神經(jīng)網(wǎng)絡(luò)進(jìn)行訓(xùn)練，然后對(duì)WiFi采集模塊采集到的數(shù)據(jù)進(jìn)行位置估計(jì)，并 將估計(jì)結(jié)果發(fā)送給機(jī)器人調(diào)度模塊。機(jī)器人調(diào)度模塊主要功能為發(fā)現(xiàn)智能空間網(wǎng)絡(luò)中的 R0S機(jī)器人并取到相應(yīng)設(shè)備描述，接收R0S機(jī)器人定位請(qǐng)求消息，調(diào)用神經(jīng)網(wǎng)絡(luò)模塊進(jìn)行機(jī) 器人位置定位，之后調(diào)用roscontrol服務(wù)中的SetPose動(dòng)作向amcl節(jié)點(diǎn)發(fā)送initialpose消 息。
[0049] 公知地，WiFi指紋定位的原理基于以下四個(gè)條件：
[0050] 1.在于每一個(gè)無線AP都有一個(gè)全球唯一的MAC(Media Access Control，媒體介入 控制層，屬于0SI中的數(shù)據(jù)鏈路層)地址，并且一般來說無線AP在一段時(shí)間內(nèi)是不會(huì)移動(dòng)的。
[0051] 2.設(shè)備在開啟WiFi的情況下，即可掃描并收集周圍的AP信號(hào)，無論是否加密，是否 已連接，甚至信號(hào)強(qiáng)度不足以顯示在無線信號(hào)列表中，都可以獲取到AP廣播出來的MAC地 址。
[0052] 3.設(shè)備將這些能夠標(biāo)示AP的數(shù)據(jù)發(fā)送到位置服務(wù)器，服務(wù)器檢索出每一個(gè)AP的地 理位置，并結(jié)合每個(gè)信號(hào)的強(qiáng)弱程度，計(jì)算出設(shè)備的地理位置并返回到用戶設(shè)備。
[0053] 4.位置服務(wù)商要不斷更新、補(bǔ)充自己的數(shù)據(jù)庫，以保證數(shù)據(jù)的準(zhǔn)確性，畢竟無線AP 不像基站塔那樣基本100 %不會(huì)移動(dòng)。
[0054] 由此，當(dāng)智能空間中存在多個(gè)AP時(shí)，多路采樣得到的WiFi信號(hào)強(qiáng)度具備匹配相應(yīng) 位置的特征，從而具有定位的技術(shù)基礎(chǔ)。
[0055] 關(guān)于服務(wù)機(jī)器人本體：
[0056]依據(jù)本發(fā)明的實(shí)施例中，使用基于R0S平臺(tái)的家庭輪式服務(wù)機(jī)器人，R0S是一個(gè)分 布式模塊化的機(jī)器人軟件平臺(tái)，它擁有大量軟件庫以及相應(yīng)工具幫助人們開發(fā)機(jī)器人應(yīng) 用。R0S機(jī)器人平臺(tái)具有較好的可擴(kuò)展性和分布性，近幾年得到廣泛關(guān)注。如圖1所示，該服 務(wù)機(jī)器人定位系統(tǒng)主要包含〇dometery、map_server、rplidar、ROSUPnP和amcl節(jié)點(diǎn)。
[0057]其中，ROSUPnP節(jié)點(diǎn)為機(jī)器人設(shè)備抽象節(jié)點(diǎn)，它將服務(wù)機(jī)器人提供的服務(wù)抽象成統(tǒng) 一的用戶接口，并利用libupnp SDK開發(fā)庫將智能服務(wù)機(jī)器人改造成UPnP設(shè)備。智能空間通 過該節(jié)點(diǎn)可調(diào)用R0S機(jī)器人的相應(yīng)服務(wù)，實(shí)現(xiàn)智能空間與R0S機(jī)器人之間的松耦合以及零配 置，提高系統(tǒng)的擴(kuò)展性與兼容性。當(dāng)R0S機(jī)器人接入智能空間網(wǎng)絡(luò)時(shí)，該節(jié)點(diǎn)將R0S機(jī)器人設(shè) 備描述文檔和服務(wù)描述文檔發(fā)布到網(wǎng)絡(luò)中，從而使智能空間調(diào)度模塊發(fā)現(xiàn)相應(yīng)的機(jī)器人設(shè) 備，智能空間調(diào)度模塊只需調(diào)用服務(wù)描述文件中的統(tǒng)一接口而不需要理解服務(wù)的具體實(shí) 現(xiàn)。
[0058] 本實(shí)施例中，ROS機(jī)器人提供的服務(wù)為全局定位服務(wù)(roscontrol)，該服務(wù)相應(yīng)的 服務(wù)結(jié)構(gòu)描述文檔如圖2所示，其中，在roscontrol服務(wù)中，接口以動(dòng)作的形式提供，該服務(wù) 提供SetPose的動(dòng)作。SetPose動(dòng)作包含wx、wy和mea三個(gè)輸入?yún)?shù)，其中wx和wy為機(jī)器人位 置的WIFI定位估計(jì)坐標(biāo)，mea為相應(yīng)誤差范圍。這些輸入?yún)?shù)與機(jī)器人內(nèi)部的x、y和mea狀態(tài) 變量相關(guān)聯(lián)，x和y表示R0S機(jī)器人中initialpose消息的初始點(diǎn)狀態(tài)變量，mea表示機(jī)器人中 i n i t i a 1 po s e消息的粒子撒點(diǎn)范圍的狀態(tài)變量。
[0059] odometery、map_server、rplidar和amcl為R0S平臺(tái)中導(dǎo)航包中的節(jié)點(diǎn)。其中， odometery為里程計(jì)節(jié)點(diǎn)，它將R0S機(jī)器人的里程計(jì)信息發(fā)送給amcl定位節(jié)點(diǎn)。rplidar為激 光測(cè)距傳感器節(jié)點(diǎn)，負(fù)責(zé)發(fā)布激光數(shù)據(jù)。map_server為機(jī)器人環(huán)境地圖服務(wù)，主要為amcl定 位節(jié)點(diǎn)提供相應(yīng)地圖。amcl為定位節(jié)點(diǎn)，它通過里程計(jì)數(shù)據(jù)、激光器數(shù)據(jù)以及地圖信息進(jìn)行 粒子濾波定位。
[0060] 關(guān)于服務(wù)機(jī)器人基于粒子撒點(diǎn)及相應(yīng)的地圖信息所進(jìn)行的定位，可參見"基于粒 子濾波器的移動(dòng)機(jī)器人定位和地圖創(chuàng)建研究進(jìn)展"（《機(jī)器人》，2007，29(3): 281-289.)。"基 于R0S的機(jī)器人即時(shí)定位與地圖構(gòu)建技術(shù)的研究"（《東北大學(xué)》，2013,夏漢均）。
[0061] 基于WIFI指紋與粒子濾波的服務(wù)機(jī)器人全局定位系統(tǒng)算法主要由兩部分組成。優(yōu) 選地，首先，利用BP神經(jīng)網(wǎng)絡(luò)對(duì)機(jī)器人位置進(jìn)行粗定位;其次在粗定位的基礎(chǔ)上進(jìn)行粒子撒 點(diǎn)，根據(jù)激光器掃描數(shù)據(jù)與局部地圖數(shù)據(jù)得到機(jī)器人的精確位置。
[0062] 關(guān)于基于BP神經(jīng)網(wǎng)絡(luò)的WIFI指紋定位：
[0063] 無線網(wǎng)絡(luò)定位方法主要有T0A(time of arrival)方法、RSSI方法、位置指紋方法 等。其中，T0A主要根據(jù)參考節(jié)點(diǎn)與未知節(jié)點(diǎn)之間的信號(hào)到達(dá)時(shí)間計(jì)算出未知節(jié)點(diǎn)的位置。
[0064] RSSI方法是在未知節(jié)點(diǎn)中獲取到已知位置的節(jié)點(diǎn)的信號(hào)強(qiáng)度，然后根據(jù)無線信號(hào) 隨距離衰減模型，求出待測(cè)節(jié)點(diǎn)與已知節(jié)點(diǎn)的距離，最后根據(jù)距離求出未知節(jié)點(diǎn)的位置。
[0065] 位置指紋方法分為兩個(gè)階段:離線階段和定位階段。在離線階段，首先從環(huán)境中選 取一些位置作為采樣點(diǎn)，然后在采樣點(diǎn)中采集環(huán)境中多個(gè)AP點(diǎn)的強(qiáng)度信息作為該位置的指 紋，并利用這些指紋建立指紋庫。在定位階段，未知節(jié)點(diǎn)首先測(cè)量環(huán)境中的AP強(qiáng)度指紋，并 將該點(diǎn)AP強(qiáng)度指紋與指紋庫匹配，從而得到未知節(jié)點(diǎn)的位置。
[0066] T0A方法盡管具有較好的定位精度，但需要參考節(jié)點(diǎn)與未知節(jié)點(diǎn)之間的嚴(yán)格時(shí)間 同步，同時(shí)要求無線信號(hào)具有較高的穿透力，所以需要專業(yè)的定位設(shè)備，造價(jià)較高。由于室 內(nèi)環(huán)境復(fù)雜同時(shí)存在大量的障礙物，無線信號(hào)隨距離衰減模型往往很難獲得，所以RSSI方 法不太適合于復(fù)雜的室內(nèi)環(huán)境。盡管位置指紋方法定位精度不如T0A方法高，但是技術(shù)實(shí)現(xiàn) 和成本要求較低，不需要設(shè)計(jì)專門的定位設(shè)備，可以通過純軟件實(shí)現(xiàn)，所以本文選用指紋定 位方法進(jìn)行服務(wù)機(jī)器人的粗定位。
[0067]指紋定位的離線學(xué)習(xí)和訓(xùn)練可以通過神經(jīng)網(wǎng)絡(luò)有效實(shí)現(xiàn)。BP神經(jīng)網(wǎng)絡(luò)是神經(jīng)網(wǎng)絡(luò) 中比較常用的一種，主要由輸入層、隱含層和輸出層3層組成。首先，輸入信號(hào)經(jīng)過輸入層與 隱含層的處理，在輸出層得到相應(yīng)的估計(jì)值，比較估計(jì)值與實(shí)際值得到輸出誤差，將該誤差 反向傳播至隱含層所有單元，從而得到各個(gè)單元的誤差信號(hào)并根據(jù)該誤差調(diào)節(jié)神經(jīng)網(wǎng)絡(luò)中 的各個(gè)權(quán)值，然后重復(fù)該過程直到輸出誤差小于期望值。
[0068]與傳統(tǒng)加權(quán)K鄰近和最近鄰指紋定位算法相比，基于BP神經(jīng)網(wǎng)絡(luò)的指紋定位算法 具有較好的非線性映射能力，可以有效地逼近智能空間中的WIFI強(qiáng)度模型。
[0069]另外，BP神經(jīng)網(wǎng)絡(luò)具有較好的自學(xué)習(xí)能力，可有效地更新智能空間中的WIFI定位 模型。同時(shí)，BP神經(jīng)網(wǎng)絡(luò)還具有較好的容錯(cuò)性，當(dāng)智能空間中一些AP點(diǎn)出現(xiàn)故障導(dǎo)致WIFI強(qiáng) 度信息不全時(shí)，BP神經(jīng)網(wǎng)絡(luò)仍可以進(jìn)行定位。
[0070] 基于BP神經(jīng)網(wǎng)絡(luò)的指紋定位方法的具體訓(xùn)練步驟如下：
[0071] Stepl:對(duì)AP信號(hào)強(qiáng)度以及位置坐標(biāo)進(jìn)行歸一化，得到神經(jīng)網(wǎng)絡(luò)輸入向量X=(X1， X2, . . .，xn)，n為AP點(diǎn)的個(gè)數(shù)，期望輸出向量d°=(di，d2,. . .，dq)，q為輸出層的個(gè)數(shù)。
[0072] Step2:選取第k組AP點(diǎn)的強(qiáng)度數(shù)據(jù)x(k) = (xi(k)，X2(k), ? ? ?，xn(k))，以及相應(yīng)坐 標(biāo)(1°(1〇 = ((11(1〇，(12(1〇，...，(1(1(1〇)，計(jì)算神經(jīng)網(wǎng)絡(luò)各層的輸入和輸出。首先，利用式（1)計(jì) 算出隱含層輸入向量h lh(k)，其中wlh為輸入層與隱含層的連接權(quán)重矩陣，bh為隱含層各神經(jīng) 元的閾值向量，P為隱含層個(gè)數(shù)。然后利用式⑵得到隱含層輸出向量h h°(k)，其中f()為激活 函數(shù)。同理可得，輸出層輸入向量ylcl(k)和輸出層輸出向量y°(k)。
[0073] hih(k) = WihX (k)-bh (1)
[0074] hh0(k)=f(hih(k)) (2)
[0075] Step3:利用BP神經(jīng)網(wǎng)絡(luò)的估計(jì)位置和機(jī)器人實(shí)際位置計(jì)算出誤差函數(shù)(式（3))， 并利用式(4)計(jì)算輸出層各神經(jīng)元的偏導(dǎo)數(shù)6以），j = l，2,…，q，按照式(5)求出誤差函數(shù) 對(duì)隱含層各神經(jīng)元偏導(dǎo)數(shù)句伙)，j = 1，2，…，p。
[0079] Step4:利用式(6)和(7)修正神經(jīng)網(wǎng)絡(luò)隱含層和輸出層之間的連接權(quán)重wh。以及輸 入層和隱含層之間的連接權(quán)重^^
[0080] C+1 = 〇 如。(辦h°T(左) (6)
[0081 ] ^ (k)x(k) (7)
[0082] Step5:計(jì)算機(jī)器人位置的全局誤差，并判斷全局誤差是否滿足要求，如果滿足則 訓(xùn)練結(jié)束，否則返回St印2。
[0083]為了驗(yàn)證上述方法的定位效果，在山東大學(xué)服務(wù)機(jī)器人實(shí)驗(yàn)室中，搭建5個(gè)AP點(diǎn)， 如圖4所示。每隔1.8m選取20個(gè)采樣點(diǎn)，每個(gè)采樣點(diǎn)采樣20組數(shù)據(jù)，利用400組AP點(diǎn)強(qiáng)度信息 構(gòu)建實(shí)驗(yàn)室WiFi指紋數(shù)據(jù)庫。使用BP神經(jīng)網(wǎng)絡(luò)對(duì)這些AP強(qiáng)度數(shù)據(jù)進(jìn)行訓(xùn)練。該BP網(wǎng)絡(luò)具有5 個(gè)輸入層節(jié)點(diǎn)、7個(gè)隱含層節(jié)點(diǎn)、2個(gè)輸出層節(jié)點(diǎn)。隱含層激活函數(shù)為sigmoid函數(shù)（如式 (8))，輸出層激活函數(shù)為線性函數(shù)。訓(xùn)練過程中神經(jīng)網(wǎng)絡(luò)誤差下降曲線如圖5所示。最后利 用訓(xùn)練好的BP神經(jīng)網(wǎng)絡(luò)進(jìn)行機(jī)器人位置的粗定位。
[0085]關(guān)于基于蒙特卡洛算法的粒子濾波定位：
[0086] 上述基于神經(jīng)網(wǎng)絡(luò)的WiFi指紋定位算法精度約1.5m~3m左右，所以單一的WiFi指 紋定位不能滿足服務(wù)機(jī)器人的定位需求，需要在WiFi指紋定位的基礎(chǔ)上，利用機(jī)器人地圖 信息、里程計(jì)信息和激光器信息進(jìn)行機(jī)器人精確定位。設(shè)^為〖時(shí)刻的機(jī)器人傳感器測(cè)量數(shù) 據(jù)，at為t時(shí)刻的里程計(jì)信息數(shù)據(jù)，g t為t時(shí)刻的激光測(cè)距傳感器數(shù)據(jù)，&=(^^，0*)表示當(dāng) 前機(jī)器人狀態(tài)，P(st | 表示狀態(tài)估計(jì)的后驗(yàn)概率密度分布。Ct= {(st⑴，wt(i)) | i = 1， 2,. . .，n}表示t時(shí)刻離散采樣樣本集，其中st(1)為機(jī)器人在t時(shí)刻的可能狀態(tài)，wt (1)為機(jī)器人 狀態(tài)為st(1)的概率。該定位過程主要分為兩個(gè)階段：
[0087] (1)預(yù)測(cè)階段
[0088]設(shè)在t_l時(shí)刻得到WIFI指紋粗定位坐標(biāo)以及誤差范圍，利用該坐標(biāo)與誤差范圍初 始化t_l時(shí)刻的樣本集。根據(jù)運(yùn)動(dòng)學(xué)模型對(duì)樣本集Ct-i進(jìn)行采樣得到樣本 以°，所有采樣得到的樣本集為G，用C樣本集近似表示先驗(yàn)概率密度分布p(St|〇M)的采 樣集(如式(9))。
[0090] (2)更新階段
[0091] 設(shè)t時(shí)刻服務(wù)機(jī)器人的獲得的激光器信息gt，利用式（10)更新樣本集C,中的權(quán)重 值M'，并利用式（11)對(duì)權(quán)重進(jìn)行歸一化。用更新后的C,樣本集近似表示后驗(yàn)概率密度分 布P( St|〇H)的采樣集。根據(jù)％(<)對(duì)樣本集G進(jìn)行重采樣得到樣本C t。重采樣過程主要是為 了盡量增加權(quán)重較大粒子的數(shù)目。首先通過產(chǎn)生一個(gè)隨機(jī)數(shù)r，r G [0，1 ]，如果整數(shù)m滿足式 (12)，則將st(m)作為Ct的一個(gè)樣本。重復(fù)此過程直到得到n個(gè)樣本。
[0095]在WiFi粗定位的范圍中，不斷迭代調(diào)用上述步驟，使服務(wù)機(jī)器人的樣本集Ct不斷 更新，并將最終得到的樣本集中權(quán)重最大的機(jī)器人位置估計(jì)作為服務(wù)機(jī)器人位置的估計(jì) 值。
[0096] 仿真實(shí)驗(yàn)及結(jié)果分析（Simulation experiments and results analysis):
[0097] 本實(shí)施例所配置的實(shí)驗(yàn)環(huán)境為山東大學(xué)服務(wù)機(jī)器人實(shí)驗(yàn)室如圖5所示，機(jī)器人平 臺(tái)為基于R0S平臺(tái)的家庭輪式服務(wù)機(jī)器人，激光傳感器為RoboPeak團(tuán)隊(duì)開發(fā)的低成本360°2 維激光雷達(dá)RPLIDAR A1M1，機(jī)器人環(huán)境地圖分辨率大小為354 X 674，精度為0.025。在該實(shí) 驗(yàn)環(huán)境中選取多個(gè)隨機(jī)點(diǎn)進(jìn)行機(jī)器人全局定位。
[0098] 圖5為其中一組定位結(jié)果對(duì)比。圖5(a)為未進(jìn)行WiFi定位的全局定位初始樣本集 分布情況，左中部黑色圓點(diǎn)為初始隨機(jī)位置。圖5(b)為進(jìn)行WiFi定位后的樣本集分布情況， 左中部黑色圓點(diǎn)為WiFi指紋定位估計(jì)點(diǎn)。圖5(c)為未進(jìn)行WiFi定位的全局定位(GLOBAL-PF 全局定位)迭代50次之后的最終樣本分布，標(biāo)號(hào)3表示實(shí)際位置，標(biāo)號(hào)4表示估計(jì)位置；圖5 (d)為進(jìn)行WiFi初始定位的全局定位(WiFi-PF全局定位)迭代50次之后的最終樣本分布。它 們的誤差隨迭代次數(shù)變化曲線如圖6所示。
[0099]圖6(a)為已知的GL0BAL-PF全局定位的誤差隨迭代次數(shù)變化曲線，可以看出當(dāng)誤 差達(dá)到2m左右后不再下降。圖6(b)為依據(jù)本發(fā)明的WiFi-PF全局定位誤差隨迭代次數(shù)的變 化曲線，可以看出誤差最終小于0.5m。圖7為GL0BAL-PF全局定位、WiFi-PF全局定位和WiFi 指紋定位的平均誤差對(duì)比。由圖7和圖8可以看出，WiFi-PF全局定位的定位誤差最小，在 0.5m左右，可以滿足服務(wù)機(jī)器人定位精度要求。圖8為GL0BAL-PF全局定位和WiFi -PF全局定 位迭代50次所用的時(shí)間對(duì)比，說明WiFi指紋定位的引入有效縮短了服務(wù)機(jī)器人的迭代時(shí) 間。本實(shí)驗(yàn)通過WiFi指紋定位、GL0BAL-PF全局定位和WIFI-PF全局定位之間對(duì)比說明，依據(jù) 本發(fā)明實(shí)施例的系統(tǒng)可有效提高服務(wù)機(jī)器人定位精度和縮短迭代時(shí)間。
[0100] 綜上所述：
[0101] 依據(jù)本發(fā)明較佳的實(shí)施例，針對(duì)機(jī)器人在沒有任何初始位姿先驗(yàn)知識(shí)的情況下通 過傳感器感知信息確定位姿的全局定位問題，結(jié)合智能空間WiFi指紋定位和蒙特卡洛算法 的粒子濾波方法，設(shè)計(jì)了一種智能空間與R0S服務(wù)機(jī)器人相結(jié)合的復(fù)合全局定位系統(tǒng)。該全 局定位系統(tǒng)首先在智能空間中進(jìn)行基于BP神經(jīng)網(wǎng)絡(luò)的WIFI指紋粗定位并將估計(jì)位置與估 計(jì)誤差發(fā)送給R0S服務(wù)機(jī)器人，機(jī)器人結(jié)合激光器信息、里程計(jì)和地圖信息利用基于蒙特卡 洛算法的粒子濾波方法最終獲得服務(wù)機(jī)器人的精確位置。
[0102] 該全局定位系統(tǒng)實(shí)現(xiàn)了 R0S機(jī)器人與智能空間之間的零配置與松耦合，R0S服務(wù)機(jī) 器人可動(dòng)態(tài)加入或離開智能空間，不需要人工干預(yù)并實(shí)現(xiàn)R0S服務(wù)機(jī)器人在智能空間下的 精確全局定位。在實(shí)際環(huán)境中的實(shí)驗(yàn)說明了基于WiFi指紋與粒子濾波的服務(wù)機(jī)器人全局定 位系統(tǒng)可有效提高服務(wù)機(jī)器人定位精度和縮短迭代時(shí)間。為進(jìn)一步改善WiFi指紋定位誤差 過大時(shí)的全局定位結(jié)果，可以利用指紋信息中的特征判斷神經(jīng)網(wǎng)絡(luò)的結(jié)果是否正確，并利 用粒子濾波的定位結(jié)果自適應(yīng)校準(zhǔn)神經(jīng)網(wǎng)絡(luò)，這將是
【申請(qǐng)人】在本發(fā)明的基礎(chǔ)下一步需要深 入研究的工作。
【主權(quán)項(xiàng)】
1. 一種機(jī)器人定位方法，待定位的機(jī)器人基于WiFi接入無線局域網(wǎng)，其特征在于，包括 以下步驟： 基于WiFi指紋定位確定機(jī)器人當(dāng)前所在的估計(jì)位置； 調(diào)用所述估計(jì)位置的地圖并在該地圖約束的估計(jì)位置撒點(diǎn)，采用粒子濾波算法得到機(jī) 器人的精確位置。2. 根據(jù)權(quán)利要求1所述的機(jī)器人定位方法，其特征在于，所述估計(jì)位置為機(jī)器人估計(jì)點(diǎn) 位和估計(jì)點(diǎn)位誤差所約束。3. 根據(jù)權(quán)利要求2所述的機(jī)器人定位方法，其特征在于，所述粒子濾波算法為基于蒙特 卡洛算法的粒子濾波算法。4. 根據(jù)權(quán)利要求3所述的機(jī)器人定位方法，其特征在于，1)預(yù)測(cè)：所述粒子濾波算法以 前一時(shí)刻基于粒子分布所采樣的用于表征機(jī)器人狀態(tài)的離散數(shù)據(jù)及機(jī)器人在相應(yīng)機(jī)器人 狀態(tài)時(shí)的概率作為先驗(yàn)概率密度分布的采樣集； 2)更新：以機(jī)器人所搭載激光測(cè)距傳感器的信息修正所述采樣集，用以更新采樣集中 樣本的權(quán)重值，得到估計(jì)位置的后驗(yàn)數(shù)據(jù)，進(jìn)而增加權(quán)重較大的粒子的數(shù)目，減少權(quán)重較小 的粒子的數(shù)目，產(chǎn)生在估計(jì)位置的粒子的重新分布，而產(chǎn)生新的采樣集； 迭代預(yù)定次數(shù)的步驟2)，輸出權(quán)重最大的粒子所適配的機(jī)器人位置估計(jì)作為機(jī)器人位 置的精確值； 其中，所述地圖為匹配所述激光測(cè)距傳感器的激光地圖。5. 根據(jù)權(quán)利要求4所述的機(jī)器人定位方法，其特征在于，基于機(jī)器人地圖和里程計(jì)信息 所匹配的運(yùn)動(dòng)學(xué)模型產(chǎn)生采樣集中的樣本，該樣本即所述離散數(shù)據(jù)和相應(yīng)的概率。6. 根據(jù)權(quán)利要求4所述的機(jī)器人定位方法，其特征在于，更新步驟中的增加權(quán)重較大的 粒子的數(shù)目，減少權(quán)重較小的粒子的數(shù)目的方法是： 基于樣本的當(dāng)前權(quán)重隨機(jī)出預(yù)定數(shù)目的樣本。7. 根據(jù)權(quán)利要求4所述的機(jī)器人定位方法，其特征在于，在更新步驟中，在更新采樣集 中樣本的權(quán)重值前，對(duì)樣本的權(quán)重進(jìn)行歸一化處理。8. 根據(jù)權(quán)利要求4所述的機(jī)器人定位方法，其特征在于，基于激光測(cè)距傳感器更新采樣 集中樣本的權(quán)重值是以在采樣集條件下基于激光測(cè)距傳感器采樣得到的機(jī)器人狀態(tài)的發(fā) 生概率。9. 根據(jù)權(quán)利要求1-8任一所述的機(jī)器人定位方法，其特征在于，WiFi指紋定位為基于BP 神經(jīng)網(wǎng)絡(luò)的WiFi指紋定位。10. 根據(jù)權(quán)利要求9所述的機(jī)器人定位方法，其特征在于，支持BP神經(jīng)網(wǎng)絡(luò)的AP點(diǎn)在室 內(nèi)的分布包括對(duì)室內(nèi)機(jī)器人移動(dòng)區(qū)域熱點(diǎn)的中心分布和在移動(dòng)區(qū)域外圍的邊緣分布； 其中，中心分布是在一個(gè)矩形空間內(nèi)的一個(gè)對(duì)角線上的若干AP陣列，邊緣分布則是在 與AP陣列相交叉的方向上相異的兩端的各一個(gè)AP。
【文檔編號(hào)】G01S5/02GK105911518SQ201610195047
【公開日】2016年8月31日
【申請(qǐng)日】2016年3月31日
【發(fā)明人】路飛, 田國會(huì), 劉國良, 王宇恒
【申請(qǐng)人】山東大學(xué)