尹洪亮，張 嶸，郝 強(qiáng)，于東康

(1.清華大學(xué)精密儀器系，北京 100084；2.中國艦船研究院，北京 100192；3.哈爾濱工業(yè)大學(xué)儀器科學(xué)與工程學(xué)院，哈爾濱 150001)

0 引 言

由于慣性導(dǎo)航系統(tǒng)（Inertal Navigation System,INS）與全球定位系統(tǒng)（Global Positioning System，GPS）組合可以提供較高精度的速度、位置信息，所以INS/GPS 組合系統(tǒng)被廣泛應(yīng)用在船舶、飛機(jī)、導(dǎo)彈等領(lǐng)域。但因INS/GPS 組合系統(tǒng)量測(cè)信息僅包括位置和速度信息，缺少姿態(tài)信息觀測(cè)量，導(dǎo)致系統(tǒng)的可觀測(cè)性較差，容易造成系統(tǒng)在長(zhǎng)時(shí)間導(dǎo)航應(yīng)用中精度不高，甚至姿態(tài)信息發(fā)散的問題[1-3]。

天文導(dǎo)航系統(tǒng)（Celestial Navigation System，CNS）采用星體敏感器測(cè)量天體方位信息，在經(jīng)過解算后輸出載體姿態(tài)信息，其特點(diǎn)是姿態(tài)測(cè)量精度高、測(cè)量誤差誤差不隨時(shí)間積累、系統(tǒng)可靠性高，是一種新型的高精度姿態(tài)測(cè)量系統(tǒng)[4-6]。

為增加INS/GPS 組合系統(tǒng)的量測(cè)信息，提高系統(tǒng)導(dǎo)航精度，改善系統(tǒng)航向誤差發(fā)散的問題，將CNS 與傳統(tǒng)INS/GPS 系統(tǒng)組合是目前的研究熱點(diǎn)。張科設(shè)計(jì)了基于聯(lián)邦濾波的INS/GPS/CNS 組合導(dǎo)航算法，但由于其采用的是線性卡爾曼濾波框架，不能很好適應(yīng)INS/GPS/CNS 這類典型非線性系統(tǒng)，造成濾波結(jié)果精度較低；吳坤峰考慮到CNS 可能存在輸出不連續(xù)的問題，利用GPS 和CNS 輸出的高精度導(dǎo)航信息，分別設(shè)計(jì)了GPS/INS 和CNS/INS 濾波方案，在獲得濾波后的導(dǎo)航信息后，將其作為全信息觀測(cè)量進(jìn)行二次濾波，最終獲得高精度的導(dǎo)航信息。該方法計(jì)算較為繁瑣，且二次濾波會(huì)造成系統(tǒng)實(shí)時(shí)性變差的問題，不宜開展算法的工程化應(yīng)用[7-8]。

針對(duì)上述問題，本文提出一種基于擴(kuò)展卡爾曼濾波（ Extended Kalman filter， EKF） 的INS/GPS/CNS 全信息導(dǎo)航濾波算法，利用GPS 和CNS 輸出的高精度導(dǎo)航信息，結(jié)合非線性擴(kuò)展卡爾曼濾波框架，設(shè)計(jì)全觀測(cè)量的濾波算法，通過研制的INS/GPS/CNS 三組合導(dǎo)航系統(tǒng)進(jìn)行試驗(yàn)驗(yàn)證，驗(yàn)證了所提算法的高精度和長(zhǎng)時(shí)間工作條件下的穩(wěn)定性。

1 INS/GPS/CNS 濾波模型

根據(jù)INS，GPS 和CNS 的系統(tǒng)特點(diǎn)，建立INS/GPS/CNS 系統(tǒng)的數(shù)學(xué)模型。

1.1 狀態(tài)方程模型

根據(jù)文獻(xiàn)[9]，建立INS 數(shù)學(xué)模型如下：

1）速度誤差模型

式中： i 為慣性坐標(biāo)系； e 為地球坐標(biāo)系； fn為加速度計(jì)測(cè)得的比力在導(dǎo)航系中的投影；為東、北、天向的速度誤差；，為東、北、天向的平臺(tái)失準(zhǔn)角； ?n為加速度計(jì)零偏在導(dǎo)航坐標(biāo)系中的投影； ωen為牽連角速度； ωie為地球自轉(zhuǎn)角速度，根據(jù)INS 系統(tǒng)特點(diǎn)，有

其中： L 為當(dāng)?shù)氐乩砭暥龋?h為系統(tǒng)重心距地面高度，R為地球半徑。

2）平臺(tái)失準(zhǔn)角誤差方程

式中： εn為陀螺常值漂移在導(dǎo)航坐標(biāo)系中的投影；

3）位置誤差方程

式中：

基于上述分析，選用INS 模型作為INS/CNS/GPS系統(tǒng)濾波的基礎(chǔ)框架，可獲得狀態(tài)模型如下：

其中： A為 狀態(tài)矩陣； B為系統(tǒng)噪聲矩陣； W 為系統(tǒng)噪聲，具體表達(dá)式可參見文獻(xiàn)[9]； X為系統(tǒng)狀態(tài)參量，表達(dá)式如下：

1.2 量測(cè)方程模型

1）INS/GPS 量測(cè)方程

由于GPS 可提供高精度的位置和速度信息，所以取I N S 和G P S 輸出的位置和速度信息之差作為INS/GPS 系統(tǒng)的觀測(cè)量，定義量測(cè)方程為：

2）INS/CNS 量測(cè)方程

由于INS 輸出為機(jī)體系下姿態(tài)（ ΦIb），CNS 為慣性系下姿態(tài)（ ΦCb），所以需將兩坐標(biāo)系轉(zhuǎn)化后構(gòu)建量測(cè)方程。將姿態(tài)信息轉(zhuǎn)化到導(dǎo)航系下：

獲得量測(cè)方程如下：

3）INS/GPS/CNS 量測(cè)方程

基于上述量測(cè)方程，構(gòu)建INS/GPS/CNS 全信息觀測(cè)量方程，如下：

2 基于EKF 的INS/GPS/CNS 濾波算法

將式（ 8） 和式（ 13） 離散化獲得系統(tǒng)離散化模型：

根據(jù)上述離散化模型建立擴(kuò)展卡爾曼濾波方程如下：

其中： Fk為狀態(tài)矩陣 Ak的雅克比行列式， Pk， Qk分別為系統(tǒng)噪聲和量測(cè)噪聲協(xié)方差矩陣。

在初始狀態(tài)向量 X0和 初始系統(tǒng)噪聲 P0已知時(shí)，再根據(jù)k 時(shí)刻的量測(cè)信息 Zk，即可計(jì)算得到k 時(shí)刻狀態(tài)向量的最優(yōu)估計(jì)，獲得高精度穩(wěn)定的導(dǎo)航信息輸出。

3 試驗(yàn)結(jié)果與分析

為驗(yàn)證本文提出的基于EKF 全信息導(dǎo)航算法的有效性，搭建INS/GPS/CNS 三組合導(dǎo)航系統(tǒng)進(jìn)行精度驗(yàn)證，系統(tǒng)由激光慣導(dǎo)（陀螺漂移 0.001°/h）、GPS 接收機(jī)（定位誤差100 m）和星敏感器組成，為便于精度驗(yàn)證，采用高精度轉(zhuǎn)臺(tái)作為姿態(tài)基準(zhǔn)進(jìn)行搖擺試驗(yàn)測(cè)試，速度、位置基準(zhǔn)分別為0 m/s 和0 m。為驗(yàn)證系統(tǒng)在長(zhǎng)航時(shí)下的精度穩(wěn)定性，試驗(yàn)時(shí)間設(shè)為24 h。采用傳統(tǒng)兩組合濾波算法作為對(duì)比，為更直觀體現(xiàn)算法穩(wěn)定性，在進(jìn)行統(tǒng)計(jì)時(shí)，以初始時(shí)刻的導(dǎo)航誤差 e0為基準(zhǔn)，計(jì)算后續(xù)誤差 ek相 對(duì)于 e0的穩(wěn)定性A，如下式：

試驗(yàn)結(jié)果如圖1、圖2 及圖3 所示。

由圖1 可知，2 種濾波算法的處理結(jié)果均收斂，但從數(shù)據(jù)波動(dòng)程度來看，兩組合算法波動(dòng)較大，且根據(jù)穩(wěn)定性比值峰峰值，三組合算法峰峰值較小，證明算法穩(wěn)定性較好。由圖2 東向速度試驗(yàn)結(jié)果，三組合算法誤差收斂很快，誤差遠(yuǎn)小于兩組合結(jié)果，證明三組合算法精度較高。由圖3 姿態(tài)誤差試驗(yàn)結(jié)果可知，在長(zhǎng)航時(shí)條件下，姿態(tài)信息有不同程度的發(fā)散，這是由于INS 等效東向陀螺漂移的長(zhǎng)期穩(wěn)定性很難保證所致，在增加了CNS 的姿態(tài)信息作為觀測(cè)量后，濾波效果得到明顯改善，姿態(tài)信息收斂且保持穩(wěn)定。所以，相較于傳統(tǒng)的INS/GPS 兩組合擴(kuò)展卡爾曼濾波算法，INS/GPS/CNS 三組合擴(kuò)展卡爾曼濾波算法結(jié)果精度更高，長(zhǎng)航時(shí)工作條件下穩(wěn)定性更好。

圖 1 位置誤差試驗(yàn)結(jié)果對(duì)比圖Fig.1 Comparison of position error test results

圖 2 速度誤差試驗(yàn)結(jié)果對(duì)比圖Fig.2 Comparison of speed error test results

圖 3 姿態(tài)誤差試驗(yàn)結(jié)果對(duì)比圖Fig.3 Comparison of attitude error test results

4 結(jié) 語

本文針對(duì)INS/GPS 兩組合系統(tǒng)長(zhǎng)航時(shí)條件下姿態(tài)精度發(fā)散、導(dǎo)航精度較低、系統(tǒng)穩(wěn)定性較差的問題，引入CNS 系統(tǒng)的姿態(tài)信息，構(gòu)建一種基于INS/GPS/CNS 的全信息導(dǎo)航濾波模型，提出一種基于EKF 的INS/GPS/CNS 濾波算法。搭建INS/GPS/CNS 三組合系統(tǒng)對(duì)算法有效性進(jìn)行驗(yàn)證，試驗(yàn)結(jié)果表明，相較于傳統(tǒng)INS/GPS 兩組合算法，所提算法在長(zhǎng)航時(shí)條件下穩(wěn)定性強(qiáng)、收斂性好且精度高，工程應(yīng)用意義較大。