[發明專利]一種基于MEMS傳感器的航天器姿態測量方法

有效

• 申請號: 201310019753.2
• 申請日: 2013-01-21
• 公開（公告）號: CN103090870A
• 公開（公告）日: 2013-05-08
• 發明（設計）人: 常洪龍;趙彥明;申強;袁廣民;謝建兵;容建剛
• 申請（專利權）人: 西北工業大學
• 主分類號: G01C21/24
• 分類號: G01C21/24
• 代理公司: 西北工業大學專利中心 61204
• 代理人: 呂湘連

1.一種基于MEMS傳感器的航天器姿態測量方法。所述的航天器姿態測量方法,主要利用GPS雙天線、三軸MEMS陀螺儀、三軸MEMS磁強計等傳感器。所述姿態測量方法采用地理坐標系（g）作為導航坐標系（n）。將測量系統與航天器載體固連，使得傳感器的敏感軸分別位于載體坐標系的三個軸上；

所述的姿態測量方法步驟如下：

步驟1：系統初始對準；在初始對準時刻t₀，系統利用GPS雙天線測量俯仰角橫滾角三軸MEMS磁強計測量航向角求得初始時刻的姿態陣和姿態四元數Q(t₀)；具體求解公式如下：

Cbn(t0)=cosγ~0cosψ~0+sinγ~0sinψ~0sinθ~0sinψ~0cosθ~0sinγ~0cosψ~0-cosγ~0sinψ~0sinθ~0-cosγ~0sinψ~0+sinγ~0cosψ~0sinθ~0cosψ~0cosθ~0-sinγ~0sinψ~0-cosγ~0cosψ~0sinθ~0-sinγ~0cosθ~0sinθ~0cosγ~0cosθ~0---(1)]]>

設初始對準后，姿態陣初值為則姿態四元數的初始值Q(t₀)可以由下面公式確定：

q0=121+T11+T22+T33q1=12sign(T32-T23)1+T11-T22-T33q2=12sign(T13-T31)1-T11+T22-T33q3=12sign(T21-T12)1-T11-T22+T33---(2)]]>

步驟2：信號采集；輸入通道按照一定的采樣周期T，采集傳感器的輸出信號；

步驟3：陀螺儀姿態更新；利用四元數的初始值Q(t₀)和陀螺輸出角速度信號，采用等效旋轉矢量三子樣優化算法，遞推解算姿態四元數Q(t_k)和三個姿態角實時值ψ_k、θ_k、γ_k；

設時間間隔[t_k-1,t_k]內，Δθ_i（i=1,2,3）為姿態更新周期h的三等分時間間隔內陀螺的角增量輸出則優化三子樣旋轉矢量算法公式如下：

Φ(h)=Δθ1+Δθ2+Δθ3+92(Δθ1×Δθ3)+2740Δθ2×(Δθ3-Δθ1)---(3)]]>

q(h)=cos(|Φ|2)+Φ|Φ|sin(|Φ|2)---(4)]]>

Q(tk)=Q(tk-1)⊗q(h)---(5)]]>

設Q(t_k)＝[q₀??q₁??q₂??q₃]^T，由Q(t_k)依次求出和姿態角ψ_k、θ_k、γ_k；

Ψ和γ的真值可以根據主值查詢反正切函數的真值表得到；

步驟4：雙天線GPS/磁強計姿態實時計算；系統運行過程中，利用GPS雙天線測量俯仰角和橫滾角的實時值三軸磁強計測量實時值H→kb=Hx,kbHy,kbHz,kbT,]]>航天器所在軌道位置的磁偏角為α_k，計算得到ψ_magmain,k；

Ψmagmain,k=arctan(-cosγ~k·Hx,kb+sinγ~k·Hz,kbsinγ~k·sinθ~k·Hx,kb+cosθ~k·Hy,kb-cosγ~k·sinθ~k·Hz,kb) ---(8)]]>

查真值表，得到磁航向的真值ψ_mag,k，再根據得到航向角的實時值

步驟5：kalman濾波；

構造狀態和量測Zk=θ~k-θkγ~k-γkψ~k-ψkT,]]>建立系統的狀態空間模型；進行kalman濾波遞推；

系統的狀態方程：

X·=FX+Gw---(9)]]>

其中，w＝[w_gx?w_gy?w_gz?w_ex?w_ey?w_ez]^T為系統的驅動噪聲，方差矩陣為q_f；

F_6×6的非零元為：

F1,2=ωiesinL+VERN+htanL]]>F1,3=-(ωiecosL+VERN+h)]]>

F_1,4＝-T₁₁??F_1,5＝-T₁₂

F_1,6＝-T₁₃F2,1=-(ωiesinL+VERN+htanL)]]>

F2,3=-VNRM+h]]>F_2,4＝-T₂₁

F_2,5＝-T₂₂??F_2,6＝-T₂₃

F3,1=ωiecosL+VERN+h]]>F3,2=VNRM+h]]>

F_3,4＝-T₃₁??F_3,5＝-T₃₂

F_3,6＝-T₃₃F4,4=-1τex]]>

F5,5=-1τey]]>F6,6=-1τez]]>

上面F矩陣元素中，L、h、V_E、V_N分別為航天器的實時緯度、高度、東向速度、北向速度，均由GPS實時提供；ω_ie為地球自轉角速度，為常數；R_M、R_N分別為所在位置的子午圈曲率半徑和卯酉圈曲率半徑，由L按照經驗公式計算得到；τ_ex、τ_ey、τ_ez為三軸MEMS陀螺等效一階馬爾可夫漂移的相關時間；

G_6×6矩陣的非零元為：

G_1,1＝-T_1,1?G_1,2＝-T_1,2?G_1,3＝-T_1,3?G_2,1＝-T_2,1?G_2,2＝-T_2,2?G_2,3＝-T_2,3

G_3,1＝-T_3,1?G_3,2＝-T_3,2?G_3,3＝-T_3,3?G_4,4＝1?G_5,5＝1?G_6,6＝1

設濾波周期為T,對公式(9)離散化得如公式（10）所示：

X_k＝Φ_k,k-1X_k-1+W_k-1(10)

其中，Φ_k,k-1＝I+TF_k-1，W_k-1為系統的驅動白噪聲序列，方差矩陣為Q_f,k；Q_f,k按照下面的方法計算：M_i+1＝F_kM_i+(F_kM_i)^T（i=1,2,3…），

離散形式的kalman濾波量測方程如下：

Zk=θ~k-θkγ~k-γkψ~k-ψkT=HkXk+Vk---(11)]]>其中，H_k＝[I_3×3??0_3×3]，V_k為量測白噪聲序列，方差矩陣為R_k；

公式(10)和公式(11)聯立構成離散kalman濾波的狀態空間模型，利用離散型kalman濾波基本方程進行遞推計算就得到各時刻狀態的最優估計

步驟6：輸出校正；利用對陀螺姿態更新的姿態角進行前饋校正，如下式所示：

步驟7：返回步驟2；返回步驟2循環執行步驟2～步驟7。