胡 敬，田俊曦，鄒世明，晁 濤，楊 明

（哈爾濱工業(yè)大學(xué)航天學(xué)院，哈爾濱 150001）

1 引 言

過(guò)去20年里，隨著微機(jī)電系統(tǒng)和導(dǎo)航技術(shù)的發(fā)展，無(wú)人機(jī)技術(shù)得到了迅速的發(fā)展。由于無(wú)人機(jī)集群之間的協(xié)同具有低成本、隱蔽性強(qiáng)等特點(diǎn)，被認(rèn)為是未來(lái)戰(zhàn)爭(zhēng)中應(yīng)用最廣泛的方向[1]。無(wú)人機(jī)通過(guò)其自身搭載的慣性設(shè)備和全球?qū)Ш叫l(wèi)星系統(tǒng)（Global Navigation Satellite System, GNSS）接收機(jī)，可以在開(kāi)闊地帶精確完成定位，從而完成電力巡檢、無(wú)人機(jī)航拍、農(nóng)業(yè)自動(dòng)化等任務(wù)[2]。然而，在山地、峽谷、隧道或者未來(lái)戰(zhàn)場(chǎng)的復(fù)雜環(huán)境下，無(wú)人機(jī)極有可能面臨GNSS 信號(hào)拒止的問(wèn)題，在沒(méi)有引入外部信息源解決慣導(dǎo)設(shè)備由零偏不穩(wěn)定性帶來(lái)的位置發(fā)散問(wèn)題時(shí)，依靠低成本的慣導(dǎo)設(shè)備進(jìn)行位置解算會(huì)導(dǎo)致嚴(yán)重的誤差發(fā)散問(wèn)題。因此，研究GNSS 信號(hào)拒止環(huán)境下的導(dǎo)航問(wèn)題極具意義[3]。

慣性導(dǎo)航系統(tǒng)（Inertial Navigation System,INS）是應(yīng)用最廣泛也是最基本的導(dǎo)航定位方法，GNSS/INS 組合導(dǎo)航被廣泛應(yīng)用于各類(lèi)無(wú)人智能系統(tǒng)中[4]。然而在GNSS 拒止環(huán)境缺少有效觀測(cè)輸入的情況下，單純的慣導(dǎo)設(shè)備由于其零偏、噪聲等因素，積分求解位置將累積誤差，從而導(dǎo)致定位解算的發(fā)散。計(jì)算機(jī)視覺(jué)以及激光雷達(dá)可以為拒止環(huán)境下的無(wú)人機(jī)提供局部位置以及速度信息，這也是當(dāng)前的主流做法[5]。然而兩者同樣面臨很多亟待解決的問(wèn)題，比如視覺(jué)同步定位與地圖創(chuàng)建（Simultaneous Localization And Mapping,SLAM）可能會(huì)遇到特征點(diǎn)稀疏或障礙物等問(wèn)題[6]。對(duì)于激光雷達(dá)SLAM，在某些外部干擾（如降雨和平滑反射表面）下定位也存在魯棒性問(wèn)題[7]。

基于無(wú)線(xiàn)電的定位可以有效避免以上問(wèn)題。因此，作為一種視覺(jué)SLAM 和激光雷達(dá)SLAM 定位的冗余方法，無(wú)線(xiàn)電定位技術(shù)同樣具有極高的應(yīng)用價(jià)值。無(wú)線(xiàn)電定位通常使用基于接收信號(hào)強(qiáng)度（Received Signal Strength, RSS）的測(cè)距設(shè)備，如Wi-Fi 和Zigbee；基于到達(dá)時(shí)間差（Time of Arrival, TOA）的設(shè)備，如超寬帶（Ultra Wide Band,UWB）；基于TOA 的定位，考慮到成本和無(wú)人機(jī)負(fù)載問(wèn)題，本文主要關(guān)注基于UWB 的TOA 測(cè)距和基于Zigbee 設(shè)備的RSS 測(cè)距?；赨WB 的TOA 測(cè)距具有傳輸速率高、精度高、成本低、功耗低等特點(diǎn)，在不安裝功率放大器時(shí)測(cè)距范圍只有20～50m[5]；高功率UWB 設(shè)備雖然可以增加測(cè)距范圍，但會(huì)帶來(lái)成本、功耗和安全性問(wèn)題[7]。Xu 等[8]提出了在非視距環(huán)境中利用到達(dá)時(shí)間差和到達(dá)角度差以及慣導(dǎo)信息進(jìn)行定位，顯著降低了成本。Li 等[9]利用UWB 信息和慣導(dǎo)信息，提出了一種基于擴(kuò)展卡爾曼濾波（EKF）的融合算法，充分考慮了UWB 高延遲和低帶寬的問(wèn)題。上述兩種方法都提高了位置估計(jì)精度，但是忽略了低成本測(cè)距設(shè)備范圍約束條件下UWB 測(cè)距短的問(wèn)題。Zigbee 最初應(yīng)用于工業(yè)物聯(lián)網(wǎng)，可用于無(wú)人機(jī)集群之間的通信。同時(shí)，它也可以作為一種無(wú)成本的補(bǔ)充測(cè)距方法。在UWB 信號(hào)衰弱的遠(yuǎn)距離測(cè)距環(huán)境中，UWB 的測(cè)距精度甚至不如RSS[10]。Ahmed 等[11]介紹了無(wú)人機(jī)空對(duì)空和空對(duì)地環(huán)境下Zigbee 通信信道的特點(diǎn)，同時(shí)并概述了該領(lǐng)域未來(lái)的研究方向。Liu 等[12]利用慣導(dǎo)元件、進(jìn)程UWB 和遠(yuǎn)程RSS 信息，通過(guò)粒子濾波實(shí)現(xiàn)精確且魯棒的定位系統(tǒng)，但同時(shí)也帶來(lái)了功率限制與計(jì)算成本問(wèn)題。

為了實(shí)現(xiàn)無(wú)人機(jī)集群在GNSS 拒止環(huán)境下的精確定位問(wèn)題，本文首先引入慣導(dǎo)的動(dòng)力學(xué)方程作為濾波的狀態(tài)方程，基于TOA 測(cè)距和RSS 測(cè)距的優(yōu)缺點(diǎn)，通過(guò)最小二乘方法解算得到局部坐標(biāo)系下的無(wú)人機(jī)位置，并將其作為量測(cè)值輸入序貫擴(kuò)展卡爾曼濾波算法與慣導(dǎo)進(jìn)行融合，最終得到對(duì)位置的估計(jì)值。同時(shí)，考慮到在近程TOA/RSS 信號(hào)冗余，以及在長(zhǎng)程下TOA 測(cè)距失效，RSS 測(cè)距易受到環(huán)境干擾等問(wèn)題，提出了自適應(yīng)因子進(jìn)一步改進(jìn)濾波算法。最后，通過(guò)戶(hù)外飛行實(shí)驗(yàn)，驗(yàn)證了算法的有效性。

2 模型建立

在城市建筑物遮擋、隧道、峽谷或者是未來(lái)戰(zhàn)場(chǎng)的極端環(huán)境下，GNSS 信號(hào)會(huì)出現(xiàn)拒止的情況。此時(shí)，無(wú)人機(jī)通過(guò)結(jié)合自身測(cè)距信息并進(jìn)行適當(dāng)?shù)穆窂揭?guī)劃，一部分無(wú)人機(jī)可以到達(dá)開(kāi)闊的地帶，可以降低GNSS 信號(hào)拒止對(duì)集群的影響,無(wú)人機(jī)可以通過(guò)集群之間的信息來(lái)恢復(fù)自身的位置估計(jì)。本文著重關(guān)注對(duì)無(wú)人機(jī)傳感器的狀態(tài)實(shí)現(xiàn)精確且具有魯棒性的估計(jì)。

假設(shè)在GNSS 拒止的環(huán)境下，無(wú)人機(jī)集群中只有少數(shù)無(wú)人機(jī)可以自主規(guī)劃路徑搜索GNSS 信息，并充當(dāng)“偽衛(wèi)星”。同時(shí)，大多數(shù)無(wú)人機(jī)只能獲得自身慣導(dǎo)估計(jì)信息和測(cè)距信息來(lái)實(shí)現(xiàn)位置估計(jì)。無(wú)人機(jī)集群定位示意圖如圖1所示。假定無(wú)人機(jī)集群中M架無(wú)人機(jī)UAVm，m∈ {1,… ,M}可以自主搜索GNSS 信息，它們的局部坐標(biāo)信息可直接獲取，記坐標(biāo)pm=(x m,ym)∈R2，則可以將其視為定位系統(tǒng)中的錨節(jié)點(diǎn)；同時(shí)，以集群中一架不能自主搜尋GNSS 信息的無(wú)人機(jī)UAV0為例，記其真實(shí)位置估計(jì)為p0=(x0,y0)∈R2，而位置估計(jì)值為

圖1 GNSS 失鎖環(huán)境下定位場(chǎng)景Fig.1 Localization under GNSS loss-of-lock environment

2.1 慣導(dǎo)模型

慣性導(dǎo)航是應(yīng)用最廣泛的定位技術(shù)，但是在沒(méi)有GNSS 作為輔助導(dǎo)航的情況下，由于慣導(dǎo)單元（Inertial Measurement Unit, IMU）的不穩(wěn)定性及噪聲等特點(diǎn)，其解算出來(lái)的速度、位置誤差會(huì)隨著時(shí)間積累而發(fā)散。慣導(dǎo)器件中加速度計(jì)可測(cè)量無(wú)人機(jī)的加速度信息，陀螺儀可輸出角速度信息，磁力計(jì)可感知地磁強(qiáng)度。通過(guò)對(duì)系統(tǒng)動(dòng)力學(xué)模型泰勒展開(kāi)并忽略高階項(xiàng)，對(duì)其離散化可以得到如式(1)、(5)所示狀態(tài)方程。其中，無(wú)人機(jī)的姿態(tài)四元數(shù)、速度、位置狀態(tài)更新方程如式(1)所示，加速度計(jì)偏置、陀螺儀偏置、磁力計(jì)偏置的更新如式(5)所示。

其中，bθ表示陀螺儀偏置項(xiàng)，θn表示三軸測(cè)量誤差，服從均值為0，方差為的高斯分布。速度v為導(dǎo)航坐標(biāo)系下的矢量，其單位為m/s。Cbn為機(jī)體坐標(biāo)系到導(dǎo)航坐標(biāo)系下的旋轉(zhuǎn)矩陣。gN=[0 0 1]T為重力加速度在導(dǎo)航坐標(biāo)系下投影，其單位為重力加速度g。記加速度計(jì)輸出為，而其真實(shí)值為與陀螺儀表示類(lèi)似，加速度真值可以表示為：

同理，a b為陀螺儀零偏，a n為陀螺儀三軸測(cè)量噪聲，服從均值為0，方差為的高斯分布。在位置解算方程中，位置p的迭代可由速度迭代求解。

陀螺儀、加速度計(jì)、磁力計(jì)的零偏是非靜態(tài)的，對(duì)于傳感器的研究一般將其視為一階馬爾可夫過(guò)程，在姿態(tài)四元數(shù)、速度、位置更新時(shí)一并考慮，本文為了簡(jiǎn)化模型，參考PX4 源碼，有：

2.2 TOA/RSS 測(cè)距模型

無(wú)人機(jī)所搭載的UWB 模塊基于超帶寬載波的到達(dá)時(shí)間差來(lái)計(jì)算距離，假設(shè)集群中每一架無(wú)人機(jī)所搭載的UWB 模塊的時(shí)間戳是同步的，則集群中完全處于GNSS 失鎖狀態(tài)的無(wú)人機(jī)UA0V與那些能自主搜尋GNSS 信息從而位置狀態(tài)已知的無(wú)人機(jī)UAmV之間的距離可寫(xiě)成如下形式：

其中，εt,m0為UWB 模塊測(cè)距誤差，包括測(cè)量噪聲以及時(shí)間戳的誤差，同樣假定其為均值為0，方差為σt2的高斯過(guò)程。

無(wú)人機(jī)所搭載的Zigbee 模塊在負(fù)責(zé)通信的同時(shí)也能夠依據(jù)功率衰減原理估算距離，則無(wú)人機(jī)UAV0和無(wú)人機(jī)UAVm接收功率可建模為：

其中，RSSref為單位距離下的功率衰減，其單位為dBm。nr為路徑損耗指數(shù)，通常與環(huán)境有關(guān)。同樣，εr,m0為RSS 測(cè)距的誤差，假定其也服從均值為0，方差為σr2的正態(tài)分布。

通過(guò)實(shí)驗(yàn)采集大量數(shù)據(jù)，可得RSS 功率衰減與距離關(guān)系如圖2所示，同時(shí)可擬合出式(7)中參數(shù)：

圖2 RSS 功率衰減擬合曲線(xiàn)Fig.2 RSS attenuation model

3 自適應(yīng)擴(kuò)展卡爾曼濾波算法

本文虛擬觀測(cè)量為局部坐標(biāo)系下各無(wú)人機(jī)的坐標(biāo)，而坐標(biāo)的解算則依靠上節(jié)所介紹的測(cè)距模塊輸出的距離，依據(jù)最小二乘法解算而來(lái)。當(dāng)無(wú)人機(jī)同時(shí)收到TOA 和RSS 信息時(shí)，序貫卡爾曼濾波可以在短距離范圍內(nèi)估計(jì)出較為精確的位置。同時(shí)，在中遠(yuǎn)距離進(jìn)行狀態(tài)估計(jì)時(shí)同樣具有較高精度和魯棒性。與僅用UWB/IMU 或Zigbee/IMU 的濾波算法相比，范圍進(jìn)一步拓展。此外，由于基于Zigbee 的RSS 測(cè)距精度遠(yuǎn)不如TOA，因此在濾波過(guò)程中加入自適應(yīng)因子以動(dòng)態(tài)調(diào)整量測(cè)噪聲方差陣。

3.1 狀態(tài)量更新

由式(1)(5)可知，狀態(tài)量可寫(xiě)成：

狀態(tài)量依次為姿態(tài)四元數(shù)、速度、位置、陀螺儀偏置、加速度計(jì)偏置以及磁力計(jì)偏置。則離散狀態(tài)的預(yù)測(cè)更新過(guò)程如下：

過(guò)程噪聲方差陣如下：

其中，P-(k)為狀態(tài)預(yù)測(cè)的協(xié)方差矩陣，Q(k)=為過(guò)程噪聲協(xié)方差矩陣。Φ(k)為狀態(tài)轉(zhuǎn)移矩陣，其子塊部分通過(guò)偏導(dǎo)數(shù)求出[13]。

3.2 觀測(cè)量更新

序貫卡爾曼濾波本質(zhì)上為一種解耦卡爾曼濾波，要求每個(gè)量測(cè)值不相關(guān)。本文中，UWB 和Zigbee 的測(cè)距值同樣是獨(dú)立的。同時(shí)，利用M個(gè)錨節(jié)點(diǎn)依據(jù)最小二乘法對(duì)定位點(diǎn)進(jìn)行位置解算，公式如下：

在有了位置的虛擬觀測(cè)量后，可以通過(guò)相鄰兩點(diǎn)的位置狀態(tài)和時(shí)間間隔得到速度的觀測(cè)量，則可得到量測(cè)量：

量測(cè)矩陣可以寫(xiě)成如下形式：

自適應(yīng)擴(kuò)展卡爾曼濾波的量測(cè)更新如下：

由于TOA 和RSS 的數(shù)據(jù)頻率不同，解算出來(lái)的虛擬觀測(cè)量會(huì)不同時(shí)地進(jìn)入濾波器中，為此，本文采用自適應(yīng)序貫擴(kuò)展卡爾曼濾波算法，則公式可重寫(xiě)如下：

其中，為卡爾曼濾波增益，α k為自適應(yīng)因子，可以寫(xiě)成如下形式：

這里，c為閾值，依靠經(jīng)驗(yàn)通常設(shè)置在1.0～2.0[14]，Vk=Z k-Xk-+1為量測(cè)和狀態(tài)預(yù)測(cè)的差，由此，式(9)～(20)給出了自適應(yīng)序貫卡爾曼濾波算法。

4 實(shí)驗(yàn)結(jié)果及分析

本實(shí)驗(yàn)中，測(cè)距模塊通過(guò)自行設(shè)計(jì)的PCB 板與飛控系統(tǒng)進(jìn)行通信，TOA 和RSS 傳輸頻率分別為5Hz、2Hz。實(shí)驗(yàn)用無(wú)人機(jī)整機(jī)圖如圖3所示。

圖3 實(shí)驗(yàn)所用無(wú)人機(jī)Fig.3 UAV used in the experiment

首先驗(yàn)證TOA 和RSS 兩種測(cè)距方式的特性，綜合比較二者丟包率，實(shí)驗(yàn)結(jié)果如圖4所示。

圖4 兩種測(cè)距方式丟包率對(duì)比Fig.4 Packet Loss Rate

可以明顯看出，RSS 不存在明顯的丟包現(xiàn)象。受發(fā)射功率以及周?chē)h(huán)境的影響，UWB 在超出一定范圍后由于功率限制逐漸無(wú)法接收到測(cè)距信號(hào)。本文中經(jīng)過(guò)實(shí)測(cè)，對(duì)于低成本UWB 而言，大約在40～50 m 之間，同時(shí)受到客觀條件差異影響，每個(gè)設(shè)備極限測(cè)距距離可能并不一致。同時(shí)，根據(jù)圖4中紅圈可以看到，在到達(dá)極限測(cè)距距離后，在較短范圍內(nèi)，受電能波動(dòng)影響，同樣可能存在斷續(xù)的接收信號(hào)，該現(xiàn)象并不長(zhǎng)久存在。增加大功率的UWB 可以達(dá)到更遠(yuǎn)的距離，但是本文為了方便實(shí)驗(yàn)，僅使用了低成本低功耗的設(shè)備來(lái)驗(yàn)證TOA/RSS 聯(lián)合定位的算法。

以飛行任務(wù)起點(diǎn)位置為原點(diǎn)，建立東北天導(dǎo)航坐標(biāo)系，假定有4 架無(wú)人機(jī)自主規(guī)劃路徑到達(dá)特定位置充當(dāng)“偽衛(wèi)星”，實(shí)驗(yàn)中，將集成UWB和Zigbee 設(shè)備的電路板放在體育場(chǎng)內(nèi)，坐標(biāo)依次為(-7,-23)，(9,-19)，(-22,37)，(-2,42)，高度均為2 m，以模擬這些“偽衛(wèi)星”。同時(shí)，一架無(wú)人機(jī)以0.5 m/s 的速度在2 m 的高度上飛行，通過(guò)采集UWB、Zigbee 解算出的位置信息作為狀態(tài)觀測(cè)輸入EKF 以及ASEKF 與INS 進(jìn)行融合，從而獲得對(duì)位置的估計(jì)。作為參考，以實(shí)際飛行中PX4內(nèi)部的EKF 融合INS/GNSS，得到對(duì)參考位置的估計(jì)，可繪制不同信息源得出的飛行軌跡。

在圖5中，綠色曲線(xiàn)代表PX4 內(nèi)部提供的GNSS/INS 融合定位點(diǎn)，可作為參考軌跡。紅色曲線(xiàn)為T(mén)OA 解算的位置坐標(biāo)，當(dāng)距離增大時(shí)，可以看到局部放大圖中紅色的TOA 解算點(diǎn)不再密集，說(shuō)明UWB 出現(xiàn)丟包現(xiàn)象以致無(wú)法進(jìn)行定位，此時(shí)下方兩個(gè)錨點(diǎn)(-7,-23)(9,-19)與當(dāng)前解算點(diǎn)均約為50 m。為了驗(yàn)證RSS/TOA 無(wú)論在短程還是長(zhǎng)程定位下均可作為觀測(cè)信息源，為慣導(dǎo)提供位置信息，本實(shí)驗(yàn)設(shè)置上方兩個(gè)錨點(diǎn)(-22,37)(-2,42)與初始解算點(diǎn)距離均約為42 m，使得無(wú)人機(jī)在飛行尾程可以接收 RSS 信息而無(wú)法接收UWB 信息。同時(shí)，由RSS 信息解算出的位置如藍(lán)色散點(diǎn)所示，分布在參考軌線(xiàn)周?chē)?。通過(guò)圖6，可以更清晰地觀察觀測(cè)值分布。

圖5 不同信息源飛行軌線(xiàn)解算Fig.5 Positioning results of GNSS,TOA and RSS

圖6 飛行速度0.5 m/s 時(shí)TOA/RSS 定位結(jié)果Fig.6 Positioning results of TOA/RSS at speed of 0.5 m/s

可以看到，對(duì)于UWB 定位而言，定位精度較高，隨著無(wú)人機(jī)的飛行移動(dòng)定位結(jié)果具有很強(qiáng)的趨勢(shì)，RSS 定位越分散且定位精度相較于UWB定位波動(dòng)更大?？梢钥匆?jiàn)，在0～50 s 內(nèi)，UWB定位值較為平均且并未出現(xiàn)丟包的狀況，RSS 定位值波動(dòng)較大，但是始終保持一定的接受頻率。在50～60 s 內(nèi)，根據(jù)趨勢(shì)，可以看出UWB 定位值仍存在部分值，但相較于50 s 之前，其觀測(cè)數(shù)量已大為減少。最后，在60 s 之后，基本已經(jīng)看不見(jiàn)UWB 定位值，此時(shí)UWB 設(shè)備受到功率限制，已經(jīng)達(dá)到測(cè)距極限。

將前文解算出的位置坐標(biāo)作為觀測(cè)量，自適應(yīng)因子c設(shè)置為1.2，分別進(jìn)行擴(kuò)展卡爾曼和序貫卡爾曼濾波得到最終的位置估計(jì)，結(jié)合參考軌跡，與單獨(dú)用慣導(dǎo)解算對(duì)比，可得如下結(jié)論。

慣導(dǎo)系統(tǒng)（INS）、GNSS/INS 基于EKF 提供的參考軌跡（REF-EKF）、基于無(wú)線(xiàn)電定位的TOA/RSS/INS 聯(lián)合擴(kuò)展卡爾曼濾波（Radio-EKF）、基于無(wú)線(xiàn)電定位的TOA/RSS/INS 聯(lián)合序貫擴(kuò)展卡爾曼濾波算法（Radio-ASEKF）解算定位結(jié)果如圖6～7 所示，根據(jù)圖5可知無(wú)人機(jī)以0.5 m/s 速度飛行約35 m，行程共計(jì)約70 s，飛行期間在X軸方向位移約11 m，Y軸方向約34 m。顯然，根據(jù)圖7～8，僅用慣導(dǎo)系統(tǒng)進(jìn)行定位估算很快就會(huì)發(fā)散。在短距離范圍內(nèi)，Radio-EKF 以及Radio-ASEKF 均可提供較為準(zhǔn)確的位置估計(jì)值，同時(shí)Radio-ASEKF 的估計(jì)信息更為貼近參考軌跡。隨著距離的增大，在60～70 s 內(nèi)，UWB 模塊丟包明顯，可用信息只有RSS 信息，其測(cè)距方差較大，導(dǎo)致最后10s 內(nèi)劇烈波動(dòng)，在自適應(yīng)因子的調(diào)節(jié)下，Radio-ASEKF 比Radio-EKF 取得了更好的結(jié)果。此外，ASEKF 算法在前60 s 內(nèi)比EKF算法更接近參考軌跡，且對(duì)于后期的估算相對(duì)平滑，這也驗(yàn)證了算法的有效性。

圖7 x 方向不同算法解算結(jié)果Fig.7 Positioning results of x component at speed of 0.5 m/s

同樣的，y方向解算結(jié)果如8 所示。

圖8 y 方向不同算法解算結(jié)果Fig.8 Positioning results of y component at speed of 0.5 m/s

在得到上述研究結(jié)果后，為了進(jìn)一步探討其他因素對(duì)本文所提算法的影響，本文在同一布置場(chǎng)景下，將無(wú)人機(jī)速度提高至1 m/s，TOA 和RSS傳輸頻率分別為5 Hz、2 Hz，自適應(yīng)因子c設(shè)置為1.2。首先本文給出觀測(cè)值分布如圖9所示。

圖9 飛行速度1m/s 時(shí)TOA/RSS 定位結(jié)果Fig.9 Positioning results of TOA/RSS at speed of 1m/s

可以看到，在更高速度飛行時(shí)，定位點(diǎn)更少。將定位結(jié)果作為觀測(cè)輸入EKF 以及ASEKF 進(jìn)行對(duì)比，可以得到圖10～11。

圖10 x 方向1 m/s 飛行速度下不同算法解算結(jié)果Fig.10 Positioning results of x component at speed of 1m/s

根據(jù)圖10～11 可知，ASEKF 均取得了比EKF更貼近參考軌跡的結(jié)果。同樣，純慣性定位由于受到大量干擾，很快發(fā)散，以基于 EKF 的GNSS/INS 組合導(dǎo)航作為參考軌跡，對(duì)比EKF 及ASEKF，在圖10 中可以看出，ASEKF 取得了更貼近參考軌跡的定位結(jié)果，面對(duì)25 s 后，TOA 定位結(jié)果斷續(xù)或失效的情況，對(duì)于RSS 定位波動(dòng)大的結(jié)果也有一定的自適應(yīng)效果，在圖11 中，前5 s內(nèi)ASEKF 快速收斂，對(duì)于25 s 后RSS 定位波動(dòng)通用有一定的自適應(yīng)效果。將圖10 與圖7對(duì)比，圖11 與圖8對(duì)比，可以明顯看出圖10 中的定位波動(dòng)更大，圖11 中部分時(shí)刻波動(dòng)范圍比圖8中波動(dòng)范圍更大。這說(shuō)明飛行速度對(duì)最終定位結(jié)果影響較大，進(jìn)一步對(duì)比圖9以及圖6中觀測(cè)值數(shù)量，可知無(wú)人機(jī)在固定位置下以較大速度飛行時(shí)，TOA/RSS 定位的頻率將降低，同時(shí)根據(jù)定位設(shè)備的用戶(hù)手冊(cè)可知，RSS 測(cè)距最高頻率約15 Hz，而TOA 定位頻率約100 Hz，兩者測(cè)距頻率同時(shí)受到傳輸數(shù)據(jù)大小影響，與之成反比，即傳輸數(shù)據(jù)越多，測(cè)距頻率越低。同時(shí)，低速飛行下高頻率測(cè)距得到較多的冗余數(shù)據(jù)可以有效剔除離群點(diǎn)。因此，以TOA/RSS 定位作為觀測(cè)輸入濾波時(shí)，飛行速度將會(huì)影響觀測(cè)值數(shù)量以及精度，進(jìn)一步影響濾波結(jié)果。

圖11 y 方向1 m/s 飛行速度下不同算法解算結(jié)果Fig.11 Positioning results of y component at speed of 1m/s

5 結(jié) 論

本文針對(duì)GNSS 信息拒止環(huán)境下定位問(wèn)題，提出了一種基于TOA/RSS 聯(lián)合的自適應(yīng)序貫卡爾曼濾波算法，通過(guò)引入自適應(yīng)因子，結(jié)合TOA精度高、測(cè)距范圍短以及RSS 精度低、測(cè)距范圍廣的特點(diǎn)，擴(kuò)展了TOA/RSS 聯(lián)合測(cè)距范圍，并提高了位置估計(jì)的平滑性。最終的實(shí)驗(yàn)結(jié)果表明，這種自適應(yīng)序貫卡爾曼濾波算法相對(duì)于傳統(tǒng)擴(kuò)展卡爾曼濾波，有效提高了定位精度，對(duì)波動(dòng)更具魯棒性。