DIY使用下行TDOA技术的UWB精确定位系统

此前，我写了几篇介绍从零开始实现一个使用 TDOA 技术的 UWB 精确定位系统的文章，其中介绍的 TDOA 技术是上行 TDOA（Uplink TDOA）。最近，我完整实现了基于 下行 TDOA（Downlink TDOA） 技术的 UWB 精确定位系统。在研究下行 TDOA 技术期间，我写过几篇相关的文章。现在，我把全部信息汇总，并结合最近的一些心得体会，写成此文。

本文面向有嵌入式开发经验但没有 UWB 定位背景的软硬件工程师。因此，在正式进入系统设计之前，我会先花一些篇幅介绍 UWB 和 TDOA 定位的基础知识。如果你已经熟悉这些内容，可以直接跳到感兴趣的章节。

1.1 UWB 简介

在进入 TDOA 原理之前，先介绍一下 UWB（Ultra-Wide Band，超宽带）的基本概念，帮助没有 UWB 背景的工程师快速建立认知。

什么是 UWB？

UWB 是一种短距离无线通讯与测距技术。它被 IEEE 802.15.4z 标准所规范（该标准于 2020 年发布，在早期 IEEE 802.15.4a 的基础上增强了安全性和测距精度）。UWB 与传统的 WiFi、蓝牙有本质的不同：

  • WiFi/蓝牙使用持续的正弦载波来传输数据，接收方通过解调载波来恢复信息。这类信号的带宽通常只有几十 MHz（WiFi 5 的一个 80MHz 信道已经算很宽了）。
  • UWB 使用极短的脉冲信号（通常在亚纳秒到几纳秒之间），信号带宽达到 500MHz 以上。正是因为带宽极大，所以被称为"超宽带"。

下面用一幅简化的示意图来直观对比这两类信号：

graph LR
	subgraph "窄带信号（WiFi/蓝牙）"
		NB["连续正弦载波<br/>带宽: ~20-80MHz<br/>持续时间: 长<br/>时间分辨率: ~数十纳秒"]
	end
	subgraph "UWB 脉冲信号"
		UWB["极短脉冲序列<br/>带宽: ≥500MHz<br/>脉冲宽度: ~2ns<br/>时间分辨率: ~亚纳秒"]
	end

为什么"带宽大"就能"定位准"？

这个问题可以从时域和频域两个角度来理解：

  • 时域角度：脉冲越窄，在时间轴上的分辨率就越高。UWB 脉冲信号的分辨能力可以精确到亚纳秒级别。电磁波以光速传播——1 纳秒在空气中飞行大约 30 厘米——这意味着如果我们能精确测量信号到达的时间，理论上就能得到厘米级的距离测量精度。
  • 频域角度：根据信号处理理论，信号带宽越大，其时域分辨率（即区分两个紧邻信号的能力）越高。500MHz 带宽对应的时间分辨率约为 2ns，也就是约 60cm 的空间分辨率。芯片内部进一步通过过采样和插值算法，将实际精度提升到远高于此理论极限。

相比之下，WiFi 信号的带宽只有几十 MHz，对应数米的分辨率——这就是为什么 WiFi 定位（基于 RSSI 或 RTT）的精度通常只能做到 13 米，而 UWB 可以轻松做到 1030 厘米。

UWB 的工作原理——打雷的类比

如果你对"基于时间的定位"还不太理解，可以想象一下打雷： 闪电瞬间发生（光速传播几乎没有延迟），之后我们才听到雷声（声速约 340 m/s）。通过闪电和雷声之间的时间差，我们可以估算出雷电发生地离我们的距离。UWB 定位的原理非常类似，只不过我们捕捉的不是声波而是电磁脉冲——因为光速极快（约 $3 \times 10^8$ m/s），所以 UWB 芯片里的计时器需要拥有极高的分辨率，才能精准抓取这短暂的飞行时间差异。

DW3000 芯片的计时器

在本文中，我们选用的 UWB 芯片是 Qorvo DW3000（Decawave 被 Qorvo 收购后的品牌）。DW3000 的内部计时器为 40 位宽，时钟频率约 63.8976 GHz（即每个 tick 的时间间隔约为 15.65 皮秒，对应约 4.7 毫米的空间分辨率）。

40 位计时器的满量程约为 $2^{40} \times 15.65 \text{ps} \approx 17.2 \text{秒}$。也就是说，计时器每隔约 17.2 秒就会溢出归零（overflow/wrap-around）。这个溢出在软件设计中需要特别注意——如果两个时间戳分处溢出点的两侧，简单的相减会得到错误的结果。后续章节我们会详细讲解如何处理这个问题。

小贴士： 15.65 皮秒的计时器分辨率虽然很高，但这并不意味着 DW3000 的实际测距精度就是 4.7mm。实际精度受到很多因素的影响，包括天线延迟、多径效应、时钟稳定性等。在理想条件下（无遮挡、视距传播），DW3000 的测距精度通常在 ±10cm 以内。

1.2 TDOA 定位的原理

什么是 TDOA？

TDOA 的英文全称是 Time Difference of Arrival（到达时间差）。顾名思义，它通过测量同一信号到达不同接收点的时间差来计算发射源的位置。

其实我们日常使用的 GPS/北斗导航定位技术在本质上就是一种 TDOA——手机端的 GPS 芯片接收多颗卫星发出的信号，通过这些信号到达手机的时间差，计算出手机所在位置。GPS/北斗使用的是下行 TDOA定位模式，就是由被定位的终端自己计算自己的坐标。

TDOA vs TOA/TWR：

与 TDOA 经常一起被提到的是 TOA（Time of Arrival） 和 TWR（Two-Way Ranging）。TOA 通过测量信号从发射到到达的绝对飞行时间来计算距离，这要求发射端和接收端的时钟严格同步。TWR 则通过发送方和接收方之间的两次（或多次）往返通讯来测量距离，无需时钟同步，但每次测距都需要占用空口资源进行双向通讯。TDOA 只需要单向信号加上接收端之间的时间差，不需要知道信号发出的绝对时间，因此在系统设计上更灵活，也更适合大规模部署。

用上行 TDOA 解释原理

下行 TDOA 介绍起来比较复杂，我们先以上行 TDOA为例说明 TDOA 定位的原理。

在上行 TDOA 中，需要被定位的 Tag（标签） 发出 UWB 无线定位信号，附近的 Anchor（基站/锚点） 会收到 Tag 发出的定位信号。无线电波在空中以光速飞行（约 $3 \times 10^8$ m/s），因为各个 Anchor 到 Tag 的距离不一样，各个 Anchor 会在不同的时刻收到这个定位信号——距离远的晚一点收到，距离近的早一点收到。

例如有 2 个 Anchor $A$ 和 $B$，Anchor $A$ 距离 Tag 远一点，Anchor $B$ 近一点，这两个 Anchor 收到信号的时刻不同。两个时刻相减，就是 Tag 发出的信号到 Anchor $A/B$ 的到达时间差。把时间差乘以光速，就得到了 Tag 到两个 Anchor 的距离差。

双曲线定位的数学原理

从数学的角度来看，假设 Tag 到 $A$、$B$ 间的距离差是 $\Delta d_{AB}$。如果以 $A$、$B$ 分别作为两个焦点，我们可以在平面上画出一条双曲线（或三维空间中的双曲面），这条双曲线上的任意一个点到 $A$、$B$ 间的距离差都恰好等于 $\Delta d_{AB}$。也就是说，Tag 必定位于这条双曲线上的某个位置。

双曲线

仅凭一条双曲线，我们只知道 Tag 在线上的某个位置，无法确定精确坐标。如果再增加一个 Anchor $C$，我们就可以得到另一条独立的双曲线（例如以 $A/C$ 为焦点，距离差为 $\Delta d_{AC}$）。两条双曲线的交点（通常有一到两个）就是 Tag 的候选位置。

双曲线TDOA定位

关于独立方程数量的说明：

有些读者可能会疑惑：3 个 Anchor 不是有 3 对组合（AB、AC、BC）吗？为什么只有 2 条独立的双曲线？原因是 $\Delta d_{BC} = \Delta d_{AC} - \Delta d_{AB}$，第三个时间差可以由前两个推导得出，并不提供新的独立信息。一般来说，$n$ 个 Anchor 产生 $n-1$ 个独立的 TDOA 方程。

  • 二维定位（求 $x, y$）：需要至少 3 个 Anchor（2 个独立方程解 2 个未知数）
  • 三维定位（求 $x, y, z$）：需要至少 4 个 Anchor（3 个独立方程解 3 个未知数）

注意： 上面的双曲线图示只是帮助你直观理解定位原理的二维示意。实际上我们的空间是三维的，数学上对应的是双曲面。3 个双曲面相交得到的是一条曲线（而非一个点），所以三维定位至少需要 4 个 Anchor。

关于 Z 轴的处理

即使我们只想做二维定位（仅求 $x/y$），但 Anchor 通常部署在比较高的位置（例如天花板，高度 3~5 米），而 Tag 则在比较矮的位置（例如佩戴在人的胸前，高度约 1.5 米）。Anchor 和 Tag 之间存在显著的高度差。如果忽略这个高度差，直接在二维平面上做计算，会引入系统性的距离误差。因此，计算坐标时仍然要按照三维空间来计算，只不过可以把 Tag 的 $z$ 坐标设置为一个固定常数（例如 150cm）。这样，方程中的未知数只有 $x$ 和 $y$，但距离的计算仍然考虑了三维空间中的真实距离。

增加 Anchor 提升精度

在实际工程中，为了得到更好的定位效果，我们部署的 Anchor 数量通常会超过最小数量要求。多余的 Anchor 提供了冗余信息（过定系统），可以通过最小二乘法等统计方法来提高定位精度和鲁棒性。

在做三维定位时，还要注意 Anchor 的高度分布——不能把所有 Anchor 都装在天花板上。如果所有 Anchor 都在同一个高度平面上，$z$ 方向的信息就非常薄弱（GDOP 在 $z$ 方向发散）。应该把部分 Anchor 部署在较低的位置，例如与 Tag 大致同高甚至更低的地方。

关于"几何精度因子（GDOP）“的概念：

跟 GPS 卫星定位类似，Anchor 的空间几何分布对定位精度有极大的影响。如果所有 Anchor 都挤在一个很小的区域内，那么双曲面几乎是平行的，微小的时间测量误差就会引起巨大的坐标偏差——就像两条几乎平行的直线，微小的角度变化会使交点发生极大位移。反之，如果 Anchor 均匀分布在 Tag 周围（理想情况下从多个方向包围住 Tag），定位精度会好得多。

这就是几何精度因子（Geometric Dilution of Precision, GDOP）的概念。GDOP 是一个无量纲的倍数因子——GDOP 越小，几何构型越好，测量误差被放大的程度越低。在部署 Anchor 时，务必注意它们的空间分布均匀性，避免所有 Anchor 排列成一条直线或聚集在一个角落。

上行 TDOA 的数据流

上面所描述的过程，就是上行 TDOA 的原理。具体的数据流如下：

  1. Tag 发出一个 UWB 数据包
  2. 多个 Anchor 收到这个数据包，各自记录接收时间戳
  3. 各 Anchor 将数据包内容及接收时间戳通过网络（Ethernet/WiFi）上报给 RTLE（Real-Time Location Engine，实时定位引擎）——运行在服务器上的定位计算软件
  4. RTLE 根据各 Anchor 的时间戳差值和已知的 Anchor 坐标，求解 Tag 的位置

通常，上行 TDOA 的坐标计算都是由 RTLE 集中完成的，Tag 本身不参与计算。

时钟同步——TDOA 系统的基石

我们知道，计算坐标时需要知道各 Anchor 接收信号的时间差。Tag 发送定位数据包的时间是确定的（同一个信号），但各个 Anchor 收到数据包时所记录的时间戳，必须基于同一个时间基准，才有可能进行比较和相减。

为什么需要时钟同步？

一般情况下，每个 Anchor 的 UWB 芯片使用独立的石英晶振（Crystal Oscillator）来驱动内部计时器。由于制造工艺的差异（晶振频率公差、负载电容偏差、芯片内部电路差异），以及运行环境的变化（温度、电压、湿度的波动），每个 UWB 芯片的内部计数器频率都略有不同。

即使在出厂时校准过，经过一段时间运行后，不同设备的时间戳之间的偏差会越来越大。而 UWB 定位对时间精度的要求是纳秒级的（1 纳秒 ≈ 30cm），即使极微小的频率偏差（百万分之几，即 ppm 级别），在很短的时间内也会积累到不可接受的程度。

类比生活经验： 大家都有过这样的体验——家里的挂钟和手表哪怕在同一天对准时间，过几天就会出现几秒甚至几十秒的偏差。UWB 芯片的晶振也是一样的道理。

做一个简单的计算：假设两个 Anchor 的晶振频率差为 5ppm（这在普通晶振中很常见）。UWB 芯片的基准频率约为 499.2MHz，5ppm 的偏差意味着两个芯片的频率差约为 $499.2\text{MHz} \times 5 \times 10^{-6} = 2.496\text{kHz}$。每秒累积的时间偏差约为 $5 \times 10^{-6} \text{s} = 5\mu\text{s}$。5 微秒对应的距离误差为 $5 \times 10^{-6} \times 3 \times 10^{8} = 1500$ 米！也就是说，如果不做时钟同步，仅仅 1 秒钟之后，两个 Anchor 之间的时间偏差就足以导致 1500 米的距离误差。

再举个形象的例子：在跑步比赛中，如果为每个运动员使用单独的秒表计时，但每个秒表的走时快慢不同。即使看上去大家都显示"10.00 秒”，走得快的那个秒表实际上只过了不到 10 秒，走得慢的那个实际上超过了 10 秒。这样的成绩当然无法公平比较。

时钟同步的基本思路

通常，我们会指定一个 Anchor 作为时钟源（Root Clock Source），也称为参考 Anchor。它定期发出时钟同步信号（TimeSync 数据包）。其他 Anchor 接收时钟源的同步信号，通过算法在内部维持一个与时钟源一致的"全局时间"（Global Time）。这个过程我们称之为时钟同步（Clock Synchronization）。

从字面上来说，“时钟同步"和"时间同步"是不同的概念——“时钟同步"侧重于使各设备的时钟频率一致（频率同步），“时间同步"侧重于使各设备的时间值一致（相位同步）。但在本文中，我们不做严格区分——我们的目标是让各个 Anchor 维持一个准确的全局时间，使程序可以把任意 Anchor/Tag 的本地时间与全局时间互相转换。

本地时间 vs 全局时间：

  • 本地时间（Local Time）：某个 UWB 芯片内部计时器的原始读数。每个芯片有自己独立的本地时间。
  • 全局时间（Global Time）：以时钟源为基准的统一时间。通过时钟同步参数，任何 Anchor/Tag 都可以将自己的本地时间转换为全局时间。

下行 TDOA

我们知道，上行 TDOA 由 Tag 发送定位信号，各个 Anchor 接收。只需要把各个 Anchor 接收信号的全局时间戳相减，就得到了到达时间差。

下行 TDOA 要复杂一些：各个 Anchor 主动发出定位数据包（称为 TimeSync 包），Tag 记录收到这些数据包的时间戳，再"想办法"得到时间差。

为什么不能直接相减？

原因在于：通常 Tag 的 UWB 接收机只有 1 个通道，同一时刻只能接收一个信号。各个 Anchor 是在不同的时刻发出的 TimeSync 数据包，Tag 当然是在不同的时刻收到它们。因为各个 Anchor 的发送时间不同，所以不能直接用 Tag 收到各数据包的本地时间戳相减来得到时间差——这些时间戳之间的差异既包含了距离差信息，也包含了 Anchor 发送时间的差异，两者混在一起无法分离。

Tag “锁定” Anchor

为了解决这个问题，Tag 需要对 Anchor 进行锁定操作。所谓"锁定”，本质上是 Tag 与某个 Anchor 进行时钟同步。通过锁定，Tag 在内部维持从该 Anchor 得到的"全局时间"信息。这样，Tag 就可以随时将自己的本地时间转换为该 Anchor 对应的全局时间。

更准确地说，Tag 通过持续接收某个 Anchor 发出的 TimeSync 包，估算出自己的本地时钟与该 Anchor 全局时钟之间的频率偏差（skew）和时间偏移（offset）。有了这两个参数，Tag 就能把任意本地时刻转换为该 Anchor 对应的全局时间。

当 Tag 锁定了多个 Anchor（例如 4 个）之后，对于某个特定的本地时刻 $t_{local}$，Tag 可以将其分别转换为 4 个 Anchor 各自对应的全局时间 $t_{G,1}, t_{G,2}, t_{G,3}, t_{G,4}$。因为 Tag 到各个 Anchor 的距离不一样，这 4 个全局时间会有差异——把它们两两相减，就得到了到达时间差（TDOA）。

为什么这个方法能分离出距离差？

关键在于"全局时间"的物理含义。假设在某个时刻，Tag 想知道"如果此刻各 Anchor 同时向我发出信号，各信号到达我需要多久？“通过时钟同步参数，Tag 可以把自己的本地时间映射到各 Anchor 的全局时间上。由于距离不同，映射结果之间的差异恰好反映了距离差引起的飞行时间差。

下面用一幅流程图来对比上行 TDOA 和下行 TDOA 的工作流程：

graph TD
	subgraph "上行 TDOA 工作流程"
		T1["Tag 发送 UWB 定位信号"] --> A1_UP["Anchor A 记录接收时间戳 Ta"]
		T1 --> A2_UP["Anchor B 记录接收时间戳 Tb"]
		T1 --> A3_UP["Anchor C 记录接收时间戳 Tc"]
		A1_UP --> RTLE["RTLE 服务器端"]
		A2_UP --> RTLE
		A3_UP --> RTLE
		RTLE --> CALC_UP["服务器集中计算时间差 & 坐标"]
	end

	subgraph "下行 TDOA 工作流程"
		A1_DL["Anchor A 发送 TimeSync 包 (含时间戳)"] --> T2["Tag 接收"]
		A2_DL["Anchor B 发送 TimeSync 包 (含时间戳)"] --> T2
		A3_DL["Anchor C 发送 TimeSync 包 (含时间戳)"] --> T2
		T2 --> LOCK["Tag 分别锁定每个 Anchor<br/>（=与每个 Anchor 建立时钟同步）"]
		LOCK --> CONV["Tag 将本地时间转换为各 Anchor 的全局时间"]
		CONV --> CALC_DL["Tag 自己计算时间差 & 坐标"]
	end

我们在后面的章节会对"时钟同步"和"锁定 Anchor"进行更详细的说明。

1.3 上行 TDOA vs 下行 TDOA

“上行 TDOA"和"下行 TDOA"的对比如下表所示：

对比项上行 TDOA下行 TDOA
定位信号发送方Tag 发送Anchor 发送
定位信号接收方Anchor 接收Tag 接收
时钟同步发生在Anchor 之间Anchor 之间 + Tag 与 Anchor 之间
时钟同步精度要求较高精度即可需要极高精度
坐标计算专门的服务器（RTLE）集中计算Tag 自己计算（无需服务器参与）
Tag 省电表现非常省电（平时休眠，定期醒来发一个信号后继续休眠）比较耗电（需要长时间保持接收状态来接收各 Anchor 的信号）
Tag 硬件成本很低（功能少，只需要能发送定位信号）较高（需要较大 RAM 和较强 MCU 来完成坐标计算）
系统基础设施成本需要专门的服务器运行定位引擎，总成本较高不需要专门的定位服务器，即使 Tag MCU 成本增加，总成本也更低
系统容量/可扩展性Tag 数量多时，空口上行竞争激烈，需要时分/频分管理Anchor 广播，Tag 数量无上限（只收不发）
隐私性坐标在服务器上计算，Tag 无法对自己的位置保密坐标在 Tag 本地计算，Tag 可以选择不上报位置

补充说明——系统容量：

上行 TDOA 中，每个 Tag 都需要在空中发送信号，Tag 数量多的时候，UWB 信道会变得拥挤，可能出现数据包碰撞（collision）。需要复杂的时分/频分机制来管理空口资源——比如给每个 Tag 分配发送时隙、或者让 Tag 使用随机退避算法（类似 WiFi 的 CSMA/CA）。即便如此，当 Tag 数量达到几百上千时，系统的更新率和可靠性都会大幅下降。

而下行 TDOA 中，Anchor 定期广播 TimeSync 信号，Tag 只接收不发送，因此 Tag 的数量在理论上没有上限——你可以在同一个区域内部署任意多个 Tag，它们互不干扰。这是下行 TDOA 在大规模部署场景中（如大型仓库、体育馆、商场）非常显著的优势。

补充说明——隐私性：

下行 TDOA 还有一个常被忽视的优势：位置隐私。由于坐标是在 Tag 本地计算的，Tag 可以选择不将自己的位置上报给任何服务器。这在某些对隐私要求较高的应用场景中（如军事、个人追踪设备）具有重要意义。

1.4 下行 TDOA 的技术要点

看过我之前系列文章的读者，应该对 TDOA 技术有了一定了解，也知道了时钟同步和坐标计算这两个核心技术难点。下面我们来分析一下下行 TDOA 在这些方面有哪些特殊的技术要求。

1.4.1 时钟同步——下行 TDOA 的最大挑战

下行 TDOA 对时钟同步的精度要求比上行 TDOA 更高。这是下行 TDOA 系统设计中最核心的难点。原因如下：

在上行 TDOA 中，所有 Anchor 收到的是同一个 Tag 发出的同一个信号。各 Anchor 记录的时间戳虽然基于各自的本地时钟，但时钟同步的误差只会影响时间戳差值的计算——而且这个误差是"共模"的，在一定程度上可以通过差分运算抵消。

而在下行 TDOA 中，各个 Anchor 在不同时刻发出信号，Tag 需要通过时钟同步参数将自己在不同时刻收到数据包的本地时间戳转换为各 Anchor 的全局时间。时钟同步的误差会直接且完全地叠加到最终的时间差上。例如：

  • 如果 Anchor 之间的时钟同步误差为 1 纳秒，那就意味着定位结果多了大约 30 厘米的偏差
  • 如果同步误差为 3 纳秒，偏差就接近 1 米
  • 如果同步误差为 10 纳秒，系统基本就不可用了

因此，下行 TDOA 对时钟同步精度的要求通常是亚纳秒级（<1ns）。

1.4.2 多径传播与第一径检测

我们知道，在无线电波传输的过程中会有遮挡和干扰，这些因素可能会让接收方收不到信号，或者收到的信号不是来自第一路径（First Path）——也就是直线传播路径。

什么是多径传播？

理论上讲，无线电波是直线传输的（这也是无线电定位的物理基础）。但实际上，无线电波存在多径传播（Multipath Propagation）现象。也就是说，无线电波从发送方到接收方的传输路径可能有很多条：

  • 第一路径（First Path / Line-of-Sight, LOS）：直线传输，路径最短，时间最早到达——这是我们进行定位时唯一想要的路径
  • 反射路径：信号碰到墙壁、天花板、地面后反弹到达接收端
  • 绕射路径：信号绕过障碍物的边缘到达接收端
  • 散射路径：信号遇到粗糙表面或小物体后向多个方向散射

只有第一路径对应真正的直线距离，其他路径的传输距离都更长，到达时间也更晚。

graph LR
	A["发送端 TX"] -- "第一径 (直线/最短/最准)" --> B(("接收端 RX"))
	style A fill:#f9f,stroke:#333,stroke-width:2px
	style B fill:#bbf,stroke:#333,stroke-width:2px
	A -. "反射路径: 经墙壁反射" .-> W1["墙壁"] -.-> B
	A -. "绕射路径: 经障碍物绕射" .-> W2["障碍物"] -.-> B

	linkStyle 0 stroke:#ff0000,stroke-width:4px;
	linkStyle 1,2 stroke:#888,stroke-dasharray: 5 5;

图解：

  • 红色直线：从发送端到接收端的第一径（First Path），路径最短，时间最准
  • 灰色虚线：经过墙壁、物体反射/绕射的**多径（Multipath）**信号，路径较长，到达时间晚

第一径被"吞没"的问题

从道理上说，接收机收到的第一个信号应该就是第一路径传过来的——因为两点之间直线距离最短，第一路径的信号理应最先到达。但现实中的情况比较复杂：

  1. 第一路径可能被部分遮挡（NLOS）： 如果发送端和接收端之间有障碍物（如墙壁、人体、金属设备），第一路径的信号会被衰减甚至完全阻断。这种情况称为 NLOS（Non-Line-of-Sight，非视距）。

  2. 强多径信号压制弱第一径信号： 即使第一路径的信号没有被完全阻断，也可能因为衰减而变得很微弱。而某些反射路径（如经过大面积金属墙面反射）的信号可能反而很强。

  3. AGC 的影响： 接收机中通常有一个 AGC（Automatic Gain Control，自动增益控制） 电路。当收到的信号太弱时，AGC 会自动提高放大倍数；如果信号太强，AGC 会降低放大倍数。AGC 的作用是将信号幅度调整到 ADC（模数转换器）的最佳输入范围。

    问题在于：如果一个较强的多径信号先触发了 AGC 的调整（降低增益），后面到达的微弱第一径信号可能就被"淹没"在噪声中了。或者反过来，如果 AGC 正处于高增益状态时收到了一个很强的多径信号，可能导致 ADC 饱和。

    收音机的类比： 使用过短波收音机的朋友可能有这样的体验——调到某个没有信号的频率时，背景噪声会逐步变大（AGC 在拼命放大）；调到有信号的频率时，背景噪声就变小甚至消失（AGC 把增益调低了）。UWB 接收机的 AGC 工作原理与此类似。

LDE——前导码检测与第一径提取

芯片是通过**前导码（Preamble）**来识别数据包的。UWB 数据包的结构大致如下：

┌──────────────┬───────────────┬──────────┬──────────────┐
│  Preamble    │     SFD       │   PHR    │   Payload    │
│  (前导码)     │ (帧定界符)    │ (帧头)   │  (有效载荷)   │
└──────────────┴───────────────┴──────────┴──────────────┘

接收机不断地接收无线信号，当它判断收到的信号与预期的前导码模式（一种特定的伪随机序列）相符时，就知道后续将要收到一个完整的数据包了。前导码会重复多次（DW3000 支持配置 16~4096 个符号的前导码长度），以确保接收机能在各种信噪比条件下正确识别。

在前导码检测过程中，芯片的 LDE（Leading Edge Detection，前沿检测算法） 负责在接收到的信号中精确锁定第一径的到达时刻。由于多径效应，接收到的信号波形是多个延迟信号副本的叠加，LDE 需要从中分辨出最先到达的那个脉冲——即前沿（Leading Edge）。

在信号比较弱、或者信号比较强甚至达到饱和的情况下，LDE 在提取第一径时可能会出现偏差，导致接收机给出的"接收时间戳"不够准确。这种偏差在高精度时钟同步中是不可忽略的。

什么是 CFO（Carrier Frequency Offset）？

DW3000 芯片在接收数据包时，其内部的载波恢复环路会估算出载波频率偏移（CFO）。这个值反映了发送端和接收端晶振频率之间的差异。

CFO 的物理本质就是两个晶振之间的频率差，通常以 ppm（百万分之一） 或 ppb（十亿分之一） 为单位。CFO 可以作为辅助信息来判断时钟漂移率（drift/skew），也可以帮助识别异常的接收事件（如果某个数据包的 CFO 与历史值相差太大，可能说明该数据包受到了干扰或来自非第一路径）。后续章节在讨论高级时钟同步时会用到这个值。

实际部署中的应对

在实际部署中，Anchor 的安装位置是经过仔细考虑的——通常安装在开阔位置、互相之间无遮挡。因此 Anchor 之间的通讯大部分时候都能准确收到第一路径的信号。但仍可能因为临时遮挡（例如有人搬了一个金属柜子挡住了信号路径）或瞬时干扰导致收到的不是第一路径信号。这时，我们需要在软件中设计异常检测和过滤机制——通过检查信号质量指标（如首径信号功率、接收信号功率、CFO 等）来判断当前这次接收是否可信，不可信的数据直接丢弃。

1.4.3 时钟同步中的距离补偿

在时钟同步过程中，还有一个重要的问题必须处理：时钟源发出 TimeSync 信号，其他 Anchor 接收到这个信号——但信号从时钟源飞到各 Anchor 需要时间！这个飞行时间等于它们之间的距离除以光速。

在上行 TDOA 中，这个距离偏差可以在 RTLE 定位引擎中统一进行补偿——因为 RTLE 知道所有 Anchor 的坐标，可以在计算时考虑进去。

但在下行 TDOA 中，不再有集中式的定位引擎。时钟源与各 Anchor 之间的距离偏差必须在时钟同步阶段就进行补偿——也就是说，每个 Anchor 在收到时钟同步信号后，需要根据自己与时钟源之间的已知距离，把信号的飞行时间减去，才能得到准确的全局时间。

这就要求在系统配置时，把各个 Anchor 的坐标、时钟源的坐标等信息预先配置到每个 Anchor 的固件中。Anchor 在启动时根据这些坐标信息自动计算与时钟源的距离，并在时钟同步过程中进行补偿。

这对部署意味着什么？ 每次部署或移动 Anchor 后，都需要重新配置坐标信息。这增加了部署的复杂度，但为了保证定位精度，这一步是必不可少的。

1.5 系统规划

大致上，整个下行 TDOA 定位系统的结构如下图所示：

graph TD
	subgraph "现场部署 — Anchor 网络"
		A0["Anchor A0<br/>(根时钟源, Level 0)"]
		A1["Anchor A1<br/>(Level 1)"]
		A2["Anchor A2<br/>(Level 1)"]
		A3["Anchor A3<br/>(Level 2)"]
		A0 == "时钟同步" ==> A1
		A0 == "时钟同步" ==> A2
		A1 == "时钟同步" ==> A3
	end

	subgraph "定位标签"
		T1["Tag 1"]
		T2["Tag 2"]
	end

	A0 -. "TimeSync 广播" .-> T1
	A1 -. "TimeSync 广播" .-> T1
	A2 -. "TimeSync 广播" .-> T1
	A3 -. "TimeSync 广播" .-> T1
	A0 -. "TimeSync 广播" .-> T2
	A1 -. "TimeSync 广播" .-> T2

	T1 -- "WiFi" --> AGG["数据聚合服务器"]
	T2 -- "WiFi" --> AGG
	AGG --> MAP["前端地图显示"]

	PC["配置程序 (PC)"] -- "USB / WiFi" --> A0
	PC -- "USB / WiFi" --> A1
	PC -- "USB / WiFi" --> T1

时钟同步层级（Level）

在上图中，你可能注意到了 Anchor 后面标注的 “Level 0”、“Level 1”、“Level 2”。这是时钟同步的层级结构（Hierarchy）：

  • Level 0：根时钟源（Root Clock Source），是整个系统的时间基准。整个定位区域只有一个 Level 0 Anchor。
  • Level 1：直接从 Level 0 获取时钟同步的 Anchor。它们能"听到"Level 0 发出的 TimeSync 信号。
  • Level 2：无法直接听到 Level 0 信号的 Anchor，转而从 Level 1 Anchor 获取时钟同步。
  • 以此类推，可以有 Level 3、Level 4……

为什么需要多级结构？因为 UWB 信号的传输距离有限（DW3000 在室内环境下，有效通讯距离通常在 20~40 米左右，取决于发射功率、天线增益和环境遮挡）。如果定位区域很大（例如一个几千平方米的仓库），远处的 Anchor 可能根本收不到 Level 0 的信号。通过多级级联，可以把时钟同步信号"接力"传递到更远的 Anchor。

注意： 每增加一个层级，时钟同步误差就会累积一层。因此，层级数量不宜过多（通常建议不超过 2~3 级）。在规划系统时，应尽量把根时钟源放在定位区域的中心位置，以减少所需的层级数量。

系统组成说明

  • Anchor 网络：在定位现场部署若干 Anchor 硬件。Anchor 之间通过 UWB 保持时钟同步，同时通过 WiFi 接入局域网（用于配置管理和状态监控）
  • Tag（定位标签）：固定的或移动的被定位设备。Tag 接收 Anchor 发出的 TimeSync 广播包，自行计算出自己的坐标
  • Tag 的应用模式：Tag 可以是一个不联网的独立应用（例如带屏幕的手持设备，自己显示坐标），也可以通过 WiFi 联网，把坐标数据发送给应用服务器
  • PC 端配置程序：通过 USB 或 WiFi 与设备通讯，对 Anchor 和 Tag 的各种参数进行配置（如坐标、WiFi SSID/Password、时钟同步层级、UWB 信道参数等）
  • 数据聚合服务器：为方便应用系统的开发，可编写一个数据聚合的服务端程序，收集各 Tag 发来的坐标信息，再统一与应用系统对接（例如通过 WebSocket 或 MQTT 推送给前端）
  • 前端地图软件：为方便部署和调试，可编写一个简易的前端地图应用，把各 Tag 的定位结果实时显示在平面图上

开发任务清单

针对上面的规划，要做的事大致有：

  1. 定位 Anchor 的硬件设计 — 包含 ESP32-S3 + DW3000 + WiFi 天线 + UWB 天线 + 供电电路
  2. 定位 Tag 的硬件设计 — 包含 ESP32-S3 + DW3000 + 锂电池供电/充电电路
  3. 定位 Anchor 的固件 — 时钟同步、TimeSync 广播、WiFi 联网、配置管理
  4. 定位 Tag 的固件 — 锁定 Anchor、时钟同步、TDOA 坐标计算、WiFi 联网
  5. PC 端配置程序 — USB 通讯 + WiFi 通讯、参数配置界面
  6. 数据聚合后端程序 — 收集多个 Tag 的坐标数据，提供 API 接口
  7. 地图显示前端 — 加载场地平面图，实时显示 Tag 位置

2. 硬件选型和设计

2.1 选型

2.1.1 MCU 选型

看过我之前文章的同学，应该知道我之前设计的嵌入式系统大多使用 STM32 系列 MCU 作为主控。但后来我更倾向于使用 ESP32 系列 MCU。弃用 STM32 的原因有几个：

  • 价格波动大、供货不稳定。 我遇到过几次 STM32 涨价的冲击（2020~2021 年芯片荒期间尤为严重，部分型号价格翻了 10 倍以上）。国产的类 STM32 芯片（如 GD32、AT32 等）虽然很多，但每次 STM32 涨价，它们也跟着水涨船高。
  • 国产替代芯片尚需时间考验。 在资料完整性、技术支持响应速度等方面还不够完善。顺便提一下，很多国产芯片厂家都习惯性地保密，甚至下载 Datasheet 都要签 NDA（保密协议）。遇到技术问题，厂家经常像挤牙膏一样只提供尽量少的信息，让开发者非常被动。
  • STM32 资源偏少。 STM32 的目标应用偏向传统工业控制场景，价格合适的型号（如 STM32F1/F4）RAM/Flash 资源都比较紧张（F103 只有 20KB RAM），资源充裕的型号（如 STM32H7 系列）往往价格不菲。

这次我们的设计选择了 ESP32-S3。与上面几条正好相反：

特性STM32（常见型号）ESP32-S3
价格稳定性波动大稳定、性价比高
RAM20KB~1MB（取决于型号和价格）512KB 片上 + 最大 8MB 外扩 PSRAM
Flash64KB~2MB最大 16MB 外扩 SPI Flash
CPU单核 72~480MHz（Cortex-M 系列）双核 240MHz Xtensa LX7
WiFi无（需外加模组）内置 WiFi 4（802.11 b/g/n）
蓝牙无（需外加模组）内置 BLE 5.0
USB部分型号有 USB OTG内置 USB OTG（原生 USB）
文档/社区丰富非常丰富（ESP-IDF 官方文档 + 社区）

不过，ESP32 也有一些问题需要注意（例如后面会提到的 ISR 中浮点运算限制、字节对齐异常等硬件/软件陷阱），后面我会详细说到。

在 ESP32 系列 MCU 选型时，可选的型号也很多（ESP32 经典版、ESP32-S2、ESP32-S3、ESP32-C3、ESP32-C6、ESP32-H2 等）。其实我最初是打算用 ESP32（经典版本）的，后来仔细比较了几个型号之后，才决定使用 ESP32-S3。

我们对 MCU 有几条核心要求：

  • WiFi。 如果 MCU 自带 WiFi 功能，就不需要外加 WiFi 芯片/模组了，既节省成本和 PCB 面积，也减少固件设计的复杂度。ESP32 系列除 ESP32-H2 外，都内置 WiFi。

  • USB 配网。 我们需要用某种方式告知固件 WiFi 的 SSID 和密码。常见的方案有：

    • WiFi 配网协议（如 SmartConfig、SoftAP 模式）——需要手机 APP
    • 蓝牙配网——也需要手机 APP
    • USB 配网——只需要 PC 端配置程序

    我个人更倾向于使用 USB 来完成配网（用 PC 端配置程序直接下发 WiFi 凭证），这样无需开发和维护手机 APP。因此 MCU 需要支持原生 USB（USB OTG）。在 ESP32 系列中，ESP32-S2 和 ESP32-S3 支持原生 USB。

  • 计算能力。 ESP32 系列有单核和双核的 MCU。考虑到 Tag 需要完成坐标计算（涉及矩阵运算和数值迭代），双核更为合适——一个核处理 UWB 数据收发和时钟同步，另一个核处理坐标计算和 WiFi 通讯，互不阻塞。ESP32（经典版）和 ESP32-S3 是双核。

  • 稳定性与生态。 ESP32 系列有些型号使用 RISC-V 核心（如 ESP32-C3、C6、H2），虽然 RISC-V 架构本身很有前景，但相比久经考验的 Xtensa 指令集，其工具链和生态成熟度稍逊。从保守和稳定的角度，选择 Xtensa 内核的 MCU 更让人放心。

综合以上要求，最终选择了 ESP32-S3。

2.1.2 UWB 芯片选型

我之前使用过 Decawave DW1000，对其性能和驱动接口都比较熟悉。但是因为中国无线电管理部门的频率管制要求，DW1000 支持的频段在中国不被允许使用。目前要用 Decawave（现被 Qorvo 收购）系列的芯片，只有 DW3000 可选。

DW1000 vs DW3000 频段差异：

DW1000 主要工作在 Channel 17（中心频率约 3.5GHz6.5GHz），其中低频段（特别是 Channel 14，中心频率 3.54.5GHz 附近）在中国未被批准用于 UWB。DW3000 支持 Channel 5（中心频率 6489.6MHz）和 Channel 9（中心频率 7987.2MHz），其中 Channel 9 在中国的 UWB 管理规定中是被允许使用的。

如果你的产品需要在中国市场销售，务必确认使用 Channel 9 并符合国家无线电管理局的相关技术要求（包括发射功率谱密度限制等）。

我也考察过其他公司的 UWB 芯片：

  • NXP（恩智浦）：他们的 UWB 芯片（如 Trimension 系列）很难在公开市场买到，技术文档也需要签 NDA 才能获取。NXP 的 UWB 产品线似乎更关注手机/汽车等大客户场景（如数字车钥匙、安全支付），对小批量的定位系统开发者不太友好。有客户反映 NXP 主推的定位模式是 TWR（Two-Way Ranging），而非 TDOA，这与我们的技术路线不符。

  • 某国产 UWB 芯片：对标 DW3000，集成了 Cortex-M0 内核的 SoC，支持 4 天线、最大 31Mbps 通讯速率，功能很丰富。但厂家对小客户的技术支持不太到位，数据手册都要签 NDA 才能拿到。对于需要深入到寄存器级别调试的定位系统开发来说，缺乏公开技术文档是一个致命的问题。

所以，最终的选择只有 Qorvo DW3000。在 UWB 定位领域，DW3000 是目前文档最齐全、社区最活跃、技术支持最好的选择。

DW3000 驱动程序的注意事项

DW3000 与 DW1000 类似，Qorvo 也提供了封装底层寄存器操作的驱动程序（Driver/API）。可能是为了与 DW1000 保持一定的 API 兼容性，提供的大部分函数名与 DW1000 基本一致。

但千万不要被函数名迷惑—— DW3000 的寄存器布局与 DW1000 有很大不同，功能上也有较大差异（例如 DW3000 增加了 STS——Scrambled Timestamp Sequence，用于安全测距；数据速率支持到 6.8Mbps；前导码配置选项也有变化）。即使有 DW1000 的编程经验，也必须仔细查阅 DW3000 User Manual。

DW3000 的驱动程序为了兼顾扩展性和多平台兼容性，增加了一层平台抽象层（PAL）的封装。这导致一些项目中用不到的功能函数也会被编译链接进来，增加了固件体积。对于 RAM/Flash 比较紧张的 MCU，可能需要自行对 DW3000 驱动进行裁剪（注释掉不需要的函数、去掉未使用的特性代码）。不过对于 ESP32-S3 来说，Flash 和 RAM 通常都比较充裕，这个问题不太严重。

2.1.3 供电

供电设计是嵌入式产品中非常重要的一环，直接影响系统的稳定性、续航和散热。

Anchor 供电

之前的上行 TDOA 项目，我选用 PoE（Power over Ethernet，以太网供电） 作为 Anchor 的供电方式——一根网线同时解决数据传输和供电两个问题，部署非常方便。

新项目的 Anchor 放弃了 Ethernet 而改用 WiFi 联网，因此 PoE 不再可用。Anchor 改为使用以下供电方式：

  • USB 供电（5V）：通过 USB Type-C 接口供电，方便使用充电宝或 USB 适配器
  • DC 12V 供电：通过 DC 电源插座供电，适合长期固定安装场景（使用 12V 开关电源适配器）

某些型号的 Anchor 还可以内置锂电池，以方便部署在没有市电的场景。可以定期更换 Anchor 或把 Anchor 取下充电后再放回原处。

Tag 供电与充电

Tag 作为移动设备，需要使用锂电池供电。充电方式延续老项目——USB 充电或QI 无线充电 + 锂电池。但充电管理芯片做了重要的升级。

之前使用的 TP4057 是一款线性充电芯片：充电电流直接从输入端流向电池，输入电压与电池电压之差乘以充电电流等于热功耗。

$$P_{heat} = (V_{in} - V_{batt}) \times I_{charge}$$

例如，使用 5V USB 充电，电池电压为 3.8V 时，电压差为 1.2V。如果充电电流为 500mA：

$$P_{heat} = 1.2V \times 0.5A = 0.6W$$

0.6W 的发热对于一个小体积设备来说已经相当可观了。如果为了控制发热而限制充电电流，充电时间就会很长；如果增大充电电流，发热更加严重，甚至可能影响锂电池的安全。

新项目的充电芯片换成了 SLM6600——一款 DC-DC 开关充电芯片。DC-DC 充电芯片的产生的热量取决于芯片的转换效率，而非输入输出电压差。SLM6600 在典型工况下的充电效率约为 92% 以上，这意味着：

$$P_{heat} = P_{in} \times (1 - \eta) = \frac{V_{batt} \times I_{charge}}{\eta} \times (1-\eta)$$

同样 3.8V/500mA 充电条件下：

$$P_{heat} \approx \frac{3.8 \times 0.5}{0.92} \times 0.08 \approx 0.17W$$

只有线性充电方案的约 1/3 发热量。更重要的是，我们可以安全地把充电电流提高到 1A 甚至更高，大幅缩短充电时间，而发热仍然可控。

线性充电 vs DC-DC 充电直观对比：

线性充电（如 TP4057）DC-DC 充电（如 SLM6600）
充电效率约 $V_{batt}/V_{in}$（≈76%@3.8V/5V）≈92%
500mA 充电时发热功率~0.6W~0.17W
能否提高充电电流受限于散热，很难超过 500mA可以安全提高到 1A 或更高
外围元件极少（电阻电容各 1~2 个）需要电感和续流二极管（成本稍增）
PCB 面积小稍大（电感较占空间）

3.3V 稳压——为什么选择升降压芯片

对于锂电池供电的设备，如何提供稳定的 3.3V 工作电压也是一个令人纠结的设计决策。锂电池电压在 3.0V~4.2V 之间变化（满电 4.2V，放电终止约 3.0V）。要转换为 3.3V，有两种常见方案：

  • LDO（Low Dropout Regulator，低压差线性稳压器）：电路简单、成本低、输出纹波小，但效率低——锂电池电压高于 3.3V 时多余的能量全部转化为热量；当电池电压低于 3.3V + LDO 压差时，输出电压会跌落，无法维持 3.3V。
  • DC-DC 开关稳压器：效率高（通常 85%~95%），但电路较复杂，且存在开关纹波噪声。

老项目的 Tag 使用 XC6206P332 LDO 芯片做电压转换，因为老项目的 Tag 功能简单、功耗低，LDO 的低效率问题不太突出。

新项目使用 TI TPS63100 作为电压转换芯片。TPS63100 输入电压范围为 1.8V~5.5V，输出可配置（通过外部电阻分压设置，我们设为 3.3V），标称输出电流 1.5A。

TPS63100 效率曲线图

TPS63100 最重要的特点是支持无缝升降压（Buck-Boost）。锂电池在满电时电压为 4.2V（高于 3.3V，需要降压 / Buck），而在放电后期可能降至 3.0V 甚至更低（低于 3.3V，需要升压 / Boost）。TPS63100 能在整个电池工作电压范围内自动切换升降压模式，始终稳定输出 3.3V。

这一特性对 UWB 系统尤为重要：DW3000 芯片在发射 UWB 信号时的瞬间峰值电流较大（DW3000 IC 的峰值发射电流约 85mA@3.3V，加上天线匹配网络和 PCB 走线损耗，系统级峰值电流可达 100~140mA），如果此时电源电压因电池放电而下降甚至跌出 DW3000 的最低工作电压（约 2.8V），可能导致 DW3000 复位或发射功率异常。使用 TPS63100 可以确保即使电池电压降低，DW3000 仍然获得稳定的 3.3V 供电。

2.1.4 网络

网络联网方式上，我纠结了很久。之前的上行 TDOA 项目使用 Ethernet，顺便用 PoE 解决供电问题。Ethernet 的好处是：

  • 网络连接稳定可靠，不受无线干扰影响
  • 接上就能用，无需配网
  • 延迟低、带宽高

但 Ethernet 的缺点也很明显：

  • 网线的材料成本高（每根 Cat5e 网线从 PoE 交换机拉到 Anchor 安装位置，长度可能数十米）
  • 部署人工费很高（尤其是在天花板上布线走线槽，需要专业施工队伍）
  • 每个安装点都需要预留网口或线槽

而 WiFi 不使用网线，部署成本低得多——只需要保证 WiFi AP 的信号覆盖即可。但 WiFi 存在一个配网问题：在连接 AP 之前，需要为设备设置 WiFi SSID 和 Password。

现在很多 WiFi IoT 设备通过智能手机上的 APP（使用 WiFi SmartConfig 广播或蓝牙 BLE）来完成配网。但这意味着需要额外开发和维护一个手机 APP（iOS + Android），增加了系统复杂性。

为了减少系统的复杂性，我使用 USB 来配置网络。方案如下：

  1. 设备（Anchor 或 Tag）通过 USB Type-C 连接到 PC
  2. PC 端配置程序通过 USB CDC（虚拟串口）与设备通讯
  3. 在配置程序中输入 WiFi SSID 和 Password，点击"下发”
  4. 设备收到 WiFi 凭证后保存到 Flash（掉电不丢失），然后自动连接 WiFi

因为我们本来就需要一个 PC 端配置程序来对设备的各种参数进行配置（坐标、层级、信道参数等），让这个程序同时具备 USB 通讯能力来下发 WiFi 凭证，是最自然和优雅的方案——无需额外开发手机 APP，也无需在设备上实现 SoftAP 或蓝牙配网功能。

2.1.5 电量计

本项目中，部分 Anchor 和 Tag 使用锂电池供电，那么电量管理就是一个重要的需求——用户需要知道设备还能用多久、什么时候需要充电。

之前的老项目中，我使用电阻分压 + ADC 来检测电池电压，然后通过查表或简单的线性映射来估算电量百分比。但这种方法精度很差，原因是锂电池的放电曲线呈现高度非线性特征：

  • 头部（4.2V → 3.9V）：电压下降较快，但这只消耗了约 20% 的电量
  • 中间段（3.9V → 3.6V）：电压曲线非常平坦，对应约 60% 的电量——这意味着微小的电压测量误差会导致巨大的电量估算偏差
  • 尾部（3.6V → 3.0V）：电压急剧下降，对应剩余约 20% 的电量

为了得到更准确的电量估算，我使用了 CW2015 电量计芯片。CW2015 内置了锂电池放电模型（OCV-SOC 曲线表），通过电池电压查表来估算剩余电量百分比（SOC, State of Charge）。用户还可以通过厂家提供的工具，根据实际使用的电池型号定制放电曲线表，进一步提高精度。

CW2015 通过 I2C 接口与 MCU 通讯，在固件中只需定期读取其寄存器即可获得电量百分比和电池电压，使用非常方便。

CW2015 vs 库仑计： 库仑计（如 TI BQ27441）通过累积充放电电流来精确计算电量，精度最高（可达 ±1%），但需要串联一个检流电阻在电池和负载之间（增加了功耗和 PCB 面积），且价格较高。CW2015 只需要检测电池电压（不需要检流电阻），精度虽然不如库仑计（约 ±3%~5%），但对我们的应用场景已经足够了。

2.1.6 设备指示——远程识别 Anchor

在施工现场，Anchor 部署完成后，我们通过 PC 端配置程序可以看到有很多 Anchor 在线。但是，某一个 Anchor 真的是我们期望的那一个吗？ 安装在某个位置的 Anchor，我们期望它是 A（坐标配置为位置 A），但有可能实际上是 B，而 A 被错误地安装在了其他位置。

这种混淆听起来似乎不太可能，但在实际部署中非常常见——尤其是当几十个外观完全相同的 Anchor 同时安装时。我们知道，Anchor 的坐标信息是 TDOA 定位的基础——如果某些 Anchor 的位置与配置中的坐标对不上，整个系统的定位结果都会出错。

所以，我为每个 Anchor 增加了一个高亮 RGB LED（WS2812）。通过 PC 端配置程序可以远程控制这个 LED 的亮灭和颜色。部署现场的工作流程如下：

  1. 在配置程序中选中一个 Anchor（例如 “Anchor A3”）
  2. 点击"闪烁"按钮，软件通过 WiFi 发送指令给该 Anchor
  3. 该 Anchor 的 WS2812 LED 开始以特定颜色闪烁
  4. 现场施工人员抬头看看哪个设备在闪，就可以确认那个位置安装的就是 Anchor A3
  5. 如果发现位置不对，可以及时调整

实际工程经验小贴士： 别小看这个功能。实际部署中，一个场地可能有几十上百个 Anchor，外观完全一样。没有远程指示灯功能，每次排查问题都要爬梯子拆下来看序列号，非常痛苦。加了 WS2812 之后，效率提升了数倍。建议把这个功能作为"标配"加入到每一代产品中。

2.2 硬件设计

设计原则——统一引脚映射

为了固件开发的方便，首先保证 Anchor 和 Tag 的 UWB 芯片（DW3000）与 MCU（ESP32-S3）的连接使用相同的 GPIO 引脚映射。这意味着：

  • DW3000 的 SPI 接口（MOSI、MISO、SCLK、CS）在 Anchor 和 Tag 上连接到 ESP32-S3 的相同 GPIO
  • DW3000 的中断引脚（IRQ）和复位引脚（RESET）也使用相同的 GPIO
  • DW3000 的 WAKEUP 引脚使用相同的 GPIO

这样做的好处是：Anchor 和 Tag 的固件可以共用大量底层驱动代码（DW3000 驱动层、SPI 通讯层、中断处理等）。只有上层业务逻辑不同——Anchor 负责时钟同步和 TimeSync 广播，Tag 负责锁定 Anchor 和坐标计算。

快速原型验证

实际上，在项目开始时，我直接使用一块 ESP32-S3 DevKit 开发板 和 DWM3000 模块（Qorvo 官方的 DW3000 评估模块）通过杜邦线连接，组成一个最基础版本的硬件。根据需要刷写 Anchor 或 Tag 的固件，都可以正常工作。

ESP32S3-DevKit + DWM3000 模块

这种方式的好处是可以在不画 PCB 的情况下快速验证固件逻辑。等固件基本跑通后，再开始正式的硬件设计。我强烈建议在硬件设计之前先用开发板验证核心功能——否则如果画好 PCB 后才发现固件有问题，改板的时间和金钱成本都很高。

PCB 设计要点

在正式的硬件设计上，有以下几点需要注意：

  • 天线净空区（Keep-Out Zone）：UWB 芯片天线区域和 ESP32-S3 的 WiFi/BT 天线区域必须保持净空。所谓净空，是指天线周围一定范围内不能有铜箔走线、元器件或地平面（除了天线本身需要的 GND 参考平面以外）。如果其他铜箔侵入天线净空区，会改变天线的阻抗特性和辐射方向图，导致通讯距离缩短、信号质量下降。

    DW3000 的 UWB 天线和 ESP32-S3 的 WiFi 天线应尽量放在 PCB 的不同边缘或不同侧面，以减少互相干扰。

  • 电源去耦：充电芯片和 DC-DC 芯片的输入电容、输出电容要尽量靠近电源引脚放置（缩短高频电流环路面积）。DW3000 的电源引脚也需要就近放置去耦电容（推荐 100nF 陶瓷电容 + 10μF 钽电容的组合）。

  • SPI 走线：ESP32-S3 与 DW3000 之间的 SPI 总线走线应尽量短且等长，避免长距离平行走线引入串扰。DW3000 支持最高 38.4MHz 的 SPI 时钟，走线质量对信号完整性有一定要求。

  • 控制按钮：增加一个物理按钮，作为额外的控制功能入口。例如：

    • 长按 5 秒：恢复出厂设置
    • 短按：触发一次 TWR 测距（用于调试和校准）
    • 双击：切换工作模式

  • 固件烧录接口：引出 ESP32-S3 的 EN / IO0 / U0Rx / U0Tx / GND 等 5 个引脚（排针或焊盘），用于连接外部 USB-to-UART 模组烧写固件。

  • 扩展接口：I2C 总线留几个排针或 JST 连接器，方便外接 OLED 显示模块（用于在设备上显示状态信息）、IMU 传感器（加速度计/陀螺仪，用于辅助定位）等。

  • 预留 GPIO 焊盘：初始版本可以把未使用的 ESP32-S3 GPIO 引脚留出焊盘，方便后续增加功能或调试使用。这在原型阶段非常有用——你永远不知道什么时候会需要一个额外的 GPIO。

固件烧录——Auto-ISP 方案

ESP32-S3 DevKit 集成了一个 USB-to-UART 桥接芯片（如 CP2102 或 CH340），可以直接通过 USB 刷写固件。但对于量产 PCB 来说，没必要在每块板子上都放这颗桥接芯片——它会增加约 ¥1~3 的 BOM 成本和 PCB 面积。

我们的方案是使用一个单独的 USB-to-UART 模组（如 FTDI FT232RL 模组），通过排线连接到 PCB 上引出的 U0Rx、U0Tx、EN、IO0、GND 引脚来刷写固件。

为了方便固件刷写，我对 USB-to-UART 模组进行了一点改造——增加 2 个 NPN 三极管和 2 个电阻，实现**自动进入下载模式（Auto-ISP）**的功能。这样就不需要每次刷写固件时手动按住按钮操作了。

Auto-ISP 原理：

ESP32-S3 进入下载模式（Boot Mode）需要满足以下条件：在 EN 引脚释放（上升沿/芯片复位完成）时，IO0 引脚保持低电平。

通过两个 NPN 三极管分别控制 EN 和 IO0 引脚，由 USB-to-UART 模组的 DTR 和 RTS 控制线驱动。烧录工具（如 esptool.py）在开始烧录前会自动操作 DTR 和 RTS，完成以下时序：

  1. 拉低 IO0（通过 RTS → NPN → IO0）
  2. 脉冲拉低 EN 使芯片复位（通过 DTR → NPN → EN）
  3. EN 释放后芯片复位启动，检测到 IO0 为低电平，进入下载模式
  4. 释放 IO0

整个过程全自动完成，无需人工干预。

Auto-ISP 电路原理

改造后的 USB-to-UART 模组

Anchor/Tag 硬件框图

下面用框图总结 Anchor 和 Tag 的硬件组成：

graph TD
	subgraph "Anchor 硬件框图"
		MCU_A["ESP32-S3<br/>(MCU + WiFi + USB)"]
		UWB_A["DW3000<br/>(UWB 收发)"]
		PWR_A["供电电路<br/>(USB 5V / DC 12V → 3.3V)"]
		LED_A["WS2812<br/>(状态指示 LED)"]
		BTN_A["按钮"]
		ANT_W_A["WiFi 天线"]
		ANT_U_A["UWB 天线"]

		PWR_A --> MCU_A
		PWR_A --> UWB_A
		MCU_A -- "SPI" --> UWB_A
		MCU_A --> LED_A
		MCU_A --> BTN_A
		MCU_A -.- ANT_W_A
		UWB_A -.- ANT_U_A
	end

graph TD
	subgraph "Tag 硬件框图"
		MCU_T["ESP32-S3<br/>(MCU + WiFi + USB)"]
		UWB_T["DW3000<br/>(UWB 收发)"]
		BAT["锂电池"]
		CHG["SLM6600<br/>(DC-DC 充电)"]
		DCDC["TPS63100<br/>(升降压 3.3V)"]
		GAUGE["CW2015<br/>(电量计)"]
		LED_T["WS2812"]
		BTN_T["按钮"]
		ANT_W_T["WiFi 天线"]
		ANT_U_T["UWB 天线"]
		USB_T["USB Type-C"]

		USB_T --> CHG
		CHG --> BAT
		BAT --> DCDC
		BAT --> GAUGE
		DCDC --> MCU_T
		DCDC --> UWB_T
		MCU_T -- "SPI" --> UWB_T
		MCU_T -- "I2C" --> GAUGE
		MCU_T --> LED_T
		MCU_T --> BTN_T
		MCU_T -.- ANT_W_T
		UWB_T -.- ANT_U_T
	end

PCB 设计与外壳

选择一个合适的外壳（如标准 ABS 塑料盒），根据外壳的内部尺寸和安装柱位置来布局 PCB。需要注意：

  • PCB 尺寸需要与外壳内腔匹配
  • 天线区域（UWB 和 WiFi）不能被金属外壳遮挡——如果使用金属外壳，天线需要通过开窗或外置天线来解决。推荐使用塑料外壳
  • USB 接口、按钮、LED 指示灯的位置需要与外壳开孔对齐
  • 如果有电池，需要预留电池仓位置

Anchor PCB 2D 布局 Anchor PCB 3D 渲染

3. 软件设计

软件分为几类：Anchor 固件、Tag 固件、PC 端配置程序、数据汇聚程序、前端展示等。本章将重点讨论 Anchor 和 Tag 的固件设计，这是整个下行 TDOA 系统中最核心、技术难度最高的部分。配置程序和前端展示属于常规的应用层开发，不涉及 UWB 底层技术，本文不做详细讨论。

3.1 系统的网络架构

Anchor 和 Tag 是否必须联网？

Anchor 和 Tag 的联网不是必须的。对于纯定位功能来说，Anchor 的作用只有两个：

  • 充当时钟同步链条中的一个节点——与上级 Anchor 同步，维持上级 Anchor 的全局时间，同时向下级 Anchor 和 Tag 发出时钟同步包
  • 向 Tag 发出定位数据包（实际上就是时钟同步包本身，后面 §3.2.3 会详细解释为什么它们是同一种包）

Tag 也只需要接收到足够多的 Anchor 发出的时钟同步包，就可以自己计算自己的坐标，根本不需要联网。

但是，作为一个实际的嵌入式系统产品，它不可能是一个信息孤岛。定位系统的价值在于为其他应用系统提供位置服务——如果不联网，其应用价值就大大降低了。联网还带来以下好处：

  • 远程配置管理：通过 WiFi 远程修改 Anchor 参数（如坐标、同步层级），无需到设备跟前用 USB 线连接
  • 状态监控：实时监控每个 Anchor 的同步状态、信号质量、电池电量等，发现问题及时报警
  • OTA 固件升级：通过网络远程升级 Anchor/Tag 的固件，无需逐个拆下来刷写
  • 数据上报：Tag 将坐标数据通过 WiFi 上传给应用服务器

回顾：上行 TDOA 的网络架构

在介绍下行 TDOA 定位系统的网络架构之前，先简要回顾老的上行 TDOA 定位系统的架构，以便对比：

上行 TDOA 系统中，Tag 不联网（它只需要定期发送 UWB 信号），Anchor 使用 Ethernet 连接到本地局域网。从业务角度看，主要有两条数据链路：

  • Anchor 配置链路：Anchor 作为 TCP 服务器，接受来自 AnchorConfig 配置程序的 TCP 连接，实现参数配置功能。Anchor 与配置程序之间通过 UDP 广播包实现 Anchor 的自动发现——配置程序启动后自动扫描局域网内的所有 Anchor，无需手动输入 IP 地址。
  • Anchor → RTLE 数据链路：上行 TDOA 的坐标计算由专门的服务器软件 RTLE（Real-Time Location Engine，实时定位引擎）来完成。RTLE 作为 TCP 服务器端，接受来自 Anchor 的连接。RTLE 和 Anchor 之间也通过 UDP 广播实现 RTLE 的自动发现。

RTLE 计算好坐标后，向应用系统提供多种接口方式（TCP/WebSocket/UART 等），将坐标数据推送给应用系统。

下行 TDOA 的网络架构

对本项目，我们基本使用类似的网络架构模式。核心区别是：我们不再需要 RTLE 定位引擎（因为坐标计算由 Tag 自己完成），但需要一个数据汇聚程序（Data Aggregation Server），把来自各个 Tag 的坐标数据汇聚后统一发送给应用系统。

下面这幅图展示了整个系统的网络拓扑：根时钟源（Level 0）如何将时间传递给下级 Anchor（Level 1、Level 2……），**观察者（Observer）**如何提供反馈来提高同步精度，以及 Tag 如何独立计算坐标并选择性地联网上传数据。

graph TD
	subgraph "定位 Anchor (时钟同步链)"
		A0["Anchor A0 (Level 0, 根时钟源)"] -- "ClockSync" --> A1["Anchor A1 (Level 1)"]
		A1 -- "ClockSync" --> A2["Anchor A2 (Level 2)"]
		A2 -- "ClockSync" --> A3["Anchor A3 (Level 3)"]
	end

	subgraph "观察者反馈机制"
		OBS1["Observer 1"] -. "Feedback" .-> A1
		A0 -- "ClockSync" --> OBS1
		A1 -- "ClockSync" --> OBS1

		OBS2["Observer 2"] -. "Feedback" .-> A2
		A1 -- "ClockSync" --> OBS2
		A2 -- "ClockSync" --> OBS2

		OBS3["Observer 3"] -. "Feedback" .-> A3
		A2 -- "ClockSync" --> OBS3
		A3 -- "ClockSync" --> OBS3
	end

	subgraph "定位 Tag"
		T0["Tag T0: 自己计算坐标"]
		A0 -- "ClockSync" --> T0
		A1 -- "ClockSync" --> T0
		A2 -- "ClockSync" --> T0
		A3 -- "ClockSync" --> T0
	end

	T0 -- "WiFi" --> Server["数据汇聚服务器"]
	Server -- "WebSocket" --> UI["前端地图"]

图解：

  • Anchor A0 (Level 0)：整个系统的根时钟源，它的本地时间就是全局时间的参考基准。
  • 实线向下箭头（ClockSync）：表示层级间的时钟同步包传递。每个 Anchor 定期广播时钟同步包，下级 Anchor 接收后维持与上级一致的全局时间。
  • Observer（观察者）：这是下行 TDOA 系统中提升同步精度的关键机制。每个观察者同时接收一对"父子” Anchor 的时钟同步包，计算它们之间的全局时间差异，并将差异作为反馈发回给子 Anchor，帮助子 Anchor 修正同步误差。
  • 虚线（Feedback）：表示观察者发给目标 Anchor 的误差反馈包（单播）。
  • Tag (T0)：独立接收所有可见 Anchor 的 ClockSync 包，通过"锁定"多个 Anchor 来计算自己的坐标，并可选地通过 WiFi 将坐标上传给数据汇聚服务器。

为什么需要观察者（Observer）？

简单来说，时钟同步仅靠"父 → 子"的单向传递，精度存在天花板——天线延迟校准误差、Anchor 间距离测量误差等系统性偏差无法通过统计滤波消除，而且误差会在多级级联中逐步累积。

观察者提供了一个独立的第三方视角——它同时监听父和子的信号，计算出子相对于父的时间偏差，然后告诉子去修正。这类似于工业控制中的闭环反馈控制（Closed-Loop Feedback Control）。没有观察者的同步是"开环"的（子只能被动接受，无法知道自己偏了多少），有了观察者就变成"闭环"的（子能知道自己偏了多少并主动修正）。后面 §3.2.2.4 会详细解释。

Observer 的硬件是什么？ 观察者并不是一种新的硬件设备——它就是一个普通的 Anchor，只是在配置上被赋予了"观察者"的角色。也就是说，一个 Anchor 可以同时充当时钟同步链上的节点和另一对父子 Anchor 的观察者，只要它能同时收到那对父子 Anchor 的信号即可。

3.2 固件设计

Anchor 固件和 Tag 固件有很大的相似性。主要的器件是 ESP32-S3 和 DW3000，两个设备的 UWB 外设使用相同的 SPI 引脚映射（这是我们在硬件设计阶段刻意保证的），所以两个设备的固件可以共用大量底层驱动代码。

MCU 与 DW3000 的通信方式：

ESP32-S3 通过 SPI 总线 与 DW3000 通信。DW3000 的所有寄存器读写、数据包收发、配置修改都是通过 SPI 完成的。ESP32-S3 充当 SPI Master，DW3000 充当 SPI Slave。SPI 时钟频率通常设置为 16~20MHz（DW3000 支持的最大 SPI 时钟频率为 38.4MHz，但实际中由于 PCB 走线质量和信号完整性的限制，通常不会开到最大）。

此外，DW3000 的 IRQ 引脚连接到 ESP32-S3 的一个 GPIO，用于中断通知——当 DW3000 内部发生需要 MCU 关注的事件时（如数据包接收完成），它会拉高 IRQ 引脚触发 ESP32-S3 的外部中断。

很违反直觉的是，Anchor 的固件几乎是 Tag 固件的子集（或者说简化版）。原因是：Anchor 固件拥有的功能，Tag 固件也都有；而 Tag 还有一些特有的功能（如坐标计算、Anchor 锁定管理、多 Anchor 时钟同步维护等），Anchor 则没有。这在软件设计上意味着：如果你先把 Tag 固件做好，Anchor 固件只需要裁剪掉 Tag 特有的功能即可。

在我的设想中，Tag 将来会拥有很多功能和外设。作为初版，先把最基础的功能——UWB 定位——做好，其他附加功能以后再逐步加上。计划中的附加功能包括：

  • IMU（惯性测量单元）：增加磁力计、加速度计、陀螺仪。可以在 UWB 信号丢失时辅助惯性导航（INS/UWB 融合定位），提高定位的连续性和精度；还可以检测 Tag 静止状态并自动进入休眠模式以节省电量。
  • 振动马达和蜂鸣器：作为警报和提醒功能（例如进入危险区域时振动告警、偏离指定路线时蜂鸣提示）。
  • 显示屏：可以是 OLED、TFT、E-Ink 等，用于显示文字消息（如 SMS）、简易地图、坐标数值等。
  • 麦克风和喇叭：可以实现控制中心与 Tag 佩戴者之间的语音通讯。

以下的固件设计部分不再严格区分 Anchor 和 Tag，统一讨论。在涉及两者差异的地方会特别指出。

3.2.1 MCU 与 UWB 芯片交互

轮询 vs 中断

DW3000 支持两种与 MCU 交互的操作模式：中断（Interrupt）和轮询（Polling）。

当 DW3000 芯片内部发生特定事件（如成功接收数据包、发送完成、接收超时、接收错误等）时，可以通过 IRQ 引脚触发硬件中断，MCU 在中断服务程序（ISR）中对芯片进行读写。也可以不使用中断，而是在程序主循环中周期性地读取芯片的状态寄存器，检查是否有新事件发生。这两种方式各有利弊：

轮询方式（Polling）

轮询方式只需要在程序的主循环中周期性地检测 DW3000 的状态寄存器。如果发现收到了新数据包，就读取数据包内容和时间戳，然后继续轮询。

  • ✅ 程序结构简单：所有流程都是顺序处理的，不会被中断打断，不需要设置临界区/信号量/互斥锁来保护共享资源。代码调试也更容易。
  • ❌ 实时性差、容易丢包：新数据包到达后，如果不及时读取，会妨碍后续数据包的接收——DW3000 的接收缓冲区只有一个包的容量，新包会覆盖旧包。主循环中的其他操作（如 WiFi 通讯、传感器读取）都会延迟对 UWB 事件的响应。

中断方式（Interrupt）

中断方式下，MCU 平时正常运行自己的任务。当 DW3000 有新事件发生时（如新数据包到达），DW3000 通过 IRQ 引脚触发硬件中断，MCU 立即跳转到中断服务程序读取事件信息。

  • ✅ 实时性好：数据包到达后立即触发中断，中断程序可以即刻读取数据内容和时间戳，极大地减少了数据包丢失的概率。
  • ❌ 程序结构复杂：中断会打断程序的正常执行流程。ISR 中需要访问的资源有可能正在被主任务使用，因此必须设置**临界区（Critical Section）或信号量（Semaphore）**来防止访问冲突（Race Condition）。

我以前设计的上行 TDOA 系统使用轮询方式。那时 Anchor 只需要接收 Tag 发来的定位信号，数据包到达频率较低，轮询方式的延迟是可以接受的。

这次下行 TDOA 改用中断方式。主要原因是：Tag 需要同时跟踪多个 Anchor（通常 48 个）的时钟同步包，每个 Anchor 每秒发送 1050 个同步包，Tag 每秒需要处理的数据包数量可达数十到上百个。如此高频率的数据包到达，轮询方式很难保证不丢包。

ESP32-S3 上的浮点运算陷阱

⚠️ 重要提醒——ISR 中的浮点运算限制：

ESP32-S3 带有硬件浮点运算单元（FPU），可以利用硬件加速执行 float（单精度）类型的运算。但是，float 类型的运算不能在 ISR 中执行！

原因是：ESP-IDF 的 FreeRTOS 移植版本在进入 ISR 时，不会保存和恢复 FPU 的寄存器上下文（协处理器上下文保存）。如果在 ISR 中使用 float 运算，FPU 寄存器的内容会被 ISR 修改，但退出 ISR 后不会恢复——这会破坏被中断打断的那个任务的 FPU 状态，导致不可预见的数值错误。更麻烦的是，这种错误往往是间歇性的、难以复现的，因为它取决于中断发生时主任务是否正好在做浮点运算。

有趣的是，double（双精度）类型的运算反而可以在 ISR 中安全执行。原因是 ESP32-S3 的 FPU 只支持单精度（float），double 运算是通过软件模拟实现的——软件模拟只使用通用寄存器（GPR），不涉及 FPU 寄存器，因此不存在上下文破坏的问题。

实际影响：在 rx_ok_cb 等 ISR 中，如果需要做时间戳的数学运算，请使用 uint64_t 整数或 double 类型，绝对不要使用 float。这个坑很隐蔽，建议在代码审查时特别关注。

FreeRTOS 任务架构

我们为 DW3000 创建一个单独的 FreeRTOS 任务（task_uwb_chip），专门处理与 UWB 芯片相关的所有操作。整个数据流如下图所示：

graph LR
	subgraph "ISR 上下文（中断）"
		IRQ["DW3000 IRQ 触发"] --> ISR["rx_ok_cb / tx_done_cb"]
		ISR --> Q["FreeRTOS 队列<br/>(xQueueSendFromISR)"]
	end

	subgraph "task_uwb_chip 任务上下文"
		Q --> DEQUEUE["从队列取出事件<br/>(xQueueReceive)"]
		DEQUEUE --> PARSE["解析数据包内容"]
		PARSE --> SYNC["更新时钟同步参数<br/>(Kalman 滤波器)"]
		SYNC --> CALC["坐标计算<br/>(仅 Tag)"]
	end

	subgraph "其他 FreeRTOS 任务"
		CALC --> WIFI["WiFi 任务：上报坐标"]
		PARSE --> CONFIG["配置任务：处理配置命令"]
	end

为什么把 UWB 操作集中在一个任务中？

把所有 DW3000 的 SPI 通讯集中在一个任务中，可以避免多个任务同时通过 SPI 访问 DW3000 而产生总线冲突。SPI 是一个共享资源，如果多个任务同时发起 SPI 事务，数据会互相干扰。虽然可以用互斥锁来保护 SPI 总线，但这会引入锁等待的延迟，影响时钟同步包的处理及时性。

我们的方案是：所有对 DW3000 的 SPI 读写都在 task_uwb_chip 的上下文中完成。ISR 中只做最必要的 SPI 操作（读取时间戳和数据包内容），然后将数据投递到队列，由 task_uwb_chip 在任务上下文中进行后续处理。

void task_uwb_chip(void* p_arg)
{
    // 初始化 DW3000: SPI配置, 芯片复位, 加载配置参数
    // 设置中断回调: rx_ok_cb, tx_done_cb, rx_error_cb, rx_timeout_cb
    // 打开接收机, 开始监听 UWB 信号
    // ...
    while (1) {
        // 1. 从队列中取出 UWB 事件并处理
        //    - RX_OK: 解析时钟同步包, 更新 Kalman 滤波器
        //    - TX_DONE: 发送完成, 重新打开接收机
        //    - RX_ERROR/TIMEOUT: 错误处理, 重新打开接收机

        // 2. 检查是否到了发送时钟同步包的时间 (Anchor 角色)
        //    如果是, 中止接收并启动延迟发射

        // 3. 处理排队发送的低优先级数据包 (如反馈包)
    }
}

接收成功的 ISR

为接收成功事件注册一个回调函数（本质上是 ISR 的一部分）：

static void rx_ok_cb(const dwt_cb_data_t* cb_data)
{
    DW_EVENT event;
    event.rx_timestamp = dw_get_rx_timestamp();    // 读取 40-bit 接收时间戳
    event.rx_length = cb_data->datalength;         // 数据包长度
    interface_read_rx_frame(cb_data->dw,
        event.frame_data, cb_data->datalength);    // 读取数据包内容
    event.off_hw = dwt_readclockoffset();          // 读取 CFO（载波频率偏移）
    putUwbEventFromISR(UWB_EVENT_RX_OK, &event);   // 放入 FreeRTOS 队列
    chip_start_rx();                                // 立即重新打开接收机
}

关键设计决策：ISR 中要做多少事？

在嵌入式开发中，通常的最佳实践是"ISR 中做尽量少的事，把复杂处理留给主任务”。但这里的 ISR 做了比较多的工作——读取时间戳、读取整个数据包内容、读取 CFO。这些操作都需要通过 SPI 与 DW3000 通信，耗时大约几十微秒（取决于数据包长度和 SPI 时钟频率）。

为什么不把这些操作延迟到主任务中？

原因是：DW3000 的接收缓冲区只能容纳一个数据包。 如果 ISR 中不立即读取当前数据包的内容，而是先重新打开接收机等待下一个包，那么当下一个数据包到达时，DW3000 会覆盖接收缓冲区中尚未读取的上一个包——数据就永远丢失了。

所以必须在 ISR 中完成"读取数据 → 投递到队列"的操作，然后才能安全地调用 chip_start_rx() 重新打开接收机。队列起到了缓冲的作用——即使主任务暂时来不及处理，数据包也安全地保存在队列中不会丢失。

当成功收到一个 UWB 数据包后，我们通过 putUwbEventFromISR（内部使用 FreeRTOS 的 xQueueSendFromISR）将事件投递到队列，然后立即再次打开接收机等待下一个数据包。

发送完成的 ISR

static void tx_done_cb(const dwt_cb_data_t* cb_data)
{
    putUwbEventFromISR(UWB_EVENT_TX_DONE, NULL);
}

发送完成的 ISR 非常简单——只需要通知主任务"发送已完成"即可。主任务收到这个事件后，会重新打开接收机恢复正常的监听状态。

主循环中的事件处理

在 task_uwb_chip 的主循环中，我们从队列中取出 UWB 事件进行处理（如解析时钟同步包、更新 Kalman 滤波器状态、触发坐标计算等）。

需要发送的 UWB 数据包，我们分为两种优先级：

  • 立即发送型（高优先级）：对发送时间有严格要求的数据包。例如时钟同步包——一旦到了预定的发送时刻，就必须立即发送，延迟会直接影响同步精度。
  • 排队发送型（低优先级）：对发送时间要求不严格的数据包，可以在主循环有空闲时再发送。例如反馈包、配置应答包等。

对于高优先级数据包，我们在主循环的每次迭代中检查：如果到了发送时间，立即中止接收并开始发送。

延迟发射（Delayed TX）机制

下面详细讲解 Anchor 定期发送时钟同步包的核心流程。这里用到了 DW3000 的一个重要特性——延迟发射（Delayed Transmit）。

为什么需要延迟发射？

时钟同步包中最重要的信息是"这个包精确地在什么时刻离开了天线”。如果使用"立即发射"（Immediate TX），会遇到以下问题：

  1. MCU 通过 SPI 写入数据包内容和发射指令
  2. DW3000 在收到指令后的某个不确定时刻发射数据包
  3. 数据包离开天线的实际时刻受到 SPI 通信延迟、MCU 处理时间、DW3000 内部处理延迟等多种不可预知因素的影响
  4. 虽然可以在发射完成后从 DW3000 的寄存器中读回实际发射时间戳，但此时数据包已经发出去了——里面携带的时间戳字段已经无法修改

这意味着什么？ 如果使用立即发射，数据包 payload 中携带的时间戳只能是事后从寄存器读出并在下一个包中补发的，接收方需要额外的逻辑来匹配时间戳和数据包，系统复杂度大增。

延迟发射的巧妙之处在于：

  1. 我们预先告诉 DW3000：“在未来某个精确的时刻发射这个数据包”
  2. 因为发射时刻是预设的，我们可以在发射之前就精确计算出该时刻对应的全局时间戳
  3. 把计算好的全局时间戳写入数据包的 payload
  4. DW3000 的硬件定时器会在精确的时刻自动触发发射

这样，数据包中携带的时间戳与实际发射时刻就是完全一致的——不存在任何"事后补发"的问题。

延迟发射的时序
sequenceDiagram
	participant MCU as ESP32-S3 (MCU)
	participant DW as DW3000

	MCU->>DW: dwt_forcetrxoff() 强制停止接收
	MCU->>DW: dwt_readsystimestamphi32() 读取当前DW3000时间
	Note over MCU: 计算: tx_time32 = 当前时间 + 1.3ms
	MCU->>DW: dwt_setdelayedtrxtime(tx_time32) 设置延迟发射时间
	Note over MCU: 根据 tx_time32 精确计算 40-bit 发射时刻<br/>+ TX天线延迟 → 信号离开天线的精确全局时间<br/>写入时钟同步包的 payload
	MCU->>DW: dwt_writetxdata() 写入数据包内容
	MCU->>DW: dwt_starttx(DWT_START_TX_DELAYED) 启动延迟发射
	Note over DW: DW3000 内部计时器<br/>到达 tx_time32 时自动发射
	DW-->>MCU: tx_done_cb() 发射完成中断
延迟发射的代码实现
if (TimeIsOver(last_clock_sync_time, deviceConfig.clock_sync_interval)) {
    last_clock_sync_time = getSystemTimeMS();
    BROADCAST_DL_CLOCK_SYNC_MESSAGE dlClockSyncMessage;

    dwt_forcetrxoff();  // 强制停止接收

    /* Step 1: 读取当前 DW3000 时间（仅高 32 位）
     * DW3000 的系统时间是 40-bit 的，但延迟发射寄存器只接受高 32 位（低 8 位被忽略） */
    uint32_t tx_time32 = dwt_readsystimestamphi32();

    /* Step 2: 在当前时间上加入 ~1.3ms 的延迟
     * 这个延迟必须足够长，确保 MCU 有时间完成后续的数据包准备工作
     * （计算全局时间戳、组装 payload、写入 DW3000 发射缓冲区）
     * UUS_TO_DWT_TIME 是微秒到 DW3000 tick 的转换常数
     * 右移 8 位是因为 tx_time32 对应 40-bit 时间的高 32 位 */
    tx_time32 += ((1300 * UUS_TO_DWT_TIME) >> 8);
    tx_time32 &= 0xFFFFFFFE;   /* 确保最低位为 0
                                 * DW3000 延迟发射寄存器的低 9 位（对应 40-bit 时间的低 1 位）
                                 * 不参与比较，置 0 可以避免潜在的对齐问题 */

    /* Step 3: 设置延迟发射时间 */
    dwt_setdelayedtrxtime(tx_time32);

    /* Step 4: 精确计算信号实际离开天线的 40-bit 本地时间
     * 延迟发射寄存器只有高 32 位，需要左移 8 位恢复为 40-bit
     * 再加上 TX 天线延迟（信号从芯片发射引脚到离开天线的传播延迟）
     * TX 天线延迟是在出厂校准时测定的，存储在设备的永久配置中 */
    uint64_t tx_local_40 = ((uint64_t)tx_time32 << 8)
                           + permanent_data.tx_antenna_delay;
    tx_local_40 &= UWB_MASK_40BIT;  // 确保不超过 40 位（处理溢出）

    /* Step 5: 将本地时间转换为全局时间 */
    uint64_t tx_global;
    if (s_sync.is_root) {
        /* 根 Anchor（Level 0）: 本地时间即全局时间 */
        tx_global = tx_local_40;
    }
    else {
        /* 子 Anchor: 需要把本地时间转换为全局时间
         * sync_extend_timestamp: 将 40-bit 扩展为 64-bit（处理计时器溢出）
         * sync_local_to_global: 使用 Kalman 滤波器维护的 offset 和 drift 参数
         *                       进行线性变换: global = local * (1+drift) + offset */
        uint64_t tx_local_full = sync_extend_timestamp(&s_sync, tx_local_40);
        uint64_t tx_global_full = sync_local_to_global(&s_sync, tx_local_full);
        tx_global = tx_global_full & UWB_MASK_40BIT;
    }

    /* Step 6: 生成同步包，将全局时间戳写入 payload */
    GenerateUWBMessage_BROADCAST_DL_CLOCK_SYNC_MESSAGE(
        &dlClockSyncMessage, tx_global);

    /* Step 7: 写入 DW3000 发射缓冲区并启动延迟发射 */
    dwt_writetxdata(sizeof(dlClockSyncMessage),
                    (uint8_t*)&dlClockSyncMessage, 0);
    dwt_writetxfctrl(sizeof(dlClockSyncMessage), 0, 1);

    int err = dwt_starttx(DWT_START_TX_DELAYED);
    if (err == DWT_SUCCESS) {
        chip_state = CHIP_STATE_TX;
    }
    else {
        /* 延迟发射失败: 通常是因为设定的发射时间已经过去了
         * 放弃本次发送, 重新打开接收机, 等待下一个发送周期 */
        chip_state = CHIP_STATE_IDLE;
    }
}

延迟发射失败的处理：

dwt_starttx(DWT_START_TX_DELAYED) 返回错误码 DWT_ERROR 通常意味着我们设定的发射时间点已经过去了——即 MCU 在准备数据包的过程中花费了超过 1.3ms 的时间。这在正常运行中极少发生，但以下情况可能导致超时：

  • MCU 被高优先级的 WiFi 中断长时间抢占
  • SPI 总线发生异常（如 DW3000 在 INIT/IDLE 状态时 SPI 读写速度变慢）
  • 同步参数计算（特别是 sync_local_to_global）耗时过长

遇到这种情况，我们放弃本次发送，等待下一个同步周期重试。代码中必须处理这个边界情况，否则 DW3000 会停留在错误状态。

关于 40-bit 时间戳的扩展：

DW3000 的计时器是 40 位宽的，每 ~17.2 秒溢出一次。在做时间差计算时，如果两个时间戳分处溢出边界的两侧，简单的减法会得到错误的结果。sync_extend_timestamp 函数的作用是将 40-bit 时间戳扩展到 64-bit——它根据已知的时间上下文（如上次接收的时间戳）判断是否发生了溢出，如果发生了就加上 $2^{40}$ 进行补偿。扩展到 64 位后，时间戳的有效范围变为约 $2^{64} \times 15.65\text{ps} \approx 9.1 \text{年}$，在任何实际应用中都不会再次溢出。

3.2.2 时钟同步

在 Part 1 中，我们已经解释了时钟同步的重要性——它是 TDOA 定位的基石。下行 TDOA 的 Tag 需要"锁定"附近的 Anchor 才能计算坐标。所谓"锁定"，本质上就是 Tag 与这些 Anchor 进行时钟同步——在 Tag 内部为每个锁定的 Anchor 维护一套同步参数（offset 和 drift），随时可以将 Tag 的本地时间转换为该 Anchor 的"全局时间"。

因为多种原因（晶振制造误差、温度变化、电压波动等），UWB 设备的振荡频率会有差异，导致各设备计时器的"走速"不同。通常，我们指定一个 Anchor 作为时钟源（Root Clock Source），其他 Anchor 和 Tag 与时钟源进行时钟同步，维持与时钟源一致的全局时间。

3.2.2.1 简易版时钟同步

在之前的上行 TDOA 项目中我使用的时钟同步方法，我称之为"简易版时钟同步"。原理确实很简单，实现也简单，但已经能在理想环境下工作得不错。我们先讲解简易版，理解了它的局限性之后，再介绍高级版。

算法推导

假设要把 Anchor A 的时间同步给 Anchor B。换句话说，Anchor A 是时钟源，Anchor B 是需要同步的设备——Anchor B 需要在内部维持一个与 Anchor A 一致的全局时间，并提供 Anchor B 本地时间与 Anchor A 全局时间之间的双向转换。

Anchor A 定期发送时钟同步数据包，包中携带发送时的全局时间戳。Anchor B 接收这些同步包，同时记录自己收到每个包时的本地时间戳。

我们观察连续的 3 个数据包。假设 Anchor A 的发送时间戳（全局时间）分别是 $TC_1$、$TC_2$、$TC$，Anchor B 对应的接收时间戳（本地时间）分别是 $TA_1$、$TA_2$、$TA$。

简易版时钟同步

如果这两个 Anchor 的时钟在短期内都是稳定且均匀的（即各自的频率恒定，只是两者的频率不完全相同），那么 Anchor A 的时间间隔与 Anchor B 的时间间隔之间存在恒定的比例关系：

$$\frac{TC_2 - TC_1}{TA_2 - TA_1} = \frac{TC - TC_2}{TA - TA_2}$$

直觉理解： 这个等式的含义是——两把"尺子"的刻度不同（因为走速不同），但刻度的比例是固定的。无论你量多长的"时间段"，两者之间的比例关系始终不变（在短期内）。

上面的等式变换一下，可以得到任意本地时刻 $TA$ 对应的全局时间 $TC$：

$$TC = \frac{TC_2 - TC_1}{TA_2 - TA_1} \times (TA - TA_2) + TC_2$$

如果令 $k = \frac{TC_2 - TC_1}{TA_2 - TA_1}$，则：

$$TC = k \times (TA - TA_2) + TC_2$$

这是典型的直线方程 $y = kx + b$。$k$ 是直线的斜率，代表两个时钟之间的频率比。在正常情况下，$k$ 的值应该非常接近 $1.0$——因为两个 Anchor 的晶振频率差异通常只有几个 ppm（百万分之几）。

为了方便表达和提高数值计算精度，通常我们会定义一个 factor：

$$factor = k - 1.0 = \frac{TC_2 - TC_1}{TA_2 - TA_1} - 1.0$$

这样 $factor$ 是一个接近于零的小数，避免了在计算中因为 $k \approx 1.0$ 而损失有效数字精度。

factor 的物理含义：

  • $factor > 0$：说明 Anchor A（时钟源）的时间间隔比 Anchor B 大，即 Anchor B 的时钟走得慢于 Anchor A。从 B 的视角来看，A 的时间"跑得更快"。
  • $factor < 0$：说明 Anchor B 的时钟走得快于 Anchor A。
  • $factor$ 的典型值在 $\pm 20 \times 10^{-6}$（即 ±20ppm）范围内，对应普通石英晶振的频率偏差范围。
  • 如果使用 TCXO（温补晶振），$factor$ 可以降到 $\pm 2 \times 10^{-6}$ 甚至更低。
时钟漂移的直观图解

为了让大家更直观地理解时钟漂移（Clock Drift）的概念，请看下面的示意图：

xychart-beta
	title "时钟漂移示意图 (Clock Drift)"
	x-axis "全局时间 (Global Time)" [0, 10, 20, 30, 40, 50]
	y-axis "本地计数器 (Local Timer)" 0 --> 60
	line [0, 10, 20, 30, 40, 50]
	line [0, 12, 24, 36, 48, 60]

图解：

  • 横轴（Global Time）：绝对准确的全局时间（即时钟源 A0 的时间）。
  • 纵轴（Local Timer）：两个设备的本地计数器值。
  • 蓝线（斜率 = 1.0）：代表时钟源 A0，它的本地时间与全局时间完全一致（$factor = 0$）。
  • 橙线（斜率 > 1.0）：代表 Anchor A1，它的晶振频率比 A0 快约 20%（图中为了演示效果做了夸张），所以计数器值增长更快（$factor < 0$，即 A1 走得快）。

两条线的斜率不同，表明它们的走速不同。factor 本质上就是这两条线斜率的比值减去 1。Tag 需要为它锁定的每个 Anchor 实时维护 $factor$ 值，才能在任意时刻将自己的本地时间准确转换为该 Anchor 的全局时间。

实际中的 factor 变化幅度很小：20ppm 的 factor 意味着两条线的斜率差异只有 $0.00002$——在图上几乎看不出来。但以 DW3000 的计时器精度（15.65ps/tick），经过 1 秒（约 $6.4 \times 10^{10}$ ticks），20ppm 的偏差累积为 $\approx 1.28 \times 10^{6}$ ticks，对应约 20μs，即 6000 米的距离误差。这就是为什么必须持续进行时钟同步。

转换公式

将 $factor$ 的定义代回原式，得到完整的本地时间 → 全局时间转换公式：

$$TC = (1.0 + factor) \times (TA - TA_2) + TC_2$$

各变量含义如下：

变量含义
$factor$时钟差异因子（频率偏差比），接近于 0 的小数
$TA_2$最近一次收到时钟同步包时，Anchor B（接收方）的本地接收时间戳
$TC_2$最近一次收到的同步包中携带的 Anchor A（时钟源）全局发送时间戳
$TA$Anchor B 当前的本地时间（即我们想要转换的时间点）
$TC$$TA$ 对应的 Anchor A 全局时间（转换结果）

数值精度提示： 在实际实现中，$factor$ 是一个非常小的浮点数（$|factor| < 0.00002$），而 $(TA - TA_2)$ 可能是一个很大的整数（取决于距上次同步过了多久）。两者相乘时，如果用 float（单精度，约 7 位有效数字）可能会损失精度。建议使用 double（双精度，约 15 位有效数字）来进行这个计算。

改进：滤波处理

每次收到新的时钟同步包时，我们可以用当前的同步参数（$factor$、$TA_2$、$TC_2$）预测该同步包的发送全局时间戳，然后与包中实际携带的发送时间戳比较。两者之间的差异（称为预测残差或同步误差）反映了同步参数的精度——残差越小，说明我们的 $factor$ 越准确。

大部分情况下，预测值与实际值会有一定差异。这也是我们需要持续、定期进行时钟同步的原因——每当收到新的同步包，都要重新计算 $factor$，更新 $TA_2$ 和 $TC_2$。

但是，每次重新计算得到的 $factor$ 可能会因为接收时间戳的随机抖动（jitter）而产生波动。如果直接使用每次的原始计算值，时间转换结果也会跟着抖动。为了保持 $factor$ 的稳定性，我们可以使用滤波手段来平滑：

  • 滑动均值滤波（Moving Average）：保存最近 N 次计算的 $factor$ 值，取平均值作为当前 $factor$。简单有效，但对异常值（outlier）抵抗力弱。
  • 加权滑动均值：根据信号质量（如首径功率、CFO 偏差大小）给每次计算的 $factor$ 赋予不同的权重。
  • 卡尔曼滤波（Kalman Filter）：更高级的方法，能根据系统模型和测量噪声自适应地调整滤波强度。

工程经验： 在简单场景下（Anchor 间距小、无遮挡、环境干净），用最近 5~10 次同步包计算 $factor$ 的滑动平均值就足够了。但在干扰较大的环境中，需要更高级的方案——这就是下一节"高级版时钟同步"要解决的问题。

3.2.2.2 高级版时钟同步

前面的"简易版时钟同步"在办公室等理想环境下工作得不错，但在干扰较大的环境（如工厂车间、仓库等金属结构物较多的场所）会遇到问题——时间戳抖动大、同步精度不稳定。

当需要多级时钟同步时，问题尤为突出。DW3000 在 6.8Mbps 的通讯速率下，有效通讯距离大约在 30 米以内；在 850Kbps 的速率下，范围可扩大到 100~200 米。而实际定位区域通常比这大得多。在这种情况下，只能把全部 Anchor 按层级划分，通过级联方式进行时钟同步。

多级时钟同步

级联同步的核心风险——误差累积：

级联方式同步时，每一级都可能引入时间戳误差，而且这个误差会逐级传播并累积。例如：

  • Level 0 → Level 1 引入了 0.5ns 误差
  • Level 1 → Level 2 又引入了 0.5ns 误差
  • 到 Level 2 就累积了约 1.0ns 的误差（对应约 30cm 的定位偏差）

如果有更多层级，误差会继续叠加。这也是为什么我们在 Part 1 中提到：同步层级数量不宜超过 2~3 级。

在比较恶劣的环境下，接收时间戳并不一定准确。导致不准确的因素包括：

  • 多径效应：电波的反射/散射导致芯片锁定的不是第一径，而是延迟的多径信号
  • 遮挡（NLOS）：遮挡物削弱第一径信号，导致 LDE 算法提取到的到达时间偏晚
  • 信号饱和：信号太强导致 AGC 来不及调整、ADC 饱和，LDE 提取的时间偏早
  • 晶振短期漂变：温度急变导致晶振频率发生非线性变化
  • 电磁干扰（EMI）：附近设备产生的电磁噪声干扰 UWB 接收

接收时间戳的不准确，会导致 $factor$ 的计算值波动。简易版的滑动平均滤波虽然能平滑 $factor$，但仅仅平滑 $factor$ 是不够的！

“参考基准点"的抖动问题

虽然 $factor$（漂移率/斜率）被滤波平滑了，但转换公式中的参考基准点 $(TA_2, TC_2)$ 本身也受到接收时间戳抖动的影响。每次收到新的同步包时，$TA_2$ 会更新为当前包的接收时间戳。如果这个时间戳因为多径效应或噪声而偏差了 $\delta$，那么转换结果也会偏差 $\delta$——因为 $TA_2$ 是公式中的"锚点”，它的任何偏差都会被原样传递到输出结果中。

类比理解： 想象一条直线 $y = kx + b$。即使斜率 $k$ 被滤波得非常稳定（一阶导数稳定），但如果直线经过的那个固定参考点 $(x_0, y_0)$ 在不断抖动，那么整条直线也会跟着上下平移抖动——这就是 $TA_2$ 抖动对转换结果的影响。

以下介绍几种针对性的改进方案：

3.2.2.2.1 从"点对点转换"转向"线性回归（Linear Regression）“模型

简易版的方法使用最后一次收到的同步包作为唯一的参考原点。一旦这个包的 $TA_2$（接收时间戳）因为多径效应或硬件噪声偏了 300ps（对应约 9cm），整个计算结果就会立刻偏 300ps。

改进方法： 不使用"最后一次"收到的包作为唯一参考，而是维护一个滑动窗口（例如最近 10~20 个同步包），记录多组 $(TA_i, TC_i)$ 时间戳对。

  • 对这些数据点使用最小二乘线性回归（Ordinary Least Squares, OLS）进行直线拟合
  • 拟合出的直线方程为：$TC = a \times TA + b$
  • 斜率 $a$ 就是 $(1 + factor)$，截距 $b$ 则是综合了多次测量噪声后的最优时间偏移
  • 需要进行时间转换时，直接代入拟合出的直线方程即可

这种方法的优点：

  1. 抗单点噪声能力强：单次 $TA_i$ 的抖动会被其他正常采样点"拉回来”，不会导致结果剧烈跳变
  2. 同时平滑了 $factor$ 和参考基准点：与简易版只平滑 $factor$ 相比，线性回归对斜率和截距同时进行了最优估计
  3. 实现相对简单：最小二乘法只需要维护几个累加量（$\sum TA_i$, $\sum TC_i$, $\sum TA_i \cdot TC_i$, $\sum TA_i^2$, $N$），每次来新数据更新这些累加量即可

注意事项： 线性回归假设 $factor$ 在整个窗口期间是恒定的。如果窗口太长（例如超过数秒），晶振频率可能已经因为温度变化而发生了非线性漂变，线性回归的假设就不再成立。窗口大小需要根据实际环境调优——通常在 0.5~3 秒之间（如果每秒 20 个同步包，对应 10~60 个数据点）。

3.2.2.2.2 利用 DW3000 的频率偏移估计（CFO）

DW3000 在接收数据包时，内部的载波恢复环路会给出 Carrier Frequency Offset (CFO) 的估计值。

  • CFO 反映的是发送端和接收端晶振频率的差异——它本质上就是对 $factor$ 的一个独立物理测量
  • 做法：将 CFO 值转换为频率漂移率（ppm），并将其作为一个独立的观测值引入到滤波器中

为什么 CFO 很有价值？

CFO 是对射频载波频率的直接测量，其噪声来源（PLL 噪声、载波恢复环路的收敛精度等）与接收时间戳 $TA$ 的噪声来源（LDE 算法、多径效应、AGC 抖动等）是基本不相关的。

引入两个独立的、不相关的观测源来估计同一个物理量（$factor$），可以显著提高估计精度——这是传感器融合（Sensor Fusion）的基本原理。

CFO 读取方法： 在 DW3000 的驱动中，dwt_readclockoffset() 函数返回 CFO 值。在前面的 rx_ok_cb ISR 中，我们已经在每次接收成功时读取了这个值（event.off_hw = dwt_readclockoffset()）。需要注意的是，这个函数返回的是一个比例值（相对于芯片内部基准频率的偏移比），需要乘以适当的转换系数才能得到 ppm 单位的频率偏差。

3.2.2.2.3 使用卡尔曼滤波器（Kalman Filter）

如果简单的滑动均值或一阶滤波效果不理想（收敛慢、抗噪声差、跟踪不及时），可以使用卡尔曼滤波器来维护同步参数。卡尔曼滤波器是最优线性估计器，特别适合处理这种"有噪声的线性系统状态估计"问题。

状态模型设计：

  • 状态向量 $\mathbf{x} = [offset, drift]^T$

    • $offset$：当前时刻的时间偏移量（本地时间与全局时间的差值，单位：DW3000 tick）
    • $drift$：时钟漂移率（即 $factor$，无量纲，单位：tick/tick）

  • 状态转移方程（预测步）：

    $$offset_{k+1} = offset_k + drift_k \times \Delta t$$

    $$drift_{k+1} = drift_k$$

    含义：在没有新的观测数据时，$offset$ 按照已知的 $drift$ 线性增长，而 $drift$ 本身假设短期内不变（随机游走模型）。

  • 观测方程（更新步）： 利用新到的同步包计算出的瞬时 offset 作为观测值 $z_k$，更新状态估计。如果同时使用 CFO，可以构造第二个观测方程，将 CFO 转换后的 drift 值作为另一个观测值。

  • 核心技巧：在需要进行"本地时间 → 全局时间"转换时，直接使用卡尔曼滤波器维护的状态量 $offset$ 和 $drift$：

    $$TC = TA + offset + drift \times (TA - TA_{ref})$$

    而不是使用某一个具体的 $TA_2$ / $TC_2$ 时间戳对。这样转换结果基于多次观测的最优融合估计，而不是依赖某一个可能严重受干扰的单次接收。

卡尔曼滤波器的直觉理解：

如果你不熟悉卡尔曼滤波，可以这样简单理解：它是一种"智能加权平均"。

每次来了新的测量数据，卡尔曼滤波器不会全盘接受，也不会完全忽略，而是根据两个因素来决定给新数据多大的权重：

  1. “我对当前状态有多确定？”（用协方差矩阵 $P$ 表示）——如果系统已经跑了很久、状态很稳定，$P$ 很小，新来的一个异常值几乎不会动摇当前估计。
  2. “新测量有多可靠？”（用观测噪声方差 $R$ 表示）——如果新数据的噪声很大，给它的权重就很低。

这就是为什么卡尔曼滤波器既能快速收敛（启动时一切不确定，$P$ 大，新数据权重高），又能抵抗噪声（稳态时 $P$ 小，异常值影响有限）。

在我们的应用中，系统启动时 $offset$ 和 $drift$ 都未知，卡尔曼滤波器会在最初几个同步包中快速收敛。之后进入稳态，即使偶尔收到一个因多径效应严重偏差的时间戳，整体的同步精度也不会受到太大影响。

3.2.2.3 Anchor 间距离对时钟同步的影响

在时钟同步时，Anchor 到上级 Anchor（时钟源）之间的物理距离对同步精度有直接影响。这是一个容易被忽略但非常重要的问题。

DW3000 的内部计时器单位（DTU, Device Time Unit，通常简称 tick）约为 15.65ps，对应约 0.47cm 的空间距离。

当时钟同步包从时钟源发出，经过一段距离后到达下级 Anchor 时，下级 Anchor 对比"同步包中携带的发射时间戳"和"自己记录的接收时间戳"来计算同步参数。但这里有一个容易被忽略的问题：同步包在空中飞行是需要时间的！

从"上帝视角"来看，当下级 Anchor 收到同步包的那一刻，时钟源的时钟实际上已经继续走过了"同步包飞行时间"那么长。也就是说，同步包中的发射时间戳相对于"此刻时钟源的真实时间"是滞后的。

要得到时钟源的"当前真实时间"，需要把同步包中的发射时间戳加上数据包的总飞行时间（Time of Flight）。

数据包的"飞行时间"由三部分组成：

组成部分说明典型值
TX 天线延迟信号从芯片发射引脚到离开天线的延迟~16ns（与天线设计有关）
空间传播时间信号在空中以光速飞行的时间 = 距离 / $c$10m → ~33ns
RX 天线延迟信号从进入天线到芯片打上时间戳的延迟~16ns（与天线设计有关）

例如，两个 Anchor 相距 10 米，总飞行时间约为 $16 + 33 + 16 = 65\text{ns}$，对应约 $65 / 0.01565 \approx 4153$ 个 DW3000 tick。

天线延迟的校准

设备在出厂前需要进行 TX 天线延迟和 RX 天线延迟的校准。通常使用已知距离的 **TWR（Two-Way Ranging）**测距来标定——在已知精确距离 $d$ 的两个设备之间做双向测距，TWR 得到的距离值与实际距离的差值就是两端天线延迟之和。

但天线延迟的精确校准并不容易，原因包括：

  • 天线延迟受 PCB 布局、焊接质量、天线阻抗匹配等因素影响，每块板子都略有不同
  • 校准时的环境条件（温度、测量距离精度）也会引入误差
  • Decawave/Qorvo 在 DW1000 的开发板 TREK1000 的示例代码中，做 DS-TWR 测距时还会根据使用的频道（Channel）和通讯速率对测距结果进行修正

频率/通讯速率对"飞行时间"的影响：

从物理角度看，电磁波的频率和调制速率不会改变其在自由空间的传播速度（都是光速 $c$）。但实际上，不同的频率和速率配置会影响 DW3000 芯片内部的信号处理延迟——包括前导码的积累长度、LDE 算法的处理时间、模拟前端的群延迟等。从程序的角度看，这些内部延迟的变化会体现为"似乎飞行时间变了"的效果。所以 Decawave 提供了针对不同配置的修正系数来补偿这些差异。

综合来看，天线延迟和距离的校准都存在不可完全消除的系统性误差。我们可以使用反馈机制来系统性地解决这些令人头疼的校准问题——这就是下一节的内容。

3.2.2.4 时钟同步的反馈机制

使用高级版的时钟同步算法后，$factor$ 和 $offset$ 的滤波精度已经有了显著改进，但依然存在不可忽略的系统性偏差——天线延迟校准误差、Anchor 间距离测量误差等常量误差是无法通过统计滤波消除的（统计滤波只能消除随机误差，无法消除系统性偏差）。

为此，我们引入反馈机制——通过闭环控制来消除系统性误差。

反馈机制的工作原理
graph LR
	A0["A0 (时钟源)"] -- "ClockSync" --> A1["A1 (子 Anchor)"]
	A0 -- "ClockSync" --> A2["A2 (观察者)"]
	A1 -- "ClockSync" --> A2

	A2 -- "计算 A1 相对于<br/>A0 的时间偏差" --> CALC["偏差 = T_A1全局 - T_A0全局"]
	CALC -. "Feedback 包<br/>(误差 ticks)" .-> A1
	A1 -- "调整自己的<br/>offset 补偿" --> A1

假设 A0 是时钟源，A1 是 A0 的下级——A1 接收 A0 的同步包，在内部维持与 A0 一致的全局时间。我们使用第三方 Anchor A2 作为观察者（Observer）。

A2 的工作方式：

A2 能同时收到 A0 和 A1 的时钟同步包。A2 在内部同时与 A0 和 A1 进行时钟同步（即维护两套独立的同步参数）。当 A2 收到 A1 发出的时钟同步包时，它可以计算出两个值：

  1. 通过 A1 同步得到的"全局时间"（即 A1 认为自己转换出来的全局时间——这个值有误差）
  2. 通过 A0 同步得到的"全局时间"（即 A0 的全局时间——这是"真正的"全局时间参考）

这两个值的差异，我们可以合理地认为就是 A1 在与 A0 同步时引入的系统性误差。

关于 A2 自身的误差： A2 作为观察者，它自己与 A0 和 A1 做时钟同步时也不是完全准确的。但是，A2 的观察误差是随机的（每次独立测量的随机抖动），而 A1 的系统性偏差（如天线延迟校准误差）是恒定的。通过多次观察取平均，随机误差会趋向于零（根据大数定律），系统性偏差则会被凸显出来。

反馈的执行

A2 向 A1 发出一个反馈数据包（Feedback Packet），报告它观察到的 A1 与 A0 之间的全局时间差异（以 DW3000 tick 为单位）。A1 收到反馈后，将这个差异叠加到自己的 offset 补偿量中。

A2 观察到的这个误差——无论其成因是什么（A1 的接收时间戳不准确、A1 的 RX 天线延迟有偏差、A0 的 TX 天线延迟有偏差、两者之间距离估算不准确等等）——都可以通过这个反馈机制被系统性地修正。这就是反馈机制最强大的地方：它不需要知道误差的来源，只需要观测到误差并修正它。

A2 的距离补偿：

A2 到 A0 和 A1 的距离通常不相等。这意味着 A2 收到 A0 和 A1 的同步包时，存在由于距离差引起的飞行时间差。A2 在计算全局时间差异时，必须把这些距离差转换为 DW3000 tick 并进行补偿。因此，系统配置时需要提前将各个 Anchor 的坐标信息加载到 Observer 中，Observer 启动时自动计算与各 Anchor 的距离。

通过 A2 的持续反馈，A1 逐步调整自己的同步参数。从 A2 的视角来看，A1 和 A0 的全局时间差异会越来越小，最终趋近于零（收敛到 A2 自身的随机观测噪声水平）。

反馈机制的核心价值： 无论 Anchor 之间的距离测量有多少误差、无论天线延迟的校准有多少偏差、无论环境变化引起了什么样的系统性偏移，通过闭环反馈都可以将各 Anchor 的全局时间拉齐。这使得在多级级联同步时，也能保持令人满意的同步精度。

类比自动控制中的 PID： 反馈机制在概念上类似于经典的 PID 控制——观察者是"传感器"，它测量的"偏差信号"被反馈给被控对象（A1），A1 据此调整自己。只不过这里的控制量是时间偏移（offset），而不是温度或电机转速。与 PID 控制一样，反馈的增益（即 A1 每次根据反馈调整多少 offset）需要适当设计——太大会导致振荡，太小会导致收敛太慢。

3.2.2.5 时钟同步包和反馈包的结构

下面给出时钟同步包和反馈包的 C 语言结构体定义，以便读者理解空口数据包的具体构成。

时钟同步包
typedef struct PACK_ATTRIBUTE {
    uint8_t frame_ctrl[2];          // IEEE 802.15.4 帧控制字段
    uint8_t seq8;                   // 8-bit 序列号
    union {
        uint8_t pan_addr[2];
        uint16_t pan_id;            // PAN ID
    };
    union {
        uint8_t dest_addr[2];
        uint16_t dest_id16;         // 16-bit 短地址（广播时为 0xFFFF）
    };
    union {
        uint8_t source_addr[8];
        EUI64 source_id;            // 64-bit 源地址（本 Anchor 的唯一 ID）
    };
    uint8_t message_type;           // 消息类型标识
    uint8_t seq32_3[3];             // 32-bit 序列号的高 24 位
                                    //（与 seq8 组合成完整的 32-bit 序列号）
    uint8_t timestamp40[5];         // 40-bit 全局发射时间戳

    float x;                        // Anchor 的 X 坐标（米）
    float y;                        // Anchor 的 Y 坐标（米）
    float z;                        // Anchor 的 Z 坐标（米）
    uint8_t cs_level;               // 时钟同步级别
    union {
        uint8_t parent_clock_source_addr[8];
        EUI64 parent_clock_source_id;  // 上级时钟源的 EUI64
    };
    union {
        uint8_t observer_addr[8];
        EUI64 observer_id;          // 指定的观察者 EUI64
    };

    uint8_t fcs[2];                 // FCS（帧校验序列，CRC-16）
} BROADCAST_DL_CLOCK_SYNC_MESSAGE;

关于 PACK_ATTRIBUTE 宏：

这个宏通常定义为 __attribute__((packed))（GCC/Clang）或 #pragma pack(1)（MSVC），告诉编译器不要在结构体成员之间插入对齐填充字节（padding），确保结构体在内存中的布局与 UWB 空口数据包的字节流完全一致。

ESP32（Xtensa 架构）的对齐陷阱：

在 ESP32-S3（Xtensa LX7 内核）上使用 packed 结构体需要格外小心。Xtensa 处理器默认要求 uint16_t 类型按 2 字节对齐、uint32_t/float 按 4 字节对齐。如果通过指针直接访问 packed 结构体中未自然对齐的成员（例如 float x 字段前面是奇数个字节），可能触发 LoadStoreAlignment 异常（硬件异常，导致程序崩溃）。

应对方法：

  • 使用 memcpy 代替指针直接访问来读写未对齐的成员
  • 或者在结构体设计时有意识地安排字段顺序，让需要对齐的类型处于自然对齐的位置
  • 或者启用 ESP-IDF 的非对齐访问异常处理（性能有一定损失）
关键字段说明
字段大小说明
timestamp405 字节40-bit 全局时间戳，表示数据包离开天线的精确全局时间。这是时钟同步最核心的数据。
x / y / z各 4 字节本 Anchor 的坐标（单位：米，float 类型）。Tag 需要这些信息来计算自己的位置。
cs_level1 字节时钟同步级别。根时钟源为 0，逐级递增。数字越小表示离根时钟源越近、同步精度越高。Tag 在选择"参考 Anchor"时会优先信任 cs_level 较低的 Anchor。
parent_clock_source_id8 字节本 Anchor 的上级时钟源的 EUI64。用于在系统启动阶段让下级 Anchor 知道要跟谁同步。
observer_id8 字节指定的观察者 Anchor 的 EUI64。管理员在配置阶段为每个 Anchor 选择一个合适的观察者。目标 Anchor 只接受来自指定观察者的反馈，忽略其他来源的反馈包。
空口时间优化——大包 / 小包分离

从上面的结构定义可以看出，有些字段在运行过程中几乎不会变化：

  • Anchor 坐标（x / y / z）——除非人为修改
  • 上级 Anchor ID（parent_clock_source_id）——除非重新配置
  • 同步级别（cs_level）——除非层级拓扑变更
  • 观察者 ID（observer_id）——除非重新配置

如果每次发送时钟同步包都携带这些"准静态"字段，会浪费宝贵的 UWB 空口时间（Air Time）。UWB 数据包越长，发送耗时越长（DW3000 在 6.8Mbps 速率下，每多一个字节约增加 1.2μs 的空口占用），也意味着这段时间内接收机无法接收其他数据包。

优化方案： 定义两种同步包：

  • 精简版（小包）：只包含 timestamp40、seq、source_id 等必要字段（约 20~25 字节）。平时的高频同步只发送小包。
  • 完整版（大包）：包含上述全部字段（约 50~55 字节）。偶尔（例如每 30 秒或每分钟一次）发送一次。
gantt
	title 时钟同步包发送时间线
	dateFormat X
	axisFormat %s

	section Anchor 广播
	小包 :a1, 0, 1
	小包 :a2, 2, 3
	小包 :a3, 4, 5
	小包 :a4, 6, 7
	小包 :a5, 8, 9
	大包 (含坐标等) :crit, a6, 10, 12
	小包 :a7, 14, 15
	小包 :a8, 16, 17

这样既节省了空口时间（绝大部分时候发的是小包），又保证了准静态字段有变化时能及时通知所有下级 Anchor 和 Tag。

反馈包结构
typedef struct PACK_ATTRIBUTE {
    uint8_t frame_ctrl[2];
    uint8_t seq8;
    union {
        uint8_t pan_addr[2];
        uint16_t pan_id;
    };
    union {
        uint8_t dest_addr[8];       // 注意: 8 字节目标地址（单播）
        EUI64 dest_id;
    };
    union {
        uint8_t source_addr[8];
        EUI64 source_id;            // 观察者的地址
    };
    uint8_t message_type;

    EUI64 reference_id;             // 参考 Anchor 的 EUI64
    int32_t error_ticks;            // 观测到的误差（单位：DW3000 tick）
    uint16_t confidence;            // 置信度（0~65535）
    uint8_t fcs[2];
} UNBROADCAST_DL_CLOCK_SYNC_FEEDBACK_MESSAGE;

注意： 反馈包是**单播（Unicast）**的——dest_addr 是 8 字节的 EUI64 地址，只发给特定的目标 Anchor。这与同步包的广播（dest_id16 = 0xFFFF）不同。因为反馈信息只对特定 Anchor 有意义，广播给所有设备不仅浪费空口，还可能引起混淆。

关键字段说明：

字段说明
reference_id参考 Anchor（即时钟源）的 EUI64。观察者比较的是"目标 Anchor"和"其上级时钟源"，所以这个 ID 通常就是目标 Anchor 同步包中的 parent_clock_source_id。
error_ticks观察到的同步误差，单位是 DW3000 tick（有符号整数）。正值表示目标 Anchor 的全局时间超前于时钟源，负值表示滞后。目标 Anchor 收到后将此值叠加到自己的 offset 补偿量中。
confidence置信度，表示这个反馈包的可靠程度（0 = 完全不可信，65535 = 非常可信）。通常根据观察者到两个 Anchor 的信号质量（首径功率、信噪比）、观察者自身同步状态的稳定性等因素综合计算。目标 Anchor 在采纳反馈时，会根据 confidence 决定给予多大的调整权重。

3.2.3 定位数据包——“一包两用”

最初设计系统时，我考虑使用专门的定位数据包——即 Anchor 除了发送时钟同步包之外，还额外发送一种"定位包"给 Tag 使用。

但当时钟同步功能开发完成后，我意识到：时钟同步包本身就是最好的定位数据包！ 不需要再单独设计一种新的包类型。

原因很简单：

  1. Tag 的"锁定"本质上就是时钟同步。 Tag 需要"锁定"它能看到的 Anchor，从本质上说，Tag 就是在与这些 Anchor 做时钟同步。Tag 处理时钟同步包的逻辑与 Anchor 处理时钟同步包的逻辑几乎完全一样——都是读取接收时间戳、提取发送时间戳、更新 offset/drift 参数。区别只是 Tag 不需要发送反馈包，也不需要向下级转发同步包。

  2. 信息完备性。 时钟同步包中已经包含了定位所需的全部信息：

    • 精确的全局时间戳 → 用于时间差计算
    • Anchor 坐标（$x, y, z$）→ 用于位置解算
    • 时钟同步级别 → 用于 Tag 选择最可信的参考 Anchor
    • Anchor 的 EUI64 → 用于 Tag 区分不同的 Anchor

  3. 减少复杂度和空口占用。 再定义一种类型的数据包只会增加系统的复杂度（需要额外的包解析逻辑、发送调度逻辑），并增加空口占用（更多的包意味着更多的碰撞概率），完全没有必要。

graph TD
	classDef anchor fill:#4a90e2,stroke:#333,stroke-width:2px,color:#fff,rx:5px,ry:5px;
	classDef tagNode fill:#f39c12,stroke:#333,stroke-width:2px,color:#fff,rx:20px,ry:20px;

	A0["Anchor A0 (Level 0)"]:::anchor
	A1["Anchor A1 (Level 1)"]:::anchor
	A2["Anchor A2 (Level 2)"]:::anchor
	A3["Anchor A3 (Level 3)"]:::anchor
	Tag(("Tag")):::tagNode

	A0 -- "ClockSync" --> A1
	A0 -- "ClockSync" --> Tag
	A0 -. "ClockSync (广播)" .-> OTHER1["其他设备..."]

	A1 -- "ClockSync" --> A2
	A1 -- "ClockSync" --> Tag

	A2 -- "ClockSync" --> A3
	A2 -- "ClockSync" --> Tag

	A3 -- "ClockSync" --> Tag
	A3 -. "ClockSync (广播)" .-> OTHER2["其他设备..."]

图例说明：

  • 蓝色方块：4 个 Anchor 设备，按层级从 Level 0 到 Level 3 排列。每个 Anchor 定期向四周广播 ClockSync 包。
  • 橙色圆形：Tag 设备。它同时接收来自所有可见 Anchor 的 ClockSync 包，与每个 Anchor 建立时钟同步（“锁定”），然后利用同步参数计算自己的坐标。
  • 实线箭头：指向具体接收方（下级 Anchor 或 Tag）。
  • 虚线箭头：表示广播信号辐射到周围空间，可被任何设备接收。

如上图所示，时钟同步包既用于 Anchor 间的时间同步，又被 Tag 接收用于定位——一包两用，简洁而高效。这种设计大大简化了系统架构，也减少了空口上不同类型数据包的冲突。

3.2.4 Anchor 锁定

在前面的章节中我们已经说过，Tag 使用时钟同步包来计算坐标。因为各个 Anchor 并不是同时发出时钟同步包的（每个 Anchor 有自己独立的发送节拍），Tag 需要锁定附近的多个 Anchor——即与这些 Anchor 分别建立时钟同步关系。这样，Tag 就可以在任意时刻将自己的本地时间转换为每个锁定 Anchor 的全局时间，再根据这些全局时间的差异来计算出自己的坐标。

“锁定"的本质： 对于 Anchor 来说，“时钟同步"是与上级 Anchor（时钟源）同步。对于 Tag 来说，“锁定"某个 Anchor 也是在做时钟同步——只不过 Tag 是被动接收，不需要发送反馈包，也不需要向下游转发同步包。Tag 为每个锁定的 Anchor 维护一套独立的同步参数（包括 Kalman 滤波器状态、滑动窗口历史等），这套参数和 Anchor 内部用于与上级同步的参数结构完全一样。

为什么 Tag 可以"被动"锁定？ 在下行 TDOA 体系中，Anchor 周期性地广播时钟同步包，包中携带了 Anchor 的全局发送时间戳、坐标、时钟同步级别等信息。Tag 只需要"静静地听”，不需要回复任何东西。这种纯被动的工作模式有两个重要优势：（1）Tag 不会产生 UWB 射频发射，节省电量；（2）系统中 Tag 的数量不受限制——无论有多少个 Tag 同时在监听，都不会给 Anchor 带来额外负担。

我们创建一个结构来存储单个 Anchor 的跟踪状态：

/** 单个 Anchor 的跟踪状态 */
typedef struct {
	EUI64 anchor_id;                 // 被跟踪的 Anchor 的唯一 ID
	UwbSyncInstance sync_inst;       /**< 时钟同步实例，包含 Kalman 滤波器等
									  *   Tag 模式下不使用反馈功能 */
	float x, y, z;                   // Anchor 的坐标（从同步包中获取）
	uint8_t cs_level;                // Anchor 的时钟同步级别
	uint32_t last_seen_ms;           // 最后一次收到该 Anchor 数据包的时间（ESP 系统时间, ms）
	bool is_active;                  // 该跟踪槽位是否激活
	/** 最近一次该 Anchor 的全局发射时刻，用于后续 TDOA 计算 */
	uint64_t last_remote_tx;
} TagAnchorTracker;

last_seen_ms 的作用： 如果一个 Anchor 长时间没有被收到（例如 Tag 移动到了该 Anchor 的覆盖范围之外），我们需要将它标记为非活跃（is_active = false），并在需要时用新发现的 Anchor 替换它。last_seen_ms 就是用来判断"多久没收到"的依据。通常，如果超过若干个同步周期都没有收到某 Anchor 的包（比如连续缺失 5 个以上的同步周期），就可以认为该 Anchor 已经"丢失”。

工程提示： last_seen_ms 使用的是 ESP32 的系统时间（esp_timer_get_time() 返回的微秒值转换为毫秒），而不是 UWB 时间戳。这是因为我们只需要粗略判断"多久没收到”，不需要高精度——用系统时间即可，避免引入不必要的复杂性。

其中的 UwbSyncInstance sync_inst 是时钟同步的核心数据结构，定义如下：

/**
 * @brief 同步实例 (每个设备维护一个, 对应一个上级时钟源)
 *
 * Anchor 用它来维持与上级时钟源的同步;
 * Tag 用它来"锁定"某个 Anchor。
 * 两者使用的是完全相同的算法和数据结构。
 */
typedef struct PACK_ATTRIBUTE {
	/* ---- 配置区 ---- */
	uint64_t my_id;             /**< 本机 EUI64 */
	uint64_t parent_cs_id;      /**< 上级时钟源 EUI64 (Tag 模式下为被锁定的 Anchor ID) */
	bool     is_root;           /**< 是否为根 Anchor (Tag 永远为 false) */
	float    my_x, my_y, my_z;  /**< 本机坐标 (米) */

	/* ---- 40-bit 溢出追踪 (本地 RX 时间域) ---- */
	uint64_t last_raw_tick;     /**< 上次读到的 40-bit 原始值 */
	uint64_t overflow_count;    /**< 溢出次数 */

	/* ---- 40-bit 溢出追踪 (远端全局 TX 时间域) ---- */
	uint64_t last_remote_raw;   /**< 上次收到的远端 40-bit 原始值 */
	uint64_t remote_overflow;   /**< 远端溢出次数 */

	/* ---- Kalman 滤波器 ---- */
	KalmanSync kf;

	/* ---- 滑动窗口历史 ---- */
	SyncHistoryEntry history[SYNC_HISTORY_SIZE];
	int      history_idx;       /**< 环形缓冲写指针 */
	int      history_count;     /**< 有效条目数 */

	/* ---- 质量与统计 ---- */
	float    quality_score;     /**< 同步质量 0.0 ~ 1.0 */
	uint32_t sync_count;        /**< 收到的同步包总数 */
	uint32_t outlier_streak;    /**< 连续异常值计数 */

	/* ---- 反馈缓冲 (仅 Anchor 模式使用, Tag 模式不使用) ---- */
	int32_t  fb_buf[SYNC_FEEDBACK_BUF_SIZE];
	uint16_t fb_conf[SYNC_FEEDBACK_BUF_SIZE];
	uint32_t fb_time_ms[SYNC_FEEDBACK_BUF_SIZE];
	int      fb_count;
	uint32_t fb_last_apply_ms;

	/* ---- 反馈偏差补偿 ----
	 * 独立于 Kalman 的累积偏差修正。
	 * 父节点同步操作 kf.offset, 而 fb_offset_bias 在
	 * local_to_global / global_to_local 时叠加,
	 * 保证反馈修正不会被父节点同步覆盖。 */
	double   fb_offset_bias;

	/* ---- 级联级别 ---- */
	uint8_t  my_cs_level;
	uint8_t  parent_cs_level;

	/* ---- Tag 模式 ----
	 * Tag 被动监听, tof_ticks 设为 0（Tag 不知道自己到 Anchor 的精确距离）。
	 * CFO 读数在不同硬件批次/芯片之间可能与实际时间戳 skew 存在系统偏差,
	 * 启用 tag_mode 后跳过 kf_update_cfo, 让 Kalman 纯靠 offset 观测收敛 skew。 */
	bool     tag_mode;
} UwbSyncInstance;

关于 40-bit 溢出追踪：

DW3000 的计时器是 40 位宽的，满量程约 17.2 秒就会溢出归零。但我们的同步算法需要计算跨越多次溢出的时间差。因此，软件中使用 overflow_count 来记录溢出次数，并通过 sync_extend_timestamp() 函数将 40-bit 时间戳扩展为 64-bit，从而得到一个不会溢出的连续时间轴。这是一个非常重要但容易忽略的工程细节。

判断溢出的方法： 如果当前读到的 40-bit 值比上次的值小（例如上次是 0xF000000000，这次是 0x0100000000），说明发生了一次溢出，overflow_count++。需要注意的是，这种判断方法隐含一个假设：相邻两次采样之间最多只发生一次溢出。对于 17.2 秒的溢出周期和通常 100~200ms 的同步包间隔来说，这个假设完全成立。但如果设备长时间休眠后醒来，可能会错过多次溢出——这时需要特殊处理（例如重新初始化同步状态）。

工程提示（双域溢出追踪）： 注意结构体中有两组溢出追踪变量——一组用于本地 RX 时间域（last_raw_tick / overflow_count），另一组用于远端 TX 时间域（last_remote_raw / remote_overflow）。这是因为本地时钟和远端时钟是独立的，它们的溢出时刻不同，必须分别追踪。初学者常犯的一个错误是只维护一组溢出计数，结果在计算时间差时得到巨大的异常值。

跟踪数组

我们建立一个数组来管理所有被跟踪的 Anchor：

#define MAX_TRACKED_ANCHORS 8
TagAnchorTracker s_trackers[MAX_TRACKED_ANCHORS];

s_trackers 用于记录 Tag 当前视野范围内的 Anchor。如果 Tag 的内存较大，可以增加 MAX_TRACKED_ANCHORS 的值——能跟踪的 Anchor 越多，坐标计算时可用的数据就越多，定位精度和鲁棒性就越好。

MAX_TRACKED_ANCHORS 的选取建议：

每个 TagAnchorTracker 中包含一个 UwbSyncInstance，后者内含 Kalman 滤波器和历史缓冲区，每个实例占用数百字节到几 KB 的 RAM（取决于 SYNC_HISTORY_SIZE 和 SYNC_FEEDBACK_BUF_SIZE）。8 个跟踪槽位在 ESP32-S3（520KB SRAM + 可选 8MB PSRAM）上完全没有压力。对于更大的部署场景（如 Tag 需要穿越覆盖数十个 Anchor 的区域），可以增大到 16 或更多。

需要注意的是，更多的跟踪槽位也意味着每次坐标计算时需要处理更多的 DDOA 组合，计算量会随 Anchor 数量的平方增长。在 ESP32-S3 的主频（240MHz）下，8~16 个 Anchor 的计算量完全可以在毫秒级内完成，不会成为瓶颈。

每次收到时钟同步包时的处理流程

flowchart TD
	RX["收到 ClockSync 包"] --> FIND["在 s_trackers 中查找<br/>该 Anchor 的跟踪槽位"]
	FIND -->|"找到"| PREDICT["1. 预测: 将 Kalman 状态<br/>推进到当前时刻"]
	FIND -->|"未找到 (新 Anchor)"| REPLACE["替换 s_trackers 中<br/>最老/最弱的槽位"]
	REPLACE --> INIT["初始化新的 UwbSyncInstance"]
	INIT --> PREDICT

	PREDICT --> OBS_OFFSET["2. 时间观测:<br/>z_offset = (remote_tx + tof) - local_rx"]
	OBS_OFFSET --> OUTLIER{"3. 异常值检测:<br/>|innovation| > 门限?"}
	OUTLIER -->|"是 (异常)"| SKIP["跳过本次 Kalman 更新<br/>outlier_streak++"]
	OUTLIER -->|"否 (正常)"| UPDATE["更新 Kalman 状态<br/>(offset + drift)"]
	UPDATE --> CFO["4. CFO 观测:<br/>z_skew = cfo_raw × 转换系数<br/>(tag_mode 下跳过)"]
	CFO --> HISTORY["5. 更新滑动窗口历史缓冲"]
	HISTORY --> QUALITY["更新 quality_score"]

每次 Tag 收到一个时钟同步包，都会执行以下操作：

  1. 预测（Predict）：将 Kalman 滤波器的状态推进到当前时刻（基于已知的 drift 进行外推）。这一步使用的是标准 Kalman 滤波器的预测方程：$\hat{x}_{k|k-1} = F \cdot \hat{x}_{k-1|k-1}$，$P_{k|k-1} = F \cdot P_{k-1|k-1} \cdot F^T + Q$，其中 $F$ 是状态转移矩阵，$Q$ 是过程噪声协方差。

  2. 时间观测（Offset Observation）：计算 z_offset = (remote_tx + tof) − local_rx，其中 tof（Time of Flight，飞行时间）在 Tag 模式下设为 0（因为 Tag 不知道自己到 Anchor 的精确距离）。这个设为 0 的近似会引入一个与距离成正比的偏置误差，但由于 TDOA 计算使用的是时间差（两个 Anchor 的全局时间相减），这个偏置在差分运算中会被大部分消除——除非 Tag 到两个 Anchor 的距离差非常大。

  3. 异常值检测（Outlier Detection）：如果观测值与预测值的偏差超过设定的门限，认为这是一个异常包（可能由于多径干扰），跳过本次 Kalman 更新，同时 outlier_streak++。

    什么是 Innovation？ 在 Kalman 滤波理论中，innovation（新息）是指实际观测值 $z_k$ 与滤波器预测的观测值 $\hat{z}_{k|k-1}$ 之间的差：$\nu_k = z_k - H \cdot \hat{x}_{k|k-1}$。如果滤波器状态估计准确，innovation 应该是一个均值为零的随机序列，其方差由 $S_k = H \cdot P_{k|k-1} \cdot H^T + R$ 给出（$R$ 为观测噪声协方差）。当 $|\nu_k|$ 显著偏离零值（例如超过 $3\sqrt{S_k}$，即 3-sigma 门限），就说明这个观测值很可能是异常的——它与滤波器已有的状态估计严重矛盾。此时跳过更新是一种稳健的做法，可以防止一次坏的观测"污染"已经收敛的滤波器状态。

    连续异常的处理： outlier_streak 记录连续被判定为异常的次数。如果连续异常达到一定次数（比如 10 次以上），说明可能不是偶然的干扰，而是滤波器状态已经偏离了真实值（例如 Tag 发生了快速移动导致时钟关系突变）。此时应该重新初始化该 Anchor 的同步状态，让 Kalman 滤波器从头开始收敛。

  4. CFO 观测（仅 Anchor 模式）：将 DW3000 硬件报告的 CFO（Carrier Frequency Offset，载波频偏）值转换为 skew 并作为第二观测源更新 Kalman。Tag 模式下跳过此步骤——原因是 CFO 读数在不同硬件批次/芯片之间可能与实际时间戳 skew 存在系统性偏差，Tag 没有机会像 Anchor 那样通过反馈机制来校准这个偏差，因此干脆不使用 CFO，让 Kalman 纯靠时间观测收敛 skew。

  5. 更新历史缓冲：将本次同步数据记录到滑动窗口中。历史缓冲区是一个环形缓冲（ring buffer），记录最近 N 次同步的原始数据。这些历史数据用于计算 quality_score（同步质量评分），也可以在调试时用来回溯分析同步状态的变化趋势。

总之，Tag 持续跟踪每个锁定的 Anchor，保持时钟同步。如果收到了来自新 Anchor 的同步包（s_trackers 中没有它的记录），则从数组中移除最"旧"或同步质量最差的 Anchor，用新 Anchor 替换。

替换策略的详细设计：

当 Tag 收到一个新 Anchor 的同步包，而 s_trackers 数组已满时，需要决定替换哪个已有的跟踪槽位。简单的"替换最老的"策略可能不是最优的——比如一个"老"Anchor 一直表现很好（quality_score 高），不应该仅仅因为存在时间最久就被替换掉。

更好的做法是为每个跟踪槽位计算一个综合替换优先级分数，分数越高的槽位越容易被替换。这个分数可以综合考虑以下因素：

因素权重方向说明
is_active == false最高优先替换已经标记为非活跃的槽位应最先被替换
last_seen_ms 距今过久高优先替换长时间未收到数据的 Anchor 可能已经超出覆盖范围
quality_score 低高优先替换同步质量差的 Anchor 对定位贡献有限
outlier_streak 高高优先替换连续异常说明该 Anchor 信号环境恶劣
cs_level 高（级联层级深）中优先替换级联层级越深，累积误差越大，同步精度越低

一个简单的实现方式：

int find_weakest_tracker() {
    int worst_idx = 0;
    float worst_score = -1e9f;
    for (int i = 0; i < MAX_TRACKED_ANCHORS; i++) {
        float score = 0;
        if (!s_trackers[i].is_active) return i;  // 非活跃槽位直接返回
        uint32_t age_ms = now_ms - s_trackers[i].last_seen_ms;
        score += age_ms * 0.001f;                 // 越久未见，分越高
        score += (1.0f - s_trackers[i].sync_inst.quality_score) * 100.0f;
        score += s_trackers[i].sync_inst.outlier_streak * 10.0f;
        score += s_trackers[i].cs_level * 5.0f;
        if (score > worst_score) {
            worst_score = score;
            worst_idx = i;
        }
    }
    return worst_idx;
}

权重系数需要根据实际部署场景调整。例如在 Tag 快速移动的场景下，可以增大 last_seen_ms 的权重，使得失联 Anchor 更快被替换；在信号环境复杂的场景下，可以增大 quality_score 的权重。

3.2.5 坐标计算

作为 Tag，这是整个系统中最关键的一步——把时钟同步的中间结果最终转化为有意义的三维坐标。

从时间差到距离差

在前面的章节中我们说过，各个 Anchor 在不同时刻发出定位数据包（时钟同步包），Tag 当然是在不同时刻收到的。因为发送时间不同，不能直接用接收时间戳相减来得到距离差。

Tag 通过"锁定"多个 Anchor（为每个 Anchor 维护独立的同步参数），可以在任意时刻将自己的本地时间转换为每个 Anchor 对应的全局时间。

假设 Tag 在本地时间 $T_{local}$ 这一刻，将它转换为 4 个 Anchor 的全局时间，得到 $T_{A0}$、$T_{A1}$、$T_{A2}$、$T_{A3}$。因为 Tag 到各个 Anchor 的距离不一样，这 4 个全局时间是有差异的——把它们两两相减，然后乘以光速，就得到距离差了。

直觉理解： 假设某一瞬间 Tag 同时向 4 个 Anchor 各发一个脉冲。距离近的 Anchor 先收到，距离远的 Anchor 后收到。收到的时刻之差 × 光速 = 距离之差。在下行 TDOA 中方向是反的（Anchor 发，Tag 收），但数学原理是对称的。

更严谨的表述： 设 Tag 位置为 $(x, y, z)$，Anchor $i$ 的坐标为 $(x_i, y_i, z_i)$，Tag 到 Anchor $i$ 的距离为 $d_i = \sqrt{(x - x_i)^2 + (y - y_i)^2 + (z - z_i)^2}$。Tag 转换到 Anchor $i$ 的全局时间为 $T_i = T_{local} + offset_i$（其中 $offset_i$ 是 Kalman 滤波器估计的时钟偏移）。如果时钟同步完全准确，那么 $T_i - T_j$ 正好等于 $(d_i - d_j) / c$，其中 $c$ 是光速。我们将 $(T_i - T_j) \times c$ 作为距离差的估计值。

坐标计算的时机

理论上，只要"锁定"了足够多的 Anchor，Tag 可以在任意时刻计算坐标。但如果没有收到新的数据包，多次重复计算得到的结果都是相同的——浪费算力没有意义。

所以，我们选择在每次收到新的同步包后触发坐标计算：构造距离差数组 DDOA，如果构造出的数组满足计算坐标的最低要求，就调用坐标计算函数，否则跳过。

最低 Anchor 数量要求：

  • 二维定位（已知 $z$，求解 $x, y$ 两个未知数）：至少需要 3 个 Anchor。3 个 Anchor 可以构造 $C_3^2 = 3$ 组 DDOA 对，其中有 2 组是独立的（$N-1 = 3-1 = 2$），恰好可以求解 2 个未知数。
  • 三维定位（求解 $x, y, z$ 三个未知数）：至少需要 4 个 Anchor。4 个 Anchor 可以构造 $C_4^2 = 6$ 组 DDOA 对，其中有 3 组是独立的（$N-1 = 4-1 = 3$），恰好可以求解 3 个未知数。

为什么 $N$ 个 Anchor 只有 $N-1$ 个独立的距离差？ 因为所有距离差都可以通过"链式相减"得到。例如有 A、B、C 三个 Anchor，知道了 $\Delta d_{AB}$（A 和 B 的距离差）和 $\Delta d_{BC}$（B 和 C 的距离差），就可以算出 $\Delta d_{AC} = \Delta d_{AB} + \Delta d_{BC}$——第三组距离差不是独立的。数学上，这等价于 $N$ 个 Anchor 形成 $N-1$ 个独立的双曲线（面）方程。

在代码中，我们用构造出的 DDOA 对总数作为判断依据：如果 DDOA 对数 $\geq 3$（对应至少 3 个 Anchor 的二维定位）或 $\geq 6$（对应至少 4 个 Anchor 的三维定位），则认为数据充分，可以进行坐标计算。使用总对数而不是独立对数来判断，是因为冗余的 DDOA 对虽然不增加几何自由度，但参与最小二乘求解时可以提高鲁棒性。

计算频率举例： 假设某个区域内有 4 个 Anchor，每个 Anchor 的时钟同步包发送间隔为 150ms（可配置）。那么 Tag 平均每 $150 / 4 = 37.5ms$ 就会收到一个新包，即坐标计算频率约为 26Hz。这个频率对于大多数人员/物资定位场景已经足够。如果觉得过于频繁（比如 Tag 基本静止），也可以设置一个最小计算间隔来降低频率，节省电量。

工程提示： 每次收到同步包时并非只更新一个 DDOA 对——而是重新构造所有活跃 Anchor 之间的 DDOA 组合。这样做的好处是每次坐标计算都使用最新的全部同步状态，而不仅仅依赖于刚收到的那一个 Anchor 的信息。

距离差（DDOA）

TDOA（Time Difference of Arrival）的本质是距离差。为了方便后续的坐标解算，我们定义一个结构来记录两个 Anchor 之间的距离差：

typedef struct PACK_ATTRIBUTE ___tag_ddoa___ {
	EUI64 aId;              // Anchor A 的 ID
	float ax;               // Anchor A 的 X 坐标（米）
	float ay;               // Anchor A 的 Y 坐标
	float az;               // Anchor A 的 Z 坐标

	EUI64 bId;              // Anchor B 的 ID
	float bx;               // Anchor B 的 X 坐标
	float by;               // Anchor B 的 Y 坐标
	float bz;               // Anchor B 的 Z 坐标

	float deltaDistance;    // Tag 到 A 与到 B 的距离差（米）
							// 正值表示 Tag 离 A 更远
} DDOA;

再定义一个数组来存放所有的距离差组合：

#define MAX_DDOA_NUM    20
DDOA listDDOAs[MAX_DDOA_NUM];

DDOA 数量与 Anchor 数量的关系： 如果 Tag 锁定了 $N$ 个 Anchor，理论上可以构造 $C_N^2 = N(N-1)/2$ 组距离差。例如锁定 4 个 Anchor 可构造 6 组，锁定 6 个 Anchor 可构造 15 组。MAX_DDOA_NUM = 20 足以支持 6~7 个 Anchor 的全组合。但其实这些距离差不是全部独立的——$N$ 个 Anchor 只有 $N-1$ 个独立的距离差。冗余的距离差对于提高鲁棒性（识别异常 Anchor）有帮助，但不会增加定位的几何自由度。

deltaDistance 符号约定： 在这个结构中，deltaDistance 定义为 Tag 到 Anchor A 的距离减去 Tag 到 Anchor B 的距离。正值表示 Tag 离 A 更远。在构造 DDOA 时保持一致的符号约定非常重要——如果不同地方对 A、B 的顺序不一致，就会产生符号相反的距离差，导致坐标计算结果完全错误。这是一个很容易在编码时犯的低级错误。

坐标计算算法

有了各个 Anchor 之间的距离差 listDDOAs，就可以进行坐标解算了。

二维定位 vs 三维定位

虽然我们在处理空间坐标时都使用三维坐标（Anchor 的坐标是三维的），但 Tag 的 $z$（高度）坐标是一个麻烦问题。

通常，Anchor 部署在比较高的位置（天花板上、墙壁上、室外灯杆上）。部署在高处的好处显而易见——“站得高看得远”，Tag 更容易接收到 Anchor 的信号，Line-of-Sight（视距）覆盖更好。但是，如果要计算三维坐标，就需要部分 Anchor 部署在地面附近。

“内插"vs"外推"的定位精度差异：

无论使用哪种算法，当 Tag 位于 Anchor 组成的多边形（二维）或多面体（三维）内部时，计算出的坐标会更准确——这就是"内插”（Interpolation）。当 Tag 在 Anchor 包围区域的外部时，定位精度会显著下降——这就是"外推"（Extrapolation）。

对于三维定位，如果所有 Anchor 都在天花板上，而 Tag 在地面，那么 $z$ 方向永远是外推，$z$ 坐标的精度会很差。只有在天花板和地面都部署 Anchor 时，$z$ 方向才能实现内插。但地面 Anchor 面临严重的遮挡问题——人、家具、货架等都会阻挡信号。

量化理解： 假设 Anchor 部署在 3 米高的天花板上，Tag 在地面（$z=1.5m$）。$z$ 方向上 Tag 完全位于 Anchor “下方”，属于单侧外推。一般经验是，外推方向上的定位误差是内插方向的 310 倍。如果二维 $(x, y)$ 的精度是 20cm，那 $z$ 方向的精度可能只有 0.62m——这对大多数应用来说是不可接受的。

因此，大多数实用的 UWB 定位系统都只做二维定位（仅求 $x$、$y$），只在特殊场景（如多层仓库、竖井等）才使用三维定位。

二维定位的做法：为 Tag 预设一个固定的高度 $z$ 值（例如 1.5m，代表人胸前佩戴的 Tag 高度），然后在三维方程中将 $z$ 视为已知常数，只求解 $x$ 和 $y$。本质上，二维坐标计算是三维坐标计算的一个特例。

工程提示（$z$ 值的影响）： 预设的 $z$ 值与实际高度的差异会影响 $(x, y)$ 的计算精度。差异越大，影响越大——特别是当 Anchor 部署高度较低时（比如只有 2 米）。如果 Anchor 部署在 4 米以上的高度，而 Tag 高度变化范围在 0.8~1.8 米之间，那么 $z$ 误差对 $(x, y)$ 计算的影响通常在厘米级，可以忽略。

求解方法——迭代逼近

理论上，计算 Tag 坐标就是解方程——求出 $(x, y, z)$ 使得恰好满足 listDDOAs 中的所有距离差关系。但由于噪声和测量误差的存在，这个方程组通常没有精确解。我们只能使用各种数值迭代法，找到一个使残差（方程误差）最小的近似解。

flowchart LR
	START["选取初始估计点<br/>(x₀, y₀, z₀)"] --> CALC["代入 DDOA 方程组<br/>计算残差"]
	CALC --> CHECK{"残差是否<br/>足够小?"}
	CHECK -->|"否"| UPDATE["根据算法规则<br/>计算更优的点<br/>(x₁, y₁, z₁)"]
	UPDATE --> CALC
	CHECK -->|"是"| OUTPUT["输出当前点<br/>作为最终坐标"]

业界常用的坐标解算算法包括：

算法特点适用场景
Chan 算法闭式解，一步出结果，速度快初始估计、Anchor 数量略多于最小值时
Taylor 算法迭代式，需要初始值，精度高在有较好初始估计时做精化
Chan-Taylor 混合先 Chan 给初始值，再 Taylor 迭代兼顾速度和精度
Gauss-Newton 算法经典非线性最小二乘迭代通用性强，适合超定方程组
LSR (Least Squares Range)基于距离的最小二乘Anchor 数量较多时

这些算法的本质区别： 都是从某个初始点出发，根据当前的 DDOA 数据寻找使误差更小的下一个点，反复迭代直到收敛。不同算法的区别主要在于：（1）如何选择搜索方向；（2）如何计算步长；（3）如何判断收敛。Chan 算法比较特殊，它通过代数变换得到一个近似的闭式解，不需要迭代，计算速度极快，但在噪声大时精度不如迭代法。

Chan 算法简介

由于 Chan 算法在 TDOA 定位中使用非常广泛，且是本系统默认的初始估计算法，这里对其核心思想做简要介绍。

Chan 算法（出自 Y.T. Chan 的经典论文 “A Simple and Efficient Estimator for Hyperbolic Location”，1994 年）的核心思想是通过引入辅助变量，将非线性的双曲线方程组线性化，从而得到闭式（closed-form）解，避免迭代。

TDOA 定位的基本方程是非线性的。以二维为例，对于 Anchor $i$ 和 Anchor $j$，距离差方程为：

$$\sqrt{(x - x_i)^2 + (y - y_i)^2} - \sqrt{(x - x_j)^2 + (y - y_j)^2} = \Delta d_{ij}$$

这个方程含有平方根，直接求解非常困难。Chan 算法的巧妙之处在于：

  1. 选定一个参考 Anchor（比如 Anchor 0），将所有距离差都表示为相对于它的形式：$\Delta d_{i0} = d_i - d_0$
  2. 引入辅助变量 $R = d_0$（Tag 到参考 Anchor 的距离），以及 $r_i = d_i$。利用 $r_i = \Delta d_{i0} + R$ 的关系，将方程改写为关于 $(x, y, R)$ 的形式
  3. 平方展开消除平方根：对 $r_i^2 = (x - x_i)^2 + (y - y_i)^2$ 展开，并将不同 Anchor 的方程两两相减，消去 $x^2 + y^2$ 项，得到线性方程组
  4. 用加权最小二乘法（WLS） 求解这个线性方程组，直接得到 $(x, y)$ 的估计值

Chan 算法的优势是速度极快（只需矩阵运算，无需迭代），在噪声不大时精度接近理论下界（CRLB，克拉美-罗下界）。其劣势是在噪声较大或 Anchor 几何构型不佳（如 Anchor 接近共线）时，精度会明显下降。因此在实践中，常用 Chan 算法的输出作为 Taylor 或 Gauss-Newton 等迭代算法的初始值，形成 Chan-Taylor 混合 策略。

工程提示： 如果你要自行实现 Chan 算法，需要注意矩阵求逆的数值稳定性。当 Anchor 数量较少或几何构型接近退化（如三个 Anchor 接近共线）时，矩阵条件数会很大，直接求逆可能产生巨大误差。建议使用 SVD（奇异值分解）或 QR 分解代替直接求逆。

这些算法我都集成到了 Tag 的固件中，可以通过配置参数选择使用哪种算法。如果你要自行实现坐标解算，可能需要阅读相关的学术论文（如 Chan 的经典论文），然后编写代码实现。对于初学者，建议先从 Chan 算法入手——它的数学推导相对直观，代码实现也不复杂（核心部分约 100~200 行 C 代码），而且可以立即得到可用的定位结果。

坐标计算中反馈包的利用

在 Part2 中介绍时钟同步反馈机制时，我们提到 Observer 会发送 UNBROADCAST_DL_CLOCK_SYNC_FEEDBACK_MESSAGE 反馈包，其中的 error_ticks 字段反映了目标 Anchor 与其上级 Anchor 之间的全局时钟差异。

Tag 如果也收到了这些反馈包（反馈包虽然是单播给目标 Anchor 的，但 UWB 的广播特性意味着附近的 Tag 也能接收到），可以利用 error_ticks 来修正对应 Anchor 的全局时间估计。具体做法是在生成 listDDOAs 时，将这个误差补偿量叠加到对应 Anchor 的全局时间上。

这样可以使相应 Anchor 的全局时间更加准确，从而提高定位精度。某种意义上，这类似于 GPS 定位中的 RTK（实时动态差分） 技术——通过一个已知位置的参考站来修正定位误差。

工程提示： 并非所有 Tag 都能收到反馈包。反馈包是由 Observer（通常部署在固定位置）通过 UWB 发送给特定 Anchor 的，只有在 Observer 附近的 Tag 才有机会接收到。因此，反馈包修正是一个"锦上添花"的优化，不能作为核心依赖。Tag 即使完全没有收到反馈包，也应该能够正常计算坐标。

坐标质量评估

计算出的坐标必然是近似值而非精确解。一个自然的问题是：计算出的坐标与真实位置相差多少？

在实际运行中，由于多径干扰、信号遮挡等原因，某些时刻计算出的坐标可能与真实位置相差很大（偏差数米甚至数十米）。如果直接将这些"坏坐标"输出给应用系统，会造成混乱。因此我们需要建立一套坐标质量评估机制。

但问题是：我们不知道真实坐标是什么（这正是我们要计算的东西），如何评估计算出的坐标的质量呢？

基本思路——残差分析

大多数情况下，listDDOAs 中的各组距离差数据之间存在一定程度的"矛盾"——这也正是我们只能得到近似解的原因。但"矛盾"的程度是有差异的。

举个例子：假设某个正方形区域内，4 个 Anchor 部署在 4 个角上，Tag 正好在正方形的中心。此时 Tag 到所有 Anchor 的距离相等，理论上所有距离差都应该为 0。如果因为干扰，其中某个 Anchor 的全局时间出现了 10 tick（约 4.7cm）的误差，那么这个 Anchor 与其他 3 个 Anchor 的距离差都会有 4.7cm 的偏移，但其他 3 个 Anchor 相互之间的距离差仍然是 0。

显然，这组数据是"自相矛盾"的。正确的那 3 个 Anchor 其实已经足以确定 Tag 的坐标了，但有误差的第 4 个 Anchor 参与计算后，会把结果往错误的方向拉。

我们可以量化这种矛盾的程度来评估坐标质量：

  1. 用最小二乘法求出最优的 $(x, y, z)$
  2. 将计算出的坐标代回到所有 DDOA 方程中，计算每组方程的残差（理论距离差 vs 实测距离差）
  3. 计算所有残差的均方根（RMS）作为质量评分。RMS 越小，说明各组数据越"和谐"，坐标越可信

对于质量评分低于阈值的坐标，我们直接丢弃不输出。只输出质量较好的坐标给上层应用。

质量评估的局限性： 这种基于残差的评估并非万无一失。有时所有 Anchor 都同时出现了误差，而且这些误差恰好"和谐"地指向同一个错误方向——此时残差很小，我们误以为坐标质量很好，但其实是错误的。这种情况类似于 GPS 中的"共模误差"。不过在实际中，这种巧合概率很低。

进一步的质量评估手段： 除了残差分析外，还可以结合以下手段来提高质量评估的可靠性：

  • 运动学约束：如果 Tag 佩戴在人身上，人的最大移动速度约为 2m/s（步行），不太可能在 37ms 内移动超过 7.5cm。如果计算出的坐标与上一次坐标之间的位移超出合理范围，可以降低当前坐标的可信度。
  • 历史一致性：连续多次计算的坐标如果突然出现大的跳变（而残差指标并没有明显恶化），可能说明某个 Anchor 的同步状态发生了突变。
  • DOP（Dilution of Precision）：类似于 GPS 中的 GDOP/PDOP 概念，可以根据 Anchor 的几何分布计算一个精度衰减因子。DOP 越大，说明 Anchor 几何构型越差（如 Anchor 接近共线），即使残差小也不能过分信赖结果。

3.2.6 USB HID 配置

如前所述，我们使用 WiFi 联网，那么 WiFi SSID 和密码的初始配置就是一个"先有鸡还是先有蛋"的问题——设备还没联网，怎么通过网络告诉它 WiFi 凭证？

ESP-IDF 提供了几种配网方案（SmartConfig、BluFi 等），但我都不太满意——要么需要用户安装手机 APP，要么对网络环境有特殊要求。最终我决定使用 USB HID 来进行 WiFi 设置和管理员密码设置。

为什么选择 USB HID 而不是 USB CDC（串口）？

USB HID（Human Interface Device）设备在 Windows、Linux、macOS 上都免驱动——插上就能用，不需要安装任何驱动程序。这对于部署现场的施工人员来说非常友好。而 USB CDC（Virtual COM Port）虽然传输能力更强，但在 Windows 上可能需要安装额外的 CDC 驱动（虽然 Win10+ 已经自带）。

此外，USB HID 有一个很实用的特性：Feature Report。与键盘/鼠标使用的 Input/Output Report 不同，Feature Report 是通过控制端点（Control Endpoint）传输的，不占用中断端点的带宽，适合用来传输配置数据。PC 端可以使用 hid_get_feature_report() 读取、hid_send_feature_report() 写入，操作非常简洁。

在老的上行 TDOA 项目中，我使用 USB HID 配置 Tag，体验非常好。

USB HID 的数据量限制

然而，USB HID 有一个令人头疼的限制：标准 HID 的单个 Report 最大只有 64 字节。本来，如果能发送大数据包，所有的 Anchor 配置都可以通过 USB HID 完成（与网络配置并行），但 64 字节的限制使得分包传输大参数集变得很复杂。

为什么是 64 字节？ USB HID 规范中，低速设备的中断端点最大包大小为 8 字节，全速设备最大为 64 字节。ESP32-S3 的原生 USB 控制器工作在全速模式（12Mbps），因此中断端点的最大包大小为 64 字节。如果需要更大的数据传输，要么走"Report"分包（增加应用层复杂度——需要自己实现分包/组包协议、序号管理、重传机制等），要么改用 USB Bulk 传输类型（不再是 HID 设备，就失去了免驱动的优势）。

最终，我决定让 USB HID 只负责最基础的配置：管理员名称和密码、WiFi SSID 和密码、IP 地址。其他的高级配置（如 Anchor 坐标、UWB 参数、时钟同步层级等）通过 TCP 网络连接完成——反正有了 WiFi 凭证后设备就能联网了。

即使如此，WiFi SSID 和密码的最大长度也被迫缩减了。标准的 WiFi SSID 最长 32 字节，密码最长 63 字节，但为了适应 64 字节的 Report 容量，我将它们各限制在 21 字节以内。

// HID 配置：管理员信息
typedef struct PACK_ATTRIBUTE __HID_MESSAGE_ADMIN_INFO__ {
	char     admin_name[32];        // 管理员名称
	char     admin_password[32];    // 管理员密码
} HID_MESSAGE_ADMIN_INFO;          // sizeof = 64 字节，恰好一个 Report

// HID 配置：WiFi 网络
typedef struct PACK_ATTRIBUTE __HID_MESSAGE_WIFI_INFO__ {
	uint8_t  wifi_ssid[21];         // WiFi SSID（最长 20 字符 + '\0'）
	uint8_t  wifi_password[21];     // WiFi 密码（最长 20 字符 + '\0'）
	uint8_t  wifi_auto_get_ip;      // 是否使用 DHCP 自动获取 IP
	uint8_t  wifi_ip[4];            // 静态 IP 地址
	uint8_t  wifi_subnet[4];        // 子网掩码
	uint8_t  gateway[4];            // 默认网关
	uint8_t  primary_dns[4];        // 首选 DNS
	uint8_t  secondary_dns[4];      // 备用 DNS
} HID_MESSAGE_WIFI_INFO;           // sizeof = 63 字节

工程提示（WiFi 密码长度限制的影响）： 21 字节的限制意味着 WiFi 密码最长只能是 20 个字符。对于大多数家用/企业 WiFi 场景来说，20 个字符的密码已经足够。但如果遇到使用超长 SSID 或密码的网络，就需要修改路由器设置或者考虑使用分包传输。在实际部署中，建议将 WiFi SSID 和密码都控制在 20 字符以内，并在配置程序中对输入长度做校验。

TinyUSB 库的坑

我使用 TinyUSB 作为 USB HID 的底层驱动。TinyUSB 是一个优秀的开源 USB 协议栈，支持 ESP32-S3 原生 USB，可以节省很多开发时间。

然而我遇到了一个令人困惑的问题：虽然 HID 描述符中声明 Report 大小为 64 字节，但实际上设备端只能使用 63 字节。

当 PC 端调用 hid_get_feature_report() 读取 Feature Report 时：

// PC 端 (使用 hidapi 库)
int n = hid_get_feature_report(pHidDev, buf,
	USB_HID_GET_SET_SLAVE_LIST_REPORT_LENGTH + 1);  // 数据长度 + 1字节 report_id = 65

PC 请求读取 65 字节（64 字节数据 + 1 字节 Report ID）。这里的 +1 是因为 HID 协议的约定：Feature Report 的第一个字节总是 Report ID，后面才是实际数据。所以要获取 64 字节的有效数据，PC 必须请求 65 字节。

但在设备端的回调函数中：

uint16_t tud_hid_get_report_cb(uint8_t instance, uint8_t report_id,
	hid_report_type_t report_type, uint8_t* buffer, uint16_t reqlen)

reqlen 参数总是 63 而不是 64！这意味着设备端最多只能往 buffer 中填充 63 字节的数据，少了 1 字节。

根因分析： 追踪到 TinyUSB 源码 hid_device.c 第 332 行：

uint16_t req_len = tu_min16(request->wLength, CFG_TUD_HID_EP_BUFSIZE);

问题的根源在于这行代码的逻辑：

  1. request->wLength = 65（这是 PC 通过 USB 控制传输请求的总字节数，包含 1 字节 Report ID + 64 字节数据）
  2. CFG_TUD_HID_EP_BUFSIZE 默认定义为 64
  3. tu_min16(65, 64) = 64（取最小值为 64）
  4. TinyUSB 在发送 Feature Report 时，会自动在数据前面加上 1 字节的 Report ID，因此留给用户数据的空间 = 64 - 1 = 63 字节

所以 reqlen 变成了 63，而不是我们期望的 64。这个问题的本质是 CFG_TUD_HID_EP_BUFSIZE 被设计为包含 Report ID 的总缓冲区大小，而非纯用户数据大小。当 PC 请求 65 字节时，这个 64 字节的缓冲区不够用，TinyUSB 就默默地截断了。

解决方案： 将 CFG_TUD_HID_EP_BUFSIZE 修改为 65 或更大的值（比如 128）。但要注意两个陷阱：

  1. 描述符中不能使用修改后的值：HID 描述符中声明的 Report Size 仍然必须保持 64，否则 PC 端枚举设备时会失败。需要在描述符生成代码中把相关的值硬编码为 64，而不是引用 CFG_TUD_HID_EP_BUFSIZE 宏。

  2. 宏的多处定义：CFG_TUD_HID_EP_BUFSIZE 在 TinyUSB 代码中有多处定义（头文件和配置文件），需要确保所有位置都统一修改。建议修改为 128（而不是 65），这样既能避免潜在的字节对齐问题（128 能被 4/8 整除），又留有足够余量。

教训： 使用第三方库时，如果遇到数据传输长度"差一个"的问题，先看看库的源码中是否有硬编码的 buffer size 限制。这类 off-by-one 问题光靠文档和示例代码几乎无法发现，只有深入阅读库的源码才能定位原因。这也说明了一个道理——在嵌入式开发中，使用开源库虽然节省了大量时间，但也要做好"必要时阅读库源码"的心理准备。

USB HID 的 PC 端程序会在后面介绍配置程序时再讨论。

3.2.7 省电

如果我们使用电池供电，必须得考虑如何省电。对于嵌入式系统来说，省电最直接简单的办法就是进入休眠模式——让 MCU 和外围芯片在不需要工作时进入低功耗状态，只在必要时唤醒。

我们分别分析一下 Anchor 和 Tag 的省电相关设计。

3.2.7.1 Anchor 省电

Anchor 的主要功能包括：

  • 接收来自上级 Anchor 的时钟同步包
  • 接收来自观察者（Observer）的反馈包
  • 发送时钟同步包给下级 Anchor 和 Tag

这些收发操作都是周期性的、可预测的。我们可以利用这一特点来制定一个日程表（Schedule），精确安排 MCU 的工作和休眠时间。

日程表的构建方法：

  • 根据历史上收到的时钟同步包的时间规律，推测下一次接收时钟同步包的时间，将这个时间填写到日程表中
  • 根据历史上收到的反馈包的时间规律，推测下一次接收反馈包的时间，将这个时间填写到日程表中
  • 根据自身的发送周期，计算下次发送时钟同步包的时间，将这个时间填写到日程表中

然后 MCU 根据日程表来设置自己的状态：

  • 不处于 UWB 收发时段时，进入休眠状态（ESP32-S3 的 Light Sleep 模式，功耗可降至 mA 级以下）
  • 在接收时间和发送时间到来前，提前一小段时间（例如 2~5ms）唤醒，确保 UWB 收发模块就绪
  • 一旦收发工作完成后，调整日程表，重新计算下一个唤醒时间，并立即进入休眠
  • 如果接收或发射失败（例如超时未收到预期的包），则不进入休眠，保持接收状态等待可能的延迟到达

为什么推测时间是可行的？ 因为 Anchor 之间的时钟同步包是按固定间隔发送的（例如 150ms），而且 Anchor 之间的时钟同步精度很高（纳秒级），所以每个 Anchor 可以非常精确地预测下一个同步包到达的时间。即使考虑无线传播的随机延迟和处理延迟，预测窗口在 ±1ms 内是完全可行的。

DW3000 的功耗管理：

因为 DW3000 如果进入深度休眠（Deep Sleep）状态后唤醒，醒来后需要 100ms 以上的时间进行初始化（重新加载配置、校准振荡器等），这个时间太长了——考虑到同步包间隔只有 150ms，花 100ms 在唤醒初始化上是不可接受的。所以我们不使用 DW3000 的深度休眠模式。

但是为了减小 DW3000 的电力消耗，我们可以利用 DW3000 的 IDLE 状态。DW3000 在 IDLE 状态下仅保持时钟和寄存器内容，不进行射频收发，功耗从发送/接收时的约 100200mA 降至约 1020mA。我们在 MCU 进入休眠状态前，先将 DW3000 切换到 IDLE 状态。

DW3000 的功耗状态对比：

状态典型功耗唤醒时间说明
TX/RX（收发）~100-200mA-射频活跃
IDLE~10-20mA几微秒时钟运行，寄存器保持
IDLE_RC~2-5mA~1ms仅 RC 振荡器运行
Deep Sleep~50-100nA>100ms几乎完全断电

对于 Anchor 来说，IDLE 或 IDLE_RC 是省电与响应速度之间的最佳平衡点。

工程提示（电池供电的 Anchor）： 如果 Anchor 使用电池供电（例如临时部署场景），省电就更加关键。此时可以考虑降低同步包的发送频率（从 150ms 增加到 300ms 甚至 500ms），以牺牲一些定位更新率来换取更长的电池寿命。同时，如果 Anchor 是叶子节点（没有下级 Anchor 需要同步），可以进一步减少发送频率。

3.2.7.2 Tag 省电

Tag 的省电设计比 Anchor 复杂得多。其根本原因在于：Tag 可能处于移动状态，其射频环境是动态变化的。

具体来说，Tag 面临以下挑战：

  • Tag 需要持续锁定多个 Anchor，而由于 Tag 可能在移动，随时有新的 Anchor 进入覆盖范围，也有老的 Anchor 离开覆盖范围
  • 新 Anchor 的时钟同步包何时到来是无法预知的——Tag 事先不知道新 Anchor 的发送节拍
  • 如果某个预期的同步包没有收到，可能是 Tag 已经离开了该 Anchor 的覆盖范围，也可能是因为瞬时干扰或遮挡导致的偶发丢包——两种情况需要不同的处理策略，而 Tag 无法立即区分

这些挑战使得 Tag 不能像 Anchor 那样简单地按日程表休眠/唤醒。

方法一：基于 IMU 的运动检测（推荐）

最优雅的解决方案是为 Tag 配备一个低功耗的 IMU（惯性测量单元），如常见的 MPU6050、LSM6DS3 等。这些传感器支持运动检测中断功能——当检测到加速度变化超过预设阈值时，输出一个中断信号。

工作原理：

  1. 当 IMU 检测到 Tag 静止（加速度持续在某阈值以下），MCU 进入深度休眠，DW3000 进入 Deep Sleep，整体功耗降至微安级别
  2. 当 Tag 开始移动，IMU 触发运动中断，唤醒 MCU 和 DW3000
  3. DW3000 唤醒后重新进行初始化（100ms+），然后 Tag 重新开始锁定周围的 Anchor、计算坐标
  4. 当 IMU 再次检测到 Tag 静止，经过一段延迟（防止频繁切换），再次进入休眠

这种方案对于人员定位特别有效——人在大多数时间是静止的（坐着办公、开会、休息），只有少数时间在走动。粗略估算，如果一个人每天有效走动时间为 2 小时，那么剩下的 22 小时 Tag 都可以处于深度休眠状态，电池寿命可以延长 10 倍以上。

IMU 本身的功耗： 像 LSM6DS3 这类低功耗 IMU，在运动检测模式下的功耗仅约 10~50μA，远低于 DW3000 的任何工作状态。因此，增加 IMU 带来的额外功耗几乎可以忽略不计，但带来的省电效果非常显著。

方法二：无 IMU 的部分省电策略

在没有 IMU 的情况下，仍然可以实现部分省电。核心思想是：将时间分成固定的周期循环，在部分周期中完全开启接收，在其他周期中按日程表休眠/唤醒。

Anchor 的时钟同步包间隔为 150ms，我们以 150ms 为一个周期来规划。以每 3 个周期为一个循环：

周期模式说明
第 1 周期（0~150ms）完全开启RX 持续开启，可以接收到任何 Anchor 的同步包（包括新 Anchor）。根据收到的同步包更新日程表，预测后续周期会收到哪些包及其到达时间
第 2 周期（150~300ms）日程表模式仅在日程表标注的时间段开启 RX，其余时间 MCU 和 DW3000 进入低功耗状态
第 3 周期（300~450ms）日程表模式同第 2 周期

这样的设计实现了以下平衡：

  • 新 Anchor 发现延迟可控：即使新 Anchor 恰好在第 2 或第 3 周期发出第一个同步包，Tag 最多只需等到下一个第 1 周期（最多错过 2 个同步包，延迟约 300ms）就能发现它。对于大多数应用场景，300ms 的发现延迟完全可以接受。
  • 省电效果明显：在第 2、3 周期中，DW3000 只需在预测的接收窗口（通常只有几毫秒）开启 RX，其余时间处于 IDLE 状态。粗略估算，如果每个周期中有效接收时间为 5ms，则 DW3000 的工作比（Duty Cycle）从 100% 降至约 $(150 + 5 + 5) / (150 \times 3) \approx 36\%$。
  • 已跟踪 Anchor 的同步质量不受影响：因为在第 2、3 周期中，已知 Anchor 的同步包仍然能被接收到（通过日程表精确定时）。

可调参数： “每 3 个周期循环一次"中的数字 3 是可配置的。增大到 5 或 10 可以进一步省电，但新 Anchor 的发现延迟也会相应增大。需要根据具体应用场景的要求来权衡。

工程提示： 第 2、3 周期的日程表中，接收窗口应该比预测的到达时间略宽（前后各留 1~2ms 的余量），以应对时钟漂移造成的预测偏差。如果余量设得太小，可能会因为预测不够准确而错过同步包；设得太大则省电效果打折。

3.2.8 OTA

固件能在线升级很重要。在设备长期的运行中，我们可能会发现 Bug，或者需要增加某些功能，如果无法对设备的固件进行在线升级，我们只能把设备拆下来重新刷写固件，这非常麻烦——特别是当 Anchor 安装在天花板或灯杆上时，拆卸和重新安装的成本很高。

ESP-IDF 提供了 OTA（Over-The-Air）功能，我们可以很容易实现在线固件升级。ESP-IDF 的 OTA 功能基于 ESP32-S3 的 分区表（Partition Table） 机制——Flash 中划分两个应用分区（通常叫 ota_0 和 ota_1），新固件写入当前未使用的分区，写入成功后切换引导分区并重启。这种双分区方案保证了升级过程的安全性：如果新固件有问题，可以回滚到旧固件。

但是，ESP-IDF 提供的 OTA 功能中，我们能利用的只有对 Flash 的操作部分（分区管理、写入、校验、切换等）。至于固件的上传通道，ESP-IDF 提供的方案（如 HTTPS OTA、本地 HTTP 服务器等）对我们并不实用——因为我们的设备使用 TCP 自定义协议与配置程序通信，不想引入额外的 HTTP 服务器。因此，固件的传输协议我们自己实现。

OTA 升级的完整流程如下：

sequenceDiagram
	participant PC as PC 配置程序
	participant DEV as 设备 (Anchor/Tag)

	Note over PC: 新固件编译完成
	PC->>PC: 1. 读取固件二进制文件
	PC->>PC: 2. 生成随机 AES-256-GCM IV (12字节)
	PC->>PC: 3. 使用 AES-256-GCM 加密固件<br/>得到密文 + Auth Tag (16字节)
	PC->>PC: 4. 将加密后的固件分块<br/>(每块 1024 字节)

	PC->>DEV: OTA_START 消息<br/>(含总大小、块数、IV、Auth Tag)
	DEV->>DEV: 验证空间是否足够<br/>初始化 OTA 分区
	DEV->>PC: OTA_START_ACK (ready)

	loop 逐块传输
		PC->>DEV: OTA_DATA 消息<br/>(块序号 + 加密数据)
		DEV->>DEV: 解密当前块<br/>校验 AES GCM Tag/AAD<br/>写入 Flash OTA 分区
		DEV->>PC: OTA_DATA_ACK (块序号 + 状态)
	end

	PC->>DEV: OTA_FINISH 消息
	DEV->>DEV: 验证完整固件的完整性<br/>切换 OTA 引导分区
	DEV->>PC: OTA_FINISH_ACK (success)
	DEV->>DEV: 重启设备，运行新固件

	Note over DEV: 新固件首次启动后<br/>进行自检，确认正常后<br/>标记当前分区为有效<br/>(否则下次重启回滚到旧固件)

OTA 有几个要点需要重点说明：

固件保护

作为商业产品，我们要保护固件不被非法获取，也不允许运行被非法修改过的固件。也就是说，需要同时保证固件的机密性（不泄露）和完整性（不被篡改）。

在 OTA 的过程中，固件以加密形态在网络上传输——用户（或攻击者）能接触到的只是加密后的密文，无法从中还原出原始固件。加密算法使用 AES-256-GCM（Galois/Counter Mode）。

为什么选择 AES-256-GCM？

AES-256-GCM 是一种**认证加密（Authenticated Encryption）**算法，它在一次操作中同时提供：

  • 机密性：AES-256 加密，密钥长度 256 位，安全级别极高
  • 完整性：GCM 模式会生成一个 16 字节的 Authentication Tag（认证标签），任何对密文或附加认证数据（AAD）的篡改都会导致认证失败
  • 防重放：每次加密使用不同的 IV（Initialization Vector，初始化向量），确保相同的明文在不同次加密中产生不同的密文

相比于先加密再做 HMAC 校验（Encrypt-then-MAC）的两步方案，AES-256-GCM 更加高效，且避免了两步方案中可能出现的实现错误（如先验证再解密的顺序错误）。

ESP32-S3 的硬件 AES 加速器直接支持 AES-GCM 模式，加解密速度可达数十 MB/s，不会成为 OTA 传输的瓶颈。

AES-256-GCM 加密的工作流程：

  1. 密钥管理：AES-256 密钥（32 字节）存储在设备端的**加密 NVS（Non-Volatile Storage）**中。ESP-IDF 提供了 NVS 加密功能，NVS 分区本身也是加密存储的，外部无法通过读取 Flash 芯片来获取密钥。PC 配置程序中持有相同的密钥。
  2. 加密过程（PC 端）：生成随机 12 字节 IV → 使用 AES-256-GCM 对固件明文进行加密 → 得到密文和 16 字节 Authentication Tag → 将 IV、密文、Auth Tag 一起发送给设备
  3. 解密过程（设备端）：使用存储在 NVS 中的密钥和收到的 IV 进行解密 → 同时验证 Authentication Tag → 如果验证通过，说明数据完整且未被篡改，写入 Flash；如果验证失败，丢弃数据并报错

ESP32-S3 的 Flash 加密：

除了 OTA 传输过程中的加密外，ESP32-S3 还提供了透明 Flash 加密（Flash Encryption）功能。启用该功能后，ESP32-S3 会在写入 Flash 时自动加密，读取 Flash 时自动解密——整个过程对应用程序完全透明。这意味着即使有人物理拆解设备、用编程器直接读取 Flash 芯片上的数据，得到的也是加密后的密文，无法还原出有意义的固件。

Flash 加密使用的密钥存储在 ESP32-S3 的 eFuse（一次性可编程熔丝）中，一旦写入就无法读取或修改（即使通过 JTAG 调试接口也不行）。这提供了硬件级别的固件保护。

工程提示（Flash 加密的注意事项）：

  • Flash 加密在开发模式下可以重新刷写（方便调试），但在发布模式下一旦启用就不可逆——eFuse 会被永久锁定。建议在开发阶段使用开发模式，量产时切换到发布模式。
  • 启用 Flash 加密后，OTA 写入的固件会被自动加密存储。但 OTA 写入的数据本身应该是明文（已经解密后的），ESP32-S3 的 Flash 控制器会在写入时自动进行加密。不要将 OTA 传输过程中的 AES-GCM 加密与 Flash 存储的加密混淆——这是两层独立的加密。

固件传送

加密后的固件被嵌入到桌面配置程序中（也可以在升级时从文件系统加载）。我们定义了设备与桌面配置程序之间的 OTA 交互协议，包含以下消息类型：

消息类型方向说明
OTA_STARTPC → 设备通知设备准备接收新固件，携带固件总大小、分块数、IV、Auth Tag 等元信息
OTA_START_ACK设备 → PC设备确认准备就绪（或报告空间不足等错误）
OTA_DATAPC → 设备传送一块加密的固件数据，携带块序号
OTA_DATA_ACK设备 → PC确认收到并成功处理该块（或报告解密/校验失败）
OTA_FINISHPC → 设备所有块传输完毕，通知设备完成 OTA
OTA_FINISH_ACK设备 → PC设备确认 OTA 完成，准备重启

每块数据传输后都需要等待 ACK 确认，确保加密后的固件能分块正确地传送到设备。如果某块传输失败（如网络中断），PC 端可以从断点处重新发送。

工程提示（OTA 的可靠性设计）：

  • 每个 OTA_DATA 消息中包含块序号，设备端会校验序号的连续性。如果收到乱序的块（可能是网络丢包后重传），设备会请求重发。
  • 设备在 OTA_FINISH 后不会立即标记新分区为有效。新固件首次启动后，需要执行一系列自检（如验证关键外设能正常工作），确认一切正常后才调用 esp_ota_mark_app_valid_cancel_rollback() 标记当前分区为有效。如果新固件启动后崩溃或自检失败，ESP-IDF 的回滚机制会在下次重启时自动恢复到旧固件。

3.2.9 固件中的其他功能

3.2.9.1 LED 指示灯

设备中设计有一个 WS2812 RGB LED 作为设备指示灯。这个指示灯的主要用途是现场设备辨识。

场景举例：Anchor 在现场安装完毕后，如果调试时发现定位不对或输出混乱，很有可能是 Anchor 安装位置搞混了——比如应该安装在 A 处的 Anchor 被装到了 B 处。现场的 Anchor 通常安装在高处（天花板、灯杆），外观完全一样，不爬上去看铭牌根本分不清谁是谁。

有了 WS2812 指示灯，只需在 PC 端配置程序中点击某个 Anchor，让它点亮特定颜色的 LED（比如绿色闪烁），然后在现场抬头看看哪个设备在闪，就可以确认物理位置与逻辑 ID 的对应关系。

ESP-IDF 中的驱动方式： ESP-IDF 提供了 led_strip 组件，可以通过 RMT（Remote Control Transceiver） 外设来驱动 WS2812。RMT 是 ESP32 特有的一个外设，原本设计用于红外遥控器协议的编解码，但因为它能精确控制脉冲时序，被广泛用于驱动 WS2812 这类需要精确时序的 LED。

工程提示： WS2812 的数据协议对时序要求非常严格（0/1 码的高低电平时间窗口仅为几百纳秒），如果直接用 GPIO 翻转来模拟时序，很容易被中断打断导致时序不准。使用 RMT 外设可以完全由硬件控制时序，不受软件中断的影响。

3.2.9.2 按钮

我选用的 Anchor 外壳上有一个按钮孔位。我为这个按钮设计了 3 种触发模式：

触发模式定义计划功能
短按按下后在 1 秒内松开触发 Debug 信息输出（如各任务栈使用情况）
长按按下后持续 2 秒以上再松开恢复出厂默认设置
按着开机上电前按钮已处于按下状态进入特殊模式（如 TWR 自动定位模式）

一些具体应用场景：

  • 恢复出厂设置：配置参数搞乱后，用户可以长按按钮恢复默认值，然后重新配置需要修改的参数。
  • TWR 自动定位：如果已经有数个 Anchor 的坐标被手动配置好了，新 Anchor 可以通过 TWR（Two-Way Ranging）测量自己到已知 Anchor 的距离，再用三点定位法自动计算自己的坐标。这个功能也可以通过配置程序远程触发。
  • 软复位：对于内置锂电池的 Anchor（无法直接断电），可以通过按钮触发软件复位。

工程提示（按钮消抖）： 机械按钮在按下和松开的瞬间会产生接触抖动（Bounce），通常持续 5~20ms。如果不做消抖处理，一次按下可能会被软件识别为多次按下。常见的消抖方法有硬件消抖（RC 低通滤波）和软件消抖（检测到电平变化后等待一段时间再确认）。在本项目中使用软件消抖，延迟设为 20ms。

3.2.9.3 显示屏

Anchor 是否需要显示屏，取决于应用场景。在大多数场景下（Anchor 长期安装在天花板或灯杆上），显示屏毫无用处，只增加成本。但在某些需要临时部署或电池供电的场景下（需要查看电池电量、设备状态等），显示屏会有价值。

Tag 带显示屏则比较有意义：可以显示计算出的实时坐标、控制中心发来的文字通知等。目前我的开发重心在定位核心功能上，显示屏功能计划在后续版本中加入。

3.2.9.4 麦克风和喇叭

Tag 可以利用 ESP32-S3 的 I2S 接口连接麦克风和喇叭，通过 WiFi 实现类似对讲机的语音通讯功能。目前尚未实现，留作后续扩展。

3.2.10 固件设计中可能遇到的问题

在固件开发过程中，我遇到了一些"坑”，记录在这里供参考。

3.2.10.1 字节对齐

对于 C/C++ 程序员来说，字节对齐是个"老生常谈"的问题。但在 ESP32 上，它可能以一种意想不到的方式出现。以我的经验来说，字节对齐问题通常有以下几种表现：

平台表现
Windows (MSVC)编译器自动插入 padding，程序正常运行但 sizeof() 比预期大
STM32 (ARM + IAR/GCC)编译器可能发出警告；如果用 packed 属性，ARM Cortex-M 可以处理未对齐访问（有轻微性能损失）
ESP32 (Xtensa)直接 Crash！ Xtensa 指令集不支持未对齐内存访问，硬件会触发 LoadStoreAlignmentCause 异常

这次我遇到了一个更隐蔽的问题——代码运行看上去正常，但读和写访问的是不同的地址。

typedef struct __tag_ddoa___ {
	EUI64 aId;      // 偏移 0,  大小 8
	float ax;       // 偏移 8,  大小 4
	float ay;       // 偏移 12, 大小 4
	float az;       // 偏移 16, 大小 4

	EUI64 bId;      // 偏移 20, 大小 8  ← 问题在这里！
	float bx;       // 偏移 28, 大小 4
	float by;
	float bz;

	float deltaDistance;
} DDOA;

开始没有加 __attribute__((packed))。在生成 listDDOAs 的过程中，给 bId 赋值后立即读回，发现 bId 之后的字段全部不正确。

dump 内存数据后发现：编译器在写入 bId 时将它放在了偏移 20 的位置（紧接在 az 之后），但在读取时却从偏移 24 开始读（因为编译器在另一个编译单元中认为 EUI64 应该 8 字节对齐，在 az 和 bId 之间插入了 4 字节的 padding）。

根本原因： 同一个结构体在不同的编译单元中，由于编译优化级别或编译选项的微小差异，编译器可能做出不同的对齐决策。如果没有显式的 packed 属性，结构体的内存布局就依赖于编译器的默认行为——而这个行为可能不一致。

这个问题特别隐蔽，因为在大多数情况下，同一个编译器对同一个结构体的处理是一致的。只有在以下条件同时满足时才会出问题：（1）不同编译单元使用了不同的编译选项；（2）结构体中存在需要对齐的大类型（如 8 字节的 uint64_t）出现在非对齐位置。

解决方法：在结构体定义时加上 __attribute__((packed))。当然这会带来一些性能开销（未对齐访问在 Xtensa 上需要软件模拟），另一种做法是手动调整字段顺序来保证自然对齐——例如把所有 8 字节字段放在前面，4 字节字段紧随其后。

工程提示（推荐的字段排列原则）： 按照字段大小从大到小排列——先放 8 字节字段，再放 4 字节字段，再放 2 字节字段，最后放 1 字节字段。这样可以在不使用 packed 的情况下最小化 padding，同时保证自然对齐。如果实在无法调整字段顺序（例如需要与网络协议或文件格式的字段顺序保持一致），则使用 packed + 手动对齐读写。

3.2.10.2 ISR 中的日志打印

在 ESP-IDF 中，ISR 中不能使用 ESP_LOGI() 等常规日志函数——因为这些函数内部会尝试获取互斥锁、分配内存等操作，这些在 ISR 上下文中是被禁止的。

ESP-IDF 提供了 ESP_DRAM_LOGI() 系列函数专门用于 ISR 中的日志输出。但是要注意：ESP_DRAM_LOGI() 不支持 int64_t / uint64_t 类型的格式化输出。传入 64 位整数参数时，它只会打印低 32 位，高 32 位被丢弃——而且不会有任何警告！

这在调试时钟同步代码时特别坑人——DW3000 的 40-bit 时间戳存储在 uint64_t 变量中，如果你在 ISR 中打印这个时间戳来调试却总是看到一个奇怪的小数字，可能就是因为高位被截断了。

解决方法： 在 ISR 中将 64 位值拆分为两个 32 位部分分别打印，或者将调试数据通过队列发送到主任务中再使用 ESP_LOGI() 打印。推荐后者——ISR 中应尽量减少执行时间，日志打印（即使是 DRAM 版本）也会占用宝贵的 ISR 时间。使用 xQueueSendFromISR() 将数据发送到日志任务，是更干净的做法。

工程提示： 在 ESP-IDF 中，ISR 只能调用以 FromISR 结尾的 FreeRTOS API（如 xQueueSendFromISR()、xSemaphoreGiveFromISR()），使用非 FromISR 版本会导致不可预测的行为甚至系统崩溃。ESP-IDF 在 debug 构建中会通过断言检查这类错误，建议开发阶段始终使用 debug 构建。

3.3 配置程序

无论是 Anchor 还是 Tag，都有很多参数需要配置——UWB 通信参数（频道、速率、前导码长度等）、Anchor 坐标、时钟同步层级、观察者指定、WiFi 凭证等等。

配置程序的实现方案大致有三种：

方案优点缺点
设备内置 WebServer无需安装任何软件，浏览器即可配置WiFi 以太网首次配网的"鸡蛋问题"；页面存在 Flash 中占空间；JSON 编解码耗 RAM；无法批量配置
手机 APP人人有手机，随身携带方便屏幕小、操作不便；需要同时适配 iOS/Android；开发维护成本高
PC 桌面程序屏幕大、操作方便；支持批量配置；可集成 USB HID 功能需要到现场带电脑

在老的上行 TDOA 项目中，我使用 Delphi 开发配置程序。选择 Delphi 当时主要是因为开发桌面程序简单且我比较熟悉。但后来很多客户反馈：现在会 Delphi 的开发人员越来越少，程序维护困难。再加上整个系统的固件都是 C/C++ 编写的，很多基础代码（如消息定义、结构体定义等）无法与 Delphi 共用，需要单独维护一份——增加了工作量也容易出现不一致。

新项目我决定使用 C++ + Qt 来开发配置程序。好处是：

  • C++ 可以直接共用固件中的消息定义头文件
  • Qt 跨平台（Windows/Linux/macOS）
  • Qt 生态成熟，图形界面开发效率高

通信协议——二进制 vs JSON

配置程序与 Anchor/Tag 之间的数据交换格式，使用二进制还是 JSON，各有利弊：

二进制格式JSON 格式
优点可共用固件中的结构体定义；传输效率高；解析速度快向前/向后兼容性好（字段可增减）；人类可读，便于调试
缺点修改字段时兼容性差（新旧版本可能不兼容）设备端需要 JSON 编解码库，占用 Flash/RAM；编解码过程繁琐

经过权衡，我最终选择了二进制格式。主要原因是 ESP32-S3 的 RAM 虽然不小，但一旦引入成熟的 JSON 库（如 cJSON），还是会占用不少资源。而二进制格式只需在 PC 和设备两端使用相同的结构体定义即可——这正是 C++ 配置程序的优势之一。

兼容性问题的应对策略： 每种消息都有一个 message_type 字段和隐含的版本号。当未来需要修改消息结构时，可以：（1）保留老的消息类型不变，增加新的消息类型；（2）在消息头中加入版本号字段，收发两端根据版本号决定如何解析。

工程提示（二进制协议的陷阱）： 使用二进制协议时要特别注意**字节序（Endianness）**问题。ESP32-S3 和大多数 PC（x86/x64）都是小端序（Little-Endian），所以在本项目中不需要做字节序转换。但如果将来需要适配大端序平台（如某些 ARM 处理器），就需要在协议层统一字节序。另外，前面提到的字节对齐问题在二进制协议中同样存在——PC 端和设备端必须使用相同的 packed 属性来确保结构体布局一致。

配置程序的基本架构

graph LR
	subgraph "PC 端配置程序"
		UDP["UDP 广播<br/>设备发现"] --> TCP["TCP Client<br/>与设备建立连接"]
		TCP --> CONFIG["参数读写<br/>(二进制消息)"]
		HID["USB HID<br/>(hidapi 库)"] --> WIFI_CONFIG["WiFi/管理员配置"]
	end

	subgraph "设备端 (Anchor/Tag)"
		TCP_SVR["TCP Server"] --> FW["固件配置模块"]
		HID_DEV["USB HID Device<br/>(TinyUSB)"] --> FW
	end

	UDP -.-> TCP_SVR
	TCP --> TCP_SVR
	HID --> HID_DEV
  1. 设备发现：配置程序启动后通过 UDP 广播发送发现请求，局域网内的所有 Anchor/Tag 收到后回复自己的 IP 地址和基本信息。
  2. 建立连接：配置程序作为 TCP Client，与发现的设备建立 TCP 长连接。通过这个连接获取设备的完整配置并进行修改。
  3. USB HID 配置：对于尚未联网的设备，通过 USB 连接使用 HID 协议配置 WiFi 凭证和管理员密码。我使用 hidapi 开源库来实现 PC 端的 HID 读写操作。

设备插拔检测： hidapi 库本身不提供 USB 设备插拔的事件通知，只能通过轮询 hid_enumerate() 来检测。为了更好的用户体验，在 Windows 版本中我改为监听 Windows 的 WM_DEVICECHANGE 消息来实时检测 USB 设备的插拔。Linux 版本目前尚未适配，如有需要可以通过 udev 机制实现类似功能。

日常对设备的配置使用网络。设备作为 TCP Server，配置程序作为 TCP Client。在建立起 TCP 连接之前，设备与配置程序之间会使用 UDP 广播包进行交互，让配置程序可以发现局域网中的设备（获取 IP 地址），然后配置程序会主动向设备发起 TCP 连接。

配置程序的界面如下： 设备列表 上图是主界面，设备列表中的列可以让用户自定义。

设备基本信息 上图是设备基本信息

网络设置 上图是网络设置

UWB设置 上图是 UWB 设置

时钟同步设置 上图是时钟同步设置

3.4 消息汇聚程序以及前端地图和数据可视化

3.4.1 消息汇聚服务器

这是一个辅助程序，作为 PC 上的后台服务运行。

当 Tag 计算出坐标后，要么仅供 Tag 自身使用（如在本地显示屏上显示），要么上报给应用系统。如果系统中有很多 Tag，每个 Tag 都各自直接与应用系统对接，对应用系统的开发者来说会非常繁琐（需要管理大量连接）。

因此我编写了一个消息汇聚服务程序——它充当中间层，统一收集所有 Tag 的数据，再对外提供标准化的接口。

graph LR
	T1["Tag 1"] -- "TCP" --> AGG["消息汇聚服务器<br/>(Node.js)"]
	T2["Tag 2"] -- "TCP" --> AGG
	T3["Tag 3"] -- "TCP" --> AGG
	A0["Anchor A0"] -- "TCP" --> AGG

	AGG -- "WebSocket" --> MAP["前端地图<br/>(浏览器)"]
	AGG -- "WebSocket" --> VIS["数据可视化<br/>(浏览器)"]
	AGG -- "WebSocket/TCP" --> APP["第三方应用系统"]

这个程序同时作为 TCP Server（接受来自各个 Anchor/Tag 的连接）和 WebSocket Server（接受来自浏览器和应用程序的连接）。它把来自 TCP Client 的消息经过整理后转发给 WebSocket Client。

为什么选择 Node.js？ 消息汇聚服务器的核心工作是 I/O 密集型（接收 TCP 消息、转发 WebSocket 消息），不涉及复杂的计算。Node.js 的异步事件驱动模型非常适合这类场景——用单线程就能高效地管理数百个并发连接。另外，Node.js 的 ws 库提供了简洁高效的 WebSocket 实现，开发非常快速。

3.4.2 前端地图

前端地图使用 Node.js 开发，使用 OpenLayers 作为地图前端组件，以 OpenStreetMap 作为底图。

地图前端在浏览器中加载后，会与消息汇聚程序建立 WebSocket 连接，实时接收来自汇聚程序的消息（主要是 Tag 坐标和 Anchor 坐标），把 Anchor 和 Tag 的位置实时显示在地图上。

前端地图

这个地图主要用来看定位效果。我加了历史轨迹，让 Tag 拖一个小尾巴，可以直观地看到定位精度。红色是计算出来的原始坐标，绿色是经过 Kalman 滤波后的坐标。

前端地图

从上图来看，大部分情况下精度在 20cm 以内。

实际部署提示： 对于室内定位场景，OpenStreetMap 的底图通常不包含室内楼层平面图。实际项目中可以将建筑的 CAD 平面图导出为图片，作为自定义图层叠加在地图上。OpenLayers 支持通过 ImageLayer 来加载自定义的楼层底图。

关于 Kalman 滤波后处理： 地图上显示的绿色轨迹使用了一个简单的二维 Kalman 滤波器来平滑坐标输出。这个后处理 Kalman 滤波器与固件中用于时钟同步的 Kalman 滤波器是完全独立的——前者是在坐标域（$x$, $y$）上工作，用于平滑定位抖动；后者是在时间域上工作，用于跟踪时钟偏移和漂移。在坐标域上的 Kalman 滤波可以有效抑制单次定位结果的随机波动，代价是引入少量延迟（跟踪响应会稍慢于真实运动）。

3.4.3 数据可视化

数据可视化使用 HTML + JavaScript 开发，是一个简单但非常实用的调试页面。

该页面加载到浏览器后，同样与消息汇聚程序建立 WebSocket 连接，但接收的是时钟同步相关 的诊断消息（如各 Anchor 的同步误差、$factor$ 值变化、反馈量等），把时钟同步的状态以曲线方式实时显示。这使我们可以直观地看到：

  • 各个 Anchor 的同步误差随时间的变化趋势
  • 反馈机制是否正常工作（误差是否在逐步收敛）
  • 是否存在周期性的干扰或异常跳变

数据可视化

调试利器： 在开发过程中，这个可视化页面帮了大忙。很多时钟同步的问题（如 Kalman 滤波器参数不合适导致的振荡）仅凭日志输出很难发现，但在曲线图上一眼就能看出来。强烈建议任何 TDOA 系统的开发者都建设类似的可视化工具。

工程提示（可视化工具的价值）： 对于 TDOA 定位系统这类"中间过程不可直接观测"的项目，数据可视化工具是不可或缺的。相比于翻阅成千上万行日志，一张实时曲线图能让你在几秒钟内发现问题所在。建议在项目初期就搭建基本的可视化框架，而不是等到系统开发完成后才"锦上添花"。

如果你正在考虑自己动手实现一套下行 TDOA 定位系统，希望本文能为你提供一个完整的技术路线图和实践参考。下行 TDOA 的核心难点在于时钟同步的精度和稳定性——这在文中做了详细的讨论，建议重点研读。一旦时钟同步做好了，坐标计算反而是相对直接的部分。

在开发 TDOA 定位系统的过程中，我最大的体会是：永远不要低估工程细节的复杂性。 原理看起来很简单——“时间差乘以光速等于距离差”——但从原理到能稳定运行的产品之间，有大量的工程问题需要解决：40-bit 溢出处理、字节对齐、ISR 约束、TinyUSB 的 buffer size 陷阱……这些问题在任何教科书和论文中都不会提到，只有在真正的工程实践中才会遇到。希望本文中记录的这些"坑"能帮助后来者少走弯路。

天迹软件

©︎Copyright SkyTrack Software Studio, 2025-2026.