UWB定位系统中TWR算法实现与精度优化

在UWB（Ultra-Wideband）高精度定位中，双边双向测距（Two-Way Ranging, TWR / Double-Sided TWR） 是消除标签与锚点间时钟偏移、获得纳秒级飞行时间（ToF）的主流方法。本文基于Decawave DW1000/DW3000寄存器模型，给出DS-TWR计算流程、时间戳处理要点及常见误差抑制手段。

一、TWR基本原理与公式

单端TWR受两端时钟漂移影响大；Double-Sided TWR（DS-TWR） 用两次往返消除钟差：

Tag  --Poll-->  Anchor

Anchor --Resp--> Tag      (Ra1, Rb1 记录)

Tag  --Final--> Anchor   (Ra2, Rb2 记录)

时间戳定义（单位：UWB 符号周期 ≈ 1/499.2MHz ≈ 2ns）：

• T_t1：Tag发送Poll时刻

• T_r1：Anchor接收Poll时刻

• T_t2：Tag接收Resp时刻

- T_r2：Anchor接收Final时刻

• T_t3：Tag发送Final时刻

• T_r3：Anchor接收Final时刻（若需三次交换，通常二次足够）

DS-TWR飞行时间近似：

ToF = [ (T_r1 - T_t1) + (T_r2 - T_t2) - ((T_t3 - T_t2) - (T_r2 - T_r1)) ] / 2

实际工程用对称形式避免符号错：

ToF = ( (Ra - Tb) + (Rb - Ta) - ( (T_final - T_resp) - (R_resp - R_poll) ) ) / 2

距离 d = ToF × c（c ≈ 299792458 m/s，UWB通常用近似值 3e8）

二、DW1000时间戳读取与单位转换

DW1000提供40位RX/TX时间戳，单位为 1/499.2MHz × 2^40 细分（约15.65ps/LSB）。

2.1 读时间戳（伪代码）

// 读接收时间戳（高32位+低8位组合）

uint8_t ts_raw[5];

dwt_readrxtimestamp(ts_raw);  // 从DW1000寄存器0x0C~0x0E

uint64_t rx_ts = ((uint64_t)ts_raw[4]<<32)

              | ((uint64_t)ts_raw[3]<<24)

              | ((uint64_t)ts_raw[2]<<16)

              | ((uint64_t)ts_raw[1]<<8)

              | ts_raw[0];

2.2 转为秒与距离

#define UWB_TIME_UNIT_SEC  (1.0 / (499.2e6 * 128.0))  // ~15.65ps per LSB

double tof_sec = (double)tof_ticks * UWB_TIME_UNIT_SEC;

double dist_m  = tof_sec * 299792458.0;

注意：DW1000推荐用 dwt_readtxtimestamp / dwt_readrxtimestamp API，确保捕捉到确切符号边沿。

三、DS-TWR实现示例（Tag侧逻辑）

typedef struct { uint64_t poll_tx, resp_rx, final_tx; } tag_ts_t;

typedef struct { uint64_t poll_rx, resp_tx, final_rx; } anchor_ts_t;

double calc_ds_twr(tag_ts_t *tg, anchor_ts_t *anc)

{

   // 为清晰用浮点，量产可用定点Q格式

   double Ra = (double)(anc->poll_rx - tg->poll_tx);

   double Rb = (double)(anc->final_rx - tg->final_tx);

   double Ta = (double)(tg->resp_rx  - tg->poll_tx);

   double Tb = (double)(anc->resp_tx - anc->poll_rx);

   double tof_ticks = (Ra + Rb - (Tb - Ta)) / 2.0;

   return tof_ticks * UWB_TIME_UNIT_SEC * 299792458.0;

}

Anchor侧需在收到Final后做类似校验，或仅Tag计算后上报坐标服务器。

四、精度优化关键措施

4.1 天线延迟校准（Antenna Delay）

每块模块需实测并写入 ANTDLY 寄存器（典型 514~515 符号周期）：

dwt_setrxantennadelay(ANT_DELAY_RX);

dwt_settxantennadelay(ANT_DELAY_TX);

未校准会引入固定偏差（可达十几厘米）。

4.2 时钟偏移补偿

即使DS-TWR消去了大部分钟差，极高进度要求仍需用标称频率比修正：

ToF_corrected = ToF_measured × (f_tag_nominal / f_tag_actual)

通常用PPM级晶体（±10ppm），影响<3cm可不修正。

4.3 NLOS与多径抑制

• 用 CIR（Channel Impulse Response） 首径检测（DW1000 dwt_readCIR）

- 若首径与峰值差过大 → 标记为NLOS，加权降低置信度或切换锚点

4.4 温度补偿

晶体温漂影响ToF→距离，高温(>70℃)建议做两点校准或选用TCXO。

五、现场调试与验证

1. 静止点测：Tag与Anchor固定1m，连续100次测距，统计均值/标准差——标准差应<5cm（室内）

2. 动态轨迹：三角剖分解算坐标，比对全站仪参考点

3. 异常排查：

  - 若距离周期性跳变 → 时间戳回绕未处理（40位需模2^40）

  • 若单向严重偏长 → Antenna Delay未写或写错Bank

  • 若偶发NaN → Final帧丢失，需重发或状态机保护

六、结语

UWB定位精度取决于DS-TWR时间戳捕获准确性 + 天线延迟校准 + NLOS判别。在DW1000平台上按上述流程实现双边双向测距，配合CIR首径选取与温度/时钟补偿，可在典型办公环境下获得10cm级（1σ）静态精度，满足工业人员/资产定位需求。