CN101290229A - 硅微航姿系统惯性/地磁组合方法 - Google Patents

Abstract

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

Description

硅微航姿系统惯性/地磁组合方法

技术领域

本发明涉及一种测量方法，特别是涉及一种捷联航姿系统的组合导航方法，尤其涉及一种硅微航姿系统惯性/地磁组合导航方法。

背景技术

硅微航姿系统包括由三个硅微陀螺仪、三个硅微加速度计和三轴磁强计组成的微惯性测量单元(MIMU)和导航计算机，是一种捷联式惯性组合导航系统，硅微陀螺仪和加速度计直接固连在运载体上，测量出载体的旋转运动角速率和线运动加速度信息(包括重力加速度)，然后送至导航计算机中进行实时的姿态矩阵解算，并从捷联姿态矩阵的有关元素中计算出载体的姿态角。硅微加速度计及硅微陀螺是基于MEMS技术的一种新型惯性传感器，目前其精度相对常规惯性器件要低很多，陀螺的漂移和噪声水平要高于地球的自转速率，因此目前航姿系统大多采用微惯性器件与磁强计组合使用的方案。

硅微航姿系统用加速度计和磁强计测量的重力向量和地磁向量为参考信息通过Kalman组合滤波来补偿陀螺的漂移、修正陀螺测量的载体运动角速度积分后的角度漂移，确定的载体姿态信息既有长期的稳定精度又具有载体动态运动的快速实时性，明显的提高了航姿系统的精度、增强了系统的鲁棒性。因此如何构建一种更加充分利用加速度计、磁强计和陀螺测量信息的Kalman组合滤波方法对于实现硅微航姿系统具有重要的意义。

在已报道和所能查阅到的国内外相关文献中，给出文献综合分析结果：

由于国外的硅微惯性技术发展得较为成熟，目前研究内容均集中在硅微惯性组合系统在各行业的应用研究，而国外公司的产品技术出于商业保密原因，并未见诸详细报道；而国内的相关研究则大多局限在系统的各项具体技术的理论研究上，如惯性器件的测试、误差建模与补偿、矩阵更新算法等。

发明内容

本发明的目的是提供一种能够有效抑制载体机动影响和地磁干扰的硅微航姿系统惯性/地磁组合方法。

为了达到上述的发明目的，本发明包括下列步骤：

(1)利用微惯性测量组件MIMU中的硅微传感器感应载体运动：硅微陀螺敏感载体运动沿其轴向的角速度信号，硅微加速度计敏感载体运动沿其轴向的加速度及重力加速度信号；利用MIMU中的磁强计敏感地磁向量，并将信号送至导航计算机。

(2)利用加速度计和磁强计对重力向量和地磁向量的观测来确定载体的初始姿态重力向量a和地磁向量m在地理坐标系n系的投影 $a^n=\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]$ 和 $m^n=\left[\begin{array}{c}{m}_{0x}^n\\ m_{0y}^n\\ m_{0z}^n\end{array}\right]$ 为已知常量，mⁿ为地磁场在当地n系的投影向量，可根据经纬度和海拔高度由国际参考地磁场查得。在载体坐标系b系中的投影a^b和m^b，可由加速度计和磁强计测得。

载体坐标系可通过导航坐标系(地理坐标系n系)按航向角(ψ)、俯仰角(φ)、横滚角(θ)三次转动确定，如附图2所示。

构造三个正交向量：

q＝a

r＝a×m

s＝r×a

则它们在n系中的投影构成矩阵

可由aⁿ和mⁿ求得；在b系中的投影构成矩阵可由a^b和m^b求得。

因为 $\left\{\begin{array}{c}{q}^b=C_n^b\·q^n\\ r^b=C_n^b\·r^n\\ s^b=C_n^b\·s^n\end{array}\right.,$ 所以

$\left[\begin{array}{ccccc}{q}^b& \begin{array}{c}.\\ .\\ .\end{array}& r^b& \begin{array}{c}.\\ .\\ .\end{array}& s^b\end{array}\right]=C_n^b\·\left[\begin{array}{ccccc}{q}^n& \begin{array}{c}.\\ .\\ .\end{array}& r^n& \begin{array}{c}.\\ .\\ .\end{array}& s^n\end{array}\right]$

即 $C_n^b=\left[\begin{array}{ccccc}{q}^b& \begin{array}{c}.\\ .\\ .\end{array}& r^b& \begin{array}{c}.\\ .\\ .\end{array}& s^b\end{array}\right]\·{\left[\begin{array}{ccccc}{q}^n& \begin{array}{c}.\\ .\\ .\end{array}& r^n& \begin{array}{c}.\\ .\\ .\end{array}& s^n\end{array}\right]}^{-1}---\left(1\right)$

由此，可求出姿态矩阵C_n ^b的估计

具体地根据下面的两个公式即可求得Kalman滤波器的初值

$\left|{\hat{q}}_{01}\right|=\frac{1}{2}\sqrt{1+C_{11}-C_{22}-C_{33}}$

$\begin{array}{cc}\left|{\hat{q}}_{02}\right|=\frac{1}{2}\sqrt{1-C_{11}+C_{22}-C_{33}}& \begin{array}{c}\mathrm{sig}\left({\hat{q}}_{00}\right)=1\\ \mathrm{sig}\left({\hat{q}}_{01}\right)=\mathrm{sign}(C_{32}-C_{23})\end{array}\end{array}$

$\begin{array}{cc}\left|{\hat{q}}_{03}\right|=\frac{1}{2}\sqrt{1-C_{11}-C_{22}+C_{33}}& \begin{array}{c}\mathrm{sig}\left({\hat{q}}_{02}\right)=\mathrm{sign}(C_{13}-C_{31})\\ \mathrm{sig}\left({\hat{q}}_{03}\right)=\mathrm{sign}(C_{21}-C_{12})\end{array}\end{array}$

$\left|{\hat{q}}_{00}\right|=\frac{1}{2}\sqrt{1+q_{01}^2-q_{02}^2-q_{03}^2}$

则

(3)利用加速度计和磁强计对重力向量和地磁向量的观测来修正陀螺给出的姿态角。具体地，把这两个参考向量在n系中的理论值通过陀螺给出的姿态角投影到b系中，与加速度计和磁强计的测量值求差作为观测量来完成对姿态的修正。本步骤包含如下步骤：

①建立卡尔曼滤波的状态方程：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(3\right)$

式中，X(t)是t时刻系统的状态向量；F(t)、G(t)分别为系统状态矩阵和噪声矩阵；W(t)为系统的噪声向量。

系统的状态矢量为：

$X=\left[\begin{array}{c}{q}_e\\ \mathrm{\ΔB}\end{array}\right]={\left[\begin{array}{cccccc}{q}_{e1}& q_{e2}& q_{e3}& {\mathrm{\ΔB}}_x& {\mathrm{\ΔB}}_y& {\mathrm{\ΔB}}_z\end{array}\right]}^T---\left(4\right)$

系统的状态噪声向量为：

$W\left(t\right)=\left(\begin{array}{c}{W}_1\left(t\right)\\ W_2\left(t\right)\end{array}\right)={\left(\begin{array}{cccccc}{\mathrm{\ω}}_{\mathrm{rx}}& {\mathrm{\ω}}_{\mathrm{ry}}& {\mathrm{\ω}}_{\mathrm{rz}}& {\mathrm{\ω}}_{\mathrm{gx}}& {\mathrm{\ω}}_{\mathrm{gy}}& {\mathrm{\ω}}_{\mathrm{gz}}\end{array}\right)}^T---\left(5\right)$

其中，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\left(t\right)=\left[\begin{array}{cc}{Q}_1& 0_{3\×3}\\ 0_{3\×3}& Q_2\end{array}\right]$ 为对角阵。Q₁的对角线元素根据陀螺噪声来确定，工程上可近似取为噪声RMS值的平方除以带宽f^*：

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

Q₂的对角线元素根据陀螺零位漂移的特点来确定。

系统的噪声系数矩阵为： $G\left(t\right)=\left[\begin{array}{cc}-\frac{1}{2}{I}_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& I_{3\×3}\end{array}\right]---\left(6\right)$

系统的状态转移矩阵为：

上式中，

为载体角速度向量在载体坐标系中的估计值，

为向量

的反对称矩阵，。

②根据加速度计测量值判断系统加速运动情况和磁强计测量值判断地磁受干扰情况，如果重力向量和地磁向量没有受到干扰，转到步骤③；如果重力向量或地磁向量受到干扰，转到步骤④；

③建立卡尔曼滤波的观测方程，并转到步骤⑤：

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\left(t\right)=\left[\begin{array}{c}{V}_1\left(t\right)\\ V_2\left(t\right)\end{array}\right]---\left(10\right)$

{V(t)}为独立均值高斯白噪声过程，其协方差矩阵：

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

$R_d\left(t\right)=\left[\begin{array}{cc}{R}_{d1}& 0_{3\×3}\\ 0_{3\×3}& R_{d2}\end{array}\right]$ 为对角阵，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

重力向量、地磁向量和两者叉乘构造的法向量如附图3所示。

在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\left(t\right)=\left[\begin{array}{c}{V}_1\left(t\right)\\ V_3\left(t\right)\end{array}\right]---\left(10\right)$

V₃(t)为法向量的测量噪声，与V₁(t)和V₂(t)有关

{V(t)}为独立均值高斯白噪声过程，其协方差矩阵：

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

$R_d\left(t\right)=\left[\begin{array}{cc}{R}_{d1}& 0_{3\×3}\\ 0_{3\×3}& R_{d3}\end{array}\right]$ 为对角阵

系统观测矩阵为：H(t)＝[H₁(t)0_6×3]

上式中，

(b)重力向量受到载体长期机动运动干扰的情况：

系统量测向量为：

观测噪声向量为： $V\left(t\right)=\left[\begin{array}{c}{V}_3\left(t\right)\\ V_2\left(t\right)\end{array}\right]---\left(10\right)$

{V(t)}为独立均值高斯白噪声过程，其协方差矩阵：

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

$R_d\left(t\right)=\left[\begin{array}{cc}{R}_{d3}& 0_{3\×3}\\ 0_{3\×3}& R_{d2}\end{array}\right]$ 为对角阵

系统观测矩阵为：H(t)＝[H₁(t)0_6×3]

上式中，

⑤根据扩展卡尔曼滤波递推方程和步骤①、③或④的结果，建立卡尔曼滤波的时间传播方程：

式中ω^b＝[ω_x ω_y ω_z]^T为当前时刻三轴陀螺的输出；为上一时刻更新后的陀螺零偏估计；Δt为计算步长。

$\stackrel{\·}{\stackrel{\‾}{q}}=\frac{1}{2}\stackrel{\‾}{q}\⊗\stackrel{\‾}{\mathrm{\ω}}=\frac{1}{2}\stackrel{\‾}{q}\⊗\left[\begin{array}{c}0\\ \stackrel{\‾}{\mathrm{\ω}}\left(t^{-}\right)\end{array}\right]$

式中，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₁₃

横滚角主值

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

φ＝φ_主

本发明使用的硅微航姿系统中硬件包括微惯性测量单元(简称：MIMU，包括三个相互垂直的硅微陀螺、三个相互垂直的硅微加速度计和三个相互垂直的磁强计)、导航计算机，具体硬件组成如附图1所示；MIMU的测量坐标系(为载体坐标系b系)与地理坐标系n系的关系及载体姿态角如附图2所示。

本发明的方法具有以下优点：(1)航姿系统体积小、重量轻；(2)具有完全的自主性，不需要外部输入信息；(3)受载体机动和地磁干扰的影响小，能够有效提高航姿系统的输出姿态精度；(4)航姿系统可以为其它装置提供每秒50次以上的实时姿态信号。

对本发明的有益效果说明如下：

(1)航姿系统仿真实验

航姿系统置于姿态零位，从第10秒到第20秒，在南北方向加入0.1M(M为地磁场的垂直分量)的磁场偏置；从第35秒到第45秒，在东西方向加入0.1M的磁场偏置，如附图4a所示。系统采用重力向量和地磁向量作为参考向量的姿态仿真结果如附图4c所示。由于地磁场受到干扰，利用本发明所述方法的系统采用重力向量和法向量作为参考向量的姿态仿真结果如附图4d所示。由图4b可见法向量的方向总保持水平，因此图4d中没有产生图4c的水平姿态的影响，本发明所述方法可以减小地磁场干扰对航姿系统姿态的影响。

(2)航姿系统的三轴转台实验

将采用本发明研制的硅微航姿系统安装在三轴转台上进行静态和三轴摇摆试验，所用硅微航姿系统的器件精度如下：

硅微陀螺零位稳定性：5°/s

硅微陀螺刻度因数线性度：0.1％

硅微陀螺刻度因数温度系数：0.1％/℃

硅微陀螺噪声：

硅微加速度计零位稳定性：0.02m/s²

硅微加速度计刻度因数线性度：0.2％

硅微加速度计刻度因数稳定性：0.05m/s²(-25℃～+80℃)

硅微加速度计噪声：

磁强计零位稳定性：0.5mGauss

磁强计刻度因数线性度：2％

磁强计刻度因数稳定性：0.5mGauss

磁强计噪声：0.5mGauss

利用发明所述方法得到三轴转台指北、水平静止状态硅微航姿系统航向角、俯仰角、横滚角曲线分别如附图5a、图5b和图5c所示；三轴摇摆状态航向角、俯仰角、横滚角曲线分别如附图6a、图6b和图6c所示。结果表明硅微航姿系统具有较好的精度。

(3)航姿系统的跑车实验

动态跑车实验在一段较为平直的公路上进行，车辆的姿态大致水平(其中一段较为颠簸)，航向角基本保持为315°，跑车过程包括匀速运动(时速40km/h)、加速运动、减速运动过程。以车辆上另外一套高精度光纤捷联组合惯导系统(PHINS)提供的姿态输出作为参考，得到了跑车过程中硅微航姿系统的航向姿态。颠簸路段横滚角曲线图7a所示，平坦路段横滚角曲线图7b所示，俯仰角、航向角曲线分别如图8、图9所示。

动态跑车实验结果表明，航姿系统利用本发明的方法在动态跑车实验中达到了实用的水平。

附图说明

图1为本发明的硅微航姿系统惯性/地磁组合原理图；

图2为载体坐标系(b系)与地理坐标系(n系)的关系图；

图3为重力向量、地磁向量与法向量的关系图；

图4a-图4d为地磁场受干扰的航姿系统仿真结果图；

图5a-图5c为静态状态航姿系统测试结果图；

图6a-图6c为摇摆状态航姿系统测试结果图；

图7a-图7b为动态跑车航姿系统横滚角测试结果图；

图8为动态跑车航姿系统俯仰角测试结果图；

图9为动态跑车航姿系统航向角测试结果图；

图10为本发明的硅微航姿系统软件流程图。

具体实施方式

下面举例对本发明作更详细地描述，本实施例包括以下步骤：

(1)利用微惯性测量组件MIMU中的硅微传感器感应载体运动：硅微陀螺敏感载体运动沿其轴向的角速度信号，硅微加速度计敏感载体运动沿其轴向的加速度及重力加速度信号；利用MIMU中的磁强计敏感地磁向量，并将信号送至导航计算机。

(2)利用加速度计和磁强计对重力向量和地磁向量的观测来确定载体的初始姿态重力向量a和地磁向量m在地理坐标系n系的投影 $a^n=\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]$ 和 $m^n=\left[\begin{array}{c}{m}_{0x}^n\\ m_{0y}^n\\ m_{0z}^n\end{array}\right]$ 为已知常量，mⁿ为地磁场在当地n系的投影向量，可根据经纬度和海拔高度由国际参考地磁场查得。在载体坐标系b系中的投影a^b和m^b，可由加速度计和磁强计测得。

载体坐标系可通过导航坐标系(地理坐标系n系)按航向角(ψ)、俯仰角(φ)、横滚角(θ)三次转动确定，如附图2所示。

构造三个正交向量：

q＝a

r＝a×m

s＝r×a

则它们在n系中的投影构成矩阵

可由aⁿ和mⁿ求得；在b系中的投影构成矩阵

可由a^b和m^b求得。

因为 $\left\{\begin{array}{c}{q}^b=C_n^b\·q^n\\ r^b=C_n^b\·r^n\\ s^b=C_n^b\·s^n\end{array}\right.,$ 所以

$\left[\begin{array}{ccccc}{q}^b& \begin{array}{c}.\\ .\\ .\end{array}& r^b& \begin{array}{c}.\\ .\\ .\end{array}& s^b\end{array}\right]=C_n^b\·\left[\begin{array}{ccccc}{q}^n& \begin{array}{c}.\\ .\\ .\end{array}& r^n& \begin{array}{c}.\\ .\\ .\end{array}& s^n\end{array}\right]$

即 $C_n^b=\left[\begin{array}{ccccc}{q}^b& \begin{array}{c}.\\ .\\ .\end{array}& r^b& \begin{array}{c}.\\ .\\ .\end{array}& s^b\end{array}\right]\·{\left[\begin{array}{ccccc}{q}^n& \begin{array}{c}.\\ .\\ .\end{array}& r^n& \begin{array}{c}.\\ .\\ .\end{array}& s^n\end{array}\right]}^{-1}---\left(1\right)$

由此，可求出姿态矩阵C_n ^b的估计

具体地根据下面的两个公式即可求得Kalman滤波器的初值

$\left|{\hat{q}}_{01}\right|=\frac{1}{2}\sqrt{1+C_{11}-C_{22}-C_{33}}$

$\begin{array}{cc}\left|{\hat{q}}_{02}\right|=\frac{1}{2}\sqrt{1-C_{11}+C_{22}-C_{33}}& \begin{array}{c}\mathrm{sig}\left({\hat{q}}_{00}\right)=1\\ \mathrm{sig}\left({\hat{q}}_{01}\right)=\mathrm{sign}(C_{32}-C_{23})\end{array}\end{array}$

$\begin{array}{cc}\left|{\hat{q}}_{03}\right|=\frac{1}{2}\sqrt{1-C_{11}-C_{22}+C_{33}}& \begin{array}{c}\mathrm{sig}\left({\hat{q}}_{02}\right)=\mathrm{sign}(C_{13}-C_{31})\\ \mathrm{sig}\left({\hat{q}}_{03}\right)=\mathrm{sign}(C_{21}-C_{12})\end{array}\end{array}$

$\left|{\hat{q}}_{00}\right|=\frac{1}{2}\sqrt{1+q_{01}^2-q_{02}^2-q_{03}^2}$

则

(3)利用加速度计和磁强计对重力向量和地磁向量的观测来修正陀螺给出的姿态角。具体地，把这两个参考向量在n系中的理论值通过陀螺给出的姿态角投影到b系中，与加速度计和磁强计的测量值求差作为观测量来完成对姿态的修正。本步骤包含如下步骤：

①建立卡尔曼滤波的状态方程：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(3\right)$

式中，X(t)是t时刻系统的状态向量；F(t)、G(t)分别为系统状态矩阵和噪声矩阵；W(t)为系统的噪声向量。

系统的状态矢量为：

$X=\left[\begin{array}{c}{q}_e\\ \mathrm{\ΔB}\end{array}\right]={\left[\begin{array}{cccccc}{q}_{e1}& q_{e2}& q_{e3}& {\mathrm{\ΔB}}_x& {\mathrm{\ΔB}}_y& {\mathrm{\ΔB}}_z\end{array}\right]}^T---\left(4\right)$

系统的状态噪声向量为：

$W\left(t\right)=\left(\begin{array}{c}{W}_1\left(t\right)\\ W_2\left(t\right)\end{array}\right)={\left(\begin{array}{cccccc}{\mathrm{\ω}}_{\mathrm{rx}}& {\mathrm{\ω}}_{\mathrm{ry}}& {\mathrm{\ω}}_{\mathrm{rz}}& {\mathrm{\ω}}_{\mathrm{gx}}& {\mathrm{\ω}}_{\mathrm{gy}}& {\mathrm{\ω}}_{\mathrm{gz}}\end{array}\right)}^T---\left(5\right)$

其中，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\left(t\right)=\left[\begin{array}{cc}{Q}_1& 0_{3\×3}\\ 0_{3\×3}& Q_2\end{array}\right]$ 为对角阵。Q₁的对角线元素根据陀螺噪声来确定，工程上可近似取为噪声RMS值的平方除以带宽f^*：

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

Q₂的对角线元素根据陀螺零位漂移的特点来确定。

系统的噪声系数矩阵为： $G\left(t\right)=\left[\begin{array}{cc}-\frac{1}{2}{I}_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& I_{3\×3}\end{array}\right]---\left(6\right)$

系统的状态转移矩阵为：

上式中，

为载体角速度向量在载体坐标系中的估计值，

为向量

的反对称矩阵，。

②根据加速度计测量值判断系统加速运动情况和磁强计测量值判断地磁受干扰情况，如果重力向量和地磁向量没有受到干扰，转到步骤③；如果重力向量或地磁向量受到干扰，转到步骤④；

③建立卡尔曼滤波的观测方程，并转到步骤⑤：

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\left(t\right)=\left[\begin{array}{c}{V}_1\left(t\right)\\ V_2\left(t\right)\end{array}\right]---\left(10\right)$

{V(t)}为独立均值高斯白噪声过程，其协方差矩阵：

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

$R_d\left(t\right)=\left[\begin{array}{cc}{R}_{d1}& 0_{3\×3}\\ 0_{3\×3}& R_{d2}\end{array}\right]$ 为对角阵，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

重力向量、地磁向量和两者叉乘构造的法向量如附图3所示。

在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\left(t\right)=\left[\begin{array}{c}{V}_1\left(t\right)\\ V_3\left(t\right)\end{array}\right]---\left(10\right)$

V₃(t)为法向量的测量噪声，与V₁(t)和V₂(t)有关

{V(t)}为独立均值高斯白噪声过程，其协方差矩阵：

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

$R_d\left(t\right)=\left[\begin{array}{cc}{R}_{d1}& 0_{3\×3}\\ 0_{3\×3}& R_{d3}\end{array}\right]$ 为对角阵

系统观测矩阵为：H(t)＝[H₁(t)0_6×3]

上式中，

(b)重力向量受到载体长期机动运动干扰的情况：

系统量测向量为：

观测噪声向量为： $V\left(t\right)=\left[\begin{array}{c}{V}_3\left(t\right)\\ V_2\left(t\right)\end{array}\right]---\left(10\right)$

{V(t)}为独立均值高斯白噪声过程，其协方差矩阵：

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

$R_d\left(t\right)=\left[\begin{array}{cc}{R}_{d3}& 0_{3\×3}\\ 0_{3\×3}& R_{d2}\end{array}\right]$ 为对角阵

系统观测矩阵为：H(t)＝[H₁(t)0_6×3]

上式中，

⑤根据扩展卡尔曼滤波递推方程和步骤①、③或④的结果，建立卡尔曼滤波的时间传播方程：

式中ω^b＝[ω_x ω_y ω_z]^T为当前时刻三轴陀螺的输出；为上一时刻更新后的陀螺零偏估计；Δt为计算步长。

$\stackrel{\·}{\stackrel{\‾}{q}}=\frac{1}{2}\stackrel{\‾}{q}\⊗\stackrel{\‾}{\mathrm{\ω}}=\frac{1}{2}\stackrel{\‾}{q}\⊗\left[\begin{array}{c}0\\ \stackrel{\‾}{\mathrm{\ω}}\left(t^{-}\right)\end{array}\right]$

式中，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₁₃

横滚角主值

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

φ＝φ_主

如图10所示，本实施例的工作流程如下：

(1)系统初始化

硅微航姿系统在静态条件下正常启动。首先进行系统的初始化，包括导航计算机硬件自检，各接口初始化，对MIMU信号的检测，相应状态的设置，以及卡尔曼滤波器的初始化等。

(2)初始对准

静基座下，由MIMU输出信号经器件误差补偿后使用公式1和公式2解算出四元数初值与初始姿态矩阵

(3)器件误差补偿

定时采集到MIMU输出信号后，根据采集的环境温度和器件的温度误差模型完成器件的温度误差补偿；补偿器件的非正交安装误差、零位误差、刻度因数误差等。

(4)姿态矩阵更新

由经过误差补偿的陀螺测量角速度信号求解四元数姿态方程，根据姿态算法中的四元数方法完成四元数更新与姿态矩阵更新。

(5)重力、地磁干扰判断

判断重力、地磁向量是否被干扰，根据重力、地磁向量被干扰情况选取卡尔曼滤波器观测向量，建立相应的观测方程。

(6)姿态组合卡尔曼滤波模块

根据公式3～公式16，建立卡尔曼滤波器。

(7)四元数与陀螺零位误差修正

由卡尔曼滤波估计出的陀螺零位误差偏差和四元数误差及时修正陀螺零位误差和四元数，以便在下一循环中分别应用到流程(3)和流程(4)。

(8)四元数归一化处理

将上一流程(7)得到的修正后的四元数进行归一化处理。

(9)航姿角计算

将上一流程(8)经过归一化处理的四元数使用公式2得出姿态矩阵估计

再使用步骤(4)可以计算出航向姿态角。

(10)航姿角数据输出

系统根据外部设备的需要，设置航姿信息的格式、输出频率以及校验方式等。

Claims (3)

1.硅微航姿系统惯性/地磁组合方法，其特征是：

(1)利用微惯性测量组件MIMU中的硅微传感器感应载体运动：硅微陀螺敏感载体运动沿其轴向的角速度信号，硅微加速度计敏感载体运动沿其轴向的加速度及重力加速度信号，利用MIMU中的磁强计敏感地磁向量，并将信号送至导航计算机；

(2)利用加速度计和磁强计对重力向量和地磁向量的观测来确定载体的初始姿态：重力向量a和地磁向量m在地理坐标系n系的投影 $a^n=\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]$ 和 $m^n=\left[\begin{array}{c}{m}_{0x}^n\\ m_{0y}^n\\ m_{0z}^n\end{array}\right]$ 为已知常量，mⁿ为地磁场在当地地理坐标系n系中的投影向量，在载体坐标系b系中的投影为a^b和m^b，构造三个正交向量：

q＝a

r＝a×m

s＝r×a

它们在n系中的投影构成矩阵[

]，由aⁿ和mⁿ求得；在b系中的投影构成矩阵[

]，由a^b和m^b求得，

$\left\{\begin{array}{c}{q}^b=C_n^b\·q^n\\ r^b=C_n^b\·r^n\\ s^b=C_n^b\·s^n\end{array}\right.,$

$\left[q^b\begin{array}{c}\·\\ \·\\ \·\end{array}{r}^b\begin{array}{c}\·\\ \·\\ \·\end{array}{s}^b\right]=C_n^b\·\left[q^n\begin{array}{c}\·\\ \·\\ \·\end{array}{r}^n\begin{array}{c}\·\\ \·\\ \·\end{array}{s}^n\right]$

$C_n^b=\left[q^b\begin{array}{c}\·\\ \·\\ \·\end{array}{r}^b\begin{array}{c}\·\\ \·\\ \·\end{array}{s}^b\right]\·{\left[q^n\begin{array}{c}\·\\ \·\\ \·\end{array}{r}^n\begin{array}{c}\·\\ \·\\ \·\end{array}{s}^n\right]}^{-1}---\left(1\right)$

由此，求出姿态矩阵C_n ^b的估计

根据下面的两个公式求得Kalman滤波器的初值

$\left|{\hat{q}}_{01}\right|=\frac{1}{2}\sqrt{1+C_{11}-C_{22}-C_{33}}$ $\mathrm{sig}\left({\hat{q}}_{00}\right)=1$

$\left|{\hat{q}}_{02}\right|=\frac{1}{2}\sqrt{1-C_{11}+C_{22}-C_{33}}$ $\mathrm{sig}\left({\hat{q}}_{01}\right)=\mathrm{sign}(C_{32}-C_{23})$

$\left|{\hat{q}}_{03}\right|=\frac{1}{2}\sqrt{1-C_{11}-C_{22}+C_{33}}$ $\mathrm{sig}\left({\hat{q}}_{02}\right)=\mathrm{sign}(C_{13}-C_{31})$

$\left|{\hat{q}}_{00}\right|=\frac{1}{2}\sqrt{1+q_{01}^2-q_{02}^2-q_{03}^2}$ $\mathrm{sig}\left({\hat{q}}_{03}\right)=\mathrm{sign}(C_{21}-C_{12})$

则

(3)利用加速度计和磁强计对重力向量和地磁向量的观测来修正陀螺给出的姿态角，即把这两个参考向量在n系中的理论值通过陀螺给出的姿态角投影到b系中，与加速度计和磁强计的测量值求差作为观测量来完成对姿态的修正；本步骤包含如下步骤：

①建立卡尔曼滤波的状态方程：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(3\right)$

式中，X(t)是t时刻系统的状态向量，F(t)、G(t)分别为系统状态矩阵和噪声矩阵，W(t)为系统的噪声向量；

系统的状态矢量为：

$X=\left[\begin{array}{c}{q}_e\\ \mathrm{\ΔB}\end{array}\right]={\left[\begin{array}{cccccc}{q}_{e1}& q_{e2}& q_{e3}& {\mathrm{\ΔB}}_x& \mathrm{\Δ}{B}_y& \mathrm{\Δ}{B}_z\end{array}\right]}^T---\left(4\right)$

系统的状态噪声向量为：

$W\left(t\right)=\left(\begin{array}{c}{W}_1\left(t\right)\\ W_2\left(t\right)\end{array}\right)={\left(\begin{array}{cccccc}{\mathrm{\ω}}_{\mathrm{rx}}& {\mathrm{\ω}}_{\mathrm{ry}}& {\mathrm{\ω}}_{\mathrm{rz}}& {\mathrm{\ω}}_{\mathrm{gx}}& {\mathrm{\ω}}_{\mathrm{gy}}& {\mathrm{\ω}}_{\mathrm{gz}}\end{array}\right)}^T---\left(5\right)$

其中，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\left(t\right)=\left[\begin{array}{cc}{Q}_1& 0_{3\×3}\\ 0_{3\×3}& Q_2\end{array}\right]$ 为对角阵，Q₁的对角线元素根据陀螺噪声来确定，取为噪声RMS值的平方除以带宽f^*：

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

Q₂的对角线元素根据陀螺零位漂移的特点来确定，

系统的噪声系数矩阵为： $G\left(t\right)=\left[\begin{array}{cc}-\frac{1}{2}{I}_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& I_{3\×3}\end{array}\right]---\left(6\right)$

系统的状态转移矩阵为：

上式中，

为载体角速度向量在载体坐标系中的估计值，

为向量

的反对称矩阵；

②根据加速度计测量值判断系统加速运动情况和磁强计测量值判断地磁受干扰情况，如果重力向量和地磁向量没有受到干扰，转到步骤③；如果重力向量或地磁向量受到干扰，转到步骤④：

③建立卡尔曼滤波的观测方程，并转到步骤⑤：

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\left(t\right)=\left[\begin{array}{c}{V}_1\left(t\right)\\ V_2\left(t\right)\end{array}\right]---\left(10\right)$

{V(t)}为独立均值高斯白噪声过程，其协方差矩阵：

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

$R_d\left(t\right)=\left[\begin{array}{cc}{R}_{d1}& 0_{3\×3}\\ 0_{3\×3}& R_{d2}\end{array}\right]$ 为对角阵，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\left(t\right)=\left[\begin{array}{c}{V}_1\left(t\right)\\ V_3\left(t\right)\end{array}\right]---\left(10\right)$

V₃(t)为法向量的测量噪声，与V₁(t)和V₂(t)有关{V(t)}为独立均值高斯白噪声过程，其协方差矩阵：

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

$R_d\left(t\right)=\left[\begin{array}{cc}{R}_{d1}& 0_{3\×3}\\ 0_{3\×3}& R_{d3}\end{array}\right]$ 为对角阵

系统观测矩阵为：H(t)＝[H₁(t)0_6×3]

上式中，

(b)重力向量受到干扰的情况：

系统量测向量为：

观测噪声向量为： $V\left(t\right)=\left[\begin{array}{c}{V}_3\left(t\right)\\ V_2\left(t\right)\end{array}\right]---\left(10\right)$

{V(t)}为独立均值高斯白噪声过程，其协方差矩阵：

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

$R_d\left(t\right)=\left[\begin{array}{cc}{R}_{d3}& 0_{3\×3}\\ 0_{3\×3}& R_{d2}\end{array}\right]$ 为对角阵

系统观测矩阵为：H(t)＝[H₁(t)0_6×3]

上式中，

⑤根据扩展卡尔曼滤波递推方程和步骤①、③或④的结果，建立卡尔曼滤波的时间传播方程：

式中ω^b＝[ω_x ω_y ω_z]^T为当前时刻三轴陀螺的输出；

为上一时刻更新后的陀螺零偏估计；Δt为计算步长；

$\stackrel{\·}{\stackrel{\‾}{q}}=\frac{1}{2}\stackrel{\‾}{q}\text{\⊗}\stackrel{\‾}{\mathrm{\ω}}=\frac{1}{2}\stackrel{\‾}{q}\⊗\left[\begin{array}{c}0\\ \stackrel{\‾}{\mathrm{\ω}}\left(t^{-}\right)\end{array}\right]$

式中，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₁₃

横滚角主值

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

φ＝φ_主

2.如权利要求1所述的硅微航姿系统惯性/地磁组合方法，其特征是：步骤(3)中步骤③或步骤④卡尔曼滤波器观测方程的构造，量测向量Z在载体坐标b系中构造。

3.如权利要求1所述的硅微航姿系统惯性/地磁组合方法，其特征是：重力向量或地磁向量受到干扰时，采用步骤(3)中步骤④的卡尔曼滤波器观测方程。

Images