基于已知环境下的移动机器人路径规划方法
【专利摘要】本发明涉及基于已知环境下的移动机器人路径规划方法,包括以下步骤:1、记录点坐标和转向角;2、生成地图拓扑图;3、计算并填充有向图;4、设置点的数组;5、遍历各点;6、用Floyd算法计算最短路径矩阵。本发明提出一种简单可靠的已知环境下的机器人路径规划方法,本方法计算量小,规划算法快速可靠,并可以实现多机器人单一轨道的路径规划,尤其适用在多机器人、单行道条件下的机器人协同工作。
【专利说明】
基于已知环境下的移动机器人路径规划方法
技术领域
[0001]本发明涉及机器人控制方法技术领域,具体的说是基于已知环境下的移动机器人路径规划方法。
【背景技术】
[0002]目前,依靠StarGazer等传感器来实现机器人自主定位与导航,已经成为目前各大主流服务机器人厂商的选择,其原理是StarGazer传感器通过不断的读取顶标并计算出机器人所在位置,来实现的定位,通过控制中心,判断并指定机器人下一步需要到达的位置,最终来实现机器人导航。其中,通过对已知环境下的机器人进行路径规划与导航,一直是一个难以解决的问题。即机器人到达某一个指定点选择一条可靠的最短路径,而且实际情况下,此路径往往都只是单行道,即一条路径下只允许一个机器人的行走,当有多个机器人的时候,这种情况尤为难以解决。
[0003]目前,各大厂商利用的主要方法为基于Dijkstra的扩展,主要是求取两点之间的最短路径,并结合栅格法,可以实现机器人沿着最短线路行走。但目前主要缺点是机器人算法复杂,计算量大,无法动态调整已经规划好的路线,尤其无法解决多机器人的路径规划和动态调整。
【发明内容】
[0004]为解决上述问题,本发明提供一种基于已知环境下的移动机器人路径规划方法。
[0005]—种基于已知环境下的移动机器人路径规划方法,包括以下步骤:
[0006]第一步,采集坐标:需要获取整个地图的全局坐标,建立好全局坐标系后,需要让机器人在整个空间走动一遍,并记录机器人需要经过的点,记录点的相应坐标,以及移动到下一点需要的转向角,如果无法采集转向角,贝lJ采用计算角,计算角需要加上一定的偏移量。
[0007]第二步,生成地图拓扑图:利用对应关系,通过转换实际坐标系到屏幕坐标系,将相应的点生成对应的地图拓扑图,以判断相关点的对应关系是否正确,以及判断拓扑结构是否合理。
[0008]第三步,计算并填充有向图:将步骤二中的对应关系整理成数据结构中的有向图;有向图是数据结构中的一种,具体包含一个二维数组,数组中的每个元素是一个包含两点之间的权值和角度的结构体,权值为两点间计算的长度;有向图结构中包含两个整形变量,一个变量用于存储有向图中点的个数,另一个用于存储有向图中边的个数;有向图结构中包含一个一维数组,用以校正对应的点信息,数组的容量为有向图中点的个数。
[0009]第四步,设置点的数组:设置一个点前驱的二维数组,一个最短距离的二维数组,一个点的终点角度数组,数组大小即为点的大小。点前驱的二维数组用来记录到这个点的最短路径的前一个应该到达的点,最短距离的二维数组是存储两点之间的经过最短路径所经过的距离,点的终点角度数组需要存储点作为终点或者停止点的角度。
[0010]第五步,遍历各点:初始化各变量,遍历之前记录的点的拓扑图,如果两点之间存在连接关系,将此两点之间的距离信息设置为计算距离值,角度信息填写为对应角度,即转向角度。若两点之间没有连接关系,设置为整形最大值,其中,由于是双向图,所以图的矩阵非对称矩阵,矩阵下标相等点为值O。
[0011]第六步,用Floyd算法计算最短路径矩阵:利用Floyd算法计算出两点之间的最短路径,其原理是遍历任意两个点,从图的带权邻接矩阵A= [a(i,j)]开始,递归地进行η次更新,即由矩阵D(O)=A,按一个公式,构造出矩阵D(I);又用同样地公式由D(I)构造出D
(2);……;最后又用同样的公式由D(n-l)构造出矩阵D(n)。矩阵D(n)的i行j列元素便是i号顶点到j号顶点的最短路径长度,称D(n)为图的距离矩阵,同时还可引入一个后继节点矩阵path来记录两点间的最短路径,前驱二维数组存储前驱点坐标,距离二维数据存储最短距离。
[0012]当有机器人请求给定终点进行路径查询时,首先遍历所有点,找出离起始点最近的采集点和终点最近的采集点,由前驱数组来找出从起始采集点O到终点N前的一个点M,再由前驱数组找出从起点到M点前驱点L,……,一直到查询到前驱点为起始采集点O时停止。输出所有经过的路径,发送到机器人让机器人执行。
[0013]当机器人开始启动执行时,首先将所有经过路段“加锁”,即将双向图中反方向路径权值加上一个最大整形(如65535),每当运行到一个点,将运行过的点进行“解锁”,即将两点之间的权值减去最大整形,这样保证其他机器人每次检索出来是一个相对的最端路径,而且这条路径不会有其他机器人沿着这条路径反方向行走,当其他机器人查找路径时,会让机器人选择绕行,而不是按原路返回,这样在单行道场合完成最大化使用效率,满足多机器人的单行道路径规划。
[0014]本发明的有益效果是:
[0015]本发明提出一种简单可靠的已知环境下的机器人路径规划方法,本方法计算量小,规划算法快速可靠,并可以实现多机器人单一轨道的路径规划,尤其适用在多机器人、单行道条件下的机器人协同工作。
【附图说明】
[0016]下面结合附图和实施例对本发明进一步说明。
[0017]图1为本发明的控制流程图;
[0018]图2为本发明的路径展示图。
【具体实施方式】
[0019]为了使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解,下面对本发明进一步阐述。
[0020]如图1、图2所示,基于已知环境下的移动机器人路径规划方法,包括以下步骤:首先,需要利用StarGazer传感器建立整个活动空间的全局坐标,具体建立过程详情参考StarGazer传感器手册。本发明主要是利用此传感器获得整个地图的全局坐标,但不仅仅局限于StarGazer传感器。
[0021]建立好全局坐标系后,让机器人在整个空间走动一遍,并记录机器人需要经过的点,这些点主要是机器人需要转换角度或者停止的点。记录点的相应坐标,以及移动到下一点需要转动的角度。记录方式为1_{3,103;5,254;...},表示点I与点3,点5等连接,从点I往点3移动时转向角度为103度,从点3往点5移动时的转向角度为254度等,点的命名从O开始,只需记录点小5IJ点大的点,例如只需记录I点到3点和5点的连接关系和角度,而3点和5点至IJI点的相关信息并不需要记录。本发明可以利用安装在机器人上的传感器记录并输出相应连接角度,也可以通过计算得到相应的连接角度,但需要根据实际场景添加一定的角度偏移量。
[0022]第二步,利用对应关系,通过转换实际坐标系到屏幕坐标系,将相应的点生成对应的地图拓扑图,可以很直观的判断相关点的对应关系是否正确,以及判断拓扑结构是否合理。
[0023]第三步,计算并填充有向图:将步骤二中的对应关系整理成数据结构中的有向图;有向图是数据结构中的一种,具体包含一个二维数组,数组中的每个元素是一个包含两点之间的权值和角度的结构体,权值为两点间计算的长度;有向图结构中包含两个整形变量,一个变量用于存储有向图中点的个数,另一个用于存储有向图中边的个数;有向图结构中包含一个一维数组,用以校正对应的点信息,数组的容量为有向图中点的个数。
[0024]第四步,准备一个点前驱的二维数组、一个最短距离的二维数组、一个点的终点角度数组,数组大小即为点的大小。点前驱的二维数组用来记录到这个点的最短路径的前一个应该到达的点,最短距离的二维数组是存储两点之间的经过最短路径所经过的距离,点的终点角度数组需要存储点作为终点或者停止点的角度。
[0025]第五步,初始化各变量,遍历之前记录的点的拓扑图,如果两点之间存在连接关系,将此两点之间的距离信息设置为计算距离值,角度信息填写为对应角度,即转向角度。若两点之间没有连接关系,设置为整形最大值,其中,由于是双向图,所以图的矩阵非对称矩阵,矩阵下标相等点为值O。
[0026]第六步,利用Floyd算法计算出两点之间的最短路径,其原理是遍历任意两个点,从图的带权邻接矩阵A = [a(i,j)]开始,递归地进行η次更新,S卩由矩阵D(O) = A,按一个公式,构造出矩阵D(I);又用同样地公式由D(I)构造出D(2);……;最后又用同样的公式由D(η-1)构造出矩阵D(n)。矩阵D(n)的i行j列元素便是i号顶点到j号顶点的最短路径长度,称D(n)为图的距离矩阵,同时还可引入一个后继节点矩阵path来记录两点间的最短路径,前驱二维数组存储前驱点坐标,距离二维数据存储最短距离。
[0027]当有机器人请求给定终点进行路径查询时,首先遍历所有点,找出离起始点最近的采集点和终点最近的采集点,由前驱数组来找出从起始采集点O到终点N前的一个点M,再由前驱数组找出从起点到M点前驱点L,……,一直到查询到前驱点为起始采集点O时停止。输出所有经过的路径,发送到机器人让机器人执行。
[0028]当机器人开始启动执行时,首先将所有经过路段“加锁”,即将双向图中反方向路径权值加上一个最大整形(如65535),注意是加上最大整形,而不是无穷大,每当运行到一个点,将运行过的点进行“解锁”,即将两点之间的权值减去最大整形,这样保证其他机器人每次检索出来是一个相对的最端路径。例如,路径检索出1-4-6-9,则先将9-6,6-4,4_1路径的权值加上最大整形,为了防止经过路段有机器人检索到返回路径。当机器人到达4点,则4-1路径解锁,到达终点9时,取终点角度数组中的角度,让机器人转动到指定角度。这种方法,一方面可以让机器人会检索到一条最短的路径,而且这条路径不会有其他机器人沿着这条路径反方向行走,当其他机器人查找路径时,会让机器人选择绕行,而不是按原路返回,这样在单行道场合完成最大化使用效率。
[0029]以上显示和描述了本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解,本发明不受上述实施例的限制,上述实施例和说明书中描述的只是本发明的原理,在不脱离本发明精神和范围的前提下,本发明还会有各种变化和改进,这些变化和改进都落入要求保护的本发明内。本发明要求保护范围由所附的权利要求书及其等效物界定。
【主权项】
1.基于已知环境下的移动机器人路径规划方法,其特征在于:包括以下步骤: 第一步,记录点坐标和转向角; 第二步,生成地图拓扑图:利用对应关系,通过转换实际坐标系到屏幕坐标系,将相应的点生成对应的地图拓扑图; 第三步,计算并填充有向图:将步骤二中的对应关系整理成数据结构中的有向图; 第四步,设置点的数组:设置一个点前驱的二维数组、一个最短距离的二维数组、一个点的终点角度数组,数组大小即为点的大小; 第五步,遍历各点:初始化各变量,遍历之前记录的点的拓扑图; 第六步,用Floyd算法计算最短路径矩阵。2.根据权利要求1所述的基于已知环境下的移动机器人路径规划方法,其特征在于:第一步中,所述转向角为采集所得转向角或计算角,计算角需要加上一定的偏移量。3.根据权利要求1或2所述的基于已知环境下的移动机器人路径规划方法,其特征在于:第三步中,安装在机器人上的传感器为能够输出角度信息的传感器,所述角度为记录角度。4.根据权利要求1或2所述的基于已知环境下的移动机器人路径规划方法,其特征在于:第三步中,安装在机器人上的传感器不能输出角度信息,所述角度为计算角度,计算角度需根据实际需求添加一定的偏移量。5.根据权利要求1所述的基于已知环境下的移动机器人路径规划方法,其特征在于:第四步中,点前驱的二维数组用来记录到这个点的最短路径的前一个应该到达的点,最短距离的二维数组是存储两点之间的经过最短路径所经过的距离,点的终点角度数组需要存储点作为终点或者停止点的角度。6.根据权利要求1所述的基于已知环境下的移动机器人路径规划方法,其特征在于:第五步中,由于拓扑图是双向图,所以图的矩阵非对称矩阵,矩阵下标相等点为值O。7.根据权利要求1至6中任一项所述的基于已知环境下的移动机器人路径规划方法,其特征在于:当机器人开始启动执行时,先将所有经过路段“加锁”,每当运行到一个点,将运行过的点进行“解锁”,当其他机器人查找路径时,会让机器人选择绕行。
【文档编号】G05D1/02GK106094834SQ201610569810
【公开日】2016年11月9日
【申请日】2016年7月19日
【发明人】郭龙, 陈双, 范冬林, 胡超
【申请人】芜湖哈特机器人产业技术研究院有限公司