征程6上多传感器融合感知：摄像头+毫米波雷达+BPU调度实战经验

L2+级智驾系统不能只靠摄像头，毫米波雷达在恶劣天气和测距精度上有不可替代的优势。最近项目里需要在征程6上实现摄像头+毫米波雷达的融合感知，把两个传感器的数据在BPU上统一处理。这篇把融合方案的设计、BPU调度策略、以及踩过的坑完整记下来。

一、为什么要做融合？

摄像头和毫米波雷达各有优劣：

能力 摄像头 毫米波雷达

目标分类 强（可以识别行人、车辆、交通标志） 弱（只能区分动静目标）

测距精度 中（单目误差大，双目计算量大） 强（直接输出距离、速度）

恶劣天气 弱（雨雾天能见度低） 强（不受光照和天气影响）

角分辨率 高（图像像素级） 低（角度分辨率通常只有几度）

速度测量 弱（需要通过帧间位移估算） 强（直接输出径向速度）

成本 低 中

融合的思路很简单：用摄像头做目标识别和精细定位，用雷达做距离测量和速度测量，两者互补。

二、融合架构设计

我们的融合架构分三层：

```

传感器层

├── 摄像头：1920x1080@30fps，ISP输出NV12

└── 毫米波雷达：CAN接口，20Hz，输出目标列表（距离、角度、速度、RCS）

感知层

├── 摄像头分支：BPU跑YOLOv5m检测模型，输出2D框+类别

├── 雷达分支：CPU做聚类+跟踪，输出雷达目标（带ID和速度）

└── 融合模块：CPU做数据关联+状态融合，输出融合目标

应用层

└── 融合目标列表（包含：位置、速度、类别、置信度、跟踪ID）

```

关键设计决策：

1. 摄像头分支跑在BPU上：检测模型用BPU加速，延迟22ms

2. 雷达分支跑在CPU上：雷达数据量小（每帧最多64个目标），CPU处理就够了

3. 融合模块跑在CPU上：融合逻辑涉及数据关联和Kalman filter，不适合BPU

4. 两个分支并行：摄像头和雷达的处理是独立的，可以并行跑

三、摄像头分支：BPU检测

摄像头分支用YOLOv5m在BPU上跑检测，和单摄像头方案基本一致。需要注意的几点：

1. 输出格式要兼容融合模块

检测输出除了2D框（x, y, w, h）和类别，还需要输出深度估计（如果不用雷达测距的话）。但我们有雷达，所以摄像头分支只需要输出2D框和类别就够了，距离和速度由雷达提供。

2. 时间同步

摄像头30fps（33ms一帧），雷达20Hz（50ms一帧）。两者的数据不是同时产生的，需要做时间同步。

我们的做法：以摄像头为基准（30fps），每帧摄像头数据到来时，找时间上最近的雷达数据（前后25ms内）进行融合。

```cpp

struct SensorData {

uint64_t timestamp; // 微秒

// ... 其他数据

};

RadarData FindNearestRadar(uint64_t camera_timestamp,

const std::vector& radar_buffer) {

RadarData nearest;

uint64_t min_diff = UINT64_MAX;

for (const auto& radar : radar_buffer) {

uint64_t diff = abs((int64_t)radar.timestamp - (int64_t)camera_timestamp);

if (diff < min_diff && diff < 25000) { // 25ms内

min_diff = diff;

nearest = radar;

}

}

return nearest;

}

```

3. BPU推理和雷达处理的并行

摄像头ISP输出NV12格式，需要先转成RGB再送给BPU。这个预处理在IPU上做，不占用CPU和BPU资源。

```cpp

// 并行pipeline

thread camera_thread([&](){

while (running) {

auto frame = camera.Capture(); // ISP输出NV12

auto rgb = IPUConvertNV12toRGB(frame); // IPU硬件转换

bpu_input_queue.push(rgb);

}

});

thread bpu_thread([&](){

while (running) {

auto rgb = bpu_input_queue.pop();

auto detections = BPUInference(yolov5m_model, rgb);

detection_queue.push(detections);

}

});

thread radar_thread([&](){

while (running) {

auto radar_targets = ParseRadarCANFrame(can_bus.Read());

radar_buffer.push_back(radar_targets);

if (radar_buffer.size() > 10) radar_buffer.erase(radar_buffer.begin());

}

});

thread fusion_thread([&](){

while (running) {

auto detections = detection_queue.pop();

auto radar = FindNearestRadar(detections.timestamp, radar_buffer);

auto fused = Fusion(detections, radar);

output_queue.push(fused);

}

});

```

这个pipeline的瓶颈在BPU推理（22ms），其他环节都小于10ms。端到端延迟约30ms，帧率30fps。

四、雷达分支：聚类+跟踪

毫米波雷达原始输出是反射点（point cloud），需要先聚类成目标，再做跟踪。

聚类（DBSCAN）

```cpp

std::vector ClusterRadarPoints(const std::vector& points) {

std::vector targets;

std::vector visited(points.size(), false);

for (size_t i = 0; i < points.size(); i++) {

if (visited[i]) continue;

// 找邻居

std::vector neighbors;

for (size_t j = 0; j < points.size(); j++) {

if (Distance(points[i], points[j]) < EPSILON) {

neighbors.push_back(j);

}

}

if (neighbors.size() < MIN_POINTS) {

visited[i] = true;

continue; // 噪声点

}

// 形成新目标

RadarTarget target;

for (size_t idx : neighbors) {

visited[idx] = true;

target.points.push_back(points[idx]);

}

// 计算目标中心

target.x = average(target.points, [](p){ return p.x; });

target.y = average(target.points, [](p){ return p.y; });

target.vx = average(target.points, [](p){ return p.vx; });

target.vy = average(target.points, [](p){ return p.vy; });

target.rcs = max(target.points, [](p){ return p.rcs; });

targets.push_back(target);

}

return targets;

}

```

DBSCAN参数选择：

· EPSILON（邻域半径）：对于77GHz雷达，距离分辨率约0.2m，建议设成0.5m

· MIN_POINTS（最小点数）：设成3，避免把单个噪声点当成目标

跟踪（Kalman Filter）

雷达目标需要跟踪来维持ID一致性（同一辆车在多帧之间要有相同的ID）。

```cpp

class RadarTracker {

struct Track {

int id;

KalmanFilter kf; // 状态：[x, y, vx, vy]

int age; // 连续跟踪帧数

int missed; // 连续丢失帧数

};

std::vector tracks_;

int next_id_ = 0;

public:

std::vector Update(const std::vector& detections) {

// 1. 预测

for (auto& track : tracks_) {

track.kf.Predict();

}

// 2. 数据关联（匈牙利算法）

auto matches = HungarianAssociation(tracks_, detections);

// 3. 更新匹配的目标

for (const auto& [track_idx, det_idx] : matches) {

tracks_[track_idx].kf.Update(detections[det_idx]);

tracks_[track_idx].age++;

tracks_[track_idx].missed = 0;

}

// 4. 删除丢失太久的track

tracks_.erase(

std::remove_if(tracks_.begin(), tracks_.end(),

[](const Track& t) { return t.missed > 5; }),

tracks_.end()

);

// 5. 创建新track

for (size_t i = 0; i < detections.size(); i++) {

if (!IsMatched(i, matches)) {

Track new_track;

new_track.id = next_id_++;

new_track.kf.Init(detections[i]);

new_track.age = 1;

new_track.missed = 0;

tracks_.push_back(new_track);

}

}

// 返回所有活跃的track

std::vector results;

for (const auto& track : tracks_) {

if (track.age > 3) { // 至少跟踪3帧才输出

RadarTarget t;

t.id = track.id;

auto state = track.kf.GetState();

t.x = state[0]; t.y = state[1];

t.vx = state[2]; t.vy = state[3];

results.push_back(t);

}

}

return results;

}

};

```

Kalman Filter的状态向量： [x, y, vx, vy]，观测向量：[x, y]（雷达直接测距和角度，可以转成x, y）。

注意：

雷达的测量噪声和目标的RCS（雷达截面积）有关。大车辆RCS大，测量精度高；小行人RCS小，测量噪声大。Kalman Filter的观测噪声矩阵R应该根据RCS动态调整：

```cpp

// RCS越大，测量越准，R越小

float R_scale = 1.0f / (target.rcs / 10.0f + 1.0f); // 归一化

R = R_base * R_scale;

```

五、融合模块：数据关联+状态融合

数据关联

摄像头检测出N个目标，雷达跟踪出M个目标，需要判断哪些是同一个物理目标。

关联依据：

1. 空间距离：摄像头目标的2D框中心和雷达目标的投影位置距离

2. 速度一致性：摄像头目标的速度（通过帧间位移估算）和雷达目标的测速应该一致

3. 尺寸一致性：摄像头目标的大小和雷达目标的RCS应该正相关

```cpp

float AssociationScore(const CameraDetection& cam, const RadarTarget& radar) {

// 1. 空间距离分数（2D图像平面）

float cam_center_x = cam.x + cam.w / 2;

float cam_center_y = cam.y + cam.h / 2;

// 雷达目标投影到图像平面（需要相机标定参数）

float radar_img_x, radar_img_y;

ProjectRadarToImage(radar.x, radar.y, radar_img_x, radar_img_y);

float spatial_dist = sqrt(pow(cam_center_x - radar_img_x, 2) +

pow(cam_center_y - radar_img_y, 2));

float spatial_score = exp(-spatial_dist / 100.0f); // 距离越近分数越高

// 2. 速度一致性分数

float cam_speed = sqrt(cam.vx * cam.vx + cam.vy * cam.vy);

float radar_speed = sqrt(radar.vx * radar.vx + radar.vy * radar.vy);

float velocity_diff = abs(cam_speed - radar_speed);

float velocity_score = exp(-velocity_diff / 5.0f); // 速度差越小分数越高

// 3. 加权求和

return 0.6f * spatial_score + 0.4f * velocity_score;

}

```

匈牙利算法做全局最优关联：

```cpp

// 构建关联矩阵

Eigen::MatrixXf cost_matrix(cam_detections.size(), radar_tracks.size());

for (size_t i = 0; i < cam_detections.size(); i++) {

for (size_t j = 0; j < radar_tracks.size(); j++) {

cost_matrix(i, j) = 1.0f - AssociationScore(cam_detections[i], radar_tracks[j]);

}

}

// 匈牙利算法求解

auto matches = HungarianAlgorithm(cost_matrix);

```

状态融合

关联上之后，融合摄像头和雷达的测量值。

融合策略： 用加权平均，权重根据各传感器的置信度确定。

```cpp

FusedTarget Fuse(const CameraDetection& cam, const RadarTarget& radar) {

FusedTarget fused;

// 位置融合：雷达测距准，摄像头测角度准

// 转换到世界坐标系融合

float cam_world_x, cam_world_y;

ImageToWorld(cam.x + cam.w/2, cam.y + cam.h, cam_world_x, cam_world_y);

float radar_world_x = radar.x;

float radar_world_y = radar.y;

// 权重：雷达距离权重高，摄像头横向（角度）权重高

float w_radar_dist = 0.7f; // 雷达距离权重

float w_cam_angle = 0.7f; // 摄像头角度权重

fused.x = w_radar_dist * radar_world_x + (1 - w_radar_dist) * cam_world_x;

fused.y = w_cam_angle * radar_world_y + (1 - w_cam_angle) * cam_world_y;

// 速度：雷达直接测量，权重更高

fused.vx = 0.8f * radar.vx + 0.2f * cam.vx;

fused.vy = 0.8f * radar.vy + 0.2f * cam.vy;

// 类别：摄像头识别

fused.class_id = cam.class_id;

fused.class_confidence = cam.confidence;

// 融合置信度

fused.confidence = 0.6f * cam.confidence + 0.4f * (radar.rcs / 100.0f);

// 跟踪ID：继承雷达的ID（雷达跟踪更稳定）

fused.track_id = radar.id;

return fused;

}

```

注意： 摄像头和雷达的坐标系不同，融合前必须统一坐标系。一般做法是：

1. 摄像头图像坐标 -> 相机坐标（用内参矩阵）

2. 相机坐标 -> 世界坐标（用外参矩阵，即相机安装位置和朝向）

3. 雷达坐标已经是世界坐标（以雷达为原点）

4. 在世界坐标系下做融合

六、踩坑记录

坑1：雷达和摄像头的安装位置标定

雷达和摄像头的安装位置有偏差（比如摄像头在挡风玻璃后，雷达在保险杠），两者的坐标原点不一致。如果标定不准，融合后的目标位置会系统性偏差。

解决：

用标定板同时标定两个传感器。在空旷场地放置标定板，让两个传感器同时检测，用最小二乘法求外参矩阵。

坑2：时间同步误差

摄像头和雷达的时钟可能不同步（比如摄像头用系统时钟，雷达用CAN总线时钟）。如果时差超过5ms，快速运动的目标（比如对向车辆）位置偏差会很大。

解决： 用PTP（Precision Time Protocol）同步两个传感器的时钟，或者用GPS授时。

坑3：雷达多径效应

雷达信号在隧道、桥梁、高架桥下会发生多径反射，产生虚假目标。这些虚假目标和真实目标在雷达数据里无法区分，但摄像头可以看到那里没有目标。

解决：

融合模块里加一层验证：如果雷达检测到一个目标但摄像头在对应位置没有检测到，降低该目标的置信度或者标记为"待验证"。

坑4：摄像头漏检时的雷达目标处理

雷达检测到一个目标但摄像头没有检测到（比如目标在摄像头盲区或者被遮挡）。这时候融合模块不能只依赖摄像头，要保留雷达目标。

解决：

融合输出里保留"雷达-only"的目标，类别标记为"unknown"，置信度按雷达RCS计算。

七、性能数据

融合系统在J6M上的性能：

模块 延迟 运行单元

摄像头ISP 5ms ISP硬件

BPU检测 22ms BPU

雷达聚类 2ms CPU

雷达跟踪 1ms CPU

数据关联 3ms CPU

状态融合 1ms CPU

端到端 30ms -

帧率30fps，满足L2+级智驾的实时性要求。

八、注意事项总结

1. 摄像头和雷达必须时间同步：时差超过5ms，高速目标的位置偏差会很大。

2. 坐标系统一后再融合：摄像头图像坐标要先转到世界坐标。

3. 雷达多径虚假目标要靠摄像头过滤：隧道/高架桥场景特别注意。

4. Kalman Filter的R矩阵要动态调整：RCS大的目标测量精度高，R可以设小。

5. 匈牙利算法做全局最优关联：贪婪匹配容易出错。

6. 摄像头漏检时保留雷达目标：标记为"unknown"，不要丢弃。

7. DBSCAN参数根据雷达型号调：77GHz雷达EPSILON设0.5m。

8. 跟踪至少3帧才输出：避免瞬时噪声被当成目标。

9. BPU和CPU任务并行：ISP、BPU、CPU后处理形成pipeline。

10. 融合权重根据场景动态调整：恶劣天气时雷达权重提高，摄像头权重降低。