CN101915579A - 一种基于ckf的sins大失准角初始对准新方法 - Google Patents

Abstract

本发明的目的在于提供一种基于CKF的SINS大失准角初始对准新方法。利用GPS确定载体的初始位置参数，采集光纤陀螺仪和石英加速度计输出的数据，采用解析法来完成系统的粗对准，初步确定载体的姿态信息，建立捷联惯性导航系统初始对准非线性模型，建立静基座下以速度误差为状态变量的CKF滤波状态方程及速度误差为量测量的量测方程，以CKF滤波方法进行滤波估计，估计出平台失准角，利用平台失准角修正系统的捷联初始姿态矩阵，得到精确的捷联初始姿态矩阵，从而完成精确的初始对准。本发明可以大幅度提高大失准角下捷联惯性导系统的对准精度，为导航过程提供了准确的初始姿态矩阵。

Description

一种基于CKF的SINS大失准角初始对准新方法

技术领域

本发明涉及的是一种用于导航领域的初始姿态对准方法。

背景技术

初始对准是捷联惯性导航系统(Strapdown Inertial Navigation Systems)关键技术之一。初始对准精度直接影响捷联惯性导航系统的工作精度，初始对准时间也是反映系统快速反应能力的重要战术指标。捷联惯性导航系统初始对准的主要目的是建立姿态矩阵的初始值，初始对准中通过初始对准状态空间模型，利用卡尔曼滤波将初始失准角状态估计出来并用以校正姿态矩阵。传统的对准过程包括粗对准和精对准两个阶段，首先用粗对准模型粗略估计出失准角的大小，然后再利用精对准模型估计出失准角的大小从而实现精对准。误差传播模型和滤波算法是影响初始对准精度的主要因素。捷联惯性导航系统的严格数学误差模型是一组非线性微分方程，在小失准角捷联惯性导航系统误差模型中，基于Φ角法或Ψ角法推导出一组线性微分方程误差模型，显然，以线性模型去逼近非线性模型，必然存在一定的建模误差。小失准角线性模型只有在假设各种误差源较小的条件下才成立，而实际中粗对准的失准角在很多情况下为大角度，因此直接采用非线性模型更能真实的反映误差传播特性，而非线性模型决定了滤波方法的采用。

发明内容

本发明的目的在于提供可以大幅度提高对准精度、提供准确初始姿态矩阵的一种基于CKF的SINS大失准角初始对准新方法。

本发明的目的是这样实现的：

本发明一种基于CKF的SINS大失准角初始对准新方法，其特征是：

(1)利用GPS确定载体的初始位置参数；

(2)采集光纤陀螺仪和石英加速度计输出的数据；

(3)采用解析法来完成系统的粗对准，初步确定载体的姿态信息

(0)：

$C_b^{n^{\′}}\left(0\right)=\left(\begin{array}{ccc}\cos {\mathrm{\γ}}_0\cos {\mathrm{\ψ}}_0-\sin {\mathrm{\γ}}_0\sin {\mathrm{\θ}}_0\sin {\mathrm{\ψ}}_0& -\cos {\mathrm{\θ}}_0\sin {\mathrm{\ψ}}_0& \sin {\mathrm{\γ}}_0\cos {\mathrm{\ψ}}_0+\cos {\mathrm{\γ}}_0\sin {\mathrm{\θ}}_0\sin {\mathrm{\ψ}}_0\\ \cos {\mathrm{\γ}}_0\sin {\mathrm{\ψ}}_0+\sin {\mathrm{\γ}}_0\sin {\mathrm{\θ}}_0\cos {\mathrm{\ψ}}_0& \cos {\mathrm{\θ}}_0\cos {\mathrm{\ψ}}_0& \sin {\mathrm{\γ}}_0\sin {\mathrm{\ψ}}_0-\cos {\mathrm{\γ}}_0\sin {\mathrm{\θ}}_0\cos {\mathrm{\ψ}}_0\\ -\sin {\mathrm{\γ}}_0\cos {\mathrm{\θ}}_0& \sin {\mathrm{\θ}}_0& \cos {\mathrm{\γ}}_0\cos {\mathrm{\θ}}_0\end{array}\right),$

其中θ₀、γ₀和ψ₀分别为初始俯仰角、初始倾斜角和初始航向角；

(4)建立捷联惯性导航系统初始对准非线性模型

SINS非线性姿态误差方程为

其中φ_x、φ_y和φ_z为平台东向、北向和天向失准角，表示SINS模拟的数学平台n′与理想导航坐标系n系(即理想数学平台)之间的转动角度，C_ij(i＝1，2，3 j＝1，2，3)为带有误差的捷联姿态矩阵

的对应元素，

$C_b^{n^{\′}}=\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),$

θ、γ和ψ分别为含有误差的俯仰角、倾斜角和航向角，δv_x和δv_y分别表示东向和北向速度误差，ω_ie表示地球自转角速度，R_m和R_n分别表示地球子午、卯酉曲率半径，

表示当地纬度，ε_x、ε_y和ε_z为三个陀螺漂移；

静基座速度误差非线性方程为

$\mathrm{\δ}\stackrel{\·}{v}=[I-{\left(C_n^{n^{\′}}\right)}^T]C_b^{n^{\′}}{\hat{f}}^b+{\left(C_n^{n^{\′}}\right)}^T{C}_b^{n^{\′}}\mathrm{\δ}{f}^b-(2{\widehat{\mathrm{\ω}}}_{\mathrm{ie}}^n+{\widehat{\mathrm{\ω}}}_{\mathrm{en}}^n)\×\mathrm{\δv}+\mathrm{\δ}{g}^n,\mathrm{\δ}{\stackrel{\·}{v}}^n={\left[\begin{array}{cc}\mathrm{\δ}{\stackrel{\·}{v}}_x& \mathrm{\δ}{\stackrel{\·}{v}}_y\end{array}\right]}^T$

为东向、北向速度误差微分，I为2×2单位阵，

为理想导航坐标系n系

到SINS模拟的数学平台n′的方向余弦矩阵，

为加速度计真实测量的比力，δf^b为加速计测量误差，

为在n′系中计算

时的误差，

为在n′系中计算

时的误差，δgⁿ为在n′系中计算重力加速度g时的误差；

(5)建立静基座下以速度误差为状态变量的CKF滤波状态方程及速度误差为量测量的量测方程

陀螺漂移

ε^b为陀螺常值漂移，为陀螺零均值高斯白噪声；加速度计测量误差

▽^b为加速度计常值零偏，

为加速度计零均值高斯白噪声，同时忽略重力误差项δgⁿ，取将速度误差微分方程展开，得到由十个方程组成的捷联惯性导航系统静基座初始对准非线性模型状态方程：

，状态向量取

噪声向量取

建立滤波状态模型，并以SINS速度误差Z＝δv＝[δv_x δv_y]^T为观测量建立观测方程：

其中

H＝[I_2×2 0_2×8]，V为测量噪声，以CKF滤波方法进行滤波估计，估计出平台失准角；

(6)利用步骤(5)估计出的平台失准角修正系统的捷联初始姿态矩阵

得到精确的捷联初始姿态矩阵

即

从而完成精确的初始对准。

本发明的优势在于：对准过程中，速度模型和姿态误差模型均采用非线性形式，从而准确地反映出真实的捷联惯性导航系统误差传播特性；对准过程中首次采用全新的CKF滤波方法，从而有效的解决了大失准角下初始对准非线性模型的滤波问题，保证了滤波精度的收敛性和快速性。所以本发明可以大幅度提高大失准角下捷联惯性导系统的对准精度，从而为导航过程提供了准确的初始姿态矩阵。

附图说明

图1为本发明的流程图；

图2为本发明具体实施方式的东向失准角误差实验曲线；

图3为本发明具体实施方式的北向失准角误差实验曲线；

图4为本发明具体实施方式的天向失准角误差实验曲线。

具体实施方式

下面结合附图举例对本发明做更详细地描述：

结合图1～4，本发明一种基于CKF的SINS大失准角初始对准新方法，其特征是：

(1)利用GPS确定载体的初始位置参数；

(2)采集光纤陀螺仪和石英加速度计输出的数据；

(3)采用解析法来完成系统的粗对准，初步确定载体的姿态信息

$C_b^{n^{\′}}\left(0\right)=\left(\begin{array}{ccc}\cos {\mathrm{\γ}}_0\cos {\mathrm{\ψ}}_0-\sin {\mathrm{\γ}}_0\sin {\mathrm{\θ}}_0\sin {\mathrm{\ψ}}_0& -\cos {\mathrm{\θ}}_0\sin {\mathrm{\ψ}}_0& \sin {\mathrm{\γ}}_0\cos {\mathrm{\ψ}}_0+\cos {\mathrm{\γ}}_0\sin {\mathrm{\θ}}_0\sin {\mathrm{\ψ}}_0\\ \cos {\mathrm{\γ}}_0\sin {\mathrm{\ψ}}_0+\sin {\mathrm{\γ}}_0\sin {\mathrm{\θ}}_0\cos {\mathrm{\ψ}}_0& \cos {\mathrm{\θ}}_0\cos {\mathrm{\ψ}}_0& \sin {\mathrm{\γ}}_0\sin {\mathrm{\ψ}}_0-\cos {\mathrm{\γ}}_0\sin {\mathrm{\θ}}_0\cos {\mathrm{\ψ}}_0\\ -\sin {\mathrm{\γ}}_0\cos {\mathrm{\θ}}_0& \sin {\mathrm{\θ}}_0& \cos {\mathrm{\γ}}_0\cos {\mathrm{\θ}}_0\end{array}\right),$

其中θ₀、γ₀和ψ₀分别为初始俯仰角、初始倾斜角和初始航向角；

(4)建立捷联惯性导航系统初始对准非线性模型

SINS非线性姿态误差方程为

其中φ_x、φ_y和φ_z为平台东向、北向和天向失准角，表示SINS模拟的数学平台n′与理想导航坐标系n系(即理想数学平台)之间的转动角度，C_ij(i＝1，2，3 j＝1，2，3)为带有误差的捷联姿态矩阵

的对应元素，

$C_b^{n^{\′}}=\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),$

θ、γ和ψ分别为含有误差的俯仰角、倾斜角和航向角，δv_x和δv_y分别表示东向和北向速度误差，ω_ie表示地球自转角速度，R_m和R_n分别表示地球子午、卯酉曲率半径，

表示当地纬度，ε_x、ε_y和ε_z为三个陀螺漂移；

静基座速度误差非线性方程为

$\mathrm{\δ}\stackrel{\·}{v}=[I-{\left(C_n^{n^{\′}}\right)}^T]C_b^{n^{\′}}{\hat{f}}^b+{\left(C_n^{n^{\′}}\right)}^T{C}_b^{n^{\′}}\mathrm{\δ}{f}^b-(2{\widehat{\mathrm{\ω}}}_{\mathrm{ie}}^n+{\widehat{\mathrm{\ω}}}_{\mathrm{en}}^n)\×\mathrm{\δv}+\mathrm{\δ}{g}^n,\mathrm{\δ}{\stackrel{\·}{v}}^n={\left[\begin{array}{cc}\mathrm{\δ}{\stackrel{\·}{v}}_x& \mathrm{\δ}{\stackrel{\·}{v}}_y\end{array}\right]}^T$

为东向、北向速度误差微分，I为2×2单位阵，

为理想导航坐标系n系到SINS模拟的数学平台n′的方向余弦矩阵，

为加速度计真实测量的比力，δf^b为加速计测量误差，

为在n′系中计算

时的误差，

为在n′系中计算时的误差，δgⁿ为在n′系中计算重力加速度g时的误差；

(5)建立静基座下以速度误差为状态变量的CKF滤波状态方程及速度误差为量测量的量测方程

陀螺漂移

ε^b为陀螺常值漂移，

为陀螺零均值高斯白噪声；加速度计测量误差

▽^b为加速度计常值零偏，

为加速度计零均值高斯白噪声，同时忽略重力误差项δgⁿ，取将速度误差微分方程展开，得到由十个方程组成的捷联惯性导航系统静基座初始对准非线性模型状态方程：

，状态向量取

噪声向量取

建立滤波状态模型，并以SINS速度误差Z＝δv＝[δv_x δv_y]^T为观测量建立观测方程：

其中

H＝[I_2×2 0_2×8]，V为测量噪声，以CKF滤波方法进行滤波估计，估计出平台失准角；

(6)利用步骤(5)估计出的平台失准角修正系统的捷联初始姿态矩阵

得到精确的捷联初始姿态矩阵

即从而完成精确的初始对准。

对该方法进行仿真实验：

载体初始位置：北纬45.7996°，东经126.6705°；

初始失准角：φ_x＝°1，φ_y＝1°，φ_z＝10°；

赤道半径：R＝6378393m；

椭球度：e＝3.367e-3；

地球表面重力加速度：g₀＝9.78049；

地球自转角速度(弧度/秒)：ω_ie＝7.2921158e-5；

陀螺仪常值漂移：0.1度/小时；

加速度计零偏：10^-4g₀；

常数：π＝3.1415926

根据以上初值，利用本发明所述方法得到载体东向失准角误差曲线、北向失准角误差曲线和方位失准角误差曲线分别如图2、图3、图4所示。结果表明在水平小失准角和方位大失准角的情况下，采用本发明的方法可以获得很高的对准精度，而且收敛时间很快，有效的解决了捷联惯性导航系统大失准角初始对准精度和快速性的问题。

Claims (1)

1.一种基于CKF的SINS大失准角初始对准新方法，其特征是：

(1)利用GPS确定载体的初始位置参数；

(2)采集光纤陀螺仪和石英加速度计输出的数据；

(3)采用解析法来完成系统的粗对准，初步确定载体的姿态信息

$C_b^{n^{\′}}\left(0\right)=\left(\begin{array}{ccc}\cos {\mathrm{\γ}}_0\cos {\mathrm{\ψ}}_0-\sin {\mathrm{\γ}}_0\sin {\mathrm{\θ}}_0\sin {\mathrm{\ψ}}_0& -\cos {\mathrm{\θ}}_0\sin {\mathrm{\ψ}}_0& \sin {\mathrm{\γ}}_0\cos {\mathrm{\ψ}}_0+\cos {\mathrm{\γ}}_0\sin {\mathrm{\θ}}_0\sin {\mathrm{\ψ}}_0\\ \cos {\mathrm{\γ}}_0\sin {\mathrm{\ψ}}_0+\sin {\mathrm{\γ}}_0\sin {\mathrm{\θ}}_0\cos {\mathrm{\ψ}}_0& \cos {\mathrm{\θ}}_0\cos {\mathrm{\ψ}}_0& \sin {\mathrm{\γ}}_0\sin {\mathrm{\ψ}}_0-\cos {\mathrm{\γ}}_0\sin {\mathrm{\θ}}_0\cos {\mathrm{\ψ}}_0\\ -\sin {\mathrm{\γ}}_0\cos {\mathrm{\θ}}_0& \sin {\mathrm{\θ}}_0& \cos {\mathrm{\γ}}_0\cos {\mathrm{\θ}}_0\end{array}\right),$

其中θ₀、γ₀和ψ₀分别为初始俯仰角、初始倾斜角和初始航向角；

(4)建立捷联惯性导航系统初始对准非线性模型

SINS非线性姿态误差方程为

其中φ_x、φ_y和φ_z为平台东向、北向和天向失准角，表示SINS模拟的数学平台n′与理想导航坐标系n系之间的转动角度，C_ij(i＝1，2，3 j＝1，2，3)为带有误差的捷联姿态矩阵

的对应元素，

$C_b^{n^{\′}}=\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),$

θ、γ和ψ分别为含有误差的俯仰角、倾斜角和航向角，δv_x和δv_y分别表示东向和北向速度误差，ω_ie表示地球自转角速度，R_m和R_n分别表示地球子午、卯酉曲率半径，

表示当地纬度，ε_x、ε_y和ε_z为三个陀螺漂移；

静基座速度误差非线性方程为

$\mathrm{\δ}\stackrel{\·}{v}=[I-{\left(C_n^{n^{\′}}\right)}^T]C_b^{n^{\′}}{\hat{f}}^b+{\left(C_n^{n^{\′}}\right)}^T{C}_b^{n^{\′}}\mathrm{\δ}{f}^b-(2{\widehat{\mathrm{\ω}}}_{\mathrm{ie}}^n+{\widehat{\mathrm{\ω}}}_{\mathrm{en}}^n)\×\mathrm{\δv}+\mathrm{\δ}{g}^n,$

为东向、北向速度误差微分，I为2×2单位阵，

为理想导航坐标系n系到SINS模拟的数学平台n′的方向余弦矩阵，

为加速度计真实测量的比力，δf^b为加速计测量误差，

为在n′系中计算

时的误差，

为在n′系中计算时的误差，δgⁿ为在n′系中计算重力加速度g时的误差；

(5)建立静基座下以速度误差为状态变量的CKF滤波状态方程及速度误差为量测量的量测方程

陀螺漂移

ε^b为陀螺常值漂移，

为陀螺零均值高斯白噪声；加速度计测量误差

▽^b为加速度计常值零偏，

为加速度计零均值高斯白噪声，同时忽略重力误差项δgⁿ，取

将速度误差微分方程展开，得到由十个方程组成的捷联惯性导航系统静基座初始对准非线性模型状态方程：

，状态向量取

噪声向量取

建立滤波状态模型，并以SINS速度误差Z＝δv＝[δv_x δv_y]^T为观测量建立观测方程：

其中

H＝[I_2×2 0_2×8]，V为测量噪声，以

CKF滤波方法进行滤波估计，估计出平台失准角；

(6)利用步骤(5)估计出的平台失准角修正系统的捷联初始姿态矩阵

得到精确的捷联初始姿态矩阵

即

从而完成精确的初始对准。

Images