模糊算法路径规划C语言实现的程序，感激不尽。

仅供参考~

#define MAX_VERTEX_NUM 100 //最大顶点数

#define MAX_INT 10000 //无穷大

typedef int AdjType

typedef struct{

int pi[MAX_VERTEX_NUM]//存放v到vi的一条最短路径

int end

}PathType

typedef char VType//设顶点为字符类型

typedef struct{

VType V[MAX_VERTEX_NUM]//顶点存储空间

AdjType A[MAX_VERTEX_NUM][MAX_VERTEX_NUM]//邻接矩阵

}MGraph//邻接矩阵表示的图

//Floyd算法

//求网G（用邻接矩阵表示）中任意两点间最短路径

//D[][]是最短路径长度矩阵，path[][]最短路径标志矩阵

void Floyd(MGraph * G,int path[][MAX_VERTEX_NUM],int D[][MAX_VERTEX_NUM],int n){

int i,j,k

for(i=0i<ni++){//初始化

for(j=0j<nj++){

if(G->A[i][j]<MAX_INT){

path[i][j]=j

}else{

path[i][j]=-1

}

D[i][j]=G->A[i][j]

}

}

for(k=0k<nk++){//进行n次试探

for(i=0i<ni++){

for(j=0j<nj++){

if(D[i][j]>D[i][k]+D[k][j]){

D[i][j]=D[i][k]+D[k][j]//取小者

path[i][j]=path[i][k]//改Vi的后继

}

}

}

}

}

int main(){

int i,j,k,v=0,n=6//v为起点，n为顶点个数

MGraph G

int path[MAX_VERTEX_NUM][MAX_VERTEX_NUM]//v到各顶点的最短路径向量

int D[MAX_VERTEX_NUM][MAX_VERTEX_NUM]//v到各顶点最短路径长度向量

//初始化

AdjType a[MAX_VERTEX_NUM][MAX_VERTEX_NUM]={

{0,12,18,MAX_INT,17,MAX_INT},

{12,0,10,3,MAX_INT,5},

{18,10,0,MAX_INT,21,11},

{MAX_INT,3,MAX_INT,0,MAX_INT,8},

{17,MAX_INT,21,MAX_INT,0,16},

{MAX_INT,5,11,8,16,0}

}

for(i=0i<ni++){

for(j=0j<nj++){

G.A[i][j]=a[i][j]

}

}

Floyd(&G,path,D,6)

for(i=0i<ni++){//输出每对顶点间最短路径长度及最短路径

for(j=0j<nj++){

printf("V%d到V%d的最短长度:",i,j)

printf("%d\t",D[i][j])//输出Vi到Vj的最短路径长度

k=path[i][j]//取路径上Vi的后续Vk

if(k==-1){

printf("There is no path between V%d and V%d\n",i,j)//路径不存在

}else{

printf("最短路径为:")

printf("(V%d",i)//输出Vi的序号i

while(k!=j){//k不等于路径终点j时

printf(",V%d",k)//输出k

k=path[k][j]//求路径上下一顶点序号

}

printf(",V%d)\n",j)//输出路径终点序号

}

printf("\n")

}

}

system("pause")

return 0

}

姓名：车文扬 学号：16020199006

【嵌牛导读】：A*算法的逐步详解

【嵌牛鼻子】：启发式算法

【嵌牛提问】：A*算法的原理是什么？

【嵌牛正文】：

A*算法

路径规划是指的是机器人的最优路径规划问题，即依据某个或某些优化准则（如工作代价最小、行走路径最短、行走时间最短等），在工作空间中找到一个从起始状态到目标状态能避开障碍物的最优路径。机器人的路径规划应用场景极丰富，最常见如游戏中NPC及控制角色的位置移动，百度地图等导航问题，小到家庭扫地机器人、无人机大到各公司正争相开拓的无人驾驶汽车等。

目前路径规划算法分为：

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，蓝色为障碍物。

我们把要搜寻的区域划分成了正方形的格子。这是寻路的第一步，简化搜索区域。这个特殊的方法把我们的搜索区域简化为了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。

下一步，我们需要从open list中选一个与起点A相邻的方格。但是到底选择哪个方格好呢？选F值最小的那个。我们看看下图中的一些方格。在标有字母的方格中G = 10 。这是因为水平方向从起点到那里只有一个方格的距离。与起点直接相邻的上方，下方，左方的方格的G 值都是10 ，对角线的方格G 值都是14 。H值通过估算起点到终点( 红色方格) 的Manhattan 距离得到，仅作横向和纵向移动，并且忽略沿途的障碍。使用这种方式，起点右边的方格到终点有3 个方格的距离，因此H = 30 。这个方格上方的方格到终点有4 个方格的距离( 注意只计算横向和纵向距离) ，因此H = 40 。

比较open list中节点的F值后，发现起点A右侧节点的F=40，值最小。选作当前处理节点，并将这个点从Open List删除，移到Close List中。

对这个节点周围的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 的方格更快。因此选择起点右下方的方格，如下图所示。

接下来把起点右下角F值为54的方格作为当前处理节点，检查其相邻的方格。我们发现它右边是墙（墙下面的一格也忽略掉，假定墙角不能直接穿越)，忽略之。这样还剩下 5 个相邻的方格。当前方格下面的 2 个方格还没有加入 open list ，所以把它们加入，同时把当前方格设为他们的父亲。在剩下的 3 个方格中，有 2 个已经在 close list 中 ( 一个是起点，一个是当前方格上面的方格，外框被加亮的 ) ，我们忽略它们。最后一个方格，也就是当前方格左边的方格，检查经由当前方格到达那里是否具有更小的 G 值。没有，因此我们准备从 open list 中选择下一个待处理的方格。

不断重复这个过程，直到把终点也加入到了open list 中，此时如下图所示。注意在起点下方2 格处的方格的父亲已经与前面不同了。之前它的G值是28并且指向它右上方的方格。现在它的G 值为20 ，并且指向它正上方的方格。这是由于在寻路过程中的某处使用新路径时G值更小，因此父节点被重新设置，G和F值被重新计算。

那么我们怎样得到实际路径呢？很简单，如下图所示，从终点开始，沿着箭头向父节点移动，直至回到起点，这就是你的路径。

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. 从终点开始，每个方格沿着父节点移动直至起点，形成路径。

路径规划模块需要根据局部环境感知、可用的全局车道级路径、相关交通规则，提供能够将车辆引导向目的地（或目的点）的路径。路径规划可分为全局路径规划方法、局部路径规划方法和混合路径规划方法三种。

---