CN111459188A - 一种基于四元数的多旋翼非线性飞行控制方法 - Google Patents

Abstract

本发明公开了一种基于四元数的多旋翼非线性飞行控制方法。该方法步骤如下：首先，对环境变量和多旋翼的运动条件进行假设以简化多旋翼建模；其次，根据牛顿第二定律建立多旋翼位置变化方程，跟据动量矩定理和四元数的运动学方程建立姿态变化方程；最后，将多旋翼的控制分解为内外环结构的位置控制和姿态控制，并用反步法结合Lyapunov稳定性定理设计多旋翼飞行器的外环位置控制器和内环姿态控制器。本方法将多旋翼的姿态用四元数来描述，并结合反步法来设计控制器，有效避免了欧拉角描述姿态角可能会出现的奇异问题，并且在处理非线性问题时有较好的表现，能够稳定的控制多旋翼的位置和姿态。

Description

一种基于四元数的多旋翼非线性飞行控制方法

技术领域

本发明属于多旋翼飞行控制技术领域，特别是一种基于四元数的多旋翼非线性飞行控制方法。

背景技术

在众多微型无人机中，多旋翼飞行器结构和操控简单，只需要控制电机的转速就可以实现飞行器的垂直起飞降降落等复杂运动，而且起降时对环境的影响小，发出的噪声小，相较于其他的飞行器有许多优势：独立起降能力，适应环境能力，能够在多种模式下飞行，如悬停、前飞、侧飞、后飞、偏航旋转等。在飞行控制领域，由于多旋翼飞行器是一个非线性模型，有强耦合、欠驱动等特点。因此，多旋翼飞行器具有广泛的应用前景和研究意义。

目前，对于多旋翼的姿态运动描述，大都是基于欧拉角的。欧拉角相对来说好理解，但是并不能直观的表示姿态变化，对于相对复杂的一次姿态变化，一般是分解为机体绕各个坐标轴的多次旋转得到。而用单位四元数表示旋转是轴角法的一种，对于机体的姿态变化，都可以用四元数来直观的表示多旋翼的定轴转动，不存在奇异问题和万向节死锁问题。

在工程上，对多旋翼的控制大多是基于PID控制方法设计的传统控制器，这些控制器不依赖于建模精度，忽略外界环境对多旋翼的影响，假定多旋翼的飞行是近似线性的。然而，这只在无外界干扰的低速飞行或者悬停状态下才成立，当进行高速飞行或者外界干扰比较强的时候，控制器无法有效的控制多旋翼的稳定。由于这些控制器的设计步骤简单，难以控制欠驱动，强耦合，易受外界影响的多旋翼***，当多旋翼在进行航迹跟踪飞行时，很难在要求跟踪精度和响应速度时顾及抗干扰能力。

发明内容

本发明的目的在于提供一种能够稳定的控制多旋翼的位置和姿态的基于四元数的多旋翼非线性飞行控制方法。

实现本发明目的的技术解决方案是：一种基于四元数的多旋翼非线性飞行控制方法，其特征在于，包括以下步骤：

步骤1，对外界环境和飞行器运动条件做出假设，得到四旋翼飞行器动力学方程；

步骤2，建立控制模型：根据牛顿第二定律建立多旋翼位置控制模型，跟据动量矩定理和四元数运动学方程建立姿态控制模型；

步骤3，将多旋翼的飞行控制分解为外环位置控制和内环姿态控制，建立欧拉角与单位四元数之间的转换关系；

步骤4，利用反步法结合Lyapunov原理，分别构建外环位置控制器和内环姿态控制器，控制多旋翼的位置和姿态。

进一步地，步骤1所述对外界环境和飞行器运动条件做出假设，包括：

1)在飞行过程中，重力加速度g不变化，多旋翼是质量不变的六自由度刚体；

2)多旋翼形状结构关于中心对称，各部分的材质相同，质心位于机体中心；

3)惯性坐标系下，忽略地球的曲率；

4)不考虑地球公转和自转的影响。

进一步地，步骤2所述控建立控制模型，包括多旋翼在以北-东-地为坐标轴的导航系下的位置控制模型和在机体系下的姿态控制模型：

所述的位置控制模型：

其中

分别表示x、y、z方向上的速度，

分别表示x、y、z方向上的加速度，φ表示滚转角，θ表示俯仰角，ψ表示偏航角，T表示旋翼拉力，m表示机体质量，g表示重力加速度，k表示空气阻力；

所述的刚体动量矩定理：

其中J＝diag{J_bx,J_by,J_bz}表示转动惯量矩阵，τ＝[τ_p,τ_q,τ_r]^T表示电机提供的驱动力矩，ω＝[p,q,r]^T表示多旋翼的转动角速度，[]_×表示反对称矩阵，τ_rotor表示旋翼的陀螺力矩；

J_rotor表示转子转动惯量，Ω为各电机转子的转速和；

所述的四元数的运动学方程为：

其中，P(ω)＝[0 ω^T]^T是角速度矢量ω的四元数形式，q＝[q₀q₁q₂q₃]^T＝[η ε^T]^T为多旋翼的单位四元数姿态，q₀、q₁、q₂、q₃分别为单位四元数的四个参数；η为标量，表示四元数的实部；ε为矢量，表示四元数的虚部，

表示四元数乘法；由四元数乘法，得四元数运动学方程

其中T(q)∈R^4×3，表达式为：

因此，结合动量矩定理式(2)和四元数运动学方程式(5)得到多旋翼的姿态控制模型：

进一步地，步骤3所述的将多旋翼的飞行控制分解为外环位置控制和内环姿态控制，建立欧拉角与单位四元数之间的转换关系，具体如下：

将多旋翼的飞行控制分解为外环位置控制和内环姿态控制的依据是：由式(1)多旋翼的位置变化方程知，多旋翼位置变化不仅与旋翼提供的拉力T有关，还与多旋翼的欧拉角即姿态有关；而由式(7)多旋翼的姿态变化方程知，姿态运动能够直接通过控制多旋翼围绕质心的转动力矩来实现；内环控制多旋翼的姿态，外环位置控制多旋翼在导航坐标系下的位置；

所述的欧拉角与单位四元数的转换关系为：在导航坐标系下，用φ表示滚转角，θ表示俯仰角，ψ表示偏航角，则对应的单位四元数q(φ,θ,ψ)由欧拉角表示为：

进一步地，步骤4中构建外环位置控制器，具体如下：

首先，位置控制只考虑旋翼推力、重力和空气阻力，根据多旋翼的位置动态方程式(1)建立状态向量x和位置虚拟控制量u：

然后，通过反步法结合Lyapunov原理，得到x、y、z方向的虚拟反馈控制量u_x、u_x、u_x为分别为：

其中δx₁、δx₃、δx₅和δx₂、δx₄、δx₆分别是x_n、y_n、z_n方向位置跟踪偏差和速度跟踪偏差，x_1d、x_3d、x_3d，

分别为x_n、y_n、z_n方向上的期望位置和期望速度，k_x1、k_x2、k_x3、k_x4、k_x5、k_x6是大于零的控制参数；

由式(9)、(13)，得到多旋翼输入拉力T的大小：

进一步地，步骤4中构建内环姿态控制器，具体如下：

首先，由式(9)得到多旋翼的期望姿态角φ_d、θ_d，φ_d、θ_d的绝对值小于45°：

期望偏航角ψ_d由控制指令给出，然后通过式(8)将期望姿态角转化为期望单位四元数q_d；

然后，通过实际四元数姿态为q＝[η ε^T]^T和目标四元数姿态为q_d＝[η_d ε_d ^T]^T得到偏差四元数为q_e：

最后，通过反步法结合Lyapunov原理得到求得

再通过式(7)得到反馈控制量τ_d：

其中k_e1＞0，K_e2＝diag{k_e21,k_e22,k_e2}，k_e21、k_e22、k_e23＞0，e₁为四元数位置误差，e₂为角速度误差，ω为实际角速度，ω_d为期望角速度。

本发明与现有技术相比，其显著优点为：(1)将多旋翼的姿态用四元数来描述，并结合反步法来设计控制器，有效避免了欧拉角描述姿态角可能会出现的奇异问题；(2)在处理非线性问题时有较好的表现，能够稳定的控制多旋翼的位置和姿态。

附图说明

图1为本发明实例中的基于四元数的多旋翼非线性飞行控制方法流程图。

图2为本发明实例中的实例多旋翼的结构图。

图3为本发明实例中的内外环控制策略示意图。

图4为本发明实例中的仿真中采用阶跃输入时的姿态响应曲线图。

图5为本发明实例中的仿真中采用阶跃输入时的位置响应曲线图。

图6为本发明实例中的仿真中采用单位斜坡输入时的位置响应曲线图。

图7为本发明实例中位置跟踪飞行测试中多旋翼跟踪位置“回”形轨迹的效果图。

图8为本发明实例中的姿态跟踪飞行测试中多旋翼跟踪“螺旋线”轨迹的效果图。

具体实施例

一种基于四元数的多旋翼非线性飞行控制方法，其特征在于，包括以下步骤：

步骤1，对外界环境和飞行器运动条件做出假设，得到四旋翼飞行器动力学方程；

步骤2，建立控制模型：根据牛顿第二定律建立多旋翼位置控制模型，跟据动量矩定理和四元数运动学方程建立姿态控制模型；

步骤3，将多旋翼的飞行控制分解为外环位置控制和内环姿态控制，建立欧拉角与单位四元数之间的转换关系；

步骤4，利用反步法结合Lyapunov原理，分别构建外环位置控制器和内环姿态控制器，控制多旋翼的位置和姿态。

进一步地，步骤1所述对外界环境和飞行器运动条件做出假设，包括：

1)在飞行过程中，重力加速度g不变化，多旋翼是质量不变的六自由度刚体；

2)多旋翼形状结构关于中心对称，各部分的材质相同，质心位于机体中心；

3)惯性坐标系下，忽略地球的曲率；

4)不考虑地球公转和自转的影响。

进一步地，步骤2所述控建立控制模型，包括多旋翼在以北-东-地为坐标轴的导航系下的位置控制模型和在机体系下的姿态控制模型：

所述的位置控制模型：

其中

分别表示x、y、z方向上的速度，

分别表示x、y、z方向上的加速度，φ表示滚转角，θ表示俯仰角，ψ表示偏航角，T表示旋翼拉力，m表示机体质量，g表示重力加速度，k表示空气阻力；

所述的刚体动量矩定理：

其中J＝diag{J_bx,J_by,J_bz}表示转动惯量矩阵，τ＝[τ_p,τ_q,τ_r]^T表示电机提供的驱动力矩，ω＝[p,q,r]^T表示多旋翼的转动角速度，[]_×表示反对称矩阵，τ_rotor表示旋翼的陀螺力矩；

J_rotor表示转子转动惯量，Ω为各电机转子的转速和；

所述的四元数的运动学方程为：

其中，P(ω)＝[0 ω^T]^T是角速度矢量ω的四元数形式，q＝[q₀q₁q₂q₃]^T＝[η ε^T]^T为多旋翼的单位四元数姿态，q₀、q₁、q₂、q₃分别为单位四元数的四个参数；η为标量，表示四元数的实部；ε为矢量，表示四元数的虚部，

表示四元数乘法；由四元数乘法，得四元数运动学方程

其中T(q)∈R^4×3，表达式为：

因此，结合动量矩定理式(2)和四元数运动学方程式(5)得到多旋翼的姿态控制模型：

进一步地，步骤3所述的将多旋翼的飞行控制分解为外环位置控制和内环姿态控制，建立欧拉角与单位四元数之间的转换关系，具体如下：

将多旋翼的飞行控制分解为外环位置控制和内环姿态控制的依据是：由式(1)多旋翼的位置变化方程知，多旋翼位置变化不仅与旋翼提供的拉力T有关，还与多旋翼的欧拉角即姿态有关；而由式(7)多旋翼的姿态变化方程知，姿态运动能够直接通过控制多旋翼围绕质心的转动力矩来实现；内环控制多旋翼的姿态，外环位置控制多旋翼在导航坐标系下的位置；

所述的欧拉角与单位四元数的转换关系为：在导航坐标系下，用φ表示滚转角，θ表示俯仰角，ψ表示偏航角，则对应的单位四元数q(φ,θ,ψ)由欧拉角表示为：

进一步地，步骤4中构建外环位置控制器，具体如下：

首先，位置控制只考虑旋翼推力、重力和空气阻力，根据多旋翼的位置动态方程式(1)建立状态向量x和位置虚拟控制量u：

然后，通过反步法结合Lyapunov原理，得到x、y、z方向的虚拟反馈控制量u_x、u_x、u_x为分别为：

其中δx₁、δx₃、δx₅和δx₂、δx₄、δx₆分别是x_n、y_n、z_n方向位置跟踪偏差和速度跟踪偏差，x_1d、x_3d、x_3d，

分别为x_n、y_n、z_n方向上的期望位置和期望速度，k_x1、k_x2、k_x3、k_x4、k_x5、k_x6是大于零的控制参数；

由式(9)、(13)，得到多旋翼输入拉力T的大小：

进一步地，步骤4中构建内环姿态控制器，具体如下：

首先，由式(9)得到多旋翼的期望姿态角φ_d、θ_d，φ_d、θ_d的绝对值小于45°：

期望偏航角ψ_d由控制指令给出，然后通过式(8)将期望姿态角转化为期望单位四元数q_d；

然后，通过实际四元数姿态为q＝[η ε^T]^T和目标四元数姿态为q_d＝[η_d ε_d ^T]^T得到偏差四元数为q_e：

最后，通过反步法结合Lyapunov原理得到求得

再通过式(7)得到反馈控制量τ_d：

其中k_e1＞0，K_e2＝diag{k_e21,k_e22,k_e2}，k_e2、k_e22、k_e2＞0，e₁为四元数位置误差，e₂为角速度误差，ω为实际角速度，ω_d为期望角速度。

下面将结合本发明实例中的附图，来详细的介绍本发明实施例中的技术方案。

实施例

结合图1，本发明实施例提供一种基于四元数的多旋翼非线性飞行控制方法，包括：

S1，对飞行器运动过程及外界环境做出假设；

S2，建立多旋翼位置控制模型和姿态控制模型；

S3，将多旋翼的飞行控制分解为外环位置控制和内环姿态控制；

S4，利用反步法结合Lyapunov原理设计外环位置控制器；

S5，利用反步法结合Lyapunov原理设计内环姿态控制器。

本发明实例所述的基于四元数的多旋翼飞行控制方法，首先，为了得到多旋翼的飞行器动力学方程，对飞行器运动过程及外界环境做出假设，再根据牛顿第二定律建立多旋翼位置控制模型，跟据动量矩定理和四元数运动学方程建立姿态控制模型。其次，为了降低***地耦合度，提高多旋翼的控制性能和控制精度，将多旋翼的飞行控制分解为外环位置控制和内环姿态控制，同时多旋翼内外环之间通过欧拉角与单位四元数的转换关系联系在一起。最后，利用反步法结合Lyapunov原理设计外环位置控制器和内环姿态控制器。与欧拉角相比，四元数能够更直观地表示旋转，能够有效的避免欧拉角可能会产生的奇异和万向节死锁问题。

在上述的基于四元数的多旋翼飞行控制方法中的具体实施方式中，可选的，依据控制复杂程度和实际应用的情况，选取四旋翼作为具体实施实例。跟据多旋翼的控制模型和数学精度的要求，建立的多旋翼的数学模型包括：

多旋翼飞行器的参数选取EPFL的OS4(“十”字型四旋翼)，机体结构和相关参数如图2和表1所示。电机提供的控制量分别为：旋翼拉力T，驱动力矩τ_p，τ_q，τ_r，其表达式为：

其中，ω₁，ω₂，ω₃，ω₄分别表示四旋翼四个转子的转速，K_T表示推力系数，K_Q表示阻力系数，l_x、l_y分别表示机体x、y轴力臂长度，Ω表示各个转子转速和。

本发明实例中，基于牛顿第二定律和空气动力学，对多旋翼进行受力分析，可以得到多旋翼的位置控制模型：

其中

分别表示x、y、z方向上的速度和加速度，φ表示滚转角，θ表示俯仰角，ψ表示偏航角，T表示旋翼拉力，m表示机体质量，g表示重力加速度，k表示空气阻力。

表1本发明实施例提供的EPFL的OS4飞行器参数

本发明实例中，利用动量矩定理和四元数运动学方程，得到四元数的姿态控制模型：

其中，q＝[q₀q₁q₂q₃]^T＝[η ε^T]^T为多旋翼的单位四元数姿态，q₀、q₁、q₂、q₃分别为四元数的四个参数，η为标量，表示四元数的实部，ε为矢量，表示四元数的虚部，

ω＝[p,q,r]^T表示多旋翼的转动角速度，

为角加速度，转动惯量矩阵J＝diag{J_bx,J_by,J_bz}，驱动力矩τ＝[τ_pτ_qτ_r]^T，τ_rotor为多旋翼的陀螺力矩，J_rotor为转子的转动惯量，则：

跟据多旋翼位置和姿态控制模型分析***的耦合关系，结合实例得到控制策略示意图3。

跟据多旋翼的数学模型和控制策略图，设计基于反步法和Lyapunov稳定性原理的反馈控制器。主要步骤包括：建立多旋翼输入向量与多旋翼状态之间的关系，设计多旋翼的外环控制器和内环控制器，并求出控制量。

所述的多旋翼的输入量为：τ_p，τ_q，τ_r，T，多旋翼的状态可以分为姿态状态和位置状态，由式(2)和(3)可知，多旋翼的姿态由输入向量τ＝[τ_p τ_q τ_r]^T来控制，多旋翼的位置可以由输入T和多旋翼的姿态(欧拉角)共同控制。

在本发明实例中，跟据多旋翼位置、姿态与输入之间的耦合关系，结合其欠驱动、非线性、易受外界干扰等特点，为了设计出精度高、抗干扰性强的控制***，基于双环控制的思想，将多旋翼的控制分解为外环位置控制和内环姿态控制。

在本发明实例中，基于反步法和Lyapunov稳定性原理得到位置反馈补偿量的详细过程如下：

位置控制只考虑旋翼推力，重力和空气阻力，根据多旋翼的位置动态方程(2)建立状态向量x和虚拟控制量u：

首先，设计x方向上的位置控制器，其子***可以表示为：

首先，假定μ₁为x速度的虚拟控制量，δx₁为x方向的位置跟踪误差，δx₂为x方向的速度跟踪误差且δx₂(t₀)＝0，x_1d为期望位置，V₁是关于δx₁的Lyapunov能量函数，

对V₁求导可得：

设控制参数k_x1＞0,令虚拟控制量μ₁满足：

μ₁＝x_2d+k_x1δx₁ (9)

则有：

其中，第一项为正数，第二项的符号未知。为了使

为负并引入实际输入，需设计Lyapunov能量函数V₂。

由式(7)(9)可得：δx₂＝x₂-μ₁＝x₂-x_2d-k_x1δx₁，再结合(7)(9)可得：

设V₂(δx₁,δx₂)＝V₁+δx₂ ²，再对能量函数V₂求导，可得：

令加速度

k_x2＞0为控制参数,则有：

由Lyapunov稳定性定理可知，得到的平衡点是全局稳定点。

因此，x方向的虚拟控制量u_x为：

同样的，y方向：

其中，x_3d、δx₃是y方向期望位置和位置偏差，δx₄是y方向速度x₄的跟踪偏差，k_x3＞0,k_x4＞0是控制参数。

z方向：

其中，x_5d、δx₅是z方向期望位置和位置偏差，δx₆是z方向速度x₆的跟踪偏差，k_x5＞0,k_x6＞0是控制参数。

通过式(5)，求出多旋翼的旋翼拉力T：

在本发明实例中，基于反步法和Lyapunov稳定性原理得到反馈控制量τ_d的详细过程如下：

首先列出无外界干扰条件下的多旋翼控制模型：

其次，通过步骤4的位置控制率和位置控制方程，得到多旋翼的期望姿态角，为了保证在姿态角求逆的过程中有意义，以及多旋翼飞行过程中的稳定，需要限定φ_d、θ_d的值，其绝对值小于45°。

期望偏航角ψ_d由位置控制指令给出，然后将期望姿态角转化为期望单位四元数q_d。

转换公式如下：

再通过实际四元数姿态为q＝[η ε^T]^T和目标四元数姿态为q_d＝[η_d ε_d ^T]^T得到偏差四元数为q_e。

接下来用反步法结合Lyapunov原理设计姿态控制器：

假设λ₁为角速度ω跟踪虚拟量，e₂为角速度跟踪误差且e₂(t₀)＝0，e₁为四元数实部跟踪误差，ω_d为期望角速度，V₃为Lyapunov能量函数，

对V₃求导可得：

设控制参数k_e1＞0，令跟踪虚拟量λ₁满足：

λ₁＝ω_d+k_e1ε_e (24)

则有，

第一项为负数，第二项符号不确定。为了使

为负数，且引入实际的输入，需要设计Lyapunov能量函数V₄。

由(22)(24)可得：e₂＝ω-λ₁＝ω-ω_d-k_e1ε_e，再结合式(22)(24)可得：

设

对V₄(e₁,e₂)求导，可得：

令

其中K_e2＝diag{k_e2,k_e22,k_e23}，且有k_e2、k_e22、k_e23＞0，为控制参数，将

带入

所以有：

因此，得到的平衡点是全局稳定的。把

带入式(14)，可得：

综合式(14)(15)(16)(17)(29)，多旋翼的控制器设计完毕。

本发明实例中，利用上述方法设计非线性控制器，在Simulink仿真平台上搭建多旋翼的仿真模型，在理论研究和实际工程中，通过调节控制器的参数和分析多旋翼的姿态和位置响应，分析设计出的非线性控制器能否调节***性能，达到预期要求。***性能的评测主要在理想无外界干扰的Simulink仿真环境下，控制器的响应速度、超调量、稳态误差等指标。

在本发明实例中，首先验证内环非线性控制器能否实现对姿态的控制，设多旋翼飞行器姿态角及其角速度初始状态为0，位置及其速度也为0(q₀＝[1 0 0 0]^T)。期望横滚、俯仰、偏航角都为为30°(q₀＝[0.92 0.18 0.31 0.18]^T)。当k_e1＝17，K_e2＝[10；10；10]时，姿态角响应曲线如图4所示，姿态角调节时间约为0.5秒，调节时间小于1秒，且没有超调。

对于外环控制器的验证，先验证其对阶跃信号的反应，其次，验证对斜坡信号的响应。经过多次调节，得到外环控制器的控制参数：k_x1＝k_x2＝k_x3＝k_x4＝1.3，k_x5＝2，k_x6＝1.6。

外环控制器对阶跃信号的响应如图5，通过仿真曲线可以看出，所有的位置的阶跃响应误差都趋近于0，无超调，调节时间约为2s，这表明***的稳态性能和动态性能均满足设计指标。

外环控制器对单位斜坡信号的响应如图6，通过仿真曲线，外环位置控制器能够准确地跟踪单位斜坡信号，***稳定后，稳态误差为0.0015，基本与期望轨迹重合，不存在滞后现象。仿真结果表明：设计的控制器能够有效的跟踪单位斜坡信号。

在本发明实施例中，验证控制器对多旋翼飞行器按给定航迹飞行的跟踪特性。以“回”形和“螺旋”形航迹为例。

“回”形轨迹的起飞点为(0,0,0)，中间先后经过(0,0,5)、(5,0,5)、(5,5,5)、(0,5,5)、(0,0,5)后，最后返回起飞点(0,0,0)，航点飞行响应曲线如图7所示。通过对三维空间中轨线跟踪曲线的分析，飞行器能比较快速、准确的跟踪目标轨线，在各个方向上的实时误差收敛时间都小于2.5s。当水平方向切换指令时，高度z方向最大的误差绝对值小于0.08m。仿真结果表明：设计的非线性控制器能够有效的跟踪“回”形轨迹。

从仿真曲线图8可以看出，多旋翼能够准确地跟踪“螺旋线”轨迹，并到达稳态。多旋翼起始位置为坐标原点。其中高度指令为单位斜坡指令(z＝t)，高度稳态误差能够在极短时间(5s)内追踪到航迹指令，并将稳态误差减小到0.0015m。水平方向的轨迹指令为正弦信号(x＝5sin(ωt)，y＝5cos(ωt)，ω＝π/40)，由图可以看出在x方向上，当***稳定后由于相位差引起的误差绝对值小于0.003m。

综上所述，上述仿真实验初步验证了基于四元数的多旋翼非线性控制方法能够对多旋翼的飞行进行有效的控制，控制性能指标包括：响应时间、稳态误差和抗干扰能力。

Claims (6)

1.一种基于四元数的多旋翼非线性飞行控制方法，其特征在于，包括以下步骤：

步骤1，对外界环境和飞行器运动条件做出假设，得到四旋翼飞行器动力学方程；

步骤2，建立控制模型：根据牛顿第二定律建立多旋翼位置控制模型，跟据动量矩定理和四元数运动学方程建立姿态控制模型；

步骤3，将多旋翼的飞行控制分解为外环位置控制和内环姿态控制，建立欧拉角与单位四元数之间的转换关系；

步骤4，利用反步法结合Lyapunov原理，分别构建外环位置控制器和内环姿态控制器，控制多旋翼的位置和姿态。

2.根据权利要求1所述的基于四元数的多旋翼非线性飞行控制方法，其特征在于，步骤1所述对外界环境和飞行器运动条件做出假设，包括：

1)在飞行过程中，重力加速度g不变化，多旋翼是质量不变的六自由度刚体；

2)多旋翼形状结构关于中心对称，各部分的材质相同，质心位于机体中心；

3)惯性坐标系下，忽略地球的曲率；

4)不考虑地球公转和自转的影响。

3.根据权利要求1或2所述的基于四元数的多旋翼非线性飞行控制方法，其特征在于，步骤2所述控建立控制模型，包括多旋翼在以北-东-地为坐标轴的导航系下的位置控制模型和在机体系下的姿态控制模型：

所述的位置控制模型：

其中

分别表示x、y、z方向上的速度，

分别表示x、y、z方向上的加速度，φ表示滚转角，θ表示俯仰角，ψ表示偏航角，T表示旋翼拉力，m表示机体质量，g表示重力加速度，k表示空气阻力；

所述的刚体动量矩定理：

其中J＝diag{J_bx，J_by，J_bz}表示转动惯量矩阵，τ＝[τ_p，τ_q，τ_r]^T表示电机提供的驱动力矩，ω＝[p，q，r]^T表示多旋翼的转动角速度，[ ]_×表示反对称矩阵，τ_rotor表示旋翼的陀螺力矩；

J_rotor表示转子转动惯量，Ω为各电机转子的转速和；

所述的四元数的运动学方程为：

其中，P(ω)＝[0 ω^T]^T是角速度矢量ω的四元数形式，q＝[q_o q₁ q₂ q₃]^T＝[η ε^T]^T为多旋翼的单位四元数姿态，q₀、q₁、q₂、q₃分别为单位四元数的四个参数；η为标量，表示四元数的实部；ε为矢量，表示四元数的虚部，

表示四元数乘法；由四元数乘法，得四元数运动学方程

其中T(q)∈R^4×3，表达式为：

因此，结合动量矩定理式(2)和四元数运动学方程式(5)得到多旋翼的姿态控制模型：

4.根据权利要求3所述的基于四元数的多旋翼非线性飞行控制方法，其特征在于，步骤3所述的将多旋翼的飞行控制分解为外环位置控制和内环姿态控制，建立欧拉角与单位四元数之间的转换关系，具体如下：

将多旋翼的飞行控制分解为外环位置控制和内环姿态控制的依据是：由式(1)多旋翼的位置变化方程知，多旋翼位置变化不仅与旋翼提供的拉力T有关，还与多旋翼的欧拉角即姿态有关；而由式(7)多旋翼的姿态变化方程知，姿态运动能够直接通过控制多旋翼围绕质心的转动力矩来实现；内环控制多旋翼的姿态，外环位置控制多旋翼在导航坐标系下的位置；

所述的欧拉角与单位四元数的转换关系为：在导航坐标系下，用φ表示滚转角，θ表示俯仰角，ψ表示偏航角，则对应的单位四元数q(φ，θ，ψ)由欧拉角表示为：

5.根据权利要求4所述的基于四元数的多旋翼非线性飞行控制方法，其特征在于，步骤4中构建外环位置控制器，具体如下：

首先，位置控制只考虑旋翼推力、重力和空气阻力，根据多旋翼的位置动态方程式(1)建立状态向量x和位置虚拟控制量u：

然后，通过反步法结合Lyapunov原理，得到x、y、z方向的虚拟反馈控制量u_x、u_x、u_x为分别为：

其中δx₁、δx₃、δx₅和δx₂、δx₄、δx₆分别是x_n、y_n、z_n方向位置跟踪偏差和速度跟踪偏差，x_1d、x_3d、x_3d，

分别为x_n、y_n、z_n方向上的期望位置和期望速度，k_x1、k_x2、k_x3、k_x4、k_x5、k_x6是大于零的控制参数；

由式(9)、(13)，得到多旋翼输入拉力T的大小：

6.根据权利要求5所述的基于四元数的多旋翼非线性飞行控制方法，其特征在于，步骤4中构建内环姿态控制器，具体如下：

首先，由式(9)得到多旋翼的期望姿态角φ_d、θ_d，φ_d、θ_d的绝对值小于45°：

期望偏航角ψ_d由控制指令给出，然后通过式(8)将期望姿态角转化为期望单位四元数q_d；

然后，通过实际四元数姿态为q＝[η ε^T]^T和目标四元数姿态为q_d＝[η_d ε_d ^T]^T得到偏差四元数为q_e：

最后，通过反步法结合Lyapunov原理得到求得

再通过式(7)得到反馈控制量τ_d：

其中k_e1＞0，K_e2＝diag{k_e21，k_e2，k_e23}，k_e21、k_e22、k_e23＞0，e₁为四元数位置误差，e₂为角速度误差，ω为实际角速度，ω_d为期望角速度。

Images