1、前言

在這篇博文中，我將為您介紹A算法、D算法、人工勢(shì)場(chǎng)算法和Dijkstra算法等路徑規(guī)劃算法。我將詳細(xì)解釋它們的原理和優(yōu)缺點(diǎn)，并展示一些學(xué)者編寫的Python程序來(lái)演示它們的運(yùn)行效果。通過(guò)閱讀本文，您將對(duì)這些算法有一定的了解，并能夠在實(shí)際應(yīng)用中選擇最適合的算法。

2、D*算法

2.1簡(jiǎn)介

D*規(guī)劃算法是一種用于路徑規(guī)劃的增量式搜索算法。它可以在已知環(huán)境地圖的情況下，根據(jù)實(shí)時(shí)的傳感器信息和環(huán)境變化來(lái)動(dòng)態(tài)更新路徑規(guī)劃結(jié)果。

D*規(guī)劃算法的核心思想是通過(guò)不斷地修正路徑的代價(jià)值來(lái)逐步優(yōu)化路徑規(guī)劃結(jié)果。它使用了兩個(gè)重要的數(shù)據(jù)結(jié)構(gòu)：狀態(tài)圖和代價(jià)圖。狀態(tài)圖表示了地圖中的各個(gè)位置以及它們之間的連接關(guān)系，而代價(jià)圖則記錄了每個(gè)位置到目標(biāo)位置的代價(jià)值。

D*規(guī)劃算法的基本流程如下：

1. 初始化：設(shè)置起始位置和目標(biāo)位置，并初始化狀態(tài)圖和代價(jià)圖。
2. 通過(guò)啟發(fā)式函數(shù)計(jì)算起始位置到目標(biāo)位置的估計(jì)代價(jià)值，并將起始位置加入到開放列表中。
3. 進(jìn)入循環(huán)，直到找到最優(yōu)路徑或者無(wú)法找到路徑：
   a. 選擇開放列表中代價(jià)最小的位置作為當(dāng)前位置。
   b. 更新當(dāng)前位置周圍的鄰居節(jié)點(diǎn)的代價(jià)值，并將其加入到開放列表中。
   c. 如果目標(biāo)位置的代價(jià)值發(fā)生變化，則重新計(jì)算路徑。
4. 根據(jù)最終的路徑結(jié)果進(jìn)行導(dǎo)航或執(zhí)行其他操作。
   D*規(guī)劃算法具有較好的實(shí)時(shí)性能和適應(yīng)性，可以應(yīng)對(duì)環(huán)境變化和動(dòng)態(tài)障礙物的情況。

2.2優(yōu)缺點(diǎn)

2.2.1 優(yōu)點(diǎn)

1. 實(shí)時(shí)性：D*規(guī)劃算法能夠在運(yùn)行時(shí)根據(jù)環(huán)境的變化實(shí)時(shí)更新路徑信息，適用于動(dòng)態(tài)環(huán)境中的路徑規(guī)劃問題。
2. 高效性：D*規(guī)劃算法通過(guò)增量式的更新方式，避免了對(duì)整個(gè)地圖進(jìn)行重新規(guī)劃，從而減少了計(jì)算量，提高了路徑規(guī)劃的效率。
3. 適應(yīng)性：D*規(guī)劃算法能夠根據(jù)環(huán)境的變化自適應(yīng)地調(diào)整路徑，可以應(yīng)對(duì)障礙物的移動(dòng)或新增等情況。

2.2.2 缺點(diǎn)

1. 初始規(guī)劃開銷較大：D*規(guī)劃算法需要進(jìn)行一次全局規(guī)劃來(lái)初始化路徑信息，這個(gè)過(guò)程可能會(huì)消耗較多的計(jì)算資源和時(shí)間。
2. 對(duì)地圖信息要求高：D*規(guī)劃算法需要準(zhǔn)確的環(huán)境地圖信息，如果地圖信息不準(zhǔn)確或者不完整，可能會(huì)導(dǎo)致路徑規(guī)劃結(jié)果不理想。
3. 對(duì)計(jì)算資源要求高：D*規(guī)劃算法在實(shí)時(shí)更新路徑信息時(shí)需要進(jìn)行大量的計(jì)算，對(duì)計(jì)算資源要求較高。

2.3 python程序

python程序如下：

"""D* grid planningauthor: Nirnay Roy"""
import mathfrom sys import maxsizeimport matplotlib.pyplot as pltshow_animation = Trueclass State:def __init__(self, x, y):self.x = xself.y = yself.parent = Noneself.state = "."self.t = "new"  # tag for stateself.h = 0self.k = 0def cost(self, state):if self.state == "#" or state.state == "#":return maxsizereturn math.sqrt(math.pow((self.x - state.x), 2) +math.pow((self.y - state.y), 2))def set_state(self, state):""".: new#: obstaclee: oparent of current state*: closed states: current state"""if state not in ["s", ".", "#", "e", "*"]:returnself.state = stateclass Map:def __init__(self, row, col):self.row = rowself.col = colself.map = self.init_map()def init_map(self):map_list = []for i in range(self.row):tmp = []for j in range(self.col):tmp.append(State(i, j))map_list.append(tmp)return map_listdef get_neighbors(self, state):state_list = []for i in [-1, 0, 1]:for j in [-1, 0, 1]:if i == 0 and j == 0:continueif state.x + i < 0 or state.x + i >= self.row:continueif state.y + j < 0 or state.y + j >= self.col:continuestate_list.append(self.map[state.x + i][state.y + j])return state_listdef set_obstacle(self, point_list):for x, y in point_list:if x < 0 or x >= self.row or y < 0 or y >= self.col:continueself.map[x][y].set_state("#")class Dstar:def __init__(self, maps):self.map = mapsself.open_list = set()def process_state(self):x = self.min_state()if x is None:return -1k_old = self.get_kmin()self.remove(x)if k_old < x.h:for y in self.map.get_neighbors(x):if y.h <= k_old and x.h > y.h + x.cost(y):x.parent = yx.h = y.h + x.cost(y)elif k_old == x.h:for y in self.map.get_neighbors(x):if y.t == "new" or y.parent == x and y.h != x.h + x.cost(y) \or y.parent != x and y.h > x.h + x.cost(y):y.parent = xself.insert(y, x.h + x.cost(y))else:for y in self.map.get_neighbors(x):if y.t == "new" or y.parent == x and y.h != x.h + x.cost(y):y.parent = xself.insert(y, x.h + x.cost(y))else:if y.parent != x and y.h > x.h + x.cost(y):self.insert(y, x.h)else:if y.parent != x and x.h > y.h + x.cost(y) \and y.t == "close" and y.h > k_old:self.insert(y, y.h)return self.get_kmin()def min_state(self):if not self.open_list:return Nonemin_state = min(self.open_list, key=lambda x: x.k)return min_statedef get_kmin(self):if not self.open_list:return -1k_min = min([x.k for x in self.open_list])return k_mindef insert(self, state, h_new):if state.t == "new":state.k = h_newelif state.t == "open":state.k = min(state.k, h_new)elif state.t == "close":state.k = min(state.h, h_new)state.h = h_newstate.t = "open"self.open_list.add(state)def remove(self, state):if state.t == "open":state.t = "close"self.open_list.remove(state)def modify_cost(self, x):if x.t == "close":self.insert(x, x.parent.h + x.cost(x.parent))def run(self, start, end):rx = []ry = []self.insert(end, 0.0)while True:self.process_state()if start.t == "close":breakstart.set_state("s")s = starts = s.parents.set_state("e")tmp = startwhile tmp != end:tmp.set_state("*")rx.append(tmp.x)ry.append(tmp.y)if show_animation:plt.plot(rx, ry, "-r")plt.pause(0.01)if tmp.parent.state == "#":self.modify(tmp)continuetmp = tmp.parenttmp.set_state("e")return rx, rydef modify(self, state):self.modify_cost(state)while True:k_min = self.process_state()if k_min >= state.h:breakdef main():m = Map(100, 100)ox, oy = [], []for i in range(-10, 60):ox.append(i)oy.append(-10)for i in range(-10, 60):ox.append(60)oy.append(i)for i in range(-10, 61):ox.append(i)oy.append(60)for i in range(-10, 61):ox.append(-10)oy.append(i)for i in range(-10, 40):ox.append(20)oy.append(i)for i in range(0, 40):ox.append(40)oy.append(60 - i)print([(i, j) for i, j in zip(ox, oy)])m.set_obstacle([(i, j) for i, j in zip(ox, oy)])start = [10, 10]goal = [50, 10]if show_animation:plt.plot(ox, oy, ".k")plt.plot(start[0], start[1], "og")plt.plot(goal[0], goal[1], "xb")plt.axis("equal")start = m.map[start[0]][start[1]]end = m.map[goal[0]][goal[1]]dstar = Dstar(m)rx, ry = dstar.run(start, end)if show_animation:plt.plot(rx, ry, "-r")plt.show()if __name__ == '__main__':main()

3、A*算法

A*算法是一種常用的啟發(fā)式搜索算法，用于在圖形結(jié)構(gòu)中找到最短路徑。它綜合了廣度優(yōu)先搜索和貪婪最佳優(yōu)先搜索的特點(diǎn)，通過(guò)評(píng)估函數(shù)來(lái)選擇下一步要擴(kuò)展的節(jié)點(diǎn)。

A*算法的原理如下：
首先，定義一個(gè)開放列表和一個(gè)關(guān)閉列表。開放列表用于存儲(chǔ)待擴(kuò)展的節(jié)點(diǎn)，關(guān)閉列表用于存儲(chǔ)已經(jīng)擴(kuò)展過(guò)的節(jié)點(diǎn)。
將起始節(jié)點(diǎn)加入到開放列表中，并將其評(píng)估函數(shù)值設(shè)為0。
重復(fù)以下步驟直到找到目標(biāo)節(jié)點(diǎn)或者開放列表為空：
a. 從開放列表中選擇評(píng)估函數(shù)值最小的節(jié)點(diǎn)作為當(dāng)前節(jié)點(diǎn)。
b. 將當(dāng)前節(jié)點(diǎn)從開放列表中移除，并加入到關(guān)閉列表中。
c. 對(duì)當(dāng)前節(jié)點(diǎn)的相鄰節(jié)點(diǎn)進(jìn)行遍歷：
如果相鄰節(jié)點(diǎn)已經(jīng)在關(guān)閉列表中，則忽略。
如果相鄰節(jié)點(diǎn)不在開放列表中，則將其加入到開放列表，并計(jì)算其評(píng)估函數(shù)值。
如果相鄰節(jié)點(diǎn)已經(jīng)在開放列表中，比較當(dāng)前路徑和之前路徑的評(píng)估函數(shù)值，如果當(dāng)前路徑更優(yōu)，則更新評(píng)估函數(shù)值和父節(jié)點(diǎn)。
如果開放列表為空，則表示無(wú)法找到目標(biāo)節(jié)點(diǎn)，搜索失敗。

3.1 優(yōu)缺點(diǎn)：

3.1.1 優(yōu)點(diǎn)：

可以保證找到最短路徑，因?yàn)樗C合了廣度優(yōu)先搜索和貪婪最佳優(yōu)先搜索的特點(diǎn)。
在啟發(fā)函數(shù)選擇合適的情況下，可以高效地搜索大規(guī)模圖形結(jié)構(gòu)。

3.1.2 缺點(diǎn)：

啟發(fā)函數(shù)的選擇對(duì)算法的性能有很大影響，不同的啟發(fā)函數(shù)可能導(dǎo)致不同的結(jié)果。
在某些情況下，A*算法可能會(huì)陷入局部最優(yōu)解，無(wú)法找到全局最優(yōu)解。

3.2 python程序

python程序：

"""A* grid planningauthor: Atsushi Sakai(@Atsushi_twi)Nikos Kanargias (nkana@tee.gr)"""import mathimport matplotlib.pyplot as pltshow_animation = Trueclass AStarPlanner:def __init__(self, ox, oy, resolution, rr):"""Initialize grid map for a star planningox: x position list of Obstacles [m]oy: y position list of Obstacles [m]resolution: grid resolution [m]rr: robot radius[m]"""self.resolution = resolutionself.rr = rrself.min_x, self.min_y = 0, 0self.max_x, self.max_y = 0, 0self.obstacle_map = Noneself.x_width, self.y_width = 0, 0self.motion = self.get_motion_model()self.calc_obstacle_map(ox, oy)class Node:def __init__(self, x, y, cost, parent_index):self.x = x  # index of gridself.y = y  # index of gridself.cost = costself.parent_index = parent_indexdef __str__(self):return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.parent_index)def planning(self, sx, sy, gx, gy):"""A star path searchinput:s_x: start x position [m]s_y: start y position [m]gx: goal x position [m]gy: goal y position [m]output:rx: x position list of the final pathry: y position list of the final path"""start_node = self.Node(self.calc_xy_index(sx, self.min_x),self.calc_xy_index(sy, self.min_y), 0.0, -1)goal_node = self.Node(self.calc_xy_index(gx, self.min_x),self.calc_xy_index(gy, self.min_y), 0.0, -1)open_set, closed_set = dict(), dict()open_set[self.calc_grid_index(start_node)] = start_nodewhile True:if len(open_set) == 0:print("Open set is empty..")breakc_id = min(open_set,key=lambda o: open_set[o].cost + self.calc_heuristic(goal_node,open_set[o]))current = open_set[c_id]# show graphif show_animation:  # pragma: no coverplt.plot(self.calc_grid_position(current.x, self.min_x),self.calc_grid_position(current.y, self.min_y), "xc")# for stopping simulation with the esc key.plt.gcf().canvas.mpl_connect('key_release_event',lambda event: [exit(0) if event.key == 'escape' else None])if len(closed_set.keys()) % 10 == 0:plt.pause(0.001)if current.x == goal_node.x and current.y == goal_node.y:print("Find goal")goal_node.parent_index = current.parent_indexgoal_node.cost = current.costbreak# Remove the item from the open setdel open_set[c_id]# Add it to the closed setclosed_set[c_id] = current# expand_grid search grid based on motion modelfor i, _ in enumerate(self.motion):node = self.Node(current.x + self.motion[i][0],current.y + self.motion[i][1],current.cost + self.motion[i][2], c_id)n_id = self.calc_grid_index(node)# If the node is not safe, do nothingif not self.verify_node(node):continueif n_id in closed_set:continueif n_id not in open_set:open_set[n_id] = node  # discovered a new nodeelse:if open_set[n_id].cost > node.cost:# This path is the best until now. record itopen_set[n_id] = noderx, ry = self.calc_final_path(goal_node, closed_set)return rx, rydef calc_final_path(self, goal_node, closed_set):# generate final courserx, ry = [self.calc_grid_position(goal_node.x, self.min_x)], [self.calc_grid_position(goal_node.y, self.min_y)]parent_index = goal_node.parent_indexwhile parent_index != -1:n = closed_set[parent_index]rx.append(self.calc_grid_position(n.x, self.min_x))ry.append(self.calc_grid_position(n.y, self.min_y))parent_index = n.parent_indexreturn rx, ry@staticmethoddef calc_heuristic(n1, n2):w = 1.0  # weight of heuristicd = w * math.hypot(n1.x - n2.x, n1.y - n2.y)return ddef calc_grid_position(self, index, min_position):"""calc grid position:param index::param min_position::return:"""pos = index * self.resolution + min_positionreturn posdef calc_xy_index(self, position, min_pos):return round((position - min_pos) / self.resolution)def calc_grid_index(self, node):return (node.y - self.min_y) * self.x_width + (node.x - self.min_x)def verify_node(self, node):px = self.calc_grid_position(node.x, self.min_x)py = self.calc_grid_position(node.y, self.min_y)if px < self.min_x:return Falseelif py < self.min_y:return Falseelif px >= self.max_x:return Falseelif py >= self.max_y:return False# collision checkif self.obstacle_map[node.x][node.y]:return Falsereturn Truedef calc_obstacle_map(self, ox, oy):self.min_x = round(min(ox))self.min_y = round(min(oy))self.max_x = round(max(ox))self.max_y = round(max(oy))print("min_x:", self.min_x)print("min_y:", self.min_y)print("max_x:", self.max_x)print("max_y:", self.max_y)self.x_width = round((self.max_x - self.min_x) / self.resolution)self.y_width = round((self.max_y - self.min_y) / self.resolution)print("x_width:", self.x_width)print("y_width:", self.y_width)# obstacle map generationself.obstacle_map = [[False for _ in range(self.y_width)]for _ in range(self.x_width)]for ix in range(self.x_width):x = self.calc_grid_position(ix, self.min_x)for iy in range(self.y_width):y = self.calc_grid_position(iy, self.min_y)for iox, ioy in zip(ox, oy):d = math.hypot(iox - x, ioy - y)if d <= self.rr:self.obstacle_map[ix][iy] = Truebreak@staticmethoddef get_motion_model():# dx, dy, costmotion = [[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 motiondef main():print(__file__ + " start!!")# start and goal positionsx = 10.0  # [m]sy = 10.0  # [m]gx = 50.0  # [m]gy = 50.0  # [m]grid_size = 2.0  # [m]robot_radius = 1.0  # [m]# set obstacle positionsox, 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.0 - i)if show_animation:  # pragma: no coverplt.plot(ox, oy, ".k")plt.plot(sx, sy, "og")plt.plot(gx, gy, "xb")plt.grid(True)plt.axis("equal")a_star = AStarPlanner(ox, oy, grid_size, robot_radius)rx, ry = a_star.planning(sx, sy, gx, gy)if show_animation:  # pragma: no coverplt.plot(rx, ry, "-r")plt.pause(0.001)plt.show()if __name__ == '__main__':main()

4、人工勢(shì)場(chǎng)算法

Potential Field based path planner是一種基于勢(shì)場(chǎng)的路徑規(guī)劃算法，它模擬了物體在勢(shì)場(chǎng)中的運(yùn)動(dòng)方式來(lái)尋找最優(yōu)路徑。該算法通過(guò)將環(huán)境劃分為障礙物和目標(biāo)區(qū)域，并為每個(gè)區(qū)域分配一個(gè)勢(shì)能值，來(lái)引導(dǎo)機(jī)器人或其他移動(dòng)物體避開障礙物并朝向目標(biāo)。

該算法的原理如下：

• 定義勢(shì)能場(chǎng)：將環(huán)境劃分為障礙物和目標(biāo)區(qū)域，并為每個(gè)區(qū)域分配一個(gè)勢(shì)能值。通常情況下，障礙物區(qū)域的勢(shì)能值較高，而目標(biāo)區(qū)域的勢(shì)能值較低。
• 計(jì)算合力：根據(jù)機(jī)器人當(dāng)前位置和周圍環(huán)境的勢(shì)能值，計(jì)算合力。合力由兩部分組成：引力和斥力。引力指向目標(biāo)區(qū)域，斥力來(lái)自障礙物區(qū)域。
• 更新機(jī)器人位置：根據(jù)合力的方向和大小，更新機(jī)器人的位置。機(jī)器人會(huì)受到引力的吸引，同時(shí)受到斥力的排斥，從而朝著勢(shì)能值較低的目標(biāo)區(qū)域移動(dòng)。
• 重復(fù)步驟2和3，直到機(jī)器人到達(dá)目標(biāo)區(qū)域或無(wú)法找到可行路徑。

4.1優(yōu)缺點(diǎn)

4.1.1優(yōu)點(diǎn)：

• 算法簡(jiǎn)單易實(shí)現(xiàn)：該算法的原理相對(duì)簡(jiǎn)單，容易理解和實(shí)現(xiàn)。

• 實(shí)時(shí)性較好：由于只需要計(jì)算當(dāng)前位置周圍的勢(shì)能值和合力，因此算法具有較好的實(shí)時(shí)性能。

• 能夠處理動(dòng)態(tài)環(huán)境：由于勢(shì)能場(chǎng)是根據(jù)當(dāng)前環(huán)境計(jì)算的，因此可以適應(yīng)動(dòng)態(tài)環(huán)境的變化。

4.1.2缺點(diǎn)：

• 容易陷入局部最優(yōu)：由于算法只考慮當(dāng)前位置周圍的勢(shì)能值，可能會(huì)導(dǎo)致機(jī)器人陷入局部最優(yōu)解，無(wú)法找到全局最優(yōu)路徑。
• 難以處理復(fù)雜環(huán)境：在存在大量障礙物或復(fù)雜形狀的環(huán)境中，勢(shì)能場(chǎng)的計(jì)算和更新可能變得復(fù)雜且耗時(shí)。
• 缺乏路徑規(guī)劃的優(yōu)化策略：該算法主要通過(guò)勢(shì)能場(chǎng)來(lái)引導(dǎo)機(jī)器人移動(dòng)，缺乏對(duì)路徑規(guī)劃的優(yōu)化策略，可能導(dǎo)致路徑長(zhǎng)度較長(zhǎng)或不夠平滑。

4.2 python程序

"""Potential Field based path plannerauthor: Atsushi Sakai (@Atsushi_twi)"""from collections import deque
import numpy as np
import matplotlib.pyplot as plt# Parameters
KP = 5.0  # attractive potential gain
ETA = 100.0  # repulsive potential gain
AREA_WIDTH = 30.0  # potential area width [m]
# the number of previous positions used to check oscillations
OSCILLATIONS_DETECTION_LENGTH = 3show_animation = Truedef calc_potential_field(gx, gy, ox, oy, reso, rr, sx, sy):minx = min(min(ox), sx, gx) - AREA_WIDTH / 2.0miny = min(min(oy), sy, gy) - AREA_WIDTH / 2.0maxx = max(max(ox), sx, gx) + AREA_WIDTH / 2.0maxy = max(max(oy), sy, gy) + AREA_WIDTH / 2.0xw = int(round((maxx - minx) / reso))yw = int(round((maxy - miny) / reso))# calc each potentialpmap = [[0.0 for i in range(yw)] for i in range(xw)]for ix in range(xw):x = ix * reso + minxfor iy in range(yw):y = iy * reso + minyug = calc_attractive_potential(x, y, gx, gy)uo = calc_repulsive_potential(x, y, ox, oy, rr)uf = ug + uopmap[ix][iy] = ufreturn pmap, minx, minydef calc_attractive_potential(x, y, gx, gy):return 0.5 * KP * np.hypot(x - gx, y - gy)def calc_repulsive_potential(x, y, ox, oy, rr):# search nearest obstacleminid = -1dmin = float("inf")for i, _ in enumerate(ox):d = np.hypot(x - ox[i], y - oy[i])if dmin >= d:dmin = dminid = i# calc repulsive potentialdq = np.hypot(x - ox[minid], y - oy[minid])if dq <= rr:if dq <= 0.1:dq = 0.1return 0.5 * ETA * (1.0 / dq - 1.0 / rr) ** 2else:return 0.0def get_motion_model():# dx, dymotion = [[1, 0],[0, 1],[-1, 0],[0, -1],[-1, -1],[-1, 1],[1, -1],[1, 1]]return motiondef oscillations_detection(previous_ids, ix, iy):previous_ids.append((ix, iy))if (len(previous_ids) > OSCILLATIONS_DETECTION_LENGTH):previous_ids.popleft()# check if contains any duplicates by copying into a setprevious_ids_set = set()for index in previous_ids:if index in previous_ids_set:return Trueelse:previous_ids_set.add(index)return Falsedef potential_field_planning(sx, sy, gx, gy, ox, oy, reso, rr):# calc potential fieldpmap, minx, miny = calc_potential_field(gx, gy, ox, oy, reso, rr, sx, sy)# search pathd = np.hypot(sx - gx, sy - gy)ix = round((sx - minx) / reso)iy = round((sy - miny) / reso)gix = round((gx - minx) / reso)giy = round((gy - miny) / reso)if show_animation:draw_heatmap(pmap)# for stopping simulation with the esc key.plt.gcf().canvas.mpl_connect('key_release_event',lambda event: [exit(0) if event.key == 'escape' else None])plt.plot(ix, iy, "*k")plt.plot(gix, giy, "*m")rx, ry = [sx], [sy]motion = get_motion_model()previous_ids = deque()while d >= reso:minp = float("inf")minix, miniy = -1, -1for i, _ in enumerate(motion):inx = int(ix + motion[i][0])iny = int(iy + motion[i][1])if inx >= len(pmap) or iny >= len(pmap[0]) or inx < 0 or iny < 0:p = float("inf")  # outside areaprint("outside potential!")else:p = pmap[inx][iny]if minp > p:minp = pminix = inxminiy = inyix = minixiy = miniyxp = ix * reso + minxyp = iy * reso + minyd = np.hypot(gx - xp, gy - yp)rx.append(xp)ry.append(yp)if (oscillations_detection(previous_ids, ix, iy)):print("Oscillation detected at ({},{})!".format(ix, iy))breakif show_animation:plt.plot(ix, iy, ".r")plt.pause(0.01)print("Goal!!")return rx, rydef draw_heatmap(data):data = np.array(data).Tplt.pcolor(data, vmax=100.0, cmap=plt.cm.Blues)def main():print("potential_field_planning start")sx = 0.0  # start x position [m]sy = 10.0  # start y positon [m]gx = 30.0  # goal x position [m]gy = 30.0  # goal y position [m]grid_size = 0.5  # potential grid size [m]robot_radius = 50.0  # robot radius [m]ox = [15.0, 5.0, 20.0, 25.0]  # obstacle x position list [m]oy = [25.0, 15.0, 26.0, 25.0]  # obstacle y position list [m]if show_animation:plt.grid(True)plt.axis("equal")# path generation_, _ = potential_field_planning(sx, sy, gx, gy, ox, oy, grid_size, robot_radius)if show_animation:plt.show()if __name__ == '__main__':print(__file__ + " start!!")main()print(__file__ + " Done!!")

5、Dijkstra算法

是一種用于解決單源最短路徑問題的經(jīng)典算法。它可以找到從一個(gè)起始節(jié)點(diǎn)到其他所有節(jié)點(diǎn)的最短路徑。

Dijkstra算法的流程如下：

• 創(chuàng)建一個(gè)集合S，用于存放已經(jīng)找到最短路徑的節(jié)點(diǎn)。 初始化距離數(shù)組dist，將起始節(jié)點(diǎn)的距離設(shè)為0，其他節(jié)點(diǎn)的距離設(shè)為無(wú)窮大。
• 重復(fù)以下步驟，直到集合S包含所有節(jié)點(diǎn)：
  a. 從距離數(shù)組dist中選擇一個(gè)距離最小的節(jié)點(diǎn)u，將其加入集合S。
  b. 對(duì)于節(jié)點(diǎn)u的每個(gè)鄰居節(jié)點(diǎn)v，如果通過(guò)節(jié)點(diǎn)u到達(dá)節(jié)點(diǎn)v的路徑比當(dāng)前記錄的最短路徑更短，則更新距離數(shù)組dist中節(jié)點(diǎn)v的距離。
• 距離數(shù)組dist中記錄的就是從起始節(jié)點(diǎn)到每個(gè)節(jié)點(diǎn)的最短路徑。

5.1優(yōu)缺點(diǎn)

5.1.1優(yōu)點(diǎn)：

• 算法保證能夠找到最短路徑。
• 可以處理有向圖和無(wú)向圖。
• 可以處理帶有負(fù)權(quán)邊的圖，但不能處理帶有負(fù)權(quán)環(huán)的圖。

5.1.2缺點(diǎn)：

-對(duì)于大規(guī)模圖來(lái)說(shuō)，算法的時(shí)間復(fù)雜度較高，為O(V^2)，其中V是節(jié)點(diǎn)的數(shù)量。
-無(wú)法處理帶有負(fù)權(quán)環(huán)的圖，因?yàn)樵诿看蔚?#xff0c;算法會(huì)選擇當(dāng)前距離最小的節(jié)點(diǎn)，而負(fù)權(quán)環(huán)會(huì)導(dǎo)致無(wú)限循環(huán)。

5.2python程序

python程序：

"""Grid based Dijkstra planningauthor: Atsushi Sakai(@Atsushi_twi)"""import matplotlib.pyplot as plt
import mathshow_animation = Trueclass Dijkstra:def __init__(self, ox, oy, resolution, robot_radius):"""Initialize map for a star planningox: x position list of Obstacles [m]oy: y position list of Obstacles [m]resolution: grid resolution [m]rr: robot radius[m]"""self.min_x = Noneself.min_y = Noneself.max_x = Noneself.max_y = Noneself.x_width = Noneself.y_width = Noneself.obstacle_map = Noneself.resolution = resolutionself.robot_radius = robot_radiusself.calc_obstacle_map(ox, oy)self.motion = self.get_motion_model()class Node:def __init__(self, x, y, cost, parent_index):self.x = x  # index of gridself.y = y  # index of gridself.cost = costself.parent_index = parent_index  # index of previous Nodedef __str__(self):return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.parent_index)def planning(self, sx, sy, gx, gy):"""dijkstra path searchinput:s_x: start x position [m]s_y: start y position [m]gx: goal x position [m]gx: goal x position [m]output:rx: x position list of the final pathry: y position list of the final path"""start_node = self.Node(self.calc_xy_index(sx, self.min_x),self.calc_xy_index(sy, self.min_y), 0.0, -1)goal_node = self.Node(self.calc_xy_index(gx, self.min_x),self.calc_xy_index(gy, self.min_y), 0.0, -1)open_set, closed_set = dict(), dict()open_set[self.calc_index(start_node)] = start_nodewhile True:c_id = min(open_set, key=lambda o: open_set[o].cost)current = open_set[c_id]# show graphif show_animation:  # pragma: no coverplt.plot(self.calc_position(current.x, self.min_x),self.calc_position(current.y, self.min_y), "xc")# for stopping simulation with the esc key.plt.gcf().canvas.mpl_connect('key_release_event',lambda event: [exit(0) if event.key == 'escape' else None])if len(closed_set.keys()) % 10 == 0:plt.pause(0.001)if current.x == goal_node.x and current.y == goal_node.y:print("Find goal")goal_node.parent_index = current.parent_indexgoal_node.cost = current.costbreak# Remove the item from the open setdel open_set[c_id]# Add it to the closed setclosed_set[c_id] = current# expand search grid based on motion modelfor move_x, move_y, move_cost in self.motion:node = self.Node(current.x + move_x,current.y + move_y,current.cost + move_cost, c_id)n_id = self.calc_index(node)if n_id in closed_set:continueif not self.verify_node(node):continueif n_id not in open_set:open_set[n_id] = node  # Discover a new nodeelse:if open_set[n_id].cost >= node.cost:# This path is the best until now. record it!open_set[n_id] = noderx, ry = self.calc_final_path(goal_node, closed_set)return rx, rydef calc_final_path(self, goal_node, closed_set):# generate final courserx, ry = [self.calc_position(goal_node.x, self.min_x)], [self.calc_position(goal_node.y, self.min_y)]parent_index = goal_node.parent_indexwhile parent_index != -1:n = closed_set[parent_index]rx.append(self.calc_position(n.x, self.min_x))ry.append(self.calc_position(n.y, self.min_y))parent_index = n.parent_indexreturn rx, rydef calc_position(self, index, minp):pos = index * self.resolution + minpreturn posdef calc_xy_index(self, position, minp):return round((position - minp) / self.resolution)def calc_index(self, node):return (node.y - self.min_y) * self.x_width + (node.x - self.min_x)def verify_node(self, node):px = self.calc_position(node.x, self.min_x)py = self.calc_position(node.y, self.min_y)if px < self.min_x:return Falseif py < self.min_y:return Falseif px >= self.max_x:return Falseif py >= self.max_y:return Falseif self.obstacle_map[node.x][node.y]:return Falsereturn Truedef calc_obstacle_map(self, ox, oy):self.min_x = round(min(ox))self.min_y = round(min(oy))self.max_x = round(max(ox))self.max_y = round(max(oy))print("min_x:", self.min_x)print("min_y:", self.min_y)print("max_x:", self.max_x)print("max_y:", self.max_y)self.x_width = round((self.max_x - self.min_x) / self.resolution)self.y_width = round((self.max_y - self.min_y) / self.resolution)print("x_width:", self.x_width)print("y_width:", self.y_width)# obstacle map generationself.obstacle_map = [[False for _ in range(self.y_width)]for _ in range(self.x_width)]for ix in range(self.x_width):x = self.calc_position(ix, self.min_x)for iy in range(self.y_width):y = self.calc_position(iy, self.min_y)for iox, ioy in zip(ox, oy):d = math.hypot(iox - x, ioy - y)if d <= self.robot_radius:self.obstacle_map[ix][iy] = Truebreak@staticmethoddef get_motion_model():# dx, dy, costmotion = [[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 motiondef main():print(__file__ + " start!!")# start and goal positionsx = -5.0  # [m]sy = -5.0  # [m]gx = 50.0  # [m]gy = 50.0  # [m]grid_size = 2.0  # [m]robot_radius = 1.0  # [m]# set obstacle positionsox, 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.0 - i)if show_animation:  # pragma: no coverplt.plot(ox, oy, ".k")plt.plot(sx, sy, "og")plt.plot(gx, gy, "xb")plt.grid(True)plt.axis("equal")dijkstra = Dijkstra(ox, oy, grid_size, robot_radius)rx, ry = dijkstra.planning(sx, sy, gx, gy)if show_animation:  # pragma: no coverplt.plot(rx, ry, "-r")plt.pause(0.01)plt.show()if __name__ == '__main__':main()

說(shuō)明：
接下來(lái)，根據(jù)生成的樣本，生成道路地圖的。輸入?yún)?shù)包括采樣點(diǎn)的x坐標(biāo)和y坐標(biāo)，機(jī)器人安全半徑，以及占用柵格的KDTree二叉樹。輸出是一個(gè)表示道路地圖的列表。

算法首先對(duì)采樣點(diǎn)進(jìn)行了KDTree的構(gòu)建，以便進(jìn)行高效的搜索。然后，遍歷每個(gè)采樣點(diǎn)，對(duì)其進(jìn)行搜索和路徑生成。對(duì)于每個(gè)采樣點(diǎn)，通過(guò)查詢KDTree獲得與其最近的其他采樣點(diǎn)，并檢查路徑是否可達(dá)。如果路徑可達(dá)，則將相應(yīng)的索引添加到邊的ID列表中。如果邊的ID數(shù)量達(dá)到了最大路徑數(shù)N_KNN，則結(jié)束搜索。最后，將邊的ID列表添加到道路地圖中。

整個(gè)算法的流程是：構(gòu)建KDTree -> 遍歷采樣點(diǎn) -> 查詢最近的其他采樣點(diǎn) -> 檢查路徑可達(dá)性 -> 添加邊的ID到列表 -> 添加列表到道路地圖。

該算法是迪杰斯特拉路徑規(guī)劃算法（Dijkstra Planning Algorithm），用于在給定的地圖中尋找起點(diǎn)到目標(biāo)點(diǎn)的最短路徑。算法接受起點(diǎn)和目標(biāo)點(diǎn)的坐標(biāo)，以及可能的路徑地圖、樣本坐標(biāo)列表作為輸入。

算法首先將起點(diǎn)節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)初始化為PNode對(duì)象，并將起點(diǎn)節(jié)點(diǎn)添加到open_set字典中。然后，進(jìn)入循環(huán)，如果open_set為空，則表示找不到路徑，打印提示信息并跳出循環(huán)。否則，從open_set中選擇cost最小的節(jié)點(diǎn)作為當(dāng)前節(jié)點(diǎn)，進(jìn)行路徑搜索的下一步。

在路徑搜索的過(guò)程中，如果開啟了動(dòng)畫展示（show_animation為True），則每次經(jīng)過(guò)閉集中節(jié)點(diǎn)數(shù)量為偶數(shù)時(shí)會(huì)繪制當(dāng)前節(jié)點(diǎn)，并監(jiān)聽esc鍵以停止模擬。如果當(dāng)前節(jié)點(diǎn)的索引等于目標(biāo)節(jié)點(diǎn)索引，則表示找到了目標(biāo)節(jié)點(diǎn)，將目標(biāo)節(jié)點(diǎn)的父節(jié)點(diǎn)索引設(shè)置為當(dāng)前節(jié)點(diǎn)的父節(jié)點(diǎn)索引，并跳出循環(huán)。

接下來(lái)，將當(dāng)前節(jié)點(diǎn)從open_set中刪除，并將其添加到closed_set中。然后，根據(jù)運(yùn)動(dòng)模型，擴(kuò)展搜索網(wǎng)格，計(jì)算當(dāng)前節(jié)點(diǎn)與目標(biāo)節(jié)點(diǎn)之間的距離，并創(chuàng)建一個(gè)新的PNode對(duì)象。如果新節(jié)點(diǎn)的索引已經(jīng)在closed_set中，則跳過(guò)當(dāng)前循環(huán)。否則，如果新節(jié)點(diǎn)的索引已經(jīng)在open_set中，則比較新節(jié)點(diǎn)的cost與open_set中已有節(jié)點(diǎn)的cost，如果新節(jié)點(diǎn)的cost更小，則更新open_set中的節(jié)點(diǎn)的cost和父節(jié)點(diǎn)索引為新節(jié)點(diǎn)的cost和父節(jié)點(diǎn)索引。否則，將新節(jié)點(diǎn)添加到open_set中。

最后，如果沒有找到路徑（path_found為False），則返回空列表。否則，生成最終的路徑坐標(biāo)列表rx和ry。從目標(biāo)節(jié)點(diǎn)開始，迭代找到每個(gè)節(jié)點(diǎn)的父節(jié)點(diǎn)，將節(jié)點(diǎn)的x和y坐標(biāo)添加到rx和ry中。直到父節(jié)點(diǎn)索引為-1，表示到達(dá)起點(diǎn)。返回rx和ry作為最短路徑的坐標(biāo)列表。

總之，該算法使用迪杰斯特拉算法，在給定的路徑地圖中尋找起點(diǎn)到目標(biāo)點(diǎn)的最短路徑，并返回路徑的坐標(biāo)列表。