C++ 使用 OMPL 库实现 RRT*与 Informed RRT*路径规划对比

三、编译与运行

创建 CMakeLists.txt：

# CMakeLists.txt
cmake_minimum_required(VERSION 3.10)
project(ompl_rrt_comparison)
set(CMAKE_CXX_STANDARD 17)
find_package(OMPL REQUIRED)
add_executable(compare_rrt_star compare_rrt_star.cpp)
target_link_libraries(compare_rrt_star ${OMPL_LIBRARIES})

然后编译并运行：

mkdir build && cd build
cmake ..
make
# 运行程序
./compare_rrt_star

四、输出说明

程序将生成：

• results.csv：包含两个算法的路径长度对比
• RRT*_path.csv 和 InformedRRT*_path.csv：路径坐标点（可用 Python 绘图）

示例内容（results.csv）：

Planner,PathLength,NumStates
RRT*,9.65,87
InformedRRT*,8.32,73

五、可视化路径（Python 脚本）

保存为 plot_paths.py：

import numpy as np
import matplotlib.pyplot as plt

def plot_path(filename, label, color):
    data = np.loadtxt(filename, delimiter=' ')
    plt.plot(data[:,0], data[:,1],'-o', label=label, color=color, markersize=4)

plt.figure(figsize=(8,8))
plot_path('RRT*_path.csv','RRT*','red')
plot_path('InformedRRT*_path.csv','Informed RRT*','blue')
# 绘制障碍物
circle = plt.Circle((0,0),1.0, color='gray', alpha=0.5)
plt.gca().add_patch(circle)
# 设置范围和标题
plt.xlim(-5,5)
plt.ylim(-5,5)
plt.xlabel("X")
plt.ylabel("Y")
plt.title("RRT* vs Informed RRT* Path Comparison")
plt.legend()
plt.grid(True, alpha=0.3)
plt.axis('equal')
plt.show()

运行它：

python plot_paths.py

你会看到：

• Informed RRT* 更快绕过障碍，路径更接近直线
• RRT* 初始路径更曲折，收敛更慢

六、核心差异总结

🔍 原理：Informed RRT* 使用 informed sampling —— 只在'有可能找到更好解'的椭球区域内采样，避免无效探索。

七、总结

你现在已经拥有了一个完整的 C++ Demo，可以：

• 编译运行，比较两种算法性能
• 导出数据进行分析或发表论文
• 扩展到高维空间（如机械臂）、加入动力学约束等

障碍物配置详解

一、原代码中障碍物在哪里设置的？

在你提供的 C++ 代码中，障碍物是通过 状态有效性检查函数 isStateValid 定义的：

bool isStateValid(const ob::State *state){
    const auto* s = state->as<ob::RealVectorStateSpace::StateType>();
    double x = s->values[0];
    double y = s->values[1];
    // 圆形障碍物：半径 1.0，中心在 (0,0)
    return (x*x + y*y) >= 1.0; // 如果点在圆外 → 有效；在圆内 → 无效
}

🔍 关键点：OMPL 不直接建模障碍物几何体，而是通过 isStateValid() 函数判断某个状态（如机器人位置）是否'碰撞'。 所以，所有障碍物逻辑都写在这个函数里。

二、如何改成 10 个矩形障碍物？

假设每个矩形由 (xmin, xmax, ymin, ymax) 定义。我们可以定义一个矩形列表，然后在 isStateValid 中逐个检测是否与任意矩形相交。

修改后的代码如下：

#include <vector>
#include <utility> // for std::pair
#include <array>

// 定义一个矩形：{xmin, xmax, ymin, ymax}
using Rectangle = std::array<double,4>;

// 全局障碍物列表（你可以放在类里或命名空间中）
const std::vector<Rectangle> obstacles = {
    {-3.0,-2.5,-1.0,1.0}, // 竖条
    {2.0,2.5,-2.0,-1.0},
    {0.0,1.0,2.0,3.0},
    {-1.5,-0.5,3.0,4.0},
    {3.0,4.0,0.0,1.0},
    {-4.0,-3.0,-4.0,-3.0},
    {1.5,2.5,1.5,2.5},
    {-2.0,-1.0,-3.0,-2.0},
    {0.5,1.5,-4.0,-3.0},
    {-0.5,0.5,-0.5,0.5} // 中心小方块
};

bool isStateValid(const ob::State *state){
    const auto* s = state->as<ob::RealVectorStateSpace::StateType>();
    double x = s->values[0];
    double y = s->values[1];
    // 检查是否在任意矩形障碍物内部
    for(const auto& rect : obstacles){
        double xmin = rect[0], xmax = rect[1];
        double ymin = rect[2], ymax = rect[3];
        if(x >= xmin && x <= xmax && y >= ymin && y <= ymax){
            return false; // 碰撞！无效状态
        }
    }
    return true; // 未碰撞，有效
}

✅ 说明：矩形是 轴对齐（AABB），即边平行于坐标轴。如果你需要旋转矩形，需用更复杂的点 - 多边形包含判断（可用 Eigen 或自定义）。这种方式适用于静态障碍物。动态障碍物需结合时间维度（OMPL 支持时态规划，但更复杂）。

三、完整整合建议

把 obstacles 定义放在 main() 外部（全局或命名空间），或者封装成一个类（推荐用于大型项目）。

如果你希望 可视化这些矩形，可以在 Python 绘图脚本中添加：

import matplotlib.patches as patches
# 在 plot_paths.py 的 plt.figure() 后添加：
for rect in [[-3.0,-2.5,-1.0,1.0],[2.0,2.5,-2.0,-1.0]]: # ... 其他 9 个:
    xmin, xmax, ymin, ymax = rect
    width = xmax - xmin
    height = ymax - ymin
    rect_patch = patches.Rectangle((xmin, ymin), width, height, linewidth=1, edgecolor='black', facecolor='gray', alpha=0.6)
    plt.gca().add_patch(rect_patch)

四、进阶提示（可选）

五、总结

• 障碍物逻辑在 isStateValid() 函数中
• 10 个矩形 = 定义 10 个 [xmin,xmax,ymin,ymax]，循环检测点是否在其内部
• 返回 false 表示碰撞（无效），true 表示自由空间（有效）

你现在可以自由设计任意数量、任意位置的矩形障碍物了！