轨迹分段算法最短

A. 求写最短路径算法。由A地到E地，途经B（B1，B2，B3）C（C1，C2，C3）地，基于矩阵乘法求最短路径。给出步骤

们把求A →E 的最短路分解为四个阶段A →B →C→D →E 来求解。每一个阶段可以用一个矩阵来表示，这个矩阵称为权矩阵。相邻阶段的路径可以用权矩阵的乘积来表示。但这里的矩阵乘法和普通矩阵乘积运算的区别是:普通矩阵乘积其对应元素是相应元素乘积的代数和,这里把元素相乘改为相加,元素的代数和改为取小运算,如果不同层节点间没有连接,则视它们之间的距离为无穷大. 如果是求极大,改为取大运算,此时如果不同层节点间没有连接，则视它们的距离为0。
如下：
由A地到B地的距离可表示为：A[2 5 8]
由B地到C地的权矩阵可表示为
[3，6，5；7，10，8；4，9，6]
因此由A到C的权矩阵为[2，5，8][3，6，5；7，10，8；4，9，6]＝[5，8，7]
因此由A到D的权矩阵为[5，8，7)][7，5；3，4；5，2]＝[11 ，9]
由A→E的权矩阵为：[11 ，9][4，2)]=[15,11]
因此从家里到学校的最短距离为11百米，最近的路径为从A地出发经过B1地C1地D2地到达E地。

下面我们给出基于“矩阵乘法”求解最短路的算法:
第一阶段:计算出图中从起始点到终点最短路的长度.
step1 划分出该网络图中的层次关系(网络划分为N 层,起点为第一层,终点为第N 层) ;
step2 依次给出从第i 层到第i + 1 层的权矩阵( i= 1 ,2 , …, N21) ; (若第i 层有m 个顶点;第i + 1 层有n
个顶点, 则从第i 层到第i + 1 层的权矩阵为m *n
阶) .
step3 按照我们定义的矩阵乘法计算出最短路的
数值.
第二阶段:寻找最短路所经过的中间点.
(利用第一阶段中step2 的数据) 计算出从第i 层到
终点的最短路, 对比与i21 层到终点的最短路, 从而确
定出第i 层上最短路所经过的顶点( i = 2 , …, N21) .

B. 最短路径算法（Dijkstra）

Dijkstra（ 迪科斯特拉 ）算法是用来解决核激唯单源最短路径的算法，要求路径权值非负数。该算法利用了深度优先搜索和贪心的算法。

下面是一个有权图，求从A到各个节点的最短路径。

第1步：从A点出发，判断每个点到A点的路径（如果该点不能直连A点则距离值为无穷大，如果该点能和A直连则是当前的权值），计算完之后把A点上色，结果如下图:

第2步：从除A点之外的点查找到距离A点最近的点C，从C点出发查找其邻近的节点（除去已上色的点），并重新计算C点的邻近点距离A点的值，如图中B点，若新值（C点到A点的值+C点到该点的路径）小于原值，则将值更新为5，同理更新D、E点。同时将C标铅陵记为已经处理过，如图所示涂色。

第3步：从上色的节点中查找距离A最近的B点，重复第3步操作。

第4步： 重复第3步，改培2步，直到所有的节点都上色。

最后就算出了从A点到所有点的最短距离。

leetcode 743题

C. 最短路径的解决方法

用于解决最短路径问题的算法被称做“最短路径算法”， 有时被简称作“路径算法”。 最常用的路径算法有：
Dijkstra算法
SPFA算法Bellman-Ford算法
Floyd算法Floyd-Warshall算法
Johnson算法
A*算法
所谓单源最短路径问题是指：已知图G=（V，E），我们希望找出从某给定的源结点S∈V到V中的每个结点的最短路径。
首先，我们可以发现有这样一个事实：如果P是G中从vs到vj的最短路，vi是P中的一个点，那么，从vs沿P到vi的路是从vs到vi的最短路。

D. 最短路径算法

Dijkstra算法，A*算法和D*算法

Dijkstra算法是典型最短路算法，用于计算一个节点到其他所有节点的最短路径。主要特点是以起始点为中心向外层层扩展，直到扩展到终点为止。Dijkstra算法能得出最短路径的最优解，但由于它遍历计算的节点很多，所以效率低。

Dijkstra算法是很有代表性的最短路算法，在很多专业课程中都作为基本内容有详细的介绍，如数据结构，图论，运筹学等等。

Dijkstra一般的表述通常有两种方式，一种用永久和临时标号方式，一种是用OPEN, CLOSE表方式，Drew为了和下面要介绍的 A* 算法和 D* 算法表述一致，这里均采用OPEN,CLOSE表的方式。

大概过程：
创建两个表，OPEN, CLOSE。
OPEN表保存所有已生成而未考察的节点，CLOSED表中记录已访问过的节点。
1． 访问路网中里起始点最近且没有被检查过的点，把这个点放入OPEN组中等待检查。
2． 从OPEN表中找出距起始点最近的点，找出这个点的所有子节点，把这个点放到CLOSE表中。
3． 遍历考察这个点的子节点。求出这些子节点距起始点的距离值，放子节点到OPEN表中。
4． 重复2，3，步。直到OPEN表为空，或找到目标点。

提高Dijkstra搜索速度的方法很多，常用的有数据结构采用Binary heap的方法，和用Dijkstra从起始点和终点同时搜索的方法。

A*（A-Star)算法是一种启发式算法，是静态路网中求解最短路最有效的方法。

公式表示为： f(n)=g(n)+h(n),
其中f(n) 是节点n从初始点到目标点的估价函数，
g(n) 是在状态空间中从初始节点到n节点的实际代价，
h(n)是从n到目标节点最佳路径的估计代价。

保证找到最短路径（最优解的）条件，关键在于估价函数h(n)的选取：
估价值h(n)<= n到目标节点的距离实际值，这种情况下，搜索的点数多，搜索范围大，效率低。但能得到最优解。
如果 估价值>实际值, 搜索的点数少，搜索范围小，效率高，但不能保证得到最优解。
估价值与实际值越接近，估价函数取得就越好。
例如对于几何路网来说，可以取两节点间欧几理德距离（直线距离）做为估价值，即f=g(n)+sqrt((dx-nx)*(dx-nx)+(dy-ny)*(dy-ny))；这样估价函数f在g值一定的情况下，会或多或少的受估价值h的制约，节点距目标点近，h值小，f值相对就小，能保证最短路的搜索向终点的方向进行。明显优于Dijstra算法的毫无无方向的向四周搜索。
conditions of heuristic
Optimistic (must be less than or equal to the real cost)
As close to the real cost as possible
主要搜索过程：
创建两个表，OPEN表保存所有已生成而未考察的节点，CLOSED表中记录已访问过的节点。
遍历当前节点的各个节点，将n节点放入CLOSE中，取n节点的子节点X,->算X的估价值->
While(OPEN!=NULL)
{
从OPEN表中取估价值f最小的节点n;
if(n节点==目标节点) break;
else
{
if(X in OPEN) 比较两个X的估价值f //注意是同一个节点的两个不同路径的估价值
if( X的估价值小于OPEN表的估价值 )
更新OPEN表中的估价值; //取最小路径的估价值
if(X in CLOSE) 比较两个X的估价值 //注意是同一个节点的两个不同路径的估价值
if( X的估价值小于CLOSE表的估价值 )
更新CLOSE表中的估价值; 把X节点放入OPEN //取最小路径的估价值
if(X not in both)
求X的估价值;
并将X插入OPEN表中; //还没有排序
}
将n节点插入CLOSE表中;
按照估价值将OPEN表中的节点排序; //实际上是比较OPEN表内节点f的大小，从最小路径的节点向下进行。
}

A*算法和Dijistra算法的区别在于有无估价值，Dijistra算法相当于A*算法中估价值为0的情况。

动态路网，最短路算法 D*A* 在静态路网中非常有效（very efficient for static worlds），但不适于在动态路网，环境如权重等不断变化的动态环境下。

D*是动态A*（D-Star,Dynamic A*） 卡内及梅隆机器人中心的Stentz在1994和1995年两篇文章提出，主要用于机器人探路。是火星探测器采用的寻路算法。

主要方法：
1.先用Dijstra算法从目标节点G向起始节点搜索。储存路网中目标点到各个节点的最短路和该位置到目标点的实际值h,k（k为所有变化h之中最小的值,当前为k=h。每个节点包含上一节点到目标点的最短路信息1(2),2(5),5(4)，4（7）。则1到4的最短路为1-2-5-4。
原OPEN和CLOSE中节点信息保存。
2.机器人沿最短路开始移动，在移动的下一节点没有变化时，无需计算，利用上一步Dijstra计算出的最短路信息从出发点向后追述即可，当在Y点探测到下一节点X状态发生改变，如堵塞。机器人首先调整自己在当前位置Y到目标点G的实际值h(Y)，h(Y)=X到Y的新权值c(X,Y)+X的原实际值h(X).X为下一节点(到目标点方向Y->X->G），Y是当前点。k值取h值变化前后的最小。
3.用A*或其它算法计算，这里假设用A*算法,遍历Y的子节点，点放入CLOSE,调整Y的子节点a的h值，h(a)=h(Y)+Y到子节点a的权重C(Y,a),比较a点是否存在于OPEN和CLOSE中，方法如下：
while()
{
从OPEN表中取k值最小的节点Y;
遍历Y的子节点a,计算a的h值 h(a)=h(Y)+Y到子节点a的权重C(Y,a)
{
if(a in OPEN) 比较两个a的h值
if( a的h值小于OPEN表a的h值 )
{ 更新OPEN表中a的h值;k值取最小的h值
有未受影响的最短路经存在
break;
}
if(a in CLOSE) 比较两个a的h值 //注意是同一个节点的两个不同路径的估价值
if( a的h值小于CLOSE表的h值 )
{
更新CLOSE表中a的h值; k值取最小的h值;将a节点放入OPEN表
有未受影响的最短路经存在
break;
}
if(a not in both)
将a插入OPEN表中; //还没有排序
}
放Y到CLOSE表；
OPEN表比较k值大小进行排序；
}
机器人利用第一步Dijstra计算出的最短路信息从a点到目标点的最短路经进行。

D*算法在动态环境中寻路非常有效，向目标点移动中，只检查最短路径上下一节点或临近节点的变化情况，如机器人寻路等情况。对于距离远的最短路径上发生的变化，则感觉不太适用。

E. 数学最短路径问题最方便的解法是什么

用于解决最短路径问题的算法被称做“最短路径算法” ，有时被简称作“路径算法” 。最常用 的路径算法有： Dijkstra 算法、 A*算法、 SPFA 算法、 Bellman-Ford 算法和 Floyd-Warshall 算法， 本文主要介绍其中的三种。 最短路径问题是图论研究中的一个经典算法问题，旨在寻找图（由结点和路径组成的）中两 结点之间的最短路径。 算法具体的形式包括： 确定起点的最短路径问题：即已知起始结点，求最短路径的问题。 确定终点的最短路径问题：与确定起点的问题相反，该问题是已知终结结点，求最短路径的 问题。 在无向图中该问题与确定起点的问题完全等同， 在有向图中该问题等同于把所有路径 方向反转的确定起点的问题。 确定起点终点的最短路径问题：即已知起点和终点，求两结点之间的最短路径。 全局最短路径问题：求图中所有的最短路径。 Floyd 求多源、无负权边的最短路。用矩阵记录图。时效性较差，时间复杂度 O(V^3)。 Floyd-Warshall 算法（Floyd-Warshall algorithm）是解决任意两点间的最短路径的一种算法， 可以正确处理有向图或负权的最短路径问题。 Floyd-Warshall 算法的时间复杂度为 O(N^3)，空间复杂度为 O(N^2)。 Floyd-Warshall 的原理是动态规划： 设 Di,j,k 为从 i 到 j 的只以(1..k)集合中的节点为中间节点的最短路径的长度。 若最短路径经过点 k，则 Di,j,k = Di,k,k-1 + Dk,j,k-1； 若最短路径不经过点 k，则 Di,j,k = Di,j,k-1。 因此，Di,j,k = min(Di,k,k-1 + Dk,j,k-1 , Di,j,k-1)。 在实际算法中，为了节约空间，可以直接在原来空间上进行迭代，这样空间可降至二维。 Floyd-Warshall 算法的描述如下： 1.for k ← 1 to n do 2.for i ← 1 to n do 3.for j ← 1 to n do 4.if (Di,k + Dk,j<Di,j) then 5.Di,j ← Di,k + Dk,j; 其中 Di,j 表示由点 i 到点 j 的代价，当 Di,j 为∞表示两点之间没有任何连接。 Dijkstra 求单源、无负权的最短路。时效性较好，时间复杂度为 O（V*V+E） 。 源点可达的话，O（V*lgV+E*lgV）=>O（E*lgV） 。 当是稀疏图的情况时，此时 E=V*V/lgV，所以算法的时间复杂度可为 O（V^2） 。若是斐波那 契堆作优先队列的话，算法时间复杂度，则为 O（V*lgV + E） 。 Bellman-Ford 求单源最短路，可以判断有无负权回路（若有，则不存在最短路） ，时效性较好，时间复杂 度 O（VE） 。 Bellman-Ford 算法是求解单源最短路径问题的一种算法。 单源点的最短路径问题是指：给定一个加权有向图 G 和源点 s，对于图 G 中的任意一点 v， 求从 s 到 v 的最短路径。 与 Dijkstra 算法不同的是，在 Bellman-Ford 算法中，边的权值可以为负数。设想从我们可以 从图中找到一个环路（即从 v 出发，经过若干个点之后又回到 v）且这个环路中所有边的权 值之和为负。那么通过这个环路，环路中任意两点的最短路径就可以无穷小下去。如果不处 理这个负环路，程序就会永远运行下去。而 Bellman-Ford 算法具有分辨这种负环路的能力。 SPFA是 Bellman-Ford 的队列优化，时效性相对好，时间复杂度 O（kE）（k<<V） 。 。 与 Bellman-ford 算法类似， SPFA 算法采用一系列的松弛操作以得到从某一个节点出发到达图 中其它所有节点的最短路径。所不同的是，SPFA 算法通过维护一个队列，使得一个节点的 当前最短路径被更新之后没有必要立刻去更新其他的节点， 从而大大减少了重复的操作次数。 SPFA 算法可以用于存在负数边权的图，这与 dijkstra 算法是不同的。 与 Dijkstra 算法与 Bellman-ford 算法都不同，SPFA 的算法时间效率是不稳定的，即它对于不 同的图所需要的时间有很大的差别。 在最好情形下，每一个节点都只入队一次，则算法实际上变为广度优先遍历，其时间复杂度 仅为 O(E)。另一方面，存在这样的例子，使得每一个节点都被入队(V-1)次，此时算法退化为 Bellman-ford 算法，其时间复杂度为 O(VE)。 SPFA 算法在负边权图上可以完全取代 Bellman-ford 算法， 另外在稀疏图中也表现良好。 但是 在非负边权图中，为了避免最坏情况的出现，通常使用效率更加稳定的 Dijkstra 算法，以及 它的使用堆优化的版本。通常的 SPFA 算法在一类网格图中的表现不尽如人意。 

F. 网格中如何求任意两点间的最短路径 matlab算法

function [L,Z]=dijkstra(W,S,T)
%用 Dijkstra 算法求最短路径
% 算法
% 1. 对每个点I指定一个离点S的距离初始值L(I). 在始点S的值为零, 即L(S)=0，其它点的值为Inf.
% 2. 所有的点标记为未走访的. 置始点S为当前点C.
% 3. 对于当前点C, 考虑它的所有未走访的相邻点J, 并更新J的距离值为
% L(J)=min(L(J), L(C)+W(C,J))
% 4. 把当前点C标记为走访过的点. 走访过的点C的距离L(C)就是点S到C的最短距离, 而且以后不再检查走访过得点了.
% 5. 如果所有的点都是走访过的点, 完成. 不然, 把未走访的点中具有最小距离值的点作为下一个当前点C, 转
N=length(W(:,1));%顶点数
W(find(W==0))=inf;
L=Inf*ones(1,N);
L(S)=0;%L赋初值，在S点为0，其它点为Inf
C=S; %当前点为始点S
Q=1:N;% 未走访的顶点集
Z=S*ones(1,N);
Z(S)=0;% Z赋初值,因始点 S 无父亲点,故把 S 点的Z值改为0
for K=1:N % 更新 L 和 Z 的循环
Q=setdiff(Q,C); %Matlab自带函数，显示Q中除了C之外的点集，即当前点 C 未走访的点集
[L(Q),ind]=min([L(Q);L(C)+W(C,Q)]);%当前点C已走访了所有的相邻的未走访的点，找出与C相邻的距离最短的点，记录最短距离和结点的索引值，更新 L
%如果L(Q)
Z(Q(find(ind==2)))=C; %更新Z, 找出Q中索引值为2的结点，将其父亲结点更新为C,至此可以确定C已是走访过的点了
if T&C==T %若 C 点是终点 T, 不用再计算到其它未走访的点的最短路径.先判断C==T，再判断&
L=L(T); %最短路径长度；
road=T;%最短路径终点；
while T~=S%追溯最短路径上的点
T=Z(T); %从终点往前寻找其父亲结点
road=[road,T]; %从终点开始倒序记录最短路径上的结点
end
Z=road(length(road):-1:1); %颠倒次序；
return;
end;
[null, mC]=min(L(Q));
if null==Inf
% disp('到值是Inf的点的路不通！');
Z(find(L==Inf))=nan; %NaN或者nan都是“非数”的意思，“0/0”、“∞/∞”、“0*∞”都会产生这种结果
return;
else
C=Q(mC);% 把未走访的点集Q中与始点距离最近的点作为新的当前点C;
end
end
end