[發(fā)明專利]面向高速無人旋翼飛行器的室內(nèi)定位方法

在審

• 申請(qǐng)?zhí)枺?/td>: 201410322947.4
• 申請(qǐng)日: 2014-07-08
• 公開（公告）號(hào): CN104159293A
• 公開（公告）日: 2014-11-19
• 發(fā)明（設(shè)計(jì)）人: 于拓;周子龍;張陽;張哲慧;吳煒捷;楊峰;王新兵
• 申請(qǐng)（專利權(quán)）人: 上海交通大學(xué)
• 主分類號(hào): H04W64/00
• 分類號(hào): H04W64/00
• 代理公司: 上海漢聲知識(shí)產(chǎn)權(quán)代理有限公司 31236
• 代理人: 郭國中;樊昕

1.一種面向高速無人旋翼飛行器的室內(nèi)定位方法，其特征在于，包括如下步驟：

步驟1：在需要提供定位服務(wù)的區(qū)域內(nèi)設(shè)置多個(gè)無線路由器；

步驟2：將需要提供定位服務(wù)的區(qū)域劃分為多個(gè)正方形網(wǎng)格，并測(cè)量各個(gè)網(wǎng)格內(nèi)的來自于各個(gè)無線路由器的WiFi信號(hào)強(qiáng)度，并將測(cè)量的結(jié)果上傳至定位服務(wù)器；

步驟3：無人旋翼飛行器在飛行過程中持續(xù)測(cè)量來自于各個(gè)無線路由器的WiFi信號(hào)強(qiáng)度，并存儲(chǔ)于本地；

步驟4：飛行器需要定位時(shí)，向服務(wù)器請(qǐng)求定位，并上傳目前為止的WiFi信號(hào)強(qiáng)度測(cè)量數(shù)據(jù)；

步驟5：服務(wù)器根據(jù)上傳的測(cè)量數(shù)據(jù)，計(jì)算飛行器位置，并將結(jié)果發(fā)送給飛行器；

步驟6：飛行器接收定位結(jié)果。

2.根據(jù)權(quán)利要求1所述的面向高速無人旋翼飛行器的室內(nèi)定位方法，其特征在于，所述步驟1中的無線路由器均正常工作于SSID廣播可見模式，即以固定周期發(fā)送Beacon廣播信號(hào)。

3.根據(jù)權(quán)利要求1所述的面向高速無人旋翼飛行器的室內(nèi)定位方法，其特征在于，所述步驟2包括如下步驟：

步驟2.1：每個(gè)網(wǎng)格內(nèi)的WiFi信號(hào)強(qiáng)度測(cè)試過程為：在每個(gè)網(wǎng)格內(nèi)針對(duì)各個(gè)無線路由器分別測(cè)量N次來自于無線路由器的WiFi信號(hào)輻射強(qiáng)度，并將測(cè)量結(jié)果上傳至定位服務(wù)器；

步驟2.2：定位服務(wù)器根據(jù)上傳的測(cè)量結(jié)果，進(jìn)行以下計(jì)算：

設(shè)步驟2中網(wǎng)格總數(shù)為W，則對(duì)于網(wǎng)格w，計(jì)算得到以下一系列函數(shù)：

pwj(o)=1NΣi=1N12πσexp(-(o-owji)2σ2),j=1,2,...,M,w=1,2,...,W]]>

其中，p_wj(·)表示在網(wǎng)格w處接收到的來自于第j個(gè)路由器的信號(hào)強(qiáng)度概率密度分布函數(shù)，N為在網(wǎng)格w內(nèi)的測(cè)試次數(shù)，o_wji為在網(wǎng)格w處接收到的來自于第j個(gè)路由器的信號(hào)強(qiáng)度的第i次測(cè)試所得到的值，σ為常數(shù)，M為無線路由器的總數(shù)，o表示接收信號(hào)強(qiáng)度；

定位服務(wù)器將以上計(jì)算所得的函數(shù)p_wj(o)存儲(chǔ)于數(shù)據(jù)庫中。

4.根據(jù)權(quán)利要求1所述的面向高速無人旋翼飛行器的室內(nèi)定位方法，其特征在于，所述步驟3包括如下步驟：

步驟3.1：無人旋翼飛行器在飛行過程中以固定時(shí)間間隔T持續(xù)測(cè)量來自于各個(gè)無線路由器的WiFi信號(hào)強(qiáng)度，每次測(cè)量可以得到一組來自于各個(gè)無線路由器的信號(hào)t＝T,2T,...，其中是時(shí)間為t時(shí)，來自第j個(gè)路由器的信號(hào)強(qiáng)度，M為無線路由器的總數(shù)，時(shí)間間隔T的最小值等于無人飛行器執(zhí)行一次WiFi信號(hào)強(qiáng)度測(cè)量的最小耗時(shí)；

步驟3.2：所述無人旋翼飛行器安裝有蜂窩網(wǎng)或WiFi端口設(shè)備，能夠經(jīng)由蜂窩網(wǎng)或WiFi連接定位服務(wù)器。

5.根據(jù)權(quán)利要求3所述的面向高速無人旋翼飛行器的室內(nèi)定位方法，其特征在于，所述步驟5包括如下步驟：

步驟5.1：根據(jù)無人旋翼飛行器上傳所得到的所有WiFi信號(hào)強(qiáng)度測(cè)量數(shù)據(jù)，進(jìn)行以下計(jì)算：

對(duì)于網(wǎng)格w，計(jì)算以下概率值：

Pwt=Πj=1,...,Mpwj(ojt),w=1,...,W;t=T,2T,...KT]]>

其中P_wt為時(shí)間t時(shí)無人旋翼飛行器處于網(wǎng)格w中的概率，M為無線路由器的總數(shù)，p_wj(·)為步驟2.2中所得函數(shù)，o_jt為移動(dòng)終端在步驟3中在時(shí)間t時(shí)所測(cè)得的來自于第j個(gè)無線路由器的信號(hào)強(qiáng)度，KT為無人旋翼飛行器最后一次定位的時(shí)間；

最后，對(duì)于不同t，從P_wt,w＝1,...,W中找出值最大者或者最大者之一所對(duì)應(yīng)的網(wǎng)格w，該網(wǎng)格w即為所計(jì)算得出的在時(shí)間t時(shí)無人旋翼飛行器所在的位置，記為zt=xt′yt′,]]>t＝T,2T,...KT，其中，x′_t，y′_t為該網(wǎng)格中心的橫、縱坐標(biāo)值；

步驟5.2：使用卡爾曼濾波對(duì)z_t進(jìn)行處理，具體如下：

設(shè)定無人旋翼飛機(jī)運(yùn)動(dòng)狀態(tài)向量：

st=xtytvxtvyt]]>

其中，x_t，y_t為無人旋翼飛機(jī)在時(shí)間t時(shí)的位置橫縱坐標(biāo)值，v_xt，v_yt為無人旋翼飛機(jī)在時(shí)間t時(shí)的運(yùn)動(dòng)速度向量在橫、縱方向的分量值；

設(shè)定無人旋翼飛機(jī)運(yùn)動(dòng)模型矩陣：

F=10T0010T00100001]]>

設(shè)定運(yùn)動(dòng)方差矩陣：

Q=σ120000σ120000σ120000σ12]]>

其中為運(yùn)動(dòng)方差，為常數(shù)；

設(shè)定測(cè)量結(jié)果矩陣：

H=10000100]]>

設(shè)定測(cè)量方差矩陣：

R=σ2200σ22]]>

其中為測(cè)量方差，為常數(shù)；

卡爾曼濾波處理過程如下：從t＝T到t＝KT，執(zhí)行以下循環(huán)計(jì)算：

P‾t=FPt-TFT+Q]]>

Kt=P‾tHT(HP‾tHT+R)-1]]>

Pt=(E-KtH)P‾t]]>

其中P_t為時(shí)間t時(shí)的濾波矩陣，為時(shí)間t時(shí)的修正濾波矩陣，K_t為時(shí)間t時(shí)的濾波權(quán)重矩陣，E為單位矩陣，為時(shí)間t時(shí)的濾波結(jié)果，F(xiàn)^T、H^T分別表示F和H的轉(zhuǎn)置矩陣，循環(huán)的初始值設(shè)置為P0=0000000000000000,]]>

由此，可以得到卡爾曼濾波結(jié)果取作為定位結(jié)果輸出。