[发明专利]硅微航姿系统惯性/地磁组合方法

摘要

本发明硅微航姿系统惯性/地磁组合方法。具体涉及：利用重力向量和地磁向量的观测量确定载体的初始姿态；基于重力场、地磁场这两个姿态参考向量选取最佳观测向量的方法；组合Kalman滤波器观测方程坐标系的选取原则；惯性/地磁Kalman组合滤波器的实现方法。通过上述发明，可以用低精度硅微惯性元件与磁传感器组合实现硅微航姿系统，实时给出载体的航向姿态。本发明对低精度惯性/地磁组合方法具有通用性，能减小载体运动加速度对重力的影响和外界磁场对地磁场的影响，可广泛应用于硅微航姿系统。

主权项

1.硅微航姿系统惯性/地磁组合方法，其特征是：(1)利用微惯性测量组件MIMU中的硅微传感器感应载体运动：硅微陀螺敏感载体运动沿其轴向的角速度信号，硅微加速度计敏感载体运动沿其轴向的加速度及重力加速度信号，利用MIMU中的磁强计敏感地磁向量，并将信号送至导航计算机；(2)利用加速度计和磁强计对重力向量和地磁向量的观测来确定载体的初始姿态：重力向量a和地磁向量m在地理坐标系n系的投影an=00g]]>和mn=m0xnm0ynm0zn]]>为已知常量，mⁿ为地磁场在当地地理坐标系n系中的投影向量，在载体坐标系b系中的投影为a^b和m^b，构造三个正交向量：q＝ar＝a×ms＝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×3Q₂的对角线元素根据陀螺零位漂移的特点来确定，系统的噪声系数矩阵为：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₁₃横滚角主值由上述主值按照如下公式判断真值：φ＝φ_主