UWB自动跟随避障系统设计实战：从定位原理到工程调优

去年在调试一台跟随行李箱原型机的时候我踩过最狠的一个坑UWB模块在空旷客厅里静态测距误差能稳定在±5cm但只要测试人突然侧过身测距值瞬间跳了40cm小车直接原地刹停。那一刻我意识到UWB自动跟随避障系统的难点从来不在芯片选型而在链路里的每一个细节。整套系统的本质就是用UWB做高精度相对定位再叠加避障传感器让移动底盘实时跟着目标跑、同时避开路上障碍。如果你正在做跟随小车、跟随行李架、巡检机器人这类项目这篇文章可以帮你理清从定位原理、硬件选型、算法设计到避障策略的完整设计思路也能帮你在实测阶段省下至少一周的调参时间。1. 自动跟随的定位链路为什么UWB比蓝牙和视觉方案更适合1.1 跟随系统要解决的第一性问题自动跟随系统的核心任务不是跑而是先搞清楚目标相对于自己在哪。这里的关键词是相对位姿——目标相对本体的距离、方位角以及两者的相对朝向。室外场景可以用RTK或者视觉SLAM但室内、半室外、遮挡较多的场景下RTK信号不稳视觉又受光照和目标外观变化影响很大。UWB超宽带技术的优势在于它直接测量无线电信号的飞行时间天然就能给出两个节点间的距离。只要在移动底盘上布置两个已知位置的定位基站目标身上佩戴一个标签就可以通过几何关系解算出目标在底盘坐标系中的坐标进而得到距离和角度。有了距离和角度差速底盘就能完成线速度与角速度的闭环控制。1.2 UWB测距为什么能到厘米级UWB和蓝牙、WiFi的本质区别在于信号带宽。UWB使用纳秒级以下的窄脉冲占用带宽通常在500MHz以上。时间分辨率越高测距精度就越高。光速约为3×10^8 m/s1ns的飞行时间对应约30cm的距离。如果系统能测到0.1ns级别的时延差理论上精度就能达到3cm。实际应用中UWB测距主要采用TWR双端双向测距方式也就是标签给基站发一个Poll帧基站回一个Response帧标签再发Final帧通过记录四个时间戳算出飞行时间。这种方法不需要标签和基站做高精度时钟同步规避了系统同步这个最大的工程难题。实测下来视距且天线姿态良好的情况下DWM1000模块的静态测距误差可以稳定在±5cm到±8cm。除此之外UWB的抗多径能力比WiFi和蓝牙强不少。脉冲信号在时间上很窄接收机可以通过首径检测识别出最先到达的直达路径把墙面、地面反射的多径信号区分开。这也是UWB能在室内金属边框、桌椅较多的环境里依然保持相对稳定的原因。1.3 和蓝牙、WiFi、视觉、激光雷达的取舍我早期也考虑过用蓝牙RSSI做跟随。蓝牙信号强度在天线角度稍微变一下之后就会剧烈波动测距误差动辄1-3米这个精度做跟随基本等于盲走。WiFi指纹定位需要提前建库环境一变就失效不适合移动场景。视觉方案可以做到无标签跟随通过训练目标检测模型锁定人形但需要较高算力而且光线昏暗、目标侧身或遮挡时很容易丢帧。激光雷达在测距和避障上很强但单台单线雷达的成本是UWB模块的好几倍而且只能获得平面二维信息无法直接识别到特定目标。UWB的分工很明确它不负责感知障碍物只负责跟踪目标。所以在这套系统里我选了UWB相对定位 超声波/ToF避障的组合把两件事拆开每件用最合适的传感器去做成本和稳定性都能兼顾。方案精度目标识别环境依赖适合跟随吗蓝牙RSSI米级无天线姿态影响大不适合Wi-Fi指纹米级无需要建库不适合UWB厘米级无需标签非视距敏感适合视觉变化大需模型训练光照影响大商用的多激光雷达厘米级无法区分目标对反射物敏感配合定位用关键结论UWB用目标佩戴标签换来了厘米级定位精度这个代价在跟随行李箱、跟随机器人这类场景中是完全可以接受的。2. 系统硬件搭建与天线布局的实操记录2.1 定位模块选型我用的是DWM1000我用的定位模块是DWM1000它基于Decawave DW1000芯片模块自带天线SPI接口直接和MCU通信。DWM1000支持多通道、多数据率4GHz频段和6.5GHz频段都可选。一般跟随场景我建议用850kbps或6.8Mbps的数据率前者接收灵敏度更高适合远距离后者数据交互更快适合高更新率。如果你用的是集成好的底板模块比如带I2C/串口输出测距值的型号开发会更简单但灵活度会低一些。我因为需要自己控制双向测距时序所以直接用裸模块挂在SPI总线上配合协议栈跑TWR。建议新手先从带底板、有串口AT指令的模块开始先把链路跑通再做底层移植。2.2 主控、底盘和避障传感器选型主控用的是STM32F103C8T6价格便宜、资料多SPI和PWM资源够用。如果不想折腾编译环境用ESP32也可以但要注意ESP32的ADC和PWM在某些情况下会有噪声测距模块要尽量远离电机驱动。底盘我选的是两轮差速底盘两个直流电机加一个万向轮转向半径小靠左右轮速差就能实现原地转圈非常契合跟随控制的需求。电机驱动用的TB6612相比L298N的压降小很多热量也低。避障传感器我选了两种互补正前方装一个超声波模块HC-SR04负责中近距离障碍探测量程3-400cm但盲区有3cm左右实际安全距离要留余量。左右前方各装一个VL53L0X ToF激光测距模块量程约200cm测距更准但视场角很小只能探测正前方向上的点。也可以用单线激光雷达做更全面的避障但成本高、点云处理复杂。我在原型阶段用超声波ToF足够验证整套控制逻辑了。2.3 天线布局最容易翻车的设计点UWB天线对金属、遮挡和极化方向极其敏感。我第一版样机把两个UWB基站贴在底盘铝板正上方间距只有12cm结果目标角度解算结果抖动得根本没法用。后来改成用支架把天线抬高到离金属板5cm以上两个基站的间距拉大到30cm角度稳定性立刻好了很多。这里有一个被很多人忽略的点两个基站之间的距离基线直接决定角度解算精度。目标角度是根据两个测距值之差计算出来的基线越短同样的测距误差算出来的角度误差就越大。举个例子两个基站间距10cm测距误差±10cm时角度误差可能超过45度间距拉到30cm同样的测距误差对应角度误差只有约19度。所以能拉多远就拉多远至少保证30cm以上。标签戴在目标身上也要注意。如果标签挂在侧面人体遮挡会让UWB信号变成非视距测距值会明显跳变。最好是让标签挂在胸前或腰前天线垂直朝上保证和底盘天线之间有一道相对干净的视距通道。2.4 电源设计的一个关键细节直流电机启动瞬间电流可能到2A以上如果UWB模块和主控跟电机驱动共用同一个电源芯片电机一转SPI数据就出错。我后来给电机驱动独立供电主控和UWB模块用另一路稳压输出中间再加一个LC滤波器UWB的丢包率从肉眼可见的频繁变成几乎为零。记住一句话在跟随系统里供电噪声比程序Bug更容易让UWB数据发疯。3. 跟随定位算法实现从原始测距到平滑的差速控制3.1 双基站坐标解算的推导把两个UWB基站看作底盘坐标系中的两个固定点。假设两个基站的中心在原点两个基站的坐标分别为A点(-L/2, 0)B点(L/2, 0)其中L是两个基站之间的间距。标签T的坐标设为(x, y)通过TWR测距分别得到距离d1 ATd2 BT。于是可以得到方程组(x L/2)^2 y^2 d1^2 (x - L/2)^2 y^2 d2^2两个式子相减把x解出来x (d1^2 - d2^2) / (2L)有了x之后代回任何一个式子可以求出y^2y^2 d1^2 - (x L/2)^2y取正值即可因为跟随目标基本都在底盘前方。最后得到目标相对底盘的极坐标r sqrt(x^2 y^2)θ atan2(x, y)这套解算逻辑我用C语言实现之后跑得非常顺但需要处理一个情况如果因为测距误差导致y^2小于0也就是两个圆根本没有交点说明本次测距值存在明显的多径干扰应该把这一帧标记为无效上帧结果保持或者用卡尔曼预测值补上。3.2 测距数据滤波先处理跳变再谈控制UWB原始测距值偶尔会出现0.3m到1m的跳变如果不做滤波PID控制器的微分项会被瞬间放大小车会突然猛打方向。我用的滤波组合是限幅滤波 滑动中值滤波 滑动均值滤波。限幅规则很简单如果当前测量值相对上一有效值的偏差超过0.3m就丢弃这一帧继续沿用上一帧的值。为什么要设置0.3m因为正常跟随过程中目标移动速度如果按1.5m/s算在20ms一个测距周期里位移最多3cm即使加上测距噪声跳到0.3m以上基本可以判定是异常值。之后再做窗口长度为5的中值滤波专门克掉孤立的野点。最后做一阶低通滤波或者滑动平均让数据曲线平滑。这里贴一段我实际用过的C代码框架float uwb_raw; // 原始测量值 float uwb_filtered; // 滤波后输出 float uwb_previous_valid 0.0f; // 上一帧有效值 float window[5]; int window_idx 0; // 限幅 if (fabs(uwb_raw - uwb_previous_valid) 0.3f) { // 丢弃本帧直接返回上一帧 uwb_filtered uwb_previous_valid; return; } // 写入滑动窗口 window[window_idx] uwb_raw; if (window_idx 5) window_idx 0; // 取中值简单冒泡 float temp[5]; memcpy(temp, window, sizeof(temp)); for (int i 0; i 4; i) for (int j 0; j 4 - i; j) if (temp[j] temp[j1]) { float t temp[j]; temp[j] temp[j1]; temp[j1] t; } float median temp[2]; // 滑动平均 static float sum 0.0f; sum median - sum / 5.0f; uwb_filtered sum / 5.0f; uwb_previous_valid uwb_filtered;如果你想让动态响应更好可以用卡尔曼滤波。状态量可以选择距离和距离的变化率观测值就是UWB原始测距值。我一般把过程噪声Q设为0.01测量噪声R设为0.1出来的效果比纯滑动平均跟手很多尤其是在目标突然加速或者转弯的时候。3.3 差速底盘PID控制与死区设计有了目标距离r和目标角度θ剩下就是经典的底盘运动学分解。目标距离误差e_d r - r_target。r_target一般设在0.8m到1.2m之间太近容易撞到目标太远又容易跟丢。角度误差直接用e_θ θ。线速度v由距离环PID控制角速度ω由角度环PID控制。差速底盘的左右轮速为v_left v - ω * B / 2v_right v ω * B / 2其中B是底盘轮距。注意如果目标在底盘正前方偏右θ是正值那么角速度ω也会是正值上述公式中v_left小于v_right小车会右转方向是对的。调参顺序建议先调角度环后调距离环。先在原地左右摆动观察小车是否快速、不振荡地对准目标。角度环P从0.5开始一点一点往上加直到出现轻微抖动再回退20%。角度环稳定之后再给距离环一个阶跃信号观察小车是否匀速接近目标、会不会冲过头。死区很重要。我的经验是当|e_d|小于0.1m且|e_θ|小于5度时把期望线速度和角速度都置零。否则电机一直在微调不仅费电还会让底盘发出呜呜的抖动声。3.4 控制周期与任务调度UWB测距更新率我设置成50Hz20ms一次避障传感器单独用10ms周期读取主控制周期是20ms。为了避免UWB的SPI阻塞占用主循环我用了一个简单的时间片调度主循环长这样while (1) { // 1ms定时器中断里拉高标志位 if (flag_1ms) { flag_1ms 0; counter_10ms; counter_20ms; } if (counter_10ms 10) { counter_10ms 0; read_obstacle_sensors(); // 超声波 ToF } if (counter_20ms 20) { counter_20ms 0; uwb_update_position(); // 检查UWB测距数据 filter_position(); compute_control(); set_motor_speed(v_left, v_right); } // 可加系统状态灯、串口输出等耗时操作 }这样即使UWB串口打印卡顿也不会直接影响电机控制整个系统实时性会好很多。4. 避障策略如何让小车在躲障碍时不会把目标跟丢4.1 避障优先级的边界在哪里自动跟随系统里有一个天然矛盾避障要求小车偏离目标方向而跟随要求小车始终朝向目标。如果避障优先级过高小车动不动就绕个弯目标身上的UWB天线很容易因为被目标自身遮挡而丢信号。如果跟随优先级过高小车会一头撞上障碍物。我采用的状态机分四档正常跟随没有障碍直接按目标距离和角度控制。减速接近前方障碍距离小于80cm线速度输出打5折角速度照常。避障绕行/停车前方障碍距离小于安全距离40cm进入避障模式。优先根据目标方向和障碍分布决定停还是绕。重新锁定绕行结束后先在原地旋转搜索UWB测距是否连续有效确认重新锁定目标后再切换回正常跟随。这样设计的核心思路是避障只处理必须处理的障碍能不大动就不大动尽量让底盘姿态变化最小化从而降低UWB丢信号的概率。4.2 多传感器融合的决策表我把前方120度的空间分成左、中、右三块每块用对应的避障传感器探测距离。再结合目标相对底盘的方位角θ生成控制决策。这是我实际调试时使用的决策表目标方位前方距离左前距离右前距离执行动作目标居中大于安全距离--正常跟随目标居中小于安全距离大于安全距离大于安全距离减速绕行方向选障碍距离更大的一侧目标居中小于安全距离小于安全距离大于安全距离向右绕行线速度减半目标居中小于安全距离大于安全距离小于安全距离向左绕行线速度减半目标居中三面都近--停车等待直到有一个方向解除告警目标偏左前方正常--正常跟随目标在左车自然左转目标偏左左侧障碍近前方近-先停车因为绕行方向被堵继续跟随会撞目标偏右右侧障碍近-前方近同理停车只要决策表写清楚代码就是简单的switch-case不需要上什么复杂的人工智能。重点在于每次绕行之后要回头看目标角度是否还保持在上一次有效范围内如果偏差超过30度就优先停车等待信号恢复。4.3 绕行轨迹与UWB目标角度约束绕行我一般不给小车预设固定的圆弧半径而是采用虚拟目标点的方式。比如探测到正前方障碍我就临时把目标点从实际目标位置往障碍较少的那一侧平移0.5m然后让PID去追这个虚拟目标点。由于UWB继续提供真实目标的角度虚拟目标点只在避障期间生效等于在跟随轨迹上叠加了一个偏移量。这种方式的好处是底盘的运动始终是平滑的不会出现先猛拐再猛回的顿挫感。同时因为实际目标角度一直参与计算只要目标没有大幅移动绕行结束之后能自然收敛回来不需要额外加一个搜索算法。4.4 传感器布局的影响避障传感器装的位置和数量决定了决策表的实际效果。超声波传感器的波束角比较大测到的是某个锥形区域最近的障碍物装左前和右前两个配合正前方一个就够了。但超声波模块之间有串扰问题多个传感器轮询时得错开触发时间否则一个模块发出的回波会弹进另一个模块的接收端导致测出很短的假距离。VL53L0X ToF模块的视场角很窄适合精确测某个方向的距离但装的时候要注意盲区模块正下方10cm内的物体会测不到。如果小车底盘比较低最好把ToF模块朝前下方倾斜10度可以兼顾近处地面障碍和前方障碍。5. 实测数据与三个月里遇到的五个深坑5.1 不同场景下的实测表现我做了四组典型场景测试结果整理如下测试场景静态测距误差动态跟随稳态偏差丢信号频率室内走廊3m宽±6cm±12cm约1次/分钟客厅有金属桌腿±8cm±15cm约2次/分钟室外水泥地面±5cm±10cm约0.5次/分钟人员密集区域±15cm±25cm约4次/分钟动态跟随不到30cm的偏差对普通跟随场景来说已经能用了。但人员密集区域的表现提醒我UWB在人体遮挡严重的环境里还是会退化这时候滤波和决策表必须兜住。5.2 深坑一金属墙面造成的多径跳变有一次在走廊测试小车走到离金属配电箱约2米的位置测距值每隔几秒就跳一次幅度在0.4m左右。用上位机看原始数据发现是UWB接收机把墙面反射的第二径信号当成了首径。解决办法有三个一是调整天线高度让天线尽量高于或避开反射路径二是在软件里做跳变更正即突然大跳后紧接着往回跳就取前后平均三是在多个测距周期内统计距离变化率超过正常人移动速度的距离跳变直接丢弃。5.3 深坑二电机电磁干扰直接打乱UWB串口低速状态下系统一切正常一旦电机PWM占空比加大UWB模块的SPI接收缓冲区就出现大量错误帧测距更新率直接掉到原来的五分之一。排查过程很痛苦先怀疑是SPI时序后来用示波器看电源纹波发现电机启动瞬间VCC上叠加了约300mV的毛刺。后来把电机电源独立分开UWB模块的供电从主控3.3V LDO单独引并在靠近模块电源脚加一个100uF电解电容和一个0.1uF陶瓷电容问题立即消失。另外电机PWM频率从10kHz提升到20kHz噪声也小了很多。5.4 深坑三标签放在腰包导致间歇性失联测试人员把标签放在随身的腰包里走路时腰包会摆动人体正好挡在天线和底盘基站之间。UWB信号是非视距状态测距值不是变成0就是漂到好几米。后来把标签改成挂绳式挂在胸前天线朝外失联率显著下降。这个坑提醒我跟随系统的目标端也必须参与设计不能只优化机器人本体。给标签做一个简单的固定支架比在算法里加一万个滤波器都有效。5.5 深坑四双基站基线太短导致角度解算抖动前面说过基线长度影响角度精度。实际测试数据是基线10cm时目标站在2m外角度解算结果在±25度之间乱跳小车根本没法走直线基线加到30cm后同样条件下角度抖动降到±6度控制效果完全是两个档次。如果底盘尺寸限制导致基线无法加长可以考虑用一个三基站阵列但解算复杂度会上升。最好还是在结构设计阶段就给两个UWB天线留出尽量宽的安装位。5.6 深坑五目标转弯时小车反应慢半拍目标以约1.2m/s的速度向左转弯时小车总是会先直行一小段然后突然左转给人一种愣神的感觉。排查后主要是两个原因一是滑动中值滤波窗口太大把真实角度变化拉平了二是角度环的P偏小响应不够快。我做了两处调整中值窗口从5缩短到3角度环P系数提高30%同时给角速度加了一点微分抑制防止超调。改完后目标转弯时小车能在大约200ms内跟上基本不会冲出弯道。实测做下来我最大的体会是UWB自动跟随避障系统想调稳顺序应该是供电、天线、滤波、控制最后才是PID参数。很多人上来就调PID但数据源头一跳一跳的再好的控制器也压不住。如果你正准备复现这套系统先别急着写控制代码把UWB原始测距接到电脑上跑一个实时曲线观察正常行走、转身、靠近金属这几个场景的数据质量你会对算法到底要补偿什么心里有数得多。