曹以龍, 景曉娟, 景旭川

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

近年來(lái),無(wú)人機(jī)被廣泛應(yīng)用于室內(nèi)環(huán)境中[1],但如何在室內(nèi)對(duì)四旋翼無(wú)人機(jī)實(shí)現(xiàn)準(zhǔn)確定位仍然是一個(gè)具有挑戰(zhàn)性的問(wèn)題。ZHOU M等人[2]提出了一種利用室內(nèi)公共WLAN進(jìn)行室內(nèi)無(wú)人機(jī)定位的方法,在定位空間中建立指紋數(shù)據(jù)庫(kù),通過(guò)將實(shí)際信息與數(shù)據(jù)庫(kù)中的參數(shù)進(jìn)行對(duì)比來(lái)實(shí)現(xiàn)定位,不過(guò)該方法易受環(huán)境干擾,導(dǎo)致結(jié)果出現(xiàn)較大波動(dòng)。于拓[3]提出了一種面向動(dòng)態(tài)四旋翼無(wú)人機(jī)的定位方法,利用歷史定位數(shù)據(jù)來(lái)修正無(wú)人機(jī)的飛行路徑,從而提高基于WIFI的定位系統(tǒng)的精度,但如果歷史定位數(shù)據(jù)有較大誤差,就會(huì)對(duì)后續(xù)定位產(chǎn)生影響。YOON P K等人[4]提出了一種使用藍(lán)牙低功耗和慣性測(cè)量單元的新型室內(nèi)定位方法,在藍(lán)牙測(cè)量存在異常值的情況下,該算法可以保持較高的位置跟蹤精度,不過(guò)藍(lán)牙信號(hào)傳輸功率小,不適用于大型室內(nèi)場(chǎng)合的定位。

本文提出了一種基于慣性導(dǎo)航系統(tǒng)(Inertial Navigation System,INS)和WIFI指紋定位技術(shù)(以下簡(jiǎn)稱“WIFI定位”)的四旋翼無(wú)人機(jī)室內(nèi)定位方法,解決了INS定位的誤差累積問(wèn)題和WIFI定位的波動(dòng)問(wèn)題,使得定位結(jié)果更加穩(wěn)定、精確。

1 INS室內(nèi)定位技術(shù)

INS定位技術(shù)通過(guò)內(nèi)部的傳感器數(shù)據(jù)估計(jì)無(wú)人機(jī)的最新位置,根據(jù)已知的當(dāng)前位置來(lái)估計(jì)下一位置。INS用于檢測(cè)和測(cè)量加速度、傾斜、沖擊、振動(dòng)、旋轉(zhuǎn)和多自由度運(yùn)動(dòng),是負(fù)責(zé)導(dǎo)航、定向和運(yùn)動(dòng)載體控制的重要部件。它包含加速度計(jì)、陀螺儀和磁力計(jì)3部分:加速度計(jì)計(jì)算加速度和傾角,陀螺儀測(cè)量無(wú)人機(jī)的旋轉(zhuǎn)角度,磁力計(jì)用于獲取無(wú)人機(jī)的方向信息。所有的運(yùn)動(dòng)都可以看作一個(gè)直線運(yùn)動(dòng)和一個(gè)旋轉(zhuǎn)運(yùn)動(dòng)的合成,因此可以由INS得出無(wú)人機(jī)的運(yùn)動(dòng)狀態(tài)[5]。在無(wú)人機(jī)飛行過(guò)程中,可使用歐拉角、四元數(shù)或方向余弦矩陣來(lái)表示無(wú)人機(jī)的姿態(tài)。歐拉角由俯仰角θ、翻滾角φ和偏航角ψ組成,可以用來(lái)表示無(wú)人機(jī)的姿態(tài)角,四旋翼無(wú)人機(jī)中的歐拉角如圖1所示。

圖1 四旋翼飛行器中的歐拉角示意

由于歐拉角會(huì)出現(xiàn)萬(wàn)向鎖問(wèn)題,因此本文中采用四元數(shù)來(lái)表示。其轉(zhuǎn)換公式為

(1)

式中:q0,q1,q2,q3——四元數(shù)。

由式(1)可以得到四元數(shù)與歐拉角的轉(zhuǎn)換公式為

(2)

INS能夠檢測(cè)到無(wú)人機(jī)的姿態(tài)信息和加速度信息,經(jīng)過(guò)公式轉(zhuǎn)換即可得到飛行器的位置信息。

2 WIFI定位

基于指紋數(shù)據(jù)庫(kù)的WIFI定位,其原理是由于在復(fù)雜的室內(nèi)空間中,不同位置接收到的信號(hào)受到信號(hào)衰減、多徑傳播的影響,使得空間內(nèi)信號(hào)分布不均勻,不同位置采集到的信號(hào)存在特征上的差異性。這種差異性使得接收信號(hào)與地理位置構(gòu)成了一種映射關(guān)系,稱為“位置指紋”[6]。四旋翼無(wú)人機(jī)通過(guò)測(cè)量周圍WIFI熱點(diǎn)發(fā)送的信標(biāo)得到信號(hào)接收強(qiáng)度(Received Signal Strength,RSS)和WIFI熱點(diǎn)的物理地址信息,然后對(duì)比指紋數(shù)據(jù)庫(kù)中的位置指紋信息推算出無(wú)人機(jī)在空間中所處的位置。

WIFI定位分為指紋數(shù)據(jù)庫(kù)的建立和在線定位兩個(gè)部分。數(shù)據(jù)庫(kù)的建立通過(guò)記錄空間位置中的RSS和接入點(diǎn)位置信息,生成對(duì)應(yīng)的位置指紋信息;在線定位階段則是利用匹配算法對(duì)比無(wú)人機(jī)所處位置的RSS和指紋庫(kù)中各位置的RSS,結(jié)果最為相似的即為無(wú)人機(jī)自身所處的位置。本文所使用的匹配算法為改進(jìn)型k-近鄰法(Weightedk-Nearest Neighbor,WKNN)。

2.1 k近鄰法

k-近鄰法(k-Nearest Neighbor,KNN)是數(shù)據(jù)分類技術(shù)中常用的方法之一。指紋數(shù)據(jù)庫(kù)中未知點(diǎn)的RSS與第i個(gè)指紋的歐氏距離可以表示為

(3)

式中:n——無(wú)人機(jī)實(shí)時(shí)接收到的物理地址相同的WIFI信號(hào)的個(gè)數(shù);

SRSS,ik——指紋數(shù)據(jù)庫(kù)中第i個(gè)物理地址相同的WIFI信號(hào)的信號(hào)強(qiáng)度;

SRSS,k——實(shí)時(shí)接收到的第k個(gè)物理地址相同的WIFI信號(hào)的信號(hào)強(qiáng)度。

KNN算法是將k個(gè)參考點(diǎn)坐標(biāo)的平均值作為目標(biāo)點(diǎn)的估計(jì)坐標(biāo),但是k個(gè)參考點(diǎn)到目標(biāo)點(diǎn)的歐氏距離不相同,映射到實(shí)際距離也不相等,這k個(gè)參考點(diǎn)對(duì)于目標(biāo)點(diǎn)位置估計(jì)的貢獻(xiàn)程度也不相同。在計(jì)算n個(gè)距離后,選擇距離最小的k(2≤k≤6)個(gè)值來(lái)估計(jì)坐標(biāo),為

(4)

(xi,yi)——第i個(gè)參考點(diǎn)的坐標(biāo)。

2.2 改進(jìn)型k近鄰法

采用KNN算法時(shí),取k個(gè)最相似的參考點(diǎn)的坐標(biāo)均值計(jì)算目標(biāo)點(diǎn)的位置,則這些點(diǎn)對(duì)最終的位置估計(jì)貢獻(xiàn)相同。兩個(gè)點(diǎn)之間的信號(hào)強(qiáng)度越接近,其位置距離理論上也越近,而KNN算法中取均值的方式并沒(méi)有體現(xiàn)出歐氏距離與實(shí)際距離的關(guān)系。

針對(duì)k個(gè)參考點(diǎn)的歐氏距離大小的差異,在 KNN 算法的基礎(chǔ)上,對(duì)k個(gè)參考點(diǎn)賦予一個(gè)權(quán)重因子,計(jì)算目標(biāo)點(diǎn)估計(jì)坐標(biāo)時(shí)使用加權(quán)后的坐標(biāo)。這一改進(jìn)算法被稱為改進(jìn)型k-近鄰法。WKNN算法也是選擇k個(gè)參考點(diǎn)來(lái)估計(jì)未知點(diǎn)的位置,對(duì)參考點(diǎn)賦予權(quán)重因子wi。wi越大,表示相對(duì)應(yīng)的歐氏距離越近;反之,越遠(yuǎn)。利用 WKNN算法計(jì)算目標(biāo)點(diǎn)估計(jì)位置坐標(biāo)為

(5)

(6)

式中:ε——一個(gè)值很小的調(diào)節(jié)因子,用以防止計(jì)算值的分母為零。

WKNN算法流程如圖2所示。

圖2 WKNN算法流程

3 基于INS和WIFI定位的四旋翼無(wú)人機(jī)室內(nèi)定位方法

INS的定位不依賴于外部環(huán)境,而是一個(gè)加速度隨時(shí)間積分的過(guò)程,誤差會(huì)隨著時(shí)間的延長(zhǎng)而逐漸累積;WIFI定位中RSS值容易受到環(huán)境的影響而不斷變化,如果在在線定位階段檢測(cè)到未測(cè)量的WIFI信號(hào)或錯(cuò)誤的RSS值,將導(dǎo)致未知點(diǎn)與參考點(diǎn)的歐氏距離出現(xiàn)極大的誤差[7]。對(duì)此,本文提出了一種結(jié)合INS定位和WIFI定位的四旋翼無(wú)人機(jī)室內(nèi)定位方法。

3.1 無(wú)人機(jī)初始位置確定

精準(zhǔn)的初始位置可以使INS定位和WIFI定位兩種方法更好地融合。初始位置的確定方法如下。

步驟1 無(wú)人機(jī)開(kāi)機(jī)啟動(dòng)WIFI定位,對(duì)一段時(shí)間內(nèi)的多次定位位置取中位平均值,并將此位置作為INS定位的初始位置。

3.2 INS定位和WIFI定位的融合

根據(jù)INS定位和WIFI定位的不同特性,進(jìn)行優(yōu)勢(shì)互補(bǔ)。短時(shí)間內(nèi)INS定位較準(zhǔn)確,而時(shí)間過(guò)長(zhǎng)則輸出誤差較大;單獨(dú)的WIFI定位沒(méi)有累積誤差,長(zhǎng)時(shí)間后仍可使用,但定位波動(dòng)較大。因此,每隔一段時(shí)間用WIFI定位修正INS定位的結(jié)果,就不會(huì)出現(xiàn)累積誤差,也不會(huì)出現(xiàn)波動(dòng)較大的WIFI定位誤差。具體的INS定位和WIFI定位融合步驟如下。

步驟1 由于INS定位的頻率相較于WIFI定位更高,因此在WIFI定位結(jié)果未更新時(shí)使用INS定位結(jié)果作為無(wú)人機(jī)的當(dāng)前位置。

步驟2 當(dāng)WIFI定位結(jié)果更新后,將該定位結(jié)果與對(duì)應(yīng)時(shí)刻的INS定位結(jié)果進(jìn)行比較,若誤差較大,則說(shuō)明WIFI定位出現(xiàn)了較大誤差,使用INS定位結(jié)果作為無(wú)人機(jī)的當(dāng)前位置;若誤差在閾值范圍內(nèi),則將WIFI定位結(jié)果與INS定位結(jié)果的加權(quán)平均值作為無(wú)人機(jī)的當(dāng)前位置,具體權(quán)重根據(jù)實(shí)際環(huán)境中INS定位與WIFI定位的準(zhǔn)確性測(cè)試取值。

步驟3 將當(dāng)前位置作為INS定位位置,回到步驟1,開(kāi)始下一階段INS定位與WIF定位。

4 飛行控制系統(tǒng)整體方案

四旋翼無(wú)人機(jī)的飛行控制系統(tǒng)主要由INS信號(hào)采集、姿態(tài)解算、姿態(tài)控制器、水平位置控制器、高度位置控制器5個(gè)部分組成。系統(tǒng)總體設(shè)計(jì)框圖如圖3所示。圖3中,根據(jù)INS測(cè)量的數(shù)據(jù)進(jìn)行姿態(tài)解算得到無(wú)人機(jī)的當(dāng)前姿態(tài),并根據(jù)速度和位置信息進(jìn)行水平位置控制。采用超聲波傳感器和氣壓計(jì)來(lái)測(cè)量無(wú)人機(jī)的高度信息,并根據(jù)數(shù)據(jù)融合后的高度信息進(jìn)行高度控制。

圖3 無(wú)人機(jī)總體設(shè)計(jì)框圖

5 實(shí)驗(yàn)結(jié)果與分析

對(duì)四旋翼無(wú)人機(jī)的定位方法進(jìn)行實(shí)驗(yàn)驗(yàn)證,通過(guò)手持無(wú)人機(jī)移動(dòng)的方式,開(kāi)啟無(wú)人機(jī)定位系統(tǒng)。實(shí)驗(yàn)數(shù)據(jù)通過(guò)藍(lán)牙發(fā)送到電腦進(jìn)行存儲(chǔ)和處理。該四旋翼無(wú)人機(jī)平臺(tái)主體是上海錫月科技公司產(chǎn)品BIRD2.0,掃描WIFI RSS的WIFI模塊型號(hào)為ESP8266,INS型號(hào)為MPU6500,主控芯片是DSP28379,通過(guò)機(jī)身上的樹(shù)莓派建立指紋數(shù)據(jù)庫(kù),基于WKNN匹配算法進(jìn)行WIFI定位。

圖4為不同定位方法下的無(wú)人機(jī)直線運(yùn)動(dòng)的定位結(jié)果。由圖4可以看出,采用INS定位時(shí)定位軌跡逐漸偏離實(shí)際路徑,這是由于該方法主要采用對(duì)加速度積分的方式預(yù)測(cè)運(yùn)動(dòng)軌跡,誤差會(huì)隨著時(shí)間的增加而累積;WIFI定位結(jié)果波動(dòng)較大,這是由于RSS信號(hào)易受環(huán)境干擾所導(dǎo)致;采用INS定位和WIFI定位相結(jié)合的方法后,定位軌跡比較接近于真實(shí)路徑。

圖4 不同定位方法的定位效果

圖5為定位系統(tǒng)初始位置不準(zhǔn)確情況下的定位結(jié)果。由圖5可以看出,當(dāng)初始位置較真實(shí)位置有較大偏離時(shí)會(huì)對(duì)后續(xù)的定位效果產(chǎn)生影響,但隨著定位進(jìn)程的進(jìn)行,INS定位與WIFI定位結(jié)合方法的定位誤差逐漸減小,靠攏到真實(shí)位置附近。對(duì)比圖4可以看出,本文所提方法能較準(zhǔn)確地定位無(wú)人機(jī)的初始位置,可為后續(xù)定位提供良好的開(kāi)始。

圖5 初始位置定位不準(zhǔn)確的定位結(jié)果

圖6為本文所提定位方法在復(fù)雜路徑下的定位結(jié)果。由圖6可以看出,WIFI定位位置較為雜亂地分布在真實(shí)軌跡附近;采用INS定位與WIFI定位結(jié)合的方法時(shí),定位軌跡接近于真實(shí)路徑。這說(shuō)明本文所提定位策略有較強(qiáng)的穩(wěn)定性,在復(fù)雜路徑下也有良好的定位效果。

圖6 復(fù)雜路徑下的定位結(jié)果

6 結(jié) 語(yǔ)

本文對(duì)四旋翼無(wú)人機(jī)的室內(nèi)定位技術(shù)進(jìn)行了研究,提出了一種基于INS定位和WIFI定位相結(jié)合的無(wú)人機(jī)定位方法。該方法根據(jù)INS測(cè)量的姿態(tài)信息和加速度信息修正WIFI定位的結(jié)果,將指紋定位結(jié)果中不穩(wěn)定的值剔除,使得定位系統(tǒng)保留了INS定位短時(shí)間內(nèi)精確度高和WIFI定位長(zhǎng)時(shí)間穩(wěn)定的優(yōu)點(diǎn)。實(shí)驗(yàn)結(jié)果證明,基于INS定位和WIFI定位相結(jié)合的四旋翼無(wú)人機(jī)室內(nèi)定位方法有較高的準(zhǔn)確度。