多传感器融合定位2（封装KF和EKF）

最新推荐文章于 2025-05-28 20:07:40 发布

try_again_later

最新推荐文章于 2025-05-28 20:07:40 发布

阅读量2k

点赞数

CC 4.0 BY-SA版权

分类专栏：无人驾驶

本文链接：https://blog.youkuaiyun.com/try_again_later/article/details/105164066

本文介绍了如何将激光雷达的卡尔曼滤波（KF）和毫米波雷达的扩展卡尔曼滤波（EKF）封装在同一类中，以实现多传感器融合定位。通过对激光雷达和毫米波雷达数据的分析，展示了如何处理和融合这两种传感器的输入数据，以提高定位精度。在实际应用中，每帧数据会依次通过KF和EKF进行处理，从而结合各自优势。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

有了上一节分别对激光雷达做KF、对毫米波雷达做EKF两种流程之后，我们合并KF和EKF的跟踪算法的代码，将他们封装在一个名为KalmanFilter的类中，方便后续调用。下面做一个多传感器融合定位实验，采集到的数据为两种交替出现的数据。
代码整体框架如图所示：
在这里插入图片描述

前言

无论是KF处理线性问题还是EKF处理非线性问题，它都只涉及单一传感器的障碍物跟踪。激光雷达测量位置精度更高但是无法测量速度，毫米波雷达测量速度准确，但是在位置测量精度上低。为了充分利用各个传感器的优势，需要做多传感器融合定位。

激光雷达Lidar：发射激光束，利用光的直线传播能够直接获得障碍物在笛卡尔坐标系下x、y、z方向上距离。建立三维点云图，探测目标的位置、速度等特征量。
状态向量分别是位置和速度的x、y方向（x,y,vx,vy）

在这里插入图片描述
毫米波雷达Radar：发射毫米波（电磁波），利用多普勒效应，所测数据都是在极坐标系下。毫米波导引头穿透雾、烟、灰尘的能力强，成本低，但是探测距离受到频段损耗的直接制约。
状态向量为极坐标系下雷达的距离ρ，方向角ϕ和距离的变化率（径向速度）ρ’。
在这里插入图片描述
Udacity 无人驾驶工程师学位

对比：
Lidar探测精度高、探测范围广，稳定性强，成本高。但是在雨雪雾等极端天气下、光束受遮挡后性能较差，采集的数据量过大。64线70万元人民币。

Radar穿透雾、烟、灰尘的能力强，探测距离受到频段损耗的直接制约（想要探测的远，就必须使用高频段雷达），也无法感知行人，并且对周边所有障碍物无法进行精准的建模。100美元左右。

一、输入数据分析

在data目录下名为sample-laser-radar-measurement-data-2.txt的文件，这就是传感器融合的输入数据。
假设：Lidar和Radar都固定在原点（0,0）处，且检测障碍物频率相同，两种雷达交替触发检测。
在这里插入图片描述
每行第一个字母L是Lidar，R是Radar。
第1、2行表示原点位置所以都只有时间戳。
Lidar：X方向测量值、Y方向测量值、时间戳、X方向位置真值、Y方向位置真值、X方向速度真值、Y方向速度真值。
Radar：障碍物在极坐标系下的距离ρ、角度ϕ、径向速度ρ’、时间戳、X方向位置真值、Y方向位置真值、X方向速度真值、Y方向速度真值。

主函数main()

框架为：
在这里插入图片描述
这部分我们重点考虑填充测量数据和真值数据，把传感器输入数据sample-laser-radar-measurement-data-2.txt填充进来。

1、导入txt文件

这部分可以说是从txt文件中导入数据的标配了。

// 设置毫米波雷达/激光雷达输入数据的路径
    // Set radar & lidar data file path
    std::string input_file_name = "../data/sample-laser-radar-measurement-data-2.txt";

    // 打开数据，若失败则输出失败信息，返回-1，并终止程序
    // Open file. if failed return -1 & end program
    std::ifstream input_file(input_file_name.c_str(), std::ifstream::in);
    if (!input_file.is_open()) {
   
   
        std::cout << "Failed to open file named : " << input_file_name << std::endl;
        return -1;
    }

2、测量值类和真值类

测量值MeasurementPackage类：时间戳、传感器类型、位置（xy）

class MeasurementPackage {
   
   
public:
  long long timestamp_;
  enum SensorType{
   
   
    LASER,
    RADAR
  } sensor_type_;
  Eigen::VectorXd raw_measurements_;
};

真值GroundTruthPackage类：时间戳、传感器类型、数据(距离、角度、镜像速度)

class GroundTruthPackage {
   
   
public:
  long timestamp_;
  enum SensorType{
   
   
    LASER,
    RADAR
  } sensor_type_;
  Eigen::VectorXd gt_values_;
};

3、txt数据导入测量值类和真值类

值得注意的是，未来主要用的数据是测量数据，真值是为了做对比才出现的，作用仅限于最后用于画图。我们使用测量数据进行process（）函数，如果是Lidar数据就用KF更新，如果是Radar数据就用EKF更新。

   std::vector<MeasurementPackage> measurement_pack_list;
    std::vector<GroundTruthPackage> groundtruth_pack_list;

    std::string line;
    while (getline(input_file, line)) {
   
   // getline后是string类型
        std::string sensor_type;
        MeasurementPackage meas_package;
        GroundTruthPackage gt_package;
        std::istringstream iss(line);// 从string转为数据流格式
        long long timestamp;

        // 读取当前行的第一个元素，L代表Lidar数据，R代表Radar数据
        // Reads first element from the curr