在运动规划的研究场景里很多已经存在的经典算法，在运动规划的入门阶段绕不过去的五种基础算法分别有：Dijkstra、Astar（A*）、PRM（概率路线图法Probabilistic Road Maps）、RRT（重复探索随机树 Rapidly Exploring Random Trees）、人工势场法（Artificial Potential Fields）

笔者将会更新这个“运动规划入门”系列，尽我所能详细地给小伙伴们分别讲解这五种算法。由于笔者能力有限，讲解内容有任何错误的话，请务必指正。

Dijkstra与“图（Graph）”不得不说的那些事

“图”是一种用于描述对象和对象之间的数学方法。如下图所示，这是一个具有7个节点（node）的无向图，节点和节点之间的连线称为边（edge），每条边都具备一定的权重（weight），“图”在运动规划中是一个十分重要的数学工具。

由于这样子更加直观，所以通常我们都会使用图来讲解Dijkstra的算法原理。从贴合直觉的角度来描述图的话，举个例子：A，B，C，D，E，F，G分别是7个城镇，城镇和城镇之间有些铺设了高速公路，有些则没有，这就是“边”。假设我们现在要找出从城镇A到城镇G之间路程最短的路线，而每段高速公路的路程都可能不一样，那么我们就把路程看为权重或者说是代价（cost），很明显我们就是要找出一条总代价（总权重）最小的路线。再假设，我们现在不想找最短路线了，我想找最经济的路线（路费最低），那么我们就会将费用视为代价，并尝试找出总代价最低的路线。

Dijkstra基本原理图解

ok，我们就用下面这个图来距离Dijkstra算法，假设起点是A，终点是G，每条边具备不同的代价值，我们的任务就是找出从A到G代价最小的路径。每个节点还具备两个属性值，一个是node_cost，这个node_cost记录了当前节点到起点的已知的最小总代价值，这个node_cost会在算法迭代过程中由于规划出来的路径不相同而被更新成不同的值。另一个属性值是parent，parent表明了根据当下规划出来的路径，该节点对应的父节点，比如说：节点F的父节点可能是B也可能是D也可能是E，在完全取决于我们选择哪条路径。

在示意图中圆圈中的“C，1，A”表示：该节点是C，按照当前规划的路径，C节点的父节点是A，且C节点到起点的代价值为1。

为了方便起见，我们用绿色代表起点，黄色代表终点，红色代表该点已经完成遍历，蓝色节点表明该节点尚未完成遍历。白色代表尚未纳入考虑范围之内。在Dijkstra算法开始之前，我们将起点的node_cost设为0，其余节点的node_cost设为无穷（infinite），所有节点的parent均设置为空。未完成列表里只有起点。已完成列表为空。

Dijkstra是一个迭代的过程，在每轮迭代的开始，我们会找出未完成遍历列表中的所有节点中node_cost最小的那个节点并设置为current_node，并将这个被选中的current_node移除未完成列表，设置为已完成遍历。每轮迭代中，我们都会对current_node的所有未完成遍历的相邻节点进行单独的判定，判定相邻节点当前的node_cost是否大于current_node的node_cost与连接边的代价值之和。如果大于的话，将相邻节点的node_cost更新为current_node的node_cost加上连接边的代价值，并且将相邻节点的parent更新为current_node，如果这个相邻节点尚未被加入未完成列表，则将其添加进列表中。如果小于的话，则保持原样，不执行任何操作。

举个例子：在第一轮迭代开始时，由于A是node_cost最小的节点，所以A被设置为current_node，并被标记为已完成遍历，接着我们会从A开始进行第一轮搜索，我们可以看到与A相邻的节点分别有B，C，D。对于A的相邻节点B来说，当前B的node_cost = inf，A的node_cost = 0，连接边的cost = 2。很明显B.node_cost > A.node_cost + edge.cost，那么我们就将B的node_cost更新为2，B的parent更新为A。同样的道理，对其余两个相邻节点进行判定。结果如下图所示。

那么第二轮迭代我们在未完成列表中寻找具备最小node_cost的节点，很明显最小的是节点C，将C移出未完成列表后，我们就对C的相邻节点进行单独判定。完成判定后结果如下：

以此类推，第三轮迭代我们选择B为current_node，则的结果为：

第三轮迭代我们选择D为current_node，则的结果如下，我们注意到了F的node_cost被更新为3，这就意味着找到了一条从起点A到达F花费代价更低的路径，这便是Dijkstra算法的精髓所在——每次迭代都在产生规划出代价更低的路径。你品，你细品。

在第四次迭代中，我们毫无悬念地选择了E作为current_node，我们发现G是E的相邻节点，那么恭喜，我们成功找到了A到G的路径。但是有个问题值得思考：Dijkstra找到的最短路径是唯一的吗？

当我们完成规划以后该如何根据规划结果找到那条最短路径呢？这时我们之前一直没有发挥作用的属性值parent就要大显神威了，这一招有点像我们成语讲的“顺藤摸瓜”，我们从终点开始，根据当前节点的parent值一个一个往回走就能回到起点了。这也就把路径描绘出来了。

Dijkstra的matlab实现

地图表示法：网络占用法

表示地图以及机器人在地图中的位置的方式有很多种，比如可以把地图表示在笛卡尔坐标系中，比如一个二维的地图：坐标[x, y]表明机器人在地图中的位置，地图中的机器人本体和障碍物分别用一个个多边形表示。这种表示方式的优点就是简单，我们可以只使用很少的信息就能表达出一张完备的地图。但是优点明显的同时，其缺点也很明显。假设我们要对机器人和障碍物之间进行碰撞检测，那么我们需要将代表机器人和障碍物的多边形进行判定，判定其是否产生重叠。实际上我们在实际操作时往往将多边形视为由多个三角形组成，因为三角形和三角形之间的重叠检测比较容易。但是这种检测方式要求对三角形的每条边都进行计算测试，如果是一张小地图的话那么计算压力还不算太大，但是如果地图稍微大一点，或者障碍物稍微多一点的话，那么这种地图表示法所需的计算成本就会大幅度上升。

所以在此，我们选择另一种地图表示法：网络占用法。这种方法会将整个地图区域分割为若干个单位网络栅格，每个栅格的最基本状态有非占用（free space）和占用（obstacle）。

非占用栅格，即机器人可以自由通过的栅格，比如下图中的白色栅格。占用栅格也就是机器人无法通过的栅格，即障碍物，比如下图中的黑色栅格。单位网络栅格的大小决定了地图的分辨率，单元格的大小往往根据应用场景的不同设置为不同的数值。当区域扩大或者单元格缩小时，地图所需的存储空间就会增大。但是对于现代计算机的硬件水平而言，这种方法是非常可行的。

按照惯例我们需要做一些必要的假设：机器人在地图网络中活动，且只占用一格的空间。机器人可以移动到任意相邻非占用栅格。

地图生成

前面说网络占用法的时候老是栅格来栅格去的，那么我们究竟该如何在matlab中表示一张占用网络地图呢？其实说破不值一毛钱，在matlab中一个二维的占用网络地图就是一个二维的矩阵，如下图所示：在这张地图中，我们用0来表示非占用栅格，1表示占用栅格

在Robotic Tool Box（后文简称RTB）里有专门的函数可以帮助我们生成一张地图，毕竟如果要你手动来一个个栅格单独配置的话实在是太麻烦了。

RTB里构建地图的函数为makemap()，比如我们生成一个20*20的地图，我们可以使用命令makemap(20)，执行命令后将会弹出一个网络图，以及帮助信息。

可以看到它的帮助信息十分的简单明确，你可以用鼠标左键在网络图中点击或者拖动来创建图案，或者选择figure窗口并在键盘上输入快捷命令来生成多边形或者圆形等等。

比如我在这画了一个圆（分辨率比较低。。。）

然后回车，就能得到生成好的map了，是不是巨方便

最后将生成的map保存下来，就能在自己的代码中重复使用这个地图了。

Dijkstra算法流程

在代码实现中，NodeCostList除了记录总代价值之外同时兼备未完成列表的功能，每轮迭代中，我们在获得一个新的current_node之后，便会马上将current_node的NodeCost设置为Inf，确保在下一轮迭代中，由于Inf比任何值都大，所以只要NodeCostList至少出现一个非Inf的值，那么这个节点不会被再次选中。

在流程图中未出现的变量是我们的占用地图map，在实际代码中，map的栅格属性不单单只有占用和非占用两者，为了演示和实现的便利，我们一共引入了7种状态值，分别是非占用、占用、起点、终点、已完成遍历、未完成遍历以及路径（路径状态只有在完成Dijkstra算法后才被赋予）

核心代码

这部分就没什么好讲的了，直接放代码吧，需要完整matlab源码的小伙伴可以在评论区找我要蛤~

while true
	[min_node_cost, current_node_index] = min(node_cost_list(:));
	if(min_node_cost == inf || current_node_index == destination_index)
		plan_succeeded = true;
		break;
    end
    node_cost_list(current_node_index) = inf;
    map(current_node_index) = Finished;
	[x,y] = ind2sub(MapSize, current_node_index);
    for k = 0:3	% four direction
		if(k == 0)
		    adjacent_node = [x-1,y];
		elseif (k == 1)
			adjacent_node = [x+1,y];
		elseif (k == 2)
			adjacent_node = [x,y-1];
		elseif(k == 3)
			adjacent_node = [x,y+1];
		end
		
		if((adjacent_node(1) > 0 && adjacent_node(1) <= MapSize(1)) && (adjacent_node(2) > 0 && adjacent_node(2) <= MapSize(2))) % make sure the adjacent_node don't exceeds the map
            if(map(adjacent_node(1),adjacent_node(2)) ~= Obstacle && map(adjacent_node(1),adjacent_node(2)) ~= Finished)
                if(node_cost_list(adjacent_node(1),adjacent_node(2)) > min_node_cost + 1)
                    node_cost_list(adjacent_node(1),adjacent_node(2)) = min_node_cost + 1;
                    if(map(adjacent_node(1),adjacent_node(2)) == Origin)
                        parent_list(adjacent_node(1),adjacent_node(2)) = 0;	% Set the parent 0 if adjacent_node is the origin.
                    else
                        parent_list(adjacent_node(1),adjacent_node(2)) = current_node_index; % Set the parent current_node_index
                    end
                    map(adjacent_node(1),adjacent_node(2)) = Unfinished; % Mark the adjacent_node as unfinished
                end
            end
        end
	end
end

最终的演示效果，可以看到方块就像洪水一样向四周扩散，直到找到终点为止。当然如果当前的规划问题是无解的话Dijkstra也会返回错误警告（或者空结果）。

Dijkstra的优缺点

Dijkstra的优点我觉得其实就是算法简单，像洪水一样向四周无脑扩散就完事儿了。在上面的演示中其实看起来效果还是不错的，但是请注意，这只是一张20*20的地图而已，那么假如我们把地图扩大到100*100的时候会是什么效果呢？

是这样的：。。。。

是不是你都没耐心看下去了，我都没耐心录完全程。所以一旦地图扩大，Dijkstra的无脑扩散就显得力不从心了。下一篇笔者将为你解说A*是如何解决这个问题的。