路径规划方法、系统、电子设备及存储介质

1.本发明涉及路径规划技术领域，尤其涉及一种路径规划方法、系统、电子设备及存储介质。
技术背景
2.路径规划导航已经广泛的应用于日常生活中了，a星算法是被广泛使用路径规划算法之一，a星(a-star)算法是一种静态路网中求解最短路径最有效的直接搜索方法，也是解决许多搜索问题的有效算法。传统的a星路径规划算法基于四邻域或八邻域遍历的方式查找下一个节点，遍历过程中需要耗费的时间较长，路径规划所需的时间比较慢，而且躲避障碍物时无法主动回到原规划路线，导致整个后续路线均需要重新规划，进一步影响了规划结果的时效性。


技术实现要素：

3.本发明要解决的技术问题在于针对现有技术中的缺陷，提供一种基于a星算法进行快速路径规划的方法。
4.本发明解决其技术问题所采用的技术方案是：一种路径规划方法，包括，
5.s1：对地图进行网格化处理，标记障碍物位置及障碍物的安全范围，确定起点和终点位置得到预设路径，以起点作为当前点；
6.s2：如果当前点安全范围内不存在障碍物，则转至s3，否则转至s5；
7.s3：在当前点的邻域内，基于a星算法查找下一节点；
8.s4：将当前点更新为下一节点，如果当前节点为终点，则转至s8，否则返回s2；
9.s5：在当前点的邻域内，基于优化a星算法查找下一节点；
10.所述优化a星算法利用当前点与预设路径的距离优化最小估计代价，利用运动速度优化最小代价；
11.s6：将当前点更新为下一节点，如果当前节点为终点，转至s8，否则转至s7；
12.s7：如果当前点处于预设路径上，转至s2，否则转至s5；
13.s8：停止寻找路径节点，将所有当前点依次连接，得到规划路径。
14.优选的，所述预设路径为起点到终点的直线。
15.优选的，所述基于a星算法查找下一节点的方法为：
16.分别计算当前点邻域内每个节点k的总代价值f(k)，
17.f(k)＝g(k)+h(k)
18.其中，g(k)表示节点k与起始点s(xs,ys)的最小代价值，h((k)表示节点k与终点d(xd,yd)的最小估计代价值；所述代价值为欧式距离，表示为，
[0019][0020]
当前点的邻接节点集合为，
[0021]
mov＝{mov1，mov2，
…
，movn，}
[0022]
下一节点为邻域内总代价值最小的节点，表示为，
[0023]
p
min
＝{mov|(fk
mov1
，fk
mov2
，
…
，fk
movn
)}
[0024]
其中，fk
movn
表示节点movn的总代价值，p
min
表示总代价值最小的节点。
[0025]
优选的，所述利用当前点与预设路径的距离优化最小估计代价的方法为：
[0026]
优化后的总代价值表示为，
[0027][0028]
其中，j(k)为引入的优化代价值，当目标没有进入障碍区域或者处于障碍区域时，f(k)取g(k)+h(k)，当目标离开障碍区域时，f(k)取g(k)+h(k)+j(k)，
[0029]
优化代价值j(k)与当前点和预设路径的距离相关，表示为，
[0030][0031]
其中，t是当前点到预设路径的最小距离，l为地图中每格的长度；
[0032]
预设路径的方程为，
[0033][0034]
设当前点坐标为(xn,yn)，得到t的数值为，
[0035][0036]
则优化后的总代价值表示为，
[0037]
。
[0038]
优选的，所述利用运动速度优化最小代价的方法为，
[0039]
在最小代价值中加入目标的运动速度，优化后的公式为，
[0040][0041]
其中，和β为常数，表示权重，dd，n表示当前点n到终点d的欧氏距离，v表示目标的速度。
[0042]
优选的，β＝0.8。
[0043]
优选的，所述基于优化a星算法查找的下一节点为，
[0044]
p
min
＝{mov|(fk
mov1
，fk
mov2
，
…
，fk
movn
)}
[0045]
总代价值的计算公式为，
[0046]
。
[0047]
本发明还提供了一种路径规划系统，包括，
[0048]
预处理模块，对地图进行网格化处理，标记障碍物位置及障碍物的安全范围，确定起点和终点位置得到预设路径，以起点作为当前点；
[0049]
障碍物检测模块，如果当前点安全范围内不存在障碍物，则出发第一节点查找模块，否则出发第二节点查找模块；
[0050]
第一节点查找模块，在当前点的邻域内，基于a星算法查找下一节点；
[0051]
第一节点判断模块，将当前点更新为下一节点，如果当前节点为终点，则转至规划路径输出模块，否则返回障碍物检测模块；
[0052]
第二节点查找模块，在当前点的邻域内，基于优化a星算法查找下一节点；
[0053]
所述优化a星算法利用当前点与预设路径的距离优化最小估计代价，利用运动速度优化最小代价；
[0054]
第二节点判断模块，将当前点更新为下一节点，如果当前节点为终点，转至规划路径输出模块，否则转至预设路径匹配模块；
[0055]
预设路径匹配模块，如果当前点处于预设路径上，转至障碍物检测模块，否则转至第二节点查找模块；
[0056]
规划路径输出模块，停止寻找路径节点，将所有当前点依次连接，得到规划路径。
[0057]
本发明还提供了一种电子设备，包括处理器和存储器，所述存储器存储有能够被所述处理器执行的计算机程序指令，所述处理器执行所述计算机程序指令时，实现所述的路径规划方法。
[0058]
本发明还提供了一种计算机可读存储介质，存储有计算机程序指令，所述计算机程序指令在被处理器调用和执行时实现所述的路径规划方法。
[0059]
与现有技术相比，本发明具有如下的有益效果：
[0060]
先基于起点和终点位置设置预设路径，然后在前进时基于目标的安全范围和不同障碍物的安全范围进行避障和下一节点的寻找，在节点寻找中通过对a星算法的优化促使路径逐渐回归预设路径，同时为了应对二次事故的发生，在路径规划中考虑了目标的前进速度，从而进行更加合理的路径规划，使规划路径更接近于预设路径，降低算法计算量，提高运算速度，满足时效性要求。
附图说明
[0061]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
[0062]
图1是本发明实施例提供的路径规划方法的流程图；
[0063]
图2是本发明实施例提供的路径规划系统的示意图。
具体实施方式
[0064]
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术
人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进。这些都属于本发明的保护范围。
[0065]
本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”、“第三”、“第四”等(如果存在)是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例，例如能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
[0066]
下面以具体地实施例对本发明的技术方案进行详细说明。下面这几个具体的实施例可以相互结合，对于相同或相似的概念或过程可能在某些实施例不再赘述。
[0067]
实施例1
[0068]
如图1所示，本实施例提供了一种路径规划方法，其特征在于：包括，
[0069]
s1：对地图进行网格化处理，标记障碍物位置及障碍物的安全范围，确定起点和终点位置得到预设路径，以起点作为当前点；
[0070]
s2：如果当前点安全范围内不存在障碍物，则转至s3，否则转至s5；
[0071]
s3：在当前点的邻域内，基于a星算法查找下一节点；
[0072]
s4：将当前点更新为下一节点，如果当前节点为终点，则转至s8，否则返回s2；
[0073]
s5：在当前点的邻域内，基于优化a星算法查找下一节点；
[0074]
所述优化a星算法利用当前点与预设路径的距离优化最小估计代价，利用运动速度优化最小代价；
[0075]
s6：将当前点更新为下一节点，如果当前节点为终点，转至s8，否则转至s7；
[0076]
s7：如果当前点处于预设路径上，转至s2，否则转至s5；
[0077]
s8：停止寻找路径节点，将所有当前点依次连接，得到规划路径。
[0078]
本实施例先基于起点和终点位置设置预设路径，然后在前进时基于目标的安全范围和不同障碍物的安全范围进行避障和下一节点的寻找，在节点寻找中通过对a星算法的优化促使路径逐渐回归预设路径，同时为了应对二次事故的发生，在路径规划中考虑了目标的前进速度，从而进行更加合理的路径规划，使规划路径更接近于预设路径，降低算法计算量，提高运算速度，满足时效性要求。
[0079]
具体的，本实施例提供的路径规划方法包括，
[0080]
一种路径规划方法，其特征在于：包括，
[0081]
s1：对地图进行网格化处理，标记障碍物位置及障碍物的安全范围，确定起点和终点位置得到预设路径，以起点作为当前点；
[0082]
在地图中确定不同障碍物的类型，根据预设的规则或习惯为不同类型的障碍物定义安全范围，同时也需要为目标定义安全范围。在不考虑障碍物的情况下，预设路径应满足距离最短的要求，因此设置为从起点到终点的直线。
[0083]
s2：如果当前点安全范围内不存在障碍物，则转至s3，否则转至s5；
[0084]
根据目标和障碍物的安全范围确定是否存在交叉，从而确定当前点的安全范围内是否存在障碍物。
[0085]
s3：在当前点的邻域内，基于a星算法查找下一节点；具体方法为，
[0086]
分别计算当前点邻域内每个节点k的总代价值f(k)，
[0087]
f(k)＝g(k)+h((k)
[0088]
其中，g(k)表示节点k与起始点s(xs,ys)的最小代价值，h((k)表示节点k与终点d(xd,yd)的最小估计代价值；所述代价值为欧式距离，表示为，
[0089][0090]
当前点的邻接节点集合为，
[0091]
mov＝{mov1，mov2，
…
，movn，}
[0092]
下一节点为邻域内总代价值最小的节点，表示为，
[0093]
p
min
＝{mov|(fk
mov1
，fk
mov2
，
…
，fk
movn
)}
[0094]
其中，fk
movn
表示节点movn的总代价值，p
min
表示总代价值最小的节点。
[0095]
本实施例基于网络流模型进行邻域点的遍历，能够提高算法运行速度，快速找到下一节点。
[0096]
s4：将当前点更新为下一节点，如果当前节点为终点，则转至s8，否则返回s2；
[0097]
s5：在当前点的邻域内，基于优化a星算法查找下一节点；
[0098]
所述优化a星算法利用当前点与预设路径的距离优化最小估计代价，利用运动速度优化最小代价；
[0099]
所述利用当前点与预设路径的距离优化最小估计代价的方法为：
[0100]
优化后的总代价值表示为，
[0101][0102]
其中，j(k)为引入的优化代价值，当目标没有进入障碍区域或者处于障碍区域时，f(k)取g(k)+h((k)，当目标离开障碍区域时，f(k)取g(k)+h((k)+j(k)，
[0103]
优化代价值j(k)与当前点和预设路径的距离相关，表示为，
[0104][0105]
其中，t是当前点到预设路径的最小距离，l为地图中每格的长度；
[0106]
预设路径的方程为，
[0107][0108]
设当前点坐标为(xn,yn)，得到t的数值为，
[0109][0110]
则优化后的总代价值表示为，
[0111]
。
[0112][0113]
所述利用运动速度优化最小代价的方法为，
[0114]
在最小代价值中加入目标的运动速度，优化后的公式为，
[0115][0116]
其中，和β为常数，表示权重，dd，n表示当前点n到终点d的欧氏距离，v表示目标的速度。
[0117]
所述基于优化a星算法查找的下一节点为，
[0118]
p
min
＝{mov|(fk
mov1
，fk
mov2
，
…
，fk
movn
)}
[0119]
总代价值的计算公式为，
[0120]
。
[0121]
s6：将当前点更新为下一节点，如果当前节点为终点，转至s8，否则转至s7；
[0122]
s7：如果当前点处于预设路径上，转至s2，否则转至s5；
[0123]
s8：停止寻找路径节点，将所有当前点依次连接，得到规划路径。
[0124]
实施例2
[0125]
本实施例还提供了一种路径规划系统，包括，
[0126]
预处理模块，对地图进行网格化处理，标记障碍物位置及障碍物的安全范围，确定起点和终点位置得到预设路径，以起点作为当前点；
[0127]
障碍物检测模块，如果当前点安全范围内不存在障碍物，则出发第一节点查找模块，否则出发第二节点查找模块；
[0128]
第一节点查找模块，在当前点的邻域内，基于a星算法查找下一节点；
[0129]
第一节点判断模块，将当前点更新为下一节点，如果当前节点为终点，则转至规划路径输出模块，否则返回障碍物检测模块；
[0130]
第二节点查找模块，在当前点的邻域内，基于优化a星算法查找下一节点；
[0131]
所述优化a星算法利用当前点与预设路径的距离优化最小估计代价，利用运动速度优化最小代价；
[0132]
第二节点判断模块，将当前点更新为下一节点，如果当前节点为终点，转至规划路径输出模块，否则转至预设路径匹配模块；
[0133]
预设路径匹配模块，如果当前点处于预设路径上，转至障碍物检测模块，否则转至第二节点查找模块；
[0134]
规划路径输出模块，停止寻找路径节点，将所有当前点依次连接，得到规划路径。
[0135]
实施例3
[0136]
与实施例1相对应的本实施例还提供一种电子设备，包括处理器和存储器，所述存储器存储有能够被所述处理器执行的计算机程序指令，所述处理器执行所述计算机程序指
令时，实现如下方法：
[0137]
s1：对地图进行网格化处理，标记障碍物位置及障碍物的安全范围，确定起点和终点位置得到预设路径，以起点作为当前点；
[0138]
s2：如果当前点安全范围内不存在障碍物，则转至s3，否则转至s5；
[0139]
s3：在当前点的邻域内，基于a星算法查找下一节点；
[0140]
s4：将当前点更新为下一节点，如果当前节点为终点，则转至s8，否则返回s2；
[0141]
s5：在当前点的邻域内，基于优化a星算法查找下一节点；
[0142]
所述优化a星算法利用当前点与预设路径的距离优化最小估计代价，利用运动速度优化最小代价；
[0143]
s6：将当前点更新为下一节点，如果当前节点为终点，转至s8，否则转至s7；
[0144]
s7：如果当前点处于预设路径上，转至s2，否则转至s5；
[0145]
s8：停止寻找路径节点，将所有当前点依次连接，得到规划路径。
[0146]
实施例4
[0147]
与实施例1相对应的本实施例还提供一种计算机可读存储介质，存储有计算机程序指令，所述计算机程序指令在被处理器调用和执行时实现如下方法：
[0148]
s1：对地图进行网格化处理，标记障碍物位置及障碍物的安全范围，确定起点和终点位置得到预设路径，以起点作为当前点；
[0149]
s2：如果当前点安全范围内不存在障碍物，则转至s3，否则转至s5；
[0150]
s3：在当前点的邻域内，基于a星算法查找下一节点；
[0151]
s4：将当前点更新为下一节点，如果当前节点为终点，则转至s8，否则返回s2；
[0152]
s5：在当前点的邻域内，基于优化a星算法查找下一节点；
[0153]
所述优化a星算法利用当前点与预设路径的距离优化最小估计代价，利用运动速度优化最小代价；
[0154]
s6：将当前点更新为下一节点，如果当前节点为终点，转至s8，否则转至s7；
[0155]
s7：如果当前点处于预设路径上，转至s2，否则转至s5；
[0156]
s8：停止寻找路径节点，将所有当前点依次连接，得到规划路径。
[0157]
本领域内的技术人员应明白，本发明的实施例可提供为方法、设备(系统)或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。
[0158]
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。
[0159]
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变形或修改，这并不影
响本发明的实质内容。

技术特征：
1.一种路径规划方法，其特征在于，包括：s1：对地图进行网格化处理，标记障碍物位置及障碍物的安全范围，确定起点和终点位置得到预设路径，以起点作为当前点；s2：如果当前点安全范围内不存在障碍物，则转至s3，否则转至s5；s3：在当前点的邻域内，基于a星算法查找下一节点；s4：将当前点更新为下一节点，如果当前节点为终点，则转至s8，否则返回s2；s5：在当前点的邻域内，基于优化a星算法查找下一节点；所述优化a星算法利用当前点与预设路径的距离优化最小估计代价，利用运动速度优化最小代价；s6：将当前点更新为下一节点，如果当前节点为终点，转至s8，否则转至s7；s7：如果当前点处于预设路径上，转至s2，否则转至s5；s8：停止寻找路径节点，将所有当前点依次连接，得到规划路径。2.根据权利要求1所述的一种路径规划方法，其特征在于，所述预设路径为起点到终点的直线。3.根据权利要求2所述的一种路径规划方法，其特征在于，所述基于a星算法查找下一节点的方法为：分别计算当前点邻域内每个节点k的总代价值f(k)，f(k)＝g(k)+h(k)其中，g(k)表示节点k与起始点s(x
s
,y
s
)的最小代价值，h(k)表示节点k与终点d(x
d
,y
d
)的最小估计代价值；所述代价值为欧式距离，表示为，当前点的邻接节点集合为，mov＝{mov1，mov2，
···
，mov
n
，}下一节点为邻域内总代价值最小的节点，表示为，p
min
＝{mov|(fk
mov1
，fk
mov2
，
···
，fk
movn
)}其中，fk
movn
表示节点mov
n
的总代价值，p
min
表示总代价值最小的节点。4.根据权利要求3所述的一种路径规划方法，其特征在于，所述利用当前点与预设路径的距离优化最小估计代价的方法为：优化后的总代价值表示为，其中，j(k)为引入的优化代价值，当目标没有进入障碍区域或者处于障碍区域时，f(k)取g(k)+h(k)，当目标离开障碍区域时，f(k)取g(k)+h(k)+j(k)，优化代价值j(k)与当前点和预设路径的距离相关，表示为，其中，t是当前点到预设路径的最小距离，l为地图中每格的长度；预设路径的方程为，
设当前点坐标为(x
n
,y
n
)，得到t的数值为，则优化后的总代价值表示为，5.根据权利要求4所述的一种路径规划方法，其特征在于，所述利用运动速度优化最小代价的方法为，在最小代价值中加入目标的运动速度，优化后的公式为，其中，和β为常数，表示权重，d
d，n
表示当前点n到终点d的欧氏距离，v表示目标的速度。6.根据权利要求5所述的一种路径规划方法，其特征在于，β＝0.8。7.根据权利要求6所述的一种路径规划方法，其特征在于，所述基于优化a星算法查找的下一节点为，p
min
＝{mov|(fk
mov1
，fk
mov2
，
···
，fk
movn
)}总代价值的计算公式为，。8.一种路径规划系统，其特征在于，包括：预处理模块，对地图进行网格化处理，标记障碍物位置及障碍物的安全范围，确定起点和终点位置得到预设路径，以起点作为当前点；障碍物检测模块，如果当前点安全范围内不存在障碍物，则出发第一节点查找模块，否则出发第二节点查找模块；第一节点查找模块，在当前点的邻域内，基于a星算法查找下一节点；第一节点判断模块，将当前点更新为下一节点，如果当前节点为终点，则转至规划路径输出模块，否则返回障碍物检测模块；第二节点查找模块，在当前点的邻域内，基于优化a星算法查找下一节点；所述优化a星算法利用当前点与预设路径的距离优化最小估计代价，利用运动速度优化最小代价；第二节点判断模块，将当前点更新为下一节点，如果当前节点为终点，转至规划路径输出模块，否则转至预设路径匹配模块；预设路径匹配模块，如果当前点处于预设路径上，转至障碍物检测模块，否则转至第二节点查找模块；
规划路径输出模块，停止寻找路径节点，将所有当前点依次连接，得到规划路径。9.一种电子设备，其特征在于，包括处理器和存储器，所述存储器存储有能够被所述处理器执行的计算机程序指令，所述处理器执行所述计算机程序指令时，实现权利要求1-7任一项所述的方法。10.一种计算机可读存储介质，其特征在于，存储有计算机程序指令，所述计算机程序指令在被处理器调用和执行时实现权利要求1-7任一项所述的方法。

技术总结
本发明涉及一种路径规划方法、系统、电子设备及存储介质，通过对当前点的范围内判断是否存在障碍物，并分别使用现有的A星算法和优化的A星算法进行下一点的查找，直到运动到终点位置，完成路径规划。在节点寻找中通过对A星算法的优化促使路径逐渐回归预设路径，同时为了应对二次事故的发生，在路径规划中考虑了目标的前进速度，从而进行更加合理的路径规划，使规划路径更接近于预设路径，降低算法计算量，提高运算速度，满足时效性要求。满足时效性要求。满足时效性要求。


技术研发人员：曹开田 高莘尧 姜梦彦
受保护的技术使用者：上海应用技术大学
技术研发日：2021.11.11
技术公布日：2022/3/8