16-18年做过一阵子无人驾驶，那时候痴迷于移动规划；然而当时可学习的资料非常少，网上的论文也不算太多。基本就是Darpa的几十篇无人越野几次比赛的文章，基本没有成系统的文章和代码讲解实现。所以对移动规划的认识不算全面，这几年随着自动驾驶、无人机的研究和应用的增多，很多的论文课程成体系的开始介绍这方面的内容。对于一个理工男来说机器人并且是能自动的、智能规划的，相信没有多少理工男是可以抗拒不想去做进一步了解的。所以一直在收集资料，筹划这哪一天可以出一个这方面系列，然后在code一个项目出来在机器人上捣腾各种实现。再一次加速本人对这一想法落实是两年前看到fast-lab高飞团队出的一系列飞行走廊解决无人机路径规划的工作视频。第一次看到视频时候真被震惊到，移动规划原来还可以这么玩，如此优美的数学框架。讲了这么多只是想致敬过去的经历，开启这个专题第一讲。这个系列主线就是围绕高飞老师《移动机器人动态规划》课程讲稿，里面会补充一些算法细节和自己的思考。这个课程对移动规划体系框架构建非常棒，内容排布的也非常好，唯一缺憾就是对于动态不确定障碍物的规划会少一些，因为课程本来就是针对无人机设计的。
现代机器人学和自动驾驶等领域，路径规划是一个重要的主题. 它涉及到在给定的环境中找到从起点到终点的最优路径. 这个过程通常分为两个部分：前端路径搜索和后端轨迹规划. 前端路径搜索在地图中搜索出一条避开障碍物的轨迹，而后端轨迹规划则对搜索到的轨迹进行优化，使其符合机器人的运动学和动力学约束.
实环境中的机器人运动规划是一个比较复杂的问题，对于复杂的问题人类的解法一般都是分步求解：先做个大概、然后在大概轮廓上逐步的复杂精细。机器人运动规划的学院派解法也是如此：
在开始这部分内容介绍前，需要介绍几个概念。介绍这几个概念的目的在于更贴近实际的去理解搜索在业务中应用。搜索路径规划中是把机器人当成一个质点来考虑的，然而实际的机器人是有一定形状和占用空间的，如果把机器人当成质点来考虑很可能是会搜索出一条实际上不可行的（会碰到障碍物的）路径的。为了解决这个问题呢，我们可以简单的物体的形状转移到地图（让地图障碍物区域加上物体占用空间）。在这样的地图里把机器人当成质点来搜索可行路径。
在配置空间中规划¹²³
在工作空间中
如果我们保守地将机器人建模为半径为 _ 的球，那么可以通过在所有方向上膨胀障碍物 _ 来构造C-space1。这是一种常见的机器人碰撞检测方法，通过确保球体中心在膨胀地图的自由空间中来实现碰撞评估1。然而，这种保守的方法并未考虑到机器人的形状和大小。
在路径规划中，构建搜索地图是一个关键步骤。这通常涉及到将实际环境抽象为一个图（Graph），其中节点（Nodes）代表可能的位置，边（Edges）代表从一个位置到另一个位置的移动。以下是一个详细的例子：
假设我们有一个机器人需要在一个室内环境中导航。这个环境可以是一个房间，有一些障碍物，比如桌子和椅子。
栅格地图（Grid Map）则是把环境划分成一系列栅格，在数学视角下是由边联结起来的结点的集合，一个基于图块拼接的地图可以看成是一个栅格图，每个图块(tile)是一个结点，图块之间的连接关系如短线。
概率图（Cost Map）如果在栅格图的基础上，每一栅格给定一个可能值，表示该栅格被占据的概率，则该图为概率图。
特征地图（Feature Map）特征地图用有关的几何特征（如点、直线、面）表示环境。常见于vSLAM（视觉SLAM）技术中。它一般通过如GPS、UWB以及摄像头配合稀疏方式的vSLAM算法产生，优点是相对数据存储量和运算量比较小，多见于最早的SLAM算法中。
拓扑地图（Topological Map）是指地图学中一种统计地图, 一种保持点与线相对位置关系正确而不一定保持图形形状与面积、距离、方向正确的抽象地图。包括有有向图和无向图（字面意思）。
栅格地图
概率图
特征地图
拓扑地图-有向图
拓扑地图-无向图
有了这么多种的地图，那么对应每种图可以用什么对应的算法来做路径的规划呢？下面是地图对应路径搜索算法：
1. 栅格地图 / 概率图
1. Dijkstra
2. BFS（Best-First-Search）
3. A*
4. hybrid A*
5. D *
6. RRT
7. RRT*
8. 蚁群算法
9. Rectangular Symmetry Reduction (RSR)
10. BUG
11. Beam search
12. Iterative Deepeningc
13. Dynamic weighting
14. Bidirectional search
15. Dynamic A* and Lifelong Planning A *
16. Jump Point Search
17. Theta *
2. 拓扑地图
1. Dijkstra
2. BFS（Best-First-Search）
3. A*
4. CH
5. HH
6. CRP
:::success
常用的图搜索有3大类的搜索结构，其它算法都是在这三个大的框架之下做改进。
深度优先搜索（Depth-First Search, DFS）：
广度优先搜索（Breadth-First Search, BFS）：
贪婪搜索（Greedy Search）：
算法核心的三个问题是：
深度优先算法：数据结构维护一个后进先出（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 deque
def 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': # 检查这个节点是否为终点"G"
print("nfind the destination!")
return True
else:
search_queue += graph[node] # 将此节点的相邻节点都添加到节点列表中
searched.Append(node) # 将这个节点标记为检查过
# 如果节点列表为空仍没找到终点，则返回False
return False
print(search("A"))
广度优先搜索算法：
数据结构：维护一个先进先出（FIFO）的容器（即队列），算法操作：移除/扩展容器中最浅的节点。具体代码参考上面深度搜索算法，把“node = search_queue.pop() #深度优先”换成“node = search_queue.popleft() # 广度优先取出节点列表中最左边的节点”即可。可以看出BFS和DFS差别就在于根据“先入”或“后入”的原则，从边界中选择下一个节点。
贪婪搜索（Greedy Search）：
贪心算法的特点是考虑了从目标节点找到任意点的代价，而一般算法考虑的是从起始节点到任意点的代价。即贪心算法考虑的是如何快速的找到目标节点，使得到达目标节点的时间成本最小；而一般算法考虑的是目标节点到达目标节点的花费代价是最小的，而不是快速找到目标节点。基于贪心策略试图向目标移动尽管这不是正确的路径。由于它仅仅考虑到达目标的代价，而忽略了当前已花费的代价，于是尽管路径变得很长，它仍然继续走下去。
贪婪算法中“行动的成本”可以用启发式函数h(n)来算从任意结点n到目标结点的最小代价评估值；启发函数决定了贪婪算法运算书读，所以选择一个好的启发函数很重要。
然而，这两个例子都仅仅是最简单的情况——地图中没有障碍物，最短路径是直线的。现在我们来考虑前边描述的凹型障碍物。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)。
如果alpha是0，则改进后的代价函数的值总是1。这种情况下，地形代价被完全忽略，A工作变成简单地判断一个网格可否通过。如果alpha是1，则最初的代价函数将起作用，然后你得到了A的所有优点。你可以设置alpha的值为0到1的任意值。
可以考虑对启发式函数的返回值做选择：绝对最小代价或者期望最小代价。例如，如果你的地图大部分地形代价为2，其它一些地方是代价为1的道路，那么你可以考虑让启发式函数不考虑道路，而只返回2距离。
速度和精确度之间的选择并不是全局固定对。在地图上的某些区域，精确度是重要的，你可以基于此进行动态选择。例如，假设我们可能在某点停止重新计算路径或者改变方向，则在接近当前位置的地方，选择一条好的路径则是更重要的，对于在地图上的一个安全区域，最短路径也许并不十分重要，但是当从一个危险区域脱离对时候，轨迹的精度是最重要的。
同样通过对g(n)或者f(n)的调节，也可以达成A具体动作的控制
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搜索的区域非常少：
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前先了解它的规则
例子：
更详细跳点搜索可以参考下面文章：
https://blog.csdn.net/LIQIANGEASTSUN/article/details/118766080
本文介绍了motion plan学院派的框架：
并且详细介绍了前端路径规划常用的搜索规划，介绍了搜索规划的一些前置知识：
详细介绍了了几种贪心搜索算法原理和实现思路：
并且介绍了：累计成本，启发函数，以及这两个函数的物理意义；如何调控两个参数来实现计算速度和最优路径的平衡。
京公网安备 11010802035086号