基于改进A＊算法的灾后井下无人机航迹规划.pdf

第 44 卷 第 5 期 2018年 5 月 工矿自动化 Industry and Mine Automation Vol. 44 No. 5 May 2018 文章编号671-251X 201805-0085-06 DOI 10. 13272/j. issn. 1671-251x. 2017100058 基 于 改 进 算 法 的 灾 后 并 下 无 八 机 航 迹 规 划 吕文红1 夏双双2 魏博文2 殷立杰2 郭银景2 1.山东科技大学交通学院，山 东 青 岛 266590; 2.山东科技 电子通信与物 ，山 东 青 岛 266590 摘要 针对传统A*算法应用在煤矿灾后井下环境侦测的无人机航迹规划中存在搜索点冗余、 遇到突发 威胁时实时性较差等问题， 提出了 一种逆向变权重稀疏A*算法。根据无人机自身性能约束及灾后井下威胁 模型，从目标点到起始点进行全局静态航迹规划，避免大量无效搜索； 根据无人机执行任务的需要设置不同 权重系数， 得到侧重航程或安全的航迹； 通过引入次目标点策略， 仅对被突发威胁覆盖的航迹进行修正， 可在 短时间内有效避开突发威胁。仿真结果表明， 利用该算法进行航迹规划用时较短， 无人机受到的威胁较小， 可有效保障航迹规划的实时性和安全性。 关键词灾后井下环境侦测；无人机； 航迹规划； A*算法；逆向搜索；变权重；次目标点；突发威胁 中图分类号TD679 文献标志码A 网络出版地址 Ettp //kns. cnki. net/kcms/detail/32. 1627. TP. 20180418. 1527. 003. html Route planning of unmanned aerial vehicle in post-disaster underground based on improved A* algorithm LYU 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 perance when encountering sudden threat existed in application of traditional A* algorithm in route planning of unmanned aerial vehicle in environment detection for post-disaster underground, a reverse variable weight sparse A* algorithm was proposed. According to perance 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 perance and safety of route planning. 收稿日期2017-10-25;修回日期2018-04-04;责任编辑 盛男。 基金项目 国家自然科学基金资助项目“1471224 中国煤炭工业协会科学技术研究指导性计划项目（MTKJ2016-293。 作者筒 介 吕文红（1968 女 ， 山东青岛人， 教 授 ， 博 士 ， 主要研究方向为无人飞行器导航与控制、 无人机飞行管理信息系统规划与设计， E-mail klwh sdust. edu. cn。 引用格 式 格 式 吕文红， 夏双双， 魏博文， 等.基于改进A*算法的灾后井下无人机航迹规划[J].工矿自动化， 2018， 445 85-90. LYU Wenhong，XIA Shuangshuang， WEI Bowen， et al. Route planning of unmanned aerial vehicle in post-disaster underground based on improved A* algorithm[J]. Industry and Mine Automation,2018,445 85-90. 86 工 矿 自 动 化2 0 1 8 年 第 4 4 卷 Key words environment detection for post-disaster underground; unmanned aerial vehicle; route planning； A* algorithm； reverse search; variable weight; sub-target point; sudden threat 〇 引言 煤矿发生灾害后， 需要对灾害地点尽早实施救 援 ， 但由于灾害 不明确， 人 灾害现场容易 受伤， 所以可应用微小型旋翼无人机侦测井下环境 信息。 井下发生灾害时， 空间中充斥大量有毒可 体 ， 伴随爆炸产生高温、 有毒烟 [12]， 造 矿井间 ， 爆炸、 、 突发 严重影响 无人机飞行。因此， 精准高效的航迹自主规划能力 是无人机在灾后井 中安全完 的重要 。 目前常用的航迹规划算法有人工势场法[35]、 粒 子群优化算法M、 A*算法〜11]等 。人工势场法可用 于动态环境下的航迹规划， 实时性较好， 但存在局部 最优问题； 粒子群优化算法原理简单， 实现方便， 前 期搜索速度快， 但同样存在局部最优问题； A*算法 是一种经典的启发式搜索算法， 理论上可保证全局 最优， 但存在搜索点多、 耗时长的问题。针对A *算 法存在的问题， 文献[12]提出了结合约束条件的稀 疏 A*算 法 ， 通过去除大量不必要的搜索点， 提高算 法的搜索速度； 文献[13]提出了逆向A*算 法 ， 从目 标点向起始点搜索， 减少不必要的搜索， 提高算法效 。但上 述 2 种算法在无人机遇到突发 时 ， 都 需要重新规划航迹， 耗时较长。本文在文献[12-13] 的基础上， 提出了逆向变权重稀疏A *算 法 ， 利用该 算法规划无人机全局静态航迹， 并引人次目标点策 ， 能对突发 航 航迹修正， 具有较高的实 时 安全 。 1航迹规划建模 无人机航迹规划是指在满足飞行约束条件的情 况下， 规划从起始点到目标点的最小代价路径。约 束条件一般为无人机自身性能约束、 环境约束及突 发 约 。 1.1 无人机自身性能约束分析 针对井下灾害 ， 无人机自身性能约束条件 一般考虑以下方面 1最大/最小飞行高度。假设无人机最小飞 行高度为Hm, n， 最大飞行高度为HmC第 * * 1， 2, 为航段总数） 段航段的飞行高度为扰， 则需 约为 fS m ax SP n为起始点 置节点的综合威胁 概率;H n为高 价 。 9 式 中 “ n fn为当前位置节点到目标点的综合威胁 ;P S _ 为 航 P n fn为 置节 点到目标点的航迹长度;W1分别为Pnf n， Pnf n， H n的权重系数。 通过调节权重系数， 可实现侧重安全或航程的 航迹规划。 人机需要在短时间快速 时 ， 则设置， 这样规划的全 态航 重 P-n.n 3 Pr, n . n 6 3 -n .n 6 1 1 1 3 Pf Cn ，yn I 3 Pw C n ， .n C 6 m 1 1 式中 P n. n为 无 人 机 在 当 前 位 置 节 点 坐 标 n . n处受到的总威胁概率； Pr n . n为岩石 ; PBm n. n 为 爆 炸 胁 概 率 ; Pf n. n为 ； Pw /m in 由于突发威胁覆盖了部分全局静态航迹， 无 人机如果继续按全 态航 ， 将遭遇突发威 胁 ， 如 图 3 所示。此时需要对全 态航 在 线修正， 以规避突发 。为提高航迹修正速度， 提 出 目标点 ， 将距突发 最近的 航 88 工 矿 自 动 化2 0 1 8 年 第 4 4 卷 迹点作为次目标点， 引导算法向次目标点搜索， 舍弃 被突发威胁覆盖的航迹点， 对无人 置节点 目标点的航 修正， 再结合次目标点到目 标点的航迹即可获 置节点到目标点的航 。 计算无人 置节点之后各航迹点与 突发 中心的距离（ 式“ 0 来确 目标点， 即 当 第 1 次出现航迹点与突发威胁中心的距离4大 于 作用半径r 时 ， 该航迹点即为次目标点。 dT 槡xn J i x0 2.Ji y〇 26 zn J i z0 2 10 式 中 （ - 1 ， . 1 ， Zn 1 为次 目标点坐标 为 全局静态航迹上被突发 的航迹点数。 2. 3. 2安全距离计算 受到突发 时 ， 需要无人机修正航迹以快速 躲 ， 考虑修正需要时间， 定义安全飞行规划 Q safe Vt 6 11 式中Z 为无人机飞行速度 为修正航迹所需平均 时间为因风速等阻力和计算误差等产生的误差， 为方便有限 的实验 ， 本文以最小航 /m in的整数 替 。 2. 3. 3高度规划 发生突发 时修正航迹点与煤矿巷道顶部高 度的差为A H ， 每个航迹点对应范围内突发 的 最大高度为H ， 取无人机安全工作高度为AH/2,则 修正航迹点的高度为 H/2 H 。 3仿真分析 在操作系统为Window 7 、 主 频 为 2.30 GHz、 内存为4 G B 的计算机上， 采 用 Matlab软件进行仿 真 。假设无人机在500 mX 500 m 的工作区域执行 ， 煤矿井 采 用 图 1 构建的数字地图 ， 并随机生成突发 ⑭ 。设置无人机的起始 点坐标为S 34， 159， 目标点坐标为451， 444， 最大飞行高度Hmaj5 m， 最大航程Lma j2 km， 最 小航段长度/m in 3. 5 m， 权重系数W3 0 . 1， 各威胁 中心坐标、 威胁作用半径和高度等数据见表1。 在 不 同 权 重 系 数 和 吻 下 ， 对 稀 疏 A* 算 表1威胁中心坐标、 作用半径和高度数据 Table1 Center coordinate， action radius and height of threat 号类中心坐标 作用半径/cm i威胁高度/m ①岩石83,413501 . 3 ②火焰197,448650. 8 ③废墟 383,409 30 0 . 7 ④火焰 149,197 800. 1 ⑤火焰196,295600. 2 ⑥岩石 210,252 381.5 ⑦爆 炸 399,145 452.3 ⑧墟 168,128 459.0 ⑨岩石319 101563.0 ⑩爆 炸45 45531.3 ⑪爆 炸20025502.5 ⑫墟296447216.0 ⑬墟451346214.0 法 、 逆 向 算 法 及 本 文 提 出 的 逆 向 变 权 重 稀 疏 算 法进行比较， 基于不同算法的无人机航迹规划路径 如 图 4一图 6 所 示 （ 实线表示全 态航 ， 虚线表示遇到突发威胁⑭ 后修正的航迹） ， 基于 各算法的航迹规划时间、 航 见 2。 从 图 4一图 6 和 表 2 可看出， it 较大时， 利用 3 种算法规划的航 的 ， 无人机受 的 ， 但航 ， 航迹规划用 时 ， 表明规划的航 重保证无人机安全； 2 0 1 8 年 第 5 期吕 文 红 等 基 于 改 进A* 算 法 的 灾 后 井 下 无 人 机 航 迹 规 划 89 a [ 0 . 3 , w20. 6 b [0. 6 , w20. 3 图图4基于稀疏基于稀疏A*算法的无人机航迹规划路径*算法的无人机航迹规划路径 Route planning path of unmanned aerial vehicle based on sparse A* algorithm a [0. 3， w20. 6 b [0. 6， w20. 3 图图5基于逆向基于逆向A*算法的无人机航迹规划路径*算法的无人机航迹规划路径 Route planning path of unmanned aerial vehicle based on reverse A* algorithm a [0. 3， [20. 6b [0 6， w20. 3 图图6基于逆向变权重稀疏基于逆向变权重稀疏A*算法的无人机航迹规划路径*算法的无人机航迹规划路径 Fig. 6 Route planning path of unmanned aerial vehicle based on reverse variable weight sparse A* algorithm 较大时， 利 用 3 种算法规划的航迹长度较短， 航迹规 本文算法与其他2 种算法相比， 航迹规划用时和航 划用时 ， 但航 的 ， 无人机受 ， 且无人机受到的 ， 仅对 的 ， 表明规划的航 重航 ， 突发 的航 修正， 可 地 航迹 可保证无人 时间内完 ； 在 ， 规划的实时 安全性。 90 工 矿 自 动 化2 0 1 8 年 第 4 4 卷 表2不同权重系数下3种算法性能对比 Table 2 Perance comparison of three algorithms under different weight coefficients 航 规 划航 规 划航航总威胁总威胁 算法算法权重系数权重系数 时间时间/s 长度长度/m概率概率 [1 0. 3， ， 1 145 522 523.43700.1063 稀疏稀疏A* [ 2 0.6 算算 [1 0. 6， ， 2.112 260 540.54490.0673 [2 083 [1 0.3 0.581 553 546.39460.0817 逆向逆向A* [2 086 算算 [1 0.6 0.611 790 562.56790.0421 [2 083 逆向变权逆向变权 [1 0.3 086 0.050155161.39460.0357 [2 重稀疏重稀疏 A*算法*算法 [1 0.6 0.071309162.56790.0201 [2 083 4结语 针对煤矿井下灾害环境下无人机航迹规划问 题 ， 对A*算法进行改进， 提出了逆向变权重稀疏A* 算法， 并引人次目标点策略， 能在短时间内有效规划 全局静态航迹， 以及在保持原有不受威胁航迹不变 的基础上仅修正被突发 的航迹。 3吉果 表明， 基于该算法规划的航迹能有 开突发威胁， 且航迹规划用时及航 ， 可保证无人机在 时间内安全完 。 参 考 文 献 （参 考 文 献 （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“ 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“ 1120-1128. [6 ] SUN S， LI J. A two-swarm cooperative particle swarms optimization [ J ]. Swarm g Evolutionary Computation , 2014,152 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 l Flight path planning for mini rotor UAVs [C]//The 11th IEEE International Conference on Control g Automation， Taichung , 2014 1339-1344. [9 ] ELHALAWANY B M，ABDEL H M，TAGELDEEN A,et al. Modified A* algorithm for safer mobile robot navigation [ C ]//Proceedings of International Conference on Modelling , Identification g Control， Cairo，201374-78. [l〇 ]王帅.煤矿井下基于改进A*算法的移动机器人路径 规划[J].煤矿机械， 2008， 291165-67. WANG Shuai. Path planning of mobile robot based on advanced A* algorithm under coal mine[J]. Coal Mine Machinery , 2008,2911 65-67. [11] 黄文刚， 张怡， 姜文毅， 等.变步长稀疏A*算法的无人 机航路规划[J].计算机工程与应用， 2012,4829 206-2098 HUANG Wergrng，ZHANG Yi JIANG Wenyi，et l SAS algorithm with changeable steps for route planning of UAVs [ J ]. Computer Engineering and Applications ,2012,4829 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 g Electronics Systems， 2000， 363 869-878. [ 1 3 ] 李得伟， 韩宝明， 韩宇.一种逆向改进型A*路径搜索 算法[J].系统仿真学报， 2007， 1922 5175-5177. LI Dewei， HAN Baoming， HAN Yu. Conversely improved a star route search algorithm[J]. Journal of System Simulation , 2007,1922 5175-5177.