[發明專利]一種基于地球重力場與地磁場序貫修正的姿態解算方法

有效

• 申請號: 201810384477.2
• 申請日: 2018-04-26
• 公開（公告）號: CN108871319B
• 公開（公告）日: 2022-05-17
• 發明（設計）人: 李志;王錄;汪璟;倪瑞
• 申請（專利權）人: 李志;王錄;汪璟;倪瑞
• 主分類號: G01C21/08
• 分類號: G01C21/08
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種基于地球重力場與地磁場序貫修正的姿態解算方法，其特征在于，具體步驟為：

(1)根據航姿系統中MEMS陀螺儀采集的角速率計算系統模型的狀態轉移矩陣，并利用離散化的四元數微分方程更新載體姿態四元數，具體過程是：

a.建立陀螺儀動力學模型：

即

其中，X＝[X₀ X₁ X₂ X₃]^T表示全局坐標系G到載體坐標系B的旋轉四元數，表示在載體坐標系中測量的MEMS陀螺角速度信息，且

利用四元數微分方程的比卡求解法，使上式離散化可得

其中，X_k-1為t_k-1時刻全局坐標系G到載體坐標系B的旋轉四元數，X_k為t_k時刻全局坐標系G到載體坐標系B的旋轉四元數，

且

b.獲得載體坐標系B下的載體角速度信息

c.根據所述載體角速度信息計算系統狀態轉移矩陣

c.載體姿態四元數狀態一步預測

d.計算系統噪聲協方差矩陣Q_k

對于MEMS陀螺儀在微小時間間隔Δt，其真實的角增量Δθ°是無法測量的，假設在t_k-1時刻到t_k時刻的時間間隔內，陀螺儀的實際輸出與真實角增量的誤差為δθ_k/k-1，有

其中，Δθ_k/k-1表示陀螺儀的實際角增量，表示陀螺儀的真實角增量；

相應的，實際狀態轉移矩陣與真實狀態轉移矩陣的誤差為ΔΦ_k/k-1，則有

將上式代入步驟a中陀螺儀離散化的動力學模型中，可得

變形為

-δΘ_k/k-1X_k-1＝-Ξ(X_k-1)δθ_k/k-1

式中

設陀螺儀的輸出噪聲為n_gyro，則有

則系統噪聲協方差矩陣為：

其中，Σ_gyro為陀螺儀噪聲協方差矩陣；

f.計算姿態四元數狀態一步預測均方誤差P_k/k-1

其中，P_k-1表示步驟(3)中的狀態估計均方誤差；

(2)根據航姿系統中MEMS加速度計采集的載體重力加速度對步驟(1)中更新得到的姿態四元數由均方誤差最小原則進行修正，具體過程是：

a.建立加速度計觀測模型：

將三維矢量g與a看作零標量的四元數，則g與a間的變換關系可用四元數乘法表示為

其中，g為在全局坐標系G中測量的重力加速度，a為在載體坐標系B中測量的重力加速度，且滿足||g||＝||a||＝1；表示姿態四元數X_k的逆；

上式經過變換可得

M′(a)X_k-M(g)X_k＝0

其中，M′(a)與M(g)表示由零標量的四元數g與a構成的矩陣，如下

b.獲得載體坐標系B下的載體重力加速度信息a＝[a_x a_y a_z]^T；

c.根據所述載體重力加速度信息及先驗重力加速度計算量測矩陣

d.計算重力加速度量測噪聲協方差矩陣Rg′_k

設加速度計的輸出噪聲為n_acc，則有

n_acc＝a-a°

其中，a°表示載體坐標系下加速度計的真實值；

實際量測矩陣與理想量測矩陣誤差為

其中，表示理想量測矩陣；

將上式代入步驟a中的加速度計觀測模型，可得

0＝Hg_kX_k-ΔHg_kX_k

-ΔHg_kX_k變形為

其中，

令則量測噪聲協方差矩陣為

其中，Σ_acc為加速度計噪聲協方差矩陣；

上式得到的量測噪聲協方差矩陣在應用的過程中很有可能是奇異的，為了避免矩陣的奇異，需要對該量測噪聲協方差矩陣進行改進，即

Rg′_k＝Rg_k+β_gI β_g0；

e.計算重力加速度修正的濾波增益Kg_k

Kg_k＝P_k/k-1(Hg_k)^T(Hg_kP_k/k-1(Hg_k)^T+Rg′_k)^-1；

f.基于重力加速度的姿態四元數狀態估計

g.基于重力加速度的姿態四元數狀態估計均方誤差Pg_k

Pg_k＝(I-Kg_kHg_k)P_k/k-1；

(3)根據航姿系統中MEMS磁力計采集的載體地磁場矢量對步驟(2)中修正得到的姿態四元數由均方誤差最小原則進行第二次修正，具體過程是：

a.建立磁力計觀測模型：

把三維矢量h與m看作零標量的四元數，則h與m間的變換關系可用四元數乘法表示為

其中，h為在全局坐標系G中測量的地磁場矢量，m為在載體坐標系B中測量的地磁場矢量，且滿足||g||＝||a||＝1，

上式經過變換可得

M′(m)X_k-M(h)X_k＝0

其中，M′(m)與M(h)表示由零標量的四元數h與m構成的矩陣，如下

b.獲得載體坐標系B下的載體地磁場矢量信息m＝[m_x m_y m_z]^T；

c.計算先驗地磁場矢量h

全局參考坐標系G的^Gz軸選取重力加速度g的反方向，^Gx軸選取地磁北極，^Gy按照右手法則選取，則先驗地磁場矢量h與全局參考坐標系G的^Gxo^Gz面重合即h＝[h_x 0 h_y]^T，則有

其中，l＝[l_x l_y l_z]^T表示帶有誤差的先驗地磁場矢量，

則有

式中

Γ＝l_x²+l_y²

d.根據所述載體地磁場矢量信息及先驗地磁場矢量計算量測矩陣

e.計算地磁場矢量量測噪聲協方差矩陣Rm′_k

設磁力計的輸出噪聲為n_mag，則有

n_mag＝m-m°

其中，m°表示載體坐標系下加速度計的真實值；

實際量測矩陣與理想量測矩陣誤差為

其中，表示理想量測矩陣；

把上式代入步驟a中的磁力計觀測模型，可得

0＝Hm_kX_k-ΔHm_kX_k

-ΔHm_kX_k變形為

其中，

令則量測噪聲協方差矩陣為

其中，Σ_mag為磁力計噪聲協方差矩陣；

上式得到的量測噪聲協方差矩陣在應用的過程中很有可能是奇異的，為了避免矩陣的奇異，需要對該量測噪聲協方差矩陣進行改進，即

Rm′_k＝Rm_k+β_mI β_m0；

f.計算地磁場矢量修正的濾波增益Km_k

Km_k＝Pg_k(Hm_k)^T(Hm_kPg_k(Hm_k)^T+Rm′_k)^-1；

g.基于地磁場矢量的姿態四元數狀態估計

h.基于地磁場矢量的姿態四元數狀態估計均方誤差Pm_k

Pm_k＝(I-Km_kHm_k)Pg_k

其中，I表示單位矩陣；

Pm_k→P_k-1。