基于分段随机和角度随机算法的无人机三维航路规划方法

1.本发明涉及无人机航路规划技术领域，具体涉及基于分段随机和角度随机算法的无人机三维航路规划方法。


背景技术：

2.在现代工农业活动，以及军事领域中，无人机(unmanned aerial vehicle,uav)发挥着重要作用，对无人机的研究也越来越受到重视；无人机的航路规划是无人机研究领域的重要方向之一，是指在给定的规划空间内，寻找从起始位置到达到目标位置，且满足给定约束条件和性能指标的最优或可行航路，以满足无人机安全、适时地抵达目标区域，并最大可能完成指定任务的需求；随着无线通信、智能控制、传感器探测等技术的快速发展，无人机按规划航路自主飞行的能力不断增强，而通过地面人员远程遥控飞行的功能需求逐渐降低，更加凸显了无人机航路规划的不可替代性。无人机航路规划得到了越来越多研究者的重视；
3.无人机航路规划是一个np(non-deterministic polynomial)问题，直接求解往往容易导致组合爆炸；因此为加速规划进程，研究者在单无人机航路规划方面开展了深入研究，取得了大量成果，最优控制法、人工势场法、启发式a*搜索法、动态规划法等传统数值方法，被广泛应用到航路规范中；随着智能算法的发展，特别是群体智能算法的快速发展，粒子群优化算法(particle swarm optimization,pso)、蚁群算法(ant colony optimization,aco)、遗传算法(genetic algorithm，ga)、模拟退火算法(simulated annealing,sa)等也被广泛应用到航路规划，并取得了相对于传统方法较优的规划结果，但在无人机三维航路的规划中，仍存在诸多问题满意解决，如
4.在文献“huang c,fei j,deng w.anovel route planning method of fixed-wing unmanned aerial vehicle based on improved qpso[j].ieee access,2020,8:65071-65084”中，提出一种量子pso(quantum pso,qpso)算法，用于无人机三维航路规划，但该算法对于复杂障碍环境的避障能力有限，且航路规划的平滑性较差；
[0005]
在文献“张德丰.matlab r2020a智能算法及实例分析[m].北京:电子工业出版社，2021:164-172”中，提出一种基于aco算法的三维航路规划，该方法以地形的网格化为航路规划单元，但该算法对航路首端点(startpoint,stp)和末端点(endpoint,enp)位置较为敏感，且对航路及高度规划的平滑性较差；
[0006]
实际上，在面对复杂地形地貌及障碍区的飞行空间规划时，当前基于群体智能的航路规划一般难以取得较优结果，因为该类型算法多是以不断调整航路点(waypoint,wp)在空间中的位置坐标来实现航路规划的，容易存在规划的航路点往返波动，航路点间距不均匀，难以避免的产生航路平滑性差，避障能力不足等问题；一些算法采用栅格方法对飞行空间进行网格化等分处理，沿着某一坐标轴依次递进，在垂直于该轴的平面进行规划；由于只能沿着固定坐标轴一个方向前进，在一些复杂飞行空间的航路规划时，通常会出现在坐标轴正负方向均移动的情况，此时，栅格法显然不能适应这种复杂飞行空间的航路规划；另
一方面，单一航路规划难以满足特定任务的备份性、隐蔽性需求，因此多航路规划研究更具有应用价值；
[0007]
因此亟需设计一种新的无人机三维航路规划方法，以解决上述现有技术存在的航路点间距不均匀、平滑性差，避障能力不足，对航路的首端点和末端点位置敏感，航路规划单一等关键问题。


技术实现要素：

[0008]
针对上述存在的问题，本发明旨在提供基于分段随机和角度随机算法的无人机三维航路规划方法，本方法通过对航路首末端连线进行随机等分，以随机角度在过分段点的垂直线上确定一个构造航路点，将各构造航路点间插入填充航路点，再将所有航路点依次相连，形成一条航路；同时，对航路进行避障、平滑等处理，进行多航路规划，以克服上述存在的问题，提升无人机航路规划质量，保障无人机执行任务的安全性和高效性，具有航路规划准确率和安全性高的特点。
[0009]
为了实现上述目的，本发明所采用的技术方案如下：
[0010]
基于分段随机和角度随机算法的无人机三维航路规划方法，包括步骤
[0011]
s1.利用随机等分的方法对无人机航路首末端连线进行等分，得到等分后首末端连线在水平投影平面上的分段点坐标；
[0012]
s2.在得到分段点坐标后，利用角度随机的方式，初始化无人机航路上的构造航路点坐标、以及填充航路点的个数及坐标，确定初始航路；
[0013]
s3.判断步骤s2得到的初始航路是否存在航路点在障碍区内，若在，则转入步骤s4，若不在，则令避障航路＝ir，转入步骤s7；
[0014]
s4.根据srar算法的避障方法确定移动航路点，使初始航路点移出障碍区，形成避障航路oar；
[0015]
s5.计算每次迭代中形成的所有避障航路oar的适应度值，对不满足约束要求的解赋予惩罚系数；
[0016]
s6.利用非关联更新迭代方式进行种群更新，直至在迭代终止时，输出种群的最优解r
opt
；
[0017]
s7.对迭代终了时输出的最优解r
opt
进行平滑处理，最终输出无人机航路规划的最佳解r
best
，完成一次单航璐规划。
[0018]
优选的，步骤s1所述的对无人机航路首末端连线进行等分，得到等分后首末端连线在水平投影平面上的分段点坐标的过程包括
[0019]
s101.设x轴与无人机航路首末端连线的夹角为α，若将其等分为s段，每段长度为l，产生s-1个分段点，有s-1条直线过分段点且垂直于首末端连线；
[0020]
s102.设分段点集合为[stp,sep1,
…
,sepi,
…
,sep
s-1
,enp]，共有s+1个点，包含了首端点和末端点，依据三角函数可计算在水平投影平面上的分段点坐标。
[0021]
优选的，步骤s2所述的初始航路的确定过程包括
[0022]
s201.经步骤s1得到的分段点集合[stp,sep1,
…
,sepi,
…
,sep
s-1
,enp]中，除分段点集合中的sep
s-1
，enp两点外，依次以分段点集合中的点为旋转点，以首末端连线为起始轴，随机顺时针或逆时针产生一个角度a，从旋转点沿着角度a与垂线的连接点，构成一个构
造航路点，则依次可构成s-1个构造航路点；
[0023]
s202.首末端点和各构造航路点形成一个构造航路点集合[stp,cwp1,
…
,cwpi,
…
,cwp
s-1
,enp]，将构造航路点集合中各点连接起来，则初始化了一条在水平投影面的航线；在构造航路点集合中各点间均匀填充点，形成填充航路点，进而确定构造航路点坐标；
[0024]
s203.利用经步骤s202得到的首末端点、构造航路点和填充航路点共同构成一条完整的初始航路；
[0025]
s204.得到等分后无人机航路在水平投影平面上随机生成的初始解为
[0026]sij
＝[s,a1,
…
,ai,
…
,a
s-1
]，
[0027]
其中，s
ij
表示第i次的第j个可行解，s表示分段数，ai为第i个航路点的角度；
[0028]
s205.设航路首末端连线长度为l，每段长度为l＝l/s，首端点的坐标为stp＝(st
x
,sty)，末端点的坐标为enp＝(en
x
,eny)，第i个分段点的坐标sepi＝(se
ix
,se
iy
)为：
[0029][0030]
s206.第i个构造航路点坐标cwpi＝(cw
ix
,cw
iy
)由下式计算获得：
[0031][0032]
式(2)中，i＝1，
…
，s-1，若i-1＝0，则se
(i-1)x
＝st
x
，se
(i-1)y
＝sty；sign(
·
)是符号函数，当en
x-st
x
≥0时，sign(en
x-st
x
)＝1，否则sign(en
x-st
x
)＝-1；
[0033]
s207.构造航路点坐标确定后，再按照公式(1)的方式在构造航段点集合的各相邻点间均匀插入填充航路点；设航路点总数均为d，且含首端点和末端点，则填充航路点的个数是d-s-1，共有s段，各段填充航路点个数di为：
[0034][0035]
在式(3)中，i＝1,
…
,s-1，若i-1＝0，则se
(i-1)x
＝st
x
，se
(i-1)y
＝sty，round()是四舍五入函数；
[0036]
s208.通过随机生成s个参数，利用式(1)—(3)，完成对构造航路点坐标、填充航路点个数及坐标的确定，形成一条完整的航路，确定初始航路ir。
[0037]
优选的，步骤s4所述的避障航路oar的计算过程包括
[0038]
s401.设wp1到wp4是一条经过oa的航路，wp2和wp3在oa内，航路必须避开oa，以wp1到wp4的连线作为横轴，wp1为圆点，形成3个向量形成3个向量按照向量运算法则，的模为mwp1到cp的距离；
[0039]
s402.以wp1为旋转点，以wp1到wp4的连线为起始轴，计算出wp1到wp4连线与wp1到cp连线间夹角为α，同时计算出wp1和wp2间的距离为lm，wp1和cp间的距离为lc；得到的模
为lc，复角为α，的模为lm/cos(γ)，复角为γ；
[0040]
s403.利用式(4)求出γ：
[0041][0042]
其中，在式(4)中，ε表示安全裕度；
[0043]
式(4)整理后得：
[0044][0045]
由式(5)可得出γ的唯一解，并且γ是使式(5)成立的最小角度；
[0046]
当γ的值确定以后，可按照公式(2)求出mwp1的坐标；同理，可以求出mwp2的坐标；形成可以完全避开障碍区的航路。
[0047]
优选的，当航路经wp1依次到wp4，再依次到wp7时：
[0048]
s404.要把wp1和wp7连接，再把连接线段按照原航段数进行等分；最后按照步骤s401-s403所述的方法，构造出从wp1依次到mwp1，
……
，mwp5、wp7的避障航路。
[0049]
优选的，步骤s5所述的计算每次迭代中形成的所有避障航路oar的适应度值，对不满足约束要求的解赋予惩罚系数的过程包括
[0050]
s501.设计考虑约束条件的优化模型：
[0051][0052]
其中，ψ表示水平转角的约束，θ表示俯仰角的约束，l
li
表示航段长度的约束，zi表示航路点高度的约束，n(i)表示第i个移动航路点集合中的元素个数，即在第i个障碍区中的航路点个数；
[0053]
s502.适应度函数fit(
·
)表示为：
[0054][0055]
其中，s＝[s,a1,
…
,ai,
…
,a
s-1
]，r表示规划航路点坐标集合，s
→
r表示由s所确定的避障航路中各航路点坐标集合，n
st
表示不满足的约条件个数，p
st
表示当任一约束条件不满时的惩罚系数，p
st
取较大的数，当全部约束条件都满足时，适应度值就等于航程。
[0056]
优选的，步骤s6所述的利用非关联更新迭代方式进行种群更新，直至在迭代终止时，输出种群的最优解r
opt
的过程包括
[0057]
设迭代最大次数为iter
max
，可行解数量为ns，s
opt
表示优化解，s
ij
是第i次的第j个可行解，则i≤iter
max
，j≤ns，若min(fit(s
ij
→rij
))≤fit(s
opt
→ropt
)，则令s
opt
＝s
ij
，r
opt
＝r
ij
，直至满足迭代终止条件，输出种群的最优解r
opt
。
[0058]
优选的，步骤s7所述的对迭代终了时输出的最优解r
opt
进行平滑处理的过程包括
[0059]
s701.对优化解进行位置平滑；
[0060]
s702.对经位置平滑后的优化解进行超高处置，得到经平滑处理后的无人机航路规划的最佳解r
best
。
[0061]
优选的，所述规划方法还包括步骤s8.多航路规划
[0062]
s801.对s1-s2步骤作调整，按照指定途中目标点、指定航向及距离、指定航路点及角度、混合方法等多航路规划方法对构造航路点坐标进行初始化，得到初始化后的构造航路点坐标；
[0063]
s802.在得到初始化后的构造航路点坐标后，再按照s3-s7步骤进行迭代优化，最后经平滑后输出最佳多航路规划。
[0064]
本发明的有益效果是：本发明公开了基于分段随机和角度随机算法的无人机三维航路规划方法，与现有技术相比，本发明的改进之处在于：
[0065]
本发明提出了一种基于分段随机和角度随机算法的无人机三维航路规划方法，本方法在使用时：
[0066]
(1)采用随机对首末端连线进行分段，通过随机角度确定构造航路点，再连接构造航路点和填充航路点的方式确定初始航路；(2)对有向线段角度进行规定，设计圆形障碍区避障方法，航路平滑方法，以及多航路规划方法，提升航路规划质量，满足航路规划多样性要求，使得srar算法具有寻优参数少、计算开销低的优点；(3)提出求解变长解的非关联迭代更新方法进行变长解种群的更新，有效解决了航路规划中对首末端位置类型限制、航路点间距不均匀、航路平滑性差、避障超高处置能力弱、航路规划单一等问题，提升了在复杂多约束条件下的航路规划质量，满足了航路规划多样性要求，具有航路规划准确率和安全性高的优点。
附图说明
[0067]
图1为本发明基于分段随机和角度随机算法的无人机三维航路规划方法的算法流程图。
[0068]
图2为本发明5srar算法航路初始化示意图。
[0069]
图3为本发明4有向线段的位置关系图。
[0070]
图4为本发明srar算法的避障原理示意图。
[0071]
图5为本发明特色避障情况示意图。
[0072]
图6为本发明位置平滑示意图。
[0073]
图7为本发明位置平滑流程图。
[0074]
图8为本发明超高处置示意图。
[0075]
图9为本发明多航路规范方法演示图。
[0076]
图10为本发明实施例2复杂3维地形图
[0077]
图11为本发明实施例2不同首末端类型的验证图
[0078]
图12为本发明实施例2对超高识别的验证图。
[0079]
图13为本发明实施例2对航路平滑效果的验证图。
[0080]
图14为本发明实施例2对算法避障能力的限制图。
[0081]
图15为本发明实施例2对多航路规划性能的验证图。
[0082]
图16为本发明实施例34种算法对航路1的规划比较图。
[0083]
图17为本发明实施例34种算法对航路2的规划比较图。
[0084]
图18为本发明实施例34种算法对航路3的规划比较图。
[0085]
图19为本发明实施例34种算法对航路4的规划比较图。
[0086]
其中：在图6中，图6(a)表示位置平滑前的避障情况示意图，图6(b)表示位置平滑后的避障情况示意图；
[0087]
在图10中，图10(a)表示3维地形图，图10(b)表示地形图在水平上的投影图；
[0088]
在图12中，图12(a)表示三维地形图srar算法的航路规划图，图12(b)表示水平投影srar算法的航路规划图。
具体实施方式
[0089]
为了使本领域的普通技术人员能更好的理解本发明的技术方案，下面结合附图和实施例对本发明的技术方案做进一步的描述。
[0090]
实施例1：参照附图1-19所示的一种基于分段随机和角度随机(srar)算法的无人机三维航路规划方法，包括步骤
[0091]
s1.利用随机等分的方法对无人机航路首末端连线进行等分，得到等分后首末端连线在水平投影平面上的分段点(segment point)坐标
[0092]
s101.如图2所示，设x轴与无人机航路首末端连线的夹角为α，若将其等分为3段，则分段数s＝3，每段长度为l，产生s-1个分段点，有s-1条直线过分段点且垂直于首末端连线；
[0093]
s102.设分段点集合(segment-point set，sps)为[stp,sep1,
…
,sepi,
…
,sep
s-1
,enp]，共有s+1个点，包含了首端点和末端点，依据三角函数可计算在水平投影平面上的分段点坐标；
[0094]
s2.在得到分段点坐标后，利用角度随机的方式，初始化无人机航路上的构造航路点(constructed way point，cwp)坐标、以及填充航路点(filled way point，fwp)的个数及坐标，最终确定初始航路(initial route,ir)
[0095]
s201.如图2所示，经步骤s1得到的分段点集合[stp,sep1,
…
,sepi,
…
,sep
s-1
,enp]中，除分段点集合中的sep
s-1
，enp两点外，依次以分段点集合中的点为旋转点，以首末端连线为起始轴，随机顺时针或逆时针产生一个角度a，从旋转点沿着角度a与垂线的连接点，构成一个构造航路点(constructed way point，cwp)，则依次可构成s-1个构造航路点；
[0096]
s202.首末端点和各构造航路点形成一个构造航路点集合(constructed way-point set,cwps)：[stp,cwp1,
…
,cwpi,
…
,cwp
s-1
,enp]，将构造航路点集合中各点连接起来，则初始化了一条在水平投影面的航线；为保证后续避障算方法的实现，在构造航路点集合中各点间均匀填充点，形成填充航路点(filled way point，fwp)，进而确定构造航路点坐标；
[0097]
s203.利用经步骤s202得到的首末端点、构造航路点和填充航路点共同构成一条完整的初始航路(initial route,ir)，同时由于填充航段点是均匀填充的，其位置不会再随机调整，这样就能保证各航路点的间距始终是均匀的；
[0098]
s204.因此得到等分后无人机航路在水平投影平面上随机生成的初始解为
[0099]sij
＝[s,a1,
…
,ai,
…
,a
s-1
]，
[0100]
其中，s
ij
表示第i次的第j个可行解，s表示分段数，ai为第i个航路点的角度；
[0101]
其中，所述的航路点的角度a的求解方式为：
[0102]
因在航路规划时，航路是有向的，为能够确保在水平投影面上的任意一首端点，经过任意路径，最终到达任意末端点，本实施例明确了任意两条有向线段间的夹角计算方法；
[0103]
两条有向线段中，一条规定为起始轴(starting axis,sa)，则另一条为旋转轴(rotation axis,ra)，旋转轴所在位置是以起始轴首端点为旋转点，以起始轴为初始位置，顺时针或逆时针旋转得到的；一条有向线段在水平坐标平面上的方位关系有4种，即可分别在四个象限存在，则两条有向线段在水平坐标平面上的方位关系有16种；图3显示起始轴在第一象限时，旋转轴可能的象限分布，其中αs为x轴与起始轴的夹角，αr为x轴与旋转轴的夹角，β为从起始轴到旋转轴间的夹角；
[0104]
表1显示了起始轴和旋转轴在不同象限时，两条有向线段间夹角β的计算公式；
[0105]
表1：有向线段夹角计算公式表
[0106][0107]
s205.设航路首末端连线长度为l，每段长度为l＝l/s，首端点的坐标为stp＝(st
x
,sty)，末端点的坐标为enp＝(en
x
,eny)，则第i个分段点的坐标sepi＝(se
ix
,se
iy
)可表示为：
[0108][0109]
实际上，式(1)所求的分段点坐标即是在首末端点间均匀填充获得的；
[0110]
s206.在图2中，第i个构造航路点坐标cwpi＝(cw
ix
,cw
iy
)可由下式计算获得：
[0111][0112]
式(2)中，i＝1，
…
，s-1，若i-1＝0，则se
(i-1)x
＝st
x
，se
(i-1)y
＝sty；sign(
·
)是符号函数，当en
x-st
x
≥0时，sign(en
x-st
x
)＝1，否则sign(en
x-st
x
)＝-1，sign(en
x-st
x
)的作用是保证求构造航路点坐标时，公式(2)适用于四个象限的首末端位置类型；
[0113]
s207.构造航路点坐标确定后，再按照公式(1)的方式在构造航段点集合的各相邻点间均匀插入填充航路点；为使得在迭代过程中搜寻的航路点数量一致，设航路点总数均为d，且含首端点和末端点，则填充航路点的个数是d-s-1，共有s段，各段填充航路点个数di为：
[0114][0115]
同样的，在式(3)中，i＝1,
…
,s-1，若i-1＝0，则se
(i-1)x
＝st
x
，se
(i-1)y
＝sty。round()是四舍五入函数；通过随机生成s个参数，利用式(1)—(3)，完成对构造航路点坐标、填充航路点个数及坐标的确定，形成一条完整的航路，即确定初始航路ir，该方法能完全满足四个象限首末端位置类型的应用需求；
[0116]
s3.判断步骤s2得到的初始航路是否存在航路点wp在障碍区(oa)内，若在，则转入步骤s4，若不在，则令避障航路(obstacle avoidance route,oar)＝ir，转入步骤s7
[0117]
本实施例所述障碍区指对飞行安全有威胁的区域，包括击落威胁、超高威胁、气象威胁、禁止飞行等区域，将障碍区等效为圆形区域进行处理，便于通过比较航路点到圆心(center point,cp)距离来判断航路点是否在障碍区内；
[0118]
s4.根据srar算法的避障方法确定移动航路点(mwp)，使初始航路点移出障碍区，形成避障航路oar
[0119]
本实施例所述srar算法的避障原理如图4所示；
[0120]
情况1：s401.wp1到wp4是一条经过oa的航路，而wp2和wp3在oa内，因此航路规划必须要避开此oa，具体方法为；过在oa内的航路点wp2、wp3作垂线，以wp1为旋转点，以wp1到wp4的线段为起始轴，逆时针旋转γ，旋转线与过wp2垂线相交得到的在oa外的移动航路点(moved wp,mwp)mwp1；同理，可以得到wp3在oa外的mwp2，连接wp1、mwp1、mwp2、wp4，使初始航路成为的避障航路(obstacle avoidance route,oar)，完全避开了oa；
[0121]
为避开障碍区，关键是要确定旋转角度γ，借助向量法进行求解；如图4所示，作坐标变换，以wp1到wp4的连线作为横轴，wp1为圆点，则可形成3个向量，分别为按照向量运算法则，而的模就是mwp1到cp的距离；
[0122]
s402.以wp1为旋转点，以wp1到wp4的连线为起始轴，可计算出wp1到wp4连线与wp1到cp连线间夹角为α，同时计算出wp1和wp2间的距离为lm，wp1和cp间的距离为lc；因此，的模为lc，复角为α，的模为lm/cos(γ)，复角为γ；
[0123]
s403.则利用式(4)可求出γ，式(4)表达如下：
[0124][0125]
其中，在式(4)中，ε表示安全裕度，主要是平衡弧形障碍区对mpw的影响，保证航路规划能完全避开障碍区；
[0126]
式(4)整理后可得：
[0127][0128]
式(5)实际是求解一个一元二次方程，按照几何关系，α和γ的正负号刚好是相反的，据此可以得出γ的唯一解，并且γ是使得式(5)成立的最小角度；当γ的值确定以后，则
可按照公式(2)求出mwp1的坐标；同理，可以求出mwp2的坐标；新构成的航路则可完全避开障碍区；
[0129]
情况2：如图5所示，航路经wp1依次到wp4，再依次到wp7，显然规划的航路经过了障碍区，且不是一条直线，此时：
[0130]
s404.需要把wp1和wp7连接起来，再把连接线段按照原航段数进行等分，图4进行了6等分；再按照步骤s401-s403所示的方法，则可构造出从wp1依次到mwp1，
……
，mwp5、wp7的避障航路oar；
[0131]
s5.计算每次迭代中形成的所有避障航路oar的适应度值，对不满足约束要求的解赋予惩罚系数
[0132]
s501.设计适应度函数fit(
·
)，适应度函数fit(
·
)包含了优化目标，以及约束条件，是定量评价解优劣的标准；srar算法作为一种启发式随机搜索算法，其fit(
·
)设计主要考虑无人机的飞行性能特点，以及障碍区威胁等，采用惩罚系数来对违反约束的解进行赋值，从而排除违反约束条件的解；本实施例中主要对固定翼无人机进行研究，固定翼无人机较旋翼无人机，对飞行性能要求更高，约束条件更复杂；主要的约束条件有：
[0133]
(1)水平转角的约束
[0134]
水平转角ψ限制了无人机沿某一航段飞行，转入下一航段时，航向角变化只能在限制在一定范围，以满足飞行性能需求；航段是指两个相邻航路点间的线段，是一条有向线段；水平转角ψ是按照表1所示方法，计算出wpi到wp
i+1
间有向线段与wp
i+1
到wp
i+2
间有向线段的夹角；则该约束可表示为：|ψ|≤ψ
max
；
[0135]
(2)俯仰角的约束
[0136]
俯仰角θ限制了所规划航路在高度方向爬升或下滑的最大角度，该约束是由无人机性能决定的；设相邻wpi、wp
i+1
的位置分别为(x
i,
yi,zi)和(x
i+1,yi+1
,z
i+1
)，则|θ|≤θ
max
，该约束可表示为：
[0137][0138]
(3)航段长度的约束
[0139]
航段长度l
li
反映了无人机在开始改变飞行状态前必须保持直飞的最短距离；这一现状不仅取决于无人机的飞行性能，通常也和导航要求联系在一起，满足导航状态要求；该约束可表示为l
li
≤l
lmin
，则wpi和wp
i+1
间航段约束可表示为：
[0140][0141]
(4)航路点高度的约束
[0142]
航路点高度zi限制了无人机在所规划航路上的飞行高度范围，不同类型的飞行任务具有不同的高度约束范围，设zi为第i个航路点水平坐标对应的地形高度，最大飞行高度为z
max
，最佳飞行高度是z
best
，飞行高度与地形高度应不低于相对高度z
rel
；为满足无人机执行任务的需要，则各航路点的规划高度为：
[0143][0144]
式(8)较好地模拟了无人机从地面起飞到降落的全过程，航路点高度的约束为：zi≤z
max
；
[0145]
(5)障碍区的约束
[0146]
当航路规划穿越障碍区时，以n(i)表示第i个移动航路点集合中的元素个数，即在第i个障碍区中的航路点个数，n为初始规划时，穿越的障碍区数；该约束是为了保障无人机完全避开障碍区，确保飞行安全，可表示为：
[0147]
为更好地体现srar算法的规划效果，本实施例采用规划航路航程最小的优化目标，则整个优化模型如下式表示：
[0148][0149]
s502.适应度函数fit(
·
)可表示为：
[0150][0151]
其中：s＝[s,a1,
…
,ai,
…
,a
s-1
]，r表示规划航路点坐标集合，s
→
r表示由s所确定的避障航路中各航路点坐标集合，n
st
表示不满足的约条件个数，p
st
表示当任一约束条件不满时的惩罚系数，因为优化问题是最小化优化问题，p
st
通常取较大的数，本实施例令p
st
＝1000，当全部约束条件都满足时，适应度值就等于航程；
[0152]
s6.利用非关联更新迭代方式进行种群更新，直至在迭代终止时，输出种群的最优解r
opt
[0153]
srar算法是一种启发式随机搜索算法，因此寻优过程也需要不断迭代更新来实现；由于srar寻优的解向量是一个变长解，s＝[s,a1,
…
,ai,
…
,a
s-1
]，s中元素的个数取决于s；而传统的群体智能算法都是基于定长解的迭代过程；因此，为了解决变长解的问题，同时便于对多航路进行规划，本实施例提出一种非关联的迭代更新方式；所谓非关联，是指第i次迭代求解si时，不依赖第i-1次迭代求出的s
i-1
、极值等计算结果；第i次的si仅靠随机产生；这种非关联的迭代方式是适合于srar算法的，可以很好地应对变长解的问题；
[0154]
设迭代最大次数为iter
max
，可行解数量为ns，s
opt
表示优化解，s
ij
是第i次的第j个可行解，则i≤iter
max
，j≤ns，若min(fit(s
ij
→rij
))≤fit(s
opt
→ropt
)，则令s
opt
＝s
ij
，r
opt
＝rij
，直至满足迭代终止条件，输出种群的最优解r
opt
；
[0155]
s7.对迭代终了时输出的最优解r
opt
进行平滑处理，包含位置平滑和超高处置，最终输出无人机航路规划的最佳解r
best
，完成一次单航璐规划
[0156]
尽管srar算法的航路规划都是按照直线进行规划，但规划的航路(最优解r
opt
)依然需要进行平滑处理，以对航路进一步优化，最终得到最优航路，以确保航路规划质量；图6显示的是航路平滑的意义；如图6(a)所示，按照srar算法，规划了一条航路，各航路点间距均匀，完全避开障碍区，但该航路依然需要进行平滑处理，平滑的原则就是尽量用直线来表示航路，减少转角的存在，尽量缩短航程；图6(b)虚线是平滑处理前的航路，粗实线是平滑后的最优航路；显然，平滑后的航路要比平滑前更加高效、简洁，航程更短；
[0157]
srar算法的平滑方法是在寻优结束后，对优化解进行的操作，包含位置平滑(position smoothing)和超高处置(over height disposal)两个步骤；
[0158]
s701.对优化解进行位置平滑
[0159]
如图6所示，从stp到enp规划航路，假设依次共避开n个oa，必然会形成相应的移动航路点，设避开第i个oa形成的移动航路点，构成的第i个移动航路点集合(mwpset,mwps)为[mwp
i1
,mwp
i2
,
…
,mwp
in(i)
]，n(i)表示第i个移动航路点集合中的元素个数；图7显示了位置平滑的流程；
[0160]
位置平滑的步骤如下：
[0161]
s7011.每一个mwp
i1
和mwp
in(i)
分配一个构造航路点集合中的距离最近点(nearest segment point，nsp)，分别记为nsp
i1
和nsp
in(i)
；在第i个mwps中，若n(i)为偶数，选择编号为n(i)/2和(n(i)+1)/2的两个移动航路点作为优化连接点(optimized connection point,ocp)[ocp
i1
,ocp
i2
]；若n(i)为奇数，选择编号为(n(i)+1)/2的移动航路点作为优化连接点[ocp
i1
,ocp
i2
]，此时，ocp
i1
＝ocp
i2
；同时，将ocp按序插入到构造航路点集合中；
[0162]
s7012.对于第1个mwps，则在ocp
11
到stp间重新均匀填充航路点，并判断是否有新生成的航路点在oa内；若无，则利用ocp
11
到stp间新均匀填充航路点代替原来ocp
11
到stp间的航路点；若有，则利用ocp
11
到nsp
i1
间新均匀填充航路点代替原来ocp
11
到nsp
i1
间的航路点；
[0163]
s7013.对于第n个mwps，则在ocp
n2
到enp间重新均匀填充航路点，并判断是否有新生成的航路点在oa内；若无，则利用ocp
n2
到enp间新均匀填充航路点代替原来ocp
n2
到enp间的航路点；若有，则利用ocp
n2
到nsp
nn(i)
间新均匀填充航路点代替原来ocp
n2
到nsp
nn(i)
的航路点；
[0164]
s7014.对于第2到第n-1个mwps，设j＝2,
…
,,n-2，依次在在ocp
j2
到ocp
(j+1)1
间重新均匀填充航路点，并判断是否有新生成的航路点在oa内；若无，则利用ocp
j2
到ocp
(j+1)1
间新均匀填充航路点代替原来ocp
j2
到ocp
(j+1)1
间的航路点；若有，则求出ocp
j2
到ocp
(j+1)1
间的mwps，嵌入执行步骤s5011—s5014，直至ocp
j2
到ocp
(j+1)1
间新均匀填充的航路点不经过oa为止；
[0165]
s702.对经位置平滑后的优化解进行超高处置
[0166]
对航路进行平滑处理，实际是在各优化连接点间重新均匀填充航路点，但对三维航路规划而言，会存在新均匀填充的航路点在高度上不满足约束的可能，出现超高航路点(over high way point,ohwp)，因此需要对这些航路点进行处理，使其满足高度约束；图8
显示了超高处置的原理；
[0167]
实质上，图8不仅显示了超高处置的原理，也显示了位置平滑的过程，细虚线是寻优后输出的航路，从stp到enp，中间包含粗虚线的航路是位置平滑后的航路，但中间存在航路点超高，因此需要进行超高处置；
[0168]
具体处置步骤如下：
[0169]
s7021.检测平滑后的航路点是否不满足高度约束条件；若有，将其对应替换的原航路点作为一个mwps，设mwps共有n个元素；
[0170]
s7022.确定mwps的nsp和ocp；在构造航路点集合中确定nsp1和nspn；将mwps中的mwp1和mwpn作为ocp1和ocp2；
[0171]
s7023.在nsp1和mwp1，nsp2和mwp2间重新均匀填充航路点，并判断是否有新生成的航路点在oa内；若无，则利用nsp1到mwp1，nsp2到mwp2间新均匀填充航路点代替原来nsp1到mwp1，nsp2到mwp2间的航路点；若有，则计算出所有mwps，包含因超高产生的mwps，将nsp1作为stp，nsp2作为enp，按照图7所示的方法进行位置平滑；此时，所获得的航路必然满足高度约束的要求，得到经平滑处理后的无人机航路规划的最佳解r
best
；
[0172]
如图8所示，为满足高度约束条件，最终经过位置平滑、超高处置后输出的航路是从stp到enp粗实线航路。
[0173]
根据上述步骤s1-s7，完成一次单航璐规划过程。
[0174]
优选的，本实施例所述方法还包括步骤
[0175]
s8.多航路规划
[0176]
在得到无人机单一航路的最优航路规划的基础上，进行无人机多航路规划，在无人机多航路规划的过程中：
[0177]
s801.需对s1-s2步骤作调整，按照指定途中目标点、指定航向及距离、指定航路点及角度、混合方法等多航路规划方法对构造航路点(constructed way point，cwp)坐标进行初始化，得到初始化后的构造航路点坐标；
[0178]
s802.在得到初始化后的构造航路点坐标后，再按照s3-s7步骤进行迭代优化，最后经平滑后输出最佳多航路规划；
[0179]
其中，所述的指定途中目标点、指定航向及距离、指定航路点及角度、混合方法等多航路规划方法分别包括：
[0180]
1.指定途中目标点的航路规划
[0181]
设定某一点为途中目标点(midway target point，mtp)，坐标为mtp＝[mtp
x
,mtpy]，航路规划时必须经过该点；如图9所示，假设将stp到enp间连线随机分为s段，可求出stp到mtp的距离l，且以stp为旋转点，将stp到enp间连线作为起始轴，按照表1方法求出如图8所示的夹角a1，因此第1段的信息是确定的，即把该mtp作为第一个cwp1；余下的s-1段按照前述方法随机确定构造航路点和填充航路点；则可以保证所规划航路由stp开始，必经mtp，最终到达enp；
[0182]
2.指定航向及距离的航路规划
[0183]
本方法本质上和指定途中目标点方法是一致的，如图9所示，指定途中目标点方法是先确定mtp坐标，再来确定a1和l；而本方法是先确定[a1,l]，即航向和距离，再来确定第一个cwp1；余下的s-1段按照前述方法确定的航路点和填充航路点；
[0184]
3.指定航路点及角度的航路规划
[0185]
如图9所示，设航路点总数为d，任意选择其中第i个航路点作为选择航路点(selectedway point,swp)；规定stp为旋转点，将stp到enp间连线作为起始轴，旋转至stp到swp间连线，按表1方法求出如图9所示的夹角δ，此时再求出满足不小于δ的最短规划航路，因此，该方法需要指定[swp,δ]；
[0186]
4.混合方法的航路规划
[0187]
混合方式是在将指定中途目标点和指定航路点及角度点相结合的方式，先指定中途目标点mtp，再确定[swp,δ]；规定以mtp为旋转点，mtp到enp间连线为起始轴，旋转至mtp到swp间连线，并计算产生的夹角δ，求出不小于δ的最短规划航路。
[0188]
实施例2：与实施例1不同的是，本实施例通过实验对实施例1所述的基于分段随机和角度随机算法的无人机三维航路规划方法的可行性进行验证，包括步骤：
[0189]
s9.为验证srar算法的无人机航路规划性能，将生成一幅100km
×
100km复杂地形图，用于检验算法对首末端类型处置、超高识别、航路优化、复杂障碍区避障、多航路规划、寻优性能等进行验证，该地图三维坐标(xi,yi,zi)间关系按照如下公式进行表示：
[0190][0191]
其中：a，b，c，d，e均是1
×
50维的系数向量，其值为:
[0192][0193]
则由式(12)生成的地形图如图10所示，其中图10(a)是3维地形图，图10(b)是地形图在水平上的投影；图10(b)中红色圆圈表示障碍区，共有24个障碍区，障碍区密度较大，且相互交错，形成复杂障碍区分布，极大增加了航路规划难度；同时24个障碍区也远超其他文献中所设置的障碍区数量
[14,17,20]
，障碍区之间也能对算法造成耦合影响，可有效检验算法的航路规划质量；
[0194]
s901.对不同首末端类型规划的性能验证
[0195]
根据平面直角坐标系可以看出，一共有4种首末端类型；在地形图上找4对首末端点，分别为：stp1＝[50,50]，enp1＝[6,9]；stp2＝[50,50]，enp2＝[10,88]；stp3＝[50,50]，enp3＝[96,89]；stp4＝[50,50]，enp4＝[98,4]，代表4种首末端类型；按照srar算法进行航路规划，设2≤s≤4，a∈[-40
°
,40
°
]，iter
max
＝50，ns＝50，d＝40，ψ
max
＝80
°
，θ
max
＝25
°
，ε＝0.5km，l
lmin
＝1km，z
max
＝2.8km，z
best
＝1km，z
rel
＝0.5km,，则4条航路规划如图11所示；为了显示方便，且规划高度符合约束要求，图11略去了地形信息，仅在2维平面上显示规划航路；
[0196]
图11显示了srar算法规划的4条航路，在均满足约束条件的情况下，成功避开障碍区，到达目的地，表明了srar算法能够应对任意类型的首末端位置类型，可以规划从任意位置到任意位置的航路，这符合航路规划的实际需求；因此，srar算法增强了航路规划的实用性；
[0197]
s902.对超高识别的性能验证
[0198]
为验证srar算法在航路规划时，对超高识别的能力，避免出现超高规划的情况，将图10(b)两个最高峰的障碍区去掉，按照步骤s901的算法设置，在z
max
分别为2.8km和3.8km时，验证从stp＝[32,95]到enp＝[84,69]，以及从stp＝[6,9]到enp＝[70,47]间规划航路的超高识别性能；如图12所示，其中图12(a)是从三维地形图直观展示了srar算法的航路规划效果，图12(b)是从水平投影显示了算法的超高识别性能；
[0199]
如图12所示，航路1和航路3在最高2.8km的高度限制下，航路最高高度分别为2.789km和2.792km，航程分别为62.09km和77.50km；而航路2和航路4在最高3.8km的高度限制下，航路最高高度分别为3.408km和3.057km，航程分别为59.78km和74.75km。显然，在不同高度约束下，算法会自动选择合适的航路，以满足高度约束要求；因此，srar算法能够有效避免航路超高；
[0200]
s903.航路平滑效果的验证
[0201]
本实施例提出对寻优后的输出航路进行平滑处理，以提高航路规划质量，为验证srar算法的航路平滑效果，按照步骤s901的算法设置，选择从stp＝[44,2]到enp＝[63,49]之间的规划航路，验证航路优化能力；如图13所示，航路1是寻优后，未经任何平滑处理，直接输出的航路，航程为56.97km，航路1的航程最大，且不太平滑；航路2是对航路1进行位置平滑和超高处置后输出的航路，航程为55.50km，航路2比航路1平滑；而航路3是对航路1仅经过位置平滑后输出的航路，航程为54.89km，航路3未执行超高处置，如图13所示，两超高航路点间的航路点均超高，尽管航程最短，但不满足高度约束条件；
[0202]
图13很好地证明了经寻优输出的航路进行平滑处理的必要性，但在平滑处理时，必须同时考虑位置和高度的约束条件，进行位置平滑和高度处置，才能得到更高质量的规划航路。
[0203]
s904.对复杂障碍区避障能力以及寻优性能的验证
[0204]
一般而言，复杂障碍区指障碍区之间彼此相互靠近，交叉排列；这对航路规范算法是一个较大的挑战；同时，为验证srar算法提出的非关联迭代方式的寻优性能，在复杂障碍区的航路规划条件下，将srar算法连续运行50次，分别取适应度值的最小值(min)、均值(mean)、最大值(max)、标准差(standard deviation,std)衡量算法的寻优性能；按照步骤s901的算法设置，选择从stp＝[53,3]到enp＝[32,95]规划航路1，从stp＝[44,87]到enp＝[18,11]规划航路2，从stp＝[78,86]到enp＝[85,5]规划航路3，以及从stp＝[44,2]到enp＝[67,87]规划航路4，连续规划50次；这4条航路间均存在相互交叉、排列紧密的障碍区；图14选择50次规划中适应度值最小的航路，4条航路均避开了复杂障碍区，表明srar算法有很好地避障能力，能够保障航路规划的安全性，确保无人机顺利完成任务；
[0205]
4条规划航路50次计算的统计量如表2所示，表2还给出了最小和最大航程时对应的解；表2表明：在面对复杂障碍区规划时，srar算法规划的航路能满足所有约束条件，航程变化幅度很小，输出的航路具有较强的鲁棒性和稳定性，因此算法具有优异的寻优性能，能够满足复杂航路规划要求；
[0206]
表2：srar算法寻优能力验证表
[0207]
[0208][0209]
s905.对多航路规划性能的验证
[0210]
为满足不同条件下的航路规划需要，srar算法具有多航路规划能力，为验证srar算法的多航路规划能力，按照步骤s901的算法设置，选择从stp＝[7,40]到enp＝[95,40]之间规划航路，航路1按照指定中途目标点方法进行规划，中途目标点坐标mtp＝[18,77]，航程为131.2868km；航路2和航路3按照指定航向及距离方式进行规划，航向和距离分别为[60
°
,25]和[-40
°
,15]，航程分别为104.6126km和95.3105km；航路4和航路5按照指定航路点及角度的方式进行规划，选择航路点swp＝21，角度分别γ≥20
°
，γ≤-20
°
，航程分别为99.6968km和100.8934km；航路6和航路7按照混合方式进行规划，中途目标点坐标mtp＝[38,5]，选择航路点swp＝25，角度分别γ≥5
°
，γ≤-5
°
，航程分别为117.6766km和118.8198km；图15显示了4种多航路方法规划的7条航路，7条航路均满足了约束要求；显然，在多航路规划时，航程最短是在满足航路规划方式的前提下实现的，从而增加航路规划的多样性和备份性，满足特定任务条件下的规划需求，多航路规划对实际航路规划具有重要现实作用。
[0211]
实施例3：与上述实施例不同的是，为验证本实施例所述的基于分段随机和角度随机算法的无人机三维航路规划方法的优越性，实际本实施例将的基于分段随机和角度随机算法的无人机三维航路规划方法与其它航路规划算法的性能进行比较
[0212]
为了验证实施例1提出的srar算法的航路规划性能，证明srar算法设计的合理性、实用性和科学性，将srar算法与同样是基于群体智能的其他航路规划方法进行比较，分别与基于qpso算法的航路规划方法、基于ga算法的航路规划方法、基于aco算法的航路规范方法进行比较，既采用连续计算50次的方法比较算法寻优性能，又比较应对不同障碍区的航路规划性能；4种算法将规划4条航路，从stp＝[2,40]到enp＝[57,30]之间规划航路1，从stp＝[63,98]到enp＝[98,4]之间规划航路2，从stp＝[34,34]到enp＝[45,76]之间规划航路3，从stp＝[32,95]到enp＝[53,3]之间规划航路4；这4条航路各有特点，航路1是简单航路，航路上不需避开任何障碍区；航路2是一般航路，航程较长，容易避开航路上的障碍区；航路3和航路4属于复杂航路，对避障能力要求较高，两者区别在于航程的长短；
[0213]
航路规划的各约束条件如实施例1步骤s7所示；srar算法参数设置如实施例2步骤s901所示；qpso算法、ac算法和ga算法的iter
max
＝100，ns＝200；上述3种算法本质上均是对各航路点位置坐标进行优化，qpso和ga算法的航路点个数固定，设d＝40，而aco算法采用栅格法，航路点个数取决于对固定坐标轴的等分数量；各算法航路规划质量比较如表3所示，表3包含了各算法在同一运行条件下，计算一次的平均时间消耗(elapse time,et)，单位为秒(s)；需要寻优的参数个数(number of parameters,np)；50次计算出航程的最小值(min)、均值(mean)、最大值(max)、标准差(standard deviation,std)；50次计算中违反约束条件(constraint violations,cv)的次数；对违反各约束条件的次数进行统计，用1
×
6维向量cv表示，cv中的元素依次对应违反3.1节约束条件的次数；如表3中的[00060]，表示6
次违反第4个约束条件，即违反高度约束；
[0214]
表3：4种算法航路规划性能比较
[0215][0216]
表3表明：在不同的航路规划中，面对多条件约束，srar算法时间消耗最小、寻优参数最少、性能最稳定、鲁棒性最强、航路规划质量最高，有力证明了srar算法设计的合理有效；其他3种算法由于自身原理等因素，难以应对多约束的复杂航路规划问题；图13至图19分别显示了对4条航路规划时，不同算法获得最短航程的航路比较，直观显示了srar算法的航路规划质量；造成其他3种算法规划质量低的原因是：除了算法自身的寻优性能外，主要是通过调整航路点坐标位置来实现对复杂的、多约束航路规划时，很难保证所有航路点间的坐标都获得协调一致的恰当位置；而srar算法巧妙地通过选择角度来确定坐标，因此避开了因直接调整航路点坐标而造成的普遍性问题，保障了航路规章质量。
[0217]
结合上述实施例2和实施例3的结果可以看出，本发明实施例1所述的基于分段随机和角度随机算法的无人机三维航路规划方法能够用于规划复杂空间、多约束的无人机三维航路规划，有效的解决了航路规划中对首末端位置类型限制、航路点间距不均匀、航路平滑性差、避障超高处置能力弱、航路规划单一等问题，升了在复杂多约束条件下的航路规划质量，满足了航路规划多样性要求。
[0218]
以上显示和描述了本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。