第五章：路徑規劃算法

歡迎回來，未來的機器人專家－＝≡(?ω?)

在之前的章節中，我們已為機器人配備了核心知識：它能夠跟蹤自身的機器人狀態/位姿，利用環境表示（柵格地圖）理解周圍環境，通過機器人運動模型預測運動軌跡，并借助定位濾波器精確定位。

現在，機器人已掌握"自身位置“和”環境信息"。但若要讓其從A點移動到B點呢？

當存在墻壁或障礙物時，它無法直線行進。它需要一份"移動計劃"。

這正是路徑規劃算法的用武之地！

什么是路徑規劃算法？

想象我們試圖在一個陌生城市中從當前位置前往朋友家。

打開手機GPS導航，它不僅能顯示直線距離，還會規劃出避開建筑、湖泊和單行道的實際路線。

路徑規劃算法就如同機器人的高級GPS系統。

其核心任務是：基于地圖（柵格地圖）信息避開障礙物，為機器人找到從起點到目標點的安全高效移動序列（即"路徑"），通常還需優化特定指標（如最短路徑或最快路徑）。

若無路徑規劃，機器人只會漫無目的游蕩或直接撞上首個障礙物！這是指導機器人高層移動決策的"大腦"。

---

路徑規劃的核心概念

理解機器人路徑規劃需掌握以下關鍵概念：

概念	描述	類比
起點	機器人的初始位置和朝向（基于機器人狀態/位姿）	GPS上標注的當前位置
目標點	機器人需要到達的最終位置和朝向	朋友家的詳細地址
障礙物	機器人不可進入或穿過的區域，通常定義在柵格地圖中	地圖上的建筑物、河流或禁行區
路徑	引導機器人從起點到目標點的中間點或狀態序列，需避開障礙物	GPS推薦的藍色導航路線
代價	表示路徑"成本"的數值，可以是距離、時間、能耗或其組合	GPS顯示的預計行程時間或里程數
優化	根據特定目標（如最短路徑、最快路徑）尋找最小化（或最大化）代價的路徑	在GPS中選擇"最短路線"或"最快路線"選項

---

如何使用路徑規劃算法（A*算法示例）

PythonRobotics包含多種路徑規劃算法。

我們從A*（A-Star）搜索算法開始，這是一種廣為人知且直觀的算法，尤其適用于柵格地圖。

假設我們需要機器人在存在障礙物的地圖中從(10, 10)米移動到(50, 50)米。

以下是使用PathPlanning/AStar/a_star.py中AStarPlanner類設置并運行A*規劃器的典型流程：

import matplotlib.pyplot as plt # 可視化工具
from PathPlanning.AStar.a_star import AStarPlanner# 1. 定義起點和目標點坐標（單位：米）
sx, sy = 10.0, 10.0  # 起點X,Y
gx, gy = 50.0, 50.0  # 目標點X,Y# 2. 定義地圖屬性及機器人尺寸
grid_size = 2.0      # 每個柵格單元為2.0m×2.0m
robot_radius = 1.0   # 機器人視為半徑1.0m的圓形# 3. 定義障礙物（障礙物中心點坐標）
# 簡單示例：創建墻狀障礙物
ox, oy = [], []
for i in range(10, 40): # 在x=20處創建垂直墻（y從10到39）ox.append(20.0)oy.append(float(i))
for i in range(20, 50): # 在y=30處創建水平墻（x從20到49）ox.append(float(i))oy.append(30.0)# 4. 創建A*規劃器對象
planner = AStarPlanner(ox, oy, grid_size, robot_radius)# 5. 執行路徑規劃！
print("開始路徑規劃...")
# rx, ry將存儲規劃路徑的x,y坐標
rx, ry = planner.planning(sx, sy, gx, gy)if rx: # 找到路徑時print(f"找到包含{len(rx)}個路徑點的路徑")# 可進行可視化：# plt.plot(ox, oy, ".k") # 繪制障礙物# plt.plot(sx, sy, "og") # 繪制起點# plt.plot(gx, gy, "xb") # 繪制目標點# plt.plot(rx, ry, "-r") # 繪制路徑# plt.grid(True); plt.axis("equal"); plt.show()
else:print("未找到可行路徑！")

代碼解析：

• 創建AStarPlanner對象時需提供障礙物坐標(ox, oy)、柵格尺寸(grid_size，用于生成內部柵格地圖)和機器人半徑(robot_radius，確保與障礙物保持安全距離)。
• planning()方法接收起點(sx, sy)和目標點(gx, gy)坐標，返回表示最優路徑的rx, ry坐標列表。若目標點被阻塞則返回None。

輸出結果rx, ry即為機器人應遵循的詳細路徑坐標序列

---

路徑規劃算法內部原理（A*算法拆解）

🎢啟發式函數

啟發式函數是一種“經驗法則”，用于估計從當前狀態到目標狀態的近似距離，幫助算法更快找到最優路徑。比如在地圖導航中，啟發式函數可以簡單計算兩點間的直線距離。

A*算法是一種智能路徑搜索算法，通過結合當前路徑成本和預估剩余距離（啟發式函數）來高效找到最優路徑，類似導航軟件選擇最快路線。

A*算法與BFS

A*算法可以看作BFS的智能升級版。

• BFS會盲目擴展所有可能路徑
• 而A通過啟發式函數優先探索更接近目標的路徑。
  兩者都用隊列管理待探索節點，但A的隊列按“實際成本+預估成本”排序，效率更高。

A*算法高層流程

A*代碼核心組件

AStarPlanner類管理地圖、障礙物及搜索過程，關鍵部分如下：

1. Node類：
A*算法中，"節點"代表算法正在考慮的柵格單元。其不僅包含x, y坐標，還存儲搜索所需信息（詳見第六章：路徑規劃搜索節點）。

PathPlanning/AStar/a_star.py中的簡化實現：

class AStarPlanner:# ... (其他代碼) ...class Node:def __init__(self, x, y, cost, parent_index):self.x = x            # 柵格單元X索引self.y = y            # 柵格單元Y索引self.cost = cost      # g_cost（從起點到本節點的累計代價）self.parent_index = parent_index # 到達本節點的父節點索引（用于路徑重建）

解析：

• x, y：柵格索引（非實際米制坐標），由AStarPlanner內部轉換。
• cost：即g_cost（從起點到當前節點的實際累計代價）。
• parent_index：關鍵回溯信息，記錄到達當前節點的前一節點。找到目標后，沿此索引逆向重建完整路徑。

2. 運動模型（定義可行移動方向）：
柵格化A*的"運動模型"定義了機器人可移動的相鄰單元。
a_star.py中的get_motion_model()靜態方法定義了8個可能方向（上下左右及對角線）：

class AStarPlanner:# ... (其他代碼) ...@staticmethoddef get_motion_model():# dx, dy, costmotion = [[1, 0, 1],      # 右移(X+1,Y+0)，代價1[0, 1, 1],      # 上移(X+0,Y+1)，代價1[-1, 0, 1],     # 左移(X-1,Y+0)，代價1[0, -1, 1],     # 下移(X+0,Y-1)，代價1[-1, -1, math.sqrt(2)], # 對角線(X-1,Y-1)，代價√2[-1, 1, math.sqrt(2)],  # 對角線(X-1,Y+1)，代價√2[1, -1, math.sqrt(2)],  # 對角線(X+1,Y-1)，代價√2[1, 1, math.sqrt(2)]]   # 對角線(X+1,Y+1)，代價√2return motion

解析：

• 每個子列表[dx, dy, cost]描述一種移動方式，dx和dy為柵格索引變化量。
• cost為移動代價。水平/垂直移動代價為1，對角線移動為實際距離√2，確保算法找到真正最短路徑。

3. 代價計算（f_cost = g_cost + h_cost）：
A*算法通過"啟發式函數"引導搜索方向，避免盲目探索。每個節點包含兩種代價：

代價類型	描述	類比
g_cost	從起點到當前節點的實際代價	已從起點行走的實際距離
h_cost	當前節點到目標的預估代價（啟發式）	預估剩余距離（如直線距離猜測）
f_cost	總預估代價（g_cost + h_cost）	路徑總長度的綜合預估

A*算法始終選擇開放集合中f_cost最低的節點擴展

a_star.py中的calc_heuristic方法通過歐氏距離計算h_cost：

import mathclass AStarPlanner:# ... (其他代碼) ...@staticmethoddef calc_heuristic(n1, n2):# n1: 當前節點, n2: 目標節點# 計算兩柵格點間的直線距離d = math.hypot(n1.x - n2.x, n1.y - n2.y)return d

解析：

• n1.x - n2.x和n1.y - n2.y為坐標差。
• math.hypot()計算直角三角形斜邊長度，作為引導A*高效搜索的"啟發式估計"。

4. 碰撞檢測（verify_node）：
規劃器需確認新柵格單元的安全性，包括檢查地圖邊界及障礙物（基于柵格地圖生成的obstacle_map），并考慮機器人半徑的安全距離。

class AStarPlanner:# ... (其他代碼) ...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)# 1. 檢查是否超出地圖邊界if not (self.min_x <= px < self.max_x and self.min_y <= py < self.max_y):return False# 2. 檢查障礙物碰撞# self.obstacle_map為二維數組（類似柵格地圖）# True表示占據/碰撞，False表示空閑if self.obstacle_map[node.x][node.y]: # 簡化檢測return Falsereturn True # 節點安全

解析：

• calc_grid_position將柵格索引轉換為實際坐標以進行邊界檢查。
• self.obstacle_map為預先生成的障礙物柵格圖，若obstacle_map[node.x][node.y]為True，表示該單元存在碰撞風險。

---

PythonRobotics中的其他路徑規劃算法

盡管A*在柵格地圖中表現優異，PythonRobotics還提供多種適應不同場景的規劃算法：

• 動態窗口法（DWA）(PathPlanning/DynamicWindowApproach/dynamic_window_approach.py)：一種局部路徑規劃算法。DWA不預先規劃全局路徑，而是在實時窗口中評估可行的速度與轉向指令，結合機器人動力學、障礙物規避及目標趨近進行決策。常用于實時避障
• 快速擴展隨機樹（RRT）(PathPlanning/RRT/rrt.py)：基于采樣的算法，通過隨機生成節點構建樹狀結構連接起點與目標。適用于復雜高維空間或狹窄通道環境，克服柵格方法的局限性
• Frenet最優軌跡（FOT）(PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py)：常用于自動駕駛的高級算法。在"Frenet坐標系"（與參考路徑如道路中線對齊）中生成多項式軌跡，通過優化速度、舒適度及避障等代價選擇最優路徑

這些算法展示了路徑規劃技術的多樣性，但均以實現安全高效移動為核心目標。

---

小結

本章深入探討了路徑規劃算法的關鍵作用。作為機器人的"智能導航系統"，該算法通過結合柵格地圖信息與啟發式搜索，在避開障礙物的同時優化路徑成本。

我們以A*算法為例，解析了節點擴展、代價計算及碰撞檢測的核心機制，并簡介了DWA、RRT和Frenet等高級算法

搜索節點作為多數規劃算法的核心組件，將在下一章詳細剖析其結構與應用邏輯。

下一章：路徑規劃搜索節點