PurePath Controller（PP_controller）路径跟踪控制器

PurePath Controller（PP_controller）路径跟踪控制器

链接：PP_controller控制器
欢迎大家交流学习！

一个基于Lyapunov稳定性原理的平滑控制律（Smooth Control Law）的纯净C++机器人局部路径跟踪控制器，零ROS依赖，可移植到任何嵌入式平台。

目录

• 项目特点
• 原理介绍
• 功能特性
• 系统架构
• 文件结构
• 安装与配置
• 使用方法
• 参数说明
• 未来改进方向
• 许可证

项目特点

纯净C++实现

本项目核心控制器采用纯净C++17实现，无任何ROS依赖：

• graceful_controller_standalone.hpp- 头文件，包含所有接口定义
• graceful_controller_standalone.cpp- 实现文件，零外部依赖

控制器仅依赖标准库（<vector>,<cmath>,<memory>等），可轻松移植到：

• 嵌入式Linux系统
• STM32 / ESP32 等微控制器
• 裸机环境（Bare Metal）
• 其他机器人中间件（ROS1, ROS2, Cyber RT等）

ROS2仅用于可视化测试

ros2_ws/目录下的ROS2包装器仅用于：

• 在Gazebo/RViz中可视化测试控制器效果
• 提供激光雷达数据输入示例
• 演示如何将控制器集成到ROS2系统

##生产环境使用时，可直接移植核心库到目标平台。

原理介绍

平滑控制律 (Smooth Control Law)

本控制器基于 Kanayama 等人提出的平滑控制律，通过将目标点转换到机器人坐标系下的极坐标表示，计算曲率和速度指令，并对其进行了优化。

核心数学公式

1. 极坐标转换 (Egocentric Polar Coordinates)

将目标点从世界坐标系转换到机器人坐标系：

r = √(dx² + dy²) // 距离 φ = θ_target - atan2(dy, dx) // 目标方向与视线夹角 δ = θ_current - atan2(dy, dx) // 当前方向与视线夹角

2. 曲率计算 (Curvature Calculation)

κ = -[k_δ(δ - atan(-k_φ·φ)) + (1 + k_φ/(1+(k_φ·φ)²))·sin(δ)] / r

其中：

• k_φ: 角度误差增益
• k_δ: 方向误差增益
• r: 机器人到目标点的距离
• φ: 目标方向与视线方向的夹角
• δ: 当前方向与视线方向的夹角

3. 速度调节 (Velocity Scaling)

根据曲率动态调整线速度，实现平滑运动：

v = v_max / (1 + β·|κ|^λ)

其中：

• β: 曲率影响系数
• λ: 曲率指数因子

4. 接近减速 (Slowdown Near Goal)

当接近目标点时自动减速：

if r < slowdown_radius: v = v_max × (r / slowdown_radius)

前视距离 (Lookahead Distance)

控制器使用动态前视距离来选择路径上的跟踪目标点：

lookahead = clamp(dist_to_goal, min_lookahead, max_lookahead)

• 最小前视距离: 保证控制稳定性
• 最大前视距离: 限制跟踪精度

功能特性

核心功能

• [1]平滑路径跟踪: 基于曲率的速度调节，实现平滑运动
• [2]动态前视距离: 自适应调整跟踪目标点
• [3]实时碰撞检测: 基于激光雷达的障碍物检测与避障
• [4]初始旋转对齐: 运动前自动调整朝向
• [5]终点精确定位: 到达目标点后精确调整姿态
• [6]速度限制管理: 支持动态速度限制设置
• [7]轨迹仿真: 控制前进行轨迹碰撞检测
• [8]零ROS依赖: 核心库仅使用C++标准库

碰撞检测

classLaserCollisionChecker:publicICollisionChecker{// 基于激光雷达的碰撞检测// 支持机器人 footprint 碰撞检测// 可配置安全距离和检测半径};

支持的机器人模型

• 差速驱动机器人 (Differential Drive)
• 支持自定义机器人 footprint

系统架构

┌─────────────────────────────────────────────────────────────┐ │ Graceful Controller │ ├─────────────────────────────────────────────────────────────┤ │ ┌─────────────────┐ ┌─────────────────┐ ┌─────────────┐ │ │ │ SmoothControl │ │ ParameterHandler│ │ ICollision │ │ │ │ Law │ │ │ │ Checker │ │ │ └────────┬────────┘ └─────────────────┘ └──────┬──────┘ │ │ │ │ │ │ ▼ ▼ │ │ ┌─────────────────────────────────────────────────────┐ │ │ │ GracefulController │ │ │ │ - 路径处理 - 速度计算 - 碰撞检测 - 轨迹仿真 │ │ │ └─────────────────────────────────────────────────────┘ │ │ │ │ └────────────────────────────┼────────────────────────────────┘ │ ┌──────────────┴──────────────┐ ▼ ▼ ┌─────────────────────┐ ┌─────────────────────┐ │ Standalone Lib │ │ ROS2 Wrapper │ │ (C++ Library) │ │ (Visualization) │ │ Zero Dependencies │ │ Optional │ └─────────────────────┘ └─────────────────────┘

类结构

文件结构

Graceful_controller_c++/ ├── graceful_controller_standalone.hpp # 核心头文件（无ROS依赖） ├── graceful_controller_standalone.cpp # 核心实现（无ROS依赖） ├── CMakeLists.txt # 独立库编译配置 ├── config/ │ ├── controller_params.yaml # 参数配置文件 │ └── graceful_test.rviz # RViz配置文件 └── ros2_ws/ # ROS2可视化测试（可选） └── src/graceful_controller_ros2/ ├── src/graceful_controller_ros2.cpp ├── launch/controller.launch.py └── config/controller_params.yaml

安装与配置

环境要求

核心库（无ROS依赖）:

• C++17 兼容编译器 (GCC 7+, Clang 5+, MSVC 2017+)
• CMake 3.10+
• 仅依赖C++标准库

ROS2可视化测试（可选）:

• Ubuntu 20.04+ / Windows 10+
• ROS2 Foxy / Humble / Rolling

独立库编译（推荐）

# 克隆仓库gitclone https://github.com/Robot-Nav/A-PurePath-Controller.gitcdA-PurePath-Controller# 创建构建目录mkdirbuild&&cdbuild# 编译cmake..make# 安装（可选）sudomakeinstall

集成到你的项目

方法1: 直接包含源文件

add_executable(your_robot_node your_main.cpp graceful_controller_standalone.cpp # 直接包含 ) target_include_directories(your_robot_node PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} )

方法2: 作为静态库

add_subdirectory(graceful_controller) target_link_libraries(your_robot_node graceful_controller_standalone )

ROS2可视化测试（可选）

cdros2_ws colcon build --symlink-installsourceinstall/setup.bash# 启动Gazebo仿真exportTURTLEBOT3_MODEL=burger ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py# 启动控制器ros2 launch graceful_controller_ros2 controller.launch.py# 发布测试路径ros2 run graceful_controller_ros2 publish_offline_path.py

使用方法

详见readme步骤

参数说明

控制参数

速度参数

行为参数

碰撞检测参数

机器人 footprint

// C++代码中设置Footprint footprint={{0.3,0.3},// 右前{-0.3,0.3},// 左前{-0.3,-0.3},// 左后{0.3,-0.3}// 右后};controller.setRobotFootprint(footprint);

未来改进方向

1. 障碍物跟随功能 (Obstacle Following)

目标: 实现沿墙行走或障碍物边界跟踪功能

实现思路:

classObstacleFollower{public:// 检测障碍物边界booldetectObstacleBoundary(constLaserScan&scan);// 计算沿墙速度指令TwistcomputeFollowVelocity(constPose2D&current_pose,constLaserScan&scan,doublefollow_distance,FollowSide side);// LEFT or RIGHT// 切换跟随方向voidswitchFollowSide(FollowSide side);private:// 提取障碍物边界点std::vector<Point2D>extractBoundaryPoints(constLaserScan&scan);// 计算与障碍物的最近距离doublecomputeDistanceToObstacle(constLaserScan&scan);};

应用场景:

• 狭窄通道通过
• 未知环境探索
• 紧急避障后的路径恢复

2. 动态障碍物预测

目标: 预测移动障碍物的轨迹，提前规划避障路径

实现思路:

• 使用卡尔曼滤波跟踪动态障碍物
• 预测障碍物未来位置
• 在轨迹仿真中考虑动态障碍物

3. 自适应参数调整

目标: 根据环境复杂度动态调整控制参数

实现思路:

classAdaptiveParameterTuner{public:voidupdateParameters(constEnvironmentContext&context);private:// 根据路径曲率调整 lookaheaddoubleadaptLookahead(doublepath_curvature);// 根据障碍物密度调整速度doubleadaptSpeed(doubleobstacle_density);};

4. 多路径候选评估

目标: 生成多条候选轨迹，选择最优路径

实现思路:

• 基于 DWA (Dynamic Window Approach) 生成多组速度指令
• 评估每条轨迹的安全性、平滑性、效率
• 选择最优轨迹执行

调试与故障排除

常见问题

1. 机器人运动不平稳

• 检查beta和lambda参数
• 适当减小v_linear_max
• 增大min_lookahead

2. 跟踪精度不足

• 减小max_lookahead
• 增大k_phi和k_delta
• 检查路径发布频率

3. 碰撞检测误报

• 调整safety_distance
• 检查 laser scan 数据质量
• 验证 footprint 配置

日志输出

控制器内置日志输出，可在控制台查看运行状态：

[SmoothControlLaw] ego: r=1.234, phi=0.123, delta=0.045, curvature=0.678, v=0.456, w=0.234 [GracefulController] lookahead=0.8, closest_idx=5, target=(0.8,0.1), dist_to_target=0.8, dist_to_goal=5.2

引用

如果您在研究中使用了本项目，请引用：

@software{purepath_controller, title = {PurePath Controller: A Pure C++ Path Tracking Controller for Mobile Robots}, author = {Robot Nav}, year = {2026}, url = {https://github.com/Robot-Nav/A-PurePath-Controller} }

参考文献

1. Kanayama, Y., Kimura, Y., Miyazaki, F., & Noguchi, T. (1990). A stable tracking control method for an autonomous mobile robot. Proceedings of the IEEE International Conference on Robotics and Automation.

2. Park, J., & Kuipers, B. (2011). A smooth control law for graceful motion of differential wheeled mobile robots in 2D environment. Proceedings of the IEEE International Conference on Robotics and Automation.

许可证

本项目采用 MIT License 开源许可证。

MIT License Copyright (c) 2026 Graceful Controller Contributors Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

贡献

欢迎提交 Issue 和 Pull Request！

开发流程

1. Fork 本仓库
2. 创建特性分支 (git checkout -b feature/amazing-feature)
3. 提交更改 (git commit -m 'Add amazing feature')
4. 推送到分支 (git push origin feature/amazing-feature)
5. 创建 Pull Request

Star 这个项目如果它对您有帮助！