一种移动机器人混合路径规划方法与流程

本发明属于机器人的控制技术领域，特别涉及一种移动机器人混合路径规划方法。

背景技术：

机器人技术发展水平是衡量一个国家综合国力的一个重要因素。目前许多国家已将机器人技术列入本国高科技发展计划，其发展也呈现出两个突出特点：一方面，其应用领域不断扩大，种类也日益繁多，正逐渐成为人类改造世界的有力工具；另一方面，随着机器人相关技术的不断发展，人工智能及仿生学等前沿学科的逐渐融入，机器人的交互性和安全性在稳步提高，与人类活动密切也变得日益密切。

如今，我们正处于机器人技术革命的浪潮。已经开发了各种机器人系统，它们在执行不同类型的任务方面表现出了有效性和高效性，包括智能家居环境、机场、购物中心、制造实验室。将智能嵌入到移动机器人中以确保所考虑的任务得到(接近)最优地方案，并有效地完成任务。然而，目前仍然有大量的研究问题需要解决，例如路径规划问题，这是移动机器人可以在复杂环境中自主导航和探索之前，这是必须解决的最基本问题之一。

路径规划问题可以描述为：在静态和动态障碍物环境、模型约束以及不确定性的情况下，移动机器人根据特定的性能标准搜索从初始状态到目标状态的最佳或次优路径。路径规划问题中需要考虑三个主要问题：效率，准确性和安全性。任何机器人都应该在较短的时间内找到可行路径，同时需要消耗最少的能量。除此之外，机器人应该还必须遵循满足最优性能指标的移动准则，安全地避开环境中存在的障碍物。

移动机器人路径规划基本上依赖于现有的地图作为识别初始和目标位置以及它们之间的联系的参考，所以根据环境信息的已知程度可以将路径规划算法分为全局路径规划和局部路径规划。目前，无论是全局路径规划还是局部路径规划，在不同的应用场合都会存在或多或少的局限性，而且还没有研究出通用的算法或方法可以解决所有的路径规划情况。

技术实现要素：

本发明提供一种移动机器人混合路径规划方法，将全局和局部路径规划进行有机结合，解决现有技术中的问题。

为了达到本发明的目的，本发明提供的技术方案是：

一种移动机器人混合路径规划方法，首先构建全局地图，在全局地图的基础上以快速探索随机树算法和时间弹性带算法相结合进行混合路径规划。

进一步的，其具体规划流程如下：

步骤1、构建全局地图：建立移动机器人工作空间的环境模型，发布移动机器人的初始状态和目标位姿；

步骤2、使用快速探索随机树算法规划出一条全局静态最优路径；

步骤3、添加满足运动学和动力学约束的时间信息，将移动机器人局部范围内的初始路径转换成一条时间弹性带；

步骤4、更新时间弹性带算法的配置参数，关联导航点或障碍物信息，以调整运动轨迹和规划范围；

步骤5、将时间弹性带算法的优化函数映射生成超图，使用通用图优化框架中稀疏系统的大规模优化算法优化超图，得到最优的系统输入参数速度v和角速度w；

步骤6、获取的速度命令发送给移动机器人，使机器人进行运动，重复步骤3到步骤6，直到移动机器人到达目标位姿。

进一步的，步骤1中，构建全局地图的具体步骤如下：

首先，利用空中视觉采集环境信息，然后利用包括中值滤波、形态学去噪和图像校正方式进行图像预处理；其次，使用图像分割和连通域检测的方法识别障碍物区域，最后运用栅格法构建出全局地图。

与现有技术相比，本发明的优点是：

本发明将全局路径规划和局部路径规划两类方法混合，以达到在约束条件下的相互弥补，提高路径规划解决方案的质量和效率；本发明方法相比于单一规划方法，可以快速规划全局路径，并实现动态规划和避障功能，保证了移动机器人运动过程中的实时性和安全性。

附图说明

图1为构建全局地图的基本流程；

图2为室内环境和构建出的全局地图；

图3为rrt优化算法基本流程；其中，图3（a）为连通区域偏向采样，图3（b）为扩大连通区域，图3（c）为寻找可行路径，图3（d）为路径修剪；

图4为障碍物膨胀模型建立示意图；

图5为混合算法流程图；

图6为rrt和teb混合算法仿真规划；

图7为rrt和teb混合算法实际实验；其中，图7（a）为无动态障碍物；图7（b）为添加临时障碍物。

具体实施方式

下面通过具体实施方式结合附图对本发明作进一步详细说明。说明书中所描述的特点、操作或者特征可以以任意适当的方式结合形成各种实施方式。同时，方法描述中的各步骤或者动作也可以按照本领域技术人员所能显而易见的方式进行顺序调换或调整。因此，说明书和附图中的各种顺序只是为了清楚描述某一个实施例，并不意味着是必须的顺序，除非另有说明其中某个顺序是必须遵循的。

本发明的一种移动机器人混合路径规划方法，该方法主要包括构建全局地图、全局路径规划和局部路径规划。

在构建全局地图部分，由于光照变化、移动机器人视角变化或操作人员操作失误，利用slam技术构建全局地图有时会出现障碍物识别不全、构建地图不完整等问题，本发明使用空中视觉采集室内环境信息，然后利用图像分割和连通域检测识别障碍物区域，最终运用栅格法构建二维空间的环境模型。

在全局路径规划部分，针对快速探索随机树算法（rrt）收敛速度缓慢，采样节点密集，路径曲折复杂等问题，提出了一种新的改进rrt离线算法。该方法使用连通区域、节点拒绝和路径修剪三种策略来实现路径优化。

在局部路径规划部分，通过研究时间弹性带算法（teb）的工作原理、运动学模型和约束函数，使用障碍物膨胀处理和状态分级策略保证移动机器人运动过程中可以及时有效地避开障碍物，从而提高安全性。

本发明使用空中视觉采集室内环境信息，然后利用图像分割和连通域检测识别障碍物区域，最终运用栅格法构建二维空间的环境模型。构建全局地图的基本流程如图1所示。

首先，利用空中视觉采集室内的环境信息，然后利用中值滤波、形态学去噪和图像校正等方式进行图像预处理；其次，使用图像分割和连通域检测的方法识别障碍物区域，最后运用栅格法构建出全局地图，如图2所示。

得到全局地图后，利用快速探索随机树算法得到一条全局静态最优路径。全局路径规划需要考虑的因素有：

可行性：可以规划出从起始点到目标点的一条可行路径；

最优性：可以优化出一条具有最小路径成本的最优路径；

快速性：保证较快的收敛速度，减少规划的执行时间。

rrt算法是通过随机探索自由空间，并构建一棵从初始状态开始寻找朝向目标状态的可行路径的树。在迭代的过程中，创建随机点并搜索距离该点最近的顶点，然后以固定步长得到该方向上的新节点，并检查其是否属于自由空间，直至达到目标。虽然rrt算法具有概率完备性，但生成的路径往往不是最优的并且需要进行平滑优化。

针对rrt算法收敛速度缓慢，采样节点密集，路径曲折复杂等问题，本发明采用连通区域、节点拒绝和路径修剪三种策略进行优化。它以起始点为根来初始化探索树，先确定起始点和目标点之间的连通区域，然后随机选择连通区域中的节点进行偏向采样，找到可行的初始路径。同时对初始路径使用节点拒绝技术，去除掉高成本节点和无效节点，然后进行原始路径修剪，获得一条长度更小，节点更少的优化路径，具体过程如图3所示。

当环境中存在动态障碍物情况时，移动机器人需要进入局部规划状态，通过更新的局部地图和全局导航点，局部规划生成动态障碍物的避免策略，并尽可能地将轨迹与全局规划器提供的导航点相匹配。局部路径规划的具体实现步骤如下：

1）添加满足运动学和动力学约束的时间信息，将移动机器人局部范围内的初始路径转换成一条时间弹性带；

2）更新时间弹性带算法（teb）配置参数，关联导航点或障碍物信息，以调整运动轨迹和规划范围；

3）将teb优化函数映射生成超图（hyper-graph），使用g2o框架中稀疏系统的大规模优化算法优化超图，得到最优的系统输入参数速度v和角速度w；

4）获取的速度命令发送给移动机器人，使机器人进行运动，重复步骤1到步骤4，直到移动机器人到达目标位姿。

上述步骤中，主要是对出现的动态障碍物的检测和局部动态重规划，而对障碍物的检测是通过传感器获取障碍物信息完成的。为了保证移动机器人运行过程中的安全性，本发明提出障碍物膨胀处理和状态分级策略。

障碍物膨胀处理就是约束一个机器人到障碍物的最小距离，然后以该最小距离对在全局障碍物进行膨胀，如图4所示。状态分级策略是将机器人运动过程中可能会出现的状态分为安全状态、潜在危险状态和危险状态。

（1）安全状态：窗口中的模拟轨迹未进入障碍物的膨胀区域，即轨迹前端位于外圈以外的区域；

（2）潜在危险状态：窗口中的模拟轨迹进入障碍物的膨胀区域，但未达到轨迹到障碍物的最小距离，即轨迹前端位于外圈与内圈之间的区域；

（3）危险状态：模拟轨迹到障碍物之间的距离小于最小距离或模拟轨迹触及到障碍物本身，即轨迹前端位于内圈以内的区域。

根据图5所示混合算法流程图，最后在基于ros的移动机器人实验平台上使用rrt和teb混合的路径规划算法完成了室内环境的路径规划任务，避开临时障碍物，顺利到达目标点。如图6和图7所示。

以上应用了具体个例对本发明进行阐述，只是用于帮助理解本发明，并不用以限制本发明。对于本发明所属技术领域的技术人员，依据本发明的思想，还可以做出若干简单推演、变形或替换。