基于扩展卡尔曼滤波的姿态解算系统及解算方法

技术领域

本发明属于固定翼无人机姿态解算技术领域，具体涉及基于扩展卡尔曼滤波的姿态解算系统及解算方法。

背景技术

固定翼无人机的飞控系统包含姿态控制、组合导航、故障诊断等众多模块，是无人机的核心部件，其决定了无人机的可靠性和稳定性，而对采集的数据进行融合处理并进行姿态解算，获得较为准确的姿态角，是整个飞控环节的第一步。目前在固定翼无人机上采用的姿态解算方法，普遍使用的有mahony互补滤波器和扩展卡尔曼滤波器。

互补滤波器(complementary filter,CF)从加速度计和磁罗盘获得低频数据融合从陀螺仪获得的高频数据进行姿态解算，其显著优势是计算量小，是现在商用最广泛的滤波器，但是动态性能低于卡尔曼滤波器，导致飞控系统有滞后现象，截止频率附近衰减较慢，处理后的数据存在误差，精确度较低。

扩展卡尔曼滤波器(Extended Kalman Filter,EKF)利用泰勒级数展开将非线性系统进行线性化处理，其动态性能良好，仅依靠参数矩阵进行解算，不影响估计精度，但也因此十分依靠先验知识，线性化过程存在一定的线性化误差。

神经网络(neural network,NN)本质上是从输入空间到目标空间的一个非线性映射，对于载体机动和惯导器件之间的误差相关性能够准确拟合，Elman NN相对于普通的NN有一个特殊的承接层，它能够使网络独有动态记忆功能，因此也很好符合了惯导器件的惯性特性，可以结合固定翼无人机的惯性测量单元进行辅助使用。

发明内容

为克服上述现有技术的不足，本发明的目的在于提供基于扩展卡尔曼滤波的姿态解算系统及解算方法，利用神经网络校准扩展卡尔曼滤波，解决传统姿态解算方法的局限性以及过度依赖陀螺仪、加速度计传感器性能的缺陷，本发明在标准扩展卡尔曼滤波的基础上，加入线性互补滤波算法，综合两种滤波算法用于固定翼无人机(Unmanned AerialVehicle,UAV)惯性测量单元(Inertial Measurement Unit,IMU)两个传感器的数据融合与姿态解算，再加入Elman神经网络校准三轴姿态角，提高了解算精度，降低了对惯性测量单元的依赖性。本发明不仅可以改善传感器受温漂、振动等误差影响，而且加入Elman网络，降低了姿态解算对惯导系统的依赖性，提高了姿态解算的自适应性，响应速度和收敛速度更快，能够快速准确地解算出无人机的三轴姿态角。

为实现上述目的，本发明采用的技术方案是：

基于扩展卡尔曼滤波的姿态解算系统，包括有数据处理模块、解算模块、校准模块；

所述的数据处理模块：用于处理IMU陀螺仪和加速度计两个传感器数据；

所述的解算模块：用于将IMU陀螺仪和加速度计两个传感器数据经过线性互补滤波和扩展卡尔曼滤波进行数据融合和姿态解算，得到校准前的姿态信息；

所述的校准模块：用于将IMU陀螺仪和加速度计两个传感器数据和校准前的姿态信息作为Elman神经网络的输入进行训练和预测，从而得到校准后的姿态信息。

利用基于扩展卡尔曼滤波的姿态解算系统解算的方法，包括以下步骤：

步骤1，数据处理模块对数据进行处理，具体做法是：

导入传感器含噪数据，传感器含噪数据包括有陀螺仪测得的姿态角速度和加速度计测得的姿态角重力加速度；传感器含噪数据含有噪声，对导入数据做平滑滤波、归一化处理，获得处理后数据，供步骤2使用；

步骤2，解算模块进行姿态解算，具体做法是：

首先，神经网络用线性互补滤波算法对步骤1处理后数据进行滤波，利用旋转矩阵对不同坐标系下的加速度g做相应转换和向量叉乘，并对陀螺仪数据进行PI积分补偿，通过融合加速度计高通滤波和陀螺仪低通滤波，得到更接近真实测量结果的数据；

再次，以线性互补滤波之后的陀螺仪数据作为扩展卡尔曼滤波算法的初始状态变量，以加速度计测量数据为量测变量，求得雅克比系数矩阵、过程噪声协方差矩阵以及测量噪声协方差矩阵，代入扩展卡尔曼滤波方程组迭代计算，得到当前时刻的姿态角的四元数表示法，以此类推，得到一组线性的四元数形式姿态角，达到实时准确跟踪姿态信息的目的；

步骤3，校准模块对三轴姿态角进行校准，具体做法是：

输入两个传感器数据和未校准的三轴姿态角至Elman NN，采用在线训练学习；工作开始时，设置Elman NN的初始权值和阈值为随机值，取姿态角测量值为Elman NN的预测值，采用梯度下降的学习方法，通过实时更新神经网络的权值和阈值，经学习后输出校准后的三轴姿态角。

所述的步骤2，神经网络以线性互补滤波和扩展卡尔曼滤波解算后的三轴姿态角为输入；

为避免训练过程的交叉耦合，采用三个并行的Elman神经网络结构。

所述的三轴姿态角，通过解算和校准无人机惯性测量单元上的陀螺仪和加速度计两个传感器的数据得到；

所述的线性互补滤波，结合PI积分补偿校正陀螺仪数据，将处理后的两个传感器数据导入扩展卡尔曼滤波进行数据融合和姿态解算，Elman神经网络将两个传感器数据和解算后的三轴姿态角信息作为输入在神经网络结构中进行在线训练，输出校准后的三轴姿态角，提高解算精度和系统的冗余性。

所述的扩展卡尔曼滤波算法，以前一时刻的姿态角的四元数表示法为初始变量，以线性互补滤波之后的陀螺仪测量数据为状态变量，以加速度计测量数据为量测变量，给定系数矩阵，经扩展卡尔曼滤波方程组迭代计算得到当前时刻的姿态角的四元数表示法，以此类推，得到一组线性的四元数形式姿态角，达到实时准确跟踪姿态信息的目的。

与现有技术相比，本发明的有益效果是：

本发明基于改进扩展卡尔曼滤波的姿态解算方法综合两种算法的优势，减少了各自使用时的局限性，提高了解算性能，具有更高的实时性和稳定性。

本发明的改进扩展卡尔曼滤波模块和Elman神经网络模块不单单是对单一传感器的数据进行算法处理再进行姿态解算，通过融合陀螺仪和加速度计两个传感器数据，并经过神经网络在线训练，能够实时解算出精度较高的三轴姿态角估计值。

本发明仅使用UAV-IMU上的陀螺仪和加速度计两个传感器，节约了数据空间，节省了数据分析和处理的时间，提高了解算速度，简化了解算流程。

本发明利用神经网络校准扩展卡尔曼滤波的固定翼无人机姿态解算方法，通过解算和校准UAV-IMU上的陀螺仪和加速度计两个传感器的数据，得到精度较高的三轴姿态角；数据处理模块对IMU传感器测量数据进行平滑滤波、归一化等处理，解算模块首先利用线性互补滤波结合PI积分补偿校正陀螺仪数据，然后将处理后的两传感器数据导入扩展卡尔曼滤波进行数据融合和姿态解算，校准模块利用Elman神经网络进行在线训练，以IMU传感器信息和解算模块输出的校准前的姿态信息为输入，以姿态角测量值为神经网络预测值，为了避免训练过程的交叉耦合，采用三个并行的Elman神经网络结构，实时更新神经网络的权值和阈值，输出校准后的三轴姿态角，提高解算精度和系统的冗余性。

附图说明

图1为本发明的系统的方法流程图。

图2为本发明的系统原理框图。

图3是本发明实施例所述的Elman神经网络结构图。

具体实施方式

下面结合附图和具体实施方式对本发明专利进行详细说明。

基于扩展卡尔曼滤波的姿态解算系统，包括有数据处理模块、解算模块、校准模块；

所述的数据处理模块：用于处理IMU传感器数据；

所述的解算模块：用于将IMU陀螺仪和加速度计两个传感器数据经过线性互补滤波和扩展卡尔曼滤波进行数据融合和姿态解算，得到校准前的姿态信息；

所述的校准模块：用于将IMU陀螺仪和加速度计两个传感器数据和校准前的姿态信息作为Elman神经网络的输入进行训练和预测，从而得到校准后的姿态信息。

利用基于扩展卡尔曼滤波的姿态解算系统解算的方法，包括以下步骤：

步骤1，数据处理模块对数据进行处理，具体做法是：

导入传感器含噪数据，传感器含噪数据包括有陀螺仪测得的姿态角速度和加速度计测得的姿态角重力加速度；传感器含噪数据含有噪声，对导入数据做平滑滤波、归一化处理，获得处理后数据，供步骤2使用；

步骤2，解算模块进行姿态解算，具体做法是：

首先，神经网络用线性互补滤波算法对步骤1处理后数据进行滤波，利用旋转矩阵对不同坐标系下的加速度g做相应转换和向量叉乘，并对陀螺仪数据进行PI积分补偿，通过融合加速度计高通滤波和陀螺仪低通滤波，得到更接近真实测量结果的数据；

再次，以线性互补滤波之后的陀螺仪数据作为扩展卡尔曼滤波算法的初始状态变量，以加速度计测量数据为量测变量，求得雅克比系数矩阵、过程噪声协方差矩阵以及测量噪声协方差矩阵，代入扩展卡尔曼滤波方程组迭代计算，得到当前时刻的姿态角的四元数表示法，以此类推，得到一组线性的四元数形式姿态角，达到实时准确跟踪姿态信息的目的；

步骤3，校准模块对三轴姿态角进行校准，具体做法是：

输入两个传感器数据和未校准的三轴姿态角至Elman NN，采用在线训练学习；工作开始时，设置Elman NN的初始权值和阈值为随机值，取姿态角测量值为Elman NN的预测值，采用梯度下降的学习方法，通过实时更新神经网络的权值和阈值，经学习后输出校准后的三轴姿态角。

所述的步骤2，神经网络以线性互补滤波和扩展卡尔曼滤波解算后的三轴姿态角为输入；

为避免训练过程的交叉耦合，采用三个并行的Elman神经网络结构。

所述的三轴姿态角，通过解算和校准无人机惯性测量单元上的陀螺仪和加速度计两个传感器的数据得到；

所述的线性互补滤波，结合PI积分补偿校正陀螺仪数据，将处理后的两个传感器数据导入扩展卡尔曼滤波进行数据融合和姿态解算，Elman神经网络将两个传感器数据和解算后的三轴姿态角信息作为输入在神经网络结构中进行在线训练，输出校准后的三轴姿态角，提高解算精度和系统的冗余性。

所述的扩展卡尔曼滤波算法，以前一时刻的姿态角的四元数表示法为初始变量，以线性互补滤波之后的陀螺仪测量数据为状态变量，以加速度计测量数据为量测变量，给定系数矩阵，经扩展卡尔曼滤波方程组迭代计算得到当前时刻的姿态角的四元数表示法，以此类推，得到一组线性的四元数形式姿态角，达到实时准确跟踪姿态信息的目的。

具体实施方法为：首先导入采集的传感器含噪数据，并对数据做平滑滤波、归一化等处理；之后将处理后的数据导入解算模块进行姿态解算，不断更新得到解算的三轴姿态角；接着将传感器信息和姿态角信息输入到ElmanNN进行在线训练，经过更新合适的权值和阈值后，实时输出校准后的三轴姿态角。结果显示，相对于标准扩展卡尔曼滤波解算和Mohony互补滤波解算，本发明解算的三轴姿态角均方误差更小，特别是在姿态角度变化较大的时间点，本发明解算的过程更加稳定和准确。

实施例

固定翼无人机基于改进扩展卡尔曼滤波算法和Elman神经网络辅助的姿态解算方法，包括以下步骤：

步骤1，导入数据并处理，具体做法是：

导入传感器含噪数据，分别包含有陀螺仪测得的姿态角速度和加速度计测得的姿态角重力加速度。由于数据含有一定噪声，因此再对导入数据做平滑滤波、归一化处理，以供步骤2使用；

步骤2，解算模块进行姿态解算，具体做法是：

首先，线性互补滤波算法对步骤1处理数据进行滤波，通过融合加速度计高通滤波和陀螺仪低通滤波，并对陀螺仪数据进行PI积分补偿，得到更接近真实测量结果的数据；

再次，以线性互补滤波之后的陀螺仪数据作为扩展卡尔曼滤波算法的初始状态变量，以加速度计测量数据为量测变量，给定系数矩阵，经扩展卡尔曼滤波方程组迭代计算得到当前时刻的姿态角的四元数表示法，以此类推，得到一组线性的四元数形式姿态角，达到实时准确跟踪姿态信息的目的；

步骤3，校准模块对三轴姿态角进行校准，具体做法是：

输入两个传感器数据和未校准的三轴姿态角至Elman NN，采用在线训练学习，工作开始时，设置Elman NN的初始权值和阈值为随机值，采用梯度下降的学习方法，通过实时更新神经网络的权值和阈值，最后输出校准后的三轴姿态角。

上述技术方案中，步骤2使用线性互补滤波算法的具体过程如下：

首先，确定地理坐标系n中标准重力加速度g，定义为g

上式中的旋转矩阵是三轴姿态角按照ZYX的旋转顺序，以四元数矩阵Q＝[q

设加速度计三个轴的值分别为a

求模：

归一化：

接着，对g

然后，再使用PI控制器进行滤波，准确地说是消除漂移误差，只要存在误差，控制器便会持续作用，直至误差为0，控制的效果取决于P和I参数，分别对应于比例控制和积分控制的参数，PI控制器滤波公式如下：

最后，将补偿后的陀螺仪角速度的数值w代入扩展卡尔曼滤波算法中，更新姿态四元数。

上述技术方案中，步骤2使用扩展卡尔曼滤波算法主要分为两个过程，预测过程和测量过程：

在预测过程中，首先利用线性互补滤波之后的三轴陀螺仪测得的角速度矢量[w

上面的方程中

其中

接着，将上式离散化得到系统状态方程如式(8)所示。由于地理坐标系相对于时间t的机载坐标系的方向可以通过使用采样周期Δt对四元数导数进行数值积分来计算，因此描述预测过程的离散状态转移向量方程可以由(7)导出，如下所示：

x

其中w

最后按照公式(9)实现对四元数的更新，计算当前时刻的四元数并对其进行归一化处理。

在测量过程中，利用加速度计在静止状态下能够测量重力的特性，当加速度计发生倾斜时，其三轴上的重力加速度就会发生变化，进而计算出姿态角。

首先对于一个三轴加速度计，把任意时刻测得的加速度矢量，作为观测变量，记作

z(k)＝[a

式中a

接着，根据旋转矩阵进行坐标转换。已知四元数形式的旋转矩阵为

上式中的旋转矩阵是三轴姿态角按照ZYX的旋转顺序，以四元数矩阵Q＝[q

则系统的观测方程可以表示为下式所示：

然后，根据式(13)，h(x(k),k)为非线性函数，需要通过泰勒展开式将非线性部分线性化，求得雅克比矩阵为

最后为了保持四元数单位范数的性质，在测量更新阶段后，采用最后的归一化步骤对估计进行归一化处理。

经过预测过程和测量过程的迭代和计算，最终解算出融合陀螺仪和加速度计的三轴姿态角的四元数形式，通过欧拉角反解公式求解出三个姿态角：横滚角、俯仰角、航向角。

在上述技术方案中，步骤3采用的Elman神经网络主体为4层网络结构，具有多输入、单输出的结构，输入层为并行输入，来自三轴(x,y,z)陀螺仪信息、三轴(x,y,z)加速度信息和3个校准前的姿态信息共9个信息作为Elman神经网络的输入层神经元，其网络结构的参数都可以通过网络学习进行在线训练，Elman神经网络结构图如图3所示。

Elman神经网络的计算公式为：

其中，w

学习方法采用梯度下降法，激励函数f(x)为sigmoid，公式如下：

最后应说明的是：以上所述仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行同等替换。凡在本发明的精神和原则之内，所作的任何修改、同等替换、改进等，均应包含在本发明的保护范围之内。