基于改進粒子濾波的無人機編隊協(xié)同導(dǎo)航算法

岳敬軒，王紅茹，朱東琴，ALEKSANDR Chupalov

哈爾濱工程大學(xué) 信息與通信工程學(xué)院，哈爾濱 150001

隨著無人機相關(guān)技術(shù)的不斷成熟與相關(guān)產(chǎn)業(yè)的不斷發(fā)展，無人機憑借著其體積小、成本低、機動靈活和視角寬廣等特點，在快遞投送、空中攝影和節(jié)目表演等民用領(lǐng)域以及軍事偵察與探測領(lǐng)域得到了廣泛的應(yīng)用與關(guān)注［1］。與此同時，隨著軍事民用領(lǐng)域所執(zhí)行任務(wù)日趨多樣化、復(fù)雜化以及作戰(zhàn)模式趨于多元化，單個無人機執(zhí)行能力有限且無法滿足需求，這也就促進了無人機編隊相關(guān)技術(shù)的誕生［2］。而精確的導(dǎo)航和定位信息是編隊技術(shù)的關(guān)鍵，是實現(xiàn)無人機群航跡規(guī)劃、穩(wěn)定飛行和任務(wù)順利執(zhí)行的重要保障［3-4］。

目前，衛(wèi)星導(dǎo)航是無人機群定位的主要方法，但僅依靠衛(wèi)星導(dǎo)航，無法應(yīng)對無人機群在城市和叢林等區(qū)域執(zhí)行任務(wù)時，由于遮擋所導(dǎo)致的衛(wèi)星導(dǎo)航無法使用的情況［5］。因此，通過無人機之間的信息交互來完成協(xié)同導(dǎo)航，從而提升無人機群的定位精度的方法已成為當(dāng)下的研究熱點［6-7］。

擴展卡爾曼濾波（Extended Kalman Filter，EKF）和無跡卡爾曼濾波（Unscented Kalman Filter， UKF）是非線性協(xié)同導(dǎo)航系統(tǒng)中最常見的融合算法［7］。EKF 需對非線性方程進行泰勒展開，通常只保留到一階項，忽略其高階項，將其近似為線性系統(tǒng)再進行卡爾曼估計，在線性化時可能會引入誤差，從而導(dǎo)致系統(tǒng)發(fā)散。UKF 結(jié)合了無跡變換（Unscented Transform，UT）和卡爾曼濾波（Kalman Filter， KF），通過選取采樣點，利用這些采樣點的均值和協(xié)方差逼近非線性系統(tǒng)，避免了求解雅可比矩陣，對于強非線性問題有較好效果，但是其參數(shù)選擇問題沒有得到完全解決，且濾波效果易受初值影響［7-8］。此外，這2 種算法都是在高斯假設(shè)的前提下得到的最優(yōu)估計，在算法仿真過程中采用的是理想化的高斯噪聲，而在實際復(fù)雜環(huán)境下受各種因素的影響，多無人機（Unmanned Aerial Vehicle，UAV）協(xié)同系統(tǒng)的量測噪聲常常會出現(xiàn)野值，這會導(dǎo)致量測噪聲的概率密度不再服從高斯分布。

粒子濾波（Particle Filter， PF）理論上適用于任何狀態(tài)空間模型，不受高斯線性條件的約束，其思想是基于蒙特卡羅方法，通過從后驗概率中抽取的隨機狀態(tài)粒子來表達其概率分布［8］，其主要包括序貫重要性采樣（Sequential Importance Sampling， SIS）和重采樣2 個階段［9］。研究表明，重要性采樣的權(quán)重方差會隨著時間的推移而增加，導(dǎo)致粒子退化現(xiàn)象，即多數(shù)粒子權(quán)重幾乎為零［10］。在實時系統(tǒng)中，為了在粒子數(shù)較少的情況下獲得較高的估計精度，處理該退化問題的另一個重要方法是選擇合適的重要性概率密度函數(shù)（Probability Density Function， PDF）［11］。文獻［12-13］提出了擴展粒子濾波器（Extended Particle Filter， EPF）和無跡粒子濾波器（Unscented Particle Filter， UPF），并利用EKF 和UKF 給出粒子建議分布函數(shù)。EPF 可以獲得較好的重要性密度，但是線性化處理帶來了誤差；UPF 雖然導(dǎo)航精度較高，但是計算效率太低且穩(wěn)定性差，并且狀態(tài)估計誤差協(xié)方差接近過程噪聲協(xié)方差［14］。文獻［15］提出Huber 魯棒容積裂變粒子濾波，在容積卡爾曼基礎(chǔ)上基于Huber 函數(shù)融合L2 與L1 估計，得到粒子建議分布函數(shù)，容積粒子濾波可以計算重要性PDF 的二階矩和高階矩，但本身穩(wěn)定性易受到容積點影響，且提出的算法也只是在高斯模型下進行了驗證，有一定的局限性。在重采樣階段，常用的重采樣方法是直接丟棄小權(quán)重粒子并復(fù)制大權(quán)重粒子，這樣容易導(dǎo)致粒子多樣性缺失［16］。人工智能算法為解決粒子匱乏問題提供了一條有效的研究思路［17］。文獻［18-20］提出了基于遺傳算法改進的PF，基本原理是重采樣過程中對粒子進行交叉變異操作從而改變粒子權(quán)重，但是遺傳算法等智能算法自身存在過早收斂問題，且計算成本也很大，導(dǎo)致濾波效果不理想。文獻［21］提出了UKF 和自適應(yīng)差分進化算法聯(lián)合優(yōu)化的PF 算法，在傳統(tǒng)差分算法基礎(chǔ)上采用自適應(yīng)策略，避免出現(xiàn)過早收斂造成局部最優(yōu)的現(xiàn)象，然而，該方法通過大量迭代來尋求最優(yōu)解且依賴初值選取，在對實時性要求較高的高維協(xié)同導(dǎo)航系統(tǒng)中并不適用。

在現(xiàn)有研究基礎(chǔ)上，針對單主多從式無人機搭載低精度傳感器的情況，提出了一種改進的PF算法實現(xiàn)協(xié)同導(dǎo)航的數(shù)據(jù)融合，解決了復(fù)雜環(huán)境中強非線性模型下量測噪聲時變且不滿足高斯特性的問題，在保證實時性的情況下得到了更高的定位精度。首先利用Levenberg-Marquardt（LM）迭代優(yōu)化的EPF 來近似估計狀態(tài)量的后驗PDF，與傳統(tǒng)PF 使用先驗概率作為重要性PDF不同的是，該方法需要在重要性采樣階段結(jié)合最新觀測數(shù)據(jù)得到粒子的重要性PDF，從而優(yōu)化采樣粒子的位置分布。隨后引入自適應(yīng)權(quán)重因子提出快速重采樣方法，可以實時調(diào)整重采樣粒子數(shù)目，不同于傳統(tǒng)PF 直接丟棄小權(quán)重粒子，快速重采樣方法根據(jù)粒子權(quán)重將粒子分類，只對低于閾值的其他粒子進行交叉操作來提升小權(quán)重粒子的權(quán)重，從而保留了粒子多樣性且降低了重采樣計算復(fù)雜度。

1 模型構(gòu)建

1.1 單主多從無人機編隊運動模型

在無人機協(xié)同導(dǎo)航領(lǐng)域，一般以二維空間建立導(dǎo)航模型，即將無人機飛行軌跡投影到同一高度下的二維平面以簡化問題。這樣的模型構(gòu)建雖然可以減少計算量，但同時無法知悉無人機群高度上的變化，與實際情況會有較大誤差。因此，為了使研究更具有普適性，根據(jù)勻加速曲線運動學(xué)式（1）建立式（2）所示的系統(tǒng)狀態(tài)方程。

式中：s0和v0分別為初始位置和速度；s和v分別為當(dāng)前位置和速度；a為加速度；t為運動時間。

式中：Xk=[x，y，z，vx，vy，vz，ax，ay，az]T為其中一架從機k時刻的三維位置、速度和加速度狀態(tài)量；Wk為模型k時刻的系統(tǒng)過程噪聲且滿足均值為0、協(xié)方差為Qk的高斯分布；G為過程噪聲驅(qū)動陣；T為采樣時間；F為狀態(tài)轉(zhuǎn)移矩陣，即

該模型本質(zhì)上利用的是航位推算原理［22］，由于過程噪聲的存在以及系統(tǒng)遞推模型的性質(zhì)，采用此方法構(gòu)建的模型來估計無人機的位置時，每一采樣時刻的速度和加速度噪聲都會疊加到下一時刻的狀態(tài)量上，從而導(dǎo)致誤差隨時間累積，以及導(dǎo)航精度的降低。因此，構(gòu)建合適的觀測模型，引入觀測數(shù)據(jù)來校正狀態(tài)量成為了重點問題。

1.2 單主多從無人機編隊觀測模型

考慮到單主多從式無人機群都搭載高精度的導(dǎo)航設(shè)備成本太高，規(guī)定主機搭載高精度導(dǎo)航設(shè)備，所有從機搭載用于自身定位的低精度傳感器。從機可以利用主機的高精度導(dǎo)航設(shè)備獲取的準(zhǔn)確位置信息通過解算來校正從機的狀態(tài)量，實現(xiàn)協(xié)同信息的融合，抑制誤差累積。觀測模型為［2，23］

式中：p和v分別為GPS 獲得的從機絕對位置和速度信息；S、α和β分別為主從無人機之間的相對距離、相對航向角和相對俯仰角的觀測量。

相對導(dǎo)航信息的具體量測函數(shù)為［2］

式中：xm、ym和zm為主機位置信息；x、y和z為從機位置信息；R'k為相對觀測噪聲。

2 改進粒子濾波協(xié)同導(dǎo)航算法

2.1 LM 優(yōu)化的迭代擴展粒子濾波算法

傳統(tǒng)的PF 算法并沒有考慮最新的量測信息，在觀測模型精度較高的情況下容易導(dǎo)致較大誤差，同時采樣粒子的位置分布準(zhǔn)確度很大程度上決定了算法的性能優(yōu)劣，選取合適的重要性PDF 將會改善這類問題。

EPF 是在采樣階段用EKF 計算每個粒子的一階矩、二階矩，計算的同時引入最新的量測量來近似后驗PDF［23］。產(chǎn)生合理的建議PDF 的過程中，利用LM 方法在狀態(tài)更新時多次迭代，同時調(diào)整協(xié)方差矩陣，使粒子的分布更接近于目標(biāo)的后驗分布。

式中：pN(Xk|Z1：k)為滿足高斯特性的PDF；N(·)為高斯分布和為狀態(tài)量均值和協(xié)方差。基本步驟和原理如下所示。

步驟1初始化

k=0 時，設(shè)定初始狀態(tài)量X0，從先驗PDFp(X0)中采樣N個初始粒子～p(X0)，初始粒子權(quán)重為=1/N，初始協(xié)方差矩陣P0為

步驟2重要性采樣

1） 采用局部線性化的方式得到狀態(tài)轉(zhuǎn)移矩陣F、過程噪聲驅(qū)動陣G、量測矩陣H、量測噪聲驅(qū)動陣R，利用EKF 算法得到狀態(tài)預(yù)測值和先驗協(xié)方差。

式中：為k-1 時刻第i個粒子的估計值。

2） 采用LM 迭代優(yōu)化的EKF 算法對式（10）和式（11）得到的均值和協(xié)方差進行迭代處理。大多數(shù)的算法更新迭代過程是利用Gauss-Newton 方法求解，然而由于線性化等誤差的引入以及狀態(tài)空間模型與觀測數(shù)據(jù)存在差異，導(dǎo)致狀態(tài)的觀測更新不能保障估計誤差的一致減少，使得協(xié)方差陣的估計值要比真實值偏低，因此Gauss-Newton 迭代方法收斂速度較慢，性能不太穩(wěn)定?；贚M 的迭代方法將量測更新過程轉(zhuǎn)換為最小二乘問題，在每次迭代過程中使用參數(shù)μi調(diào)整狀態(tài)量的協(xié)方差陣來進行更新，當(dāng)μi較大時，LM 迭代更接近Gauss-Newton 迭代。μi一般由模型的雅可比矩陣求得，具體取值參考文獻［25］，該文獻已從數(shù)學(xué)推理角度證明其收斂性。在不同環(huán)境下分配給μi不同的參數(shù)值，可以獲得最優(yōu)的狀態(tài)估計和方差估計。

基于LM 優(yōu)化修正后的協(xié)方差矩陣為

設(shè)進行M次迭代，對于j(1 ≤i≤M)次迭代，有如下計算步驟：

式中：為第i個粒子在第1 次迭代的初始后驗信息；為k時刻的先驗信息。

觀測矩陣雅可比矩陣為

濾波增益矩陣為

第j+1 次迭代的協(xié)方差矩陣為

更新后的狀態(tài)量估計值為

迭代終止條件為

式中：ε為預(yù)先設(shè)定的最小誤差閾值。

假設(shè)c次迭代后迭代終止，則k時刻的狀態(tài)估計和協(xié)方差為

3） 通過重要性采樣得到重要性PDF，即

步驟3計算粒子權(quán)重

按式（23）和式（24）更新粒子的相應(yīng)權(quán)重并進行歸一化處理，分子中的第1 項和第2 項分別是似然函數(shù)和狀態(tài)轉(zhuǎn)移概率［9］。

2.2 快速重采樣模型

重采樣主要是為了進一步解決SIS 中存在的粒子退化現(xiàn)象，即經(jīng)過幾步迭代之后，出現(xiàn)大部分粒子的權(quán)重變得很小而少部分粒子占據(jù)較大權(quán)重的現(xiàn)象。圖1 為重采樣示意圖，圓點表示粒子，圓點的面積大小代表了粒子權(quán)重大小。為了防止大量的計算浪費在大部分的小權(quán)重粒子身上，傳統(tǒng)的PF 算法直接舍棄這些粒子，從而導(dǎo)致了粒子匱乏，降低了粒子多樣性。

圖1 重采樣示意圖Fig.1 Resampling diagram

由于對所有粒子都進行重采樣操作會增加計算量，導(dǎo)致實時性較差。為解決此問題，本文提出了快速重采樣（Fast Resampling， FR）方法，在保留粒子多樣性的同時降低了算法復(fù)雜度。FR 方法首先在重采樣時刻對粒子進行預(yù)處理，讓小權(quán)重粒子向高似然區(qū)域移動，提高其權(quán)重。然后根據(jù)設(shè)定的粒子權(quán)重閾值對粒子進行分類，得到2 類粒子即中等權(quán)重粒子以及其他粒子。中等權(quán)重粒子不對其進行重采樣，對另一集合包含了大、小權(quán)重的粒子進行抽樣尺度判斷，判斷是否進行重采樣。具體步驟如下所示。

步驟1預(yù)處理

k時刻的加權(quán)粒子集合對可以表示為如果則產(chǎn)生新粒子重新計算其權(quán)重，并保證新的權(quán)重介于和之間。

步驟2粒子集分類

其中，A 類粒子為大、小權(quán)重粒子的混合集合，B 類粒子為中等權(quán)重的粒子集合。B 類粒子權(quán)重大小中等，粒子穩(wěn)定性高，直接保留。

步驟3A 類粒子重采樣

求出A 類粒子的有效抽樣尺度為

設(shè)定抽樣尺度閾值為Nth=2×NA/3，當(dāng)Neff＞Nth時，粒子集有效性高，不進行重采樣。否則，選取2 個粒子使用式（26）所示的歐式距離表示其分布關(guān)系，歐氏距離可以簡單有效地反映粒子位置拓撲關(guān)系。

式中：γ為粒子維度。

優(yōu)化處理后新的粒子權(quán)重為

式中：Ta=1+log2dmn，Ta為自適應(yīng)權(quán)重因子，一般為整數(shù)。當(dāng)dmn接近0 時，2 個粒子位置接近，2個粒子不需處理。

經(jīng)過自適應(yīng)重采樣處理后的小權(quán)重粒子達到B 類集合的選取范圍則保留下來，否則丟棄，由大權(quán)重粒子復(fù)制補齊粒子集。

為了進一步證明所提出的FR 方法計算效率高，參考文獻［26］對其他幾種經(jīng)典重采樣算法的復(fù)雜度進行比較，表1［26］記錄了不同重采樣算法的運算操作數(shù)。其中M為殘差重采樣的殘留粒子數(shù)。對比表1，本文提出的FR 方法采用分類處理的手段，參與重采樣的粒子數(shù)最大為NA，大幅降低了計算復(fù)雜度，提高了協(xié)同導(dǎo)航實時性。

表1 不同重采樣算法的運算操作數(shù)比較［26］Table 1 Comparison of operational operands of different resampling algorithms［26］

圖2 描述了采用PF 和快速重采樣PF（FastResampling Particle Filter， FRPF）在狀態(tài)估計中的粒子權(quán)重大小對比。可見，PF 進行狀態(tài)估計時小權(quán)重粒子占比越來越多，大權(quán)重粒子占比較少，容易導(dǎo)致多數(shù)粒子被淘汰；FRPF 將小權(quán)重粒子經(jīng)過優(yōu)化處理后，權(quán)重會增大。這會避免經(jīng)過多次迭代后，大部分粒子都是由少數(shù)權(quán)重較高粒子復(fù)制而來的情況，大幅降低粒子貧化現(xiàn)象，在進行狀態(tài)估計時幾乎不存在粒子權(quán)重差異較大的現(xiàn)象。因此，本文所提出的方法能夠有效地保留粒子多樣性。

圖2 粒子權(quán)重對比Fig.2 Comparison of particle weights

步驟4重采樣并輸出系統(tǒng)狀態(tài)估計

由式（24）先對權(quán)重歸一化，最終得出N個粒子的加權(quán)值，即改進PF 的濾波結(jié)果：

綜上所述，改進PF 的無人機編隊協(xié)同導(dǎo)航算法步驟如算法1 所示。

?

3 協(xié)同導(dǎo)航實驗結(jié)果與分析

為了驗證算法的可實施性和導(dǎo)航效果的精確性，本文進行了實驗數(shù)據(jù)的離線仿真，所設(shè)置的傳感器參數(shù)參考自開源導(dǎo)航網(wǎng)站（www.psins.org.cn）和文獻［23］。表2 列出了編隊中主從無人機所搭載的傳感器設(shè)備的配置參數(shù)，機群的導(dǎo)航信息由搭載的傳感器獲得，所有無人機均可與主機之間通信。圖3 為實驗用到的傳感器，傳感器噪聲服從N(0，σ)的高斯模型。

表2 傳感器配置參數(shù)Table 2 Parameters of sensors configuration

圖3 實驗設(shè)備Fig.3 Experimental equipments

同時設(shè)置了以下場景仿真所提出的算法的有效性：主從式無人機協(xié)同導(dǎo)航網(wǎng)絡(luò)中主機為位置已知的移動節(jié)點，從機為位置解算的其余移動節(jié)點。實驗仿真時長為1 000 s，采樣周期為1 s，在此期間，主機依據(jù)高精度RTK 和INS 設(shè)備進行絕對導(dǎo)航得到較為精確的導(dǎo)航信息。從機通過GPS 獲取位置速度絕對導(dǎo)航信息，同時對主機測距，并測得相對航向角和相對俯仰角來進行協(xié)同導(dǎo)航。

主機位置、速度和加速度初始狀態(tài)為[0 m，0 m，0 m，10 m/s，-10 m/s，2 m/s，0.01 m/s2，-0.02 m/s2， 0 m/s2]T。主機飛行軌跡如圖4所示，其中，x、y和z軸分別指向北、東和天方向。

圖4 主機飛行狀態(tài)Fig.4 Master UAV flight state

從無人機位置、速度和加速度初始狀態(tài)為[50 m，-40 m，0 m，10 m/s，-10 m/s，2 m/s，0.01 m/s2，-0.02 m/s2， 0 m/s2]T。狀態(tài)方程在各方向上的位置誤差ΔS=1 m，速度誤差Δv=0.1 m/s。

非高斯噪聲有閃爍噪聲、異常觀測值等類型，一般按式（29）處理方式簡化表示為不同方差高斯噪聲加權(quán)。

式中：σ1為傳感器噪聲；σ2為量測異常值；λ取值0.7；vk為k時刻加權(quán)噪聲變量。

觀測噪聲中相對測距誤差的測量時變野值Sk為

相對測角誤差的測量時變野值θk為

考慮到無人機群是飛行速度較快的群體，因此導(dǎo)航的實時性非常重要。算法中采樣粒子數(shù)不宜過大，N=100 為宜，LM 迭代次數(shù)為2?；谏鲜龇抡鏃l件，對單主從無人機編隊進行仿真，分析其中一架從機的狀態(tài)估計誤差。用所提出的改進PF 算法與PF、EPF 和UPF 進行對比，其余算法均采用隨機重采樣，實驗結(jié)果如圖5～圖7 所示。

圖5 仿真軌跡對比Fig.5 Comparison of simulation track

圖6 各方向算法位置誤差對比Fig.6 Comparison of position error of each algorithm in each direction

圖7 各方向算法速度誤差對比Fig.7 Comparison of velocity error of each algorithmin each direction

為了定量地分析4 種算法下的定位誤差，進行了N次蒙特卡羅實驗并統(tǒng)計了均方根誤差（Root Mean Square Error， RMSE）來表示主從無人機協(xié)同導(dǎo)航的精度：

同時，仿真中記錄了每一步迭代不同算法狀態(tài)估計所消耗的時間來驗證導(dǎo)航實時性，并求出平均值。表3 為1 000 s 仿真時間內(nèi)4 種算法的三維位置誤差均值和平均計算時間。其中位置誤差均值計算公式為

表3 各算法平均位置誤差和單步計算時間Table 3 Average position error and single step calculation time of each algorithm

式中：Errx、Erry和Errz分別為x、y和z軸的誤差。

通過圖5～圖7 和表3 可以清楚看出，PF 具有最快的計算速度，但隨著時間的推移，會出現(xiàn)誤差累積，并且三維坐標(biāo)系中的位置和速度均方誤差是最不穩(wěn)定的。EPF 軌跡在初始階段具有較好的濾波效果，但當(dāng)噪聲伴隨異常值時，精度在x軸方向的下降更為明顯，速度誤差高達4 m/s，濾波后的軌跡也開始偏移飛行軌跡，計算速度較快，但導(dǎo)航效果并不理想。UPF 由于太過依賴Sigma 點的位置選取，導(dǎo)致其各方向上濾波效果差異較大，計算速度慢，導(dǎo)航效果一般。本文提出的基于LM 調(diào)整預(yù)測協(xié)方差的EPF 結(jié)合快速重采樣方法的改進PF 實現(xiàn)的導(dǎo)航效果最好，計算速度與PF 相差不大。隨著濾波收斂，各方向位置誤差基本在8 m 以下，整個仿真時間的三維位置誤差均值為11.87 m，各方向速度均方誤差基本在1 m/s 左右，在前150 s 的高斯噪聲下，各方向位置和速度誤差穩(wěn)定，誤差較低；在后續(xù)的非高斯噪聲下，誤差波動較小，狀態(tài)估計值不會隨著時間的推移而發(fā)散，導(dǎo)航效果明顯優(yōu)于PF、EPF 和UPF。

為了進一步驗證復(fù)雜環(huán)境下所提出的算法的魯棒性，進行了GNSS 拒止條件下的實驗。在上述仿真條件基礎(chǔ)上，主從無人機GNSS 設(shè)備無法提供絕對導(dǎo)航信息。主機依靠高精度INS 設(shè)備短時間內(nèi)依舊可以保持較高的導(dǎo)航精度，從機只利用相對導(dǎo)航信息來減小誤差，表4 和圖8 為實驗結(jié)果。

表4 復(fù)雜環(huán)境下改進PF 算法的三維誤差Table 4 Three-dimensional error of improved PF algorithm in complex environment

圖8 復(fù)雜環(huán)境下改進PF 算法的魯棒性Fig.8 Robustness of improved PF algorithm in complex environment

在GNSS 拒止環(huán)境下，本文所提出的算法在僅利用協(xié)同信息時仍能實現(xiàn)定位。雖然主機只使用INS 進行導(dǎo)航本身帶來了誤差累積，但從機位置誤差與GPS 可用情況下的誤差基本接近，失去低精度的GPS 絕對信息后雖然誤差波動略大但收斂速度略快。此外，由于GPS 可以觀測絕對速度，而導(dǎo)航傳感器則無法觀測到速度信息，因此本文所提出的算法雖然速度誤差雖然較高，但也基本維持在1.5 m/s 以下。

4 結(jié) 論

本文提出了一種改進的PF 算法，用于搭載低精度傳感器無人機群的協(xié)同導(dǎo)航。該方法由LM 優(yōu)化的EPF 結(jié)合最新的量測量得到粒子的重要性PDF，從而使得粒子分布位置更為準(zhǔn)確；同時提出快速重采樣策略來改進傳統(tǒng)PF 的重采樣階段，從而保留了粒子多樣性且避免了粒子退化，大幅提高了算法的速度，保證了協(xié)同導(dǎo)航的實時性。仿真結(jié)果表明，與PF、EPF 和UPF 相比，本文提出的算法整體濾波效果較優(yōu)，計算速度較快。改進的PF 算法僅通過粗略的測量便可準(zhǔn)確地校正誤差，這可以減少編隊中高精度導(dǎo)航設(shè)備的數(shù)量，降低成本；同時所提出的算法在GNSS 拒止條件下魯棒性較高，這也提高了復(fù)雜環(huán)境下協(xié)同導(dǎo)航性能。但單主結(jié)構(gòu)太過依賴主機，未來可以在多主結(jié)構(gòu)上深入研究，并進一步與協(xié)同導(dǎo)航算法相結(jié)合。