Millimeter Wave ISAC-SLAM: Framework and RFSoC Prototype

文章目录

- 摘要
- [I. 引言](#I. 引言)
- [II. 系统模型](#II. 系统模型)
- - [A. 天线阵列与信号模型](#A. 天线阵列与信号模型)
  - [B. 时分 ISAC 协议](#B. 时分 ISAC 协议)
  - - [1. 面向 OFDM 通信的波束管理](#1. 面向 OFDM 通信的波束管理)
    - [2. 面向 OFDM 感知的单基地雷达](#2. 面向 OFDM 感知的单基地雷达)
  - [C. 定位模型](#C. 定位模型)
  - [D. 基于 RCS 的栅格地图模型](#D. 基于 RCS 的栅格地图模型)
- [III. 毫米波 ISAC-SLAM 框架](#III. 毫米波 ISAC-SLAM 框架)
- - [A. OFDM 通信期间的定位与建图](#A. OFDM 通信期间的定位与建图)
  - - [1. LOS 与一阶 NLOS 提取](#1. LOS 与一阶 NLOS 提取)
    - [2. 基于 EKF 的定位细化](#2. 基于 EKF 的定位细化)
    - [3. 使用一阶 NLOS 的粗建图（Rough Mapping With 1st NLOS）](#3. 使用一阶 NLOS 的粗建图（Rough Mapping With 1st NLOS）)
  - [B. OFDM 感知期间的精细建图](#B. OFDM 感知期间的精细建图)
  - [C. 全局地图拼接](#C. 全局地图拼接)
- [IV. 原型系统设计](#IV. 原型系统设计)
- [V. 实验结果](#V. 实验结果)
- - [A. 实验设置](#A. 实验设置)
  - - [1. 场景描述](#1. 场景描述)
    - [2. 原型设置](#2. 原型设置)
    - [3. 系统参数](#3. 系统参数)
  - [B. OFDM 通信期间的定位与建图](#B. OFDM 通信期间的定位与建图)
  - [C. OFDM 感知期间的精细建图](#C. OFDM 感知期间的精细建图)
  - [D. 全局地图拼接](#D. 全局地图拼接)
- [VI. 结论](#VI. 结论)
- 参考文献
- 作者简介

摘要

本文建立了一个基于 Xilinx RFSoC 的毫米波（mmWave）感知与通信一体化同步定位与建图（integrated sensing and communication simultaneous localization and mapping, ISAC-SLAM）完整框架，并给出了真实世界原型。该 ISAC-SLAM 框架采用时分协议，使用户在通信和感知之间交替工作。

在通信阶段，用户与基站（base station, BS）执行波束管理，从中分别提取视距（line-of-sight, LOS）路径和一阶非视距（first-order non-line-of-sight, 1st NLOS）路径，用于粗略用户定位和地图重建。进一步地，本文设计了一种融合用户粗略位置、惯性测量单元（inertial measurement unit, IMU）和轮式编码器测量的算法，以实现精确定位。
在感知阶段，本文提出精细的信号处理基带，使用户能够作为单基地正交频分复用（orthogonal frequency division multiplexing, OFDM）雷达工作并细化地图。
最后，本文设计了地图拼接方法（a map stitching method），使多个用户和基站能够协作生成大尺度地图。
不同于多数传统工作，本文提出基于雷达散射截面（radar cross section, RCS）的栅格地图（RCS-based grid map），用来同时表示环境轮廓和材料信息（contour and material of the environment），并在 RFSoC 上实现了具有高鲁棒性和实时实用性的 ISAC-SLAM 原型。实验表明，ISAC-SLAM 的定位平均绝对误差为 0.0347 m，建图平均绝对误差为 0.2610 m，同时保持超过 1 Gbps 的实时数据传输。此外，ISAC-SLAM 地图在大尺度上呈现出与 2D LiDAR 地图相似的轮廓，并包含环境材料信息。

索引词：ISAC；通信 SLAM；RFSoC 原型；毫米波通信。

I. 引言

感知与通信一体化（ISAC）被认为是第六代（6G）通信系统的关键特征之一 [1]。ISAC 的本质是在同一套波形、射频（radio-frequency, RF）系统和硬件结构上同时实现通信与感知功能。因此，ISAC 技术能够显著降低硬件开销、能耗和部署复杂度，从而使通信网络中的感知应用成为可能，例如用户设备（user equipment, UE）定位和环境重建 [2]。UE 定位和环境重建合在一起就是同步定位与建图（SLAM），它是机器人智能与自动化的重要支柱 [3]-[5]。凭借 ISAC 的高分辨率感知能力，使用 ISAC 技术的 SLAM（ISAC-SLAM）可以把通信设备直接作为 SLAM 传感器，而无需额外使用昂贵的 LiDAR 或摄像头 [6]-[25]。虽然基于 LiDAR 和摄像头的 SLAM 能够生成精细的 2D/3D 地图，并准确估计位置和姿态 [26]-[31]，但 ISAC-SLAM 具有独特优势。

ISAC-SLAM 的一个内在特点是多个基站与多个用户之间可以交互，借此能够实现大尺度全局环境重建。同时，利用无线电波的 SLAM 在多种天气和光照条件下能保持相对稳定的性能，例如雨、雪、雾等条件，而在这些条件下 LiDAR 和摄像头 SLAM 的性能会显著下降。所提出 ISAC-SLAM 的潜在应用非常广泛，包括提升复杂环境中手机用户的导航精度，以及在多种天气条件下、或没有摄像头/LiDAR 的情况下，利用通信设备完成城市尺度的多车辆、多行人协作建图。

根据基础框架，现有 ISAC-SLAM 研究可以分为两类：雷达驱动方法和通信驱动方法。

雷达驱动方法中，通信收发机通过时分复用等方式被同时用作单基地雷达，ISAC-SLAM 通过通信信号的自发自收来实现 [6]-[13]。在毫米波频段，[7]、[8] 使用压缩感知方法构建距离-角度（range-angle, RA）图，并采用扩展卡尔曼滤波（extended Kalman filter, EKF）进行环境重建；[10]-[13] 引入考虑每个栅格 RCS 值的状态空间模型，提出基于栅格的贝叶斯建图方法。[9] 将 5G NR 标准中的多个频率信道拼接起来，用 GHz 级带宽 OFDM 信号实现成像与建图。在太赫兹（THz）频段，[6] 提出了一种基于 Fourier-Mellin 变换的新型 SLAM 算法。实验表明，雷达驱动方法的建图精度基本与带宽所限制的距离分辨率处于同一数量级。不过，尽管雷达驱动方法精度很高，它们通常不考虑基站与用户之间的通信和交互。因此，多基站、多用户协作 ISAC-SLAM 的优势无法充分发挥，建图最大范围也受限于单个用户的探测范围。
另一方面，通信驱动方法利用基站与用户之间通信信道中的多径分量，其思想类似于使用通信信号的双基地雷达。在毫米波和太赫兹等高频频段，多径分量大多由镜面反射（specular reflection）生成，因此可利用多径分量的几何特征估计环境地图、基站位置和用户位置 [14]-[25]、[32]、[33]。
- 在 [14] 和 [15] 中，作者设计链路状态分类神经网络，将路径分为 LOS、一阶 NLOS 和高阶 NLOS，再由一阶 NLOS 路径推导反射点作为环境地图。虽然这些工作简洁地利用了镜面反射的几何性质进行导航和建图，但神经网络泛化能力较弱，实际落地困难。为了提高实用性，多数通信驱动方法把环境表示为一组简单地标，包括基站和关于反射平面对称的虚拟基站，并使用到达角（AoA）、离开角（AoD）、到达时间（ToA）和接收信号强度（RSS）等多径特征估计基站、虚拟基站和用户位置。
- 在 [16]、[17]、[20]-[23] 中，AoA、AoD、ToA 与 RSS 被提取用于 ISAC-SLAM。
- 然而，这些方法受限于精确 ToA 所需的严格时间同步，以及 RSS 测量的低鲁棒性。为避免精确 ToA 和 RSS 获取困难，[18]、[19]、[24]、[25] 只利用角度特征完成 SLAM，并进行了真实实现。
- 通信驱动方法通常不改变 5G NR 协议中的通信流程，并充分利用多个基站和用户之间的协作，因此一般能够在较大区域内重建地图。不过，由于高频频段多径稀疏，且仅依靠少量地标的地图模型过于简单，通信驱动方法在真实世界中的定位精度和地图分辨率并不令人满意。

综上，雷达驱动 ISAC-SLAM 能提供较高定位精度和地图分辨率，但无法兼顾用户通信需求，也不能利用通信系统中多用户、多基站协作的优势。通信驱动 ISAC-SLAM 不额外占用通信资源，却受限于多径稀疏性和地标地图模型能力，表现较差。特别是在已有硬件实验的工作中，硬件平台大多采用 USRP 或频谱分析仪方案。USRP 的带宽限制和较高信号处理时延阻碍了宽带信号 ISAC-SLAM 演示；频谱分析仪天然缺少实时信号处理基带，无法满足 ISAC-SLAM 的实时要求，也无法验证 ISAC-SLAM 算法在真实世界中的实用性。

注：

本文这里明确对比的是 RFSoC 原型与 USRP、频谱分析仪等既有实验平台：USRP 主要受带宽和处理时延限制，频谱分析仪缺少实时信号处理基带；RFSoC 的优势是可在 FPGA/SoC 上实时实现 OFDM 通信与感知基带，并共享同一毫米波 RF 链路。

因此，论文强调的是 RFSoC 作为实时 ISAC-SLAM 原型平台的优势，并非对 RFSoC 与商用毫米波芯片/常规毫米波设备做完整横向比较。

本文提出一种新的毫米波 ISAC-SLAM 框架和真实世界 RFSoC 原型。不同于既有 ISAC-SLAM 框架 [32]、[33]，本文通过时分 ISAC 协议（leveraging time-division ISAC protocol）融合通信驱动和雷达驱动 ISAC-SLAM，使系统在保持通信的同时，在大尺度区域内实现精确定位和高分辨率地图重建。本文主要贡献如下。

建立毫米波 ISAC-SLAM 框架。该框架采用时分 ISAC 协议，使用户在 OFDM 通信和感知之间交替工作。
- 通信阶段，用户基于波束管理与基站通信。本文提出算法，通过分析每个波束的功率提取 LOS 和一阶 NLOS 路径。利用 LOS 路径实现粗略用户定位，并设计 EKF 融合 IMU 和轮式编码器测量以提升精度。本文提出 RCS 栅格地图，用于同时表示环境轮廓和材料信息，并利用一阶 NLOS 路径重建粗地图。
- 感知阶段，用户作为单基地 OFDM 雷达工作。本文提出带滤波和功率补偿的信号处理基带，获得细化 RA 图（refined RA charts），避免噪声和衰减造成的失真，并基于 RA 图设计地图重建算法细化粗地图。最后，本文设计拼接算法，融合所有用户的细化地图，构建大尺度 RCS 栅格地图。
构建紧凑型 ISAC-SLAM RFSoC 原型。该原型包括 Xilinx RFSoC 和计算机。在 Xilinx RFSoC 中，本文把时分 OFDM 通信与感知信号处理基带实现到同一个 FPGA 处理器中，并共享同一毫米波 RF 链路。在计算机中，本文基于机器人操作系统（Robot Operating System，ROS）[34] 部署并行多传感器数据采集和 SLAM 框架（a parallel multi-sensor data-acquisition and SLAM framework），其中来自 RFSoC 的 OFDM ISAC 基带结果与 IMU、轮式编码器测量融合，实现实时、低时延 ISAC-SLAM。
在多个复杂室内场景中对 ISAC-SLAM 原型进行全面评估与演示。实验结果表明，所提出原型可达到与 2D LiDAR Cartographer SLAM [27] 相近的地图重建性能。定位和建图平均绝对误差分别为 0.0347 m 和 0.2610 m，接近带宽所限制的最细建图分辨率。此外，构建的 RCS 栅格地图（the constructed RCS-based grid map）能够清晰区分环境中的金属和其他材料。在 ISAC-SLAM 过程中，用户稳健地维持超过 1 Gbps 的实时数据传输，验证了 ISAC-SLAM 在通信与感知两方面的有效性。

图 1. 本文考虑的 OFDM 毫米波 ISAC 场景，以及所提出 ISAC-SLAM 框架的总体模型。每个 UE 均可执行 OFDM 通信和感知，从而实现 ISAC-SLAM。

本文结构如下：第二节给出 OFDM 信号、时分 ISAC 协议、定位和 RCS 栅格地图模型；第三节描述所提出的毫米波 ISAC-SLAM 框架；第四节详细介绍原型系统实现；第五节分析实验结果；第六节总结全文。

记号约定： x x x 表示标量， x \mathbf{x} x 表示向量， X \mathbf{X} X 表示矩阵， X \mathcal{X} X 表示集合； [ X ; Y ] [\mathbf{X};\mathbf{Y}] [X;Y] 和 [ X , Y ] [\mathbf{X},\mathbf{Y}] [X,Y] 分别表示矩阵按行维度和按列维度拼接。

II. 系统模型

如图 1 所示，本文考虑具有多个静止基站和移动用户的 OFDM 毫米波 ISAC 场景。每个用户在其探测范围内构建环境局部地图，中央基站通过融合所有用户上传的局部地图形成全局地图。具体而言，用户可以通过波束管理进行通信，也可以通过自发自收进行感知。同时，每个用户始终具备与至少两个基站建立下行 LOS 连接的能力。由于毫米波通信高度依赖 LOS 路径，已有许多研究提出每个用户应保持与多个基站的 LOS 连接，以提高信道容量并避免 LOS 路径遮挡导致的中断 [35]-[37]。考虑真实使用场景，每个用户还配备 IMU 和轮式编码器，用于辅助精确定位。为简化 ISAC-SLAM 系统，本文只考虑 2D ISAC-SLAM 场景，因此在水平面上建立天线阵列和信号模型。

图 2. 基站和 UE 处天线阵列的俯视理论布局。由于一个 ULA 天线只能覆盖 120° 范围，采用三扇区部署以实现 360° 覆盖。

A. 天线阵列与信号模型

水平面的全向覆盖对基站和 UE 都至关重要。在 5G 系统中，这通常通过组合多个天线阵列实现，每个天线阵列覆盖特定角度范围 [38]。本文采用图 2 所示的俯视布局实现全向覆盖。在基站和 UE 处均采用三组 32 × 1 32 \times 1 32×1 毫米波均匀线阵（uniform linear array, ULA），并使用全模拟波束成形，每组 ULA 提供 ± 60 ∘ \pm 60^\circ ±60∘ 覆盖。所提布局是实际 5G 系统中的标准扇区化方法，也称三扇区布局 [39]、[40]，每个 ± 60 ∘ \pm 60^\circ ±60∘ 覆盖区域称为一个扇区。

在每个扇区中，基站和 UE 都设置一条 RF 链路，该链路使用全模拟波束成形 ULA 天线收发 OFDM 信号。令接收和发射 ULA 的天线阵元数分别为 N a n t r x N_{\mathrm{ant}}^{\mathrm{rx}} Nantrx 和 N a n t t x N_{\mathrm{ant}}^{\mathrm{tx}} Nanttx，本文中 N a n t r x = N a n t t x = 32 N_{\mathrm{ant}}^{\mathrm{rx}}=N_{\mathrm{ant}}^{\mathrm{tx}}=32 Nantrx=Nanttx=32。通信信号模型可表示为

r k = f r H H k f t s k + n k , (1) r_k=\mathbf{f}_r^{H}\mathbf{H}_k\mathbf{f}_t s_k+n_k , \tag{1} rk=frHHkftsk+nk,(1)

其中 r k , s k ∈ C r_k,s_k\in\mathbb{C} rk,sk∈C， H k ∈ C N a n t r x × N a n t t x \mathbf{H}k\in\mathbb{C}^{N{\mathrm{ant}}^{\mathrm{rx}}\times N_{\mathrm{ant}}^{\mathrm{tx}}} Hk∈CNantrx×Nanttx， n k ∼ C N ( 0 , ν 2 ) n_k\sim \mathcal{CN}(0,\nu^2) nk∼CN(0,ν2)，分别为第 k k k 个 OFDM 子载波上的接收复信号、发射复信号、信道矩阵和高斯噪声。此外， f r ∈ C N a n t r x × 1 \mathbf{f}r\in\mathbb{C}^{N{\mathrm{ant}}^{\mathrm{rx}}\times 1} fr∈CNantrx×1 与 f t ∈ C N a n t t x × 1 \mathbf{f}t\in\mathbb{C}^{N{\mathrm{ant}}^{\mathrm{tx}}\times 1} ft∈CNanttx×1 为接收与发射波束成形向量。 f r \mathbf{f}r fr 和 f t \mathbf{f}t ft 分别从接收码本 R X U L A \mathrm{RX}{\mathrm{ULA}} RXULA 和发射码本 T X U L A \mathrm{TX}{\mathrm{ULA}} TXULA 中选取。本文所有 ULA 天线均使用 DFT 码本。若接收和发射码本大小分别为 N d i r r x N_{\mathrm{dir}}^{\mathrm{rx}} Ndirrx 与 N d i r t x N_{\mathrm{dir}}^{\mathrm{tx}} Ndirtx，本文均取 31，按 Δ Ψ = 4 ∘ \Delta\Psi=4^\circ ΔΨ=4∘ 的间隔均匀覆盖 ± 60 ∘ \pm 60^\circ ±60∘。

对 T X U L A \mathrm{TX}{\mathrm{ULA}} TXULA 中第 i t x i{\mathrm{tx}} itx 个波束，发射波束成形向量为

f t ( θ i t x ) = [ 1 , e − j π sin ⁡ θ i t x , e − 2 j π sin ⁡ θ i t x , ... , e − ( N a n t t x − 1 ) j π sin ⁡ θ i t x ] T , θ i t x = ( i t x − ⌊ N d i r t x 2 ⌋ ) Δ Ψ , i t x ∈ { 0 , 1 , 2 , ... , N d i r t x − 1 } . (2) \begin{aligned} \mathbf{f}t(\theta{i_{\mathrm{tx}}}) &=\left[1,e^{-j\pi\sin\theta_{i_{\mathrm{tx}}}},e^{-2j\pi\sin\theta_{i_{\mathrm{tx}}}},\ldots, e^{-(N_{\mathrm{ant}}^{\mathrm{tx}}-1)j\pi\sin\theta_{i_{\mathrm{tx}}}}\right]^T,\\ \theta_{i_{\mathrm{tx}}} &=\left(i_{\mathrm{tx}}-\left\lfloor\frac{N_{\mathrm{dir}}^{\mathrm{tx}}}{2}\right\rfloor\right)\Delta\Psi,\quad i_{\mathrm{tx}}\in\{0,1,2,\ldots,N_{\mathrm{dir}}^{\mathrm{tx}}-1\}. \end{aligned} \tag{2} ft(θitx)θitx=[1,e−jπsinθitx,e−2jπsinθitx,...,e−(Nanttx−1)jπsinθitx]T,=(itx−⌊2Ndirtx⌋)ΔΨ,itx∈{0,1,2,...,Ndirtx−1}.(2)

这里 i t x i_{\mathrm{tx}} itx 也称为 TX 码本索引，每个 TX 码本索引对应一个 TX 波束方向。类似地，对 R X U L A \mathrm{RX}{\mathrm{ULA}} RXULA 中第 i r x i{\mathrm{rx}} irx 个波束，接收波束成形向量为

f r ( θ i r x ) = [ 1 , e − j π sin ⁡ θ i r x , e − 2 j π sin ⁡ θ i r x , ... , e − ( N a n t r x − 1 ) j π sin ⁡ θ i r x ] T , θ i r x = ( i r x − ⌊ N d i r r x 2 ⌋ ) Δ Ψ , i r x ∈ { 0 , 1 , 2 , ... , N d i r r x − 1 } . (3) \begin{aligned} \mathbf{f}r(\theta{i_{\mathrm{rx}}}) &=\left[1,e^{-j\pi\sin\theta_{i_{\mathrm{rx}}}},e^{-2j\pi\sin\theta_{i_{\mathrm{rx}}}},\ldots, e^{-(N_{\mathrm{ant}}^{\mathrm{rx}}-1)j\pi\sin\theta_{i_{\mathrm{rx}}}}\right]^T,\\ \theta_{i_{\mathrm{rx}}} &=\left(i_{\mathrm{rx}}-\left\lfloor\frac{N_{\mathrm{dir}}^{\mathrm{rx}}}{2}\right\rfloor\right)\Delta\Psi,\quad i_{\mathrm{rx}}\in\{0,1,2,\ldots,N_{\mathrm{dir}}^{\mathrm{rx}}-1\}. \end{aligned} \tag{3} fr(θirx)θirx=[1,e−jπsinθirx,e−2jπsinθirx,...,e−(Nantrx−1)jπsinθirx]T,=(irx−⌊2Ndirrx⌋)ΔΨ,irx∈{0,1,2,...,Ndirrx−1}.(3)

i r x i_{\mathrm{rx}} irx 称为 RX 码本索引，每个 RX 码本索引对应一个 RX 波束方向。本文 ULA 天线半功率波束宽度为 3 ∘ 3^\circ 3∘。

毫米波通信信道 H k \mathbf{H}_k Hk 可由几何信道模型 [41] 描述为

H k = ∑ n = 0 N − 1 ∑ l = 1 L α l e − j 2 π k K n d ( n T s − τ l ) a r ( ϕ l r x ) a t H ( ϕ l t x ) , (4) \mathbf{H}k= \sum{n=0}^{N-1}\sum_{l=1}^{L} \alpha_l e^{-j\frac{2\pi k}{K}n} d(nT_s-\tau_l)\mathbf{a}_r(\phi_l^{\mathrm{rx}}) \mathbf{a}_t^{H}(\phi_l^{\mathrm{tx}}), \tag{4} Hk=n=0∑N−1l=1∑Lαle−jK2πknd(nTs−τl)ar(ϕlrx)atH(ϕltx),(4)

其中 L L L 表示多径数量， K K K 为子载波数量， N N N 为循环前缀长度， T s T_s Ts 为采样间隔； α l \alpha_l αl、 τ l \tau_l τl、 ϕ l r x \phi_l^{\mathrm{rx}} ϕlrx 与 ϕ l t x \phi_l^{\mathrm{tx}} ϕltx 分别表示第 l l l 条路径的增益、ToA、AoA 与 AoD； a r ( ⋅ ) ∈ C N a n t r x × 1 \mathbf{a}r(\cdot)\in\mathbb{C}^{N{\mathrm{ant}}^{\mathrm{rx}}\times 1} ar(⋅)∈CNantrx×1 和 a t ( ⋅ ) ∈ C N a n t t x × 1 \mathbf{a}t(\cdot)\in\mathbb{C}^{N{\mathrm{ant}}^{\mathrm{tx}}\times 1} at(⋅)∈CNanttx×1 分别是接收和发射 ULA 的波束导向向量； d ( ⋅ ) d(\cdot) d(⋅) 表示脉冲成形滤波器。注意，与环境相关的全部信息都包含在信道矩阵中。

B. 时分 ISAC 协议

图 3. ISAC-SLAM 框架采用的时分 ISAC 协议。每帧被分成两个阶段：OFDM 通信和 OFDM 感知。

所提出的时分 ISAC 协议如图 3 所示，每一帧分成通信阶段和感知阶段。通信阶段中，用户与基站协作执行波束管理，然后传输业务数据；感知阶段中，用户作为单基地雷达工作，即用户在所有方向上发射并接收 OFDM 信号，以生成 RA 图。

1. 面向 OFDM 通信的波束管理

对于使用窄波束的毫米波通信，需要精细波束管理。为避免干扰数据传输，本文考虑 5G NR 协议 [42] 中规定的下行波束管理流程。波束管理的目标是建立并维护具有最大增益的一组最优波束对。波束管理分为两种模式，即初始接入和波束跟踪。当尚未形成最优波束对集合时，系统激活初始接入。基站和用户协作遍历发射与接收码本中的全部波束，用户对每个波束对执行参考信号接收功率（reference signals received power, RSRP）测量。初始接入时，若以 f t ( θ i t x ) \mathbf{f}t(\theta{i_{\mathrm{tx}}}) ft(θitx) 和 f r ( θ i r x ) \mathbf{f}r(\theta{i_{\mathrm{rx}}}) fr(θirx) 表示待测的发射与接收波束成形向量，则波束对 ( θ i t x , θ i r x ) (\theta_{i_{\mathrm{tx}}},\theta_{i_{\mathrm{rx}}}) (θitx,θirx) 的 RSRP 可写为

p ( θ i t x , θ i r x ) = 1 N c ∑ k = 0 N c ∣ r k ( i t x , i r x ) ∣ 2 , (5) p(\theta_{i_{\mathrm{tx}}},\theta_{i_{\mathrm{rx}}}) =\frac{1}{N_c}\sum_{k=0}^{N_c}\left|r_k^{(i_{\mathrm{tx}},i_{\mathrm{rx}})}\right|^2, \tag{5} p(θitx,θirx)=Nc1k=0∑Nc rk(itx,irx) 2,(5)

r k ( i t x , i r x ) = f r H ( θ i r x ) H k f t ( θ i t x ) s k + n k , (6) r_k^{(i_{\mathrm{tx}},i_{\mathrm{rx}})} =\mathbf{f}r^{H}(\theta{i_{\mathrm{rx}}}) \mathbf{H}k \mathbf{f}t(\theta{i{\mathrm{tx}}})s_k+n_k, \tag{6} rk(itx,irx)=frH(θirx)Hkft(θitx)sk+nk,(6)

其中 N c N_c Nc 表示参考信号中的子载波数量。遍历接收和发射码本即可得到所有 RSRP 测量构成的矩阵

P = [ p ( θ i t x , θ i r x ) ] ( i t x , i r x ) ∈ R N d i r t x × N d i r r x . (7) \mathbf{P}=[p(\theta_{i_{\mathrm{tx}}},\theta_{i_{\mathrm{rx}}})]{(i{\mathrm{tx}},i_{\mathrm{rx}})} \in\mathbb{R}^{N_{\mathrm{dir}}^{\mathrm{tx}}\times N_{\mathrm{dir}}^{\mathrm{rx}}}. \tag{7} P=[p(θitx,θirx)](itx,irx)∈RNdirtx×Ndirrx.(7)

初始接入后，系统从 P \mathbf{P} P 中提取 RSRP 最高的前 K K K 个波束对，建立一组最优波束对。为了维护该集合，系统激活波束跟踪。基站和用户探测邻近波束，并根据探测结果微调最优集合，从而实现波束跟踪和细化。简言之，系统通过波束管理获得并维护最优波束对集合。若记第 i i i 个最优波束对为 b i b_i bi，则 b i b_i bi 以及时刻 t t t 的集合可表示为

b i = ( θ ^ i t x , θ ^ i r x , P ^ θ ^ i t x , θ ^ i r x ) , (8) b_i=\left(\hat{\theta}_i^{\mathrm{tx}},\hat{\theta}i^{\mathrm{rx}}, \hat{P}{\hat{\theta}_i^{\mathrm{tx}},\hat{\theta}_i^{\mathrm{rx}}}\right), \tag{8} bi=(θ^itx,θ^irx,P^θ^itx,θ^irx),(8)

B ( t ) = { b 1 , b 2 , ... , b K ( t ) } . (9) \mathcal{B}(t)=\{b_1,b_2,\ldots,b_{K(t)}\}. \tag{9} B(t)={b1,b2,...,bK(t)}.(9)

其中 θ ^ i t x \hat{\theta}_i^{\mathrm{tx}} θ^itx 和 θ ^ i r x \hat{\theta}i^{\mathrm{rx}} θ^irx 是集合中产生第 i i i 高 RSRP 的发射和接收波束方向， P ^ θ ^ i t x , θ ^ i r x \hat{P}{\hat{\theta}_i^{\mathrm{tx}},\hat{\theta}_i^{\mathrm{rx}}} P^θ^itx,θ^irx 为对应 RSRP， K ( t ) K(t) K(t) 是时刻 t t t 集合中的波束对数量。由于假设每个用户始终具备与至少两个基站建立 LOS 连接的能力，LOS 路径始终包含在 B ( t ) \mathcal{B}(t) B(t) 中。此外， B ( t ) \mathcal{B}(t) B(t) 中的一阶 NLOS 路径可以用于推导用户与基站之间的反射平面，进而重建环境粗地图。

2. 面向 OFDM 感知的单基地雷达

在 OFDM 感知阶段，用户侧的发射和接收毫米波 ULA 天线可以协作形成单基地 OFDM 雷达收发机。如果把 OFDM 单基地感知过程的单位冲激响应与式 (4) 中的 OFDM 通信信道类比，则对第 l l l 个回波，AoA ϕ l r \phi_l^r ϕlr 与 AoD ϕ l t \phi_l^t ϕlt 表示第 l l l 个散射体的角度位置。需要注意，在单基地模式下二者相同，即 ϕ l r = ϕ l t = ϕ l \phi_l^r=\phi_l^t=\phi_l ϕlr=ϕlt=ϕl。同时，第 l l l 个回波的时延可表示为 τ l = 2 R l / c \tau_l=2R_l/c τl=2Rl/c，其中 R l R_l Rl 是收发机与第 l l l 个散射体之间的距离。此外，信道矩阵中的 α l \alpha_l αl 表示路径增益，与传播衰减和第 l l l 个散射体的 RCS 紧密相关。由于某个散射体的 RCS 值会随入射波束模式变化且难以描述，本文用 σ l \sigma_l σl 表示第 l l l 个散射体的最大 RCS。OFDM 感知的目标是以 RA 图的形式精细估计所有 ( ϕ l , R l , σ l ) (\phi_l,R_l,\sigma_l) (ϕl,Rl,σl)。OFDM 感知可通过遍历码本中的全部波束完成。由于本文采用天线数量相同的全模拟 ULA 天线，在探测第 i i i 个波束方向时，发射和接收 ULA 使用相同的波束成形向量 f i \mathbf{f}_i fi 收发信号。

C. 定位模型

图 4. 本文中的定位模型和基于 RCS 的栅格地图模型。

定位和建图模型如图 4 所示。用户 u u u 始终保持与基站 #A 和 #B 的 LOS 连接。基站 #A 和 #B 的位置及法向方向作为先验已知，分别记为 ( x A , y A , ψ A ) (x_A,y_A,\psi_A) (xA,yA,ψA) 和 ( x B , y B , ψ B ) (x_B,y_B,\psi_B) (xB,yB,ψB)。若用户 u u u 与两个基站之间两条 LOS 路径的 AoA、AoD 与 RSRP 分别表示为 ( θ ^ A t x , θ ^ A r x , P ^ θ ^ A t x , θ ^ A r x ) (\hat{\theta}_A^{\mathrm{tx}},\hat{\theta}A^{\mathrm{rx}},\hat{P}{\hat{\theta}_A^{\mathrm{tx}},\hat{\theta}_A^{\mathrm{rx}}}) (θ^Atx,θ^Arx,P^θ^Atx,θ^Arx) 和 ( θ ^ B t x , θ ^ B r x , P ^ θ ^ B t x , θ ^ B r x ) (\hat{\theta}_B^{\mathrm{tx}},\hat{\theta}B^{\mathrm{rx}},\hat{P}{\hat{\theta}_B^{\mathrm{tx}},\hat{\theta}_B^{\mathrm{rx}}}) (θ^Btx,θ^Brx,P^θ^Btx,θ^Brx)，则用户 u u u 的位置和朝向可由几何关系计算为

ψ u = 1 2 ( θ ^ A r x − θ ^ B r x + ψ B + θ ^ B t x − ψ A − θ ^ A t x ) . (10) \psi_u=\frac{1}{2} \left( \hat{\theta}_A^{\mathrm{rx}}-\hat{\theta}_B^{\mathrm{rx}} +\psi_B+\hat{\theta}_B^{\mathrm{tx}} -\psi_A-\hat{\theta}_A^{\mathrm{tx}} \right). \tag{10} ψu=21(θ^Arx−θ^Brx+ψB+θ^Btx−ψA−θ^Atx).(10)

其余两个坐标由两条 LOS 射线在全局坐标系中的交点给出：

x u = x A + cos ⁡ ( ψ B + θ ^ B t x ) cos ⁡ ( ψ A + θ ^ A t x ) sin ⁡ ( θ ^ A r x + θ ^ B r x ) ∣ y A − y B ∣ + sin ⁡ ( ψ B + θ ^ B t x ) cos ⁡ ( ψ A + θ ^ A t x ) sin ⁡ ( θ ^ A r x + θ ^ B r x ) ∣ x A − x B ∣ . (11) \begin{aligned} x_u &=x_A+ \frac{ \cos(\psi_B+\hat{\theta}_B^{\mathrm{tx}}) \cos(\psi_A+\hat{\theta}_A^{\mathrm{tx}}) }{ \sin(\hat{\theta}_A^{\mathrm{rx}}+\hat{\theta}_B^{\mathrm{rx}}) }|y_A-y_B|\\ &\quad+ \frac{ \sin(\psi_B+\hat{\theta}_B^{\mathrm{tx}}) \cos(\psi_A+\hat{\theta}_A^{\mathrm{tx}}) }{ \sin(\hat{\theta}_A^{\mathrm{rx}}+\hat{\theta}_B^{\mathrm{rx}}) }|x_A-x_B|. \end{aligned} \tag{11} xu=xA+sin(θ^Arx+θ^Brx)cos(ψB+θ^Btx)cos(ψA+θ^Atx)∣yA−yB∣+sin(θ^Arx+θ^Brx)sin(ψB+θ^Btx)cos(ψA+θ^Atx)∣xA−xB∣.(11)

y u = y A − cos ⁡ ( ψ B + θ ^ B t x ) sin ⁡ ( ψ A + θ ^ A t x ) sin ⁡ ( θ ^ A r x + θ ^ B r x ) ∣ y A − y B ∣ + sin ⁡ ( ψ B + θ ^ B t x ) sin ⁡ ( ψ A + θ ^ A t x ) sin ⁡ ( θ ^ A r x + θ ^ B r x ) ∣ x A − x B ∣ . (12) \begin{aligned} y_u &=y_A- \frac{ \cos(\psi_B+\hat{\theta}_B^{\mathrm{tx}}) \sin(\psi_A+\hat{\theta}_A^{\mathrm{tx}}) }{ \sin(\hat{\theta}_A^{\mathrm{rx}}+\hat{\theta}_B^{\mathrm{rx}}) }|y_A-y_B|\\ &\quad+ \frac{ \sin(\psi_B+\hat{\theta}_B^{\mathrm{tx}}) \sin(\psi_A+\hat{\theta}_A^{\mathrm{tx}}) }{ \sin(\hat{\theta}_A^{\mathrm{rx}}+\hat{\theta}_B^{\mathrm{rx}}) }|x_A-x_B|. \end{aligned} \tag{12} yu=yA−sin(θ^Arx+θ^Brx)cos(ψB+θ^Btx)sin(ψA+θ^Atx)∣yA−yB∣+sin(θ^Arx+θ^Brx)sin(ψB+θ^Btx)sin(ψA+θ^Atx)∣xA−xB∣.(12)

两组角度还应满足几何约束

( ψ A + θ ^ A t x ) + ( ψ B + θ ^ B t x ) − ( θ ^ A r x + θ ^ B r x ) = 0. (13) \left(\psi_A+\hat{\theta}_A^{\mathrm{tx}}\right) +\left(\psi_B+\hat{\theta}_B^{\mathrm{tx}}\right) -\left(\hat{\theta}_A^{\mathrm{rx}}+\hat{\theta}_B^{\mathrm{rx}}\right)=0. \tag{13} (ψA+θ^Atx)+(ψB+θ^Btx)−(θ^Arx+θ^Brx)=0.(13)

借助式 (13)，可以从波束管理测量 B A ( t ) \mathcal{B}_A(t) BA(t) 和 B B ( t ) \mathcal{B}_B(t) BB(t) 中识别并提取两条对应的 LOS 路径，这两个集合分别表示时刻 t t t 下 u u u-#A 和 u u u-#B 连接的最优波束集合。运动过程中，由两条 LOS 路径估计出的用户绝对位置和朝向可与 IMU、轮式编码器测量融合，实现更精确定位。由于式 (13) 的约束较严格，考虑测量噪声，本文将其放宽为

∣ ( ψ A + θ ^ A t x ) + ( ψ B + θ ^ B t x ) − ( θ ^ A r x + θ ^ B r x ) ∣ ≤ η , (14) \left| \left(\psi_A+\hat{\theta}_A^{\mathrm{tx}}\right) +\left(\psi_B+\hat{\theta}_B^{\mathrm{tx}}\right) -\left(\hat{\theta}_A^{\mathrm{rx}}+\hat{\theta}_B^{\mathrm{rx}}\right) \right|\leq \eta, \tag{14} (ψA+θ^Atx)+(ψB+θ^Btx)−(θ^Arx+θ^Brx) ≤η,(14)

其中 η \eta η 为放宽裕量，在原型中通过试错设置为 6 ∘ 6^\circ 6∘。

D. 基于 RCS 的栅格地图模型

本文采用 [12] 提出的栅格地图模型。2D 环境被划分为栅格，每个栅格被视为一个散射体，栅格值表示其反射强度，即 RCS 值。栅格地图可表示为由浮点实数组成的矩阵

M = [ σ i j ] i j , (15) \mathbf{M}=[\sigma_{ij}]_{ij}, \tag{15} M=[σij]ij,(15)

其中 σ i j \sigma_{ij} σij 为第 ( i , j ) (i,j) (i,j) 个栅格的 RCS 值，定义为该栅格的最大 RCS 值。第 ( i , j ) (i,j) (i,j) 个栅格的绝对位置为 ( x o + i d x , y o + j d y ) (x_o+i d_x,y_o+j d_y) (xo+idx,yo+jdy)，其中 ( x o , y o ) (x_o,y_o) (xo,yo) 是第一个栅格的位置， d x d_x dx 和 d y d_y dy 分别表示 X X X 轴和 Y Y Y 轴方向的栅格尺寸。RCS 栅格地图同时展示物体轮廓以及环境对通信信号的影响，因此在材料识别和数据传输优化 [43] 中具有潜在应用。

III. 毫米波 ISAC-SLAM 框架

本文将所提出 ISAC-SLAM 的完整过程总结为算法 1。如算法 1 所示，所提出框架包含三个部分：OFDM 通信阶段的定位与建图、OFDM 感知阶段的精细建图，以及全局地图拼接。框架总体结构如图 5 所示。

text 复制代码

算法 1：ISAC-SLAM

输入：总时间步 T
输出：所有用户的位置、速度和朝向，所有用户的局部地图，以及更新后的全局地图 M^(g)
初始化：与用户通信的所有基站的位置和朝向

for 每个用户 u 及其对应基站 A、B do
  for t = 0 到 T do
    通信阶段定位与建图：
    if 激活波束管理 then
      执行波束管理，获得 B_A(t) 和 B_B(t)，提取 LOS 与一阶 NLOS。
      由两条 LOS 计算用户位置 (x_u^(t), y_u^(t), psi_u^(t))，并结合轮式编码器执行 EKF 校正。
      基于一阶 NLOS 路径获得粗地图，并与局部地图 M 融合。
    end if
    if 获得轮式编码器数据 then
      执行 EKF 校正。
    end if
    if 获得 IMU 数据 then
      执行 EKF 预测。
    end if

    感知阶段精细建图：
    if 激活 OFDM 感知 then
      通过波束扫描获得回波，解析 RA 图 Z[i,j]。
      去除 Z[i,j] 中的 TX-RX 直通、噪声、旁瓣和杂波。
      执行功率补偿以恢复 RCS。
      使用 (x_u, y_u, psi_u) 更新局部地图 M。
    end if
  end for
end for

全局地图拼接：
  将所有局部地图 M 传输到中央节点。
  中央节点将 M 拼接为全局地图 M^(g)。

图 5. 所提出毫米波 ISAC-SLAM 框架的整体结构。每个用户由 UE 智能体机器人、一组 UE Xilinx RFSoC 与 TX/RX 毫米波 ULA 天线，以及 UE 计算机构成。每个 BS 由一组 BS Xilinx RFSoC 与 TX/RX 毫米波 ULA 天线，以及 BS 计算机构成。

A. OFDM 通信期间的定位与建图

1. LOS 与一阶 NLOS 提取

在 OFDM 通信期间，用户到两个基站的两条 LOS 路径被提取用于用户自定位，一阶 NLOS 路径被用于粗地图重建。高阶 NLOS 路径需要被去除，以避免产生虚假点。真实世界中无法直接观察毫米波通信信道传播，因此本文利用信道特征差异判断某条信道路径属于 LOS、一阶 NLOS 还是高阶 NLOS。对 LOS 和一阶 NLOS 路径而言，RSRP 测量值更高，AoA 和 AoD 的角扩展小于高阶 NLOS [44]。因此，本文提出一种滤波算法，从波束管理结果 B ( t ) \mathcal{B}(t) B(t) 中选择 LOS 和一阶 NLOS 路径。

如第二节 B.1 所述，在标准 5G NR 波束管理的波束跟踪过程中，会对最优波束对集合 B ( t ) \mathcal{B}(t) B(t) 中的每个波束对 b i b_i bi 执行局部波束扫描。为了从 B ( t ) \mathcal{B}(t) B(t) 中选择 LOS 和一阶 NLOS 路径，本文记录每个 b i b_i bi 的波束跟踪结果，即 b i b_i bi 邻近全部波束对的 RSRP 测量值。假设 b i b_i bi 对应 TX 码本 T X U L A \mathrm{TX}{\mathrm{ULA}} TXULA 中第 i t x i{\mathrm{tx}} itx 个波束，以及 RX 码本 R X U L A \mathrm{RX}{\mathrm{ULA}} RXULA 中第 i r x i{\mathrm{rx}} irx 个波束。若 TX 与 RX 的局部波束扫描范围分别为 2 k i t x + 1 2k_i^{\mathrm{tx}}+1 2kitx+1 和 2 k i r x + 1 2k_i^{\mathrm{rx}}+1 2kirx+1，则对最优波束对 b i b_i bi 进行波束跟踪后生成的 RSRP 矩阵为

P b i = [ p ( θ i t x + m , θ i r x + l ) ] ( m , l ) ∈ R ( 2 k i t x + 1 ) × ( 2 k i r x + 1 ) , (16) \mathbf{P}^{b_i} =[p(\theta_{i_{\mathrm{tx}}+m},\theta_{i_{\mathrm{rx}}+l})]_{(m,l)} \in\mathbb{R}^{(2k_i^{\mathrm{tx}}+1)\times(2k_i^{\mathrm{rx}}+1)}, \tag{16} Pbi=[p(θitx+m,θirx+l)](m,l)∈R(2kitx+1)×(2kirx+1),(16)

其中 m ∈ { − k i t x , − k i t x + 1 , ... , k i t x } m\in\{-k_i^{\mathrm{tx}},-k_i^{\mathrm{tx}}+1,\ldots,k_i^{\mathrm{tx}}\} m∈{−kitx,−kitx+1,...,kitx}， l ∈ { − k i r x , − k i r x + 1 , ... , k i r x } l\in\{-k_i^{\mathrm{rx}},-k_i^{\mathrm{rx}}+1,\ldots,k_i^{\mathrm{rx}}\} l∈{−kirx,−kirx+1,...,kirx}，并满足 i t x − k i t x ≥ 0 i_{\mathrm{tx}}-k_i^{\mathrm{tx}}\geq0 itx−kitx≥0、 i t x + k i t x ≤ N d i r t x − 1 i_{\mathrm{tx}}+k_i^{\mathrm{tx}}\leq N_{\mathrm{dir}}^{\mathrm{tx}}-1 itx+kitx≤Ndirtx−1、 i r x − k i r x ≥ 0 i_{\mathrm{rx}}-k_i^{\mathrm{rx}}\geq0 irx−kirx≥0、 i r x + k i r x ≤ N d i r r x − 1 i_{\mathrm{rx}}+k_i^{\mathrm{rx}}\leq N_{\mathrm{dir}}^{\mathrm{rx}}-1 irx+kirx≤Ndirrx−1。

对 b i b_i bi，本文统计 RSRP 高于 γ t h r e P ^ b i \gamma_{\mathrm{thre}}\hat{P}{b_i} γthreP^bi 的邻近波束数量，其中 γ t h r e \gamma{\mathrm{thre}} γthre 是满足 0 < γ t h r e ≤ 1 0<\gamma_{\mathrm{thre}}\leq1 0<γthre≤1 的阈值比例， P ^ b i \hat{P}_{b_i} P^bi 是波束对 b i b_i bi 的 RSRP。将该数量记为 N i N_i Ni，统计过程为

N i = ∑ m = 0 2 k i t x + 1 ∑ l = 0 2 k i r x + 1 I ( P b i [ m , l ] > γ t h r e P ^ b i ) , (17) N_i=\sum_{m=0}^{2k_i^{\mathrm{tx}}+1}\sum_{l=0}^{2k_i^{\mathrm{rx}}+1} \mathbb{I}\left(\mathbf{P}^{b_i}[m,l]>\gamma_{\mathrm{thre}}\hat{P}_{b_i}\right), \tag{17} Ni=m=0∑2kitx+1l=0∑2kirx+1I(Pbi[m,l]>γthreP^bi),(17)

其中当 x x x 为真时 I ( x ) = 1 \mathbb{I}(x)=1 I(x)=1，否则为 0。考虑到波束对 [ m , l ] [m,l] [m,l] 可能超出波束跟踪范围，进一步计算 N i N_i Ni 的比例 ξ i \xi_i ξi：

ξ i = N i ∑ m = 0 2 k i t x + 1 ∑ l = 0 2 k i r x + 1 I ( [ m , l ] i n b e a m t r a c k i n g r a n g e ) . (18) \xi_i= \frac{N_i}{ \sum_{m=0}^{2k_i^{\mathrm{tx}}+1} \sum_{l=0}^{2k_i^{\mathrm{rx}}+1} \mathbb{I}([m,l]\ \mathrm{in\ beam\ tracking\ range})}. \tag{18} ξi=∑m=02kitx+1∑l=02kirx+1I([m,l] in beam tracking range)Ni.(18)

本质上， ξ i \xi_i ξi 表示波束对的角扩展，较大的 ξ i \xi_i ξi 意味着较大的角扩展。然后，为 ξ i \xi_i ξi 设置阈值 ξ t h r e \xi_{\mathrm{thre}} ξthre，粗略判断某个波束对是否属于高阶 NLOS 路径。如果 ξ i > ξ t h r e \xi_i>\xi_{\mathrm{thre}} ξi>ξthre，则认为 b i b_i bi 是高阶 NLOS 路径，因为大角扩展是高阶 NLOS 路径的关键特征。经过该滤波过程后，可从原始最优波束对集合 B ( t ) \mathcal{B}(t) B(t) 中提取所有低角扩展波束对，即 ξ i ≤ ξ t h r e \xi_i\leq\xi_{\mathrm{thre}} ξi≤ξthre，从而形成 LOS 与一阶 NLOS 波束对集合 B ^ ( t ) \hat{\mathcal{B}}(t) B^(t)。随后，本文将 B ^ ( t ) \hat{\mathcal{B}}(t) B^(t) 中满足式 (13) 且 ξ i \xi_i ξi 最低的波束视作 LOS 路径，其余波束归为一阶 NLOS 路径。

注：

LOS 和一阶 NLOS 通常表现为较尖锐的波束峰值，能量集中在少数 TX/RX 波束对上；高阶 NLOS 经过多次反射/散射后角扩展更大，强响应会分布到更多邻近波束。

因此， 5 × 5 5\times5 5×5 邻域过滤的作用是先剔除"散得很宽"的高阶 NLOS，避免这类路径在建图时产生虚假反射点。

式 (13) 用于在剩余低角扩展路径中检查两条 BS-UE 路径的几何一致性；满足该几何约束且角扩展最小的路径被视为 LOS，其余低角扩展路径归为一阶 NLOS。

2. 基于 EKF 的定位细化

为了提升定位精度，本文采用 EKF 算法融合 IMU、轮式编码器和两条 LOS 路径的 AoA/AoD 测量，实现用户同步定位。在 EKF 中，连续时间被离散为由 EKF 最短运行时间决定的时间步；换言之，第 k k k 个时间步对应第 k k k 轮 EKF 操作。令 ( x u ( k ) , y u ( k ) ) (x_u^{(k)},y_u^{(k)}) (xu(k),yu(k))、 ( v x ( k ) , v y ( k ) ) (v_x^{(k)},v_y^{(k)}) (vx(k),vy(k)) 和 ψ u ( k ) \psi_u^{(k)} ψu(k) 分别表示第 k k k 个时间步获得的优化用户位置、速度和朝向；令 ( a ˉ x ( k ) , a ˉ y ( k ) , β ˉ ( k ) ) (\bar{a}_x^{(k)},\bar{a}_y^{(k)},\bar{\beta}^{(k)}) (aˉx(k),aˉy(k),βˉ(k)) 表示第 k k k 时刻在用户局部坐标系下由 IMU 估计的加速度和角速度；令 ( x ˉ u ( k ) , y ˉ u ( k ) , ψ ˉ u ( k ) ) (\bar{x}_u^{(k)},\bar{y}_u^{(k)},\bar{\psi}_u^{(k)}) (xˉu(k),yˉu(k),ψˉu(k)) 表示第 k k k 时刻由式 (10)-式 (12) 计算得到的粗略位置和朝向；令 ( v ˉ x ( k ) , v ˉ y ( k ) ) (\bar{v}_x^{(k)},\bar{v}_y^{(k)}) (vˉx(k),vˉy(k)) 表示第 k k k 时刻用户坐标系下由轮式编码器估计的速度。

应用 EKF 的关键在于定义控制向量、状态向量和测量向量。用户的状态向量 x ( k ) \mathbf{x}^{(k)} x(k)、控制向量 u ( k ) \mathbf{u}^{(k)} u(k) 和测量向量 z ( k ) \mathbf{z}^{(k)} z(k) 分别为

x ( k ) = [ x u ( k ) , y u ( k ) , v x ( k ) , v y ( k ) , ψ u ( k ) ] T , (19) \mathbf{x}^{(k)}= \left[x_u^{(k)},y_u^{(k)},v_x^{(k)},v_y^{(k)},\psi_u^{(k)}\right]^T, \tag{19} x(k)=[xu(k),yu(k),vx(k),vy(k),ψu(k)]T,(19)

u ( k ) = [ a ˉ x ( k ) , a ˉ y ( k ) , β ˉ ( k ) ] T , (20) \mathbf{u}^{(k)}= \left[\bar{a}_x^{(k)},\bar{a}_y^{(k)},\bar{\beta}^{(k)}\right]^T, \tag{20} u(k)=[aˉx(k),aˉy(k),βˉ(k)]T,(20)

z ( k ) = [ x ˉ u ( k ) , y ˉ u ( k ) , v ˉ x ( k ) , v ˉ y ( k ) , ψ ˉ u ( k ) ] T . (21) \mathbf{z}^{(k)}= \left[ \bar{x}_u^{(k)},\bar{y}_u^{(k)},\bar{v}_x^{(k)},\bar{v}_y^{(k)},\bar{\psi}_u^{(k)} \right]^T. \tag{21} z(k)=[xˉu(k),yˉu(k),vˉx(k),vˉy(k),ψˉu(k)]T.(21)

定义这些向量后，EKF 即可按常规方式工作。当新的 IMU 测量可用时，执行 EKF 预测 [45] 以估计系统状态；当来自轮式编码器的新速度测量或来自两条 LOS 路径的粗略定位可用时，执行 EKF 校正 [45] 以细化系统状态。通过反复执行预测与校正，定位精度逐步提高。

注：

小车位置估计可以理解为"两步走"：毫米波通信阶段先给出绝对粗位置，IMU 和轮式编码器再通过 EKF 融合细化轨迹。

论文中的"小车"对应 UE 机器人平台；系统假设两个基站 BS #A、BS #B 的位置和朝向已知，并且 UE 始终能与这两个基站保持 LOS 直达连接。

在波束管理中，每条 LOS 路径由一组 AoD/AoA 表示：基站发射角给出从 BS 指向 UE 的射线方向，UE 接收角用于约束 UE 朝向；两条来自不同 BS 的 LOS 射线在全局平面中的交点给出 UE 的粗略二维位置。

式 (13)/(14) 用来从候选波束路径中筛选几何一致的 LOS 路径；原型中把约束放宽到约 6 ∘ 6^\circ 6∘，以容忍波束量化和测量噪声。

EKF 的状态量是 [ x u , y u , v x , v y , ψ u ] T [x_u,y_u,v_x,v_y,\psi_u]^T [xu,yu,vx,vy,ψu]T：IMU 的加速度和角速度用于预测，轮式编码器速度和 LOS 粗定位结果用于校正；这样既利用了里程计/IMU 的短时平滑性，也利用毫米波 LOS 提供的绝对位置来抑制累积漂移。

3. 使用一阶 NLOS 的粗建图（Rough Mapping With 1st NLOS）

为了利用稀疏一阶 NLOS 路径进行建图，本文假设一阶 NLOS 路径由镜面反射产生。对 AoA 与 AoD 记为 ( θ ^ n l o s t x , θ ^ n l o s r x ) (\hat{\theta}{\mathrm{nlos}}^{\mathrm{tx}},\hat{\theta}{\mathrm{nlos}}^{\mathrm{rx}}) (θ^nlostx,θ^nlosrx) 的一阶 NLOS 路径，其对应反射平面的中心和延展方向，即 ( x n l o s , y n l o s , ψ n l o s ) (x_{\mathrm{nlos}},y_{\mathrm{nlos}},\psi_{\mathrm{nlos}}) (xnlos,ynlos,ψnlos)，由以下几何关系给出：

ψ n l o s = 1 2 [ θ ^ n l o s r x + θ ^ A r x − θ ^ n l o s t x − θ ^ A t x ] − ψ u , (22) \psi_{\mathrm{nlos}} =\frac{1}{2} \left[ \hat{\theta}{\mathrm{nlos}}^{\mathrm{rx}} +\hat{\theta}{A}^{\mathrm{rx}} -\hat{\theta}{\mathrm{nlos}}^{\mathrm{tx}} -\hat{\theta}{A}^{\mathrm{tx}} \right]-\psi_u, \tag{22} ψnlos=21[θ^nlosrx+θ^Arx−θ^nlostx−θ^Atx]−ψu,(22)

x n l o s = x A + sin ⁡ ( θ ^ n l o s r x − θ ^ A r x ) cos ⁡ ( θ ^ n l o s t x − θ ^ A t x ) sin ⁡ [ ( θ ^ n l o s r x − θ ^ A r x ) − ( θ ^ n l o s t x + θ ^ A t x ) ] A u ‾ , (23) x_{\mathrm{nlos}} =x_A+ \frac{ \sin(\hat{\theta}{\mathrm{nlos}}^{\mathrm{rx}}-\hat{\theta}{A}^{\mathrm{rx}}) \cos(\hat{\theta}{\mathrm{nlos}}^{\mathrm{tx}}-\hat{\theta}{A}^{\mathrm{tx}}) }{ \sin\left[ (\hat{\theta}{\mathrm{nlos}}^{\mathrm{rx}}-\hat{\theta}{A}^{\mathrm{rx}}) - (\hat{\theta}{\mathrm{nlos}}^{\mathrm{tx}}+\hat{\theta}{A}^{\mathrm{tx}}) \right] }\overline{Au}, \tag{23} xnlos=xA+sin[(θ^nlosrx−θ^Arx)−(θ^nlostx+θ^Atx)]sin(θ^nlosrx−θ^Arx)cos(θ^nlostx−θ^Atx)Au,(23)

y n l o s = y A + sin ⁡ ( θ ^ n l o s r x − θ ^ A r x ) sin ⁡ ( θ ^ n l o s t x − θ ^ A t x ) sin ⁡ [ ( θ ^ n l o s r x − θ ^ A r x ) − ( θ ^ n l o s t x + θ ^ A t x ) ] A u ‾ , (24) y_{\mathrm{nlos}} =y_A+ \frac{ \sin(\hat{\theta}{\mathrm{nlos}}^{\mathrm{rx}}-\hat{\theta}{A}^{\mathrm{rx}}) \sin(\hat{\theta}{\mathrm{nlos}}^{\mathrm{tx}}-\hat{\theta}{A}^{\mathrm{tx}}) }{ \sin\left[ (\hat{\theta}{\mathrm{nlos}}^{\mathrm{rx}}-\hat{\theta}{A}^{\mathrm{rx}}) - (\hat{\theta}{\mathrm{nlos}}^{\mathrm{tx}}+\hat{\theta}{A}^{\mathrm{tx}}) \right] }\overline{Au}, \tag{24} ynlos=yA+sin[(θ^nlosrx−θ^Arx)−(θ^nlostx+θ^Atx)]sin(θ^nlosrx−θ^Arx)sin(θ^nlostx−θ^Atx)Au,(24)

A u ‾ = cos ⁡ ( ψ B + θ ^ B t x ) sin ⁡ ( θ ^ A r x + θ ^ B r x ) ∣ y A − y B ∣ + sin ⁡ ( ψ B + θ ^ B t x ) sin ⁡ ( θ ^ A r x + θ ^ B r x ) ∣ x A − x B ∣ . (25) \overline{Au} = \frac{ \cos(\psi_B+\hat{\theta}{B}^{\mathrm{tx}}) }{ \sin(\hat{\theta}{A}^{\mathrm{rx}}+\hat{\theta}{B}^{\mathrm{rx}}) }|y_A-y_B| + \frac{ \sin(\psi_B+\hat{\theta}{B}^{\mathrm{tx}}) }{ \sin(\hat{\theta}{A}^{\mathrm{rx}}+\hat{\theta}{B}^{\mathrm{rx}}) }|x_A-x_B|. \tag{25} Au=sin(θ^Arx+θ^Brx)cos(ψB+θ^Btx)∣yA−yB∣+sin(θ^Arx+θ^Brx)sin(ψB+θ^Btx)∣xA−xB∣.(25)

反射平面的长度取为半功率波束宽度在反射平面上的截取长度。本文将反射平面上栅格点的 RCS 值设置为对应一阶 NLOS 波束对的 RSRP，从而创建粗略 RCS 栅格地图。为了将粗地图与本地 RCS 栅格地图融合，本文用一个因子对粗地图进行缩放，使粗地图的平均 RCS 值与局部地图平均 RCS 值对齐。

注：

这里把一阶 NLOS 波束对的 RSRP 近似作为反射平面栅格的 RCS 值；但 RSRP 还受 BS-反射面-UE 传播距离、路径损耗、天线增益和入射角等因素影响，因此不是纯粹的材料反射截面。

因此通信阶段的粗建图主要提供反射面位置和轮廓的初步估计；后续 OFDM 感知阶段会利用 RA 图中的距离 bin 做功率补偿，再生成更可靠的 RCS 局部地图。

B. OFDM 感知期间的精细建图

在 OFDM 感知期间，UE 在所有方向上执行类似雷达的波束扫描，并解析回波以获得 RA 图。具体而言，UE 将 TX 和 RX ULA 天线对齐到同一方向发射和接收 OFDM 信号，并依次扫描码本中的所有波束方向。需要注意，TX 与 RX 码本必须相同。由于 OFDM 感知的信号处理与第二节 A 中的 OFDM 通信不同，本文重新构建 OFDM 感知信号模型。

对某一方向 θ i \theta_i θi，由多个目标反射而来的第 μ \mu μ 个接收 OFDM 符号可表示为

y i ( t ) = ∑ μ = 0 N s − 1 ∑ m = 0 N t g − 1 ∑ n = 0 N c − 1 A ( m ) p T X ( μ N c + n ) e j 2 π f n ( t − 2 R m c ) e j 2 π f D t . (26) y_i(t)= \sum_{\mu=0}^{N_s-1}\sum_{m=0}^{N_{\mathrm{tg}}-1}\sum_{n=0}^{N_c-1} A(m)p_{\mathrm{TX}}(\mu N_c+n) e^{j2\pi f_n(t-\frac{2R_m}{c})} e^{j2\pi f_D t}. \tag{26} yi(t)=μ=0∑Ns−1m=0∑Ntg−1n=0∑Nc−1A(m)pTX(μNc+n)ej2πfn(t−c2Rm)ej2πfDt.(26)

其中 N c N_c Nc 和 N s N_s Ns 分别为一个 OFDM 通信时隙中的子载波数和符号数； T s T_s Ts、 f n f_n fn 和 f D f_D fD 分别表示 OFDM 符号持续时间、第 n n n 个子载波频率和多普勒频移； N t g N_{\mathrm{tg}} Ntg 是目标数量， R m R_m Rm 是第 m m m 个目标的距离； p T X ( μ N c + n ) p_{\mathrm{TX}}(\mu N_c+n) pTX(μNc+n) 和 A ( m ) A(m) A(m) 分别为调制数据和传输过程中的衰减。以 1 / T s 1/T_s 1/Ts 速率采样并进行 DFT 后，接收 OFDM 信号第 μ \mu μ 个符号的频域表达式为

Y μ i [ κ ] = ∑ m = 0 N t g − 1 ∑ n = 0 N c − 1 A ( m ) p T X ( μ N c + n ) e − j 4 π n R m / ( c T s ) e j 2 π μ f D T s × e j π γ ( n , κ ) N c − 1 N c sin ⁡ ( π γ ( n , κ ) ) sin ⁡ ( π γ ( n , κ ) / N c ) , (27) \begin{aligned} Y_\mu^i[\kappa] &= \sum_{m=0}^{N_{\mathrm{tg}}-1}\sum_{n=0}^{N_c-1} A(m)p_{\mathrm{TX}}(\mu N_c+n) e^{-j4\pi nR_m/(cT_s)} e^{j2\pi\mu f_D T_s}\\ &\quad \times e^{j\pi\gamma(n,\kappa)\frac{N_c-1}{N_c}} \frac{\sin(\pi\gamma(n,\kappa))} {\sin\left(\pi\gamma(n,\kappa)/N_c\right)}, \end{aligned} \tag{27} Yμi[κ]=m=0∑Ntg−1n=0∑Nc−1A(m)pTX(μNc+n)e−j4πnRm/(cTs)ej2πμfDTs×ejπγ(n,κ)NcNc−1sin(πγ(n,κ)/Nc)sin(πγ(n,κ)),(27)

其中 γ ( n , κ ) = n − κ + f D T s \gamma(n,\kappa)=n-\kappa+f_DT_s γ(n,κ)=n−κ+fDTs。根据 [46] 的推导和 OFDM 基带参数，可对式 (27) 进行简化。

考虑移动行人或车辆通信场景，目标距离 R m R_m Rm 一般在 0 ∼ 10 3 0\sim10^3 0∼103 m 范围内，目标速度 v m v_m vm 基本在 0 ∼ 30 0\sim30 0∼30 m/s 范围内。对 OFDM 通信而言， T s T_s Ts 与基带基本频率处于同一数量级。在该原型中，OFDM 基带带宽为 B W = 820 BW=820 BW=820 MHz，子载波数为 N c = 512 N_c=512 Nc=512，RF 链路中心频率为 f c = 26 f_c=26 fc=26 GHz。因此，式 (27) 中各指数项数量级可估计为

f D = v m c f c ∼ 10 3 , T s = N c B W ∼ 10 − 6 , R m c T s ∼ 10 1 , f D T s ∼ 10 − 3 . (28) f_D=\frac{v_m}{c}f_c\sim 10^3,\quad T_s=\frac{N_c}{BW}\sim 10^{-6},\quad \frac{R_m}{cT_s}\sim 10^1,\quad f_D T_s\sim 10^{-3}. \tag{28} fD=cvmfc∼103,Ts=BWNc∼10−6,cTsRm∼101,fDTs∼10−3.(28)

可见， f D T s f_DT_s fDTs 的量级为 10 − 3 10^{-3} 10−3，而 R m / ( c T s ) R_m/(cT_s) Rm/(cTs) 的量级为 10 1 10^1 101，因此 e j 2 π μ f D T s e^{j2\pi\mu f_D T_s} ej2πμfDTs 可忽略。此外，考虑式 (27) 中类 sinc 项在 κ = n \kappa=n κ=n 时近似为 N c N_c Nc，其他位置近似为 0， Y μ i [ κ ] Y_\mu^i[\kappa] Yμi[κ] 可简化为

Y μ i [ κ ] ≈ ∑ m = 0 N t g − 1 N c A ( m ) p T X ( μ N c + κ ) e − j 4 π κ R m / ( c T s ) . (29) Y_\mu^i[\kappa]\approx \sum_{m=0}^{N_{\mathrm{tg}}-1} N_c A(m)p_{\mathrm{TX}}(\mu N_c+\kappa) e^{-j4\pi \kappa R_m/(cT_s)}. \tag{29} Yμi[κ]≈m=0∑Ntg−1NcA(m)pTX(μNc+κ)e−j4πκRm/(cTs).(29)

随后，通过逐元素除法消去调制数据，得到只包含距离和衰减项的表达式

Z μ i [ κ ] = Y μ i [ κ ] p T X ( μ N c + κ ) = ∑ m = 0 N t g − 1 N c A ( m ) e − j 4 π κ R m / ( c T s ) . (30) Z_\mu^i[\kappa] =\frac{Y_\mu^i[\kappa]}{p_{\mathrm{TX}}(\mu N_c+\kappa)} =\sum_{m=0}^{N_{\mathrm{tg}}-1} N_c A(m)e^{-j4\pi \kappa R_m/(cT_s)}. \tag{30} Zμi[κ]=pTX(μNc+κ)Yμi[κ]=m=0∑Ntg−1NcA(m)e−j4πκRm/(cTs).(30)

为提取目标距离和衰减，对 Z μ i [ κ ] Z_\mu^i[\kappa] Zμi[κ] 执行 IDFT，并在波束扫描方向 θ i \theta_i θi 上生成 RA 图值 z μ i [ l ] z_\mu^i[l] zμi[l]：

z μ i [ l ] = ∑ m = 0 N t g − 1 N c A ( m ) e j π ( l − l m ) sin ⁡ [ π ( l − l m ) ] sin ⁡ [ π ( l − l m ) / N c ] , l m = 2 N c R m c T s . (31) z_\mu^i[l]= \sum_{m=0}^{N_{\mathrm{tg}}-1} N_c A(m)e^{j\pi(l-l_m)} \frac{\sin[\pi(l-l_m)]}{\sin[\pi(l-l_m)/N_c]}, \quad l_m=\frac{2N_cR_m}{cT_s}. \tag{31} zμi[l]=m=0∑Ntg−1NcA(m)ejπ(l−lm)sin[π(l−lm)/Nc]sin[π(l−lm)],lm=cTs2NcRm.(31)

于是 RA 图可表示为

Z = [ z 0 ; z 1 ; ⋯ ; z N r − 1 ] , z i = [ z μ i [ 0 ] , z μ i [ 1 ] , ... , z μ i [ N c − 1 ] ] . (32) \mathbf{Z}=[\mathbf{z}0;\mathbf{z}1;\cdots;\mathbf{z}{N_r-1}],\quad \mathbf{z}i=[z\mu^i[0],z\mu^i[1],\ldots,z_\mu^i[N_c-1]]. \tag{32} Z=[z0;z1;⋯;zNr−1],zi=[zμi[0],zμi[1],...,zμi[Nc−1]].(32)

注意， Z \mathbf{Z} Z 是 N c × N d i r N_c\times N_{\mathrm{dir}} Nc×Ndir 复矩阵，这里的 N d i r N_{\mathrm{dir}} Ndir 表示 ULA 天线码本大小。直观地看，对式 (32)，若取 Z \mathbf{Z} Z 每个元素的绝对幅度形成 Z a b s \mathbf{Z}{\mathrm{abs}} Zabs，则 Z a b s \mathbf{Z}{\mathrm{abs}} Zabs 中的局部最大幅度会出现在对应目标距离和角度的 RA 单元处。 Z a b s \mathbf{Z}_{\mathrm{abs}} Zabs 表示为

Z a b s [ i , j ] = ∣ R e ( Z [ i , j ] ) ∣ 2 + ∣ I m ( Z [ i , j ] ) ∣ 2 , (33) Z_{\mathrm{abs}}[i,j]= \sqrt{|\mathrm{Re}(Z[i,j])|^2+|\mathrm{Im}(Z[i,j])|^2}, \tag{33} Zabs[i,j]=∣Re(Z[i,j])∣2+∣Im(Z[i,j])∣2 ,(33)

其中 R e ( ⋅ ) \mathrm{Re}(\cdot) Re(⋅) 和 I m ( ⋅ ) \mathrm{Im}(\cdot) Im(⋅) 分别表示取复数实部和虚部， ∣ ⋅ ∣ |\cdot| ∣⋅∣ 表示取实数绝对值。

由于发射和接收 ULA 天线总是放置得很近，RA 图会受到 TX-RX 直通污染。对同一套硬件，TX-RX 直通及其旁瓣通常固定在某些距离 bin 上，并出现在 RA 图的所有角度 bin 中。因此，本文预先通过将 TX 与 RX ULA 天线同时指向天空且无目标出现的方向，测量 TX-RX 直通对应的距离 bin，然后用设备噪声值替换被污染的 bin。

由于 RA 图中的噪声、旁瓣和虚假点可能降低建图性能，本文在 RA 图上执行 CA-CFAR 算法，只保留 RCS 值高于 CA-CFAR 阈值的元素，并将其他元素置为 0。由于多次反射、测量误差等原因，远离主要杂波的区域仍可能存在若干小型离散杂波。本文将这些离散杂波定义为异常值，并进一步利用 K 近邻滤波（KNN）在图上检测和剔除它们。

回波幅度同时受到目标材料以及收发机与目标之间距离的影响。为了在地图中恢复仅由材料差异造成的 RCS 值，本文对 RA 图执行功率补偿。对第 i i i 个距离 bin，其对应距离为 i c / ( 2 B W ) i c/(2BW) ic/(2BW)，补偿因子函数设计为

k [ i ] = ( 1 + K c o m p ⋅ i N r ) δ , (34) k[i]=\left(1+K_{\mathrm{comp}}\cdot\frac{i}{N_r}\right)^\delta, \tag{34} k[i]=(1+Kcomp⋅Nri)δ,(34)

其中 K c o m p K_{\mathrm{comp}} Kcomp 和 δ \delta δ 由真实实验调节。RA 图中每个元素乘以相应补偿因子完成补偿。

注：

K c o m p K_{\mathrm{comp}} Kcomp 是距离功率补偿的强度系数，控制距离 bin 增大时补偿因子的整体增幅；值越大，远距离回波被放大得越多。

δ \delta δ 是补偿公式的指数，控制补偿随距离增长的非线性程度；值越大，远距离 bin 的补偿增长越快。

在得到滤波并补偿后的 RA 图后，即可构建无失真的局部 RCS 栅格地图。根据 RCS 栅格地图模型，局部地图中落入 RA 图某个 bin 的每个栅格都被赋予与该 bin 相同的 RCS 值。考虑时刻 t t t 的用户位置 ( x u , y u , ψ u ) (x_u,y_u,\psi_u) (xu,yu,ψu)，坐标为 [ p , q ] [p,q] [p,q] 的栅格点若落在第 i i i 个角度 bin 和第 j j j 个距离 bin 内，应满足

( i − 1 2 ) Δ θ ≤ arctan ⁡ y o + q d y − y u x o + p d x − x u < ( i + 1 2 ) Δ θ , (35) \left(i-\frac{1}{2}\right)\Delta\theta \leq \arctan\frac{y_o+qd_y-y_u}{x_o+pd_x-x_u} < \left(i+\frac{1}{2}\right)\Delta\theta, \tag{35} (i−21)Δθ≤arctanxo+pdx−xuyo+qdy−yu<(i+21)Δθ,(35)

( j − 1 2 ) Δ R ≤ ( x o + p d x − x u ) 2 + ( y o + q d y − y u ) 2 < ( j + 1 2 ) Δ R , (36) \left(j-\frac{1}{2}\right)\Delta R \leq \sqrt{(x_o+pd_x-x_u)^2+(y_o+qd_y-y_u)^2} < \left(j+\frac{1}{2}\right)\Delta R, \tag{36} (j−21)ΔR≤(xo+pdx−xu)2+(yo+qdy−yu)2 <(j+21)ΔR,(36)

其中 d x d_x dx 和 d y d_y dy 分别为 X X X 轴和 Y Y Y 轴方向栅格尺寸， Δ θ \Delta\theta Δθ 与 Δ R \Delta R ΔR 分别为 RA 图的角度分辨率和距离分辨率。由于 RCS 值与多种电磁属性有关，同一地图栅格点在不同 RA 图中的测量结果可能不同。为简化建图算法并强调环境材料属性，本文选择同一栅格点的最大 RCS 值作为估计值。因此，当获取一帧新的 RA 图时，栅格值 M [ p , q ] M[p,q] M[p,q] 更新为

M [ p , q ] = max ⁡ { M [ p , q ] , Z [ i , j ] } . (37) M[p,q]=\max\{M[p,q],Z[i,j]\}. \tag{37} M[p,q]=max{M[p,q],Z[i,j]}.(37)

注：

M [ p , q ] M[p,q] M[p,q] 中的 p , q p,q p,q 是局部二维地图的 X / Y X/Y X/Y 栅格索引，不是角度和距离； Z [ i , j ] Z[i,j] Z[i,j] 中的 i , j i,j i,j 才是 RA 图的角度 bin 与距离 bin。

式 (35)-(36) 先把地图格子 [ p , q ] [p,q] [p,q] 对应的空间位置，依据当前 UE 位姿转换为相对 UE 的角度和距离，从而找到对应的 RA 单元 Z [ i , j ] Z[i,j] Z[i,j]；式 (37) 再用该观测值更新同一物理位置的地图格子，并保留较大的 RCS 值。

"强调环境材料属性"是指保留同一栅格历史观测中的最大 RCS，使金属、玻璃等强反射材料在地图中更明显；这样可减少弱角度、遮挡或噪声观测对材料反射特征的稀释。

由 OFDM 通信得到的粗 RCS 栅格地图（comm-map）与由 OFDM 感知得到的 RCS 栅格地图（sens-map）合并，形成某个用户的局部地图。合并时，首先用一个因子对 comm-map 加权，该因子为 sens-map 平均 RCS 值与 comm-map 平均 RCS 值之比。然后，对每个栅格在 sens-map 与加权 comm-map 之间选择最大 RCS 值，得到局部 RCS 栅格地图。

C. 全局地图拼接

局部建图完成后，所有用户把局部地图发送到相应基站，所有基站再把局部地图发送到某个中央基站。随后，根据每个基站的绝对位置，将所有局部地图拼接为全局地图。以基站 #A 为例，记其局部地图和位置为 M A ( l ) \mathbf{M}_A^{(l)} MA(l) 与 ( x A , y A , ψ A ) (x_A,y_A,\psi_A) (xA,yA,ψA)。注意，地图 M A ( l ) \mathbf{M}A^{(l)} MA(l) 是在基站 #A 的局部坐标系 A \mathcal{A} A 中构建的，局部坐标系 A \mathcal{A} A 的原点为基站 #A 的位置。传输到中央节点后，局部地图被对齐到全局坐标系。对 M A ( l ) \mathbf{M}A^{(l)} MA(l) 中第 ( i , j ) (i,j) (i,j) 个栅格，其在局部坐标系 A \mathcal{A} A 下的坐标为 ( x i , j ( l ) , y i , j ( l ) ) (x{i,j}^{(l)},y{i,j}^{(l)}) (xi,j(l),yi,j(l))，则其在全局坐标系下的坐标为