分治确定性路径寻优算法的制作方法

本发明涉及路径规划领域，具体应用于精确求取复杂障碍下的机器人的最优路径。

背景技术：

栅格路径规划的优点在于地图经栅格化处理后，可以将障碍分割为多个固定尺度的小栅格，这样可以精确识别出障碍的位置、大小及形状，使机器人更易于实现避障。但是地图环境经栅格化后，可选取的路线只能为栅格的四个角或中心点，不能直接选择两个目标的矢量连线路径，因此最终得到的路径并非最短路径。与栅格路径规划不同，在矢量路径规划中，选取的路线则可以为两个目标点的矢量连线路径，并没有栅格路径中的必须经过栅格中心点的约束。

传统的路径规划算法有a*算法，dijkstra算法，prm路径规划算法等。当a*算法和dijkstra算法应用于栅格路径规划中时，这两种算法在某种程度上属于一种贪婪算法，通过不停的比较周围可行路径的权值来选取最优路径。然而随着地图的面积的扩大，这两种算法的搜索范围也将增加，算法的时间复杂度和搜索复杂度随之增大。即使是在无障碍的空白区域中，从起点到终点，这两种算法都无法直接搜索出起点到终点的直线路径，仍需要对周围的路径进行搜索比较。prm路径规划算法一般应用于矢量路径规划中。该算法通常在地图中的可行范围内随机生成一定数量的可行点，然后将这些可行点连接，在未穿过障碍区域的连线中通过a*算法求取出最优路径。该算法的缺点在于，只有生成大量的随机可行点才有可能得到有一条从起点到终点的路径，且算法的复杂度也会随着可行点数量的增加而增大，尤其在存在狭缝通道的障碍地图中，一旦没有随机点落在该狭缝中，prm算法将无法搜索出起点到终点的可行路径。同时，由于这些可行点是随机生成的，因此得到的最优路径并不是最短路径。

技术实现要素：

本发明的目的在于提供可通过从经障碍区域角点部分的可行连线路径集合中搜索出一条最优路径的分治确定性路径寻优算法。

本发明包括以下步骤：

1)将需要搜索的环境地图分割成栅格，每个栅格只有空白状态或赋值状态，空白状态表示该栅格可以通行，赋值状态表示该栅格为障碍区域，无法通行；根据实际已知信息，在相隔并且中间存在赋值栅格的空白栅格中确定起点和终点位置；

在步骤1)中，所述需要搜索的环境地图可进行栅格化处理，根据障碍复杂程度及要求精度，设置所要产生地图矩阵的大小，生成一个n×n的方阵，将环境地图分割成多个规则的正方形栅格，其中空白栅格赋值为“0”，表示该区域可以通行，而障碍栅格赋值为“1”，表示该区域禁止通行；所述空白状态或赋值状态可对应于计算机中的“0”和“1”两种状态。

2)根据障碍区域的位置及数量，提出一种角点合力法求取栅格障碍的角点位置，并进行标记；

在步骤2)中，所述栅格障碍的角点位置可使用角点合力法计算并标记，所述角点合力法的计算步骤可为：

(1)对单位栅格障碍周围的上、下、左、右四个方向的栅格可行区域加权赋值1；

(2)当单位障碍的左或右和上或下的可行区域块都具有加权值1时，单位障碍被定义为障碍连通区域的角点位置；

(3)当单位障碍的左方和上方空白栅格的权值都为1时，标记该单位障碍的左上角的空白栅格；同理，当单位障碍的右方和上方空白栅格的权值都为1时，标记该单位障碍的右上角的空白栅格；当单位障碍的右方和下方空白栅格的权值都为1时，标记该单位障碍的右下角的空白栅格；当单位障碍的左方和下方空白栅格的权值都为1时，标记该单位障碍的左下角的空白栅格，通过力的合成公式：

规定力的合成方向指向障碍的角点处，其中，f为分力f1和分力f2的合力，α为两个分力的夹角。

3)将所有的标记角点两两相连，形成多条可行路径；然后将每段可行路径分割为m个线段，每一线段的线长范围用一个矩形包围，接着求出该矩形四个角的坐标值，并对坐标值进行取整处理，把这个矩形的四个角的坐标值与步骤1)中经栅格化处理生成的矩阵坐标进行比较，判断每个坐标点是否位于空白栅格中；若是，则为可行路径，否则为穿过障碍的路径，将其剔除可行路径集合外；

在步骤3)中，所述将所有的标记角点两两相连，形成多条可行路径，可连接各标记角点、起点和终点，产生多条路径；接着剔除经过障碍的连线路径，生成可行路径集合；引入微元的思想，将每一段连线微元化成m个线段，每一线段用一个矩形来等同其线长的范围；然后求出该矩形四个角的坐标值，并对坐标值进行取整处理，把矩形的四个角的坐标值与步骤1)中经栅格化处理生成的矩阵坐标进行比较，若这四个对角坐标中有一个坐标点位于障碍区域中，则该连线被视为穿过了障碍区域，由此被剔除出可行连线路径的集合外；剔除经过障碍连线的具体方法可为：引入微元法的思想，将所有的线段路径分割成多个线段，则每个线段所经过的地图范围用一个矩形包围，该矩形的四个角点的坐标表示如下：

式中，(x3,1,y3,1)，(x3,2,y3,2)，(x3,3,y3,3)，(x3,4,y3,4)分别为矩形的四个定角坐标；为向下取整函数，为向上取整函数；dir为连线与地图水平轴的夹角，且dir＝arctan((y2-y1)/(x2-x1))；d表示每段小线段的长度，且存在关系d＝d/n，n＝d/0.5；因此只需通过判断矩形的四个顶角坐标点是否存在位于障碍区域的坐标，便可进一步判断出该连线路径是否穿过障碍区域，即是否属于可行路径集合；若穿过，则应删除。

4)以起点作为搜索出发点，终点作为搜索结束点，比较各搜索路径的距离大小，从中搜索出一条从起点到终点的最优路径。

在步骤4)中，所述比较各搜索路径的距离大小可采用动态对比的方法。

本发明通过从经过障碍角点区域的可行连线路径中搜索出一条最优路径，搜索最优路径所消耗的时间比dijkstra算法更少，搜索范围比a*算法和dijkstra算法更小，搜索得到的最优路径距离为最短，本发明搜索最优路径所消耗的时间比dijkstra算法更少，搜索范围比dijkstra算法更小，搜索得到的最优路径距离为最短。与prm算法相比，本发明提出的算法的标记角点是随独立的障碍连通区域的角点的数量而生成的，并不是随机生成的，对于复杂障碍尤其是狭缝障碍等均可搜索出一条最优路径，并且可以保证该路径为最短路程。

附图说明

图1为本发明的流程图。

图2为两种不同的栅格类型地图算法测试。在图2中，(a)为“迷宫型”栅格类型地图，(b)为“陷阱型”栅格类型地图。

图3为测试本发明所提角点合力法的两幅障碍地图的赋值结果。在图3中，(a)为“迷宫型”障碍地图，(b)为“陷阱型”障碍地图。

图4为四种障碍栅格区域周围的空白栅格区域的搜索及加权赋值示意图。在图4中，(a)为左上角障碍栅格区域，(b)为右上角障碍栅格区域，(c)为右下角障碍栅格区域，(d)为左下角障碍栅格区域。

图5为使用对角合力法将图4所示障碍类型的赋值和标记结果。在图5中，(a)为左上角障碍类型，(b)为右上角障碍类型，(c)为右下角障碍类型，(d)为左下角障碍类型。

图6为测试本发明的两幅障碍地图的角点标记结果。在图6中，(a)为“迷宫型”障碍地图，(b)为“陷阱型”障碍地图。

图7为经过栅格的最短路径的几何示意图。

图8为采用微元法判断连线路径是否穿过障碍的示意图。

图9为剔除图6穿过障碍区域的标记点间的连线路径的仿真结果。在图9中，(a)为“迷宫型”障碍地图，(b)为“陷阱型”障碍地图。

图10为本发明中举例说明的一个具有障碍的环境地图。

图11为可行路径记录表openlist从搜索开始到结束的部分更新记录。

图12为图10中起点、终点和各角点之间可行路径的距离记录。在图12中，(a)为所有角点的可行路径距离的更新记录，(b)为起点的可行路径距离的更新记录。

图13为图10中起点到各角点及终点的可行路径距离的部分更新记录。

图14为测试本发明的两幅障碍地图搜索的全局最优路径与a*算法和dijkstra算法搜索的全局最优路径的仿真结果。在图14中，实线“d&c”为本发明的仿真结果，虚线“a*”为a*算法的仿真结果，点线“dij”为dijkstra算法的仿真结果；(a)为测试“迷宫型”障碍地图的仿真结果，(b)为测试“陷阱型”障碍地图的仿真结果。

具体实施方式

以下实施例将结合附图对本发明作进一步的说明。

参见图1，本发明实施例包括以下步骤：

1)将需要搜索的地图分割成规则且均匀的小栅格，每个栅格只有空白或赋值两种状态，这对应于计算机中的“0”和“1”两种状态。空白状态表示该栅格可以通行，赋值状态表示该栅格为障碍区域，无法通行；根据实际已知信息，在相隔并且中间存在赋值栅格的空白栅格中确定起点和终点位置；

在步骤1)中，对环境地图进行栅格化处理，根据障碍复杂程度及要求精度，设置所要产生地图矩阵的大小，生成一个n×n的方阵，将环境地图分割成多个规则的正方形小栅格。其中空白栅格赋值为“0”，表示该区域可以通行，而障碍栅格赋值为“1”，表示该区域禁止通行。本发明以2个50×50方阵为仿真对象，赋值为0的栅格以空白的形式表示，赋值为1(即障碍区域)的栅格以涂黑表示，如图2(a)和图2(b)所示。

2)根据障碍区域的位置及数量，提出一种角点合力法求取栅格障碍的角点位置，并进行标记；

在步骤2)中，使用角点合力法计算并标记出障碍角点，其效果如图6(a)和图6(b)所示。角点合力法的详细计算步骤为：

(1)对单位栅格障碍周围的上、下、左、右四个方向的栅格可行区域加权赋值1；

(2)当单位障碍的左或右和上或下的可行区域块都具有加权值1时，该单位障碍被定义为障碍连通区域的角点位置，如图3(a)和图3(b)所示；

(3)当单位障碍的左方和上方空白栅格的权值都为1时，如图4(a)所示，标记该单位障碍的左上角的空白栅格；同理，当单位障碍的右方和上方空白栅格的权值都为1时，如图4(b)所示，标记该单位障碍的右上角的空白栅格；当单位障碍的右方和下方空白栅格的权值都为1时，如图4(c)所示，标记该单位障碍的右下角的空白栅格；当单位障碍的左方和下方空白栅格的权值都为1时，如图4(d)所示，标记该单位障碍的左下角的空白栅格。通过力的合成公式：

得到上述4种情况的力的合成方向如图5(a)～(d)所示，规定力的合成方向指向障碍的角点处。其中，f为分力f1和分力f2的合力，α为两个分力的夹角。以图5(a)为例，障碍栅格(黑色区域)对附近的具有权值为1的空白栅格(白色区域)根据其在单位障碍的方位产生向上或向下或向左或向右方向力的作用。因此，该例产生指向上方的力f1和指向左方的力f2，由上述力的合成原理公式，得到合力f，合力f指向的空白栅格位置，并标记为角点。

3)将所有的标记角点两两相连，形成的多条可行路径；然后将每段可行路径分割为m个小线段，每一小线段的线长范围用一个小矩形包围，如图8所示，接着求出该矩形四个角的坐标值，并对坐标值进行取整处理，把这个小矩形的四个角的坐标值与步骤1)中经栅格化处理生成的矩阵坐标进行比较，判断每个坐标点是否位于空白栅格中。若是，则该这条路径为可行路径，否则为穿过障碍的路径，应将其剔除可行路径集合外；

在步骤3)中，连接各标记角点，起点和终点，产生多条路径。接着剔除经过障碍的连线路径，生成可行路径集合。引入微元的思想，将每一段连线微元化成m个小线段，每一小线段用一个小矩形来等同其线长的范围。然后求出该矩形四个角的坐标值，并对坐标值进行取整处理，把这个小矩形的四个角的坐标值与步骤1)中经栅格化处理生成的矩阵坐标进行比较，若这四个对角坐标中有一个坐标点位于障碍区域中，则该连线被视为穿过了障碍区域，由此被剔除出可行连线路径的集合外。剔除经过障碍连线具体方法为：引入微元法的思想，将所有的线段路径分割成多个小线段，则每个小线段所经过的地图范围用一个小矩形包围，如图8所示，该矩形的四个角点的坐标表示分别如下式：

式中，(x3,1,y3,1)，(x3,2,y3,2)，(x3,3,y3,3)，(x3,4,y3,4)分别为小矩形的四个定角坐标；为向下取整函数，为向上取整函数；dir为连线与地图水平轴的夹角，且dir＝arctan((y2-y1)/(x2-x1))；d表示每段小线段的长度，且存在关系d＝d/n，n＝d/0.5。因此只需通过判断小矩形的四个顶角坐标点是否存在位于障碍区域的坐标，便可进一步判断出该连线路径是否穿过了障碍区域，即是否属于可行路径集合。若穿过，则应删除，结果如图9(a)和图9(b)所示。

4)以起点作为搜索出发点，终点作为搜索结束点，采用动态对比的方法比较各搜索路径的距离大小，从中搜索出一条从起点到终点的最优路径。

在步骤4)中，以下内容以图10为例进行详细说明。将步骤3)中选取出的可行连线路径放入openlist中，如图11中的“openlist”所示。将起点到所有标记角点的连线路径的长度放入distancelist中，如图12(a)，使用动态对比的方法，比较标记角点t到达表记角点k的多条可行路径的距离，决策得到起点到标记角点k的最短距离，如下式所示：

其中，fs,k表示将要搜索的从起点到标记角点k的最短可行路径距离，fs,t表示已决策的从起点到标记点t的最短可行路径距离，d(t,v,k)表示从标记角点t经标记角点v到标记角点k的可行路径距离，d(t,u,k)表示从标记角点t经标记角点u到标记角点k的可行路径距离(从标记角点t到标记角点k的可行路径可能需要通过多个其他标记角点才能到达，此处举例为经过一个其他标记角点即可到达)。fs,q表示起点到标记角点q无可行路径，其中，k,t,v,u,q为包括终点及所有标记角点的集合u内任意一标记角点。

接着对distancelist进行更新，同时更新记录到达每个角点最优路径的父节点表flaglsit，并将已确认为局部最优的连线路径从openlist中剔除，直至openlist为空。以图10为例，起点到标记角点1距离为3.16，到标记角点2距离为6.08，到标记点角3距离为∞(即无法直接到达)，到标记角点4距离为∞，到标记角点5距离为8.06，到标记角点6的距离为5.10，到终点的距离为∞，如图12(a)中的“起点的distancelist”所示。通过路径距离的第一次比较，起点可以通过标记角点1到达标记角点2再到达标记角点3，其路径距离为“起点—1”、“1—2”与“2—3”连线路径的距离之和，即3.16+3+4.47＝10.63；起点还可以通过标记角点2到达标记角点3，其路径距离为“起点—2”与“2—3”连线路径的距离之和，即6.08+4.47＝10.55，而10.55＜10.63＜∞，故起点的distancelist中到达标记角点3的距离更新为10.55，如图12(b)中“起点的distancelist第一次更新”所示。标记角点3的flaglist更新为“起点—2—3”,如图13中“标记角点3的flaglist”所示，openlist中将“起点—1”、“1—2”、“起点—2”与“2—3”剔除，因为这三条连线路径已经比较完毕，如图11中“openlist第一次更新”所示。同理，通过比较标记角点i到标记点j的路径距离，并更新其distancelist和flaglist，直至openlist为空，如图11中“openlist更新完毕”所示。最终从终点的flaglist中得到起点到终点的最短路径连接点记录，如图13的“终点的flaglist”所示，由此得到起点到终点的最短路径为“起点—5”和“5—终点”的连线路径，其最短路径距离为8.06+4.12＝12.18。

本发明通过从经过障碍角点区域的可行连线路径中搜索出一条最优路径，对于图2(a)的障碍地图，本发明搜索最优路径所消耗的时间比dijkstra算法更少，搜索范围比a*算法和dijkstra算法更小，搜索得到的最优路径距离为最短，测试“迷宫型”障碍地图的相关数据如表1所示；对于图2(b)的障碍地图，本发明搜索最优路径所消耗的时间比dijkstra算法更少，搜索范围比dijkstra算法更小，搜索得到的最优路径距离为最短，测试“陷阱型”障碍地图的相关数据如表2所示。与prm算法相比，本发明提出的算法的标记角点是随独立的障碍连通区域的角点的数量而生成的，并不是随机生成的，对于复杂障碍尤其是狭缝障碍等均可搜索出一条最优路径，并且可以保证该路径为最短路程。

表1

表2

表1和2为本发明算法与a*算法和dijkstra算法对于图2的障碍地图搜索最优路径的时间、最优路径的距离和搜索范围的相关数据。

下面通过仿真实验结果测试本发明所提算法的有效性，并对本发明的具体实施步骤及过程作进一步地说明，需要强调的是，以下的仿真结果图均为本发明的真实测试结果。

第一步：首先将要搜索的地图进行了栅格化处理，将障碍区域分割成大小相同的栅格，本发明以50×50方阵为例，使用两种不同的栅格类型地图进行算法测试，如图2(a)、图2(b)所示。

第二步：提出角点合力法用于求取地图中的栅格角点区域，该方法类似于物理中力的合成，把栅格分为4种类型，并对栅格区域周围的空白区域进行赋值，分别如图4(a)、图4(b)、图4(c)和图4(d)所示。接着根据合力方向确定标记点，分别如图5(a)、图5(b)、图5(c)和图5(d)所示。对本发明的两幅障碍测试地图，对图2(a)和图2(b)使用角点合力法，赋值和标记结果如图6(a)和图6(b)所示。

第三步：连接图6(a)和图6(b)中的所有的已标记点、起点和终点，引入微元思想，对各连接路线进行片段化处理，判断每一条连线路径是否穿过的障碍区域，剔除穿过障碍区域的连线路径，得到所有的可行路径如图9(a)和图9(b)。该操作可以极大的缩小路径规划的搜索范围，为路径规划节省了时间。

第四步：计算图9(a)和图9(b)中所有的连线路径的长度并存储，然后对路径距离进行动态对比，如图11、图12(a)和图12(b)、图13所示，记录起点到各个标记点的最优路径所要经过的连线路径。最终可以从以结束点为终点的最优路径记录中得到起点到终点的全局最短路径，实验仿真结果如图14(a)和图14(b)所示，相关数据如表1和2所示。

在无障碍情形下，显然两点之间直线路径最短。若障碍不位于这条直线路径上，此时这些障碍不影响最短路径的选取。若障碍位于该直线路径上，则需要绕过该障碍通往终点，且通过几何知识可以证明，如图7所示，只有“贴着”障碍角点的路径才有可能为最短路径。受此启发，本发明提出一种分治确定性路径寻优算法。栅格化处理地图后，采用“分而治之”的思想，将障碍的凸出角点分为四种情形，使用角点合力法来快速确定地图中的障碍区域凸出角点的位置，并标记障碍区域的角点位置，然后连接各标记角点，通过线段微元法，进一步判断连线是否穿过了障碍区域，从而删除穿过障碍的连线。在剩余的可行路线中，对路径距离采用动态对比的方法比较局部线段的长度值，从而搜索出一条从起点到终点的全局最优路径。