一种基于球面矢量鸽群算法的三维避障航路规划方法

1.本发明涉及飞行器任务规划技术领域，尤其是一种基于球面矢量鸽群算法的三维避障航路规划方法。


背景技术：

2.无人机具有性价比高、灵活性强、隐蔽性佳等优势，近年来在军事及民用领域得到了广泛的应用，受到世界各国的认可。随着航空技术、电子信息技术及相关制造工艺的不断发展，世界各国都加大了对无人机领域的关注和投入。完善的任务规划系统是无人机顺利完成任务的重要保障，而航路规划则是其中的核心部分，是提高无人机安全性能的重要手段。无人机航路规划就是在特定的任务背景下，为无人机寻找一条从起始位置到达目标位置的最优或可行飞行航路，同时满足无人机自身的物理约束条件，并能够避开障碍和威胁。无人机进行航路规划时所采用的算法，将直接影响到航路规划的效果。
3.传统的航路规划方法如数学规划方法、人工势场法等在面对三维空间时存在收敛速度慢的缺点，且计算量过于庞大，有时还会找不到最优航路。目前伴随无人机越来越广泛的应用，其任务环境愈加复杂，这对算法的计算量及时效性带来了不小的挑战。而通过模仿自然界中生物的行为规律，来探索分布在一定范围内解空间中最优解的群智能优化算法，可对多维搜索空间进行求解，且具备较强的搜索能力和鲁棒性，在航路规划领域具有显著优势，受到了众多研究者的喜爱。
4.鸽群优化算法(pio)就是其中的一种新型群智能优化算法，它通过模仿鸽子的归巢行为来实现最优解的搜索，算法结构简单、参数少，已经被用于多种不同领域。但是原始的鸽群优化算法与许多其他群体智能优化算法一样，当它在全局最优解附近展开搜索时，会产生种群多样性不足，容易陷入局部最优的问题。


技术实现要素：

5.本发明所要解决的技术问题在于，提供一种基于球面矢量鸽群算法的三维避障航路规划方法，能够提高初始解的质量，增强鸽群算法的全局搜索能力，实现无人机的最优航路搜索。
6.为解决上述技术问题，本发明提供一种基于球面矢量鸽群算法的三维避障航路规划方法，包括如下步骤：
7.步骤1、建立无人机航路规划三维环境；
8.步骤2、根据无人机及其飞行航路的各种需求和约束条件，确定无人机航路规划任务的目标函数；
9.步骤3、利用基于球面矢量与tent混沌策略的改进鸽群算法在航路规划环境中对无人机航路代价函数进行优化，最终得到无人机的最优航路。
10.优选的，步骤1中，建立无人机航路规划三维环境具体为：确定任务地图边界、威胁区域和无人机飞行任务信息，包括起始点坐标s(xs,ys,zs)和目标点坐标e(xe,ye,ze)，航路
点数量n；
11.在规划空间中，无人机的飞行航路用许多航路点表示，基于球面矢量的鸽群算法spio将每条路径编码为一组向量，每个向量描述了无人机从一个航路点到另一个航路点的运动，这些向量在球面坐标系中表示，球坐标系包括三个分量：幅值ρ∈(0,path_length)、仰角ψ∈(-π/2,π/2)和方位角φ∈(-π,π)，一条有n个节点的飞行路径ωi就由一个3n维的超球面向量表示；
12.ωi＝(ρ
i1
,ψ
i1
,φ
i1
,ρ
i2
,ψ
i2
,φ
i2
,
…
,ρ
in
,ψ
in
,φ
in
),n＝n-2
13.通过将鸽子的位置描述为ωi，与该鸽子相关的速度可以用一个增量矢量来描述：
14.δωi＝(δρ
i1
,δψ
i1
,δφ
i1
,δρ
i2
,δψ
i2
,δφ
i2
,
…
,δρ
in
,δψ
in
,δφ
in
)
15.将球向量(ρ
ij
,ψ
ij
,φ
ij
)表示为u
ij
，速度(δρ
ij
,δψ
ij
,δφ
ij
)表示为δu
ij
。
16.优选的，步骤2中，根据无人机及其飞行航路的各种需求和约束条件，确定无人机航路规划任务的目标函数具体为：考虑与飞行路径xi相关的最优性、安全性和可行性约束，无人机在飞行过程中的总代价函数f(xi)定义为如下形式：
[0017][0018]
其中bk为权重系数，f1(xi)是指航路长度代价，f2(xi)是指威胁所产生的代价，f3(xi)是指飞行高度代价，f4(xi)是指平滑度代价，决策变量是飞行路径xi，由无人机需要飞过的n个航路点来表示，每个航路点对应于搜索地图中的一个路径节点，其坐标为p
ij
＝(x
ij
,y
ij
,z
ij
)；
[0019]
对于航路长度所产生的代价f1(xi)定义如下：
[0020][0021]
假设k是所有威胁的集合，每个威胁都被规定在一个圆柱体中，其投影的中心坐标为ck，半径为rk，对于一个给定的航路段相关的威胁成本与它到ck的距离dk成正比，通过考虑无人机的直径d和危险距离s，威胁成本所产生的代价f2(xi)定义如下：
[0022][0023]
在飞行过程中，最小和最大高度分别为h
min
和h
max
，高度代价f3(xi)通过下式计算得到：
[0024][0025]
[0026]
其中，h
ij
为相对地面的飞行高度，h
ij
为与航路点p
ij
相关的高度成本的计算式，对所有航路点的h
ij
进行求和就可以得到高度代价f3(xi)；
[0027]
平滑度代价f4(xi)对转弯角和爬升角进行评估；
[0028]
转弯角是两个连续航路段和在水平面oxy上的投影之间的夹角，转弯角由下式计算得到：
[0029][0030]
爬升角ψ
ij
是航路段与其在水平面上的投影之间的夹角，爬升角由下式计算得到：
[0031][0032]
平滑度代价f4(xi)可通过下式计算得到：
[0033][0034]
其中，a1和a2分别为转弯角和爬升角的惩罚系数。
[0035]
优选的，步骤3中，利用基于球面矢量与tent混沌策略的改进鸽群算法在航路规划环境中对无人机航路代价函数进行优化，最终得到无人机的最优航路具体包括如下步骤：
[0036]
步骤3.1：初始化算法中种群数量、地图与罗盘算子操作阶段迭代次数和地标算子操作阶段迭代次数、地图罗盘因子以及各代价权重，并依照规划空间确定搜索的上下限值，进行种群初始化操作；
[0037]
步骤3.2：开始地图与罗盘算子迭代阶段，根据总代价函数公式计算当前鸽群里每一只鸽子的适应度值；
[0038]
步骤3.3：更新局部最优解和全局最优解，并判断是否达到地图与罗盘算子迭代次数，若达到迭代次数，则进行下一步骤，否则，跳至上一步进行下一次迭代；
[0039]
步骤3.4：开始地标算子迭代阶段，按照当前鸽群适应度值对所有鸽子进行排序操作，一部分适应度值低的鸽子会跟随适应度值高的鸽子，接着找到所有鸽子的中心，所有鸽子将跟随中心调整自己的飞行方向，最后存储最佳解参数和最佳成本值；
[0040]
步骤3.5：判断是否到达地标算子迭代次数，若达到，则停止地标算子操作，输出结果，否则，执行上一步。
[0041]
优选的，步骤3.1中，对鸽子种群进行初始化操作时，利用tent混沌映射初始化种群，所采用的tent混沌映射的公式为：
[0042][0043]
即
[0044]
其中，n
t
为混沌序列内的粒子个数；rand(0,1)为[0,1]之内的随机数。
[0045]
优选的，步骤3.2中，在地图与罗盘算子阶段，每次迭代都按照如下式子对鸽子的速度和位置进行更新：
[0046][0047][0048]
其中，r为地图罗盘算子，rand是一个随机数，qg为当前种群的全局最优位置。
[0049]
优选的，步骤3.3中，更新局部最优解和全局最优解，并判断是否达到地图与罗盘算子迭代次数，若达到迭代次数，则进行下一步骤，否则，跳至上一步进行下一次迭代；
[0050]
qi＝(q
i1
,q
i2
,
…
,q
i,n
)和qg＝(q
g1
,q
g2
,
…
,q
g,n
)分别为表示鸽子i局部最佳解和全局最佳解的向量集，为了确定qi和qg，需要将基于矢量的飞行路径ωi映射到直接路径xi，以便评估相关成本，矢量u
ij
＝(ρ
ij
,ψ
ij
,φ
ij
)∈ωi到航点p
ij
＝(x
ij
,y
ij
,z
ij
)∈xi的映射可进行如下：
[0051]
x
ij
＝x
i,j-1
ρ
ij
sinψ
ij
cosφ
ij
[0052]yij
＝y
i,j-1
ρ
ij
sinψ
ij
sinφ
ij
[0053]zij
＝z
i,j-1
ρ
ij
cosψ
ij
[0054]
将映射表示为ξ：ω
→
x，局部和全局最佳解可以通过下式计算得到：
[0055][0056]
优选的，步骤3.4中，在地标算子阶段，鸽子的位置按照如下式子进行更新：
[0057][0058][0059]
其中，fitness为适应度函数。
[0060]
本发明的有益效果为：针对三维空间下的无人机航路规划问题，提出了一种新的基于球面矢量的鸽群优化算法，通过矢量的幅度、仰角和方位角分量与无人机的速度、转角和爬升角之间的相互关系可以实现无人机飞行的安全增强，算法在配置空间中搜索解决方案，可以提高搜索到高质量解的更高概率；更重要的是，航路规划过程中，与转弯角和爬升角有关的约束可以通过球面矢量的仰角和方位角直接实现，这样可以大大减少搜索空间；
在某些情况下，比如当无人机以恒定速度飞行时，可以将球面矢量的幅度固定，以进一步减少搜索空间，扩大算法在配置空间中的搜索能力；在鸽群算法种群初始化阶段引入tent混沌策略获得混沌序列，提高初始解的质量，增强鸽群算法的全局搜索能力，实现无人机的最优航路搜索。
附图说明
[0061]
图1为本发明的威胁成本示意图。
[0062]
图2为本发明的高度成本示意图。
[0063]
图3为本发明的方法流程示意图。
[0064]
图4(a)为本发明实施例中基于球面矢量鸽群算法所得的无人机航路的三维视图。
[0065]
图4(b)为本发明实施例中基于原始鸽群算法所得的无人机航路的三维视图。
[0066]
图5(a)为本发明实施例中基于球面矢量鸽群算法所得的无人机航路的俯视图。
[0067]
图5(b)为本发明实施例中基于原始鸽群算法所得的无人机航路的俯视图。
[0068]
图6(a)为本发明实施例中基于球面矢量鸽群算法的收敛曲线图。
[0069]
图6(b)为本发明实施例中基于原始鸽群算法的收敛曲线图。
具体实施方式
[0070]
如图3所示，一种基于球面矢量鸽群算法的三维避障航路规划方法，包括如下步骤：
[0071]
步骤1：建立无人机航路规划三维环境；
[0072]
确定任务地图边界、威胁区域和无人机飞行任务信息，包括起始点坐标s(xs,ys,zs)和目标点坐标e(xe,ye,ze)，航路点数量n。
[0073]
在规划空间中，无人机的飞行航路可以用许多航路点表示。基于球面矢量的鸽群算法(spio)将每条路径编码为一组向量，每个向量描述了无人机从一个航路点到另一个航路点的运动。这些向量在球面坐标系中表示，球坐标系包括三个分量：幅值ρ∈(0,path_length)、仰角ψ∈(-π/2,π/2)和方位角φ∈(-π,π)。一条有n个节点的飞行路径ωi就由一个3n维的超球面向量表示。
[0074]
ωi＝(ρ
i1
,ψ
i1
,φ
i1
,ρ
i2
,ψ
i2
,φ
i2
,
…
,ρ
in
,ψ
in
,φ
in
),n＝n-2
[0075]
通过将鸽子的位置描述为ωi，与该鸽子相关的速度可以用一个增量矢量来描述：
[0076]
δωi＝(δρ
i1
,δψ
i1
,δφ
i1
,δρ
i2
,δψ
i2
,δφ
i2
,
…
,δρ
in
,δψ
in
,δφ
in
)
[0077]
将球向量(ρ
ij
,ψ
ij
,φ
ij
)表示为u
ij
，速度(δρ
ij
,δψ
ij
,δφ
ij
)表示为δu
ij
。
[0078]
步骤2：根据无人机及其飞行航路的各种需求和约束条件，确定无人机航路规划任务的目标函数；
[0079]
考虑与飞行路径xi相关的最优性、安全性和可行性约束，无人机在飞行过程中的总代价函数f(xi)定义为如下形式：
[0080][0081]
其中bk为权重系数，f1(xi)是指航路长度代价，f2(xi)是指威胁所产生的代价，f3(xi)是指飞行高度代价，f4(xi)是指平滑度代价。决策变量是飞行路径xi，由无人机需要飞
过的n个航路点来表示，每个航路点对应于搜索地图中的一个路径节点，其坐标为p
ij
＝(x
ij
,y
ij
,z
ij
)。
[0082]
对于航路长度所产生的代价f1(xi)定义如下：
[0083][0084]
假设k是所有威胁的集合，每个威胁都被规定在一个圆柱体中，其投影的中心坐标为ck，半径为rk，如图1所示。对于一个给定的航路段相关的威胁成本与它到ck的距离dk成正比。通过考虑无人机的直径d和危险距离s，威胁成本所产生的代价f2(xi)定义如下：
[0085][0086]
在飞行过程中，最小和最大高度分别为h
min
和h
max
，高度代价f3(xi)可通过下式计算得到：
[0087][0088][0089]
其中，h
ij
为相对地面的飞行高度，如图2所示，h
ij
为与航路点p
ij
相关的高度成本的计算式，对所有航路点的h
ij
进行求和就可以得到高度代价f3(xi)。
[0090]
平滑度代价f4(xi)对转弯角和爬升角进行评估。
[0091]
转弯角是两个连续航路段和在水平面oxy上的投影之间的夹角。转弯角可由下式计算得到：
[0092][0093]
爬升角ψ
ij
是航路段与其在水平面上的投影之间的夹角，爬升角可由下式计算得到：
[0094][0095]
平滑度代价f4(xi)可通过下式计算得到：
[0096][0097]
其中，a1和a2分别为转弯角和爬升角的惩罚系数。
[0098]
步骤3：利用基于球面矢量与tent混沌策略的改进鸽群算法在航路规划环境中对无人机航路代价函数进行优化，最终得到无人机的最优航路，具体包括：
[0099]
步骤3.1：初始化算法中种群数量、地图与罗盘算子操作阶段迭代次数和地标算子操作阶段迭代次数、地图罗盘因子以及各代价权重，并依照规划空间确定搜索的上下限值，进行种群初始化操作；
[0100]
混沌映射采用tent混沌映射，其解析式为：
[0101][0102]
即
[0103]
其中，n
t
为混沌序列内的粒子个数；rand(0,1)为[0,1]之内的随机数。
[0104]
步骤3.2：开始地图与罗盘算子迭代阶段，根据总代价函数公式计算当前鸽群里每一只鸽子的适应度值；
[0105]
在地图与罗盘算子阶段，每次迭代都按照如下式子对鸽子的速度和位置进行更新：
[0106][0107][0108]
其中，r为地图罗盘算子，rand是一个随机数，qg为当前种群的全局最优位置。
[0109]
步骤3.3：更新局部最优解和全局最优解，并判断是否达到地图与罗盘算子迭代次数，若达到迭代次数，则进行下一步骤，否则，跳至上一步进行下一次迭代；
[0110]
qi＝(q
i1
,q
i2
,
…
,q
i,n
)和qg＝(q
g1
,q
g2
,
…
,q
g,n
)分别为表示鸽子i局部最佳解和全局最佳解的向量集。为了确定qi和qg，需要将基于矢量的飞行路径ωi映射到直接路径xi，以便评估相关成本。矢量u
ij
＝(ρ
ij
,ψ
ij
,φ
ij
)∈ωi到航点p
ij
＝(x
ij
,y
ij
,z
ij
)∈xi的映射可进行如下：
[0111]
x
ij
＝x
i,j-1
ρ
ij
sinψ
ij
cosφ
ij
[0112]yij
＝y
i,j-1
ρ
ij
sinψ
ij
sinφ
ij
[0113]zij
＝z
i,j-1
ρ
ij
cosψ
ij
[0114]
将映射表示为ξ：ω
→
x，局部和全局最佳解可以通过下式计算得到：
[0115][0116]
[0117]
步骤3.4：开始地标算子迭代阶段，按照当前鸽群适应度值对所有鸽子进行排序操作，一部分适应度值低的鸽子会跟随适应度值高的鸽子，接着找到所有鸽子的中心，所有鸽子将跟随中心调整自己的飞行方向，最后存储最佳解参数和最佳成本值；
[0118]
在地标算子阶段，鸽子的位置按照如下式子进行更新：
[0119][0120][0121]
其中，fitness为适应度函数。
[0122]
步骤3.5：判断是否到达地标算子迭代次数，若达到，则停止地标算子操作，输出结果，否则，执行上一步。
[0123]
为了验证方法的可行性和有效性，下面结合仿真实验做进一步的说明。
[0124]
规划空间如图4(a)和图4(b)所示，包含6个威胁区，威胁区数据见表1。无人机起点坐标为(200，100，150)，终点坐标为(800，800，150)。
[0125]
表1威胁区数据表
[0126][0127][0128]
设定鸽群算法的相关参数，种群数量为50，迭代次数为200次，其中地图罗盘算子150次，地标算子50次。在相同的威胁环境下，分别使用基于球面矢量的鸽群算法和原始鸽群算法进行规划仿真，根据设定参数计算出的仿真结果即基于球面矢量的鸽群算法的航路规划路线图如图5(a)和图5(b)所示，其收敛曲线如图6(a)和图6(b)所示。