一种动态场景下的跳点搜索路径规划方法

1.本发明涉及人工智能与无人系统领域，具体涉及自动驾驶路径规划技术领域，尤其涉及一种动态场景下的跳点搜索路径规划方法。


背景技术：

2.在无人系统领域无人车的自动驾驶中，路径规划是关键的技术之一，针对这一技术，截止至今已经开展了大量的研究工作。无人车路径规划可以分为静态路径规划和动态路径规划。其中a*算法、dijkstra算法等目前主要应用于已知的静态场景中，而人工势场法、d*算法等则大多应用于环境未知动态场景中。其中跳点搜索(jumppointsearch,jps)是对a*搜索算法的优化，通过剪枝规则，打破栅格的对称性，减少无效节点的访问，保留关键“跳点”，能让搜索在栅格上直线长“跳跃”，具有较好的路径寻优性能。但是，跳点搜索算法依然只能处理静态路径规划问题。对于复杂化且实时更新的动态栅格地图，若仍想利用在静态场景下效率更高的跳点搜索算法进行寻路工作，则需反复不停的调用静态的跳点搜索算法，会在节点搜索和跳点识别阶段会消耗大量时间，且每次寻路后的计算结果不会被保存，在下一次刷新地图后仍需重新进行起点到终点的探索，所耗时间过长，无法处理无人车的动态避障问题，需做大量优化处理。
3.针对上述问题，文献《面向未知环境的机器人动态路径规划算法研究》提出面对动态地图的跳点搜索算法，存在如下问题：
4.(1)该文章中，仅将障碍物可能出现的情况分为“直线型障碍物”以及“凹形障碍物”，并以此设计出两种应对策略，策略分别为“指定动态跳点策略”和“重搜索策略”。但实际的复杂场景中，障碍物的变化情况复杂多样，无法仅用这两种“障碍物模型”来穷举组合出所有更新障碍物对已得路径造成的影响，这无疑是简化了动态地图中各种复杂的障碍物变化情况；并且，在原始的静态场景下，跳点搜索算法便已经可解决一切基于栅格点的避障寻路问题，应对动态场景时，无需再根据自己所设定的“障碍物模型”重新设计一套寻路方案，只需沿用原算法中的跳点搜索逻辑，做距离更短的jps寻路即可。
5.(2)该文章中提到的“指定动态跳点策略”将其分为了“双临近非斜向”，“单临近斜向”以及“运动堵塞”三种情况。其提到的前两种情况，只是原始 jps探索的其中部分过程：将当前节点相邻处的非障碍物节点加入到邻居节点中，以邻居节点所在方向，扫描直线段上的跳点位置，并将搜索到的跳点加入到openlist中，全部搜索完毕以后，在openlist选择估计代价值最小的跳点，加入closelist中，并以当前节点继续向目标点发起下一步迭代寻路搜索。该文章中所提方案仅为当前所述jps算法部分步骤的简化，并且无法应对可能出现的所有情况，只能解决这种“直线型障碍物”出现在原始路径中的情况。
6.(3)针对该文章中所述的第三种“运动阻塞”情况，该文章所述方案中，在探索到“直线型障碍物”阻塞了前方路径，就退出运算并放弃寻路。但实际的场景中，到达目标点的路径可能不止一条。同时跳点搜索算法为全局寻路算法，不应仅因为扫描到机器人前方出现局部阻断路径的“直线型障碍物”，便放弃全局寻路探索；
7.(4)该文章中提出的“重搜索策略”，会以当前机器人的运动位置，向终点处重新进行jps探索，这代表完全放弃了上次寻路探索中所得路径，重新进行了大量无用节点的扫描，大幅度降低了算法面对复杂场景时的运算效率；
8.(5)该文章提出的障碍物检测策略，是根据机器人所在位置处判断是否在路径中出现了新的障碍物。只有当机器人前方检测到了障碍物时，才调用对应处理逻辑修正运动路径。此种障碍物检测方法的弊端在于，该方法只能对距离机器人位置很近处的障碍物进行检测。如果在接下来的机器人要走的路径中，后端路径发生大幅度变动，导致本应重新修正机器人移动路线时，却未能及时做出决策，这便会导致机器人不停的在走“冤枉路”，不停的陷入“局部最优”。并且这种探测方式，未利用到栅格法探测时可以快速探测大量节点的优势，探测节点仅限于机器人位置附近。运行效率同样不及人工势场法，无法处理快速复杂场景下的动态地图变化。


技术实现要素：

9.本发明提供一种动态场景下的跳点搜索路径规划方法，考虑到地图的实时更新，针对不同障碍物更新情况，设计一套完整的跳点与路径的更新规则。首先检测跳点是否被更新为障碍物，如果检测到障碍物变化，调用“跳点-障碍物处理函数”，接着检测直线段路径是否更新出障碍物，如果检测到障碍物出现，则调用“直线路径-障碍物处理函数”，最后检测是否由于障碍物的更新出现了新的“近路”，同时对于路径进行优化处理。设计改进的动态跳点搜索算法实现了动态场景下的快速寻路。在检测障碍物变化的时候，不需要再进行全地图的地形变化探索，只对路径段及其附近节点的地形变化进行检索，减少了无效探索时间。同时，利用原算法中未寻路结束的“最优的废弃路径”，限制探索寻路的长度，保证路径的最优性，降低了算法的时间复杂度和空间复杂度。
10.为达到上述目的，本发明提供一种动态场景下的跳点搜索路径规划方法，包括以下步骤：
11.s1、采用栅格法将地图划分区域，扫描障碍物所在位置，将对应栅格标记为不可行走区域；
12.s2、运行静态jps算法，完成初次寻路探索；
13.s3、重新扫描地图中障碍物位置变化，更新对应栅格中的障碍物位置信息；
14.s4、调用动态jps算法，对更新后的地图进行路径更新，扫描在原始路径上的障碍物变化，判断是否在原始路径上出现新的障碍物，以及扫描路径附近的邻接点，判断是否出现了新的“近路”并实现路径优化过程。
15.所述的s2中，使用跳点搜索算法进行初次寻路工作：
16.将每个节点的对应邻居节点所在方向处的邻居节点加入邻居数组中，分为斜向，横向和纵向三种情况进行讨论，依据邻居节点识别规则，裁剪掉冗余的邻居节点；
17.依据跳点搜索算法的路径探索规则，循环处理所有邻居节点。同时依据剪枝规则，去掉冗余的对称节点，确定强制邻节点和跳点。沿每个邻居节点方向上探索有没有跳点存在，如果是横向或纵向探索，则沿原路径递归调用此函数迭代搜索；如果是斜向探索，则先进行横向或纵向的迭代探索，在每个节点的横纵两个方向都探索完毕以后，再沿着斜向继续迭代探索下一个节点；
18.剪枝规则：
19.在栅格地图上进行探索分为水平方向，竖直方向和斜向，对于水平和竖直方向，裁剪掉所有符合以下条件的节点n：
20.len(＜p(a),
…
,n＞\a)≤len(＜p(a),a,n＞)
21.其中：len(＜p(a),
…
,n＞\a)为p(a)节点不经过a节点到达n节点的最短路径， len(＜p(a),a,n＞)为p(a)节点经过a节点到达n节点的最短路径，a表示a节点，n表示n节点，p(a)表示节点a的父节点；
22.对于斜向，裁剪掉所有符合以下条件的节点n：
23.len(＜p(a),
…
,n＞\a)＜len(＜p(a),a,n＞)
24.对于强制邻节点，根据以下规则进行筛选：
25.len(＜p(a),a,n＞)＜len(＜p(a),
…
,n＞\a)
26.跳点识别规则可表示为：b＝a kd，从a节点出发，通过在d方向移动k 步到达b节点，其中拥有最小k的节点b称为a的跳点，且满足如下条件之一：
27.(1)节点b为目标点；
28.(2)节点b含有至少一个强制邻节点；
29.(3)若d为对角线移动，存在c＝b k
idi
，其中ki∈n，c是b的跳点，则b 也是a的跳点。
30.在全部邻居节点方向探索完毕以后，对跳点进行处理。记录一下当前节点与该跳点的距离，并根据启发式估价函数，计算出当前节点的步长gi(n)，曼哈顿距离hi(n)和代价值f(n)。
31.启发式估价函数为：
32.f(n)＝gi(n) hi(n) cross
×
0.001
33.其中：f(n)表示正向搜索从起始节点到当前节点到目标节点的代价函数， gi(n)表示起始节点到当前节点的步长，即当前路径中到当前节点的累计实际代价，hi(n)表示的是当前节点到目标节点的曼哈顿距离，以此作为当前节点到终点的估计代价，cross表示原起点指向原终点的向量与当前节点指向原终点向量的内积，向量下标s表示起始节点，c表示当前节点，e表示目标节点，且采用欧式距离作距离表达。
34.所述的s4中，动态跳点搜索具体包括以下子步骤：
35.s41、从终点开始对pathlist中所有跳点进行循环探测，判断当前跳点是否更新为障碍物，如果检测到其被更新为障碍物，则调用“跳点-障碍物”处理函数对当前节点进行处理。循环检测直到检测完起点为止；
[0036]“跳点-障碍物”处理函数：
[0037]
(1)沿当前节点到其子节点方向，对于两者之间的中间节点依次发起探索，搜索不为障碍物的节点。将被探索到的首个不为障碍物的节点，更新为跳点，并标记名为“新起点”，将当前节点的父节点的子节点更换为被探索的节点；
[0038]
(2)沿当前节点到其子节点方向，对于两者之间的中间节点依次发起探索，搜索不为障碍物的节点。将被探索到的首个不为障碍物的节点处，更新为跳点，并标记名为“新终点”，将当前节点的子节点的父节点更换为被探索的节点；
[0039]
(3)以“新起点”为起点，向“新终点”发起探索，进行单层循环的限制长度的jps探
索：
[0040]
①
令当前“新起点”位置处的节点，调用步骤s21至s23；
[0041]
②
调用s24至s26中程序，其中在当前步骤下，将openlist和closelist替换为new_openlist和new_closelist，与第一次寻路探索中得到的跳点序列加以区分。同时，因为最终要将得到的路径拼接在原始路径上，这里的步长gi(n)计算的是新探索到的跳点与“新终点”之间的实际累计代价值与原路径的总长度之和，估计代价hi(n)是该跳点与“新终点”之间的曼哈顿距离，而代价值 f(n)为gi(n)与hi(n)的总和。
[0042]
(4)依次遍历在首次jps寻路时得到的openlist中的节点，找到openlist 中代价值f(n)最小的节点，其父节点必然已存于closelist，并且沿着父节点方向进行探索，必然能搜索到首次寻路时的起点。而将其连起来所得路径，即为除了得到的最优路径以外，首次探索时没有完成寻路工作的最优的“废弃路径”；
[0043]
(5)令当前openlist中的节点，递归调用步骤s41至s42中的程序，以检验“最优的废弃路径中”有无障碍物更新的情况。如果检测到了障碍物更新，则会优先处理“最优的废弃路径”中的路径更新；如果未检测到障碍物更新，则跳转至步骤(6)；
[0044]
(6)比较步骤(3)中选中的new_openlist中的跳点的代价值f(n)，以及步骤(4)中选中的openlist中的跳点中的代价值f(n)，选择代价值更小的一方，继续执行下一轮的“单层循环的限制长度的jps探索”。循环执行步骤(3)至(6)，每次都选择代价值更小的一方来执行下一轮的“单层循环的限制长度的jps探索”，直到一方先找到终点为止；如果new_openlist中的跳点先找到“新终点”，则跳转至步骤(7)；如果openlist中的跳点先找到终点，则跳转至步骤(8)；如果new_openlist中的跳点先一步搜索完毕，但没有搜索到终点，则跳转到步骤(9)；
[0045]
(7)将“新起点”与“新终点”中的路径，拼接在原始路径上，得到一条新路径，重新计算新路径中每个跳点的代价值f(n)，以便下一轮更新地图障碍物信息时使用：
[0046]
①
遍历删除原始路径中的pathlist中的节点，重新将新路径中的所有跳点，存入双向链表pathlist中；
[0047]
②
将“新终点”中存入的代价值f(n)赋值给原来的终点；
[0048]
③
沿着终点向其父节点方向进行反向探索，令终点的代价值f(n)减去父节点的估计代价值hi(n)，得到的即为父节点刷新后的代价值f(n)。以此类推，沿着父节点方向进行更新，直到最终更新完起点中的代价值信息为止。
[0049]
(8)更新后的新路径为上次探索时得到的“最优的废弃路径”，依然需要对“新终点”和“新起点”中未探索完的节点进行处理，以便下一轮更新地图障碍物信息时使用：
[0050]
①
遍历删除原始路径中的pathlist中的节点，重新将新路径中的所有跳点，存入双向链表pathlist中；
[0051]
②
将“新终点”中存入的代价值f(n)赋值给原来的终点；
[0052]
③
沿着终点向其父节点方向进行反向探索，令终点的代价值f(n)减去父节点的估计代价值hi(n)，得到的即为父节点刷新后的代价值f(n)。此步骤执行到“新终点”位置停止。在下一轮更新障碍物以后，再次调用动态jps算法时，此条路径即为“上一轮探索时最优的废弃路径”，会以当前的路径断开位置，令保存在new_openlist中的节点继续朝“新终点”方向进行探索。重复执行上述步骤。
[0053]
(9)如果出现new_openlist中的跳点先一步搜索完毕，但没有搜索到终点的此种情况，是因为“新起点”被更新后的障碍物围住，与“新终点”不在一个连通区间，因此导致的寻路失败。为应对这种情况，以当前“新起点”的父节点为新的“新起点”，重复调用步骤s4中的程序。如若以更新后的“新起点”依然搜索不到终点，则继续执行步骤(9)：以当前的父节点为新的“新起点”，直到“新起点”和起点两个节点重合时，如果依然搜索不到，则证明路径中的起点与路径中的终点也不在一个连同区间，则宣告寻路失败。
[0054]
s42、从终点开始依次向当前节点的父节点方向进行循环探测，检查原路径中直线部分是否有障碍物更新，如果检测到其被更新为障碍物，则调用“直线路径-障碍物”处理函数，对当前节点进行处理。循环检测直到起点为止；
[0055]
直线路径-障碍物处理函数：
[0056]
(1)对当前节点的父节点方向进行探测，当探测到障碍物时，将当前节点的前一个节点更新为跳点，此时可以保证，终点至当前插入的跳点部分的路径处于同一连通区域的。将当前节点的父节点设置为“新起点”，将插入的跳点设置为“新终点”，将插入的跳点设置为当前节点的子节点的新的父节点；
[0057]
(2)运行步骤s41-(3)至s41-(9)中的程序。
[0058]
s43、检测是否在障碍物更新以后出现了新的“近路”，同时对路径进行优化处理。从起点开始，对路径上所有节点，都执行以下步骤：
[0059]
(1)对于当前节点，对除了与其子节点的连接方向以及与其父节点的连接方向以外的其他六个方向的进行直线方向的探索，依次对该方向上每个相邻节点进行迭代探测；如果检测到了障碍物，或者探索到了地图边界，则跳转至步骤(2)；如果检测到了其他路径节点，则跳转至步骤(3)；
[0060]
(2)跳出本次循环，终止该方向的探索，继续对下个方向进行探测；直至该节点的所有方向全部探索完毕，则跳转至步骤(4)；
[0061]
(3)如果在当前节点的当前方向上检测到了其他路径节点，根据两点之间直线段最短的原理，可知当前找到了一条新的“近路”，则对当前的路径做优化处理。执行以下步骤：
[0062]
①
将当前节点以及当前方向上被检测到的路径节点都更新为跳点；
[0063]
②
将被检测到的路径节点的父节点设置为当前节点，将被检测到的路径节点子节点设置为原路径上的下一个跳点，将当前节点前面的一个跳点设置为当前节点的父节点；
[0064]
③
计算在原路径上当前节点与被检测到的路径节点之间的路径距离，计算当前节点与被检测到的路径节点的直线距离，二者相减，将相减得到的结果保存为变量。令被检测到的路径节点，在路径上的后续所有跳点的代价值f(n)都减去此变量值，以此更新路径长度；
[0065]
④
以更新后的新路径的下一个节点，继续向下探测，执行步骤(4)。
[0066]
(4)以当前节点开始，循环重复执行步骤s43，直到探索到终点为止；如果探索到终点，则跳转至步骤(5)；
[0067]
(5)将更新后的新路径中的跳点替换掉原来pathlist中的节点。
[0068]
本发明具有如下有益效果：
[0069]
1.通过对原路径中的跳点以及直线路径进行障碍物检测，再进行“近路探索”以及
路径优化工作，不是仅用简单的“障碍物模型”来讨论复杂地图中的各种情况，而是设计出了一套完整的跳点-路径更新规则，提供了一种将跳点搜索算法高效应用在动态场景下的设计方案。本发明中，只需执行步骤s41-(1)至 s41-(3)，在对应位置插入跳点，进行短距离的jps寻路探索，便已经可以应对“直线型障碍物”，“凹形障碍物”以及其他更多的复杂障碍物变化的情况。
[0070]
2.本发明中，会对首次探测时所得的原始路径段以及附近节点进行障碍物检测。既不会像原始的jps算法一样不停的对整个地图中，各个方向的节点都进行障碍物的更新探测，也不会仅对机器人所在位置附近的节点进行障碍物的探测。既能避免机器人不停的陷入此时的“局部最优”，又能减少了无效的节点探索时间，使其可以应用于快速变化的复杂场景下；
[0071]
3.利用原算法中未寻路结束的“最优的废弃路径”，将该路径与由于障碍物出现，导致需要进行更新的原路径进行比较，双方通过比较估计代价值的大小，交替迭代探索，直到一方先探索到目标点为止。限制探索寻路的长度，既保证路径的最优性，又大幅度降低了算法的时间复杂度和空间复杂度。
附图说明
[0072]
通过阅读参照以下附图所作的对非限制性实施例所作的详细描述，本专利的其它特征、目的和优点将会变得更明显：
[0073]
图1动态场景下的跳点搜索路径规划算法的流程框图；
[0074]
图2为跳点被更新为障碍物的一种情况的示意图；
[0075]
图3为跳点附近出现大范围障碍物更新的情况的示意图；
[0076]
图4为直线路径段出现新的障碍物的一种情况的示意图；
[0077]
图5为直线路径段出现障碍物但父节点被障碍物围住，继续利用其他节点进行寻路的示意图；
[0078]
图6为路径附近由于障碍物的消失出现新的“近路”的一种情况的示意图；
[0079]
图7为复杂场景下的地图的原始jps的探索过程的示意图；
[0080]
图8为复杂场景下的地图更新后的动态jps路径更新过程的示意图。
具体实施方式
[0081]
为了更加清晰地解释本发明的运作方式以及优势所在，以下结合仿真图，对不同场景下的运行效果进行分析，还原算法的运算逻辑，对本发明进一步详细说明。
[0082]
本发明提供了一种动态场景下的跳点搜索路径规划方法，具体包括以下步骤：
[0083]
s1、采用栅格法将地图划分区域，扫描障碍物所在位置，将对应栅格标记为不可行走区域；
[0084]
s2、运行静态jps算法，完成初次寻路探索，具体包括以下子步骤：
[0085]
s21、将起点压入到openlist中；
[0086]
s22、将当前节点的对应邻居节点所在方向处的邻居节点加入邻居数组中，如果当前节点是起点，就对周围的八个方向都进行判断，如果当前节点不是起点，就根据其父节点到当前节点的方向，分为斜向，横向和纵向三种情况进行讨论，依据邻居节点识别规则，裁
剪掉冗余的邻居节点，只将需要的邻居节点放入到邻居数组中；
[0087]
s23、依据跳点搜索算法的路径探索规则，循环处理所有邻居节点。同时依据剪枝规则，去掉冗余的对称节点，确定强制邻节点和跳点。沿每个邻居节点方向上探索有没有跳点存在，如果是横向或纵向探索，则沿原路径递归调用此函数迭代搜索；如果是斜向探索，则先进行横向或纵向的迭代探索，在每个节点的横纵两个方向都探索完毕以后，再沿着斜向继续迭代探索下一个节点；
[0088]
剪枝规则：
[0089]
在栅格地图上进行探索分为水平方向，竖直方向和斜向，对于水平和竖直方向，裁剪掉所有符合以下条件的节点n：
[0090]
len(＜p(a),
…
,n＞\a)≤len(＜p(a),a,n＞)
[0091]
其中：len(＜p(a),
…
,n＞\a)为p(a)节点不经过a节点到达n节点的最短路径， len(＜p(a),a,n＞)为p(a)节点经过a节点到达n节点的最短路径，a表示a节点，n表示n节点，p(a)表示节点a的父节点；
[0092]
对于斜向，裁剪掉所有符合以下条件的节点n：
[0093]
len(＜p(a),
…
,n＞\a)＜len(＜p(a),a,n＞)
[0094]
对于强制邻节点，根据以下规则进行筛选：
[0095]
len(＜p(a),a,n＞)＜len(＜p(a),
…
,n＞\a)
[0096]
跳点识别规则可表示为：b＝a kd，从a节点出发，通过在d方向移动k 步到达b节点，其中拥有最小k的节点b称为a的跳点，且满足如下条件之一：
[0097]
(1)节点b为目标点；
[0098]
(2)节点b含有至少一个强制邻节点；
[0099]
(3)若d为对角线移动，存在c＝b k
idi
，其中ki∈n，c是b的跳点，则b 也是a的跳点。
[0100]
s24、在全部邻居节点方向探索完毕以后，对跳点进行处理。记录一下当前节点与该跳点的距离，并根据启发式估价函数，计算出当前节点的步长gi(n)，曼哈顿距离hi(n)和代价值f(n)。判断当前搜索到的跳点是否在closelist中，如果当前跳点已经被加入到closelist里了，则不对当前跳点做任何处理；判断当前搜索到的跳点是否在openlist中，如果当前跳点不在openlist中，则将此跳点压入到openlist中，并将这个跳点的父节点标记为当前节点；如果在，则证明当前跳点曾经被探索过，比较后一次来此节点的父节点的代价值是否比前一次来此节点的父节点的代价值小，如果后一次来此节点的父节点其代价值更小，则将后一次来此节点的父节点刷新为当前节点的父节点，并刷新当前节点的代价值；如果前一次来此节点的父节点的代价值更小，则不对当前节点做任何处理；
[0101]
启发式估价函数为：
[0102]
f(n)＝gi(n) hi(n) cross
×
0.001
[0103]
其中：f(n)表示正向搜索从起始节点到当前节点到目标节点的代价函数， gi(n)表示起始节点到当前节点的步长，即当前路径中到当前节点的累计实际代价，hi(n)表示的是当前节点到目标节点的曼哈顿距离，以此作为当前节点到终点的估计代价，cross表示原起点指向原终点的向量与当前节点指向原终点向量的内积，向量下标s表示起始节点，c表示当前节点，e表示目标节点，且采用欧式距离作距离表达。
[0104]
s25、处理完本轮搜索时搜索到的所有跳点以后，判断此时openlist中是否还有节点，如果没有，则证明寻路失败；
[0105]
s26、弹出openlist中代价值最小的一个节点，将其加入到closelist中；
[0106]
s27、如果弹出的节点是终点，则沿着原路径反向循迹，并将路径上的全部跳点加入到双向链表pathlist中；如果弹出的节点不是终点，则以弹出的节点为目标节点跳转回s22迭代操作上述步骤，直到找到终点得到路径，宣告寻路结束，或者直到清空openlist中全部节点，宣告寻路失败为止。
[0107]
s3、重新扫描地图中障碍物位置变化，更新对应栅格中的障碍物位置信息；
[0108]
s4、调用动态jps算法，对更新后的地图进行路径更新，扫描在原始路径上的障碍物变化，判断是否在原始路径上出现新的障碍物，以及扫描路径附近的邻接点，判断是否出现了新的“近路”并实现路径优化过程。动态jps算法具体包括以下步骤：
[0109]
s41、从终点开始对pathlist中所有跳点进行循环探测，判断当前跳点是否更新为障碍物，如果检测到其被更新为障碍物，则调用“跳点-障碍物”处理函数对当前节点进行处理。循环检测直到检测完起点为止；
[0110]“跳点-障碍物”处理函数：
[0111]
(1)沿当前节点到其子节点方向，对于两者之间的中间节点依次发起探索，搜索不为障碍物的节点。将被探索到的首个不为障碍物的节点，更新为跳点，并标记名为“新起点”，将当前节点的父节点的子节点更换为被探索的节点；
[0112]
(2)沿当前节点到其子节点方向，对于两者之间的中间节点依次发起探索，搜索不为障碍物的节点。将被探索到的首个不为障碍物的节点处，更新为跳点，并标记名为“新终点”，将当前节点的子节点的父节点更换为被探索的节点；
[0113]
(3)以“新起点”为起点，向“新终点”发起探索，进行单层循环的限制长度的jps探索：
[0114]
①
令当前“新起点”位置处的节点，调用步骤s21至s23；
[0115]
②
调用s24至s26中程序，其中在当前步骤下，将openlist和closelist替换为new_openlist和new_closelist，与第一次寻路探索中得到的跳点序列加以区分。同时，因为最终要将得到的路径拼接在原始路径上，这里的步长gi(n)计算的是新探索到的跳点与“新终点”之间的实际累计代价值与原路径的总长度之和，估计代价hi(n)是该跳点与“新终点”之间的曼哈顿距离，而代价值 f(n)为gi(n)与hi(n)的总和。
[0116]
(4)依次遍历在首次jps寻路时得到的openlist中的节点，找到openlist 中代价值f(n)最小的节点，其父节点必然已存于closelist，并且沿着父节点方向进行探索，必然能搜索到首次寻路时的起点。而将其连起来所得路径，即为除了得到的最优路径以外，首次探索时没有完成寻路工作的最优的“废弃路径”；
[0117]
(5)令当前openlist中的节点，递归调用步骤s41至s42中的程序，以检验“最优的废弃路径中”有无障碍物更新的情况。如果检测到了障碍物更新，则会优先处理“最优的废弃路径”中的路径更新；如果未检测到障碍物更新，则跳转至步骤(6)；
[0118]
(6)比较步骤(3)中选中的new_openlist中的跳点的代价值f(n)，以及步骤(4)中选中的openlist中的跳点中的代价值f(n)，选择代价值更小的一方，继续执行下一轮的“单层循环的限制长度的jps探索”。循环执行步骤(3)至(6)，每次都选择代价值更小的一方来
执行下一轮的“单层循环的限制长度的jps探索”，直到一方先找到终点为止；如果new_openlist中的跳点先找到“新终点”，则跳转至步骤(7)；如果openlist中的跳点先找到终点，则跳转至步骤(8)；如果new_openlist中的跳点先一步搜索完毕，但没有搜索到终点，则跳转到步骤(9)；
[0119]
(7)将“新起点”与“新终点”中的路径，拼接在原始路径上，得到一条新路径，重新计算新路径中每个跳点的代价值f(n)，以便下一轮更新地图障碍物信息时使用：
[0120]
①
遍历删除原始路径中的pathlist中的节点，重新将新路径中的所有跳点，存入双向链表pathlist中；
[0121]
②
将“新终点”中存入的代价值f(n)赋值给原来的终点；
[0122]
③
沿着终点向其父节点方向进行反向探索，令终点的代价值f(n)减去父节点的估计代价值hi(n)，得到的即为父节点刷新后的代价值f(n)。以此类推，沿着父节点方向进行更新，直到最终更新完起点中的代价值信息为止。
[0123]
(8)更新后的新路径为上次探索时得到的“最优的废弃路径”，依然需要对“新终点”和“新起点”中未探索完的节点进行处理，以便下一轮更新地图障碍物信息时使用：
[0124]
①
遍历删除原始路径中的pathlist中的节点，重新将新路径中的所有跳点，存入双向链表pathlist中；
[0125]
②
将“新终点”中存入的代价值f(n)赋值给原来的终点；
[0126]
③
沿着终点向其父节点方向进行反向探索，令终点的代价值f(n)减去父节点的估计代价值hi(n)，得到的即为父节点刷新后的代价值f(n)。此步骤执行到“新终点”位置停止。在下一轮更新障碍物以后，再次调用动态jps算法时，此条路径即为“上一轮探索时最优的废弃路径”，会以当前的路径断开位置，令保存在new_openlist中的节点继续朝“新终点”方向进行探索。重复执行上述步骤。
[0127]
(9)如果出现new_openlist中的跳点先一步搜索完毕，但没有搜索到终点的此种情况，是因为“新起点”被更新后的障碍物围住，与“新终点”不在一个连通区间，因此导致的寻路失败。为应对这种情况，以当前“新起点”的父节点为新的“新起点”，重复调用步骤s4中的程序。如若以更新后的“新起点”依然搜索不到终点，则继续执行步骤(9)：以当前的父节点为新的“新起点”，直到“新起点”和起点两个节点重合时，如果依然搜索不到，则证明路径中的起点与路径中的终点也不在一个连同区间，则宣告寻路失败。
[0128]
s42、从终点开始依次向当前节点的父节点方向进行循环探测，检查原路径中直线部分是否有障碍物更新，如果检测到其被更新为障碍物，则调用“直线路径-障碍物”处理函数，对当前节点进行处理。循环检测直到起点为止；
[0129]
直线路径-障碍物处理函数：
[0130]
(1)对当前节点的父节点方向进行探测，当探测到障碍物时，将当前节点的前一个节点更新为跳点，此时可以保证，终点至当前插入的跳点部分的路径处于同一连通区域的。将当前节点的父节点设置为“新起点”，将插入的跳点设置为“新终点”，将插入的跳点设置为当前节点的子节点的新的父节点；
[0131]
(2)运行步骤s41-(3)至s41-(9)中的程序。
[0132]
s43、检测是否在障碍物更新以后出现了新的“近路”，同时对路径进行优化处理。从起点开始，对路径上所有节点，都执行以下步骤：
[0133]
(1)对于当前节点，对除了与其子节点的连接方向以及与其父节点的连接方向以外的其他六个方向的进行直线方向的探索，依次对该方向上每个相邻节点进行迭代探测；如果检测到了障碍物，或者探索到了地图边界，则跳转至步骤(2)；如果检测到了其他路径节点，则跳转至步骤(3)；
[0134]
(2)跳出本次循环，终止该方向的探索，继续对下个方向进行探测；直至该节点的所有方向全部探索完毕，则跳转至步骤(4)；
[0135]
(3)如果在当前节点的当前方向上检测到了其他路径节点，根据两点之间直线段最短的原理，可知当前找到了一条新的“近路”，则对当前的路径做优化处理。执行以下步骤：
[0136]
①
将当前节点以及当前方向上被检测到的路径节点都更新为跳点；
[0137]
②
将被检测到的路径节点的父节点设置为当前节点，将被检测到的路径节点子节点设置为原路径上的下一个跳点，将当前节点前面的一个跳点设置为当前节点的父节点；
[0138]
③
计算在原路径上当前节点与被检测到的路径节点之间的路径距离，计算当前节点与被检测到的路径节点的直线距离，二者相减，将相减得到的结果保存为变量。令被检测到的路径节点，在路径上的后续所有跳点的代价值f(n)都减去此变量值，以此更新路径长度；
[0139]
④
以更新后的新路径的下一个节点，继续向下探测，执行步骤(4)。
[0140]
(4)以当前节点开始，循环重复执行步骤s43，直到探索到终点为止；如果探索到终点，则跳转至步骤(5)；
[0141]
(5)将更新后的新路径中的跳点替换掉原来pathlist中的节点。
[0142]
结合具体实施例，对动态跳点搜索算法作进一步说明，以下将结合几种具体场景进行分析。附图中，采用字母a～t和数字1～16分别对栅格地图边界的横向和纵向单元格作标记，黑色单元格为障碍物栅格，
△
型符号节点为openlist或new_openlist中的跳点，
×
型符号节点为加入到closelist或 new_closelist中的跳点，
○
型符号节点为加入到pathlist中的跳点，其连线即为最终寻路所得路径。
[0143]
图2为跳点被更新为障碍物的一种简单情况，其中图2-(a)为步骤s2中首次调用jps寻路后所得路径。图2-(b)中，反向探测时检测到坐标(j,10)被更新为障碍物。图2-(c)执行步骤s41-(1)至s41-(2)，在障碍物其朝向父节点和子节点的方向上插入跳点。图2-(d)执行步骤s41-(3)至s41-(6)，在插入的两个跳点位置处进行限制长度的单步jps运算，图2-(e)执行步骤s41-(7)，将寻得的路径拼接在原始路径上，得到一条新路径。
[0144]
图3为跳点被更新为障碍物的另外一种情况演示，障碍物更新时，可能不只针对单一节点。如图3-(b)，在原跳点的位置(n,7)以及其相邻节点处都被更新为障碍物。图3-(a)为步骤s2中首次调用jps寻路后的路径。图3-(b)中执行地图更新过程，且执行步骤s41-(1)至s41-(2)。图3-(c)执行步骤s41-(3)至s41
‑ꢀ
(7)，通过动态jps寻路得到一条新路径。图3-(d)执行步骤s43中的路径优化过程，在原始路径上从起点开始对各个方向进行探测，原始路径上坐标为(p,9)的节点处，向左探测时检测到(n,9)的位置处有同为路径的节点，将该节点也更新为跳点。图3-(e)即为最终更新后的路径。
[0145]
图4为直线路径段出现障碍物的一种情况演示。图4-(a)为步骤s2中首次调用jps寻路后的路径。图4-(b)执行地图更新，检测到在路径中新出现了障碍物。图4-(c)执行步骤
s42-(1)，在检测到障碍物的前方一个节点插入跳点，可以保证在终点到该跳点之间的中间路径是可连通的。接下来执行步骤s42-(2)，图4-(d)即为更新后的路径。图4-(e)执行步骤s43中的路径优化过程，在原始路径上从起点开始对各个方向进行探测，在原始路径上的坐标为(m,6)的节点处，向左探测时探测到(i,6)的位置处有同为路径节点的节点，将该节点也更新为跳点。图4-(f)为最终更新后的新路径。
[0146]
图5也为直线路径段更新障碍物的情况。图5-(a)为在步骤s2中首次调用 jps寻路后的路径。图5-(b)更新地图，检测到在路径中出现了新的障碍物。图5-(c)执行步骤s42-(1)至s42-(2)，在检测到障碍物的前方一个节点(l,7)处插入跳点，可以保证在终点到该跳点之间的中间路径是连通的。以坐标(o,7)位置向(l,7)位置进行寻路，探索完毕后，发现无法由(o,7)位置节点搜索到(l,7)位置节点，寻路失败。图5-(d)中执行步骤s41-(9)，将(o,7)位置处的跳点其父节点(s,11)设为新的“新起点”继续寻路。图5-(e)为寻路结束更新的最终路径。
[0147]
图6为一种路径附近障碍物消失并出现新的“近路”情况。图6-(a)为步骤 s2中首次调用jps寻路后的路径。图6-(b)更新地图信息，去除(i,10)位置处的障碍物。图6-(c)执行步骤s43-(1)至s43-(3)，在(j,11)处的节点向周围方向扫描时，扫描到(g,8)处也有路径节点，将该节点也更新为跳点。图6-(d)为更新后的新路径。图6-(e)继续执行步骤s43-(4)，以更新后的新路径的下一个节点开始继续向各个方向进行探测，检测是否出现新的“近路”。在(i,10)处的节点向周围方向扫描时，扫描到(e,10)处也有路径节点，将该节点也更新为跳点。图6
‑ꢀ
(f)为最终更新后的新路径。
[0148]
上述情况，都为一些简单场景下的障碍物更新情况，直接将jps探索得到的部分路径拼接在原始路径上，再进行路径优化。但在实际的探索过程中，仍有一种复杂情况需讨论。在地图的更新过程中，新出现的障碍物可能会将原路径直接“阻断”，想要通过“绕行”的方式得到新路径会花费大量的时间，且路径会变的很长。此时，便需要结合本算法中的另一条方案来解决此问题。本发明中结合使用原始jps算法探索得到的openlist和closelist节点，对此种情况下的寻路过程进行简化。为更清晰的叙述本发明中应对此种情况的采取的方案，结合图例进行说明，图中涉及到原始jps的前向探索过程以及动态jps的路径更新过程。其中，图7为地图的原始jps的探索过程。图8为地图更新后的动态jps路径更新过程。
[0149]
图7-(a)为原始地图场景，图7-(b)为执行步骤s21至s26的第一轮jps探索后的结果。将(k,9)，(p,9)两个节点加入openlist中。图7-(c)为执行步骤s21 至s26的第二轮jps探索后的结果。选择当前代价值最小的openlist中的节点，即(k,9)加入到closelist中。并以当前节点展开第二轮搜索，将(j,8)，(g, 9)两个节点加入openlist中。图7-(d)为第三轮jps探索后的结果，将(j,8)加入到closelist中，并以当前节点展开第三轮搜索，将(k,7)，(j,3)加入openlist 中。图7-(e)为第四轮jps探索后的结果，将(k,7)加入到closelist中，并以当前节点展开第四轮搜索，将(m,7)加入openlist中。图7-(f)为第五轮jps探索后的结果，将(p,9)加入到closelist中，并以当前节点展开第五轮搜索，将(q, 10)，(q,8)加入openlist中。图7-(g)为第六轮jps探索后的结果，将(q,8)加入到closelist中，并以当前节点展开第六轮搜索，将(p,7)加入openlist中。图7-(g)为第七轮jps探索后的结果，将(q,8)加入到closelist中，并以当前节点展开第七轮搜索，将(n,6)加入openlist中。图7-(h)为第八轮jps探索后的结果，将(m,7)加入到closelist中，并以当前节点展开第八轮搜索，将
(p,7)加入 openlist中。图7-(i)为第九轮jps探索后的结果，将(n,6)加入到closelist中，并以当前节点展开第九轮搜索，将(n,5)加入openlist中。图7-(j)为第十轮jps 探索的结果，当搜索到终点位置处，执行步骤s27，沿着终点反向寻路，将寻到的路径中的节点加入到pathlist中。经过以上步骤，完成了步骤s2中的初次 jps寻路探索。
[0150]
图8-(a)为图7中寻路得到的结果，图8-(b)中更新地图，(k,9)处的节点被更新为障碍物。图8-(c)执行步骤s41-(1)至s41-(2)，将(j,8)处节点更新为跳点并加入到new_openlist中，设置该节点为“新起点”。将(l,9)处节点更新为跳点并加入到new_openlist中，设置该节点为“新终点”。图8-(d)执行步骤 s41-(3)至s41-(6)将(j,8)加入到new_closelist中，并以当前节点为起点，向(l, 9)处节点进行单步限制长度的jps探索，将(j,3)，(i,9)加入到new_openlist 中。图8-(e)继续执行步骤s41-(3)至s41-(6)将(i,9)加入到new_closelist中，继续向(l,9)处节点进行单步限制长度的jps探索，将(g,9)处节点加入到 new_openlist中。图8-(f)执行步骤s41-(4)至s41-(6)，将(p,7)加入到closelist 中，向原终点(m,4)进行单步限制长度的jps探索，将(m,7)，(n,5)处的节点加入到openlist中。图8-(e)执行步骤s41-(4)至s41-(8)，将原先的“废弃路径”延伸，先一步连接至终点位置。将pathlist路径更新为原先的“废弃路径”。其中的节点依次为：起点(m,9)，跳点(p,9)，跳点(q,8)，跳点(p,7)，跳点(n, 5)，终点(m,4)。
[0151]
结合上述图7与图8中的具体案例，依照步骤s4中的方案，调用上一轮中未寻路结束的路径，重新启用上一轮中保存于openlist中的节点，优化了原先路径被障碍物阻断的情况下的路径更新问题。
[0152]
以上所述，仅是本发明的较佳实施例而已，并非对本发明作任何形式上的限制，任何熟悉本专业的技术人员在不脱离本发明技术方案范围内，当可利用上述揭示的技术内容做出些许更动或修饰为等同变化的等效实施例，但凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所做的任何简单修改、等同变化与修饰，均仍属于本发明技术方案的范围内。