一种基于Harris算法与RRT算法复合的自主路径规划方法

本发明涉及机器人自动导引路径规划方法领域，具体是一种基于harris算法与rrt算法复合的自主路径规划方法。

背景技术：

如今，智能机器人的应用已经逐渐成为主流，如智能停车场、智能家居等，其中室内家居逐渐转向智能化的发展趋势更为明显，用于室内家居的各种机器人如扫地机器人等已经成为众多学者研究的热点课题。路径规划作为机器人等智能化的核心部分，目前主要有两种规划方式：基于环境已知的全局路径规划和基于环境未知的局部路径规划方法。而针对基于环境已知的全局路径规划对环境变化的适应性较低，尤其是针对大场景下如果特定路径的起点和终点，往往机器人需要很长的时间才能到达所设定的终点，并且其规划的路径往往比较复杂，导致无法及时对室内突发状况进行处理，以及无法及时处理常规情景。

技术实现要素：

本发明的目的是提供一种基于harris算法与rrt算法复合的自主路径规划方法，以解决现有技术全局路径规划方法存在对环境变化适应性低的问题。

为了达到上述目的，本发明所采用的技术方案为：

一种基于harris算法与rrt算法复合的自主路径规划方法，其特征在于：包括以下步骤：

步骤1、以目标场地的外接矩形的任意一个顶角作为原点o，以所述原点o相连的两条边分别为x轴和y轴，从而建立二维的直角坐标系oxy；

在所述直角坐标系oxy中，将所述目标场地划分成m×n的二维的栅格地图bm×n如公式(1)所示

其中，m表示所述栅格地图bm×n的横向栅格数，n表示栅格地图bm×n的纵向栅格数；gmn表示所述栅格地图bm×n上坐标为(nx，ny)的坐标点被占用的状态，m>nx>0,n>ny>0，若gmn＝0表示所述的坐标点为空闲点，若gmn＝1表示所述的坐标点为障碍点；

在栅格地图bm×n中设置坐标为(sx，sy)的起始点nodes，以及坐标为(gx，gy)的目标点nodeg；

步骤2、采用骨架提取算法对步骤1得到的栅格地图bm×n进行计算，经过骨架提取算法计算后，栅格地图bm×n中的连通区域分别被细化成细线，由此得到包含栅格地图中所有可行路径的多条细线；

步骤3、采用harris角点检测算法对细化出来的各个可行路径分别进行计算，提取得到各个可行路径对应的特征点nodec，然后对各个特征点nodec进行自适应阈值处理，得到各自对应的拓扑节点nodet；

步骤4、根据拓扑节点nodet，建立拓扑结构地图mapt；

步骤5、采用随机快速搜索树算法，首先把拓扑节点nodet添加到随机快速搜索树算法的初始树treei中，然后基于随机快速搜索树算法进行初始路径的搜寻与查找；

步骤6、根据自适应阈值公式(3)和公式(4)来对由拓扑节点所构成的初始路径进行局部优化，公式(3)、(4)如下：

threshold2＝μ*(m+n)/4(3)

其中：threshold2是一个自适应阈值，作用是为对由拓扑节点所构成的初始路径进行分段，当拓扑节点之间的距离大于自适应阈值threshold2时，其下面的拓扑节点就变成下一段路径；

μ是根据栅格地图的长、宽和栅格地图大小的数量级来定义的自适应参数，m和n为栅格地图的长和宽；

a＝min{m，n}为栅格地图中较短的边，b＝max{m，n}为栅格地图中较长的边，ε为栅格地图大小的数量级，当栅格地图较短的边a和较长的边b相等时令μ为1，当栅格地图较短的边a和较长的边b不等时，令μ为公式(4)；

通过threshold2这个阈值把初始路径分段，这些分段的路径为局部路径；

步骤7、将由拓扑节点所构成的初始路径根据公式(2)进行自适应阈值处理后对其进行局部优化，每一段局部路径优化时都被限制在了椭圆内；

在每段局部路径规划中调用informedrrt^*算法，从所构成每段局部路径规划中的第一个拓扑节点nodet1向最后一个拓扑节点站点nodetl进行扩展搜索，利用公式(5)计算各个相邻拓扑节点之间的距离di，其中如果通过informedrrt^*算法扩展到的节点nodee所得到的路径比由拓扑节点所构成的路径短，那么则用nodee代替相应的拓扑节点，公式(5)如下：

公式(5)中，实际路径函数值ld代表的是相邻两个节点之间的距离，其中每一段优化后的局部路径的长度可由公式(6)表示：

li＝ld1+ld2+…+lde+…+ldl(6)

其中，ld1代表第一个拓扑节点nodet1和第二个拓扑节点nodet2；lde代表通过informedrrt^*算法扩展到的节点nodee和相应的拓扑节点之间的距离；ldl代表最后一个拓扑节点nodetl和其相邻的拓扑节点之间的距离；

步骤8、将所述每段优化后的路径li按照拼接规则进行拼接后形成新的路径{l1，l2，l3，…，lm，…，ln-1，ln}，由此得到优化后的全局路径la。

所述的harris算法与rrt算法复合的自主路径规划方法，其特征在于：步骤3中，基于公式(2)对特征点nodec进行自适应阈值处理，公式(2)如下：

公式(2)中，threshold为自适应节点阈值，经过骨架提取后的栅格地图对其进行角点检测算法会得到很多特征点nodec，为了消除冗余的节点通过自适应阈值threshold来消除冗余的节点；消除冗余的特征点后还剩下的特征点为nodet，ε为栅格地图大小的数量级。

所述的基于harris算法与rrt算法复合的自主路径规划方法，其特征在于：步骤5中，初始树treei按如下规则生成：

(5.1)、所有由拓扑节点nodet所组成的闭合环路生成为一颗初始树；

(5.2)、在交叉支路上的拓扑节点可以有多个父节点，定义为parent1，parent2,…,parentn，以这样的方式生成一颗初始树；

(5.3)、当新节点noden和最近的拓扑节点nodetn的子节点的连线穿过障碍物时，令起点为最近的拓扑节点的父节点，然后把起点插入到初始树treei中；

(5.4)、当新节点noden和最近的拓扑节点nodetn的子节点的连线没有穿过障碍物时，令起点为最近的拓扑节点的子节点的父节点，同样把起点插入到初始树treei中。

所述的一种基于harris算法与rrt算法复合的自主路径规划方法，其特征在于：还包括步骤9：判断拼接后的全局路径la最后一个节点是否为目标点nodeg，若是，则表示路径规划完成，且所述全局路径la为扫地机器人行驶的完整路径，否则返回步骤7顺序执行。

本发明提出一种基于harris算法与rrt算法复合的自主路径规划方法，通过rrt算法在全局路径规划上的速度优势并且根据初始路径进行优化让其更好的适应扫地机器人的动力学模型，从而提高了大场景下室内环境的路径规划的效率和对不同室内环境的适应性。

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

1.本发明通过将harris角点检测算法与rrt算法相结合的方式，通过harris角点检测算法对已经进行骨架处理后的栅格地图提取出的拓扑节点，可以有效地引导rrt算法的扩展方向。防止其进行盲目无效的扩展，可以大大的缩短路径规划的时间，从而可以有效处理固定室内环境下的路径规划时间长和大场景下室内导航算法难以自适应的问题。

2、本发明通过使用informedrrt^*算法，并采取对整个路径分段局部进行优化的方式有效地提高了rrt算法所规划出的路径的曲折性，并且降低了规划路径的长度，从而提高了路径规划的柔性，更好的适应了扫地机器人的动力学模型，降低了室内环境对于处理设备高性能的要求，降低了能源的消耗。

3.本发明通过将harris角点检测算法与rrt算法相结合的方式来进行路径规划，并对规划的路径进行分段优化，相比于现有的扫地机器人都是遇到障碍物进行旋转，没有进行有效地路径规划，本发明有效的提高了对于静态环境的路径规划的时效性、自适应性、高效性。

附图说明

图1为本发明实施例根据室内环境所仿真的真实场景。

图2为本发明实施例根据室内环境所建立直角坐标系oxy。

图3为本发明实施例根据harris角点检测算法生成的拓扑结构图。

图4a为本发明实施例中通过informedrrt^*算法扩展新节点的过程。

图4b为本发明实施例中为新节点选择路径代价最小的父节点。

图5为本发明实施例中新节点寻找子节点的过程

图6为本发明实施例在静态环境时根据harris角点检测算法与rrt算法搜索并生成的初始路径。

图7为本发明实施例在对初始路径进行分段局部路径优化后所得到的适应于机器人动力学模型的路径。

具体实施方式

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

本实例中以室内扫地机器人为例，自重1.5kg，正常行驶速度为1.1m/s，急刹车以及制动时间t约为2s。总体按如下过程实现harris角点检测算法与rrt算法复合的自主路径规划方法：

1.根据试验场地的外接矩形确定栅格地图的大小；

2.根据室内环境的栅格地图进行骨架提取，然后根据骨架提取后的栅格地图结果，对其进行harris角点检测得到相应的特征点。通过自适应阈值消除冗余的特征点，其余剩下的称为拓扑节点，构成拓扑结构图。

3.根据harris角点检测算法与rrt算法快速得到适合此时室内环境的初始路径。

4.采用自适应阈值的方式将由拓扑节点所组成的初始路径进行分段局部优化。

5.将局部路径以拼接规则拼接，并取代之前的全局路径。

具体地说，该自主路径规划方法，是应用于由室内环境较为复杂、若干个障碍物和扫地机器人组成的智能家居系统中。如图1所示，在整个室内场景中，有床柜子还有桌子垃圾桶分布在室内的栅格地图内。栅格地图的每个坐标点都包含坐标属性和状态属性。坐标属性值代表了该坐标点相较于整个室内环境的相对位置，状态属性值代表了室内环境中该位置的是否被障碍物所占用。

本实施例具体过程如下：

步骤1：如图2所示，以仿真的室内环境的外接矩形的任意一个顶角作为原点o，以原点o相连的两条边分别为x轴和y轴，从而建立直角坐标系oxy。

在所述直角坐标系oxy中，将所述目标场地划分成m×n的二维的栅格地图bm×n如公式(1)所示

其中，m表示所述栅格地图bm×n的横向栅格数，n表示栅格地图bm×n的纵向栅格数；gmn表示所述栅格地图bm×n上坐标为(nx，ny)的坐标点被占用的状态，m>nx>0,n>ny>0，若gmn＝0表示所述的坐标点为空闲点，若gmn＝1表示所述的坐标点为障碍点。

在栅格地图bm×n中设置坐标为(sx，sy)的起始点nodes，以及坐标为(gx，gy)的目标点nodeg。

扫地机器人接收到主人发送的清扫任务，任务内容：从扫地机器人的当前位置坐标为(sx，sy)的起始点nodes到坐标为(gx，gy)的终点nodeg对地上的垃圾进行清理。起始点nodes与目标点nodeg的相对距离大于两个rrt算法中扩展步长的距离，仅有两个扩展步长的路径规划空间太小，无法给扫地机器人提供足够的应急反应时间；以扫地机器人的当前车头方向作为起始点nodes的路径搜索方向。

步骤2:采用骨架提取算法对步骤1得到的栅格地图bm×n进行计算，经过骨架提取算法计算后，栅格地图bm×n中的连通区域分别被细化成细线，由此得到包含栅格地图中所有可行路径的多条细线。

步骤3：采用harris角点检测算法对细化出来的各个可行路径分别进行计算，提取得到各个可行路径对应的特征点nodec，然后对各个特征点nodec进行自适应阈值处理，得到各自对应的拓扑节点nodet。

由于所提取出的特征点nodec在栅格地图上的分布不均，所以本实施例根据公式(2)对特征点nodec进行自适应阈值处理，减少冗余的特征点nodec。经过自适应阈值处理后的特征点为拓扑节点nodet，这样可以为接下来扫地机器人进行rrt算法进行路径搜索时，提供了有效地扩展方向，提高了扫地机器人的效率，大大的减少了寻路时间。公式(2)如下：

公式(2)中，threshold为自适应节点阈值，经过骨架提取后的栅格地图对其进行角点检测算法会得到很多特征点nodec。为了消除冗余的节点通过自适应阈值threshold来消除冗余的节点。消除冗余的特征点后还剩下的特征点为nodet，ε为栅格地图大小的数量级。

步骤4：在得到拓扑节点nodet后，根据拓扑节点nodet，建立拓扑结构地图mapt，如图3所示。

步骤5：根据扫地机器人得到的任务指令由起始点nodes到终点nodeg。然后其采用随机快速搜索树(rrt)算法进行路径的搜寻与查找，具体的首先把根据对室内环境的栅格地图进行harris角点检测算法后所得到的拓扑节点添加到随机快速搜索树(rrt)算法的初始树treei中，初始树treei按如下规则生成：

(5.1)、所有由拓扑节点nodet所组成的闭合环路生成为一颗初始树

(5.2)、在交叉支路上的拓扑节点可以有多个父节点，定义为parent1，parent2,…,parentn，以这样的方式生成一颗初始树

(5.3)、当任务所设定的起点nodes或终点nodeg与其最近的拓扑节点nodetn的子节点的连线穿过障碍物时，令起点nodes或终点nodeg为最近的拓扑节点的父节点，然后把起点nodes和终点nodeg插入到初始树treei中。

(5.4)、当任务所设定的起点nodes或终点nodeg与其最近的拓扑节点nodetn的子节点的连线没有穿过障碍物时，令起点为最近的拓扑节点的子节点的父节点，同样把起点nodes和终点nodeg插入到初始树treei中。通过这样可以有效地把新节点插入到拓扑结构中。

传统的rrt算法中的每个节点只有一个父节点，这样节点不能和拓扑结构相结合，因此本发明提出的算法让拓扑节点可以同时具有多个父节点，这样可以有效的把拓扑结构以树的形式表示出来。

步骤5主要可以概括为首先在初始树中找到距离起点和终点最近的拓扑节点。经过步骤(5.3)和(5.4)的判断，通过本发明所设定的父子关系把起点和终点分别替代最近的拓扑节点，可以有效的把起点和终点加入到拓扑结构中来扩充初始树，这样可以使扫地机器人快速的得到初始路径，其初始路径的结果如图6所示，可以有效减少等待时间，防止需要清扫的东西对地面进行污染，为智能家居系统提高了运行的效率。

步骤6：扫地机器人根据自适应阈值公式(2)和公式(3)来对由拓扑节点所构成的初始路径进行局部优化，在进行局部优化时，可以让扫地机器人一边进行这一段优化好的路径进行行驶，一边对下一段路程进行优化，这样可以加快算法的收敛，同时减少了时间上的浪费，优化后的路径更为平滑，适应扫地机器人的运行，不会出现较大的曲折路径。公式(3)、(4)如下：

threshold2＝μ*(m+n)/4(3)

threshold2是一个自适应阈值，目的是为对由拓扑节点所构成的初始路径进行分段，当拓扑节点之间的距离大于自适应阈值threshold2时，其下面的拓扑节点就变成下一段路径了，通过threshold2这个阈值可以把初始路径分段，这些分段的路径称为局部路径，然后对每段局部路径通过步骤七进行优化。μ是根据栅格地图的长、宽和栅格地图大小的数量级来定义的自适应参数，m和n为栅格地图的长和宽。

a＝min{m,n}为栅格地图中较短的边，b＝max{m,n}为栅格地图中较长的边，ε为栅格地图大小的数量级，当栅格地图较短的边a和较长的边b相等时令μ为1，当栅格地图较短的边a和较长的边b不等时，令μ为公式(4)；

步骤7：将由拓扑节点所构成的初始路径根据公式(3)和(4)进行自适应阈值处理后对其进行局部优化，每一段局部路径优化时都被限制在了椭圆内，并且优化时都分两个过程：1.为新节点选择代价最小的父节点，2.重新布线原则。具体如下：

1.为新节点选择代价最小的父节点。如图4a和图4b所示，首先根据扫地机器人通过随机采样得到的采样点noder(noderand)作为起点nodes(nodestart)寻找目前随机树中最近的节点noden(nodenear)，然后通过informedrrt^*算法中的扩展方式得到了一个新节点nodene(nodenew)。接下来以nodene为圆心，一定半径画一个圆，逐个计算起点到圆内节点的距离与圆内节点到新节点nodene的距离之和，其中最小的距离和就是起点到新节点nodene的路径代价，相应的节点为nodene的父节点。nodes就是扫地机器人目前所在的位置，nodene就是其通过rrt算法扩展出来的一个新节点。扫地机器人需要对这个新扩展出来的节点寻找路径代价最小的父节点。

2.重新布线原则,也就是为扫地机器人所扩展的新节点寻找子节点的过程。在由新节点nodene所构成的圆中，对圆中的任意一个节点，计算以新节点为圆中节点的父节点时，其距离代价是否会降低，如果会，那么将该节点的父节点赋值为新节点，对应的代价更新为新的代价。从图4b中可以得到，节点e距离扫地机器人原来的路径代价为23，如果以节点c为父节点，那么新的路径代价为20，小于原来的路径代价，那么将节点e的父节点改为节点c，结果图5所示：

在每段局部路径规划(这个局部路径规划是通过步骤6的自适应阈值threshold2对初始路径进行分段得到的)中调用informedrrt^*算法，从所构成每段局部路径规划中的第一个拓扑节点nodet1向最后一个拓扑节点站点nodetl进行扩展搜索，利用公式(5)计算各个相邻拓扑节点之间的距离di，其中如果通过informedrrt^*算法扩展到的节点nodee所得到的路径比由拓扑节点所构成的路径短，那么则用nodee代替相应的拓扑节点，公式(5)如下：

公式(5)中，实际路径函数值ld代表的是相邻两个节点之间的距离，其中每一段优化后的局部路径的长度可由公式(6)表示

li＝ld1+ld2+…+lde+…+ldl(6)

其中，ld1代表第一个拓扑节点nodet1和第二个拓扑节点nodet2。lde代表通过informedrrt^*算法扩展到的节点nodee和相应的拓扑节点之间的距离。ldl代表最后一个拓扑节点nodetl和其相邻的拓扑节点之间的距离；

步骤8：扫地机器人将所述每段优化后的路径li按照拼接规则进行拼接后形成新的路径{l1,l2，l3,…,lm，…，ln-1,ln}得到优化后的全局路径la。

对于距离远的全局路径规划一般耗时过长、效率低下，因此本发明采用harris角点检测算法与rrt算法，减少了计算的复杂度，从而减少了对室内环境计算设备的高性能要求，降低了实施成本，也提高了计算的效率，对智能家居扫地机器人常发生的不必要的碰撞，以及无效的路径移动有了很大的改善与提高。比如刚刚清理完的区域会有二次重复清理的可能，间接的节约能源的消耗。

所述拼接规则为：按照构成初始路径的拓扑节点的顺序依次将所得到的将所得到的优化后的局部路径按顺序进行拼接，其拼接后的优化路径结果如图7所示。

由于室内环境现场可能出现一定的变化，但是其主要的拓扑节点不会出现较大的改变，如果扫地机器人移动的路线中遇到了障碍物，可以通过机器人上的雷达预知情况采取制动，一般民用较便宜的雷达对障碍物的精准感知距离为3m，发现障碍和系统处理时间约为0.5s，急刹车以及制动时间t约为2s，此时扫地机器人的制动距离为s＝v×t＝1×2.5＝2.5m，距离与障碍物相撞还差0.5m，有效地避免了碰撞，如果家中有较为珍贵的物品，可以有效地减少财产损失，同时立即对其进行rrt算法对路径进行重新搜索，当rrt算法所扩展的节点与拓扑节点吻合时，继续进行步骤5、6、7。

步骤9：判断拼接后的全局路径la最后一个节点是否为目标点nodeg，若是，则表示路径规划完成，且所述全局路径la为扫地机器人行驶的完整路径，任务完成后上报控制中心已经完成，等待下一条任务指令，若是没有任务则在原地等待，否则返回步骤7顺序执行。

本发明所述的实施例仅仅是对本发明的优选实施方式进行的描述，并非对本发明构思和范围进行限定，在不脱离本发明设计思想的前提下，本领域中工程技术人员对本发明的技术方案做出的各种变型和改进，均应落入本发明的保护范围，本发明请求保护的技术内容，已经全部记载在权利要求书中。