算法的灾后井下无人机航迹规划算法的灾后井下无人机航迹规划吕文红1，夏双双2，魏博文2，殷立杰2，郭银景2
(1.山东科技大学 交通学院，山东 青岛 266590；2.山东科技大学 电子通信与物理学院，山东 青岛 266590)
摘要：针对传统
算法应用在煤矿灾后井下环境侦测的无人机航迹规划中存在搜索点冗余、遇到突发威胁时实时性较差等问题，提出了一种逆向变权重稀疏算法。根据无人机自身性能约束及灾后井下威胁模型，从目标点到起始点进行全局静态航迹规划，避免大量无效搜索；根据无人机执行任务的需要设置不同权重系数，得到侧重航程或安全的航迹；通过引入次目标点策略，仅对被突发威胁覆盖的航迹进行修正，可在短时间内有效避开突发威胁。仿真结果表明，利用该算法进行航迹规划用时较短，无人机受到的威胁较小，可有效保障航迹规划的实时性和安全性。
关键词：灾后井下环境侦测；无人机；航迹规划；算法；逆向搜索；变权重；次目标点；突发威胁
煤矿发生灾害后，需要对灾害地点尽早实施救援，但由于灾害环境不明确，人员进入灾害现场容易受伤，所以可应用微小型旋翼无人机侦测井下环境信息。
井下发生灾害时，空间中充斥大量有毒可燃气体，伴随爆炸产生高温、有毒烟雾及强烈气流[1-2]，造成矿井间歇性坍塌，爆炸、火焰等突发威胁严重影响无人机飞行。因此，精准高效的航迹自主规划能力是无人机在灾后井下环境中安全完成任务的重要保障。
目前常用的航迹规划算法有人工势场法[3-5]、粒子群优化算法[6-7]、算法[8-11]等。人工势场法可用于动态环境下的航迹规划，实时性较好，但存在局部最优问题；粒子群优化算法原理简单，实现方便，前期搜索速度快，但同样存在局部最优问题；算法是一种经典的启发式搜索算法，理论上可保证全局最优，但存在搜索点多、耗时长的问题。针对算法存在的问题，文献[12]提出了结合约束条件的稀疏算法，通过去除大量不必要的搜索点，提高算法的搜索速度；文献[13]提出了逆向算法，从目标点向起始点搜索，减少不必要的搜索，提高算法效率。但上述2种算法在无人机遇到突发威胁时，都需要重新规划航迹，耗时较长。本文在文献[12-13]的基础上，提出了逆向变权重稀疏算法，利用该算法规划无人机全局静态航迹，并引入次目标点策略，能对突发威胁航段进行航迹修正，具有较高的实时性和安全性。
无人机航迹规划是指在满足飞行约束条件的情况下，规划从起始点到目标点的最小代价路径。约束条件一般为无人机自身性能约束、环境约束及突发威胁约束等。
针对井下灾害环境，无人机自身性能约束条件一般考虑以下方面：
(1) 最大/最小飞行高度。假设无人机最小飞行高度为Hmin，最大飞行高度为Hmax，第i(i=1，2，…，I，I为航段总数)段航段的飞行高度为Hi，则需满足约束条件为
Hmax≥Hi≥Hmin
(1)
(2) 最小航段长度。无人机在改变飞行姿态前需要保持一段直线飞行距离用于调整参数，假设无人机第i段航段长度为li，最小航段长度为lmin，则约束条件为
li≥lmin
(2)
(3) 最大航程。假设无人机当前位置节点为n，且无人机允许最大航程为Lmax，则约束条件为
ds(n)+de(n)≤Lmax
(3)
式中：ds(n)为已规划航迹距离；de(n)为无人机当前位置节点到目标点的直线距离，de(n)小于无人机当前位置节点到目标点的实际距离。
煤矿井下环境中主要威胁分为岩石、废墟等静态威胁和火焰、爆炸等突发威胁，本文将静态威胁的影响空间等效为圆椎体和长方体。
火焰威胁模型为
(4)
式中：PF(xn,yn)为无人机在当前位置节点坐标(xn,yn)处受到的火焰威胁概率；dF为无人机与火焰中心的实际距离；rF为火焰的威胁作用半径；λ为井下惰性气体比例。
爆炸威胁模型为
(5)
式中：PB(xn,yn)为无人机在当前位置节点坐标(xn,yn)处受到的爆炸威胁概率；dB为无人机与爆炸中心的实际距离；rB为爆炸的威胁作用半径。
将威胁模型与环境数字地图按式(6)叠加，生成威胁概率综合数字模型，如图1所示。威胁①⑥⑨为岩石威胁，是井下原有存在的岩石，是事先已知的；威胁③⑧
为煤矿自身及生产设备受到破坏后产生的废墟，具有突然性及不确定性；威胁②④⑤为火焰威胁，对无人机的威胁程度与其高度及范围有关；威胁⑦⑩
为爆炸威胁，具有突发性及破坏性大等特点。威胁概率综合数字模型包含了各种威胁信息，利用其在大于威胁范围的作业高度规划航迹，结合已有的二维地图实现三维地图范围的航迹规划，可提高航迹规划速度。
(6)
式中：P(xn,yn)为无人机在当前位置节点坐标(xn,yn)处受到的总威胁概率；PRm1(xn,yn)为岩石威胁概率；PBm2(xn,yn)为爆炸威胁概率；PFm3(xn,yn)为火焰威胁概率；PWm4(xn,yn)为废墟威胁概率；N1，N2，N3，N4分别为岩石、爆炸、火焰、废墟威胁的数量。
图1 威胁概率综合数字模型
Fig.1 Integrated digital model of threat probability
算法的无人机航迹规划无人机航迹总代价为
M(n)=g(n)+h(n)
(7)
式中：g(n)为起始点到当前位置节点的实际航迹代价；h(n)为当前位置节点到目标点的估计航迹代价。
g(n)=g(n-1)+dnb(n,n-1)+P(n)+H(n)
(8)
式中：dnb(n,n-1)为当前位置节点和前一节点的欧氏距离；P(n)为起始点到当前位置节点的综合威胁概率；H(n)为高度代价。
(9)
式中：Pnf(n)为当前位置节点到目标点的综合威胁概率；Psum为总航迹威胁概率；Lnf(n)为当前位置节点到目标点的航迹长度；w1,w2,w3分别为Pnf(n),Lnf(n),H(n)的权重系数。
通过调节权重系数，可实现侧重安全或航程的航迹规划。当无人机需要在短时间快速执行任务时，则设置w2较大，这样规划的全局静态航迹偏重航程较短；当需要保证无人机较高的安全性来执行任务时，则设置w1较大，这样规划的全局静态航迹偏重保证无人机安全；当无人机执行任务的环境垂直空间较小、无人机工作过程中易受上方物体威胁时，则设置w3较大，这样规划的全局静态航迹在保证无人机安全的前提下，以较低的高度保持低空飞行。权重系数w1,w2,w3中，w3和其他2个权重系数兼容性较大，可在侧重安全或航程时，以合适的飞行高度飞行，而w1和w2兼容性较差，即如果无人机工作环境较恶劣，难以同时满足航程和安全的要求。
为节省航迹规划时间，采用逆向变权重稀疏算法规划全局静态航迹，即从目标点向起始点进行规划，计算每次搜索当前位置节点到起始点的最小航迹总代价，具体流程如图2所示。
图2 基于逆向变权重稀疏
算法的全局静态航迹规划流程
Fig.2 Global static route planning flow based on reverse variable weight sparsealgorithm
2.3.1 次目标点设置
假设无人机按照全局静态航迹飞行到当前位置节点n(xn,yn,zn)时，机载传感器探测到突发威胁，其中心坐标为o(xo,yo,zo)，突发威胁的作用半径为r，无人机与突发威胁中心的距离为d(d-r>lmin)。由于突发威胁覆盖了部分全局静态航迹，无人机如果继续按全局静态航迹飞行，将遭遇突发威胁，如图3所示。此时需要对全局静态航迹进行在线修正,以规避突发威胁。为提高航迹修正速度，提出一种次目标点策略，将距突发威胁最近的下一航迹点作为次目标点，引导算法向次目标点搜索，舍弃被突发威胁覆盖的航迹点，对无人机当前位置节点到次目标点的航迹进行修正，再结合次目标点到目标点的航迹即可获得当前位置节点到目标点的航迹。通过计算无人机当前位置节点之后各航迹点与突发威胁中心的距离(式(10))来确定次目标点，即当第1次出现航迹点与突发威胁中心的距离dτ大于威胁作用半径r时，该航迹点即为次目标点。
(10)
式中:(xn+j+1,yn+j+1,zn+j+1)为次目标点坐标;j为全局静态航迹上被突发威胁覆盖的航迹点数。
图3 无人机与突发威胁位置关系
Fig.3 Position relation between unmanned aerial vehicle and sudden threat
2.3.2 安全距离计算
受到突发威胁时，需要无人机修正航迹以快速躲避威胁，考虑修正需要时间，定义安全飞行规划距离：
dsafe=v t+ε
(11)
式中：v为无人机飞行速度；t为修正航迹所需平均时间；ε为因风速等阻力和计算误差等产生的误差，为方便有限条件下的实验仿真，本文以最小航段长度lmin的整数倍代替ε。
2.3.3 高度规划
发生突发威胁时修正航迹点与煤矿巷道顶部高度的差为ΔH，每个航迹点对应范围内突发威胁的最大高度为H，取无人机安全工作高度为ΔH/2，则修正航迹点的高度为ΔH/2+H。
在操作系统为Windows 7、主频为2.30 GHz、内存为4 GB的计算机上，采用Matlab软件进行仿真。假设无人机在500 m×500 m的工作区域执行任务，煤矿井下环境威胁采用图1构建的数字地图模型，并随机生成突发威胁
。设置无人机的起始点坐标为S(34，159)，目标点坐标为E(451，444)，最大飞行高度Hmax=5 m，最大航程Lmax=2 km，最小航段长度lmin=3.5 m，权重系数w3=0.1，各威胁中心坐标、威胁作用半径和高度等数据见表1。
在不同权重系数w1和w2下，对稀疏算法、逆向算法及本文提出的逆向变权重稀疏算法进行比较，基于不同算法的无人机航迹规划路径仿真结果如图4—图6所示(实线表示全局静态航迹，虚线表示遇到突发威胁后修正的航迹)，基于各算法的航迹规划时间、航迹长度及总威胁概率见表2。
表1 威胁中心坐标、作用半径和高度数据
Table 1 Center coordinate,action radius and height of threat
从图4—图6和表2可看出，w1较大时，利用3种算法规划的航迹与威胁的距离较远，无人机受到的总威胁概率较小，但航迹长度较长，航迹规划用时较长，表明规划的航迹侧重保证无人机安全；w2较大时，利用3种算法规划的航迹长度较短，航迹规划用时较短，但航迹与威胁的距离较近，无人机受到的总威胁概率较大，表明规划的航迹侧重航程较短，可保证无人机较短时间内完成任务；在相同条件下，本文算法与其他2种算法相比,航迹规划用时和航迹长度较短，且无人机受到的总威胁概率较小，仅对突发威胁覆盖的航迹进行修正，可较好地保障航迹规划的实时性和安全性。
(a)w1=0.3,w2=0.6
(b)w1=0.6,w2=0.3
图4 基于稀疏算法的无人机航迹规划路径
Fig.4 Route planning path of unmanned aerial vehicle based on sparsealgorithm
(a)w1=0.3,w2=0.6
(b)w1=0.6,w2=0.3
图5 基于逆向算法的无人机航迹规划路径
Fig.5 Route planning path of unmanned aerial vehicle based on reversealgorithm
(a)w1=0.3,w2=0.6
(b)w1=0.6,w2=0.3
图6 基于逆向变权重稀疏算法的无人机航迹规划路径
Fig.6 Route planning path of unmanned aerial vehicle based on reverse variable weight sparsealgorithm
表2 不同权重系数下3种算法性能对比
Table 2 Performance comparison of three algorithms under different weight coefficients
针对煤矿井下灾害环境下无人机航迹规划问题，对算法进行改进，提出了逆向变权重稀疏算法，并引入次目标点策略，能在短时间内有效规划全局静态航迹，以及在保持原有不受威胁航迹不变的基础上仅修正被突发威胁覆盖的航迹。仿真结果表明，基于该算法规划的航迹能有效避开突发威胁，且航迹规划用时及航迹长度较短，可保证无人机在较短时间内安全完成任务。
参考文献(References)：
[1] 李晓鹏.煤矿探测机器人姿态控制与局部路径规划研究[D].西安:西安科技大学,2011.
[2] 秦玉鑫.煤矿灾害信息探测机器人系统研制及其地图构建与路径规划研究[D].焦作:河南理工大学,2015.
[3] DONG H K,WANG H,SHIN S.Decentralized control of autonomous swarm systems using artificial potential functions: analytical design guidelines[J].Journal of Intelligent and Robotic Systems,2006,45(4):369-394.
[4] CHEN X,ZHANG J.The three-dimension path planning of UAV based on improved artificial potential field in dynamic environment[C]//International Conference on Intelligent Human-Machine Systems and Cybernetics,Hangzhou,2013:144-147.
[5] 方旭,刘金琨.四旋翼无人机三维航迹规划及跟踪控制[J].控制理论与应用,2015,32(8):1120-1128.
FANG Xu,LIU Jinkun.Three-dimension path planning and trajectory tracking control for quadrotor unmanned aerial vehicle[J].Control Theory and Applications,2015,32(8):1120-1128.
[6] SUN S,LI J.A two-swarm cooperative particle swarms optimization[J].Swarm & Evolutionary Computation,2014,15(2):1-18.
[7] WANG Qiang,ZHANG An,QI Linghui.Three-dimensional path planning for UAV based on improved PSO algorithm[C]//The 26th Chinese Control and Decision Conference,Changsha,2014:3981-3985.
[8] LIN C L,LEE C S,TSAI Y J,et al.Flight path planning for mini rotor UAVs[C]//The 11th IEEE International Conference on Control & Automation,Taichung,2014:1339-1344.
[9] ELHALAWANY B M,ABDEL H M,TAGELDEEN A,et al.Modifiedalgorithm for safer mobile robot navigation[C]//Proceedings of International Conference on Modelling,Identification & Control,Cairo,2013:74-78.
[10] 王帅.煤矿井下基于改进算法的移动机器人路径规划[J].煤矿机械,2008,29(11):65-67.
WANG Shuai.Path planning of mobile robot based on advancedalgorithm under coal mine[J].Coal Mine Machinery,2008,29(11):65-67.
[11] 黄文刚,张怡,姜文毅,等.变步长稀疏算法的无人机航路规划[J].计算机工程与应用,2012,48(29):206-209.
HUANG Wengang,ZHANG Yi,JIANG Wenyi,et al.SAS algorithm with changeable steps for route planning of UAVs[J].Computer Engineering and Applications,2012,48(29):206-209.
[12] SZCZERBA R J,GALKOWSKI P,GLICKTEIN I S,et al.Robust algorithm for real-time route planning[J].IEEE Transactions on Aerospace & Electronics Systems,2000,36(3):869-878.
[13] 李得伟,韩宝明,韩宇.一种逆向改进型路径搜索算法[J].系统仿真学报,2007,19(22):5175-5177.
LI Dewei,HAN Baoming,HAN Yu.Conversely improved a star route search algorithm[J].Journal of System Simulation,2007,19(22):5175-5177.
algorithmLYU Wenhong1, XIA Shuangshuang2, WEI Bowen2, YIN Lijie2, GUO Yinjing2
(1.College of Transportation,Shandong University of Science and Technology,Qingdao 266590,China; 2.College of Electronic,Communication and Physics,Shandong University of Science and Technology,Qingdao 266590,China)
Abstract:In view of problems of redundant search point and poor real-time performance when encountering sudden threat existed in application of traditionalalgorithm in route planning of unmanned aerial vehicle in environment detection for post-disaster underground,a reverse variable weight sparsealgorithm was proposed. According to performance constraint of unmanned aerial vehicle and threat model in post-disaster underground,global static route planning is carried out from target point to start point,so as to avoid a large number of invalid searches. Different weight coefficients are set according to mission requirement of unmanned aerial vehicle,so as to obtain route focuses on distance or safety. Only the path covered by sudden threat is corrected by introducing sub-target point strategy,so as to avoid sudden threat effectively in short time. The simulation results show that the algorithm used in route planning can save time of route planning and reduce threat to unmanned aerial vehicle,which can effectively guarantee real-time performance and safety of route planning.
Key words:environment detection for post-disaster underground; unmanned aerial vehicle; route planning;algorithm; reverse search; variable weight; sub-target point; sudden threat
文章编号：1671-251X(2018)05-0085-06
DOI:10.13272/j.issn.1671-251x.2017100058
中图分类号：TD679
文献标志码：A
网络出版地址：http://kns.cnki.net/kcms/detail/32.1627.TP.20180418.1527.003.html
收稿日期：2017-10-25；
修回日期：2018-04-04；
责任编辑：盛男。
基金项目：国家自然科学基金资助项目(61471224)；中国煤炭工业协会科学技术研究指导性计划项目(MTKJ2016-293)。
作者简介：吕文红(1968-)，女，山东青岛人，教授，博士,主要研究方向为无人飞行器导航与控制、无人机飞行管理信息系统规划与设计，E-mail:zklwh@sdust.edu.cn。
引用格式：吕文红，夏双双，魏博文，等.基于改进
算法的灾后井下无人机航迹规划[J].工矿自动化，2018,44(5)：85-90.
LYU Wenhong,XIA Shuangshuang,WEI Bowen,et al. Route planning of unmanned aerial vehicle in post-disaster underground based on improvedalgorithm[J].Industry and Mine Automation，2018,44(5)：85-90.