CN103644913B - 基于直接导航模型的无迹卡尔曼非线性初始对准方法 - Google Patents

Abstract

本发明提供了一种基于直接导航模型的无迹卡尔曼非线性初始对准方法。主要步骤包括：凝固粗对准；建立基于导航参数微分方程（即直接导航模型）以及以导航系速度为量测量的非线性滤波模型及其离散化；构建时间更新和量测更新不同步的简化无迹卡尔曼滤波算法，实现捷联惯性导航初始对准，为捷联惯性导航初始对准提供了一种新方案。优点在于直接导航模型非线性模型更为准确，简化的无迹卡尔曼非线性滤波更具有普适性，滤波过程中只需要一套滤波算法同时实现滤波与姿态更新过程，而且滤波后直接输出导航参数，不需要进行误差修正，算法更简单。

Description

基于直接导航模型的无迹卡尔曼非线性初始对准方法

技术领域

本发明涉及一种捷联惯性导航系统初始对准技术，可用于捷联惯性系统的初始姿态的确定，属于导航、制导与控制技术领域。

背景技术

捷联惯性导航系统没有实体平台，在导航定位解算前必须初始对准，确定初始的位置、速度及姿态信息，其中初始姿态信息的确定是捷联惯性导航系统初始对准的关键内容。捷联惯性导航系统的惯性器件直接安装在载体上，载体的晃动干扰直接加给惯性器件，因此捷联惯性导航系统通常工作在具有各种噪声的动态环境中，初始对准的环境并不理想。

现有的捷联惯性导航系统初始对准分为粗对准和精对准两个阶段。粗对准阶段，依靠重力矢量和地球速率矢量的测量值，粗略估算出载体坐标系到导航参考坐标系的变换矩阵，与真实的姿态矩阵存在偏差，尤其是在外部干扰比较大的情况下。精对准阶段，常利用系统误差模型估计出地理坐标系与真实地理坐标系之间的失准角，并补偿误差，提高对准精度。捷联惯性导航系统误差模型为一组非线性微分方程，本质是非线性的，滤波估计的是导航参数误差，具体应用时往往需要进行简化、近似或者加入约束条件，而受到很多限制，例如小失准角时常将系统误差方程近似为线性误差模型而使用线性卡尔曼滤波，要求粗对准的精度高，失准角很小，然而这在对准环境恶劣情况中往往达不到。采用系统误差模型滤波时通常为两套算法，一套为姿态更新算法，另一套为误差估计滤波算法，待误差估计后，对导航参数进行误差修正，算法复杂。本发明采用直接导航模型，而不是系统误差模型，量测量采用东、北、天三个方向的速度，而不是速度的误差，没有任何近似和约束条件，模型更为准确；滤波过程中时间更新过程与姿态更新过程合并完成，待有量测数据时进行量测更新，滤波输出为导航参数，不需要进行误差修正，算法更简单，采用无迹卡尔曼非线性滤波算法，不受限于线性模型，应用更广泛，具有普适性。

发明内容

本发明的目的在于不采用传统的捷联惯性导航系统的误差模型，而采用更简单的基于直接导航模型和实用性更为广泛的非线性滤波方法，实现捷联惯性导航系统的初始对准，为捷联惯性导航系统的初始对准提供一种新途径和新方案。

本发明的目的是这样实现的，它包括以下步骤：

步骤1、根据惯性测量单元中陀螺仪测得的角速度信息和加速度计测得的线加速度信息，应用基于凝固解析粗对准算法完成捷联惯性导航系统粗对准，求得粗略的初始纵摇角θ、横摇角γ和航向角ψ；

步骤2、根据捷联惯性导航系统的速度微分方程、欧拉角微分方程、传感器模型，以导航系速度为量测量，建立基于导航参数微分方程的非线性滤波连续系统模型；

步骤3、将步骤2中建立的非线性连续系统模型离散化，形成非线性离散系统模型；

步骤4、根据步骤3中建立的非线性离散系统模型构建用于初始对准的简化无迹卡尔曼非线性滤波器，滤波算法中，时间更新与量测更新不同步；

步骤5、将步骤1提取的粗对准姿态角以及陀螺仪和加速度计的输出代入步骤4中，完成捷联惯性导航系统初始对准，直接输出导航参数。

本发明还可以包括这样一些特征：

1.所述的步骤2根据捷联惯性导航系统的速度微分方程、欧拉角微分方程、传感器模型，利用导航系速度为量测量，建立基于导航参数微分方程(即直接导航模型)的非线性滤波连续系统模型，具体为：

捷联惯性导航系统的速度微分方程为

${\stackrel{\·}{v}}_{en}^n=f^n-(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×v_{en}^n+g^n---\left(1\right)$

式中，v_e、v_n和v_u分别为东、北、天速度，[.]^T表示矩阵转置；

ω_ie为地球自转角速率，L为当地地理纬度；

R_n为子午圈曲率半径，R_e为卯酉圈曲率半径；R_n＝R_q(1-2e+3esin²L)，R_e＝R_q(esin²L+1)，R_q为参考椭球赤道平面半径，e为地球扁率；

gⁿ＝[0,0,-g]^T，g为重力加速度；

且 和分别为三只加速度计的测量值， 和分别为三只加速度计的测量误差，分为两部分，一部分为随机常值▽_x、▽_y和▽_z，一部分为随机噪声w_ax、w_ay和w_az；

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

捷联惯性导航系统的欧拉角微分方程

$\left[\begin{array}{c}\stackrel{\·}{\mathrm{\θ}}\\ \stackrel{\·}{\mathrm{\γ}}\\ \stackrel{\·}{\mathrm{\ψ}}\end{array}\right]=\frac{1}{\cos \mathrm{\θ}}\left[\begin{array}{ccc}cos\mathrm{\θ}cos\mathrm{\γ}& 0& sin\mathrm{\γ}cos\mathrm{\θ}\\ sin\mathrm{\θ}sin\mathrm{\γ}& cos\mathrm{\θ}& -cos\mathrm{\γ}sin\mathrm{\θ}\\ -sin\mathrm{\γ}& 0& \cos \mathrm{\γ}\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_{nbx}^b\\ {\mathrm{\ω}}_{nby}^b\\ {\mathrm{\ω}}_{nbz}^b\end{array}\right]---\left(2\right)$

式中， 和为三只陀螺仪测量值， 和为三只陀螺仪的测量误差，分为两部分，一部分为随机常值ε_x、ε_y和ε_z，一部分为随机噪声w_gx、w_gy和w_gz。

捷联惯性导航系统的传感器模型

${\stackrel{\·}{\▿}}_x=0,{\stackrel{\·}{\▿}}_y=0,{\stackrel{\·}{\▿}}_z=0,{\stackrel{\·}{\mathrm{\ϵ}}}_x=0,{\stackrel{\·}{\mathrm{\ϵ}}}_y=0,{\stackrel{\·}{\mathrm{\ϵ}}}_z=0---\left(3\right)$

取12维系统状态向量为

x＝[v_e,v_n,v_u,θ,γ,ψ,▽_x,▽_y,▽_z,ε_x,ε_y,ε_z]^T

式中，x为状态向量；

系统噪声向量为

w＝[w_ax,w_ay,w_az,w_gx,w_gy,w_gz,0,0,0,0,0,0]^T

式中，w为系统噪声向量；

取3维量测向量为

z＝[v_e,v_n,v_u]^T

式中，z为量测向量；

量测噪声向量为

η＝[w_ve,w_vn,w_vu]^T

式中，η为量测噪声向量，w_ve、w_vn、w_vu分别为东、北、天速度的量测噪声，假设为零均值高斯白噪声；

根据捷联惯性导航系统的速度微分方程、姿态欧拉角微分方程和传感器模型以及量测量建立的基于导航参数微分方程(即直接导航模型)的非线性滤波连续系统模型为：

$\left\{\begin{array}{c}\stackrel{\·}{x}=F\left(x\right)+G\left(x\right)w\\ z=Hx+\mathrm{\η}\end{array}\right.$

式中，H＝[I_3×3 0_3×9]；F(.)为非线性的状态函数，F(x)和G(x)参照式(1)-(3)写出。

2.所述的步骤3中连续系统模型离散化后，非线性系统模型为

$\left\{\begin{array}{c}{x}_k=F\left(x_{k-1}\right)+w_{k-1}\\ z_k=H_k{x}_k+{\mathrm{\η}}_k\end{array}\right.$

式中，w_k-1和η_k满足

$\left\{\begin{array}{c}E\[w_k\]=0,E\[w_k{w}_j^T\]=Q_k{\mathrm{\δ}}_{kj}\\ E\[{\mathrm{\η}}_k\]=0,E\[{\mathrm{\η}}_k{\mathrm{\η}}_j^T\]=R_k{\mathrm{\δ}}_{kj}\\ E\[w_k{\mathrm{\η}}_j^T\]=0\end{array}\right.$

式中，δ_kj是δ函数，E[.]表示均值。

离散的系统噪声方差强度阵为

Q_k＝G(x_k)QG(x_k)^TT

离散的量测噪声方差强度阵为

R_k＝R/T_f

式中，Q_k为离散系统噪声w_k的方差强度阵；Q为连续系统噪声w的方差强度阵；T为时间更新周期；R_k为离散系统噪声η_k的方差强度阵；R为连续系统噪声η的方差强度阵；T_f为滤波周期；

状态方程离散化采用四阶龙格库塔方法。

3.所述的步骤4中用于初始对准的简化无迹卡尔曼非线性滤波器的算法框图如附图2所示，具体滤波步骤为

1)初始化状态变量及其均方差

${\hat{x}}_0=E\left(x_0\right),P_0=E\left(\right({\hat{x}}_0-x_0\left){\left({\hat{x}}_0-x_0\right)}^T\right)$

2)时间更新

分解前一步的方差阵：

$P_{k-1|k-1}=S_{k-1|k-1}{S}_{k-1|k-1}^T$

计算状态积分点：

${\mathrm{\χ}}_{i,k-1|k-1}=S_{k-1|k-1}{\mathrm{\ξ}}_i+{\hat{x}}_{k-1|k-1}$

计算非线性状态函数传播的积分点：

${\mathrm{\χ}}_{i,k|k-1}^{*}=F\left({\mathrm{\χ}}_{i,k-1|k-1}\right)$

状态一步预测：

${\hat{x}}_{k|k-1}=\underset{i=1}{\overset{m}{\Σ}}{\mathrm{\χ}}_{i,k|k-1}^{*}{a}_i$

计算一步预测方差阵：

$P_{k|k-1}=\underset{i=1}{\overset{m}{\Σ}}{a}_i{\mathrm{\χ}}_{i,k|k-1}^{*}{\mathrm{\χ}}_{i,k|k-1}^{*T}-{\hat{x}}_{k|k-1}{\hat{x}}_{k|k-1}^T+Q_{k-1}$

状态及状态方差阵更新：

${\hat{x}}_{k|k-1}\→{\hat{x}}_{k|k}$

P_k|k-1→P_k|k

式中，A→B表示把A的值赋值给B；

3)若量测更新周期时间到，则进行量测更新

计算一步预测量测值：

${\hat{z}}_{k|k-1}=H_k{\hat{x}}_{k|k-1}$

计算一步预测量测方差阵：

$P_{zz,k|k-1}=H_k{P}_{k|k-1}{H}_k^T+R_k$

计算一步预测协方差阵：

$P_{xz,k|k-1}=P_{k|k-1}{H}_k^T$

计算卡尔曼增益：

$K_k=P_{xz,k|k-1}{P}_{zz,k|k-1}^{-1}$

状态更新：

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z_k-{\hat{z}}_{k|k-1})$

状态方差阵更新：

$P_{k|k}=P_{k|k-1}-K_k{P}_{zz,k|k-1}{K}_k^T$

上述算法中，m、ξ_i和a_i分别为无迹卡尔曼积分点总数目、积分点及对应权重。

无迹卡尔曼积分点以及相应权重为

${\mathrm{\ξ}}_i=\left\{\begin{array}{c}{\[0,0,...0\]}^T,i=0\\ \left(\sqrt{(n+k)}\right){\[1\]}_i,i=1,2,...2n\end{array}\right.$

式中，每个列向量有n个元素，n为状态量个数，n＝12，共有2n个列向量；

$a_i=\left\{\begin{array}{c}\frac{k}{n+k},i=0\\ \frac{1}{2(n+k)},i=1,2,...2n\end{array}\right.$

式中，k＝-7；

积分点总数目为

m＝2n+1＝25。

4.所述的步骤4中简化非线性滤波器时间更新与量测更新不同步，滤波过程与姿态更新过程合并完成，时间更新周期与传感器数据更新周期一致，即时间T，时间更新也是姿态更新过程；量测更新周期与量测量更新周期一致，为滤波周期，即时间T_f。

5.所述的步骤5中滤波输出即为导航参数，不需要进行误差修正，滤波输出为v_e，v_n，v_u，θ，γ及ψ。

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

本发明的优点在于：

1)本发明采用直接导航模型，没有任何近似和约束条件，较传统的系统误差模型，模型更为准确；

2)本发明滤波过程采用简化的无迹卡尔曼非线性滤波，较线性滤波方法，非线性滤波更具有普适性；

3)本发明滤波过程中只需要一套滤波算法，滤波的同时进行姿态更新，较传统的误差模型算法中的两套算法，算法简单；

4)本发明滤波后直接输出导航参数，不需要进行误差修正，较传统的估计误差的算法更简单，且不受限于线性模型，应用更广泛；

因此本发明为捷联惯性导航系统初始对准提供了一种新的解决方案，可以精确估计出初始姿态角。

本发明提出的方案通过如下半物理试验加以验证：

1)本试验数据来源于某型惯导IMU的三轴摇摆台数据，晃动频率为0.2Hz，晃动幅值为5°，最后转台停下来，航向为45°，以检验初始对准的精度；

2)传感器数据采样时间T为5ms，滤波周期T_f为1s，仿真时间半小时，前面5分钟用于粗对准；

3)三只加速度计随机常值均设为0.2mg，随机噪声均设0.2mg，三只陀螺仪随机常值0.03°/h，随机噪声0.03°/h；

4)位置：初始纬度为26.5017°，初始经度为106.71667°；

5)滤波器的初始条件设为

x＝[0,0,0,θ,γ,ψ,0,0,0,0,0,0]^T，式中θ，γ，ψ为5分钟粗对准结束时的姿态角；

z＝[0,0,0]^T；

P₀＝diag{(0.1m/s)²,(0.1m/s)²,(0.1m/s)²,(0.1°)²,(0.1°)²,(0.3°)²,(0.2mg)²,(0.2mg)²,(0.2mg)²,(0.03°/h)²,(0.03°/h)²,(0.03°/h)²}；

Q＝diag{(0.2mg)²,(0.2mg)²,(0.2mg)²,(0.03°/h)²,(0.03°/h)²,0.03°/h)²,0,0,0,0,0,0}；

R＝diag{(0.1m/s)²,(0.1m/s)²,(0.1m/s)²}。

式中，diag{.}表示对角矩阵。

通过计算机仿真，摇摆台的初始对准姿态角θ，γ，ψ的曲线如附图3-5所示。对准误差分别为0.41′，0.17′，12.85′，满足系统要求。

附图说明

图1为本发明实现捷联惯性导航系统初始对准的具体实施方式；

图2为本发明的简化无迹卡尔曼非线性滤波的算法框图；

图3为本发明的半物理仿真初始对准的纵摇角θ的曲线图及其局部放大图；

图4为本发明的半物理仿真初始对准的横摇角γ的曲线图及其局部放大图；

图5为本发明的半物理仿真初始对准的航向角ψ的曲线图及其局部放大图。

具体实施方式

下面结合附图1，通过具体的半物理仿真，对本发明做详尽描述：

步骤1、根据惯性测量单元中陀螺仪测得的角速度信息和加速度计测得的线加速度信息，应用基于凝固解析粗对准算法完成捷联惯性导航系统粗对准，求得粗略的初始纵摇角θ、横摇角γ和航向角ψ；

步骤2、根据捷联惯性导航系统的速度微分方程、欧拉角微分方程、传感器模型，以导航系速度为量测量，建立基于导航参数微分方程的非线性滤波连续系统模型；

捷联惯性导航系统的速度微分方程为

${\stackrel{\·}{v}}_{en}^n=f^n-(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×v_{en}^n+g^n---\left(1\right)$

式中，v_e、v_n和v_u分别为东、北、天速度，[.]^T表示矩阵转置；

ω_ie为地球自转角速率，L为当地地理纬度；

R_n为子午圈曲率半径，R_e为卯酉圈曲率半径；R_n＝R_q(1-2e+3esin²L)，R_e＝R_q(esin²L+1)，R_q为参考椭球赤道平面半径，e为地球扁率；

gⁿ＝[0,0,-g]^T，g为重力加速度；

且 和分别为三只加速度计的测量值， 和分别为三只加速度计的测量误差，分为两部分，一部分为随机常值▽_x、▽_y和▽_z，一部分为随机噪声w_ax、w_ay和w_az；

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

捷联惯性导航系统的欧拉角微分方程

$\left[\begin{array}{c}\stackrel{\·}{\mathrm{\θ}}\\ \stackrel{\·}{\mathrm{\γ}}\\ \stackrel{\·}{\mathrm{\ψ}}\end{array}\right]=\frac{1}{\cos \mathrm{\θ}}\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\γ}& 0& \sin \mathrm{\γ}\cos \mathrm{\θ}\\ \sin \mathrm{\θ}\sin \mathrm{\γ}& \cos \mathrm{\θ}& -\cos \mathrm{\γ}\sin \mathrm{\θ}\\ -\sin \mathrm{\γ}& 0& \cos \mathrm{\γ}\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_{nbx}^b\\ {\mathrm{\ω}}_{nby}^b\\ {\mathrm{\ω}}_{nbz}^b\end{array}\right]---\left(2\right)$

式中， 和为三只陀螺仪测量值， 和为三只陀螺仪的测量误差，分为两部分，一部分为随机常值ε_x、ε_y和ε_z，一部分为随机噪声w_gx、w_gy和w_gz。

捷联惯性导航系统的传感器模型

${\stackrel{\·}{\▿}}_x=0,{\stackrel{\·}{\▿}}_y=0,{\stackrel{\·}{\▿}}_z=0,{\stackrel{\·}{\mathrm{\ϵ}}}_x=0,{\stackrel{\·}{\mathrm{\ϵ}}}_y=0,{\stackrel{\·}{\mathrm{\ϵ}}}_z=0---\left(3\right)$

取12维系统状态向量为

x＝[v_e,v_n,v_u,θ,γ,ψ,▽_x,▽_y,▽_z,ε_x,ε_y,ε_z]^T

式中，x为状态向量。

系统噪声向量为

w＝[w_ax,w_ay,w_az,w_gx,w_gy,w_gz,0,0,0,0,0,0]^T

式中，w为系统噪声向量；

取3维量测向量为

z＝[v_e,v_n,v_u]^T

式中，z为量测向量；

量测噪声向量为

η＝[w_ve,w_vn,w_vu]^T

式中，η为量测噪声向量，w_ve、w_vn、w_vu分别为东、北、天速度的量测噪声，假设为零均值高斯白噪声；

根据捷联惯性导航系统的速度微分方程、姿态欧拉角微分方程和传感器模型以及量测量建立的基于导航参数微分方程(即直接导航模型)的非线性滤波连续系统模型为：

$\left\{\begin{array}{c}\stackrel{\·}{x}=F\left(x\right)+G\left(x\right)w\\ z=Hx+\mathrm{\η}\end{array}\right.$

式中，H＝[I_3×30_3×9]；F(.)为非线性的状态函数，F(x)和G(x)参照式(1)-(3)写出。

步骤3、将步骤2中建立的非线性连续系统模型离散化，形成非线性离散系统模型；

步骤2中建立的连续系统模型中状态方程是非线性的，采用四阶龙格库塔方法将状态方程离散化，离散化后，非线性系统模型为

$\left\{\begin{array}{c}{x}_k=F\left(x_{k-1}\right)+w_{k-1}\\ z_k=H_k{x}_k+{\mathrm{\η}}_k\end{array}\right.$

式中，w_k-1和η_k满足

$\left\{\begin{array}{c}E\[w_k\]=0,E\[w_k{w}_j^T\]=Q_k{\mathrm{\δ}}_{kj}\\ E\[{\mathrm{\η}}_k\]=0,E\[{\mathrm{\η}}_k{\mathrm{\η}}_j^T\]=R_k{\mathrm{\δ}}_{kj}\\ E\[w_k{\mathrm{\η}}_j^T\]=0\end{array}\right.$

式中，δ_kj是δ函数，E[.]表示均值。

离散的系统噪声方差强度阵为

Q_k＝G(x_k)QG(x_k)^TT

离散的量测噪声方差强度阵为

R_k＝R/T_f

式中，Q_k为离散系统噪声w_k的方差强度阵；Q为连续系统噪声w的方差强度阵；T为时间更新周期；R_k为离散系统噪声η_k的方差强度阵；R为连续系统噪声η的方差强度阵；T_f为滤波周期；

步骤4、根据步骤3中建立的非线性离散系统模型构建用于初始对准的简化无迹卡尔曼非线性滤波器，滤波算法中，时间更新与量测更新不同步；

建立的离散模型为状态方程非线性、量测方程线性的具有加性噪声的系统模型，可采用简化无迹卡尔曼非线性滤波方法，步骤4中用于初始对准的简化无迹卡尔曼非线性滤波的滤波步骤为

1)初始化状态变量及其均方差

${\hat{x}}_0=E\left(x_0\right),P_0=E\left(\right({\hat{x}}_0-x_0\left){\left({\hat{x}}_0-x_0\right)}^T\right)$

2)时间更新

分解前一步的方差阵：

$P_{k-1|k-1}=S_{k-1|k-1}{S}_{k-1|k-1}^T$

计算状态积分点：

${\mathrm{\χ}}_{i,k-1|k-1}=S_{k-1|k-1}{\mathrm{\ξ}}_i+{\hat{x}}_{k-1|k-1}$

计算非线性状态函数传播的积分点：

${\mathrm{\χ}}_{i,k|k-1}^{*}=F\left({\mathrm{\χ}}_{i,k-1|k-1}\right)$

状态一步预测：

${\hat{x}}_{k|k-1}=\underset{i=1}{\overset{m}{\Σ}}{\mathrm{\χ}}_{i,k|k-1}^{*}{a}_i$

计算一步预测方差阵：

$P_{k|k-1}=\underset{i=1}{\overset{m}{\Σ}}{a}_i{\mathrm{\χ}}_{i,k|k-1}^{*}{\mathrm{\χ}}_{i,k|k-1}^{*T}-{\hat{x}}_{k|k-1}{\hat{x}}_{k|k-1}^T+Q_{k-1}$

状态及状态方差阵更新：

${\hat{x}}_{k|k-1}\→{\hat{x}}_{k|k}$

P_k|k-1→P_k|k

式中，A→B表示把A的值赋值给B。

3)若量测更新周期时间到，则进行量测更新

计算一步预测量测值：

${\hat{z}}_{k|k-1}=H_k{\hat{x}}_{k|k-1}$

计算一步预测量测方差阵：

$P_{zz,k|k-1}=H_k{P}_{k|k-1}{H}_k^T+R_k$

计算一步预测协方差阵：

$P_{xz,k|k-1}=P_{k|k-1}{H}_k^T$

计算卡尔曼增益：

$K_k=P_{xz,k|k-1}{P}_{zz,k|k-1}^{-1}$

状态更新：

${\hat{x}}_{k|k}+{\hat{x}}_{k|k-1}+K_k(z_k-{\hat{z}}_{k|k-1})$

状态方差阵更新：

$P_{k|k}=P_{k|k-1}-K_k{P}_{zz,k|k-1}{K}_k^T$

上述算法中，m、ξ_i和a_i分别为无迹卡尔曼积分点总数目、积分点及对应权重。

无迹卡尔曼积分点以及相应权重为

${\mathrm{\ξ}}_i=\left\{\begin{array}{c}{\[0,0,...0\]}^T,i=0\\ \left(\sqrt{(n+k)}\right){\[1\]}_i,i=1,2,...2n\end{array}\right.$

式中，每个列向量有n个元素，n为状态量个数，n＝12，共有2n个列向量；

$a_i=\left\{\begin{array}{c}\frac{k}{n+k},i=0\\ \frac{1}{2(n+k)},i=1,2,...2n\end{array}\right.$

式中，k＝-7；

积分点总数目为

m＝2n+1＝25；

简化无迹卡尔曼非线性滤波器过程中，时间更新与量测更新不同步，滤波过程与姿态更新过程合并完成，时间更新周期与传感器数据更新周期一致，即时间T，时间更新也是姿态更新过程；量测更新周期与量测量更新周期一致，为滤波周期，即时间T_f。

步骤5、将步骤1提取的粗对准姿态角以及陀螺仪和加速度计的输出代入步骤4中，完成捷联惯性导航系统初始对准，直接输出导航参数。

滤波输出即为导航参数，不需要进行误差修正，滤波输出为v_e，v_n，v_u，θ，γ及ψ。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。

Claims (1)

1.基于直接导航模型的无迹卡尔曼非线性初始对准方法，包括下列步骤：

步骤1、根据惯性测量单元中陀螺仪测得的角速度信息和加速度计测得的线加速度信息，使用基于凝固解析粗对准算法完成捷联惯性导航系统粗对准，求得粗略的初始纵摇角θ、横摇角γ和航向角ψ；

步骤2、根据捷联惯性导航系统的速度微分方程、欧拉角微分方程、传感器模型，以导航系速度为量测量，建立基于导航参数微分方程的非线性滤波连续系统模型；

具体为：

捷联惯性导航系统的速度微分方程为

${\stackrel{\·}{v}}_{en}^n=f^n-(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×v_{en}^n+g^n---\left(1\right)$

式中，v_e、v_n和v_u分别为东、北、天速度，[.]^T表示矩阵转置；

ω_ie为地球自转角速率，L为当地地理纬度；

R_n为子午圈曲率半径，R_e为卯酉圈曲率半径；

R_n＝R_q(1-2e+3esin²L)，R_e＝R_q(esin²L+1)，R_q为参考椭球赤道平面半径，e为地球扁率；

gⁿ＝[0,0,-g]^T，g为重力加速度；

且 和分别为三只加速度计的测量值， 和分别为三只加速度计的测量误差，分为两部分，一部分为随机常值和一部分为随机噪声w_ax、w_ay和w_az；

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

捷联惯性导航系统的欧拉角微分方程

$\left[\begin{array}{c}\stackrel{\·}{\mathrm{\θ}}\\ \stackrel{\·}{\mathrm{\γ}}\\ \stackrel{\·}{\mathrm{\ψ}}\end{array}\right]=\frac{1}{\cos \mathrm{\θ}}\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\γ}& 0& \sin \mathrm{\γ}\cos \mathrm{\θ}\\ \sin \mathrm{\θ}\sin \mathrm{\γ}& \cos \mathrm{\θ}& -\cos \mathrm{\γ}\sin \mathrm{\θ}\\ -\sin \mathrm{\γ}& 0& \cos \mathrm{\γ}\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_{nbx}^b\\ {\mathrm{\ω}}_{nby}^b\\ {\mathrm{\ω}}_{nbz}^b\end{array}\right]---\left(2\right)$

式中， 和为三只陀螺仪测量值， 和为三只陀螺仪的测量误差，分为两部分，一部分为随机常值ε_x、ε_y和ε_z，一部分为随机噪声w_gx、w_gy和w_gz；

捷联惯性导航系统的传感器模型

${\stackrel{\·}{\▿}}_x=0,{\stackrel{\·}{\▿}}_y=0,{\stackrel{\·}{\▿}}_z=0\stackrel{\·}{,}{\stackrel{\·}{\mathrm{\ϵ}}}_x=0\stackrel{\·}{,}{\stackrel{\·}{\mathrm{\ϵ}}}_y=0\stackrel{\·}{,}{\stackrel{\·}{\mathrm{\ϵ}}}_z=0---\left(3\right)$

取12维系统状态向量为

$x={\[v_e,v_n,v_u,\mathrm{\θ},\mathrm{\γ},\mathrm{\ψ},{\▿}_x,{\▿}_y,{\▿}_z,{\mathrm{\ϵ}}_x,{\mathrm{\ϵ}}_y,{\mathrm{\ϵ}}_z\]}^T$

式中，x为状态向量；

系统噪声向量为

w＝[w_ax,w_ay,w_az,w_gx,w_gy,w_gz,0,0,0,0,0,0]^T

式中，w为系统噪声向量；

取3维量测向量为

z＝[v_e,v_n,v_u]^T

式中，z为量测向量；

量测噪声向量为

η＝[w_ve,w_vn,w_vu]^T

式中，η为量测噪声向量，w_ve、w_vn、w_vu分别为东、北、天速度的量测噪声，假设为零均值高斯白噪声；

根据捷联惯性导航系统的速度微分方程、姿态欧拉角微分方程和传感器模型以及量测量建立的基于导航参数微分方程的非线性滤波连续系统模型为：

$\left\{\begin{array}{c}\stackrel{\·}{x}=F\left(x\right)+G\left(x\right)w\\ z=Hx+\mathrm{\η}\end{array}\right.$

式中，H＝[I_3×3 0_3×9]；F(.)为非线性的状态函数，F(x)和G(x)参照式(1)-(3)写出；

步骤3、将步骤2中建立的非线性连续系统模型离散化，形成非线性离散系统模型；

其中连续系统模型离散化后，非线性系统模型为

$\left\{\begin{array}{c}{x}_k=F\left(x_{k-1}\right)+w_{k-1}\\ z_k=H_k{x}_k+{\mathrm{\η}}_k\end{array}\right.$

式中，w_k-1和η_k满足

$\left\{\begin{array}{c}E\[w_k\]=0,E\[w_k{w}_j^T\]=Q_k{\mathrm{\δ}}_{kj}\\ E\[{\mathrm{\η}}_k\]=0,E\[{\mathrm{\η}}_k{\mathrm{\η}}_j^T\]=R_k{\mathrm{\δ}}_{kj}\\ E\[w_k{\mathrm{\η}}_j^T\]=0\end{array}\right.$

式中，δ_kj是δ函数，E[.]表示均值；

离散的系统噪声方差强度阵为

Q_k＝G(x_k)QG(x_k)^TT

离散的量测噪声方差强度阵为

R_k＝R/T_f

式中，Q_k为离散系统噪声w_k的方差强度阵；Q为连续系统噪声w的方差强度阵；T为时间更新周期；R_k为离散系统噪声η_k的方差强度阵；R为连续系统噪声η的方差强度阵；T_f为滤波周期；

状态方程离散化采用四阶龙格库塔方法；

步骤4、根据步骤3中建立的非线性离散系统模型构建用于初始对准的简化无迹卡尔曼非线性滤波器，滤波算法中，时间更新与量测更新不同步；

其中简化无迹卡尔曼非线性滤波器的滤波步骤为

1)初始化状态变量及其均方差

${\hat{x}}_0=E\left(x_0\right),P_0=E\left(\right({\hat{x}}_0-x_0\left){\left({\hat{x}}_0-x_0\right)}^T\right)$

2)时间更新

分解前一步的方差阵：

$P_{k-1|k-1}=S_{k-1|k-1}{S}_{k-1|k-1}^T$

计算状态积分点：

${\mathrm{\χ}}_{i,k-1|k-1}=S_{k-1|k-1}{\mathrm{\ξ}}_i+{\hat{x}}_{k-1|k-1}$

计算非线性状态函数传播的积分点：

${\mathrm{\χ}}_{i,k|k-1}^{*}=F\left({\mathrm{\χ}}_{i,k-1|k-1}\right)$

状态一步预测：

${\hat{x}}_{k|k-1}=\underset{i=1}{\overset{m}{\Σ}}{\mathrm{\χ}}_{i,k|k-1}^{*}{a}_i$

计算一步预测方差阵：

$P_{k|k-1}=\underset{i=1}{\overset{m}{\Σ}}{a}_i{\mathrm{\χ}}_{i,k|k-1}^{*}{\mathrm{\χ}}_{i,k|k-1}^{*T}-{\hat{x}}_{k|k-1}{\hat{x}}_{k|k-1}^T+Q_{k-1}$

状态及状态方差阵更新：

${\hat{x}}_{k|k-1}\→{\hat{x}}_{k|k}$

P_k|k-1→P_k|k

式中，A→B表示把A的值赋值给B；

3)若量测更新周期时间到，则进行量测更新

计算一步预测量测值：

${\hat{z}}_{k|k-1}=H_k{\hat{x}}_{k|k-1}$

计算一步预测量测方差阵：

$P_{zz,k|k-1}=H_k{P}_{k|k-1}{H}_k^T+R_k$

计算一步预测协方差阵：

$P_{xz,k|k-1}=P_{k|k-1}{H}_k^T$

计算卡尔曼滤波增益：

$K_k=P_{xz,k|k-1}{P}_{zz,k|k-1}^{-1}$

状态更新：

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z_k-{\hat{z}}_{k|k-1})$

状态方差阵更新：

$P_{k|k}=P_{k|k-1}-K_k{P}_{zz,k|k-1}{K}_k^T$

上述算法中，m、ξ_i和a_i分别为无迹卡尔曼积分点总数目、积分点及对应权重；

无迹卡尔曼积分点以及相应权重为

${\mathrm{\ξ}}_i=\left\{\begin{array}{c}{\[0,0,...0\]}^T,i=0\\ \left(\sqrt{(n+k)}\right){\[1\]}_i,i=1,2,...2n\end{array}\right.$

式中，每个列向量有n个元素，n为状态量个数，n＝12，共有2n个列向量；

$a_i=\left\{\begin{array}{c}\frac{k}{n+k},i=0\\ \frac{1}{2(n+k)},i=1,2,...2n\end{array}\right.$

式中，k＝-7；

积分点总数目为

m＝2n+1＝25；

简化无迹卡尔曼非线性滤波器在滤波过程中时间更新与量测更新不同步，滤波过程与姿态更新过程合并完成，时间更新周期与传感器数据更新周期一致，即时间T，时间更新也是姿态更新过程；量测更新周期与量测量更新周期一致，为滤波周期，即时间T_f；

步骤5、将步骤1提取的粗对准姿态角以及陀螺仪和加速度计的输出代入步骤4中，完成捷联惯性导航系统初始对准，直接输出导航参数，不需要进行误差修正，滤波输出为v_e，v_n，v_u，θ，γ及ψ。