CN108681787A - 基于改进双向快速扩展随机树算法的无人机路径优化方法 - Google Patents

Abstract

本发明公开了基于改进双向快速扩展随机树算法的无人机路径优化方法，该方法包括以下几个步骤：一、建立无人机运动学约束模型和确定二维空间内起始点和目标点；二、基于后向扇形区域采样方法扩展双向随机树的节点；三、基于动态自适应步长方法解决在障碍物附近生长局限性的问题；四、通过连接双向搜索树的节点，形成一条完整的飞行路径。本发明在经典B‑RRT*算法的基础上，基于后向扇形区域采样方法，当节点扩展时把随机树采样点限制在后向扇形区域，同时基于动态自适应步长方法解决了在障碍物附近生长局限性的问题，最后连接各节点，生成一条符合无人机真实可飞的飞行路径，为低空下无人机自主导航飞行提供了可行的方法。

Description

基于改进双向快速扩展随机树算法的无人机路径优化方法

技术领域

本发明涉及基于改进双向快速扩展随机树算法的无人机路径优化方法，属于低空下无人机自主导航飞行及运行安全保障技术领域。

背景技术

随着电子信息技术的进步和科技的发展，无人机UAV(Unmanned Aerial Vehicle)凭借体积小、成本低、效率较高、无人员伤亡、生存能力强、机动性能好、使用方便等优点，越来越受到各国的青睐。并且无人机的用途从一开始仅限于军用领域扩展到了民用领域。随着军民用无人机市场的迅猛发展，考虑到无人机的种类繁多，执行任务也各不相同，对每一架无人机进行路径规划是很有必要的。随着未来低空空域逐步放开，无人机数量持续增加，如何保证在低空空域下多无人机执行不同任务的安全飞行就成为研究的一个重点。因此对无人机进行路径规划，实现自主导航是很有必要的。

无人机路径规划算法很多，最常见的有以下几种：传统的路径规划算法如遗传算法、蚁群算法、粒子群算法等，这些算法在处理简单的路径规划问题时具有一定的优越性，但是在复杂环境下和高维空间中时，这些算法的复杂性会急剧增加，导致收敛时间长、求解困难。另外，基于启发式函数和人工势场的算法，如A*、D*和人工势场法，在处理规划问题时虽然能满足最优性和实时性等要求，但是因并未考虑到无人机自身性能约束和运动学限制，使得规划的路径不一定能被无人机所执行。基于采样的航迹规划算法已被证明可以高效地解决航迹规划问题，概率路线图算法(Probability Roadmap Method，PRM)和快速扩展随机树算法(Rapidly-exploring Random-Tree，RRT)是目前两种主要的采样方法。RRT算法收敛速度快，效率高，且树的航迹搜索比图的航迹搜索更容易实现。

RRT算法是随机在空间生成采样点，利用树结构进行节点扩展，最后通过对节点连接，形成完整路径的一种算法。但是RRT算法在算法复杂度和收敛速度上都存在局限，针对此，近些年也相继出现了很多基于RRT算法的改进型算法。如RRT*算法，有效改进了由基本RRT算法产生的并非概率最优解的问题；利用一个轻微变异的贪心RRT-connect作为启发函数来连接两颗随机树的B-RRT*算法，提升了算法的趋向性和智能性；还有基于三角几何选择节点提出的TG-RRT*算法，解决了得到最优解所需要的迭代次数，从而加速算法快速收敛；基于双向快速扩展随机树算法，通过智能样本插入的启发函数使得B-RRT*算法快速收敛到最优路径，但是该算法并未考虑到无人机自身性能约束和运动学约束模型的限制。

发明内容

本发明所要解决的技术问题是：提供基于改进双向快速扩展随机树算法的无人机路径优化方法，解决了现有技术中B-RRT*算法不能直接用于无人机路径规划和效率不佳的问题。

本发明为解决上述技术问题采用以下技术方案：

基于改进双向快速扩展随机树算法的无人机路径优化方法，包括以下步骤：

步骤1，考虑无人机自身的飞行性能约束，建立无人机动力学约束模型，同时确定无人机在二维空间内的起始点和目标点；

步骤2，分别从起始点和目标点开始，基于改进双向快速扩展随机树算法扩展对应的两个随机树节点，且在扩展时基于后向扇形区域采样方法对节点进行采样；具体方法为：从起始点x_init开始，扩展随机树T₁，当随机树T₁进行第n个节点的扩展时，依据已经扩展好的第n-2、n-1个节点，以第n-2个节点为端点形成一条射线，该射线经过第n-1个节点，以第n-1个节点为圆心、两倍的最大转弯角为圆心角、最小航迹段长度为弦长形成后向扇形区域，且该后向扇形区域被射线划分为两个对称的扇形，n为大于等于3的正整数，基于改进双向快速扩展随机树算法生成采样点，判断该采样点是否位于上述形成的后向扇形区域内；

若采样点位于后向扇形区域内，则第n个节点表示为：

若采样点不位于后向扇形区域内，则第n个节点表示为：

其中，x_n表示第n个节点，x_n-1表示已经扩展好的第n-1个节点，μ为步长，x_采样点表示基于改进双向快速扩展随机树算法生成的采样点，ξ为随机变量，ξ∈[-1,1]，为最大转弯角；

从目标点x_goal开始，扩展随机树T₂的方法与起始点相同；

步骤3，基于动态自适应步长方法，根据节点在扩展过程中距离障碍物的远近动态调整步长；

步骤4，根据步骤2和步骤3对随机树T₁、T₂进行扩展，直至随机树T₁、T₂相遇，当随机树T₁、T₂相遇后，连接起始点、随机数T₁的节点、随机树T₂的节点、目标点，生成最终的无人机规划路径。

作为本发明的一种优选方案，步骤1所述无人机动力学约束模型为：

其中，x,y分别表示无人机在惯性坐标系中的横,纵坐标，v为无人机的速度，ψ为无人机的航向角，R_min为无人机的最小转弯半径，u∈{-1,0,1}，为无人机的转弯角，为最大转弯角，Path为无人机的规划航迹长度，M为最大航迹长度。

作为本发明的一种优选方案，所述步骤3的具体方法为：

对于随机树T₁，调整步骤2扩展的第n个节点与第n-1个节点之间的步长，以第n-1个节点为圆心、两倍的最大转弯角为圆心角、最小航迹段长度为弦长形成后向扇形区域，该后向扇形区域被以第n-1个节点为端点的射线划分为两个对称的扇形，检测该后向扇形区域内是否有障碍物，若存在障碍物，则根据下式调整步长：μ_min≤μ′≤μ_max；若不存在障碍物，则μ′＝μ_max；其中，μ′表示调整后的步长，D表示距离第n个节点最近障碍物的距离，K为常数，μ_min、μ_max分别为最小、最大步长；

对于随机树T₂的步长调整与随机树T₁相同。

作为本发明的一种优选方案，步骤4所述随机树T₁、T₂相遇的判断条件为：

|x_init-x_goal|≤L

其中，x_init表示起始点，x_goal表示目标点，L为最小航迹段长度。

作为本发明的一种优选方案，所述最大转弯角为60°。

本发明采用以上技术方案与现有技术相比，具有以下技术效果：

1、本发明根据实际任务进行简化，建立无人机运动学约束模型，为后续的航迹规划算法提供了基础。

2、本发明所提出的基于经典B-RRT*算法的后向扇形区域采样方法，能够提高算法趋向性，减少随机点扩展，使生成的飞行路径更符合无人机自身性能条件约束。

3、本发明所提出的动态自适应步长方法能够有效解决固定步长在障碍物附近生长局限的问题，提高算法收敛速度。

附图说明

图1是本发明基于改进双向快速扩展随机树算法的无人机路径优化方法的流程图。

图2是本发明后向扇形区域采样示意图。

图3是动态自适应步长示意图，其中，(a)、(b)为障碍物位于检测范围内的情况；(c)、(d)为障碍物位于检测范围外的情况。

图4是双向快速扩展随机树扩展情况分布示意图，其中，(a)为经典B-RRT*算法；(b)为本发明算法。

图5是经典B-RRT*算法得到的无人机规划路径与本发明算法得到的无人机规划路径对比示意图。

图6是经典B-RRT*算法与本发明算法下的渐近最优性验证对比图。

具体实施方式

下面详细描述本发明的实施方式，所述实施方式的示例在附图中示出。下面通过参考附图描述的实施方式是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。

如图1所示，为本发明基于改进双向快速扩展随机树算法的无人机路径优化方法的流程图。包括如下步骤：

步骤一、建立无人机运动学约束模型，并在一定的条件下，对无人机实际任务进行简化，确定二维空间内起始点和目标点；

无人机根据任务区域空间环境和威胁参数，要求无碰撞的规划出一条从当前规划起点到目标点的可行航迹，需要满足无人机自身的飞行性能约束，同时要求航迹长度较短，假设无人机以恒定的速度和高度飞行，无人机的运动学约束可以采用以下模型进行描述：

x＝vcosψ (1)

y＝vsinψ (2)

式中(x,y)表示无人机在惯性坐标系中的位置，v表示无人机的速度，ψ为无人机的航向角；

ψ＝(v/R_min)u (3)

R_min为无人机的最小转弯半径，u∈{-1,0,1}；

此外，还需要满足另外两个约束条件：

Path≤M (5)

其中，最大转弯角无人机只能在小于或等于自身水平平面机动性能预先确定的最大转弯角范围内转弯。

最大航迹长度M：无人机的规划航迹的长度必须小于或等于一个预先设置的最大距离，数值由无人机自身携带的燃料决定，加入此约束可在一定程度上可少扩展节点的随机性和节点数。

步骤二、基于后向扇形区域采样方法扩展随机树节点；

本发明是基于一种基于改进双向快速扩展随机树算法的无人机路径进行优化，具体步骤是在随机树节点进行扩展时，可能会超出无人机最大转弯角，导致采样点不在无人机正后方的扇形区域内。本发明基于经典B-RRT*算法，当进行随机采样点扩展时，加入一些限制约束，使其只能在规定的后向扇形区域进行采样，这样就把无人机的运动学约束和B-RRT*算法中节点的扩展结合到一起，使生成的路径可被无人机直接使用。

对于搜索树T₁，调用扩展节点函数，在后向扇形区域内生成采样点x_rand，找出已经扩展的树节点x中离x_rand最近的一个树节点x_near，在(x_near,x_rand)中生成插入点x_new，检验是否比之前生成的总路径更短，如果是，则将其添加到搜索树T₁中；对于搜索树T₂同理。

无人机正常飞行时，受到最大转弯角的约束，在进行随机树采样点扩展时，为使航迹可飞，必须满足最大转弯角的限制条件。

当随机点x_rand位于可行域(即后向扇形区域Q)内时，节点扩展不变x_new表示为：

当随机点x_rand不在可行域内时，此时引入随机变量ξ∈[-1,1]，通过航向确定随机点的选择，生成x_new表示为：

其中，x_new为即将要扩展的节点；x_near为当前已经扩展的节点；x_rand为随机采样点。

步骤三、基于动态自适应步长方法，根据节点在扩展过程中距离障碍物的远近动态调整步长，加快算法收敛速度；

经典B-RRT*算法保留了RRT*算法以固定步长生长树的方式，步长越大生长越快。生长至障碍物附近的节点有较大的概率由于无法通过碰撞检测而被抛弃，较小的步长虽然在障碍物附近可以通过碰撞检测，但是在空旷的环境中生长相对较慢，导致算法效率相对较低。对于搜索树T₁，在扩展节点过程中，如在(x_near,L)范围内检测到障碍物，调用动态自适应步长函数调整步长，并将生成的节点添加到搜索树T₁中；对于搜索树T₂同理。由此本发明针对固定步长在障碍物附近生长局限性的问题，通过在源程序中加入动态步长的约束，在(x_near,L)范围内寻找距离点x_near最近障碍物的距离，再依据公式计算步长。具体方法如下：

通过这一方法可以动态调整步长，且数值在合理范围内(μ_min≤μ′≤μ_max)，障碍物距离x_near越近，步长越接近μ_min；当处在开阔环境中没有障碍物时，步长可为μ_max。

其中，D表示距离点x_near最近障碍物的距离；K为常数，根据选取的步长范围等参数确定。

步骤四、连接双向树节点，形成一条完整的飞行路径。

无人机当前位置起始点x_init和目标点x_goal，在扩展节点的过程中，如果满足步骤二、三，则分别加入到搜索树T₁、T₂中，直至两颗搜索树相遇。

当

|x_init-x_goal|≤L (9)

其中L为最小航迹段长度，是无人机在开始改变飞行姿态前必须要保持直飞的最短距离，数值取决于无人机的机动能力。

满足则返回{(x_init,x_goal)}，为最终生成的路径长度。

下面结合具体实施例说明本发明的具体方法：

步骤一、按照表1中所列参数，搭建模拟仿真环境。

表1

在此基础上，考虑到无人机自身的飞行性能约束，要求符合飞行路径的较短航迹，建立无人机运动学约束模型，确定无人机在二维空间内起始点和目标点，还有无人机飞行时所满足的其他条件。

步骤二、基于后向扇形区域采样方法扩展随机树节点：当随机树进行下一节点的扩展时，它会依据上一节点与此节点的连线，在延长线两侧对等形成一定的夹角，把随机树下一节点限制在后向扇形区域内。如图2所示，为后向扇形区域采样示意图。

步骤三、基于动态自适应步长方法，根据节点在扩展过程中距离障碍物的远近动态调整步长，加快算法收敛速度。图3的(a)、(b)、(c)、(d)为动态调整自适应步长示意图。

步骤四、连接双向树节点，形成一条完整的飞行路径。对经典B-RRT*算法和本发明的改进B-RRT*算法进行比较，如果对比发现其采样点个数变少，路径长度变短，所用时间减少，则视为发明有效。

由前述理论可知算法的最优性和后向扇形区域采样方法、动态自适应步长方法都有关。经过后向扇形区域采样方法、动态自适应步长方法都改进的B-RRT*算法为改进B-RRT*算法。本发明取其与经典的B-RRT*算法进行比较，得到随机树扩展情况分布结果如图4所示。图4可以看出，跟经典的B-RRT*算法相比，改进B-RRT*算法的探索点减少，树的生成方向更具有明确性，而且几乎没有冗余的扩展点，大大节省了时间，提高了效率。

对比涉及到的两种算法，得到优化后的无人机规划路径与原有的无人机规划路径的对比结果为图5。

仿真100次得到结果作比较如表2，改进B-RRT*算法在三个指标中表现较好，说明其较优结果具有稳定性。

表2

	采样点个数	路径长度	所用时间
				经典B-RRT*	101	1.3413	0.3056
改进B-RRT*	68	1.1629	0.1627

对比两种算法可以看出，经过优化后的改进B-RRT*算法比经典B-RRT*算法采样点个数变少，路径长度变短，所用时间减少，证明了改进B-RRT*算法比经典B-RRT*算法更有效率。

在路径长度这一指标上，每个算法重复试验100次，得到两种算法的渐进性结果如图6所示。经典B-RRT*算法在前几次中表现一般，但随着仿真次数的增加，可以明显看出其具有渐近最优性，经过优化后的改进B-RRT*算法表现稳定，从图中可以看出两种算法的较优性大小关系为改进B-RRT*>经典B-RRT*。因此得出结论，在无人机即时性和渐近最优性上，本发明提出的改进B-RRT*算法明显优于经典B-RRT*算法。

仿真实验结果表明，本发明经过改进的B-RRT*算法比经典B-RRT*算法更优，能够直接生成供无人机飞行的路径，验证了该算法的有效性和较优性。

以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (5)

1.基于改进双向快速扩展随机树算法的无人机路径优化方法，其特征在于，包括以下步骤：

步骤1，考虑无人机自身的飞行性能约束，建立无人机动力学约束模型，同时确定无人机在二维空间内的起始点和目标点；

步骤2，分别从起始点和目标点开始，基于改进双向快速扩展随机树算法扩展对应的两个随机树节点，且在扩展时基于后向扇形区域采样方法对节点进行采样；具体方法为：从起始点x_init开始，扩展随机树T₁，当随机树T₁进行第n个节点的扩展时，依据已经扩展好的第n-2、n-1个节点，以第n-2个节点为端点形成一条射线，该射线经过第n-1个节点，以第n-1个节点为圆心、两倍的最大转弯角为圆心角、最小航迹段长度为弦长形成后向扇形区域，且该后向扇形区域被射线划分为两个对称的扇形，n为大于等于3的正整数，基于改进双向快速扩展随机树算法生成采样点，判断该采样点是否位于上述形成的后向扇形区域内；

若采样点位于后向扇形区域内，则第n个节点表示为：

若采样点不位于后向扇形区域内，则第n个节点表示为：

其中，x_n表示第n个节点，x_n-1表示已经扩展好的第n-1个节点，μ为步长，x_采样点表示基于改进双向快速扩展随机树算法生成的采样点，ξ为随机变量，ξ∈[-1,1]，为最大转弯角；

从目标点x_goal开始，扩展随机树T₂的方法与起始点相同；

步骤3，基于动态自适应步长方法，根据节点在扩展过程中距离障碍物的远近动态调整步长；

步骤4，根据步骤2和步骤3对随机树T₁、T₂进行扩展，直至随机树T₁、T₂相遇，当随机树T₁、T₂相遇后，连接起始点、随机数T₁的节点、随机树T₂的节点、目标点，生成最终的无人机规划路径。

2.根据权利要求1所述基于改进双向快速扩展随机树算法的无人机路径优化方法，其特征在于，步骤1所述无人机动力学约束模型为：

其中，x,y分别表示无人机在惯性坐标系中的横,纵坐标，v为无人机的速度，ψ为无人机的航向角，R_min为无人机的最小转弯半径，u∈{-1,0,1}，为无人机的转弯角，为最大转弯角，Path为无人机的规划航迹长度，M为最大航迹长度。

3.根据权利要求1所述基于改进双向快速扩展随机树算法的无人机路径优化方法，其特征在于，所述步骤3的具体方法为：

对于随机树T₁，调整步骤2扩展的第n个节点与第n-1个节点之间的步长，以第n-1个节点为圆心、两倍的最大转弯角为圆心角、最小航迹段长度为弦长形成后向扇形区域，该后向扇形区域被以第n-1个节点为端点的射线划分为两个对称的扇形，检测该后向扇形区域内是否有障碍物，若存在障碍物，则根据下式调整步长：μ_min≤μ′≤μ_max；若不存在障碍物，则μ′＝μ_max；其中，μ′表示调整后的步长，D表示距离第n个节点最近障碍物的距离，K为常数，μ_min、μ_max分别为最小、最大步长；

对于随机树T₂的步长调整与随机树T₁相同。

4.根据权利要求1所述基于改进双向快速扩展随机树算法的无人机路径优化方法，其特征在于，步骤4所述随机树T₁、T₂相遇的判断条件为：

|x_init-x_goal|≤L

其中，x_init表示起始点，x_goal表示目标点，L为最小航迹段长度。

5.根据权利要求1所述基于改进双向快速扩展随机树算法的无人机路径优化方法，其特征在于，所述最大转弯角为60°。