CN103727941A - 基于载体系速度匹配的容积卡尔曼非线性组合导航方法 - Google Patents

Abstract

本发明提供了基于载体系速度匹配的容积卡尔曼非线性组合导航方法。主要步骤包括：初始化组合导航系统；建立直接以导航参数为状态量以及以载体系速度为量测量的非线性滤波模型及其离散化；构建状态方程和量测方程均为非线性的容积卡尔曼滤波算法，实现组合导航的信息融合和精确的导航定位。优点在于本发明建立的非线性模型以及使用的非线性滤波算法，直接输出的是导航参数，不需要进行误差修正，且捷联惯导系统参数更新与滤波器的时间更新同步实现，算法简单，为以捷联惯性导航系统为主的组合导航系统信息融合提供了一种新方案。

Description

基于载体系速度匹配的容积卡尔曼非线性组合导航方法

技术领域

本发明涉及一种捷联惯性导航系统与提供载体系速度的辅助装置（如多普勒测速仪、里程计等导航设备）构成组合导航系统时的信息融合技术，属于导航、制导与控制技术领域。

背景技术

捷联惯性导航系统中惯性测量单元主要包含三只陀螺仪和三只加速度计，通过对陀螺仪测得的角速度信息和加速度计测得的线加速度信息进行积分来获得载体位置、速度和姿态等导航信息，具有信息完备、自主性强，不依赖于任何外界信息而可以独立工作，短时精度高等优点，在很多情况下，成为运载体的首选导航方式。但是，捷联惯性导航系统的误差随着时间积累，长时间导航时需要其它传感器辅助，因此常与其他导航设备构成组合导航系统。

目前以捷联惯性导航为主的组合导航系统的滤波模型大多是基于误差模型，在误差视为小量的情况下，将误差模型线性化，然后采用卡尔曼滤波对误差进行估计，而捷联惯性导航系统的误差模型本质是非线性的，在做线性化近似时会引入额外误差；采用速度匹配方式时，量测量通常为导航系中的速度误差，而实际情况下量测量通常都是载体系的速度，这需要将载体系速度转换到导航系中；采用误差模型时通常为两套算法，一套为捷联惯性导航系统参数更新算法，另一套为滤波算法，估计导航参数误差，并且需对导航参数进行误差修正。本发明中状态量直接采用导航参数，而不是误差，量测量采用载体系速度，而不是常用的导航系速度误差，模型不做任何近似和约束，更为准确；采用容积卡尔曼非线性滤波算法，不需要线性假设，更符合惯导系统的非线性本质，适用于状态方程和量测方程均为非线性的情况；捷联惯导系统的导航参数更新与滤波过程的时间更新同步实现，且滤波输出为导航参数，不需要进行误差修正，算法简单。

发明内容

本发明的目的在于状态量直接采用导航参数、量测量采用载体系速度、利用容积卡尔曼非线性滤波方法来实现数据融合计算，为以捷联惯性导航系统为主的组合导航系统信息融合提供一种新方案。

本发明主要包含以下步骤：

步骤1、初始化组合导航系统，为系统的导航参数更新提供初始值：使用位置传感器获取载体的初始位置参数；根据陀螺仪测得的角速度信息和加速度计测得的线加速度信息，以及速度传感器测得的载体系速度，由捷联惯性导航系统初始对准算法获取载体的初始姿态欧拉角信息和初始导航系速度信息；

步骤2、根据捷联惯性导航系统的导航参数微分方程、惯性传感器模型，以及以载体系速度为量测量的量测方程，选定10维系统状态量和3维量测量，建立非线性连续系统模型；

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

步骤4、根据步骤3中建立的非线性离散系统模型的状态方程与量测方程均为非线性的特点，构建容积卡尔曼非线性滤波算法，滤波过程中，时间更新周期与量测更新周期不同；

步骤5、将惯性传感器的输出以及辅助传感器提供的载体系速度代入步骤4的滤波算法中，进行非线性滤波，直接输出组合后的导航参数。

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

1.所述的步骤2根据捷联惯性导航系统的速度微分方程、欧拉角微分方程、惯性传感器模型，以及以载体系速度为量测量的量测方程，选定10维状态量和3维量测量，建立非线性连续系统模型，具体为：

以东北天（east-north-up）地理坐标系作为导航坐标系(n系)，以载体右前上（bx-by-bz）方向矢量右手定则构成的坐标系作为载体坐标系（b系）。

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

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

式中，

v_east、v_north和v_up分别为导航系中的东向、北向和天向速度；[.]^T表示矩阵转置；

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

rM为子午圈曲率半径，r_N为卯酉圈曲率半径；r_M=r_q(1-2ρ+3ρsin²L)，r_N=r_q(ρsin²L+1)，r_q为参考椭球赤道平面半径，ρ为地球扁率；

且 $f_{\mathrm{ib}}^b={[{\tilde{f}}_{\mathrm{ibx}}^b-\mathrm{\δ}{f}_{\mathrm{ibx}}^b,{\tilde{f}}_{\mathrm{iby}}^b-\mathrm{\δ}{f}_{\mathrm{iby}}^b,{\tilde{f}}_{\mathrm{ibz}}^b-\mathrm{\δ}{f}_{\mathrm{ibz}}^b]}^T,$

和

分别为载体系坐标轴方向三只加速度计的测量值，

和

分别为三只加速度计相应的测量误差，测量误差分为两部分，一部分为随机常值 和

另一部分为随机噪声w_ax和w_ay和w_az，随机噪声均假设为零均值高斯白噪声， $\mathrm{\δ}{f}_{\mathrm{ibx}}^b={\▿}_{\mathrm{ax}}+w_{\mathrm{ax}},\mathrm{\δ}{f}_{\mathrm{iby}}^b={\▿}_{\mathrm{ay}}+w_{\mathrm{ay}},\mathrm{\δ}{f}_{\mathrm{ibz}}^b={\▿}_{\mathrm{az}}+w_{\mathrm{az}};C_b^n={\left(C_n^b\right)}^T,$

$C_n^b=\begin{array}{}\end{array}\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],$ θ为纵摇角，γ为横摇角，ψ为航向角；gⁿ=[0,0,-g]^T，g为重力加速度；

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

$\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{\ω}}_{\mathrm{nbx}}^b\\ {\mathrm{\ω}}_{\mathrm{nby}}^b\\ {\mathrm{\ω}}_{\mathrm{nbz}}^b\end{array}\right]---\left(2\right)$

式中， $\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{nbx}}^b\\ {\mathrm{\ω}}_{\mathrm{nby}}^b\\ {\mathrm{\ω}}_{\mathrm{nbz}}^b\end{array}\right]={\mathrm{\ω}}_{\mathrm{ib}}^b-C_n^b({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n),{\mathrm{\ω}}_{\mathrm{ib}}^b={[{\widetilde{\mathrm{\ω}}}_{\mathrm{ibx}}^b-{\mathrm{\δ\ω}}_{\mathrm{ibx}}^b,{\widetilde{\mathrm{\ω}}}_{\mathrm{iby}}^b-{\mathrm{\δ\ω}}_{\mathrm{iby}}^b,{\widetilde{\mathrm{\ω}}}_{\mathrm{ibz}}^b-{\mathrm{\δ\ω}}_{\mathrm{ibz}}^b]}^T,$

和

为载体系坐标轴方向三只陀螺仪测量值，

和

为三只陀螺仪相应的测量误差，测量误差分为两部分，一部分为随机常值ε_gx、ε_gy和ε_gz，另一部分为随机噪声w_gx、w_gy和w_gz，随机噪声均假设为零均值高斯白噪声， ${\mathrm{\δ\ω}}_{\mathrm{ibx}}^b={\mathrm{\ϵ}}_{\mathrm{gx}}+w_{\mathrm{gx}},{\mathrm{\δ\ω}}_{\mathrm{iby}}^b={\mathrm{\ϵ}}_{\mathrm{gy}}+w_{\mathrm{gy}},{\mathrm{\δ\ω}}_{\mathrm{ibz}}^b={\mathrm{\ϵ}}_{\mathrm{gz}}+w_{\mathrm{gz}};$

捷联惯性导航系统的惯性传感器（陀螺仪与加速度计）测量误差的随机常值部分模型为

${\stackrel{\·}{\▿}}_{\mathrm{ax}}=0{\stackrel{\·}{,\▿}}_{\mathrm{ay}}=0,{\stackrel{\·}{\▿}}_{\mathrm{az}}=0,{\stackrel{\·}{\mathrm{\ϵ}}}_{\mathrm{gx}}=0,{\stackrel{\·}{\mathrm{\ϵ}}}_{\mathrm{gy}}=0,{\stackrel{\·}{\mathrm{\ϵ}}}_{\mathrm{gz}}=0---\left(3\right)$

选定的10维系统状态向量为

$x=[v_{\mathrm{east}},v_{\mathrm{north}},\mathrm{\θ},\mathrm{\γ},\mathrm{\ψ},{\▿}_{\mathrm{ax}},{\▿}_{\mathrm{ay}},{\mathrm{\ϵ}}_{\mathrm{gx}},{\mathrm{\ϵ}}_{\mathrm{gy}},{\mathrm{\ϵ}}_{\mathrm{gz}}{]}^T$

式中，x为系统状态向量；

系统噪声向量为

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

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

3维量测量为

z＝[v_bx,v_by,v_bz]^T

式中，z为量测向量；v_bx、v_by和v_bz分别为载体系三个坐标轴方向的速度分量；

量测噪声向量为

η＝[η_vbx,η_vby,η_vbz]^T

式中，η为量测噪声向量，η_vbx、η_vby、η_vbz分别为速度v_bx、v_by和v_bz的量测噪声，均假设为零均值高斯白噪声；

量测方程为

$z=C_n^b{[v_{\mathrm{east}},v_{\mathrm{north}},v_{\mathrm{up}}]}^T+\mathrm{\η}---\left(4\right)$

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

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

式中，F(x)为非线性状态转移函数；H(x)为非线性量测函数；G(x)为系统噪声输入函数；F(x)和G(x)参照式（1）-（3）写出，H(x)参照式（4）写出。

2.所述的步骤3中将步骤2中建立的非线性连续系统模型离散化，得到非线性离散系统模型。离散时采用四阶龙格库塔方法将状态方程离散化，非线性离散系统模型为

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

式中，x_k和x_k-1分别为t_k和t_k-1时刻的系统状态向量；z_k为t_k时刻的量测向量；F(x_k-1)为t_k-1时刻的非线性状态转移函数；H(x_k)为t_k时刻的非线性量测函数；w_k和w_k-1分别为t_k和t_k-1时刻的系统噪声向量；η_k为t_k时刻的量测噪声向量，且w_k和η_k满足

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

式中，δ_kj是δ函数。

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

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

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

R_k＝R/T_f

式中，Q_k为离散系统噪声w_k的方差强度阵；G(x_k)为t_k时刻的系统噪声输入函数；Q为连续系统噪声w的方差强度阵；T_s为时间更新周期；R_k为离散系统噪声η_k的方差强度阵；R为连续系统噪声η的方差强度阵；T_f为滤波周期。

3.所述的步骤3中容积卡尔曼非线性滤波器算法框图如附图2所示，根据步骤2中建立的非线性离散系统模型的状态方程与量测方程均为非线性的特点，构建容积卡尔曼非线性滤波器，其滤波步骤为

1）初始化系统状态向量及其方差矩阵

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

式中，

和P₀分别为t₀的时刻的系统状态向量估计值及其方差阵；

2）时间更新

分解前一步的方差阵：

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

式中，P_k-1|k-1和S_k-1|k-1分别为t_k-1时刻的系统状态向量方差阵及其平方根阵；

计算状态容积点：

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

式中，χ_i,k-1|k-1为t_k-1时刻的系统状态向量的第i个容积点；

为t_k-1时刻的系统状态向量估计值；ξ_i为容积卡尔曼第i个容积点；

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

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

式中，

为χ_i,k-1|k-1通过非线性状态函数F(χ_i,k-1|k-1)传播的容积点；

计算状态的一步预测值：

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

式中，为一步预测的t_k时刻系统状态向量;a_i为容积点ξ_i对应的权重；m为容积点总个数；i为容积点的序号；

计算一步预测方差阵：

$P_{k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{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}$

式中，P_k|k-1为一步预测的tk时刻系统状态向量方差阵;Q_k-1为t_k-1时刻的系统噪声方差强度阵；

更新状态及状态方差阵：

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

P_k|k-1→P_k|k

式中，

为t_k时刻系统状态向量估计值;P_k|k为t_k时刻系统状态向量方差阵；

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

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

分解一步预测方差阵：

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

式中，S_k|k-1为一步预测的t_k时刻系统状态向量方差阵的平方根阵；

计算一步预测容积点：

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

式中，χ_i,k|k-1为一步预测的tk时刻系统状态向量的第i个容积点；

计算通过非线性量测函数传播的量测容积点：

Z_i,k|k-1＝H(χ_i,k|k-1)

式中，Z_i,k|k-1为χ_i,k|_k-1通过非线性量测函数H(χ_i,k|k-1)传播的容积点，即一步预测的t_k时刻系统量测向量的第i个容积点；

计算一步预测量测值：

${\hat{z}}_{k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{Z}_{i,k|k-1}{a}_i$

式中，

为一步预测的t_k时刻量测向量；

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

$P_{\mathrm{zz},k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{a}_i{Z}_{i,k|k-1}{Z}_{i,k|k-1}^T-{\hat{z}}_{k|k-1}{\hat{z}}_{k|k-1}^T+R_k$

式中，P_zz,k|k-1为一步预测的tk时刻量测方差阵；

计算一步预测协方差阵：

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

式中，P_xz,k|k-1为一步预测的t_k时刻协方差阵；

计算卡尔曼滤波增益：

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

式中，K_k为t_k时刻卡尔曼滤波增益阵；

更新状态：

${\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}_{\mathrm{zz},k|k-1}{K}_K^T$

容积卡尔曼容积点以及相应权重为

$\left\{\begin{array}{c}{\mathrm{\ξ}}_i=\sqrt{10}{\left[1\right]}_i\\ a_i=\frac{1}{20}\end{array}\right.,i=1,...,20$

式中，

${\left[1\right]}_i\∈\left\{\begin{array}{}\end{array}\left(\begin{array}{c}1\\ 0\\ \·\\ \·\\ \·\\ \·\end{array}\right),\left(\begin{array}{c}0\\ 1\\ \·\\ \·\\ \·\\ 0\end{array}\right),\·\·\·,\left(\begin{array}{c}0\\ 0\\ \·\\ \·\\ \·\\ 1\end{array}\right),\left(\begin{array}{c}-1\\ 0\\ \·\\ \·\\ \·\\ 0\end{array}\right),\left(\begin{array}{c}0\\ -1\\ \·\\ \·\\ \·\\ 0\end{array}\right),\·\·\·,\left(\begin{array}{c}0\\ 0\\ \·\\ \·\\ \·\\ -1\end{array}\right)\right\},$

且每个列向量有10个元素，共有20个列向量；

容积点总数目为

m=20；

容积卡尔曼非线性滤波中时间更新周期与量测更新周期不同，时间更新周期与陀螺仪和加速度计的数据更新周期一致，周期为T_s；时间更新过程中同时实现捷联导航参数更新；量测更新周期与量测量更新周期一致，为滤波周期，周期为T_f。

4.所述的步骤5中由于系统状态向量为导航参数，因此滤波输出不需要进行误差修正，直接为导航参数，即v_east，v_north，θ，γ及ψ。

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

本发明的优点在于：

1）本发明中状态量直接采用导航参数，量测量采用载体系速度，状态方程和量程方程均为非线性，模型不做任何近似和约束，更为准确；

2）本发明中滤波输出直接为导航参数，不需要进行误差修正，捷联惯导系统导航参数更新与滤波器的时间更新同步实现，算法简单；

3）本发明采用容积卡尔曼非线性滤波算法，更符合惯导系统的非线性本质；

因此本发明为以捷联惯性导航系统为主的组合导航系统信息融合提供了一种新的解决方案，可以实现精确的导航定位。

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

1）本试验数据来源于某型惯导IMU的实际跑车数据，里程计辅助导航，提供载体系前向速度量测量，载体右、上两个方向速度为0，构成捷联惯导/里程计组合导航系统，通过与外置差分GPS输出的位置信息进行比较得定位误差；

2）惯性传感器数据更新周期T_s为5ms，滤波周期T_f为0.1s，仿真时间75分钟，前面5分钟用于组合导航系统初始化，总行程为33.5km，最后停在预设终点；

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

4）初始位置：纬度为29.654°，初始经度为115.98°；

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

x=[-v_bysin(ψ)cos(θ),v_bycos(ψ)cos(θ),θ,γ,ψ,0,0,0,0,0]^T，式中θ,，γ，ψ、v_by分别为5分钟组合导航系统初始化结束时的姿态角和里程计提供的前向速度；

z=[0,v_by,0]^T，v_by为里程计实时提供的前向速度；

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

Q=diag{(0.2mg)²,(0.2mg)²,(0.03^°/h)²,(0.03^°/h)²,(0.03^°/h)²,0,0,0,0,0}；R=diag{(0.1m/s)²,(0.1m/s)²,(0.1m/s)²}。

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

通过计算机仿真，组合导航系统的东向速度v_east、北向速度v_north、纵摇角θ、横摇角γ、航向角ψ以及定位曲线如附图3-图8所示。终点定位精度为0.47%D（D为里程数），能够实现精确定位，完全满足组合导航系统要求。

附图说明

图1为本发明的组合导航方法的具体实施方式；

图2为本发明的容积卡尔曼非线性滤波的算法框图；

图3为本发明的半物理仿真组合导航系统的东向速度v_east的曲线图；

图4为本发明的半物理仿真组合导航系统的北向速度v_north的曲线图；

图5为本发明的半物理仿真组合导航系统的纵摇角θ的曲线图；

图6为本发明的半物理仿真组合导航系统的横摇角γ的曲线图；

图7为本发明的半物理仿真组合导航系统的航向角ψ的曲线图；

图8为本发明的半物理仿真组合导航系统的定位曲线图。

具体实施方式

下面结合附图1，通过跑车实测数据进行半物理仿真试验，对本发明做详尽描述：

步骤1、初始化组合导航系统，为系统的导航参数更新提供初始值：使用位置传感器获取载体的初始位置参数；根据陀螺仪测得的角速度信息和加速度计测得的线加速度信息，以及速度传感器测得的载体系速度，由捷联惯性导航系统初始对准算法获取载体的初始姿态欧拉角信息和初始导航系速度信息。

步骤2、根据捷联惯性导航系统的导航参数微分方程、惯性传感器模型，以及以载体系速度为量测量的量测方程，选定10维系统状态量和3维量测量，建立非线性连续系统模型，具体为：

以东北天（east-north-up）地理坐标系作为导航坐标系(n系)，以载体右前上（bx-by-bz）方向矢量右手定则构成的坐标系作为载体坐标系（b系）。

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

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

式中，

v_east、v_north和v_up分别为导航系中的东向、北向和天向速度；[.]T表示矩阵转置；

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

r_M为子午圈曲率半径，r_N为卯酉圈曲率半径；r_M=r_q(1-2ρ+3ρsin²L)，rN=r_q(ρsin²L+1)，r_q为参考椭球赤道平面半径，ρ为地球扁率；

且 $f_{\mathrm{ib}}^b={[{\tilde{f}}_{\mathrm{ibx}}^b-\mathrm{\δ}{f}_{\mathrm{ibx}}^b,{\tilde{f}}_{\mathrm{iby}}^b-\mathrm{\δ}{f}_{\mathrm{iby}}^b,{\tilde{f}}_{\mathrm{ibz}}^b-\mathrm{\δ}{f}_{\mathrm{ibz}}^b]}^T,$

和

分别为载体系坐标轴方向三只加速度计的测量值，

和

分别为三只加速度计相应的测量误差，测量误差分为两部分，一部分为随机常值

和

另一部分为随机噪声w_ax和w_ay和w_az，随机噪声均假设为零均值高斯白噪声， $\mathrm{\δ}{f}_{\mathrm{ibx}}^b={\▿}_{\mathrm{ax}}+w_{\mathrm{ax}},\mathrm{\δ}{f}_{\mathrm{iby}}^b={\▿}_{\mathrm{ay}}+w_{\mathrm{ay}},\mathrm{\δ}{f}_{\mathrm{ibz}}^b={\▿}_{\mathrm{az}}+w_{\mathrm{az}};C_b^n={\left(C_n^b\right)}^T,$ $C_n^b=\begin{array}{}\end{array}\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],$ θ为纵摇角，γ为横摇角，ψ为航向角；gⁿ=[0,0,-g]^T，g为重力加速度；

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

$\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{\ω}}_{\mathrm{nbx}}^b\\ {\mathrm{\ω}}_{\mathrm{nby}}^b\\ {\mathrm{\ω}}_{\mathrm{nbz}}^b\end{array}\right]---\left(2\right)$

式中， $\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{nbx}}^b\\ {\mathrm{\ω}}_{\mathrm{nby}}^b\\ {\mathrm{\ω}}_{\mathrm{nbz}}^b\end{array}\right]={\mathrm{\ω}}_{\mathrm{ib}}^b-C_n^b({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n),{\mathrm{\ω}}_{\mathrm{ib}}^b={[{\widetilde{\mathrm{\ω}}}_{\mathrm{ibx}}^b-{\mathrm{\δ\ω}}_{\mathrm{ibx}}^b,{\widetilde{\mathrm{\ω}}}_{\mathrm{iby}}^b-{\mathrm{\δ\ω}}_{\mathrm{iby}}^b,{\widetilde{\mathrm{\ω}}}_{\mathrm{ibz}}^b-{\mathrm{\δ\ω}}_{\mathrm{ibz}}^b]}^T,$ 和

为载体系坐标轴方向三只陀螺仪测量值，

和

为三只陀螺仪相应的测量误差，测量误差分为两部分，一部分为随机常值ε_gx、ε_gy和ε_gz，另一部分为随机噪声w_gx、w_gy和w_gz，随机噪声均假设为零均值高斯白噪声， ${\mathrm{\δ\ω}}_{\mathrm{ibx}}^b={\mathrm{\ϵ}}_{\mathrm{gx}}+w_{\mathrm{gx}},{\mathrm{\δ\ω}}_{\mathrm{iby}}^b={\mathrm{\ϵ}}_{\mathrm{gy}}+w_{\mathrm{gy}},{\mathrm{\δ\ω}}_{\mathrm{ibz}}^b={\mathrm{\ϵ}}_{\mathrm{gz}}+w_{\mathrm{gz}};$

捷联惯性导航系统的惯性传感器（陀螺仪与加速度计）测量误差的随机常值部分模型为

${\stackrel{\·}{\▿}}_{\mathrm{ax}}=0{\stackrel{\·}{,\▿}}_{\mathrm{ay}}=0,{\stackrel{\·}{\▿}}_{\mathrm{az}}=0,{\stackrel{\·}{\mathrm{\ϵ}}}_{\mathrm{gx}}=0,{\stackrel{\·}{\mathrm{\ϵ}}}_{\mathrm{gy}}=0,{\stackrel{\·}{\mathrm{\ϵ}}}_{\mathrm{gz}}=0---\left(3\right)$

选定的10维系统状态向量为

$x=[v_{\mathrm{east}},v_{\mathrm{north}},\mathrm{\θ},\mathrm{\γ},\mathrm{\ψ},{\▿}_{\mathrm{ax}},{\▿}_{\mathrm{ay}},{\mathrm{\ϵ}}_{\mathrm{gx}},{\mathrm{\ϵ}}_{\mathrm{gy}},{\mathrm{\ϵ}}_{\mathrm{gz}}{]}^T$

式中，x为系统状态向量；

系统噪声向量为

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

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

3维量测量为

z＝[v_bx,v_by,v_bz]^T

式中，z为量测向量；v_bx、v_by和v_bz分别为载体系三个坐标轴方向的速度分量；

量测噪声向量为

η＝[η_vbx,η_vby,η_vbz]^T

式中，η为量测噪声向量，η_vbx、η_vby、η_vbz分别为速度v_bx、v_by和v_bz的量测噪声，均假设为零均值高斯白噪声；

量测方程为

$z=C_n^b{[v_{\mathrm{east}},v_{\mathrm{north}},v_{\mathrm{up}}]}^T+\mathrm{\η}---\left(4\right)$

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

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

式中，F(x)为非线性状态转移函数；H(x)为非线性量测函数；G(x)为系统噪声输入函数；F(x)和G(x)参照式（1）-（3）写出，H(x)参照式（4）写出。

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

离散时采用四阶龙格库塔方法将状态方程离散化，离散化后，非线性离散系统模型为

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

式中，x_k和x_k-1分别为t_k和t_k-1时刻的系统状态向量；z_k为t_k时刻的量测向量；F(x_k-1)为t_k-1时刻的非线性状态转移函数；H(x_k)为t_k时刻的非线性量测函数；w_k和w_k-1分别为t_k和t_k-1时刻的系统噪声向量；η_k为t_k时刻的量测噪声向量，且w_k和η_k满足

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

式中，δk_j是δ函数；

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

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

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

R_k＝R/T_f

式中，Q_k为离散系统噪声w_k的方差强度阵；G(x_k)为t_k时刻的系统噪声输入函数；Q为连续系统噪声w的方差强度阵；T_s为时间更新周期；R_k为离散系统噪声η_k的方差强度阵；R为连续系统噪声η的方差强度阵；T_f为滤波周期。

步骤4、根据步骤3中建立的非线性离散系统模型的状态方程与量测方程均为非线性的特点，构建容积卡尔曼非线性滤波算法，滤波过程中，时间更新周期与量测更新周期不同；

容积卡尔曼非线性滤波器的滤波步骤为

1）初始化系统状态向量及其方差矩阵

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

式中，和P₀分别为t₀的时刻的系统状态向量估计值及其方差阵；

2）时间更新

分解前一步的方差阵：

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

式中，P_k-1|k-1和S_k-1|k-1分别为t_k-1时刻的系统状态向量方差阵及其平方根阵；

计算状态容积点：

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

式中，χ_i,k-1|k-1为t_k-1时刻的系统状态向量的第i个容积点；

为t_k-1时刻的系统状态向量估计值；ξ_i为容积卡尔曼第i个容积点；

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

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

式中，

为χ_i,k-1|k-通过非线性状态函数F(χ_i,k-1|k-1)传播的容积点；

计算状态的一步预测值：

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

式中，

为一步预测的t_k时刻系统状态向量;a_i为容积点ξ_i对应的权

$P_{k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{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}$

式中，P_k|k-1为一步预测的tk时刻系统状态向量方差阵;Q_k-1为t_k-1时刻的系统噪声方差强度阵；

更新状态及状态方差阵：

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

P_k|k-1→P_k|k

式中，

为t_k时刻系统状态向量估计值;P_k|k为t_k时刻系统状态向量方差阵；

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

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

分解一步预测方差阵：

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

式中，S_k|k-1为一步预测的t_k时刻系统状态向量方差阵的平方根阵；

计算一步预测容积点：

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

式中，χ_i,_k|k-1为一步预测的t_k时刻系统状态向量的第i个容积点；

计算通过非线性量测函数传播的量测容积点：

Z_i,k|_k-1＝H(χ_i,k|k-1)

式中，Z_i,k|k-1为χ_i,k|k-1通过非线性量测函数H(χ_i,_k|k-1)传播的容积点，即一步预测的t_k时刻系统量测向量的第i个容积点；

计算一步预测量测值：

${\hat{z}}_{k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{Z}_{i,k|k-1}{a}_i$

式中，

为一步预测的t_k时刻量测向量；

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

$P_{\mathrm{zz},k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{a}_i{Z}_{i,k|k-1}{Z}_{i,k|k-1}^T-{\hat{z}}_{k|k-1}{\hat{z}}_{k|k-1}^T+R_k$

式中，P_zz,k|k-1为一步预测的t_k时刻量测方差阵；

计算一步预测协方差阵：

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

式中，P_xz,k|k-1为一步预测的t_k时刻协方差阵；

计算卡尔曼滤波增益：

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

式中，K_k为t_k时刻卡尔曼滤波增益阵；

更新状态：

${\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}_{\mathrm{zz},k|k-1}{K}_K^T$

容积卡尔曼容积点以及相应权重为

$\left\{\begin{array}{c}{\mathrm{\ξ}}_i=\sqrt{10}{\left[1\right]}_i\\ a_i=\frac{1}{20}\end{array}\right.,i=1,...,20$

式中，

${\left[1\right]}_i\∈\left\{\begin{array}{}\end{array}\left(\begin{array}{c}1\\ 0\\ \·\\ \·\\ \·\\ \·\end{array}\right),\left(\begin{array}{c}0\\ 1\\ \·\\ \·\\ \·\\ 0\end{array}\right),\·\·\·,\left(\begin{array}{c}0\\ 0\\ \·\\ \·\\ \·\\ 1\end{array}\right),\left(\begin{array}{c}-1\\ 0\\ \·\\ \·\\ \·\\ 0\end{array}\right),\left(\begin{array}{c}0\\ -1\\ \·\\ \·\\ \·\\ 0\end{array}\right),\·\·\·,\left(\begin{array}{c}0\\ 0\\ \·\\ \·\\ \·\\ -1\end{array}\right)\right\},$

且每个列向量有10个元素，共有20个列向量；

容积点总数目为

m=20；

容积卡尔曼非线性滤波中时间更新周期与量测更新周期不同，时间更新周期与陀螺仪和加速度计的数据更新周期一致，周期为T_s；时间更新过程中同时实现捷联导航参数更新；量测更新周期与量测量更新周期一致，为滤波周期，周期为T_f。

步骤5、将惯性传感器的输出以及辅助传感器提供的载体系速度代入步骤4的滤波算法中，进行非线性滤波，直接输出组合后的导航参数。

由于状态量为导航参数，因此滤波输出不需要进行误差修正，直接为导航参数，即v_east，v_north，θ，γ及ψ。

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

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

Claims (5)

1.基于载体系速度匹配的容积卡尔曼非线性组合导航方法，其特征在于该方法包括下列步骤：

步骤1、初始化组合导航系统，为系统的导航参数更新提供初始值：使用位置传感器获取载体的初始位置参数；根据陀螺仪测得的角速度信息和加速度计测得的线加速度信息，以及速度传感器测得的载体系速度，通过捷联惯性导航系统使用初始对准算法获取载体的初始姿态欧拉角信息和初始导航系速度信息；

步骤2、根据捷联惯性导航系统的导航参数微分方程、惯性传感器模型，以及以载体系速度为量测量的量测方程，选定10维系统状态量和3维量测量，建立非线性连续系统模型；

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

步骤4、根据步骤3中建立的非线性离散系统模型的状态方程与量测方程均为非线性的特点，构建容积卡尔曼非线性滤波算法，滤波过程中，时间更新与量测更新周期不同；

步骤5、将惯性传感器的输出以及辅助传感器提供的载体系速度代入步骤4的滤波算法中，进行非线性滤波，直接输出组合后的导航参数。

2.根据权利要求1所述的基于载体系速度匹配的容积卡尔曼非线性组合导航方法，其中所述的步骤2根据捷联惯性导航系统的速度微分方程、欧拉角微分方程、惯性传感器模型，以及以载体系速度为量测量的量测方程，选定10维系统状态量和3维量测量，建立非线性连续系统模型，具体为：

以东北天地理坐标系作为导航坐标系-n系，以载体右前上方向矢量右手定则构成的坐标系作为载体坐标系-b系；

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

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

式中，v_east、v_north和v_up分别为导航系中的东向、北向和天向速度；[.]^T表示矩阵转置；

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

r_M为子午圈曲率半径，r_N为卯酉圈曲率半径；r_M=r_q(1-2ρ+3ρsin²L)，r_N=r_q(ρsin²L+1)，r_q为参考椭球赤道平面半径，ρ为地球扁率；

且 $f_{\mathrm{ib}}^b={[{\tilde{f}}_{\mathrm{ibx}}^b-\mathrm{\δ}{f}_{\mathrm{ibx}}^b,{\tilde{f}}_{\mathrm{iby}}^b-\mathrm{\δ}{f}_{\mathrm{iby}}^b,{\tilde{f}}_{\mathrm{ibz}}^b-\mathrm{\δ}{f}_{\mathrm{ibz}}^b]}^T,$

和

分别为载体系坐标轴方向三只加速度计的测量值，

和

分别为三只加速度计相应的测量误差，测量误差分为两部分，一部分为随机常值

和另一部分为随机噪声w_ax和w_ay和w_az，随机噪声均假设为零均值高斯白噪声， $\mathrm{\δ}{f}_{\mathrm{ibx}}^b={\▿}_{\mathrm{ax}}+w_{\mathrm{ax}},\mathrm{\δ}{f}_{\mathrm{iby}}^b={\▿}_{\mathrm{ay}}+w_{\mathrm{ay}},\mathrm{\δ}{f}_{\mathrm{ibz}}^b={\▿}_{\mathrm{az}}+w_{\mathrm{az}};C_b^n={\left(C_n^b\right)}^T,$ $C_n^b=\begin{array}{}\end{array}\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],$ θ为纵摇角，γ为横摇角，ψ为航向角；gⁿ=[0,0,-g]^T，g为重力加速度；

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

$\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{\ω}}_{\mathrm{nbx}}^b\\ {\mathrm{\ω}}_{\mathrm{nby}}^b\\ {\mathrm{\ω}}_{\mathrm{nbz}}^b\end{array}\right]---\left(2\right)$

式中， $\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{nbx}}^b\\ {\mathrm{\ω}}_{\mathrm{nby}}^b\\ {\mathrm{\ω}}_{\mathrm{nbz}}^b\end{array}\right]={\mathrm{\ω}}_{\mathrm{ib}}^b-C_n^b({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n),{\mathrm{\ω}}_{\mathrm{ib}}^b={[{\widetilde{\mathrm{\ω}}}_{\mathrm{ibx}}^b-{\mathrm{\δ\ω}}_{\mathrm{ibx}}^b,{\widetilde{\mathrm{\ω}}}_{\mathrm{iby}}^b-{\mathrm{\δ\ω}}_{\mathrm{iby}}^b,{\widetilde{\mathrm{\ω}}}_{\mathrm{ibz}}^b-{\mathrm{\δ\ω}}_{\mathrm{ibz}}^b]}^T,$

和

为载体系坐标轴方向三只陀螺仪测量值，

和

为三只陀螺仪相应的测量误差，测量误差分为两部分，一部分为随机常值ε_gx、ε_gy和ε_gz，另一部分为随机噪声w_gx、w_gy和w_gz，随机噪声均假设为零均值高斯白噪声 ${\mathrm{\δ\ω}}_{\mathrm{ibx}}^b={\mathrm{\ϵ}}_{\mathrm{gx}}+w_{\mathrm{gx}},{\mathrm{\δ\ω}}_{\mathrm{iby}}^b={\mathrm{\ϵ}}_{\mathrm{gy}}+w_{\mathrm{gy}},{\mathrm{\δ\ω}}_{\mathrm{ibz}}^b={\mathrm{\ϵ}}_{\mathrm{gz}}+w_{\mathrm{gz}};$

捷联惯性导航系统的惯性传感器（陀螺仪与加速度计）测量误差的随机常值部分模型为

${\stackrel{\·}{\▿}}_{\mathrm{ax}}=0{\stackrel{\·}{,\▿}}_{\mathrm{ay}}=0,{\stackrel{\·}{\▿}}_{\mathrm{az}}=0,{\stackrel{\·}{\mathrm{\ϵ}}}_{\mathrm{gx}}=0,{\stackrel{\·}{\mathrm{\ϵ}}}_{\mathrm{gy}}=0,{\stackrel{\·}{\mathrm{\ϵ}}}_{\mathrm{gz}}=0---\left(3\right)$

选定的10维系统状态向量为

x＝[v_east,v_north,θ,γ,ψ,▽_ax,▽_ay,ε_gx,ε_gy,ε_gz]^T

式中，x为系统状态向量；

系统噪声向量为

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

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

3维量测量为

z＝[v_bx,v_by,v_bz]^T

式中，z为量测向量；v_bx、v_by和v_bz分别为载体系三个坐标轴方向的速度分量；

量测噪声向量为

η＝[η_vbx,η_vby,η_vbz]^T

式中，η为量测噪声向量，η_vbx、η_vby、η_vbz分别为速度v_bx、v_by和v_bz的量测噪声，均假设为零均值高斯白噪声；

量测方程为

$z=C_n^b{[v_{\mathrm{east}},v_{\mathrm{north}},v_{\mathrm{up}}]}^T+\mathrm{\η}---\left(4\right)$

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

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

式中，F(x)为非线性状态转移函数；H(x)为非线性量测函数；G(x)为系统噪声输入函数；F(x)和G(x)参照式（1）-（3）写出，H(x)参照式（4）写出。

3.根据权利要求1所述的基于载体系速度匹配的容积卡尔曼非线性组合导航方法，其中所述的步骤3中将步骤2中建立的非线性连续系统模型离散化，得到非线性离散系统模型。采用四阶龙格库塔方法将状态方程离散化，非线性离散系统模型为

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

式中，x_k和x_k-1分别为t_k和t_k-1时刻的系统状态向量；z_k为t_k时刻的量测向量；F(x_k-1)为t_k-1时刻的非线性状态转移函数；H(x_k)为t_k时刻的非线性量测函数；w_k和w_k-1分别为t_k和t_k-1时刻的系统噪声向量；η_k为t_k时刻的量测噪声向量，且w_k和η_k满足

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

式中，δ_kj是δ函数；

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

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

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

R_k＝R/T_f

式中，Q_k为离散系统噪声w_k的方差强度阵；G(x_k)为t_k时刻的系统噪声输入函数；Q为连续系统噪声w的方差强度阵；T_s为时间更新周期；R_k为离散系统噪声η_k的方差强度阵；R为连续系统噪声η的方差强度阵；T_f为滤波周期。

4.根据权利要求1所述的基于载体系速度匹配的容积卡尔曼非线性组合导航方法的步骤，其中所述的步骤3中容积卡尔曼非线性滤波器的滤波步骤为

1）初始化系统状态向量及其方差矩阵

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

式中，

和P₀分别为t₀的时刻的系统状态向量估计值及其方差阵；

2）时间更新

分解前一步的方差阵：

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

式中，P_k-1|k-1和S_k-1|k-1分别为t_k-1时刻的系统状态向量方差阵及其平方根阵；

计算状态容积点：

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

式中，χ_i,k-1|k-1为t_k-1时刻的系统状态向量的第i个容积点；

为t_k-1时刻的系统状态向量估计值；ξ_i为容积卡尔曼第i个容积点；

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

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

式中，

为χ_i,k-1|k-1通过非线性状态函数F(χ_i,k-1|k-1)传播的容积点；

计算状态的一步预测值：

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

式中，

为一步预测的t_k时刻系统状态向量;a_i为容积点ξ_i对应的权重；m为容积点总个数；i为容积点的序号；

计算一步预测方差阵：

$P_{k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{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}$

式中，P_k|k-1为一步预测的tk时刻系统状态向量方差阵;Q_k-1为t_k-1时刻的系统噪声方差强度阵；

更新状态及状态方差阵：

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

P_k|k-1→P_k|k

式中，

为t_k时刻系统状态向量估计值;P_k|k为t_k时刻系统状态向量方差阵；

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

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

分解一步预测方差阵：

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

式中，S_k|k-1为一步预测的tk时刻系统状态向量方差阵的平方根阵；

计算一步预测容积点：

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

式中，χ_i,k|k-1为一步预测的t_k时刻系统状态向量的第i个容积点；

计算通过非线性量测函数传播的量测容积点：

Z_i,k|k-1＝H(χ_i,k|k-1)

式中，Z_i,k|k-1为χ_i,k|_k-1通过非线性量测函数H(χ_i,k|k-1)传播的容积点，即一步预测的t_k时刻系统量测向量的第i个容积点；

计算一步预测量测值：

${\hat{z}}_{k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{Z}_{i,k|k-1}{a}_i$

式中，为一步预测的t_k时刻量测向量；

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

$P_{\mathrm{zz},k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{a}_i{Z}_{i,k|k-1}{Z}_{i,k|k-1}^T-{\hat{z}}_{k|k-1}{\hat{z}}_{k|k-1}^T+R_k$

式中，P_zz,k|k-1为一步预测的t_k时刻量测方差阵；

计算一步预测协方差阵：

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

式中，P_xz,k|k-1为一步预测的t_k时刻协方差阵；

计算卡尔曼滤波增益：

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

式中，K_k为t_k时刻卡尔曼滤波增益阵；

更新状态：

${\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}_{\mathrm{zz},k|k-1}{K}_K^T$

容积卡尔曼容积点以及相应权重为

$\left\{\begin{array}{c}{\mathrm{\ξ}}_i=\sqrt{10}{\left[1\right]}_i\\ a_i=\frac{1}{20}\end{array}\right.,i=1,...,20$ 式中，

${\left[1\right]}_i\∈\left\{\begin{array}{}\end{array}\left(\begin{array}{c}1\\ 0\\ \·\\ \·\\ \·\\ \·\end{array}\right),\left(\begin{array}{c}0\\ 1\\ \·\\ \·\\ \·\\ 0\end{array}\right),\·\·\·,\left(\begin{array}{c}0\\ 0\\ \·\\ \·\\ \·\\ 1\end{array}\right),\left(\begin{array}{c}-1\\ 0\\ \·\\ \·\\ \·\\ 0\end{array}\right),\left(\begin{array}{c}0\\ -1\\ \·\\ \·\\ \·\\ 0\end{array}\right),\·\·\·,\left(\begin{array}{c}0\\ 0\\ \·\\ \·\\ \·\\ -1\end{array}\right)\right\},$

且每个列向量有10个元素，共有20个列向量；

容积点总数目为

m=20；

容积卡尔曼非线性滤波中时间更新周期与量测更新周期不同，时间更新周期与陀螺仪和加速度计的数据更新周期一致，周期为T_s；时间更新过程中同时实现捷联导航参数更新；量测更新周期与量测量更新周期一致，为滤波周期，周期为T_f。

5.根据权利要求1所述的基于载体系速度匹配的容积卡尔曼非线性组合导航方法，其中所述的步骤5中由于系统状态向量为导航参数，从而滤波输出不需要进行误差修正，直接为导航参数，即v_east，v_north，θ，γ及ψ。

Images