CN109000663A - 基于时变自调整的滚动时域估计到达代价计算方法 - Google Patents

Abstract

本发明属于飞行器姿态估计领域，针对滚动时域估计中现有计算到达代价的方法精度不高，鲁棒性不强的问题，本发明设计了一种基于时变自调整的滚动时域估计到达代价计算方法。首先，为了提高姿态估计精度，设计了四元数MEMS陀螺/TAM组合姿态估计模型，以弥补MEMS陀螺单独姿态估计精度低的缺点；然后，针对TAM在线工作易受外界干扰的特点，设计了基于时变自调整的到达代价求解方法以增强系统鲁棒性。

Description

基于时变自调整的滚动时域估计到达代价计算方法

技术领域

本发明属于飞行器姿态估计滤波技术领域，更具体地，本发明涉及一种滚动时域估计滤波的时变自调整到达代价计算方法，可用于强干扰下的飞行器姿态估计。

背景技术

在过去的几十年里，基于优化的滚动时域估计(Moving Horizon Estimation,MHE)在非线性约束问题中应用非常广泛。和全信息估计相比，MHE通过仅考虑过去有限量测值来减小计算负担。过去历史信息对当前状态的影响即为到达代价。对于线性无约束姿态估计系统，标准卡尔曼滤波方差更新公式可以用来计算到达代价，但是对于非线性有约束姿态估计系统，到达代价的通用解析表达式不再适用。

对于非线性系统的到达代价的计算方法，目前主要研究方向都是针对EKF需要泰勒级数展开精度不高以及雅可比矩阵计算较为复杂的问题进行讨论。然而，以上这些文献都没有考虑算法的鲁棒性。事实上，由于传感器实际工作状态与线下状态有很大差别，极端的温度、压力和电磁环境使实际系统的参数发生变动，造成原来比较准确的模型也与实际系统不再匹配。同时，由于受到外界电磁信号的干扰，所建模型的噪声统计特性与实际噪声的统计特性可能有较大的差异。所有这些因素，包括对初始状态的不准确估计，都会影响滤波器的精度。因而，寻找一种鲁棒性强的到达代价计算方法在滤波估计系统中至关重要。

带有时变自调整因子的滤波器可以克服这些问题。它是一种效果良好的强跟踪滤波器，可以在线调整滤波增益，从而达到算法自适应的目的。不仅具有极强的模型失配鲁棒性，且具有概念清晰、计算简单的优点。将时变自调整因子滤波器的自适应性与UKF的非线性高逼近性结合起来，使得本方法具有较强的自适应能力和鲁棒性。

发明内容

本发明的目的在于突破传统滤波算法在抗干扰能力的不足，提高系统鲁棒性，提高系统自适应能力，提出一种基于强跟踪的滚动时域估计到达代价计算方法。其中解决的主要问题包括：

(1)对于非线性系统的到达代价的计算方法，目前主要研究方向都是针对EKF需要泰勒级数展开精度不高以及雅可比矩阵计算较为复杂的问题进行讨论。没有考虑算法的鲁棒性。

(2)由于传感器实际工作状态与线下状态有很大差别，极端的温度、压力和电磁环境使实际系统的参数发生变动，造成原来比较准确的模型也与实际系统不再匹配。

(3)由于较容易受到外界电磁信号的干扰，所建模型的噪声统计特性与实际噪声的统计特性可能有较大的差异。

(4)对初始状态的不准确估计，会影响滤波器的精度。

本发明所述的基于时变自调整的滚动时域估计到达代价计算方法，其特征在于包括以下技术措施：

步骤一：利用状态方程、观测方程的线性化矩阵，加入渐消因子，计算时变自调整因子，以实现对到达代价的时变自调整；

步骤二：在得到时变自调整因子的基础上，利用状态方程及观测方程，计算带有时变自调整因子的系统状态及方差的粗略估计值；

步骤三：在得到状态及方差的粗略估计值，利用带有时变自调整因子的协方差计算式，计算下一时刻状态及方差粗略估计值精调整值；

步骤四：将上步中输出的下一时刻状态及方差粗略估计值精调整值带入MHE系统计算到达代价。

对比现有技术，本技术方案所述的基于时变自调整因子的滚动时域估计到达代价计算方法，有益效果在于：

(1)本方法通过时变自调整因子无迹卡尔曼滤波计算滚动时域估计问题的到达代价，充分考虑了算法的鲁棒性；

(2)当传感器实际工作状态与线下差别较大，系统参数发生变化时，计算误差较小；

(3)当受到外界电磁信号干扰时，具有较强的抵抗能力，所建模型的噪声统计特性与实际特性差异大时能够自适应修正；

(4)可以修正对初始状态的不准确估计。

附图说明

附图1是基于时变自调整因子的滚动时域估计到达代价计算方法实施流程图。

附图2是滚动时域估计滤波方法基本原理图。

附图3是滚动时域估计滤波计算方法流程图。

具体实施方式

为了解决滚动时域估计方法鲁棒性不强，抗干扰能力较差的问题，提出了基于时变自调整因子的到达代价计算方法。首先，设计了时变自调整次优调节因子的计算方法，然后将该方法带入到UKF滤波算法中，计算滚动时域估计“窗口”外的状态估计值以及方差，最后将得到的估计值和方差带入到滚动时域估计系统中计算到达代价。

以下结合说明书附图1对本发明做进一步的详细描述。参照说明书附图1，本发明的处理流程分以下步骤：

(1)系统开始状态的赋值

(2)针对n维状态向量，根据中心扩展采样法选取代表点并计算加权数

假设状态向量x为n维，选取2n+1个采样点x_k,p0，p0＝0,...,2n。中心扩展采样法规则如下：

加权数计算方法如下：

其中，ρ₀＝τ²(n+κ)-n，τ通常取一个很小的数，对于高斯先验分布的情形μ＝2最优，κ通常取0。

(3)时变自调整次优调节因子的计算

时变自调整次优调节滤波器的一个充分条件是在线选择一个适当的时变增益矩阵使得残差正交。当滤波器状态估值偏离系统的状态时，结果会在残差序列的均值和幅值上表现出来。这时，可以通过增加时变次优调节因子Λ来实时调整状态预报误差协方差，强迫残差序列仍然保持正交，从而达到调整滤波增益矩阵形成强跟踪滤波器的目的。时变滤波增益矩阵中的次优调节因子：

其中

Λ₀＝trN_k/trM_k

F_k是状态方程的线性化矩阵，在算法中形式是H_k是观测方程的线性化矩阵，在算法中的形式为tr(·)为矩阵的迹；

0≤ρ≤1为渐消因子，通常取值0.95。引入渐消因子的目的是为了进一步提高滤波器的快速跟踪能力。滤波的实际误差不应小于理论误差，且不同的应用场合对系统的跟踪性能和滤波性能的侧重不同。Φ为减小系数，一般取经验值。

(4)下一时刻状态及方差的粗略估计值的计算

χ_p0,k|k-1＝f(x_p0,k-1)，p0＝0,...,2n

γ_p0,k|k-1＝h(χ_p0,k|k-1)

(5)下一时刻状态及方差的粗略估计值精调整

(6)滚动时域估计算法到达代价的计算

滚动时域估计算法的基本思想为，只考虑当前时刻以前固定数量的数据，同时将历史数据对估计的影响用一种方法来近似描述，则每次参与优化的数据量不变，这种方法实现起来就像一个滚动的观测“窗口”。这个“窗”包含过去有限的量测值，“窗”的最后一个输出为当前状态估计，附图2为滚动时域估计滤波方法基本原理图。

设窗口长度为L(i＝1,2,..,L)，其中窗口最后的一个点(i＝L)代表现在时刻k。滚动时域估计问题可表示为

s.t.

x_i+1＝f(x_i,u_i)+w_i,for i＝1,…,L-1 (3b)

y_i＝h(x_i)+v_i,for i＝2,…,L (3c)

l_i(x_i)＝0,for i＝1,…,L (3d)

其中x_i是待估计的状态。式(3b)描述了在离散情况下的状态递推过程。在该递推方程中u_i是已知输入，它的过程噪声为w_i，过程噪声方差为Q_i。方程(3c)为量测方程，y_i为量测值，v_i、R_i分别为量测噪声和量测噪声方差。在时域内的第一个时刻点为方差为P₁ ⁰。(3a)第三项θ_k为代价函数，它描述了到达代价。值得注意的是i＝1时刻的量测值并没有包含在代价函数中。假定从量测中得到的所有信息已经包含在先验估计中。等式约束由式(3d)给出。滚动时域估计问题的输出为x_L：时域“窗”中最后一个时刻的估计状态向量。

将式(1)、(2)带入到滚动时域估计系统(3e)中就可以得到到达代价θ_k。附图3是滚动时域估计滤波计算方法流程图。

Claims (5)

1.基于时变自调整的滚动时域估计到达代价计算方法，其特征在于，包括以下技步骤：

步骤一：利用状态方程、观测方程的线性化矩阵，加入渐消因子，计算时变自调整因子，以实现对到达代价的时变自调整；

步骤二：在得到时变自调整因子的基础上，利用状态方程及观测方程，计算带有时变自调整因子的系统状态及方差的粗略估计值；

步骤三：在得到状态及方差的粗略估计值，利用带有时变自调整因子的协方差计算式，计算下一时刻状态及方差粗略估计值精调整值；

步骤四：将上步中输出的下一时刻状态及方差粗略估计值精调整值带入MHE系统计算到达代价。

2.根据权利1要求所述基于时变自调整的滚动时域估计到达代价计算方法，其特征在于，步骤一中时变自调整因子的计算方法为：

其中

Λ₀＝trN_k/trM_k

F_k是k时刻状态方程的线性化矩阵，H_k是观测方程的线性化矩阵，tr(·)为矩阵的迹，0≤ρ≤1为渐消因子，取值0.95，Φ为减小系数，过程噪声方差为Q_k，R_k是量测噪声方差，状态方差为P_k，ε_k是状态残差。

3.根据权利2要求所述基于时变自调整的滚动时域估计到达代价计算方法，其特征在于，步骤二中带有时变自调整因子的系统状态及方差的粗略估计值的计算方法为：

其中，

χ_p0,k|k-1＝f(x_p0,k-1)，p0＝0,...,2n

γ_p0,k|k-1＝h(χ_p0,k|k-1)

为权重系数，f(·)为系统状态方程，h(·)为系统量测方程，为第k时刻的状态预测值，x_p0,k-1表示k-1时刻第p0个状态采样点。

4.根据权利3要求所述基于时变自调整的滚动时域估计到达代价计算方法，其特征在于，步骤三中下一时刻状态及方差粗略估计值精调整值计算方法为：

其中，

5.根据权利4要求所述基于强跟踪的滚动时域估计到达代价计算方法，其特征在于，步骤四中将上步中下一时刻状态及方差粗略估计值精调整值带入MHE系统计算到达代价方法为：

其中，窗口长度为L(i＝1,2,..,L)，其中窗口最后的一个点(i＝L)是现在时刻k，在窗口内的第一个时刻状态为方差为P₁ ⁰，θ_k为到达代价，将式(1)、(2)带入到式(3)中就得到到达代价θ_k。