文章目录前言一、输入 / 输出输入输出二、源码逻辑createPlan入口函数makePlan主流程函数第 0 步清空路径 打头第 1 步起点世界坐标 → 栅格坐标第 2 步清除起点格的障碍容易被忽略但重要第 3 步锁地图 同步 NavFn 网格尺寸第 4 步把代价地图搬进 NavFn核心准备相当于另一篇讲navfn算法的第一步第 5~6 步起点/终点都转成栅格坐标第 7 步势场扩散整个函数的核心相当于另一篇讲navfn算法的第二步第 8 步目标点可达性检查第 9 步容差搜索目标不可达时的妥协第 10 步提取路径相当于另一篇讲navfn算法的第三步总结前言前面说了nav2_planner而其默认选择的路径生成算法模块是nav2_navfn_planner。这个算法和控制中的DWA算法不同DWA本身就是一种标准的基础算法。而navfn则是属于ros自研的一种算法。navfn的讲解就还是需要看我的另外一篇博客【一看就会】nav2中的核心导航算法NavfnNavigation Function算法有了另外一篇的理解这个源码讲解就会相对简单。本文从其源码出发进行讲解。一、输入 / 输出输入起点和终点都是在nav2_planner中的createPlan(start, goal)函数调用输入的。代价函数costmap_是nav2_planner自建之后在configure时给对应的规划插件的。输出函数返回路径nav_msgs/Path一条从起点到终点的路径点序列二、源码逻辑整个算法逻辑就不细讲了这里主要讲源码逻辑。createPlan入口函数这个函数可以分为三步1.更新检查规划器状态2.处理起点和终点重叠的情况3.执行路径规划makePlan其中最主要的就是调用了makePlan函数。源码/** * brief NavFn规划器的路径生成函数 * * 该函数是NavFnNaviation Function导航函数规划器的核心实现 * 使用基于势场的方法进行路径规划。 * * NavFn规划器的工作原理 * 1. 在代价地图上生成一个势场目标点具有最低势能 * 2. 从起始点开始沿着势能下降最快的方向到达目标点 * 3. 自动避开障碍物障碍物区域势能极高 * * param start 起始位姿位置和朝向 * param goal 目标位姿 * return nav_msgs::msg::Path 生成的路径如果失败则返回空路径 */nav_msgs::msg::Path NavfnPlanner::createPlan(constgeometry_msgs::msg::PoseStampedstart,constgeometry_msgs::msg::PoseStampedgoal){#ifdefBENCHMARK_TESTING// 性能测试模式记录函数开始时间用于计算规划耗时steady_clock::time_point asteady_clock::now();#endif// 1. 检查并更新规划器状态 // 如果代价地图的大小发生了变化如地图更新或扩展// 需要重新初始化规划器的导航数组if(isPlannerOutOfDate()){// 设置规划器导航数组的大小为当前代价地图的尺寸planner_-setNavArr(costmap_-getSizeInCellsX(),// X方向单元格数量costmap_-getSizeInCellsY());// Y方向单元格数量}// 创建空路径对象nav_msgs::msg::Path path;// 2. 处理特殊边界情况 // 当起始点与目标点重合时起点终点if(start.pose.position.xgoal.pose.position.xstart.pose.position.ygoal.pose.position.y){// 2.1 将起始点从世界坐标转换为地图坐标单元格坐标unsigned int mx,my;costmap_-worldToMap(start.pose.position.x,start.pose.position.y,mx,my);// 2.2 检查该单元格是否为致命障碍物if(costmap_-getCost(mx,my)nav2_costmap_2d::LETHAL_OBSTACLE){// 如果起点/目标点位于障碍物上无法生成路径RCLCPP_WARN(logger_,Failed to create a unique pose path because of obstacles);returnpath;// 返回空路径}// 2.3 创建单点路径只有一个位姿点path.header.stampclock_-now();// 设置时间戳path.header.frame_idglobal_frame_;// 设置坐标系通常是地图坐标系geometry_msgs::msg::PoseStamped pose;pose.headerpath.header;pose.pose.position.z0.0;// Z轴位置设为02D规划pose.posestart.pose;// 使用起始点的位置// 2.4 处理方向朝向的优先级// 如果起始朝向和目标朝向不同且不强制使用最终接近朝向if(start.pose.orientation!goal.pose.orientation!use_final_approach_orientation_){// 通常使用目标朝向因为最终到达时机器人应该面向目标方向pose.pose.orientationgoal.pose.orientation;}// 如果 use_final_approach_orientation_ 为true则保持起始朝向// 这可以避免局部规划器在起点处产生不必要的旋转运动// 将位姿添加到路径中path.poses.push_back(pose);returnpath;// 返回单点路径}// 3. 执行实际的路径规划 // 调用底层规划算法生成从起点到目标点的路径// makePlan使用势场方法在代价地图上寻找最优路径// 参数起始位姿、目标位姿、规划容忍度、输出路径if(!makePlan(start.pose,goal.pose,tolerance_,path)){// 如果规划失败记录警告日志RCLCPP_WARN(logger_,%s: failed to create plan with tolerance %.2f.,name_.c_str(),tolerance_);}#ifdefBENCHMARK_TESTING// 性能测试模式计算规划耗时并输出steady_clock::time_point bsteady_clock::now();durationdoubletime_spanduration_castdurationdouble(b-a);std::coutIt took time_span.count()*1000std::endl;#endif// 返回生成的路径可能为空returnpath;}makePlan主流程函数这个函数就是nav2_navfn_planner生成路径的主流程函数也就是实现navfn的函数。makePlan函数可以分为十步第 0 步清空路径 打头plan.poses.clear();plan.header.stampclock_-now();plan.header.frame_idglobal_frame_;// map 系这个没什么要说的。第 1 步起点世界坐标 → 栅格坐标if(!worldToMap(wx,wy,mx,my)){...robots start position is off the global costmapreturnfalse;// 起点不在代价地图范围内 → 直接失败}坐标转换起点检查第 2 步清除起点格的障碍容易被忽略但重要clearRobotCell(mx,my);把起点所在格子的代价强制设为空地。原因机器人的当前位置在代价地图上可能正好贴着障碍或站在障碍边缘因为地图更新滞后。如果不清势场把起点当障碍路径就出不来了。第 3 步锁地图 同步 NavFn 网格尺寸std::unique_lock...lock(*(costmap_-getMutex()));planner_-setNavArr(costmap_-getSizeInCellsX(),costmap_-getSizeInCellsY());锁 costmap防止传感器线程并发改地图setNavArr地图尺寸变了就重建 NavFn 内部数组第 4 步把代价地图搬进 NavFn核心准备相当于另一篇讲navfn算法的第一步planner_-setCostmap(costmap_-getCharMap(),true,allow_unknown_);lock.unlock();就是要把代价栅格地图做一下规整能让后续算法看懂第 5~6 步起点/终点都转成栅格坐标map_start[0]mx;map_start[1]my;// 起点栅格wxgoal.position.x;wygoal.position.y;if(!worldToMap(wx,wy,mx,my)){...goal off the global costmap// 终点不在图内 → 失败returnfalse;}map_goal[0]mx;map_goal[1]my;// 终点栅格第 7 步势场扩散整个函数的核心相当于另一篇讲navfn算法的第二步planner_-setStart(map_goal);// ★ 反着传planner_-setGoal(map_start);if(use_astar_){planner_-calcNavFnAstar();}else{planner_-calcNavFnDijkstra(true);}这个就是生成势场地图每个格子都由自己的势值。这个是最重要的默认用的dijkstra算法具体的算法逻辑在另外一篇博客中讲过了本篇主要是讲源码就不细讲算法逻辑了。第 8 步目标点可达性检查pgoal;double potentialgetPointPotential(p.position);// 查目标格子的势场值if(potentialPOT_HIGH){best_posep;found_legaltrue;// 目标点势场被更新过 可达}getPointPotential 把目标点世界坐标转栅格读 potarr 里那个格子的值。势场值 阈值POT_HIGH1e10说明扩散到过它 从目标能到达。第 9 步容差搜索目标不可达时的妥协// 目标点在障碍里/不可达时在 tolerance 范围默认 0.5m网格扫一遍p.position.ygoal.position.y-tolerance;while(p.position.ygoal.position.ytolerance){p.position.xgoal.position.x-tolerance;while(p.position.xgoal.position.xtolerance){potentialgetPointPotential(p.position);if(potentialPOT_HIGHsdistbest_sdist){// 合法且离目标最近best_sdistsdist;best_posep;found_legaltrue;}p.position.xresolution;}p.position.yresolution;}为什么需要用户点的目标可能就卡在障碍边缘/墙里。这时在全图势场已算好的前提下在目标周围 tolerance × tolerance 的方框里找最近的、势场可达的格子作为替代终点。所以路径会停在目标点附近最近的合法位置。第 10 步提取路径相当于另一篇讲navfn算法的第三步if(found_legal){if(getPlanFromPotential(best_pose,plan)){// 见下方smoothApproachToGoal(best_pose,plan);// 打磨末端if(use_final_approach_orientation_){...}// 终点朝向修正}else{RCLCPP_ERROR(...);}}return!plan.poses.empty();// 路径非空 成功getPlanFromPotentialnavfn_planner.cpp:371内部把 best_pose 设成 NavFn 的起点回溯起点planner_-calcPath(max_cycles) → 从起点沿梯度下山到目标产出 pathx/pathy浮点亚格子坐标转回世界坐标填进 plan.posessmoothApproachToGoalnavfn_planner.cpp:346若路径倒数第二个点离目标比离终点更近就把终点替换成真正的 goal 点消除离散网格的锯齿尾巴。源码/** * brief NavFn规划器的底层路径生成函数 * * 该函数是NavFn规划器的核心算法实现负责 * 1. 将起始点和目标点转换为地图坐标 * 2. 检查起点和终点是否在代价地图范围内 * 3. 构建导航函数势场并计算路径 * 4. 处理目标不可达的情况寻找最近的可达点 * 5. 从势场中提取路径点 * 6. 平滑处理接近目标段的路径 * 7. 处理最终接近朝向 * * param start 起始位姿世界坐标 * param goal 目标位姿世界坐标 * param tolerance 目标容忍半径在范围内视为可达 * param plan 输出的路径对象引用 * return bool 规划是否成功 */boolNavfnPlanner::makePlan(constgeometry_msgs::msg::Posestart,constgeometry_msgs::msg::Posegoal,double tolerance,nav_msgs::msg::Pathplan){// 1. 初始化路径 // 清空路径中可能存在的旧数据plan.poses.clear();// 设置路径的头信息plan.header.stampclock_-now();// 时间戳plan.header.frame_idglobal_frame_;// 坐标系地图坐标系// 2. 转换起始点坐标 double wxstart.position.x;double wystart.position.y;RCLCPP_DEBUG(logger_,Making plan from (%.2f,%.2f) to (%.2f,%.2f),start.position.x,start.position.y,goal.position.x,goal.position.y);// 将起始点从世界坐标转换为地图坐标单元格坐标unsigned int mx,my;if(!worldToMap(wx,wy,mx,my)){// 如果起始点在地图之外无法规划RCLCPP_WARN(logger_,Cannot create a plan: the robots start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?);returnfalse;}// 3. 清除起始单元格的障碍物标记 // 因为机器人当前位置不可能有障碍物所以清除该单元格的障碍物标记// 这可以避免因地图更新延迟导致的误判clearRobotCell(mx,my);// 4. 锁定代价地图并更新规划器 // 使用互斥锁保护代价地图数据防止在规划过程中被修改std::unique_locknav2_costmap_2d::Costmap2D::mutex_tlock(*(costmap_-getMutex()));// 确保规划器的导航数组大小与代价地图匹配planner_-setNavArr(costmap_-getSizeInCellsX(),costmap_-getSizeInCellsY());// 将代价地图数据传递给规划器// allow_unknown_ 控制是否允许穿过未知区域planner_-setCostmap(costmap_-getCharMap(),true,allow_unknown_);// 提前解锁允许其他线程访问代价地图lock.unlock();// 5. 设置起始点和目标点 int map_start[2];map_start[0]mx;map_start[1]my;// 转换目标点坐标wxgoal.position.x;wygoal.position.y;if(!worldToMap(wx,wy,mx,my)){// 如果目标点在地图之外无法规划RCLCPP_WARN(logger_,The goal sent to the planner is off the global costmap. Planning will always fail to this goal.);returnfalse;}int map_goal[2];map_goal[0]mx;map_goal[1]my;// 注意这里start和goal在设置时是反的// setStart接收的是目标点setGoal接收的是起始点// 这是因为NavFn算法是从目标点向起始点传播势场// 这样计算出的势场可以直接用于路径提取planner_-setStart(map_goal);// 从目标点开始传播势场planner_-setGoal(map_start);// 传播到起始点结束// 6. 计算导航函数势场 if(use_astar_){// 使用A*算法启发式搜索planner_-calcNavFnAstar();}else{// 使用Dijkstra算法在栅格地图上使用Dijkstra计算势场// true表示使用潜在的场planner_-calcNavFnDijkstra(true);}// 7. 处理目标点不可达的情况 double resolutioncostmap_-getResolution();geometry_msgs::msg::Pose p,best_pose;bool found_legalfalse;// 7.1 检查目标点本身是否可达pgoal;double potentialgetPointPotential(p.position);if(potentialPOT_HIGH){// POT_HIGH表示无限大势能不可达区域// 目标点本身势能较低可直接到达best_posep;found_legaltrue;}else{// 7.2 目标点不可达在容忍范围内搜索最近的可达点// 原理在目标点周围tolerance半径内搜索势能最低的点double best_sdiststd::numeric_limitsdouble::max();// 在目标点周围的正方形区域内进行搜索步长为地图分辨率p.position.ygoal.position.y-tolerance;while(p.position.ygoal.position.ytolerance){p.position.xgoal.position.x-tolerance;while(p.position.xgoal.position.xtolerance){potentialgetPointPotential(p.position);double sdistsquared_distance(p,goal);// 到目标点的平方距离// 选择可达势能低且距离目标最近的候选点if(potentialPOT_HIGHsdistbest_sdist){best_sdistsdist;best_posep;found_legaltrue;}p.position.xresolution;}p.position.yresolution;}}// 8. 从势场中提取路径 if(found_legal){// 8.1 从最佳目标点开始沿着势能下降方向提取路径到起始点if(getPlanFromPotential(best_pose,plan)){// 8.2 平滑接近目标段smoothApproachToGoal(best_pose,plan);// 9. 处理最终接近朝向 // 如果配置了使用最终接近朝向if(use_final_approach_orientation_){size_t plan_sizeplan.poses.size();if(plan_size1){// 只有单个点起点终点使用起始朝向plan.poses.back().pose.orientationstart.orientation;}elseif(plan_size1){// 计算路径最后一段的方向作为最终朝向double dx,dy,theta;auto last_poseplan.poses.back().pose.position;// 最后一个点auto approach_poseplan.poses[plan_size-2].pose.position;// 倒数第二个点// 处理特殊情况如果最后两个点重合可能是算法产生的冗余点if(std::abs(last_pose.x-approach_pose.x)0.0001std::abs(last_pose.y-approach_pose.y)0.0001plan_size2){// 使用倒数第三个点来计算方向approach_poseplan.poses[plan_size-3].pose.position;}// 计算方向角dxlast_pose.x-approach_pose.x;dylast_pose.y-approach_pose.y;thetaatan2(dy,dx);// 将方向角转换为四元数只绕Z轴旋转保持水平plan.poses.back().pose.orientationnav2_util::geometry_utils::orientationAroundZAxis(theta);}}}else{RCLCPP_ERROR(logger_,Failed to create a plan from potential when a legal potential was found. This shouldnt happen.);}}// 10. 返回规划结果 // 如果路径非空表示规划成功return!plan.poses.empty();}总结这个模块是nav2默认的适配差速车的全局路径规划算法负责从起点到终点的路径规划。