基于自适应积分鲁棒的四旋翼轨迹/姿态复合抗干扰跟踪控制方法与流程

本发明涉及导航制导方向的四旋翼轨迹/姿态跟踪领域，具体为一种基于自适应积分鲁棒的四旋翼轨迹/姿态复合抗干扰跟踪控制方法，主要应用于解决考虑质量、三轴转动惯量等参数不确定性和外部干扰并存情况下的四旋翼轨迹/姿态精准跟踪控制问题。

背景技术：

近年来，四旋翼飞行器因操控性方便、扩展性强、运动敏捷性高、可垂直起降等优异性能受到国内外研究学者的广泛关注。相对于固定翼飞机，四旋翼飞行器具有体积小、质量轻、造价低等优点，被广泛应用于侦查、追踪、灾害救援、影像拍摄等场合。飞行控制系统作为四旋翼的“神经中枢”，是保证四旋翼稳定、精准飞行，完成飞行任务的前提。然而，四旋翼自身的运动/动力学模型具有欠驱动、强耦合、非线性的特性，并且四旋翼飞行过程中极易受到外界随机干扰的影响。此外，随着四旋翼飞行任务向多样化、复杂化和精细化方向发展，对闭环系统稳定性、暂态性能、稳态精度以及抗干扰能力提出了更为苛刻的要求。以比例-积分-微分(pid)、线性反馈化为代表的常规控制方法已经难以适用，亟需开展适应四旋翼典型模型特征以及具备强抗扰能力的飞行控制方法研究。

参数不确定性和外部干扰是影响四旋翼飞行器轨迹/姿态控制品质的关键因素。具体而言，受限于缺少精密测量仪器，四旋翼的三轴转动惯量参数难以获取，四旋翼外设的变化往往导致飞行过程中飞行器质量发生变化，上述质量和转动惯量测量难题造成了四旋翼的参数不确定性问题，而外部干扰主要指大气风场引起的未知干扰力矩。若在控制器设计过程中对上述干扰的影响不加以考虑，将会导致四旋翼飞控性能急剧恶化、甚至失稳，难以保证飞行安全。现有用于四旋翼飞行器的抗干扰控制方法仅仅考虑了外部风扰和气动参数不确定性，对于严重影响飞行品质的转动惯量等参数不确定性缺乏重要关注。此外，已有研究大多将外部干扰和参数不确定性视为有界集总扰动，采用自适应估计或者干扰观测器对上述干扰作用进行观测并补偿，最后与滑模或动态面控制器综合，以应对未知干扰的不利影响。不难发现，已有研究中并未对施加的干扰进行分离与分类，仅采用单一的抗干扰机制(如干扰补偿或参数估计)处理未知外界扰动和不确定性，控制效果具有较大保守性，控制性能存在鲁棒性不足的缺陷。因此针对上述控制方法中的多种缺陷，需要提供一种综合考虑并克服多种缺陷的复合抗干扰控制方法。

技术实现要素：

本发明针对四旋翼空间飞行过程中广泛存在的质量、转动惯量等参数不确定性和外部干扰，通过有效复合参数辨识和干扰抑制两种抗干扰措施，提出了一种基于自适应积分鲁棒的四旋翼轨迹/姿态复合抗干扰控制方法。

本发明是通过如下技术方案来实现的：一种基于自适应积分鲁棒的四旋翼轨迹/姿态复合抗干扰跟踪控制方法，包括以下步骤：

(1)建立考虑参数不确定性和外部干扰的四旋翼运动/动力学矢量化模型，四旋翼运动/动力学矢量化模型为：

其中，x1＝[x11,x12,x13]^t、x2＝[x21,x22,x23]^t为四旋翼在惯性坐标系下的位置矢量和速度矢量；m为四旋翼的质量；π1＝diag(kx,ky,kz)∈r^3×3为空气动力阻尼矩阵，ki(i＝x,y,z)为气动阻尼系数；f＝g1u1-g，u1为四旋翼四个螺旋桨产生的升力之和，g1＝[c(ψ)s(θ)c(φ)+s(ψ)s(φ),s(ψ)s(θ)c(φ)-c(ψ)s(φ),c(θ)c(φ)]^t表示和姿态角相关的位置回路输入矩阵，s(·)、c(·)此处表示sin(·)、cos(·)，g＝[0,0,mg]^t；x3＝[x31,x32,x33]^t、x4＝[x41,x42,x43]^t为四旋翼在机体坐标系下的姿态角矢量和姿态角速率矢量；j＝[j1,j2,j3]^t，ji(i＝1,2,3)表示四旋翼在机体坐标系下关于x,y,z轴的转动惯量；π2＝diag(kφ,kθ,kψ)∈r^3×3为空气动力阻尼矩阵，kj(j＝φ,θ,ψ)为气动阻尼系数；g2＝diag(l,l,c)∈r^3×3，其中，l为四旋翼机体中心到螺旋桨轴心的距离，c为力矩因子；τ＝[τ1,τ2,τ3]^t表示四旋翼在机体坐标系下沿x,y,z轴的输入力矩；d2(t)＝[d21(t),d22(t),d23(t)]^t，d4(t)＝[d41(t),d42(t),d43(t)]^t分别为四旋翼位置和姿态回路的外部干扰。

(2)上述矢量化模型分解为可描述三通道位置、速度、姿态、角速率变化的标量模型，将步骤(1)中四旋翼运动/动力学矢量化模型分解为步骤(2)中可描述三通道位置、速度、姿态、角速率变化的标量模型：

其中，x1i(i＝1,2,3)、x2i(i＝1,2,3)分别为x1、x2中的第i个元素；ki为π1中对应的对角线元素，fi(i＝1,2,3)为f＝[f1,f2,f3]^t中的第i个元素；x3i(i＝1,2,3)、x4i(i＝1,2,3)分别为x3、x4中的第i个元素；ji(i＝1,2,3)为j中第i个对角线元素，kj为π2中对应的对角线元素；τi(i＝1,2,3)为τ中的第i个元素，g2i(i＝1,2,3)为g2中的第i个对角线元素；d2i(t)(i＝1,2,3)、d4i(t)(i＝1,2,3)分别为d2(t)、d4(t)中的第i个元素。

(3)根据位置回路的标量模型，构造基于自适应积分鲁棒的四旋翼轨迹复合抗干扰跟踪控制器，为后续姿态控制器设计提供必要的参考指令，其中，采用自适应律对未知质量进行参数估计，通过具有非线性反馈功能的积分鲁棒控制器对外部干扰进行充分抑制，实现对于给定轨迹信号的精确跟踪；

依据步骤(2)中的四旋翼标量模型构造步骤(3)中所述的基于自适应积分鲁棒的四旋翼轨迹复合抗干扰跟踪控制器：

设计四旋翼轨迹跟踪控制器，首先定义如下的位置回路误差变量：

其中，e1＝[e11,e12,e13]^t为四旋翼位置跟踪误差，x1d＝[x11d,x12d,x13d]^t为四旋翼在惯性坐标系下的位置参考指令，e2＝[e21,e22,e23]^t为四旋翼速度跟踪误差，k1、k2为正的反馈增益；r1＝[r11,r12,r13]^t表示为提高控制器的设计自由度所定义的误差；由上述误差变量，可得如下的基于自适应积分鲁棒的四旋翼轨迹复合抗干扰跟踪控制器：

其中，为四旋翼质量的估计值，x1id(i＝1,2,3)为x1d中的第i个元素；kr1、k2、β1为正的反馈增益，sign(e2i)在此处表示符号函数；e2i(i＝1,2,3)为e2中的第i个元素；γ11为自适应律参数，r11为r1中的第1个元素，为估计四旋翼质量的参数自适应律：

上述控制器中，fi分为fia和fis两部分：fia为基于参数自适应的模型前馈补偿项：一方面，通过自适应律对不确定质量进行在线辨识，以消除质量不确定性对闭环系统的影响；另一方面，利用位置回路模型辅助信息并以前馈方式嵌入控制项中，可解决位置回路中的非线性模型补偿问题；fis为积分鲁棒项，通过积分鲁棒线性反馈和非线性反馈，抑制未知的外界扰动，提高系统的鲁棒性；最后将自适应律和积分鲁棒两种控制方法复合，实现四旋翼轨迹精准跟踪控制；

由上述控制器f＝[f1,f2,f3]^t可推得期望姿态角指令，f与期望姿态角指令的关系如下：

由公式(6)逆变换出如下公式：

上式中，x3d＝[φd,θd,ψd]^t为四旋翼期望的姿态角指令。

(4)根据姿态回路的标量模型，结合步骤(3)生成的期望姿态角指令，综合自适应律对三通道转动惯量的精确辨识能力与积分鲁棒对外部干扰的强鲁棒抗干扰能力，构造基于自适应积分鲁棒的四旋翼姿态复合抗干扰跟踪控制器，实现对于给定姿态指令的稳定跟踪：设计四旋翼姿态跟踪控制器，首先定义如下的姿态回路误差变量：

其中，e3＝[e31,e32,e33]^t为四旋翼姿态跟踪误差，e4＝[e41,e42,e43]^t为四旋翼姿态角速率跟踪误差，k3、k4为正的反馈增益；r2＝[r21,r22,r23]^t表示为提高控制器的设计自由度所定义的误差；由上述误差变量，可得如下的基于自适应积分鲁棒的四旋翼姿态复合抗干扰跟踪控制器：

其中，为四旋翼转动惯量ji的估计值,x3id(i＝1,2,3)为x3d中的第i个元素；kr2、k4、β2为正的反馈增益，sign(e4i)在此处表示符号函数，e4i(i＝1,2,3)为e4中的第i个元素；γ2i为自适应律对角矩阵γ2＝diag(γ21,γ22,γ23)中第i个对角线元素；r2i为r2中的第i个元素；为估计四旋翼三轴转动惯量的参数自适应律：

在上述控制器中，τi分为τia和τis两部分：τia为基于参数自适应的模型前馈补偿项：一方面，通过自适应律对三轴转动惯量进行在线估计，以消除三轴转动惯量不确定性对闭环系统的影响；另一方面，利用姿态回路模型辅助信息并以前馈方式嵌入至控制项中，解决姿态回路中的非线性模型补偿问题；τis为积分鲁棒项，通过积分鲁棒线性反馈和非线性反馈，抑制未知的外界扰动，提高系统的鲁棒性；最后将自适应律和积分鲁棒两种控制方法复合，实现四旋翼姿态精准跟踪控制。

本发明主要是针对四旋翼飞行器空间飞行控制过程中广泛存在的未知质量、三轴转动惯量引起的参数不确定性和加性外部干扰，利用一种将自适应与积分鲁棒复合的抗干扰控制方法实现对四旋翼轨迹/姿态的精准跟踪：首先，建立考虑参数不确定性和外部干扰的四旋翼运动/动力学矢量化模型；其次，将矢量化模型分解为可描述三通道位置、速度、姿态、角速率变化的标量模型；然后，根据位置回路的标量模型，构造基于自适应积分鲁棒的四旋翼轨迹复合抗干扰跟踪控制器；最后，根据姿态回路的标量模型以及由位置回路控制器解算生成的期望姿态角指令，构造基于自适应积分鲁棒的四旋翼姿态复合抗干扰跟踪控制器。

与现有技术相比本发明具有以下有益效果：本发明提供的一种基于自适应积分鲁棒的四旋翼轨迹/姿态复合抗干扰控制方法，其创新之处在于利用自适应参数估计将参数不确定性的抗干扰问题转化为参数辨识问题，借助积分鲁棒的非线性鲁棒反馈项提升控制器对于外部干扰的鲁棒性，该复合抗干扰控制器由于充分考虑了干扰的不同特性，可大大降低抗干扰控制器设计的保守性，显著提升四旋翼对于未知干扰和参数不确定性的抗干扰能力，可实现对于给定轨迹/姿态的高精度跟踪控制。

附图说明

图1为自适应积分鲁棒复合抗干扰控制方法的流程图。

图2为自适应积分鲁棒复合抗干扰控制方法作用下四旋翼位置跟踪效果。

图3为自适应积分鲁棒复合抗干扰控制方法作用下四旋翼姿态跟踪效果。

图4为自适应更新律作用下四旋翼质量、三轴转动惯量的在线估计过程。

具体实施方式

以下结合具体实施例和附图对本发明作进一步说明。

一种基于自适应积分鲁棒的四旋翼轨迹/姿态复合抗干扰跟踪控制方法，如图1所示，包括以下步骤：

(1)建立考虑参数不确定性和外部干扰的四旋翼运动/动力学矢量化模型，四旋翼运动/动力学矢量化模型为：

其中，x1＝[x11,x12,x13]^t、x2＝[x21,x22,x23]^t为四旋翼在惯性坐标系下的位置矢量和速度矢量；x1，x2的初始取值为x1＝[0,0,0]^tm，x2＝[0,0,0]^tm/s，m为四旋翼的质量；π1＝diag(kx,ky,kz)∈r^3×3为空气动力阻尼矩阵，ki(i＝x,y,z)为气动阻尼系数；f＝g1u1-g，u1为四旋翼四个螺旋桨产生的升力之和，g1＝[c(ψ)s(θ)c(φ)+s(ψ)s(φ),s(ψ)s(θ)c(φ)-c(ψ)s(φ),c(θ)c(φ)]^t表示和姿态角相关的位置回路输入矩阵，s(·)、c(·)此处表示sin(·)、cos(·)，g＝[0,0,mg]^t；x3＝[x31,x32,x33]^t、x4＝[x41,x42,x43]^t为四旋翼在机体坐标系下的姿态角矢量和姿态角速率矢量，x3，x4的初始取值为x3＝[0,0,0]^tdeg，x4＝[0,0,0]^tdeg/sec；j＝[j1,j2,j3]^t，ji(i＝1,2,3)表示四旋翼在机体坐标系下关于x,y,z轴的转动惯量；π2＝diag(kφ,kθ,kψ)∈r^3×3为空气动力阻尼矩阵，kj(j＝φ,θ,ψ)为气动阻尼系数；g2＝diag(l,l,c)∈r^3×3，其中，l为四旋翼机体中心到螺旋桨轴心的距离，c为力矩因子；τ＝[τ1,τ2,τ3]^t表示四旋翼在机体坐标系下沿x,y,z轴的输入力矩；d2(t)＝[d21(t),d22(t),d23(t)]^t，d4(t)＝[d41(t),d42(t),d43(t)]^t分别为四旋翼位置和姿态回路的外部干扰。

上述四旋翼运动/动力学模型中具体参数和所考虑的外部干扰如下表所示：

表1参数与外部干扰

(2)上述矢量化模型分解为可描述三通道位置、速度、姿态、角速率变化的标量模型，将步骤(1)中四旋翼运动/动力学矢量化模型分解为步骤(2)中可描述三通道位置、速度、姿态、角速率变化的标量模型：

其中，x1i(i＝1,2,3)、x2i(i＝1,2,3)分别为x1、x2中的第i个元素；ki为π1中对应的对角线元素，fi(i＝1,2,3)为f＝[f1,f2,f3]^t中的第i个元素；x3i(i＝1,2,3)、x4i(i＝1,2,3)分别为x3、x4中的第i个元素；ji(i＝1,2,3)为j中第i个对角线元素，kj为π2中对应的对角线元素；τi(i＝1,2,3)为τ中的第i个元素，g2i(i＝1,2,3)为g2中的第i个对角线元素；d2i(t)(i＝1,2,3)、d4i(t)(i＝1,2,3)分别为d2(t)、d4(t)中的第i个元素。

(3)根据位置回路的标量模型，构造基于自适应积分鲁棒的四旋翼轨迹复合抗干扰跟踪控制器，为后续姿态控制器设计提供必要的参考指令，其中，采用自适应律对未知质量进行参数估计，通过具有非线性反馈功能的积分鲁棒控制器对外部干扰进行充分抑制，实现对于给定轨迹信号的精确跟踪；

依据步骤(2)中的四旋翼标量模型构造步骤(3)中所述的基于自适应积分鲁棒的四旋翼轨迹复合抗干扰跟踪控制器：

设计四旋翼轨迹跟踪控制器，首先定义如下的位置回路误差变量：

其中，e1＝[e11,e12,e13]^t为四旋翼位置跟踪误差，x1d＝[x11d,x12d,x13d]^t为四旋翼在惯性坐标系下的位置参考指令，初值取x1d＝[cos(t+2)-cos(2),sin(t+2.4)-sin(2.4),t]^t，e2＝[e21,e22,e23]^t为四旋翼速度跟踪误差，k1、k2为正的反馈增益；r1＝[r11,r12,r13]^t表示为提高控制器的设计自由度所定义的误差；由上述误差变量，可得如下的基于自适应积分鲁棒的四旋翼轨迹复合抗干扰跟踪控制器：

其中，为四旋翼质量的估计值(由于四旋翼质量为标量，我们此处仅需要选取x11d进行质量自适应估计)，x1id(i＝1,2,3)为x1d中的第i个元素；kr1、k2、β1为正的反馈增益，sign(e2i)在此处表示符号函数；e2i(i＝1,2,3)为e2中的第i个元素；γ11为自适应律参数，r11为r1中的第1个元素，为估计四旋翼质量的参数自适应律：

上述控制器中，fi分为fia和fis两部分：fia为基于参数自适应的模型前馈补偿项；fis为积分鲁棒项；最后将自适应律和积分鲁棒两种控制方法复合，实现四旋翼轨迹精准跟踪控制；

由上述控制器f＝[f1,f2,f3]^t可推得期望姿态角指令，f与期望姿态角指令的关系如下：

由公式(6)逆变换出如下公式：

上式中，x3d＝[φd,θd,ψd]^t为四旋翼期望的姿态角指令。

(4)根据姿态回路的标量模型，结合步骤(3)生成的期望姿态角指令，综合自适应律对三通道转动惯量的精确辨识能力与积分鲁棒对外部干扰的强鲁棒抗干扰能力，构造基于自适应积分鲁棒的四旋翼姿态复合抗干扰跟踪控制器，实现对于给定姿态指令的稳定跟踪：设计四旋翼姿态跟踪控制器，首先定义如下的姿态回路误差变量：

其中，e3＝[e31,e32,e33]^t为四旋翼姿态跟踪误差，e4＝[e41,e42,e43]^t为四旋翼姿态角速率跟踪误差，k3、k4为正的反馈增益；r2＝[r21,r22,r23]^t表示为提高控制器的设计自由度所定义的误差；由上述误差变量，可得如下的基于自适应积分鲁棒的四旋翼姿态复合抗干扰跟踪控制器：

其中，为四旋翼转动惯量ji的估计值,x3id(i＝1,2,3)为x3d中的第i个元素；kr2、k4、β2为正的反馈增益，sign(e4i)在此处表示符号函数，e4i(i＝1,2,3)为e4中的第i个元素；γ2i为自适应律对角矩阵γ2＝diag(γ21,γ22,γ23)中第i个对角线元素；r2i为r2中的第i个元素；为估计四旋翼三轴转动惯量的参数自适应律：

在上述控制器中，τi分为τia和τis两部分：τia为基于参数自适应的模型前馈补偿项；τis为积分鲁棒项；最后将自适应律和积分鲁棒两种控制方法复合，实现四旋翼姿态精准跟踪控制。

上述轨迹跟踪控制器和姿态跟踪控制器中的相关参数设计如下：

表2轨迹跟踪控制器和姿态跟踪控制器中的相关参数设计

自适应积分鲁棒复合抗干扰控制方法作用下四旋翼位置跟踪效果如图2所示。由图2可以看出，即使位置回路存在质量不确定性和附加扰动，三轴轨迹跟踪误差依然可以渐近收敛于零域的小范围内，较好实现了对于给定轨迹参考指令的跟踪控制。

自适应积分鲁棒复合抗干扰控制方法作用下四旋翼姿态跟踪效果如图3所示。不难发现，四旋翼姿态输出可以精确地跟踪相应的期望信号，姿态跟踪误差均稳定维持在零域的小偏差范围内。

自适应更新律作用下四旋翼质量、三轴转动惯量的在线估计过程如图4所示。由图4可以看出，质量、转动惯量的估计值可以快速逼近相应的参数真值，确保了所提控制方法对于参数不确定性的良好辨识能力。

综上，本实施例通过复合自适应更新律对参数不确定性的优异辨识能力与积分鲁棒对于外部干扰的强抑制能力，所提供的抗干扰控制方法可以实现参数不确定性和外部干扰并存情况下的四旋翼轨迹/姿态高精度跟踪控制。

本发明要求保护的范围不限于以上具体实施方式，而且对于本领域技术人员而言，本发明可以有多种变形和更改，凡在本发明的构思与原则之内所作的任何修改、改进和等同替换都应包含在本发明的保护范围之内。