路径寻找问题

路径寻找问题其实就是帮我们在复杂的环境中找到从起点到终点的最佳路线。比如说，在城市里找一条最快的驾车路线，或者让游戏里的角色避开障碍物，顺利到达目标点。

你可以想象这是一个迷宫，从起点走到终点，我们希望找到一条最快、最省力的路。路上可能有各种障碍物，比如墙、河流，或者有些路会比其他路更难走（需要花费更多时间）。

Pathfinding3D

Pathfinding3D是一个多功能的Python库，专门用于解决3D路径寻找问题。

此库基于python-pathfinding进行二次开发，提供了多种优化算法，适用 3D应用场景。

支持8种路径算法：

安装方法

依赖要求

• Python 版本 >= 3.8
• numpy

使用pip安装即可：

pip install pathfinding3d

一个简单的示例

import numpy as np
from pathfinding3d.core.diagonal_movement import DiagonalMovement
from pathfinding3d.core.grid import Grid
from pathfinding3d.finder.a_star import AStarFinder

# 创建一个 3D numpy 数组，1 代表可行路径，0 代表障碍物
matrix = np.ones((10, 10, 10), dtype=np.int8)
matrix[5, 5, 5] = 0  # 设置中心为障碍物

# 使用 numpy 数组创建 Grid 对象
grid = Grid(matrix=matrix)

# 设置起点和终点
start = grid.node(0, 0, 0)
end = grid.node(9, 9, 9)

# 创建 A* 路径算法实例，允许对角线移动
finder = AStarFinder(diagonal_movement=DiagonalMovement.always)
path, runs = finder.find_path(start, end, grid)

# 将路径转换为坐标元组列表
path = [p.identifier for p in path]

print("算法运行次数:", runs, "路径长度:", len(path))
print("路径:", path)

输出：

算法运行次数: 52 路径长度: 11
路径: [(0, 0, 0), (1, 1, 1), (2, 2, 2), (3, 3, 3), (4, 4, 4), (5, 4, 4), (6, 5, 5), (7, 6, 6), (8, 7, 7), (9, 8, 8), (9, 9, 9)]

路径可视化

你可以通过Grid类的visualize方法将网格和路径进行可视化。如果希望使用此功能，需要安装plotly库：

pip install pathfinding3d[vis] 

使用以下代码可视化路径：

grid.visualize(
  path=path,
  start=start,
  end=end,
  visualize_weight=True,  # 可视化节点权重
  save_html=True,  # 保存为 HTML 文件
  save_to="path_visualization.html",  # 文件保存路径
  always_show=True  # 自动在浏览器中打开
)

再看一个复杂一点的

import os
import numpy as np

# 导入路径寻找所需的模块
from pathfinding3d.core.diagonal_movement import DiagonalMovement  # 对角线移动策略
from pathfinding3d.core.grid import Grid  # 网格表示
from pathfinding3d.finder.a_star import AStarFinder  # A* 算法

# 尝试导入 Open3D 库用于 3D 可视化
USE_OPEN3D = True
try:
# Open3D 仅支持 Python 3.8, 3.9 和 3.10 版本
import open3d as o3d
except ImportError:
    USE_OPEN3D = False
print("未安装 Open3D。请使用 'pip install open3d' 安装")

# 加载 3D 地图文件，作为 numpy 数组
# 矩阵中的每个元素代表空间中的一个点:
# 0 表示障碍物，1 表示可通行区域
sample_map_path = "./sample_map.npy"# 地图文件路径
matrix = np.load(sample_map_path)  # 加载地图数据

# 定义起点和终点的 [x, y, z] 坐标
start_pt = [21, 21, 21]  # 起点
end_pt = [5, 38, 33]  # 终点

# 创建网格对象并设置起点和终点的节点
grid = Grid(matrix=matrix)  # 用地图矩阵创建网格
start = grid.node(*start_pt)  # 获取起点节点
end = grid.node(*end_pt)  # 获取终点节点

# 初始化 A* 寻路器，并设置对角线移动规则
finder = AStarFinder(diagonal_movement=DiagonalMovement.only_when_no_obstacle)

# 使用 A* 寻路器找到路径
path, runs = finder.find_path(start, end, grid)  # 找到路径并返回运行次数

# 打印路径结果
path_cost = end.g  # 终点的 g 值代表路径代价
print(f"路径代价: {path_cost:.4f}, 路径长度: {len(path)}, 运行次数: {runs}")

# 将路径转换为坐标元组的列表
path = [p.identifier for p in path]  # 提取每个路径节点的坐标
print(f"路径: {path}")

# 如果安装了 Open3D，则可视化路径
if USE_OPEN3D:
# 识别地图中的障碍物，并将其设置为蓝色
    obstacle_indices = np.where(matrix == 0)  # 找到地图中的障碍物索引
    xyz_pt = np.stack(obstacle_indices, axis=-1).astype(float)  # 障碍物的 3D 坐标
    colors = np.zeros((xyz_pt.shape[0], 3))  # 初始化颜色矩阵为全黑
    colors[:, 2] = obstacle_indices[2] / np.max(obstacle_indices[2])  # 障碍物设置为蓝色

# 设置起点和终点的颜色
    start_color = np.array([[1.0, 0, 0]])  # 红色表示起点
    end_color = np.array([[0, 1.0, 0]])  # 绿色表示终点
    path_colors = np.full((len(path) - 2, 3), [0.7, 0.7, 0.7])  # 路径的中间点设为灰色

# 组合路径点、起点、终点和障碍物进行可视化
    xyz_pt = np.concatenate((xyz_pt, [start_pt], [end_pt], path[1:-1]))  # 组合所有点的坐标
    colors = np.concatenate((colors, start_color, end_color, path_colors))  # 组合所有点的颜色

# 创建点云对象
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(xyz_pt)  # 设置点的坐标
    pcd.colors = o3d.utility.Vector3dVector(colors)  # 设置点的颜色

# 通过点云创建体素网格
    voxel_grid = o3d.geometry.VoxelGrid.create_from_point_cloud(pcd, voxel_size=1.0)
    axes = o3d.geometry.TriangleMesh.create_coordinate_frame(size=15.0, origin=np.array([-3.0, -3.0, -3.0]))

# 显示体素网格和坐标系
    o3d.visualization.draw_geometries([axes, voxel_grid], window_name="DHub pathfinding3d", width=1024, height=768)

输出：

最后简单介绍实现原理

所有路径寻找算法都继承自Finder类，主要步骤如下：

1. 调用find_path。
2. init_find初始化open_list，重置全局变量。
3. 处理open_list中的节点，依次探索邻居节点。
4. 对 A* 算法来说，check_neighbors弹出具有最小'f'值的节点，更新状态。
5. 继续探索邻居节点，直到找到目标节点。
6. 返回最终路径。

以下是流程图示：

应用场景

3D路径规划在多个领域都具有实际应用价值，从机器人导航到游戏开发、医疗影像和无人驾驶。

A*算法和类似的路径寻找算法在这些领域中通过为复杂环境提供高效的路径规划，帮助系统和设备更好地避开障碍物并找到最优路径。

也适合配电箱的铜排设计。