A star算法优化一

A*算法 求最优解

算法一直维护两个表: Open和Close

• 将起点S加入Open中

• 将所有S可到达的点（障碍物以及位于Close表中的点均看成不可达）加入到Open中。将起点从Open中删去，并加入到Close中

• ①从Open中删去F值最小的点Min，并将其加入到Close中

• ②将Min点所有可到达的点加入Open中，并设这些点的父节点为Min。若某点已经在Open中，则比较其F值，若新路径F值较小，说明从Min走路更短，更新其父节点为Min；否则不更新此点

• 循环①②，直到Open中出现目的点E 
  

公式表示为： f(n)=g(n)+h(n),

其中 f(n) 是从初始状态经由状态n到目标状态的代价估计，

g(n) 是在状态空间中从初始状态到状态n的实际代价，

h(n) 是从状态n到目标状态的最佳路径的估计代价。

通俗一点讲:

g(n)代表你从起始点到下一点的实际距离（制定到下一点的距离的规则）

h(n)是自己设计的函数，可以是到目的地大致的距离

可将循环过程封装成函数：

[cpp]  view plain  copy

1.     while (isNotEnd()) {  
2.         Find_deleteMinFromOpen_AddToClose();  
3.         putReachableIntoOpen(close.back());  
4.     }  

举个栗子：

对于以下图：5行15列

000000000000000

0000000x0000000

00s0000x0000e00

0000000x0000000

000000000000000

其中x为墙壁，s为起点，e为终点，建立合适的模型，调用A star算法，找到一条s到e的最短路径。

取直走G值为10，斜走G值为14

这里H值设定为无视障碍到达终点所需的 步数*10

我们看开始的几步：

000000000000000

0000000x0000000

00s0000x0000e00

0000000x0000000

000000000000000

灰色的点G=10，H=9*10 ,其F值最小，加入Close

000000000000000

0000000x0000000

00s0000x0000e00

0000000x0000000

000000000000000

灰色的点G=10+10，H=8*10 ,其F值最小，加入Close

000000000000000

0000000x0000000

00s0000x0000e00

0000000x0000000

000000000000000

灰色的点G=10+10+10，H=7*10 ,其F值最小，加入Close

000000000000000

0000000x0000000

00s0000x0000e00

0000000x0000000

000000000000000

灰色的点G=10+10+10+10，H=6*10 ,其F值最小，加入Close

以此循环，直到e在Open中，此时只需要沿着父节点往回走就可以到达起点了，这条路就是当前情况下最优的解

结果：

000000000000000

0000000x0000000

00s0000x0000e00

0000000x0000000

000000000000000

C++实现：

[cpp]  view plain  copy

1. #include#include#include#includeusing namespace std;  
2. char square[5][15] = {//待求数据  
3.     '0','0','0','0','0','0','0','0','0','0','0','0','0','0','0',  
4.     '0','0','0','0','0','0','0','x','0','0','0','0','0','0','0',  
5.     '0','0','s','0','0','0','0','x','0','0','0','0','e','0','0',  
6.     '0','0','0','0','0','0','0','x','0','0','0','0','0','0','0',  
7.     '0','0','0','0','0','0','0','0','0','0','0','0','0','0','0'  
8. };  
9.   
10. class point {  
11.   
12. public:  
13.     point(char s) {  
14.         v = s;  
15.         G = 0;  
16.         H = 0;  
17.         F = 0;  
18.     }  
19.     pair ParentPosi;  
20.     pair posi;  
21.     char v;//value  
22.     int F;  
23.     int G;  
24.     int H;  
25.     int UpdateF() {  
26.         F = G + H;  
27.         return F;  
28.     }  
29.     int UpdateH() {  
30.         int x = posi.first - 2;  
31.         int y = posi.second - 12;  
32.         x *= 10;  
33.         y *= 10;  
34.         if (x < 0) {  
35.             x = -x;  
36.         }  
37.         if (y < 0) {  
38.             y = -y;  
39.         }  
40.         H = x + y;  
41.         return H;  
42.     }  
43.     void setPosi(pair x) {  
44.         posi = x;  
45.     }  
46.     void setParentPosi(pair x) {  
47.         ParentPosi= x;  
48.     }  
49.     void setG(int g) {  
50.         G = g;  
51.     }  
52.     void setH(int h) {  
53.         H = h;  
54.     }  
55.     point &operator = (point &s) {  
56.         (*this).v=(s).v;  
57.         (*this).ParentPosi = s.ParentPosi;  
58.         (*this).posi = s.posi;  
59.         (*this).F = s.F;  
60.         (*this).G = s.G;  
61.         (*this).H = s.H;  
62.         return *this;  
63.     }  
64. };  
65. vector open;  
66. vector close;  
67. point squ[5][15] = {  
68.     0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,  
69.     0,0,0,0,0,0,0,'x',0,0,0,0,0,0,0,  
70.     0,0,'s',0,0,0,0,'x',0,0,0,0,'e',0,0,  
71.     0,0,0,0,0,0,0,'x',0,0,0,0,0,0,0,  
72.     0,0,0,0,0,0,0,0,0,0,0,0,0,0,0  
73. };  
74. bool isInOpenList(pair s) {  
75.     for (int i = 0;i<open.size();i++) {  
76.         if (open[i].posi == s) {  
77.             return true;  
78.         }  
79.     }  
80.     return false;  
81. }  
82. bool isInCloseList(pair s) {  
83.     for (int i = 0;i<close.size();i++) {  
84.         if (close[i].posi == s) {  
85.             return true;  
86.         }  
87.     }  
88.     return false;  
89. }  
90. void putReachableIntoOpen(point min) {  
91.     int x = min.posi.first;  
92.     int y = min.posi.second;  
93.   
94.     int direc[8][2] = {  
95.         0,1,  
96.         1,1,  
97.         1,0,  
98.         1,-1,  
99.         0,-1,  
100.         -1,-1,  
101.         -1,0,  
102.         -1,1  
103.     };  
104.     for (int i = 0;i < 8;i++) {  
105.         x = x + direc[i][0];  
106.         y = y + direc[i][1];  
107.         if (isInOpenList(make_pair(x, y))&&close.size()>0) {  
108.             int tempi = 0;  
109.             for (int i = 0;i < open.size();i++) {  
110.                 if (open[i].posi == make_pair(x, y)) {  
111.                     tempi = i;  
112.                 }  
113.             }  
114.             if (direc[i][0] * direc[i][1] != 0) {//斜向  
115.                 int G_now = close.back().G + 14;  
116.                 if (G_now < open[tempi].G) { //G比较小就更新路径  
117.                     open[tempi].ParentPosi = make_pair(x, y);  
118.                     squ[open[tempi].posi.first][open[tempi].posi.second].ParentPosi = make_pair(x, y);  
119.                 }  
120.             }  
121.             else {  
122.                 int G_now = close.back().G + 10;  
123.             }  
124.             continue;  
125.         }  
126.         //既不在关闭也不在开启列表中而且可到达 就将其加入开启列表  
127.         if ((!isInOpenList(make_pair(x, y))) && (!isInCloseList(make_pair(x,y)))&&x >= 0 && x < 5 && square[x][y] != 'x') {  
128.             squ[x][y].setParentPosi(min.posi);  
129.             open.push_back(squ[x][y]);  
130.             if (direc[i][0] * direc[i][1] != 0) {//斜向  
131.                 squ[x][y].setG(squ[x][y].G+14);  
132.             }  
133.             else {  
134.                 squ[x][y].setG(squ[x][y].G + 10);  
135.             }  
136.             //cout << "(" << squ[x][y].posi.first << "," << squ[x][y].posi.second << ")" << endl;  
137.         }  
138.         x = x - direc[i][0];  
139.         y = y - direc[i][1];  
140.     }  
141.     //cout << "------------------------" << "(" << x << "," << y << "):" << "------------------------" << endl;  
142. }  
143. void Find_deleteMinFromOpen_AddToClose() {  
144.     point min_= open[0];  
145.     int tempi = 0;  
146.     for (int i = 0;i < open.size();i++) {  
147.         if (open[i].UpdateF() < min_.UpdateF()) {  
148.             min_ = open[i];  
149.             tempi = i;  
150.         }  
151.     }  
152.     close.push_back(min_);  
153.     std::vector::iterator it=open.begin()+tempi;  
154.     open.erase(it);  
155.     //cout << "close:           (" << min_.posi.first << "," << min_.posi.second << ")" << endl;  
156.     //cout << "closeSize()=" << close.size() << endl;  
157.     //cout << "openSize()=" << open.size() << endl;  
158. }  
159. bool isNotEnd() {  
160.     for (int i=0;i<open.size();i++) {  
161.         if (open[i].v == 'e') {  
162.             open[i].ParentPosi=close.back().posi;  
163.             return false;  
164.         }  
165.     }  
166.     return true;  
167. }  
168.   
169. void findPath(pair begin,pairend) {  
170.     //将起点放入open  
171.     open.push_back(squ[2][2]);  
172.     putReachableIntoOpen(squ[2][2]);  
173.     int tempi = 0;  
174.     for (int i = 0;i < open.size();i++) {  
175.         if (open[i].v == 's') {  
176.             tempi = i;  
177.         }  
178.     }  
179.     std::vector::iterator it = open.begin()+tempi;//删除起点  
180.   
181.       
182.     while (isNotEnd()) {  
183.         Find_deleteMinFromOpen_AddToClose();  
184.         putReachableIntoOpen(close.back());  
185.     }  
186. }  
187. void print_path() {  
188.     for (int i = 0;i < 5;i++) {  
189.         for (int j = 0;j < 15;j++) {  
190.             squ[i][j].posi = make_pair(i, j);