CN104850127A - 一种可动感操控四旋翼飞行器的方法 - Google Patents

Abstract

本发明属于飞行控制领域，具体涉及一种可动感操控四旋翼飞行器的方法。本发明包括：获取初始姿态；四元数姿态更新；规范化四元素；互补滤波器数据融合。动感操控四旋翼飞行器的方法具有可直观快速性操控四旋翼飞行器的突出优点，适合在紧急或危险情况下高效快速地控制四旋翼飞行器平稳飞行的问题。同时也具有体积小、操控过程中主动性强等优点，从而也大大降低了操控者的门槛。

Description

一种可动感操控四旋翼飞行器的方法

技术领域

本发明属于飞行控制领域，具体涉及一种可动感操控四旋翼飞行器的方法。

背景技术

四旋翼飞行器是一种可垂直起降的无人飞行器，相对于其他飞行器来说具有机械结构简单紧凑、操作灵活、稳定性好、环境适应性强、具有良好的操作性能等特点，可以在小范围实现起飞、悬停、降落。由于这些特点，四旋翼飞行器已广泛应用于航拍、监视、侦查、搜救、农业病虫害防治等诸多领域。

传统的基于四旋翼飞行器的操控方法，大多使用遥控器，通过摇动其摇杆遥控飞行器使相关输出量转化四旋翼飞行状态，或利用手机端控制模拟遥控器摇杆控制方式，通过无线通信模块使相关输出量转化为四旋翼飞行状态。这些传统的操控方式不适合直观快速性地操控四旋翼，更不适合在紧急或危险情况下高效快速地控制四旋翼平稳飞行的问题；传统四旋翼飞行器操控方式不仅对操控者的熟悉操控程度要求很高，而且重量较重、体积较大、容易损毁，同时大大的影响了四旋翼飞行器的响应性能。

发明内容

本发明的目的是提供一种可动感操控四旋翼飞行器的方法。

本发明的目的是这样实现的：

(1)获取初始姿态

使用欧拉角表示姿态，令ψ,θ和代表ZYX欧拉角，分别称为偏航角、俯仰角和横滚角，载体坐标系下的加速度()和参考坐标系下的加速度()之间的关系为，

其中c和s分别代表cos和sin，

参考系下的加速度等于重力加速度，即

$\left[\begin{array}{c}{a}_{x_N}\\ a_{y_N}\\ a_{y_N}\end{array}\right]=\left[\begin{array}{c}0\\ 0\\ 0\end{array}\right]$

$\mathrm{\θ}=\arctan \left(\frac{a_{x_B}}{\sqrt{a_{y_B}^2+a_{z_B}^2}}\right)$

为初始俯仰角和横滚角，通过加速度计得到载体坐标系下的加速度即可将其解出，

载体坐标系下的磁场强度()和参考坐标系下的磁场强度()之间的关系为：

通过磁力计可以得到载体坐标系下得磁场强度，得()，解得初始偏航角，

(2)四元数姿态更新

首先将欧拉角转换为四元数，

采用毕卡算法进行四元数的更新，

$\begin{array}{c}{q}_0=q_0+(-q_1{\mathrm{\ω}}_{x_B}-q_2{\mathrm{\ω}}_{y_B}-q_3{\mathrm{\ω}}_{z_B})\frac{\mathrm{\Δt}}{2}\\ q_1=q_1+(q_0{\mathrm{\ω}}_{x_B}-q_2{\mathrm{\ω}}_{y_B}-q_3{\mathrm{\ω}}_{z_B})\frac{\mathrm{\Δt}}{2}\\ q_2=q_2+(q_0{\mathrm{\ω}}_{y_B}-q_1{\mathrm{\ω}}_{z_B}+q_3{\mathrm{\ω}}_{x_B})\frac{\mathrm{\Δt}}{2}\\ q_3=q_3+(q_0{\mathrm{\ω}}_{z_B}+q_1{\mathrm{\ω}}_{y_B}-q_2{\mathrm{\ω}}_{x_B})\frac{\mathrm{\Δt}}{2}\end{array},$

其中Δt代表陀螺采样时间间隔，和代表载体坐标系下的角速度，由陀螺仪得到，

(3)规范化四元素

(4)互补滤波器数据融合

将更新后的四元数转换为欧拉角：

使用互补滤波器进行矫正，

其中下标g代表由陀螺得到的欧拉角，下标a代表由加速度计得到的欧拉角，下标e代表经互补滤波器矫正后的欧拉角估计值。

本发明的有益效果在于：

动感操控四旋翼飞行器的方法具有可直观快速性操控四旋翼飞行器的突出优点，适合在紧急或危险情况下高效快速地控制四旋翼飞行器平稳飞行的问题。同时也具有体积小、操控过程中主动性强等优点，从而也大大降低了操控者的门槛。

附图说明

图1为一种可动感操控四旋翼飞行器的微型设备总体结构框图；

图2为姿态解算流程图；

图3为姿态解算具体流程；

图4为互补滤波器原理示意图。

具体实施方式

下面结合附图对本发明做进一步描述。

本发明重点解决的传统操控方式性能问题，解决传统的操控方式不适合直观快速性操控四旋翼飞行器、更不适合在紧急或危险情况下高效快速地控制四旋翼飞行器平稳飞行的问题。

本发明所要解决的技术问题是，提供一种基于可显著提高四旋翼无人机操控性能的控制方法。开发了一种全新的操控四旋翼飞行器的方法：一种可动感操控四旋翼飞行器的方法。

本发明涉及一种可动感操控四旋翼飞行器的方法。特别是涉及一种基于AHRS(AttitudeandHeading Reference System,航姿参考系统)的动感操控设备进行操控四旋翼飞行器的控制方法。

本系统工作原理是在启动动感操控设备与四旋翼之后，分别进行初始姿态对准，并定义动感设备参考坐标系与四旋翼参考坐标系都为同一坐标系，即地理坐标系。利用安装在动感设备上的AHRS得到动感设备此时的动作与姿态信息，通过无线发送模块发送给四旋翼飞行器的无线接收机模块，即四旋翼飞行器此时刻需要进行的动作与需要调整的姿态。四旋翼飞行器接收到命令后，经过数据处理与分析，执行相关动作，如起飞、降落、前进、后退、向左飞、向右飞、转向、上升、翻滚等动作。四旋翼主控给定飞行器指定命令后，等待下一时刻动感设备信息的到来，从而执行新的动作。

基于AHRS的姿态解算的具体原理：

本发明一种可动感操控四旋翼飞行器的方法，利用AHRS测量动感设备姿态与动作。AHRS由三轴陀螺仪、三轴加速度计与三轴地磁传感器组成。通过三轴加速度计与三轴地磁传感器得到当前初始姿态。通过三轴陀螺仪得到的角速率使用四元数算法进行姿态更新，再将四元数转换为欧拉角(横滚角、俯仰角、偏航角)。通过三轴加速度计得到的加速度进行计算得到横滚角、俯仰角，通过三轴地磁传感器测量到的磁场大小得到偏航角。最后得到的欧拉角通过互补滤波器进行姿态数据矫正，最后将矫正后的欧拉角转换为四元数并将其规范化，进行下一次的姿态更新。

姿态解算具体过程为：

(1)初始姿态获取

使用欧拉角表示姿态，令ψ,θ和代表ZYX欧拉角，分别称为偏航角、俯仰角和横滚角。载体坐标系下的加速度()和参考坐标系下的加速度()之间的关系可表示为(1)。

(1)其中c和s分别代表cos和sin。

起飞前，飞行器处于静止状态，无法由陀螺仪得到初始姿态。此时参考

系下的加速度等于重力加速度，即

$\left[\begin{array}{c}{a}_{x_N}\\ a_{y_N}\\ a_{y_N}\end{array}\right]=\left[\begin{array}{c}0\\ 0\\ 0\end{array}\right]---\left(2\right)$

把(2)代入(1)可以解的

$\mathrm{\θ}=\arctan \left(\frac{a_{x_B}}{\sqrt{a_{y_B}^2+a_{z_B}^2}}\right)---\left(3\right)$

(3)(4)即为初始俯仰角和横滚角，通过加速度计得到载体坐标系下的加速度即可将其解出。

载体坐标系下的磁场强度()和参考坐标系下的磁场强度()之间的关系可表示为(5)。通过磁力计可以得到载体坐标系下得磁场强度，将其代入(5)，并结合(3)和(4)可以求得()。再由(6)可以解得初始偏航角。

(2)四元数姿态更新

首先通过(7)将欧拉角转换为四元数，然后采用毕卡算法进行四元数的更新，这里使用一阶近似算法，如(8)所示，其中Δt代表陀螺采样时间间隔。和代表载体坐标系下的角速度，可由陀螺仪得到。

$\begin{array}{c}{q}_0=q_0+(-q_1{\mathrm{\ω}}_{x_B}-q_2{\mathrm{\ω}}_{y_B}-q_3{\mathrm{\ω}}_{z_B})\frac{\mathrm{\Δt}}{2}\\ q_1=q_1+(q_0{\mathrm{\ω}}_{x_B}-q_2{\mathrm{\ω}}_{y_B}-q_3{\mathrm{\ω}}_{z_B})\frac{\mathrm{\Δt}}{2}\\ q_2=q_2+(q_0{\mathrm{\ω}}_{y_B}-q_1{\mathrm{\ω}}_{z_B}+q_3{\mathrm{\ω}}_{x_B})\frac{\mathrm{\Δt}}{2}\\ q_3=q_3+(q_0{\mathrm{\ω}}_{z_B}+q_1{\mathrm{\ω}}_{y_B}-q_2{\mathrm{\ω}}_{x_B})\frac{\mathrm{\Δt}}{2}\end{array}---\left(8\right)$

(3)规范化四元素

(4)互补滤波器数据融合

由于陀螺零点漂移和离散采样产生的累积误差，由陀螺得到的四元数只能保证短期的精度，需要使用加速度计和磁力计对其进行矫正。首先使用(9)将更新后的四元数转换为欧拉角，然后使用互补滤波器进行矫正，基本原理如图4所示，其公式表示为(10)。其中下标g代表由陀螺得到的欧拉角，下标a代表由加速度计得到的欧拉角，下标e代表经互补滤波器矫正后的欧拉角估计值。

本发明的工作原理和工作过程：

本发明“一种可动感操控四旋翼飞行器的方法”通过AHRS得到动感设备当前的动作与姿态信息，通过无线发送模块发送给四旋翼飞行器的无线接收机模块，即四旋翼飞行器此时刻需要进行的动作与需要调整的姿态。四旋翼飞行器接收到命令后，经过数据处理与分析，执行相关动作。四旋翼主控给定飞行器指定命令后，等待下一时刻动感设备信息的到来，从而执行新的动作。从而实现动感操控四旋翼飞行器的目的。

本发明的发明点：

给出了一种可显著提高四旋翼无人机操控性能的解决方案，以及一种高效的适用性广的一种基于AHRS的动感操控方法。

十要求保护的技术关键：

本发明的关键技术是完成了一种可动感操控四旋翼飞行器的方法，在本处理方法中利用的关键技术有AHRS的设计及其配套的数据处理方法，全新的可显著提高四旋翼无人机操控性能的操控方法。

Claims (1)

1.一种可动感操控四旋翼飞行器的方法，其特征在于:

(1)获取初始姿态

使用欧拉角表示姿态，令ψ,θ和代表ZYX欧拉角，分别称为偏航角、俯仰角和横滚角，载体坐标系下的加速度和参考坐标系下的加速度之间的关系为，

其中c和s分别代表cos和sin，

参考系下的加速度等于重力加速度，即

$\left[\begin{array}{c}{a}_{x_N}\\ a_{y_N}\\ a_{y_N}\end{array}\right]=\left[\begin{array}{c}0\\ 0\\ 0\end{array}\right]$

$\mathrm{\θ}=\arctan \left(\frac{a_{x_B}}{\sqrt{a_{y_B}^2+a_{z_B}^2}}\right)$

θ、为初始俯仰角和横滚角，通过加速度计得到载体坐标系下的加速度即可将其解出，

载体坐标系下的磁场强度和参考坐标系下的磁场强度之间的关系为：

通过磁力计可以得到载体坐标系下得磁场强度，得解得初始偏航角，

(2)四元数姿态更新

首先将欧拉角转换为四元数，

采用毕卡算法进行四元数的更新，

$q_0=q_0+(-q_1{\mathrm{\ω}}_{x_B}-q_2{\mathrm{\ω}}_{y_B}-q_3{\mathrm{\ω}}_{z_B})\frac{\mathrm{\Δt}}{2}$

$q_1=q_1+(q_0{\mathrm{\ω}}_{x_B}-q_2{\mathrm{\ω}}_{t_B}-q_3{\mathrm{\ω}}_{z_B})\frac{\mathrm{\Δt}}{2}$

$q_2=q_2+(q_0{\mathrm{\ω}}_{y_B}-q_2{\mathrm{\ω}}_{z_B}-q_3{\mathrm{\ω}}_{x_B})\frac{\mathrm{\Δt}}{2},$

$q_3=q_3+(q_0{\mathrm{\ω}}_{z_B}-q_1{\mathrm{\ω}}_{y_B}-q_2{\mathrm{\ω}}_{x_B})\frac{\mathrm{\Δt}}{2}$

其中Δt代表陀螺采样时间间隔，和代表载体坐标系下的角速度，由陀螺仪得到，

(3)规范化四元素

(4)互补滤波器数据融合

将更新后的四元数转换为欧拉角：

θ＝arcsin(2(q₀q₂-q₁q₃))，

$\mathrm{\ψ}=\arctan \left(\frac{2(q_0{q}_1+q_2{q}_3)}{q_0^2-q_1^2-q_2^2+q_3^2}\right)$

使用互补滤波器进行矫正，

其中下标g代表由陀螺得到的欧拉角，下标a代表由加速度计得到的欧拉角，下标e代表经互补滤波器矫正后的欧拉角估计值。