毕业设计论文 基于RRT的无人机实时避障算法实现 .doc

国防科学技术大学本科毕业论文本 科 毕 业 论 文 题目： 基于RRT的无人机实时避障算法实现 学员姓名： 王琳 专业： 仿真工程 培养类型： 工程技术类 学号： 200803012039 所属学院： 机电工程与自动化学院年级： 2008级 指导教员姓名： 牛轶峰 职称： 讲师 所属单位： 机电工程与自动化学院自动化研究所 国防科学技术大学训练部制ii目录目录I图表目录III摘要iAbstractii第一章 绪论11.1 课题研究背景21.2 国内外研究进展51.2.1无人机实时避障路径规划发展现状31.2.2无人机实时避障路径规划技术发展趋势31.3文章结构安排6第二章 无人机实时避障路径规划问题分析与建模72.1无人机实时避障问题分析72.2无人机实时避障问题建模92.3实时避障路径规划常用方法及其具体应用，优劣分析10第三章 基于改进RRT的无人机实时避障路径规划213.1 RRT基本原理及其应用213.2 结合问题特性的RRT223.3 基于RRT的避障路径规划373.4 改进算法的实时性37第四章 Matlab仿真实验验证434.1 实验方案4.2 算例测试第五章 结论致谢44参考文献45附录47图表目录图1.2.1 无人机自主空中防撞系统主要功能模块11图22 多种算法的特征提取流程12图3.1.1 RRT的节点扩展示意图14图3.3.1 改进的RRT节点扩展16图3.3.2 基于改进RRT的航迹规划算法描述图3.4.1 RRT算法，控制避障安全距离实验结果21图3.4.2 改进RRT算法，控制避障安全距离实验结果22图3.4.3 RRT算法，控制搜索步长实验结果24图3.4.4 改进 RRT算法，控制搜索步长实验结果25图3.4.5避碰安全距离为5，步长与扩展节点数的关系26图3.4.6避碰安全距离为5，步长与规划时间的关系27图3.4.7 搜索步长为5，避碰安全距离与扩展节点数的关系28-29图3.4.8 搜索步长为5，避碰安全距离与规划时间的关系29-30图4.1无人机预定航迹与威胁航迹31图4.2 无人机重规划的航迹与威胁航迹32-33图4.3无人机预定航迹、重规划的航迹与威胁航迹34-35 摘要在预定路径下，自主避障式的航迹规划是无人机执行区域飞行任务的有效保证。航迹规划算法是无人机关键技术之一，也是任务规划系统核心之一。本文阐述了无人机三维航路规划面临的关键性问题及发展趋势，针对非协助式的三维障碍规避问题，提出了一种基于快速随机扩展树（RRT）方法的三维路径规划算法。由于无人机避障的空间搜索范围大，三维航迹规划一直是航迹规划中的难点，而只有进行三维规划才能提高避障的实时性和灵活性。本文在RRT的基础上，通过改进随机扩展树的节点选择，引入启发式信息，结合考虑无人机的性能特点等方法降低了航迹规划的时间代价和扩展的节点数，取得了较好的效果。仿真测试结果表明，算法简单、快速、有效地规划出了三维可行航迹。关键词：实时避障；三维路径规划；快速随机扩展树（RRT）AbstractOn condition that the scheduled path given , Autonomous Route Planning is the prerequisite for Unmanned Aerial Vehicles (UAVs) to perform area search mission effectively . Path planning is essential of both UAVs and mission planning system . A conclusion of the key problems and the development trend of them are elaborated . With regard to the non-cooperant 3-D route planning , a modified (Rapidly-exploring Random Tree) RRT-based UAV route planning algorithm is proposed . It is very difficult to implement 3-D route planning of Unmanned Air Vehicle (UAV)because of the large searching space .Precise 3-D route planning can increase the real-time planning and flexibility . Based on RRT algorithm , the modified RRT method has introduced heuristic information , improved the extension of RRT node , and took UAVs properties into consideration , which can reduce the node number in search tree and time cost .It has been proved to be successful . Keywords: real-time planning , 3-D route planning , Rapidly-exploring Random Tree第一章 绪论1.1课题研究背景无人机（Unamanned Aerial Vehicle，UAV）是一种无人驾驶、动力驱动、可重复使用的飞行器。目前，无人机已广泛使用于情报侦察、通信中继、目标搜索与跟踪和对地攻击等各种作战任务中，并发挥了重大作用。无人机实时避障是无人机在感知周围静态与动态障碍的基础上，为避免发生碰撞，或者产生冲突事件，而进行实时规避机动的行为。在避开障碍之后，无人机重新恢复到安全状态，按预先规划的航线飞行，或者执行既定的作战任务。无人机飞行途中遇到的障碍可分为协作式障碍和非协作式障碍。协作式障碍是指可以通过信息共享来实现避碰和防撞。非协作式障碍是指无人机通过自身传感器自主检测到的障碍，并且无法通过信息共享来避开这类障碍。随着无人机系统技术的不断发展，众多功能各异的无人机被广泛应用于各种军事和非军事行动中。因此，对无人机实时感知并避开飞行途中的障碍和威胁的研究，作为无人机安全飞行的保障及实现其自主能力的关键技术之一，正引起各国重视。然而，现有的避障技术并不能满足无人机实现高等级自主控制的需求，迫切需要研究面向无人机的实时避障算法。针对未来无人机自主控制的发展以及无人机避障行为决策技术的需求，利用RRT算法实现无人机的实时避障路径规划问题是本课题的重点。1.2国内外研究现状1.2.1无人机实时避障路径规划的发展现状无人机感知规避系统可以划分为五个主要功能模块：察觉、检测、感知、生成逃逸机动，如图1.2.1所示图1.2.1 无人机自主空中防撞系统主要功能模块（1） 察觉功能。系统监视环境，并收集入侵者当前合适的状态信息，例如，飞机位置、速度、朝向等。（2） 检测功能。系统获取察觉数据，并处理得到有用信息，发现并管理无人机可能遇到的碰撞风险。（3） 感知功能。监视环境态势，在危急情况下取得飞机管控权，并在情况恢复正常时归还控制权。（4） 生成逃逸航线。探测到将可能发生冲突碰撞时，生成逃逸航线和实现逃逸机动的功能被激活，并决定采取什么行为如何机动规避。（5） 实现逃逸机动。考虑无人机平台的性能，完成规避机动行为。围绕第四和第五个功能模块，目前，国内外研究的避障路径规划问题主要算法大致分为五类，即预定义避障方法，基于协议的分布式避障方法，逃逸航线优化方法，势场方法，基于视觉的避障方法等。（1）预定义避障方法这类方法基于一套固定的预定义规则实施避障，优点是避障反应时间短，缺点是规划处的逃逸机动动作并不很有效，也不够优化。当出现未预料到的障碍时，很难改变已经下指令机动的动作。（2）基于协议的分布式避障方法这类方法很好的解决了多无人机编队的避碰航行问题。多机间共享位置、速度、航向等信息，基于一套事先定好的规则进行决策。这类方法是分布式的，能保证飞行安全，但是可能生成不必要的较长航线，从而延长完成任务的时间（3）逃逸航线优化方法这类方法将避障问题建模为优化问题，从而各种优化方法可用于解决此类问题。这类方法将无人机机的运动学模型与一套约束条件相结合，然后基于优化约束计算出最优的策略。防撞系统并不是试图给出一条逃逸航线，相反，而是请求一个爬升或俯冲机动。这种思想意味着，系统知道不考虑威胁的路径不安全，但它只在发现障碍威胁时才寻找一条避障航线，否则就一直向着目的地飞行。此外，还有其他一些优化方法，例如遗传算法，专家系统，模糊控制等，都更为复杂，需要大量的规则来覆盖所有可能的情形。因此，可能导致需要强大的计算处理能力。（4）势场算法势场算法最早由Khatib提出，这类方法在飞行器之间建立势场，将飞行器看做一个带电粒子，利用飞行器间的斥力生成逃逸航线。这类方法也被视为一类路径规划算法，通过对先前的路径进行滤波来生成新的航线。但这类方法可能使机动动作在执行过程中突然中断，而且需要极高精度的飞行导航，导致增加了问题的复杂性。（5）基于视觉的避障方法这类方法主要基于图像特征和光流分布等实现障碍规避。Brandon采用图像特征跟踪方法研究小型无人机的避障问题。他首先利用遗传算法生成一条可以避开所有已知障碍的航路，然后在无人机飞行过程中，利用机载前视摄像机检测障碍。通过图像的三维成像粗略估计障碍位置，利用滑模控制原理控制飞行器避开障碍。研究结果显示，避障成功率达到96%。国外？国内西北工业大学王振华等人进行了基于改进概率地图的无人机实时避障研究。针对无人机路径规划问题，以等高线地图作为任务空间，提出了一种新的采样模型，在该模型框架下，依据适当的规则构造临近点集，便可以避免相交检验，提高了路径规划速度。姚远等人针对不同属性的障碍物所构成的威胁分布模型，提出了一种基于稀疏A*搜索算法预规划和改进人工势场相结合的无人机动态避障算法。仿真结果表明，该方法能够规划出给定威胁指标下的全局最优路径并达到良好的动态规避性能。李耀军等人基于多源信息融合对无人机感知与规避做了深入研究。通过多传感器信息融合技术结算并共享无人机与有人机的坐标和实时运动状态，利用多模态图像的优势互补，实现昼夜、远距离、高分辨率的目标检测、识别与跟踪，提高无人机感知与规避的实时性与可靠性。除此之外，周炜等人提出了基于层次分解策略无人机编队避障的方法。张跃东等人提出了基于单目视觉的无人机障碍探测算法。王一凡等人提出了一种用于小型无人机避障的快速视差测距方法。宗玲蓓等人提出了基于MAS的无人机编队飞行智能优化控制。1.2.2 无人机实时避障路径规划技术的发展趋势针对无人机实时避障路径规划问题，主要在两方面开展研究，一方面，研发适用于无人机的新型避障系统，提高无人机自主感知能力；另一方面，基于不同类型的传感器，对规避问题进行数学建模，深入研究问题和关键技术，实现效率较高的规避控制算法。另外，基于各类先进的无人机飞行平台，国外的研究已经进行了一些实验验证，而不仅仅停留在仿真阶段。国内研究无人机感知与规避问题并没有完全满足无人机进入民用空域飞行的需求，关键在于无人机自主决策的智能性仍受到质疑。随着人工智能、机器视觉、机器人、传感器技术的发展，无人机感知与规避问题有望得到更好的解决，从而为大幅度提升无人机自主控制能力奠定基础。（1）对非协作式环境中无源探测技术的研究非协作式感知问题是最具挑战性，也是各研究机构关注的热点。对于非协作式机动障碍，无人机很难预测其飞行路线，不便做出合理的规避策略，甚至在复杂的环境中，还可能会引起多次冲突，对有人机而言，有人机传感器配合人眼能很好的探测发现威胁并准确感知。但是有人机上装备的有源探测感知设备无法安装在中小型无人机上，且无源探测传感器容易受天气条件的影响，不能直接提供距离、速度等信息，并且在执行地面探测任务时难以兼顾感知规避。另外，对于体积小、重量轻和动力差的无人机系统，也可以尝试使用声学传感器。对于大型飞机，也可以尝试用其辅助增强光电传感器，尤其在飞机离开或接近机场时，因为那时飞机通常飞行较低，发生碰撞的可能性也最大。（2）基于多传感器信息融合的自主探测感知方法逐渐得到应用传统的无人机探测更多的依赖于外部数据链为其提供信息，依靠地面操作员对无人机传感器获得的信息进行分析并作出决策。然而，数据链传输存在两个问题：其一，传输需要时间。无人机从探测到障碍物到作出决策并实时机动规避，其间留有的时间是很短暂的。通过数据链传输信息到操纵员做出决策这样一个大回路来控制无人机已不能满足实时性要求，很可能使无人机错过了避障的时机，导致与障碍相撞。其二，数据链容易受干扰。数据链传输存在不稳定和易受干扰性，一旦数据链断开，无人机就失去了感知能力，将严重威胁无人机和其它飞行器的飞行安全。因此需要发展自主探测并感知的技术，不仅要开发新型的雷达探测感知传感器，还要研究实时性好、自主程度高的信息融合与知识挖掘技术，使无人机在没有数据链支持的情况下，也能很好的感知识别周围的环境态势。（3）信息不一致条件下的多机航线避碰协调对于无人机自主防撞系统，维持多架无人机的信息一致和状态一致是实现分布式协同的一个重要基础。近年来，分布式多平台一致性问题成为协同控制领域的一个研究热点。针对无人机实现稳定的编队构型、对无人机高度一致、速度一致、时间一致等问题，国内外提出了很多一致性协议，并展开了大量理论研究工作。但对于动态网络条件下的信息一致性问题，如何借鉴现有一致性理论研究的成果，在信息不一致的条件下实现多无人机一致决策将是未来一个重要的研究方向，并将为无人机避开协作式障碍奠定基础。（4）规避行为决策由简单反射向智能推理型发展随着机器人智能体智能水平的提高，之前的预定义方法将得到改善，从简单的反射型发展为高级的推理型。由于不可能事先考虑到所有可能出现的情况，简单的反射型避障无法满足复杂多变的战场环境，而基于推理的避障将使无人机在遭遇障碍时，能够实时的做出合理的决策，从而具有更高的智能性和更好的战场适应性。在这方面，智能控制领域也有很多成熟的理论方法，为行为决策由简单反射型向智能推理型发展提供了方法。（5）面向无人机避障的实时快速路径规划感知规避技术强调在出现预料到的障碍和威胁时进行实时重规划，从而无人机探测到威胁到做出机动动作只有10秒左右的时间，对路径规划实时性和正确性要求都非常高。往往难点在于估计非协作式机动障碍物的轨迹，避免做出与之相反的规避策略，造成二次冲突。很多用于预先航迹规划的优化计算方法，如A*算法、遗传算法、蚁群算法等，由于需要不断迭代优化，其计算过程十分耗时，很难直接应用，必须结合无人机避碰问题特点，研究快速在线航迹规划方法。另外，尽管无人机能承受的机动过载较有人机大得多，但轨迹还必须考虑无人机的物理性能，以确保无人机能执行所规划机动动作。1.3文章结构安排第一章 绪论：本章明确论文选题的背景、意义以及目的，介绍无人机实时避障路径规划的发展现状，无人机实时避障路径规划技术的发展趋势。第二章 无人机实时避障路径规划问题分析与建模：本章将简述无人机实时避障问题的分析和建模，并对实时避障路径规划常用方法及其具体应用进行优劣分析。第三章 基于改进RRT的无人机实时避障路径规划：本章众多研究RRT算法的原理特点，基于RRT的避障路径规划。第四章 Matlab实验对RRT算法的规避策略进行仿真验证：本章在Matlab平台进行实验，仿真RRT算法规避策略，验证其有效性。第五章 结论：本章是对全文工作的总结以及对未来工作的展望。第二章无人机实时避障路径规划问题分析与建模2.1无人机实时避障问题分析无人机实时避障问题，实际是一个复杂的多目标优化过程，即要求找到一条从当前位置到期望目标位置的最优可飞航路，同时满足一定的约束条件。它涉及飞行力学、自动控制、作战效能分析、人工智能、运筹学、计算机和图像处理等多个学科和专业，是个综合性的研究课题。规划出的飞行路径应该使无人机能避开威胁，同时尽量缩短航程，并满足机动性能以及无人机的最大加速度、俯仰角、偏航角等约束要求。为了减小搜索空间，提高算法的收敛速度，大多数情况下人们往往只考虑二维平面内的路径规划搜索，将水平面和垂直平面的规划问题分开考虑，但是这么做很难有效地规避威胁得到最优飞行路径，因此，必须考虑真正意义上的三维实时避障路径规划，并要求实时性和有效性，以适应现代无人机执行任务的快速反应。影响实时避障路径规划的因素主要有以下三方面：预定路径价值衡量无人机尽快回到预定路径必要程度的综合指标，用表示。威胁代价威胁代价主要与其威胁范围s相关，威胁代价可表示为。油耗代价由于无人机在地面一次所加的油量是有限的，因此它的航路必然受到油耗的限制，且由于无线电的作用距离受限，无人机执行任务的位置不能超过其作战半径，因此必须考虑油耗代价。2.2无人机实时避障路径规划问题建模无人机多数情况下执行特定的侦察监视飞行任务，若无特殊情况则按预定路径飞行，若在探测范围内发现威胁则在以当前点为起点重新规划飞行路径，并对威胁实时探测，在保证不与威胁相撞的前提下，规划可行飞行路径回到预定路径上继续执行任务，目标函数可表示为式中：为预定路径价值；为航路代价。对于航路代价而言，根据航路规划的影响因素分析，它包括威胁代价和油耗代价。威胁代价与油耗代价按一定权重进行求和，威胁代价权重为，油耗代价权重为，且。则航路代价为通过上述分析，得航路的目标函数可表示为:无人机实际飞行过程中还需要考虑动力学约束，主要包括以下几个方面：（1）无人机的最小飞行高度。记每一段航路飞行高度为，则应满足：，(i=1，n)不同于二维实时避障路径规划仅考虑水平方向航路规划，三维实时避障路径规划必需考虑高度影响。（2）无人机的最大航程。无人机在整个飞行过程中的飞行路程，受到飞机燃油和飞行时间配给的限制。记最大航路长度为，则每一个航段距离应满足。（3）无人机的最大俯仰角。无人机在三维航路规划时爬升俯冲飞行时必须考虑俯仰角的约束，具体可写为：其中表示在x-y平面与原点的距离。（4）无人机的最大偏航角。无人机在飞行过程中在二维平面中的左右偏转角度受其最大偏航角约束，具体可写为：（5）无人机的最远视距F。这项参数决定了无人机在按预定航迹飞行时发现未知威胁的最远距离。若该无人机的最远视距大，则其路径规划的时间就较为充裕，若最远视距较小，则对其机动能力有较高要求，防止在发现未知威胁后还未来得及规划路径就与威胁相撞。（6）无人机的最小步长。最小步长决定了无人机在改变飞行姿态前必须直飞的最短距离。2.3实时避障路径规划常用方法及其具体应用，优劣分析在搜索智能领域中，无人机的实时路径规划问题受到了相当的重视。当具备完整精确的环境信息或是已知预定飞行路径节点时，可用一次性的全局规划来得到一条自起点到终点的最优飞行路径。但实际上，在现实的环境中，会有许多未知的威胁出现，其中包括高空的静态威胁以及动态飞行器等威胁，只有当无人机飞到距离它们在探测距离内时才能发现，这时地面一次性全局规划已不能满足要求。在这种情况下，无人机需要在飞行的过程中实时处理新出现的威胁，依靠实时探测到的环境及威胁信息，经过重规划来获得当前已知信息下的最优飞行航迹。重规划要考虑的主要问题是航迹的最优性以及规划的实时性。在过去的十年中，人们提出了一些算法已经成功应用于解决高维空间的路径规划问题，比如随机势力场法和概率地图法。随机势力场依赖于一个好的启发式函数，当遇到威胁、运动学上的差异约束或是动力约束时，它就会变得困难起来。在概率地图法中，我们通过产生随机形状，连接不同形状产生路径的方法。规划完整系统时，局部步长可能是高效的。然而，在在非完整约束的情况下，连接问题与设计一个非线性控制问题一样麻烦。概率地图法中可能需要连接许多的节点或是状态来寻找解决方案，如果每个连接点都类似于一个非线性控制问题，这样路径规划的实时性是很难实现的。近年来，许多学者在三维航路规划方面作了大量的工作，主要研究如下：海军航空工程学院范洪达将三维航路分解为水平方向和垂直方向的两个航路并分别应用改进的遗传算法和适应角法进行规划仿真。空军工程大学李锐通过引入状态空间节点的概念生成三维航路搜索空间，将原来的航路规划问题简化为在状态空间中的搜索寻优问题。陈琳采用粒子群优化(PSO)算法进行无人机的三维航路规划，以最大水平转弯角、最大爬升下滑角、最小步长、最小离地高度和最短距离作为适应度函数的评价指标。北京航空航天大学洪晔为解决算法时空开销大、无人机航向改变频繁的缺点，提出一种基于状态聚类方法的HMDP模型，并将其拓展到三维规划中。夏洁提出一种基于启发式搜索和禁忌搜索技术的任务路径规划问题的有效算法，通过对不同重要程度的任务进行分层调度，得到较为满意的决策结果，该算法具有搜索宅间小、求解速度快的优点。南京航空航天大学陈谋研究了一种基于改进蚁群算法的无人机三维航路规划方法，将最短路径的信息反馈到系统中作为搜索的指导信号，并改进节点选择方法，以提高应用蚁群算法搜索无人机三维航路的效率。鲍帆针对无人机的快速航路规划展开研究，结合无人机在飞行过程中的约束条件，提出动态权值的三维LPA*的航路规划方法对二维航线产生的航线点之间进行规划，满足地形回避和威胁回避，当数字地图改变时，LPA*算法可通过检测局部一致性来避免进行重新的全局搜索，缩减了搜索空问，从而可以快速地修正航路。西北工业大学陈冬利用粒子群优化算法，将约束条件和搜索算法相结合，从而有效减小搜索空间，得到一条全局最优路径。华中科技大学严江江针对三维航路规划的实时性问题，不同于通常的最优优先算法，该方法使用可行优先的准则，有效地剪除了搜索空间，提高了搜索效率，从而使三维航路规划能够应用于实时航路规划中。综上所述，无人机三维航路规划的优化算法主要可归纳为以下几类：智能化方法。包括粒子群优化算法、遗传算法、蚁群算法、禁忌搜索及启发式搜索等。几何方法。如Voronoi图表等结合一些图论搜索方法如Dijkstm法快速寻找最优路径。人工势场法等。其中，遗传算法收敛速度过慢，且容易陷入局部最优，可以和模拟退火算法结合使用，但仍不能真正解决上述缺点；启发式搜索A+算法是用于路径搜索和规划的经典方法，但它在搜索过程中暴露的不足也很明显；人工势场法一般结合Voronoi图或遗传算法进行优化，存在的问题是算法收敛时间太长，一般用做完成航路规划的后期处理。以上方法的基本思路是通过降维减少搜索空间，提高搜索效率，其实质仍然是二维航路规划，但是这么做很难同时考虑威胁回避、航迹的最优性以及规划的实时性。随着信息化战争进程的发展及科技的日益创新，防空体系日益完善，雷达及导弹性能更加优越，事先很难完成探测和定位，因此，三维低空突防航路规划及实时动态规划具有很强的实战意义，但无人机航路规划尚存在以下问题：（1） 航路系统约束过于简化。目前对无人机动力学约束考虑过于简单，造成由此制定的航路往往受动力学约束而实际不可飞；此外，对威胁场空间降维处理至二维空间，由此规划的航路往往难以投入实际运用，尤其对于必须进行低空突防的航路规划。（2） 时效性要求。对于未确定战场环境，需要实时规划或局部规划，但目前存在的问题是计算时间过长而达不到及时规避威胁的要求或模型考虑过于简单，导致实际飞行的威胁大于规划的威胁，因此规划出的航路实际不可飞。综上两点，未来在无人机航路规划的发展方向及研究重点主要有：（1） 低空突防三维航路规划。不是简单的降维，而是真正可用的三维航路规划技术，同时兼顾无人机动力学约束及威胁环境约束，并保证算法的时效性，以适应无人机低空突防的要求。（2） 实时航路规划。针对未确定威胁场，无人机必须进行在线航路规划，以规避风险，实现在未确定环境下，为有效利用各种信息，对战场态势进行评估，并对多传感信息进行数据融合，最终实现自主航路规划。本文采用一种路径规划的随机数据结构以解决航迹的最优性以及规划的实时性问题。这就引出了我们将在第三章中介绍的快速扩展随机树（RRT）算法。RRT算法最独特的优点，在于它可以直接用于非完整约束的系统规划，适合于解决包含几何约束和动力学约束的实时避障路径规划问题。第三章 基于改进RRT的无人机实时避障路径规划31 RRT基本原理及其应用快速扩展随机树（rapidly-exploring random tree，RRT）是一种基于采样的单查询随机搜索算法，1998年由S.M. LaValle 首次提出。该算法能够根据当前环境快速有效地搜索高维空间，通过组态空间的随机采样点，将搜索导向空白区域，适合于解决包含集合约束和动力学约束的航迹问题，目前已应用于飞行器运动规划和移动机器人路径规划。RRT方法具备如下特性：（1） RRT的节点扩展不需要再规划前执行预处理，可以直接根据当前的环境状况快速有效地搜索规划空间；（2） RRT实际上是对空间的一种采样，且RRT树结构中叶节点的分布逼近于采样分布；（3） RRT算法在概率意义上市完全的，如果规划空间中存在一条可行路径，RRT就一定可以找到相应的路径；（4） 相对于其他基于采样的方法，例如概率路标图（PRM）方法，RRT方法的计算更加简单；（5） 由于RRT是树结构，即使是在边最少的情况下，其节点始终是连通的，而PRM等随机采样方法则无法保证节点的连通特性；（6） RRT方法可以与路径规划、航迹规划问题紧密结合起来，其最显著的特性是可以与系统动态特性相结合，并在执行过程中动态修改参数，因此，它能更快地适应外界的动态变化，非常适合于解决包含几何约束和动力学约束的运动规划问题。RRT采用一种特殊的增量方式进行构造，从而能够逐步地迅速缩短随机状态点与期望状态点的距离，其节点示意图如图3.1.1所示，基本的RRT算法描述如下：图3.1.1 RRT的节点扩展示意图在算法实现时，需要结合具体应用建立以下三个关键函数：（1） Extend函数。与无人机动力学特性紧密相关，用于计算从某点以一定的搜索步长达到的新点。注意到新点一般是朝目标点靠近的，如果算法在向新点行进的过程中遇到威胁，则返回空值NULL，表示扩展失败。（2） NearestNeighbour函数。用于计算当前搜索树T内，距离目标点最近的点，该过程需要把已有的搜索树进行遍历，然后基于代价函数Distance计算达到目标点的距离估计，从而获得RRT中距离目标点最近的点。（3） RandomState函数。用于获取随机点，具体实现时可以采用不同的随机采样策略。由RRT算法描述可以看出，该算法的主要特点在于通过伸向随机目标节点的树枝在线构造出可行航迹树，在尽可能小的空间中进行有效地搜索，同时省去了路标图的构造与更新过程，这使得RRT方法具有良好的时间特性，计算量小，能在较短时间内获得可行航迹。基本RRT方法在搜索过程中并没有考虑航迹的综合代价，并且其目标节点选择的任意性使得扩展树的生长形状具有很大的随机性，这导致规划出来的航迹也具有随机性，对同一条件下的规划过程缺乏可重复性，航迹的性能往往是不可控的。换句话说，基本RRT方法随机性太强，只能够保证高效快速的获得可行航迹，无法获得较优的航迹。所以需要改进随机点的选择方式，引入航迹评价启发信息，剪裁冗余节点，对航路进行平滑等，提高规划的性能。另一方面，RRT算法可以和几何约束和动力约束相结合，但要基于飞行性能模型的无人机快速航线重规划，还需要深入研究RRT算法和飞行性能模型结合的机理和改进措施，从而获得更为精准的避障航线。3.2 结合问题特性的RRT本文将具体研究无人机按预定航路飞行，在最远视距范围内探测到未知动态威胁，立即作出实时避障路径规划，得到新的航路点，并尽快回到预定航路上。以探测到威胁时刻的那个航路点作为初始点，在RRT树中将它作为根节点，采用一种特殊的增量方式构造搜索空间，通过逐渐增加叶节点的方式生成随机扩展树。首先产生随机点集，从中得到目标点，选取目标点时一要考虑无人机最大俯仰角、偏航角以及最大步长等性能约束，二要考虑与预定航路的距离，确保尽快回到预定航路上，具体表示如下：式中i为当前节点序号。然后从当前RRT树节点中寻找距离最近的叶节点求出满足最小的i的航路点即为，并以一定的步长， 为满足无人机最大步长约束的距离常量，计算新点式中为产生01随机数的函数。如果在向新点行进的过程中探测到属于威胁范围内，则返回EmptyState，并重新选择继续计算。3.3基于改进RRT的避障路径规划RRT算法的随机性导致扩展树的生长形状具有随机性，并且对同一条件下的规划缺乏可重复性，这样计算出来的航路往往不是较优的航路。为了将RRT算法用于无人机实时避障航迹规划上，本文提出以下改进策略：（1）改进随机点的选择。如图3.3.1所示，在RRT扩展树生长时，从未搜索的区域内产生一组随机点，以为目标点，分别计算临时节点。为了使规划出来的航迹满足无人机可飞性约束，需要基于当前位置和飞行航向，根据无人机最大偏航角和最小直飞距离的限制条件来计算。对于超出无人机可达范围内的目标点，则引入随机变量,通过航向和步长计算。图3.3.1 改进的RRT节点扩展(2)引入启发信息。在新增叶节点时，引入启发式信息，通过计算每个节点到目标点的估计距离来选择新增叶节点。计算出随机节点和临时节点后，根据和所对应搜索图上的到目标点的距离，选择距离目标点最近的临时节点作为最优节点加入扩展树。这样的启发信息削弱了新增叶节点的随机性，从而使得规划出来的航迹接近最优搜索航迹。其中，估计距离为到目标点的欧式距离，如式(4)所示。 （4）根据上述策略，基于改进RRT的航迹规划算法描述如下：Step l 以当前无人机的位置作为初始节点，初始化搜索树结构只包含一个节点。Step 2按照以下步骤扩展搜索树：(1)在未搜索区域范围内产生一组位于威胁区域外的随机点；对于每一个，kl，2，N，从当前搜索树的叶节点中选出距离目标点最近的节点，并按照约束条件计算；(2)根据搜索图计算到的估计距离，从，k1，2，N中选择距离目标最近的节点作为；(3)如果不在威胁范围内，则将加入搜索树；否则返回Step 2；(4)如果 ，则搜索到目标点，跳到Step 3；否则返回Step 2；(5)如果搜索树的节点数超过阈值MaxNum，则强制结束扩展，跳到Step 3；Step 3返回形成的扩展搜索树，获得从到的路径。如果是强制结束，则返回从到距离最近的叶节点的路径。图3.3.2 基于改进RRT的航迹规划算法描述3.4改进实时性为改进本文提出避障路径规划实时性的要求，采用VC6.0编程实现并改进算法。由RRT算法可知，影响算法时间性能的重要参数为扩展步长和避碰安全距离，这些参数觉得了扩展随机数在空间中的节点分布和生长方向，从而影响航迹规划时间。为改进算法的时间特性，在不同参数条件下运行多次仿真，对基本RRT算法和改进RRT算法的规划时间和扩展节点数要进行测试和比较，实验结果如下：图3.4.1 RRT算法，控制避障安全距离实验结果图3.4.2 改进RRT算法，控制避障安全距离实验结果图3.4.3 RRT算法，控制搜索步长实验结果图3.4.4 改进 RRT算法，控制搜索步长实验结果比较RRT算法与改进实时性后的RRT算法，采用控制变量法，控制搜索步长、避障安全距离其中一个量不变，另一个量变化，观察RRT算法和改进RRT算法得到的扩展节点数和规划时间的变化。为了更方便直观地反映改进RRT算法与基本RRT算法在实时性方面的改进，作了如下图进行比较：图3.4.5避碰安全距离为5，步长与扩展节点数的关系 图3.4.6避碰安全距离为5，步长与规划时间（ms）的关系图3.4.7 搜索步长为5，避碰安全距离与扩展节点数的关系 图3.4.8 搜索步长为5，避碰安全距离与规划时间（ms）的关系图3.4.1和图3.4.2分别显示了避碰安全距离为5的条件下，步长对扩展节点数和规划时间的影响；图3.4.3和图3.4.4分别显示了在步长为8的条件下，避碰安全距离对扩展节点数和规划时间的影响。从实验结果看出：避碰的安全距离越大，算法扩展的节点数也越多，相应的计算时间也越多；扩展步长很小的情况下，为了到达目标点，所需要产生的节点数也很多，当然规划时间也越长。通过比较，在相同条件下，本文提出的改进RRT算法由于在扩展随机数时受到启发函数的引导，在效率上有了明显的提高，完全可以满足无人机实时避障路径规划。第四章Matlab实验验证及结果分析4.1实验方案利用Matlab的三维平台显示无人机在探测到威胁后做出的实时避障路径规划，比较无人机的预定航迹与重规划航迹之间的区别，直观地反应出算法为避障做出的重规划航迹是否可行。在VC6.0平台上已经实现了基于RRT算法的避障路径规划，我们得到了重规划后的航路点，保存到path.txt文件中，Matlab将其与威胁路径thr_path.txt，预定路径pre_path.txt一起读入。根据得到的航路点用最小二乘法拟合出航迹曲线。4.2算例测试图4.1显示的是无人机预定航迹与威胁航迹之间的关系，其中蓝色曲线为无人机预定航迹，红色曲线为威胁航迹，图中威胁航迹上的球是威胁与预定航迹的最近距离，球半径为最小避碰安全距离，可见预定航迹与威胁航迹发生冲突。图4.2显示的是无人机重规划后的航迹与威胁航迹之间的关系。其中绿色曲线为无人机探测到威胁后做出规避得到的新航迹，红色曲线仍为威胁航迹。可以看到图中重规划得到的航迹与威胁球没有接触，也就是说该航迹是可行的。图4.4显示的是图4.3无人机预定航迹、重规划的航迹与威胁航迹之间的关系。其中蓝色曲线为预定航迹，绿色曲线为重规划后的航迹，红色曲线为威胁航迹。可以看到重规划航迹在避障后回到了预定航迹上。图4.1无人机预定航迹与威胁航迹图4.2 无人机重规划的航迹与威胁航迹图4.3无人机预定航迹、重规划的航迹与威胁航迹 第五章结 论本文针对探测到威胁后的无人机实时避障航迹规划问题，在比较多种规划算法的基础上，提出了一种基于RRT算法的无人机自主航迹规划方法。首先，我们对国内外近年来对无人机避障方法的研究进行了一个整理，主要分为五类：预定义避障方法，基于协议的分布式避障方法，逃逸航线优化方法，势场方法，基于视觉的避障方法等。无人机实时避障路径规划技术的发展趋势有以下五方面：对非协作式环境中无源探测技术的研究，基于多传感器信息融合的自主探测感知方法逐渐得到应用，信息不一致条件下的多机航线避碰协调，规避行为决策由简单反射向智能推理型发展，面向无人机避障的实时快速路径规划。然后，我们对几种常用的路径规划算法进行了一个总结与比较。其中主要有随机势力场法和概率地图法。随机势力场依赖于一个好的启发式函数，当遇到威胁、运动学上的差异约束或是动力约束时，它就会变得困难起来。概率地图法在规划非完整约束系统时，效率低下。于是，我们提出了基于RRT的实时避障航迹规划算法。接着对基本RRT算法的原理及特性进行了介绍，并在基本RRT算法的基础上，改进了随机扩展树的节点选择方式，在随机树扩展过程中引入启发式信息，有效改善了算法的实时性，并在VC6.0平台上实现了算法，对基本RRT和改进后的算法进行了直观的实验比较。之后，我们在Matlab平台对避障规划的路径进行了三维显示，仿真试验的结果表明：改进后的RRT航迹规划算法，使得无人机在探测到威胁后能够快速有效地重规划飞行路径，并在较短的时间内回到预定航迹上继续完成任务。综上所述，通过比较，本文提出的基于RRT的实时避障路径规划算法由于在扩展随机树时受到启发函数的引导，在效率上有了明显的提高，完全可以满足无人机实时避障路径规划。致谢本次毕业设计首先要感谢牛轶峰老师，他为我完成这次毕业设计提供了大量的书籍和论文资料，不仅使我从理论上对于所做工作有了深入了解，还让我在算法的实现上节约了大量时间。在整个毕业设计的过程中，牛老师一直关心我毕业设计的完成情况，对于我如何安排时间以及需要哪些相关知识都予以了指导，为我提供了必要的帮助。然后还要衷心感谢雷鑫学姐。雷鑫学姐在我进行毕业设计的初期为我提供了算法核心思想的介绍以及相关的论文资料，使得我的毕业设计可以快速展开。在毕业设计的过程中，学姐的帮助和提点使我在算法设计过程少走很多弯路。在毕业设计的最后这段时间，学姐一直督促我抓紧算法的完善以及论文的撰写。在这里对她无私关心表示深深的感谢。最后，感谢在毕业设计过程中给予我帮助的所有人，是大家的热情帮助，让我的毕业设计能够完成，这份友谊我将铭记于心。参考文献（还未全面）区域目标搜索中基于改进RRT的无人机实时航迹规划。彭辉，王林，沈林成8范洪达，叶文，马向玲面向低空突防的三维航路规划仿真研究J计算机仿真，2004，21(2)：35-379李锐，刘占辰，荆献勇基于启发式算法的无人机三维航迹规划仿真研究J电光与控制，2009，16(8)：27-3110洪晔，房建成基于HMDP的无人机三维路径规划J北京航空航天大学学报，2009，35(1)：10010311夏洁，高金源，余舟毅基于禁忌搜索的启发式任务路径规划算法J控制与决策，2002，17(11)：773-77612陈谋，肖健，姜长生基于改进蚁群算法的无人机三维航路规划J吉林大学学报(工学)，2008，38(4)：99l-991513鲍帆，姜长生基于LPA+算法的无人机三维航迹快速规划研究J电光与控制，2008，15(12)：101314陈冬，周德云，冯琦基于粒子群优化算法的无人机航迹规划J弹箭与制导学报，2007，27(4)：34034215严江江，丁明跃，周成平，等一种基于可行优先的三维航迹规划方法J宇航学报，2009，30(1)：139-144附录附录（一）基于RRT的实时避障路径算法程序#include #include #include #include #include #define pi 3.1415926#define safe_dis 6#define con 5#define step 5#define fy (70.0/180.0)*pi #define ph (70.0/180.0)*pifloat PreThrDistance;struct path_typefloat lon;float lat;float alt;pre_path100,thr_path100,save100,*pp,*qq;float random()float a,r;srand(int)time(0); /设置随机数种子，未调用srand函数时种子的值默认为一，随机数只产生一次，以后的都一样a=rand()%100;return(a);float computer_distance(float x1,float y1,float z1,float x2,float y2,float z2)float dis;dis=(x1-x2)*(x1-x2)+(y1-y2)*(y1-y2)+(z1-z2)*(z1-z2);dis=sqrt(dis);return(dis);void init()FILE *fp1,*fp2;pp = pre_path;qq = thr_path;if(fp1=fopen(pre_path.txt,rb)=NULL)printf(cannot openn);exit(0);if(fp2=fopen(thr_path.txt,rb)=NULL)printf(cannot openn);exit(0);for(int k=0 ; klon,&pp-lat,&pp-alt);/printf(%f %f %f,pre_pathk.lon,pre_pathk.lat,pre_pathk.alt);fscanf(fp2,%f %f %fn,&qq-lon,&qq-lat,&qq-alt);fclose(fp1);fclose(fp2);float computer_distance(float x1,float y1,float z1,float x2,float y2,float z2)float dis;dis=(x1-x2)*(x1-x2)+(y1-y2)*(y1-y2)+(z1-z2)*(z1-z2);dis=sqrt(dis);return(dis);void init()FILE *fp1,*fp2;pp = pre_path;qq = thr_path;if(fp1=fopen(pre_path.txt,rb)=NULL)printf(cannot openn);exit(0);if(fp2=fopen(thr_path.txt,rb)=NULL)printf(cannot openn);exit(0);void rrt(int num)float r;float xrand10,yrand10,zrand10;float rrtx50=0,rrty50=0,rrtz50=0;float anglex11=0,anglez11=0;float xtemp10,ytemp10,ztemp10;float d50;for(int a=0;a50;a+)da=50;rrtx0 = pre_pathnum-1.lon;rrty0 = pre_pathnum-1.lat;rrtz0 = pre_pathnum-1.alt;float endx = pre_pathnum+5.lon;float endy = pre_pathnum+5.lat;float endz = pre_pathnum+5.alt;for(int j=0 ; j8)/产生10个随机点为目标点if(rrtxm endx)for(j=0 ; j= endx)for(j=0 ; j10 ; j+)r=random();r = sqrt(r-j)/10.0);xrandj=rrtxm-2*r*con;if(rrtym endy)for(j=0 ; j= endy)for(j=0 ; j10 ; j+)r=random();r = sqrt(r-j)/10.0);yrandj=rrtym-2*r*con;if(rrtzm endz)for(j=0 ; j= endz)for(j=0 ; j= endx)for(j=0 ; j10 ; j+)r=random();r = sqrt(r-j)/10.0);xrandj=rrtxm-2*r*con;if(rrtym endy)for(j=0 ; j= endy)for(j=0 ; j10 ; j+)r=random();r = sqrt(r-j)/10.0);yrandj=rrtym-2*r*con;/找当前RRT树（m个点）中离随机点最近的节点float min1 = 1000;float det_anglex,det_anglez;float min_step,min_dis=10000;/大循环for(j=0 ; j10 ; j+) for(int t=0 ; tm+1 ; t+)float min = computer_distance(xrandj,yrandj,zrandj,rrtxt,rrtyt,rrtzt);if(min step)min_step = step ;else min_step = min1;det_anglex = anglexj+1-anglexj;det_anglez = anglezj+1-anglezj;r = random()/100.0;/得到10组tempt点if(det_anglex ph) xtempj=rrtxkmin+min_step*r*cos(anglexj+ph); ytempj=rrtykmin+min_step*r*sin(anglexj+ph);else if(-det_anglex ph)xtempj=rrtxkmin+min_step*r*cos(angl