先引用一篇文章 https://www.cnblogs.com/21207-iHome/p/6048969.html
已经不知道原始出处了，。
- Dijkstra算法
迪杰斯特拉（Dijkstra）算法是典型的最短路径的算法，由荷兰计算机科学家迪杰斯特拉于1959年提出，用来求得从起始点到其他所有点最短路径。该算法采用了贪心的思想，每次都查找与该点距离最近的点，也因为这样，它不能用来解决存在负权边的图。解决的问题可描述为：在无向图 G=(V,E) 中，假设每条边 E[i] 的长度为 w[i]，找到由顶点vs到其余各点的最短路径。
1) 基本思想：
通过Dijkstra计算图G中的最短路径时，需要指定起点vs(即从顶点vs开始计算)。此外，引进两个集合S和U。S的作用是记录已求出最短路径的顶点(以及相应的最短路径长度)，而U则是记录还未求出最短路径的顶点(以及该顶点到起点vs的距离)。初始时，S中只有起点vs；U中是除vs之外的顶点，并且U中顶点的路径是"起点vs到该顶点的路径"。然后，从U中找出路径最短的顶点，并将其加入到S中；接着，更新U中的顶点和顶点对应的路径。 然后，再从U中找出路径最短的顶点，并将其加入到S中；接着，更新U中的顶点和顶点对应的路径。重复该操作，直到遍历完所有顶点。
2) 算法步骤：
a.初始时，S只包含源点，即S＝{vs}，vs的距离为0。U包含除vs外的其他顶点，即U={其余顶点}，若u不是vs的出边邻接点，则<u,vs>权值为∞；
b.从U中选取一个距离vs最小的顶点k，把k加入S中（该选定的距离就是vs到k的最短路径长度min）；
c.以k为新考虑的中间点，修改U中各顶点的距离；若从源点vs到顶点u的距离（经过顶点k）比原来距离（不经过顶点k）短，则修改顶点u的距离值，即dist[u] = min( dist[u], min + w[k][u] )；
d.重复步骤b和c直到所有顶点都包含在S中。
3) 算法实例：
先给出一个无向图
image
用Dijkstra算法找出以A为起点的单源最短路径步骤如下：
image
具体实现的代码如下：
image
View Code
输入和输出如下图所示：
image
- A算法*
路径规划是指的是机器人的最优路径规划问题，即依据某个或某些优化准则（如工作代价最小、行走路径最短、行走时间最短等），在工作空间中找到一个从起始状态到目标状态能避开障碍物的最优路径。机器人的路径规划应用场景极丰富，最常见如游戏中NPC及控制角色的位置移动，百度地图等导航问题，小到家庭扫地机器人、无人机大到各公司正争相开拓的无人驾驶汽车等。
目前路径规划算法分为：
image
A算法原理：*
在计算机科学中，A*算法作为Dijkstra算法的扩展，因其高效性而被广泛应用于寻路及图的遍历，如星际争霸等游戏中就大量使用。在理解算法前，我们需要知道几个概念：
- 搜索区域（The Search Area）：图中的搜索区域被划分为了简单的二维数组，数组每个元素对应一个小方格，当然我们也可以将区域等分成是五角星，矩形等，通常将一个单位的中心点称之为搜索区域节点（Node）。
- 开放列表(Open List)：我们将路径规划过程中待检测的节点存放于Open List中，而已检测过的格子则存放于Close List中。
- 父节点（parent）：在路径规划中用于回溯的节点，开发时可考虑为双向链表结构中的父结点指针。
- 路径排序（Path Sorting）：具体往哪个节点移动由以下公式确定：F(n) = G + H 。G代表的是从初始位置A沿着已生成的路径到指定待检测格子的移动开销。H指定待测格子到目标节点B的估计移动开销。
- 启发函数（Heuristics Function）：H为启发函数，也被认为是一种试探，由于在找到唯一路径前，我们不确定在前面会出现什么障碍物，因此用了一种计算H的算法，具体根据实际场景决定。在我们简化的模型中，H采用的是传统的曼哈顿距离（Manhattan Distance），也就是横纵向走的距离之和。
如下图所示，绿色方块为机器人起始位置A，红色方块为目标位置B，蓝色为障碍物。
image
我们把要搜寻的区域划分成了正方形的格子。这是寻路的第一步，简化搜索区域。这个特殊的方法把我们的搜索区域简化为了 2 维数组。数组的每一项代表一个格子，它的状态就是可走 (walkalbe)或不可走 (unwalkable) 。现用A*算法寻找出一条自A到B的最短路径，每个方格的边长为10，即垂直水平方向移动开销为10。因此沿对角移动开销约等于14。具体步骤如下：
从起点 A 开始，把它加入到一个由方格组成的open list(开放列表) 中，这个open list像是一个购物清单。Open list里的格子是可能会是沿途经过的，也有可能不经过。因此可以将其看成一个待检查的列表。查看与A相邻的8个方格 ，把其中可走的 (walkable) 或可到达的(reachable) 方格加入到open list中。并把起点 A 设置为这些方格的父节点 (parent node) 。然后把 A 从open list中移除，加入到close list(封闭列表) 中，close list中的每个方格都是不需要再关注的。
如下图所示，深绿色的方格为起点A，它的外框是亮蓝色，表示该方格被加入到了close list 。与它相邻的黑色方格是需要被检查的，他们的外框是亮绿色。每个黑方格都有一个灰色的指针指向他们的父节点A。
image
下一步，我们需要从open list中选一个与起点A相邻的方格。但是到底选择哪个方格好呢？选F值最小的那个。我们看看下图中的一些方格。在标有字母的方格中G = 10 。这是因为水平方向从起点到那里只有一个方格的距离。与起点直接相邻的上方，下方，左方的方格的 G 值都是 10 ，对角线的方格 G 值都是14 。H值通过估算起点到终点 ( 红色方格 ) 的 Manhattan 距离得到，仅作横向和纵向移动，并且忽略沿途的障碍。使用这种方式，起点右边的方格到终点有3 个方格的距离，因此 H = 30 。这个方格上方的方格到终点有 4 个方格的距离 ( 注意只计算横向和纵向距离 ) ，因此 H = 40 。
image
比较open list中节点的F值后，发现起点A右侧节点的F=40，值最小。选作当前处理节点，并将这个点从Open List删除，移到Close List中。
image
对这个节点周围的8个格子进行判断，若是不可通过（比如墙，水，或是其他非法地形）或已经在Close List中，则忽略。否则执行以下步骤：
- 若当前处理节点的相邻格子已经在Open List中，则检查这条路径是否更优，即计算经由当前处理节点到达那个方格是否具有更小的 G值。如果没有，不做任何操作。相反，如果G值更小，则把那个方格的父节点设为当前处理节点 ( 我们选中的方格 ) ，然后重新计算那个方格的 F 值和 G 值。
- 若当前处理节点的相邻格子不在Open List中，那么把它加入，并将它的父节点设置为该节点。
按照上述规则我们继续搜索，选择起点右边的方格作为当前处理节点。它的外框用蓝线打亮，被放入了close list 中。然后我们检查与它相邻的方格。它右侧的3个方格是墙壁，我们忽略。它左边的方格是起点，在 close list 中，我们也忽略。其他4个相邻的方格均在 open list 中，我们需要检查经由当前节点到达那里的路径是否更好。我们看看上面的方格，它现在的G值为14 ，如果经由当前方格到达那里， G值将会为20( 其中10为从起点到达当前方格的G值，此外还要加上从当前方格纵向移动到上面方格的G值10) ，因此这不是最优的路径。看图就会明白直接从起点沿对角线移动到那个方格比先横向移动再纵向移动要好。
当把4个已经在 open list 中的相邻方格都检查后，没有发现经由当前节点的更好路径，因此不做任何改变。接下来要选择下一个待处理的节点。因此再次遍历open list ，现在open list中只有 7 个方格了，我们需要选择F值最小的那个。这次有两个方格的F值都是54，选哪个呢？没什么关系。从速度上考虑，选择最后加入 open list 的方格更快。因此选择起点右下方的方格，如下图所示。
image
接下来把起点右下角F值为54的方格作为当前处理节点，检查其相邻的方格。我们发现它右边是墙（墙下面的一格也忽略掉，假定墙角不能直接穿越)，忽略之。这样还剩下 5 个相邻的方格。当前方格下面的 2 个方格还没有加入 open list ，所以把它们加入，同时把当前方格设为他们的父亲。在剩下的 3 个方格中，有 2 个已经在 close list 中 ( 一个是起点，一个是当前方格上面的方格，外框被加亮的 ) ，我们忽略它们。最后一个方格，也就是当前方格左边的方格，检查经由当前方格到达那里是否具有更小的 G 值。没有，因此我们准备从 open list 中选择下一个待处理的方格。
不断重复这个过程，直到把终点也加入到了 open list 中，此时如下图所示。注意在起点下方 2 格处的方格的父亲已经与前面不同了。之前它的G值是28并且指向它右上方的方格。现在它的 G 值为 20 ，并且指向它正上方的方格。这是由于在寻路过程中的某处使用新路径时G值更小，因此父节点被重新设置，G和F值被重新计算。
image
那么我们怎样得到实际路径呢？很简单，如下图所示，从终点开始，沿着箭头向父节点移动，直至回到起点，这就是你的路径。
image
A算法总结：*
1. 把起点加入 open list 。
2. 重复如下过程：
a. 遍历open list ，查找F值最小的节点，把它作为当前要处理的节点，然后移到close list中
b. 对当前方格的 8 个相邻方格一一进行检查，如果它是不可抵达的或者它在close list中，忽略它。否则，做如下操作：
□ 如果它不在open list中，把它加入open list，并且把当前方格设置为它的父亲
□ 如果它已经在open list中，检查这条路径 ( 即经由当前方格到达它那里 ) 是否更近。如果更近，把它的父亲设置为当前方格，并重新计算它的G和F值。如果你的open list是按F值排序的话，改变后你可能需要重新排序。
c. 遇到下面情况停止搜索：
□ 把终点加入到了 open list 中，此时路径已经找到了，或者
□ 查找终点失败，并且open list 是空的，此时没有路径。
3. 从终点开始，每个方格沿着父节点移动直至起点，形成路径。
根据算法描述，伪代码如下：
function A*(start, goal)
// The set of nodes already evaluated.
closedSet := {}
// The set of currently discovered nodes still to be evaluated. Initially, only the start node is known.
openSet := {start}
// For each node, which node it can most efficiently be reached from.
// If a node can be reached from many nodes, cameFrom will eventually contain the most efficient previous step.
cameFrom := the empty map
// For each node, the cost of getting from the start node to that node.
gScore := map with default value of Infinity
// The cost of going from start to start is zero.
gScore[start] := 0
// For each node, the total cost of getting from the start node to the goal
// by passing by that node. That value is partly known, partly heuristic.
fScore := map with default value of Infinity
// For the first node, that value is completely heuristic.
fScore[start] := heuristic_cost_estimate(start, goal)
while openSet is not empty
current := the node in openSet having the lowest fScore[] value
if current = goal
return reconstruct_path(cameFrom, current)
openSet.Remove(current)
closedSet.Add(current)
for each neighbor of current
if neighbor in closedSet
continue // Ignore the neighbor which is already evaluated.
// The distance from start to a neighbor
tentative_gScore := gScore[current] + dist_between(current, neighbor)
if neighbor not in openSet // Discover a new node
openSet.Add(neighbor)
else if tentative_gScore >= gScore[neighbor]
continue // This is not a better path.
// This path is the best until now. Record it!
cameFrom[neighbor] := current
gScore[neighbor] := tentative_gScore
fScore[neighbor] := gScore[neighbor] + heuristic_cost_estimate(neighbor, goal)
return failure
function reconstruct_path(cameFrom, current)
total_path := [current]
while current in cameFrom.Keys:
current := cameFrom[current]
total_path.append(current)
return total_path
根据上面的伪代码，用Python实现一个简单的最短路径搜寻。使用二维数组表示地图，其中1表示有障碍节点，0表示无障碍节点。
使用Spyder IDE可以在Variable explorer中查看和修改二维数组，数组中的值根据大小以不同颜色显示。将搜寻到的路径节点赋予一个较大的值，可以直观地看出从起点到终点的路径。
image
使用Pillow、OpenCV或Matplotlib等图像处理库，可以在自己绘制的图片上进行寻路：
import numpy
from heapq import heappush,heappop
def heuristic_cost_estimate(neighbor, goal):
x = neighbor[0] - goal[0]
y = neighbor[1] - goal[1]
return abs(x) + abs(y)
def dist_between(a, b):
return (b[0] - a[0]) ** 2 + (b[1] - a[1]) ** 2
def reconstruct_path(came_from, current):
path = [current]
while current in came_from:
current = came_from[current]
path.append(current)
return path
# astar function returns a list of points (shortest path)
def astar(array, start, goal):
directions = [(0,1),(0,-1),(1,0),(-1,0),(1,1),(1,-1),(-1,1),(-1,-1)] # 8个方向
close_set = set()
came_from = {}
gscore = {start:0}
fscore = {start:heuristic_cost_estimate(start, goal)}
openSet = []
heappush(openSet, (fscore[start], start)) # 往堆中插入一条新的值
# while openSet is not empty
while openSet:
# current := the node in openSet having the lowest fScore value
current = heappop(openSet)[1] # 从堆中弹出fscore最小的节点
if current == goal:
return reconstruct_path(came_from, current)
close_set.add(current)
for i, j in directions: # 对当前节点的 8 个相邻节点一一进行检查
neighbor = current[0] + i, current[1] + j
## 判断节点是否在地图范围内，并判断是否为障碍物
if 0 <= neighbor[0] < array.shape[0]:
if 0 <= neighbor[1] < array.shape[1]:
if array[neighbor[0]][neighbor[1]] == 1: # 1为障碍物
continue
else:
# array bound y walls
continue
else:
# array bound x walls
continue
# Ignore the neighbor which is already evaluated.
if neighbor in close_set:
continue
# The distance from start to a neighbor via current
tentative_gScore = gscore[current] + dist_between(current, neighbor)
if neighbor not in [i[1] for i in openSet]: # Discover a new node
heappush(openSet, (fscore.get(neighbor, numpy.inf), neighbor))
elif tentative_gScore >= gscore.get(neighbor, numpy.inf): # This is not a better path.
continue
# This path is the best until now. Record it!
came_from[neighbor] = current
gscore[neighbor] = tentative_gScore
fscore[neighbor] = tentative_gScore + heuristic_cost_estimate(neighbor, goal)
return False
if __name__ == "__main__":
nmap = numpy.array([
[0,0,0,0,0,0,0,0,0,0,0,0,0,0],
[1,1,1,1,1,1,1,1,1,1,1,1,0,1],
[0,0,0,0,0,0,0,0,0,0,0,0,0,0],
[1,0,1,1,1,1,1,1,1,1,1,1,1,1],
[0,0,0,0,0,0,0,0,0,0,0,0,0,0],
[1,1,1,1,1,1,1,1,1,1,1,1,0,1],
[0,0,0,0,0,0,0,0,0,0,0,0,0,0],
[1,0,1,1,1,1,1,1,1,1,1,1,1,1],
[0,0,0,0,0,0,0,0,0,0,0,0,0,0],
[1,1,1,1,1,1,1,1,1,1,1,1,0,1],
[0,0,0,0,0,0,0,0,0,0,0,0,0,0]])
path = astar(nmap, (0,0), (10,13))
for i in range(len(path)):
nmap[path[i]] = 100
image
上面产生的路径贴着障碍物边缘，如果对实际机器人或者游戏中的角色进行路径规划，要考虑其实际大小，将地图上的障碍物尺寸扩大（inflate），避免与障碍物发生碰撞。
以上转载自 https://www.cnblogs.com/21207-iHome/p/6048969.html
我的实现实例：
/**
* 生成移动路径
* 使用 a star 算法 f() = g() + h()
* g():初始位置沿着已生成的路径到指定待检测格子的移动开销 -- 用 移动格子数 * 单位格子数开销
* h():到目标节点的估计移动开销 -- 用 曼哈顿距离
* 障碍点直接进closed列表
* 只存在上下左右的临边移动，没有斜向的移动
*
* @param {site} __srcSite__ 原点
* @param {site} __tarSite__ 目标点
* @param {site} __reachableSites__ 全部可移动的点
* @param {site} __obstacleSites__ 障碍点
* @return {[site]} 结果路径
*/
XXX.prototype.createMovingPath = function (__srcSite__, __tarSite__, __reachableSites__) {
//
// 1. 把障碍点加入到closedArr中。
// 2. 从起点__srcSite__开始，把__srcSite__设置为当前节点，把它加入到openArr中。
// 3. 把由__srcSite__可达的相邻节点加入到openArr中，并把__srcSite__设置为他们的父节点。
// 4. 把__srcSite__从openArr中移除，并把__srcSite__加入到closedArr中。
// （所用待判断的点都从openArr中取，closedArr中的点都不需要再关注。）
// 5. 从openArr取f值最小的节点设置为当前节点，并将这个点从openArr中移除，并把它加入到closedArr和pendingMovingArr中。
// 6. 若当前处理节点的相邻的节点已经在openArr中，计算经由当前处理节点到达那个方格是否具有更小的g值，如果没有，不做任何操作。
// 相反，如果g值更小，则把那个节点的父节点设为当前处理节点(我们选中的节点)，然后重新计算那个节点的g值和f值。
// 7. 若当前处理节点的相节点不在openArr中，那么把它加入到openArr中，并将它的父节点设置为该节点。
// 8. 循环重复这个过程（ 2 ~ 7 ），直到把终点也加入到了openArr中。
// 9. 在pendingMovingArr中，以__tarSite__为起始位置每个节点的父节点，直至遍历到__srcSite__，形成最终最短路径，计算终了。
//
// 判断目标节点是否在可选节点中，如果不在可以直接返回。
let isTarInReachable = false;
for (const iterator of __reachableSites__) {
if (iterator.x == __tarSite__.x && iterator.y == __tarSite__.y) {
isTarInReachable = true;
break;
}
}
if (!isTarInReachable) {
return null;
}
// 开放列表
let openArr = [];
// 关闭列表
let closedArr = [];
// 获取地图数据
let mapData = this._battleAPI._battleData.getMapData();
// 将障碍点加入到closedArr中
for (let i = 0; i < mapData.length; i++) {
const colElements = mapData[i];
for (let j = 0; j < colElements.length; j++) {
const element = colElements[j];
if (element.type == 1 || element.actor != undefined || element.actor != null) {
closedArr.push(element);
}
}
}
// 初始化起始点信息
__srcSite__.parent = __srcSite__;
__srcSite__.gV = 0;
__srcSite__.hV = Math.abs(__tarSite__.x - __srcSite__.x) + Math.abs(__tarSite__.y - __srcSite__.y);
__srcSite__.fV = __srcSite__.gV + __srcSite__.hV
// 将当前节点游标指向起始点
let curSite = __srcSite__;
// 待定的移动路径节点
let pendingMovingArr = new Array();
// 起始点加入待定的移动路径节点
pendingMovingArr.push(curSite);
while (curSite.x != __tarSite__.x || curSite.y != __tarSite__.y) {
// 当前节点加入关闭列表
closedArr.push(curSite);
// 当前节点四周 reachableSites 便利顺序 ：左上右下
let tempAroundSites = new Array();
tempAroundSites.push({ x: curSite.x - 1, y: curSite.y });
tempAroundSites.push({ x: curSite.x, y: curSite.y + 1 });
tempAroundSites.push({ x: curSite.x + 1, y: curSite.y });
tempAroundSites.push({ x: curSite.x, y: curSite.y - 1 });
// 循环判断遍历当前节点四周的节点，找出哪个节点可作为下步的节点
for (const iterator of tempAroundSites) {
// 判断tempAroundSites中的节点是否可以加入到openArr中（这个点是否可选）
// 条件1：tempAroundSites中的节点在可选节点中
// 条件2：tempAroundSites中的节点不在关闭节点中
let pushFlag = false;
// 条件1：tempAroundSites中的节点在可选节点中
for (const iteratorReachable of __reachableSites__) {
if (iteratorReachable.x == iterator.x && iteratorReachable.y == iterator.y) {
pushFlag = true;
break;
}
}
// 条件2：tempAroundSites中的节点不在关闭节点中
for (const iteratorClosed of closedArr) {
if (iteratorClosed.x == iterator.x && iteratorClosed.y == iterator.y) {
pushFlag = false;
break;
}
}
// 不可选就直接判断下一个节点
if (!pushFlag) {
continue;
}
// 判断tempAroundSites中的节点是否已经在openArr中
let isInOpenArr = false
for (const iteratorOpen of openArr) {
if (iteratorOpen.x == iterator.x && iteratorOpen.y == iterator.y) {
isInOpenArr = true;
break;
}
}
if (isInOpenArr) {
// 若当前处理节点的相邻的节点已经在openArr中，计算经由当前处理节点到达那个方格是否具有更小的g值，如果没有，不做任何操作。
// 相反，如果g值更小，则把那个节点的父节点设为当前处理节点(我们选中的节点)，然后重新计算那个节点的g值和f值。
if (iterator.gV != null && curSite.gV + 1 < iterator.gV) {
iterator.parent = curSite;
iterator.gV = curSite.gV + 1;
iterator.hV = Math.abs(__tarSite__.x - iterator.x) + Math.abs(__tarSite__.y - iterator.y);
iterator.fV = iterator.gV + iterator.hV;
}
} else {
// 若当前处理节点的相节点不在openArr中，那么把它加入到openArr中，并将它的父节点设置为该节点。
iterator.parent = curSite;
iterator.gV = curSite.gV + 1;
iterator.hV = Math.abs(__tarSite__.x - iterator.x) + Math.abs(__tarSite__.y - iterator.y);
iterator.fV = iterator.gV + iterator.hV;
openArr.push(iterator);
}
}
// 取openArr中f值最小的节点
let minFVSite = {
fV: 9999,
};
for (const iterator of openArr) {
if (iterator.fV <= minFVSite.fV) {
minFVSite = iterator;
}
}
// 将当前节点设置为这个f值最小的点
curSite = minFVSite;
// 把当前节点加入到待选节点中，并从openArr移除，同时在下一次循环开始时会把当前节点加入到closedArr中。
for (const key in openArr) {
if (openArr.hasOwnProperty(key)) {
const element = openArr[key];
if (element.x == curSite.x && element.y == curSite.y) {
pendingMovingArr.push(element);
openArr.splice(key, 1);
break;
}
}
}
}
// 最终最短路径
let movingArr = new Array();
// 用于找父节点的临时变量，pendingMovingArr的最后一个就应该为终点节点，因为循环是在判断是终点的时候停止的。
let tempRTSite = pendingMovingArr.pop();
// 将终点节点加入到最短路径结果中
movingArr.push(tempRTSite);
// 循环找临时变量的父节点，判断是否已经为起始点。
while (tempRTSite.x != __srcSite__.x || tempRTSite.y != __srcSite__.y) {
movingArr.push(tempRTSite.parent);
tempRTSite = tempRTSite.parent;
}
return movingArr;
};
只是一个小算法，并不关键，抄用请留情面。
网友评论