多传感器融合感知算法-后融合

1.传感器

需求分析

超声波

camera

radar-多普勒效应

径向速度

车远离人，频率下降；车接近人，频率上升。速度变化会引起频率变化。

幅值不变，频率变化。

毫米波雷达为何那么多误检？

在频域中的阈值选取有关。取频域的峰值。阈值取的低，噪声多一些。

德尔福厂家取阈值低，Continental（大陆）厂家取得高。

毫米波雷达没有高度信息？

毫米波雷达无法检测静态物体。

deta_f = 2 * deta_v / lamda , if deta_v = 0, deta_f = 0

挑战1：数据稳定性差

数据的不稳定性对后续的软件算法提出了较高的要求。

挑战2：对金属敏感

由于毫米波雷达发出的电磁波对金属极为敏感，在实际测试过程中会发现近处路面上突然出现的钉子、远距离外的金属广告牌都会被认为是障碍物。一旦车辆高速行驶，被这些突然跳出的障碍物干扰时，会导致刹车不断，导致汽车的舒适性下降。

挑战3：高度信息缺失

毫米波雷达的数据只能提供距离和角度信息，不能像激光雷达那样提供高度信息。没有高度信息的障碍物点会给技术开发带来很多挑战。

硬件系统设计

较少使用超声波雷达，因为近距离障碍物检测，激光雷达可以稳定检测。

adas传感器配置

摄像头部分，包括了前视摄像头，位置在车顶前挡风玻璃后：

- 前视主摄像头（Main Cam，HFOV52）：用于交通信号灯检测（识别红绿灯）、自动紧急制动AEB、自适应巡航ACC、前向碰撞预警FCW、车道偏离预警LDW，探测距离50-100米；

- 前视窄角摄像头（Narrow Cam，HFOV28）：用于AEB、ACC和FCW，探测距离150-200米，可能分辨率1828*948，15fps；

- 前视广角摄像头（Wide Cam，HFOV100），用于交通信号灯检测（辅助前视主摄像头）、雨量检测和防加塞（Anti-Cut in），探测距离0-50米，60fps；

后视摄像头（Rear Cam，HFOV52），一般为2M像素，帧率30fps，位于汽车尾部牌照灯位置，实现自动变道辅助ALC（Auto Lane Change）、盲点监测（Bind Spot Detection）、后面碰撞预警（Rear Collision Warning）的功能，探测距离为50-100米。

此外，还有两颗侧前视摄像头（Corner Cam）和两颗侧后视摄像头（Wing Cam），HFOV100，一般为1M像素，帧率30fps/60fps，探测距离50米左右。

- 侧前视摄像头，主要关注侧前方目标，一般位于B柱上（特斯拉）或前后镜下方（小鹏），60fps帧率，分辨率457*237，防加塞（Anti-Cut in）和侧向车辆检测（Cross Trafic）；

- 侧后视摄像头，主要关注侧边和侧后方目标，一般位于前翼子板上往后看，60fps帧率，主要用于自动变道辅助ALC（Auto Lane Change）、开门预警（Door Open Warning）、盲点监测（Bind Spot Detection）功能。

FCW 实现原理

首先，利用传感器获取前方道路信息，对前方车辆进行识别和跟踪。如果有车辆被识别出来，就会测量前方车距。同时，结合车速估计，利用安全车距预警模型判断是否存在追尾风险。一旦存在追尾危险，系统会根据预警规则及时给予驾驶员主动预警。FCW 系统主要的功能包括：

具体来说，我们可以将以上过程分为三个部分。

FCW 指的是前方碰撞预警系统（Forward Collision Warning System），它是一种车辆主动安全技术，可以实时监测前方道路和车辆的状况，预测可能发生的碰撞危险，为驾驶员提供预警和辅助制动等措施，从而避免交通事故的发生。FCW 系统主要的功能包括：

1. 前方车辆探测：通过雷达或摄像头等传感器，实时监测前方车辆和道路状况。

2. 碰撞预测：分析前方车辆行驶速度、方向变化等数据，预测是否存在前方碰撞的风险。

3. 驾驶员预警：当系统检测到可能存在交通事故风险时，会发出视觉、声音、震动等多种方式的警告，提醒驾驶员及时采取措施。

当前 FCW 一般和 AEB 功能结合使用，所以有些厂家 FCW 还包括以下功能：

4. 自动制动：如果驾驶员无法及时响应警告并采取避让措施，FCW 系统会自动进行制动操作，减少碰撞危险。

单目摄像头前向碰撞预警（FCW）算法‌主要依赖于深度学习和单目摄像头测距技术。其工作原理如下：

‌单目测距‌：通过深度学习识别图像中的车辆，利用相机的内外参数等信息，通过图像中的车辆大小和位置信息计算与本车的距离。
‌多目标跟踪‌：在实时视频流中检测并跟踪多个前方车辆的位置和运动轨迹。通过对视频流进行处理和分析，提取目标物体的特征信息，并实时更新其位置和状态，确保系统能够准确判断前方车辆的行驶状态，为后续的碰撞预警提供可靠数据支持‌。
‌深度学习分析‌：利用深度学习模型对图像进行特征提取和距离估计。通过训练模型，可以更准确地估计车辆之间的距离，并在存在碰撞风险时及时发出预警信号，提醒驾驶员注意安全‌。
‌传感器融合‌：虽然单目摄像头在测距上有优势，但为了进一步提高系统的准确性和可靠性，通常会结合其他传感器如毫米波雷达或激光雷达。这些传感器提供的数据可以进一步验证和补充单目摄像头的测量结果‌。

2. 后融合系统输入输出解析

根据数据融合方式的不同，主要可以分为前融合和后融合。前融合主要是指数据/特征层次的融合，优点在于数据利用更加充分；而后融合主要是指目标/结果层次的融合，优点在于处理相对简单，运行效率更高。

输入主要包括LiDAR、Camera和Radar的检测处理结果。

LiDAR：原始数据为三维空间点云数据，通过对其进行3D检测和分割，以及多目标跟踪，得到3D空间的目标元素；
Camera：原始数据为图像，通过对其进行2D的检测，得到2D空间的目标元素；
Radar：原始数据为稀疏的二维离散点云，但具备径向速度信息，通过对其进行数据处理，得到2.5D的目标元素。

lidar 3D检测任务

输出为LiDARObject形式的列表：

lidar 3D分割任务

输出为点云或者Polygons形式数据：

Camera 2D检测任务

输出为CameraObject形式的列表：

Camera 2D分割任务

输出为带标签的同尺寸Image数据：

Radar 检测任务

融合

输出主要是3D空间下完整的boundingbox目标及其语义信息。

3. 后融合系统问题建模

融合系统的核心是希望对不同模态的传感器数据进行综合处理，提高传感器系统的信息丰富度，输出更加稳定可靠的感知结果。

从上述后融合系统的输入输出角度来看，需要将多传感器获取的观测量与待估计的目标状态量进行融合，由此可以将问题转化为系统的状态估计问题。
不同传感器的数据采集频率和数据处理的耗时不同，使得各模态数据结果接入到后融合模块的时间通常也是存在差异的，由此也会导致时空对齐的问题。
通常传感器单帧数据中会存在多个目标，如何将不同传感器观测到的目标及单个传感器帧和帧之间的目标信息关联起来，由此也就涉及目标匹配问题。

4 后融合系统结构设计

针对上述多传感器后融合中涉及到的三个问题：多观测条件下的系统状态估计问题、时空对齐（预测）问题、目标匹配问题，进行多传感器后融合系统结构的设计构建。

预测-时空问题

通常LiDAR的频率为10hz，Camera为30+hz，Radar为20hz。假设状态量是100ms更新一次，则在两次状态量之间，状态量相对于新到来的观测量在时空维度上都是滞后的，因此需要时空对齐，保证状态量与观测量在融合时是一致的。由于要做的工作是时空对齐，因此只需要关注引起空间位置变换的状态量和观测量即可：

位姿状态：如yaw和yaw_rate;
运动状态：如位置、速度和加速度。

对齐方法：
对于不同频率的LiDAR、Camera、Radar检测结果，将其输入到预测模块中，根据自车的运动和目标的状态估计，进行时空对齐，使得融合时的状态量和观测量是位于同一时空下的。原则上下述两种方式均可以，但更推荐方式一，原因是State本身是基于多传感器数据的得到的状态量，精度上比各单传感器精度更高，预测误差也更小。

目标关联-匹配问题

不同传感器经处理后得到的目标维度也不尽相同，那么如何进行目标级对象的匹配呢？首先，需要明确的是，融合的目的是希望通过多传感器（LiDAR + Camera + Radar）的目标数据，得到不同时刻下同一目标的3D bounding box信息。当然，如果只有Camera + Radar，则融合得到的可能是2D 的bounding box及其对应的距离、速度信息，但最终的目的还是希望得到3D 的bounding box信息。

目标级对象匹配算法—关联的度量

以LiDAR + Camera + Radar融合为例：三者融合之后得到的目标状态量通常为完整的 3D bounding box，估状态量与观测量之间的匹配度量可以表示如下：

LiDAR观测量与 3D bounding box

通常LiDAR输入的观测量是3D bounding box，因此两者之间的匹配可以通过计算两个目标 3D bounding box之间的交并比** IoU(Intersection over Union)** 来度量。

2. Camera 观测量与 3D bounding box

通常Camera输入的观测量是2D bounding box，而状态量是3D bounding box，因此需要先将 3D bounding box 投影到图像上以获取其 2D bounding box信息，然后计算两个2D bounding box 的 IoU 来度量。

3. Radar观测量与 3D bounding box

通常毫米波雷达输入的观测量没有尺寸和高度信息（这里暂不考虑4D毫米波雷达，仅以L2常用车载毫米波雷达为例），因此通常认为其检测到的目标点处于3D bounding box 内部时，及认为满足匹配要求。

目标级对象匹配算法—帧间匹配算法

通常每帧数据检测出的Object可能会有若干个，而为了实现不同模态检测结果的融合，势必设计到Object集合之间的匹配问题，即下图中红色点和绿色点的匹配。

一般而言，待匹配的目标是带有权重的（如单个红色与其他任意绿色点之间的权重可以通过前面提到的目标关联的度量值来表示），这样帧间的目标匹配工作就可以转化成两个带权重集合的匹配问题。对于这种带权二分图最优匹配问题，可以考虑使用KM算法（Kuhn-Munkres Algorithm）。

状态估计问题

多传感器融合的数学本质：在输入、输出和状态空间已知的条件下，获取系统状态最优估计。
经过前面的时空对齐和目标关联处理之后，已经可以拿到时空对齐后的 t-1 时刻目标状态量、t 时刻的目标观测量，以及状态量和观测量之间的目标匹配关系了。现在就需要在已知这些信息的基础上，对目标状态量进行更新，获取 t时刻下目标状态的最优估计量。
首先需要进行融合信息的拆解，融合对象包括目标级的运动属性融合（位置、速度、角速度、加速度等）、语义级的对象融合（位置、属性）以及目标级其他属性融合（尺寸、类型等），各部分融合都可以单独看做状态估计问题。

状态估计问题的求解思路

状态估计问题求解：卡尔曼滤波器

卡尔曼滤波是一种时域递推算法，能够根据上一时刻状态的估计值和当前时刻状态的观测值推测出当前时刻状态的最优值，是一种能排除随机干扰，提高测量精度的方法。
首先需要进行状态方程和观测方程的构建，先明确几个变量的含义：

状态方程的构建：

观测方程的构建：

卡尔曼滤波的工作过程：

总的来说，卡尔曼滤波器的工作流程分为预测和更新两部分：

补充：因卡尔曼滤波本身是基于线性马尔科夫性质进行求解的，当状态方程和观测方程不不满足线性系统时，可以考虑使用扩展卡尔曼滤波。主要差别在于，需要将原有状态方程中的状态转移矩阵 F_k 和观测方程中的测量矩阵 H_k 进行线性转换才可以使用。通过泰勒公式一阶展开来对其进行局部线性化展开，进而来实现线性系统的状态估计。展开过程可参考如下：

状态估计-目标级运动属性融合建模

运动方程：
在自动驾驶系统中，通常假设对象满足匀加速/匀速旋转运动，状态转移方程（运动方程）如下：

观测方程：
在自动驾驶系统中，通常假设对象满足匀加速/匀速旋转运动，观测方程如下：

状态估计-语义级对象融合建模

状态估计-目标级其它属性融合

关于卡尔曼滤波建模中的噪声协方差

5 实践

5.1. 融合器数据输入格式

// 单个LiDAR目标信息
struct LiDARObject {double time_ns = 0.0f;int id = -1;float x = 0.0f;float y = 0.0f;float z = 0.0f;float length = 0.0f;float width = 0.0f;float height = 0.0f;float velo_x = 0.0f;float velo_y = 0.0f;float velo_z = 0.0f;int label = -1;float confidence = 0.0f;
};
using LiDARObjectPtr = std::shared_ptr<LiDARObject>;// 单帧多个LiDAR目标信息
struct LiDARObjectList {double time_ns = 0.0f;std::string frame_id = "";std::vector<LiDARObjectPtr> objs;
};
using LiDARObjectListPtr = std::shared_ptr<LiDARObjectList>;struct Frame {double time_ns = 0.0f;LiDARObjectListPtr lidar_objs;CameraObjectListPtr camera_objs;RadarObjectListPtr radar_objs;
};
using FramePtr = std::shared_ptr<Frame>;// 单个Camera目标信息
struct CameraObject {double time_ns = 0.0f;int id = -1;float ux = 0.0;float vy = 0.0;float width = 0.0f;float height = 0.0f;int label = -1;float confidence = 0.0f;
};
using CameraObjectPtr = std::shared_ptr<CameraObject>;// 单帧多个Camera目标信息
struct CameraObjectList {double time_ns = 0.0f;std::string frame_id = "";std::vector<CameraObjectPtr> objs;
};
using CameraObjectListPtr = std::shared_ptr<CameraObjectList>;// 单个Radar目标信息
struct RadarObject {double time_ns = 0.0f;int id = -1;float x = 0.0f;float y = 0.0f;float z = 0.5f;float velo_x = 0.0f;float velo_y = 0.0f;float velo_z = 0.0f;float angle = 0.0f;float rcs = 0.0f;  //雷达截面反射强度RCS信息
};
using RadarObjectPtr = std::shared_ptr<RadarObject>;// 单帧多个Radar目标信息
struct RadarObjectList {double time_ns = 0.0f;std::string frame_id = "";std::vector<RadarObjectPtr> objs;
};
using RadarObjectListPtr = std::shared_ptr<RadarObjectList>;// 单个Fusion目标信息
struct FusionObject {double time_ns = 0.0f;	// 存储对象的时间戳（单位可能是纳秒）int id = -1;			// 描述对象IDfloat x = 0.0f;			// 目标的三维空间位置float y = 0.0f;float z = 0.0f;float length = 0.0f;	// 目标的三维尺寸float width = 0.0f;float height = 0.0f;float velo_x = 0.0f;	// 目标的三维速度float velo_y = 0.0f;float velo_z = 0.0f;int label = -1;			// 目标的类别标签float confidence = 0.0f;	// 目标的置信度float ux = 0.0f;		// 目标在二维图像中的横坐标(像素坐标系)float vy = 0.0f;		// 目标在二维图像中的纵坐标(像素坐标系)float width_2d = 0.0f;	// 目标在二维图像中的宽度float height_2d = 0.0f;	// 目标在二维图像中的高度int life_duration = 0;	// 目标的生命周期float rcs = 0.0f;		// 目标的雷达截面积反射强度RCS// 通过一个仿射变换（Affine transformation）来更新对象的位置和速度：线性变换”+“平移”void transformBy(const Eigen::Affine3d& trans) {Eigen::Vector3d pos(x, y, z); Eigen::Vector3d velo(velo_x, velo_y, velo_z);// 使用仿射变换 trans 来更新对象的位置。pos = trans * pos;//  仅使用仿射变换的线性部分（不包括平移）来更新对象的速度.velo = trans.linear() * velo;x = pos(0);y = pos(1);z = pos(2);velo_x = velo(0);velo_y = velo(1);velo_z = velo(2);}
};
using FusionObjectPtr = std::shared_ptr<FusionObject>;// 单帧多个Fusion目标信息
struct FusionObjectList {double time_ns = 0.0f;std::string frame_id = "";std::vector<FusionObjectPtr> objs;
};
using FusionObjectListPtr = std::shared_ptr<FusionObjectList>;

除了传感器各自的数据格式，以及融合数据格式外，我们是希望将近似同一时刻的传感器数据进行融合的，这里可以定义一个帧Frame（包含不同传感器同一时刻的检测数据）。

5.2 融合器架构设计

多模态据融合需要考虑如下三个问题：观测量时空对齐问题、目标关联问题和多观测条件下的系统状态估计问题。
因此融合器也可以封装成三部分：预测模块（Predictor）、匹配模块（Matcher）、融合更新模块（Tracker）。这里需要强调的一点是，融合框架采用主从传感器的设计方式，即

主传感器（LiDAR）可创建状态量中的目标，主从传感器（LiDAR/Camera/Radar等）能更新状态量；
融合触发方式为支持所有传感器触发，即任何模态数据更新，立即触发多传感器融合动作；

class FusionWrapper {public:FusionWrapper(const std::string& config_file);// 融合器主入口：输入多模传感器帧数据bool Update(const FramePtr &frame);bool GetFusionResult(const FusionObjectListPtr &res);private:std::shared_ptr<fusion::Tracker> tracker_;	// 跟踪更新模块（KF/EKF算法）std::shared_ptr<fusion::Matcher> matcher_;	// 匹配模块(KM算法)std::shared_ptr<fusion::Predictor> predictor_;	// 预测模块（时空外推对齐）
};

5.2.1 预测模块

class Predictor {public:Predictor() = default;bool Predict(const FusionObjectListPtr &fusion_obj_list, double ts);}; // class Predictor/* 函数功能：用于将输入目标数据预测对齐到指定时间戳 ts 位置参数：需要预测的目标数据 fusion_obj_list，以及 目标时间戳输出：预测操作执行结果
*/
bool Predictor::Predict(const FusionObjectListPtr &fusion_obj_list, double ts) {// Predict global objects to local timestampif(fusion_obj_list->objs.empty()) return true;// 计算时间差（obj -> ts）及自车位姿变换矩阵float dt = ts - fusion_obj_list->time_ns;Eigen::MatrixXd T_ego;T_ego = Eigen::MatrixXd(5,5);T_ego << 1, 0, dt, 0, 0,0, 1, 0, dt, 0,0, 0, 1, 0,  0,0, 0, 0, 1,  0,0, 0, 0, 0,  1;for(size_t i = 0; i < fusion_obj_list->objs.size(); ++i){Eigen::VectorXd state(5);// 目标运动补偿值计算state << fusion_obj_list->objs[i]->x,fusion_obj_list->objs[i]->y,fusion_obj_list->objs[i]->velo_x,fusion_obj_list->objs[i]->velo_y,fusion_obj_list->objs[i]->label;state = T_ego * state;// 目标运动套补偿值更新fusion_obj_list->objs[i]->x = state[0];fusion_obj_list->objs[i]->y = state[1];fusion_obj_list->objs[i]->velo_x = state[2];fusion_obj_list->objs[i]->velo_y = state[3];fusion_obj_list->objs[i]->label = state[4];        }// 预测结束后需要将目标帧的时间戳与预测时刻保持一致fusion_obj_list->time_ns = ts;return true;
}

5.2.2 目标关联模块

不同模态的目标关联代价可以使用如下方式进行度量，在帧和帧目标得到代价权重矩阵之后，可以使用KM算法求解带权二分图的最优匹配。进而得到目标与目标之间的匹配关联关系。

LiDAR目标和Fusion目标匹配度量

通过3D目标中心点的距离度量即可。（KM求解是最小权匹配问题）

float Matcher::IoULiDARToFusion(const LiDARObjectPtr& lidar_obj, const FusionObjectPtr& fusion_obj) {return std::sqrt(std::pow(lidar_obj->x - fusion_obj->x, 2) +std::pow(lidar_obj->y - fusion_obj->y, 2));
}

Radar目标和Fusion目标匹配度量

通过2D目标中心店与3D目标中心点距离度量即可。（KM求解是最小权匹配问题）

float Matcher::IoURadarToFusion(const RadarObjectPtr& radar_obj, const FusionObjectPtr& fusion_obj) {float dist = 0.0f;dist = std::sqrt(std::pow(radar_obj->x - fusion_obj->x, 2) +std::pow(radar_obj->y - fusion_obj->y, 2));return dist;
}

Camera目标和Fusion目标匹配度量

需要先将融合目标的3D Box信息通过内外参转换投影到像素坐标系下，计算2D Box的交并比。

struct BBox2D {float x = 0.0f;float y = 0.0f;float width = 0.0f;float height = 0.0f;
};struct BBox3D {float x = 0.0f;float y = 0.0f;float z = 0.0f;float width = 0.0f;float height = 0.0f;float length = 0.0f;
};float Matcher::IoUCamToFusion(const CameraObjectPtr& cam_obj, const FusionObjectPtr& fusion_obj) {float dist = 0.0f;BBox2D pred;pred.x = cam_obj->ux;pred.y = cam_obj->vy;pred.width = cam_obj->width;pred.height = cam_obj->height;BBox3D tgt;tgt.x = fusion_obj->x;tgt.y = fusion_obj->y;tgt.z = fusion_obj->z;tgt.width = fusion_obj->width;tgt.height = fusion_obj->height;tgt.length = fusion_obj->length;dist = IoUIn2D(pred, tgt, extrinsic_camera_to_baselink_.inverse(), cam_intrinsic_);return dist;
}float IoUIn2D(const BBox2D &box_2d, const BBox3D &box_3d,const Eigen::Affine3d& extrinsic, const Vector6d& intrinsic) {BBox2D tgt;// 将3D Box投影成 2D BoxProject3DBoxTo2DBox(box_3d, extrinsic, intrinsic, tgt);return IoUIn2D(box_2d, tgt);
}float IoUIn2D(const BBox2D &pred, const BBox2D &tgt) {float x1_min = pred.x - pred.width * 0.5;float y1_min = pred.y - pred.height * 0.5;float x1_max = pred.x + pred.width * 0.5;float y1_max = pred.y + pred.height * 0.5;float x2_min = tgt.x - tgt.width * 0.5;float y2_min = tgt.y - tgt.height * 0.5;float x2_max = tgt.x + tgt.width * 0.5;float y2_max = tgt.y + tgt.height * 0.5;if ((x1_max <= x2_min || y1_max <= y2_min) ||(x2_max <= x1_min || y2_max <= y1_min)) {return 0.0;}float x_min = std::max(x1_min, x2_min);float y_min = std::max(y1_min, y2_min);float x_max = std::min(x1_max, x2_max);float y_max = std::min(y1_max, y2_max);float inter = (y_max - y_min) * (x_max - x_min);float uni = (pred.width * pred.height + tgt.width * tgt.height) - inter;return inter / uni;
}

KM算法求解目标匹配度

匹配模块的算法流程：

根据输入数据类型的不同计算目标之间的匹配权重，构建带权二分图；
利用KM算法求解二分图的最优匹配方式；
输出匹配结果

// 以LiDARObject和FusionObject的匹配为例：
bool Matcher::Match(const LiDARObjectListPtr& lidar_obj_list,const FusionObjectListPtr& global_obj_list,std::map<size_t, int>& lidar_global_map) {lidar_global_map.clear();for (size_t i = 0; i < lidar_obj_list->objs.size(); ++i) {lidar_global_map[i] = -1;}if (global_obj_list->objs.size() == 0) {return true;;}// 构建带权二分图std::vector<std::vector<float>> weights;for (size_t i = 0; i < lidar_obj_list->objs.size(); ++i) {std::vector<float> row(global_obj_list->objs.size(), 0);weights.push_back(row);for (size_t j = 0; j < global_obj_list->objs.size(); ++j) {weights[i][j] = IoULiDARToFusion(lidar_obj_list->objs[i], global_obj_list->objs[j]);}}// 调用KM算法进行目标匹配auto f = [&](size_t r, size_t c) {return weights[r][c];};auto res = munkres_algorithm<float>(lidar_obj_list->objs.size(),global_obj_list->objs.size(),f);auto is_matching = [&](const size_t r, const size_t c) {const auto ii = std::find_if(begin(res), end(res), [&](const auto& x) {return (x.first == r and x.second == c);});return ii != end(res);};// 输出匹配结果for (size_t i = 0; i < lidar_obj_list->objs.size(); ++i) {for (size_t j = 0; j < global_obj_list->objs.size(); ++j) {if (is_matching(i, j)) {lidar_global_map[i] = j;}}}return true;
}

5.2.3 状态估计模块

在得到目标状态量与观测量之间的匹配关系之后，就可以使用卡尔曼滤波来执行融合操作了。这里以LiDAR观测与全局目标的融合更新为例介绍主要流程：

取出匹配成功的状态目标和观测目标，并将其输入到对应的滤波器模块；
注意，如果是首次输入，这里需要通过主传感器LiDAR的观测来构建全局目标状态量；同时为对应目标创建滤波器；
滤波器依次执行预测 -> 更新操作；
状态目标更新。

bool Tracker::Update(const LiDARObjectListPtr &lidar_obj_list,const std::map<size_t, int>& map) {for (auto m : map) {// 如果存在满足匹配的 全局目标if (m.second >= 0) {Measurement lidar_mea;const auto &lidar_obj = lidar_obj_list->objs[m.first];const auto &global_obj = global_obj_list_->objs[m.second];lidar_mea.sensor_type = SensorType::LIDAR;Eigen::VectorXd meas(5);meas << lidar_obj->x,lidar_obj->y,lidar_obj->velo_x,lidar_obj->velo_y,lidar_obj->label;lidar_mea.time_ns = lidar_obj->time_ns;lidar_mea.meas = meas;State state;meas << global_obj->x,global_obj->y,global_obj->velo_x,global_obj->velo_y,global_obj->label;state.time_ns = global_obj->time_ns;state.meas = meas;// 通过卡尔曼滤波器融合 全局状态量 和 lidar 观测量motion_filters_[m.second]->UpdateMotion(state, lidar_mea);global_obj->x = motion_filters_[m.second]->GetState()[0];global_obj->y = motion_filters_[m.second]->GetState()[1];global_obj->z = lidar_obj->z;global_obj->velo_x = motion_filters_[m.second]->GetState()[2];global_obj->velo_y = motion_filters_[m.second]->GetState()[3];global_obj->label = motion_filters_[m.second]->GetState()[4];global_obj->length = lidar_obj->length;global_obj->width = lidar_obj->width;global_obj->height = lidar_obj->height;int dura = global_obj_list_->objs[m.second]->life_duration;global_obj_list_->objs[m.second]->life_duration =  (dura < startup_duration_ ? dura+1 : startup_duration_);std::cout << "updating exist global object: " << global_obj_list_->objs[m.second]->ToString() << std::endl;} else {// 通过 lidar 数据创建新的 全局状态量FusionObjectPtr new_obj =std::make_shared<FusionObject>(FusionObject());const auto &lidar_obj = lidar_obj_list->objs[m.first];new_obj->time_ns = lidar_obj->time_ns;new_obj->id = lidar_obj->id;new_obj->x = lidar_obj->x;new_obj->y = lidar_obj->y; new_obj->z = lidar_obj->z;new_obj->length = lidar_obj->length;new_obj->width = lidar_obj->width;new_obj->height = lidar_obj->height;new_obj->velo_x = lidar_obj->velo_x;new_obj->velo_y = lidar_obj->velo_y;new_obj->velo_z = lidar_obj->velo_z;new_obj->label = lidar_obj->label;new_obj->confidence = lidar_obj->confidence;new_obj->life_duration = startup_duration_;global_obj_list_->objs.push_back(new_obj);std::shared_ptr<FusionEKF> motion_filter =std::make_shared<FusionEKF>(FusionEKF());motion_filters_.push_back(motion_filter);std::cout << "create new global object: " << new_obj->ToString() << std::endl;}global_obj_list_->time_ns = lidar_obj_list->time_ns;}return true;
}

卡尔曼滤波器-预测

void KalmanFilter::Predict() {// 状态预测及协方差更新x_ = F_ * x_;MatrixXd Ft = F_.transpose();P_ = F_ * P_ * Ft + Q_;
}

卡尔曼滤波器-更新

void KalmanFilter::Update(const VectorXd &z) {VectorXd z_pred = H_ * x_;VectorXd y = z - z_pred;MatrixXd Ht = H_.transpose();MatrixXd PHt = P_ * Ht;MatrixXd S = H_ * PHt + R_;MatrixXd Si = S.inverse();MatrixXd K = PHt * Si;// 后验状态估计x_ = x_ + (K * y);long x_size = x_.size();MatrixXd I = MatrixXd::Identity(x_size, x_size);P_ = (I - K * H_) * P_;
}

卡尔曼滤波器主流程

{R_laser_ = MatrixXd(5, 5); 
H_laser_ = MatrixXd(5, 5);
P_ = MatrixXd(5, 5);
R_laser_ << 0.0225, 0, 0, 0, 0,0, 0.0225, 0, 0, 0,0, 0, 0.0225, 0, 0,0, 0, 0, 0.0225, 0,0, 0, 0, 0, 5;		// 经验值-lidar观测噪声协方差矩阵H_laser_ << 1, 0, 0, 0, 0,0, 1, 0, 0, 0,0, 0, 1, 0, 0,0, 0, 0, 1, 0,0, 0, 0, 0, 1;	
// 经验值：过程噪声，时间越长不确定性越高
noise_ax = 9;
noise_ay = 9;
noise_attr_ = 5;
}

// 卡尔曼滤波器工作主流程
void FusionEKF::UpdateMotion(const State &state, const Measurement &measurement) {/****************************************************************************** Initialization ****************************************************************************/motion_filter_.x_ = VectorXd(5);motion_filter_.P_ = P_;if (measurement.sensor_type == SensorType::LIDAR) {// Initialize state.motion_filter_.x_ << state.meas[0], state.meas[1], state.meas[2], state.meas[3],state.meas[4];}/******************************************************************************  Prediction-预测****************************************************************************/float dt = measurement.time_ns - state.time_ns;  //  in secondsfloat dt_2 = dt * dt;float dt_3 = dt_2 * dt;float dt_4 = dt_3 * dt;// 更新状态转移矩阵motion_filter_.F_ = MatrixXd(5, 5);motion_filter_.F_ << 1, 0, dt, 0, 0,0, 1, 0, dt, 0,0, 0, 1, 0, 0,0, 0, 0, 1, 0,0, 0, 0, 0, 1;// 更新过程噪声协方差矩阵// Use noise_ax = 9 and noise_ay = 9 for your Q matrix.(经验值)motion_filter_.Q_ = MatrixXd(5, 5);motion_filter_.Q_ << dt*noise_ax, 0, 0, 0, 0,0, dt*noise_ay, 0, 0, 0,0, 0, dt*noise_ax, 0, 0,0, 0, 0, dt*noise_ay, 0,0, 0, 0, 0, dt*noise_attr_;motion_filter_.Predict();/******************************************************************************  Update-更新****************************************************************************/if(measurement.sensor_type == SensorType::LIDAR){motion_filter_.H_ = H_laser_;motion_filter_.R_ = R_laser_;motion_filter_.Update(measurement.meas);}return;
}

3. 框架主流程

输入多模态数据，构建观测帧Frame，并将观测帧数据输入到Fusion融合模块中。

int main(int argc, char** argv)
{//argv[1] config, argv[2] location, argv[3] camera, argv[4] lidar, argv[5] ground truthros::init(argc, argv, "fusion_node");if (argc != 5){std::cout << "Need 4 args!\n";return 0;}std::vector<std::tuple<double, proto_input::FileTag, std::string> >&& data_frame_seq = proto_input::mergeInputSequence({proto_input::buildSensorInputSequence(argv[2], "Location", proto_input::FileTag::LOCATION),proto_input::buildSensorInputSequence(argv[3], "Lidar", proto_input::FileTag::CAMERA),proto_input::buildSensorInputSequence(argv[4], "Lidar", proto_input::FileTag::LIDAR),//proto_input::buildSensorInputSequence(argv[5], "Radar", FileTag::RADAR),});ros::NodeHandle nh;ros::Publisher lidar_pub = nh.advertise<sensor_msgs::PointCloud>("lidar_det", 50);ros::Publisher res_pub = nh.advertise<sensor_msgs::PointCloud>("fusion_result", 50);ros::Rate r(2);ezcfg::Interpreter itp;kit::perception::fusion::FusionWrapper fusion{ argv[1] };for (auto  it = data_frame_seq.begin(); it != data_frame_seq.end();){//get vehicle position and orientationitp.loadFile(std::get<2>(*it));proto_input::Location pose;itp.lex.matchID("header");itp.parse(pose.header);itp.lex.matchID("pose");itp.parse(pose.pose);++it;std::shared_ptr<kit::perception::fusion::Frame> frame = std::make_shared<kit::perception::fusion::Frame>();// 获取 camera 检测的目标数据if (std::get<1>(*it) == proto_input::FileTag::CAMERA){proto_input::CameraObject raw_co;kit::perception::fusion::CameraObjectListPtr co_list = std::make_shared<kit::perception::fusion::CameraObjectList>();itp.loadFile(std::get<2>(*it));while (itp.lex.getToken() == ezcfg::Token::ID && itp.lex.getTokenText() == "perception_obstacle"){itp.lex.next();itp.parse(raw_co);co_list->objs.push_back(makeCameraObjectPtr(raw_co, std::get<0>(*it)));}co_list->time_ns = std::get<0>(*it);frame->camera_objs = std::move(co_list);++it;}// 获取 lidar 检测的目标数据if (std::get<1>(*it) == proto_input::FileTag::LIDAR){proto_input::LidarObject raw_lo;kit::perception::fusion::LiDARObjectListPtr lo_list = std::make_shared<kit::perception::fusion::LiDARObjectList>();itp.loadFile(std::get<2>(*it));while (itp.lex.getToken() == ezcfg::Token::ID && itp.lex.getTokenText() == "perception_obstacle"){itp.lex.next();itp.parse(raw_lo);raw_lo.anchor_point.x = raw_lo.anchor_point.x - pose.pose.position.x;raw_lo.anchor_point.y = raw_lo.anchor_point.y - pose.pose.position.y;raw_lo.anchor_point.z = raw_lo.anchor_point.z - pose.pose.position.z;lo_list->objs.push_back(makeLiDARObjectPtr(raw_lo, std::get<0>(*it)));}lidar_pub.publish(transToPointCloud(*lo_list));lo_list->time_ns = std::get<0>(*it);frame->lidar_objs = std::move(lo_list);++it;}// 获取 radar 检测的目标数据（因参考资料中的数据原因，暂不实现，但可在此处添加扩展）if (std::get<1>(*it) == proto_input::FileTag::RADAR){}// 融合器Fusionfusion.Update(frame);kit::perception::fusion::FusionObjectListPtr fusion_res = std::make_shared<kit::perception::fusion::FusionObjectList>(kit::perception::fusion::FusionObjectList());fusion.GetFusionResult(fusion_res);//result outputstd::cout << "********************************************************************\n";for (size_t i = 0; i < fusion_res->objs.size(); i++)std::cout << fusion_res->objs[i]->ToString() << std::endl;res_pub.publish(transToPointCloud(*fusion_res));std::cout << "********************************************************************\n";r.sleep();}ros::shutdown();return 0;
}

融合模块对Frame数据进行处理

bool FusionWrapper::Update(const FramePtr &frame) {fusion::FusionObjectListPtr global_obj_list =std::make_shared<fusion::FusionObjectList>(fusion::FusionObjectList());// 融合 lidar 观测数据if (frame->lidar_objs->objs.size() > 0) {std::cout << "updating with lidar objects..." << std::endl;tracker_->GetGlobalObjects(global_obj_list);if (!predictor_->Predict(global_obj_list, frame->lidar_objs->time_ns)) {std::cout << "predict error for lidar measurement." << std::endl;return false; }std::map<size_t, int> local_global_map;if (!matcher_->Match(frame->lidar_objs, global_obj_list, local_global_map)) {std::cout << "match failed for lidar and global." << std::endl;return false;}tracker_->Update(frame->lidar_objs, local_global_map);}// 融合 radar 观测数据if (frame->radar_objs->size() > 0) {PreProcess(frame->radar_objs, extrinsic_radar_to_baselink_);tracker_->GetGlobalObjects(global_obj_list);if (!predictor->Predict(global_obj_list, frame->lidar_objs->time_ns)) {std::cout << "predict error for radar measurement." << std::endl;return false; }std::map<size_t, int> associate_arr;matcher_->Match(frame->radar_objs, global_obj_list, associate_arr);tracker_->Update(frame->radar_objs, global_obj_list, associate_arr);}// 融合 camera 观测数据if (frame->camera_objs->objs.size() > 0) {std::cout << "updating with camera objects..." << std::endl;tracker_->GetGlobalObjects(global_obj_list);if (!predictor_->Predict(global_obj_list, frame->camera_objs->time_ns)) {std::cout << "predict error for camera measurement." << std::endl;return false; }std::map<size_t, int> local_global_map;if (!matcher_->Match(frame->camera_objs, global_obj_list, local_global_map)) {std::cout << "match failed for camera and global." << std::endl;return false;}tracker_->Update(frame->camera_objs, local_global_map);}return true;
}

参考

https://blog.csdn.net/weixin_49401384/category_12612266.html

多传感器融合系列之感知后融合实战_ros 多传感器融合-CSDN博客

多传感器后融合算法-实战练习_多传感器融合实战-CSDN博客