OMAPL138异构平台上的毫米波雷达与单目相机融合避障实践

做无人机自主避障，我被毫米波雷达和单目相机“打架”这件事折磨过很久。即便 OMAPL138 DSP+ARM+FPGA 这套异构平台已经算很能扛，想把 77GHz 毫米波雷达输出的点云和一帧普通 RGB 图像融合成避障指令，中间还是踩了无数坑。这篇内容不是产品说明书，而是我把这套系统从立项到整机飞行踩出来的记录。适合正在做嵌入式异构计算、无人机感知、传感器融合的同学参考，尤其是想用低功耗异构平台而不是大算力开发板做避障的人。

1. 项目整体思路：为什么非要用三种芯片去伺候一套避障

先说结论：这个系统的本质不是把三个处理器凑在一起炫技，而是用最合适的器件处理最合适的数据。传感器融合最怕的就是某一瞬间某一路数据迟到、丢失、坐标系不统一。OMAPL138 的 ARM 和 DSP，再加一颗外部 FPGA，刚好能把这些事情拆开并行处理。

1.1 单目相机和毫米波雷达各有什么短板

单目相机最大的问题是“有图像、没距离”。一帧 720p 画面里能看清树干、电线、楼房边缘，但单靠像素根本算不出可靠深度，尤其是一条电线悬在空中，纯视觉很容易把它和背景混在一起。毫米波雷达正好相反，它直接给距离和多普勒速度，不怕光照变化，也不怕扬尘和雨雾，但角分辨率低，点云稀疏，拿它画不出“这是个什么东西”。

我做这套系统之前也想过只用雷达，后来发现雷达对细长目标容易产生反射点漂移，而且没有语义信息，无法判断前方是墙壁还是可穿过的树枝。只用视觉也不行，飞起来之后太阳一晃，画面过曝，或者出现大片相似纹理，视觉检测框就开始乱跳。毫米波雷达提供“距离和速度锚点”，单目视觉提供“语义和轮廓锚点”，两者在 OMAPL138 上做融合，才算把各自的短板补上。

1.2 OMAPL138 上 ARM、DSP、FPGA 怎么分工

OMAPL138 这颗芯片最大的特点是 ARM 和 DSP 封装在一起，ARM 负责控制流，DSP 负责密集计算。外挂 FPGA 是我后来坚持加上去的，因为雷达点云和图像同时进来时，纯靠 ARM 中断处理数据会非常容易丢帧。

实际分工是这样的：

很多人问：为什么不让 DSP 把视觉检测也干了？能，但没必要。DSP 更适合做确定性的数字信号处理，而不是复杂分支的图像语义分析。图像预处理放在 FPGA 的流水线里，DSP 只处理已经压缩过的候选目标和雷达点云，负载压力小很多。

1.3 融合系统要解决的三个核心问题

我把整个项目拆成三个核心问题，这是贯穿始终的主线：

第一个是空间对齐。雷达坐标系、相机坐标系、无人机机体坐标系不是同一个原点，毫米波雷达装在机头下方，相机装在机身前方，两者之间有一个固定的旋转和平移关系，必须标定，否则雷达点会投射到图像的错误位置。

第二个是时间对齐。雷达一帧数据 20ms，相机一帧 33ms，两路数据到达 ARM 的时间点不一样。如果直接用“最新数据”去融合，控制周期里可能混入早 60ms 的雷达点，无人机早就飞出老远。

第三个是置信度融合。雷达看到但视觉没看到的障碍，和视觉看到但雷达没检测到的障碍，不能一视同仁。必须给每一类目标算出一个置信度，再决定是紧急制动还是绕行。

这三个问题处理不好，后面所有算法都是空中楼阁。

2. 硬件接口与数据通路搭建

这个项目里硬件连接比算法更早让我崩溃。雷达、相机、FPGA、OMAPL138，每一对之间都有接口协议和电平匹配问题。

2.1 毫米波雷达的安装位置和数据格式

我用的是一颗 77GHz 毫米波雷达模块，水平视场角大约 90°，俯仰视场角大约 15°。雷达安装在机头正前方偏下位置，天线面朝正前方，最好用一个非金属支架把它固定，避免碳纤维机架产生二次反射。雷达输出的是稀疏点云，每个点包含距离、相对速度、方位角、俯仰角和信噪比，输出频率可以配置在 20Hz 到 50Hz，我最终用了 50Hz。

雷达坐标系定义一般是：以雷达天线阵面中心为原点，正前方为 x 轴，左侧为 y 轴，上方为 z 轴。拿到每个点的极坐标（距离 R、方位角 α、俯仰角 β）之后，先转成直角坐标：

x_r = R * cos(β) * cos(α) y_r = R * cos(β) * sin(α) z_r = R * sin(β)

这个转换看起来简单，但实际工程里很容易栽在角度符号上。各个雷达模块对“左侧为正还是右侧为正”定义不一样，我调试时发现投影到图像上的点总是左右镜像，后来才意识到是方位角符号反了。建议第一次接入雷达时，先拿一个角反射器放在正前方、正左、正右分别测试，把输出点位置和实际方位核对一遍。

2.2 相机标定与雷达坐标对齐

单目相机我用的是普通工业相机，分辨率 1280x720，30fps，镜头水平视场角大约 90°。先做相机内参标定，得到焦距 fx、fy 和主点 cx、cy。相机投影模型是：

u = fx * x_cam / z_cam + cx v = fy * y_cam / z_cam + cy

其中 x_cam、y_cam、z_cam 是目标在相机坐标系下的三维坐标。雷达点要投到图像上，需要先做外参标定，把雷达坐标系里的点转到相机坐标系：

p_cam = R_ext * p_radar + t_ext

R_ext 是旋转矩阵，t_ext 是平移向量。我的标定方法和大多数人不太一样。我没用复杂的联合标定靶，而是在无人机前方 5m、10m、15m 处各放一个角反射器，先用雷达测出角反射器的精确坐标，再在图像里手动标记角反射器中心的像素坐标，用三组以上数据解出外参。这个方法精度够用，而且操作比折腾联合标定靶省事得多。

标定的关键点是“雷达相位中心”和“相机光心”不可能做到物理重合，所以近距离目标的外参会比较敏感，距离越远，角度误差放大越明显。我一般会保留一组 20m 外的验证点，专门检查投影误差。

2.3 FPGA 做预处理：时间戳、ROI 裁剪和乒乓缓存

FPGA 在系统里的角色是“数据搬运工+预处理流水线”。它通过并行接口与 OMAPL138 连接，内存映射到 ARM 地址空间。FPGA 内部做的事情包括：

• 接收相机 MIPI/DVP 信号，完成 Bayer 转 RGB、RGB 转灰度；
• 在灰度图上做 Sobel 边缘检测，输出边缘图；
• 根据 DSP 下发的 ROI 区域，裁剪原始图像；
• 接收雷达点云串口/UART 数据，解析成固定结构体；
• 给每帧图像、每帧雷达数据打硬件时间戳；
• 用双缓冲（乒乓缓存）写入共享内存，避免 ARM 读到半帧数据。

用 FPGA 的主要原因不是炫技，而是它能保证“时间戳可靠”。软件打时间戳中间隔着操作系统和协议栈，抖动很大。FPGA 在硬件上升沿记录定时器值，时间误差可以做到微秒级，这让后面的时间对齐轻松很多。

双缓冲必须做。如果 FPGA 直接往一个内存地址写图像，ARM 可能刚读到帧头就遇到下一帧覆盖，产生“撕裂”。我用的方案是两个 1MB 缓冲区交替写入，FPGA 写完当前帧后切换缓冲区，同时置位一个标志位通知 ARM 去读旧缓冲。ARM 读完再释放。这个机制简单但非常有效。

3. 融合算法落地：从点云到避障指令

硬件通路跑通之后，真正花时间的是融合算法。我推荐的流程不是上来就写复杂的贝叶斯网络，而是先做一条“雷达主导、视觉验证”的可运行链路，再逐步加东西。

3.1 雷达点云处理链路

雷达点云从 FPGA 中断进来之后，我先在 DSP 里做一轮点云过滤。毫米波雷达最大的问题不是没数据，而是杂波太多。静止的地面、金属栏杆、无人机自身螺旋桨都可能产生反射点。我的处理顺序是：

1. 去掉距离小于 0.5m 的点，这些基本是机体自身反射；
2. 去掉信噪比低于设定阈值的点，阈值在实测中调；
3. 根据多普勒速度区分静态和动态目标，静态目标保留，动态目标重点标记；
4. 对剩余点做基于距离的 DBSCAN 聚类，把同一目标的多点合并成一个目标点。

聚类半径我设的是 0.5m，每个簇至少 3 个点才算有效目标。这个参数不能拍脑袋。我实测发现，树干的点比较集中，0.5m 聚类很好使；但电线的点非常稀疏，一个簇可能只有 1-2 个点，所以后面又加了一条规则：视觉检测到细长目标时，允许单点雷达目标进入融合。

雷达处理周期严格控制在 20ms 以内。DSP 在算 64x64 的 range-Doppler 矩阵时，我用的是 FFT 库，CFAR 检测只搜索 2m 到 60m 的 distance bin，不搜索近处，减少了约 30% 的计算量。

3.2 视觉检测与单目测距的边界

这套平台上我没有跑大型深度学习网络，ARM 性能扛不住。视觉侧实际跑的是 FPGA 预处理之后的一套轻量检测流程：边缘图生成、形态学膨胀、连通域分析、候选框输出。对于树干、电线杆、墙体这类有明显边缘的目标，这个流程已经够用。

视觉检测输出的是候选框和类别置信度，没有距离信息。单目测距理论上可以用地面假设或者目标尺寸估计，但实际误差很大。比如一根电线杆，如果不知道真实高度，单目算出的距离可能差一倍。因此在融合系统里，视觉不直接提供距离，只提供“目标在图像中的位置和类别”。

如果后续想加分类能力，我建议在 DSP 或 ARM 上跑一个轻量分类器，用来对视觉候选框做二级确认，而不是直接让它做实时检测。先把雷达的点云距离作为主要测距来源，视觉作为语义修正，系统的可靠性会明显更高。

3.3 空间对齐和时间对齐的实现细节

空间对齐的核心流程是：把每个雷达点从雷达坐标系转到相机坐标系，再投到图像像素坐标，看它落在哪个视觉候选框内。如果落在框内，就把这个距离赋给目标；如果多个雷达点落在同一个框内，取这些点的中位距离，而不是平均值。中位数对离群点更鲁棒。

具体关联逻辑可以写成这样：

for each radar_point: p_cam = R_ext * radar_point + t_ext; u = fx * p_cam.x / p_cam.z + cx; v = fy * p_cam.y / p_cam.z + cy; if (u >= box.x1 && u <= box.x2 && v >= box.y1 && v <= box.y2) { box.distance_list.add(radar_point.range); box.doppler_list.add(radar_point.doppler); }

时间对齐我做了两层。第一层是硬件层，FPGA 给雷达帧和图像帧打时间戳，ARM 收到数据后按时间戳排序；第二层是算法层，如果某时刻雷达和视觉没有刚好同一时间戳的数据，用最近邻匹配，并要求时间差小于 10ms。如果时间差大于 10ms，对雷达目标做一阶线性外推：

x(t) = x(t_i) + vx * (t - t_i) y(t) = y(t_i) + vy * (t - t_i)

速度来自雷达多普勒和连续帧位置差分的融合结果。实测下来这套简单策略比盲目等待同步信号稳定，因为在室外飞的时候，硬件触发偶尔会被 GPS 秒脉冲抢优先级，软件端有一定容错能力更重要。

3.4 避障决策与飞控接口

融合结果是一串目标列表，每个目标包含类别、像素框、距离、相对速度、方位角和置信度。ARM 拿到这个列表后，使用一个简化的“速度障碍物”思路：

• 如果目标距离大于 15m，暂时不处理；
• 如果目标距离小于 8m 且相对接近速度大于 1m/s，触发避障；
• 计算目标是静态还是动态，动态目标按当前速度外推轨迹；
• 选择一个没有碰撞风险的新航向角，输出给飞控。

避障指令我用的是自定义串口协议，20Hz 周期发送。指令包括三部分：目标 ID、建议偏航角、期望速度。飞控端只做简单的优先级判断：避障系统指令优先级高于航线指令，但低于遥控器手动指令。这样万一算法抽风，操控手还能接管。

融合置信度计算也很直接。最终置信度等于视觉置信度和雷达确认度的加权和。视觉检测到、雷达点也确认的目标，置信度最高；雷达检测到、视觉没发现的动态目标，置信度中等，但如果雷达点连续 3 帧稳定存在，也会触发避障；只有视觉候选框但无雷达点确认的目标，默认不触发紧急制动，只作为提示信息。这套规则帮我过滤了绝大多数误报。

4. 工程化过程中的关键实现与性能调优

算法能跑和能稳定跑是两回事。回到 OMAPL138 这种资源受限的平台上，工程化比算法本身更容易劝退人。

4.1 DSP 端的定点化改造

虽然 OMAPL138 的 C674x DSP 支持浮点运算，我在实际做共享数据通信时，还是把大部分角度、距离参数归一化成了 Q15 定点数。这么做不是因为浮点算不了，而是因为 ARM 和 DSP 之间共享内存，用定点数可以减少带宽占用，也避免两核对字节序和浮点格式理解不一致。

角度存储我用了 int16_t，单位是 0.01 度，距离用 int16_t 表示 0.01m，速度用 int16_t 表示 0.01m/s。定点化之后，关联算法里做坐标转换时，我不再频繁调用三角和开方函数，三角函数用查表，开方只在目标输出阶段调用一次。

#define Q15_ONE 32768.0f int16_t range_q15 = (int16_t)(range_m * Q15_ONE / RANGE_MAX); int16_t angle_q15 = (int16_t)(azimuth_rad * 32768.0f / PI);

这个改动让 DSP 端一帧雷达处理时间从约 35ms 降到了 18ms。代价是代码可读性变差，所以我在每个变量命名里都带上了单位后缀，比如 range_cm、vel_cm_s、azimuth_0p01deg，避免几个星期后自己看不懂。

4.2 多核间通信的 Cache 一致性处理

ARM 和 DSP 共享外部 DDR 内存，最容易踩的坑是 Cache 一致性问题。ARM 写了一个共享标志位，DSP 去读却可能读到 Cache 里的旧值。我一开始没注意，现象就是系统跑几分钟后避障逻辑突然完全不响应，重启就好，非常隐蔽。

解决方法是规范一套共享内存通信协议。我在固定地址放了一个结构体数组，所有消息要么是“控制命令”，要么是“数据帧描述符”，不允许跨核传指针。每次 ARM 写数据前做 Cache clean，DSP 读数据前做 Cache invalidate；反向操作同理。

共享内存结构体大概长这样：

typedef struct { uint32_t magic; uint32_t frame_id; uint32_t ts_ms; uint16_t radar_num; uint16_t vision_num; int16_t obstacle_dist_cm[16]; int16_t obstacle_angle_0p01deg[16]; uint16_t obstacle_conf[16]; } fusion_result_t;

这个结构体大小固定，避免动态内存分配。两个核之间用一个 32 位的标志位做握手，谁写谁置位，读完立即清位。千万不要写“先清标志再读数据”这种伪同步，数据还没读完就把标志清了，另一核会立刻覆盖缓冲区。

4.3 系统实时性与帧率平衡

整机联调时，我最开始把雷达处理放在 20ms 定时中断里，视觉 ROI 处理放在主循环里，结果一旦视觉处理时间抖动，雷达中断就会超时。后来改成三层调度：

• FPGA 中断：只负责把数据搬进共享内存，置位标志，不处理业务；
• DSP 定时中断：20ms 处理一次雷达点云和融合；
• ARM 主循环：50ms 发一次避障指令，10ms 检查一遍心跳。

雷达处理优先级最高，视觉 ROI 更新可以隔几帧做一次。实测下来，避障指令周期能稳定在 20ms，最差不超过 25ms，满足低速无人机的避障需求。如果你的飞控响应更快，可以把 FPGA 触发周期改成 10ms，但 DSP 端的 FFT 和聚类都要跟着优化，否则中断会越积越多。

4.4 整机联调流程

我的联调流程分四步，每步都设了明确的通过标准：

第一步是桌面测试。无人机固定在地面，人拿着角反射器在正前方 2m 到 20m 之间移动，通过上位机查看雷达点和视觉框是否对齐。通过标准是投影误差小于 15 像素，距离误差小于 0.5m。

第二步是转台测试。无人机通电但不解锁，左右旋转机头，观察目标在图像和雷达坐标中是否同步移动。这一步能筛掉大量外参标定错误。

第三步是系留飞行。无人机用绳索系留，在 1m 高度悬停，前方放一块泡沫板，缓慢靠近，验证避障指令能正确触发。

第四步才是自由飞行。先在小范围、无障碍位置手动飞行，然后逐渐增加障碍物。这个顺序看着保守，但能让你在每一层发现问题，而不是直接炸机。

5. 常见问题与排障实录

这里记录几个我在实际过程中遇到最多的问题，按照“现象-原因-处理方式”整理成速查表。

5.1 雷达点和画面里的目标对不上

这个是最常见，也最让人恼火的。雷达点在正前方，但投影到图像里偏了 20 多个像素。排查时先不要怀疑算法，先检查机械安装。我用激光水平尺确认雷达和相机的安装面是否平行，发现云台固定板的加工公差导致雷达实际向左偏了 1.3°。1.3°在 20m 外就是 0.45m 的偏差，足够让投影完全错位。

解决办法是做一个简单的安装定位工装，保证雷达和相机在同一个基准平面，再用角反射器做一次整体外参验证。标定结果我用了一个矩阵检查：把雷达原点投影到图像上，应该落在相机画面的正中心附近。如果偏得离谱，说明外参初值给错了。

5.2 一上电 FPGA 不干活

FPGA 不干活大多数不是逻辑写错了，而是配置时序或复位时序问题。我遇到过一次：相机传感器初始化没完成，FPGA 一直在等待 I2C 应答，导致图像通路永远无数据。后来我在 FPGA 里加了一个独立看门狗，如果相机 I2C 配置在 500ms 内没完成，FPGA 主动重新初始化一次。

另一个坑是复位信号。ARM 通过 GPIO 控制 FPGA 的复位脚，如果 ARM 系统时钟还没稳定就开始拉高复位，FPGA 起来后状态混乱。正确顺序是：FPGA 先自己复位延时 200ms，再等待 ARM 释放复位，然后握手成功后再开始传输数据。上电时序这种东西，看起来是小事，实际能浪费你一整天。

5.3 无人机飞起来后雷达数据抖动严重

雷达在地面测试非常稳定，一解锁电机转起来，点云数量突然变少，杂点变多。排查后发现给雷达供电的 3.3V 是从电调 BEC 出来的，电机负载变化导致电压跌落超过 5%。雷达模块对电源纹波特别敏感，尤其是毫米波雷达的锁相环，一旦电源波动，帧同步就会丢。

处理方式是给雷达单独加了一路低纹波 LDO，并在电源输入端加了大电容和磁珠。另外把雷达模块尽量远离电调和电机，用尼龙柱固定，避免金属机架反射。改动之后，雷达在悬停时点云数量恢复稳定，丢帧率从 8% 降到 1% 以内。

5.4 DSP 计算时间过长导致控制周期超时

硬件和算法联调后，最让人头疼的是 DSP 端耗时抖动。我最初把 CFAR 检测写成两层 for 循环，遍历所有距离和速度 bin，在 64x64 的 range-Doppler 图上还能忍，但真机飞行时雷达目标多，聚类耗时开始波动。

优化思路是缩短搜索空间。既然无人机避障只关心正前方 60m 内、水平 ±45° 内的目标，DSP 完全可以只处理雷达点云形成后的目标，而不是每一帧都对整个 range-Doppler 图做检测。我把 CFAR 检测和聚类合并：先粗检测出峰值点，再对峰值点附近的相邻点做连通性判断，计算量直接减半。最后再加了一个性能计数器，如果 DSP 端某帧处理超过 22ms，就把该帧丢弃，不送到融合模块，避免控制周期被拖垮。

6. 个人踩坑后的几点体会

如果只让我说一个最深的体会，那就是：异构平台项目的瓶颈从来不是某个核心性能不够，而是数据在核与核之间、传感器与处理器之间流动时的不确定性。算法写得再漂亮，如果 FPGA 给的时间戳漂移、共享内存被两核同时改写，结果一样是错的。所以我会建议所有做类似项目的朋友，先把“数据通路”当做一个独立子项目来做，通路稳定了再谈融合算法。

另一个体会是：一定要先做数据录制和离线回放。我在室外飞行测试前，把雷达点云、图像、融合结果、避障指令全部按照固定的文件格式录制下来。出了问题后，我可以反复回放同一段数据，调整融合参数，而不是每次都要重新飞一遍。这套“录制-回放-调参”的习惯，救了我很多次。

最后分享一个小技巧：系统里所有关键参数，比如外参标定矩阵、ROI 区域、安全距离阈值、融合置信度权重，都做成启动时从参数文件加载，而不是硬编码在代码里。这样每次飞行测试后改参数只需要改配置文件，不用重新编译烧写。否则一次飞行试下来要重新编译三次，效率低到你想放弃。

如果你也正在做类似的 OMAPL138 DSP+ARM+FPGA 融合避障项目，我的建议是从最简单的雷达点云+视觉框关联开始，先跑通一版“不那么聪明但稳定”的避障功能，再逐步加置信度、预测滤波和更复杂的决策逻辑。多传感器融合系统的可靠度是聊出来的、试出来的，不是一次设计出来的。