第4章测距算法实现

第4章测距算法实现：双边双向测距（DS-TWR）

好，咱们进入正题。DS-TWR，全称是Double-Sided Two-Way Ranging。名字挺长，说白了就是「两边各发一次，来回测两次」。为什么要这么折腾？因为单边测距（SS-TWR）有个硬伤——时钟误差会直接吃掉你的精度。

我记得刚入行那会儿，第一次用UWB做车钥匙定位，图省事用了单边测距。结果呢？车停在跟前，钥匙显示距离5米。我当时就懵了。后来才明白，晶振那点漂移，在单边测距里会被放大得不成样子。

核心观点：DS-TWR不是炫技，是刚需。它通过两次往返测量，把时钟误差的影响降到最低。

4.1 原理：为什么非要「双边」？

先看单边测距的问题。设备A发一个包，设备B回一个包。A记录下发送和接收的时间戳，算出往返时间。听起来没问题，对吧？但问题在于——A和B的时钟不是同步的。哪怕晶振标称20ppm，实际跑起来偏差可能更大。

你想想看，如果A的时钟比B快0.01%，那测出来的时间差就会跟着偏。在短距离（比如10米以内）可能还好，但车钥匙场景下，我们要的是厘米级精度。这点偏差就受不了。

DS-TWR怎么解决？它让A和B各发起一次测距。A发一次，B回一次；然后B发一次，A回一次。这样我们得到两个往返时间，再做个加权平均。时钟误差在两次测量中方向相反，一平均就抵消了大半。

公式其实不复杂：

T_prop = (T_round1 × T_round2 - T_reply1 × T_reply2) / (T_round1 + T_round2 + T_reply1 + T_reply2)

嗯，看着有点唬人。但你仔细看，分子是两个往返时间乘积减去两个回复时间乘积，分母是四个时间之和。这个公式的妙处在于——它几乎不依赖时钟的绝对精度。

我的经验：实际项目中，用DS-TWR配合好的晶振（±10ppm以内），测距误差可以控制在±5cm以内。这在车钥匙场景里已经非常够用了。

4.2 代码实现：从原理到落地

理论说完了，咱们看代码。我用的是Decawave的DW1000芯片，这是目前UWB车钥匙方案里的主流选择。代码基于C语言，跑在STM32上。

先定义几个关键结构体：

typedef struct {
    uint64_t tx_time;      // 发送时间戳
    uint64_t rx_time;      // 接收时间戳
    uint32_t sequence;     // 帧序号
} uwb_timestamp_t;

typedef struct {
    uwb_timestamp_t poll;      // A发起的轮询
    uwb_timestamp_t response;  // B的响应
    uwb_timestamp_t final;     // A的最终消息
} ds_twr_session_t;

这里要注意，时间戳是64位的，单位是UWB的时钟周期（约15.65皮秒）。别想着用微秒，精度不够。

接下来是核心的测距函数：

float ds_twr_calculate_distance(ds_twr_session_t *session) {
    uint64_t t_round1, t_reply1, t_round2, t_reply2;
    uint64_t t_prop_raw;
    float distance;
    
    // 计算第一次往返
    t_round1 = session->response.rx_time - session->poll.tx_time;
    t_reply1 = session->response.tx_time - session->response.rx_time;
    
    // 计算第二次往返
    t_round2 = session->final.rx_time - session->response.tx_time;
    t_reply2 = session->final.tx_time - session->final.rx_time;
    
    // DS-TWR核心公式
    t_prop_raw = (t_round1 * t_round2 - t_reply1 * t_reply2) 
               / (t_round1 + t_round2 + t_reply1 + t_reply2);
    
    // 转换为米（光速约3e8 m/s，时间单位是15.65e-12 s）
    distance = (float)t_prop_raw * 15.65e-12 * 3e8;
    
    return distance;
}

这段代码我调试了整整两天。你猜问题出在哪？溢出。t_round1 * t_round2这个乘法，两个64位数相乘，结果可能超过128位。我一开始没注意，结果算出来的距离忽大忽小。

避坑指南：我曾经在乘法运算上栽过跟头。建议用128位中间变量，或者分段计算。DW1000的SDK里其实有现成的64位乘法函数，别自己手写。

4.3 误差分析：哪些因素在偷你的精度？

代码写好了，跑起来也能出数。但你真的信任这个数吗？我建议你做个误差分析。下面这个表是我在实际测试中总结的：

误差来源	典型影响	我的应对方法
时钟漂移	±2~5cm	用DS-TWR + 温补晶振
多径效应	±10~30cm	首径检测 + 信道滤波
天线延迟	固定偏差（5~15cm）	出厂校准 + 查表补偿
帧处理延迟	±1~3cm	硬件时间戳，避免软件延迟

你看，多径效应是最大的坑。车钥匙在车里使用时，信号会在金属车身、座椅、玻璃之间来回反射。接收到的第一个信号可能不是直射路径，而是反射路径。这会导致测出来的距离偏大。

怎么解决？DW1000有个功能叫「首径检测」（First Path Detection）。它会分析接收信号的脉冲形状，找到最早到达的那个路径。我建议你把这个功能打开，并且配合信道冲激响应（CIR）做二次确认。

还有一个容易被忽略的点——天线延迟。每根天线的物理特性不同，信号从芯片到天线辐射出去需要时间。这个延迟是固定的，但不同天线差异可能达到几纳秒。换算成距离就是几十厘米。

我的习惯：每块板子在出厂前都做一次天线延迟校准。方法很简单——把两块板子放在已知距离（比如1米），测出来的距离减去真实距离，就是天线延迟的等效距离。把这个值写到Flash里，每次上电读出来补偿。

4.4 实战中的几个小技巧

最后分享几个我在项目中积累的经验：

帧间隔别太短。我见过有人把轮询间隔设到1ms，结果信道冲突严重。建议至少10ms以上。
做多次测量取中位数。单次测量可能有野值，连续测5次取中位数，稳定性好很多。
注意温度变化。晶振的频率会随温度漂移。车钥匙在夏天车内可能到70°C，冬天零下20°C。我建议做温度补偿，或者至少在不同温度下标定一次。
别迷信理论精度。芯片手册上写的±10cm，那是在实验室理想环境下。实际车钥匙场景，能做到±15cm已经很不错了。

嗯，DS-TWR这块就讲这么多。说白了，原理不复杂，但落地时细节很多。你只要把时钟误差、多径效应、天线延迟这三个大头管住了，精度基本就稳了。下一章咱们聊怎么把测距结果用起来——也就是定位算法。

📚 课程目录