CN117742364B - 含姿态规划器的四旋翼无人机的轨迹跟踪控制方法及系统 - Google Patents

Abstract

本发明公开了含姿态规划器的四旋翼无人机的轨迹跟踪控制方法及系统，涉及无人机控制领域，包括：实时获取四旋翼无人机质心坐标、偏航角、俯仰角、滚转角、沿坐标方向的线速度和对应转向角的角速度；计算基于四元数的四旋翼无人机的动力学模型；根据四元数动力学模型，构造光滑姿态规划器；根据规划的姿态角速度，首次利用Rodrigues参数计算出四旋翼无人机的姿态误差动力系统模型；结合位置动力系统模型和基于Rodrigues参数的姿态误差动力系统模型，利用分层控制技术构造外环轨迹跟踪控制模块和内环姿态控制模块；得到含姿态规划器的四旋翼无人机的轨迹跟踪控制结果。本发明克服了现有的四旋翼无人机控制技术中出现的锁死，控制器计算冗余，解卷现象。

Description

含姿态规划器的四旋翼无人机的轨迹跟踪控制方法及系统

技术领域

本发明涉及无人机控制技术领域，特别涉及一种含姿态规划器的四旋翼无人机的轨迹跟踪控制方法及系统。

背景技术

四旋翼无人机的控制具有极大的挑战性，是因为四旋翼无人机属于4控6的欠驱动系统，这种系统具有拓扑障碍，即，连续反馈不能全局镇定具有转动自由度的机械系统的姿态。因此人们针对四旋翼无人机提出了基于不同姿态描述的多种控制技术，主要包括基于欧拉角，旋转矩阵和四元数的姿态控制器的设计。

基于欧拉角的姿态描述具有最小维数3，易于测量的欧拉角设计和直观的物理意义，被广泛用于工业机器人、飞行器、车辆、船舶等具有旋转关节的设备的系统建模与控制中。但是，基于欧拉角描述的最大缺点是在特定位置有锁死问题，其表现为陀螺仪的万向节卡住，不能在无限小的时间内移动有限量，这使得基于欧拉角设计的控制器做不到全局控制。

旋转矩阵是无奇点的姿态描述形式。尽管基于旋转矩阵的控制器能够克服锁死问题，但是旋转矩阵是3*3维的，这导致控制器具有计算冗余问题，存储成本高，在大型的计算模拟中很少被采用。另一方面，旋转矩阵属于特殊的三阶正交群，其对任何欧几里得空间都是不可压缩的，因此基于旋转矩阵的连续反馈控制器也做不到全局控制。

基于四元数模型的连续控制器尽管能克服锁死和计算冗余问题，但是又会出现解卷现象，即基于四元数的控制器会导致无人机进行不必要的完整旋转。为克服解卷问题，现有的技术不得不引入具有迟滞的混杂控制，因此基于四元数的控制器难于实现，且不具有良好的鲁棒性。

发明内容

本发明的目的是提供一种含姿态规划器的四旋翼无人机的轨迹跟踪控制方法及系统，同时解决现有的基于不同姿态描述的四旋翼无人机控制技术中出现的锁死(即，陀螺仪万向节卡住)，控制器计算冗余，解卷现象(即，无人机进行不必要的完整旋转)，以及避免为克服解卷现象引入的不连续、迟滞混杂控制等问题。

为实现上述目的，本发明提供了如下方案：

第一方面，本发明提供了一种含姿态规划器的四旋翼无人机的轨迹跟踪控制方法，包括：

获取四旋翼无人机的质心坐标历史数据集、偏航角历史数据集、俯仰角历史数据集、滚转角历史数据集、沿坐标方向的线速度历史数据集和对应转向角的角速度历史数据集；

实时获取四旋翼无人机质心坐标、偏航角、俯仰角、滚转角、沿坐标方向的线速度和对应转向角的角速度；

对所述四旋翼无人机的质心坐标历史数据集、偏航角历史数据集、俯仰角历史数据集、滚转角历史数据集、沿坐标方向的线速度历史数据集和对应转向角的角速度历史数据集进行运动学计算、四个螺旋桨力学计算和机体力学计算，并计算姿态四元数，得到基于四元数的四旋翼无人机的动力学模型；

设计期望的全驱力控制，并根据期望的全驱力控制，计算体坐标系的主推力方向与惯性坐标系的期望推力方向，构造相对四元数；

基于所述相对四元数构建光滑姿态规划器；

基于所述光滑姿态规划器确定姿态误差四元数和角速度误差四元数；

基于所述姿态误差四元数和角速度误差四元数利用Rodrigues参数计算四旋翼无人机的姿态误差；

基于所述无人机的姿态误差确定位置控制外环设计结果和姿态控制内环设计结果；

根据所述位置控制外环设计结果和姿态控制内环设计结果，构建含姿态规划器的四旋翼无人机的位置和姿态控制模型；

将实时获取的四旋翼无人机质心坐标、偏航角、俯仰角、滚转角、沿坐标方向的线速度和对应转向角的角速度输入至所述含姿态规划器的四旋翼无人机的位置和姿态控制模型，得到含姿态规划的四旋翼无人机的跟踪控制结果。

可选的，所述基于四元数的四旋翼无人机的动力学模型的表达式如下：

其中，r＝(x,y,z)^T和v分别表示四旋翼无人机的位置和速度，q＝(q₁,q₂,q₃,q₄)＝(η,ε^T)表示由欧拉角计算得到的姿态单位四元数，其中和为四元数的标量和矢量分量部分，表示全体实数集，表示质量，g表示重力加速度，和表示计算四个螺旋桨得到的提升力和扭矩矢量，e₃＝(0,0,0)^T为z_b轴上的单位向量，为体惯性矩阵，表示旋转角速度，为角速度四元数，d_p和d_a分别表示平动系统和转动系统受到的不确定干扰，如阻力效应和风湍流，为r的一阶导数，为v的一阶导数，为q的一阶导数，为ω的一阶导数，q^-1为q的逆。

可选的，所述期望的全驱力控制的表达式如下：

其中，μ_d表示期望的全驱动力控制，表示质量，表示全体实数集，g表示重力加速度，e₃表示z_b轴上的单位向量，是r_*的二阶导数，r_*表示参考轨迹，c₂,d₁及c₁均表示正的设计参数，r_e表示轨迹跟踪误差，v_e表示平均速度误差。

可选的，基于所述相对四元数构建光滑姿态规划器具体包括以下步骤：

基于所述期望的全驱动力控制经四元数开方，结合偏航角四元数q_ψ*进行期望姿态运算得到期望姿态四元数q_d；

实时姿态四元数q与所述期望姿态四元数q_d经姿态误差运算，得到姿态误差四元数q_e和Rodrigues参数姿态误差ρ_e；

所述期望姿态四元数q_d经期望姿态求导运算，结合所述姿态误差四元数q_e和实时旋转角速度ω进行角速度误差计算，得到旋转角速度误差ω_e和期望旋转角速度ω_d的变换数据ω_p；

姿态四元数q经旋转矩阵计算得到旋转矩阵R。

可选的，所述姿态控制内环设计结果的表达式如下：

其中，c₄表示正的设计参数，ω_Z表示表示角速度误差，J表示体惯性矩阵，表示角速度镇定函数的一阶导数，表示基于四元数描述的期望角速度的一阶导数，ω表示角速度，Φ^T表示基于Rodrigues参数的旋转矩阵，ρ_e表示基于Rodrigues参数的姿态误差。

第二方面，基于本发明中的上述方法，本发明另外提供一种含姿态规划器的四旋翼无人机的轨迹跟踪控制系统，包括：

第一数据集获取模块，用于获取四旋翼无人机的质心坐标历史数据集、偏航角历史数据集、俯仰角历史数据集、滚转角历史数据集、沿坐标方向的线速度历史数据集和对应转向角的角速度历史数据集；

第二数据集获取模块，用于实时获取四旋翼无人机质心坐标、偏航角、俯仰角、滚转角、沿坐标方向的线速度和对应转向角的角速度；

基于四元数的四旋翼无人机的动力学模型构建模块，用于对所述四旋翼无人机的质心坐标历史数据集、偏航角历史数据集、俯仰角历史数据集、滚转角历史数据集、沿坐标方向的线速度历史数据集和对应转向角的角速度历史数据集进行运动学计算、四个螺旋桨力学计算和机体力学计算，并计算姿态四元数，得到基于四元数的四旋翼无人机的动力学模型；

相对四元数构造模块，用于设计期望的全驱力控制，并根据期望的全驱力控制，计算体坐标系的主推力方向与惯性坐标系的期望推力方向，构造相对四元数；

光滑姿态规划器构建模块，用于基于所述相对四元数构建光滑姿态规划器；

姿态误差四元数和角速度误差四元数确定模块，用于基于所述光滑姿态规划器确定姿态误差四元数和角速度误差四元数；

四旋翼无人机的姿态误差计算模块，用于基于所述姿态误差四元数和角速度误差四元数利用Rodrigues参数计算四旋翼无人机的姿态误差；

外环设计结果和内环设计结果确定模块，用于基于所述无人机的姿态误差确定位置控制外环设计结果和姿态控制内环设计结果；

四旋翼无人机的位置和姿态控制模型构建模块，用于根据所述位置控制外环设计结果和姿态控制内环设计结果，构建含姿态规划器的四旋翼无人机的位置和姿态控制模型；

跟踪控制结果确定模块，用于将实时获取的四旋翼无人机质心坐标、偏航角、俯仰角、滚转角、沿坐标方向的线速度和对应转向角的角速度输入至所述含姿态规划器的四旋翼无人机的位置和姿态控制模型，得到含姿态规划的四旋翼无人机的跟踪控制结果。

可选的，所述基于四元数的四旋翼无人机的动力学模型的表达式如下：

其中，r＝(x,y,z)^T和v分别表示四旋翼无人机的位置和速度，q＝(q₁,q₂,q₃,q₄)＝(η,ε^T)表示由欧拉角计算得到的姿态单位四元数，其中和为四元数的标量和矢量分量部分，表示全体实数集，表示质量，g表示重力加速度，和表示计算四个螺旋桨得到的提升力和扭矩矢量，e₃＝(0,0,0)^T为zb轴上的单位向量，为体惯性矩阵，表示旋转角速度，为角速度四元数，d_p和d_a分别表示平动系统和转动系统受到的不确定干扰，如阻力效应和风湍流，为r的一阶导数，为v的一阶导数，为q的一阶导数，为ω的一阶导数，q^-1为q的逆。

可选的，所述期望的全驱力控制的表达式如下：

其中，μ_d表示期望的全驱动力控制，表示质量，表示全体实数集，g表示重力加速度，e₃表示z_b轴上的单位向量，是r_*的二阶导数，r_*表示参考轨迹，c₂,d₁及c₁均表示正的设计参数，r_e表示轨迹跟踪误差，v_e表示平均速度误差。

第三方面，本发明提供了一种电子设备，包括存储器及处理器，所述存储器用于存储计算机程序，所述处理器运行所述计算机程序以使所述电子设备执行上述的含姿态规划器的四旋翼无人机的轨迹跟踪控制方法。

第四方面，本发明提供了一种计算机可读存储介质，其存储有计算机程序，所述计算机程序被处理器执行时实现上述的含姿态规划器的四旋翼无人机的轨迹跟踪控制方法。

根据本发明提供的具体实施例，本发明公开了以下技术效果：

在本发明，通过引入虚拟力控制将四旋翼无人机的欠驱动位置动力学模型化为全驱形式，避免了Brockett的限制条件，从而可以设计得到连续的主推力控制，与现有的混杂控制相比，易于实现，控制精度高，且具有更强的鲁棒性。

首次利用Rodrigues参数设计光滑姿态规划器，将基于四元数描述姿态引起的解卷问题的奇异平衡点±1(一个稳定，另一个不稳定，同时又表示同一姿态)化为基于Rodrigues向量描述的姿态动力学系统的无穷远处，而平衡点为坐标原点，可以用典型的反推方法设计光滑或连续的反馈力矩控制器，从而有效避免了解卷问题和引入混杂控制，且具有更好的机动性。

此外，由于基于Rodrigues向量的姿态系统是由四元数转化而来，利用四元数的归一化减轻了角度漂移的影响，所设计的力矩控制器也可以有效的避免计算冗余问题，提高了实际适用性。

由于位置控制外环设计结果(位置控制器)和姿态控制内环设计结果(姿态控制器)都利用了典型的反推方法，控制器设计易于与自适应观测器相结合，能够解决更复杂的控制问题，以提高四旋翼无人机的控制精度。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明提供的含姿态规划器的四旋翼无人机的轨迹跟踪控制方法流程图；

图2为本发明提供的四旋翼无人机坐标系和结构示意图；

图3为本发明提供的位置和姿态动力学模型计算模块结构示意图；

图4为本发明提供的外环控制模块结构图；

图5为本发明提供的姿态规划器模块结构示意图；

图6为本发明提供的内环姿态控制模块结构示意图；

图7为本发明提供的控制结果实验展示模块；

图8为本发明提供的含姿态规划器的四旋翼无人机的轨迹跟踪控制结构图；

图9为本发明提供的四旋翼无人机在无干扰情形下的低空飞行实验仿真结果；(a)部分为x,x_*仿真结果，(b)部分为y,y_*仿真结果，(c)部分为z,z_*仿真结果，(d)部分为基于Rodrigues参数的姿态误差ρ_e仿真结果，(e)部分为提升力F仿真结果，(f)部分为力矩控制τ的仿真结果；

图10(a)为本发明提供的四旋翼无人机在无干扰情形下低空飞行实验的初始阶段仿真结果图；

图10(b)为本发明提供的四旋翼无人机在无干扰情形下的低空飞行实验的完整飞行轨迹；

图11为本发明提供的四旋翼无人机在3D虚拟城市低空飞行的起飞阶段示意图；

图12为本发明提供的四旋翼无人机在3D虚拟城市低空飞行的上升阶段示意图；

图13为本发明提供的四旋翼无人机在3D虚拟城市低空飞行的巡航阶段示意图；

图14为本发明提供的四旋翼无人机在3D虚拟城市低空飞行的下降阶段示意图；

图15为本发明提供的四旋翼无人机在3D虚拟城市低空飞行的着陆阶段示意图；

图16为本发明提供的四旋翼无人机在有外部干扰情形下的实验仿真结果示意图；(a)部分为x,x_*仿真结果，(b)部分为y,y_*仿真结果，(c)部分为z,z_*仿真结果，(d)部分为基于Rodrigues参数的姿态误差ρ_e仿真结果，(e)部分为提升力F仿真结果，(f)部分为力矩控制τ的仿真结果；

图17为本发明提供的四旋翼无人机在高度、巡航半径和外部干扰都加倍的情形下的实验仿真结果示意图；(a)部分为x,x_*仿真结果，(b)部分为y,y_*仿真结果，(c)部分为z,z_*仿真结果，(d)部分为基于Rodrigues参数的姿态误差ρ_e仿真结果，(e)部分为提升力F仿真结果，(f)部分为力矩控制τ的仿真结果；

图18为本发明提供的技术路线图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明的目的是提供一种含姿态规划器的四旋翼无人机的轨迹跟踪控制方法及系统，同时解决现有的基于不同姿态描述的四旋翼无人机控制技术中出现的锁死(即，陀螺仪万向节卡住)，控制器计算冗余，解卷现象(即，无人机进行不必要的完整旋转)，以及避免为克服解卷现象引入的不连续、迟滞混杂控制等问题。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

实施例一

图1为本发明提供的含姿态规划器的四旋翼无人机的轨迹跟踪控制方法流程图，如图1所示，所述方法包括：

步骤101：获取四旋翼无人机的质心坐标历史数据集、偏航角历史数据集、俯仰角历史数据集、滚转角历史数据集、沿坐标方向的线速度历史数据集和对应转向角的角速度历史数据集。

步骤102：实时获取四旋翼无人机质心坐标、偏航角、俯仰角、滚转角、沿坐标方向的线速度和对应转向角的角速度。

步骤103：对所述四旋翼无人机的质心坐标历史数据集、偏航角历史数据集、俯仰角历史数据集、滚转角历史数据集、沿坐标方向的线速度历史数据集和对应转向角的角速度历史数据集进行运动学计算、四个螺旋桨力学计算和机体力学计算，并计算姿态四元数，得到基于四元数的四旋翼无人机的动力学模型。

具体如下：

如图2所示，图2为四旋翼无人机的坐标系示意图，基于四元数的四旋翼无人机动力学模型如下：

其中r＝(x,y,z)^T和v分别表示四旋翼无人机的位置和速度，q＝(q₁,q₂,q₃,q₄)＝(η,ε^T)表示由欧拉角(偏航角、俯仰角、滚转角)计算得到的姿态单位四元数，其中和为四元数的标量和矢量分量部分，表示全体实数集。表示质量，g表示重力加速度，和表示计算四个螺旋桨得到的提升力和扭矩矢量，e₃＝(0,0,0)^T为z_b轴上的单位向量，为体惯性矩阵，表示旋转角速度，为角速度四元数，d_p和d_a分别表示平动系统和转动系统受到的不确定干扰，如阻力效应和风湍流，为r的一阶导数，为v的一阶导数，为q的一阶导数，为ω的一阶导数，q^-1为q的逆。

步骤104：设计期望的全驱力控制，并根据期望的全驱力控制，计算体坐标系的主推力方向与惯性坐标系的期望推力方向，构造相对四元数。

步骤105：基于所述相对四元数构建光滑姿态规划器。

步骤106：基于所述光滑姿态规划器确定姿态误差四元数和角速度误差四元数。

步骤107：基于所述姿态误差四元数和角速度误差四元数利用Rodrigues参数计算四旋翼无人机的姿态误差。

为了克服解卷问题，同时又能设计光滑或连续的反馈控制器，使无人机完成轨迹的全局渐近跟踪。对η≠0，取Rodrigues向量引入用Rodrigues向量表示的姿态运动学模型

由于四旋翼无人机属于四控六的欠驱动系统，故最终只能有4个参考输出变量自由给定。根据实际需求，参考位置可以任意给定，而偏航角根据需要给定或从轨迹中提取。

步骤108：基于所述无人机的姿态误差确定位置控制外环设计结果和姿态控制内环设计结果。

步骤109：根据所述位置控制外环设计结果和姿态控制内环设计结果，构建含姿态规划器的四旋翼无人机的位置和姿态控制模型。

步骤110：将实时获取的四旋翼无人机质心坐标、偏航角、俯仰角、滚转角、沿坐标方向的线速度和对应转向角的角速度输入至所述含姿态规划器的四旋翼无人机的位置和姿态控制模型，得到含姿态规划的四旋翼无人机的跟踪控制结果。

本发明的设计目标为：设计提升力控制F和姿态扭矩控制τ和姿态规划器估计使得四旋翼无人机能够跟踪期望的位置信号r_*，同时保证系统的其他信号有界。

要求无人机下降的加速度不超过自由落体的加速度。

为了实现上述目标，本发明中的技术方案又可分为多个模块，具体包括：动力学模型计算模块、外环控制模块、姿态规划器模块、内环控制模块、四旋翼动力学模型计算模块以及控制结果实验展示模块。下面对每个模块一一展开介绍：

动力学模型计算模块

该模块包括：合成动力学模型计算模块、位置误差动力学模型以及Rodrigues向量表示的姿态误差动力学模型。

1)合成动力学模型计算模块

合成动力学模型计算模块也即位置和姿态动力学模型计算模块，具体如图3所示，其中，提升力F是外环控制器模块计算得到，R是经姿态规划器计算得到的期望姿态对应的旋转矩阵，τ表示内环控制模块计算得到的力矩控制。v表示位置动力学模型计算模块输出的平动速度，r表示位置；ω是姿态动力学模型计算模块得到的旋转角速度。

动力学模型计算模块的计算过程：由外环控制器模块得到的提升力F、姿态规划器输出的旋转矩阵R和姿态动力学模型计算模块输出的姿态q，经位置动力学模型模块得到位置r、平动速度v；由内环控制模块输出的力矩信号τ作为输入信号进入姿态动力学模型计算模块，得到姿态q和角速度ω信号。

对于期望的位置和姿态，构造位置误差和姿态误差动力学模型，并将跟踪控制问题转化为误差动力学模型的镇定问题。

2)位置误差动力学模型

对期望位置r_*＝(x_*,y_*,z_*)^T，定义位置误差re＝r-r*，可得位置误差动力学模型

其中Δ＝μ-μ_d。

3)Rodrigues向量表示的姿态误差动力学模型

旋转姿态误差定义为通过计算等式的导数，得到期望姿态四元数逆元的导数

计算姿态误差q_e的导数，得到姿态误差运动学模型

其中体坐标系下的角速度误差ω_e＝ω-ω_p，得到Rodrigues参数表示的姿态误差运动学模型

结合四旋翼无人机力学计算，得到体坐标系下的姿态误差动力学模型

基于Rodrigues向量表示的姿态误差动力学模型的奇点在无穷远处，而平衡点在0处，因此可以设计连续或光滑的反馈控制器，从而避免现有控制方法中基于欧拉角、旋转矩阵和四元数所表示的姿态误差动力学模型处理锁死和解卷问题，也避免了引入混杂控制。

外环控制模块

第一步：引入平动速度误差其中，v_α是待设计的期望平动速度控制。定义平动位移Lyapunov函数

根据链式求导法则，得到其时间导数如下：

其中，表示V_p1的一阶导数；根据位置误差运动学模型，设计期望的平动速度控制为

其中c₁为正的设计参数，得到Lyapunov函数V_p1的导数如下：

第二步：计算平动速度误差v_e和期望的平动速度控制v_α的导数，得到平动位置误差动力学模型的新表述如下：

其中，为v_e的一阶导数，是r_e的二阶导数，为v_α的一阶导数，是r_*的二阶导数，μ_d是待设计的全驱力控制。

定义平动系统的控制Lyapunov函数

根据链式求导法则，得到其时间导数：

根据Lyapunov稳定性定理和耦合项Δ的定义Δ＝FR_d(R_e-I₃)e₃，得到期望的全驱力控制

其中，c₂,d₁是正的设计参数。外环控制模块结构图如图4所示。

参见图4，其中，外环控制模块的输入信号包括：位置动力学模型计算模块提供的实时位置r和实时平动速度v；参考轨迹r_*及其一阶导数和二阶导数外环控制模块的输出包括：期望的力控制μ_d和提升力F。

外环控制模块的运算过程：实时位置r和参考轨迹r_*经跟踪误差计算模块得到轨迹跟踪误差r_e；结合平动速度v、参考轨迹r_*的一阶导数和二阶导数经期望力控制计算模块，得到期望的力控制μ_d；经大小数值计算模块得到提升力F。

姿态规划器模块

由外环控制计算模块得到的期望的力控制向量其对应存在一个期望的姿态四元数其中，θ_d表示期望的旋转角度，n_d表示期望的旋转轴，使得期望的力控制满足

一旦计算出期望的力控制向量μ_d，就可以得到提升力F＝|μ_d|，并且得到期望力控制向量μ_d的方向

在得到期望力控制向量μ_d的方向后，通过反向求解可得期望的姿态q_d。首先利用在体坐标系z_b轴上的单位向量e₃与期望力控制的方向合成一个四元数：

对Q_d进行开方运算，就可以得到期望的姿态四元数满足

由任意给定的偏航角ψ_*定义的姿态四元数满足可以合成包含偏航角的期望姿态四元数

这将作为姿态规划器的输出，用于合成姿态误差。通过反解得到期望的角速度由此完成姿态规划器模块的设计。姿态规划器的结构图如图5所示。

其中，姿态规划器模块中的q_ψ*表示从参考轨迹中提取的偏航角四元数，μ_d表示期望的外环力控制，q表示获取的实时姿态四元数，ω表示获取的实时旋转角速度；R表示由姿态规划器模块计算得到的实时姿态四元数对应的旋转矩阵，ρ_e表示Rodrigues参数姿态误差，ω_e表示旋转角速度误差信号，ω_p表示期望角速度ω_d的变换数据。

姿态规划器模块的计算过程为：力控制μ_d经四元数开方计算模块，结合偏航角四元数q_ψ*进入期望姿态运算模块得到期望姿态四元数q_d；实时姿态四元数q与期望姿态四元数q_d经姿态误差运算模块，得到姿态误差四元数q_e和Rodrigues参数姿态误差ρ_e；期望姿态四元数q_d经期望姿态求导运算，结合姿态误差四元数q_e和实时旋转角速度ω进入角速度误差模块，得到旋转角速度误差ω_e和旋转角速度ω_d的变换数据ω_p；姿态四元数q经旋转矩阵计算模块得到旋转矩阵R。

内环控制模块

利用姿态规划器模块计算得到的Rodrigues参数姿态误差ρ_e，旋转角速度误差ω_e，ω_p和旋转矩阵，设计内环姿态力矩控制器τ。

第一步：引入角速度误差ω_z＝ω_e-ω_α，其中ω_α是待设计的期望角速度控制。定义基于Rodrigues参数的姿态误差Lyapunov函数

根据链式求导法则，沿Rodrigues参数姿态误差运动学模型计算得到其时间导数如下：

其中，S(ρ_e)表示对ρ_e的矩阵运算，为V_a1的一阶导数。设计期望的角速度控制为

ω_α＝-c₃ρ_e-φ

其中，c₃,d₁是正的设计参数。得到Lyapunov函数V_p1的导数如下：

第二步：计算角速度误差ω_z和期望的角速度控制ω_α的导数，得到姿态误差动力学模型的新表述如下：

定义姿态误差动力学系统的Lyapunov函数为

根据链式求导法则，得到其时间导数：

根据Lyapunov稳定性定理，得到期望的力矩控制

其中，c₄是正的设计参数。内环控制模块结构图如图6所示。

参见图6，内环控制模块的输入信号包括：由外环控制模块输出的期望力控制μ_d和提升力F，参考轨迹r_*，以及由四旋翼无人机姿态动力学模型计算得到的实时姿态四元数q和实时角速度ω。内环控制模块的输出信号包括：姿态规划器模块输出的旋转矩阵R，经内环力矩控制模块计算得到的力矩控制τ。

内环控制模块的计算过程：参考轨迹r_*经期望的偏航角提取模块得到偏航角四元数q_ψ*，其将作为姿态规划器的输入；结合期望力控制μ_d、姿态四元数q和角速度ω经姿态规划器模块得到Rodrigues参数姿态误差ρ_e和角速度误差ω_e，ω_p；再结合提升力F经内环力矩控制计算模块得到力矩控制τ。

根据所得外环力控制设计结果和内环姿态力矩控制设计结果，构建含姿态规划器的四旋翼无人机的位置和姿态控制模型；将所得四旋翼无人机质心坐标、偏航角、俯仰角、滚转角、沿坐标方向的线速度和对应转向角的角速度输入含姿态规划器的四旋翼无人机的位置和姿态控制模型，得到含姿态规划的四旋翼无人机的跟踪控制器，具体包括：

其中，c₁,c₂,c₃,c₄,d₁均为正的设计参数。

控制结果实验展示模块

实验结果展式模块如图7所示，闭环动力学模型计算得到实时位置和实时姿态经3D实验场景和实时轨迹展示模块对控制结果进行虚拟现实展示。

联合所构造的外环控制模块、姿态规划器模块、内环控制模块、四旋翼动力学模型计算模块，以及控制结果实验展示模块得到如图8所示的含姿态规划器的四旋翼无人机的轨迹跟踪控制结构图。

由参考轨迹生成器生成参考轨迹信号，结合由动力学模型计算模块反馈的位置r和平动速度v的实时数据进入外环位置控制模块，经外环位置控制模块计算得到期望的力控制μ_d及其大小提升力F；结合由动力学模型计算模块反馈的姿态q和转动角速度ω的实时数据进入含有姿态规划器的内环姿态控制模块，经计算得到姿态旋转矩阵R和力矩控制τ；联合外环位置控制模块计算得到的期望力控制μ_d和内环姿态控制模块计算得到的姿态旋转矩阵R和力矩控制τ，经动力学模型计算模块对四旋翼无人机的位置和姿态进行实时控制，使其完成对参考轨迹的渐近跟踪；控制结果展示模块可以实时展示四旋翼无人机的位置和姿态效果。

控制器的性能分析

计算所得外环全驱的力控制设计结果和内环姿态的力矩控制设计结果，构建稳定性分析的闭环误差动力学模型；具体公式如下：

计算所述稳定性分析闭环误差动力学模型的Lyapunov函数，得到含姿态规划的四旋翼无人机的跟踪控制模型；将所得四旋翼无人机质心坐标、偏航角、俯仰角、滚转角、沿坐标方向的线速度和对应转向角的角速度输入含姿态规划器的四旋翼无人机的跟踪控制模型，得到含姿态规划的四旋翼无人机的跟踪控制具有如下功能：

功能1：通过调节设计参数，含姿态规划的四旋翼无人机的跟踪控制器使得闭环误差动力学模型是以外部环境噪声(d_p,d_a)为输入，平动速度误差和角速度误差(v_e,ω_z)为输出的严格无源系统。

功能2：当外部环境噪声(d_p,d_a)有界时，通过在含姿态规划的四旋翼无人机的跟踪控制器中引入阻尼项，并适当调节设计参数，能使闭环系统所有信号有界，输出位置信号r可以几乎全局渐近跟踪r_*。当无外部噪声干扰时，闭环系统达到指数稳定，输出位置信号r可以几乎全局实际跟踪r_*。

实验

四旋翼无人机的轨迹跟踪控制实验，针对不同的跟踪任务和不同的干扰情形，采用相同的控制器参数进行实验。

第一种情形：设系统参数m＝0.8kg，J₁＝J₂＝0.005kg/m²，J₃＝0.015kg/m²，重力加速度g＝9.8m/s²。控制器参数为c₁＝5,c₂＝c₃＝c₄＝1，d₁＝1。无人机初始位置和初始速度为：r₀＝(r,0,0)^T,v₀＝(0,0,0)^T，初始姿态和角速度为：q₀＝1,ω₀＝(0,0,0)^T。参考轨迹分为上升阶段、巡航阶段、下降阶段，具体公式为：

x_*＝rcos(k₁t),

y_*＝rsin(k₂t),

其中干扰d_p＝0,d_a＝0，参数k₁＝k₂＝0.2，高度h＝100m，巡航半径为r＝20m，仿真时长T＝20π秒。仿真结果如图9所示

通过观察图9的仿真结果，跟踪误差趋于零，姿态误差趋于零，提升力和扭转力矩是饱和的。飞行路线的三维图见图10，由初始阶段的三维图可见，无人机的体坐标X轴(红色)沿轨线在XY平面中的投影的切线方向。由完整的飞行路径可见，无人机能够精确回到出发点，并让头部指向出发的方向。

图11-图15是四旋翼无人机飞行的离地、上升、巡航、下降、着陆的完整过程。

第二种情形：干扰d_p＝0.1W₁，d_a＝0.1W₂，而W₁和W₂的元素是单位白噪声，其余同第一种情形。仿真结果如图16所示。在存在外部干扰的情况下，控制器依然能够完成实际跟踪任务，与渐近跟踪难以区分。所设计的跟踪控制器具有较强的抗干扰能力。

第三种情形：设系统参数m＝8kg，J₁＝J₂＝0.05kg/m²，J₃＝0.15kg/m²。控制器参数中的d₁＝0.1，干扰d_p＝W₁，d_a＝W₂是第二种情形的10倍，参数k₁＝k₂＝0.04，高度h＝1000m，巡航半径为r＝200m，仿真时长T＝100π秒。其余同第一种情形。仿真结果如图17所示。通过与情形一和情形二比较，系统的质量和转动惯量增加到10倍，飞行高度和半径扩大10倍，飞行时间延长为5倍。这表明在控制方案可以适应不同的跟踪任务，并且在外部干扰变大的情况下依然能达到很好的跟踪效果，因此所述控制方法具有较强的鲁棒性。

综上所述，本发明中的技术方案具有以下有益效果：

在本发明实施例中，通过引入虚拟力控制将四旋翼无人机的欠驱动位置动力学模型化为全驱形式，避免了Brockett的限制条件，从而可以设计得到连续的主推力控制，与现有的混杂控制相比，易于实现，控制精度高，且具有更强的鲁棒性。

首次利用Rodrigues参数设计光滑姿态规划器，将基于四元数描述姿态引起的解卷问题的奇异平衡点±1(一个稳定，另一个不稳定，同时又表示同一姿态)化为基于Rodrigues向量描述的姿态动力学系统的无穷远处，而平衡点为坐标原点，可以与典型的反推方法设计光滑或连续的反馈力矩控制器，从而有效避免了解卷问题和引入混杂控制，且具有更好的机动性。

此外，由于基于Rodrigues向量的姿态系统是由四元数转化而来，利用四元数的归一化减轻了角度漂移的影响，所设计的力矩控制器也可以有效的避免计算冗余问题，提高了实际适用性。

由于位置控制外环设计结果(位置控制器)和姿态控制内环设计结果(姿态控制器)都利用了典型的反推方法，控制器设计易于与自适应观测器相结合，能够解决更复杂的控制问题，以提高四旋翼无人机的控制精度。

实施例二

为了执行上述实施例一对应的方法，以实现相应的功能和技术效果，下面提供了一种含姿态规划器的四旋翼无人机的轨迹跟踪控制系统，包括：

第一数据集获取模块，用于获取四旋翼无人机的质心坐标历史数据集、偏航角历史数据集、俯仰角历史数据集、滚转角历史数据集、沿坐标方向的线速度历史数据集和对应转向角的角速度历史数据集；

第二数据集获取模块，用于实时获取四旋翼无人机质心坐标、偏航角、俯仰角、滚转角、沿坐标方向的线速度和对应转向角的角速度；

基于四元数的四旋翼无人机的动力学模型构建模块，用于对所述四旋翼无人机的质心坐标历史数据集、偏航角历史数据集、俯仰角历史数据集、滚转角历史数据集、沿坐标方向的线速度历史数据集和对应转向角的角速度历史数据集进行运动学计算、四个螺旋桨力学计算和机体力学计算，并计算姿态四元数，得到基于四元数的四旋翼无人机的动力学模型；

相对四元数构造模块，用于设计期望的全驱力控制，并根据期望的全驱力控制，计算体坐标系的主推力方向与惯性坐标系的期望推力方向，构造相对四元数；

光滑姿态规划器构建模块，用于基于所述相对四元数构建光滑姿态规划器；

姿态误差四元数和角速度误差四元数确定模块，用于基于所述光滑姿态规划器确定姿态误差四元数和角速度误差四元数；

四旋翼无人机的姿态误差计算模块，用于基于所述姿态误差四元数和角速度误差四元数利用Rodrigues参数计算四旋翼无人机的姿态误差；

外环设计结果和内环设计结果确定模块，用于基于所述无人机的姿态误差确定位置控制外环设计结果和姿态控制内环设计结果；

四旋翼无人机的位置和姿态控制模型构建模块，用于根据所述位置控制外环设计结果和姿态控制内环设计结果，构建含姿态规划器的四旋翼无人机的位置和姿态控制模型；

跟踪控制结果确定模块，用于将实时获取的四旋翼无人机质心坐标、偏航角、俯仰角、滚转角、沿坐标方向的线速度和对应转向角的角速度输入至所述含姿态规划器的四旋翼无人机的位置和姿态控制模型，得到含姿态规划的四旋翼无人机的跟踪控制结果。

实施例三

本发明实施例三提供一种电子设备，包括存储器及处理器，该存储器用于存储计算机程序，该处理器运行计算机程序以使电子设备执行实施例一提供的含姿态规划的四旋翼无人机轨迹跟踪控制方法。

实施例四

基于实施例三的描述，本发明实施例四提供一种存储介质，其上存储有计算机程序，计算机程序可被处理器执行以实现实施例一的含姿态规划器的四旋翼无人机的轨迹跟踪控制方法。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (10)

1.一种含姿态规划器的四旋翼无人机的轨迹跟踪控制方法，其特征在于，包括：

获取四旋翼无人机的质心坐标历史数据集、偏航角历史数据集、俯仰角历史数据集、滚转角历史数据集、沿坐标方向的线速度历史数据集和对应转向角的角速度历史数据集；

实时获取四旋翼无人机质心坐标、偏航角、俯仰角、滚转角、沿坐标方向的线速度和对应转向角的角速度；

对所述四旋翼无人机的质心坐标历史数据集、偏航角历史数据集、俯仰角历史数据集、滚转角历史数据集、沿坐标方向的线速度历史数据集和对应转向角的角速度历史数据集进行运动学计算、四个螺旋桨力学计算和机体力学计算，并计算姿态四元数，得到基于四元数的四旋翼无人机的动力学模型；

设计期望的全驱力控制，并根据期望的全驱力控制，计算体坐标系的主推力方向与惯性坐标系的期望推力方向，构造相对四元数；

基于所述相对四元数构建光滑姿态规划器；

基于所述光滑姿态规划器确定姿态误差四元数和角速度误差四元数；

基于所述姿态误差四元数和角速度误差四元数利用Rodrigues参数计算四旋翼无人机的姿态误差；

基于所述无人机的姿态误差确定位置控制外环设计结果和姿态控制内环设计结果；

根据所述位置控制外环设计结果和姿态控制内环设计结果，构建含姿态规划器的四旋翼无人机的位置和姿态控制模型；

将实时获取的四旋翼无人机质心坐标、偏航角、俯仰角、滚转角、沿坐标方向的线速度和对应转向角的角速度输入至所述含姿态规划器的四旋翼无人机的位置和姿态控制模型，得到含姿态规划的四旋翼无人机的跟踪控制结果。

2.根据权利要求1所述的含姿态规划器的四旋翼无人机的轨迹跟踪控制方法，其特征在于，所述基于四元数的四旋翼无人机的动力学模型的表达式如下：

其中，r＝(x,y,z)^T和v分别表示四旋翼无人机的位置和速度，q＝(q₁,q₂,q₃,q₄)＝(η,ε^T)表示由欧拉角计算得到的姿态单位四元数，其中和为四元数的标量和矢量分量部分，表示全体实数集，表示质量，g表示重力加速度，和表示计算四个螺旋桨得到的提升力和扭矩矢量，e₃＝(0,0,0)^T为z_b轴上的单位向量，为体惯性矩阵，表示旋转角速度，为角速度四元数，d_p和d_a分别表示平动系统和转动系统受到的不确定干扰，如阻力效应和风湍流，为r的一阶导数，为v的一阶导数，为q的一阶导数，为ω的一阶导数，q^-1为q的逆。

3.根据权利要求1所述的含姿态规划器的四旋翼无人机的轨迹跟踪控制方法，其特征在于，所述期望的全驱力控制的表达式如下：

其中，μ_d表示期望的全驱动力控制，表示质量，表示全体实数集，g表示重力加速度，e₃表示z_b轴上的单位向量，是r_*的二阶导数，r_*表示参考轨迹，c₂,d₁及c₁均表示正的设计参数，r_e表示轨迹跟踪误差，v_e表示平动速度误差。

4.根据权利要求1所述的含姿态规划器的四旋翼无人机的轨迹跟踪控制方法，其特征在于，基于所述相对四元数构建光滑姿态规划器具体包括以下步骤：

基于所述期望的全驱动力控制经四元数开方，结合偏航角四元数q_ψ*进行期望姿态运算得到期望姿态四元数q_d；

实时姿态四元数q与所述期望姿态四元数q_d经姿态误差运算，得到姿态误差四元数q_e和Rodrigues参数姿态误差ρ_e；

所述期望姿态四元数q_d经期望姿态求导运算，结合所述姿态误差四元数q_e和实时旋转角速度ω进行角速度误差计算，得到旋转角速度误差ω_e和期望旋转角速度ω_d的变换数据ω_p；

姿态四元数q经旋转矩阵计算得到旋转矩阵R。

5.根据权利要求1所述的含姿态规划器的四旋翼无人机的轨迹跟踪控制方法，其特征在于，所述姿态控制内环设计结果的表达式如下：

其中，c₄表示正的设计参数，ω_Z表示角速度误差，J表示体惯性矩阵，表示角速度镇定函数的一阶导数，表示基于四元数描述的期望角速度的一阶导数，ω表示角速度，Φ^T表示基于Rodrigues参数的旋转矩阵，ρ_e表示基于Rodrigues参数的姿态误差。

6.一种含姿态规划器的四旋翼无人机的轨迹跟踪控制系统，其特征在于，包括：

第一数据集获取模块，用于获取四旋翼无人机的质心坐标历史数据集、偏航角历史数据集、俯仰角历史数据集、滚转角历史数据集、沿坐标方向的线速度历史数据集和对应转向角的角速度历史数据集；

第二数据集获取模块，用于实时获取四旋翼无人机质心坐标、偏航角、俯仰角、滚转角、沿坐标方向的线速度和对应转向角的角速度；

基于四元数的四旋翼无人机的动力学模型构建模块，用于对所述四旋翼无人机的质心坐标历史数据集、偏航角历史数据集、俯仰角历史数据集、滚转角历史数据集、沿坐标方向的线速度历史数据集和对应转向角的角速度历史数据集进行运动学计算、四个螺旋桨力学计算和机体力学计算，并计算姿态四元数，得到基于四元数的四旋翼无人机的动力学模型；

相对四元数构造模块，用于设计期望的全驱力控制，并根据期望的全驱力控制，计算体坐标系的主推力方向与惯性坐标系的期望推力方向，构造相对四元数；

光滑姿态规划器构建模块，用于基于所述相对四元数构建光滑姿态规划器；

姿态误差四元数和角速度误差四元数确定模块，用于基于所述光滑姿态规划器确定姿态误差四元数和角速度误差四元数；

四旋翼无人机的姿态误差计算模块，用于基于所述姿态误差四元数和角速度误差四元数利用Rodrigues参数计算四旋翼无人机的姿态误差；

外环设计结果和内环设计结果确定模块，用于基于所述无人机的姿态误差确定位置控制外环设计结果和姿态控制内环设计结果；

四旋翼无人机的位置和姿态控制模型构建模块，用于根据所述位置控制外环设计结果和姿态控制内环设计结果，构建含姿态规划器的四旋翼无人机的位置和姿态控制模型；

跟踪控制结果确定模块，用于将实时获取的四旋翼无人机质心坐标、偏航角、俯仰角、滚转角、沿坐标方向的线速度和对应转向角的角速度输入至所述含姿态规划器的四旋翼无人机的位置和姿态控制模型，得到含姿态规划的四旋翼无人机的跟踪控制结果。

7.根据权利要求6所述的含姿态规划器的四旋翼无人机的轨迹跟踪控制系统，其特征在于，

所述基于四元数的四旋翼无人机的动力学模型的表达式如下：

其中，r＝(x,y,z)^T和v分别表示四旋翼的位置和速度，q＝(q₁,q₂,q₃,q₄)＝(η,ε^T)表示由欧拉角计算得到的姿态单位四元数，其中和为四元数的标量和矢量分量部分，表示全体实数集，表示质量，g表示重力加速度，和表示计算四个螺旋桨得到的提升力和扭矩矢量，e₃＝(0,0,0)^T为zb轴上的单位向量，为体惯性矩阵，表示旋转角速度，为角速度四元数，d_p和d_a分别表示平动系统和转动系统受到的不确定干扰，如阻力效应和风湍流，为r的一阶导数，为v的一阶导数，为q的一阶导数，为ω的一阶导数，q^-1为q的逆。

8.根据权利要求6所述的含姿态规划器的四旋翼无人机的轨迹跟踪控制系统，其特征在于，

所述期望的全驱力控制的表达式如下：

其中，μ_d表示期望的全驱动力控制，表示质量，表示全体实数集，g表示重力加速度，e₃表示z_b轴上的单位向量，是r_*的二阶导数，r_*表示参考轨迹，c₂,d₁及c₁均表示正的设计参数，r_e表示轨迹跟踪误差，v_e表示平均速度误差。

9.一种电子设备，其特征在于，包括存储器及处理器，所述存储器用于存储计算机程序，所述处理器运行所述计算机程序以使所述电子设备执行权利要求1-5中任一项所述的含姿态规划器的四旋翼无人机的轨迹跟踪控制方法。

10.一种计算机可读存储介质，其特征在于，其存储有计算机程序，所述计算机程序被处理器执行时实现如权利要求1-5中任一项所述的含姿态规划器的四旋翼无人机的轨迹跟踪控制方法。