CN101000245A - 一种sins/gps/磁罗盘组合导航系统的数据融合方法 - Google Patents

Abstract

一种SINS/GPS/磁罗盘组合导航系统的数据融合方法，涉及一种组合导航系统数据融合算法，该方法是将SINS和GPS进行位置速度误差组合，经第一次卡尔曼滤波估计出载体的位置、速度和姿态误差，对SINS进行反馈校正；通过校正后SINS输出的横滚角和俯仰角计算载体坐标系到水平坐标系的方向余弦矩阵，辅助磁罗盘得到载体的磁航向角进而得到航向角；将SINS和磁罗盘进行航向角误差组合，经第二次卡尔曼滤波估计出载体的姿态误差，对SINS进行反馈校正，由SINS向用户输出载体的位置、速度和姿态信息。本发明具有计算简单、精度高、容错能力强和应用领域广的优点，不仅可用于提高磁罗盘的定姿精度，而且可以提高航天器、航空器、船舶或地面车辆用SINS/GPS/磁罗盘组合导航系统的导航精度。

Description

一种SINS/GPS/磁罗盘组合导航系统的数据融合方法

技术领域

本发明涉及一种SINS/GPS/磁罗盘组合导航系统的数据融合方法，该方法具有计算简单、精度高、容错能力强和应用领域广的优点，可用于提高航空器、航天器、舰船或地面车辆用SINS/GPS/磁罗盘组合导航系统的导航精度。

背景技术

捷联惯性导航系统(Strapdown Inertial Navigation System，SINS)是一种完全自主的导航系统，可以连续、实时地提供位置、速度和姿态(横滚角、俯仰角和航向角)信息，其短时精度很高，且具有隐蔽性好，不受气候条件限制等优点，因而广泛应用于航空、航天、航海等领域。但是，SINS误差随时间增长，因此常与全球卫星定位系统(Global Positioning System，GPS)组合构成SINS/GPS组合导航系统。在SINS/GPS组合导航系统中，利用卡尔曼滤波技术可以有效地降低组合导航系统的位置误差和速度误差，并且能够把横滚角误差和俯仰角误差抑制到一定的精度，但是很难抑制组合导航系统航向角误差的积累。磁罗盘可以输出地磁场矢量在载体坐标系的投影，且精度不发散，利用磁罗盘的信息与SINS、GPS进行组合，可以抑制航向角误差的发散。因此SINS/GPS/磁罗盘组合导航系统应用较为广泛。

对于SINS/GPS/磁罗盘组合导航系统的数据融合方法，许多研究人员进行了大量的理论研究，取得了很多进展。目前SINS/GPS/磁罗盘组合导航系统的数据融合有两种常用的方法，一种是直接采用磁场强度误差(通过磁罗盘的测量值和参考系内的已知矢量值相比较得到)作为量测信息与SINS、GPS进行组合，该种方法提高了系统的航向角的精度(航天领域使用较多)；但是此种组合估计方法必须以已知当时的轨迹信息和地磁场模型为前提，计算较复杂。另一种是采用航向角误差(磁罗盘计算得到的航向角与SINS得到的航向角之差)作为量测信息与SINS、GPS进行组合，抑制了系统的航向角误差的发散(航空领域使用较多)；但是利用磁罗盘输出的地磁场强度得到航向角必须以已知横滚角和俯仰角为前提，如果横滚角和俯仰角存在很大的误差将直接影响磁罗盘计算航向角的精度，目前是利用东向和北向加速度计信息来计算横滚角和俯仰角，从而辅助磁罗盘计算航向角，但是此种方法只适合载体处于非加速运动的情况，当载体处于加速运动时，航向角精度将会很差。

总而言之，目前的SINS/GPS/磁罗盘组合导航系统数据融合方法存在着计算量大、精度较差、应用范围较窄等缺点，难以满足航天器、航空器、船舶或地面车辆高精度的导航要求。

发明内容

本发明的技术解决问题是：克服现有技术的不足，提供了一种SINS/GPS/磁罗盘组合导航系统的数据融合方法，该方法实现了系统的高精度、高容错能力和减小了计算量。

本发明的技术解决方案为：一种SINS/GPS/磁罗盘组合导航系统的数据融合方法，其特点在于包括下列步骤：

(1)将SINS和GPS进行位置速度误差组合得到误差方程，经第一次卡尔曼滤波估计出载体的位置、速度和姿态误差，对SINS的位置、速度、横滚角和俯仰角进行反馈校正；

(2)利用反馈校正后SINS输出的横滚角和俯仰角来计算载体坐标系x_by_bz_b至水平坐标系x_hy_hz_h的方向余弦矩阵C_b ^h；

(3)利用C_b ^h将磁罗盘输出载体坐标系x_by_bz_b下的三维磁场强度转化为水平坐标系x_hy_hz_h下的三维磁场强度；

(4)利用水平坐标系x_hy_hz_h下的三维磁场强度计算载体的磁航向角，进而得到载体的航向角；

(5)将SINS和磁罗盘进行航向角误差组合得到误差方程，经第二次卡尔曼滤波估计出载体的姿态误差，对SINS的航向角进行反馈校正，可由SINS向用户输出载体的位置、速度和姿态信息。

上述的步骤(1)中误差方程的状态矢量为：

X₁＝[φ_E φ_N δV_E δV_N δV_U δL δλ δh ε_E ε_N __U]，观测矢量为：

Z₁＝[δL δλ δh δV_E δV_N δV_U]^T；其中，φ_E和φ_N为东向和北向平台失准角；δV_E、

δV_N和δV_U为东向、北向和天向的速度误差；δL、δλ和δh为纬度误差、经度误差和高度误差；ε_E和ε_N为东向和北向陀螺仪的随机常值漂移；__U为天向加速度计的随机常值漂移。

上述的步骤(2)中载体坐标系x_by_bz_b至水平坐标系x_hy_hz_h的方向余弦矩阵C_b ^h为：

$C_b^h=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\θ}& \sin \mathrm{\θ}\\ 0& -\sin \mathrm{\θ}& \cos \mathrm{\θ}\end{array}\right]\left[\begin{array}{ccc}\cos \mathrm{\γ}& 0& -\sin \mathrm{\γ}\\ 0& 1& 0\\ \sin \mathrm{\γ}& 0& \cos \mathrm{\γ}\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\γ}& 0& -\sin \mathrm{\γ}\\ \sin \mathrm{\γ\θ}\sin & \cos \mathrm{\θ}& \cos \mathrm{\γ\θ}\sin \\ \sin \mathrm{\γ\θ}\cos & -\sin \mathrm{\θ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]$

其中，γ和θ分别为经过GPS校正后SINS输出的载体的横滚角和俯仰角。

上述的步骤(3)中水平坐标系x_hy_hz_h下的三维磁场强度为：

$\left[\begin{array}{c}{M}_{\mathrm{hx}}\\ M_{\mathrm{hy}}\\ M_{\mathrm{hz}}\end{array}\right]=C_b^h\left[\begin{array}{c}{M}_{\mathrm{bx}}\\ M_{\mathrm{by}}\\ M_{\mathrm{bz}}\end{array}\right]$

其中，M_bx、M_by、M_bz为载体坐标系x_by_bz_b的三维磁场强度；M_hx、M_hy、M_hz为水平坐标系x_hy_hz_h的三维磁场强度。

上述的步骤(4)中载体的磁航向角ψ_MC和航向角ψ_C计算公式为：

ψ_MC＝arctan(-M_hx/M_hy)

ψ_C＝ψ_MC+α

其中，α为当地地理北向与当地地磁北向的夹角。

上述的步骤(5)中误差方程的状态矢量为X₂＝[φ_E φ_N φ_U ε_N ε_U]^T，观测矢量为Z₂＝φ_U；其中，φ_E、φ_N和φ_U为三个轴上平台失准角；ε_E、ε_N和ε_U为东向、北向和天向陀螺仪的随机常值漂移。

本发明的原理是：将SINS和GPS进行位置速度误差组合，经第一次卡尔曼滤波估计出载体的位置、速度和姿态误差，对SINS进行反馈校正；利用反馈校正后SINS输出的横滚角和俯仰角来辅助磁罗盘计算磁航向角进而得到航向角；并将SINS和磁罗盘进行航向角误差组合，经第二次卡尔曼滤波估计出载体的姿态误差，对SINS进行反馈校正，可由SINS向用户输出精度高且不发散的载体的位置、速度和姿态信息。

本发明与现有技术相比的优点在于：(1)通过GPS校正后的SINS输出的横滚角和俯仰角来辅助磁罗盘计算航向角，不需要知道当时的轨迹信息和地磁场模型，对于载体处于非加速运动的情况下此方法也适用，并且计算简单。(2)利用SINS和GPS进行位置速度误差组合以及SINS和磁罗盘进行航向角误差组合的两个模型简单的卡尔曼滤波来估计位置、速度和姿态的误差，该方法提高了组合系统的精度、容错性和实时性。

附图说明

图1为本发明的数据融合方法的计算流程图；

图2为本发明的原理框图；

图3为本发明的卡尔曼滤波基本算法的解算流程图。

具体实施方式

如图1、2、3所示，本发明的具体方法如下：

(1)将SINS和GPS进行位置速度误差组合得到误差方程，经第一次卡尔曼滤波估计出载体的位置、速度和姿态误差，对SINS的位置、速度、横滚角和俯仰角进行反馈校正；

①SINS和GPS进行位置速度误差组合的误差方程包括系统状态方程和量测方程，分别如公式(1)和公式(2)所示。

系统状态方程：

其中，X₁＝[φ_E φ_N δV_E δV_N δV_U δL δλ δh ε_E ε_N __U]为系统状态矢量，W₁＝[w_εE w_εN w_εU w_{_E} w_{_N} w_{_U}]^T为系统噪声矢量，F₁为系统转移矩阵，G₁为噪声转换矩阵。φ_E和φ_N为东向和北向平台失准角；δV_E、δV_N和δV_U为东向、北向和天向的速度误差；δL、δλ和δh为纬度误差、经度误差和高度误差；ε_E和ε_N为东向和北向陀螺仪的随机常值漂移；__U为天向加速度计的随机常值漂移。

系统的量测方程：

Z₁＝H₁X₁+υ₁                    (2)

其中：Z₁＝[δL δλ δh δV_E δV_N δV_U]T为观测矢量，H₁为观测矩阵，

υ₁＝[σ_L σ_λ σ_h σ_VE σ_VN σ_VU]^T为量测噪声矢量。观测矢量具体从下面式子中得到：

δL＝L_SINS-L_GPS   δλ＝λ_SINS-λ_GPS    δh＝h_SINS-h_GPS

δV_E＝(V_E)_SINS-(V_E)_GPS   δV_N＝(V_N)_SINS-(V_N)_GPS  δV_U＝(V_U)_SINS-(V_U)_GPS

其中，L、λ和h为纬度、经度和高度；V_E、V_N和V_U为东向、北向和天向的速度；下标为SINS表示由SINS得到，下标为GPS表示由GPS得到。

②卡尔曼滤波基本算法编排，该算法的流程图如图3所示。

状态一步预测方程：

${\hat{X}}_{k/k-1}={\mathrm{\φ}}_{k,k-1}{\hat{X}}_{k-1}---\left(3\right)$

状态估值计算方程：

${\hat{X}}_k={\hat{X}}_{k/k-1}+{\hat{K}}_k(Z_k-H_k{\hat{X}}_{k/k-1})---\left(4\right)$

滤波增量方程：

${\hat{K}}_k={\hat{P}}_{k/k-1}{H}_k^T{(H_k{\hat{P}}_{k/k-1}{H}_k^T+R_k)}^{-1}---\left(5\right)$

一步预测均方误差方程：

${\hat{P}}_{k/k-1}={\mathrm{\φ}}_{k,k-1}{\hat{P}}_{k-1}{\mathrm{\φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(6\right)$

估计均方误差方程：

${\hat{P}}_k=(I-{\hat{K}}_k{H}_k){\hat{P}}_{k/k-1}{(I-{\hat{K}}_k{H}_k)}^T+{\hat{K}}_k{R}_k{\hat{K}}_k^T---\left(7\right)$

③把滤波估计出的状态矢量

反馈给SINS中的位置、速度和姿态。

位置和速度可以直接校正：

$L_{\mathrm{SINS}}=L_{\mathrm{SINS}}-\mathrm{\δ}\hat{L}$ ${\mathrm{\λ}}_{\mathrm{SINS}}={\mathrm{\λ}}_{\mathrm{SINS}}-\mathrm{\δ}\widehat{\mathrm{\λ}}$ $h_{\mathrm{SINS}}=h_{\mathrm{SINS}}-\mathrm{\δ}\hat{h}$

$V_E={\left(V_E\right)}_{\mathrm{SINS}}-\mathrm{\δ}{\hat{V}}_E$ $V_N={\left(V_N\right)}_{\mathrm{SINS}}-\mathrm{\δ}{\hat{V}}_N$ $V_U={\left(V_U\right)}_{\mathrm{SINS}}-\mathrm{\δ}{\hat{V}}_U$

姿态校正利用估计的平台误差角间接校正：

计算导航坐标系x_cy_cz_c与导航坐标系x_ny_nz_n之间的转换阵为：

$C_n^c=\left[\begin{array}{ccc}1& {\widehat{\mathrm{\φ}}}_U& -{\widehat{\mathrm{\φ}}}_N\\ -{\widehat{\mathrm{\φ}}}_U& 1& {\widehat{\mathrm{\φ}}}_E\\ {\widehat{\mathrm{\φ}}}_N& -{\widehat{\mathrm{\φ}}}_E& 1\end{array}\right]---\left(8\right)$

又因为 $C_b^c=C_n^c{C}_b^n$ ，故 $C_b^n={\left(C_n^c\right)}^{-1}{C}_b^c$ ，其中C_b ^c为SINS输出的三个姿态计算得到的载体坐标系x_by_bz_b相对计算导航坐标系x_cy_cz_c的方向余弦矩阵。通过校正后的C_b ⁿ计算姿态角γ、θ和ψ。具体方法如下：

由于

$=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]---\left(9\right)$

所以

θ＝sin^-1(T₂₃)    (10)

姿态角计算的主要问题是区间的判断。俯仰角θ定义在[-90°，90°]之间，和反正弦函数的主值一致，不存在多值问题。滚转角γ定义在[-180°，180°]之间，航向角ψ定义范围为[0，360°]，而反正切函数的取值范围为[-90°，90°]，所以都涉及多值问题，需要对其进行真值判断。

(2)利用反馈校正后SINS输出的横滚角γ和俯仰角θ来计算载体坐标系x_by_bz_b至水平坐标系x_hy_hz_h的方向余弦矩阵C_b ^h。载体的姿态角由俯仰角θ、航向角ψ和横滚角γ这三个欧拉角来描述。载体坐标系x_by_bz_b通过采用Y→X的旋转顺序(即先绕Y轴转横滚角γ，再绕X轴转俯仰角θ)转到水平坐标系x_hy_hz_h。这两次转动的方向余弦阵分别为：

$T_{\mathrm{\γ}}=\left[\begin{array}{ccc}\cos \mathrm{\γ}& 0& -\sin \mathrm{\γ}\\ 0& 1& 0\\ \sin \mathrm{\γ}& 0& \cos \mathrm{\γ}\end{array}\right]$ $T_{\mathrm{\θ}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\θ}& \sin \mathrm{\θ}\\ 0& -\sin \mathrm{\θ}& \cos \mathrm{\θ}\end{array}\right]$

则方向余弦矩阵C_b ^h为：

$C_b^h=T_{\mathrm{\θ}}{T}_{\mathrm{\γ}}=\left[\begin{array}{ccc}\cos \mathrm{\γ}& 0& -\sin \mathrm{\γ}\\ \sin \mathrm{\γ\θ}\sin & \cos \mathrm{\θ}& \cos \mathrm{\γ\θ}\sin \\ \sin \mathrm{\γ\θ}\cos & -\sin \mathrm{\θ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]---\left(13\right)$

(3)利用C_b ^h将磁罗盘输出载体坐标系x_by_bz_b下的三维磁场强度M_bx、M_by和M_bz转化为水平坐标系x_hy_hz_h下的三维磁场强度M_hx、M_hy和M_hz，具体计算公式为：

$\left[\begin{array}{c}{M}_{\mathrm{hx}}\\ M_{\mathrm{hy}}\\ M_{\mathrm{hz}}\end{array}\right]=C_b^h\left[\begin{array}{c}{M}_{\mathrm{bx}}\\ M_{\mathrm{by}}\\ M_{\mathrm{bz}}\end{array}\right]---\left(14\right)$

(4)利用水平坐标系x_hy_hz_h下的三维磁场强度M_hx、M_hy和M_hz计算载体的磁航向角ψ_MC，进而得到载体的航向角ψ_C，公式如下：

ψ_MC＝arctan(-M_hx/M_hy)    (15)

由于一般的航向角定义域为[0°，360°]，且是逆时针为正，因此对上式进行象限判断：

则航向角ψ_C为：

ψ_C＝ψ_MC+α    (16)

其中α为当地地理北向与地磁北向的夹角。

(5)将SINS和磁罗盘进行航向角误差组合得到误差方程，经第二次卡尔曼滤波估计出载体的姿态误差，对SINS的航向角进行反馈校正，可由SINS向用户输出载体的位置、速度和姿态信息。

①SINS和磁罗盘进行航向角误差组合的误差方程包括系统状态方程和量测方程，分别如公式(17)和公式(18)所示：

系统状态方程：

其中，X₂＝[φ_E φ_N φ_E ε_E ε_N ε_U]^T为系统状态矢量，W₂＝[w_εE w_εN w_εU]^T为系统噪声矢量，F₂为系统转移矩阵，G₂为噪声转换矩阵；φ_E、φ_N和φ_U为三个轴上平台失准角；ε_E、ε_N和ε_U为东向、北向和天向陀螺仪的随机常值漂移。

系统的量测方程：

Z₂＝H₂X₂+υ₂                (18)

其中：Z₂＝φ_U为观测矢量，H₂为观测矩阵，υ₂＝σ_φU为量测噪声矢量。

观测矢量具体从下面式子中得到：

设SINS得到的航向角为ψ，又 $C_n^c=C_b^c{C}_n^b$ ，其中C_b ^c为SINS输出的ψ和γ、θ三个姿态计算得来的载体坐标系x_by_bz_b相对计算导航坐标系x_cy_cz_c的方向余弦矩阵，C_n ^b由磁罗盘得到ψ_C和SINS得到的γ、θ计算得到(具体表达式见公式(9))。由C_n ^c的表达式即公式(8)得到φ_U。

②滤波公式同公式(3)～公式(7)。

③估计出载体的姿态误差并进行反馈校正SINS，姿态校正同步骤(1)中的③，可由SINS向用户输出载体的位置、速度和姿态信息。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (7)

1、一种SINS/GPS/磁罗盘组合导航系统的数据融合方法，其特征在于包括下列步骤：

(1)将SINS和GPS进行位置速度误差组合得到误差方程，经第一次卡尔曼滤波估计出载体的位置、速度和姿态误差，对SINS的位置、速度、横滚角和俯仰角进行反馈校正；

(2)利用反馈校正后SINS输出的横滚角和俯仰角来计算载体坐标系x_by_bz_b至水平坐标系x_hy_hz_h的方向余弦矩阵C_b ^h；

(3)利用C_b ^h将磁罗盘输出载体坐标系x_by_bz_b下的三维磁场强度转化为水平坐标系x_hy_hz_h下的三维磁场强度；

(4)利用水平坐标系x_hy_hz_h下的三维磁场强度计算载体的磁航向角，进而得到载体的航向角；

(5)将SINS和磁罗盘进行航向角误差组合得到误差方程，经第二次卡尔曼滤波估计出载体的姿态误差，对SINS的航向角进行反馈校正，可由SINS向用户输出载体的位置、速度和姿态信息。

2、根据权利要求1所述的SINS/GPS/磁罗盘组合导航系统的数据融合方法，其特征在于：所述的步骤(1)中误差方程的状态矢量为：X₁＝[φ_E φ_N δV_E δV_N δV_U δL δλ δh ε_E ε_N __U]，观测矢量为：Z₁＝[δL δλ δh δV_E δV_N δV_U]^T；其中，φ_E和φ_N为东向和北向平台失准角；δV_E、δV_N和δV_U为东向、北向和天向的速度误差；δL、δλ和δh为纬度误差、经度误差和高度误差；ε_E和ε_N为东向和北向陀螺仪的随机常值漂移；__U为天向加速度计的随机常值漂移。

3、根据权利要求1所述的SINS/GPS/磁罗盘组合导航系统的数据融合方法，其特征在于：所述的步骤(2)中载体坐标系x_by_bz_b至水平坐标系x_hy_hz_h的方向余弦矩阵C_b ^h为：

$C_b^h=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\θ}& \sin \mathrm{\θ}\\ 0& -\sin \mathrm{\θ}& \cos \mathrm{\θ}\end{array}\right]\left[\begin{array}{ccc}\cos \mathrm{\γ}& 0& -\sin \mathrm{\γ}\\ 0& 1& 0\\ \sin \mathrm{\γ}& 0& \cos \mathrm{\γ}\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\γ}& 0& -\sin \mathrm{\γ}\\ \sin \mathrm{\γ}\sin \mathrm{\θ}& \cos \mathrm{\θ}& \cos \mathrm{\γ}\sin \mathrm{\θ}\\ \sin \mathrm{\γ}\cos \mathrm{\θ}& -\sin \mathrm{\θ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]$

其中，γ和θ分别为经过GPS校正后SINS输出的载体的横滚角和俯仰角。

4、根据权利要求1所述的SINS/GPS/磁罗盘组合导航系统的数据融合方法，其特征在于：所述的步骤(3)中水平坐标系x_hy_hz_h下的三维磁场强度为：

$\left[\begin{array}{c}{M}_{\mathrm{hx}}\\ M_{\mathrm{hy}}\\ M_{\mathrm{hz}}\end{array}\right]=C_b^h\left[\begin{array}{c}{M}_{\mathrm{bx}}\\ M_{\mathrm{by}}\\ M_{\mathrm{bz}}\end{array}\right]$

其中，M_bx、M_by、M_bz为载体坐标系x_by_bz_b的三维磁场强度；M_hx、M_hy、M_hz为水平坐标系x_hy_hz_h的三维磁场强度。

5、根据权利要求1所述的SINS/GPS/磁罗盘组合导航系统的数据融合方法，其特征在于：所述的步骤(4)中载体的磁航向角ψ_MC和航向角ψ_C计算公式为：

ψ_MC＝arctan(-M_hx/M_hy)

ψ_C＝ψ_MC+α

其中，α为当地地理北向与当地地磁北向的夹角。

6、根据权利要求1所述的SINS/GPS/磁罗盘组合导航系统的数据融合方法，其特征在于：所述的步骤(5)中误差方程的状态矢量为：X₂＝[φ_E φ_N φ_U ε_E ε_N ε_U]^T，观测矢量为Z₂＝φ_U；其中，φ_E、φ_N和φ_U为东向、北向和天向平台失准角；ε_E、ε_N和ε_U为东向、北向和天向陀螺仪的随机常值漂移。

7、根据权利要求1所述的SINS/GPS/磁罗盘组合导航系统的数据融合方法，其特征在于：所述的步骤(1)和(5)中的卡尔曼滤波为线性卡尔曼滤波、扩展卡尔曼滤波或Unscented卡尔曼滤波。

Images