卡尔曼滤波融合雷达与摄像头进行目标跟踪-CSDN博客

本文链接：https://blog.csdn.net/MonkeyDhouzi/article/details/149802404

工程实践中我们常常碰到这个问题，检测到前方有一辆车，摄像头提供了该车的位置（但不太准），雷达提供了该车的速度（但也有噪声）。我们希望通过卡尔曼滤波来融合这两个信息，持续估计该车的位置和速度。

🧠 卡尔曼滤波是什么？

卡尔曼滤波是一种递归的估计方法，用于从一系列不完整、有噪声的观测数据中估计系统的真实状态。它的核心思想是：结合预测和观测，得到更准确的估计结果。

🚗 在自动驾驶中卡尔曼滤波的应用场景

1. 多传感器融合

自动驾驶车辆通常配备多个传感器，比如：

摄像头（视觉信息）
雷达（距离和速度信息）
激光雷达（高精度空间信息）
IMU（惯性测量单元，提供加速度和角速度）
GPS（定位信息）

这些传感器各有优劣，卡尔曼滤波可以：

融合不同来源的数据，比如将雷达的速度信息和摄像头的位置信息结合起来，得到更稳定的目标轨迹。
滤除噪声，比如GPS定位误差较大时，卡尔曼滤波可以结合IMU数据进行修正。

2. 目标跟踪

在自动驾驶中，车辆需要持续跟踪周围的动态物体（如其他车辆、行人、自行车等）。卡尔曼滤波可以：

预测目标的下一时刻位置和速度
结合当前传感器观测进行修正
实现平滑、连续的轨迹估计，避免“跳动”或“丢失”

3. 自身定位

车辆自身的位置估计也可以通过卡尔曼滤波来优化：

GPS提供粗略位置
IMU提供短时间内的运动信息
卡尔曼滤波将两者结合，得到更精确的定位结果，尤其在GPS信号弱的区域（如隧道、城市峡谷）非常重要。

🧩 状态定义

我们定义目标车辆的状态为：

$x$ ：位置
$v$ ：速度

状态向量为：

$X=\binom{x}{v}$

🔄 卡尔曼滤波流程

1. 预测阶段

根据上一帧的状态和时间间隔 $\Delta t$ ，预测当前状态：

$_{X_{pred}}=\begin{bmatrix}x+v\Delta t & & \\ v \end{bmatrix}$

2. 观测阶段

摄像头观测位置 $^{z_1{}}$
雷达观测速度 $^{z_2{}}$

观测向量为：

$z=\begin{bmatrix} z_1{}\\ z_2{} \end{bmatrix}$

3. 更新阶段

融合预测值和观测值，更新状态估计：

$X_{new}=X_{pred}+K\cdot (z-H\cdot X_{pred})$

其中：

$K$ 是卡尔曼增益，决定了预测和观测的权重
$H$ 是观测矩阵，映射状态到观测空间

用 C++ 实现的卡尔曼滤波器，用于跟踪目标的一维位置和速度，并融合来自摄像头（位置）和雷达（速度）的观测数据。

📦 一维卡尔曼滤波（位置 + 速度）

#include <iostream>
#include <Eigen/Dense>

using namespace Eigen;
using namespace std;

int main() {
    // 时间间隔
    double dt = 0.1;

    // 状态向量 [位置, 速度]
    Vector2d x;
    x << 0, 0;

    // 状态转移矩阵 F
    Matrix2d F;
    F << 1, dt,
         0, 1;

    // 观测矩阵 H（摄像头测位置，雷达测速度）
    Matrix2d H;
    H << 1, 0,
         0, 1;

    // 状态协方差矩阵 P
    Matrix2d P;
    P << 1, 0,
         0, 1;

    // 过程噪声协方差 Q
    Matrix2d Q;
    Q << 0.01, 0,
         0, 0.01;

    // 观测噪声协方差 R（摄像头和雷达的误差）
    Matrix2d R;
    R << 0.1, 0,
         0, 0.1;

    // 单位矩阵 I
    Matrix2d I = Matrix2d::Identity();

    // 模拟观测数据（位置和速度）
    vector<Vector2d> measurements = {
        {1.0, 0.5},
        {2.0, 0.4},
        {3.0, 0.6},
        {4.0, 0.7},
        {5.0, 0.9}
    };

    for (const auto& z : measurements) {
        // 预测
        x = F * x;
        P = F * P * F.transpose() + Q;

        // 卡尔曼增益
        Matrix2d K = P * H.transpose() * (H * P * H.transpose() + R).inverse();

        // 更新
        x = x + K * (z - H * x);
        P = (I - K * H) * P;

        // 输出当前估计
        cout << "Estimated position: " << x(0) << ", velocity: " << x(1) << endl;
    }

    return 0;
}

要让卡尔曼滤波器支持 ROS 接入，我们通常会将滤波器封装为一个 ROS 节点或组件，使其能够接收传感器数据（如雷达、摄像头、IMU）并发布融合后的状态估计。下面是一个简化的流程和代码结构说明，假设我们使用 ROS 1（如 Melodic 或 Noetic）和 C++。

🧱 ROS 接入的基本结构

1. 创建 ROS 包

catkin_create_pkg kalman_tracker roscpp std_msgs sensor_msgs geometry_msgs

2. 节点结构概览

// 文件名：kalman_tracker_node.cpp

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include "KalmanFilter.hpp"  // 自定义滤波器类

KalmanFilter kf;

void positionCallback(const geometry_msgs::PointStamped::ConstPtr& msg) {
    kf.updatePosition(msg->point.x);  // 摄像头位置
}

void velocityCallback(const geometry_msgs::TwistStamped::ConstPtr& msg) {
    kf.updateVelocity(msg->twist.linear.x);  // 雷达速度
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "kalman_tracker_node");
    ros::NodeHandle nh;

    ros::Subscriber pos_sub = nh.subscribe("/camera/position", 10, positionCallback);
    ros::Subscriber vel_sub = nh.subscribe("/radar/velocity", 10, velocityCallback);
    ros::Publisher state_pub = nh.advertise<geometry_msgs::TwistStamped>("/kf/state", 10);

    ros::Rate rate(10);  // 10 Hz
    while (ros::ok()) {
        kf.predict();

        geometry_msgs::TwistStamped state_msg;
        state_msg.twist.linear.x = kf.getPosition();
        state_msg.twist.linear.y = kf.getVelocity();
        state_pub.publish(state_msg);

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

3. 卡尔曼滤波器类封装（KalmanFilter.hpp）

你可以将之前的 Eigen 实现封装为一个类，提供 predict()、updatePosition()、updateVelocity() 等接口。

4. launch 文件

<launch>
  <node name="kalman_tracker_node" pkg="kalman_tracker" type="kalman_tracker_node" output="screen"/>
</launch>

同步时间戳：确保不同传感器数据时间对齐，避免融合误差。
使用 message_filters：可以用 message_filters::TimeSynchronizer 来同步多个话题。
调试可视化：使用 rqt_plot 或 rviz 查看滤波前后的轨迹。
参数配置：将 Q、R 等参数放入 ROS 参数服务器中，便于调参。

🧱 ROS 2 Humble

1️⃣ `kalman_filter.hpp`：卡尔曼滤波器类封装

#pragma once
#include <Eigen/Dense>

class KalmanFilter {
public:
    KalmanFilter(double dt);
    void predict();
    void update(double position, double velocity);
    double getPosition() const;
    double getVelocity() const;

private:
    double dt_;
    Eigen::Vector2d x_;
    Eigen::Matrix2d F_, H_, P_, Q_, R_, I_;
};

2️⃣ `kalman_tracker_node.cpp`：ROS 2 节点实现

#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/point_stamped.hpp"
#include "geometry_msgs/msg/twist_stamped.hpp"
#include "kalman_filter.hpp"

class KalmanTrackerNode : public rclcpp::Node {
public:
    KalmanTrackerNode() : Node("kalman_tracker_node"), kf_(0.1) {
        pos_sub_ = this->create_subscription<geometry_msgs::msg::PointStamped>(
            "/camera/position", 10,
            this {
                last_position_ = msg->point.x;
                has_position_ = true;
            });

        vel_sub_ = this->create_subscription<geometry_msgs::msg::TwistStamped>(
            "/radar/velocity", 10,
            this {
                last_velocity_ = msg->twist.linear.x;
                has_velocity_ = true;
            });

        state_pub_ = this->create_publisher<geometry_msgs::msg::TwistStamped>("/kf/state", 10);

        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(100),
            std::bind(&KalmanTrackerNode::timerCallback, this));
    }

private:
    void timerCallback() {
        kf_.predict();
        if (has_position_ && has_velocity_) {
            kf_.update(last_position_, last_velocity_);
            has_position_ = false;
            has_velocity_ = false;
        }

        auto msg = geometry_msgs::msg::TwistStamped();
        msg.twist.linear.x = kf_.getPosition();
        msg.twist.linear.y = kf_.getVelocity();
        state_pub_->publish(msg);
    }

    KalmanFilter kf_;
    double last_position_ = 0.0;
    double last_velocity_ = 0.0;
    bool has_position_ = false;
    bool has_velocity_ = false;

    rclcpp::Subscription<geometry_msgs::msg::PointStamped>::SharedPtr pos_sub_;
    rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr vel_sub_;
    rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr state_pub_;
    rclcpp::TimerBase::SharedPtr timer_;
};

3️⃣ `CMakeLists.txt` 关键配置

find_package(rclcpp REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(Eigen3 REQUIRED)

add_executable(kalman_tracker_node src/kalman_tracker_node.cpp)
target_include_directories(kalman_tracker_node PRIVATE ${EIGEN3_INCLUDE_DIRS})
target_link_libraries(kalman_tracker_node Eigen3::Eigen)
ament_target_dependencies(kalman_tracker_node rclcpp geometry_msgs)
install(TARGETS kalman_tracker_node DESTINATION lib/${PROJECT_NAME})