ROS2 自动驾驶算法开发：SLAM 融合、点云感知、路径规划仿真项目精讲

原创

用户12885745

发布于 2026-11-09 17:46:36

自动驾驶算法开发的工程难点不在单个算法，而在多源传感器的时间同步、坐标系一致性、模块间的数据契约。本文按 SLAM 融合、点云感知、路径规划三段链路，给出 ROS2 下的关键代码实现与工程要点。

一、工程基础：TF 树与时间同步

所有算法模块共享同一套坐标系与时钟，这是系统能跑通的前提。

1. TF 树配置（URDF 片段）

<robot name="auto_vehicle">
  <link name="base_link"/>
  <link name="laser_link"/>
  <link name="imu_link"/>
  <link name="camera_link"/>

  <joint name="laser_joint" type="fixed">
    <parent link="base_link"/>
    <child link="laser_link"/>
    <origin xyz="0.0 0.0 1.2" rpy="0 0 0"/>
  </joint>

  <joint name="imu_joint" type="fixed">
    <parent link="base_link"/>
    <child link="imu_link"/>
    <origin xyz="0.1 0.0 0.3" rpy="0 0 0"/>
  </joint>
</robot>

2. 时间同步与 QoS 配置

from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy

# 传感器数据用 best_effort，控制指令用 reliable
SENSOR_QOS = QoSProfile(
    reliability=ReliabilityPolicy.BEST_EFFORT,
    history=HistoryPolicy.KEEP_LAST,
    depth=10,
)
CONTROL_QOS = QoSProfile(
    reliability=ReliabilityPolicy.RELIABLE,
    history=HistoryPolicy.KEEP_LAST,
    depth=1,
)

坑点：仿真环境必须所有节点设置 use_sim_time=true；QoS 不匹配会导致话题静默丢数据，且不报错。

二、SLAM 融合：激光 + IMU + 里程计

1. 多源融合的 EKF 配置（robot_localization）

# ekf_fusion.yaml
ekf_filter_node:
  ros__parameters:
    frequency: 30.0
    sensor_timeout: 0.1
    two_d_mode: true
    publish_tf: true

    map_frame: map
    odom_frame: odom
    base_link_frame: base_link
    world_frame: odom

    # 轮式里程计：使用 vx, vyaw
    odom0: /wheel_odom
    odom0_config: [false, false, false,
                   false, false, false,
                   true,  false, false,
                   false, false, true,
                   false, false, false]
    odom0_differential: false
    odom0_queue_size: 10

    # IMU：使用 yaw 角速度与线加速度
    imu0: /imu/data
    imu0_config: [false, false, false,
                  false, false, false,
                  false, false, false,
                  false, false, true,
                  true,  false, false]
    imu0_differential: false
    imu0_remove_gravitational_acceleration: true

2. 激光 SLAM 建图（SLAM Toolbox 配置）

slam_toolbox:
  ros__parameters:
    solver_plugin: solver_plugins::CeresSolver
    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
    ceres_preconditioner: SCHUR_JACOBI

    odom_frame: odom
    map_frame: map
    base_frame: base_link
    scan_topic: /scan
    mode: mapping            # mapping / localization

    resolution: 0.05
    max_laser_range: 20.0
    minimum_time_interval: 0.5
    transform_timeout: 0.2

    use_scan_matching: true
    use_scan_barycenter: true
    minimum_travel_distance: 0.3
    minimum_travel_heading: 0.3
    loop_search_maximum_distance: 3.0
    do_loop_closing: true
    loop_match_minimum_chain_size: 10

3. 融合定位输出校验

import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from tf2_ros import Buffer, TransformListener
import math

class FusionMonitor(Node):
    def __init__(self):
        super().__init__("fusion_monitor")
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        self.create_subscription(Odometry, "/odometry/filtered", self.on_odom, 10)
        self.create_timer(1.0, self.check_tf)

    def on_odom(self, msg: Odometry):
        p = msg.pose.pose.position
        self.get_logger().debug(f"pos=({p.x:.2f},{p.y:.2f})")

    def check_tf(self):
        """校验 map->base_link 链路是否完整，定位是否丢失"""
        try:
            t = self.tf_buffer.lookup_transform(
                "map", "base_link", rclpy.time.Time(), timeout=rclpy.duration.Duration(seconds=0.1))
            yaw = math.atan2(
                2 * (t.transform.rotation.w * t.transform.rotation.z),
                1 - 2 * t.transform.rotation.z ** 2)
            self.get_logger().info(f"localization ok, yaw={math.degrees(yaw):.1f}")
        except Exception as e:
            self.get_logger().error(f"TF lookup failed: {e}")

坑点：里程计标定不准会导致建图严重变形；回环检测在大场景必须开启；长走廊等特征稀疏场景定位易丢失，需融合 IMU 并设计降级策略。

三、点云感知：地面分割、聚类与障碍物提取

1. 地面分割 + 欧氏聚类（PCL 实现）

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/common.h>

using PointT = pcl::PointXYZI;

// 1. 体素降采样：控制计算量
pcl::PointCloud<PointT>::Ptr downsample(const pcl::PointCloud<PointT>::Ptr& in) {
    auto out = std::make_shared<pcl::PointCloud<PointT>>();
    pcl::VoxelGrid<PointT> vg;
    vg.setInputCloud(in);
    vg.setLeafSize(0.2f, 0.2f, 0.2f);   // 20cm 体素
    vg.filter(*out);
    return out;
}

// 2. RANSAC 地面分割
pcl::PointCloud<PointT>::Ptr removeGround(const pcl::PointCloud<PointT>::Ptr& in) {
    auto obstacles = std::make_shared<pcl::PointCloud<PointT>>();
    pcl::SACSegmentation<PointT> seg;
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    pcl::ModelCoefficients::Ptr coeff(new pcl::ModelCoefficients);

    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setDistanceThreshold(0.15);          // 地面点阈值
    seg.setAxis(Eigen::Vector3f(0, 0, 1));
    seg.setEpsAngle(0.15);                    // 允许坡度
    seg.setInputCloud(in);
    seg.segment(*inliers, *coeff);

    pcl::ExtractIndices<PointT> extract;
    extract.setInputCloud(in);
    extract.setIndices(inliers);
    extract.setNegative(true);                // 保留非地面点
    extract.filter(*obstacles);
    return obstacles;
}

// 3. 欧氏聚类提取障碍物
std::vector<pcl::PointCloud<PointT>::Ptr> clusterObstacles(
    const pcl::PointCloud<PointT>::Ptr& cloud) {
    pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
    tree->setInputCloud(cloud);

    std::vector<pcl::PointIndices> clusters;
    pcl::EuclideanClusterExtraction<PointT> ec;
    ec.setClusterTolerance(0.5);              // 聚类半径
    ec.setMinClusterSize(10);
    ec.setMaxClusterSize(2000);
    ec.setSearchMethod(tree);
    ec.setInputCloud(cloud);
    ec.extract(clusters);

    std::vector<pcl::PointCloud<PointT>::Ptr> result;
    for (const auto& idx : clusters) {
        auto c = std::make_shared<pcl::PointCloud<PointT>>();
        for (int i : idx.indices) c->push_back((*cloud)[i]);
        result.push_back(c);
    }
    return result;
}

2. 障碍物包围盒输出（ROS2 消息）

#include <vision_msgs/msg/detection3_d.hpp>

vision_msgs::msg::Detection3D toDetection(const pcl::PointCloud<PointT>::Ptr& cluster,
                                          int id) {
    PointT min_pt, max_pt;
    pcl::getMinMax3D(*cluster, min_pt, max_pt);

    vision_msgs::msg::Detection3D det;
    det.header.frame_id = "base_link";
    det.results.resize(1);
    det.results[0].hypothesis.class_id = "obstacle";
    det.results[0].hypothesis.score = 1.0;
    det.bbox.center.position.x = (min_pt.x + max_pt.x) / 2.0;
    det.bbox.center.position.y = (min_pt.y + max_pt.y) / 2.0;
    det.bbox.center.position.z = (min_pt.z + max_pt.z) / 2.0;
    det.bbox.size.x = max_pt.x - min_pt.x;
    det.bbox.size.y = max_pt.y - min_pt.y;
    det.bbox.size.z = max_pt.z - min_pt.z;
    return det;
}

坑点：点云降采样参数决定感知精度与算力平衡；地面阈值设小会误删坡道，设大会把低矮障碍当地面；聚类参数需按场景（室内/室外）分别调优。

四、路径规划：全局与局部协同

1. 全局规划（A* 在栅

import heapq
import numpy as np

def astar(grid: np.ndarray, start: tuple, goal: tuple):
    """grid: 0=free, 1=occupied；返回路径点列表"""
    h, w = grid.shape
    open_set = [(0, start)]
    g_score = {start: 0.0}
    came_from = {}
    dirs = [(-1,0),(1,0),(0,-1),(0,1),(-1,-1),(-1,1),(1,-1),(1,1)]

    def heuristic(a, b):
        return np.hypot(a[0]-b[0], a[1]-b[1])

    while open_set:
        _, cur = heapq.heappop(open_set)
        if cur == goal:
            path = [cur]
            while cur in came_from:
                cur = came_from[cur]
                path.append(cur)
            return path[::-1]

        for dx, dy in dirs:
            nx, ny = cur[0]+dx, cur[1]+dy
            if not (0 <= nx < h and 0 <= ny < w) or grid[nx, ny] == 1:
                continue
            step = np.hypot(dx, dy)
            tentative = g_score[cur] + step
            if tentative < g_score.get((nx, ny), float("inf")):
                came_from[(nx, ny)] = cur
                g_score[(nx, ny)] = tentative
                f = tentative + heuristic((nx, ny), goal)
                heapq.heappush(open_set, (f, (nx, ny)))
    return None

2. 局部规划（DWA 核心采样逻辑）

import math

def dwa_plan(pose, goal, obstacles, config):
    """pose: (x, y, yaw)；返回最优 (v, w)"""
    best = None
    min_cost = float("inf")

    for v in np.arange(config["v_min"], config["v_max"], config["v_step"]):
        for w in np.arange(config["w_min"], config["w_max"], config["w_step"]):
            traj = predict(pose, v, w, config["dt"], config["horizon"])
            if collide(traj, obstacles):
                continue
            cost = (config["w_goal"] * goal_cost(traj, goal)
                    + config["w_obs"] * obs_cost(traj, obstacles)
                    + config["w_speed"] * abs(config["v_ref"] - v))
            if cost < min_cost:
                min_cost, best = cost, (v, w)
    return best

def predict(pose, v, w, dt, horizon):
    x, y, yaw = pose
    traj = []
    for _ in range(int(horizon / dt)):
        x += v * math.cos(yaw) * dt
        y += v * math.sin(yaw) * dt
        yaw += w * dt
        traj.append((x, y))
    return traj

3. 局部规划器接入 Nav2

# nav2_params.yaml 片段
controller_server:
  ros__parameters:
    controller_frequency: 20.0
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      min_vel_x: 0.0
      max_vel_x: 0.8
      max_vel_theta: 1.2
      acc_lim_x: 0.5
      decel_lim_x: -0.8
      vx_samples: 20
      vtheta_samples: 40
      sim_time: 2.0
      xy_goal_tolerance: 0.15

local_costmap:
  local_costmap:
    ros__parameters:
      global_frame: odom
      robot_base_frame: base_link
      rolling_window: true
      width: 6.0
      height: 6.0
      resolution: 0.05
      plugins: ["obstacle_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        inflation_radius: 0.5
        cost_scaling_factor: 3.0

坑点：膨胀半径设小会蹭墙、设大会过不了窄通道；DWA 的速度窗口要结合车辆动力学（加速度限制）；全局路径与局部规划的分辨率不一致会导致跟踪抖动。

五、验收与调优顺序

模块	验收项	标准
TF/时钟	坐标链路	map→base_link 完整、无延迟
SLAM	建图与定位	地图闭合无重影、定位漂移可控
点云感知	障碍物输出	检测率达标、误检率可接受
规划控制	跟踪与避障	横向误差达标、无碰撞

联调排查顺序：定位 → 感知 → 规划 → 控制，任何一环不准，后面的调参都是徒劳。

结语

ROS2 自动驾驶算法开发的核心，是让三个模块在同一套坐标系和时钟下正确协同：SLAM 融合保证"位置准"，点云感知保证"障碍清"，路径规划保证"走得对"。代码只是骨架，真正的功力在于每一处的参数标定、异常兜底和降级策略。从打通 TF 树和第一个点云聚类节点开始。

原创声明：本文系作者授权腾讯云开发者社区发表，未经许可，不得转载。

如有侵权，请联系 cloudcommunity@tencent.com 删除。

自动驾驶