胡丹丹，顧 圓，高慶吉

（中國(guó)民航大學(xué)電子信息與自動(dòng)化學(xué)院，天津 300300）

精密進(jìn)近航道指示器（PAPI，precision approach path indicator）系統(tǒng)是為進(jìn)近著陸中的飛機(jī)提供正確下滑航道的助航燈光系統(tǒng)，能夠在飛機(jī)降落過程中為飛行員提供正確的下滑航道指示信息和不同的偏離指示信息。機(jī)場(chǎng)在燈具安裝設(shè)置完成后，會(huì)在首次投入使用之前和規(guī)定的維護(hù)周期間隔對(duì)PAPI 系統(tǒng)的仰角進(jìn)行飛行校驗(yàn)[1-3]。目前PAPI 系統(tǒng)的飛行校驗(yàn)都是由駕駛員駕駛滿足飛行校驗(yàn)要求的民航客機(jī)并攜帶相關(guān)設(shè)備完成[2]，采用真實(shí)飛機(jī)實(shí)施人工校飛的方法操作過程繁雜、牽扯部門多，存在校驗(yàn)成本較高、時(shí)間耗費(fèi)多等問題。因此，采用無人機(jī)替代飛機(jī)對(duì)PAPI 進(jìn)行自主校驗(yàn)逐漸成為一種趨勢(shì)。

機(jī)場(chǎng)設(shè)計(jì)手冊(cè)中規(guī)定應(yīng)糾正大于1′的PAPI 燈具仰角角度誤差[2]，通過校驗(yàn)原理及計(jì)算分析可知，無人機(jī)以距PAPI 一定水平距離定點(diǎn)水平校驗(yàn)時(shí)，其校驗(yàn)誤差等于高度測(cè)量誤差，因此，旋翼無人機(jī)高度測(cè)量精度是決定PAPI 角度校驗(yàn)誤差的關(guān)鍵。

胡永紅[4]提出采用卡爾曼濾波對(duì)氣壓高度計(jì)、全球定位系統(tǒng)（GPS）和線加速度信息進(jìn)行融合，并根據(jù)測(cè)量信息的冗余設(shè)計(jì)了系統(tǒng)的故障檢測(cè)和隔離算法，但仿真實(shí)驗(yàn)忽略了動(dòng)態(tài)情況下的誤差特性。Drak 等[5]提出一種基于卡方檢測(cè)的檢錯(cuò)隔離的高度信息融合測(cè)量方法，該方法直接以各傳感器融合高度狀態(tài)量作為參考量，對(duì)各傳感器的測(cè)量狀態(tài)量進(jìn)行故障檢錯(cuò)。

根據(jù)旋翼無人機(jī)校驗(yàn)PAPI 燈實(shí)際應(yīng)用需求，采用慣導(dǎo)在短時(shí)間內(nèi)二次積分量作為參考信號(hào)，通過檢錯(cuò)器進(jìn)行狀態(tài)卡方檢測(cè)與殘差卡方檢測(cè)，得到不同傳感器的故障系數(shù)及權(quán)重系數(shù)，將該有效高度數(shù)據(jù)融合得到精準(zhǔn)高度信息。最后，將該方法應(yīng)用于校驗(yàn)無人機(jī)飛行校驗(yàn)中并驗(yàn)證該方法的有效性和精確性。

1 容錯(cuò)卡爾曼濾波的設(shè)計(jì)

以卡爾曼濾波器輸出的估計(jì)誤差及高度誤差作為測(cè)量量，以慣導(dǎo)在短時(shí)間內(nèi)二次積分的估計(jì)誤差及高度誤差為檢錯(cuò)器參考量，通過檢錯(cuò)器對(duì)各傳感器測(cè)量信息進(jìn)行檢錯(cuò)隔離，如圖1所示。

圖1 容錯(cuò)卡爾曼濾波方法示意圖Fig.1 Schematic diagrams of fault-tolerant Kalman filtering method

1.1 KFT 濾波算法

文中所涉及每種卡爾曼濾波都是標(biāo)準(zhǔn)的，以卡爾曼濾波G 為例。卡爾曼濾波的狀態(tài)矢量可表示為

其中：φx、φy、φz為姿態(tài)誤差；δvx、δvy、δvz為速度誤差；δx、δy、δz為位置誤差；εx、εy、εz為加速度測(cè)量誤差；Δx、Δy、Δz為角度測(cè)量誤差。

假設(shè)k 次真實(shí)狀態(tài)由k-1 狀態(tài)演變而來，卡爾曼濾波器G 模型[6]描述如下

其中：Fg，k為k 時(shí)刻組合高度測(cè)量系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣Xg，k-1的狀態(tài)轉(zhuǎn)換模型；Wg，k為k 時(shí)刻系統(tǒng)噪聲。

卡爾曼濾波器G 的測(cè)量向量可表示為在k 時(shí)刻，真實(shí)狀態(tài)X 的觀察量可表示為

其中：H 為將真實(shí)狀態(tài)空間映射到觀察空間的觀測(cè)矩陣；V 為觀察噪聲，在卡爾曼濾波中，假定V 為期望為0、協(xié)方差為R 的高斯白噪聲。

卡爾曼濾波G 的方程[6]可表示為以下3 部分：

1）預(yù)測(cè)

2）更新

3）狀態(tài)遞推

其中：P 為狀態(tài)預(yù)測(cè)的誤差方陣；Q 為過程激勵(lì)噪聲協(xié)方差陣；R 為測(cè)量噪聲協(xié)方差陣；I 為單位矩陣；i 為狀態(tài)遞推器下標(biāo)，i=1，2。

1.2 容錯(cuò)KFT 濾波算法

測(cè)試檢查信號(hào)與參考信號(hào)間一致性的卡方測(cè)試，廣泛應(yīng)用于隨機(jī)動(dòng)態(tài)系統(tǒng)的故障檢測(cè)。根據(jù)測(cè)試統(tǒng)計(jì)使用方法的不同，卡方測(cè)試可分為3 類：殘差卡方檢測(cè)、具有一個(gè)狀態(tài)遞推器的狀態(tài)卡方檢測(cè)和具有兩個(gè)狀態(tài)遞推器的狀態(tài)卡方檢測(cè)。

由于卡爾曼濾波器的狀態(tài)向量都相同，只能采用帶有兩個(gè)狀態(tài)遞推器并行結(jié)構(gòu)的狀態(tài)卡方測(cè)試和殘差卡方測(cè)試的故障檢測(cè)器來增加運(yùn)算速度，如圖2所示。如果狀態(tài)卡方測(cè)試或殘差卡方測(cè)試檢測(cè)到故障，則mg=0，否則mg=1。檢驗(yàn)統(tǒng)計(jì)量服從卡方分布，故障檢測(cè)是

其中，εd的閾值由錯(cuò)誤警告率決定。

圖2 傳感器故障檢錯(cuò)器結(jié)構(gòu)示意圖Fig.2 Structure diagram of sensor fault detector

2 容錯(cuò)卡爾曼濾波的校驗(yàn)無人機(jī)測(cè)高法

校驗(yàn)無人機(jī)采用氣壓計(jì)、聲吶、GPS 和捷聯(lián)式慣導(dǎo)（IMU）同時(shí)進(jìn)行高度測(cè)量，信息高度融合控制回路結(jié)構(gòu)圖[7]如圖3所示。通過容錯(cuò)卡爾曼濾波器，得到各傳感器的可信度，再對(duì)可信度分配適當(dāng)權(quán)值進(jìn)行數(shù)據(jù)融合，最后獲得一個(gè)最優(yōu)高度信息。

圖3 高度數(shù)據(jù)融合控制回路結(jié)構(gòu)圖Fig.3 Structure diagram of height data fusion control loop

各傳感器的輸出權(quán)值可根據(jù)每個(gè)傳感器輸出高度誤差計(jì)算得出，如GPS 傳感器的權(quán)值計(jì)算如下

其中：σg為GPS 傳感器輸出誤差的方差；σb為氣壓計(jì)輸出誤差的方差；σr為聲吶傳感器輸出誤差的方差。而融合后的最佳高度為

其中：w 為各個(gè)傳感器測(cè)量數(shù)據(jù)的權(quán)值系數(shù)；m 為各個(gè)傳感器的故障系數(shù)，若某傳感器的某數(shù)值報(bào)錯(cuò)則該值為0，否則為1；hg、hr、hb為各傳感器的測(cè)量高度。高度測(cè)量方法如圖4所示，算法步驟如下。

步驟1計(jì)算高度狀態(tài)估計(jì)值與誤差值。采用卡爾曼濾波算法分別計(jì)算各傳感器輸出高度誤差的狀態(tài)預(yù)測(cè)值、估計(jì)值及不同高度的誤差值。

步驟2計(jì)算傳感器的故障系數(shù)。根據(jù)步驟1 得到的高度估計(jì)值與誤差值，采用卡方檢測(cè)原理，分別對(duì)不同傳感器高度值進(jìn)行殘差卡方測(cè)試與狀態(tài)卡方測(cè)試，得到不同傳感器的故障系數(shù)。

步驟3計(jì)算傳感器的權(quán)重系數(shù)。結(jié)合不同傳感器的輸出高度值，計(jì)算高度誤差方差，然后根據(jù)不同傳感器的誤差方差計(jì)算傳感器的權(quán)重值。

步驟4最優(yōu)化高度融合。結(jié)合步驟2、3 以及傳感器的輸出高度值進(jìn)行融合計(jì)算，得到容錯(cuò)卡爾曼濾波數(shù)據(jù)融合后的測(cè)試高度。

圖4 高度信息融合算法示意圖Fig.4 Flow chart of high-level information fusion method

3 實(shí)驗(yàn)驗(yàn)證

3.1 高度信息融合仿真

設(shè)初始狀態(tài)特征向量X=[0.02]，表示高度誤差，定高飛行各時(shí)刻狀態(tài)的高度誤差基本一致，因此令狀態(tài)轉(zhuǎn)移矩陣Fg=[1]，系統(tǒng)測(cè)量矩陣Hg=[1]，初始協(xié)方差P0=0.4，過程激勵(lì)噪聲協(xié)方差Q=0.000 1。

針對(duì)GPS 傳感器發(fā)生故障進(jìn)行仿真驗(yàn)證。仿真各傳感器信息如圖5（a）所示。根據(jù)權(quán)值計(jì)算公式計(jì)算各傳感器仿真數(shù)據(jù)權(quán)值，分別為wg=0.072 1，wb=0.902 1，wr=0.025 8，經(jīng)容錯(cuò)卡爾曼濾波數(shù)據(jù)融合后的仿真結(jié)果如圖5（b）所示。

圖5 測(cè)高系統(tǒng)仿真結(jié)果Fig.5 Simulation results chart of height measurement system

對(duì)比圖5（a）和圖5（b）中數(shù)據(jù)融合前后的高度值，經(jīng)過容錯(cuò)卡爾曼濾波算法處理融合后的輸出數(shù)據(jù)更接近精確測(cè)量值，且對(duì)中間GPS 傳感器的某一錯(cuò)誤數(shù)據(jù)能夠準(zhǔn)確定位并剔除，實(shí)現(xiàn)了系統(tǒng)的實(shí)時(shí)在線檢錯(cuò)與錯(cuò)誤隔離。

為進(jìn)一步驗(yàn)證該算法的融合精度，對(duì)GPS、氣壓計(jì)、聲吶及數(shù)據(jù)融合的4 組數(shù)據(jù)進(jìn)行誤差分析，誤差曲線如圖6所示，數(shù)據(jù)融合后的高度誤差值曲線如圖6（b）所示，GPS 傳感器局部測(cè)量誤差較大，但融合誤差在期望范圍內(nèi)。

圖6 高度誤差曲線Fig.6 Height error graph

為更直觀的觀測(cè)各組誤差信息，各傳感器的誤差均值與方差，如表1所示。從均值角度看，GPS 傳感器有兩個(gè)故障數(shù)據(jù)，該傳感器誤差均值較大即該傳感器測(cè)量數(shù)值波動(dòng)較大。從方差角度看，卡爾曼濾波數(shù)據(jù)融合后的誤差方差最小，測(cè)量波動(dòng)性更小。

表1 各傳感器誤差均值及方差Tab.1 Error mean and variance of each sensor

綜上所述，容錯(cuò)卡爾曼濾波數(shù)據(jù)融合結(jié)果比單獨(dú)一個(gè)或多個(gè)卡爾曼濾波融合得到的數(shù)據(jù)可靠性更高。

3.2 飛行試驗(yàn)及結(jié)果分析

基于自行設(shè)計(jì)的旋翼飛行器平臺(tái)進(jìn)行實(shí)驗(yàn)。場(chǎng)景如圖7所示。

圖7 旋翼校驗(yàn)無人機(jī)平臺(tái)Fig.7 Rotorcraft verification UAV platform

旋翼飛行器平臺(tái)上面搭載了慣性導(dǎo)航系統(tǒng)、mb1212 聲吶、mb1212 氣壓計(jì)及UBLOX GPS。使飛行器分別在1.2 m、1.8 m、2.6 m 和1.5 m 左右采用定高模式飛行。

在飛行過程中改變定高目標(biāo)值，實(shí)現(xiàn)平臺(tái)式定高試驗(yàn)，可看出多傳感器高度信息在容錯(cuò)卡爾曼濾波法融合后，高度信息更加平穩(wěn)，如圖8所示。

圖8 數(shù)據(jù)融合高度信息Fig.8 Data fusion height information

圖9采用容錯(cuò)卡爾曼濾波融合法融合定高飛行高度數(shù)據(jù)在第24 s 時(shí)給聲吶一個(gè)短暫的平臺(tái)干擾，可看出融合后的高度信息沒有發(fā)生大幅度突變。

圖9 旋翼無人機(jī)定高飛行Fig.9 Rotorcraft UAV set high

從圖10 中高度誤差曲線可知，融合算法計(jì)算后的高度值更接近于真實(shí)高度。此外對(duì)3 組數(shù)據(jù)的方差與均值進(jìn)行計(jì)算與分析，計(jì)算結(jié)果如表2所示。融合后的數(shù)據(jù)均值與方差最小，說明融合后的數(shù)據(jù)波動(dòng)性最小、精度高，得到的數(shù)據(jù)更穩(wěn)定，且高度誤差為0.1～0.5 m，可達(dá)到校驗(yàn)無人機(jī)飛行校驗(yàn)精度。

表2 各傳感器誤差均值及方差Tab.2 Error mean and variance of each sensor

圖10 高度誤差曲線Fig.10 Height error graph

4 結(jié)語

針對(duì)旋翼無人機(jī)在飛行校驗(yàn)過程中遇到各種干擾，使某個(gè)測(cè)高傳感器的數(shù)據(jù)發(fā)生突變的問題，提出了卡方波狀態(tài)檢錯(cuò)和卡爾曼濾波器結(jié)合，進(jìn)行在線故障檢測(cè)和錯(cuò)誤隔離，得到可信度后再對(duì)其進(jìn)行數(shù)據(jù)融合的高度測(cè)量方法。通過仿真與實(shí)驗(yàn)，驗(yàn)證了該方法的有效性。雖實(shí)驗(yàn)與仿真結(jié)果有一定誤差，但其測(cè)量精度與穩(wěn)定性可滿足校驗(yàn)需求。