CN103414451B - 一种应用于飞行器姿态估计的扩展卡尔曼滤波方法 - Google Patents

Abstract

本发明提出一种应用于飞行器姿态估计的扩展卡尔曼滤波方法，能够处理各种复杂干扰的非线性系统姿态估计。第一步：建立基于观测器的飞行器的动力学模型；第二步：根据第一步建立的模型方程，对加速度计和陀螺仪所测量三轴的姿态角θ_加和三轴姿态角速度ω_陀进行互补滤波，滤除高频和低频干扰，得到较纯净的信息值θ，去除可能系统内部及外部的扰动；第三步：利用第二步得到的信息值θ，采用扩展Kalman滤波方法对状态变量进行估计，得到的值即为姿态的估计值，从而完成了对飞行器状态的估计。

Description

一种应用于飞行器姿态估计的扩展卡尔曼滤波方法

技术领域

本发明涉及飞行器姿态估计、导航、信息处理和数据融合技术领域。

背景技术

姿态估计是飞行器，包括卫星，导航系统中的一项关键技术。在理想条件下，传统意义上的卡尔曼滤波方法就能得到很好的估计结果，然而，由于系统建模的不精准以及在实际中多重干扰导致的不确定性和量测误差，都将在一定程度上影响姿态估计的精度。

扩展卡尔曼滤波方法是一种直接用来解决非线性估计问题的方法，该方法是通过对非线性问题进行线性化，来得到最优的估计值。互补滤波方法，可以对不同的信息源信号进行滤波，尤其在惯性导航系统中，采用互补滤波的方法可以很好的抑制加速度计和陀螺仪的偏差，可以快速估计姿态角，比起传统的低通滤波可以大幅减少之后带来的影响。互补滤波器对于陀螺仪是高通特性，所以陀螺的偏差可以得到很好的控制；同时，对加速度计互补滤波器具有低通特性，可以控制加速度计的噪声。通过选取合适的参数值，可以很好的选取适合系统的阶段频率。

发明内容

本发明的目的在于提出一种应用于飞行器姿态估计的扩展卡尔曼滤波方法，能够处理各种复杂干扰的非线性系统姿态估计。

本发明的技术方案如下：

该应用于飞行器姿态估计的扩展卡尔曼滤波方法，包括以下步骤：

第一步：建立基于观测器的飞行器的动力学模型，该模型方程具体如下：

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_1\left(t\right)=-S\left[{\mathrm{\ω}}_m\left(t\right)\right]x_1\left(t\right)+S\left[b_{\mathrm{\ω}}\left(t\right)\right]x_1\left(t\right)\\ {\stackrel{\·}{x}}_2\left(t\right)=-S\left[{\mathrm{\ω}}_m\left(t\right)\right]x_2\left(t\right)+S\left[b_{\mathrm{\ω}}\left(t\right)\right]x_2\left(t\right)\\ {\stackrel{\·}{b}}_{\mathrm{\ω}}\left(t\right)=0\\ y_1\left(t\right)=x_1\left(t\right)\\ y_2\left(t\right)=x_2\left(t\right)\end{array}\right.$

其中x₁，x₂是陀螺仪和加速度计的敏感值；b_ω是角速度误差；S[ω(t)]为一个对称矩阵，其值是由载体坐标系到本地坐标系的转换矩阵R(t)确定，关系式为 $\stackrel{\·}{R}\left(t\right)=\left(t\right)S\left[\mathrm{\ω}\left(t\right)\right];$

第二步：根据第一步建立的模型方程，对加速度计和陀螺仪所测量三轴的姿态角θ_加和三轴姿态角速度ω_陀进行互补滤波，滤除高频和低频干扰，得到较纯净的信息值θ，去除可能系统内部及外部的扰动；

第三步：利用第二步得到的信息值θ，采用扩展Kalman滤波方法对状态变量进行估计，得到的值即为姿态的估计值，从而完成了对飞行器状态的估计。

本发明的有益效果：

本发明以飞行器姿态估计为背景，结合扩展Kalman滤波方法和互补滤波方法，保证了所设计的滤波器的一致完全可观性，以及对低频和高频干扰都能有很好的处理结果。这种改进的方法相比于传统的姿态估计方法，在对三个轴的姿态角进行估计时，有着更好的动态性能和稳态性能，并且能够很好的控制实际系统中存在的各种干扰。

具体实施方式

下面通过具体实施例说明本发明通过计算机仿真实验验证提出方案的可行性和有效性。

首先建立非线性的系统模型

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_1\left(t\right)=-S\left[{\mathrm{\ω}}_m\left(t\right)\right]x_1\left(t\right)+S\left[b_{\mathrm{\ω}}\left(t\right)\right]x_1\left(t\right)\\ {\stackrel{\·}{x}}_2\left(t\right)=-S\left[{\mathrm{\ω}}_m\left(t\right)\right]x_2\left(t\right)+S\left[b_{\mathrm{\ω}}\left(t\right)\right]x_2\left(t\right)\\ {\stackrel{\·}{b}}_{\mathrm{\ω}}\left(t\right)=0\\ y_1\left(t\right)=x_1\left(t\right)\\ y_2\left(t\right)=x_2\left(t\right)\end{array}\right.$

考虑非线性模型的如下参数，过程噪声是零均值的高斯白噪声，增益σ＝2，量测噪声协方差N＝1，初始状态矩阵的均值和方差分别为和initcov＝100。量测噪声和观测噪声也分别包含非线性因素，比如指数项和三角函数项，这些都使得模型更接近实际的系统。另外扩展Kalman滤波的参数，加性干扰的期望值和传感器噪声的期望值如下

Ξ＝diag[0.05,0.05,0.05,0.3,0.3,0.3,10^-6,10^-6,10^-6]

Θ＝diag[0.05,0.05,0.05,0.3,0.3,0.3]

并且实际的系统中，传感器噪声可能通过实验来近似逼近，而频率权重的选择也可以提高滤波器的性能。x，y和z轴的初始姿态角全部设为0。

然后，由互补滤波原理设计滤波器

$\hat{x}\left(s\right)=\frac{C\left(s\right)}{s+C\left(s\right)}{y}_x\left(s\right)+\frac{s}{C\left(s\right)+s}\frac{y_u\left(s\right)}{s}$

$=T\left(s\right)y_x\left(s\right)+S\left(s\right)\frac{y_u\left(s\right)}{s}$

最后，将互补滤波器的输出给扩展EKF实现优化的结果

仿真实验使用MATLAB软件，通过分别编写EKF_ContinuousSystemModel，EKF_ObserverModel，EKF_SystemModel以及EKF_Transform四个子程序，来模拟实际非线性系统以及状态观测器等模型，以验证方案的可行性。其中，EKF_ContinuousSystemModel是连续系统的状态模型函数，EKF_ObserverModel是观测器模型函数，EKF_SystemModel是系统模型函数，EKF_Transform是状态变换函数。

扩展Kalman滤波器可以很好的估计x，y和z轴的姿态角，并且能很好的控制三轴姿态角的误差。

然而考虑到扩展Kalman滤波结果的光滑程度，结合互补滤波之后的扩展Kalman滤波的结果，仿真结果显示改进后的扩展Kalman滤波器关于x，y和z轴的姿态角的估计要比传统扩展Kalman滤波器更快的稳定，收敛性更好，也说明了当存在复杂干扰时，改进后的扩展Kalman滤波器能很好的控制非线性系统的性能。进一步的可以看到，这种改进的方法可以在初始未知，干扰复杂的情况下也能更快、更精确的进行估计。

Claims (2)

1.一种应用于飞行器姿态估计的扩展卡尔曼滤波方法，其特征在于，包括以下步骤：

第一步：建立基于观测器的飞行器的动力学模型，该模型方程具体如下：

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_1\left(t\right)=-S\left[{\mathrm{\ω}}_m\left(t\right)\right]x_1\left(t\right)+S\left[b_{\mathrm{\ω}}\left(t\right)\right]x_1\left(t\right)\\ {\stackrel{\·}{x}}_2\left(t\right)=-S\left[{\mathrm{\ω}}_m\left(t\right)\right]x_2\left(t\right)+S\left[b_{\mathrm{\ω}}\left(t\right)\right]x_2\left(t\right)\\ {\stackrel{\·}{b}}_{\mathrm{\ω}}\left(t\right)=0\\ y_1\left(t\right)=x_1\left(t\right)\\ y_2\left(t\right)=x_2\left(t\right)\end{array}\right.$

其中x₁，x₂是陀螺仪和加速度计的敏感值；b_ω是角速度误差；S[ω(t)]为一个对称矩阵，其值是由载体坐标系到本地坐标系的转换矩阵R(t)确定，关系式为 $\stackrel{\·}{R}\left(t\right)=\left(t\right)S\left[\mathrm{\ω}\left(t\right)\right];$

第二步：根据第一步建立的模型方程，对加速度计和陀螺仪所测量三轴的姿态角θ_加和三轴姿态角速度ω_陀进行互补滤波，滤除高频和低频干扰，得到较纯净的信息值θ，去除可能系统内部及外部的扰动；

第三步：利用第二步得到的信息值θ，采用扩展Kalman滤波方法对状态变量进行估计，得到的值即为姿态的估计值，从而完成了对飞行器状态的估计。

2.如权利要求1所述的一种应用于飞行器姿态估计的扩展卡尔曼滤波方法，其特征在于，该方法可以采用MATLAB软件进行仿真实验，通过分别编写EKF_ContinuousSystemModel，EKF_ObserverModel，EKF_SystemModel以及EKF_Transform四个子程序，来模拟实际非线性系统以及状态观测器模型，其中，EKF_ContinuousSystemModel是连续系统的状态模型函数，EKF_ObserverModel是观测器模型函数，EKF_SystemModel是系统模型函数，EKF_Transform是状态变换函数。