基于MEMS/GNSS 的多機(jī)協(xié)同無源定位

張 治，盧鴻謙*，班曉軍，黃顯林，劉雪梅

（1.哈爾濱工業(yè)大學(xué) 航天學(xué)院，黑龍江 哈爾濱 150000；2.航天飛行器生存技術(shù)與效能評估實驗室，北京 100085）

1 引言

相比于有源定位，無源定位是指設(shè)備本身不主動發(fā)射電磁波，只接收、處理目標(biāo)信號而完成定位的技術(shù)［1］。從未來戰(zhàn)場環(huán)境的需求分析，無源定位具有良好的隱蔽特性和穩(wěn)健的生存能力，且?guī)缀醪皇芨蓴_，環(huán)境適應(yīng)性強(qiáng)、作用范圍廣［2］，因此成為現(xiàn)代電子戰(zhàn)中不可或缺的重要環(huán)節(jié)，也是當(dāng)前國內(nèi)外研究的熱點。按照觀測量，用于無源定位的方法有多普勒頻率變化率定位（Dop?pler Rate of Chang，DRC）、相位差變化率定位（Phase Difference Rate of Chang，PDRC）、測向交叉定位（Angle of Arrival，AOA）、到達(dá)時差定位（Time Difference of Arrival，TDOA）、頻差定位（Frequency Difference of Arrival，F(xiàn)DOA）以及多種聯(lián)合定位方法等［3-5］。

目前，對于無源定位的研究主要集中于軍事需求，但無源定位也可在民用領(lǐng)域發(fā)揮巨大的作用。搭載無源定位功能的無人機(jī)可以更好地完成對非合作目標(biāo)在復(fù)雜環(huán)境條件下的科研勘探、所搜定位、應(yīng)急救援等任務(wù)。多無人機(jī)可簡單且高效地利用視覺圖像信息獲取目標(biāo)視線角，并以導(dǎo)航系統(tǒng)解算出的自身位置信息作為參考，基于AOA 定位算法完成對非合作目標(biāo)的定位，本文基于此背景開展研究。

MEMS 慣組具有小型、輕便的特點，適用于作為無人船［6］、無人機(jī)等小型無人載具的核心導(dǎo)航設(shè)備。近年來，隨著MEMS 慣組工藝的不斷優(yōu)化，該類傳感器的性能參數(shù)不斷提高［7］。使用較高精度的MEMS 慣組對于改善載機(jī)作為無源定位系統(tǒng)平臺的自身信息準(zhǔn)確性，從而改善定位性能有重要意義。MEMS 慣組無法單獨定位，需要組合導(dǎo)航算法實現(xiàn)載具的定位，學(xué)者們基于不同的組合導(dǎo)航問題開展了研究。劉鵬飛引入里程計信息減輕了組合導(dǎo)航中出現(xiàn)的衛(wèi)星信號失靈帶來的導(dǎo)航發(fā)散［8］。邢東峰等提出了一種基于模糊方法和信息自適應(yīng)估計的UKF 算法提升了低成本組合導(dǎo)航的速度和位置精度［9］。本文中載機(jī)所使用的SINS/GNSS 組合導(dǎo)航方法是一種被廣泛應(yīng)用的經(jīng)典導(dǎo)航方法，簡單可靠，滿足對文中定位算法性能分析的需求。

在測角定位算法中，由于單站無法感知距離信息，基于測向的無源定位一般需要多站協(xié)同工作［10］。得益于所需量測量少、物理意義明確、原理簡單和易于實現(xiàn)等特點，測向定位法一直以來都是無源定位領(lǐng)域的研究熱點。在AOA 定位模型中，測量角與坐標(biāo)狀態(tài)變量之間的關(guān)系是非線性的。對于偽線性估計器的偏差問題，研究者將MLF 方法、最速下降法、高斯-牛頓法等應(yīng)用于AOA 目標(biāo)定位中［11-14］，用于補(bǔ) 償或修 正這一 偏差。Pang 等在角度測量噪聲先驗信息缺失的情況下，通過將工具變量法與偽線性估計相結(jié)合所設(shè)計的估計器來提高系統(tǒng)估計的性能［15］。吳文龍、駱吉安等將最小二乘法、卡爾曼濾波等方法用于AOA 定位模型［16-17］。然而以上研究的條件設(shè)置中，測量站站址誤差及測角誤差均為高斯白噪聲，所用方法旨在進(jìn)一步提高系統(tǒng)在正常工作時的定位精度及計算效率。本文在眾多研究成果的基礎(chǔ)上，利用實測的某型號高精度MEMS慣組的噪聲數(shù)據(jù)，對使用SINS/GNSS 組合導(dǎo)航算法計算載機(jī)導(dǎo)航參數(shù)時的性能表現(xiàn)進(jìn)行了研究，使結(jié)果更加貼近實際應(yīng)用。然后，使用載機(jī)位置解算結(jié)果作為基站坐標(biāo)，將擴(kuò)展卡爾曼濾波應(yīng)用到測角定位算法中，給出對目標(biāo)的定位信息估計。最后，將具有較強(qiáng)容錯能力的聯(lián)邦卡爾曼濾波應(yīng)用于多機(jī)協(xié)同定位算法中，結(jié)合實際系統(tǒng)中可能出現(xiàn)的情況設(shè)計了一種含有故障的子系統(tǒng)，并在該情況下利用聯(lián)邦濾波算法提高了系統(tǒng)的容錯能力和可靠性。

2 工作原理

2.1 基于AOA 算法的雙機(jī)協(xié)同定位原理

本文基于實際應(yīng)用需求，載機(jī)自身的導(dǎo)航定位參數(shù)通過基于MEMS 的捷聯(lián)慣導(dǎo)系統(tǒng)與GNSS 的組合導(dǎo)航獲取。在多機(jī)協(xié)同定位非合作目標(biāo)的過程中，每架載機(jī)以自身為主機(jī)即載機(jī)A，其他載機(jī)為輔機(jī)即載機(jī)B，在主機(jī)體坐標(biāo)系下建立狀態(tài)方程。在本實驗的仿真條件中，基于擴(kuò)展卡爾曼濾波對目標(biāo)M 進(jìn)行定位。基于AOA 定位算法的原理如圖1 所示。其中，A，B，M代表不同載機(jī)的幾何中心。

圖1 雙機(jī)測角無源定位模型Fig.1 Passive positioning model of double-machine an?gle measurement

圖1 中，載機(jī)通過組合導(dǎo)航系統(tǒng)測得載機(jī)A的位置坐標(biāo)為（xA，yA，zA)，載機(jī)B 的位置坐標(biāo)為(xB，yB，zB)，M 為待定位目標(biāo)。則在A的體坐標(biāo)系下，B 的坐標(biāo)為（xAB，yAB，zAB)。B'，M'分別為B和M在A的體坐標(biāo)系中xoy平面的投影點，(φAM，ψAM)，(φBM，ψBM)分別為A，B對M的視線角。利用載機(jī)及目標(biāo)的空間坐標(biāo)與載機(jī)對目標(biāo)視線角之間的幾何關(guān)系，建立基于載機(jī)體坐標(biāo)系的狀態(tài)方程，并對目標(biāo)M進(jìn)行定位。

2.2 多機(jī)協(xié)同定位原理

基于AOA 測角定位模型，以僅有視線角的雙機(jī)無源定位算法為基礎(chǔ)，可拓展到多機(jī)對非合作目標(biāo)的定位。以3 架載機(jī)A，B，C 對非合作目標(biāo)定位為例，由卡爾曼濾波、數(shù)據(jù)融合等模塊實現(xiàn)對非合作目標(biāo)的定位。3 架載機(jī)中每兩架為一組，可組合成3 個獨立的子系統(tǒng)。以載機(jī)A 為例，以A 為主機(jī)，可獲得AB，AC 兩個子系統(tǒng)，測量計算所得的兩組目標(biāo)定位信息，利用聯(lián)邦卡爾曼濾波對兩組數(shù)據(jù)進(jìn)行融合，獲得目標(biāo)與A 的相對狀態(tài)最優(yōu)估計。而當(dāng)某個子系統(tǒng)出現(xiàn)故障無法實現(xiàn)精確定位時，本文所采用的聯(lián)邦卡爾曼濾波器也能夠自適應(yīng)地對錯誤數(shù)據(jù)進(jìn)行隔離，保證全局定位數(shù)據(jù)的最優(yōu)性和可靠性。B，C 可使用相同的算法實現(xiàn)對目標(biāo)的定位。

3 基于MEMS 組合導(dǎo)航的多機(jī)協(xié)同定位算法

3.1 載機(jī)的SINS/GNSS 組合導(dǎo)航定位算法

選取東-北-天為導(dǎo)航坐標(biāo)系，若Φk為k時刻據(jù)陀螺儀輸出計算所得的旋轉(zhuǎn)矢量，則由旋轉(zhuǎn)矢量計算所得的四元數(shù)變化公式為：

另有速度、位置更新公式如下：

式中：L，λ，h，ΔT分別為緯度、經(jīng)度、高度與時間間隔，RM以及后文中的RN為當(dāng)?shù)刈游缛?、卯酉圈半徑?/p>

使用SINS/GNSS 松組合模型，選取失準(zhǔn)角、速度、位置誤差、以及陀螺加表的隨機(jī)常值偏移作為狀態(tài)，即有：

系統(tǒng)狀態(tài)方程可由捷聯(lián)慣導(dǎo)的誤差傳播方程推導(dǎo)出，如下：

其中：下標(biāo)a代表姿態(tài)、v代表速度、p代表位置，系統(tǒng)方程中的矩陣M為誤差量間的關(guān)系矩陣；wε，w?是為了避免隨機(jī)常值對應(yīng)的系統(tǒng)噪聲陣過度收斂而加入的虛擬噪聲。陀螺可敏感到地球自轉(zhuǎn)角速度，故無需簡化該模型。

選取速度、位置誤差為量測量，量測方程為：

為了方便后續(xù)處理，經(jīng)組合導(dǎo)航算法修正的載機(jī)坐標(biāo)需統(tǒng)一由經(jīng)-緯-高坐標(biāo)系轉(zhuǎn)化至地心地固坐標(biāo)系，其轉(zhuǎn)換公式為：

其中e為第一偏心率。

3.2 基于組合導(dǎo)航參數(shù)的測角定位幾何精度誤差分析

幾何精度因子（Geometric Dilution of Preci?sion，GDOP）能夠直觀地描述目標(biāo)與定位基站的相對位置與理論定位誤差的關(guān)系。在獲得組合導(dǎo)航定位誤差參數(shù)后，本文利用GDOP 分析目標(biāo)在雙機(jī)和典型多機(jī)編隊下的誤差分布情況。

假設(shè)有N架載機(jī)對目標(biāo)進(jìn)行定位，目標(biāo)的定位坐標(biāo)與各載機(jī)的角度測量值的關(guān)系可表示為：

式中：(x，y，z)為目標(biāo)的坐標(biāo)，(xi，yi，zi)為第i架載機(jī)的坐標(biāo)(i=1，2，…，N)，(φi，ψi)為第i架載機(jī)對目標(biāo)的視線角測量值。求導(dǎo)可得：

式中：

則誤差的協(xié)方差陣為：

其中B=(FTF)?1FT。那么，測角定位法的GDOP 表達(dá)式為：

圖2 為基于組合導(dǎo)航獲得的載機(jī)定位誤差下，幾種典型布站方式的GDOP 等高線分布圖。除載機(jī)導(dǎo)航誤差參數(shù)之外，其他仿真條件為：載機(jī)間最短距離為1.5 km，測角誤差為0.5°，載機(jī)高度為0，目標(biāo)高度為5 km，載機(jī)布站中心點為坐標(biāo)原點，觀測目標(biāo)在x=?15～15 km、y=?15～15 km 內(nèi)移動時GDOP 理論值的分布。

圖2 典型基站分布下的GDOP 等高線分布圖Fig.2 GDOP contour distribution under typical base station distribution

由圖2 可知，在距中心5 km 時，理論定位誤差為0.1 km，在實際應(yīng)用中0.1 km 的定位誤差是不能接受的，因此，需進(jìn)一步研究基于AOA 的定位算法。

3.3 基于EKF 的雙機(jī)協(xié)同定位算法

在系統(tǒng)最優(yōu)估計的應(yīng)用中，卡爾曼濾波的貢獻(xiàn)是很大的，但目標(biāo)的坐標(biāo)與觀測角的關(guān)系是非線性的，本文應(yīng)用擴(kuò)展卡爾曼濾波給出對目標(biāo)定位的最優(yōu)估計。通過組合導(dǎo)航獲得的載機(jī)A 的狀態(tài)為，載機(jī)B的狀態(tài)為為獲得目標(biāo)M相對A 的坐標(biāo)及速度，以A 為主機(jī)，B 為輔機(jī)，在A 的體坐標(biāo)系下，定義狀態(tài)變量X=為M 在A 的體坐標(biāo)系下的坐標(biāo)-速度分量。B 在A 的體坐標(biāo)系下的狀態(tài)為，A 的狀態(tài) 為ψA）為載機(jī)A對目標(biāo)的視線角，（φB，ψB）為載機(jī)B對目標(biāo)的視線角。依據(jù)載機(jī)A，B 及目標(biāo)M 的坐標(biāo)與Z的關(guān)系，k時刻動態(tài)系統(tǒng)可描述為：

A，B 對M 的觀測方程可描述為：

其中V為系統(tǒng)過程噪聲和測量噪聲?？梢姡^測方程為非線性方程，則擴(kuò)展卡爾曼濾波流程可描述為：

3.4 基于聯(lián)邦濾波的多機(jī)協(xié)同定位算法

基于聯(lián)邦濾波器的思想，在多機(jī)協(xié)同定位的情況下，為降低算法的維度，提高計算效率，并增強(qiáng)系統(tǒng)的容錯能力，本文將每兩架作為一個分系統(tǒng)，基于3.3 節(jié)的雙機(jī)協(xié)同定位算法，完成多機(jī)協(xié)同定位。以A，B，C 三機(jī)為例，AB，AC 分別組成兩個分系統(tǒng)，利用各子系統(tǒng)單步濾波更新之后的協(xié)方差陣的跡設(shè)計信息分配系數(shù)，具體聯(lián)邦濾波算法流程如下：

（1）系統(tǒng)過程噪聲為Q，用k?1 時刻的全局最優(yōu)估計X(k?1)更新各子系統(tǒng)k?1 時刻的狀態(tài)，各子系統(tǒng)的狀態(tài)協(xié)方差陣為Pab(k?1)、Pac(k?1)，根據(jù)信息守恒原則，設(shè)計：

（2）對k時刻的狀態(tài)進(jìn)行估計：各子系統(tǒng)利用3.2 節(jié)的算法處理自己的量測信息，獲得局部最優(yōu)估計Xab(k)，Pab(k)，Xac(k)和Pac(k)；

（3）對分配系數(shù)γab，γac和子系統(tǒng)過程噪聲協(xié)方差陣Qab，Qac進(jìn)行更新；

（4）獲得全局最優(yōu)估計：

由式（22）可得，假設(shè)AC 子系統(tǒng)的某一狀態(tài)估計值誤差較大，則其對應(yīng)的協(xié)方差陣的跡也較大，信息分配系數(shù)γac會減小，AC 子系統(tǒng)的信息在全局估計的影響也會減小。根據(jù)信息守恒原則，γab將大于γac，進(jìn)而增加AB 子系統(tǒng)在全局估計中的影響。AC 子系統(tǒng)的估計結(jié)果誤差越大，甚至接近無窮大，此方法對錯誤信息的隔離效果越好。

4 仿真結(jié)果

本文仿真中所用MEMS 及GNSS 參數(shù)見表1。本文仿真設(shè)置為A，B，C 三架載機(jī)協(xié)同對目標(biāo)完成定位，系統(tǒng)仿真時間為140 s，前30 s 只做基于MEMS 的組合導(dǎo)航參數(shù)解算，之后組合導(dǎo)航參數(shù)解算基本穩(wěn)定，與多機(jī)協(xié)同定位算法同步進(jìn)行。載機(jī)對目標(biāo)的視線角測量誤差為0.5（?3σ）的高斯白噪聲，系統(tǒng)采樣頻率為1 kHz。理論上，如果一個系統(tǒng)是濾波穩(wěn)定的，那么濾波初值是任意給定的，本文將利用載機(jī)的坐標(biāo)及對目標(biāo)的視線角信息，經(jīng)過三角形解算獲得的目標(biāo)相對坐標(biāo)作為濾波初值，以保證系統(tǒng)能夠更快收斂。

表1 MEMS 及GNSS 的性能參數(shù)Tab.1 Performance parameters of MEMS and GNSS

4.1 系統(tǒng)在正常環(huán)境下的仿真

當(dāng)AB 子系統(tǒng)、AC 子系統(tǒng)均能正常工作時，通過仿真可以得出以下結(jié)論：（1）從圖3～圖4 坐標(biāo)誤差、速度跟蹤曲線和相對距離誤差得出，無論是基于聯(lián)邦濾波的多機(jī)協(xié)同定位算法，還是基于EKF 的雙機(jī)協(xié)同定位算法，經(jīng)過有限次迭代后，都能夠給出高精度的定位結(jié)果；（2）值得注意的是，由于本文初值選取的方式，圖4 的初始距離誤差在百米量級，這與本文3.2 節(jié)GDOP 誤差分布是吻合的；（3）經(jīng)過數(shù)據(jù)分析，原本AOA 定位算法的理論誤差在百米量級，本文的算法將相對距離誤差降到了10 米量級，這樣的誤差水平在實際應(yīng)用是可以接受的；（4）聯(lián)邦濾波與子系統(tǒng)EKF 濾波在精度上并沒有差別，在沒有更高精度的測量值的情況下，增加載機(jī)數(shù)量并不能提高定位精度，但聯(lián)邦濾波能夠提高系統(tǒng)的可靠性和容錯能力。也就是說，如果個別載機(jī)由于自身定位不準(zhǔn)、無法識別目標(biāo)等因素造成單個子系統(tǒng)不能對目標(biāo)完成定位或定不準(zhǔn)，整個系統(tǒng)也能夠利用其他正常工作的子系統(tǒng)的信息完成對目標(biāo)的定位，保證任務(wù)成功。

圖4 聯(lián)邦濾波與子系統(tǒng)EKF 濾波相對距離誤差對比Fig.4 Comparison of relative distance errors between federated filter and subsystem EKF

4.2 子系統(tǒng)故障時的仿真結(jié)果

假設(shè)載機(jī)B 由于自身系統(tǒng)故障或受到干擾不能正確接收GNSS 信號，導(dǎo)致自身導(dǎo)航參數(shù)出錯，且AB 間距離遠(yuǎn)小于目標(biāo)與A，B 的距離，綜合造成AB 子系統(tǒng)對目標(biāo)定位不準(zhǔn)，其他仿真條件不變，仿真結(jié)果如圖5～圖7 所示。

圖5 聯(lián)邦濾波與AB子系統(tǒng)EKF濾波相對距離誤差對比Fig.5 Comparison of relative distance errors between federated filter and EKF of AB subsystem

圖6 聯(lián)邦濾波與AC子系統(tǒng)EKF濾波相對距離誤差對比Fig.6 Comparison of relative distance errors between federated filter and AC subsystem EKF

圖7 聯(lián)邦濾波與集中式濾波相對距離誤差對比Fig.7 Comparison of relative distance errors between federated filter and centralized filter

圖5 顯示，AB 子系統(tǒng)的定位誤差已經(jīng)超過了2.0×104m，但聯(lián)邦濾波的結(jié)果要遠(yuǎn)好于AB子系統(tǒng)的結(jié)果。由圖6 可知，聯(lián)邦濾波的結(jié)果雖不及AC 子系統(tǒng)的濾波結(jié)果，但聯(lián)邦濾波的結(jié)果在100 s 之后，由于AB 子系統(tǒng)濾波結(jié)果的急劇增大，聯(lián)邦濾波誤差反而呈下降趨勢。圖7 給出了聯(lián)邦濾波與集中式濾波的對比結(jié)果，集中式濾波由于綜合了所有子系統(tǒng)信息，在AB 子系統(tǒng)誤差急劇變化的情況下，集中式濾波的全局估計誤差也急劇增大，聯(lián)邦濾波則展示了隔離錯誤的能力。為更加清晰地對比不同定位算法的性能，表2 給出了各子系統(tǒng)及全局聯(lián)邦濾波算法獲得的20～100 s 之間相對距離誤差均值、誤差率（相對距離誤差/相對距離×100%）及相對速度標(biāo)誤差均值。

表2 不同定位算法相對距離及速度誤差對比Tab.2 Comparison of relative distance and velocity errors of different positioning algorithms

5 結(jié)論

本文基于MEMS/GNSS 獲取的載機(jī)導(dǎo)航定位參數(shù)展開對多機(jī)協(xié)同無源定位算法的研究。在一個解算周期內(nèi)，首先基于民用MEMS 的實測誤差數(shù)據(jù)，通過組合導(dǎo)航算法解算載機(jī)的定位參數(shù)；然后在僅有載機(jī)對目標(biāo)的視線角測量的情況下，基于導(dǎo)航解算獲得的載機(jī)坐標(biāo)，利用EKF通過測角定位算法給出對目標(biāo)相對載機(jī)的定位信息的最優(yōu)估計。此外，為提高系統(tǒng)的可靠性，本文基于聯(lián)邦卡爾曼濾波對目標(biāo)完成多機(jī)協(xié)同定位。仿真實驗表明，基于EKF 的測角無源定位算法在通過MEMS/GNSS 解算載機(jī)導(dǎo)航參數(shù)的情況下，將理論定位誤差從100 m 量級降到10 m 量級，完成對目標(biāo)的定位；相比集中濾波，基于聯(lián)邦卡爾曼濾波的多機(jī)協(xié)同定位算法，能夠在某載機(jī)導(dǎo)航定位出現(xiàn)故障，子系統(tǒng)對目標(biāo)定位出現(xiàn)嚴(yán)重偏差的情況下，大大降低對全局定位信息的影響，提高了全局定位的精度。多機(jī)協(xié)同定位算法在基于MEMS/GNSS 組合導(dǎo)航算法獲取載機(jī)坐標(biāo)的情況下能完成對目標(biāo)的定位，且利用聯(lián)邦濾波算法能夠?qū)ψ酉到y(tǒng)載機(jī)因自身定位不準(zhǔn)及軌跡偏差等因素造成的錯誤數(shù)據(jù)進(jìn)行隔離，保證全局估計的有效性和可靠性。在當(dāng)前研究中，量測信息為含有高斯白噪聲的模擬數(shù)據(jù)，項目團(tuán)隊的進(jìn)一步研究將在真實量測數(shù)據(jù)下展開。