CN113156970B - 一种通行区域的路径融合规划方法、机器人及芯片 - Google Patents

Abstract

本发明公开一种通行区域的路径融合规划方法、机器人及芯片，机器人预先在障碍物分布较多的狭窄通道内搜索出可供融合的候选路线，然后使用所述路径融合规划方法融合启发式搜索算法和符合搜索条件的候选路线规划出整体导航路径，克服地图栅格标记误差，以便解决机器人在栅格地图中的通道较窄而容易产生标记误差的可通行区域内的导航路径规划问题。

Description

一种通行区域的路径融合规划方法、机器人及芯片

技术领域

本发明涉及机器人路径规划的技术领域，涉及一种通行区域的路径融合规划方法、机器人及芯片。

背景技术

现有技术中的移动机器人是一类利用传感器自主探测周围环境、利用控制器来决定机器本体的运动，利用执行机构(如轮子)来实现运动的机器人。现有技术的清洁机器人经常移动至家庭环境中凳子的四条脚、茶几的入口等由各种家具组件之间限制形成的狭窄通道、房间的门打开形成的狭小通道处。机器人进入这些狭窄通道时，由于机器人存在打滑情况、用于定位的传感器存在累计误差或视觉地图优化也产生误差，所以机器人容易在即时构建的栅格地图上将这些狭窄通道标记为障碍物占据的区域，导致映射到机器人构建的栅格地图中的狭窄通道的入口是堵住的，而实际运动场景中没有被真正堵住。因此，基于常规的路径搜索算法无法绕过因地图误差标记的障碍物栅格，不利于机器人导航进出这一类因通道较窄而容易产生标记误差的可通行区域。

发明内容

本发明公开一种通行区域的路径融合规划方法、机器人及芯片，机器人预先在障碍物分布较多的狭窄通道内搜索出可供融合的候选路线，然后使用所述路径融合规划方法融合启发式搜索算法和符合搜索条件的候选路线规划出整体导航路径，克服地图栅格标记误差，以便解决机器人在栅格地图中的通道较窄而容易产生标记误差的可通行区域内的进出问题。具体的技术方案如下：

一种通行区域的路径融合规划方法，该路径融合规划方法包括：步骤1、在栅格地图中设置导航起点和导航终点，并创建待遍历节点缓存空间；步骤2、将导航起点设置为当前父节点，并加入待遍历节点缓存空间；步骤3、以当前父节点为搜索中心在栅格地图中进行邻域搜索，其中，与当前父节点相邻的8个栅格点分别作为其子节点；步骤4、当步骤3在邻域搜索到的子节点是预先搜索出的候选路线坐标集合内对应的一条候选路线的一个端点时，将对应的一条候选路线的另一个端点加入待遍历节点缓存空间；同时将对应的一条候选路线的两个端点之间的所有中间节点与该端点都设置为不可重复搜索的节点，并将步骤3搜索到的不在所述对应的一条候选路线上的空闲栅格点加入待遍历节点缓存空间；然后进入步骤5；其中，所述候选路线坐标集合内对应的一条候选路线是允许穿过栅格地图上标记的障碍物栅格点；所有加入待遍历节点缓存空间的节点都对应记录下其父节点的位置信息，用以后续回溯路径节点；步骤5、从步骤4所述的待遍历节点缓存空间内选择出路径代价和值最小的节点作为下一个父节点，再判断所述下一个父节点是否为所述导航终点，是则基于前述记录的父节点的位置信息，从所述导航终点开始，除了步骤4所述候选路线的所有中间节点及其相应的一个端点之外，依次连接子节点及其父节点，直至连接到所述导航起点，规划出从所述导航起点到所述导航终点的路径；否则将所述下一个父节点更新步骤3所述的当前父节点，再返回步骤3。

与现有技术相比，该路径规划方案从导航起点开始，依次从待遍历节点缓存空间内选择出路径代价和值最小的节点作为父节点进行邻域搜索，并将邻域搜索得到的不处于候选路线上的空闲栅格点和处于候选路线上具有标识意义的一个端点（起点或终点）都加入待遍历节点缓存空间，并记录相应的父节点以作为路径节点回溯信息，同时排除掉候选路线上的中间节点（虽然都是适用于这些狭窄通道的导航目标点）的对重复搜索造成的干扰，直到作为父节点是导航终点，再按照子节点和父节点的位置关系，从导航终点开始通过连接搜索到的候选路线反向连接到导航起点，从而将符合通行条件的候选路线融合到成熟的路径搜索算法中以规划出一条导航路径，克服映射到栅格地图中的狭窄通道较窄而容易产生标记误差，使得机器人沿着规划好的整体导航路径有效进出这一狭窄通道，减少路径导航失败的概率。

进一步地，步骤4还包括:步骤3在邻域搜索到的子节点不是所述候选路线坐标集合内的所有候选路线的端点时，将步骤3在邻域搜索到的空闲栅格点都加入所述待遍历节点缓存空间内，并记录步骤3在邻域搜索到的空闲栅格点的父节点的位置信息，然后进入步骤5。从而扩大子节点的搜索范围，也有利于在更大的范围内去扩展搜索所述候选路线坐标集合内的路线起止点，使得所规划的路径具有更好的实用性和代表性。

进一步地，在所述步骤4中，当步骤3在邻域搜索到的子节点是所述候选路线坐标集合内对应的一条候选路线的起点时，将对应的一条候选路线的终点加入所述待遍历节点缓存空间，并记录所述对应的一条候选路线的终点的父节点的位置信息，以使得在所述候选路线坐标集合内，每个候选路线使用其终点作为路线标识信息；同时将对应的一条候选路线的两个端点之间的所有中间节点与同一条候选路线的起点都设置为不可重复搜索的节点；其中，沿着所述对应的一条候选路线的起点指向其终点的路径延伸方向，所述对应的一条候选路线上的每一个节点的父节点位于与该节点相邻的节点位置处。

进一步地，在所述步骤4中，当步骤3在邻域搜索到的子节点是所述候选路线坐标集合内对应的一条候选路线的终点时，将对应的一条候选路线的起点加入所述待遍历节点缓存空间，并记录对应的一条候选路线的起点的父节点的位置信息，以使得在所述候选路线坐标集合内，每个候选路线使用其起点作为路线标识信息；同时将对应的一条候选路线的两个端点之间的所有中间节点与同一条候选路线的终点都设置为不可重复搜索的节点；其中，沿着所述对应的一条候选路线的终点指向其起点的路径延伸方向，所述对应的一条候选路线上的每一个节点的父节点位于与该节点相邻的节点位置处。

步骤4所对应的两种技术方案，将所述候选路线的起点或其终点配置为能唯一确定一条候选路线，作为对应一条候选路线的标识并记录对应的父节点信息，而同一候选路线的剩余节点则配置为已重复搜索的节点，减少路径规划过程中的搜索量。

进一步地，在所述步骤5中，所述基于前述记录的父节点的位置信息，从所述导航终点开始，除了步骤4所述候选路线的所有中间节点及其相应的一个端点之外，依次连接子节点及其父节点，直至连接到所述导航起点的具体步骤包括：步骤51、基于前述记录的父节点的位置信息，从所述导航终点开始，连接所述导航终点及其父节点；然后进入步骤52；步骤52、以当前确定的父节点为子节点，再基于前述记录的父节点的位置信息，连接该子节点的父节点；步骤53、重复步骤52，直至连接到步骤4所述对应的一条候选路线的一个端点，然后从步骤4所述对应的一条候选路线的另一个端点开始，连接步骤4所述对应的一条候选路线的另一个端点及其父节点，然后返回步骤52；步骤54、重复步骤52和步骤53，直至连接到所述导航起点，实现逆向连接得到从导航起点到导航终点的一条导航路径。

与现有技术相比，规划出上述导航路径之后，本技术方案实现：在机器人经过狭窄通道的过程中，运用启发式搜索算法或增量启发式搜索算法搜索路径时，若检测到狭窄通道，则在狭窄通道附近由进入启发式搜索算法或增量启发式搜索算法搜索出的导航路径切换为进入所述候选路线；若检测到离开狭窄通道，则由所述候选路线切换为进入启发式搜索算法或增量启发式搜索算法搜索出的导航路径；让机器人可以有效穿过狭窄通道，提高穿越狭窄通道的导航成功率。

进一步地，在所述步骤5中，所述路径代价和值是已遍历路径代价与预测路径代价相加或加权得到，其中，已遍历路径代价是所述待遍历节点缓存空间内的一个指定节点距离所述导航起点的代价，预测路径代价是所述待遍历节点缓存空间内的同一个指定节点距离所述导航终点的代价；当所述路径代价和值越小，则配置指定节点在所述待遍历节点缓存空间内的遍历优先级越高；其中，两个相邻节点之间的移动代价使用曼哈顿距离、对角距离或欧式距离来表示。有利于搜寻出导航成本最低的最短路径。

进一步地，从导航起点开始，依次连接每个已到遍历过的栅格中心点直至连接到所述指定节点，再以栅格的边长获取当前连接而成的连线的长度，并将该长度作为所述已遍历路径代价；将所述指定节点设置为父节点，获取该父节点对应的每个子节点到导航终点的所有连线方案，再以栅格的边长获取每个子节点对应的每个连线方案的长度，选取每个子节点对应的最短长度作为其对应的预测路径代价；其中，栅格点用栅格中心点表示，用于代表一个栅格的位置特征。简化路径代价的计算方式。其中，本技术方案计算的代价值是对机器人运动轨迹成本的度量，表示由起点行驶到指定节点，再行驶到终点的成本，包括路径长度。

进一步地，所述路径融合规划方法还创建一个已遍历节点缓存空间，用于存储所述步骤4中设置为不可重复搜索的节点；其中，存在于所述已遍历节点缓存空间内节点不允许加入所述待遍历节点缓存空间。以实现在规划过程中，标识出已搜索出的节点或视为已搜索出的节点。

进一步地，所述步骤4还包括：若将当前父节点的邻域中的不位于所述对应的一条候选路线上的空闲栅格点或当前父节点的邻域中的所有空闲栅格点都加入待遍历节点缓存空间，则从所述待遍历节点缓存空间中移除这个当前父节点，再将这个当前父节点加入所述已遍历节点缓存空间。防止同一搜索中心进行重复扩展。

进一步地，在执行所述路径融合规划方法之前，所述候选路线坐标集合内相对应的候选路线的搜索方法具体包括：步骤S1、控制机器人沿着预设路径移动的过程中，直至判断到预搜索区域满足第一预设圆域通行条件，再进入步骤S2；步骤S2、将机器人的当前位置记录为第一路径节点，同时创建一个新的预测通行坐标集合,并将第一路径节点存入所述预测通行坐标集合，然后进入步骤S3；步骤S3、控制机器人继续沿着所述预设路径移动，直至判断到机器人移动至与最新记录的路径节点的直线距离为大于或等于机器人的机身直径的位置，再进入步骤S4；步骤S4、将机器人的当前位置记录为第二路径节点，同时判断第二路径节点是否满足第二预设圆域通行条件，是则进入步骤S5，否则进入步骤S6；步骤S5、判定机器人当前处于所述狭窄通道，并将第二路径节点加入步骤S2所述的预测通行坐标集合，再返回步骤S3；步骤S6、判定机器人当前没有处于所述狭窄通道，并根据所述预测通行坐标集合内部存有的路径节点的数目，保存所述预测通行坐标集合到同一个候选路线坐标集合的内部，并将所述预测通行坐标集合内存储的路径节点按照先后加入的次序连接成对应的一条候选路线，然后返回步骤S1；其中，同一个候选路线坐标集合的内部，任一个所述预测通行坐标集合内部的首元素及其尾元素都是唯一的，且同一个所述预测通行坐标集合内部的首元素及其尾元素不是相同的；其中，所述狭窄通道是两个或两个以上障碍物的夹缝通道，夹缝通道是其中两个障碍物之间的最窄处对应的缝隙，这个缝隙宽度是大于或等于机器人的机身直径；其中，所述预设路径是机器人预先规划的路径；路径节点支持用栅格点表示。

与现有技术相比，本技术方案适用于机器人进入狭窄通道的状态下实施路线搜索，先设置第一预设圆域通行条件作为狭窄通道的预判条件，并提供所述预测通行坐标集合对应的候选路线的路径节点来源；再让机器人沿着原路径继续移动，然后设置第二预设圆域通行条件作为狭窄通道的精判条件，并继续提供所述预测通行坐标集合对应的候选路线的栅格点来源，使得移动一定的距离后机器人才具备判断狭窄通道的资格，所述预测通行坐标集合收集的用于连成栅格点的候选路线更加的完整，更加适应于狭窄通道下的地图栅格误差环境，为机器人提供实际可通行的路线而无需实时关注相应栅格的标记信息。另外，在此基础上，在不满足相应的预设圆域通行条件时，停止为所述预测通行坐标集合继续搜索，并确定单独一个所述预测通行坐标集合内的路径节点可以连接为一条独立的候选路线。通过迭代执行前述相关步骤机器人在正常工作移动的状态下搜索能够克服地图漂移误差造成栅格不可通行问题的路线点集合。增加机器人在障碍物空间布局较复杂的场景中找到有效行驶路径的成功率。

进一步地，若机器人当前检测到由处于所述狭窄通道变为没有处于所述狭窄通道，则执行所述步骤1至所述步骤5，以开始执行所述路径融合规划方法。实现迭代处理所述步骤1至所述步骤5，从而在机器人离开所述狭窄通道时，将已搜索过的适应于穿过所述狭窄通道的候选路线的坐标点融合到当前位置处使用启发式搜索算法中以规划出一条导航路径，而不是只依靠邻域搜索来进行导航路径规划，克服在狭窄通道处搜索到的空闲栅格点容易被标记为障碍物栅格点而导致规划的路径无法通过狭窄通道的问题。

进一步地，所述根据所述预测通行坐标集合内部存有的路径节点的数目，保存所述预测通行坐标集合到同一个候选路线坐标集合的方法具体包括：当所述预测通行坐标集合内部存有的路径节点的数目小于2时，删除所述预测通行坐标集合，再返回步骤S1以创建一个新的预测通行坐标集合；当所述预测通行坐标集合内部存有的路径节点的数目大于或等于2时，将所述预测通行坐标集合保存到所述候选路线坐标集合内，以对应保存一条新的候选路线，再返回步骤S1以创建一个新的预测通行坐标集合；其中，所述第一路径节点和所述第二路径节点是按照记录的先后顺序加入所述预测通行坐标集合内，使得所述预测通行坐标集合内存储的路径节点以一定的次序连接成对应的一条候选路线；其中，每一个预测通行坐标集合内的首元素对应的路径节点是对应的一条候选路线的起点，每一个预测通行坐标集合内的尾元素对应的路径节点是对应的一条候选路线的终点。使得每一个预测通行坐标集合内的路径节点都匹配连接成一条候选路线，形成预测机器人在相应区域内能够无障碍通行的候选路线；在该技术方案中，若一个预测通行坐标集合最终获取的路径节点的数目少到难以连接出一条线时，可删除，减少无效的路径节点；一个预测通行坐标集合最终获取的路径节点作为整体保存到用于保存一套候选路线的所述候选路线坐标集合，使得候选路线的存取结构合理有序。

进一步地，所述第一预设圆域通行条件包括：所述预搜索区域内的第一不可通行区域所占的面积比例大于第一通行评估值；其中，第一不可通行区域是在所述第一圆形区域对应的栅格区域内，由未知栅格点和障碍物栅格点组成的栅格区域；第一通行评价值是为了克服构建栅格地图中存在的空闲栅格的标记误差而设置的预判阈值；其中，所述预搜索区域是以机器人的当前位置为圆心、机器人的机身直径为半径的第一圆形区域。可以用于初步判断机器人是否开始进入狭窄通道，属于一种粗糙判断条件，后续有赖于机器人在继续移动过程中作进一步的判断。但不用考虑单个栅格的标记信息是否允许机器人通行，减少单个栅格的标记误差的影响。

进一步地，所述第二预设圆域通行条件包括：在以机器人的当前位置为圆心、机器人的机身直径为半径的第二圆形区域内，第二不可通行区域所占的面积比例大于第二通行评价值；其中，第二不可通行区域是在第二圆形区域对应的栅格区域内，由未知栅格点和障碍物栅格点组成的栅格区域；第二通行评价值是用于克服构建栅格地图中存在的空闲栅格的标记误差而设置的判断阈值，且大于第一通行评价值。该技术方案在所述第一圆形区域的基础上或上一次搜索的第二圆形区域的基础上，机器人移动至距离最新记录的路径节点一个机器人的机身直径后，进一步地作第二不可通行区域所占的面积比例的判断，不用考虑单个栅格点的标记信息的影响，提高狭窄通道的判断精度。

进一步地，所述第二预设圆域通行条件包括：在以最新记录的第一路径节点为圆心、预设倍数的机身直径为半径的第二圆形区域内，使用路径搜索算法搜索到最新记录的第二路径节点通向最新记录的第一路径节点的路径；其中，预设倍数的机身直径设置为：控制第二圆形区域不与其它已标记的栅格区域交叉。与现有技术相比，该技术方案对于第二圆形区域这一搜索区域的大小设置合理，避免与其他已知地图区域相交叉以把相关区域内规划的路线加入其中，导致机器人被导向其他区域而不再被引导通过当前的狭窄通道；另一方面，还通过判断是否能够在当前探索的第二圆形区域内规划出一段起点通往终点的完整的导航路径，来证明这个第二圆形区域的可通行区域已经不受障碍物的影响或不受障碍物栅格的标记位置的影响；从而提高所述狭窄通道的判断精度。

一种芯片，该芯片用于存储程序代码，程序代码用于执行前述任一技术方案所述路径融合规划方法。用于在障碍物分布较多的狭窄通道内，融合启发式搜索算法和符合搜索条件的候选路线规划出一条导航路径，克服地图栅格中通道较窄而容易产生标记误差，使得机器人沿着规划好的整体导航路径有效进出这一狭窄通道，减少障碍物空间布局较复杂的场景中的路径导航失败的概率。

一种机器人，该机器人内置所述芯片，该芯片用于控制机器人执行所述路径融合规划方法。在机器人按照所述路径融合规划方法规划的导航路径经过狭窄通道的过程中，运用启发式搜索算法或增量启发式搜索算法搜索路径时，若检测到狭窄通道，则在狭窄通道附近由进入启发式搜索算法或增量启发式搜索算法搜索出的导航路径切换为进入所述候选路线；若检测到离开狭窄通道，则由所述候选路线切换为进入启发式搜索算法或增量启发式搜索算法搜索出的导航路径；让机器人可以有效穿过狭窄通道，提高穿越狭窄通道的导航成功率。

附图说明

图1是本发明一实施例公开一种通行区域的路径融合规划方法的流程图。

图2是本发明另一实施例公开候选路线坐标集合内相对应的候选路线的搜索方法的流程图。

具体实施方式

下面结合附图对本发明的具体实施方式作进一步说明。

需要说明的是，对于本领域技术人员可理解：栅格地图中标记有机器人的当前位置周围的环境信息，机器人构建的地图区域内的栅格包括标记为空闲(free)、占用(occupied) 和未知(unknown)三种状态；这些栅格在本实施例中使用栅格点表示，即栅格的中心点；空闲状态的栅格点是指未被障碍所占用的栅格，是机器人可到达的栅格位置点，是空闲栅格点，可以组成未占用区域；占用状态的栅格点是指被障碍物所占用的栅格，是障碍物栅格点，可以组成占用区域；未知栅格点是指机器人构建地图过程中的不清楚具体情况的栅格区域，其位置点处往往被障碍物所遮挡，可以组成未知区域。

需要说明的是，采用搜索算法解决问题时，需要构造一个表明自身位置的状态特征和不同位置的状态之间关系的数据结构，这种数据结构称为节点。不同的问题需要用不同的数据结构描述。根据搜索问题所给定的条件，从一个节点出发，可以生成一个或多个新的节点，这个过程通常称为扩展。节点之间的关系一般可以表示成相邻接的父节点和子节点。搜索算法的搜索过程实际上就是根据初始条件和扩展规则构造路径去寻找符合目标状态的节点的过程。

需要说明的是，现有技术的智能扫地机器人经常移动至家庭环境中凳子的四条脚、茶几的入口等由各种家具组件之间限制形成的狭窄通道、房间的门打开形成的狭窄通道。需要补充的是，所述狭窄通道是各个障碍物的夹缝通道，其中，各个障碍物的夹缝通道是其中两个障碍物之间的最窄处对应的缝隙，这个缝隙宽度是大于机器人的机身直径的，是允许机器人通行的。由于机器人存在打滑情况、用于定位的传感器存在累计误差或视觉地图优化也产生误差，所以机器人容易在即时扫描构建的栅格地图上将所述狭窄通道标记为障碍物占据的区域，即将所述狭窄通道原本映射的空闲栅格点错误地标记为障碍物占据的区域，导致映射到机器人构建的栅格地图中的狭窄通道的入口是堵住的，而实际运动场景中没有被真正堵住，使得机器人无法使用成熟的路径搜索算法完成一条贯穿所述狭窄通道的导航路径，比如A*算法等启发式搜索算法、或D*算法等增量启发式搜索算法。

因此，本发明公开一种通行区域的路径融合规划方法，本发明需先控制机器人在原先的正常导航工作状态下，预先遍历障碍物分布较多的狭窄通道以搜索出可供融合的候选路线，然后使用所述路径融合规划方法融合启发式搜索算法（主要是这一类搜索算法的路径节点搜索思路）和符合搜索条件的候选路线规划出整体导航路径，克服地图栅格标记误差，以便解决机器人在栅格地图中的通道较窄而容易产生标记误差的可通行区域内的导航路径规划问题。

作为一种实施例，本发明实施例公开一种通行区域的路径融合规划方法，该路径融合规划方法可以实施于机器人完全离开所述狭窄通道之后、或者离开当前的狭窄通道后即将进入新的狭窄通道之前。如图1所示，该路径融合规划方法包括：

步骤S101、在栅格地图中设置导航起点和导航终点，并创建待遍历节点缓存空间；然后进入步骤S102。其中，栅格地图是机器人构建的包括起点和终点的局部地图，这个栅格地图还包括各种布局的障碍物信息，并包含足够的空间区域以便机器人根据导航起点和导航终点进行路径轨迹的规划。导航起点和导航终点可以通过栅格地图的栅格坐标、栅格中心点坐标、或其它类型的导航数据表示，对此不做限定，但都能转换到栅格地图中参与路径规划。另一方面，步骤S101创建待遍历节点缓存空间，并初始化为空的，其内部元素的优先级是与路径代价相关，元素的优先级可以是遍历优先级、规划优先级。

步骤S102、将导航起点设置为当前父节点，具体是以导航起点所在的栅格或栅格点设置为当前父节点，并加入待遍历节点缓存空间；然后进入步骤S103。在一种可选的实施方式中，在将导航起点加入待遍历节点缓存空间前，初始的待遍历节点缓存空间可以为空，则导航起点是加入待遍历节点缓存空间的第一个节点。优选地，位于机器人坐标系的原点（局部坐标系的原点）处的父节点与其在所述四个象限区域内扩展搜索出的子节点都是相邻接的位置关系。

其中，所述待遍历节点缓存空间视为本实施例的路径规划的候选路径节点的集合，将机器人可能经过的路径节点加入其中，以便后续的筛选处理。所述待遍历节点缓存空间可以记录内部的每个路径节点的位置、以及机器人移动到对应路径节点的状态。本示例性实施方式中，对于栅格地图中的路径节点，通过状态参数表示机器人移动到达路径节点时的状态，至于路径节点的状态参数，至少包括：时刻，即机器人到达该路径节点的时刻；此外，状态参数还可以包括：姿态，包括机器人到达路径节点时的方向、转向角(即机器人本身处于直行还是转向的状态，以及转向的角度)等；速度，即机器人到达路径节点时的运动速度。在开始进行路径规划时，可以先确定导航起点的状态参数，其中，可以将当前时刻或零时刻作为导航起点的时刻，将当前机器人运动状态作为起点对应的姿态和速度，例如机器人的起始为静止状态，则导航起点对应的转向角和速度均为0。在一种示例性实施例中，所述待遍历节点缓存空间是优先队列的数据结构，以先入先出的存储顺序加入对应的候选路径节点，保存于机器人的存储空间内。

步骤S103、以当前父节点为中心在栅格地图中进行邻域搜索，其中，与当前父节点相邻的8个栅格点分别作为其子节点；然后进入步骤S104。其中，从父节点（步骤S103所述的当前父节点）开始搜索邻域上的子节点的过程可称为“扩展”。在一次扩展的过程中，可以在当前父节点的邻域中朝着顺时针方向逐个栅格点地进行搜索（邻域搜索），也可以在当前父节点的邻域中朝着逆时针方向逐个栅格点地进行搜索（邻域搜索），以搜索出符合相关条件的子节点。

步骤S104、判断步骤S103在邻域搜索到的子节点是否属于预先搜索出的候选路线坐标集合内对应的一条候选路线的一个端点，是则进入步骤S105，否则进入步骤S106。即将判断步骤S103在邻域搜索到的子节点是否属于预先搜索出的候选路线坐标集合内代表候选路线的起点或其终点，以便于将规划的导航路径连接上搜索到的候选路线，用于实际导航行走，而不用考虑实时检测到的栅格点上的标记信息的误差对常规的成熟的路径搜索算法规划的导航路径的影响，尤其是误将空闲栅格点标记为障碍物栅格点，让常规的成熟的路径算法规划的导航路径无法通过该误标记下的障碍物栅格点，不利于机器人通过所述狭窄通道。其中，所述候选路线的一个端点是起点时，所述候选路线的另一个端点是终点；所述候选路线的一个端点是终点时，所述候选路线的另一个端点是起点。

步骤S105、将步骤S104搜索到的子节点所属的一条候选路线的另一个端点加入所述待遍历节点缓存空间，即将所属的一条候选路线的另一个端点也存入步骤S101创建的待遍历节点缓存空间内，同时将步骤S103搜索到的不在步骤S104对应的一条候选路线上的空闲栅格点加入步骤S101创建的同一个待遍历节点缓存空间内，作为后续用于路径规划的候选节点的集合，同时，记录加入所述待遍历节点缓存空间的路径节点的父节点的位置信息，并将路径节点的位置信息连同对应的父节点的位置信息一起存入所述待遍历节点缓存空间内；优选地，还记录子节点及其父节点的时刻、姿态、速度等其他信息到所述待遍历节点缓存空间内；在当前执行的步骤S105中加入所述待遍历节点缓存空间的路径节点中，除了所属的一条候选路线的另一个端点之外，其余路径节点的父节点都是步骤S103所述的当前父节点；优选地，所属的一条候选路线的另一个端点的父节点是在所属候选路线上的相邻的节点或是所属候选路线之外的相邻的节点，以便于搜索到该端点时直接通过其父节点来回溯出该条候选路线的位置分布信息，加快路径规划的速度。

其中，选择所属的一条候选路线的其中一个端点而不是两个端点以及这两个端点之间的中间节点加入所述待遍历节点缓存空间内的原因在于：所属的一条候选路线是预先搜索出来的，在执行所述路径融合规划方法之前就已知搜索出来相关坐标点集合并能依次连接成路径段，这时只需要使用该候选路线的其中一个端点就可以起到相对应的路线的标识作用或作为该路线上相应的起始点的索引，因此，该步骤只使用邻域搜索到的一个端点所属的一条候选路线上的另一个端点（等效于未搜索到的路径节点）加入所述待遍历节点缓存空间内，从而后续可以通过以加入所述待遍历节点缓存空间的端点为中心继续搜索与该候选路线相关联的具备通行意义的新的路径节点。至于不在步骤S104对应的一条候选路线上的属于所述当前父节点的邻域的空闲栅格点，则用以提供具有最短路径基础的空闲栅格点。

因此，在执行步骤S107的过程中，将步骤S104搜索到的子节点所属的一条候选路线的两个端点之间的所有中间节点与步骤S104搜索到的端点都设置为不可重复搜索的节点。本实施例还为此专门创建一个已遍历节点缓存空间，用于存储设置为不可重复搜索的节点；需要说明的是，存在于所述已遍历节点缓存空间内节点不允许加入所述待遍历节点缓存空间，以实现在规划过程中，标识出已遍历的节点或视为已搜索出的节点，避免被重复搜索处理。其中，步骤S105和步骤S107可以是同时执行的。在执行完步骤S107之后进入执行步骤S108。

优选地，若将所述当前父节点的邻域中的不位于所述对应的一条候选路线上的空闲栅格点或当前父节点的邻域中的所有空闲栅格点都加入待遍历节点缓存空间，则从所述待遍历节点缓存空间中移除这个当前父节点，再将这个当前父节点加入所述已遍历节点缓存空间，防止出现同一搜索中心进行重复扩展的现象，因为当前父节点已经在步骤S103执行过一次扩展。所述已遍历节点缓存空间可以以列表这一数据存储结构的形式存在于机器人的内部。

值得注意的是，所述候选路线坐标集合内对应的一条候选路线是允许穿过栅格地图上标记的障碍物栅格点，而不考虑这个障碍物栅格点是否为地图误差而标记出的。

在本实施例中，栅格标记误差是由于传感器误差或地图漂移误差或障碍物移动等误差反映到栅格地图中形成的。

步骤S106、步骤S103在邻域搜索到的子节点不是所述候选路线坐标集合内的所有候选路线的端点时，将步骤S103在邻域搜索到的空闲栅格点都加入所述待遍历节点缓存空间内，并记录步骤S103在邻域搜索到的空闲栅格点的父节点的位置信息，然后进入步骤S108。从而扩大邻域搜索过程中的子节点的搜索范围，也有利于在更大的范围内去扩展搜索所述候选路线坐标集合内的路线起止点，使得所规划的路径具有更好的实用性和代表性。

作为步骤S105的一种实施例，当步骤S103在邻域搜索到的子节点是所述候选路线坐标集合内对应的一条候选路线的起点时，将对应的一条候选路线的终点加入所述待遍历节点缓存空间，并记录对应的一条候选路线的终点的父节点的位置信息，以使得在所述候选路线坐标集合内，每个候选路线使用其终点作为路线标识信息；同时将对应的一条候选路线的两个端点之间的所有中间节点与同一条候选路线的起点都设置为不可重复搜索的节点。沿着所述对应的一条候选路线的起点指向其终点的路径延伸方向，所述对应的一条候选路线上的每一个节点的父节点位于与该节点相邻的节点位置处。具体地，在所述候选路线坐标集合内，从所述对应的一条候选路线的起点开始，从这条候选路线的起点指向其终点的第一路径延伸方向，依次记录这条候选路线的每个节点的父节点的位置信息，直至记录到这条候选路线的终点的父节点的位置信息，其中，这条候选路线的每一个节点的父节点是其在所属候选路线的第一路径延伸方向上的相邻的节点。所以，这条候选路线的终点的父节点是位于所属候选路线之外且与这条候选路线的终点相邻，而这条候选路线的终点的子节点是位于所属候选路线上且与这条候选路线的终点相邻；这条候选路线的起点的父节点是位于所属候选路线上且与这条候选路线的起点相邻，而这条候选路线的起点的子节点是位于所属候选路线上且与这条候选路线的起点相邻。

作为步骤S105的另一种实施例，当步骤S103在邻域搜索到的子节点是所述候选路线坐标集合内对应的一条候选路线的终点时，将对应的一条候选路线的起点加入所述待遍历节点缓存空间，并记录对应的一条候选路线的起点的父节点的位置信息，以使得在所述候选路线坐标集合内，每个候选路线使用其起点作为路线标识信息；同时将对应的一条候选路线的两个端点之间的所有中间节点与同一条候选路线的终点都设置为不可重复搜索的节点。其中，沿着所述对应的一条候选路线的终点指向其起点的路径延伸方向，所述对应的一条候选路线上的每一个节点的父节点位于与该节点相邻的节点位置处。具体地，在所述候选路线坐标集合内，从这条候选路线的终点开始，沿着所述对应的一条候选路线的终点指向其起点的第二路径延伸方向，依次记录这条候选路线的每个节点的父节点的位置信息，直至记录到这条候选路线的起点的父节点的位置信息，这条候选路线的每一个节点的父节点是其在所属候选路线的第二路径延伸方向上的相邻的节点，所以，这条候选路线的起点的父节点是位于所属候选路线之外且与这条候选路线的起点相邻，而这条候选路线的起点的子节点是位于所属候选路线上且与这条候选路线的起点相邻。

上述基于步骤S105的两种实施例，将所述候选路线的起点或其终点配置为能唯一确定一条候选路线，作为对应一条候选路线的标识并记录对应的父节点信息，而同一候选路线的剩余节点则配置为已重复搜索的节点，减少路径规划过程中的搜索量，加快回溯出相匹配的候选路线。

步骤S108、从前述待遍历节点缓存空间内选择出路径代价和值最小的节点作为下一个父节点，再进入步骤S109。在本实施例中，通过通过比较所述待遍历节点缓存空间内所有节点的路径代价，选取一个更优的节点作为下一个父节点；其中，路径代价和值越小的节点优先级越高，路径代价和值最小（优先级最高）的节点作为下一个父节点，开始新的一次的邻域搜索，继续以所述下一个父节点为搜索中心搜索并加入新的邻域栅格点（区别于已存在于所述已遍历节点缓存空间内的节点）。

在本实施例中，所述路径代价和值是已遍历路径代价与预测路径代价相加或加权获得的，其中，已遍历路径代价是所述待遍历节点缓存空间内的一个指定节点距离所述导航起点的代价，预测路径代价是所述待遍历节点缓存空间内的同一个指定节点距离所述导航终点的代价，这些路径代价的定义是来源于A*算法的启发函数。当所述路径代价和值越小，则配置指定节点在所述待遍历节点缓存空间内的遍历优先级越高；本实施例基于A*启发函数，在计算两个相邻节点之间的移动代价时使用曼哈顿距离、对角距离或欧式距离来表示；其中，如果机器人当前所处的地图区域中只允许朝上下左右四个方向移动，则可以使用曼哈顿距离；如果机器人当前所处的地图区域中允许朝八个方向移动，则可以使用对角距离；如果机器人当前所处的地图区域中允许朝任何方向移动，则可以使用欧式距离。有利于搜寻出导航成本最低的最短路径。

在计算路径代价时，从导航起点开始，依次连接每个已到遍历过的栅格中心点直至连接到所述指定节点，再以栅格的边长获取当前连接而成的连线的长度，并将该长度作为所述已遍历路径代价；在计算路径代价时，将所述指定节点设置为父节点，获取该父节点对应的每个子节点到导航终点的所有连线方案，再以栅格的边长获取每个子节点对应的每个连线方案的长度，选取每个子节点对应的最短长度作为其对应的预测路径代价；其中，前述栅格点用栅格中心点表示，用于代表一个栅格的位置特征。本实施例计算的代价值是对机器人运动轨迹成本的度量，表示由起点行驶到指定节点，再行驶到终点的成本，包括路径长度。

需要说明的是，对于每个节点，可以计算其代价值，代价值是对机器人的运动轨迹成本的度量，表示由起点运动到该节点，再运动到终点的成本，包括路径长度、所需时间、是否发生碰撞、是否频繁切换速度方向等因素。

步骤S109、判断所述下一个父节点是否为所述导航终点，是则进入步骤S110，否则进入步骤S111。

步骤S111、将所述下一个父节点更新步骤S103所述的当前父节点，再返回步骤S103。

对于栅格地图，前述步骤对应的实施方式实质上是从导航起点开始，不断扩展到达导航终点的过程，扩展搜索的路径节点与所述候选路线连接形成最终规划出的导航路径。因此，步骤S111是属于循环执行的触发步骤，每一次返回步骤S103则为一次扩展，使得机器人从当前父节点出发，经过预设间隔时间的行驶，所能到达的节点为子节点；简而言之，每一次扩展表示机器人“走一步”，对应在地图中跨越一次栅格；需要说明的是，预设间隔时间是每次扩展的周期性时间，表示较小的时间单位，例如可以是5秒、10秒等，预设间隔时间越短，所规划的导航路径越精细，因此可以根据实际需求确定预设间隔时间。

在本实施例中，后续以下一个父节点为搜索中心进行扩展的过程中，若在邻域中搜索到当前父节点作为所述下一个父节点的子节点，则不用将当前父节点加入所述待遍历节点缓存空间内。

步骤S110、确定所述待遍历节点缓存空间内存在导航终点，即在前述的邻域搜索过程中已经搜索到（或扩展至）导航终点，则基于前述步骤记录下的父节点的位置信息，从所述导航终点开始，除了步骤S104所述候选路线的所有中间节点及其相应的一个端点之外，依次连接子节点及其父节点，直至连接到所述导航起点，规划出从所述导航起点到所述导航终点的路径。

与现有技术相比，前述实施例控制机器人在同一位置处，从导航起点开始依次从待遍历节点缓存空间内选择出路径代价和值最小的节点作为父节点进行邻域搜索，并将邻域搜索得到的不处于候选路线上的空闲栅格点和处于候选路线上具有标识意义的一个端点（起点或终点）都加入待遍历节点缓存空间，并记录相应的父节点以作为路径节点回溯信息，同时排除掉候选路线上的中间节点（虽然都是适用于这些狭窄通道的导航目标点）的对重复搜索造成的干扰，直到作为父节点是导航终点，再按照子节点和父节点的位置关系，从导航终点开始通过连接搜索到的候选路线反向连接到导航起点，从而将符合通行条件的候选路线融合到启发式搜索算法中以规划出一条导航路径，克服地图栅格中通道较窄而容易产生标记误差，使得机器人沿着规划好的整体导航路径有效进出这一狭窄通道，减少路径导航失败的概率。

具体地，当判断到所述的下一个父节点为所述导航终点时，所述基于前述记录的父节点的位置信息，从所述导航终点开始，除了步骤4所述候选路线的所有中间节点及其相应的一个端点之外，依次连接子节点及其父节点，直至连接到所述导航起点的具体步骤包括：

步骤51、从所述导航终点开始，基于已经记录下的父节点的位置信息或存储于所述待遍历节点缓存空间内的父节点及子节点的位置信息，连接所述导航终点及其父节点；然后进入步骤52。从而开始逐个节点地进行逆向连接。

步骤52、以当前确定的父节点为子节点，即将步骤51确定的父节点更新为子节点，包括位置信息；再基于前述记录的父节点的位置信息，连接该子节点的父节点；然后进入步骤53。

步骤53、重复步骤52，直至连接到步骤S105确定搜索出的对应一条候选路线的一个端点，然后从这条候选路线的另一个端点开始，连接所述对应的一条候选路线的另一个端点及其父节点，然后返回步骤52。其中，所述对应的一条候选路线的另一个端点的父节点是：步骤S105确定搜索出的端点指向所属的一条候选路线的另一端点的路径延伸方向上，与所述对应的一条候选路线的另一个端点相邻的且位于这条候选路线的外部的节点。

作为一种实施例，当步骤53连接到步骤S105确定搜索出的对应一条候选路线的起点时，基于前述实施例可知，所述对应一条候选路线的起点是步骤52更新过的子节点的父节点，即所述对应一条候选路线的起点成为所述对应的一条候选路线的起点指向其终点的路径延伸方向的反方向上与其相邻的节点的父节点，由此可确定沿着所述对应的一条候选路线的起点指向其终点的路径延伸方向，所述对应的一条候选路线上的每一个节点的父节点位于与该节点相邻的节点位置处，同时确定这条候选路线的起点及中间节点都属于不可重复搜索的节点，即已遍历的路径节点。所以步骤53从这条候选路线的终点开始，连接这条候选路线的终点及其父节点，因为这条候选路线的终点的父节点在这条候选路线的外部，以便于返回步骤52继续回溯至这条候选路线的外部区域。

作为另一种实施例，当步骤53连接到步骤S105确定搜索出的对应一条候选路线的终点时，基于前述实施例可知，所述对应一条候选路线的终点是步骤52更新过的子节点的父节点，即所述对应一条候选路线的终点成为所述对应的一条候选路线的起点指向其终点的路径延伸方向上与其相邻的节点的父节点，由此可确定沿着所述对应的一条候选路线的终点指向其起点的路径延伸方向，所述对应的一条候选路线上的每一个节点的父节点位于与该节点相邻的节点位置处，同时确定这条候选路线的起点及中间节点都属于不可重复搜索的节点，即已遍历的路径节点。所以步骤53从这条候选路线的起点开始，连接这条候选路线的起点及其父节点，因为这条候选路线的起点的父节点在这条候选路线的外部，以便于返回步骤52继续回溯至这条候选路线的外部区域。

步骤54、重复前述步骤52和前述步骤53，直至连接到所述导航起点，实现基于前述记录下的父节点的位置信息将前述步骤S101至步骤S111搜索出的父节点逐个逆向连接以得到从导航起点到导航终点的一条导航路径。在执行步骤51至步骤54的过程中，依次连接该子节点（包括候选路线的端点）的父节点再以该父节点为子节点的过程可称为“逆向扩展”，实质上是从导航终点开始，通过不断扩展并连接到候选路线的起止点，直至到达导航起点的路径节点回溯过程，逆向扩展的轨迹即：获得连接导航起点、能够穿越障碍物栅格点的候选路线、导航终点的导航路径。

与现有技术相比，经过前述步骤规划出上述导航路径之后，本实施例实现：在机器人经过狭窄通道的过程中，运用启发式搜索算法或增量启发式搜索算法搜索路径时，若检测到狭窄通道，则在狭窄通道附近由进入启发式搜索算法或增量启发式搜索算法搜索出的导航路径切换为进入所述候选路线；若检测到离开狭窄通道，则由所述候选路线切换为进入启发式搜索算法或增量启发式搜索算法搜索出的导航路径；让机器人可以有效穿过狭窄通道，提高穿越狭窄通道的导航成功率。

优选地，还可以将每个节点的状态参数标记入所述导航路径中，其中，每个节点的状态参数包括每个节点的时刻，即根据规划，机器人移动至每个节点的时刻。由此，可以将状态参数添加到所述导航路径中，例如将每个节点的时刻标注到导航路径中，当地图中存在所述狭窄通道时，可以推算出机器人是否会长时间困于所述狭窄通道内，从而在复杂障碍物布局场景中实现有效的路径规划，满足实际需求。

值得注意的是，若没有新的子节点加入所述待遍历节点缓存空间，无法在邻域中继续扩展，则表示可能发生在地图中存在难以逾越的障碍物时，比如障碍物之间形成的通道小于机身宽度。

另外，若重复执行步骤52至步骤53达到特定次数，仍然未能连接到所述导航起点，说明处理过程可能发生了异常，该特定次数可以和导航起点与导航终点之间的距离相关。

作为另一种实施例，本发明还公开所述候选路线坐标集合内相对应的候选路线的搜索方法，该搜索方法在执行所述路径融合规划方法之前执行，如图2所示，具体包括：

步骤S201、在控制机器人沿着预设路径移动的过程中，实时判断预搜索区域是否满足第一预设圆域通行条件，是则进入步骤S202，否则控制机器人继续沿着预设路径移动。其中，所述预设路径是机器人预先规划的路径；路径节点支持用栅格点表示；预设路径是机器人正常工作路径或导航路径，当机器人是扫地机器人时，预设路径可以是弓字型移动路径、沿边行走路径、回字形路径等规划清扫路径，在所述预设路径上移动的机器人，可以在正常工作过程中进行适用于穿越所述狭窄通道的路线坐标点或路线搜索，或者在进入狭窄通道过程中进行相应的路线搜索。需要说明的是，所述预搜索区域是以机器人的当前位置为圆心、机器人的机身直径为半径的第一圆形区域，覆盖到机器人周围的最小可通行区域。

步骤S202、将机器人的当前位置记录为第一路径节点，同时创建一个新的预测通行坐标集合,并将第一路径节点存入所述预测通行坐标集合，然后进入步骤S203。在本实施例中，所述预测通行坐标集合内部的元素是配置为按顺序存储节点，以形成一条路线的路径节点的集合，其中，所述预测通行坐标集合内部的首元素及其尾元素都是唯一的，使得所述预测通行坐标集合内部的首元素或其尾元素成为其表示的路线的唯一标识信息，可以作为规划路径的过程中的索引节点或作为回溯路径的标示节点，而且，同一个所述预测通行坐标集合内部的首元素及其尾元素不是相同的，因为在同一个所述预测通行坐标集合内部的首元素及其尾元素相同时，所述预测通行坐标集合内部的元素按照记录的先后顺序围成封闭的图形，形成没有导航意义的路径。

在本实施例执行步骤S201时，若机器人在其当前位置处判断到所述预搜索区域是满足第一预设圆域通行条件，则记录机器人的当前位置为新的第一路径节点并加入预测通行坐标集合，能够配置为对应的候选路线的起点，等效于导航出发点。

在本实施例中，所述第一预设圆域通行条件包括：所述预搜索区域内的第一不可通行区域所占的面积比例大于第一通行评估值；其中，第一不可通行区域是在所述第一圆形区域对应的栅格区域内，由未知栅格点和障碍物栅格点组成的栅格区域；第一通行评价值是为了克服构建栅格地图中存在的空闲栅格的标记误差而设置的预判阈值，是在所述狭窄通道内反复实验的结果；第一通行评价值优选地设置为50%。当机器人判断到所述第一圆形区域对应的栅格区域内的第一不可通行区域所占的面积比例小于或等于50%时，控制机器人维持沿着预设路径移动，直到机器人判断到所述第一圆形区域对应的栅格区域内的第一不可通行区域所占的面积比例大于50%。因此，所述第一预设圆域通行条件用于初步判断机器人是否开始进入狭窄通道，属于一种粗糙判断条件，后续有赖于机器人在继续移动过程中作进一步的判断。但不用考虑单个栅格的标记信息是否允许机器人通行，减少单个栅格的标记误差的影响。

步骤S203、在控制机器人继续沿着所述预设路径移动，实时判断机器人是否移动至与最新记录的路径节点的直线距离为大于或等于机器人的机身直径的位置，是则进入步骤S204，否则控制机器人继续沿着所述预设路径移动。该步骤S203检测机器人至与上一次记录的路径节点的直线距离，包括机器人的当前位置至与第一路径节点的直线距离，其中，机器人保持实时检测当前位置与上一次记录的路径节点的直线距离，也可以是每隔一定的检测周期才进行距离采样检测，在满足一定的通行条件时才将实时检测到的路径节点记录下来。

步骤S204、检测到机器人的当前位置与最新记录的路径节点的直线距离为大于或等于机器人的机身直径时，将机器人的当前位置记录为第二路径节点，然后进入步骤S205。

步骤S205、判断所述第二路径节点是否满足第二预设圆域通行条件，是则进入步骤S206，否则进入步骤S207；因此，前述步骤S204最新记录的路径节点包括上一次记录的第一路径节点或上一次记录的第二路径节点。其中，步骤S204记录的第二路径节点相对于上一次执行步骤S204对应记录下的第二路径节点是新的第二路径节点或步骤S202对应记录下的第一路径节点是新的路径节点。具体地，当由步骤S202执行至步骤S203时，步骤S203所述的最新记录的路径节点是步骤S202记录的第一路径节点，这时，当前记录的第一路径节点和第二路径节点所加入的预测通行坐标集合，不是上一次执行的步骤S202所创建的预测通行坐标集合；当不经过步骤S202而重复执行一次步骤S203（由后续步骤返回迭代执行的）时，步骤S203所述的最新记录的路径节点是上一次执行步骤S204所记录的第二路径节点，这些经过迭代处理获得第二路径节点，都依次保存入同一个预测通行坐标集合内。

作为第二预设圆域通行条件的一种实施方式，所述第二预设圆域通行条件包括：在以机器人的当前位置（即最新记录的第二路径节点）为圆心、机器人的机身直径为半径的第二圆形区域内，第二不可通行区域所占的面积比例大于第二通行评价值；其中，第二不可通行区域是在第二圆形区域对应的栅格区域内，由未知栅格点和障碍物栅格点组成的栅格区域；第二通行评价值是用于克服构建栅格地图中存在的空闲栅格的标记误差而设置的判断阈值，且大于第一通行评价值以提高判断精度。第二通行评价值优选地设置为75%。当机器人判断到所述第二圆形区域对应的栅格区域内的第二不可通行区域所占的面积比例小于或等于75%时，进入步骤S207。其中，第二圆形区域的覆盖区域相对于第一圆形区域的覆盖区域不同，因为机器人的位置发生变化；因此在所述第一圆形区域的基础上或上一次搜索的第二圆形区域的基础上，机器人移动至距离最新记录的路径节点一个机器人的机身直径后，进一步地作第二不可通行区域所占的面积比例的判断，不用考虑单个栅格点的标记信息的影响，提高狭窄通道的判断精度。本实施例只是将所述第二圆形区域的探索半径设置为机器人的机身直径，而不是更大的数值，避免控制不相关（探测范围之外的）栅格区域参与计算，减少时间开销。

作为第二预设圆域通行条件的另一种实施方式，所述第二预设圆域通行条件包括：在以最新记录的第一路径节点为圆心、预设倍数的机身直径为半径的第二圆形区域内，使用路径搜索算法搜索到最新记录的第二路径节点通向最新记录的第一路径节点的路径，用于证明当前的第二圆形区域内的路径搜索已经不受障碍物的影响或障碍物栅格的错误标记的影响；由于所述第二圆形区域可能处于狭窄通道的附近，所以最新记录的第二路径节点通向最新记录的第一路径节点之间可能存在障碍物阻碍，且本实施例将可能发生碰撞的节点称为无效节点或非法节点，在规划路径时需要避开这些节点，因此才有使用成熟稳定的路径搜索算法去检验区域的可通行性的必要，本实施例使用的路径搜索算法是A*算法，有效快速地在所述第二圆形区域内搜索出最新记录的第二路径节点通向最新记录的第一路径节点的导航路径。需要说明的是，本实施例可以将所述第一圆形区域或所述第二圆形区域视为清扫区域。其中，预设倍数的机身直径设置为：让第二圆形区域不与其它已标记的栅格区域交叉，以避免将其它已标记的栅格区域内已经规划出的路径参与判断，进而避免出现误判。所述预设倍数的机身直径设置为三分之二的机身直径，使得形成的第二圆形区域大于所述第一圆形区域内。与现有技术相比，本实施例对于第二圆形区域这一搜索区域的大小设置合理，避免与其他已知地图区域相交叉以把相关区域内规划的路线加入其中，导致机器人被导向其他区域而不再被引导通过当前的狭窄通道；另一方面，还通过判断是否能够在当前探索的第二圆形区域内规划出一段起点通往终点的完整的导航路径，来证明这个第二圆形区域的可通行区域已经不受障碍物的影响或不受障碍物栅格的标记位置的影响；从而提高所述狭窄通道的判断精度。

步骤S206、判定机器人当前处于所述狭窄通道，并将第二路径节点加入步骤S202所述的预测通行坐标集合内，再返回步骤S203；因为在机器人识别出其当前位置处于所述狭窄通道后，通过返回步骤S203，继续在所述狭窄通道内沿着所述预设路径移动。因此，在本实施例中，机器人在搜索到第一路径节点后，每移动一个机身直径的直线距离时，判断机器人的当前位置或当前的探索区域是否满足第二预设圆域通行条件；然后，通过第二预设圆域通行条件确定机器人当前处于所述狭窄通道时，继续沿着所述预设路径移动于所述狭窄通道，继续搜索出新的第二路径节点以连接出用于穿过所述狭窄通道的路线。需要说明的是，其中，所述狭窄通道是两个或两个以上障碍物的夹缝通道，夹缝通道是其中两个障碍物之间的最窄处对应的缝隙，这个缝隙宽度是大于或等于机器人的机身直径。

步骤S207、判定机器人当前没有处于所述狭窄通道，并根据所述预测通行坐标集合内部存有的路径节点的数目，保存所述预测通行坐标集合到同一个候选路线坐标集合的内部，并将所述预测通行坐标集合内存储的路径节点按照先后加入的次序连接成对应的一条候选路线，使得一个预测通行坐标集合在所述候选路线坐标集合内成为代表一条候选路线的点集合，然后返回步骤S201，以在所述候选路线坐标集合内创建一个新的预测通行坐标集合以描述一条新的候选路线；其中，同一个候选路线坐标集合的内部，存在多个所述预测通行坐标集合，分别代表其它的互不相同的候选路线，因为同一个候选路线坐标集合的内部，任一个所述预测通行坐标集合内部的首元素及其尾元素都是唯一的。

具体地，根据所述预测通行坐标集合内部存有的路径节点的数目，保存所述预测通行坐标集合到同一个候选路线坐标集合的方法具体包括：当所述预测通行坐标集合内部存有的路径节点的数目小于2时，表示机器人可能检测到难以逾越的障碍物或被困或出现其它异常情况，使得收集到所述预测通行坐标集合内部存有的路径节点是无效节点，则选择删除所述预测通行坐标集合，再返回步骤S201以创建一个新的预测通行坐标集合。因此，在本实施例中，若一个预测通行坐标集合最终获取的路径节点的数目少到难以连接出一条线时，可删除相关的集合以减少无效的路径节点。

当所述预测通行坐标集合内部存有的路径节点的数目大于或等于2时，使用所述预测通行坐标集合表示单独一条候选路线，并保存到所述候选路线坐标集合内，以供路径搜索算法调用，可以是被启发式搜索算法使用，再通过返回步骤S201，以创建一个新的预测通行坐标集合；本实施例中，一个预测通行坐标集合最终获取的路径节点可以通过连接作为整体路线，保存到用于保存一套候选路线的所述候选路线坐标集合，使得候选路线的存取结构合理有序。其中，所述第一路径节点和所述第二路径节点是按照记录的先后顺序加入所述预测通行坐标集合内，使得所述预测通行坐标集合内存储的路径节点以记录好的次序连接成对应的一条候选路线。需要说明的是，每一个预测通行坐标集合内的首元素对应的路径节点是对应的一条候选路线的起点，每一个预测通行坐标集合内的尾元素对应的路径节点是对应的一条候选路线的终点。使得每一个预测通行坐标集合内的路径节点都匹配连接成一条候选路线，形成预测机器人在相应区域内能够无障碍通行的候选路线；

与现有技术相比，前述步骤S201至步骤S207所述的候选路线的搜索方法适用于机器人进入狭窄通道的状态下实施路线搜索，先设置第一预设圆域通行条件作为狭窄通道的预判条件，并提供所述预测通行坐标集合对应的候选路线的路径节点来源；再让机器人沿着原路径继续移动，然后设置第二预设圆域通行条件作为狭窄通道的精判条件，并继续提供所述预测通行坐标集合对应的候选路线的栅格点来源，使得移动一定的距离后机器人才具备判断狭窄通道的资格，所述预测通行坐标集合收集的用于连成栅格点的候选路线更加的完整，更加适应于狭窄通道下的地图栅格误差环境，为机器人提供实际可通行的路线而无需实时关注相应栅格的标记信息。另外，在此基础上，在不满足相应的预设圆域通行条件时，停止为所述预测通行坐标集合继续搜索，并确定单独一个所述预测通行坐标集合内的路径节点可以连接为一条独立的候选路线。通过迭代执行前述相关步骤机器人在正常工作移动的状态下搜索能够克服地图漂移误差造成栅格不可通行问题的路线点集合。增加机器人在障碍物空间布局较复杂的场景中找到有效行驶路径的成功率。

特别要注意的是，一个预测通行坐标集合内是允许包括障碍物栅格点，表示对应的候选路线是有可能存在障碍物栅格点。由步骤S201至步骤S207可知，机器人自身移动至一个特定的路径节点，比如在一次迭代执行的过程中执行到步骤S202（满足初始状态条件）、或者在一次迭代执行的过程中执行到步骤S206时对应的机器人当前位置，且在该路径节点上判断到满足所述第一预设圆域通行条件或所述第二预设圆域通行条件时，则将该路径节点加入所述预测通行坐标集合，作为连接到对应候选路线上的一个路径节点，然而由于传感器探测误差、地图漂移误差等，该路径节点在栅格地图上的标记的栅格信息可能不是空闲状态，反而被误标记为障碍物占据状态，即所述障碍物栅格点，该路径节点虽然在栅格地图中标记为障碍物栅格点，但是也加入所述预测通行坐标集合，作为对应候选路线上的一个路径节点，而实际机器人是可以移动至该路径节点，证明该路径节点是可通行的或可连通的。

在前述实施例的基础上，若机器人当前检测到由处于所述狭窄通道变为没有处于所述狭窄通道，则开始执行所述路径融合规划方法，即开始迭代执行前述步骤S101至步骤S111，以规划出支持机器人自由进出所述狭窄通道的导航路径，克服地图漂移误差。从而在机器人离开所述狭窄通道时或离开当前的狭窄通道而即将进入新的狭窄通道之前，将已搜索过的适应于穿过所述狭窄通道的候选路线的坐标点融合到当前位置处使用启发式搜索算法中以规划出一条导航路径，而不是只依靠邻域搜索来进行导航路径规划，克服在狭窄通道处搜索到的空闲栅格点容易被标记为障碍物栅格点而导致规划的路径无法通过狭窄通道的问题。

一种芯片，该芯片用于存储程序代码，程序代码用于执行前述任一技术方案所述路径融合规划方法。用于在障碍物分布较多的狭窄通道内，融合启发式搜索算法和符合搜索条件的候选路线规划出一条导航路径，克服地图栅格中通道较窄而容易产生标记误差，使得机器人沿着规划好的整体导航路径有效进出这一狭窄通道，减少障碍物空间布局较复杂的场景中的路径导航失败的概率。

一种机器人，该机器人内置所述芯片，该芯片用于控制机器人执行所述路径融合规划方法。在机器人按照所述路径融合规划方法规划的导航路径经过狭窄通道的过程中，运用启发式搜索算法或增量启发式搜索算法搜索路径时，若检测到狭窄通道，则在狭窄通道附近由进入启发式搜索算法或增量启发式搜索算法搜索出的导航路径切换为进入所述候选路线；若检测到离开狭窄通道，则由所述候选路线切换为进入启发式搜索算法或增量启发式搜索算法搜索出的导航路径，让机器人沿着相应的候选路线连接而成的导航路径，让机器人可以有效穿过狭窄通道，提高穿越狭窄通道的导航成功率。

Claims (17)

1.一种通行区域的路径融合规划方法，其特征在于，该路径融合规划方法包括：

步骤1、在栅格地图中设置导航起点和导航终点，并创建待遍历节点缓存空间；

步骤2、将导航起点设置为当前父节点，并加入待遍历节点缓存空间；

步骤3、以当前父节点为搜索中心在栅格地图中进行邻域搜索，其中，与当前父节点相邻的8个栅格点分别作为其子节点；

步骤4、当步骤3在邻域搜索到的子节点是预先搜索出的候选路线坐标集合内对应的一条候选路线的一个端点时，将对应的一条候选路线的另一个端点加入待遍历节点缓存空间；同时将对应的一条候选路线的两个端点之间的所有中间节点与该端点都设置为不可重复搜索的节点，并将步骤3搜索到的不在所述对应的一条候选路线上的空闲栅格点加入待遍历节点缓存空间；然后进入步骤5；其中，所述候选路线坐标集合内对应的一条候选路线是允许穿过栅格地图上标记的障碍物栅格点；所有加入待遍历节点缓存空间的节点都对应记录下其父节点的位置信息，用以后续回溯路径节点；

步骤5、从步骤4所述的待遍历节点缓存空间内选择出路径代价和值最小的节点作为下一个父节点，再判断所述下一个父节点是否为所述导航终点，是则基于前述记录的父节点的位置信息，从所述导航终点开始，除了步骤4所述候选路线的所有中间节点及其相应的一个端点之外，依次连接子节点及其父节点，直至连接到所述导航起点，规划出从所述导航起点到所述导航终点的路径；否则将所述下一个父节点更新步骤3所述的当前父节点，再返回步骤3。

2.根据权利要求1所述路径融合规划方法，其特征在于，步骤4还包括:

步骤3在邻域搜索到的子节点不是所述候选路线坐标集合内的所有候选路线的端点时，将步骤3在邻域搜索到的空闲栅格点都加入所述待遍历节点缓存空间内，并记录步骤3在邻域搜索到的空闲栅格点的父节点的位置信息，然后进入步骤5。

3.根据权利要求1所述路径融合规划方法，其特征在于，在所述步骤4中，当步骤3在邻域搜索到的子节点是所述候选路线坐标集合内对应的一条候选路线的起点时，将对应的一条候选路线的终点加入所述待遍历节点缓存空间，并记录所述对应的一条候选路线的终点的父节点的位置信息，以使得在所述候选路线坐标集合内，每个候选路线使用其终点作为路线标识信息；同时将对应的一条候选路线的两个端点之间的所有中间节点与同一条候选路线的起点都设置为不可重复搜索的节点；

其中，沿着所述对应的一条候选路线的起点指向其终点的路径延伸方向，所述对应的一条候选路线上的每一个节点的父节点位于与该节点相邻的节点位置处。

4.根据权利要求1所述路径融合规划方法，其特征在于，在所述步骤4中，当步骤3在邻域搜索到的子节点是所述候选路线坐标集合内对应的一条候选路线的终点时，将对应的一条候选路线的起点加入所述待遍历节点缓存空间，并记录对应的一条候选路线的起点的父节点的位置信息，以使得在所述候选路线坐标集合内，每个候选路线使用其起点作为路线标识信息；同时将对应的一条候选路线的两个端点之间的所有中间节点与同一条候选路线的终点都设置为不可重复搜索的节点；

其中，沿着所述对应的一条候选路线的终点指向其起点的路径延伸方向，所述对应的一条候选路线上的每一个节点的父节点位于与该节点相邻的节点位置处。

5.根据权利要求3或4所述路径融合规划方法，其特征在于，在所述步骤5中，所述基于前述记录的父节点的位置信息，从所述导航终点开始，除了步骤4所述候选路线的所有中间节点及其相应的一个端点之外，依次连接子节点及其父节点，直至连接到所述导航起点的具体步骤包括：

步骤51、基于前述记录的父节点的位置信息，从所述导航终点开始，连接所述导航终点及其父节点；然后进入步骤52；

步骤52、以当前确定的父节点为子节点，再基于前述记录的父节点的位置信息，连接该子节点的父节点；

步骤53、重复步骤52，直至连接到步骤4所述对应的一条候选路线的一个端点，然后从步骤4所述对应的一条候选路线的另一个端点开始，连接步骤4所述对应的一条候选路线的另一个端点及其父节点，然后返回步骤52；

步骤54、重复步骤52和步骤53，直至连接到所述导航起点，实现逆向连接得到从导航起点到导航终点的一条导航路径。

6.根据权利要求5所述路径融合规划方法，其特征在于，在所述步骤5中，所述路径代价和值是已遍历路径代价与预测路径代价相加或加权得到，其中，已遍历路径代价是所述待遍历节点缓存空间内的一个指定节点距离所述导航起点的代价，预测路径代价是所述待遍历节点缓存空间内的同一个指定节点距离所述导航终点的代价；当所述路径代价和值越小，则配置指定节点在所述待遍历节点缓存空间内的遍历优先级越高；

其中，两个相邻节点之间的移动代价使用曼哈顿距离、对角距离或欧式距离来表示。

7.根据权利要求6所述路径融合规划方法，其特征在于，从导航起点开始，依次连接每个已到遍历过的栅格中心点直至连接到所述指定节点，再以栅格的边长获取当前连接而成的连线的长度，并将该长度作为所述已遍历路径代价；

将所述指定节点设置为父节点，获取该父节点对应的每个子节点到导航终点的所有连线方案，再以栅格的边长获取每个子节点对应的每个连线方案的长度，选取每个子节点对应的最短长度作为其对应的预测路径代价；

其中，栅格点用栅格中心点表示，用于代表一个栅格的位置特征。

8.根据权利要求1所述路径融合规划方法，其特征在于，所述路径融合规划方法还创建一个已遍历节点缓存空间，用于存储所述步骤4中设置为不可重复搜索的节点；

其中，存在于所述已遍历节点缓存空间内节点不允许加入所述待遍历节点缓存空间。

9.根据权利要求8所述路径融合规划方法，其特征在于，所述步骤4还包括：若将当前父节点的邻域中的不位于所述对应的一条候选路线上的空闲栅格点或当前父节点的邻域中的所有空闲栅格点都加入待遍历节点缓存空间，则从所述待遍历节点缓存空间中移除这个当前父节点，再将这个当前父节点加入所述已遍历节点缓存空间。

10.根据权利要求7所述路径融合规划方法，其特征在于，在执行所述路径融合规划方法之前，所述候选路线坐标集合内相对应的候选路线的搜索方法具体包括：

步骤S1、控制机器人沿着预设路径移动的过程中，直至判断到预搜索区域满足第一预设圆域通行条件，再进入步骤S2；

步骤S2、将机器人的当前位置记录为第一路径节点，同时创建一个新的预测通行坐标集合,并将第一路径节点存入所述预测通行坐标集合，然后进入步骤S3；

步骤S3、控制机器人继续沿着所述预设路径移动，直至判断到机器人移动至与最新记录的路径节点的直线距离为大于或等于机器人的机身直径的位置，再进入步骤S4；

步骤S4、将机器人的当前位置记录为第二路径节点，同时判断第二路径节点是否满足第二预设圆域通行条件，是则进入步骤S5，否则进入步骤S6；

步骤S5、判定机器人当前处于狭窄通道，并将第二路径节点加入步骤S2所述的预测通行坐标集合，再返回步骤S3；

步骤S6、判定机器人当前没有处于所述狭窄通道，并根据所述预测通行坐标集合内部存有的路径节点的数目，保存所述预测通行坐标集合到同一个候选路线坐标集合的内部，并将所述预测通行坐标集合内存储的路径节点按照先后加入的次序连接成对应的一条候选路线，然后返回步骤S1；

其中，同一个候选路线坐标集合的内部，任一个所述预测通行坐标集合内部的首元素及其尾元素都是唯一的，且同一个所述预测通行坐标集合内部的首元素及其尾元素不是相同的；

其中，所述狭窄通道是两个或两个以上障碍物的夹缝通道，夹缝通道是其中两个障碍物之间的最窄处对应的缝隙，这个缝隙宽度是大于或等于机器人的机身直径；

其中，所述预设路径是机器人预先规划的路径。

11.根据权利要求10所述路径融合规划方法，其特征在于，若机器人当前检测到由处于所述狭窄通道变为没有处于所述狭窄通道，则执行所述步骤1至所述步骤5，以开始执行所述路径融合规划方法。

12.根据权利要求10所述路径融合规划方法，其特征在于，所述根据所述预测通行坐标集合内部存有的路径节点的数目，保存所述预测通行坐标集合到同一个候选路线坐标集合的方法具体包括：

当所述预测通行坐标集合内部存有的路径节点的数目小于2时，删除所述预测通行坐标集合，再返回步骤S1以创建一个新的预测通行坐标集合；

当所述预测通行坐标集合内部存有的路径节点的数目大于或等于2时，将所述预测通行坐标集合保存到所述候选路线坐标集合内，以对应保存一条新的候选路线，再返回步骤S1以创建一个新的预测通行坐标集合；

其中，所述第一路径节点和所述第二路径节点是按照记录的先后顺序加入所述预测通行坐标集合内，使得所述预测通行坐标集合内存储的路径节点以一定的次序连接成对应的一条候选路线；

其中，每一个预测通行坐标集合内的首元素对应的路径节点是对应的一条候选路线的起点，每一个预测通行坐标集合内的尾元素对应的路径节点是对应的一条候选路线的终点。

13.根据权利要求10所述路径融合规划方法，其特征在于，所述第一预设圆域通行条件包括：

所述预搜索区域内的第一不可通行区域所占的面积比例大于第一通行评估值；

其中，所述预搜索区域是以机器人的当前位置为圆心、机器人的机身直径为半径的第一圆形区域；

其中，第一不可通行区域是在所述第一圆形区域对应的栅格区域内，由未知栅格点和障碍物栅格点组成的栅格区域；第一通行评价值是为了克服构建栅格地图中存在的空闲栅格的标记误差而设置的预判阈值。

14.根据权利要求13所述路径融合规划方法，其特征在于，所述第二预设圆域通行条件包括：

在以机器人的当前位置为圆心、机器人的机身直径为半径的第二圆形区域内，第二不可通行区域所占的面积比例大于第二通行评价值；其中，第二不可通行区域是在第二圆形区域对应的栅格区域内，由未知栅格点和障碍物栅格点组成的栅格区域；第二通行评价值是用于克服构建栅格地图中存在的空闲栅格的标记误差而设置的判断阈值，且大于第一通行评价值。

15.根据权利要求13所述路径融合规划方法，其特征在于，所述第二预设圆域通行条件包括：

在以最新记录的第一路径节点为圆心、预设倍数的机身直径为半径的第二圆形区域内，使用路径搜索算法搜索到最新记录的第二路径节点通向最新记录的第一路径节点的路径；

其中，预设倍数的机身直径设置为：控制第二圆形区域不与其它已标记的栅格区域交叉。

16.一种芯片，其特征在于，该芯片用于存储程序代码，程序代码用于执行权利要求1至15任一项所述路径融合规划方法。

17.一种机器人，其特征在于，该机器人内置权利要求16所述芯片，该芯片用于控制机器人执行权利要求1至15任一项所述路径融合规划方法。

Images