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

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

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