基于改進粒子濾波的無人機編隊協同導航算法

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

哈爾濱工程大學 信息與通信工程學院，哈爾濱 150001

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

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

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

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

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

1 模型構建

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

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

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

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

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

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

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

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

相對導航信息的具體量測函數為［2］

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

2 改進粒子濾波協同導航算法

2.1 LM 優化的迭代擴展粒子濾波算法

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

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

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

步驟1初始化

k=0 時，設定初始狀態量X0，從先驗PDFp(X0)中采樣N個初始粒子～p(X0)，初始粒子權重為=1/N，初始協方差矩陣P0為

步驟2重要性采樣

1） 采用局部線性化的方式得到狀態轉移矩陣F、過程噪聲驅動陣G、量測矩陣H、量測噪聲驅動陣R，利用EKF 算法得到狀態預測值和先驗協方差。

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

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

基于LM 優化修正后的協方差矩陣為

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

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

觀測矩陣雅可比矩陣為

濾波增益矩陣為

第j+1 次迭代的協方差矩陣為

更新后的狀態量估計值為

迭代終止條件為

式中：ε為預先設定的最小誤差閾值。

假設c次迭代后迭代終止，則k時刻的狀態估計和協方差為

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

步驟3計算粒子權重

按式（23）和式（24）更新粒子的相應權重并進行歸一化處理，分子中的第1 項和第2 項分別是似然函數和狀態轉移概率［9］。

2.2 快速重采樣模型

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

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

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

步驟1預處理

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

步驟2粒子集分類

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

步驟3A 類粒子重采樣

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

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

式中：γ為粒子維度。

優化處理后新的粒子權重為

式中：Ta=1+log2dmn，Ta為自適應權重因子，一般為整數。當dmn接近0 時，2 個粒子位置接近，2個粒子不需處理。

經過自適應重采樣處理后的小權重粒子達到B 類集合的選取范圍則保留下來，否則丟棄，由大權重粒子復制補齊粒子集。

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

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

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

圖2 粒子權重對比Fig.2 Comparison of particle weights

步驟4重采樣并輸出系統狀態估計

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

綜上所述，改進PF 的無人機編隊協同導航算法步驟如算法1 所示。

?

3 協同導航實驗結果與分析

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

表2 傳感器配置參數Table 2 Parameters of sensors configuration

圖3 實驗設備Fig.3 Experimental equipments

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

主機位置、速度和加速度初始狀態為[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 主機飛行狀態Fig.4 Master UAV flight state

從無人機位置、速度和加速度初始狀態為[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。狀態方程在各方向上的位置誤差ΔS=1 m，速度誤差Δv=0.1 m/s。

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

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

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

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

考慮到無人機群是飛行速度較快的群體，因此導航的實時性非常重要。算法中采樣粒子數不宜過大，N=100 為宜，LM 迭代次數為2。基于上述仿真條件，對單主從無人機編隊進行仿真，分析其中一架從機的狀態估計誤差。用所提出的改進PF 算法與PF、EPF 和UPF 進行對比，其余算法均采用隨機重采樣，實驗結果如圖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次蒙特卡羅實驗并統計了均方根誤差（Root Mean Square Error， RMSE）來表示主從無人機協同導航的精度：

同時，仿真中記錄了每一步迭代不同算法狀態估計所消耗的時間來驗證導航實時性，并求出平均值。表3 為1 000 s 仿真時間內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 具有最快的計算速度，但隨著時間的推移，會出現誤差累積，并且三維坐標系中的位置和速度均方誤差是最不穩定的。EPF 軌跡在初始階段具有較好的濾波效果，但當噪聲伴隨異常值時，精度在x軸方向的下降更為明顯，速度誤差高達4 m/s，濾波后的軌跡也開始偏移飛行軌跡，計算速度較快，但導航效果并不理想。UPF 由于太過依賴Sigma 點的位置選取，導致其各方向上濾波效果差異較大，計算速度慢，導航效果一般。本文提出的基于LM 調整預測協方差的EPF 結合快速重采樣方法的改進PF 實現的導航效果最好，計算速度與PF 相差不大。隨著濾波收斂，各方向位置誤差基本在8 m 以下，整個仿真時間的三維位置誤差均值為11.87 m，各方向速度均方誤差基本在1 m/s 左右，在前150 s 的高斯噪聲下，各方向位置和速度誤差穩定，誤差較低；在后續的非高斯噪聲下，誤差波動較小，狀態估計值不會隨著時間的推移而發散，導航效果明顯優于PF、EPF 和UPF。

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

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

圖8 復雜環境下改進PF 算法的魯棒性Fig.8 Robustness of improved PF algorithm in complex environment

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

4 結 論

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