Navigation框架不足分析

Navigation框架在移动机器人中是比较常用的框架，在使用过程中发现框架本身存在了一些问题，包括组件高耦合、单体类职责过多、硬编码状态机、反馈信息贫乏、恢复机制不灵活、接口陈旧等问题，对Navigation框架进行分析和总结。

Navigation (Noetic版本) 整体功能包

navigation (metapackage)
├─ amcl                      # 定位
├─ base_local_planner        # 基础局部规划器
├─ carrot_planner            # 简单全局规划器
├─ clear_costmap_recovery    # 恢复行为
├─ costmap_2d                # 代价地图
├─ dwa_local_planner         # DWA 局部规划器
├─ fake_localization         # 假定位
├─ global_planner            # 全局规划器
├─ map_server                # 地图服务器
├─ move_base                 # 核心协调器
├─ move_base_msgs            # 消息定义
├─ move_slow_and_clear       # 恢复行为
├─ navfn                     # Dijkstra 规划器
├─ nav_core                  # 接口定义
├─ rotate_recovery           # 恢复行为
└─ voxel_grid                # 体素网格

1、组件间耦合度高

内部耦合度高，难以替换单个组件。

高耦合组件：
move_base
├─→ costmap_2d (强耦合)
├─→ nav_core (接口)
├─→ base_local_planner (强制)
├─→ navfn (强制)
├─→ clear_costmap_recovery (强制)
└─→ rotate_recovery (强制)

base_local_planner
├─→ costmap_2d (强耦合)
├─→ nav_core (接口)
└─→ voxel_grid (强耦合)

dwa_local_planner
├─→ base_local_planner (继承耦合)
├─→ costmap_2d (强耦合)
└─→ nav_core (接口)

navfn
├─→ costmap_2d (强耦合)
└─→ nav_core (接口)

global_planner
├─→ costmap_2d (强耦合)
├─→ navfn (依赖)
└─→ nav_core (接口)


move_base 依赖图简化：
        move_base
           ↓
    ┌──────┴──────┐
    ↓             ↓
costmap_2d    nav_core
    ↓             ↓
voxel_grid   planners
                  ↓
              costmap_2d (循环)

2、单体类职责过多

无分层设计，职责过多；成员变量过多，难以理解和维护。

move_base 类，进行状态机管理、线程管理、规划器管理、代价地图管理、恢复行为管理、Action 服务器管理。

class MoveBase {
private:
  // 状态机
  MoveBaseState state_;  // PLANNING, CONTROLLING, CLEARING
  RecoveryTrigger recovery_trigger_;
  
  // 规划器（直接持有）
  boost::shared_ptr<nav_core::BaseLocalPlanner> tc_;
  boost::shared_ptr<nav_core::BaseGlobalPlanner> planner_;
  
  // 代价地图（直接持有）
  costmap_2d::Costmap2DROS* planner_costmap_ros_;
  costmap_2d::Costmap2DROS* controller_costmap_ros_;
  
  // 恢复行为（直接持有）
  std::vector<boost::shared_ptr<nav_core::RecoveryBehavior>> recovery_behaviors_;
  
  // 线程管理
  boost::thread* planner_thread_;
  boost::recursive_mutex planner_mutex_;
  boost::condition_variable_any planner_cond_;
  
  // 路径缓冲
  std::vector<geometry_msgs::PoseStamped>* planner_plan_;
  std::vector<geometry_msgs::PoseStamped>* latest_plan_;
  std::vector<geometry_msgs::PoseStamped>* controller_plan_;
  
  // ... 50+ 个成员变量
};

**costmap_2d **

costmap_2d 混合了多种职责，除了数据结构、消息定义和TF，还有传感器数据处理还有laser_geometry,、sensor_msgs、message_filters等。

<depend>dynamic_reconfigure</depend>
<depend>eigen</depend>
<depend>geometry_msgs</depend>
<depend>laser_geometry</depend>
<depend>map_msgs</depend>
<depend>message_filters</depend>
<depend>nav_msgs</depend>
<depend>pluginlib</depend>
<depend>roscpp</depend>
<depend>rostest</depend>
<depend>sensor_msgs</depend>
<depend>std_msgs</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>visualization_msgs</depend>
<depend>voxel_grid</depend>

3、多线程混乱导致的同步问题

move_base 的线程模型非常复杂且难以控制，规划器需要频繁加锁访问代价地图，代价地图更新时持有锁，阻塞规划器，可能导致规划器使用过时的代价地图数据，频繁的锁竞争，可能导致碰撞。

导航流程：Global Costmap -> Planner -> Local Costmap -> Planner -> Command Velocities
一旦全局规划线程（Global Planner）计算出路径，它会立即、自动地将路径交换给局部规划器（Local Planner）所在的控制线程。
    
	[主线程 / Control Thread]                      [规划线程 / Planner Thread]
               |                                              |
      1. 收到目标 (executeCb)                                  | 挂起等待 (wait)
               |                                              |
      2. 唤醒规划线程 (notify) ------------------------------>  | 醒来
               |                                              |
      3. 进入控制循环 (executeCycle)                            | 4. 锁定全局地图
         (20Hz)                                               |    运行 makePlan
               |                                              |    
               |                                              |
               |                                              | 5. 规划完成
               |                                              |    锁定 planner_mutex_
               |                                              |    交换指针: planner_plan -> latest_plan
               | <----------(new_global_plan_ = true)-------- |    解锁
               |                                              |    继续挂起或再次规划
               |
      6. 发现新路径标志
         锁定 planner_mutex_
         交换指针: latest_plan -> controller_plan_
         解锁
               |
      7. 锁定局部地图
         运行 computeVelocityCommands
         发布 cmd_vel


// move_base/src/move_base.cpp
void MoveBase::executeCb(...) {
  // 主线程
  ros::Rate r(controller_frequency_);
  
  while(n.ok()) {
    // 访问代价地图（需要加锁）
    boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(
      *(planner_costmap_ros_->getCostmap()->getMutex())
    );
    
    // 全局规划（可能需要几百毫秒）
    planner_->makePlan(start, goal, global_plan_);
    
    lock.unlock();  // 释放锁
    
    // 局部规划（又需要加锁）
    boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock2(
      *(controller_costmap_ros_->getCostmap()->getMutex())
    );
    
    controller_->computeVelocityCommands(cmd_vel);
  }
}

// 代价地图更新线程
void Costmap2DROS::mapUpdateLoop(double frequency) {
  ros::Rate r(frequency);  // 例如 5Hz
  
  while (ros::ok() && !stop_updates_) {
    // 获取锁
    boost::unique_lock<boost::recursive_mutex> lock(*(layered_costmap_->getCostmap()->getMutex()));
    
    // 更新代价地图（可能需要几十毫秒）
    layered_costmap_->updateMap(x, y, yaw);
    
    lock.unlock();
    
    r.sleep();
  }
}


//多线程协调有时出现7秒左右的延迟（github仓库提交的问题）
时间轴：
T0: 传感器检测到障碍物
T1: 代价地图更新线程处理数据（可能延迟）
T2: 局部规划器读取代价地图（可能读到旧数据）
T3: 机器人执行速度命令（可能已经撞上障碍物）

4、move_base 硬编码状态机

在标准 move_base 中，导航逻辑被硬编码为一个固定的有限状态机（FSM）：PLANNING -> CONTROLLING -> CLEARING（恢复），限制了机器人的复杂行为。

无法插入自定义逻辑，比如"规划前先旋转"、"失败后等待一段时间"等。

// move_base/src/move_base.cpp
void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal) {
  // 状态机完全硬编码
  ros::Rate r(controller_frequency_);
  
  while(n.ok()) {
    if(as_->isPreemptRequested()) {
      // 处理抢占
      as_->setPreempted();
      return;
    }

    // 固定流程：全局规划 → 局部规划 → 恢复行为
    if(runPlanner_) {
      boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(planner_costmap_ros_->getCostmap()->getMutex()));
      
      if(!planner_->makePlan(start, goal, global_plan_)) {
        // 规划失败 → 执行恢复行为
        state_ = CLEARING;
      }
    }

    if(state_ == CONTROLLING) {
      if(!controller_->computeVelocityCommands(cmd_vel)) {
        // 控制失败 → 执行恢复行为
        state_ = CLEARING;
      }
    }

    if(state_ == CLEARING) {
      // 固定的恢复行为序列
      if(recovery_index_ < recovery_behaviors_.size()) {
        recovery_behaviors_[recovery_index_]->runBehavior();
        recovery_index_++;
      } else {
        // 所有恢复行为都失败
        as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid plan");
        return;
      }
    }
  }
}

5、 move_base 反馈贫乏

move_base 的 Action 接口反馈非常有限，通常只返回当前的位置（base position），无法知道进度、当前状态详情、剩余距离。

# move_base_msgs/MoveBase.action

geometry_msgs/PoseStamped target_pose
---
---
geometry_msgs/PoseStamped base_position

6、恢复行为机制不灵活

无论何种失败都执行相同序列 → 清除、旋转、清除、旋转
无法根据失败类型定制
可能执行无效的恢复动作

void MoveBase::loadDefaultRecoveryBehaviors() {
  recovery_behaviors_.clear();
  try {
    // 1. 保守清除代价地图
    boost::shared_ptr<nav_core::RecoveryBehavior> cons_clear(
      recovery_loader_.createInstance("clear_costmap_recovery/ClearCostmapRecovery"));
    cons_clear->initialize("conservative_reset", &tf_, planner_costmap_ros_, controller_costmap_ros_);
    recovery_behaviors_.push_back(cons_clear);
    
    // 2. 原地旋转
    boost::shared_ptr<nav_core::RecoveryBehavior> rotate(
      recovery_loader_.createInstance("rotate_recovery/RotateRecovery"));
    if(clearing_rotation_allowed_) {
      rotate->initialize("rotate_recovery", &tf_, planner_costmap_ros_, controller_costmap_ros_);
      recovery_behaviors_.push_back(rotate);
    }
    
    // 3. 激进清除代价地图
    boost::shared_ptr<nav_core::RecoveryBehavior> ags_clear(
      recovery_loader_.createInstance("clear_costmap_recovery/ClearCostmapRecovery"));
    ags_clear->initialize("aggressive_reset", &tf_, planner_costmap_ros_, controller_costmap_ros_);
    recovery_behaviors_.push_back(ags_clear);
    
    // 4. 再次原地旋转
    if(clearing_rotation_allowed_)
      recovery_behaviors_.push_back(rotate);
  } catch(...) { ... }
}

7、nav_core 接口设计陈旧

nav_core 近十年无更新，nav_core 定义的接口（makePlan, computeVelocityCommands）返回值通常只是简单的 bool值（成功/失败），返回 false 时，无法知道失败原因，是没有路径？是检测到障碍物？是速度限制？还是算法内部错误？

通过引用参数返回路径，不符合现代 C++ 设计。

// nav_core::BaseGlobalPlanner
bool makePlan(
    const PoseStamped& start,
    const PoseStamped& goal,
    std::vector<PoseStamped>& plan  // 输出参数
);

8、nav_core魔法数字

COST_NEUTRAL：值为 50，但为什么是 50？注释缺失

COST_FACTOR：代价缩放因子，计算逻辑不透明

COST_DIAGONAL：对角线移动代价，硬编码为 sqrt(2)

难以理解的魔法数字，难以阅读和修改，另外在某些情况下会产生不安全的路径。

// navfn/src/navfn.cpp
bool NavFn::calcNavFnDijkstra(bool atStart) {
  setupNavFn(true);

  int cycle = 0;
  int startCell = getIndex(start[0], start[1]);
  
  for (int i = 0; i < ns; i++) {
    costarr[i] = COST_NEUTRAL + COST_FACTOR * costmap[i];  // 魔法公式
  }

  // 硬编码的代价计算
  float cost = costarr[c] + COST_NEUTRAL;  // 为什么是 COST_NEUTRAL？
  float dist = (abs(dx) > abs(dy)) ? 
    (float)abs(dx) + COST_DIAGONAL * (float)abs(dy) :  // 魔法常数
    (float)abs(dy) + COST_DIAGONAL * (float)abs(dx);
  
  float dnew = pc + cost + dist;  // 组合公式难以理解
}

9、强制使用 Layers

costmap_2d 强制使用“层（Layers）”架构，但有时用户只需要一个简单的网格。

// costmap_2d/costmap_2d_ros.cpp
class Costmap2DROS {
public:
  Costmap2DROS(std::string name, tf::TransformListener& tf) {
    // 强制创建 LayeredCostmap
    layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window_, track_unknown_space_);
    
    // 必须通过插件系统加载 layers
    if (!loadPlugins()) {
      ROS_ERROR("Failed to load costmap plugins");
    }
  }

private:
  LayeredCostmap* layered_costmap_;  // 不能为空
  std::vector<boost::shared_ptr<Layer>> plugins_;  // 必须有至少一个 layer
};

10、OccupancyGrid 数据丢失问题

256 个唯一值被压缩到 101 个值（-1 到 100），中间的 252 个值（1-252）被映射到 98 个值（1-98），更丰富的信息（如不同的障碍物类型或膨胀层级），但在传输过程中精度被丢失了。

地图数据死板地绑定为 unsigned char 数组，并强依赖于 ROS 消息格式，只能存 0-255 的整数，无法直接存储浮点数（如距离场、概率值）或自定义结构体。

原始代价值: [0, 1, 2, ..., 252, 253, 254, 255]
            ↓ 转换
发布值:     [0, 1, 1, ..., 98,  99,  100, -1]
            ↑ 多个原始值映射到同一个发布值

// costmap_2d/src/costmap_2d_publisher.cpp
char* Costmap2DPublisher::cost_translation_table_ = NULL;

void Costmap2DPublisher::prepareGrid() {
  // 初始化转换表
  if (cost_translation_table_ == NULL) {
    cost_translation_table_ = new char[256];
    
    // 特殊值映射
    cost_translation_table_[0] = 0;      // FREE_SPACE
    cost_translation_table_[253] = 99;   // INSCRIBED_INFLATED_OBSTACLE
    cost_translation_table_[254] = 100;  // LETHAL_OBSTACLE
    cost_translation_table_[255] = -1;   // NO_INFORMATION
    
    // 中间值线性映射到 [1, 98]
    for (int i = 1; i < 253; i++) {
      cost_translation_table_[i] = char(1 + (97 * (i - 1)) / 251);
    }
  }

  // 转换代价地图数据
  unsigned int size = costmap_->getSizeInCellsX() * costmap_->getSizeInCellsY();
  grid_.data.resize(size);
  unsigned char* data = costmap_->getCharMap();
  for (unsigned int i = 0; i < size; i++) {
    grid_.data[i] = cost_translation_table_[data[i]];
  }
}

11、6DOF 位姿冗余

强制使用 3D 数据结构，标准导航栈强制使用 geometry_msgs/PoseStamped，即使是 2D 地面机器人也必须携带 Z 轴坐标和四元数（Quaternion）。

简单的角度计算需要反复进行“欧拉角 <-> 四元数”转换，不仅繁琐而且消耗算力。（在 10Hz 控制频率下，1000 次姿态操作累计需 23 毫秒）

// geometry_msgs/PoseStamped
struct Pose {
    Point position;    // x, y, z  <- z 冗余
    Quaternion orientation;  // x, y, z, w  <- 只需要 yaw
};

// 即使是 2D 导航也用 3D 数据
geometry_msgs::PoseStamped goal;
goal.pose.position.x = 5.0;
goal.pose.position.y = 3.0;
goal.pose.position.z = 0.0;  // 永远是 0，但必须存在
goal.pose.orientation = tf::createQuaternionMsgFromYaw(1.57);  // 四元数转换开销

// 额外计算：每次访问都需要四元数转换
double yaw = tf::getYaw(pose.pose.orientation);

12、局部规划器的调试困难

参数较多，30+ 个参数，很难调整其评分标准（Critics）或理解其为何选择某条轨迹，调参困难。

base_local_planner 参数示例：
├─ max_vel_x, min_vel_x
├─ max_vel_theta, min_vel_theta
├─ acc_lim_x, acc_lim_theta
├─ xy_goal_tolerance, yaw_goal_tolerance
├─ sim_time, sim_granularity
├─ path_distance_bias, goal_distance_bias
├─ occdist_scale
└─ ...

参考资料

http://wiki.ros.org/navigation

https://github.com/ros-planning/navigation

robot

#导航

Navigation框架不足分析

http://example.com/2026/03/30/Navigation框架不足分析/

作者

EthanYLiang

发布于

2026年3月30日

更新于

2026年3月30日

许可协议

四轮全向底底盘运动学分析下一篇