蔣銳，李俊，徐友云，王小明，李大鵬

（1.南京郵電大學(xué)通信與信息工程學(xué)院，江蘇 南京 210003；2.南京郵電大學(xué)通信與網(wǎng)絡(luò)技術(shù)國家工程研究中心，江蘇 南京 210003）

0 引言

隨著科技水平的不斷提高，人們對于導(dǎo)航定位[1]方面的需求和應(yīng)用也發(fā)生了改變，逐漸表現(xiàn)出多元化和復(fù)雜化的特點(diǎn)。首先是定位精度方面，高精度的定位技術(shù)顯得尤其重要；其次是自動化定位方面，要求導(dǎo)航系統(tǒng)能夠自主實(shí)現(xiàn)包括定位、建圖與路徑規(guī)劃在內(nèi)的多項(xiàng)功能。因此，單獨(dú)使用各導(dǎo)航系統(tǒng)已經(jīng)很難滿足當(dāng)下的導(dǎo)航性能要求，采用組合導(dǎo)航技術(shù)已經(jīng)成為提高導(dǎo)航系統(tǒng)整體性能的有效途徑。

全球衛(wèi)星導(dǎo)航系統(tǒng)能夠提供全球范圍內(nèi)的實(shí)時(shí)導(dǎo)航定位服務(wù)。其中，美國的全球定位系統(tǒng)（GPS,global positioning system）[2]應(yīng)用較廣泛，且系統(tǒng)定位精度高，定位誤差不隨時(shí)間累積。但由于該GPS是非自主導(dǎo)航，定位結(jié)果受環(huán)境影響很大，在城市峽谷、隧道和山區(qū)中，衛(wèi)星信號經(jīng)常因受到遮擋而無法正常工作。同時(shí)GPS 也存在著導(dǎo)航數(shù)據(jù)采樣頻率低、無法提供載體姿態(tài)信息等固有缺點(diǎn)。

捷聯(lián)慣性導(dǎo)航系統(tǒng)（SINS,strap-down inertial navigation system）可以對GPS 的固有缺點(diǎn)進(jìn)行補(bǔ)償。SINS 是一種自主式導(dǎo)航系統(tǒng)[3]，具有采樣頻率高、噪聲低、隱蔽性強(qiáng)、不受外界干擾等眾多優(yōu)點(diǎn)，但系統(tǒng)定位誤差隨時(shí)間累積，長期工作精度差。SINS 硬件結(jié)構(gòu)簡單、體積小、重量輕，常用于陸用導(dǎo)航中。利用SINS 輔助GPS 定位，可以增加多余觀測量，提高衛(wèi)星接收機(jī)的動態(tài)特性，也可以修正SINS 的累積誤差。

然而，在衛(wèi)星信號受到嚴(yán)重干擾的情況下，GPS定位誤差增大，SINS 由于累積誤差得不到修正而失去作用。此時(shí)，引入5G 基站定位[4]為融合帶來了新的可能。5G 基站定位中，基于到達(dá)角（AOA,angle of arrival）的定位方法利用陣列天線估計(jì)發(fā)射信號的到達(dá)角度，構(gòu)建以2 個(gè)基站為端點(diǎn)的射線，其交點(diǎn)即發(fā)射源位置[5]。5G 大規(guī)模多輸入多輸出（mMIMO,massive MIMO）技術(shù)[6]和超密集組網(wǎng)（UDN,ultra-dense network）技術(shù)[7]極大提高了AOA定位的精度。mMIMO 技術(shù)在基站端配置有幾十根甚至上百根天線，可以通過波束成形使基站發(fā)出一個(gè)具有很窄范圍和高度定向性的波束，通過結(jié)合空間譜估計(jì)技術(shù)[8]，對角度的估計(jì)更為精確，極大提高了AOA 的估計(jì)精度。UDN 高密度部署小微基站，基站之間的間距可以縮短至幾百米甚至幾十米，單位覆蓋面積內(nèi)基站數(shù)目的增多，減少了基站與用戶之間的距離，增加了基站定位時(shí)的視線線路（LOS,line of sight）概率，對于AOA 定位，距離越近，角度發(fā)生微小偏差時(shí)產(chǎn)生的位置誤差范圍越小。5G基站AOA 定位在室內(nèi)定位[9]中會受到反射、折射的情況，造成各路信號能量混疊，mMIMO 利用子空間分解技術(shù)可以準(zhǔn)確分離出多路信號的來向，達(dá)到小于1°的AOA 估計(jì)精度；室外定位則易遭受多徑、非視距、衰落情況的影響，無法知道來波準(zhǔn)確方向，UDN 增加定位時(shí)視線線路概率，縮短距離，室外定位誤差可達(dá)米級。

由Carlson[10]提出的聯(lián)邦卡爾曼濾波器的基本思想是將多個(gè)子濾波器的信息先進(jìn)行處理然后再集中融合，利用方差上界技術(shù)和信息守恒原則消除局部濾波器的相關(guān)性，容錯(cuò)性好、濾波精度高、從局部濾波到全局濾波的融合算法簡單、計(jì)算量少。通過聯(lián)邦卡爾曼濾波器，可以融合GPS、AOA 和SINS 的優(yōu)點(diǎn)，使三者構(gòu)成的組合導(dǎo)航系統(tǒng)可以有效解決單獨(dú)GPS 定位出現(xiàn)的問題，提供載體位置、速度和姿態(tài)信息，增強(qiáng)系統(tǒng)連續(xù)定位能力，克服復(fù)雜環(huán)境影響，實(shí)現(xiàn)高精度定位。

然而，當(dāng)GPS 子系統(tǒng)出現(xiàn)故障時(shí)，常規(guī)聯(lián)邦卡爾曼濾波器不能有效抑制定位發(fā)散，因此需要對聯(lián)邦卡爾曼濾波器做進(jìn)一步改進(jìn)以提高組合導(dǎo)航的可靠性。文獻(xiàn)[11]提出了一種自適應(yīng)聯(lián)邦卡爾曼濾波器導(dǎo)航方法，在載體發(fā)生擾動異?；虼嬖谳^大動力學(xué)模型誤差時(shí)，利用觀測預(yù)報(bào)殘差自適應(yīng)調(diào)節(jié)聯(lián)邦卡爾曼濾波器的分配因子，補(bǔ)償導(dǎo)航過程中出現(xiàn)的異常誤差，但是信息分配因子的不同取值不會影響聯(lián)邦卡爾曼濾波器主濾波器的狀態(tài)估計(jì)精度，只能改變子濾波器的狀態(tài)估計(jì)精度。文獻(xiàn)[12]改變了由于傳統(tǒng)假設(shè)量測噪聲統(tǒng)計(jì)特性不變而造成濾波結(jié)果精度受限的問題，并在GPS-SINS 組合導(dǎo)航系統(tǒng)中進(jìn)行驗(yàn)證。但該方案每次濾波時(shí)都要多次調(diào)用模糊推理系統(tǒng)，增加了運(yùn)行時(shí)間，而且反饋沒有涉及誤差協(xié)方差矩陣，整體定位精度不高。文獻(xiàn)[13]針對松耦合GPS-SINS 組合導(dǎo)航系統(tǒng)，提出了一種改進(jìn)的聯(lián)邦強(qiáng)跟蹤卡爾曼濾波器方案。該方案設(shè)計(jì)了三層容錯(cuò)結(jié)構(gòu)，通過對錯(cuò)誤數(shù)據(jù)的判斷、子濾波器的故障檢測和主濾波器的故障檢測，可以有效地檢驗(yàn)量測數(shù)據(jù)異常，并對均方誤差協(xié)方差矩陣進(jìn)行加權(quán)，從而間接調(diào)整濾波增益。但該方案使用前后時(shí)刻的速度誤差對故障進(jìn)行檢測，需要知道正常情況下速度誤差的先驗(yàn)統(tǒng)計(jì)特性，而且對加權(quán)系數(shù)的設(shè)計(jì)也比較復(fù)雜。

綜上所述，本文提出了一種基于聯(lián)邦卡爾曼的容錯(cuò)GPS-AOA-SINS 組合導(dǎo)航算法。該算法較好地融合了GPS、AOA 和SINS 的優(yōu)點(diǎn)，并針對系統(tǒng)特點(diǎn)進(jìn)一步對聯(lián)邦卡爾曼濾波器進(jìn)行改進(jìn)，在子濾波器后加入故障檢測與處理模塊，當(dāng)故障檢測出異常時(shí)，利用殘差卡方檢測法設(shè)定的閾值與故障檢測函數(shù)之比對故障子濾波器濾波增益矩陣K進(jìn)行自適應(yīng)調(diào)節(jié)，改變故障子濾波器中預(yù)測值和量測值的比重，并進(jìn)一步將故障處理后的聯(lián)邦卡爾曼濾波結(jié)果與故障子濾波器時(shí)間更新值融合。該算法可以有效抑制因異常量測值帶來的定位發(fā)散，提高組合導(dǎo)航系統(tǒng)的可靠性與穩(wěn)定性。

1 GPS-AOA-SINS 組合導(dǎo)航系統(tǒng)設(shè)計(jì)

改進(jìn)的聯(lián)邦卡爾曼濾波的組合導(dǎo)航系統(tǒng)結(jié)構(gòu)設(shè)計(jì)如圖1 所示，SINS 用作主濾波器和2 個(gè)子濾波器的參考系統(tǒng)，子濾波器1 是SINS 和GPS 的融合結(jié)果，子濾波器2 是SINS 和AOA 的融合結(jié)果[14]?？紤]到聯(lián)邦卡爾曼處理的數(shù)據(jù)需要是同一時(shí)刻的，因此在組合導(dǎo)航系統(tǒng)中，AOA 移動臺自身的時(shí)間戳基準(zhǔn)需要和GPS 完成時(shí)間同步。由于SINS、GPS 和AOA 數(shù)據(jù)采樣頻率不一致，通常SINS 采樣頻率高，而GPS 和AOA 采樣頻率低，子濾波器1 和子濾波器2 以采樣頻率低的周期進(jìn)行融合，對其他時(shí)刻采樣頻率高的數(shù)據(jù)不予利用，之后再以固定時(shí)間間隔輸出子濾波器狀態(tài)估計(jì)值到主濾波器進(jìn)行融合。

圖1 改進(jìn)的聯(lián)邦卡爾曼濾波的組合導(dǎo)航系統(tǒng)結(jié)構(gòu)設(shè)計(jì)

子濾波器狀態(tài)估計(jì)值進(jìn)入主濾波器進(jìn)行融合前，需要進(jìn)行故障檢測與處理。故障檢測采用殘差卡方檢測法和根據(jù)新息序列進(jìn)行的滑窗平均法結(jié)合共同進(jìn)行判斷，一旦確定子濾波器存在故障，說明此時(shí)該子濾波器的狀態(tài)估計(jì)值不正確，利用殘差卡方檢測法中閾值與故障檢測函數(shù)的比值自適應(yīng)調(diào)節(jié)故障子濾波器，將處理后的結(jié)果輸入主濾波器進(jìn)行融合，并將融合結(jié)果進(jìn)一步與故障子濾波器時(shí)間更新值進(jìn)行融合。該結(jié)構(gòu)通過對聯(lián)邦卡爾曼濾波器進(jìn)行改進(jìn)，利用包含在異常量測值中的有用信息，降低異常誤差對濾波的影響，提高了組合導(dǎo)航系統(tǒng)的濾波精度和容錯(cuò)能力，可以滿足一定程度上的實(shí)時(shí)定位。

1.1 組合導(dǎo)航系統(tǒng)的狀態(tài)方程

1.1.1 系統(tǒng)狀態(tài)方程

系統(tǒng)狀態(tài)方程是被所有子濾波器所共享的?；趯INS 的誤差源的分析[15]，將SINS 導(dǎo)航參數(shù)的誤差量作為系統(tǒng)的狀態(tài)量。導(dǎo)航坐標(biāo)系選取當(dāng)?shù)貣|?北?天坐標(biāo)系，選取的15 維狀態(tài)變量為

其中，?E、?N、?U分別是導(dǎo)航坐標(biāo)系下的東、北、天方向上的平臺姿態(tài)角誤差；δvE、δvN、δvU分別是東、北、天方向上的速度誤差；Lδ、δλ、hδ是位置誤差；?gx、?gy、?gz是沿載體坐標(biāo)系x、y、z軸上的陀螺儀漂移誤差，并假設(shè)其為隨機(jī)常值漂移；?ax、?ay、?az是加速度計(jì)偏置誤差，并假設(shè)為隨機(jī)常值偏置零偏。

組合導(dǎo)航系統(tǒng)的狀態(tài)方程為

其中，F(xiàn)(t)是狀態(tài)轉(zhuǎn)移矩陣，G(t)是控制矩陣，F(xiàn)(t)和G(t)可參考文獻(xiàn)[16]；W(t)是陀螺儀和加速度計(jì)的隨機(jī)噪聲，假設(shè)其為高斯白噪聲，計(jì)算式為

1.1.2 系統(tǒng)量測方程

GPS-SINS 和AOA-SINS 子濾波器結(jié)合可對SINS 誤差模型的姿態(tài)、速度、位置進(jìn)行修正。

將GPS 導(dǎo)航輸出的速度、位置信息與SINS 的速度、位置信息作差，可以得到子濾波器1 的量測方程和量測矩陣分別為

其中，L、λ、h分別表示緯度、經(jīng)度和高程位置信息，VSINSE、VSINSN、VSINSU和LSINS、λSINS、hSINS分別表示SINS 的速度和位置信息，VGPSE、VGPSN、VGPSU和LGPS、λGPS、hGPS分別表示GPS 測量得到的速度和位置信息，V1(t)=[vVGPSEvVGPSNvVGPSUvglvgλ vgh]T表示GPS 速度和位置的測量噪聲。

將5G 基站AOA 定位給出的位置和SINS 系統(tǒng)給出的位置作差，可以得到子濾波器2 的量測方程和量測矩陣分別為

其中，LSINS、λSINS、hSINS是SINS 的位置信息，LAOA、λAOA、hAOA是5G 基站AOA 定位的位置信息，是5G 基站AOA 定位的位置量測噪聲。

1.1.3 主濾波器信息融合

本文中聯(lián)邦卡爾曼濾波器采用帶有反饋重置的結(jié)構(gòu)。這種結(jié)構(gòu)的子濾波器的故障與隔離能力好，但是主濾波器的故障與隔離能力差；融合后的全局濾波精度高，子濾波因?yàn)橛蟹答佒刂镁纫驳玫搅颂岣摺?/p>

將GPS-AOA-SINS 組合導(dǎo)航模型的狀態(tài)方程和量測方程離散化[17]，得到的系統(tǒng)模型為

其中，X(k)是組合導(dǎo)航系統(tǒng)的狀態(tài)向量，F(xiàn)(k,k?1)是該系統(tǒng)的一階轉(zhuǎn)移矩陣，G(k)是該系統(tǒng)的噪聲矩陣，W(k)是該系統(tǒng)的噪聲向量。W(k)的協(xié)方差矩陣是Q(k)，Vi(k)的協(xié)方差矩陣是Ri(k)。

主濾波器的融合方程為

主濾波器對各個(gè)子濾波器進(jìn)行信息反饋，此過程是在各個(gè)子濾波器之間進(jìn)行全局最優(yōu)估計(jì)信息的分配。子濾波器的融合方程為

式(10)和式(11)中，i=1,2,…,N表示子濾波器的個(gè)數(shù)，m表示主濾波器的個(gè)數(shù)；Pi、Qi分別表示子濾波器的預(yù)測協(xié)方差矩陣和過程噪聲協(xié)方差矩陣，表示子濾波器的狀態(tài)估計(jì)值；Pg、Qg分別表示全局預(yù)測協(xié)方差矩陣和過程噪聲協(xié)方差矩陣，表示全局狀態(tài)估計(jì)值，本文中主濾波器無信息分配，不進(jìn)行濾波，只進(jìn)行融合，其估計(jì)值就取為；βi(0<βi≤1)表示第i個(gè)子濾波器的信息分配系數(shù)，需要滿足式(12)所示的信息守恒原則。

其中，βm=0。

2 改進(jìn)的聯(lián)邦卡爾曼濾波器

GPS-AOA-SINS 融合可以實(shí)現(xiàn)高精度定位，但在周圍環(huán)境復(fù)雜的情況下，GPS 子系統(tǒng)容易出現(xiàn)定位異常甚至衛(wèi)星不可見的情況，造成組合導(dǎo)航系統(tǒng)定位精度下降。帶有反饋重置的聯(lián)邦卡爾曼子濾波器輸出的狀態(tài)估計(jì)值在經(jīng)過主濾波器融合后，會通過反饋進(jìn)一步污染其他子濾波器，導(dǎo)致定位結(jié)果不可靠甚至發(fā)散。所以，本文算法在子濾波器和主濾波器之間加入故障檢測與處理器[18]，對故障進(jìn)行實(shí)時(shí)處理，從而保證全局狀態(tài)估計(jì)的精度。

2.1 故障檢測

2.1.1 殘差卡方檢測

導(dǎo)航系統(tǒng)中常常使用卡方檢測算法進(jìn)行故障檢測，其中殘差卡方檢測[19-20]利用量測值殘差（即新息）可以較好地檢測和隔離某些子濾波器的突變故障，且該算法簡單。但因?yàn)殚撝档脑O(shè)定，會有一定概率的漏檢和誤檢現(xiàn)象發(fā)生，所以需要和其他故障檢測算法聯(lián)合使用。

卡爾曼濾波中，殘差表示為

其中，Zk是k時(shí)刻濾波器的量測信息，Xk|k?1是濾波器的一步預(yù)測信息。

當(dāng)導(dǎo)航系統(tǒng)模型準(zhǔn)確，k時(shí)刻量測信息Zk無異常時(shí)，卡爾曼濾波的殘差rk是高斯白噪聲，其均值為0，如式(14)所示。

rk的方差為

當(dāng)系統(tǒng)有故障發(fā)生時(shí)，殘差rk的均值不再為0，均值和方差分別為

定義故障檢測函數(shù)為

其中，λk服從自由度為n的卡方分布，即λk～χ2(n)，n是量測值Zk的維度。

當(dāng)沒有故障發(fā)生時(shí)，HkXk|k?1=Zk是對Zk的最好預(yù)測估計(jì)，rk理論上很??；但是當(dāng)Zk發(fā)生突變故障時(shí)，殘差rk會出現(xiàn)較大偏差，λk也會相應(yīng)地增加。因此，需要設(shè)定相應(yīng)的檢測規(guī)則去檢測故障發(fā)生。

檢測規(guī)則如下：當(dāng)λk>Td時(shí)，判定系統(tǒng)發(fā)生故障，需要進(jìn)行處理；當(dāng)λk≤Td時(shí)，判定系統(tǒng)正常工作。其中，Td是故障檢測閾值，Td的選取反映了故障檢測能力。在給定誤警概率P(λk>Td)=α的情況下，Td可以由式(18)求得

2.1.2 根據(jù)新息序列進(jìn)行的滑窗平均法

理論殘差方差的定義如式(15)所示，實(shí)際殘差方差可以對殘差序列進(jìn)行滑窗平均得到。標(biāo)準(zhǔn)的殘差序列滑窗平均定義為[21]

其中，cr是估計(jì)殘差協(xié)方差，表示對前M個(gè)殘差向量方差求平均的結(jié)果；i0=k?M+1，M根據(jù)經(jīng)驗(yàn)視具體情況而定，一般可以選為8～10?；捌骄ㄖ饕鹌交淖饔?，可以消除偶然出現(xiàn)異常的情況，但是由于窗口有一定長度，在故障結(jié)束之后，殘差估計(jì)值會受到故障值的影響，導(dǎo)致實(shí)際故障判定區(qū)域被加寬，直至窗口離開故障區(qū)域這種影響才會消失。

定義理論殘差協(xié)方差A(yù)k與估計(jì)殘差協(xié)方差cr之間的關(guān)系為[22]

其中，Ak是6 維理論殘差協(xié)方差矩陣，其對角陣分別對應(yīng)于速度和位置協(xié)方差，由于二者協(xié)方差值相差過大，本文在GPS 三軸速度上添加噪聲，如果帶有位置協(xié)方差去求矩陣的跡，會看不出明顯的故障現(xiàn)象發(fā)生，因此進(jìn)行求跡運(yùn)算時(shí)需要對Ak進(jìn)行降秩處理，只在速度對應(yīng)的矩陣位置進(jìn)行求秩運(yùn)算。

AOCk的大小反映了觀測值在當(dāng)前時(shí)刻的穩(wěn)定性。當(dāng)量測噪聲準(zhǔn)確時(shí)，Ak與cr近似相等，AOCk約等于1；當(dāng)量測噪聲不準(zhǔn)確，觀測值出現(xiàn)異常時(shí)，cr會發(fā)生突變偏離Ak，AOCk會遠(yuǎn)大于或者小于1。因此，量測值是否異?？梢酝ㄟ^AOCk的大小進(jìn)行判斷，在1 附近（±0.5）認(rèn)為量測值無異常，遠(yuǎn)大于或者小于1 認(rèn)為量測值發(fā)生異常，濾波器出現(xiàn)故障，需要進(jìn)行處理。

僅僅通過一種故障判別的手段很容易出現(xiàn)漏檢和誤檢的情況，而一旦漏檢的異常值進(jìn)入濾波，就會對融合結(jié)果產(chǎn)生影響，降低融合濾波精度，還會污染其他子濾波器。所以，應(yīng)該采用2 種算法結(jié)合去對故障進(jìn)行判斷，降低漏檢概率，從而減小異常值的影響，提高融合濾波精度。

2.2 故障處理

故障檢測后需要對故障濾波器進(jìn)行處理，通常的處理方法是對有故障的濾波器進(jìn)行隔離，其輸出狀態(tài)估計(jì)值不進(jìn)入主濾波器進(jìn)行融合或者只利用故障子濾波器的一階狀態(tài)預(yù)測值，這樣使子濾波器的量測信息被完全拋棄，沒有得到合理利用。因此本文提出一種算法，可以最大化利用量測值中的有用信息。

聯(lián)邦卡爾曼子濾波器進(jìn)行經(jīng)典卡爾曼濾波時(shí)間更新和量測更新過程如下[23]。

狀態(tài)的一步預(yù)測為

狀態(tài)估計(jì)為

濾波增益矩陣為

一步預(yù)測誤差方差陣為

估計(jì)誤差方差陣為

從式(22)中可以看出，狀態(tài)估計(jì)結(jié)果與k時(shí)刻量測值和預(yù)測值有關(guān)，通過自適應(yīng)調(diào)節(jié)濾波增益矩陣Kk[24]可以調(diào)整對量測值和狀態(tài)預(yù)測值的信任程度。當(dāng)Zk的可用程度大時(shí)，Kk就相應(yīng)取得大；當(dāng)Zk的可用程度小時(shí)，Kk就相應(yīng)取得小，減弱觀測噪聲對濾波結(jié)果的影響。所以，當(dāng)量測值出現(xiàn)異常時(shí)，可以通過自適應(yīng)調(diào)節(jié)濾波增益，達(dá)到對量測值的充分利用，使融合后的結(jié)果平穩(wěn)性更好，可靠性更高。

改進(jìn)后的濾波增益矩陣為

其中，ak是自適應(yīng)系數(shù)，且ak∈(0,1)。對ak設(shè)置如下規(guī)則

由于λk在有突變的時(shí)候數(shù)值會發(fā)生較大的增加，因此取殘差卡方檢測閾值Td和λk的比值調(diào)節(jié)濾波增益。當(dāng)量測值出現(xiàn)異常時(shí)，ak變得很小，Kk會大大降低對異常量測值的信任，使濾波結(jié)果精度更高，穩(wěn)定性更好。

自適應(yīng)調(diào)節(jié)濾波增益在濾波異常時(shí)，融合濾波結(jié)果不發(fā)散，在異常處定位誤差大大減小，且在異?；謴?fù)后可以很快向無故障情況收斂。但在異常處定位誤差與只考慮k時(shí)刻預(yù)測值的情況相比誤差值較高，收斂速度較快，結(jié)合二者優(yōu)缺點(diǎn)，在主濾波器融合后，進(jìn)一步和當(dāng)前時(shí)刻故障子濾波器時(shí)間更新值融合，既降低了定位誤差，也加快了收斂速度。

其中，P1,k|k1?是故障子濾波器一步預(yù)測誤差協(xié)方差，反映時(shí)間更新的質(zhì)量優(yōu)劣，其值越小表示狀態(tài)估計(jì)的結(jié)果越精確；是故障子濾波器一階狀態(tài)預(yù)測值，這里研究的是GPS 出現(xiàn)故障時(shí)的情況，因此故障子濾波器就是子濾波器1；Pe是故障處理后的全局誤差估計(jì)協(xié)方差；Xe是故障處理后的全局狀態(tài)估計(jì)值，即故障子濾波器時(shí)間更新值與故障處理后經(jīng)過主濾波器融合后輸出的狀態(tài)估計(jì)值加權(quán)融合結(jié)果。

3 仿真結(jié)果與分析

3.1 仿真條件設(shè)置

本文采用仿真實(shí)驗(yàn)來驗(yàn)證GPS-AOA-SINS 組合導(dǎo)航系統(tǒng)的優(yōu)越性，并進(jìn)一步驗(yàn)證改進(jìn)聯(lián)邦卡爾曼濾波可提高組合導(dǎo)航定位可靠性。仿真勻速直線運(yùn)動的物體的運(yùn)行軌跡，物體運(yùn)動時(shí)間為300 s，運(yùn)動物體模擬軌跡作為其運(yùn)行過程中的真實(shí)值，初始位置為北緯32.081°、東經(jīng)118.771°、高程40 m。SINS 典型的輸出頻率為100～1 000 Hz，故取SINS的數(shù)據(jù)更新頻率為100 Hz，與SINS 相比，GPS 輸出頻率低，典型輸出頻率為10 Hz，由于物體運(yùn)動速度不是非?？欤瑢PS 的數(shù)據(jù)更新頻率限制為1 Hz，對于基站AOA 定位每1 s 進(jìn)行定位數(shù)據(jù)的訪問，主濾波器的融合濾波周期設(shè)置為1 s。殘差卡方檢測在置信區(qū)間為95%時(shí)對故障的檢測最為精確，故誤警率設(shè)置為0.05，相應(yīng)的檢測閾值為12.592。SINS 多采用光學(xué)陀螺和加速度計(jì)構(gòu)成，零偏是這2 個(gè)儀器都會表現(xiàn)出的常值誤差，在不同等級的慣性測量單元中，加速度計(jì)和陀螺的典型零偏也不同，主要有航海級、航空級、中等、戰(zhàn)術(shù)級和消費(fèi)級[25]。本文模擬航空級SINS，其加速度計(jì)零偏為0.03～0.10 mg，陀螺零偏為0.01 °/h 左右。同時(shí)，由于受到多種誤差源的影響，所有的慣性型傳感器都表現(xiàn)出隨機(jī)噪聲，航空級加速度計(jì)隨機(jī)噪聲大概為，光纖陀螺隨機(jī)噪聲為。根據(jù)以上條件，仿真SINS 誤差源的設(shè)定如下：陀螺儀常值漂移為0.03°/h，角度隨機(jī)游走為，加速度計(jì)隨機(jī)常值偏置為 1×10?4g，速度隨機(jī)游走為。SINS 提供導(dǎo)航結(jié)果之前需要對姿態(tài)、位置和速度進(jìn)行初始化。姿態(tài)的初始化是針對橫滾角、俯仰角和航向角而言，橫滾角和俯仰角與加速度計(jì)零偏有關(guān)，一般希望可以達(dá)到1 mrad（3.438'）或者更高的初始化精度，航向角則與陀螺零偏有關(guān)，且通常航向角誤差比橫滾角、俯仰角誤差大一個(gè)數(shù)量級。SINS 初始位置和速度信息需要外部信息源提供，GPS 導(dǎo)航系統(tǒng)可以為SINS 位置和速度誤差進(jìn)行初始化提供基準(zhǔn)。所以，設(shè)置SINS 初始對準(zhǔn)橫滾角誤差為0.5′，俯仰角誤差為0.5′，航向角誤差為20′，初始平臺對準(zhǔn)誤差為1 m、1 m、3 m，初始速度誤差為0.1 m/s、0.1 m/s、0.1 m/s。

為了驗(yàn)證算法的有效性，模擬GPS 定位子系統(tǒng)出現(xiàn)故障，人為在130～149 s 對GPS 東、北、天方向上的速度觀測量加入干擾，干擾值是正常觀測噪聲的8 倍。通過蒙特卡羅方法仿真2 500 次，并對不同的故障處理算法進(jìn)行對比。經(jīng)過實(shí)驗(yàn)驗(yàn)證，GPS-AOA-SINS 組合導(dǎo)航可以提高單獨(dú)GPS 系統(tǒng)的定位性能，改進(jìn)聯(lián)邦卡爾曼濾波器可以有效降低故障子濾波器的影響，通過合理利用異常觀測值中的有用信息，提高了融合定位系統(tǒng)的濾波精度，保證了組合導(dǎo)航系統(tǒng)的正常運(yùn)行。

在此基礎(chǔ)上，本文還對AOA 定位性能進(jìn)行了分析。在邊長為1 000 m 的空間內(nèi)設(shè)置了3～6 個(gè)基站模擬5G 宏基站部署，設(shè)置了不同基站空間分布密度模擬UDN 部署[26-27]。根據(jù)mMIMO 對天線數(shù)目的要求，設(shè)置了不同數(shù)量的天線分別為64、128和256 根，在視線線路情況下的角度測量誤差設(shè)置成均值為0、方差為0.8°，仿真實(shí)驗(yàn)驗(yàn)證5G mMIMO和UDN 技術(shù)對AOA 角度估計(jì)和定位精度的影響。

3.2 仿真結(jié)果分析

圖2 是不同天線數(shù)目下的角度估計(jì)誤差，天線數(shù)目分別為64、128 和256 根，基站數(shù)目為3。從圖2 可以看出，在信噪比一定、基站數(shù)目一定的情況下，天線陣列中的天線數(shù)目越多，對角度估計(jì)的誤差就越小，且角度估計(jì)誤差在0.10°以下。

圖2 不同天線數(shù)目下的角度估計(jì)誤差

圖3 給出了不同天線和基站數(shù)目下的定位均方根誤差（RMSE,root mean square error）對比。在邊長為1 000 m 的正方形空間中，基站數(shù)目從3 個(gè)增加到6 個(gè)，基站之間間距為200～500 m，符合5G 室外宏基站部署間距。橫向比較可以看出，隨著基站數(shù)目的增加，在相同天線數(shù)目的情況下，AOA 定位RMSE 值在不斷減小，這是因?yàn)殡S著基站的增多，發(fā)送端與接收端之間的距離減小，視線線路概率增大，角度誤差引起的位置誤差范圍也縮小了，定位結(jié)果更靠近實(shí)際結(jié)果。縱向比較可以看出，在基站數(shù)目相同的情況下，天線數(shù)目越多，AOA 定位誤差越小，大規(guī)模的天線讓子空間估計(jì)技術(shù)對角度的估計(jì)更加精確，從而使定位結(jié)果也更精確。

圖3 不同天線和基站數(shù)目下的定位RMSE 對比

圖4 是不同基站分布密度和天線數(shù)目下的定位RMSE 曲線。根據(jù)UDN 基站部署密度為 103cell/km2，在以1 000 m 為邊長的空間內(nèi)部署了1 000 個(gè)基站進(jìn)行定位仿真，此時(shí)相鄰基站間距縮短至十幾米至幾十米，定位時(shí)移動端固定與周圍距離最近的6 個(gè)基站相連接。同時(shí)，由于基站部署還需考慮其他屬性，實(shí)際室外基站部署密度基本不會這么密集，因此，對不同基站空間分布密度也進(jìn)行了仿真。仿真結(jié)果表明，基站空間分布密度越大，基站間距離縮短，視線線路概率越高，定位精度也越高，可以達(dá)到1 m?？v向?qū)Ρ瓤梢钥闯觯诨痉植济芏认嗤那闆r下，基站端天線數(shù)目越多，其定位精度也越高。仿真驗(yàn)證了5G mMIMO 和UDN 技術(shù)提升了基站AOA 定位的精度。

圖4 不同基站分布密度和天線數(shù)目下的定位RMSE 曲線

圖5 是單獨(dú)GPS 定位和AOA 定位的RMSE 曲線，其中，AOA 定位視線線路情況下的測量誤差符合均值為0、方差為0.8°的高斯分布。從圖5 可以看出，兩者定位RMSE 均值基本一致。

圖5 單獨(dú)GPS 定位和AOA 定位的RMSE 曲線

圖6 是單獨(dú)GPS 定位、GPS-SINS 組合定位和GPS-AOA-SINS 組合導(dǎo)航定位RMSE 誤差對比曲線。從圖6 中可以看出，單獨(dú)GPS 定位效果不佳，其定位RMSE 值上下波動起伏，說明單獨(dú)GPS 定位不穩(wěn)定，信號容易受到遮擋等不利因素影響，整體定位RMSE 均值在18 m 左右，與真實(shí)位置偏差較大，不能滿足實(shí)時(shí)定位的需求；GPS-AOA-SINS組合導(dǎo)航定位在開始時(shí)刻由于SINS 的初始對準(zhǔn)誤差較大，組合導(dǎo)航定位RMSE 值在3 m 左右。隨后RMSE 逐漸收斂，RMSE 均值穩(wěn)定在1.5 m 左右，定位曲線比較平滑，整體定位RMSE 值相比于單獨(dú)GPS 定位大大降低，可以滿足日常定位需求。對比GPS-SINS 和 GPS-AOA-SINS 可以看出，GPS-AOA-SINS組合導(dǎo)航定位RMSE誤差明顯小于GPS-SINS 組合定位RMSE 誤差，表明AOA 的輔助定位有效提高了組合導(dǎo)航的定位精度。以上實(shí)驗(yàn)結(jié)果證明，GPS-AOA-SINS 組合導(dǎo)航定位可以對單獨(dú)GPS 定位時(shí)定位性能不好的問題進(jìn)行改善，極大地提升了定位精度，展現(xiàn)出了一定的優(yōu)越性。

圖6 單獨(dú)GPS 定位、GPS-SINS 組合定位和GPS-AOA-SINS 組合導(dǎo)航定位RMSE 誤差對比曲線

圖7 和圖8 分別是無故障時(shí)組合導(dǎo)航系統(tǒng)定位誤差和速度誤差曲線。從圖7 和圖8 中可以看出，初始時(shí)刻的定位誤差經(jīng)緯度誤差在1 m，高程誤差在3 m，這是因?yàn)镾INS 一開始會有一個(gè)初始對準(zhǔn)誤差，隨著時(shí)間的增長，定位誤差逐漸收斂，緯度和經(jīng)度誤差大概穩(wěn)定在0.5 m 左右，高程誤差大概穩(wěn)定在0.25 m 左右；速度誤差同樣因?yàn)镾INS 的初始對準(zhǔn)誤差存在，剛開始在東、北、天方向上的誤差較大，均為0.1 m，前50 s 內(nèi)有一些小小的波動，之后曲線逐漸穩(wěn)定，穩(wěn)定后的東、北、天誤差分別為0.01 m/s、0.02 m/s、0.01 m/s 左右。實(shí)驗(yàn)結(jié)果證明，本文設(shè)計(jì)的組合導(dǎo)航系統(tǒng)可以改善單獨(dú)GPS定位的問題，其定位誤差均在米級以下，滿足定位需求。

圖7 無故障時(shí)組合導(dǎo)航系統(tǒng)定位誤差曲線

圖8 無故障時(shí)組合導(dǎo)航系統(tǒng)速度誤差曲線

圖9 和圖10 分別是有故障時(shí)定位誤差和速度誤差曲線。從圖9 和圖10 中可以看出，同樣由于SINS 初始對準(zhǔn)導(dǎo)致在初始時(shí)刻速度和緯度的誤差較大，并在50 s 后定位和速度誤差均值均逐漸趨向收斂。在130～149 s，定位和速度誤差均值出現(xiàn)明顯波動，說明此時(shí)融合濾波不穩(wěn)定，濾波結(jié)果出現(xiàn)異常，因?yàn)樵谶@個(gè)區(qū)間內(nèi)真實(shí)量測噪聲方差是初始量測噪聲方差的8 倍。由于誤差均值正負(fù)相抵，因此故障區(qū)間內(nèi)的誤差均值幅度不大。在149 s 故障解除以后，速度和定位誤差都逐漸收斂至正常水平，常規(guī)聯(lián)邦卡爾曼濾波對故障有一定的適應(yīng)能力。

圖9 有故障時(shí)組合導(dǎo)航系統(tǒng)定位誤差曲線

圖10 有故障時(shí)組合導(dǎo)航系統(tǒng)速度誤差曲線

圖11 和圖12 分別是改進(jìn)的聯(lián)邦卡爾曼濾波組合導(dǎo)航系統(tǒng)定位和速度誤差均值曲線。對比圖9 和圖10 可以看出，在130～149 s 的故障發(fā)生范圍內(nèi)，基于改進(jìn)的聯(lián)邦卡爾曼濾波組合導(dǎo)航系統(tǒng)定位和速度誤差均值均沒有出現(xiàn)明顯波動，說明故障檢測能有效檢測出子濾波器輸出狀態(tài)值異常，故障處理算法能有效降低異常值對融合濾波結(jié)果的影響，減小對其他子濾波器的污染。對比圖7 和圖8 可以看出，無故障時(shí)組合導(dǎo)航系統(tǒng)的定位和速度誤差曲線與改進(jìn)的聯(lián)邦卡爾曼濾波組合導(dǎo)航系統(tǒng)定位和速度誤差曲線基本相似，除在故障發(fā)生處的誤差均值有些許提高造成故障結(jié)束后產(chǎn)生一個(gè)越變外，整體誤差均值曲線平穩(wěn)，收斂快速。仿真表明，改進(jìn)的聯(lián)邦卡爾曼濾波可以增強(qiáng)GPS-AOA-SINS 組合導(dǎo)航系統(tǒng)的穩(wěn)定性和可靠性。

圖11 改進(jìn)的聯(lián)邦卡爾曼濾波組合導(dǎo)航系統(tǒng)定位誤差曲線

圖12 改進(jìn)的聯(lián)邦卡爾曼濾波組合導(dǎo)航系統(tǒng)速度誤差曲線

圖13 為有故障、無故障和改進(jìn)的聯(lián)邦卡爾曼濾波組合導(dǎo)航系統(tǒng)的定位RMSE 曲線。從圖13 中可以看出，無故障時(shí)的組合導(dǎo)航系統(tǒng)定位RMSE 在1.6 m 處收斂，故障發(fā)生導(dǎo)致組合導(dǎo)航系統(tǒng)的定位結(jié)果在130 s 處出現(xiàn)發(fā)散現(xiàn)象，RMSE 遠(yuǎn)遠(yuǎn)偏離正常值，在150 s 處發(fā)散至5 m 左右，故障結(jié)束后由于受故障信息的污染，融合濾波結(jié)果收斂緩慢。改進(jìn)的聯(lián)邦卡爾曼濾波將自適應(yīng)故障處理過的融合定位結(jié)果再次與故障子濾波器的時(shí)間更新值融合，大大降低了異常值引起的濾波發(fā)散，RMSE 最大值在1.8 m 左右，且在故障結(jié)束后，RMSE 快速收斂到正常定位精度，說明本文所提改進(jìn)的聯(lián)邦卡爾曼濾波極大地改善了組合導(dǎo)航系統(tǒng)的定位精度，提升了組合導(dǎo)航系統(tǒng)對突發(fā)故障的適應(yīng)能力，使系統(tǒng)在有故障的情況下仍然能以降低性能的方式正常工作。

圖13 有故障、無故障和改進(jìn)的聯(lián)邦卡爾曼濾波組合導(dǎo)航系統(tǒng)的定位RMSE 曲線

圖14 和圖15 分別是GPS 在不同遮擋程度下引起的故障大小和故障處理誤差結(jié)果。GPS 分別以1 倍、5 倍和10 倍的程度加上不同的噪聲，噪聲的統(tǒng)計(jì)分布是均值為0，方差分別為0.8、4和8。從圖14 中可以看出，隨著噪聲方差的增大，故障處由噪聲引起的定位RMSE 也增大。從圖15 中可以看出，經(jīng)過故障處理后的結(jié)果故障處的定位RMSE 大大減小了，對于不同程度的噪聲影響，故障處理后的RMSE 值相差不多，而且在故障收斂時(shí)，噪聲方差大的反而收斂的RMSE值更小。

圖14 GPS 在不同遮擋程度下的故障大小

圖15 GPS 在不同遮擋程度下的故障處理誤差結(jié)果

圖16 是改進(jìn)的聯(lián)邦卡爾曼濾波4 種不同故障處理方法的定位RMSE 對比。這4 種故障處理方法分別是隔離故障子濾波器（即故障子濾波器輸出值不參與主濾波器的融合）、采用故障子濾波器一階狀態(tài)預(yù)測值進(jìn)行融合、自適應(yīng)故障處理、自適應(yīng)故障處理與故障子濾波器時(shí)間更新值相結(jié)合。前2 種方法是其他論文中故障檢測與隔離的常用方法，后2 種方法是本文所提算法，由于第3 種算法在抑制故障處表現(xiàn)不佳，第4 種方法對其進(jìn)行了進(jìn)一步改進(jìn)。

圖16 改進(jìn)的聯(lián)邦卡爾曼濾波4 種不同故障處理方法的定位RMSE 對比

隔離故障子濾波器和利用一階狀態(tài)預(yù)測值的方法都沒有利用到異常量測值中的信息。隔離故障子濾波器需要利用其他子濾波器的狀態(tài)輸出估計(jì)值進(jìn)行融合，并對故障子濾波器進(jìn)行重構(gòu)，但由于AOA-SINS 子濾波器只能修正位置信息，因此在故障發(fā)生時(shí)也出現(xiàn)了發(fā)散現(xiàn)象；利用一階狀態(tài)預(yù)測值的方法受前一時(shí)刻狀態(tài)估計(jì)值的影響大，受故障的影響小，對故障的改善比較好；自適應(yīng)故障處理方法的RMSE 在前2 種方法之間但其收斂速度要遠(yuǎn)快于利用一階狀態(tài)預(yù)測值的方法。綜上所述，為了同時(shí)達(dá)到精度高和收斂速度快的要求，將自適應(yīng)故障處理后的結(jié)果進(jìn)一步與故障子濾波器時(shí)間更新結(jié)果融合。從圖16 中可以明顯看到，自適應(yīng)故障處理與故障子濾波器時(shí)間更新相結(jié)合的方法RMSE 值的均值最小，在150 s 處的RMSE 值最小，收斂速度也得到了明顯提高。實(shí)驗(yàn)結(jié)果表明，本文所提算法可以實(shí)時(shí)性地處理組合導(dǎo)航系統(tǒng)故障，有效補(bǔ)償了組合導(dǎo)航過程中的異常誤差，減少了故障時(shí)系統(tǒng)濾波精度損失，提高了組合導(dǎo)航定位系統(tǒng)的融合定位精度。

定量分析改進(jìn)的聯(lián)邦卡爾曼濾波4 種不同故障處理方法性能對比，結(jié)果如表1 所示。

表1 4 種不同故障處理方法性能對比

從表1 中可以看出，自適應(yīng)故障處理+故障子濾波器時(shí)間更新的方法RMSE 均值最小，RMSE 最大值也最小。發(fā)生故障時(shí)的組合導(dǎo)航系統(tǒng)RMSE 最大值是4.952，經(jīng)過故障處理后，RMSE 最大值分別減小了58.16%、61.53%、60.70%、61.89%，無故障時(shí)RMSE 均值為1.618 5，有故障時(shí)RMSE 均值為2.485 7，經(jīng)過處理后的RMSE 均值分別減小了31.47%、32.67%、32.70%、32.86%。通過定量分析進(jìn)一步表明，本文提出的改進(jìn)聯(lián)邦卡爾曼濾波算法在增強(qiáng)GPS-AOA-SINS 組合導(dǎo)航系統(tǒng)定位可靠性方面有一定的優(yōu)勢，可以提高故障發(fā)生時(shí)系統(tǒng)的濾波精度。

4 結(jié)束語

本文提出的容錯(cuò)GPS-AOA-SINS 組合導(dǎo)航系統(tǒng)可以有效補(bǔ)償GPS 在單獨(dú)定位時(shí)受遮擋造成定位不穩(wěn)定和無法提供載體姿態(tài)信息等缺點(diǎn)，極大地提高了定位精度，定位RMSE 可以達(dá)到1.6 m 左右。在三者的融合過程中，為了增加組合導(dǎo)航系統(tǒng)對突發(fā)故障的適應(yīng)能力，提高系統(tǒng)的可靠性，進(jìn)一步改進(jìn)了聯(lián)邦卡爾曼濾波器。改進(jìn)的聯(lián)邦卡爾曼濾波器增加了故障檢測和處理，通過自適應(yīng)調(diào)節(jié)濾波增益矩陣達(dá)到對量測值的合理利用，并將全局濾波結(jié)果進(jìn)一步與故障子濾波器時(shí)間更新值融合。仿真結(jié)果表明，當(dāng)速度真實(shí)量測噪聲方差是初始量測噪聲方差的8 倍，所提算法可以使融合定位RMSE 均值降低32.86%，顯著提高了融合濾波精度、可靠性和容錯(cuò)能力，實(shí)時(shí)的故障信息處理也提高了組合導(dǎo)航系統(tǒng)定位的連續(xù)性。