ROS自定义全局路径规划插件：从预存轨迹到动态避障的融合实践

1. 为什么需要自定义全局路径规划插件在仓储物流场景中机器人经常需要在固定路线上往返行驶比如沿着货架间的通道移动。传统全局路径规划算法如A*、Dijkstra每次都会重新计算路径不仅消耗计算资源而且生成的路径可能不符合实际业务需求。想象一下如果每次送货都让机器人走不同的路线不仅效率低下还可能撞到货架。我做过一个实际项目客户要求AGV必须严格沿着地面磁条行驶。但磁条路线有时会被临时堆放的货物阻挡这时候就需要动态调整路径。这就是我们开发自定义插件的初衷——在固定路线基础上实现智能避障。ROS自带的全局规划器无法直接满足这种混合需求。我们需要自己实现一个插件在makePlan方法中完成两个核心功能加载预存的固定路线比如示教轨迹或地图标记的通道根据实时代价地图costmap动态调整路径避开障碍物2. 插件开发环境准备2.1 基础依赖安装首先确保你的ROS环境已经安装了navigation功能包sudo apt-get install ros-distro-navigation我用的是Melodic版本实际开发中发现需要特别注意这些依赖项costmap_2d提供代价地图数据nav_core规划器接口基类pluginlib插件加载机制注意如果遇到Could not find pluginlib错误试试安装ros-distro-pluginlib2.2 创建功能包建议单独创建规划器功能包与其他导航组件解耦catkin_create_pkg custom_planner roscpp costmap_2d nav_core pluginlib我习惯的代码结构是这样的custom_planner/ ├── include/ │ └── custom_planner/ │ └── global_planner.h ├── src/ │ ├── global_planner.cpp │ └── trajectory_loader.cpp └── cfg/ └── PlannerConfig.cfg3. 实现插件核心逻辑3.1 头文件设计继承nav_core::BaseGlobalPlanner是必须的但我会增加几个关键成员变量class HybridPlanner : public nav_core::BaseGlobalPlanner { public: void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) override; bool makePlan(const geometry_msgs::PoseStamped start, const geometry_msgs::PoseStamped goal, std::vectorgeometry_msgs::PoseStamped plan) override; private: // 预存轨迹容器 std::vectorgeometry_msgs::PoseStamped preset_path_; // 代价地图接口 costmap_2d::Costmap2DROS* costmap_ros_; // 动态避障参数 double obstacle_threshold_; // 轨迹加载方法 bool loadPresetPath(const std::string file_path); };3.2 makePlan方法实现这是最核心的部分我的实现分三步走第一步匹配最近路径点// 在预存路径中找到离起点最近的点 auto closest_iter std::min_element(preset_path_.begin(), preset_path_.end(), [start](const auto a, const auto b) { return hypot(a.pose.position.x - start.pose.position.x, a.pose.position.y - start.pose.position.y) hypot(b.pose.position.x - start.pose.position.x, b.pose.position.y - start.pose.position.y); });第二步代价地图检查// 检查路径点是否在障碍物上 unsigned int mx, my; costmap_-worldToMap(point.pose.position.x, point.pose.position.y, mx, my); if(costmap_-getCost(mx, my) obstacle_threshold_) { // 触发避障逻辑 adjustForObstacle(point); }第三步路径平滑处理实测发现直接拼接的路径会让机器人急转弯我加入了三次样条插值void smoothPath(std::vectorgeometry_msgs::PoseStamped path) { // 创建样条曲线对象 tk::spline spline_x, spline_y; // 添加控制点... // 生成平滑路径点 for(double t 0; t 1.0; t 0.02) { geometry_msgs::PoseStamped new_pose; new_pose.pose.position.x spline_x(t); new_pose.pose.position.y spline_y(t); path.push_back(new_pose); } }4. 动态避障的三种策略在仓储环境中我测试过这些避障方法4.1 局部绕行法当检测到障碍物时在预存路径两侧生成候选点选择代价最低的std::vectorstd::pairdouble, double candidates { {0.3, 0}, {-0.3, 0}, // 左右平移 {0, 0.3}, {0, -0.3} // 前后移动 }; for(const auto offset : candidates) { double new_x original.pose.position.x offset.first; double new_y original.pose.position.y offset.second; // 检查新位置是否可行... }4.2 临时重规划对于长时间阻塞的情况我会在局部调用A*算法navfn::NavfnROS fallback_planner; fallback_planner.initialize(fallback, costmap_ros_); fallback_planner.makePlan(detour_start, detour_goal, temp_plan);4.3 等待策略有时候最聪明的做法就是等待。我给插件增加了超时机制ros::Time start_time ros::Time::now(); while(ros::Time::now() - start_time ros::Duration(5.0)) { if(costmap_-getCost(mx, my) obstacle_threshold_) { return original_path; // 障碍物已移开 } ros::Duration(0.1).sleep(); }5. 性能优化技巧在100m×100m的仓库实测中我总结了这些经验5.1 路径点采样密度密度太高0.1m规划耗时长容易抖动密度太低1m避障不灵敏最佳实践直线段0.5m转弯处0.2m5.2 代价地图更新频率// 在initialize中设置 costmap_ros_-updateMap(); costmap_ros_-start();我发现更新间隔设为0.5秒是性价比最高的选择可以通过动态参数配置dynamic_reconfigure::Servercustom_planner::PlannerConfig server; server.setCallback(boost::bind(HybridPlanner::reconfigureCB, this, _1, _2));5.3 内存管理预存路径可能很长我用shared_ptr避免拷贝std::shared_ptrstd::vectorgeometry_msgs::PoseStamped preset_path_;6. 实际部署中的坑与解决方案坑1坐标系转换问题有次机器人突然走偏发现是预存轨迹的坐标系map和实时定位坐标系odom没统一。现在我会强制检查if(preset_path_.header.frame_id ! start.header.frame_id) { ROS_ERROR(Frame mismatch! Preset:%s vs Start:%s, preset_path_.header.frame_id.c_str(), start.header.frame_id.c_str()); return false; }坑2动态参数生效延迟动态重配置参数有时需要2-3秒才能生效我的解决办法是加缓存变量void reconfigureCB(PlannerConfig config, uint32_t level) { boost::mutex::scoped_lock lock(mutex_); cached_obstacle_thresh_ config.obstacle_threshold; }坑3插件加载失败遇到过插件明明编译成功但roslaunch报错Plugin not found最后发现是package.xml漏了导出export nav_core plugin${prefix}/plugin_description.xml/ /export7. 进阶功能扩展7.1 多轨迹切换通过服务调用动态更换预存路径ros::ServiceServer switch_service nh_.advertiseService( switch_path, HybridPlanner::handleSwitchRequest, this);7.2 速度规划集成在路径点中加入速度信息for(auto pose : plan) { pose.twist.linear.x calculateSpeed(pose); }7.3 机器学习预测用LSTM预测障碍物移动趋势提前调整路径。需要额外开启一个线程prediction_thread_ std::thread(HybridPlanner::runPrediction, this);在真实仓库环境中跑了两周后这套方案的避障成功率从最初的78%提升到了96%。最关键的体会是固定路线提供确定性动态避障保证安全性二者结合才是工业场景的实用方案。下次可以聊聊怎么用Octomap处理三维避障那又是另一个精彩的故事了。