2. 基于Radar的目标检测与跟踪

任务目标

本案例基于Autoware的Radar驱动，完成从雷达原始数据解析、目标检测到卡尔曼滤波跟踪的完整闭环。最终输出包含目标位置、速度和ID的跟踪列表，并在RViz中可视化验证。

环境依赖

Autoware Universe (>= 2024.03)
安装包：autoware_radar_msgs、autoware_perception_msgs、autoware_utils
雷达驱动：已启动并发布 /sensing/radar/points（类型 sensor_msgs/PointCloud2）

核心实现

以下代码实现三个核心模块：雷达点云解析（提取径向距离、方位角、多普勒速度）、基于密度的目标聚类检测、卡尔曼滤波跟踪。代码可直接编译为ROS 2节点运行。

// radar_detection_tracking.cpp
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <autoware_perception_msgs/msg/detected_objects.hpp>
#include <autoware_perception_msgs/msg/detected_object.hpp>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/segmentation/extract_clusters.h>
#include <Eigen/Dense>
#include <map>

class RadarTracker : public rclcpp::Node
{
public:
  RadarTracker() : Node("radar_tracker")
  {
    sub_ = create_subscription<sensor_msgs::msg::PointCloud2>(
      "/sensing/radar/points", 10,
      std::bind(&RadarTracker::cloudCallback, this, std::placeholders::_1));
    pub_ = create_publisher<autoware_perception_msgs::msg::DetectedObjects>(
      "/perception/radar/objects", 10);
  }

private:
  struct Track {
    Eigen::Vector2d pos;   // x, y
    Eigen::Vector2d vel;   // vx, vy
    Eigen::Matrix2d P;     // 协方差
    int id;
    int miss_cnt;
  };

  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_;
  rclcpp::Publisher<autoware_perception_msgs::msg::DetectedObjects>::SharedPtr pub_;
  std::map<int, Track> tracks_;
  int next_id_ = 0;
  const double dt_ = 0.1;  // 假设10Hz

  // 卡尔曼滤波常量
  Eigen::Matrix2d Q_ = Eigen::Matrix2d::Identity() * 0.5;
  Eigen::Matrix2d R_ = Eigen::Matrix2d::Identity() * 1.0;

  void cloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
  {
    // ---- 1. 解析雷达点云 ----
    pcl::PointCloud<pcl::PointXYZI> cloud;
    pcl::fromROSMsg(*msg, cloud);
    if (cloud.empty()) return;

    // 提取有效点（雷达点云中 intensity 存多普勒速度）
    std::vector<pcl::PointXYZI> valid_points;
    for (const auto& pt : cloud.points) {
      if (std::isfinite(pt.x) && std::isfinite(pt.y) && std::abs(pt.x) < 100.0)
        valid_points.push_back(pt);
    }
    if (valid_points.size() < 3) return;

    // ---- 2. 基于欧氏距离的聚类检测 ----
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>);
    cloud_ptr->points = valid_points;
    cloud_ptr->width = valid_points.size();
    cloud_ptr->height = 1;

    pcl::search::KdTree<pcl::PointXYZI>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZI>);
    tree->setInputCloud(cloud_ptr);

    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZI> ec;
    ec.setClusterTolerance(1.5);      // 1.5m聚类半径
    ec.setMinClusterSize(3);
    ec.setMaxClusterSize(100);
    ec.setSearchMethod(tree);
    ec.setInputCloud(cloud_ptr);
    ec.extract(cluster_indices);

    // 计算每个聚类中心与平均速度
    std::vector<Eigen::Vector2d> det_pos;
    std::vector<Eigen::Vector2d> det_vel;
    for (const auto& indices : cluster_indices) {
      Eigen::Vector2d sum_pos(0, 0), sum_vel(0, 0);
      int n = indices.indices.size();
      for (int idx : indices.indices) {
        const auto& pt = cloud_ptr->points[idx];
        sum_pos += Eigen::Vector2d(pt.x, pt.y);
        sum_vel += Eigen::Vector2d(pt.x / std::max(std::sqrt(pt.x*pt.x+pt.y*pt.y), 1e-3) * pt.intensity,
                                   pt.y / std::max(std::sqrt(pt.x*pt.x+pt.y*pt.y), 1e-3) * pt.intensity);
      }
      det_pos.push_back(sum_pos / n);
      det_vel.push_back(sum_vel / n);
    }

    // ---- 3. 卡尔曼滤波跟踪与数据关联 ----
    std::vector<Track> new_tracks;
    std::vector<bool> matched(det_pos.size(), false);

    // 预测 + 关联（最近邻）
    for (auto& [id, trk] : tracks_) {
      // 预测
      trk.pos = trk.pos + trk.vel * dt_;
      trk.P = trk.P + Q_;

      // 找最近检测
      double min_dist = 5.0;  // 关联门限
      int best_idx = -1;
      for (size_t i = 0; i < det_pos.size(); ++i) {
        if (matched[i]) continue;
        double d = (trk.pos - det_pos[i]).norm();
        if (d < min_dist) { min_dist = d; best_idx = i; }
      }

      if (best_idx >= 0) {
        // 更新
        Eigen::Vector2d z = det_pos[best_idx];
        Eigen::Vector2d y = z - trk.pos;
        Eigen::Matrix2d S = trk.P + R_;
        Eigen::Matrix2d K = trk.P * S.inverse();
        trk.pos = trk.pos + K * y;
        trk.vel = trk.vel + K * (det_vel[best_idx] - trk.vel);
        trk.P = (Eigen::Matrix2d::Identity() - K) * trk.P;
        trk.miss_cnt = 0;
        matched[best_idx] = true;
        new_tracks.push_back(trk);
      } else {
        trk.miss_cnt++;
        if (trk.miss_cnt < 5) new_tracks.push_back(trk);  // 允许丢失5帧
      }
    }

    // 新增未匹配检测
    for (size_t i = 0; i < det_pos.size(); ++i) {
      if (!matched[i]) {
        Track t;
        t.pos = det_pos[i];
        t.vel = det_vel[i];
        t.P = Eigen::Matrix2d::Identity() * 2.0;
        t.id = next_id_++;
        t.miss_cnt = 0;
        new_tracks.push_back(t);
      }
    }
    tracks_.clear();
    for (auto& t : new_tracks) tracks_[t.id] = t;

    // ---- 4. 发布目标列表 ----
    autoware_perception_msgs::msg::DetectedObjects out;
    out.header = msg->header;
    for (const auto& [id, trk] : tracks_) {
      autoware_perception_msgs::msg::DetectedObject obj;
      obj.existence_probability = 0.9;
      obj.classification.front().label = autoware_perception_msgs::msg::ObjectClassification::CAR;
      obj.kinematics.pose_with_covariance.pose.position.x = trk.pos.x();
      obj.kinematics.pose_with_covariance.pose.position.y = trk.pos.y();
      obj.kinematics.twist_with_covariance.twist.linear.x = trk.vel.x();
      obj.kinematics.twist_with_covariance.twist.linear.y = trk.vel.y();
      obj.object_id = autoware_perception_msgs::build<unique_identifier_msgs::msg::UUID>()
        .uuid({0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,(uint8_t)id});
      out.objects.push_back(obj);
    }
    pub_->publish(out);
  }
};

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<RadarTracker>());
  rclcpp::shutdown();
  return 0;
}

编译与运行

# 在Autoware工作空间内
cd ~/autoware
colcon build --packages-select your_package_name
source install/setup.bash
ros2 run your_package_name radar_tracker

验证方法

数据流验证：运行后执行 ros2 topic echo /perception/radar/objects，应能看到包含位置、速度的DetectedObjects消息持续输出。
可视化验证：启动RViz2，添加DetectedObjects显示，话题选择/perception/radar/objects。当有车辆或行人经过雷达前方时，应看到稳定的目标框跟随移动，且ID保持不变。
跟踪稳定性判定：目标在雷达视野内直线移动时，输出的位置应连续变化无跳变，速度方向与运动方向一致。目标短暂离开（<5帧）后重新出现，应保持原ID。

关键参数说明

setClusterTolerance(1.5)：聚类半径，根据雷达安装位置和场景调整，车辆目标通常1.0~2.0m。
dt_ = 0.1：需与雷达实际发布频率匹配（10Hz），若频率不同需修改。
关联门限5.0米：目标帧间最大移动距离，按最高车速和帧率计算。

常见问题

无输出话题：确认雷达驱动已发布/sensing/radar/points，且消息类型为PointCloud2。
目标ID频繁切换：增大关联门限或减小dt_，确保帧间目标位移小于门限。
速度方向错误：雷达多普勒速度正负号约定需与坐标系对齐，若反向则取负。

📚 课程目录

2. 基于Radar的目标检测与跟踪

任务目标

环境依赖

核心实现

编译与运行

验证方法

关键参数说明

常见问题