CN109214596A - 求取具有方向约束和障碍限制的栅格最短路径afw算法 - Google Patents

Abstract

求取具有方向约束和障碍限制的栅格最短路径AFW算法，涉及移动机器人路径规划领域，实施步骤：首先规定算法拓扑规则，包括水流贪婪原则、顺流—平流—断流原则和水流方向约束，然后根据地势生成规则生成地势图；最后按照算法拓扑规则和地势生成规则寻找在方向约束和障碍限制下的最短路径。本发明根据自然界水流动的启示，结合实际机器人搜索过程，可求取在方向约束和障碍限制下的最短路径，规定的拓扑规则降低了算法的复杂度，可使算法避免局部最优，可完成无方向约束最短路径寻优、无方向约束有障碍限制条件下最短路径寻优、有方向约束最短路径寻优、有方向约束且具有障碍限制的最短路径寻优和简单的三维障碍限制下的最短路径寻优。

Description

求取具有方向约束和障碍限制的栅格最短路径AFW算法

技术领域

本发明涉及移动机器人领域，尤其是无人机等具有飞行方向约束和障碍限制的机器人路径规划领域。

背景技术

按照数据模型，最短路径算法可分为矢量数据最短路径和栅格模型最短路径。TSP问题、地图软件寻找的最短路径等均为矢量最短路径，一般通过智能算法，贝尔曼原理等寻求最短路径，这种方法往往用于在固定的路径数和路径方向以及不精确路径精度要求。在矢量图中，这些路径对应于不同的矢量线段或弧度。而对于机器人路径规划，火灾的传播路径等大范围的精确路径寻优，则使用栅格路径模型更佳。目前栅格最短路径算法主要有Dijikstra算法、A*算法、势场法等，这些算法存在运行时间长、复杂度较大、易陷入局部最优路径、无方向约束、以及无法应对复杂障碍限制等缺陷。

传统的栅格最短路径算法如A*算法，其旨在求取无航向角约束下的最短路径，然而增加航向角限制后，该算法得到的最优路径可能为无效路径，如图1所示，假设使用A*算法得到的最优点O，但是增加附加条件限制后，该点可能不在可行域内，此时为无效点。因此这类算法在求取带有方向约束下的最短路径时并不适用。

发明内容

本发明的目的在于解决现有技术中的上述问题，提供求取具有方向约束和障碍限制的栅格最短路径AFW算法。

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

受自然界水流扩散现象的启发，结合图论知识，本发明提出一种AFW(ArtificialFlowing Water)最短路径算法。以图2为例，假设各线段代表一条水渠，A点代表高地势，B点代表低地势，不同的水渠代表水流从A流向B的不同路径。从A点到B点有多条路径可以流动，假设有足够多的水，且水流的速度在任何时刻都相同。在A点连续不断地注入水，则必有一股水流最先流到B点，根据t＝s/v，已知水流速度v保持恒定，因此时间t越小，距离s越小。显然这条水流所经过的路径就是A点到B点的最短路径。根据上述水流现象，可以得知人工水流具有如下自治性：

1、水渠走向即为水流方向，对应机器人的可移动方向；

2、水流流向低地势，表征水流的一种贪婪属性；

3、水流可平流，这是由于水流本身的势能及上一时刻水流的动能；

4、水流可分为顺流(高地势流向低地势)、平流(在地势相同栅格中流动)、断流(水无法从低地势向高地势逆流，此时处于断流)。

本发明包括以下步骤：

步骤1、确定水流贪婪原则、水流方向约束和顺流—平流—断流原则；

步骤2、根据地势生成规则生成地势图，包括起点、终点和障碍；

步骤3、获取当前点的地势值和水流方向，并由步骤1的原则判断下一流向点；

步骤4、循环步骤3，标记每个流向点，直至有一股水流最先流向终点，此时算法终止，该股水流流经的有向线段即为最短路径(可能有多条最短路径)。

本发明规定水流符号W，第i代水流W_i，每一代水流可流动方向d有：左流1、直流0、右流-1，W_i ^d表示第i代水流可流动的方向为d，d＝{1,0,-1}，W_i ^0,0表示第i代的直流支路，为W_i ^0,0的子代，且有三条支流，下角标表示从第i代支流延伸出的第i+1代支流，上角标表示有左流、直流、右流三个方向。终点地势符号为T_e，终点地势圈第i圈的地势为T_e,i；起点地势符号为T_s＝2N,起点地势圈只有一圈，其地势为T_s,1＝T_s-1＝2N-1；地势图中某一点(m,n)的地势为T(m,n)。

步骤1中，水流贪婪原则和顺流—平流—断流原则相当于一种“剪枝”操作，在所有可能路径中排除不可能选择。通过该原则可以大大减少遍历路径的数量，不仅符合自然界的水流现象，还减少了算法的时间复杂度和空间复杂度。

步骤1中，保证算法可以求取最短路径的水流贪婪原则包括两个水流贪婪原则，水流贪婪原则1：水流每次在可流动的方向中选择地势更低的路径，即若且则该支路选择直流，此时有d＝{0}，该规则可保证水流获取最短路径；水流贪婪原则2：若存在相同的地势差路径，则全部选择，即若则同时选择直流和左流，此时有d＝{0,1}，该规则可避免贪心算法导致的局部最优，同时可以得到多条最短路径；

顺流—平流—断流原则，顺流：水流由高地势流向低地势栅格，即有该原则保证算法收敛于终点；平流：水流在两个地势相同的栅格流动，即有可以保证在获取多条最短路径的同时避免算法陷入局部收敛；断流：水流由低地势流向高地势栅格则发生断流，即有该规则减小了算法的搜索范围，降低了算法的时间复杂度和空间复杂度。水流只会出现顺流—平流或逆流两种现象中的一种。

水流方向约束：只能直流0°、左流45°、右流-45°，即有d＝{1,0,-1}。对于一般慢速机器人，可以完成8个方向转弯，此时d＝{0,1,2,3,4,5,6,7}，此外，该算法还可用于求取4个方向约束和5个方向约束下的最短路径问题。然而对于快速机器人如无人机，在飞行过程中无法直接向后飞行或垂直于飞行方向转弯，因此存在更为严格的飞行方向约束，只能实现3个方向的选择。同时该算法亦可退化为无方向约束或任意方向约束。在水流贪婪原则与中顺流—平流—断流原则下，得到8个方向约束下的最短路径，这体现了本发明提出算法的具有广阔的应用范围和极强的寻优能力。

步骤2中，首先把方形地图栅格化，获取地图边长大小的值N，然后根据实际研究问题确定终点和起点位置，规定终点地势符号为T_e，且T_e＝0，第1圈地势T_e,1＝1，第N圈地势为T_e,N＝N。起点地势符号为T_s，且T_s＝2N，起点地势圈只有1圈，T_s,1＝2N-1。在起点和终点第一次相遇的序列处做差，该处地势值为T_s-e，再令障碍的地势为T_o＝2N，生成无障碍地势图或有障碍地势图。

步骤3中，在步骤2生成的地势图中，获取当前点的坐标某一点(m,n)的地势T_i(m,n)及可流动方向d，然后获取d＝{1,0,-1}的3个方向上的下一点地势值T_i+1，通过步骤(2)顺流—平流—断流原则比较的大小关系，判断下一点是否可流动。

步骤4中，重复步骤3，若不符合顺流—平流原则，则该水流支路断流，在所有符合步骤1和步骤3的水流路径中，选择最先到达终点的路径作为最短路径。

相对于现有技术，本发明技术方案取得的有益效果是：

1、与现有算法相比，本发明算法不仅收敛速度快，同时空间复杂度小，可以完成无方向约束最短路径寻优、无方向约束有障碍限制条件下最短路径寻优、有方向约束最短路径寻优、有方向约束且具有障碍限制的最短路径寻优和简单的三维障碍限制下的最短路径寻优。

2、水流贪婪原则减小了算法的空间复杂度，同时可避免局部最优。

3、顺流—平流—断流原则，既符合自然水流自治性，又保证算法可快速寻找到最短路径。

4、栅格化使得搜寻到的路径更加精确。

5、方向约束可适用于不同类型机器人的最短路径搜索。

附图说明

图1为求解具有限制条件下的最优解分析图；

图2为自然界水流分流现象；

图3为步骤3和4的算法流程图；

图4(a)为无方向约束机器人的8个限制方向移动图；

图4(b)为具有方向约束的机器人4个方向约束移动图；

图4(c)为具有方向约束的机器人5个方向约束移动图；

图5为具有方向约束的机器人3个方向约束移动图；

图6为终点地势图；

图7为起点—终点地势图；

图8为无障碍限制的地势图；

图9为具有障碍限制的地势图；

图10为仿真实验生成的地势图；

图11(a)为方向2约束下无障碍限制的所有水流路径；

图11(b)为方向2约束下无障碍限制的最短路径；

图12(a)为方向5约束下无障碍限制的所有水流路径；

图12(b)为方向5约束下无障碍限制的最短路径；

图13(a)为2方向约束和具有障碍限制下的地势图；

图13(b)为2方向约束和具有障碍限制下的最短路径。

具体实施方式

为了使本发明所要解决的技术问题、技术方案及有益效果更加清楚、明白，以下结合附图和实施例，对本发明做进一步详细说明。

为了快速搜寻到具有方向约束和障碍限制下的栅格最短路径，避免陷入局部最优和算法空间复杂度过大问题，同时兼顾机器人行走的方向约束，本发明制定了水流贪婪原则、顺流—平流—断流原则和水流方向约束。本实施例进行了不同方向约束和障碍限制下的仿真实验，仿真结果验证了该算法的有效性，以下对各步骤做进一步说明。

本发明包括以下步骤：

步骤1、确定水流贪婪原则、水流方向约束和顺流—平流—断流原则；

步骤2、根据地势生成规则生成地势图，包括起点、终点和障碍；

步骤3、获取当前点的地势值和水流方向，并由步骤1的原则判断下一流向点；

步骤4、循环步骤3，标记每个流向点，直至有一股水流最先流向终点，此时算法终止，该股水流流经的有向线段即为最短路径(可能有多条最短路径)。

步骤1中，水流贪婪原则包括两个水流贪婪原则，水流贪婪原则1：水流每次在可流动的方向中选择地势更低的路径，该规则可保证获取最短路径；水流贪婪原则2：若存在相同的地势差路径，则全部选择，该规则可避免贪心思想导致的局部最优，同时可以得到多条最短路径；

顺流—平流—断流原则，顺流：水流由高地势流向低地势栅格，保证算法收敛于终点；平流：水流在两个地势相同的栅格流动，保证可以获取多条最短路径；断流：水流由低地势流向高地势栅格，若不存在顺流或平流，则发生断流。该规则减小了算法的搜索范围，降低了算法的时间复杂度和空间复杂度；

水流方向约束：只能直流0°、左流45°、右流-45°；对于一般慢速机器人，可以完成8个方向转弯，如图4(a)所示，此外，该算法还可用于求取4个方向约束和5个方向约束下的最短路径问题，如图4(b)和图4(c)所示。然而对于快速机器人如无人机，在飞行过程中无法直接向后飞行或垂直于飞行方向转弯，因此存在更为严格的飞行方向约束，只能实现3个方向的选择，如图5所示。同时该算法亦可退化为无方向约束或任意方向约束，在水流贪婪原则与中顺流—平流—断流原则下，亦可找到最短路径。

步骤2中，首先结合图论知识和水流现象，生成栅格图，确定终点的位置，并规定其地势为0，依次向外扩展至栅格边长N，如图6所示；找到起点的位置，并规定该点地势为2N，N为UAV(Unmanned Aerial Vehicle，无人机)正方形搜索地图的边长，向周围八个方向扩展一个栅格，其地势值为2N-1，如图7所示。对起点和终点地势扩展首次相遇的行或列做差，生成无障碍限制下的的地势图，如图8所示。若存在障碍，则令障碍的地势为2N，生成具有障碍限制下的地势图，如图9所示。

如图3所示，为步骤3和4的算法流程图；步骤3中，首先在步骤2生成的地势图中，获取当前点的坐标及可流动方向，然后获取3个可流动方向的下一点地势值，通过步骤2顺流—平流—断流原则判断下一点是否可流动，若该点地势小于下一点地势，则该方向直流断流；若该点地势大于或等于下一点地势，则比较下一点地势，取最小地势方向流入，并标记该点；如果该点为终点，输出标记点坐标及流向路径，然后结束；如果该点不是终点，则返回获取当前点坐标及其可流方向，进行循环操作，直至标记出该点为终点，在所有符合步骤1和步骤3的水流路径中，选择最先到达终点的路径作为最短路径。

本发明的实验结果如下：

1、仿真参数如下：

(1)地势图边界N大小：20，(2)起点坐标：(5,5)，(3)起点方向：2和5，(4)终点坐标：(15,13)；

2、输入上述参数，运行程序，首先得到地势图，如图10所示；

3、选定起始点方向为2，运行程序，得到在方向2约束下无障碍限制的水流路径，如图11(a)所示；

4、选定起始点方向为2，运行程序，得到在方向2约束下无障碍限制的最短路径，如图11(b)所示；

5、选定起始点方向为5，运行程序，得到在方向5约束下无障碍限制的水流路径，如图12(a)所示；

6、选定起始点方向为5，运行程序，得到在方向5约束下无障碍限制的最短路径，如图12(b)所示，最短路径为11个步长；

7、选定起始点方向为2，设置障碍坐标点(10,8)＝40、(10,7)＝40、(10,6)＝40、(10,5)＝40、(13,11)＝40、(13,12)＝40、(13,13)＝40、(13,14)＝40，运行程序，得到在方向2约束和具有障碍限制下的地势图，如图13(a)所示，最短路径为15个步长；

8、选定起始点方向为2，设置障碍坐标点(10,8)＝40、(10,7)＝40、(10,6)＝40、(10,5)＝40、(13,11)＝40、(13,12)＝40、(13,13)＝40、(13,14)＝40，运行程序，得到在方向2约束和具有障碍限制下的最短路径，如图13(b)所示，最短路径为11个步长。

Claims (5)

1.求取具有方向约束和障碍限制的栅格最短路径AFW算法，其特征在于：包括以下步骤：

步骤1、确定水流贪婪原则、水流方向约束和顺流—平流—断流原则；

步骤2、根据地势生成规则生成地势图，包括起点、终点和障碍；

步骤3、获取当前点的地势值和水流方向，并由步骤1的原则判断下一流向点；

步骤4、循环步骤3，标记每个流向点，直至有一股水流最先流向终点，此时算法终止，该股水流流经的有向线段即为最短路径。

2.如权利要求1所述的求取具有方向约束和障碍限制的栅格最短路径AFW算法，其特征在于：步骤1中，水流贪婪原则包括两个水流贪婪原则，水流贪婪原则1：水流每次在可流动的方向中选择地势最低的路径；水流贪婪原则2：若存在相同的地势差路径，则全部选择；水流方向约束：只能直流0°、左流45°、右流-45°；顺流—平流—断流原则，顺流：水流由高地势流向低地势栅格；平流：水流在两个地势相同的栅格流动；断流：水流由低地势流向高地势栅格，水流只会出现顺流—平流或逆流两种现象中的一种。

3.如权利要求1所述的求取具有方向约束和障碍限制的栅格最短路径AFW算法，其特征在于：步骤2中，首先生成终点地势图：设定终点地势为0，依次向外扩展至N，生成终点地势图；然后生成起点—终点地势图：设定地势为2N，N为UAV正方形搜索地图的边长，向外扩展一层，生成起点—终点地势图；最后对起点地势圈和终点地势圈第一次相遇的位置做差，生成无障碍限制地势图，规定障碍地势为2N，生成具有障碍限制的地势图。

4.如权利要求1所述的求取具有方向约束和障碍限制的栅格最短路径AFW算法，其特征在于：步骤3中，首先获取起点的地势及其可流动方向，然后根据水流贪婪原则、水流方向约束和顺流—平流—断流原则及生成的地势图，决定水流的下一流向点。

5.如权利要求1所述的求取具有方向约束和障碍限制的栅格最短路径AFW算法，其特征在于：步骤4中，重复步骤3，若不符合顺流—平流原则，则该水流支路断流，在所有符合步骤1和步骤3的水流路径中，选择最先到达终点的路径作为最短路径。