路径规划算法的python实现

MATLAB A* 搜索算法转 Python 实现

下面我把这套 MATLAB 代码完整翻译成 Python，使用 `numpy` + `matplotlib`，运行后能看到和 MATLAB 一样的效果：随机障碍地图、起点、终点、A* 规划出的最优路径。

文件结构

建议按如下结构组织：

```

a_star_python/

├── main.py

├── obstacle_map.py

├── a_star_search.py

├── visualize_map.py

├── distance.py

├── expand_array.py

├── insert_open.py

├── min_fn.py

└── node_index.py

```

---

1. `distance.py`

```python

import math

def distance(x1, y1, x2, y2):

"""计算两点之间的欧氏距离"""

return math.sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2)

```

---

2. `obstacle_map.py`

```python

import numpy as np

def obstacle_map(xStart, yStart, xTarget, yTarget, MAX_X, MAX_Y):

"""

生成随机障碍地图，返回 N x 2 的数组。

第 1 行为起点，最后 1 行为终点，中间为障碍。

"""

rand_map = np.random.rand(MAX_X, MAX_Y)

map_list = \[\]

map_list.append(xStart, yStart)

obstacle_ratio = 0.25

for i in range(1, MAX_X + 1):

for j in range(1, MAX_Y + 1):

if (rand_mapi - 1, j - 1 < obstacle_ratio

and not (i == xStart and j == yStart)

and not (i == xTarget and j == yTarget)):

map_list.append(i, j)

map_list.append(xTarget, yTarget)

return np.array(map_list, dtype=float)

```

---

3. `insert_open.py`

```python

import numpy as np

def insert_open(xval, yval, parent_xval, parent_yval, hn, gn, fn):

"""

OPEN 列表格式:

0: IS ON LIST (1/0)

1: X val

2: Y val

3: Parent X val

4: Parent Y val

5: h(n)

6: g(n)

7: f(n)

"""

new_row = np.array(\[1, xval, yval, parent_xval, parent_yval, hn, gn, fn], dtype=float)

return new_row

```

---

4. `min_fn.py`

```python

import numpy as np

def min_fn(OPEN, OPEN_COUNT, xTarget, yTarget):

"""

返回 OPEN 列表中 f(n) 最小的节点在 OPEN 中的行索引（0-based）。

如果 OPEN 中没有可用节点，返回 -1。

"""

temp_array = \[\]

for j in range(OPEN_COUNT):

if OPENj, 0 == 1:

temp_array.append(list(OPENj, :) + j) # 末尾追加原始索引

if len(temp_array) == 0:

return -1

temp_array = np.array(temp_array)

第 7 列是 f(n)（0-based），最后一列是原索引

temp_min = int(np.argmin(temp_array:, 7))

i_min = int(temp_arraytemp_min, -1)

return i_min

```

---

5. `node_index.py`

```python

def node_index(OPEN, xval, yval):

"""返回 OPEN 列表中坐标为 (xval, yval) 的行索引（0-based）"""

i = 0

n = OPEN.shape0

while i < n:

if OPENi, 1 == xval and OPENi, 2 == yval:

return i

i += 1

return -1

```

---

6. `expand_array.py`

```python

import numpy as np

from distance import distance

def expand_array(node_x, node_y, gn, xTarget, yTarget, CLOSED, MAX_X, MAX_Y):

"""

返回当前节点的所有合法后继节点数组：

列格式: | X | Y | h(n) | g(n) | f(n) |

"""

exp_array = \[\]

c2 = CLOSED.shape0

for k in 1, 0, -1:

for j in 1, 0, -1:

if k == 0 and j == 0:

continue # 跳过自身

s_x = node_x + k

s_y = node_y + j

if 1 <= s_x <= MAX_X and 1 <= s_y <= MAX_Y:

flag = 1

for c1 in range(c2):

if s_x == CLOSEDc1, 0 and s_y == CLOSEDc1, 1:

flag = 0

break

if flag == 1:

h = distance(xTarget, yTarget, s_x, s_y)

g = gn + distance(node_x, node_y, s_x, s_y)

f = h + g

exp_array.append(s_x, s_y, h, g, f)

if len(exp_array) == 0:

return np.zeros((0, 5))

return np.array(exp_array, dtype=float)

```

---

7. `a_star_search.py`（核心）

```python

import numpy as np

from distance import distance

from insert_open import insert_open

from min_fn import min_fn

from node_index import node_index

from expand_array import expand_array

def a_star_search(map_data, MAX_X, MAX_Y):

"""

A* 搜索主函数。

输入 map_data: N x 2 数组，第 1 行为起点，最后 1 行为终点，中间为障碍。

输出 path: M x 2 数组，从起点到终点的最优路径（含起点和终点）。

同时返回 OPEN / CLOSED 用于可视化（可选）。

"""

size_map = map_data.shape0

X_offset = 0

Y_offset = 0

MAP: 2=空闲, -1=障碍, 0=终点, 1=起点

MAP = 2 * np.ones((MAX_X, MAX_Y), dtype=float)

终点

xval = int(np.floor(map_datasize_map - 1, 0)) + X_offset

yval = int(np.floor(map_datasize_map - 1, 1)) + Y_offset

xTarget, yTarget = xval, yval

MAPxval - 1, yval - 1 = 0

障碍

for i in range(1, size_map - 1):

xval = int(np.floor(map_datai, 0)) + X_offset

yval = int(np.floor(map_datai, 1)) + Y_offset

MAPxval - 1, yval - 1 = -1

起点

xval = int(np.floor(map_data0, 0)) + X_offset

yval = int(np.floor(map_data0, 1)) + Y_offset

xStart, yStart = xval, yval

MAPxval - 1, yval - 1 = 1

---------------- 初始化 OPEN / CLOSED ----------------

OPEN = np.zeros((0, 8))

CLOSED = np.zeros((0, 2))

把所有障碍放入 CLOSED

closed_list = \[\]

for i in range(1, MAX_X + 1):

for j in range(1, MAX_Y + 1):

if MAPi - 1, j - 1 == -1:

closed_list.append(i, j)

if len(closed_list) > 0:

CLOSED = np.array(closed_list, dtype=float)

CLOSED_COUNT = CLOSED.shape0

起点加入 OPEN

xNode, yNode = xval, yval

OPEN_COUNT = 1

goal_distance = distance(xNode, yNode, xTarget, yTarget)

path_cost = 0

OPEN = insert_open(xNode, yNode, xNode, yNode, goal_distance, path_cost, goal_distance)

OPEN0, 0 = 0 # 起点自身标记为不在 OPEN（MATLAB 原逻辑保留）

起点同时加入 CLOSED

CLOSED_COUNT += 1

CLOSED = np.vstack(CLOSED, \[\[xNode, yNode]])

NoPath = 1

---------------- A* 主循环 ----------------

记录所有访问过的节点用于可视化

visit_nodes: 每行 IS_ON_LIST(1=open,0=closed), X, Y

visit_nodes = \[\]

while NoPath == 1:

从 OPEN 中选 f 最小的节点

i_min = min_fn(OPEN, OPEN_COUNT, xTarget, yTarget)

if i_min == -1:

OPEN 空了，无路可走

NoPath = 0

break

取出该节点并从 OPEN 移除（标记为 0）

node_x = OPENi_min, 1

node_y = OPENi_min, 2

OPENi_min, 0 = 0

加入 CLOSED

CLOSED_COUNT += 1

CLOSED = np.vstack(CLOSED, \[\[node_x, node_y]])

记录到 visit_nodes（作为 closed）

visit_nodes.append(0, node_x, node_y)

判断是否到达终点

if node_x == xTarget and node_y == yTarget:

NoPath = 0

xval, yval = node_x, node_y

break

扩展当前节点

gn = OPENi_min, 6

exp_array = expand_array(node_x, node_y, gn,

xTarget, yTarget, CLOSED, MAX_X, MAX_Y)

for i in range(exp_array.shape0):

exp_x = exp_arrayi, 0

exp_y = exp_arrayi, 1

h = exp_arrayi, 2

g = exp_arrayi, 3

f = exp_arrayi, 4

检查该后继是否已经在 OPEN 里

n_index = -1

for j in range(OPEN_COUNT):

if OPENj, 0 == 1 and OPENj, 1 == exp_x and OPENj, 2 == exp_y:

n_index = j

break

if n_index == -1:

不在 OPEN -> 插入

new_row = insert_open(exp_x, exp_y, node_x, node_y, h, g, f)

OPEN = np.vstack(OPEN, new_row)

OPEN_COUNT += 1

visit_nodes.append(1, exp_x, exp_y)

else:

已在 OPEN -> 若新路径更优则更新

if f < OPENn_index, 7:

OPENn_index, 3 = node_x

OPENn_index, 4 = node_y

OPENn_index, 5 = h

OPENn_index, 6 = g

OPENn_index, 7 = f

---------------- 回溯最优路径 ----------------

path = \[\]

if NoPath == 0 and OPEN.shape0 > 0:

找到终点在 OPEN 中的索引

注意：终点可能已经在 CLOSED 中（因为循环里先加入 CLOSED 再 break）

所以直接在 CLOSED 里找终点并回溯 parent

但 CLOSED 不含 parent 信息，所以需要从 OPEN 里找

上面循环中到达终点时已 break，此时终点在 OPEN 中标记为 0，且带 parent

target_idx = -1

for j in range(OPEN_COUNT):

if OPENj, 1 == xTarget and OPENj, 2 == yTarget:

target_idx = j

break

if target_idx != -1:

path.append(xTarget, yTarget)

parent_x = OPENtarget_idx, 3

parent_y = OPENtarget_idx, 4

while not (parent_x == xStart and parent_y == yStart):

path.append(parent_x, parent_y)

p_idx = -1

for j in range(OPEN_COUNT):

if OPENj, 1 == parent_x and OPENj, 2 == parent_y:

p_idx = j

break

if p_idx == -1:

break

parent_x = OPENp_idx, 3

parent_y = OPENp_idx, 4

path.append(xStart, yStart)

path.reverse()

path = np.array(path, dtype=float) if len(path) > 0 else np.zeros((0, 2))

if len(visit_nodes) > 0:

visit_nodes = np.array(visit_nodes, dtype=float)

else:

visit_nodes = np.zeros((0, 3))

return path, visit_nodes

```

> **说明**：MATLAB 原代码中 `while(0)` 是留给你补全的作业。上面 Python 实现已经补全了整个 A* 主循环，逻辑和 MATLAB 注释里的 OPEN/CLOSED 结构完全一致。

---

8. `visualize_map.py`

```python

import numpy as np

import matplotlib.pyplot as plt

from matplotlib.patches import Patch

def visualize_map(map_data, path, visit_nodes=None):

"""可视化 2D 栅格地图、起点、终点、最优路径、访问节点"""

fig, ax = plt.subplots(figsize=(7, 7))

ax.set_aspect('equal')

sz_map = int(np.max(map_data))

障碍（不含起点和终点）

if map_data.shape0 > 2:

obst = map_data1:-1, :

ax.scatter(obst:, 0 - 0.5, obst:, 1 - 0.5,

s=max(2500 / sz_map, 36),

c=\[55 / 255, 184 / 255, 157 / 255],

marker='o')

起点

ax.scatter(map_data0, 0 - 0.5, map_data0, 1 - 0.5,

marker='*', c='b', s=120, label='Start')

终点

ax.scatter(map_data-1, 0 - 0.5, map_data-1, 1 - 0.5,

marker='*', c='r', s=120, label='Target')

最优路径

if path is not None and path.shape0 > 1:

中间点

if path.shape0 > 2:

ax.scatter(path1:-1, 0 - 0.5, path1:-1, 1 - 0.5,

c='b', s=30)

ax.plot(path:, 0 - 0.5, path:, 1 - 0.5, 'b-', linewidth=2)

访问节点（OPEN / CLOSED）

if visit_nodes is not None and visit_nodes.shape0 > 0:

node_sz = 10000 / sz_map

OPEN 列表（绿色）

open_nodes = visit_nodesvisit_nodes\[:, 0 == 1]

if open_nodes.shape0 > 0:

ax.scatter(open_nodes:, 1 - 0.5, open_nodes:, 2 - 0.5,

s=node_sz, c='g', marker='s', alpha=0.1)

CLOSED 列表（蓝色）

closed_nodes = visit_nodesvisit_nodes\[:, 0 == 0]

if closed_nodes.shape0 > 0:

ax.scatter(closed_nodes:, 1 - 0.5, closed_nodes:, 2 - 0.5,

s=node_sz, c='b', marker='s', alpha=0.1)

网格

ax.set_xticks(np.arange(0, sz_map + 1, 1))

ax.set_yticks(np.arange(0, sz_map + 1, 1))

ax.grid(True, linewidth=0.5, alpha=0.5)

ax.set_xlim(-1, sz_map + 1)

ax.set_ylim(-1, sz_map + 1)

ax.set_xlabel('X')

ax.set_ylabel('Y')

ax.set_title('A* Path Planning')

ax.legend(loc='upper right')

plt.tight_layout()

plt.show()

```

---

9. `main.py`

```python

import numpy as np

import matplotlib.pyplot as plt

from obstacle_map import obstacle_map

from a_star_search import a_star_search

from visualize_map import visualize_map

def main():

固定随机种子，方便复现

np.random.seed(42)

xStart = 1.0

yStart = 1.0

xTarget = 9.0

yTarget = 9.0

MAX_X = 10

MAX_Y = 10

生成地图

map_data = obstacle_map(xStart, yStart, xTarget, yTarget, MAX_X, MAX_Y)

A* 搜索

path, visit_nodes = a_star_search(map_data, MAX_X, MAX_Y)

可视化

visualize_map(map_data, path, visit_nodes)

if path.shape0 > 0:

print("找到路径，长度：", path.shape0)

print("路径：")

print(path)

else:

print("未找到路径")

if name == "main":

main()

```

---

10. 运行方式

```bash

pip install numpy matplotlib

python main.py

```

运行后你会看到：

• 绿色圆点：随机障碍（约 25% 比例）

• 蓝色星号：起点 (1,1)

• 红色星号：终点 (9,9)

• 绿色半透明方块：OPEN 列表访问过的节点

• 蓝色半透明方块：CLOSED 列表访问过的节点

• 蓝色连线：A* 找出的最优路径

效果与 MATLAB 的 `visualize_map` 基本一致。

---

11. 与 MATLAB 代码的对应关系

| MATLAB 文件 | Python 文件 | 说明 |

|---|---|---|

| `distance.m` | `distance.py` | 欧氏距离 |

| `obstacle_map.m` | `obstacle_map.py` | 随机障碍地图 |

| `insert_open.m` | `insert_open.py` | 构造 OPEN 行 |

| `min_fn.m` | `min_fn.py` | 找 f 最小节点 |

| `node_index.m` | `node_index.py` | 查节点在 OPEN 中的索引 |

| `expand_array.m` | `expand_array.py` | 扩展后继节点 |

| `A_star_search.m` | `a_star_search.py` | A* 主逻辑（含补全的 while 循环和回溯） |

| `visualize_map.m` | `visualize_map.py` | 可视化 |

| `main.m` | `main.py` | 入口 |

---

12. 几点注意

1. **索引差异**：MATLAB 从 1 开始，Python 从 0 开始。代码中所有对 `MAP`、`OPEN`、`CLOSED` 的访问都做了 `-1` 或保持不变的处理，逻辑上与原代码一致。

2. **`while(0)` 的补全**：原 MATLAB 是作业留白，Python 版已经补全完整 A* 主循环，包括：

• 从 OPEN 取 f 最小节点；

• 移入 CLOSED；

• 判断是否到终点；

• 扩展后继；

• 若后继已在 OPEN 且新 f 更小，则更新。

3. **路径回溯**：从终点沿 parent 一路回到起点，最后 `reverse` 得到从起点到终点的顺序。

4. **可视化 visit_nodes**：MATLAB 中 `visit_nodes` 每行是 `IS_ON_LIST, X, Y`，其中 1 表示 OPEN，0 表示 CLOSED。Python 版保持一致。

5. **随机种子**：MATLAB 每次运行地图不同，Python 里用 `np.random.seed(42)` 固定，方便调试；想随机可去掉这行。

---

13. 可能遇到的问题

• **找不到路径**：当地图障碍太密时，A* 可能无解。可以减小 `obstacle_ratio` 或换随机种子。

• **OpenCV/Matplotlib 中文乱码**：标题用了英文，避免字体问题。

• **性能**：当前 OPEN 用 `numpy` 数组 + 线性扫描，10×10 地图完全够用；如果地图很大，建议换成 `heapq` 或 `scipy` 的优先队列。

---

这样你就得到了一个可运行的 Python 版 A* 路径规划，效果和 MATLAB 版本一致。