一种非均匀采样FMT*的移动机器人路径规划方法

技术领域

本发明涉及移动机器人路径规划技术领域，尤其是一种非均匀采样FMT*的移动机器人路径规划方法。

背景技术

现如今智能机器人为人类提供的服务都将自动导航作为机器人的重要功能之一，如自动驾驶、空中无人机和服务机器人等领域。而路径规划作为其导航的主要部分之一，旨在让对象在规定的运行空间范围内找到一条从起始目标到终止目标的无碰撞安全路径，即为机器人在复杂环境中运动寻找无碰撞路径，同时要优化机器人的运动路径，使其尽可能达到更短、更平滑的要求。因此，智能机器人和无人操作技术的发展在很大程度上取决于能否更快地为运动规划问题提供切实有效的方案。

路径规划发展迅速，已产生许多规划算法。基于采样的算法，如快速探索随机树(RRT)，概率路线图(PRM)，以及它们的变体，因其能够有效地解决约束动力学和高维空间规划问题而越来越受欢迎。在许多应用领域，除了生成可行的解决方案路径之外，还需要获得高质量的求解路径，因此具有渐近最优性的方法，如RRT*和PRM*被提出，使得采样算法成为应用最广泛的算法之一，然而，采样算法的一个局限性是它们收敛到最优解的速度缓慢，需要大量的迭代过程或样本，渐近地找到规划问题的最优解。在实际应用中，如无人机或移动机器人，为了节省时间或电源，需要快速计算一条较短的路径来确保工程的实施。

样本偏置是一种常见的方法，方法试图通过偏置手段将采样点分布在有利的区域来进行探索。例如，Rodriguez,S.等人提出的Obstacle-Based RRT或者K.Cao等人提出的基于混合偏置的PRM是利用障碍物的信息，或在障碍物附近增加采样概率来指导采样过程，主要应用于环境存在狭窄通道的问题，但这只提高了计算效率，确保算法的成功率。X.Zhao等人提出的混合高斯模型进行偏差采样，可以有效地利用局部空间，同时保持全局路径规划的效率。结合启发式的样本偏置控制采样区域，对样本进行启发式估计进行筛选；例如A.Yershova andL.Jaillet等人的Dynamic-Domain RRTs及其自适应版本通过在障碍物近处形成小区域来进行局部采样，指导算法摆脱陷阱问题，然而，RRT的使用意味着该解决方案几乎肯定是次优。

基于局部采样对配置空间中的有效轨迹进行采样也是常见方法，InformedAnytime Fast Marching Tree(IAFMT*)根据寻找到的初始路径来构造椭圆启发式区域，一旦找到初始解决方案，算法将在区域内继续迭代或采样，这增加了改进当前路径的概率，并收缩区域范围，从而收敛路径到最优，能够有效的提高FMT*算法的收敛速度，但当路径弯曲复杂时，形成的椭圆区域大于地图，那么区域无法收缩导致无法起到提速的作用。基于Motion Planning using Lower Bounds(MPLB)[26]的FMT*上，通过求解一系列独立的问题来实现，并且只有在子问题解决后才能返回更好的解，同时减少最近邻查询来减少时间的损耗，提高算法收敛速度。Safety certificates利用每个采样点与障碍物形成的自由区域，来减少区域中样本的碰撞检测次数，随着样本覆盖整个空间，碰撞检测次数将为零，从而提高算法的计算效率。

路径偏置试图通过围绕当前解决方案路径进行采样来增加路径改善的概率，例如RRT*-Smart将路径偏置与平滑相结合，当找到解决方案时，RRT*-Smart首先平滑并将路径减少到其最小状态数，然后使用这些状态作为进一步采样的偏差。这增加了路径平滑算法的复杂性，同时仍然需要全局采样以避免局部最优；双树结构的RRT*算法使用路径偏置一旦找到初始解决方案，该算法将花费迭代百分比来细化当前路径。但当前解如果与最优解非同伦，则偏置算法需要结合全局采样来避免局部最优。因此这些方法虽然可以提高算法在某些特定环境下的收敛速度，但并不具备普遍性，有时由于启发式算法也可能误导规划性能并降低规划性能，减少算法的收敛速度。

通过初始化机器人的工作空间来引导算法进行更直接的探索，以此来提高其收敛到解的速度。例如exploring/exploiting tree(EET)通过波前扩展，在地图中构建一条球体树通道，然后在通道中进行RRT探索，减少了对多余空间的探索时间，但通道的建立过程无法保证该通道所在区域是最优的，并且RRT也是非优的。Transition-based RRT*(T-RRT*)将地图化为类似山地的成本图，勘探有利于山谷和鞍点的扩张，这些地方连接到低成本地区，引导算法走向有利位置，提高RRT*的收敛速度。

发明内容

本发明提出一种非均匀采样FMT*的移动机器人路径规划方法，克服了现有路径规划算法FMT*的不足，其算法收敛速度快于FMT*，能快速得到最优解，并且在单查询规划中仍具有优势。

本发明采用以下技术方案。

一种非均匀采样FMT*的移动机器人路径规划方法，包括如下步骤：

步骤S1、地图初始化，其方法为：将环境中所有的已知障碍物用均匀分布的点集合进行离散化表示，并假设障碍物数量为k，组成集合{C

步骤S2、利用增量构造方法建立广义Voronoi图，即GVG，由广义Voronoi边和顶点组成；

步骤S3、寻找初始化路径，其方法为：设置机器人的初始位置、目标位置，在路线图GVG中寻找初始路径，并离散化路径，实现最优路径与初始路径同伦；

步骤S4、安全隧道建立，其方法为通过GVG建立过程得到距离障碍物的距离d，将其作为半径R，并以圆心在路径离散点，半径R的连续圆形区域，建立覆盖初始路径的安全隧道；

步骤S5、非均匀采样，其方法为：设置总采样数N，在安全隧道内进行随机采样，得到采样集合V

步骤S6、构造采样集合V

步骤S7、寻找探索集合V

步骤S8、对于每个点x∈X

步骤S9、计算所有点y∈Y

步骤S10、判断y

步骤S11、探索完X

步骤S12、判断Z是否是目标点，若是则返回路径解，若不是则返回步骤S7继续；

步骤S13、机器人接收到算法规划的可行路径解，并把可行路径解中的路径转化为所需线速度和角速度，控制机器人移动到目标位置，路径规划结束。

所述步骤S2具体实现方式为：距离d

若点x周围有许多障碍物，则定义x到环境的距离为

所述GVG的基本组成部分是每个点到障碍物C

由公式一获得的所有等距边的交汇点为广义Voronoi顶点；进一步地，GVG由广义Voronoi边和顶点组成；每个顶点和Voronoi边都保留距离d，GVG形成的路线图分布在所有的障碍物周围，并且保证路线图中的任意两点之间存在一条有GVG边缘和交汇点形成的路径将两点连接。

所述步骤S3的寻找初始化路径，将采用A*最优图搜索方法在GVG中寻找当前最优解，根据GVG的连通性，当前解将与全局最优路径互为同伦解。

所述步骤S4的安全隧道建立，方法为通过连续圆形连接的通道覆盖初始路径的同伦区域；所述通道的覆盖率与初始路径的离散化程度有关，离散化越大，则圆形通道越稀疏，路径收敛效果越差，离散化越小，则通道覆盖越全面，路径收敛效果越好，但会使计算速度下降；

所述规划方法可通过提高离散化程度来加快计算速度，以快速为机器人提供路径解。

所述步骤S5的非均匀采样是在安全隧道内进行的均匀随机采样过程，所述采样过程不进行碰撞检测以促使样本均匀分布在路径周围。

所述步骤S9为FMT*规划过程，FMT*在集合中的已分布的样本上进行树扩展搜索，通过样本点计算找出代价最小点；

所述样本点计算中，计算得出已探索树中所有点中到该点的最小代价的点，通过一次连接到最短路径，避免了重新连接的过程，并且直接判断最后连接边的碰撞检测，减少了检测次数，提高计算效率；

所述样本点计算执行至机器人路径的目标点时，算法结束并输出最优路径。

相较于现有技术，本发明具有以下有益效果：

相比于FMT*、PRM*、RRT、RRT*、Informed-RRT*算法，本发明提出的算法有如下优势：

(1)本发明提出一种结合GVG预处理和安全隧道建立的非均匀采样算法，作为一种元算法，可作为预处理方法结合任何基于采样的规划算法，提升其收敛性能，适用性强，是具有完备性和渐进最优性的算法；

(2)收敛到最优的效率是FMT*和PRM*等渐进最优算法的主要性能，本发明通过预处理将样本重点布置在最优路径所在区域，能够让算法快速的从中捕获最优路径解，对于多查询算法，一次预处理即可为后续任意目标进行规划；

(3)对于大多规划算法而言，碰撞检测所花费的计算时间是较长的，包括样本的碰撞检测，环境越复杂算法效率越低。本发明通过构建安全隧道来省略了碰撞检测，在隧道内的样本采样都是无碰撞的，可以提高算法计算效率。

附图说明

下面结合附图和具体实施方式对本发明进一步详细的说明：

附图1是本发明的流程示意图；

附图2是本发明的算法原理说明示意图；

附图3是本发明的FMT*探索示意图；

附图4是本发明的GVG组成示意图；

附图5是本发明的安全隧道示意图；

附图6是本发明的静态场景一仿真结果示意图；

附图7是本发明的静态场景二仿真结果示意图；

附图8是本发明的静态场景三仿真结果示意图；

附图9是本发明的静态场景四仿真结果示意图；

附图10是本发明的静态真实实验示意图。

具体实施方式

如图所示，一种非均匀采样FMT*的移动机器人路径规划方法，包括如下步骤：

步骤S1、地图初始化，其方法为：将环境中所有的已知障碍物用均匀分布的点集合进行离散化表示，并假设障碍物数量为k，组成集合{C

步骤S2、利用增量构造方法建立广义Voronoi图，即GVG，由广义Voronoi边和顶点组成；如图2中的a区域所示；

步骤S3、寻找初始化路径，其方法为：设置机器人的初始位置、目标位置，在路线图GVG中寻找初始路径，并离散化路径，实现最优路径与初始路径同伦；如图2中的b区域所示；

步骤S4、安全隧道建立，其方法为通过GVG建立过程得到距离障碍物的距离d，将其作为半径R，并以圆心在路径离散点，半径R的连续圆形区域，建立覆盖初始路径的安全隧道；如图2中的c区域所示；

步骤S5、非均匀采样，其方法为：设置总采样数N，在安全隧道内进行随机采样，得到采样集合V

步骤S6、构造采样集合V

步骤S7、寻找探索集合V

步骤S8、对于每个点x∈X

步骤S9、计算所有点y∈Y

步骤S10、判断y

步骤S11、探索完X

步骤S12、判断Z是否是目标点，若是则返回路径解，如图2中的d区域所示；若不是则返回步骤S7继续；

步骤S13、机器人接收到算法规划的可行路径解，并把可行路径解中的路径转化为所需线速度和角速度，控制机器人移动到目标位置，路径规划结束。

本发明的具体流程图如图1所示。

所述步骤S2具体实现方式为：距离d

若点x周围有许多障碍物，则定义x到环境的距离为

所述GVG的基本组成部分是每个点到障碍物C

由公式一获得的所有等距边的交汇点为广义Voronoi顶点；进一步地，GVG由广义Voronoi边和顶点组成；每个顶点和Voronoi边都保留距离d，GVG形成的路线图分布在所有的障碍物周围，并且保证路线图中的任意两点之间存在一条有GVG边缘和交汇点形成的路径将两点连接。

所述步骤S3的寻找初始化路径，将采用A*最优图搜索方法在GVG中寻找当前最优解，根据GVG的连通性，当前解将与全局最优路径互为同伦解。

所述步骤S4的安全隧道建立，方法为通过连续圆形连接的通道覆盖初始路径的同伦区域；所述通道的覆盖率与初始路径的离散化程度有关，离散化越大，则圆形通道越稀疏，路径收敛效果越差，离散化越小，则通道覆盖越全面，路径收敛效果越好，但会使计算速度下降；

所述规划方法可通过提高离散化程度来加快计算速度，以快速为机器人提供路径解，如图5所示。

所述步骤S5的非均匀采样是在安全隧道内进行的均匀随机采样过程，所述采样过程不进行碰撞检测以促使样本均匀分布在路径周围。

所述步骤S9为FMT*规划过程，FMT*在集合中的已分布的样本上进行树扩展搜索，通过样本点计算找出代价最小点；

所述样本点计算中，计算得出已探索树中所有点中到该点的最小代价的点，通过一次连接到最短路径，避免了重新连接的过程，并且直接判断最后连接边的碰撞检测，减少了检测次数，提高计算效率；

所述样本点计算执行至机器人路径的目标点时，算法结束并输出最优路径如图3中的b区域所示。

实施例：

本实施例中，采用的算法的完整伪代码，具体如下：

其中，Algorithm 1中的第1、2、3为本发明采用的改进的RRT*算法相比于原来的FMT*算法的改进部分。

以下用具体的实验对本发明的实施方式进行详细说明，本发明提供一种完备且渐进最优的运动规划算法，主要通过仿真实验和真实实验来验证其有效性。具体实验设置如下：

仿真实验：

仿真实验在MATLAB R2016b软件中进行。

(1)静态场景一

仿真静态场景地图大小为X＝Y＝500m，所有障碍物皆为静态障碍物。起始点为(250，450)，目标点为(250，40)，初始路径离散化程度为50％。如图6所示，为静态场景一的仿真地图，S点表示起始点，G表示目标点，青色表示已探索的样本，红色线表示节点数为2000时本发明提出的算法规划的路径。

(2)静态场景二

仿真静态场景地图大小为X＝Y＝500m，所有障碍物皆为静态障碍物。起始点为(50，460)，目标点为(460，40)，初始路径离散化程度为50％。如图7所示，为静态场景二的仿真地图，S点表示起始点，G表示目标点，青色表示已探索的样本，红色线表示节点数为3000时本发明提出的算法规划的路径。

(3)静态场景三

仿真静态场景地图大小为X＝Y＝500m，所有障碍物皆为静态障碍物。起始点为(250，170)，目标点为(290，460)，初始路径离散化程度为50％。如图8所示，为静态场景三的仿真地图，S点表示起始点，G表示目标点，青色表示已探索的样本，红色线表示节点数为3000时本发明提出的算法规划的路径。

(4)静态场景四

仿真静态场景地图大小为X＝Y＝500m，所有障碍物皆为静态障碍物。起始点为(430，50)，目标点为(80，460)，初始路径离散化程度为50％。如图9所示，为静态场景四的仿真地图，S点表示起始点，G表示目标点，青色表示已探索的样本，红色线表示节点数为5000时本发明提出的算法规划的路径。

真实实验：

本发明可以直接用于移动机器人的路径规划、机械臂的运动规划还有无人机的轨迹规划中，在ROS操作系统中编写一个新的全局路径规划器并将其作为插件添加到ROS操作系统中，使用时通过move_base包调用即可。

静态真实实验环境为Ubuntu16.04、ROS Kinetic，使用搭载Kinect深度相机的Turtlebot2及Intel Core i7-6700HQ，内存为8GB的联想笔记本电脑。场景地图大小为X＝Y＝10m，所有障碍物皆为已知静态障碍物。起始点为(9.5，0.5)，目标点为(1.5，0.5)，初始路径离散化程度为50％。如图10所示，为静态真实实验过程，左侧图为实验过程实拍，右侧图为实验过程中本发明提出的算法规划路径时Rviz可视化界面，绿色表示规划得到的最优路径。

以上所述仅为本发明的较佳实施例，对于本领域的普通技术人员而言，根据本发明的教导，设计出不同形式的运动规划算法并不需要创造性的劳动，在不脱离本发明的原理和精神的情况下凡依本发明申请专利范围所做的均等变化、修改、替换和变型，皆应属本发明的涵盖范围。