基于模糊虚拟力的无人机航路规划方法

摘要

为解决基于虚拟力航路规划方法出现的局部极小问题，并实现规划过程中实时的自适应规划参数设置，本发明采用基于模糊虚拟力(FVF)的无人机航路规划方法，其采用贝叶斯网络和模糊逻辑推理相结合进行实时的自适应规划参数设置，并提出威胁合并法解决虚拟力法的局部极小问题。本发明的基于模糊虚拟力的无人机航路规划方法包含以下步骤：1)设置无人机航路规划的初始条件，包括规划起始点、目标点、威胁分布及属性；2)设置无人机航路规划的迭代步长；3)设置规划参数，从而确定虚拟斥力系数与虚拟引力系数之间的关系；4)进行航路规划；5)判断是否进入局部极小，若是，则进行威胁合并，若否，则继续进行航路规划直至目标点。

说明书

技术领域

本发明涉及一种无人机的航路规划方法，特别是一种基于模糊虚拟力的无 人机航路规划方法。

背景技术

航路规划是无人机领域的重要研究内容，航路规划问题是在特定约束的条 件下，实时求取一条介于起始点与目标点间的最优或可行的航路，使得执行战 术任务的无人机能突防敌方的威胁环境，并在敌方防空区域内完成特定任务， 同时保存自己，达到最佳的作战效果。

目前研究中，常采用基于虚拟力(VF)的航路规划方法。该方法最初是专 为快速移动机器人实时避障而设计的，可使机器人在障碍物之间快速、连续并 且平稳地运动。其基本思想是将机器人作为工作空间的一个质点，在虚拟力的 作用下移动。虚拟力函数通常定义为自由空间中来自目标点的引力与来自障碍 物的斥力的叠加。虚拟力法的最显著的特点是算法简洁、实时性强。

利用虚拟力方法的航路规划过程如下：

定义无人机航路规划过程为一个虚拟的无人机沿给定起始点和目标点的特定航 路移动的过程。虚拟力分为两部分：来自规划目标点的虚拟引力以及来自各威 胁的虚拟斥力。虚拟力法航路规划，其方向的改变取决于虚拟力的方向，因此 基于虚拟力的航路规划过程可看成是一个混杂系统的优化问题。其中移动过程 是连续的，方向改变是离散的。在这个混杂系统中，由于目标给定因此末态已 知，而时间未知。

混杂系统优化主要是解决航路规划在何时进行切换来保证航路最优，代价最 小。

该混杂系统的演化规律可由如下微分动态方程表示：

$\left(\begin{array}{c}\stackrel{·}{x}\\ \stackrel{·}{y}\end{array}\right)=U_1\left(\begin{array}{c}1\\ 0\end{array}\right)+U_2\left(\begin{array}{c}0\\ 1\end{array}\right)+U_3\left(\begin{array}{c}\sqrt{2}/2\\ \sqrt{2}/2\end{array}\right)+$

$U_4\left(\begin{array}{c}-\sqrt{2}/2\\ \sqrt{2}/2\end{array}\right)+U_5\left(\begin{array}{c}\sqrt{2}/2\\ -\sqrt{2}/2\end{array}\right)$

其中，状态变量x，y为规划点的横、纵坐标。U_i(i＝1，2，…，5)表示离散输入，其 中每个元素的取值为{0，v}，v为虚拟无人机的速度。U_i可以表示为 U_i＝g(x，y，Φ，Δ)，其中Φ为总的威胁度，Δ为目标点位置。

代价函数定义为

J＝a·∫_TΦ·v·dt+b·Γ

其中α，b分别为航路代价与转弯代价的权值，Γ为航路转弯次数。如果虚拟无 人机的方向改变达到s次，混杂系统的优化问题则是找到合适的离散时序， T＝[τ₀，τ₁]，[τ₁，τ₂]，[τ₂，τ₃]，…[τ_s-1，τ_s]，使得代价函数J最小。τ_j(j＝1，2，…，s)方向改变 的时间点，[τ_j-1，τ_j]虚拟无人机沿航路移动的时间间隔。

用混杂系统理论对基于虚拟力的无人机航路规划过程进行建模，并将混杂 自动机描述成A＝(V_D，Q，μ₁，μ₂，μ₃)，其中：

(1)数据变量：V_D＝{x(t)，y(t)}表示规划点位置集合，状态变量x，y为规划点 的横、纵坐标。

(2)状态：Q＝{1，2，3，4，5}表示对应规划方向的有限状态集。

(3)行为：

代表无人机在xy平面内沿x轴正方向上的运动；

代表无人机在xy平面内沿y轴正方向上的运动；

代表无人机在xy平面内沿第一象限角平分线方向上 的运动；

代表无人机在xy平面内沿第二象限角平分线方向 上的运动；

代表无人机在xy平面内沿第四象限角平分线方向 上的运动；

其中，v为虚拟无人机的速度。

(4)状态恒量：

μ₂→{M_min≤J＝a·∫_TΦ·v·dt+b·Γ≤M_max}，其中M表示边界代价值。

(5)切换条件：

μ₃→{J＝M_max，U_i＝g(x，y，Φ，Δ)}

无人机航路规划问题包括诸多约束条件，如最小规划步长，航路距离限制， 最大转弯角等等。具体解决方案如下：

最小规划步长(L_min)：L_min＜(τ_j+1-τ_j)·v

航路距离限制(L_max)：∫_Tv·dt＜L_max

给定行为(μ₁(1)，μ₁(2)，…，μ₁(5))，可以确保最大转弯角约束条件的满足。

基于上述混杂自动机的优化求解步骤如下：

(1)定义实时代价将当前点设为起始点，Δd_j表示单位距离；

(2)从当前点开始计算J_rt；

(3)当J_rt≥M_max且(τ_i+1-τ_i)·v＞L_min时，对应的航路点设置为当前点，重新计 算虚拟合力的映射方向，即为切换结果；

(4)重复步骤(2)(3)，直至到达目标点；

(5)计算J＝a·∑J_rt+b·Γ和∫_T v·dt，改变M_max值，循环执行(1)(2)(3)(4)，求 得J最小时的M_max和对应的规划航路，即为J意义下的最优航路。

然而，虚拟力方法存在如下的局限性：

(1)虚拟斥力所定义的威胁种类不够丰富，参数的设置尚缺乏指导；

(2)未对规划空间进行合理划分，规划过程未对航路进行平滑；

(3)该方法缺乏完善的数学描述，一些技术原则难以进行理论上的证明；

(4)该规划是一种最速梯度下降过程，因此存在局部极小和震荡的缺陷。

发明内容

为解决基于虚拟力航路规划方法出现的局部极小问题，并实现规划过程中 实时的自适应规划参数设置，本发明采用基于模糊虚拟力(FVF)的无人机航 路规划方法，其采用贝叶斯网络和模糊逻辑推理相结合进行实时的自适应规划 参数设置，并提出威胁合并法解决虚拟力法的局部极小问题。

本发明的基于模糊虚拟力的无人机航路规划方法包含以下步骤：

1)设置无人机航路规划的初始条件，包括规划起始点、目标点、威胁分布 及属性；

2)设置无人机航路规划的迭代步长；

3)设置规划参数k，从而确定虚拟斥力系数与虚拟引力系数之间的关系， 其中，k＝G_R/G_A，G_A表示虚拟引力系数，G_R表示虚拟斥力系数；

4)进行航路规划，航路规划坐标变化量Δx和Δy为：其 中，F_Ax和F_Ay分别表示虚拟引力在x轴和y轴 的投影，R_A是当前点和目标点间的距离，θ_A是当前点与目标点连线 和x轴的夹角；

F_Rx和F_Ry分别表示虚拟斥力在x轴和y轴的投影， R_R是当前点和威胁间的距离，r₀是常量可设置为威胁的半径，θ_R是当前点与 威胁连线和x轴的夹角；δ为航路规划的迭代步长，且 α＝[(F_Ax+∑F_Rx)²+(F_Ay+∑F_Ry)²]^-1/2。

5)判断是否进入局部极小，若是，则进行威胁合并，若否，则继续进行航 路规划直至目标点。

进一步的，迭代步长应满足δ≤H(1+β)，其中，F_Rx＝β_xF_Ax，F_Ry＝β_yF_Ay且 β＝min(β_x，β_y)；H＝2αG_A/d_max，d_max为航路规划当前点与航路规划目标点之间距 离的最大值。

进一步的，基于模糊逻辑推理，将k的确定规则采用模糊集合描述如下：

如果规划航路所需飞行时间要求TmR低且平台能力PCL弱，则k大；

如果规划航路所需飞行时间要求TmR低且平台能力PCL强，则k中；

如果规划航路所需飞行时间要求TmR中且平台能力PCL中，则k中；

如果规划航路所需飞行时间要求TmR高且平台能力PCL弱，则k中；

如果规划航路所需飞行时间要求TmR高且平台能力PCL强，则k小；最后采 用重心法解模糊化得到k的数值。

进一步的，判断是否进入局部极小准则为：如果|F_N|＝0且没有到达目标点， 则规划陷入局部极小，或者如果F_N＝-F_N+1且没有到达目标点，则规划陷入局部 极小；其中F_N为第N步规划时虚拟斥力和虚拟引力的矢量和，F_N+1为规划第N +1步时虚拟斥力和虚拟引力的矢量和。

进一步的，如果进入局部极小，进行威胁合并的步骤为：

根据任意两个威胁所对应的规划参数k₁、k₂模糊推理得到两个威胁间CritiDis为 临界间距；定义为威胁间距，其中TTDis为两个威 胁中心之间的距离，GapDis为两个威胁影响范围间的最短距离，r和R分别为 两个威胁的影响半径；

将ThrtDis进行归一化处理，使之处于[0，1]之间，比较任意两个威胁对应的 ThrtDis和依据重心法解模糊后的CritiDis之间关系，得到合并标志CmbVal为： 合并标志CmbVal为1的两个威胁构成威胁组。

进一步的，对任意两个威胁所对应的规划参数k₁、k₂进行模糊推理的步骤 为：

如果k₁大且k₂大，则CritiDis大；

如果k₁大且k₂小，则CritiDis中；

如果k₁小且k₂大，则CritiDis中；

如果k₁小且k₂小，则CritiDis小。

进一步的，对所有威胁进行分组的步骤为：

首先构造邻接矩阵AdjM，该矩阵为方阵，且维数等于威胁的数量。矩阵中的元 素可表示为AdjM[i][j]＝CmbVal_i，j，其中i，j为威胁编号； 对邻接矩阵AdjM进行搜索，从而得到威胁分组情况；其中，第m个威胁组用 邻接向量AdjV[m]表示，AdjV[m]中第n个元素可表示为 其中，∪(·)表示逻辑“或”运算，p为威胁 数量；

按照公式循环运算，直至AdjV[m]不变。 AdjV[m][n]中数值为1的元素对应的威胁构成第m组。威胁组数即为合并结束 后新威胁的数量。

附图说明

附图1为本发明基于模糊虚拟力的无人机航路规划方法。

附图2为无人机PCL的贝叶斯网络模型。

附图3为局部极小的描述。

附图4为威胁距离的定义。

具体实施方式

模糊虚拟力法包含了以下三个组成部分：采用定步长降低求解的计算量， 采用贝叶斯网络和模糊逻辑推理的方法设置规划参数，提出威胁合并的方法解 决局部极小问题。

1、定步长法

基于虚拟力的最优求解航路规划方法属于变步长寻优，计算量较大，不利 于工程应用，因此采用固定步长法，即规划航路沿虚拟合力的映射方向，以固 定的步长迭代生成。

$\left(\begin{array}{c}{F}_{\mathrm{Ax}}=G_A·\cos \left({\theta }_A\right)/{R_A}^2\\ F_{\mathrm{Ay}}=G_A·\sin \left({\theta }_A\right)/{R_A}^2\end{array}\right)$

F_Ax和F_Ay分别表示虚拟引力在x轴和y轴的投影，G_A表示引力常数，R_A是 当前点和目标点间的距离，θ_A是当前点与目标点连线和x轴的夹角，

$\left(\begin{array}{c}{F}_{\mathrm{Rx}}=-G_R·\cos \left({\theta }_R\right)·e^{(-R_R/r_0)}\\ F_{\mathrm{Ry}}=-G_R·\sin \left({\theta }_R\right)·e^{(-R_R/r_0)}\end{array}\right)$

F_Rx和F_Ry分别表示虚拟斥力在x轴和y轴的投影，G_R表示斥力常数，R_R是 当前点和威胁间的距离，r₀是常量可设置为威胁的半径，θ_R是当前点与威胁 连线和x轴的夹角，定义规划参数k为

k＝G_R/G_A

则航路规划坐标变化量Δx和Δy为：

$\left(\begin{array}{c}\mathrm{\Delta x}=\delta ·\alpha ·(F_{\mathrm{Ax}}+\Sigma F_{\mathrm{Rx}})\\ \mathrm{\Delta y}=\delta ·\alpha ·(F_{\mathrm{Ay}}+\Sigma F_{\mathrm{Ry}})\end{array}\right)---\left(1\right)$

其中δ为规划步长，且α＝[(F_Ax+∑F_Rx)²+(F_Ay+∑F_Ry)²]^-1/2。航路规划过程中， 航路点坐标按照式(1)迭代进行。

要达到给定目标点，规划步长δ必须满足定理1。

定理1(航路规划的可达性条件)：

在基于虚拟力的航路规划中，记F_Rx＝β_xF_Ax，F_Ry＝β_yF_Ay且β＝min(β_x，β_y)， 如果所选步长δ满足如下不等式：

δ≤H(1+β)

那么，经过有限步数，必满足条件d≤Δd，Δd≥δ。其中，H和Δd是所选择的 判断阈值，认为d≤Δd时到达目标。

证明：假设目标点为G(x_g，y_g)，第N(N＞0)步规划后的规划点P_N(x_N，y_N)作为当 前位置点。通过坐标原点的适当选取可保证x_g-x_N≥0，y_g-y_N≥0，则当前点与 目标点的距离满足

d_N²＝(x_g-x_N)²+(y_g-y_N)²(2)

根据已知条件，当前位置还应满足：

|F_A|＝G_A/R_A²(3)

$\left|F_{\mathrm{Ax}}\right|=\frac{x_g-x_N}{d_N}\left|F_A\right|,\left|F_{\mathrm{Ay}}\right|=\frac{y_g-y_N}{d_N}\left|F_A\right|---\left(4\right)$

若航路规划以步长δ沿虚拟力方向推进，则第N+1步后的位置点P_N+1(x，y) 满足：

x＝x_N+δ·α·(F_Ax+F_Rx)

y＝y_N+δ·α·(F_Ay+F_Ry)(5)

则P_N+1与目标点之间的距离为

d_N+1²＝(x_g-x)²+(y_g-y)²(6)

将式(4)、(5)代入式(2)、(6)，整理并推导可得：

${d_{N+1}}^2-{d_N}^2\le {\delta }^2-2\mathrm{\delta \alpha }\frac{\left|F_A\right|}{d_N}(1+\beta ).---\left(7\right)$

$[{(x_g-x_N)}^2+{(y_g-y_N)}^2]$

代入式(2)，可知：

d_N+1²-d_N²≤δ²-2δα|F_A|d_N(1+β)(8)

因此，若满足条件

δ＜2α|F_A|d_N(1+β)(9)

则有d_N+1²-d_N²＜0，可保证随规划步数增加，与目标点距离不断收敛。将式(3) 代入式(9)，可得

δ＜2αG_A(1+β)/d_N(10)

记H＝2αG_A/d_max，则H＞0。对航路邻域各点，若δ≤H(1+β)，必满足定理 中的可达条件。

式(10)在β＞-1时有解，这说明，如果途经点的总斥力在各方向上的分量小 于引力的对应分量，则目标点必可达。

步长选取要合适，如果过小则相应的计算量较大，反之则规划结果不合理 甚至出现无法到达目标点等问题。

2、基于贝叶斯网络和模糊逻辑推理的自适应参数设置

由虚拟力法的规划原理，规划参数k决定了规划航路与给定威胁间的关系， 规划参数k越大，则规划得到的无人机航路将远离所有威胁，选取较远的安全 航路；反之，将得到距离近的航路，弱化威胁代价。

以往研究中，k一般是根据经验人为指定，然而战场环境、任务需求和无 人机状态信息等实时变化，在线的评估与推理确保航路规划能够动态地反映态 势变化，使得规划更加智能和精确。

这里选定如下因素作为推理要素：

推理要素1：平台能力PCL

推理要素2：时间要求TmR

定义PCL是用来感知和预测无人机的健康状况和能力，PCL可通过贝叶斯 网络评估得到。

2.1贝叶斯网络

贝叶斯网络模型中，节点Z有q个子节点Y_l，...，Y_q和一个父节点U。 给出如下定义：

Bel：节点Z的信度值，即后验概率分布；

λ：来自子节点的诊断概率，即结果事件的出现对待诊断原因的影响；

π：因果概率，反映了来自父节点以及兄弟节点的因果影响。

M_Z|U＝P(Z|U)是在给定父节点U前提下，子节点Z的条件概率。 网络受新的事件信息或先验知识触发，按照如下三个步骤进行更新： 第一步，根据新获取的信息更新本节点的信度：

Bel(z)＝σλ(z)π(z)

$\lambda \left(z\right)=\underset{i}{\Pi }{\lambda }_{Y_i}\left(z\right)$

π(u)＝π_Z(u)×M_Z|U；

第二步，自底向上的传播：λ_Z(u)＝λ(z)×M_Z|U；

第三步，自顶向下的更新：

其中π_Z(u)是从节点U到Z的因果预测概率，是从子节点Y_i到Z的事件诊 断概率。归一化算子σ保证

PCL可通过平台状态、武器状态、燃油信息及故障信息等，利用上述贝叶 斯网络计算得到，对应的具体模型见附图2。

2.2模糊逻辑推理

规划参数k可通过模糊推理得到，推理规则的一般形式见表1。

表1求解规划参数k的模糊规则

推理过程采用Mamdani方法，解模糊采用重心法。

解模糊后，其中K表示模糊集合，μ_K(·)是K中k_i的隶属度函 数。

3、采用威胁合并法，解决局部极小问题

虚拟力法的局部极小问题本质上是由规划空间存在虚拟势场的凹分布引起。 虚拟力可由虚拟势场的梯度W求得：

上式可知，x点的虚拟力指向局部极小区域的中心。航路规划一旦进入该 区域则无法继续进行。

局部极小问题可描述成如下形式：给定集合A和B，如果对A中任意元素 x₁满足f(x₁)∈B，且对B中任意元素x₂满足f(x₂)∈A，则A和B的并集构成局 部极小区域，f(·)表示规划过程。

为了判断航路规划过程是否陷入局部极小，给出如下两个准则：

准则1.如果|F_N|＝0且没有到达目标点，则规划陷入局部极小。

准则2.如果F_N＝-F_N+1且没有到达目标点，则规划陷入局部极小。

为了解决局部极小问题，提出了一种威胁合并的新方法。将产生局部极小 的威胁进行合并。由于如果将虚拟势场变为凸分布，原局部极小区域的势能比 周围更高，规划不会中止。因此，威胁合并法的原理是将势能的凹分布改变为 凸分布。

对任意两个威胁间距离ThrtDis进行定义：

$\mathrm{ThrtDis}=\frac{\mathrm{GapDis}}{\mathrm{TTDis}}=\frac{\mathrm{TTDis}-r-R}{\mathrm{TTDis}}$

其中TTDis为两个威胁中心间距离，GapDis为两个威胁影响范围间的最短距离， r和R分别为两个威胁的影响半径。将ThrtDis进行归一化处理，使之处于[0，1] 之间。

重新定义模糊集合K，将K中和K小合并成K小，K大不变。依照表2规 则，根据任意两个威胁所对应的规划参数k₁、k₂推理得到两个威胁间的临界距 离CritiDis。

表2临界距离的推理规则

比较任意两个威胁对应的ThrtDis和依据重心法解模糊后的CritiDis之间关系， 得到合并标志CmbVal：

$\mathrm{CmbVal}=\left(\begin{array}{cc}1& \mathrm{ThrtDis}\le \mathrm{CritiDis}\\ 0& \mathrm{ThrtDis}>\mathrm{CritiDis}\end{array}\right)$

合并标志CmbVal为1的两个威胁构成威胁组，威胁的分组过程可采用如下 邻接矩阵法进行搜索。

首先构造邻接矩阵AdjM，矩阵为方阵，且维数等于威胁的数量。矩阵中的 元素可表示为AdjM[i][j]＝CmbVal_i，j，其中i，j为威胁编号，显然，该矩阵为对称 阵。

对邻接矩阵AdjM进行搜索，从而得到威胁分组情况。其中，第m个威胁 组用邻接向量AdjV[m]表示，AdjV[m]中第n个元素可表示为

$\mathrm{AdjV}\left[m\right]\left[n\right]=\underset{i=0}{\overset{p-1}{\cup }}\left(\mathrm{AdjM}\right[m\left]\right[i]\times \mathrm{AdjM}[i\left]\right[n\left]\right)---\left(11\right)$

其中，∪(·)表示逻辑“或”运算，p为威胁数量。按照式(11)循环运算，直至 AdjV[m]不变。AdjV[m][n]中数值为1的元素对应的威胁构成第m组。威胁组数 即为合并结束后新威胁的数量。

航路规划算法按照新的威胁信息进行重新规划则可消除局部极小。

以上所述仅为本发明的较佳实施方式，凡依本发明权利要求所做的均等变 化与修饰，皆应属本发明的涵盖范围。