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

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

背景技术：

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

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

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

技术实现要素：

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

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

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

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

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

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

对于搜索空间cfree在三个维度上都有边界(xmin，xmax)、(ymin，ymax)、(zmin，zmax)，对于每一组边界，按需设置一个步长每隔设置一个索引边界xi、yi、zi。对于tsearch中的任意节点q，设它的空间位置为(xq，yq，zq)，其中xq∈(xt，xt+1]、yq∈(yt，yt+1]、zq∈(zt，zt+1]。那么q应当处于键为(aq，bq，cq)的节点集合中。

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

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

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

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

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

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

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

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

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

步骤4.1，设qrand的空间位置为(xr，yr，zr)，应当有一个索引查询范围ω，且应当满足对于xr，yr，zr三个数据，分别±ω，从而组合出8个采样点。

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

步骤4.3，若q不为空集合，则从q中获取满足min(d(qrand，qi))的节点qnearest。否则放弃索引，直接从搜索树中查找。

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

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

否则本次生长的长度为：

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

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

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

步骤5.1，设qnew的空间位置为(xnew，ynew，znew)，应当有一个搜索范围μ，且应当满足对于xnew，ynew，znew三个数据，分别±μ，从而组合出8个采样点。

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

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

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

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

附图说明

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

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

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

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

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

具体实施方式

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

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

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

首先应该将无人机飞行的环境cfree以三维地图的形式输入程序，并确定本次寻路的起点qinit和终点qgoal的位置坐标信息。此外，还需要根据环境cfree的特点来设定本次算法的相关参数，其中包括索引中，每一块索引覆盖范围的步长朝向目标扩展概率pg自适应变化的最大值pgmax和最小值pgmin；使用索引查找qnearest的寻找范围ω；每次扩展的最小步长dmin；最优父节点寻找范围μ；用于控制自适应生长长度变化速率的参数δ；判定抵达终点的阈值τ。

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

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

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

将搜索空间cfree在三个维度上的边界(xmin，xmax)、(ymin，ymax)、(zmin，zmax)使用之前确定好的索引覆盖范围的步长进行划分，将cfree划分为若干个空间分区，之后tsearch生成的新节点，都会根据其坐标位置将其加入到对应空间分区的节点子集中去。

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

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

步骤3.2a，若随机数小于pg，那么本次扩展使用qgoal作为qrand。

步骤3.2b，若随机数大于pg，那么本次扩展根据cfree在三个维度上的边界(xmin，xmax)、(ymin，ymax)、(zmin，zmax)，分别以它们为上下边界进行三次随机数生成，设得到的值分别为(xr，yr，zr)，此坐标即为qrand的坐标。

步骤4，找到tsearch中距离qrand最近节点qnearest，其步骤如下：

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

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

步骤4.3a，若q不为空集合，则从q中获取满足min(d(qrand，qi))的节点qnearest。

步骤4.3b，若q为空集合，则直接遍历tsearch的节点集合，从中获取满足min(d(qrand，qi))的节点qnearest。

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

步骤5.1，使用qrand和qnearest的坐标求差值，获取到向量(xr-xn，yr-yn，zr-zn)，计算后设为(xdirection，ydirection，zdirection)，该向量表示了qnearest指向qrand的方向信息。之后根据步骤3中是否使用随机qrand，来选择下一步。

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

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

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

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

更新d的公式如下：

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

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

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

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

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

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

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

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

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

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