尾座式垂直起降无人机在时变侧风干扰下的轨迹跟踪控制
吴林峰, 李春文    
清华大学 自动化系, 北京 100084
摘要:该文针对尾座式垂直起降无人机面临的时变侧风干扰问题, 设计了一种基于新型姿态提取算法的轨迹跟踪控制方法。首先, 根据侧风干扰与无人机姿态的关系建立数学模型, 在此模型基础上推导出一种利用无人机滚转角的新型姿态提取算法, 显著降低了干扰力。在反步式控制方法的基础上, 采用嵌套饱和函数方法对控制输入进行限制, 解决推力限幅问题, 并通过PD+前馈控制方法解决姿态跟踪控制问题。此外, 采用指令滤波器解决了时变侧风干扰造成的期望姿态无法求导的问题。结果表明:只要滤波频率足够大, 轨迹跟踪误差便可达到任意小, 并通过仿真实验验证了其有效性。
关键词尾座式无人机    垂直起降    侧风干扰    
Position tracking control for a tailsitter VTOL UAV experiencing time-varying crosswind disturbances
WU Linfeng, LI Chunwen    
Department of Automation, Tsinghua University, Beijing 100084, China
Abstract: A position tracking control method was designed with an attitude extraction algorithm for a tailsitter vertical take-off and landing (VTOL) unmanned aerial vehicle (UAV) subjected to time-varying crosswind disturbances. A mathematical model was constructed to predict the relationship between the crosswind disturbance and the aircraft's attitude. An attitude extraction algorithm was then developed using the aircraft's roll angle which significantly reduces the effect of the disturbance. A backstepping control method was used with a saturated function technique to account for the thrust boundedness with a PD plus feedforward control method for the attitude tracking control. The unknown derivatives of the desired attitude caused by the time-varying crosswind disturbance were resolved by a command filter. Tests show that the tracking error can be made arbitrarily small as long as the command filter frequency is sufficiently large.
Key words: tailsitter unmanned aerial vehicle    vertical take-off and landing    crosswind disturbances    

近年来,尾座式垂直起降无人机受到了国内外学者们越来越多的关注。相较于传统机型,尾座式垂直起降无人机融合了旋翼机的垂直起降和固定翼机的高速巡航等优点[1]。在起降阶段,尾座式无人机利用主推进器、舵面以及推力矢量系统,保持机头竖直朝上、机尾朝下的姿态,实现与旋翼机相同的垂直起降和悬停效果。经由过渡飞行过程,尾座式垂直起降无人机可以在竖直和水平飞行2种状态间切换。在进行远航程任务时,尾座式无人机可转换为水平飞行模式,达到与固定翼飞机相近的巡航速度。由于不需要加装额外的升力机构,尾座式垂直起降无人机精简了结构设计,减少了冗余死重[2-3]

尽管尾座式垂直起降无人机有着上述优势,但在控制器设计过程面临2个主要难题:1) 垂直起降过程中,无人机所需升力全部由推进器的推力产生,在设计控制器时必须考虑推力限幅的问题[4-6];2) 由于机翼迎风面积较大,很容易在侧风影响下承受较大的干扰力,导致垂直起降无人机位置偏移或姿态失控[7-8]。现有研究工作在考虑推力限幅和侧风干扰问题时,通常会假设侧风干扰力存在一个足够小的上界,使推力在满足限幅条件的情况下可以抵消侧风干扰力的影响。然而对于竖直状态下迎风面较大的的尾座式垂直起降无人机而言,侧风干扰力较大,且飞行器升力全部由发动机推力提供,因而该假设很难成立。此外,现有垂直起降控制器采用的传统姿态提取算法没有考虑飞行器姿态和侧风干扰力之间相互影响的关系。

本文在反步控制方法的基础上[9]提出了一种姿态提取算法,能够在时变侧风干扰下利用风向信息和飞行器滚转自由度大幅减少侧风干扰力,并采用指令滤波器解决侧风干扰造成的期望姿态无法求导的问题;同时通过嵌套饱和函数方法对控制输入加以限制,结合PD+前馈控制方法实现姿态环的跟踪控制[10];最后证明了所提出的控制方法的稳定性,并通过仿真实验验证了控制效果。

1 系统模型 1.1 研究对象

本文所研究的尾座式无人机如图 1所示。机身采用木板结构,尾部装配2台涡喷发动机,通过舵机控制尾喷管方向,可实现矢量推力。尾喷管在俯仰方向的差动运动可以为无人机提供滚转力矩,在俯仰方向和偏航方向的联动运动可以分别为无人机提供俯仰力矩和偏航力矩[3]

图 1 尾座式无人机

在露天环境执行任务时,无人机容易受到侧风干扰而发生位置偏移[11]。特别是当风向、风速随时间产生无规律变化时,无人机的位置也会产生无规律的偏移,严重影响轨迹跟踪的控制效果。为了解决该问题,除了对无人机动力学进行建模外,还要对侧风干扰进行建模。

1.2 无人机动力学模型

本文将尾座式无人机视为刚体。$\mathcal{F}_{\mathrm{i}} \triangleq\left\{\hat{\boldsymbol{e}}_{1}, \hat{\boldsymbol{e}}_{2}, \hat{\boldsymbol{e}}_{3}\right\}$$\mathcal{F}_{\mathrm{b}} \triangleq\left\{\hat{\boldsymbol{e}}_{1 \mathrm{~b}}, \hat{\boldsymbol{e}}_{2 \mathrm{~b}}, \hat{\boldsymbol{e}}_{3 \mathrm{~b}}\right\}$分别表示惯性坐标系和机体坐标系。

p$\mathbb{R}^3$v$\mathbb{R}^3$分别表示惯性坐标系$\mathcal{F}_{i}$下无人机的位置和线速度。无人机的姿态用单位四元数$\boldsymbol{Q} \triangleq\left(\eta, \boldsymbol{q}^{\mathrm{T}}\right)^{\mathrm{T}}$来表示,其中η$\mathbb{R}$为标量部分,q$\mathbb{R}^3$为向量部分,满足单位约束η2+qTq=1。单位四元数的逆表示为Q-1≜(η, -qT)T。2个单位四元数Q1≜(η1, q1T)TQ2≜(η2, q2T)T的乘积定义如下:

$ \boldsymbol{Q}_{1} \odot \boldsymbol{Q}_{2}=\left(\begin{array}{c} \eta_{1} \eta_{2}-\boldsymbol{q}_{1}^{\mathrm{T}} \boldsymbol{q}_{2} \\ \eta_{1} \boldsymbol{q}_{2}+\eta_{2} \boldsymbol{q}_{1}+\boldsymbol{s}\left(\boldsymbol{q}_{1}\right) \boldsymbol{q}_{2} \end{array}\right). $
图 2 惯性坐标系和机体坐标系

其中S(·)代表叉积的矩阵形式,即对任意给定向量v1=(v1, v2, v3)Tv2$\mathbb{R}^3$S(v1)满足:

$ \boldsymbol{S}\left(\boldsymbol{v}_{1}\right)=\left(\begin{array}{ccc} 0 & -v_{3} & v_{2} \\ v_{3} & 0 & -v_{1} \\ -v_{2} & v_{1} & 0 \end{array}\right), $
$ \boldsymbol{S}\left(\boldsymbol{v}_{1}\right) \boldsymbol{v}_{2}=\boldsymbol{v}_{1} \times \boldsymbol{v}_{2}. $

此外,旋转矩阵R(Q)∈SO(3)表示从$\mathcal{F}_{\mathrm{i}}$$\mathcal{F}_{\mathrm{b}}$的映射,满足R(Q)=I3-2η S(q)+2S(q)2I3为3阶单位矩阵。

T$\mathbb{R}$为表示无人机沿机体轴线方向产生的主推力,τ$\mathbb{R}^3$表示机体坐标系$\mathcal{F}_{\mathrm{b}}$下无人机通过双发推力矢量系统产生的力矩。当无人机的质量m和质心到发动机受力点的轴向距离a足够大,满足ma≫1时,可以忽略位置环与姿态环之间的输入耦合[12],此时无人机的动力学方程表示如下:

$ \left\{\begin{array}{l} \dot{p}=\boldsymbol{v}, \\ \dot{\boldsymbol{v}}=g \hat{\boldsymbol{e}}_{3}-\frac{T}{m} \boldsymbol{R}(\boldsymbol{Q})^{\mathrm{T}} \hat{\boldsymbol{e}}_{3}+\boldsymbol{z}, \end{array}\right. $ (1)
$ \left\{\begin{array}{l} \dot{Q}=\frac{1}{2}\left(\begin{array}{c} -\boldsymbol{q}^{\mathrm{T}} \\ \eta \boldsymbol{I}_{3}+\boldsymbol{S}(\boldsymbol{q}) \end{array}\right) \boldsymbol{\omega}, \\ \boldsymbol{I} \boldsymbol{\dot \omega}=\boldsymbol{\tau}-\boldsymbol{S}(\boldsymbol{\omega}) \boldsymbol{I} \boldsymbol{\omega} . \end{array}\right. $ (2)

其中:g为重力加速度,I为机体坐标系$\mathcal{F}_{\mathrm{b}}$下的惯量矩阵,ω为机体坐标系$\mathcal{F}_{\mathrm{b}}$下的角速度,z为惯性坐标系$\mathcal{F}_{\mathrm{i}}$下无人机受到的侧风干扰力。

由式(1)和(2)可以看出,本文所讨论的尾座式垂直起降无人机与多旋翼垂直起降无人机在动力学方程的形式上没有区别,仅在执行机构上存在区别。实际控制器设计中,需要把控制输入Tτ转化为双涡喷发动机的转速指令及矢量喷管的舵机偏转指令,具体转化步骤可参考文[6]。当垂直起降无人机进入水平飞行状态时,其动力学特性与固定翼无人机相同,由于本文着重探讨竖直飞行状态下的控制问题,因而不再赘述。

1.3 侧风干扰模型

设单位向量w$\mathbb{R}^3$表示惯性坐标系$\mathcal{F}_{\mathrm{i}}$下的风向,设Kw$\mathbb{R}$表示风速。显然,无人机受到的侧风干扰力z可沿机体轴分解,即

$ \boldsymbol{z}=K_{1} \hat{\boldsymbol{e}}_{1 \mathrm{b}}+K_{2} \hat{\boldsymbol{e}}_{2 \mathrm{b}}+K_{3} \hat{\boldsymbol{e}}_{3 \mathrm{b}} . $

其中:K1, K2, K3$\mathbb{R}$,代表分量大小。

设无人机处于低速运动状态,有||v||≪Kw。由于机身较为扁平,机翼迎风面积较大,大部分侧风干扰力来自于侧风对机身平面的垂向冲力,且该分量通常远大于沿机体纵轴和横轴的分量,即||K1||≫||K2||+||K3||。将z分为垂向分量ziK1 ê1b和平行分量znK2 ê2b+K3 ê3b 2部分,忽略低速状态下的复杂气动力效应,将无人机机身视为一个垂直于ê1b的平面,则其所受垂向分量干扰力主要来自于侧风对木板平面的垂直冲力,方向与ê1b平行。根据动量定理,无人机受到的垂直分量干扰力幅值K1满足:

$ K_{1}=\rho_{\mathrm{a}} \cdot S_{\mathrm{a}} \cdot K_{\mathrm{w}} \cdot \boldsymbol{w}^{\mathrm{T}} \cdot \boldsymbol{R}(\boldsymbol{Q})^{\mathrm{T}} \hat{\boldsymbol{e}}_{1}. $ (3)

其中:ρa代表空气密度,Sa代表机身平板面积。设Ka=ρa·Sa,则可将式(1)改写为

$ \left\{\begin{array}{l} \dot{\boldsymbol{p}}=\boldsymbol{v}, \\ \dot{\boldsymbol{v}}=g \hat{\boldsymbol{e}}_{3}-\frac{T}{m} \boldsymbol{R}(\boldsymbol{Q})^{\mathrm{T}} \hat{\boldsymbol{e}}_{3}+K_{1} \boldsymbol{R}(\boldsymbol{Q})^{\mathrm{T}} \hat{\boldsymbol{e}}_{1}+\boldsymbol{R}(\boldsymbol{Q})^{\mathrm{T}} \boldsymbol{z}_{\mathrm{n}}, \\ K_{1}=K_{\mathrm{a}} K_{\mathrm{w}} \cdot \boldsymbol{w}^{\mathrm{T}} \cdot \boldsymbol{R}(\boldsymbol{Q})^{\mathrm{T}} \hat{\boldsymbol{e}}_{1}. \end{array}\right. $ (4)

进一步,如果忽略幅值较小且难以估计的平行干扰力分量zn,则可将式(4)简化为

$ \left\{\begin{array}{l} \dot{\boldsymbol{p}}=\boldsymbol{v}, \\ \dot{\boldsymbol{v}}=g \hat{\boldsymbol{e}}_{3}-\frac{T}{m} \boldsymbol{R}(\boldsymbol{Q})^{\mathrm{T}} \hat{\boldsymbol{e}}_{3}+K_{1} \boldsymbol{R}(\boldsymbol{Q})^{\mathrm{T}} \hat{\boldsymbol{e}}_{1}, \\ K_{1}=K_{\mathrm{a}} K_{\mathrm{w}} \cdot \boldsymbol{w}^{\mathrm{T}} \cdot \boldsymbol{R}(\boldsymbol{Q})^{\mathrm{T}} \hat{\boldsymbol{e}}_{1} . \end{array}\right. $ (5)

注意到风速Kw和风向w均为随时间的变化量,虽然其变化规律不可知,但可假设其连续可导,且满足以下条件:

$ \begin{cases}\left|K_{\mathrm{w}}\right| \leqslant \lambda_{K}, & \|\boldsymbol{w}\| \leqslant \lambda_{w} ; \\ \left|\dot{K}_{\mathrm{w}}\right| \leqslant \lambda_{K_{1}}, & \|\dot{\boldsymbol{w}}\| \leqslant \lambda_{w_{1}} ; \\ \left|\ddot{K}_{\mathrm{w}}\right| \leqslant \lambda_{K_{2}}, & \|\ddot{\boldsymbol{w}}\| \leqslant \lambda_{w_{2}} .\end{cases} $ (6)

其中:λK, λw>0分别是Kww的绝对值上界,λK1, λw1>0分别是$\dot{K}_{\mathrm{w}}$$\mathit{\boldsymbol{\dot w}}$的绝对值上界,λK2, λw2>0分别是$\ddot{K}_{w}$$\mathit{\boldsymbol{\ddot w}}$的绝对值上界。该假设符合一般实际情况。由式(3)可以看出,如果能够调整飞机姿态使其满足wT·R(Q)Tê1→0,则有干扰幅值K1→0,即最大限度地减少了侧风对无人机的冲力干扰。

1.4 控制问题描述

在忽略zn的情况下,式(2)和(5)构成了侧风干扰下的尾座式垂直起降无人机动力学模型。轨迹跟踪控制任务的目标是以上述动力学模型为控制对象,以推力T和力矩τ为控制输入,使得无人机能够跟踪期望的运动轨迹pd(t)。定义跟踪误差如下:

$ \overline{\boldsymbol{p}}(t) \triangleq \boldsymbol{p}(t)-\boldsymbol{p}_{\mathrm{d}}(t), \overline{\boldsymbol{v}}(t) \triangleq \boldsymbol{v}(t)-\dot{\boldsymbol{p}}_{\mathrm{d}}(t). $ (7)

注意期望轨迹pd(t)通常可以根据飞行任务进行人为设计,使其在t≥0满足下述条件:

$ \left\|\boldsymbol{p}_{\mathrm{d}}^{(n)}(t)\right\| \leqslant \lambda_{\mathrm{p}}, 0 \leqslant n \leqslant 4 . $ (8)

其中λp>0为期望轨迹及其各阶导数的绝对值上界。定义中间变量F≜(μ1, μ2, μ3)T和误差函数f(Q, Qd)如下:

$ \left\{\begin{array}{l} \boldsymbol{F} \triangleq g \hat{\boldsymbol{e}}_{3}-\frac{T_{\mathrm{d}}}{m} \boldsymbol{R}\left(\boldsymbol{Q}_{\mathrm{d}}\right)^{\mathrm{T}} \hat{\boldsymbol{e}}_{3}, \\ f\left(\boldsymbol{Q}, \boldsymbol{Q}_{\mathrm{d}}\right) \triangleq \frac{T}{m} \boldsymbol{R}(Q)^{\mathrm{T}} \hat{\boldsymbol{e}}_{3}-\frac{T_{\mathrm{d}}}{m} \boldsymbol{R}\left(\boldsymbol{Q}_{\mathrm{d}}\right)^{\mathrm{T}} \hat{\boldsymbol{e}}_{3} . \end{array}\right. $ (9)

其中:Td是期望推力,Qd≜(ηd, qdT)T是期望的姿态四元数。将式(7)、(9)代入式(5),可得位置环跟踪误差动态系统如下:

$ \left\{\begin{array}{l} \dot{\overline{\boldsymbol{p}}}=\overline{\boldsymbol{v}}, \\ \dot{\overline{\boldsymbol{v}}}=\boldsymbol{F}-\ddot{\overline{\boldsymbol{p}}}_{\mathrm{d}}-f\left(\boldsymbol{Q}, \boldsymbol{Q}_{\mathrm{d}}\right)+K_{1} \boldsymbol{R}(\boldsymbol{Q})^{\mathrm{T}} \hat{\boldsymbol{e}}_{1},\\ K_{1}=K_{\mathrm{a}} K_{\mathrm{w}} \cdot \boldsymbol{w}^{\mathrm{T}} \cdot \boldsymbol{R}(\boldsymbol{Q})^{\mathrm{T}} \hat{\boldsymbol{e}}_{1} . \end{array}\right. $ (10)

显然,只要将中间变量F视为上述位置误差跟踪系统的控制输入,将f(Q, Qd)和K1R(Q)T ê1视为干扰项,通过设计控制律确定量F的值后,则可根据式(9)求得期望推力Td,即

$ T_{\mathrm{d}}=m\left\|g \hat{\boldsymbol{e}}_{3}-\boldsymbol{F}\right\|. $ (11)

其中:对于任意xgF需满足条件F≠(0,0, x)T。该条件意味着涡喷发动机对无人机机体的推力方向必须向上,符合垂直起降飞行任务的一般情况。注意在求出Td后可令实际推力T=Td,由于实际推力存在上限,可设λT为最大推力,即|T|≤λT,在设计控制律时应保证|Td|≤λT

期望姿态四元数Qd的值也可根据F的值和式(9)进行提取,但提取算法和结果并不唯一。节2.2会对姿态提取算法展开进一步讨论。

提取出期望姿态Qd后,可将Qd视为姿态环的跟踪目标,定义姿态跟踪误差Q≜(η, qT)T如下:

$ \bar{\boldsymbol{Q}}=\boldsymbol{Q}_{\rm d}^{-1} \odot \boldsymbol{Q}. $ (12)

姿态环跟踪误差动态系统表示为

$ \left\{\begin{array}{l} \dot{\overline{\boldsymbol{Q}}}=\frac{1}{2}\left(\begin{array}{c} -\overline{\boldsymbol{q}}^{\mathrm{T}} \\ \bar{\eta} \boldsymbol{I}_{3}+\boldsymbol{S}(\overline{\boldsymbol{q}}) \end{array}\right) \overline{\boldsymbol{\omega}}, \\ \overline{\boldsymbol{\omega}}=\boldsymbol{\omega}-\boldsymbol{R}(\overline{\boldsymbol{Q}}) \boldsymbol{\omega}_{\mathrm{d}}, \\ \boldsymbol{I} \overline{\boldsymbol{\omega}}=\boldsymbol{\tau}-\boldsymbol{S}(\boldsymbol{\omega}) \boldsymbol{I} \boldsymbol{\omega} . \end{array}\right. $ (13)

其中ωd是期望姿态Qd的期望角速度。显然,如果将τ视为姿态跟踪误差系统的控制输入,设计控制律使得Q→(±1, 0, 0, 0)T成立,则有QQd以及干扰项f(Q, Qd)→0。

综上所述,本文所研究的轨迹跟踪控制问题可以分为3步:1) 针对位置跟踪误差系统式(10),设计以中间变量F为控制输入的控制律,在设计过程中应满足推力限幅条件|Td|≤λT;2) 根据侧风干扰模型(3)和条件约束(9),设计期望姿态四元数Qd的提取算法,尽可能减少侧风干扰项K1R(Q)T ê1的影响;3) 针对姿态跟踪误差系统式(13),设计以力矩τ为控制输入的控制律。

2 控制律设计 2.1 位置环控制设计

为满足|Td|≤λT的推力限幅条件,采用嵌套饱和函数法[13]设计位置环控制律如下:

$ \boldsymbol{F}=\ddot{\boldsymbol{p}}_{\mathrm{d}}-\hat{\boldsymbol{\sigma}}_{2}\left(\overline{\boldsymbol{v}}+\hat{\boldsymbol{\sigma}}_{1}(\overline{\boldsymbol{p}})\right). $ (14)

对于给定的任意向量$\hat{\boldsymbol{x}} \triangleq\left(x_{1}, x_{2}, x_{3}\right)^{\mathrm{T}}$$\hat{\boldsymbol{\sigma}}_{i}(\hat{\boldsymbol{x}})$代表该向量的饱和函数,定义如下:

$ \hat{\boldsymbol{\sigma}}_{i}(\hat{\boldsymbol{x}})=\left(\begin{array}{l} \sigma_{i}\left(x_{1}\right) \\ \sigma_{i}\left(x_{2}\right) \\ \sigma_{i}\left(x_{3}\right) \end{array}\right), i=1,2 . $ (15)

对于任意给定标量x$\mathbb{R}$σi(x)表示该标量x的单调非减可导饱和函数,具体定义如下:

$ \begin{cases}x \sigma_{i}(x) \geqslant 0, & \forall x ; \\ \sigma_{i}(x)=k_{i} x, & \forall x \leqslant l_{i} ; \\ \sigma_{i}(x)=M_{i}(\operatorname{sign}(x)), & \forall x \geqslant L_{i} ; \\ 0 \leqslant \frac{\partial \sigma_{i}(x)}{\partial x} \leqslant k_{i}, & \forall x ; \\ \left|\frac{\partial^{2} \sigma_{i}(x)}{\partial^{2} x}\right| \leqslant \frac{M_{i}-k_{i} l_{i}}{L_{i}-l_{i}}, & \forall x .\end{cases} $ (16)

其中:kiliMiLi是正数,满足Mi>kiliLi>li。对于控制律式(14)中的饱和函数$\hat{\boldsymbol{\sigma}}_{1}(\hat{\boldsymbol{x}})$$\hat{\boldsymbol{\sigma}}_{2}(\hat{\boldsymbol{x}})$,选择参数k1k2l1l2M1M2满足下列条件:

$ \left\{\begin{array}{l} k_{1} l_{1}>l_{2}, \\ k_{2} l_{2}>\frac{\lambda_{\mathrm{T}}}{m} \gamma_{\mathrm{T}}+k_{1}\left(l_{2}+M_{1}\right), \\ \gamma_{\mathrm{T}}<\frac{k_{2} l_{2}-k_{1}\left(l_{2}+M_{1}\right)}{g+\lambda_{\mathrm{p}}+M_{2}} . \end{array}\right. $ (17)

其中γT是正常数,满足γT < 1。式(17)可推导出

$ \lambda_{\mathrm{T}} \geqslant m\left(g+\lambda_{\mathrm{p}}+M_{2}\right). $ (18)

结合式(11)和式(14)可得|Td|≤λT

直观地说,条件式(17)意味着最大推力足够满足控制律式(14)的需求。由于期望轨迹pd(t)、饱和函数$\hat{\boldsymbol{\sigma}}_{1}(\hat{\boldsymbol{x}})$$\hat{\boldsymbol{\sigma}}_{2}(\hat{\boldsymbol{x}})$都可以按照控制需求调整设计,因此选择满足条件式(17)的参数是可行的。

2.2 姿态提取算法

文[12]提出了一种满足式(9)约束的姿态提取算法,设该算法所求得的期望姿态为Qt≜(ηt, qtT)T,在已知F≜(μ1, μ2, μ3)TTd的情况下,qt表示如下:

$ \eta_{\mathrm{t}}=\sqrt{\frac{1}{2}+\frac{m\left(g-\mu_{3}\right)}{2 T_{\mathrm{d}}}}, \boldsymbol{q}_{\mathrm{t}}=\frac{m}{2 T_{\mathrm{d}} \eta_{\mathrm{t}}}\left(\begin{array}{c} \mu_{2} \\ -\mu_{1} \\ 0 \end{array}\right) . $ (19)

通过计算可验证下式成立:

$ \boldsymbol{F}=g \hat{\boldsymbol{e}}_{3}-\frac{T_{\mathrm{d}}}{m} \boldsymbol{R}\left(\boldsymbol{Q}_{\mathrm{t}}\right)^{\mathrm{T}} \hat{\boldsymbol{e}}_{3} $

上述姿态提取算法计算简便,可求出具有最小旋转角的旋转Qt,但并没有充分利用无人机的滚转自由度。对于本文所研究的侧风干扰问题,无人机的滚转姿态与侧风干扰量的大小直接相关,因此在设计期望姿态的提取算法时,优先考虑的不是最小旋转,而是如何显著减少侧风干扰项K1R(Q)T ê1。因此,改进的姿态提取算法的核心在于利用滚转自由度使侧风干扰幅值K1→0,具体计算步骤如下。

根据传统姿态提取算法式(19)计算具有最小旋转角的期望姿态Qt,进而求得期望的推力方向Dt$\mathbb{R}^3$ 如下:

$ \boldsymbol{D}_{\mathrm{t}}=\boldsymbol{R}\left(\boldsymbol{Q}_{\mathrm{t}}\right)^{\mathrm{T}} \hat{\boldsymbol{e}}_{3} . $ (20)

DmR(Qt)T ê1表示无人机在传统期望姿态Qt下的机身平面法向量,DnR(Qd)T ê1代表无人机在改进后的期望姿态Qd下的机身平面法向量。显然,当Dn垂直于风向w和期望推力方向Dt所在的平面时,有wTR(Qd)T ê1=0成立,即当QQd时,干扰量幅值K1→0,因此Dn可通过下式计算:

$ \boldsymbol{D}_{\mathrm{n}}=\boldsymbol{w} \times \boldsymbol{D}_{\mathrm{t}}. $ (21)

设四元数Qr≜(ηr, qrT)T表示从DmDn的旋转,即有

$ \boldsymbol{D}_{\mathrm{n}}=\boldsymbol{R}\left(\boldsymbol{Q}_{\mathrm{r}}\right)^{\mathrm{T}} \boldsymbol{D}_{\mathrm{m}}. $ (22)

Qr的一个解为

$ \eta_{\mathrm{r}}=\sqrt{\frac{1+\boldsymbol{D}_{\mathrm{m}}^{\mathrm{T}} \boldsymbol{D}_{\mathrm{n}}}{2}}, \boldsymbol{q}_{\mathrm{r}}=\frac{1}{2 \eta_{\mathrm{r}}} \boldsymbol{D}_{\mathrm{m}} \times \boldsymbol{D}_{\mathrm{n}}. $ (23)

注意,式(23)代表的旋转轴同时垂直于DmDn,即Qr是以最小旋转角进行的滚转运动[14]。最后将2个旋转QtQr进行叠加,即可得到改进后的期望姿态Qd

$ \boldsymbol{Q}_{\mathrm{d}}=\boldsymbol{Q}_{\mathrm{r}} \odot \boldsymbol{Q}_{\mathrm{t}}. $ (24)

通过计算可验证下式成立:

$ \boldsymbol{w}^{\mathrm{T}} \boldsymbol{R}\left(\boldsymbol{Q}_{\mathrm{d}}\right)^{\mathrm{T}} \hat{\boldsymbol{e}}_{1}=0, $
$ \boldsymbol{F}=g \hat{\boldsymbol{e}}_{3}-\frac{T_{\mathrm{d}}}{m} \boldsymbol{R}\left(\boldsymbol{Q}_{\mathrm{d}}\right)^{\mathrm{T}} \hat{\boldsymbol{e}}_{3} . $

可以看出,改进后的姿态提取算法不仅满足式(9)约束,同时还利用滚转自由度消除了侧风干扰项式(3)的影响。

在求得期望姿态Qd后,期望姿态角速度ωd和角加速度${\boldsymbol{\dot \omega }_{\rm{d}}}$ 可通过下式计算:

$ \left\{\begin{array}{l} \boldsymbol{\omega}_{\mathrm{d}}=\boldsymbol{\varphi}\left(\boldsymbol{\theta}_{\mathrm{d}}\right) \dot{\boldsymbol{\theta}}_{\mathrm{d}}, \\ \dot{\boldsymbol{\omega}}_{\mathrm{d}}=\dot{\boldsymbol{\varphi}}\left(\boldsymbol{\theta}_{\mathrm{d}}\right) \dot{\boldsymbol{\theta}}_{\mathrm{d}}+\boldsymbol{\varphi}\left(\boldsymbol{\theta}_{\mathrm{d}}\right) \ddot{\boldsymbol{\theta}}_{\mathrm{d}} . \end{array}\right. $ (25)

其中:θd≜(θ1d, θ2d, θ3d)T代表期望旋转矩阵R(Qd)以1→2→3顺序旋转,旋转角依次为θ1dθ2dθ3d的Euler角向量,具体计算方式如下:

$ \boldsymbol{\theta}_{\mathrm{d}} \triangleq\left(\begin{array}{l} \theta_{1 \mathrm{d}} \\ \theta_{2 \mathrm{d}} \\ \theta_{3 \mathrm{d}} \end{array}\right)=\left(\begin{array}{c} \arctan \left(\mu_{2} /\left(\mu_{3}-g\right)\right) \\ \arcsin \left(\mu_{1} m / T_{\mathrm{d}}\right) \\ \arccos \left(\boldsymbol{D}_{\mathrm{m}}^{\mathrm{T}} \boldsymbol{D}_{\mathrm{n}}\right) \end{array}\right). $ (26)

其中φ(θd)的定义如下:

$ \boldsymbol{\varphi}\left(\boldsymbol{\theta}_{\mathrm{d}}\right) \triangleq\left(\begin{array}{ccc} \cos \left(\theta_{2 \mathrm{d}}\right) \cos \left(\theta_{3 \mathrm{d}}\right) & -\sin \left(\theta_{3 \mathrm{d}}\right) & 0 \\ \cos \left(\theta_{2 \mathrm{d}}\right) \sin \left(\theta_{3 \mathrm{d}}\right) & \cos \left(\theta_{3 \mathrm{d}}\right) & 0 \\ -\sin \left(\theta_{2 \mathrm{d}}\right) & 0 & 1 \end{array}\right) . $ (27)

注意,尽管θd可以根据式(26)计算得出,但其导数$\dot{\boldsymbol{\theta}}_{\mathrm{d}}$和二阶导数$\ddot{\boldsymbol{\theta}}_{\mathrm{d}}$无法直接计算,因为在求导过程中需要风向的导数$\mathit{\boldsymbol{\dot w}}$$\mathit{\boldsymbol{\ddot w}}$,而风向的变化往往在实际应用中是难以获知的。因此,需要设计指令滤波来对难以直接求取的导数进行估计。

2.3 指令滤波器

设计指令滤波器[15]如下:

$ \dot{\boldsymbol{c}}_{1}=\boldsymbol{c}_{2}, \dot{\boldsymbol{c}}_{2}=-2 \xi \omega_{\mathrm{n}} \boldsymbol{c}_{2}-\omega_{\mathrm{n}}^{2}\left(\boldsymbol{c}_{1}-\boldsymbol{\theta}_{\mathrm{d}}\right). $ (28)

其中:(c1, c2)∈$\mathbb{R}^3$×$\mathbb{R}^3$为滤波器系统状态,θd作为滤波器输入,ξ$\mathbb{R}$ωn$\mathbb{R}$分别为滤波器的阻尼比和频率。滤波器初始状态设置为c1(0)=θd(0)以及c2(0)=0。滤波器的输出定义为

$ \boldsymbol{\theta}_{\mathrm{c}} \triangleq \boldsymbol{c}_{1}, \dot{\boldsymbol{\theta}}_{\mathrm{c}} \triangleq \boldsymbol{c}_{2}, \ddot{\boldsymbol{\theta}}_{\mathrm{c}} \triangleq-2 \xi \omega_{\mathrm{n}} \boldsymbol{c}_{2}-\omega_{\mathrm{n}}^{2}\left(\boldsymbol{c}_{1}-\boldsymbol{\theta}_{\mathrm{d}}\right). $ (29)

在姿态跟踪控制器设计中,不再将期望Euler角θd和期望姿态四元数Qd作为直接跟踪目标,而是以指令滤波器输出的Euler角θc≜(θ1c, θ2c, θ3c)T及其对应的姿态四元数Qc≜(ηc, qcT)T作为跟踪目标。由于本文坐标系定义的特殊性,由Euler角θc到旋转矩阵R(θc)的变换公式为

$ \boldsymbol{R}\left(\boldsymbol{\theta}_{\mathrm{c}}\right) \triangleq\left[\begin{array}{lll} R_{11} & R_{12} & R_{13} \\ R_{21} & R_{22} & R_{23} \\ R_{31} & R_{32} & R_{33} \end{array}\right]=\boldsymbol{R}_{3} \boldsymbol{R}_{2} \boldsymbol{R}_{1} \boldsymbol{R}_{\mathrm{vh}}. $ (30)

R1R2R3Rvh定义如下:

$ \boldsymbol{R}_{1}=\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & \cos \left(\theta_{1 \mathrm{c}}\right) & \sin \left(\theta_{1 \mathrm{c}}\right) \\ 0 & -\sin \left(\theta_{1 \mathrm{c}}\right) & \cos \left(\theta_{1 \mathrm{c}}\right) \end{array}\right], $
$ \boldsymbol{R}_{2}=\left[\begin{array}{ccc} \cos \left(\theta_{2 \mathrm{c}}\right) & 0 & -\sin \left(\theta_{2 \mathrm{c}}\right) \\ 0 & 1 & 0 \\ \sin \left(\theta_{2 \mathrm{c}}\right) & 0 & \cos \left(\theta_{1 \mathrm{c}}\right) \end{array}\right], $
$ \boldsymbol{R}_{3}=\left[\begin{array}{ccc} \cos \left(\theta_{3 \mathrm{c}}\right) & \sin \left(\theta_{3 \mathrm{c}}\right) & 0 \\ -\sin \left(\theta_{3 \mathrm{c}}\right) & \cos \left(\theta_{3 \mathrm{c}}\right) & 0 \\ 0 & 0 & 1 \end{array}\right], $
$ \boldsymbol{R}_{\mathrm{vh}}=\left[\begin{array}{ccc} 0 & 0 & -1 \\ 0 & 1 & 0 \\ 1 & 0 & 0 \end{array}\right]. $

得到旋转矩阵R(θc)后,可以按照文[17]中旋转矩阵到四元数的变换公式求取Qc,这里不再赘述。

2.4 姿态环控制设计

在得到θc$\dot{\boldsymbol{\theta}}_{\mathrm{c}}、\ddot{\boldsymbol{\theta}}_{\mathrm{c}}$后,可求出指令滤波器输出的角速度ωc和角加速度$\dot{\boldsymbol{\omega}}_{\mathrm{c}}$如下:

$ \left\{\begin{array}{l} \boldsymbol{\omega}_{\mathrm{c}}=\boldsymbol{\varphi}\left(\boldsymbol{\theta}_{\mathrm{c}}\right) \dot{\boldsymbol{\theta}}_{\mathrm{c}}, \\ \dot{\boldsymbol{\omega}}_{\mathrm{c}}=\ddot{\boldsymbol{\varphi}}\left(\boldsymbol{\theta}_{\mathrm{c}}\right) \dot{\boldsymbol{\theta}}_{\mathrm{c}}+\boldsymbol{\varphi}\left(\boldsymbol{\theta}_{\mathrm{c}}\right) \ddot{\boldsymbol{\theta}}_{\mathrm{c}} . \end{array}\right. $ (31)

Qcωc为跟踪目标,姿态跟踪误差重写为

$ \bar{\boldsymbol{Q}}=\boldsymbol{Q}_{\mathrm{c}}^{-1} \odot \boldsymbol{Q}. $ (32)

姿态跟踪误差动态系统重写为

$ \left\{\begin{array}{l} \dot{\bar{\boldsymbol{Q}}}=\frac{1}{2}\left(\begin{array}{c} -\overline{\boldsymbol{q}}^{\mathrm{T}} \\ \bar{\eta} \boldsymbol{I}_{3}+\boldsymbol{S}(\overline{\boldsymbol{q}}) \end{array}\right) \overline{\boldsymbol{\omega}}, \\ \overline{\boldsymbol{\omega}}=\boldsymbol{\omega}-\boldsymbol{R}(\overline{\boldsymbol{Q}}) \boldsymbol{\omega}_{\mathrm{c}}, \\ \boldsymbol{I} \boldsymbol{\dot \omega}=\boldsymbol{\tau}-\boldsymbol{S}(\boldsymbol{\omega}) \boldsymbol{I} \boldsymbol{\omega} . \end{array}\right. $ (33)

针对姿态跟踪误差动态系统式(33),设计PD+ 前馈控制律[10]如下:

$ \begin{gathered} \boldsymbol{\tau}=-k_{\mathrm{p}} \bar{q}-k_{\mathrm{v}} \overline{\boldsymbol{\omega}}+\boldsymbol{R}(\overline{\boldsymbol{Q}}) \boldsymbol{\omega}_{\mathrm{c}} \times \\ \boldsymbol{\boldsymbol { I R }}(\overline{\boldsymbol{Q}}) \boldsymbol{\omega}_{\mathrm{c}}+\boldsymbol{I R}(\overline{\boldsymbol{Q}}) \dot{\boldsymbol{\omega}}_{\mathrm{c}} . \end{gathered} $ (34)

其中kp$\mathbb{R}^+$kv$\mathbb{R}^+$为控制器参数。

3 稳定性分析

本节将证明垂直起降飞行器的位置和速度跟踪误差最终有界,且只要选取的指令滤波器的频率ωn足够大,该上界可以达到任意小。

首先,对于位置环误差跟踪系统有如下引理:

引理1  考虑下述以($\tilde{x}_{1}$, $\tilde{x}_{2}$)∈$\mathbb{R}$×$\mathbb{R}$为状态量,以d(t): $\mathbb{R}^+$$\mathbb{R}$为干扰项的动态系统,

$ \left\{\begin{array}{l} \dot{\widetilde{x}}_{1}=\widetilde{x}_{2}, \\ \dot{\widetilde{x}}_{2}=u+d(t) . \end{array}\right. $ (35)

以及控制律

$ u \triangleq-\sigma_{2}\left(\widetilde{x}_{2}+\sigma_{1}\left(\widetilde{x}_{1}\right)\right). $ (36)

其中σ1σ2为式(16)所定义的饱和函数。如果参数满足下述条件:

$ \left\{\begin{array}{l} k_{1} l_{1}>l_{2}, \\ k_{2} l_{2}>\lambda_{\mathrm{d}}+k_{1}\left(l_{1}+M_{1}\right) . \end{array}\right. $ (37)

其中λd为|d(t)|的上界,则状态量($\tilde{x}_{1}$, $\tilde{x}_{2}$)最终有界于O(λd)。

证明:显然,如果所有状态在有限时间内进入了饱和函数的线性非饱和区域,则系统式(16)变为

$ \begin{gathered} {\left[\begin{array}{c} \dot{\tilde{x}}_{1} \\ \dot{\widetilde{x}}_{2} \end{array}\right]=\boldsymbol{A}\left[\begin{array}{c} \tilde{x}_{1} \\ \tilde{x}_{2} \end{array}\right]+\left[\begin{array}{c} 0 \\ d(t) \end{array}\right],} \\ \boldsymbol{A}=\left[\begin{array}{cc} 0 & 1 \\ -k_{1} k_{2} & -k_{2} \end{array}\right] . \end{gathered} $ (38)

由于A是Hurwitz矩阵,设初始时间为t0,总是存在正常数ab满足

$ \|\widetilde{x}(t)\| \leqslant a \mathrm{e}^{-b\left(t-t_{0}\right)}\left\|\widetilde{x}\left(t_{0}\right)\right\|+\frac{a}{b} \lambda_{\mathrm{d}} . $ (39)

上式说明了系统对d(t)满足输入-状态稳定。因此,下面只需证明所有状态在有限时间内会进入线性非饱和区域。

y=$\tilde{x}_{2}$+σ1($\tilde{x}_{1}$),首先证明y(t)在有限时间内进入区域Ω2≜{y: |y|≤l2}。设初始时间为t0,不失一般性,假设y(t0)≥l2. 如果y(t)在有限时间内没有进入Ω2,由式(36)和(37)可得

$ \begin{gathered} \dot{\widetilde{x}}_{2}=-\sigma_{2}(y)+d(t) \leqslant-k_{2} l_{2}+d(t) \\ \quad \Rightarrow \widetilde{x}_{2}(t) \leqslant \widetilde{x}_{2}\left(t_{0}\right)-\left(k_{2} l_{2}-\lambda_{\mathrm{d}}\right) t \\ \Rightarrow y(t) \leqslant y\left(t_{0}\right)+2 M_{1}-\left(k_{2} l_{2}-\lambda_{\mathrm{d}}\right) t . \end{gathered} $ (40)

由上式可以看出,当t→∞时,y(t)→-∞,与假设不符。因此y(t)可在有限时间内进入Ω2,设进入时间为t1,则t1的上界为

$ t_{1} \leqslant t_{0}+\frac{y\left(t_{0}\right)+2 M_{1}-l_{2}}{k_{2} l_{2}-\lambda_{\mathrm{d}}}. $ (41)

接着只需证明在Ω2的边界上,y(t)(t) < 0。在|y(t)|=l2的条件下,由式(36)和(37)可得

$ \begin{gathered} y \dot{y}=y\left(-\sigma_{2}(y)+d(t)+\frac{\partial \sigma_{1}\left(\tilde{x}_{1}\right)}{\partial \widetilde{x}_{1}} \dot{\widetilde{x}}_{1}\right)= \\ y\left(-k_{2} l_{2} \operatorname{sign}(y)+d(t)+\frac{\partial \sigma_{1}\left(\widetilde{x}_{1}\right)}{\partial \widetilde{x}_{1}}\left(y-\sigma_{1}\left(\tilde{x}_{1}\right)\right)\right) \leqslant \\ |y|\left(-k_{2} l_{2}+\lambda_{\mathrm{d}}+k_{1}\left(l_{2}+M_{1}\right)\right)<0 . \end{gathered} $ (42)

上式证明了Ω2为正不变域。

同样的方法可用于证明$\tilde{x}_{1}$(t)在有限时间内进入区域Ω1≜{y|y|≤l1},这里不再赘述。

引理1简化了下述定理的证明:

定理1  考虑以F为控制输入,以式(14)为控制律的闭环误差系统式(10),且相关参数满足条件式(17)。假设存在ε>0和tf>0,当t>tf时,对于所有Q(t)和Q(tf),均有||-f(Q, Qd)+K1R(Q)T ê1||≤ε成立,且满足下述条件:

$ \varepsilon<\frac{\lambda_{\mathrm{T}}}{m} \gamma_{\mathrm{T}}. $ (43)

则系统式(10)的状态(p, v)∈$\mathbb{R}^3$×$\mathbb{R}^3$最终有界于O(ε)。

证明:注意系统式(35)及其控制律式(36)正是系统(10)及其控制律(14)的标量形式。当t>tf时,将-f(Q, Qd)+K1R(Q)T ê1视作干扰项d(t),即有λd≜ε,再结合式(17)和(43)可得条件(37)成立。综上,由引理1可得,系统状态(p, v)∈$\mathbb{R}^3$×$\mathbb{R}^3$最终有界于O(ε)。

定理1说明,如果干扰项-f(Q, Qd)+K1·R(Q)T ê1在有限时间内有界于一个足够小的正常数ε,则轨迹跟踪误差最终有界。为了证明干扰项的有界性,首先介绍如下引理:

引理2  对于闭环系统式(9)-(14)及姿态提取算法式(26),总是存在独立于滤波频率ωn的正常数ρ1和单调增正函数ρ2(·),满足

$ \left\|\dot{\theta}_{\rm d}\right\| \leqslant \rho_{1},\left\|\ddot{\theta}_{\rm d}\right\| \leqslant \rho_{2}(\|\boldsymbol{\omega}\|) . $ (44)

证明:首先证明中间变量F及其导数的有界性,由位置环控制律式(14)可得F有界,对其求导可得

$ \dot{\boldsymbol{F}}=\boldsymbol{p}_{\mathrm{d}}^{(3)}-\dot{\hat{\boldsymbol{\sigma}}}_{2}\left(\overline{\boldsymbol{v}}+\hat{\boldsymbol{\sigma}}_{1}(\overline{\boldsymbol{p}})\right) \cdot\left(\dot{\overline{\boldsymbol{p}}}+\dot{\hat{\boldsymbol{\sigma}}}_{1}(\overline{\boldsymbol{p}}) \dot{\overline{\boldsymbol{p}}}\right) . $ (45)

因为pd(3)$\dot{\hat{\boldsymbol{\sigma}}}_{2}$$\ddot{\overline{\boldsymbol{p}}}$$\dot{\hat{\boldsymbol{\sigma}}}_{1}$(p)$\dot{\overline{\boldsymbol{p}}}$有界,可得$\dot{\boldsymbol{F}}$有界,进一步求导可得

$ \begin{gathered} \ddot{\boldsymbol{F}}=\boldsymbol{p}_{\mathrm{d}}^{(4)}-\hat{\boldsymbol{\sigma}}_{2}\left(\overline{\boldsymbol{v}}+\hat{\boldsymbol{\sigma}}_{1}(\overline{\boldsymbol{p}})\right) \cdot\left(\ddot{\overline{\boldsymbol{p}}}+\dot{\hat{\boldsymbol{\sigma}}}_{1}(\overline{\boldsymbol{p}}) \dot{\overline{\boldsymbol{p}}}\right)^{2}- \\ \dot{\hat{\boldsymbol{\sigma}}}_{2}\left(\overline{\boldsymbol{v}}+\hat{\boldsymbol{\sigma}}_{1}(\overline{\boldsymbol{p}})\right) \cdot\left(\dddot{\overline{\boldsymbol{p}}}+\hat{\boldsymbol{\sigma}}_{1}(\overline{\boldsymbol{p}}) \dot{\overline{\boldsymbol{p}}}{}^{2}+\dot{\hat{\boldsymbol{\sigma}}}_{1}(\overline{\boldsymbol{p}}) \ddot{\overline{\boldsymbol{p}}}\right)^{2}. \end{gathered} $ (46)

因为pd(4)$\hat{\dot{\sigma}}_{1}$$\hat{\ddot{\sigma}}_{2}$$\mathop {\overline {\bf{p}} }\limits^{..}$$\hat{\dot{\sigma}}_{1}$(p)$\dot{\overline{\boldsymbol{p}}}$$\hat{\ddot{\sigma}}_{1}$(p)$\dot{\overline{\boldsymbol{p}}}^{2}$有界,而$\mathop {\overline {\bf{p}} }\limits^{...}$的上界由ω决定,可知$\ddot{\boldsymbol{F}}$的上界也由ω决定。

yD=DmTDnT,按照传统姿态提取算法计算的期望姿态Qt≜(ηt, q1t, q2t, q3t)T,经过简单计算可得

$ y_{D}=\left[\begin{array}{l} 2 \eta_{\mathrm{t}} q_{3 \mathrm{t}}+2 q_{1 \mathrm{t}} q_{2 \mathrm{t}} \\ 1-2 q_{1 \mathrm{t}}^{2}-2 q_{3 \mathrm{t}}^{2} \\ 2 q_{2 \mathrm{t}} q_{3 \mathrm{t}}-2 \eta_{\mathrm{t}} q_{1 \mathrm{t}} \end{array}\right]^{\mathrm{T}} \boldsymbol{w}. $ (47)

由条件(6)可知,风向w及其一阶、二阶导数有界,简单计算可得yD有界,D的上界取决于$\dot{\boldsymbol{F}}$ÿD的上界取决于$\ddot{\boldsymbol{F}}$

对式(26)求导可得:

$ \dot{\theta}_{1 \mathrm{d}}=-\frac{1}{\left(\mu_{3}-g\right)^{2}+\mu_{2}^{2}}\left[\left(\mu_{3}-g\right) \dot{\mu}_{2}-\mu_{2} \dot{\mu}_{3}\right], $
$ \dot{\theta}_{2 \mathrm{d}}=\frac{1}{\sqrt{\left(\mu_{3}-g\right)^{2}+\mu_{2}^{2}}}\left(\dot{\mu}_{1}-\mu_{1} \frac{\ddot{T}_{\mathrm{d}}}{T_{\mathrm{d}}}\right), $
$ \dot{\theta}_{3 \mathrm{d}}=\frac{1}{\sqrt{1-y_{D}^{2}}} \dot{y}_{D} . $

注意μ1μ2μ3均为F的分量,简单计算可验证$\frac{\dot{T}_{\mathrm{d}}}{T_{\mathrm{d}}}$有界。综上,存在独立于滤波频率ωn的正常数ρ1使$\left\|\dot{\theta}_{\mathrm{d}}\right\| \leqslant \rho_{1}$成立。

进一步求导可得:

$ \begin{gathered} \ddot{\theta}_{1 \mathrm{d}}=\frac{2\left(\mu_{3}-g\right) \dot{\mu}_{3}+2 \mu_{2} \dot{\mu}_{2}}{\left[\left(\mu_{3}-g\right)^{2}+\mu_{2}^{2}\right]^{2}}\left[\left(\mu_{3}-g\right) \dot{\mu}_{2}-\mu_{2} \dot{\mu}_{3}\right]- \\ \frac{\left(\mu_{3}-g\right) \ddot{\mu}_{2}-\mu_{2} \ddot{\mu}_{3}}{\left(\mu_{3}-g\right)^{2}+\mu_{2}^{2}}, \end{gathered} $
$ \begin{gathered} \ddot{\theta}_{2 \mathrm{d}}=-\frac{\mu_{2} \dot{\mu}_{2}+\left(\mu_{3}-g\right) \dot{\mu}_{3}}{\left[\left(\mu_{3}-g\right)^{2}+\mu_{2}^{2}\right]^{3 / 2}}\left[\dot{\mu}_{1}-\mu_{1} \frac{\ddot{T}_{\mathrm{d}}}{T_{\mathrm{d}}}\right]+ \\ \frac{1}{\sqrt{\left(\mu_{3}-g\right)^{2}+\mu_{2}^{2}}}\left(\ddot{\mu}_{1}-\dot{\mu}_{1} \frac{\ddot{T}_{\mathrm{d}}}{T_{\mathrm{d}}}-\mu_{1} \frac{\ddot{T}_{\mathrm{d}}}{T_{\mathrm{d}}}+\mu_{1} \frac{\ddot{T}_{\mathrm{d}}^{2}}{T_{\mathrm{d}}^{2}}\right) \end{gathered} $
$ \ddot{\theta}_{3 \mathrm{d}}=\frac{1}{\sqrt{1-y_{D}^{2}}} \ddot{y}_{D}^{2}-\frac{1-2 y_{D} \dot{y_{D}}}{2\left(1-y_{D}^{2}\right)^{3 / 2}} \dot{y}_{D} . $

简单计算可验证$\frac{\ddot{T}_{\mathrm{d}}}{T_{\mathrm{d}}}$的上界取决于$\ddot{\boldsymbol{F}}$,而$\ddot{\boldsymbol{F}}$的上界取决于ω。综上,存在独立于滤波频率ωn的单调增正函数ρ2(·)使$\left\|\ddot{\theta}_{\mathrm{d}}\right\| \leqslant \rho_{2}(\|\boldsymbol{\omega}\|)$成立。

在引理2的基础上,引理3介绍了指令滤波器的相关性质:

引理3  考虑指令滤波器式(28)。假设存在一个独立于ωn的正常数C>0,对于所有t>0,都有||ω(t)||≤C成立,则

$ \sup \limits_{t \geqslant 0}\left\|\boldsymbol{\theta}_{\mathrm{c}}(t)-\boldsymbol{\theta}_{\mathrm{d}}(t)\right\| \approx O\left(1 / \omega_{\mathrm{n}}\right). $

此外,如果c2(0)=0,则下式成立:

$ \begin{gathered} \left\|\boldsymbol{c}_{2}(t)\right\| \leqslant \frac{3 \rho_{1}}{2 \xi \sqrt{1-\xi^{2}}}+ \\ \frac{1}{2 \xi^{2} \sqrt{1-\xi^{2}}} \max \limits_{0 \leqslant \tau \leqslant t} \frac{\rho_{2}(\|\boldsymbol{\omega}(\tau)\|)}{\omega_{\mathrm{n}}} . \end{gathered} $ (48)

具体证明过程可参考文[15]。引理3说明,在ω有界的条件下,只要选取足够大的滤波频率ωn,就可使姿态误差跟踪系统通过跟踪θc来实现跟踪θd的效果。

关于姿态误差跟踪系统,有如下引理:

引理4  考虑姿态误差跟踪系统式(33)及其控制律式(34),当t→∞时有Q(t)→0和ω(t)→0,并且总是存在一个单调增正函数α满足

$ \|\overline{\boldsymbol{\omega}}(t)\| \leqslant \alpha(\|\boldsymbol{\chi}\|) . $ (49)

其中χ≜(||Q(0)||, ||ω(0)||)T.

证明:构造Lyapnov函数如下:

$ \boldsymbol{V}=\left(k_{\mathrm{p}}+c k_{\mathrm{v}}\right)\left[(1-\bar{\eta})^{2}+\overline{\boldsymbol{q}}^{\mathrm{T}} \overline{\boldsymbol{q}}\right)+\frac{\overline{\boldsymbol{\omega}}^{\mathrm{T}} I \overline{\boldsymbol{\omega}}}{2}+c \overline{\boldsymbol{q}}^{\mathrm{T}} \boldsymbol{I} \overline{\boldsymbol{\omega}} . $

其中c>0为一个足够小的正常数。

根据文[10]中的推导可得$\dot{\boldsymbol{V}} \leqslant 0$,即当t→∞时有Q(t)→0和ω(t)→0,姿态误差跟踪系统全局渐近稳定。因此,总是可以找到单调增正函数α1α2α,满足

$ \begin{gathered} \|\overline{\boldsymbol{\omega}}(t)\| \leqslant \alpha_{1}(\boldsymbol{V}(t)) \leqslant \alpha_{2}(\boldsymbol{V}(0)) \leqslant \\ \alpha_{1}\left(\alpha_{2}(\|\boldsymbol{\chi}\|)\right) \triangleq \alpha(\|\boldsymbol{\chi}\|), \end{gathered} $

对于任意t≥0均成立。

在引理4的基础上,关于ω的有界性,有如下引理:

引理5  考虑闭环系统式(10)、(33)及其控制律式(14)、(34),采用姿态提取算法式(26)及指令滤波器式(28)、(29)。如果选取一个足够大的滤波频率ωn,那么总是可以找到一个独立于ωn的正常数C>0,使得||ω(t)||≤C对于所有t>0均成立。

证明:结合式(31)、(48),可得

$ \begin{gathered} \left\|\boldsymbol{\omega}_{\mathrm{c}}(t)\right\|=\left\|\varphi\left(\boldsymbol{\theta}_{\mathrm{c}}\right) \dot{\boldsymbol{\theta}}_{\mathrm{c}}\right\| \leqslant\left\|\dot{\boldsymbol{\theta}}_{\mathrm{c}}\right\|=\left\|c_{2}(t)\right\| \leqslant \\ \frac{3 \rho_{1}}{2 \xi \sqrt{1-\xi^{2}}}+\frac{1}{2 \xi^{2} \sqrt{1-\xi^{2}}} \max \limits_{0 \leqslant \tau \leqslant t} \frac{\rho_{2}(\|\boldsymbol{\omega}(\tau)\|)}{\omega_{\mathrm{n}}}= \\ C_{0}+\frac{1}{\omega_{\mathrm{n}}} \max \limits_{0 \leqslant r \leqslant}(\|\boldsymbol{\omega}(\tau)\|) . \end{gathered} $

其中

$ C_{0} \triangleq \frac{3 \rho_{1}}{2 \xi \sqrt{1-\xi^{2}}}, \rho(\|\boldsymbol{\omega}(\tau)\|) \triangleq \frac{\rho_{2}(\|\omega(\tau)\|)}{2 \xi^{2} \sqrt{1-\xi^{2}}} . $

由引理4可得

$ \|\overline{\boldsymbol{\omega}}(t)\|=\left\|\boldsymbol{\omega}(t)-\boldsymbol{R}(\overline{\boldsymbol{Q}}) \boldsymbol{\omega}_{\mathrm{c}}(t)\right\| \leqslant \alpha(\|\boldsymbol{\chi}\|), $

则有

$ \begin{gathered} \|\boldsymbol{\omega}(t)\| \leqslant\left\|\boldsymbol{R}(\overline{\boldsymbol{Q}}) \boldsymbol{\omega}_{\mathrm{c}}(t)\right\|+\alpha(\|\boldsymbol{\chi}\|) \leqslant \\ \left\|\boldsymbol{\omega}_{\mathrm{c}}(t)\right\|+\alpha(\|\boldsymbol{\chi}\|) \leqslant C_{0}+ \\ \frac{1}{\omega_{\mathrm{n}}} \max \limits_{0 \leqslant \tau \leqslant t}(\|\boldsymbol{\omega}(\tau)\|)+\alpha(\|\boldsymbol{\chi}\|) . \end{gathered} $

通过反证法可证明只要选取足够大的ωn满足

$ \omega_{\mathrm{n}}>\rho\left(C_{0}+\alpha(\|\boldsymbol{\chi}\|)+1\right), \quad \forall t>0 . $

且初始状态满足

$ \|\boldsymbol{\omega}(0)\|<C_{0}+\alpha(\|\boldsymbol{\chi}\|)+1 \text {. } $

则对于所有t≥0,均有

$ \|\boldsymbol{\omega}(t)\|<C_{0}+\alpha(\|\boldsymbol{\chi}\|)+1 . $

此外,容易证明C0α(||χ||)均独立于ωn,定义CC0+α(||χ||)+1,命题得证。

最后,定理2给出了闭环系统稳定性的结论:

定理2  考虑闭环系统式(10)、(33)及其控制律(14)、(34),采用姿态提取算法(26)及指令滤波器(28)、(29)。通过选取一个足够大的滤波频率ωn,可以使闭环系统的状态(p, v)∈$\mathbb{R}^3$×$\mathbb{R}^3$最终有界于O(1/ωn)。

证明:由引理3和5可知,对于t>0,只要选取足够大的滤波频率ωn,||θc(t)-θd(t)||=O(1/ωn)可以达到任意小,也即||R(Qc(t))-R(Qd(t))||=O(1/ωn)可以达到任意小。

由引理4可知Q(t)→Qc(t),即有

$ \left\|\boldsymbol{R}(\boldsymbol{Q}(t))-\boldsymbol{R}\left(\boldsymbol{Q}_{\mathrm{c}}(t)\right)\right\| \rightarrow 0. $

又因为

$ \begin{gathered} \left\|\boldsymbol{R}(\boldsymbol{Q})-\boldsymbol{R}\left(\boldsymbol{Q}_{\mathrm{d}}\right)\right\| \leqslant\left\|\boldsymbol{R}(\boldsymbol{Q})-\boldsymbol{R}\left(\boldsymbol{Q}_{\mathrm{c}}\right)\right\|+ \\ \left\|\boldsymbol{R}\left(\boldsymbol{Q}_{\mathrm{c}}\right)-\boldsymbol{R}\left(\boldsymbol{Q}_{\mathrm{d}}\right)\right\|, \end{gathered} $

可得

$ \left\|\boldsymbol{R}(\boldsymbol{Q}(t))-\boldsymbol{R}\left(\boldsymbol{Q}_{\mathrm{d}}(t)\right)\right\| \rightarrow O\left(1 / \omega_{\mathrm{n}}\right). $

定义干扰项d(t)≜-f(Q, Qd)+K1R(Q)T ê1,变换可得

$ \begin{gathered} d(t)=-f\left(\boldsymbol{Q}, \boldsymbol{Q}_{\mathrm{d}}\right)+K_{1} \boldsymbol{R}(\boldsymbol{Q})^{\mathrm{T}} \hat{\boldsymbol{e}}_{1}= \\ -\frac{T_{\mathrm{d}}}{m}\left[\boldsymbol{R}(\boldsymbol{Q})-\boldsymbol{R}\left(\boldsymbol{Q}_{\mathrm{d}}\right)\right]^{\mathrm{T}} \hat{\boldsymbol{e}}_{3}+ \\ K_{\mathrm{a}} K_{\mathrm{w}} \boldsymbol{w}^{\mathrm{T}} \boldsymbol{R}(\boldsymbol{Q})^{\mathrm{T}} \hat{\boldsymbol{e}}_{1} \boldsymbol{R}(\boldsymbol{Q})^{\mathrm{T}} \hat{\boldsymbol{e}}_{1}= \\ {\left[\boldsymbol{R}(Q)-\boldsymbol{R}\left(\boldsymbol{Q}_{\mathrm{d}}\right)\right]^{\mathrm{T}}\left[K_{\mathrm{a}} K_{\mathrm{w}} \boldsymbol{w}^{\mathrm{T}} \hat{\boldsymbol{e}}_{1} \boldsymbol{R}(\boldsymbol{Q})^{\mathrm{T}} \hat{\boldsymbol{e}}_{1}-\frac{T_{\mathrm{d}}}{m} \hat{\boldsymbol{e}}_{3}\right]+} \\ K_{\mathrm{a}} K_{\mathrm{w}} \boldsymbol{w}^{\mathrm{T}} \boldsymbol{R}\left(\boldsymbol{Q}_{\mathrm{d}}\right)^{\mathrm{T}} \hat{\boldsymbol{e}}_{1} \boldsymbol{R}(\boldsymbol{Q})^{\mathrm{T}} \hat{\boldsymbol{e}}_{1} . \end{gathered} $

注意wTR(Qd)T=0,上式可进一步化简为

$ \begin{gathered} d(t)=\left[\boldsymbol{R}(\boldsymbol{Q})-\boldsymbol{R}\left(\boldsymbol{Q}_{\mathrm{d}}\right)\right]^{\mathrm{T}} \\ {\left[K_{\mathrm{a}} K_{\mathrm{w}} \boldsymbol{w}^{\mathrm{T}} \hat{\boldsymbol{e}}_{1} \boldsymbol{R}(\boldsymbol{Q})^{\mathrm{T}} \hat{\boldsymbol{e}}_{1}-\frac{T_{\mathrm{d}}}{m} \hat{\boldsymbol{e}}_{3}\right] .} \end{gathered} $

显然||KaKwwT ê1R(Q)T ê1-$\frac{T_{\mathrm{d}}}{m} \hat{\boldsymbol{e}}_{3}$||有界,由此可得,当t→∞时

$ \|d(t)\| \leqslant O\left(1 / \omega_{\mathrm{n}}\right). $

即只要选取足够大的滤波频率ωn,根据定理1,可以证明闭环系统的状态(p, v)∈$\mathbb{R}^3$×$\mathbb{R}^3$最终有界于O(1/ωn)。

定理2说明了垂直起降飞行器的位置和速度跟踪误差最终有界,且只要所选的指令滤波器的频率ωn足够大,该上界便可达到任意小。该结果也得益于飞行器的滚转运动抵消了风扰的影响,否则,该跟踪误差的上界不仅取决于滤波频率,还将取决于风扰的上界。

4 仿真实验结果

本节使用SIMULINK作为仿真工具来验证该控制算法的有效性。

根据图 1所示的实验用无人机的机体数据,设定飞行器质量m为28.8 kg,设定发动机最大推力幅值λT=400 N,设定惯量矩阵I为((20, 2, 0.9)T, (2, 17, 0.5)T, (0.9, 0.5, 15)T) kg·m2,设定机身平面面积Sa=3.25 m2。风速采用正弦函数Kw(t)=0.5sin(0.2t) m/s进行模拟,风向单位向量w(t)设定为(cos(0.01t+1), sin(0.01t+1), 0)T,空气密度ρa=1.293 kg/m3。设定飞行器的期望跟踪轨迹pd=(cos(0.2t+2), sin(0.2t+2.4), 0.2t)Tm,初始姿态四元数设定为(1, 0, 0, 0)T,其他飞行器初始状态均设为0。指令滤波器参数ξ=0.7,ωn=100。嵌套饱和函数参数l1=10,l2=65,L1=20,L2=70,M1=12,M2=60,γT=0.02。控制器参数k1=0.66,k2=0.86,kp=200,kv=300。

仿真结果如图 3-8所示。图 3-7说明本文提出的采用新型姿态提取算法的控制算法能够使飞行器各状态跟踪误差迅速收敛到接近0的数值,表明本文提出的轨迹跟踪控制器的有效性。图 8进一步对比了新型姿态提取算法与传统姿态提取算法在风力干扰下的跟踪效果。其中,蓝色点型曲线表示期望跟踪轨迹pd(t),红色曲线表示采用新型姿态提取算法的控制器的跟踪轨迹p(t),黑色点型曲线表示采用传统姿态提取算法的控制器跟踪轨迹$\hat{\boldsymbol{p}}(t)$。从图 4可以看出,相比于采用传统姿态提取算法的轨迹跟踪控制器,本文提出的轨迹跟踪控制器能够更好地消除风力造成的影响,显著减少了跟踪误差,保证了垂直起降无人机在复杂地形和风扰条件下能够精准地执行飞行及起降任务。

图 3 轨迹跟踪误差

图 4 速度跟踪误差

图 5 加速度跟踪误差

图 6 姿态四元数跟踪误差

图 7 角速度跟踪误差

图 8 轨迹跟踪3D对比示意图

5 结论

本文提出了一种采用新型姿态提取算法的垂直起降无人机轨迹跟踪控制器,能够利用风向信息和滚转自由度,有效消除垂直起降过程中时变侧风对飞行造成的干扰。通过嵌套饱和函数和PD+前馈控制方法的结合,使无人机能够在推力限幅的条件下稳定飞行。对于时变侧风干扰导数未知所造成期望姿态无法求导的问题,采用了指令滤波器进行解决,并证明了只要滤波频率足够大,轨迹跟踪误差便可以达到任意小。最后的仿真结果表明,本文所设计的轨迹跟踪控制器能够较好地消除侧风的干扰,显著减少跟踪误差,使无人机能够适应更为复杂的地形和时变风扰情况下的垂直起降、跟踪飞行等任务。需要指出的是,本文所提出的控制器基于一个较为简单的时变侧风干扰模型,而在实际应用中,由于无人机机身构造较为复杂,侧风所造成的干扰很难通过简单的数学模型进行表述。因此,未来的工作将围绕不同机身构造下侧风干扰的精确建模展开,并对本文所提出的控制算法进一步扩展,以期适应更为复杂的应用场景。

参考文献
[1]
KNOEBE N B, MCLAIN T W. Adaptive quaternion control of a miniature tailsitter UAV [C]// Proceedings of 2008 American Control Conference. Seattle, USA: IEEE, 2008.
[2]
GUERRERO J A, LOZANO R, ROMERO G, et al. Robust control design based on sliding mode control for hover flight of a mini tail-sitter Unmanned Aerial Vehicle [C]// Proceedings of the 35th Annual Conference of IEEE Industrial Electronics. Porto, Portugal: IEEE, 2009.
[3]
XU Y, LIANG D, WU L N, et al. Design and implementation of twin-rotor tail-sitter UAV [C]// Proceedings of 2015 IEEE Advanced Information Technology, Electronic and Automation Control Conference. Chongqing, China: IEEE, 2015.
[4]
AILON A. Simple tracking controllers for autonomous VTOL aircraft with bounded inputs[J]. IEEE Transactions on Automatic Control, 2010, 55(3): 737-743. DOI:10.1109/TAC.2010.2040493
[5]
HUA M D, HAMEL T, MORIN P, et al. A control approach for thrust-propelled underactuated vehicles and its application to VTOL drones[J]. IEEE Transactions on Automatic Control, 2009, 54(8): 1837-1853. DOI:10.1109/TAC.2009.2024569
[6]
WU L F, LI H Y, LI Y J, et al. Position tracking control of tailsitter VTOL UAV with bounded thrust-vectoring propulsion system[J]. IEEE Access, 2019(7): 137054-137064.
[7]
PFLIMLIN J M, SOUERES P, HAMEL T. Position control of a ducted fan VTOL UAV in crosswind[J]. International Journal of Control, 2007, 80(5): 666-683. DOI:10.1080/00207170601045034
[8]
FLEMING J, JONES T, NG W, et al. Improving control system effectiveness for ducted fan VTOL UAVs operating in crosswinds [C]// Proceedings of the 2nd AIAA "Unmanned Unlimited" Conference and Workshop & Exhibit. San Diego, USA: AAA, 2003: 6514.
[9]
FRAZZOLI E, DAHLEH M A, FERON E. Trajectory tracking control design for autonomous helicopters using a backstepping algorithm [C]// Proceedings of the 2000 American Control Conference. Chicago, USA: IEEE, 2000.
[10]
WEN J T Y, KREUTZ-DELGADO K. The attitude control problem[J]. IEEE Transactions on Automatic Control, 1991, 36(10): 1148-1162. DOI:10.1109/9.90228
[11]
WU L F, LI C W, DING Q Q, et al. VTOL control of tailsitter UAV under crosswind disturbances [C]// Proceedings of 2020 Chinese Control And Decision Conference. Hefei, China: IEEE, 2020.
[12]
ROBERTS A, TAYEBI A. Adaptive position tracking of VTOL UAVs[J]. IEEE Transactions on Robotics, 2011, 27(1): 129-142. DOI:10.1109/TRO.2010.2092870
[13]
GAYAKA S, LU L, YAO B. Global stabilization of a chain of integrators with input saturation and disturbances: A new approach[J]. Automatica, 2012, 48(7): 1389-1396. DOI:10.1016/j.automatica.2011.11.012
[14]
REYNOLDS R G. Quaternion parameterization and a simple algorithm for global attitude estimation[J]. Journal of Guidance, Control, and Dynamics, 1998, 21(4): 669-672.
[15]
HU J C, ZHANG H H. Immersion and invariance based command-filtered adaptive backstepping control of VTOL vehicles[J]. Automatica, 2013, 49(7): 2160-2167. DOI:10.1016/j.automatica.2013.03.019