前言

上篇我们搭建了系统整体架构，选用了Cartographer激光SLAM为主建图方案，并通过EKF融合了轮式里程计、视觉里程计和IMU，为导航提供了稳定可靠的定位信息。本篇将在此基础之上，深入配置Nav2导航栈，包括AMCL定位、MPPI控制器、SmacPlanner规划器以及行为树。同时，利用IQ-9100的NPU加速实现动态障碍物检测与轨迹预测，并将其注入代价地图，实现具有预见性的动态避障能力。

1. Nav2 导航栈深度配置

Nav2导航栈整体架构与数据流：

图中展示了Nav2导航栈的核心模块及其数据交互关系，整体可分为六个层次：

1. 用户输入：通过RVIZ或API设置目标点（/initialpose），触发导航任务。
2. 行为树（BT Navigator）：负责任务编排，如navigate_to_pose，管理正常流程与异常恢复。
3. 全局规划层（Planner Server）：
   • 使用SmacPlannerHybrid规划全局路径。
   • 依赖全局代价地图（静态层+障碍物层+膨胀层），该地图基于激光雷达/scan构建。
4. 局部规划与控制层（Controller Server）：
   • 核心是MPPIController，通过一组Critics（评估器） 对候选轨迹打分。
   • 使用局部代价地图（体素层+AI障碍物层+膨胀层），其中AI障碍物点云/ai_obstacles来自NPU检测节点。
5. 传感器与定位：
   • AMCL提供全局定位/amcl_pose，robot_localization的EKF输出融合里程计/odometry/filtered。
   • 激光雷达和AI障碍物分别喂给全局/局部代价地图。
6. 执行层：最终输出速度指令/cmd_vel给机器人底盘。

一句话总结：用户目标经行为树调度，由全局规划器生成路径，局部控制器结合实时传感器与动态障碍物信息，输出平滑的控制指令。

1.1 Nav2 参数配置

# config/nav2_params.yaml
# Nav2 导航栈完整配置 (针对IQ-9100室内机器人优化)

amcl:
  ros__parameters:
    alpha1: 0.2
    alpha2: 0.2
    alpha3: 0.2
    alpha4: 0.2
    alpha5: 0.2
    base_frame_id: "base_link"
    beam_skip_distance: 0.5
    beam_skip_error_threshold: 0.9
    beam_skip_threshold: 0.3
    do_beams_skip: false
    global_frame_id: "map"
    lambda_short: 0.1
    laser_likelihood_max_dist: 2.0
    laser_max_range: 12.0
    laser_min_range: 0.2
    laser_model_type: "likelihood_field"
    max_beams: 60
    max_particles: 2000
    min_particles: 500
    odom_frame_id: "odom"
    pf_err: 0.05
    pf_z: 0.99
    recovery_alpha_fast: 0.1
    recovery_alpha_slow: 0.001
    resample_interval: 1
    robot_model_type: "nav2_amcl::DifferentialMotionModel"
    save_pose_rate: 0.5
    sigma_hit: 0.2
    tf_broadcast: true
    transform_tolerance: 1.0
    update_min_a: 0.2
    update_min_d: 0.25
    z_hit: 0.5
    z_max: 0.05
    z_rand: 0.5
    z_short: 0.05
    scan_topic: scan

bt_navigator:
  ros__parameters:
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odometry/filtered
    bt_loop_duration: 10
    default_server_timeout: 20
    navigators: ["navigate_to_pose", "navigate_through_poses"]
    navigate_to_pose:
      plugin: "nav2_bt_navigator::NavigateToPoseNavigator"
    navigate_through_poses:
      plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator"
    error_code_names:
      - compute_path_error_code
      - follow_path_error_code

controller_server:
  ros__parameters:
    controller_frequency: 20.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    progress_checker_plugins: ["progress_checker"]
    goal_checker_plugins: ["general_goal_checker"]
    controller_plugins: ["FollowPath"]

    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0

    general_goal_checker:
      plugin: "nav2_controller::SimpleGoalChecker"
      stateful: true
      xy_goal_tolerance: 0.15
      yaw_goal_tolerance: 0.1

    FollowPath:
      plugin: "nav2_mppi_controller::MPPIController"
      time_steps: 56
      model_dt: 0.05
      batch_size: 2000
      vx_std: 0.2
      vy_std: 0.0
      wz_std: 0.4
      vx_max: 0.8
      vx_min: -0.35
      vy_max: 0.0
      wz_max: 1.5
      iteration_count: 1
      prune_distance: 1.7
      transform_tolerance: 0.1
      temperature: 0.3
      gamma: 0.015
      motion_model: "DiffDrive"
      visualize: true
      retry_attempt_limit: 1
      critics: [
        "ConstraintCritic", "CostCritic", "GoalCritic",
        "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic",
        "PathAngleCritic", "PreferForwardCritic", "ObstaclesCritic"
      ]
      ConstraintCritic:
        enabled: true
        cost_weight: 4.0
        cost_power: 1
      GoalCritic:
        enabled: true
        cost_weight: 5.0
        cost_power: 1
        threshold_to_consider: 1.4
      GoalAngleCritic:
        enabled: true
        cost_weight: 3.0
        cost_power: 1
        threshold_to_consider: 0.5
      ObstaclesCritic:
        enabled: true
        cost_weight: 1.0
        consider_footprint: true
        collision_margin_distance: 0.1
        near_goal_distance: 0.5
      CostCritic:
        enabled: true
        cost_weight: 3.0
        cost_power: 1
        critical_cost: 300.0
      PathAlignCritic:
        enabled: true
        cost_weight: 14.0
        cost_power: 1
        max_path_occupancy_ratio: 0.05
        threshold_to_consider: 0.5
      PathFollowCritic:
        enabled: true
        cost_weight: 5.0
        offset_from_furthest: 5
        threshold_to_consider: 1.4
      PreferForwardCritic:
        enabled: true
        cost_weight: 5.0
        cost_power: 1
        threshold_to_consider: 0.5

planner_server:
  ros__parameters:
    expected_planner_frequency: 10.0
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_smac_planner::SmacPlannerHybrid"
      tolerance: 0.25
      downsampling_factor: 1
      allow_unknown: true
      max_iterations: 1000000
      max_on_approach_iterations: 1000
      max_planning_time: 2.0
      motion_model_for_search: "DUBIN"
      angle_quantization_bins: 72
      analytic_expansion_ratio: 3.5
      analytic_expansion_max_length: 3.0
      minimum_turning_radius: 0.20
      cost_travel_multiplier: 2.0
      cost_penalty: 2.0
      lookup_table_size: 20.0
      cache_obstacle_heuristic: false
      smooth_path: true

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 10.0
      publish_frequency: 5.0
      global_frame: odom
      robot_base_frame: base_link
      rolling_window: true
      width: 5
      height: 5
      resolution: 0.05
      robot_radius: 0.25
      plugins: ["voxel_layer", "ai_obstacle_layer", "inflation_layer"]

      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: true
        publish_voxel_map: false
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: true
          marking: true
          data_type: "LaserScan"
          raytrace_max_range: 12.0
          raytrace_min_range: 0.2
          obstacle_max_range: 10.0
          obstacle_min_range: 0.2

      ai_obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: true
        observation_sources: ai_obstacles
        ai_obstacles:
          topic: /ai_obstacles
          data_type: "PointCloud2"
          marking: true
          clearing: false
          max_obstacle_height: 2.0

      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 2.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      robot_radius: 0.25
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]

      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: true

      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: true
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: true
          marking: true
          data_type: "LaserScan"

      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55

其中，MPPI（Model Predictive Path Integral）控制器Critics评估器组成如图，帮助理解每个Critic的作用。

MPPI控制器通过随机采样多条轨迹，并使用一组Critics（评估器） 对每条轨迹进行代价评分，最终选出综合代价最低的轨迹执行。

图中9个Critics分为三类：

• 运动学与安全性：

  • ConstraintCritic：检查运动学约束（速度、加速度是否超出限制）。
  • ObstaclesCritic：评估与障碍物的碰撞风险，设置安全边距。
  • CostCritic：综合代价地图的代价值（如接近膨胀层区域扣分）。

• 路径跟踪与对齐：

  • PathAlignCritic：鼓励轨迹与全局路径对齐。
  • PathFollowCritic：惩罚偏离路径的行为。
  • PathAngleCritic：检查轨迹朝向是否与路径角度一致。

• 目标达成与前进偏好：

  • GoalCritic：奖励接近目标的轨迹。
  • GoalAngleCritic：确保接近目标时朝向正确。
  • PreferForwardCritic：鼓励前进而非原地旋转或倒车。

一句话总结：MPPI控制器将安全、路径跟踪、目标趋近等约束转化为量化代价，综合加权后选出最平滑、安全且高效的局部轨迹。

1.2 NPU加速的动态障碍物检测节点

传统代价地图只处理静态/准静态障碍物，但真实环境中行人、推车等动态目标需要AI理解。 下面是NPU动态障碍物检测与预测节点处理流程：

# navigation/dynamic_obstacle_node.py
# NPU加速的动态障碍物检测与轨迹预测
# 发布到Nav2代价地图的AI障碍物层

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image, PointCloud2, PointField
from vision_msgs.msg import Detection2DArray
from geometry_msgs.msg import Point
from std_msgs.msg import Header
from cv_bridge import CvBridge
import numpy as np
import struct
import time
import math
from collections import deque
from dataclasses import dataclass, field
from typing import Dict, List, Tuple

@dataclass
class DynamicObject:
    """动态障碍物跟踪状态"""
    obj_id: int
    label: str
    position: np.ndarray   # [x, y] in base_link
    velocity: np.ndarray   # [vx, vy] 估计速度
    positions_history: deque
    last_seen: float
    predicted_path: List[np.ndarray] = field(default_factory=list)

class DynamicObstacleNode(Node):
    """
    动态障碍物检测 + 轨迹预测 → 注入代价地图
    利用NPU检测人/车/推车等动态物体，预测其未来2秒轨迹，在代价地图中标记预测区域
    """

    DYNAMIC_CLASSES = {0: "person", 1: "bicycle", 2: "car", 3: "motorcycle", 5: "bus", 7: "truck"}

    def __init__(self):
        super().__init__('dynamic_obstacle')
        self.declare_parameter('prediction_horizon', 2.0)
        self.declare_parameter('prediction_dt', 0.2)
        self.declare_parameter('safety_radius_person', 0.5)
        self.declare_parameter('safety_radius_vehicle', 1.0)
        self.declare_parameter('focal_length', 500.0)
        self.declare_parameter('camera_height', 0.3)

        self.pred_horizon = self.get_parameter('prediction_horizon').value
        self.pred_dt = self.get_parameter('prediction_dt').value

        self._tracked_objects: Dict[int, DynamicObject] = {}
        self._next_id = 0

        self.create_subscription(Detection2DArray, '/cam0/detections', self._detection_callback, 10)
        self.create_subscription(Detection2DArray, '/cam1/detections', self._detection_callback, 10)

        self.obstacle_pub = self.create_publisher(PointCloud2, '/ai_obstacles', 10)
        self.create_timer(0.1, self._prediction_loop)   # 10Hz

        self.get_logger().info('Dynamic obstacle node started')

    def _detection_callback(self, msg: Detection2DArray):
        """处理AI检测结果，转为3D位置并更新跟踪"""
        now = time.time()
        focal = self.get_parameter('focal_length').value
        cam_h = self.get_parameter('camera_height').value

        for det in msg.detections:
            if not det.results:
                continue
            class_id = int(det.results[0].hypothesis.class_id)
            if class_id not in self.DYNAMIC_CLASSES:
                continue

            score = det.results[0].hypothesis.score
            if score < 0.4:
                continue

            bbox_h = det.bbox.size_y
            bbox_cx = det.bbox.center.position.x
            bbox_bottom_y = det.bbox.center.position.y + bbox_h / 2

            real_heights = {0: 1.7, 1: 1.0, 2: 1.5, 3: 1.0, 5: 3.0, 7: 3.0}
            real_h = real_heights.get(class_id, 1.5)
            depth = (real_h * focal) / max(bbox_h, 10)
            depth = min(max(depth, 0.5), 10.0)

            img_cx = 320.0
            angle = math.atan2(bbox_cx - img_cx, focal)
            x = depth * math.cos(angle)
            y = depth * math.sin(angle)
            pos = np.array([x, y])

            self._update_tracking(pos, class_id, now)

    def _update_tracking(self, position: np.ndarray, class_id: int, timestamp: float):
        """更新目标跟踪（最近邻匹配）"""
        best_match = None
        best_dist = 1.5

        for oid, obj in self._tracked_objects.items():
            dist = np.linalg.norm(position - obj.position)
            if dist < best_dist:
                best_dist = dist
                best_match = oid

        if best_match is not None:
            obj = self._tracked_objects[best_match]
            dt = timestamp - obj.last_seen
            if dt > 0.01:
                obj.velocity = 0.7 * obj.velocity + 0.3 * (position - obj.position) / dt
            obj.position = position
            obj.last_seen = timestamp
            obj.positions_history.append(position.copy())
        else:
            self._tracked_objects[self._next_id] = DynamicObject(
                obj_id=self._next_id,
                label=self.DYNAMIC_CLASSES.get(class_id, "unknown"),
                position=position,
                velocity=np.zeros(2),
                positions_history=deque([position.copy()], maxlen=30),
                last_seen=timestamp,
            )
            self._next_id += 1

        expired = [oid for oid, obj in self._tracked_objects.items() if timestamp - obj.last_seen > 2.0]
        for oid in expired:
            del self._tracked_objects[oid]

    def _prediction_loop(self):
        """预测动态物体未来轨迹，发布为代价地图点云"""
        if not self._tracked_objects:
            return

        obstacle_points = []
        steps = int(self.pred_horizon / self.pred_dt)

        for obj in self._tracked_objects.values():
            speed = np.linalg.norm(obj.velocity)
            if speed < 0.1:
                obstacle_points.append((obj.position[0], obj.position[1], 0.0))
                continue

            obj.predicted_path = []
            for t in range(steps):
                dt = (t + 1) * self.pred_dt
                pred_pos = obj.position + obj.velocity * dt
                obj.predicted_path.append(pred_pos)

                if obj.label == "person":
                    radius = self.get_parameter('safety_radius_person').value
                else:
                    radius = self.get_parameter('safety_radius_vehicle').value

                for angle in np.linspace(0, 2*math.pi, 8, endpoint=False):
                    px = pred_pos[0] + radius * math.cos(angle)
                    py = pred_pos[1] + radius * math.sin(angle)
                    obstacle_points.append((px, py, 0.0))

        if obstacle_points:
            cloud_msg = self._build_pointcloud(obstacle_points)
            self.obstacle_pub.publish(cloud_msg)

    def _build_pointcloud(self, points: list) -> PointCloud2:
        """构建PointCloud2消息"""
        header = Header()
        header.stamp = self.get_clock().now().to_msg()
        header.frame_id = "base_link"

        fields = [
            PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
            PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
            PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
        ]

        point_data = b''
        for x, y, z in points:
            point_data += struct.pack('fff', x, y, z)

        msg = PointCloud2()
        msg.header = header
        msg.height = 1
        msg.width = len(points)
        msg.fields = fields
        msg.is_bigendian = False
        msg.point_step = 12
        msg.row_step = 12 * len(points)
        msg.data = point_data
        msg.is_dense = True
        return msg

def main(args=None):
    rclpy.init(args=args)
    node = DynamicObstacleNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

下篇预告
Nav2导航栈与NPU动态避障已经就绪，下一篇我们将进入真正的实战环节：使用Cartographer完成建图、启动完整导航系统、通过Nav2 SimpleCommander API实现单点导航和多点巡逻，并加入定位失效自动恢复策略，保障长期运行的鲁棒性。敬请关注！