OMPL 库在 C++ 中的路径规划应用，演示了 RRT*与 Informed RRT*算法的对比。通过定义状态有效性检查函数处理圆形及矩形障碍物，提供完整的编译运行流程及 Python 可视化脚本。文章涵盖安装步骤、代码实现、参数配置及结果分析，帮助开发者快速掌握基于采样的高级运动规划方法及其优化策略。
我们来用 C++ 和 OMPL 库 实现一个完整的 Informed RRT* vs RRT* 对比 Demo。
这个程序会：
RRT* 和 Informed RRT*# Ubuntu/Debian
sudo apt-get install libompl-dev
# 或从源码编译（推荐，支持更多功能）：
git clone https://github.com/ompl/ompl.git
cd ompl && mkdir build && cd build
cmake ..
make -j4
sudo make install
compare_rrt_star.cpp）// compare_rrt_star.cpp
#include <ompl/base/SpaceInformation.h>
#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/geometric/planners/rrt/RRTstar.h>
#include <ompl/geometric/planners/rrt/InformedRRTstar.h>
#include <ompl/geometric/SimpleSetup.h>
#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
#include <iostream>
#include <fstream>
#include <vector>
namespace ob = ompl::base;
namespace og = ompl::geometric;
// 状态有效性检查：避开原点附近的圆形障碍
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
return (x*x + y*y) >= 1.0;
}
// 运行指定的规划器，并记录优化过程
void planWithPlanner(const std::string& plannerName, const ob::PlannerPtr& planner, og::SimpleSetup& ss, std::ofstream& logFile){
std::cout << "\n=== Running " << plannerName << " ===" << std::endl;
// 设置优化目标：最小化路径长度
auto obj(std::make_shared<ob::PathLengthOptimizationObjective>(ss.getSpaceInformation()));
ss.setOptimizationObjective(obj);
ss.setPlanner(planner);
// 设置起始和目标状态
ob::ScopedState<>start(ss.getStateSpace());
ob::ScopedState<>goal(ss.getStateSpace());
start->as<ob::RealVectorStateSpace::StateType>()->values[0]=-4.0;
start->as<ob::RealVectorStateSpace::StateType>()->values[1]=-4.0;
goal->as<ob::RealVectorStateSpace::StateType>()->values[0]=4.0;
goal->as<ob::RealVectorStateSpace::StateType>()->values[1]=4.0;
ss.setStartAndGoalStates(start, goal);
// 尝试求解（最多 10 秒或 10000 次采样）
ob::PlannerStatus solved = ss.solve(10.0);
if(solved){
std::cout << plannerName << ": Found solution!" << std::endl;
// 简化路径
ss.simplifySolution();
const auto&path = ss.getSolutionPath();
double length = path.length();
double smoothness = path.smoothness();
std::cout << plannerName << " Final Path Length: " << length << std::endl;
// 记录到日志文件
logFile << plannerName << "," << length << "," << path.getStateCount() << std::endl;
// 可选：导出路径点用于绘图
std::ofstream pf((plannerName + "_path.csv").c_str());
path.printAsMatrix(pf);
pf.close();
}else{
std::cout << plannerName << ": No solution found." << std::endl;
logFile << plannerName << ",inf,0" << std::endl;
}
}
int main(){
// 创建 2D 状态空间
auto space(std::make_shared<ob::RealVectorStateSpace>(2));
space->setBounds(-5.0,5.0); // x, y 范围
// 创建简单设置
og::SimpleSetup ss(space);
ss.setStateValidityChecker(isStateValid);
// 设置空间信息
ss.getSpaceInformation()->setStateValidityCheckingResolution(0.01);
// 日志文件记录结果
std::ofstream logFile("results.csv");
logFile << "Planner,PathLength,NumStates\n";
// === 1. RRT* ===
auto rrtstar = std::make_shared<og::RRTstar>(ss.getSpaceInformation());
rrtstar->setRange(0.5); // 步长
rrtstar->setGoalBias(0.05); // 目标偏置
rrtstar->setRewireFactor(1.2); // 启用重布线
planWithPlanner("RRT*", rrtstar, ss, logFile);
// 清除上一次的结果
ss.clear();
// === 2. Informed RRT* ===
auto informed_rrtstar = std::make_shared<og::InformedRRTstar>(ss.getSpaceInformation());
informed_rrtstar->setRange(0.5);
informed_rrtstar->setGoalBias(0.05);
informed_rrtstar->setRewireFactor(1.2);
planWithPlanner("InformedRRT*", informed_rrtstar, ss, logFile);
logFile.close();
std::cout << "\n✅ Results saved to 'results.csv' and path files." << std::endl;
return 0;
}
创建 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
保存为 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
你会看到：
| 特性 | RRT* | Informed 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()函数判断某个状态（如机器人位置）是否'碰撞'。 所以，所有障碍物逻辑都写在这个函数里。
假设每个矩形由 (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)
| 需求 | 建议 |
|---|---|
| 障碍物很多（>100） | 用 空间加速结构（如 KD-Tree、Grid Hash）避免 O(n) 检测 |
| 任意多边形障碍 | 将障碍物表示为顶点列表，使用 射线法 判断点是否在多边形内 |
| 机器人有尺寸 | 使用 Minkowski 和膨胀障碍物，或将机器人视为点 + 膨胀障碍 |
| 动态障碍 | 引入时间维度，状态变为 (x, y, t)，障碍物随时间移动 |
isStateValid() 函数中[xmin,xmax,ymin,ymax]，循环检测点是否在其内部false 表示碰撞（无效），true 表示自由空间（有效）你现在可以自由设计任意数量、任意位置的矩形障碍物了！
微信公众号「极客日志」，在微信中扫描左侧二维码关注。展示文案：极客日志 zeeklog
使用加密算法（如AES、TripleDES、Rabbit或RC4）加密和解密文本明文。 在线工具，加密/解密文本在线工具，online
生成新的随机RSA私钥和公钥pem证书。 在线工具，RSA密钥对生成器在线工具，online
基于 Mermaid.js 实时预览流程图、时序图等图表，支持源码编辑与即时渲染。 在线工具，Mermaid 预览与可视化编辑在线工具，online
将字符串编码和解码为其 Base64 格式表示形式即可。 在线工具，Base64 字符串编码/解码在线工具，online
将字符串、文件或图像转换为其 Base64 表示形式。 在线工具，Base64 文件转换器在线工具，online
将 Markdown（GFM）转为 HTML 片段，浏览器内 marked 解析；与 HTML转Markdown 互为补充。 在线工具，Markdown转HTML在线工具，online