一種基于無(wú)線傳感器網(wǎng)絡(luò)的移動(dòng)機(jī)器人自動(dòng)定位算法
【專(zhuān)利摘要】本發(fā)明公開(kāi)了一種基于無(wú)線傳感器網(wǎng)絡(luò)的移動(dòng)機(jī)器人自動(dòng)定位算法，屬于移動(dòng)機(jī)器人自動(dòng)定位領(lǐng)域。包括以下步驟：均勻安裝無(wú)線傳感器節(jié)點(diǎn)，將工作空間分割為多個(gè)等大小的矩形柵格，各個(gè)節(jié)點(diǎn)知道其標(biāo)定位置。移動(dòng)機(jī)器人在行進(jìn)過(guò)程中，其所在矩形柵格4個(gè)傳感器節(jié)點(diǎn)通過(guò)無(wú)線通信將其位置及速度信息告知機(jī)器人。移動(dòng)機(jī)器人對(duì)節(jié)點(diǎn)信息先通過(guò)局部卡爾曼濾波器分別進(jìn)行局部最優(yōu)估計(jì)；再利用全局濾波器，通過(guò)一定的權(quán)值分配策略，將各局部濾波器結(jié)果進(jìn)行融合，得到最優(yōu)融合結(jié)果。本發(fā)明中采用融合--反饋式的聯(lián)邦卡爾曼濾波器，將各局部濾波器的局部最優(yōu)估計(jì)送入全局濾波器，進(jìn)行信息融合，得到全局最優(yōu)融合，提高了定位精度。
【專(zhuān)利說(shuō)明】一種基于無(wú)線傳感器網(wǎng)絡(luò)的移動(dòng)機(jī)器人自動(dòng)定位算法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明屬于移動(dòng)機(jī)器人自動(dòng)定位領(lǐng)域，涉及一種基于無(wú)線傳感器網(wǎng)絡(luò)的移動(dòng)機(jī)器人自動(dòng)定位算法。
【背景技術(shù)】
[0002]隨著計(jì)算機(jī)技術(shù)、傳感技術(shù)等的發(fā)展，機(jī)器人也從最初在工業(yè)流水線上執(zhí)行簡(jiǎn)單、重復(fù)的作業(yè)到具有更高的智能，能夠完成更復(fù)雜的任務(wù)，從靜止走向了移動(dòng)。隨著機(jī)器人性能不斷地完善，移動(dòng)機(jī)器人的應(yīng)用范圍大為擴(kuò)展，不僅在工業(yè)，農(nóng)業(yè)，國(guó)防，醫(yī)療，服務(wù)等行業(yè)中得到廣泛的應(yīng)用，而且在排雷，搜捕，救援，輻射和空間領(lǐng)域等有害與危險(xiǎn)場(chǎng)合得到很好的應(yīng)用。因此，移動(dòng)機(jī)器人技術(shù)已經(jīng)得到世界各國(guó)的普遍關(guān)注。自動(dòng)定位技術(shù)是其研究核心，也是實(shí)現(xiàn)真正智能化和完全自主移動(dòng)的關(guān)鍵技術(shù)。目前，移動(dòng)機(jī)器人常用的定位方法主要有航跡推算、信號(hào)燈定位、基于地圖的定位、路標(biāo)定位、基于視覺(jué)的定位等。各種定位方法的研究無(wú)非是為了高精度的定位結(jié)果，這就要求機(jī)器人在沒(méi)有人的干預(yù)下自行分析推算出自己的精確位置，將自己的位置和速度信息顯示并存儲(chǔ)。
[0003]目前，移動(dòng)機(jī)器人的自動(dòng)定位方面存在著以下的困難:
[0004](I)定位精度有限；
[0005](2)外部環(huán)境對(duì)定位的影響； [0006](3)材料成本較高；
[0007](4)缺乏完備的自主性；
[0008]隨著無(wú)線傳感器網(wǎng)絡(luò)的不斷發(fā)展，如何將無(wú)線傳感器網(wǎng)絡(luò)與移動(dòng)機(jī)器人技術(shù)相結(jié)合成為當(dāng)今科學(xué)家的熱門(mén)課題之一。步入21世紀(jì)以來(lái)，通過(guò)無(wú)線傳感器網(wǎng)絡(luò)對(duì)移動(dòng)機(jī)器人定位涌現(xiàn)了許多方法，但是已提出的理論和方法仍然存在許多不足，需要進(jìn)一步完善。
[0009]移動(dòng)機(jī)器人自動(dòng)定位算法實(shí)際上主要解決數(shù)據(jù)來(lái)源和數(shù)據(jù)處理兩個(gè)問(wèn)題:如何獲取位置信息并對(duì)其處理，無(wú)線傳感器網(wǎng)絡(luò)為其提供了一個(gè)很好地途徑。無(wú)線傳感器網(wǎng)絡(luò)定位技術(shù)具有自主性及可靠性高、成本低、使用靈活、易于安排等特點(diǎn)，但在實(shí)際應(yīng)用中無(wú)線傳感器網(wǎng)絡(luò)所得的數(shù)據(jù)信息或多或少會(huì)受到外部溫度、噪聲等因素的影響，那么濾波方法及信息融合對(duì)于移動(dòng)機(jī)器人定位的精度就起著至關(guān)重要的作用。

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

[0010]為解決移動(dòng)機(jī)器人自動(dòng)定位存在的上述技術(shù)問(wèn)題，，本發(fā)明的目的在于提供一種基于無(wú)線傳感器網(wǎng)絡(luò)的移動(dòng)機(jī)器人自動(dòng)定位算法。利用融合--反饋式的聯(lián)邦卡爾曼濾波對(duì)傳感器節(jié)點(diǎn)的信息進(jìn)行濾波、融合、加權(quán)，從而實(shí)現(xiàn)對(duì)移動(dòng)機(jī)器人的定位。該方法在移動(dòng)機(jī)器人定位的精度方面明顯優(yōu)于傳統(tǒng)無(wú)線傳感器網(wǎng)絡(luò)濾波方法。
[0011]為達(dá)到上述目的，本發(fā)明提供如下技術(shù)方案:
[0012]一種基于無(wú)線傳感器網(wǎng)絡(luò)的移動(dòng)機(jī)器人自動(dòng)定位算法。包括以下步驟:
[0013]步驟一:在給定空間中均勻安裝無(wú)線傳感器節(jié)點(diǎn)，從而將空間分割為多個(gè)等大小的矩形柵格，每個(gè)節(jié)點(diǎn)均知道其標(biāo)定的位置，移動(dòng)機(jī)器人在行進(jìn)過(guò)程中，其所在矩形柵格的臨近4個(gè)傳感器節(jié)點(diǎn)通過(guò)無(wú)線通信將其位置告知機(jī)器人；
[0014]步驟二:移動(dòng)機(jī)器人接收傳感器節(jié)點(diǎn)的位置及速度信息后，對(duì)各節(jié)點(diǎn)的信息進(jìn)行局部卡爾曼最優(yōu)估計(jì)，同時(shí)得到估計(jì)誤差協(xié)方差矩陣；
[0015]步驟三:采用融合一反饋模式聯(lián)邦卡爾曼濾波器，將各局部濾波器的局部最優(yōu)估計(jì)送入全局濾波器，進(jìn)行信息融合，得到全局最優(yōu)融合；
[0016]步驟四:全局濾波器的過(guò)程信息按照一定的分配準(zhǔn)則與局部濾波器進(jìn)行信息分配，同時(shí)利用信息分配因子分別對(duì)各局部濾波器進(jìn)行加權(quán)；
[0017]步驟五:移動(dòng)機(jī)器人將最終位置顯示在顯示屏上，并把自己的位置信息存入數(shù)據(jù)庫(kù)。
[0018]本發(fā)明的有益技術(shù)效果為:本發(fā)明中采用融合一反饋式的聯(lián)邦卡爾曼濾波器，將各局部濾波器的局部最優(yōu)估計(jì)送入全局濾波器，進(jìn)行信息融合，得到全局最優(yōu)融合，提高了定位精度；通過(guò)全局濾波器按照分配準(zhǔn)則與局部濾波器進(jìn)行信息分配并利用信息分配因子對(duì)局部濾波器的數(shù)據(jù)進(jìn)行加權(quán)，有效地減小了局部卡爾曼濾波的誤差影響，并且在不改變測(cè)量噪聲協(xié)方差矩陣的情況下，通過(guò)改變分配因子來(lái)達(dá)到間接改變各子系統(tǒng)估計(jì)誤差的目的，從而達(dá)到全局最優(yōu)濾波，大幅提聞定位精度。
【專(zhuān)利附圖】

【附圖說(shuō)明】
[0019]為了使本發(fā)明的目的、技術(shù)方案和有益效果更加清楚，本發(fā)明提供如下附圖進(jìn)行說(shuō)明:
[0020]圖1為本發(fā)明所述自動(dòng)定位方法的流程圖
[0021]圖2為移動(dòng)機(jī)器人在無(wú)線傳感器網(wǎng)絡(luò)中的移動(dòng)軌跡圖
[0022]圖3為基于無(wú)線傳感器網(wǎng)絡(luò)的移動(dòng)機(jī)器人聯(lián)邦卡爾曼定位算法的流程圖
【具體實(shí)施方式】
[0023]下面將結(jié)合附圖，對(duì)本發(fā)明的優(yōu)選實(shí)施例進(jìn)行詳細(xì)的描述。
[0024]發(fā)明中采用融合一反饋式的聯(lián)邦卡爾曼濾波器，將各局部濾波器的局部最優(yōu)估計(jì)送入全局濾波器，進(jìn)行信息融合，得到全局最優(yōu)融合，提高了定位精度；通過(guò)全局濾波器按照分配準(zhǔn)則與局部濾波器進(jìn)行信息分配并利用信息分配因子對(duì)局部濾波器的數(shù)據(jù)進(jìn)行加權(quán)，有效地減小了局部卡爾曼濾波的誤差影響，并且在不改變測(cè)量噪聲協(xié)方差矩陣的情況下，通過(guò)改變分配因子來(lái)達(dá)到間接改變各子系統(tǒng)估計(jì)誤差的目的，從而達(dá)到全局最優(yōu)濾波，大幅提聞定位精度。
[0025]圖1為本發(fā)明所述移動(dòng)機(jī)器人自動(dòng)定位方法的流程圖。如圖所示，該基于無(wú)線傳感器網(wǎng)絡(luò)的移動(dòng)機(jī)器人定位包括兩個(gè)工作過(guò)程，即數(shù)據(jù)來(lái)源和數(shù)據(jù)處理。本發(fā)明采用基于無(wú)線傳感器網(wǎng)絡(luò)的移動(dòng)機(jī)器人自動(dòng)定位過(guò)程如下:
[0026](I)在給定空間中均勻安裝無(wú)線傳感器節(jié)點(diǎn)，從而將空間分割為多個(gè)等大小的矩形柵格，各個(gè)節(jié)點(diǎn)知道其標(biāo)定位置；
[0027](2)移動(dòng)機(jī)器人在行進(jìn)過(guò)程中，其所在矩形柵格的臨近4個(gè)傳感器節(jié)點(diǎn)通過(guò)無(wú)線通信將其位置信息告知機(jī)器人；[0028]圖2為移動(dòng)機(jī)器人在無(wú)線傳感器網(wǎng)絡(luò)中的二維空間圖。在二維空間中，在采樣k時(shí)刻，將機(jī)器人所在矩形柵格節(jié)點(diǎn)分別編號(hào)為ak、bk、ck、dk。節(jié)點(diǎn)ak、bk、ck、dk將其位置信息[xa，ya]，[xb，yb]，[xc, yc]? [xd, yd]發(fā)送給機(jī)器人。χ、y表示為橫向及縱向位置。當(dāng)機(jī)器人進(jìn)入下一個(gè)柵格時(shí)，其所在的矩形柵格節(jié)點(diǎn)分別編號(hào)為ak+1、bk+1、ck+1、dk+1。點(diǎn)ak+1、bk+1、Ck+1、dk+1再將其信息發(fā)送給機(jī)器人，不斷重復(fù)，從而實(shí)現(xiàn)機(jī)器人的不斷定位。
[0029](3)移動(dòng)機(jī)器人接收傳感器節(jié)點(diǎn)的位置及速度信息后，對(duì)各節(jié)點(diǎn)的信息進(jìn)行局部卡爾曼最優(yōu)估計(jì)，同時(shí)得到估計(jì)誤差協(xié)方差矩陣。圖3為基于無(wú)線傳感器網(wǎng)絡(luò)的移動(dòng)機(jī)器人聯(lián)邦卡爾曼定位算法的流程圖，具體實(shí)施步驟如下:
[0030]根據(jù)不同傳感器的測(cè)量值，設(shè)小車(chē)的狀態(tài)變量為X= [X，vx，y，vy]T，其中:x、y表示橫向及縱向位置，Vx, Vy分別為橫向及縱向速度。根據(jù)節(jié)點(diǎn)a、b、C、d提供的不同位置信息設(shè)計(jì)4個(gè)局部濾波器。狀態(tài)方程為
[0031]Xk l =AXk +V1k (I)
[0032]其中i為a、b、C、d。其中:A為狀態(tài)移矩陣h力過(guò)程噪聲,滿足均值為零,標(biāo)準(zhǔn)
差為σ丨的高斯分布，并記Qi力?,的協(xié)方差矩陣；
[0033]下標(biāo)k表示更新時(shí)刻。以T表示定位信息更新周期，則
【權(quán)利要求】
1.一種基于無(wú)線傳感器網(wǎng)絡(luò)的移動(dòng)機(jī)器人自動(dòng)定位算法；在給定空間中均勻安裝無(wú)線傳感器節(jié)點(diǎn)，從而將空間分割為多個(gè)等大小的矩形柵格，各個(gè)節(jié)點(diǎn)知道其標(biāo)定位置；移動(dòng)機(jī)器人在行進(jìn)過(guò)程中，其所在矩形柵格的鄰近4個(gè)傳感器節(jié)點(diǎn)通過(guò)無(wú)線通信將其位置及速度信息告知機(jī)器人，移動(dòng)機(jī)器人對(duì)周?chē)?節(jié)點(diǎn)給予的位置及速度信息先通過(guò)局部卡爾曼濾波器分別進(jìn)行局部最優(yōu)估計(jì)；再利用全局濾波器，通過(guò)一定的權(quán)值分配策略，將各局部濾波器結(jié)果進(jìn)行融合，得到全局最優(yōu)融合，再把全局濾波器的過(guò)程信息按照一定的分配準(zhǔn)則與局部濾波器進(jìn)行信息分配，同時(shí)利用信息分配因子分別對(duì)各局部濾波器進(jìn)行加權(quán)，將結(jié)果信息顯示出來(lái)并存；具體包括以下步驟: 步驟一:在給定空間中均勻安裝無(wú)線傳感器節(jié)點(diǎn)，從而將空間分割為多個(gè)等大小的矩形柵格，每個(gè)節(jié)點(diǎn)均知道其標(biāo)定的位置，移動(dòng)機(jī)器人在行進(jìn)過(guò)程中，其所在矩形柵格的臨近4個(gè)傳感器節(jié)點(diǎn)通過(guò)無(wú)線通信將其位置告知機(jī)器人； 步驟二:移動(dòng)機(jī)器人接收傳感器節(jié)點(diǎn)的位置及速度信息后，對(duì)各節(jié)點(diǎn)的信息進(jìn)行局部卡爾曼最優(yōu)估計(jì)，同時(shí)得到估計(jì)誤差協(xié)方差矩陣； 步驟三:采用融合一反饋模式聯(lián)邦卡爾曼濾波器，將各局部濾波器的局部最優(yōu)估計(jì)送入全局濾波器，進(jìn)行信息融合，得到全局最優(yōu)融合； 步驟四:全局濾波器的過(guò)程信息按照一定的分配準(zhǔn)則與局部濾波器進(jìn)行信息分配，同時(shí)利用信息分配因子分別對(duì)各局部濾波器進(jìn)行加權(quán)； 步驟五:移動(dòng)機(jī)器人將最終位置顯示在顯示屏上，并把自己的位置和速度存入數(shù)據(jù)庫(kù)。
2.根據(jù)權(quán)利要求1所述的基于無(wú)線傳感器網(wǎng)絡(luò)的移動(dòng)機(jī)器人自動(dòng)定位算法，其特征在于:步驟一中，在二維空間中，在采樣k時(shí)刻，將小車(chē)所在矩形柵格節(jié)點(diǎn)分別編號(hào)為a，b，c，d ;節(jié)點(diǎn)a,b,c,d將其位置信息[xa, ya]，[xb, yb]，[xc, y。]，[xd, yd]發(fā)送給小車(chē);x、y表示為橫向及縱向位置。
3.根據(jù)權(quán)利要求1所述的基于無(wú)線傳感器網(wǎng)絡(luò)的移動(dòng)機(jī)器人自動(dòng)定位算法，其特征在于:步驟二所述的子卡爾曼濾波器對(duì)小車(chē)所在柵格的鄰近傳感器位置信息進(jìn)行局部卡爾曼最優(yōu)估計(jì)；根據(jù)不同傳感器的測(cè)量值，設(shè)小車(chē)的狀態(tài)變量為X= [x，vx，y，vy]T，其中:x、y表示為橫向及縱向位置，vx、Vy分別為橫向及縱向速度；根據(jù)節(jié)點(diǎn)a，b，c，d提供的不同位置信息設(shè)計(jì)4個(gè)局部濾波器；機(jī)器人位置狀態(tài)方程力Xk =AX[+V'k，i為a，b，c，d ;其中:A為狀態(tài)轉(zhuǎn)移矩陣；Vk為過(guò)程噪聲，滿足均值為零，標(biāo)準(zhǔn)差為 < 的高斯分布，并記Q為Vk的協(xié)方差矩陣；下標(biāo)k表示更新時(shí)刻；以T表示定位信息更新周期，則
4.根據(jù)權(quán)利要求1所述的基于無(wú)線傳感器網(wǎng)絡(luò)的移動(dòng)機(jī)器人自動(dòng)定位算法，其特征在于:步驟三所述的利用全局濾波器將局部最優(yōu)估計(jì)與全局濾波器狀態(tài)信息進(jìn)行融合，具體步驟為: 1)將各局部濾波器的局部最優(yōu)估計(jì)送入全局濾波器；
2)按式(
5.根據(jù)權(quán)利要求1所述的基于無(wú)線傳感器網(wǎng)絡(luò)的移動(dòng)機(jī)器人自動(dòng)定位算法，其特征在于:步驟四中，全局濾波器中不進(jìn)行測(cè)量，只進(jìn)行時(shí)間更新，因此將主濾波器的過(guò)程信息按 照式
6.一種應(yīng)用權(quán)利要求1至5中任一項(xiàng)所述的基于無(wú)線傳感器網(wǎng)絡(luò)的移動(dòng)機(jī)器人自動(dòng)定位算法。
【文檔編號(hào)】G01S5/00GK104035067SQ201410261581
【公開(kāi)日】2014年9月10日 申請(qǐng)日期:2014年6月13日 優(yōu)先權(quán)日:2014年6月13日
【發(fā)明者】魏善碧, 屈劍鋒, 陸震宇, 倪政, 林哲明, 周展 申請(qǐng)人:重慶大學(xué)