基于TLFK的單軸旋轉(zhuǎn)SINS在線自標(biāo)定方法

胡 杰，嚴(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)動(dòng)角速率和比力信息實(shí)時(shí)計(jì)算載體導(dǎo)航參數(shù)，具有全自主、全天候、隱蔽性強(qiáng)等優(yōu)點(diǎn)，但是SINS中慣性測(cè)量單元(Inertial Measurement Unit， IMU)漂移會(huì)引起系統(tǒng)隨時(shí)間累積性誤差。目前提高SINS導(dǎo)航精度主要有2種途徑：一是提高IMU測(cè)量精度；二是采用系統(tǒng)級(jí)方法抑制IMU漂移引起的導(dǎo)航誤差。短期內(nèi)提高IMU精度難度較大且成本高，系統(tǒng)級(jí)補(bǔ)償方法具有成本低、見效快等優(yōu)點(diǎn)[1]。旋轉(zhuǎn)調(diào)制技術(shù)是一種常用的系統(tǒng)級(jí)誤差補(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)測(cè)試環(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誤差對(duì)導(dǎo)航精度影響，其軸向IMU誤差引起的位置累積誤差依然存在，如軸向陀螺常值漂移為0.01°/h，緯度為45°時(shí)，純慣性導(dǎo)航24h后位置誤差約為10n mile，是高精度長航時(shí)SINS主要誤差源之一，為此許多學(xué)者對(duì)如何精確標(biāo)校單軸旋轉(zhuǎn)SINS中軸向陀螺漂移進(jìn)行了研究。Sun Wei[10]針對(duì)傳統(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é)模型，并利用最小二乘法辨識(shí)軸向陀螺常值漂移。胡杰[12]在文獻(xiàn)[11]的研究基礎(chǔ)上，針對(duì)經(jīng)緯度誤差中的舒拉振蕩誤差對(duì)陀螺漂移辨識(shí)精度影響，提出利用水平阻尼網(wǎng)絡(luò)抑制舒拉振蕩誤差以精確估計(jì)軸向陀螺漂移。Gao Wei[13]、孫偉[14]建立了旋轉(zhuǎn)式SINS精對(duì)準(zhǔn)Kalman濾波狀態(tài)方程和量測(cè)方程，在初始對(duì)準(zhǔn)的同時(shí)對(duì)IMU誤差進(jìn)行估計(jì)，仿真結(jié)果顯示IMU水平誤差、“數(shù)學(xué)平臺(tái)”失準(zhǔn)角等誤差降低了軸向陀螺常值漂移估計(jì)精度。綜上文獻(xiàn)可知，目前最小二乘法和Kalman濾波法是單軸旋轉(zhuǎn)SINS中常用的兩種估計(jì)軸向陀螺常值漂移的方法，其中，最小二乘法耗時(shí)長且需要載體保持靜止?fàn)顟B(tài)，而Kalman濾波法中其它狀態(tài)誤差會(huì)影響軸向陀螺常值漂移估計(jì)精度，為此需要結(jié)合SINS導(dǎo)航誤差模型，分析誤差傳遞特性，研究一種適用于單軸旋轉(zhuǎn)SINS的軸向陀螺常值漂移自標(biāo)定方法。

本文首先對(duì)單軸旋轉(zhuǎn)SINS調(diào)制原理進(jìn)行了分析，并給出了導(dǎo)航誤差模型，然后開展在線自標(biāo)定數(shù)學(xué)模型研究，給出了在線自標(biāo)定Kalman濾波狀態(tài)方程和量測(cè)方程，最后針對(duì)單軸旋轉(zhuǎn)SINS中軸向陀螺誤差傳遞特性，提出利用兩級(jí)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誤差會(huì)引起位置隨時(shí)間積累性誤差，無法滿足高精度導(dǎo)航需求。單軸旋轉(zhuǎn)SINS中，IMU安裝在能夠繞軸向旋轉(zhuǎn)的內(nèi)部平臺(tái)上，使得IMU(s系，oxsyszs)相對(duì)載體坐標(biāo)系(b系，oxbybzb)旋轉(zhuǎn)，從而水平方向上IMU誤差呈周期性變化，通過積分予以抵消。

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

(1)

(2)

(3)

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

(4)

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

(5)

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

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

旋轉(zhuǎn)調(diào)制技術(shù)的本質(zhì)是通過控制IMU做周期性的轉(zhuǎn)動(dòng)以改變SINS姿態(tài)矩陣，使得短時(shí)間內(nèi)IMU誤差對(duì)導(dǎo)航系統(tǒng)的積分等于零。旋轉(zhuǎn)式SINS導(dǎo)航算法與非旋轉(zhuǎn)時(shí)具有一致性，因此，可得旋轉(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)系三個(gè)方向陀螺常值漂移，δKgx、δKgy、δKgz為沿著旋轉(zhuǎn)系三個(gè)方向陀螺標(biāo)度因數(shù)誤差。根據(jù)旋轉(zhuǎn)式SINS誤差方程式(6)～(8)可以得到精對(duì)準(zhǔn)狀態(tài)轉(zhuǎn)移矩陣A為

(11)

式中

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

Z=HX+V

(12)

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

(13)

4 兩級(jí)Kalman濾波算法流程

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

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

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

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

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

由圖2可以看出，首先，第一級(jí)Kalman濾波器利用速度和位置基準(zhǔn)信息估計(jì)數(shù)學(xué)平臺(tái)“失準(zhǔn)角”以及水平方向陀螺常值漂移和加速度計(jì)常值偏置，并進(jìn)行補(bǔ)償后作為第二級(jí)Kalman濾波器的輸入，然后，進(jìn)一步利用基準(zhǔn)速度和位置信息對(duì)軸向陀螺常值漂移以及陀螺標(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)動(dòng)，姿態(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時(shí)保持轉(zhuǎn)位機(jī)構(gòu)勻速旋轉(zhuǎn)運(yùn)動(dòng)，并在到達(dá)指定位置后以-8°/s2的加速度減速，直至最終角速度為0，其測(cè)角裝置誤差假設(shè)為5″。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

表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)級(jí)標(biāo)校，主要是完成轉(zhuǎn)軸方向上陀螺漂移誤差的估計(jì)與補(bǔ)償，其后長時(shí)間定位僅有1.96n mile，相比無系統(tǒng)級(jí)標(biāo)校，其定位精度得到提高。

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

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

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

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

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

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

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

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

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

7 結(jié)束語

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