CN112947594A - 一种面向无人机的航迹规划方法 - Google Patents

Abstract

本发明提供一种面向无人机的航迹规划方法，涉及无人机应用技术领域。本发明基于快速扩展随机树模型进行无人航迹规划，首先，采用多目标点同时搜索的方式加快搜索速度；然后利用目标启发策略降低搜索范围加快算法的收敛速度；最后，采用冗余点删除策略可以减少路由长度，通过平滑处理方式提高其平滑度，从而使其更好的应用于无人机航迹规划，以保证在复杂战场上的安全可以高效执行。本发明搜索空间冗余量少，在搜索时间损耗上用时较短，适用于快速航迹规划作战环境。

Description

一种面向无人机的航迹规划方法

技术领域

本发明涉及无人机应用技术领域，尤其涉及一种面向无人机的航迹规划方法。

背景技术

无人机(Unmanned Aerial Vehicle,UAV)是指机上不乘坐任何操控人员，能够自主远程飞行、并且可以携带一定载荷的装置。它不仅可以执行军事侦察、监视、搜索等非攻击性侦察任务，还可以执行地面攻击、轰炸等攻击任务。同时无人机的应用十分广泛，在军事、农业、物流等生活中都有着广泛的应用。

在军事领域中，由于战场任务复杂度的极大提高，无人机被安排到几乎覆盖了每个国家的每一个地区用于检测威胁。同时由于无人机的低成本和灵活性，不用考虑机载人员的生理限制，因此可以更加灵活、自由的完成各项复杂飞行动作，并可探索人类无法靠近的危险区域，能够替代人们执行各种重复性高、工作环境恶劣、工作性质危险的任务，随着无人机技术的飞速发展，战场的复杂性大大提高，越来越多的无人机将用于未来战场，而合理的航迹规划是无人机顺利完成任务的重要保障。

在民用领域无人机的作用也日益重要，逐渐成为帮助人类发现未知，探索自然，维护社会公共安全，参加重大抗震救灾等任务不可或缺的力量，尤其在航拍摄像，城市监控，天气预报，自然检测，基础设施检查、环境监测、区域绘图和地面机器人支持等领域有长足发展。但是无人机在执行复杂的任务时，如自动目标检测，或自主探索等任务还面临着许多挑战。这些挑战在室内环境中难度更大，因为在室内全球定位系统(Global PositioningSystem，GPS)数据不能用，为了高效地执行具有高水平自治性的此类高级任务，需要一种健壮且灵活的体系结构。因此需要分配具有避障的轨迹，轨迹控制用于使无人机沿着由航迹规划给出的期望轨迹移动。

无人机航迹规划指的是从起始点到目标点一段飞行区域内在满足约束条件的情况下选择一条最佳到达路径，该路径应当满足无人机自身约束条件以及环境约束条件并获得最大收益，从而以最小的代价到达目的地完成任务的执行。无人机航迹规划是无人机执行任务完成任务的关键，一个理想的航迹规划应该具备要求：首先能够自由控制所规划的航迹的精度，只要调节航迹点的数目即可；然后将原来的复杂问题简化，只需要确定一个航迹点或两个航迹点之间的航迹段是否满足条件即可；最后要便于实现分布式计算。

综上，无论是军事领域的搜索救援还是民用领域的自动目标检测和识别，使用多个UAVS进行协调都可以更快和更有效地执行任务。但是，当多个UAVs同时完成任务时，合理的航迹规划方法就成为了保证任务成功执行的至关重要的因素。

发明内容

本发明要解决的技术问题是针对上述现有技术的不足，提供一种面向无人机的航迹规划方法，基于快速扩展随机树模型进行无人航迹规划，更适用于实际应用环境，以保证高效、顺利地完成任务。

为解决上述技术问题，本发明所采取的技术方案是：

一种面向无人机的航迹规划方法，具体步骤如下：

步骤1、搭建网络环境；

搭建实际场景下的环境模型，确定要搜索的区域、可拓展的区域和无法拓展的区域，根据实际的任务需要，确定无人机执行任务的起始点和目标点；

步骤2、在确定下一跳节点时，利用目标启发式思想生成多个候选点，以阈值概率q将目标点设置为候选点，以概率1-q在空间中生成多个候选点，并寻找最优路由点；具体如下：

步骤2-1、设定概率阈值q；

步骤2-2、在(0,1)之间生成一个随机数，将值设置为概率p；

步骤2-3、比较p与q，如果p大于q，则转到步骤2-4，否则转到2-8；

步骤2-4、将目标点的值设置为生成的随机节点；

步骤2-5、在树上找到距离目标点最近的点定义为下一跳点；

步骤2-6、在下一跳方向确定步长生成下一个节点的位置；

步骤2-7、如果该点在威胁区域内，则返回步骤2-2，否则将该点加入到树中后返回2-2；

步骤2-8、在规划区域中，随机生成m个节点；

步骤2-9、在m个点中选择最合适的点作为下一跳点；

步骤2-10、如果该点在威胁区域内，则返回步骤2-2，否则将该点加入到树中后返回2-2；

步骤3、把起始位置与目标位置分别当作对方的终点进行双向扩展，直到它们连接起来终止扩展；

步骤4、交换两颗树的搜索路径从而得到起始点到目标点的路径规划；

步骤5、在确定路由点之后，对不必要的路径进行剪枝处理，具体步骤如下：

步骤5-1、为简化路由航迹建立初始路由表；

步骤5-2、增加目标点到路由表中；

步骤5-3、判断路由表的点到目标点之间是否有威胁存在，有则删除该路由点，没有则检查路由表中的下一个路由点；

步骤6、对锐角转弯的路径进行满足曲率约束的平滑处理，具体步骤如下：

步骤6-1、生成转弯圆弧；

步骤6-2、生成切线轨迹；

步骤6-3、生成对准轨迹圆圆心；

步骤7、输出一条从起始点到目标点的平滑路径。

采用上述技术方案所产生的有益效果在于：本发明提供的面向无人机的航迹规划方法，首先，采用多目标点同时搜索的方式加快搜索速度；然后利用目标启发策略降低搜索范围加快算法的收敛速度；最后，采用冗余点删除策略可以减少路由长度，通过平滑处理方式提高其平滑度，从而使其更好的应用于无人机航迹规划，以保证在复杂战场上的安全可以高效执行。本发明搜索空间冗余量少，在搜索时间损耗上用时较短，适用于快速航迹规划作战环境。

附图说明

图1为本发明实施例提供的无人机航迹规划的应用场景图；

图2为本发明实施例提供的无人机航迹规划的原理图；

图3为本发明实施例提供的快速扩展随机树的原理图；

图4为本发明实施例提供的的快速扩展随机树从目标点逆向搜索原理图；

图5为本发明实施例提供的的快速扩展随机树双向搜索算法原理图；

图6为本发明实施例提供的未进行剪枝处理的规划路径图；

图7为本发明实施例提供的剪枝处理后的规划路径图；

图8为本发明实施例提供的无人机航迹平滑处理前航迹图；

图9为本发明实施例提供的无人机航迹平滑处理后航迹图；

图10为本发明实施例提供的Dubins处理原理图；

图11为本发明实施例提供的钝角平滑处理示意图；

图12为本发明实施例提供的无人机任务规划软件演示控制界面图；

图13为本发明实施例提供的无人机航迹规划侦察阶段仿真图；

图14为本发明实施例提供的无人机航迹规划仿真图；

图15为本发明实施例提供的无人机航迹规划返航阶段仿真图；

图16为传统航迹规划策略仿真结果图；

图17为本发明实施例提供的航迹仿真结果图；

图18为本发明实施例提供的航迹平滑仿真图。

具体实施方式

下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。以下实施例用于说明本发明，但不用来限制本发明的范围。

本发明主要解决多无人机在执行任务时的飞行路径规划问题，目的是帮助无人机快速、高效的完成任务。本实施例中的航迹规划方法的具体步骤如下。

步骤1、搭建系统环境；

无人机航迹规划的应用如图1，其目的是为无人机找到一条合适的路径，其原理如图2所示，假设要搜索的区域为R，R_free表示可扩展区域，R_obs表示无法扩展区域，R_free与R_obs都是R的子集，且满足R＝R_free∪R_obs与

起始位置q_init∈R_free和目标位置q_goal∈R_free。

本发明的核心是利用快速随机生成树的生长来进行航迹规划，该方法的原理如图3所示，首先在航迹规划的选定区域内选定起始位置q_init当作树的根节点，然后通过从根节点不断的向周围扩展子节点然后将其连接起来构建随机树。在延伸的过程中如果该点与威胁区域或者存在障碍区域则此扩展点不符合要求，放弃此节点重新进行随机目标点q_rand，如果没有障碍威胁则确定此节点为新节点q_new。通过这种不断重复不断迭代，当随机数的叶子节点不断靠近目标点离叶子节点足够进的时候，则认为扩展完成。然后以离目标点最近的点当作开始节点以次搜索父节点，得到的一条从起始点到目标点的可行路径就是要求的航迹拓展路径。

步骤2、在起始点q_init之后，利用目标启发式思想生成多个候选点q_rand，在所有的候选点中合适的点q_rand作为下一跳点q_near；

当随机节点q_rand生成时，在一定概率下选择将目标点设为q_rand。它可以帮助搜索树向目标点扩展，加速算法收敛。在每个迭代的m个未确定节点中，它们由评估函数进行评估，并且只有最佳节点将被作为新节点q_new赋予搜索树。因此，可以在搜索树中高精度节点减少性能不佳的节点。这种优化策略的具体步骤如下：

步骤2-1、确定概率阈值p_gc(0＜p_gc＜1)，在每迭代m次后产生的随机数；

步骤2-2、在(0，1)之间生成一个随机数，将值设置为概率p_g；

步骤2-3、比较p_g和p_gc。如果p_g＞p_gc，则转到步骤2-4；如果不是，转到步骤2-6；

步骤2-4、在树上找到最近的q_goal＝q_rand的节点，并将其定义为q_near；根据步长，生成未确定的节点q_unrand；

步骤2-5、如果q_unrand位于障碍威胁区域中则返回步骤2-2；如果不在，则将q_unrand.添加到搜索树并转到步骤2-2；

步骤2-6、在规划区域中，随机生成m个节点，并用

表示；比较最近的点并用

表示；

步骤2-7、对m个确定点

通过式1计算最小值q_min；

步骤2-8、如果P_min在威胁区域内返回步骤2-2，否则，增加P_min到树中并返回步骤2-2；价值函数主要包括威胁代价函数、路径长度代价函数和路径转向代价函数，形成如下：

W＝k₁W_thread+k₂W_path+k₃W_turn (1)

其中，k₁、k₂与k₃为三个消耗系数，并且0≤k₁,k₂,k₃≤1且k₁+k₂+k₃＝1；W_thread、W_path、W_turn分别表示威胁消耗、路径消耗、转弯角消耗。这三个消耗指标通过表1表示：

表1代价消耗指标描述

对于未确定点三个消耗函数值可以通过如下公式计算：

其中，n是雷达威胁数量，

是第j个雷达到

的概率。

是从其实点到

的路径长度，

表示

到目标点的距离。

表示无人机航向变化值。

步骤3、把起始点q_init和目标点q_goal分别当作对方的终点进行双向扩展，直到它们连接起来终止扩展；

搜索树双向连接的设计是为了解决搜索树收敛速度慢的问题而提出的，且使用条件与搜索树相比没有任何不同。在这种情况下，增量运算的重要性并不重要。该方法基于两个条件：试图移动较长距离的连接启发式，以及搜索树是从起始点q_init和目标点q_goal的两个方向同时搜索。图3是从起始点正向搜索，图4是从目标点逆向搜索的过程。

步骤4、交换两颗树的搜索路径从而得到起始点与目标点的路径规划；

把起始位置与目标位置分别当作对方的终点进行双向扩展，直到它们连接起来终止扩展，找到了快速规划飞行路径的解决方案。位拓展原理图如图5所示，在每次迭代中，一棵树是前-倾向于尝试将另一棵树的最近顶点连接到新的顶点。这使得两棵树都在探索各自最短路径，同时试图建立它们之间的联系。然后，通过交换两棵树的搜索路径来逆转它们，从而得到起始点与目标点的路径规划。两个搜索树的增长适用于动态规划；然而，在每次迭代中，两棵树都向自己目标点随机配置进行了扩展。当前的算法也试图使树木向彼此生长。使收敛时间大大降低路径规划的更加迅速，更有利于动态规划的应用。

步骤5、在完成路由点寻找后，路由点{p₁,p₂,…,p_n}被列出来，其中p₁表示开始点p₂表示结束点；在搜索树中。每个节点只有一个父节点，而起始节点是唯一的根节点。在完成搜索树扩展后，由于节点选择的随机性和步长的不精确性，在路由点列表中存在大量冗余的路由点，如图6所示。为了进一步减少路径长度的无人机状态调整，将通过删除冗余的路由点来简化路径。在冗余点删除过程中，如果没有威胁两个路由点之间的空间相邻，它们之间的其他点被视为冗余路由点，可以从路由点删除名单如图7所示。具体步骤如下：

步骤5-1、为简化路由航迹建立初始清单Γ点。最初Γ是空的

步骤5-2、增加目标点p_n到Γ，让j＝n，i＝1；

步骤5-3、如果存在威胁障碍点在p_j与p_i中返回第四步，否则，执行5-5；

步骤5-4、如果i＝j-1，则j＝j-1，i＝1返回5-3；否则i＝i+1，返回5-3；

步骤5-5、增加p_i到Γ并且让j＝i，i＝1。如果j＝1，则删除该点，否则返回5-3。

步骤6、对路径进行满足曲率约束的平滑处理。

在无人机航迹规划过程中由于规划的航迹是由搜索点组成的，如果前后航线夹角过于陡峭则无人机由于受限于最小转完半径的约束不能进行飞行，如图8所示，因此必须进行航迹平滑处理。对其处理过的航迹满足无人机最小转弯半径，使之在任何情况下都能不受限于转向角的约束，如图9所示。

无人机执行任务的飞行速度为v_t，无人机约束最大转向角速率为ω，此时对应的最小转向半径为R，二维平面下则无人机相对位置(x,y)和航向θ的运动方程可简化为以下方程：

x(t)＝vcosθ(t) (3)

y(t)＝vsinθ(t) (4)

θ(t)＝Ω Ω∈[-ω,ω] (5)

R＝v/ω (6)

无人机进行航迹交接处对接可看作在二维平面内具有特定方向向量的两点，无人机飞行进入弯道约束区域切点我们称之为起始点，无人机转向结束离开自适应过度进入下一航段的交接点称之为结束点，交接自适应过度轨迹为Dubins曲线轨迹，该轨迹曲线曲率满足大于1/R。Dubins曲线轨迹就是一个或多个圆弧构造而成，图8以航线夹角是锐角的情况为例，介绍基于Dubins曲线的自适应过度轨迹的规划。

图8锐角的航迹平滑处理在受到最小转弯半径，和转弯速度的约束下可以通过以下3个步骤实现平滑。具体步骤如下：

步骤6-1：生成转弯圆弧，圆心为o；

根据进入航段ab建立直角坐标系，其中航迹点a、b、c的坐标分别为(x_a,y_a)，(x_b,y_b)，(x_c,y_c)，航段进入点d的坐标为(x_d,y_d)，航段离开点坐标为(x_n,y_n)

无人机最小转弯约束半径为R，无人机宽度为D。

根据无人机的约束条件通过几何知识可以求出转弯Dubins曲线圆弧的圆心o位于锐角的角平分线上，同时可以求得圆心o的坐标(x_o,y_o)为：

步骤6-2：生成切线轨迹

从航迹进入点d向转弯轨迹做一条切线，切点为e，得到切线长de。由已知条件及几何知识可知圆弧圆心o点与e点距离为半径R，设圆心o点与d点距离为l_od，则点e与点d点距离为l_ed，切线航迹e点坐标(x_e,y_e)为：

步骤6-3：生成对准轨迹圆心

对准轨迹圆也是满足转弯约束条件生成的Dubins曲线轨迹圆对准轨迹圆心记为m，转弯轨迹圆心o与b点距离为l_ob，转弯轨迹圆心o点与对准轨圆心m点距离为2R，通过求解可得到圆心m坐标(x_m,y_m)为：

由上面三个步骤可求解出满足约束条件的基于Dubins曲线的锐角自适应过度轨迹，根据几何关系以及原理图10，可以计算出平滑后路线图，在不同转弯方向情况下注意计算中坐标距离符号即可。当前后两航段夹角为钝角或直角情况，自适应过度轨迹规划较航段夹角为锐角的情况简单，其生成的轨迹如图11所示

步骤7、输出一条从起始点到目标点的平滑路径；

为了更好的应用本实施例的方法，设计了多无人机任务规划控制界面如图12所示，主要包括侦察数量模块，威胁数量模块，攻击数量模块，算法模块，以及打开地图模块和开始模块。

其中侦察数量模块的功能是在无人机在出发执行任务之前先派出侦察机进行任务侦察，将侦察到的信息返回给控制台，然后控制台根据侦察信息做出航迹规划。威胁数量模块的功能是指仿照无人机战场环境下的障碍，地方雷达等威胁，设置的威胁数量参数，可设置参数范围0到100；攻击数量参数模块的功能是指可以参战的无人机的数量，可设置参数范围是0到100；算法模块的功能是指仿真搜索所选用的算法；打开地图模块的功能是指保存仿真图按钮；开始模块的功能是指开始仿真的功能；

多无人机航迹规划主界面如图13所示，主要有十部分组成，分别是战场背景显示功能、开始功能、计时功能、隐藏路径功能、显示战雾功能、加速功能、显示树点功能、路径长度显示功能、以及生成文件存储功能。

在控制界面设置好仿真条件后点击开始按钮，仿真进入显示主界面，背景模块中的侦察机开始进行侦察搜索，搜过过的区域由暗色变为亮色，逐步全覆盖范围进行搜索。当遇到多个目标时控制中心根据无人机条件约束以及作战环境进行航迹规划，规划仿真如图14所示：在无人机完成任务之后或者无法继续执行接下来的任务，则无人机进行返航航迹规划，通过生成树策略结合环境约束条件，对其进行仿真演示如图15所示。

同时为了验证本发明中的方法有效性，设置任务环境为1000×1000的矩形区域，无人机起始位置坐标为(0,0)，目标位置坐标为(1000,1000)。设置威胁数目100个威胁。与原始方法进行对比，图16和图17为对比结果。可以得知本实施例方法在搜索空间冗余上比原始方法的冗余量少。

图18显示了经过平滑处理之后的轨迹路线，可以看出通过Dubins方法对航迹进行曲线自适应平滑处理可以适应作战条件下的无人机航迹平滑处理。从而可以改变转换角过于陡峭，限制无人机转向问题，使无人机更好的应用于广泛的作战环境中。

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明权利要求所限定的范围。

Claims (1)

1.一种面向无人机的航迹规划方法，其特征在于：包括以下步骤：

步骤1、搭建系统环境；

在长宽分别为L,W的矩形区域R内有若干处雷达监测点和威胁区域，所以该区域可以分为可拓展区域R_free，和法扩展区域R_obs，R_free与R_obs都是R的子集，且满足R＝R_free∪R_obs与

起始位置q_init∈R_free和目标位置q_goal∈R_free；

步骤2、在确定下一跳节点时，利用目标启发式的思想生成多个候选点q_rand，在所有的候选点中选择合适的点作为下一跳点q_near；具体过程如下：

步骤2-1、确定概率阈值p_gc，0＜p_gc＜1，在每迭代m次后产生的随机数；

步骤2-2、在(0，1)之间生成一个随机数，将值设置为概率p_g；

步骤2-3、比较p_g和p_gc；如果p_g＞p_gc，则转到步骤2-4；如果不是，转到步骤2-6；

步骤2-4、在树上找到最近的q_goal＝q_rand的节点，并将其定义为q_near；根据步长，生成未确定的节点q_unrand；

步骤2-5、如果q_unrand位于障碍威胁区域中则返回步骤2-2；如果不在，则将q_unrand添加到搜索树并转到步骤2-2；

步骤2-6、在规划区域中，随机生成m个节点，并用

表示；比较最近的点并用

表示；

步骤2-7、对m个确定点

最小值q_min；

步骤2-8、如果P_min在威胁区域内返回步骤2-2，否则增加P_min到树中并返回步骤2-2；

步骤3、把起始点q_init和目标点q_goal分别当作对方的终点进行双向扩展，直到它们连接起来终止扩展；

步骤4、交换两颗树的搜索路径从而得到起始点与目标点的路径规划；

步骤5、检查路由表，删除路由表中的冗余路有点；具体步骤如下：

步骤5-1、为简化路由航迹建立初始清单Γ点；最初Γ是空的；

步骤5-2、增加目标点p_n到Γ，让j＝n，i＝1；

步骤5-3、如果存在威胁障碍点在p_j与p_i中返回第四步，否则，执行5-5；

步骤5-4、如果i＝j-1，则j＝j-1，i＝1返回5-3；否则i＝i+1，返回5-3；

步骤5-5、增加p_i到Γ并且让j＝i，i＝1；如果j＝1，则删除该点，否则返回5-3；

步骤6、对路径进行满足曲率约束的平滑处理；

假设无人机执行任务的飞行速度为v_t，无人机约束最大转向角速率为ω，此时对应的最小转向半径为R，二维平面下则无人机相对位置(x,y)和航向θ的运动方程简化为以下方程：

x(t)＝vcosθ(t) (1)

x(t)＝vsinθ(t) (2)

θ(t)＝Ω Ω∈[-ω,ω] (3)

R＝v/ω (4)

无人机进行航迹交接处对接看作在二维平面内具有特定方向向量的两点，无人机飞行进入弯道约束区域切点称之为起始点，无人机转向结束离开自适应过度进入下一航段的交接点称之为结束点，交接自适应过度轨迹为Dubins曲线轨迹，该轨迹曲线曲率满足大于1/R；通过以下3个步骤实现平滑，具体步骤如下：

步骤6-1：生成转弯圆弧，圆心为o；

根据进入航段ab建立直角坐标系，其中航迹点a、b、c的坐标分别为(x_a,y_a)，(x_b,y_b)，(x_c,y_c)，航段进入点d的坐标为(x_d,y_d)，航段离开点坐标为(x_n,y_n)；

无人机最小转弯约束半径为R，无人机宽度为D；

根据无人机的约束条件通过几何知识求出转弯Dubins曲线圆弧的圆心o位于锐角的角平分线上，同时求得圆心o的坐标(x_o,y_o)为：

步骤6-2：生成切线轨迹；

从航迹进入点d向转弯轨迹做一条切线，切点为e，得到切线长de；由已知条件及几何知识可知圆弧圆心o点与e点距离为半径R，设圆心o点与d点距离为l_od，则点e与点d点距离为l_ed，切线航迹e点坐标(x_e,y_e)为：

步骤6-3：生成对准轨迹圆心；

对准轨迹圆也是满足转弯约束条件生成的Dubins曲线轨迹圆对准轨迹圆心记为m，转弯轨迹圆心o与b点距离为l_ob，转弯轨迹圆心o点与对准轨迹圆心m点距离为2R，通过求解得到圆心m坐标(x_m,y_m)为：

由上面三个步骤求解出满足约束条件的基于Dubins曲线的锐角自适应过度轨迹；

步骤7、输出一条从起始点到目标点的平滑路径。

Images