CN103092213A - 六旋翼飞行器的轨迹跟踪控制方法 - Google Patents

Abstract

本发明公开了一种六旋翼飞行器的轨迹跟踪控制方法，属于航空飞行器控制领域。该方法首先根据牛顿-欧拉方程，建立飞行器的运动学模型以及飞行器存在复合干扰情况的动力学模型；再利用符号函数积分的二阶滑模复合干扰估计算法，以提供连续的干扰补偿项；然后采用线性和非线性微分器相结合的方法设计快速收敛的非线性微分器；将上述复合干扰估计算法、非线性微分器以及backstepping相结合，设计飞行器姿态***的复合控制器；最后采用PD方法，设计飞行器的位置控制器，解算出跟踪预定轨迹所需的姿态角信息和控制升力。本发明可在复合干扰下，使飞行器仍具备良好的稳定和控制性能，并可精准跟踪预定轨迹。

Description

六旋翼飞行器的轨迹跟踪控制方法

技术领域

本发明涉及一种六旋翼飞行器的轨迹跟踪控制方法，属于航空飞行器控制领域。

背景技术

六旋翼飞行器具有6个对称分布在机体四周、正反转成对的旋翼，其在军事和民事领域均具有广阔的应用前景。

该种飞行器普遍采用电能驱动，具有结构简单、飞行稳定、易于操控、低噪声、无污染、携带方便、安全危害性小等特点，非常适合于执行中短距离的飞行任务。该飞行器与现在广泛研究的四旋翼飞行器相比具有执行机构（电机和旋翼组件）冗余的优势，其中任一执行机构发生故障，可通过容错控制来保持机体姿态稳定，以提高飞行器的飞行可靠性和安全性，从而体现载荷和抗风能力较强的优势。

然而，目前六旋翼飞行器在国内还处于外形结构设计阶段，还未出现针对其控制方法的相关研究报道。

发明内容

本发明的目的在于：提出一种能适用于复合干扰情况下的六旋翼飞行器的轨迹跟踪控制方法。

该方法包括如下步骤：

步骤1：根据牛顿-欧拉方程，建立飞行器的运动学模型以及飞行器存在复合干扰情况的动力学模型；

步骤2：根据步骤1中所述的复合干扰，采用利用符号函数积分的二阶滑模复合干扰估计算法，用以提供连续的干扰补偿项；

步骤3：在引入摄动参数下，采用线性和非线性微分器相结合的方法设计快速收敛的非线性微分器；

步骤4：将步骤2中所述的复合干扰估计算法、步骤3中所述的快速收敛的非线性微分器以及backstepping相结合，设计飞行器姿态***的复合控制器；

步骤5：采用比例微分方法，设计飞行器的位置控制器，解算出跟踪预定轨迹所需的姿态角信息和控制升力。

技术效果：

1、应用本方法，可在六旋翼飞行器动力学模型存在复合干扰情况下，飞行器仍具备稳定的飞行特性、良好的控制性能以及强鲁棒性和精准跟踪预定轨迹的特性。

2、方法中提出的六旋翼飞行器动力学模型中加入了建模动态误差、内部不确定性以及外部干扰，使得动力模型更切合物理实际。

3、方法中提出的利用符号函数积分的二阶滑模干扰估计算法具有设计过程简单、易于工程实现、更进一步抑制抖振现象的优点。

4、方法中提出的快速收敛的非线性微分器不仅通过摄动参数充分减弱因符号函数引起的微分估计抖振现象，而且通过引入线性部分提高了微分器的动态性能。此外，将线性微分器和非线性微分器结合使得所提微分器具有较强的鲁棒性和快速收敛的特点。

5、方法中提出的将快速收敛的非线性微分器用于姿态控制器的设计过程中，可有效避免对虚拟控制量求导过程中的复杂解析计算。

附图说明

图1为本发明方法的控制实现原理图。

图2为六旋翼飞行器的结构及坐标系示意图。

图3为六旋翼飞行器的简化结构图。

图4为例1中x,y,z轴的实际轨迹（实线）与期望轨迹（虚线）曲线图。

图5为例1中x,y,z轴的实际轨迹（实线）与期望轨迹（虚线）三维图。

图6为例1中姿态角响应（实线）与跟踪期望轨迹所需姿态角（虚线）曲线图。

图7为例1中控制升力和控制力矩曲线图。

图8为例1中估计干扰（实线）和实际干扰（虚线）曲线图。

图9为例1中角速率导数估计值（实线）与参考文献方法角速率导数估计值（虚线）的曲线图。

图10为例2中x,y,z轴的实际轨迹（实线）与期望轨迹（虚线）曲线图。

图11为例2中x,y,z轴的实际轨迹（实线）与期望轨迹（虚线）三维图。

图12为例2中姿态角响应（实线）与跟踪期望轨迹所需姿态角（虚线）曲线图。

图13为例2中控制升力和控制力矩曲线图。

图14为例2中估计干扰（实线）和实际干扰（虚线）曲线图。

图15为例2中角速率导数估计值（实线）与参考文献方法角速率导数估计值（虚线）的曲线图。

具体实施方式

本发明六旋翼飞行器的轨迹跟踪控制方法的控制实现原理如图1所示，该方法主要包括如下步骤：

步骤1：根据牛顿-欧拉方程，建立飞行器的运动学模型以及飞行器存在复合干扰（包括建模动态误差、内部不确定性以及外部干扰）情况的动力学模型；

步骤2：根据步骤1中所述的复合干扰，采用一种利用符号函数积分的二阶滑模复合干扰估计算法，用以提供连续的干扰补偿项；

步骤3：在引入摄动参数下，采用线性和非线性微分器相结合的方法设计快速收敛的非线性微分器；

步骤4：将步骤2中所述的复合干扰估计算法、步骤3中所述的快速收敛的非线性微分器以及backstepping相结合，设计飞行器姿态***的复合控制器；

步骤5：采用易于工程实现的比例微分（PD）方法，设计飞行器的位置控制器，解算出跟踪预定轨迹所需的姿态角信息和控制升力。

下面对每个步骤作进一步详细说明：

步骤1中：飞行器的运动学模型和飞行器存在复合干扰情况的动力学模型

六旋翼飞行器的结构以及机体坐标系oxyz和地面坐标系o_gx_gy_gz_g如图2所示。六旋翼飞行器飞行高度较低，速度较慢，可视为刚体，且飞行器机体结构对称并忽略旋翼与飞行器平面之间的高度差距，因此该飞行器的简化结构如图3所示，其中：l₁为OB、OE的长度；l₂为AM、DM、CN、FN的长度；l₃为OM、ON的长度；α为AM与OM之间所夹锐角。

定义飞行器的位置向量为p=(x,y,z)^T，欧拉角为ξ=(φ，θ，ψ)^T，φ为滚转角，θ为俯仰角，ψ为偏航角。机体坐标系到地面坐标系的转换关系可通过坐标转换矩阵R(φ，θ，ψ)表示：

$R=\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}\cos \mathrm{\ψ}\sin \mathrm{\φ}-\sin \mathrm{\ψ}\cos \mathrm{\φ}& \sin \mathrm{\θ}\cos \mathrm{\ψ}\cos \mathrm{\φ}+\sin \mathrm{\ψ}\sin \mathrm{\φ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\θ}\sin \mathrm{\ψ}\sin \mathrm{\φ}+\cos \mathrm{\ψ}\cos \mathrm{\φ}& \sin \mathrm{\θ}\sin \mathrm{\ψ}\cos \mathrm{\φ}-\cos \mathrm{\ψ}\sin \mathrm{\φ}\\ -\sin \mathrm{\θ}& \cos \mathrm{\θ}\sin \mathrm{\φ}& \cos \mathrm{\θ}\cos \mathrm{\φ}\end{array}\right]$

飞行器的空间运动分为两个部分：质心运动和绕质心运动。定义三个质心运动的线速度为v=(v_x,v_y,v_z)^T，三个绕质心运动的角速率为Ω=(p,q,r)^T，p为滚转角速率，q为俯仰角速率，r为偏航角速率。

飞行器在飞行过程中会受到螺旋桨提供的升力（方向沿着机体坐标系z轴垂直向上）以及重力（方向沿着地面坐标系z_g轴垂直向下）的作用。当飞行器的旋翼转动时，旋翼会产生向上的升力和与旋翼转动方向相反的扭矩，它们都与旋翼的转速平方成正比且比例系数均为常数，比例系数可分别表示为b和d。故单个旋翼产生的升力可表示为

反扭矩可表示为

其中i＝1,2,…,6，ω为电机的转速。

因此，六个旋翼产生的总的升力和总的反扭矩为：

$T=\underset{i=1}{\overset{6}{\mathrm{\Σ}}}{f}_i=b\underset{i=1}{\overset{6}{\mathrm{\Σ}}}{\mathrm{\ω}}_i^2---\left(1\right)$

$Q=\underset{i=\mathrm{3,4,5}}{\mathrm{\Σ}}{Q}_i-\underset{i=\mathrm{1,2,6}}{\mathrm{\Σ}}{Q}_i=d(\underset{i=\mathrm{3,4,5}}{\mathrm{\Σ}}{\mathrm{\ω}}_i^2-\underset{i=\mathrm{1,2,6}}{\mathrm{\Σ}}{\mathrm{\ω}}_i^2)---\left(2\right)$

根据牛顿第二定律，飞行器的运动学模型可描述为：

$\stackrel{.}{p}=v$

$\stackrel{.}{v}=-g{z}_e+\frac{T}{m}R{z}_e---\left(3\right)$

式（3）中：p为飞行器的位置向量；v为飞行器运动的线速度；m为飞行器的重量；g为重力加速度；z_e=(0,0,1)^T表示地面坐标系中的单位向量；T为六个旋翼提供的总的升力；R为机体坐标系到地面坐标系的转换矩阵。

六旋翼飞行器的结构对称，故飞行器的惯性矩阵可表示为如下对角阵：

$J=\left[\begin{array}{ccc}{J}_x& 0& 0\\ 0& J_y& 0\\ 0& 0& J_z\end{array}\right]---\left(4\right)$

式（4）中：J_x、J_y、J_z分别为机体x轴、y轴、z轴的转动惯量。

飞行器旋翼旋转时，由于陀螺效应会产生附加力矩（陀螺力矩），该附加力矩可表示为下式：

$G_a=J_r(\mathrm{\Ω}\×z_e)(\underset{i=\mathrm{3,4,5}}{\mathrm{\Σ}}{\mathrm{\ω}}_i-\underset{i=\mathrm{1,2,6}}{\mathrm{\Σ}}{\mathrm{\ω}}_i)---\left(5\right)$

式（5）中：J_r为电机的转动惯量。

根据图2和式（2）可得，机体x轴，y轴，z轴上由旋翼转动产生的对飞行器的控制力矩为：

${\mathrm{\τ}}_a=\left[\begin{array}{c}{\mathrm{\τ}}_{a1}\\ {\mathrm{\τ}}_{a2}\\ {\mathrm{\τ}}_{a3}\end{array}\right]=\left[\begin{array}{c}{\mathrm{bl}}_2\sin \mathrm{\α}({\mathrm{\ω}}_4^2+{\mathrm{\ω}}_6^2-{\mathrm{\ω}}_1^2-{\mathrm{\ω}}_3^2)+{\mathrm{bl}}_1({\mathrm{\ω}}_5^2-{\mathrm{\ω}}_2^2)\\ b(l_2\cos \mathrm{\α}+l_3)({\mathrm{\ω}}_1^2+{\mathrm{\ω}}_4^2-{\mathrm{\ω}}_3^2-{\mathrm{\ω}}_6^2)\\ d({\mathrm{\ω}}_3^2+{\mathrm{\ω}}_4^2+{\mathrm{\ω}}_5^2-{\mathrm{\ω}}_1^2-{\mathrm{\ω}}_2^2-{\mathrm{\ω}}_6^2)\end{array}\right]---\left(6\right)$

根据牛顿-欧拉方程，飞行器的非线性动力学模型则可表示为：

$\stackrel{.}{\mathrm{\ξ}}=\mathrm{W\Ω}---\left(7\right)$

$\stackrel{.}{\mathrm{\Ω}}=J^{-1}(-\mathrm{\Ω}\×\mathrm{J\Ω})-J^{-1}{G}_a+J^{-1}{\mathrm{\τ}}_a+D_{\mathrm{\Ω}}---\left(8\right)$

式（7）、（8）中： $W=\left[\begin{array}{ccc}1& \sin \mathrm{\φ}\tan \mathrm{\θ}& \cos \mathrm{\φ}\tan \mathrm{\θ}\\ 0& \cos \mathrm{\φ}& -\sin \mathrm{\φ}\\ 0& \sin \mathrm{\φ}\sec \mathrm{\θ}& \cos \mathrm{\φ}\sec \mathrm{\θ}\end{array}\right]$ 为绕质心运动的角速率到欧拉角的转换矩阵；J为飞行器的惯性矩阵；D_Ω为角速率回路的复合干扰（飞行器在建模过程中存在建模动态误差、内部不确定性以及在飞行过程中受到的外部干扰）。

步骤2中：复合干扰估计算法

滑模干扰观测器（Sliding Mode Disturbance Observer，SMDO）具有设计简单、易于实现、快速收敛等优点。但目前文献采用SMDO所设计的干扰估计项均含有符号函数，这必会引起干扰估计和补偿的不连续以及抖振现象。鉴于此，本发明提出一种利用符号函数积分估计复合干扰，以提供连续的干扰补偿项的二阶滑模干扰估计算法，该算法的表达形式为：

式（9）中：i＝Ω；s_i=[s_i1 s_i2 s_i3]^T∈R³为辅助滑模向量；x_i和z_i为二阶滑模状态；Γ_i=diag{k_i1,k_i2,k_i3}∈R^3×3为设计的正定矩阵；R为实数集；

sign(s_i)=[sign(s_i1)sign(s_i2)sign(s_i3)]^T；

为干扰估计值；χ_i为设计的第一正常数。

步骤3中：快速收敛的非线性微分器

为避免采用backstepping方法通过繁琐的解析计算求取虚拟控制量，提出一种在引入摄动参数下，采用线性和非线性微分器相结合的方法设计快速收敛的非线性微分器。其中，在保证所提微分器鲁棒性的前提下，将摄动参数引入非线性项中，以减弱微分估计值的抖振，同时，利用具有摄动参数的线性项以加快微分器的收敛速度并抑制高频噪声干扰。

记 $\mathrm{sig}{\left(x\right)}^m={[\mathrm{sig}\left(x_l^m\right),\·\·\·,\mathrm{sig}\left(x_n^m\right)]}^T,$ 其中 $\mathrm{sig}\left(x_l^m\right)={\left|x_i\right|}^m\mathrm{sign}\left(x_i\right),$ m>0，x∈Rⁿ，sign为符号函数。

以下给出快速收敛非线性微分器的设计过程：

针对如下表述形式的***：

$\left\{\begin{array}{c}{\stackrel{.}{x}}_1\left(t\right)=x_2\left(t\right)\\ {\stackrel{.}{x}}_2\left(t\right)=-R_1^1{x}_1\left(t\right)-R_1^2\mathrm{sig}{\left(x_1\right)}^{\mathrm{\ϵ}/(2-\mathrm{\ϵ})}\left(t\right)-R_2^1{x}_2\left(t\right)-R_2^2\mathrm{sig}{\left(x_2\right)}^{\mathrm{\ϵ}}\left(t\right)\end{array}\right.---\left(10\right)$

式（10）中：x₁(t),x₂(t)∈Rⁿ为***状态； $R_1^1=\mathrm{diag}\{r_{11}^1,\·\·\·,r_{1n}^1\},$ $R_1^2=\mathrm{diag}\{r_{11}^2,\·\·\·,r_{1n}^2\},$

以及0<ε<1均为设计的参数。对于任意初始有界的x₁(0)和x₂(0)，式（10）在有限的时间内收敛于原点，即存在时间常数t_f>0，使得x₁(t)≡0，x₂(t)≡0，t≥t_f成立。

针对式（10），选择如下Lyapunov方程：

$V=\frac{1}{2}(x_1^T{R}_1^1{x}_1+x_2^T{x}_2)+\underset{j=1}{\overset{n}{\mathrm{\&Sigma;}}}\frac{r_{1j}^2(2-\mathrm{\&epsiv;})}{2}{\left|x_{1j}\right|}^{2/(2-\mathrm{\&epsiv;})}---\left(11\right)$

对V沿着式（10）求导，可得：

$\stackrel{.}{V}=x_1^T{R}_1^1{x}_2+x_2^T[-R_1^1{x}_1-R_1^2\mathrm{sig}{\left(x_1\right)}^{\mathrm{\&epsiv;}/(2-\mathrm{\&epsiv;})}\left(t\right)-R_2^1{x}_2-R_2^2\mathrm{sig}{\left(x_2\right)}^{\mathrm{\&epsiv;}}]+\underset{j=1}{\overset{n}{\mathrm{\&Sigma;}}}{r}_{1j}^2\mathrm{sig}{\left(x_{1j}\right)}^{\mathrm{\&epsiv;}/(2-\mathrm{\&epsiv;})}{x}_{2j}---\left(12\right)$

$\&le;-R_2^1{x}_2^T{x}_2-\underset{j=1}{\overset{n}{\mathrm{\&Sigma;}}}{r}_{2j}^2{\left|x_{2j}\right|}^{1+\mathrm{\&epsiv;}}<0$

在有界初值x₁(0)和x₂(0)下，***式（10）的解可根据文献[Ibrir,S,Xie W F,Su C Y.Adaptive tracking of nonlinear system with non-symmetric dead-zone input[J].Automatica,2007,43（3）,522-530.]求取。此外，由式（10）的解、式（12）和Lasalle不变原理易知：***式（10）是渐近稳定的。

为了避免backstepping设计姿态控制器过程中通过繁琐的解析计算求取虚拟控制量的微分，此处根据***式（10）和文献[Wang X,Chen Z,Yang,G. Finite-time-convergent differentiatorbased on singular perturbation technique[J].IEEE Transactions on Automatic Control,2007,52（8）,1731-1737.]的定理1提出的快速收敛非线性微分器如下：

$\left\{\begin{array}{c}{\stackrel{.}{z}}_1=z_2\\ {\mathrm{\&kappa;}}^2{\stackrel{.}{z}}_2=-R_1^1(z_1-\mathrm{\&nu;})-R_1^2\mathrm{sig}{(z_1-\mathrm{\&nu;})}^{\mathrm{\&epsiv;}/(2-\mathrm{\&epsiv;})}-R_2^1\mathrm{\&kappa;}{z}_2-R_2^2\mathrm{sig}{\left(\mathrm{\&kappa;}{z}_2\right)}^{\mathrm{\&epsiv;}}\end{array}\right.---\left(13\right)$

式（13）中：z₁,z₂∈Rⁿ为微分器的状态；ν∈Rⁿ为已知向量场，并具有几乎处处连续的二阶导数；κ=diag{κ₁,…,κ_n}>0为设计的摄动参数；0<ε<1。

步骤4中：六旋翼飞行器姿态***的复合控制器（包括姿态角回路控制器和角速率回路控制器）

①针对姿态角回路式（7），设计理想虚拟角速率指令Ω^d。

定义如下姿态角与跟踪设定值之间的误差：

Z_ξ=ξ-ξ_c    （14）

式（14）中：ξ_c为姿态角设定的参考输入。

由式（7）和（14）可得Z_ξ的微分为：

${\stackrel{.}{Z}}_{\mathrm{\&xi;}}=W(Z_{\mathrm{\&Omega;}}+{\mathrm{\&Omega;}}^d)-{\stackrel{.}{\mathrm{\&xi;}}}_c---\left(15\right)$

式（15）中：Z_Ω=Ω-Ω^d为角速率跟踪误差。

选择理想虚拟角速率指令Ω^d（姿态角回路控制器）为：

${\mathrm{\&Omega;}}^d=W^{-1}(-C_{\mathrm{\&xi;}}{Z}_{\mathrm{\&xi;}}+{\stackrel{.}{\mathrm{\&xi;}}}_c)---\left(16\right)$

式（16）中：C_ξ为设计的正定对角阵。

②在角速率回路式（8）存在复合干扰，并考虑理想虚拟角速率指令Ω^d微分不可测时，设计控制力矩τ_a，以实现Ω渐近跟踪Ω^d。

对角速率跟踪误差Z_Ω=Ω-Ω^d求导，并由式（9）可得：

${\stackrel{.}{Z}}_{\mathrm{\&Omega;}}=J^{-1}(-\mathrm{\&Omega;}\&times;\mathrm{J\&Omega;})-J^{-1}{G}_a+J^{-1}{\mathrm{\&tau;}}_a+D_{\mathrm{\&Omega;}}-{\stackrel{.}{\mathrm{\&Omega;}}}^d---\left(17\right)$

为实现Z_Ω稳定，选择控制力矩τ_a（角速率回路控制器）为：

${\mathrm{\&tau;}}_a=J[-C_{\mathrm{\&Omega;}}{Z}_{\mathrm{\&Omega;}}-W^T{Z}_{\mathrm{\&xi;}}-J^{-1}(-\mathrm{\&Omega;}\&times;\mathrm{J\&Omega;})+J^{-1}{G}_a+{\widehat{\stackrel{.}{\mathrm{\&Omega;}}}}^d-{\hat{D}}_{\mathrm{\&Omega;}}+r_{\mathrm{\&Omega;}}]---\left(18\right)$

式（18）中：C_Ω为设定的正定参数矩阵；

为角速率回路未知复合干扰的估计向量；

是以Ω^d为输入的非线性微分器的角速率微分估计矢量；r_Ω是为减小角速率回路干扰误差而设计的鲁棒控制器，结构为：r_Ω=-τ_ΩZ_Ω，τ_Ω为设计的第二正常数。

步骤5中：设计飞行器位置控制器，解算跟踪预定轨迹所需的姿态角信息和控制升力

首先定义六旋翼飞行器的位置误差：P_e=P_c-P，其中P_c=(x_c,y_c,z_c)^T为期望位置指令，P为飞行器的实时飞行位置。

构造如下位置闭环方程：

${\stackrel{..}{P}}_e+K_d{\stackrel{.}{P}}_e+K_p{P}_e=0---\left(19\right)$

式（19）中：K_d、K_p为这里设计的正定矩阵。

根据劳斯-赫尔维茨判据，位置误差P_e指数收敛于0，故式（19）可表示为：

$\stackrel{..}{P}={\stackrel{..}{P}}_c+K_d({\stackrel{.}{P}}_c-\stackrel{.}{P})+K_p(P_c-P)---\left(20\right)$

定义虚拟控制量 $U=\stackrel{..}{P}={(U_1,U_2,U_3)}^T,$ 并代入式（3）可得：

$U=-g{z}_e+\frac{T}{m}R{z}_e---\left(21\right)$

对式（21）进一步整理可得：

$R^T(U+g{z}_e)=\frac{1}{m}T{z}_e---\left(22\right)$

即：

$R^T\left[\begin{array}{c}{U}_1\\ U_2\\ U_3+g\end{array}\right]=\frac{1}{m}\left[\begin{array}{c}0\\ 0\\ T\end{array}\right]$

对其经过简单的数学计算，可得如下等式：

U₁cosθcosψ+U₂cosθsinψ-(U₃+g)sinθ=0    （23）

U₁(sinθcosψsinφ-sinψcosφ)+U₂(sinθsinψsinφ+cosψcosφ)+(U₃+g)cosθsinφ=0

（24）

$U_1(\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}\cos \mathrm{\&phi;}+\sin \mathrm{\&psi;}\sin \mathrm{\&phi;})+U_2(\sin \mathrm{\&theta;}\sin \mathrm{\&psi;}\cos \mathrm{\&phi;}-\cos \mathrm{\&psi;}\sin \mathrm{\&phi;})+(U_3+g)\cos \mathrm{\&theta;}\cos \mathrm{\&phi;}=\frac{1}{m}T---\left(25\right)$

在实际飞行过程中，飞行器俯仰角

故cosθ≠0。

式（23）两边同除以cosθ可得：

$\mathrm{\&theta;}=\arctan \left(\frac{U_1\cos \mathrm{\&psi;}+U_2\sin \mathrm{\&psi;}}{U_3+g}\right)---\left(26\right)$

式（24）×sinφ减去式（25）×cosφ可得：

$U_1\sin \mathrm{\&psi;}-U_2\cos \mathrm{\&psi;}=\frac{1}{m}T\sin \mathrm{\&phi;}---\left(27\right)$

同时，根据式（22）可得：

${(U+g{z}_e)}^T(U+g{z}_e)={\left(\frac{1}{m}T{z}_e\right)}^T\left(\frac{1}{m}T{z}_e\right)$

经整理可得：

式（28）代入式（27）可得：

$\mathrm{\&phi;}=\arcsin \left(\frac{U_1\sin \mathrm{\&psi;}-U_2\cos \mathrm{\&psi;}}{\sqrt{U_1^2+U_2^2+{(U_3+g)}^2}}\right)---\left(29\right)$

故，根据式（26）和式（29）即可计算出飞行器跟踪预定轨迹所需的姿态角信息，根据式（20）可计算出其中的虚拟控制信息。因此，跟踪预定轨迹所需的姿态角闭环形式可表示为：

$\left\{\begin{array}{c}{\mathrm{\&phi;}}_c=\arcsin \left(\frac{U_1\sin {\mathrm{\&psi;}}_c-U_2\cos {\mathrm{\&psi;}}_c}{\sqrt{U_1^2+U_2^2+{(U_3+g)}^2}}\right)\\ {\mathrm{\&theta;}}_c=\arctan \left(\frac{U_1\cos {\mathrm{\&psi;}}_c+U_2\sin {\mathrm{\&psi;}}_c}{U_3+g}\right)\end{array}\right.---\left(30\right)$

除此之外，根据式（25）可得飞行器跟踪预定轨迹的控制升力为：

T=m[u₁(sinθcosψcosφ+sinψsinφ)+u₂(sinθsinψcosφ-cosψsinφ)+(u₃+g)cosθcosφ]

（31）

式（30）、（31）中：U₁、U₂、U₃为PD控制器计算得到的虚拟控制量；ψ_c为期望的偏航角指令。

下面介绍两个实施例：

下面以六旋翼飞行器为对象，在Matlab/Simulink环境下对其进行仿真验证。

六旋翼飞行器数学模型的具体参数如下：m=1.7Kg，l₁=281.5mm，l₂=287.6mm，l₃=40.08mm，α=45°，J_x=0.019Kg.m2，J_y=0.019Kg.m2，J_z=0.031Kg.m²，J_r=3.357×10^-5Kg.m²。

例1：六旋翼执行垂直方向上的矩形飞行模式

假设六旋翼飞行器的跟踪的期望轨迹为：

$\left\{\begin{array}{c}{x}_c=\mathrm{tfsg}(t,\mathrm{0,20})+20\mathrm{fsg}(t,\mathrm{20,40})-60-t)\mathrm{fsg}(t,\mathrm{40,60})\\ y_c=0\\ z_c=5\mathrm{fsg}(t,\mathrm{20,60})\end{array}\right.$

式中： $\mathrm{fsg}=\frac{\mathrm{sign}(x-a)-\mathrm{sign}(x-b)}{2};$ 期望偏航角：ψ_c=0.15rad。

仿真中，飞行器的初始状态选择如下：初始姿态角为[φ₀ θ₀ ψ₀]^T=[0 0 0]^T,rad，角速率为[p₀ q₀ r₀]^T=[0 0 0]^T,rad/s。同时，假设角速率通道在t=0s时加入复合干扰：

D_Ω(t)=[0.5cos(r),0.5sin(t),0.5cos(p)+0.3]

在仿真实验中各参数选择如下：

复合干扰估计算法参数选择：Γ_Ω=diag{1.2,1.0,1.5}（当i＝Ω时，Γ_Ω=Γ_i）；微分器参数选择： $R_1^1={4i}_{3\&times;3},$ $R_1^2=1.5I_{3\&times;3},$ $R_2^1=R_2^2=0.5I_{3\&times;3},$ κ=diag{0.01,0.15,0.4}；姿态角和角速率回路控制器的参数选择：C_ξ=diag{2,2,2}，C_Ω=diag{4,4,4}，τ_Ω=45；位置控制器参数选择：K_d=diag{3,3,3}，K_p=diag{1,1,1}。

例1的仿真结果如图4~9所示。

例2：六旋翼飞行器执行水平方向上的盘旋飞行模式

假设六旋翼飞行器的跟踪的期望轨迹为：

$\left\{\begin{array}{c}{x}_c=5\cos \left(0.5t\right)\\ y_c=6\sin \left(0.5t\right)\\ z_c=5,{\mathrm{\&psi;}}_c=0.15\mathrm{rad}\end{array}\right.$

复合干扰估计算法、微分器以及控制器的参数选择与例1参数的选取相同，例2的仿真结果如图10~15所示。

从仿真结果图4~6以及图10~12可以看出，利用复合干扰估计、补偿以及基于快速收敛非线性微分器的姿态***复合控制器，当复合干扰发生后，飞行器的姿态角能在短时间内达到跟踪位置控制环节解算出的期望姿态角，并且能快速达到对期望轨迹精准跟踪的要求。

从仿真结果图8和图14可以看出，复合干扰估计算法能实现对实际干扰值的准确估计。同时，从仿真结果图9和图15可以看出，本发明所提的快速收敛非线性微分器较参考文献[杨光达,周游.非线性跟踪微分器稳态性能分析及仿真研究[J].光电技术与应用,2010,25（5）,80-82.]中的方法相比具有收敛速度更快，鲁棒性更强的特点。

在本说明书中未作详细描述的内容属本领域技术人员的公知技术。

Claims (6)

1.一种六旋翼飞行器的轨迹跟踪控制方法，其特征在于包括如下步骤：

步骤1：根据牛顿-欧拉方程，建立飞行器的运动学模型以及飞行器存在复合干扰情况的动力学模型；

步骤2：根据步骤1中所述的复合干扰，采用利用符号函数积分的二阶滑模复合干扰估计算法，用以提供连续的干扰补偿项；

步骤3：在引入摄动参数下，采用线性和非线性微分器相结合的方法设计快速收敛的非线性微分器；

步骤4：将步骤2中所述的复合干扰估计算法、步骤3中所述的快速收敛的非线性微分器以及backstepping相结合，设计飞行器姿态***的复合控制器；

步骤5：采用比例微分方法，设计飞行器的位置控制器，解算出跟踪预定轨迹所需的姿态角信息和控制升力。

2.根据权利要求1所述的六旋翼飞行器的轨迹跟踪控制方法，其特征在于：

所述步骤1中的飞行器的运动学模型为：

$\stackrel{.}{p}=v$

$\stackrel{.}{v}=-g{z}_e+\frac{T}{m}R{z}_e$

式中：p为飞行器的位置向量；v为飞行器运动的线速度；m为飞行器的重量；g为重力加速度；z_e=(0,0,1)^T表示地面坐标系中的单位向量；T为六个旋翼提供的总的升力；R为机体坐标系到地面坐标系的转换矩阵；

所述步骤1中的飞行器存在复合干扰情况的动力学模型为：

$\stackrel{.}{\mathrm{\&xi;}}=\mathrm{W\&Omega;}$

$\stackrel{.}{\mathrm{\&Omega;}}=J^{-1}(-\mathrm{\&Omega;}\&times;\mathrm{J\&Omega;})-J^{-1}{G}_a+J^{-1}{\mathrm{\&tau;}}_a+D_{\mathrm{\&Omega;}}$

式中：ξ为飞行器的欧拉角；Ω为飞行器绕质心运动的角速率；W为欧拉角与绕质心运动的角速率之间的转换关系；J为飞行器的惯性矩阵；G_a为飞行器由电机转动而产生的陀螺力矩；τ_a为飞行器的控制力矩；D_Ω为角速率回路的复合干扰。

3.根据权利要求1所述的六旋翼飞行器的轨迹跟踪控制方法，其特征在于：所述步骤2中的复合干扰估计算法为：

式中：i＝Ω；s_i=[s_i1 s_i2 s_i3]^T为辅助滑模向量；Γ_i=diag{k_i1,k_i2,k_i3}为正定矩阵；

sign(s_i)=[sign(s_i1)sign(s_i2)sign(s_i3)]^T；

为干扰估计值；x_i和z_i为二阶滑模状态；χ_i为第一正常数。

4.根据权利要求1所述的六旋翼飞行器的轨迹跟踪控制方法，其特征在于：所述步骤3中的快速收敛的非线性微分器为：

$\left\{\begin{array}{c}{\stackrel{.}{z}}_1=z_2\\ {\mathrm{\&kappa;}}^2{\stackrel{.}{z}}_2=-R_1^1(z_1-\mathrm{\&nu;})-R_1^2\mathrm{sig}{(z_1-\mathrm{\&nu;})}^{\mathrm{\&epsiv;}/(2-\mathrm{\&epsiv;})}-R_2^1\mathrm{\&kappa;}{z}_2-R_2^2\mathrm{sig}{\left(\mathrm{\&kappa;}{z}_2\right)}^{\mathrm{\&epsiv;}}\end{array}\right.$

式中：z₁、z₂为微分器的状态；ν为已知向量场；κ=diag{κ₁,…,κ_n}>0为摄动参数；

0<ε<1均为设计参数。

5.根据权利要求1所述的六旋翼飞行器的轨迹跟踪控制方法，其特征在于：所述步骤4中的飞行器姿态***的复合控制器包括姿态角回路控制器和角速率回路控制器，其中：

1）姿态角回路控制器为：

${\mathrm{\&Omega;}}^d=W^{-1}(-C_{\mathrm{\&xi;}}{Z}_{\mathrm{\&xi;}}+{\stackrel{.}{\mathrm{\&xi;}}}_c)$

式中：C_ξ为正定对角阵；ξ_c为姿态角设定的参考输入；Z_ξ=ξ-ξ_c；

2）角速率回路控制器为：

${\mathrm{\&tau;}}_a=J[-C_{\mathrm{\&Omega;}}{Z}_{\mathrm{\&Omega;}}-W^T{Z}_{\mathrm{\&xi;}}-J^{-1}(-\mathrm{\&Omega;}\&times;\mathrm{J\&Omega;})+J^{-1}{G}_a+{\widehat{\stackrel{.}{\mathrm{\&Omega;}}}}^d-{\stackrel{.}{D}}_{\mathrm{\&Omega;}}+r_{\mathrm{\&Omega;}}]$

式中：C_Ω为正定参数矩阵；

为角速率回路复合干扰的估计向量；

是以Ω^d为输入的非线性微分器的角速率微分估计矢量；r_Ω是用于减小角速率回路干扰误差的鲁棒控制器，r_Ω=-τ_ΩZ_Ω，τ_Ω为第二正常数，Z_Ω=Ω-Ω^d。

6.根据权利要求1所述的六旋翼飞行器的轨迹跟踪控制方法，其特征在于：

所述步骤5中的姿态角信息和控制升力分别为：

$\left\{\begin{array}{c}{\mathrm{\&phi;}}_c=\arcsin \left(\frac{U_1\sin {\mathrm{\&psi;}}_c-U_2\cos {\mathrm{\&psi;}}_c}{\sqrt{U_1^2+U_2^2+{(U_3+g)}^2}}\right)\\ {\mathrm{\&theta;}}_c=\arctan \left(\frac{U_1\cos {\mathrm{\&psi;}}_c+U_2\sin {\mathrm{\&psi;}}_c}{U_3+g}\right)\end{array}\right.$

T=m[U₁(sinθcosψcosφ+sinψsinφ)+U₂(sinθsinψcosφ-cosψsinφ)+(U₃+g)cosθcosφ]

式中：U₁、U₂、U₃为PD控制器计算得到的虚拟控制量；ψ_c为期望的偏航角指令。

Images