CN114265428A - 一种基于非约束力矢量和方向控制的拖曳体位置控制方法 - Google Patents

Abstract

本发明公开了一种基于非约束力矢量和方向控制的拖曳体位置控制方法，属于飞行器导航、制导与控制领域；具体为：首先，基于多刚体建模方法，建立考虑拖曳体外力的缆绳动力学模型；然后，考虑缆绳拉力及风扰动影响，建立拖曳体的六自由度动力学模型；利用缆绳动力学模型，建立基于缆绳位形与拖曳体的非约束力矢量和方向关系的拖曳体位置外环控制器，用于输出期望的拖曳体气流角；基于固定时间控制理论和拖曳体的六自由度动力学模型，设计内环的抗扰动控制器，并输入期望的拖曳体气流角，快速精确控制拖曳体的位置；本发明实现拖曳体对风扰动和参数摄动的抵抗和对位置指令的快速精确跟踪。

Description

一种基于非约束力矢量和方向控制的拖曳体位置控制方法

技术领域

本发明属于飞行器导航、制导与控制领域，具体涉及一种基于非约束力矢量和方向控制的拖曳体位置控制方法。

背景技术

拖曳体指的是由一架飞行器在空中通过吊挂的缆绳等柔性结构，拖曳另一架具有主动控制能力的飞行器。在空中回收，空中加油的输油段以及空中拖靶等任务中，需要对拖曳体进行精确、稳定和快速地位置控制，以较好地完成任务，如何设计相应的拖曳体位置控制器，是一个亟待解决的难题。

拖曳体会受到复杂的气流扰动(大气紊流，阵风，母机尾涡等)，缆绳的拖曳力(快变扰动)，以及自身的气动力和重力等影响，呈现出快时变、强非线性和强耦合等特性，而目前现有的飞行器抗扰动控制方法，例如自抗扰控制(ADRC)和基于干扰观测器的控制方法(DOBC)，不能很好地解决拖曳体位置控制中存在的快变扰动、强非线性和强耦合等难点，会导致拖曳体出现飘摆运动，难以快速稳定。

如何通过深入分析缆绳多体动力学特性以及飘摆现象产生的原因，转变控制思路，成为从根本上解决上述难题的关键。

发明内容

本发明为了解决传统抗扰动控制方法在拖曳体位置控制中，难以抵抗由缆绳拉力引起的快变扰动以及难以解决柔性缆绳结构带来的强耦合和强非线性特性等问题，进一步提高在多重风扰动以及参数摄动存在时的拖曳体位置控制性能，提出一种基于非约束力矢量和方向控制的拖曳体位置控制方法。

具体包括如下步骤：

步骤一、基于多刚体建模方法，建立考虑拖曳体外力的缆绳动力学模型；

将缆绳作为多刚体结构，并离散为N个刚体连杆，在拖曳坐标系O_d-X_dY_dZ_d下建立缆绳的多刚体动力学模型f_C，如下：

其中，p表示缆绳各个节点的三轴位置，为矩阵形式；F表示缆绳各个节点所受的气动力矩阵；Q_A表示拖曳体所受到的非约束力矢量和(包含气动力，重力和推力等，不包含缆绳拉力)，为向量形式。

步骤二、考虑缆绳拉力及风扰动影响，建立拖曳体的六自由度动力学模型；

拖曳体的六自由度动力学模型f_U，简写为：

其中，s为拖曳体状态向量，u为拖曳体控制输入向量，d为拖曳体受到的风扰动向量；

步骤三、利用缆绳动力学模型，建立基于缆绳位形与拖曳体的非约束力矢量和方向关系的拖曳体位置外环控制器，用于输出期望的拖曳体气流角；

具体步骤如下：

步骤301、基于缆绳动力学模型，建立拖曳体非约束力矢量和方向与拖曳体的垂向位置Z的映射关系，得到非约束力矢量和方向角ξ；

建立如下映射关系式：

其中，|Q_A|为缆绳末端的非约束力矢量和的幅值，ξ是非约束力矢量和Q_A与拖曳体重力

G_A在拖曳坐标系的XOZ平面内的分量夹角，

是由拟合函数f_ξ(·)估计的夹角ξ稳态值；

步骤302、基于拖曳体的升阻特性，建立拖曳体的非约束力矢量和的幅值|Q_A|与非约束力矢量和方向角ξ之间的关系；

其中，F_AX(α)表示气动力在坐标系O_d-X_dY_dZ_d中X轴向的分量，T_dX(α)表示推力在坐标系O_d-X_dY_dZ_d中X轴向的分量；G_AZ表示拖曳体重力在坐标系O_d-X_dY_dZ_d中Z轴向的分量；

步骤303、使用拟合函数f_ξ(·)来估计在给定不同拖曳体位置的期望值Z^*以及推力T_d下，拖曳体的非约束力矢量和方向角估计值

为非约束力矢量和方向角ξ在稳态情况下的估计值；

其中，α₀表示拖曳体当前的攻角α，在无实数解的情况下，

被设置为当前值ξ₀；

步骤304、利用PID控制器抑制在拖曳体状态转换过程中的位置变化，并补偿非约束力矢量和方向角估计值

的估计误差；

其中，K_PY，K_IY和K_DY为Y向PID控制器的三个参数，K_PZ，K_IZ和K_DZ为Z向PID控制器的三个参数，μ^*为倾侧角指令，ξ_c为PID位置控制器产生的非约束力矢量和方向指令，e_Y为Y向位置跟踪误差，主要由风扰引起，数值较小，e_Z为Z向位置跟踪误差。

步骤305、将非约束力矢量和方向角ξ^*转化为攻角指令α^*；

与α^*之间的转换公式为：

其中，α_l和α_u表示拖曳体攻角α的下边界和上边界；

步骤306、输出拖曳体基于非约束力矢量和方向的期望气流角Ω^*＝[α^* 0 μ^*]。

步骤四、基于固定时间控制理论和拖曳体的六自由度动力学模型，设计内环的抗扰动控制器，并输入期望的拖曳体气流角，快速控制拖曳体最终的实际位置；

具体步骤如下：

步骤401、基于时标分离原理和奇异摄动理论，将期望的气流角和六自由度动力学模型中拖曳体的角速度，改写为利于控制器设计的仿射非线性形式；

改写公式为：

其中，ω＝[p,q,r]^T为拖曳体的角速度，δ＝[δ_a,δ_e,δ_r]^T分别表示拖曳体的副翼，升降舵和方向舵的偏转角，f_Ω表示拖曳体气流角的集总扰动，f_ω表示拖曳体角速度的集总扰动，B_Ω表示气流角的控制矩阵，B_ω表示角速度的控制矩阵。

步骤402、基于气流角和角速度的仿射非线性模型，分别设计各自对应的固定时间控制器和固定时间扩张状态观测器；根据气流角和角速度固定时间控制器输出舵偏角。

步骤403、将舵偏角输入到缆绳动力学模型中，计算出缆绳末端实际的非约束力矢量和方向，并进一步计算出缆绳的实际位形，最后得到拖曳体的实际位置，并在下个周期内对位置偏差进行调整。

本发明的优点在于：

(1)一种基于非约束力矢量和方向控制的拖曳体位置控制方法，通过将拖曳体的位置控制转化为非约束力矢量和方向控制，可以降低缆绳-拖曳体组合系统的非线性和耦合特性，并且可以补偿发动机停车以及缆绳拉力等快变扰动；

(2)一种基于非约束力矢量和方向控制的拖曳体位置控制方法，基于非约束力矢量和方向与缆绳形状之间的关系设计前馈控制，进一步提升拖曳体位置控制的响应速度；

(3)一种基于非约束力矢量和方向控制的拖曳体位置控制方法，基于固定时间控制理论，设计内环控制器和干扰观测器，实现对风扰动和参数摄动的抵抗和对非约束力矢量和方向指令的快速跟踪。

附图说明

图1是本发明一种基于非约束力矢量和方向控制的拖曳体位置控制方法的流程图；

图2是本发明缆绳-拖曳体组合系统的结构图；

图3是本发明中非约束力矢量和方向和拖曳体位置、非约束力矢量和大小之间的关系图；

图4是本发明一种基于非约束力矢量和方向控制的拖曳体位置控制方法的控制框图；

图5是本发明实例中的风扰动情况图；

图6是本发明实例中发动机关机不同控制算法的时域响应对比图；

图7是本发明实例中发动机关机不同控制算法的拖曳体YZ平面轨迹对比；

图8a是本发明实例中拖曳体参数摄动-10％情况下不同控制算法的时域响应对比图；

图8b是本发明实例中拖曳体参数摄动+10％情况下不同控制算法的时域响应对比图；

图9是本发明实例中给定不同期望位置信号情况下不同控制算法的跟踪性能对比图。

具体实施方式

为了便于本领域普通技术人员理解和实施本发明，下面结合附图和实例对本发明作进一步的详细描述。

本发明一种基于非约束力矢量和方向控制的拖曳体位置控制方法，基于对缆绳动力学系统的深入研究，发现拖曳体的非约束力矢量和方向(除缆绳拉力外其它力的矢量和方向)与缆绳位形密切相关，首先将拖曳体位置控制转化为非约束力矢量和控制，大幅降低了缆绳-拖曳体组合系统的非线性和耦合特性，进一步基于非约束力矢量和方向与缆绳位形之间的关系，设计前馈控制器，提升拖曳体位置控制的响应速度；最后，基于固定时间控制理论，设计内回路的固定时间抗扰动控制器，以实现对风扰动和参数摄动的抵抗和对非约束力矢量和方向指令的快速跟踪。本发明对实现拖曳体的抗扰动精确快速位置控制具有重要的意义。

如图1所示，具体包括如下步骤：

步骤一、基于多刚体建模方法，建立考虑拖曳体外力的缆绳动力学模型；

为实现拖曳体的位置控制，需要研究影响缆绳位形的关键因素，通过控制缆绳位形，进而控制位于缆绳末端的拖曳体位置。

如图2所示，假设缆绳是一个多刚体结构，将缆绳离散为N个刚体连杆，忽略各个刚体连杆之间的柔性、延展性和摩擦力，连杆自身没有质量，其质量被等效在节点处，并假设母机处于平飞状态，在拖曳坐标系O_d-X_dY_dZ_d(与惯性坐标系O_g-X_gY_gZ_g对齐，起点位于缆绳初始点)下建立如下的缆绳多刚体动力学模型，如下：

r_i＝-l[cosθ_i1 cosθ_i2 sinθ_i2 -sinθ_i1 cosθ_i2]^T＝-ln_i,i＝1,...,N (1)

其中，p_i表示第i个节点的位置向量，r_i＝p_i-p_i-1表示相对位置向量，l＝L₀/N表示第i个连杆的长度，L₀表示缆绳的总长度，n_i表示第i-1个连杆的方向向量，θ_i1和θ_i2表示第i-1个连杆分别与O_d-X_dY_dZ_d纵向平面和侧向平面的夹角：

其中，

表示第i个节点的加速度，ω_i,τ_i表示O_d-X_dY_dZ_d相对于O_g-X_gY_gZ_g的旋转角速度和旋转角加速度。

接着，缆绳的外力可由下式建模：

其中，Q_i为节点i的合外力(包含气动力F_i和重力)；g是重力加速度；Q_N为作用在第N个节点(缆绳末端)上的合外力；Q_A为拖曳体受到的非约束合外力矢量和(包含气动力，重力，发动机推力，不包含缆绳拉力)；ρ为大气密度；V_ai是第i个节点的空速(包含母机尾涡和大气紊流等风扰动)；d_c是缆绳直径；c_s和c_d分别是表面摩擦力系数和阻力系数；m表示每一个缆绳节点的质量。

缆绳拉力是一个内部力，无法直接测量获得，但可以通过计算下述代数方程求取：

其中，t_i表示作用在节点i上的缆绳拉力幅值，t_i＝t_in_i表示缆绳拉力矢量。

最终，节点i的加速度a_i可用下式建模：

根据式(1)-(5)，可以建立拖曳缆绳系统的动力学模型；如下：

其中，p表示缆绳各个节点的三轴位置，为矩阵形式；F表示缆绳各个节点所受的气动力，为矩阵形式；

由式(6)可知，缆绳位形基本由缆绳受到的气动力和缆绳末端的非约束力矢量和决定，而缆绳受到的气动力变化相对较小，因此缆绳位形主要由缆绳末端所受到的非约束力矢量和决定。

步骤二、考虑缆绳拉力及风扰动影响，建立拖曳体的六自由度动力学模型；

缆绳末端的非约束力矢量和主要由拖曳体的气动力、重力组成，而拖曳体的气动力可通过改变气流角来进行控制，因此，需要建立拖曳体的动力学模型，为后续控制器的设计提供模型基础。

借鉴固定翼飞行器的六自由度动力学建模方法，建立拖曳体在航迹系下的六自由度动力学模型：

其中，p_N＝[X Y Z]^T是拖曳体的位置向量；R^d/k是从航迹系到拖曳系O_d-X_dY_dZ_d的转换矩阵；V_k＝[V_k 0 0]^T是拖曳体的航迹速度向量；V_k是航迹速度；V₀＝[V₀ 0 0]^T是O_d-X_dY_dZ_d相对于O_g-X_gY_gZ_g的速度向量。

其中，m_A是拖曳体的质量，R^k/b是从拖曳体机体系到航迹系的转换矩阵，R^k/a是从气流系到航迹系的转换矩阵，R^k/d是从O_d-X_dY_dZ_d到航迹系的转换矩阵，F_A＝[L D C]^T是拖曳体的气动力，μ是航迹滚转角，α是攻角，β是侧滑角，α和β合称气流角。在风扰动下，α＝α_k+α_w,β＝β_k+β_w，α_k,β_k是由航迹速度V_k产生的气流角分量，α_w,β_w是由风扰产生的气流角分量，V＝V_k+V_w表示空速，V_w表示由风扰产生的速度。

其中，p,q,r是角速度，

是气动力矩。

Q_A＝m_Ag+R^d/aF_A(V,α,β)+R^d/bT (11)

其中，R^d/a为从气流系到O_d-X_dY_dZ_d的转换矩阵，R^d/b是从机体系到O_d-X_dY_dZ_d的转换矩阵。

最终，通过将(11)计算出的Q_A代入(3)中，整个缆绳-拖曳体组合系统的动力学模型建立完成，拖曳体的六自由度动力学模型f_U，简写为：

其中，s为拖曳体状态向量，u为拖曳体控制输入向量，d为拖曳体受到的风扰动向量；

步骤三、利用缆绳动力学模型，建立基于缆绳位形与拖曳体的非约束力矢量和方向关系的拖曳体位置外环控制器，用于输出期望的拖曳体气流角；

拖曳体作为缆绳的末节点，其拖曳位置由缆绳形状决定，在缆绳长度保持不变且母机处于平稳飞行的情况下，缆绳的形状完全由拖曳体所受的非约束力决定，缆绳末端的方向由拖曳体的非约束力矢量和方向决定，缆绳的弯曲程度由拖曳体的非约束力矢量和幅值决定。对于拖曳体来说，非约束力矢量和方向比非约束力矢量和幅值大小更容易控制且对缆绳形状的影响更为明显。因此，提出基于非约束力矢量和方向的拖曳体位置控制方法，通过控制拖曳体的气流角，进而改变其气动力，进而实现对非约束力矢量和方向的控制，以达到对缆绳形状的改变，最终实现对拖曳体位置的控制。因此，建立缆绳位形与拖曳体非约束力矢量和方向之间的关系，并基于此，建立拖曳体位置控制器；具体步骤如下：

步骤301、基于缆绳动力学模型，建立拖曳体非约束力矢量和方向与拖曳体的垂向位置Z的映射关系，得到非约束力矢量和方向角ξ；

拖曳体的非约束力矢量和方向是指：作用在拖曳体上的除缆绳拉力这一约束力之外的其它非约束力(重力，气动力和推力等)的矢量和。经研究发现，在母机处于稳定飞行，并且缆绳长度不变的情况下，缆绳的形状完全由拖曳体的非约束力矢量和决定。

在拖曳体处于稳态时，基于缆绳动力学模型，建立如下映射关系式：

其中，|Q_A|为缆绳末端的非约束力矢量和Q_A的幅值，ξ是非约束力矢量和Q_A与拖曳体重力G_A＝m_Ag在拖曳坐标系的XOZ平面内的分量夹角，

是由拟合函数f_ξ(·)估计的夹角ξ稳态值；拟合函数f_ξ(·)通过插值拟合的方式获得，拟合数据可以通过仿真或者飞行实验获取。

步骤302、基于拖曳体的升阻特性，建立拖曳体的非约束力矢量和的幅值|Q_A|与非约束力矢量和方向角ξ之间的关系；

其中，F_AX(α)表示气动力在坐标系O_d-X_dY_dZ_d中X轴向的分量，T_dX(α)表示推力在坐标系O_d-X_dY_dZ_d中X轴向的分量；G_AZ表示拖曳体重力在坐标系O_d-X_dY_dZ_d中Z轴向的分量；Y轴向的侧力被忽略；因为假设拖曳体采用无侧滑飞行，即β^*＝0，因此侧力很小。

由式(14)可知，ξ和|Q_A|由α控制，在合理范围内，ξ随α的变化是单调的，但|Q_A|随α的变化关系未知。且ξ与拖曳体稳态的Z是一一对应的关系，因此，将拖曳体的外环位置控制转化为ξ控制，并将气流角作为内环，对ξ进行跟踪。

如图3所示，经仿真发现，ξ与Z之间的关系是接近线性的，因此，通过将拖曳体的位置控制转化为非约束力矢量和方向ξ控制，可以降低缆绳系统的非线性和耦合特性。

步骤303、使用拟合函数f_ξ(·)来估计在给定不同拖曳体位置的期望值Z^*以及推力T_d下，拖曳体的非约束力矢量和方向角估计值

为非约束力矢量和方向角ξ在稳态情况下的估计值；

其中，Z^*为Z的期望值；α₀表示拖曳体当前的攻角α；

约束条件为一元多次方程，在通常情况下，有实数解，并且

根据

求得；在无实数解的情况下，

被设置为当前值ξ₀：

当给定不同的期望Z向位置，或者发动机关机等因素导致推力发生变化时，可以利用上式估计出拖曳体最终稳定后的

因此，通过将

用于前馈控制，可以提升非约束力矢量和方向控制的响应速度。

步骤304、利用PID控制器抑制在拖曳体状态转换过程中的位置变化，并补偿非约束力矢量和方向角估计值

的估计误差；

其中，K_PY，K_IY和K_DY为Y向PID控制器的三个参数，K_PZ，K_IZ和K_DZ为Z向PID控制器的三个参数，μ^*为倾侧角指令，ξ_c为PID位置控制器产生的非约束力矢量和方向的指令，e_Y为Y向位置跟踪误差，主要由风扰引起，数值较小，采用倾侧角μ对其进行控制，并设置β^*＝0。倾侧方式比侧滑方式可以更快地消除侧偏，并且由于β^*＝0，可以减少侧力对缆绳-拖曳体组合系统的影响。e_Z为Z向位置跟踪误差。

步骤305、将非约束力矢量和方向角ξ^*转化为攻角指令α^*；

与α^*之间的转换公式为：

其中，α_l和α_u表示拖曳体攻角α的下边界和上边界；

约束条件是一元多次方程，通常情况下具有实数解；否则，α^*被设置为：

步骤306、构成了拖曳体基于非约束力矢量和方向的位置控制器；并输出期望气流角回路的指令Ω^*＝[α^* 0 μ^*]。

步骤四、基于固定时间控制理论和拖曳体的六自由度动力学模型，设计内环(气流角和角速度回路)的抗扰动控制器，并输入外环位置控制器产生的期望气流角，快速跟踪并控制拖曳体最终的实际位置；

由于期望位置的变化或者发动力推力发生变化，外环产生的非约束力矢量和方向指令会发生较大的变化，进而要求内环的气流角可以较快地跟踪指令信号，以减小在过渡过程中的拖曳体飘摆现象，因此，基于固定时间控制理论，设计内环(气流角和角速度回路)的抗扰动控制器，对外环基于非约束力矢量和方向的位置控制器产生的气流角指令进行准确快速跟踪，并对存在的多重风扰动和参数摄动引起的扰动进行补偿。具体步骤如下：

步骤401、基于时标分离原理和奇异摄动理论，充分考虑模型参数不确定性和风扰动，将期望的气流角和角速度的非线性动力学方程组，改写为利于控制器设计的仿射非线性形式；

改写公式为：

其中，ω＝[p,q,r]^T为拖曳体的角速度，δ＝[δ_a,δ_e,δ_r]^T分别表示拖曳体的副翼，升降舵和方向舵的偏转角，f_Ω表示拖曳体气流角的集总扰动(包含风扰和参数摄动)，f_ω表示拖曳体角速度的集总扰动，B_Ω表示气流角的控制矩阵，B_ω表示角速度的控制矩阵。

步骤402、基于气流角和角速度的仿射非线性模型，分别设计各自对应的固定时间控制器和固定时间扩张状态观测器(FXESO)；根据气流角和角速度固定时间控制器输出舵偏角。

设计气流角回路的固定时间控制器为如下形式：

其中，

为对集总扰动f_Ω的估计值，e_Ω＝Ω-Ω^*为气流角Ω的跟踪误差，

表示由跟踪微分器(TD)对期望气流角

的估计值，η_Ωc1∈(0,1)，τ_Ωc1＞1，K_Ωci＞0(i＝1,2)为待设计的控制器参数。

设计气流角回路的固定时间扩张状态观测器为如下形式：

其中，

表示气流角Ω的估计误差，η_Ωo1∈(0,1)，τ_Ωo1＞1，η_Ωo2＝2η_Ωo1-1，

以及K_Ωoi＞0(i＝1,...,4)为待设计的观测器参数。

设计角速度回路的固定时间控制器为如下形式：

其中，

为集总扰动f_ω的估计值，e_ω＝ω-ω^*为角速度ω的跟踪误差，

表示由跟踪微分器对期望角速度

的估计值，η_ωc1∈(0,1)，τ_ωc1＞1，K_ωci＞0(i＝1,2)为待设计的控制器参数。

设计角速度回路的固定时间扩张状态观测器为如下形式：

其中，

表示角速度ω的估计误差，η_ωo1∈(0,1)，τ_ωo1＞1，η_ωo2＝2η_ωo1-1，τ_ωo2＝2τ_ωo1-1，

以及K_ωoi＞0(i＝1,...,4)为待设计的观测器参数。

步骤403、将舵偏角输入到缆绳动力学模型中，计算出缆绳末端实际的非约束力矢量和方向，并进一步计算出缆绳的实际位形，最后得到拖曳体的实际位置，并在下个周期内对位置偏差进行调整。

经过上述设计，总体的控制框图如图4所示。

为检验本发明的有效性与较传统拖曳体位置控制方法的优越性，以某型缆绳-拖曳体组合系统为例，进行仿真验证。缆绳-拖曳体组合系统的动力学模型参数如表1所示：

表1

控制器参数如表2所示：

表2

依据本发明的具体实施步骤，首先给出了风扰动情况，如图5所示，主要包含大气紊流和母机尾涡两部分；接着，验证在拖曳体发动机关机(推力消失)情况下，不同控制方法的响应情况：

控制方法1：气流角稳定控制，无位置控制；

控制方法2：内外回路均采用传统自抗扰控制；

控制方法3：外回路采用非约束力矢量和方向控制，内回路采用传统自抗扰控制；

本发明方法：外回路采用非约束力矢量和方向控制，内回路采用固定时间抗扰动控制；

发动机关机会导致推力骤然消失，对缆绳-拖曳体组合系统造成快时变的大扰动，如图6所示，发动机在50s时刻关闭，相比于其它三种方法，本发明所提方法在Y向和Z向上的变化范围最小，并且收敛时间最快，YZ平面的轨迹变化如图7所示，本发明可见所提方法的受扰动变化范围最小，验证了本发明方法对快变扰动的抵抗能力。

图8a和图8b展示了在参数摄动正负10％的情况下，不同控制方法的响应情况，可以看出，本发明所提方法依然具有最小的运动范围和最短的收敛时间，验证了本发明方法对参数摄动的鲁棒性。

图9给出了给定期望位置情况下，不同控制方法的跟踪情况，可以看出本发明方法跟踪最快，误差最小，验证了本发明方法的跟踪性能。

综合上述对实施例的仿真验证，证明了本发明一种基于非约束力矢量和方向控制的拖曳体位置控制方法的有效性。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (3)

1.一种基于非约束力矢量和方向控制的拖曳体位置控制方法，其特征在于，具体包括如下步骤：

步骤一、基于多刚体建模方法，建立考虑拖曳体外力的缆绳动力学模型；

将缆绳作为多刚体结构，并离散为N个刚体连杆，在拖曳坐标系O_d-X_dY_dZ_d下建立缆绳的多刚体动力学模型f_C，如下：

其中，p表示缆绳各个节点的三轴位置的矩阵；F表示缆绳各个节点所受的气动力矩阵；Q_A表示拖曳体所受到的非约束力矢量和向量；

步骤二、考虑缆绳拉力及风扰动影响，建立拖曳体的六自由度动力学模型；

拖曳体的六自由度动力学模型f_U，简写为：

其中，s为拖曳体状态向量，u为拖曳体控制输入向量，d为拖曳体受到的风扰动向量；

步骤三、利用缆绳动力学模型，建立基于缆绳位形与拖曳体的非约束力矢量和方向关系的拖曳体位置外环控制器，用于输出期望的拖曳体气流角；

步骤四、基于固定时间控制理论和拖曳体的六自由度动力学模型，设计内环的抗扰动控制器，并输入期望的拖曳体气流角，快速控制拖曳体最终的实际位置；

具体步骤如下：

步骤401、基于时标分离原理和奇异摄动理论，将拖曳体的六自由度动力学模型中的气流角和角速度回路，改写为利于控制器设计的仿射非线性形式；

改写公式为：

其中，ω＝[p,q,r]^T为拖曳体的角速度，δ＝[δ_a,δ_e,δ_r]^T分别表示拖曳体的副翼，升降舵和方向舵的偏转角，f_Ω表示拖曳体气流角的集总扰动，f_ω表示拖曳体角速度的集总扰动，B_Ω表示气流角的控制矩阵，B_ω表示角速度的控制矩阵；

步骤402、基于气流角和角速度的仿射非线性模型，分别设计各自对应的固定时间控制器和固定时间扩张状态观测器；根据气流角和角速度固定时间控制器输出舵偏角；

步骤403、将舵偏角输入到拖曳体动力学模型中，计算出缆绳末端实际的非约束力矢量和方向，并根据缆绳动力学模型进一步计算出缆绳的实际位形，最后得到拖曳体的实际位置，并在下个周期内对位置偏差进行调整。

2.如权利要求1所述的一种基于非约束力矢量和方向控制的拖曳体位置控制方法，其特征在于，所述的步骤三具体为：

步骤301、基于缆绳动力学模型，建立拖曳体非约束力矢量和方向与拖曳体的垂向位置Z的映射关系，得到非约束力矢量和方向角ξ；

建立如下映射关系式：

其中，|Q_A|为缆绳末端的非约束力矢量和的幅值，ξ是非约束力矢量和Q_A与拖曳体重力G_A在拖曳坐标系的XOZ平面内的分量夹角，

是由拟合函数f_ξ(·)估计的夹角ξ稳态值；

步骤302、基于拖曳体的升阻特性，建立拖曳体的非约束力矢量和的幅值|Q_A|与非约束力矢量和方向角ξ之间的关系；

其中，F_AX(α)表示气动力在坐标系O_d-X_dY_dZ_d中X轴向的分量，T_dX(α)表示推力在坐标系O_d-X_dY_dZ_d中X轴向的分量；G_AZ表示拖曳体重力在坐标系O_d-X_dY_dZ_d中Z轴向的分量；

步骤303、使用拟合函数f_ξ(·)来估计在给定不同拖曳体位置的期望值Z^*以及推力T_d下，拖曳体的非约束力矢量和方向角估计值

为非约束力矢量和方向角ξ在稳态情况下的估计值；

其中，α₀表示拖曳体当前的攻角α，在无实数解的情况下，

被设置为当前值ξ₀；

步骤304、利用PID控制器抑制在拖曳体状态转换过程中的位置变化，并补偿非约束力矢量和方向角估计值

的估计误差；

其中，K_PY，K_IY和K_DY为Y向PID控制器的三个参数，K_PZ，K_IZ和K_DZ为Z向PID控制器的三个参数，μ^*为倾侧角指令，ξ_c为PID位置控制器产生的非约束力矢量和方向指令，e_Y为Y向位置跟踪误差，主要由风扰引起，数值较小，e_Z为Z向位置跟踪误差；

步骤305、将非约束力矢量和方向角ξ^*转化为攻角指令α^*；

与α^*之间的转换公式为：

其中，α_l和α_u表示拖曳体攻角α的下边界和上边界；

步骤306、输出拖曳体基于非约束力矢量和方向的期望气流角Ω^*＝[α^* 0 μ^*]。

3.如权利要求1所述的一种基于非约束力矢量和方向控制的拖曳体位置控制方法，其特征在于，所述的步骤402中，气流角回路的固定时间控制器为如下形式：

其中，

为对集总扰动f_Ω的估计值，e_Ω＝Ω-Ω^*为气流角Ω的跟踪误差，

表示由跟踪微分器对期望气流角

的估计值，η_Ωc1∈(0,1)，τ_Ωc1＞1，K_Ωci＞0(i＝1,2)为待设计的控制器参数；

气流角回路的固定时间扩张状态观测器为如下形式：

其中，

表示气流角Ω的估计误差，η_Ωo1∈(0,1)，τ_Ωo1＞1，η_Ωo2＝2η_Ωo1-1，τ_Ωo2＝2τ_Ωo1-1，

以及K_Ωoi＞0(i＝1,...,4)为待设计的观测器参数；

角速度回路的固定时间控制器为如下形式：

其中，

为集总扰动f_ω的估计值，e_ω＝ω-ω^*为角速度ω的跟踪误差，

表示由跟踪微分器对期望角速度

的估计值，η_ωc1∈(0,1)，τ_ωc1＞1，K_ωci＞0(i＝1,2)为待设计的控制器参数；

角速度回路的固定时间扩张状态观测器为如下形式：

其中，

表示角速度ω的估计误差，η_ωo1∈(0,1)，τ_ωo1＞1，η_ωo2＝2η_ωo1-1，τ_ωo2＝2τ_ωo1-1，

以及K_ωoi＞0(i＝1,...,4)为待设计的观测器参数。

Images