[發明專利]硅微航姿系統慣性/地磁組合方法

無效

• 申請號: 200810064714.3
• 申請日: 2008-06-13
• 公開（公告）號: CN101290229A
• 公開（公告）日: 2008-10-22
• 發明（設計）人: 黃衛權;趙琳;閆保中;許兆新;郝勇
• 申請（專利權）人: 哈爾濱工程大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/18;G01C21/08
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.硅微航姿系統慣性/地磁組合方法，其特征是：

(1)利用微慣性測量組件MIMU中的硅微傳感器感應載體運動：硅微陀螺敏感載體運動沿其軸向的角速度信號，硅微加速度計敏感載體運動沿其軸向的加速度及重力加速度信號，利用MIMU中的磁強計敏感地磁向量，并將信號送至導航計算機；

(2)利用加速度計和磁強計對重力向量和地磁向量的觀測來確定載體的初始姿態：重力向量a和地磁向量m在地理坐標系n系的投影an=00g]]>和mn=m0xnm0ynm0zn]]>為已知常量，mⁿ為地磁場在當地地理坐標系n系中的投影向量，在載體坐標系b系中的投影為a^b和m^b，構造三個正交向量：

q＝a

r＝a×m

s＝r×a

它們在n系中的投影構成矩陣[]，由aⁿ和mⁿ求得；在b系中的投影構成矩陣[]，由a^b和m^b求得，

qb=Cnb·qnrb=Cnb·rnsb=Cnb·sn,]]>

[qb···rb···sb]=Cnb·[qn···rn···sn]]]>

Cnb=[qb···rb···sb]·[qn···rn···sn]-1---(1)]]>

由此，求出姿態矩陣C_n^b的估計根據下面的兩個公式求得Kalman濾波器的初值

|q^01|=121+C11-C22-C33]]>sig(q^00)=1]]>

|q^02|=121-C11+C22-C33]]>sig(q^01)=sign(C32-C23)]]>

|q^03|=121-C11-C22+C33]]>sig(q^02)=sign(C13-C31)]]>

|q^00|=121+q012-q022-q032]]>sig(q^03)=sign(C21-C12)]]>

則

(3)利用加速度計和磁強計對重力向量和地磁向量的觀測來修正陀螺給出的姿態角，即把這兩個參考向量在n系中的理論值通過陀螺給出的姿態角投影到b系中，與加速度計和磁強計的測量值求差作為觀測量來完成對姿態的修正；本步驟包含如下步驟：

①建立卡爾曼濾波的狀態方程：

X·(t)=F(t)X(t)+G(t)W(t)---(3)]]>

式中，X(t)是t時刻系統的狀態向量，F(t)、G(t)分別為系統狀態矩陣和噪聲矩陣，W(t)為系統的噪聲向量；

系統的狀態矢量為：

X=qeΔB=qe1qe2qe3ΔBxΔByΔBzT---(4)]]>

系統的狀態噪聲向量為：

W(t)=W1(t)W2(t)=ωrxωryωrzωgxωgyωgzT---(5)]]>

其中，q_e＝[q_e1?q_e2?q_e3]^T為姿態四元數向量估計誤差，ΔB＝[ΔB_x?ΔB_y?ΔB_z]^T為陀螺零偏估計誤差，W₁(t)＝(ω_rx?ω_ry?ω_rz)^T為陀螺誤差噪聲，W₂(t)＝(ω_gx?ω_gy?ω_gz)^T為陀螺零偏估計誤差隨機游走模型的零均值白噪聲；

{W(t)}為獨立零均值高斯白噪聲過程，其協方差矩陣：

E{W(t)W^T(τ)}＝Q(t)δ(t-τ)

其中，Q(t)=Q103×303×3Q2]]>為對角陣，Q₁的對角線元素根據陀螺噪聲來確定，取為噪聲RMS值的平方除以帶寬f^*：

Q₁＝[(RMS_gyro)²/f^*]·I_3×3

Q₂的對角線元素根據陀螺零位漂移的特點來確定，

系統的噪聲系數矩陣為：G(t)=-12I3×303×303×3I3×3---(6)]]>

系統的狀態轉移矩陣為：

上式中，為載體角速度向量在載體坐標系中的估計值，為向量的反對稱矩陣；

②根據加速度計測量值判斷系統加速運動情況和磁強計測量值判斷地磁受干擾情況，如果重力向量和地磁向量沒有受到干擾，轉到步驟③；如果重力向量或地磁向量受到干擾，轉到步驟④：

③建立卡爾曼濾波的觀測方程，并轉到步驟⑤：

Z(t)＝H(t)X(t)+V(t)????(8)

式中，Z(t)為t時刻系統的量測向量，H(t)為系統量測矩陣，V(t)為系統的量測噪聲向量；

系統量測向量為：

觀測向量

式中，a_x?a_y?a_z為三軸加速度計輸出的加速度；

觀測向量

式中，m_x?m_y?m_z為三軸磁強計輸出的磁強；

觀測噪聲向量為：V(t)=V1(t)V2(t)---(10)]]>

{V(t)}為獨立均值高斯白噪聲過程，其協方差矩陣：

E{V(t)V^T(τ)}＝R_d(t)δ(t-τ)

Rd(t)=Rd103×303×3Rd2]]>為對角陣，R_d1和R_d2的對角線元素分別根據加速度計和磁強計的噪聲來確定，工程上可近似取為噪聲RMS值的平方：

R_d1＝(RMS_acc)²·I_3×3，R_d2＝(RMS_mag)²·I_3×3

系統觀測矩陣為：H(t)＝[H₁(t)?0_6×3]????(11)

上式中，

④構造重力向量和地磁向量的法向量：

r＝a×m

在n系中投影的法向量理論值為rⁿ＝aⁿ×mⁿ，可以由aⁿ和mⁿ求得；

在b系中法向量的測量值為r^b＝a^b×m^b，可以由a^b和m^b求得；建立卡爾曼濾波的觀測方程：

Z(t)＝H(t)X(t)+V(t)

式中，Z(t)為t時刻系統的量測向量；H(t)為系統量測矩陣；V(t)為系統的量測噪聲向量；

(a)地磁向量受到干擾的情況：

系統量測向量為：

觀測向量

觀測噪聲向量為：V(t)=V1(t)V3(t)---(10)]]>

V₃(t)為法向量的測量噪聲，與V₁(t)和V₂(t)有關{V(t)}為獨立均值高斯白噪聲過程，其協方差矩陣：

E{V(t)V^T(τ)}＝R_d(t)δ(t-τ)

Rd(t)=Rd103×303×3Rd3]]>為對角陣

系統觀測矩陣為：H(t)＝[H₁(t)0_6×3]

上式中，

(b)重力向量受到干擾的情況：

系統量測向量為：

觀測噪聲向量為：V(t)=V3(t)V2(t)---(10)]]>

{V(t)}為獨立均值高斯白噪聲過程，其協方差矩陣：

E{V(t)V^T(τ)}＝R_d(t)δ(t-τ)

Rd(t)=Rd303×303×3Rd2]]>為對角陣

系統觀測矩陣為：H(t)＝[H₁(t)0_6×3]

上式中，

⑤根據擴展卡爾曼濾波遞推方程和步驟①、③或④的結果，建立卡爾曼濾波的時間傳播方程：

式中ω^b＝[ω_x?ω_y?ω_z]^T為當前時刻三軸陀螺的輸出；為上一時刻更新后的陀螺零偏估計；Δt為計算步長；

q‾·=12q‾⊗ω‾=12q‾⊗0ω‾(t-)]]>

式中，qω分別表示四元數向量，表示四元數乘法，具體為：

P(t+Δt)^-)＝Φ(t+Δt，t)P(t⁺)Φ^T(t+Δt，t)+Q_d(t)????(13)

式中，Φ(t+Δt，t)＝I+F(t)Δt，Q_d(t)＝Q(t)Δt

⑥根據擴展卡爾曼濾波遞推方程和步驟①、②的結果，建立卡爾曼濾波的測量修正方程：

K(t)＝P(t^-)H^T(HP(t^-)H^T+R_d)^-1????(14)

P(t⁺)＝(I-K(t)H)P(t^-)????(15)

經過補償更新后需進行規一化處理；

(4)輸出載體姿態參數：由步驟(3)估計得到的姿態四元數即可由公式(2)求得姿態矩陣由姿態矩陣的元素計算得出姿態角，具體步驟：

航向角主值

俯仰角主值φ_主＝-sin^-1C₁₃

橫滾角主值

由上述主值按照如下公式判斷真值：

φ＝φ_主