最近几年UWB这个词出镜率很高从苹果AirTag到各种智能家居都把它当作定位追踪的“杀手锏”。不过把UWB用在自动跟随和避障上市面上现成的方案其实并不多见。我自己在搭建这套“UWB自动跟随避障系统”的时候踩了不少坑也积累了一些可复用的经验。这篇文章不聊虚的直接从方案选型讲到算法设计再到代码实现和实测数据把整个系统的设计研究过程拆开揉碎给你看。1. 为什么选UWB来做跟随先搞清楚定位方案的底牌自动跟随系统不是新概念市面上有蓝牙、Wi-Fi、视觉、激光雷达跟随等一堆方案。但我在选型阶段就锁定了UWB核心原因是它的三个物理特性恰好长在跟随场景的刚需上。厘米级测距精度UWB通过测量信号飞行时间TOF来计算距离理论精度能到10厘米以内。蓝牙RSSI受环境干扰严重误差经常以米为单位Wi-Fi定位在室内多径环境下更是飘得厉害。跟随这种任务距离误差如果超过30厘米控制系统就会开始“抖”所以精度是天花板指标。高刷新率与低延迟UWB的测距刷新率可以达到50Hz甚至更高延迟控制在毫秒级。这意味着机器人对目标位置变化的响应几乎是实时的。相比之下视觉方案需要经过图像采集、目标检测、坐标解算这一大串pipeline延迟普遍在100毫秒以上做高速跟随时会明显“钝”。抗多径干扰能力强UWB发射的是纳秒级窄脉冲在多径环境下能通过时间分辨把直射路径和反射路径分离开。这一点在室内有家具、玻璃、金属面的场景下比蓝牙和Wi-Fi的稳定性强得多。当然UWB也不是万能的。它的主要弱点是通信距离相对较短典型10-30米而且如果目标被人体或金属物体完全遮挡信号衰减会非常严重。所以在系统设计时需要把它和避障传感器做冗余融合而不是单靠UWB一条路走到底。2. 系统整体架构从标签到执行器的链路怎么搭整套系统的硬件链路其实不复杂但软件层面的数据流需要设计清楚。我的系统分为三个核心节点移动端标签Tag、跟随端基站Anchor、以及执行端的运动控制板。2.1 硬件选型清单模块作用推荐型号备注UWB模组测距/定位数据源DWM1000或DWM3000后者支持802.15.4z精度更高主控制器数据解算、算法决策STM32F407处理速度够外设丰富运动控制板驱动电机基于STM32F103的底盘驱动板与主控串口通信即可底盘执行机构双轮差速底盘为佳转向灵活控制模型简单避障传感器近距紧急制动超声波/单线激光雷达二选一建议加一个激光雷达做中距避障电源模块供电12V/5V双路输出注意UWB和电机驱动分开供电防干扰2.2 数据链路设计UWB模组输出的原始数据是测距值如果是双标签加锚点可以做二维坐标解算。这个数据经过串口传给STM32F407主控主控内部完成以下工作数据滤波卡尔曼滤波平滑距离值跟随控制算法解算目标速度速度指令通过串口下发到运动控制板运动控制板通过PID闭环控制电机输出PWM这里有个关键点UWB模组和电机驱动板一定要分电源供电。我试过共用电源结果电机启动的瞬间UWB测距值跳了20多厘米这精度根本没法用。加了个DC-DC隔离模块之后问题就消失了。3. UWB测距模块的底层原理与工程实现现在市面上的UWB模组基本把底层的测距协议封装好了直接读出距离值就能用。但如果你想做深入的性能调优必须理解它内部是怎么工作的。UWB测距的主流方式是双边双向测距DS-TWR。简单理解就是标签与基站之间来回发消息记录每一帧的发送和接收时间戳通过四次时间戳算出信号飞行时间再乘以光速就得到距离。Tprop (Tround1 * Tround2 - Treply1 * Treply2) / (Tround1 Tround2 Treply1 Treply2)这个公式的核心思想是抵消掉应答延迟带来的误差。如果只看单边测距SS-TWR时钟频率偏移会直接导致测距误差随距离线性放大双边测距则通过数学运算把时钟偏移的影响降了至少一个量级。实测下来DWM3000模组在室内LOS视距环境下静态测距的标准差大概在2-3厘米动态跟随中误差会放大到5-10厘米这个量级对跟随控制来说是可以接受的。3.1 天线朝向对测距的影响这个坑我必须拎出来单独说。UWB模组的全向天线其实不是真正的全向在朝向机器人背面或者夹角很大的时候测距值会出现周期性跳变。我的解决方法是使用两个UWB锚点分别安装在机器人前后端根据两个锚点的距离突变情况动态切换数据源对单点测距值做中值滤波窗口取5剔除毛刺如果你也打算做跟随建议把基站的安装位置抬高到1米左右这样可以有效降低人体对信号遮挡的影响。雷达放在膝盖高度的话目标转身时测距值能掉一半。3.2 标签端的续航与功耗控制标签是跟随目标随身携带的续航是实际体验的重要指标。DWM3000处于活跃测距状态时功耗大约在90mA如果加上MCU和LED指示总电流很容易超过150mA。一块800mAh的小锂电池撑不过6小时。要延长续航可以把标签的工作策略改成事件驱动——当接收信号强度RSSI或距离变化量超过一定阈值时才全速测距其余时间降频到1Hz心跳模式。这样实测能把平均电流压到40mA左右续航能到一天以上。4. 跟随控制算法从坐标解算到速度指令生成测距数据拿到之后最核心的问题就是怎么把“距离值”变成“机器人左右轮的速度指令”。4.1 绝对位置解算三角定位法单基站单标签只能提供距离没法提供方位。要实现真正意义上的“跟随”至少需要一个基站对两个锚点来构建二维平面坐标。我的方案是在跟随端放两个相距固定的UWB锚点间距L1米通过两个测距值 d1 和 d2利用三角测量法解算标签相对于机器人中心的坐标x (d1² - d2² L²) / (2L) y -sqrt(d1² - x²)这里要注意三角形的交点有两个y值一正一负。我会根据前一帧的位置连续性来判断该取哪个解否则标签走过锚点连线前方的中性面时坐标会瞬间翻转180度机器人会原地“打架”。4.2 跟随控制器的三环设计控制算法采用经典的PID三环架构角度环目标是让机器人朝向始终对准标签方向算出期望角速度 w距离环期望距离设为一个安全值我设为1.2米实际距离与期望距离的偏差通过PID输出期望线速度 v速度环线速度和角速度合成到左右轮速再交由底盘驱动板的电机PID闭环去跟随核心算法伪代码如下// 坐标解算 float d1 getRange(anchor1); float d2 getRange(anchor2); float x (d1*d1 - d2*d2 L*L) / (2*L); float y_abs sqrt(d1*d1 - x*x); float y (last_y 0) ? y_abs : -y_abs; // 角度和目标距离计算 float target_angle atan2(y, x); float current_angle robot_yaw; // IMU提供 float angle_error target_angle - current_angle; float distance d1; float distance_error distance - TARGET_DIST; // PID输出 float w Kp_angle * angle_error Kd_angle * (angle_error - last_angle_error)/dt; float v Kp_dist * distance_error Ki_dist * distance_error_sum; // 左右轮合成 float v_left v - w * WHEEL_BASE / 2; float v_right v w * WHEEL_BASE / 2;4.3 PID参数整定的血泪经验PID参数调起来是真的磨人。最开始我参考了网上别人给的默认参数结果机器人要么原地转圈要么冲出去又猛刹回来完全没法看。后面总结了一套自己的整定顺序先调角度环把距离环禁用单独给定一个固定线速度调比例和微分让机器人的转向足够平滑且少超调再调距离环给定固定角度调比例让机器人在直线上能稳定停住最后做联合整定这个时候要注意负载变化的影响。在电池电压从满电到亏电的过程中同样PWM对应的实际速度差别很大所以给速度控制加了一个前馈补偿直接把PID的输出量叠加一个与目标速度成正比的基准值实测下来我的参数大概是角度环Kp2.0Kd0.3距离环Kp1.5Ki0.05Kd0.2。这三个数值是可以直接当起步参考的但不同底盘一定要重新微调。5. 避障系统的设计与应急机制跟随做得再顺避障不过关就是花架子。我的系统里避障分为两级基于UWB距离的软减速和基于超声波/雷达的硬制动。5.1 为什么视觉方案在这里不是最优很多人第一反应是上视觉方案做避障我之前也试过YOLOv5部署到Jetson Nano上但最终放弃了原因有三个单目摄像头没有置信的深度信息靠近障碍物时测距误差大视觉推理有100ms以上延迟人走路速度大约是1.5m/s这一步算下来就是15厘米的距离差做紧急避让太滞后环境光照变了之后泛化能力急转直下还得做数据集扩充和重新训练维护成本高最终我选了单线激光雷达超声波的组合激光雷达负责中距离0.2m-2m的障碍物检测超声波负责近距0.02m-0.5m的盲区补偿。5.2 避障-跟随的仲裁逻辑这是整个系统设计里我花时间最多的地方。避障和跟随的目标在逻辑上天然互斥跟随想让机器人追着人走避障想让机器人躲开障碍物。我设计了一个简单的优先级仲裁器规则如下当机器人与障碍物的最近距离小于安全阈值0.5m立即进入避障模式跟随控制输出被冻结避障模式下机器人朝障碍物最少的方向后退或绕行直到障碍物距离大于阈值若UWB测距值在避障期间出现突变目标突然靠近或远离系统会以最近一次有效数据为基准重新规划路径避障超过5秒仍未解除系统判定为“目标丢失”机器人原地停车等待这一套逻辑下来系统可以应对“目标突然转身走进一个窄通道”、“机器人被墙角卡住”、“目标突然驻足回头”这些典型场景。5.3 紧急制动的冗余链路避障传感器的输出和主控逻辑之间我做了一条硬件冗余链路。超声波模块的测量结果直接接一个比较器电路当距离小于20厘米时比较器输出一个高电平直接打断电机驱动的EN使能引脚强制电机停止。这个设计的价值在于即使主控的代码跑飞卡死硬件层面仍然能保证机器人不会继续往前怼。从和避障相关的工程落地来说这个冗余设计是整台车安全性的压舱石。主控恢复正常前除非拨动下物理复位按键否则机器人不会恢复运动。这种设计对行人流量大的环境尤其重要。6. 软硬件集成与部署调试系统从裸板到能稳定跑起来经历了非常多的调试环节。这里我按时间线把自己走过的重要节点列出来作为参考。6.1 部署清单底盘组装与电机驱动验证UWB模组固件烧录与串口调试双锚点坐标解算模块的离线测试IMU姿态滤波与航向角解算PID基本运动控制调试跟随控制联调避障传感器安装与阈值标定避障与跟随仲裁策略部署长时间运动下的稳定性测试6.2 时间同步问题的处理UWB测距数据到达主控的节奏不是严格均匀的受无线干扰和重传机制影响而IMU数据的频率是固定的。如果直接把两路数据混着用角度环和距离环的时间基准会错位。我的处理方式是在主控里维护一个简单的时间戳队列把UWB测距数据按照它到串口的时间戳插入队列IMU数据用DMA定时采集并缓存。控制循环每运行一次都从两个队列中取出时间戳最接近一帧数据进行解算。这个方法不算高级但非常有效。实测控制频率稳定在50Hz左右时间偏差控制在1个控制周期内。6.3 部署现场容易忽略的三个细节底盘和UWB锚点的坐标系对齐锚点安装的朝向和底盘前进方向之间的夹角必须在软件里补偿掉否则机器人会一直往某个方向偏。我用了一个量角器手工标定的原始方法精度够用。标签位置不是固定的如果目标把标签从口袋移到手上跟随的目标点会变化表现为机器人忽远忽近。建议在标签端加一个高度传感器气压计或者干脆约定标签必须保持在腰部以上位置从使用规范上规避这个问题。全向轮的标识与检测如果场地地面有起伏差速轮打滑会让里程计漂移间接影响控制表现。这时候不能只依赖PID速度闭环建议加一个简单IMU航向闭环来修正轮速差导致的方向偏差。7. 实测数据与性能指标复盘最后说一下实测表现。在室内走廊环境光照均匀、地面平整、有少量金属立柱下我用这套系统做了50组跟随测试统计出的核心数据如下指标实测结果备注静态测距精度±3厘米方差很小动态跟随距离误差±10厘米正常步行速度下最大跟随速度1.2m/s再快会触发避障误判避障响应延迟100ms由激光雷达触发系统整体控制频率50Hz稳定一次充电连续运行6-8小时电池容量5000mAh从测试结果来看影响整体性能最大的瓶颈不是UWB本身的精度而是底盘电机的响应速度和避障传感器的布局。如果想让系统跑得更激进比如2m/s的跟随速度就要上高性能舵轮底盘和中远距离的3D雷达了。还有一个值得留意的数据在目标进行急转弯时UWB信号的遮挡程度会显著增加测距跳变概率从平时的2%上升到15%左右。因此对跳变数据的鲁棒性处理如中值滤波、异常值剔除、前向预测是整个系统稳定性的胜负手。8. 常见问题与排查技巧实录在实际开发和测试过程中我遇到了很多棘手的问题这里挑几个有代表性的记录下来按出现频率排序。8.1 机器人原地打转排查步骤先检查UWB坐标解算的Y轴是否频繁翻转在串口监视器打印raw数据和滤波后的坐标看是否在正负之间跳变。如果是将坐标切换逻辑改成基于历史轨迹预测而不是单纯靠上一帧符号锁定。8.2 跟随过程中突然加速冲向目标原因分析UWB信号跳变导致距离值突然变小距离环PID看到误差突变后给出一个非常大的速度指令。解决方案是给距离误差变化率加限制即把PID输出做限幅同时给距离值加一个基于里程计的前向预测——当机器人向前移动但UWB距离没有相应变小时判定距离值为异常数据。8.3 避障动作过于“神经质”表现为稍微有一点障碍物就停下来或者绕路绕得很远。根本原因是阈值设置太死板。优化方案是把安全阈值设计成与当前速度相关——速度越高阈值越大低速时阈值可以缩小到刚好能通过窄通道的宽度。代码如下float dynamic_threshold BASE_THRESHOLD fabs(current_speed) * 0.5f;这个改动非常有效机器人在低速通过窄缝和高速前进时的行为都自然了很多。8.4 测距值周期性跳动类似正弦波这个一般是天线方向性和多径效应叠加造成的尤其是标签和锚点高度一致反射面又多的时候。先尝试调整安装角度确保天线面大致朝向对方如果不行就把锚点位置错开大约半米高度比如一个在0.8米一个在1.3米形成空间分集。实测这个状态下跳动幅值能下降60%。8.5 跟随过程中系统失锁标签走出UWB有效通信范围或者被重物隔断信号后系统会失锁。我的处理方式是连续3秒没有有效测距数据就进入自动巡航模式机器人缓速向前移动并不断尝试重新建链同时扬声器播放提示音提醒目标“走得太远了”。9. 个人的工程设计复盘与展望整套系统从零开始做到稳定跑起来前后大概花了我六周时间其中有两周完全耗在PID整定和UWB跳变数据的处理上。现在回头复盘有三点经验是非常想分享给想入坑的朋友的。模块化测试比整体联调更重要。我一开始直接把UWB和控制算法放在一起调出了问题根本分不清是测距不准还是控制不稳。后面改成每个模块先独立验证模块间通过约定的数据格式对接效率提升了不止一倍。数据滤波不是越猛越好。我曾经用过一个窗口长度达到20的中值滤波器测距曲线确实平滑得像丝一样但系统响应速度也缓慢到令人崩溃——目标走了半米机器人还没反应过来。最后折中到窗口长度5再用一个轻量级的卡尔曼滤波处理动态过程平衡了平滑性和响应性。测试场景要覆盖极端情况。只在标准环境下测试并不可靠。我后期加入了一些窄通道、玻璃墙、金属门等特殊地形测试通过这些场景把很多只在特定环境下才暴露的问题都逼了出来。尤其是玻璃墙对UWB信号的反射相当致命单纯依赖UWB而不融合雷达数据的话玻璃门一挡基本就“眼瞎”了。关于后续的扩展UWB本身可以升级到多锚点组网来实现室内全局定位这样跟随系统就能与场景中的其他设备联动。比如可以做一个密集人流环境中自动跟随购物车的同时提前规避迎面走来的行人时间复杂度并不高但实用性很强。另外如果标签端增加惯性传感器如六轴IMU把UWB的绝对位置和IMU的短期推演融合起来可以进一步缩小信号被遮挡瞬间的定位空白让跟随动作更加连贯。这套系统的代码和硬件设计文件我已经整理归档里面包含了完整的主控程序、底盘驱动配置以及UWB通信协议封装做嵌入式或者机器人方向的朋友如果感兴趣完全可以拿来做二次开发的起点。设计过程中的测试记录和参数整定日志也一并保留了下来后续有空的话我会把避障仲裁策略的完整推导和调参表格也整理整理继续分享出来。