【笔记：3D航路规划算法】一、随机搜索锚点（python实现，讲解思路）

3D路径规划是在三维空间中寻找从起点到终点的最短或最优路径的一种技术。它广泛应用于无人机导航、机器人运动规划、虚拟现实等领域

关键概念

1. 节点和网格：

   • 在3D路径规划中，节点表示三维空间中的位置，通常用(x, y, z)坐标表示。
   • 网格是将三维空间离散化的方式，可以是规则的立方体网格或其他形式的分区。

2. 代价函数：

   • 代价函数用于评估从一个节点移动到另一个节点的代价，通常包括距离和可能的障碍物影响。
   • 常见的距离度量包括欧几里得距离、曼哈顿距离等。

3. 启发式函数：

   • 启发式函数用于估计当前节点到目标节点的代价，在A*算法中非常重要。
   • 例如，三维欧几里得距离可以用作启发式函数。

4. 障碍物：

   • 障碍物表示在三维空间中不可通过的区域。
   • 在路径规划中需要检测和避开这些区域。

3D路径规划算法

1. A*算法

A算法是最常用的路径规划算法之一，它结合了启发式搜索和实际代价来找到最优路径。以下是A算法在3D空间中的步骤：

1. 初始化：

   • 创建起点节点和终点节点。
   • 将起点节点添加到开放集合（open set）中。

2. 搜索过程：

   • 从开放集合中取出代价最小的节点。
   • 生成当前节点的所有邻居节点。
   • 对每个邻居节点进行评估，如果节点在障碍物中，跳过该节点。
   • 如果邻居节点是终点节点，记录路径并结束搜索。
   • 否则，计算邻居节点的代价，并将其添加到开放集合中。

3. 路径构建：

   • 从终点节点开始回溯到起点节点，构建最终路径。

2. 快速随机锚点

随机锚点是一种适用于高维空间的路径规划算法，通过随机采样和锚点的扩展找到路径。

1. 初始化：

创建一个搜索空间

space_size = np.array([(0, 1000), (0, 1000), (0, 1000)])  # 搜索空间的维度
Z = search_tree(space_size)

生成若干障碍物，这里均为长方体，简单一点可以构建2个顶点就可以制作好障碍物
还需要输入起始位置和目标位置，不能让这两点在障碍物中

n = 100 # n个障碍物
z_start = (0, 0, 0)  # 起始位置
z_end = (1000, 1000, 1000)  # 目标位置
Cuboid = generate_random_cuboid(Z, z_start, z_end, n)

2. 实例化搜索算法：

q = 50  # 在路径规划过程中，每次扩展时，新的节点之间的最大距离。这个距离越大，扩展速度越快，但可能会导致不够精细的路径
r = 1  # 检测新生成的路径是否与障碍物相交的最小边长度。如果新生成的边长度小于 r，则需要检查该边是否与障碍物相交
max_explore = 512  # 在路径规划过程中最多允许采样的次数。如果达到这个次数还没有找到有效路径，则算法会超时
p = 0.5  # 检查连接到目标的概率。在每次扩展时，有一定的概率直接尝试将新节点连接到目标位置。这个概率值越大，算法越倾向于直接尝试连接到目标，而不是继续随机扩展。
xxx = XXX(Z, q, z_start, z_end, max_explore, r, p)

3. 路径生成：

• 将起点添加到空间中
• 开始循环
  • 随机生成一个锚点
  • 找到随机锚点距离最近的空间节点(一开始就是起点)
  • 找到随机锚点和空间节点，距离为空间节点q的一个点
  • 检查锚点是否在长方体内，如果不在则返回
  • 判断锚点和空间节点的边是否在障碍物内，若不在，则相连
  • 根据检测概率p检查当前新锚点是否可以连接到目标，如果可以则视为成功推出循环
• 从终点反推起点生成路径

4. 绘制图像：

可旋转探索效果展示