一种多传感器融合的陀螺漂移滤波系统及方法
【专利摘要】本发明公开一种多传感器融合的陀螺漂移滤波系统，信号初始处理模块对加速度计信号和陀螺信号进行初始处理，并将经初始处理的加速度计信号输出至所述传感器信号融合模块以及将经初始处理的陀螺信号输出至信号闭环处理模块；信号闭环处理模块用于对输入的经所述初始处理的陀螺信号进行闭环处理后并将经闭环处理的陀螺信号输出至传感器信号融合模块；传感器信号融合模块将经所述初始处理的加速度计信号与经闭环处理的陀螺信号融合后，输出去除陀螺漂移后的速度和位置信号。同时公开了相应的陀螺漂移滤波方法。本发明降低了陀螺滤波方法结构的复杂性，减少了计算量，提高了准确性和可靠性；同时滤波方法闭环可控，保证其工程化的实现。
【专利说明】一种多传感器融合的陀螺漂移滤波系统及方法
【技术领域】
[0001] 本发明属于陀螺信号处理领域，具体涉及一种陀螺漂移滤波系统及方法。
【背景技术】
[0002] 陀螺是一种敏感角速度的惯性组件，并且随着当今科技的发展正在向小型化、高 可靠性，高精度发展，但是对陀螺精度要求越高成本也成倍的增加，因此人们更多的在成本 和精度之间折中，这种折中带来的就是陀螺的随机漂移较大，也是制约其在工程应用当中 最大的问题。
[0003] 陀螺信号的输出基本上包括以下几个变量：陀螺输出的角速度信号、陀螺的常值 误差、陀螺的随机游走及零均值的陀螺白噪声。由上可知，陀螺输出信号误差主要包括常值 误差和随机漂移两大类。常值误差可以通过建立较为精确的模型进行有效补偿，而随机漂 移包括量化噪声、角度随机游走、角速率随机游走、零偏不稳定和速率斜坡噪声，且受外部 环境等多种不确定因素的影响，具有非线性和慢时变性，无法建立其精确的模型，因此对随 机噪声的滤波成为了当前研究的重点。
[0004] 目前常用的陀螺滤波方法主要包括：Kalman滤波方法、小波变换滤波方法、ALLAN 方差补偿技术、压缩感知滤波方法等，概要介绍如下：
[0005] Kalman滤波是一种递推线性最小方差估计，只通过前一个时刻的估计值和现在的 测量值来计算现在的状态估计值。它可以实现最小均方估计误差意义下的随机信号的最 优线性滤波。但是该方法的缺点是需要建立噪声的AR(自回归）模型，根据模型参数来设 定方法的初始参数，而对于陀螺随机漂移是非线性的、慢时便的，因此很难建立起准确的模 型，只能靠方法本身进行弥补。
[0006] 小波变换阈值滤波是根据信号和噪声在不同尺度上小波变换的不同形态表现，构 造出相应的规则，对信号和噪声的小波变换系数进行处理，处理的实质在于减小以至完全 剔除由噪声产生的系数，同时最大限度的保留有用信号对应的小波系数。小波阈值去噪过 程分为三步：第一步对带噪信号进行小波分解，得到各层分解后的逼近系数和细节系数； 第二步对各层细节系数进行阈值量化处理；第三步对处理后的系数进行小波逆变换，得到 去噪信号。目前来说，小波函数和小波分解层数的选取还没有一个明确的理论作指导，一般 仅凭借经验和反复试验选择。而且相对于其他的滤波方法，小波变换计算量较为复杂，工程 实现难度较大。
[0007] Allan方差法是利用不同相关时间内方差所表现出的不同特征来描述各种噪声源 的。其最大优点是可以简便地分离、辨识出陀螺随机误差的各项分量，并确定各误差项对 总误差的贡献。但是对于该方法需要离线利用大量数据建立噪声模型，然后通过Allan对 模型进行分析，而且计算量相对较大，不利于工程实现。
[0008] 压缩感知滤波算法是通过辨识信号的稀疏特性，在远小于Nyquist采样率的条件 下，用随机采样获取信号的随机样本，然后通过非线性重建算法完美重建信号。目前对于陀 螺滤波方面的应用主要基于小波基础之上，由于无噪声信号通过Mallat (多尺度分析）算 法得到的小波系数应该是稀疏的，但当信号中存在噪声时小波系数的稀疏性将大大降低， 因此可以通过压缩感知理论可以恢复小波系数的稀疏性，已达到降噪的目的。该方法理论 上可以有效提高小波变换的滤波精度，但是由于其理论较为复杂，而且计算量较大，很难工 程实现。
[0009] 综上可知，现有技术中大多数滤波方法均需要建立陀螺输出噪声的模型，利用其 模型参数进行分析计算。但是陀螺噪声是非线性的、时变的，因此增加了模型建立的难度， 引入了模型误差，降低了现有方法的准确性；现有算法的模型复杂且计算中存在较多的矩 阵运算，因此在实时性方面较差，且计算过于复杂，难以工程实现；现有算法的数据分析及 滤波多采用单一的陀螺数据作为滤波系统的输入，数据特征单一，无法有效保证结果的可 靠性和准确性；现有算法多采用开环滤波算法，系统无法感知滤波输出效果的好坏，因此无 法对其进行进一步的修正。
【发明内容】
[0010] 本发明为了解决上述至少一个问题和/或不足，并提供下述至少一种优点，相应 的提供了一种多传感器融合的陀螺漂移滤波系统及方法。
[0011] 一方面，本发明提供了一种多传感器融合的陀螺漂移滤波系统，包括信号初始处 理模块、信号闭环处理模块、传感器信号融合模块；
[0012] 所述加速度传感器用于敏感并向所述信号初始处理模块输出加速度计信号；所述 陀螺传感器用于敏感并向所述信号初始处理模块输出陀螺信号；
[0013] 所述信号初始处理模块对所述加速度计信号和所述陀螺信号进行初始处理后，并 将经所述初始处理的加速度计信号输出至所述传感器信号融合模块以及将经所述初始处 理的陀螺信号输出至所述信号闭环处理模块；
[0014] 所述信号闭环处理模块用于对输入的经所述初始处理的陀螺信号进行闭环处理 后并将经所述闭环处理的陀螺信号输出至所述传感器信号融合模块；
[0015] 所述传感器信号融合模块将经所述初始处理的加速度计信号与经所述闭环处理 的陀螺信号融合后，输出去除陀螺漂移后的速度和位置信号。
[0016] 进一步的，所述信号初始处理模块对输入的所述陀螺信号进行陀螺固定漂移校 正，并采用软件低通滤波滤除输入的所述加速度计信号和所述陀螺信号中的高频震动噪 声。
[0017] 进一步的，所述信号闭环处理模块用于将经所述初始处理后的陀螺信号进行闭环 跟踪处理，将设定在阀值以下的陀螺漂移信号进行剔除，并将经所述闭环跟踪处理后的陀 螺信号和大于阀值的陀螺信号进行融合。
[0018] 进一步的，所述传感器信号融合模块采用互补算法将经所述初始处理的加速度计 信号与经所述闭环处理的陀螺信号进行融合。
[0019] 另一方面，本发明提供了一种多传感器融合的陀螺漂移滤波方法，包括以下步 骤：
[0020] 获取加速度计信号和陀螺信号；
[0021] 分别对所述加速度计信号和所述陀螺信号进行初始处理，获得经所述初始处理的 加速度计信号和经所述初始处理的陀螺信号；
[0022] 对经所述初始处理的陀螺信号进行闭环处理，获得经所述闭环处理的陀螺信号；
[0023] 将经所述初始处理的加速度计信号与经所述闭环处理的陀螺信号进行信号融合， 获得去除陀螺漂移后的速度和位置信号。
[0024] 进一步的，所述初始处理是对所述陀螺信号进行陀螺固定漂移校正，以及采用软 件低通滤波滤除所述加速度计信号和所述陀螺信号中的高频震动噪声。
[0025] 进一步的，所述闭环处理是将经所述初始处理后的陀螺信号进行闭环跟踪处理， 将设定在阀值以下的陀螺漂移信号进行剔除，然后将经所述闭环跟踪处理后的陀螺信号和 大于阀值的陀螺信号进行融合。
[0026] 进一步的，所述信号融合是采用互补算法将经所述初始处理的加速度计信号与经 所述闭环处理的陀螺信号进行融合。
[0027] 进一步的，所述信号融合包括：将经所述初始处理的加速度计信号进行定时积分， 将所述定时积分结果与经所述闭环处理的陀螺信号用互补方法进行融合，得到速度变量；
[0028] 将经所述初始处理的加速度计信号进行定时二次积分，将所述定时二次积分结果 与经所述闭环处理的陀螺信号用互补算法进行融合，得到位置变量。
[0029] 针对现有滤波方法大多需要建立陀螺输出噪声的模型，进而利用其模型参数进行 分析计算，本发明采用的滤波方法摆脱了对与噪声模型的建立的依赖，从而有效避免了对 非线性系统及时变系统的处理，而且保证了滤波方法的有效性，降低了方法实现的难度。
[0030] 针对现有滤波方法模型复杂，且存在大量的矩阵运算，因此在算法实时性方面较 差，且难以工程实现。本发明针对上述问题对滤波方法中的计算进行了优化，降低了算法结 构的复杂度，减少了计算的难度，提高了算法的实时性。
[0031] 针对现有滤波方法数据源较为单一仅为陀螺，无法保证数据在复杂情况下的可靠 性和准确性，本发明针对上述问题采用了多传感器融合的技术，引入了加速度计作为陀螺 滤波方法的参考输入，通过互补融合算法将两者数据进行融合计算以达到数据滤波的可靠 性和准确性。
[0032] 针对现有滤波方法大多采用开环滤波控制方法，滤波效果的好坏系统无法感知， 因此不存在自我优化的过程，本发明采用了闭环滤波方法，根据输出的陀螺漂移量进行闭 环变参数控制，保证了滤波效果的可控和系统的稳定性。
[0033] 概括地说本发明降低了陀螺滤波方法结构的复杂性，减少了计算量，提高了准确 性和可靠性；同时滤波方法闭环可控，保证其工程化的实现。
【专利附图】
【附图说明】
[0034] 图1为多传感器融合的陀螺漂移滤波系统的结构示意图；
[0035] 图2为信号初始处理模块的软件流程图；
[0036] 图3为信号初始处理模块滤波前后的数据仿真效果图；
[0037] 图4为信号闭环处理模块的闭环处理示意图；
[0038] 图5为传感器信号融合模块信号融合的结构示意图；
[0039] 图6为传感器信号融合模块的融合仿真效果图。
【具体实施方式】
[0040] 在下文中，参照附图来更充分地描述本发明的实施例，附图中示出了本发明的实 施例。然而，本发明的实施例可以以许多不同的形式来实施，而不应该限于在此列出的实施 例。相反，提供这些示例性实施例，使得本公开是彻底的，并且将本发明的范围充分传达给 本领域技术人员，在附图中，为了清晰起见，可以夸大层和区域的尺寸和相对尺寸。将省略 对已知的功能和结构的详细描述以避免模糊实施例的主体问题。附图中相同的标号用来表 示相同的元件。
[0041] 除非另外定义，否则所有在此使用的术语（包括技术术语和科学术语）具有与本 发明所述领域的普通技术人员所通常理解的意思相同的意思。应该进一步理解，除非这里 明确定义，否则术语（如在通用的字典中定义的术语）应该被解释为具有与在有关技术的 上下文中的意思一致的意思，并不应该以理想化的或过于正式的意义来解释它们的意思。
[0042] 如下结合图1?图6对本发明多传感器融合的陀螺漂移滤波系统及方法进行说 明。图1为多传感器融合的陀螺漂移滤波系统的结构示意图；图2为信号初始处理模块的 软件流程图；图3为信号初始处理模块滤波前后的数据仿真效果图；图4为信号闭环处理 模块的闭环处理示意图；图5为传感器信号融合模块信号融合的结构示意图；图6为传感 器信号融合模块的融合仿真效果图。
[0043] 参考图1，本发明提供了一种多传感器融合的陀螺漂移滤波方法，该方法由信号初 始处理模块、信号闭环处理模块、传感器信号融合模块组成。三者连接关系如下：加速度传 感器输出的原始信号①和陀螺传感器输出的原始信号②连接到信号初始处理模块作为其 输入信号；信号初始处理模块的输出信号初始处理后的陀螺信号③和初始处理后的加速度 计信号④分别连接到信号闭环处理模块和传感器信号融合模块；信号闭环处理模块的输出 信号即闭环处理后的陀螺信号⑤，连接到传感器融合模块和信号④在本模块进行融合；传 感器融合模块的输出⑥为去除陀螺漂移后的速度和位置信号。
[0044] 信号初始处理模块的软件流程图如图2所示，其滤波前后的数据仿真效果图如图 3所示。信号初始处理模块的具体实施方法和步骤如下 ：
[0045] 1、定义滑动滤波数组A[n]，并初始化为零；其中η为采样时刻；
[0046] 2、按照队列的方式对数组Α[η]进行更新，即A[n] = Α[η-1]，Α[η-1]= Α[η-2]......Α[2] = A[l]，A[l] = newData。其中，newData 为图 1 中的信号①或者②；
[0047] 3、计算数组A[n]的均值和方差；
[0048] 4、判断下一次最新的输入数据newData与均值和方差的关系，据此决定滑动滤波 数组更新的方法。
[0049] 其中，判断条件：newData_mean(A[n]) < k*std(A[n])
[0050] k为滑动滤波系数;mean (A [η])是求A [η]的平均值；std (A [η])是求A [η]的标准 偏差。
[0051] 具体执行：
[0052] 上述条件成立则执行以下操作A[n] = A[n-1]，A[n-1] = Α[η-2]......Α[2]= Α[1]，A[l] = newData ;
[0053] 上述条件不成立则执行下述操作Α[η] = Α[η-1]，Α[η-1] = Α[η-2]......Α[2]= Α[1],Α[1] = newData*n+newDataLast*(1-η)；
[0054] 其中，newDataLast为上次输入的最新数据；
[0055] 5、计算A[n]的均值，作为信号处理模块的输出信号如图1中的③或④。
[0056] 6、重复步骤3到步骤6。
[0057] 上述步骤4中条件不成立时，A[l]数据更新采用了低通滤波算法，滤波系数η的 推导过程如下：
[0058]
【权利要求】
1. 一种多传感器融合的陀螺漂移滤波系统，其特征在于，包括信号初始处理模块、信号 闭环处理模块、传感器信号融合模块； 所述加速度传感器用于敏感并向所述信号初始处理模块输出加速度计信号；所述陀螺 传感器用于敏感并向所述信号初始处理模块输出陀螺信号； 所述信号初始处理模块对所述加速度计信号和所述陀螺信号进行初始处理后，并将经 所述初始处理的加速度计信号输出至所述传感器信号融合模块以及将经所述初始处理的 陀螺信号输出至所述信号闭环处理模块； 所述信号闭环处理模块用于对输入的经所述初始处理的陀螺信号进行闭环处理后并 将经所述闭环处理的陀螺信号输出至所述传感器信号融合模块； 所述传感器信号融合模块将经所述初始处理的加速度计信号与经所述闭环处理的陀 螺信号融合后，输出去除陀螺漂移后的速度和位置信号。
2. 如权利要求1所述系统，其特征在于，所述信号初始处理模块对输入的所述陀螺信 号进行陀螺固定漂移校正，并采用软件低通滤波滤除输入的所述加速度计信号和所述陀螺 信号中的高频震动噪声。
3. 如权利要求1所述系统，其特征在于，所述信号闭环处理模块用于将经所述初始处 理后的陀螺信号进行闭环跟踪处理，将设定在阀值以下的陀螺漂移信号进行剔除，并将经 所述闭环跟踪处理后的陀螺信号和大于阀值的陀螺信号进行融合。
4. 如权利要求1所述系统，其特征在于：所述传感器信号融合模块采用互补算法将经 所述初始处理的加速度计信号与经所述闭环处理的陀螺信号进行融合。
5. -种多传感器融合的陀螺漂移滤波方法，其特征在于，包括以下步骤： 获取加速度计信号和陀螺信号； 分别对所述加速度计信号和所述陀螺信号进行初始处理，获得经所述初始处理的加速 度计信号和经所述初始处理的陀螺信号； 对经所述初始处理的陀螺信号进行闭环处理，获得经所述闭环处理的陀螺信号； 将经所述初始处理的加速度计信号与经所述闭环处理的陀螺信号进行信号融合，获得 去除陀螺漂移后的速度和位置信号。
6. 如权利要求5所述的滤波方法，其特征在于，所述初始处理是对所述陀螺信号进行 陀螺固定漂移校正，以及采用软件低通滤波滤除所述加速度计信号和所述陀螺信号中的高 频震动噪声。
7. 如权利要求5所述的滤波方法，其特征在于，所述闭环处理是将经所述初始处理后 的陀螺信号进行闭环跟踪处理，将设定在阀值以下的陀螺漂移信号进行剔除，然后将经所 述闭环跟踪处理后的陀螺信号和大于阀值的陀螺信号进行融合。
8. 如权利要求5所述的滤波方法，其特征在于，所述信号融合是采用互补算法将经所 述初始处理的加速度计信号与经所述闭环处理的陀螺信号进行融合。
9. 如权利要求5或8所述的滤波方法，其特征在于，所述信号融合包括 将经所述初始处理的加速度计信号进行定时积分，将所述定时积分结果与经所述闭环 处理的陀螺信号用互补方法进行融合，得到速度变量； 将经所述初始处理的加速度计信号进行定时二次积分，将所述定时二次积分结果与经 所述闭环处理的陀螺信号用互补算法进行融合，得到位置变量。
【文档编号】G01C25/00GK104280047SQ201410458063
【公开日】2015年1月14日 申请日期:2014年9月10日 优先权日:2014年9月10日
【发明者】胡红亮, 郗小鹏, 张勇, 陈立鹏, 张志军, 韩旭, 李创 申请人:天津航天中为数据系统科技有限公司