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)是自己設計的函數，可以是到目的地大致的距離
可將循環過程封裝成函數：
while (isNotEnd()) {
Find_deleteMinFromOpen_AddToClose();
putReachableIntoOpen(close.back());
}
舉個栗子：
對於以下圖：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++實現：
#include#include#include#includeusing namespace std;
char square[5][15] = {//待求數據
'0','0','0','0','0','0','0','0','0','0','0','0','0','0','0',
'0','0','0','0','0','0','0','x','0','0','0','0','0','0','0',
'0','0','s','0','0','0','0','x','0','0','0','0','e','0','0',
'0','0','0','0','0','0','0','x','0','0','0','0','0','0','0',
'0','0','0','0','0','0','0','0','0','0','0','0','0','0','0'
};
class point {
public:
point(char s) {
v = s;
G = 0;
H = 0;
F = 0;
}
pair ParentPosi;
pair posi;
char v;//value
int F;
int G;
int H;
int UpdateF() {
F = G + H;
return F;
}
int UpdateH() {
int x = posi.first - 2;
int y = posi.second - 12;
x *= 10;
y *= 10;
if (x < 0) {
x = -x;
}
if (y < 0) {
y = -y;
}
H = x + y;
return H;
}
void setPosi(pair x) {
posi = x;
}
void setParentPosi(pair x) {
ParentPosi= x;
}
void setG(int g) {
G = g;
}
void setH(int h) {
H = h;
}
point &operator = (point &s) {
(*this).v=(s).v;
(*this).ParentPosi = s.ParentPosi;
(*this).posi = s.posi;
(*this).F = s.F;
(*this).G = s.G;
(*this).H = s.H;
return *this;
}
};
vector open;
vector close;
point squ[5][15] = {
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
0,0,0,0,0,0,0,'x',0,0,0,0,0,0,0,
0,0,'s',0,0,0,0,'x',0,0,0,0,'e',0,0,
0,0,0,0,0,0,0,'x',0,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
};
bool isInOpenList(pair s) {
for (int i = 0;i<open.size();i++) {
if (open[i].posi == s) {
return true;
}
}
return false;
}
bool isInCloseList(pair s) {
for (int i = 0;i<close.size();i++) {
if (close[i].posi == s) {
return true;
}
}
return false;
}
void putReachableIntoOpen(point min) {
int x = min.posi.first;
int y = min.posi.second;
int direc[8][2] = {
0,1,
1,1,
1,0,
1,-1,
0,-1,
-1,-1,
-1,0,
-1,1
};
for (int i = 0;i < 8;i++) {
x = x + direc[i][0];
y = y + direc[i][1];
if (isInOpenList(make_pair(x, y))&&close.size()>0) {
int tempi = 0;
for (int i = 0;i < open.size();i++) {
if (open[i].posi == make_pair(x, y)) {
tempi = i;
}
}
if (direc[i][0] * direc[i][1] != 0) {//斜向
int G_now = close.back().G + 14;
if (G_now < open[tempi].G) { //G比較小就更新路徑
open[tempi].ParentPosi = make_pair(x, y);
squ[open[tempi].posi.first][open[tempi].posi.second].ParentPosi = make_pair(x, y);
}
}
else {
int G_now = close.back().G + 10;
}
continue;
}
//既不在關閉也不在開啓列表中而且可到達 就將其加入開啓列表
if ((!isInOpenList(make_pair(x, y))) && (!isInCloseList(make_pair(x,y)))&&x >= 0 && x < 5 && square[x][y] != 'x') {
squ[x][y].setParentPosi(min.posi);
open.push_back(squ[x][y]);
if (direc[i][0] * direc[i][1] != 0) {//斜向
squ[x][y].setG(squ[x][y].G+14);
}
else {
squ[x][y].setG(squ[x][y].G + 10);
}
//cout << "(" << squ[x][y].posi.first << "," << squ[x][y].posi.second << ")" << endl;
}
x = x - direc[i][0];
y = y - direc[i][1];
}
//cout << "------------------------" << "(" << x << "," << y << "):" << "------------------------" << endl;
}
void Find_deleteMinFromOpen_AddToClose() {
point min_= open[0];
int tempi = 0;
for (int i = 0;i < open.size();i++) {
if (open[i].UpdateF() < min_.UpdateF()) {
min_ = open[i];
tempi = i;
}
}
close.push_back(min_);
std::vector::iterator it=open.begin()+tempi;
open.erase(it);
//cout << "close: (" << min_.posi.first << "," << min_.posi.second << ")" << endl;
//cout << "closeSize()=" << close.size() << endl;
//cout << "openSize()=" << open.size() << endl;
}
bool isNotEnd() {
for (int i=0;i<open.size();i++) {
if (open[i].v == 'e') {
open[i].ParentPosi=close.back().posi;
return false;
}
}
return true;
}
void findPath(pair begin,pairend) {
//將起點放入open
open.push_back(squ[2][2]);
putReachableIntoOpen(squ[2][2]);
int tempi = 0;
for (int i = 0;i < open.size();i++) {
if (open[i].v == 's') {
tempi = i;
}
}
std::vector::iterator it = open.begin()+tempi;//刪除起點
while (isNotEnd()) {
Find_deleteMinFromOpen_AddToClose();
putReachableIntoOpen(close.back());
}
}
void print_path() {
for (int i = 0;i < 5;i++) {
for (int j = 0;j < 15;j++) {
squ[i][j].posi = make_pair(i, j);
squ[i][j].UpdateH();//算出所有H
}
}//初始化point.posi
findPath(make_pair(2,2),make_pair(2,12));
point temp = squ[2][12];
vector<pair> point_out;
while (temp.posi!=squ[2][2].posi) {
//cout << "(" << temp.posi.first << "," << temp.posi.second << ")" << endl;
point_out.push_back(temp.posi);
temp=squ[temp.ParentPosi.first][temp.ParentPosi.second];
}
point_out.push_back(squ[2][2].posi);
while (point_out.size() != 0) {
cout << "(" << point_out.back().first<< "," << point_out.back().second<< ")" << endl;
point_out.pop_back();
}
}
void print() {
for (int i = 0;i < 5;i++) {
for (int j = 0;j < 15;j++) {
cout << square[i][j] << ' ';
}
cout << endl;
}
}
int main() {
//print();
print_path();
return 0;
}
