景曉娟，曹以龍，景旭川

(上海電力大學(xué) 電子與信息工程學(xué)院，上海 201306)

0 引 言

在室內(nèi)環(huán)境，衛(wèi)星信號(hào)的傳播受到嚴(yán)重阻礙，GPS等傳統(tǒng)定位系統(tǒng)的定位精度達(dá)不到要求，無人機(jī)無法正常飛行。因此研究人員提出了應(yīng)用各種方法的無人機(jī)室內(nèi)定位方案[1-3]。目前，無人機(jī)室內(nèi)定位方法主要方法分為基于超聲信息和基于視覺的定位。文獻(xiàn)[4]提出了一種利用超寬帶(UWB)無線電信號(hào)的無人機(jī)定方法，該方法通過將光線跟蹤算法的校正值應(yīng)用于UWB測(cè)距數(shù)據(jù)，可以改善非視線情況的定位性能，但是此方法需要部署多個(gè)定位基站，增加了成本。文獻(xiàn)[5]提出了一種基于WIFI信號(hào)測(cè)距的無人機(jī)室內(nèi)定位方法，但是基于距離的WIFI定位方法需要提前知道每個(gè)AP的精確位置，這造成了無人機(jī)定位實(shí)施的困難度。文獻(xiàn)[6]提出了一種將攝像機(jī)的光流數(shù)據(jù)與慣性傳感器信息融合的四旋翼無人機(jī)室內(nèi)定位方法，但是在視覺定位中圖像處理量巨大，一般計(jì)算機(jī)無法完成運(yùn)算，因此該方法耗電高且定位實(shí)時(shí)性較差。

由于WIFI接入點(diǎn)(AP)幾乎在每座建筑內(nèi)廣泛存在，基于WIFI接收信號(hào)強(qiáng)度(RSS)指紋的方法被廣泛使用[7]，這種方法無需安裝其它專用設(shè)備，成本低廉，但是無線信號(hào)傳播的不穩(wěn)定性會(huì)引起位置估計(jì)誤差，無人機(jī)的高速飛行更會(huì)嚴(yán)重影響定位精度。對(duì)此，本文提出了一種多傳感器協(xié)同WIFI指紋定位的無人機(jī)室內(nèi)定位方案。該方案解決了基于WIFI RSS指紋定位應(yīng)用于無人機(jī)室內(nèi)定位的一些突出問題，通過利用超聲波傳感器提供的無人機(jī)高度信息，減輕了WIFI室內(nèi)定位的計(jì)算量，提升了系統(tǒng)定位速度；通過利用機(jī)載慣性測(cè)量單元(IMU)信息和設(shè)計(jì)的卡爾曼濾波器以及提出的數(shù)據(jù)擬合方法提升了系統(tǒng)定位精度，使得本定位系統(tǒng)可應(yīng)用于無人機(jī)室內(nèi)定位。

1 定位系統(tǒng)框架

本文所提出的無人機(jī)室內(nèi)定位系統(tǒng)由兩部分組成：無人機(jī)客戶端和服務(wù)器，如圖1所示。無人機(jī)客戶端主要包含3個(gè)傳感器模塊。在飛行定位過程中，無人機(jī)客戶端中的WIFI模塊定時(shí)采集周圍AP的RSS信息及其物理地址(MAC)，同時(shí)，超聲波傳感器測(cè)量無人機(jī)在WIFI掃描周期的平均高度，慣性傳感器測(cè)量飛機(jī)的三軸加速度及位姿信息，這些測(cè)量數(shù)據(jù)通過移動(dòng)網(wǎng)絡(luò)/無線通信發(fā)送到服務(wù)器，服務(wù)器則根據(jù)采集的信息完成無人機(jī)的實(shí)時(shí)定位并把定位結(jié)果傳回給無人機(jī)。

圖1 無人機(jī)室內(nèi)定位系統(tǒng)框架

服務(wù)器包含有離線構(gòu)建的定位區(qū)域的WIFI RSS指紋數(shù)據(jù)庫(kù)，WIFI定位算法以及后續(xù)定位結(jié)果的優(yōu)化均是在此服務(wù)器中完成的。通過測(cè)量的RSS信息以及飛機(jī)高度數(shù)據(jù)，可以完成本文所提的WIFI定位方法。然后將此WIFI定位結(jié)果作為卡爾曼濾波器預(yù)測(cè)階段的輸入，結(jié)合無人機(jī)實(shí)時(shí)三軸加速度數(shù)據(jù)，完成對(duì)WIFI定位結(jié)果的優(yōu)化。結(jié)合無人機(jī)的直線飛行特點(diǎn)，再通過本系統(tǒng)的路徑擬合模塊，利用歷史定位數(shù)據(jù)，對(duì)無人機(jī)當(dāng)前的位置信息可以得到更準(zhǔn)確的估計(jì)。

2 WIFI指紋定位方法

WIFI指紋定位包含兩個(gè)階段：離線階段和在線階段。

離線階段：在定位空間內(nèi)各位置采集多個(gè)WIFI信號(hào)的接收信號(hào)強(qiáng)度，對(duì)同一點(diǎn)進(jìn)行多次測(cè)量采樣并取平均值，保證采集的數(shù)據(jù)庫(kù)信息的準(zhǔn)確性，把空間位置與各AP的RSS值存儲(chǔ)起來，形成WIFI指紋數(shù)據(jù)庫(kù)，如表1所示。

表1 WIFI指紋數(shù)據(jù)庫(kù)

其中，RPi(i=1,2,…,n)為采樣點(diǎn)，xi,yi,zi表示對(duì)應(yīng)的空間位置的三軸坐標(biāo)，MACj(j=1,2,…,m)表示第j個(gè)AP的MAC地址信息，RSSmn表示在第n個(gè)采樣點(diǎn)測(cè)得的MAC地址為MACm的AP的RSS值。

在線階段：在線定位階段主要是利用匹配算法將無人機(jī)當(dāng)前測(cè)量得到的WIFI信息與指紋數(shù)據(jù)庫(kù)中的信息進(jìn)行比較，進(jìn)而得到當(dāng)前時(shí)刻無人機(jī)在空間中所處的位置。本文采用的算法為加權(quán)k-近鄰法(WKNN)匹配算法[8,9]。WKNN的實(shí)現(xiàn)方法是比較無人機(jī)實(shí)時(shí)采集的WIFI信息特征與WIFI指紋數(shù)據(jù)庫(kù)中所有位置的WIFI信息特征的相似度，利用無人機(jī)在定位點(diǎn)實(shí)時(shí)采集的AP的MAC地址及其RSS，再計(jì)算與指紋庫(kù)數(shù)據(jù)中各位置相同MAC地址的RSS值之間的歐氏距離(Euclidean distance)的大小來衡量?jī)蓚€(gè)位置之間的相似程度。歐氏距較小的WIFI指紋數(shù)據(jù)庫(kù)中的位置，則認(rèn)為是與當(dāng)前位置更加接近[8]。比較指紋庫(kù)中所有位置與當(dāng)前位置的歐氏距離后，選擇歐氏距離最小的k個(gè)指紋庫(kù)位置作為實(shí)時(shí)接收信號(hào)的位置估計(jì)，具體的歐氏距離計(jì)算公式如式(1)所示

(1)

在計(jì)算n個(gè)歐式距離之后，選擇距離最小的k個(gè)指紋庫(kù)位置來估計(jì)無人機(jī)所處位置。由于歐氏距離越小的點(diǎn)理論上越接近無人機(jī)的位置，為了表示這種相關(guān)性，對(duì)k個(gè)參考點(diǎn)賦予一個(gè)權(quán)重wi，wi越大，表示參考點(diǎn)距離目標(biāo)點(diǎn)的歐氏距離越近，反之越遠(yuǎn)，wi可表示為

(2)

式中：di表示第i個(gè)指紋庫(kù)位置點(diǎn)到無人機(jī)目標(biāo)點(diǎn)的歐氏距離，ε表示一個(gè)值很小的調(diào)節(jié)因數(shù)，來防止計(jì)算的時(shí)候分母為零。利用權(quán)重因子wi對(duì)無人機(jī)位置進(jìn)行估計(jì)，如式(3)所示，得到的結(jié)果即為WKNN算法匹配到的位置

(3)

3 多傳感器與WIFI指紋定位的融合

3.1 結(jié)合超聲波傳感器的WIFI定位改進(jìn)

傳統(tǒng)的WIFI指紋定位方法是在二維平面進(jìn)行的，離線階段只需在一個(gè)平面選取參考位置并采集WIFI RSS指紋，但是應(yīng)用于無人機(jī)定位的WIFI定位方法需要選取多個(gè)平面的參考位置，因?yàn)闊o人機(jī)是在空間中運(yùn)動(dòng)的。這樣，相比于二維WIFI RSS指紋數(shù)據(jù)庫(kù)，三維WIFI RSS指紋數(shù)據(jù)庫(kù)中的參考點(diǎn)大大增多。離線數(shù)據(jù)庫(kù)采集時(shí)，若在一個(gè)平面選取m個(gè)參考位置，采集n個(gè)平面層的三維空間就會(huì)有m*n個(gè)參考位置，由式(1)可知，在一次WIFI定位計(jì)算中需要計(jì)算m*n個(gè)歐氏距離，并且在這m*n個(gè)歐氏距離中找出最小的k個(gè)位置用以估計(jì)無人機(jī)位置，由于大量的計(jì)算時(shí)間導(dǎo)致定位頻率較低，使得無人機(jī)無法準(zhǔn)確導(dǎo)航。顯然，簡(jiǎn)單地將傳統(tǒng)的WIFI指紋定位方法應(yīng)用于無人機(jī)室內(nèi)定位是不可行的。

本文通過在無人機(jī)上配置超聲波傳感器來解決這一問題。超聲波傳感器測(cè)量無人機(jī)高度位置，知道無人機(jī)高度信息后，WIFI指紋定位在線階段只需選取此高度的上、下兩個(gè)平面的參考位置進(jìn)行定位，參考點(diǎn)大大減少。這樣只需計(jì)算m*2個(gè)歐式距離，相比于普通匹配方法，減少了m*(n-2)個(gè)點(diǎn)的歐式距離的計(jì)算量，顯然WIFI定位速度會(huì)更快。具體實(shí)現(xiàn)方法如圖2所示，在一個(gè)WIFI掃描周期中，超聲波傳感器不斷測(cè)量無人機(jī)高度，并將結(jié)果累加，掃描結(jié)束后計(jì)算掃描周期內(nèi)無人機(jī)所處的平均高度H，設(shè)建立數(shù)據(jù)庫(kù)時(shí)兩個(gè)參考點(diǎn)平面高度相差Δz，通過H/Δz得到當(dāng)前應(yīng)選取的兩個(gè)平面的參考點(diǎn)。

圖2 改進(jìn)位置匹配算法

3.2 慣導(dǎo)信息與WIFI定位位置的融合

由于無人機(jī)是一直運(yùn)動(dòng)的，加上RSS存在波動(dòng)，故WIFI指紋定位的初步位置是不能直接使用的?？柭鼮V波器是解決隨機(jī)信號(hào)預(yù)測(cè)和隨機(jī)信號(hào)與隨機(jī)噪聲分離的著名方法之一，可對(duì)初步定位位置進(jìn)行最優(yōu)估計(jì)[10,11]。因此，本文將WIFI定位結(jié)果作為卡爾曼濾波器預(yù)測(cè)階段的輸入，通過卡爾曼濾波器融合慣導(dǎo)數(shù)據(jù)得到更準(zhǔn)確的無人機(jī)位置估計(jì)。利用卡爾曼濾波模型對(duì)無人機(jī)的運(yùn)動(dòng)狀態(tài)進(jìn)行狀態(tài)方程建模，狀態(tài)方程如式(4)所示

Xk=AXk-1+BUk-1+ωk-1

(4)

慣性傳感器測(cè)量的無人機(jī)加速度信息作為輸入，可以得到無人機(jī)的速度信息，公式如式(5)所示

(5)

式中：Δt是一個(gè)定位周期的時(shí)間間隔，利用速度信息，可以得到無人機(jī)的位置信息，公式如式(6)所示

(6)

根據(jù)卡爾曼狀態(tài)方程，可以得到A和B分別為

(7)

(8)

(9)

(10)

卡爾曼濾波過程如下：

(1)通過狀態(tài)方程來計(jì)算預(yù)測(cè)當(dāng)前時(shí)刻的位置

(11)

(2)計(jì)算預(yù)測(cè)當(dāng)前時(shí)刻誤差的協(xié)方差矩陣

Pk-=APk-1AT+Qk

(12)

(3)計(jì)算卡爾曼增益

Kk=Pk-HT(HPk-HT+Rk)-1

(13)

(4)預(yù)測(cè)當(dāng)前狀態(tài)向量

(14)

(5)更新協(xié)方差矩陣

Pk=(E6-KkH)Pk-

(15)

3.3 采用數(shù)據(jù)擬合方法對(duì)定位結(jié)果優(yōu)化

(1)當(dāng)無人機(jī)改變路徑，檢測(cè)到姿態(tài)角有較大變化之后并保持穩(wěn)定時(shí)，當(dāng)前定位周期設(shè)為T0。

4 仿真與分析

為了驗(yàn)證所提定位策略的有效性，在MATLAB平臺(tái)上搭建了仿真模型進(jìn)行模擬實(shí)驗(yàn)。本文設(shè)置了一個(gè)20 m*5 m*5 m的三維空間，為了模擬真實(shí)WIFI環(huán)境，在空間內(nèi)隨機(jī)選取8個(gè)位置設(shè)置8個(gè)AP，各AP發(fā)射功率同樣隨機(jī)設(shè)置，WIFI信號(hào)RSS符合無線信號(hào)衰減模型分布在空間內(nèi)。離線WIFI指紋庫(kù)建立時(shí)，設(shè)置采樣距離間隔0.5 m，對(duì)空間各點(diǎn)采樣(該位置所有AP的RSS值)，建立樣式如表1所示的WIFI指紋數(shù)據(jù)庫(kù)。

在空間內(nèi)創(chuàng)建一條目標(biāo)路徑，目標(biāo)點(diǎn)按此路徑移動(dòng)。移動(dòng)過程中，以5 Hz的頻率獲得目標(biāo)點(diǎn)所在位置的8組RSS，由于RSS是周期性波動(dòng)的以及無線信號(hào)的干擾，所以對(duì)獲得的RSS值添加了一個(gè)方差為-5的隨機(jī)噪聲。對(duì)于定位系統(tǒng)中超聲波傳感器的高度值測(cè)量，本文將目標(biāo)點(diǎn)的高度值加上方差為0.2的噪聲作為超聲波傳感器的測(cè)量值。慣性傳感器測(cè)量的三軸加速度信息，則是根據(jù)目標(biāo)點(diǎn)的加速度值加上方差為0.05的噪聲得到的。路徑擬合時(shí)所需的無人機(jī)路徑改變識(shí)別，則是根據(jù)目標(biāo)點(diǎn)的轉(zhuǎn)折代替的。

4.1 定位效果評(píng)估

模擬定位中，令目標(biāo)點(diǎn)先后沿(2,4,0.8),(2,4,1.8),(18,4,1.8),(18,4,3.8),(18,2,3.8),(2,2,3.8),(2,2,1.8)以0.5 m/s的速度直線移動(dòng)。圖3展示了一次模擬定位中系統(tǒng)各階段定位數(shù)據(jù)的三維圖?？梢钥吹?，WIFI定位位置分布于整個(gè)空間，說明僅使用WIFI RSS指紋定位方法會(huì)有較大定位誤差。當(dāng)采用了卡爾曼濾波器對(duì)WIFI指紋定位結(jié)果和無人機(jī)三軸加速度信息融合之后，可以看出定位位置比較接近真實(shí)路徑，這說明本文設(shè)計(jì)的卡爾曼濾波器發(fā)揮出了較好的效果，通過機(jī)身加速度信息與WIFI定位的結(jié)合，使定位精度得到了極大提升。圖4分別展示了該次模擬定位中卡爾曼濾波器對(duì)WIFI指紋定位結(jié)果的估計(jì)位置和采用二次多項(xiàng)式對(duì)此估計(jì)位置擬合后的位置軌跡在3個(gè)坐標(biāo)面的投影軌跡?？梢钥吹?，經(jīng)過卡爾曼估計(jì)的軌跡在每一個(gè)坐標(biāo)面的投影都比較接近真實(shí)軌跡，說明通過卡爾曼濾波器將機(jī)載慣性傳感器信息和指紋定位信息融合的效果在3個(gè)方向都有良好的效果?？柭烙?jì)位置路徑存在一定的抖動(dòng)，通過本文所提路徑擬合策略將估計(jì)坐標(biāo)擬合后，由圖3、圖4可以看出，經(jīng)過二次多項(xiàng)式擬合的路徑更加平滑，更接近于真實(shí)的軌跡。這說明在無人機(jī)趨于線性運(yùn)動(dòng)的情況下，本文所提的路徑擬合策略能有效減小定位位置的抖動(dòng)，提升定位系統(tǒng)精度。整體看來，本文所提的每一步定位策略都能起到一定作用，使得基于WIFI RSS指紋的定位方法成功應(yīng)用于無人機(jī)室內(nèi)定位中，且有較好的定位精度。

圖3 三維定位路徑

圖4 定位軌跡在3個(gè)平面的投影

4.2 定位頻率對(duì)定位精度的影響

為了驗(yàn)證本文所改進(jìn)的WIFI指紋定位中WKNN匹配策略是有效的，圖5(a)、圖5(b)分別展示了應(yīng)用改進(jìn)WIFI指紋定位和應(yīng)用傳統(tǒng)WIFI指紋定位誤差的累積分布(CDF)。因?yàn)樵谀M中不能體現(xiàn)處理器處理數(shù)據(jù)的時(shí)間耗費(fèi)，為了體現(xiàn)所提改進(jìn)WIFI指紋定位算法的快速性，我們將傳統(tǒng)的WIFI定位算法的輸出結(jié)果更新頻率設(shè)置為2.5 Hz，結(jié)合超聲波傳感器的改進(jìn)的WIFI定位算法輸出結(jié)果更新頻率設(shè)置為5 Hz。目標(biāo)點(diǎn)移動(dòng)速度恒為0.5 m/s，在空間中沿不同路徑移動(dòng)，兩種WIFI定位算法分別進(jìn)行10次定位模擬。從圖中可以看出，在采用改進(jìn)的WIFI RSS指紋定位方法后，WIFI定位CDF在誤差為4 m時(shí)達(dá)到1，這說明改進(jìn)的WIFI定位誤差在移動(dòng)速度為0.5 m/s的情況下都在4 m之內(nèi)。將WIFI定位結(jié)果經(jīng)過卡爾曼濾波器融合加速度數(shù)據(jù)后，定位誤差CDF在1.6 m達(dá)到1，這說明定位誤差均分布在1.6 m以內(nèi)。將卡爾曼濾波后的定位位置經(jīng)過本文的路徑擬合策略之后，定位誤差CDF在1.3 m達(dá)到1，說明該擬合方案是有效的。采用普通的WIFI RSS指紋定位算法，WIFI定位誤差CDF在誤差為4.3 m時(shí)達(dá)到1，在對(duì)定位位置進(jìn)行路徑估計(jì)和路徑擬合之后分別在1.8 m和1.5 m達(dá)到1。對(duì)比可知，改進(jìn)的WIFI RSS指紋定位算法因?yàn)闇p少了大量的計(jì)算時(shí)間，使得系統(tǒng)定位頻率提升，從而實(shí)現(xiàn)了更高的定位精度。同時(shí)，我們可以看到采用WIFI定位的初步定位結(jié)果誤差較大，是不能直接應(yīng)用到無人機(jī)室內(nèi)定位的，通過設(shè)計(jì)的卡爾曼濾波器融合慣性傳感器加速度數(shù)據(jù)后的定位精度有了大幅提升，增加一級(jí)路徑擬合策略后的定位誤差更小，實(shí)現(xiàn)了基于WIFI RSS指紋定位方法成功應(yīng)用于無人機(jī)室內(nèi)定位。

圖5 兩種WIFI定位方法下定位誤差累積分布

4.3 飛行速度對(duì)定位精度的影響

由于無人機(jī)能以不同的速度飛行，因此本文還評(píng)估了速度對(duì)系統(tǒng)定位精度的影響。分別讓目標(biāo)點(diǎn)以0.5 m/s，1 m/s，2 m/s，3 m/s的速度沿同一路徑移動(dòng)，進(jìn)行10次模擬定位，統(tǒng)計(jì)平均定位誤差，如表2所示?？梢钥闯?，定位誤差隨著無人機(jī)飛行速度的增加而增加，在飛行速度達(dá)到2 m/s時(shí)，定位誤差出現(xiàn)了一個(gè)較大的增幅。這是由于當(dāng)無人機(jī)以較高的速度運(yùn)動(dòng)時(shí)，WIFI RSS測(cè)量的次數(shù)會(huì)受到限制，這是RSS應(yīng)用于無人機(jī)的主要缺點(diǎn)。從表2還可以得出，使用本文的定位策略，無人機(jī)飛行速度為3 m/s時(shí)，平均定位誤差可降低至2 m，相比于原始的WIFI定位的3.5 m誤差，減小了42.8%，這個(gè)定位誤差對(duì)于無人機(jī)是可以正常工作的，說明本定位系統(tǒng)在無人機(jī)以較高速度飛行時(shí)仍能發(fā)揮作用，有效減小了WIFI定位的誤差，在無人機(jī)高速飛行時(shí)仍能保持較高的穩(wěn)定性。

表2 不同飛行速度下的定位誤差

相比于其它定位方案，本文所提方法基本上是RSS指紋匹配和無人機(jī)必備的傳感器，系統(tǒng)成本極低，定位功耗極低，實(shí)驗(yàn)的目的是驗(yàn)證低成本的基于WIFI RSS指紋識(shí)別的方法用于無人機(jī)室內(nèi)定位的可行性。從模擬結(jié)果來看，本文所提方法可實(shí)現(xiàn)基于WIFI RSS指紋定位的無人機(jī)室內(nèi)定位，并且有較好的效果。

5 結(jié)束語

本文提出了一種基于WIFI RSS指紋技術(shù)與多傳感器數(shù)據(jù)融合的四旋翼無人機(jī)室內(nèi)定位方法。由于傳統(tǒng)的WIFI RSS指紋定位方法不能直接用于無人機(jī)室內(nèi)定位，因此針對(duì)傳統(tǒng)的WIFI RSS指紋定位方法提出了改進(jìn)，所做的工作有：

(1)利用超聲波測(cè)量高度信息，根據(jù)測(cè)量得到的高度信息判斷無人機(jī)所處的高度層，利用該臨近兩個(gè)層面參考點(diǎn)進(jìn)行WIFIRSS指紋定位，提升位置的匹配速度，從而提高無人機(jī)定位的頻率。

(2)設(shè)計(jì)了卡爾曼濾波器，對(duì)WIFI指紋定位位置和慣性傳感器測(cè)量的數(shù)據(jù)進(jìn)行了融合，提升了無人機(jī)定位系統(tǒng)的定位精度。

(3)采用二次多項(xiàng)式數(shù)據(jù)擬合方法對(duì)卡爾曼估計(jì)位置進(jìn)行擬合，減小無人機(jī)的定位抖動(dòng)問題，進(jìn)一步提升了無人機(jī)的定位精度。