路径搜索算法——RRT算法-CSDN博客

本文链接：https://blog.csdn.net/swaggytiger/article/details/149258103

路径搜索算法——RRT算法

一、RRT算法核心思想

RRT算法是一种基于随机采样的路径规划算法，适用于在高维空间或复杂环境中寻找从起点到目标点的可行路径。其核心思想是通过随机生成点，逐步扩展一棵以起点为根节点的树，探索配置空间（Configuration Space），直到找到目标点或目标区域。RRT特别适合处理非凸障碍物和高维空间问题，因为它通过随机采样避免了显式构建整个空间的网格。

RRT的关键特点是：

随机性：通过随机采样点来探索空间，保证覆盖性。
增量式扩展：逐步构建树，每次扩展一个节点，计算量较小。
概率完备性：随着采样次数增加，若存在可行路径，RRT几乎一定能找到。

二、数学基础

在深入流程之前，先介绍几个关键概念：

配置空间（Configuration Space, $CspaceC_{\text{space}}$ ）：表示机器人或系统所有可能状态的空间。例如，对于一个二维平面上的移动机器人， $CspaceC_{\text{space}}$ 可能是 $(x, y)$ 坐标；对于机械臂，可能是关节角度的集合。
自由空间（ $CspaceC_{\text{space}}$ ）： $CspaceC_{\text{space}}$ 中不包含障碍物的部分，路径必须位于此空间内。
障碍空间（ $CobsC_{\text{obs}}$ ）： $CspaceC_{\text{space}}$ 中包含障碍物的部分。
距离度量：用于确定两个配置点之间的“距离”，通常是欧几里得距离或其他适合的度量函数。
步长（Step Size）：每次扩展树时，新节点与最近节点的距离，通常是固定值或动态调整。

RRT的目标是在$C_{\text{free}} $中找到一条从起点 $qinitq_{\text{init}}$ 到目标点 $q_{\text{goal}} $（或目标区域）的路径。

三、RRT算法详细流程

以下是RRT算法的详细步骤分解：

步骤1：初始化

创建一棵树 $T$ ，以起点 $qinitq_{\text{init}}$ 作为根节点。
定义配置空间 $C$ ，包括自由空间 $CfreeC_{\text{free}}$ 和障碍空间 $CobsC_{\text{obs}}$ 。
设置目标点 $qgoalq_{\text{goal}}$ 或目标区域，以及最大迭代次数或终止条件。

步骤2：随机采样

在配置空间 $C$ 中随机生成一个点 $qrandq_{\text{rand}}$ 。
为了提高效率，有时会以一定概率（例如 $10$ ）直接选择$ q_{\text{goal}} $作为 $q_{\text{rand}} $，以引导树向目标方向生长。这种策略称为目标偏置（Goal Bias）。

步骤3：寻找最近节点

在当前树 $T$ 中，找到距离 $qrandq_{\text{rand}}$ 最近的节点 $qnearq_{\text{near}}$ 。
距离通常通过欧几里得距离或其他度量函数计算： $qnear=arg⁡min⁡q∈Tdist(q,qrand)q_{\text{near}} = \arg\min_{q \in T} \text{dist}(q, q_{\text{rand}})$ 其中 $dist\text{dist}$ 是距离函数。

步骤4：生成新节点

从 $qnearq_{\text{near}}$ 向 $qrandq_{\text{rand}}$ 方向扩展一个新节点 $qnewq_{\text{new}}$ 。
扩展通常受限于一个固定步长 $ϵ\epsilon$ ，即：
- 如果 $dist(qnear,qrand)≤ϵ\text{dist}(q_{\text{near}}, q_{\text{rand}}) \leq \epsilon$ ，则 $qnew=qrandq_{\text{new}} = q_{\text{rand}}$ 。
- 否则， $qnew是沿qnearq_{\text{new}} 是沿 q_{\text{near}}$ 到 $qrandq_{\text{rand}}$ 方向，距离 $qnear为ϵq_{\text{near}} 为 \epsilon$ 的点： $qnew=qnear+ϵ⋅qrand−qnear∥qrand−qnear∥q_{\text{new}} = q_{\text{near}} + \epsilon \cdot \frac{q_{\text{rand}} - q_{\text{near}}}{\| q_{\text{rand}} - q_{\text{near}} \|}$

在这里插入图片描述

步骤5：碰撞检测

检查从 $qnear到qnewq_{\text{near}} 到 q_{\text{new}}$ 的路径是否位于自由空间 $CfreeC_{\text{free}}$ ，即是否与障碍物相交。
如果路径无碰撞，则将 $qnewq_{\text{new}}$ 加入树 $T$ ，并添加边 $(qnear,qnew)(q_{\text{near}}, q_{\text{new}})$ 。
如果有碰撞，则丢弃 $qnewq_{\text{new}}$ ，返回步骤2继续采样。

步骤6：检查目标

检查 $qnewq_{\text{new}}$ 是否足够接近目标点 $qgoalq_{\text{goal}}$ （例如，距离小于某个阈值），或是否进入目标区域。
如果满足条件，算法终止，并通过回溯从 $qnewq_{\text{new}}$ 到 $qinitq_{\text{init}}$ 生成路径。
如果不满足条件，返回步骤2继续迭代。

步骤7：路径生成

当找到目标后，从 $qgoalq_{\text{goal}}$ （或靠近目标的节点）回溯到 $qinitq_{\text{init}}$ ，提取路径： $Path=[qgoal,qparent,…,qinit]\text{Path} = [q_{\text{goal}}, q_{\text{parent}}, \dots, q_{\text{init}}]$

四、伪代码

以下是RRT算法的伪代码，清晰展示其逻辑：

function RRT(q_init, q_goal, C, max_iterations):
    T = Tree(q_init)  # 初始化树，根节点为起点
    for i = 1 to max_iterations:
        q_rand = RandomSample(C)  # 在配置空间随机采样
        if Random(0, 1) < goal_bias:  # 目标偏置
            q_rand = q_goal
        q_near = NearestNode(T, q_rand)  # 找到最近节点
        q_new = Extend(q_near, q_rand, step_size)  # 向随机点方向扩展
        if CollisionFree(q_near, q_new, C_free):  # 碰撞检测
            T.add_node(q_new)  # 添加新节点
            T.add_edge(q_near, q_new)  # 添加边
            if Distance(q_new, q_goal) < threshold:  # 检查是否接近目标
                return Path(T, q_new, q_init)  # 生成路径
    return None  # 未找到路径

五、python简化实现代码

import numpy as np
import matplotlib.pyplot as plt
import random

# 定义 Node 类，用于表示树中的每个节点
class Node:
    def __init__(self, x, y):
        self.x = x  # 节点的 x 坐标
        self.y = y  # 节点的 y 坐标
        self.parent = None  # 节点的父节点，用于回溯路径

# 定义 RRT 类，用于实现 RRT 算法
class RRT:
    def __init__(self, start, goal, obstacle_list, rand_area, expand_dis=0.5, goal_sample_rate=10, max_iter=500):
        """
        初始化 RRT 算法的参数
        :param start: 起点坐标 [x, y]
        :param goal: 目标点坐标 [x, y]
        :param obstacle_list: 障碍物列表，每个障碍物为 [x, y, radius]
        :param rand_area: 随机采样区域的范围 [min, max]
        :param expand_dis: 树扩展的步长，默认值为 0.5
        :param goal_sample_rate: 直接采样目标点的概率（百分比），默认值为 10
        :param max_iter: 最大迭代次数，默认值为 500
        """
        self.start = Node(start[0], start[1])  # 创建起点节点
        self.goal = Node(goal[0], goal[1])  # 创建目标节点
        self.min_rand = rand_area[0]  # 随机采样区域的最小值
        self.max_rand = rand_area[1]  # 随机采样区域的最大值
        self.expand_dis = expand_dis  # 每次扩展的步长
        self.goal_sample_rate = goal_sample_rate  # 直接采样目标点的概率
        self.max_iter = max_iter  # 最大迭代次数
        self.obstacle_list = obstacle_list  # 存储障碍物列表
        self.node_list = [self.start]  # 树节点列表，初始化时只包含起点

    def planning(self):
        """
        主规划函数，用于生成从起点到目标的路径
        :return: 如果找到路径，返回路径坐标列表；否则返回 None
        """
        for i in range(self.max_iter):  # 循环执行最大迭代次数
            # 随机采样点
            if random.randint(0, 100) > self.goal_sample_rate:
                # 以 (100 - goal_sample_rate) 的概率在随机区域内采样
                rnd = [random.uniform(self.min_rand, self.max_rand),
                       random.uniform(self.min_rand, self.max_rand)]
            else:
                # 以 goal_sample_rate 的概率直接采样目标点（目标偏置）
                rnd = [self.goal.x, self.goal.y]

            # 找到距离随机点最近的已有节点
            nind = self.get_nearest_node_index(self.node_list, rnd)
            nearest_node = self.node_list[nind]  # 获取最近节点对象

            # 计算扩展方向并生成新节点
            theta = np.arctan2(rnd[1] - nearest_node.y, rnd[0] - nearest_node.x)  # 计算与随机点的角度
            new_node = Node(nearest_node.x + self.expand_dis * np.cos(theta),  # 新节点的 x 坐标
                           nearest_node.y + self.expand_dis * np.sin(theta))   # 新节点的 y 坐标
            new_node.parent = nearest_node  # 设置新节点的父节点为最近节点

            # 检查新节点是否与障碍物碰撞
            if not self.check_collision(new_node, self.obstacle_list):
                self.node_list.append(new_node)  # 如果没有碰撞，将新节点加入树中

                # 检查新节点是否接近目标
                if self.calc_dist_to_goal(new_node.x, new_node.y) <= self.expand_dis:
                    print("到达目标")  # 输出提示信息
                    return self.generate_final_path(len(self.node_list) - 1)  # 生成并返回路径

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

    def get_nearest_node_index(self, node_list, rnd):
        """
        找到树中距离随机点最近的节点的索引
        :param node_list: 当前树中的节点列表
        :param rnd: 随机采样点的坐标 [x, y]
        :return: 最近节点的索引
        """
        dlist = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) ** 2 for node in node_list]  # 计算每个节点到随机点的平方距离
        return dlist.index(min(dlist))  # 返回最小距离对应的节点索引

    def check_collision(self, node, obstacle_list):
        """
        检查节点是否与障碍物发生碰撞
        :param node: 要检查的节点
        :param obstacle_list: 障碍物列表
        :return: True 表示有碰撞，False 表示无碰撞
        """
        for obs in obstacle_list:  # 遍历所有障碍物
            dx = obs[0] - node.x  # 计算节点与障碍物中心的 x 距离
            dy = obs[1] - node.y  # 计算节点与障碍物中心的 y 距离
            d = dx ** 2 + dy ** 2  # 计算平方距离
            if d <= obs[2] ** 2:  # 如果距离小于等于障碍物半径的平方，表示碰撞
                return True
        return False  # 遍历完所有障碍物后未发现碰撞

    def calc_dist_to_goal(self, x, y):
        """
        计算给定点到目标点的欧几里得距离
        :param x: 点的 x 坐标
        :param y: 点的 y 坐标
        :return: 到目标点的距离
        """
        return np.sqrt((x - self.goal.x) ** 2 + (y - self.goal.y) ** 2)  # 使用欧几里得距离公式

    def generate_final_path(self, goal_ind):
        """
        生成从起点到目标的最终路径
        :param goal_ind: 目标节点的索引
        :return: 路径坐标列表，从起点到目标
        """
        path = [[self.goal.x, self.goal.y]]  # 路径从目标点开始
        node = self.node_list[goal_ind]  # 获取目标节点
        while node.parent is not None:  # 回溯父节点直到起点
            path.append([node.x, node.y])  # 添加当前节点坐标
            node = node.parent  # 移动到父节点
        path.append([self.start.x, self.start.y])  # 添加起点坐标
        return path[::-1]  # 反转路径，使其从起点到目标

    def draw_graph(self):
        """
        可视化当前的树结构和障碍物
        """
        plt.clf()  # 清除当前图形
        for node in self.node_list:  # 遍历所有节点
            if node.parent:  # 如果节点有父节点
                plt.plot([node.x, node.parent.x], [node.y, node.parent.y], "-g")  # 绘制连接线

        for obs in self.obstacle_list:  # 遍历所有障碍物
            circle = plt.Circle((obs[0], obs[1]), obs[2], color='r')  # 创建圆形表示障碍物
            plt.gca().add_patch(circle)  # 添加到图形中

        plt.plot(self.start.x, self.start.y, "xr")  # 绘制起点（红色 x）
        plt.plot(self.goal.x, self.goal.y, "xb")  # 绘制目标点（蓝色 x）
        plt.axis("equal")  # 设置坐标轴比例相等
        plt.pause(0.01)  # 短暂暂停以更新图形

# 主程序入口
if __name__ == '__main__':
    # 定义参数
    start = [0, 0]  # 起点坐标
    goal = [10, 10]  # 目标坐标
    obstacle_list = [(5, 5, 1), (3, 6, 2), (7, 8, 2)]  # 障碍物列表，每个为 [x, y, 半径]
    rand_area = [0, 15]  # 随机采样区域范围

    # 创建 RRT 实例并执行路径规划
    rrt = RRT(start, goal, obstacle_list, rand_area)
    path = rrt.planning()

    # 可视化结果
    if path:
        print("找到路径")  # 输出成功信息
        plt.clf()  # 清除图形
        for node in rrt.node_list:  # 绘制树的所有边
            if node.parent:
                plt.plot([node.x, node.parent.x], [node.y, node.parent.y], "-g")
        for obs in obstacle_list:  # 绘制所有障碍物
            circle

 = plt.Circle((obs[0], obs[1]), obs,目前[2], color='r')
            plt.gca().add_patch(circle)
        plt.plot(start[0], start[1], "xr")  # 绘制起点
        plt.plot(goal[0], goal[1], "xb")  # 绘制目标点
        plt.plot([x for x, y in path], [y for x, y in path], '-b')  # 绘制最终路径（蓝色线）
        plt.axis("equal")  # 设置坐标轴比例相等
        plt.show()  # 显示图形
    else:
        print("未找到路径")  # 输出失败信息

六、c++简化实现代码

#include <iostream>
#include <vector>
#include <cmath>
#include <random>
#include <ctime>

// 定义 Node 类，用于表示树中的每个节点
class Node {
public:
    double x;  // 节点的 x 坐标
    double y;  // 节点的 y 坐标
    Node* parent;  // 父节点指针

    Node(double x, double y) : x(x), y(y), parent(nullptr) {}
};

// 定义 Obstacle 类，用于表示障碍物（圆形）
class Obstacle {
public:
    double x;  // 障碍物中心的 x 坐标
    double y;  // 障碍物中心的 y 坐标
    double radius;  // 障碍物的半径

    Obstacle(double x, double y, double radius) : x(x), y(y), radius(radius) {}
};

// 定义 RRT 类，用于实现 RRT 算法
class RRT {
public:
    Node* start;  // 起点节点
    Node* goal;   // 目标节点
    double min_rand;  // 随机采样区域的最小值
    double max_rand;  // 随机采样区域的最大值
    double expand_dis;  // 每次扩展的步长
    int goal_sample_rate;  // 直接采样目标点的概率（百分比）
    int max_iter;  // 最大迭代次数
    std::vector<Obstacle> obstacle_list;  // 障碍物列表
    std::vector<Node*> node_list;  // 树节点列表

    // 构造函数，初始化 RRT 参数
    RRT(double start_x, double start_y, double goal_x, double goal_y,
        const std::vector<Obstacle>& obstacles, double min_rand, double max_rand,
        double expand_dis = 0.5, int goal_sample_rate = 10, int max_iter = 500)
        : min_rand(min_rand), max_rand(max_rand), expand_dis(expand_dis),
          goal_sample_rate(goal_sample_rate), max_iter(max_iter), obstacle_list(obstacles) {
        start = new Node(start_x, start_y);  // 创建起点节点
        goal = new Node(goal_x, goal_y);    // 创建目标节点
        node_list.push_back(start);          // 将起点加入树中
    }

    // 析构函数，释放所有节点内存
    ~RRT() {
        for (auto node : node_list) {
            delete node;
        }
    }

    // 主路径规划函数，返回找到的路径
    std::vector<Node*> planning() {
        std::random_device rd;  // 随机数设备
        std::mt19937 gen(rd()); // 随机数生成器
        std::uniform_real_distribution<> dis(min_rand, max_rand);  // 均匀分布随机数
        std::uniform_int_distribution<> goal_dis(0, 100);           // 用于目标采样的随机数

        for (int i = 0; i < max_iter; ++i) {
            // 随机采样点
            double rnd_x, rnd_y;
            if (goal_dis(gen) > goal_sample_rate) {  // 以一定概率采样随机点
                rnd_x = dis(gen);
                rnd_y = dis(gen);
            } else {  // 直接采样目标点
                rnd_x = goal->x;
                rnd_y = goal->y;
            }

            // 找到距离随机点最近的节点
            Node* nearest_node = get_nearest_node(rnd_x, rnd_y);

            // 计算新节点位置（向随机点方向扩展一步）
            double theta = std::atan2(rnd_y - nearest_node->y, rnd_x - nearest_node->x);
            Node* new_node = new Node(nearest_node->x + expand_dis * std::cos(theta),
                                      nearest_node->y + expand_dis * std::sin(theta));
            new_node->parent = nearest_node;  // 设置父节点

            // 检查新节点是否与障碍物碰撞
            if (!check_collision(new_node)) {
                node_list.push_back(new_node);  // 无碰撞则加入树中

                // 检查是否接近目标
                if (calc_dist_to_goal(new_node->x, new_node->y) <= expand_dis) {
                    std::cout << "到达目标" << std::endl;
                    return generate_final_path(new_node);  // 返回路径
                }
            } else {
                delete new_node;  // 有碰撞则删除新节点
            }
        }
        return {};  // 未找到路径，返回空向量
    }

private:
    // 找到距离随机点最近的节点
    Node* get_nearest_node(double rnd_x, double rnd_y) {
        double min_dist = std::numeric_limits<double>::max();
        Node* nearest = nullptr;
        for (auto node : node_list) {
            double d = std::pow(node->x - rnd_x, 2) + std::pow(node->y - rnd_y, 2);
            if (d < min_dist) {
                min_dist = d;
                nearest = node;
            }
        }
        return nearest;
    }

    // 检查节点是否与障碍物碰撞
    bool check_collision(Node* node) {
        for (const auto& obs : obstacle_list) {
            double dx = obs.x - node->x;
            double dy = obs.y - node->y;
            double d = dx * dx + dy * dy;
            if (d <= obs.radius * obs.radius) {
                return true;  // 碰撞
            }
        }
        return false;  // 无碰撞
    }

    // 计算节点到目标的距离
    double calc_dist_to_goal(double x, double y) {
        return std::sqrt(std::pow(x - goal->x, 2) + std::pow(y - goal->y, 2));
    }

    // 生成最终路径（从目标回溯到起点）
    std::vector<Node*> generate_final_path(Node* goal_node) {
        std::vector<Node*> path;
        Node* current = goal_node;
        while (current != nullptr) {
            path.push_back(current);
            current = current->parent;
        }
        std::reverse(path.begin(), path.end());  // 反转路径，从起点到目标
        return path;
    }
};

// 主函数
int main() {
    // 设置参数
    double start_x = 0.0;  // 起点 x 坐标
    double start_y = 0.0;  // 起点 y 坐标
    double goal_x = 10.0;  // 目标 x 坐标
    double goal_y = 10.0;  // 目标 y 坐标
    std::vector<Obstacle> obstacles = {Obstacle(5.0, 5.0, 1.0), Obstacle(3.0, 6.0, 2.0), Obstacle(7.0, 8.0, 2.0)};
    double min_rand = 0.0;   // 随机采样区域最小值
    double max_rand = 15.0;  // 随机采样区域最大值

    // 创建 RRT 实例
    RRT rrt(start_x, start_y, goal_x, goal_y, obstacles, min_rand, max_rand);

    // 执行路径规划
    std::vector<Node*> path = rrt.planning();

    // 输出结果
    if (!path.empty()) {
        std::cout << "找到路径" << std::endl;
        for (const auto& node : path) {
            std::cout << "(" << node->x << ", " << node->y << ")" << std::endl;
        }
    } else {
        std::cout << "未找到路径" << std::endl;
    }

    return 0;
}