CN103776449A - 一种提高鲁棒性的动基座初始对准方法 - Google Patents

Abstract

本发明涉及一种提高鲁棒性的动基座初始对准方法，其技术特点是：将初始对准模型进行抽象；通过对比滤波算法中的新息协方差和计算的新息协方差得到模型噪声方差的变化情况，然后对模型噪声w进行自适应缩放；根据观测值和滤波值的残差，自适应地判别每一次滤波后自适应矩阵迹的门限,进而修正状态估计。本发明设计合理，其从模型噪声自适应和观测干扰自适应两个方面同时对UKF算法进行改进，一方面通过监视UKF中预测方差，使用缩放因子达到平抑模型噪声的目的；另一方面通过监测基于新息特性的自适应矩阵的迹并进行实时修改，能够取得更加精确的滤波结果，达到抑制观测干扰的目的，具有较强的鲁棒性，能够快速对其进行平抑。

Description

一种提高鲁棒性的动基座初始对准方法

技术领域

本发明属于捷联惯导***技术领域，尤其是一种提高鲁棒性的动基座初始对准方法。

背景技术

初始对准是捷联式惯性导航***(捷联惯导***，Strapdown InertialNavigation System,SINS)的关键技术之一，它直接影响捷联惯导***的导航性能，即导航精度和反应时间。按照惯导***安装基座的运动状态可分为静基座对准和动基座对准。在静基座对准时，通常初始位置已知且初速度为零，仅需要研究如何使捷联惯导平台坐标系按照导航坐标系定向。而动基座对准的对准环境非常复杂，涉及到环境建模以及测量等方面的技术，因此，动基座对准是基座对准的一项技术难题。

在进行捷联惯导***动基座初始对准时，最常用的方法就是采用卡尔曼滤波(Kalman Filter,KF)技术，均差滤波器与遗传算法在初始对准中有着尚佳地表现。同时，对于非线性***，扩展卡尔曼滤波(Extended Kalman Filter,EKF)也是比较常用的。相对来说，无迹卡尔曼滤波(Unscented Kalman Filter,UKF)有着最为广泛的实际应用,但是，模型噪声和观测干扰会对估值产生影响，进而造成对准结果的失准。为了改进初始对准的精度和速度，研究热点主要集中在两个方面：第一方面是改进初始对准的模型，第二方面是改进UKF算法。无迹粒子滤波(Unscented Particle Filter,UPF)以其处理非线性非高斯问题的特有优势，在初始对准中亦有所应用，但是，UPF运算量过大，在工程实践中难以实现，为克服此缺点而采用与KF相结合的方式应用于初始对准，提高了精度，但耗时仍较长。

惯性导航***的动基座对准是一个非线性***，在现有技术中，常常假设姿态误差角是小量，以忽略其高阶项，得到常用的线性误差模型。在导航数据出现较大误差时使用线性模型模拟便不能对实际情况进行有效仿真。同时，在武器使用的复杂环境中，动基座初始对准***还要经受非高斯和非平稳的噪声和干扰。这些都是上述KF和普通UKF难以胜任的。

传统KF是线性***中的最佳滤波方法，在针对非线性***时，拟合概率分布要比拟合非线性函数本身更容易。UKF以无迹变换(Unscented Transformation,UT)为核心，来实现概率分布上对非线性函数的近似，以卡尔曼滤波器为框架，在非线性***估值中有着良好的性能。不同的采样策略对应了不同的UT变换方法，有对称采样、单形采样等，最为常见的是采用尺度可变对称采样策略的UKF算法，如图1所示。现对上述方法分析如下：

（1）UKF滤波器的权重因子及相关参数如下：

$w_0^{\left(m\right)}=\mathrm{\λ}/L+\mathrm{\λ},i=0---\left(1\right)$

$w_0^{\left(c\right)}=\mathrm{\λ}/L+\mathrm{\λ}+(1-a^2+\mathrm{\β}),i=0---\left(2\right)$

$w_i^{\left(m\right)}=w_i^{\left(c\right)}=\mathrm{\λ}/2(L+\mathrm{\λ}),i=1,...,2L---\left(3\right)$

$\begin{array}{c}\mathrm{\γ}=\sqrt{L+\mathrm{\λ}},\mathrm{\λ}=a^2(L+\mathrm{\κ})-L^2,\\ \mathrm{\κ}=0,a=1\end{array}---\left(4\right)$

其中，

和分别为计算均值和方差所用加权；γ,λ均是尺度参数；常量a为采样点的分布范围，通常取10-4<a<1；κ是二阶尺度参数，

常取为0；β用于体现变量先验分布信息，高斯分布下常取β=2。

(2)初始化及构造sigma点集：

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

${\mathrm{\&chi;}}_{k-1}=E\left[\begin{array}{ccc}{\hat{x}}_{k-1}& {\hat{x}}_{k-1}+\mathrm{\&gamma;}\sqrt{P_{k-1}}& {\hat{x}}_{k-1}\end{array},-\mathrm{\&gamma;}\sqrt{P_{k-1}}\right]---\left(6\right)$

其中，是k-1时刻的状态最优估计值，P_k-1是k-1时刻的方差。

(3)状态更新：

χ_k|k-1＝f(χ_k-1)     (7)

${\hat{x}}_{k-1}^{-}=\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{{W_i}^m\mathrm{\&chi;}}_{i,k|k-1}---\left(8\right)$

$P_k^{-}=\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{W_i}^m({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{x}}_k^{-}){({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{x}}_k^{-})}^T+Q---\left(9\right)$

y_k|k-1＝h(χ_k|k-1)     (10)

${\hat{y}}_k=\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{W_i}^m{y}_{i,k|k-1}---\left(11\right)$

(4)观测更新：

$P_{x_k{y}_k}=\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{W}_i({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{x}}_k^{-}){(y_{i,k|k-1}-{\hat{y}}_k^{-})}^T---\left(12\right)$

$P_{y_k{y}_k}=\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{W}_i(y_{i,k|k-1}-{\hat{y}}^{-}){(y_{i,k|k-1}-{\hat{y}}_k^{-})}^T+P_V---\left(13\right)$

$K_k=P_{x_k{y}_k}{P}_{y_k{y}_k}^{-1}---\left(14\right)$

${\hat{x}}_k={\hat{x}}_k^{-}+K_k(y_k-{\hat{y}}^{-})---\left(15\right)$

$P_k=P_k^{-}-K_k{P}_{y_k{y}_k}{K}_k^T---\left(16\right)$

通过以上分析可以看出现有的UKF算法存在的问题是：增益矩阵无法根据滤波效果和***残差及时地进行快速调控，使得滤波器的估值无法快速跟踪***状态。

发明内容

本发明的目的在于克服现有技术的不足，提供一种设计合理、精度高且性能稳定的提高鲁棒性的动基座初始对准方法。

本发明解决现有的技术问题是采取以下技术方案实现的：

一种提高鲁棒性的动基座初始对准方法，包括以下步骤：

步骤1、将初始对准模型抽象为如下***：

$\left\{\begin{array}{c}{x}_{k+1}=f\left(x_k\right)+w_k\\ y_{k+1}={\mathrm{Hx}}_{k+1}+E_k+v_{k+1}\end{array}\right.$

式中：f(·)代表非线性状态转移；H代表观测矩阵；w和v分别代表模型噪声和观测噪声；x∈Rⁿ代表状态量；y∈R^m代表观测输出；R代表v的方差；E_k代表未知干扰的分布函数；

步骤2、通过对比滤波算法中的新息协方差

和计算的新息协方差

得到模型噪声方差的变化情况，然后对模型噪声w进行自适应缩放；

步骤3、根据观测值和滤波值的残差，自适应地判别每一次滤波后自适应矩阵迹的门限,进而修正状态估计。

而且，所述步骤2滤波算法中的新息协方差为：

${\hat{P}}_k^{~}=\frac{1}{m}\underset{j=1}{\overset{m}{\mathrm{\&Sigma;}}}({\hat{x}}_k-{\hat{x}}_k^{-}){({\hat{x}}_k-{\hat{x}}_k^{-})}^T$

所述计算的新息协方差为：

$P_k^{-}=\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{W_i}^{\left(m\right)}({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{x}}_k^{-}){({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{x}}_k^{-})}^T+P_w$

其中，

是状态矢量，分别为计算均值和方差所用加权，χ是UKF算法中的sigma点，P_w是噪声方差。

而且，所述步骤2对模型噪声w进行自适应缩放采用如下公式进行处理：

${\hat{Q}}_k=Q_{k-1}\sqrt{{\mathrm{\&beta;}}_k}$

上式中，Q代表观测噪声的方差；β_k表示缩放因子，其计算公式如下：

${\mathrm{\&beta;}}_k=\frac{\mathrm{tr}\left(H_k{\tilde{P}}_k^{-}{H}_k^T\right)}{\mathrm{tr}\left(H_k{\hat{P}}_k^{-}{H}_k^T\right)}=\frac{\mathrm{tr}(\frac{1}{m}\underset{j=1}{\overset{m}{\mathrm{\&Sigma;}}}{d}_{k-j}{d}_{k=j}^T-R_k)}{\mathrm{tr}\left(H_k{\hat{P}}_k^{-}{H}_k^T\right)}$

上式中，tr()为取矩阵的迹，H为观测矩阵，

为新息方差，d_k表示观测方程中产生的新息，其计算公式如下：

$d_k=(H_k{x}_k+v_k)-H_k{\hat{x}}_x^{-}=H_k(x_k-{\hat{x}}_k^{-})+v_k$

H为观测矩阵，

是状态矢量，v是观测噪声的期望。

而且，所述步骤3中观测值和滤波值的残差为：

$e\left(k\right)=y_k-{\tilde{y}}_k$

式中：y_k表示观测值；

表示滤波值；

所述的自适应矩阵S(k)为：

$S\left(k\right)=\frac{1}{\mathrm{\&mu;}}\underset{j=k-\mathrm{\&mu;}+1}{\overset{k}{\mathrm{\&Sigma;}}}e\left(j\right)e{\left(j\right)}^T-P_{\mathrm{yy}}(k+1|k)-R\left(k\right)$

式中：P_yy为UKF算法中观测更新的协方差；R是观测噪声方差，u是开窗大小

所述的门限m为：

$m={\mathrm{\&chi;}}_{\mathrm{\&alpha;}}^2(k-1)P_{y_k{y}_k}$

所述的修正状态估计为：

$\hat{x}(k+1)=\hat{x}(k+1|k)+K\left(k\right)g\left(k\right)e\left(k\right)$

是自由度为k-1的卡方分布值，K是卡尔曼滤波增益，g为校正状态的估计函数。

本发明的优点和积极效果是：

本发明从模型噪声自适应和观测干扰自适应两个方面同时对UKF算法进行改进，一方面通过监视UKF中预测方差，使用缩放因子达到平抑模型噪声的目的；另一方面通过监测基于新息特性的自适应矩阵的迹并进行实时修改，能够取得更加精确的滤波结果，达到抑制观测干扰的目的，具有较强的鲁棒性，能够快速对其进行平抑。

附图说明

图1是现有UKF算法的流程示意图；

图2是传统卡尔曼滤波的失准角估计误差结果示意图；

图3是扩展卡尔曼滤波的失准角估计误差结果示意图；

图4是采用现有UKF算法的失准角估计误差结果示意图；

图5是本发明采用双重自适应UKF算法的失准角估计误差结果示意图。

具体实施方式

以下结合附图对本发明实施例做进一步详述。

一种提高鲁棒性的动基座初始对准方法从模型噪声自适应和观测干扰自适应两个方面同时对UKF进行改进，形成了双重自适应UKF算法，以取得更加精确的滤波结果，具体包括以下步骤：

步骤1、将初始对准模型抽象为如下***：

$\left\{\begin{array}{c}{x}_{k+1}=f\left(x_k\right)+w_k\\ y_{k+1}={\mathrm{Hx}}_{k+1}+E_k+v_{k+1}\end{array}\right.$

式中，f(·)代表非线性状态转移；H代表观测矩阵；w和v分别代表模型噪声和观测噪声；x∈Rⁿ代表状态量；y∈R^m代表观测输出；E_k代表未知干扰的分布函数；Q和R分别代表w和v的方差。

在UKF中，由于模型噪声和观测噪声影响了当前一次滤波过程中当前信息和前一步观测信息分别所占的权重，因此，两者对于UKF的滤波精度有着举足轻重的作用。而在实际应用中，观测噪声较易求得，而敏感部件在测量过程中的所受的干扰以其不可预知性而使滤波产生偏差。本方法假设观测噪声的统计特性是已知的。

步骤2、对模型噪声w进行自适应缩放。具体方法为：通过对比滤波算法中的新息协方差

和计算的新息协方差

(即一步预测方差

)得到模型噪声方差的变化情况判断模型噪声方差是否需要缩放。

在滤波算法中，滤波算法中的新息协方差

和计算的新息协方差

对于基于方差的预测产生至关重要的作用。其中，预测残差的序列即新息协方差的计算公式为：

${\hat{P}}_k^{~}=\frac{1}{m}\underset{j=1}{\overset{m}{\mathrm{\&Sigma;}}}({\hat{x}}_k-{\hat{x}}_k^{-}){({\hat{x}}_k-{\hat{x}}_k^{-})}^T$

计算的新息协方差的计算公式为：

$P_k^{-}=\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{W_i}^{\left(m\right)}({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{x}}_k^{-}){({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{x}}_k^{-})}^T+P_w$

通过对比

和

得到模型噪声方差的变化情况，从而判断模型噪声方差是否需要缩放。

而观测方程中产生的新息为：

$d_k=(H_k{x}_k+v_k)-H_k{\hat{x}}_x^{-}=H_k(x_k-{\hat{x}}_k^{-})+v_k---\left(17\right)$

在最优滤波的情况下，d_k是期望为0的高斯白噪声序列，上式两端开窗取方差得到：

$\frac{1}{m}\underset{j=1}{\overset{m}{\mathrm{\&Sigma;}}}{d}_{k-j}{d}_{k-j}^T=H_k{\hat{P}}_k^{~}{H}_k^T+R_k---\left(18\right)$

引入以下缩放因子：

${\mathrm{\&beta;}}_k=\frac{\mathrm{tr}\left(H_k{\tilde{P}}_k^{-}{H}_k^T\right)}{\mathrm{tr}\left(H_k{\hat{P}}_k^{-}{H}_k^T\right)}=\frac{\mathrm{tr}(\frac{1}{m}\underset{j=1}{\overset{m}{\mathrm{\&Sigma;}}}{d}_{k-j}{d}_{k=j}^T-R_k)}{\mathrm{tr}\left(H_k{\hat{P}}_k^{-}{H}_k^T\right)}---\left(19\right)$

式中tr()为取矩阵的迹。

因此，模型噪声方差的缩放可用如下公式处理，同时达到平滑的效果：

${\hat{Q}}_k=Q_{k-1}\sqrt{{\mathrm{\&beta;}}_k}---\left(20\right)$

本步骤中，Q和R分别代表w和v的方差。

步骤3、对观测干扰E_k存在观测干扰情况下的改进。

定义观测值和滤波值的残差为:

$e\left(k\right)=y_k-{\tilde{y}}_k,$

式中：y_k表示观测值；

表示滤波值。

当滤波结果无偏时，e(k)方差的开窗均值满足如下方程：

$\frac{1}{\mathrm{\&mu;}}\underset{j=k-\mathrm{\&mu;}+1}{\overset{k}{\mathrm{\&Sigma;}}}e\left(j\right)e{\left(j\right)}^T=P_{\mathrm{yy}}(k+1|k)+R\left(k\right)---\left(22\right)$

式中：μ表示滚动窗口的大小；P_yy表示观测估计方差；R表示观测噪声方差矩阵。

当估值***中出现观测干扰时，实际的误差将超过理论值，方程式(22)左右两边将不再相等，因此有必要加入自适应矩阵S(k)。定义自适应矩阵满足S(k)下列式子：

$\frac{1}{\mathrm{\&mu;}}\underset{j=k-\mathrm{\&mu;}+1}{\overset{k}{\mathrm{\&Sigma;}}}e\left(j\right)e{\left(j\right)}^T=P_{\mathrm{yy}}(k+1|k)+S\left(k\right)+R\left(k\right)---\left(23\right)$

上式中，P_yy为UKF算法中观测更新的协方差。

由此可得到自适应矩阵S(k)定义式如下：

$S\left(k\right)=\frac{1}{\mathrm{\&mu;}}\underset{j=k-\mathrm{\&mu;}+1}{\overset{k}{\mathrm{\&Sigma;}}}e\left(j\right)e{\left(j\right)}^T-P_{\mathrm{yy}}(k+1|k)-R\left(k\right)---\left(24\right)$

由于上式右边三个量均为高斯变量，所以S(k)的迹的统计特性服从χ²分布，通过检测S(k)的迹可以得知非正常值是否存在：

式中，χ²(m)为χ²()分布，m一般情况代表观测变量的个数，但是在大数据量且统计标准并不明确的情况下，上述方法误差较大，所以本方法在综合考虑残差方差这一因素上，提出一种新的判决门限，自适应地判别每一次滤波后自适应矩阵迹的门限，提高准确度。

设Z₁,Z₂,Z₃,……,Z_k是来自正态总体N(μ,σ²)的一个样本，其无偏方差为：

$s^2=\frac{1}{n-1}\underset{i=1}{\overset{k}{\mathrm{\&Sigma;}}}{(Z_i-\stackrel{\&OverBar;}{Z})}^2---\left(26\right)$

根据χ²分布的性质，有：

$\frac{(n-1)s^2}{{\mathrm{\&sigma;}}^2}~{\mathrm{\&chi;}}^2(n-1)---\left(27\right)$

其中，

为样本均值。另外，χ²分布上侧α分位数定义为：对于给定的正数α，0＜α＜1，满足条件

$P({\mathrm{\&chi;}}^2>{\mathrm{\&chi;}}_{\mathrm{\&alpha;}}^2\left(n\right))=\mathrm{\&alpha;}---\left(28\right)$ 的数

称为χ²(n)分布的上侧α分位数。是一个只与自由度n和概率α有关的函数，具体的

值可由χ²分布表查得。

依据χ²分布性质，将式(26)代入式(27)，得到：

$\frac{\underset{i=1}{\overset{k}{\mathrm{\&Sigma;}}}{(Z_i-\stackrel{\&OverBar;}{Z})}^2}{{\mathrm{\&sigma;}}^2}~{\mathrm{\&chi;}}^2(n-1)---\left(29\right)$

上式中，方差σ²即为式(13)中的

样本均值为零。当已知故障发生概率α时，设：

$Y=\underset{i=1}{\overset{k}{\mathrm{\&Sigma;}}}{(Z_i-\stackrel{\&OverBar;}{Z})}^2---\left(30\right)$

门限值为m，且关于Y值大于m的概率为α有P(Y＞m)＝α，则：

$\begin{array}{c}P(\frac{Y}{{\mathrm{\&sigma;}}^2}>\frac{m}{{\mathrm{\&sigma;}}^2})=\mathrm{\&alpha;}\\ P(\frac{\underset{i=1}{\overset{k}{\mathrm{\&Sigma;}}}{(Z_i-\stackrel{\&OverBar;}{Z})}^2}{{\mathrm{\&sigma;}}^2}>\frac{m}{{\mathrm{\&sigma;}}^2})=\mathrm{\&alpha;}\end{array}---\left(31\right)$

根据式(29)可知此时大于号左边

符合自由度为k-1的χ²分布，且根据χ²分布上侧α分位数定义式(28)可知：

$\frac{m}{{\mathrm{\&sigma;}}^2}={\mathrm{\&chi;}}_{\mathrm{\&alpha;}}^2(k-1)---\left(32\right)$

$m={\mathrm{\&chi;}}_{\mathrm{\&alpha;}}^2(k-1){\mathrm{\&sigma;}}^2---\left(33\right)$

从而得到门限值m大小为：

$m={\mathrm{\&chi;}}_{\mathrm{\&alpha;}}^2(k-1)P_{y_k{y}_k}---\left(34\right)$

本方法在综合考虑残差方差这一因素上，提出一种新的判决门限，自适应地判别每一次滤波后自适应矩阵迹的门限m，提高准确度。

m确定后定义用于校正状态估计的函数

进而修正状态估计为

$\hat{x}(k+1)=\hat{x}(k+1|k)+K\left(k\right)g\left(k\right)e\left(k\right)---\left(36\right)$

通过以上三个步骤实现对捷联式惯性导航***的动基座初始对准功能，提高了动基座初始对准的鲁棒性。

为了对本发明效果进行说明，下面采用数学仿真的方式对发明的初始对准方法进行了建模，并通过赋值实现了对真实场景的模拟。具体过程分以下四个步骤进行：

（1）仿真方案的选择

初始对准时，主惯导***一般具有更高一级的精度，并且提供导航计算需要的初始值。传递对准的匹配方法有两种，即计算参数匹配方案（包含速度匹配和姿态匹配）和测量参数匹方案（包含角速度匹配和比力匹配）。各种匹配方法在传递对准中的效果也不尽相同，速度匹配是一种较为成熟的方法，它在各个工程领域被广泛应用。速度匹配可以较快地估计失准角并具有相当的精度，而且在遇到弹性变形时所受到的影响很小。本发明使用传递对准匹配方案中研究较为成熟的“速度匹配”方案。

在“速度匹配”方案中，主惯导***使用平台惯导***，精度高，误差可以忽略，子惯导***为捷联式惯导***。传递对准时，失准角通过加速度使速度差产生变化，利用速度差作为观测量，通过卡尔曼滤波器估计出失准角，实现传递对准。

（2）仿真数学模型的建立

SINS误差模型是初始对准的基础。根据具体的初始对准条件、假设和约束，SINS误差模型就可以简化为对准误差模型。取初始对准的导航坐标系为东北天(ENU)当地地理坐标系。本发明传递对准数学模型由速度误差、平台失准角、陀螺仪漂移误差、加速度计误差等动态方程组成。

***状态矢量为

$X=[{\mathrm{\&phi;}}_E,{\mathrm{\&phi;}}_N,{\mathrm{\&phi;}}_U,{\mathrm{\&delta;v}}_E,{\mathrm{\&delta;v}}_N,{\mathrm{\&delta;v}}_U,{\mathrm{\&epsiv;}}_{\mathrm{bx}},{\mathrm{\&epsiv;}}_{\mathrm{by}},{\mathrm{\&epsiv;}}_{\mathrm{bz}},{\mathrm{\&epsiv;}}_{\mathrm{rx}},{\mathrm{\&epsiv;}}_{\mathrm{ry}},{\mathrm{\&epsiv;}}_{\mathrm{rz}},{\&dtri;}_x,{\&dtri;}_y,{\&dtri;}_z]$

其中：φ_E、φ_N、φ_U为东、北、天三个方向的平台失准角；δv_E、δv_N、δv_U为东、北、天三个方向的速度误差；ε_bx、ε_by、ε_bz为陀螺仪随机常数；ε_rx、ε_ry、ε_rz为陀螺仪一阶马尔可夫过程；为加速度计一阶马尔可夫过程。

已知捷联惯导***的误差模型：

${\stackrel{\&CenterDot;}{\mathrm{\&phi;}}}_E=-\frac{{\mathrm{\&delta;v}}_N}{R_M+h}+({\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{v_E}{R_N+h}\tan L){\mathrm{\&phi;}}_N-({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h}){\mathrm{\&phi;}}_N+{\mathrm{\&epsiv;}}_E---\left(37\right)$

${\stackrel{\&CenterDot;}{\mathrm{\&phi;}}}_N=-\frac{{\mathrm{\&delta;v}}_E}{R_N+h}-{\mathrm{\&omega;}}_{\mathrm{ie}}\sin \mathrm{L\&delta;L}-({\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{v_E}{R_N+h}\tan L){\mathrm{\&phi;}}_E-\frac{v_N}{R_N+h}{\mathrm{\&phi;}}_U+{\mathrm{\&epsiv;}}_N---\left(38\right)$

$\begin{array}{c}{\stackrel{\&CenterDot;}{\mathrm{\&phi;}}}_U=\frac{{\mathrm{\&delta;v}}_N}{R_N+h}\tan L+({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h}{\sec }^{2}L)\mathrm{\&delta;L}\\ +({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h}){\mathrm{\&phi;}}_E+\frac{v_N}{R_M+h}{\mathrm{\&phi;}}_N+{\mathrm{\&epsiv;}}_U\end{array}---\left(39\right)$

$\begin{array}{c}\mathrm{\&delta;}{\stackrel{\&CenterDot;}{v}}_E=f_N{\mathrm{\&phi;}}_U-f_U{\mathrm{\&phi;}}_U+(\frac{v_N}{R_M+h}\tan L-\frac{v_E}{R_M+h})\mathrm{\&delta;}{v}_E+(2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{v_E}{R_M+h}\tan L)\mathrm{\&delta;}{v}_N\\ +(2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L{v}_N+\frac{v_E{v}_N}{R_N+h}{\mathrm{see}}^{2}L+2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L{v}_U)\mathrm{\&delta;L}-(2{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h})\mathrm{\&delta;}{v}_U+{\&dtri;}_E\end{array}---\left(40\right)$

$\begin{array}{c}\mathrm{\&delta;}{\stackrel{\&CenterDot;}{v}}_N=f_U{\mathrm{\&phi;}}_E-f_E{\mathrm{\&phi;}}_U-(2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{v_E}{R_N+h}\tan L)\mathrm{\&delta;}{v}_E-\frac{v_U}{R_M+h}\mathrm{\&delta;}{v}_N\\ -\frac{v_N}{R_M+h}\mathrm{\&delta;}{v}_U-(2{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h}{\sec }^{2}L)v_E\mathrm{\&delta;L}+{\&dtri;}_N\end{array}---\left(41\right)$

$\begin{array}{c}\mathrm{\&delta;}{\stackrel{\&CenterDot;}{v}}_U=f_E{\mathrm{\&phi;}}_N-f_N{\mathrm{\&phi;}}_E+(2{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h})\mathrm{\&delta;}{v}_E+\\ 2\frac{v_N}{R_M+h}\mathrm{\&delta;}{v}_N-2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L{v}_E\mathrm{\&delta;L}+{\&dtri;}_N+\frac{2g}{R}\mathrm{\&delta;h}\end{array}---\left(42\right)$

其中：v_E、v_N、v_U为东、北、天三个方向的速度；ε_E、ε_N、ε_U为东、北、天三个方向陀螺仪噪声；R_M为地球子午圈半径；R_N为地球卯酉圈半径；L为当地纬度；ω_ie为地球自转角速度；h为当地海拔高度；f_E、f_N、f_U为东、北、天三个方向的比力输出；

为东、北、天三个方向的加速度计测误差。

以东向速度误差、北向速度误差、天向速度误差作为观测量建立***的观测方程：

$Z={[\mathrm{\&delta;}{v}_E,\mathrm{\&delta;}{v}_N,\mathrm{\&delta;}{v}_U]}^T=\begin{array}{}\end{array}\left[\begin{array}{cc}{I}_3& 0_{3*12}\end{array}\right]X^T+\left[\begin{array}{c}{E}_E\\ E_N\\ E_U\end{array}\right]+\left[\begin{array}{c}{\mathrm{\&eta;}}_E\\ {\mathrm{\&eta;}}_N\\ {\mathrm{\&eta;}}_U\end{array}\right]...\left(43\right)$

式中：Z是观测矢量；[E_E,E_N,E_U]^T为***观测干扰矢量，[η_E,η_N,η_U]^T为***观测噪声矢量。

（3）仿真模型中算法的优化

以上的误差模型可以转化并抽象为如下的***状态空间模型

$\left\{\begin{array}{c}{x}_{k+1}=f\left(x_k\right)+w_k\\ y_{k+1}={\mathrm{Hx}}_{k+1}+E_k+v_{k+1}\end{array}\right.$

式中：f(·)代表非线性状态转移；H代表观测矩阵；w和v分别代表模型噪声和观测噪声；x∈Rⁿ代表状态量；y∈R^m代表观测输出；E_k代表未知干扰的分布函数。Q和R分别代表w和v的方差。

其中状态矢量 $x=[{\mathrm{\&phi;}}_E,{\mathrm{\&phi;}}_N,{\mathrm{\&phi;}}_U,{\mathrm{\&delta;v}}_E,{\mathrm{\&delta;v}}_N,{\mathrm{\&delta;v}}_U,{\mathrm{\&epsiv;}}_{\mathrm{bx}},{\mathrm{\&epsiv;}}_{\mathrm{by}},{\mathrm{\&epsiv;}}_{\mathrm{bz}},{\mathrm{\&epsiv;}}_{\mathrm{rx}},{\mathrm{\&epsiv;}}_{\mathrm{ry}},{\mathrm{\&epsiv;}}_{\mathrm{rz}},{\&dtri;}_x,{\&dtri;}_y,{\&dtri;}_z{]}^T,$

观测矢量Z=[δv_E,δv_N,δv_U]^T。

a.引入以下缩放因子

${\mathrm{\&beta;}}_k=\frac{\mathrm{tr}\left(H_k{\tilde{P}}_k^{-}{H}_k^T\right)}{\mathrm{tr}\left(H_k{\hat{P}}_k^{-}{H}_k^T\right)}=\frac{\mathrm{tr}(\frac{1}{m}\underset{j=1}{\overset{m}{\mathrm{\&Sigma;}}}{d}_{k-j}{d}_{k=j}^T-R_k)}{\mathrm{tr}\left(H_k{\hat{P}}_k^{-}{H}_k^T\right)}$

所以，模型噪声方差的缩放可用如下公式处理，同时达到平滑的效果

${\hat{Q}}_k=Q_{k-1}\sqrt{{\mathrm{\&beta;}}_k}$

b.引入如下大小为m的门限值

$m={\mathrm{\&chi;}}_{\mathrm{\&alpha;}}^2(k-1)P_{y_k{y}_k}---\left(44\right)$

m确定后定义用于校正状态估计的函数

进而修正状态估计为：

$\hat{x}(k+1)=\hat{x}(k+1|k)+K\left(k\right)g\left(k\right)e\left(k\right)---\left(46\right)$

（4）仿真场景的模拟

由于航向角变化是最常见的情况，因此仿真中，采用圆形运动的机动方式，运动中，航向变化角速率设定为10°/s。同时，经实验验证，本发明方法用于其他的运动方式时，例如摇摆运动和S型运动，也取得了相似的结果。

仿真中的重点初始参数如下：仿真时间为60s；步长为0.1s；主惯导陀螺漂移为0.01°/h；主惯导加速度计零偏为10-5g；子惯导陀螺漂移为0.1°/h；子惯导加速度计零偏为5*10-5g。其中，主、子惯导的陀螺漂移和加速度计零偏均为一阶马尔可夫过程。

由于模型噪声和观测干扰都是在正常值附近的小范围内波动，因此本仿真中选择sin函数模拟模型噪声和观测干扰。

为了检测改进算法对模型噪声变化的自适应功能，在第0秒至第60秒之间，对模型噪声方差的变动做出如下假设

ΔQ＝|sin(0.1*t)|*I₁₅     (44)

为了检测改进算法对观测干扰的自适应功能，设定在第0秒至第60秒之间产生观测干扰，观测干扰函数如下所示

E_k＝sin(0.1*t)     (45)

在上述假定情况下，比较平台失准角估计误差在卡尔曼滤波、扩展卡尔曼滤波、UKF和本发明专利中应用的双重自适应UKF四种滤波方法，进行指标测试，经过测试，对初始对准中重要指标仿真结果如下：

图2给出了传统卡尔曼滤波对本文设定的模型噪声和观测干扰的滤波结果。可以看出，在仿真时间内，失准角误差不能收敛，且误差随时间增大。

图3给出了扩展卡尔曼滤波对模型噪声和观测干扰的滤波结果。失准角误差有收敛趋势，但收敛后，误差仍较大且波动剧烈。对准时间为5秒左右，误差范围为0.05至0.2度。

图4应用了针对非线性***的UKF对平台失准角进行估计。随滤波进行，失准角误差快速减小，并明显收敛，但仍有小范围波动。对准时间为3秒左右，误差范围为0.05至0.1度。

图5应用本发明专利中的双重自适应UKF，针对模型噪声和观测干扰进行滤波。应用本方法，失准角误差快速收敛并接近于零，且稳定在0.01度左右无明显波动，对准时间提高至2秒左右。

由上述四图对比可知，本发明专利中应用的双重自适应UKF算法对模型噪声和观测干扰有很好的滤波效果，既能适应模型噪声的变化，又能对观测干扰进行有效抑制，使失准角误差大幅度减小且接近于零，同时使其快速收敛，在对准时间上有微弱改善，因此，本发明的算法具有较好的鲁棒性。

需要强调的是，本发明所述的实施例是说明性的，而不是限定性的，因此本发明包括并不限于具体实施方式中所述的实施例，凡是由本领域技术人员根据本发明的技术方案得出的其他实施方式，同样属于本发明保护的范围。

Claims (4)

1.一种提高鲁棒性的动基座初始对准方法，其特征在于包括以下步骤：

步骤1、将初始对准模型抽象为如下***：

$\left\{\begin{array}{c}{x}_{k+1}=f\left(x_k\right)+w_k\\ y_{k+1}={\mathrm{Hx}}_{k+1}+E_k+v_{k+1}\end{array}\right.$

式中：f(·)代表非线性状态转移；H代表观测矩阵；w和v分别代表模型噪声和观测噪声；x∈Rⁿ代表状态量；y∈R^m代表观测输出；R代表v的方差；E_k代表未知干扰的分布函数；

步骤2、通过对比滤波算法中的新息协方差

和计算的新息协方差

得到模型噪声方差的变化情况，然后对模型噪声w进行自适应缩放；

步骤3、根据观测值和滤波值的残差，自适应地判别每一次滤波后自适应矩阵迹的门限,进而修正状态估计。

2.根据权利要求1所述的一种提高鲁棒性的动基座初始对准方法，其特征在于：所述步骤2滤波算法中的新息协方差为：

${\hat{P}}_k^{~}=\frac{1}{m}\underset{j=1}{\overset{m}{\mathrm{\&Sigma;}}}({\hat{x}}_k-{\hat{x}}_k^{-}){({\hat{x}}_k-{\hat{x}}_k^{-})}^T$

所述计算的新息协方差为：

$P_k^{-}=\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{W_i}^{\left(m\right)}({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{x}}_k^{-}){({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{x}}_k^{-})}^T+P_w$

其中，是状态矢量，

分别为计算均值和方差所用加权，χ是UKF算法中的sigma点，P_w是噪声方差。

3.根据权利要求1所述的一种提高鲁棒性的动基座初始对准方法，其特征在于：所述步骤2对模型噪声w进行自适应缩放采用如下公式进行处理：

${\hat{Q}}_k=Q_{k-1}\sqrt{{\mathrm{\&beta;}}_k}$

上式中，Q代表观测噪声的方差；β_k表示缩放因子，其计算公式如下：

${\mathrm{\&beta;}}_k=\frac{\mathrm{tr}\left(H_k{\tilde{P}}_k^{-}{H}_k^T\right)}{\mathrm{tr}\left(H_k{\hat{P}}_k^{-}{H}_k^T\right)}=\frac{\mathrm{tr}(\frac{1}{m}\underset{j=1}{\overset{m}{\mathrm{\&Sigma;}}}{d}_{k-j}{d}_{k=j}^T-R_k)}{\mathrm{tr}\left(H_k{\hat{P}}_k^{-}{H}_k^T\right)}$

上式中，tr()为取矩阵的迹，H为观测矩阵，为新息方差，d_k表示观测方程中产生的新息，其计算公式如下：

$d_k=(H_k{x}_k+v_k)-H_k{\hat{x}}_x^{-}=H_k(x_k-{\hat{x}}_k^{-})+v_k$

H为观测矩阵，

是状态矢量，v是观测噪声的期望。

4.根据权利要求1所述的一种提高鲁棒性的动基座初始对准方法，其特征在于：所述步骤3中观测值和滤波值的残差为：

$e\left(k\right)=y_k-{\tilde{y}}_k$

式中：y_k表示观测值；

表示滤波值；

所述的自适应矩阵S(k)为：

$S\left(k\right)=\frac{1}{\mathrm{\&mu;}}\underset{j=k-\mathrm{\&mu;}+1}{\overset{k}{\mathrm{\&Sigma;}}}e\left(j\right)e{\left(j\right)}^T-P_{\mathrm{yy}}(k+1|k)-R\left(k\right)$

式中：P_yy为UKF算法中观测更新的协方差；R是观测噪声方差，u是开窗大小

所述的门限m为：

$m={\mathrm{\&chi;}}_{\mathrm{\&alpha;}}^2(k-1)P_{y_k{y}_k}$

所述的修正状态估计为：

$\hat{x}(k+1)=\hat{x}(k+1|k)+K\left(k\right)g\left(k\right)e\left(k\right)$

是自由度为k-1的卡方分布值，K是卡尔曼滤波增益，g为校正状态的估计函数。

Images