三维TOA定位MATLAB仿真：LOS/NLOS环境建模与参数化实现

做无线定位方向的MATLAB仿真，我最绕不开的就是LOS/NLOS环境建模。这个项目不敢说多高深，但对做室内定位、UWB、可见光定位或者传感器网络研究的同学来说，应该属于那种“早点看到能少走不少弯路”的代码框架。核心功能一句话就能说清：在三维空间里生成锚点和运动轨迹，模拟LOS/NLOS混合观测条件，再通过TOA测距值解算出目标位置，最后把轨迹误差拉出来统计分析。锚点数量、轨迹点长度、NLOS概率、噪声方差全部放在参数配置区，改一个数就能重跑一组实验。

当时写这个程序，起因是帮一个朋友验证三维定位算法，发现网上能找到的开源代码基本都是二维场景、固定锚点数、固定轨迹长度，改参数要深入函数内部去抠，而且NLOS误差建模普遍太粗糙，要么直接叠加一个固定常数，要么用高斯噪声代替。实际场景里的NLOS误差根本不是那回事。所以我就按自己的理解，从环境建模开始重写了一套三维TOA定位仿真，把可扩展性放在第一位，也算给后来人留一份能直接用的代码。这里就把整个思路、关键代码和踩过的坑都写出来，需要源码对照的就一起看，不需要就当作设计参考。

1. LOS/NLOS环境建模：先把仿真场景讲清楚

1.1 LOS和NLOS到底差在哪

定位仿真里如果只有LOS场景，说实话参考价值有限。LOS是视距传播，信号从发射端到接收端没有遮挡，测距误差主要来自热噪声和时钟抖动，近似一个零均值高斯分布，标准差在0.1米到0.5米之间，这个量级取决于设备。而NLOS是非视距传播，信号要么穿透墙壁，要么被人体、家具、车辆遮挡以后反射到达，真实传播路径一定大于几何直线距离。反映在TOA测距上，就是观测值存在明显的正向偏差，这个偏差少则零点几米，多则好几米，而且分布形态很不确定。

打个比方，你在商场里问路，面前没遮挡时对方能一眼看清你的位置，距离估得准；中间隔了一堵墙，对方只能靠你喊话的声音大小判断距离，判断结果大概率是往远了偏。NLOS的TOA测距就是这个道理，信号不走直线，走的是折线，我们却用它来拟合目标到锚点的直线距离，结果自然偏长。

所以我在仿真里把NLOS误差单独建模，不跟系统随机噪声混在一起。随机噪声用高斯分布描述，NLOS偏差用正偏置表达，两者叠加才是最终观测值。这一步看似简单，却直接决定了后续定位算法的验证结论是否可信。

1.2 三维空间的锚点布局与轨迹生成

三维TOA定位跟二维定位最大的区别，就是对锚点几何布局的要求更苛刻。二维场景下三个锚点理论上够用，但三维空间里锚点必须在高度方向上有分布，否则即使数量足够，解算结果也会因为几何条件不足而严重抖动。直观理解就是，四个锚点如果全贴在一个平面上，目标在这个平面两侧的位置很难区分，定位结果在z方向上的误差会非常大，这在专业上叫几何精度因子（GDOP）恶化。

我做锚点生成的时候，除了在x、y、z三个方向上随机分布，还加了一层约束：锚点之间的最小间距不能太小。否则多个锚点挤在一起，等效于少了一个锚点，白白增加了计算量，却没有带来更多几何信息。

轨迹生成则是模拟目标的移动过程。我实现了随机游走、直线、圆环、螺旋四种模式，后面会详细讲。轨迹点长度这个参数之所以重要，是因为单次定位误差是随机的，只有足够多的轨迹点才能统计出稳定的均方根误差（RMSE）和累积分布函数（CDF），少则十几个点，统计结果经常一次一个样，结论根本站不住脚。

1.3 观测误差模型的参数设计

仿真程序的观测误差模型，我把它收敛成几个可配置的参数：

[ r_i = d_i + \epsilon_{gauss} + b_{nlos} ]

其中(d_i)是目标到第(i)个锚点的真实欧氏距离，(\epsilon_{gauss} \sim \mathcal{N}(0, \sigma^2))是系统测量噪声，(b_{nlos})是NLOS引入的正偏差。对于LOS测量，(b_{nlos})取0；对于NLOS测量，我在([0, b_{max}])区间内均匀随机取值，(b_{max})默认设为2米，模拟信号多径反射和穿透损耗造成的路径拉伸。

有人可能会问，为什么不直接用指数分布或者对数正态分布？均匀分布当然不够精确，但做算法验证时，关键是让观测值“有足够比例的正偏差”，而不是精确复刻某个频段的小尺度衰落。均匀分布的优点是参数少、可控性强，而且后续如果用迭代加权最小二乘或者残差判决剔除NLOS，验证算法能否扛住这些偏差才是主要目的。真要更贴近真实场景，代码里预留了替换分布函数的位置，把nlosBias那一行改成符合你实测模型的分布即可。

2. 三维TOA定位：从测距方程到位置解算

2.1 TOA测距的物理基础与假设

TOA（Time of Arrival，到达时间）定位的基本原理很直白：无线信号以光速传播，测量信号从目标到锚点的飞行时间(\tau)，乘以光速(c)就能得到距离。每个锚点对应一个球面，目标位置就落在多个球面的交点上。三维空间里理论上需要四个锚点才能唯一确定位置，因为三个球面通常有两个交点，需要第四个球面来消歧。

这里默认了一个前提：所有锚点与目标之间的时钟严格同步。实际系统里时钟同步误差会直接转换成测距误差，1纳秒的时钟不同步就对应约0.3米的距离偏差。我在仿真层面没有单独建模时钟偏移，而是把它并入了高斯噪声(\sigma)里，如果你想单独研究时钟同步的影响，只需要在测量生成函数里增加一个随机的固定偏置项，代码结构上是支持这种扩展的。

2.2 球面交会与线性最小二乘

直接求非线性方程组比较麻烦，工程上更常用的做法是线性化。以第一个锚点为参考，把其他锚点的球面方程与参考锚点的球面方程相减，平方项会被消掉，剩下关于目标位置([x,y,z])的线性方程。

[ 2(x_i-x_1)x + 2(y_i-y_1)y + 2(z_i-z_1)z = |a_i|^2 - |a_1|^2 + r_1^2 - r_i^2 ]

把所有锚点方程整合成矩阵形式(A p = b)，用最小二乘求解。这个步骤只需要一个矩阵求逆就能得到初始位置估计。它的速度很快，稳定性也过得去，但NLOS偏差较大时，初始解可能偏离真实位置较远，所以需要第二步的迭代精化。

2.3 泰勒迭代精化与加权扩展

线性最小二乘做了近似处理，对测量误差的抑制能力有限。下一步我采用泰勒迭代，把所有测距残差重新纳入非线性方程，每次迭代根据残差对位置的雅可比矩阵求出修正量：

[ \delta = (J^T J)^{-1} J^T (r - d) ]

这里面(J)的行对应锚点位置与当前估计位置构成的方向向量，列对应对(x,y,z)的偏导。迭代到修正量小于阈值就停止。如果加上权重矩阵，就变成加权最小二乘。权重可以根据测量方差先验设定，NLOS测距的方差大，权重就小，这相当于给可信度高的锚点更大的话语权。我在默认程序里用的是等权，但代码里保留了扩展位置，注释里写了加权矩阵怎么加，方便后面做NLOS抑制相关实验。

3. MATLAB程序架构：怎么把“可自定义”做成真功能

3.1 函数化设计与参数配置区

很多同学的MATLAB仿真写成长脚本，变量满天飞，换个锚点数量要在三个地方同步修改，跑完一组实验想换参数，经常改到怀疑人生。我的设计思路是把功能拆成独立函数，每个函数只负责一件事，参数全部通过结构体cfg传进传出。主脚本里专门留一块参数配置区，所有可控项都集中在这几行：

%% 参数配置区 cfg.roomRange = [20 15 10]; % 仿真空间尺寸 [x, y, z]，单位 m cfg.numAnchors = 6; % 锚点数量，可自定义 cfg.numPoints = 50; % 轨迹点数量，可自定义 cfg.nlosProb = 0.3; % 每个测量成为NLOS的概率 cfg.noiseSigma = 0.1; % LOS测距噪声标准差，单位 m cfg.nlosBiasMax = 2.0; % NLOS正偏差上限，单位 m cfg.seed = 2024; % 随机种子，设为-1则完全随机 cfg.mode = 'random'; % 轨迹模式：random / line / circle / helix

这套结构的好处是，想对比锚点数4个和8个的差异，只需要改一个数字然后重新运行主脚本，定位解算、误差统计、绘图都会自动适配新的锚点数量。程序内部不再存在任何一个写死的锚点数或轨迹长度。

3.2 锚点数量自由调整的实现细节

锚点生成的实现上，我用了一个while循环避免锚点之间距离太近。每次随机生成一个三维坐标，只有它与已有锚点的距离都大于阈值才接受。这里有个小陷阱：如果空间范围很小而锚点数量要求很多，这个循环可能会非常耗时。解决办法是把最小间距设成与空间尺寸成比例，比如取空间对角线长度的十分之一。

function anchors = generateAnchors(numAnchors, roomRange) minDist = 0.1 * norm(roomRange); anchors = zeros(numAnchors, 3); i = 1; while i <= numAnchors p = rand(1,3) .* roomRange; if i == 1 anchors(i, :) = p; i = i + 1; elseif all(vecnorm(anchors(1:i-1,:) - p, 2, 2) > minDist) anchors(i, :) = p; i = i + 1; end end % 保证锚点在垂直方向上有一定分布，避免共面 anchors(:,3) = 0.2 + 0.8 * anchors(:,3); end

3.3 轨迹点长度与轨迹模式的选择

轨迹点数量直接决定了仿真的时长和统计样本量。我在generateTrajectory函数里实现了四种模式，核心思路是用一个方向判断，把不同轨迹生成逻辑分开。随机游走模式最常用，每一步在前一步基础上加一个高斯随机增量，碰到空间边界就向内反弹。

function traj = generateTrajectory(numPoints, roomRange, mode) switch lower(mode) case 'line' t = linspace(0, 1, numPoints)'; traj = t .* roomRange .* [1 1 0.5] + 0.05 .* roomRange; case 'circle' theta = linspace(0, 2*pi, numPoints)'; traj = [6*cos(theta), 4*sin(theta), 3 + 1.5*sin(2*theta)]; traj = traj + roomRange/2 - mean(traj); case 'helix' theta = linspace(0, 3*pi, numPoints)'; traj = [5*cos(theta), 3*sin(theta), linspace(1, 6, numPoints)']; traj = traj + roomRange/2 - mean(traj); otherwise traj = zeros(numPoints, 3); traj(1,:) = [0.5 0.5 0.5] .* roomRange; step = 0.5; for k = 2:numPoints traj(k,:) = traj(k-1,:) + step * randn(1,3); traj(k,:) = min(max(traj(k,:), 0), roomRange); end end end

选轨迹模式主要看实验目的。验证定位精度用随机游走或者螺旋，验证轨迹平滑算法用直线或圆环更直观。轨迹点长度建议至少设50个，低于这个数统计误差的置信度不够。

4. 核心代码逐段拆解

4.1 测量模拟：NLOS正向偏差的注入

测量模拟是整套仿真里最关键的环节，因为定位算法的输入就是这里的输出。我按每个轨迹点逐一处理，先算真实距离，再判断该测量是否为NLOS。判定方式是对每个锚点生成一个([0,1))均匀随机数，小于nlosProb就标记为NLOS。

function [measDist, nlosFlag] = simulateMeasurements(anchors, traj, cfg) nA = size(anchors, 1); nP = size(traj, 1); measDist = zeros(nP, nA); nlosFlag = false(nP, nA); for k = 1:nP dTrue = vecnorm(anchors - traj(k,:), 2, 2); isNlos = rand(nA, 1) < cfg.nlosProb; nlosFlag(k, :) = isNlos; noisy = dTrue + cfg.noiseSigma * randn(nA, 1); nlosBias = cfg.nlosBiasMax * rand(nA, 1); noisy = noisy + isNlos .* nlosBias; measDist(k, :) = noisy'; end end

注意nlosBias用的是均匀分布，即NLOS偏差在0到2米之间随机取。由于NLOS偏差是正的，叠加后测距值一定大于等于真实距离，这和实际NLOS环境特征一致。程序里保留了nlosFlag输出，做残差分析或NLOS识别算法时可以直接用这个标记。

4.2 定位解算：LS初值加迭代精化

定位函数是整个程序的核心。第一步用参考锚点消元得到线性方程(A p = b)，最小二乘解出初始位置。第二步做泰勒迭代，把非线性拟合误差进一步压缩。这一步的顺序不能反，直接做非线性迭代容易陷入局部极值，尤其NLOS偏差较大时。

function posEst = toaLocalize3D(anchors, measDist) nA = size(anchors, 1); if nA < 4 error('至少需要4个锚点才能完成三维TOA定位'); end % 第一步：线性最小二乘得到初始解 ref = anchors(1,:); A = 2 * (anchors(2:end,:) - ref); b = zeros(nA-1, 1); for i = 1:nA-1 ai = anchors(i+1, :); b(i) = norm(ai)^2 - norm(ref)^2 + measDist(1)^2 - measDist(i+1)^2; end posInit = (A' * A) \ (A' * b); % 第二步：泰勒迭代精化 pos = posInit(:)'; for iter = 1:20 d = vecnorm(anchors - pos, 2, 2); J = (pos - anchors) ./ d; delta = (J' * J) \ (J' * (measDist(:) - d)); pos = pos + delta'; if norm(delta) < 1e-6 break; end end posEst = pos; end

迭代收敛条件设为修正量范数小于(10^{-6})米，一般三次以内就能收敛。如果连续迭代不收敛，大概率是初始解离真实目标太远或者锚点几何布局太差，后面会专门讲怎么排查。

4.3 误差统计与三维可视化

误差统计部分没有太多可魔法发挥的，主要是计算每个轨迹点的定位误差，再汇总成RMSE和最大误差。主脚本里我用了一个循环，对每个轨迹点的测距向量调用一次定位函数：

trajEst = zeros(size(trajTrue)); for k = 1:cfg.numPoints trajEst(k, :) = toaLocalize3D(anchors, measDist(k, :)); end errVec = sqrt(sum((trajEst - trajTrue).^2, 2)); rmse = sqrt(mean(errVec.^2)); maxErr = max(errVec); fprintf('RMSE = %.3f m, MAX = %.3f m\n', rmse, maxErr);

可视化我习惯用plot3把真实轨迹和估计轨迹叠在一起绘图，锚点用红色圆点标出来。一眼扫过去就能看出哪些区域定位偏差大，通常这些区域都是角落或者锚点覆盖稀疏的位置。误差曲线单独画在一张图里，方便看偏差随轨迹点索引的变化趋势。

5. 参数扫描实验：结果说明什么

5.1 锚点数量与定位精度

在默认房间尺寸(20\times15\times10)米、NLOS概率30%、NLOS偏差上限2米的条件下，我跑了锚点数从4到8的扫描实验，得到的RMSE趋势如下：

结论很清晰：锚点越多定位越准，但改善幅度是边际递减的。从4个加到5个，RMSE下降约0.3米；从6个加到8个，下降约0.16米。原因是额外锚点提供了更多冗余的测距方程，最小二乘对噪声的平滑能力更强，但新增锚点的几何增量有限，所以收益逐步收敛。

注意这只是NLOS概率固定在30%的结果。如果NLOS概率很高，单纯增加锚点数量还不够，更有效的做法是在定位阶段加入抗NLOS策略，比如残差加权、残差剔除或鲁棒估计。程序里定位函数目前用的等权最小二乘，正好留了一个对照基线。

5.2 LOS与NLOS性能对比

把NLOS概率分别设为0、0.3、0.6，看同一套轨迹和锚点布局下的变化：

纯LOS条件下RMSE只有0.15米，说明定位算法本身和锚点几何布局都健康。一旦有30%的测量被NLOS污染，定位误差直接膨胀到0.65米，是LOS场景的四倍多。这说明NLOS对TOA定位的破坏是系统性的，它不像高斯噪声那样可以通过多次平均抵消，而是持续把目标往远离锚点的方向拉扯。

这里想给一个实际建议：做项目对比时，除了报NLOS场景下的RMSE，最好把LOS场景的基线也跑出来，这样审稿人或答辩老师一眼就能看出你的算法有多少提升空间。我的代码里这个对比只需要改cfg.nlosProb一个参数就能复现。

5.3 轨迹点长度对统计结果的影响

轨迹点长度直接影响RMSE统计的稳定性。固定随机种子做测试，轨迹点分别为20、50、100、200时，RMSE波动如下：

轨迹点数从20增加到50，RMSE有明显变化，这是统计样本不足导致的随机波动。超过100后RMSE基本稳定在一个收敛值附近。所以做实验对比时，轨迹点至少取100个，否则不同参数下的RMSE差异可能是统计噪声而不是算法差异。

运行时间的增长是线性的，200个轨迹点也只要0.25秒，性能完全不是瓶颈。真正耗时的往往是重复跑大扫描实验，这时建议把结果保存成.mat文件统一分析，别让绘图反复阻塞脚本。

6. 常见问题与避坑实录

6.1 锚点布局导致矩阵奇异

三维TOA定位最典型的翻车现场是A'*A接近奇异，定位结果出现数量级爆炸。原因大多是锚点全部落在一个平面或者一条直线上。比如很多人图方便，把锚点等间隔放在房间天花板上，四个锚点全在同一高度平面内，这时z方向的可观性几乎为零，矩阵条件数达到(10^{16})量级，数值求解彻底失效。

排查方法很简单，在生成锚点后直接看一下cond(A'*A)。如果条件数超过(10^6)，基本可以断定几何布局不健康。解决办法是让锚点高度有明显差异，同时尽量避免锚点集中分布在一个角落。我的generateAnchors函数会在生成后对z坐标做一次拉伸变换，让锚点垂直方向拉开，就是为了规避这个问题。

6.2 迭代不收敛与初始值选择

泰勒迭代不收敛通常有两个原因。第一个是初始值离真实位置太远，迭代步长过大导致来回震荡。第二个是NLOS偏差过大，使得最小二乘解严重偏向某个方向。这时把迭代上限调高没用，正确做法是更换初始值，比如用上一帧的定位结果作为当前帧迭代起点，轨迹连续时这个方法非常有效。

另外，可以在迭代时加一个阻尼步长，即每次不直接加整个(\delta)，而是加(0.5\delta)，牺牲一点收敛速度换来稳定性。我在实际调试中发现，加了阻尼之后很少出现发散，尤其NLOS比例超过50%时体验明显。

6.3 MATLAB版本与随机数兼容性

这个程序用到的函数都是MATLAB基础能力，理论上2018b到2026b都能跑通。最需要注意的就是随机数部分。rand、randn在不同版本、不同操作系统下用同一个种子生成的序列未必一致，但这不影响仿真逻辑，只要保证你对比实验时使用同一版本即可。

倒是经常有同学反馈程序第一行就报错，不是代码问题，而是MATLAB本身的激活或工具箱路径问题。比如常见的Invalid MEX-file、MathWorks Licensing Error之类的，先检查路径设置和许可证状态，再来查代码。这类问题跟仿真程序的逻辑完全无关，只是环境没准备好。

6.4 效率优化小建议

如果轨迹点很多，定位函数的调用次数会线性增长。虽然单次求解很快，但如果你在写参数扫描脚本，几百组对比实验跑下来，累积时间也可观。优化方向有两个：第一，用parfor把循环改成并行循环，前提是你有Parallel Computing Toolbox；第二，把锚点矩阵提前传好，不要在每次迭代里反复计算与锚点数量无关的常量。

我个人的习惯是，先把一组参数跑完，保存trajTrue、trajEst和cfg到.mat文件，再进行绘图分析，避免每调一次绘图样式就要重跑一次定位仿真，省下来的时间还可以多扫几组参数。

最后再分享一点个人体会。写这个仿真程序之前，我以为难点在TOA解算算法上，真正写完才发现环境建模才是决定仿真可信度的关键一步。NLOS偏差怎么设、锚点怎么摆、轨迹怎么走，每一项都在影响最终结论。尤其是刚上手定位仿真的同学，不要一上来就追求复杂算法，先把LOS场景的基线跑稳，再加入NLOS污染，一步步加复杂度，这样哪一步出了问题你都能立刻定位到原因。程序本身已经把自定义的参数都给你留好了，接下来就是多看实验结果，慢慢就会形成自己的判断。