大家好，今天和各位分享一下機(jī)器人路徑規(guī)劃中非常經(jīng)典的 A* 算法，感興趣的點(diǎn)個(gè)關(guān)注，文末有 python 代碼，那我么開始吧。
1. 算法介紹
A* 算法是 1968 年 P.E.Hart[1]等人所提出的在全局地圖環(huán)境中所有已知情形下求解最短路徑問題的方法，由于其簡潔高效，容易實(shí)施等優(yōu)點(diǎn)而受到人們的廣泛關(guān)注。但是，A*算法在實(shí)際應(yīng)用過程中也暴露出其嚴(yán)重弊端，例如：在搜索空間較大的環(huán)境下，增加了算法的執(zhí)行時(shí)間，從而大大降低了路線規(guī)劃效果；復(fù)雜環(huán)境下，由于地圖精度不高，從而減小了獲取最佳路徑的可能性；在搜索過程中存在相同 F 值無法得到最佳路線等問題。
而面對上述問題，不同的研發(fā)人員也以不同的方法以對其做出了改善，并獲得了不少研究成果。
Korf 等人[2]在 A*算法的基礎(chǔ)上構(gòu)建了迭代改進(jìn)的 IDA* 算法，在計(jì)算過程中根據(jù)迭代次數(shù)得到最優(yōu)解，解決了A*算法容易陷入盲目搜索的不足；Tran HAM[3]等人在 A*算法基礎(chǔ)上構(gòu)建了一種基于視覺的路徑規(guī)劃和導(dǎo)航算法，然后對搜索的路徑進(jìn)行優(yōu)化。Szczerba[4]等人提出了稀疏 A*算法，在傳統(tǒng) A*算法基礎(chǔ)上添加相應(yīng)的約束條件，根據(jù)不同的約束條件修改 A*算法的搜索空間，提升了路線規(guī)劃效果，大大增加了獲取最佳路徑的可能性。
參考文獻(xiàn)：
[1] Hart P E, Nilsson N J, Raphael B. A formal basis for the heuristic determination of minimum cost paths[J]. IEEE transactions on Systems Science and Cybernetics, 1968, 4(2): 100-107.?
[2] Korf, ?Richard ?E. ?Depth-first ?iterative-deepening: ?an ?optimal ?admissible ?tree ?search[J]. Artificial Intelligence, 1985, 27(1):97-109.?
[3] Tran H ?A M, Ngo H Q ?T, ?Nguyen ?T ?P, ?et ?al. ?Implementation ?of ?vision-based ?autonomous mobile platform to control by A* algorithm[C]//2018 2nd International Conference on Recent Advances in Signal Processing, Telecommunications & omputing (SigTelCom). IEEE, 2018: 39-44. ?
[4] Szczerba R J, Galkowski P, Glicktein I S, et al. Robust algorithm for real-time route planning[J]. IEEE Transactions on Aerospace & Electronic Systems, 2000, 36(3):869-878. ??
2. 算法原理
經(jīng)典 A* 算法主要用在靜態(tài)且周圍環(huán)境已知的情況下，是建立在 Dijkstra 和BFS 基礎(chǔ)上的啟發(fā)式遍歷搜索算法，在路徑規(guī)劃時(shí)不僅要考慮自身與最近節(jié)點(diǎn)位置的距離（Dijkstra 實(shí)現(xiàn)），還需要考慮自身位置與目標(biāo)點(diǎn)的距離（BFS 實(shí)現(xiàn)）。與傳統(tǒng)的遍歷搜索算法相比，彌補(bǔ)了搜索過程中的貪心機(jī)制，從而很好地完成路徑規(guī)劃的任務(wù)。
2.1 基本原理
A* 算法以起點(diǎn)為中心開始進(jìn)行路徑規(guī)劃，并在路徑規(guī)劃過程中擴(kuò)展周圍的鄰近節(jié)點(diǎn)。通過 A*算法公式對起始點(diǎn)到當(dāng)前位置和當(dāng)前位置到目標(biāo)點(diǎn)的代價(jià)值進(jìn)行計(jì)算。在所有的節(jié)點(diǎn)中選取代價(jià)值最小的節(jié)點(diǎn)作為當(dāng)前最優(yōu)節(jié)點(diǎn)，以當(dāng)前最優(yōu)節(jié)點(diǎn)繼續(xù)搜索，直到搜索到目標(biāo)點(diǎn)，最后生成一條從起始點(diǎn)到目標(biāo)點(diǎn)的路徑。
A* 算法本身遵循 821 定律，同時(shí)在搜索過程中將節(jié)點(diǎn)的狀態(tài)定義為未檢查、待檢查、已檢查三種準(zhǔn)備狀態(tài)；8 是指以當(dāng)前定位點(diǎn)周圍的 8 個(gè)鄰域的代價(jià)值作為計(jì)算目標(biāo)，2 是指兩個(gè)參數(shù)列表，一個(gè)是 open list 列表，存放待檢查的節(jié)點(diǎn)；另外一個(gè)是 close list 列表，存放已檢查完成不再需要關(guān)注的節(jié)點(diǎn)，1 是指一個(gè)計(jì)算公式，如下。?
其中，f(n) 是指當(dāng)前節(jié)點(diǎn) n 的全局代價(jià)值，g(n) 是指從起始點(diǎn)到當(dāng)前節(jié)點(diǎn) n 的實(shí)際代價(jià)值，h(n) 是指當(dāng)前節(jié)點(diǎn) n 到終點(diǎn)的預(yù)測值。
2.2 案例解析
A* 算法整體搜索過程如下圖所示，圖中綠色格子為尋徑起始點(diǎn)，紅色格子為尋徑終止點(diǎn)，藍(lán)色代表障礙物。從起始點(diǎn)開始，將其放到 open list 列表中，同時(shí)讓其作為當(dāng)前節(jié)點(diǎn)。通過當(dāng)前節(jié)點(diǎn)查找與其相鄰的 8 個(gè)鄰域節(jié)點(diǎn)，如圖中帶有綠色線條的方格。8 個(gè)鄰域節(jié)點(diǎn)方格中，箭頭指向的方向?yàn)楫?dāng)前相鄰節(jié)點(diǎn) n 的父節(jié)點(diǎn)，用于代價(jià)值的計(jì)算以及最終路徑的回溯。?
以起始點(diǎn)作為當(dāng)前節(jié)點(diǎn)，通過 A* 算法代價(jià)公式計(jì)算相鄰節(jié)點(diǎn)的 f 值，并將起始節(jié)點(diǎn)放入 close list 列表中。起始點(diǎn)周圍每個(gè)方格節(jié)點(diǎn)的三個(gè)夾角處由數(shù)字標(biāo)識，其中左下角表示的是從起始點(diǎn)到當(dāng)前節(jié)點(diǎn) n 的真實(shí)代價(jià)值 g(n)，如果起始點(diǎn)到當(dāng)前節(jié)點(diǎn) n 是在橫向上往前/后移動(dòng)了一個(gè)節(jié)點(diǎn)，或者在豎直方向上往上/下移動(dòng)了一個(gè)節(jié)點(diǎn)，普遍設(shè)定 g(n) 移動(dòng)一個(gè)節(jié)點(diǎn)的代價(jià)為 10；如果起始點(diǎn)到當(dāng)前節(jié)點(diǎn) n 是經(jīng)過對角方向的移動(dòng)，普遍設(shè)定 g(n) 的移動(dòng)代價(jià)值為 14。右下角表示的是從當(dāng)前節(jié)點(diǎn) n 到終點(diǎn)的預(yù)估值 h(n)，通過啟發(fā)函數(shù)進(jìn)行計(jì)算。左上角表示的是全局代價(jià)值 f(n)，將以上計(jì)算出的 g(n)與 h(n)相加可得到對應(yīng)的 f(n)。?
基于以上計(jì)算，進(jìn)一步判斷當(dāng)前節(jié)點(diǎn)的 8 個(gè)鄰域節(jié)點(diǎn)是否在 open list 列表中，如果某個(gè)鄰域節(jié)點(diǎn)未在 open list 列表中，則將帶有 f? 值的鄰節(jié)點(diǎn)添加到 open?list 列表中；如果某個(gè)鄰域節(jié)點(diǎn)已經(jīng)存放在 open ?list 表中，則以當(dāng)前方格為父節(jié)點(diǎn)檢查經(jīng)由當(dāng)前父節(jié)點(diǎn)到達(dá)那個(gè)方格是否具有更小的 g 值，如果沒有不做任何操作，如果存在更小的 g 值，把那個(gè)方格的父節(jié)點(diǎn)設(shè)置為當(dāng)前方格，然后重新計(jì)算那個(gè)方格的 g 值和 f 值。判斷完成之后，獲取 open list 列表中最小代價(jià)值節(jié)點(diǎn)。從上圖中可以看出起始點(diǎn)周圍相鄰節(jié)點(diǎn)的全局代價(jià)值 f 根據(jù)從上到下、從左向右的順序分別為 124、110、104、110、90、104、90、84，從以上數(shù)字中選取 f 值最小的 84 的點(diǎn)作為當(dāng)前節(jié)點(diǎn)。?
循環(huán)上述搜索步驟，直至搜索到右側(cè)的紅色目標(biāo)點(diǎn)，將目標(biāo)點(diǎn)添加到 close list 列表中。根據(jù)close list 列表中的各個(gè)節(jié)點(diǎn)按照父節(jié)點(diǎn)的指向逆序輸出，從而得到搜索路徑。如下圖所示，帶有黃色邊框的格子節(jié)點(diǎn)為最終搜索到的最佳路徑。?
2.3 啟發(fā)函數(shù)
在路徑規(guī)劃時(shí)，通常需要尋找一些與搜索環(huán)境相關(guān)的特征信息，在計(jì)算過程中，將這些特征信息稱為啟發(fā)函數(shù)。啟發(fā)函數(shù)被用來估計(jì)從當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)生成的代價(jià)值。常用的啟發(fā)函數(shù)主要包括曼哈頓距離函數(shù)、歐氏距離函數(shù)、對角距離函數(shù)三種。?
假設(shè)起點(diǎn)的坐標(biāo)為 (??, ??)，終點(diǎn)的坐標(biāo)為 (??????????, ??????????)，對三種啟發(fā)函數(shù)進(jìn)行分析。?
（1）曼哈頓距離函數(shù)。曼哈頓距離為起點(diǎn)與終點(diǎn)坐標(biāo)點(diǎn)絕對值總和，公式如下：
（2）歐式距離函數(shù)。?歐式距離為搜索環(huán)境中起止兩節(jié)點(diǎn)間的最短直線距離，公式如下：
（3）對角距離函數(shù)。?對角距離首先允許沿對角線運(yùn)動(dòng)，然后通過水平和垂直運(yùn)動(dòng)完成剩下路線，
或者相反。公式如下：?
沿對角線運(yùn)動(dòng)的公式：
?沿水平和垂直運(yùn)動(dòng)的公式：
將對角線、水平和垂直運(yùn)動(dòng)組合構(gòu)成的對角距離公式：
下面對三種啟發(fā)函數(shù)做實(shí)驗(yàn)，其中圖中黃色方格節(jié)點(diǎn)為尋徑起始點(diǎn)，紅色方格節(jié)點(diǎn)為尋徑終點(diǎn)?？瞻讌^(qū)域?yàn)榭尚凶甙踩珔^(qū)，深紅色連續(xù)方格節(jié)點(diǎn)為不可通行障礙物區(qū)域。
圖中藍(lán)色曲線為從起始點(diǎn)到目標(biāo)點(diǎn)搜索的路徑，而路徑周圍黃色的方格節(jié)點(diǎn)代表在路徑搜索過程中添加到 open list 列表的節(jié)點(diǎn)。根據(jù)上述三種路徑搜索對比可知，在無障礙物情況下曼哈頓距離和對角距離運(yùn)行情況相對較好，而歐式距離搜索出的路徑不夠平滑；在存在障礙物情況下曼哈頓距離搜索路徑較平滑，歐式距離和對角距離在搜索過程中產(chǎn)生了大量的搜索節(jié)點(diǎn)，搜索路徑出現(xiàn)較多的轉(zhuǎn)折點(diǎn)，運(yùn)行時(shí)間較長。?
綜上所知，在 A*算法中選擇曼哈頓啟發(fā)函數(shù)不僅能減少搜索節(jié)點(diǎn)數(shù)，還能能到相對平滑路徑。
3. 代碼實(shí)現(xiàn)
3.1 算法基本步驟
（1）設(shè)置 open list 列表和 close list 列表，將起始點(diǎn)存放 open list 列表中，此時(shí) close list 列表為空；
（2）取出 open list 列表中 f 值最小的節(jié)點(diǎn) P 作為當(dāng)前節(jié)點(diǎn)，并進(jìn)行以下操作：判斷當(dāng)前節(jié)點(diǎn) P 是否為目標(biāo)點(diǎn) end，如果點(diǎn) P 為目標(biāo)點(diǎn)尋徑結(jié)束，將目標(biāo)點(diǎn)end 的父節(jié)點(diǎn)設(shè)置為節(jié)點(diǎn) P，然后返回由目標(biāo)節(jié)點(diǎn)逆序倒推其父節(jié)點(diǎn)至起始點(diǎn)的所有節(jié)點(diǎn)組成的路徑。如果不是目標(biāo)點(diǎn)，將當(dāng)前節(jié)點(diǎn) P 加入到 close list 列表中并從 open list 列表刪除，執(zhí)行步驟（3）；?
（3）算法遍歷當(dāng)前節(jié)點(diǎn) P 的相鄰節(jié)點(diǎn) P'，并進(jìn)行以下操作；?
（4）如果相鄰節(jié)點(diǎn) p' 不在 open list 列表中，計(jì)算 g、h 值，設(shè)置相鄰節(jié)點(diǎn) P' 的父節(jié)點(diǎn)設(shè)置為節(jié)點(diǎn) P，最后將節(jié)點(diǎn) P' 存放到 open list 列表中；?
（5）如果相鄰節(jié)點(diǎn) P' 是不可行區(qū)域或在 close list 列表中，則不做任何處理；
（6）如果相鄰節(jié)點(diǎn) P' 已在 open list 列表中，計(jì)算經(jīng)由當(dāng)前節(jié)點(diǎn) P 到達(dá)鄰節(jié)點(diǎn) P' 的 g 值是否比鄰節(jié)點(diǎn) P' 此時(shí)的 g 值更小，如果更小，則將相鄰節(jié)點(diǎn) P' 的 g 值更新為從當(dāng)前節(jié)點(diǎn) P 到相鄰節(jié)點(diǎn) P' 的 g 值，并將相鄰節(jié)點(diǎn) P' 的父節(jié)點(diǎn)設(shè)置為節(jié)點(diǎn)P；如果不會(huì)更小，則不做任何處理；?
（7）返回步驟（3），繼續(xù)處理余下的相鄰節(jié)點(diǎn)；?
（8）判斷 open list 列表是否為空，如果為空，搜索結(jié)束，未找到有效路徑；若非空，執(zhí)行步驟（2）。?
重復(fù)執(zhí)行以上八步，直至找到目標(biāo)點(diǎn)。
3.2 代碼展示
左圖為節(jié)點(diǎn)搜索過程，右圖為最優(yōu)路徑
文章來源：http://www.zghlxwxcb.cn/news/detail-779617.html
完整代碼如下，注釋很全，有問題在評論區(qū)留言文章來源地址http://www.zghlxwxcb.cn/news/detail-779617.html
import math
import matplotlib.pyplot as plt
min_set = 10
show_animation = True # 繪圖
# 創(chuàng)建一個(gè)類
class Dijkstra:
# 初始化
def __init__(self, ox, oy, resolution, robot_radius):
# 屬性分配
self.min_x = None
self.min_y = None
self.max_x = None
self.max_y = None
self.x_width = None
self.y_width = None
self.obstacle_map = None
self.resolution = resolution # 網(wǎng)格大小(m)
self.robot_radius = robot_radius #
self.calc_obstacle_map(ox, oy) # 繪制柵格地圖
self.motion = self.get_motion_model() # 機(jī)器人運(yùn)動(dòng)方式
# 構(gòu)建節(jié)點(diǎn)，每個(gè)網(wǎng)格代表一個(gè)節(jié)點(diǎn)
class Node:
def __init__(self, x, y, cost, parent_index):
self.x = x # 網(wǎng)格索引
self.y = y
self.cost = cost # 路徑值
self.parent_index = parent_index # 該網(wǎng)格的父節(jié)點(diǎn)
def __str__(self):
return str(self.x) + ',' + str(self.y) + ',' + str(self.cost) + ',' + str(self.parent_index)
# 尋找最優(yōu)路徑，網(wǎng)格起始坐標(biāo)(sx,sy)，終點(diǎn)坐標(biāo)（gx,gy）
def planning(self, sx, sy, gx, gy):
# 節(jié)點(diǎn)初始化
# 將已知的起點(diǎn)和終點(diǎn)坐標(biāo)形式轉(zhuǎn)化為節(jié)點(diǎn)類型，0代表路徑權(quán)重，-1代表無父節(jié)點(diǎn)
start_node = self.Node(self.calc_xy_index(sx, self.min_x),
self.calc_xy_index(sy, self.min_y), 0.0, -1)
# 終點(diǎn)
goal_node = self.Node(self.calc_xy_index(gx, self.min_x),
self.calc_xy_index(gy, self.min_y), 0.0, -1)
# 保存入庫節(jié)點(diǎn)和待計(jì)算節(jié)點(diǎn)
open_set, closed_set = dict(), dict()
# 先將起點(diǎn)入庫，獲取每個(gè)網(wǎng)格對應(yīng)的key
open_set[self.calc_index(start_node)] = start_node
# 循環(huán)
while 1:
# 選擇擴(kuò)展點(diǎn)，添加了啟發(fā)項(xiàng)，f(n)= g(n) + h(n)
c_id = min(open_set,
key=lambda o: open_set[o].cost + \
self.calc_heuristic(goal_node, open_set[o]))
current = open_set[c_id] # 從字典中取出該節(jié)點(diǎn)
# 繪圖
if show_animation:
# 網(wǎng)格索引轉(zhuǎn)換為真實(shí)坐標(biāo)
plt.plot(self.calc_position(current.x, self.min_x),
self.calc_position(current.y, self.min_y), 'xc')
plt.pause(0.0001)
# 判斷是否是終點(diǎn)，如果選出來的損失最小的點(diǎn)是終點(diǎn)
if current.x == goal_node.x and current.y == goal_node.y:
# 更新終點(diǎn)的父節(jié)點(diǎn)
goal_node.cost = current.cost
# 更新終點(diǎn)的損失
goal_node.parent_index = current.parent_index
break
# 在外庫中刪除該最小損失點(diǎn)，把它入庫
del open_set[c_id]
closed_set[c_id] = current
# 遍歷鄰接節(jié)點(diǎn)
for move_x, move_y, move_cost in self.motion:
# 獲取每個(gè)鄰接節(jié)點(diǎn)的節(jié)點(diǎn)坐標(biāo)，到起點(diǎn)的距離，父節(jié)點(diǎn)
node = self.Node(current.x + move_x,
current.y + move_y,
current.cost + move_cost, c_id)
# 獲取該鄰居節(jié)點(diǎn)的key
n_id = self.calc_index(node)
# 如果該節(jié)點(diǎn)入庫了，就看下一個(gè)
if n_id in closed_set:
continue
# 鄰居節(jié)點(diǎn)是否超出范圍了，是否在障礙物上
if not self.verify_node(node):
continue
# 如果該節(jié)點(diǎn)不在外庫中，就作為一個(gè)新節(jié)點(diǎn)加入到外庫
if n_id not in open_set:
open_set[n_id] = node
# 節(jié)點(diǎn)在外庫中時(shí)
else:
# 如果該點(diǎn)到起點(diǎn)的距離，要小于外庫中該點(diǎn)的距離，就更新外庫中的該點(diǎn)信息，更改路徑
if node.cost <= open_set[n_id].cost:
open_set[n_id] = node
# 找到終點(diǎn)
rx, ry = self.calc_final_path(goal_node, closed_set)
return rx, ry
# ------------------------------ #
# A* 的啟發(fā)函數(shù)
# ------------------------------ #
@staticmethod
def calc_heuristic(n1, n2): # n1終點(diǎn)，n2當(dāng)前網(wǎng)格
w = 1.0 # 單個(gè)啟發(fā)函數(shù)的權(quán)重，如果有多個(gè)啟發(fā)函數(shù)，權(quán)重可以設(shè)置的不一樣
d = w * math.hypot(n1.x-n2.x, n1.y-n2.y) # 當(dāng)前網(wǎng)格和終點(diǎn)距離
return d
# 機(jī)器人行走的方式，每次能向周圍移動(dòng)8個(gè)網(wǎng)格移動(dòng)
@staticmethod
def get_motion_model():
# [dx, dy, cost]
motion = [[1,0,1], # 右
[0,1,1], # 上
[-1,0,1], # 左
[0,-1,1], # 下
[-1,-1,math.sqrt(2)], # 左下
[-1,1,math.sqrt(2)], # 左上
[1,-1,math.sqrt(2)], # 右下
[1,1,math.sqrt(2)]] # 右上
return motion
# 繪制柵格地圖
def calc_obstacle_map(self, ox, oy):
# 地圖邊界坐標(biāo)
self.min_x = round(min(ox)) # 四舍五入取整
self.min_y = round(min(oy))
self.max_x = round(max(ox))
self.max_y = round(max(oy))
# 地圖的x和y方向的柵格個(gè)數(shù)，長度/每個(gè)網(wǎng)格的長度=網(wǎng)格個(gè)數(shù)
self.x_width = round((self.max_x-self.min_x)/self.resolution) # x方向網(wǎng)格個(gè)數(shù)
self.y_width = round((self.max_y-self.min_y)/self.resolution) # y方向網(wǎng)格個(gè)數(shù)
# 初始化地圖，二維列表，每個(gè)網(wǎng)格的值為False
self.obstacle_map = [[False for _ in range(self.y_width)]
for _ in range(self.x_width)]
# 設(shè)置障礙物
for ix in range(self.x_width): # 遍歷x方向的網(wǎng)格 [0:x_width]
x = self.calc_position(ix, self.min_x) # 根據(jù)網(wǎng)格索引計(jì)算x坐標(biāo)位置
for iy in range(self.y_width): # 遍歷y方向的網(wǎng)格 [0:y_width]
y = self.calc_position(iy, self.min_y) # 根據(jù)網(wǎng)格索引計(jì)算y坐標(biāo)位置
# 遍歷障礙物坐標(biāo)(實(shí)際坐標(biāo))
for iox, ioy in zip(ox, oy):
# 計(jì)算障礙物和網(wǎng)格點(diǎn)之間的距離
d = math.hypot(iox-x, ioy-y)
# 膨脹障礙物，如果障礙物和網(wǎng)格之間的距離小，機(jī)器人無法通行，對障礙物膨脹
if d <= self.robot_radius:
# 將障礙物所在網(wǎng)格設(shè)置為True
self.obstacle_map[ix][iy] = True
break # 每個(gè)障礙物膨脹一次就足夠了
# 根據(jù)網(wǎng)格編號計(jì)算實(shí)際坐標(biāo)
def calc_position(self, index, minp):
# minp代表起點(diǎn)坐標(biāo)，左下x或左下y
pos = minp + index * self.resolution # 網(wǎng)格點(diǎn)左下左下坐標(biāo)
return pos
# 位置坐標(biāo)轉(zhuǎn)為網(wǎng)格坐標(biāo)
def calc_xy_index(self, position, minp):
# (目標(biāo)位置坐標(biāo)-起點(diǎn)坐標(biāo))/一個(gè)網(wǎng)格的長度==>目標(biāo)位置的網(wǎng)格索引
return round((position-minp) / self.resolution)
# 給每個(gè)網(wǎng)格編號，得到每個(gè)網(wǎng)格的key
def calc_index(self, node):
# 從左到右增大，從下到上增大
return node.y * self.x_width + node.x
# 鄰居節(jié)點(diǎn)是否超出范圍
def verify_node(self, node):
# 根據(jù)網(wǎng)格坐標(biāo)計(jì)算實(shí)際坐標(biāo)
px = self.calc_position(node.x, self.min_x)
py = self.calc_position(node.y, self.min_y)
# 判斷是否超出邊界
if px < self.min_x:
return False
if py < self.min_y:
return False
if px >= self.max_x:
return False
if py >= self.max_y:
return False
# 節(jié)點(diǎn)是否在障礙物上，障礙物標(biāo)記為True
if self.obstacle_map[node.x][node.y]:
return False
# 沒超過就返回True
return True
# 計(jì)算路徑, parent屬性記錄每個(gè)節(jié)點(diǎn)的父節(jié)點(diǎn)
def calc_final_path(self, goal_node, closed_set):
# 先存放終點(diǎn)坐標(biāo)（真實(shí)坐標(biāo)）
rx = [self.calc_position(goal_node.x, self.min_x)]
ry = [self.calc_position(goal_node.y, self.min_y)]
# 獲取終點(diǎn)的父節(jié)點(diǎn)索引
parent_index = goal_node.parent_index
# 起點(diǎn)的父節(jié)點(diǎn)==-1
while parent_index != -1:
n = closed_set[parent_index] # 在入庫中選擇父節(jié)點(diǎn)
rx.append(self.calc_position(n.x, self.min_x)) # 節(jié)點(diǎn)的x坐標(biāo)
ry.append(self.calc_position(n.y, self.min_y)) # 節(jié)點(diǎn)的y坐標(biāo)
parent_index = n.parent_index # 節(jié)點(diǎn)的父節(jié)點(diǎn)索引
return rx, ry
def main():
# 設(shè)置起點(diǎn)和終點(diǎn)
sx = -5.0
sy = -5.0
gx = 50.0
gy = 50.0
# 網(wǎng)格大小
grid_size = 2.0
# 機(jī)器人半徑
robot_radius = 1.0
# 設(shè)置障礙物位置
ox, oy = [], []
for i in range(-10,60): ox.append(i); oy.append(-10.0) # 下邊界
for i in range(-10,60): ox.append(60.0); oy.append(i) # 右邊界
for i in range(-10,61): ox.append(i); oy.append(60.0) # 上邊界
for i in range(-10,61): ox.append(-10.0); oy.append(i) # 左邊界
for i in range(-10,40): ox.append(20.0); oy.append(i) # 左圍欄
for i in range(0,40): ox.append(40.0); oy.append(60-i) # 右圍欄
# 繪圖
if show_animation:
plt.plot(ox, oy, '.k') # 障礙物黑色
plt.plot(sx, sy, 'og') # 起點(diǎn)綠色
plt.plot(gx, gy, 'xb') # 終點(diǎn)藍(lán)色
plt.grid(True)
plt.axis('equal') # 坐標(biāo)軸刻度間距等長
# 實(shí)例化，傳入障礙物，網(wǎng)格大小
dijkstra = Dijkstra(ox, oy, grid_size, robot_radius)
# 求解路徑，返回路徑的 x 坐標(biāo)和 y 坐標(biāo)列表
rx, ry = dijkstra.planning(sx, sy, gx, gy)
# 繪制路徑經(jīng)過的網(wǎng)格
if show_animation:
plt.plot(rx, ry, '-r')
plt.pause(0.01)
plt.show()
if __name__ == '__main__':
main()
到了這里，關(guān)于【路徑規(guī)劃】(2) A* 算法求解最短路，附python完整代碼的文章就介紹完了。如果您還想了解更多內(nèi)容，請?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
