翼伞系统由于其高升阻比气动性能、优良滑翔能力、良好操控性和稳定性，目前已被广泛地应用于航空航天、军事和民用领域。近些年来，GPS(global position system)技术的引入和测控科学的发展极大促进了翼伞系统的自主控制研究^[1-2]。在翼伞系统轨迹跟踪控制相关研究中, Slegers等^[3-4]通过把翼伞系统简化为线性模型，利用拉格朗日插值法得到期望输出航向，采用模型预测控制设计轨迹跟踪控制器。熊菁^[5]针对简化的线性翼伞系统模型，采用增益调节型模糊PD控制算法设计了航迹跟踪控制器。谢亚荣等^[6]提出了基于模糊干扰观测器的非线性预测控制策略。李永新等^[7]采用基于模糊控制与预测控制切换的航迹跟踪控制方法。Benjamin^[8]在大型翼伞系统的贵行控制中采用了L1自适应控制方法。Culpepper等^[9]对故障情况下翼伞系统的控制问题进行了研究，提出了一种具有自适应能力的控制逻辑来应对翼伞系统可能出现的伞衣破损或者伞绳缠绕的情况。Rademacher等^[10]提出了一种在线轨迹规划的方法和一种轨迹误差的计算方法，并设计了消除轨迹误差的比例控制率。Aoustin等^[11]基于动力翼伞系统的纵向面模型，设计了基于部分反馈线性化方法的动力翼伞纵向面轨迹的非线性控制器。Tao等^[12-13]针对动力翼伞系统设计了自抗扰控制(active disturbance rejection control，ADRC)双通道控制器，进行横向轨迹跟踪和纵向高度控制，但实验结果局限于仿真研究。实际空投应用中，ALEX型翼伞空投系统使用了简单的比例控制器进行归航控制^[14]。Pegasus翼伞空投系统采用了比例积分微分(proportional-integral-derivative, PID)算法进行轨迹跟踪控制^[15]。然而以上研究中，对于智能控制方法，均需获取精确的动力学模型，由于翼伞系统总存在着参数不确定性和建模误差以及外部环境干扰，因而导致数学模型和实际系统之间不可避免地存在误差，使得轨迹跟踪控制精度难以保证。传统PID算法虽然不依赖于数学模型，但却难以在强干扰环境下获得高精度、快速无超调的控制效果。

ADRC是由中科院的韩京清提出的一种面向工程应用的控制算法^[16]。继承了PID不依赖于被控对象数学模型的优点，通过采用扩张状态观测器估计系统扰动，将系统扰动补偿为串联积分型，从而实现扰动抑制。其良好的抗扰和解决不确定性问题的能力已经在许多实验系统和实际工程中得到了检验^{[12, 13, 17-18]}。ADRC最初是以一种非线性形式提出的，具有较多的待定参数，不易调试。高志强^[19]通过带宽参数化的方法，将ADRC简化为LADRC，易于调试和工程实现。

为克服翼伞系统自身和环境不确定性对轨迹跟踪控制产生的影响，本文基于LADRC对翼伞系统轨迹跟踪控制进行了研究，并通过仿真和空投实验验证了所提出的轨迹跟踪控制方法的可行性。

1 翼伞系统动力学模型

由于伞绳、吊带和连接带所选用的是强度大、形变小的材料，各绳索的弹性形变可以忽略，从而忽略伞体和负载之间的相对滚转运动，只考虑两体之间的相对俯仰和相对偏航运动，建立翼伞系统八自由度动力学模型，包括伞体的6个自由度和负载的2个自由度。

令V_f=[u_f v_f w_f]^T表示风场在大地坐标系下三轴的速度，则伞体在伞体坐标系下的速度表示为V_p=[u_p v_p w_p]^T-T_d-p×V_f，负载在负载坐标系下的速度用V_w=[u_w v_w w_w]^T-T_w-p×T_d-p×V_f表示。其中T_d-p和T_w-p分别表示大地坐标系和负载坐标系到伞体坐标系的转换矩阵。令W_p=[p_p q_p r_p]和V_w=[u_w v_w w_w]分别表示伞体和负载的角速度。利用动量和动量矩定理分别对伞体和负载进行分析，可得：

$ \frac{{\partial {\mathit{\boldsymbol{P}}_{\rm{p}}}}}{{\partial t}} + {\mathit{\boldsymbol{W}}_{\rm{p}}} \times {\mathit{\boldsymbol{P}}_{\rm{p}}} = \mathit{\boldsymbol{F}}_{\rm{p}}^{{\rm{aero}}} + \mathit{\boldsymbol{F}}_{\rm{p}}^{\rm{G}} + \mathit{\boldsymbol{F}}_{\rm{p}}^{\rm{t}} $

(1)

$ \begin{array}{*{20}{c}} {\frac{{\partial {\mathit{\boldsymbol{H}}_{\rm{p}}}}}{{\partial t}} + {\mathit{\boldsymbol{W}}_{\rm{p}}} \times {\mathit{\boldsymbol{H}}_{\rm{p}}} + {\mathit{\boldsymbol{V}}_{\rm{p}}} \times {\mathit{\boldsymbol{P}}_{\rm{p}}} = }\\ {\mathit{\boldsymbol{M}}_{\rm{p}}^{{\rm{aero}}} + \mathit{\boldsymbol{M}}_{\rm{p}}^{\rm{f}} + \mathit{\boldsymbol{M}}_{\rm{p}}^{\rm{G}} + \mathit{\boldsymbol{M}}_{\rm{p}}^{\rm{t}}} \end{array} $

(2)

$ \frac{{\partial {\mathit{\boldsymbol{P}}_{\rm{w}}}}}{{\partial t}} + {\mathit{\boldsymbol{W}}_{\rm{w}}} \times {\mathit{\boldsymbol{P}}_{\rm{w}}} = \mathit{\boldsymbol{F}}_{\rm{w}}^{{\rm{aero}}} + \mathit{\boldsymbol{F}}_{\rm{w}}^{\rm{t}} + \mathit{\boldsymbol{F}}_{\rm{w}}^{\rm{G}} $

(3)

$ \frac{{\partial {\mathit{\boldsymbol{H}}_{\rm{w}}}}}{{\partial t}} + {\mathit{\boldsymbol{W}}_{\rm{w}}} \times {\mathit{\boldsymbol{H}}_{\rm{w}}} = \mathit{\boldsymbol{M}}_{\rm{w}}^{{\rm{aero}}} + \mathit{\boldsymbol{M}}_{\rm{w}}^{\rm{f}} + \mathit{\boldsymbol{M}}_{\rm{w}}^{\rm{t}} $

(4)

式中：下标p和w分别表示对伞体和负载的分析，上标earo、f、G和t分别表示气动力、阻力、重力和吊绳拉力，F和M分别表示力和力矩。

伞体和负载的动量P和动量矩H表示为

$ \left[ {\begin{array}{*{20}{c}} {{\mathit{\boldsymbol{P}}_{\rm{p}}}}\\ {{\mathit{\boldsymbol{H}}_{\rm{p}}}} \end{array}} \right] = \left[ {{\mathit{\boldsymbol{A}}_{\rm{a}}} + {\mathit{\boldsymbol{A}}_{\rm{r}}}} \right]\left[ {\begin{array}{*{20}{c}} {{\mathit{\boldsymbol{V}}_{\rm{p}}}}\\ {{\mathit{\boldsymbol{W}}_{\rm{p}}}} \end{array}} \right] $

(5)

$ \left[ {\begin{array}{*{20}{c}} {{\mathit{\boldsymbol{P}}_{\rm{w}}}}\\ {{\mathit{\boldsymbol{H}}_{\rm{w}}}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {{m_{\rm{w}}}}&0\\ 0&{{J_{\rm{w}}}} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{\mathit{\boldsymbol{V}}_{\rm{w}}}}\\ {{\mathit{\boldsymbol{W}}_{\rm{w}}}} \end{array}} \right] $

(6)

式中：A_a和A_r分别为伞体的附加质量和真实质量矩阵；m_w和J_w分别为负载的质量和转动惯量。

伞体和负载通过绳索连接，两者之间存在的约束关系为

$ {\mathit{\boldsymbol{V}}_{\rm{w}}} + {\mathit{\boldsymbol{W}}_{\rm{w}}} \times {\mathit{\boldsymbol{L}}_{{\rm{w}} - {\rm{c}}}} = {\mathit{\boldsymbol{V}}_{\rm{p}}} + {\mathit{\boldsymbol{W}}_{\rm{p}}} \times {\mathit{\boldsymbol{L}}_{{\rm{p}} - {\rm{c}}}} $

(7)

$ {\mathit{\boldsymbol{W}}_{\rm{w}}} = {\mathit{\boldsymbol{W}}_{\rm{p}}} + {\mathit{\boldsymbol{\tau }}_{\rm{p}}} + {\mathit{\boldsymbol{\kappa }}_{\rm{w}}} $

(8)

式中：$ {\mathit{\boldsymbol{\tau }}_{\rm{p}}} = {[0\;\;0\;\;{{\dot \psi }_{\rm{r}}}]^{\rm{T}}}$和${\mathit{\boldsymbol{k}}_{\rm{w}}} = {[0\;\;{{\dot \theta }_{\rm{r}}}\;\;0]^{\rm{T}}} $分别为伞体和负载的相对偏航角和相对俯仰角；L_w-c和L_p-c分别为负载质心w和翼伞质心p到负载上两个悬挂绳的中心点C的距离。

联合式(1)~(8)可以得到翼伞系统的八自由度的动力学模型，详细的建模过程见参考文献[20]。

2 基于LADRC轨迹跟踪控制 2.1 基于制导的2D轨迹跟踪策略

借鉴Breivik和Fossen提出的基于制导的路径跟踪的概念^[21]，采用基于制导的2D轨迹跟踪策略设计翼伞系统飞行的制导律，如图 1所示。

在图 1中，理想点p代表 2D平面上翼伞系统所处的位置，表示为p=[x y]^T∈R²，其速度描述为$\mathit{\boldsymbol{\dot p}} = {\left[{\mathit{\boldsymbol{\dot x}}\;\;\mathit{\boldsymbol{\dot y}}} \right]^{\rm{T}}} $ ∈R²，其速度的大小定义为${U_{\rm{d}}} = |\mathit{\boldsymbol{\dot p}}{\mathit{\boldsymbol{|}}_2} = \sqrt {{{\mathit{\boldsymbol{\dot p}}}^{\rm{T}}}\mathit{\boldsymbol{\dot p}}} \in {{\bf{R}}^2} $，其速度的方向角表示为${\chi _{\rm{d}}} = {\rm{arctan}}\left( {\frac{{\dot y}}{{\dot x}}} \right) $。

假设p的跟踪目标是参考轨迹上的运动目标点p_p，而p_p的位置是按照尺度变量${\bar \omega } $更新的，表示为p_p(${\bar \omega } $)=[x_p(${\bar \omega } $) y_p(${\bar \omega } $)]^T∈R²，其变化率表示为p′_p(${\bar \omega } $)=[x′_p(${\bar \omega } $) y′_p(${\bar \omega } $)]^T∈R²，此时p_p沿参考轨迹运动的方向角表示为

$ {\chi _p}\left( {\bar \omega } \right) = \arctan \left( {\frac{{{{y'}_p}\left( {\bar \omega } \right)}}{{{{x'}_p}\left( {\bar \omega } \right)}}} \right) $

(9)

式中：$ x{\prime _p}\left( {\bar \omega } \right) = \frac{{{\rm{d}}{x_p}}}{{{\rm{d}}\bar \omega }}\left( {\bar \omega } \right), {\rm{ }}y{\prime _p}\left( {\bar \omega } \right) = \frac{{{\rm{d}}{y_p}}}{{{\rm{d}}\bar \omega }}\left( {\bar \omega } \right)$。

以p_p的运动方向为x轴建立参考轨迹坐标系，建立惯性坐标系到参考轨迹坐标系的旋转矩阵为

$ {\mathit{\boldsymbol{T}}_{p - {\rm{I}}}}\left( {{\chi _p}} \right) = \left[ {\begin{array}{*{20}{c}} {{c_{{\chi _p}}}}&{ - {s_{{\chi _p}}}}\\ {{s_{{\chi _p}}}}&{{c_{{\chi _p}}}} \end{array}} \right] $

(10)

因此，任意点p到参考轨迹点p_p的距离误差可表示为

$ \mathit{\boldsymbol{\varepsilon }} = \mathit{\boldsymbol{T}}_{p - {\rm{I}}}^{\rm{T}}\left( {p - {p_p}\left( {\bar \omega } \right)} \right) $

(11)

式中：ε=[s e]^T∈R²，s表示前向误差，e表示横向误差。

文献[21]用李雅普诺夫方法已经证明其前向误差和横向误差在满足一定约束条件下是收敛的。在翼伞系统轨迹跟踪过程中，其参考目标点p_p对U_p和${\bar \omega } $进行更新可以实现s的消除，因此只需设置控制器控制翼伞系统的偏航角ψ跟踪参考方向角就能消除e。

2.2 LADRC设计

翼伞系统与跟踪的理想点之间的横向误差可以通过控制翼伞系统航迹方位角ψ进行消除，翼伞系统当前的航迹方位角计算为

$ \psi \left( t \right) = \arctan \left( {\frac{{\dot y}}{{\dot x}}} \right) $

(12)

式中x和y分别代表翼伞系统水平面的位置信息。

结合翼伞系统动力学方程，翼伞系统航迹方位角的二阶形式可表示为

$ \dot \psi \left( t \right) = {f_1}\left( \cdot \right) + {f_2}\left( u \right) $

(13)

式中：f₁(·)是关于翼伞系统状态变量的表达式，f₂(u)是含有控制量u的表达式，f₁(·)和f₂(u)均无法获得准确的数学表达式。

为了便于LADRC的设计，将式(13)改写为

$ \ddot \psi \left( t \right) = {f_1}\left( \cdot \right) + {f_2}\left( u \right) - {b_0}u + {b_0}u $

(14)

令f=f₁(·)+f₂(u)-b₀u，将f视为翼伞系统总扰动，构造翼伞系统航迹方位角的线性扩张状态观测器(linear extended state observer, LESO)。利用LESO可以估计出翼伞系统当前航迹方位角、方位角的变化率及整个系统的总扰动，再通过误差状态反馈控制率的动态补偿，从而实现对其航迹方位角的跟踪控制。

由于实际系统中，状态观测器存在瞬态响应极值。因此引入饱和限幅函数sat()防止系统出现饱和峰值，限幅方式选取为

$ u\left( t \right) = M\;{\rm{sat}}\left( {\frac{{\tilde u\left( t \right)}}{M}} \right) $

(15)

式中：$\tilde u\left( t \right) $代表限幅之前的控制律，M表示加载到舵机上的最大的控制量，其与伞衣后缘左右两侧伞绳的最大下拉长度相对应，限幅函数sat()定义为

$ \left\{ \begin{array}{l} {\rm{sat}}\left( x \right) = 1,\;\;\;\;\left| x \right| > 1\\ {\rm{sat}}\left( x \right) = x,\;\;\;\left| x \right| \le 1 \end{array} \right. $

由文献[22]对LESO和ADRC收敛性的证明，对于一般的二阶系统，如式(14)形式，其LESO的估计误差是有界的，而且误差的上界会随着观测器带宽的增大而单调减小。其ADRC控制系统的跟踪误差及其一阶导数也是有界的，且误差的上界会随着控制器带宽的增大而单调减小。

3 仿真与空投实验

作为实例，本文选用某一实际空投伞型进行仿真和空投实验，具体参数为展长7.43 m，弦长2.19 m，伞衣面积16.5 m，伞绳长度4.81 m，安装角10°，伞衣质量5 kg，负载质量40 kg。

3.1 仿真实验

根据所选的伞型，其仿真模型初始运动参数设置为：初始速度(u, v, w)=(15.9 m/s, 0, 2.1 m/s), 初始欧拉角(ξ, θ, ψ)=(0, 0, 0), 初始角速度(p, q, r)=(0, 0, 0)。实际系统搭载GPS模块的采样频率为4 Hz，设定LESO的更新频率为4 Hz。同时，考虑到控制舵机完成伞绳全部回收的时间为1 s，所以设置控制量的输出间隔为1 s。

为验证轨迹跟踪控制性能，设置参考轨迹为圆心为水平坐标原点(0，0)，半径为250 m的圆形。翼伞系统的初始平面位置为(0, -250 m)，翼空投高度为2 000 m。采用本文构建的LADRC控制系统与相同结构的PID控制系统分别在无扰动、大气扰动、附加质量扰动和全扰动下进行翼伞系统轨迹跟踪控制仿真实验，仿真时间设置为200 s，仿真条件设置如下：

1) 无扰动；

2) 大气扰动。第50 s在系统中加入沿大地坐标系y轴正方向3 m/s的平均风；在第100 s向系统中叠加入与平均风同方向的突风^[23]，最大风值为3 m/s，作用时间为15 s；

3) 附加质量扰动。相关参数测量或计算方法误差造成的附加质量有+20%的增量；

4) 全扰动。包含上述2中扰动。

控制器参数选择如下：LADRC的参数设置为：ω₀=30，k_p=0.18，k_d=1，b₀=0.04；PID控制器的参数设置为：k_p=20，k_d=100，k_i=0.2。

仿真实验结果如图 2~5所示。

为了更好地衡量轨迹跟踪效果，定义评价标准控制能耗$ s = \sum\limits_{i = 1}^N {|u\left( i \right)-u\left( {i-1} \right)|} $；空间位置的最大误差$e_{\max }^{{\rm{pos}}} = \mathop {{\rm{max}}}\limits_i |l\left( i \right)| $；平均误差$e_{{\rm{ave}}}^{{\rm{pos}}} = \frac{1}{N}\sum\limits_{i = 1}^N {|l\left( i \right)|} $和标准差${\sigma ^{{\rm{pos}}}} = \sqrt {\frac{1}{N}{{\sum\limits_{i = 1}^N {\left( {l\left( i \right)} \right)} }^2}} $。其中，u(i)为第i个采样时间点的控制量，l(i)为第i个采用时间点的参考平面位置和实际平面位置相对距离，N为总采样次数。

表 1为4种工况下LADRC轨迹跟踪位置误差结果。4种工况下PID轨迹跟踪位置误差结果如表 2所示。

由图 2所示，在无扰动的情况下，LADRC和PID控制器均能较好的控制翼伞系统跟踪参考圆形轨迹。在初始阶段，LADRC能够控制翼伞系统快速的跟踪参考轨迹，而PID控制器响应速度较慢，控制量输出超调大，距离误差收敛速度较慢。从表 1和表 2位置误差数据对比可以看出，两种控制器轨迹跟踪的位置误差相差不大，LADRC略优。

由图 3所示，在大气扰动的情况下，LADRC也能够很好的跟踪参考轨迹，PID控制轨迹虽能跟踪参考轨迹，但是因为风扰的存在，跟踪效果比LADRC差。尤其在叠加突风扰动后，LADRC比PID控制器能够快速克服突风对翼伞系统的影响，实现控制量的平稳输出，保证翼伞系统的水平轨迹跟踪精度。PID控制器在受到突风扰动时，则产生较长时间的震荡。

由图 4所示，在翼伞系统受到内扰作用时，LADRC显示了比PID控制器更加优秀的性能，特别是在控制能耗方面。从表 1和表 2可知，在位置误差控制方面，LADRC也略优于PID。

由图 5可知，当翼伞系统受到内扰和外扰同时作用时，系统内部存在着较强的耦合性，LADRC可以将耦合作用通过LESO进行观测并进行动态补偿，实现抗扰控制。此时PID控制器则在突风扰动下出现了短时的输出饱和。通过进一步仿真实验发现，当继续增大水平风扰至5 m/s时，PID控制器持续输出饱和，系统发散。

通过表 1和表 2各种单一扰动相比可知，大气干扰的误差和标准差最大，是实现翼伞系统精确轨迹跟踪控制的最大障碍，由建模误差引起的附加质量扰动对控制性能影响较小，其控制性能指标与无扰动情况相差不大。

综上所述，采用本文搭建的LADRC控制系统无论是在外扰、内扰还是在综合扰动下均能达到满意的轨迹规划效果，且未出现发散现象，与PID搭建的控制系统相比，LADRC控制系统具有更优的抗扰性能和鲁棒性。

全干扰下LADRC控制翼伞系统跟踪参考轨迹过程中速度和欧拉角变化如图 6~7所示。

图 6显示的是翼伞系统速度变化曲线。从图中可以看出，在加入大气扰动前，翼伞系统的速度基本保持不变，其水平速度基本维持在11.9 m/s，垂直速度保持在3.1 m/s。在突风作用期间，其水平速度和垂直速度均发生了一定程度的震荡，这是由于翼伞系统水平面和纵向面运动存在着强耦合导致。由于惯性的存在，负载的水平速度和垂直速度在过渡阶段和风扰阶段总是略大于伞体的相应速度。

图 7显示的是翼伞系统轨迹跟踪过程中欧拉角的变化曲线。由图中结果可知，翼伞系统的滚转角仅在叠加突风作用时波动较大，在稳定转弯或滑翔时，基本保持稳定。其俯仰角也是在突风作用时发生较大波动，而相对俯仰角在整个过程中基本保持稳定。翼系统的偏航角随着参考轨迹规律的变化，受大气干扰影响较小；负载和伞体之间的相对偏航角在突风作用下产生高频振荡。

综上可知，翼伞系统在归航过程中保持了自身姿态稳定，没有出现失速或失稳状况。

3.2 空投实验

仿真分析为实际系统的开发提供了理论指导，是科学研究的有效手段和必要的补充，但仿真分析不能代替实际实验，仿真的结果需要空投实验进行验证。为此，本项目组开发了一套小型翼伞空投系统，并进行了翼伞系统空投实验。

空投实验翼伞系统由热气球搭载升空，到达预定高度后投放。空投实验的基本信息如下：空投场地海拔高度90 m，投放海拔高度460 m，翼伞系统开伞损失高度30 m和雀降预留高度20 m。目标轨迹设定为连接投放点与目标点的直线，采用基于LADRC的轨迹跟踪策略，参数设置与上述仿真实验相同。翼伞空投系统实际轨迹与相同条件下仿真实验轨迹对比如图 8所示。

从图 8中可以看出，翼伞空投系统的实际飞行轨迹与仿真轨迹在开始阶段基本重合，虽然随着时间的增长二者之间的距离逐渐扩大，但整体趋势是一致的，说明基于LADRC轨迹跟踪系统的有效性。

对于误差产生的原因，主要有以下两个方面：一是实际空投场地环境的复杂性，翼伞空投系统运动区域的风场不是标准的稳定风场，同时存在突风、紊流，但目前在仿真环境中尚无法对翼伞空投场地的风场进行无差别的仿真。另一方面，实际翼伞系统是大时滞惯性系统，舵机执行操纵指令是一个动态的、连续的过程，目前在仿真模型无法进行完全的模拟。除去开伞高度损失和雀降预留高度，本次空投实验翼伞系统实际操纵高度仅为320 m，不足以完成完整的直线轨迹跟踪实验。从仿真结果来看，如果能够提高投放高度，则可以实现翼伞空投系统对参考直线轨迹的较好跟踪。

4 结论

1) 基于制导的路径跟踪思想，提出基于制导的翼伞系统2D轨迹跟踪策略，利用翼伞系统的平面位置信息同理想轨迹之间的误差转换，将对平面轨迹的跟踪问题转化为对翼伞系统航迹方位角的控制问题。

2) 在翼伞系统轨迹跟踪控制中将内扰和外扰统一作为总扰动，通过LESO进行观测并进行动态补偿，最终实现抗扰控制，同时对所有扰动具有较好的抑制作用。相比于PID轨迹跟踪方法，本文提出的基于LADRC轨迹跟踪控制方法跟踪效果更好，抗扰能力更强。

3) 通过空投实验对翼伞系统轨迹跟踪算法进行了验证，实验数据和仿真结果具有较高的契合度，说明了通过模型仿真能够为空投实验提供较好的指导。