UWB 定位实现

2026 年 10 月 21 日定位

本文以一个标签、四个固定基站为例，串起测距、数据上报和二维坐标解算。代码使用模拟距离验证计算流程，接入设备后用同一轮实测距离替换输入。

1. 硬件与坐标规划

需要一个可移动标签、四个支持 TWR 的 UWB 基站，以及一台接收测距结果的上位机。板卡之间使用匹配的测距固件、信道与帧参数，标签和基站分别分配唯一 ID。

以下坐标单位均为米，原点放在场地一角，X、Y 方向按现场固定：

基站	X	Y
A0	0	0
A1	6	0
A2	6	4
A3	0	4

本例让标签和基站处于同一高度，因此实测距离可直接用于二维平面解算。二维定位至少需要三个不共线的基站，第四个基站增加一条观测约束。

现场应尽量让定位区域落在基站围成的范围内，测量并记录每个基站的实际坐标。需要解算高度时，应采用三维模型与不共面的基站布置。

2. 从双向测距得到距离

TWR 通过消息往返和芯片时间戳估计无线信号的飞行时间，再按 距离 = 传播速度 × 飞行时间 换算。

采用 Poll、Response、Final 的双边测距时，定义：

Ra：标签发送 Poll 到收到 Response 的时间间隔
Da：标签收到 Response 到发送 Final 的时间间隔
Rb：基站发送 Response 到收到 Final 的时间间隔
Db：基站收到 Poll 到发送 Response 的时间间隔

ToF = (Ra × Rb - Da × Db) / (Ra + Rb + Da + Db)

间隔取自各设备本地的硬件时间戳，并按芯片的时间单位换算，不能用上位机接收串口数据的时间代替。双边测距的原理和时间戳关系可参考 Qorvo / Decawave APS013。

先跑通一个标签与一个基站之间的测距，再扩展为标签按时隙依次与各基站交换数据。每一轮只汇总同一个标签、同一个轮次的距离。

3. 约定上报格式

上位机示例使用一行一个 JSON 对象。下面是本教程约定的输入协议；设备或串口适配程序负责把厂商原始报文转换成这个格式：

{"tag_id":"T1","round_id":1,"ranges":{"A0":2.5,"A1":4.2720018727,"A2":4.7169905660,"A3":3.2015621187}}

tag_id：标签标识。
round_id：测距轮次，用于区分不同时刻的观测。
ranges：基站 ID 到距离的映射，单位统一为米。

这组数据对应模拟位置 (2.0, 1.5)。某轮只有两个有效基站时应跳过定位，不要用上一轮的数据补齐。

4. 二维位置解算

标签位置为 (x, y)，基站位置为 (xi, yi)，测得距离为 ri，则每个基站提供一个距离约束：

(x - xi)² + (y - yi)² = ri²

选第一个基站为参考，把其他方程减去参考方程，二次项会消掉。以参考基站为局部原点，得到：

dx = xi - x0
dy = yi - y0
u = x - x0
v = y - y0

2 × dx × u + 2 × dy × v = r0² - ri² + dx² + dy²

用多条线性方程求最小二乘解，再把局部坐标加回参考基站坐标。核心实现如下，只依赖 Python 标准库：

import math


def locate_2d(anchors, ranges):
    """Return (x, y, range_rmse) in metres; anchors and tag share one height."""
    if not isinstance(ranges, dict) or len(ranges) < 3:
        raise ValueError("至少需要三个基站的距离")
    points = []
    for anchor_id, raw_range in ranges.items():
        if anchor_id not in anchors:
            raise ValueError(f"未知基站：{anchor_id}")
        if isinstance(raw_range, bool):
            raise ValueError("距离必须为正数")
        distance = float(raw_range)
        coordinate = tuple(float(v) for v in anchors[anchor_id])
        if len(coordinate) != 2 or not all(math.isfinite(v) for v in coordinate):
            raise ValueError("基站坐标必须是两个有限数值")
        if not math.isfinite(distance) or distance <= 0:
            raise ValueError("距离必须是有限正数")
        points.append((*coordinate, distance))

    x0, y0, r0 = points[0]
    rows = []
    for xi, yi, ri in points[1:]:
        dx, dy = xi - x0, yi - y0
        rows.append((2 * dx, 2 * dy, r0 * r0 - ri * ri + dx * dx + dy * dy))
    aa = sum(a * a for a, b, c in rows)
    ab = sum(a * b for a, b, c in rows)
    bb = sum(b * b for a, b, c in rows)
    ac = sum(a * c for a, b, c in rows)
    bc = sum(b * c for a, b, c in rows)
    determinant = aa * bb - ab * ab
    if determinant <= 1e-12 * max(aa * bb, 1.0):
        raise ValueError("基站共线或布置退化，无法进行二维定位")
    x = x0 + (ac * bb - bc * ab) / determinant
    y = y0 + (bc * aa - ac * ab) / determinant
    rmse = math.sqrt(sum((math.hypot(x - xi, y - yi) - ri) ** 2
                         for xi, yi, ri in points) / len(points))
    return x, y, rmse

该算法适合验证数据链路和生成位置初值。存在噪声时，可以继续对原始距离残差做非线性优化。输出的 range_rmse_m 表示测距约束的拟合残差，不能直接当成真实位置误差。

5. 运行示例

下载完整 Python 示例，保存为 uwb_position.py。Python 3.8 及以上可运行：

python uwb_position.py --demo

模拟数据的预期输出：

{"tag_id":"T1","round_id":1,"x_m":2.0,"y_m":1.5,"range_rmse_m":0.0}

把第 3 节的 JSON 保存到 ranges.jsonl，也可以从文件输入：

python uwb_position.py < ranges.jsonl

在现场使用时，先按实际测量值修改脚本中的 ANCHORS，再让串口、网络接收程序按上述格式输出完整测距轮次，交给解算程序读取。

6. 接入实测数据

推荐按以下顺序完成联调：

单链路测距：在多个已知距离上记录连续测距结果，检查单位、稳定性和系统偏差。
基站坐标测量：统一原点、坐标方向和高度，填写真实坐标。
轮次汇总：检查标签 ID、基站 ID、轮次和有效状态，拒绝过期数据与无效距离。
静态定位：把标签放到几个已知点位，对照解算坐标。
移动定位：记录完整轨迹，观察丢包、跳点和延迟，再决定滤波与更新频率。

标签与基站高度不同时，若高度差 dz 已知，可先把空间距离 r 转为水平距离 sqrt(r² - dz²)，并确认 r >= |dz|；高度未知时需要调整定位模型。

脚本会拒绝距离不足、未知基站、非有限数值、非正距离，以及共线或严重退化的基站布局。

7. 校准与误差排查

固定偏移：核对坐标原点、长度单位、天线参考位置与天线延迟校准。
部分区域偏差大：检查遮挡、反射和基站布局，先观察原始测距再判断是否需要滤波。
位置突然跳变：检查是否混入不同轮次或不同标签的数据，记录参与解算的原始距离。
残差较小但位置不准：重新核对基站坐标与高度模型，拟合良好不代表坐标基准正确。

天线延迟与板卡设计、安装方式有关，应按所用模块的校准流程处理，可参考 Qorvo 关于天线延迟的说明。

滤波应放在数据校验与基础测距确认之后。评估时同时记录位置误差、有效定位比例和端到端延迟，用现场数据决定参数。

快捷键