手机站电影,最近发生的热点事件,国家对网站建设补补贴,建查查官网背景#xff1a;
16-18年做过一阵子无人驾驶#xff0c;那时候痴迷于移动规划#xff1b;然而当时可学习的资料非常少#xff0c;网上的论文也不算太多。基本就是Darpa的几十篇无人越野几次比赛的文章#xff0c;基本没有成系统的文章和代码讲解实现。所以对移动规划的认…
背景
16-18年做过一阵子无人驾驶那时候痴迷于移动规划然而当时可学习的资料非常少网上的论文也不算太多。基本就是Darpa的几十篇无人越野几次比赛的文章基本没有成系统的文章和代码讲解实现。所以对移动规划的认识不算全面这几年随着自动驾驶、无人机的研究和应用的增多很多的论文课程成体系的开始介绍这方面的内容。对于一个理工男来说机器人并且是能自动的、智能规划的相信没有多少理工男是可以抗拒不想去做进一步了解的。所以一直在收集资料筹划这哪一天可以出一个这方面系列然后在code一个项目出来在机器人上捣腾各种实现。再一次加速本人对这一想法落实是两年前看到fast-lab高飞团队出的一系列飞行走廊解决无人机路径规划的工作视频。第一次看到视频时候真被震惊到移动规划原来还可以这么玩如此优美的数学框架。讲了这么多只是想致敬过去的经历开启这个专题第一讲。这个系列主线就是围绕高飞老师《移动机器人动态规划》课程讲稿里面会补充一些算法细节和自己的思考。这个课程对移动规划体系框架构建非常棒内容排布的也非常好唯一缺憾就是对于动态不确定障碍物的规划会少一些因为课程本来就是针对无人机设计的。
正文
现代机器人学和自动驾驶等领域路径规划是一个重要的主题. 它涉及到在给定的环境中找到从起点到终点的最优路径. 这个过程通常分为两个部分前端路径搜索和后端轨迹规划. 前端路径搜索在地图中搜索出一条避开障碍物的轨迹而后端轨迹规划则对搜索到的轨迹进行优化使其符合机器人的运动学和动力学约束.实环境中的机器人运动规划是一个比较复杂的问题对于复杂的问题人类的解法一般都是分步求解先做个大概、然后在大概轮廓上逐步的复杂精细。机器人运动规划的学院派解法也是如此
1.前端路径规划
基于搜索的方法 通用图搜索深度优先搜索DFS广度优先搜索BFSDijkstra 和 A* 搜索跳点搜索 基于采样的方法 概率路线图PRM快速探索随机树RRTRRT*有信息的 RRT* 带动力学约束路径规划 状态-状态边界值最优控制问题状态栅格搜索动力学RRT*混合A*
2.后端轨迹生成
最小抖动轨迹生成 微分平坦性最小抖动优化最小抖动的闭式解时间分配实际应用 软硬约束轨迹优化 软约束轨迹优化硬约束轨迹优化
3.不确定性状态求解移动障碍物、突变环境、设备建模变化
基于马尔可夫决策过程的规划MDP 规划中的不确定性和马尔可夫决策过程最小最大成本规划和期望成本最小规划值迭代和实时动态规划 机器人规划的模型预测控制MPC 线性模型预测控制非线性模型预测控制
前端——搜索路径规划
在开始这部分内容介绍前需要介绍几个概念。介绍这几个概念的目的在于更贴近实际的去理解搜索在业务中应用。搜索路径规划中是把机器人当成一个质点来考虑的然而实际的机器人是有一定形状和占用空间的如果把机器人当成质点来考虑很可能是会搜索出一条实际上不可行的会碰到障碍物的路径的。为了解决这个问题呢我们可以简单的物体的形状转移到地图让地图障碍物区域加上物体占用空间。在这样的地图里把机器人当成质点来搜索可行路径。在配置空间中规划¹²³
机器人在C-space中被表示为一个点例如位置在R3中的一个点姿态在(3)中的一个点等等⁴⁷⁸障碍物需要在配置空间中表示在运动规划之前的一次性工作称为配置空间障碍物或C-障碍⁴⁵⁶C-space (C-障碍) ∪ (C-自由)⁴⁵⁶路径规划是在C-自由中找到从起点qstart到目标点qgoal的路径⁹[10]¹⁵
在工作空间中
机器人有形状和大小即难以进行运动规划在配置空间C-space中 机器人是一个点即易于进行运动规划⁶ 在进行运动规划之前障碍物在C-space中表示⁸[10]在C-space中表示障碍物可能非常复杂。因此在实践中使用近似但更保守的表示。
如果我们保守地将机器人建模为半径为 _ 的球那么可以通过在所有方向上膨胀障碍物 _ 来构造C-space1。这是一种常见的机器人碰撞检测方法通过确保球体中心在膨胀地图的自由空间中来实现碰撞评估1。然而这种保守的方法并未考虑到机器人的形状和大小。
构建地图
在路径规划中构建搜索地图是一个关键步骤。这通常涉及到将实际环境抽象为一个图Graph其中节点Nodes代表可能的位置边Edges代表从一个位置到另一个位置的移动。以下是一个详细的例子假设我们有一个机器人需要在一个室内环境中导航。这个环境可以是一个房间有一些障碍物比如桌子和椅子。
定义节点Nodes首先我们需要确定节点的位置。在这个例子中我们可以将房间的每一个可达的位置定义为一个节点。例如我们可以创建一个网格Grid每一个网格单元都是一个节点。定义边Edges然后我们需要确定边。如果机器人可以直接从一个节点移动到另一个节点那么这两个节点之间就有一条边。在我们的例子中如果两个网格单元相邻并且没有障碍物阻挡那么这两个网格单元即节点之间就有一条边。定义权重Weights最后我们需要为每一条边定义一个权重。权重可以根据实际的移动成本来确定。例如如果从一个节点到另一个节点的距离更远或者路径上有斜坡那么这条边的权重就应该更大。
地图种类
栅格地图Grid Map则是把环境划分成一系列栅格在数学视角下是由边联结起来的结点的集合一个基于图块拼接的地图可以看成是一个栅格图每个图块(tile)是一个结点图块之间的连接关系如短线。概率图Cost Map如果在栅格图的基础上每一栅格给定一个可能值表示该栅格被占据的概率则该图为概率图。特征地图Feature Map特征地图用有关的几何特征如点、直线、面表示环境。常见于vSLAM视觉SLAM技术中。它一般通过如GPS、UWB以及摄像头配合稀疏方式的vSLAM算法产生优点是相对数据存储量和运算量比较小多见于最早的SLAM算法中。拓扑地图Topological Map是指地图学中一种统计地图, 一种保持点与线相对位置关系正确而不一定保持图形形状与面积、距离、方向正确的抽象地图。包括有有向图和无向图字面意思。
栅格地图概率图特征地图拓扑地图-有向图拓扑地图-无向图
搜索算法介绍
有了这么多种的地图那么对应每种图可以用什么对应的算法来做路径的规划呢下面是地图对应路径搜索算法
1. 栅格地图 / 概率图1. Dijkstra2. BFSBest-First-Search3. A*4. hybrid A*5. D *6. RRT7. RRT*8. 蚁群算法9. Rectangular Symmetry Reduction (RSR) 10. BUG11. Beam search12. Iterative Deepeningc13. Dynamic weighting14. Bidirectional search15. Dynamic A* and Lifelong Planning A *16. Jump Point Search17. Theta *2. 拓扑地图1. Dijkstra2. BFSBest-First-Search3. A*4. CH5. HH6. CRP图搜索算法结构
:::success
维护一个容器来存储所有待访问的节点该容器以起始状态XS进行初始化循环 根据某个预定义的评分函数从容器中移除一个节点 访问一个节点 扩展获取该节点的所有邻居 发现所有的邻居将它们邻居推入容器 扩展获取该节点的所有邻居结束循环 :::
通用搜索算法结构
常用的图搜索有3大类的搜索结构其它算法都是在这三个大的框架之下做改进。深度优先搜索Depth-First Search, DFS
原理DFS是一种用于遍历或搜索树或图的算法。这个算法会尽可能深地搜索树的分支。当节点v的所在边都己被探寻过搜索将回溯到发现节点v的那条边的起始节点。这一过程一直进行到已发现从源节点可达的所有节点为止。如果还存在未被发现的节点则选择其中一个作为源节点并重复以上过程整个进程反复进行直到所有节点都被访问为止。优点实现简单当目标明确时搜索效率高。缺点不保证找到最短路径有可能会导致搜索陷入无限循环。
广度优先搜索Breadth-First Search, BFS
原理BFS是一种广度优先的搜索算法用于搜索树或图。这个算法从根节点开始沿着树的宽度遍历树的节点如果所有节点均被访问则算法结束。优点可以找到最短路径结果可靠。缺点空间复杂度高当解空间大时内存消耗大。
贪婪搜索Greedy Search
原理贪婪搜索是一种在每一步选择中都采取在当前看来最好的选择希望通过一系列的最优选择能够产生一个全局最优的解决方案。优点简单易于实现计算速度快。缺点不能保证找到全局最优解只能保证找到局部最优解。
深度优先搜索DFSDFS会沿着一条路径不断往下搜索直到不能再继续为止然后再折返开始搜索下一条路径。这种搜索策略可以看作是“先入后出”因此在实现DFS时通常使用栈Stack这种数据结构。DFS的优点是实现简单当目标明确时搜索效率高。然而DFS的缺点是不保证找到最短路径有可能会导致搜索陷入无限循环。广度优先搜索BFS相比之下BFS会根据离起点的距离按照从近到远的顺序对各节点进行搜索。这种搜索策略可以看作是“先入先出”因此在实现BFS时通常使用队列Queue这种数据结构。BFS的优点是可以找到最短路径结果可靠。然而BFS的缺点是空间复杂度高当解空间大时内存消耗大。
算法核心的三个问题是• 问题1何时结束循环 • 可能的选项当容器为空时结束循环 • 问题2如果图是循环的怎么办 • 当一个节点从容器中移除扩展/访问后它就不应该再被添加回容器 • 问题3如何移除正确的节点以便尽快到达目标状态从而减少图节点的扩展。深度优先算法数据结构维护一个后进先出LIFO的容器即栈算法移除/扩展容器中最深的节点
#生成示例数据
graph {}
graph[A] [B, D, F]
graph[B] [C, E]
graph[D] [C]
graph[F] [G, H]
graph[C] []
graph[E] []
graph[G] []
graph[H] []from collections import deque
search_queue deque() # 创建一个节点列表
search_queue graph[A] # 表示将A的相邻节点都添加到节点列表中
from collections import dequedef search(start_node):search_queue deque()search_queue graph[start_node]searched [] # 这个数组用于记录检查过的节点while search_queue: # 只要节点列表不为空node search_queue.pop() #深度优先#node search_queue.popleft() # 广度优先取出节点列表中最左边的节点print(node, end ) # 打印出当前节点if not node in searched: # 如果这个节点没检查过if node G: # 检查这个节点是否为终点Gprint(\nfind the destination!)return Trueelse:search_queue graph[node] # 将此节点的相邻节点都添加到节点列表中searched.append(node) # 将这个节点标记为检查过# 如果节点列表为空仍没找到终点则返回Falsereturn Falseprint(search(A))广度优先搜索算法数据结构维护一个先进先出FIFO的容器即队列算法操作移除/扩展容器中最浅的节点。具体代码参考上面深度搜索算法把“node search_queue.pop() #深度优先”换成“node search_queue.popleft() # 广度优先取出节点列表中最左边的节点”即可。可以看出BFS和DFS差别就在于根据“先入”或“后入”的原则从边界中选择下一个节点。 贪婪搜索Greedy Search**贪心算法的特点是考虑了从目标节点找到任意点的代价而一般算法考虑的是从起始节点到任意点的代价。**即贪心算法考虑的是如何快速的找到目标节点使得到达目标节点的时间成本最小而一般算法考虑的是目标节点到达目标节点的花费代价是最小的而不是快速找到目标节点。基于贪心策略试图向目标移动尽管这不是正确的路径。由于它仅仅考虑到达目标的代价而忽略了当前已花费的代价于是尽管路径变得很长它仍然继续走下去。贪婪算法中“行动的成本”可以用启发式函数h(n)来算从任意结点n到目标结点的最小代价评估值启发函数决定了贪婪算法运算书读所以选择一个好的启发函数很重要。• 实际的搜索问题中从一个节点到其邻居有一个“C”的成本• 可以作为启发函数计算代价的有长度时间能量等 • 当所有权重都为1时贪婪算法找到最优解 • 对于一般情况如何尽快找到最小成本路径Dijkstra算法 Dijkstra算法算是贪心思想实现的其可以适用与拓扑图或者栅格图具体实现方法是首先把起点到所有点的距离存下来找个最短的然后松弛一次再找出最短的所谓的松弛操作就是遍历一遍看通过刚刚找到的距离最短的点作为中转站会不会更近如果更近了就更新距离这样把所有的点找遍之后就存下了起点到其他所有点的最短距离 • 策略扩展/访问具有最低累积成本g(n)的节点 g(n)从起始状态到节点“n”的累积成本的当前最佳估计 更新所有未扩展邻居“m”的累积成本g(m) 已经被扩展/访问的节点保证具有从起始状态到该节点的最小成本 :::success 维护一个优先队列来存储所有待扩展的节点 用起始状态XS初始化优先队列 设置g(XS)0, 对图中的其他所有节点设置g(n)无穷 循环: 如果队列为空,返回FALSE并退出循环从优先队列中取出g(n)最小的节点“n”将节点“n”标记为已扩展如果节点“n”是目标状态,返回TRUE并退出循环对节点“n”的所有未扩展的邻居节点“m”: 如果g(m) 无穷 g(m) g(n) Cnm将节点“m”加入队列 如果g(m) g(n) Cnm g(m) g(n) Cnm 结束对邻居节点的循环 结束主循环 ::: BFSBest-First-Search算法BFSBest-First-Search算法也是可以看作基于启发式的深度优先算法其按照和Dijkstra类似的流程运行不同的是它能够评估任意结点到目标点的代价即启发式函数。与选择离初始结点最近的结点不同的是它选择离目标最近的结点。BFS不能保证找到一条最短路径。但是它比Dijkstra算法快的多因为它用了一个启发式函数heuristic 能快速地导向目标结点。例如如果目标位于出发点的南方BFS将趋向于导向南方的路径。在下面的图中越黄的结点代表越高的启发值移动到目标的代价高而越黑的结点代表越低的启发值移动到目标的代价低。这表明了与Dijkstra 算法相比BFS运行得更快。然而这两个例子都仅仅是最简单的情况——地图中没有障碍物最短路径是直线的。现在我们来考虑前边描述的凹型障碍物。Dijkstra算法运行得较慢但确实能保证找到一条最短路径另一方面BFS运行得较快但是它找到的路径明显不是一条好的路径由于BFS是基于贪心策略的它试图向目标移动尽管这不是正确的路径。由于它仅仅考虑到达目标的代价而忽略了当前已花费的代价于是尽管路径变得很长它仍然继续走下去。结合两者的优点不是更好吗1968年发明的A算法就是把启发式方法heuristic approaches如BFS和常规方法如Dijsktra算法结合在一起的算法。有点不同的是类似BFS的启发式方法经常给出一个近似解而不是保证最佳解。然而尽管A基于无法保证最佳解的启发式方法A却能保证找到一条最短路径。*A: 带有启发式函数的Dijkstra算法 **把Dijkstra算法靠近初始点的结点和BFS算法靠近目标点的结点的信息块结合起来。在A的标准术语中g(n)表示从初始结点到任意结点n的代价h(n)表示从结点n到目标点的启发式评估代价heuristic estimated cost。当从初始点向目标点移动时A*权衡这两者。每次进行主循环时它检查f(n)最小的结点n其中f(n) g(n) h(n)。• 累积成本 • g(n): 从起始状态到节点“n”的累积成本的当前最佳估计 • 启发式函数 • h(n): 从节点n到目标状态即目标成本的预计最小成本 • 从起始状态到通过节点“n”的目标状态的最小预计成本是 f(n) g(n) h(n) • 策略: 扩展具有最便宜的 f(n) g(n) h(n) 的节点 • 更新所有未扩展邻居“m”的节点“n”的累积成本 g(m) • 已经扩展的节点保证具有从起始状态到该节点的最小成本 :::success 维护一个优先队列来存储所有待扩展的节点 对所有节点预定义启发函数h(n) 用起始状态XS初始化优先队列 设置g(XS)0,对图中的其他节点设置g(n)无穷 循环: 如果队列为空,返回FALSE并退出循环从队列中取出**f(n)g(n)h(n)**最小的节点“n”将节点“n”标记为已扩展如果节点“n”是目标状态,返回TRUE并退出循环对节点“n”的所有未扩展邻居节点“m”: 如果g(m)无穷 g(m) g(n) Cnm将节点“m”加入队列 如果g(m)g(n)Cnm g(m) g(n) Cnm 结束对邻居节点循环 结束主循环 ::: 通过对启发式函数的调节可以达成控制A*的行为 一种极端情况如果h(n)是0则只有g(n)起作用此时A*演变成Dijkstra算法这保证能找到最短路径。 如果h(n)经常都比从n移动到目标的实际代价小或者相等则A保证能找到一条最短路径。h(n)越小A扩展的结点越多运行就得越慢。 如果h(n)精确地等于从n移动到目标的代价则A 将会仅仅寻找最佳路径而不扩展别的任何结点这会运行得非常快。尽管这不可能在所有情况下发生但是你仍可以在一些特殊情况下让它们精确地相等。只要提供完美的信息A会运行得很完美认识这一点很好。 如果h(n)有时比从n移动到目标的实际代价高则A*不能保证找到一条最短路径但它运行得更快。 另一种极端情况如果h(n)比g(n)大很多则只有h(n)起作用A*就演变成了BFS算法。
如果目标的引力太低会得到最短路径不过速度变慢了如果目标引力太高那就放弃了最短路径但A运行得更快所以最优路径和最快搜索在复杂情况下需要有一个取舍/平衡。A的这个特性非常有用。例如你会发现在某些情况下你希望得到一条好的路径而不是一条完美的路径为了权衡g(n)和h(n)你可以修改任意一个。如果alpha是0则改进后的代价函数的值总是1。这种情况下地形代价被完全忽略A工作变成简单地判断一个网格可否通过。如果alpha是1则最初的代价函数将起作用然后你得到了A的所有优点。你可以设置alpha的值为0到1的任意值。可以考虑对启发式函数的返回值做选择绝对最小代价或者期望最小代价。例如如果你的地图大部分地形代价为2其它一些地方是代价为1的道路那么你可以考虑让启发式函数不考虑道路而只返回2距离。速度和精确度之间的选择并不是全局固定对。在地图上的某些区域精确度是重要的你可以基于此进行动态选择。例如假设我们可能在某点停止重新计算路径或者改变方向则在接近当前位置的地方选择一条好的路径则是更重要的对于在地图上的一个安全区域最短路径也许并不十分重要但是当从一个危险区域脱离对时候轨迹的精度是最重要的。同样通过对g(n)或者f(n)的调节也可以达成A具体动作的控制
通过加上障碍物cost function到g(n)或者f(n)这两个动作是一个意思可以实现规划路径在障碍物中间。通过加上车辆几何或者轨迹kappa平滑度cost function的到g(n)或者f(n)可以实现规划出来的路径是平滑变化的。通过加上到way point的cost function的到g(n)或者f(n)规划出来的路径则倾向于走way points的方向。
构造精确启发函数的一种方法是预先计算任意一对结点之间最短路径的长度。有几种方法可以近似模拟这种启发函数
1. 【降采样地图-预计算】在密集栅格图的基础上添加一个分辨率更大的稀疏栅格图。预计算稀疏图中任意两个栅格的最短路径。2. 【waypoings-预计算】在密集栅格图上预计算任意两个way points的的最短路径。通过以上方法添加一个启发函数h’用于评估从任意位置到达邻近导航点/中继点waypoints的代价。最终的启发式函数可以是h(n) h’(n, w1) distance(w1, w2), h’(w2, goal)网格地图中的启发式算法在网格地图中有一些众所周知的启发式函数计算方法
1. 曼哈顿距离
2. 对角线距离
3. 欧几里得距离
4. 欧几里德距离平方曼哈顿距离标准的启发式函数是曼哈顿距离Manhattan distance。考虑代价函数并找到从一个位置移动到邻近位置的最小代价D。因此h是曼哈顿距离的D倍
H(n) D \ * (abs ( n.x – goal.x ) abs ( n.y – goal.y ) ) 对角线距离如果在地图中允许对角运动那么需要考虑对角线距离。4 east, 4 north的曼哈顿距离将变成8D。然而可以简单地移动4 northeast代替所以启发函数应该是4D。这个函数使用对角线假设直线和对角线的代价都是D
H(n) D * max(abs(n.x - goal.x), abs(n.y - goal.y))如果对角线运动的代价不是D但类似于D2 sqrt(2) * D则准确的计算方法如下
h_diagonal(n) min(abs(n.x - goal.x), abs(n.y - goal.y))h_straight(n) (abs(n.x - goal.x) abs(n.y - goal.y))H(n) D2\* h_diagonal(n) D\* (h_straight(n) - 2\ *h_diagonal(n)))计算h_diagonal(n)沿着斜线可以移动的步数h_straight(n)曼哈顿距离然后合并这两项让所有的斜线步都乘以D2剩下的所有直线步(注意这里是曼哈顿距离的步数减去2倍的斜线步数)都乘以D。欧几里德距离如果单位可以沿着任意角度移动而不是网格方向那么应该使用直线距离
H(n) D* sqrt((n.x-goal.x)^2 (n.y-goal.y)^2) 然而如果是这样的话直接使用A时将会遇到麻烦因为代价函数g不匹配启发函数h。因为欧几里得距离比曼哈顿距离和对角线距离都短你仍可以得到最短路径不过A将运行得更久一些欧几里德距离平方还有一个方法是使用距离的平方替代距离避免进行平方根开方运算从而减少计算消耗
H(n) D* ((n.x-goal.x)^2 (n.y-goal.y)^2) 不过这样做会明显地导致衡量单位的问题。当A计算f(n) g(n) h(n)距离的平方将比g的代价大很多并且会因为启发式函数评估值过高而停止。对于更长的距离这样做会靠近g(n)的极端情况而不再计算任何东西A退化成BFS启发函数的启发因子导致A搜索低性能的另外一个原因是启发函数的启发因子。当某些路径具有相同的f值的时候它们都会被探索比较函数无法打破比较平衡点尽管我们这时候只需要搜索其中的一条下图为没有添加启发因子的效果为了解决这个问题我们可以为启发函数添加一个较小的启发因子。启发因子对于每个结点必须是确定的唯一的而且它必须让f值体现区别。因为A将会对f值进行堆排序让f值不同意味着只有一个f值会被检测。
一种添加启发因子的方式是稍微改变h的衡量单位。如果我们减少衡量单位那么当我们朝着目标移动的时候f将逐渐增加。很不幸这意味着A倾向于扩展到靠近初始点的结点而不是靠近目标的结点。我们可以稍微的微调h的衡量单位甚至是0.1%A就会倾向于扩展到靠近目标的结点。
heuristic \ \ * (1.0 p) 其中这里的启发因子需要满足
p 移动一步的最小代价 / 期望的最长路径长度。假设你不希望你的路径超过1000步step你可以使p 1 / 1000。添加这个附加值的结果是A比以前搜索的结点更少了。如下图所示。当存在障碍物时当然仍要在它们周围寻找路径但要意识到当绕过障碍物以后A搜索的区域非常少
Steven van Dijk的建议是直接把h放到比较函数中。当f值相等时比较函数将会通过比较两个节点h的大小实现启发因子的功能打破比较平衡点。Cris Fuhrman的建议的启发因子是给每个节点加一个决定性的任意数例如所在坐标系中位置的hash值最后一种方法类似于frenet坐标系的做法对比起点到终点的直连线段的投影距离计算方法入下
dx1 current.x - goal.x
dy1 current.y - goal.y
dx2 start.x - goal.x
dy2 start.y - goal.y
cross abs(dx1*dy2 - dx2*dy1)
heuristic cross*0.001其目的是计算初始-目标向量和当前-目标向量的向量叉乘cross-product。当向量偏离方向后其叉乘将会提供一个较大的启发因子。结果是这段代码选择的路径稍微倾向于从初始点到目标点的直线。当没有障碍物时A不仅搜索很少的区域而且它找到的路径看起来非常棒跳点搜索Jump Point Search (JPS) 是一种改进的 A_ 算法它保留了 A_ 算法的主体框架但在寻找后继节点的操作上进行了优化。在 A 算法中会将当前节点的所有未访问邻居节点加入 openlist。而 JPS 则使用一些方法将有“价值”的节点加入 openlist。JPS 的核心就是寻找跳点 (Jump Point)在 JPS 中就是将跳点加入 openlist。跳点就是路径的转折点。 JPS明智地进行探索因为它总是根据规则向前看。强调了其在搜索过程中的智能性和前瞻性。JPS 算法的基本流程与 A* 一致代价函数 f(n) 仍然表示如下 f(n)g(n)h(n)。JPS 算法的优点在于由于它只扩展跳点跳点间的栅格被跳过不加入 OpenList因此它的搜索效率比 A* 算法提高了一个等级。在实现JPS前先了解它的规则1.强迫邻居节点X的邻居节点有障碍物且X的父节点P经过X到达N的距离代价比不经过X到达N的任一路径的距离代价都小则称N是X的强迫邻居。2.跳点(Jump Point)什么样的节点可以作为跳点(1)节点 A 是起点、终点.(2)节点A 至少有一个强迫邻居.(3)父节点在斜方向(斜向搜索)节点A的水平或者垂直方向上有满足 (1)、(2) 的节点在搜索过程中它可以将水平和垂直方向两个分量分解为一个方向为(1, 0)的水平搜索一个方向为(0, 1)的垂直搜索同理斜向有四种方向左上 (-1, 1) ——对应的水平 (-1, 0)垂直 ( 0, 1)右上 ( 1, 1) ——对应的水平 ( 1, 0)垂直 ( 0, 1)右下 ( 1, -1) ——对应的水平 ( 1, 0)垂直 ( 0, -1)左下 (-1, -1) ——对应的水平 (-1, 0)垂直 ( 0, -1)如上所说(3)的情形即为如下
• 递归应用直线剪枝规则并将 y 识别为 x 的跳点后继。这个节点很有趣因为它有一个邻居 z除非通过访问 x 然后 y 的路径否则无法最优地到达。 • 递归应用对角线剪枝规则并将 y 识别为 x 的跳点后继。 • 在每次对角线步骤之前我们首先递归直线。只有当两次直线递归都未能识别出跳点时我们才再次对角线步进。 • 节点 wx 的强制邻居正常扩展。也推入开放列表优先队列。记住:你只能直线跳跃或对角线跳跃;不能分段跳跃。 :::success
维护一个优先队列来存储所有待扩展的节点对所有节点预先定义启发函数h(n)用起始状态XS初始化优先队列设置g(XS)0,对图中的其他节点设置g(n)无穷循环: 如果队列为空,返回FALSE并退出循环从队列中取出f(n)g(n)h(n)最小的节点“n”将节点“n”标记为已扩展如果节点“n”是目标状态,返回TRUE并退出循环对节点“n”的所有未扩展邻居节点“m”: 如果g(m)无穷 g(m) g(n) Cnm将节点“m”加入队列 如果g(m)g(n)Cnm g(m) g(n) Cnm 结束对邻居节点的循环 结束主循环 ::: openlist查找具体流程如下²
初始化起点节点 start 将起点周围四个角落的空闲节点相对于起点的相对位置加入起点节点的 forced_neighbor_list。创建一个 openlist 将 start 加入 openlist。while openlist is not empty: node ← openlist.Pop ()从 node 开始跳跃首先进行直线跳跃再进行对角线跳跃。用 parent 表示从 node 进行对角线跳跃得到的节点用 current 表示从 parent 进行直线跳跃得到的节点。如果 current 是跳点而 parent 与 node 是同一个节点则将 current 加入 openlist同时将 current 的父节点指向 node如果 current 是跳点而 parent 与 node 不是同一个节点则将 parent 和 current 加入 openlist同时将 current 的父节点指向 parent将 parent 的父节点指向 node如果 current 是障碍物或者边界则进行对角线跳跃如果 parent 是障碍物或者边界则进入下一轮循环。
例子
扩展—对角线移动最终找到一个关键节点,将其加入开放列表。从开放列表中弹出它(唯一的节点)。垂直扩展,在障碍物处结束。 水平扩展,遇到一个具有强制邻居的节点。将其添加到开放列表。 对角线扩展,扩展后没有发现任何新的节点。完成当前节点的扩展。 对角线移动。先沿垂直和水平方向扩展。 没有发现任何新的节点。对角线移动。
更详细跳点搜索可以参考下面文章https://blog.csdn.net/LIQIANGEASTSUN/article/details/118766080
小结
本文介绍了motion plan学院派的框架1.前端路径规划2.后端轨迹生成3.不确定障碍物预估规划并且详细介绍了前端路径规划常用的搜索规划介绍了搜索规划的一些前置知识1.c-space为了方便物体质点化处理建图时把物体形状构建转移到图2.各种不同图如何构建成适合搜索算法的数据格式以及不同图适合的搜索算法3.搜索算法的三个基本框架深度搜索、广度搜索、贪心搜索详细介绍了了几种贪心搜索算法原理和实现思路1.Dijkstra算法2.A*搜索3.跳点搜索并且介绍了累计成本启发函数以及这两个函数的物理意义如何调控两个参数来实现计算速度和最优路径的平衡。• 累积成本 • g(n): 从起始状态到节点“n”的累积成本的当前最佳估计 • 启发式函数 • h(n): 从节点n到目标状态即目标成本的预计最小成本
