船舶路径规划系列算法——RRT算法

        RRT（Rapidly-exploring Random Tree）是一种用于路径规划的随机化算法，尤其适用于高维空间中的复杂环境。RRT的核心思想是通过随机采样的方式，迅速探索空间，并建立从起点到目标点的路径。该算法能够处理动态障碍物，并能有效地生成可行路径，广泛应用于机器人导航、无人驾驶和其他自动化系统中

1.RRT算法原理

RRT算法的基本原理是通过在状态空间中构建一棵树来表示已探索的路径。每次迭代，算法都会随机生成一个新的目标点，并从当前树中找到一个与该点距离最近的节点，然后从该节点朝向随机点扩展出一个新的节点。通过这种方式，RRT能够高效地探索高维空间，避开障碍物并找到路径。

2.RRT路径规划算法的主要步骤

步骤1：初始化

设置起点和终点节点

定义障碍物、地图边界和其他参数，包括最大迭代次数、步长、目标采样率等

创建一个节点列表，初始化时只包含起点

步骤2：从空间中随机采样一个节点（rnd_node）

生成一个0到1范围内的随机数

如果随机数大于目标采样率，在地图的宽度和高度范围内生成一个随机节点，并返回该节点

如果随机数小于或等于目标采样率，随机节点为终点节点

步骤3：寻找最近节点

遍历已探索的节点，找到与随机采样节点（rnd_node）距离最近的节点（nearest_node）

步骤4：扩展新节点

从最近节点（nearest_node）朝向随机节点（rnd_node）方向以一定步长扩展出一个新节点（new_node）

步骤5：碰撞检测

首先检查新节点 (new_node) 是否位于任何障碍物内部。如果是，则不允许添加此节点

其次，检查新节点与最近节点 (nearest_node) 之间的连线是否穿过任何障碍物。如果连线穿过障碍物，则不允许添加此节点

如果新节点既不在障碍物内部，又不与最近节点的连线穿过任何障碍物，则将新节点加入节点列表，并将新节点的父节点设置为最近节点

步骤6：路径发现与结果返回

如果新节点（new_node）到终点节点的距离小于等于步长，表示找到路径。此时，追溯新节点的父节点直到起点，生成从起点到目标的完整路径，并返回路径列表

如果新节点（new_node）到终点节点的距离大于步长，转到步骤7

步骤7：重复迭代

重复执行步骤2到步骤6，直到找到路径或达到最大迭代次数，如果达到最大迭代次数仍未找到路径，返回None

3.实例验证

4.主代码

 # 主路径规划函数
    def plan(self):
        for _ in range(self.max_iter):  # 进行最大迭代次数
            rnd_node = self.sample()  # 从空间中随机采样一个节点
            nearest_node = self.get_nearest_node(self.nodes, rnd_node)  # 找到与随机节点最近的已探索节点
            new_node = self.steer(nearest_node, rnd_node)  # 从最近节点朝向随机节点扩展出一个新节点

            if not self.check_collision(new_node):  # 检查新节点是否与障碍物碰撞
                self.nodes.append(new_node)  # 如果没有碰撞，将新节点添加到节点列表
                # 如果新节点到目标的距离小于等于步长，表示找到了路径
                if self.distance(new_node, self.goal) <= self.step_size:
                    print("Path found!")
                    return self.generate_final_course(new_node)  # 生成最终路径

        return None  # 如果迭代结束仍未找到路径，返回None