一种基于改进四元数二阶互补滤波的飞行器姿态解算方法技术

本发明专利技术提出的一种基于改进四元数互补滤波的飞行器姿态快速收敛方法，属于数字滤波和多传感器数据融合技术领域，主要作用于飞行器姿态解算系统中，使之获得准确的姿态角便于进行后续姿态控制。该方法采用加速度计和磁力计的误差值经过PI调节后对陀螺仪进行补偿和校正，并在飞行器的启动阶段自适应的调节滤波器截止频率，用补偿后的陀螺仪数据去更新四元数并归一化，再用更新后的四元数计算出姿态变换矩阵并解算出姿态角。本发明专利技术不仅具有传统互补滤波不需对噪声精确建模的优点，而且飞行器初始姿态收敛速度快，获得精确姿态角，同时减少滤波算法计算量，实现飞行器的快速平稳启动，增强其稳定性能。

【技术实现步骤摘要】
一种基于改进四元数二阶互补滤波的飞行器姿态解算方法
本专利技术提出的一种基于改进四元数互补滤波的飞行器姿态解算方法，属于数字滤波和多传感器数据融合
，本专利技术提供的方法适用于非线性姿态测量系统。
技术介绍
姿态解算的精度和速度将直接影响飞行器控制算法的稳定性、可靠性和实现的难易程度。目前飞行器姿态的描述参数主要包括：欧拉角法、四元数法、罗德里格斯参数和修正罗德里格斯参数法。欧拉角法在90°时存在奇点且有大量的超越方程运算；而四元数法虽然存在一个冗余参数但是可以全姿态工作，且计算简便，计算量小。因此，四元数法是目前惯导系统姿态解算的常用参数。随着微机电技术和计算机技术的发展，由于陀螺仪和加速度计组成的低成本惯性测量单元具有体积小、重量轻、功耗低、性价比高等特点，在飞行器姿态的测量系统得到普遍应用。微机电陀螺仪具有随机漂移特性，随着时间的增长会产生累计积分误差。加速度计和磁力计不存在累积误差，但容易受到机体振动等干扰，输出噪声较大。因此，如何融合IMU多传感器的数据，滤除噪声干扰得到高可靠性、高精度的姿态数据是一项非常具有挑战性的工作。由于互补滤波算法计算量小且不需对噪声精确建模，能较好结合系统中各个传感器的优势，在非理想高斯白噪声条件下的低精度姿态测量系统中相比于时域滤波器具有更好的滤波效果，在低成本的嵌入式姿态测量系统中应用广泛。但是传统的互补滤波算法采用固定的截止频率，噪声较大时滤波效果不理想，在实时性能高的场合往往难以满足要求。
技术实现思路
本专利技术专利针对上述存在的问题和不足，提出一种用于飞行器姿态检测的改进四元数互补滤波解算方法，改进算法通过在四旋翼飞行器启动阶段自适应调整滤波器截止频率使姿态解算算法快速响应初始状态的姿态角，最终提高四旋翼飞行器初始姿态收敛速度，实现其初始时刻快速平稳启动。本专利技术具体过程如下：步骤1：选取飞行器的参考坐标系，获取姿态角初始值，选取姿态变换矩阵，初始化四元数与姿态变换矩阵；步骤2：采集加速度计、磁力计的输出数据，选取加速度计、磁力计的参考向量，通过姿态变换矩阵计算加速度计与磁力计的输出数据与参考向量之间的误差值；步骤3：采集加速度计和陀螺仪输出数据获取它们的方差参数，根据方差参数和二阶互补滤波的PI调节器结构，设计一个自适应PI调节器的比例系数；步骤4：利用PI调节器，设计一个二阶互补滤波器，将加速度计、磁力计的输出数据与参考向量之间的误差值和陀螺仪的输出值经二阶互补滤波器后得到陀螺仪的修正值；步骤5：用修正后的陀螺仪数据去更新四元数并归一化，再用更新后的四元数计算出姿态变换矩阵并解算出姿态角。进一步地，步骤1中由于本专利技术涉及俯仰角θ、横滚角γ和航向角ψ，因此飞行器姿态数据采集方案采用陀螺仪、加速度计和磁力计采集姿态数据，选取地理坐标系(东北天)作为导航坐标系，导航系与载体系之间的变换关系可用如下姿态变换矩阵表示：定义四元数Q＝[q1，q2，q3，q4]T，其中q1为四元数标量部分，q2，q3，q4为四元数矢量部分，初始四元数Q0可由初始姿态角获得：进一步地，步骤2中参考向量分别为加速度计在导航系下的输出gn＝[00-g]T和磁力计在导航系下的输出Mn＝[0ENEU]T，则按下式将加速度计实际测量值与重力在载体系下的分量进行向量积运算可以得到加速度计的误差值ea类似地，按下式将磁力计实际测量值与参考向量在载体坐标系下的分量进行向量积运算可以得到磁力计误差值em其中，姿态变换矩阵可按下式由四元数表示：进一步地，步骤3中考虑到加速度计和磁力计高频噪声分布不完全相同，二阶互补滤波的PI调节参数kap、kmp分别选取，其中kap、kmp分别为PI参数中对加速度误差、磁力计误差的比例系数；kap可以根据如下函数自适应获取：kap＝f(t)+f(Ra，Rg，ΔE)+f(ΔZ)f(ΔZ)＝χ·ΔZ其中，为理想值，t为运行时间，α，β，μ，χ为调节系数；λ为门限值；Ra为加速度计三轴测量方差模值；Rg为陀螺仪三轴测量方差模值；ΔE为飞行器状态辨别量；ΔZ加速度计与陀螺仪解算角度差值。加速度计和陀螺仪的三轴测量方差模值为：其中和分别为x、y、z轴的陀螺仪和加速度计协方差，可由加速度计和陀螺仪测量的多个数据计算得到：状态辨别变量ΔE为：其中，n加速度计和陀螺仪测量的多个数据量，||·||为二范数，abs(·)为绝对值函数，A(i)为第i个解算得到的加速度矢量模值，Ax、Ay、Az分别为x、y、z轴的加速度计输出，为当前时刻解算得到的加速度矢量和模值的均值，ω(i)为第i个解算得到的陀螺仪矢量模值，为陀螺仪输出矢量和模值的均值，为上一时刻陀螺仪解算得到的重力加速度矢量和的模值，磁力计用于偏航角的姿态解算，在飞行器启动过程中不影响控制的稳定性，因此不对其参数kmp进行自适应设计，采用固定常数。进一步地，步骤4中，通过步骤3中设计的一个自适应PI调节器的比例系数，利用PI调节器，设计一个二阶互补滤波器，其传递函数如下：其中，s为微分算子，kmp和kai、kmi分别为加速度计和磁力计的比例系数、积分系数，为系统输出值、为陀螺仪输出值、φa为加速度误差值、φm为磁力计误差值，Φg、Φa、Φm分别为φa、φm的Laplace形式，整个滤波系统为二阶互补滤波，其中，GH(s)为控制输入作用下的传递函数，由GH(s)传递函数形式可知GH(s)具有高通滤波性能；GLA(s)、GLM(s)分别为对加速度计与磁力计扰动作用下的传递函数，由GLA(s)、GLM(s)传递函数形式可知GLA(s)、GLM(s)具有低通滤波性能；根据上述传递函数，有以下时域计算式：其中分别为当前时刻的累积误差，分别为下一时刻的累积误差，为修正前的陀螺仪输出，为修正后的陀螺仪输出。进一步地，步骤5中按下式使用步骤三中修正后的陀螺仪输出对当前时刻的四元数进行更新：其中，I4×4为4阶单位矩阵，分别为补偿后的陀螺仪输出在x、y、z轴上的分量。对更新后的四元数Q(k+1)进行归一化处理，通过姿态变换矩阵求出姿态角：θ＝arcsin(2(q3q4+q1q2))本专利技术具有如下优点：采用四元数法进行姿态的解算与更新，相对于欧拉角法和方向余弦法来说，可以全姿态工作，无奇点存在，同时计算量降低；由于改进互补滤波方法在t≤1200ms的飞行器启动阶段自适应的调节截止频率，故而能够快速响应初始状态的姿态角，较于传统的固定频率互补滤波器有较好的响应快速性。附图说明图1改进四元数互补滤波算法原理图图2传统算法与改进算法的飞行器三轴姿态角仿真轨迹图图3传统互补滤波实验波形图4改进互补滤波实验波形具体实施方式下面详细描述本专利技术的实施例，本实施例是示例性的，仅用于解释本专利技术，而不能理解为对本专利技术的限制。参照说明书附图对本专利技术的一种基于改进四元数互补滤波的飞行器姿态解算方法作以下详细地说明：如图1所示，为四元数互补滤波算法原理图。首先选取飞行器姿态角及相应的参考坐标系，确定姿态变换矩阵；然后由初始姿态角获得初始四元数；通过姿态变换矩阵、参考向量和加速度计与磁力计的输出值得到加速度计和磁力计误差值；将加速度计和磁力计在不同坐标系下测量的误差值经过PI调节后直接对陀螺仪进行补偿和校正；用补偿后的陀螺仪数据去更新四元数并归一化，再用更新后的四元数计算出姿态本文档来自技高网...

【技术保护点】
1.一种基于改进四元数互补滤波的飞行器姿态解算方法，其特征在于，具体包括以下步骤：步骤1：选取飞行器的参考坐标系，获取姿态角初始值，选取姿态变换矩阵，初始化四元数与姿态变换矩阵；步骤2：采集加速度计、磁力计的输出数据，选取加速度计、磁力计的参考向量，通过姿态变换矩阵计算加速度计、磁力计的输出数据与参考向量之间的误差值；步骤3：采集加速度计和陀螺仪输出数据获取它们的方差参数，根据方差参数和二阶互补滤波的PI调节器结构，设计一个自适应PI调节器的比例系数；步骤4：利用PI调节器，设计一个二阶互补滤波器，将加速度计、磁力计的输出数据与参考向量之间的误差值和陀螺仪的输出值经二阶互补滤波器后得到陀螺仪的修正值；步骤5：用修正后的陀螺仪数据去更新四元数并归一化，再用更新后的四元数计算出姿态变换矩阵并解算出姿态角。

【技术特征摘要】
1.一种基于改进四元数互补滤波的飞行器姿态解算方法，其特征在于，具体包括以下步骤：步骤1：选取飞行器的参考坐标系，获取姿态角初始值，选取姿态变换矩阵，初始化四元数与姿态变换矩阵；步骤2：采集加速度计、磁力计的输出数据，选取加速度计、磁力计的参考向量，通过姿态变换矩阵计算加速度计、磁力计的输出数据与参考向量之间的误差值；步骤3：采集加速度计和陀螺仪输出数据获取它们的方差参数，根据方差参数和二阶互补滤波的PI调节器结构，设计一个自适应PI调节器的比例系数；步骤4：利用PI调节器，设计一个二阶互补滤波器，将加速度计、磁力计的输出数据与参考向量之间的误差值和陀螺仪的输出值经二阶互补滤波器后得到陀螺仪的修正值；步骤5：用修正后的陀螺仪数据去更新四元数并归一化，再用更新后的四元数计算出姿态变换矩阵并解算出姿态角。2.根据权利要求1所述的一种基于改进四元数互补滤波的飞行器姿态解算方法，其特征在于：步骤3中设计一个自适应PI调节器的比例系数kap，kap为PI参数中对加速度误差的比例系数；kap根据如下函数自适应获取：kap＝f(t)+f(Ra，Rg，ΔE)+f(ΔZ)f(ΔZ)＝χ·ΔZ其中，为理想值，...

【专利技术属性】
技术研发人员：周翟和，钟雨露，伏家杰，曾庆喜，孔德明，田祥瑞，游霞，陈燕，
申请(专利权)人：南京航空航天大学，
类型：发明
国别省市：江苏,32