楊潤豐，駱春波，張智聰，李銘釗

基于擴展卡爾曼濾波算法的無人機定位*

楊潤豐**1，駱春波2，張智聰3，李銘釗4

（1.東莞職業(yè)技術學院電子工程系，廣東東莞523808；
2.英國?？巳卮髮W數(shù)學與計算機科學系，英國?？巳谽X4 4QF；3.東莞理工學院機械工程學院，廣東東莞523808；4.中國電子信息產(chǎn)業(yè)集團有限公司，北京100846）

無人機的移動定位是應對無人機機動性和應用環(huán)境復雜性的關鍵技術。為解決無人機中的全球定位系統(tǒng)（GPS）信號失效問題，提出了一種通過機載無線射頻的接收信號強度解決定位問題的技術，分別采用擴展卡爾曼濾波方法估計距離和最小二乘方法估計路徑損耗因子兩種處理方法。理論分析和實驗測試結果證實所提算法對有色噪聲干擾下的接收信號有較好的增強效果，基于80%的置信水平，新算法相對于白噪聲模型將估算誤差從9.5 m減少到了4 m，還進一步提供了融合慣性導航的算法。

無人機；移動定位；擴展卡爾曼濾波；距離估算

1 引 言

無人機在野外搜救應用中，需要進行信息采集、數(shù)據(jù)融合等移動通信任務[1-3]，準確快速的移動定位是完成這些任務的保障。因此，移動定位是應對無人機機動性、靈活性及其應用環(huán)境復雜性的關鍵技術?，F(xiàn)有的無人機定位技術主要基于全球定位系統(tǒng)（Global Positioning SYstem，GPS），但是當衛(wèi)星遭遇傳輸信道受阻、天氣條件干擾等因素影響的時候，GPS信號將變微弱甚至失效，因此不能保證無人機GPS信號的正常接收，這給無人機的控制帶來難以預測的困難，甚至給無人機帶來危險?，F(xiàn)有的其他技術包括利用無線射頻信號的到達時間或到達時差[4]、到達角[5]、接收信號強度[6]等方法實現(xiàn)距離的測量和定位。前兩個方法需要高度精確的設備或者天線陣列和復雜信號處理模塊的支持，增加了無人機平臺的負載、復雜性和成本。目前，國際上研究提出了基于接收信號強度的定位方法，并證明了其是一個更為方便和經(jīng)濟的解決方案。

無人機定位研究是當前國內(nèi)外研究的一個熱點問題[7-8]，例如，基于接收信號強度的定位技術，雖然具有算法簡單等特點，但是它的一個突出問題是當無線信號受到噪音和干擾的影響時，信號強度的測量值比較復雜且無規(guī)則。本文所提定位采用了最小二乘法來處理，然而，最小二乘法在白噪聲影響下所測的定位結果較準確的優(yōu)勢受到了一定的限制[9]。由于無人機飛行姿態(tài)的多樣性、環(huán)境的復雜性以及信號傳輸會受環(huán)境影響等，使用最小二乘法會產(chǎn)生大比例的誤差。為了提高基于接收信號強度算法定位的精確度，本文進一步提出擴展卡爾曼濾波的處理方法，并在有色噪聲傳輸模型中進行了測試和分析。此外，影響算法精度的另一個因素是傳播路徑損耗因子。因為在無線信道中路徑損耗因子并不是常量，本文還提出在無人機飛行應用中實時估算路徑損耗因子的自適應算法，從而提高了移動定位的精確度，更好地保證了無人機群的控制，相對于已有的算法[7，10]，本算法具有更高的精度和適應性。

為了方便理論分析，文中相關的符號表示如下：T表示矩陣轉置，lg表示以10為底的對數(shù)函數(shù)，min{?}和max{?}分別表示選擇輸入?yún)?shù)的最小值和最大值，║?║表示歐幾里得范數(shù)，δ（?）表示狄拉克沖激函數(shù)，E{?}表示矩陣期望值。

2 系統(tǒng)模型

2.1 系統(tǒng)模型

確定一個物體的位置通常需要3個以上已知自身位置信息的參考節(jié)點。如圖1所示，共有J個已知位置信息的參考節(jié)點，標記為Rj（j＝1，2，…，J），共有I臺需定位的無人機，標記為Ui（i＝1，2，…，I）。這些參考節(jié)點可以是其他無人機、基站或接入點等，它們能夠獲取自己的準確位置信息和時鐘同步的校準信息。每一臺需定位的無人機使用無線射頻信號按時間戳廣播一個預定義的控制消息，這些信息經(jīng)過信道時，可能遭受衰減、干擾和其他噪音的影響。

圖1 無人機位置估算模型Fig.1 UAVs Position estimation model

采用這一模型的關鍵是使用機載無線射頻通信模塊的接收信號強度估算無人機和參考節(jié)點之間的距離。無人機組以剛體組合形式建模，它們的位置信息和參考節(jié)點位置可分別由式（1）和式（2）表示：

式中：n表示時間戳。

在圖1模型中，可根據(jù)3個參考節(jié)點的距離信息估算一臺無人機的位置。例如，無人機Ui的位置可表示為

2.2 信道模型

電磁波在無線電信道傳播中常遭受噪聲等各種因素干擾影響，而且發(fā)射端和接收端的移動也會帶來多普勒效應和不穩(wěn)定狀況。因此，在信道建模中，引入自適應參數(shù)估算算法以應對物理環(huán)境變化的復雜無線信道。信道模型[8]表示為

式中：Pr和Pt分別是接收功率和發(fā)射功率，單位是dBm；K是常數(shù)，其值取決于與d0測量值相關的平均信道衰減和天線特性；dij是無人機Ui與參考點Rj的距離；γ是路徑損耗因子；υ是以dBm為單位的噪聲功率。

設d0＝1 m，以時間戳表示路徑損耗模型，如公式（5）所示：

并用其測量距離。通常公式（5）中的噪聲可采用高斯分布建模，但有色噪聲在實際環(huán)境中對無人機無線通信影響極大。

3 基于色噪聲建模的算法設計

3.1 噪聲分析

通過應用阿倫方差的群集分析工具對一段無人機的接收射頻信號進行統(tǒng)計分析發(fā)現(xiàn)，除了白噪聲外，無線電信號也受有色噪聲的影響（如圖2所示），因此需要進行更準確的噪聲建模。

圖2 無人機接收信號的阿倫方差Fig.2 The Allan variance of the received radio signal for a UAV

假設收集了N個數(shù)據(jù)φ（n），n＝1，2，…，N，把這些數(shù)據(jù)分組成M＝N/f個集群，然后利用公式（6）計算阿倫方差：

式中：τ為關聯(lián)時間和f為采樣頻率；μk（M）＝

圖2反映了無線接收信號的阿倫方差情況，從雙對數(shù)譜的斜率可以看出，因為仿真數(shù)據(jù)添加了人為的白噪音，其斜率是-1，而此時（lgτ＜1）真實數(shù)據(jù)因為受有色噪聲的嚴重影響，其斜率的變化較大，特別是在圖中右端，竟呈現(xiàn)正數(shù)斜率。

隨著頻率f的增大，阿倫方差平方的誤差減少，其置信區(qū)間如公式（7）所示：

式中：σ2是真實方差值；F是估算量自由度；χ2是卡方分布的累積分布函數(shù)；ε是接近0的非負數(shù)。

3.2 有色噪聲建模

根據(jù)以上相關噪聲的觀察分析可以看到，加性高斯白噪聲和色噪聲的建模區(qū)別主要在于普通噪聲在頻譜范圍內(nèi)具有常數(shù)值的功率密度，而色噪聲在不同的頻段上具有不同的功率譜密度。在時域來看，白噪聲在相關圖像上具有接近直線的下降趨勢，而色噪聲的曲線比較復雜，通過估計色噪聲的阿倫方差曲線，可以獲得對色噪聲的近似模擬，即應用高斯馬爾可夫過程建立一個連續(xù)時間自回歸模型[11]：

式中：λ是高斯馬爾可夫過程系數(shù)，它描述了色噪聲在不同相關時間內(nèi)的特征，對于色噪聲的建模和處理非常重要，后文將詳細介紹它在卡爾曼濾波器中的應用；W是加性白噪聲W2（t-T）]＝δ（T）。

把公式（8）轉換為離散時間域表示：

式中：Δ為時間增量。

由于激勵噪聲W2（n）的影響，在不失一般性的情況下，設n≥m，噪聲φ（m）和φ（n）的關聯(lián)可表達為

為了保持模型的收斂性，公式（10）中λ必須小于0。當m和n的值足夠大時，式中第一項為0。第二項需要展開，把求和項進行求和運算，求和的結果是，進而簡化得

3.3 位置估算

應用擴展卡爾曼濾波方法實現(xiàn)距離估算。首先，根據(jù)以上信道和噪聲的分析，結合公式（3），無人機Ui與參考點Rj的距離可建模型為

式中：ψ（n-1）是可選的控制輸入，如慣性測量；W（n-1）是噪聲估算，?？煽醋鳛楦咚狗植糔（0，δ2）；φ（n）是有色噪聲測量值；υ（n）是含方差R的白噪聲測量值；設初始條件為n＝0時，d（0）＝d0和ψ（0）＝0。

公式（12）的第一個方程表示狀態(tài)，第二個方程是測量方程。結合公式（8），有色噪聲狀態(tài)可表達為

式中：W2（n-1）是公式（7）中帶方差的白噪聲。

利用公式（14）可把公式（12）的第一個方程與有色噪聲狀態(tài)方程（13）合并：

式中：c（n-1）＝[d（n-1），φ（n-1）]T，ψ（n-1）＝[ψ（n-1），0]T，并且W（n-1）＝[W（n-1），W2（n-1）]T。噪聲處理矩陣為Q＝E{WWH}，f（?）的雅可比矩陣可計算為

對卡爾曼濾波器的測量方程為

式中：h（c（n））＝10γ（n）lgc1（n）＋φ（n），并結合其偏導矩陣（雅可比矩陣）

由公式（14）和（16）推導得卡爾曼濾波算法的預測狀態(tài)向量為

對先驗估計誤差協(xié)方差的計算：

式中：P（n-1）是后驗誤差協(xié)方差估計，通常初始條件設P（0）＝1。

誤差協(xié)方差受測量信號的影響可表示為

那么，卡爾曼增益可更新為

預測狀態(tài)估計可更新得

最終，后驗誤差協(xié)方差估計更新為

4 路徑損耗因子自適應估計算法的增強方案

隨著無人機的移動，其運行的物理環(huán)境不斷變化，在無線通信模型中采用固定的路徑損耗因子顯然是不現(xiàn)實的，我們應用改進最小二乘法對路徑損耗因子的估算，路徑損耗模型（5）可表示為

式中：f（γ（n））＝-10γ（n）lgdij（n），并設已有M個測量數(shù)據(jù)包括dij，誤差函數(shù)可寫成

寫成向量模型，可得

并且

因為LT（n）L（n）是常量，ε（γ（n））的最小值為

由于F（n）含有一系列未知參數(shù)的非線性函數(shù)，不能從其一階導數(shù)中明確γ的表達式，對一階導數(shù)賦0值的線性方法來獲得理想γ值并不可行，因而采用改進的高斯-牛頓算法通過迭代估算第n時刻的γ值?；谧钚《朔ǖ脑恚玫墓浪憧杀硎緸?/p>

式中：Θ（s）＝JT（γ（s））J-1（γ（s））JT（γ（s））e（n）；s是迭代系數(shù)。并將計算所得的雅可比矩陣J（γ）＝[?e1（n）/?γ，…，?eN-M＋1（n）/?γ]T用于迭代。由于高斯-牛頓算法很大程度取決于初始值并會產(chǎn)生偏離，這里采用調(diào)整因子θ提高算法性能，并且設θ＝0.5能簡化運算的同時，也能較好地滿足實驗的需要。迭代的停止條件為

計算γ（n）的步驟如下：

（1）按提供的初始值計算Θ（0）和γ（0）；

（2）結合公式（26）與（28）計算γ（s）和εγ（s）；

（3）如果滿足迭代的停止條件，則進行步驟4，否則返回步驟2；

（4）γ（s）是γ（n）的輸出。

5 算法實現(xiàn)與試驗驗證

通過無人機的實際飛行獲取實驗數(shù)據(jù)，實驗無人機裝載1個GPS接收器和1個慣性測量單元（集成3個陀螺儀、1個三軸加速度計、3個磁力儀、1個壓力傳感器）。一個地面GPS基站提供參考節(jié)點信息。我們在一個空曠的草坪采集了12組射頻信號數(shù)據(jù)。無線射頻信號的收發(fā)通過兩個802.11 b/g無線模塊（GigabYte GN-WI01GT）完成。為了免受其他無線電波傳輸?shù)挠绊?，?jīng)過測試后無線模塊的通信信道設定為2.462 GHz，發(fā)射功率設定為5 dBm，傳輸速率設定為1 Mb/s，采集范圍控制在60 m以內(nèi)。本文的實驗環(huán)境設置與真實場景基本一致，無人機實驗驗證基于真實測試數(shù)據(jù)，差分GPS信號僅用于性能對比。

5.1 距離測量誤差實驗

本實驗對測量的接收信號強度數(shù)據(jù)分別應用擴展卡爾曼濾波和最小二乘算法進行距離估算。在擴展卡爾曼濾波模型中，其迭代速率為10次/s，所有的算法都沒有應用慣性測量單元所測信息。從圖3可以看出，利用最小二乘算法估算無人機移動距離的誤差變化較大，而利用擴展卡爾曼濾波算法估算的結果較穩(wěn)定，并且估算精確度更高。這兩種方法所產(chǎn)生的不同精度主要在于如何利用歷史數(shù)據(jù)。最小二乘法使用當前和歷史數(shù)據(jù)對系統(tǒng)建模，如果過程模型和觀測模型有較大的瞬時差異，最小二乘法會產(chǎn)生大比例的誤差，如圖4所示。而卡爾曼濾波算法根據(jù)測量噪聲方差對自己的參數(shù)進行調(diào)整，而且它僅觀察有限而必要的部分，因而增強了對噪聲瞬時變化的抵抗性。圖4中的誤差是通過估算距離和GPS定位系統(tǒng)所得位置的數(shù)據(jù)計算比較所得，當置信水平為80%時，通過卡爾曼濾波法所測的誤差為4 m，最小二乘法所測的誤差為10 m。隨著無人機飛行距離變遠，兩種算法所測的誤差也逐漸增大。

圖3 從接收信號強度估算的無人機距離Fig.3 Estimation distance of a UAV from the received radio signal

圖4 兩種算法的估算距離誤差比較Fig.4 The comParison of estimation distance error betWeen tWo algorithms

5.2 噪聲影響

由于有色噪聲具有一定的規(guī)律性，在信號處理中容易除掉。在使用擴展卡爾曼濾波法估算時，如果只考慮白噪聲的影響，那么距離估算誤差將遠遠高于同時對白噪聲和有色噪聲進行處理后的結果，如圖5所示。當置信水平為80%時，只處理白噪聲所測的誤差約為8.5 m，而根據(jù)公式（12）和（13）推導的卡爾曼濾波算法，同時處理白噪聲和有色噪聲所得的誤差僅為4 m。值得提到的是，所得的差異并不總是相同的，還受到環(huán)境影響，如噪聲方差等。

圖5 白噪聲和有色噪聲對距離估算誤差的影響Fig.5 Effects of White noise and color noise on distance estimation error

5.3 路徑損耗因子影響

采用不同的路徑損耗因子也會產(chǎn)生不同的距離估算誤差，如圖6所示。在測試的距離內(nèi)，路徑損耗因子值越高，估算性能越好。受無線傳輸路徑上多種反射和衰減的影響，路徑損耗因子在實際環(huán)境中極不穩(wěn)定，本文提出的自適應路徑損耗因子估計算法雖然不是具有最好的性能，但總體距離估算誤差的結果處于比較穩(wěn)定的水平，特別在置信水平大于70%時，其估算誤差結果優(yōu)于任何采用固定值的方法。

圖6 路徑損耗因子對距離估算誤差的影響Fig.6 Effects of Path-loss factors on estimation error

5.4 慣性測量單元影響

慣性測量單元的信息主要反映了無人機的速率。當加入慣性測量單元所測信息后，距離估算的精確度有明顯提高，如圖7所示。當置信水平為80%時，使用慣性測量單元輔助所測的距離估算誤差約為1.7 m，使用該方法的前提是慣性測量單元所測信息具有一定的準確性，這主要取決于慣性傳感器的性能，其精確度通常隨時間的增長而降低。

圖7 慣性測量單元的信息輔助對距離估算誤差的影響Fig.7 Effect of assistance of inertial measurement unit on distance estimation error

6 結束語

本文利用無線射頻信號估算無人機的移動定位，主要提出了應用擴展卡爾曼濾波算法實現(xiàn)距離估算，通過對信號模型及噪聲分量進行分析，較好地處理了有色噪聲和路徑損耗對距離估算的不良影響。此算法提高了無人機群移動定位的精度，可用于無人機協(xié)同傳感或其他任務，使無人機得到了更廣泛的應用。

相對于已有的算法，本研究工作提高了無人機定位的準確性和環(huán)境適應性。下一步工作將研究白噪聲的消除和定位系統(tǒng)精度增強，對飛行姿態(tài)、天線特征和環(huán)境等影響因素進行具體分析和量化。

[1] TEACY W，NIE J，MCCLEAN S，et al.Maintaining con_ nectivitY in UAV sWarm sensing[C]//Proceedings of 2010 IEEE GLOBECOM WorkshoPs.Miami，USA：IEEE，2011：1771-1776.

[2] 楊潤豐，駱春波，張智聰.適用于無人搜救機組的新型移動中繼通信模型[J].電訊技術，2014，54（10）：1330-1334. YANG Runfeng，LUO Chunbo，ZHANG Zhicong.A Novel Mobile RelaY Communication Model for Search and Res_ cue UAVs[J].Telecommunication Engineering，2014，54（10）：1330-1334.

[3] BRYSON M，SUKKARIEH S.ObservabilitY analYsis and ac_ tive control for airborne SLAM[J].IEEE Transactions on AerosPace and Electronic SYstems，2008，4（1）：261-280.

[4] GOLDEN S，BATEMAN S.Sensor measurements for Wifi location With emPhasis on time-of-arrival ranging[J]. IEEE Transactions on Mobile ComPuting，2007，6（10）：185-1198.

[5] PATWARI N，HERO A O，PERKINS M，et al.Relative lo_ cation estimation in Wireless sensor[J].IEEE Transac_ tions on Signal Processing，2003，51（8）：2137-2148.

[6] WEISS A.On the accuracY of a cellular location sYstem based on RSS measurements[J].IEEE Transactions on Vehicular TechnologY，2003，52（6）：1508-1518.

[7] HAN J H，KWON J H，LEE I，et al.Position and attitude determination for UAV-based GPS，IMU and AT Without GCPs[C]//Proceedings of 2011 International WorkshoP on in Multi-Platform/Multi-Sensor Remote Sensing and MaPPing（M2RSM）.Xiamen：IEEE，2011：1-5.

[8] MOSHE B，SHVALB N，BAADANI J，et al.Indoor Positio_ ning and navigation for micro UAV drones[C]//Proceed_ ings of 2012 IEEE 27th Convention of Electrical＆Elec_ tronics Engineers in Israel.Eilat，Israel：IEEE，2012：1-5.

[9] BANDIERA F，COLUCCIA A，RICCI G.A cognitive al_ gorithm for received signal strength based localization [J].IEEE Transactions on Signal Processing，2015，63（7）：1726-1736.

[10] MENIEM M H A，HAMAD A M，SHAABAN E.Relative RSS-based GSM localization technique[C]//Proceedings of 2013 IEEE International Conference on Electro/Infor_ mation TechnologY.Indiana，USA：IEEE，2013：1-6.

[11] ALIZADEH P，CHEVALEYRE Y，ZUCKER J D.AP_ Proximate regret based elicitation in Markov decision Process[C]//Proceedings of 2015 IEEE International Conference on Research，Innovation，and Vision for the Future.Can Tho，Vietnam：IEEE，2015：47-52.

楊潤豐（1979—），男，廣東東莞人，2009年于英國雷丁大學獲電子工程專業(yè)博士學位，現(xiàn)為副教授、教研室主任，主要研究方向為數(shù)字信號處理、無線通信；

YANG Runfeng Was born in Dongguan，Guandong Province，in 1979.He received the Ph.D.degree from UniversitY of Reading，UK， in 2009.He is noW an associated Professor and dean of teaching -research section.His research concerns digital signal Process_ ing and Wireless communication.

Email：Yangrf@dgPt.edu.cn

駱春波（1983—），男，重慶人，2011年于英國雷丁大學獲電子工程專業(yè)博士學位，現(xiàn)為副教授，主要研究方向為數(shù)字信號處理、無線通信；

LUO Chunbo Was born in Chongqing，in 1983.He received the Ph.D.degree from UniversitY of Reading，UK，in 2011.He is noW an associated Professor.His research concerns digital sig_ nal Processing and Wireless communication.

Email：c.luo@exeter.ac.uk

張智聰（1980—），男，廣東東莞人，2007年于清華大學獲系統(tǒng)工程專業(yè)博士學位，現(xiàn)為教授，主要研究方向為系統(tǒng)工程；

ZHANG Zhicong Was born in Dongguan，Guangdong Prov_ ince，in 1980.He received the Ph.D.degree from Tsinghua Uni_ versitY in 2007.He is noW a Professor.His research concerns sYstem engineering.

Email：stePhen1998@gmail.com

李銘釗（1980—），男，天津人，2006年于英國華威大學獲系統(tǒng)工程專業(yè)碩士學位，現(xiàn)為高級工程師，主要研究方向為系統(tǒng)工程。

LI Mingzhao Was born in Tianjin，in 1980.He received the M.S.degree from UniversitY of WarWick，UK，in 2006.He is noW a senior engineer.His research concerns sYstem engineering.

Email：limzh@cec.com.cn

UAV Positioning Based on Extended Kalman Filter Algorithm

YANG Runfeng1，LUO Chunbo2，ZHANG Zhicong3，LI Mingzhao4
（1.DePartment of Electronic Engineering，Dongguan PolYtechnic，Dongguan 523808，China；2.DePartment of Mathematics and ComPuter Science，UniversitY of Exeter，EX4 4QF，UK；3.School of Mechanical Engineering，Dongguan UniversitY of TechnologY，Dongguan 523808，China；4.China Electronics CorPoration，Beijing 100846，China）

Mobile Positioning is essential for Unmanned Aerial Vehicles（UAVs）to coPe With UAV mobilitY and the comPlexitY of dePloYed environments.To solve the Problem that UAVs'Global Positioning SYstem（GPS）signal fails，radio frequencY（RF）signal strength from the on-board communication module is a_ doPted.For the Processing of received signals，the extended Kalman filter（EKF）method is used to esti_ mate distance and the Least Square（LS）algorithm is used to estimate the Path loss factor.Theoretical a_ nalYsis and exPeriment results demonstrate that the ProPosed algorithms offer better Performance for enhan_ cing received signals under coloured noise and imProving distance estimation accuracY.Given a confidence level of 80%，the neW algorithm has imProved the estimation accuracY from 9.5 m to 4 m.The further en_ hancement With data fusion from the gYro information is also Provided.

unmanned aerial vehicle（UAV）；mobile Positioning；extended Kalman filter；distance estimation

The National Natural Science Foundation of China（No.71201026）；The Science and Technological Program for Dongguan’s Higher Education，Science and Research Institutions（2014106101034）；The Dongguan PolYtechnic Science and Research Funding ScholarshiP（2015b06）

TN925

A

1001-893X（2016）01-0060-07

10.3969/j.issn.1001-893x.2016.01.011

楊潤豐，駱春波，張智聰，等.基于擴展卡爾曼濾波算法的無人機定位[J].電訊技術，2016，56（1）：60-66.[YANG Runfeng，LUO Chunbo，ZHANG Zhicong，et al.UAV Positioning based on extended Kalman filter algorithm[J].Telecommunication Engineering，2016，56（1）：60-66.]

2015-05-18；

2015-12-14 Received date：2015-05-18；Revised date：2015-12-14

國家自然科學基金資助項目（71201026）；東莞市高等院校、科研機構科技項目（2014106101034）；東莞職業(yè)技術學院科研基金項目（2015b06）

**通信作者：Yangrf@dgPt.edu.cn Corresponding author：Yangrf@dgPt.edu.cn