CN104914874A - 一种基于自适应互补融合的无人机姿态控制系统及方法 - Google Patents

Abstract

本发明公开了一种基于自适应互补融合的无人机姿态控制系统及方法，首先读取传感器数据并对进行相应滤波；然后针对不同传感器的频域特性进行自适应互补融合，得到当前飞行器的空中姿态信息，进而得到控制所需的欧拉角；接着对飞行器进行数学建模并设计了两个控制器，其一是用来对俯仰角和横滚角控制；其二用来对航向角控制；最后对两个控制器输出进行叠加获得一个总的输出，并通过驱动器改变电机转速实现对飞行器的平稳控制。本发明实现了姿态宽范围高精度解算，数据收敛更快，精度更高。

Description

一种基于自适应互补融合的无人机姿态控制系统及方法

技术领域

本发明属于飞行器控制、信息处理、导航技术领域，具体涉及一种基于自适应互补融合的无人机姿态控制系统及方法。

背景技术

常见无人机按其结构可分为固定翼无人机和旋翼式无人机，与固定翼无人机相比,旋翼无人机具有能够向后飞行、垂直起降和悬停的特点,对起飞、降落场地的条件要求很少,控制起来非常灵活,能够满足多种用途,因此旋翼无人机具有更大的研究价值。四旋翼飞行器与普通旋翼飞行器相比，具有结构简单、故障率低、控制灵活和单位体积能够产生更大的升力等优点，并且非常适合在狭小的空间内执行任务，拥有广泛的应用前景。

但是四旋翼飞行器同时又是一个典型的欠驱动系统，具有多变量、强耦合、非线性等特性的系统，而且涉及学科多，领域广，包括机体的合理设计、传感器数据的融合、整机的控制算法等。加上飞行过程中状态复杂，飞行器本身承受多种物理作用和气流等外部环境的干扰，如旋翼的弹性形变、振动、机身自旋等问题，以及传感器的温漂对控制性能产生的影响。上述问题给飞行器及其控制系统的设计带来很大的困难。

发明内容

本发明的目的在于提供一种基于自适应互补融合的无人机姿态控制系统及方法，以克服上述现有技术存在的缺陷，本发明实现了姿态宽范围高精度解算，数据收敛更快，精度更高。

为达到上述目的，本发明采用如下技术方案：

一种基于自适应互补融合的无人机姿态控制系统，包括电机和与其连接的电机驱动器，电机驱动器的控制信号输入端连接至并联的第一控制器和第二控制器；

所述的第一控制器的信号输入端连接自互补型自适应滤波器的信号输出端，互补型自适应滤波器的输入端分别连接第一低通滤波器和四元数微分方程模块，第一低通滤波器的输入端连接加速度计的信号输出端，四元数微分方程模块的信号输入端连接至陀螺仪的信号输出端；

所述的第二控制器的信号输入端连接互补滤波器的信号输出端，互补滤波器的输出端连接四元数微分方程模块和第二低通滤波器，第二低通滤波器的输入端连接至磁力计的信号输出端；

所述第一控制器和第二控制器的信号输入端均连接至遥控器。

进一步地，所述的陀螺仪为MPU-6050六轴传感器。

进一步地，所述的磁力计为HMC5883L磁力计。

进一步地，所述的电机驱动器为电子调速器，电机为无刷电机。

一种基于自适应互补融合的无人机姿态控制系统的控制方法，包括以下步骤：

步骤一：获取加速度计、陀螺仪和磁力计传感器测得的数据，并对加速度计和磁力计测得的数据进行低通滤波处理；

步骤二：对陀螺仪测得的数据以及低通滤波处理后的加速度计和磁力计数据进行融合获得机体姿态数据；

步骤三：根据机体姿态数据和遥控器指令，控制器控制电机，最终控制飞行器姿态。

进一步地，步骤二中对陀螺仪测得的数据以及低通滤波处理后的加速度计和磁力计数据进行融合获得机体姿态数据的方法为：

A)陀螺仪数据通过四元数微分方程得到第一姿态四元数；

四元数微分方程为：

${\stackrel{\·}{Q}}_{\mathrm{\ω},t}=\frac{1}{2}{\mathrm{\ω}}_{\mathrm{nb}}^b{Q}_{t-1}$

其中 $Q_{t-1}=\left[\begin{array}{c}{q}_{0,t-1}\\ q_{1,t-1}\\ q_{2,t-1}\\ q_{3,t-1}\end{array}\right]$ 为上一周期求得的最终四元数向量，为当前周期四元数的导数； ${\mathrm{\ω}}_{\mathrm{nb}}^b=\left[\begin{array}{cccc}0& -w_{\mathrm{nbx}}^b& -w_{\mathrm{nby}}^b& -w_{\mathrm{nbz}}^b\\ -w_{\mathrm{nbx}}^b& 0& w_{\mathrm{nbz}}^b& -w_{\mathrm{nby}}^b\\ w_{\mathrm{nby}}^b& -w_{\mathrm{nbz}}^b& 0& w_{\mathrm{nbx}}^b\\ w_{\mathrm{nbz}}^b& w_{\mathrm{nby}}^b& -w_{\mathrm{nbx}}^b& 0\end{array}\right]$ 为角速度四元数向量；对该方程近似求解得到第一姿态四元数T为系统更新周期。

B)加速度计数据通过梯度下降法得到第二姿态四元数；

最速下降法求取第二姿态四元数公式为式中 $Q_{t-1}=\left[\begin{array}{c}{q}_{0,t-1}\\ q_{1,t-1}\\ q_{2,t-1}\\ q_{3,t-1}\end{array}\right]$ 为上一周期所求姿态四元数， $Q_{\▿,t}=\left[\begin{array}{c}{q}_{\▿0,t}\\ q_{\▿1,t}\\ q_{\▿2,t}\\ q_{\▿3,t}\end{array}\right]$ 为当前周期所要求的第二姿态四元数；μ_t为步长，其中，

$J_g^T\left(\hat{q}\right)=\frac{d{f}_g(\hat{q},\hat{a})}{d\hat{q}}=\left[\begin{array}{cccc}-q_{2,t-1}& 2q_{3,t-1}& -2q_{0,t-1}& 2q_{1,t-1}\\ 2q_{1,t-1}& 2q_{0,t-1}& 2q_{3,t-1}& 2q_{2,t-1}\\ 0& -4q_{1,t-1}& -4q_{2,t-1}& 0\end{array}\right]$

$f_g(\hat{q},\hat{a})=\left[\begin{array}{c}2(q_{1,t-1}{q}_{3,t-1}-q_{0,t-1}{q}_{2,t-1})-a_x\\ 2(q_{0,t-1}{q}_{1,t-1}+q_{2,t-1}{q}_{3,t-1})-a_y\\ 2(\frac{1}{2}-q_{1,t-1}^2-q_{2,t-1}^2)-a_z\end{array}\right]$

C)将第一姿态四元数和第二姿态四元数进行互补滤波获得宽范围姿态数据；

互补滤波公式为：Q_t＝(1-α)Q_▽,t+αQ_ω,t,0≤α≤1

式中Q_ω,t为通过步骤A)所求的，Q_t为最终姿态四元数，Q_▽,t为步骤B)中所求得，α为互补滤波动态系数，通过步骤D)自适应调整；

D)引入加速度计值与重力值之差作为互补滤波动态系数，实现自适应调整参数；

式中a_x、a_y、a_z为加速度计测量值，g为当地重力加速度，K是调节系数，B为静止时滤波系数；

E)将步骤C求得的最终姿态四元数 $Q_t=\left[\begin{array}{c}{q}_{0,t}\\ q_{1,t}\\ q_{2,t}\\ q_{3,t}\end{array}\right]$ 转换成横滚角和俯仰角；

$\mathrm{\γ}=-\arctan \frac{2(q_{2,t}{q}_{3,t}+q_{0,t}{q}_{1,t})}{q_{0,t}^2-q_{1,t}^2-q_{2,t}^2+q_{3,t}^2}$

θ＝arcsin2(q_1,tq_3,t-q_0,tq_2,t)

其中，θ为俯仰角，γ为横滚角；

F)磁力计数据结合步骤E)的γ和θ求得航向角；

航向角计算公式如下：

$\left\{\begin{array}{c}{H}_x=m_x\×\cos \left(\mathrm{\θ}\right)+m_y\×\sin \left(\mathrm{\θ}\right)+m_z\×\cos \left(\mathrm{\γ}\right)\×\sin \left(\mathrm{\γ}\right)\\ H_y=m_y\×\cos \left(\mathrm{\γ}\right)+m_z\×\sin \left(\mathrm{\γ}\right)\\ {\mathrm{\ψ}}_{m,t}={\tan }^{-1}(H_y/H_x)\end{array}\right.$

式中m_x、m_y、m_z为磁力计输出数据，θ为俯仰角，γ为横滚角，ψ_m,t为所求航向角；

G)步骤F)求得的航向角与通过步骤A)求得航向角数据互补融合获取宽范围航向角；

其融合方式为：ψ_t＝(1-β)ψ_m,t+βψ_ω,t,0≤β≤1，式中ψ_t为最终所求姿态航向角，为依据步骤A)中的第一姿态四元数 $Q_{\mathrm{\ω},t}=\left[\begin{array}{c}{q}_{\mathrm{\ω}0,t}\\ q_{\mathrm{\ω}1,t}\\ q_{\mathrm{\ω}2,t}\\ q_{\mathrm{\ω}3,t}\end{array}\right]$ 所得。

进一步地，第一控制器采用PID控制，第二控制器采用PD控制；且第一控制器和第二控制器输出与遥控器输出指令线性耦合叠加输出，通过电机驱动器驱动电机，进而控制飞行姿态，实现水平运动、垂直运动和旋转运动。

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

本发明一种基于自适应互补融合的无人机姿态控制系统，通过加速度计传感器、陀螺仪传感器和磁力计同时采集信号，之后通过低通滤波器、四元数微分方程模块、自适应互补滤波器对信号进行处理，并将处理后的信号分别传输给控制器，控制器通过控制电机驱动器对电机进行控制，从而实现了对四轴无人机姿态的控制。

一种基于自适应互补融合的无人机姿态控制方法通过获取加速度计、陀螺仪和磁力计传感器数据，并进行滤波处理；对处理后的数据进行数据融合获取机体姿态；根据机体姿态数据和遥控数据通过控制器控制电机转速，最终控制飞行器姿态。采用了精炼高效的算法，可以使程序在低成本处理器下实时运行。并达到精确控制。对比与同类互补滤波算法，本发明在四元数基础上引入自适应控制，实现了姿态宽范围高精度解算，数据收敛更快，精度更高。本发明有效的将磁场数据有效的分离出来，无人机在受到磁场干扰时不会出现全姿态错误，避免了在磁场受到干扰时坠机事故的发生。

附图说明

图1为本发明系统总体流程图；

图2为本发明最速下降法融合自适应互补滤波算法流程图；

图3为本发明磁力计单独计算航向角流程图；

图4为本发明程序执行流程图；

图5为本发明与传统控制方法姿态响应精度比较图。

具体实施方式

下面结合附图对本发明作进一步详细描述：

参见图1至图4，一种基于自适应互补融合的无人机姿态控制系统，包括电机和与其连接的电机驱动器，电机驱动器的控制信号输入端连接至并联的第一控制器和第二控制器；

所述的第一控制器的信号输入端连接自互补型自适应滤波器的信号输出端，互补型自适应滤波器的输入端分别连接第一低通滤波器和四元数微分方程模块，第一低通滤波器的输入端连接加速度计的信号输出端，四元数微分方程模块的信号输入端连接至陀螺仪的信号输出端；

所述的第二控制器的信号输入端连接互补滤波器的信号输出端，互补滤波器的输出端连接四元数微分方程模块和第二低通滤波器，第二低通滤波器的输入端连接至磁力计的信号输出端；

所述第一控制器和第二控制器的信号输入端均连接至遥控器。

本发明所用加速度计要求ADC精度不低于12位，陀螺仪要求ADC精度不低于14位，这里采用invensense公司的MPU-6050六轴传感器，整合了三轴加速度计和三轴陀螺仪，16ADC位精度，数字输出；所用磁力计要求ADC精度不低于12位,这里采用HMC5883L磁力计，12位精度，数字量输出；采用处理器为ST公司出产的STM32F103系列芯片，时钟可达72MHz，内部资源丰富；所用电机驱动为好盈电子调速器，电机为朗宇无刷电机；所用遥控器为天地飞六通道遥控器，可发送六个ppm信号。

其中加速度计通过测量比力大小进而测量加速度大小，物体静止时加速度计输出为重力加速度，方向竖直向下，当有其他方向加速度时，加速度计输出加速度之和。加速度计根据重力加速度可计算出载体角度；陀螺仪是角速度陀螺仪，其测量的是角速度大小，对加速度积分可得角度，此角度会随着陀螺仪零飘缓慢漂移；磁力计用来测量地磁场，并根据地磁场的方向计算出载体方向。

一种基于自适应互补融合的无人机姿态控制系统的控制方法，包括以下步骤：

步骤一：通过处理器获取加速度计、陀螺仪和磁力计传感器测得的数据，并对加速度计和磁力计测得的数据进行低通滤波处理；

步骤二：由处理器对陀螺仪测得的数据以及低通滤波处理后的加速度计和磁力计数据进行融合获得机体姿态数据；

具体方法为：A)陀螺仪数据通过四元数微分方程得到第一姿态四元数；

四元数微分方程为：

${\stackrel{\·}{Q}}_{\mathrm{\ω},t}=\frac{1}{2}{\mathrm{\ω}}_{\mathrm{nb}}^b{Q}_{t-1}$

其中 $Q_{t-1}=\left[\begin{array}{c}{q}_{0,t-1}\\ q_{1,t-1}\\ q_{2,t-1}\\ q_{3,t-1}\end{array}\right]$ 为上一周期求得的最终四元数向量，为当前周期四元数的导数； ${\mathrm{\ω}}_{\mathrm{nb}}^b=\left[\begin{array}{cccc}0& -w_{\mathrm{nbx}}^b& -w_{\mathrm{nby}}^b& -w_{\mathrm{nbz}}^b\\ -w_{\mathrm{nbx}}^b& 0& w_{\mathrm{nbz}}^b& -w_{\mathrm{nby}}^b\\ w_{\mathrm{nby}}^b& -w_{\mathrm{nbz}}^b& 0& w_{\mathrm{nbx}}^b\\ w_{\mathrm{nbz}}^b& w_{\mathrm{nby}}^b& -w_{\mathrm{nbx}}^b& 0\end{array}\right]$ 为角速度四元数向量；对该方程近似求解得到第一姿态四元数T为系统更新周期。

B)加速度计数据通过梯度下降法得到第二姿态四元数；

最速下降法求取第二姿态四元数公式为式中 $Q_{t-1}=\left[\begin{array}{c}{q}_{0,t-1}\\ q_{1,t-1}\\ q_{2,t-1}\\ q_{3,t-1}\end{array}\right]$ 为上一周期所求姿态四元数， $Q_{\▿,t}=\left[\begin{array}{c}{q}_{\▿0,t}\\ q_{\▿1,t}\\ q_{\▿2,t}\\ q_{\▿3,t}\end{array}\right]$ 为当前周期所要求的第二姿态四元数；μ_t为步长，其中，

$J_g^T\left(\hat{q}\right)=\frac{d{f}_g(\hat{q},\hat{a})}{d\hat{q}}=\left[\begin{array}{cccc}-q_{2,t-1}& 2q_{3,t-1}& -2q_{0,t-1}& 2q_{1,t-1}\\ 2q_{1,t-1}& 2q_{0,t-1}& 2q_{3,t-1}& 2q_{2,t-1}\\ 0& -4q_{1,t-1}& -4q_{2,t-1}& 0\end{array}\right]$

$f_g(\hat{q},\hat{a})=\left[\begin{array}{c}2(q_{1,t-1}{q}_{3,t-1}-q_{0,t-1}{q}_{2,t-1})-a_x\\ 2(q_{0,t-1}{q}_{1,t-1}+q_{2,t-1}{q}_{3,t-1})-a_y\\ 2(\frac{1}{2}-q_{1,t-1}^2-q_{2,t-1}^2)-a_z\end{array}\right]$

C)将第一姿态四元数和第二姿态四元数进行互补滤波获得宽范围姿态数据；

互补滤波公式为：Q_t＝(1-α)Q_▽,t+αQ_ω,t,0≤α≤1

式中Q_ω,t为通过步骤A)所求的，Q_t为最终姿态四元数，Q_▽,t为步骤B)中所求得，α为互补滤波动态系数，通过步骤D)自适应调整；

D)引入加速度计值与重力值之差作为互补滤波动态系数，实现自适应调整参数；

式中a_x、a_y、a_z为加速度计测量值，g为当地重力加速度，K是调节系数，B为静止时滤波系数；

E)将步骤C求得的最终姿态四元数 $Q_t=\left[\begin{array}{c}{q}_{0,t}\\ q_{1,t}\\ q_{2,t}\\ q_{3,t}\end{array}\right]$ 转换成横滚角和俯仰角；

$\mathrm{\γ}=-\arctan \frac{2(q_{2,t}{q}_{3,t}+q_{0,t}{q}_{1,t})}{q_{0,t}^2-q_{1,t}^2-q_{2,t}^2+q_{3,t}^2}$

θ＝arcsin2(q_1,tq_3,t-q_0,tq_2,t)

其中，θ为俯仰角，γ为横滚角；

F)磁力计数据结合步骤E)的γ和θ求得航向角；

航向角计算公式如下：

$\left\{\begin{array}{c}{H}_x=m_x\×\cos \left(\mathrm{\θ}\right)+m_y\×\sin \left(\mathrm{\θ}\right)+m_z\×\cos \left(\mathrm{\γ}\right)\×\sin \left(\mathrm{\γ}\right)\\ H_y=m_y\×\cos \left(\mathrm{\γ}\right)+m_z\×\sin \left(\mathrm{\γ}\right)\\ {\mathrm{\ψ}}_{m,t}={\tan }^{-1}(H_y/H_x)\end{array}\right.$

式中m_x、m_y、m_z为磁力计输出数据，θ为俯仰角，γ为横滚角，ψ_m,t为所求航向角；

G)步骤F)求得的航向角与通过步骤A)求得航向角数据互补融合获取宽范围航向角；

其融合方式为：ψ_t＝(1-β)ψ_m,t+βψ_ω,t,0≤β≤1，式中ψ_t为最终所求姿态航向角，为依据步骤A)中的第一姿态四元数 $Q_{\mathrm{\ω},t}=\left[\begin{array}{c}{q}_{\mathrm{\ω}0,t}\\ q_{\mathrm{\ω}1,t}\\ q_{\mathrm{\ω}2,t}\\ q_{\mathrm{\ω}3,t}\end{array}\right]$ 所得。

步骤三：根据机体姿态数据和遥控器指令，控制器控制电机，最终控制飞行器姿态，第一控制器采用PID控制，第二控制器采用PD控制；且第一控制器和第二控制器输出与遥控器输出指令线性耦合叠加输出，通过电机驱动器驱动电机，进而控制飞行姿态，实现水平运动、垂直运动和旋转运动。

下面对本发明的实施过程作进一步详细说明：

将程序下载到处理器后，上电时处理器首先初始化传感器，并对其进行零偏校正，处理器分别读取各传感器200次数据，取其平均值作为零偏值。然后等待遥控发送指令，当处理器接收到解锁指令时，处理器以2ms为周期读取MPU-6050传感器数值并对加速度数据进行低通滤波，低通滤波采用滑动均值滤波，然后进行横滚角和俯仰角的姿态解算。姿态解算后对俯仰角、横滚角进行PID控制对航向角进行PD控制，计算出对应四个电机所需PWM。在空闲时间连续读取磁力计数值并进行航向角姿态解算。最终解算的欧拉姿态角送到响应控制器进行控制。遥控发送的数据实时传送到控制器里进行高度，方向的控制。图5为本发明与传统控制方法姿态响应精度比较图，从图5可以看出：梯度自适应算法比传统AHRS算法响应更快，更准，动态精度更高，更适用于实际飞行中的无人机控制，可以开发精确度更高的算法。

Claims (7)

1.一种基于自适应互补融合的无人机姿态控制系统，其特征在于，包括电机和与其连接的电机驱动器，电机驱动器的控制信号输入端连接至并联的第一控制器和第二控制器；

所述的第一控制器的信号输入端连接自互补型自适应滤波器的信号输出端，互补型自适应滤波器的输入端分别连接第一低通滤波器和四元数微分方程模块，第一低通滤波器的输入端连接加速度计的信号输出端，四元数微分方程模块的信号输入端连接至陀螺仪的信号输出端；

所述的第二控制器的信号输入端连接互补滤波器的信号输出端，互补滤波器的输出端连接四元数微分方程模块和第二低通滤波器，第二低通滤波器的输入端连接至磁力计的信号输出端；

所述第一控制器和第二控制器的信号输入端均连接至遥控器。

2.根据权利要求1所述的一种基于自适应互补融合的无人机姿态控制系统，其特征在于，所述的陀螺仪为MPU-6050六轴传感器。

3.根据权利要求1所述的一种基于自适应互补融合的无人机姿态控制系统，其特征在于，所述的磁力计为HMC5883L磁力计。

4.根据权利要求1所述的一种基于自适应互补融合的无人机姿态控制系统，其特征在于，所述的电机驱动器为电子调速器，电机为无刷电机。

5.权利要求1-4任意一项所述的一种基于自适应互补融合的无人机姿态控制系统的控制方法，其特征在于，包括以下步骤：

步骤一：获取加速度计、陀螺仪和磁力计传感器测得的数据，并对加速度计和磁力计测得的数据进行低通滤波处理；

步骤二：对陀螺仪测得的数据以及低通滤波处理后的加速度计和磁力计数据进行融合获得机体姿态数据；

步骤三：根据机体姿态数据和遥控器指令，控制器控制电机，最终控制飞行器姿态。

6.根据权利要求5所述的一种基于自适应互补融合的无人机姿态控制系统的控制方法，其特征在于，步骤二中对陀螺仪测得的数据以及低通滤波处理后的加速度计和磁力计数据进行融合获得机体姿态数据的方法为：

A)陀螺仪数据通过四元数微分方程得到第一姿态四元数；

四元数微分方程为：

${\stackrel{\·}{Q}}_{\mathrm{\ω},t}=\frac{1}{2}{\mathrm{\ω}}_{\mathrm{nb}}^b{Q}_{t-1}$

其中 $Q_{t-1}=\left[\begin{array}{c}{q}_{0,t-1}\\ q_{1,t-1}\\ q_{2,t-1}\\ q_{3,t-1}\end{array}\right]$ 为上一周期求得的最终四元数向量，为当前周期四元数的导数； ${\mathrm{\ω}}_{\mathrm{nb}}^b=\left[\begin{array}{cccc}0& -w_{\mathrm{nbx}}^b& -w_{\mathrm{nby}}^b& -w_{\mathrm{nbz}}^b\\ -w_{\mathrm{nbx}}^b& 0& w_{\mathrm{nbz}}^b& -w_{\mathrm{nby}}^b\\ w_{\mathrm{nby}}^b& -w_{\mathrm{nbz}}^b& 0& w_{\mathrm{nbx}}^b\\ w_{\mathrm{nbz}}^b& w_{\mathrm{nby}}^b& -w_{\mathrm{nbx}}^b& 0\end{array}\right]$ 为角速度四元数向量；对该方程近似求解得到第一姿态四元数T为系统更新周期；

B)加速度计数据通过梯度下降法得到第二姿态四元数；

最速下降法求取第二姿态四元数公式为式中 $Q_{t-1}=\left[\begin{array}{c}{q}_{0,t-1}\\ q_{1,t-1}\\ q_{2,t-1}\\ q_{3,t-1}\end{array}\right]$ 为上一周期所求姿态四元数， $Q_{\▿,t}=\left[\begin{array}{c}{q}_{\▿0,t}\\ q_{\▿1,t}\\ q_{\▿2,t}\\ q_{\▿3,t}\end{array}\right]$ 为当前周期所要求的第二姿态四元数；μ_t为步长， ${\▿f}_g(\hat{q},\hat{a})=J_g^T\left(\hat{q}\right)*f_g(\hat{q},\hat{a}),$ 其中，

$J_g^T\left(\hat{q}\right)=\frac{{\mathrm{df}}_g(\hat{q},\hat{a})}{d\hat{q}}=\left[\begin{array}{cccc}-q_{2,t-1}& {2q}_{3,t-1}& -{2q}_{0,t-1}& {2q}_{1,t-1}\\ {2q}_{1,t-1}& {2q}_{0,t-1}& {2q}_{3,t-1}& {2q}_{2,t-1}\\ 0& -{4q}_{1,t-1}& -{4q}_{2,t-1}& 0\end{array}\right]$

$f_g(\hat{q},\hat{a})=\left[\begin{array}{c}2(q_{1,t-1}{q}_{3,t-1}-q_{0,t-1}{q}_{2,t-1})-a_x\\ 2(q_{0,t-1}{q}_{1,t-1}+q_{2,t-1}{q}_{3,t-1})-a_y\\ 2(\frac{1}{2}-q_{1,t-1}^2-q_{2,t-1}^2)-a_z\end{array}\right]$

C)将第一姿态四元数和第二姿态四元数进行互补滤波获得宽范围姿态数据；

互补滤波公式为： $Q_t=(1-\mathrm{\α})Q_{\▿,t}+\mathrm{\α}{Q}_{\mathrm{\ω},t},0\≤\mathrm{\α}\≤1$

式中Q_ω,t为通过步骤A)所求的，Q_t为最终姿态四元数，为步骤B)中所求得，α为互补滤波动态系数，通过步骤D)自适应调整；

D)引入加速度计值与重力值之差作为互补滤波动态系数，实现自适应调整参数；

式中a_x、a_y、a_z为加速度计测量值，g为当地重力加速度，K是调节系数，B为静止时滤波系数；

E)将步骤C求得的最终姿态四元数 $Q_t=\left[\begin{array}{c}{q}_{0,t}\\ q_{1,t}\\ q_{2,t}\\ q_{3,t}\end{array}\right]$ 转换成横滚角和俯仰角；

$\mathrm{\γ}=-\arctan \frac{2(q_{2,t}{q}_{3,t}+q_{0,t}{q}_{1,t})}{q_{0,t}^2-q_{1,t}^2-q_{2,t}^2+q_{3,t}^2}$

θ＝arcsin2(q_1,tq_3,t-q_0,tq_2,t)

其中，θ为俯仰角，γ为横滚角；

F)磁力计数据结合步骤E)的γ和θ求得航向角；

航向角计算公式如下：

$\left\{\begin{array}{c}{H}_x=m_x\×\cos \left(\mathrm{\θ}\right)+m_y\×\sin \left(\mathrm{\θ}\right)+m_z\×\cos \left(\mathrm{\γ}\right)\×\sin \left(\mathrm{\γ}\right)\\ H_y=m_y\×\cos \left(\mathrm{\γ}\right)+m_z\×\sin \left(\mathrm{\γ}\right)\\ {\mathrm{\ψ}}_{m,t}={\tan }^{-1}(H_y/H_x)\end{array}\right.$

式中m_x、m_y、m_z为磁力计输出数据，θ为俯仰角，γ为横滚角，ψ_m,t为所求航向角；

G)步骤F)求得的航向角与通过步骤A)求得航向角数据互补融合获取宽范围航向角；

其融合方式为：ψ_t＝(1-β)ψ_m,t+βψ_ω,t,0≤β≤1，式中ψ_t为最终所求姿态航向角，为依据步骤A)中的第一姿态四元数 $Q_{\mathrm{\ω},t}=\left[\begin{array}{c}{q}_{\mathrm{\ω}0,t}\\ q_{\mathrm{\ω}1,t}\\ q_{\mathrm{\ω}2,t}\\ q_{\mathrm{\ω}3,t}\end{array}\right]$ 所得。

7.根据权利要求5所述的一种基于自适应互补融合的无人机姿态控制系统的控制方法，其特征在于，第一控制器采用PID控制，第二控制器采用PD控制；且第一控制器和第二控制器输出与遥控器输出指令线性耦合叠加输出，通过电机驱动器驱动电机，进而控制飞行姿态，实现水平运动、垂直运动和旋转运动。