### 記錄一下使用Python實現ROS平臺A*算法路徑規劃 ###

代碼可自取? ：Xz/little_projecthttps://gitee.com/Xz_zh/little_project.git

---

目錄

一、思路分析

二、算法實現

三、路徑規劃實現

---

一、思路分析

????????要求使用A*算法實現路徑規劃，可以將該任務分為三大部分內容。首先需要對輸入的圖片和起點終點進行處理，其次就是通過A*算法輸出規劃的路徑，最后再對路徑進行處理并實現可視化。

????????其中第一部分需要對輸入的地圖進行處理，將世界地圖轉化為柵格地圖，如果是彩色地圖還需要進行灰度處理并進行二值化處理（判斷障礙點），存入相關信息。第二部分A*算法通過代價函數，遍歷從起點到終點的路徑點并找到f值最小的路徑。最后需要將A*算法返回的柵格路徑轉化為世界地圖坐標，再進行可視化。

????????偽代碼：

# 初始化 ROS 節點
初始化 ROS 節點 "astar_planner"
訂閱 "/map" 以獲取 OccupancyGrid 地圖
訂閱 "/move_base_simple/goal" 以獲取 Rviz 選取的目標點
發布 "/astar_path" 以發布路徑# 處理地圖數據
函數 map_callback(msg):解析地圖信息（寬度、高度、分辨率、原點）轉換為二值柵格地圖（障礙物=1, 可通行=0）# 世界坐標 <-> 柵格坐標轉換
函數 world_to_map(x, y):計算柵格坐標并限制范圍函數 map_to_world(map_x, map_y):計算世界坐標# 處理選取的目標點
函數 clicked_point_callback(msg):解析 Rviz 選取的點，轉換為柵格坐標如果沒有起點，設置為起點如果已經有起點，設置終點并運行 A* 規劃調用 publish_path() 發送路徑# A* 算法
函數 astar(grid_map, start, end):初始化 開放列表 open_list，關閉集合 closed_set創建起點、終點節點while open_list 不是空:取出 f 最小的節點 current_node如果 current_node == 終點:回溯生成路徑返回路徑遍歷當前節點的四個鄰居:如果鄰居是障礙物或已在 closed_set，跳過計算 g, h, f如果 f 值比 open_list 中的同位置節點更優，加入 open_list返回 None（無可行路徑）# 發布路徑
函數 publish_path(path):構建 Path 消息依次添加路徑點，轉換回世界坐標發布 "/astar_path"

---

二、算法實現

????????A*算法是一種有序的搜索算法，常常用于優化問題求取最短路徑。其特點主要在于對估價函數的定義上。A*算法主要通過其估價函數來計算各點之間的代價值，從而找到代價最小的路徑即為最優路徑。估價函數?f(n)用于評估從起點經過當前節點?nn?到目標節點的總代價，其公式為：

• ：從起點到當前節點?nn?的實際代價（已知值）。
• ：從當前節點?nn?到目標節點的啟發式估計代價（預測值）。通常情況下利用曼哈頓函數計算。

這里關于A*算法的原理不在贅述。

????????由于需要存儲遍歷點的上一節點和對應的f 值，這里可以利用類實現。每個點是Node類的對象，每個點有position(當前點的坐標)、parent(父節點坐標)、g、h、f(代價函數)。

為了方便計算h值，封裝曼哈頓函數：

核心算法

三、路徑規劃實現

首先開啟ROS核心

啟動map_server加載PGM圖像

運行A*算法代碼

Rviz實現規劃路徑可視化