公司在做機器人相關的項目，用到了A star算法，因此今天有時間好好研究下該算法
????A*算法是很經典的只能啟發式搜索算法，關于只能搜索算法和一般的搜索算法(例如DFS，BFS之類)，在語言描述上的區別，我覺得用《代碼大全》中的一句話描述的非常好：“駕駛汽車達到某人家，寫成算法是：沿167號高速往南行至Puyallup，從XX出口后往山上開4.5英里，在一個雜貨店旁邊的紅綠燈右轉，接著在第一個路口左轉，從左邊的褐色大房子的車道進去就是；而啟發式是：找出上一次我們給你寄的信，照著信上地址開車到這個鎮，到了那里你問一下我們房子在那里，這里的每一個人都認識我們，肯定會有人愿意幫助你，如果你找不到人，就找一個電話亭打電話給我們，我們出來接你”。聽上去還是有點抽象。那么下面我們具體看看A*算法！
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ??
我們要想從綠色的點到紅色的點，需要啟發式得去找一條路徑，到底怎么去找呢，開始沒有辦法，只能從開始點慢慢嘗試！我們需要定義一個OPEN表，OPEN表中放的是什么呢？就是當前考慮的點，及其周邊的點需要添加進來，作為可能的路徑上的點。這樣說可能有點抽象，那么先看下面：
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ??
我們從起始點開始搜索：
? ?1：從點A開始，并且把它作為待處理點存入一個OPEN表。你的路徑可能會通過它包含的方格，也可能不會。基本上，這是一個待檢查方格的列表。這些鄰居節點認當前點為父節點 。(父節點的保存是必須的！因為當我們找到最后一個點的時候，需要通過父節點回溯，找到所有點直到開始節點！那么就是一個完整的路徑！)從開啟列表中刪除點 A ，把它加入到一個 CLOSE列表 ，列表中保存所有不需要再次檢查的方格。
在這一點，你應該形成如圖的結構。在圖中，暗綠色方格是你起始方格的中心。它被用淺藍色描邊，以表示它被加入到關閉列表中了。所有的相鄰格現在都在開啟列表中，它們被用淺綠色描邊。每個方格都有一個灰色指針反指他們的父方格，也就是開始的方格。
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ??
結下來我們應該選擇哪一個點繼續考慮呢！我想大家應該知道所謂的啟發函數，所謂權值之類(此處所謂的權值就是路勁的長度)。YES，我們需要OPEN表中權值F最小的那個點！為什么呢，當然是權值越小，越靠近目標點咯！
對于權值我們設為F，那么F怎么計算到的！我們有兩個項！G和H，?
G = 從起點A，沿著產生的路徑，移動到網格上指定方格的路徑耗費。預估 移動耗費。這經常被稱為啟發式 的。這樣叫的原因是因為它只是個猜測。我們沒辦法事先知道路徑的長度。(但是我們需要知道：雖然只是猜測，但是只要是基于一個統一的標準，相對遠近的趨勢是不變的！這一點是很重要的！ ?)
例如：H值 的估計采用“曼哈頓”法， 也就是當前的點，到目標點，橫向和縱向的格子數相加，就是H值！
? ? ? ? ? ?( 注意：對于障礙物我們是不考慮是否跳過的問題！即，障礙物也當做正常的一個格子！這也是H值僅僅是預測的而已的原因！即所謂啟發式！ )
那么對于第一幅圖，起始點離終點的H值是：橫向相差4個格子，縱向相差0個格子，那么H=4+0=4；
當然也有其他的辦法，例如使用直線距離，sqrt( pow( des.x - src.x ) + pow( des.y - src.y ) )也是可以的~
對于G值！ 在這個例子，令水平或者垂直移動的耗費為10 ，對角線方向耗費為14 。我們取這些值是因為沿對角線的距離是沿水平或垂直移動耗費的的根號2 約1.414 倍。為了簡化，我們用10 和14 近似。有時候簡化是很重要的~
( 其實距離只要反應了基本的倍數關系就可以了！ )
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ??
對于G值，只要是橫向和豎向的鄰居，設為+10，斜向的鄰居+14就可以了~計算真的很easy吧~呵呵~
對于H值，就是數格子就是咯~
F = G + H
注意上面的鄰居節點都加入OPEN表了哦~~~ ? 起點從OPEN中刪除，加入CLOSE中~
接著計算：
( 至于我們怎么選出最小的那個點呢！？我們使用堆排序處理的，對于選出最小值是很快的~ )
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ??
此時對于其鄰居有幾種可能性
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?
? ? ? ? ? ? ? ? ? ? ? ? ?? ? ? ???
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ???
1：鄰居已經在CLOSE表中了，那么不需要考慮了~
??注意：當父節點改變后，OPEN表中的最小點可能會變化，那么需要再次排序得到最
基本的原理就是這樣的~
[cpp] ?view plaincopy
#include?<stdio.h> ?? #include?<stdlib.h> ?? ?? #define?STARTNODE???1 ?? #define?ENDNODE?????2 ?? #define?BARRIER?????3 ?? ?? typedef ? struct ?AStarNode?? {?? ????int ?s_x;???? ?? ????int ?s_y;?? ????int ?s_g;???? ?? ????int ?s_h;???? ?? ????int ?s_style; ?? ????struct ?AStarNode?*?s_parent;???? ?? ????int ?s_is_in_closetable;????? ?? ????int ?s_is_in_opentable;?????? ?? }AStarNode,?*pAStarNode;?? ?? AStarNode??map_maze[10][10];?????????? pAStarNode?open_table[100];??????? pAStarNode?close_table[100];?????????? int <span?style= "white-space:pre" >???</span>???open_node_count;??<span?style= "white-space:pre" >??</span> ?? int ????close_node_count;????<span?style= "white-space:pre" >??</span> ?? pAStarNode?path_stack[100];??????? int ????????top?=?-1;???????????? ?? ?? ?? ?? ?? void ?swap(? int ?idx1,? int ?idx2?)???? {???? ????pAStarNode?tmp?=?open_table[idx1];?? ????open_table[idx1]?=?open_table[idx2];?? ????open_table[idx2]?=?tmp;?? }???? ?? ?? ?? void ?adjust_heap(? int ? nIndex?)?????? {????? ????int ?curr?=?nIndex;?? ????int ?child?=?curr?*?2?+?1;??? ?? ????int ?parent?=?(?curr?-?1?)?/?2;?? ?? ?? ????if ?(nIndex?<?0?||?nIndex?>=?open_node_count)?? ????{?? ????????return ;?? ????}?? ?????? ?????? ?????? ????while ?(?child?<?open_node_count?)?? ????{?? ?????????? ?????????? ????????if ?(?child?+?1?<?open_node_count?&&?open_table[child]->s_g?+?open_table[child]->s_h??>?open_table[child+1]->s_g?+?open_table[child+1]->s_h?)?? ????????{?? ????????????++child;<span?style="white-space:pre" >??????????????</span> ?? ????????}?? ?????????? ????????if ?(open_table[curr]->s_g?+?open_table[curr]->s_h?<=?open_table[child]->s_g?+?open_table[child]->s_h)?? ????????{?? ????????????break ;?? ????????}?? ????????else ?? ????????{?? ????????????swap(?child,?curr?);?????????????? ????????????curr?=?child;????????????????? ????????????child?=?curr?*?2?+?1;????????????? ????????}?? ????}?? ?????? ????if ?(curr?!=?nIndex)?? ????{?? ????????return ;?? ????}?? ?? ?????? ?????? ????while ?(curr?!=?0)?? ????{?? ????????if ?(open_table[curr]->s_g?+?open_table[curr]->s_h?>=?open_table[parent]->s_g?+?open_table[parent]->s_h)?? ????????{?? ????????????break ;?? ????????}?? ????????else ?? ????????{?? ????????????swap(?curr,?parent?);?? ????????????curr?=?parent;?? ????????????parent?=?(curr-1)/2;?? ????????}?? ????}?? }???? ?? ?? ?? void ?insert_to_opentable(? int ?x,? int ?y,?pAStarNode?curr_node,?pAStarNode?end_node,? int ?w?)?? {?? ????int ?i;?? ?? ????if ?(?map_maze[x][y].s_style?!=?BARRIER?)???????? ?? ????{?? ????????if ?(?!map_maze[x][y].s_is_in_closetable?)??? ?? ????????{?? ????????????if ?(?map_maze[x][y].s_is_in_opentable?)? ?? ????????????{?? ?????????????????? ?????????????????? ????????????????if ?(?map_maze[x][y].s_g?>?curr_node->s_g?+?w?)???? ?? ????????????????{?? ????????????????????map_maze[x][y].s_g?=?curr_node->s_g?+?w;?? ????????????????????map_maze[x][y].s_parent?=?curr_node;?? ?? ????????????????????for ?(?i?=?0;?i?<?open_node_count;?++i?)?? ????????????????????{?? ????????????????????????if ?(?open_table[i]->s_x?==?map_maze[x][y].s_x?&&?open_table[i]->s_y?==?map_maze[x][y].s_y?)?? ????????????????????????{?? ????????????????????????????break ;?? ????????????????????????}?? ????????????????????}?? ?? ????????????????????adjust_heap(?i?);????????????????????? ????????????????}?? ????????????}?? ????????????else ???????????????????????????????????? ?? ????????????{?? ????????????????map_maze[x][y].s_g?=?curr_node->s_g?+?w;?? ????????????????map_maze[x][y].s_h?=?abs(end_node->s_x?-?x?)?+?abs(end_node->s_y?-?y);?? ????????????????map_maze[x][y].s_parent?=?curr_node;?? ????????????????map_maze[x][y].s_is_in_opentable?=?1;?? ????????????????open_table[open_node_count++]?=?&(map_maze[x][y]);?? ????????????}?? ????????}?? ????}?? }?? ?? ?? ?? ?? void ?get_neighbors(?pAStarNode?curr_node,?pAStarNode?end_node?)?? {?? ????int ?x?=?curr_node->s_x;?? ????int ?y?=?curr_node->s_y;?? ?? ?????? ?????? ????if ?(?(?x?+?1?)?>=?0?&&?(?x?+?1?)?<?10?&&?y?>=?0?&&?y?<?10?)?? ????{?? ????????insert_to_opentable(?x+1,?y,?curr_node,?end_node,?10?);?? ????}?? ?? ????if ?(?(?x?-?1?)?>=?0?&&?(?x?-?1?)?<?10?&&?y?>=?0?&&?y?<?10?)?? ????{?? ????????insert_to_opentable(?x-1,?y,?curr_node,?end_node,?10?);?? ????}?? ?? ????if ?(?x?>=?0?&&?x?<?10?&&?(?y?+?1?)?>=?0?&&?(?y?+?1?)?<?10?)?? ????{?? ????????insert_to_opentable(?x,?y+1,?curr_node,?end_node,?10?);?? ????}?? ?? ????if ?(?x?>=?0?&&?x?<?10?&&?(?y?-?1?)?>=?0?&&?(?y?-?1?)?<?10?)?? ????{?? ????????insert_to_opentable(?x,?y-1,?curr_node,?end_node,?10?);?? ????}?? ?? ????if ?(?(?x?+?1?)?>=?0?&&?(?x?+?1?)?<?10?&&?(?y?+?1?)?>=?0?&&?(?y?+?1?)?<?10?)?? ????{?? ????????insert_to_opentable(?x+1,?y+1,?curr_node,?end_node,?14?);?? ????}?? ?? ????if ?(?(?x?+?1?)?>=?0?&&?(?x?+?1?)?<?10?&&?(?y?-?1?)?>=?0?&&?(?y?-?1?)?<?10?)?? ????{?? ????????insert_to_opentable(?x+1,?y-1,?curr_node,?end_node,?14?);?? ????}?? ?? ????if ?(?(?x?-?1?)?>=?0?&&?(?x?-?1?)?<?10?&&?(?y?+?1?)?>=?0?&&?(?y?+?1?)?<?10?)?? ????{?? ????????insert_to_opentable(?x-1,?y+1,?curr_node,?end_node,?14?);?? ????}?? ?? ????if ?(?(?x?-?1?)?>=?0?&&?(?x?-?1?)?<?10?&&?(?y?-?1?)?>=?0?&&?(?y?-?1?)?<?10?)?? ????{?? ????????insert_to_opentable(?x-1,?y-1,?curr_node,?end_node,?14?);?? ????}?? }?? ?? ?? int ?main()?? {??? ?????? ?????? ????AStarNode?*start_node;???????????? ????AStarNode?*end_node;?????????????? ????AStarNode?*curr_node;????????????? ????int ???????is_found;????????? ?? ????int ?maze[][10]?={??????????? ?? ????????????????????????{?1,0,0,3,0,3,0,0,0,0?},?? ????????????????????????{?0,0,3,0,0,3,0,3,0,3?},?? ????????????????????????{?3,0,0,0,0,3,3,3,0,3?},?? ????????????????????????{?3,0,3,0,0,0,0,0,0,3?},?? ????????????????????????{?3,0,0,0,0,3,0,0,0,3?},?? ????????????????????????{?3,0,0,3,0,0,0,3,0,3?},?? ????????????????????????{?3,0,0,0,0,3,3,0,0,0?},?? ????????????????????????{?0,0,2,0,0,0,0,0,0,0?},?? ????????????????????????{?3,3,3,0,0,3,0,3,0,3?},?? ????????????????????????{?3,0,0,0,0,3,3,3,0,3?},?? ????????????????????};?? ????int ???????i,j,x;?? ?????? ?????? ?????? ????for (?i?=?0;?i?<?10;?++i?)?? ????{?? ????????for ?(?j?=?0;?j?<?10;?++j?)?? ????????{?? ????????????map_maze[i][j].s_g?=?0;?? ????????????map_maze[i][j].s_h?=?0;?? ????????????map_maze[i][j].s_is_in_closetable?=?0;?? ????????????map_maze[i][j].s_is_in_opentable?=?0;?? ????????????map_maze[i][j].s_style?=?maze[i][j];?? ????????????map_maze[i][j].s_x?=?i;?? ????????????map_maze[i][j].s_y?=?j;?? ????????????map_maze[i][j].s_parent?=?NULL;?? ?? ????????????if ?(?map_maze[i][j].s_style?==?STARTNODE?)?? ?? ????????????{?? ????????????????start_node?=?&(map_maze[i][j]);?? ????????????}?? ????????????else ? if (?map_maze[i][j].s_style?==?ENDNODE?)???? ?? ????????????{?? ????????????????end_node?=?&(map_maze[i][j]);?? ????????????}?? ?? ????????????printf("%d?" ,?maze[i][j]);?? ????????}?? ?? ????????printf("\n" );?? ????}?? ?? ?????? ?????? ????open_table[open_node_count++]?=?start_node;??????????? ?????? ????start_node->s_is_in_opentable?=?1;????????????????? ????start_node->s_g?=?0;?? ????start_node->s_h?=?abs(end_node->s_x?-?start_node->s_x)?+?abs(end_node->s_y?-?start_node->s_y);?? ????start_node->s_parent?=?NULL;?? ?????? ????if ?(?start_node->s_x?==?end_node->s_x?&&?start_node->s_y?==?end_node->s_y?)?? ????{?? ????????printf("起點==終點！\n" );?? ????????return ?0;?? ????}?? ?????? ????is_found?=?0;?? ?? ????while (?1?)?? ????{?? ?????????? ?????????? ? ? ? ? ? ?? ????????curr_node?=?open_table[0];???????? ????????open_table[0]?=?open_table[--open_node_count];???? ????????adjust_heap(?0?);????????????????? ?????????? ????????close_table[close_node_count++]?=?curr_node;?????? ????????curr_node->s_is_in_closetable?=?1;????????? ?? ????????if ?(?curr_node->s_x?==?end_node->s_x?&&?curr_node->s_y?==?end_node->s_y?) ?? ????????{?? ????????????is_found?=?1;?? ????????????break ;?? ????????}?? ?? ????????get_neighbors(?curr_node,?end_node?);????????????? ?? ????????if ?(?open_node_count?==?0?)????????????? ?? ????????{?? ????????????is_found?=?0;?? ????????????break ;?? ????????}?? ????}?? ?? ????if ?(?is_found?)?? ????{?? ????????curr_node?=?end_node;?? ?????????? ????????while (?curr_node?)?? ????????{?? ????????????path_stack[++top]?=?curr_node;?? ????????????curr_node?=?curr_node->s_parent;?? ????????}?? ?? ????????while (?top?>=?0?)???????? ?? ????????{?? ????????????if ?(?top?>?0?)?? ????????????{?? ????????????????printf("(%d,%d)-->" ,?path_stack[top]->s_x,?path_stack[top--]->s_y);?? ????????????}?? ????????????else ?? ????????????{?? ????????????????printf("(%d,%d)" ,?path_stack[top]->s_x,?path_stack[top--]->s_y);?? ????????????}?? ????????}?? ????}?? ????else ?? ????{?? ????????printf("么有找到路徑" );?? ????}?? ?? ????puts("" );?? ?? ????return ?0;?? }?? 總結
以上是生活随笔 為你收集整理的A star算法 的全部內容，希望文章能夠幫你解決所遇到的問題。
如果覺得生活随笔 網站內容還不錯，歡迎將生活随笔 推薦給好友。
