CN102393204B - 一种基于sins/cns的组合导航信息融合方法 - Google Patents

Abstract

本发明提供一种基于SINS/CNS的组合导航信息融合方法。本发明包括以下步骤：(1)采集星敏感器的输出；(2)求解地球坐标系相对于惯性坐标系之间的转换以及地球坐标系与地理坐标系的转换矩阵；(3)通过上述步骤给出的信息，解算得到模拟的姿态矩阵；(4)由姿态矩阵求解得到载体的姿态角；(5)根据实际姿态角以及系统的失准角计算载体的误差姿态角；(6)建立组合导航系统的量测方程；(7)建立组合导航系统的状态方程；(8)对系统的误差进行估计补偿。本发明提供的组合导航信息融合方法能够有效地降低误差，提高导航精度。

Description

一种基于SINS/CNS的组合导航信息融合方法

技术领域

本发明涉及的是一种导航技术，特别是涉及一种高精度的导航技术，尤其是涉及一种基于SINS/CNS的组合导航技术。

背景技术

在现代导航系统中，对导航信息量的要求越来越多，对导航精度的要求越来越高。要使系统性能得到提高，只靠提高单一导航系统的精度，不仅在技术上难度很大，而且在实际中效果也不十分明显。组合导航技术的出现，使得这一问题有了理想的解决方案。在实际应用中将几种导航系统安装在同一载体上，构成多传感器组合导航系统。利用卡尔曼滤波器进行导航参数和系统状态估计，可以综合各种系统的优点，达到取长补短、提高性能的目的，更好地满足载体对导航系统的要求。捷联惯性导航系统可以不依赖任何外界信息，实现完全自主导航。然而，由于陀螺漂移的存在，导致捷联惯导系统的精度随着时间的积累而降低，而天文导航可提供控制所需的各种高精度姿态数据且测量误差不随时间积累，但是数据更新率低且容易受到天气的影响。可以看到，二者具有极强的互补性和非相似性，如果将它们进行适当的组合，充分利用各子系统的信息，相互取长补短，就能达到提高导航精度和可靠性的目的。

为此，本发明提出将SINS与CNS适当的组合起来，采用姿态匹配的方法，利用卡尔曼滤波技术实现高精度导航，既能克服SINS测量误差随时间积累的问题，又能克服星敏感器自身的缺点，从而达到取长补短的目的。由于两种系统均不向外辐射任何信息，所以抗干扰性强，完全自主，因此本发明提出的SINS/CNS组合导航方法具有精度高、隐蔽性好、自主性强等显著优点。

发明内容

本发明的目的在于提供一种基于SINS/CNS的组合导航信息融合方法。

本发明通过包括以下步骤实现：

(1)采集星敏感器的输出；

(2)求解地球坐标系相对于惯性坐标系之间的转换以及地球坐标系与地理坐标系的转换矩阵；

(3)通过上述步骤给出的信息，解算得到模拟的姿态矩阵；

(4)由姿态矩阵求解得到载体的姿态角；

(5)根据实际姿态角以及系统的失准角计算载体的误差姿态角；

(6)建立组合导航系统的量测方程；

(7)建立组合导航系统的状态方程；

(8)对系统的误差进行估计补偿。

本发明的工作原理是：

第一步：采集星敏感器的输出：星敏感器的坐标系相对于惯性坐标系(i系：天球坐标系)之间的姿态信息

第二步：通过星敏感器自身携带的世界标准时间系统，可以得到地球坐标系(e系)相对于惯性坐标系(i系)之间的转换矩阵：

$C_i^e=\left[\begin{array}{ccc}\cos (k+{\mathrm{\ω}}_{\mathrm{ie}}\·t)& \sin (k+{\mathrm{\ω}}_{\mathrm{ie}}\·t)& 0\\ -\sin (k+{\mathrm{\ω}}_{\mathrm{ie}}\·t)& \cos (k+{\mathrm{\ω}}_{\mathrm{ie}}\·t)& 0\\ 0& 0& 1\end{array}\right],$

其中，ω_ie为地球自转角速度，t是世界标准时间系统提供的具体时间，k是初始位置(经度和纬度)与春分点之间的夹角，

同时由外测信息采集当地的经纬度，由此信息可以得到地球坐标系与地理坐标系的转换矩阵：

$C_e^n=\left[\begin{array}{ccc}-\sin \mathrm{\λ}& \cos \mathrm{\λ}& 0\\ -\sin L\cos \mathrm{\λ}& -\sin L\sin \mathrm{\λ}& \cos L\\ \cos L\cos \mathrm{\λ}& \cos L\sin \mathrm{\λ}& \sin L\end{array}\right],$

其中，L为载体的地理纬度，λ为载体的地理经度；

第三步：利用步骤1与步骤2的实时数据，可以得到载体的高精度姿态矩阵，

$C_b^n=C_e^n{C}_i^e{\left(C_s^b{C}_i^s\right)}^{-1},$

其中，

为星敏感器坐标系(s系)与载体坐标系(b系)之间的转换矩阵，它可以在安装时通过光学瞄准精确获得；

第四步：利用步骤3采集的数据可得，

$C_b^n={\left(C_n^b\right)}^{-1}=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}-\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\cos \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\γ}\cos \mathrm{\ψ}+\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}\\ \cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}& \sin \mathrm{\θ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]$

，

其中，ψ、θ与γ分别为星敏感器提供的载体的真实姿态的航向角、俯仰角与横滚角；

第五步：采集捷联惯导系统自身解算得到的带有误差的姿态信息

第六步：利用步骤(4)与步骤(5)采集的信息并根据坐标转换理论，可以得到如下关系

其中，

为捷联惯导系统的三个平台失准角；

第七步：利用步骤5采集到的信息可以解算出载体的三个带有误差的姿态角分别为航向角ψ_I、俯仰角θ_I、横滚角γ_I，并假设有下式

ψ_I＝ψ+δψ，θ_I＝θ+δθ，γ_I＝γ+δγ，

其中，δψ、δθ、δγ为载体的三个姿态误差角；

第八步：利用步骤(4)与步骤(6)可以得到如下关系

第九步：利用步骤(4)，步骤(7)和步骤(8)可得到载体的三个姿态误差角，

第十步：利用步骤(9)建立组合导航系统的量测方程，

$Z=\left[\begin{array}{c}{\mathrm{\ψ}}_I-\mathrm{\ψ}\\ {\mathrm{\θ}}_I-\mathrm{\θ}\\ {\mathrm{\γ}}_I-\mathrm{\γ}\end{array}\right]=\left[\begin{array}{c}\mathrm{\δ\ψ}\\ \mathrm{\δ\θ}\\ \mathrm{\δ\γ}\end{array}\right]=H_{1}X\left(t\right)+V\left(t\right)=\left[\begin{array}{ccc}{H}_{11}& H_{12}& H_{13}\end{array}\right]X\left(t\right)+V\left(t\right),$

其中，H₁₁＝0_3×3， $H_{12}=\left[\begin{array}{ccc}\frac{C_{12}{C}_{32}}{C_{12}^2+C_{22}^2}& \frac{C_{22}{C}_{32}}{C_{12}^2+C_{22}^2}& -1\\ -\frac{C_{22}}{\sqrt{1-C_{32}^2}}& \frac{C_{12}}{\sqrt{1-C_{32}^2}}& 0\\ \frac{C_{12}{C}_{33}-C_{23}{C}_{31}}{C_{31}^2+C_{33}^2}& \frac{C_{13}{C}_{31}-C_{11}{C}_{33}}{C_{31}^2+C_{33}^2}& 0\end{array}\right],$ H_1-3＝0_3×6，

V(t)为系统的量测噪声，

其中，X(t)∈R¹²为系统的状态向量，

其中，δV_e、δV_n、δV_u分别为东向速度误差、北向速度误差和天向速度误差；为三个平台失准角；ε_x为东向陀螺常值漂移，ε_y为北向陀螺常值漂移，ε_z为方位陀螺常值漂移；Δ_x为东向加速度计零偏，Δ_y为北向加速度计零偏，Δ_z为天向加速度计零偏；

第十一步：建立组合导航系统的状态方程，

其中，F(t)∈R^12×12为系统的状态转移矩阵，G(t)为系统的噪声驱动阵，W(t)为系统的噪声向量；

第十二步：利用步骤(10)与步骤(11)并通过卡尔曼滤波技术对系统的误差进行估计补偿。

本发明提供的组合导航信息融合方法能够有效地降低误差，提高导航精度。

附图说明

图1为本发明提出的方法对平台失准角的仿真估计曲线。

图2为单纯捷联惯导对平台失准角的仿真曲线。

图3为本发明提出的方法对陀螺常值漂移的仿真估计曲线。

具体实施方式

本发明涉及的坐标系有：i-惯性坐标系；s-星敏感器坐标系；n-地理坐标系(导航坐标系)；n′-数学平台坐标系；b-载体坐标系；e-地球坐标系。两个坐标系之间的转换用方向余弦矩阵表示。

步骤1、采集星敏感器输出的高精度姿态信息，即星敏感器的坐标系相对于惯性坐标系(i系：天球坐标系)之间的姿态信息

步骤2、通过星敏感器自身携带的世界标准时间系统，可以得到地球坐标系(e系)相对于惯性坐标系(i系)之间的转换矩阵，

$C_i^e=\left[\begin{array}{ccc}\cos (k+{\mathrm{\ω}}_{\mathrm{ie}}\·t)& \sin (k+{\mathrm{\ω}}_{\mathrm{ie}}\·t)& 0\\ -\sin (k+{\mathrm{\ω}}_{\mathrm{ie}}\·t)& \cos (k+{\mathrm{\ω}}_{\mathrm{ie}}\·t)& 0\\ 0& 0& 1\end{array}\right]$

其中ω_ie为地球自转角速度，t是世界标准时间系统提供的具体时间，k是初始位置(经度和纬度)与春分点之间的夹角。同时，由外测信息采集当地的经纬度，由此信息可以得到地球坐标系与地理坐标系的转换矩阵，即位置矩阵

$C_e^n=\left[\begin{array}{ccc}-\sin \mathrm{\λ}& \cos \mathrm{\λ}& 0\\ -\sin L\cos \mathrm{\λ}& -\sin L\sin \mathrm{\λ}& \cos L\\ \cos L\cos \mathrm{\λ}& \cos L\sin \mathrm{\λ}& \sin L\end{array}\right]$

其中L为载体的地理纬度，λ为载体的地理经度；

步骤3、利用步骤1与步骤2的实时数据，可以得到载体的高精度姿态矩阵，

$C_b^n=C_e^n{C}_i^e{\left(C_s^b{C}_i^s\right)}^{-1},$

其中为星敏感器坐标系(s系)与载体坐标系(b系)之间的转换矩阵，它可以在安装时通过光学瞄准精确获得；

步骤4、利用步骤3采集的数据可得，

$C_b^n={\left(C_n^b\right)}^{-1}=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}-\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\cos \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\γ}\cos \mathrm{\ψ}+\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}\\ \cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}& \sin \mathrm{\θ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]$

其中ψ、θ与γ分别为星敏感器提供的载体的航向角、俯仰角与横滚角，认为是载体的真实姿态；

步骤5、采集捷联惯导系统自身解算得到的姿态信息，此信息带有误差，

步骤6、利用步骤4与步骤5采集的信息并根据坐标转换理论，

可以得到如下关系，

其中

为捷联惯导系统的三个平台失准角；

步骤7、利用步骤5采集到的信息可以解算出载体的三个带有误差的姿态角分别为航向角ψ_I、俯仰角θ_I、横滚角γ_I，并假设有下式，

ψ_I＝ψ+δψ，θ_I＝θ+δθ，γ_I＝γ+δγ，

其中δψ、δθ、δγ为载体的三个姿态误差角；

步骤8、利用步骤4与步骤6可以得到如下关系，

步骤9、利用步骤4，步骤7和步骤8可得到载体的三个姿态误差角，

步骤10、利用步骤9建立组合导航系统的量测方程，

$Z=\left[\begin{array}{c}{\mathrm{\ψ}}_I-\mathrm{\ψ}\\ {\mathrm{\θ}}_I-\mathrm{\θ}\\ {\mathrm{\γ}}_I-\mathrm{\γ}\end{array}\right]=\left[\begin{array}{c}\mathrm{\δ\ψ}\\ \mathrm{\δ\θ}\\ \mathrm{\δ\γ}\end{array}\right]=H_{1}X\left(t\right)+V\left(t\right)=\left[\begin{array}{ccc}{H}_{11}& H_{12}& H_{13}\end{array}\right]X\left(t\right)+V\left(t\right)$

其中H₁₁＝0_3×3， $H_{12}=\left[\begin{array}{ccc}\frac{C_{12}{C}_{32}}{C_{12}^2+C_{22}^2}& \frac{C_{22}{C}_{32}}{C_{12}^2+C_{22}^2}& -1\\ -\frac{C_{22}}{\sqrt{1-C_{32}^2}}& \frac{C_{12}}{\sqrt{1-C_{32}^2}}& 0\\ \frac{C_{12}{C}_{33}-C_{23}{C}_{31}}{C_{31}^2+C_{33}^2}& \frac{C_{13}{C}_{31}-C_{11}{C}_{33}}{C_{31}^2+C_{33}^2}& 0\end{array}\right];$ H₁₃＝0_3×6；

V(t)为系统的量测噪声；

步骤11、建立组合导航系统的状态方程，

其中F(t)∈R^12×12为系统的状态转移矩阵，G(t)为系统的噪声驱动阵，W(t)为系统的噪声向量，X(t)∈R¹²为系统的状态向量，本实施例取X(t)为

其中δV_e、δV_n、δV_u分别为东向速度误差、北向速度误差和天向速度误差；

为三个平台失准角；ε_x为东向陀螺常值漂移，ε_y为北向陀螺常值漂移，ε_z为方位陀螺常值漂移；Δ_x为东向加速度计零偏，Δ_y为北向加速度计零偏，Δ_z为天向加速度计零偏；

步骤12、利用步骤10与步骤11并通过卡尔曼滤波技术对系统的误差进行估计补偿，以达到提高系统导航精度的目的。

本发明提供的基于SINS/CNS的组合导航信息融合方法具有以下优点：

一方面，高精度的星敏感器测量信息不间断地修正捷联惯导，将其误差限制在一定范围内，从而显著地提高了系统的长时间导航精度；另一方面，通过将SINS与CNS进行组合，降低了对惯性器件的精度要求，即使采用中等精度的陀螺仪和加速度计，也能完成高精度的导航任务。因此，在实际工程使用中，当星敏感器由于输出延迟，在短时间内无法给出恒星方向矢量的观测值时，就可以仅仅依靠捷联惯导来进行导航，而在一段较短的时间内，惯导系统的误差不会有太大变化，所以惯导系统在这个时间段最后一刻输出的参数值可以近似地认为是这个时间段内输出参数的平均值，这样当星敏感器输出恢复时，则可以继续使用组合系统进行导航，同时星敏感器的高精度测量信息继续对惯导系统进行修正，从而使惯导系统能够始终保持较高的精度。

为了进一步说明所述方法的有益效果，在以下初始条件下分别对组合导航系统与单纯捷联惯导系统进行了仿真，仿真结果如图1、图2与图3所示，并对其进行了分析比较。

初始条件：

假设载体的初始位置为东经126.6705°，北纬45.7796°；

东向与北向速度均为10m/s；

初始航向角为45°，初始俯仰角为0°，初始横滚角为0°；

三个失准角初值均为0.5°；

陀螺常值漂移为0.1度/小时，随机漂移为0.01度/小时加速度计零漂为1×10^-4g；

星敏感器的测量精度为3角秒；采样周期为0.1秒，仿真时间为一小时。

分析比较：

根据图2可以看出，由于受到单纯惯导系统本身误差的影响，经过1小时的仿真，平台失准角明显成成发散趋势，可见仅靠纯惯性系统是不能完成长时间的高精度导航任务的。根据图1可以看出，采用所述方法估计出的系统平台失准角能够快速收敛，且具有非常高的精度，在1小时的仿真时间里估计精度达到99％以上。同时，由图3可以看出，通过组合导航卡尔曼滤波算法，完全估计出了陀螺的常值漂移，这将有助于补偿校正陀螺仪的误差，从而进一步提高系统的精度，这是单纯捷联惯导系统所不能比拟的，也进一步说明了所述方法的优越性。

Claims (1)

1.一种基于SINS/CNS的组合导航信息融合方法，其特征在于包括以下步骤：

(1)采集星敏感器的输出：星敏感器的坐标系相对于惯性坐标系(i系：天球坐标系)之间的姿态信息

(2)通过星敏感器自身携带的世界标准时间系统，可以得到地球坐标系(e系)相对于惯性坐标系(i系)之间的转换矩阵：

$C_i^e=\left[\begin{array}{ccc}\cos (k+{\mathrm{\ω}}_{\mathrm{ie}}\·t)& \sin (k+{\mathrm{\ω}}_{\mathrm{ie}}\·t)& 0\\ -\sin (k+{\mathrm{\ω}}_{\mathrm{ie}}\·t)& \cos (k+{\mathrm{\ω}}_{\mathrm{ie}}\·t)& 0\\ 0& 0& 1\end{array}\right],$

其中，ω_ie为地球自转角速度，t是世界标准时间系统提供的具体时间，k是初始位置(经度和纬度)与春分点之间的夹角，

同时由外测信息采集当地的经纬度，由此信息可以得到地球坐标系与地理坐标系的转换矩阵：

$C_e^n=\left[\begin{array}{ccc}-\sin \mathrm{\λ}& \cos \mathrm{\λ}& 0\\ -\sin L\cos \mathrm{\λ}& -\sin L\sin \mathrm{\λ}& \cos L\\ \cos L\cos \mathrm{\λ}& \cos L\sin \mathrm{\λ}& \sin L\end{array}\right],$

其中，L为载体的地理纬度，λ为载体的地理经度；

(3)利用步骤(1)与步骤(2)的实时数据，可以得到载体的高精度姿态矩阵，

$C_b^n=C_e^n{C}_i^e{\left(C_s^b{C}_i^s\right)}^{-1},$

其中，

为星敏感器坐标系(s系)与载体坐标系(b系)之间的转换矩阵，它可以在安装时通过光学瞄准精确获得；

(4)利用步骤(3)采集的数据可得，

$C_b^n={\left(C_n^b\right)}^{-1}=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}-\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\cos \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\γ}\cos \mathrm{\ψ}+\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}\\ \cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}& \sin \mathrm{\θ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right],$

其中，ψ、θ与γ分别为星敏感器提供的载体的真实姿态的航向角、俯仰角与横滚角；

(5)采集捷联惯导系统自身解算得到的带有误差的姿态信息

(6)利用步骤(4)与步骤(5)采集的信息并根据坐标转换理论，可以得到如下关系

其中，

为捷联惯导系统的三个平台失准角；

(7)利用步骤(5)采集到的信息可以解算出载体的三个带有误差的姿态角分别为航向角ψ_I、俯仰角θ_I、横滚角γ_I，并假设有下式

ψ_I＝ψ+δψ，θ_I＝θ+δθ，γ_I＝γ+δγ，

其中，δψ、δθ、δγ为载体的三个姿态误差角；

(8)利用步骤(4)与步骤(6)可以得到如下关系

(9)利用步骤(4)，步骤(7)和步骤(8)可得到载体的三个姿态误差角，

(10)利用步骤(9)建立组合导航系统的量测方程，

$Z=\left[\begin{array}{c}{\mathrm{\ψ}}_I-\mathrm{\ψ}\\ {\mathrm{\θ}}_I-\mathrm{\θ}\\ {\mathrm{\γ}}_I-\mathrm{\γ}\end{array}\right]=\left[\begin{array}{c}\mathrm{\δ\ψ}\\ \mathrm{\δ\θ}\\ \mathrm{\δ\γ}\end{array}\right]=H_{1}X\left(t\right)+V\left(t\right)=\left[\begin{array}{ccc}{H}_{11}& H_{12}& H_{13}\end{array}\right]X\left(t\right)+V\left(t\right),$

其中，H₁₁＝0_3×3， $H_{12}=\left[\begin{array}{ccc}\frac{C_{12}{C}_{32}}{C_{12}^2+C_{22}^2}& \frac{C_{22}{C}_{32}}{C_{12}^2+C_{22}^2}& -1\\ -\frac{C_{22}}{\sqrt{1-C_{32}^2}}& \frac{C_{12}}{\sqrt{1-C_{32}^2}}& 0\\ \frac{C_{12}{C}_{33}-C_{23}{C}_{31}}{C_{31}^2+C_{33}^2}& \frac{C_{13}{C}_{31}-C_{11}{C}_{33}}{C_{31}^2+C_{33}^2}& 0\end{array}\right],$ H₁₃＝0_3×6，

V(t)为系统的量测噪声，

其中，X(t)∈R¹²为系统的状态向量，

其中，δV_e、δV_n、δV_u分别为东向速度误差、北向速度误差和天向速度误差；为三个平台失准角；ε_x为东向陀螺常值漂移，ε_y为北向陀螺常值漂移，ε_z为方位陀螺常值漂移；Δ_x为东向加速度计零偏，Δ_y为北向加速度计零偏，Δ_z为天向加速度计零偏；

(11)建立组合导航系统的状态方程，

其中，F(t)∈R^12×12为系统的状态转移矩阵，G(t)为系统的噪声驱动阵，W(t)为系统的噪声向量；

(12)利用步骤(10)与步骤(11)并通过卡尔曼滤波技术对系统的误差进行估计补偿。

Images