决策规划，全局路径规划常用算法
正菜之前，我们先来了解一下图（包括有向图和无向图）的概念。图是图论中的基本概念，用于表示物体与物体之间存在某种关系的结构。在图中，物体被称为节点或顶点，并用一组点或小圆圈表示。节点间的关系称作边，可以用直线或曲线来表示节点间的边。
如果给图的每条边规定一个方向，那么得到的图称为有向图，其边也称为有向边，如图10所示。在有向图中，与一个节点相关联的边有出边和入边之分，而与一个有向边关联的两个点也有始点和终点之分。相反，边没有方向的图称为无向图。
图10 有向图示例
数学上，常用二元组g =（v，e）来表示其数据结构，其中集合v称为点集，e称为边集。对于图6所示的有向图，v可以表示为{a，b，c，d，e，f，g}，e可以表示为{，，，，，，}。表示从顶点a发向顶点b的边，a为始点，b为终点。
在图的边中给出相关的数，称为权。权可以代表一个顶点到另一个顶点的距离、耗费等，带权图一般称为网。
在全局路径规划时，通常将图11所示道路和道路之间的连接情况，通行规则，道路的路宽等各种信息处理成有向图，其中每一个有向边都是带权重的，也被称为路网（route network graph）。
图11 道路连接情况
那么，全局路径的规划问题就变成了在路网中，搜索到一条最优的路径，以便可以尽快见到那个心心念念的她，这也是全局路径规划算法最朴素的愿望。而为了实现这个愿望，诞生了dijkstra和a*两种最为广泛使用的全局路径搜索算法。
dijkstra算法
戴克斯特拉算法（dijkstra’s algorithm）是由荷兰计算机科学家edsger w. dijkstra在1956年提出，解决的是有向图中起点到其他顶点的最短路径问题。
假设有a、b、c、d、e、f五个城市，用有向图表示如图12，边上的权重代表两座城市之间的距离，现在我们要做的就是求出起点a城市到其它城市的最短距离。
图12 五个城市构建的有向图
用dijkstra算法求解步骤如下：
（1）创建一个二维数组e来描述顶点之间的距离关系，如图13所示。e[b][c]表示顶点b到顶点c的距离。自身之间的距离设为0，无法到达的顶点之间设为无穷大。
图13 顶点之间的距离关系
（2）创建一个一维数组dis来存储起点a到其余顶点的最短距离。一开始我们并不知道起点a到其它顶点的最短距离，一维数组dis中所有值均赋值为无穷大。接着我们遍历起点a的相邻顶点，并将与相邻顶点b和c的距离3（e[a][b]）和10（e[a][c]）更新到dis[b]和dis[c]中，如图14所示。这样我们就可以得出起点a到其余顶点最短距离的一个估计值。
图14 dis经过一次遍历后得到的值
（3）接着我们寻找一个离起点a距离最短的顶点，由数组dis可知为顶点b。顶点b有两条出边，分别连接顶点c和d。因起点a经过顶点b到达顶点c的距离8（e[a][b] + e[b][c] = 3 + 5）小于起点a直接到达顶点c的距离10，因此dis[c]的值由10更新为8。同理起点a经过b到达d的距离5（e[a][b] + e[b][d] = 3 + 2）小于初始值无穷大，因此dis[d]更新为5，如图15所示。
图15 dis经过第二次遍历后得到的值
（4）接着在剩下的顶点c、d、e、f中，选出里面离起点a最近的顶点d，继续按照上面的方式对顶点d的所有出边进行计算，得到dis[e]和dis[f]的更新值，如图16所示。
图16 dis经过第三次遍历后得到的值
（5）继续在剩下的顶点c、e、f中，选出里面离起点a最近的顶点c，继续按照上面的方式对顶点c的所有出边进行计算，得到dis[e]的更新值，如图17所示。
图17 dis经过第四次遍历后得到的值
（6）继续在剩下的顶点e、f中，选出里面离起点a最近的顶点e，继续按照上面的方式对顶点e的所有出边进行计算，得到dis[f]的更新值，如图18所示。
图18 dis经过第五次遍历后得到的值
（6）最后对顶点f所有点出边进行计算，此例中顶点f没有出边，因此不用处理。至此，数组dis中距离起点a的值都已经从“估计值”变为了“确定值”。
基于上述形象的过程，dijkstra算法实现过程可以归纳为如下步骤：
（1）将有向图中所有的顶点分成两个集合p和q，p用来存放已知距离起点最短距离的顶点，q用来存放剩余未知顶点。可以想象，一开始，p中只有起点a。同时我们创建一个数组flag[n]来记录顶点是在p中还是q中。对于某个顶点n，如果flag[n]为1则表示这个顶点在集合p中，为1则表示在集合q中。
（2）起点a到自己的最短距离设置为0，起点能直接到达的顶点n，dis[n]设为e[a][n]，起点不能直接到达的顶点的最短路径为设为∞。
（3）在集合q中选择一个离起点最近的顶点u（即dis[u]最小）加入到集合p。并计算所有以顶点u为起点的边，到其它顶点的距离。例如存在一条从顶点u到顶点v的边，那么可以通过将边u->v添加到尾部来拓展一条从a到v的路径，这条路径的长度是dis[u]+e[u][v]。如果这个值比目前已知的dis[v]的值要小，我们可以用新值来替代当前dis[v]中的值。
（4）重复第三步，如果最终集合q结束，算法结束。最终dis数组中的值就是起点到所有顶点的最短路径。
a*算法
1968年，斯坦福国际研究院的peter e. hart, nils nilsson以及bertram raphael共同发明了a*算法。a*算法通过借助一个启发函数来引导搜索的过程，可以明显地提高路径搜索效率。
下文仍以一个实例来简单介绍a*算法的实现过程。如图19所示，假设小马要从a点前往b点大榕树底下去约会，但是a点和b点之间隔着一个池塘。为了能尽快提到达约会地点，给姑娘留下了一个守时踏实的好印象，我们需要给小马搜索出一条时间最短的可行路径。
图19 约会场景示意图
a*算法的第一步就是简化搜索区域，将搜索区域划分为若干栅格。并有选择地标识出障碍物不可通行与空白可通行区域。一般地，栅格划分越细密，搜索点数越多，搜索过程越慢，计算量也越大；栅格划分越稀疏，搜索点数越少，相应的搜索精确性就越低。
如图20所示，我们在这里将要搜索的区域划分成了正方形（当然也可以划分为矩形、六边形等）的格子，图中蓝色格子代表a点（小马当前的位置），紫色格子代表b点（大榕树的位置），灰色格子代表池塘。同时我们可以用一个二维数组s来表示搜素区域，数组中的每一项代表一个格子，状态代表可通行和不可通行。
图20 经过简化后的搜索区域
接着我们引入两个集合openlist和closelist，以及一个估价函数f = g + h。openlist用来存储可到达的格子，closelist用来存储已到达的格子。g代表从起点到当前格子的距离，h表示在不考虑障碍物的情况下，从当前格子到目标格子的距离。f是起点经由当前格子到达目标格子的总代价，值越小，综合优先级越高。
g和h也是a*算法的精髓所在，通过考虑当前格子与起始点的距离，以及当前格子与目标格子的距离来实现启发式搜索。对于h的计算，又有两种方式，一种是欧式距离，一种是曼哈顿距离。
欧式距离用公式表示如下，物理上表示从当前格子出发，支持以8个方向向四周格子移动（横纵向移动+对角移动）。
曼哈顿距离用公式表示如下，物理上表示从当前格子出发，支持以4个方向向四周格子移动（横纵向移动）。这是a*算法最常用的计算h值方法，本文h值的计算也采用这种方法。
现在我们开始搜索，查找最短路径。首先将起点a放入到openlist中，并计算出此时openlist中f值最小的格子作为当前方格移入到closelist中。由于当前openlist中只有起点a这个格子，所以将起点a移入closelist，代表这个格子已经检查过了。
接着我们找出当前格子a上下左右所有可通行的格子，看它们是否在openlist当中。如果不在，加入到openlist中计算出相应的g、h、f值，并把当前格子a作为它们的父节点。本例子，我们假设横纵向移动代价为10，对角线移动代价为14。
我们在每个格子上标出计算出来的f、g、h值，如图21所示，左上角是f，左下角是g，右下角是h。通过计算可知s[3][2]格子的f值最小，我们把它从openlist中取出，放到closelist中。
图21 第一轮计算后的结果
接着将s[3][2]作为当前格子，检查所有与它相邻的格子，忽略已经在closelist或是不可通行的格子。如果相邻的格子不在openlist中，则加入到openlist，并将当前方格子s[3][2]作为父节点。
已经在openlist中的格子，则检查这条路径是否最优，如果非最优，不做任何操作。如果g值更小，则意味着经由当前格子到达openlist中这个格子距离更短，此时我们将openlist中这个格子的父节点更新为当前节点。
对于当前格子s[3][2]来说，它的相邻5个格子中有4个已经在openlist，一个未在。对于已经在openlist中的4个格子，我们以它上面的格子s[2][2]举例，从起点a经由格子s[3][2]到达格子s[2][2]的g值为20（10+10）大于从起点a直接沿对角线到达格子s[2][2]的g值14。显然a经由格子s[3][2]到达格子s[2][2]不是最优的路径。当把4个已经在openlist 中的相邻格子都检查后，没有发现经由当前方格的更好路径，因此我们不做任何改变。
对于未在openlist的格子s[2][3]（假设小马可以斜穿墙脚），加入openlist中，并计算它的f、g、h值，并将当前格子s[3][2]设置为其父节点。经历这一波骚操作后，openlist中有5个格子，我们需要从中选择f值最小的那个格子s[2][3]，放入closelist中，并设置为当前格子，如图22所示。
图22 第二轮计算后的结果
重复上面的故事，直到终点也加入到openlist中。此时我们以当前格子倒推，找到其父节点，父节点的父节点……，如此便可搜索出一条最优的路径，如图23中红色圆圈标识。
图23 最后计算得到的结果
基于上述形象的过程，a*算法实现过程可以归纳为如下步骤：
（1）将搜索区域按一定规则划分，把起点加入openlist。
（2）在openlist中查找f值最小的格子，将其移入closelist，并设置为当前格子。
（3）查找当前格子相邻的可通行的格子，如果它已经在openlist中，用g值衡量这条路径是否更好。如果更好，将该格子的父节点设置为当前格子，重新计算f、g值，如果非更好，不做任何处理；如果不在openlist中，将它加入openlist中，并以当前格子为父节点计算f、g、h值。
（4）重复步骤（2）和步骤（3），直到终点加入到openlist中。
两种算法比较
dijkstra算法的基本思想是“贪心”，主要特点是以起点为中心向周围层层扩展，直至扩展到终点为止。通过dijkstra算法得出的最短路径是最优的，但是由于遍历没有明确的方向，计算的复杂度比较高，路径搜索的效率比较低。且无法处理有向图中权值为负的路径最优问题。
a*算法将dijkstra算法与广度优先搜索（breadth-first-search，bfs）算法相结合，并引入启发函数（估价函数），大大减少了搜索节点的数量，提高了搜索效率。但是a*先入为主的将最早遍历路径当成最短路径，不适用于动态环境且不太适合高维空间，且在终点不可达时会造成大量性能消耗。
图24是两种算法路径搜索效率示意图，左图为dijkstra算法示意图，右图为a*算法示意图，带颜色的格子表示算法搜索过的格子。由图24可以看出，a*算法更有效率，手术的格子更少。
图24 dijkstra算法和a*算法搜索效率对比图（图片来源：https://mp.weixin.qq.com/s/myu204uq3tfuikhgd3oefw）
特斯拉研发运动检测传感器，可检测留在车内的儿童防中暑
什么是隔离数字输入
东芝技术发展战略,转型分为三点
基于nRF24E1与TMC2023的汽车防撞系统
继电器的工作原理和特性详细解说
决策规划，全局路径规划常用算法
东大金智科技10G DWDM XFP光模块特征
8848手机宣布关停加密通话功能
荣耀正式从华为独立，定一个国内手机第一的小目标
大华存储亮相中东GITEX展 获评2022 ICT年度新兴存储供应商
海康机器人LMR×读码产品实现物料搬运自动化及数字化
反相同相比例运算电路
R语言的前世今生（起源、趋势、优势）
618前好福利，联想打印机又到好价
牵手首汽 奇瑞新能源迎里程碑式发展
2015年五大显示技术 明年谁唱主角？
智能供配电设备有哪些，它的作用是怎样的
动力电池需求急剧增长 “电池荒“成最大瓶颈
降低Clock Uncertainty流程
原创 8曲面智慧手机荣耀MAGIC拆机：传感器众多，做工细致！
如果给图的每条边规定一个方向，那么得到的图称为有向图，其边也称为有向边，如图10所示。在有向图中，与一个节点相关联的边有出边和入边之分，而与一个有向边关联的两个点也有始点和终点之分。相反，边没有方向的图称为无向图。
图10 有向图示例
数学上，常用二元组g =（v，e）来表示其数据结构，其中集合v称为点集，e称为边集。对于图6所示的有向图，v可以表示为{a，b，c，d，e，f，g}，e可以表示为{，，，，，，}。表示从顶点a发向顶点b的边，a为始点，b为终点。
在图的边中给出相关的数，称为权。权可以代表一个顶点到另一个顶点的距离、耗费等，带权图一般称为网。
在全局路径规划时，通常将图11所示道路和道路之间的连接情况，通行规则，道路的路宽等各种信息处理成有向图，其中每一个有向边都是带权重的，也被称为路网（route network graph）。
图11 道路连接情况
那么，全局路径的规划问题就变成了在路网中，搜索到一条最优的路径，以便可以尽快见到那个心心念念的她，这也是全局路径规划算法最朴素的愿望。而为了实现这个愿望，诞生了dijkstra和a*两种最为广泛使用的全局路径搜索算法。
dijkstra算法
戴克斯特拉算法（dijkstra’s algorithm）是由荷兰计算机科学家edsger w. dijkstra在1956年提出，解决的是有向图中起点到其他顶点的最短路径问题。
假设有a、b、c、d、e、f五个城市，用有向图表示如图12，边上的权重代表两座城市之间的距离，现在我们要做的就是求出起点a城市到其它城市的最短距离。
图12 五个城市构建的有向图
用dijkstra算法求解步骤如下：
（1）创建一个二维数组e来描述顶点之间的距离关系，如图13所示。e[b][c]表示顶点b到顶点c的距离。自身之间的距离设为0，无法到达的顶点之间设为无穷大。
图13 顶点之间的距离关系
（2）创建一个一维数组dis来存储起点a到其余顶点的最短距离。一开始我们并不知道起点a到其它顶点的最短距离，一维数组dis中所有值均赋值为无穷大。接着我们遍历起点a的相邻顶点，并将与相邻顶点b和c的距离3（e[a][b]）和10（e[a][c]）更新到dis[b]和dis[c]中，如图14所示。这样我们就可以得出起点a到其余顶点最短距离的一个估计值。
图14 dis经过一次遍历后得到的值
（3）接着我们寻找一个离起点a距离最短的顶点，由数组dis可知为顶点b。顶点b有两条出边，分别连接顶点c和d。因起点a经过顶点b到达顶点c的距离8（e[a][b] + e[b][c] = 3 + 5）小于起点a直接到达顶点c的距离10，因此dis[c]的值由10更新为8。同理起点a经过b到达d的距离5（e[a][b] + e[b][d] = 3 + 2）小于初始值无穷大，因此dis[d]更新为5，如图15所示。
图15 dis经过第二次遍历后得到的值
（4）接着在剩下的顶点c、d、e、f中，选出里面离起点a最近的顶点d，继续按照上面的方式对顶点d的所有出边进行计算，得到dis[e]和dis[f]的更新值，如图16所示。
图16 dis经过第三次遍历后得到的值
（5）继续在剩下的顶点c、e、f中，选出里面离起点a最近的顶点c，继续按照上面的方式对顶点c的所有出边进行计算，得到dis[e]的更新值，如图17所示。
图17 dis经过第四次遍历后得到的值
（6）继续在剩下的顶点e、f中，选出里面离起点a最近的顶点e，继续按照上面的方式对顶点e的所有出边进行计算，得到dis[f]的更新值，如图18所示。
图18 dis经过第五次遍历后得到的值
（6）最后对顶点f所有点出边进行计算，此例中顶点f没有出边，因此不用处理。至此，数组dis中距离起点a的值都已经从“估计值”变为了“确定值”。
基于上述形象的过程，dijkstra算法实现过程可以归纳为如下步骤：
（1）将有向图中所有的顶点分成两个集合p和q，p用来存放已知距离起点最短距离的顶点，q用来存放剩余未知顶点。可以想象，一开始，p中只有起点a。同时我们创建一个数组flag[n]来记录顶点是在p中还是q中。对于某个顶点n，如果flag[n]为1则表示这个顶点在集合p中，为1则表示在集合q中。
（2）起点a到自己的最短距离设置为0，起点能直接到达的顶点n，dis[n]设为e[a][n]，起点不能直接到达的顶点的最短路径为设为∞。
（3）在集合q中选择一个离起点最近的顶点u（即dis[u]最小）加入到集合p。并计算所有以顶点u为起点的边，到其它顶点的距离。例如存在一条从顶点u到顶点v的边，那么可以通过将边u->v添加到尾部来拓展一条从a到v的路径，这条路径的长度是dis[u]+e[u][v]。如果这个值比目前已知的dis[v]的值要小，我们可以用新值来替代当前dis[v]中的值。
（4）重复第三步，如果最终集合q结束，算法结束。最终dis数组中的值就是起点到所有顶点的最短路径。
a*算法
1968年，斯坦福国际研究院的peter e. hart, nils nilsson以及bertram raphael共同发明了a*算法。a*算法通过借助一个启发函数来引导搜索的过程，可以明显地提高路径搜索效率。
下文仍以一个实例来简单介绍a*算法的实现过程。如图19所示，假设小马要从a点前往b点大榕树底下去约会，但是a点和b点之间隔着一个池塘。为了能尽快提到达约会地点，给姑娘留下了一个守时踏实的好印象，我们需要给小马搜索出一条时间最短的可行路径。
图19 约会场景示意图
a*算法的第一步就是简化搜索区域，将搜索区域划分为若干栅格。并有选择地标识出障碍物不可通行与空白可通行区域。一般地，栅格划分越细密，搜索点数越多，搜索过程越慢，计算量也越大；栅格划分越稀疏，搜索点数越少，相应的搜索精确性就越低。
如图20所示，我们在这里将要搜索的区域划分成了正方形（当然也可以划分为矩形、六边形等）的格子，图中蓝色格子代表a点（小马当前的位置），紫色格子代表b点（大榕树的位置），灰色格子代表池塘。同时我们可以用一个二维数组s来表示搜素区域，数组中的每一项代表一个格子，状态代表可通行和不可通行。
图20 经过简化后的搜索区域
接着我们引入两个集合openlist和closelist，以及一个估价函数f = g + h。openlist用来存储可到达的格子，closelist用来存储已到达的格子。g代表从起点到当前格子的距离，h表示在不考虑障碍物的情况下，从当前格子到目标格子的距离。f是起点经由当前格子到达目标格子的总代价，值越小，综合优先级越高。
g和h也是a*算法的精髓所在，通过考虑当前格子与起始点的距离，以及当前格子与目标格子的距离来实现启发式搜索。对于h的计算，又有两种方式，一种是欧式距离，一种是曼哈顿距离。
欧式距离用公式表示如下，物理上表示从当前格子出发，支持以8个方向向四周格子移动（横纵向移动+对角移动）。
曼哈顿距离用公式表示如下，物理上表示从当前格子出发，支持以4个方向向四周格子移动（横纵向移动）。这是a*算法最常用的计算h值方法，本文h值的计算也采用这种方法。
现在我们开始搜索，查找最短路径。首先将起点a放入到openlist中，并计算出此时openlist中f值最小的格子作为当前方格移入到closelist中。由于当前openlist中只有起点a这个格子，所以将起点a移入closelist，代表这个格子已经检查过了。
接着我们找出当前格子a上下左右所有可通行的格子，看它们是否在openlist当中。如果不在，加入到openlist中计算出相应的g、h、f值，并把当前格子a作为它们的父节点。本例子，我们假设横纵向移动代价为10，对角线移动代价为14。
我们在每个格子上标出计算出来的f、g、h值，如图21所示，左上角是f，左下角是g，右下角是h。通过计算可知s[3][2]格子的f值最小，我们把它从openlist中取出，放到closelist中。
图21 第一轮计算后的结果
接着将s[3][2]作为当前格子，检查所有与它相邻的格子，忽略已经在closelist或是不可通行的格子。如果相邻的格子不在openlist中，则加入到openlist，并将当前方格子s[3][2]作为父节点。
已经在openlist中的格子，则检查这条路径是否最优，如果非最优，不做任何操作。如果g值更小，则意味着经由当前格子到达openlist中这个格子距离更短，此时我们将openlist中这个格子的父节点更新为当前节点。
对于当前格子s[3][2]来说，它的相邻5个格子中有4个已经在openlist，一个未在。对于已经在openlist中的4个格子，我们以它上面的格子s[2][2]举例，从起点a经由格子s[3][2]到达格子s[2][2]的g值为20（10+10）大于从起点a直接沿对角线到达格子s[2][2]的g值14。显然a经由格子s[3][2]到达格子s[2][2]不是最优的路径。当把4个已经在openlist 中的相邻格子都检查后，没有发现经由当前方格的更好路径，因此我们不做任何改变。
对于未在openlist的格子s[2][3]（假设小马可以斜穿墙脚），加入openlist中，并计算它的f、g、h值，并将当前格子s[3][2]设置为其父节点。经历这一波骚操作后，openlist中有5个格子，我们需要从中选择f值最小的那个格子s[2][3]，放入closelist中，并设置为当前格子，如图22所示。
图22 第二轮计算后的结果
重复上面的故事，直到终点也加入到openlist中。此时我们以当前格子倒推，找到其父节点，父节点的父节点……，如此便可搜索出一条最优的路径，如图23中红色圆圈标识。
图23 最后计算得到的结果
基于上述形象的过程，a*算法实现过程可以归纳为如下步骤：
（1）将搜索区域按一定规则划分，把起点加入openlist。
（2）在openlist中查找f值最小的格子，将其移入closelist，并设置为当前格子。
（3）查找当前格子相邻的可通行的格子，如果它已经在openlist中，用g值衡量这条路径是否更好。如果更好，将该格子的父节点设置为当前格子，重新计算f、g值，如果非更好，不做任何处理；如果不在openlist中，将它加入openlist中，并以当前格子为父节点计算f、g、h值。
（4）重复步骤（2）和步骤（3），直到终点加入到openlist中。
两种算法比较
dijkstra算法的基本思想是“贪心”，主要特点是以起点为中心向周围层层扩展，直至扩展到终点为止。通过dijkstra算法得出的最短路径是最优的，但是由于遍历没有明确的方向，计算的复杂度比较高，路径搜索的效率比较低。且无法处理有向图中权值为负的路径最优问题。
a*算法将dijkstra算法与广度优先搜索（breadth-first-search，bfs）算法相结合，并引入启发函数（估价函数），大大减少了搜索节点的数量，提高了搜索效率。但是a*先入为主的将最早遍历路径当成最短路径，不适用于动态环境且不太适合高维空间，且在终点不可达时会造成大量性能消耗。
图24是两种算法路径搜索效率示意图，左图为dijkstra算法示意图，右图为a*算法示意图，带颜色的格子表示算法搜索过的格子。由图24可以看出，a*算法更有效率，手术的格子更少。
图24 dijkstra算法和a*算法搜索效率对比图（图片来源：https://mp.weixin.qq.com/s/myu204uq3tfuikhgd3oefw）
特斯拉研发运动检测传感器，可检测留在车内的儿童防中暑
什么是隔离数字输入
东芝技术发展战略,转型分为三点
基于nRF24E1与TMC2023的汽车防撞系统
继电器的工作原理和特性详细解说
决策规划，全局路径规划常用算法
东大金智科技10G DWDM XFP光模块特征
8848手机宣布关停加密通话功能
荣耀正式从华为独立，定一个国内手机第一的小目标
大华存储亮相中东GITEX展 获评2022 ICT年度新兴存储供应商
海康机器人LMR×读码产品实现物料搬运自动化及数字化
反相同相比例运算电路
R语言的前世今生（起源、趋势、优势）
618前好福利，联想打印机又到好价
牵手首汽 奇瑞新能源迎里程碑式发展
2015年五大显示技术 明年谁唱主角？
智能供配电设备有哪些，它的作用是怎样的
动力电池需求急剧增长 “电池荒“成最大瓶颈
降低Clock Uncertainty流程
原创 8曲面智慧手机荣耀MAGIC拆机：传感器众多，做工细致！