CN102654406A - 基于非线性预测滤波与求容积卡尔曼滤波相结合的动基座初始对准方法 - Google Patents

Abstract

本发明的目的在于提供基于非线性预测滤波与求容积卡尔曼滤波相结合的动基座初始对准方法，包括以下步骤：采集陀螺仪及加速度的输出数据；完成粗对准，初步确定载体的姿态矩阵；建立大方位失准角条件下捷联惯导系统初始对准的非线性误差模型、非线性预测滤波与求容积卡尔曼滤波相结合的非线性滤波器；载体先做匀加速直线运动，然后做匀速直线运动；进行捷联解算，同时测量载体的运动速度；取捷联解算的速度与测量的载体的运动速度作为量测量，利用、建立的非线性滤波器对三个平台误差角进行估计；利用估计出的平台误差角对此时的姿态矩阵进行修正，从而完成初始对准。本发明当海况不佳载体出现大幅晃动时，动基座初始对准对准精度高，收敛速度快。

Description

基于非线性预测滤波与求容积卡尔曼滤波相结合的动基座初始对准方法

技术领域

本发明涉及的是一种捷联惯导系统的对准方法。

背景技术

捷联惯导系统是将惯性器件(陀螺仪及加速度计)直接固联在载体上的系统。利用陀螺仪、加速度计测量运载体的角运动和线运动的信息。通过计算机进行积分运算得载体的姿态、速度及位置等导航参数。它与平台式惯导系统相比具有体积小，重量轻，成本低，可靠性高，便于维护等优点，因此得到越来越广泛的应用。初始对准的精度之间关系到惯导系统的工作精度，初始对准的时间是惯导系统的重要战术技术指标。因此，初始对准是惯导系统最重要的关键技术之一。

当海况不佳时，船舶即使处于系泊静止状态下也会产生大幅晃动。从而使得捷联惯导系统无法快速完成自主初始对准。因此在这种情况下需要在运动中借助其他辅助导航设备(如多普勒计程仪)，进行运动中对准。当舰船处于系泊静止状态若载体晃动十分严重时，粗略的获得方位信息都很困难。故此时需要采用非线性滤波方法来实现多普勒计程仪辅助捷联惯导系统的运动中对准。长期以来，扩展卡尔曼滤波器(EKF)因结构简单、易于实现、快速收敛等优点得到广泛的应用。然而EKF存在理论上的局限性：如当系统模型为强非线性时，EKF会导致很大的线性化误差，造成滤波器精度降低，甚至发散；而且需要计算繁琐的雅克比矩阵。针对EKF的不足，近年来人们基于近似一个高斯分布比近似一个非线性函数更加容易的观点，提出了一套全新的非线性滤波方法，即Sigma-Point卡尔曼滤波器(SPKF)，其利用加权统计线性回归技术，通过一组确定性采样点来捕获系统的相关统计参量。随着研究的不断深入，2009年SimonHaykin等提出了一种新的非线性滤波器，即求容积卡尔曼滤波器。它是一种独立于EKF、SPKF算法体系的新的滤波策略，相比与EKF、SPKF等传统高斯域非线性滤波器具有更优的非线性逼近性能、数值精度以及滤波稳定性。然而，与EKF及SPKF类似，CKF的不足之处在于它对模型误差比较敏感，并将其作为高斯白噪声来处理。而非线性预测滤波器能够实时的估计系统的模型误差，可用于模型误差较大的非线性系统的滤波，且不受系统噪声高斯分布条件的限制。缺点是收敛速度慢，因此通过非线性预测滤波器与求容积卡尔曼滤波器相结合来提高大方位失准角条件下动基座初始对准的精度具有重要的意义。

发明内容

本发明的目的在于提供能够有效提高大方位失准角下捷联惯导系统动基座对准精度的基于非线性预测滤波与求容积卡尔曼滤波相结合的动基座初始对准方法。

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

本发明基于非线性预测滤波与求容积卡尔曼滤波相结合的动基座初始对准方法，其特征是：

(1)在系泊状态下，采集陀螺仪及加速度的输出数据；

(2)采用解析法完成捷联惯导系统的粗对准，初步确定载体的姿态矩阵；

(3)建立大方位失准角条件下捷联惯导系统初始对准的非线性误差模型；

(4)建立非线性预测滤波与求容积卡尔曼滤波相结合的非线性滤波器；

(5)载体先做匀加速直线运动，然后做匀速直线运动；

(6)进行捷联解算，同时测量载体的运动速度；

(7)取捷联解算的速度与测量的载体的运动速度作为量测量，利用步骤(4)建立的非线性滤波器对三个平台误差角进行估计；

(8)利用估计出的平台误差角对此时的姿态矩阵进行修正，从而完成初始对准。

本发明还可以包括：

1、所述的建立大方位失准角条件下捷联惯导系统初始对准的非线性误差模型的步骤为：

(1)建立捷联惯导系统初始对准非线性误差方程

使用一阶非线性随机微分方程来描述捷联惯导系统非线性误差方程如下：

$\stackrel{\·}{x}\left(t\right)=f(x\left(t\right),t)+G_d(x\left(t\right),t)d\left(t\right)+w\left(t\right)$

式中，x(t)为t时刻系统的状态变量，f(x(t)，t)为模型向量，G_d(x(t)，t)为模型误差扰动矩阵，d(t)为系统模型误差向量，w(t)为系统的噪声向量；

系统的状态向量为：

x＝[δv_x δv_y φ_x φ_y φ_z]^T

系统模型误差向量为：

$d\left(t\right){\left[\begin{array}{ccccc}{\▿}_x& {\▿}_y& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z\end{array}\right]}^T$

系统的白噪声向量为：

$w\left(t\right)={\left[\begin{array}{ccccc}{w}_{{\▿}_x}& w_{{\▿}_y}& w_{{\mathrm{\ϵ}}_x}& w_{{\mathrm{\ϵ}}_y}& w_{{\mathrm{\ϵ}}_z}\end{array}\right]}^T$

其中，δv_x δv_y分别为系统东向和北向的速度误差，φ_x φ_y φ_z分别为系统东向、北向、天向的姿态误差，

分别为x、y轴加速度计的零偏，ε_x ε_yε_z分别为x、y、z轴陀螺仪的常值漂移，

分别为x、y轴加速度计的白噪声误差，

分别为x、y、z轴陀螺仪的白噪声误差；

模型向量为：

f(x(t)，t)＝A(t)·x+p(x，t)

式中A(t)_5×5为系统线性部分的系数矩阵；p(x，t)为非线性部分，

A(t)_5×5中的非零项元素为：

其中 $\left[\begin{array}{c}{f}_x\\ f_y\\ f_z\end{array}\right]=C_b^{n^{\′}}{\hat{f}}^b;$

其中w_ie为地球自转角速度，

为载体的纬度，R为地球半径，v_x为载体的东向速度，v_y为载体的北向速度，

为计算的捷联矩阵，

为加速度计的输出；

模型误差扰动矩阵：

$G_d(x\left(t\right),t)={\left[\begin{array}{cc}{C}_{b2\×2}^n& 0_{2\×3}\\ 0_{3\×2}& -C_b^{n^{\′}}\end{array}\right]}_{5\×5},$

其中 $C_{b2\×2}^n=\left[\begin{array}{cc}{C}_b^n\left(\mathrm{1,1}\right)& C_b^n\left(\mathrm{1,2}\right)\\ C_b^n\left(\mathrm{2,1}\right)& C_b^n\left(\mathrm{2,2}\right)\end{array}\right];$

(2)建立量测方程：

取捷联惯导系统解算的速度与量测的载体的速度之差作为量测量，则使用一阶线性微分方程来描述捷联惯导系统的量测方程如下：

y(t)＝h(t)x(t)+v(t)

式中y(t)表示t时刻的量测向量，h(t)表示系统的量测矩阵，v(t)为系统的量测噪声；

系统量测矩阵为：

h(t)＝[I_2×2 O_2×3]。

2、所述的建立非线性预测滤波与求容积卡尔曼滤波相结合的非线性滤波器为：

设采样时间为Δt，利用四阶龙格库塔法对所建立的非线性误差模型和量测方程进行离散化处理，可得：

$\left\{\begin{array}{c}{x}_{k+1}=F(x_k,d_k)+w_k\\ y_{k+1}=H_{k+1}{x}_{k+1}+v_{k+1}\end{array}\right.$

则非线性预测滤波与求容积卡尔曼滤波相结合的非线性滤波器包括以下几个部分：

(1)利用非线性预测滤波器进行模型误差预测：

设t_k时刻系统的状态估计值为

则预测输出为：

${\hat{y}}_k=H{\hat{x}}_k$

设t_k+1时刻获得的量测值为y_k+1，则可估计出[t_k，t_k+1]区间内的模型误差

为：

${\hat{d}}_k{\left\{{\left[\mathrm{\Λ}\left(\mathrm{\Δt}\right)U\left({\hat{x}}_k\right)\right]}^T{R}^{-1}\right[\mathrm{\Λ}\left(\mathrm{\Δt}\right)U\left({\hat{x}}_k\right)]+w\}}^{-1}{\left[\mathrm{\Λ}\left(\mathrm{\Δt}\right)U\left({\hat{x}}_k\right)\right]}^T{R}^{-1}[{\hat{y}}_k+Z({\hat{x}}_k,\mathrm{\Δt})-y_{k+2}]$

其中

为m维列向量，其各分量表达式为：

$Z_i(\hat{x}\left(t\right),\mathrm{\Δt})=\underset{k=1}{\overset{p_i}{\mathrm{\Σ}}}\frac{{\mathrm{\Δt}}^k}{k!}{L}_f^k\left(h_i\right)i=i,2,...m$

p_i为表示d(t)中任何分量出现在h_i微分中的最低阶数，称为第i个输出对应的相对阶；

Λ(Δt)∈R^m×m为对角矩阵，其对角元素为：

${\mathrm{\λ}}_{\mathrm{ii}}={\mathrm{\Δt}}^{p_i}/p_i!$ i＝i，2，…m

称为灵敏度矩阵，表示为：

$U\left(\hat{x}\left(t\right)\right)=\left[\begin{array}{ccc}{L}_{g_1}{L}_f^{p_1-1}\left(h_1\right)& ...& L_{g_q}{L}_f^{p_1-1}\left(h_1\right)\\ L_{g_1}{L}_f^{p_2-1}\left(h_2\right)& ...& L_{g_q}{L}_f^{p_2-1}\left(h_2\right)\\ .& .& .\\ .& .& .\\ .& .& .\\ L_{g_1}{L}_f^{p_m-1}\left(h_m\right)& ...& L_{g_q}{L}_f^{p_m-1}\left(h_m\right)\end{array}\right]$

其中为标量函数h_i关于向量场f(x(t)，t)的k阶李导数，且有：

$\left\{\begin{array}{cc}{L}_f^0\left(h_i\right)=h_i& k=0\\ L_f^k\left(h_i\right)=\frac{{\∂L}_f^{k-1}\left(h_i\right)}{\∂x}f(x\left(t\right),t)& k\≥1\end{array}\right.$

其中

为标量函数

关于向量场g_j(x(t)，t)的一阶李导数，其表达式为：

$L_{g_j}=L_f^k\left(h_i\right)=\frac{{\∂L}_f^k\left(h_i\right)}{\∂x}{g}_j(\hat{x}\left(t\right),t)$

W为模型误差加权矩阵；

(2)利用估计出模型误差来修正求容积卡尔曼滤波器的状态一步预测值，则可得求容积卡尔曼滤波器的时间更新为：

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

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

$X_{i,k|k-1}^{*}=F(X_{i,k-1|k-1},{\hat{d}}_k)$

${\hat{x}}_{x|k-1}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{X}_{i,k|k-1}^{*}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}F(X_{i,k-1|k-1},{\hat{d}}_k)$

$P_{k|k-1}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{X}_{i,k|k-1}^{*}{X}_{i,k|k-1}^T-{\hat{x}}_{k|k-1}{\hat{x}}_{k|k-1}^T+Q_{k-1}$

(3)利用t_k+1的量测值y_k+1对滤波器进行量测更新：

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

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

Y_i，k|k-1＝H(X_i，k|k-1)

${\hat{y}}_{k|k-1}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{Y}_{i,k|k-1}$

$P_{\mathrm{zz},k|k-1}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{Y}_{i,k|k-1}{Y}_{i,k|k-1}^T-{\hat{y}}_{k|k-1}{\hat{y}}_{k|k-1}^T{R}_k$

$P_{\mathrm{xz},k|k-1}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{X}_{i,k|k-1}{Y}_{i,k|k-1}^T-{\hat{x}}_{x|k-1}{\hat{y}}_{k|k-1}^T$

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

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

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

(4)已知t_k+1时刻系统的状态估计值

利用上述方法计算出[t_k+1，t_k+2]区间内的模型误差如此不断循环，实时修正和估计系统状态。

3、所述的载体先做匀加速直线运动，然后做匀速直线运动为在完成粗对准以后，载体首先做50s匀加速直线运动，加速度为0.5m/s²，然后再做250s匀速直线运动。

本发明的优势在于：(1)非线性预测滤波与求容积卡尔曼滤波相结合的非线性滤波器可以有效克服求容积卡尔曼滤波器对系统模型误差比较敏感的不足，从而提高了大方位失准角条件下捷联惯导系统的对准精度；(2)该滤波器把惯性器件的量测误差作为模型误差来处理，降低了系统维数，从而减少了滤波器的计算量。(3)当海况不佳，载体出现大幅晃动时，动基座初始对准对准精度高，收敛速度快。

附图说明

图1为捷联惯导系统动基座初始对准原理方框图；

图2为利用Matlab仿真得到的东向失准角估计误差曲线图；

图3为利用Matlab仿真得到的北向失准角估计误差曲线图；

图4为利用Matlab仿真得到的天向失准角估计误差曲线图。

具体实施方式

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

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

(1)在系泊状态下，采集陀螺仪及加速度的输出数据；

(2)采用解析法完成捷联惯导系统的粗对准，初步确定载体的姿态矩阵；

(3)建立大方位失准角条件下捷联惯导系统初始对准的非线性误差模型；

1)建立捷联惯导系统初始对准非线性误差方程

使用一阶非线性随机微分方程来描述捷联惯导系统非线性误差方程如下：

$\stackrel{\·}{x}\left(t\right)=f(x\left(t\right),t)+G_d(x\left(t\right),t)d\left(t\right)+w\left(t\right)$

式中，x(t)为t时刻系统的状态变量；f(x(t)，t)为模型向量；G_d(x(t)，t)为模型误差扰动矩阵；d(t)为系统模型误差向量；w(t)为系统的噪声向量；

系统的状态向量为：

x＝[δv_x δv_y φ_x φ_y φ_z]^T

系统模型误差向量为：

$d\left(t\right){\left[\begin{array}{ccccc}{\▿}_x& {\▿}_y& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z\end{array}\right]}^T$

系统的白噪声向量为：

$w\left(t\right)={\left[\begin{array}{ccccc}{w}_{{\▿}_x}& w_{{\▿}_y}& w_{{\mathrm{\ϵ}}_x}& w_{{\mathrm{\ϵ}}_y}& w_{{\mathrm{\ϵ}}_z}\end{array}\right]}^T$

其中，δv_x δv_y分别为系统东向和北向的速度误差；φ_x φ_y φ_z分别为系统东向、北向、天向的姿态误差；

分别为x、y轴加速度计的零偏；ε_x ε_yε_z分别为x、y、z轴陀螺仪的常值漂移；

分别为x、y轴加速度计的白噪声误差；

分别为x、y、z轴陀螺仪的白噪声误差；

模型向量为：

f(x(t)，t)＝A(t)·x+p(x，t)

式中A(t)_5×5为系统线性部分的系数矩阵；p(x，t)为非线性部分：

A(t)_5×5中的非零项元素为：

其中 $\left[\begin{array}{c}{f}_x\\ f_y\\ f_z\end{array}\right]=C_b^{n^{\′}}{\hat{f}}^b;$

其中w_ie为地球自转角速度；为载体的纬度；R为地球半径；v_x为载体的东向速度；v_y为载体的北向速度；

为计算的捷联矩阵；为加速度计的输出。

模型误差扰动矩阵：

$G_d(x\left(t\right),t)={\left[\begin{array}{cc}{C}_{b2\×2}^n& 0_{2\×3}\\ 0_{3\×2}& -C_b^{n^{\′}}\end{array}\right]}_{5\×5}$

其中 $C_{b2\×2}^n=\left[\begin{array}{cc}{C}_b^n\left(\mathrm{1,1}\right)& C_b^n\left(\mathrm{1,2}\right)\\ C_b^n\left(\mathrm{2,1}\right)& C_b^n\left(\mathrm{2,2}\right)\end{array}\right]$

2)建立量测方程：

取捷联惯导系统解算的速度与多普勒计程仪量测的速度之差作为量测量，则使用一阶线性微分方程来描述捷联惯导系统的量测方程如下：

y(t)＝h(t)x(t)+v(t)

式中，y(t)表示t时刻的量测向量；h(t)表示系统的量测矩阵；v(t)为系统的量测噪声；

系统量测矩阵为：

h(t)＝[I_2×2 O_2×3]

(4)建立非线性预测滤波与求容积卡尔曼滤波相结合的非线性滤波器；

设采样时间为Δt，利用四阶龙格库塔法对步骤(3)所建立的非线性误差模型和量测方程进行离散化处理，可得：

$\left\{\begin{array}{c}{x}_{k+1}=F(x_k,d_k)+w_k\\ y_{k+1}=H_{k+1}{x}_{k+1}+v_{k+1}\end{array}\right.$

则非线性预测滤波与求容积卡尔曼滤波相结合的非线性滤波器包括以下几个部分：

1)利用非线性预测滤波器进行模型误差预测

设t_k时刻系统的状态估计值为

则预测输出为：

${\hat{y}}_k=H{\hat{x}}_k$

设t_k+1时刻获得的量测值为y_k+1，则可估计出[t_k+1，t_k+1]区间内的模型误差

为：

${\hat{d}}_k{\left\{{\left[\mathrm{\Λ}\left(\mathrm{\Δt}\right)U\left({\hat{x}}_k\right)\right]}^T{R}^{-1}\right[\mathrm{\Λ}\left(\mathrm{\Δt}\right)U\left({\hat{x}}_k\right)]+w\}}^{-1}{\left[\mathrm{\Λ}\left(\mathrm{\Δt}\right)U\left({\hat{x}}_k\right)\right]}^T{R}^{-1}[{\hat{y}}_k+Z({\hat{x}}_k,\mathrm{\Δt})-y_{k+2}]$

其中

为m维列向量，其各分量表达式为：

$Z_i(\hat{x}\left(t\right),\mathrm{\Δt})=\underset{k=1}{\overset{p_i}{\mathrm{\Σ}}}\frac{{\mathrm{\Δt}}^k}{k!}{L}_f^k\left(h_i\right)i=i,2,...m$

p_i为表示d(t)中任何分量出现在h_i微分中的最低阶数，称为第i个输出对应的相对阶。

Λ(Δt)∈R^m×m为对角矩阵，其对角元素为：

${\mathrm{\λ}}_{\mathrm{ii}}={\mathrm{\Δ}}^{t^{p_i}}/p_i!$ i＝i，2，…m

称为灵敏度矩阵，表示为：

$Y\left(\hat{x}\left(t\right)\right)=\left[\begin{array}{ccc}{L}_{g_1}{L}_f^{p_1-1}\left(h_1\right)& ...& L_{g_q}{L}_f^{p_1-1}\left(h_1\right)\\ L_{g_1}{L}_f^{p_2-1}\left(h_2\right)& ...& L_{g_q}{L}_f^{p_2-1}\left(h_2\right)\\ .& .& .\\ .& .& .\\ .& .& .\\ L_{g_1}{L}_f^{p_m-1}\left(h_m\right)& ...& L_{g_q}{L}_f^{p_m-1}\left(h_m\right)\end{array}\right]$

其中

为标量函数h_i关于向量场f(x(t)，t)的k阶李导数，且有：

$\left\{\begin{array}{cc}{L}_f^0\left(h_i\right)=h_i& k=0\\ L_f^k\left(h_i\right)=\frac{{\∂L}_f^{k-1}\left(h_i\right)}{\∂x}f(x\left(t\right),t)& k\≥1\end{array}\right.$

其中

为标量函数

关于向量场g_j(x(t)，t)的一阶李导数，其表达式为：

$L_{g_j}=L_f^k\left(h_i\right)=\frac{{\∂L}_f^k\left(h_i\right)}{\∂x}{g}_j(\hat{x}\left(t\right),t)$

W为模型误差加权矩阵；但值得注意的是：W的取值对估计效果的影响很大，当模型误差较大时，W的值取小一些，当模型误差较小时，W的值应取大一些。

2)利用估计出模型误差

来修正求容积卡尔曼滤波器的状态一步预测值，则可得求容积卡尔曼滤波器的时间更新为：

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

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

$X_{i,k|k-1}^{*}=F(X_{i,k-1|k-1},{\hat{d}}_k)$

${\hat{x}}_{x|k-1}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{X}_{i,k|k-1}^{*}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}F(X_{i,k-1|k-1},{\hat{d}}_k)$

$P_{k|k-1}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{X}_{i,k|k-1}^{*}{X}_{i,k|k-1}^T-{\hat{x}}_{k|k-1}{\hat{x}}_{k|k-1}^T+Q_{k-1}$

3)利用t_k+1的量测值y_k+1对滤波器进行量测更新：

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

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

Y_i，k|k-1＝H(X_i，k|k-1)

${\hat{y}}_{k|k-1}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{Y}_{i,k|k-1}$

$P_{\mathrm{zz},k|k-1}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{Y}_{i,k|k-1}{Y}_{i,k|k-1}^T-{\hat{y}}_{k|k-1}{\hat{y}}_{k|k-1}^{^}+R_k$

$P_{\mathrm{xz},k|k-1}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{X}_{i,k|k-1}{Y}_{i,k|k-1}^T-{\hat{x}}_{x|k-1}{\hat{y}}_{k|k-1}^T$

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

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

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

4)已知t_k+1时刻系统的状态估计值利用上述方法计算出[t_k+1，t_k+2]区间内的模型误差

如此不断循环，实时修正和估计系统状态。

(5)0-50s载体做匀加速直线运动，加速度为0.5m/s²，50-300s做匀速直线运动。在进行上述步骤(5)的同时进行下列步骤：

(6)导航计算机进行捷联解算，同时多普勒计程仪测量载体的运动速度；

(7)取捷联解算的速度与多普勒计程仪量测的速度作为量测量，利用步骤(4)建立的非线性滤波器对三个平台误差角进行估计。

(8)利用估计出的平台误差角对此时的姿态矩阵进行修正，从而完成初始对准。

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

MATLAB仿真实验

在以下的仿真条件下，对该方法进行仿真实验：

船体作三轴摇摆运动。载体以正弦规律绕航向角、纵摇角和横摇角摇摆，其数学模型为：

Ψ＝Ψ_msin(w_Ψt+φ_Ψ)+k

θ＝θ_msin(w_θt+φ_θ)

γ＝γ_msin(w_γt+φ_r)

其中Ψ，θ，γ分别表示绕航向角、纵摇角和横摇角的摇摆角度变量；Ψ_m，θ_m，γ_m分别表示相应的摇摆角度幅值；w_Ψ，w_θ，w_γ分别表示相应的摇摆角频率；φ_Ψ，φ_θ，φ_r分别表示相应的初相位；而ω_i＝2π/T_i，i＝Ψ，θ，γ，T_i表示相应的摇摆周期；k为真航迹。仿真时取Ψ_m＝6°，θ_m＝3°，γ_m＝3°，T_Ψ＝9s，T_θ＝6s，T_γ＝8s，k＝0。

同时0 50s舰船做匀加速直线运动，加速度为0.5m/s²，50 300s做匀速直线运动。

载体初始位置：北纬45.7796°，东经126.6705°；

初始姿态误差角：横摇误差角角0.5°，纵摇误差角0.5°，方位误差角10°；

赤道半径：R_e＝6378393.0m；

椭球度：e＝3.367e-3；

由万有引力可得的地球表面重力加速度：g₀＝9.78049；

地球自转角速度(弧度/秒)：7.2921158e-5；

陀螺仪常值漂移：0.02度/小时；

陀螺仪白噪声误差：0.01度/小时；

加速度计零偏：10^-4g₀；

加速度计白噪声误差：5×10^-5g₀：

常数：π＝3.1415926；

利用发明所述方法得到东向失准角估计误差曲线、北向失准角估计误差曲线和天向失准角估计误差曲线分别如图2、图3和图4所示。结果表明在大方位失准角条件下，动基座初始对准采用本发明方法可以获得较高的对准精度和对准速度。

Claims (5)

1.基于非线性预测滤波与求容积卡尔曼滤波相结合的动基座初始对准方法，其特征是：

(1)在系泊状态下，采集陀螺仪及加速度的输出数据；

(2)采用解析法完成捷联惯导系统的粗对准，初步确定载体的姿态矩阵；

(3)建立大方位失准角条件下捷联惯导系统初始对准的非线性误差模型；

(4)建立非线性预测滤波与求容积卡尔曼滤波相结合的非线性滤波器；

(5)载体先做匀加速直线运动，然后做匀速直线运动；

(6)进行捷联解算，同时测量载体的运动速度；

(7)取捷联解算的速度与测量的载体的运动速度作为量测量，利用步骤(4)建立的非线性滤波器对三个平台误差角进行估计；

(8)利用估计出的平台误差角对此时的姿态矩阵进行修正，从而完成初始对准。

2.根据权利要求1所述的基于非线性预测滤波与求容积卡尔曼滤波相结合的动基座初始对准方法，其特征是：所述的建立大方位失准角条件下捷联惯导系统初始对准的非线性误差模型的步骤为：

(1)建立捷联惯导系统初始对准非线性误差方程

使用一阶非线性随机微分方程来描述捷联惯导系统非线性误差方程如下：

式中，x(t)为t时刻系统的状态变量，f(x(t)，t)为模型向量，G_d(x(t)，t)为模型误差扰动矩阵，d(t)为系统模型误差向量，w(t)为系统的噪声向量；

系统的状态向量为：

x＝[δv_x δv_y φ_x φ_y φ_z]^T

系统模型误差向量为：

$d\left(t\right)={\left[\begin{array}{ccccc}{\▿}_x& {\▿}_y& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z\end{array}\right]}^T$

系统的白噪声向量为：

$w\left(t\right)={\left[\begin{array}{ccccc}{w}_{{\▿}_x}& w_{{\▿}_y}& w_{{\mathrm{\ϵ}}_x}& w_{{\mathrm{\ϵ}}_y}& w_{{\mathrm{\ϵ}}_z}\end{array}\right]}^T$

其中，δv_x δv_y分别为系统东向和北向的速度误差，φ_x φ_y φ_z分别为系统东向、北向、天向的姿态误差，

分别为x、y轴加速度计的零偏，ε_x ε_yε_z分别为x、y、z轴陀螺仪的常值漂移，

分别为x、y轴加速度计的白噪声误差，

分别为x、y、z轴陀螺仪的白噪声误差；

模型向量为：

f(x(t)，t)＝A(t)·x+p(x，t)

式中A(t)_5×5为系统线性部分的系数矩阵；p(x，t)为非线性部分，A(t)_5×5中的非零项元素为：

其中 $\left[\begin{array}{c}{f}_x\\ f_y\\ f_z\end{array}\right]=C_b^{n^{\′}}{\hat{f}}^b;$

其中w_ie为地球自转角速度，

为载体的纬度，R为地球半径，v_x为载体的东向速度，v_y为载体的北向速度，

为计算的捷联矩阵，

为加速度计的输出；

模型误差扰动矩阵：

$G_d(x\left(t\right),t)={\left[\begin{array}{cc}{C}_{b2\×2}^n& 0_{2\×3}\\ 0_{3\×2}& {-C}_b^{n^{\′}}\end{array}\right]}_{5\×5},$

其中 $C_{b2\×2}^n=\left[\begin{array}{cc}{C}_b^n\left(\mathrm{1,1}\right)& C_b^n\left(\mathrm{1,2}\right)\\ C_b^n\left(\mathrm{2,1}\right)& C_b^n\left(\mathrm{2,2}\right)\end{array}\right];$

(2)建立量测方程：

取捷联惯导系统解算的速度与量测的载体的速度之差作为量测量，则使用一阶线性微分方程来描述捷联惯导系统的量测方程如下：

y(t)＝h(t)x(t)+v(t)

式中y(t)表示t时刻的量测向量，h(t)表示系统的量测矩阵，v(t)为系统的量测噪声；

系统量测矩阵为：

h(t)＝[I_2×2 0_2×3]。

3.根据权利要求1或2所述的基于非线性预测滤波与求容积卡尔曼滤波相结合的动基座初始对准方法，其特征是：所述的建立非线性预测滤波与求容积卡尔曼滤波相结合的非线性滤波器为：

设采样时间为Δt，利用四阶龙格库塔法对所建立的非线性误差模型和量测方程进行离散化处理，可得：

$\left\{\begin{array}{c}{x}_{k+1}=F(x_k,d_k)+w_k\\ y_{k+1}=H_{k+1}{x}_{k+1}+v_{k+1}\end{array}\right.$

则非线性预测滤波与求容积卡尔曼滤波相结合的非线性滤波器包括以下几个部分：

(1)利用非线性预测滤波器进行模型误差预测：

设t_k时刻系统的状态估计值为则预测输出为：

${\hat{y}}_k=H{\hat{x}}_k$

设t_k+1时刻获得的量测值为y_k+1，则可估计出[t_k，t_k+1]区间内的模型误差

为：

${\hat{d}}_k={\left\{{\left[\mathrm{\Λ}\left(\mathrm{\Δt}\right)U\left({\hat{x}}_k\right)\right]}^T{R}^{-1}\right[\mathrm{\Λ}\left(\mathrm{\Δt}\right)U\left({\hat{x}}_k\right)]+W\}}^{-1}{\left[\mathrm{\Λ}\left(\mathrm{\Δt}\right)U\left({\hat{x}}_k\right)\right]}^T{R}^{-1}[{\hat{y}}_k+Z({\hat{x}}_k,\mathrm{\Δt})-y_{k+1}]$

其中

为m维列向量，其各分量表达式为：

$Z_i(\hat{x}\left(t\right),\mathrm{\Δt})=\underset{k=1}{\overset{p_i}{\mathrm{\Σ}}}\frac{{\mathrm{\Δt}}^k}{k!}{L}_f^k\left(h_i\right)$ i＝i，2，·m

p_i为表示d(t)中任何分量出现在h_i微分中的最低阶数，称为第i个输出对应的相对阶；

Λ(Δt)∈R^m×m为对角矩阵，其对角元素为：

${\mathrm{\λ}}_{\mathrm{ii}}={\mathrm{\Δt}}^{p_i}/p_i!$ i＝i，2，·m

称为灵敏度矩阵，表示为：

$U\left(\hat{x}\left(t\right)\right)=\left[\begin{array}{ccc}{L}_{g_1}{L}_f^{p_1-1}\left(h_1\right)& \·& L_{g_q}{L}_f^{p_1-1}\left(h_1\right)\\ L_{g_1}{L}_f^{p_2-1}\left(h_2\right)& \·& L_{g_q}{L}_f^{p_2-1}\left(h_2\right)\\ \·& \·& \·\\ L_{g_1}{L}_f^{p_m-1}\left(h_m\right)& \·& L_{g_q}{L}_f^{p_m-1}\left(h_m\right)\end{array}\right]$

其中

为标量函数h_i关于向量场f(x(t)，t)的k阶李导数，且有：

$\left\{\begin{array}{cc}{L}_f^0\left(h_i\right)=h_i& k=0\\ L_f^k\left(h_i\right)=\frac{{\∂L}_f^{k-1}\left(h_i\right)}{\∂x}f(x\left(t\right),t)& k\≥1\end{array}\right.$

其中

为标量函数

关于向量场g_j(x(t)，t)的一阶李导数，其表达式为：

$L_{g_j}{L}_f^k\left(h_i\right)=\frac{{\∂L}_f^k\left(h_i\right)}{\∂x}{g}_j(\hat{x}\left(t\right),t)$

W为模型误差加权矩阵；

(2)利用估计出模型误差来修正求容积卡尔曼滤波器的状态一步预测值，则可得求容积卡尔曼滤波器的时间更新为：

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

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

$X_{i,k|k-1}^{*}=F(X_{i,k-1|k-1},{\hat{d}}_k)$

${\hat{x}}_{x|k-1}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{X}_{i,k|k-1}^{*}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}F(X_{i,k-1|k-1},{\hat{d}}_k)$

$P_{k|k-1}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{X}_{i,k|k-1}^{*}{X}_{i,k|k-1}^{*T}-{\hat{x}}_{k|k-1}{\hat{x}}_{k|k-1}^T+Q_{k-1}$

(3)利用t_k+1的量测值y_k+1对滤波器进行量测更新：

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

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

Y_i，k|k-1＝H(X_i，k|k-1)

${\hat{y}}_{k|k-1}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{Y}_{i,k|k-1}$

$P_{\mathrm{zz},k|k-1}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{Y}_{i,k|k-1}{Y}_{i,k|k-1}^T-{\hat{y}}_{k|k-1}{\hat{y}}_{k|k-1}^T+R_k$

$P_{\mathrm{xz},k|k-1}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{X}_{i,k|k-1}{Y}_{i,k|k-1}^T-{\hat{x}}_{x|k-1}{\hat{y}}_{k|k-1}^T$

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

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

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

(4)已知t_k+1时刻系统的状态估计值

利用上述方法计算出[t_k+1，t_k+2]区间内的模型误差

如此不断循环，实时修正和估计系统状态。

4.根据权利要求1或2所述的基于非线性预测滤波与求容积卡尔曼滤波相结合的动基座初始对准方法，其特征是：所述的载体先做匀加速直线运动，然后做匀速直线运动为在完成粗对准以后，载体首先做50s匀加速直线运动，加速度为0.5m/s²，然后再做250s匀速直线运动。

5.根据权利要求3所述的基于非线性预测滤波与求容积卡尔曼滤波相结合的动基座初始对准方法，其特征是：所述的载体先做匀加速直线运动，然后做匀速直线运动为在完成粗对准以后，载体首先做50s匀加速直线运动，加速度为0.5m/s²，然后再做250s匀速直线运动。

Images