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

Abstract

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

Description

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

技术领域

本发明提出的一种基于改进四元数互补滤波的飞行器姿态解算方法，属于数字滤波和多传感器数据融合技术领域，本发明提供的方法适用于非线性姿态测量***。

背景技术

姿态解算的精度和速度将直接影响飞行器控制算法的稳定性、可靠性和实现的难易程度。目前飞行器姿态的描述参数主要包括：欧拉角法、四元数法、罗德里格斯参数和修正罗德里格斯参数法。欧拉角法在90°时存在奇点且有大量的超越方程运算；而四元数法虽然存在一个冗余参数但是可以全姿态工作，且计算简便，计算量小。因此，四元数法是目前惯导***姿态解算的常用参数。

随着微机电技术和计算机技术的发展，由于陀螺仪和加速度计组成的低成本惯性测量单元具有体积小、重量轻、功耗低、性价比高等特点，在飞行器姿态的测量***得到普遍应用。微机电陀螺仪具有随机漂移特性，随着时间的增长会产生累计积分误差。加速度计和磁力计不存在累积误差，但容易受到机体振动等干扰，输出噪声较大。因此，如何融合IMU多传感器的数据，滤除噪声干扰得到高可靠性、高精度的姿态数据是一项非常具有挑战性的工作。

由于互补滤波算法计算量小且不需对噪声精确建模，能较好结合***中各个传感器的优势，在非理想高斯白噪声条件下的低精度姿态测量***中相比于时域滤波器具有更好的滤波效果，在低成本的嵌入式姿态测量***中应用广泛。但是传统的互补滤波算法采用固定的截止频率，噪声较大时滤波效果不理想，在实时性能高的场合往往难以满足要求。

发明内容

本发明专利针对上述存在的问题和不足，提出一种用于飞行器姿态检测的改进四元数互补滤波解算方法，改进算法通过在四旋翼飞行器启动阶段自适应调整滤波器截止频率使姿态解算算法快速响应初始状态的姿态角，最终提高四旋翼飞行器初始姿态收敛速度，实现其初始时刻快速平稳启动。

本发明具体过程如下：

步骤1：选取飞行器的参考坐标系，获取姿态角初始值，选取姿态变换矩阵，初始化四元数与姿态变换矩阵；

步骤2：采集加速度计、磁力计的输出数据，选取加速度计、磁力计的参考向量，通过姿态变换矩阵计算加速度计与磁力计的输出数据与参考向量之间的误差值；

步骤3：采集加速度计和陀螺仪输出数据获取它们的方差参数，根据方差参数和二阶互补滤波的PI调节器结构，设计一个自适应PI调节器的比例系数；

步骤4：利用PI调节器，设计一个二阶互补滤波器，将加速度计、磁力计的输出数据与参考向量之间的误差值和陀螺仪的输出值经二阶互补滤波器后得到陀螺仪的修正值；

步骤5：用修正后的陀螺仪数据去更新四元数并归一化，再用更新后的四元数计算出姿态变换矩阵并解算出姿态角。

进一步地，步骤1中由于本发明涉及俯仰角θ、横滚角γ和航向角ψ，因此飞行器姿态数据采集方案采用陀螺仪、加速度计和磁力计采集姿态数据，选取地理坐标系(东北天)作为导航坐标系，导航系与载体系之间的变换关系可用如下姿态变换矩阵表示：

定义四元数Q＝[q₁，q₂，q₃，q₄]^T，其中q₁为四元数标量部分，q₂，q₃，q₄为四元数矢量部分，初始四元数Q₀可由初始姿态角获得：

进一步地，步骤2中参考向量分别为加速度计在导航系下的输出gⁿ＝[0 0 -g]^T和磁力计在导航系下的输出Mⁿ＝[0 E_N E_U]^T，则按下式将加速度计实际测量值与重力在载体系下的分量进行向量积运算可以得到加速度计的误差值e_a

类似地，按下式将磁力计实际测量值与参考向量在载体坐标系下的分量进行向量积运算可以得到磁力计误差值e_m

其中，姿态变换矩阵可按下式由四元数表示：

进一步地，步骤3中考虑到加速度计和磁力计高频噪声分布不完全相同，二阶互补滤波的PI调节参数k_ap、k_mp分别选取，其中k_ap、k_mp分别为PI参数中对加速度误差、磁力计误差的比例系数；

k_ap可以根据如下函数自适应获取：

k_ap＝f(t)+f(R_a，R_g，ΔE)+f(ΔZ)

f(ΔZ)＝χ·ΔZ

其中，为理想值，t为运行时间，α，β，μ，χ为调节系数；λ为门限值；R_a为加速度计三轴测量方差模值；R_g为陀螺仪三轴测量方差模值；ΔE为飞行器状态辨别量；ΔZ加速度计与陀螺仪解算角度差值。

加速度计和陀螺仪的三轴测量方差模值为：

其中和分别为x、y、z轴的陀螺仪和加速度计协方差，可由加速度计和陀螺仪测量的多个数据计算得到：

状态辨别变量ΔE为：

其中，n加速度计和陀螺仪测量的多个数据量，||·||为二范数，abs(·)为绝对值函数，A(i)为第i个解算得到的加速度矢量模值，A_x、A_y、A_z分别为x、y、z轴的加速度计输出，为当前时刻解算得到的加速度矢量和模值的均值，ω(i)为第i个解算得到的陀螺仪矢量模值，为陀螺仪输出矢量和模值的均值，为上一时刻陀螺仪解算得到的重力加速度矢量和的模值，磁力计用于偏航角的姿态解算，在飞行器启动过程中不影响控制的稳定性，因此不对其参数k_mp进行自适应设计，采用固定常数。

进一步地，步骤4中，通过步骤3中设计的一个自适应PI调节器的比例系数，利用PI调节器，设计一个二阶互补滤波器，其传递函数如下：

其中，s为微分算子，k_mp和k_ai、k_mi分别为加速度计和磁力计的比例系数、积分系数，为***输出值、为陀螺仪输出值、φ_a为加速度误差值、φ_m为磁力计误差值，Φ_g、Φ_a、Φ_m分别为φ_a、φ_m的Laplace形式，整个滤波***为二阶互补滤波，其中，G_H(s)为控制输入作用下的传递函数，由G_H(s)传递函数形式可知G_H(s)具有高通滤波性能；G_LA(s)、G_LM(s)分别为对加速度计与磁力计扰动作用下的传递函数，由G_LA(s)、G_LM(s)传递函数形式可知G_LA(s)、G_LM(s)具有低通滤波性能；

根据上述传递函数，有以下时域计算式：

其中分别为当前时刻的累积误差，分别为下一时刻的累积误差，为修正前的陀螺仪输出，为修正后的陀螺仪输出。

进一步地，步骤5中按下式使用步骤三中修正后的陀螺仪输出对当前时刻的四元数进行更新：

其中，I_4×4为4阶单位矩阵，分别为补偿后的陀螺仪输出在x、y、z轴上的分量。

对更新后的四元数Q(k+1)进行归一化处理，通过姿态变换矩阵求出姿态角：

θ＝arcsin(2(q₃q₄+q₁q₂))

本发明具有如下优点：采用四元数法进行姿态的解算与更新，相对于欧拉角法和方向余弦法来说，可以全姿态工作，无奇点存在，同时计算量降低；由于改进互补滤波方法在t≤1200ms的飞行器启动阶段自适应的调节截止频率，故而能够快速响应初始状态的姿态角，较于传统的固定频率互补滤波器有较好的响应快速性。

附图说明

图1改进四元数互补滤波算法原理图

图2传统算法与改进算法的飞行器三轴姿态角仿真轨迹图

图3传统互补滤波实验波形

图4改进互补滤波实验波形

具体实施方式

下面详细描述本发明的实施例，本实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。参照说明书附图对本发明的一种基于改进四元数互补滤波的飞行器姿态解算方法作以下详细地说明：

如图1所示，为四元数互补滤波算法原理图。

首先选取飞行器姿态角及相应的参考坐标系，确定姿态变换矩阵；然后由初始姿态角获得初始四元数；通过姿态变换矩阵、参考向量和加速度计与磁力计的输出值得到加速度计和磁力计误差值；将加速度计和磁力计在不同坐标系下测量的误差值经过PI调节后直接对陀螺仪进行补偿和校正；用补偿后的陀螺仪数据去更新四元数并归一化，再用更新后的四元数计算出姿态变换矩阵并解算出姿态角。

为更好体现本发明的具体步骤实施及效果，搭建如下仿真实验：在仿真实验中设计了数据发生器模拟飞行器的姿态角角速率动态变化情况，采用国际单位(rad/s)：

仿真实验环境相关参数如下：采样周期T_S＝10ms，总采样时间60s。陀螺仪冷启动时间设置为100ms，此时陀螺仪无数据输出。加速度、磁力计冷启动时间较短可忽略不计。四旋翼启动解算载体振动较大，在100ms内，加速度计噪声协方差设置为diag{0.001 0.0010.001}。启动结束后陀螺仪噪声协方差diag{0.01 0.01 0.01}，加速度计噪声协方差diag{0.0001 0.0001 0.0001}，磁力计噪声协方差diag{0.0001 0.0001 0.0001}。

针对以上仿真实验环境，具体步骤实施如下：

1)选取飞行器的参考坐标系，获取姿态角初始值，选取姿态变换矩阵，初始化四元数与姿态变换矩阵

由于本发明涉及俯仰角θ、横滚角γ和航向角ψ，因此飞行器姿态数据采集方案采用陀螺仪、加速度计和磁力计采集姿态数据，选取地理坐标系(东北天)作为导航坐标系，导航系与载体系之间的变换关系可用如下姿态变换矩阵表示：

定义四元数Q＝[q₁，q₂，q₃，q₄]^T，其中q₁为四元数标量部分，q₂，q₃，q₄为四元数矢量部分，初始四元数Q₀可由初始姿态角获得：

2)计算加速度计与磁力计的输出数据与参考向量之间的误差值

参考向量分别为加速度计在导航系下的输出gⁿ＝[0 0 -g]^T和磁力计在导航系下的输出Mⁿ＝[0 E_N E_U]^T，则按下式将加速度计实际测量值与重力在载体系下的分量进行向量积运算可以得到加速度计的误差值e_a

类似地，按下式将磁力计实际测量值与参考向量在载体坐标系下的分量进行向量积运算可以得到磁力计误差值e_m

其中，姿态变换矩阵可按下式由四元数表示：

3)自适应调整比例系数

考虑到加速度计和磁力计高频噪声分布不完全相同，二阶互补滤波的PI调节参数k_ap、k_mp分别选取，其中k_ap、k_mp分别为PI参数中对加速度误差、磁力计误差的比例系数；

k_ap可以根据如下函数自适应获取：

k_ap＝f(t)+f(R_a，R_g，ΔE)+f(ΔZ)

f(ΔZ)＝χ·ΔZ

其中，为理想值，t为运行时间，α，β，μ，χ为调节系数；λ为门限值；R_a为加速度计三轴测量方差模值；R_g为陀螺仪三轴测量方差模值；ΔE为飞行器状态辨别量；ΔZ加速度计与陀螺仪解算角度差值。

加速度计和陀螺仪的三轴测量方差模值为：

其中和分别为x、y、z轴的陀螺仪和加速度计协方差，可由加速度计和陀螺仪测量的多个数据计算得到：

状态辨别变量ΔE为：

其中，n加速度计和陀螺仪测量的多个数据量，||·||为二范数，abs(·)为绝对值函数，A(i)为第i个解算得到的加速度矢量模值，A_x、A_y、A_z分别为x、y、z轴的加速度计输出，为当前时刻解算得到的加速度矢量和模值的均值，ω(i)为第i个解算得到的陀螺仪矢量模值，为陀螺仪输出矢量和模值的均值，为上一时刻陀螺仪解算得到的重力加速度矢量和的模值，磁力计用于偏航角的姿态解算，在飞行器启动过程中不影响控制的稳定性，因此不对其参数k_mp进行自适应设计，采用固定常数。针对本仿真实验，设置参数n＝10。

4)对陀螺仪进行补偿和校正

进一步地，步骤4中，通过步骤3中设计的一个自适应PI调节器的比例系数，利用PI调节器，设计一个二阶互补滤波器，其传递函数如下：

其中，s为微分算子，k_mp和k_ai、k_mi分别为加速度计和磁力计的比例系数、积分系数，为***输出值、为陀螺仪输出值、φ_a为加速度误差值、φ_m为磁力计误差值，Φ_g、Φ_a、Φ_m分别为φ_a、φ_m的Laplace形式，整个滤波***为二阶互补滤波，其中，G_H(s)为控制输入作用下的传递函数，由G_H(s)传递函数形式可知G_H(s)具有高通滤波性能；G_LA(s)、G_LM(s)分别为对加速度计与磁力计扰动作用下的传递函数，由G_LA(s)、G_LM(s)传递函数形式可知G_LA(s)、G_LM(s)具有低通滤波性能；

根据上述传递函数，有以下时域计算式：

其中分别为当前时刻的累积误差，分别为下一时刻的累积误差，为修正前的陀螺仪输出，为修正后的陀螺仪输出。

5)更新四元数并解算姿态角

按下式使用步骤三中修正后的陀螺仪输出对当前时刻的四元数进行更新：

其中，I_4×4为4阶单位矩阵，分别为补偿后的陀螺仪输出在x、y、z轴上的分量。

对更新后的四元数Q(k+1)进行归一化处理，通过姿态变换矩阵求出姿态角：

θ＝arcsin(2(q₃q₄+q₁q₂))

为了观察研究方便，仅截选前20s仿真波形图。图2为改进的互补滤波和传统的互补滤波对比仿真结果：可以看出采用改进的互补滤波算法，俯仰角和偏航角分别在100ms左右和300ms左右开始收敛，传统的互补滤波分别在800ms左右和3000ms才完成收敛。

如图3所示，传统的互补滤波算法在初始阶段很难快速跟上四旋翼的初始真实角度值，动态性能差，收敛慢。当在四旋翼中运用本文所述的改进的互补滤波算法，实验结果如图4所示，可以发现能很好地解决互补滤波在初始时刻的姿态收敛问题，具体表现为启动平稳、迅速，没有磕绊不适感。

Claims (3)

1.一种基于改进四元数互补滤波的飞行器姿态解算方法，其特征在于，具体包括以下步骤：

步骤1：选取飞行器的参考坐标系，获取姿态角初始值，选取姿态变换矩阵，初始化四元数与姿态变换矩阵；

步骤2：采集加速度计、磁力计的输出数据，选取加速度计、磁力计的参考向量，通过姿态变换矩阵计算加速度计、磁力计的输出数据与参考向量之间的误差值；

步骤3：采集加速度计和陀螺仪输出数据获取它们的方差参数，根据方差参数和二阶互补滤波的PI调节器结构，设计一个自适应PI调节器的比例系数；

步骤4：利用PI调节器，设计一个二阶互补滤波器，将加速度计、磁力计的输出数据与参考向量之间的误差值和陀螺仪的输出值经二阶互补滤波器后得到陀螺仪的修正值；

步骤5：用修正后的陀螺仪数据去更新四元数并归一化，再用更新后的四元数计算出姿态变换矩阵并解算出姿态角。

2.根据权利要求1所述的一种基于改进四元数互补滤波的飞行器姿态解算方法，其特征在于：步骤3中设计一个自适应PI调节器的比例系数k_ap，k_ap为PI参数中对加速度误差的比例系数；

k_ap根据如下函数自适应获取：

k_ap＝f(t)+f(R_a，R_g，ΔE)+f(ΔZ)

f(ΔZ)＝χ·ΔZ

其中，为理想值，t为运行时间，α，β，μ，χ为调节系数，λ为门限值，R_a为加速度计三轴测量方差模值，R_g为陀螺仪三轴测量方差模值，ΔE为飞行器状态辨别量，ΔZ加速度计与陀螺仪解算角度差值。

3.根据权利要求1所述的一种基于改进四元数互补滤波的飞行器姿态解算方法，其特征在于：步骤4中通过步骤3中设计的一个自适应PI调节器的比例系数，利用PI调节器，设计一个二阶互补滤波器，其传递函数如下：

其中，s为微分算子，k_mp和k_ai、k_mi分别为加速度计和磁力计的比例系数、积分系数，为***输出值、为陀螺仪输出值、φ_a为加速度误差值、φ_m为磁力计误差值，Φ_g、Φ_a、Φ_m分别为φ_a、φ_m的Laplace形式，整个滤波***为二阶互补滤波，其中，G_H(s)为控制输入作用下的传递函数，由G_H(s)传递函数形式可知G_H(s)具有高通滤波性能；G_LA(s)、G_LM(s)分别为对加速度计与磁力计扰动作用下的传递函数，由G_LA(s)、G_LM(s)传递函数形式可知G_LA(s)、G_LM(s)具有低通滤波性能；

根据上述传递函数，有以下时域计算式：

其中分别为当前时刻的累积误差，分别为下一时刻的累积误差，为修正前的陀螺仪输出，为修正后的陀螺仪输出。