張復(fù)建 單 斌 王躍鋼 楊 波 騰紅磊

火箭軍工程大學(xué)導(dǎo)彈工程學(xué)院，西安710025

傳統(tǒng)的高精度慣性器件在捷聯(lián)慣性導(dǎo)航系統(tǒng)(SINS)中難以廣泛應(yīng)用的最大因素是其成本和重量，隨著微機(jī)電系統(tǒng)(MEMS)技術(shù)的不斷發(fā)展，基于微機(jī)電技術(shù)的微型陀螺儀和加速度因具有成本低、功耗低和體積小等優(yōu)勢(shì)，在民用和軍事方面，都應(yīng)用的越來越廣泛[1]。

然而，低成本的微型慣性測(cè)量單元(MIMU，簡(jiǎn)稱：微慣組)，由于受微機(jī)電技術(shù)工藝等所限，微慣組的輸出具有較大的偏差和噪聲[2]，其測(cè)量精度相對(duì)傳統(tǒng)的慣性器件低很多，尤其是陀螺儀在靜止?fàn)顟B(tài)時(shí)，無法敏感地球自轉(zhuǎn)角速率，因此，微慣組無法使用雙矢量法完成初始對(duì)準(zhǔn)，必須依靠外部測(cè)量信息進(jìn)行輔助對(duì)準(zhǔn)。隨著全球衛(wèi)星定位系統(tǒng)(GPS)的不斷發(fā)展，李士心[3]采用GPS提供的速度信息輔助低成本的IMU/GPS組合導(dǎo)航系統(tǒng)進(jìn)行初始對(duì)準(zhǔn)，較好地估計(jì)了航向角誤差和IMU參數(shù)誤差，龐春雷[4]針對(duì)低精度的捷聯(lián)慣性導(dǎo)航無法實(shí)現(xiàn)航向角的自對(duì)準(zhǔn)和GPS雙天線側(cè)向需要較長(zhǎng)時(shí)間初始化的問題，研究了二者相互輔助的算法，同時(shí)，許多學(xué)者研究了GPS測(cè)量姿態(tài)角的方法[5-6]，對(duì)此，本文通過GPS測(cè)量航向角，融合加速度計(jì)輸出信息，實(shí)現(xiàn)粗對(duì)準(zhǔn)過程，采用改進(jìn)的自適應(yīng)卡爾曼濾波方法實(shí)現(xiàn)精對(duì)準(zhǔn)過程。

1 靜基座粗對(duì)準(zhǔn)

1.1 常用坐標(biāo)系及轉(zhuǎn)換關(guān)系

(1)

式中：c表示余弦函數(shù)；s表示正弦函數(shù)。

當(dāng)采用四元數(shù)[q0,q1,q2,q3]T表示姿態(tài)矩陣時(shí)，其對(duì)應(yīng)關(guān)系為[7]：

(2)

姿態(tài)角與四元數(shù)的轉(zhuǎn)換關(guān)系為：

(3)

1.2 粗對(duì)準(zhǔn)原理

當(dāng)載體處于靜止?fàn)顟B(tài)時(shí)，加速度計(jì)輸出為載體重力矢量在載體系的投影：

(4)

式中，g為重力加速度,式(4)單位化后得到：

(5)

將式(2)帶入(5)并整理得到：

(6)

因此，可通過加速度計(jì)輸出計(jì)算載體的水平姿態(tài)角θm和γm。

同時(shí)，利用GPS雙天線測(cè)向原理[8]，可輸出載體的航向角ψm。按照式(1)計(jì)算姿態(tài)矩陣，完成粗對(duì)準(zhǔn)過程。值得注意的是，在粗對(duì)準(zhǔn)過程中，通過GPS多次測(cè)量取平均值的方式，可以精確得到載體的緯度L。

2 靜基座精對(duì)準(zhǔn)

在粗對(duì)準(zhǔn)階段，由于加速度計(jì)存在零偏誤差，影響水平姿態(tài)角的計(jì)算精度，GPS雙天線測(cè)向得到的航向角誤差，影響粗對(duì)準(zhǔn)得到的姿態(tài)矩陣的準(zhǔn)確。本文通過采用卡爾曼濾波方法，在粗對(duì)準(zhǔn)基礎(chǔ)上，濾除噪聲誤差影響，進(jìn)一步估計(jì)得到載體的高精度姿態(tài)矩陣，完成精對(duì)準(zhǔn)過程。載體的對(duì)準(zhǔn)流程如圖1。

圖1 初始對(duì)準(zhǔn)流程

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

采用四元數(shù)作為精對(duì)準(zhǔn)系統(tǒng)的狀態(tài)量，四元數(shù)的微分方程如下：

(7)

慣性元件(MIMU)的量測(cè)誤差較大，降低了系統(tǒng)整體初始對(duì)準(zhǔn)精度，與加速度計(jì)誤差相比，陀螺儀漂移誤差對(duì)系統(tǒng)初始對(duì)準(zhǔn)性能的影響更大，因此，將陀螺儀的常值漂移誤差狀態(tài)列入系統(tǒng)狀態(tài)量中，對(duì)應(yīng)模型[9]表示如下：

(8)

式中，D表示陀螺儀常值漂移誤差;w(t) 表示白噪聲;β和σ分別為馬爾科夫過程的相關(guān)系數(shù)和驅(qū)動(dòng)白噪聲的標(biāo)準(zhǔn)方差。

選取系統(tǒng)狀態(tài)變量如下：

x=[q0,q1,q2,q3,Dx,Dy,Dz]T

(9)

由式(7)和(9)建立系統(tǒng)的狀態(tài)微分方程：

(10)

離散化后得到：

xk+1=Φkxk+Gkwk

(11)

式中,Φk為狀態(tài)轉(zhuǎn)移矩陣；Gk為系統(tǒng)噪聲分配矩陣；wk為系統(tǒng)噪聲，滿足wk～N(0,Qk)。

2.2 系統(tǒng)量測(cè)方程

載體的水平姿態(tài)角θm和γm由加速度計(jì)輸出計(jì)算得到，航向角ψm由GPS提供，可根據(jù)式(3)得到量測(cè)四元數(shù)如下：

(12)

定義系統(tǒng)觀測(cè)量為：

(13)

式中，上標(biāo)m表示對(duì)應(yīng)變量為量測(cè)值。則系統(tǒng)的量測(cè)方程表示為：

zk=Hkxk+vk

(14)

式中，觀測(cè)矩陣Hk=I4，vk為觀測(cè)噪聲，滿足vk～N(0,Rk)，且與系統(tǒng)噪聲wk不相關(guān)。

2.3 卡爾曼濾波算法

經(jīng)典的卡爾曼濾波算法以線性最小方差為估計(jì)原則，通過量測(cè)值按一定遞推算法實(shí)現(xiàn)狀態(tài)向量的估計(jì)，其算法過程如下：

1)初始化：

(15)

2)時(shí)間更新：

(16)

(17)

3)量測(cè)更新：

(18)

(19)

(20)

3 改進(jìn)的自適應(yīng)濾波算法

傳統(tǒng)的卡爾曼濾波算法，濾波效果取決于系統(tǒng)準(zhǔn)確的先驗(yàn)統(tǒng)計(jì)知識(shí)，包括系統(tǒng)噪聲矩陣Q和觀測(cè)噪聲矩陣R，因此，當(dāng)噪聲統(tǒng)計(jì)特性不準(zhǔn)確或者可變時(shí)，直接影響濾波效果。針對(duì)此情況，經(jīng)典卡爾曼濾波的基礎(chǔ)上，依據(jù)量測(cè)新息和狀態(tài)殘差，自適應(yīng)的在線調(diào)整噪聲矩陣，實(shí)現(xiàn)對(duì)狀態(tài)量的精準(zhǔn)估計(jì)。

3.1 自適應(yīng)的系統(tǒng)噪聲估計(jì)

(21)

(22)

式中，j0=k-N+1，N表示采樣點(diǎn)的數(shù)目，采用估計(jì)的新息協(xié)方差替換其理論協(xié)方差得到：

(23)

(24)

式中，LR為移動(dòng)窗口的大小。

3.2 自適應(yīng)的觀測(cè)噪聲估計(jì)

定義狀態(tài)殘差為：

(25)

狀態(tài)殘差dk表示狀態(tài)向量估計(jì)值在量測(cè)更新前與更新后的差別，大的狀態(tài)殘差表明狀態(tài)的一步預(yù)測(cè)誤差較大，當(dāng)采用新的量測(cè)新息更新狀態(tài)估計(jì)值時(shí)，會(huì)出現(xiàn)大的估計(jì)跳躍現(xiàn)象。為了提高算法的預(yù)測(cè)性能，應(yīng)對(duì)狀態(tài)殘差作適當(dāng)修正，類似于觀測(cè)噪聲，系統(tǒng)噪聲修正如下：

(26)

(27)

式中，LQ為移動(dòng)窗口的大小。

4 仿真實(shí)驗(yàn)

為有效驗(yàn)證本文對(duì)準(zhǔn)方法，在上述條件下，設(shè)置4組實(shí)驗(yàn)分別為：

實(shí)驗(yàn)1：θ=γ=5°，ψ=40°；

實(shí)驗(yàn)2：θ=γ=10°，ψ=40°；

實(shí)驗(yàn)3：θ=γ=20°，ψ=40°；

實(shí)驗(yàn)4：θ=γ=30°，ψ=40°。

仿真時(shí)間設(shè)置400s，前120s為粗對(duì)準(zhǔn)過程，根據(jù)粗對(duì)準(zhǔn)過程的結(jié)果，完成精對(duì)準(zhǔn)過程中卡爾曼濾波器的初始化，120s之后切換為精對(duì)準(zhǔn)過程。

上述實(shí)驗(yàn)中，當(dāng)微慣組靜止時(shí)，基于加速度計(jì)解算的水平姿態(tài)角，由于受到加速度計(jì)誤差的影響，水平姿態(tài)角誤差較大，且波動(dòng)幅度變化劇烈。為便于合理掌握精對(duì)準(zhǔn)過程中量測(cè)噪聲統(tǒng)計(jì)特性，將粗對(duì)準(zhǔn)過程中所測(cè)量的姿態(tài)角數(shù)據(jù)序列的平均值視為姿態(tài)角真實(shí)值，并轉(zhuǎn)換為對(duì)應(yīng)四元數(shù)序列，將對(duì)應(yīng)四元數(shù)誤差的方差視為量測(cè)噪聲統(tǒng)計(jì)特性。

從4組實(shí)驗(yàn)得出的結(jié)論是：

1)采用改進(jìn)的自適應(yīng)卡爾曼濾波方式能有效降低對(duì)準(zhǔn)的姿態(tài)誤差，俯仰角誤差從20′基本降到5′之內(nèi)，航向角誤差從60′基本降到20′之內(nèi);

2)雖然采用濾波算法能有效降低橫滾角誤差，但是，針對(duì)不同橫滾角狀態(tài)，其濾波效果不如俯仰角誤差及航向角誤差，且隨著水平姿態(tài)角的增大，對(duì)應(yīng)的橫滾角對(duì)準(zhǔn)誤差均值逐漸增大，其誤差波動(dòng)范圍也有增大的趨勢(shì);

3)為了能得到較好的初始對(duì)準(zhǔn)結(jié)果，應(yīng)當(dāng)盡量保證載體處于小角度的水平狀態(tài)。

圖2 俯仰角誤差

圖3 橫滾角誤差

圖4 航向角誤差

圖5 精對(duì)準(zhǔn)過程的誤差均值

圖6 精對(duì)準(zhǔn)過程的誤差標(biāo)準(zhǔn)差

5 結(jié)論

當(dāng)載體處于靜止?fàn)顟B(tài)時(shí)，采用GPS輔助微慣組進(jìn)行初始對(duì)準(zhǔn)，實(shí)驗(yàn)驗(yàn)證結(jié)果表明，保持姿態(tài)處于水平狀態(tài)(小于5°)時(shí)，采用本文方法可以較高精度地完成微慣組的初始對(duì)準(zhǔn)。