机器人的端侧AI：从ROS2节点到SLAM导航的实时推理

一、场景痛点与技术挑战

移动机器人需要实时感知与决策能力。
云端推理延迟高达数百毫秒。
网络波动导致指令丢失和响应中断。
紧急避障场景不允许任何网络依赖。
端侧AI是机器人自主性的技术基础。

核心痛点有四个。
一是端侧算力受限。
嵌入式平台GPU/NPU算力仅2-8 TOPS。
SLAM算法需要处理30Hz的激光雷达点云。
深度模型推理与SLAM算法竞争CPU资源。
二是实时性要求严格。
避障决策必须在50ms内完成。
路径规划每100ms更新一次。
超时决策等于机器人失控。
三是模型轻量化与精度矛盾。
INT8量化后SLAM特征提取精度下降。
关键特征点丢失导致定位漂移。
四是ROS2与AI推理框架集成困难。
ROS2节点间的通信是C++ DDS。
AI推理框架用Python/ONNX Runtime。
语言边界和数据拷贝是性能瓶颈。

二、核心原理与架构设计

端侧AI系统架构分四个核心模块。

感知模块负责多传感器融合。
激光雷达提供360度距离信息。
摄像头提供视觉语义信息。
IMU提供加速度和角速度数据。
传感器数据在ROS2节点间通过topic传递。

SLAM模块负责定位与建图。
激光雷达SLAM用Cartographer算法。
视觉SLAM用ORB-SLAM3算法。
IMU辅助校正里程计漂移。
输出机器人当前位置和已建地图。

AI推理模块负责目标检测和语义理解。
YOLOv8-nano做实时目标检测。
推理延迟<15ms满足实时要求。
检测结果通过ROS2 topic发布。
路径规划模块消费检测结果做避障决策。

路径规划模块基于Nav2框架。
全局规划用NavFn算法。
局部规划用DWB控制器。
动态避障用Costmap2D层叠加。
静态地图层+动态障碍层+膨胀层。

SLAM算法选型策略。
室外大场景用Cartographer。
室内小场景用Gmapping。
视觉+IMU融合用ORB-SLAM3。
计算资源紧张时退化为AMCL纯定位。

AI模型轻量化策略。
YOLOv8-nano参数量仅3.2M。
INT8量化后模型体积缩小4倍。
推理延迟从30ms降到12ms。
关键层保持FP16精度避免特征丢失。

三、生产级代码实现

ROS2 SLAM节点实现

"""ROS2 SLAM导航节点"""
import rclpy
from rclpy.node import Node
from nav_msgs.msg import OccupancyGrid, Odometry
from sensor_msgs.msg import LaserScan, Image, Imu
from geometry_msgs.msg import Twist, PoseStamped
from std_msgs.msg import Header
import numpy as np
import math

class SLAMNode(Node):
    """SLAM定位与建图节点"""

    def __init__(self):
        super().__init__('slam_node')

        # 订阅传感器数据
        self.lidar_sub = self.create_subscription(
            LaserScan, '/scan', self.lidar_callback, 10)
        self.imu_sub = self.create_subscription(
            Imu, '/imu/data', self.imu_callback, 10)

        # 发布定位和地图
        self.odom_pub = self.create_publisher(Odometry, '/odom', 10)
        self.map_pub = self.create_publisher(OccupancyGrid, '/map', 10)

        # 状态变量
        self.current_pose = np.array([0.0, 0.0, 0.0])  # x, y, theta
        self.map_grid = np.zeros((200, 200), dtype=np.int8)
        self.last_scan = None
        self.imu_orientation = 0.0

        # 定位更新频率30Hz
        self.timer = self.create_timer(0.033, self.update_loop)

    def lidar_callback(self, msg: LaserScan):
        """激光雷达数据处理"""
        ranges = np.array(msg.ranges)
        # 过滤无效数据
        ranges = np.where(ranges < msg.range_max, ranges, np.inf)
        ranges = np.where(ranges > msg.range_min, ranges, np.inf)
        self.last_scan = ranges

        # 更新地图（简化：Bresenham射线投射）
        angle_min = msg.angle_min
        angle_inc = msg.angle_increment
        for i, dist in enumerate(ranges):
            if dist == np.inf or dist < 0.1:
                continue
            angle = angle_min + i * angle_inc + self.current_pose[2]
            end_x = self.current_pose[0] + dist * math.cos(angle)
            end_y = self.current_pose[1] + dist * math.sin(angle)
            # 标记障碍点
            gx = int(end_x * 5 + 100)  # 0.2m分辨率映射到网格
            gy = int(end_y * 5 + 100)
            if 0 <= gx < 200 and 0 <= gy < 200:
                self.map_grid[gy, gx] = 100  # 障碍物

    def imu_callback(self, msg: Imu):
        """IMU数据辅助定位"""
        # 从四元数提取偏航角
        q = msg.orientation
        self.imu_orientation = math.atan2(
            2.0 * (q.w * q.z + q.x * q.y),
            1.0 - 2.0 * (q.y * q.y + q.z * q.z)
        )

    def update_loop(self):
        """30Hz定位更新循环"""
        if self.last_scan is None:
            return

        # IMU辅助校正姿态
        self.current_pose[2] = self.imu_orientation

        # 发布里程计
        odom = Odometry()
        odom.header = Header(stamp=self.get_clock().now().to_msg())
        odom.header.frame_id = 'odom'
        odom.child_frame_id = 'base_link'
        odom.pose.pose.position.x = self.current_pose[0]
        odom.pose.pose.position.y = self.current_pose[1]
        # 角度转四元数
        odom.pose.pose.orientation.z = math.sin(self.current_pose[2] / 2)
        odom.pose.pose.orientation.w = math.cos(self.current_pose[2] / 2)
        self.odom_pub.publish(odom)

        # 发布地图（降低频率，每3帧发布一次）
        if self.map_pub.get_subscription_count() > 0:
            map_msg = OccupancyGrid()
            map_msg.header = Header(stamp=self.get_clock().now().to_msg())
            map_msg.header.frame_id = 'map'
            map_msg.info.resolution = 0.2
            map_msg.info.width = 200
            map_msg.info.height = 200
            map_msg.info.origin.position.x = -20.0
            map_msg.info.origin.position.y = -20.0
            map_msg.data = self.map_grid.flatten().tolist()
            self.map_pub.publish(map_msg)

class ObjectDetectNode(Node):
    """目标检测节点：ONNX Runtime推理"""

    def __init__(self, model_path: str):
        super().__init__('object_detect_node')

        self.cam_sub = self.create_subscription(
            Image, '/camera/image_raw', self.detect_callback, 5)
        self.detect_pub = self.create_publisher(
            PoseStamped, '/detected_objects', 10)

        # ONNX Runtime初始化
        import onnxruntime as ort
        sess_opts = ort.SessionOptions()
        sess_opts.graph_optimization_level = ort.GraphOptimizationLevel.ORT_ENABLE_ALL
        self.session = ort.InferenceSession(
            model_path, sess_opts,
            providers=['CUDAExecutionProvider', 'CPUExecutionProvider']
        )
        self.input_name = self.session.get_inputs()[0].name
        self.get_logger().info(f"模型加载完成: {model_path}")

    def detect_callback(self, msg: Image):
        """摄像头帧目标检测"""
        # Image msg转numpy
        frame = np.frombuffer(msg.data, dtype=np.uint8)
        frame = frame.reshape((msg.height, msg.width, 3))

        # 预处理：resize到640x640, 归一化
        import cv2
        resized = cv2.resize(frame, (640, 640))
        input_data = resized.astype(np.float32) / 255.0
        input_data = np.transpose(input_data, (2, 0, 1))
        input_data = np.expand_dims(input_data, axis=0)

        # ONNX推理
        outputs = self.session.run(None, {self.input_name: input_data})

        # 解析YOLOv8输出（简化）
        detections = self._parse_yolo_output(outputs[0], msg.width, msg.height)

        # 发布检测结果到ROS2
        for det in detections:
            pose = PoseStamped()
            pose.header = Header(stamp=self.get_clock().now().to_msg())
            pose.header.frame_id = 'camera_link'
            pose.pose.position.x = det['x3d']
            pose.pose.position.y = det['y3d']
            pose.pose.position.z = det['z3d']
            self.detect_pub.publish(pose)

    def _parse_yolo_output(self, output, orig_w, orig_h):
        """解析YOLOv8检测结果"""
        # YOLOv8输出形状: [1, 84, 8400]
        # 84 = 4(bbox) + 80(classes)
        predictions = output[0].transpose(1, 0)  # [8400, 84]

        detections = []
        for pred in predictions:
            confidence = pred[4:].max()
            if confidence < 0.5:
                continue
            class_id = pred[4:].argmax()

            # bbox: cx, cy, w, h -> 转换到原图坐标
            cx, cy, w, h = pred[:4]
            cx = cx / 640 * orig_w
            cy = cy / 640 * orig_h
            w = w / 640 * orig_w
            h = h / 640 * orig_h

            # 估算3D位置（简化：单目深度估计）
            z3d = 1.0 / (h / orig_h + 0.01)  # 距离反比于高度
            x3d = (cx - orig_w / 2) * z3d / 600
            y3d = 0.0

            detections.append({
                'class': int(class_id),
                'confidence': float(confidence),
                'bbox': [cx-w/2, cy-h/2, w, h],
                'x3d': x3d, 'y3d': y3d, 'z3d': z3d,
            })
        return detections

def main(args=None):
    rclpy.init(args=args)
    slam_node = SLAMNode()
    detect_node = ObjectDetectNode('/models/yolov8n_int8.onnx')

    # 多线程执行器并行运行节点
    from rclpy.executors import MultiThreadedExecutor
    executor = MultiThreadedExecutor()
    executor.add_node(slam_node)
    executor.add_node(detect_node)

    try:
        executor.spin()
    finally:
        executor.shutdown()
        rclpy.shutdown()

Nav2路径规划配置

# Nav2路径规划配置
bt_navigator:
  ros__parameters:
    use_sim_time: false
    global_frame: map
    robot_base_frame: base_link
    bt_loop_duration: 10
    default_server_timeout: 20

planner_server:
  ros__parameters:
    expected_planner_duration: 5.0
    use_sim_time: false
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5
      use_final_approach_orientation: false

controller_server:
  ros__parameters:
    use_sim_time: false
    controller_frequency: 10.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"]
    FollowPath:
      plugin: "dwb_core/DWBLocalPlanner"
      debug_trajectory_details: true
      min_speed_xy: 0.0
      max_speed_xy: 0.3
      max_speed_theta: 1.0
      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist"]
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time: 10.0

costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      rolling_window: false
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_topic: /map
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: true
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 0.05
          clearing: true
          marking: true
          raytrace_max_range: 3.0
          raytrace_min_range: 0.1
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55

四、性能优化与工程实践

端侧推理性能是系统瓶颈。
Jetson Nano GPU算力仅0.5 TOPS。
YOLOv8-nano INT8量化后推理12ms。
Cartographer建图每帧处理30ms。
两个模块并行运行需合理分配资源。

优化策略一：优先级调度。
避障决策优先级最高，独占NPU资源。
SLAM建图优先级次之，使用CPU多核。
地图发布频率从30Hz降到10Hz。
地图更新频率降低不影响定位精度。

优化策略二：模型量化精度平衡。
YOLOv8-nano全INT8量化精度损失5%。
关键层（特征提取前3层）保持FP16。
混合量化策略精度损失控制在2%以内。
推理延迟从30ms降到15ms。

优化策略三：ROS2通信零拷贝。
使用shared_memory传输Image大帧。
避免Python/C++边界的数据序列化。
DDS QoS配置 Reliable+TransientLocal。
保证关键消息不丢失。

优化策略四：多线程执行器。
MultiThreadedExecutor并行运行SLAM和检测节点。
每个节点独立线程，互不阻塞。
订阅队列深度控制在5-10。
避免内存占用无限增长。

传感器时间同步。
激光雷达和摄像头的时间戳必须对齐。
ROS2 message_filters的TimeSynchronizer。
最大时间差容限50ms。
超过容限的数据包丢弃不做融合。

生产环境稳定性。
ROS2节点崩溃自动重启。
launch文件配置on_failure=respawn。
最多重启3次，超过则整机重启。
日志输出到/rosout topic统一收集。

五、总结与技术提炼

1. 端侧AI推理与SLAM并行运行。
   避障决策独占NPU，SLAM使用CPU多核。
   优先级调度保证实时性要求。

2. YOLOv8-nano INT8混合量化。
   关键层保持FP16避免特征丢失。
   非关键层INT8压缩模型体积和推理延迟。
   精度损失控制在2%以内。

3. ROS2节点间通过topic解耦。
   传感器数据、检测结果、路径指令各独立topic。
   MultiThreadedExecutor并行运行多节点。
   shared_memory传输避免大帧数据拷贝。

4. Nav2路径规划三层Costmap叠加。
   静态地图层+动态障碍层+膨胀层。
   全局规划用NavFn，局部规划用DWB控制器。

5. 传感器时间同步是融合前提。
   TimeSynchronizer对齐激光雷达和摄像头时间戳。
   最大容限50ms，超时数据丢弃。

6. 节点崩溃自动重启保障稳定性。
   on_failure=respawn配置自动恢复。
   最多重启3次后整机重启兜底。