一种地磁场模型辅助的载体姿态和航向计算方法和系统

摘要

本发明公开一种地磁场模型辅助的载体姿态和航向计算方法和系统，该方法包括以下步骤：根据载体的位置信息和地磁场模型，计算理想磁倾角；根据加速度计的输出值和磁传感器的输出值，计算估计磁倾角；根据理想磁倾角和估计磁倾角，判断是否存在环境磁干扰；当存在环境磁干扰时，根据磁传感器量测值的自适应权重，构建磁传感器量测值的等价权矩阵；根据加速度计量测值、磁传感器量测值和磁传感器量测值的等价权矩阵，计算载体姿态信息和航向信息。本发明根据理想磁倾角和估计磁倾角检测环境磁干扰，并在检测到环境磁干扰时，构建磁传感器量测值的等价权矩阵，并实现磁传感器量测值对状态估计值贡献的自适应调整，使计算得到的姿态信息和航向信息具有抗差性。

说明书

技术领域

本发明涉及通信技术领域，特别涉及一种地磁场模型辅助的载体姿态和航向计算 方法和系统。

背景技术

姿态和航向参考系统(AttitudeandHeadingReferenceSystem，简称AHRS)是 一种可以精确测量载体在空间坐标系下三轴姿态的测量装置，由三轴磁强计、三轴陀螺仪 和三轴加速度计等硬件和相应的软件构成，为载体提供实时的姿态和航向信息，在航空航 天、机器人、汽车工业以及行人导航定位领域得到广泛的应用。随着微机电技术的发展，基 于微机电系统传感器的姿态和航向参考系统应用更广泛。但由于陀螺仪存在漂移，导致在 计算载体姿态和航向信息，其误差不断累积。为解决此问题，在姿态和航向参考系统中引入 加速度计和磁传感器的信息，并以重力场和地磁场作为参考矢量，利用加速计和磁强计输 出值与陀螺仪的输出值实时计算载体的姿态和航向信息。

虽然姿态和航向参考系统可为载体实时提供精确的姿态和航向信息，但在近地运 动的载体经常受到外部环境的干扰，特别是磁传感器受环境磁干扰明显。当受到外部磁干 扰时，利用磁传感器输出值作为量测值估计载体的姿态和航向信息存在误差，甚至不能正 确计算载体的姿态和航向信息，从而给载体的自主控制带来很大的风险。

发明内容

本发明提供了一种地磁场模型辅助的载体姿态和航向计算方法和系统，以解决存 在环境磁干扰时载体姿态和航向实时计算的技术问题。

本发明提供了一种地磁场模型辅助的载体姿态和航向计算方法，包括以下步骤：

根据载体的位置信息和地磁场模型，计算理想磁倾角；根据加速度计的输出值和 磁传感器的输出值，计算估计磁倾角；

根据所述理想磁倾角和所述估计磁倾角，判断是否存在环境磁干扰；

当存在环境磁干扰时，根据磁传感器量测值的归一化残差和量测值协方差矩阵， 构建所述磁传感器量测值的自适应权重，并根据所述磁传感器量测值的自适应权重，构建 所述磁传感器量测值的等价权矩阵；

根据所述加速度计量测值、所述磁传感器量测值和所述磁传感器量测值的等价权 矩阵，计算载体姿态信息和航向信息。

可选地，根据所述加速度计量测值、所述磁传感器量测值和所述磁传感器量测值 的等价权矩阵，计算所述载体姿态信息和航向信息，具体包括：

根据所述加速度计量测值更新状态预测值，得到状态更新值，并使用所述状态更 新值更新姿态四元数的估计值；

根据所述磁传感器量测值和所述磁传感器量测值的等价权矩阵，更新所述状态更 新值，得到状态估计值，并使用所述状态估计值更新所述姿态四元数的估计值；

根据所述姿态四元数的估计值以及从导航坐标系到载体坐标系的转化矩阵，计算 所述载体姿态信息和航向信息。

可选地，根据所述加速度计量测值更新状态预测值，得到状态更新值，具体包括：

根据惯性传感器误差模型和姿态误差四元数更新模型，构建Kalman滤波的状态方 程；根据所述加速度计的输出值以及根据重力场参考矢量估计的所述加速度计的输出值， 构建第一量测方程；

根据所述Kalman滤波的状态方程、所述第一量测方程以及所述加速度计量测值更 新状态预测值，得到状态更新值。

可选地，根据所述磁传感器量测值和所述磁传感器量测值的等价权矩阵，更新所 述状态更新值，得到状态估计值，具体包括：

根据惯性传感器误差模型和姿态误差四元数更新模型，构建Kalman滤波的状态方 程；根据所述磁传感器的输出值以及根据世界地磁场模型估计的所述磁传感器的输出值， 构建第二量测方程；

根据所述Kalman滤波的状态方程、所述第二量测方程、所述磁传感器量测值以及 所述磁传感器量测值的等价权矩阵，更新状态更新值，得到状态估计值。

可选地，所述Kalman滤波的状态方程，具体为：

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

$F\left(t\right)=\left(\begin{array}{ccc}-[{\hat{\omega }}^b\times ]& -0.5I& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right)$

$W\left(t\right)=\left(\begin{array}{c}{-0.5v}_{\omega }\\ v_{b_{\omega }}\\ v_{b_a}\end{array}\right)$

其中，X(t)＝[q_eb_ωb_a]^T∈R^9×1为状态量，T为转置，F(t)为状态转移矩阵，W(t)为 过程噪声，为陀螺仪输出值的反对矩阵，v_ω为陀螺仪的随机漂移噪声，为陀螺仪 随机常值模型噪声，为加速度计随机常值模型噪声，q_e为误差四元数的矢量部分，b_ω为陀 螺仪随机常值，b_a为加速度计偏差。

可选地，所述第一量测方程，具体为：

$L_a={\hat{a}}^b-C_n^b\left(\hat{q}\right)g^n=2[C_n^b\left(\hat{q}\right)g^n\times ]q_e+b_a+v_a$

其中，L_a为所述加速度计的输出值与根据重力场参考矢量估计的所述加速度计的 输出值之间的差值，为加速度计的输出值，为从导航坐标系到载体坐标系的转化矩阵， 为姿态四元数的估计值，gⁿ为重力加速度在导航坐标系下的投影，v_a为加速度计的随机误 差。

可选地，根据所述Kalman滤波的状态方程、所述第一量测方程式以及所述加速度 计量测值更新状态预测值，得到状态更新值，具体为：

采用以下公式更新状态预测值，得到状态更新值：

${\hat{X}}_{a,k}={\bar{X}}_k+K_{a,k}(L_{a,k}-A_{a,k}{\bar{X}}_k)$

$K_{a,k}={\Sigma }_{{\bar{X}}_k}{A}_{a,k}^T{(A_{a,k}^T{\Sigma }_{{\bar{X}}_k}{A}_{a,k}+R_a)}^{-1}$

${\Sigma }_{{\hat{X}}_{a,k}}=\left({I-K}_{a,k}{A}_{a,k}\right){\Sigma }_{{\bar{X}}_k}{\left({I-K}_{a,k}{A}_{a,k}\right)}^T+K_{a,k}{R}_a{K}_{a,k}^T$

其中，为k时刻的状态更新值，为k时刻的状态预测值，L_a，k为k时刻的所述加 速度计量测值，A_a，k为所述第一量测方程在k时刻的量测矩阵，I为单位阵，K_a，k为增益矩阵， 为的协方差矩阵，R_a为加速度计量测噪声的协方差矩阵；为k时刻的协方差矩 阵。

可选地，使用所述状态更新值更新姿态四元数的估计值，具体为：

采用以下公式，更新姿态四元数的估计值：

$\hat{q}\Leftarrow \hat{q}\otimes q_e$

$\hat{q}\Leftarrow \hat{q}/\left|\right|\hat{q}\left|\right|$

$q_e={\hat{X}}_{a,k}(1:3)$

其中，为所述姿态四元数的估计值，为四元数乘法，为的归一化，为赋值 符号，q_e为所述状态更新值的分量，为所述状态更新值。

可选地，所述第二量测方程，具体为：

$L_m={\hat{m}}^b-C_n^b\left(\hat{q}\right)m^n=2[C_n^b\left(\hat{q}\right)g^n\times ]q_e+v_m$

其中，L_m为所述磁传感器的输出值与根据世界地磁场模型估计的所述磁传感器的 输出值之间的差值，为磁传感器的输出值，为从导航坐标系到载体坐标系的转化矩阵， 为姿态四元数的估计值，mⁿ为理想地磁场输出值的归一化结果，v_m磁传感器的随机误差。

可选地，根据所述Kalman滤波的状态方程、所述第二量测方程、所述磁传感器量测 值以及所述磁传感器量测值的等价权矩阵，更新状态更新值，得到状态估计值，具体为：

采用以下公式更新状态更新值，得到状态估计值：

${\hat{X}}_k={\hat{X}}_{a,k}+K_{m,k}(L_{m,k}-A_{m,k}{\hat{X}}_{a,k})$

${\Sigma }_{{\hat{X}}_k}=\left({I-K}_{m,k}{A}_{m,k}\right){\Sigma }_{{\hat{X}}_{a,k}}{\left({I-K}_{m,k}{A}_{m,k}\right)}^T+K_{m,k}{{\bar{P}}_{m,k}}^{-1}{K}_{m,k}^T$

$K_{m,k}=\left(\begin{array}{cc}{r}_3{r}_3^T& 0_{3\times 6}\\ 0_{6\times 3}& 0_{6\times 6}\end{array}\right){\Sigma }_{{\bar{X}}_{m,k}}{A}_{m,k}^T{(A_{m,k}^T{\Sigma }_{{\bar{X}}_{m,k}}{A}_{m,k}+{{\bar{P}}_{m,k}}^{-1})}^{-1}$

${\Sigma }_{{\bar{X}}_{m,k}}=\left(\begin{array}{cc}{\Sigma }_{{\hat{X}}_{a,k}}(1:\mathrm{3,1}:3)& 0_{3\times 6}\\ 0_{6\times 3}& 0_{6\times 6}\end{array}\right)$

$r_3=C_n^b\left(\hat{q}\right){\left(\begin{array}{ccc}0& 0& 1\end{array}\right)}^T$

其中，为k时刻的状态估计值，为k时刻的状态更新值，L_m，k为k时刻的所述磁 传感器量测值，为的协方差矩阵，K_m，k为k时刻的增益矩阵，A_m，k为k时刻的量测矩阵， 为k时刻的所述磁传感器量测值的等价权矩阵。

可选地，使用所述状态估计值更新所述姿态四元数的估计值，具体为：

采用以下公式，更新姿态四元数的估计值：

$\hat{q}\Leftarrow \hat{q}\otimes q_e$

$\hat{q}\Leftarrow \hat{q}/\left|\right|\hat{q}\left|\right|$

$q_e={\hat{X}}_a(1:3)$

其中，为所述姿态四元数的估计值，为四元数乘法，为的归一化，为赋值 符号，q_e为所述状态估计值的分量，为所述状态估计值。

可选地，根据加速度计的输出值和磁传感器的输出值，计算估计磁倾角，具体为：

采用以下公式计算估计磁倾角：

D_cacul＝arccos(n_a·n_m)

其中，D_cacul为所述估计磁倾角，n_a为所述加速度计在载体系输出值的方向矢量，n_m为所述磁传感器在载体系输出值的方向矢量。

可选地，根据所述理想磁倾角和所述估计磁倾角，判断是否存在环境磁干扰，具体 为：

判断所述理想磁倾角和所述估计磁倾角是否满足以下条件：

|D_cacul-D_reference|＞λ_D

其中，D_reference为所述理想磁倾角，D_cacul为所述估计磁倾角，λ_D为预设阈值；

如果满足上述条件，则确定存在环境磁干扰；否则，确定不存在环境磁干扰。

可选地，根据磁传感器量测值的归一化残差和量测值协方差矩阵，构建所述磁传 感器量测值的自适应权重，具体为：

采用以下公式构建所述磁传感器量测值的自适应权重：

不存在环境磁干扰时；

存在环境磁干扰时；

其中，为k时刻所述磁传感器第i个量测值的自适应权重，p_ki为量测值协方差矩 阵的第i个主对角线元素，c为常数，V_i′为磁传感器量测值的归一化残差。

可选地，根据磁传感器量测值的归一化残差和量测值协方差矩阵，构建所述磁传 感器量测值的自适应权重之前，还包括：

采用以下公式计算所述磁传感器量测值的归一化残差：

${V_{i,k}}^{\prime }=(L_{\mathrm{mi},k}-A_{\mathrm{mi},k}{\bar{X}}_k)/\left|{\sigma }_i\right|,i=\mathrm{1,2,3}$

其中，L_mi为所述第二量测方程的第i个测量方程，A_mi为量测矩阵的第i行；|σ_i|为磁传感器量测噪声协方差矩阵R_m第i个主对角线元素的平方根。

本发明还提供了一种姿态和航向参考系统，包括：

第一计算模块，用于根据载体的位置信息和地磁场模型，计算理想磁倾角；

第二计算模块，用于根据加速度计的输出值和磁传感器的输出值，计算估计磁倾 角；

判断模块，用于根据所述理想磁倾角和所述估计磁倾角，判断是否存在环境磁干 扰；

第一构建模块，用于在所述判断模块判断出存在环境磁干扰时，根据磁传感器量 测值的归一化残差和量测值协方差矩阵，构建所述磁传感器量测值的自适应权重；

第二构建模块，用于根据所述磁传感器量测值的自适应权重，构建所述磁传感器 量测值的等价权矩阵；

第三计算模块，用于根据所述加速度计量测值、所述磁传感器量测值和所述磁传 感器量测值的等价权矩阵，计算载体姿态信息和航向信息。

可选地，所述第三计算模块，包括：

第一更新子模块，用于根据所述加速度计量测值更新状态预测值，得到状态更新 值；

第二更新子模块，用于使用所述状态更新值更新姿态四元数的估计值；

第三更新子模块，用于根据所述磁传感器量测值和所述磁传感器量测值的等价权 矩阵，更新所述状态更新值，得到状态估计值；

第四更新子模块，用于使用所述状态估计值更新所述姿态四元数的估计值；

计算子模块，用于根据所述姿态四元数的估计值以及从导航坐标系到载体坐标系 的转化矩阵，计算所述载体姿态信息和航向信息。

可选地，所述第一更新子模块，具体用于根据惯性传感器误差模型和姿态误差四 元数更新模型，构建Kalman滤波的状态方程；根据所述加速度计的输出值以及根据重力场 参考矢量估计的所述加速度计的输出值，构建第一量测方程；根据所述Kalman滤波的状态 方程、所述第一量测方程以及所述加速度计量测值更新状态预测值，得到状态更新值。

可选地，所述第三更新子模块，具体用于根据惯性传感器误差模型和姿态误差四 元数更新模型，构建Kalman滤波的状态方程；根据所述磁传感器的输出值以及根据世界地 磁场模型估计的所述磁传感器的输出值，构建第二量测方程；根据所述Kalman滤波的状态 方程、所述第二量测方程、所述磁传感器量测值以及所述磁传感器量测值的等价权矩阵，更 新状态更新值，得到状态估计值。

可选地，所述Kalman滤波的状态方程，具体为：

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

$F\left(t\right)=\left(\begin{array}{ccc}-[{\hat{\omega }}^b\times ]& -0.5I& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right)$

$W\left(t\right)=\left(\begin{array}{c}{-0.5v}_{\omega }\\ v_{b_{\omega }}\\ v_{b_a}\end{array}\right)$

其中，X(t)＝[q_eb_ωb_a]^T∈R^9×1为状态量，T为转置，F(t)为状态转移矩阵，W(t)为 过程噪声，为陀螺仪输出值的反对矩阵，v_ω为陀螺仪的随机漂移噪声，为陀螺仪 随机常值模型噪声，为加速度计随机常值模型噪声，q_e为误差四元数的矢量部分，b_ω为陀 螺仪随机常值，b_a为加速度计偏差。

可选地，所述第一量测方程，具体为：

$L_a={\hat{a}}^b-C_n^b\left(\hat{q}\right)g^n=2[C_n^b\left(\hat{q}\right)g^n\times ]q_e+b_a+v_a$

其中，L_a为所述加速度计的输出值与根据重力场参考矢量估计的所述加速度计的 输出值之间的差值，为加速度计的输出值，为从导航坐标系到载体坐标系的转化矩阵， 为姿态四元数的估计值，gⁿ为重力加速度在导航坐标系下的投影，v_a为加速度计的随机误 差。

可选地，所述第一更新子模块，具体用于根据惯性传感器误差模型和姿态误差四 元数更新模型，构建Kalman滤波的状态方程；根据所述加速度计的输出值以及根据重力场 参考矢量估计的所述加速度计的输出值，构建第一量测方程，并采用以下公式更新状态预 测值，得到状态更新值：

${\hat{X}}_{a,k}={\bar{X}}_k+K_{a,k}(L_{a,k}-A_{a,k}{\bar{X}}_k)$

$K_{a,k}={\Sigma }_{{\bar{X}}_k}{A}_{a,k}^T{(A_{a,k}^T{\Sigma }_{{\bar{X}}_k}{A}_{a,k}+R_a)}^{-1}$

${\Sigma }_{{\hat{X}}_{a,k}}=\left({I-K}_{a,k}{A}_{a,k}\right){\Sigma }_{{\bar{X}}_k}{\left({I-K}_{a,k}{A}_{a,k}\right)}^T+K_{a,k}{R}_a{K}_{a,k}^T$

其中，为k时刻的状态更新值，为k时刻的状态预测值，L_a，k为k时刻的所述加 速度计量测值，A_a，k为所述第一量测方程在k时刻的量测矩阵，I为单位阵，K_a，k为增益矩阵， 为的协方差矩阵，R_a为加速度计量测噪声的协方差矩阵；为k时刻的协方差矩 阵。

可选地，所述第二更新子模块，具体用于采用以下公式，更新姿态四元数的估计 值：

$\hat{q}\Leftarrow \hat{q}\otimes q_e$

$\hat{q}\Leftarrow \hat{q}/\left|\right|\hat{q}\left|\right|$

$q_e={\hat{X}}_{a,k}(1:3)$

其中，为所述姿态四元数的估计值，为四元数乘法，为的归一化，为赋值 符号，q_e为所述状态更新值的分量，为所述状态更新值。

可选地，所述第二量测方程，具体为：

$L_m={\hat{m}}^b-C_n^b\left(\hat{q}\right)m^n=2[C_n^b\left(\hat{q}\right)g^n\times ]q_e+v_m$

其中，L_m为所述磁传感器的输出值与根据世界地磁场模型估计的所述磁传感器的 输出值之间的差值，为磁传感器的输出值，为从导航坐标系到载体坐标系的转化矩阵， 为姿态四元数的估计值，mⁿ为理想地磁场输出值的归一化结果，v_m磁传感器的随机误差。

可选地，所述第三更新子模块，具体用于根据惯性传感器误差模型和姿态误差四 元数更新模型，构建Kalman滤波的状态方程；根据所述磁传感器的输出值以及根据世界地 磁场模型估计的所述磁传感器的输出值，构建第二量测方程，并采用以下公式更新状态更 新值，得到状态估计值：

${\hat{X}}_k={\hat{X}}_{a,k}+K_{m,k}(L_{m,k}-A_{m,k}{\hat{X}}_{a,k})$

${\Sigma }_{{\hat{X}}_k}=\left({I-K}_{m,k}{A}_{m,k}\right){\Sigma }_{{\hat{X}}_{a,k}}{\left({I-K}_{m,k}{A}_{m,k}\right)}^T+K_{m,k}{{\bar{P}}_{m,k}}^{-1}{K}_{m,k}^T$

$K_{m,k}=\left(\begin{array}{cc}{r}_3{r}_3^T& 0_{3\times 6}\\ 0_{6\times 3}& 0_{6\times 6}\end{array}\right){\Sigma }_{{\bar{X}}_{m,k}}{A}_{m,k}^T{(A_{m,k}^T{\Sigma }_{{\bar{X}}_{m,k}}{A}_{m,k}+{{\bar{P}}_{m,k}}^{-1})}^{-1}$

${\Sigma }_{{\bar{X}}_{m,k}}=\left(\begin{array}{cc}{\Sigma }_{{\hat{X}}_{a,k}}(1:\mathrm{3,1}:3)& 0_{3\times 6}\\ 0_{6\times 3}& 0_{6\times 6}\end{array}\right)$

$r_3=C_n^b\left(\hat{q}\right){\left(\begin{array}{ccc}0& 0& 1\end{array}\right)}^T$

其中，为k时刻的状态估计值，为k时刻的状态更新值，L_m，k为k时刻的所述磁 传感器量测值，为的协方差矩阵，K_m，k为k时刻的增益矩阵，A_m，k为k时刻的量测矩阵， 为k时刻的所述磁传感器量测值的等价权矩阵。

可选地，所述第四更新子模块，具体用于采用以下公式，更新姿态四元数的估计 值：

$\hat{q}\Leftarrow \hat{q}\otimes q_e$

$\hat{q}\Leftarrow \hat{q}/\left|\right|\hat{q}\left|\right|$

$q_e={\hat{X}}_a(1:3)$

其中，为所述姿态四元数的估计值，为四元数乘法，为的归一化，为赋值 符号，q_e为所述状态估计值的分量，为所述状态估计值。

可选地，所述第二计算模块，具体用于采用以下公式计算估计磁倾角：

D_cacul＝arccos(n_a·n_m)

其中，D_cacul为所述估计磁倾角，n_a为所述加速度计在载体系输出值的方向矢量，n_m为所述磁传感器在载体系输出值的方向矢量。

可选地，所述判断模块，具体用于判断所述理想磁倾角和所述估计磁倾角是否满 足以下条件：

|D_cacul-D_reference|＞λ_D

其中，D_reference为所述理想磁倾角，D_cacul为所述估计磁倾角，λ_D为预设阈值；

如果满足上述条件，则确定存在环境磁干扰；否则，确定不存在环境磁干扰。

可选地，所述第一构建模块，具体用于在所述判断模块判断出存在环境磁干扰时， 采用以下公式构建所述磁传感器量测值的自适应权重：

不存在环境磁干扰时；

存在环境磁干扰时；

其中，为k时刻所述磁传感器第i个量测值的自适应权重，p_ki为量测值协方差矩 阵的第i个主对角线元素，c为常数，V_i′为磁传感器量测值的归一化残差。

可选地，所述的系统，还包括：

第四计算模块，用于采用以下公式计算所述磁传感器量测值的归一化残差：

${V_{i,k}}^{\prime }=(L_{\mathrm{mi},k}-A_{\mathrm{mi},k}{\bar{X}}_k)/\left|{\sigma }_i\right|,i=\mathrm{1,2,3}$

其中，L_mi为所述第二量测方程的第i个测量方程，A_mi为量测矩阵的第i行；|σ_i|为磁传感器量测噪声协方差矩阵R_m第i个主对角线元素的平方根。

本发明根据理想磁倾角和估计磁倾角检测环境磁干扰，并在检测到环境磁干扰 时，构建磁传感器量测值的等价权矩阵，并实现磁传感器量测值对状态估计值贡献的自适 应调整，使计算得到的姿态信息和航向信息具有抗差性，能够适应地磁场畸变环境，并解决 存在环境磁干扰时载体姿态和航向实时计算的技术问题。

附图说明

图1为本发明实施例中的一种地磁场模型辅助的载体姿态和航向计算方法流程 图；

图2为本发明实施例中的另一种地磁场模型辅助的载体姿态和航向计算方法流程 图；

图3为本发明实施例中的一种姿态和航行参考系统的结构示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完 整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于 本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他 实施例，都属于本发明保护的范围。

本发明实施例提供了一种地磁场模型辅助的载体姿态和航向计算方法，如图1所 示，包括以下步骤：

步骤101，根据载体的位置信息和地磁场模型，计算理想磁倾角；根据加速度计的 输出值和磁传感器的输出值，计算估计磁倾角。

步骤102，根据理想磁倾角和估计磁倾角，判断是否存在环境磁干扰。

步骤103，当存在环境磁干扰时，根据磁传感器量测值的归一化残差和量测值协方 差矩阵，构建磁传感器量测值的自适应权重，并根据磁传感器量测值的自适应权重，构建磁 传感器量测值的等价权矩阵。

步骤104，根据加速度计量测值、磁传感器量测值和磁传感器量测值的等价权矩 阵，计算载体姿态信息和航向信息。

本发明实施例根据理想磁倾角和估计磁倾角检测环境磁干扰，并在检测到环境磁 干扰时，构建磁传感器量测值的等价权矩阵，并实现磁传感器量测值对状态估计值贡献的 自适应调整，使计算得到的姿态信息和航向信息具有抗差性，能够适应地磁场畸变环境，并 解决存在环境磁干扰时载体姿态和航向实时计算的技术问题。

本发明实施例还提供了另一种地磁场模型辅助的载体姿态和航向计算方法，如图 2所示，包括以下步骤：

步骤201，根据载体的位置信息和地磁场模型，计算理想地磁场的总磁场强度、磁 倾角和磁偏角，将理想地磁场的磁倾角作为理想磁倾角。

具体地，可以根据美国国家地球物理信息中心(NGDC)和英国地质测量局(BGS)制 作，并由美国国家地理空间情报局(NGA)发布的世界地磁模型(WorldMagneticModel，简 称WMM)以及载体运动的轨迹数据，实时计算载体所在位置处的地磁场信息，该地磁场信息 包括总磁场强度、磁偏角和磁倾角。

步骤202，根据加速度计的输出值和磁传感器的输出值，计算估计磁倾角。

具体地，可以采用以下公式计算估计磁倾角：

D_cacul＝arccos(n_a·n_m)(1)

其中，D_cacul为估计磁倾角，n_a和n_m分别为加速度计和磁传感器在载体系输出值的 方向矢量。

步骤203，根据理想磁倾角和估计磁倾角，判断是否存在环境磁干扰。

具体地，可以判断理想磁倾角和估计磁倾角是否满足以下条件：

|D_cacul-D_reference|＞λ_D(2)

其中，D_reference为理想磁倾角，D_cacul为估计磁倾角，λ_D为预设阈值。

如果满足上述条件，则确定存在环境磁干扰；否则，确定不存在环境磁干扰。

本实施例中，λ_D＝0.55°。

步骤204，根据惯性传感器误差模型和姿态误差四元数更新模型，构建Kalman滤波 的状态方程。

具体地，可以取误差四元数的矢量部分q_e、陀螺仪随机常值b_ω和加速度计偏差b_a作 为状态量，构建Kalman滤波的状态方程：

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

$F\left(t\right)=\left(\begin{array}{ccc}-[{\hat{\omega }}^b\times ]& -0.5I& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right)---\left(4\right)$

$W\left(t\right)=\left(\begin{array}{c}{-0.5v}_{\omega }\\ v_{b_{\omega }}\\ v_{b_a}\end{array}\right)---\left(5\right)$

其中，X(t)＝[q_eb_ωb_a]^T∈R^9×1为状态量；T为转置；F(t)为状态转移矩阵；W(t)为 过程噪声；为陀螺仪输出值的反对矩阵；v_ω为陀螺仪的随机漂移噪声；为陀螺仪 随机常值模型噪声；为加速度计随机常值模型噪声；

步骤205，根据地球重力场参考向量和地磁场参考向量在载体坐标系的投影值分 别与加速度计和磁传感器实测值之差建立量测方程。

具体地，利用加速度计的输出值与加速度计输出值的估计值的差L_a以及磁传感器 输出值和磁传感器输出值的估计值的差L_m分别构建第一量测方程和第二量测方程：

$L_a={\hat{a}}^b-C_n^b\left(\hat{q}\right)g^n=2[C_n^b\left(\hat{q}\right)g^n\times ]q_e+b_a+v_a---\left(6\right)$

$L_m={\hat{m}}^b-C_n^b\left(\hat{q}\right)m^n=2[C_n^b\left(\hat{q}\right)g^n\times ]q_e+v_m---\left(7\right)$

其中，为加速度计的输出值；为磁传感器的输出值；为从导航坐标系n到载 体坐标系b的转化矩阵；为四元数的估计值；gⁿ＝[00-g]^T为重力加速度g在导航坐标系n 下的投影；mⁿ＝[0cosαsinα]^T为理想地磁场输出值的归一化结果；α为磁倾角；v_a和v_m分别 为加速度计和磁力计的随机误差。

本实施例中，gⁿ＝9.8m/s2。

步骤206，采用两步观测更新的Kalman滤波算法估计四元数误差和惯性传感器误 差，通过姿态四元数更新方程实现载体的姿态和航向估计。

具体地，根据式(6)，利用在k时刻的观测值L_a，k更新k时刻的状态预测值

${\hat{X}}_{a,k}={\bar{X}}_k+K_{a,k}(L_{a,k}-A_{a,k}{\bar{X}}_k)---\left(8\right)$

$K_{a,k}={\Sigma }_{{\bar{X}}_k}{A}_{a,k}^T{(A_{a,k}^T{\Sigma }_{{\bar{X}}_k}{A}_{a,k}+R_a)}^{-1}---\left(9\right)$

${\Sigma }_{{\hat{X}}_{a,k}}=\left({I-K}_{a,k}{A}_{a,k}\right){\Sigma }_{{\bar{X}}_k}{\left({I-K}_{a,k}{A}_{a,k}\right)}^T+K_{a,k}{R}_a{K}_{a,k}^T---\left(10\right)$

其中，为量测方程式(6)在k时刻的量测矩阵；I为单位阵； 为在k时刻状态预测值的更新值；K_a，k为增益矩阵；为在k时刻状态预测值的协 方差矩阵；R_a为加速度计量测噪声的协方差矩阵；为在k时刻状态的协方差矩阵；

本实施例中，R_a＝(500μg)²I。

利用状态量中的分量更新

$\hat{q}\Leftarrow \hat{q}\otimes q_e---\left(11\right)$

$\hat{q}\Leftarrow \hat{q}/\left|\right|\hat{q}\left|\right|---\left(12\right)$

其中，符号为四元数乘法；为四元数的归一化；为赋值符号，将等式右侧的 计算值赋给等式左侧的变量；

根据式(7)，利用在k时刻的量测值L_m，k更新k时刻的获得k时刻的状态估计值 和状态估计值的协方差矩阵

${\hat{X}}_k={\hat{X}}_{a,k}+K_{m,k}(L_{m,k}-A_{m,k}{\hat{X}}_{a,k})---\left(13\right)$

${\Sigma }_{{\hat{X}}_k}=\left({I-K}_{m,k}{A}_{m,k}\right){\Sigma }_{{\hat{X}}_{a,k}}{\left({I-K}_{m,k}{A}_{m,k}\right)}^T+K_{m,k}{{\bar{P}}_{m,k}}^{-1}{K}_{m,k}^T---\left(14\right)$

$K_{m,k}=\left(\begin{array}{cc}{r}_3{r}_3^T& 0_{3\times 6}\\ 0_{6\times 3}& 0_{6\times 6}\end{array}\right){\Sigma }_{{\bar{X}}_{m,k}}{A}_{m,k}^T{(A_{m,k}^T{\Sigma }_{{\bar{X}}_{m,k}}{A}_{m,k}+{{\bar{P}}_{m,k}}^{-1})}^{-1}---\left(15\right)$

${\Sigma }_{{\bar{X}}_{m,k}}=\left(\begin{array}{cc}{\Sigma }_{{\hat{X}}_{a,k}}(1:\mathrm{3,1}:3)& 0_{3\times 6}\\ 0_{6\times 3}& 0_{6\times 6}\end{array}\right)---\left(16\right)$

其中，K_m，k为k时刻的增益矩阵；L_m，k为k时刻的由磁传感器计算的量测值；A_m，k为k时 刻的量测矩阵；为k时刻磁传感器量测值的等价权矩阵；

根据式(11)和式(12)利用状态量中的分量来更新并利用来 计算载体的姿态和航向信息。

步骤207，根据步骤203中的环境磁干扰的检测结果，利用等价权矩阵在线调整磁 传感器量测值的权重，使计算出的载体姿态和航向信息具有抗差性。

根据量测方程式(7)计算磁传感器观测值的归一化残差为

${V_{i,k}}^{\prime }=(L_{\mathrm{mi},k}-A_{\mathrm{mi},k}{\bar{X}}_k)/\left|{\sigma }_i\right|,i=\mathrm{1,2,3}---\left(17\right)$

其中，L_mi为量测方程式(7)的第i个测量方程；A_mi为量测矩阵的第i行；|σ_i|为磁传感器量测噪声协方差矩阵R_m第i个主对角线元素的平方根；

本实施例中，R_m＝0.001I；

根据环境磁干扰检测准则和磁传感器量测值的归一化残差构建量测值权重因子 为

式中，为在k时刻磁传感器第i个量测值的自适应权重；p_ki为给定量测值协方差 矩阵的第i个主对角线元素；c为常数；

本实施例中，c＝1.3-2.0。

本发明实施例利用k时刻磁传感器量测值的自适应权重构建磁传 感器量测值的等价权矩阵并实现磁传感器量测值对状态估计值贡献的自适应调整，使 姿态和航向信息计算具有抗差性。

本发明的优点在于：将姿态和航向参考系统信息计算的过程采用两步滤波的方法 来实现，第一步以加速度计为量测传感器来估计四元数的误差并更新四元数；第二步以磁 传感器为量测传感器估计四元数的误差，并利用磁干扰检测算法实时检测载体所处环境的 磁干扰，通过量测残差和实际残差的比值自适应调节磁传感器观测数据在滤波估计中的权 重，更新四元数并计算载体的姿态和航向参考系统信息。提高了载体姿态和航向参考系统 信息的抗差性和可靠性。

基于上述地磁场模型辅助的载体姿态和航向计算方法，本发明实施例还提供了一 种姿态和航行参考系统，如图3所示，包括：

第一计算模块310，用于根据载体的位置信息和地磁场模型，计算理想磁倾角；

第二计算模块320，用于根据加速度计的输出值和磁传感器的输出值，计算估计磁 倾角；

判断模块330，用于根据理想磁倾角和估计磁倾角，判断是否存在环境磁干扰；

第一构建模块340，用于在判断模块330判断出存在环境磁干扰时，根据磁传感器 量测值的归一化残差和量测值协方差矩阵，构建磁传感器量测值的自适应权重；

第二构建模块350，用于根据磁传感器量测值的自适应权重，构建磁传感器量测值 的等价权矩阵；

第三计算模块360，用于根据加速度计量测值、磁传感器量测值和磁传感器量测值 的等价权矩阵，计算载体姿态信息和航向信息。

其中，第三计算模块360，包括：

第一更新子模块，用于根据所述加速度计量测值更新状态预测值，得到状态更新 值；

第二更新子模块，用于使用所述状态更新值更新姿态四元数的估计值；

第三更新子模块，用于根据所述磁传感器量测值和所述磁传感器量测值的等价权 矩阵，更新所述状态更新值，得到状态估计值；

第四更新子模块，用于使用所述状态估计值更新所述姿态四元数的估计值；

计算子模块，用于根据所述姿态四元数的估计值以及从导航坐标系到载体坐标系 的转化矩阵，计算所述载体姿态信息和航向信息。

具体地，上述第一更新子模块，具体用于根据惯性传感器误差模型和姿态误差四 元数更新模型，构建Kalman滤波的状态方程；根据所述加速度计的输出值以及根据重力场 参考矢量估计的所述加速度计的输出值，构建第一量测方程；根据所述Kalman滤波的状态 方程、所述第一量测方程以及所述加速度计量测值更新状态预测值，得到状态更新值。

上述第三更新子模块，具体用于根据惯性传感器误差模型和姿态误差四元数更新 模型，构建Kalman滤波的状态方程；根据所述磁传感器的输出值以及根据世界地磁场模型 估计的所述磁传感器的输出值，构建第二量测方程；根据所述Kalman滤波的状态方程、所述 第二量测方程、所述磁传感器量测值以及所述磁传感器量测值的等价权矩阵，更新状态更 新值，得到状态估计值。

其中，Kalman滤波的状态方程，具体为：

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

$F\left(t\right)=\left(\begin{array}{ccc}-[{\hat{\omega }}^b\times ]& -0.5I& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right)$

$W\left(t\right)=\left(\begin{array}{c}{-0.5v}_{\omega }\\ v_{b_{\omega }}\\ v_{b_a}\end{array}\right)$

其中，X(t)＝[q_eb_ωb_a]^T∈R^9×1为状态量，T为转置，F(t)为状态转移矩阵，W(t)为 过程噪声，为陀螺仪输出值的反对矩阵，v_ω为陀螺仪的随机漂移噪声，为陀螺仪 随机常值模型噪声，为加速度计随机常值模型噪声，q_e为误差四元数的矢量部分，b_ω为陀 螺仪随机常值，b_a为加速度计偏差。

其中，第一量测方程，具体为：

$L_a={\hat{a}}^b-C_n^b\left(\hat{q}\right)g^n=2[C_n^b\left(\hat{q}\right)g^n\times ]q_e+b_a+v_a$

其中，L_a为所述加速度计的输出值与根据重力场参考矢量估计的所述加速度计的 输出值之间的差值，为加速度计的输出值，为从导航坐标系到载体坐标系的转化矩阵， 为姿态四元数的估计值，gⁿ为重力加速度在导航坐标系下的投影，v_a为加速度计的随机误 差。

相应地，上述第一更新子模块，具体用于根据惯性传感器误差模型和姿态误差四 元数更新模型，构建Kalman滤波的状态方程；根据所述加速度计的输出值以及根据重力场 参考矢量估计的所述加速度计的输出值，构建第一量测方程，并采用以下公式更新状态预 测值，得到状态更新值：

${\hat{X}}_{a,k}={\bar{X}}_k+K_{a,k}(L_{a,k}-A_{a,k}{\bar{X}}_k)$

$K_{a,k}={\Sigma }_{{\bar{X}}_k}{A}_{a,k}^T{(A_{a,k}^T{\Sigma }_{{\bar{X}}_k}{A}_{a,k}+R_a)}^{-1}$

${\Sigma }_{{\hat{X}}_{a,k}}=\left({I-K}_{a,k}{A}_{a,k}\right){\Sigma }_{{\bar{X}}_k}{\left({I-K}_{a,k}{A}_{a,k}\right)}^T+K_{a,k}{R}_a{K}_{a,k}^T$

其中，为k时刻的状态更新值，为k时刻的状态预测值，L_a，k为k时刻的所述加 速度计量测值，A_a，k为所述第一量测方程在k时刻的量测矩阵，I为单位阵，K_a，k为增益矩阵， 为的协方差矩阵，R_a为加速度计量测噪声的协方差矩阵；为k时刻的协方差矩 阵。

上述第二更新子模块，具体用于采用以下公式，更新姿态四元数的估计值：

$\hat{q}\Leftarrow \hat{q}\otimes q_e$

$\hat{q}\Leftarrow \hat{q}/\left|\right|\hat{q}\left|\right|$

$q_e={\hat{X}}_{a,k}(1:3)$

其中，为所述姿态四元数的估计值，为四元数乘法，为的归一化，为赋值 符号，q_e为所述状态更新值的分量，为所述状态更新值。

其中，上述第二量测方程，具体为：

$L_m={\hat{m}}^b-C_n^b\left(\hat{q}\right)m^n=2[C_n^b\left(\hat{q}\right)g^n\times ]q_e+v_m$

其中，L_m为所述磁传感器的输出值与根据世界地磁场模型估计的所述磁传感器的 输出值之间的差值，为磁传感器的输出值，为从导航坐标系到载体坐标系的转化矩阵， 为姿态四元数的估计值，mⁿ为理想地磁场输出值的归一化结果，v_m磁传感器的随机误差。

相应地，上述第三更新子模块，具体用于根据惯性传感器误差模型和姿态误差四 元数更新模型，构建Kalman滤波的状态方程；根据所述磁传感器的输出值以及根据世界地 磁场模型估计的所述磁传感器的输出值，构建第二量测方程，并采用以下公式更新状态更 新值，得到状态估计值：

${\hat{X}}_k={\hat{X}}_{a,k}+K_{m,k}(L_{m,k}-A_{m,k}{\hat{X}}_{a,k})$

${\Sigma }_{{\hat{X}}_k}=\left({I-K}_{m,k}{A}_{m,k}\right){\Sigma }_{{\hat{X}}_{a,k}}{\left({I-K}_{m,k}{A}_{m,k}\right)}^T+K_{m,k}{{\bar{P}}_{m,k}}^{-1}{K}_{m,k}^T$

$K_{m,k}=\left(\begin{array}{cc}{r}_3{r}_3^T& 0_{3\times 6}\\ 0_{6\times 3}& 0_{6\times 6}\end{array}\right){\Sigma }_{{\bar{X}}_{m,k}}{A}_{m,k}^T{(A_{m,k}^T{\Sigma }_{{\bar{X}}_{m,k}}{A}_{m,k}+{{\bar{P}}_{m,k}}^{-1})}^{-1}$

${\Sigma }_{{\bar{X}}_{m,k}}=\left(\begin{array}{cc}{\Sigma }_{{\hat{X}}_{a,k}}(1:\mathrm{3,1}:3)& 0_{3\times 6}\\ 0_{6\times 3}& 0_{6\times 6}\end{array}\right)$

$r_3=C_n^b\left(\hat{q}\right){\left(\begin{array}{ccc}0& 0& 1\end{array}\right)}^T$

其中，为k时刻的状态估计值，为k时刻的状态更新值，L_m，k为k时刻的所述磁 传感器量测值，为的协方差矩阵，K_m，k为k时刻的增益矩阵，A_m，k为k时刻的量测矩阵， 为k时刻的所述磁传感器量测值的等价权矩阵。

上述第四更新子模块，具体用于采用以下公式，更新姿态四元数的估计值：

$\hat{q}\Leftarrow \hat{q}\otimes q_e$

$\hat{q}\Leftarrow \hat{q}/\left|\right|\hat{q}\left|\right|$

$q_e={\hat{X}}_a(1:3)$

其中，为所述姿态四元数的估计值，为四元数乘法，为的归一化，为赋值 符号，q_e为所述状态估计值的分量，为所述状态估计值。

上述第二计算模块320，具体用于采用以下公式计算估计磁倾角：

D_cacul＝arccos(n_a·n_m)

其中，D_cacul为所述估计磁倾角，n_a为所述加速度计在载体系输出值的方向矢量，n_m为所述磁传感器在载体系输出值的方向矢量。

上述判断模块330，具体用于判断理想磁倾角和估计磁倾角是否满足以下条件：

|D_cacul-D_reference|＞λ_D

其中，D_reference为所述理想磁倾角，D_cacul为所述估计磁倾角，λ_D为预设阈值；

如果满足上述条件，则确定存在环境磁干扰；否则，确定不存在环境磁干扰。

上述第一构建模块340，具体用于在判断模块330判断出存在环境磁干扰时，采用 以下公式构建所述磁传感器量测值的自适应权重：

不存在环境磁干扰时；

存在环境磁干扰时；

其中，为k时刻所述磁传感器第i个量测值的自适应权重，p_ki为量测值协方差矩 阵的第i个主对角线元素，c为常数，V_i′为磁传感器量测值的归一化残差。

进一步地，上述系统，还包括：

第四计算模块，用于采用以下公式计算所述磁传感器量测值的归一化残差：

${V_{i,k}}^{\prime }=(L_{\mathrm{mi},k}-A_{\mathrm{mi},k}{\bar{X}}_k)/\left|{\sigma }_i\right|,i=\mathrm{1,2,3}$

其中，L_mi为所述第二量测方程的第i个测量方程，A_mi为量测矩阵 的第i行；|σ_i|为磁传感器量测噪声协方差矩阵R_m第i个主对角线 元素的平方根。

本发明的优点在于：将姿态和航向参考系统信息计算的过程采用两步滤波的方法 来实现，第一步以加速度计为量测传感器来估计四元数的误差并更新四元数；第二步以磁 传感器为量测传感器估计四元数的误差，并利用磁干扰检测算法实时检测载体所处环境的 磁干扰，通过量测残差和实际残差的比值自适应调节磁传感器观测数据在滤波估计中的权 重，更新四元数并计算载体的姿态和航向参考系统信息。提高了载体姿态和航向参考系统 信息的抗差性和可靠性。

在本申请所提供的几个实施例中，应该理解到，所揭露的方法和系统，可以通过其 它的方式实现。以上所描述的设备实施例仅仅是示意性的，例如，所述单元的划分，仅仅为 一种逻辑功能划分，实际实现时可以有另外的划分方式，例如：多个单元或组件可以结合， 或可以集成到另一个系统，或一些特征可以忽略，或不执行。此外，所显示或讨论的各组成 部分相互之间的耦合、或直接耦合、或通信连接可以是通过一些接口，设备或单元的间接耦 合或通信连接，可以是电性的、机械的或其它形式的。

上述作为分离部件说明的单元可以是、或也可以不是物理上分开的，作为单元显 示的部件可以是、或也可以不是物理单元，即可以位于一个地方，也可以分布到多个网络单 元上；可以根据实际的需要选择其中的部分或全部单元来实现本实施例方案的目的。

此外，在本发明各实施例中的各功能单元可以全部集成在一个处理模块中，也可 以是各单元分别单独作为一个单元，也可以两个或两个以上单元集成在一个单元中；上述 集成的单元既可以采用硬件的形式实现，也可以采用硬件加软件功能单元的形式实现。

本领域普通技术人员可以理解：实现上述方法实施例的全部或部分步骤可以通过 程序指令相关的硬件来完成，前述的程序可以存储于一计算机可读取存储介质中，该程序 在执行时，执行包括上述方法实施例的步骤；而前述的存储介质包括：移动存储设备、只读 存储器(Read-OnlyMemory，ROM)、随机存取存储器(RandomAccessMemory，RAM)、磁碟或 者光盘等各种可以存储程序代码的介质。

本发明适用于载体姿态和航向参考系统信息计算。以上所述，仅为本发明的具体 实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明 揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本 发明的保护范围应以所述权利要求的保护范围为准。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何 熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵 盖在本发明的保护范围之内。因此，本发明的保护范围应所述以权利要求的保护范围为准。