简介这份资源围绕无线通信与导航系统中的经典定位算法展开面向从事物联网、雷达、卫星导航及无线网络定位开发的学习者与研究人员重点讲解CHAN、Taylor与AOA三种基本方法的原理与实现思路。压缩包共37个文件以m脚本与mat文件为主辅以asv备份和doc文档整体约830KB涵盖TDOA时差解算、泰勒级数近似、到达角三角测量等核心模块并附有仿真结果图文档便于对照验证。已有1680人学习下载说明其在定位算法入门与仿真实践中具有一定参考价值。读者可借助其中的MATLAB代码理解非线性方程迭代求解、误差抑制与多算法融合定位的实现细节快速搭建仿真环境并对比不同基站数量下的定位性能为后续三维精确定位与鲁棒性优化提供可复用的脚本基础。1. 从三个公式到一张定位图CHAN、Taylor、AOA 到底在算什么室内定位、蜂窝网定位、声源定位绕不开三个名字CHAN、Taylor、AOA。很多刚接触的开发者第一反应是「这不就是三个公式吗」真上手才发现同一组基站数据CHAN 解出来是虚数Taylor 迭代不收敛AOA 角度一抖结果偏出几十米。问题不在公式在于你没搞清它们各自假设了什么、在什么条件下才成立。这篇笔记就围绕这三个基本定位算法展开CHAN 怎么用两步加权最小二乘把 TDOA 变成闭式解Taylor 怎么在初值附近做迭代收敛AOA 怎么靠角度交会定出坐标以及三者怎么组合、参数怎么调、翻车点在哪。适合已经懂一点最小二乘、手上有基站或阵列数据、想把定位精度从「能跑」推到「能用」的工程师。读完你应该能自己搭一套仿真、跑通三种算法、看懂残差曲线为什么长那样。2. CHAN 算法两步加权最小二乘怎么把 TDOA 变成闭式解CHAN 算法的核心价值是「闭式解」——不需要迭代给定 TDOA 测量值就能直接算出坐标。代价是它对测量噪声的统计特性有明确假设噪声一大或者基站几何摆得不好第二步的矩阵就可能不正定解直接崩掉。所以理解 CHAN重点不是背公式而是搞清它两步分别在干什么、哪一步最脆弱。2.1 第一步把非线性 TDOA 方程线性化设基站 i 坐标为 $(x_i, y_i)$移动台为 $(x, y)$基站 1 为参考站。TDOA 测量给出的是距离差$$r_{i,1} r_i - r_1 c \cdot \tau_{i,1}$$其中 $r_i \sqrt{(x_i-x)^2 (y_i-y)^2}$。直接解这组方程是非线性的CHAN 的做法是引入辅助变量 $r_1$把方程平方后整理成关于 $(x, y, r_1)$ 的线性方程组$$(x_i - x_1)x (y_i - y_1)y r_{i,1} \cdot r_1 \frac{1}{2}\left[(x_i^2y_i^2) - (x_1^2y_1^2) - r_{i,1}^2\right]$$写成矩阵形式 $h G_a z_a$其中 $z_a [x, y, r_1]^T$。这一步的巧妙之处在于原本非线性的距离关系被 $r_1$ 这个辅助变量吸收掉了。但注意$r_1$ 和 $(x,y)$ 并不独立它们满足 $r_1^2 (x-x_1)^2(y-y_1)^2$这个约束在第一步里被暂时丢掉了留到第二步去补。用加权最小二乘解第一步import numpy as np def chan_step1(bs, tdoa, c3e8): bs: (N,2) 基站坐标第 0 个为参考站 tdoa: (N-1,) 相对参考站的距离差测量值 (米) 返回: z_a [x, y, r1] 的第一次估计 N bs.shape[0] x1, y1 bs[0] Ga np.zeros((N-1, 3)) h np.zeros((N-1, 1)) for i in range(1, N): xi, yi bs[i] ri1 tdoa[i-1] Ga[i-1] [xi - x1, yi - y1, ri1] h[i-1] 0.5 * (xi**2 yi**2 - x1**2 - y1**2 - ri1**2) # 第一次权重取单位阵先拿一个粗解 za np.linalg.lstsq(Ga, h, rcondNone)[0] return za, Ga, h逻辑说明Ga的第三列是距离差h里减掉了 $r_{i,1}^2$这是线性化的关键。参数上tdoa单位必须是米如果你手上是秒先乘光速。第一次权重用单位阵是 CHAN 的标准做法因为此时还没有残差信息去估协方差。2.2 第二步用残差估协方差再做一次加权第一步解出的 $z_a$ 是有偏的因为忽略了 $r_1$ 和 $(x,y)$ 的约束。CHAN 的第二步做两件事一是用第一步的残差估计测量噪声协方差二是利用约束关系再解一次。def chan_step2(za, Ga, bs, tdoa, c3e8): 用第一步结果构造协方差做第二次 WLS N bs.shape[0] # 用第一步解算出的位置反推距离估残差 x, y, r1 za.flatten() B np.eye(N-1) for i in range(1, N): ri np.linalg.norm(np.array([x, y]) - bs[i]) B[i-1, i-1] ri # 噪声协方差假设各 TDOA 独立同分布方差 sigma^2 sigma2 1.0 # 实际用残差估计 cov sigma2 * (B B.T) W np.linalg.inv(cov) za2 np.linalg.solve(Ga.T W Ga, Ga.T W h) return za2参数说明B矩阵把「距离差噪声」映射到「距离噪声」这是 CHAN 第二步的精髓。sigma2在仿真里可以设 1实测里要用第一步残差的均方值去估。W是加权矩阵噪声越大权重越小这是加权最小二乘的标准逻辑。2.3 第二步之后怎么从 $z_a$ 里把坐标抠出来第二步解出的还是 $[x, y, r_1]$但此时它已经比第一步准了。真正要的是 $(x, y)$而 $r_1$ 只是个辅助量。有些实现会再做一次「约束修正」用 $r_1^2 (x-x_1)^2(y-y_1)^2$ 再解一个小二乘把 $x, y$ 的精度再提一档。这一步不是必须但在基站数少比如 4 个时能明显降偏差。实操里我一般会检查两件事一是第二步解出的 $r_1$ 是否为正负值说明几何或噪声出了问题二是把解代回原始 TDOA 方程看残差残差如果比测量噪声还大一个量级基本可以判定这次解不可信。3. Taylor 算法初值、雅可比与收敛判据怎么设Taylor 是迭代法思路和 CHAN 完全相反不追求闭式而是在一个初值附近做泰勒展开把非线性方程线性化反复迭代逼近。它的好处是精度可以比 CHAN 高坏处是——初值不好就发散迭代次数和收敛阈值设不好就白跑。3.1 泰勒展开到底展在哪设当前估计为 $(x_k, y_k)$真实位置 $(x, y) (x_k \Delta x, y_k \Delta y)$。把距离函数在 $(x_k, y_k)$ 处一阶展开$$r_i(x,y) \approx r_i(x_k,y_k) \frac{\partial r_i}{\partial x}\Delta x \frac{\partial r_i}{\partial y}\Delta y$$其中偏导数是$$\frac{\partial r_i}{\partial x} \frac{x_k - x_i}{r_i(x_k,y_k)}, \quad \frac{\partial r_i}{\partial y} \frac{y_k - y_i}{r_i(x_k,y_k)}$$对 TDOA 而言把第 i 个方程减去参考站方程得到关于 $[\Delta x, \Delta y]$ 的线性方程组。每迭代一次解一个最小二乘更新 $(x_k, y_k)$直到 $\sqrt{\Delta x^2 \Delta y^2}$ 小于阈值。def taylor_tdoa(bs, tdoa, init, max_iter50, tol1e-3, c3e8): bs: 基站坐标 (N,2) tdoa: 距离差 (N-1,) init: 初值 [x0, y0] tol: 收敛阈值米 x, y init for it in range(max_iter): H [] b [] r1 np.linalg.norm([x - bs[0,0], y - bs[0,1]]) for i in range(1, bs.shape[0]): ri np.linalg.norm([x - bs[i,0], y - bs[i,1]]) # 雅可比行对 x,y 的偏导差 H.append([(x-bs[i,0])/ri - (x-bs[0,0])/r1, (y-bs[i,1])/ri - (y-bs[0,1])/r1]) # 残差测量距离差 - 当前估计距离差 b.append(tdoa[i-1] - (ri - r1)) H np.array(H); b np.array(b) delta np.linalg.lstsq(H, b, rcondNone)[0] x delta[0]; y delta[1] if np.linalg.norm(delta) tol: break return np.array([x, y]), it逻辑说明H是雅可比矩阵每一行对应一个 TDOA 方程对 $\Delta x, \Delta y$ 的偏导。b是当前估计与测量的残差。delta是这一步该走多远。参数上tol设 1e-3 米对室内够用max_iter设 50 是保险值正常 5 到 10 次就收敛。3.2 初值怎么给CHAN 打底是最稳的组合Taylor 最怕初值离真值太远。实操里最常用的组合是先用 CHAN 解一个闭式解把它当 Taylor 的初值。这样既避免了 Taylor 从零点起步乱飞又用 Taylor 把 CHAN 的偏差再压一压。za, Ga, h chan_step1(bs, tdoa) za2 chan_step2(za, Ga, bs, tdoa) init za2[:2].flatten() pos, iters taylor_tdoa(bs, tdoa, init) print(fCHAN 初值: {init}, Taylor 收敛: {pos}, 迭代 {iters} 次)这个组合在基站数 4 到 6 个、噪声中等时通常比单用 CHAN 精度高 20% 到 40%。但要注意如果 CHAN 第一步就解出虚数或负的 $r_1$这个初值本身就是错的Taylor 只会沿着错误方向收敛。所以初值给进去之前先做一次合理性检查。3.3 收敛判据和迭代上限的取舍tol设太小迭代次数暴涨收益却很小设太大还没收敛就停了。经验值是室内场景tol取 0.01 到 0.001 米室外取 0.1 到 1 米。max_iter不要设成 1000因为一旦发散迭代次数再多也救不回来反而浪费时间。我一般设 30 到 50配合一个「残差不再下降就退出」的判据。另一个容易忽略的点雅可比矩阵可能病态。如果基站几乎共线H的条件数会很大lstsq解出来的delta会剧烈震荡。这时候要么加正则项要么直接判定这次几何不可用。4. AOA 算法角度交会、阵列误差与最小二乘融合AOA 和前面两个不是一回事。CHAN 和 Taylor 吃的是 TDOA时间差AOA 吃的是角度——每个基站或阵列测出一个来波方向多条方向线交会出位置。它的优势是不需要基站间时间同步劣势是角度测量对阵列校准极其敏感一度误差在远距离上就是几十米。4.1 两条方向线交会最简形式设基站 i 测得方位角 $\theta_i$则该基站到目标的射线方程为$$y - y_i \tan(\theta_i) \cdot (x - x_i)$$两个基站两条线直接解交点def aoa_intersect(bs, angles): bs: (N,2) 基站坐标 angles: (N,) 方位角弧度 返回: 最小二乘交会点 A []; b [] for (xi, yi), th in zip(bs, angles): # 射线方程整理成 A[x,y]^T b A.append([np.sin(th), -np.cos(th)]) b.append(np.sin(th)*xi - np.cos(th)*yi) A np.array(A); b np.array(b) pos np.linalg.lstsq(A, b, rcondNone)[0] return pos逻辑说明把 $y - y_i \tan\theta (x-x_i)$ 整理成 $\sin\theta \cdot x - \cos\theta \cdot y \sin\theta \cdot x_i - \cos\theta \cdot y_i$这样避免了 $\tan$ 在 $\theta 90°$ 时的数值爆炸。参数上angles必须是弧度且要统一参考方向正北还是正东。两个基站时是精确交点多于两个时是最小二乘交会。4.2 角度误差怎么放大成位置误差AOA 的精度瓶颈不在算法在角度测量。一个直观的估算目标距离基站 $d$角度误差 $\sigma_\theta$则横向位置误差约 $d \cdot \sigma_\theta$。$d 50$ 米、$\sigma_\theta 2°$约 0.035 弧度横向误差就有 1.75 米。如果目标在 200 米外同样的角度误差直接变成 7 米。所以 AOA 落地时阵列校准比算法重要得多。常见做法是先在已知方位放几个校准源测出每个角度的系统偏差做一张「角度-偏差」查找表实测时先查表修正再交会。这一步不做后面算法调得再细也是白搭。4.3 把 AOA 和 TDOA 融在一起加权最小二乘单一算法都有短板实操里更常见的是融合。AOA 给角度约束TDOA 给距离差约束两者量纲不同融合的关键是权重。一个可用的做法是把两类观测写成统一的残差方程用各自噪声方差定权def fusion_wls(bs, tdoa, angles, sigma_t1.0, sigma_a0.02): sigma_t: TDOA 噪声标准差米 sigma_a: 角度噪声标准差弧度 rows []; rhs []; w [] x0, y0 bs[0] # TDOA 方程线性化后 for i in range(1, bs.shape[0]): ri1 tdoa[i-1] rows.append([bs[i,0]-x0, bs[i,1]-y0]) rhs.append(0.5*(bs[i,0]**2bs[i,1]**2-x0**2-y0**2-ri1**2)) w.append(1.0/sigma_t**2) # AOA 方程 for (xi, yi), th in zip(bs, angles): rows.append([np.sin(th), -np.cos(th)]) rhs.append(np.sin(th)*xi - np.cos(th)*yi) w.append(1.0/sigma_a**2) rows np.array(rows); rhs np.array(rhs) W np.diag(w) pos np.linalg.solve(rows.T W rows, rows.T W rhs) return pos参数说明sigma_t和sigma_a是两类观测的噪声水平权重是方差的倒数。这两个值必须按实际测量标定拍脑袋设会导致某一类观测被完全压制。融合的收益在基站数少时最明显——3 个基站单靠 TDOA 几何不够加上 AOA 就能定出唯一解。5. 避坑与排查定位解算翻车的五个典型现场5.1 现象CHAN 解出虚数或负的 $r_1$原因通常是基站几何太差接近共线或者 TDOA 噪声过大导致第二步的协方差矩阵不正定。解决先检查基站布局任意三个基站不要接近一条直线再检查 TDOA 数据里有没有明显离群值一个坏点就能毁掉整次解算。如果几何无法改退化成用 Taylor 加正则项。5.2 现象Taylor 迭代次数打满还不收敛八成是初值太远。解决不要用零点或随机值当初值用 CHAN 的解打底。如果 CHAN 也崩了说明数据本身有问题先回去查 TDOA 测量链路别在算法层死磕。另一个可能是tol设得比噪声还小迭代在噪声里来回跳把tol放到噪声量级即可。5.3 现象AOA 交会点飘到基站背后角度参考方向搞反了或者弧度和角度混用。解决统一用弧度统一参考方向并且在交会前做一次「射线方向合理性检查」——如果解出的点在某个基站射线的反方向直接判定这次角度数据不可信。这个检查能挡掉大部分低级错误。5.4 现象融合结果比单算法还差权重设错了。如果sigma_a设得远小于实际角度噪声AOA 观测会主导解算把 TDOA 的好信息压掉。解决先用单算法分别跑一遍看各自残差量级再按残差反推权重。融合不是无脑叠加权重标定是必须做的一步。5.5 现象仿真精度很好实测差一个量级仿真里噪声是高斯白噪声实测里噪声有偏、有色、还有多径。解决仿真只用来验证算法逻辑实测精度必须用实测数据标定。常见做法是留一段已知位置的轨迹做验证集看残差随距离和角度的变化找出系统偏差再补偿。6. 从仿真到实测一套可复现的验证流程与参数标定技巧把三种算法串起来跑通只是第一步真正决定这套方案值不值得投入的是你能不能建立一套可复现的验证流程。我一般按四步走造数据、跑单算法、跑融合、看残差分布。造数据阶段基站布局不要图省事摆成正多边形。正多边形在仿真里好看但实测里基站位置受建筑和安装条件限制往往是歪的。我习惯随机撒 5 到 8 个基站保证没有三点共线然后让目标在覆盖区内走一条 S 形轨迹每个点生成 TDOA 和 AOA 观测噪声按实测标定的方差加。def simulate(bs, traj, sigma_t1.0, sigma_a0.02): 生成带噪声的 TDOA 和 AOA 观测 tdoa_all, aoa_all [], [] for (x, y) in traj: r np.linalg.norm(bs - [x, y], axis1) tdoa (r[1:] - r[0]) np.random.randn(len(bs)-1)*sigma_t ang np.arctan2(y - bs[:,1], x - bs[:,0]) \ np.random.randn(len(bs))*sigma_a tdoa_all.append(tdoa); aoa_all.append(ang) return np.array(tdoa_all), np.array(aoa_all)跑单算法时重点看两件事CHAN 的解有没有出现虚数或负 $r_1$Taylor 的迭代次数分布是否集中在 5 到 10 次。如果 Taylor 迭代次数普遍超过 20说明初值质量不行回去查 CHAN。跑融合时把sigma_t和sigma_a按实测标定值设不要用仿真里的理想值。看残差分布是最后一步也是最容易被跳过的一步。把每个轨迹点的解算位置和真值比画残差随距离的变化曲线。如果残差随距离线性增长说明有系统偏差没补掉如果残差是随机散布说明算法和噪声模型匹配得不错。这一步能帮你判断当前精度瓶颈到底在算法、在几何、还是在测量。参数标定的技巧TDOA 的sigma_t可以用静止目标的多次测量估AOA 的sigma_a要用已知方位的校准源估。两个值都标定完之后融合权重才有意义。我踩过的最大的坑就是拿仿真噪声当实测噪声用结果融合权重全错解算精度还不如单用 TDOA。最后一个习惯每次改完参数不要只看平均误差要看误差的分布尾部。定位场景里平均误差好看但尾部有几个大偏差点实际使用体验会很差。把 95 分位误差也打出来这个指标比均值更能反映方案能不能用。希望帮到你。本文还有配套的精品资源点击获取