一种基于自适应互补融合的无人机姿态控制系统及方法
【技术领域】
[0001] 本发明属于飞行器控制、信息处理、导航技术领域，具体涉及一种基于自适应互补 融合的无人机姿态控制系统及方法。
【背景技术】
[0002] 常见无人机按其结构可分为固定翼无人机和旋翼式无人机，与固定翼无人机相 比，旋翼无人机具有能够向后飞行、垂直起降和悬停的特点，对起飞、降落场地的条件要求 很少，控制起来非常灵活，能够满足多种用途，因此旋翼无人机具有更大的研宄价值。四 旋翼飞行器与普通旋翼飞行器相比，具有结构简单、故障率低、控制灵活和单位体积能够产 生更大的升力等优点，并且非常适合在狭小的空间内执行任务，拥有广泛的应用前景。
[0003] 但是四旋翼飞行器同时又是一个典型的欠驱动系统，具有多变量、强耦合、非线性 等特性的系统，而且涉及学科多，领域广，包括机体的合理设计、传感器数据的融合、整机的 控制算法等。加上飞行过程中状态复杂，飞行器本身承受多种物理作用和气流等外部环境 的干扰，如旋翼的弹性形变、振动、机身自旋等问题，以及传感器的温漂对控制性能产生的 影响。上述问题给飞行器及其控制系统的设计带来很大的困难。
【发明内容】
[0004] 本发明的目的在于提供一种基于自适应互补融合的无人机姿态控制系统及方法， 以克服上述现有技术存在的缺陷，本发明实现了姿态宽范围高精度解算，数据收敛更快，精 度更高。
[0005] 为达到上述目的，本发明采用如下技术方案：
[0006] 一种基于自适应互补融合的无人机姿态控制系统，包括电机和与其连接的电机驱 动器，电机驱动器的控制信号输入端连接至并联的第一控制器和第二控制器；
[0007] 所述的第一控制器的信号输入端连接自互补型自适应滤波器的信号输出端，互补 型自适应滤波器的输入端分别连接第一低通滤波器和四元数微分方程模块，第一低通滤波 器的输入端连接加速度计的信号输出端，四元数微分方程模块的信号输入端连接至陀螺仪 的信号输出端；
[0008] 所述的第二控制器的信号输入端连接互补滤波器的信号输出端，互补滤波器的输 出端连接四元数微分方程模块和第二低通滤波器，第二低通滤波器的输入端连接至磁力计 的信号输出端；
[0009] 所述第一控制器和第二控制器的信号输入端均连接至遥控器。
[0010] 进一步地，所述的陀螺仪为MPU-6050六轴传感器。
[0011] 进一步地，所述的磁力计为HMC5883L磁力计。
[0012] 进一步地，所述的电机驱动器为电子调速器，电机为无刷电机。
[0013] 一种基于自适应互补融合的无人机姿态控制系统的控制方法，包括以下步骤：
[0014] 步骤一：获取加速度计、陀螺仪和磁力计传感器测得的数据，并对加速度计和磁力 计测得的数据进行低通滤波处理；
[0015] 步骤二：对陀螺仪测得的数据以及低通滤波处理后的加速度计和磁力计数据进行 融合获得机体姿态数据；
[0016] 步骤三：根据机体姿态数据和遥控器指令，控制器控制电机，最终控制飞行器姿 ??τ O
[0017] 进一步地，步骤二中对陀螺仪测得的数据以及低通滤波处理后的加速度计和磁力 计数据进行融合获得机体姿态数据的方法为：
[0018] Α)陀螺仪数据通过四元数微分方程得到第一姿态四元数；
[0019] 四元数微分方程为：
[0020]
[0021] 其中
为上一周期求得的最终四元数向量，为当前周期四元数的 导数；
为角速度四元数向量；对该方程近似求解得到第 一姿态四元数
；T为系统更新周期。
[0022] Β)加速度计数据通过梯度下降法得到第二姿态四元数；
[0023] 最速下降法求取第二姿态四元数公式为 式中
(
为上一周期所求姿态四元数， 为当前周期所要求的第二姿态四元 数；μ A步长，IGW) = 乂:⑷，其中，
[0024]
[0025]
[0026] C)将第一姿态四元数和第二姿态四元数进行互补滤波获得宽范围姿态数据；
[0027] 互补滤波公式为：Qt= (l-α )Q v，t + a Qu，t，〇 彡 α 彡 1
[0028] 式中Qtst为通过步骤Α)所求的，Qt为最终姿态四元数，Q v，t为步骤Β)中所求得， a为互补滤波动态系数，通过步骤D)自适应调整；
[0029] D)引入加速度计值与重力值之差作为互补滤波动态系数，实现自适应调整参数；
[0030]
,式中ax、ay、a z为加速度计测量值，g为当地重力加速 度，K是调节系数，B为静止时滤波系数；
[0031] E)将步骤C求得的最终姿态四元数
转换成横滚角和俯仰角；
[0032]
[0033] Θ = arcsin2(q1；tq3；t-q〇；tq2；t)
[0034] 其中，Θ为俯仰角，γ为横滚角；
[0035] F)磁力计数据结合步骤Ε)的γ和Θ求得航向角；
[0036] 航向角计算公式如下：
[0037]
[0038] 式中mx、my、mz为磁力计输出数据，Θ为俯仰角，γ为横滚角，φ _^为所求航向角；
[0039] G)步骤F)求得的航向角与通过步骤Α)求得航向角数据互补融合获取宽范围航向 角；
[0040] 其融合方式为：Itt= (1-β) Φ w+β Φ ω;?，0彡β彡1，式中Itt为最终所求 姿态航向角，
为依据步骤Α)中的第一姿态四元数
[0041] 进一步地，第一控制器采用PID控制，第二控制器采用ro控制；且第一控制器和第 二控制器输出与遥控器输出指令线性耦合叠加输出，通过电机驱动器驱动电机，进而控制 飞行姿态，实现水平运动、垂直运动和旋转运动。
[0042] 与现有技术相比，本发明具有以下有益的技术效果：
[0043] 本发明一种基于自适应互补融合的无人机姿态控制系统，通过加速度计传感器、 陀螺仪传感器和磁力计同时采集信号，之后通过低通滤波器、四元数微分方程模块、自适应 互补滤波器对信号进行处理，并将处理后的信号分别传输给控制器，控制器通过控制电机 驱动器对电机进行控制，从而实现了对四轴无人机姿态的控制。
[0044] 一种基于自适应互补融合的无人机姿态控制方法通过获取加速度计、陀螺仪和磁 力计传感器数据，并进行滤波处理；对处理后的数据进行数据融合获取机体姿态；根据机 体姿态数据和遥控数据通过控制器控制电机转速，最终控制飞行器姿态。采用了精炼高效 的算法，可以使程序在低成本处理器下实时运行。并达到精确控制。对比与同类互补滤波 算法，本发明在四元数基础上引入自适应控制，实现了姿态宽范围高精度解算，数据收敛更 快，精度更高。本发明有效的将磁场数据有效的分离出来，无人机在受到磁场干扰时不会出 现全姿态错误，避免了在磁场受到干扰时坠机事故的发生。
【附图说明】
[0045] 图1为本发明系统总体流程图；
[0046] 图2为本发明最速下降法融合自适应互补滤波算法流程图；
[0047] 图3为本发明磁力计单独计算航向角流程图；
[0048] 图4为本发明程序执行流程图；
[0049] 图5为本发明与传统控制方法姿态响应精度比较图。
【具体实施方式】
[0050] 下面结合附图对本发明作进一步详细描述：
[0051] 参见图1至图4,一种基于自适应互补融合的无人机姿态控制系统，包括电机和与 其连接的电机驱动器，电机驱动器的控制信号输入端连接至并联的第一控制器和第二控制 器；
[0052] 所述的第一控制器的信号输入端连接自互补型自适应滤波器的信号输出端，互补 型自适应滤波器的输入端分别连接第一低通滤波器和四元数微分方程模块，第一低通滤波 器的输入端连接加速度计的信号输出端，四元数微分方程模块的信号输入端连接至陀螺仪 的信号输出端；
[0053] 所述的第二控制器的信号输入端连接互补滤波器的信号输出端，互补滤波器的输 出端连接四元数微分方程模块和第二低通滤波器，第二低通滤波器的输入端连接至磁力计 的信号输出端；
[0054] 所述第一控制器和第二控制器的信号输入端均连接至遥控器。
[0055] 本发明所用加速度计要求ADC精度不低于12位，陀螺仪要求ADC精度不低于14 位，这里采用invensense公司的MPU-6050六轴传感器，整合了三轴加速度计和三轴陀螺 仪，16ADC位精度，数字输出；所用磁力计要求ADC精度不低于12位，这里采用HMC5883L 磁力计，12位精度，数字量输出；采用处理器为ST公司出产的STM32F103系列芯片，时钟可 达72MHz，内部资源丰富；所用电机驱动为好盈电子调速器，电机为朗宇无刷电机；所用遥 控器为天地飞六通道遥控器，可发送六个ppm信号。
[0056] 其中加速度计通过测量比力大小进而测量加速度大小，物体静止时加速度计输出 为重力加速度，方向竖直向下，当有其他方向加速度时，加速度计输出加速度