CN102305636B - 一种基于非线性初始对准模型的快速对准方法 - Google Patents

Abstract

本发明公开了一种基于非线性初始对准模型的快速对准方法，主要步骤包括：合理编排捷联惯性导航系统的惯性器件；建立水平姿态角和航向角计算模型；建立非线性初始对准模型；构建用于初始对准的扩展kalman滤波器；由导航计算机，采集惯性器件输出信息，并完成初始对准滤波。本发明巧妙的冗余编排惯性系统水平方向上的惯性器件，充分利用惯性器件的冗余量测信息，建立的非线性初始对准模型。该模型直接利用惯性器件的量测输出作为观测值，3个失准角作为状态变量，有效的降低了模型的维数和提高了模型系统的可观测度，从而保证了初始对准的精度和速度。本发明具有实用性好、精度高、对准时间短的特点。

Description

一种基于非线性初始对准模型的快速对准方法

技术领域

本发明涉及一种捷联惯性导航系统静基座下初始对准方法，可用于捷联惯性系统的初始姿态的确定，特别适合于需要快速对准的情况。

背景技术

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

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

以速度误差作为观测量的静基座精对准系统的可观测性和可观测度差，是制约其对准精度和快速性的重要原因，特别是方位角的观测度低，导致方位失准角的估计效果很差。关于快速对准，已有的文献的常规思路都是如何提高方位角的可观测度，如增加方位传感器或者多位置对准等。如专利号：201010142839.0的发明专利：一种基于粒子滤波的惯性导航系统初始对准方法；建立了基于二位置的非线性初始对准模型，该方法需要精密的位置转台，限制了工程应用，同时在对准速度上提高不明显。如专利号：ZL200510130615.7的发明专利：一种捷联惯性导航系统的任意双位置初始对准方法；如专利号：ZL200810064146.7的发明专利：基于滤波的光纤陀螺捷联惯导系统两位置初始对准方法；是在对准过程中利用位置转台进行多位置对准来提高系统的可观性和可观度，虽然这些方法已经不需要难以实现的精密位置转台了，但还是需要在位置转台上进行，限制了工程应用。如专利号：200810019357.9的发明专利：捷联惯性导航系统的快速精对准方法；是通过增加磁传感器和倾角传感器来提高捷联惯导系统的可观性，但是该方法利用的方位传感器一般都是地磁传感器，这种地磁传感很容易收外部的干扰，尤其载体是磁导体时，地磁传感很难正常工作，很难实际使用。因此在实际工程应用中，找到一种能够提高惯性导航系统的对准精度和反应能力的方法，对提高整个捷联导航系统的性能具有非常重要的军事意义和实用价值。本发明巧妙的冗余编排惯性系统水平方向上的惯性器件，充分利用惯性器件的冗余量测信息，建立的非线性初始对准模型。该模型直接利用惯性器件的量测输出作为观测值，3个失准角作为状态变量，有效的降低了模型的维数和提高了模型系统的可观测度，从而保证了初始对准的精度和速度。该方法是一种实用性好、精度高、对准时间短的精对准方法，国内外还未发现有人提出利用惯性器件的冗余量测信息来建立非线性初始对准模型来实现快速对准。

发明内容

本发明的目的是针对现有技术的不足，提供一种精确快速、方便的基于非线性初始对准模型的快速对准方法。该方法可以实现惯性导航系统的快速静基座自主初始对准。

为了达到上述目的，本发明采用的技术方案是：一种基于非线性初始对准模型的快速对准方法，包括如下步骤：

步骤1、将水平方向上的惯性器件正交冗余配置，同一坐标轴的惯性器件的量测轴相位相差180度，即第一陀螺1-1和第一加速度计2-1的量测轴方向为x轴的正向，第四陀螺1-4和第四加速度2-4的量测轴方向为x轴的反向，第二陀螺1-2和第二加速度计2-2的量测轴方向为y轴的正向，第五陀螺1-5和第五加速度2-5的量测轴方向为y轴的反向，第三陀螺1-3和第三加速度计2-3的量测轴方向为z轴的正向；

步骤2、根据步骤1中的惯性器件的配置，建立水平姿态角和航向角计算模型；

俯仰角： $\mathrm{\θ}=\arcsin \frac{(f_y^2-f_y^5)-({\▿}_y^2-{\▿}_y^5)-(w_y^2-w_y^5)}{2g},$

横滚角： $\mathrm{\γ}=\arcsin \frac{(f_x^1-f_x^4)-({\▿}_x^1-{\▿}_x^4)-(w_x^1-w_x^4)}{-2\cos \mathrm{\θ}\·g},$

航向角： $\mathrm{\ψ}=\arccos \frac{({\mathrm{\ω}}_y^2-{\mathrm{\ω}}_y^5)-({\mathrm{\ϵ}}_y^2-{\mathrm{\ϵ}}_y^5)-(v_y^2-v_y^5)-2\sin \mathrm{\θ}\·{\mathrm{\ω}}_{\mathrm{ie}}\sin L}{2\cos \mathrm{\θ}\·{\mathrm{\ω}}_{\mathrm{ie}}\cos L},$ 或者

$(-2\cos \mathrm{\γ}\sin \mathrm{\ψ}+2\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}){\mathrm{\ω}}_{\mathrm{ie}}\mathrm{conL}-2\sin \mathrm{\γ}\cos \mathrm{\θ}\·{\mathrm{\ω}}_{\mathrm{ie}}\sin L=({\mathrm{\ω}}_x^1-{\mathrm{\ω}}_x^4)-({\mathrm{\ϵ}}_x^1-{\mathrm{\ϵ}}_x^4)-(v_x^1-v_x^4),$

式中：

表示第一加速度计2-1在x轴方向的量测值，

表示第四加速度计2-4在x轴方向的量测值，表示第一加速度计2-1的随机常值，

表示第四加速度计2-4的随机常值，

表示第一加速度计2-1的量测噪声，

表示第四加速度计2-4的量测噪声，

表示第二加速度计2-2在y轴方向的量测值，

表示第五加速度计2-5在y轴方向的量测值，

表示第二加速度计2-2的随机常值，

表示第五加速度计2-5的随机常值，

表示第二加速度计2-2的量测噪声，

表示第五加速度计2-5的量测噪声，表示第一陀螺1-1在x轴方向的量测值，

表示第四陀螺1-4在x轴方向的量测值，

表示第一陀螺1-1的随机常值，

表示第四陀螺1-4的随机常值，

表示第一陀螺1-1的量测噪声，

表示第四陀螺1-4的量测噪声，

表示第二陀螺1-2在y轴方向的量测值，

表示第五陀螺1-5在y轴方向的量测值，

表示第二陀螺1-2的随机常值，

表示第五陀螺1-5的随机常值，

表示第二陀螺1-2的量测噪声，

表示第五陀螺1-5的量测噪声，θ表示俯仰角，γ表示横滚角，ψ表示航向角，g表示重力矢量，ω_ie表示地球的自转角速度，L为当地的地理纬度；

步骤3、根据步骤2中的3个姿态角的计算模型，可以建立非线性初始对准模型；

$\left[\begin{array}{c}{\mathrm{\θ}}_k\\ {\mathrm{\γ}}_k\\ {\mathrm{\ψ}}_k\end{array}\right]=\left[\begin{array}{c}{\mathrm{\θ}}_{k-1}\\ {\mathrm{\γ}}_{k-1}\\ {\mathrm{\ψ}}_{k-1}\end{array}\right]+\left[\begin{array}{c}{w}_{\mathrm{\θ}}\\ w_{\mathrm{\γ}}\\ w_{\mathrm{\ψ}}\end{array}\right]$

$\left[\begin{array}{c}{f}_{\mathrm{yk}}^2-f_{\mathrm{yk}}^5\\ f_{\mathrm{xk}}^1-f_{\mathrm{xk}}^4\\ {\mathrm{\ω}}_{\mathrm{xk}}^1-{\mathrm{\ω}}_{\mathrm{xk}}^4\end{array}\right]=\left[\begin{array}{c}2\sin {\mathrm{\θ}}_k\·g\\ -2\sin {\mathrm{\γ}}_k\cos {\mathrm{\θ}}_k\·g\\ (-2\cos {\mathrm{\γ}}_k\sin {\mathrm{\ψ}}_k+2\sin {\mathrm{\γ}}_k\sin {\mathrm{\θ}}_k\cos {\mathrm{\ψ}}_k){\mathrm{\ω}}_{\mathrm{ie}}\cos L-2\sin {\mathrm{\γ}}_k\cos {\mathrm{\θ}}_k\·{\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right]-\left[\begin{array}{c}{\▿}_y^2-{\▿}_y^5\\ {\▿}_x^1-{\▿}_x^4\\ {\mathrm{\ϵ}}_x^1-{\mathrm{\ϵ}}_x^4\end{array}\right]-\left[\begin{array}{c}{w}_{\mathrm{yk}}^2-w_{\mathrm{yk}}^5\\ w_{\mathrm{xk}}^1-w_{\mathrm{xk}}^4\\ v_{\mathrm{xk}}^1-v_{\mathrm{xk}}^4\end{array}\right]$

式中：θ_k表示k时刻的俯仰角，γ_k表示k时刻的横滚角，ψ_k表示k时刻的航向角，w_θ表示俯仰角的微小波动值，w_γ表示横滚角的微小波动值，w_ψ表示航向角的微小波动值，

表示k时刻第一加速度计2-1的x轴方向量测值，

表示k时刻第四加速度计2-4的x轴方向量测值，

表示k时刻第二加速度计2-2的y轴方向量测值，

表示k时刻第五加速度计2-5的y轴方向量测值，

表示k时刻第一陀螺1-1的x轴方向量测值，表示k时刻第四陀螺1-4的x轴方向量测值；

步骤4、根据步骤3中的初始对准模型构建用于初始对准的扩展kalman滤波器；

步骤5、由导航计算机采集惯性器件输出信息，并完成初始对准滤波。

本发明还包括如下特征：

1、步骤1捷联惯性导航中的惯性器件的编排

在水平方向上，冗余配置惯性器件，在东向和北向轴上分别有两套的陀螺仪和加速度计，且分别背靠背安装。具体的配置如附图1。图中第一加速度计2-1与第四加速度计2-4的量测轴的相位相差180°，第二加速度计2-2与第五加速度计2-5的量测轴的相位相差180°，第一陀螺1-1与第四陀螺1-4的测量轴的相位也相差180°，第二陀螺1-2与第五陀螺1-5的测量轴的相位也相差180°。

为了方便以下的模型推导，假设捷联惯性导航系统的惯性器件的编排如附图2。图中第一加速度计2-1与第四加速度计2-4的量测轴的相位相差180°，第二加速度计2-2与第五加速度计2-5的量测轴的相位相差180°，第三加速度计2-3与第六加速度计2-6的量测轴的相位相差180°，第一陀螺1-1与第四陀螺1-4的测量轴的相位也相差180°，第二陀螺1-2与第五陀螺1-5的测量轴的相位也相差180°，第三陀螺1-3与第六陀螺1-6的测量轴的相位也相差180°。

2、步骤2中水平姿态角计算模型的建立

假定静基座情况下，惯性导航系统具有一定的姿态角，其俯仰角为θ，横滚角为γ，航向角为ψ，用fⁿ和ωⁿ分别表示当地导航系中的重力加速度和地球自转角速度，用f^b和ω^b分别表示载体系中的加速度计和陀螺仪的测量值，则：fⁿ＝[0 0 g]^T，ωⁿ＝[0 ω_ie cosL ω_ie sinL]^T，f^b＝[f_x f_y f_z]^T，ω^b＝[ω_x ω_y ω_z]^T式中，g表示重力矢量，ω_ie表示地球的自转角速度，L为当地的地理纬度，T表示矩阵的转置，b表示载体坐标系，n表示导航坐标系。

加速度计输出为：

$f^b=C_n^b{f}^n+\mathrm{\Δ}{f}^b=C_n^b{f}^n+{\▿}^b+w^b---\left(1\right)$

式中，

表示各轴加速度计在载体坐标系下的随机常值，f^b代表载体坐标系下的加速度计输出，Δf^b代表加速度计误差，w^b代表载体坐标系下加速度计输出的测量噪声，考虑为白噪声，

表示了从n坐标系到b坐标系的转换关系矩阵，满足：

$C_n^b=\left[\begin{array}{ccc}\cos \mathrm{\γ}& 0& -\sin \mathrm{\γ}\\ 0& 1& 0\\ \sin \mathrm{\γ}& 0& \cos \mathrm{\γ}\end{array}\right]\·\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{\ψ}& -\sin \mathrm{\ψ}& 0\\ \sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ 0& 0& 1\end{array}\right]---\left(2\right)$

由(1)式可得 $C_n^b{f}^n=f^b-{\▿}^b-w^b---\left(3\right)$

第一加速度计2-1、第二加速度计2-2、第三加速度计2-3的比力可表示为：

$f_{1-3}^b=C_n^b{f}^n+{\▿}_{1-3}^b+w_{1-3}^b---\left(4\right)$

$C_n^b{f}^n=f_{1-3}^b-{\▿}_{1-3}^b-w_{1-3}^b---\left(5\right)$

将fⁿ和f^b带入式(5)，可得：

$C_n^b\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]=\left[\begin{array}{c}{f}_x^1\\ f_y^2\\ f_z^3\end{array}\right]-\left[\begin{array}{c}{\▿}_x^1\\ {\▿}_y^2\\ {\▿}_z^3\end{array}\right]-\left[\begin{array}{c}{w}_x^1\\ w_y^2\\ w_z^3\end{array}\right]---\left(6\right)$

$\left[\begin{array}{c}-\sin \mathrm{\γ}\cos \mathrm{\θ}\·g\\ \sin \mathrm{\θ}\·g\\ \cos \mathrm{\γ}\cos \mathrm{\θ}\·g\end{array}\right]=\left[\begin{array}{c}{f}_x^1\\ f_y^2\\ f_z^3\end{array}\right]-\left[\begin{array}{c}{\▿}_x^1\\ {\▿}_y^2\\ {\▿}_z^3\end{array}\right]-\left[\begin{array}{c}{w}_x^1\\ w_y^2\\ w_z^3\end{array}\right]---\left(7\right)$

由于第四加速度计2-4、第五加速度计2-5、第六加速度计2-6的安装相位分别与第一加速度计2-1、第二加速度计2-2、第三加速度计2-3相差180°，因此第四加速度计2-4、第五加速度计2-5、第六加速度计2-6的比力可表示为：

$f_{4-6}^b=C_t{C}_n^b{f}^n+{\▿}_{4-6}^b+w_{4-6}^b---\left(8\right)$

将式(8)移项可以得到：

$C_t{C}_n^b{f}^n=f_{4-6}^b-{\▿}_{4-6}^b-w_{4-6}^b---\left(9\right)$

其中 $C_t=\left[\begin{array}{ccc}-1& 0& 0\\ 0& -1& 0\\ 0& 0& -1\end{array}\right]$

将fⁿ、f^b和C_t带入式(9)，可得：

$C_t{C}_n^b\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]=\left[\begin{array}{c}{f}_x^4\\ f_y^5\\ f_z^6\end{array}\right]-\left[\begin{array}{c}{\▿}_x^4\\ {\▿}_y^5\\ {\▿}_z^6\end{array}\right]-\left[\begin{array}{c}{w}_x^4\\ w_y^5\\ w_z^6\end{array}\right]---\left(10\right)$

$\left[\begin{array}{c}\sin \mathrm{\γ}\cos \mathrm{\θ}\·g\\ -\sin \mathrm{\θ}\·g\\ -\cos \mathrm{\γ}\cos \mathrm{\θ}\·g\end{array}\right]=\left[\begin{array}{c}{f}_x^4\\ f_y^5\\ f_z^6\end{array}\right]-\left[\begin{array}{c}{\▿}_x^4\\ {\▿}_y^5\\ {\▿}_z^6\end{array}\right]-\left[\begin{array}{c}{w}_x^4\\ w_y^5\\ w_z^6\end{array}\right]---\left(11\right)$

将式(7)减去式(11)，可得：

$\left[\begin{array}{c}-2\sin \mathrm{\γ}\cos \mathrm{\θ}\·g\\ 2\sin \mathrm{\θ}\·g\\ 2\cos \mathrm{\γ}\cos \mathrm{\θ}\·g\end{array}\right]=\left[\begin{array}{c}{f}_x^1-f_x^4\\ f_y^2-f_y^5\\ f_z^3-f_z^6\end{array}\right]-\left[\begin{array}{c}{\▿}_x^1-{\▿}_x^4\\ {\▿}_y^2-{\▿}_y^5\\ {\▿}_z^3-{\▿}_z^6\end{array}\right]-\left[\begin{array}{c}{w}_x^1-w_x^4\\ w_y^2-w_y^5\\ w_z^3-w_z^6\end{array}\right]---\left(12\right)$

由式(12)可以得到：

$\mathrm{\θ}=\arcsin \frac{(f_y^2-f_y^5)-({\▿}_y^2-{\▿}_y^5)-(w_y^2-w_y^5)}{2g}---\left(13\right)$

$\mathrm{\γ}=\arcsin \frac{(f_x^1-f_x^4)-({\▿}_x^1-{\▿}_x^4)-(w_x^1-w_x^4)}{-2\cos \mathrm{\θ}\·g}---\left(14\right)$

3、步骤2中航向角计算模型的建立

与水平姿态角求解方法类似，静基座情况下陀螺输出为：

${\mathrm{\ω}}^b=C_n^b{\mathrm{\ω}}^n+\mathrm{\Δ}{\mathrm{\ω}}^b=C_n^b{\mathrm{\ω}}^n+{\mathrm{\ϵ}}^b+v^b---\left(15\right)$

式中，ε^b代表各轴陀螺在载体坐标系下的随机常值，ω^b代表载体坐标系下的陀螺输出，Δω^b代表陀螺误差，v^b代表载体坐标系下陀螺输出的测量噪声，考虑为白噪声。

$C_n^b{\mathrm{\ω}}^n={\mathrm{\ω}}^b-{\mathrm{\ϵ}}^b-v^b---\left(16\right)$

第一陀螺1-1、第二陀螺1-2、第三陀螺1-3的输出为：

${\mathrm{\ω}}_{1-3}^b=C_n^b{\mathrm{\ω}}^n+{\mathrm{\ϵ}}_{1-3}^b+v_{1-3}^b---\left(17\right)$

$C_n^b{\mathrm{\ω}}^n={\mathrm{\ω}}_{1-3}^b-{\mathrm{\ϵ}}_{1-3}^b-v_{1-3}^b---\left(18\right)$

将ωⁿ和ω^b带入式(18)，可得：

$C_n^b\left[\begin{array}{c}0\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L\\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_x^1\\ {\mathrm{\ω}}_y^2\\ {\mathrm{\ω}}_z^3\end{array}\right]-\left[\begin{array}{c}{\mathrm{\ϵ}}_x^1\\ {\mathrm{\ϵ}}_y^2\\ {\mathrm{\ϵ}}_z^3\end{array}\right]-\left[\begin{array}{c}{v}_x^1\\ v_y^2\\ v_z^3\end{array}\right]---\left(19\right)$

$\left[\begin{array}{c}(-\cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}){\mathrm{\ω}}_{\mathrm{ie}}\mathrm{conL}-\sin \mathrm{\γ}\cos \mathrm{\θ}\·{\mathrm{\ω}}_{\mathrm{ie}}\sin L\\ \cos \mathrm{\θ}\cos \mathrm{\ψ}\·{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\sin \mathrm{\θ}\·{\mathrm{\ω}}_{\mathrm{ie}}\sin L\\ (-\sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}){\mathrm{\ω}}_{\mathrm{ie}}\cos L+\cos \mathrm{\γ}\cos \mathrm{\θ}\·{\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_x^1\\ {\mathrm{\ω}}_y^2\\ {\mathrm{\ω}}_z^3\end{array}\right]-\left[\begin{array}{c}{\mathrm{\ϵ}}_x^1\\ {\mathrm{\ϵ}}_y^2\\ {\mathrm{\ϵ}}_z^3\end{array}\right]-\left[\begin{array}{c}{v}_x^1\\ v_y^2\\ v_z^3\end{array}\right]---\left(20\right)$

由于第四陀螺1-4、第五陀螺1-5、第六陀螺1-6的安装相位分别与第一陀螺1-1、第二陀螺1-2、第三陀螺1-3相差180°，因此第四陀螺1-4、第五陀螺1-5、第六陀螺1-6的输出可表示为：

${\mathrm{\ω}}_{4-6}^b={C_{t}C}_n^b{\mathrm{\ω}}^n+{\mathrm{\ϵ}}_{4-6}^b+v_{4-6}^b---\left(21\right)$

$C_t{C}_n^b{\mathrm{\ω}}^n={\mathrm{\ω}}_{4-6}^b-{\mathrm{\ϵ}}_{4-6}^b-v_{4-6}^b---\left(22\right)$

将ωⁿ、ω^b和C_t带入式(22)，可得：

$C_t{C}_n^b\left[\begin{array}{c}0\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L\\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_x^4\\ {\mathrm{\ω}}_y^5\\ {\mathrm{\ω}}_z^6\end{array}\right]-\left[\begin{array}{c}{\mathrm{\ϵ}}_x^4\\ {\mathrm{\ϵ}}_y^5\\ {\mathrm{\ϵ}}_z^6\end{array}\right]-\left[\begin{array}{c}{v}_x^4\\ v_y^5\\ v_z^6\end{array}\right]---\left(23\right)$

$\left[\begin{array}{c}(\cos \mathrm{\γ}\sin \mathrm{\ψ}-\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}){\mathrm{\ω}}_{\mathrm{ie}}\mathrm{conL}+\sin \mathrm{\γ}\cos \mathrm{\θ}\·{\mathrm{\ω}}_{\mathrm{ie}}\sin L\\ -\cos \mathrm{\θ}\cos \mathrm{\ψ}\·{\mathrm{\ω}}_{\mathrm{ie}}\cos L-\sin \mathrm{\θ}\·{\mathrm{\ω}}_{\mathrm{ie}}\sin L\\ (\sin \mathrm{\γ}\sin \mathrm{\ψ}+\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}){\mathrm{\ω}}_{\mathrm{ie}}\cos L-\cos \mathrm{\γ}\cos \mathrm{\θ}\·{\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_x^4\\ {\mathrm{\ω}}_y^5\\ {\mathrm{\ω}}_z^6\end{array}\right]-\left[\begin{array}{c}{\mathrm{\ϵ}}_x^4\\ {\mathrm{\ϵ}}_y^5\\ {\mathrm{\ϵ}}_z^6\end{array}\right]-\left[\begin{array}{c}{v}_x^4\\ v_y^5\\ v_z^6\end{array}\right]---\left(24\right)$

将式(20)减去式(24)，可得：

$\left[\begin{array}{c}(-2\cos \mathrm{\γ}\sin \mathrm{\ψ}+2\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}){\mathrm{\ω}}_{\mathrm{ie}}\mathrm{conL}-2\sin \mathrm{\γ}\cos \mathrm{\θ}\·{\mathrm{\ω}}_{\mathrm{ie}}\sin L\\ 2\cos \mathrm{\θ}\cos \mathrm{\ψ}\·{\mathrm{\ω}}_{\mathrm{ie}}\cos L+2\sin \mathrm{\θ}\·{\mathrm{\ω}}_{\mathrm{ie}}\sin L\\ (-2\sin \mathrm{\γ}\sin \mathrm{\ψ}-2\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}){\mathrm{\ω}}_{\mathrm{ie}}\cos L+2\cos \mathrm{\γ}\cos \mathrm{\θ}\·{\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_x^1-{\mathrm{\ω}}_x^4\\ {\mathrm{\ω}}_y^2-{\mathrm{\ω}}_y^5\\ {\mathrm{\ω}}_z^3-{\mathrm{\ω}}_z^6\end{array}\right]$

(25)

$-\left[\begin{array}{c}{\mathrm{\ϵ}}_x^1-{\mathrm{\ϵ}}_x^4\\ {\mathrm{\ϵ}}_y^2-{\mathrm{\ϵ}}_y^5\\ {\mathrm{\ϵ}}_z^3-{\mathrm{\ϵ}}_z^6\end{array}\right]-\left[\begin{array}{c}{v}_x^1-v_x^4\\ v_y^2-v_y^5\\ v_z^3-v_z^6\end{array}\right]$

由式(25)可以得到：

$\mathrm{\ψ}=\arccos \frac{({\mathrm{\ω}}_y^2-{\mathrm{\ω}}_y^5)-({\mathrm{\ϵ}}_y^2-{\mathrm{\ϵ}}_y^5)-(v_y^2-v_y^5)-2\sin \mathrm{\θ}\·{\mathrm{\ω}}_{\mathrm{ie}}\sin L}{2\cos \mathrm{\θ}\·{\mathrm{\ω}}_{\mathrm{ie}}\cos L}---\left(26\right)$

或 $(-2\cos \mathrm{\γ}\sin \mathrm{\ψ}+2\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}){\mathrm{\ω}}_{\mathrm{ie}}\mathrm{conL}-2\sin \mathrm{\γ}\cos \mathrm{\θ}\·{\mathrm{\ω}}_{\mathrm{ie}}\sin L=({\mathrm{\ω}}_x^1-{\mathrm{\ω}}_x^4)-({\mathrm{\ϵ}}_x^1-{\mathrm{\ϵ}}_x^4)-(v_x^1-v_x^4)$

4、步骤3中非线性初始对准模型的建立

根据以上推导，可以建立非线性初始对准模型。考虑初始对准时惯性导航系统具有一定的姿态角，各个姿态角可认为是一固定不变的角度，由于受外界干扰，表现为围绕某一固定角度微小波动。因此初始对准状态方程如下：

$X_k=\left[\begin{array}{c}{\mathrm{\θ}}_k\\ {\mathrm{\γ}}_k\\ {\mathrm{\ψ}}_k\end{array}\right]=\left[\begin{array}{c}{\mathrm{\θ}}_{k-1}\\ {\mathrm{\γ}}_{k-1}\\ {\mathrm{\ψ}}_{k-1}\end{array}\right]+\left[\begin{array}{c}{w}_{\mathrm{\θ}}\\ w_{\mathrm{\γ}}\\ w_{\mathrm{\ψ}}\end{array}\right]---\left(27\right)$

$\left[\begin{array}{c}{f}_{\mathrm{yk}}^2-f_{\mathrm{yk}}^5\\ f_{\mathrm{xk}}^1-f_{\mathrm{xk}}^4\\ {\mathrm{\ω}}_{\mathrm{xk}}^1-{\mathrm{\ω}}_{\mathrm{xk}}^4\end{array}\right]=\left[\begin{array}{c}2\sin {\mathrm{\θ}}_k\·g\\ -2\sin {\mathrm{\γ}}_k\cos {\mathrm{\θ}}_k\·g\\ (-2\cos {\mathrm{\γ}}_k\sin {\mathrm{\ψ}}_k+2\sin {\mathrm{\γ}}_k\sin {\mathrm{\θ}}_k\cos {\mathrm{\ψ}}_k){\mathrm{\ω}}_{\mathrm{ie}}\cos L-2\sin {\mathrm{\γ}}_k\cos {\mathrm{\θ}}_k\·{\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right]-\left[\begin{array}{c}{\▿}_y^2-{\▿}_y^5\\ {\▿}_x^1-{\▿}_x^4\\ {\mathrm{\ϵ}}_x^1-{\mathrm{\ϵ}}_x^4\end{array}\right]-\left[\begin{array}{c}{w}_{\mathrm{yk}}^2-w_{\mathrm{yk}}^5\\ w_{\mathrm{xk}}^1-w_{\mathrm{xk}}^4\\ v_{\mathrm{xk}}^1-v_{\mathrm{xk}}^4\end{array}\right]---\left(28\right)$

令 $\mathrm{NC}=\left[\begin{array}{c}{\▿}_y^2-{\▿}_y^5\\ {\▿}_x^1-{\▿}_x^4\\ {\mathrm{\ϵ}}_x^1-{\mathrm{\ϵ}}_x^4\end{array}\right],$ $\mathrm{NR}=\left[\begin{array}{c}{w}_{\mathrm{yk}}^2-w_{\mathrm{yk}}^5\\ w_{\mathrm{xk}}^1-w_{\mathrm{xk}}^4\\ v_{\mathrm{xk}}^1-v_{\mathrm{xk}}^4\end{array}\right]$

$h\left(X\right)=\left[\begin{array}{c}2\sin {\mathrm{\θ}}_k\·g\\ -2\sin {\mathrm{\γ}}_k\cos {\mathrm{\θ}}_k\·g\\ (-2\cos {\mathrm{\γ}}_k\sin {\mathrm{\ψ}}_k+2\sin {\mathrm{\γ}}_k\sin {\mathrm{\θ}}_k\cos {\mathrm{\ψ}}_k){\mathrm{\ω}}_{\mathrm{ie}}\cos L-2\sin {\mathrm{\γ}}_k\cos {\mathrm{\θ}}_k\·{\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right]$

5、步骤4中扩展kalman滤波器滤波估计

状态估计系统的状态方程和量测方程分别为：

X_k+1＝X_k+W_k    (29)

z_k＝h(X)    (30)

Kalman滤波算法如下：

P_k+1/k＝P_k    (31)

$K_{k+1}=P_{k+1/k}{H}_{k+1}^T{(H_{k+1}{P}_{k+1/k}{H}_{k+1}^T+R_{k+1})}^{-1}---\left(32\right)$

P_k+1＝(I-K_k+1H_k+1)P_k+1/k    (33)

${\hat{X}}_{k+1}={\hat{X}}_k+K_{k+1}(z_{k+1}-h\left({\hat{X}}_k\right))---\left(34\right)$

$H_{k+1}=\frac{\∂}{\∂d}h\left(X\right){|}_{X={\hat{X}}_k}---\left(35\right)$

其中K_k是观测增益，

是状态X_k的估计值，z_k是观测值，P_k是状态协方差矩阵，R_k是观测噪声协方差矩阵。h(·)是传感器量测方程。

以速度误差作为观测量的静基座精对准系统，东向误差角和北向误差角能够很快的收敛，但是方位误差角却收敛很慢。究其原因是方位误差角的观测度比水平误差角的观测度要差，关于改善观测度的方法，已经有很多文献给出来了。本发明涉及的方法提供了一个非线性初始对准模型，直接利用惯性器件的量测输出作为观测值，3个失准角作为状态变量，有效的降低了模型的维数和提高了模型系统的可观测度，从而保证了初始对准的精度和速度。

本发明与现有的技术相比具有以下主要优点和有益效果：

(1)将系统的维数降至三维，降低了初始对准的计算复杂度，保证了对准滤波的实时性；

(2)无需提供精密的位置转台，工程实现容易、可操作性高；

(3)有效提高了捷联惯性导航系统的初始对准的快速性，在5s内保证三个失准角同时收敛。

附图说明

图1是惯性器件的编排图；

图2是推导模型时假设的惯性器件的编排图；

图3是本发明的方法流程图；

图4是基于传统对准模型的俯仰角误差曲线图；

图5是基于传统对准模型的横滚角误差曲线图；

图6是基于传统对准模型的航向角误差曲线图；

图7是基于非线性对准模型的俯仰角误差曲线图；

图8是基于非线性对准模型的横滚角误差曲线图；

图9是基于非线性对准模型的航向角误差曲线图。

图中：1-1.第一陀螺，1-2.第二陀螺，1-3.第三陀螺，1-4.第四陀螺，1-5.第五陀螺，1-6.第六陀螺；2-1.第一加速度计，2-2.第二加速度计，2-3.第三加速度计，2-4.第四加速度计，2-5.第五加速度计，2-6.第六加速度计。

具体实施方式：

下面并结合附图，通过具体实施例，对本发明的一种基于非线性初始对准模型的快速对准方法作进一步详细的说明。

本发明所涉及的捷联惯性导航系统中惯性器件的编排如附图1所示，新建立的非线性初始对准模型如式(28)，对准的流程图如附图3所示。本发明主要目的是改进传统的以速度误差为观测量的精对准方法在可观性和观测度方面的不足，同时避免现有文献中的多位置对准方法存在实用性、可操作性方面的问题，针对捷联惯性导航系统提出了一种实用性好、精度高的对准方法，该方法极大提高捷联惯性系统的对准速度。为了达到这个目的，需要完成如下工作：

一种基于非线性初始对准模型的快速对准方法，包括下列步骤：

步骤1、将水平方向上的惯性器件正交冗余配置，同一坐标轴的惯性器件的量测轴相位相差180度，即第一陀螺1-1和第一加速度计2-1的量测轴方向为x轴的正向，第四陀螺1-4和第四加速度2-4的量测轴方向为x轴的反向，第二陀螺1-2和第二加速度计2-2的量测轴方向为y轴的正向，第五陀螺1-5和第五加速度2-5的量测轴方向为y轴的反向，第三陀螺1-3和第三加速度计2-3的量测轴方向为z轴的正向；

步骤2、根据步骤1中的惯性器件的配置，建立水平姿态角和航向角计算模型；

俯仰角： $\mathrm{\θ}=\arcsin \frac{(f_y^2-f_y^5)-({\▿}_y^2-{\▿}_y^5)-(w_y^2-w_y^5)}{2g}$

横滚角： $\mathrm{\γ}=\arcsin \frac{(f_x^1-f_x^4)-({\▿}_x^1-{\▿}_x^4)-(w_x^1-w_x^4)}{-2\cos \mathrm{\θ}\·g}$

航向角： $\mathrm{\ψ}=\arccos \frac{({\mathrm{\ω}}_y^2-{\mathrm{\ω}}_y^5)-({\mathrm{\ϵ}}_y^2-{\mathrm{\ϵ}}_y^5)-(v_y^2-v_y^5)-2\sin \mathrm{\θ}\·{\mathrm{\ω}}_{\mathrm{ie}}\sin L}{2\cos \mathrm{\θ}\·{\mathrm{\ω}}_{\mathrm{ie}}\cos L},$ 或者

$(-2\cos \mathrm{\γ}\sin \mathrm{\ψ}+2\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}){\mathrm{\ω}}_{\mathrm{ie}}\mathrm{conL}-2\sin \mathrm{\γ}\cos \mathrm{\θ}\·{\mathrm{\ω}}_{\mathrm{ie}}\sin L=({\mathrm{\ω}}_x^1-{\mathrm{\ω}}_x^4)-({\mathrm{\ϵ}}_x^1-{\mathrm{\ϵ}}_x^4)-(v_x^1-v_x^4)$

式中：

表示第一加速度计2-1在x轴方向的量测值，表示第四加速度计2-4在x轴方向的量测值，

表示第一加速度计2-1的随机常值，

表示第四加速度计2-4的随机常值，

表示第一加速度计2-1的量测噪声，

表示第四加速度计2-4的量测噪声，

表示第一加速度计2-2在y轴方向的量测值，表示第五加速度计2-5在y轴方向的量测值，

表示第二加速度计2-2的随机常值，

表示第五加速度计2-5的随机常值，

表示第二加速度计2-2的量测噪声，表示第五加速度计2-5的量测噪声，

表示第一陀螺1-1在x轴方向的量测值，

表示第四陀螺1-4在x轴方向的量测值，

表示第一陀螺1-1的随机常值，

表示第四陀螺1-4的随机常值，

表示第一陀螺1-1的量测噪声，

表示第四陀螺1-4的量测噪声，表示第二陀螺1-2在y轴方向的量测值，

表示第五陀螺1-5在y轴方向的量测值，

表示第二陀螺1-2的随机常值，

表示第五陀螺1-5的随机常值，

表示第二陀螺1-2的量测噪声，

表示第五陀螺1-5的量测噪声，θ表示俯仰角，γ表示横滚角，ψ表示航向角，g表示重力矢量，ω_ie表示地球的自转角速度，L为当地的地理纬度；

步骤3、根据步骤2中的3个姿态角的计算模型，可以建立非线性初始对准模型；

$\left[\begin{array}{c}{\mathrm{\θ}}_k\\ {\mathrm{\γ}}_k\\ {\mathrm{\ψ}}_k\end{array}\right]=\left[\begin{array}{c}{\mathrm{\θ}}_{k-1}\\ {\mathrm{\γ}}_{k-1}\\ {\mathrm{\ψ}}_{k-1}\end{array}\right]+\left[\begin{array}{c}{w}_{\mathrm{\θ}}\\ w_{\mathrm{\γ}}\\ w_{\mathrm{\ψ}}\end{array}\right]$

$\left[\begin{array}{c}{f}_{\mathrm{yk}}^2-f_{\mathrm{yk}}^5\\ f_{\mathrm{xk}}^1-f_{\mathrm{xk}}^4\\ {\mathrm{\ω}}_{\mathrm{xk}}^1-{\mathrm{\ω}}_{\mathrm{xk}}^4\end{array}\right]=\left[\begin{array}{c}2\sin {\mathrm{\θ}}_k\·g\\ -2\sin {\mathrm{\γ}}_k\cos {\mathrm{\θ}}_k\·g\\ (-2\cos {\mathrm{\γ}}_k\sin {\mathrm{\ψ}}_k+2\sin {\mathrm{\γ}}_k\sin {\mathrm{\θ}}_k\cos {\mathrm{\ψ}}_k){\mathrm{\ω}}_{\mathrm{ie}}\cos L-2\sin {\mathrm{\γ}}_k\cos {\mathrm{\θ}}_k\·{\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right]-\left[\begin{array}{c}{\▿}_y^2-{\▿}_y^5\\ {\▿}_x^1-{\▿}_x^4\\ {\mathrm{\ϵ}}_x^1-{\mathrm{\ϵ}}_x^4\end{array}\right]-\left[\begin{array}{c}{w}_{\mathrm{yk}}^2-w_{\mathrm{yk}}^5\\ w_{\mathrm{xk}}^1-w_{\mathrm{xk}}^4\\ v_{\mathrm{xk}}^1-v_{\mathrm{xk}}^4\end{array}\right]$

式中：θ_k表示k时刻的俯仰角，γ_k表示k时刻的横滚角，ψ_k表示k时刻的航向角，w_θ表示俯仰角的微小波动值，w_γ表示横滚角的微小波动值，w_ψ表示航向角的微小波动值，

表示k时刻第一加速度计2-1的x轴方向量测值，表示k时刻第四加速度计2-4的x轴方向量测值，

表示k时刻第二加速度计2-2的y轴方向量测值，表示k时刻第五加速度计2-5的y轴方向量测值，

表示k时刻第一陀螺1-1的x轴方向量测值，

表示k时刻第四陀螺1-4的x轴方向量测值；

步骤4、根据步骤3中的初始对准模型构建用于初始对准的扩展kalman滤波器；

步骤5、由导航计算机采集惯性器件输出信息，并完成初始对准滤波。

对本发明的一种基于非线性初始对准模型的快速对准方法的有益效果进行分析如下：

假设捷联惯性导航系统所处位置为东经120°，北纬45°；惯性元件选为中等精度，其中，陀螺常值漂移为0.02(°)/h，陀螺随机漂移为0.01(°)/h，加速度计的常值漂移为100μg，加速度计的随机漂移为50μg，滤波周期为1s。初始姿态角为0°、0°、90°(俯仰、横滚、航向)，初始姿态角误差为10′、10′、1°(俯仰、横滚、航向)。根据传统的初始对准模型和非线性初始对准模型的初始对准仿真如附图4-附图6。

通过计算机仿真表明，基于传统的静基座对准模型，在精对准过程中，3个失准角中的2个水平失准角的估计效果较好，收敛速度很快，但是航向失准角的收敛速度比水平失准角慢得多，大约在300s以上。在估计精度方面，水平误差角稳态估计误差为20″，航向角的稳态估计误差约为7′，姿态误差曲线图如附图4-附图6。

本发明所涉及的非线性模型(式28)中有两个误差项——NC和NR。在扩展Kalman滤波时，将随机误差项NR作为观测噪声来处理。常值误差项NC是一个具有确定取值范围的误差项，若惯性器件的常值漂移均相等，则常值误差项NC为0，若惯性器件的常值漂移的符号相反，则常值误差项NC值最大，其最大值为：

$\max \left(\mathrm{NC}\right)=\left[\begin{array}{c}\left|{\▿}_y^2\right|+\left|{\▿}_y^5\right|\\ \left|{\▿}_x^1\right|+\left|{\▿}_x^4\right|\\ \left|{\mathrm{\ϵ}}_x^1\right|+\left|{\mathrm{\ϵ}}_x^4\right|\end{array}\right]$

当常值误差项NC取其最大值max(NC)时，基于非线性对准模型的姿态角误差图如附图7-附图9。由附图7-附图9可以看出，基于非线性对准模型的精对准过程中，3个失准角收敛速度很快，大约在5s内就完全收敛。在估计精度方面，在惯性器件常值误差NC项取最大值时，水平误差角稳态估计误差约为20″，航向角的稳态估计误差约为6.5′。对照附图4-附图6，可以看出，非线性对准模型的精对准稳定误差与传统的相似。但是非线性对准模型的精对准收敛时间则是传统的静基座对准模型无法比拟的，3个失准角可以在5s内同时收敛。不同对准模型下的估计精度和收敛时间对比如表1。

表1不同对准模型下的估计精度和收敛时间

Claims (1)

1.一种基于非线性初始对准模型的快速对准方法，其特征在于该方法包括下列步骤：

步骤1、将水平方向上的惯性器件正交冗余配置，同一坐标轴的惯性器件的量测轴相位相差180度，即第一陀螺（1-1）和第一加速度计（2-1）的量测轴方向为x轴的正向，第四陀螺（1-4）和第四加速度（2-4）的量测轴方向为x轴的反向，第二陀螺（1-2）和第二加速度计（2-2）的量测轴方向为y轴的正向,第五陀螺（1-5）和第五加速度（2-5）的量测轴方向为y轴的反向，第三陀螺（1-3）和第三加速度计（2-3）的量测轴方向为z轴的正向；

步骤2、根据步骤1中的惯性器件的配置，建立水平姿态角和航向角计算模型；

俯仰角： $\mathrm{\θ}=\arcsin \frac{(f_y^2-f_y^5)-({\▿}_y^2-{\▿}_y^5)-(w_y^2-w_y^5)}{2g},$

横滚角： $\mathrm{\γ}=\arcsin \frac{(f_x^1-f_x^4)-({\▿}_x^1-{\▿}_x^4)-(w_x^1-w_x^4)}{-2\cos \mathrm{\θ}\·g},$

航向角： $\mathrm{\ψ}=\arccos \frac{({\mathrm{\ω}}_y^2-{\mathrm{\ω}}_y^5)-({\mathrm{\ϵ}}_y^2-{\mathrm{\ϵ}}_y^5)-(v_y^2-v_y^5)-2\sin \mathrm{\θ}\·{\mathrm{\ω}}_{\mathrm{ie}}\sin L}{2\cos \mathrm{\θ}\·{\mathrm{\ω}}_{\mathrm{ie}}\cos L}$ 或者

$(-2\cos \mathrm{\γ}\sin \mathrm{\ψ}+2\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}){\mathrm{\ω}}_{\mathrm{ie}}\mathrm{conL}-2\sin \mathrm{\γ}\cos \mathrm{\θ}\·{\mathrm{\ω}}_{\mathrm{ie}}\sin L=({\mathrm{\ω}}_x^1-{\mathrm{\ω}}_x^4)-({\mathrm{\ϵ}}_x^1-{\mathrm{\ϵ}}_x^4)-(v_x^1-v_x^4),$

式中：

表示第一加速度计（2-1）在x轴方向的量测值，

表示第四加速度计（2-4）在x轴方向的量测值，表示第一加速度计（2-1）的随机常值，

表示第四加速度计（2-4）的随机常值，

表示第一加速度计（2-1）的量测噪声，

表示第四加速度计（2-4）的量测噪声，

表示第二加速度计（2-2）在y轴方向的量测值，

表示第五加速度计（2-5）在y轴方向的量测值，

表示第二加速度计（2-2）的随机常值，

表示第五加速度计（2-5）的随机常值，

表示第二加速度计（2-2）的量测噪声，

表示第五加速度计（2-5）的量测噪声，

表示第一陀螺（1-1）在x轴方向的量测值，

表示第四陀螺（1-4）在x轴方向的量测值，

表示第一陀螺（1-1）的随机常值，

表示第四陀螺（1-4）的随机常值，

表示第一陀螺（1-1）的量测噪声，

表示第四陀螺（1-4）的量测噪声，

表示第二陀螺（1-2）在y轴方向的量测值，

表示第五陀螺（1-5）在y轴方向的量测值，

表示第二陀螺（1-2）的随机常值,

表示第五陀螺（1-5）的随机常值，

表示第二陀螺（1-2）的量测噪声，

表示第五陀螺（1-5）的量测噪声，θ表示俯仰角，γ表示横滚角，ψ表示航向角，g表示重力矢量，ω_ie表示地球的自转角速度，L为当地的地理纬度；

步骤3、根据步骤2中的3个姿态角的计算模型，可以建立非线性初始对准模型；

$\left[\begin{array}{c}{\mathrm{\θ}}_k\\ {\mathrm{\γ}}_k\\ {\mathrm{\ψ}}_k\end{array}\right]=\left[\begin{array}{c}{\mathrm{\θ}}_{k-1}\\ {\mathrm{\γ}}_{k-1}\\ {\mathrm{\ψ}}_{k-1}\end{array}\right]+\left[\begin{array}{c}{w}_{\mathrm{\θ}}\\ w_{\mathrm{\γ}}\\ w_{\mathrm{\ψ}}\end{array}\right]$

$\left[\begin{array}{c}{f}_{\mathrm{yk}}^2-f_{\mathrm{yk}}^5\\ f_{\mathrm{xk}}^1-f_{\mathrm{xk}}^4\\ {\mathrm{\ω}}_{\mathrm{xk}}^1-{\mathrm{\ω}}_{\mathrm{xk}}^4\end{array}\right]=\left[\begin{array}{c}2\sin {\mathrm{\θ}}_k\·g\\ -2\sin {\mathrm{\γ}}_k\cos {\mathrm{\θ}}_k\·g\\ (-2\cos {\mathrm{\γ}}_k\sin {\mathrm{\ψ}}_k+2\sin {\mathrm{\γ}}_k\sin {\mathrm{\θ}}_k\cos {\mathrm{\ψ}}_k){\mathrm{\ω}}_{\mathrm{ie}}\cos L-2\sin {\mathrm{\γ}}_k\cos {\mathrm{\θ}}_k\·{\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right]-\left[\begin{array}{c}{\▿}_y^2-{\▿}_y^5\\ {\▿}_x^1-{\▿}_x^4\\ {\mathrm{\ϵ}}_x^1-{\mathrm{\ϵ}}_x^4\end{array}\right]-\left[\begin{array}{c}{w}_{\mathrm{yk}}^2-w_{\mathrm{yk}}^5\\ w_{\mathrm{xk}}^1-w_{\mathrm{xk}}^4\\ v_{\mathrm{xk}}^1-v_{\mathrm{xk}}^4\end{array}\right]$

式中：θ_k表示k时刻的俯仰角，γ_k表示k时刻的横滚角，ψ_k表示k时刻的航向角，w_θ表示俯仰角的微小波动值，w_γ表示横滚角的微小波动值，w_ψ表示航向角的微小波动值，

表示k时刻第一加速度计（2-1）的x轴方向量测值，

表示k时刻第四加速度计（2-4）的x轴方向量测值，表示k时刻第二加速度计（2-2）的y轴方向量测值，表示k时刻第五加速度计（2-5）的y轴方向量测值，

表示k时刻第一陀螺（1-1）的x轴方向量测值，

表示k时刻第四陀螺（1-4）的x轴方向量测值；

步骤4、根据步骤3中的初始对准模型构建用于初始对准的扩展kalman滤波器；

系统的状态方程和量测方程分别为：

X_k+1=X_k+W_k

z_k=h(X)

针对上述的初始对准模型，其中X_k-[θ_k γ_k ψ_k]^T，W_k=[w_θ w_γ w_ψ]^T， $z_k=\left[\begin{array}{c}{f}_{\mathrm{yk}}^2-f_{\mathrm{yk}}^5\\ f_{\mathrm{xk}}^1-f_{\mathrm{xk}}^4\\ {\mathrm{\ω}}_{\mathrm{xk}}^1-{\mathrm{\ω}}_{\mathrm{xk}}^4\end{array}\right],$ $h\left(X\right)=\left[\begin{array}{c}2\sin {\mathrm{\θ}}_k\·g\\ -2\sin {\mathrm{\γ}}_k\cos {\mathrm{\θ}}_k\·g\\ (-2\cos {\mathrm{\γ}}_k\sin {\mathrm{\ψ}}_k+2\sin {\mathrm{\γ}}_k\sin {\mathrm{\θ}}_k\cos {\mathrm{\ψ}}_k){\mathrm{\ω}}_{\mathrm{ie}}\cos L-2\sin {\mathrm{\γ}}_k\cos {\mathrm{\θ}}_k\·{\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right];$ 同时将 $\left[\begin{array}{c}{\▿}_y^2-{\▿}_y^5\\ {\▿}_x^1-{\▿}_x^4\\ {\mathrm{\ϵ}}_x^1-{\mathrm{\ϵ}}_x^4\end{array}\right]$ 作为恒值误差项处理，将 $\left[\begin{array}{c}{w}_{\mathrm{yk}}^2-w_{\mathrm{yk}}^5\\ w_{\mathrm{xk}}^1-w_{\mathrm{xk}}^4\\ v_{\mathrm{xk}}^1-v_{\mathrm{xk}}^4\end{array}\right]$ 作为观测噪声；

Kalman滤波算法如下：

P_k+1/k=P_k

$K_{k+1}=P_{k+1/k}{H}_{k+1}^T{(H_{k+1}{P}_{k+1/k}{H}_{k+1}^T+R_{k+1})}^{-1}$

P_k+1=(I-K_k+1H_k+1)P_k+1/k

${\hat{X}}_{k+1}={\hat{X}}_k+K_{k+1}(z_{k+1}-h\left({\hat{X}}_k\right))$

$H_{k+1}=\frac{\∂}{\∂d}h\left(X\right){|}_{X={\hat{X}}_k}$

其中K_k是观测增益，

是状态X_k的估计值，z_k是观测值，P_k是状态协方差矩阵，R_k是观测噪声协方差矩阵，h(·)是传感器量测方程；

步骤5、由导航计算机采集惯性器件输出信息，并完成初始对准滤波。

Images