简介：本资源为基于Python的UWB超宽带室内定位系统源码包，面向从事室内定位算法研究、嵌入式开发及物联网应用的学生与工程师。项目采用迭代最小二乘法不断更新目标位置估计以提升定位精度，已实现位置解算并可在TREK1000设备上运行，同时附带Arduino与MATLAB实现，便于跨平台验证与移植。压缩包共975个文件，以h头文件与cpp源文件为主，另有ino、m、py脚本及umfpack、svd、sparseqr等稀疏矩阵求解模块，整体约3.45MB，目录结构清晰，涵盖锚点位置定义、模拟距离测量与算法验证等模块。目前已有201人学习下载。读者可从中获取完整的定位解算流程、多语言实现参考与排错思路，适合作为课程设计、竞赛项目或科研原型的基础框架，后续可在此基础上提升鲁棒性并完善文档。
1. 从一份 Python UWB 定位系统源码说起：它能解决什么、适合谁
如果你手上正好有一份基于 Python 的 UWB 定位系统源码，或者正在评估要不要自己搭一套室内定位系统，那这篇笔记就是写给你的。UWB（超宽带）定位这两年从实验室走向了工程现场，核心原因很简单：它能在室内做到厘米级精度，而 Wi-Fi、蓝牙这类方案在同样场景下往往只能给到米级。Python 在这里扮演的角色不是"跑在标签上的固件"，而是上位机侧的数据采集、解算、可视化和算法验证层——标签和基站负责测距，Python 负责把距离变成坐标、把坐标变成轨迹、把轨迹变成能看懂的图。
这份源码能解决的问题很具体：多基站 TWR（双向测距）或 TDoA 数据的串口读取、距离解算、最小二乘或三边定位、实时轨迹绘制、历史数据落盘。适合谁？做机器人室内导航的、做仓储人员/资产定位的、做实验室定位算法验证的、以及想拿一套能跑通的 Python 工程当二次开发底座的开发者。不适合谁？想直接拿去做室外 GPS 替代的、要求毫秒级硬实时的、以及完全不想碰硬件只想要纯软件仿真的——后者可以看，但价值有限。
我见过太多人拿到源码后卡在第一步：环境装不上、串口读不到、坐标全是 NaN。所以下面不按"源码目录结构"讲，而是按"你要让它跑起来需要理解什么、改什么、避什么"来讲。
2. UWB 定位的数学底座：从距离到坐标到底怎么算
2.1 三边定位为什么在真实场景里经常解不出坐标
三边定位（Trilateration）的原理教科书上写得很清楚：已知三个基站坐标和标签到三个基站的距离，联立三个圆方程求交点。但真实场景里，三个圆往往不相交于一点——因为测距有噪声，圆会"错开"。这时候硬解方程组会得到复数解或者无解，程序就崩了或者输出 NaN。
工程上的做法是把这个问题转成优化问题：找一组坐标 (x, y)，使得它到各基站的距离与实测距离的残差平方和最小。这就是最小二乘。二维场景下至少需要 3 个基站，三维需要 4 个。基站数量越多，冗余方程越多，解越稳，但计算量也上去。
import numpy as np from scipy.optimize import least_squares def residuals(pos, anchors, distances): """ pos: 待求坐标 [x, y] anchors: 基站坐标列表 [[x1,y1], [x2,y2], ...] distances: 标签到各基站的实测距离 [d1, d2, ...] 返回: 每个基站的测距残差 """ pos = np.asarray(pos) anchors = np.asarray(anchors) # 计算当前估计位置到各基站的计算距离 calc = np.linalg.norm(anchors - pos, axis=1) return calc - np.asarray(distances) def solve_position(anchors, distances, init=None): """用最小二乘求解标签坐标""" if init is None: # 初值取所有基站坐标的均值，避免从原点出发陷入局部解 init = np.mean(anchors, axis=0) res = least_squares(residuals, init, args=(anchors, distances)) return res.x, res.cost这段代码的关键在least_squares的初值选择。如果初值取 (0,0)，而基站都在远离原点的位置，优化器可能收敛到错误的局部极小。取基站坐标均值是个稳妥的起点。res.cost是残差平方和，可以拿来做质量判断——cost 突然变大说明这一帧测距数据有问题，应该丢弃而不是硬解。
2.2 TWR 和 TDoA 两条路线，Python 侧处理逻辑差在哪
TWR（Two-Way Ranging）是标签主动和每个基站依次测距，得到的是标签到各基站的绝对距离。优点是实现简单、标签和基站时钟不需要严格同步；缺点是标签容量低，多个标签同时工作时会碰撞，刷新率上不去。
TDoA（Time Difference of Arrival）是标签发一次广播，所有基站接收，通过到达时间差来定位。优点是标签容量大、刷新率高；缺点是需要基站之间时钟严格同步，硬件成本高，解算也更复杂——你拿到的是时间差，要先转成距离差，再用双曲线方程求解。
Python 侧的区别体现在数据入口：TWR 模式下你从串口读到的是"标签ID + 基站ID + 距离"这样的三元组，直接进最小二乘就行。TDoA 模式下读到的是各基站的到达时间戳，你需要先做时钟同步校正，再算时间差，再乘光速得到距离差，最后用 Chan 算法或 Taylor 展开求解。源码里如果只实现了 TWR，想上 TDoA 不是改几行的事，整个数据流都要重写。
提示：选路线之前先想清楚你的场景是"少量标签高精度"还是"大量标签中等精度"。前者 TWR 够用，后者必须 TDoA。
2.3 坐标系对齐：为什么你的轨迹图看起来是歪的
基站坐标是你自己摆的，但你在代码里填的坐标值必须和实际摆放一致。常见错误是：基站实际摆成一个矩形，但代码里填的坐标是随手写的，结果解出来的轨迹整体旋转或缩放。另一个坑是单位——UWB 模块输出的距离单位可能是毫米、厘米或米，如果你填的基站坐标是米而距离是毫米，解出来的坐标会差三个数量级。
# 基站坐标配置示例，单位统一为米 ANCHORS = [ [0.0, 0.0], # 基站A [5.0, 0.0], # 基站B [5.0, 3.0], # 基站C [0.0, 3.0], # 基站D ] # 距离单位转换：如果模块输出毫米，统一转米 def normalize_distance(raw_dist, unit='mm'): if unit == 'mm': return raw_dist / 1000.0 elif unit == 'cm': return raw_dist / 100.0 return raw_dist坐标系对齐的验证方法：把标签放在一个已知坐标的位置（比如基站A正前方1米处），看解算结果是否接近 (1.0, 0.0)。如果偏差大，先查单位，再查基站坐标填写，最后查天线延迟补偿。
3. 把源码跑起来：环境、串口、数据流三步走
3.1 Python 环境与依赖安装的最小可用集
这份源码大概率依赖 numpy、scipy、pyserial、matplotlib 这几个库。Python 版本建议 3.8 以上，太老的版本 scipy 装不上。安装命令：
# 创建虚拟环境，避免污染系统 Python python -m venv uwb_env source uwb_env/bin/activate # Linux/Mac # uwb_env\Scripts\activate # Windows # 安装核心依赖 pip install numpy scipy pyserial matplotlib如果你用的是 Anaconda，conda install numpy scipy pyserial matplotlib也行。装完验证一下：
import numpy, scipy, serial, matplotlib print(numpy.__version__, scipy.__version__, serial.__version__)版本号不要求完全一致，但 numpy 建议 1.20 以上，scipy 建议 1.7 以上。如果pip install卡在编译环节，多半是 scipy 在从源码编译，换成pip install scipy --only-binary :all:强制用预编译包。
3.2 串口读取：波特率、超时和粘包处理
UWB 模块通常通过 USB 转串口和上位机通信。Windows 下是 COM3、COM4 这种，Linux 下是 /dev/ttyUSB0 或 /dev/ttyACM0。读串口最常见的三个坑：波特率不对（读出来全是乱码）、超时设置不当（程序卡死）、粘包（多帧数据粘在一起解析错位）。
import serial import time def open_serial(port, baudrate=115200, timeout=0.1): """打开串口，timeout 设小一点避免阻塞""" ser = serial.Serial( port=port, baudrate=baudrate, bytesize=serial.EIGHTBITS, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE, timeout=timeout ) return ser def read_frames(ser, frame_header=b'\xAA\xBB', frame_len=16): """ 按帧头解析，处理粘包 frame_header: 帧起始标志 frame_len: 单帧总长度 """ buffer = b'' while True: chunk = ser.read(ser.in_waiting or 1) if chunk: buffer += chunk # 在缓冲区里找帧头 idx = buffer.find(frame_header) if idx == -1: # 没找到帧头，保留最后几个字节防止帧头被截断 buffer = buffer[-len(frame_header):] continue # 丢弃帧头之前的数据 buffer = buffer[idx:] if len(buffer) >= frame_len: frame = buffer[:frame_len] buffer = buffer[frame_len:] yield frame time.sleep(0.001)timeout=0.1意味着 read 最多阻塞 0.1 秒就返回，不会让程序卡死。粘包处理的核心是维护一个缓冲区，每次从缓冲区里找帧头，找到后判断是否够一帧长度，够了就切出来，不够就等下一批数据。帧头和帧长度要根据你实际用的 UWB 模块协议来改，不同厂家不一样。
3.3 数据流串起来：从一帧原始数据到屏幕上的一个点
完整的数据流是：串口读帧 → 解析出标签ID、基站ID、距离 → 按标签ID分组 → 对每个标签调用定位解算 → 得到坐标 → 存入轨迹队列 → 绘图刷新。这个链条里任何一环出问题，屏幕上都看不到点。
from collections import defaultdict, deque class UWBPipeline: def __init__(self, anchors, max_trail=200): self.anchors = anchors self.distances = defaultdict(dict) # {tag_id: {anchor_id: dist}} self.trails = defaultdict(lambda: deque(maxlen=max_trail)) def feed(self, tag_id, anchor_id, distance): """喂入一条测距数据""" self.distances[tag_id][anchor_id] = distance # 当某个标签的距离数够定位时，触发解算 if len(self.distances[tag_id]) >= 3: dist_list = [] anchor_list = [] for aid, d in self.distances[tag_id].items(): anchor_list.append(self.anchors[aid]) dist_list.append(d) pos, cost = solve_position(anchor_list, dist_list) if cost < 1.0: # 残差过大则丢弃 self.trails[tag_id].append(pos) return pos, cost return None, Nonemaxlen=200控制轨迹保留的点数，避免内存无限增长。cost < 1.0这个阈值需要根据你的实际测距噪声调整——噪声大就放宽，要求高就收紧。这个类把数据流封装起来，主循环里只需要不断调feed，然后定期重绘就行。
4. 避坑与排查：UWB 定位系统跑不通的 5 个高频原因
4.1 现象：串口能打开但读不到任何数据
原因通常有三个：一是波特率不匹配，模块用的是 9600 而你设了 115200；二是接线问题，TX/RX 接反了；三是模块没上电或者没进入数据输出模式。
解决：先用串口助手（Windows 下用 SSCOM，Linux 下用 minicom）手动连一下，确认能看到数据。如果串口助手也看不到，就是硬件或接线问题，跟 Python 代码无关。确认波特率的方法是查模块手册，或者逐个试常见波特率（9600、19200、115200、460800）。
4.2 现象：解算出的坐标偶尔跳到很远的地方
原因是某一帧的测距值出现了野值（outlier），比如多径反射导致测距突然大了好几米，最小二乘被这个野值带偏了。
解决：在解算前做一层滤波。最简单的是中值滤波——对每个基站的距离维护一个长度为 5 的滑动窗口，取中值参与解算。进阶一点用卡尔曼滤波，把位置和速度作为状态量，测距作为观测，能平滑轨迹同时抑制野值。
from collections import deque import numpy as np class MedianFilter: def __init__(self, window=5): self.buf = deque(maxlen=window) def update(self, value): self.buf.append(value) return float(np.median(self.buf))4.3 现象：轨迹整体偏移，但形状是对的
原因是天线延迟没有补偿。UWB 测距包含射频前端和天线的固定延迟，这个延迟会表现为一个固定的距离偏差，通常几十厘米。所有距离都偏同一个值，解出来的坐标就整体平移。
解决：做一次校准。把标签放在已知距离（比如正好 1 米）处，读测距值，差值就是延迟补偿量。在代码里对每个基站的距离减去这个补偿值。注意每个基站的天线延迟可能不同，严格做法是逐基站校准。
4.4 现象：多标签时数据串了，A 标签的坐标出现在 B 标签的轨迹上
原因是解析帧的时候没有正确区分标签ID，或者字典的 key 用错了。多标签场景下，数据帧里一定有一个字段标识标签ID，解析时必须按这个字段分组。
解决：检查解析代码里 tag_id 的提取位置是否正确，打印几帧原始数据看看。另外注意 defaultdict 的用法——self.distances[tag_id][anchor_id] = distance这种嵌套字典，如果 tag_id 提取错了，数据就会串。
4.5 现象：程序跑几分钟后越来越卡，最后无响应
原因是绘图在主线程里频繁刷新，或者轨迹队列没有限制长度导致内存增长。matplotlib 的plt.pause在数据量大时会成为瓶颈。
解决：把绘图和数据读取分到两个线程，用队列通信。或者降低绘图刷新率，比如每收到 10 帧才重绘一次。轨迹队列一定要设 maxlen，不然跑一晚上内存就爆了。
import threading import queue data_queue = queue.Queue(maxsize=1000) def serial_worker(port): ser = open_serial(port) for frame in read_frames(ser): if not data_queue.full(): data_queue.put(frame) # 主线程从 data_queue 取数据做解算和绘图 t = threading.Thread(target=serial_worker, args=('COM3',), daemon=True) t.start()daemon=True保证主线程退出时子线程也跟着退出。maxsize=1000防止生产太快消费太慢导致内存堆积。
5. 进阶技巧：用残差和基站几何布局反推系统瓶颈
5.1 残差曲线是最好的诊断工具
很多人跑通系统后就不看残差了，只看轨迹图。但轨迹图只能告诉你"结果对不对"，残差曲线能告诉你"为什么不对"。把每帧解算的 cost 画成时间序列，正常情况应该在某个基线附近波动。如果残差突然尖峰，说明那一帧有野值；如果残差整体缓慢上升，说明某个基站的距离在漂移，可能是硬件发热或者电池电压下降。
import matplotlib.pyplot as plt cost_history = deque(maxlen=500) # 在解算循环里 pos, cost = pipeline.feed(tag_id, anchor_id, distance) if cost is not None: cost_history.append(cost) # 定期绘制 plt.clf() plt.subplot(2, 1, 1) plt.plot(list(cost_history)) plt.ylabel('Residual Cost') plt.subplot(2, 1, 2) for tag_id, trail in pipeline.trails.items(): if len(trail) > 1: arr = np.array(trail) plt.plot(arr[:, 0], arr[:, 1], label=f'Tag {tag_id}') plt.legend() plt.pause(0.01)5.2 基站布局的几何精度因子（GDOP）
同样的测距噪声，基站摆成什么形状，解算出来的坐标精度差别很大。如果四个基站几乎在一条直线上，垂直于这条线的方向精度会极差。理想布局是基站均匀分布在定位区域四周，且不要共线。一个简单的判断方法：算一下基站坐标矩阵的条件数，条件数越大，布局越差。
def gdop_score(anchors): """粗略评估基站布局质量，值越小越好""" A = np.asarray(anchors) # 中心化后算奇异值 A_centered = A - A.mean(axis=0) s = np.linalg.svd(A_centered, compute_uv=False) return s[0] / s[-1] # 条件数这个值如果超过 10，说明布局偏"扁"，考虑调整基站位置。实际部署时，基站高度也要考虑——如果标签和基站不在同一水平面，二维解算会有系统偏差，要么做三维解算，要么把基站和标签尽量放在同一高度。
5.3 我自己的习惯：先仿真再上硬件
每次改定位算法，我不会直接拿硬件试，而是先在 Python 里造一组仿真数据：给定真实轨迹，加上高斯噪声和随机野值，跑一遍解算看误差。这样改算法迭代快，也不会因为硬件问题干扰判断。仿真通过后再上真实数据，通常一次就能跑通。
def simulate(true_traj, anchors, noise_std=0.1, outlier_ratio=0.05): """生成仿真测距数据""" data = [] for pos in true_traj: dists = [] for a in anchors: d = np.linalg.norm(np.array(a) - np.array(pos)) d += np.random.normal(0, noise_std) if np.random.rand() < outlier_ratio: d += np.random.uniform(1, 5) # 野值 dists.append(d) data.append(dists) return data这个习惯帮我省了很多时间。硬件调试的成本远高于仿真，能仿真验证的逻辑就不要上硬件试。希望帮到你。
本文还有配套的精品资源，点击获取
