PurePath ControllerPP_controller路径跟踪控制器链接PP_controller控制器欢迎大家交流学习一个基于Lyapunov稳定性原理的平滑控制律Smooth Control Law的纯净C机器人局部路径跟踪控制器零ROS依赖可移植到任何嵌入式平台。目录项目特点原理介绍功能特性系统架构文件结构安装与配置使用方法参数说明未来改进方向许可证项目特点纯净C实现本项目核心控制器采用纯净C17实现无任何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 │ └─────────────────────┘ └─────────────────────┘类结构类名功能描述SmoothControlLaw核心控制律实现计算曲率和速度GracefulController主控制器整合路径跟踪、碰撞检测ParameterHandler参数管理支持动态参数更新ICollisionChecker碰撞检测接口LaserCollisionChecker激光雷达碰撞检测实现文件结构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依赖:C17 兼容编译器 (GCC 7, Clang 5, MSVC 2017)CMake 3.10仅依赖C标准库ROS2可视化测试可选:Ubuntu 20.04 / Windows 10ROS2 Foxy / Humble / Rolling独立库编译推荐# 克隆仓库gitclone https://github.com/Robot-Nav/A-PurePath-Controller.gitcdA-PurePath-Controller# 创建构建目录mkdirbuildcdbuild# 编译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_MODELburger 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步骤参数说明控制参数参数名默认值描述min_lookahead0.25最小前视距离 (m)max_lookahead1.0最大前视距离 (m)k_phi2.0角度误差增益k_delta1.0方向误差增益beta0.4曲率影响系数lambda2.0曲率指数因子 (≥1)速度参数参数名默认值描述v_linear_min0.1最小线速度 (m/s)v_linear_max0.5最大线速度 (m/s)v_angular_max1.0最大角速度 (rad/s)v_angular_min_in_place0.25原地旋转最小角速度 (rad/s)行为参数参数名默认值描述slowdown_radius1.5减速半径 (m)initial_rotationtrue启用初始旋转对齐initial_rotation_tolerance0.75初始旋转容差 (rad)prefer_final_rotationtrue终点优先旋转到目标方向rotation_scaling_factor0.5旋转速度缩放因子allow_backwardfalse允许后退运动碰撞检测参数参数名默认值描述use_collision_detectiontrue启用碰撞检测collision_check_radius0.5碰撞检测半径 (m)safety_distance0.3安全距离 (m)in_place_collision_resolution0.1原地旋转碰撞检测分辨率 (rad)机器人 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(constLaserScanscan);// 计算沿墙速度指令TwistcomputeFollowVelocity(constPose2Dcurrent_pose,constLaserScanscan,doublefollow_distance,FollowSide side);// LEFT or RIGHT// 切换跟随方向voidswitchFollowSide(FollowSide side);private:// 提取障碍物边界点std::vectorPoint2DextractBoundaryPoints(constLaserScanscan);// 计算与障碍物的最近距离doublecomputeDistanceToObstacle(constLaserScanscan);};应用场景:狭窄通道通过未知环境探索紧急避障后的路径恢复2. 动态障碍物预测目标: 预测移动障碍物的轨迹提前规划避障路径实现思路:使用卡尔曼滤波跟踪动态障碍物预测障碍物未来位置在轨迹仿真中考虑动态障碍物3. 自适应参数调整目标: 根据环境复杂度动态调整控制参数实现思路:classAdaptiveParameterTuner{public:voidupdateParameters(constEnvironmentContextcontext);private:// 根据路径曲率调整 lookaheaddoubleadaptLookahead(doublepath_curvature);// 根据障碍物密度调整速度doubleadaptSpeed(doubleobstacle_density);};4. 多路径候选评估目标: 生成多条候选轨迹选择最优路径实现思路:基于 DWA (Dynamic Window Approach) 生成多组速度指令评估每条轨迹的安全性、平滑性、效率选择最优轨迹执行调试与故障排除常见问题1. 机器人运动不平稳检查beta和lambda参数适当减小v_linear_max增大min_lookahead2. 跟踪精度不足减小max_lookahead增大k_phi和k_delta检查路径发布频率3. 碰撞检测误报调整safety_distance检查 laser scan 数据质量验证 footprint 配置日志输出控制器内置日志输出可在控制台查看运行状态[SmoothControlLaw] ego: r1.234, phi0.123, delta0.045, curvature0.678, v0.456, w0.234 [GracefulController] lookahead0.8, closest_idx5, target(0.8,0.1), dist_to_target0.8, dist_to_goal5.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} }参考文献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.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开发流程Fork 本仓库创建特性分支 (git checkout -b feature/amazing-feature)提交更改 (git commit -m Add amazing feature)推送到分支 (git push origin feature/amazing-feature)创建 Pull RequestStar 这个项目如果它对您有帮助
