CN104914874B - 一种基于自适应互补融合的无人机姿态控制***及方法 - Google Patents
一种基于自适应互补融合的无人机姿态控制***及方法 Download PDFInfo
- Publication number
- CN104914874B CN104914874B CN201510313361.6A CN201510313361A CN104914874B CN 104914874 B CN104914874 B CN 104914874B CN 201510313361 A CN201510313361 A CN 201510313361A CN 104914874 B CN104914874 B CN 104914874B
- Authority
- CN
- China
- Prior art keywords
- mrow
- msub
- mtd
- attitude
- mtr
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 22
- 230000004927 fusion Effects 0.000 title claims abstract description 18
- 230000003044 adaptive effect Effects 0.000 title claims abstract description 8
- 230000000295 complement effect Effects 0.000 claims description 32
- 238000001914 filtration Methods 0.000 claims description 27
- 230000001133 acceleration Effects 0.000 claims description 13
- 230000005484 gravity Effects 0.000 claims description 8
- 238000004364 calculation method Methods 0.000 claims description 7
- 238000012545 processing Methods 0.000 claims description 5
- 238000011478 gradient descent method Methods 0.000 claims description 4
- 230000003068 static effect Effects 0.000 claims description 4
- 230000004044 response Effects 0.000 description 5
- 238000002945 steepest descent method Methods 0.000 description 3
- 150000001875 compounds Chemical class 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 230000005358 geomagnetic field Effects 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000005489 elastic deformation Effects 0.000 description 1
- 230000010365 information processing Effects 0.000 description 1
- 238000007500 overflow downdraw method Methods 0.000 description 1
- 230000000704 physical effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000000926 separation method Methods 0.000 description 1
- 238000009987 spinning Methods 0.000 description 1
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Gyroscopes (AREA)
Abstract
本发明公开了一种基于自适应互补融合的无人机姿态控制***及方法，首先读取传感器数据并对进行相应滤波；然后针对不同传感器的频域特性进行自适应互补融合，得到当前飞行器的空中姿态信息，进而得到控制所需的欧拉角；接着对飞行器进行数学建模并设计了两个控制器，其一是用来对俯仰角和横滚角控制；其二用来对航向角控制；最后对两个控制器输出进行叠加获得一个总的输出，并通过驱动器改变电机转速实现对飞行器的平稳控制。本发明实现了姿态宽范围高精度解算，数据收敛更快，精度更高。
Description
技术领域
本发明属于飞行器控制、信息处理、导航技术领域，具体涉及一种基于自适应互补融合的无人机姿态控制***及方法。
背景技术
常见无人机按其结构可分为固定翼无人机和旋翼式无人机，与固定翼无人机相比,旋翼无人机具有能够向后飞行、垂直起降和悬停的特点,对起飞、降落场地的条件要求很少,控制起来非常灵活,能够满足多种用途,因此旋翼无人机具有更大的研究价值。四旋翼飞行器与普通旋翼飞行器相比，具有结构简单、故障率低、控制灵活和单位体积能够产生更大的升力等优点，并且非常适合在狭小的空间内执行任务，拥有广泛的应用前景。
但是四旋翼飞行器同时又是一个典型的欠驱动***，具有多变量、强耦合、非线性等特性的***，而且涉及学科多，领域广，包括机体的合理设计、传感器数据的融合、整机的控制算法等。加上飞行过程中状态复杂，飞行器本身承受多种物理作用和气流等外部环境的干扰，如旋翼的弹性形变、振动、机身自旋等问题，以及传感器的温漂对控制性能产生的影响。上述问题给飞行器及其控制***的设计带来很大的困难。
发明内容
本发明的目的在于提供一种基于自适应互补融合的无人机姿态控制***及方法，以克服上述现有技术存在的缺陷，本发明实现了姿态宽范围高精度解算，数据收敛更快，精度更高。
为达到上述目的，本发明采用如下技术方案：
一种基于自适应互补融合的无人机姿态控制***，包括电机和与其连接的电机驱动器，电机驱动器的控制信号输入端连接至并联的第一控制器和第二控制器；
所述的第一控制器的信号输入端连接自互补型自适应滤波器的信号输出端，互补型自适应滤波器的输入端分别连接第一低通滤波器和四元数微分方程模块，第一低通滤波器的输入端连接加速度计的信号输出端，四元数微分方程模块的信号输入端连接至陀螺仪的信号输出端；
所述的第二控制器的信号输入端连接互补滤波器的信号输出端，互补滤波器的输出端连接四元数微分方程模块和第二低通滤波器，第二低通滤波器的输入端连接至磁力计的信号输出端；
所述第一控制器和第二控制器的信号输入端均连接至遥控器。
进一步地，所述的陀螺仪为MPU-6050六轴传感器。
进一步地，所述的磁力计为HMC5883L磁力计。
进一步地，所述的电机驱动器为电子调速器，电机为无刷电机。
一种基于自适应互补融合的无人机姿态控制***的控制方法，包括以下步骤：
步骤一：获取加速度计、陀螺仪和磁力计传感器测得的数据，并对加速度计和磁力计测得的数据进行低通滤波处理；
步骤二：对陀螺仪测得的数据以及低通滤波处理后的加速度计和磁力计数据进行融合获得机体姿态数据；
步骤三：根据机体姿态数据和遥控器指令，控制器控制电机，最终控制飞行器姿态。
进一步地，步骤二中对陀螺仪测得的数据以及低通滤波处理后的加速度计和磁力计数据进行融合获得机体姿态数据的方法为：
A)陀螺仪数据通过四元数微分方程得到第一姿态四元数；
四元数微分方程为：
其中为上一周期求得的最终四元数向量，为当前周期四元数的导数；为角速度四元数向量；对该方程近似求解得到第一姿态四元数T为***更新周期。
B)加速度计数据通过梯度下降法得到第二姿态四元数；
最速下降法求取第二姿态四元数公式为式中为上一周期所求姿态四元数，为当前周期所要求的第二姿态四元数；μt为步长，其中，
C)将第一姿态四元数和第二姿态四元数进行互补滤波获得宽范围姿态数据；
互补滤波公式为：Qt＝(1-α)Q▽,t+αQω,t,0≤α≤1
式中Qω,t为通过步骤A)所求的，Qt为最终姿态四元数，Q▽,t为步骤B)中所求得，α为互补滤波动态系数，通过步骤D)自适应调整；
D)引入加速度计值与重力值之差作为互补滤波动态系数，实现自适应调整参数；
式中ax、ay、az为加速度计测量值，g为当地重力加速度，K是调节系数，B为静止时滤波系数；
E)将步骤C求得的最终姿态四元数转换成横滚角和俯仰角；
θ＝arcsin2(q1,tq3,t-q0,tq2,t)
其中，θ为俯仰角，γ为横滚角；
F)磁力计数据结合步骤E)的γ和θ求得航向角；
航向角计算公式如下：
式中mx、my、mz为磁力计输出数据，θ为俯仰角，γ为横滚角，ψm,t为所求航向角；
G)步骤F)求得的航向角与通过步骤A)求得航向角数据互补融合获取宽范围航向角；
其融合方式为：ψt＝(1-β)ψm,t+βψω,t,0≤β≤1，式中ψt为最终所求姿态航向角，为依据步骤A)中的第一姿态四元数所得。
进一步地，第一控制器采用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)陀螺仪数据通过四元数微分方程得到第一姿态四元数；
四元数微分方程为：
其中为上一周期求得的最终四元数向量，为当前周期四元数的导数；为角速度四元数向量；对该方程近似求解得到第一姿态四元数T为***更新周期。
B)加速度计数据通过梯度下降法得到第二姿态四元数；
最速下降法求取第二姿态四元数公式为式中为上一周期所求姿态四元数，为当前周期所要求的第二姿态四元数；μt为步长，其中，
C)将第一姿态四元数和第二姿态四元数进行互补滤波获得宽范围姿态数据；
互补滤波公式为：Qt＝(1-α)Q▽,t+αQω,t,0≤α≤1
式中Qω,t为通过步骤A)所求的，Qt为最终姿态四元数，Q▽,t为步骤B)中所求得，α为互补滤波动态系数，通过步骤D)自适应调整；
D)引入加速度计值与重力值之差作为互补滤波动态系数，实现自适应调整参数；
式中ax、ay、az为加速度计测量值，g为当地重力加速度，K是调节系数，B为静止时滤波系数；
E)将步骤C求得的最终姿态四元数转换成横滚角和俯仰角；
θ＝arcsin2(q1,tq3,t-q0,tq2,t)
其中，θ为俯仰角，γ为横滚角；
F)磁力计数据结合步骤E)的γ和θ求得航向角；
航向角计算公式如下：
式中mx、my、mz为磁力计输出数据，θ为俯仰角，γ为横滚角，ψm,t为所求航向角；
G)步骤F)求得的航向角与通过步骤A)求得航向角数据互补融合获取宽范围航向角；
其融合方式为：ψt＝(1-β)ψm,t+βψω,t,0≤β≤1，式中ψt为最终所求姿态航向角，为依据步骤A)中的第一姿态四元数所得。
步骤三：根据机体姿态数据和遥控器指令，控制器控制电机，最终控制飞行器姿态，第一控制器采用PID控制，第二控制器采用PD控制；且第一控制器和第二控制器输出与遥控器输出指令线性耦合叠加输出，通过电机驱动器驱动电机，进而控制飞行姿态，实现水平运动、垂直运动和旋转运动。
下面对本发明的实施过程作进一步详细说明：
将程序下载到处理器后，上电时处理器首先初始化传感器，并对其进行零偏校正，处理器分别读取各传感器200次数据，取其平均值作为零偏值。然后等待遥控发送指令，当处理器接收到解锁指令时，处理器以2ms为周期读取MPU-6050传感器数值并对加速度数据进行低通滤波，低通滤波采用滑动均值滤波，然后进行横滚角和俯仰角的姿态解算。姿态解算后对俯仰角、横滚角进行PID控制对航向角进行PD控制，计算出对应四个电机所需PWM。在空闲时间连续读取磁力计数值并进行航向角姿态解算。最终解算的欧拉姿态角送到响应控制器进行控制。遥控发送的数据实时传送到控制器里进行高度，方向的控制。图5为本发明与传统控制方法姿态响应精度比较图，从图5可以看出：梯度自适应算法比传统AHRS算法响应更快，更准，动态精度更高，更适用于实际飞行中的无人机控制，可以开发精确度更高的算法。
Claims (1)
1.一种基于自适应互补融合的无人机姿态控制***的控制方法，其特征在于，采用基于自适应互补融合的无人机姿态控制***，包括电机和与其连接的电机驱动器，电机驱动器的控制信号输入端连接至并联的第一控制器和第二控制器；
所述的第一控制器的信号输入端连接自互补型自适应滤波器的信号输出端，互补型自适应滤波器的输入端分别连接第一低通滤波器和四元数微分方程模块，第一低通滤波器的输入端连接加速度计的信号输出端，四元数微分方程模块的信号输入端连接至陀螺仪的信号输出端；
所述的第二控制器的信号输入端连接互补滤波器的信号输出端，互补滤波器的输入端连接四元数微分方程模块和第二低通滤波器，第二低通滤波器的输入端连接至磁力计的信号输出端；
所述第一控制器和第二控制器的信号输入端均连接至遥控器；
其中，陀螺仪为MPU-6050六轴传感器，磁力计为HMC5883L磁力计，电机驱动器为电子调速器，电机为无刷电机；
所述控制方法包括以下步骤：
步骤一：获取加速度计、陀螺仪和磁力计传感器测得的数据，并对加速度计和磁力计测得的数据进行低通滤波处理；
步骤二：对陀螺仪测得的数据以及低通滤波处理后的加速度计和磁力计数据进行融合获得机体姿态数据；
步骤三：根据机体姿态数据和遥控器指令，控制器控制电机，最终控制飞行器姿态；
步骤二中对陀螺仪测得的数据以及低通滤波处理后的加速度计和磁力计数据进行融合获得机体姿态数据的方法为：
A)陀螺仪数据通过四元数微分方程得到第一姿态四元数；
四元数微分方程为：
<mrow>
<msub>
<mover>
<mi>Q</mi>
<mo>&CenterDot;</mo>
</mover>
<mrow>
<mi>&omega;</mi>
<mo>,</mo>
<mi>t</mi>
</mrow>
</msub>
<mo>=</mo>
<mfrac>
<mn>1</mn>
<mn>2</mn>
</mfrac>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>n</mi>
<mi>b</mi>
</mrow>
<mi>b</mi>
</msubsup>
<msub>
<mi>Q</mi>
<mrow>
<mi>t</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</mrow>
其中为上一周期求得的最终四元数向量，为当前周期四元数的导数；为角速度四元数向量；对该方程近似求解得到第一姿态四元数T为***更新周期；
B)加速度计数据通过梯度下降法得到第二姿态四元数；
梯度下降法求取第二姿态四元数公式为式中为上一周期所求姿态四元数，为当前周期所要求的第二姿态四元数；μt为步长，其中，
<mrow>
<msubsup>
<mi>J</mi>
<mi>g</mi>
<mi>T</mi>
</msubsup>
<mrow>
<mo>(</mo>
<mover>
<mi>q</mi>
<mo>^</mo>
</mover>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfrac>
<mrow>
<msub>
<mi>df</mi>
<mi>g</mi>
</msub>
<mrow>
<mo>(</mo>
<mover>
<mi>q</mi>
<mo>^</mo>
</mover>
<mo>,</mo>
<mover>
<mi>a</mi>
<mo>^</mo>
</mover>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<mi>d</mi>
<mover>
<mi>q</mi>
<mo>^</mo>
</mover>
</mrow>
</mfrac>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>q</mi>
<mrow>
<mn>2</mn>
<mo>,</mo>
<mi>t</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mn>2</mn>
<msub>
<mi>q</mi>
<mrow>
<mn>3</mn>
<mo>,</mo>
<mi>t</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mn>2</mn>
<msub>
<mi>q</mi>
<mrow>
<mn>0</mn>
<mo>,</mo>
<mi>t</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mn>2</mn>
<msub>
<mi>q</mi>
<mrow>
<mn>1</mn>
<mo>,</mo>
<mi>t</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mn>2</mn>
<msub>
<mi>q</mi>
<mrow>
<mn>1</mn>
<mo>,</mo>
<mi>t</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mn>2</mn>
<msub>
<mi>q</mi>
<mrow>
<mn>0</mn>
<mo>,</mo>
<mi>t</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mn>2</mn>
<msub>
<mi>q</mi>
<mrow>
<mn>3</mn>
<mo>,</mo>
<mi>t</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mn>2</mn>
<msub>
<mi>q</mi>
<mrow>
<mn>2</mn>
<mo>,</mo>
<mi>t</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mn>4</mn>
<msub>
<mi>q</mi>
<mrow>
<mn>1</mn>
<mo>,</mo>
<mi>t</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mn>4</mn>
<msub>
<mi>q</mi>
<mrow>
<mn>2</mn>
<mo>,</mo>
<mi>t</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
<mrow>
<msub>
<mi>f</mi>
<mi>g</mi>
</msub>
<mrow>
<mo>(</mo>
<mover>
<mi>q</mi>
<mo>^</mo>
</mover>
<mo>,</mo>
<mover>
<mi>a</mi>
<mo>^</mo>
</mover>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>2</mn>
<mo>(</mo>
<msub>
<mi>q</mi>
<mrow>
<mn>1</mn>
<mo>,</mo>
<mi>t</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msub>
<mi>q</mi>
<mrow>
<mn>3</mn>
<mo>,</mo>
<mi>t</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>q</mi>
<mrow>
<mn>0</mn>
<mo>,</mo>
<mi>t</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msub>
<mi>q</mi>
<mrow>
<mn>2</mn>
<mo>,</mo>
<mi>t</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>)</mo>
<mo>-</mo>
<msub>
<mi>a</mi>
<mi>x</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>q</mi>
<mrow>
<mn>0</mn>
<mo>,</mo>
<mi>t</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msub>
<mi>q</mi>
<mrow>
<mn>1</mn>
<mo>,</mo>
<mi>t</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>+</mo>
<msub>
<mi>q</mi>
<mrow>
<mn>2</mn>
<mo>,</mo>
<mi>t</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msub>
<mi>q</mi>
<mrow>
<mn>3</mn>
<mo>,</mo>
<mi>t</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mo>-</mo>
<msub>
<mi>a</mi>
<mi>y</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<mfrac>
<mn>1</mn>
<mn>2</mn>
</mfrac>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mrow>
<mn>1</mn>
<mo>,</mo>
<mi>t</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mrow>
<mn>2</mn>
<mo>,</mo>
<mi>t</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
<mn>2</mn>
</msubsup>
<mo>)</mo>
</mrow>
<mo>-</mo>
<msub>
<mi>a</mi>
<mi>z</mi>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
C)将第一姿态四元数和第二姿态四元数进行互补滤波获得宽范围姿态数据；
互补滤波公式为：
式中Qω,t为通过步骤A)所求的，Qt为最终姿态四元数，为步骤B)中所求得，α为互补滤波动态系数，通过步骤D)自适应调整；
D)引入加速度计值与重力值之差作为互补滤波动态系数，实现自适应调整参数；
式中ax、ay、az为加速度计测量值，g为当地重力加速度，K是调节系数，B为静止时滤波系数；
E)将步骤C求得的最终姿态四元数转换成横滚角和俯仰角；
<mrow>
<mi>&gamma;</mi>
<mo>=</mo>
<mo>-</mo>
<mi>arctan</mi>
<mfrac>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>q</mi>
<mrow>
<mn>2</mn>
<mo>,</mo>
<mi>t</mi>
</mrow>
</msub>
<msub>
<mi>q</mi>
<mrow>
<mn>3</mn>
<mo>,</mo>
<mi>t</mi>
</mrow>
</msub>
<mo>+</mo>
<msub>
<mi>q</mi>
<mrow>
<mn>0</mn>
<mo>,</mo>
<mi>t</mi>
</mrow>
</msub>
<msub>
<mi>q</mi>
<mrow>
<mn>1</mn>
<mo>,</mo>
<mi>t</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<msubsup>
<mi>q</mi>
<mrow>
<mn>0</mn>
<mo>,</mo>
<mi>t</mi>
</mrow>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mrow>
<mn>1</mn>
<mo>,</mo>
<mi>t</mi>
</mrow>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mrow>
<mn>2</mn>
<mo>,</mo>
<mi>t</mi>
</mrow>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>q</mi>
<mrow>
<mn>3</mn>
<mo>,</mo>
<mi>t</mi>
</mrow>
<mn>2</mn>
</msubsup>
</mrow>
</mfrac>
</mrow>
θ＝arcsin2(q1,tq3,t-q0,tq2,t)
其中，θ为俯仰角，γ为横滚角；
F)磁力计数据结合步骤E)的γ和θ求得航向角；
航向角计算公式如下：
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>H</mi>
<mi>x</mi>
</msub>
<mo>=</mo>
<msub>
<mi>m</mi>
<mi>x</mi>
</msub>
<mo>&times;</mo>
<mi>cos</mi>
<mrow>
<mo>(</mo>
<mi>&theta;</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>m</mi>
<mi>y</mi>
</msub>
<mo>&times;</mo>
<mi>sin</mi>
<mrow>
<mo>(</mo>
<mi>&theta;</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>m</mi>
<mi>z</mi>
</msub>
<mo>&times;</mo>
<mi>cos</mi>
<mrow>
<mo>(</mo>
<mi>&gamma;</mi>
<mo>)</mo>
</mrow>
<mo>&times;</mo>
<mi>sin</mi>
<mrow>
<mo>(</mo>
<mi>&gamma;</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>H</mi>
<mi>y</mi>
</msub>
<mo>=</mo>
<msub>
<mi>m</mi>
<mi>y</mi>
</msub>
<mo>&times;</mo>
<mi>cos</mi>
<mrow>
<mo>(</mo>
<mi>&gamma;</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>m</mi>
<mi>z</mi>
</msub>
<mo>&times;</mo>
<mi>sin</mi>
<mrow>
<mo>(</mo>
<mi>&gamma;</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&psi;</mi>
<mrow>
<mi>m</mi>
<mo>,</mo>
<mi>t</mi>
</mrow>
</msub>
<mo>=</mo>
<msup>
<mi>tan</mi>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msup>
<mrow>
<mo>(</mo>
<msub>
<mi>H</mi>
<mi>y</mi>
</msub>
<mo>/</mo>
<msub>
<mi>H</mi>
<mi>x</mi>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
式中mx、my、mz为磁力计输出数据，θ为俯仰角，γ为横滚角，ψm,t为所求航向角；
G)步骤F)求得的航向角与通过步骤A)求得航向角数据互补融合获取宽范围航向角；
其融合方式为：ψt＝(1-β)ψm,t+βψω,t,0≤β≤1，式中ψt为最终所求姿态航向角，为依据步骤A)中的第一姿态四元数所得；
其中，第一控制器采用PID控制，第二控制器采用PD控制；且第一控制器和第二控制器输出与遥控器输出指令线性耦合叠加输出，通过电机驱动器驱动电机，进而控制飞行姿态，实现水平运动、垂直运动和旋转运动。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510313361.6A CN104914874B (zh) | 2015-06-09 | 2015-06-09 | 一种基于自适应互补融合的无人机姿态控制***及方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510313361.6A CN104914874B (zh) | 2015-06-09 | 2015-06-09 | 一种基于自适应互补融合的无人机姿态控制***及方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN104914874A CN104914874A (zh) | 2015-09-16 |
| CN104914874B true CN104914874B (zh) | 2017-12-19 |
Family
ID=54084030
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201510313361.6A Active CN104914874B (zh) | 2015-06-09 | 2015-06-09 | 一种基于自适应互补融合的无人机姿态控制***及方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN104914874B (zh) |
Families Citing this family (21)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105739510A (zh) * | 2016-01-13 | 2016-07-06 | 天津中科智能识别产业技术研究院有限公司 | 一种用于救灾无人机姿态控制的互补滤波方法 |
| CN105739511B (zh) * | 2016-01-31 | 2019-02-12 | 中国人民解放军国防科学技术大学 | 一种缺失迹向控制的欠驱动航天器悬停渐近控制方法 |
| CN107368087A (zh) * | 2016-05-13 | 2017-11-21 | 威海明达创新科技有限公司 | 微型四轴飞行器及其控制方法 |
| CN105938371A (zh) * | 2016-06-21 | 2016-09-14 | 深圳市博飞航空科技有限公司 | 无人飞行器飞行控制***、方法及遥控器 |
| WO2018053680A1 (en) * | 2016-09-20 | 2018-03-29 | SZ DJI Technology Co., Ltd. | Systems and methods for providing redundancy to electronic speed control systems |
| CN106441277A (zh) * | 2016-09-28 | 2017-02-22 | 深圳市普渡科技有限公司 | 一种基于编码器与惯性导航单元的机器人位姿估计方法 |
| CN106394871A (zh) * | 2016-09-29 | 2017-02-15 | 大连理工大学 | 一种固定翼飞行器控制装置 |
| CN106483966A (zh) * | 2016-10-08 | 2017-03-08 | 江苏润桐数据服务有限公司 | 无人机数据采集*** |
| CN106444809B (zh) * | 2016-10-12 | 2024-04-16 | 湖南绿野航空科技有限公司 | 一种无人机飞行控制器 |
| CN106774378B (zh) * | 2017-03-01 | 2019-08-09 | 杭州谷航科技有限公司 | 一种无人机飞行控制及定位方法 |
| CN107765708A (zh) * | 2017-12-07 | 2018-03-06 | 智灵飞（北京）科技有限公司 | 一种六旋翼无人机飞行控制***及控制方法 |
| CN108170154A (zh) * | 2017-12-19 | 2018-06-15 | 广东省航空航天装备技术研究所 | 一种无人机多传感器正向摄影倾斜飞控调试方法 |
| CN108398128B (zh) * | 2018-01-22 | 2021-08-24 | 北京大学深圳研究生院 | 一种姿态角的融合解算方法和装置 |
| CN108444468B (zh) * | 2018-02-06 | 2020-08-04 | 浙江大学 | 一种融合下视视觉与惯导信息的定向罗盘 |
| CN108681332A (zh) * | 2018-06-11 | 2018-10-19 | 山东超越数控电子股份有限公司 | 一种四旋翼飞行器控制***及控制方法 |
| CN109108936A (zh) * | 2018-10-24 | 2019-01-01 | 电子科技大学 | 一种多源传感器数据融合的自平衡机器人控制***和控制方法 |
| CN109506646A (zh) * | 2018-11-20 | 2019-03-22 | 石家庄铁道大学 | 一种双控制器的无人机姿态解算方法及*** |
| CN109708647A (zh) * | 2019-03-08 | 2019-05-03 | 哈尔滨工程大学 | 一种基于融合特征元素的室内拓扑地图行人定位方法 |
| CN110319829B (zh) * | 2019-07-08 | 2022-11-18 | 河北科技大学 | 基于自适应多态融合蚁群算法的无人机航迹规划方法 |
| CN110531613A (zh) * | 2019-09-05 | 2019-12-03 | 广东工业大学 | 二轴无人机控制方法、装置、设备及计算机可读存储介质 |
| CN112179342B (zh) * | 2020-09-21 | 2021-08-27 | 中国安全生产科学研究院 | 一种估计无人机姿态的方法及*** |
Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103512584A (zh) * | 2012-06-26 | 2014-01-15 | 北京赛佰特科技有限公司 | 导航姿态信息输出方法、装置及捷联航姿参考*** |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US7593793B2 (en) * | 2004-07-30 | 2009-09-22 | The Boeing Company | Adaptive control in the presence of input constraints |
-
2015
- 2015-06-09 CN CN201510313361.6A patent/CN104914874B/zh active Active
Patent Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103512584A (zh) * | 2012-06-26 | 2014-01-15 | 北京赛佰特科技有限公司 | 导航姿态信息输出方法、装置及捷联航姿参考*** |
Non-Patent Citations (5)
| Title |
|---|
| Estimation of IMU and MARG orientation using a gradient descent algorithm;Sebastian O.H. 等;《2011 IEEE International Conference on Rehabilitation Robotics》;20110701;全文 * |
| 基于卡尔曼滤波的MUH姿态信号融合算法研究;余彦霖 等;《电子测量技术》;20150515;第38卷(第5期);全文 * |
| 基于自适应显式互补滤波的姿态解算方法;孙金秋 等;《测控技术》;20150418;第34卷(第4期);第24-31页 * |
| 基于预测控制的小型无人直升机自主飞行研究;杜建福;《中国博士学位论文全文数据库工程科技Ⅱ辑》;20110415;第55-61页 * |
| 自适应混合滤波算法在微型飞行器姿态估计中的应用;傅忠云 等;《传感技术学报》;20140515;第27卷(第5期);第698-703页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN104914874A (zh) | 2015-09-16 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN104914874B (zh) | 一种基于自适应互补融合的无人机姿态控制***及方法 | |
| CN106249745B (zh) | 四轴无人机的控制方法 | |
| CN105353762B (zh) | 基于双余度姿态传感器的六旋翼无人机的控制方法 | |
| CN102426458B (zh) | 一种适用于旋翼无人机的地面控制*** | |
| CN105739513B (zh) | 一种四旋翼飞行机器人非线性轨迹跟踪控制器及其跟踪控制方法 | |
| US8209069B1 (en) | Aircraft sensor anticipator system | |
| CN103837151B (zh) | 一种四旋翼飞行器的气动模型辅助导航方法 | |
| CN107247459B (zh) | 抗干扰飞行控制方法及装置 | |
| CN104765272A (zh) | 一种基于pid神经元网络控制(pidnn)的四旋翼飞行器控制方法 | |
| CN202939489U (zh) | 一种多旋翼自动平衡飞行控制器 | |
| Zhu et al. | Adaptive backstepping control for a miniature autonomous helicopter | |
| González et al. | Real-time attitude stabilization of a mini-uav quad-rotor using motor speed feedback | |
| CN107357302A (zh) | 一种四旋翼飞行器的翻滚动作实现方法和装置 | |
| CN113296525A (zh) | 基于数据驱动的倾转三旋翼无人机非线性控制方法 | |
| CN112665574A (zh) | 基于动量梯度下降法的水下机器人姿态采集方法 | |
| Etemadi | Mathematical dynamics, kinematics modeling and PID equation controller of quadcopter | |
| Reséndiz et al. | System Identification of a Quad-rotor in X Configuration from Experimental Data. | |
| Zhan et al. | Control system design and experiments of a quadrotor | |
| FARI | Guidance and control for a fixed-wing UAV | |
| Ko et al. | Basic multicopter control with inertial sensors | |
| Alcan et al. | Robust hovering control of a quadrotor using acceleration feedback | |
| Cai et al. | Design of STM32-based Quadrotor UAV Control System. | |
| Al-Sharman | Attitude estimation for a small-scale flybarless helicopter | |
| Athayde et al. | Experimental Nonlinear and Incremental Control Stabilization of a Tail-Sitter UAV with Hardware-in-the-Loop Validation | |
| Baranek et al. | Model-based attitude estimation for multicopters |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20210129 Address after: Room 508, block a, Rongcheng cloud Valley, 57 Keji 3rd road, Zhangba Street office, high tech Zone, Xi'an City, Shaanxi Province, 710075 Patentee after: Xi'an Huizhi Information Technology Co.,Ltd. Address before: 710064 No. 33, South Second Ring Road, Shaanxi, Xi'an Patentee before: CHANG'AN University |
|
| TR01 | Transfer of patent right |