A*寻路算法的探寻与改良（二）

by:田宇轩                                                    

第二部分：这部分内容主要是使用C语言编程实现A*，想了解A*算法的优化内容的朋友们可以跳过这部分并阅读稍后更新的其他内容

2.1 回顾

       你可以点击回顾文章的第一部分。

       在我的上一篇文章中，我们通过抽象的思维方式得出了A*算法的概念和原理，这一章内容中主要探讨如何用编程实现A*算法。

       在数据结构与算法的学习中，每个算法都应该结合一定的数据结构在计算机中存储，然后用对应的函数操控这些数据结构，A*算法也不例外，从上一篇文章中，我们知道，A*算法需要：

（1）地图，这是一个存储静态路网的结构，由格子组成

（2）格子，格子是组成地图的基本单位，每个格子都有坐标，F，G，H，父节点这五种属性；

（3）开启列表，用于记录等待处理的格子；

（4）关闭列表，用于记录已经处理的格子；

（5）起点和终点，用于接受用户输入指定哪个点为起点，哪个点为终点；

       这些存储结构都是A*算法需要的，其实为了实现A*算法，我们还需要更多的存储结构，这些结构我们将会在用到的时候抽象出来的。弄清思路之后，我们先用C语言定义一下这些结构，如果您是其他语言的使用者，也可以按照这些结构的描述用其他语言的定义和实现。下面就是C语言对A*所需结构的实现，下面这段代码可以单独定义在一个头文件中，拥有全局作用域，其实让这些代码拥有全局作用域的方式我们并不提倡，这里只是方便教学和理解用。

1 #define MAX_number 5   //这是地图的最大值，可以自己修改以符合实际需求 2  3 //一个比较基础的结构体，用于和地图联动 4 struct baseNode 5 { 6     int i; 7     int j; 8     int weigt; 9 10     int f;11     int g;12     int h;13 14     struct baseNode *father;15 };16 17 //定义了一个全局变量用来存储二维矩阵地图18 struct baseNode map[MAX_number][MAX_number];19 20 //记录起点和终点的数组，起点为Ascenery[0],终点为Ascenery[1]21 struct baseNode AsceneryList[2];22 23 //开启列表，用于A*算法24 struct baseNode OpenList[MAX_number*MAX_number];25 26 //关闭列表，用于A*算法27 struct baseNode CloseList[MAX_number*MAX_number];28 29 //用于记录开启列表中元素的个数30 int openListCount = 0;31 32 //用于记录关闭列表中元素的个数33 int closeListCount = 0;

代码2.1.1—A*的基本存储结构

 

2.2 A*算法的流程

2.2.1 设计代码体系结构

      为了方便用户输入和输出，也方便我们直观地看到A*的结果，在代码结构上，我们准备设计三个头文件：data.h，用于存储A*依赖的结构，func.h，用于写一些功能，process_control.h，用于显示一个简单的用户界面，控制用户流程，识别用户的输入是否在合法范围内，最后，我们用main.c调用所有这些内容。主函数很简单，这里先写出来：

1 #include "process_control.h"2 3 int main()4 {5     testStart();6 7     return 0;8 }

代码2.2.1.1—主函数

2.2.2 流程控制函数

      我们把前面定义的存储结构写在了data.h里面。然后我们稍微设计一下控制流程的process.h。这是main.c唯一引用的头文件，里面包含一个testStart函数。

1 #pragma once 2  3 #include "funcs.h" 4  5 //用于控制整个校园导航系统流程 6 void testStart() 7 { 8         //flag用于辅助程序判断有没有足够条件执行各功能 9     int flag1 = 0, flag2 = 0;10 11         //不断的让用户选择菜单12     for (;;)13     {14         printf("基于A*算法的校园导航系统程序\n\n");15 16         printf("你可以进行以下操作：\n");17         printf("1.设定校园地图地形\n");18         printf("2.设定寻径的起点和终点\n");19         printf("3.找出最佳路径\n");20         printf("0.退出系统\n\n");21 22                 //让用户输入自己的选择23         int userInput = 0;24         scanf("%d", &userInput);25 26                //根据自己的选择分别执行inputmap,setroad,Astar三个函数27         switch (userInput)28         {29         case 1:30             inputmap(); 31             flag1 = 1;32             printf("设定校园地图地形成功\n\n");33             break;34 35         case 2:36             if (flag1 == 1)37             {38                 setRoad();39                 flag2 = 1;40                 printf("起点终点设定完毕\n\n");41             }42             else43             {44                 printf("请先完成地图设定\n");45             }46             break;47 48         case 3:49             if (flag1 == 1&&flag2==1)50             {51                 Astar();52                 printf("寻径完毕\n\n");53             }54             else55             {56                 printf("请先完成地图、起点终点设定\n");57             }58             break;59 60         case 0:61             exit(0);62             break;63 64         default:65             printf("输入不在指定范围内，请重新输入\n\n");66             break;67         }68     }69 }

代码2.2.2.1—流程控制函数

2.2.3 设定地图样式的函数inputmap和设定起点终点的函数setroad

     让我们先设定好地图再进行A*算法本体的编写，这部分没有什么难度，因此也不先写伪代码和分析逻辑了，对此不感兴趣的朋友可以直接往后看Astar函数的实现。

1 #pragma once 2  3 #include 
     
       4 #include 
      
        5 #include 
       
         6 #include 
        
          7 #include "data_define.h" 8  9 //设定地图的函数10 void inputmap()11 {12     printf("目前地图大小为%d * %d。\n",MAX_number,MAX_number);13     printf("请通过输入整数的形式填充地图，0代表地形不可通过，\n其他正整数代表权值，权值越大，地形越不方便通过\n\n");14 15     for (int i = 0; i 
        
       
      
     

代码2.2.3.1—设定地图样式和起点终点的函数

2.2.4 Astar函数的设计

     由于Astar算法需要对每个点的邻居都分析其F值，而且这里我们用二维矩阵来设定地图，因此一个格子最多有8个邻居格子，为了一一处理这些邻居格子，我们设计一种大小为8的邻居数组，用循环从邻居数组的第一个元素处理到邻居数组的最大第八个元素。邻居数组定义如下：

1 //邻居列表，用于记录每个当前点的所有邻居2 struct baseNode Neibo[8];3 4 //用于记录邻居的个数5 int neibNum = 0;

代码2.2.4.1—邻居列表的定义

     接下来我们按照上前文章总结的流程编写一个代码框架，先不实现各个函数的具体功能。先回顾一下上一篇文章的A*寻路流程：

2.2.4.1—A*寻路流程

       复习了流程之后，我们就可以按照流程上面的大致打一个框架：

1 //假设我们预先把起点放入迭代器iter，终点设为ender 2 //把起点放入开启列表 3     putInOpenList(iter); 4  5     //当开启列表为空或者终点在关闭列表中，结束寻径 6     for (;  openListCount != 0 && isInCloseList(ender)==0;) 7     { 8         //取出开启列表中f值最小的节点（之一），并设为iter（当前点） 9         iter = readTopOpenList();10 11         //把当前点从开启列表中删除12         outOpenList(iter);13 14         //把当前点记录在关闭列表中15         putInCloseList(iter);16 17         //把当前点的邻居加入邻居列表18         addNeibo(iter);19 20         //对于每个邻居，分三种情况进行操作21         for (int i = 0; i < neibNum; ++i)22         {23             //如果这个邻居节点不可通过，或者这个邻居节点在关闭列表中，略过它24             if (Neibo[i].weigt==0 || isInCloseList(Neibo[i]))25             {26             }27             //如果这个邻居节点已经在开启列表中28             else if(isInOpenList(Neibo[i]))29             {   //看看以当前格子为父节点，算出来的新G值是不是比原来的G值小，如果更小，就改变这一格的父节点，G值，重新计算F值30                 if (NewG(Neibo[i],iter)

代码2.2.4.2—A*寻路流程的代码

     然后，只要分别实现上面代码逻辑中的函数就好了：

1 //以下函数都是A*算法的一部分///  2   3 //把一个元素插入开启列表中/  4 void putInOpenList(struct baseNode inList)  5 {  6     OpenList[openListCount] = inList;  7     ++openListCount;  8 }  9  10 //取出开启列表中最小的数 11 struct baseNode readTopOpenList() 12 { 13     struct baseNode temp; 14      15     for (int i = 0; i < openListCount-1; ++i) 16     { 17         if (OpenList[i].f
     
      = 0;--i) 41     { 42         if (OpenList[i].i==iter.i&&OpenList[i].j==iter.j) 43         { 44             break; 45         } 46     } 47  48     for (int j = i; j < openListCount-1; ++j) 49     { 50         OpenList[j] = OpenList[j+1]; 51     } 52     --openListCount; 53 } 54  55 //对于一路上的每个点，分析它的最多八个邻居，并加入邻居列表 56 void addNeibo(struct baseNode iter) 57 { 58     neibNum = 0; 59  60     for (int i = iter.i - 1; i <= iter.i + 1; ++i) 61     { 62         for (int j = iter.j - 1; j <= iter.j + 1; ++j) 63         { 64             if (i >= 0 && i <= MAX_number - 1 && j >= 0 && j <= MAX_number - 1) 65             { 66                 if (i == iter.i&&j == iter.j) 67                 { 68                 } 69                 else 70                 { 71                     map[i][j].h = manhatten(i, j); 72  73                     Neibo[neibNum] = map[i][j]; 74                     ++neibNum; 75                 } 76             } 77         } 78     } 79 } 80  81 //查看临近格在不在开启列表中的函数 82 int isInOpenList(struct baseNode neibo) 83 { 84     for (int i = 0; i < openListCount - 1; ++i) 85     { 86         if (OpenList[i].i == neibo.i&&OpenList[i].j == neibo.j) 87         { 88             return 1; 89         } 90     } 91     return 0; 92 } 93  94 //查看指定的temp在不在关闭列表中的函数 95 int isInCloseList(struct baseNode temp) 96 { 97     for (int i = 0; i < closeListCount-1; ++i) 98     { 99         if (CloseList[i].i == temp.i&&CloseList[i].j == temp.j)100         {101             return 1;102         }103     }104     return 0;105 }106 107 //A*中的启发式函数，用于求指定位置和终点之间的曼哈顿距离108 int manhatten(int i, int j)109 {110     return (abs(AsceneryList[1].node.i - i) + abs(AsceneryList[1].node.j - j))*10;111 }112 113 //求当前点与父亲节点的距离114 int increment(struct baseNode this)115 {116     if ((abs(this.father->i-this.i)==1) && (abs(this.father->j - this.j) == 1))117     {118         return 14*this.weigt;119     }120     else if ((this.father->i - this.i) == 0 && (this.father->j - this.j) == 0)121     {122         return 0;123     }124     else125     {126         return 10*this.weigt;127     }128 }129 130 //求出用当前点作为父节点时这个点的G值131 int NewG(struct baseNode this,struct baseNode father)132 {133     if (abs(father.i - this.i) == 1 && abs(father.j - this.j) == 1)134     {135         return father.g+14;136     }137     else if (abs(father.i - this.i) == 0 && abs(father.j - this.j) == 0)138     {139         return father.g;140     }141     else142     {143         return father.g+10;144     }145 }
     

代码2.2.4.3—刚才代码中具体函数的分别实现

    经过函数实现之后，我们就得出来了一条最短的从起点A到终点B的路径，这条路径上（包含A和B），每一个格子都指向它的父节点，因此我们可以从终点开始，一直遍历父节点，并设置一个迭代器，把每个格子的父节点赋值给迭代器，再储存入一个存储路径的数组里，我们就得到了这条路径：

1 //用来记录路径经过的点的个数2 int AstackCount = 0;3 4 //用来储存整理后的路径5 struct baseNode Astack[MAX_number*MAX_number];

代码2.2.4.4—存储最佳路线的数组

1 //把A*算法的节点按倒序整理到Astack里面 2 void arrange(struct baseNode iter) 3 { 4     AstackCount = 0; 5     for (; ; iter=map[iter.father->i][iter.father->j]) 6     { 7         Astack[AstackCount] = iter; 8         ++AstackCount; 9         if (iter.i == AsceneryList[0].node.i&&iter.j == AsceneryList[0].node.j)10         {11             break;12         }13     }14 }15 16 //打印出A*算法的路径矩阵17 printAstar()18 {19     printf("A为最佳路径，Q为不经过区域\n\n");20     int boole = 0;21 22     for (int i = 0; i < MAX_number; ++i)23     {24         for (int j = 0; j < MAX_number; ++j)25         {26             for (int w=0; w

代码2.2.4.5—迭代整理和输出

大功告成......等等，还差一步，我们在做A*操作时曾经假设iter初始化为起点，ender为终点，记得吗？因此，我们在做A*算法之前还要做这种类似初始化的操作：

//每次执行A*算法，都初始化开启/关闭列表    openListCount = 0;    closeListCount = 0;    //创建一个迭代器,每次都等于f值最小的节点    struct baseNode iter;    //让这个迭代器的初值为起点    iter.i = AsceneryList[0].i;    iter.j = AsceneryList[0].j;    iter.weigt = map[AsceneryList[0].i][AsceneryList[0].j].weigt;    //起点的没有父节点，且为唯一G值为0的点    iter.g = 0;    iter.h = manhatten(iter.i,iter.j);    iter.f = iter.g + iter.h;    //创建终点    struct baseNode ender;    ender.i = AsceneryList[1].i;    ender.j = AsceneryList[1].j;    //把起点放入开启列表    putInOpenList(iter);

代码2.2.4.6—初始化A*

2.3 A*算法总结

      这里我们按顺序写一遍A*算法的完整代码，默认是折叠的。看了上文自己做代码的朋友如果实际操作遇到问题，就可以参考以下代码：（下面的代码因为课设加了一些无关紧要的功能）

1 #pragma once 2  3 #define MAX_number 5 4  5 //一个比较基础的结构体，用于和地图联动 6 struct baseNode 7 { 8     int i; 9     int j;10     int weigt;11 12     int f;13     int g;14     int h;15 16     struct baseNode *father;17 };18 19 //定义了一个全局变量用来存储二维矩阵地图20 struct baseNode map[MAX_number][MAX_number];21 22 //用于记录景点的数组元素23 struct scenerySpotsList24 {25     struct baseNode node;26     char placeName[20];27 };28 29 //邻居列表，用于记录每个当前点的所有邻居30 struct baseNode Neibo[8];31 32 //记录景点，起点和终点的数组33 struct scenerySpotsList AsceneryList[MAX_number*MAX_number];34 35 //开启列表，用于A*算法36 struct baseNode OpenList[MAX_number*MAX_number];37 38 //关闭列表，用于A*算法39 struct baseNode CloseList[MAX_number*MAX_number];40 41 //用于记录现在的景点个数，第1个景点记录在AsceneryList【2】里，AsceneryList【0】和AsceneryList【1】分别用来记录起点和终点42 int sceneryCount = 2;43 44 //用于记录开启列表中元素的个数45 int openListCount = 0;46 47 //用于记录关闭列表中元素的个数48 int closeListCount = 0;49 50 //用于记录邻居的个数51 int neibNum = 0;52 53 //用来储存整理后的路径54 struct baseNode Astack[MAX_number*MAX_number];55 56 //用来记录路径经过的点的个数57 int AstackCount = 0;

part1为数据定义的头文件

1 #pragma once  2   3 #include 
      
         4 #include 
       
          5 #include 
        
           6 #include 
         
            7 #include "data_define.h"  8   9 //设定地图的函数 10 void inputmap() 11 { 12     printf("目前地图大小为%d * %d。\n",MAX_number,MAX_number); 13     printf("请通过输入整数的形式填充地图，0代表地形不可通过，\n其他正整数代表权值，权值越大，地形越不方便通过\n\n"); 14  15     for (int i = 0; i 
          
           = 0;--i)187     {188         if (OpenList[i].i==iter.i&&OpenList[i].j==iter.j)189         {190             break;191         }192     }193 194     for (int j = i; j < openListCount-1; ++j)195     {196         OpenList[j] = OpenList[j+1];197     }198     --openListCount;199 }200 201 //对于一路上的每个点，分析它的最多八个邻居，并加入邻居列表202 void addNeibo(struct baseNode iter)203 {204     neibNum = 0;205 206     for (int i = iter.i - 1; i <= iter.i + 1; ++i)207     {208         for (int j = iter.j - 1; j <= iter.j + 1; ++j)209         {210             if (i >= 0 && i <= MAX_number - 1 && j >= 0 && j <= MAX_number - 1)211             {212                 if (i == iter.i&&j == iter.j)213                 {214                 }215                 else216                 {217                     map[i][j].h = manhatten(i, j);218 219                     Neibo[neibNum] = map[i][j];220                     ++neibNum;221                 }222             }223         }224     }225 }226 227 //查看临近格在不在开启列表中的函数228 int isInOpenList(struct baseNode neibo)229 {230     for (int i = 0; i < openListCount - 1; ++i)231     {232         if (OpenList[i].i == neibo.i&&OpenList[i].j == neibo.j)233         {234             return 1;235         }236     }237     return 0;238 }239 240 //查看指定的temp在不在关闭列表中的函数241 int isInCloseList(struct baseNode temp)242 {243     for (int i = 0; i < closeListCount-1; ++i)244     {245         if (CloseList[i].i == temp.i&&CloseList[i].j == temp.j)246         {247             return 1;248         }249     }250     return 0;251 }252 253 //A*中的启发式函数，用于求指定位置和终点之间的曼哈顿距离254 int manhatten(int i, int j)255 {256     return (abs(AsceneryList[1].node.i - i) + abs(AsceneryList[1].node.j - j))*10;257 }258 259 //求当前点与父亲节点的距离260 int increment(struct baseNode this)261 {262     if ((abs(this.father->i-this.i)==1) && (abs(this.father->j - this.j) == 1))263     {264         return 14*this.weigt;265     }266     else if ((this.father->i - this.i) == 0 && (this.father->j - this.j) == 0)267     {268         return 0;269     }270     else271     {272         return 10*this.weigt;273     }274 }275 276 //求出用当前点作为父节点时这个点的G值277 int NewG(struct baseNode this,struct baseNode father)278 {279     if (abs(father.i - this.i) == 1 && abs(father.j - this.j) == 1)280     {281         return father.g+14;282     }283     else if (abs(father.i - this.i) == 0 && abs(father.j - this.j) == 0)284     {285         return father.g;286     }287     else288     {289         return father.g+10;290     }291 }292 293 //把A*算法的节点按倒序整理到Astack里面294 void arrange(struct baseNode iter)295 {296     AstackCount = 0;297     for (; ; iter=map[iter.father->i][iter.father->j])298     {299         Astack[AstackCount] = iter;300         ++AstackCount;301         if (iter.i == AsceneryList[0].node.i&&iter.j == AsceneryList[0].node.j)302         {303             break;304         }305     }306 }307 308 //打印出A*算法的路径矩阵309 printAstar()310 {311     printf("A为最佳路径，Q为不经过区域\n\n");312     int boole = 0;313 314     for (int i = 0; i < MAX_number; ++i)315     {316         for (int j = 0; j < MAX_number; ++j)317         {318             for (int w=0; w
          
         
        
       
      

part2为函数的头文件

1 #pragma once 2  3 #include "funcs.h" 4  5 //用于控制整个校园导航系统流程 6 void testStart() 7 { 8     int flag1 = 0, flag3 = 0; 9 10     for (;;)11     {12         printf("基于A*算法的校园导航系统程序\n\n");13 14         printf("你可以进行以下操作：\n");15         printf("1.设定校园地图地形\n");16         printf("2.设定校园景点\n");17         printf("3.设定寻径的起点和终点\n");18         printf("4.找出最佳路径\n");19         printf("0.退出系统\n\n");20 21         int userInput = 0;22         scanf("%d", &userInput);23 24         switch (userInput)25         {26         case 1:27             inputmap(); 28             flag1 = 1;29             printf("设定校园地图地形成功\n\n");30             break;31 32         case 2:33             if (flag1==1)34             {35                 setView();36                 printf("校园景点设定完毕\n\n");37             }38             else39             {40                 printf("请先完成地图设定\n");41             }42             break;43 44         case 3:45             if (flag1 == 1)46             {47                 setRoad();48                 flag3 = 1;49                 printf("起点终点设定完毕\n\n");50             }51             else52             {53                 printf("请先完成地图设定\n");54             }55             break;56 57         case 4:58             if (flag1 == 1&&flag3==1)59             {60                 Astar();61                 printf("寻径完毕\n\n");62             }63             else64             {65                 printf("请先完成地图、起点终点设定\n");66             }67             break;68 69         case 0:70             exit(0);71             break;72 73         default:74             printf("输入不在指定范围内，请重新输入\n\n");75             break;76         }77     }78 }

part3为控制流程的头文件，被主函数调用

1 #include "process_control.h"2 3 int main()4 {5     testStart();6 7     return 0;8 }

part4为主函数

     其实，了解数据结构的人会看出来，A*里的开启列表每次都要找出里面的最小值，本文中逐个搜索取最小值的方法并不是最好的方法，这涉及到查找，二叉排序树等等知识，在下一篇文章中，我们开始正式分析如何优化这个算法。

     作为参考，这篇文章里的程序在VS2015中运行的结果差不多是这样的：

         

 

 点击查看文章第三部分的内容。