一种用于IMU多传感器数据融合的滤波方法技术

本发明专利技术提出了一种用于IMU多传感器数据融合的滤波方法，首先对数据进行采集，由陀螺仪、加速度计等多个传感器组成的IMU惯性测量单元获取飞行器的姿态数据，并建立传感器输出信息的数学模型表达式；其次对输出数据进行处理，主要包括利用滑动均值滤波去除干扰信息和噪声，再基于四元素法解算各传感器的旋转角度；然后利用高通滤波器和低通滤波器相结合，在低通滤波部分加入PI控制参数，引入比例系数变量Kp和积分系数变量KI设计二阶互补滤波器的误差修正负反馈，实现各传感器数据融合，最后输出姿态角。本发明专利技术通过对PI控制参数的引入和设定，设计含有负反馈的二阶互补滤波算法对各传感器信息融合，提高系统的测量精度和动态性能。

【技术实现步骤摘要】

本专利技术属于数字图像处理领域，涉及滤波方法，特指一种用于IMU多传感器数据融合的滤波方法。
技术介绍
随着微机电系统技术的发展，为了满足广泛应用的需求，采用微机械陀螺仪、加速度计、磁力计等构造微小型低成本的航姿参考系统。同时微小型无人机具有成本低、隐蔽性好、生存能力强等特点，近年来在军事、民用等相关领域中得到越来越多的应用。姿态测量是无人机实现姿态控制的前提，也是导航系统中不可分割的重要组成部分。无人机飞行器姿态主要包括姿态解算和飞行控制两部分，其中，姿态解算是飞行器设计的核心和重点。姿态解算需要依靠惯性测量器件，加速度计测量载体的姿态角主要利用地球重力场，当载体偏转时，在各方向轴上受的重力不同，容易受其他力矩的影响，但具有较好的静态特性，即低频段的动态响应好；陀螺仪以较短时间间隔读取测量载体坐标系下机体在各方向轴上的角速度，存在温度漂移和零点漂移，随着时间累积其通过积分计算的角速度信息会有较大的姿态误差，但具有较好的高频段动态特性。因此研究具有高可靠性、高精度的IMU多传感器数据融合算法一直是国内外学者关注的焦点。航姿参考系统利用惯性器件来测量载体姿态角，通常采用多种传感器组合测量，输出含有噪声的姿态信息，经信息滤波筛选之后进行姿态解算，由于各传感器的特点如动态响应速度、累计误差等不同，需要采用相关算法对每个传感器的数据信号进行融合，提高测量精度和系统的动态特性。常用的数据融合算法有：卡尔曼滤波方法、扩展卡尔曼滤波方法、粒子滤波姿态估计法、梯度下降法、互补滤波方法等。卡尔曼滤波方法应用于惯性导航中取得了较大的成功，但该算法较复杂、运算量大，需要较高的硬件成本，电量耗损大。扩展卡尔曼滤波方法是应用最广泛的一种非线性系统滤波方法，但存在线性化误差、截断误差及发散问题。粒子滤波姿态估计法提高了滤波精度，解决了系统非线性和非白噪声对姿态解算的影响，但计算量较大，不适用于低成本的航姿系统。梯度下降法在姿态解算中性能优于卡尔曼滤波，计算量少，对初始点要求低，但收敛速度较慢。互补滤波方法原理简单、计算量小，通过平率分辨消除噪声，能较好地结合加速度计的静态精度和陀螺仪的高动态姿态数据，利用两者在频域角度的互补特性，提高姿态测量的精度和动态响应，以减少测量和估计的误差，但其高通滤波器和低通滤波器的转接频率难以确定。
技术实现思路
针对现有方法的不足，本专利技术提出一种用于IMU多传感器数据融合的滤波方法。其创新性在于各传感器信息的动态响应速度、累计误差不同，解算出的姿态信息具有一定的互补性，通过对PI控制参数的引入和设定，设计含有负反馈信息的二阶互补滤波算法对各传感器信息融合，提高系统的测量精度和动态性能。为实现上述技术目的，本专利技术的技术方案是，一种用于IMU多传感器数据融合的滤波方法，包括以下步骤：(1)利用陀螺仪、加速度计组成的IMU惯性检测系统获取飞行器的姿态信息，对各传感器的输出信号进行表示，建立传感器输出信号的数学模型表达式；陀螺仪的误差主要由随机漂移和噪声构成，设ywt为陀螺仪的输出信号，wt为真实角速度值，vwt为测量噪声，b为漂移量，则数学模型可表示为：ywt＝wt+vwt+b(1)同理，加速度计的输出信号为yat，载体运动的加速度为at，地球重力加速度为gt，其中为旋转矩阵，vat表示高斯白噪声，则加速度计的数学模型可表示为：yat＝at-gt+vat(2)(2)利用滑动均值滤波去除步骤(1)数学模型表达式中的测量噪声和高斯白噪声以及干扰信息，提高系统的精度；其中滑动均值滤波器公式为：其中yi为各传感器输出的原始数据，w为滑动均值滤波器的输出结果，n为滑动均值滤波器阶数。由公式(3)可以看出，每次参与滤波的传感器数据始终为当前传感器最新采样的n个数据，滑动均值滤波器只涉及线性运算，可以保证滤波的实时性。(3)基于四元素向量法，将各传感器去噪后的输出数据转化为四元素，建立载体坐标系与参考坐标系之间的空间角位置关系，利用变换矩阵得到姿态角的四元素表达式，分别进行加速度计和陀螺仪的旋转加速度和旋转角速度姿态角解算。a.确定两坐标系间的关系飞行器的姿态由俯仰角θ、横滚角φ、偏航角ψ描述，选取东北天坐标系(g系)为参考坐标系，即以坐标系载体的重心为坐标原点，Xg指向正北方向，Yg指向正东方向，Zg轴与Xg轴和Yg轴构成的平面垂直，指向天空；载体坐标系(b系)以载体作为参考，坐标原点为载体重心，Xb为载体横轴，Yb为载体纵轴，Zb轴垂直于Xb轴和Yb轴构成的平面，XbYbZb构成右手坐标系。参考坐标系被认为是固定不变的，载体坐标系相对于参考坐标系的方位变化，相对变化情况即为待求的载体姿态，常用姿态角表示，两坐标系之间的关系可以用旋转矩阵表示为：b.将各传感器经步骤(2)去噪处理后的输出数据转化为四元素表示形式。由式(4)可知，当俯仰角θ为正负90°时，欧拉角计算中会出现奇异点，导致数值不稳定，因此可将传感器采集到的姿态数据经过去噪处理之后，转化为四元素表示方法，其表达式为：其中q0、q1、q2、q3均为实数，i、j、k为相互正交的单位向量。c.将四元素向量规范化处理之后，变换矩阵可表示为：d.用四元素法中的参数推导得到姿态角表达式为：基于四元素法进行姿态解算利用了加速度计和陀螺仪所测数据，加速度计主要是测量载体坐标系下旋转加速度，陀螺仪主要测量载体坐标系下的旋转角速度。以加速度计在无加速状态(即静止或匀速运动)时的输出结果计算IMU惯性检测系统的初始欧拉角，联立(4)(5)(6)式求出初始四元素，对陀螺仪的角速度进行等时间间隔定时采样得到IMU惯性检测系统角增量，结合四元素微分方程，根据上一时刻姿态四元素求解当前时刻更新的姿态四元素，代入(7)式完成姿态解算。四元素微分方程如下式所示：其中Δθx、Δθy、Δθz分别为定时采样时三个轴的角增量，I为单位矩阵，由th时刻定时采样的角增量，结合四元素微分方程式可求得th+1时刻的四元素Q(th+1)。(4)设计二阶互补滤波器对姿态解算得到的加速度计姿态角进行低通滤波，对陀螺仪姿态角进行高通滤波，并实现各传感器数据融合，最后输出姿态角。以俯仰角θ为例，θa为姿态解算得到的加速度计俯仰角，θw为姿态解算得到的陀螺仪俯仰角，在二阶互补滤波器中的低通滤波部分，通过姿态解算得到的加速度计俯仰角θa与前一次滤波更新后姿态角中的俯仰角θ之间存在的误差值θerr作为二阶互补滤波器的二次输入值即反馈角度作为新的输入值形成负反馈，实现二阶互补滤波的误差修正，二阶互补滤波器各部分的具体表达式为：上式中，x1和x2为中间变量，Kp为比例系数变量，KI为积分系数变量，τ为滤波器时间常数，1/τ为滤波器截止频率，dt为滤波器采样时间，θt为最终输出的俯仰角。同理，利用二阶互补滤波器对横滚角φw和偏航角ψw姿态数据融合，得到最终输出的横滚角φt和偏航角ψt。其中，Kp为比例系数变量，KI为积分系数变量的确定方法有很多，主要有临界比例法、反映曲线法、衰减法等，一般先确定比例系数Kp，再确定积分系数KI。具体方法为首先设定积分系数KI为0，将二阶互补滤波器变为纯比例调节，然后将Kp的值由零开始逐步增大，直至二阶互补滤波器出现振荡，再反过来调节将Kp值逐步减小，直至二阶互补滤波器振荡消失，记录当本文档来自技高网...

【技术保护点】
一种用于IMU多传感器数据融合的滤波方法，其特征在于，包括以下步骤：(1)利用陀螺仪、加速度计获取飞行器的姿态信息，建立传感器输出信号的数学模型表达式；陀螺仪的误差主要由随机漂移和噪声构成，设ywt为陀螺仪的输出信号，wt为真实角速度值，vwt为测量噪声，b为漂移量，则数学模型可表示为：ywt＝wt+vwt+b    (1)加速度计的输出信号为yat，载体运动的加速度为at，地球重力加速度为gt，其中为旋转矩阵，vat表示高斯白噪声，则加速度计的数学模型可表示为：yat＝at‑gt+vat   (2)(2)利用滑动均值滤波器去除步骤(1)数学模型表达式中的测量噪声和高斯白噪声以及干扰信息，提高系统的精度；(3)基于四元素向量法，将各传感器去噪后的输出数据转化为四元素，建立载体坐标系与参考坐标系之间的空间角位置关系，利用变换矩阵得到姿态角的四元素表达式，分别进行加速度计和陀螺仪的旋转加速度和旋转角速度姿态角解算；(4)设计二阶互补滤波器对姿态解算得到的加速度计姿态角进行低通滤波，对陀螺仪姿态角进行高通滤波，并实现各传感器数据融合，最后输出姿态角。

【技术特征摘要】
1.一种用于IMU多传感器数据融合的滤波方法，其特征在于，包括以下步骤：(1)利用陀螺仪、加速度计获取飞行器的姿态信息，建立传感器输出信号的数学模型表达式；陀螺仪的误差主要由随机漂移和噪声构成，设ywt为陀螺仪的输出信号，wt为真实角速度值，vwt为测量噪声，b为漂移量，则数学模型可表示为：ywt＝wt+vwt+b(1)加速度计的输出信号为yat，载体运动的加速度为at，地球重力加速度为gt，其中为旋转矩阵，vat表示高斯白噪声，则加速度计的数学模型可表示为：yat＝at-gt+vat(2)(2)利用滑动均值滤波器去除步骤(1)数学模型表达式中的测量噪声和高斯白噪声以及干扰信息，提高系统的精度；(3)基于四元素向量法，将各传感器去噪后的输出数据转化为四元素，建立载体坐标系与参考坐标系之间的空间角位置关系，利用变换矩阵得到姿态角的四元素表达式，分别进行加速度计和陀螺仪的旋转加速度和旋转角速度姿态角解算；(4)设计二阶互补滤波器对姿态解算得到的加速度计姿态角进行低通滤波，对陀螺仪姿态角进行高通滤波，并实现各传感器数据融合，最后输出姿态角。2.根据权利要求1所述的用于IMU多传感器数据融合的滤波方法，其特征在于，步骤(2)中滑动均值滤波器的公式为：w=1nΣi=0n-1yt-i---(3)]]>其中yi为各传感器输出的原始数据，w为滑动均值滤波器的输出结果，n为滑动均值滤波器阶数。3.根据权利要求1所述的用于IMU多传感器数据融合的滤波方法，其特征在于，步骤(3)的方法为：a.飞行器的姿态由俯仰角θ、横滚角φ、偏航角ψ描述，选取东北天坐标系为参考坐标系，即以坐标系载体的重心为坐标原点，Xg指向正北方向，Yg指向正东方向，Zg轴与Xg轴和Yg轴构成的平面垂直，指向天空；载体坐标系以载体作为参考，坐标原点为载体重心，Xb为载体横轴，Yb为载体纵轴，Zb轴垂直于Xb轴和Yb轴构成的平面，XbYbZb构成右手坐标系；参考坐标系被认为是固定不变的，载体坐标系相对于参考坐标系的方位变化，相对变化情况即为待求的载体姿态，用姿态角表示，两坐标系之间的关系可以用旋转矩阵表示为：Rgb=cosθcosψ-cosφsinψ+sinφsinθcosψsinφsinψ+cosφsinθcosψcosθsinψcosφcosψ+sinφsinθsinψ-sinφcosψ+cosφsinθsinψ-sinθsinφcosθcosφcosθ---(4)]]>b.将各传感器经步骤(2)去噪处理后的输出数据转化为四元素表示形式，其表达式为：Q=q0+q1i→+q2j→+q3k→---(5)]]>其中q0、q1、q2、q3均为实数，i、j、k为相互正交的单位向量；c.将四元素向量规范化处理之后，变换矩阵可表示为：Rgb=q02+q12-q22-q322(q1q2-q0q3)2(q1q3+q0q2)2(q1q2+q0q3)q02-q12+q22-q322(q2q3-q0q1)2(q1q3-q0q2)2(q2q3+q0q1)q02-q12-q22+q32---(6)]]>d.用四元素法中的参数推...

【专利技术属性】
技术研发人员：罗文峰，
申请(专利权)人：湖南优象科技有限公司，
类型：发明
国别省市：湖南;43