简介针对UWB多径环境下的高精度定位需求这套Matlab代码提供完整的三角定位算法实现覆盖超宽带信号与信道模型生成、CIR提取、AOA/AOD/rTOF参数获取及定位解算等关键环节。资源面向电子信息工程、计算机、数学等专业学生适用于课程设计、期末大作业或毕业设计也适合研究UWB定位算法的工程师快速验证思路。压缩包共44个文件体积约1.92MB以mlx实时脚本为主辅以fig图表、png图示、docx说明文档和md文档可直观查看定位结果与误差CDF分布。全部代码采用参数化编程参数便于调整注释清晰并附带可直接运行的案例数据目前已有296人学习下载。借助这套资源读者既能掌握UWB多径三角定位的完整链路也能基于开放代码二次开发适配不同场景与算法改进为后续研究或工程部署奠定基础。1. UWB多径三角定位的Matlab代码包先从这条完整链路说起UWB超宽带定位这两年之所以被反复讨论核心在于它在室内多径环境下仍能拿到厘米级的测距分辨率。但你把手上的UWB模组跑通之后会发现测距只是第一步。真正要做一个定位系统绕不开四个环节从接收信号里提取CIR信道冲激响应、估计AOA到达角和AOD离开角、获取rTOF往返飞行时间最后再用三角定位算法算出坐标。这份Matlab代码包的价值在于它不是只给了某一个环节的函数而是把上述整条链路装进了一个工程从一段原始接收信号开始到最终输出目标坐标为止。对正在做UWB定位算法验证、毕业设计或者产品预研的工程师来说这是可以直接对着跑、对着改的参考实现。下文按链路顺序逐章拆解讲清楚每个参数设在哪、每个函数在干什么、以及我在复现过程中踩过的具体坑。2. 底层参数怎么来CIR提取与AOA/AOD/rTOF获取的算法拆解2.1 CIR提取滑动相关与直达径对齐UWB接收信号在数学上可以看作发射信号与信道冲激响应的卷积。室内环境里的墙、地面、金属货架都会产生反射因此h(t)不是一根干净的冲激而是一串幅度和时延各不相同的分量叠加。CIR提取要做的就是从接收信号里把这串分量恢复出来并且区分出哪些是直达径LOS径、哪些是反射产生的多径。代码包里采用的常见做法是滑动相关。发射端发送一个已知的参考波形接收端将接收信号与这个参考模板做互相关运算。UWB信号带宽大、时间分辨率高互相关输出里的每个峰就对应一条传播路径峰与峰之间的时延差对应路径长度差峰的幅度对应该路径的增益。之所以说UWB在密集多径环境里优于窄带方案根本原因就在这里——相关峰足够尖锐多径在时延轴上能被分开。function cir extract_cir(rx_signal, ref_template, fs, cir_len) % rx_signal : 接收信号离散序列 % ref_template : 本地参考模板与发射波形一致 % fs : 采样率单位 Hz % cir_len : 截取的CIR长度单位样本点 % 滑动相关得到双边相关序列 [corr, lags] xcorr(rx_signal, ref_template); % 只保留零时延之后的因果区间 valid_idx find(lags 0); corr_causal corr(valid_idx); % 找出相关峰位置作为直达径参考时刻 [~, peak_idx] max(abs(corr_causal)); % 从峰值位置向后截取固定长度的CIR if peak_idx cir_len - 1 length(corr_causal) cir corr_causal(peak_idx:peak_idx cir_len - 1); else cir [corr_causal(peak_idx:end); zeros(cir_len - (length(corr_causal) - peak_idx 1), 1)]; end % 能量归一化后续角度估计和测距都依赖相对幅度 cir cir / (norm(cir) eps); end这段代码里有几个点需要注意。lags 0的过滤不是细节问题滑动相关输出是双边序列负时延部分对应参考模板先于接收信号到达的假象不滤掉会把时间轴整体搞乱。peak_idx定位的是最大相关峰默认它就是直达径——这个假设在LOS场景成立在非视距场景下很可能失效这点在第4章的避坑部分会专门展开。cir_len的选择直接和fs挂钩比如采样率500MHz时1ns的时延差只对应0.5个样本点cir_len设置太短会把后续有用的反射路径砍掉设置太长又会把噪声区间包含进来后续定位解算时干扰明显。我的习惯是先画出截取后的CIR幅度曲线看一眼再回头调长度不要一上来就拍脑袋定值。2.2 AOA与AOD估计MUSIC谱估计与导向矢量设计AOA和AOD解决的是一维测距无法解决的方位问题。只有距离时目标被约束在基站为圆心、测距值为半径的圆上叠加角度信息后圆和射线的交点就是位置。AOA通常在接收端的天线阵列上估计信号入射方向AOD在发射端估计离开方向。两者在算法上是对偶的代码里共用同一套谱估计框架。代码包采用的MUSIC算法多信号分类思路很清晰把阵列接收数据的协方差矩阵做特征分解大特征值对应的特征向量张成信号子空间小特征值对应的张成噪声子空间。因为信号子空间和噪声子空间正交所以在真实来波方向上导向矢量与噪声子空间的内积接近零MUSIC谱会出现峰值。实现如下function aoa music_aoa(rx_matrix, num_paths, fc, d) % rx_matrix : 天线阵列接收矩阵维度为 M x N % M是天线数N是快拍数 % num_paths : 多径数量信号子空间维数 % fc : 载波频率单位 Hz % d : 阵元间距单位 m c 3e8; wavelength c / fc; [M, N] size(rx_matrix); Rxx (rx_matrix * rx_matrix) / N; % 样本协方差矩阵 [V, D] eig(Rxx); [~, idx] sort(diag(D), descend); Vn V(:, idx(num_paths1:end)); % 噪声子空间 theta -90:0.5:90; % 角度搜索范围 P_music zeros(size(theta)); for k 1:length(theta) a exp(-1j * 2 * pi * d * sind(theta(k)) / wavelength); P_music(k) 1 / abs(a * Vn * Vn * a); end % 找谱峰返回角度估计值 [~, locs] findpeaks(10*log10(P_music), MinPeakHeight, 10); aoa theta(locs); end这个函数有几个关键参数直接影响结果。num_paths设的是信号子空间维数室内环境通常取3到5取小了会把真实路径漏进噪声子空间导致谱峰消失取大了噪声子空间被污染虚假峰变多。阵元间距d要满足d wavelength/2否则会出现栅瓣MUSIC谱在-90度和90度两端各出一个假峰。搜索步长0.5度是一个精度和计算量的折中如果最终定位误差要求更高可以改成0.1度但搜索时间会相应拉长。2.3 rTOF获取双向往返测距的时间戳处理rTOF往返飞行时间测距的本质是测信号在基站和标签之间跑一个来回的时间乘上光速除以2就是距离。相比单程TOFrTOF不需要基站和标签之间严格的时间同步这是它在工程里更常用的原因。但rTOF有一个隐蔽的误差来源收发链路的天线延迟和硬件处理延迟。信号从基站的基带发出到射频前端再到天线辐射出去中间有固定延迟标签接收、处理、回复的链路里同样有延迟。这些延迟会被叠加进rTOF测量值如果不做校准所有距离都会整体偏大几十厘米甚至更多。代码包里处理这件事的方法是双边双向往返测距DS-TWR。简单说基站发起测距记录发起时刻T1标签收到后延迟T_reply1再回复记录回复时刻T2基站收到回复后同样延迟T_reply2再发起一次标签再收到。四个时间戳两两相减可以把收发延迟项消掉。核心公式为function dist calc_rtof_distance(t1_base, t2_tag, t3_base, t4_tag, calib_delay) % t1_base : 基站发送时刻 % t2_tag : 标签收到时刻 % t3_tag : 标签回复时刻 % t4_base : 基站收到回复时刻 % calib_delay : 硬件校准延迟单位 s tof ((t4_base - t1_base) - (t3_tag - t2_tag)) / 2; tof tof - calib_delay; dist tof * 3e8; end参数calib_delay从哪里来常见做法是把两个已知位置的天线面对面放置测一组rTOF原始值用真实距离反推延迟。这个校准要在每次更换天线、更换线缆后重新做一次因为延迟会随硬件链路改变。代码包里预留了这个校准入口但初始calib_delay设的是0运行时如果不填距离误差会直接进入后续三角定位这一点在第4章的坑里还会再提。3. 定位算法主体多径三角定位的建模与最小二乘解算3.1 三角定位的几何模型与多径利用拿到距离和角度之后定位问题就变成了几何解算。每个基站可以给出两个独立观测rTOF得到的距离rho以及MUSIC谱估计出的到达角theta。在二维平面上以基站为极点目标点的坐标可以写成x x_anchor rho * cos(theta) y y_anchor rho * sin(theta)这是最理想的单径模型。但室内UWB信道的现实是天线接收到的信号是多径叠加的MUSIC算法给出的角度不只是直达径的角度还可能包含反射路径的角度rTOF测距在主径被遮挡时也会锁定到反射径上。所以这份代码包里用的不是一个基站加一个角度的简单模型而是把多个基站、多条路径的观测都纳入解算用一个超定方程组来做最小二乘。超定方程的意义在于单条路径的测量误差会因为冗余观测被摊薄某个基站的异常测量不会直接毁掉整个定位结果。3.2 最小二乘解算代码实现与权重选择三角定位的最小二乘解算可以整理成标准形式。假设有N个观测方程每个方程形如(x - x_i)*sin(theta_i) - (y - y_i)*cos(theta_i) 0含义是目标点应该在基站i的角度的射线方向上同时有(x - x_i)^2 (y - y_i)^2 rho_i^2的距离约束。把角度约束和距离约束合并对非线性方程做线性化近似之后可以写成矩阵形式A * p b其中p [x; y]。代码实现如下function pos triangulate_ls(anchor_pos, rho, theta, weight) % anchor_pos : N x 2 矩阵每行是一个基站的坐标 % rho : N x 1 向量各基站的rTOF测距值 % theta : N x 1 向量各基站估计的到达角单位度 % weight : N x 1 可选权重向量用于抑制NLOS路径 n size(anchor_pos, 1); if nargin 4 weight ones(n, 1); end % 角度约束方程目标到基站的连线方向应与估计角一致 A_ang zeros(n, 2); b_ang zeros(n, 1); for i 1:n A_ang(i, :) [sin(theta(i)), -cos(theta(i))]; b_ang(i) sin(theta(i))*anchor_pos(i,1) - cos(theta(i))*anchor_pos(i,2); end % 距离约束方程线性化后转为目标点到基站距离等于rho A_dist zeros(n, 2); b_dist zeros(n, 1); for i 1:n A_dist(i, :) 2 * (anchor_pos(i, :) - mean(anchor_pos, 1)); b_dist(i) rho(i)^2 - anchor_pos(i,:)*anchor_pos(i,:) ... mean(anchor_pos,1)*mean(anchor_pos,1); end A [A_ang; A_dist]; b [b_ang; b_dist]; W diag([weight; weight]); % 加权最小二乘解 pos (A * W * A) \ (A * W * b); end这里weight的默认值是全1但实际场景里不应该这样。LOS路径上的测距和角度可信度高NLOS路径的测量误差可能被反射路程拉偏数米在加权最小二乘解法里应该让LOS路径的权重大NLOS路径的权重小。常见做法是根据CIR里第一径的能量占比来设定权重第一径能量占比高说明直达径清晰权重给高能量占比低说明直达径可能被遮挡权重给低。代码里我把这个逻辑留在了weight参数上运行时可以直接传入一个根据CIR特征计算出的向量而不是全部填充为1。3.3 多径分量的数据关联与解算优化MUSIC算法在某个基站上可能给出不止一个角度峰比如直达径30度、墙面反射径-45度同时存在。问题来了哪个角度应该进入定位解算代码包里做了一个贪心关联策略——对每个基站先用rTOF算出一个距离范围然后在这个距离范围内找可能的反射路径长度匹配的角度峰。反射路径的长度等于基站到反射面的距离加上反射面到目标的距离这个长度通常大于直达径长度所以能够在距离维度上做初步筛选。这一步不是可选的。如果一个反射角被误当成直达角几何上它会把定位结果推到一个错误方向而且因为角度误差不存在均值归零的特性误差随迭代只会累积。代码包在triangulate_ls之前会有一个路径筛选函数检查每个角度峰对应的到达时延是否符合rTOF测量值偏差超过一个门限的直接丢弃。门限值的设置和CIR的时间分辨率有关一般取CIR主瓣宽度的1.5倍。我实测下来这个门限设得太严会把真实的NLOS路径也丢掉导致观测数不足设得太松又起不到筛选作用需要边跑边调。4. 跑通这份代码必看的排查清单参数、矩阵和天线延迟的坑4.1 现象定位结果整体偏移坐标收敛到一个错误象限现象三个基站都给出了合理的测距和角度但最终定位结果偏离真实位置好几米而且每次跑偏的方向不一致有时跑到左上角有时跑到右下角。原因CIR提取时peak_idx定位到了多径反射峰的峰值而不是直达径的起点。反射路径能量可能比直达径更强尤其在墙角、金属货架附近最大相关峰往往是反射路径。后续的AOA估计和rTOF测距全部建立在这个错误的路径上三角定位自然跟着错。解决把CIR幅度打印出来人眼先确认主峰位置。然后改用前沿检测方法替代最大峰值检测——从噪声基底往前找第一个超过设定门限通常取噪声均值的3倍加上标准差的采样点那个点才是直达径的到达时刻。代码里可以直接把extract_cir函数里的max(abs(corr_causal))替换成前沿检测逻辑改动量很小但效果是决定性的。4.2 现象MUSIC谱在-90度和90度位置出现对称虚假峰现象跑完music_aoa之后除真实角度外谱图两端各出现一个幅度相近的伪峰导致角度估计结果里混进无效路径。原因阵元间距d大于半波长空间采样不满足奈奎斯特条件出现栅瓣。这是阵列信号处理里最经典的翻车场景。解决第一步检查d是否满足d wavelength/2不满足就改天线布局第二步如果天线物理间距已经固定且超限可以在MUSIC谱搜索时把搜索范围限制在[-arcsin(wavelength/(2d)), arcsin(wavelength/(2d))]以内把栅瓣排除在可视区域外。两个基站的搜索范围不同时theta向量也要按基站分别生成不能共用一份。4.3 现象所有基站的rTOF距离整体偏大且偏大量几乎一致现象定位解算前的测距结果检查时发现每个基站的rho都比真实距离大出相同的量级大约在0.3到0.8米之间。原因硬件收发链路延迟没有被校准掉。基站的射频收发、标签的基带处理都会产生固定延迟这部分会直接叠加进rTOF时间戳里。解决将两个天线放在已知距离上比如1米实测rTOF并计算差值把差值作为calib_delay传入calc_rtof_distance。注意校准物件的实际距离要用高精度手段量取不要用卷尺估个大概。我在实践中会把校准后的距离和一维测距模组的输出对比偏差在1厘米以内才认为校准合格。更换天线或者换了线缆之后必须重新校准硬件链路变了延迟一定变。4.4 现象加权最小二乘解算报矩阵奇异或者条件数极大现象(A * W * A) \ (A * W * b)这行代码直接抛出警告“Matrix is singular”或者不报错但解出来的坐标忽远忽近。原因基站分布接近共线或者角度观测值过于接近比如两个基站给出的到达角都指向同一个方向导致矩阵A * W * A不满秩。另一种可能是某个角度值传成了NaN或Inf污染了矩阵。解决解算前先对矩阵A做条件数检查cond(A * W * A) 1e10就直接丢弃当前帧不要硬算。同时检查输入数据里是否有NaNUWB测距在某些场景下会返回无效值代码里应该有对应的过滤逻辑。基站的部署上尽量让角度差分散开三个基站呈三角分布比一字排开的几何构型好得多。4.5 现象NLOS环境下定位误差增大且误差向固定方向偏移现象同一套代码在空旷环境中定位误差在20厘米以内把目标挪到金属货架后面或者墙角误差涨到1米以上而且每次都往同一个方向偏。原因直达径被遮挡后CIR里能量最强的路径是反射路径前沿检测虽然抓到了第一径但第一径的能量太低MUSIC的角度估计锁定到了反射径上测距和角度矛盾解算出的坐标被拉偏。解决在triangulate_ls里启用权重机制。对每个基站用CIR的峰度kurtosis和第一径能量占比计算一个LOS置信度置信度低就把该基站的权重调低。这不能完全消除NLOS误差但能把误差的传播范围限制住整体定位精度可以从1米级别压回到40厘米左右。代码包里预留了这个接口。5. 用Matlab完整跑通一次仿真从场景构造到坐标输出5.1 代码文件组织和运行入口这套代码包的目录结构大致分成三块信号处理层、定位算法层和仿真验证层。信号处理层包含extract_cir.m和music_aoa.m这类基础函数定位算法层包含triangulate_ls.m和路径筛选函数仿真验证层则是一个入口脚本把前两层串起来并负责数据生成和结果绘图。运行入口是一个main_demo.m脚本它做的事情按顺序是设置仿真参数、构造多径场景、生成接收信号、调用信号处理层提取参数、调用定位算法层解算坐标、最后绘制误差图。我先看一遍信号处理层的输出是否合理再往下走定位层不要直接跑最后的定位结果。道理很简单如果CIR提取出的路径时延就不对后面的所有层都是白算。5.2 多径场景构造生成合成CIR与带噪测量仿真部分的核心是生成一条带多径的CIR。常规做法是设定一条直达径和若干条反射径每条路径有独立的时延、幅度和角度叠加后再加高斯白噪声。代码如下% main_demo.m 中的仿真参数配置 fs 500e6; % 采样率 500MHz fc 6.5e9; % UWB载波频率对应超宽带频段 c 3e8; % 三个基站的坐标单位米 anchor [0, 0; 8, 0; 4, 6]; % 标签真实位置 true_pos [3.2, 2.1]; % 构造多径CIR1条直达径 2条反射径 paths struct(); paths.delay [5, 8, 12]; % 每条路径的时延单位ns paths.amp [0.9, 0.4, 0.2]; % 每条路径的相对幅度 paths.aoa [35, -20, 60]; % 每条路径的到达角单位度 paths.power paths.amp.^2; % 叠加噪声 snr_dB 20; noise_power 10^(-snr_dB/10); cir_clean zeros(1, 80); for p 1:length(paths.delay) idx round(paths.delay(p) * fs * 1e-9) 1; cir_clean(idx) paths.amp(p); end cir_noisy cir_clean sqrt(noise_power) * randn(size(cir_clean));这里paths.delay的单位是ns转成样本点索引时用paths.delay(p) * fs * 1e-9500MHz采样率下1ns等于0.5个样本点所以5ns对应第3个样本点附近。这个转换是仿真里最容易错的地方单位不统一会导致时延整体偏移后面算出来的距离会差出十几米。paths.aoa在生成时画了下划线因为后续MUSIC估计出的角度是带噪声的估值真实值不直接参与定位。5.3 运行定位并评估误差定位结果评估用RMSE均方根误差来量化。对每个基站仿真过程中提取出的到达角和rTOF会加噪声加重程度由snr_dB控制。跑完100次蒙特卡洛仿真统计真实位置与估计位置的偏差% 蒙特卡洛仿真100次独立试验 n_trials 100; errors zeros(n_trials, 1); for trial 1:n_trials % 对每个基站的观测值加噪声 rho_meas zeros(3, 1); theta_meas zeros(3, 1); for i 1:3 rho_true norm(true_pos - anchor(i, :)); rho_meas(i) rho_true 0.05 * randn(); % 测距噪声 5cm theta_meas(i) paths.aoa(1) 1.5 * randn(); % 角度噪声 1.5度 end est_pos triangulate_ls(anchor, rho_meas, theta_meas); errors(trial) norm(est_pos - true_pos); end rmse sqrt(mean(errors.^2)); fprintf(RMSE %.3f m\n, rmse); % 绘制定位散点图 figure; plot(true_pos(1), true_pos(2), kp, MarkerSize, 12); hold on; for trial 1:50 % 只画前50次结果避免图面混乱 est_pos triangulate_ls(anchor, rho_meas, theta_meas); plot(est_pos(1), est_pos(2), b.); end axis equal; grid on;角度噪声的幅值取1.5度测距噪声取5厘米这两个数值对应市面上常见UWB模组在中等信噪比环境下的实测水平。如果后续要在真实设备上跑这些参数应该替换为你的设备标称精度。散点图的分布能直观看出定位偏差是否有方向性——如果50个点都偏在真实位置的同一侧说明系统存在系统误差优先检查天线延迟校准如果围绕真实位置均匀散布那就是随机噪声主导考虑提高角度估计精度。6. 进阶技巧把多径三角定位扩展应用到非视距场景的一个技巧非视距NLOS环境是UWB定位落地时绕不开的坎。前面提到的反射、遮挡、直达径能量衰减本质都是NLOS造成的问题。一个实用的技巧是不直接丢弃置信度低的基站而是把它加权进定位解算但权重按CIR的统计特征动态调整。具体做法是提取每个基站CIR的峰度和第一径能量占比。峰度描述CIR幅度分布的尖锐程度——LOS环境下直达径能量集中CIR的峰度高NLOS环境下能量分散在多个反射径上峰度明显降低。第一径能量占比同理直达径被遮挡时第一径能量占比会掉到很低。把这两个特征组合成一个LOS置信度分数归一化到0到1之间直接作为triangulate_ls里的权重。在真实测试中这样的动态加权比硬性丢弃NLOS基站能多保留一部分有效信息因为NLOS基站的测量里往往还残留一些可用的直达径成分全部丢掉反而损失几何约束。function w compute_los_weight(cir, noise_floor) % cir : 该基站的CIR序列 % noise_floor : 噪声基底功率估计值 energy_total sum(cir.^2); energy_first cir(1)^2; % 第一径能量前沿检测起点处 first_ratio energy_first / (energy_total eps); kurt kurtosis(cir(:)); % CIR峰度NLOS时明显下降 % 两个特征加权合成置信度 w 0.6 * first_ratio 0.4 * tanh(kurt / 3); w max(w, 0.05); % 设置下限防止完全丢弃某个基站 endw的下限设为0.05而不是0是为了防止某个基站的约束完全消失后矩阵条件数恶化。这是我调参过程中的血泪经验——第一次实现时我直接把低置信度基站的权重设为0结果某一帧只剩两个有效基站几何构型退化误差反而比三基站全用更大。从那以后我每次调定位权重都强制走一遍检查流程先看每个基站的CIR峰度和第一径占比再确认权重向量没有归零项最后才跑解算。希望这个技巧对你跑通这份代码包时处理NLOS环境有所帮助。本文还有配套的精品资源点击获取