CN108871344B - 足球机器人ggrrt路径规划方法 - Google Patents

Abstract

本发明公开了足球机器人GGRRT路径规划方法。为了提高足球机器人搜索效率，使其能在比赛中迅速做出反应，占据主动地位，本发明提出GGRRT路径规划算法。在原有RRT算法基础上引入目标导向函数，此时，生长函数不再由随机增长函数单独决定，而是由随机增长函数和目标导向函数共同决定。这相当于增加了目标点对研究对象的吸引力，可以有效地引导随机树朝着目标方向生长。实验结果表明，应用本发明所提出的GGRRT算法进行路径规划，消耗的时间仅为原来RRT算法所用时间的1/12～5/6，多余分支数也大大减少，有效地解决了足球机器人盲目搜索问题，大大提高路径规划效率。

Description

足球机器人GGRRT路径规划方法

技术领域

本发明属于机器人路径规划的优化方法，在快速扩展随机树(Rapidly-exploringRandom Trees，简称RRT)算法的基础上引入目标导向函数，形成基于目标导向的快速扩展随机树(Goal Guide Rapidly-exploring Random Trees，简称GGRRT)算法，该算法可有效避免机器人的盲目搜索，大大减少搜索时间，适用于动态的、任意复杂度的应用场景，可在机器人足球比赛中发挥重要作用。

背景技术

在最近几年中，随着人工智能火热迅猛地发展，机器人再一次受到前所未有的关注，以NAO机器人为代表的足球比赛成为各国争先竞技的热点项目，也成了衡量智能化水平的重要指标。2007年7月，NAO被机器人世界杯RoboCup的组委会选定为标准平台，RoboCup分为不同的比赛组。“标准平台组”(Platform Standard League)中，各队使用统一的机器人进行比赛，所有的参赛队伍只需比拼软件开发水平。机器人运作完全是自动式的，没有外界控制，也没有人和电脑控制。机器人如何在动态、任意复杂度的场景下迅速地规划出最优路径，是一个十分具有挑战性的问题。显而易见的是，在这样的竞技比赛中，谁能最快最准地做出反应，谁就能占据绝对优势。

机器人足球比赛中最常用到的方法是RRT算法，之所以选择这种算法，是因为与其他算法相比，RRT具有如下三点优势：1、对状态空间中的采样点进行碰撞检测，避免了对空间的建模；2、随机树扩展的速度快，使得搜索效率较高；3、适合解决动态、多障碍物的复杂环境下的路径规划问题。但是，实际比赛结果证明，将传统的RRT算法应用于机器人足球比赛中，虽然也能保证比赛的正常、顺利地进行，却丝毫不具备取胜的实力。这是什么原因呢？因为路径规划的实时性、准确性和高效性，是足球机器人的核心和关键所在，而RRT采样随机性决定了机器人搜索过程是盲目的，随机树将会扩展出很多与最终路径无关的节点，只有当某一时刻，随机树中的叶子节点包含了目标点或进入了目标区域，才可以在随机树中找到一条由树节点组成的从初始点到目标点的路径，整个过程中搜索的目的性不强，延长了路径搜索、规划的时间。因此，将这样的一套算法应用于机器人足球比赛，是没有竞争力的，需要对RRT算法进行改进。

为了提高足球机器人搜索效率，本发明提出基于目标导向的快速扩展随机树(GGRRT)算法。将目标导向思想加入RRT算法，类似于人工势场法中目标点对研究对象有一个吸引力，从而引导随机树朝着目标方向生长。

发明内容

本发明依据目标导向思想提出GGRRT算法，有效地提高路径搜索效率，避免搜索过程的盲目性。在90x90的环境下进行仿真，并设置了简单、一般、复杂三种不同的实验场景。通过多组实验结果的比对和分析，本发明证实了无论处于哪种环境，RRT算法和GGRRT算法均能找到一条由起始点到目标点的合理通路，但相比之下，GGRRT路径规划算法避免了漫无目的地搜索，多余分支显著减少，计算量也随之减小，规划时间变短，大大提高了路径规划效率。GGRRT算法的提出，使得盲目搜索问题得到极大改善，实现了对足球机器人的路径优化。因此，可以将GGRRT路径规划算法应用于任意环境下的足球机器人大赛当中。

本发明是通过以下技术方案实现的，足球机器人GGRRT路径规划方法，本方法包括以下步骤：

第一步、机器人当前位置坐标为(nearestNode.posX，nearestNode.posY)，以该节点为父节点，计算其子节点的坐标(tempNode.posX，tempNode.posY)。

第二步、判断子节点(tempNode.posX，tempNode.posY)的有效性，若该节点在激光传感器探测范围之内且在障碍物之外，则视为有效，否则视为无效。

第三步、将有效子节点作为新的父节点，按第一步方法得到新的子节点，接着按第二步方法判断新的子节点的有效性。重复该步骤，直到获得机器人当前位置下的所有有效节点。

第四步、目前已经获得机器人当前位置下的所有有效节点，将这些节点按父子关系连接成若干条分支，机器人任选一条分支进行移动，实现了局部路径规划过程。

第五步、重复以上四步，直到机器人到达最终的目标点。

一种应用于足球机器人的GGRRT路径规划方法，其特征在于包括以下步骤：

第一步、机器人当前位置坐标为(nearestNode.posX，nearestNode.posY)，以该节点为父节点，计算其子节点的坐标(tempNode.posX，tempNode.posY)。

第二步、判断子节点(tempNode.posX，tempNode.posY)的有效性，若该节点在激光传感器探测范围之内且在障碍物之外，则视为有效，否则视为无效。

第三步、将有效子节点作为新的父节点，按第一步方法得到新的子节点，接着按第二步方法判断新的子节点的有效性。重复该步骤，直到获得机器人当前位置下的所有有效节点。

第四步、目前已经获得机器人当前位置下的所有有效节点，将这些节点按父子关系连接成若干条分支，机器人任选一条分支进行移动，实现了局部路径规划过程。

第五步、重复以上四步，直到机器人到达最终的目标点。

具体而言，

第一步中：

机器人当前位置坐标为(nearestNode.posX，nearestNode.posY)，以该节点为父节点，计算其子节点的坐标(tempNode.posX，tempNode.posY)。方法如下：

tempNode.posX＝nearestNode.posX+(rrtStepSize*cosθ)+(kgoal*cosα)

tempNode.posY＝nearestNode.posY+(rrtStepSize*sinθ)+(kgoal*sinα)

其中，rrtStepSize表示机器人移动的步长，实验中取rrtStepSize＝3，kgoal表示目标引力系数，实验中取kgoal＝1，θ表示随机节点和父节点的连线与x轴的夹角，α表示目标节点和父节点的连线与x轴的夹角，随机节点的坐标为(rand()％150+1，rand()％150+1)。

第二步中：

判断子节点(tempNode.posX，tempNode.posY)的有效性，若该子节点在激光传感器探测范围之内且在障碍物之外，则视为有效，否则视为无效。方法如下：

激光传感器测距半径8m，旋转角度360°，若子节点的坐标不与障碍物坐标重合，并且满足[(tempNode.posX-nearestNode.posX)²+(tempNode.posY-nearestNode.posY)²)]^1/2≤8，则可以判定该子节点有效。

第三步中：

将有效子节点作为新的父节点，按第一步方法得到新的子节点，接着按第二步方法判断新的子节点的有效性。重复该步骤，直到获得机器人当前位置下的所有有效节点。方法如下：

计算过程和判断方法分别与步骤一、步骤二相同，需要注意的是，子节点作为新的父节点去扩展的同时，原来的父节点也会扩展出新的子节点。现举例说明：

假设机器人当前位置即父节点坐标为(0,0)，按步骤一公式计算，得到它的有效子节点(2,3)。接下来的扩展过程变得复杂，不仅要扩展出(2,3)的子节点(4,6)，还要扩展出(0,0)的另一个子节点(3,3)。倘若扩展出的其他节点均无效，则机器人当前位置下的所有有效节点共四个(0,0)、(2,3)、(3,3)和(4,6)。

第四步中：

目前已经获得机器人当前位置下的所有有效节点，将这些节点按父子关系连接成若干条分支，机器人任选一条分支进行移动，实现了局部路径规划过程。方法如下：

按照步骤三的举例，由(0,0)、(2,3)、(3,3)、(4,6)这四个节点可以获得两条分支，即：(0,0)，(2,3)，(4,6)和(0,0)，(3,3)。机器人将任选一条分支进行移动。

第五步中：

重复以上四步，直到机器人到达最终的目标点。方法如下：

规划出的若干条局部路径首尾相连，形成全局路径。

RRT路径规划方法的生长函数只与随机增长函数即rrtStepSize*cosθ有关。而随机增长函数的特点决定了足球机器人搜索过程是盲目的，搜索过程如图1。机器人每到一点，就以该点为父节点，向外随机扩展出若干子节点，每一组父节点与子节点的连线称作一个分支，所有分支的集合就组成了随机树。此外，每一个子节点又会作为新的父节点向外扩展，生成新的子节点，只有当某一时刻，随机树中的子节点包含了目标点或进入了目标区域，才能够在随机树中找到一条由树节点组成的从起始点到目标点的路径，整个过程中搜索的目的性不强，延长了路径搜索、规划的时间。

本发明提出一种优化的GGRRT路径规划算法，该算法的生长函数由随机增长函数(rrtStepSize*cosθ)和目标导向函数(kgoal*cosα)两部分共同决定，新增加的目标导向函数类似人工势场法，其目标点对研究对象有一个吸引力，从而引导随机树朝着目标方向生长。

附图说明

图1、RRT算法搜索过程，S表示起始点，G表示目标点。

图2、GGRRT算法原理图，展示的是新节点(也叫子节点)的生成过程，与RRT算法的不同之处在于增加的X_goal节点对当前节点X_cur有一个吸引力。

图3、三种不同环境下，目标点均为(25，85)时的实验结果对比图。

图4、三种不同环境下，目标点均为(85，22)时的实验结果对比图，表示方法同图3。

图5、三种不同环境下，目标点均为(15，40)时的实验结果对比图，表示方法同图3。

具体实施方式

下面对本发明的实施例作详细说明，本实施例在本发明技术方案为前提下进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

实施例：

第一步，机器人当前位置坐标为(nearestNode.posX，nearestNode.posY)，以该节点为父节点，计算其子节点的坐标(tempNode.posX，tempNode.posY)。方法如下：

tempNode.posX＝nearestNode.posX+(rrtStepSize*cosθ)+(kgoal*cosα)

tempNode.posY＝nearestNode.posY+(rrtStepSize*sinθ)+(kgoal*sinα)

其中，rrtStepSize表示机器人移动的步长，实验中取rrtStepSize＝3，kgoal表示目标引力系数，实验中取kgoal＝1，θ表示随机节点和父节点的连线与x轴的夹角，α表示目标节点和父节点的连线与x轴的夹角，随机节点的坐标为(rand()％150+1，rand()％150+1)。

第二步，判断子节点(tempNode.posX，tempNode.posY)的有效性，若该节点在激光传感器探测范围之内且在障碍物之外，则视为有效，否则视为无效。方法如下：

激光传感器测距半径8m，旋转角度360°，若子节点的坐标不与障碍物坐标重合，并且满足√(tempNode.posX-nearestNode.posX)²+(tempNode.posY-nearestNode.posY)²)≤8，则可以判定该子节点有效。

第三步，将有效子节点作为新的父节点，按第一步方法得到新的子节点，接着按第二步方法判断新的子节点的有效性。重复该步骤，直到获得机器人当前位置下的所有有效节点。方法如下：

计算过程和判断方法分别与步骤一、步骤二相同，需要注意的是，子节点作为新的父节点去扩展的同时，原来的父节点也会扩展出新的子节点。现举例说明：

假设机器人当前位置即父节点坐标为(0,0)，按步骤一公式计算，得到它的有效子节点(2,3)。接下来的扩展过程变得复杂，不仅要扩展出(2,3)的子节点(4,6)，还要扩展出(0,0)的另一个子节点(3,3)。倘若扩展出的其他节点均无效，则机器人当前位置下的所有有效节点共四个(0,0)、(2,3)、(3,3)和(4,6)。

第四步，目前已经获得机器人当前位置下的所有有效节点，将这些节点按父子关系连接成若干条分支，机器人任选一条分支进行移动，实现了局部路径规划过程。方法如下：

按照步骤三的举例，由(0,0)、(2,3)、(3,3)、(4,6)这四个节点可以获得两条分支，即：(0,0)，(2,3)，(4,6)和(0,0)，(3,3)。机器人将任选一条分支进行移动。

第五步，重复以上四步，直到机器人到达最终的目标点。方法如下：

规划出的若干条局部路径首尾相连，形成全局路径。

实施效果

依据上述步骤，本发明设计了九组对比实验，实验结果如图3、图4、图5所示。根据障碍物位置和数量，设置了简单、一般、复杂三种实验环境，每种环境又分别设置(25，85)、(85，22)、(15，40)三个不同的目标点。将RRT与GGRRT算法进行全方位对比，实验结果表明：引入目标导向函数之后形成的GGRRT算法，使得路径规划时间仅为原来的1/12～5/6，多余分支数大大减少，有效解决足球机器人盲目搜索问题。耗时结果比较见表1、表2、表3，时间单位：秒(s)。

表1.起始点为(0，0)，目标点为(25，85)的耗时情况对比

算法	简单环境	一般环境	复杂环境
				RRT	16.88	16.90	20.15
GGRRT	2.53	3.14	3.51

表2.起始点为(0，0)，目标点为(85，22)的耗时情况对比

算法	简单环境	一般环境	复杂环境
				RRT	17.12	19.50	3.61
GGRRT	1.38	3.22	3.05

表3.起始点为(0，0)，目标点为(15，40)的耗时情况对比

算法	简单环境	一般环境	复杂环境
				RRT	10.32	16.58	13.20
GGRRT	3.62	5.80	3.29

Claims (2)

1.足球机器人GGRRT路径规划方法，其特征在于，该方法包括以下步骤：

第一步、机器人当前位置坐标为（nearestNode.posX，nearestNode.posY），以该机器人当前位置为父节点，计算其子节点的坐标（tempNode.posX，tempNode.posY）；

第二步、判断子节点（tempNode.posX，tempNode.posY）的有效性，若该节点在激光传感器探测范围之内且在障碍物之外，则视为有效，否则视为无效；

第三步、将有效子节点作为新的父节点，按第一步方法得到新的子节点，接着按第二步方法判断新的子节点的有效性；重复第三步，直到获得机器人当前位置下的所有有效节点；

第四步、目前已经获得机器人当前位置下的所有有效节点，将这些节点按父子关系连接成若干条分支，机器人任选一条分支进行移动，实现了局部路径规划过程；

第五步、重复以上四步，直到机器人到达最终的目标点；

第一步中：

机器人当前位置坐标为（nearestNode.posX，nearestNode.posY），以该节点为父节点，计算其子节点的坐标（tempNode.posX，tempNode.posY）；方法如下：

tempNode.posX=nearestNode.posX+(rrtStepSize*cosθ)+(kgoal*cosα)

tempNode.posY=nearestNode.posY+(rrtStepSize*sinθ)+(kgoal*sinα)

其中，rrtStepSize表示机器人移动的步长，取rrtStepSize=3，kgoal表示目标引力系数，取kgoal=1，θ表示随机节点和父节点的连线与x轴的夹角，α表示目标节点和父节点的连线与x轴的夹角，随机节点的坐标为（rand( )%150+1，rand( )%150+1）；

第二步中：

判断子节点（tempNode.posX，tempNode.posY）的有效性，若该子节点在激光传感器探测范围之内且在障碍物之外，则视为有效，否则视为无效；方法如下：

激光传感器测距半径8m，旋转角度360°，若子节点的坐标不与障碍物坐标重合，并且满足

[（tempNode.posX-nearestNode.posX）²+（tempNode.posY-nearestNode.posY）²）]^1/2≤ 8，则判定该子节点有效。

2.根据权利要求1所述的足球机器人GGRRT路径规划方法，其特征是，第三步中：

将有效子节点作为新的父节点，按第一步方法得到新的子节点，接着按第二步方法判断新的子节点的有效性；重复该步骤，直到获得机器人当前位置下的所有有效节点；方法如下：

计算过程和判断方法分别与步骤一、步骤二相同，子节点作为新的父节点去扩展的同时，原来的父节点也会扩展出新的子节点；

假设机器人当前位置即父节点坐标为（0,0），有效子节点（2,3）；不仅扩展出（2,3）的子节点（4,6），还扩展出（0,0）的另一个子节点（3,3）；倘若扩展出的其他节点均无效，则机器人当前位置下的所有有效节点共四个（0,0）、（2,3）、（3,3）和（4,6）；

第四步中：

目前已经获得机器人当前位置下的所有有效节点，将这些节点按父子关系连接成若干条分支，机器人任选一条分支进行移动，实现了局部路径规划过程；方法如下：

由（0,0）、（2,3）、（3,3）、（4,6）这四个节点获得两条分支，即：（0,0），（2,3），（4,6） 和（0,0），（3,3）；机器人将任选一条分支进行移动。

Images