CN107219857B - 一种基于三维全局人工势函数的无人机编队路径规划算法 - Google Patents

Abstract

发明公开了一种基于三维全局人工势函数的无人机编队路径规划算法，该算法主要解决无人机在三维空间环境下的队形保持、路径规划以及三维避障等问题。该算法首先建立三维空间下全局势函数的数学模型；然后根据期望队形建立无人机在三维空间下队形几何方程；最后建立含有队形约束的编队无人机运动学方程。该方法通过队形几何方程建立多无人机在三维空间下的几何关系，并采用罚函数法对约束动力学方程进行求解得到编队无人机的飞行轨迹。与传统经典人工势函数相比不仅可以解决二维经典人工势函数的二维空间局限问题，而且可以解决经典势函数存在局部极小值等问题。本发明的算法在解决二维经典势函数的局限性以及编队无人机的路径规划有很好的有效性。

Description

一种基于三维全局人工势函数的无人机编队路径规划算法

技术领域

本发明涉及无人机编队路径规划和无人机编队队形控制领域，具体是一种基于三维全局人工势函数的无人机编队路径规划算法。

背景技术

当前随着航空航天技术的不断发展，无人机无论在军事侦查、地面打击等军事领域还是在森林火灾、灾害救援、电力巡线等民用领域都起到了举足轻重的作用。尽管如此，我们更要看到单架无人机在执行任务时由于受到个体的智能化程度和可靠性等诸多的因素的制约，造成在执行相关任务时仍然存在很多问题甚至无法完成指定任务。例如：单无人机由于受到自身传感器数量或者传感器角度限制等因素，不能够完整的获取目标区域环境的整体环境信息；在进行对地面目标进行军事打击时，由于单机作战范围较窄、载弹量有限造成执行任务效率较低等问题。

人工势场法(Artificial Potential Field)自上世纪八十年代Khatib’s提出以来，凭借建模容易、数学计算简单等优点迅速引起了广大研究人员的青睐，并且在机器人领域得到了广泛的应用。然而经过几十年的发展人工势场法依然局限于对经典二维人工势场法的改进和使用方面。

发明内容

本发明要解决的技术问题是提供一种基于三维全局人工势函数的无人机编队路径规划算法，该算法能够突破二维经典人工势函数的局限性和不足，解决无人机编队在三维空间环境下的队形保持、路径规划以及三维避障等问题。

为了实现上述发明目的，本发明采用以下技术方案：

首先建立无人机编队的三维环境、自由飞行空间和三维空间内障碍物数学模型，得到三维空间全局人工势函数；然后根据三维空间全局人工势函数对无人机编队进行受力分析，得出无人机编队在三维空间内的所受力的大小和方向；最后建立三维空间内无人机编队期望队形的几何方程，结合队形几何方程引入拉格朗日乘子建立含有队形约束的无人机编队约束动力学方程；最后求解约束动力学方程。

进一步的，所述建立无人机编队的三维环境、自由飞行空间和三维空间内障碍物的数学模型过程包括：建立三维空间简化和抽象的三维惯性坐标系O_g；其中x_g轴在水平面内指向某一方向，z_g轴垂直于地面指向正上方，y_g轴在水平面内垂直于x_g轴，方向满足右手定则；对三维空间内障碍物进行抽象；

无人机编队的三维环境、自由飞行空间W和三维空间内所有障碍物模型统一函数γ分别为：

其中：γ_i表示三维空间内障碍物模型i的三维几何方程；i＝0时γ_i表示三维空间的边界几何方程；i＝1,2,3…N-1表示三维空间内的(N-1)个障碍物；其中，γ＜0表示点位于障碍物内部或者不在三维自由空间内；γ≥0表示点位于除障碍物的三维自由空间内；

表示三维立体模型的几何方程。

进一步的，所述根据建立的三维环境和三维空间内障碍物数学模型得到三维空间全局人工势函数的过程包括：

建立三维空间内无人机编队与目标区域的之间的函数：

其中，q_i表示在无人机i的空间位置，q_i＝[x_i,y_i,z_i]^T；q_G表示目标区域位置,q_G＝[x_G,y_G,z_G]^T；||q_i-q_G||表示无人机i与目标区域之间的欧式距离；k为比例因子(k>0)；v_i表示无人机i速度因子；

定义实值函数运算关系：

f₁*f₂＝f₁(f₂(x)) (12)

其中，f₁(x)、f₂(x)均为实值映射函数；

建立同胚映射函数：定义将实值[0,+∞]映射到[0,μ]的微分同胚映射函数为：

其中，λ为映射控制参数；μ为映射参数。

建立锐化函数：定义锐化函数σ_k为关于x的k次方根函数：

σ_k(x)＝x^1/k (11)

其中，在算法中锐化函数主要应用在降低自变量幅角，使降幅角后的函数满足莫尔斯函数条件；

建立三维全局人工势函数：根据上述步骤，定义三维全局人工势函数的表达式为：

其中，N表示空间内障碍物的个数(N＝0,1，2…N-1)；q为立体空间内三维坐标q=[x,y,z]^T。

上述得到三维空间全局人工势函数的过程中，步骤可以调换顺序。

进一步的，对无人机编队进行受力分析的过程为：在对无人机编队进行受力分析时，首先对无人机编队进行总体受力分析，然后对单无人机在三维各个方向进行受力分析；无人机编队总体受力为F_Total，单无人机在三维各个方向的受力为

其中，U_Total表示三维全局势函数；

表示无人机i分别在三维x,y,z方向上的力。

进一步的，所述建立三维空间内无人机期望队形几何方程的过程为：算法定义无人机在三维空间的运动视为刚体运动，并定义三维空间内无人机编队的队形为无人机之间的相对欧氏距离不变；无人机编队个数与约束方程个数的关系为：

M＝2N-3 (19)

其中M表示约束方程个数，N表示参与编队的无人机个数。

进一步的，所述建立含有队形约束的无人机编队约束动力学方程的过程包括：

对无人机编队运动学建模：对无人机编队进行运动学建模时，首先将无人机编队运动视为无人机在受三维人工势函数产生的虚拟力的作用下向目标点飞行的正向动力学问题；同时将无人机编队的队形视为弹性队形；因此，通过对无人机编队的受力分析，结合无人机编队期望队形建立无人机编队的动力学方程如下：

ρ＝ρ(x,y,z,t)＝0 (22)

其中，q＝[x,y,z]^T为无人机三维空间位置矩阵；λ为拉格朗日乘子；

为n维的合外力矩阵；L为比例因子；M无人机编队的权重矩阵；

为无人机在三维空间内所受合外力；ρ(x,y,z,t)为队形约束方程；

为队形约束方程的雅可比矩阵；

建立无人机编队罚函数数学模型；建立罚函数数学模型时，合并队形约束方程作为一个辅助动力学系统并通过罚函数因子予以惩罚；无人机编队罚函数模型为：

其中，μ为罚函数惩罚因子(μ→+∞)；μα(x)为惩罚项；p为正整数，通常取p＝2。

罚函数的可行域为：

D＝{x,y,z|ρ(x,y,z,t)＝0} (25)

因此惩罚项μα(x)为：

求解无人机编队运动学罚函数模型：在对无人机编队约束动力学方程进行求解时，首先利用线性或者非线性的虚拟弹簧(阻尼器)代替无人机编队的队形完整性约束；此时，λ为一个基于违反队形约束程度的虚拟弹簧(阻尼器)产生的力，并且假定此时弹簧的刚度和阻尼系数恒定。因此在满足罚函数可行域D的情况下，λ的表达式为：

其中，L_S为弹性因子为n*n维的对角矩阵；L_D为阻尼因子为n*n维的对角矩阵；ρ为队形约束方程。

结合公式(20)，(26)可得罚函数下无人机编队的约束动力学方程为：

无人机编队的约束动力学方程转换到状态空间下为：

本发明的一种基于三维全局人工势函数的无人机编队路径规划算法，与二维经典势函数相比较，不仅可以解决经典人工势函数的二维空间局限问题，而且可以解决经典势函数存在局部极小值等问题。通过仿真实验表明，本发明的算法能够很好的解决无人机编队在三维空间下的路径规划和队形保持问题。在解决二维经典人工势函数的局限性和存在的问题时，有很好的灵活性和有效性。

附图说明

图1为本发明无人机编队三维典型运行环境示意图；

图2为本发明三维空间典型运行环境的简化和抽象结果示意图；

图3为三维人工势函数模型建模流程图；

图4为以6架无人机三角形编队为例的弹性队形结构示意图；

图5为为求解无人机编队约束动力学方程流程图。

具体实施方式

下面结合附图，对本发明提出的一种基于三维全局人工势函数的无人机编队路径规划算法进行详细说明。

本发明的基于三维全局人工势函数的无人机编队路径规划算法主要包括以下步骤：

首先建立无人机编队的三维环境和三维空间内障碍物数学模型，得到三维空间全局人工势函数；然后根据三维空间全局人工势函数对无人机编队进行受力分析，得出无人机编队在三维空间内的所受力的大小和方向；最后建立三维空间内无人机编队期望队形的几何方程，结合队形几何方程引入拉格朗日乘子建立含有队形约束的无人机编队约束动力学方程；最后求解约束动力学方程。

具体来讲，如图1所示，为本发明公开的一种基于三维全局人工势函数的无人机编队路径规划算法的三维典型运行环境示意图。在三维空间环境中主要包括防空导弹基地1，地面军事基地2、空中移动目标3、无人机编队4、目标空域5、山地6、地面雷达探测基地7等。在进行无人机编队路径规划时，要求无人机编队绕开所有的地面防空设施和障碍，同时保持完整的预定队形顺利到达目标空域

如图2所示，为无人机编队三维典型运行环境的简化和抽象结果示意图。在对三维空间环境进行抽象和简化时，首先建立无人机编队飞行环境的三维空间惯性坐标系，同时根据地面防空设施的特点，将它们抽象为不同的三维模型，并建立相应的三维数学障碍物模型。

具体抽象和简化步骤如下：

11)建立无人机编队三维典型运行环境惯性坐标系O_G；

12)建立三维空间简化和抽象的三维惯性坐标系O_g；其中x_g轴在水平面内指向某一方向，z_g轴垂直于地面指向正上方，y_g轴在水平面内垂直于x_g轴，方向满足右手定则；

13)根据地面和空中不同物体的特点，将它们分别进行简化和抽象。地面防空导弹1、地面军事基地2抽象为半球形；空中移动目标3抽象为球形；目标空域5抽象为圆形；山地6、地面雷达探测基地7抽象为圆锥形。

详细的抽象和简化步骤在下面的描述中会更加详细。

如图3所示，为无人机编队三维人工势函数模型建模流程图。具体步骤如下：

201)首先建立三维空间的边界数学模型

和三维空间内障碍物数学模型γ_i，该算法定义三维空间环境的数学模型为三维立体模型的几何方程

公式(1)中三维空间边界为球形三维空间边界。其中：γ_i表示三维空间内障碍物模型i的三维几何方程；i＝0时γ_i表示三维空间的边界几何方程；i＝1,2,3…N-1表示三维空间内的(N-1)个障碍物；其中，γ＜0表示点位于障碍物内部或者不在三维自由空间内；γ≥0表示点位于除障碍物的三维自由空间内。

以椭球型障碍物数学模型为例时，可以得到如下障碍物数学模型：

其中，

表示

是n维欧式空间的子集；q表示三维空间内的坐标q＝[x,y,z]^T；q₀表示三维球形空间的中心坐标，如果以原点为中心则q₀＝[0,0,0]^T；r₀表示三维球形空间的半径，a,b分别表示x,y方向的半周长，c＝a或c＝b表示分别绕y轴和x轴旋转得到的椭球，dx,dy,dz表示椭球在x,y,z方向上的偏移量。

此时，

半球型障碍物数学模型为例时，障碍物的方程为：

其中，R_x,R_y表示半球中心，R表示半球半径。

球型障碍物数学模型为例时，障碍物的方程为：

(x-a_x)²+(y-a_y)²+(z-a_z)²-R²≤0 (7)

其中，a_x,a_y,a_z表示球形中心，R表示球体半径。

圆形障碍物的数学模型为：

其中，a_tx,a_ty表示目标空域的水平面坐标，R_t表示目标空域半径，z_t目标空域半径。

圆锥形障碍物数学模型为例时，障碍物的方程为：

其中，k_c为比例因子，a_cx,a_cy为圆锥的平面坐标，H圆锥的高度。

202)建立同胚映射函数。其目的主要是将实值函数[0,+∞]映射到[0,μ]实值函数，同胚映射函数如下：

其中，λ为映射控制参数；μ为映射参数。

203)建立相应的锐化函数。其目的主要是为了降低实值函数的自变量幅角，使降幅角后的实值函数满足莫尔斯函数条件。因此定义锐化函数σ_k为关于x的k次方根函数：

σ_k(x)＝x^1/k (11)

204)重新定义函数复合运算。该算法定义实值映射函数的运算关系为：

f₁*f₂＝f₁(f₂(x)) (12)

其中，f₁(x)、f₂(x)均为实值映射函数。

205)建立目标距离函数。该算法定义三维空间内无人机编队与目标区域的之间的目标距离函数为：

其中，q_i表示在无人机i的空间位置，q_i＝[x_i,y_i,z_i]^T；q_G表示目标区域位置,q_G＝[x_G,y_G,z_G]^T；||q_i-q_G||表示无人机i与目标区域之间的欧式距离；k为比例因子(k>0)；v_i表示无人机i速度因子。

206)判断目标距离函数是否满足莫尔斯函数条件。在建立三维全局势函数之前，首先要判断目标距离函数f_k是否满足莫尔斯函数条件，如果不满足则将目标距离函数f_k带入锐化函数，按照公式(12)的运算规则计算，直到f_k满足莫尔斯函数条件。

例如：当公式(13)中的k＝2时，目标距离函数

不满足莫尔斯函数条件，因为其在原点时海森矩阵为零矩阵。然而利用锐化函数σ_k可得：

满足莫尔斯函数条件。

207)建立三维全局人工势函数。由公式(1)-(13)定义三维全局人工势函数的表达式Ψ_κ(q)为：

其中，N表示空间内障碍物的个数(N＝0,1,2…N-1)；q为立体空间内三维坐标q＝[x,y,z]^T。

208)对无人机编队进行受力分析。在对无人机编队进行受力分析时，首先对无人机编队进行总体受力分析，然后对单无人机在三维各个方向进行受力分析；无人机编队总体受力为F_Total，单无人机在三维各个方向的受力为

其中，U_Total表示三维全局势函数；

表示无人机i分别在三维x,y,z方向上的力。

209)建立三维空间内无人机期望队形几何方程：算法定义无人机在三维空间的运动视为刚体运动，并定义三维空间内无人机编队的队形为无人机之间的相对欧氏距离不变；无人机编队个数与约束方程个数的关系为：

M＝2N-3 (19)

其中M表示约束方程个数，N表示参与编队的无人机个数。

进一步的，所述建立含有队形约束的无人机编队约束动力学方程的过程包括：

210)对无人机编队运动学建模：对无人机编队进行运动学建模时，首先将无人机编队运动视为无人机在受三维人工势函数产生的虚拟力的作用下向目标点飞行的正向动力学问题；同时将无人机编队的队形视为弹性队形；因此，通过对无人机编队的受力分析，结合无人机编队期望队形建立无人机编队的动力学方程如下：

ρ＝ρ(x,y,z,t)＝0 (22)

其中，q＝[x,y,z]^T为无人机三维空间位置矩阵；λ为拉格朗日乘子；

为n维的合外力矩阵；L为比例因子；M无人机编队的权重矩阵；

为无人机在三维空间内所受合外力；ρ(x,y,z,t)为队形约束方程；

为队形约束方程的雅可比矩阵；

211)建立无人机编队罚函数数学模型。建立罚函数数学模型时，合并队形约束方程作为一个辅助动力学系统并通过罚函数因子予以惩罚；无人机编队罚函数模型为：

其中，μ为罚函数惩罚因子(μ→+∞)；μα(x)为惩罚项；p为正整数，通常取p＝2。

罚函数的可行域为：

D＝{x,y,z|ρ(x,y,z,t)＝0} (25)

因此惩罚项μα(x)为：

212)求解无人机编队运动学罚函数模型。如图4所示，为以6架无人机三角形编队为例的弹性队形结构示意图。在利用罚函数法求解无人机编队运动学方程时，首先利用线性或者非线性的虚拟弹簧(阻尼器)代替无人机编队的队形完整性约束，这使得该算法可以合并队形约束方程作为一个辅助动力学系统并通过罚函数因子予以惩罚。此时，λ为一个基于违反队形约束程度的虚拟弹簧(阻尼器)产生的力，并且假定此时弹簧的刚度和阻尼系数恒定。λ的表达式为：

其中，L_s为弹性因子为n*n维的对角矩阵；L_D为阻尼因子为n*n维的对角矩阵；ρ为队形约束方程。

结合公式(23)，(27)可得罚函数下无人机编队的约束动力学方程为：

无人机编队的约束动力学方程转换到状态空间下为：

根据公式(19)可得，以6无人机为例时无人机编队的固定队形约束方程为：

其中，d_ij为无人机i,j之间的期望距离。

如图5所示，为无人机编队约束动力学方程求解流程图。具体步骤如下：

31)首先无人机编队三维飞行环境建模；

无人机编队初始化，其中包括编队中个无人机的位置、速度以及期望队形等；

32)对编队中个无人机进行受力分析。利用公式(14)-(18)计算无人机编队在当前位置所受到的合力与分力，得到编队中各无人机在三维方向上所受的分力和合力；

33)把无人机编队在三维空间中的运动视为仅受到三维虚拟力的作用下的正向动力学问题。利用MATLAB每0.05步长求解无人机编队约束动力学方程得到编队中各个无人机下一个位置点和飞行方向角度信息；

34)判断无人机编队受到的合力是否为零，来确定当前无人机编队；

35)飞行到目标区域。如果是，则输出无人机编队的路径规划曲线、路径信息以及误差曲线，程序转到步骤36)；如果否，则跳转至步骤33)；

36)无人机编队约束动力学方程结算结束。

基于对本发明优选实施方式的描述，应该清楚，由所附的权利要求书所限定的本发明并不仅仅局限于上面说明书中所阐述的特定细节，未脱离本发明宗旨或范围的对本发明的许多显而易见的改变同样可能达到本发明的目的。

Claims (2)

1.一种基于三维全局人工势函数的无人机编队路径规划算法，其特征在于，包括以下步骤：

首先建立无人机编队的三维环境、自由飞行空间和三维空间内障碍物的数学模型，得到三维空间全局人工势函数；然后根据三维空间全局人工势函数对无人机编队进行受力分析，得出无人机编队在三维空间内的所受力的大小和方向；最后建立三维空间内无人机编队期望队形的几何方程，结合队形几何方程引入拉格朗日乘子建立含有队形约束的无人机编队约束动力学方程；最后求解约束动力学方程；

所述建立无人机编队的三维环境、自由飞行空间和三维空间内障碍物的数学模型过程包括：建立三维空间简化和抽象的三维惯性坐标系O_g；其中x_g轴在水平面内指向某一方向，z_g轴垂直于地面指向正上方，y_g轴在水平面内垂直于x_g轴，方向满足右手定则；对三维空间内障碍物进行抽象；

无人机编队的三维环境的数学模型

自由飞行空间W和三维空间内所有障碍物的数学模型统一函数γ分别为：

其中：γ_i表示三维空间内障碍物模型i的三维几何方程；i＝0时γ_i表示三维空间的边界几何方程；i＝1，2，3...N-1表示三维空间内的N-1个障碍物；其中，γ＜0表示点位于障碍物内部或者不在三维自由空间内；γ≥0表示点位于除障碍物的三维自由空间内；

表示三维立体模型的几何方程；q为立体空间内三维坐标，

根据所述建立的无人机编队的三维环境、自由飞行空间和三维空间内障碍物的数学模型，得到三维空间全局人工势函数的过程包括：

建立三维空间内无人机编队与目标区域的之间的函数：

其中，q_i表示在无人机i的空间位置，q_i＝[x_i，y_i，z_i]^T；q_G表示目标区域位置，q_G＝[x_G，y_G，z_G]^T；||q_i-q_G||表示无人机i与目标区域之间的欧式距离；为比例因子k＞0；v_i表示无人机i速度因子；

定义实值函数运算关系：

f₁*f₂＝f₁(f₂(x)) (12)

其中，f₁(x)、f₂(x)均为实值映射函数；

建立同胚映射函数：定义将实值[0，+∞]映射到[0，μ]的微分同胚映射函数为：

其中，λ为映射控制参数；μ为映射参数；

建立锐化函数：定义锐化函数σ_k为关于x的k次方根函数：

σ_k(x)＝x^1/k (11)

其中，在算法中锐化函数主要应用在降低自变量幅角，使降幅角后的函数满足莫尔斯函数条件；

建立三维全局人工势函数：根据上述步骤，定义三维全局人工势函数的表达式为：

其中，N表示空间内障碍物的个数，其中N＝0，1，2...N-1；q为立体空间内三维坐标q＝[x，y，z]^T；

所述建立含有队形约束的无人机编队约束动力学方程的过程包括：

对无人机编队运动学建模：对无人机编队进行运动学建模时，首先将无人机编队运动视为无人机在受三维人工势函数产生的虚拟力的作用下向目标点飞行的正向动力学问题；同时将无人机编队的队形视为弹性队形；因此，通过对无人机编队的受力分析，结合无人机编队期望队形建立无人机编队的动力学方程如下：

ρ＝ρ(x，y，z，t)＝0 (22)

其中，q＝[x，y，z]^T为无人机三维空间位置矩阵；λ为拉格朗日乘子；

为n维的合外力矩阵；L为比例因子；M无人机编队的权重矩阵；

为无人机编队总体受力；ρ(x，y，z，t)为队形约束方程；

为队形约束方程的雅可比矩阵；

建立无人机编队罚函数数学模型；建立罚函数数学模型时，合并队形约束方程作为一个辅助动力学系统并通过罚函数因子予以惩罚；无人机编队罚函数模型为：

其中，罚函数惩罚因子μ→+∞；μα(x)为惩罚项；p为正整数，通常取p＝2；

罚函数的可行域为：

D＝{x，y，z|ρ(x，y，z，t)＝0} (25)

因此惩罚项μα(x)为：

求解无人机编队运动学罚函数模型：在对无人机编队约束动力学方程进行求解时，首先利用线性或者非线性的虚拟弹簧代替无人机编队的队形完整性约束；此时，λ为一个基于违反队形约束程度的虚拟弹簧产生的力，并且假定此时虚拟弹簧的刚度和阻尼系数恒定；因此在满足罚函数可行域D的情况下，λ的表达式为：

其中，L_S为弹性因子为n*n维的对角矩阵；L_D为阻尼因子为n*n维的对角矩阵；ρ为队形约束方程；

结合公式(20)和(26)可得罚函数下无人机编队的约束动力学方程为：

无人机编队的约束动力学方程转换到状态空间下为：

对无人机编队进行受力分析的过程为：在对无人机编队进行受力分析时，首先对无人机编队进行总体受力分析，然后对单无人机在三维各个方向进行受力分析；无人机编队总体受力为F_Total，单无人机在三维各个方向的受力为

其中，U_Total表示三维全局势函数；

表示无人机i分别在三维x，y，z方向上的力。

2.根据权利要求1所述的基于三维全局人工势函数的无人机编队路径规划算法，其特征在于，所述建立三维空间内无人机期望队形几何方程的过程为：算法定义无人机在三维空间的运动视为刚体运动，并定义三维空间内无人机编队的队形为无人机之间的相对欧氏距离不变；无人机编队个数与约束方程个数的关系为：

M＝2N-3 (19)

其中M表示约束方程个数，N表示参与编队的无人机个数。

Images