CN115712308A - 基于分布式模型预测控制及队形控制的多机编队防撞方法 - Google Patents

Abstract

本发明涉及固定翼多无人机系统编队控制技术领域，特别涉及一种基于分布式模型预测控制及队形控制的多机编队防撞方法，解决了如何使得无人机群具有稳定的、控制精度高的协同编队飞行控制能力的问题。该方法的特殊在于，包括以下步骤：步骤一：集结无人机群，输入集结点或航迹点信息，进行基于分布式模型预测控制的编队防撞设置；步骤二：巡航控制；巡航中若要重构队形，执行步骤三；若不需要重构队形，执行步骤四；步骤三：基于重构队形要求再次进行基于分布式模型预测控制的编队防撞设置；步骤四：进行队形保持控制；步骤五：判断是否抵达目标点或完成任务；若是，结束任务；若否，返回步骤二，循环执行步骤二至五，直至抵达目标点或完成任务。

Description

基于分布式模型预测控制及队形控制的多机编队防撞方法

技术领域

本发明涉及固定翼多无人机系统编队控制技术领域，特别涉及一种基于分布式模型预测控制及队形控制的多机编队防撞方法。

背景技术

如今，随着无人机在军事、农业、科技等领域广泛应用，对于无人机的相关技术也有了更加深入的研究。由于无人机具有续航久、速度快等特点使得其能够在军事应用方面中用于侦察、测绘等任务；在民用方面可以用于对气象和地质的采集，最常用的还是航拍的任务。无人机拥有多种用途的特点使得其逐渐成为在航空领域中学者所看重的研究内容。

无人机可以根据其结构的不同分为几类，其中与固定翼无人机相关的应用和研究受到了各国学者的重点关注；其原因一方面是由于固定翼无人机具有结构简单、续航久、机动性强的特点；另一方面是由于固定翼无人机发展时间长，所以有更加丰富的理论作为支撑，可以参考和借鉴的知识十分充裕。固定翼无人机有多种应用领域，为了尽可能地发挥单架无人机的作用，实现多无人机协同编队飞行(Multi-UAV Coordinated formationFlight)的控制、决策和管理，从而提高无人机完成任务的效率，特别是在执行搜索、救援、建图等方面能够更有效地完成任务，拓宽无人机使用范围，达到安全、高可靠性地执行各种任务的目的，多无人机系统领域的一个核心问题是多智能体之间的运动规划问题。只有将多智能体之间的运动进行合理规划，才能使无人机集群在执行任务时巡航控制能力稳定、队形控制能力精准、编队防撞能力可靠。这样，多架无人机联合起来去完成指定的任务时可以将有限的单机集合起来完成任务，多个成本低的无人机可以通过集群的方法来实现功能互补，能够避免资源浪费和提高效率。

因此，如何实现具有编队协同控制、障碍规避与巡航队形控制能力的多无人机系统，使得无人机群具有稳定的、控制精度高的协同编队飞行控制能力，是我们研究中亟需解决的问题。本发明从该问题出发，力求在协同控制研究的理论和应用上有所突破和创新。

发明内容

本发明的目的是提供一种基于分布式模型预测控制及队形控制的多机编队防撞方法，以解决如何实现具有编队协同控制、障碍规避与巡航队形控制能力的多无人机系统，使得无人机群具有稳定的、控制精度高的协同编队飞行控制能力的技术问题。

本发明所采用的技术方案是，基于分布式模型预测控制及队形控制的多机编队防撞方法，其特殊之处在于，包括以下步骤：

步骤一：首先基于任务规划信息与无人机平台因素，集结无人机群，输入集结点或航迹点信息，然后进行基于分布式模型预测控制的编队防撞设置；

步骤二：基于PID算法进行巡航控制；巡航过程中，若需要重构队形，执行下述步骤三；若不需要重构队形，跳过下述步骤三，执行下述步骤四；

步骤三：基于步骤二中所述的重构队形的要求，再次进行基于分布式模型预测控制的编队防撞设置；

步骤四：基于Leader-Follower进行队形保持控制；

步骤五：判断是否抵达目标点或完成任务；若是，结束任务；若否，返回上述步骤二，循环执行步骤二至步骤五，直至抵达目标点或完成任务，结束任务。

进一步地，步骤二中所述的基于PID算法进行巡航控制的控制过程包括以下步骤：

步骤1：系统采用二阶一致性算法设计协同飞行控制率

所述

表示无人机i的飞行加速度；

步骤1.1：系统采用二阶一致性算法设计

所述

分别表示无人机i在地面坐标系下沿x、y、z三个方向的三个分加速度；具体为：

系统采用二阶一致性算法，对无人机i三个位置、加速度通道分别设计如下式(1.1)所示：

式(1.1)中：N_i表示分布式网络中无人机i的邻居；a_ij表示不同无人机之间考虑计算的加权系数；x_diF、y_diF、z_diF分别表示第i架无人机参考点的x坐标、y坐标、z坐标；x_djF、y_djF、z_djF分别表示第j架无人机参考点的x坐标、y坐标、z坐标；γ表示状态量的权重系数，通常大于0；

分别表示无人机i相对于无人机j在地面坐标系下沿x、y、z三个方向的三个相对速度分量；

无人机i相对于无人机j的运动速度

具体如下式(1.2)所示：

式(1.2)中：V_xi、V_yi、V_zi分别表示无人机i的速度V_i在地面坐标系下沿x、y、z三个方向的速度分量；V_xj、V_yj、V_zj分别表示无人机j的速度V_j在地面坐标系下沿x、y、z三个方向的速度分量；

位置协同变量ρ_iF在编队开始时是不同的，随着时间进行，当t→∞时，ρ_iF→ρ_jF。上式(1.1)给出了地面坐标系下三个分加速度指令，控制

即可实现编队的队形保持。

步骤1.2：定义协同飞行控制率

如下式(1.3)所示：

二阶一致性算法控制，既包含了位置信息，又包含了速度信息。

步骤2：将步骤1中设计的所述协同飞行控制率

转化为飞行控制指令

所述

表示无人机i飞行的加速度大小；所述

表示俯仰角速度；所述

表示偏航角速度；具体为：

所述

的控制方式如下式(1.4)所示：

依据俯仰角与速度分量的关系如下式(1.5)，以及偏航角与速度分量的关系如下式(1.6)：

式(1.5)中，θ_i表示无人机i的俯仰角；

式(1.6)中，Ψ_i表示无人机i的偏航角；

则

步骤3：定义新的协同飞行控制指令v_i；

新的协同飞行控制指令v_i为下式(1.9)所示：

式(1.9)中，

表示无人机i飞行的新的加速度大小，

表示无人机i飞行的新的俯仰角速度，

表示无人机i飞行的新的偏航角速度；

所述

的具体形式如下式(1.10)所示：

式(1.10)中，

表示平均速度；

所述

的具体形式如下式(1.11)所示：

式(1.11)中，

表示航迹参考俯仰角；

所述

的具体形式如下式(1.12)所示：

式(1.12)中，

表示航迹参考偏航角；

步骤4：将步骤3中定义的所述新的协同飞行控制指令v_i作为输入，计算无人机的实际轨迹与参考轨迹的误差，航迹控制器基于PID算法，在所述误差满足预设条件的前提下设计PID控制律u_i；具体为：

PID控制律分为纵向与横侧向通道来设计，假设无人机的纵向与横侧向通道之间耦合较小，控制增稳系统的设计常按照纵向和横侧向分开进行的，互不交联，PID控制律u_i按下式(1.13)设计：

式(1.13)中，K_p,K_I,K_D分别为比例、积分、微分系数矩阵，

表示偏差。

步骤5：基于步骤4中设计的PID控制率，调整协同飞行控制系统的控制指令输入值，从而控制无人机的飞行轨迹和姿态。

进一步地，步骤四中所述的基于Leader-Follower进行队形保持控制的具体步骤为：

步骤A：建立长机与僚机的相对运动关系模型，依据该相对运动关系模型得出长机与僚机系统的状态变量；具体为：

航迹坐标系中无人机质心运动的动力学方程如下式(2.1)所示：

式(2.1)中：

表示加速度；

表示航迹倾斜角速度；

表示航迹方位角速度；T表示发动机推力；D表示无人机受到的阻力；m表示无人机及其载荷的质量；g表示重力加速度；μ表示航迹倾斜角；L表示无人机受到的升力；C表示无人机受到的侧力；

表示航迹方位角；φ表示滚转角；V表示无人机速度；

式(2.1)中，L、D、C依据下式(2.2)进行计算：

式(2.2)中：C_L表示升力系数；ρ表示大气密度；S表示机翼面积；C_Lα表示升力系数对迎角的导数；α表示无人机迎角；α₀表示初始迎角；C_D0表示阻力系数；K表示传递系数；C_Yβ表示侧向气动力系数；β表示无人机侧滑角；

依据上述式(2.1)，将长机相对僚机的距离转换到僚机坐标系中，建立长机与僚机的相对运动关系模型，如下式(2.3)所示：

式(2.3)中：x_d表示leader在follower坐标系下的相对距离在x轴上的分量；y_d表示leader在follower坐标系下的相对距离在y轴上的分量；z_d表示leader在follower坐标系下的相对距离在z轴上的分量；L_w表示僚机升力；C_w表示僚机侧力；m_w表示僚机质量；V_w表示follower在地面坐标系下的速度；μ_w表示follower在地面坐标系下的航迹倾斜角；φ_w表示follower自身的滚转角；V_L表示leader在地面坐标系下的速度；μ_L表示leader在地面坐标系下的航迹倾斜角；

表示leader和follower的相对方位角误差；

无人机受到的升力L，阻力D和侧力C是无人机速度V，迎角α，侧滑角β的函数；因此，系统的状态变量如下式(2.4)所示：

式(2.4)中：

表示僚机航迹方位角；

表示长机航迹方位角；

步骤B：依据步骤A中得出的所述系统的状态变量，基于Leader-Follower设计队形控制律；

步骤B.1：设定x_c,y_c,z_c为无人机编队中僚机与长机按要求应该保持的距离；依据步骤A得出的所述系统的状态变量，求取误差向量；所述误差向量中的误差项包括位置误差、速度误差、航迹倾斜角误差、航迹方位角误差以及航迹滚转角误差；所述误差向量如下式(2.5)所示：

步骤B.2：依据编队控制的最终目的是使目标误差e_x，e_y和e_z为零，并且假设侧滑角β为零，依据下式(2.6)所示的编队中长机与僚机的控制量以及步骤B.1中(2.5)式所示的误差向量，对僚机的控制变量给出如下式(2.7)所示的控制律，进而编队的控制量如下式(2.8)所示；

编队中长机与僚机的控制量如下式(2.6)所示：

僚机的控制变量的控制律如下式(2.7)所示：

式(2.7)中：K_ZP和K_ZI分别为高度控制的比例、积分系数；K_μP和K_μI分别为航迹倾斜角控制的比例、积分系数；K_XP和K_XI分别为经度控制的比例、积分系数；K_VP和K_VI分别为速度控制的比例、积分系数；K_YP和K_YI分别为维度控制的比例、积分系数；K_φP和K_φI分别为滚转角控制的比例、积分系数；

编队的控制量如下式(2.8)所示：

U_c＝[α T φ]^T (2.8)。

进一步地，步骤一中和步骤三中所述的基于分布式模型预测控制的编队防撞设置的设计过程包括以下步骤：

步骤a：建立编队预测模型；

步骤b：依据步骤a中建立的所述编队预测模型，将周围所有有可能发生碰撞的无人机通过数学的形式表示成限制约束惩罚项；

步骤c：利用步骤b中所述的惩罚项构建优化函数，使无人机在满足要求的情况下的整体惩罚最小，进而求解出控制量；

步骤d：将步骤c中求解出的所述控制量解算成轨迹控制，编队防撞设置完成。

进一步地，所述步骤a：建立编队预测模型的具体建立过程为：

使用二阶积分动力学模型来表示无人机，其离散的动力学方程如下式(3.1)所示：

式(3.1)中：p_i[k+1]、v_i[k+1]分别表示第k+1时刻的位置、速度；p_i[k]、v_i[k]、a_i[k]分别表示第k时刻的位置、速度、加速度；h表示单位时间参数；

将无人机的状态表示成下式(3.2)：

式(3.2)中，

表示位置在时刻k_t+k的预测信息；

表示速度在时刻k_t+k的预测信息；k_t表示任一时刻；

使用式(3.2)可以将位置和速度表示成一个整体的状态，则K步的预测模型步长方程如下式(3.3)所示：

P_i＝A₀X_0，i+ΛU_i (3.3)；

式(3.3)中：Λ被定义为下式(3.4)所示：

式(3.3)和(3.4)中：Ψ＝[I₃ 0₃]，A₀＝[(ΨA)^T+(ΨA²)^T...(ΨA^K)^T]T；X_0，i表示初始的位置；P_i表示K步长的位置预测序列；U_i表示输入序列；

所述步骤b中的惩罚项包括期望的轨迹误差惩罚项、控制花销惩罚项、控制输入变化量惩罚项以及碰撞约束惩罚项；所述惩罚项数学的形式分别如下：

2)期望的轨迹误差惩罚项

上式(3.5)表示的是整个预测步长中从开始时刻到K时刻的轨迹和期望轨迹的误差二范数，将式(3.5)转化成二次规划形式如下式(3.6)所示：

式(3.6)中：J_e,i表示期望的轨迹误差惩罚项；P_d,i表示目的地；

表示正定的对角矩阵

给每一步的误差的权重系数；

2)控制花销惩罚项：

式(3.7)中：

表示的是惩罚项的系数；R表示控制成本的惩罚系数；

3)控制输入变化量惩罚项：

式(3.8)中：

S表示控制变化的误差惩罚系数；

4)碰撞约束惩罚项

碰撞约束项的加入可以让无人机当有碰撞的风险时，进行及时的规避：

式(3.9)中：

所述步骤c：利用步骤b中所述的惩罚项构建优化函数，使无人机在满足要求的情况下的整体惩罚最小，进而求解出控制量的具体构建过程为：

式(3.10)中，U_i表示控制量；A_in，b_in表示控制向量的不等式约束。

本发明的有益效果是：

(1)本发明中基于PID算法进行巡航控制，基于分布式模型预测控制进行编队防撞设置，基于Leader-Follower进行队形保持控制，模型预测控制作为一种现代控制方法，是一种计算简单、鲁棒性强、抑制干扰能力好、控制精度较高的控制方法，把它作为基本控制方法进行借鉴来可以提高无人机编队的控制效率，但其滚动优化的实时性问题成为预测控制在实际应用的一个瓶颈，本发明针对无人机编队控制问题，建立了相对运动方程，然后采用基于多模型的预测控制的方法设计编队控制器，将非线性滚动优化问题转化为线性二次型的优化问题，提高了非线性预测的实时性；同时，编队控制是一个多机的运动规划问题，仅通过一致性算法计算出的无人机运动路线是不考虑防碰撞的，本发明将一致性算法与分布式模型预测控制算法相结合，先通过一致性算法进行巡航控制设计，然后再使用分布式模型预测控制算法，将所有周围有可能碰撞的无人机通过数学的形式表示成限制约束，从而求解出满足多目标要求的无人机运动规划解，进而控制整个编队，整个框架是一个完整的运动规划的过程，由固定翼自动驾驶仪飞行出真实的状态，之后状态反馈到一致性和分布式模型预测控制算法，进行实时的规划从而提高控制的效率；因此，本发明解决了如何实现具有编队协同控制、障碍规避与巡航队形控制能力的多无人机系统，使得无人机群具有稳定的、控制精度高的协同编队飞行控制能力的技术问题。

(2)采用本发明的基于分布式模型预测控制及队形控制的多机编队防撞方法进行编队控制，可以提高多机协同编队巡航控制、队形控制以及编队防碰撞的稳定性、控制精度。

附图说明

图1是本发明实施例的系统总体框图；

图2是本发明实施例中巡航控制的控制过程示意图；

图3是本发明实施例中进行巡航控制时的PID算法图；

图4是本发明实施例中基于Leader-Follower进行队形保持控制的控制过程示意图；

图5是地面坐标系中长机Leader与僚机Follower位置向量之间的关系示意图；

图6是本发明实施例中将一致性算法Consensus与分布式模型预测控制算法DMPC结合进行控制的框架示意图。

具体实施方式

下面结合附图和具体实施方式对本发明进行详细说明。

参见图1，本发明基于分布式模型预测控制及队形控制的多机编队防撞方法，包括以下步骤：

步骤一：首先基于任务规划信息与无人机平台因素，集结无人机群，输入集结点或航迹点信息，然后进行基于分布式模型预测控制的编队防撞设置；

步骤二：基于PID算法进行巡航控制；巡航过程中，若需要重构队形，执行下述步骤三；若不需要重构队形，跳过下述步骤三，执行下述步骤四；

步骤三：基于步骤二中的重构队形的要求，再次进行基于分布式模型预测控制的编队防撞设置；

步骤四：基于Leader-Follower进行队形保持控制；

步骤五：判断是否抵达目标点或完成任务；若是，结束任务；若否，返回上述步骤二，循环执行步骤二至步骤五，直至抵达目标点或完成任务，结束任务。

通过上述任务描述可知，本发明的基于分布式模型预测控制及队形控制的多机编队防撞方法的执行过程包含巡航控制、队形控制、编队防碰三个方面的控制问题。

[1]巡航控制

参见图2，多无人机巡航控制包括两部分，协同飞行控制系统和协同航迹控制系统，协同飞控系统为内环，控制飞行姿态；协同航迹控制系统为外环，控制飞行轨迹，外环的输出作为内环的输入。协同航迹控制系统根据预定航线计算出相应的俯仰角、偏航角、速度等航迹指令，然后将它们传递给协同飞控系统。飞控系统接到指令后，经过解算控制舵面偏转来跟踪航迹控制指令。PID控制律分为纵向与横侧向通道来设计。假设无人机的纵向与横侧向通道之间耦合较小，控制增稳系统的设计常按照纵向和横侧向分开进行的，互不交联。在此形式下，由无人机模型、飞控系统、舵机组成的内回路是稳定的。

本实施例中，上述步骤二中基于PID算法进行巡航控制的控制过程包括以下步骤：

步骤1：系统采用二阶一致性算法设计协同飞行控制率

上述

表示无人机i的飞行加速度；

步骤1.1：系统采用二阶一致性算法设计

分别表示无人机i在地面坐标系下沿x、y、z三个方向的三个分加速度；具体为：

系统采用二阶一致性算法，对无人机i三个位置、加速度通道分别设计如下式(1.1)所示：

式(1.1)中：N_i表示分布式网络中无人机i的邻居；a_ij表示不同无人机之间考虑计算的加权系数；x_diF、y_diF、z_diF分别表示第i架无人机参考点的x坐标、y坐标、z坐标；x_djF、y_djF、z_djF分别表示第j架无人机参考点的x坐标、y坐标、z坐标；γ表示状态量的权重系数，通常大于0；

分别表示无人机i相对于无人机j在地面坐标系下沿x、y、z三个方向的三个相对速度分量；

无人机i相对于无人机j的运动速度

具体如下式(1.2)所示：

式(1.2)中：V_xi、V_yi、V_zi分别表示无人机i的速度V_i在地面坐标系下沿x、y、z三个方向的速度分量；V_xj、V_yj、V_zj分别表示无人机j的速度V_j在地面坐标系下沿x、y、z三个方向的速度分量；

位置协同变量ρ_iF在编队开始时是不同的，随着时间进行，当t→∞时，ρ_iF→ρ_jF。上式(1.1)给出了地面坐标系下三个分加速度指令，控制

即可实现编队的队形保持。

步骤1.2：定义协同飞行控制率

如下式(1.3)所示：

二阶一致性算法控制，既包含了位置信息，又包含了速度信息。

步骤2：将步骤1中设计的上述协同飞行控制率

转化为飞行控制指令

上述

表示无人机i飞行的加速度大小；上述

表示俯仰角速度；上述

表示偏航角速度；具体为：

上述

的控制方式如下式(1.4)所示：

依据俯仰角与速度分量的关系如下式(1.5)，以及偏航角与速度分量的关系如下式(1.6)：

式(1.5)中，θ_i表示无人机i的俯仰角；

式(1.6)中，Ψ_i表示无人机i的偏航角；

则

步骤3：定义新的协同飞行控制指令v_i；

新的协同飞行控制指令v_i为下式(1.9)所示：

式(1.9)中，

表示无人机i飞行的新的加速度大小，

表示无人机i飞行的新的俯仰角速度，

表示无人机i飞行的新的偏航角速度；

上述

的具体形式如下式(1.10)所示：

式(1.10)中，

表示平均速度；

上述

的具体形式如下式(1.11)所示：

式(1.11)中，

表示航迹参考俯仰角；

上述

的具体形式如下式(1.12)所示：

式(1.12)中，

表示航迹参考偏航角；

步骤4：将步骤3中定义的上述新的协同飞行控制指令v_i作为输入，计算无人机的实际轨迹与参考轨迹的误差，参见图3，航迹控制器基于PID算法，在上述误差满足预设条件的前提下设计PID控制律u_i；具体为：

PID控制律分为纵向与横侧向通道来设计，假设无人机的纵向与横侧向通道之间耦合较小，控制增稳系统的设计常按照纵向和横侧向分开进行的，互不交联，PID控制律u_i按下式(1.13)设计：

式(1.13)中，K_p,K_I,K_D分别为比例、积分、微分系数矩阵，e_yi表示偏差。

步骤5：基于步骤4中设计的PID控制率，调整协同飞行控制系统的控制指令输入值，从而控制无人机的飞行轨迹和姿态。

[2]队形控制

参见图4，编队控制器是无人机进行编队飞行的关键。在编队中，僚机的编队控制器利用各种误差信号来实现僚机与长机位置的保持,而长机的编队控制器处于“空闲”状态，只有当角色转变时才发生作用。考虑到工程应用性，本发明根据PID控制的设计原理以及其在编队控制器设计中的应用，结合编队相对运动模型，设计了此编队控制器。

在编队飞行中，由长机带领整个编队向目标点飞行，不负责编队队形的保持，由僚机的编队控制器根据编队要求来保持队形。因此本发明实施例中基于Leader-Follower进行队形保持控制的控制过程如图4所示。

本实施例中，上述步骤四中基于Leader-Follower进行队形保持控制的具体步骤为：

步骤A：建立长机与僚机的相对运动关系模型，参见图5，依据该相对运动关系模型得出长机与僚机系统的状态变量；具体为：

航迹坐标系中无人机质心运动的动力学方程如下式(2.1)所示：

式(2.1)中：

表示加速度；

表示航迹倾斜角速度；

表示航迹方位角速度；T表示发动机推力；D表示无人机受到的阻力；m表示无人机及其载荷的质量；g表示重力加速度；μ表示航迹倾斜角；L表示无人机受到的升力；C表示无人机受到的侧力；

表示航迹方位角；φ表示滚转角；V表示无人机速度；

式(2.1)中，L、D、C依据下式(2.2)进行计算：

式(2.2)中：C_L表示升力系数；ρ表示大气密度；S表示机翼面积；C_Lα表示升力系数对迎角的导数；α表示无人机迎角；α₀表示初始迎角；C_D0表示阻力系数；K表示传递系数；C_Yβ表示侧向气动力系数；β表示无人机侧滑角；

依据上述式(2.1)，将长机相对僚机的距离转换到僚机坐标系中，建立长机与僚机的相对运动关系模型，如下式(2.3)所示：

式(2.3)中：x_d表示leader在follower坐标系下的相对距离在x轴上的分量；y_d表示leader在follower坐标系下的相对距离在y轴上的分量；z_d表示leader在follower坐标系下的相对距离在z轴上的分量；L_w表示僚机升力；C_w表示僚机侧力；m_w表示僚机质量；V_w表示follower在地面坐标系下的速度；μ_w表示follower在地面坐标系下的航迹倾斜角；φ_w表示follower自身的滚转角；V_L表示leader在地面坐标系下的速度；μ_L表示leader在地面坐标系下的航迹倾斜角；

表示leader和follower的相对方位角误差；

无人机受到的升力L，阻力D和侧力C是无人机速度V，迎角α，侧滑角β的函数；因此，系统的状态变量如下式(2.4)所示：

式(2.4)中：

表示僚机航迹方位角；

表示长机航迹方位角；

步骤B：依据步骤A中得出的上述系统的状态变量，基于Leader-Follower设计队形控制律；

步骤B.1：设定x_c,y_c,z_c为无人机编队中僚机与长机按要求应该保持的距离；依据步骤A得出的上述系统的状态变量，求取误差向量；上述误差向量中的误差项包括位置误差、速度误差、航迹倾斜角误差、航迹方位角误差以及航迹滚转角误差；上述误差向量如下式(2.5)所示：

步骤B.2：依据编队控制的最终目的是使目标误差e_x，e_y和e_z为零，并且假设侧滑角β为零，依据下式(2.6)所示的编队中长机与僚机的控制量以及步骤B.1中(2.5)式所示的误差向量，对僚机的控制变量给出如下式(2.7)所示的控制律，进而编队的控制量如下式(2.8)所示；

编队中长机与僚机的控制量如下式(2.6)所示：

僚机的控制变量的控制律如下式(2.7)所示：

式(2.7)中：K_ZP和K_ZI分别为高度控制的比例、积分系数；K_μP和K_μI分别为航迹倾斜角控制的比例、积分系数；K_XP和K_XI分别为经度控制的比例、积分系数；K_VP和K_VI分别为速度控制的比例、积分系数；K_YP和K_YI分别为维度控制的比例、积分系数；K_φP和K_φI分别为滚转角控制的比例、积分系数；

编队的控制量如下式(2.8)所示：

U_c＝[α T φ]^T (2.8)。

[3]编队防碰

在无人机编队飞行的过程中，需要根据任务的需求变换队形。在相应的变换过程中，无人机彼此不能碰撞，这是编队飞行的基本要求。由于无人机控制系统本身就是非线性耦合系统，加上复杂的作战环境的约束，促使编队飞行的队形设计、变化和保持的技术要求也在不断提高，因此需要提出有效的控制策略。基于上述要求，本发明提出基于分布式模型预测控制的编队算法来解决无人机的编队控制防碰撞问题。

模型预测控制作为一种现代控制方法，是一种计算简单、鲁棒性强、抑制干扰能力好、控制精度较高的控制方法，可以把它作为基本控制方法进行借鉴来提高无人机编队的控制效率，但其滚动优化的实时性问题成为预测控制在实际应用的一个瓶颈。针对无人机编队控制问题，建立了相对运动方程，然后采用基于多模型的预测控制的方法设计编队控制器，将非线性滚动优化问题转化为线性二次型的优化问题，提高了非线性预测的实时性，实现编队队形控制。

由于编队控制是一个多机的运动规划问题，通过一致性算法计算出的无人机运动路线是不考虑防碰撞的，此时使用分布式模型预测控制算法，将所有周围有可能碰撞的无人机通过数学的形式表示成限制约束，从而求解出满足多目标要求的无人机运动规划解。

图6是本发明实施例中将一致性算法Consensus与分布式模型预测控制算法DMPC结合进行控制的框架示意图。其中Consensus表示一致性算法，DMPC代表分布式模型预测控制算法，Fixed-wing Autopilot代表固定翼自动驾驶仪。整个框架是一个完整的运动规划的过程，由固定翼自动驾驶仪飞行出真实的状态，之后状态反馈到一致性和分布式模型预测控制算法，进行实时的规划从而提高控制的效率。

本实施例中上述步骤一中和步骤三中基于分布式模型预测控制的编队防撞设置的设计过程包括以下步骤：

步骤a：建立编队预测模型；

步骤b：依据步骤a中建立的上述编队预测模型，将周围所有有可能发生碰撞的无人机通过数学的形式表示成限制约束惩罚项；

步骤c：利用步骤b中上述的惩罚项构建优化函数，使无人机在满足要求的情况下的整体惩罚最小，进而求解出控制量；

步骤d：将步骤c中求解出的上述控制量解算成轨迹控制，编队防撞设置完成。

本实施例中，上述步骤a：建立编队预测模型的具体建立过程为：

使用二阶积分动力学模型来表示无人机，其离散的动力学方程如下式(3.1)所示：

式(3.1)中：p_i[k+1]、v_i[k+1]分别表示第k+1时刻的位置、速度；p_i[k]、v_i[k]、a_i[k]分别表示第k时刻的位置、速度、加速度；h表示单位时间参数；

将无人机的状态表示成下式(3.2)：

式(3.2)中，

表示位置在时刻k_t+k的预测信息；

表示速度在时刻k_t+k的预测信息；k_t表示任一时刻；

使用式(3.2)可以将位置和速度表示成一个整体的状态，则K步的预测模型步长方程如下式(3.3)所示：

P_i＝A₀X_0，i+ΛU_i (3.3)；

式(3.3)中：Λ被定义为下式(3.4)所示：

式(3.3)和(3.4)中：Ψ＝[i₃ 0₃]，A₀＝[(ΨA)T(ΨA²)^T…(ΨA^K)^T]^T；X_0，i表示初始的位置；P_i表示K步长的位置预测序列；U_i表示输入序列；

上述步骤b中的惩罚项包括期望的轨迹误差惩罚项、控制花销惩罚项、控制输入变化量惩罚项以及碰撞约束惩罚项；上述惩罚项数学的形式分别如下：

3)期望的轨迹误差惩罚项

上式(3.5)表示的是整个预测步长中从开始时刻到K时刻的轨迹和期望轨迹的误差二范数，将式(3.5)转化成二次规划形式如下式(3.6)所示：

式(3.6)中：J_e,i表示期望的轨迹误差惩罚项；P_d,i表示目的地；

表示正定的对角矩阵

给每一步的误差的权重系数；

2)控制花销惩罚项：

式(3.7)中：

表示的是惩罚项的系数；R表示控制成本的惩罚系数；

3)控制输入变化量惩罚项：

式(3.8)中：

S表示控制变化的误差惩罚系数；

4)碰撞约束惩罚项

碰撞约束项的加入可以让无人机当有碰撞的风险时，进行及时的规避：

式(3.9)中：

上述步骤c：利用步骤b中上述的惩罚项构建优化函数，使无人机在满足要求的情况下的整体惩罚最小，进而求解出控制量的具体构建过程为：

式(3.10)中，U_i表示控制量；A_in，b_in表示控制向量的不等式约束。

采用本发明的基于分布式模型预测控制及队形控制的多机编队防撞方法进行编队控制，可以提高多机协同编队巡航控制、队形控制以及编队防碰撞的稳定性、控制精度。

Claims (5)

1.基于分布式模型预测控制及队形控制的多机编队防撞方法，其特征在于，包括以下步骤：

步骤一：首先基于任务规划信息与无人机平台因素，集结无人机群，输入集结点或航迹点信息，然后进行基于分布式模型预测控制的编队防撞设置；

步骤二：基于PID算法进行巡航控制；巡航过程中，若需要重构队形，执行下述步骤三；若不需要重构队形，跳过下述步骤三，执行下述步骤四；

步骤三：基于步骤二中所述的重构队形的要求，再次进行基于分布式模型预测控制的编队防撞设置；

步骤四：基于Leader-Follower进行队形保持控制；

步骤五：判断是否抵达目标点或完成任务；若是，结束任务；若否，返回上述步骤二，循环执行步骤二至步骤五，直至抵达目标点或完成任务，结束任务。

2.根据权利要求1所述的基于分布式模型预测控制及队形控制的多机编队防撞方法，其特征在于，步骤二中所述的基于PID算法进行巡航控制的控制过程包括以下步骤：

步骤1：系统采用二阶一致性算法设计协同飞行控制率

所述

表示无人机i的飞行加速度；

步骤1.1：系统采用二阶一致性算法设计

所述

分别表示无人机i在地面坐标系下沿x、y、z三个方向的三个分加速度；具体为：

系统采用二阶一致性算法，对无人机i三个位置、加速度通道分别设计如下式(1.1)所示：

式(1.1)中：N_i表示分布式网络中无人机i的邻居；a_ij表示不同无人机之间考虑计算的加权系数；x_diF、y_diF、z_diF分别表示第i架无人机参考点的x坐标、y坐标、z坐标；x_djF、y_djF、z_djF分别表示第j架无人机参考点的x坐标、y坐标、z坐标；γ表示状态量的权重系数，通常大于0；

分别表示无人机i相对于无人机j在地面坐标系下沿x、y、z三个方向的三个相对速度分量；

无人机i相对于无人机j的运动速度

具体如下式(1.2)所示：

式(1.2)中：V_xi、V_yi、V_zi分别表示无人机i的速度V_i在地面坐标系下沿x、y、z三个方向的速度分量；V_xj、V_yj、V_zj分别表示无人机j的速度V_j在地面坐标系下沿x、y、z三个方向的速度分量；

位置协同变量ρ_iF在编队开始时是不同的，随着时间进行，当t→∞时，ρ_iF→ρ_jF。上式(1.1)给出了地面坐标系下三个分加速度指令，控制

即可实现编队的队形保持。

步骤1.2：定义协同飞行控制率

如下式(1.3)所示：

二阶一致性算法控制，既包含了位置信息，又包含了速度信息。

步骤2：将步骤1中设计的所述协同飞行控制率

转化为飞行控制指令

所述

表示无人机i飞行的加速度大小；所述

表示俯仰角速度；所述

表示偏航角速度；具体为：

所述

的控制方式如下式(1.4)所示：

依据俯仰角与速度分量的关系如下式(1.5)，以及偏航角与速度分量的关系如下式(1.6)：

式(1.5)中，θ_i表示无人机i的俯仰角；

式(1.6)中，Ψ_i表示无人机i的偏航角；

则

步骤3：定义新的协同飞行控制指令v_i；

新的协同飞行控制指令v_i为下式(1.9)所示：

式(1.9)中，

表示无人机i飞行的新的加速度大小，

表示无人机i飞行的新的俯仰角速度，

表示无人机i飞行的新的偏航角速度；

所述

的具体形式如下式(1.10)所示：

式(1.10)中，

表示平均速度；

所述

的具体形式如下式(1.11)所示：

式(1.11)中，

表示航迹参考俯仰角；

所述

的具体形式如下式(1.12)所示：

式(1.12)中，

表示航迹参考偏航角；

步骤4：将步骤3中定义的所述新的协同飞行控制指令v_i作为输入，计算无人机的实际轨迹与参考轨迹的误差，航迹控制器基于PID算法，在所述误差满足预设条件的前提下设计PID控制律u_i；具体为：

PID控制律分为纵向与横侧向通道来设计，假设无人机的纵向与横侧向通道之间耦合较小，控制增稳系统的设计常按照纵向和横侧向分开进行的，互不交联，PID控制律u_i按下式(1.13)设计：

式(1.13)中，K_p,K_I,K_D分别为比例、积分、微分系数矩阵，

表示偏差。

步骤5：基于步骤4中设计的PID控制率，调整协同飞行控制系统的控制指令输入值，从而控制无人机的飞行轨迹和姿态。

3.根据权利要求1所述的基于分布式模型预测控制及队形控制的多机编队防撞方法，其特征在于，步骤四中所述的基于Leader-Follower进行队形保持控制的具体步骤为：

步骤A：建立长机与僚机的相对运动关系模型，依据该相对运动关系模型得出长机与僚机系统的状态变量；具体为：

航迹坐标系中无人机质心运动的动力学方程如下式(2.1)所示：

式(2.1)中：

表示加速度；

表示航迹倾斜角速度；

表示航迹方位角速度；T表示发动机推力；D表示无人机受到的阻力；m表示无人机及其载荷的质量；g表示重力加速度；μ表示航迹倾斜角；L表示无人机受到的升力；C表示无人机受到的侧力；

表示航迹方位角；φ表示滚转角；V表示无人机速度；

式(2.1)中，L、D、C依据下式(2.2)进行计算：

式(2.2)中：C_L表示升力系数；ρ表示大气密度；S表示机翼面积；C_Lα表示升力系数对迎角的导数；α表示无人机迎角；α₀表示初始迎角；C_D0表示阻力系数；K表示传递系数；C_Yβ表示侧向气动力系数；β表示无人机侧滑角；

依据上述式(2.1)，将长机相对僚机的距离转换到僚机坐标系中，建立长机与僚机的相对运动关系模型，如下式(2.3)所示：

式(2.3)中：x_d表示leader在follower坐标系下的相对距离在x轴上的分量；y_d表示leader在follower坐标系下的相对距离在y轴上的分量；z_d表示leader在follower坐标系下的相对距离在z轴上的分量；L_w表示僚机升力；C_w表示僚机侧力；m_w表示僚机质量；V_w表示follower在地面坐标系下的速度；μ_w表示follower在地面坐标系下的航迹倾斜角；φ_w表示follower自身的滚转角；V_L表示leader在地面坐标系下的速度；μ_L表示leader在地面坐标系下的航迹倾斜角；

表示leader和follower的相对方位角误差；

无人机受到的升力L，阻力D和侧力C是无人机速度V，迎角α，侧滑角β的函数；因此，系统的状态变量如下式(2.4)所示：

式(2.4)中：

表示僚机航迹方位角；

表示长机航迹方位角；

步骤B：依据步骤A中得出的所述系统的状态变量，基于Leader-Follower设计队形控制律；

步骤B.1：设定x_c,y_c,z_c为无人机编队中僚机与长机按要求应该保持的距离；依据步骤A得出的所述系统的状态变量，求取误差向量；所述误差向量中的误差项包括位置误差、速度误差、航迹倾斜角误差、航迹方位角误差以及航迹滚转角误差；所述误差向量如下式(2.5)所示：

步骤B.2：依据编队控制的最终目的是使目标误差e_x，e_y和e_z为零，并且假设侧滑角β为零，依据下式(2.6)所示的编队中长机与僚机的控制量以及步骤B.1中(2.5)式所示的误差向量，对僚机的控制变量给出如下式(2.7)所示的控制律，进而编队的控制量如下式(2.8)所示；

编队中长机与僚机的控制量如下式(2.6)所示：

僚机的控制变量的控制律如下式(2.7)所示：

式(2.7)中：K_ZP和K_ZI分别为高度控制的比例、积分系数；K_μP和K_μI分别为航迹倾斜角控制的比例、积分系数；K_XP和K_XI分别为经度控制的比例、积分系数；K_VP和K_VI分别为速度控制的比例、积分系数；K_YP和K_YI分别为维度控制的比例、积分系数；K_φP和K_φI分别为滚转角控制的比例、积分系数；

编队的控制量如下式(2.8)所示：

U_c＝[α T φ]^T (2.8)。

4.根据权利要求1至3任一所述的基于分布式模型预测控制及队形控制的多机编队防撞方法，其特征在于，步骤一中和步骤三中所述的基于分布式模型预测控制的编队防撞设置的设计过程包括以下步骤：

步骤a：建立编队预测模型；

步骤b：依据步骤a中建立的所述编队预测模型，将周围所有有可能发生碰撞的无人机通过数学的形式表示成限制约束惩罚项；

步骤c：利用步骤b中所述的惩罚项构建优化函数，使无人机在满足要求的情况下的整体惩罚最小，进而求解出控制量；

步骤d：将步骤c中求解出的所述控制量解算成轨迹控制，编队防撞设置完成。

5.根据权利要求4所述的基于分布式模型预测控制及队形控制的多机编队防撞方法，其特征在于：

所述步骤a：建立编队预测模型的具体建立过程为：

使用二阶积分动力学模型来表示无人机，其离散的动力学方程如下式(3.1)所示：

式(3.1)中：p_i[k+1]、v_i[k+1]分别表示第k+1时刻的位置、速度；p_i[k]、v_i[k]、a_i[k]分别表示第k时刻的位置、速度、加速度；h表示单位时间参数；

将无人机的状态表示成下式(3.2)：

式(3.2)中，

表示位置在时刻k_t+k的预测信息；

表示速度在时刻k_t+k的预测信息；k_t表示任一时刻；

使用式(3.2)可以将位置和速度表示成一个整体的状态，则K步的预测模型步长方程如下式(3.3)所示：

P_i＝A₀X_0，i+ΛU_i (3.3)；

式(3.3)中：Λ被定义为下式(3.4)所示：

式(3.3)和(3.4)中：Ψ＝[I₃ O₃]，

X_0，i表示初始的位置；P_i表示K步长的位置预测序列；U_i表示输入序列；

所述步骤b中的惩罚项包括期望的轨迹误差惩罚项、控制花销惩罚项、控制输入变化量惩罚项以及碰撞约束惩罚项；所述惩罚项数学的形式分别如下：

1)期望的轨迹误差惩罚项

上式(3.5)表示的是整个预测步长中从开始时刻到K时刻的轨迹和期望轨迹的误差二范数，将式(3.5)转化成二次规划形式如下式(3.6)所示：

式(3.6)中：J_e,i表示期望的轨迹误差惩罚项；P_d,i表示目的地；

表示正定的对角矩阵

给每一步的误差的权重系数；

2)控制花销惩罚项：

式(3.7)中：

表示的是惩罚项的系数；R表示控制成本的惩罚系数；

3)控制输入变化量惩罚项：

式(3.8)中：

S表示控制变化的误差惩罚系数；

4)碰撞约束惩罚项

碰撞约束项的加入可以让无人机当有碰撞的风险时，进行及时的规避：

式(3.9)中：

所述步骤c：利用步骤b中所述的惩罚项构建优化函数，使无人机在满足要求的情况下的整体惩罚最小，进而求解出控制量的具体构建过程为：

式(3.10)中，U_i表示控制量；A_in，b_in表示控制向量的不等式约束。

Images