3. 测距原理实战：TOF、TDOA、AOA 原理与代码实现
说到 UWB 定位，绕不开的就是测距原理。很多初学者一上来就问我：「老师，UWB 到底怎么算出距离的？」
其实核心就三种方法：TOF（飞行时间测距）、TDOA（到达时间差）、AOA（到达角度）。这三种方法各有各的脾气，我在项目里都踩过坑，今天咱们一个一个掰开揉碎了讲。
一句话总结：TOF 是测时间算距离，TDOA 是测时间差算位置，AOA 是测角度算方向。实际项目中往往组合使用。
3.1 TOF：飞行时间测距
TOF 的原理特别简单——测出信号从 A 到 B 的飞行时间，乘以光速，就是距离。公式就是 d = c × t。
但问题来了：你怎么知道信号是什么时候发出去的？
我刚开始做 UWB 时，以为只要发一个脉冲、收一个脉冲，算时间差就行了。结果发现，两个设备的时钟根本不同步，误差大得离谱。
3.1.1 单边双向测距（SS-TWR）
SS-TWR 的思路是：设备 A 发一个包，设备 B 收到后立刻回复，A 记录从发出到收到回复的总时间 T_round，再减去 B 的处理时间 T_reply，剩下的就是两倍的飞行时间。
// SS-TWR 伪代码示例
uint64_t t1 = get_sys_time(); // A 发出时间戳
send_poll();
// B 收到后回复
uint64_t t2 = get_sys_time(); // B 收到时间戳
send_response(t2);
// A 收到回复
uint64_t t3 = get_sys_time(); // A 收到时间戳
// 计算飞行时间
uint64_t T_round = t3 - t1;
uint64_t T_reply = t2 - t1; // 实际需要从回复包中读取
double T_prop = (T_round - T_reply) / 2.0;
double distance = T_prop * SPEED_OF_LIGHT;注意：SS-TWR 假设 T_reply 是精确已知的。但实际中 B 的时钟偏移会导致误差。我曾经在一个项目中用 SS-TWR，测距误差达到了 ±30cm，后来换成 DS-TWR 才降到 ±5cm。
3.1.2 双边双向测距（DS-TWR）
DS-TWR 多了一次来回通信，能抵消时钟偏移的影响。说白了就是多问一次，把误差平均掉。
// DS-TWR 核心计算
// 三次消息交换：Poll → Response → Final
uint64_t t1, t2, t3, t4;
// t1: A 发 Poll
// t2: B 收 Poll
// t3: B 发 Response
// t4: A 收 Response
// t5: A 发 Final
// t6: B 收 Final
uint64_t T_round1 = t4 - t1;
uint64_t T_reply1 = t3 - t2;
uint64_t T_round2 = t6 - t3;
uint64_t T_reply2 = t5 - t4;
// 飞行时间计算公式
double T_prop = (T_round1 * T_round2 - T_reply1 * T_reply2)
/ (T_round1 + T_round2 + T_reply1 + T_reply2);
double distance = T_prop * SPEED_OF_LIGHT;我的经验：DS-TWR 虽然多了一次通信，但精度提升非常明显。在室内环境下，配合 Decawave DW1000 芯片，我实测过 50 米范围内误差能控制在 ±10cm 以内。
3.2 TDOA：到达时间差
TDOA 不直接测距离，而是测「标签到两个基站的时间差」。你想想看，如果标签到基站 A 比到基站 B 快了 1 纳秒，那标签肯定离 A 更近 30 厘米。
TDOA 的核心优势是：标签只需要发一次信号，所有基站同时收。这样标签功耗极低，适合电池供电的场景。
3.2.1 时间同步是命门
TDOA 最大的坑就是时间同步。基站之间必须精确同步，误差 1 纳秒就会导致 30 厘米的定位偏差。
| 同步方式 | 精度 | 成本 | 适用场景 |
|---|---|---|---|
| 有线时钟同步 | ±0.1 ns | 高 | 固定基站、高精度场景 |
| 无线时钟同步 | ±1 ns | 中 | 灵活部署、中等精度 |
| GPS 授时同步 | ±10 ns | 低 | 室外大范围场景 |
我记得有一次做工厂人员定位，用了无线同步方案。结果车间里有大型金属设备，多径反射严重，同步误差直接飙到 5 纳秒，定位结果完全没法看。后来换成有线同步，问题才解决。
3.2.2 TDOA 定位计算
有了时间差，怎么算位置？本质上是解双曲线方程组。每个时间差对应一条双曲线，两条双曲线的交点就是标签位置。
// TDOA 定位简化示例（假设二维平面）
// 基站坐标：A(0,0), B(10,0), C(0,10)
// 测得时间差：dT_AB = tA - tB, dT_AC = tA - tC
double d_AB = dT_AB * SPEED_OF_LIGHT; // 距离差
double d_AC = dT_AC * SPEED_OF_LIGHT;
// 解双曲线方程组（这里用牛顿迭代法简化示意）
double x = 5.0, y = 5.0; // 初始猜测
for (int i = 0; i < 10; i++) {
// 计算雅可比矩阵并更新位置
// 实际项目中建议用 Chan 算法或 Taylor 级数展开
}关键点：TDOA 至少需要 3 个基站才能做二维定位，4 个基站可以做三维定位。基站数量越多，定位精度越高，但计算复杂度也越大。
3.3 AOA：到达角度
AOA 的思路不一样——它不测时间，而是测信号来的方向。通过天线阵列接收信号的相位差，计算出角度。
说白了，就是利用多个天线接收同一个信号，因为到达时间不同，相位就会有差异。这个差异就包含了角度信息。
3.3.1 相位差与角度关系
假设两个天线间距为 d，信号波长为 λ，到达角度为 θ，那么相位差 Δφ 满足：
Δφ = 2π × d × sin(θ) / λ
// 反过来，已知相位差求角度
θ = arcsin(Δφ × λ / (2π × d))注意模糊角问题：当天线间距 d > λ/2 时，会出现角度模糊。我遇到过有人为了追求高精度把天线间距拉大，结果角度计算出来全是错的。记住：d ≤ λ/2 是硬约束。
3.3.2 AOA 的工程实现
实际项目中，AOA 通常和 TOF 或 TDOA 配合使用。比如用 TOF 测距离，用 AOA 测方向，两者结合就能得到精确位置。
// AOA 角度计算示例（双天线）
// 假设天线间距 d = 0.05m，信号频率 4GHz，波长 λ = 0.075m
double d = 0.05;
double lambda = 0.075;
double phase_diff = get_phase_difference(); // 从 I/Q 数据中提取
// 计算角度
double sin_theta = phase_diff * lambda / (2 * M_PI * d);
double theta = asin(sin_theta); // 弧度
double theta_deg = theta * 180.0 / M_PI; // 转角度我的建议：AOA 对天线布局非常敏感。我做过一个项目，天线阵列的 PCB 走线长度不一致，导致相位偏移，角度误差达到了 10 度。后来强制要求所有天线走线等长，误差才降到 2 度以内。
3.4 三种方法对比与选型
到底用哪种方法？没有标准答案，得看你的场景。
| 特性 | TOF | TDOA | AOA |
|---|---|---|---|
| 精度 | ±10~30 cm | ±10~50 cm | ±2°~5° |
| 标签功耗 | 中（需收发） | 低（只发不收） | 低（只发不收） |
| 基站复杂度 | 低 | 高（需同步） | 高（天线阵列） |
| 适用场景 | 点对点测距 | 多标签定位 | 方向辅助 |
| 抗多径能力 | 中 | 中 | 弱 |
我个人习惯是：小范围高精度用 TOF，大范围多标签用 TDOA，需要方向信息时加 AOA 辅助。没有银弹，只有最适合的方案。
实战建议：如果你刚开始做 UWB 定位，先从 TOF 入手。它原理简单、调试方便，能帮你快速理解 UWB 的底层特性。等把 TOF 吃透了，再上 TDOA 和 AOA，会少走很多弯路。