【笔记：3D航路规划算法】一、RRT

news2024/11/15 15:43:09

关键概念

节点和网格：
- 在3D路径规划中，节点表示三维空间中的位置，通常用(x, y, z)坐标表示。
- 网格是将三维空间离散化的方式，可以是规则的立方体网格或其他形式的分区。
代价函数：
- 代价函数用于评估从一个节点移动到另一个节点的代价，通常包括距离和可能的障碍物影响。
- 常见的距离度量包括欧几里得距离、曼哈顿距离等。
启发式函数：
- 启发式函数用于估计当前节点到目标节点的代价，在A*算法中非常重要。
- 例如，三维欧几里得距离可以用作启发式函数。
障碍物：
- 障碍物表示在三维空间中不可通过的区域。
- 在路径规划中需要检测和避开这些区域。

3D路径规划算法

1. A*算法

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

初始化：
- 创建起点节点和终点节点。
- 将起点节点添加到开放集合（open set）中。
搜索过程：
- 从开放集合中取出代价最小的节点。
- 生成当前节点的所有邻居节点。
- 对每个邻居节点进行评估，如果节点在障碍物中，跳过该节点。
- 如果邻居节点是终点节点，记录路径并结束搜索。
- 否则，计算邻居节点的代价，并将其添加到开放集合中。
路径构建：
- 从终点节点开始回溯到起点节点，构建最终路径。

2. RRT

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

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  # 检查连接到目标的概率。在每次扩展时，有一定的概率直接尝试将新节点连接到目标位置。这个概率值越大，算法越倾向于直接尝试连接到目标，而不是继续随机扩展。
rrt = RRT(Z, q, z_start, z_end, max_explore, r, p)

3. 路径生成：

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