独立纯 C++ 实现的 Regulated Pure Pursuit（调节纯追踪）路径跟踪控制器。本项目从 ROS nav2 的 regulated_pure_pursuit_controller 完全解耦重构而来，移除了全部 ROS 依赖，提供标准化 C++ 接口，可直接嵌入任何移动机器人控制系统。
Pure Pursuit（纯追踪）是移动机器人路径跟踪中最经典的几何控制方法之一。其核心思想是在参考路径上选取一个前瞻点（lookahead point），通过计算从机器人当前位置到前瞻点的圆弧来生成转向指令。原始 Pure Pursuit 算法结构简单、易于实现，但在以下场景中存在不足：
- 曲率突变：路径存在急转弯时，机器人可能因速度过高而偏离轨迹
- 障碍物接近：在障碍物附近缺乏减速机制，存在碰撞风险
- 接近目标：到达终点时无减速策略，容易过冲
- 方向偏差大：机器人朝向与路径方向偏差过大时，直接跟踪会震荡
Regulated Pure Pursuit（RPP）通过引入多种速度调节策略解决上述问题，使纯追踪控制器能够安全、平滑地跟踪路径。
本项目将 ROS nav2 中的 regulated_pure_pursuit_controller 插件重构为独立的纯 C++ 库，目标包括：
- 完全去 ROS 化：移除 ROS 节点、话题、服务、参数服务器、TF、pluginlib 等全部依赖
- 工业级代码标准：清晰的接口抽象、线程安全设计、异常处理机制、完整单元测试
- 算法保真：严格保留原始 RPP 算法的核心数学模型与计算流程
- 易于集成：提供标准 C++ 接口，可嵌入任何 C++ 项目
Pure Pursuit 算法将路径跟踪问题转化为几何问题。给定：
- 机器人当前位姿
$(x_r, y_r, \theta_r)$ - 参考路径上距机器人
$L_d$ （前瞻距离）的前瞻点$(x_c, y_c)$
算法将前瞻点变换到机器人坐标系后，计算从机器人到前瞻点的圆弧曲率
前瞻点选取：在路径上找到第一个与机器人距离不小于
RPP 在 Pure Pursuit 基础上增加了多种速度调节策略：
根据当前路径曲率限制线速度。曲率越大（转弯越急），线速度越低：
其中
根据机器人当前位置的代价地图代价值降低速度。代价越高（越接近障碍物），速度越低：
其中
当机器人接近路径终点时，按剩余距离比例衰减速度：
且不低于最小接近速度
当机器人朝向与前瞻点方向偏差超过阈值
若
DWPP 模式结合了动态窗口法（DWA）的速度约束与 Pure Pursuit 的曲率引导：
- 计算动态窗口：根据当前速度和加速度限制，计算一个控制周期内可达的速度范围
- 应用调节约束：将 RPP 调节后的线速度作为动态窗口的边界约束
-
求解最优速度：在动态窗口内，找到最接近
$\omega = \kappa \cdot v$ 关系的速度组合
前瞻点
角速度：
全局坐标
到达目标位置附近后，旋转到目标航向角：
并限制减速到目标角度所需的最大速度：
按控制周期步进模拟机器人运动轨迹，检测足迹是否与代价地图障碍物碰撞：
RPP_controller/
├── CMakeLists.txt # 顶层 CMake 构建配置
├── LICENSE # MIT 许可证
├── README.md # 项目文档
├── .gitignore
├── cmake/
│ └── rpp_controllerConfig.cmake.in # CMake 包导出模板
├── include/rpp_controller/
│ ├── types.hpp # 基础数据类型（Point2D, Pose2D, Twist2D 等）
│ ├── exceptions.hpp # 异常类（ControllerException, NoValidControl 等）
│ ├── controller_config.hpp # 控制器配置结构体与校验
│ ├── costmap_interface.hpp # 代价地图抽象接口
│ ├── goal_checker_interface.hpp # 目标检查抽象接口 + SimpleGoalChecker
│ ├── geometry_utils.hpp # 几何工具函数声明
│ ├── regulation_functions.hpp # 速度调节函数（曲率/代价/接近速度约束）
│ ├── dynamic_window_pure_pursuit_functions.hpp # DWPP 算法
│ ├── collision_checker.hpp # 碰撞检测器
│ ├── regulated_pure_pursuit_controller.hpp # 主控制器声明
│ └── simple_costmap.hpp # 测试用代价地图实现
├── src/
│ ├── regulated_pure_pursuit_controller.cpp # 主控制器实现
│ ├── collision_checker.cpp # 碰撞检测器实现
│ └── geometry_utils.cpp # 几何工具实现
├── examples/
│ └── example_usage.cpp # 使用示例程序
└── test/
├── CMakeLists.txt # 测试构建配置
├── path_utils/
│ ├── path_utils.hpp # 路径生成工具
│ └── path_utils.cpp
├── test_regulated_pp.cpp # 主控制器测试（13 个用例）
├── test_dwpp_functions.cpp # DWPP 算法测试
├── test_regulation_functions.cpp # 调节函数测试
├── test_geometry_utils.cpp # 几何工具测试
└── test_path_utils.cpp # 路径工具测试
- C++ 编译器：支持 C++17（GCC 7+、Clang 5+、MSVC 2017+）
- CMake：3.10 或更高版本
- Google Test：1.8 或更高版本（仅测试需要）
- 线程库：POSIX Threads
Ubuntu 安装依赖：
sudo apt update
sudo apt install build-essential cmake libgtest-devcd RPP_controller
mkdir build && cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j$(nproc)cd build
ctest --output-on-failure预期输出：
100% tests passed, 0 tests failed out of 40
cd build
sudo make install安装后可通过 CMake find_package(rpp_controller) 在其他项目中引用。
| 选项 | 默认值 | 说明 |
|---|---|---|
BUILD_TESTING |
ON |
构建单元测试 |
BUILD_EXAMPLES |
ON |
构建示例程序 |
CMAKE_BUILD_TYPE |
Release |
构建类型 |
禁用测试和示例的精简构建：
cmake .. -DBUILD_TESTING=OFF -DBUILD_EXAMPLES=OFF#include "rpp_controller/regulated_pure_pursuit_controller.hpp"
#include "rpp_controller/simple_costmap.hpp"
#include "rpp_controller/goal_checker_interface.hpp"
using namespace rpp_controller;
int main()
{
// 1. 创建代价地图（实现 CostmapInterface）
SimpleCostmap costmap(20.0, 20.0, 0.05, 0.15, 0.25);
// 2. 配置控制器参数
ControllerConfig config;
config.max_linear_vel = 0.5;
config.lookahead_dist = 0.6;
config.use_collision_detection = true;
config.use_regulated_linear_velocity_scaling = true;
config.use_rotate_to_heading = true;
// 3. 初始化控制器
RegulatedPurePursuitController controller;
controller.configure(config, &costmap, 20.0);
// 4. 准备路径与目标
Path global_plan = /* ... */;
PoseStamped global_goal = global_plan.poses.back();
SimpleGoalChecker goal_checker(0.15);
// 5. 控制循环
PoseStamped robot_pose = /* ... */;
Twist2D current_velocity{0.0, 0.0, 0.0};
VelocityCommand cmd = controller.computeVelocityCommands(
robot_pose, current_velocity, &goal_checker,
global_plan, global_goal);
// cmd.linear_x, cmd.angular_z 即为输出指令
return 0;
}运行自带示例：
./build/example_usage| 参数 | 类型 | 默认值 | 说明 |
|---|---|---|---|
max_linear_vel |
double | 0.5 | 最大线速度 (m/s) |
min_linear_vel |
double | -0.5 | 最小线速度（负值允许倒车）(m/s) |
max_angular_vel |
double | 2.5 | 最大角速度 (rad/s) |
min_angular_vel |
double | -2.5 | 最小角速度 (rad/s) |
max_linear_accel |
double | 2.5 | 最大线加速度 (m/s²) |
max_linear_decel |
double | -2.5 | 最大线减速度 (m/s²) |
max_angular_accel |
double | 3.2 | 最大角加速度 (rad/s²) |
max_angular_decel |
double | -3.2 | 最大角减速度 (rad/s²) |
lookahead_dist |
double | 0.6 | 前瞻距离 (m) |
min_lookahead_dist |
double | 0.3 | 速度缩放模式最小前瞻距离 (m) |
max_lookahead_dist |
double | 0.9 | 速度缩放模式最大前瞻距离 (m) |
lookahead_time |
double | 1.5 | 速度缩放前瞻时间 (s) |
use_velocity_scaled_lookahead_dist |
bool | false | 启用速度缩放前瞻距离 |
use_rotate_to_heading |
bool | true | 启用朝向旋转 |
rotate_to_heading_angular_vel |
double | 1.8 | 朝向旋转角速度 (rad/s) |
rotate_to_heading_min_angle |
double | 0.785 | 触发朝向旋转的最小角度 (rad) |
min_approach_linear_velocity |
double | 0.05 | 最小接近速度 (m/s) |
approach_velocity_scaling_dist |
double | 0.6 | 接近速度缩放距离 (m) |
use_collision_detection |
bool | true | 启用碰撞检测 |
max_allowed_time_to_collision_up_to_carrot |
double | 1.0 | 碰撞检测最大前向时间 (s) |
use_regulated_linear_velocity_scaling |
bool | true | 启用曲率速度调节 |
use_cost_regulated_linear_velocity_scaling |
bool | true | 启用代价速度调节 |
regulated_linear_scaling_min_radius |
double | 0.9 | 曲率调节最小半径 (m) |
regulated_linear_scaling_min_speed |
double | 0.25 | 调节后最小线速度 (m/s) |
cost_scaling_dist |
double | 0.6 | 代价调节距离 (m) |
cost_scaling_gain |
double | 1.0 | 代价调节增益 |
inflation_cost_scaling_factor |
double | 3.0 | 膨胀代价缩放因子 |
use_dynamic_window |
bool | false | 启用 DWPP 模式 |
allow_reversing |
bool | false | 允许倒车（与 rotate_to_heading 互斥） |
代价地图抽象接口，用户需实现此接口以接入自有代价地图：
class CostmapInterface {
public:
virtual bool worldToMap(double wx, double wy,
unsigned int & mx, unsigned int & my) const = 0;
virtual unsigned char getCost(unsigned int mx, unsigned int my) const = 0;
virtual double getResolution() const = 0;
virtual double getSizeInMetersX() const = 0;
virtual double getSizeInMetersY() const = 0;
virtual double getInscribedRadius() const = 0;
virtual double getCircumscribedRadius() const = 0;
virtual bool isTrackingUnknown() const = 0;
virtual const std::vector<Point2D> & getRobotFootprint() const = 0;
virtual std::string getGlobalFrameID() const = 0;
virtual std::string getBaseFrameID() const = 0;
virtual double footprintCostAtPose(double x, double y, double theta,
const std::vector<Point2D> & footprint) const = 0;
};SimpleCostmap 提供了一个完整的参考实现，可直接用于测试或简单场景。
目标检查接口，用于判断机器人是否到达目标位置：
class GoalCheckerInterface {
public:
virtual bool isGoalXYReached(const Pose2D & robot_pose,
const Pose2D & goal_pose,
const Twist2D & speed,
const Path & transformed_plan) const = 0;
};SimpleGoalChecker 提供基于 XY 距离的默认实现。
class RegulatedPurePursuitController {
public:
void configure(const ControllerConfig & config,
CostmapInterface * costmap,
double control_frequency = 20.0);
VelocityCommand computeVelocityCommands(
const PoseStamped & robot_pose,
const Twist2D & current_velocity,
GoalCheckerInterface * goal_checker,
const Path & global_plan,
const PoseStamped & global_goal);
void setSpeedLimit(double speed_limit, bool percentage);
bool cancel();
void reset();
};本项目采用 MIT 许可证。
算法核心逻辑源自 ROS 2 Navigation Project 的 regulated_pure_pursuit_controller（Apache License 2.0）。