CN107450535A - 一种智能运载机器人最优路径混合图论控制规划方法 - Google Patents

Abstract

本发明公开了一种智能运载机器人最优路径混合图论控制规划方法，该方法提出对全局路径进行分层规划的思想，以每个房间或楼道的局部最优路径规划作为最小规划单元，寻找各楼层中的最优枢纽节点，将各个房间或楼道的局部最优路径连通，得到单个楼层的最优路径，最后将楼层之间的最优路径进行连通，得到完整的全局最优路径，分层规划的设计，大大的降低了路径规划过程中的计算量，能够快速实现路径规划。

Description

一种智能运载机器人最优路径混合图论控制规划方法

技术领域

本发明属于机器人路径规划领域，特别涉及一种智能运载机器人最优路径混合图论控制规划方法。

背景技术

随着现代科技的飞速发展，机器人成为了当前研究的热点之一，机器人自动化运输已成为未来重要的发展方向。物流行业、科研行业已经开始使用运载机器人来保证运输质量、提高运输效率。路径规划，作为运载机器人的主要技术难题之一，也随之受到广泛关注。运载机器人的路径规划要求机器人能自主感知周围环境，针对目标任务快速在复杂环境下从起点到终点寻找到一条无碰撞的路径，且路径需要满足安全、距离短、路线合理等多方面要求。

目前，有关运载机器人的路径规划的研究有很多，常用方法主要包括传统路径规划算法和智能仿生路径规划算法。传统算法主要有人工势场法、可视图算法、Floyd算法等，智能仿生算法主要有模拟退火算法、神经网络算法、遗传算法等。传统路径规划算法结构简单，但当路径较多时，算法搜索效率低；智能仿生路径规划算法搜索较快，但易陷入局部最小值。

目前的路径规划算法多用于单一房间的路径规划，环境较为简单，而在复杂楼层环境下，运载机器人要完成在不同房间的运输任务。这要求机器人的路径规划能实现上下电梯、绕过固有障碍物、进出不同房间、进出不同楼道、规避新增静态障碍物、规避新增动态障碍物、路径搜索快速等多项内容。而这种复杂的路径规划要求，直接使用现有的传统路径规划算法或智能仿生路径规划算法是难以实现的。

发明内容

本发明提供了一种智能运载机器人最优路径混合图论控制规划方法，其目的在于，克服上述现有技术中在复杂楼层环境下全局楼层路径点太多、搜索范围太大、机器人难以实现路径搜索的情况。

一种智能运载机器人最优路径混合图论控制规划方法，包括以下步骤：

步骤1：对运载机器人运载区域构建全局地图三维坐标系，获取在全局地图三维坐标系下的可行走区域坐标；

运载区域地面中心点为原点，正东方向为X轴，正北方向为Y轴，垂直地面方向为Z轴；

所述运载机器人运载区域为一个楼栋内所有的地板连通区域，所述可行走区域是指从所有的地板连通区域中删除楼栋内的障碍物所在区域；

在全局地图三维坐标系中，每个楼层的地板连通区域的二维平面坐标相同，z坐标不同；

步骤2：对可行区域中的所有路径点按照楼层进行划分，并对各路径点所在楼层进行标注；

步骤3：筛选楼层枢纽节点以及房间枢纽节点；

所述楼层枢纽节点是指楼层出入口对应的路径点；所述房间枢纽节点是指房间出入口对应的路径点；

楼层枢纽节点包括电梯口、楼道口、楼层门禁口等出入位置对应的路径点；

步骤4：构建各级距离矩阵；

所述各级距离矩阵依次包括一级距离矩阵、二级距离矩阵以及三级距离矩阵；

所述一级距离矩阵是指各房间或楼道中所有路径点之间的距离构成的距离矩阵；

所述二级距离矩阵是指各楼层中房间或楼道的枢纽节点之间的距离构成的距离矩阵；

所述三级距离矩阵是指各楼层枢纽节点之间的空间距离构成的距离矩阵；

步骤5：根据运输任务指令获得运输任务的起始点和终止点在全局地图三维坐标系下的坐标，若起始点与终止点在同一个一级距离矩阵中，则利用对应的距离矩阵寻找局部最优路径，按照局部最优路径执行运输任务，否则，进入步骤6；

步骤6：运载机器人按照运输任务起点和终止点所在楼层逐步调用各级距离矩阵，得到多段在各楼层中的局部最优路径，运载机器人按照楼层顺序根据局部最优路径前行，执行运输任务；

运载机器人按照运输任务起点和终止点所在楼层按照三级距离矩阵寻找最优楼层枢纽节点，再依据楼层枢纽节点按照对应的二级距离矩阵寻找最优楼道或房间枢纽节点，最后依据最优楼道或房间枢纽节点按照对应的一级距离矩阵寻找局部最优路径。

进一步地，采用D*lite算法进行局部最优路径搜索过程如下：

步骤2.1：当待搜索的房间或楼道区域对应的距离矩阵未变化时，在路径点搜索范围内，从目标路径点向起始节点执行D*lite搜索，实现局部最优路径静态搜索；

步骤2.2：当待搜索的房间或楼道区域对应的距离矩阵发生变化时，在路径点搜索范围内，利用D*lite算法通过三角不等式更新该区域的环境路径的有向带权图，对应距离矩阵，依据更新后的距离矩阵，从目标路径点向起始节点执行D*lite搜索，实现局部最优路径静态搜索。

进一步地，采用基于Reach剪枝的D*lite算法进行局部最优路径搜索，过程如下：

步骤3.1：根据Reach剪枝算法，依次以局部搜索范围中所有路径点为根，计算各根的局部最短路径树并得到m个路径点的Reach上界，m的取值范围为[1，10]；

步骤3.2：删除各根对应的局部最短路径树中已计算出Reach上界的顶点，并对各根对应的路径图进行收缩；

步骤3.3：不断迭代地执行步骤(2)和步骤(3)，直到百分之八十以上的路径点的Reach上界都被计算出，剩下路径点的Reach上界定义为无穷大，不被剪枝；

步骤3.4：未被剪枝的路径点构成的区域作为D*lite算法的局部搜索范围。

采用Reach剪枝算法缩小D*lite算法的路径点搜索范围、节省搜索时间。

进一步地，利用TNR算法对所有路径点进行分层标注和归类，并利用TNR算法的局部过滤函数判断运输任务起点和终止点之间可能的最短路径是否需要经过枢纽节点。

进一步地，根据Dijkstra算法搜索得到的起点到终点之间最短路径上的最优枢纽节点，过程如下：

步骤5.1：在无向图G＝(V,E)中，每条边E[i]的长度为w[i]，将无向图中枢纽节点集合V分为两组，第一组为已求出最短路径的枢纽节点集合S，第二组为其余未确定最短路径的枢纽节点集合U；

并且，将起点s和终点t作为虚拟枢纽节点加入枢纽节点集合V中；

步骤5.2：初始化枢纽点集合S和U，令S＝{s}，s的距离为0，U＝{其余枢纽节点}，如果s与U中的枢纽节点u有边，则<u,s>正常有权值，若u不是s的出边邻接点，则<u,s>权值为∞；

步骤5.3：从U中选取一个距离s最小的枢纽节点k，把k加入S中；

步骤5.4：以枢纽节点k为新考虑的中间点，修改U中各枢纽节点的距离，若从起点s到枢纽节点u且经过枢纽节点k之间的距离比原来不经过枢纽节点k的距离短，则修改枢纽节点u的距离值，修改后的距离值为枢纽节点k的距离加上枢纽节点k和枢纽节点u连接边上的权值；

步骤5.5：重复步骤步骤5.3和步骤5.4，直到所有枢纽节点都包含在S中，完成搜索，找到所有的最优枢纽节点。

进一步地，所述运载机器人采用Kinect实现避障，具体过程如下：

步骤A：运载机器人在实际运输任务过程中，保持Kinect开启状态，实时识别路径前方2米内是否出现新增障碍物；

步骤B：如果未出现新增障碍物，运载机器人继续沿局部最优路径行进；

步骤C：如果出现新增障碍物，运载机器人需要判定新增障碍物是否在运动，如果新增障碍物不运动，执行步骤D，如果新增障碍物运动，执行步骤E；

步骤D：若新增障碍物不运动，重新修改局部最优路径，运载机器人沿新的局部最优路径行进；

步骤E：新增障碍物在运动，运载机器人减速运行，同时，通过Kinect判断新增障碍物如何运动，如果新增障碍物未向靠近机器人的方向运动，执行步骤F，如果新增障碍物在向靠近机器人的方向运动，执行步骤G；

步骤F：重新修改局部最优路径，并且机器人沿新的局部最优路径行进；

步骤G：运载机器人停止运动、发出警报声音信号，并再次判断新增障碍物状态，如果新增障碍物远离运载机器人，执行步骤H，如果新增障碍物靠近运载机器人，执行步骤I，如果新增障碍物静止，执行步骤J；

步骤H：等待新增障碍物移动至2米外，运载机器人继续沿局部最优路径行进，并判定是否通过枢纽节点，如果不通过枢纽节点，则到达放物点后，完成运输任务，发送任务完成信号，并返回待命点；

步骤I：运载机器人随机沿远离新增障碍物的路径运动，在不碰撞其他障碍物的前提下，避让新增障碍物，直到远离新增障碍物2米，再重新修改局部最优路径，并且机器人沿新的局部最优路径行进；

步骤J：重新修改局部最优路径，并且机器人沿新的局部最优路径行进。

有益效果

本发明提供了一种智能运载机器人最优路径混合图论控制规划方法，该方法提出对全局路径进行分层规划的思想，以每个房间或楼道的局部最优路径规划作为最小规划单元，寻找各楼层中的最优枢纽节点，将各个房间或楼道的局部最优路径连通，得到单个楼层的最优路径，最后将楼层之间的最优路径进行连通，得到完整的全局最优路径，分层规划的设计，大大的降低了路径规划过程中的计算量，能够快速实现路径规划；

在本发明所述的技术方案中采用TNR算法对全局楼层进行枢纽节点划分，将全局楼层划分为多个局部区域，当机器人跨枢纽节点运输时，先通过Dijkstra算法计算机器人最优枢纽节点路径，再分别计算最优枢纽节点下各局部区域的最优路径；采用Reach剪枝算法优化的D*lite算法来完成局部区域最优路径搜索，加快了局部最优路径搜索速度、实现机器人动态路径规划；针对机器人实际运输过程中，所经路径出现的新增静态或动态障碍物，采用Kinect传感器识别，并采用避障策略和Reach剪枝算法优化的D*lite算法完成最优路径更新。本发明的实施可以有效实现复杂楼层环境下机器人的实时最优路径搜索。

附图说明

图1为本发明所述方法的流程示意图。

具体实施方式

下面将结合附图和实施例对本发明做进一步的说明。

如图1所示，一种智能运载机器人最优路径混合图论控制规划方法，包括以下步骤：

步骤1：对运载机器人运载区域构建全局地图三维坐标系，获取在全局地图三维坐标系下的可行走区域坐标；

运载区域地面中心点为原点，正东方向为X轴，正北方向为Y轴，垂直地面方向为Z轴；

所述运载机器人运载区域为一个楼栋内所有的地板连通区域，所述可行走区域是指从所有的地板连通区域中删除楼栋内的障碍物所在区域；

在全局地图三维坐标系中，每个楼层的地板连通区域的二维平面坐标相同，z坐标不同；步骤2：对可行区域中的所有路径点按照楼层进行划分，并对各路径点所在楼层进行标注，标注采用如下方式：

楼层用F表示、楼道用C表示，房间用R表示，各楼层编号为F1,F2,...，各楼道编号为L1C1,L1C2,...，各楼层内的房间编号为F1R101,F1R102,...，各楼道内路径点编号为L1C1-1,L1C1-2,...，各房间内路径点编号为F1R101-1,F1R101-2,...，以此类推，F1代表1楼，L1C1代表1楼楼道1，R101代表1楼房间101，L1C1-1代表1楼楼道1的编号为1的路径点，F1R101-1代表1楼房间101的编号为1的路径点。

步骤3：筛选楼层枢纽节点以及房间枢纽节点；

采用TNR(Transit-Node Routing)算法对全局路径点进行分层，将编号和分层信息存入远程控制系统和运输机器人。第一层枢纽节点为楼层枢纽节点，第二层枢纽节点为房间枢纽节点。所述楼层枢纽节点是指楼层出入口对应的路径点；所述房间枢纽节点是指房间出入口对应的路径点；

楼层枢纽节点包括电梯口、楼道口、楼层门禁口等出入位置对应的路径点；

步骤4：构建各级距离矩阵；

所述各级距离矩阵依次包括一级距离矩阵、二级距离矩阵以及三级距离矩阵；

所述一级距离矩阵是指各房间或楼道中所有路径点之间的距离构成的距离矩阵；

所述二级距离矩阵是指各楼层中房间或楼道的枢纽节点之间的距离构成的距离矩阵；

所述三级距离矩阵是指各楼层枢纽节点之间的空间距离构成的距离矩阵；

步骤5：运输任务开始，通过远程控制中心的无线通讯模块对运输机器人定位，获得运输任务的起始点和终止点在全局地图三维坐标系下的坐标，并获取机器人此时所处的路径点编号为F1R102-5，以该点作为运输起点。远程控制系统通过无线信号发送命令到运输机器人，终点路径编号为F2R206-1，运输机器人通过TNR(Transit-Node Routing)算法的局部过滤函数判断出起点到终点的可能的最短路径需要经过枢纽节点；

步骤6：运载机器人按照运输任务起点和终止点所在楼层逐步调用各级距离矩阵，得到多段在各楼层中的局部最优路径，运载机器人按照楼层顺序根据局部最优路径前行，执行运输任务；

步骤6.1：运输机器人通过Dijkstra算法按照运输任务起点和终止点所在楼层按照三级距离矩阵寻找最优楼层枢纽节点，再依据楼层枢纽节点按照对应的二级距离矩阵寻找最优楼道或房间枢纽节点，来搜索F1R102-5点到F2R206-1点的最优枢纽节点，得到最优枢纽节点为：起点(F1R102-5)-K2-K20-K21-K27-终点(F2R206-1)，其中K2为R102门口，K20为一楼电梯口，K21为二楼电梯口，K27为R206门口。

Dijkstra算法用来实现起点到终点的最优枢纽节点路径搜索过程如下：

(1)在无向图G＝(V,E)中，每条边E[i]的长度为w[i]，把图中枢纽节点集合V分为两组，第一组为已求出最短路径的枢纽节点集合S，第二组为其余未确定最短路径的枢纽节点集合U，此外，起点s和终点t作为虚拟枢纽节点加入枢纽节点集合；

(2)初始时，S只包含起点s，此时S＝{s}，s的距离为0，U＝{其余枢纽节点}，如果s与U中的枢纽节点u有边，则<u,s>正常有权值，若u不是s的出边邻接点，则<u,s>权值为∞；

(3)从U中选取一个距离s最小的枢纽节点k，把k加入S中；

(4)以枢纽节点k为新考虑的中间点，修改U中各枢纽节点的距离，若从起点s到枢纽节点u的距离(经过枢纽节点k)比原来距离(不经过枢纽节点k)短，则修改枢纽节点u的距离值，修改后的距离值为枢纽节点k的距离加上边上的权；

(5)重复步骤(3)和步骤(4)直到所有枢纽节点都包含在S中，完成搜索，找到最优枢纽节点路径。

步骤6.2：依据最优楼道或房间枢纽节点按照对应的一级距离矩阵寻找局部最优路径，运输机器人需要经过4个局部区域(由于K20-K21为上电梯，不计算局部路径)，各局部区域的起点终点如下：

局部区域1：起点(F1R102-5)-终点(K2)；

局部区域2：起点(K2)-终点(K20)；

局部区域3：起点(K21)-终点(K27)；

局部区域4：起点(K27)-终点(F2R206-1)。

对于各局部区域，分别通过Reach剪枝算法优化的D*lite算法依次搜索各局部最优路径，具体过程如下：

步骤6A：根据Reach剪枝算法，以局部路径的每个路径点为根，计算其局部最短路径树并得到3个路径点的Reach上界；

步骤6B：删除已计算出Reach上界的顶点，对路径图进行收缩；

步骤6C：不断迭代地执行步骤6B、步骤6C，直到大多数路径点的Reach上界都被计算出，剩下路径点的Reach上界定义为无穷大，它们不会被剪枝；

步骤6D：未被剪枝的路径点范围即为D*lite算法搜索范围；

步骤6E：在未被剪枝的路径点范围内，从目标路径点向起始节点执行D*lite搜索；

步骤6F：如果路径发生拓扑变化，如出现新增障碍物，D*lite算法通过三角不等式找到那些变化了的边，然后将这些边的目的节点进行更新，然后重新计算最短路树，在这次计算时，优先搜索那些改变了的顶点，并采用三角不等式进行更新。

步骤7：机器人在局部区域4的起点(K27)-终点(F2R206-1)运输过程中，Kinect发现原最优路径中出现新增静态障碍物，于是机器人通过Reach剪枝算法优化的D*lite算法重新修改局部最优路径，并沿新的最优路径行进，最终到达终点(F2R206-1)，完成运输任务。

本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。

Claims (6)

1.一种智能运载机器人最优路径混合图论控制规划方法，其特征在于，包括以下步骤：

步骤1：对运载机器人运载区域构建全局地图三维坐标系，获取在全局地图三维坐标系下的可行走区域坐标；

运载区域地面中心点为原点，正东方向为X轴，正北方向为Y轴，垂直地面方向为Z轴；

所述运载机器人运载区域为一个楼栋内所有的地板连通区域，所述可行走区域是指从所有的地板连通区域中删除楼栋内的障碍物所在区域；

步骤2：对可行区域中的所有路径点按照楼层进行划分，并对各路径点所在楼层进行标注；

步骤3：筛选楼层枢纽节点以及房间枢纽节点；

所述楼层枢纽节点是指楼层出入口对应的路径点；所述房间枢纽节点是指房间出入口对应的路径点；

步骤4：构建各级距离矩阵；

所述各级距离矩阵依次包括一级距离矩阵、二级距离矩阵以及三级距离矩阵；

所述一级距离矩阵是指各房间或楼道中所有路径点之间的距离构成的距离矩阵；

所述二级距离矩阵是指各楼层中房间或楼道的枢纽节点之间的距离构成的距离矩阵；

所述三级距离矩阵是指各楼层枢纽节点之间的空间距离构成的距离矩阵；

步骤5：根据运输任务指令获得运输任务的起始点和终止点在全局地图三维坐标系下的坐标，若起始点与终止点在同一个一级距离矩阵中，则利用对应的距离矩阵寻找局部最优路径，按照局部最优路径执行运输任务，否则，进入步骤6；

步骤6：运载机器人按照运输任务起点和终止点所在楼层逐步调用各级距离矩阵，得到多段在各楼层中的局部最优路径，运载机器人按照楼层顺序根据局部最优路径前行，执行运输任务；

运载机器人按照运输任务起点和终止点所在楼层按照三级距离矩阵寻找最优楼层枢纽节点，再依据楼层枢纽节点按照对应的二级距离矩阵寻找最优楼道或房间枢纽节点，最后依据最优楼道或房间枢纽节点按照对应的一级距离矩阵寻找局部最优路径。

2.根据权利要求1所述的方法，其特征在于，采用D*lite算法进行局部最优路径搜索过程如下：

步骤2.1：当待搜索的房间或楼道区域对应的距离矩阵未变化时，在路径点搜索范围内，从目标路径点向起始节点执行D*lite搜索，实现局部最优路径静态搜索；

步骤2.2：当待搜索的房间或楼道区域对应的距离矩阵发生变化时，在路径点搜索范围内，利用D*lite算法通过三角不等式更新该区域的环境路径的有向带权图，对应距离矩阵，依据更新后的距离矩阵，从目标路径点向起始节点执行D*lite搜索，实现局部最优路径静态搜索。

3.根据权利要求2所述的方法，其特征在于，采用基于Reach剪枝的D*lite算法进行局部最优路径搜索，过程如下：

步骤3.1：根据Reach剪枝算法，依次以局部搜索范围中所有路径点为根，计算各根的局部最短路径树并得到m个路径点的Reach上界，m的取值范围为[1，10]；

步骤3.2：删除各根对应的局部最短路径树中已计算出Reach上界的顶点，并对各根对应的路径图进行收缩；

步骤3.3：不断迭代地执行步骤(2)和步骤(3)，直到百分之八十以上的路径点的Reach上界都被计算出，剩下路径点的Reach上界定义为无穷大，不被剪枝；

步骤3.4：未被剪枝的路径点构成的区域作为D*lite算法的局部搜索范围。

4.根据权利要求1-3任一项所述的方法，其特征在于，利用TNR算法对所有路径点进行分层标注和归类，并利用TNR算法的局部过滤函数判断运输任务起点和终止点之间可能的最短路径是否需要经过枢纽节点。

5.根据权利要求4所述的方法，其特征在于，根据Dijkstra算法搜索得到的起点到终点之间最短路径上的最优枢纽节点，过程如下：

步骤5.1：在无向图G＝(V,E)中，每条边E[i]的长度为w[i]，将无向图中枢纽节点集合V分为两组，第一组为已求出最短路径的枢纽节点集合S，第二组为其余未确定最短路径的枢纽节点集合U；

并且，将起点s和终点t作为虚拟枢纽节点加入枢纽节点集合V中；

步骤5.2：初始化枢纽点集合S和U，令S＝{s}，s的距离为0，U＝{其余枢纽节点}，如果s与U中的枢纽节点u有边，则<u,s>正常有权值，若u不是s的出边邻接点，则<u,s>权值为∞；

步骤5.3：从U中选取一个距离s最小的枢纽节点k，把k加入S中；

步骤5.4：以枢纽节点k为新考虑的中间点，修改U中各枢纽节点的距离，若从起点s到枢纽节点u且经过枢纽节点k之间的距离比原来不经过枢纽节点k的距离短，则修改枢纽节点u的距离值，修改后的距离值为枢纽节点k的距离加上枢纽节点k和枢纽节点u连接边上的权值；

步骤5.5：重复步骤步骤5.3和步骤5.4，直到所有枢纽节点都包含在S中，完成搜索，找到所有的最优枢纽节点。

6.根据权利要求5所述的方法，其特征在于，所述运载机器人采用Kinect实现避障，具体过程如下：

步骤A：运载机器人在实际运输任务过程中，保持Kinect开启状态，实时识别路径前方2米内是否出现新增障碍物；

步骤B：如果未出现新增障碍物，运载机器人继续沿局部最优路径行进；

步骤C：如果出现新增障碍物，运载机器人需要判定新增障碍物是否在运动，如果新增障碍物不运动，执行步骤D，如果新增障碍物运动，执行步骤E；

步骤D：若新增障碍物不运动，重新修改局部最优路径，运载机器人沿新的局部最优路径行进；

步骤E：新增障碍物在运动，运载机器人减速运行，同时，通过Kinect判断新增障碍物如何运动，如果新增障碍物未向靠近机器人的方向运动，执行步骤F，如果新增障碍物在向靠近机器人的方向运动，执行步骤G；

步骤F：重新修改局部最优路径，并且机器人沿新的局部最优路径行进；

步骤G：运载机器人停止运动、发出警报声音信号，并再次判断新增障碍物状态，如果新增障碍物远离运载机器人，执行步骤H，如果新增障碍物靠近运载机器人，执行步骤I，如果新增障碍物静止，执行步骤J；

步骤H：等待新增障碍物移动至2米外，运载机器人继续沿局部最优路径行进，并判定是否通过枢纽节点，如果不通过枢纽节点，则到达放物点后，完成运输任务，发送任务完成信号，并返回待命点；

步骤I：运载机器人随机沿远离新增障碍物的路径运动，在不碰撞其他障碍物的前提下，避让新增障碍物，直到远离新增障碍物2米，再重新修改局部最优路径，并且机器人沿新的局部最优路径行进；

步骤J：重新修改局部最优路径，并且机器人沿新的局部最优路径行进。