CN101813493A - 一种基于粒子滤波的惯性导航系统初始对准方法 - Google Patents

Abstract

本发明公布了一种基于粒子滤波的惯性导航系统初始对准方法，包括下列步骤：建立基于二位置的水平姿态角计算模型；建立基于二位置的航向角计算模型；建立基于二位置的非线性初始对准模型；构建用于初始对准的高斯粒子滤波器；由导航计算机，采集惯性器件输出信息，并完成初始对准滤波。本发明利用二位置方法消除惯性器件的常值误差，因此无需对惯性器件误差进行状态扩展，从而在保证对准精度的前提下降低了初始对准模型的维数；由于二位置非线性初始对准模型的维数仅为三维，适合粒子滤波算法在初始对准中的工程应用，较好的满足了粒子滤波在惯性导航系统初始对准中的实时性要求；提高了惯性导航系统初始对准的精度和速度。

Description

一种基于粒子滤波的惯性导航系统初始对准方法

技术领域

本发明属于惯性导航技术领域，是一种用于惯性导航系统的初始对准方法，适用于惯性导航系统工作前的静基座自主初始对准。

背景技术

惯性导航系统初始对准的目的是在惯性导航系统进入导航工作状态之前，建立起惯性导航系统载体坐标系相对于某个参考坐标系的姿态关系的过程。初始对准误差是惯性导航系统的主要误差源之一，直接影响着惯性导航系统的工作精度，同时初始对准时间是反映武器系统快速反应能力的重要战术指标。较高的对准精度往往需要较长的对准时间，解决对准精度和对准速度的矛盾是惯性导航系统初始对准研究与发展的重要方向。

惯性导航系统自主对准的困难是系统不完全可观，初始对准的结果很大程度上依赖于系统状态的可观性。当可观测矩阵的秩相同时，卡尔曼滤波的效果并不相同，原因在于系统状态变量的可观测度不同。为了提高系统状态的可观测度，通过加入基座运动的方法进行初始对准能收到较好的效果，如二位置对准。二位置方法能够有效提高系统状态的可观测度，提高惯性导航系统的初始对准精度、缩短初始对准时间。

粒子滤波是一种针对非线性非高斯系统的状态估计方法，具有优异的滤波性能。作为解决非线性、非高斯动态系统最优估计的最有效方法之一，粒子滤波已经成为研究的热点。粒子滤波的缺陷在于其计算量对系统维数极其敏感，粒子数量随维数增加呈指数规律增加。常规的初始对准模型采用惯性导航系统的误差方程，系统维数高，不适合粒子滤波的工程应用。

本发明针对常规初始对准模型维数高的特点，基于二位置对准方法提出了一种适用于粒子滤波的二位置非线性初始对准模型，该模型利用二位置方法消除惯性器件的常值误差，因此无需对惯性器件误差进行状态扩展，从而在保证对准精度的前提下降低了初始对准模型的维数。将粒子滤波应用于二位置非线性初始对准模型，能够提高初始对准的精度和速度，满足粒子滤波在工程实际中的应用。

发明内容

技术问题：本发明的目的是提供一种惯性导航系统初始对准的方法，该方法可以实现惯性导航系统的高精度快速静基座自主初始对准。

技术方案：本发明为实现上述目的，采用如下技术方案：

本发明是一种基于粒子滤波的惯性导航系统初始对准方法，其特征在于包括下列步骤：

1)建立基于二位置的水平姿态角计算模型；

2)建立基于二位置的航向角计算模型；

3)建立基于二位置的非线性初始对准模型；

4)构建用于初始对准的高斯粒子滤波器；

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

优选地，所述水平姿态角计算模型的建立方法如下：

开始静基座初始对准时，惯性导航系统具有一定的姿态角：俯仰角为θ，横滚角为γ，航向角为ψ，则加速度计输出为：

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

式中，f为机体系下的加速度计输出，g为当地重力加速度，Δf代表加速度计误差，

为机体系下的加速度计随机常值，w为机体系下的加速度计随机漂移，考虑为白噪声，下标x，y，z为机体轴向；C_n ^b表示了从导航坐标系n系到机体坐标系b系的转换关系矩阵，满足：

$C_n^b=\left[\begin{array}{ccc}\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}+\cos \mathrm{\ψ}\cos \mathrm{\γ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\cos \mathrm{\γ}& -\cos \mathrm{\θ}\sin \mathrm{\γ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}& \sin \mathrm{\θ}\\ \cos \mathrm{\ψ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}& -\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]---\left(2\right)$

将(2)式代人(1)式，并移项可以得到：

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

式中，上标1表示在惯性导航系统在静基座初始时，即位置1处的值；

将惯性导航系统绕基座的天向轴旋转180度，则有：

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

式中，上标2表示将惯性导航系统绕基座的天向轴旋转180度后，即位置2处的值；

将(3)式减去(4)式可得：

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

由(5)式可以得到：

$\left\{\begin{array}{c}\mathrm{\θ}=\arcsin \frac{(f_y^1-f_y^2)+(w_y^1-w_y^2)}{2g}\\ \mathrm{\γ}=\arcsin \frac{(f_x^1-f_x^2)+(w_x^1-w_x^2)}{-2\cos \mathrm{\θ}\·g}\end{array}\right..---\left(6\right)$

优选地，所述航向角计算模型的建立方法如下：

有机体系下的陀螺输出：

$\left[\begin{array}{c}{\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right]=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{\Δ}{\mathrm{\ω}}_x\\ \mathrm{\Δ}{\mathrm{\ω}}_y\\ \mathrm{\Δ}{\mathrm{\ω}}_z\end{array}\right]=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\\ {\mathrm{\ϵ}}_y\\ {\mathrm{\ϵ}}_z\end{array}\right]+\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right]---\left(7\right)$

式中，ω为机体系下的陀螺输出，ω_ie为地球自转角速率，L为当地纬度，Δω为陀螺误差，ε为机体系下陀螺的随机常值，v为机体系下陀螺的随机漂移为白噪声，下标x，y，z为机体轴向；C_n ^b表示了从导航坐标系n系到机体坐标系b系的转换关系矩阵：

$C_n^b=\left[\begin{array}{ccc}\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}+\cos \mathrm{\ψ}\cos \mathrm{\γ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\cos \mathrm{\γ}& -\cos \mathrm{\θ}\sin \mathrm{\γ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}& \sin \mathrm{\θ}\\ \cos \mathrm{\ψ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}& -\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right];$

将C_n ^b代人(7)式，并移项可以得到：

$\left[\begin{array}{c}(-\cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}){\mathrm{\ω}}_{\mathrm{ie}}\cos L-\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{\γ}\cos \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\\ {\mathrm{\ϵ}}_y\\ {\mathrm{\ϵ}}_z\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_x^1\\ {\mathrm{\ω}}_y^1\\ {\mathrm{\ω}}_z^1\end{array}\right]+\left[\begin{array}{c}{v}_x^1\\ v_y^1\\ v_z^1\end{array}\right]---\left(8\right)$

式中，上标1表示在惯性导航系统在静基座初始时，即位置1处的值；

将惯性导航系统绕基座的天向轴旋转180度，则有：

$\left[\begin{array}{c}(\cos \mathrm{\γ}\sin \mathrm{\ψ}-\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}){\mathrm{\ω}}_{\mathrm{ie}}\cos L+\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{\γ}\cos \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\\ {\mathrm{\ϵ}}_y\\ {\mathrm{\ϵ}}_z\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_x^2\\ {\mathrm{\ω}}_y^2\\ {\mathrm{\ω}}_z^2\end{array}\right]+\left[\begin{array}{c}{v}_x^2\\ v_y^2\\ v_z^2\end{array}\right]---\left(9\right)$

式中，上标2表示将惯性导航系统绕基座的天向轴旋转180度后，即位置2处的值；

将(8)式减去(9)式可得：

$\left[\begin{array}{c}(-2\cos \mathrm{\γ}\sin \mathrm{\ψ}+2\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}){\mathrm{\ω}}_{\mathrm{ie}}\cos L-\\ 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\\ 0\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_x^1-{\mathrm{\ω}}_x^2\\ {\mathrm{\ω}}_y^1-{\mathrm{\ω}}_y^2\\ {\mathrm{\ω}}_z^1-{\mathrm{\ω}}_z^2\end{array}\right]+\left[\begin{array}{c}{v}_x^1-v_x^2\\ v_y^1-v_y^2\\ v_z^1-v_z^2\end{array}\right]---\left(10\right)$

由(10)式可以得到：

$\mathrm{\ψ=arccos}\frac{({\mathrm{\ω}}_y^1-{\mathrm{\ω}}_y^2)+(v_y^1-v_y^2)-2\sin \mathrm{\θ}\·{\mathrm{\ω}}_{\mathrm{ie}}\sin L}{2\cos \mathrm{\θ}\·{\mathrm{\ω}}_{\mathrm{ie}}\cos L}.---\left(11\right)$

优选地，所述的建立基于二位置的非线性初始对准模型的方法如下：

初始对准状态方程如下：

$\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(12\right)$

式中，θ_k为k时刻即第k次滤波的俯仰角，γ_k为k时刻横滚角，ψ_k为k时刻航向角，θ_k-1为k-1时刻俯仰角，γ_k-1为k-1时刻横滚角，ψ_k-1为k-1时刻航向角，w_θ，w_γ，w_ψ为三个姿态角的干扰为白噪声；

取(5)式前两行和(10)式第一行，并移项整理后有：

$\left[\begin{array}{c}{f}_{\mathrm{xk}}^1-f_{\mathrm{xk}}^2\\ f_{\mathrm{yk}}^1-f_{\mathrm{yk}}^2\\ {\mathrm{\ω}}_{\mathrm{xk}}^1-{\mathrm{\ω}}_{\mathrm{xk}}^2\end{array}\right]=\left[\begin{array}{c}-2\sin {\mathrm{\γ}}_k\cos {\mathrm{\θ}}_k\·g\\ 2\sin {\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}{w}_{\mathrm{xk}}^1-w_{\mathrm{xk}}^2\\ w_{\mathrm{yk}}^1-w_{\mathrm{yk}}^2\\ v_{\mathrm{xk}}^1-v_{\mathrm{xk}}^2\end{array}\right]---\left(13\right)$

其中所述(5)式和(10)式如下：

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

$\left[\begin{array}{c}(-2\cos \mathrm{\γ}\sin \mathrm{\ψ}+2\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}){\mathrm{\ω}}_{\mathrm{ie}}\cos L-\\ 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\\ 0\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_x^1-{\mathrm{\ω}}_x^2\\ {\mathrm{\ω}}_y^1-{\mathrm{\ω}}_y^2\\ {\mathrm{\ω}}_z^1-{\mathrm{\ω}}_z^2\end{array}\right]+\left[\begin{array}{c}{v}_x^1-v_x^2\\ v_y^1-v_y^2\\ v_z^1-v_z^2\end{array}\right]---\left(10\right)$

式中，f_xk ¹-f_xk ²为x轴向加速度计k时刻的二位置量测值，f_yk ¹-f_yk ²为y轴向加速度计k时刻的二位置量测值，ω_xk ¹-ω_xk ²为x轴向陀螺k时刻的二位置量测值，w_xk ¹-w_xk ²为x轴向加速度计k时刻的二位置量测噪声，w_yk ¹-w_yk ²为y轴向加速度计k时刻的二位置量测噪声，v_xk ¹-v_xk ²为x轴向陀螺k时刻的二位置量测噪声，量测噪声都为白噪声，上标1表示在惯性导航系统在静基座初始时，即位置1处的值，上标2表示将惯性导航系统绕基座的天向轴旋转180度后，即位置2处的值；k时刻即当前时刻，k-1时刻即前一时刻。

优选地，所述的初始对准的高斯粒子滤波器的构建包括如下步骤：

Step1：初始化，在p(x₀)抽取N个样本点x₀ ⁱ，相应权值1/N；

Step2：抽样 $x_k^i\·q\left(x_k\right|x_{k-1},z_k)=p\left(x_k\right|z_{1:k-1});$

Step3：计算权值并归一化， ${\hat{w}}_k^i\∝p\left(z_k\right|x_k^i),$ $w_k^i={\hat{w}}_k^i/\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\hat{w}}_k^i;$

Step4：计算估计值， ${\stackrel{\‾}{x}}_k\≈\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_k^i{x}_k^i;$

Step5：对离散的滤波密度进行高斯近似；

Step6：从近似的高斯分布中抽取新的粒子x_k ^j，相应权值1/N；

Step7：将新的粒子x_k ^j进行下一周期的滤波；

其中，p(·)为概率密度函数，q(·)为重要密度函数，N为大于1的自然数，x₀为滤波初值，x₀ ⁱ为滤波初始粒子，x_k ⁱ为k时刻粒子，x_k为k时刻状态，x_k-1为k-1时刻状态，z_k为k时刻二位置观测量，z_1:k-1为1时刻到k-1时刻的二位置观测量，

为k时刻未归一化的粒子权值，w_k ⁱ为k时刻归一化后粒子的权值，

为k时刻的滤波估值，x_k ^j为近似高斯分布中抽取的新粒子，i，j＝1，2，3......N。

优选地，步骤5)所述的初始对准完成后，在位置1处采集P次惯性器件输出数据，惯性导航系统在静基座初始时，即位置1处；然后，将惯性导航系统绕基座的天向轴旋转180度即位置2处，基座稳定后再采集P次惯性器件输出数据；然后将两次采集的数据依次构造二位置量测量，并对采集数据进行滤波处理，其中P为大于1的自然数。

有益效果：

本发明的方法具有如下优点：利用二位置方法消除惯性器件的常值误差，因此无需对惯性器件误差进行状态扩展，从而在保证对准精度的前提下降低了初始对准模型的维数；由于二位置非线性初始对准模型的维数仅为三维，适合粒子滤波算法在初始对准中的工程应用，较好的满足了粒子滤波在惯性导航系统初始对准中的实时性要求；提高了惯性导航系统初始对准的精度和速度。

附图说明

图1是用于初始对准的高斯粒子滤波流程。

图2是俯仰角误差曲线。

图3是横滚角误差曲线。

图4是航向角误差曲线。

具体实施方式

下面结合附图对发明的技术方案进行详细说明：

如图1所示，是用于初始对准的高斯粒子滤波流程。

首先根据惯性导航系统的工作原理建立基于二位置的水平姿态角计算模型；然后建立基于二位置的航向角计算模型；在此基础上建立基于二位置的非线性初始对准模型；根据所确定的非线性初始对准模型设计高斯粒子滤波器；最后由导航计算机采集惯性器件输出信息，并完成初始对准的滤波解算。

1)建立基于二位置的水平姿态角计算模型

初始对准中假定惯性导航系统经过标定，常值漂移已经进行了补偿，并考虑初始对准时间较短，将加速度计误差模型近似为随机常值加随机漂移，陀螺误差模型近似为随机常值加随机漂移。开始静基座初始对准时，惯性导航系统具有一定的姿态角，其俯仰角为θ，横滚角为γ，航向角为ψ，则加速度计输出为

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

式中，f为机体系下的加速度计输出，g为当地重力加速度，Δf代表加速度计误差，

为机体系下的加速度计随机常值，w为机体系下的加速度计随机漂移，考虑为白噪声，下标x，y，z为机体轴向。C_n ^b表示了从导航坐标系n系到机体坐标系b系的转换关系矩阵，满足：

$C_n^b=\left[\begin{array}{ccc}\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}+\cos \mathrm{\ψ}\cos \mathrm{\γ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\cos \mathrm{\γ}& -\cos \mathrm{\θ}\sin \mathrm{\γ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}& \sin \mathrm{\θ}\\ \cos \mathrm{\ψ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}& -\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]---\left(2\right)$

将(2)式代人(1)式，并移项可以得到

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

式中，上标1表示在位置1处的值。

将惯性导航系统绕基座的天向轴旋转180度，则有

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

式中，上标2表示将惯性导航系统绕基座的天向轴旋转180度后，即位置2处的值。

将(3)式减去(4)式可得：

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

由(5)式可以得到：

$\left\{\begin{array}{c}\mathrm{\θ}=\arcsin \frac{(f_y^1-f_y^2)+(w_y^1-w_y^2)}{2g}\\ \mathrm{\γ}=\arcsin \frac{(f_x^1-f_x^2)+(w_x^1-w_x^2)}{-2\cos \mathrm{\θ}\·g}\end{array}\right.---\left(6\right)$

2)建立基于二位置的航向角计算模型

与水平姿态角求解方法类似，有：

$\left[\begin{array}{c}{\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right]=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{\Δ}{\mathrm{\ω}}_x\\ \mathrm{\Δ}{\mathrm{\ω}}_y\\ \mathrm{\Δ}{\mathrm{\ω}}_z\end{array}\right]=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\\ {\mathrm{\ϵ}}_y\\ {\mathrm{\ϵ}}_z\end{array}\right]+\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right]---\left(7\right)$

式中，ω为机体系下的陀螺输出，ω_ie为地球自转角速率，L为当地纬度，Δω为陀螺误差，ε为机体系下陀螺的随机常值，v为机体系下陀螺的随机漂移，考虑为白噪声，下标x，y，z为机体轴向。C_n ^b表示了从导航坐标系n系到机体坐标系b系的转换关系矩阵。

将(2)式代人(7)式，并移项可以得到

$\left[\begin{array}{c}(-\cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}){\mathrm{\ω}}_{\mathrm{ie}}\cos L-\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{\γ}\cos \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\\ {\mathrm{\ϵ}}_y\\ {\mathrm{\ϵ}}_z\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_x^1\\ {\mathrm{\ω}}_y^1\\ {\mathrm{\ω}}_z^1\end{array}\right]+\left[\begin{array}{c}{v}_x^1\\ v_y^1\\ v_z^1\end{array}\right]---\left(8\right)$

式中，上标1表示在位置1处的值。

将惯性导航系统绕基座的天向轴旋转180度，则有

$\left[\begin{array}{c}(\cos \mathrm{\γ}\sin \mathrm{\ψ}-\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}){\mathrm{\ω}}_{\mathrm{ie}}\cos L+\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{\γ}\cos \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\\ {\mathrm{\ϵ}}_y\\ {\mathrm{\ϵ}}_z\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_x^2\\ {\mathrm{\ω}}_y^2\\ {\mathrm{\ω}}_z^2\end{array}\right]+\left[\begin{array}{c}{v}_x^2\\ v_y^2\\ v_z^2\end{array}\right]---\left(9\right)$

式中，上标2表示将惯性导航系统绕基座的天向轴旋转180度后，即位置2处的值。

将(8)式减去(9)式可得：

$\left[\begin{array}{c}(-2\cos \mathrm{\γ}\sin \mathrm{\ψ}+2\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}){\mathrm{\ω}}_{\mathrm{ie}}\cos L-\\ 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\\ 0\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_x^1-{\mathrm{\ω}}_x^2\\ {\mathrm{\ω}}_y^1-{\mathrm{\ω}}_y^2\\ {\mathrm{\ω}}_z^1-{\mathrm{\ω}}_z^2\end{array}\right]+\left[\begin{array}{c}{v}_x^1-v_x^2\\ v_y^1-v_y^2\\ v_z^1-v_z^2\end{array}\right]---\left(10\right)$

由(10)式可以得到：

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

3)建立基于二位置的非线性初始对准模型

常规初始对准模型中系统状态维数为10维(采用加性四元数时为11维)，包含3个平台误差角(加性四元数误差模型中为四个加性四元数)、2个水平速度和5个惯性器件误差状态。对粒子滤波而言，采用如此高维的误差模型，不能满足初始对准的实时性要求，因此必须在满足初始对准精度的前提下，对系统进行降维，从而降低粒子滤波应用于惯性导航系统初始对准的计算量。

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

$\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(12\right)$

式中，θ_k为k时刻即第k次滤波的俯仰角，γ_k为k时刻横滚角，ψ_k为k时刻航向角，θ_k-1为k-1时刻俯仰角，γ_k-1为k-1时刻横滚角，ψ_k-1为k-1时刻航向角，w_θ，w_γ，w_ψ为三个姿态角的干扰，考虑为白噪声。

取(5)式前两行和(10)式第一行，并移项整理后有：

$\left[\begin{array}{c}{f}_{\mathrm{xk}}^1-f_{\mathrm{xk}}^2\\ f_{\mathrm{yk}}^1-f_{\mathrm{yk}}^2\\ {\mathrm{\ω}}_{\mathrm{xk}}^1-{\mathrm{\ω}}_{\mathrm{xk}}^2\end{array}\right]=\left[\begin{array}{c}-2\sin {\mathrm{\γ}}_k\cos {\mathrm{\θ}}_k\·g\\ 2\sin {\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}{w}_{\mathrm{xk}}^1-w_{\mathrm{xk}}^2\\ w_{\mathrm{yk}}^1-w_{\mathrm{yk}}^2\\ v_{\mathrm{xk}}^1-v_{\mathrm{xk}}^2\end{array}\right]---\left(13\right)$

式中，f_xk ¹-f_xk ²为x轴向加速度计k时刻的二位置量测值，f_yk ¹-f_yk ²为y轴向加速度计k时刻的二位置量测值，ω_xk ¹-ω_xk ²为x轴向陀螺k时刻的二位置量测值，w_xk ¹-w_xk ²为x轴向加速度计k时刻的二位置量测噪声，w_yk ¹-w_yk ²为y轴向加速度计k时刻的二位置量测噪声，v_xk ¹-v_xk ²为x轴向陀螺k时刻的二位置量测噪声，量测噪声都考虑为白噪声。

式(12)和(13)构成了初始对准的状态方程和二位置非线性量测方程，状态方程和量测方程维数都为3，远远低于常规二位置方法状态方程的维数，有利于减少粒子滤波的计算量；其次，该初始对准模型未进行小角度线性化假设，模型准确性要高于常规初始对准方法，其对准精度高于常规初始对准方法；最后，由于三个姿态角都是直接可观测的，因此其对准速度要高于常规初始对准方法。

由于粒子滤波初始赋值偏差过大时(大失准角情况)，需要增加大量的粒子来保证滤波的收敛，因此可采用式(6)和(11)在精对准前进行粗对准，式中的白噪声分量可以采用多次测量取平均的方式来进行消除。

4)构建用于初始对准的高斯粒子滤波器

粒子滤波采用蒙特卡洛积分方法实现最优贝叶斯估计。最为典型的粒子滤波算法是重要性采样重采样SIR算法，该算法重要性函数计算简单，容易采样，在各个领域得到了广泛的应用。

SIR算法存在一些问题，如：经过重采样步骤之后，各粒子在统计意义上将不再独立，不能满足样本独立同分布的要求，从而使得Monte Carl积分的收敛性不能保证；经过重采样过程之后，权值较大的粒子将会被多次复制，使得重采样后的粒子集中包含了许多重复粒子，从而失去了粒子的多样性；重采样过程会损失信息，对于任何基于Monte Carl方法的估计问题，经过重采样之后其估计精度会有所下降。为了解决这些问题，目前提出了各种粒子滤波改进算法，如高斯粒子滤波。

本发明中初始对准的滤波算法采用高斯粒子滤波实现。下面给出用于初始对准的高斯粒子滤波算法基本步骤：

Step1：初始化，在p(x₀)抽取N个样本点x₀ ⁱ，相应权值1/N；

Step2：抽样 $x_k^i\·q\left(x_k\right|x_{k-1},z_k)=p\left(x_k\right|z_{1:k-1});$

Step3：计算权值并归一化， ${\hat{w}}_k^i\∝p\left(z_k\right|x_k^i),$ $w_k^i={\hat{w}}_k^i/\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\hat{w}}_k^i;$

Step4：计算估计值， ${\stackrel{\‾}{x}}_k\≈\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_k^i{x}_k^i;$

Step5：对离散的滤波密度进行高斯近似；

Step6：从近似的高斯分布中抽取新的粒子x_k ^j，相应权值1/N；

Step7：将新的粒子x_k ^j进行下一周期的滤波；

其中，p(·)为概率密度函数，q(·)为重要密度函数，N为大于1的自然数，x₀为滤波初值，x₀ ⁱ为滤波初始粒子，x_k ⁱ为k时刻粒子，x_k为k时刻状态，x_k-1为k-1时刻状态，z_k为k时刻二位置观测量，z_1:k-1为1时刻到k-1时刻的二位置观测量，

为k时刻未归一化的粒子权值，w_k ⁱ为k时刻归一化后粒子的权值，

为k时刻的滤波估值，x_k ^j为近似高斯分布中抽取的新粒子，i，j＝1，2，3......N。

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

导航计算机根据惯性导航系统初始对准的状态方程和观测方程，采集惯性器件输出信息，并完成初始对准的高斯粒子滤波解算。

采用非线性初始对准模型时各姿态角的收敛时间除了与二位置量测次数有关，而且与二位置量测的周期也有关，尤其是航向角的收敛速度还是要略低于水平姿态角的收敛速度。每次二位置量测时需要将惯性导航系统绕基座的天向轴旋转180度，由于基座转动、停止等运动状态需要较长时间，因此影响了惯性导航系统初始对准的时间。

考虑到初始对准时间较短，对于中高精度的陀螺来说，在初始对准这段时间陀螺的零偏稳定性对精度的影响较小，为缩短惯性导航系统初始对准的时间，此处二位置量测时仅将惯性导航系统绕基座的天向轴旋转1次，将数据采集和滤波分成两步进行，具体方法如下：完成粗对准后，在位置1处采集多次惯性器件输出数据，仿真中设置为400次，实际中可根据需要设置；然后，将惯性导航系统绕基座的天向轴旋转180度到位置2处，基座稳定后再采集400次惯性器件输出数据；然后将两次采集的数据依次构造二位置量测量，并对采集数据进行滤波处理。当陀螺精度较差时，可减少采集数据的次数，增加基座旋转的次数。

综上所述，对该方法进行效果分析。从图2和图3中可以看出采用二位置非线性初始对准模型的初始对准方法水平姿态角能很快收敛，经过几次二位置量测之后其水平姿态误差便收敛至6个角秒以内。图4显示，航向角的收敛速度略低于水平姿态角，但也能够迅速收敛至10个角分以内。由于在二位置非线性初始对准模型中，加速度计随机常值和陀螺仪随机常值通过二位置方法进行了消除，不需要将他们扩展入系统状态方程，因此有效地降低了系统的维数，有利于粒子滤波在初始对准中的应用。

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

Claims (6)

1.一种基于粒子滤波的惯性导航系统初始对准方法，其特征在于包括下列步骤：

1)建立基于二位置的水平姿态角计算模型；

2)建立基于二位置的航向角计算模型；

3)建立基于二位置的非线性初始对准模型；

4)构建用于初始对准的高斯粒子滤波器；

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

2.根据权利要求1所述的一种基于粒子滤波的惯性导航系统初始对准方法，其特征在于步骤1)所述水平姿态角计算模型的建立方法如下：

开始静基座初始对准时，惯性导航系统具有一定的姿态角：俯仰角为θ，横滚角为γ，航向角为ψ，则加速度计输出为：

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

式中，f为机体系下的加速度计输出，g为当地重力加速度，Δf代表加速度计误差，

为机体系下的加速度计随机常值，w为机体系下的加速度计随机漂移，考虑为白噪声，下标x，y，z为机体轴向；C_n ^b表示了从导航坐标系n系到机体坐标系b系的转换关系矩阵，满足：

$C_n^b=\left[\begin{array}{ccc}\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}+\cos \mathrm{\ψ}\cos \mathrm{\γ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\cos \mathrm{\γ}& -\cos \mathrm{\θ}\sin \mathrm{\γ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}& \sin \mathrm{\θ}\\ \cos \mathrm{\ψ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}& -\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]---\left(2\right)$

将(2)式代人(1)式，并移项可以得到：

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

式中，上标1表示在惯性导航系统在静基座初始时，即位置1处的值；

将惯性导航系统绕基座的天向轴旋转180度，则有：

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

式中，上标2表示将惯性导航系统绕基座的天向轴旋转180度后，即位置2处的值；

将(3)式减去(4)式可得：

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

由(5)式可以得到：

$\left\{\begin{array}{c}\mathrm{\θ}=\mathrm{arcsn}\frac{(f_y^1-f_y^2)+(w_y^1-w_y^2)}{2g}\\ \mathrm{\γ}=\arcsin \frac{(f_x^1-f_x^2)+(w_x^1-w_x^2)}{2g}\end{array}\right..---\left(6\right)$

3.根据权利要求1所述的一种基于粒子滤波的惯性导航系统初始对准方法，其特征在于步骤2)所述航向角计算模型的建立方法如下：

有机体系下的陀螺输出：

$\left[\begin{array}{c}{\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right]=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{\Δ}{\mathrm{\ω}}_x\\ \mathrm{\Δ}{\mathrm{\ω}}_y\\ \mathrm{\Δ}{\mathrm{\ω}}_z\end{array}\right]=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\\ {\mathrm{\ϵ}}_y\\ {\mathrm{\ϵ}}_z\end{array}\right]+\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right]---\left(7\right)$

式中，ω为机体系下的陀螺输出，ω_ie为地球自转角速率，L为当地纬度，Δω为陀螺误差，ε为机体系下陀螺的随机常值，v为机体系下陀螺的随机漂移为白噪声，下标x，y，z为机体轴向；C_n ^b表示了从导航坐标系n系到机体坐标系b系的转换关系矩阵：

$C_n^b=\left[\begin{array}{ccc}\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}+\cos \mathrm{\ψ}\cos \mathrm{\γ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\cos \mathrm{\γ}& -\cos \mathrm{\θ}\sin \mathrm{\γ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}& \sin \mathrm{\θ}\\ \cos \mathrm{\ψ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}& -\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right];$

将C_n ^b代人(7)式，并移项可以得到：

$\left[\begin{array}{c}(-\cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}){\mathrm{\ω}}_{\mathrm{ie}}\cos L-\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{\γ}\cos \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\\ {\mathrm{\ϵ}}_y\\ {\mathrm{\ϵ}}_z\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_x^1\\ {\mathrm{\ω}}_y^1\\ {\mathrm{\ω}}_z^1\end{array}\right]+\left[\begin{array}{c}{v}_x^1\\ v_y^1\\ v_z^1\end{array}\right]---\left(8\right)$

式中，上标1表示在惯性导航系统在静基座初始时，即位置1处的值；

将惯性导航系统绕基座的天向轴旋转180度，则有：

$\left[\begin{array}{c}(\cos \mathrm{\γ}\sin \mathrm{\ψ}-\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}){\mathrm{\ω}}_{\mathrm{ie}}\cos L+\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{\γ}\cos \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\\ {\mathrm{\ϵ}}_y\\ {\mathrm{\ϵ}}_z\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_x^2\\ {\mathrm{\ω}}_y^2\\ {\mathrm{\ω}}_z^2\end{array}\right]+\left[\begin{array}{c}{v}_x^2\\ v_y^2\\ v_z^2\end{array}\right]---\left(9\right)$

式中，上标2表示将惯性导航系统绕基座的天向轴旋转180度后，即位置2处的值；

将(8)式减去(9)式可得：

$\left[\begin{array}{c}(-2\cos \mathrm{\γ}\sin \mathrm{\ψ}+2\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}){\mathrm{\ω}}_{\mathrm{ie}}\cos L-\\ 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\\ 0\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_x^1-{\mathrm{\ω}}_x^2\\ {\mathrm{\ω}}_y^1-{\mathrm{\ω}}_y^2\\ {\mathrm{\ω}}_z^1-{\mathrm{\ω}}_z^2\end{array}\right]+\left[\begin{array}{c}{v}_x^1-v_x^2\\ v_y^1-v_y^2\\ v_z^1-v_z^2\end{array}\right]---\left(10\right)$

由(10)式可以得到：

$\mathrm{\ψ}=\arccos \frac{({\mathrm{\ω}}_y^1-{\mathrm{\ω}}_y^2)+(v_y^1-v_y^2)-2\sin \mathrm{\θ}\·{\mathrm{\ω}}_{\mathrm{ie}}\sin L}{2\cos \mathrm{\θ}\·{\mathrm{\ω}}_{\mathrm{ie}}\cos L}.---\left(11\right)$

4.根据权利要求1所述的一种基于粒子滤波的惯性导航系统初始对准方法，其特征在于步骤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(12\right)$

式中，θ_k为k时刻即第k次滤波的俯仰角，γ_k为k时刻横滚角，ψ_k为k时刻航向角，θ_k-1为k-1时刻俯仰角，γ_k-1为k-1时刻横滚角，ψ_k-1为k-1时刻航向角，w_θ，w_γ，w_ψ为三个姿态角的干扰为白噪声；

取(5)式前两行和(10)式第一行，并移项整理后有：

$\left[\begin{array}{c}{f}_{\mathrm{xk}}^1-f_{\mathrm{xk}}^2\\ f_{\mathrm{yk}}^1-f_{\mathrm{yk}}^2\\ {\mathrm{\ω}}_{\mathrm{xk}}^1-{\mathrm{\ω}}_{\mathrm{xk}}^2\end{array}\right]=\left[\begin{array}{c}-2{\sin \mathrm{\γ}}_k\cos {\mathrm{\θ}}_k\·g\\ 2{\sin \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}{w}_{\mathrm{xk}}^1-w_{\mathrm{xk}}^2\\ w_{\mathrm{yk}}^1-w_{\mathrm{yk}}^2\\ v_{\mathrm{xk}}^1-v_{\mathrm{xk}}^2\end{array}\right]---\left(13\right)$

其中所述(5)式和(10)式如下：

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

$\left[\begin{array}{c}(-2\cos \mathrm{\γ}\sin \mathrm{\ψ}+2\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}){\mathrm{\ω}}_{\mathrm{ie}}\cos L-\\ 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\\ 0\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_x^1-{\mathrm{\ω}}_x^2\\ {\mathrm{\ω}}_y^1-{\mathrm{\ω}}_y^2\\ {\mathrm{\ω}}_z^1-{\mathrm{\ω}}_z^2\end{array}\right]+\left[\begin{array}{c}{v}_x^1-v_x^2\\ v_y^1-v_y^2\\ v_z^1-v_z^2\end{array}\right]---\left(10\right)$

式中，f_xk ¹-f_xk ²为x轴向加速度计k时刻的二位置量测值，f_yk ¹-f_yk ²为y轴向加速度计k时刻的二位置量测值，ω_xk ¹-ω_xk ²为x轴向陀螺k时刻的二位置量测值，w_xk ¹-w_xk ²为x轴向加速度计k时刻的二位置量测噪声，w_yk ¹-w_yk ²为y轴向加速度计k时刻的二位置量测噪声，v_xk ¹-v_xk ²为x轴向陀螺k时刻的二位置量测噪声，量测噪声都为白噪声，上标1表示在惯性导航系统在静基座初始时，即位置1处的值，上标2表示将惯性导航系统绕基座的天向轴旋转180度后，即位置2处的值；k时刻即当前时刻，k-1时刻即前一时刻。

5.根据权利要求1所述的一种基于粒子滤波的惯性导航系统初始对准方法，其特征在于步骤4)所述的初始对准的高斯粒子滤波器的构建包括如下步骤：

Step1：初始化，在p(x₀)抽取N个样本点x₀ ⁱ，相应权值1/N；

Step2：抽样 $x_k^i\·q\left(x_k\right|x_{k-1},z_k)=p\left(x_k\right|z_{1;k-1});$

Step3：计算权值并归一化， ${\hat{w}}_k^i\∝p\left(z_k\right|x_k^i),w_k^i={\hat{w}}_k^i/\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\hat{w}}_k^i;$

Step4：计算估计值， ${\stackrel{\‾}{x}}_k\≈\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_k^i{x}_k^i;$

Step5：对离散的滤波密度进行高斯近似；

Step6：从近似的高斯分布中抽取新的粒子x_k ^j，相应权值1/N；

Step7：将新的粒子x_k ^j进行下一周期的滤波；

其中，p(·)为概率密度函数，q(·)为重要密度函数，N为大于1的自然数，x₀为滤波初值，x₀ ⁱ为滤波初始粒子，x_k ⁱ为k时刻粒子，x_k为k时刻状态，x_k-1为k-1时刻状态，z_k为k时刻二位置观测量，z_1:k-1为1时刻到k-1时刻的二位置观测量，

为k时刻未归一化的粒子权值，w_k ⁱ为k时刻归一化后粒子的权值，

为k时刻的滤波估值，x_k ^j为近似高斯分布中抽取的新粒子，i，j＝1，2，3......N。

6.根据权利要求1所述的一种基于粒子滤波的惯性导航系统初始对准方法，其特征在于步骤5)所述的初始对准完成后，在位置1处采集P次惯性器件输出数据，惯性导航系统在静基座初始时，即位置1处；然后，将惯性导航系统绕基座的天向轴旋转180度即位置2处，基座稳定后再采集P次惯性器件输出数据；然后将两次采集的数据依次构造二位置量测量，并对采集数据进行滤波处理，其中P为大于1的自然数。

Images