CN105242058A - 一种双频弹性干扰下角速度滤波方法 - Google Patents

Abstract

一种双频弹性干扰下角速度滤波方法，涉及一种角速度滤波方法，特别是涉及一种角速度扩展卡尔曼滤波方法。为了解决现有的传递函数形式滤波器在面临不同频率的弹性干扰时需要重新设计的问题和进行弹性干扰和测量噪声滤除时产生相位延迟而导致的角速度信号估计值不准确的问题。本发明首先针对弹性干扰下角速度信号动态过程进行建模，并建立角速度和干扰信号的状态方程；然后针对状态方程设计状态转移矩阵，同时采用EKF设计Kalman滤波方程，并利用设计的Kalman滤波方程对含有双频弹性干扰下的角速度进行滤波处理。本发明适用于角速度滤波领域。

Description

一种双频弹性干扰下角速度滤波方法

技术领域

本发明涉及一种角速度滤波方法，特别是涉及一种角速度扩展卡尔曼滤波方法。

背景技术

同陀螺器件相比，角加速度计具有体积小、成本降低空间大、无漂移、安装限制少的特点。角加速度计通常被用来组建无陀螺仪的惯性测量单元(gyro-freeinertialmeasurementunitGF-IMU)，在航天飞行器中具有很好的应用前景。在一些工程实践中将飞行器作为刚体模型来处理，认为角加速度计只受到测量噪声的影响。实际上，飞行器是一个弹性体，除了测量噪声外还受到飞行器自身弹性振动的影响。这种弹性振动对角速度测量有着很大的影响。针对该问题，在以往工程实践中，将弹性振动建模为不同频率和幅度的正弦信号，并且假定该干扰信号的频率是已知且固定的；干扰信号的幅度和角速度信号幅度的比例关系是已知的，使用传递函数式的数字滤波器对角速度信号的测量值进行滤波。这种方法存在两个缺陷。第一，滤波器传递函数的形式和弹性干扰信号的频率有关，在不同的场合下，弹性干扰的频率可能不同，导致滤波器需要重新设计。这使得该方法适用面比较窄。第二，传递函数形式的滤波器虽然对弹性干扰和测量噪声进行了滤除，但是会产生相位延迟，导致对角速度信号估计值不准确。

发明内容

本发明为了解决现有的传递函数形式滤波器在面临不同频率的弹性干扰时需要重新设计的问题和进行弹性干扰和测量噪声滤除时产生相位延迟而导致的角速度信号估计值不准确的问题。

一种双频弹性干扰下角速度滤波方法，包括下述步骤：

步骤1、针对弹性干扰下角速度信号动态过程进行建模：

飞行器的角速度动态过程为

$\stackrel{\·}{\mathrm{\ω}}=\frac{M}{J}=a---\left(1\right)$

其中，ω代表角速度，a代表角加速度，M为转动力矩，J为转动惯量；

弹性干扰信号为两个正弦信号叠加而成，设两个正弦信号的频率分别为ω₁和ω₂，单位为rad/s；两个正弦信号的相位分别为和幅度分别为b₁和b₂；

弹性干扰信号的形式如下

将弹性干扰信号中的进行处理得到弹性干扰状态v₁、v₂，将弹性干扰信号中的进行处理得到弹性干扰状态v₃、v₄；

将干扰频率ω₁和ω₂当做状态进行估计；设状态为X＝[xv₁v₂v₃v₄ω₁ω₂]^T，其中x为角速度信号，即x＝ω；则角速度和干扰信号的状态方程为

$\left[\begin{array}{c}\stackrel{\·}{x}\\ {\stackrel{\·}{v}}_1\\ {\stackrel{\·}{v}}_2\\ {\stackrel{\·}{v}}_3\\ {\stackrel{\·}{v}}_4\\ {\stackrel{\·}{\mathrm{\ω}}}_1\\ {\stackrel{\·}{\mathrm{\ω}}}_2\end{array}\right]=\left[\begin{array}{ccccccc}0& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0\\ 0& -{\mathrm{\ω}}_1^2& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& -{\mathrm{\ω}}_2^2& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}x\\ v_1\\ v_2\\ v_3\\ v_4\\ {\mathrm{\ω}}_1\\ {\mathrm{\ω}}_2\end{array}\right]+\left[\begin{array}{c}1\\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\end{array}\right]a_z---\left(3\right)$

其中，a_z角加速度的测量值；

角速度的测量方程为

z＝x+v₁+v₃+w(4)

式中，z为角速度的测量值；w是零均值高斯白噪声；

测量矩阵为

H＝[1101000](5)

步骤2、设计状态转移矩阵：

由状态方程(3)描述的系统的状态转移矩阵为

$\mathrm{\Φ}=\left[\begin{array}{ccccccc}1& 0& 0& 0& 0& 0& 0\\ 0& {\mathrm{\φ}}_1& T& 0& 0& {\mathrm{\φ}}_2& 0\\ 0& {\mathrm{\φ}}_3& {\mathrm{\φ}}_1& 0& 0& {\mathrm{\φ}}_4& 0\\ 0& 0& 0& {\mathrm{\φ}}_5& T& 0& {\mathrm{\φ}}_6\\ 0& 0& 0& {\mathrm{\φ}}_7& {\mathrm{\φ}}_5& 0& {\mathrm{\φ}}_8\\ 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 1\end{array}\right]---\left(6\right)$

式中

${\mathrm{\φ}}_1=1-\frac{1}{2}{T}^2{\mathrm{\ω}}_1^2---\left(7\right)$

φ₂＝-T²ω₁v₁(8)

${\mathrm{\φ}}_3=-{\mathrm{T\ω}}_1^2---\left(9\right)$

φ₄＝-T²ω₁v₂-2Tω₁v₁(10)

${\mathrm{\φ}}_5=1-\frac{1}{2}{T}^2{\mathrm{\ω}}_2^2---\left(11\right)$

φ₆＝-T²ω₂v₃(12)

${\mathrm{\φ}}_7=-{\mathrm{T\ω}}_2^2---\left(13\right)$

φ₈＝-T²ω₂v₄-2Tω₂v₃(14)

其中，T为测量周期；

步骤3、设计Kalman滤波方程：

角速度滤波器采用EKF(扩展卡尔曼滤波器)设计，Kalman滤波器方程为

${\hat{X}}_{k+1,k+1}={\hat{X}}_{k+1,k}+K_k(Z_{k+1}-H{\hat{X}}_{k+1,k})---\left(21\right)$

K_k+1＝P_k+1,kH^T(HP_k+1,kH^T+R)^-1(22)

$P_{k+1,k}={\mathrm{\Φ}}_{k+1,k}{P}_{k,k}{\mathrm{\Φ}}_{k+1,k}^T+Q---\left(23\right)$

P_k+1,k+1＝(I-K_k+1H)P_k+1,k(24)

式中，是对k+1时刻状态的估计值；是对k+1时刻状态的预报值；K_k和K_k+1分别是k时刻和k+1时刻的滤波器增益；Z_k+1为k+1时刻的测量值；Φ_k+1,k为k时刻到k+1时刻的状态转移矩阵；P_k+1,k为k+1时刻状态预报的协方差矩阵；P_k,k和P_k+1,k+1分别是k时刻和k+1时刻状态估计协方差矩阵；Q为过程噪声矩阵；R为测量噪声矩阵；I为7×7维单位矩阵；

步骤4、根据步骤3设计的Kalman滤波方程对含有双频弹性干扰下的角速度进行滤波处理。

本发明具有以下有益效果：

本发明设计了一种基于扩展卡尔曼滤波的角速度测量信号的滤波方法，适用于两个任意低频正弦弹性干扰下的角速度测量值的干扰和测量噪声滤除问题；设计好的滤波器不需要根据不同的弹性干扰频率重新设计或者修改，可以适用于任意低频弹性干扰信号，使得本发明的滤波方法适用范围较广；而且本发明的滤波方法可以估计出弹性干扰信号的频率。同时本发明的滤波方法针对角速度信号的估计值基本上没有相位滞后，使得估计结果更加准确，估计误差标准差相对于测量误差标准差减小了70％以上。

附图说明

图1为实施例中角速度信号真值、测量值和估计值的对比图；

图2为实施例中弹性干扰状态v₁下的估计仿真结果；

图3为实施例中弹性干扰状态v₂下的估计仿真结果；

图4为实施例中弹性干扰状态v₃下的估计仿真结果；

图5为实施例中弹性干扰状态v₄下的估计仿真结果；

图6为实施例中对两个弹性干扰信号频率的估计仿真结果。

具体实施方式

具体实施方式一：

一种双频弹性干扰下角速度滤波方法，包括下述步骤：

步骤1、针对弹性干扰下角速度信号动态过程进行建模：

飞行器的角速度动态过程为

$\stackrel{\·}{\mathrm{\ω}}=\frac{M}{J}=a---\left(1\right)$

其中，ω代表角速度，a代表角加速度，M为转动力矩，J为转动惯量；

弹性干扰信号为两个正弦信号叠加而成，设两个正弦信号的频率分别为ω₁和ω₂，单位为rad/s；两个正弦信号的相位分别为和幅度分别为b₁和b₂；

弹性干扰信号的形式如下

将弹性干扰信号中的进行处理得到弹性干扰状态v₁、v₂，将弹性干扰信号中的进行处理得到弹性干扰状态v₃、v₄；

将干扰频率ω₁和ω₂当做状态进行估计；设状态为X＝[xv₁v₂v₃v₄ω₁ω₂]^T，其中x为角速度信号，即x＝ω；则角速度和干扰信号的状态方程为

$\left[\begin{array}{c}\stackrel{\·}{x}\\ {\stackrel{\·}{v}}_1\\ {\stackrel{\·}{v}}_2\\ {\stackrel{\·}{v}}_3\\ {\stackrel{\·}{v}}_4\\ {\stackrel{\·}{\mathrm{\ω}}}_1\\ {\stackrel{\·}{\mathrm{\ω}}}_2\end{array}\right]=\left[\begin{array}{ccccccc}0& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0\\ 0& -{\mathrm{\ω}}_1^2& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& -{\mathrm{\ω}}_2^2& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}x\\ v_1\\ v_2\\ v_3\\ v_4\\ {\mathrm{\ω}}_1\\ {\mathrm{\ω}}_2\end{array}\right]+\left[\begin{array}{c}1\\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\end{array}\right]a_z---\left(3\right)$

其中，a_z角加速度的测量值；

角速度的测量方程为

z＝x+v₁+v₃+w(4)

式中，z为角速度的测量值；w是零均值高斯白噪声；

测量矩阵为

H＝[1101000](5)

步骤2、设计状态转移矩阵：

由状态方程(3)描述的系统的状态转移矩阵为

$\mathrm{\Φ}=\left[\begin{array}{ccccccc}1& 0& 0& 0& 0& 0& 0\\ 0& {\mathrm{\φ}}_1& T& 0& 0& {\mathrm{\φ}}_2& 0\\ 0& {\mathrm{\φ}}_3& {\mathrm{\φ}}_1& 0& 0& {\mathrm{\φ}}_4& 0\\ 0& 0& 0& {\mathrm{\φ}}_5& T& 0& {\mathrm{\φ}}_6\\ 0& 0& 0& {\mathrm{\φ}}_7& {\mathrm{\φ}}_5& 0& {\mathrm{\φ}}_8\\ 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 1\end{array}\right]---\left(6\right)$

式中

${\mathrm{\φ}}_1=1-\frac{1}{2}{T}^2{\mathrm{\ω}}_1^2---\left(7\right)$

φ₂＝-T²ω₁v₁(8)

${\mathrm{\φ}}_3=-{\mathrm{T\ω}}_1^2---\left(9\right)$

φ₄＝-T²ω₁v₂-2Tω₁v₁(10)

${\mathrm{\φ}}_5=1-\frac{1}{2}{T}^2{\mathrm{\ω}}_2^2---\left(11\right)$

φ₆＝-T²ω₂v₃(12)

${\mathrm{\φ}}_7=-{\mathrm{T\ω}}_2^2---\left(13\right)$

φ₈＝-T²ω₂v₄-2Tω₂v₃(14)

其中，T为测量周期；

步骤3、设计Kalman滤波方程：

角速度滤波器采用EKF(扩展卡尔曼滤波器)设计，Kalman滤波器方程为

${\hat{X}}_{k+1,k+1}={\hat{X}}_{k+1,k}+K_k(Z_{k+1}-H{\hat{X}}_{k+1,k})---\left(21\right)$

K_k+1＝P_k+1,kH^T(HP_k+1,kH^T+R)^-1(22)

$P_{k+1,k}={\mathrm{\Φ}}_{k+1,k}{P}_{k,k}{\mathrm{\Φ}}_{k+1,k}^T+Q---\left(23\right)$

P_k+1,k+1＝(I-K_k+1H)P_k+1,k(24)

式中，是对k+1时刻状态的估计值；是对k+1时刻状态的预报值；K_k和K_k+1分别是k时刻和k+1时刻的滤波器增益；Z_k+1为k+1时刻的测量值；Φ_k+1,k为k时刻到k+1时刻的状态转移矩阵；P_k+1,k为k+1时刻状态预报的协方差矩阵；P_k,k和P_k+1,k+1分别是k时刻和k+1时刻状态估计协方差矩阵；Q为过程噪声矩阵；R为测量噪声矩阵；I为7×7维单位矩阵；

步骤4、根据步骤3设计的Kalman滤波方程对含有双频弹性干扰下的角速度进行滤波处理。

具体实施方式二：

本实施方式步骤1所述测量矩阵H通过以下步骤得到：

针对角速度的测量方程有如下形式

z＝HX+w

同时，根据角速度的测量方程z＝x+v₁+v₃+w和X＝[xv₁v₂v₃v₄ω₁ω₂]^T，得到H＝[1101000]。

其它步骤及参数与具体实施方式一相同。

具体实施方式三：

本实施方式步骤1所述的将弹性干扰信号中的进行处理得到v₁、v₂，将弹性干扰信号中的进行处理得到v₃、v₄的过程如下：

设v₂为v₁关于时间的一阶导数，则对v₂取时间的导数为

同理设v₄为v₃关于时间的一阶导数，则对v₄取时间的导数为

根据上述关系，将其整理成弹性干扰信号状态方程的形式如下

$\left[\begin{array}{c}{\stackrel{\·}{v}}_1\\ {\stackrel{\·}{v}}_2\\ {\stackrel{\·}{v}}_3\\ {\stackrel{\·}{v}}_4\end{array}\right]=\left[\begin{array}{cccc}0& 1& 0& 0\\ -{\mathrm{\ω}}_1^2& 0& 0& 0\\ 0& 0& 0& 1\\ 0& 0& -{\mathrm{\ω}}_2^2& 0\end{array}\right]\left[\begin{array}{c}{v}_1\\ v_2\\ v_3\\ v_4\end{array}\right]$

其它步骤及参数与具体实施方式二相同。

实施例

在本实施例中，角速度信号为x＝asin(ωt+φ)，其中a＝0.5，ω＝1(rad/s)，相位φ为随机值在范围[0,2π)中均匀分布，选取φ＝0.7854(rad)；两个低频干扰信号角频率分别为v₁＝b₁sin(ω₁t+φ₁)和v₂＝b₂sin(ω₂t+φ₂)，其中b₁＝b₂＝0.1×a，ω₁＝20×2π(rad/s)，ω₂＝50×2π(rad/s)，相位φ₁和φ₂为随机值在范围[0,2π)中均匀分布，选取φ₁＝1.5057(rad)，φ₂＝0.0025(rad)；角速度滤波器状态初始值为X＝[000002634]^T。

根据以上参数，按照具体实施方式三的步骤进行仿真实验，实验结果如图1至图6所示。

图1显示了角速度信号真值、测量值和估计值的比较结果。可以看到测量值受到了比较强烈的弹性振动信号和测量噪声的干扰。本发明的滤波器比较显著的滤除了振动信号和测量噪声的干扰，得到的测量信号的估计值和真值很接近而且信号没有相位上的延迟。图2显示了频率为20Hz的弹性振动信号即弹性干扰状态v₁下的估计结果。由于刚开始滤波器设定的振动信号的频率不准确(代表对振动信号频率不具有准确信息)，所以在前0.2秒估计结果较差。但很快滤波器的估计结果就向真值收敛。图3，图4和图5分别显示了弹性干扰状态v₂，v₃和v₄下的估计结果，类似于状态v₁的估计，在0.2秒后滤波器的估计结果向真值收敛。图6显示了对两个弹性干扰信号频率的估计，对于频率为20Hz的弹性信号，滤波器初始值设置频率为26Hz，对于频率为50Hz的弹性信号，滤波器初始值设置频率为44Hz，代表对弹性干扰信号的频率信息事先并不完全确定。可以看到在0.2秒后，两个频率的估计值就快速趋向真值并稳定在真值上。在本实施例中，测量误差标准差为0.042rad/s，而估计误差的标准差为0.0081rad/s。估计误差的标准差相对于测量误差标准差减小了81％。

Claims (3)

1.一种双频弹性干扰下角速度滤波方法，其特征在于包括下述步骤：

步骤1、针对弹性干扰下角速度信号动态过程进行建模：

飞行器的角速度动态过程为

$\stackrel{\·}{\mathrm{\ω}}=\frac{M}{J}=a---\left(1\right)$

其中，ω代表角速度，a代表角加速度，M为转动力矩，J为转动惯量；

弹性干扰信号为两个正弦信号叠加而成，设两个正弦信号的频率分别为ω₁和ω₂，单位为rad/s；两个正弦信号的相位分别为和幅度分别为b₁和b₂；

弹性干扰信号的形式如下

将弹性干扰信号中的进行处理得到弹性干扰状态v₁、v₂，将弹性干扰信号中的进行处理得到弹性干扰状态v₃、v₄；

将干扰频率ω₁和ω₂当做状态进行估计；设状态为X＝[xv₁v₂v₃v₄ω₁ω₂]^T，其中x为角速度信号，即x＝ω；则角速度和干扰信号的状态方程为

$\left[\begin{array}{c}\stackrel{\·}{x}\\ {\stackrel{\·}{v}}_1\\ {\stackrel{\·}{v}}_2\\ {\stackrel{\·}{v}}_3\\ {\stackrel{\·}{v}}_4\\ {\stackrel{\·}{\mathrm{\ω}}}_1\\ {\stackrel{\·}{\mathrm{\ω}}}_2\end{array}\right]=\left[\begin{array}{ccccccc}0& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0\\ 0& -{\mathrm{\ω}}_1^2& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& -{\mathrm{\ω}}_2^2& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}x\\ v_1\\ v_2\\ v_3\\ v_4\\ {\mathrm{\ω}}_1\\ {\mathrm{\ω}}_2\end{array}\right]+\left[\begin{array}{c}1\\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\end{array}\right]a_z---\left(3\right)$

其中，a_z角加速度的测量值；

角速度的测量方程为

z＝x+v₁+v₃+w(4)

式中，z为角速度的测量值；w是零均值高斯白噪声；

测量矩阵为

H＝[1101000](5)

步骤2、设计状态转移矩阵：

$\mathrm{\Φ}=\left[\begin{array}{ccccccc}1& 0& 0& 0& 0& 0& 0\\ 0& {\mathrm{\φ}}_1& T& 0& 0& {\mathrm{\φ}}_2& 0\\ 0& {\mathrm{\φ}}_3& {\mathrm{\φ}}_1& 0& 0& {\mathrm{\φ}}_4& 0\\ 0& 0& 0& {\mathrm{\φ}}_5& T& 0& {\mathrm{\φ}}_6\\ 0& 0& 0& {\mathrm{\φ}}_7& {\mathrm{\φ}}_5& 0& {\mathrm{\φ}}_8\\ 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 1\end{array}\right]---\left(6\right)$

式中

${\mathrm{\φ}}_1=1-\frac{1}{2}{T}^2{\mathrm{\ω}}_1^2---\left(7\right)$

φ₂＝-T²ω₁v₁(8)

${\mathrm{\φ}}_3=-{\mathrm{T\ω}}_1^2---\left(9\right)$

φ₄＝-T²ω₁v₂-2Tω₁v₁(10)

${\mathrm{\φ}}_5=1-\frac{1}{2}{T}^2{\mathrm{\ω}}_2^2---\left(11\right)$

φ₆＝-T²ω₂v₃(12)

${\mathrm{\φ}}_7=-{\mathrm{T\ω}}_2^2---\left(13\right)$

φ₈＝-T²ω₂v₄-2Tω₂v₃(14)

其中，T为测量周期；

步骤3、设计Kalman滤波方程：

角速度滤波器采用EKF设计，Kalman滤波器方程为

${\hat{X}}_{k+1,k+1}={\hat{X}}_{k+1,k}+K_k(Z_{k+1}-H{\hat{X}}_{k+1,k})---\left(21\right)$

K_k+1＝P_k+1,kH^T(HP_k+1,kH^T+R)^-1(22)

$P_{k+1,k}={\mathrm{\Φ}}_{k+1,k}{P}_{k,k}{\mathrm{\Φ}}_{k+1,k}^T+Q---\left(23\right)$

P_k+1,k+1＝(I-K_k+1H)P_k+1,k(24)

式中，是对k+1时刻状态的估计值；是对k+1时刻状态的预报值；K_k和K_k+1分别是k时刻和k+1时刻的滤波器增益；Z_k+1为k+1时刻的测量值；Φ_k+1,k为k时刻到k+1时刻的状态转移矩阵；P_k+1,k为k+1时刻状态预报的协方差矩阵；P_k,k和P_k+1,k+1分别是k时刻和k+1时刻状态估计协方差矩阵；Q为过程噪声矩阵；R为测量噪声矩阵；I为7×7维单位矩阵；

步骤4、根据步骤3设计的Kalman滤波方程对含有双频弹性干扰下的角速度进行滤波处理。

2.根据权利要求1所述的一种双频弹性干扰下角速度滤波方法，其特征在于步骤1所述测量矩阵H通过以下步骤得到：

针对角速度的测量方程有如下形式

z＝HX+w

同时，根据角速度的测量方程z＝x+v₁+v₃+w和X＝[xv₁v₂v₃v₄ω₁ω₂]^T，得到H＝[1101000]。

3.根据权利要求1或2所述的一种双频弹性干扰下角速度滤波方法，其特征在于步骤1所述的将弹性干扰信号中的进行处理得到v₁、v₂，将弹性干扰信号中的进行处理得到v₃、v₄的过程如下：

设v₂为v₁关于时间的一阶导数，则对v₂取时间的导数为

同理设v₄为v₃关于时间的一阶导数，则对v₄取时间的导数为

根据上述关系，将其整理成弹性干扰信号状态方程的形式如下

$\left[\begin{array}{c}{\stackrel{\·}{v}}_1\\ {\stackrel{\·}{v}}_2\\ {\stackrel{\·}{v}}_3\\ {\stackrel{\·}{v}}_4\end{array}\right]=\left[\begin{array}{cccc}0& 1& 0& 0\\ -{\mathrm{\ω}}_1^2& 0& 0& 0\\ 0& 0& 0& 1\\ 0& 0& -{\mathrm{\ω}}_2^2& 0\end{array}\right]\left[\begin{array}{c}{v}_1\\ v_2\\ v_3\\ v_4\end{array}\right].$