CN102221366B - 一种基于模糊变地球自转角速度的快速精对准方法 - Google Patents

Abstract

本发明的目的在于提供一种基于模糊变地球自转角速度的快速精对准方法，包括以下步骤：确定载体的初始位置参数，采集加速度计和陀螺仪输出，确定粗略的初始姿态矩阵，根据计算地球自转角速度进行卡尔曼滤波一步迭代计算，确定精对准时间，判断是否精对准是否结束，若结束则采用卡尔曼滤波技术估计出的失准角，并用失准角来修正***的捷联姿态矩阵，完成精确初始对准，若未结束则返回进行修正直至精对准结束，之后同样采用卡尔曼滤波技术估计出的失准角，并用失准角来修正***的捷联姿态矩阵，完成精确初始对准。本发明不需要增加任何外观测设备，无需提供辅助位置转台，工程实现容易，有效提高了捷联惯性导航***的精对准的快速性。

Description

一种基于模糊变地球自转角速度的快速精对准方法

技术领域

本发明涉及的是一种导航领域的对准方法，具体地说是用于捷联惯性***的初始姿态确定的方法。

背景技术

捷联惯性导航***(Strapdown Inertial Navigation System简称SINS)的基本原理是根据牛顿提出的相对惯性空间的力学定律，利用加速度计、陀螺仪测量载体惯性空间的线运动和角运动参数，在给定的运动初始条件下，由计算机进行积分运算，连续、实时地提供位置、速度和姿态信息。根据SINS的基本原理，SINS在导航定位解算之前必须获得初始信息，包括初始位置、速度和姿态。SINS的初始位置和速度信息较初始姿态容易获得，初始姿态确定的精度就是SINS进入导航工作状态时的初始精度。因此，SINS开始工作时进行快速精确的初始姿态的确定是十分重要的一步。

现有的捷联惯导***初始姿态确定分为粗对准和精对准两个阶段。粗对准阶段就是在静基座条件下，将陀螺仪和加速度计输出直接引入导航计算机，计算出载体的初始姿态。用此方法时，常常忽略陀螺仪和加速度计的误差和外部干扰因素，然而这些因素会导致误差，因此初始姿态计算精度不高。精对准阶段是在粗对准的基础上进行，利用现代控制理论的状态空间法，对陀螺仪和加速度计输出的数据进行处理。计算出导航坐标系与真实导航坐标系的失准角，建立准确的初始姿态矩阵。捷联惯性导航***要求对准精度高，对准时间短。

以速度误差作为观测量的静基座精对准***的可观测性和可观测度差，是制约其对准精度和快速性的重要原因，特别是方位角的观测度低，导致方位失准角的估计效果很差。关于快速对准，已有的文献的常规思路都是如何提高方位角的可观测度，如增加方位传感器或者进行多位置对准等。如专利号：ZL200510130615.7的发明专利：一种捷联惯性导航***的任意双位置初始对准方法；如专利号：ZL200810064146.7的发明专利：基于滤波的光纤陀螺捷联惯导***两位置初始对准方法；是在对准过程中利用位置转台进行多位置对准来提高***的可观性和可观度，虽然这些方法已经不需要难以实现的精密位置转台了，但还是需要在位置转台上进行，限制了工程应用。如专利号：200810019357.9的发明专利：捷联惯性导航***的快速精对准方法；是通过增加磁传感器和倾角传感器来提高捷联惯导***的可观性，但是该方法利用的方位传感器一般都是地磁传感器，这种地磁传感很容易收外部的干扰，尤其载体是磁导体时，地磁传感很难正常工作，很难实际使用。因此在实际工程应用中，找到一种能够提高惯性导航***的对准精度和反应能力的方法，对提高整个捷联导航***的性能具有非常重要的军事意义和实用价值。

发明内容

本发明的目的在于提供实用性好、精度高、对准时间短的一种基于模糊变地球自转角速度的快速精对准方法。

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

本发明一种基于模糊变地球自转角速度的快速精对准方法包括以下步骤：

(1)通过外部设备确定载体的初始位置参数，并将其装订至导航计算机；

(2)SINS预热准备，采集加速度计和陀螺仪输出；

(3)利用采集到的加速度计和陀螺仪输出，通过标准的解析法进行粗对准，确定粗略的初始姿态矩阵；

(4)粗对准结束后进入精对准阶段，令计算地球自转角速度ω_e＝Ω_e，Ω_e表示真实地球自转速度；

(5)根据加速度计的输出和粗对准获得初始姿态，以及计算地球自转角速度进行卡尔曼滤波一步迭代计算；

(6)确定精对准时间t_f，使t_f的取值大于滤波的收敛时间且小于惯性导航***能够允许的最大时间。

(7)判断精对准t_f计时是否结束，若精对准已经结束则令计算地球自转角速度ω_e＝Ω_e，采用卡尔曼滤波技术估计出的失准角，并用失准角来修正***的捷联姿态矩阵，完成精确初始对准；若精对准未结束，则根据卡尔曼滤波估计误差方差Pk计算两个误差距离

和

，其中

为东向误差角估计误差的方差、为方位误差角估计误差的方差、hΔφ_D为方位角的期望稳定误差，通过模糊推理的方法估算出计算地球自转角速度ω_e＝nΩ_e，返回步骤(5)进行卡尔曼迭代计算直至精对准结束，令计算地球自转角速度ω_e＝Ω_e，采用卡尔曼滤波技术估计出的失准角，并用失准角来修正***的捷联姿态矩阵，完成精确初始对准。

本发明的优势在于：不需要增加任何外观测设备，仅需要对捷联惯性导航***的精对准算法进行更新；无需提供辅助位置转台，工程实现容易；有效提高了捷联惯性导航***的精对准的快速性。

附图说明

图1是模糊推理的输入隶属度函数；

图2是模糊推理的输出隶属度函数；

图3是捷联惯性导航***精对准结构图；

图4是本发明的流程图；

图5是不同地球自转角速度时惯导的东向误差角估计误差对比曲线；

图6是不同地球自转角速度时惯导的北向误差角估计误差对比曲线；

图7是不同地球自转角速度时惯导的方位东向误差角估计误差对比曲线；

图8是模糊推理估出的地球自转角速度与不同地球自转角速度时惯导的东向误差角估计误差对比曲线；

图9是模糊推理估出的地球自转角速度与不同地球自转角速度时惯导的北向误差角估计误差对比曲线；

图10是模糊推理估出的地球自转角速度与不同地球自转角速度时惯导的方位误差角估计误差对比曲线。

具体实施方式

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

实施方式1：

结合图1～10，本发明包括以下步骤：

(1)通过外部设备确定载体的初始位置参数，并将其装订至导航计算机；

(2)SINS预热准备，采集加速度计和陀螺仪输出；

(3)利用采集到的加速度计和陀螺仪输出，通过标准的解析法进行粗对准，确定粗略的初始姿态矩阵；

(4)粗对准结束后进入精对准阶段，令计算地球自转角速度ω_e＝Ω_e，Ω_e表示真实地球自转速度；

(5)根据加速度计的输出和粗对准获得初始姿态，以及计算地球自转角速度进行卡尔曼滤波一步迭代计算；

(6)确定精对准时间t_f，使t_f的取值大于滤波的收敛时间且小于惯性导航***能够允许的最大时间。

(7)判断精对准t_f计时是否结束，若精对准已经结束则令计算地球自转角速度ω_e＝Ω_e，采用卡尔曼滤波技术估计出的失准角，并用失准角来修正***的捷联姿态矩阵，完成精确初始对准；若精对准未结束，则根据卡尔曼滤波估计误差方差P_k计算两个误差距离

和

其中

为东向误差角估计误差的方差、

为方位误差角估计误差的方差、hΔφ_D为方位角的期望稳定误差，通过模糊推理的方法估算出计算地球自转角速度ω_e＝nΩ_e，返回步骤(5)进行卡尔曼迭代计算直至精对准结束，令计算地球自转角速度ω_e＝Ω_e，采用卡尔曼滤波技术估计出的失准角，并用失准角来修正***的捷联姿态矩阵，完成精确初始对准。

本发明还包括如下特征：

1、步骤5中用到的***状态方程和量测方程

建立精对准***的状态方程和观测方程

在东北天坐标系中，显然有

其中Ω_e表示真实地球自转速度，

表示当地的纬度，

为了研究捷联惯性***静基座下的初始对准，考虑到陀螺随机漂移误差和加速度计的随机偏差，忽略了位置误差和垂直速度误差，根据修正Bar-Itzhachk和Bermant的惯性误差模型，得到***的动态方程为：

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

Z(t)＝H(t)X(t)+V(t)    (2)

式(1)、(2)中，X(t)为***的状态向量，Z(t)为***的观测向量；A(t)为***状态转移矩阵，H(t)为***的观测矩阵；W(t)，V(t)分别为***的过程噪声向量和观测噪声向量，其均值都为零的高斯白噪声。

***的状态向量为：

$X\left(t\right)={\left[\begin{array}{cccccccccc}\mathrm{\δ}{v}_E& \mathrm{\δ}{v}_N& {\mathrm{\φ}}_E& {\mathrm{\φ}}_N& {\mathrm{\φ}}_D& {\▿}_x& {\▿}_y& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z\end{array}\right]}^T---\left(3\right)$

***的噪声向量为：

$W\left(t\right)={\left[\begin{array}{cccccccccc}{w}_{{\mathrm{\δv}}_E}& w_{{\mathrm{\δv}}_N}& w_{{\mathrm{\φ}}_E}& w_{{\mathrm{\φ}}_N}& w_{{\mathrm{\φ}}_D}& 0& 0& 0& 0& 0\end{array}\right]}^T---\left(4\right)$

式(3)、(4)中，δv_E、δv_N表示***东向和北向的速度误差，φ_E、φ_N、φ_D表示数学平台与导航平台的东向、北向和方位失准角，

表示x轴和y轴的加速度计的误差，ε_x、ε_y、ε_z表示xyz轴上的陀螺仪的误差，

表示***东向和北向的速度误差的白噪声，

表示东向、北向和方位失准角的白噪声。

***的状态转移矩阵为：

$A\left(t\right)=\left[\begin{array}{cccccccccc}0& 2{\mathrm{\Ω}}_D& 0& -g& 0& C_{11}& C_2& 0& 0& 0\\ -2{\mathrm{\Ω}}_D& 0& g& 0& 0& C_{21}& C_{22}& 0& 0& 0\\ 0& 0& 0& {\mathrm{\Ω}}_D& 0& 0& 0& C_{11}& C_{12}& C_{13}\\ 0& 0& -{\mathrm{\Ω}}_D& 0& {\mathrm{\Ω}}_n& 0& 0& C_{21}& C_{22}& C_{23}\\ 0& 0& 0& -{\mathrm{\Ω}}_N& 0& 0& 0& C_{31}& C_{32}& C_{33}\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]---\left(5\right)$

***的观测向量为：

Z(t)＝[δv_E δv_N]^T    (6)

***的观测噪声向量为：

$H=\left[\begin{array}{cccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]---\left(7\right)$

式(5)C_ij为载体坐标系相对导航坐标系的姿态矩阵的第i行和第j列元素，g为当地的地球重力角速度。

2、粗对准和精对准之间的关系

通过步骤3进行粗对准计算出来的姿态矩阵的准确性比较低，也就是说实际的导航坐标系和粗对准计算出的导航坐标系并不重合，存在一个小的偏差，它的余弦矩阵为

我们定义经过粗对准后的姿态矩阵为

，那么精确的载体坐标系与导航坐标系的方向余弦矩阵为：

$C_b^n=C_p^n{C}_b^p---\left(8\right)$

式(8)中p代表粗对准后的数学平台。

粗对准已经获得了一个粗略的姿态矩阵

，也就是说，只要获取此时数学平台与实际导航坐标的的偏差的余弦矩阵

就可以得到精确导航姿态矩阵

要获取此时数学平台与实际导航坐标的的偏差的余弦矩阵

也就是要获取此时数学平台的失准角φ。由于经过粗对准后，各个失准角是一个比较小的值，因此失准角φ与方向余弦矩阵

的关系可表示为：

$C_p^n=\left[\begin{array}{ccc}1& -{\mathrm{\φ}}_D& {\mathrm{\φ}}_N\\ {\mathrm{\φ}}_D& 1& -{\mathrm{\φ}}_E\\ -{\mathrm{\φ}}_N& {\mathrm{\φ}}_E& 1\end{array}\right]---\left(9\right)$

通过上面叙述可以知道，只要在精对准过程中获得失准角φ，就可以准确的计算出载体坐标系与导航坐标系的姿态矩阵

因此精对准的任务变为快速而准确的获取失准角φ，可以利用卡尔曼滤波状态估计出失准角φ。

3、卡尔曼滤波状态估计

在特征1、2的基础上利用标准的卡尔曼滤波方法进行迭代计算，可以估算出失准角φ。具体过程如下：

首先对***方程式(1)和观测方程式(2)进行离散化，得到

X_k＝Φ_k，k-1X_k-1+W_k-1    (10)

Z_k＝H_kX_k+V_k    (11)

式中X_k为***的状态序列，Z_k为***的观测序列；Φ_k，k-1为***状态转移矩阵，H_k为***的观测矩阵；W_k-1，V_k分别为***的过程噪声序列和观测噪声序列，其均值都为零的高斯白噪声序列。***过程噪声W_k-1的方差为Q_k-1，***观测噪声V_k的方差为R_k。

然后进行Kalman滤波：

状态一步预测： ${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}---\left(12\right)$

状态估计： ${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k[Z_k-H_k{\hat{X}}_{k,k-1}]---\left(13\right)$

滤波增益矩阵： $K_k=P_{k,k-1}{H}_k^T{[H_k{P}_{k,k-1}{H}_k^T+R_k]}^{-1}---\left(14\right)$

一步预测误差方差矩阵： $P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+Q_{k-1}---\left(15\right)$

估计误差方差矩阵： $P_k=[I-K_k{H}_k]P_{k,k-1}{[I-K_k{H}_k]}^T+K_k{R}_k{K}_k^t---\left(16\right)$

滤波初始值的选择可以参考关于用标准kalman滤波来完成初始对准的文献。

4、步骤7中根据卡尔曼滤波估计误差方差P_k计算两个误差距离

和

误差角φ的估计误差方差矩阵：M_k＝CP_kC^T    (17)

式(17)中C表示误差角观测矩阵，P_k表示kalman估计误差方差矩阵，

C＝[0_3×2 I_3×3 0_3×5]。

所以

$M_k=\left[\begin{array}{ccc}{M}_{11}^k& M_{12}^k& M_{13}^k\\ 0& M_{22}^k& M_{23}^k\\ 0& 0& M_{33}^k\end{array}\right]---\left(18\right)$

定义两个误差距离

$S_1^k=|\sqrt{M_{11}^k}-\sqrt{M_{33}^k}|---\left(19\right)$

$S_2^k=\sqrt{M_{33}^k}-\mathrm{h\Δ}{\mathrm{\φ}}_D---\left(20\right)$

表示k时刻东向误差角估计误差的方差，表示k时刻北向误差角估计误差的方差，表示k时刻方位误差角估计误差的方差，表示水平误差与方位误差之间的耦合估计误差的方差，

表示k时刻东向误差角与方位误差角的距离，

表示k时刻方位误差角到稳态值的距离，h表示方位误差角稳态误差的权值，Δφ_D为方位角的稳定误差。由于水平误差角的收敛速度与收敛误差相似，所以

与

也相近。

定义计算地球自转角速度：

ω_e＝nΩ_e    (21)

式(21)中Ω_e表示真实地球自转角速度，n表示增加地球自转角速度的权值。

可以根据

和

的大小来确定增加地球自转角速度的权值n的大小，由于n三者之间的关系是一个复杂的非线性关系，很难用一个数学关系式表达出来，因此本发明根据

和

用模糊推理的方法估算出权值n。

5、通过模糊推理的方法估算出计算地球自转角速度ω_e＝nΩ_e首先对两个距离和

进行归一化处理，得到两个新的归一化距离：

${\hat{S}}_i^k=\frac{S_i^k}{S_1^k+S_2^k},i=\mathrm{1,2}.---\left(22\right)$

设量化指标

的论域空间为A_i(i＝1，2)。设

具有相同的论域空间，即有A₁＝A₂，在论域上定义输入变量

模糊子集为：SP(正小)、MP(正中)、LP(正大)，选定模糊子集的隶属函数为高斯型函数：

如图1所示。式中：c及σ为表示模糊推理***量化输入的分布参数(设计参数)，经过模糊推理后，得到

的模糊值s_j∈[0，1]。

同样，在模糊推理***的输出空间[0，1]，定义相同的模糊子集SP(正小)、MP(正中)、LP(正大)，采用三角函数作为隶属函数，如图2所示。

在将上述的距离变量进行模糊化之后，采用如下的模糊推理法则进行推理推出权值

规则1：if s₁＝SP and s₂＝SP then N＝SP；

规则2：if s₁＝SP and s₂＝MP then N＝MP；

……

根据专家经验采用如表1所示的模糊规则表进行推理。

表1模糊规则表

根据上述模糊规则，可以得到k时刻的权值的模糊值N，将模糊值N反模糊化得到权值

地球自转角速度的权值

ω_e＝nΩ_e，F为地球自转角速度的权值允许变化最大范围(通常可以取100～5000)。

本发明利用模糊推理的方式实时的估算出合适的计算地球自转角速度，不仅加快了方位误差角的收敛速度，而且保证了水平误差和方位误差的稳态精度。对准的流程图如图4所示，精对准***的结构图如图3所示，该精对准方法，既不需要增加任何传感器，也不需要位置的转台，只要需要修改捷联惯导***的初始对准的软件就可以极大的提高精对准的速度，容易工程实现。

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

(1)改变地球自转角速度的影响

计算地球自转角速度为ω_e＝nΩ_e，现分别取n＝1，100，1000，5000时，也就是分别将计算地球自转角速度提高至1倍，100倍，1000倍，5000倍。图5-7分别为将计算地球自转角速度提高至1倍，100倍，1000倍，5000倍时东向误差、北向误差、方位误差仿真图(为了便于观察由增加计算地球自转角速度而引起的误差，将***噪声和观测噪声去除)。可以看出随权值n增大，水平误差(图5、6)的收敛速度没有太多的改变，但是方位误差(图7)的收敛速度却快速的提高。不过当权值n＝5000时，水平误差和方位误差都会发生振荡。这说明提高计算地球自转角速度能够提高初始对准的快速性，不过不是计算地球自转角速度越大越好，计算地球自转角速度大到一定程度时，稳态误差会发生振荡和发散。这就是本发明要根据误差距离实时估算计算地球自转角速度的原因，只有这样才能保证方位误差收敛的快速性和稳态误差的平稳性。

(2)利用模糊推理的方法确定权值n对收敛速度的影响。

计算地球自转角速度到底增加到多少时，能够保证水平误差和方位误差不会发生振荡或发散，同时能够保证最快的收敛速度呢？这个问题很难确切的回答，***在不同的阶段需要不同的计算地球自转角速度才能保证最优误差特性，而且不同***的特性和不同噪声特性，为了获得最优误差特性，在不同的阶段所需要的计算地球自转角速度也不尽相同。因此最优的误差特性很难得到，但可通过次优的方法估出计算地球自转角速度。本发明通过定义两个误差距离和

根据这两个距离通过模糊推理的方式实时估算出权值n，比较好的解决了这个问题。图8-10分别是通过模糊推理的方法估算权值n和固定权值的的比较图(为了便于观察由增加计算地球自转角速度而引起的误差，将***噪声和观测噪声去除)。从这些图中可以看出，通过模糊推理的方式估算出权值n方法使得水平误差(图8、9)的收敛速度明显提高，而且方位误差(图10)的收敛速度极快，45秒就完全收敛且稳态误差平稳，没有发生发散和振荡。

Claims (1)

1.一种基于模糊方法改变地球自转角速度的快速精对准方法，其特征是：

（1）通过外部设备确定载体的初始位置参数，并将其装订至导航计算机；

（2）SINS预热准备，采集加速度计和陀螺仪输出；

（3）利用采集到的加速度计和陀螺仪输出，通过标准的解析法进行粗对准，确定粗略的初始姿态矩阵；

（4）粗对准结束后进入精对准阶段，令计算地球自转角速度ω_e＝Ω_e，Ω_e表示真实地球自转速度；

（5）根据加速度计的输出和粗对准获得的初始姿态，以及计算地球自转角速度进行卡尔曼滤波迭代计算；

（6）确定精对准时间t_f，使t_f的取值大于卡尔曼滤波的收敛时间且小于SINS能够允许的最大时间；

（7）判断精对准t_f计时是否结束，若精对准未结束，则根据卡尔曼滤波估计误差方差P_k计算两个误差距离

和

其中

为东向误差角估计误差的方差、

为方位误差角估计误差的方差、hΔφ_D为方位角的期望稳定误差，通过模糊推理的方法估算出计算地球自转角速度ω_e＝nΩ_e，n表示增加地球自转角速度的权值，返回步骤（5）进行卡尔曼迭代计算直至精对准结束，令计算地球自转角速度ω_e＝Ω_e，采用卡尔曼滤波技术估计出的失准角，并用失准角来修正***的捷联姿态矩阵，完成精确初始对准；若精对准已经结束则令计算地球自转角速度ω_e＝Ω_e，采用卡尔曼滤波技术估计出的失准角，并用失准角来修正***的捷联姿态矩阵，完成精确初始对准。

Images