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

摘要

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

主权项

一种基于自适应互补融合的无人机姿态控制系统的控制方法，其特征在于，采用基于自适应互补融合的无人机姿态控制系统，包括电机和与其连接的电机驱动器，电机驱动器的控制信号输入端连接至并联的第一控制器和第二控制器；所述的第一控制器的信号输入端连接自互补型自适应滤波器的信号输出端，互补型自适应滤波器的输入端分别连接第一低通滤波器和四元数微分方程模块，第一低通滤波器的输入端连接加速度计的信号输出端，四元数微分方程模块的信号输入端连接至陀螺仪的信号输出端；所述的第二控制器的信号输入端连接互补滤波器的信号输出端，互补滤波器的输入端连接四元数微分方程模块和第二低通滤波器，第二低通滤波器的输入端连接至磁力计的信号输出端；所述第一控制器和第二控制器的信号输入端均连接至遥控器；其中，陀螺仪为MPU‑6050六轴传感器，磁力计为HMC5883L磁力计，电机驱动器为电子调速器，电机为无刷电机；所述控制方法包括以下步骤：步骤一：获取加速度计、陀螺仪和磁力计传感器测得的数据，并对加速度计和磁力计测得的数据进行低通滤波处理；步骤二：对陀螺仪测得的数据以及低通滤波处理后的加速度计和磁力计数据进行融合获得机体姿态数据；步骤三：根据机体姿态数据和遥控器指令，控制器控制电机，最终控制飞行器姿态；步骤二中对陀螺仪测得的数据以及低通滤波处理后的加速度计和磁力计数据进行融合获得机体姿态数据的方法为：A)陀螺仪数据通过四元数微分方程得到第一姿态四元数；四元数微分方程为：Q·ω,t=12ωnbbQt-1]]>其中为上一周期求得的最终四元数向量，为当前周期四元数的导数；为角速度四元数向量；对该方程近似求解得到第一姿态四元数T为系统更新周期；B)加速度计数据通过梯度下降法得到第二姿态四元数；梯度下降法求取第二姿态四元数公式为式中为上一周期所求姿态四元数，为当前周期所要求的第二姿态四元数；μt为步长，其中，JgT(q^)=dfg(q^,a^)dq^=-q2,t-12q3,t-1-2q0,t-12q1,t-12q1,t-12q0,t-12q3,t-12q2,t-10-4q1,t-1-4q2,t-10]]>fg(q^,a^)=2(q1,t-1q3,t-1-q0,t-1q2,t-1)-ax2(q0,t-1q1,t-1+q2,t-1q3,t-1)-ay2(12-q1,t-12-q2,t-12)-az]]>C)将第一姿态四元数和第二姿态四元数进行互补滤波获得宽范围姿态数据；互补滤波公式为：式中Qω,t为通过步骤A)所求的，Qt为最终姿态四元数，为步骤B)中所求得，α为互补滤波动态系数，通过步骤D)自适应调整；D)引入加速度计值与重力值之差作为互补滤波动态系数，实现自适应调整参数；式中ax、ay、az为加速度计测量值，g为当地重力加速度，K是调节系数，B为静止时滤波系数；E)将步骤C求得的最终姿态四元数转换成横滚角和俯仰角；γ=-arctan2(q2,tq3,t+q0,tq1,t)q0,t2-q1,t2-q2,t2+q3,t2]]>θ＝arcsin2(q1,tq3,t‑q0,tq2,t)其中，θ为俯仰角，γ为横滚角；F)磁力计数据结合步骤E)的γ和θ求得航向角；航向角计算公式如下：Hx=mx×cos(θ)+my×sin(θ)+mz×cos(γ)×sin(γ)Hy=my×cos(γ)+mz×sin(γ)ψm,t=tan-1(Hy/Hx)]]>式中mx、my、mz为磁力计输出数据，θ为俯仰角，γ为横滚角，ψm,t为所求航向角；G)步骤F)求得的航向角与通过步骤A)求得航向角数据互补融合获取宽范围航向角；其融合方式为：ψt＝(1‑β)ψm,t+βψω,t,0≤β≤1，式中ψt为最终所求姿态航向角，为依据步骤A)中的第一姿态四元数所得；其中，第一控制器采用PID控制，第二控制器采用PD控制；且第一控制器和第二控制器输出与遥控器输出指令线性耦合叠加输出，通过电机驱动器驱动电机，进而控制飞行姿态，实现水平运动、垂直运动和旋转运动。