一种自适应鲁棒矩阵卡尔曼滤波的高旋体空中对准方法

1.本公开属于高动态环境下高旋体空中对准技术领域，特别是涉及到一种自适应鲁棒矩阵卡尔曼滤波的高旋体空中对准方法。


背景技术：

2.高旋体空中对准指在高旋体飞行中确定sins(strap-down inertial navigation system，捷联惯性导航系统)初始姿态的过程，核心为确定弹体坐标系和参考导航坐标系之间的初始姿态矩阵。近些年来，动态粗对准问题越来越受到人们的关注。针对高精度sins，wu提出了基于优化的对准方法(optimization-based alignment，oba)。oba方法是基于姿态矩阵分解技术导出的，该方法将所需姿态矩阵分解为两个时变姿态矩阵和一个定常姿态矩阵，利用姿态更新程序直接计算时变姿态矩阵。为了得到定常姿态矩阵，可以通过构造向量观测将问题转化为wahba问题
3.目前求解wahba问题的方法一般有确定性算法，如triad、euler-q和quest(quaternion estimator，四元数估值器)，随机算法，如递归quest，最优request和矩阵卡尔曼滤波(matrix kalman filter，mkf)。triad算法使用两个矢量观测值计算姿态矩阵，利用双矢量观测存在不对称性的缺点，计算精度受传感器的限制。为了完善triad算法，euler-q算法定义一个新的损失函数，找到最优euler轴，利用euler轴的旋转特性得到最优姿态矩阵，euler轴和euler角是描述姿态矩阵最直观的参数，然而由于特征向量和特征值的计算，使得它在实时性方面存在不足。为了完善euler-q算法，quest算法给出了wahba问题的最小二乘解，既保留了快速确定性算法的计算优势，又得到了最优解，但其缺点是没有考虑传感器测量噪声和系统噪声。为了完善确定性算法，最优request算法使用所有测量矢量，用于姿态四元数最小二乘估计的递推算法。然而由于其使用恒定的通过经验选择的增益，当存在系统噪声时，它是次优的。为了完善request算法，opreq算法构造了k矩阵的状态空间模型，并利用kalman滤波技术获得了最优增益。然而它的缺点是算法复杂度高，且只适用于高斯噪声环境。为了完善opreq算法，mkf算法估计了状态矩阵和状态协方差矩阵，这些矩阵可以方便地用矩阵系统和测量方程来描述。优点是保持了矩阵方程的自然形式。然而，缺点是只考虑高斯噪声。
4.高旋体复杂高动态飞行环境下，惯性传感器和gnss极易受到外界环境干扰，一方面高旋体在飞行过程中与大气摩擦，使得高旋体存在随机振动的情况，表现为散射噪声或异常值形式；另一方面gnss为微弱电磁波信号，受到干扰后造成量测信息不服从高斯分布，表现为混合高斯噪声或异常值形式。因此，亟需一种解决存在非高斯噪声的wahab问题的方法，减少对准时间，提高对准精度与鲁棒性。


技术实现要素：

5.有鉴于此，本公开提出了一种自适应鲁棒矩阵卡尔曼滤波的高旋体空中对准方法，能够在非高斯噪声环境下实现高旋体空中对准，对准时间短且对准精度高，鲁棒性强。
6.根据本公开的一方面，提出了一种自适应鲁棒矩阵卡尔曼滤波的高旋体空中对准方法，所述方法包括：
7.以k矩阵为状态变量构建所述高旋体空中对准滤波模型；
8.根据所述高旋体空中对准滤波模型对所述k矩阵状态向量的上一时刻的估计值和当前时刻的估计值定义第一噪声评价指标，根据所述k矩阵状态向量上一刻的估计值和当前时刻所述高旋体空中对准滤波模型的量测信息定义第二噪声评价指标；
9.融合所述第一噪声评价指标和所述第二噪声评价指标得到综合噪声评价指标；
10.基于自适应鲁棒性和所述综合噪声评价指标的统计置信域对所述噪声异常程度进行自适应分类；
11.根据所述综合噪声评价指标和综合噪声评价指标的统计置信域构建自适应鲁棒矩阵卡尔曼滤波模型；
12.递推所述自适应鲁棒矩阵卡尔曼滤波模型的滤波算法得到所述高旋体空中对准滤波模型的量测信息的误差协方差矩阵，根据所述量测信息的误差协方差矩阵得到最优k矩阵，根据所述最优k矩阵进行高旋体空中对准。
13.在一种可能的实现方式中，所述以k矩阵为状态变量构建所述高旋体空中对准滤波模型，包括：
14.根据链式法将所述高旋体空中对准滤波模型的姿态矩阵进行分解为定常姿态矩阵和时变姿态矩阵；
15.根据所述高旋体微惯导速度微分方程构建所述定常姿态矩阵模型对观测矢量和进行归一化得到k矩阵，以k矩阵为状态变量构建所述高旋体空中对准滤波模型；其中，所述时变姿态矩阵根据所述高旋体微惯导旋转矢量计算得到。
16.在一种可能的实现方式中，所述综合噪声评价指标的统计置信域包括：其中，th1为第一综合噪声评价阈值、th2为第二综合噪声评价阈值、th3为第三综合噪声评价阈值，k为综合噪声评价阈值调节因子；
17.且m1为综合噪声评价指标在大于等于第一综合噪声评价阈值th1时的数量、m2为综合噪声评价指标在大于第二综合噪声评价阈值th2小于等于第一综合噪声评价阈值th1时的数量、m3为综合噪声评价指标在大于第三综合噪声评价阈值th3小于等于第二综合噪声评价阈值th2时的数量和m4为综合噪声评价指标小于等于第三综合噪声评价阈值th3时的数量。
18.在一种可能的实现方式中，根据自适应鲁棒基于所述综合噪声评价指标的统计置信域对所述噪声异常程度进行自适应分类，包括：
19.当综合噪声评价指标小于等于第三综合噪声评价阈值th3时，无噪声异常干扰；
20.当综合噪声评价指标大于第三综合噪声评价阈值th3小于等于第二综合噪声评价
阈值th2时，噪声异常干扰程度低；
21.当综合噪声评价指标大于第二综合噪声评价阈值th2小于等于第一综合噪声评价阈值th1时，噪声异常干扰程度中等；
22.当综合噪声评价指标大于等于第一综合噪声评价阈值th1时，噪声异常干扰程度中高。
23.在一种可能的实现方式中，所述根据量测信息的误差协方差矩阵得到最优k矩阵，从而实现对高旋体进行空中对准包括：
24.调整所述量测信息的误差协方差矩阵的精度得到最优k矩阵，所述最优k矩阵的最大特征值所对应的特征向量为所述高旋体空中对准滤波模型的初始姿态矩阵，根据所述初始姿态矩阵对高旋体进行空中对准。
25.本公开的自适应鲁棒矩阵卡尔曼滤波的高旋体空中对准方法，以k矩阵为状态变量构建所述高旋体空中对准滤波模型；根据所述高旋体空中对准滤波模型对所述k矩阵状态向量的上一时刻的估计值和当前时刻的估计值定义第一噪声评价指标，根据所述k矩阵状态向量上一刻的估计值和当前时刻所述高旋体空中对准滤波模型的量测信息定义第二噪声评价指标；融合所述第一噪声评价指标和所述第二噪声评价指标得到综合噪声评价指标；基于自适应鲁棒性和所述综合噪声评价指标的统计置信域对所述噪声异常程度进行自适应分类；根据所述综合噪声评价指标和综合噪声评价指标的统计置信域构建自适应鲁棒矩阵卡尔曼滤波模型；递推所述自适应鲁棒矩阵卡尔曼滤波模型的滤波算法得到所述高旋体空中对准滤波模型的量测信息的误差协方差矩阵，根据所述量测信息的误差协方差矩阵得到最优k矩阵，根据所述最优k矩阵进行高旋体空中对准。能够在非高斯噪声环境下实现高旋体空中对准，对准时间短且对准精度高，鲁棒性强。
26.根据下面参考附图对示例性实施例的详细说明，本公开的其它特征及方面将变得清楚。
附图说明
27.包含在说明书中并且构成说明书的一部分的附图与说明书一起示出了本公开的示例性实施例、特征和方面，并且用于解释本公开的原理。
28.图1示出了根据本公开一实施例的自适应鲁棒矩阵卡尔曼滤波的高旋体空中对准方法的流程图；
29.图2示出了根据本公开另一实施例的自适应鲁棒矩阵卡尔曼滤波的高旋体空中对准方法的流程图；
30.图3示出了根据本公开一实施例的自适应鲁棒矩阵卡尔曼滤波的设计原理图。
具体实施方式
31.以下将参考附图详细说明本公开的各种示例性实施例、特征和方面。附图中相同的附图标记表示功能相同或相似的元件。尽管在附图中示出了实施例的各种方面，但是除非特别指出，不必按比例绘制附图。
32.在这里专用的词“示例性”意为“用作例子、实施例或说明性”。这里作为“示例性”所说明的任何实施例不必解释为优于或好于其它实施例。
33.另外，为了更好的说明本公开，在下文的具体实施方式中给出了众多的具体细节。本领域技术人员应当理解，没有某些具体细节，本公开同样可以实施。在一些实例中，对于本领域技术人员熟知的方法、手段、元件和电路未作详细描述，以便于凸显本公开的主旨。
34.图1示出了根据本公开一实施例的自适应鲁棒矩阵卡尔曼滤波的高旋体空中对准方法的流程图。如图1所示，该方法可以包括：
35.步骤s1：以k矩阵为状态变量构建所述高旋体空中对准滤波模型。
36.在一示例中，根据链式法将所述高旋体空中对准滤波模型的姿态矩阵进行分解为定常姿态矩阵和时变姿态矩阵；
37.根据所述高旋体微惯导速度微分方程构建所述定常姿态矩阵模型对观测矢量和进行归一化得到k矩阵，以k矩阵为状态变量构建所述高旋体空中对准滤波模型；其中，所述时变姿态矩阵根据所述高旋体微惯导旋转矢量计算得到。
38.图2示出了根据本公开另一实施例的自适应鲁棒矩阵卡尔曼滤波的高旋体空中对准方法的流程图。
39.如图2所示，据链式法则，将高旋体的实时姿态矩阵分解为定常姿态矩阵和时变姿态矩阵和时变姿态矩阵式中，n(t)为在t时刻的高旋体导航坐标系，b(t)为在t时刻的高旋体弹体坐标系，n(0)为在0时刻的高旋体导航坐标系为，b(0)为在0时刻的高旋体弹体坐标系。
40.根据所述高旋体微惯导速度微分方程构建所述定常姿态矩阵模型：
41.其中，
42.其中，re为地球平均半径，ω
ie
为地球自转角速率，[l λ h]
t
为高旋体的位置，分别代表纬度、经纬和高度，为速高旋体的度，分别代表东向、北向和天向速度，fb为加速度计测量的三轴比力，gn为当地重力加速度。
[0043]
其中，观测向量和具体的递推求解如下：
[0044]
[0045]
对式(2)进行wahba问题求解，根据tk时刻的观测矢量和归一化计算矩阵：
[0046]
式中，
[0047]
根据矩阵最大特征值对应的特征向量即为初始姿态矩阵理论上，恒定不变，即kk矩阵的状态方程为：kk＝k
k-1
式(5)。
[0048]
根据式(4)和式(5)，令状态估计矩阵xk＝kk，观测量矩阵为可建立高旋体空中对准滤波模型：
[0049][0050]
式中，f＝i4，h＝i4。wk和vk分别为系统噪声和量测噪声，且满足：
[0051][0052]
式中，vec()为向量化算子,qk为系统噪声均值；qk为系统噪声方差；rk为量测噪声均值；rk为量测噪声方差。
[0053]
步骤s2：根据所述高旋体空中对准滤波模型对所述k矩阵状态向量的上一时刻的估计值和当前时刻的估计值定义第一噪声评价指标，根据所述k矩阵状态向量上一刻的估计值和当前时刻所述高旋体空中对准滤波模型的量测信息定义第二噪声评价指标。
[0054]
步骤s3：融合所述第一噪声评价指标和所述第二噪声评价指标得到综合噪声评价指标。
[0055]
如图2所示，高旋体在复杂高动态的飞行环境中，惯性器件和gnss(全球导航卫星系统)易受外界干扰，量测噪声不再服从高斯分布，使得采用mkf对kk矩阵估计的结果存在偏差。因此，需要设计非高斯噪声环境下滤波算法，以提高估计精度。
[0056]
假设滤波算法的新息为：
[0057]
式中，为当前时刻的估计值，为上一时刻的估计值。
[0058]
根据新息定义伪新息：
[0059]
其中，伪新息为当前时刻量测值与上一时刻量测值之差，能够直观反映量测信息中的噪声异常情况。
[0060]
根据滤波算法的新息和伪新息定义第一噪声评价指标：
[0061]
[0062]
式中，为frobenius范数，df
1,k
≥0，且在噪声没有较大波动时，0≤df
1,k
≤2，并集中在0附近。
[0063]
为了进一步提高噪声评价的可靠性，基于上一时刻k矩阵估计值定义第二噪声评价指标。
[0064]
根据式(2)可得：c
k-1
(0)αk≈βkꢀꢀ
式(11)，
[0065]
式中，为k矩阵上一时刻的估计初始矩阵。
[0066]
根据滤波模型的上一时刻估计值和当前时刻量测值定义直接判别因子t
1,k
和t
2,k
，通过计算t
1,k
和t
2,k
偏离1的程度反映噪声的异常情况。直接判别因子t
1,k
和t
2,k
的表达式为：
[0067][0068]
式中，||*||2为向量的2-范数。
[0069]
根据直接判别因子t
1,k
和t
2,k
得到第二噪声评价指标：
[0070]
df
2,k
＝|t
1,k-t
2,k
|
ꢀꢀ
式(13)，
[0071]
式中，
[0072]
结合第一噪声评价指标df
1,k
和第二噪声评价指标df
2,k
，得到综合噪声评价指标。首先将综合噪声评价指标无量纲化：
[0073][0074]
根据式(14)，可得综合噪声评价指标：
[0075][0076]
式中，dfk为综合噪声评价指标，λ1和λ2分别为第一噪声评价指标权重和第二噪声评价指标权重，dfk值越大，表明噪声异常程度越高。
[0077]
步骤s4：基于自适应鲁棒性和所述综合噪声评价指标的统计置信域对所述噪声异常程度进行自适应分类。
[0078]
图3示出了根据本公开一实施例的自适应鲁棒矩阵卡尔曼滤波的设计原理图
[0079]
采用“自适应外环和鲁棒内环”相结合的改进策略设计自适应鲁棒矩阵卡尔曼滤波方法(adaptive robust matrix kalman filter，armkf)，能够减少对准时间，提高对准精度与鲁棒性。
[0080]
如图3所示，对于“自适应外环”，根据综合噪声评价指标dfk的统计特性，基于统计置信域进行量测噪声异常程度分类，设置阈值，述综合噪声评价指标的统计置信域包括：
[0081]
其中，th1为第一综合噪声评价阈值、th2为第二综合噪声评价阈值、th3为第三综合噪声评价阈值，k为综合噪声评价阈值调节因子。
[0082]
根据式(16)可知，κ越大，属于th1和th3之间的综合噪声评价指标数量越多；κ越小，属于th1和th3之间的综合噪声评价指标数量越少。可以进一步统计综合噪声评价dfk处于不同噪声阈值区间内的数量，从而自适应确定κ值，则
[0083]
其中，当th1≤dfi,i＝1,2,...,k时dfi的数量为m1，当th2＜dfi≤th1,i＝1,2,...,k时dfi的数量为m2，当th3＜dfi≤th2,i＝1,2,...,k时dfi的数量为m3，当dfi≤th3,i＝1,2,...,k时dfi的数量为m4，即m1为综合噪声评价指标在大于等于第一综合噪声评价阈值th1时的数量、m2为综合噪声评价指标在大于第二综合噪声评价阈值th2小于等于第一综合噪声评价阈值th1时的数量、m3为综合噪声评价指标在大于第三综合噪声评价阈值th3小于等于第二综合噪声评价阈值th2时的数量和m4为综合噪声评价指标小于等于第三综合噪声评价阈值th3时的数量。
[0084]
根据式(17)可知，th1和th3之间dfi的数量较多时，κ值减小，从而缩小阈值区间范围；th1和th3之间dfi的数量较少时，κ值增大，从而扩大阈值区间范围，进而保证th1和th3之间dfi的数量分布合理。
[0085]
且，当dfi≤th3,i＝1,2,...,k，即综合噪声评价指标小于等于第三综合噪声评价阈值th3时，无噪声异常干扰；
[0086]
当th3＜dfi≤th2,i＝1,2,...,k，即综合噪声评价指标大于第三综合噪声评价阈值th3小于等于第二综合噪声评价阈值th2时，噪声异常干扰程度低；
[0087]
当th2＜dfi≤th1,i＝1,2,...,k，即综合噪声评价指标大于第二综合噪声评价阈值th2小于等于第一综合噪声评价阈值th1时，噪声异常干扰程度中等；
[0088]
当th1≤dfi,i＝1,2,...,k，即综合噪声评价指标大于等于第一综合噪声评价阈值th1时，噪声异常干扰程度中高。
[0089]
步骤s5：根据所述综合噪声评价指标和综合噪声评价指标的统计置信域构建自适应鲁棒矩阵卡尔曼滤波模型。
[0090]
如图3所示，根据上述“自适应外环”构建“鲁棒内环”，即构建自适应鲁棒卡尔曼滤波模型，并在线调整高旋体空中对准滤波模型的量测信息的估计误差协方差矩阵。
[0091]
首先，初始化k矩阵即e矩阵：其中，p0为k矩阵的初始化估计均方误差矩阵。
[0092]
更新k矩阵和该滤波算法的估计均方误差p0即：
[0093]
p
k,k-1
＝p
k-1
+q
k-1
ꢀꢀ
式(20)。
[0094]
根据式(15)、式(16)、式(17)分别计算得到的综合噪声评价指标、综合噪声评价指标阈值和综合噪声评价指标阈值调节因子，以及步骤s4根据综合噪声评价指标的统计置信域对噪声异常程度的自适应分类，进行自适应鲁棒卡尔曼滤波增益和估计均方误差的更新。
[0095]
在一示例中，
[0096]
当dfi≤th3,i＝1,2,...,k，无异常噪声干扰，则自适应鲁棒卡尔曼滤波的估计均方误差矩阵为：
[0097]
当th3＜dfi≤th2,i＝1,2,...,k，异常噪声干扰程度低，则自适应鲁棒卡尔曼滤波的估计均方误差矩阵为：
[0098]
当th2＜dfi≤th1,i＝1,2,...,k，异常噪声干扰程度中等，则自适应鲁棒卡尔曼滤波的估计均方误差矩阵为：
[0099]
当th1≤dfi,i＝1,2,...,k，异常噪声干扰程度高，则自适应鲁棒卡尔曼滤波的估计均方误差矩阵为：
[0100]
因此，根据自适应鲁棒卡尔曼滤波的估计均方误差矩阵能够更新得到自适应鲁棒卡尔曼滤波的滤波增益与估计均方误差：
[0101][0102]
在另一示例中，
[0103]
初始化对准滤波模型的量测信息的误差协方差矩阵为：
[0104]
当dfi≤th3,i＝1,2,...,k，无异常噪声干扰，则自适应鲁棒卡尔曼滤波的滤波增益与估计均方误差分别为：
[0105]gk
＝p
k,k-1
(p
k,k-1
+rk)-1
，
[0106]
当th3＜dfi≤th2,i＝1,2,...,k，异常噪声干扰程度低，则自适应鲁棒卡尔曼滤波的滤波增益与估计均方误差分别为：
[0107]gk
＝p
k,k-1
(p
k,k-1
+rk)-1
，
[0108]
当th2＜dfi≤th1,i＝1,2,...,k，异常噪声干扰程度中等，则自适应鲁棒卡尔曼滤波的滤波增益与估计均方误差分别为：
[0109][0110]
当th1≤dfi,i＝1,2,...,k，异常噪声干扰程度高，则自适应鲁棒卡尔曼滤波的滤波增益与估计均方误差分别为：
[0111][0112]
由上述两个示例，根据自适应鲁棒卡尔曼滤波的滤波增益与估计均方误差可以构
建自适应鲁棒卡尔曼滤波模型，则进一步得到自适应鲁棒卡尔曼滤波的估计值为：其中，的第(lj)个元素为1，其余元素为0。
[0113]
步骤s6：递推所述自适应鲁棒矩阵卡尔曼滤波模型的滤波算法得到所述高旋体空中对准滤波模型的量测信息的误差协方差矩阵，根据所述量测信息方差矩阵得到最优k矩阵，根据所述最优k矩阵进行高旋体空中对准。
[0114]
其中，k矩阵的最大特征值所对应的特征向量为高旋体空中对准滤波模型的初始姿态矩阵，因此需要根据自适应鲁棒矩阵卡尔曼滤波模型进行递推得到高旋体空中对准滤波模型的量测信息的误差协方差矩阵。
[0115]
由于传感器(例如惯性传感器或gnss传感器)存在量测误差，可将高旋体空中对准滤波模型的观测量写为加性噪声形式：
[0116]
其中，αk＝[α
k,x α
k,y α
k,z
]
t
、βk＝[β
k,x β
k,y β
k,z
]
t
，αk和βk表示真值；
[0117]
δαk＝[δα
k,x δα
k,y δα
k,z
]
t
、δβk＝[δβ
k,x δβ
k,y δβ
k,z
]
t
，δαk和δβk表示噪声，且满足：
[0118]
则在线调整之后的高旋体空中对准滤波模型的量测信息误差协方差矩阵为：
[0119]
式中，
[0120][0121][0122][0123]
然后，调整高旋体空中对准滤波模型的量测信息的误差协方差矩阵的精度得到最优k矩阵，则最优k矩阵的最大特征值所对应的特征向量为高旋体空中对准滤波模型的初始姿态矩阵，根据初始姿态矩阵对高旋体进行空中对准，从而实现对高旋体进行空中对准，能
够在非高斯噪声环境下实现高旋体空中对准，对准时间短且对准精度高，鲁棒性强。
[0124]
以上已经描述了本公开的各实施例，上述说明是示例性的，并非穷尽性的，并且也不限于所披露的各实施例。在不偏离所说明的各实施例的范围和精神的情况下，对于本技术领域的普通技术人员来说许多修改和变更都是显而易见的。本文中所用术语的选择，旨在最好地解释各实施例的原理、实际应用或对市场中的技术改进，或者使本技术领域的其它普通技术人员能理解本文披露的各实施例。