[发明专利]一种结合栅格和拓扑地图的快速路径搜索方法

权利要求书

1.一种结合栅格和拓扑地图的快速路径搜索方法，适用于地面移动机器人实时自主导航任务，其特征在于，包括以下步骤：

步骤1，基于改进的K3M算法，从栅格地图M中提取简化广义Voronoi图G＝{E,V,M_dist}作为拓扑地图，其中E为边，V为节点，M_dist为图的距离矩阵；

步骤2，利用栅格地图M，在步骤1中得到的拓扑地图中，搜索候选起始边PSE和候选目标边PTE；

步骤3，以步骤2中找到的候选起始边PSE和候选目标边PTE为启发，查找候选起始路径T_a→G和候选目标路径T_G→b；对于其中的每一对候选起始路径和候选目标路径在拓扑地图中查找连接和的子图生成所有候选路径T_a→b，取其中长度最短的路径作为结果路径；

其中，步骤2具体包括以下步骤：

步骤2.1，从栅格地图M中提取障碍物像素点集合O，如公式1所示：

式中，width为机器人运动模型的宽度，x是栅格地图M中的任一像素点，x_i是栅格地图M中的障碍物像素点；

步骤2.2，在拓扑地图中，找到所有候选起始边PSE和候选目标边PTE，如公式2和公式3所示：

PSE＝(E_a,X_a)＝{(e∈E,x∈e)|VT(l_a,e)≠none,x＝VT(l_a,e)} (2)

PTE＝(E_b,X_b)＝{(e∈E,x∈e)|VT(l_b,e)≠none,x＝VT(l_b,e)} (3)

其中，E_a和E_b分别表示候选起始边和候选目标边的边集合，X_a和X_b表示对应的途经点的集合，起点l_a从一个途经点可以到达对应的候选起始边终点l_b从一个途径点可以到达对应的候选目标边VT(l_a,e)和VT(l_b,e)分别为起点l_a和终点l_b与边e之间的可视性检测函数，其函数原型VT(x_m,e)为像素点x_m与边e之间的可视性检测函数，如公式4：

其中，函数的返回值为边e上距离x_m最近且与x_m可视的像素点，VT(x_m,x_n)为两个像素点x_m和x_n之间的可视性检测函数，如公式5：

其中，表示像素点x_m和x_n之间的线段；

其中，步骤3具体包括以下步骤：

步骤3.1，对于所有候选起始边PSE，结合栅格地图M和可视性检测函数，找到所有候选起始路径T_a→G，如公式6所示：

式中，是第i个候选起始边，是对应的途经点，和分别表示的两个节点，表示线段指示的路径，表示线段指示的路径，表示线段指示的路径；表示沿边e的，从到边的第1个节点的路径段，是边的一部分；表示沿边e的，从到边的第2个节点的路径段，是边的一部分；

步骤3.2，对于所有候选目标边PTE，结合栅格地图M和可视性检测函数，找到所有的候选目标路径T_G→b，如公式7所示：

式中，是第j个候选目标边，是对应的途经点，和分别表示的两个节点，表示线段指示的路径，表示线段指示的路径，表示线段指示的路径；表示沿边的，从边的第1个节点到的路径段，是边的一部分；表示沿边的，从边的第2个节点到的路径段，是边的一部分；

步骤3.3，找到所有的常规候选路径如公式8所示：

式中，是拓扑地图G中连接节点和的子图，可用Dijkstra算法查找；

步骤3.4，在步骤3.3中的常规候选路径的基础上，还需考虑两种情况：(I)l_a和l_b可视，即VT(l_a,l_b)＝1；(II)候选起始边PSE的边集合E_a与候选目标边PTE的边集合E_b存在交集，即因此所有的候选路径T_a→b包括如公式9所示的三种情况：

步骤3.5，选取候选路径T_a→b中长度最短的一条路径作为最终的路径搜索结果，其中length(T_i)代表路径T_i的长度，使用欧氏距离来计算。