CN113050687B - 一种多无人机编队重组轨迹规划方法 - Google Patents

Abstract

本发明公开了一种多无人机编队重组轨迹规划方法，首先建立包含动力学方程、通信距离约束条件、机动性能约束条件、安全距离约束、终端队形约束的多固定翼无人机编队重组问题的非线性最优控制模型；其次应用精准罚函数方法，针对非线性状态等式及不等式约束，将P0问题进一步处理为问题P1；最后应用hp自适应伪谱法将问题P1转化为非线性规划问题进行求解。与现有技术相比，本发明在解决多无人机编队重组轨迹优化问题时，与启发式算法相比，具有更快的收敛速度；与一般的hp自适应伪谱法相比，能够处理约束维度爆炸的问题，在不影响优化最优性的基础上，减小计算量，加快收敛速度。

Description

一种多无人机编队重组轨迹规划方法

技术领域

本发明涉及无人机技术领域，尤其涉及一种多无人机编队重组轨迹规划方法。

背景技术

无人机(UAV)在军事和民用领域具有广泛的应用，例如侦察、精确打击、飞行表演和地质勘测等。这些飞行任务通常需要无人机组改变其编队形状以满足不同任务的需要。因此，多无人机编队重组变得越来越重要。

多无人机编队重组问题，即以某些指标，如时间最快或燃油最省等为目标，求解每个无人机的控制输入，从而在满足某些约束的情况下，例如通信约束、机间防碰撞约束和飞行性能约束等，使多无人机组可以从初始队形重组为所需的新队形。为此，几种方法近年来已被应用于多无人机编队重组问题，例如PID控制[1-2]模型预测控制[3-5]，鲁棒控制[6-8]，滑模控制[9-10]。但这些方法通常将无人机编队重组问题处理为轨迹跟随问题，设计控制器使无人机组强制跟随规划好的航迹。

对于无人机组编队重组问题，可以被视为具有约束的优化问题。由于计算机性能的提高和无人机编队重配置问题的复杂非线性，通常采用数值方法来解决此类问题，主要分为间接方法和直接方法。值得一提的是，还有其他优化方法，例如基于动态规划的方法[11-12]，但当无人机组数量稍多时，动态规划方法将出现高维度灾难问题。通过间接方法，如果考虑无人机系统模型与复杂的代数约束，求解过程将变得复杂，难以求解。因此，直接方法更广泛地用于解决这些问题。通过离散化控制变量和状态变量，直接方法可以将连续时间的最优控制问题转换为有限维的非线性规划问题(NLP)，接下来即设计解决NLP问题的算法。近年来，在研究人员提出的算法中，由于能够跳出局部最优解，启发式算法在无人机组编队重组问题中应用颇多。如Duan等[13]提出了一种改进的粒子群优化(PSO)方法，基于控制参数化的方法，来搜索时间最优编队重组问题的局部和全局最优值。Duan等[14]又混合了粒子群优化算法与遗传算法(HPSOGA)，结合了PSO和GA的优势。但是，在这类方法中，尽管它能够跳出局部最优值，却不能保证收敛到所谓的全局最优值，并且其计算量大。作为直接法,伪谱方法在解决飞行器优化问题有较多应用。例如，张等[15]将hp自适应伪谱方法应用于多四旋翼无人机编队轨迹优化问题。Chai等[16]结合差分进化算法来与hp自适应Radau伪谱方法来处理飞行器再入轨迹优化问题。邵等[17]提出了改进的高斯伪谱法，用于多无人机协同轨迹规划问题。由于多无人机组编队重组问题中，原始系统维度高且需要考虑的约束多，加上伪谱法将原始问题转化为高维度的非线性规划问题，在处理高维度的无人机及无人机间复杂约束时，计算量会较大，收敛速度慢。对于上述方法，现有技术存在一些或多或少的缺点，存在的技术问题有：启发式算法等全局算法计算效率低，且无法保证收敛到全局最优解；一般的伪谱法在处理高维度的无人机及无人机间复杂约束时，计算量会较大，收敛速度慢。

发明内容

本发明为了解决以上问题，提供一种基于精确罚函数及hp自适应Radau伪谱法的多无人机编队重组轨迹优化方法，该方法首先建立包含动力学方程、通信距离约束条件、机动性能约束条件、安全距离约束的多固定翼无人机编队重组问题的非线性最优控制模型；利用hp自适应Radau伪谱法将连续时间的非线性最优控制模型转化为多区间的非线性最优控制模型,基于航迹的曲率密度函数实现伪谱法的自适应配点策略；并利用精准罚函数方法，处理在转化后的非线性规划问题中高维度的无人机及无人机间复杂约束，减小计算量，加快算法收敛速度。

为达到上述目的，本发明是按照以下技术方案实施的：

本发明一种多无人机编队重组轨迹规划方法包括以下步骤：

S1：建立包含动力学方程、通信距离约束条件、机动性能约束条件、安全距离约束、终端队形约束的多固定翼无人机编队重组问题的非线性最优控制模型；

S2：应用精准罚函数方法，针对非线性状态等式及不等式约束，将P0问题进一步处理为问题P1；

S3：应用hp自适应伪谱法将问题P1转化为非线性规划问题进行求解。

本发明的有益效果是：

本发明是一种多无人机编队重组轨迹规划方法，与现有技术相比，本发明在解决多无人机编队重组轨迹优化问题时，与启发式算法相比，具有更快的收敛速度；与一般的hp自适应伪谱法相比，能够处理约束维度爆炸的问题，在不影响优化最优性的基础上，减小计算量，加快收敛速度。

附图说明

图1是无人机群编队重构轨迹图；

图2是无人机群机间距离曲线图；

图3是无人机群推力曲线图；

图4是无人机群过载系数曲线图；

图5是无人机群滚转角曲线图。

具体实施方式

下面结合附图以及具体实施例对本发明作进一步描述，在此发明的示意性实施例以及说明用来解释本发明，但并不作为对本发明的限定。

如图1-5所示：首先建立包含动力学方程、通信距离约束条件、机动性能约束条件、安全距离约束、终端队形约束的多固定翼无人机编队重组问题的非线性最优控制模型。考虑三自由度固定翼无人机制导系统动态方程，

其中i＝1,2,...,N为无人机编号，γ为爬升角，χ为偏航角，μ为滚转角。T为推力，D为阻力，n代表的是无人机的负载系数，之后考虑无人机及及无人机间的通信距离约束条件、机动性能约束条件、安全距离约束、终端队形约束，通信距离及安全距离机间约束：

D_safe≤d_i,j(t)≤D_comm,

d_i,j(t)为任意两家无人机间的距离。

机动性能约束：v_min≤v_i≤v_max

χ_min≤χ_i≤χ_max

γ_min≤γ_i≤γ_max

T_min≤T_i≤T_max

n_min≤n_i≤n_max

假设第N架无人机为队形中心，则终端队形约束为：

h_i＝[x_i(t_f)-x_N(t_f)-x_i,N]²+[y_i(t_f)-y_N(t_f)-y_i,N]²+[z_i(t_f)-z_N(t_f)-z_i,N]²＝0

i＝1,2,...,N-1

考虑时间最有编队重组，目标函数可以表达为：

至此，多固定翼无人机编队重组问题的非线性最优控制问题可以表示为如下问题P0，

v_min≤v_i≤v_max

γ_min≤γ_i≤γ_max

T_min≤T_i≤T_max

n_min≤n_i≤n_max,i＝1,2,…,N

h_i＝0,i＝1,2,…,N-1

D_safe≤d_i,j(t)≤D_comm

第二部分为应用精准罚函数方法，针对非线性状态等式及不等式约束，将P0问题进一步处理为问题P1，目标函数变为

问题P1可以描述如下，

v_min≤v_i≤v_max

γ_min≤γ_i≤γ_max

T_min≤T_i≤T_max

n_min≤n_i≤n_max,i＝1,2,…,N

最后应用hp自适应伪谱法将问题P1转化为非线性规划问题进行求解。求解步骤大致为：

1)时间区间转换。假设飞机爬升时间为t∈[t₀,t_f]，将其分为K份网格区间[t_k-1,t_k)，k＝1,2,…,K，其中t_K＝t_f。在每个网格区间t∈[t_k-1,t_k)内，时间范围可由下式转化为τ∈[-1,1)，

2)选择LGR配点。在每个网格区间内选择M_k个LGR配点，通过求解下列M_k阶勒让德方程即可得到配点，

其中

3)状态和控制变量离散化。连续时间状态和控制变量可由拉格朗日多项式进行插值近似离散化，由于Radau伪谱法中的LGR配点不包括端点，为了保持状态变量在整个时间区间上的连续性以及终端状态值的优化，在每段时间区间上，增加τ＝1的配点，状态量与控制量可以近似为

其中，L_i(τ)为拉格朗日多项式基式。

4)动态方程转换。由上式，状态变量对τ的导数为

进而，状态方程可转化为下列代数约束

式中，

为在第k个时间区间的Radau伪谱微分矩阵。

5)目标函数以及约束近似化。考虑如下Bolza形式的目标函数

通过Radau伪谱法，目标函数可以近似为

式中

为积分权重系数。

将优化问题P1的不等式约束以及等式约束表示为

代入近似后的状态以及控制量后，约束可以近似为

式中，i＝1,2,…,M_k,k＝1,2,…,K。

经过上述步骤即可将优化问题P1转化为非线性规划问题，并进而可以通过基于梯度的非线性规划求解器来计算。

由于上述步骤将连续时间变量进行插值离散近似，因此我们需要评估离散化后的误差。若不满足误差，则需要继续增加配点数或细分当前时间区间。在第k个时间区间内选取每两个配点的中心位置，即

则在第k个网格区间内的误差可以由飞机运动方程约束满足程度来决定，

式中，1≤l≤M_k-1。设置全局最大容许误差值为E_max和最大曲率偏差值C^*，hp自适应网格更新策略如下。

(1)计算当前网格区间内运动方程约束的最大误差

判断

是否小于E_max。若小于则代表第K个网格区间满足误差要求，令k＝k+1，继续计算下一个网格区间的最大误差，直到k＝K；若

进行第2步。

(2)计算第k个网格区间内状态变量轨迹曲线的最大曲率比，其定义为

式中，

为状态轨迹的最大曲率，

为状态轨迹的平均曲率。若c_k≤C^*则进行第3步，继续将区间细分为若干个子区间；否则进行第4步，增大配点个数。

(3)增大网格区间内的配点个数M_k，新的配点个数为

其中，ceil(x)函数为向x增大的方向取整。令k＝k+1，返回第1步。

(4)继续细分网格区间，其子区间个数h_k为

令k＝k+1，返回第1步。

考虑五架无人机由初始的一字型编队重组为V字型编队，最大通信距离为200m，安全防碰撞距离为10m。根据文献[19]，本技术与hp自适应方法计算复杂度分别为O(N_pN)²、O(N_pN+N²N_p)²，N为无人机个数，N_p为hp自适应伪谱法各区间总配点数可见本技术在计算复杂度上较低。为了验证本技术的计算效率上的提高，表格1为考虑的五种不同的编队重组初始情况，本技术与hp自适应伪谱法的优化结果、优化求解耗费CPU时间对比见表格2。可见在牺牲极小的最优性的基础上，求解效率大大提高。

表格1无人机群编队重组不同情况

	无人机编号	初始坐标(m)	终端相对坐标(m)
					UAV1	(-10,30,40)	(-15,15,0)
情况1	UAV2	(20,30,55)	(-15,-15,0)
					UAV3	(50,50,65.5)	(0,0,0)
	UAV1	(0,10,40)	(-12,12,0)
				情况2	UAV2	(20,50,50)	(-12,-12,0)
	UAV3	(40,30,60)	(0,0,0)
					UAV1	(30,-20,50)	(-30,30,0)
	UAV2	(30,0,50)	(-15,15,0)
				情况3	UAV3	(30,20,50)	(0,0,0)
	UAV4	(30,40,50)	(-15,-15,0)
					UAV5	(30,60,50)	(-30,-30,0)
	UAV1	(10,-30,80)	(-30,-30,0)
					UAV2	(10,0,80)	(-15,-15,0)
情况4	UAV3	(10,30,80)	(0,0,0)
					UAV4	(10,60,80)	(-15,15,0)
	UAV5	(10,90,80)	(-30,30,0)

表格2求解时间与最优值(括号内)对比

参考文献：

[1]P.A.Sperandio Giacomin and E.M.Hemerly,“Reconfiguration betweenlongitudinal and circular formations for multi-UAV systems by usingsegments,”Journal of Intelligent and Robotic Systems,vol.78,no.2,pp.339–355,2015.

[2]M.Mahfouz,A.Hafez,M.Ashry,and G.Elnashar,“Formation configurationfor cooperative multiple UAV via backstepping PID controller,”in AIAA SPACEand Astronautics Forum and Exposition,2018.

[3]A.Hafez and S.Givigi,“Formation reconfiguration of cooperativeUAVs via learning basedmodel predictive control in an obstacle-loadedenvironment,”in 2016 Annual IEEE SystemsConference(SYSCON),2016,pp.601–608.

[4]S.Zhou,Y.Kang,H.Dai,and Z.Chao,“Multi-UAVs formation autonomouscontrol method basedon RQPSOFSM-DMPC,”Mathe-matical Problems in Engineering,2016.

[5]A.Grancharova,“UAVs trajectory planning by distributed MPC underradio communication pathloss constraints,”Journal of Intelligent andRoboticSystems,vol.79,no.1,pp.115–134,2015.

[6]D.Liu,H.Liu,Z.Li,X.Hou,and Q.Wang,“Robust attitude control fortail-sitter unmannedaerial vehicles in flight mode transitions,”InternationalJournal of Robust and NonlinearControl,vol.29,no.4,pp.1132–1149,2019.

[7]F.Liao,R.Teo,J.L.Wang,X.Dong,F.Lin,and K.Peng,“Distributedformation andreconfiguration control of VTOL UAVs,”IEEE Trans-actions onControl Systems Technology,vol.25,no.1,pp.270–277,2017.

[8]J.Ghommam,H.Mehrjerdi,and M.Saad,“Robust formation control withoutvelocitymeasurement of the leader robot,”Control Engineering Practice,vol.21,no.8,pp.1143–1156,2013.

[9]D.Wang,Q.Zong,B.Tian,H.Lu,and J.Wang,“Adaptive finite-timereconfiguration controlof unmanned aerial vehicles with a moving leader,”Nonlinear Dynamics,vol.95,no.2,pp.1099–1116,2019.

[10]F.A.Pradipta Lie and T.H.Go,“Reconfiguration control withcollision avoidanceframework for unmanned aerial vehicles in three-dimensional space,”Journal of AerospaceEngineering,vol.26,no.3,pp.637–645,2013.

[11]Z.Jin,T.Shima,and C.J.Schumacher,“Optimal scheduling forrefueling multipleautonomous aerial vehicles,”IEEE Transactions onRobotics,vol.22,no.4,pp.682–693,2006.[12]C.Colombo,M.Vasile,and G.Radice,“Optimal low-thrust trajectories to asteroids throughan algorithm based on differentialdynamic programming,”Celestial Mechanics and DynamicalAstronomy,vol.105,no.1-3,

[13]H.Duan,G.Ma,and D.Luo,“Optimal formation reconfiguration controlof multiple UCAVsusing improved particle swarm optimization,”Journal ofBionic Engineering,vol.5,no.4,pp.340–347,2008.

[14]H.Duan,Q.Luo,G.Ma,and Y.Shi,“Hybrid particle swarm optimizationand genetic algorithm for multi-UAV formation reconfiguration,”IEEEComputational Intelligence Magazine,vol.8,no.3,pp.16–27,2013.

[15]张博渊,宗群,鲁瀚辰,邵士凯.基于hp自适应伪谱法的四旋翼无人机编队轨迹优化[J].中国科学:技术科学,2017,47(03):239-248.

[16]R.Chai,A.L.Savvaris,and A.Tsourdos,“Violation learningdifferential evolution-based hp-adaptive pseudospectral method for trajectoryoptimization of space maneuver vehicle,”IEEE Transactions on Aerospace andElectronic Systems,vol.53,no.4,pp.2031–2044,AUG 2017.[17]邵士凯,彭瑜,贾慧敏,杜云.基于改进高斯伪谱法的多无人机协同轨迹规划[J].河北科技大学学报,2020,41(02):122-132.

[18]C.L.Darby,W.W.Hager,A.V.Rao,Direct trajectory optimization usinga variable low-order adaptive pseudospectral method,Journal of Spacecraft andRockets，vol.48,no.3,pp.433–445,2011.

C.Schmid,L.Biegler,Quadratic-programming methods for reduced HessianSQP,Computes and Chemical Engineering，vol.18,no.9,pp.817–832,1994.

本发明的技术方案不限于上述具体实施例的限制，凡是根据本发明的技术方案做出的技术变形，均落入本发明的保护范围之内。

Claims (1)

1.一种多无人机编队重组轨迹规划方法，其特征在于，包括以下步骤：

S1：建立包含动力学方程、通信距离约束条件、机动性能约束条件、安全距离约束、终端队形约束的多固定翼无人机编队重组问题的非线性最优控制模型；考虑三自由度固定翼无人机制导系统动态方程，

其中i＝1,2,...,N为无人机编号，γ为爬升角，χ为偏航角，μ为滚转角，T为推力，D为阻力，n代表的是无人机的负载系数，之后考虑无人机及及无人机间的通信距离约束条件、机动性能约束条件、安全距离约束、终端队形约束，通信距离及安全距离机间约束：

D_safe≤d_i,j(t)≤D_comm,

d_i,j(t)为任意两架无人机间的距离；

机动性能约束：

v_min≤v_i≤v_max

χ_min≤χ_i≤χ_max

γ_min≤γ_i≤γ_max

T_min≤T_i≤T_max

n_min≤n_i≤n_max

设第N架无人机为队形中心，则终端队形约束为：

考虑时间最有编队重组，目标函数可以表达为，

至此，多固定翼无人机编队重组问题的非线性最优控制问题可以表示为如下问题P0，

s.t.

v_min≤v_i≤v_max

γ_min≤γ_i≤γ_max

T_min≤T_i≤T_max

n_min≤n_i≤n_max,i＝1,2,…,N

h_i＝0,i＝1,2,…,N-1

D_safe≤d_i,j(t)≤D_comm

S2：应用精准罚函数方法，针对非线性状态等式及不等式约束，将P0问题进一步处理为问题P1；目标函数变为

问题P1可以描述如下：

s.t.

v_min≤v_i≤v_max

γ_min≤γ_i≤γ_max

T_min≤T_i≤T_max

n_min≤n_i≤n_max,i＝1,2,…,N

S3：应用hp自适应伪谱法将问题P1转化为非线性规划问题进行求解，包括以下步骤：

1)时间区间转换：设飞机爬升时间为t∈[t₀,t_f]，将其分为K份网格区间[t_k-1,t_k)，k＝1,2,…,K，其中t_K＝t_f；在每个网格区间t∈[t_k-1,t_k)内，时间范围可由下式转化为τ∈[-1,1)，

2)选择LGR配点：在每个网格区间内选择M_k个LGR配点，通过求解下列M_k阶勒让德方程即可得到配点，

其中

3)状态和控制变量离散化：连续时间状态和控制变量由拉格朗日多项式进行插值近似离散化，由于Radau伪谱法中的LGR配点不包括端点，为了保持状态变量在整个时间区间上的连续性以及终端状态值的优化，在每段时间区间上，增加τ＝1的配点，状态量与控制量可以近似为

其中，L_i(τ)为拉格朗日多项式基式；

4)动态方程转换：由上式，状态变量对τ的导数为

进而，状态方程可转化为下列代数约束

式中，

为在第k个时间区间的Radau伪谱微分矩阵；

5)目标函数以及约束近似化：考虑如下Bolza形式的目标函数

通过Radau伪谱法，目标函数可以近似为

式中

为积分权重系数；

将优化问题P1的不等式约束以及等式约束表示为

代入近似后的状态以及控制量后，约束可以近似为

式中，i＝1,2,…,M_k,k＝1,2,…,K；

经过上述步骤即可将优化问题P1转化为非线性规划问题，并进而能够通过基于梯度的非线性规划求解器来计算；

求解步骤后需要评估离散化后的误差，若不满足误差，则需要继续增加配点数或细分当前时间区间，在第k个时间区间内选取每两个配点的中心位置，即

则在第k个网格区间内的误差可以由飞机运动方程约束满足程度来决定，

式中，1≤l≤M_k-1；设置全局最大容许误差值为E_max和最大曲率偏差值C^*，hp自适应网格更新策略如下：

(1)计算当前网格区间内运动方程约束的最大误差

判断

是否小于E_max；若小于则代表第K个网格区间满足误差要求，令k＝k+1，继续计算下一个网格区间的最大误差，直到k＝K；若

进行第(2)步；

(2)计算第k个网格区间内状态变量轨迹曲线的最大曲率比，其定义为

式中，

为状态轨迹的最大曲率，

为状态轨迹的平均曲率；若c_k≤C^*则进行第3步，继续将区间细分为若干个子区间；否则进行第4步，增大配点个数；

(3)增大网格区间内的配点个数M_k，新的配点个数为

其中，ceil(x)函数为向x增大的方向取整；令k＝k+1，返回第(1)步；

(4)继续细分网格区间，其子区间个数h_k为

令k＝k+1，返回第(1)步。

Images