胡 杰，嚴(yán)勇杰

(1. 中國電子科技集團(tuán)公司第二十八研究所，江蘇 南京 210007；2. 空中交通管理系統(tǒng)與技術(shù)國家重點(diǎn)實(shí)驗(yàn)室，江蘇 南京 210007)

1 引言

捷聯(lián)式慣性導(dǎo)航系統(tǒng)(Strapdown Inertial Navigation System， SINS)利用正交安裝的陀螺和加速度計(jì)敏感得到載體運(yùn)動角速率和比力信息實(shí)時計(jì)算載體導(dǎo)航參數(shù)，具有全自主、全天候、隱蔽性強(qiáng)等優(yōu)點(diǎn)，但是SINS中慣性測量單元(Inertial Measurement Unit， IMU)漂移會引起系統(tǒng)隨時間累積性誤差。目前提高SINS導(dǎo)航精度主要有2種途徑：一是提高IMU測量精度；二是采用系統(tǒng)級方法抑制IMU漂移引起的導(dǎo)航誤差。短期內(nèi)提高IMU精度難度較大且成本高，系統(tǒng)級補(bǔ)償方法具有成本低、見效快等優(yōu)點(diǎn)[1]。旋轉(zhuǎn)調(diào)制技術(shù)是一種常用的系統(tǒng)級誤差補(bǔ)償方法，自20世紀(jì)以來已經(jīng)成為國內(nèi)外高精度慣導(dǎo)領(lǐng)域主要研究方法之一[2-3]。國外旋轉(zhuǎn)式SINS已經(jīng)廣泛應(yīng)用于水面艦船、潛艇以及水下機(jī)器人，典型產(chǎn)品有Sperry公司研制的MK39系列單軸旋轉(zhuǎn)SINS、MK49系列和AN/WSN-7系列雙軸旋轉(zhuǎn)式SINS，均采用旋轉(zhuǎn)調(diào)制技術(shù)抑制慣性器件漂移引起的累積誤差[4-5]。自2002年起，我國相關(guān)研究機(jī)構(gòu)也開始了旋轉(zhuǎn)式SINS的研究工作，國防科技大學(xué)于2007年研制出我國第一套旋轉(zhuǎn)式SINS，在實(shí)驗(yàn)室靜態(tài)測試環(huán)境下，其位置誤差優(yōu)于1 n mile/24h[6]。目前，東南大學(xué)[7]、北京航空航天大學(xué)[8]、哈爾濱工程大學(xué)[9]等高校也積極開展旋轉(zhuǎn)式SINS理論與樣機(jī)研制工作，取得了較大的研究成果。

單軸旋轉(zhuǎn)SINS中，方位軸的周期性旋轉(zhuǎn)調(diào)制抵消了水平方向上的IMU誤差對導(dǎo)航精度影響，其軸向IMU誤差引起的位置累積誤差依然存在，如軸向陀螺常值漂移為0.01°/h，緯度為45°時，純慣性導(dǎo)航24h后位置誤差約為10n mile，是高精度長航時SINS主要誤差源之一，為此許多學(xué)者對如何精確標(biāo)校單軸旋轉(zhuǎn)SINS中軸向陀螺漂移進(jìn)行了研究。Sun Wei[10]針對傳統(tǒng)單軸旋轉(zhuǎn)方案中軸向陀螺常值漂移無法被調(diào)制問題，提出一種旋轉(zhuǎn)軸傾斜45°的單軸旋轉(zhuǎn)方案，并進(jìn)行了仿真和系統(tǒng)驗(yàn)證實(shí)驗(yàn)。于旭東[11]建立了軸向陀螺常值漂移與經(jīng)緯度誤差數(shù)學(xué)模型，并利用最小二乘法辨識軸向陀螺常值漂移。胡杰[12]在文獻(xiàn)[11]的研究基礎(chǔ)上，針對經(jīng)緯度誤差中的舒拉振蕩誤差對陀螺漂移辨識精度影響，提出利用水平阻尼網(wǎng)絡(luò)抑制舒拉振蕩誤差以精確估計(jì)軸向陀螺漂移。Gao Wei[13]、孫偉[14]建立了旋轉(zhuǎn)式SINS精對準(zhǔn)Kalman濾波狀態(tài)方程和量測方程，在初始對準(zhǔn)的同時對IMU誤差進(jìn)行估計(jì)，仿真結(jié)果顯示IMU水平誤差、“數(shù)學(xué)平臺”失準(zhǔn)角等誤差降低了軸向陀螺常值漂移估計(jì)精度。綜上文獻(xiàn)可知，目前最小二乘法和Kalman濾波法是單軸旋轉(zhuǎn)SINS中常用的兩種估計(jì)軸向陀螺常值漂移的方法，其中，最小二乘法耗時長且需要載體保持靜止?fàn)顟B(tài)，而Kalman濾波法中其它狀態(tài)誤差會影響軸向陀螺常值漂移估計(jì)精度，為此需要結(jié)合SINS導(dǎo)航誤差模型，分析誤差傳遞特性，研究一種適用于單軸旋轉(zhuǎn)SINS的軸向陀螺常值漂移自標(biāo)定方法。

本文首先對單軸旋轉(zhuǎn)SINS調(diào)制原理進(jìn)行了分析，并給出了導(dǎo)航誤差模型，然后開展在線自標(biāo)定數(shù)學(xué)模型研究，給出了在線自標(biāo)定Kalman濾波狀態(tài)方程和量測方程，最后針對單軸旋轉(zhuǎn)SINS中軸向陀螺誤差傳遞特性，提出利用兩級Kalman濾波器實(shí)現(xiàn)IMU誤差的精確估計(jì)與補(bǔ)償。進(jìn)行了計(jì)算機(jī)仿真和實(shí)際系統(tǒng)驗(yàn)證實(shí)驗(yàn)，結(jié)果表明本文所提出方法的正確性，能夠?yàn)橹械染葐屋S旋轉(zhuǎn)SINS的研制提供理論參考。

2 單軸旋轉(zhuǎn)SINS調(diào)制原理與誤差模型

2.1 旋轉(zhuǎn)調(diào)制原理

傳統(tǒng)SINS中，IMU安裝在固定支架上并且與載體固連，IMU誤差會引起位置隨時間積累性誤差，無法滿足高精度導(dǎo)航需求。單軸旋轉(zhuǎn)SINS中，IMU安裝在能夠繞軸向旋轉(zhuǎn)的內(nèi)部平臺上，使得IMU(s系，oxsyszs)相對載體坐標(biāo)系(b系，oxbybzb)旋轉(zhuǎn)，從而水平方向上IMU誤差呈周期性變化，通過積分予以抵消。

假設(shè)初始時刻s系與b系重合，系統(tǒng)通電后s系以角速度Ω繞方位軸旋轉(zhuǎn)，則在任意t時刻s系相對b系的夾角為Ωt，其方向余弦矩陣可表示為

(1)

(2)

(3)

陀螺和加速計(jì)的測量值為

(4)

由式(2)、式(3)和式(4)可以得

(5)

由式(5)可以看出，在單軸旋轉(zhuǎn)SINS中，當(dāng)IMU繞方位軸旋轉(zhuǎn)時， IMU水平誤差在b系中呈周期性變化，通過積分可以抵消，IMU軸向誤差依然保持不變，因此對于單軸旋轉(zhuǎn)SINS而言，有必要采取其它措施對IMU軸向誤差進(jìn)行估計(jì)與補(bǔ)償，以進(jìn)一步提高SINS導(dǎo)航精度。

2.2 導(dǎo)航誤差模型

旋轉(zhuǎn)調(diào)制技術(shù)的本質(zhì)是通過控制IMU做周期性的轉(zhuǎn)動以改變SINS姿態(tài)矩陣，使得短時間內(nèi)IMU誤差對導(dǎo)航系統(tǒng)的積分等于零。旋轉(zhuǎn)式SINS導(dǎo)航算法與非旋轉(zhuǎn)時具有一致性，因此，可得旋轉(zhuǎn)式SINS導(dǎo)航誤差方程如下所示。

旋轉(zhuǎn)式SINS姿態(tài)誤差方程為

(6)

旋轉(zhuǎn)式SINS速度誤差方程為

(7)

旋轉(zhuǎn)SINS位置誤差方程為

(8)

式中，δL、δλ和δh分別表示導(dǎo)航系統(tǒng)計(jì)算得載體緯度誤差、經(jīng)度誤差和高度誤差，δvE、δvN、δvU分別東向、北向和天向速度誤差，RE和RN分別表示地球子午圈和卯酉圈的曲率半徑。

3 在線自標(biāo)定數(shù)學(xué)模型

(9)

式中，A為系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣，W為系統(tǒng)過程噪聲矢量，X為系統(tǒng)誤差狀態(tài)矢量，其表達(dá)式如式(5)所示

(10)

式中，?x、?y為沿著旋轉(zhuǎn)系水平方向上加速度計(jì)常值偏置，εx、εy、εz為沿著旋轉(zhuǎn)系三個方向陀螺常值漂移，δKgx、δKgy、δKgz為沿著旋轉(zhuǎn)系三個方向陀螺標(biāo)度因數(shù)誤差。根據(jù)旋轉(zhuǎn)式SINS誤差方程式(6)～(8)可以得到精對準(zhǔn)狀態(tài)轉(zhuǎn)移矩陣A為

(11)

式中

選取位置誤差δL、δλ和水平速度誤差δvE、δvN為觀測值，量測方程為

Z=HX+V

(12)

式中，Z為觀測向量，Z=[δLδλδvEδvN]T，V為系統(tǒng)量測噪聲矢量，H為量測矩陣，其表達(dá)式為

(13)

4 兩級Kalman濾波算法流程

單軸旋轉(zhuǎn)SINS中水平方向IMU誤差可觀測度高，而軸向IMU誤差可觀測度相對較低，為了提高軸向IMU誤差標(biāo)定精度，需要對SINS誤差傳遞形式進(jìn)行分析，圖1為與軸向陀螺常值漂移估計(jì)相關(guān)的主要誤差傳遞過程。

圖1 軸向陀螺常值漂移傳遞形式

圖1中，εE、εU分別表示等效東向、軸向陀螺常值漂移，δvN、δL分別表示SINS北向速度誤差和緯度誤差，φx、φz分別表示SINS“數(shù)學(xué)平臺”東向和天向失準(zhǔn)角。由圖1可以看出，當(dāng)Kalman濾波器利用速度或者位置誤差作為觀測量進(jìn)行狀態(tài)最優(yōu)估計(jì)時，等效東向陀螺常值漂移、方位失準(zhǔn)角、等效北向加速度計(jì)偏置等都會影響濾波器軸向陀螺常值漂移估計(jì)精度，為此需要設(shè)計(jì)一個合理的自標(biāo)定流程，以最大限度地減小這些誤差因素的影響。

針對單軸旋轉(zhuǎn)SINS中軸向陀螺常值漂移傳遞特性，為了實(shí)現(xiàn)其精確估計(jì)，設(shè)計(jì)了兩級Kalman濾波器，并在第二級Kalman濾波器中完成軸向陀螺常值漂移和標(biāo)度因素誤差的精度估計(jì)，其自標(biāo)定流程圖如圖2所示。

圖2 基于兩級Kalman濾波的自標(biāo)定流程圖

由圖2可以看出，首先，第一級Kalman濾波器利用速度和位置基準(zhǔn)信息估計(jì)數(shù)學(xué)平臺“失準(zhǔn)角”以及水平方向陀螺常值漂移和加速度計(jì)常值偏置，并進(jìn)行補(bǔ)償后作為第二級Kalman濾波器的輸入，然后，進(jìn)一步利用基準(zhǔn)速度和位置信息對軸向陀螺常值漂移以及陀螺標(biāo)度因數(shù)誤差進(jìn)行在線估計(jì)，從而完成系統(tǒng)自標(biāo)定工作并轉(zhuǎn)入導(dǎo)航工作模式。

5 仿真與分析

5.1 仿真參數(shù)設(shè)置

假設(shè)陀螺常值漂移為0.01°/h，角度隨機(jī)游走系數(shù)為0.005°/h1/2，標(biāo)度因數(shù)誤差為10ppm，安裝誤差均為5″；加速度計(jì)常值偏置為100μg，隨機(jī)游走系數(shù)為80μg·s1/2，標(biāo)度因數(shù)誤差為10ppm，安裝誤差均為5″。初始經(jīng)度106.69°，緯度26.51°；仿真系統(tǒng)模擬搖擺運(yùn)動，姿態(tài)角變化規(guī)律分別為：俯仰角θ=5°sin(2πt/5)，橫搖角γ=2°sin(2πt/1.25)，航向角ψ=5°sin(2πt/5)，假設(shè)載體處于系泊狀態(tài)，線速度為0。

仿真中轉(zhuǎn)位機(jī)構(gòu)采用四位置正反轉(zhuǎn)停旋轉(zhuǎn)方案，如圖3所示。旋轉(zhuǎn)機(jī)構(gòu)角加速度為8°/s2，當(dāng)角速度達(dá)到16°/s時保持轉(zhuǎn)位機(jī)構(gòu)勻速旋轉(zhuǎn)運(yùn)動，并在到達(dá)指定位置后以-8°/s2的加速度減速，直至最終角速度為0，其測角裝置誤差假設(shè)為5″。

圖3 四位置旋轉(zhuǎn)方案

轉(zhuǎn)位1 IMU由A點(diǎn)逆時針轉(zhuǎn)動180°到達(dá)C點(diǎn)，然后停止5min；

轉(zhuǎn)位2 IMU由C點(diǎn)逆時針旋轉(zhuǎn)90°到達(dá)D點(diǎn)，然后停止5min；

轉(zhuǎn)位3 IMU由D點(diǎn)順時針旋轉(zhuǎn)180°到達(dá)B點(diǎn)，然后停止5min；

轉(zhuǎn)位4 IMU由B點(diǎn)順時針旋轉(zhuǎn)90°到達(dá)A點(diǎn)，然后停止5min；

然后以此順序進(jìn)行循環(huán)運(yùn)動。

5.2 實(shí)驗(yàn)結(jié)果

單軸旋轉(zhuǎn)SINS啟動上電后即進(jìn)行粗對準(zhǔn)，粗對準(zhǔn)時間為3min，假設(shè)系統(tǒng)粗對準(zhǔn)后姿態(tài)角誤差為Δψ=0.5°，Δθ=Δγ=0.1°，隨后進(jìn)行一級Kalman濾波估計(jì)，經(jīng)過15min后可以得到較為精確的數(shù)學(xué)平臺“失準(zhǔn)角”與IMU水平方向誤差并進(jìn)行補(bǔ)償，隨后進(jìn)行第二級Kalman濾波，對軸向陀螺常值漂移和陀螺標(biāo)度因數(shù)誤差進(jìn)行估計(jì)，持續(xù)時間為2h，標(biāo)定結(jié)果如圖4～圖8所示，標(biāo)定結(jié)果統(tǒng)計(jì)如表1～表4所示。

圖4 “數(shù)學(xué)平臺”失準(zhǔn)角估計(jì)誤差曲線

圖5 水平方向加速度計(jì)常值偏置估計(jì)曲線

圖6 水平方向陀螺常值漂移估計(jì)曲線

圖7 軸向陀螺常值漂移估計(jì)曲線

圖8 陀螺標(biāo)度因數(shù)誤差誤差估計(jì)曲線

在單軸旋轉(zhuǎn)SINS中，周期性轉(zhuǎn)動IMU抵消了水平方向常值漂移誤差對失準(zhǔn)角估計(jì)精度影響，由圖4和表1可以看出，三個方向失準(zhǔn)角估計(jì)收斂精度高，特別是方位失準(zhǔn)角收斂誤差為0.0021′，滿足高精度SINS對初始對準(zhǔn)精度需求。同時，由圖5、圖6以及表2、表3可以看出，水平方向IMU誤差經(jīng)過15min濾波估計(jì)后，最終能夠收斂至真值附近。

表1 失準(zhǔn)角估計(jì)誤差

表2 加速度計(jì)常值偏置估計(jì)值

表3 陀螺常值漂移估計(jì)值

第一級Kalman濾波器完成初始失準(zhǔn)角以及水平方向IMU誤差估計(jì)后轉(zhuǎn)至執(zhí)行第二級Kalman濾波，開始對軸向陀螺常值漂移以及陀螺標(biāo)度因數(shù)誤差進(jìn)行估計(jì)，由于軸向陀螺常值漂移和陀螺標(biāo)度因數(shù)誤差可觀測度相對較低，因此，其估計(jì)收斂時間較長，由圖7、圖8以及表3、表4可以看出，經(jīng)過2h在線估計(jì)后，軸向陀螺常值漂移及其標(biāo)度因數(shù)誤差得以精確估計(jì)，實(shí)現(xiàn)了單軸旋轉(zhuǎn)SINS在線自標(biāo)定目的。

完成兩級Kalman濾波后進(jìn)行24h純慣性導(dǎo)航仿真，對比有無標(biāo)校實(shí)驗(yàn)前后SINS長時間導(dǎo)航精度，結(jié)果如圖9所示。

圖9 有無在線自標(biāo)定SINS位置誤差對比

表5 不同實(shí)驗(yàn)方案下的定位誤差

由圖9和表5可以看出，如果不進(jìn)行IMU常值漂移誤差系統(tǒng)標(biāo)校，捷聯(lián)慣導(dǎo)系統(tǒng)24h定位誤差為6.71n mile，通過在線系統(tǒng)級標(biāo)校，主要是完成轉(zhuǎn)軸方向上陀螺漂移誤差的估計(jì)與補(bǔ)償，其后長時間定位僅有1.96n mile，相比無系統(tǒng)級標(biāo)校，其定位精度得到提高。

6 系統(tǒng)驗(yàn)證實(shí)驗(yàn)

利用實(shí)驗(yàn)室研制的單軸旋轉(zhuǎn)SINS進(jìn)行初始對準(zhǔn)驗(yàn)證實(shí)驗(yàn)，其中，IMU由3個光纖陀螺和3個石英撓性加速度計(jì)組成，其主要性能指標(biāo)如表6所示。

表6 單軸旋轉(zhuǎn)SINS主要性能指標(biāo)

單軸旋轉(zhuǎn)SINS以及三軸轉(zhuǎn)臺如圖10所示，將SINS放置在三軸轉(zhuǎn)臺上，控制轉(zhuǎn)臺做三軸搖擺運(yùn)動，為了模擬水中兵器受海浪影響而產(chǎn)生的角運(yùn)動，設(shè)置俯仰角搖擺幅度為5°，周期為5s，橫搖角搖擺幅度為2°，周期為1.25s，航向角搖擺幅度為5°，周期為5s，Kalman濾波器更新周期為1s。

圖10 實(shí)驗(yàn)設(shè)備及環(huán)境

采用一組轉(zhuǎn)臺IMU實(shí)測數(shù)據(jù)進(jìn)行半物理驗(yàn)證實(shí)驗(yàn)，圖11為有無在線自標(biāo)定過程時SINS位置誤差對比曲線，圖中藍(lán)色曲線為無在線自標(biāo)定過程時SINS導(dǎo)航12h時位置誤差曲線，紅色點(diǎn)畫線為有在線自標(biāo)定過程時SINS導(dǎo)航12h位置誤差曲線。

圖11 有無在線自標(biāo)定SINS位置誤差對比

表7 不同實(shí)驗(yàn)方案下的定位誤差

由圖11和表7可以看出，實(shí)際導(dǎo)航系統(tǒng)中，對IMU誤差在線自標(biāo)定提高了SINS長時間導(dǎo)航定位精度，其12h位置誤差由5.14n mile減小為2.13n mile，具有工程應(yīng)用價值。

7 結(jié)束語

本文對單軸旋轉(zhuǎn)SINS軸向陀螺漂移在線自標(biāo)定方法進(jìn)行了研究。首先分析了旋轉(zhuǎn)調(diào)制原理，并給出了在線自標(biāo)定Kalman濾波狀態(tài)方程和量測方程。然后對軸向陀螺漂移誤差傳遞路徑進(jìn)行了分析，指出等效東向陀螺漂移、“數(shù)學(xué)平臺”失準(zhǔn)角、等效北向加速度計(jì)偏置等會影響軸向陀螺常值漂移估計(jì)精度，為此提出了一種基于兩級Kalman濾波的單軸旋轉(zhuǎn)SINS軸向陀螺漂移自標(biāo)校方法。最后進(jìn)行了仿真和實(shí)際系統(tǒng)驗(yàn)證實(shí)驗(yàn)，可以看出，第二級Kalman濾波器能夠較好地實(shí)現(xiàn)軸向陀螺常值漂移和標(biāo)度因數(shù)誤差的精確估計(jì)，同時轉(zhuǎn)臺驗(yàn)證實(shí)驗(yàn)中，單軸旋轉(zhuǎn)SINS經(jīng)過在線自標(biāo)定與補(bǔ)償后，其位置誤差減小了58.6%，由此驗(yàn)證了本文所提出方法的正確性。