多模态 Radio-Vision SLAM 原型：数据集复现指南、Particle-RFS 算法结构与波束管理实测
环境：MATLAB R2022b+（新增以免影响
readmatrix、深度深学习 YOLO 相关工具箱按需）+ Python 3.x（numpy / scipy / torch / matplotlib / openpyxl / Pillow）数据集：Zenodo DOI 10.5281/zenodo.22896456（约数百 MB，仓库内不含数据）
论文：Multi-Modal Radio-Vision SLAM for ISAC: Prototype Design, Experimental Validation and Beam-Management Applications, IEEE TWC vol. 25, pp. 22734-22748, 2026
摘要
本文给出一套多模态 ISAC 原型的数据集复现指南与算法结构说明：毫米波相控阵（8×8 UPA，等效 8 阵元 ULA）产生 64×64 RSRP 测量，与固定双目相机、UE 机载 IMU 融合，在基于粒子的一随机有限集（RFS）后验上联合估计 UE 状态、散射体与虚拟锚点。文中给出数据集目录与字段格式、MATLAB / Python 两条读取路径、RSRP 热力图象限重排这一必须步骤，以及全部实测结果表：明亮条件下 UE 定位误差 0.30 m、地图 OSPA 1.61 m、波束方向角误差 0.0403 rad、遮挡检出率 84.33%；并指出弱光下松耦合架构在地图构建上会倒输给模态更少的 IMU 辅助方案这一现象及其成因。
一、工程结构
text
code/
├── slam/
│ ├── run_slam_bright.m # 明亮条件主脚本
│ ├── run_slam_dark_light.m # 弱光条件主脚本
│ ├── functions_bright/ # 93 个辅助函数（含 UESLAM、initAnchors_unbiased 等）
│ └── functions_dark_light/
├── beam_management/
│ ├── main.m / main2.m / main3.m # 波束跟踪与遮挡预测
│ └── functions/ # 约 77 个辅助函数
├── common/beam2.xlsx # 波束码本
├── camera_python/ # 视觉处理脚本
└── data_example/read_dataset.py # Python 数据读取与预览工具
```
MATLAB 侧运行：
```matlab
addpath(genpath('code/slam/functions_bright'));
beammap = readmatrix(fullfile('code','common','beam2.xlsx'));
beammap = beammap(:,2);
% 主流程：code/slam/run_slam_bright.mPython 侧预览：
bash
pip install numpy scipy torch matplotlib openpyxl Pillow
python code/data_example/read_dataset.py --dataset slam_bright --frame 10
read_dataset.py中ROOT = parents[2]假定脚本位于<root>/code/data_example/，数据解压在<root>/data/。路径是相对约定，移动目录会失效。
二、数据集目录与字段
text
data/
├─ slam/
│ ├─ bright/traceS/ └─ dark/traceS/
│ ├─ rgb/ *_rgb.npy # RGB 图像
│ ├─ depth/ *_depth.npy # 深度图
│ ├─ qatm/ *_rgbQATM.txt # 多目标模板匹配检测结果
│ ├─ yolo/ *_rgb.txt # YOLO 检测：class x y w h
│ ├─ pose/ json *.txt
│ └─ rsrp/ pss *.txt # 64×64 RSRP 热力图
├─ beam_management/
│ └─ bright|dark / los / block|noblock / trace1-0-1 ... trace1-0-10
├─ imu/ IMU_S_trace.mat
└─ calibration/beam2.xlsx # 波束码本read_dataset.py 会打印各张量形状与位姿摘要，并导出 RGB / 深度 / RSRP 热力图的 PNG 预览。接触新数据集时应先跑这一步 ，先确认维度与坐标系，再写处理逻辑。
三、原型系统参数
| 部件 | 型号 | 说明 |
|---|---|---|
| SDR | USRP-2974 | 数据处理与存储核心 |
| 毫米波相控阵 | mmPSA-TR64MX | 8×8 面阵，单射频通道，64×64 二维扫描 |
| 机器人底盘 | Yunji WATER2 | 承载移动 UE |
| 双目相机 | Intel RealSense D455 | RGB + 深度，固定安装 |
射频链路：SDR 输出 2.5 GHz 中频，由相控阵内置本振上变频到 28 GHz 。两节点同层部署，水平维度之外可忽略，相控阵近似为 8 阵元 ULA ，码本 M = N = 64 M=N=64 M=N=64。
关键约束 ：波束扫描码本的角覆盖只有 2 π / 3 2\pi/3 2π/3 rad（120°），因此 UE 必须在预设航点停下原地旋转才能配合 PA 完成全角域 RSRP 采集，实验采用 stop-and-go 协议。
虚拟锚点真值： 12.93 , 0 12.93,0 12.93,0、 0 , 12.58 0,12.58 0,12.58、 0 , − 5.48 0,-5.48 0,−5.48 m。UE 最大移动速度 0.5 m/s。
三类测量与时延预算（Intel Core i7-13700）
| 环节 | 时延 |
|---|---|
| UE 侧联合处理（SLAM + 波束管理） | 0.035 s / 周期 |
| BS 侧多目标识别 | 0.1 s / 帧（CPU） |
| 完整 64×64 RSRP 采集 | 1.28 s |
瓶颈在采集而非算法：0.1 s 识别耗时相对 1.28 s 窗口余量充足，因此未做硬件加速；1.28 s 把有效波束跟踪限制在约 1 m/s。硬件原生帧结构是 20 ms，换专用处理单元即可支持高移动性。
四、算法结构
4.1 粒子化的三类状态
matlab
posteriorParticlesAgent = cell(J,1); % UE 状态粒子
posteriorParticlesScatterers = cell(J,1); % 散射体粒子
posteriorParticlesAnchors = cell(J,1); % 虚拟锚点（VA）粒子主循环三个关键调用：
matlab
initAnchors_unbiased(parameters{iterT,1}, 1, posteriorParticlesAgent{j,1}, Measurement{j,1}, dataVA);
UESLAM(11, Measurement{1,1}, parameters{iterT,1}, estimatedScatterers{j,1}, ...
posteriorParticlesScatterers{j,1}, estimatedAnchors{j,1}, Location(:,i));
multimodal_fusion_old(i, j, eSLAM, SLAM_Trajectory, theta0, Robot, Cam_numb, RGBlabel, DepthImg, ...);这就是"紧耦合"的实现含义：视觉与惯性的量测直接进入同一个后验的更新步骤，而不是先各自产生独立定位结果再加权平均。IMU 分支用协方差前向传播参与联合更新：
matlab
IMUindependentLocation = estLocation(:,i);
... = IMUindependentLocation + [noise; zeros; zeros] * randn(1, parameters{iterT,1}.numParticles);4.2 RSRP 热力图的象限重排（必做）
matlab
HeatMap0 = HeatMap(64:-1:1, 64:-1:1);
for ii = 1:64
for jj = 1:64
HeatMap0(ii,jj) = HeatMap(64+1-ii, 64+1-jj);
end
end
HeatMap1(1:32,1:32) = HeatMap0(33:end,33:end);
HeatMap1(33:end,1:32) = HeatMap0(1:32,33:end);
HeatMap1(1:32,33:end) = HeatMap0(33:end,1:32);
HeatMap1(33:end,33:end) = HeatMap0(1:32,1:32);
imagesc(HeatMap2)
xlabel('UpLink AoA'); ylabel('UpLink AoD');两次翻转加一次四象限重排，把原始 M × N M\times N M×N RSRP 矩阵对齐到统一 AoA/AoD 坐标系。缺失这一步会造成系统性镜像或平移错误，且算法不会报错，只会安静地变差。 复现时应先用已知位置的一次测量验证坐标轴方向。
4.3 评价度量
- 定位：真实与估计 UE 位置的欧氏距离；
- 地图：OSPA，截止距离 10 m，阶数 1（同时刻画 VA 数量误差与位置误差）；
- 波束：码本索引误差折算为角度误差。
五、实验结果
5.1 定位（轨迹 S，明亮）
| 方案 | 平均定位误差 |
|---|---|
| 理想相机（理论下界） | 0.10 m |
| IMU 纯积分 | 2.5 m |
| Radio-SLAM | 0.90 m |
| IMU 辅助 SLAM | 0.51 m |
| 松耦合 LC-MM | 0.32 m |
| 紧耦合 TC-MM | 0.30 m |
相对单模态 Radio-SLAM 降低 66.7%。
两点注意：理想相机在 t = 626 s 也出现过一次目标漏检，说明视觉的偶发失效是结构性的；IMU 纯积分仅 2.5 m 累积误差远好于典型纯惯导，原因是 stop-and-go 协议提供了周期性零速更新。实验协议会显著改变基线强弱，必须在论文中交代。
5.2 地图构建 OSPA
| 方案 | 明亮 | 弱光 |
|---|---|---|
| Radio-SLAM | 3.00 m | 3.10 m |
| IMU 辅助 SLAM | 2.60 m | 2.45 m |
| 松耦合 LC-MM | 2.43 m | 2.80 m |
| 紧耦合 TC-MM | 1.61 m | 2.05 m |
明亮时相对 Radio-SLAM 降低 46.3% ，弱光下相对 LC-MM 降低 26.8%。
5.3 弱光定位与性能倒挂
| 方案 | 定位误差 |
|---|---|
| Radio-SLAM | 1.82 m |
| IMU 辅助 SLAM | 0.91 m |
| 松耦合 LC-MM | 0.85 m |
| 紧耦合 TC-MM | 0.44 m |
地图构建出现倒挂：LC-MM（2.80 m）输给模态更少的 IMU 辅助 SLAM（2.45 m） 。成因是松耦合先让三模态各自产生独立定位结果再融合，视觉频繁漏检使前级输出方差剧烈波动，融合滤波器无法稳定------一个坏输入不断冲击滤波器，比不给它输入更糟。紧耦合可在视觉失效时降级到 IMU 状态演化加 NLoS 无线测量。
注意弱光下 RSRP 质量本身也下降（Radio-SLAM 0.90 → 1.82 m），差距不完全来自视觉。
5.4 波束跟踪（90 次试验）
| 条件 | UE 定位误差 | 视觉辅助成功率 | 波束索引误差 | 角度误差 |
|---|---|---|---|---|
| 明亮 | 0.23 m | 91.27% | 1.23 | 0.0403 rad（2.31°） |
| 弱光 | 0.35 m | 66.26% | 1.46 | 0.0477 rad（2.73°） |
| 单模态对照 | 1.64 m | --- | 2.17 | 0.0710 rad（4.07°） |
LoS 与 NLoS 分开评估，因为两者路径损耗差异巨大，同时评估时主瓣旁瓣会严重干扰较弱的 NLoS 波束。
反直觉现象：UE 越靠近 PA，定位越准但波束跟踪略微变差------近距离时同样位移对应更大角度变化，角度敏感度超过了定位精度的收益。因此波束跟踪误差预算必须写成"定位误差 × 几何敏感度"。
5.5 遮挡预测
| 条件 | 方案 | 检出率 | 虚警率 |
|---|---|---|---|
| 明亮 | 多模态 | 84.33% | 5.12% |
| 弱光 | 纯视觉 | 55.90% | --- |
| 弱光 | 纯无线 | --- | 32.26% |
| 弱光 | 多模态 | 82.72% | 12.10% |
弱光下视觉检出率从 91.27% 崩到 55.90%（YOLO 退化），纯无线虚警率高达 32.26%。多模态仍保持 82.72% 检出率，代价是虚警升至 12.10%------成因是定位误差传播到波束传播轨迹计算从而误触发遮挡判定 。遮挡场景下系统性能增益约 4 dB。
六、排错 checklist
| 症状 | 排查方向 |
|---|---|
| 路径找不到数据 | read_dataset.py 假定脚本在 <root>/code/data_example/，数据在 <root>/data/ |
| AoA/AoD 出现镜像或系统性偏移 | 漏做 RSRP 热力图的象限重排（见 4.2） |
readmatrix 读不到码本 |
检查 code/common/beam2.xlsx 并确认取第 2 列 |
| 视觉效果远差于论文 | 确认走的是 functions_bright 而非 functions_dark_light 版本 |
| IMU 基线误差远大于 2.5 m | 实验协议改变了，确认是否有周期性零速更新段 |
| NLoS 波束被主瓣旁瓣淹没 | LoS 与 NLoS 必须分开评估 |
| 波束跟踪反而不如近距离 | 检查是否混淆了定位误差与角度敏感度（见 5.4） |
七、已知边界
- 暗光下 RSRP 质量本身下降（Radio-SLAM 0.90 → 1.82 m）；
- stop-and-go 协议，不支持连续运动场景评估；
- 8 阵元等效 ULA，二维面阵降维使用，仅考虑水平角；
- 1.28 s 采集窗口限制有效速度约 1 m/s，硬件能力为 20 ms 帧；
- 相机固定安装，依赖外部已标定相机，与 UE 机载感知不同；
- 仅考虑一阶镜面反射，高阶反射与漫散射并入高斯噪声。
八、小结
复现这套系统时最容易踩的坑不在算法，而在数据坐标系：pss *.txt 的 RSRP 矩阵不经过象限重排就会安静地给出带镜像的角度；read_dataset.py 中 ROOT = parents[2] 的路径约定也不容移动。真正值得学习的则是它的两条设计原则------紧耦合的价值体现在失效模式而非峰值精度（弱光地图构建中松耦合倒输给更少模态就是明证），以及下游任务的误差预算必须写成"上游误差 × 几何敏感度"，否则会得到"距离基站越近跟踪越差"这种看似矛盾的结果。