简介基于到达时间差TDOA的Chan算法是超宽带UWB室内定位中经典的位置求解方法这份代码以单个MATLAB脚本呈现了完整实现。UWB信号有厘米级测距潜力但室内多径传播会让到达时间戳偏离真实值Chan算法通过构建并求解非线性方程组从多个接收器的时间差中解算出发射端坐标兼顾精度与计算效率过程涉及信号模型建立、TDOA估计与位置解算适合正在学习UWB定位原理、TDOA解算或MATLAB仿真的学生与研究人员可用于复现算法流程、检验多径和噪声条件下的定位偏差。压缩包为rar格式共1个.m脚本包体仅1KB代码精简便于逐行阅读。目前已有631人浏览学习可作为理解经典定位算法的入门素材。读者可在MATLAB中调用该脚本开展模拟观察Chan算法在理想及干扰场景下的输出坐标与误差并通过调整接收器布局或信号参数进一步体会TDOA定位的工程特性也可对比不同基站数目下的定位结果为后续改进提供参考。1. UWB定位里的TDOA与Chan算法为什么解析解比迭代解更吃香做UWB定位的人手里大概率拿到过这样一份资料包里面是基站坐标、标签的TDOA数据、若干篇论文还有一个叫“Chan算法”的m文件或Python脚本。TDOA到达时间差定位的本质是通过标签到多个基站的距离差反推坐标而Chan算法恰好是这一类问题里最经典的闭式解法。和泰勒展开这类迭代算法不同它不需要初始值猜想也不会因为初值给得差就发散尤其适合UWB这种高带宽、高精度的测距场景一次算完像素级的误差可控。这篇笔记适合正在用UWB做室内定位、资产追踪或雷达类测距应用的人从原理到参数调节把这条路走通。2. TDOA定位原理与Chan算法推导从双曲线方程组到两步加权最小二乘2.1 三个基站够不够TDOA方程组的自由度与可解条件TDOA测量的原始信息不是距离而是时间差。标签向周围广播一个UWB脉冲各个基站记录到达时刻把每个基站的时间戳减去参考基站的时间戳再乘以光速就得到距离差。二维平面里一个距离差对应一条双曲线两个距离差就是两条双曲线的交点理论上三个基站就能解出位置。这句话听起来简单实际落地时有个暗坑三个基站的TDOA方程解出来通常有两个交点一个真一个假得靠先验区域或第四条链路来剔除。用数学语言说设标签坐标是 ( \mathbf{p} (x, y) ) 基站 ( S_i ) 坐标是 ( (x_i, y_i) ) 参考基站是 ( S_1 ) 那么[ r_{i1} r_i - r_1 c \cdot \tau_{i1} ]其中 ( \tau_{i1} ) 是基站 i 与参考基站 1 的到达时间差。这个式子本身是非线性的因为 ( r_i \sqrt{(x - x_i)^2 (y - y_i)^2} ) 。直接求解需要解双曲线方程组计算路径比较曲折。所以Chan算法聪明的地方在于它不直接解非线性方程而是通过代数变形把问题改写成一个线性方程组再做加权最小二乘。自由度的问题也要说清。三个基站能解二维坐标但解的唯一性和几何布局强相关。基站如果近似排成一条直线两族双曲线的交点会重合位置解就会沿垂线方向漂移。工程上我的经验是至少放四个基站别卡着理论下限。四个基站不仅多一条冗余链路还能在Chan算法的第二步把噪声方差利用起来让解更稳。2.2 把非线性方程组拆成线性Chan算法第一步的代数变形这里把推导走一遍代码才能写得有依据。从 ( r_i r_1 r_{i1} ) 出发两边平方[ r_i^2 r_1^2 2 r_1 r_{i1} r_{i1}^2 ]把 ( r_i^2 (x_i - x)^2 (y_i - y)^2 ) 和 ( r_1^2 (x_1 - x)^2 (y_1 - y)^2 ) 代进去整理后[ (x_i^2 y_i^2 - x_1^2 - y_1^2) - 2(x_i - x_1)x - 2(y_i - y_1)y 2 r_{i1} r_1 r_{i1}^2 ]这个式子厉害的地方在于( x ) 和 ( y ) 虽然是未知数但它们是以线性形式出现的同时右侧还有一个 ( r_1 ) 也作为未知量。所以一次最小二乘解出来的向量其实是 ( [x, y, r_1]^T ) 三个未知数。这个带有中间变量 ( r_1 ) 的技巧就是Chan算法第一步的核心把非线性问题先“线性化”解一个带约束的伪线性方程组。实际计算时我习惯把矩阵形式写清楚。令 ( K_i x_i^2 y_i^2 ) 那么第 i 行方程可以写成[ -2(x_i - x_1)x - 2(y_i - y_1)y - 2 r_{i1} r_1 r_{i1}^2 - K_i K_1 ]把 ( N ) 个基站含参考站的方程拼起来得到一个 ( (N-1) \times 3 ) 的矩阵用最小二乘就能求第一轮解。这个第一轮解只是“能用”精度还不够因为右侧的 ( r_{i1} ) 本身是带噪声的测量值最小二乘没有区分噪声对不同方程的干扰程度。2.3 第二步加权最小二乘用残差协方差提升精度第一轮解出来之后下一步是构造加权矩阵。TDOA观测值不是等权的距离差的噪声与几何位置有关而且各通道之间是相关的因为它们共享同一个参考基站的时钟误差。如果无视这个相关性直接用普通最小二乘结果在靠近基站连线的区域会明显变差。Chan算法第二步的做法是用第一轮解出的 ( [x, y, r_1] ) 重新代入残差表达式求出残差的协方差矩阵然后以协方差矩阵的逆作为权重再做一次加权最小二乘。这一步等于告诉求解器哪条链路的测量更可信哪条链路该被压低权重。经过第二轮解算在TDOA噪声满足高斯分布的假设下定位精度可以逼近克拉美罗下界。这也是为什么在处理UWB数据时我优先选Chan而不是Fang算法——Fang只利用了平面几何关系精度上不如两步WLS扎实。代码落地时第二步需要算一个辅助矩阵 ( B \mathrm{diag}(r_2^0, r_3^0, \dots, r_N^0) ) 其中 ( r_i^0 ) 是第一轮解出的标签到各基站的距离然后协方差矩阵近似为[ \Phi 4 c^2 B Q B ]其中 ( Q ) 是TDOA时延观测的协方差矩阵。这里有个小坑——如果第一轮解出的 ( r_i^0 ) 出现了负值野值场景常见协方差矩阵就不是正定的加权最小二乘会算出一堆NaN。我在代码里会做一个保护对 ( r_i^0 ) 做一次 abs() 或直接丢弃非正值的链路避免整个矩阵崩掉。后面避坑章节会专门讲这条血泪经验。3. 落地UWB定位最小系统Python实现Chan算法并跑通TDOA数据链路3.1 读入基站坐标与TDOA观测值数据格式约定先约定输入格式这是整个链路里最容易翻车的地方。我做过的UWB设备比如常见的那类DWM1000模块输出的原始数据通常是这样的基站ID、时间戳、测距值或者原始时间差。我在工程里统一转换成“以参考基站为基准的距离差数组”单位是米不放时间戳原始值减少后续换算出错的可能。CSV文件的列顺序就按这个约定来文件第一行是表头后面每行是“基站ID, 坐标x, 坐标y, 距离差”。距离差的正负要注意负值表示该基站比参考基站更早收到信号这在数学上是合法输入但有的算法实现会假设所有值都是正数拿到负值直接报错。我的做法是在读入后不归一化保留正负号因为双曲线方程本身是能处理负距离差的。import numpy as np def load_uwb_data(csv_path, ref_id0): 读入CSV返回基站坐标矩阵和距离差数组以ref_id为参考。 raw np.loadtxt(csv_path, delimiter,, skiprows1) # raw: (N, 4) - [基站ID, x, y, 距离差] bs_id raw[:, 0].astype(int) bs_pos raw[:, 1:3] dist_diff raw[:, 3] # 距离差数组要求参考基站那行必须是0.0否则做差 dist_diff dist_diff - dist_diff[bs_id ref_id][0] mask bs_id ! ref_id # 去掉参考基站自己的方程 return bs_pos[mask], dist_diff[mask]这段代码只有一个容易踩坑的点参考基站自身的距离差必须强制归零否则后续矩阵计算里会出现一个恒等式干扰最小二乘。如果你从UWB设备读出来的距离差已经是“相对于参考站”的那这步可以跳过但多做一次减法不亏能顺手把浮点噪声清掉。参数说明ref_id是参考基站的编号一套定位系统里必须固定不要每帧数据换一个参考站否则无法做时间对齐。3.2 核心函数Python写一个可直接改参数的Chan算法现在给出可直接抄的Chan算法实现。这个函数兼容二维坐标、任意基站数3个以上并包含第二步加权最小二乘。我把每一步的矩阵维度写在注释里方便你用自己的数据替换。def chan_tdoa(bs_pos, dist_diff): Chan算法二维定位。 bs_pos: (N, 2) 基站坐标第一行必须是参考基站 dist_diff: (N-1,) 非参考基站相对参考基站的距离差单位米 返回: (x, y) 标签坐标 N bs_pos.shape[0] if N 3: raise ValueError(二维TDOA至少需要3个基站) ref bs_pos[0] # 参考基站坐标 K np.sum(bs_pos**2, axis1) # K_i x_i^2 y_i^2 K_ref K[0] # 第一步线性最小二乘构造 (N-1)x3 矩阵 A np.zeros((N-1, 3)) b np.zeros(N-1) for i in range(1, N): A[i-1, 0] -2 * (bs_pos[i, 0] - ref[0]) A[i-1, 1] -2 * (bs_pos[i, 1] - ref[1]) A[i-1, 2] -2 * dist_diff[i-1] # 这是 r1 的系数 b[i-1] dist_diff[i-1]**2 - K[i] K_ref # 普通最小二乘第一个解 p_ls np.linalg.lstsq(A, b, rcondNone)[0] # p_ls [x, y, r1] if p_ls[2] 0: # r1 出现非正值说明野值污染严重直接退化到3基站解 p_ls[2] np.maximum(np.linalg.norm(p_ls[:2] - ref), 1.0) # 第二步用距离残差构造协方差权重 r_i np.sqrt(np.sum((bs_pos - p_ls[:2])**2, axis1)) B np.diag(r_i[1:] / max(dist_diff[0], 1e-6)) # 归一化距离加权 # TDOA时延协方差通常给一个对角阵量级取决于UWB芯片 Q np.eye(N-1) * (0.1**2) # 0.1米标准差约0.33ns时延误差 Phi 4 * B Q B.T # 加权最小二乘 W np.linalg.inv(Phi np.eye(N-1) * 1e-6) # 加一个微小正则项防奇异 p_wls np.linalg.lstsq(W A, W b, rcondNone)[0] # 对r1做二次约束修正经典Chan可选项这里做简版 x, y, r1 p_wls d_ref np.sqrt((x - ref[0])**2 (y - ref[1])**2) if d_ref 0: x ref[0] (x - ref[0]) * r1 / d_ref y ref[1] (y - ref[1]) * r1 / d_ref return np.array([x, y])参数理解起来就几行dist_diff的单位是米不是纳秒如果设备给的是时间戳记得乘上光速c 299792458.0Q矩阵里的 0.1 米标准差是经验值——我遇到过有的模块时钟漂移大这个值要调到 0.3 甚至 0.5否则权重矩阵会把真实链路压制得太狠。最后一步对r1的二次约束是可选优化经典论文里有完整推导我这里做的是投影修正保证输出的距离与参考基站几何一致。3.3 参数怎么调光速补偿、时延校准与阈值设置UWB测距的误差源和WiFi RSSI完全不是一个量级但好数据也得配好参数。光速补偿是第一个必须处理的真空光速是 299792458 m/s但UWB信号在空气里传播速度略低工程上乘一个 0.9997 的折射率修正系数约等于把每米距离修正 0.3 毫米。如果你用默认的真空光速算距离差在 10 米量级的室内会累积 3 毫米左右的系统性偏差对TDOA来说这个量级会直接抬高定位误差下限。第二个参数是天线延时。UWB标签和基站的天线会引入固定时延通常在 1~3 纳秒量级换算成距离就是 0.3~0.9 米。这个值如果不校准TDOA的零偏会非常明显表现为定位点整体向某个方向偏移。校准方法不难把标签放在参考基站正上方或者多个基站连线的中点测一组距离差取平均值作为零偏在代码里减掉。我一般会在每次换天线或换设备批次后重做这一步因为不同批次的SMA线缆和天线相位中心不一致玄学成分挺大。第三个参数是野值阈值。UWB在室内复杂环境下偶尔会出多径导致的跳变点表现为距离差突然偏离真实值好几米。我不用复杂的滤波直接在进Chan算法前加一道卡阈值如果某条链路的距离差变化率超过 0.5 米/毫秒就把它标记为可疑并降低权重。这个判定对移动标签尤其重要静止场景也可以用一个滑动窗口中位数来替代实时值。4. Chan算法实战避坑5条常见问题与排查记录4.1 现象定位结果在基站连线上漂移有一次测试四个基站呈矩形布在房间四角标签站在中间解算坐标却在两个基站连线方向上前后抖动幅度达到 40 厘米。我起初怀疑是算法写错反复验算矩阵没问题后来打印了每条链路的距离差才看明白有两组TDOA观测值高度相关一旦参考基站那边的时钟有一个微小波动两条链路同时偏移几何上正好放大到连线的垂直方向。原因本质上是几何精度因子GDOP问题。Chan算法再怎么优化也改变不了双曲线交会时的几何放大效应。解决方式是改布站把矩形改成L型或十字型让任意一组双曲线的渐近线夹角尽量大。如果布站不能动那就得在结果后处理加一个短期平滑我用的是滑动窗口均值窗口长度 0.5 秒左右对 10Hz 的输出频率来说就是取 5 帧平均能压住一部分抖动。4.2 现象NLOS环境下误差突然变大到数米非视距NLOS是UWB室内定位最大的敌人。标签被柜子挡住时信号绕射或透射路径比直线长TDOA距离差瞬间多出 1~3 米。这种误差不是高斯噪声而是正偏置Chan算法里第二阶段加权根本救不回来——协方差矩阵还是按高斯噪声设计的。我当时的排查思路是分两步先看各条链路的距离差是否出现“同方向同时变大”如果两条链路同时变大几米多半是那一片区域整体NLOS。粗处理是在算法前加一个“通道一致性检验”把每条链路的当前值与最近10帧的中位数比偏差超过 1.2 米就丢掉该链路。要注意的是不能用均值均值会被野值拉偏中位数更稳。精确做法是离线采集一遍NLOS区域的误差分布给每条链路设置不同的权重但这个成本高一般项目我做到前一步就够用了。4.3 现象距离差数据出现负向跳变标签朝某个方向移动时某条链路的距离差突然从 2.5 米跳到 -0.3 米持续几帧后又恢复正常。这个现象一开始让我误以为时钟不同步后来检查发现是UWB模块的到达时间检测leading edge detection在多径环境下锁错了首径。首径如果被噪声抬掉检测器会锁到第一条强多径上时间提前或延后了对应的纳秒数。这类问题无法靠Chan算法本身修复必须在数据源头处理。我的做法是在算法前加一个中值滤波器窗口 3~5 帧专门削这种单帧跳变。还有一种办法是同时接收多个通道的数据做交叉验证如果某条链路和其他链路的距离差不满足三角不等式即 ( |d_{i1}| \leq d_{ij} d_{j1} ) 就判定为无效帧。这个检验在代码里很好写几行就够。4.4 现象定位结果呈系统性偏移静态时也偏系统把所有点位整体往东南方向推了半米而且不管怎么调权重都偏。这种症状十有八九是参考基站的时延没校准。TDOA是相对量参考基站自身的绝对时延不影响结果但非参考基站的时延会直接变成距离差零偏。我之前有一批基站天线型号混用每根天线的相位中心差异在厘米到分米量级导致定位出现区域性偏移。校准流程写下来供参考把标签放在房间中心静止 30 秒记录每个非参考基站距离差的平均值再用卷尺量出标签到各基站的真实距离差把两者之差作为该链路的零偏补偿写入配置。这种方案能消掉大部分固定偏差。注意换基站、换天线后必须重新标定没有后悔药可吃。4.5 现象矩阵奇异或输出NaN基站数量足够、数据看着也正常但算法偶尔输出 NaN。我在代码里已经加了正则项还是偶发。最后定位到问题是某条链路的距离差恰好为 0导致 A 矩阵中对应行的第三列是 0整行变成线性相关矩阵秩不足。另一个触发点是两个基站坐标完全一样坐标抄错了矩阵有两行完全相同最小二乘直接退化。解决方式是在矩阵求逆前检查条件数。numpy.linalg.cond(A)超过 1e10 就宣告这一帧数据无效而不是硬算。对工程系统来说丢一帧远好过给一个错得离谱的位置因为上层滤波还能插值错值会污染后续所有状态估计。我现在的代码库里这已成固定习惯动作。5. 提升TDOA定位精度的进阶手段从GDOP布站到残差检验Chan算法把定位精度推到了解析解的上限但最终系统精度还取决于两件事布站几何和观测质量检验。先说GDOP的量化用法写个简单脚本就能评估布站方案def gdop(bs_pos, tag_pos): 计算二维GDOP值越小说明几何越好。 d bs_pos - tag_pos dist np.linalg.norm(d, axis1) H d / dist[:, None] H H[1:] - H[0] # 相对参考基站 return np.sqrt(np.trace(np.linalg.inv(H.T H)))这个脚本可以用来模拟不同布站方案把候选基站坐标和预计活动区域网格点输进去求出整个区域的GDOP均值与最大值。我的经验是区域内GDOP最大值不要超过 5超过 5 的位置即使算法再准误差也会放大五倍以上。布站时优先保证活动频繁的区域GDOP均匀而不是只优化中心点。残差检验是另一个值得做的收尾动作。Chan算法输出坐标后把坐标代回原方程算出每条链路的残差与TDOA观测残差对比。如果某一帧的残差突然高于近期均值几倍说明观测里混入了NLOS野值这时我会做一个加权回溯把残差最大的链路去掉后重新解算一次。这个“失败重算”的机制在移动标签场景里能显著降低定位跳变率。最后分享一个我常年保留的习惯给每帧数据存一条记录包含原始TDOA、解算坐标、残差和GDOP值哪怕调试阶段不分析日后翻车排查时也有据可循。我记得有个项目上线后用户报“某些区域定位忽好忽坏”靠的就是这批历史数据反查出角落的GDOP爆炸。希望这套从推导到避坑再到进阶的方法能帮你在UWB定位落地时少走一趟我走过的弯路。本文还有配套的精品资源点击获取
