Dijkstra算法求解三维坐标下的最短路径（附代码）

一、迪杰斯特拉算法

Dijkstra算法是由E.W.Dijkstra于1959年提出，又叫迪杰斯特拉算法，它应用了贪心算法模式，是目前公认的最好的求解最短路径的方法。算法解决的是有向图中单个源点到其他顶点的最短路径问题，其主要特点是每次迭代时选择的下一个顶点是标记点之外距离源点最近的顶点。该算法复杂度为n^2,但是对此可以考虑用堆这种数据结构进行优化，可以将复杂度降为O((m+n)logn)。
Dijkstra算法的基本流程为：
(1)初始化每个节点i到A点（出发点）的最短距离，若该点到A点之间有直线路径可以到达，且该距离满足到达该点时的校正约束，则该点到A点的最短距离为直线距离，记为D(i,A)=SiA；若该点到A点之间没有直线距离可以到达，或者到达该点时由于距离长而违反校正约束，那么将该点到A点最短距离初始化为无穷大, 记为D(i,A)=+∞。
(2)依次遍历除B点（终点）之外的所有节点，对每一个节点进行如下操作：首先，寻找距离该点最近的可到达点，记为i，标记i点的状态为已访问；然后，依次遍历i点所有可以直线到达且未被访问过的点，记该点为j，记Sij为i到j之间的距离；最后，如果D(i,A)+Sij<D(j,A)，那么将j点到A点的最短距离D(j,A)更新为D(i,A)+Sij；
(3)直到所有顶点的最短路径都被找到。
官方的解释总是让人听不懂，下面举个例子简单说明一下算法的原理：

起点：A 终点：B
数字代表两点之间的距离
定义Di为任意i点到起点的最短距离，
并赋初始值为A点到i的直线距离，
若无直线距离则赋值正无穷，如De；
第一次循环：
找距离起点A最近的点：
AB=10;AC=2;AD=12;AE=+ꝏ；
因此C点为距离A点最近的点，更新路径A-C；
遍历除了C以外的所有点i，
若Di>AC+Ci
则更新Di=AC+Ci
第二次循环：
同理，找距离C点最近的点，
并进行更新路径操作，
直到所有的点都被遍历,De即为所求；

二、问题描述

考虑飞行器在如下图的飞行区域飞行，出发点为A点，目的地为B点，飞行器在空间飞行过程中需要实时定位，其定位误差包括垂直误差和水平误差。飞行器每飞行1米，垂直误差和水平误差将各增加δ个专用单位，到达终点时垂直误差和水平误差均应小于a个单位。

飞行器在飞行过程中需要对定位误差进行校正。飞行区域中存在一些安全位置(称之为校正点)可用于误差校正，当飞行器到达校正点即能够根据该位置的误差校正类型进行误差校正，校正垂直和水平误差的位置可根据地形在飞行路径规划前确定，若垂直误差、水平误差都能得到及时校正，则飞行器可以按照预定航线飞行，通过若干个校正点进行误差校正后最终到达目的地。

在从A点到达B点的过程中还需要满足如下约束：

1. 在出发地 A 点，飞行器的垂直和水平误差均为0

2. 飞行器在垂直误差校正点进行垂直误差校正后，其垂直误差将变为0，水平误差保持不变

3. 飞飞行器在水平误差校正点进行水平误差校正后，其水平误差将变为 0，垂直误差保持不变

4. 当飞行器的垂直误差不大于个单位，水平误差不大于个单位时才能进行垂直误差校正

5. 当飞行器的垂直误差不大于个单位，水平误差不大于个单位时才能进行水平误差校正

6. 在两点之间的垂直误差和水平误差均小于a个单位。

问：对给定的数据集分别规划满足以上约束时飞行器的飞行路径，使得飞行路径长度尽可能小。（该题目出自2019年研究生数模竞赛，此处做了一定修改，有兴趣的可以访问https://cpipc.chinadegrees.cn/cw/hp/4#contest-news）

三、算法设计

为了满足问题的约束，在最短路算法之外还应有对校正点的判断，即对水平校正点、垂直校正点和终点的特殊情况予以区分：

if(infmat[i][3]==0) //水平；
            {
                if (distance(i,0)<B1*1000)
                {
                    route[i][1].push_back(distance(i,0));
                    route[i][2].push_back(distance(i,0)*delt1);
                    route[i][3].push_back(0);}
                else {route[i][1].push_back(INF);}
            }
            else if(infmat[i][3]==1)
                {
                if (distance(i,0)<A2*1000)
                {route[i][1].push_back(distance(i,0));
                route[i][2].push_back(0);
                route[i][3].push_back(distance(i,0)*delt1);}
                else {route[i][1].push_back(INF);}
            }

四、实验结果

以下为matplotlib函数画图代码：

import matplotlib.pyplot as plt
import numpy as np
from mpl_toolkits.mplot3d import Axes3D

def loadtxt(filename):
    data=np.loadtxt(filename,dtype=np.float32,delimiter='')
    return data

if __name__=="__main__":
    data_pos=np.loadtxt('test.txt')
    data_resl=np.loadtxt('result.txt')
    print(data_pos)
    

fig = plt.figure()
ax = Axes3D(fig)
x = data_pos[:, 0]  
y = data_pos[:, 1]  
z = data_pos[:, 2] 
ax.scatter(x, y, z,s=10, c='b')

x1=[]
y1=[]
z1=[]
for i in data_resl:
   j=int(i)
   x1.append(data_pos[j,0])
   y1.append(data_pos[j,1])
   z1.append(data_pos[j,2])
   ax.scatter(x1,y1,z1,s=10,c='r')
plt.plot(x1, y1, z1,ls="-", lw=2, c='r')

ax.set_zlabel('Z', fontdict={'size': 15, 'color': 'red'})
ax.set_ylabel('Y', fontdict={'size': 15, 'color': 'red'})
ax.set_xlabel('X', fontdict={'size': 15, 'color': 'red'})
plt.show()

总的来说，迪杰斯特拉算法在求解最短路问题上具有一定的优势，本问题采用的Dijkstra算法在时间上的复杂度是O(N^{2)，相比于经典的弗洛伊德最短路算法的O(N}3)是更为快速的求解算法，针对该问题，虽然在算法上加入了多处可行点的判断算法，但是并没有对算法的整体时间复杂度产生影响，因此，本文采用的算法的时间复杂度仍为O(N^2)。

获取源代码或者历年数模研究生数模真题请移步后台留言