[發明專利]一種慣性里程計組合高精度姿態測量方法

在審

• 申請號: 201410324640.8
• 申請日: 2014-07-09
• 公開（公告）號: CN105318876A
• 公開（公告）日: 2016-02-10
• 發明（設計）人: 艾瀛濤;郭玉勝;莊廣琛;馬小艷;宋雅蘭;莫明崗
• 申請（專利權）人: 北京自動化控制設備研究所
• 主分類號: G01C21/26
• 分類號: G01C21/26
• 代理公司: 核工業專利中心 11007
• 代理人: 高尚梅

1.一種慣性里程計組合高精度姿態測量方法，其特征在于，包括以下步驟：

(1)確定卡爾曼濾波狀態方程狀態變量

卡爾曼濾波狀態方程取19個狀態變量，分別為：北向速度誤差δV_N、天向速度誤差δV_U、東向速度誤差δV_E、緯度誤差高度誤差δh、經度誤差δλ、北向失準角φ_N、天向失準角φ_U、東向失準角φ_E、X陀螺漂移ε_x、Y陀螺漂移ε_y、Z陀螺漂移ε_z、X加表零偏Y加表零偏Z加表零偏航位推算緯度誤差航位推算高度誤差δh_D、航位推算經度誤差δλ_D、里程計刻度系數誤差δK_D；

(2)確定系統各誤差方程

a)速度誤差方程

式中：

V_N、V_U、V_E——北向、天向、東向速度，單位：m/s；

RM——子午圈半徑，單位：m；

RN——卯酉圈半徑，單位：m；

h——慣性高度，單位：m；

ω_ie——地球自轉角速率，常數，取7.292115×10^-5rad/s；

——緯度，單位：rad；

f_N、f_U、f_E——濾波計算用n系下三軸比力分量，單位：m/s²；

C₁₁、C₂₁、C₃₁、C₁₂、C₂₂、C₃₂、C₁₃、C₂₃、C₃₃——姿態矩陣中的元素；

b)位置誤差方程

c)姿態角誤差方程

d)陀螺和加速度計誤差方程

ϵ·i=0▿·i=0(i=x,y,z)···(4)]]>

e)航位推算位置誤差方程

f)里程計刻度系數誤差方程

δK·D=0···(6)]]>

綜合式(1)～(6)，可得系統狀態方程如下：

X·(t)=FX(t)+W(t)···(7)]]>

式中：

X(t)——系統狀態變量，即上述的卡爾曼濾波狀態方程19個狀態變量；

F——根據公式(1)～(6)確定的系統矩陣；

W(t)——系統噪聲；

(3)確定系統觀測方程

系統觀測量取導航解算位置與航位推算位置之差，則系統觀測方程如下：

式中：

Z(t)——觀測變量；

H——觀測矩陣；

V(t)——觀測噪聲；

λ、h——慣性緯度、經度、高度，單位：rad，rad，m；

λ_D、h_D——航位推算緯度、經度、高度，單位：rad，rad，m；

將公式(7)和(8)離散化，可得：

Xk=Φk,k-1Xk-1+Wk-1Zk=HkXk+Vk···(9)]]>

式中：

X_k——狀態變量；

Φ_k,k-1——上一濾波周期至當前濾波周期的一步轉移矩陣；

Z_k——觀測量；

H_k——觀測矩陣；

W_k——系統激勵噪聲序列；

V_k——觀測噪聲序列；

一步轉移矩陣Φ_k,k-1可用下式計算：

Φk,k-1=I+Σi=1nFTn···(10)]]>

式中：

I——單位矩陣；

T_n——導航周期；

n——相鄰兩次濾波時刻之間的導航周期個數；

(4)確定卡爾曼濾波遞推方程組

卡爾曼濾波遞推方程組如下：

a)狀態一步預測

X^k/k-1=Φk,k-1X^k-1···(11)]]>

b)狀態估計

X^k/k-1=X^k/k-1+Kk(Zk-HkX^k/k-1)···(12)]]>

c)濾波增益

Kk=Pk/k-1HkT(HkPk/k-1HkT+Rk)-1···(13)]]>

d)一步預測均方誤差

Pk/k-1=Φk,k-1Pk-1Φk,k-1T+Qk-1···(14)]]>

e)估計均方誤差

Pk=(I-KkHk)Pk/k-1(I-KkHk)T+KkRkKkT···(15)]]>

式中：

——當前濾波周期的實時狀態估計值；

——上一濾波周期對當前濾波周期的狀態預測值；

K_k——當前濾波周期的濾波增益陣；

P_k/k-1——上一濾波周期對當前濾波周期的預測誤差估計的協方差陣；

P_k——當前濾波周期的實時誤差估計協方差陣；

Q_k-1——系統噪聲方差陣；

R_k——觀測噪聲方差陣；

(5)卡爾曼濾波遞推計算和反饋修正

經過卡爾曼濾波遞推計算后，估計出北向失準角φ_N、東向失準角φ_E、X陀螺漂移ε_x、Z陀螺漂移ε_z；然后對其進行反饋修正以提高姿態精度。

2.如權利要求1所述的一種慣性里程計組合高精度姿態測量方法，其特征在于：步驟(5)中，對經過卡爾曼濾波遞推計算后估計的出北向失準角φ_N、東向失準角φ_E、X陀螺漂移ε_x、Z陀螺漂移ε_z進行反饋修正，修正方法如下：

a)姿態修正，用表示修正后的姿態矩陣

失準角計算公式如下：

φN=X^k(7)φU=X^k(8)=0φE=X^k(9)···(16)]]>

X_k(i)，i＝1～19，表示上述的卡爾曼濾波狀態方程19個狀態變量中的第i個狀態變量；

姿態修正公式如下：

CF=cφEcφUsφE-cφEsφU-cφNsφEcφU+sφNsφUcφNcφEcφNsφEsφU+sφNcφUsφNsφEcφU+cφNsφU-sφNcφE-sφNsφEsφU+cφNcφU··(17)]]>

式中：c代表cos，s代表sin；

Cnb1=Cnb·CF···(18)]]>

b)陀螺漂移修正

陀螺漂移修正公式如下：

ϵx=X^k(10)ϵy=X^k(11)=0ϵz=X^k(12)···(19)]]>

將陀螺漂移補償到載體系下的角速率中，實時參與導航解算：

ω→ibb=ωibx0bωiby0bωibz0b-ϵxϵyϵz···(20)]]>

式中：

表示導航周期內載體坐標系下陀螺角速率輸出，單位：rad/s。