基于布谷鸟算法改进人工势场法的四旋翼编队避障方法与流程

1.本发明属于四旋翼技术领域，特别是一种基于布谷鸟算法改进人工势场法的四旋翼编队避障方法。


背景技术：

2.随着现代技术的飞速发展，传感器、微电子以及通信等关键技术不断革新，无人机因其机动性好、控制结构简单的优点在各行各业取得了极为广泛的应用。
3.随着任务复杂度的提升，四旋翼飞行器编队的必要性愈发显著。由于室内环境下缺少gps信号，高精度的室内定位系统开发技术难度高。目前已有研究在四旋翼编队飞行中引入了运动捕捉系统vicon，开发出四旋翼无人机室内编队控制系统。
4.人工势场法计算简单、实时性高，并且规划出来的路径一般是平滑且安全的，常被应用于机器人路径规划的避障中。但其本身存在缺点，包括目标不可达问题、振荡问题、局部极值问题。人工势场法是一种利用虚拟力在已知环境中进行路径规划的算法，因其运算速度快和高效简单而广泛应用。该算法将环境中障碍物与禁止进入的区域设为斥力点；将终点与可以进入的区域设为引力点。因此人工势场法存在以下问题：其一，当无人机受到来自斥力和引力的合力为0时，即斥力引力大小相同、方向相反时，无人机会停止运动，此时陷入了局部极小点。人工势场法很容易陷入这种局部极小点，在实际过程中，当障碍物、目标点和无人机处于同一直线时，无人机接近障碍物的过程中由于斥力不断增加、引力不断减小，因此很可能会出现斥力与引力大小相同、方向相反的情况，此时人工势场法将不再有效。其二，理论上当无人机到达目标点附近时引力与斥力都会很小，此时若忽略障碍物的斥力则无人机在到达目标点时引力刚好为0。然而，在实际应用中，目标点附近一般总是存在障碍物，当无人机接近目标点时，引力减小相比于斥力可忽略不计，此时无人机会向斥力方向运动，从而在目标点附近不断振荡循环而无法到达目标点。
5.有的研究提出建立在改进人工势场模型上的基于遗传算法的最优路径搜索方法。通过人工势场法生成路径，再通过遗传算法评价各条路径的优劣程度，进而搜索出最优路径。从而解决人工势场法中由于局部最优解产生的死锁问题。但是遗传算法的局部搜索能力较差，耗时较长。
6.还有的研究采用改进蚁群和人工势场法相结合的混合路径规划。通过粒子群参数优化的改进蚁群算法求解全局路径规划，但是收敛速度较慢、优化效率低；在求解局部路径规划时，通过引入目标距离相关函数改进的斥力函数，解决不可达性问题。
7.布谷鸟搜索算法是一种新兴启发式群智能算法，因其给定参数较少、算法易实现以及全局寻优能力强的优点而广泛使用。然而，布谷鸟算法应用于人工势场算法中因其本身存在的收敛精度不高、局部搜索结果不优的缺陷而出现不理想的情况。在人工势场算法中使用布谷鸟算法将放大其局部搜索能力弱的缺点，这样规划出的路径虽然能较好解决人工势场法的缺陷，但存在波折多、路径长、节点多等缺点。


技术实现要素：

8.本发明的目的在于针对上述现有技术存在的问题，提供一种基于布谷鸟算法改进人工势场法的四旋翼编队避障方法。
9.实现本发明目的的技术解决方案为：一种基于布谷鸟算法改进人工势场法的四旋翼编队避障方法，所述方法包括以下步骤：
10.步骤1，确定主机四旋翼的当前位置、目标位置、环境中障碍物的位置及大小，建立环境模型，并初始化人工势场法的参数；
11.步骤2，利用斥力函数和引力函数分别计算障碍物对主机四旋翼的斥力以及目标点对主机四旋翼的引力，同时计算斥力和引力的角度；
12.步骤3，基于步骤2的结果，计算斥力和引力的合力，并计算与主机四旋翼距离最近的障碍物的距离；
13.步骤4，判断步骤3中的合力是否为0，若否，则跳转至步骤5，否则判断是否到达目标点，若到达目标点则结束，否则跳转至步骤6；
14.步骤5，判断步骤3中所述的与主机四旋翼距离最近的障碍物的距离是否小于预设阈值，若是，则跳转至步骤6，否则跳转至步骤7；
15.步骤6，采用布谷鸟算法计算主机四旋翼下一步位置的最优解，之后利用差分进化算法进行循环迭代，直至摆脱局部极小值点且主机四旋翼与距离最近的障碍物的距离大于预设阈值，结束迭代，更新合力方向与步长；
16.步骤7，在合力作用下，主机四旋翼按照预设步长运动到下一步的位置；
17.步骤8，判断所述下一步的位置是否满足转角约束条件，若不满足，跳转至步骤6，重新规划下一步的位置，否则跳转至步骤9；
18.步骤9，判断是否到达目标点，若到达目标点则跳转至步骤10，否则跳转至步骤2；
19.步骤10，由上述步骤生成主机的规划飞行轨迹，之后基于飞行编队确定从机的飞行轨迹。
20.进一步地，步骤2中所述斥力函数为：
[0021][0022]
式中，m为斥力系数，ρ(q)为四旋翼q到障碍物的距离，ρ
g
为四旋翼q到目标点的距离，ρ0为斥力作用范围，n为任意正实数。
[0023]
进一步地，步骤1还包括：在环境模型中，采用障碍物膨胀法在障碍物边上设置安全裕量。
[0024]
一种基于布谷鸟算法改进人工势场法的四旋翼编队避障系统，所述系统包括依次执行的：
[0025]
初始化模块，用于确定主机四旋翼的当前位置、目标位置、环境中障碍物的位置及大小，建立环境模型，并初始化人工势场法的参数；
[0026]
第一计算模块，用于利用斥力函数和引力函数分别计算障碍物对主机四旋翼的斥力以及目标点对主机四旋翼的引力，同时计算斥力和引力的角度；
[0027]
第二计算模块，用于基于第一计算模块的结果，计算斥力和引力的合力，并计算与
主机四旋翼距离最近的障碍物的距离；
[0028]
第一判断模块，用于判断第二计算模块中的合力是否为0，若否，则跳转至第二判断模块，否则判断是否到达目标点，若到达目标点则结束，否则跳转至求解模块；
[0029]
第二判断模块，用于判断所述与主机四旋翼距离最近的障碍物的距离是否小于预设阈值，若是，则跳转至求解模块，否则跳转至运动模块；
[0030]
求解模块，用于采用布谷鸟算法计算主机四旋翼下一步位置的最优解，之后利用差分进化算法进行循环迭代，直至摆脱局部极小值点或者主机四旋翼与距离最近的障碍物的距离大于预设阈值，结束迭代，更新合力方向与步长；
[0031]
运动模块，用于在合力作用下，使主机四旋翼按照预设步长运动到下一步的位置；
[0032]
第三判断模块，用于判断所述下一步的位置是否满足转角约束条件，若不满足，跳转至求解模块，重新规划下一步的位置，否则跳转至第四判断模块；
[0033]
第四判断模块，判断是否到达目标点，若到达目标点则跳转至轨迹生成模块，否则跳转至第一计算模块；
[0034]
轨迹生成模块，用于基于上述模块的结果生成主机的规划飞行轨迹，之后基于飞行编队确定从机的飞行轨迹。
[0035]
本发明与现有技术相比，其显著优点为：1)当飞行器陷入局部极小点和振荡点时，利用改进后的布谷鸟算法寻优规划跳出极小点与震荡点，同时差分进化算法解决了布谷鸟算法局部搜索差、缺乏灵活性的缺点，使得规划路径更优、节点更少、迭代次数更少；2)通过障碍物膨胀法，在障碍物边上预留一定的安全余裕，以保证四旋翼飞行时可以避开障碍物，同时也不需要考虑四旋翼的体积的影响，简化计算过程。
[0036]
下面结合附图对本发明作进一步详细描述。
附图说明
[0037]
图1为四旋翼在环境中受力的分解图。
[0038]
图2为本发明基于布谷鸟算法改进人工势场法的四旋翼编队避障方法的流程图。
[0039]
图3为本发明中布谷鸟算法的流程图。
[0040]
图4为一个实施例中本发明规划的路径规划示意图。
[0041]
图5为一个实施例中本发明提出的算法路径优化前后效果对比图，其中图(a)、(b)分别为差分进化算法优化前、后的路径。
[0042]
图6为一个实施例中本发明算法规划的编队避障结果示意图。
具体实施方式
[0043]
为了使本技术的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本技术进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本技术，并不用于限定本技术。
[0044]
由图1所示，四旋翼在环境中的运动视为虚拟力场作用下的运动，即目标点对四旋翼产生引力，引导四旋翼向目标点运动，障碍物对四旋翼产生斥力，防止四旋翼与障碍物发生碰撞。引力和斥力共同作用，形成一个合力，控制四旋翼运动。四旋翼编队避障过程中，领航者通过人工势场算法生成预定轨迹，跟随着通过跟随领航者运动。
[0045]
在一个实施例中，结合图2，提供了一种基于布谷鸟算法改进人工势场法的四旋翼编队避障方法，所述方法包括以下步骤：
[0046]
步骤1，确定主机四旋翼的当前位置、目标位置、环境中障碍物的位置及大小，建立环境模型，并初始化人工势场法的参数；
[0047]
步骤2，利用斥力函数和引力函数分别计算障碍物对主机四旋翼的斥力以及目标点对主机四旋翼的引力，同时计算斥力和引力的角度；
[0048]
步骤3，基于步骤2的结果，计算斥力和引力的合力，并计算与主机四旋翼距离最近的障碍物的距离；
[0049]
步骤4，判断步骤3中的合力是否为0，若否，则跳转至步骤5，否则判断是否到达目标点，若到达目标点则结束，否则跳转至步骤6；
[0050]
步骤5，判断步骤3中所述的与主机四旋翼距离最近的障碍物的距离是否小于预设阈值，若是，则跳转至步骤6，否则跳转至步骤7；
[0051]
步骤6，采用布谷鸟算法计算主机四旋翼下一步位置的最优解，之后利用差分进化算法进行循环迭代，直至摆脱局部极小值点且主机四旋翼与距离最近的障碍物的距离大于预设阈值，结束迭代，更新合力方向与步长；
[0052]
步骤7，在合力作用下，主机四旋翼按照预设步长运动到下一步的位置；
[0053]
步骤8，判断所述下一步的位置是否满足转角约束条件，若不满足，跳转至步骤6，重新规划下一步的位置，否则跳转至步骤9；
[0054]
步骤9，判断是否到达目标点，若到达目标点则跳转至步骤10，否则跳转至步骤2；
[0055]
步骤10，由上述步骤生成主机的规划飞行轨迹，之后基于飞行编队确定从机的飞行轨迹。
[0056]
进一步地，在其中一个实施例中，步骤2中所述斥力函数为：
[0057][0058]
式中，m为斥力系数，ρ(q)为四旋翼q到障碍物的距离，ρ
g
为四旋翼q到目标点的距离，ρ0为斥力作用范围，n为任意正实数。
[0059]
所述引力函数为：
[0060][0061]
其中，k为引力系数，m为斥力系数，ρ(q)为四旋翼q到障碍物的距离，ρ
g
为四旋翼q到目标点的距离，ρ0为斥力作用范围，n为任意正实数。
[0062]
因此，引力为：
[0063]
f
att
＝
‑
grad(u
att
(q))＝kρ
g
(q)
[0064]
斥力为：
[0065]
[0066][0067][0068]
其中，f
rep1
(q)和f
rep2
(q)为f
rep
(q)的两个分力，如图1所示。改进后的斥力函数中引入了四旋翼到目标点的距离来优化四旋翼所受到的斥力函数，因此可将改进斥力分解成f
rep1
(q)，即障碍物对四旋翼产生的原斥力，以及f
rep2
(q)，即改进的斥力函数中产生的额外的斥力，方向由四旋翼指向目标点。
[0069]
进一步地，在其中一个实施例中，结合图3，步骤6中所述布谷鸟算法具体包括以下步骤：
[0070]
步骤6
‑1‑
1，初始化n个寄主的鸟巢：在解空间内随机选择n个解，作为寄主的鸟巢，即初始解；
[0071]
步骤6
‑1‑
2，随机取一个布谷鸟，通过莱维飞行产生一个解，该解生成过程中采用可变步长；步长l的计算公式如下：
[0072][0073]
其中，
[0074][0075]
式中，β为莱维飞行的预设参数；
[0076]
步骤6
‑1‑
3，评估解的质量，即计算适应度函数的值f＝
‑
u(q)；
[0077]
步骤6
‑1‑
4，根据适应度函数找到最优解并记录下来；
[0078]
步骤6
‑1‑
5，根据发现概率舍弃一个鸟巢，并建立新鸟巢；
[0079]
步骤6
‑1‑
6，列出当前最佳的鸟巢，跳转步骤6
‑1‑
2，继续迭代，直到达到迭代上限；
[0080]
步骤6
‑1‑
7，输出最佳鸟巢中的解即主机四旋翼下一步位置；
[0081]
步骤6
‑1‑
8，判断是否摆脱局部极小值点且主机四旋翼与距离最近的障碍物的距离是否大于预设阈值，若满足，则结束，否则跳转步骤6
‑1‑
1。
[0082]
进一步地，在其中一个实施例中，步骤6中所述利用差分进化算法进行循环迭代具体包括以下步骤：
[0083]
步骤6
‑2‑
1，初始化差分进化算法相关变量(边界范围、迭化代数g、变异因子f0和交叉算子cr)以及种群大小和染色体长度(分别代表布谷鸟算法得到鸟巢个数与维度)，与布谷鸟算法所得的最佳鸟巢匹配；
[0084]
步骤6
‑2‑
2，计算自适应变异因子，按如下公式：
[0085][0086]
式中，gen为当前进化代数，g为进化代数，f0为初始化变异因子；
[0087]
步骤6
‑2‑
3，按如下公式：
[0088]
v
i
＝x
r1
f
·
(x
r2
‑
x
r3
)
[0089]
针对每一代鸟巢位置x
i
从所有鸟巢中随机选取第r1、r2、r3个鸟巢进行变异处理，得到变异后的新鸟巢位置v
i
；其中，f为变异算子；
[0090]
步骤6
‑2‑
4，对每一代鸟巢位置x
i
以及它变异出的新鸟巢位置v
i
按如下公式：
[0091][0092]
进行交叉操作，得到交叉后的新鸟巢位置u
i
；其中，cr为交叉算子；
[0093]
步骤6
‑2‑
5，根据初始化的边界范围，对变异、交叉后的新鸟巢位置进行限幅处理，即按照以下公式：
[0094][0095]
得到差分进化算法后的鸟巢位置u
i
，其中，i为步长；
[0096]
步骤6
‑2‑
6，分别计算布谷鸟算法得到的鸟巢位置x
i
的适应值ob_x
i
与差分进化算法后得到的鸟巢位置u
i
适应值ob_u
i
，进行比较后选取适应值大的鸟巢位置组成最终鸟巢nest。
[0097]
进一步地，在其中一个实施例中，步骤7中运动至下一步的位置，所用公式为：
[0098]
x
i 1
＝x
i
l
·
cosθ
[0099]
y
i 1
＝y
i
l
·
cosθ
[0100]
式中，(x
i
,y
i
)为主机四旋翼当前位置，(x
i 1
,y
i 1
)为主机四旋翼下一步位置，θ为合力的角度。
[0101]
进一步地，在其中一个实施例中，步骤8中所述转角约束条件为：
[0102][0103]
式中，(x
i
,y
i
)为主机四旋翼当前位置，(x
i 1
,y
i 1
)为主机四旋翼下一步位置，(x
i
‑1,y
i
‑1)为四旋翼上一步位置，θ
set
为预设的最大转角。
[0104]
进一步地，在其中一个实施例中，步骤10中所述飞行编队的确定方式为：若主机与障碍物的距离小于预设阈值，则采用直线编队飞行，否则采用三角编队飞行。
[0105]
进一步地，在其中一个实施例中，步骤1还包括：在环境模型中，采用障碍物膨胀法在障碍物边上设置安全裕量。
[0106]
这里，由于采用了障碍物膨胀法设置了安全裕量，因此更可以直接产生跟随者的轨迹而不会发生碰撞。
[0107]
示例性地，根据本发明改进的人工势场算法规划得到的路径如图4所示，从图4中可以看出，该算法规划的路径可以有效避免传统人工势场算法易于陷入局部极小点的问题，并且在狭窄环境下也可以减少振荡现象的发生。如图5所示为差分进化规划的路径的局部示意图，由图可以看出，引入差分进化算法优化后的路径相较于原路径更短，规划效果更佳。针对三架四旋翼飞行器，运用本发明改进的人工势场算法设计的四旋翼编队避障方法得到的结果如图6所示，三架四旋翼飞行器呈三角形队形飞行，当与障碍物的距离小于预设阈值时，进行队形变化，四旋翼呈直线队形飞行，当与障碍物的距离大于预设阈值时，恢复原来的队形。
[0108]
在一个实施例中，提供了一种基于布谷鸟算法改进人工势场法的四旋翼编队避障系统，所述系统包括依次执行的：
[0109]
初始化模块，用于确定主机四旋翼的当前位置、目标位置、环境中障碍物的位置及大小，建立环境模型，并初始化人工势场法的参数；
[0110]
第一计算模块，用于利用斥力函数和引力函数分别计算障碍物对主机四旋翼的斥力以及目标点对主机四旋翼的引力，同时计算斥力和引力的角度；
[0111]
第二计算模块，用于基于第一计算模块的结果，计算斥力和引力的合力，并计算与主机四旋翼距离最近的障碍物的距离；
[0112]
第一判断模块，用于判断第二计算模块中的合力是否为0，若否，则跳转至第二判断模块，否则判断是否到达目标点，若到达目标点则结束，否则跳转至求解模块；
[0113]
第二判断模块，用于判断所述与主机四旋翼距离最近的障碍物的距离是否小于预设阈值，若是，则跳转至求解模块，否则跳转至运动模块；
[0114]
求解模块，用于采用布谷鸟算法计算主机四旋翼下一步位置的最优解，之后利用差分进化算法进行循环迭代，直至摆脱局部极小值点或者主机四旋翼与距离最近的障碍物的距离大于预设阈值，结束迭代，更新合力方向与步长；
[0115]
运动模块，用于在合力作用下，使主机四旋翼按照预设步长运动到下一步的位置；
[0116]
第三判断模块，用于判断所述下一步的位置是否满足转角约束条件，若不满足，跳转至求解模块，重新规划下一步的位置，否则跳转至第四判断模块；
[0117]
第四判断模块，判断是否到达目标点，若到达目标点则跳转至轨迹生成模块，否则跳转至第一计算模块；
[0118]
轨迹生成模块，用于基于上述模块的结果生成主机的规划飞行轨迹，之后基于飞行编队确定从机的飞行轨迹。
[0119]
关于基于布谷鸟算法改进人工势场法的四旋翼编队避障系统的具体限定可以参见上文中对于基于布谷鸟算法改进人工势场法的四旋翼编队避障方法的限定，在此不再赘述。上述基于布谷鸟算法改进人工势场法的四旋翼编队避障系统中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。
[0120]
综上，本发明在传统人工势场法的基础上改进斥力函数，在斥力函数中引入目标点到障碍物的距离，使得当终点附近存在障碍物时四旋翼也可到达终点，从而解决终点不可达性问题。通过障碍物膨胀法，围绕障碍物划分一定的安全裕量，避免四旋翼发生碰撞，
确保四旋翼安全避开障碍物。当机器人陷入局部极小值时，或者处于狭窄环境中时，可以通过布谷鸟算法规划路径，本发明对布谷鸟算法进行了改进，通过差分进化的可变步长的布谷鸟算法，提高了人工势场算法路径规划的适应性，解决了布谷鸟算法存在的问题，使得规划路径更优、节点更少、迭代次数更少。
[0121]
以上显示和描述了本发明的基本原理、主要特征及优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。