ROS2 Navigation Framework and System核心算法原理解析：A*与DWA路径规划技术

引言：移动机器人路径规划的核心挑战

你是否曾为移动机器人在复杂环境中的避障效率低下而困扰？是否在调试导航系统时因路径规划算法的不透明性而无从下手？本文将深入解析ROS2 Navigation Framework and System（以下简称Navigation2）中的两大核心路径规划技术——A*（A-Star）全局路径规划与DWA（Dynamic Window Approach）局部路径规划，通过代码级别的原理解析和实战案例，帮助你彻底掌握移动机器人导航的关键技术。

读完本文，你将获得：

• A*算法在Navigation2中的具体实现与优化技巧
• DWA算法的动态窗口生成与轨迹评价机制
• 两种算法的参数调优指南与性能对比
• 基于Navigation2的路径规划系统集成实战

A*全局路径规划：从理论到Navigation2实现

A*算法核心原理

A算法是一种基于启发式搜索的全局路径规划算法，它通过综合考虑从起点到当前节点的实际代价（g值）和当前节点到目标节点的估计代价（h值），来寻找最优路径。A算法的估价函数定义为：

f(n) = g(n) + h(n)

其中：

• g(n)：从起点到节点n的实际代价
• h(n)：从节点n到目标节点的估计代价（启发函数）

在Navigation2中，A*算法的实现主要位于nav2_navfn_planner包中，核心代码在navfn.cpp文件中。

Navigation2中的A*实现解析

NavFn类结构

NavFn类是Navigation2中A*算法的核心实现，它封装了路径规划所需的数据结构和算法逻辑：

class NavFn {
public:
  // 构造函数，初始化地图尺寸
  NavFn(int xs, int ys);
  
  // 设置起点和目标点
  void setGoal(int *g);
  void setStart(int *g);
  
  // A*算法主函数
  bool calcNavFnAstar(std::function<bool()> cancelChecker);
  
  // 路径生成
  int calcPath(int n, int *st = NULL);
  
  // 获取路径结果
  float *getPathX() {return pathx;}
  float *getPathY() {return pathy;}
  int getPathLen() {return npath;}
  
  // ...其他成员函数和变量
private:
  int nx, ny, ns;  // 地图尺寸
  COSTTYPE *costarr;  // 代价地图数组
  float *potarr;  // 势场数组
  bool *pending;  // 待处理节点标记
  float *gradx, *grady;  // 梯度数组
  int *pathx, *pathy;  // 路径数组
  int npath;  // 路径长度
  // ...其他成员变量
};

A*算法主流程

propNavFnAstar函数实现了A*算法的核心传播过程：

bool NavFn::propNavFnAstar(int cycles, std::function<bool()> cancelChecker) {
  int nwv = 0;  // 最大优先级队列大小
  int nc = 0;   // 处理的节点数量
  int cycle = 0;  // 当前迭代次数

  // 基于起点和目标点的距离设置初始阈值
  float dist = hypot(goal[0] - start[0], goal[1] - start[1]) * static_cast<float>(COST_NEUTRAL);
  curT = dist + curT;

  // 起点单元格索引
  int startCell = start[1] * nx + start[0];

  // 主循环
  for (; cycle < cycles; cycle++) {
    // 检查是否需要取消规划
    if (cycle % terminal_checking_interval == 0 && cancelChecker()) {
      throw nav2_core::PlannerCancelled("Planner was cancelled");
    }

    // 如果优先级队列为空，则退出
    if (curPe == 0 && nextPe == 0) {
      break;
    }

    // 统计处理的节点数量
    nc += curPe;
    if (curPe > nwv) {
      nwv = curPe;
    }

    // 重置当前优先级队列中节点的待处理标记
    int *pb = curP;
    int i = curPe;
    while (i-- > 0) {
      pending[*(pb++)] = false;
    }

    // 处理当前优先级队列中的节点
    pb = curP;
    i = curPe;
    while (i-- > 0) {
      updateCellAstar(*pb++);  // A*算法的节点更新
    }

    // 交换优先级队列
    curPe = nextPe;
    nextPe = 0;
    pb = curP;
    curP = nextP;
    nextP = pb;

    // 检查是否完成当前优先级级别的处理
    if (curPe == 0) {
      curT += priInc;  // 增加优先级阈值
      curPe = overPe;  // 将溢出队列设为当前队列
      overPe = 0;
      pb = curP;
      curP = overP;
      overP = pb;
    }

    // 检查是否到达起点
    if (potarr[startCell] < POT_HIGH) {
      break;
    }
  }

  RCLCPP_DEBUG(
    rclcpp::get_logger("rclcpp"),
    "[NavFn] Used %d cycles, %d cells visited (%d%%), priority buf max %d\n",
    cycle, nc, (int)((nc * 100.0) / (ns - nobs)), nwv);

  return (cycle < cycles) ? true : false;
}

A*节点更新策略

updateCellAstar函数实现了A*算法中节点代价的更新逻辑，是算法的核心：

inline void NavFn::updateCellAstar(int n) {
  // 获取邻居节点的势场值
  float l = potarr[n - 1];  // 左
  float r = potarr[n + 1];  // 右
  float u = potarr[n - nx];  // 上
  float d = potarr[n + nx];  // 下

  // 找到最低代价的邻居
  float ta, tc;
  if (l < r) {tc = l;} else {tc = r;}
  if (u < d) {ta = u;} else {ta = d;}

  // 如果当前单元格不是障碍物，则更新势场值
  if (costarr[n] < COST_OBS) {
    float hf = static_cast<float>(costarr[n]);  // 可通行性因子
    float dc = tc - ta;  // ta和tc之间的相对代价
    if (dc < 0) {  // ta是最低代价
      dc = -dc;
      ta = tc;
    }

    // 计算新的势场值
    float pot;
    if (dc >= hf) {  // 如果差异太大，仅使用ta更新
      pot = ta + hf;
    } else {  // 双邻居插值更新
      const float div = dc / hf;
      const float v = -0.2301 * div * div + 0.5307 * div + 0.7040;
      pot = ta + hf * v;
    }

    // 计算启发式距离（A*算法核心）
    int x = n % nx;
    int y = n / nx;
    float dist = hypot(x - start[0], y - start[1]) * static_cast<float>(COST_NEUTRAL);

    // 更新势场值并将邻居加入优先级队列
    if (pot < potarr[n]) {
      float le = INVSQRT2 * static_cast<float>(costarr[n - 1]);
      float re = INVSQRT2 * static_cast<float>(costarr[n + 1]);
      float ue = INVSQRT2 * static_cast<float>(costarr[n - nx]);
      float de = INVSQRT2 * static_cast<float>(costarr[n + nx]);

      potarr[n] = pot;
      pot += dist;  // A*算法：f(n) = g(n) + h(n)
      
      // 根据新的势场值将邻居加入不同优先级队列
      if (pot < curT) {
        if (l > pot + le) {push_next(n - 1);}
        if (r > pot + re) {push_next(n + 1);}
        if (u > pot + ue) {push_next(n - nx);}
        if (d > pot + de) {push_next(n + nx);}
      } else {
        if (l > pot + le) {push_over(n - 1);}
        if (r > pot + re) {push_over(n + 1);}
        if (u > pot + ue) {push_over(n - nx);}
        if (d > pot + de) {push_over(n + nx);}
      }
    }
  }
}

A*算法在Navigation2中的应用流程

在Navigation2中，A*算法的典型应用流程如下：

int create_nav_plan_astar(
  COSTTYPE * costmap, int nx, int ny,
  int * goal, int * start,
  float * plan, int nplan) {
  static NavFn * nav = NULL;

  // 初始化或重置NavFn对象
  if (nav == NULL || nav->nx != nx || nav->ny != ny) {
    delete nav;
    nav = new NavFn(nx, ny);
  }

  // 设置目标点和起点
  nav->setGoal(goal);
  nav->setStart(start);

  // 设置代价地图
  nav->costarr = costmap;
  
  // 初始化导航函数
  nav->setupNavFn(true);

  // 使用A*算法计算路径
  nav->priInc = 2 * COST_NEUTRAL;
  nav->propNavFnAstar(std::max(nx * ny / 20, nx + ny));

  // 生成路径
  int len = nav->calcPath(nplan);

  // 将路径结果复制到输出参数
  if (len > 0) {
    for (int i = 0; i < len; i++) {
      plan[i * 2] = nav->pathx[i];
      plan[i * 2 + 1] = nav->pathy[i];
    }
  }

  return len;
}

A*算法优化：Navigation2的独特改进

Navigation2对传统A*算法进行了多项优化：

1. 优先级队列管理：使用三级优先级队列（当前队列、下一级队列、溢出队列）来提高搜索效率
2. 势场值更新：采用二次函数近似进行势场值插值，提高路径平滑性
3. 动态阈值调整：根据当前搜索情况动态调整优先级阈值，平衡搜索效率和路径质量
4. 取消机制：支持外部取消信号，允许在规划耗时过长时终止算法

DWA局部路径规划：动态窗口方法的工程实现

DWA算法原理与优势

DWA算法是一种基于采样的局部路径规划方法，它通过在速度空间（v, ω）中采样多个速度指令，模拟这些指令在一定时间内产生的轨迹，并对这些轨迹进行评价，选择最优轨迹对应的速度指令发送给机器人执行器。

DWA算法的核心优势在于：

• 考虑机器人动力学约束，生成的轨迹更具可执行性
• 能够实时响应环境变化，避障效果好
• 计算复杂度可控，适合实时应用

Navigation2中的DWA实现：dwb_core包

Navigation2中的DWA算法实现主要位于dwb_core包中，核心代码在dwb_local_planner.cpp文件中。

DWBLocalPlanner类结构

DWBLocalPlanner类是DWA算法的核心实现：

class DWBLocalPlanner : public nav2_core::Controller {
public:
  DWBLocalPlanner();
  
  // 配置函数
  void configure(
    const nav2::LifecycleNode::WeakPtr & parent,
    std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros);
  
  // 设置全局路径
  void setPlan(const nav_msgs::msg::Path & path);
  
  // 计算速度指令
  geometry_msgs::msg::TwistStamped computeVelocityCommands(
    const geometry_msgs::msg::PoseStamped & pose,
    const geometry_msgs::msg::Twist & velocity,
    nav2_core::GoalChecker * goal_checker);
  
  // ...其他成员函数和变量
private:
  // 轨迹生成器
  std::unique_ptr<TrajectoryGenerator> traj_generator_;
  
  // 轨迹评价器（代价函数）
  std::vector<TrajectoryCritic::Ptr> critics_;
  
  // 插件加载器
  pluginlib::ClassLoader<TrajectoryGenerator> traj_gen_loader_;
  pluginlib::ClassLoader<TrajectoryCritic> critic_loader_;
  
  // 发布器
  std::unique_ptr<DWBPublisher> pub_;
  
  // ...其他成员变量
};

DWA算法核心流程

DWA算法在Navigation2中的实现遵循以下核心流程：

1. 速度采样：在可行速度空间中采样多个速度指令
2. 轨迹生成：针对每个速度指令，生成对应的轨迹
3. 轨迹评价：使用多个评价指标对每条轨迹进行打分
4. 最优轨迹选择：选择得分最高的轨迹
5. 速度指令发布：将最优轨迹对应的速度指令发送给机器人

核心算法实现：coreScoringAlgorithm函数

dwb_msgs::msg::TrajectoryScore
DWBLocalPlanner::coreScoringAlgorithm(
  const geometry_msgs::msg::Pose2D & pose,
  const nav_2d_msgs::msg::Twist2D velocity,
  std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation> & results) {
  
  // 初始化变量
  nav_2d_msgs::msg::Twist2D twist;
  dwb_msgs::msg::Trajectory2D traj;
  dwb_msgs::msg::TrajectoryScore best, worst;
  best.total = -1;
  worst.total = -1;
  IllegalTrajectoryTracker tracker;

  // 开始生成新的轨迹迭代
  traj_generator_->startNewIteration(velocity);
  
  // 遍历所有可能的速度指令
  while (traj_generator_->hasMoreTwists()) {
    // 获取下一个速度指令
    twist = traj_generator_->nextTwist();
    
    // 生成轨迹
    traj = traj_generator_->generateTrajectory(pose, velocity, twist);

    try {
      // 评价轨迹
      dwb_msgs::msg::TrajectoryScore score = scoreTrajectory(traj, best.total);
      tracker.addLegalTrajectory();
      
      // 更新最优轨迹
      if (best.total < 0 || score.total < best.total) {
        best = score;
      }
      if (worst.total < 0 || score.total > worst.total) {
        worst = score;
      }
    } catch (const dwb_core::IllegalTrajectoryException & e) {
      // 处理非法轨迹
      tracker.addIllegalTrajectory(e);
    }
  }

  // 如果没有合法轨迹，抛出异常
  if (best.total < 0) {
    throw NoLegalTrajectoriesException(tracker);
  }

  return best;
}

轨迹生成：TrajectoryGenerator

轨迹生成是DWA算法的关键步骤，Navigation2将其设计为插件接口，允许用户实现不同的轨迹生成策略。标准实现位于dwb_plugins包的standard_trajectory_generator.cpp文件中。

轨迹生成的核心逻辑：

dwb_msgs::msg::Trajectory2D
StandardTrajectoryGenerator::generateTrajectory(
  const geometry_msgs::msg::Pose2D & start_pose,
  const nav_2d_msgs::msg::Twist2D & start_vel,
  const nav_2d_msgs::msg::Twist2D & cmd_vel) {
  
  dwb_msgs::msg::Trajectory2D traj;
  traj.velocity = cmd_vel;
  
  // 初始化轨迹点
  geometry_msgs::msg::Pose2D current_pose = start_pose;
  nav_2d_msgs::msg::Twist2D current_vel = start_vel;
  
  // 时间步长
  double dt = sim_time_ / num_steps_;
  
  // 模拟轨迹
  for (unsigned int i = 0; i < num_steps_; ++i) {
    // 应用加速度限制
    applyAccelerationLimits(current_vel, cmd_vel, dt);
    
    // 积分得到位置
    integrate(current_pose, current_vel, dt);
    
    // 添加轨迹点
    traj.poses.push_back(current_pose);
    
    // 更新时间
    traj.time_offsets.push_back(rclcpp::Duration::from_seconds((i+1)*dt));
  }
  
  return traj;
}

轨迹评价：多准则决策机制

Navigation2采用插件化的轨迹评价机制，允许用户配置多个评价指标（称为Critic），共同决定轨迹的优劣。

评价函数加载与初始化

void DWBLocalPlanner::loadCritics() {
  auto node = node_.lock();
  
  // 获取配置的评价函数列表
  std::vector<std::string> critic_names;
  node->get_parameter(dwb_plugin_name_ + ".critics", critic_names);
  
  // 获取默认评价函数命名空间
  node->get_parameter(
    dwb_plugin_name_ + ".default_critic_namespaces", default_critic_namespaces_);
  
  // 如果没有默认命名空间，添加"dwb_critics"
  if (default_critic_namespaces_.empty()) {
    default_critic_namespaces_.emplace_back("dwb_critics");
  }
  
  // 加载每个评价函数
  for (unsigned int i = 0; i < critic_names.size(); i++) {
    std::string critic_plugin_name = critic_names[i];
    std::string plugin_class;
    
    // 获取评价函数类名
    node->get_parameter(
      dwb_plugin_name_ + "." + critic_plugin_name + ".class", plugin_class);
    
    // 解析评价函数类名
    plugin_class = resolveCriticClassName(plugin_class);
    
    // 创建评价函数实例
    TrajectoryCritic::Ptr plugin = critic_loader_.createUniqueInstance(plugin_class);
    
    // 初始化评价函数
    plugin->initialize(node, critic_plugin_name, dwb_plugin_name_, costmap_ros_);
    
    // 添加到评价函数列表
    critics_.push_back(plugin);
  }
}

轨迹评分过程

dwb_msgs::msg::TrajectoryScore
DWBLocalPlanner::scoreTrajectory(
  const dwb_msgs::msg::Trajectory2D & traj,
  double best_score) {
  
  dwb_msgs::msg::TrajectoryScore score;
  score.traj = traj;
  
  // 遍历所有评价函数
  for (TrajectoryCritic::Ptr & critic : critics_) {
    dwb_msgs::msg::CriticScore cs;
    cs.name = critic->getName();
    cs.scale = critic->getScale();
    
    // 如果评价函数权重为0，跳过
    if (cs.scale == 0.0) {
      score.scores.push_back(cs);
      continue;
    }
    
    // 计算评价分数
    double critic_score = critic->scoreTrajectory(traj);
    cs.raw_score = critic_score;
    score.scores.push_back(cs);
    
    // 累加加权分数
    score.total += critic_score * cs.scale;
    
    // 如果启用了短路评价且当前分数已超过最佳分数，提前退出
    if (short_circuit_trajectory_evaluation_ && best_score > 0 && score.total > best_score) {
      break;
    }
  }
  
  return score;
}

常用评价函数（Critic）解析

Navigation2提供了多种评价函数，覆盖不同的评价维度：

1. ObstacleFootprintCritic：评价轨迹与障碍物的距离
2. GoalDistCritic：评价轨迹终点与局部目标的距离
3. PathDistCritic：评价轨迹与全局路径的偏离程度
4. GoalAlignCritic：评价轨迹终点朝向与目标方向的对齐程度
5. TwistLimitCritic：评价速度是否符合机器人速度限制
6. PreferForwardCritic：鼓励机器人前进方向的运动
7. RotateToGoalCritic：在接近目标时，鼓励机器人旋转到目标朝向

每个评价函数通过scoreTrajectory方法计算轨迹得分，例如ObstacleFootprintCritic的实现：

double ObstacleFootprintCritic::scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) {
  double min_dist = INFINITY;
  
  // 检查轨迹上每个点的 footprint 与障碍物距离
  for (const auto & pose : traj.poses) {
    // 将轨迹点转换到地图坐标系
    geometry_msgs::msg::Pose2D map_pose = costmap_ros_->getCostmap()->getMapPose(pose);
    
    // 计算 footprint 与障碍物的最小距离
    double dist = footprintCost(map_pose.x, map_pose.y, map_pose.theta);
    
    // 更新最小距离
    if (dist < min_dist) {
      min_dist = dist;
      
      // 如果距离已经小于阈值，可以提前退出
      if (min_dist < lethal_threshold_) {
        return min_dist;
      }
    }
  }
  
  return min_dist;
}

A*与DWA的协同工作：Navigation2路径规划系统集成

Navigation2路径规划系统架构

Navigation2采用分层架构，A*与DWA算法在其中扮演不同角色，协同完成移动机器人的导航任务：

flowchart TD
    A[全局路径规划器] -->|提供全局路径| B[局部路径规划器]
    B -->|提供速度指令| C[机器人控制器]
    C -->|执行运动| D[移动机器人]
    D -->|传感器数据| E[环境感知模块]
    E -->|代价地图| A
    E -->|代价地图| B
    
    subgraph 全局规划层
        A
    end
    
    subgraph 局部规划层
        B
    end
    
    subgraph 执行层
        C
        D
    end
    
    subgraph 感知层
        E
    end

在这个架构中：

• A*算法作为全局路径规划器，负责在地图尺度上规划从起点到目标点的最优路径
• DWA算法作为局部路径规划器，负责在全局路径的引导下，实时避障并生成机器人可执行的速度指令

全局路径与局部路径的衔接

Navigation2通过transformGlobalPlan函数实现全局路径到局部路径的转换：

nav_2d_msgs::msg::Path2D
DWBLocalPlanner::transformGlobalPlan(const nav_2d_msgs::msg::Pose2DStamped & pose) {
  // ...
  
  // 找到全局路径中距离机器人最近的点
  auto transformation_begin = std::find_if(
    begin(global_plan_.poses), prune_point,
    [&](const auto & global_plan_pose) {
      return euclidean_distance(robot_pose.pose, global_plan_pose) < transform_start_threshold;
    });
  
  // 找到需要考虑的全局路径终点
  auto transformation_end = std::find_if(
    transformation_begin, global_plan_.poses.end(),
    [&](const auto & global_plan_pose) {
      return euclidean_distance(global_plan_pose, robot_pose.pose) > transform_end_threshold;
    });
  
  // 将全局路径的相关部分转换到机器人坐标系
  nav_2d_msgs::msg::Path2D transformed_plan;
  transformed_plan.header.frame_id = costmap_ros_->getGlobalFrameID();
  transformed_plan.header.stamp = pose.header.stamp;
  
  // 转换路径点
  std::transform(
    transformation_begin, transformation_end,
    std::back_inserter(transformed_plan.poses),
    transformGlobalPoseToLocal);
  
  // ...
  
  return transformed_plan;
}

参数调优指南：平衡性能与鲁棒性

A*算法关键参数

参数名	作用	推荐值范围	调优建议
allow_unknown	是否允许通过未知区域	true/false	室内环境建议设为false，室外大地图可设为true
default_tolerance	目标点容差	0.1-0.5m	根据机器人尺寸和定位精度调整
visualize_potential	是否可视化势场	true/false	调试时设为true，实际运行设为false
neutral_cost	中性代价	50-100	影响A*算法的启发式权重
lethal_cost	致命代价	253-255	必须大于中性代价

DWA算法关键参数

参数名	作用	推荐值范围	调优建议
max_vel_x	最大线速度	0.5-1.5m/s	根据机器人性能和环境复杂度调整
max_vel_theta	最大角速度	1.0-3.0rad/s	影响机器人转向灵活性
acc_lim_x	线加速度限制	0.5-2.0m/s²	需参考机器人实际动力学参数
acc_lim_theta	角加速度限制	1.0-4.0rad/s²	需参考机器人实际动力学参数
sim_time	轨迹模拟时间	1.0-3.0s	时间越长，轨迹越远，但计算量越大
vx_samples	线速度采样数量	3-10	影响速度空间覆盖率
vy_samples	横向速度采样数量	0-5	差分驱动机器人设为0
vth_samples	角速度采样数量	5-20	影响转向灵活性

评价函数权重调优

DWA算法的性能很大程度上取决于各评价函数的权重配置，以下是推荐的初始配置：

critics: ["ObstacleFootprintCritic", "GoalDistCritic", "PathDistCritic", "TwistLimitCritic"]

ObstacleFootprintCritic.scale: 10.0
GoalDistCritic.scale: 1.0
PathDistCritic.scale: 3.0
TwistLimitCritic.scale: 1.0

调优策略：

1. 首先确保ObstacleFootprintCritic权重最高，保证避障优先
2. 调整GoalDistCritic和PathDistCritic权重平衡目标导向和路径跟踪
3. 根据机器人动力学特性调整TwistLimitCritic权重

实战案例：基于Navigation2的路径规划系统调试与优化

常见问题诊断与解决方案

问题1：A*算法规划时间过长

可能原因：

• 地图尺寸过大
• 障碍物密度过高
• 启发函数设计不合理

解决方案：

// 优化启发函数，增加距离权重
float dist = hypot(x - start[0], y - start[1]) * static_cast<float>(COST_NEUTRAL) * 1.2;

// 增加优先级队列大小限制
if (curPe >= PRIORITYBUFSIZE * 0.8) {
  // 动态调整优先级阈值，加速搜索
  curT += priInc * 2;
}

问题2：DWA算法在狭窄通道中震荡

可能原因：

• 速度采样范围过大
• 轨迹评价函数权重配置不当
• 模拟时间过短

解决方案：

# 缩小速度采样范围
max_vel_x: 0.5
min_vel_x: 0.1
max_vel_theta: 1.5

# 增加路径跟踪权重
PathDistCritic.scale: 5.0

# 延长模拟时间
sim_time: 2.0

性能优化技巧

A*算法优化

1. 分层搜索：先在低分辨率地图上规划，再在高分辨率地图上细化
2. 双向搜索：同时从起点和目标点开始搜索，提高搜索效率
3. 剪枝策略：对明显非最优的路径分支进行剪枝

DWA算法优化

1. 自适应采样：根据环境复杂度动态调整采样数量
2. 预计算轨迹库：离线预计算常用速度指令对应的轨迹模板
3. 并行评价：多线程并行评价不同轨迹

高级应用：A*与DWA的参数自适应调整

在复杂动态环境中，固定参数难以适应所有场景，可实现参数自适应调整机制：

void adaptiveParameterTuning(const nav_2d_msgs::msg::Path2D & transformed_plan) {
  // 根据局部路径长度调整DWA模拟时间
  if (transformed_plan.poses.size() > 10) {
    traj_generator_->setSimTime(2.5);  // 长路径增加模拟时间
  } else {
    traj_generator_->setSimTime(1.0);  // 短路径减少模拟时间
  }
  
  // 根据路径曲率调整角速度采样范围
  double path_curvature = calculatePathCurvature(transformed_plan);
  if (path_curvature > 0.5) {
    // 高曲率路径增加角速度采样密度
    traj_generator_->setThetaSamples(20);
  } else {
    traj_generator_->setThetaSamples(10);
  }
}

结论与展望

本文深入剖析了ROS2 Navigation Framework and System中A*与DWA路径规划算法的实现细节，从理论原理到代码实现，全面展示了移动机器人路径规划技术的工程实践。通过对Navigation2源代码的分析，我们不仅理解了算法的工作机制，还掌握了实际应用中的调试技巧和优化方法。

未来路径规划技术的发展方向包括：

1. 深度学习与传统算法融合：利用神经网络优化启发函数和评价函数
2. 多传感器融合感知：提高复杂环境下的路径规划鲁棒性
3. 分布式路径规划：适应多机器人系统的协同导航需求

通过不断深入理解和优化路径规划算法，我们能够构建更加高效、鲁棒和智能的移动机器人导航系统，为各种实际应用场景提供强大的技术支持。

附录：Navigation2路径规划相关API速查

A*算法核心API

函数名	功能描述	参数说明
NavFn::setGoal	设置目标点	int *g: 目标点坐标数组
NavFn::setStart	设置起点	int *s: 起点坐标数组
NavFn::calcNavFnAstar	执行A*算法	cancelChecker: 取消检查函数
NavFn::calcPath	生成路径	n: 最大路径点数量

DWA算法核心API

函数名	功能描述	参数说明
DWBLocalPlanner::setPlan	设置全局路径	path: 全局路径消息
DWBLocalPlanner::computeVelocityCommands	计算速度指令	pose: 当前位姿, velocity: 当前速度
DWBLocalPlanner::scoreTrajectory	轨迹评价	traj: 待评价轨迹
DWBLocalPlanner::transformGlobalPlan	转换全局路径	pose: 当前位姿

评价函数扩展接口

class TrajectoryCritic {
public:
  // 初始化函数
  virtual void initialize(
    const nav2::LifecycleNode::WeakPtr & node,
    std::string name, std::string parent_namespace,
    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) = 0;
  
  // 轨迹评价函数
  virtual double scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) = 0;
  
  // 获取评价函数名称
  virtual std::string getName() const = 0;
  
  // 获取评价函数权重
  virtual double getScale() const = 0;
  
  // ...其他接口
};

通过这些API，开发者可以灵活扩展Navigation2的路径规划功能，实现自定义的路径规划算法和评价策略。