CN105698799A - 一种提高捷联惯导系统姿态精度的预处理最优fir滤波器 - Google Patents

Abstract

本发明公开了一种提高捷联惯导系统姿态精度的预处理最优FIR滤波器，该滤波器包括有原FIR滤波器单元(1)、姿态误差获取单元(2)和粒子群优化单元(3)。姿态误差获取单元(2)对经原FIR滤波器单元(1)过滤后的角速度和加速度信息依据四子样算法获得姿态的等效旋转矢量，然后对姿态的等效旋转矢量通过粒子群优化单元(3)的处理得到实时更新后的滤波器最优参数及最小适应值。将本发明G-FIR滤波器应用到SINS中，是先对激光陀螺输出信号进行处理，再将处理后的信号应用于姿态解算，从而降低噪声对SINS的姿态影响。

Description

一种提高捷联惯导系统姿态精度的预处理最优FIR滤波器

技术领域

本发明涉及一种FIR滤波器，更特别地说，是指一种能够提高捷联惯导系统姿态精度的预处理最优FIR滤波器。

背景技术

捷联惯导系统(SINS，strapdowninertialnavigationsystem)是在平台式惯导系统基础上发展而来的。它是一种无框架系统，由三个速率陀螺、三个线加速度计和微型导航计算机组成。捷联式惯导系统原理简图(在本申请中称为图1)请参考科学出版社于2006年5月第一版，作者秦永元，《惯性导航》第287页。

FIR(FiniteImpulseResponse)滤波器：有限长单位冲激响应滤波器，又称为非递归型滤波器，是数字信号处理系统中最基本的元件，它可以在保证任意幅频特性的同时具有严格的线性相频特性，同时其单位抽样响应是有限长的，因而滤波器是稳定的系统。

现在被广泛考虑的影响捷联惯导系统姿态精度的因素有两个：一是捷联惯导系统的惯性元件精度，二是捷联惯导系统的姿态算法精度。提高惯性元件的精度通常是以增加惯性元件的生产成本为代价。此外，受到物理、光学等学科发展的制约，惯性元件可提高的精度范围存在限制。提高姿态算法精度成本低，但是实现难度大，这主要是由于姿态算法的推导过程十分复杂。通过分析激光陀螺的特性可知，激光陀螺的输出信号受温度、振动和磁场等因素的影响，这些影响导致激光陀螺的输出信号中含有大量的噪声。如果将带有大量噪声的激光陀螺输出信号被直接用于捷联惯导系统中，则会造成姿态误差。

发明内容

本发明的目的是设计一种提高捷联惯导系统姿态精度的预处理最优FIR滤波器(简称为G-FIR滤波器)，该G-FIR滤波器对接收到的角速度和加速度信息采用原有FIR滤波器的滤波形式处理，然后采用四子样算法求解姿态的等效旋转矢量，通过等效旋转矢量构建粒子群优化算法的适应值；最后通过粒子群优化算法来优化FIR滤波器的系数。将本发明G-FIR滤波器应用到SINS中，是先对激光陀螺输出信号进行处理，再将处理后的信号应用于姿态解算，从而降低噪声对SINS的姿态影响。

本发明是一种提高捷联惯导系统姿态精度的预处理最优FIR滤波器，该滤波器包括有原FIR滤波器单元(1)、姿态误差获取单元(2)和粒子群优化单元(3)。姿态误差获取单元(2)对经原FIR滤波器单元(1)过滤后的角速度和加速度信息依据四子样算法获得姿态的等效旋转矢量，然后通过粒子群优化单元(3)的处理得到实时更新后的滤波器最优参数及最小适应值。所述原FIR滤波器单元(1)中的滤波器模型为

在本发明中，姿态误差获取单元(2)一方面采用四子样算法求解姿态的等效旋转矢量；其中，等效旋转矢量中的圆锥误差幅值δφ_s为：另一方面，将等效旋转矢量从频域转换为时域表达的姿态信息的适应值函数为F＝|γ_true-γ_com|+θ_true-θ_com|。

在本发明中，粒子群优化单元(3)进行的滤波参数优化步骤为：

步骤一，计算适应值；

步骤11，根据适应值函数F＝|γ_true-γ_com|+|θ_true-θ_com|计算出参数群中每组参数的适应值，记为γ_true表示设定的横滚角，θ_true表示设定的俯仰角，γ_com表示四子样算法计算出的横滚角，θ_com表示四子样算法计算出的俯仰角；

步骤12，根据适应值函数F＝|γ_true-γ_com|+|θ_true-θ_com|计算出自身历史最优参数群中的每组参数PBⁱ的适应值，记为

步骤13，根据适应值函数F＝|γ_true-γ_com|+|θ_true-θ_com|计算出自身历史最优参数组GB的适应值，记为F_GB；

步骤二，更新最优参数群中的每组参数；

将所述的与所述的进行比较，根据公式更新最优参数群中的每组参数：

步骤三，更新最优参数组；

将所述的与所述的F_GB进行比较，根据公式更新最优参数组：

步骤四，更新调整步长；

根据公式更新参数调整步长；c₁和c₂为固定学习因子，r₁和r₂为随机学习因子，为t+1迭代次数的参数的调整步长。

步骤五，更新参数群中每组参数；

根据公式更新参数群中每组参数，为t+1迭代次数的参数群中每组参数。

经步骤五处理后，需要检验更新后的参数是否在参数的可行域D内，如果不在，则继续执行步骤四和步骤五，直到更新后的参数在可行域D内；然后，还需检验更新后的参数是否满足G-FIR滤波器的通带截止频率小于阻带截止频率，如果不满足，则继续执行步骤四和步骤五，直到更新后的参数满足此约束条件。

步骤六，是否达到最大迭代次数；

检验迭代次数是否到达最大迭代次数Iter_max，如果没有达到最大，则返回步骤一，进入下一次迭代；如果达到最大迭代次数则终止迭代。

本发明设计的能够提高捷联惯导系统姿态精度的预处理最优FIR滤波器的优点在于：

①本发明设计的G-FIR滤波器是从软件方面提高捷联惯导系统姿态精度，它的成本远远低于一般从硬件方面提高捷联惯导系统姿态精度。

②相比于常见的通过姿态算法的改进提高捷联惯导系统姿态精度，本发明的G-FIR滤波器的设计难度更低，使用方法时更加简便。

③本发明设计的G-FIR滤波器的参数采用粒子群优化算法进行优化，大大减少了人工推算的时间。

附图说明

图1是传统捷联式惯导系统原理的简图。

图2是本发明设计的能够提高捷联惯导系统姿态精度的预处理最优FIR滤波器的结构框图。

图3是G-FIR滤波器的最优参数与原FIR滤波器的横滚角对比图。

图4是G-FIR滤波器的最优参数与原FIR滤波器的俯仰角对比图。

具体实施方式

下面将结合附图和实施例对本发明做进一步的详细说明。

参见图2所示，本发明的目的是设计一种提高捷联惯导系统姿态精度的预处理最优FIR滤波器(简称为G-FIR滤波器)，该G-FIR滤波器包括有原FIR滤波器单元1、姿态误差获取单元2和粒子群优化单元3。姿态误差获取单元(2)对经原FIR滤波器单元(1)过滤后的角速度和加速度信息依据四子样算法获得姿态的等效旋转矢量，然后对姿态的等效旋转矢量通过粒子群优化单元(3)的处理得到实时更新后的滤波器最优参数及最小适应值。将本发明G-FIR滤波器应用到SINS中，是先对激光陀螺输出信号进行处理，再将处理后的信号应用于姿态解算，从而降低噪声对SINS的姿态的影响。

在图1所示的捷联式惯导系统中，将本发明设计的G-FIR滤波器置于比力坐标变换单元与指令解算单元之间。

加速度计组合输出的加速度信息记为f，激光陀螺组合输出的角速度信息记为ω。采样时间记为Time，在本发明中，采样时间Time设置为1200s，采样时间间隔为0.005s，即1s时间里将采集到200个加速度信息f和角速度信息ω。因此，将采样时间Time里加速度计组合输出的加速度信息集记为Acc＝{f₁，f₂，f₃，…，f_P}。将采样时间Time里激光陀螺组合输出的角速度信息集记为Gyro＝{ω₁，ω₂，ω₃，…，ω_P}。P表示采样个数的标识号。

原FIR滤波器单元1

FIR滤波器的模型请参考清华大学出版社于2003年8月第2版，作者胡广书《数字信号处理理论、算法与实现》第296页，H(z)为FIR滤波器的转移函数，z为复变量，即离散的加速度信息或者角速度信息，b_n为FIR滤波器的系数，M为求和元素，n为求和指标。

在本发明中，原FIR滤波器单元1对接收到的加速度信息集

Acc＝{f₁，f₂，f₃，…，f_P}进行滤波处理，得到滤波后的加速度信息

在本发明中，原FIR滤波器单元1对接收到的角速度信息集

Gyro＝{ω₁，ω₂，ω₃，…，ω_P}进行滤波处理，得到滤波后的角速度信息

在本发明中，应用FIR滤波方式是为了滤除加速度信息和角速度信息中的高频噪声，一般地，超过50Hz频率的噪声被滤除。

姿态误差获取单元2

姿态误差获取单元2第一方面采用四子样算法求解姿态的等效旋转矢量，第二方面将等效旋转矢量从频域转换为时域表达的姿态信息。

在本发明中，等效旋转矢量中的圆锥误差幅值δφ_s为：

${\mathrm{\δ\φ}}_s=2\[H{\left(\mathrm{\Ω}\right)}^2\×\stackrel{\·}{\mathrm{\Φ}}\left(\mathrm{\Ω}\right)\]\×\underset{s=1}{\overset{3}{\Σ}}{C}_s\×d_s(\mathrm{\Ω}\×h)---\left(1\right)$

C_s为圆锥误差系数；

s为采样间隔长度；

Ω为振动频率；

h为四子样算法中的姿态更新周期；

H(Ω)为原FIR滤波器的幅频函数；

为未加载G-FIR滤波器时的圆锥漂移率；

为加载G-FIR滤波器时的圆锥漂移率；

d_s(Ω×h)为以Ω×h为自变量的相关函数。

在本发明中，由于等效旋转矢量中的圆锥误差幅值δφ_s为频域中圆锥误差幅值，应用公式(1)能够减小G-FIR滤波器的幅频特性对圆锥误差造成的影响，由于圆锥误差是姿态误差的一个重要组成部分，因此G-FIR滤波器的影响能够间接地反映在姿态误差上。公式(1)是一个频域的表达式，但是在工程上圆锥误差或者姿态误差的计算都是在时域中进行的，因此为了计算方便选用的适应值函数为F＝|γ_true-γ_com|+|θ_true-θ_com|，γ_true表示设定的横滚角，θ_true表示设定的俯仰角，γ_com表示四子样算法计算出的横滚角，θ_com表示四子样算法计算出的俯仰角；在本发明中，γ_true为1度，θ_true为5度。

在本发明中，四子样算法请参考科学出版社于2006年5月第一版，作者秦永元，《惯性导航》第311-315页。

在本发明中，将四子样算法引入FIR滤波中，四子样算法能够有效地补偿姿态的不可交换性误差，并且能在高动态的条件下求解出高精度的姿态。

粒子群优化单元3

为了获得G-FIR的最优参数，采用粒子群优化算法进行参数寻优。粒子群优化算法是基于群的智能并行全局搜索策略，与其他智能算法相比，它具备更快的收敛速度和更好的鲁棒性。寻找滤波器的最优参数是一个连续的、多变量的非线性问题。粒子群优化算法请参考电子工业出版社于2013年2月，作者刘金琨等，《系统辨识理论及MATLAB仿真》第215-217页。

在使用粒子群优化算法寻找G-FIR的最优参数时，首先必须考虑粒子群优化算法的一些初始条件，这些条件包括有：需要优化的参数数目num，每个需要优化的参数的可行域D，参数调整步长的可行域U，参数群中参数组的组数N和最大迭代次数Iter_max。其次，需要对参数群进行一些初始化操作，包括：在参数的可行域D内对参数群的每组参数进行初始化；在参数调整步长的可行域U内对每组参数的调整步长组进行初始化；为自身历史最优的参数群中的每组参数PBⁱ＝{pb^j}和自身历史最优参数组GB＝{gb^j}设置初值。i为参数组的标识号，i＝1,…,N；t为迭代次数标识号，t+1为t的下一个迭代次数，t＝1,…,Iter_max；j为参数标识号，j＝1,…,num；q为G-FIR滤波器参数，v为G-FIR滤波器参数的调整步长，pb为自身历史最优的参数群中的G-FIR滤波器参数，gb为自身历史最优参数组中的G-FIR滤波器参数。

在本发明中，粒子群优化单元3进行的滤波参数优化步骤为：

步骤一，计算适应值；

步骤11，根据适应值函数F＝|γ_true-γ_com|+|θ_true-θ_com|计算出参数群中每组参数的适应值，记为

步骤12，根据适应值函数F＝|γ_true-γ_com|+|θ_true-θ_com|计算出自身历史最优参数群中的每组参数PBⁱ的适应值，记为

步骤13，根据适应值函数F＝|γ_true-γ_com|+|θ_true-θ_com|计算出自身历史最优参数组GB的适应值，记为F_GB；

步骤二，更新最优参数群中的每组参数；

将所述的与所述的进行比较，根据公式(2)更新最优参数群中的每组参数：

${\mathrm{PB}}_{new}^i=\left\{\begin{array}{cc}{Q}_{t+1}^i& F_{Q_{t+1}^i}\≤F_{{\mathrm{PB}}^i}\\ {\mathrm{PB}}^i& F_{Q_{t+1}^i}>F_{{\mathrm{PB}}^i}\end{array}\right.---\left(2\right)$

为更新后的最优参数群中的每组参数。

为t+1迭代次数的参数群中每组参数。

为t+1迭代次数的参数群中每组参数的适应值。

步骤三，更新最优参数组；

将所述的与所述的F_GB进行比较，根据公式(3)更新最优参数组：

${\mathrm{GB}}_{new}=\left\{\begin{array}{cc}{\mathrm{PB}}^i& F_{{\mathrm{PB}}^i}\≤F_{GB}\\ GB& F_{{\mathrm{PB}}^i}>F_{GB}\end{array}\right.---\left(3\right)$

GB_new为更新后的最优参数组。

步骤四，更新调整步长；

根据公式(4)更新参数调整步长；

$v_{t+1}^j=v_t^j+c_1{r}_1({\mathrm{pb}}^j-q_t^j)+c_2{r}_2({\mathrm{gb}}^j-q_t^j)---\left(4\right)$

c₁和c₂为固定学习因子，r₁和r₂为随机学习因子，一般地c₁＝c₂＝0.4、r₁＝r₂＝0.2。

为t+1迭代次数的参数的调整步长。

步骤五，更新参数群中每组参数；

根据公式(5)更新参数群中每组参数：

$q_{t+1}^i=v_{t+1}^i+q_t^i---\left(5\right)$

为t+1迭代次数的参数群中每组参数。

在本发明中，一方面需要检验更新后的参数是否在参数的可行域D内，如果不在，则继续执行步骤四和步骤五，直到更新后的参数在可行域D内。另一方面，还需检验更新后的参数是否满足G-FIR滤波器的通带截止频率小于阻带截止频率，如果不满足，则继续执行步骤四和步骤五，直到更新后的参数满足此约束条件。

步骤六，是否达到最大迭代次数；

检验迭代次数是否到达最大迭代次数Iter_max，如果没有达到最大，则返回步骤一，进入下一次迭代；如果达到最大迭代次数则终止迭代；

在本发明中，实时的参数组(GB或者GB_new)中包含有G-FIR滤波器的最优参数及最小适应值。

实施例1

设置本发明G-FIR滤波器的通带截止频率和阻带起始频率的可行域D都设置为[0,100Hz]。G-FIR滤波器参数的调整步长的可行域U选为[-50,50]。考虑到计算量的问题，将参数群的参数组数N设置为10，最大迭代次数Iter_max设置为50。最终粒子群优化算法计算出的G-FIR滤波器的最优参数为：通带截止频率为0Hz，阻带起始频率为36.3342Hz。下面的表1中给出了G-FIR滤波器的最优参数与几组非最优参数计算的横滚角误差和俯仰角误差结果(包括系统固定安装误差)。

表1G-FIR滤波器最优参数与非最优参数的水平姿态角误差对比表

通带截止频率(Hz)	0	10.1348	19.8822	43.8744
					阻带起始频率(Hz)	36.3342	25.3489	66.0710	76.5516
横滚角误差(角秒)	67.536	244.044	234.9	243.936
					俯仰角误差(角秒)	224.928	222.084	222.228	222.3
适应值(角秒)	292.464	466.128	457.128	466.236

图3中的(a)和图4中的(a)分别展示了20分钟激光陀螺数据经过G-FIR滤波器的滤波处理后，四子样姿态算法计算出的横滚角和俯仰角，即表1中的第1列。图3中的(b)、(c)、(d)和图4中的(b)、(c)、(d)分别展示了20分钟激光陀螺数据原FIR滤波器的滤波处理后，四子样姿态算法计算出的横滚角和俯仰角，即表1中的第2～4列。

根据通带截止频率0Hz和阻带起始频率为36.3342Hz设计出的G-FIR滤波器的传递函数形式的模型，G-FIR滤波器阶次为23阶，式中的系数如表2所列：

表2G-FIR滤波器的系数列表

b1	b2	b3	b4	b5	b6	b7	b8
								0	-0.00035	-0.0026	-0.0068	-0.01	-0.0063	0.0103	0.0426
b9	b10	b11	b12	b13	b14	b15	b166 -->
								0.0869	0.1332	0.1685	0.1817	0.1685	0.1332	0.0869	0.0426
b17	b18	b19	b20	b21	b22	b23
								0.0103	-0.0063	-0.01	-0.0068	-0.0026	-0.00035	0

采用本发明G-FIR滤波器能够最大限度地降低原有FIR滤波器引起的圆锥误差，从而大幅度提高了姿态的精度。

Claims (6)

1.一种提高捷联惯导系统姿态精度的预处理最优FIR滤波器，其特征在于：该滤波器包括有原FIR滤波器单元(1)、姿态误差获取单元(2)和粒子群优化单元(3)。姿态误差获取单元(2)对经原FIR滤波器单元(1)过滤后的角速度和加速度信息依据四子样算法获得姿态的等效旋转矢量，然后通过粒子群优化单元(3)的处理得到实时更新后的滤波器最优参数及最小适应值。所述原FIR滤波器单元(1)中的滤波器模型为

2.根据权利要求1所述的一种提高捷联惯导系统姿态精度的预处理最优FIR滤波器，其特征在于：姿态误差获取单元(2)一方面采用四子样算法求解姿态的等效旋转矢量；其中，等效旋转矢量中的圆锥误差幅值δφ_s为：另一方面，将等效旋转矢量从频域转换为时域表达的姿态信息的适应值函数为F＝|γ_true-γ_com|+|θ_true-θ_com|。

3.根据权利要求1所述的一种提高捷联惯导系统姿态精度的预处理最优FIR滤波器，其特征在于：粒子群优化单元(3)进行的滤波参数优化步骤为：

步骤一，计算适应值；

步骤11，根据适应值函数F＝|γ_true-γ_com|+|θ_true-θ_com|计算出参数群中每组参数的适应值，记为γ_true表示设定的横滚角，θ_true表示设定的俯仰角，γ_com表示四子样算法计算出的横滚角，θ_com表示四子样算法计算出的俯仰角；

步骤12，根据适应值函数F＝|γ_true-γ_com|+|θ_true-θ_com|计算出自身历史最优参数群中的每组参数PBⁱ的适应值，记为

步骤13，根据适应值函数F＝|γ_true-γ_com|+|θ_true-θ_com|计算出自身历史最优参数组GB的适应值，记为F_GB；

步骤二，更新最优参数群中的每组参数；

将所述的与所述的进行比较，根据公式更新最优参数群中的每组参数：

为更新后的最优参数群中的每组参数。

为t+1迭代次数的参数群中每组参数。

为t+1迭代次数的参数群中每组参数的适应值。

步骤三，更新最优参数组；

将所述的与所述的F_GB进行比较，根据公式更新最优参数组：

GB_new为更新后的最优参数组。

步骤四，更新调整步长；

根据公式更新参数调整步长；c₁和c₂为固定学习因子，r₁和r₂为随机学习因子，为t+1迭代次数的参数的调整步长。

步骤五，更新参数群中每组参数；

根据公式更新参数群中每组参数：

为t+1迭代次数的参数群中每组参数。

经步骤五处理后，需要检验更新后的参数是否在参数的可行域D内，如果不在，则继续执行步骤四和步骤五，直到更新后的参数在可行域D内；然后，还需检验更新后的参数是否满足G-FIR滤波器的通带截止频率小于阻带截止频率，如果不满足，则继续执行步骤四和步骤五，直到更新后的参数满足此约束条件。

步骤六，是否达到最大迭代次数；

检验迭代次数是否到达最大迭代次数Iter_max，如果没有达到最大，则返回步骤一，进入下一次迭代；如果达到最大迭代次数则终止迭代。

4.根据权利要求3所述的一种提高捷联惯导系统姿态精度的预处理最优FIR滤波器，其特征在于：γ_true为1度，θ_true为5度。

5.根据权利要求3所述的一种提高捷联惯导系统姿态精度的预处理最优FIR滤波器，其特征在于：c₁＝c₂＝0.4、r₁＝r₂＝0.2。

6.根据权利要求1所述的一种提高捷联惯导系统姿态精度的预处理最优FIR滤波器，其特征在于：能够应用于捷联惯导系统中，降低噪声对捷联惯导系统的姿态影响。