一种提高捷联惯导系统姿态精度的预处理最优fir滤波器的制造方法
【技术领域】
[0001] 本发明涉及一种FIR滤波器，更特别地说，是指一种能够提高捷联惯导系统姿态精 度的预处理最优FIR滤波器。
【背景技术】
[0002] 捷联惯导系统（SINS，strapdown inertial navigation system)是在平台式惯导 系统基础上发展而来的。它是一种无框架系统，由三个速率陀螺、三个线加速度计和微型导 航计算机组成。捷联式惯导系统原理简图（在本申请中称为图1)请参考科学出版社于2006 年5月第一版，作者秦永元，《惯性导航》第287页。
[0003] FIR(Finite Impulse Response)滤波器:有限长单位冲激响应滤波器，又称为非 递归型滤波器，是数字信号处理系统中最基本的元件，它可以在保证任意幅频特性的同时 具有严格的线性相频特性，同时其单位抽样响应是有限长的，因而滤波器是稳定的系统。
[0004] 现在被广泛考虑的影响捷联惯导系统姿态精度的因素有两个:一是捷联惯导系统 的惯性元件精度，二是捷联惯导系统的姿态算法精度。提高惯性元件的精度通常是以增加 惯性元件的生产成本为代价。此外，受到物理、光学等学科发展的制约，惯性元件可提高的 精度范围存在限制。提尚姿态算法精度成本低，但是实现难度大，这主要是由于姿态算法的 推导过程十分复杂。通过分析激光陀螺的特性可知，激光陀螺的输出信号受温度、振动和磁 场等因素的影响，这些影响导致激光陀螺的输出信号中含有大量的噪声。如果将带有大量 噪声的激光陀螺输出信号被直接用于捷联惯导系统中，则会造成姿态误差。
【发明内容】
[0005] 本发明的目的是设计一种提高捷联惯导系统姿态精度的预处理最优FIR滤波器 (简称为G-FIR滤波器），该G-FIR滤波器对接收到的角速度和加速度信息采用原有FIR滤波 器的滤波形式处理，然后采用四子样算法求解姿态的等效旋转矢量，通过等效旋转矢量构 建粒子群优化算法的适应值;最后通过粒子群优化算法来优化FIR滤波器的系数。将本发明 G-FIR滤波器应用到SINS中，是先对激光陀螺输出信号进行处理，再将处理后的信号应用于 姿态解算，从而降低噪声对SINS的姿态影响。
[0006] 本发明是一种提高捷联惯导系统姿态精度的预处理最优FIR滤波器，该滤波器包 括有原FIR滤波器单元（1)、姿态误差获取单元(2)和粒子群优化单元(3)。姿态误差获取单 元(2)对经原FIR滤波器单元(1)过滤后的角速度和加速度信息依据四子样算法获得姿态的 等效旋转矢量，然后通过粒子群优化单元(3)的处理得到实时更新后的滤波器最优参数及 最小适应值。所述原FIR滤波器单元(1)中的滤波器模型为_
[0007] 在本发明中，姿态误差获取单元(2) -方面采用四子样算法求解姿态的等效旋转 矢量;其中，等效旋转矢量中的圆锥误差幅值S Φ 8为：
另一方面，将等效旋转矢量从频域转换为时域表达的姿态信息的适应值函数为？=|丫_(3- Y com |true-Θ com 〇
[0008] 在本发明中，粒子群优化单元(3)进行的滤波参数优化步骤为：
[0009] 步骤一，计算适应值；
[00?0] 步骤11，根据适应值函数F= I ytrue-y_卜计算出参数群中每组参数 的适应值，记为Fg; ; ytrue表示设定的横滚角，9true3表示设定的俯仰角，γ_表示四子样 算法计算出的横滚角，Θ。?表示四子样算法计算出的俯仰角；
[0011] 步骤12，根据适应值函数F= | γ true- γ _ | +1 9true3-0_ |计算出自身历史最优参数 群中的每组参数PB1的适应值，记为;
[0012] 步骤13,根据适应值函数F= | γ true- γ _ | +1 |计算出自身历史最优参数 组GB的适应值，记为FGB;
[0013]步骤二，更新最优参数群中的每组参数；
[0014] 将所述的^丨与所述的进行比较，更新最优参数 群中的每组参数：
[0015] 步骤三，更新最优参数组；
[0016] 将所述的与所述的FCB进行比较， 更新最优参数 组：
[0017] 步骤四，更新调整步长；
[0018] 根据公式V/+1 = < +(：?1(尸/>; -i//) + c2/;(g/r' 更新参数调整步长；ci和C2为 固定学习因子，r#Pr2为随机学习因子，v/ +1St+l迭代次数的参数的调整步长。
[0019] 步骤五，更新参数群中每组参数；
[0020] 根据公式丨更新参数群中每组参数，$+1为七+1迭代次数的参数群中每 组参数。
[0021] 经步骤五处理后，需要检验更新后的参数<+1是否在参数的可行域D内，如果不在， 则继续执行步骤四和步骤五，直到更新后的参数￥ +1在可行域D内；然后，还需检验更新后的 参数<+1是否满足G-FIR滤波器的通带截止频率小于阻带截止频率，如果不满足，则继续执 行步骤四和步骤五，直到更新后的参数￥ +1满足此约束条件。
[0022]步骤六，是否达到最大迭代次数；
[0023] 检验迭代次数是否到达最大迭代次数Itermax，如果没有达到最大，则返回步骤一， 进入下一次迭代;如果达到最大迭代次数则终止迭代。
[0024] 本发明设计的能够提高捷联惯导系统姿态精度的预处理最优FIR滤波器的优点在 于：
[0025] ①本发明设计的G-FIR滤波器是从软件方面提高捷联惯导系统姿态精度，它的成 本远远低于一般从硬件方面提高捷联惯导系统姿态精度。
[0026] ②相比于常见的通过姿态算法的改进提高捷联惯导系统姿态精度，本发明的G-FIR滤波器的设计难度更低，使用方法时更加简便。
[0027] ③本发明设计的G-FIR滤波器的参数采用粒子群优化算法进行优化，大大减少了 人工推算的时间。
【附图说明】
[0028] 图1是传统捷联式惯导系统原理的简图。
[0029] 图2是本发明设计的能够提高捷联惯导系统姿态精度的预处理最优FIR滤波器的 结构框图。
[0030] 图3是G-FIR滤波器的最优参数与原FIR滤波器的横滚角对比图。
[0031]图4是G-FIR滤波器的最优参数与原FIR滤波器的俯仰角对比图。
【具体实施方式】
[0032]下面将结合附图和实施例对本发明做进一步的详细说明。
[0033] 参见图2所示，本发明的目的是设计一种提高捷联惯导系统姿态精度的预处理最 优FIR滤波器(简称为G-FIR滤波器），该G-FIR滤波器包括有原FIR滤波器单元1、姿态误差获 取单元2和粒子群优化单元3。姿态误差获取单元(2)对经原FIR滤波器单元（1)过滤后的角 速度和加速度信息依据四子样算法获得姿态的等效旋转矢量，然后对姿态的等效旋转矢量 通过粒子群优化单元(3)的处理得到实时更新后的滤波器最优参数及最小适应值。将本发 明G-FIR滤波器应用到SINS中，是先对激光陀螺输出信号进行处理，再将处理后的信号应用 于姿态解算，从而降低噪声对SINS的姿态的影响。
[0034] 在图1所示的捷联式惯导系统中，将本发明设计的G-FIR滤波器置于比力坐标变换 单元与指令解算单元之间。
[0035] 加速度计组合输出的加速度信息记为f，激光陀螺组合输出的角速度信息记为ω。 采样时间记为Time，在本发明中，采样时间Time设置为1200s，采样时间间隔为0.005s，SPIs 时间里将采集到200个加速度信息f和角速度信息ω。因此，将采样时间Time里加速度计组 合输出的加速度信息集记为Acc = {f i，f2，f3，…，fp}。将采样时间Time里激光陀螺组合输出 的角速度信息集记为Gyro Μω^ω^ω^,ωρ} 1表示采样个数的标识号。
[0036] 原FIR滤波器单元1
[0037] FIR滤波器的模型请参考清华大学出版社于2003年8月第2版，作者胡广书《数字信 号处理理论、算法与实现》第296页，
，H(z)为FIR滤波器的转移函数，z为复 变量，g卩离散的加速度信息或者角速度信息，bn为FIR滤波器的系数，Μ为求和元素，η为求和 指标。
[0038]在本发明中，原FIR滤波器单元1对接收到的加速度信息集 [0039] Acc = {f i，f 2，f 3，…，f p }进行滤波处理，得到滤波后的加速度信息
[0041 ]在本发明中，原FIR滤波器单元1对接收到的角速度信息集
[0042] Gyro={ ωι，ω2, ω3，···，ωΡ}进行滤波处理，得到滤波后的角速度信息
[0044] 在本发明中，应用FIR滤波方式是为了滤除加速度信息和角速度信息中的高频噪 声，一般地，超过50Hz频率的噪声被滤除。
[0045] 姿态误差获取单元2
[0046]姿态误差获取单元2第一方面采用四子样算法求解姿态的等效旋转矢量，第二方 面将等效旋转矢量从频域转换为时域表达的姿态信息。
[0047]在本发明中，等效旋转矢量中的圆锥误差幅值δ φ 3为：
[0049] Cs为圆锥误差系数；
[0050] s为采样间隔长度；
[0051 ] Ω为振动频率；
[0052] h为四子样算法中的姿态更新周期；
[0053] Η( Ω )为原FIR滤波器的幅频函数；
[0054] Φ(Ω)为未加载G-FIR滤波器时的圆锥漂移率；
[0055] //(Ω)2 X 为加载G-FIR滤波器时的圆锥漂移率；
[0056] ds( Ω Xh)为以Ω Xh为自变量的相关函数。
[0057] 在本发明中，由于等效旋转矢量中的圆锥误差幅值δ c})s为频域中圆锥误差幅值， 应用公式（1)能够减小G-FIR滤波器的幅频特性对圆锥误差造成的影响，由于圆锥误差是姿 态误差的一个重要组成部分，因此G-FIR滤波器的影响能够间接地反映在姿态误差上。公式 (1)是一个频域的表达式，但是在工程上圆锥误差或者姿态误差的计算都是在时域中进行 的，因此为了计算方便选用的适应值函数为F=| 丫1；1^-丫。。111| + |91；1^-9。。111|，ytrim表示设定 的横滚角，0true3表示设定的俯仰角，表示四子样算法计算出的横滚角，θ_表示四子样 算法计算出的俯仰角;在本发明中，γ true为1度，9true为5度。
[0058]在本发明中，四子样算法请参考科学出版社于2006年5月第一版，作者秦永元，《惯 性导航》第311-315页。
[0059] 在本发