CN112987799A - 一种基于改进rrt算法的无人机路径规划方法 - Google Patents

Abstract

本发明提供了一种基于RRT算法(快速随机扩展树，Rapid‑exploration Random Tree)改进的无人机航迹规划方法，属于无人机技术领域。本发明考虑到RRT算法由于随机性过强导致的效率不够高、路径不是最优的问题，提出了一种基于上次扩展碰撞情况的自适应扩展策略、以及更优化的父节点选择方式。随机树在扩展过程中，在没有遇到障碍时，会逐渐增强其向目标点方向快速靠拢的倾向；一旦遇到障碍，将会立即降低这种倾向，最大化生长的随机性，从而使得随机树更高效地避开障碍物。同时，在每次生长时，会选择代价更小的父节点，而不是简单地选择最近节点。改进后的方法提高了规划效率和速度、降低了规划路径的距离。最后还会对规划好路径进行进一步的优化，进一步缩短规划路径长度。

Description

一种基于改进RRT算法的无人机路径规划方法

技术领域

本发明属于无人飞行器技术领域，尤其涉及一种改进RRT算法的无人机航迹规划方法。

背景技术

无人飞行器具有行动灵活、易于操作等特点，被广泛应用于军事领域和民用领域。无人飞行器在低空飞行执行任务过程中展现出了极大的优势，在实现无人飞行器自主执行飞行任务的过程中，航迹规划是极为重要的一个环节。无人飞行器应当能够获取能够规避障碍物并到达目标点的可行路径。

无人飞行器的航迹规划问题，可以看作是机器人路径规划的进一步发展。针对路径规划问题，曾经提出许多算法，如势场法、栅格法、仿生算法、A*算法等。传统的路径规划算法，如人工势场法，它采用的是基于矢量合成的方法，通过障碍物对小车的排斥力与目标点对小车的吸引力的合力作用下规划机器人的运动路径。在已知局部信息的情况下，人工势场的避障策略十分有效，但当吸引力与斥力的合力为零时，小车就陷入了陷入局部极小的情况。

大部分传统算法都需要对环境进行建模和预处理，在向高维空间进行扩展时，会极大地增加算法的复杂性。而RRT算法通过对状态空间中的随机采样点进行碰撞检测，避免了对空间的建模，能够有效的解决高维空间和复杂约束的路径规划问题。但是RRT算法也有自己的缺点，由于其较强的采样随机性，所以它规划路径用时较长，实时性不高且很难规划出最优路径。

发明内容

本发明针对RRT算法存在的问题，提出了基于上次扩展碰撞情况的自适应扩展策略，并通过优化父节点选择并在规划后优化路径的方式来对RRT算法进行改进。实现一个可用于三维空间内的基于RRT的航迹规划算法，主要用在无人机场景下，其具体步骤如下

步骤1，获取航迹规划所需的参数信息，包括航迹规划的环境范围C_free，以及C_free内部的障碍物信息，航迹规划的出发点q_ini。，航迹规划的终点q_goal。

步骤2，以q_init为根节点，在C_free的范围内，建立随机搜索树T_search，初始时T_search只有一个节点q_init。同时为T_search中的节点建立基于空间位置的索引。

其中T_search的数据结构为一个树形结构，其中每个节点应当包含以下信息，当前节点的位置信息(x，y，z)，当前节点到达q_init的距离信息d，当前节点的父节点指针q_f，当前节点的所有子节点指针集合Q_child。

而T_search中的节点基于空间位置的索引，其特征在于：

使用三维数组进行快速索引，在三维数组组成的索引的键值对中，键的值为(a，b，c)样式的字符串，而值为搜索树中节点的子集。

对于搜索空间C_free在三个维度上都有边界(x_min，x_max)、(y_min，y_max)、(z_min，z_max)，对于每一组边界，按需设置一个步长

每隔

设置一个索引边界x_i、y_i、z_i。对于T_search中的任意节点q，设它的空间位置为(x_q，y_q，z_q)，其中x_q∈(x_t，x_t+1]、y_q∈(y_t，y_t+1]、z_q∈(z_t，z_t+1]。那么q应当处于键为(a_q，b_q，c_q)的节点集合中。

其中步长

不应设置过小，否则容易出现大量无效查询。

在查找的过程中，需要根据对应点的空间位置，运算得出对应点的值在三维数组中的真实下标，即索引(a，b，c)。对于其计算方法，有如下公式：

步骤3，在自由空间C_free范围内，生成一个用于扩展的采样点q_rand，其中q_rand的生成策略是基于上次扩展碰撞情况而自适应变化的。

其中扩展采样点q_rand生成策略，其特征在于：

需要有一个概率值p_g，其表示本次扩展不使用随机方式来选取采样点q_rand，而是直接使用q_goal作为本次扩展的q_rand的概率。在开始算法前，应当确定p_g自适应变化的最大值p_gmax和最小值p_gmin，应满足0＜p_gmin＜p_gmax＜1。

而每次搜索树扩展时使用的p_g具体值由以下公式确定：

其中n的值确定方式如下，若搜索树在扩展时，使用q_goal作为本次扩展的q_rand且本次扩展的碰撞检测通过(如权利要求1，步骤4所述)，则n的值加一。若是任意一次碰撞检测不通过，则n的值置0。

步骤4，设T_search中某个节点q_i与q_rand的欧式距离为D(q_rand，q_i)，在T_search中找到一个满足min(D(q_rand，q_i))的节点设为q_nearest，然后让q_nearest朝向q_rand的方向生长一段距离，得到新节点q_new，并暂定它的父节点为q_nearest，之后对(q_new，q_nearest)这段路径进行碰撞检测，若检测不通过，则返回步骤3。其中q_new的生长长度是基于上次扩展碰撞情况而自适应变化的。

其中q_nearest的选取，需要使用之前提到的三维数组索引来进行加速，其步骤如下：

步骤4.1，设q_rand的空间位置为(x_r，y_r，z_r)，应当有一个索引查询范围ω，且应当满足

对于x_r，y_r，z_r三个数据，分别±ω，从而组合出8个采样点。

步骤4.2，根据q_rand以及八个采样点，分别获取到它们在索引中对应的键值，之后对这些键值取并集K。之后对集合K中所有键值进行查找，得到对应的搜索树子集，并取得节点并集Q。

步骤4.3，若Q不为空集合，则从Q中获取满足min(D(q_rand，q_i))的节点q_nearest。否则放弃索引，直接从搜索树中查找。

而q_new的生长长度选取策略，其特征在于：

设置一个最小步长d_mmin，当本次扩展随机方式来选取采样点q_rand，那么本次生长长度为d_min。

否则本次生长的长度为：

其中n的值确定方式如下，若搜索树在扩展时，使用q_goal作为本次扩展的q_rand且本次扩展的碰撞检测通过(如权利要求1，步骤4所述)，则n的值加一。若是任意一次碰撞检测不通过，则n的值置0。而δ∈(1，+∞)，用于控制自适应变化的速度。

步骤5，根据q_new的空间位置，在索引中，获取那些距离q_new较近的T_search节点集合Q_best。在Q_best中，找到代价最小的父节点，即通过此父节点时，使得q_init～q_new的距离最小。将q_new加入T_search。

其中节点集合Q_best的选取，也需要通过三维数组索引来加速，其步骤为：

步骤5.1，设q_new的空间位置为(x_new，y_new，z_new)，应当有一个搜索范围μ，且应当满足

对于x_new，y_new，z_new三个数据，分别±μ，从而组合出8个采样点。

步骤5.2，根据q_new以及八个采样点，分别获取到它们在索引中对应的键值，之后对这些键值取并集K。之后对集合K中所有键值进行查找，得到对应的搜索树子集，并取得节点并集Q_best。

步骤6，检测D(q_new，q_goal)是否小于特定阈值τ，若D(q_new，q_goal)＜τ则判定规划成功，否则返回步骤3，继续扩展T_search。

步骤7，针对得到的路径(q_init，...，q_i，...，q_goal)，进行优化，以q_init为起点按顺序寻找(q_init，q_s1)可以通过碰撞检测的最远节点q_s1，之后又以q_s1为起点寻找(q_s1，q_s2)可以通过碰撞检测的最远节点q_s2，以此类推直到扩展到q_goal。得到的新路径(q_init，...，qsi，...，q_goal)。

本发明通过设计一个基于上次扩展碰撞情况的自适应扩展策略，从而改进了传统RRT算法运行时长不稳定，路径不是最优化的问题；设计了一种搜索树的节点索引，加速了RRT算法选择最近节点的速度；设计了一个更优父节点的选择方式以及最终路径优化方法，从而保证得到的路径相对来说是最优化的。

附图说明

图1是本发明中RRT改进算法的流程图。

图2是本发明中的索引结构示意图。

图3是本发明中RRT算法中一次扩展的流程图。

图4是本发明中的父节点优化的二维示意图。

图5是本发明最终路径优化的二维示意图。

具体实施方式

下面将结合附图对本发明进行进一步地描述。

图1是本发明中改进的RRT算法流程图，可以看出其具体步骤如下：

步骤1，进行初始化和参数设定。

首先应该将无人机飞行的环境C_free以三维地图的形式输入程序，并确定本次寻路的起点q_init和终点q_goal的位置坐标信息。此外，还需要根据环境C_free的特点来设定本次算法的相关参数，其中包括索引中，每一块索引覆盖范围的步长

朝向目标扩展概率p_g自适应变化的最大值p_gmax和最小值p_gmin；使用索引查找q_nearest的寻找范围ω；每次扩展的最小步长d_min；最优父节点寻找范围μ；用于控制自适应生长长度变化速率的参数δ；判定抵达终点的阈值τ。

步骤2，开始进行航迹规划的准备。

初始化随机搜索树T_search，其中只有一个节点q_init。同时也需要为T_search中的节点建立基于空间位置的索引，其结构如图2所示。

该索引使用三维数组对来维护，其键的值为(a，b，c)样式的字符串，而值为搜索树中节点的子集。

将搜索空间C_free在三个维度上的边界(x_min，x_max)、(y_min，y_max)、(z_min，z_max)使用之前确定好的索引覆盖范围的步长

进行划分，将C_free划分为若干个空间分区，之后T_search生成的新节点，都会根据其坐标位置将其加入到对应空间分区的节点子集中去。

步骤3，开始扩展，首先获取扩展采样点q_rand，其步骤如下，也如图3所示：

步骤3.1，进行一次范围0-1的随机数生成，并根据上次扩展结束后计算得到的p_g值，来进行一次判断，根据其结果选择下一步。

步骤3.2a，若随机数小于p_g，那么本次扩展使用q_goal作为q_rand。

步骤3.2b，若随机数大于p_g，那么本次扩展根据C_free在三个维度上的边界(x_min，x_max)、(y_min，y_max)、(z_min，z_max)，分别以它们为上下边界进行三次随机数生成，设得到的值分别为(x_r，y_r，z_r)，此坐标即为q_rand的坐标。

步骤4，找到T_search中距离q_rand最近节点q_nearest，其步骤如下：

步骤4.1，首先尝试通过索引来进行快速查找，对于x_r，y_r，z_r三个数据，分别±ω，从而组合出8个采样点。

步骤4.2，使用q_rand以及八个采样点的坐标，分别计算出其在索引对应的键值，之后对这些键值取并集K。之后对集合K中所有键进行查找，得到对应的搜索树子集，并取得节点并集Q。根据Q的特点，选择下一步。

步骤4.3a，若Q不为空集合，则从Q中获取满足min(D(q_rand，q_i))的节点q_nearest。

步骤4.3b，若Q为空集合，则直接遍历T_search的节点集合，从中获取满足min(D(q_rand，q_i))的节点q_nearest。

步骤5，生成新节点q_new，其步骤如下：

步骤5.1，使用q_rand和q_nearest的坐标求差值，获取到向量(x_r-x_n，y_r-y_n，z_r-z_n)，计算后设为(x_direction，y_direction，z_direction)，该向量表示了q_nearest指向q_rand的方向信息。之后根据步骤3中是否使用随机q_rand，来选择下一步。

步骤5.2a，若是随机q_rand，那么本次生长长度为d_min，具体到坐标应该用以下公式计算：

步骤5.2b，若是使用q_goal作为q_rand，那么本次生长长度为上次扩展结束后计算得到的生长长度d，具体到坐标应该用以下公式计算：

步骤6，针对(q_new，q_nearest)这段路径进行碰撞检测，并更新p_g和d的值，其步骤如下：

步骤6.1，进行碰撞检测，根据结果来对p_g和d的值进行更新，更新p_g的公式如下：

更新d的公式如下：

其中n的值确定方式如下，若搜索树在扩展时，使用q_goal作为本次扩展的q_rand且本次扩展的碰撞检测通过，则n的值加一。若是任意一次碰撞检测不通过，则n的值置0。而δ∈(1，+∞)，用于控制自适应变化的速度。

之后还需要根据碰撞检测结果来确定下一步：

步骤6.2a，若碰撞检测不通过，则返回步骤3。

步骤6.2b，若碰撞检测通过，则以q_nearest为父节点，将q_new加入T_search，并更新q_new经过q_nearest到达q_init的距离信息。

步骤7，更新q_new的父节点，寻找代价最小父节点，其步骤如下：

步骤7.1，使用索引来找到潜在父节点，对于x_new，y_new，z_new三个数据，分别±μ，从而组合出8个采样点。

步骤7.2，根据q_new以及八个采样点，分别获取到它们在索引中对应的键值，之后对这些键值取并集K。之后对集合K中所有键进行查找，得到对应的搜索树子集，并取得节点并集Q_best。

步骤7.3，对Q_best中的节点进行遍历，对每一个节点，进行q_new经过该节点时，到达q_init的距离计算，并与经过当前父节点时的距离进行比较(本运算代价不大，在节点中存有当前节点到达q_init的距离信息)，若是更小，那么再进行碰撞检测，通过后，则更新父节点为当前节点，并更新q_new到达起点的代价信息。直到遍历结束，其二维示意图如图4所示。

步骤8，检测D(q_new，q_goal)是否小于特定阈值τ，若D(q_new，q_goal)＜τ则判定规划成功，否则返回步骤3，继续扩展T_search。

步骤9，针对得到的路径(q_init，...，q_i，...，q_goal)，进行优化，以q_init为起点按顺序寻找(q_init，q_s1)可以通过碰撞检测的最远节点q_s1，之后又以q_s1为起点寻找(q_s1，q_s2)可以通过碰撞检测的最远节点q_s2，以此类推直到扩展到q_goal。得到的新路径(q_init，...，q_si，...，q_goal)。整个过程如图5所示。

Claims (7)

1.一种基于RRT算法改进的无人机航迹规划方法，其特征在于，在RRT算法中引入了基于上次扩展碰撞情况的自适应扩展策略，并通过优化父节点选择并在规划后优化路径的方式来提高航迹规划的效率并降低路径的距离，具体步骤如下：

步骤1，获取航迹规划所需的参数信息，包括航迹规划的环境范围C_free，以及C_free内部的障碍物信息，航迹规划的出发点q_init，航迹规划的终点q_goal。

步骤2，以q_init为根节点，在C_free的范围内，建立随机搜索树T_search，初始时T_search只有一个节点q_init。同时为T_search中的节点建立基于空间位置的索引。

步骤3，在自由空间C_free范围内，生成一个用于扩展的采样点q_rand，其中q_rand的生成策略是基于上次扩展碰撞情况而自适应变化的。

步骤4，设T_search中某个节点q_i与q_rand的欧式距离为D(q_rand,q_i)，在T_search中找到一个满足min(D(q_rand,q_i))的节点设为q_nearest，然后让q_nearest朝向q_rand的方向生长一段距离，得到新节点q_new，并暂定它的父节点为q_nearest，之后对(q_new,q_nearest)这段路径进行碰撞检测，若检测不通过，则返回步骤3。其中q_new的生长长度是基于上次扩展碰撞情况而自适应变化的。

步骤5，根据q_new的空间位置，在索引中，获取那些距离q_new较近的T_search节点集合Q_best。在Q_best中，找到代价最小的父节点，即通过此父节点时，使得q_init～q_new的距离最小。将q_new加入T_search。

步骤6，检测D(q_new,q_goal)是否小于特定阈值τ，若D(q_new,q_goal)＜τ则判定规划成功，否则返回步骤3，继续扩展T_search。

步骤7，针对得到的路径(q_init,…,q_i,…,q_goal)，进行优化，以q_init为起点按顺序寻找(q_init,q_s1)可以通过碰撞检测的最远节点q_s1，之后又以q_s1为起点寻找(q_s1,q_s2)可以通过碰撞检测的最远节点q_s2，以此类推直到扩展到q_goal。得到的新路径(q_init,…,q_si,…,q_goal)。

2.如权利要求1，步骤2中所述的T_search的数据结构，其特征为：

其为一个树形结构，其中每个节点应当包含以下信息，当前节点的位置信息(x,y,z)，当前节点到达q_init的距离信息d，当前节点的父节点指针q_f，当前节点的所有子节点指针集合Q_child。

3.如权利要求1，步骤2中所述的T_search中的节点基于空间位置的索引，其特征在于：

使用三维数组进行快速索引，使用的索引键的值为(a,b,c)样式的字符串，而索引中的值为搜索树中节点的子集。

对于搜索空间C_free在三个维度上都有边界(x_min,x_max)、(y_min,y_max)、(z_min,z_max)，对于每一组边界，按需设置一个步长

每隔

设置一个索引边界x_i、y_i、z_i。对于T_search中的任意节点q，设它的空间位置为(x_q,y_q,z_q)，其中x_q∈(x_t,x_t+1]、y_q∈(y_t,y_t+1]、z_q∈(z_t,z_t+1]。那么q应当处于键为(a_q,b_q,c_q)的节点集合中。

其中步长

不应设置过小，否则容易出现大量无效查询。

在查找的过程中，需要根据对应点的空间位置，运算得出对应点的值在三维数组中的真实下标，即索引(a,b,c)。对于其计算方法，有如下公式：

对应节点所在的节点子集就存放在三维数组中下标为(a,b,c)的位置上。

4.如权利要求1，步骤3中所述的扩展采样点q_rand生成策略，其特征在于：

需要有一个概率值p_g，其表示本次扩展不使用随机方式来选取采样点q_rand，而是直接使用q_goal作为本次扩展的q_rand的概率。在开始算法前，应当确定p_g自适应变化的最大值p_gmax和最小值p_gmin，应满足0＜p_gmin＜p_gmax＜1。

而每次搜索树扩展时使用的p_g具体值由以下公式确定：

其中n的值确定方式如下，若搜索树在扩展时，使用q_goal作为本次扩展的q_rand且本次扩展的碰撞检测通过(如权利要求1，步骤4所述)，则n的值加一。若是任意一次碰撞检测不通过，则n的值置0。

5.如权利要求1，步骤4中所述的q_nearest的选取方式，其特征在于：

使用权利要求2中所述的三维数组索引来进行快速查找。

步骤1，设q_rand的空间位置为(x_r,y_r,z_r)，应当有一个索引查询范围ω，且应当满足

对于x_r,y_r,z_r三个数据，分别±ω，从而组合出8个采样点。

步骤2，根据q_rand以及八个采样点，分别获取到它们在索引中对应的键值，之后对这些键值取并集K。之后对集合K中所有键进行查找，得到对应的搜索树子集，并取得节点并集Q。

步骤3，若Q不为空集合，则从Q中获取满足min(D(q_rand,q_i))的节点q_nearest。否则放弃索引，直接从搜索树中查找。

6.如权利要求1，步骤4中所述的q_new的生长长度选取策略，其特征在于：

设置一个最小步长d_min，当本次扩展随机方式来选取采样点q_rand，那么本次生长长度为d_min。

否则本次生长的长度为：

其中n的值确定方式如下，若搜索树在扩展时，使用q_goal作为本次扩展的q_rand且本次扩展的碰撞检测通过(如权利要求1，步骤4所述)，则n的值加一。若是任意一次碰撞检测不通过，则n的值置0。而δ∈(1,+∞)，用于控制自适应变化的速度。

7.如权利要求1，步骤5中所述的节点集合Q_near获取方式，其特征在于：

使用权利要求2中所述的三维数组索引来进行快速查找。

步骤1，设q_new的空间位置为(x_new,y_new,z_new)，应当有一个搜索范围μ，且应当满足

对于x_new,y_new,z_new三个数据，分别±μ，从而组合出8个采样点。

步骤2，根据q_new以及八个采样点，获取到它们在索引中对应的键值，之后对这些键值取并集K。对集合K中所有键进行查找，得到对应的搜索树子集，并取得节点并集Q_best。

Images