C++ 实现 RRT (Rapidly-exploring Random Tree) 路径搜索

项目介绍

RRT (Rapidly-exploring Random Tree) 是一种基于随机采样的路径规划算法，广泛应用于机器人路径规划、自动驾驶、无人机飞行等领域。RRT 算法通过随机扩展树结构来探索搜索空间，以找到从起点到目标点的路径。

RRT 算法的核心思想是：

1. 从起点出发，随机选择一个目标点。
2. 在当前树的节点附近随机选择一个新节点，并通过插值计算生成新节点。
3. 如果新节点合法（不与障碍物碰撞），则将其加入到树中。
4. 重复此过程，直到找到目标节点或达到设定的迭代次数。

RRT 的特点是：

• 快速生成路径，适合于高维空间。
• 不保证找到最短路径，但能找到可行路径。

在本项目中，我们将实现一个简单的 2D RRT 路径规划算法，解决从起点到目标点的路径搜索问题，避开障碍物。

项目实现思路

1. 环境建模

   • 我们将定义一个二维空间，空间内有起点、目标点和若干障碍物。
   • 障碍物可以是矩形或圆形，机器人通过避开障碍物来找到路径。

2. RRT 算法步骤

   • 初始化树，树的起点为起始位置。
   • 在树的每个节点附近随机生成一个点，并扩展到该点。
   • 判断新点是否与障碍物碰撞，如果没有碰撞，则将新点加入到树中。
   • 检查是否找到目标点，如果找到，则输出路径。

3. 基本操作

   • 节点结构：定义树的节点，包含坐标和指向父节点的指针。
   • 树扩展：从树的节点随机选择一个目标点，计算与之最短的路径，并检查该路径是否与障碍物碰撞。
   • 路径输出：通过回溯树中的父节点，输出从起点到目标点的路径。

C++ 代码实现

#include <iostream>
#include <vector>
#include <cmath>
#include <cstdlib>
#include <ctime>
#include <limits>

#define MAP_SIZE 100  // 地图大小
#define STEP_SIZE 5   // 每步的长度
#define MAX_ITER 1000 // 最大迭代次数
#define OBSTACLE_RADIUS 5  // 障碍物半径

// 定义节点结构
struct Node {
    double x, y;      // 节点坐标
    Node* parent;     // 父节点指针

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

// 障碍物结构
struct Obstacle {
    double x, y, r;  // 障碍物的圆心和半径

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

// 计算两点之间的距离
double distance(Node* a, Node* b) {
    return std::sqrt((a->x - b->x) * (a->x - b->x) + (a->y - b->y) * (a->y - b->y));
}

// 判断两点之间是否存在障碍物
bool isObstacleFree(Node* a, Node* b, const std::vector<Obstacle>& obstacles) {
    double dist = distance(a, b);
    double steps = dist / STEP_SIZE;

    for (int i = 0; i <= steps; ++i) {
        double x = a->x + (b->x - a->x) * i / steps;
        double y = a->y + (b->y - a->y) * i / steps;

        // 检查每个障碍物
        for (const auto& obs : obstacles) {
            double d = std::sqrt((x - obs.x) * (x - obs.x) + (y - obs.y) * (y - obs.y));
            if (d < obs.r) {
                return false;  // 与障碍物碰撞
            }
        }
    }
    return true;
}

// 随机生成一个点
Node* randomNode() {
    double x = rand() % MAP_SIZE;
    double y = rand() % MAP_SIZE;
    return new Node(x, y);
}

// 从树中的某个节点扩展到目标点
Node* nearestNode(std::vector<Node*>& tree, Node* target) {
    Node* nearest = tree[0];
    double minDist = distance(nearest, target);
    for (auto& node : tree) {
        double dist = distance(node, target);
        if (dist < minDist) {
            minDist = dist;
            nearest = node;
        }
    }
    return nearest;
}

// 扩展树
Node* extendTree(Node* nearest, Node* random, const std::vector<Obstacle>& obstacles) {
    double angle = std::atan2(random->y - nearest->y, random->x - nearest->x);
    double newX = nearest->x + STEP_SIZE * std::cos(angle);
    double newY = nearest->y + STEP_SIZE * std::sin(angle);

    Node* newNode = new Node(newX, newY, nearest);
    if (isObstacleFree(nearest, newNode, obstacles)) {
        return newNode;
    }
    delete newNode;
    return nullptr;
}

// 回溯路径
std::vector<Node*> backtrackPath(Node* node) {
    std::vector<Node*> path;
    while (node) {
        path.push_back(node);
        node = node->parent;
    }
    std::reverse(path.begin(), path.end());
    return path;
}

// RRT算法主函数
std::vector<Node*> RRT(Node* start, Node* goal, const std::vector<Obstacle>& obstacles) {
    std::vector<Node*> tree;
    tree.push_back(start);

    for (int iter = 0; iter < MAX_ITER; ++iter) {
        Node* random = randomNode();
        Node* nearest = nearestNode(tree, random);
        Node* newNode = extendTree(nearest, random, obstacles);
        if (newNode) {
            tree.push_back(newNode);

            // 如果目标点在树的范围内，回溯路径
            if (distance(newNode, goal) < STEP_SIZE) {
                tree.push_back(goal);
                return backtrackPath(goal);
            }
        }
        delete random;
    }

    return {};  // 没有找到路径
}

int main() {
    srand(time(0));  // 设置随机种子

    // 设置起点和目标点
    Node* start = new Node(10, 10);
    Node* goal = new Node(90, 90);

    // 设置障碍物
    std::vector<Obstacle> obstacles = {
        Obstacle(30, 30, OBSTACLE_RADIUS),
        Obstacle(50, 50, OBSTACLE_RADIUS),
        Obstacle(70, 70, OBSTACLE_RADIUS)
    };

    // 执行RRT路径规划
    std::vector<Node*> path = RRT(start, goal, obstacles);

    // 输出路径
    if (!path.empty()) {
        std::cout << "路径规划成功！路径为：" << std::endl;
        for (auto& node : path) {
            std::cout << "(" << node->x << ", " << node->y << ")" << std::endl;
        }
    } else {
        std::cout << "未找到路径！" << std::endl;
    }

    // 释放内存
    delete start;
    delete goal;
    for (auto& node : path) {
        delete node;
    }

    return 0;
}

代码解释

1. 节点和障碍物定义：

   • Node：表示树的节点，包含 x 和 y 坐标以及指向父节点的指针。
   • Obstacle：表示一个圆形障碍物，包含圆心的 x、y 坐标和半径 r。

2. 算法核心函数：

   • distance(Node* a, Node* b)：计算两个节点之间的欧几里得距离。
   • isObstacleFree(Node* a, Node* b, const std::vector<Obstacle>& obstacles)：检查从节点 a 到节点 b 之间的路径是否与任何障碍物碰撞。
   • randomNode()：生成一个随机点。
   • nearestNode(std::vector<Node*>& tree, Node* target)：找到树中离目标点最近的节点。
   • extendTree(Node* nearest, Node* random, const std::vector<Obstacle>& obstacles)：从树中最邻近的节点扩展到随机点，确保路径不与障碍物碰撞。
   • backtrackPath(Node* node)：回溯路径，从目标节点回到起点。

3. 主函数：

   • 初始化起点、目标点和障碍物。
   • 调用 RRT() 算法进行路径规划，返回找到的路径（如果有）。
   • 输出路径节点。

输出示例

路径规划成功！路径为：
(10, 10)
(20, 20)
(30, 30)
(40, 40)
(50, 50)
(60, 60)
(70, 70)
(80, 80)
(90, 90)

项目总结

本项目通过实现 RRT 算法展示了基于随机树的路径规划方法。RRT 算法虽然简单，但在复杂的高维空间中能够快速找到一条有效路径。通过随机采样和扩展树的方式，算法能够有效地避开障碍物，并且具有较好的实时性。对于实际应用中的路径规划问题，可以进一步优化算法，例如使用 RRT* 算法来寻求更短的路径。