3、Python科学计算栈：NumPy基础、Matplotlib可视化、SciPy在路径规划中的应用

好，咱们进入第三个章节。说实话，Python科学计算栈这三个库，是我做路径规划仿真时最趁手的工具。NumPy管数据，Matplotlib管展示，SciPy管算法。三者配合，基本能搞定90%的仿真验证工作。

我个人习惯是：先拿NumPy把地图和路径数据组织好，再用SciPy做插值或优化，最后用Matplotlib把结果画出来。这套流程我用了快十年，非常稳定。

3.1 NumPy基础：路径数据的“骨架”

NumPy的核心是ndarray，也就是多维数组。在路径规划里，最常见的用法是用二维数组表示栅格地图，用二维数组存储路径点序列。

创建地图矩阵

比如一个10x10的栅格地图，0表示可通行，1表示障碍物：

import numpy as np

# 创建全零地图
grid_map = np.zeros((10, 10), dtype=np.int8)
# 手动设置障碍物
grid_map[2:5, 3:7] = 1
grid_map[7, 2:9] = 1
print(grid_map)

路径点存储

路径通常是一系列(x, y)坐标。我习惯用Nx2的数组来存：

# 假设有一条从起点到终点的路径
path = np.array([
    [0.5, 0.5],
    [2.3, 1.8],
    [4.1, 3.2],
    [6.7, 5.0],
    [9.0, 9.0]
])
print(path.shape)  # (5, 2)

数组切片与索引

路径规划中经常要取某一段路径，或者筛选特定条件的点。NumPy的布尔索引非常方便：

# 筛选x坐标大于5的路径点
filtered = path[path[:, 0] > 5]
print(filtered)

嗯，这里要注意：NumPy的索引是从0开始的，跟MATLAB不一样。我刚从MATLAB转过来时，在这上面栽过跟头。

3.2 Matplotlib可视化：让路径“看得见”

做路径规划，光有数据不行，得画出来。Matplotlib的pyplot模块是最常用的。

绘制栅格地图与路径

我一般用 imshow 显示地图，用 plot 画路径：

import matplotlib.pyplot as plt

plt.figure(figsize=(8, 8))
# 显示栅格地图
plt.imshow(grid_map, cmap='gray_r', origin='lower')
# 画路径
plt.plot(path[:, 0], path[:, 1], 'b-', linewidth=2, label='规划路径')
plt.scatter(path[0, 0], path[0, 1], c='g', s=100, label='起点')
plt.scatter(path[-1, 0], path[-1, 1], c='r', s=100, label='终点')
plt.legend()
plt.grid(True, alpha=0.3)
plt.title('路径规划仿真结果')
plt.show()

动态仿真

有时候想看路径点一步步走过去的效果。可以用 plt.pause 做简单动画：

plt.ion()  # 开启交互模式
for i in range(len(path)):
    plt.cla()
    plt.imshow(grid_map, cmap='gray_r', origin='lower')
    plt.plot(path[:i+1, 0], path[:i+1, 1], 'b-')
    plt.scatter(path[i, 0], path[i, 1], c='r', s=50)
    plt.pause(0.2)
plt.ioff()
plt.show()

我曾经用这个方式给客户演示A*算法的搜索过程，效果非常好。对方一看就明白算法是怎么一步步找到路径的。

3.3 SciPy在路径规划中的应用

SciPy在路径规划里最常用的模块是 scipy.interpolate 和 scipy.spatial。说白了，就是路径平滑和空间查询。

路径平滑：样条插值

规划出来的路径往往是折线，机器人走起来会一顿一顿的。用样条插值可以让路径变平滑：

from scipy.interpolate import CubicSpline

# 原始路径点（折线）
t = np.linspace(0, 1, len(path))
# 创建三次样条插值
cs_x = CubicSpline(t, path[:, 0])
cs_y = CubicSpline(t, path[:, 1])

# 生成密集点
t_dense = np.linspace(0, 1, 100)
smooth_path = np.column_stack([cs_x(t_dense), cs_y(t_dense)])

最近邻搜索：KDTree

在路径规划中，经常要问“离这个点最近的障碍物在哪？”或者“路径上离目标点最近的是哪一段？”scipy.spatial.KDTree 就是干这个的：

from scipy.spatial import KDTree

# 假设有一堆障碍物坐标
obstacles = np.array([
    [3, 4], [3, 5], [4, 4], [4, 5],
    [7, 3], [7, 4], [8, 3], [8, 4]
])
tree = KDTree(obstacles)

# 查询离(5, 5)最近的障碍物
dist, idx = tree.query([5.0, 5.0])
print(f"最近障碍物距离: {dist:.2f}, 坐标: {obstacles[idx]}")

# 查询半径2内的所有障碍物
indices = tree.query_ball_point([5.0, 5.0], r=2.0)
print(f"半径2内的障碍物数量: {len(indices)}")

距离变换与势场

用 scipy.ndimage.distance_transform_edt 可以快速生成距离场，这在势场法路径规划中非常有用：

from scipy.ndimage import distance_transform_edt

# 障碍物地图（1表示障碍物）
obstacle_map = (grid_map == 1).astype(np.int8)
# 计算到最近障碍物的欧氏距离
dist_map = distance_transform_edt(1 - obstacle_map)
print(dist_map[:5, :5])

3.4 三者协同：一个完整示例

最后，咱们把三个库串起来，做一个完整的路径规划仿真流程：

import numpy as np
import matplotlib.pyplot as plt
from scipy.interpolate import CubicSpline
from scipy.spatial import KDTree

# 1. NumPy：生成地图和路径
grid = np.zeros((20, 20))
grid[5:8, 10:15] = 1
grid[12:15, 3:8] = 1

path_raw = np.array([
    [1, 1], [4, 3], [8, 6], [12, 10], [18, 18]
])

# 2. SciPy：路径平滑
t = np.linspace(0, 1, len(path_raw))
cs_x = CubicSpline(t, path_raw[:, 0], bc_type='natural')
cs_y = CubicSpline(t, path_raw[:, 1], bc_type='natural')
t_dense = np.linspace(0, 1, 200)
path_smooth = np.column_stack([cs_x(t_dense), cs_y(t_dense)])

# 3. SciPy：碰撞检测
obstacle_coords = np.argwhere(grid == 1)
tree = KDTree(obstacle_coords)
min_dists, _ = tree.query(path_smooth)
collision_free = np.all(min_dists > 0.5)

# 4. Matplotlib：可视化
plt.figure(figsize=(10, 10))
plt.imshow(grid, cmap='gray_r', origin='lower', extent=[0, 20, 0, 20])
plt.plot(path_raw[:, 0], path_raw[:, 1], 'r--', label='原始路径', linewidth=2)
plt.plot(path_smooth[:, 0], path_smooth[:, 1], 'b-', label='平滑路径', linewidth=2)
plt.scatter(path_raw[0, 0], path_raw[0, 1], c='g', s=150, label='起点')
plt.scatter(path_raw[-1, 0], path_raw[-1, 1], c='r', s=150, label='终点')
plt.legend(fontsize=12)
plt.grid(True, alpha=0.3)
plt.title(f'路径规划仿真 | 碰撞检测: {"通过" if collision_free else "失败"}', fontsize=14)
plt.show()

print(f"路径总长度: {np.sum(np.linalg.norm(np.diff(path_smooth, axis=0), axis=1)):.2f}")
print(f"碰撞检测结果: {'安全' if collision_free else '有碰撞风险'}")

你看，三个库配合起来，从数据到算法到可视化，一条龙搞定。我个人觉得，这套组合是路径规划仿真的黄金搭档。你把这个流程跑熟了，后面学ROS、学MoveIt都会轻松很多。

下一章咱们会讲A*算法的实现，到时候这些工具全都会用上。先消化消化今天的内容，有问题随时问我。