近年来,尾座式垂直起降无人机受到了国内外学者们越来越多的关注。相较于传统机型,尾座式垂直起降无人机融合了旋翼机的垂直起降和固定翼机的高速巡航等优点[1]。在起降阶段,尾座式无人机利用主推进器、舵面以及推力矢量系统,保持机头竖直朝上、机尾朝下的姿态,实现与旋翼机相同的垂直起降和悬停效果。经由过渡飞行过程,尾座式垂直起降无人机可以在竖直和水平飞行2种状态间切换。在进行远航程任务时,尾座式无人机可转换为水平飞行模式,达到与固定翼飞机相近的巡航速度。由于不需要加装额外的升力机构,尾座式垂直起降无人机精简了结构设计,减少了冗余死重[2-3]。
尽管尾座式垂直起降无人机有着上述优势,但在控制器设计过程面临2个主要难题:1) 垂直起降过程中,无人机所需升力全部由推进器的推力产生,在设计控制器时必须考虑推力限幅的问题[4-6];2) 由于机翼迎风面积较大,很容易在侧风影响下承受较大的干扰力,导致垂直起降无人机位置偏移或姿态失控[7-8]。现有研究工作在考虑推力限幅和侧风干扰问题时,通常会假设侧风干扰力存在一个足够小的上界,使推力在满足限幅条件的情况下可以抵消侧风干扰力的影响。然而对于竖直状态下迎风面较大的的尾座式垂直起降无人机而言,侧风干扰力较大,且飞行器升力全部由发动机推力提供,因而该假设很难成立。此外,现有垂直起降控制器采用的传统姿态提取算法没有考虑飞行器姿态和侧风干扰力之间相互影响的关系。
本文在反步控制方法的基础上[9]提出了一种姿态提取算法,能够在时变侧风干扰下利用风向信息和飞行器滚转自由度大幅减少侧风干扰力,并采用指令滤波器解决侧风干扰造成的期望姿态无法求导的问题;同时通过嵌套饱和函数方法对控制输入加以限制,结合PD+前馈控制方法实现姿态环的跟踪控制[10];最后证明了所提出的控制方法的稳定性,并通过仿真实验验证了控制效果。
1 系统模型 1.1 研究对象本文所研究的尾座式无人机如图 1所示。机身采用木板结构,尾部装配2台涡喷发动机,通过舵机控制尾喷管方向,可实现矢量推力。尾喷管在俯仰方向的差动运动可以为无人机提供滚转力矩,在俯仰方向和偏航方向的联动运动可以分别为无人机提供俯仰力矩和偏航力矩[3]。
![]() |
图 1 尾座式无人机 |
在露天环境执行任务时,无人机容易受到侧风干扰而发生位置偏移[11]。特别是当风向、风速随时间产生无规律变化时,无人机的位置也会产生无规律的偏移,严重影响轨迹跟踪的控制效果。为了解决该问题,除了对无人机动力学进行建模外,还要对侧风干扰进行建模。
1.2 无人机动力学模型本文将尾座式无人机视为刚体。
设p∈
$ \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)T和v2∈
$ \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)表示从
设T∈
$ \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为机体坐标系
由式(1)和(2)可以看出,本文所讨论的尾座式垂直起降无人机与多旋翼垂直起降无人机在动力学方程的形式上没有区别,仅在执行机构上存在区别。实际控制器设计中,需要把控制输入T和τ转化为双涡喷发动机的转速指令及矢量喷管的舵机偏转指令,具体转化步骤可参考文[6]。当垂直起降无人机进入水平飞行状态时,其动力学特性与固定翼无人机相同,由于本文着重探讨竖直飞行状态下的控制问题,因而不再赘述。
1.3 侧风干扰模型设单位向量w∈
$ \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∈
设无人机处于低速运动状态,有||v||≪Kw。由于机身较为扁平,机翼迎风面积较大,大部分侧风干扰力来自于侧风对机身平面的垂向冲力,且该分量通常远大于沿机体纵轴和横轴的分量,即||K1||≫||K2||+||K3||。将z分为垂向分量zi≜K1 ê1b和平行分量zn≜K2 ê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分别是Kw和w的绝对值上界,λK1, λw1>0分别是
在忽略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) |
其中:对于任意x≥g,F需满足条件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成立,则有Q→Qd以及干扰项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{\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∈
$ \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) |
其中:ki、li、Mi、Li是正数,满足Mi>kili和Li>li。对于控制律式(14)中的饱和函数
$ \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)、饱和函数
文[12]提出了一种满足式(9)约束的姿态提取算法,设该算法所求得的期望姿态为Qt≜(ηt, qtT)T,在已知F≜(μ1, μ2, μ3)T和Td的情况下,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∈
$ \boldsymbol{D}_{\mathrm{t}}=\boldsymbol{R}\left(\boldsymbol{Q}_{\mathrm{t}}\right)^{\mathrm{T}} \hat{\boldsymbol{e}}_{3} . $ | (20) |
设Dm≜R(Qt)T ê1表示无人机在传统期望姿态Qt下的机身平面法向量,Dn≜R(Qd)T ê1代表无人机在改进后的期望姿态Qd下的机身平面法向量。显然,当Dn垂直于风向w和期望推力方向Dt所在的平面时,有wTR(Qd)T ê1=0成立,即当Q→Qd时,干扰量幅值K1→0,因此Dn可通过下式计算:
$ \boldsymbol{D}_{\mathrm{n}}=\boldsymbol{w} \times \boldsymbol{D}_{\mathrm{t}}. $ | (21) |
设四元数Qr≜(ηr, qrT)T表示从Dm到Dn的旋转,即有
$ \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)代表的旋转轴同时垂直于Dm和Dn,即Qr是以最小旋转角进行的滚转运动[14]。最后将2个旋转Qt和Qr进行叠加,即可得到改进后的期望姿态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和角加速度
$ \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)计算得出,但其导数
设计指令滤波器[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)∈
$ \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) |
R1、R2、R3、Rvh定义如下:
$ \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、
$ \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∈
本节将证明垂直起降飞行器的位置和速度跟踪误差最终有界,且只要选取的指令滤波器的频率ωn足够大,该上界可以达到任意小。
首先,对于位置环误差跟踪系统有如下引理:
引理1 考虑下述以(
$ \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)|的上界,则状态量(
证明:显然,如果所有状态在有限时间内进入了饱和函数的线性非饱和区域,则系统式(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,总是存在正常数a和b满足
$ \|\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=
$ \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为正不变域。
同样的方法可用于证明
引理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)∈
证明:注意系统式(35)及其控制律式(36)正是系统(10)及其控制律(14)的标量形式。当t>tf时,将-f(Q, Qd)+K1R(Q)T ê1视作干扰项d(t),即有λd≜ε,再结合式(17)和(43)可得条件(37)成立。综上,由引理1可得,系统状态(p, v)∈
定理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)、
$ \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)、
设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的上界取决于
对式(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的分量,简单计算可验证
进一步求导可得:
$ \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} . $ |
简单计算可验证
在引理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]中的推导可得
$ \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,定义C≜C0+α(||χ||)+1,命题得证。
最后,定理2给出了闭环系统稳定性的结论:
定理2 考虑闭环系统式(10)、(33)及其控制律(14)、(34),采用姿态提取算法(26)及指令滤波器(28)、(29)。通过选取一个足够大的滤波频率ωn,可以使闭环系统的状态(p, v)∈
证明:由引理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-
$ \|d(t)\| \leqslant O\left(1 / \omega_{\mathrm{n}}\right). $ |
即只要选取足够大的滤波频率ωn,根据定理1,可以证明闭环系统的状态(p, v)∈
定理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),黑色点型曲线表示采用传统姿态提取算法的控制器跟踪轨迹
![]() |
图 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 |