CN108388270B - 面向安全域的集群无人机轨迹姿态协同控制方法 - Google Patents

Abstract

本发明涉及无人机飞行器控制技术，为提出无人机轨迹姿态协同控制方法，实现有效避免传统集群无人机为每架无人机规划轨迹的不足，能够有效弱化对无人机定位精度的要求，为此，本发明面向安全域的集群无人机轨迹姿态协同控制方法，步骤如下：第一部分，面向控制的四旋翼无人机轨迹姿态模型建立：充分考虑四旋翼无人机的固有特性及飞行中的动力学因素，建立四旋翼无人机的轨迹姿态数学模型；第二部分，集群中心轨迹规划：实现基于伪谱法的集群无人机中心点的避障航路规划；第三部分，安全飞行区域优化：完成集群无人机安全飞行包线及集群无人机最优编队构型的优化；第四部分，分布式协同控制器设计。本发明主要应用于无人机飞行器控制场合。

Description

面向安全域的集群无人机轨迹姿态协同控制方法

技术领域

本发明涉及无人机飞行器控制技术领域，尤其涉及集群无人机轨迹姿态协同控制领域。

背景技术

无人机(Unmanned Aerial Vehicle,UAV)是指无需驾驶员直接操纵，可通过自主或远程控制完成飞行行为的空中机器人系统。早期无人机主要用于军事用途，近年来逐渐推广至农林业植保、电力巡检、货物递送、地质勘探、环境监测、及森林防火救火等越来越多的民用领域。无人机与有人机相比，主要优势在于以下两点：由于机上没有驾驶员，因此可以省去驾驶舱及相关设备，降低无人机的重量和制造成本；无人机不受驾驶员的心理和生理极限等约束，可最大限度地飞到适合其性能特点及任务需求的速度、高度、航程等，也可通过超加速升降、急转弯飞行等方式提高生存能力。因此，无人机可在超出人类所能承受的危险和恶劣环境中使用。在过去的几十年中，无人机技术取得了极大进展，各种类型的无人机纷纷研制成功，性能逐步提高并已经成功地应用于部分军用及民用领域。无人机自主化是无人机的发展趋势，势必推动无人机更广泛、更深层次的应用。

多无人机协同编队的灵感来源于自然界中鱼群及鸟群的集群行为，其显著特点是自组织、自协调、并行性、强鲁棒及智能涌现。多无人机在执行任务时，每架无人机搭载不同的传感器，无人机之间共享信息、互相配合并各司其职，整个编队具备很强的鲁棒自愈能力和故障冗余度，即使失去一部分个体也不会对整体造成致命影响。相比单个无人机，多无人机的主要优势主要体现在以下几个方面。(1)功能分布：将单个完备平台所具备的各项功能如侦察监视、电子干扰、打击与评估等能力化整为零，分散到大量低成本、功能单一的无人系统，通过大量异构、异型的个体来实现原本复杂的系统功能，系统的倍增效益将使无人机编队具备远超单一平台的作战能力；(2)去中心化：目前无人机的通信模式仍然以单机与地面站通信方式为主，信息传输仍是集中式的，去中心化的多无人机利用自组网技术可以实现无人机之间信息的高速共享，同时提高多无人机的抗故障能力及高效信息共享能力；(3)分布式探测：广泛分布传感器的能力对于主动与被动探测以及定位精度有显著优势，多无人机可以相互协作实现目标的高精度定位，当需要主动探测时，无人机间还可采取频率、波段不同的雷达进行全频谱探测，将极大提高探测能力。

目前，国内外相关学者提出的集群无人机轨迹姿态协同控制方法主要有Leader-Follower方法、基于行为法和虚拟结构法。1)Leader-Follower法：Leader-Follower方法是目前多无人机协同控制中最常用的方法之一。Leader跟踪一个预先给定的轨迹，Follower和Leader轨迹保持一定构型，并要求速度达到一致，Leader可以看成是目标追踪的对象。该方法将协同问题转化为了经典控制中的误差跟踪问题，具有较强的扩展性，但在一些特定情况下，这种依赖于单个目标体状态的协同方法很难达到预期效果。2)基于行为法：基于行为的多无人机协同方法，事先定义无人机的几种基本控制行为，如跟随、避障和队形构成等，对定义的几种行为进行加权得到协同控制方法。这使系统中的每个单体都具备依据自身决策，协同其他单体完成目标或任务的能力。在传感器数据错误或缺乏的情况下，基于行为法的多无人机能够综合考虑多无人机协同飞行中的多个行为，有效整合各种行为，从而使整个集群无人机仍然可以保持一定的构型。该方法根据预设信息和触发条件来形成控制指令，在一定程度上降低了集群无人机的适应性和灵活性。3)虚拟结构法：虚拟结构法是一种集中式控制方法，由美国加利福尼亚大学学者在1997年首次提出。该方法，将集群无人机作为一个虚拟刚体，在集群无人机中设定一个虚拟长机或虚拟几何中心，集群中所有无人机都参照虚拟长机或虚拟几何中心运动。虚拟结构法避免了领航跟随方法中领航无人机出现故障或毁机时，集群无法保持的缺点。而且该方法通过把集群误差反馈到控制器，得到了较高的协同控制精度。然而，合成虚拟长机和获取邻机位置，需要较高的通信质量和强计算能力，这使得集群可靠性较差。

综上所述，集中式控制思想的局限性是每架无人机都需要知道其它所有无人机的起始位置和目标位置，以此来规划自身轨迹。因此，计算复杂度随障碍物数量和无人机数量急剧增加。然而，使每架无人机在没有全局信息的情况下只依赖邻机信息实现集群协同尤为重要，因为只有这样才能使协同控制不受集群规模影响。

发明内容

为克服现有技术的不足，本发明旨在提出不同于以往传统集群无人机编队协同的面向安全域的集群无人机轨迹姿态协同控制方法，该方法只对集群无人机中心点的轨迹进行规划设计，可有效避免传统集群无人机为每架无人机规划轨迹的不足，集群无人机的避障通过无人机安全飞行区域的实时优化得到；在集群无人机的编队控制上，采用基于分布式的协同控制策略，基于该策略构建的协同控制器只依赖于无人机间的相对位置信息，能够有效弱化对无人机定位精度的要求。为此，本发明采用的技术方案是，面向安全域的集群无人机轨迹姿态协同控制方法，步骤如下：

第一部分，面向控制的四旋翼无人机轨迹姿态模型建立：充分考虑四旋翼无人机的固有特性及飞行中的动力学因素，建立四旋翼无人机的轨迹姿态数学模型；

第二部分，集群中心轨迹规划：实现基于伪谱法的集群无人机中心点的避障航路规划；

第三部分，安全飞行区域优化：完成集群无人机安全飞行包线及集群无人机最优编队构型的优化；

第四部分，分布式协同控制器设计：设计分布式协同控制器，使得集群无人机按照上述设计的期望构型飞行，相关技术包括外环分布式位置控制器、姿态解算及内环的姿态控制器设计。

一个实例中的具体步骤如下：

以第i架无人机为例，其余无人机以此类推：

第一步、建立四旋翼无人机的轨迹姿态数学模型如下

其中，p_i＝[x_i,y_i,z_i]∈R³表示惯性坐标系下三个坐标轴方向的位置，

表示惯性坐标系下x,y,z三个方向的线速度，Θ_i＝[φ_i,θ_i,ψ_i]∈R³表示惯性坐标系下的滚转角、俯仰角及偏航角，

表示机体坐标系下的滚转角速率、俯仰角速率及偏航角速率，g＝9.8m/s²表示地球重力加速度，e_z＝[0,0,1]表示常值向量，m表示四旋翼无人机质量，τ_F表示无人机的总升力，

表示无人机三个方向的控制力矩，I＝diag{I_x,I_y,I_z}表示无人机惯性常值矩阵，△₁和△₂分别表示位置子系统和速度子系统收到的模型参数不确定及外界干扰的总和，旋转矩阵R_i和变换矩阵W_i定义如下

控制量

与电机转速

之间的关系如下：

其中，d表示旋翼转动中心到无人机中心的距离，k_F,k_M分别表示旋翼的升力系数和扭矩系数；

第二步，集群中心轨迹规划，具体实现过程如下：

将集群无人机的中心点，考虑成质点，对公式(8)和(9)所描述的质点运动模型，以飞行时间最短为优化目标，将飞行过程中的避障约束考虑为路径约束，利用伪谱法进行航路规划设计，为阐述算法的实现过程，将集群无人机中心点的航路规划问题，归纳为如下形式的最优控制问题求解：

Ψ(x(τ₀),x(τ_f)；t₀,t_f)＝0 (12)

C(x(τ),u(τ),τ；t₀,t_f))≤0 (13)

其中(14)～(15)分别为无人机中心点航路规划的目标函数、微分方程约束、包括起始点、路径点及终点约束的边值约束及包括飞行过程中的障碍物约束及状态约束的路径约束，x和u分别表示集群无人机质点的状态量和虚拟控制量，t₀和t_f分别表示航路规划的起始时间和终止时间，基于伪谱法对问题进行离散处理时，选取的离散点个数为(N+2)，记为τ₀,τ₁,…,τ_N,τ_f，其中τ₀＝-1,τ_f＝1，τ_k(1≤k≤N)为如下N阶Legendre多项式P_N(τ)的零点，

基于上述定义，状态变量和控制变量通过下式进行逼近：

式中，L_i(τ)和

分别表示以τ_i(0≤i≤N)和τ_k(1≤k≤N)为节点的拉格朗日插值基函数，定义如下：

由式(19)～(15)不难发现，基于拉格朗日插值多项式逼近的状态和控制在插值节点处与实际的状态和控制是相等的，即：x(τ_i)＝X(τ_i),u(τ_k)＝U(τ_k)。注意到式(20)，并没有包含终端状态约束，而对于无人飞行器的航迹规划问题，其终端状态约束是必须要满足的，利用伪谱法对问题进行离散时，终端状态束通过下式求得：

式中，ω_k表示高斯型求积公式的权系数，计算公式如下：

其中，

表示N阶Legendre多项式的微分。进一步，对式(21)进行求导，获得下式：

式中拉格朗日多项式在零点处的微分，通过微分矩阵D∈R^N×(N+1)求得，当插值节点个数给定时，该矩阵为一常值，矩阵D的计算公式如下：

经过上述变换，最优控制问题的微分约束(22)可转化为如下形式的代数约束：

终端状态的微分约束通过式(16)近似，集群无人机中心点边值约束(23)，路径约束(24)可利用插值节点处的约束进行逼近，得到下式：

Ψ(X₀,t₀,X_f,t_f)＝0 (21)

C[(X_k,U_k,τ_k；t₀,t_f)]≤0 (22)

进一步，利用高斯求积公式，对航路规划问题的目标函数进行逼近，获得下式：

经过上述转化，集群无人机中心点航路规划问题(25)～(26)的求解可转化为对下述非线性规划问题的求解：求插值节点处的状态变量X(τ_k)(k＝0,...,N)、X(τ_f)，控制变量U(τ_k)(k＝1,...,N)及初始时刻t₀和终端时刻t_f，使得系统轨迹在满足终端状态约束(16)，动力学方程约束(20)，边值约束(21)和路径约束(22)的条件下，性能指标(23)最优。最后，利用SNOPT求解器可对上述构建的航路规划问题进行优化求解，获得满足所有约束的可行飞行航路；

第三步，安全飞行区域优化，实现步骤具体如下：

步骤1：基于获得的路径点，使用半正定规划进行迭代的区域扩张，以初始路径点为当前无人机编队中心位置，以下一个路径点为当前目标点g，在自由空间中找到包含所有无人机位置和目标点的最大安全飞行区域

包含所有无人机位置的中心和目标点的另一个安全飞行区域P_o→g,取两个安全飞行区域的交集

即为无人机队形变换的安全区域；

步骤2：找到安全区域后，即可以将安全区域P表示为等价的线性约束集合

其中A表示线性约束矩阵，b表示线性约束向量，n_l表示多面体P的面数，进而使用序列凸优化进行队形规划，为此我们首先定义f∈R个默认队形，如正方形，一字形或三角形，将其中任意一个默认队形表示为

编队

包括一个无人机位置的集合

和一个相对于编队中心的顶点集合

则任意队形Fⁱ中的无人机位置和顶点可以表示为

其中t∈R₃表示无人机编队中心的位置，s∈R₊表示队形扩张的大小，q表示一个单位四元数，其共轭表示为

rot表示旋转函数，具体表达式如下

将所有优化变量表示为一个向量x_i＝[t,s,q]∈R⁸，定义目标函数为当前编队参数到目标点g，到期望编队大小

及到旋转四元数

这三个误差的加权求和

其中w_t,w_s,w_q为权重，c_i为队形i的指定代价，定义编队顶点

的安全区域边界约束

总的安全区域边界约束

单位四元数q约束C₂＝{||q||²＝1}。则非线性优化问题可以表示为

为使目标函数C(x_i)取得最小值的最优解，则最优队形

的指标i^*可以表示为

使用稀疏非线性优化器SNOPT求解上述优化问题，求得最优队形

之后返回步骤1，将目标点更新为下一个路径点，将无人机位置更新为优化求得的新位置，重新计算安全飞行区域并在新安全飞行区域中规划新队形。循环执行上述步骤直到优化出最后一个目标队形；

第四步，分布式协同控制器设计具体地：

A)分布式位置控制器

考虑n架集群无人机系统，点集V＝{1,2,…,n}表示无人机编号的集合，G(V,ε,W)表示无向通信拓扑，ε是无人机之间通信关系边的集合，W＝[w_ij]是权重邻接矩阵，邻接矩阵定义为w_ii＝0且w_ij≥0(i≠j)，当且仅当无人机i可以从无人机j接收信息时w_ij>0，利用自身和邻机相对状态及无人机之间的拓扑链接关系，定义如下的相对增广位置跟踪误差：

其中p_i和p_j表示第i架无人机和第j架无人机的位置矢量信息，

和

表示第i架无人机和第j架无人机期望的位置矢量信息，其值可通过上述集群无人机的优化构型获得。基于上述定义，设计第i架无人机的分布式控制器：

其中，

表示用于形成期望无人机构型的虚拟控制输入，K_p,K_I,K_d为常值向量，表示控制器增益，

和

分别为势函数控制器产生的避碰与避障控制量，具体实现过程如下：

首先定义无人机i的避碰集合为

D_i＝{j∈V:d_ij≤R_a,i≠j} (26)

其中d_ij＝||p_i-p_j‖表示第i架无人机和第j架无人机的相对距离，R_a表示最大碰撞探测距离，定义无人机i和j之间的势函数为

其中∈_a∈R为任意小的正数，0<ρ_a<R_a且η_j>0。则无人机i的避碰控制量为

其中

当无人机与邻机发生冲突时，判断目标方向和冲突方向之间的夹角α，当夹角α小于30°时，将势能函数项产生的控制指令方向扭转30°，以此避免了无人机之间冲突与运动方向的矛盾；

则避障控制量的形式变为

其中

表示势能函数方向的扭转。类似地，编队队形规划中的安全区域约束表示为

定义第i架无人机指向障碍物平面的向量为：

则第i架无人机的避障控制量为

其中d_min为防止避障势函数饱和而引入的无人机到障碍物平面的最小距离，基于以上步骤，完成了外环分布式位置控制器的设计；

B)姿态解算算法

由于四旋翼无人机无法通过控制力矩直接对飞行轨迹进行控制，因此需要将外环获得的虚拟控制指令转化为期望的飞行姿态，具体解算公式为

其中

表示向量

的第j个元素；

C)基于干扰补偿的有限时间姿态控制器

为了实现对第i架无人机期望姿态的有效跟踪，定义第i架无人机的期望姿态跟踪误差e_i1＝Θ_i-Θ_iref，姿态角速率跟踪误差

则第i架无人机姿态跟踪误差动态方程满足如下形式

基于上述定义，后续控制任务可归纳为：设计虚拟控制力矩

使得(37)中的姿态跟踪误差e_i1收敛到零，为了实现该目的，设计如下控制律

其中λ₁,λ₂为任意正常值，ρ₁,ρ₂为介于0到1之间的正常值，滑模面s_i1定义如下：

式(38)中的k₁,k₂满足如下条件：

其中L是公式(27)中不确定△₂的上界，即满足条件‖△₂‖≤L。基于(37)，(38)中的虚拟控制力矩

可获得实际控制力矩

根据(28)，升力

实际控制力矩

可获得无人机四个旋翼的旋转角速度，计算公式如下

本发明的特点及有益效果是：

本发明通过设计集群无人机中心点轨迹、安全飞行区域及分布式协同控制策略，确保集群无人机由指定的起点，能够安全、无碰撞地绕过障碍物朝着各自目标区域飞行，每架无人机在没有全局信息的情况下只依赖邻机信息实现协同。

在集群无人机的编队控制上，本发明采用了基于分布式的协同控制策略，基于该策略构建的协同控制器只依赖于无人机间的相对位置信息，有效弱化了对无人机定位精度的要求。随后通过设置飞行场景，对本发明提出策略的有效性进行验证。

附图说明：

附图1集群无人机飞行场景示意图。

附图2集群无人机中心点航路规划。

附图3集群无人机安全飞行区域迭代优化求解示意图。

附图4 20架无人机编队飞行示意图。

附图5期望构型跟踪误差与姿态跟踪误差。

附图6无人机总升力与实际控制力矩曲线。

附图7集群无人机轨迹姿态协同控制结构框图。

附图8基于势能函数的集群无人机避碰策略示意图。

附图9基于势能函数的集群无人机避障策略示意图。

具体实施方式

针对现有控制策略的不足，本发明使用分布式思想，首次提出了面向安全域的集群无人机轨迹姿态协同控制方法。该方法通过设计集群无人机中心点轨迹、安全飞行区域及分布式协同控制策略，确保集群无人机由指定的起点，能够安全、无碰撞地绕过障碍物朝着各自目标区域飞行，每架无人机在没有全局信息的情况下只依赖邻机信息实现协同。

本发明涉及四旋翼无人机飞行控制技术领域。具体来说，首先提出了不同于以往传统集群无人机编队协同的面向安全域的集群无人机轨迹姿态协同控制方法，该方法只对集群无人机中心点的轨迹进行规划设计，可有效避免传统集群无人机为每架无人机规划轨迹的不足，集群无人机的避障通过无人机安全飞行区域的实时优化得到；在集群无人机的编队控制上，本发明采用了基于分布式的协同控制策略，基于该策略构建的协同控制器只依赖于无人机间的相对位置信息，有效弱化了对无人机定位精度的要求。随后通过设置飞行场景，对本发明提出策略的有效性进行验证。

备注：由于目前尚无关于集群无人机安全域的标准定义，课题在深入分析国内外相关资料的基础上，给出如下定义。安全域：满足集群无人机所有约束条件的安全飞行走廊与无避碰区域的集合。

本发明的目的在于提出一种面向安全域的集群无人机轨迹姿态协同控制方法，推动集群无人机在协同防御、协同侦查、协同打击、通信窃听及应急救援等重要军事领域的创新应用，建立集群无人机自主协同控制优化理论与应用方法，为突破集群无人机智能自主协同控制发展的瓶颈、促进无人机集群化与智能化发展水平，提供科学理论支撑。该方法可以在多约束的空间内，生成满足所有约束的安全飞行走廊及最优编队构型，确保集群无人机由指定的起点，能够安全、无碰撞地绕过障碍物朝着各自目标区域飞行。同时，该方法具有良好的抗干扰能力，可以在模型参数不确定及外界干扰存在的情况下实现集群无人机轨迹姿态的高精度协同控制。本发明提出的策略，通过设计集群无人机中心点轨迹、安全飞行区域及分布式协同控制策略，实现集群无人机从初始位置到目标区域的安全飞行。

本发明以理论推导和虚拟仿真技术相结合为主要研究手段，提出一种面向安全域的集群无人机轨迹姿态协同控制方法，该方法能够确保集群无人机由指定的起点，无碰撞地绕过障碍物朝着各自目标区域飞行。

本发明提出的面向安全域的集群无人机轨迹姿态协同控制方法，主要包括以下四部分：

第一部分，面向控制的四旋翼无人机轨迹姿态模型建立：充分考虑四旋翼无人机的固有特性及飞行中的动力学因素，建立四旋翼无人机的轨迹姿态数学模型。

第二部分，集群中心轨迹规划：该部分主要实现基于伪谱法的集群无人机中心点的避障航路规划。

第三部分，安全飞行区域优化：该部分主要完成集群无人机安全飞行包线及集群无人机最优编队构型的优化。

第四部分，分布式协同控制器设计：该部分主要目的是设计分布式协同控制器，使得集群无人机按照上述设计的期望构型飞行，相关技术包括外环分布式位置控制器、姿态解算及内环的姿态控制器设计。

最后，为了验证本发明提出的面向安全域的集群无人机轨迹姿态协同控制方法的有效性，在MATLAB\Simulink仿真平台上搭建集群无人机轨迹姿态协同控制系统，通过设置飞行场景，对本发明提出策略的有效性进行验证。

本发明提出的面向安全域的集群无人机轨迹姿态协同控制方法，在MATLAB/Simulink环境下完成了集成设计及验证，具体过程如下：

(1)参数设置

1)四旋翼无人机物理参数：质量m＝1.1kg，惯性参数I_x＝1.431×10^-2kgm²，I_y＝1.431×10^-3kgm²，I_z＝2.721×10^-3kgm²，旋翼转动中心到无人机中心的距离d＝0.45m，旋翼的升力系数k_F＝9.832×10^-6Nms²/rad²,旋翼的扭矩系数k_M＝1.32×10^-7Ns²/rad²。

2)控制器参数：分布式位置控制器K_p＝[0.09；0.09；0.02]，K_i＝[0.02；0.02；0.02]，K_d＝[0.01；0.01；0]，ρ_a＝0.3,R_a＝0.6,η_j＝1,∈_a＝0.25,d_min＝0.1m。基于干扰补偿的有限时间姿态控制器k₁＝9,k₂＝18,λ₁＝5,λ₂＝4,ρ₁＝0.6,ρ₂＝0.8。

3)其它参数设置：仿真测试验证过程中，采样时间设置为定步长20毫秒，不确定干扰通过时变函数△₁＝△₂＝0.5[1+sin(t),1+cos(t),1+0.5(sin(t)+cos(t))]^T模拟。

(2)仿真结果及性能分析

以20架四旋翼无人机为例，对上述提出的集群无人机中心点轨迹规划算法、安全飞行区域优化算法及分布式协同控制器的有效性进行数值仿真验证。基于本发明提出的策略，通过设定飞行场景(如图1)，在MATLAB环境中，利用本发明提出的算法对飞行场景所示的集群无人机航路规划问题进行求解，获得中心点的避障航路规划如图2所示，中心点航路规划求解时间约为0.5秒，能够很好的满足无人机航路规划实时性要求。基于本发明给出的算法，优化求解得到的集群无人机安全飞行区域如图3所示，仿真结果表明，集群无人机可在事先规划的路径点上，快速获得满足避障要求的安全飞行区域及最优构型。用于20架四旋翼无人机集群飞行的飞行示意图如图4所示，从中可以看出基于本发明给出的策略，能确保集群无人机在进入狭窄飞行区域时，实现多无人机的自主避碰与避障飞行。进一步，以第1架无人机和第2架无人机的期望构型为例，图5给出了x,y,z三个方向，集群无人机飞行过程中能够实现的轨迹跟踪精度(稳态误差)优于10^-2，以第1架无人机为例，滚转、俯仰及偏航通道的姿态跟踪精度优于0.1度，以第一架无人机为例，飞行过程中的总升力与实际控制力矩如图6所示。

下面结合附图对本发明作进一步详述。

面向安全域的集群无人机轨迹姿态协同控制策略总体结构图如图8所示。具体实现步骤如下：

第一步，面向控制的四旋翼无人机姿态模型建立。四旋翼无人机对称分布在机体的前后、左右四个方向，四个旋翼处于同一高度平面，四旋翼飞行器通过调节四个电机转速来改变旋翼转速，实现升力的变化，从而控制飞行器的姿态和位置。充分考虑四旋翼无人机的固有特性，无人机在飞行过程中的动力学因素及受力平衡关系，以第i架无人机为例，建立四旋翼无人机的轨迹姿态数学模型如下

其中，p_i＝[x_i,y_i,z_i]∈R³表示惯性坐标系下三个坐标轴方向的位置，

表示惯性坐标系下x,y,z三个方向的线速度，Θ_i＝[φ_i,θ_i,ψ_i]∈R³表示惯性坐标系下的滚转角、俯仰角及偏航角，

表示机体坐标系下的滚转角速率、俯仰角速率及偏航角速率。g＝9.8m/s²表示地球重力加速度，e_z＝[0,0,1]表示常值向量，m表示四旋翼无人机质量，τ_F表示无人机的总升力，

表示无人机三个方向的控制力矩，I＝diag{I_x,I_y,I_z}表示无人机惯性常值矩阵，△₁和△₂分别表示位置子系统和速度子系统收到的模型参数不确定及外界干扰的总和，旋转矩阵R_i和变换矩阵W_i定义如下

控制量

与电机转速

之间的关系如下：

其中，d表示旋翼转动中心到无人机中心的距离，k_F,k_M分别表示旋翼的升力系数和扭矩系数。

第二步，集群中心轨迹规划。该部分主要包括集群无人机中心点的避障航路规划，具体实现过程如下：

将集群无人机的中心点，考虑成质点，对公式(36)和(37)所描述的质点运动模型，以飞行时间最短为优化目标，将飞行过程中的避障约束考虑为路径约束，利用伪谱法进行航路规划设计。为阐述算法的实现过程，将集群无人机中心点的航路规划问题，可归纳为如下形式的最优控制问题求解：

Ψ(x(τ₀),x(τ_f)；t₀,t_f)＝0 (40)

C(x(τ),u(τ),τ；t₀,t_f))≤0 (41)

其中(42)～(43)分别为无人机中心点航路规划的目标函数、微分方程约束、边值约束(起始点、路径点及终点约束)及路径约束(飞行过程中的障碍物约束及状态约束等)，x和u分别表示集群无人机质点的状态量和虚拟控制量，t₀和t_f分别表示航路规划的起始时间和终止时间。基于伪谱法对问题进行离散处理时，选取的离散点个数为(N+2)，记为τ₀,τ₁,…,τ_N,τ_f。其中τ₀＝-1,τ_f＝1，τ_k(1≤k≤N)为如下N阶Legendre多项式P_N(τ)的零点。

基于上述定义，状态变量和控制变量可通过下式进行逼近：

式中，L_i(τ)和

分别表示以τ_i(0≤i≤N)和τ_k(1≤k≤N)为节点的拉格朗日插值基函数，定义如下：

由式(47)～(15)不难发现，基于拉格朗日插值多项式逼近的状态和控制在插值节点处与实际的状态和控制是相等的，即：x(τ_i)＝X(τ_i),u(τ_k)＝U(τ_k)。注意到式(48)，并没有包含终端状态约束，而对于无人飞行器的航迹规划问题，其终端状态约束是必须要满足的，利用伪谱法对问题进行离散时，终端状态束可通过下式求得：

式中，ω_k表示高斯型求积公式的权系数，计算公式如下：

其中，

表示N阶Legendre多项式的微分。进一步，对式(49)进行求导，可获得下式：

式中拉格朗日多项式在零点处的微分，可通过微分矩阵D∈R^N×(N+1)求得，当插值节点个数给定时，该矩阵为一常值，矩阵D的计算公式如下：

经过上述变换，最优控制问题的微分约束(50)可转化为如下形式的代数约束：

终端状态的微分约束通过式(16)近似，集群无人机中心点边值约束(51)，路径约束(52)可利用插值节点处的约束进行逼近，得到下式：

Ψ(X_0,t_0,X_f,t_f)＝0 (21)

C[(X_k,U_k,τ_k；t₀,t_f)]≤0 (22)

进一步，利用高斯求积公式，对航路规划问题的目标函数进行逼近，可获得下式：

经过上述转化，集群无人机中心点航路规划问题(53)～(54)的求解可转化为对下述非线性规划问题的求解：求插值节点处的状态变量X(τ_k)(k＝0,...,N)、X(τ_f)，控制变量U(τ_k)(k＝1,...,N)及初始时刻t₀和终端时刻t_f，使得系统轨迹在满足终端状态约束(16)，动力学方程约束(20)，边值约束(21)和路径约束(22)的条件下，性能指标(23)最优。最后，利用SNOPT求解器可对上述构建的航路规划问题进行优化求解，获得满足所有约束的可行飞行航路。

第三步，安全飞行区域优化。该部分主要任务是在上述设计的可行航路上，搜索可供集群无人机飞行的安全飞行区域，并依据飞行区域，对集群无人机的队形进行优化设计，确保提供无人机可行飞行区域和最优飞行构型，主要实现步骤如下：

步骤1：基于获得的路径点，使用半正定规划进行迭代的区域扩张，以初始路径点为当前无人机编队中心位置，以下一个路径点为当前目标点g，在自由空间中找到包含所有无人机位置和目标点的最大安全飞行区域

包含所有无人机位置的中心和目标点的另一个安全飞行区域P_o→g,取两个安全飞行区域的交集

即为无人机队形变换的安全区域。

步骤2：找到安全区域后，即可以将安全区域P表示为等价的线性约束集合

其中A表示线性约束矩阵，b表示线性约束向量，n_l表示多面体P的面数，进而使用序列凸优化进行队形规划。为此我们首先定义f∈R个默认队形，如正方形，一字形或三角形，将其中任意一个默认队形表示为

编队

包括一个无人机位置的集合

和一个相对于编队中心的顶点集合

则任意队形Fⁱ中的无人机位置和顶点可以表示为

其中t∈R³表示无人机编队中心的位置，s∈R₊表示队形扩张的大小，q表示一个单位四元数，其共轭表示为

rot表示旋转函数，具体表达式如下

将所有优化变量表示为一个向量x_i＝[t,s,q]∈R⁸，定义目标函数为当前编队参数到目标点g，到期望编队大小

及到旋转四元数

这三个误差的加权求和

其中w_t,w_s,w_q为权重，c_i为队形i的指定代价。定义编队顶点

的安全区域边界约束

总的安全区域边界约束

单位四元数q约束C₂＝{||q||²＝1}。则非线性优化问题可以表示为

为使目标函数C(x_i)取得最小值的最优解，则最优队形

的指标i*可以表示为

使用稀疏非线性优化器SNOPT求解上述优化问题，求得最优队形

之后返回步骤1，将目标点更新为下一个路径点，将无人机位置更新为优化求得的新位置。重新计算安全飞行区域并在新安全飞行区域中规划新队形。循环执行上述步骤直到优化出最后一个目标队形。在下一部分，我们将设计控制器，使得无人机能够安全、无碰撞地朝着各自目标点飞行。

第四步，分布式协同控制器设计。该部分主要目的是设计分布式协同控制器，使得集群无人机按照上述设计的期望构型飞行，相关技术包括外环分布式位置控制器、姿态解算及内环的姿态控制器设计。

D)分布式位置控制器

考虑n架集群无人机系统，点集V＝{1,2,…,n}表示无人机编号的集合，G(V,ε,W)表示无向通信拓扑，ε是无人机之间通信关系边的集合，W＝[w_ij]是权重邻接矩阵。邻接矩阵定义为w_ii＝0且w_ij≥0(i≠j)，当且仅当无人机i可以从无人机j接收信息时w_ij>0，本发明中假设拓扑是固定的。为了实现无人机安全、无碰撞的朝着各自目标点飞行，本发明对每架无人机设计了相同结构的带有势能函数的分布式位置控制器。以第i架无人机为例，利用自身和邻机相对状态及无人机之间的拓扑链接关系，定义如下的相对增广位置跟踪误差：

其中p_i和p_j表示第i架无人机和第j架无人机的位置矢量信息，

和

表示第i架无人机和第j架无人机期望的位置矢量信息，其值可通过上述集群无人机的优化构型获得。基于上述定义，设计第i架无人机的分布式控制器：

其中，

表示用于形成期望无人机构型的虚拟控制输入，K_p,K_I,K_d为常值向量，表示控制器增益，

和

分别为势函数控制器产生的避碰与避障控制量，具体实现过程如下。

首先定义无人机i的避碰集合为

D_i＝{j∈V:d_ij≤R_a,i≠j}(26)

其中d_ij＝‖p_i-p_j‖表示第i架无人机和第j架无人机的相对距离，R_a表示最大碰撞探测距离，定义无人机i和j之间的势函数为

其中∈_a∈R为任意小的正数，0<ρ_a<R_a且η_j>0。则无人机i的避碰控制量为

其中

上面势能函数容易产生局部极小，导致无人机之间发生来回排斥，延长到达目标的时间。本发明采取以下方案进行改进，如图8所示，当无人机与邻机发生冲突时，判断目标方向和冲突方向之间的夹角α，当夹角α小于30°时，将势能函数项产生的控制指令方向扭转30°，以此避免了无人机之间冲突与运动方向的矛盾。

则避障控制量的形式变为

其中

表示势能函数方向的扭转。类似地，编队队形规划中的安全区域约束可表示为

定义第i架无人机指向障碍物平面的向量为(如图9所示)：

则第i架无人机的避障控制量为

其中d_min为防止避障势函数饱和而引入的无人机到障碍物平面的最小距离。基于以上步骤，完成了外环分布式位置控制器的设计。

E)姿态解算算法

由于四旋翼无人机无法通过控制力矩直接对飞行轨迹进行控制，因此需要将外环获得的虚拟控制指令转化为期望的飞行姿态，具体解算公式为

其中

表示向量

的第j个元素。

F)基于干扰补偿的有限时间姿态控制器

为了实现对第i架无人机期望姿态的有效跟踪，定义第i架无人机的期望姿态跟踪误差e_i1＝Θ_i-Θ_iref，姿态角速率跟踪误差

则第i架无人机姿态跟踪误差动态方程满足如下形式

基于上述定义，后续控制任务可归纳为：设计虚拟控制力矩

使得(37)中的姿态跟踪误差e_i1收敛到零，为了实现该目的，设计如下控制律

其中λ₁,λ₂为任意正常值，ρ₁,ρ₂为介于0到1之间的正常值，滑模面s_i1定义如下：

式(38)中的k₁,k₂满足如下条件：

其中L是公式(55)中不确定△₂的上界，即满足条件||△₂||≤L。基于(37)，(38)中的虚拟控制力矩

可获得实际控制力矩

根据(56)，升力

实际控制力矩

可获得无人机四个旋翼的旋转角速度，计算公式如下

基于以上四步，完成了面向安全域的集群无人机轨迹姿态协同控制方法的全部设计流程，在执行具体任务时，可根据实际需要调整集群无人机数量和队形库中队形，实现灵活控制。

Claims (1)

1.一种面向安全域的集群无人机轨迹姿态协同控制方法，其特征是，步骤如下：

第一部分，面向控制的四旋翼无人机轨迹姿态模型建立：充分考虑四旋翼无人机的固有特性及飞行中的动力学因素，建立四旋翼无人机的轨迹姿态数学模型；

第二部分，集群中心轨迹规划：实现基于伪谱法的集群无人机中心点的避障航路规划；

第三部分，安全飞行区域优化：完成集群无人机安全飞行包线及集群无人机最优编队构型的优化；

第四部分，分布式协同控制器设计：设计分布式协同控制器，使得集群无人机按照上述设计的期望构型飞行，相关技术包括外环分布式位置控制器、姿态解算及内环的姿态控制器设计；具体步骤如下：

以第i架无人机为例，其余无人机以此类推：

第一步、建立四旋翼无人机的轨迹姿态数学模型如下

其中，p_i＝[x_i,y_i,z_i]∈R³表示惯性坐标系下三个坐标轴方向的位置，

表示惯性坐标系下x,y,z三个方向的线速度，Θ_i＝[φ_i,θ_i,ψ_i]∈R³表示惯性坐标系下的滚转角、俯仰角及偏航角，

表示机体坐标系下的滚转角速率、俯仰角速率及偏航角速率，g＝9.8m/s²表示地球重力加速度，e_z＝[0,0,1]表示常值向量，m表示四旋翼无人机质量，τ_F表示无人机的总升力，

表示无人机三个方向的控制力矩，I＝diag{I_x,I_y,I_z}表示无人机惯性常值矩阵，△₁和△₂分别表示位置子系统和速度子系统收到的模型参数不确定及外界干扰的总和，旋转矩阵R_i和变换矩阵W_i定义如下

控制量

与电机转速

之间的关系如下：

其中，d表示旋翼转动中心到无人机中心的距离，k_F,k_M分别表示旋翼的升力系数和扭矩系数；

第二步，集群中心轨迹规划，具体实现过程如下：

将集群无人机的中心点，考虑成质点，对公式(1)和(2)所描述的质点运动模型，以飞行时间最短为优化目标，将飞行过程中的避障约束考虑为路径约束，利用伪谱法进行航路规划设计，为阐述算法的实现过程，将集群无人机中心点的航路规划问题，归纳为如下形式的最优控制问题求解：

Ψ(x(τ₀),x(τ_f)；t₀,t_f)＝0 (10)

C(x(τ),u(τ),τ；t₀,t_f))≤0 (11)

其中(8)～(11)分别为无人机中心点航路规划的目标函数、微分方程约束、包括起始点、路径点及终点约束的边值约束及包括飞行过程中的障碍物约束及状态约束的路径约束，x和u分别表示集群无人机质点的状态量和虚拟控制量，t₀和t_f分别表示航路规划的起始时间和终止时间，基于伪谱法对问题进行离散处理时，选取的离散点个数为N+2，记为τ₀,τ₁,…,τ_N,τ_f，其中τ₀＝-1,τ_f＝1，τ_k，1≤k≤N，为如下N阶Legendre多项式P_N(τ)的零点，

基于上述定义，状态变量和控制变量通过下式进行逼近：

式中，L_i(τ)和

分别表示以τ_i和τ_k为节点的拉格朗日插值基函数，0≤i≤N，1≤k≤N，定义如下：

由式(13)～(15)不难发现，基于拉格朗日插值多项式逼近的状态和控制在插值节点处与实际的状态和控制是相等的，即：x(τ_i)＝X(τ_i),u(τ_k)＝U(τ_k)，注意到式(13)，并没有包含终端状态约束，而对于无人飞行器的航迹规划问题，其终端状态约束是必须要满足的，利用伪谱法对问题进行离散时，终端状态束通过下式求得：

式中，ω_k表示高斯型求积公式的权系数，计算公式如下：

其中，

表示N阶Legendre多项式的微分，进一步，对式(13)进行求导，获得下式：

式中拉格朗日多项式在零点处的微分，通过微分矩阵D∈R^N×(N+1)求得，当插值节点个数给定时，该矩阵为一常值，矩阵D的计算公式如下：

经过上述变换，最优控制问题的微分约束(9)可转化为如下形式的代数约束：

终端状态的微分约束通过式(16)近似，集群无人机中心点边值约束(10)，路径约束(11)可利用插值节点处的约束进行逼近，得到下式：

Ψ(X₀,t₀,X_f,t_f)＝0 (21)

C[(X_k,U_k,τ_k；t₀,t_f)]≤0 (22)

进一步，利用高斯求积公式，对航路规划问题的目标函数进行逼近，获得下式：

经过上述转化，集群无人机中心点航路规划问题(8)～(11)的求解可转化为对下述非线性规划问题的求解：求插值节点处的状态变量X(τ_k)，k＝0，…，N，状态变量X(τ_f)，k＝1,...,N,控制变量U(τ_k)及初始时刻t₀和终端时刻t_f，使得系统轨迹在满足终端状态约束(16)，动力学方程约束(20)，边值约束(21)和路径约束(22)的条件下，性能指标(23)最优，最后，利用SNOPT求解器可对上述构建的航路规划问题进行优化求解，获得满足所有约束的可行飞行航路；

第三步，安全飞行区域优化，实现步骤具体如下：

步骤1：基于获得的路径点，使用半正定规划进行迭代的区域扩张，以初始路径点为当前无人机编队中心位置，以下一个路径点为当前目标点g，在自由空间中找到包含所有无人机位置和目标点的最大安全飞行区域

包含所有无人机位置的中心和目标点的另一个安全飞行区域P_o→g,取两个安全飞行区域的交集

即为无人机队形变换的安全区域；

步骤2：找到安全区域后，即可以将安全区域P表示为等价的线性约束集合

其中A表示线性约束矩阵，b表示线性约束向量，n_l表示多面体P的面数，进而使用序列凸优化进行队形规划，为此我们首先定义f∈R个默认队形，如正方形，一字形或三角形，将其中任意一个默认队形表示为

编队

包括一个无人机位置的集合

和一个相对于编队中心的顶点集合

则任意队形Fⁱ中的无人机位置和顶点可以表示为

其中t∈R³表示无人机编队中心的位置，s∈R₊表示队形扩张的大小，q表示一个单位四元数，其共轭表示为

rot表示旋转函数，具体表达式如下

将所有优化变量表示为一个向量x_i＝[t,s,q]∈R⁸，定义目标函数为当前编队参数到目标点g，到期望编队大小

及到旋转四元数

这三个误差的加权求和

其中w_t,w_s,w_q为权重，c_i为队形i的指定代价，定义编队顶点

的安全区域边界约束

总的安全区域边界约束

单位四元数q约束C₂＝{||q||²＝1}，则非线性优化问题可以表示为

为使目标函数C(x_i)取得最小值的最优解，则最优队形

的指标i^*可以表示为

使用稀疏非线性优化器SNOPT求解上述优化问题，求得最优队形

之后返回步骤1，将目标点更新为下一个路径点，将无人机位置更新为优化求得的新位置，重新计算安全飞行区域并在新安全飞行区域中规划新队形，循环执行上述步骤直到优化出最后一个目标队形；第四步，分布式协同控制器设计具体地：

A)分布式位置控制器

考虑n架集群无人机系统，点集V＝{1,2,…,n}表示无人机编号的集合，G(V,ε,W)表示无向通信拓扑，ε是无人机之间通信关系边的集合，W＝[w_ij]是权重邻接矩阵，邻接矩阵定义为w_ii＝0且w_ij≥0，i≠j，当且仅当无人机i可以从无人机j接收信息时w_ij>0，利用自身和邻机相对状态及无人机之间的拓扑链接关系，定义如下的相对增广位置跟踪误差：

其中p_i和p_j表示第i架无人机和第j架无人机的位置矢量信息，

和

表示第i架无人机和第j架无人机期望的位置矢量信息，其值可通过上述集群无人机的优化构型获得，基于上述定义，设计第i架无人机的分布式控制器：

其中，

表示用于形成期望无人机构型的虚拟控制输入，K_p,K_I,K_d为常值向量，表示控制器增益，

和

分别为势函数控制器产生的避碰与避障控制量，具体实现过程如下：

首先定义无人机i的避碰集合为

D_i＝{j∈V:d_ij≤R_a,i≠j} (28)

其中d_ij＝||p_i-p_j||表示第i架无人机和第j架无人机的相对距离，R_a表示最大碰撞探测距离，定义无人机i和j之间的势函数为

其中ε_a∈R为任意小的正数，0<ρ_a<R_a且η_j>0，则无人机i的避碰控制量为

其中

当无人机与邻机发生冲突时，判断目标方向和冲突方向之间的夹角α，当夹角α小于30°时，将势能函数项产生的控制指令方向扭转30°，以此避免了无人机之间冲突与运动方向的矛盾；

则避障控制量的形式变为

其中

表示势能函数方向的扭转，类似地，编队队形规划中的安全区域约束表示为

定义第i架无人机指向障碍物平面的向量为：

则第i架无人机的避障控制量为

其中d_min为防止避障势函数饱和而引入的无人机到障碍物平面的最小距离，基于以上步骤，完成了外环分布式位置控制器的设计；

B)姿态解算算法

由于四旋翼无人机无法通过控制力矩直接对飞行轨迹进行控制，因此需要将外环获得的虚拟控制指令转化为期望的飞行姿态，具体解算公式为

其中

表示向量

的第j个元素,j＝1,2,3；

C)基于干扰补偿的有限时间姿态控制器

为了实现对第i架无人机期望姿态的有效跟踪，定义第i架无人机的期望姿态跟踪误差e_i1＝Θ_i-Θ_iref，姿态角速率跟踪误差

则第i架无人机姿态跟踪误差动态方程满足如下形式

基于上述定义，后续控制任务可归纳为：设计虚拟控制力矩

使得(39)中的姿态跟踪误差e_i1收敛到零，为了实现该目的，设计如下控制律

其中λ₁,λ₂为任意正常值，ρ₁,ρ₂为介于0到1之间的正常值，滑模面s_i1定义如下：

式(40)中的k₁,k₂满足如下条件：

其中L是公式(4)中不确定△₂的上界，即满足条件△₂≤L，基于(39)，(40)中的虚拟控制力矩

可获得实际控制力矩

根据(7)，升力

实际控制力矩

可获得无人机四个旋翼的旋转角速度，计算公式如下

Images