GPS輔助MIMU靜止條件下的初始對準方法

張復建 單 斌 王躍鋼 楊 波 騰紅磊

火箭軍工程大學導彈工程學院，西安710025

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

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

1 靜基座粗對準

1.1 常用坐標系及轉換關系

(1)

式中：c表示余弦函數；s表示正弦函數。

當采用四元數[q0,q1,q2,q3]T表示姿態矩陣時，其對應關系為[7]：

(2)

姿態角與四元數的轉換關系為：

(3)

1.2 粗對準原理

當載體處于靜止狀態時，加速度計輸出為載體重力矢量在載體系的投影：

(4)

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

(5)

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

(6)

因此，可通過加速度計輸出計算載體的水平姿態角θm和γm。

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

2 靜基座精對準

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

圖1 初始對準流程

2.1 系統狀態方程

采用四元數作為精對準系統的狀態量，四元數的微分方程如下：

(7)

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

(8)

式中，D表示陀螺儀常值漂移誤差;w(t) 表示白噪聲;β和σ分別為馬爾科夫過程的相關系數和驅動白噪聲的標準方差。

選取系統狀態變量如下：

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

(9)

由式(7)和(9)建立系統的狀態微分方程：

(10)

離散化后得到：

xk+1=Φkxk+Gkwk

(11)

式中,Φk為狀態轉移矩陣；Gk為系統噪聲分配矩陣；wk為系統噪聲，滿足wk～N(0,Qk)。

2.2 系統量測方程

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

(12)

定義系統觀測量為：

(13)

式中，上標m表示對應變量為量測值。則系統的量測方程表示為：

zk=Hkxk+vk

(14)

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

2.3 卡爾曼濾波算法

經典的卡爾曼濾波算法以線性最小方差為估計原則，通過量測值按一定遞推算法實現狀態向量的估計，其算法過程如下：

1)初始化：

(15)

2)時間更新：

(16)

(17)

3)量測更新：

(18)

(19)

(20)

3 改進的自適應濾波算法

傳統的卡爾曼濾波算法，濾波效果取決于系統準確的先驗統計知識，包括系統噪聲矩陣Q和觀測噪聲矩陣R，因此，當噪聲統計特性不準確或者可變時，直接影響濾波效果。針對此情況，經典卡爾曼濾波的基礎上，依據量測新息和狀態殘差，自適應的在線調整噪聲矩陣，實現對狀態量的精準估計。

3.1 自適應的系統噪聲估計

(21)

(22)

式中，j0=k-N+1，N表示采樣點的數目，采用估計的新息協方差替換其理論協方差得到：

(23)

(24)

式中，LR為移動窗口的大小。

3.2 自適應的觀測噪聲估計

定義狀態殘差為：

(25)

狀態殘差dk表示狀態向量估計值在量測更新前與更新后的差別，大的狀態殘差表明狀態的一步預測誤差較大，當采用新的量測新息更新狀態估計值時，會出現大的估計跳躍現象。為了提高算法的預測性能，應對狀態殘差作適當修正，類似于觀測噪聲，系統噪聲修正如下：

(26)

(27)

式中，LQ為移動窗口的大小。

4 仿真實驗

為有效驗證本文對準方法，在上述條件下，設置4組實驗分別為：

實驗1：θ=γ=5°，ψ=40°；

實驗2：θ=γ=10°，ψ=40°；

實驗3：θ=γ=20°，ψ=40°；

實驗4：θ=γ=30°，ψ=40°。

仿真時間設置400s，前120s為粗對準過程，根據粗對準過程的結果，完成精對準過程中卡爾曼濾波器的初始化，120s之后切換為精對準過程。

上述實驗中，當微慣組靜止時，基于加速度計解算的水平姿態角，由于受到加速度計誤差的影響，水平姿態角誤差較大，且波動幅度變化劇烈。為便于合理掌握精對準過程中量測噪聲統計特性，將粗對準過程中所測量的姿態角數據序列的平均值視為姿態角真實值，并轉換為對應四元數序列，將對應四元數誤差的方差視為量測噪聲統計特性。

從4組實驗得出的結論是：

1)采用改進的自適應卡爾曼濾波方式能有效降低對準的姿態誤差，俯仰角誤差從20′基本降到5′之內，航向角誤差從60′基本降到20′之內;

2)雖然采用濾波算法能有效降低橫滾角誤差，但是，針對不同橫滾角狀態，其濾波效果不如俯仰角誤差及航向角誤差，且隨著水平姿態角的增大，對應的橫滾角對準誤差均值逐漸增大，其誤差波動范圍也有增大的趨勢;

3)為了能得到較好的初始對準結果，應當盡量保證載體處于小角度的水平狀態。

圖2 俯仰角誤差

圖3 橫滾角誤差

圖4 航向角誤差

圖5 精對準過程的誤差均值

圖6 精對準過程的誤差標準差

5 結論

當載體處于靜止狀態時，采用GPS輔助微慣組進行初始對準，實驗驗證結果表明，保持姿態處于水平狀態(小于5°)時，采用本文方法可以較高精度地完成微慣組的初始對準。