简介：这份PDF文档面向智能硬件开发者、嵌入式工程师及自动化相关专业学生，聚焦智能跟随车定位精度不足的痛点，给出了一套基于UWB信号的完整导航定位算法方案。文档围绕TOA测距、卡尔曼滤波去噪、三角函数解算距离与偏移角度展开，并衔接PID控制调节PWM以驱动电机转速与转向，实验数据显示距离误差小于9cm、角度误差小于10°。资源包内仅含1个PDF文件，大小约439KB，内容涵盖系统总体设计、射频与高度补偿等硬件模块、主程序与滤波算法软件流程，以及9点测试的误差分析数据，结构完整、便于按章节查阅。目前已有2597人学习下载，适合需要快速掌握UWB跟随车定位原理、复现实验或撰写相关课题报告的技术人员参考。
1. 基于UWB的智能跟随车导航定位算法研究：从测距到跟随的工程落地
做跟随车最头疼的不是电机控制，而是“它到底知不知道我在哪”。红外容易被阳光干扰，视觉怕遮挡和光照突变，蓝牙RSSI精度飘到米级，而UWB（超宽带）用纳秒级窄脉冲测距，在视距条件下能把误差压到十厘米级，这才让“车跟着人走”从演示变成可用的产品。这套方案适合做服务机器人、高尔夫球童车、行李跟随箱、工厂物料搬运车的团队，也适合已经有一台差速底盘、想加跟随功能的开发者。核心链路是：标签周期性发测距帧，车端至少三个基站收信号，用TDOA或TW-TOF解算相对坐标，再把坐标送进跟随控制器。整条链路里，定位算法决定了跟随的“手感”，参数没调好，车就会画龙、急停、甚至跟丢。下面按硬件选型、测距解算、坐标变换、跟随控制、避坑、调优验证的顺序，把可复现的细节讲清楚。
2. UWB测距与定位解算：从DS-TWR到三边定位
2.1 为什么选DS-TWR而不是SS-TWR
单边双向测距（SS-TWR）只测一次往返，标签和基站时钟不同步带来的误差会直接乘进距离里。双边双向测距（DS-TWR）做两次往返，把时钟偏移项消掉，代价是测距时间翻倍。跟随车场景里标签和车都在动，刷新率比绝对精度更敏感，所以常见做法是DS-TWR配10Hz到20Hz的测距频率。如果基站和标签用同一颗晶振且做了时钟同步，SS-TWR也能用，但工程上为了省事，直接上DS-TWR更稳。
测距帧的交互逻辑是：标签发Poll，基站回Resp，标签再发Final，基站算出飞行时间。关键参数是前导码长度和脉冲重复频率（PRF）。前导码越长，抗干扰越强，但单帧时间变长；PRF选64MHz还是16MHz，取决于当地频谱法规和你要的距离分辨率。我一般先用64MHz PRF、前导码1024，在视距下跑通，再根据现场干扰调整。
2.2 三边定位的最小二乘解法
拿到标签到三个基站的距离开，就可以解二维坐标。设基站坐标已知为$(x_i, y_i)$，标签坐标$(x, y)$，距离$d_i$，方程是：
$$(x-x_i)^2 + (y-y_i)^2 = d_i^2$$
直接相减消掉二次项，得到线性方程组$Ax=b$，再用最小二乘解。三个基站时是超定方程，四个以上更稳。下面是一段Python实现，输入是基站坐标列表和测距值列表，输出标签坐标。
import numpy as np def trilateration(anchors, distances): """ anchors: [(x0,y0), (x1,y1), ...] 至少3个 distances: [d0, d1, ...] 对应测距值，单位米 返回: (x, y) 标签估计坐标 """ anchors = np.array(anchors, dtype=float) distances = np.array(distances, dtype=float) n = len(anchors) if n < 3: raise ValueError("至少需要3个基站") # 以第一个基站为参考，构造线性方程 A = [] b = [] x0, y0 = anchors[0] d0 = distances[0] for i in range(1, n): xi, yi = anchors[i] di = distances[i] A.append([2*(xi - x0), 2*(yi - y0)]) b.append(d0**2 - di**2 + xi**2 - x0**2 + yi**2 - y0**2) A = np.array(A) b = np.array(b) # 最小二乘解 pos, residuals, rank, s = np.linalg.lstsq(A, b, rcond=None) return pos[0], pos[1]这段代码的逻辑是：把非线性圆方程两两相减，转成线性形式，再用lstsq求最小二乘解。参数说明：anchors的顺序要和distances一一对应，否则解会飞。实际部署时，基站不要共线，最好呈三角形或矩形分布，高度差不要太大，否则二维假设会引入误差。如果测距值有粗大误差，先做一遍中值滤波或RANSAC再送进解算，不然一个坏点就能把坐标拉偏半米。
2.3 坐标解算后的平滑处理
原始解算坐标抖动在±5cm到±15cm之间，直接送给速度环会让车一顿一顿。常见做法是加一阶低通或卡尔曼滤波。一阶低通实现简单，但会引入相位滞后；卡尔曼滤波能融合运动模型，滞后小，但调参麻烦。我一般先用一阶低通把高频噪声压掉，截止频率设在2Hz到5Hz，再根据跟随效果决定要不要上卡尔曼。
class LowPassFilter: def __init__(self, alpha=0.3): self.alpha = alpha self.value = None def update(self, x): if self.value is None: self.value = x else: self.value = self.alpha * x + (1 - self.alpha) * self.value return self.valuealpha越小越平滑，但滞后越大。跟随车场景里，alpha取0.2到0.4比较合适，具体看你的控制周期。如果车速快，alpha要调大，否则车会反应迟钝。
3. 从标签坐标到车体控制：跟随算法的工程实现
3.1 相对坐标变换与跟随距离保持
解算出来的是标签在基站坐标系下的坐标，车要跟的是“标签相对车体的位置”。所以先要把标签坐标转到车体坐标系：车体原点在底盘中心，x轴朝前，y轴朝左。设车体位姿为$(x_c, y_c, \theta)$，标签坐标$(x_t, y_t)$，则相对坐标：
$$\begin{bmatrix} x_r \ y_r \end{bmatrix} = \begin{bmatrix} \cos\theta & \sin\theta \ -\sin\theta & \cos\theta \end{bmatrix} \begin{bmatrix} x_t - x_c \ y_t - y_c \end{bmatrix}$$
车体位姿从里程计或IMU获得。如果车没有里程计，可以用UWB基站阵列直接解算车体位姿，但那样成本高。常见做法是底盘带编码器，航向用陀螺仪，融合出粗略位姿。
跟随距离保持用比例控制：期望距离$d_{des}$，实际距离$d_r = \sqrt{x_r^2 + y_r^2}$，线速度$v = k_p (d_r - d_{des})$，限制在最大速度内。角速度用相对方位角$\phi = \arctan2(y_r, x_r)$，$\omega = k_\phi \phi$。这样车会先转向标签，再前进或后退。
3.2 速度环与转向环的参数整定
速度环和转向环分开调。先调转向环：把车架空，给一个固定方位角误差，看车转向响应。$k_\phi$太小，车转得慢，跟丢；太大，车会左右摆。我一般从0.8开始，每次加0.2，直到出现轻微超调，再退回一档。速度环$k_p$从0.5开始，观察车接近标签时是否减速平滑。如果车在目标距离附近来回窜，说明$k_p$太大或滤波滞后太大。
def follow_control(xr, yr, d_des=1.5, kp=0.6, kphi=1.2, vmax=0.8, wmax=1.5): """ xr, yr: 标签在车体坐标系下的相对坐标，单位米 d_des: 期望跟随距离，单位米 kp, kphi: 线速度、角速度比例系数 vmax, wmax: 速度限幅 返回: (v, w) 线速度、角速度指令 """ import math dr = math.hypot(xr, yr) phi = math.atan2(yr, xr) v = kp * (dr - d_des) w = kphi * phi # 限幅 v = max(-vmax, min(vmax, v)) w = max(-wmax, min(wmax, w)) # 如果方位角误差太大，先原地转，不走 if abs(phi) > 1.0: v = 0.0 return v, w这段代码里，d_des是你要保持的距离，一般1.2m到2m，看车体大小和传感器视场。kp和kphi需要实车调。abs(phi) > 1.0这个条件是防止车在标签侧后方时直接冲出去，先转正再走。实际调试时，先把vmax设小，比如0.3m/s，跑顺了再往上加。
3.3 跟随丢失与重捕获策略
标签被遮挡或测距超时，解算坐标会跳变或消失。直接用车会失控。常见做法是：连续N帧没有有效测距，就进入丢失状态，车减速停止，同时原地缓慢旋转搜索标签。N一般取5到10，对应0.5s到1s。重捕获后，不要立刻全速跟，先以低速接近，确认稳定再恢复。
class FollowState: def __init__(self, lost_threshold=8): self.lost_count = 0 self.lost_threshold = lost_threshold self.state = "tracking" # tracking / lost / recovering def update(self, has_valid_measurement): if has_valid_measurement: if self.state == "lost": self.state = "recovering" self.lost_count = 0 else: self.lost_count += 1 if self.lost_count >= self.lost_threshold: self.state = "lost" return self.state这个状态机很简单，但能避免很多翻车。lost_threshold根据你的测距频率定，10Hz测距时取8就是0.8秒。如果现场遮挡频繁，可以适当加大，但太大车会撞上障碍物才反应。
4. 避坑与常见问题排查：UWB跟随车的血泪经验
4.1 测距值跳变导致车画龙
现象：车在直线上跟随时左右摆，幅度越来越大。原因：UWB测距在多径环境下出现粗大误差，解算坐标跳变，速度环跟着抖。解决：先看原始测距值，如果某几个基站跳变明显，加中值滤波；如果所有基站都跳，检查基站安装位置是否靠近金属或地面，UWB天线要离地至少30cm，远离金属反射面。另外，解算前做一遍RANSAC，把残差大的基站剔除。
4.2 基站共线导致解算无解或镜像
现象：标签在某个区域坐标突然反到另一边。原因：三个基站近似共线，线性方程组条件数极差，最小二乘解不稳定。解决：基站不要排成一条直线，最好呈L形或三角形，高度差不要超过20cm。如果场地限制只能共线，加第四个基站，或者用TDOA代替TOF。
4.3 标签天线方向性导致距离突变
现象：人转身时，车突然加速或减速。原因：标签天线有方向性，人体遮挡或极化方向变化导致信号强度变化，测距值偏移。解决：标签天线尽量全向，或者用双天线分集。人身上佩戴时，标签不要贴肉，离身体至少5cm。如果还是跳，在跟随算法里对距离做限幅，单帧变化超过0.5m就丢弃。
4.4 控制周期与测距周期不匹配
现象：车响应迟钝，或者一冲一冲。原因：测距10Hz，控制50Hz，中间没有新数据时控制用了旧坐标。解决：控制周期和测距周期对齐，或者用预测算法外推。简单做法是控制周期降到20Hz，和测距周期接近。如果必须高频控制，加一个一阶保持器，用最近两帧坐标外推。
4.5 多径导致近距离测距偏大
现象：标签离基站很近时，测距值比实际大几十厘米。原因：近距离多径叠加，首达路径检测错误。解决：在算法里加近距离补偿，或者用双向测距的对称性做校验。硬件上，降低发射功率，减少反射。如果场地固定，可以做一次现场标定，把误差拟合成距离的函数，在解算前补偿。
5. 进阶调优与验证：让跟随车真正可用的几个技巧
5.1 用日志回放做参数整定
实车调参效率低，而且危险。我一般先把UWB原始测距、解算坐标、控制指令全部打到日志里，存成CSV，然后在PC上回放。回放时改滤波参数和控制参数，看轨迹和速度曲线，找到一组候选，再上车验证。这样能把调参时间从几天压到几小时。日志字段至少包括：时间戳、各基站测距值、解算坐标、车体位姿、速度指令。
import csv import time class Logger: def __init__(self, filename): self.f = open(filename, 'w', newline='') self.writer = csv.writer(self.f) self.writer.writerow(['t', 'd0', 'd1', 'd2', 'x', 'y', 'v', 'w']) def log(self, d_list, pos, v, w): self.writer.writerow([time.time()] + d_list + [pos[0], pos[1], v, w]) self.f.flush() def close(self): self.f.close()回放时，把d_list重新送进解算和滤波，改参数看输出。注意日志时间戳要统一，最好用同一个时钟源。
5.2 验证定位精度的几个土办法
没有激光跟踪仪也能验证。找一块空地，在地面画网格，间距0.5m，把标签放在已知点上，记录解算坐标，算误差。每个点采100帧，看均值和标准差。视距下，均值误差应该小于10cm，标准差小于5cm。如果超了，先查基站坐标是否准，再查天线延迟是否校准。UWB模块一般有天线延迟参数，出厂值不一定准，可以用已知距离标定。
5.3 跟随效果的量化指标
别只看“跟得稳不稳”，要量化。常用指标：距离跟踪误差（实际距离与期望距离的均方根）、方位角跟踪误差、速度指令的抖动幅度。距离跟踪误差在0.1m以内算合格，0.05m以内算优秀。方位角误差在5度以内。抖动幅度看速度指令的频谱，如果高频分量大，说明滤波或控制参数有问题。
5.4 一个容易被忽略的细节：基站供电与时钟同步
UWB基站对电源噪声敏感，开关电源纹波大会导致测距抖动。我一般用线性稳压器给基站供电，或者加LC滤波。另外，如果基站之间没有有线时钟同步，每个基站的时钟独立，DS-TWR能消掉标签和基站的时钟偏移，但基站之间的时钟偏移会影响TDOA解算。如果用TOF，基站不需要同步；如果用TDOA，必须做时钟同步，常见做法是基站间有线同步或无线同步。跟随车场景里，TOF更简单，推荐优先用TOF。
最后说个我自己的教训：早期做跟随车时，总想把定位精度做到极致，结果忽略了控制周期和滤波滞后，车反而跟得不好。后来把测距频率提到20Hz，滤波截止频率放到5Hz，控制周期对齐到20Hz，跟随效果立刻上了一个台阶。定位算法是基础，但跟随的“手感”是定位、滤波、控制三者匹配出来的。希望帮到你。
本文还有配套的精品资源，点击获取
