技术领域
[0001] 本
发明属于数字
图像处理领域，涉及滤波方法，特指一种用于IMU多
传感器数据融合的滤波方法。
背景技术
[0002] 随着微
机电系统技术的发展，为了满足广泛应用的需求，采用微机械
陀螺仪、
加速度计、磁
力计等构造微小型低成本的航姿参考系统。同时微小型无人机具有成本低、隐蔽性好、生存能力强等特点，近年来在军事、民用等相关领域中得到越来越多的应用。
姿态测量是无人机实现姿态控制的前提，也是
导航系统中不可分割的重要组成部分。无人机
飞行器姿态主要包括姿态解算和飞行控制两部分，其中，姿态解算是飞行器设计的核心和重点。
[0003] 姿态解算需要依靠惯性测量器件，加速度计测量载体的姿态
角主要利用地球重力场，当载体偏转时，在各方向轴上受的重力不同，容易受其他力矩的影响，但具有较好的静态特性，即低频段的动态响应好；陀螺仪以较短时间间隔读取测量载体
坐标系下
机体在各方向轴上的
角速度，存在
温度漂移和零点漂移，随着时间累积其通过积分计算的角速度信息会有较大的姿态误差，但具有较好的高频段动态特性。因此研究具有高可靠性、高
精度的IMU多传感器数据融合
算法一直是国内外学者关注的焦点。
[0004] 航姿参考系统利用惯性器件来测量载体姿态角，通常采用多种传感器组合测量，输出含有噪声的姿态信息，经信息滤波筛选之后进行姿态解算，由于各传感器的特点如动态响应速度、累计误差等不同，需要采用相关算法对每个传感器的数据
信号进行融合，提高测量精度和系统的动态特性。
[0005] 常用的数据融合算法有：卡尔曼滤波方法、
扩展卡尔曼滤波方法、粒子滤波姿态估计法、
梯度下降法、互补滤波方法等。卡尔曼滤波方法应用于惯性导航中取得了较大的成功，但该算法较复杂、运算量大，需要较高的
硬件成本，电量耗损大。扩展卡尔曼滤波方法是应用最广泛的一种非线性系统滤波方法，但存在线性化误差、截断误差及发散问题。粒子滤波姿态估计法提高了滤波精度，解决了系统非线性和非白噪声对姿态解算的影响，但计算量较大，不适用于低成本的航姿系统。梯度下降法在姿态解算中性能优于卡尔曼滤波，计算量少，对初始点要求低，但收敛速度较慢。互补滤波方法原理简单、计算量小，通过平率分辨消除噪声，能较好地结合加速度计的静态精度和陀螺仪的高动态姿态数据，利用两者在频域角度的互补特性，提高姿态测量的精度和动态响应，以减少测量和估计的误差，但其高通
滤波器和
低通滤波器的转接
频率难以确定。
发明内容
[0006] 针对现有方法的不足，本发明提出一种用于IMU多传感器数据融合的滤波方法。其创新性在于各传感器信息的动态响应速度、累计误差不同，解算出的姿态信息具有一定的互补性，通过对PI控制参数的引入和设定，设计含有
负反馈信息的二阶互补滤波算法对各传感器信息融合，提高系统的测量精度和动态性能。
[0007] 为实现上述技术目的，本发明的技术方案是，
[0008] 一种用于IMU多传感器数据融合的滤波方法，包括以下步骤：
[0009] (1)利用陀螺仪、加速度计组成的IMU惯性检测系统获取飞行器的姿态信息，对各传感器的
输出信号进行表示，建立传感器输出信号的数学模型表达式；
[0010] 陀螺仪的误差主要由随机漂移和噪声构成，设ywt为陀螺仪的输出信号，wt为真实角速度值，vwt为测量噪声，b为漂移量，则数学模型可表示为：
[0011] ywt＝wt+vwt+b (1)
[0012] 同理，加速度计的输出信号为yat，载体运动的加速度为at，地球
重力加速度为gt，其中 为旋转矩阵，vat表示高斯白噪声，则加速度计的数学模型可表示为：
[0013] yat＝at-gt+vat (2)
[0014] (2)利用滑动均值滤波去除步骤(1)数学模型表达式中的测量噪声和高斯白噪声以及干扰信息，提高系统的精度；其中滑动均值滤波器公式为：
[0015]
[0016] 其中yi为各传感器输出的原始数据，w为滑动均值滤波器的输出结果，n为滑动均值滤波器阶数。
[0017] 由公式(3)可以看出，每次参与滤波的传感器数据始终为当前传感器最新
采样的n个数据，滑动均值滤波器只涉及线性运算，可以保证滤波的实时性。
[0018] (3)基于四元素向量法，将各传感器去噪后的输出数据转化为四元素，建立载体坐标系与参考坐标系之间的空间角
位置关系，利用变换矩阵得到姿态角的四元素表达式，分别进行加速度计和陀螺仪的旋转加速度和旋转角速度姿态角解算。
[0019] a.确定两坐标系间的关系
[0020] 飞行器的姿态由
俯仰角θ、
横滚角φ、
偏航角ψ描述，选取东北天坐标系(g系)为参考坐标系，即以坐标系载体的
重心为坐标原点，Xg指向正北方向，Yg指向正东方向，Zg轴与Xg轴和Yg轴构成的平面垂直，指向天空；载体坐标系(b系)以载体作为参考，坐标原点为载体重心，Xb为载体横轴，Yb为载体纵轴，Zb轴垂直于Xb轴和Yb轴构成的平面，XbYbZb构成右手坐标系。
[0021] 参考坐标系被认为是固定不变的，载体坐标系相对于参考坐标系的方位变化，相对变化情况即为待求的载体姿态，常用姿态角表示，两坐标系之间的关系可以用旋转矩阵表示为：
[0022]
[0023] b.将各传感器经步骤(2)去噪处理后的输出数据转化为四元素表示形式。
[0024] 由式(4)可知，当俯仰角θ为正负90°时，欧拉角计算中会出现奇异点，导致数值不稳定，因此可将传感器采集到的姿态数据经过去噪处理之后，转化为四元素表示方法，其表达式为：
[0025]
[0026] 其中q0、q1、q2、q3均为实数，i、j、k为相互
正交的单位向量。
[0027] c.将四元素向量规范化处理之后，变换矩阵可表示为：
[0028]
[0029] d.用四元素法中的参数推导得到姿态角表达式为：
[0030]
[0031] 基于四元素法进行姿态解算利用了加速度计和陀螺仪所测数据，加速度计主要是测量载体坐标系下旋转加速度，陀螺仪主要测量载体坐标系下的旋转角速度。以加速度计在无加速状态(即静止或匀速运动)时的输出结果计算IMU惯性检测系统的初始欧拉角，联立(4)(5)(6)式求出初始四元素，对陀螺仪的角速度进行等时间间隔定时采样得到IMU惯性检测系统角增量，结合四元素微分方程，根据上一时刻姿态四元素求解当前时刻更新的姿态四元素，代入(7)式完成姿态解算。
[0032] 四元素微分方程如下式所示：
[0033]
[0034] 其中Δθx、Δθy、Δθz分别为定时采样时三个轴的角增量，I为单位矩阵，由th时刻定时采样的角增量，结合四元素微分方程式可求得th+1时刻的四元素Q(th+1)。
[0035] (4)设计二阶互补滤波器对姿态解算得到的加速度计姿态角进行低通滤波，对陀螺仪姿态角进行高通滤波，并实现各传感器数据融合，最后输出姿态角。
[0036] 以俯仰角θ为例，θa为姿态解算得到的加速度计俯仰角，θw为姿态解算得到的陀螺仪俯仰角，在二阶互补滤波器中的低通滤波部分，通过姿态解算得到的加速度计俯仰角θa与前一次滤波更新后姿态角中的俯仰角θ之间存在的误差值θerr作为二阶互补滤波器的二次输入值即反馈角度作为新的输入值形成负反馈，实现二阶互补滤波的误差修正，二阶互补滤波器各部分的具体表达式为：
[0037]
[0038] 上式中，x1和x2为中间变量，Kp为比例系数变量，KI为积分系数变量，τ为滤波器时间常数，1/τ为滤波器截止频率，dt为滤波器采样时间，θt为最终输出的俯仰角。
[0039] 同理，利用二阶互补滤波器对横滚角φw和偏航角ψw姿态数据融合，得到最终输出的横滚角φt和偏航角ψt。
[0040]
[0041] 其中，Kp为比例系数变量，KI为积分系数变量的确定方法有很多，主要有临界比例法、反映曲线法、衰减法等，一般先确定比例系数Kp，再确定积分系数KI。具体方法为首先设定积分系数KI为0，将二阶互补滤波器变为纯比例调节，然后将Kp的值由零开始逐步增大，直至二阶互补滤波器出现振荡，再反过来调节将Kp值逐步减小，直至二阶互补滤波器振荡消失，记录当前的比例系数值Kp；比例系数值Kp确定之后，设定一个较大的积分时间常数Ti(一般设置为允许输出最大值的60％至70％。)，然后逐渐减小，直至二阶互补滤波器出现振荡，再反过来增大Ti，直至二阶互补滤波器振荡消失，记录当前的Ti值，积分系数Ki＝Kp/Ti。
[0042] 本发明通过对PI控制参数的引入和设定，设计含有负反馈的二阶互补滤波算法对各传感器信息融合，提高系统的测量精度和动态性能。
附图说明
[0043] 图1为本发明的原理图
[0044] 图2为两坐标系角度转换关系示意图
[0045] 图3为二阶互补滤波器的原理图
具体实施方式
[0046] 为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步地详细描述。
[0047] 参照图1本发明提出了一种用于IMU多传感器数据融合的滤波方法，首先对数据进行采集，由陀螺仪、加速度计等多个传感器组成的IMU惯性检测系统获取飞行器的姿态数据，并建立传感器输出信号的数学模型表达式；其次对各传感器的输出数据进行处理，主要包括利用滑动均值滤波去除干扰信息和噪声，再基于四元素法解算各传感器的旋转角度；然后利用
高通滤波器和低通滤波器相结合，在低通滤波部分加入PI控制参数，引入比例系数变量Kp和积分系数变量KI设计二阶互补滤波器的误差修正负反馈，实现各传感器数据融合，最后输出姿态角。下面对本发明的各个步骤作进一步的详细说明。
[0048] (1)以三轴陀螺仪和三轴加速度计传感器为例，首先建立传感器的数学模型。
[0049] 陀螺仪的误差主要由随机漂移和噪声构成，设ywt为陀螺仪输出信号，wt为真实角速度值，vwt为测量噪声，b为漂移量，则数学模型可表示为：
[0050] ywt＝wt+vwt+b (1)
[0051] 同理，加速度计的输出信号为yat，载体运动的加速度为at，地球重力加速度为gt，其中 为旋转矩阵，vat表示高斯白噪声，则加速度计的数学模型可表示为：
[0052] yat＝at-gt+vat (2)
[0053] (2)如图1所示，首先对各传感器采集的信号建立数学模型表示式，输出值主要包括真实测量值、漂移量和干扰噪声。加速度传感器可以将物体的加速度信息转换为
数字量的传感器，其输出信号主要代表了直线的加速度大小和重力方向夹角，以低频信号为主，陀螺仪传感器输出信号具有高动态相应特点，输出信号中含有噪声，主要为高斯白噪声，在姿态解算之前，可以利用低通滤波器或均值滤波器对传感器输出信息去噪。
[0054] 由传感器的误差分析和数学建模表达式可知，无论是加速度计还是陀螺仪，输出值中都存在着不同程度的测量误差，为了剥离
干扰信号和噪声的影响，提高系统的精度，对传感器输出信息进行
数据处理。本
实施例采用滑动均值滤波的方法，其滤波器公式为：
[0055]
[0056] 其中：yi为各传感器输出的原始数据，w为滑动均值滤波器的输出结果，n为滑动滤波器阶数。
[0057] 由公式(3)可以看出，每次参与滤波的传感器数据始终为当前传感器最新采样的n个数据，滑动滤波器只涉及线性运算，可以保证滤波的实时性。
[0058] (3)传感器输出信号去除噪声之后，基于四元素法对其进行姿态角解算。
[0059] 首先建立坐标系关系，如图2所示。物体的运动都是相对的，计算载体的位置和姿态，需要
选定参考坐标系，以参考坐标系为基准，研究载体坐标系与参考坐标系的相对运动。
[0060] 飞行器的姿态由俯仰角θ、横滚角φ、偏航角ψ描述，选取东北天坐标系(g系)为参考坐标系，即以坐标系载体的重心为坐标原点，Xg指向正北方向，Yg指向正东方向，Zg轴与Xg轴和Yg轴构成的平面垂直，指向天空；
[0061] 载体坐标系(b系)以载体作为参考，载体与六轴传感器固定相连，坐标原点为载体重心，Xb为载体横轴，Yb为载体纵轴，Zb轴垂直于Xb轴和Yb轴构成的平面，XbYbZb构成右手坐标系。
[0062] 参考坐标系被认为是固定不变的，载体坐标系相对于参考坐标系的方位变化，相对变化情况即为我们所求的载体姿态，常用姿态角表示，如图2所示，两坐标系之间的关系可以用旋转矩阵 表示为：
[0063]
[0064] 由式子(4)可知，当俯仰角θ为正负90°时，欧拉角计算中会出现奇异点，导致数值不稳定，因此可将传感器采集到的姿态数据经过去噪处理之后，转化为四元素表示方法，其表达式为：
[0065]
[0066] 其中q0、q1、q2、q3均为实数，i、j、k为相互正交的单位向量。
[0067] 将四元素向量规范化处理之后，变换矩阵可表示为：
[0068]
[0069] 其中四元素向量规范化处理的过程：
[0070] 进一步，可用四元素法中的参数推导得到姿态角表达式为：
[0071]
[0072] 基于四元素法进行姿态解算利用了加速度计和陀螺仪所测数据，加速度计主要是测量载体坐标系下旋转加速度，陀螺仪主要测量载体坐标系下的旋转角速度。以加速度计在无加速状态(即静止或匀速运动)时的输出结果计算IMU惯性检测系统的初始欧拉角，联立(4)(5)(6)式求出初始四元素，对陀螺仪的角速度进行等时间间隔定时采样得到IMU惯性检测系统角增量，结合四元素微分方程，根据上一时刻姿态四元素求解当前时刻更新的姿态四元素，代入(7)式完成姿态解算。
[0073] (4)陀螺仪短时间内测量精度高，但存在漂移误差，随着时间累积，精度变差；加速度计短期精度差，但测量误差不会随着时间累积而增加。
[0074] 由互补滤波器的设计原理可知，加速度计的测量输出进行低通滤波，能有效的抑制加速度计的高频噪声，高通滤波器能很好地抑制陀螺仪输出信号的低频漂移。因此，利用两者在频率上的互补关系，设计互补滤波算法对姿态数据进行融合，提高姿态的测量精度和动态响应。
[0075] 由于互补滤波算法中的低通滤波器
阻带衰减较慢，当噪声较大时滤波效果不理想，因此在低通滤波部分加入PI参数控制，引入参数变量Kp作为比例系数和参数KI为积分系数，设计改进的互补滤波器即本实施例中提供的二阶互补滤波器。其中PI
控制器参数的工程整定方法有很多，主要有临界比例法、反映曲线法、衰减法等，一般先确定比例系数Kp，再确定积分系数KI。具体方法为首先设定积分系数KI为0，将系统变为纯比例调节，然后将Kp的值由零开始逐步增大，直至系统出现振荡，再反过来调节将Kp值逐步减小，直至系统振荡消失，记录当前的比例系数值Kp；比例系数值确定之后，设定一个较大的积分时间常数Ti(一般设置为允许输出最大值的60％至70％)，然后逐渐减小，直至系统出现振荡，再反过来增大Ti，直至系统振荡消失，记录当前的Ti值，积分系数Ki＝Kp/Ti。
[0076] 以俯仰角θ为例，θa为加速度计数据处理后输出的俯仰角，θw为陀螺仪数据处理后输出的俯仰角，如图3所示。以x1和x2为中间变量，对低通滤波部分进行了改进，通过加速度计姿态解算后的俯仰角θa与前一次滤波更新后的俯仰角θ之间存在的误差值θerr，然后将误差值作为改进的互补滤波器的二次输入值即反馈角度作为新的输入值形成负反馈，实现二阶互补滤波的误差修正。
[0077] 由图3可知，改进后的互补滤波器各部分的具体表达式为：
[0078]
[0079] 上式中，τ为滤波器时间常数，1/τ为滤波器截止频率，dt为滤波器采样时间，θt为最终输出的俯仰角。
[0080] 同理，利用二阶互补滤波器对横滚角φw和偏航角ψw姿态数据融合，得到最终输出的横滚角φt和偏航角ψt。
[0081]
[0082] 以上包含了本发明优选实施例的说明，这是为了详细说明本发明的技术特征，并不是想要将发明内容限制在实施例所描述的具体形式中，依据本发明内容主旨进行的其他
修改和变型也受本
专利保护。本发明内容的主旨是由
权利要求书所界定，而非由实施例的具体描述所界定。
