CN115685982A - 基于连通图和迭代搜索的导航路径规划方法 - Google Patents

Abstract

本发明公开基于连通图和迭代搜索的导航路径规划方法，该导航路径规划方法在机器人预先行走构建出的区域连通图内，按照合理的路径代价搜索出最接近实际起点的路径优化起点和最接近实际终点的路径优化终点，再在迭代搜索的方式的基础上利用启发路径代价和匹配的搜索优先级，完成路径优化起点和路径优化终点之间的导航路径规划。有效减少扩展节点的搜索量，提高区域连通图内的路径节点的搜索速度，提高机器人在区域连通图内的通过性，规划出的路径在地图上的覆盖面增大。

Description

基于连通图和迭代搜索的导航路径规划方法

技术领域

本发明涉及路径规划的技术领域，涉及一种基于连通图和迭代搜索的导航路径规划方法。

背景技术

传统的导航路径规划算法(例如Dijkstra、A*、D*)，对栅格地图的精度和面积依赖比较大，随着栅格的读取精确程度增加或所要表征的地图面积增大，搜索成本呈现指数形态增长，且传统的图搜索算法搜索出的路径对机器人通过性考虑较少。

发明内容

为了解决上述技术问题，本发明技术方案在机器人预先行走构建出的区域连通图内，按照合理的路径代价搜索出最接近实际起点的路径优化起点和最接近实际终点的路径优化终点，再按照迭代搜索的方式完成路径优化起点和路径优化终点之间的导航路径规划。具体技术方案如下：

基于连通图和迭代搜索的导航路径规划方法，包括：在预先构建的区域连通图内，利用节点在所属的运动轨迹线段上的居中程度值换算出的对应的代价值，搜索出与预先设置的导航起点相匹配的路径优化起点以及与预先设置的导航终点相匹配的路径优化终点；其中，每个节点都位于所属的一条运动轨迹线段上，运动轨迹线段是机器人预先移动产生的；在所述区域连通图内所有的待扩展节点中，若启发路径代价和最小的待扩展节点是所述路径优化终点时，根据所述区域连通图内的节点对应的父节点信息，逐层地搜索出从所述路径优化起点到所述路径优化终点的优化导航路径；再将当前搜索出的优化导航路径的两端分别连接上所述导航起点和所述导航终点，完成导航路径的规划；在所述区域连通图内所有的待扩展节点中，若启发路径代价和最小的待扩展节点不是所述路径优化终点时，则将所述启发路径代价和最小的待扩展节点设置为当前扩展节点，再将当前扩展节点的邻域节点中的未扩展节点设置为新的待扩展节点，直到当前搜索出的启发路径代价和最小的待扩展节点是所述路径优化终点；其中，在所述区域连通图内的任意节点所耗费的启发路径代价和等于从所述路径优化起点到该节点的实际移动代价与从该节点到所述路径优化终点的预测路径代价的和值。与现有技术相比，本技术方案可以有效减少扩展节点的搜索量，提高区域连通图内的路径节点的搜索速度，提高机器人在区域连通图内的通过性，规划出的路径在地图上的覆盖面增大。也在连通图这一替代性的导航地图上发挥启发式搜索算法的路径规划优势。

进一步地，所述利用节点在所属的运动轨迹线段上的居中程度值换算出的对应的代价值，搜索出与预先设置的导航起点相匹配的路径优化起点以及与预先设置的导航终点相匹配的路径优化终点的方法包括：若存在待判断节点与所述导航起点的连线不经过障碍物栅格点，则确定该待判断节点是直通所述导航起点的节点；然后对每个直通所述导航起点的节点在其所属的运动轨迹线段上的居中程度值换算出的对应的代价值进行数值排序；再将代价值最小的直通所述导航起点的节点设置为所述路径优化起点；若存在待判断节点与所述导航终点的连线不经过障碍物栅格点，则确定该待判断节点是直通所述导航终点的节点；然后对每个直通所述导航终点的节点在其所属的运动轨迹线段上的居中程度值换算出的对应的代价值进行数值排序；再将代价值最小的直通所述导航终点的节点设置为所述路径优化终点；其中，所述待判断节点属于预先构建的区域连通图内的节点；每个所述待判断节点都位于各自所属的一条运动轨迹线段上。

该技术方案在区域连通图内，利用每个节点在其所属的运动轨迹线段上的居中程度值（表示向运动轨迹线段的中点靠拢的程度、或表示为偏离运动轨迹线段的端点的程度），搜索出直通机器人的导航点且产生代价值最小的节点，作为与导航起点的距离相匹配的路径优化起点、以及与导航终点的距离相匹配的路径优化终点，开启在区域连通图内规划出以路径优化起点为起点、以路径优化终点为终点的导航路径。

进一步地，还包括：计算数值2与所述待判断节点在所属的运动轨迹线段上的居中程度值的差值，作为偏离程度差值；计算所述待判断节点与所述导航起点的欧式距离，作为导航起点匹配距离；再将偏离程度差值、导航起点匹配距离与居中程度影响系数相乘，得到的乘积是所述直通所述导航起点的节点在所属的运动轨迹线段上的居中程度值换算出的对应的代价值；计算所述待判断节点与所述导航终点的欧式距离，作为导航终点匹配距离；再将偏离程度差值、导航终点匹配距离与居中程度影响系数相乘，得到的乘积是所述直通所述导航终点的节点在所属的运动轨迹线段上的居中程度值换算出的对应的代价值；其中，居中程度值处于0至1之间；居中程度影响系数是预先设置的，用于调节待规划的路径偏离障碍物的远近程度。

本技术方案根据待规划的路径偏离障碍物的远近程度、待判断节点与所述导航起点或所述导航终点的欧式距离、待判断节点在所属的运动轨迹线段上的居中程度值的乘积运算的结果，获取待判断节点对应换算出的代价值，但不同于从所述导航终点或所述导航起点到所述待判断节点的实际移动代价，是用于评估待判断节点与所述导航起点或所述导航终点的接近程度及通行效果。

进一步地，所述从所述路径优化起点到当前扩展节点的实际移动代价等于从所述路径优化起点到当前扩展节点的父节点的实际移动代价与从当前扩展节点的父节点到当前扩展节点的实际移动代价的和值。表示所述从所述路径优化起点到当前扩展节点的实际移动代价是路径代价的累加结果，控制当前扩展节点产生的实际移动代价等于上一次扩展节点累计产生的实际移动代价与当前扩展节点到其父节点产生的实际移动代价的和值。

进一步地，从当前扩展节点的父节点到当前扩展节点的实际移动代价的计算方法包括：计算数值2与当前扩展节点在所属的运动轨迹线段上的居中程度值的差值，作为偏离程度差值；计算所述当前扩展节点与其父节点的欧式距离，作为邻域节点匹配距离；再将偏离程度差值、邻域节点匹配距离与居中程度影响系数相乘，得到的乘积是所述从当前扩展节点的父节点到当前扩展节点的实际移动代价；其中，居中程度值处于0至1之间；居中程度影响系数是预先设置的，用于调节待规划的路径偏离障碍物的远近程度。使得从当前扩展节点的父节点到当前扩展节点的实际移动代价能够表示待规划的路径远离障碍物的程度，提高当前扩展节点的父节点到当前扩展节点之间的可通行路径的搜索成功率。

进一步地，从所述路径优化起点到当前扩展节点的父节点的实际移动代价的计算方法包括：步骤a1、将当前扩展节点的父节点设置为回溯迭代节点，并设置移动代价迭代和为0；然后进入步骤a2；步骤a2、判断回溯迭代节点是否为所述路径优化起点，是则进入步骤a3，否则进入步骤a4；步骤a3、将从所述路径优化起点到当前扩展节点的父节点的实际移动代价设置为等于所述移动代价迭代和；步骤a4、计算数值2与回溯迭代节点在所属的运动轨迹线段上的居中程度值的差值，作为偏离程度差值；计算所述回溯迭代节点与其父节点的欧式距离，作为邻域节点匹配距离；再将偏离程度差值、邻域节点匹配距离与居中程度影响系数相乘，得到的乘积是从所述回溯迭代节点的父节点到所述回溯迭代节点的实际移动代价，再将从所述回溯迭代节点的父节点到所述回溯迭代节点的实际移动代价加上所述移动代价迭代和，然后将相加得到的和值更新为所述移动代价迭代和；然后进入步骤a5；步骤a5、将所述回溯迭代节点的父节点设置为下一次计算父子节点之间的移动代价的回溯迭代节点，再返回步骤a2；其中，居中程度值处于0至1之间；居中程度影响系数是预先设置的，用于表示待规划的路径偏离障碍物的远近程度；其中，更新前后的所述回溯迭代节点都属于所述区域连通图内的节点，所述回溯迭代节点及其父节点都属于所述区域连通图内的相邻接的两个节点。

该技术方案通过重复执行步骤a2至步骤a5，来对回溯迭代节点及其所有相关联的父节点（存在相邻接关系的节点）所产生的实际移动代价进行累加处理，使得从所述路径优化起点到当前扩展节点的父节点的实际移动代价代表：从所述路径优化起点与当前扩展节点的父节点之间所存在的所有相邻接的两个节点所产生的移动代价之和。不仅考虑当前获得的最佳选择节点，也综合考虑从所述路径优化起点到当前扩展节点所有遍历过的节点对应的路径成本信息。

进一步地，前述相应的节点在所属的运动轨迹线段上的居中程度值是：该节点到运动轨迹线段上相距最近的一个端点的直线距离与该运动轨迹线段的长度的一半的比值。用于表示前述相应的节点在所属的运动轨迹线段上偏离中点的程度，使得：相应的节点位于所属的运动轨迹线段的中点时，对应的居中程度值是1；相应的节点位于所属的运动轨迹线段的端点时，对应的居中程度值是0。

进一步地，所述从当前扩展节点到所述路径优化终点的预测路径代价是使用所述当前扩展节点与所述路径优化终点之间的欧式距离、曼哈顿距离、或对角距离来表示。用于表示从指定点到目标终点的估算成本。

进一步地，所述根据所述区域连通图内的节点对应的父节点信息，逐层地搜索出从所述路径优化起点到所述路径优化终点的优化导航路径的方法包括：在所述区域连通图内所有的待扩展节点中，若启发路径代价和最小的待扩展节点是所述路径优化终点时，则从所述路径优化终点开始，按照所述路径优化终点在所述区域连通图的对应邻域内所记录的父节点位置信息，依次连接父节点以及该父节点的父节点，直到在所述区域连通图内连接到所述路径优化起点，实现在所述导航起点和所述导航终点之间，逐层地搜索出从所述路径优化起点到所述路径优化终点的优化导航路径。该技术方案在所有待扩展节点对应产生的启发路径代价和中，选择启发路径代价和数值最小的且是属于所述路径优化终点的节点作为路径扩展起点，通过逐个邻接的节点互连的方式，逆向连接得到从导航起点到导航终点的导航路径。

进一步地，所述在所述区域连通图内所有的待扩展节点中，若启发路径代价和最小的待扩展节点不是所述路径优化终点时，则将所述启发路径代价和最小的待扩展节点设置为当前扩展节点，再将当前扩展节点的邻域节点中的未扩展节点设置为新的待扩展节点，直到当前搜索出的启发路径代价和最小的待扩展节点是所述路径优化终点的方法包括：步骤b1、在所述区域连通图内所有的待扩展节点中，若启发路径代价和最小的待扩展节点不是所述路径优化终点时，则将所述启发路径代价和最小的待扩展节点设置为当前扩展节点，然后在所述区域连通图内搜索出当前扩展节点的未扩展的邻域节点并将其设置为新的待扩展节点，同时将当前设置出的新的待扩展节点的父节点记为当前设置出的当前扩展节点，再将所述当前扩展节点更新为已扩展节点，然后进入步骤b2；步骤b2、在当前设置出的所有待扩展节点中，判断启发路径代价最小的待扩展节点是否为所述路径优化终点，是则执行所述根据所述区域连通图内的节点对应的父节点信息，逐层地搜索出从所述路径优化起点到所述路径优化终点的优化导航路径的方法；否则返回步骤b1。

该技术方案以当前扩展节点作为待扩展节点（备用路径节点）的搜索起点，通过邻域搜索的方式扩大待扩展节点的搜索范围，且在搜索过程中区别出已扩展节点（已遍历且已经被标记为当前扩展节点）和未扩展节点（未访问过的节点），降低重复搜索率，也有利于在更大的范围内快速地搜索出所述路径优化终点，使得所规划的路径具有更好的通行性和更高的区域覆盖性。

进一步地，机器人在执行所述导航路径规划方法的过程中，创建一个优先队列，用于存储待扩展节点；当优先队列内存储的优先级最高的待扩展节点出队时，将该优先级最高的待扩展节点更新为所述当前扩展节点；再在所述区域连通图内搜索出所述当前扩展节点的未扩展的邻域节点后，将当前扩展节点标记为已扩展节点以区别于未扩展节点，并将搜索出的当前扩展节点的未扩展的邻域节点加入所述优先队列并都更新为待扩展节点；其中，未扩展的邻域节点是属于未扩展节点，未扩展节点中不存在在先配置为待扩展节点的节点，使得当前扩展节点的未扩展的邻域节点不预先存在所述优先队列内，当前扩展节点的未扩展的邻域节点不存在所述已扩展节点；其中，所述优先队列开始出队一个待扩展节点以进行一轮节点扩展操作之前，所述优先队列已经存储有所述路径优化起点，以便于引导从所述路径优化起点开始通过邻域扩展至所述路径优化终点；其中，待扩展节点所耗费的启发路径代价和越小时，则将待扩展节点在所述优先队列中的搜索优先级配置得越高。本技术方案创建用于存储待扩展节点的优先队列，实现有序地缓存、读写操作待扩展节点和已扩展节点，有利于加快扩展出导航成本最低的最短路径。

进一步地，所述区域连通图的构建方法，包括：步骤1、在机器人预先标记的相互平行的每条运动轨迹线段中，分别沿着机器人预先标记的相互平行的每条运动轨迹线段，自运动轨迹线段的中点向两端步进地提取出具备可通行性的节点；步骤2、先基于步骤1提取出的具备可通行性的节点构建出树形结构，然后结合指定节点距离导航起点的移动代价，从所述树形结构的节点的可达邻域内搜索出一个区域连通图；其中，导航起点是预先配置到机器人内的导航信息。与现有技术相比，本技术方案在清扫轨迹的中点向两端步进搜索节点，再利用搜索的节点构建树形数据结构并在树形数据结构上进行传统图搜索算法，以搜索出的区域连通图，使得机器人在导航时可以使用区域连通图取代传统的栅格地图，可以极大的减少搜索运算量，同时也能够连接出更多的可行路径，提高区域可达的完备性。

进一步地，所述步骤2的具体方法包括：步骤21、将距离导航起点的移动代价最小的指定节点配置为搜索起点；其中，最先配置为指定节点的节点是属于步骤1提取出的任意一个具备可通行性的节点。然后进入步骤22；步骤22、在步骤1提取出的具备可通行性的节点所构建的树形结构的节点中，筛选出属于当前配置的搜索起点的可达邻域内的未遍历的邻接节点并将当前筛选出的节点都配置为指定节点，然后进入步骤23；步骤23、将当前配置的搜索起点及步骤22筛选出的邻接节点组成所述区域连通图，将当前配置的搜索起点配置为已遍历的节点使其不再是所述指定节点；然后在当前配置出的指定节点中，将距离导航起点的移动代价最小的指定节点配置为下一次筛选新的邻域节点的搜索起点，再返回步骤22；步骤24、重复步骤22至步骤23，直至不存在指定节点被配置为新的搜索起点，并确定构建出一个所述区域连通图。从而通过约束一个可达邻域来进行未遍历的邻接节点的搜索并在不断的迭代处理中更新最短路径距离信息，以确保最后一次得到的是完备性最好、互连互通的节点集合，让这些节点集合组成区域连通图。

进一步地，所述当前配置的搜索起点及步骤22筛选出的邻接节点都被配置为键值，以构造成为任意两个节点间均互连互通的所述区域连通图；其中，所述搜索起点及其在步骤22中对应筛选出的邻接节点支持构建出可通行路线；其中，所述区域连通图是一种图形结构空间，属于一种数据结构。

进一步地，在在所述步骤2中，还包括：分别计算所述步骤1提取出的每个具备可通行性的节点距离所述导航起点的移动代价，作为所述导航起点到对应的指定节点的移动代价；再利用计算出的指定节点对应的移动代价配置该指定节点的优先级，然后将配置有优先级的指定节点存储到优先队列空间内，使得优先队列空间内当前存储的优先级最高的指定节点最先被筛选出来并被放置入图形结构空间内，以组成所述区域连通图；其中，优先队列空间和图形结构空间都是用以描述节点之间的联系的数据结构；其中，所述指定节点是属于所述步骤1提取出的具备可通行性的节点，也属于所述树形结构上的节点。本技术方案使用数据结构去存储指定节点的相关联信息，包括节点的坐标信息、节点连接成的路线信息、图的信息，并可以区分是否属于已遍历节点，避免重复搜索；在前述技术方案的基础上，本技术方案基于述步骤1提取出的可构建出树形结构的每个具备可通行性的节点，结合树中各节点的邻居关系，计算节点之间的移动代价并用于配置指定节点的优先级，再按照优先级排序指定节点以便于从其所处的优先队列内出队，使得优先队列空间内当前存储的优先级最高的指定节点最先被筛选出来并被放置入图形结构空间内。方便节点信息按顺序存取，提高可连通节点的获取效率。

进一步地，优先级的配置方法包括：若指定节点距离所述导航起点的移动代价越大，则该指定节点的优先级越低；若指定节点距离所述导航起点的移动代价越小，则该指定节点的优先级越高；以使得距离导航起点的移动代价最小的指定节点是最先从所述优先队列空间中被筛选出来。

进一步地，当所述优先队列空间内当前存储的优先级最高的指定节点最先被筛选出来并被配置为所述搜索起点时，将当前配置为所述搜索起点的指定节点移除出所述优先队列空间，然后将当前执行的步骤22所筛选出的节点设置为所述指定节点并存入所述优先队列空间，同时将当前配置为所述搜索起点的指定节点存入已遍历节点集合结构内；其中，所述步骤22筛选出的节点是指没有预先存入所述已遍历节点集合结构的、也没有预先存在于所述优先队列空间内的、且是当前配置的搜索起点的可达邻域内的邻接节点。以避免将当前配置的搜索起点的可达邻域内所搜索到的上一次配置的搜索起点重新配置为新的搜索起点，造成节点的重复搜索。

进一步地，所述属于当前配置的搜索起点的可达邻域内的邻接节点同时满足以下条件：当前配置的搜索起点及其邻接节点的直线距离小于或等于机器人的机身直径；当前配置的搜索起点及其邻接节点之间的连线不穿过障碍物栅格点且不穿过未知栅格点。相对于传统的栅格地图构建方式，本技术方案侧重于在连通性更强的二维平面范围内去扩展邻接节点，使得搜索出的节点所能连接成的可通行路线更加完备，对可通行区域的覆盖率越高。

进一步地，所述步骤1的具体方法包括：首先将所述相互平行的每条运动轨迹线段的中点都配置为所述具备可通行性的节点；然后，从每条运动轨迹线段的中点开始，分别沿着对应一条运动轨迹线段的两端，每隔一个所述机身直径的采样步长提取一个节点，并将提取出的节点配置为所述具备可通行性的节点，直至采样至对应一条运动轨迹线段的两个端点为止。从而以所述机身直径为步长完成每条运动轨迹线段的可通行性的检测，提高具备可通行性的节点的提取效率，增加所述树形结构的构建精度。

进一步地，所述步骤1还包括：若按照一个所述机身直径的采样步长在对应一条运动轨迹线段上提取出的一个节点与同一条运动轨迹线段的一端点的距离小于一个所述机身直径，则将该端点也配置为所述具备可通行性的节点。提高节点提取的冗余度，改善所述区域连通图的完备性。

进一步地，所述树形结构是在一个二维数据集合上构建的Kd树，所述Kd树代表的是对二维数据集合构成的二维坐标空间的一种划分；其中，所述二维数据集合是由所述具备可通行性的节点构建而成的，包括节点坐标值和所属的运动轨迹线段的标记信息。该技术方案按照Kd树规定的特定递归顺序将无序化的所述具备可通行性的节点进行有序化排列或对所述具备可通行性的节点所处的二维平面的分割处理，实现在所述具备可通行性的节点内进行邻域搜索。

附图说明

图1是本发明一实施例公开基于连通图和迭代搜索的导航路径规划方法的流程图。

具体实施方式

下面结合附图对本发明的具体实施方式作进一步说明。应当理解，当在本说明书和所附权利要求书中使用时，术语“包括”和“包含”指示所描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其它特征、整体、步骤、操作、元素、组件和/或其集合的存在或添加。

还应当理解，在此本发明说明书中所使用的术语仅仅是出于描述特定实施例的目的而并不意在限制本发明。如在本发明说明书和所附权利要求书中所使用的那样，除非上下文清楚地指明其它情况，否则单数形式的“一”、“一个”及“该”意在包括复数形式。

还应当进一步理解，本发明说明书和所附权利要求书中使用的术语“和/或”是指相关联列出的项中的一个或多个的任何组合以及所有可能组合，并且包括这些组合。

如在本说明书和所附权利要求书中所使用的那样，术语“如果”可以依据上下文被解释为“当...时”或“一旦”或“响应于确定”或“响应于检测到”。类似地，短语“如果确定”或“如果检测到[所描述条件或事件]”可以依据上下文被解释为意指“一旦确定”或“响应于确定”或“一旦检测到[所描述条件或事件]”或“响应于检测到[所描述条件或事件]”。

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

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

为了克服传统的导航路径规划算法(例如Dijkstra、A*、D*)，对栅格地图的精度和面积依赖比较大且搜索成本随栅格地图的精度和面积的增加而呈现指数形态增长的问题，本发明实施例公开一种基于连通图和迭代搜索的导航路径规划方法，如图1所示，包括以下步骤：

步骤S1、在预先构建的区域连通图内，利用节点在所属的运动轨迹线段上的居中程度值来换算出对应的代价值，再利用换算出的对应的代价值搜索出与预先设置的导航起点相匹配的路径优化起点、以及与预先设置的导航终点相匹配的路径优化终点，在本实施例中，所述的匹配是距离上的匹配，包括距离最近、距离或移动成本处于合理范围内，以在区域连通图设定一个合理的路径起点和路径终点，减少搜索空间；然后进入步骤S2；其中，每个节点都位于所属的一条运动轨迹线段上，运动轨迹线段是机器人预先移动产生的，从而让导航路径的规划具备通行意义；步骤S1所述的节点包括但不限于本实施例的搜索算法中所通用的或更新使用的当前扩展节点、未扩展节点和待扩展节点（候选扩展节点）。

步骤S2、在所述区域连通图内所有的待扩展节点中，判断启发路径代价和最小的待扩展节点是否为所述路径优化终点，是则进入步骤S3，否则进入步骤S4。其中，在所述待扩展节点所耗费的启发路径代价和等于从所述路径优化起点到所述待扩展节点的实际移动代价与从所述待扩展节点到所述路径优化终点的预测路径代价的和值。步骤S2选择启发路径代价和最小的待扩展节点（距离成本上最小的候选扩展节点）去判断是否为所述路径优化终点，等同于判断耗费的相应移动代价和最小的节点是否为待规划的导航路径在所述区域连通图内的目标端点，便于搜索出距离处于合理范围内的目标点，本实施例以终点为导向来规划出机器人导航代价最低的路径，降低整体的导航成本。

步骤S3、根据所述区域连通图内的节点对应的父节点信息，逐层地搜索出从所述路径优化起点到所述路径优化终点的优化导航路径，然后进入步骤S5；在本实施例中，对于栅格地图上构建出的区域连通图，所述逐层地搜索对应的实施方式实质上是先从所述路径优化起点开始，不断扩展到达所述路径优化终点的过程，包括依据起点到该节点的代价最少的更新原则完成对父节点的标记及更新，然后从所述路径优化终点开始，将扩展出的相邻接的路径节点依次相连形成所述路径优化起点与所述路径优化终点之间的优化导航路径。

步骤S4、将所述启发路径代价和最小的待扩展节点设置为当前扩展节点，然后在所述区域连通图内搜索出当前扩展节点的未扩展的邻域节点并将其设置为新的待扩展节点，同时将当前设置出的当前扩展节点更新为已扩展节点，然后返回步骤S2；在步骤S4中，本实施例将所述启发路径代价和最小的待扩展节点设置为当前扩展节点，则此时新设置出的当前扩展节点已不再是在先设置的待扩展节点，优选地，所述启发路径代价和最小的待扩展节点从原先的预设存储空间内移除并转变为当前扩展节点；然后将当前扩展节点的邻域节点中的未扩展节点设置为新的待扩展节点，其中，当前扩展节点的邻域节点中的未扩展节点既不属于当前扩展节点这一类型的节点，也不属于待扩展节点这一类型的节点，优选地，将新设置出的待扩展节点加入原先的预设存储空间内并转变为待扩展节点。重复执行步骤S2至步骤S4，直至当前搜索出的启发路径代价和最小的待扩展节点是所述路径优化终点。本实施例中，在当前扩展节点的邻域节点中搜索过程是以当前扩展节点为搜索中心进行任意方向的邻域搜索，包括但不限于八邻域搜索。有利于搜寻出导航成本最低的最短路径。

在步骤S4中，在所述区域连通图内搜索出当前扩展节点的未扩展的邻域节点，允许机器人朝任何方向搜索，扩展步长或搜索步长可以使用欧式距离表示，每执行一次步骤S4则为一次扩展/搜索，使得机器人可以从当前父节点出发，经过预设间隔时间的行驶，所能到达的未扩展的邻域节点为子节点；简而言之，每一次扩展/搜索表示机器人“走一步”，对应在地图中跨越一次栅格；需要说明的是，预设间隔时间是每次扩展的周期性时间，表示较小的时间单位，例如可以是5秒、10秒等，预设间隔时间越短，所规划的导航路径越精细，因此可以根据实际需求确定预设间隔时间。

步骤S5、将当前搜索出的优化导航路径的两端分别连接上所述导航起点和所述导航终点，形成完整的导航路径，确定完成导航路径的规划。与现有技术相比，前述步骤所述的实施例可以有效减少扩展节点的搜索量，提高区域连通图内的路径节点的搜索速度，提高机器人在区域连通图内的通过性，规划出的路径在地图上的覆盖面增大。也在连通图这一替代性的导航地图上发挥启发式搜索算法（包括A*算法）的路径规划优势。

作为一种实施例，所述利用节点在所属的运动轨迹线段上的居中程度值换算出的对应的代价值，搜索出与预先设置的导航起点相匹配的路径优化起点以及与预先设置的导航终点相匹配的路径优化终点的方法包括：

若存在待判断节点与所述导航起点的连线不经过障碍物栅格点，即在所述区域连通图内搜索到一个节点与所述导航起点的连线不经过障碍物，则确定该待判断节点是直通所述导航起点的节点，其中，待判断节点是属于所述区域连通图内任意一节点；然后对每个直通所述导航起点的节点在其所属的运动轨迹线段上的居中程度值换算出的对应的代价值进行数值排序，一般是将所有直通所述导航起点的节点换算出对应 的代价值由小到大排序；再将对应的代价值最小的直通所述导航起点的节点设置为所述路径优化起点，确定已经搜索出与所述导航起点的距离上的匹配的直通节点，理解为距离最为接近的可通行的节点，在规划导航路径的过程中起到代表导航起点的作用。

同理地，若存在待判断节点与所述导航终点的连线不经过障碍物栅格点，即在所述区域连通图内搜索到一个节点与所述导航终点的连线不经过障碍物，则确定该待判断节点是直通所述导航终点的节点，其中，待判断节点是属于所述区域连通图内任意一个用于参与判断所述路径优化终点的节点；然后对每个直通所述导航终点的节点在其所属的运动轨迹线段上的居中程度值换算出的对应的代价值进行数值排序，一般是将所有直通所述导航终点的节点换算出的对应代价值由小到大排序；再将对应的代价值最小的直通所述导航终点的节点设置为所述路径优化终点，确定已经搜索出与所述导航终点的距离上的匹配的直通节点，理解为距离终点最为接近的可通行的节点，在规划导航路径的过程中起到代表导航终点的作用。

需要说明的是，所述待判断节点属于预先构建的区域连通图内的节点；每个所述待判断节点都位于各自所属的一条运动轨迹线段上 ，存在实际的通行意义。

在本实施例中，在区域连通图内，利用每个节点在其所属的运动轨迹线段上的居中程度值（表示向运动轨迹线段的中点靠拢的程度、或表示为偏离运动轨迹线段的端点的程度），搜索出直通机器人的导航起点且产生代价值最小的节点，作为与导航起点的距离相匹配的路径优化起点、以及与导航终点的距离相匹配的路径优化终点，开始在区域连通图内规划出以路径优化起点为起点、以路径优化终点为终点的导航路径。

在上述实施例的基础上，前述相应节点在所属的运动轨迹线段上的居中程度值换算出的对应的代价值的方法包括：

计算数值2与所述待判断节点在所属的运动轨迹线段上的居中程度值的差值，作为偏离程度差值；其中，居中程度值处于0至1之间；具体地，若居中程度值越接近1，则所述待判断节点越接近所属的运动轨迹线段的中点；若居中程度值越接近0，则所述待判断节点越接近所属的运动轨迹线段的一端点；前述待判断节点在所属的运动轨迹线段上的居中程度值是所述待判断节点到运动轨迹线段上相距最近的一个端点的直线距离与该运动轨迹线段的长度的一半的比值，使得所述待判断节点位于所属的运动轨迹线段的中点时，对应的居中程度值是1；所述待判断节点位于所属的运动轨迹线段的端点时，对应的居中程度值是0。

然后计算所述待判断节点与所述导航起点的欧式距离，作为导航起点匹配距离；再将偏离程度差值、导航起点匹配距离与居中程度影响系数相乘，得到的乘积是所述直通所述导航起点的节点在所属的运动轨迹线段上的居中程度值换算出的对应的代价值；其中，居中程度影响系数是预先设置的，用于调节待规划的路径偏离障碍物的远近程度；本实施例将居中程度影响系数设置为大于或等于1，当居中程度影响系数设置得越大，则提高所述居中程度值在前述得到的乘积中的影响权重，增强居中程度值对待规划的路径影响程度，使得待规划的路径远离障碍物，保证待规划的路径的通行性。进一步地，居中程度值越接近1，则换算出的对应的代价值越小，则表示所述待判断节点越靠近所述导航起点。

同理地，计算所述待判断节点与所述导航终点的欧式距离，作为导航终点匹配距离；再将偏离程度差值、导航终点匹配距离与居中程度影响系数相乘，得到的乘积是所述直通所述导航终点的节点在所属的运动轨迹线段上的居中程度值换算出的对应的代价值；其中，居中程度影响系数是预先设置的，用于调节待规划的路径偏离障碍物的远近程度；本实施例将居中程度影响系数设置为大于或等于1，当居中程度影响系数设置得越大，则提高所述居中程度值在前述得到的乘积中的权重，增强居中程度值对待规划的路径影响程度，使得待规划的路径越远离障碍物，保证待规划路径无障碍通向终点的效果。进一步地，居中程度值越接近1，则换算出的对应的代价值越小，表示所述待判断节点越靠近所述导航终点。

因此，本实施例根据待规划的路径偏离障碍物的远近程度、待判断节点与所述导航起点或所述导航终点的欧式距离、待判断节点在所属的运动轨迹线段上的居中程度值的乘积运算的结果，获取待判断节点对应换算出的代价值，但不同于从所述导航终点或所述导航起点到所述待判断节点的实际移动代价，是用于评估待判断节点与所述导航起点或所述导航终点的接近程度及通行效果。

作为一种实施例，所述从所述路径优化起点到当前扩展节点的实际移动代价等于从所述路径优化起点到当前扩展节点的父节点的实际移动代价与从当前扩展节点的父节点到当前扩展节点的实际移动代价的和值。本实施例公开的当前扩展节点对应的实际移动代价表示所述从所述路径优化起点到当前扩展节点的实际移动代价是相对应的父节点的路径代价的累加结果，控制当前扩展节点产生的实际移动代价等于上一次扩展节点累计产生的实际移动代价与当前扩展节点到其父节点产生的实际移动代价的和值。

具体地，从当前扩展节点的父节点到当前扩展节点的实际移动代价的计算方法包括：计算数值2与当前扩展节点在所属的运动轨迹线段上的居中程度值的差值，作为偏离程度差值；当前扩展节点在所属的运动轨迹线段上的居中程度值是所述当前扩展节点到运动轨迹线段上相距最近的一个端点的直线距离与该运动轨迹线段的长度的一半的比值，以使得：居中程度值越接近1，则所述当前扩展节点越接近所属的运动轨迹线段的中点，表示所述当前扩展节点趋于远离障碍物；居中程度值越接近0，则所述当前扩展节点越接近所属的运动轨迹线段的一端点，表示所述当前扩展节点趋于接近障碍物。然后，计算所述当前扩展节点与其父节点的欧式距离，作为邻域节点匹配距离，其中，当前扩展节点及其在先记录好的父节点是属于所述区域连通图内的相邻接的两个节点；再将偏离程度差值、邻域节点匹配距离与居中程度影响系数相乘，得到的乘积是所述从当前扩展节点的父节点到当前扩展节点的实际移动代价；其中，居中程度值处于0至1之间；居中程度影响系数是预先设置的，用于表示待规划的路径偏离障碍物的远近程度。居中程度影响系数大于或等于1，当居中程度影响系数设置得越大，则前述得到的乘积也越大，增强居中程度值对待规划的路径影响程度，使得待规划的路径越远离障碍物，保证待规划的路径的通行性。进一步地，居中程度值越接近1，则从当前扩展节点的父节点到当前扩展节点的实际移动代价越小，则表示当前扩展节点的父节点到当前扩展节点的移动成本越低。使得从当前扩展节点的父节点到当前扩展节点的实际移动代价能够表示待规划的路径远离障碍物的程度，提高当前扩展节点的父节点到当前扩展节点之间的可通行路径的搜索成功率。

具体地，从所述路径优化起点到当前扩展节点的父节点的实际移动代价的计算方法包括：

步骤a1、将当前扩展节点的父节点设置为回溯迭代节点，并设置移动代价迭代和为0，作为累加的代价和值；然后进入步骤a2。

步骤a2、判断回溯迭代节点是否为所述路径优化起点，是则进入步骤a3，否则进入步骤a4。

步骤a3、将从所述路径优化起点到当前扩展节点的父节点的实际移动代价设置为等于所述移动代价迭代和。具体地，若是第一次执行步骤a2，则确定从所述路径优化起点到当前扩展节点的父节点的实际移动代价等于0；若不是第一次执行步骤a2，则将从所述路径优化起点到当前扩展节点的父节点的实际移动代价设置为等于所述移动代价迭代和。

步骤a4、计算数值2与所述回溯迭代节点在所属的运动轨迹线段上的居中程度值的差值，作为偏离程度差值；回溯迭代节点在所属的运动轨迹线段上的居中程度值是所述回溯迭代节点到运动轨迹线段上相距最近的一个端点的直线距离与该运动轨迹线段的长度的一半的比值，以使得：居中程度值越接近1，则所述回溯迭代节点越接近所属的运动轨迹线段的中点；居中程度值越接近0，则所述回溯迭代节点越接近所属的运动轨迹线段的一端点。需要强调的是，所述回溯迭代节点属于所述区域连通图内的节点，并且是来源于机器人预先移动产生的所述运动轨迹线段。然后，计算所述回溯迭代节点与其父节点的欧式距离，作为邻域节点匹配距离；再将偏离程度差值、邻域节点匹配距离与居中程度影响系数相乘，得到的乘积是从所述回溯迭代节点的父节点到所述回溯迭代节点的实际移动代价，是等于一个所述回溯迭代节点对应换算出的代价值。然后，将从所述回溯迭代节点的父节点到所述回溯迭代节点的实际移动代价加上所述移动代价迭代和，再将相加得到的和值更新为所述移动代价迭代和；然后进入步骤a5。

步骤a5、将所述回溯迭代节点的父节点设置为下一次计算父子节点之间的移动代价的回溯迭代节点，再返回步骤a2；其中，更新前后的所述回溯迭代节点都属于所述区域连通图内的节点，所述回溯迭代节点及其父节点都属于所述区域连通图内的相邻接的两个节点。

本实施例通过重复执行步骤a2至步骤a5，来对回溯迭代节点及其所有相关联的父节点（存在相邻接关系的节点）所产生的实际移动代价进行累加处理，使得从所述路径优化起点到当前扩展节点的父节点的实际移动代价代表为从所述路径优化起点与当前扩展节点的父节点之间所存在的所有相邻接的两个节点所产生的移动代价之和。使得本实施例不仅考虑当前获得的最佳选择节点，也综合考虑所述区域连通图内从所述路径优化起点到当前扩展节点所有遍历过的节点对应的路径成本信息。

在前述实施例中，所述从当前扩展节点到所述路径优化终点的预测路径代价是使用所述当前扩展节点与所述路径优化终点之间的欧式距离、曼哈顿距离、或对角距离来表示。如果机器人当前所处的连通图中只允许朝上下左右四个方向移动，则可以使用曼哈顿距离；如果机器人当前所处的连通图中允许朝八个方向移动，则可以使用对角距离；如果机器人当前所处的连通图中允许朝任何方向移动，则可以使用欧式距离。有利于搜寻出导航成本最低的最短导航路径。需要说明的是，对于每个节点，可以计算其代价值，代价值是对机器人的运动轨迹成本的度量，表示由起点运动到该节点，再运动到终点的成本，包括路径长度、所需时间、是否发生碰撞、是否频繁切换速度方向等因素。

作为一种实施例，所述根据所述区域连通图内的节点对应的父节点信息，逐层地搜索出从所述路径优化起点到所述路径优化终点的优化导航路径的方法包括：在所述区域连通图内所有的待扩展节点中，若启发路径代价和最小的待扩展节点是所述路径优化终点时，则从所述路径优化终点开始，按照所述路径优化终点在所述区域连通图的对应邻域内所记录的父节点位置信息，依次连接父节点以及该父节点的父节点，直到在所述区域连通图内连接到所述路径优化终点，实现在所述导航起点和所述导航终点之间，逐层地搜索出从所述路径优化起点到所述路径优化终点的优化导航路径；其中，一个待扩展节点所耗费的启发路径代价和在所述区域连通图内当前配置出的所有待扩展节点中是最小时，则将该待扩展节点配置为最先执行扩展/搜索的节点。

所述按照所述路径优化终点在所述区域连通图的对应邻域内所记录的父节点位置信息，依次连接父节点以及该父节点的父节点的方式具体为：从当前配置出的待扩展节点内，当启发路径代价和最小的待扩展节点是所述路径优化终点时，将启发路径代价和最小的待扩展节点作为子节点，然后按照预先记录的父节点位置信息回溯其父节点并依次连接起来；再将回溯出的父节点作为下一个子节点，继续按照预先记录的父节点位置信息回溯其父节点，并依次连接起来，如此重复，不断扩展到达所述路径优化起点，其中，所述路径优化起点也属于所述待扩展节点；从而本实施例在所有待扩展节点对应产生的启发路径代价和中，选择启发路径代价和数值最小的且是属于所述路径优化终点的节点作为路径扩展起点，通过逐个邻接的节点互连的方式，逆向连接得到从导航起点到导航终点的导航路径。

作为另一种实施例，所述在所述区域连通图内所有的待扩展节点中，若启发路径代价和最小的待扩展节点不是所述路径优化终点时，则将所述启发路径代价和最小的待扩展节点设置为当前扩展节点，再将当前扩展节点的邻域节点中的未扩展节点设置为新的待扩展节点，直到当前搜索出的启发路径代价和最小的待扩展节点是所述路径优化终点的方法包括：

步骤b1、在所述区域连通图内所有的待扩展节点中，具体是当前配置出的待扩展节点内，通过比较每个待扩展节点所耗费的启发路径代价和，获取的启发路径代价和最小的待扩展节点不是所述路径优化终点时，将所述启发路径代价和最小的待扩展节点设置为当前扩展节点，然后在所述区域连通图内，以当前扩展节点为搜索中心，搜索出其邻域内的未扩展的邻域节点并将其设置为新的待扩展节点，同时将步骤b1设置出的当前扩展节点作为当前扩展出的父节点，作为当前搜索出未扩展的邻域节点的父节点；再将所述当前扩展节点更新为已扩展节点，确定将所述当前扩展节点设置为不可重复扩展的节点、或不可重复搜索的节点，已排除作为所述待扩展节点这一类节点和当前扩展节点这一类节点的可能性；然后进入步骤b2。

步骤b2、在当前标记出的所有待扩展节点中，判断启发路径代价最小的待扩展节点是否为所述路径优化终点，是则按照上述实施例公开的从所述路径优化起点到所述路径优化终点的优化导航路径的方法，从所述路径优化终点开始，逐层地搜索出从所述路径优化起点到所述路径优化终点的优化导航路径；否则返回步骤b1，在最新配置出的待扩展节点内，继续比较每个待扩展节点所耗费的启发路径代价和，获取的启发路径代价和最小的待扩展节点不是所述路径优化终点时，将所述启发路径代价和最小的待扩展节点设置为当前扩展节点，然后在所述区域连通图内，以当前扩展节点为下一个搜索中心，继续搜索出其邻域内的未扩展的邻域节点并将其设置为下一批的待扩展节点，并将所述下一个搜索中心更新为下一个父节点；如此迭代，直到在所述区域连通图内搜索出的当前扩展节点的未扩展的邻域节点是是所述路径优化终点。其中，前述的当前扩展节点的未扩展的邻域节点中不存在待扩展节点和已扩展节点。本实施例以当前扩展节点作为待扩展节点（备用路径节点）的搜索起点，通过邻域搜索的方式扩大待扩展节点的搜索范围，且在搜索过程中区别出已扩展节点（已遍历且已经被标记为当前扩展节点）和未扩展节点（未访问过的节点），降低重复搜索率，有利于向更大的范围扩展出所述路径优化终点，使得所规划的路径具有更好的通行性和更高的区域覆盖性。

在前述实施例的基础上，机器人在执行所述导航路径规划方法的过程中，创建一个优先队列，用于存储前述的待扩展节点；当优先队列内存储的优先级最高的待扩展节点出队时，将该优先级最高的待扩展节点更新为所述当前扩展节点，其中，待扩展节点所耗费的启发路径代价和越小时，则将待扩展节点的搜索优先级配置得越高，则搜索优先级最高的待扩展节点是所耗费的启发路径代价和最小的待扩展节点。本领域技术人员可理解到优先队列内当前存储的优先级最高的待扩展节点及其相关联的节点信息最先输出。优选地，所述优先队列内还记录待扩展节点的父节点的位置、姿态、速度等其他信息。

然后在所述区域连通图内搜索出所述当前扩展节点的未扩展的邻域节点后，将当前扩展节点标记为已扩展节点以区别于未扩展节点，本实施例还为此专门创建一个已遍历节点缓存空间，用于存储已扩展节点；此时，所述已扩展节点已经移除出所述优先队列，再移入所述已遍历节点缓存空间；需要说明的是，存在于所述已遍历节点缓存空间内节点不允许加入所述优先队列，以实现在规划过程中，标识出的已扩展节点或视为已搜索出的节点，防止出现同一搜索中心进行重复扩展的现象；优选地，所述已遍历节点缓存空间可以以列表这一数据存储结构的形式存在于机器人的内部。同时将搜索出的当前扩展节点的未扩展的邻域节点加入所述优先队列并都更新为待扩展节点，作为后续用于路径规划的路径节点的集合，也都是来源于组成所述区域连通图的节点；同时记录新加入所述优先队列的邻域节点的父节点的位置信息，优选地将这些邻域节点的位置信息连同对应的父节点的位置信息一起存入所述优先队列中，且这些父节点的位置信息也都是来源于组成所述区域连通图，且是来源于机器人预先移动产生的所述运动轨迹线段；以便于搜索到所述路径优化终点时直接通过其父节点回溯的方式，逐级寻找到起点到终点之间的路径，加快路径规划的速度。本实施例创建用于存储待扩展节点的优先队列，实现有序地缓存、读写操作待扩展节点和已扩展节点，有利于加快扩展出导航成本最低的最短路径。

需要说明的是，未扩展的邻域节点是属于未扩展节点，未扩展节点中不存在在先配置为待扩展节点的节点，使得当前扩展节点的未扩展的邻域节点不预先存在所述优先队列内，当前扩展节点的未扩展的邻域节点不存在所述已扩展节点；其中，所述优先队列开始出队一个待扩展节点以进行一轮节点扩展操作之前，所述优先队列已经存储有所述路径优化起点，以便于引导从所述路径优化起点开始通过邻域扩展至所述路径优化终点。其中，所述优先队列内部的待扩展节点都配置有搜索优先级，以形成有序的出队顺序，其中，所述优先队列内部的待扩展节点分别存在所述运动轨迹线段的标识信息，因为它们都是来源于机器人预先移动产生的运动轨迹线段上的。

作为一种实施例，公开前述实施例所述的区域连通图的构建方法，具体包括：步骤1、分别沿着机器人预先标记的相互平行的每条运动轨迹线段，自每条运动轨迹线段的中点向两端步进地提取出具备可通行性的节点，即以特定步长为一个采样间隔（理解为步进地），提取出具备可通行性的节点；在机器人的已工作覆盖的区域提取出所有运动轨迹线段上的具备可通行性的节点后，进入执行步骤2；步骤2、基于步骤1提取出的具备可通行性的节点构建出树形结构，再结合指定节点距离导航起点的移动代价，从所述树形结构的节点的可达邻域内搜索出一个区域连通图；其中，导航起点是预先配置到机器人内的导航信息，指定节点具有遍历的优先级，作为候选的路径节点，以使得所述树形结构的节点的可达邻域内被筛选出的节点组成一个完备性更强的区域连通图，与现有技术的随机路线图算法和图搜索算法相比，步骤2搜索出的区域连通图所覆盖的区域的连通性更好，支持构建出更多的可行路径，相对扩大机器人移动覆盖的范围。与现有技术相比，本实施例在清扫轨迹的中点向两端步进搜索节点，再利用搜索的节点构建树形数据结构并在树形数据结构上进行传统图搜索算法，以搜索出的区域连通图，使得机器人在导航时可以使用区域连通图取代传统的栅格地图，可以极大的减少搜索运算量，同时也能够连接出更多的可行路径，提高区域可达的完备性。

作为一种实施例，所述步骤2的具体方法包括：

步骤21、将距离导航起点的移动代价最小的指定节点配置为搜索起点；然后进入步骤22；其中，导航起点是预先配置到机器人内的导航信息，是属于传统的图搜索算法（包括但不限于A*算法、D*算法、Dijkstra算法）为了迭代计算移动代价（最短路径）而预先设置的路径起点；搜索起点则是当前一次迭代计算中设定的当前遍历的节点，支持在下一次迭代计算中被更新。其中，最先配置为指定节点的节点是属于步骤1提取出的任意一个具备可通行性的节点。

步骤22、在步骤1提取出的具备可通行性的节点所构建的树形结构的节点中，筛选出属于当前配置的搜索起点的可达邻域内的未遍历的邻接节点并将当前筛选出的节点都配置为指定节点，相当于从步骤1构建的树形数据结构中搜索出所述搜索起点的可达邻域内的未被搜索过的邻接节点，包括未被配置为指定节点的相邻节点，再将未曾被配置为指定节点的相邻节点都配置为新的指定节点；然后进入步骤23；因此，本实施例中，步骤22筛选出的未遍历的邻域节点中不存在预先配置的指定节点，已经配置为指定节点的节点不属于步骤22筛选出的未遍历的邻域节点。需要说明的是，邻接节点是相对于所述搜索起点的可达邻域内而言的，是属于一个节点的邻域内（比如八邻域、四邻域等邻域范围）的邻接节点，相当于一个父节点的子节点。

需要说明的是，在步骤22中，所述当前配置的搜索起点是父节点时，所述当前配置的搜索起点的可达邻域内的未遍历的邻域节点都属于该父节点的子节点。

步骤23、将当前配置的搜索起点及步骤22筛选出的邻接节点都标记成为路线节点集合内的节点元素，则这些节点元素被存储到一个图形结构，组成所述区域连通图，即路线节点集合以图结构的形式组成所述区域连通图，属于一种数据结构。同时，将当前配置的搜索起点标记为已遍历的节点，使其不再是所述指定节点，相当于已遍历的指定节点使其后续不被重新配置为搜索起点；同时步骤22筛选出的邻接节点也不能被重复搜索出来用于组成所述区域连通图，也不能被重复搜索出来并用于配置为下一次筛选新的邻接节点的搜索起点，其中，当前配置的搜索起点的可达邻域内搜索出的邻接节点中可以存在上一次配置的搜索起点。然后将距离导航起点的移动代价最小的未遍历的指定节点配置为下一次筛选新的邻接节点的搜索起点，再返回步骤22。从而，当前执行的步骤23在排除上一次配置的搜索起点的干扰的基础上，包括排除上一次步骤22筛选出的未遍历的邻接节点，在最新配置出的所有指定节点中，选择距离所述导航起点的移动代价最小的指定节点配置为下一次筛选新的邻接节点的搜索起点，实现更新掉步骤21当前配置的搜索起点，获取下一次构建可通行路线的搜索起点，再返回步骤22。

步骤24、重复步骤22至步骤23，直至不存在指定节点被配置为新的搜索起点，即遍历完所有数值的移动代价对应的具备可通行性的节点（即指定节点），相应配置的搜索路径（为完成前述迭代处理而抽象出的路径，并非所述区域连通图内的节点所能连成的可通行路线）变为空，停止搜索，并确定搜索出的搜索起点及其可达邻域内的邻接节点组成一个所述区域连通图，并确定构建出一个所述区域连通图。

前述步骤21至步骤24所述的实施例从所述导航起点开始，采用贪心算法的策略，每次遍历到导航起点距离最近且未遍历过的所述搜索起点（顶点）的特定邻域内的邻接节点，直到没有合适的节点作为新的搜索起点为止。从而通过约束一个可达邻域来进行未遍历的邻接节点的搜索并在不断的迭代处理中更新最短路径距离信息，以确保最后一次得到完备性最好、互连互通的路线图（等同于区域连通图）。

需要补充的是，Dijkstra算法是基于贪心算法思想的。所谓贪心算法即始终保持当前迭代解为当前最优解。意思就是在已知的条件下或是当前拥有的全部条件下保证最优解，若在此后的迭代中由于加入了新的条件使得产生了更优解则替代此前的最优解。通过不断的迭代不断保证每次迭代的结果都是当前最优解，那么当迭代到最后一轮时得到的就会是全局最优解。

在前述实施例中，所述当前配置的搜索起点及步骤22筛选出的邻接节点都被配置为键值，以构造成为任意两个节点间均互连互通的所述区域连通图；所述搜索起点及其在步骤22中对应筛选出的邻接节点支持构建出可通行路线，实现构建出从所述搜索起点到其在步骤22中对应筛选出的邻接节点的可通行路线，这对于任意一次配置及搜索可达邻域内的邻域节点的迭代操作都满足构建出可通行路线的技术效果。需要说明的是，所述图形结构是一种元素之间的关系是任意的数据结构，在所述图形结构中，每个节点的前驱节点数和后续节点数可以任意多个，节点元素间的关系是任意的。其他数据结构(如树、线性表等)都有明确的条件限制，图形结构中任意两个节点元素间均可是相连通的。有利于实现在导航起点和导航终点之间构建出任意两点之间都是联通可达的区域连通图。

作为一种实施例，在所述步骤2中，还包括：

分别计算所述步骤1提取出的每个具备可通行性的节点距离所述导航起点的移动代价，作为所述导航起点到对应的指定节点的移动代价，即获取每个指定节点距离同一导航起点的移动代价，所述移动代价包括但不限于导航起点到指定节点的欧式距离、曼哈顿距离。再利用计算出的指定节点对应的移动代价配置该指定节点的优先级，方便节点信息按顺序存取，提高可连通节点的获取效率，具体地，优先级的配置方法包括：若指定节点距离所述导航起点的移动代价越大，则将该指定节点的优先级配置得越低；若指定节点距离所述导航起点的移动代价越小，则将该指定节点的优先级配置得越高。以使得距离导航起点的移动代价最小的指定节点是最先从所述优先队列空间中输出。

然后将配置有优先级的指定节点存储到优先队列空间内，需要说明的是，在开始执行所述步骤2时，这个配置有优先级的指定节点是所述步骤1提取出的具备可通行性的节点所构建的树形结构上的任意节点。其中，所述优先队列空间内当前存储的优先级最高的指定节点最先被筛选出来（由于是优先队列，所以可理解为最先输出队列）并被放置入图形结构空间内，以构建所述区域连通图；在优先队列空间中元素只能是指定节点且都被赋予优先级，最高优先级元素优先被移除优先队列空间，以使得距离导航起点的移动代价最小的指定节点是最先从所述优先队列空间中被筛选出来（由于是优先队列，所以可理解为最先输出队列）。具体地，在重复执行步骤22至步骤23的过程中，若检测到所述优先队列空间为空，则表示能够被配置为新的搜索起点的指定节点已经全部从所述优先队列空间移除至所述图形结构空间内，确定构建出一个所述区域连通图。

当所述优先队列空间内当前存储的优先级最高的指定节点最先被筛选出来（由于是优先队列，所以可理解为最先输出队列）并被配置为所述搜索起点时，将当前配置为所述搜索起点的指定节点移除出所述优先队列空间，再将当前配置为所述搜索起点的指定节点存入已遍历节点集合结构内，以将当前配置为所述搜索起点的指定节点进一步地标识为不可重复搜索的节点，即已遍历节点。然后将当前执行的步骤22所筛选出的节点配置为所述指定节点，并存入所述优先队列空间。在本实施例中，所述步骤22筛选出的节点是指没有预先存入所述已遍历节点集合结构的、也没有预先存在于所述优先队列空间内的、且是当前配置的搜索起点的可达邻域内的邻接节点，使得筛选出的邻接节点中已配置为所述搜索起点的节点排除在外，即排除掉已存入已遍历节点集合结构内的节点，以避免将当前配置的搜索起点的可达邻域内所搜索到的上一次配置的搜索起点重新配置为新的搜索起点，造成节点的重复搜索。

本领域技术人员易知：已遍历节点集合结构、优先队列空间和图形结构空间都是用以描述节点之间的联系的数据结构，都能存储需要回溯的节点。其中，优先队列空间是用以描述优先级高低不同的节点之间的进出先后关系的数据结构。

本实施例使用数据结构去存储指定节点的相关联信息，包括节点的坐标信息、节点连接成的路线信息、图的信息，并可以区分是否属于已遍历节点，避免重复搜索；在前述实施例的基础上，本实施例基于述步骤1提取出的可构建出树形结构的每个具备可通行性的节点，结合树中各节点的邻居关系，计算节点之间的移动代价并用于配置指定节点的优先级，再按照优先级排序指定节点以便于从其所处的优先队列内出队，使得优先队列空间内当前存储的优先级最高的指定节点最先被筛选出来（由于是优先队列，所以可理解为最先输出队列）并被放置入图形结构空间内。方便节点信息按顺序存取，提高可连通节点的获取效率。

在前述实施例中，所述属于当前配置的搜索起点的可达邻域内的邻接节点同时满足以下条件：

（1）当前配置的搜索起点及其邻接节点的直线距离小于或等于机器人的机身直径，表示当前配置的搜索起点及当前筛选的邻接节点之间的距离尺寸在一个机身直径范围内，使得位于当前配置的搜索起点处的机器人能够覆盖到可通行的邻接节点或检测到对应的邻接节点，也提高区域连通图内的满足前述通行条件的节点的分布密度。

（2）当前配置的搜索起点及其邻接节点之间的连线不穿过障碍物栅格点且不穿过未知栅格点，使得机器人无障碍地从当前配置的搜索起点移动至当前筛选的邻接节点，否则机器人无法沿着当前配置的搜索起点与其邻接节点之间的路线移动。

其中，机器人预先标记的相互平行且相邻的两条运动轨迹线段之间的垂直距离小于或等于机器人的机身直径，使得机器人沿着运动轨迹线段做弓字形清扫的过程中可以重复清扫同一块区域。

作为一种实施例，所述步骤1的具体方法包括：

首先将所述相互平行的每条运动轨迹线段的中点都配置为所述具备可通行性的节点。然后，从每条运动轨迹线段的中点开始，分别沿着对应一条运动轨迹线段的两端，即朝着自对应一条运动轨迹线段的中点向同一条运动轨迹线段的两端的方向上，每隔一个所述机身直径的采样步长提取一个节点，并将提取出的节点配置为所述具备可通行性的节点，实现自对应一条运动轨迹线段的中点向其两侧步进地或按照特定距离间隔地提取出具备可通行性的节点，直至采样至对应一条运动轨迹线段的两个端点为止。从而以所述机身直径为步长完成步进式地检测每条运动轨迹线段的可通行性，提高具备可通行性的节点的提取效率，增加所述树形结构的构建精度。其中，所述采样步长是属于步长为一个机身直径的采样距离。

优选地，机器人移动过程中标记出的所述相互平行的每条运动轨迹线段都存储到预先设置的弓字型轨迹线集合内，然后，弓字型轨迹线集合内存储有任意一条运动轨迹线段都标记有id序号、任意一条运动轨迹线段的两个端点坐标，以建立起节点数据结构。

针对步骤1，为了减小步骤21至步骤24所述的迭代算法的搜索空间，本实施例从两点连线的中点开始向两端作步进式搜索且保持每次搜索的距离是一个所述机身直径，既可以压缩搜索量，又能保证节点之间的通行意义，由于是在覆盖区域不同的每一条运动轨迹线段都执行类似的搜索操作，所以不会降低所述区域连通图所能构建的路线的多样性，不会导致算法搜索不到可行路径。

优选地，所述步骤1还包括：若按照一个所述机身直径的采样步长在对应一条运动轨迹线段上提取出的一个节点与同一条运动轨迹线段的一端点的距离小于一个所述机身直径，则将该端点也配置为所述具备可通行性的节点。提高节点提取的冗余度，改善所述区域连通图的完备性。

优选地，所述树形结构是在一个二维数据集合上构建的Kd树，所述Kd树代表的是对二维数据集合构成的二维坐标空间的一种划分，具体是对二维地图平面的分割处理；其中，所述二维数据集合是由所述具备可通行性的节点构建而成的，包括节点坐标值和所属的运动轨迹线段的标记信息。至于所述具备可通行性的节点如何构建出所述Kd树是路径规划领域的技术人员所能掌握的，在此不再详细展开说明。总之，本实施例按照Kd树规定的特定递归顺序将无序化的所述具备可通行性的节点进行有序化排列或对所述具备可通行性的节点所处的二维平面的分割处理，实现在所述具备可通行性的节点内进行邻域搜索，包括以任一个所述具备可通行性的节点为搜索中心进行八邻域搜索。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。 而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

以上所述，仅是本发明的较佳实施例而己，并非是对本发明作其它形式的限制，任何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等效实施例。但是凡是未脱离本发明技术方案内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与改型，仍属于本发明技术方案的保护范围。

Claims (21)

1.基于连通图和迭代搜索的导航路径规划方法，其特征在于，包括：

在预先构建的区域连通图内，利用节点在所属的运动轨迹线段上的居中程度值换算出的对应的代价值，搜索出与预先设置的导航起点相匹配的路径优化起点以及与预先设置的导航终点相匹配的路径优化终点；其中，每个节点都位于所属的一条运动轨迹线段上，运动轨迹线段是机器人预先移动产生的；

在所述区域连通图内所有的待扩展节点中，若启发路径代价和最小的待扩展节点是所述路径优化终点时，根据所述区域连通图内的节点对应的父节点信息，逐层地搜索出从所述路径优化起点到所述路径优化终点的优化导航路径；再将当前搜索出的优化导航路径的两端分别连接上所述导航起点和所述导航终点，完成导航路径的规划；

在所述区域连通图内所有的待扩展节点中，若启发路径代价和最小的待扩展节点不是所述路径优化终点时，则将所述启发路径代价和最小的待扩展节点设置为当前扩展节点，再将当前扩展节点的邻域节点中的未扩展节点设置为新的待扩展节点，直到当前搜索出的启发路径代价和最小的待扩展节点是所述路径优化终点；

其中，在所述区域连通图内的任意节点所耗费的启发路径代价和等于从所述路径优化起点到该节点的实际移动代价与从该节点到所述路径优化终点的预测路径代价的和值。

2.根据权利要求1所述导航路径规划方法，其特征在于，所述利用节点在所属的运动轨迹线段上的居中程度值换算出的对应的代价值，搜索出与预先设置的导航起点相匹配的路径优化起点以及与预先设置的导航终点相匹配的路径优化终点的方法包括：

若存在待判断节点与所述导航起点的连线不经过障碍物栅格点，则确定该待判断节点是直通所述导航起点的节点；然后对每个直通所述导航起点的节点在其所属的运动轨迹线段上的居中程度值换算出的对应的代价值进行数值排序；再将代价值最小的直通所述导航起点的节点设置为所述路径优化起点；

若存在待判断节点与所述导航终点的连线不经过障碍物栅格点，则确定该待判断节点是直通所述导航终点的节点；然后对每个直通所述导航终点的节点在其所属的运动轨迹线段上的居中程度值换算出的对应的代价值进行数值排序；再将代价值最小的直通所述导航终点的节点设置为所述路径优化终点；

其中，所述待判断节点属于预先构建的区域连通图内的节点；每个所述待判断节点都位于各自所属的一条运动轨迹线段上。

3.根据权利要求2所述导航路径规划方法，其特征在于，还包括：

计算数值2与所述待判断节点在所属的运动轨迹线段上的居中程度值的差值，作为偏离程度差值；

计算所述待判断节点与所述导航起点的欧式距离，作为导航起点匹配距离；再将偏离程度差值、导航起点匹配距离与居中程度影响系数相乘，得到的乘积是所述直通所述导航起点的节点在所属的运动轨迹线段上的居中程度值换算出的对应的代价值；

计算所述待判断节点与所述导航终点的欧式距离，作为导航终点匹配距离；再将偏离程度差值、导航终点匹配距离与居中程度影响系数相乘，得到的乘积是所述直通所述导航终点的节点在所属的运动轨迹线段上的居中程度值换算出的对应的代价值；

其中，居中程度值处于0至1之间；居中程度影响系数是预先设置的，用于调节待规划的路径偏离障碍物的远近程度。

4.根据权利要求1所述导航路径规划方法，其特征在于，所述从所述路径优化起点到当前扩展节点的实际移动代价等于从所述路径优化起点到当前扩展节点的父节点的实际移动代价与从当前扩展节点的父节点到当前扩展节点的实际移动代价的和值。

5.根据权利要求4所述导航路径规划方法，其特征在于，从当前扩展节点的父节点到当前扩展节点的实际移动代价的计算方法包括：

计算数值2与当前扩展节点在所属的运动轨迹线段上的居中程度值的差值，作为偏离程度差值；计算所述当前扩展节点与其父节点的欧式距离，作为邻域节点匹配距离；再将偏离程度差值、邻域节点匹配距离与居中程度影响系数相乘，得到的乘积是所述从当前扩展节点的父节点到当前扩展节点的实际移动代价；

其中，居中程度值处于0至1之间；居中程度影响系数是预先设置的，用于调节待规划的路径偏离障碍物的远近程度。

6.根据权利要求4所述导航路径规划方法，其特征在于，从所述路径优化起点到当前扩展节点的父节点的实际移动代价的计算方法包括：

步骤a1、将当前扩展节点的父节点设置为回溯迭代节点，并设置移动代价迭代和为0；然后进入步骤a2；

步骤a2、判断回溯迭代节点是否为所述路径优化起点，是则进入步骤a3，否则进入步骤a4；

步骤a3、将从所述路径优化起点到当前扩展节点的父节点的实际移动代价设置为等于所述移动代价迭代和；

步骤a4、计算数值2与回溯迭代节点在所属的运动轨迹线段上的居中程度值的差值，作为偏离程度差值；计算所述回溯迭代节点与其父节点的欧式距离，作为邻域节点匹配距离；再将偏离程度差值、邻域节点匹配距离与居中程度影响系数相乘，得到的乘积是从所述回溯迭代节点的父节点到所述回溯迭代节点的实际移动代价，再将从所述回溯迭代节点的父节点到所述回溯迭代节点的实际移动代价加上所述移动代价迭代和，然后将相加得到的和值更新为所述移动代价迭代和；然后进入步骤a5；

步骤a5、将所述回溯迭代节点的父节点设置为下一次计算父子节点之间的移动代价的回溯迭代节点，再返回步骤a2；

其中，居中程度值处于0至1之间；居中程度影响系数是预先设置的，用于表示待规划的路径偏离障碍物的远近程度；

其中，更新前后的所述回溯迭代节点都属于所述区域连通图内的节点，所述回溯迭代节点及其父节点都属于所述区域连通图内的相邻接的两个节点。

7.根据权利要求1至6任一项所述导航路径规划方法，其特征在于，前述相应的节点在所属的运动轨迹线段上的居中程度值是：该节点到运动轨迹线段上相距最近的一个端点的直线距离与该运动轨迹线段的长度的一半的比值。

8.根据权利要求1至6任一项所述导航路径规划方法，其特征在于，所述从当前扩展节点到所述路径优化终点的预测路径代价是使用所述当前扩展节点与所述路径优化终点之间的欧式距离、曼哈顿距离、或对角距离来表示。

9.根据权利要求1所述导航路径规划方法，其特征在于，所述根据所述区域连通图内的节点对应的父节点信息，逐层地搜索出从所述路径优化起点到所述路径优化终点的优化导航路径的方法包括：

在所述区域连通图内所有的待扩展节点中，若启发路径代价和最小的待扩展节点是所述路径优化终点时，则从所述路径优化终点开始，按照所述路径优化终点在所述区域连通图的对应邻域内所记录的父节点位置信息，依次连接父节点以及该父节点的父节点，直到在所述区域连通图内连接到所述路径优化起点。

10.根据权利要求9所述导航路径规划方法，其特征在于，所述在所述区域连通图内所有的待扩展节点中，若启发路径代价和最小的待扩展节点不是所述路径优化终点时，则将所述启发路径代价和最小的待扩展节点设置为当前扩展节点，再将当前扩展节点的邻域节点中的未扩展节点设置为新的待扩展节点，直到当前搜索出的启发路径代价和最小的待扩展节点是所述路径优化终点的方法包括：

步骤b1、在所述区域连通图内所有的待扩展节点中，若启发路径代价和最小的待扩展节点不是所述路径优化终点时，则将所述启发路径代价和最小的待扩展节点设置为当前扩展节点，然后在所述区域连通图内搜索出当前扩展节点的未扩展的邻域节点并将其设置为新的待扩展节点，同时将当前设置出的新的待扩展节点的父节点记为当前设置出的当前扩展节点，再将所述当前扩展节点更新为已扩展节点，然后进入步骤b2；

步骤b2、在当前设置出的所有待扩展节点中，判断启发路径代价最小的待扩展节点是否为所述路径优化终点，是则执行所述根据所述区域连通图内的节点对应的父节点信息，逐层地搜索出从所述路径优化起点到所述路径优化终点的优化导航路径的方法；否则返回步骤b1。

11.根据权利要求9或10所述导航路径规划方法，其特征在于，机器人在执行所述导航路径规划方法的过程中，创建一个优先队列，用于存储待扩展节点；当优先队列内存储的优先级最高的待扩展节点出队时，将该优先级最高的待扩展节点更新为所述当前扩展节点；再在所述区域连通图内搜索出所述当前扩展节点的未扩展的邻域节点后，将当前扩展节点标记为已扩展节点以区别于未扩展节点，并将搜索出的当前扩展节点的未扩展的邻域节点加入所述优先队列并都更新为待扩展节点；

其中，未扩展的邻域节点是属于未扩展节点，未扩展节点中不存在在先配置为待扩展节点的节点，使得当前扩展节点的未扩展的邻域节点不预先存在所述优先队列内，当前扩展节点的未扩展的邻域节点不存在所述已扩展节点；

其中，所述优先队列开始出队一个待扩展节点以进行一轮节点扩展操作之前，所述优先队列已经存储有所述路径优化起点，以便于引导从所述路径优化起点开始通过邻域扩展至所述路径优化终点；

其中，待扩展节点所耗费的启发路径代价和越小时，则将待扩展节点在所述优先队列中的搜索优先级配置得越高。

12.根据权利要求1所述导航路径规划方法，其特征在于，所述区域连通图的构建方法包括：

步骤1、在机器人预先标记的相互平行的每条运动轨迹线段中，自运动轨迹线段的中点向其两侧步进地提取出具备可通行性的节点；

步骤2、基于步骤1提取出的具备可通行性的节点所构建的树形结构，结合指定节点距离导航起点的移动代价，从所述树形结构的节点的可达邻域内搜索出一个区域连通图；其中，导航起点是预先配置到机器人内的导航信息。

13.根据权利要求12所述导航路径规划方法，其特征在于，所述步骤2的具体方法包括：

步骤21、将距离导航起点的移动代价最小的指定节点配置为搜索起点；然后进入步骤22；其中，最先配置为指定节点的节点是属于步骤1提取出的任意一个具备可通行性的节点；

步骤22、在步骤1提取出的具备可通行性的节点所构建的树形结构的节点中，筛选出属于当前配置的搜索起点的可达邻域内的未遍历的邻域节点并将当前筛选出的节点都配置为指定节点，然后进入步骤23；其中，当前配置的搜索起点是父节点时，当前配置的搜索起点的可达邻域内的未遍历的邻域节点都是该父节点的子节点；步骤22筛选出的未遍历的邻域节点中不存在在先配置出的指定节点；

步骤23、将当前配置的搜索起点及步骤22筛选出的未遍历的邻域节点组成所述区域连通图，将当前配置的搜索起点配置为已遍历的节点使其不再是所述指定节点，同时将步骤22筛选出的未遍历的邻域节点配置为新的指定节点；然后在当前配置出的指定节点中，将距离导航起点的移动代价最小的指定节点配置为下一次筛选新的邻域节点的搜索起点，再返回步骤22；

步骤24、重复步骤22至步骤23，直至不存在指定节点被配置为新的搜索起点，并确定构建出一个所述区域连通图。

14.根据权利要求13所述导航路径规划方法，其特征在于，所述当前配置的搜索起点及步骤22筛选出的邻域节点都被配置为键值，以构造成为任意两个节点间均互连互通的所述区域连通图；

其中，所述搜索起点及其在步骤22中对应筛选出的邻域节点支持构建出可通行路线；

其中，所述区域连通图是一种图形结构空间，属于一种数据结构。

15.根据权利要求14所述导航路径规划方法，其特征在于，在所述步骤2中，还包括：

分别计算所述步骤1提取出的每个具备可通行性的节点距离所述导航起点的移动代价，作为所述导航起点到对应的指定节点的移动代价；

再利用计算出的指定节点对应的移动代价配置该指定节点的优先级，然后将配置有优先级的指定节点存储到优先队列空间内，其中，优先队列空间内当前存储的优先级最高的指定节点最先被筛选出来并被放置入图形结构空间内，以组成所述区域连通图；

其中，优先队列空间是用以描述优先级高低不同的指定节点之间的进出先后关系的数据结构。

16.根据权利要求15所述导航路径规划方法，其特征在于，优先级的配置方法包括：若指定节点距离所述导航起点的移动代价越大，则该指定节点的优先级越低；若指定节点距离所述导航起点的移动代价越小，则该指定节点的优先级越高；以使得距离导航起点的移动代价最小的指定节点是最先从所述优先队列空间中输出。

17.根据权利要求16所述导航路径规划方法，其特征在于，当所述优先队列空间内当前存储的优先级最高的指定节点最先被筛选出来并被配置为所述搜索起点时，将当前配置为所述搜索起点的指定节点移除出所述优先队列空间，然后将当前执行的步骤22所筛选出的节点设置为指定节点并存入所述优先队列空间，同时将当前配置为所述搜索起点的指定节点存入已遍历节点集合结构内；

其中，所述步骤22筛选出的节点是指没有预先存入所述已遍历节点集合结构的、也没有预先存在于所述优先队列空间内的、且是属于当前配置的搜索起点的可达邻域内的邻域节点。

18.根据权利要求13至17任一项所述导航路径规划方法，其特征在于，所述属于当前配置的搜索起点的可达邻域内的邻域节点同时满足以下条件：

当前配置的搜索起点与其邻域节点之间的直线距离小于或等于机器人的机身直径；

当前配置的搜索起点与其邻域节点之间的连线不穿过障碍物栅格点且不穿过未知栅格点。

19.根据权利要求12所述导航路径规划方法，其特征在于，所述步骤1的具体方法包括：

首先将所述相互平行的每条运动轨迹线段的中点都配置为所述具备可通行性的节点；

然后，从每条运动轨迹线段的中点开始，分别沿着对应一条运动轨迹线段的两端，每隔一个所述机身直径的采样步长提取一个节点，并将提取出的节点配置为所述具备可通行性的节点，直至采样至对应一条运动轨迹线段的两个端点为止。

20.根据权利要求19所述导航路径规划方法，其特征在于，所述步骤1还包括：

若按照一个所述机身直径的采样步长在对应一条运动轨迹线段上提取出的一个节点与同一条运动轨迹线段的一端点的距离小于一个所述机身直径，则将该端点也配置为所述具备可通行性的节点。

21.根据权利要求13至17任一项所述导航路径规划方法，其特征在于，所述树形结构是在一个二维数据集合上构建的Kd树，所述Kd树代表的是对二维数据集合构成的二维坐标空间的一种划分；

其中，所述二维数据集合是由所述具备可通行性的节点构建而成的，包括节点坐标值、和所属的运动轨迹线段的标记信息。

Images