1. 项目概述为什么选择UWB来做人体追踪在机器人圈子里让机器人“认人”并跟得上一直是个既基础又充满挑战的活儿。无论是想让它帮你搬行李、做导览还是作为智能轮椅的“隐形伴侣”核心都绕不开一个点精准、稳定地知道“人”在哪并且能平滑地跟上去。早些年大家的目光大多聚焦在计算机视觉上。摄像头便宜算法开源库也多看起来是个好路子。但真干过这行的都知道视觉方案在实验室里跑得欢一到实际场景就“娇气”得很光线一变识别率就跳水稍微有个遮挡目标就跟丢了室外强光或者昏暗环境更是直接“罢工”。更别提那些需要实时计算深度图的方案对处理器的要求可不低成本和功耗都上去了。所以当我们需要一个环境鲁棒性强、精度高、且对算力要求相对友好的追踪方案时超宽带UWB技术就进入了视野。UWB不是什么新鲜玩意儿它在短距离无线通信和精确定位领域已经深耕多年。它的原理简单说就是利用纳秒级的极窄脉冲进行通信通过测量无线电波飞行时间ToF来精确计算距离。这带来了几个核心优势厘米级定位精度、极强的抗多径干扰能力、以及较低的功耗。这意味着无论白天黑夜、室内室外只要在信号覆盖范围内UWB都能稳定地输出目标相对于基站Anchor的距离信息不受光照和常规遮挡的严重影响。我这次分享的项目就是基于UWB技术从头搭建一个能够自主、平滑追踪人体的移动机器人。它不只是一个简单的“测距-前进”的玩具而是涉及硬件架构设计、原始数据校准、抗误差定位算法、机器人运动学建模以及一套仿生式的跟踪控制策略的完整系统。下面我就把这套方案的里里外外、踩过的坑和总结的经验毫无保留地拆解给大家。2. 系统整体设计与核心思路拆解做一个追踪机器人听起来是“感知-决策-控制”的经典三步走但每一步的选型和细节都决定了最终是“丝滑跟随”还是“癫痫抽搐”。我们的核心设计思路是以UWB为唯一绝对位置感知源通过算法最大限度抑制其固有误差并设计一个符合机器人运动特性的“柔性”跟踪控制器。2.1 为什么是“UWB锚点标签”的架构常见的UWB定位系统有“基站固定标签移动”和“基站移动标签固定”两种模式。我们选择了前者即将三个UWB锚点Anchor固定在机器人身上让人佩戴一个UWB标签Tag。这么做的考量是计算负载在机器人端所有复杂的定位解算、滤波、控制算法都在机器人的主控上进行标签只是一个简单的信号发射/接收器功耗极低便于佩戴。坐标系统一定位结果直接是标签在机器人本体坐标系下的坐标x, y非常直观便于后续的跟踪控制算法直接使用。如果锚点放在环境里还需要一个额外的坐标变换到机器人坐标系增加了复杂度和误差累积。系统可移动性强机器人自成一套追踪系统不需要依赖预先部署好的环境基础设施开机就能用适用场景更广。2.2 硬件架构双核大脑与模块化设计硬件是算法的舞台。为了平衡实时计算能力和底层控制可靠性我们采用了双处理器架构。主控制器A33 SOC承担了“大脑”的角色。这是一颗四核Cortex-A7处理器性能足够运行我们的定位算法改进的双曲线解算和滤波算法移动平均滤波。它通过USB接口接收三个UWB锚点的原始距离数据完成所有上层信息处理并计算出机器人左右轮的目标速度。底层控制器STM32F103 MCU扮演了“小脑”和“神经末梢”的角色。作为一款经典的ARM Cortex-M3内核MCU它负责高可靠性的实时任务通过UART接收来自A33的左右轮速度指令。执行电机闭环控制通常是PID控制驱动H桥电路精准控制两个直流电机的转速。连接蓝牙模块用于接收用户手机APP的指令比如设定跟随距离、急停等。连接四个超声波传感器实现最基础的避障功能当跟随路径上出现突发障碍物时优先避障。传感器与供电UWB模块选择了支持双向测距TWR的商用模块精度宣称在10厘米内。三个锚点呈三角形布置在机器人顶部平台以获取更好的几何精度因子GDOP。电源管理采用开关电源模块搭配多个LDO线性稳压器为不同电压需求的芯片如5V、3.3V、1.8V提供稳定、干净的电源。这点非常重要电源噪声可能直接干扰UWB射频部分的性能。实操心得硬件选型与布局UWB模块选型不要只看宣传的精度更要关注其在动态场景、多径环境下的稳定性。有些模块静态精度高但一旦快速移动数据跳变会很厉害。最好能拿到评估板进行实测。锚点布局三个锚点尽量不要在一条直线上形成尽可能大的等腰三角形或等边三角形布局可以有效降低在某个方向上的定位误差放大效应。电源隔离电机驱动电路是最大的噪声源。务必为控制部分A33 STM32和UWB模块使用独立的电源滤波或甚至独立的电源转换器避免电机启停时导致系统复位或UWB数据异常。2.3 软件流程从原始数据到轮子转动整个系统的软件流程图清晰地描绘了数据流的闭环数据采集三个UWB锚点持续测量到标签的距离d0, d1, d2。数据校准原始距离数据存在固定的系统误差和非线性需要通过预先标定的查找表或拟合公式进行补偿。位置解算使用改进的双曲线定位算法根据d0, d1, d2和已知的锚点坐标计算出标签在机器人坐标系中的位置(x, y)。数据滤波将解算出的位置(x, y)送入移动平均滤波器平滑掉随机跳动的高频噪声得到更稳定的位置估计(x_f, y_f)。跟踪决策根据滤波后的位置计算机器人与目标的角度偏差α和距离偏差e实际距离与预设跟随距离之差。控制律计算将α和e输入到改进的虚拟弹簧模型控制器中计算出机器人应有的线速度v_l和角速度ω。速度分解根据差分驱动机器人的运动学模型将v_l和ω分解为左轮目标速度ω_l和右轮目标速度ω_r。指令下发与执行A33通过UART将ω_l, ω_r发送给STM32。STM32通过闭环控制驱动两个电机实现跟踪。这个流程以10-20Hz的频率循环运行构成了机器人实时跟踪的基础。3. 核心算法解析克服误差实现稳定追踪UWB数据“毛刺”多直接用来控制机器人会像醉汉一样摇晃。本章节深入两个核心算法用于定位的改进双曲线算法和用于控制的虚拟弹簧模型。3.1 UWB数据校准给尺子“标刻度”任何传感器出厂都有误差UWB也不例外。由于射频电路、天线特性、多径效应等模块测得的距离值d_meas和真实距离d_real之间存在系统偏差且这个偏差可能不是简单的线性关系。我们的做法是“实测标定”将标签固定在一个精密滑台或三角架上。机器人正前方从较近如0.3米到较远如2米每隔10厘米测量一个点。在每个点上同时记录UWB模块输出的距离值d_meas和用卷尺精确测量的真实距离d_real。将(d_real, d_meas)数据对绘制成散点图通常会近似一条直线。通过线性回归最小二乘法拟合出公式d_calibrated k * d_meas b。将这个校准公式k和b写入程序对每个锚点的原始数据进行实时校准。注意事项校准的环境一致性校准必须在最终使用的典型环境中进行比如室内地板环境。如果在空旷无反射的场地校准拿到充满金属桌椅的办公室使用误差模型会发生变化导致校准失效。如果条件允许可以在多个典型场景下校准取平均参数或实现简单的在线自适应。3.2 改进的双曲线定位算法在误差中找最优解经典的双曲线定位三边定位理想很美好已知三个锚点坐标A0(x0,y0), A1(x1,y1), A2(x2,y2)和到标签的距离d0, d1, d2以每个锚点为圆心对应距离为半径画圆三个圆的交点就是标签位置。但现实很骨感由于测距误差的存在三个圆往往交于一块区域而不是一个精确的点如图1所示。直接解方程会无解或得到误差极大的结果。图1测距误差导致三圆无法相交于一点我们的改进算法核心思想是“两圆相交第三圆验证”建立坐标系以机器人中心为原点前进方向为Y轴正方向建立机器人坐标系。A0和A1通常布置在机器人左右两侧X轴上A2布置在前方Y轴正半轴。忽略一个圆我们暂时“信任”d0和d1以A0、A1为圆心d0、d1为半径作圆。这两个圆必然相交于两点记为候选点T和T关于A0-A1连线对称。引入第三圆裁决计算候选点T和T到第三个锚点A2的几何距离dist_T_A2和dist_T_A2。比较裁决将这两个几何距离与UWB测量得到的d2进行比较。谁与d2的绝对值差更小谁就更可能是标签的真实位置。即选择argmin(|dist_T_A2 - d2|, |dist_T_A2 - d2|)对应的点作为最终定位结果。数学推导简化版 设A0坐标为(-L/2, 0)A1坐标为(L/2, 0)A2坐标为(0, D)其中L是A0-A1间距D是A2到原点的距离。 已知TA0 d0,TA1 d1。 根据余弦定理可以求出夹角φcosφ (d0^2 L^2 - d1^2) / (2 * d0 * L)则两个候选点T和T的坐标为T: ( -L/2 d0*cosφ, d0*sinφ )T: ( -L/2 d0*cosφ, -d0*sinφ )最后计算dist_T_A2和dist_T_A2并与d2比较即可。这个方法巧妙地利用了几何约束和“少数服从多数”的思路在误差存在的情况下给出了一个最优估计极大地提升了定位的鲁棒性。3.3 移动平均滤波让路径“丝滑”起来即使经过校准和改进算法解算出的位置点(x, y)依然会存在高频随机抖动。如果直接把这样的坐标送给控制器机器人就会产生高频震颤不仅耗电机械磨损也大。我们采用了最经典但有效的滑动窗口平均滤波。维护一个长度为N例如N5的位置队列。每次得到新的位置P_new时将其加入队列尾部并丢弃队首最旧的数据。机器人的控制使用这个队列中所有位置点的平均值P_avg。P_avg(k) (P(k) P(k-1) ... P(k-N1)) / N滤波效果与权衡窗口大小NN越大平滑效果越好但跟踪的滞后性延迟也越大。对于慢速行走的人N可以取大一些如5-10如果希望机器人更敏捷N就要小如3-5。需要根据实际场景调试。滞后补偿对于匀速运动的目标这种滞后可以通过预测算法如卡尔曼滤波部分补偿。但在我们这个初步系统中移动平均滤波在简单性和效果上取得了很好的平衡。图2和图3直观展示了滤波前后的路径对比滤波后的路径明显更干净、更符合人的实际运动轨迹。图2滤波前定位点抖动严重路径杂乱图3滤波后路径变得平滑连续4. 机器人运动与控制如何让轮子听懂算法的“话”知道了人在哪下一步就是让机器人动起来。这里涉及两个关键模型机器人本身的运动学模型和用于跟踪的控制器模型。4.1 差分驱动机器人运动学模型我们的机器人是典型的“两轮差分驱动”结构Hilare型两个主动轮独立控制前面或后面有一个万向轮起支撑作用。它的运动学模型是理解和控制的基础。核心参数(x, y)机器人中心点在全局坐标系下的坐标。θ机器人朝向角与全局X轴的夹角。v机器人中心点的线速度。ω机器人中心点的角速度转弯速率。r驱动轮半径。L两驱动轮之间的轮距。ω_l,ω_r左轮和右轮的旋转角速度。运动学关系简化忽略动力学 这是最常用的控制级模型它建立了轮速与机器人整体运动的关系v (r * (ω_r ω_l)) / 2ω (r * (ω_r - ω_l)) / L逆运动学控制所需 给定我们期望机器人达到的线速度v_d和角速度ω_d可以反推出每个轮子需要的转速ω_r (v_d / r) (L * ω_d) / (2 * r)ω_l (v_d / r) - (L * ω_d) / (2 * r)这个公式就是前面软件流程中第7步“速度分解”的数学依据。控制器输出(v_d, ω_d)通过这个公式就能得到左右轮的目标转速。4.2 改进的虚拟弹簧跟踪控制器如何根据位置偏差(e, α)产生合理的(v_d, ω_d)我们借鉴了物理中“弹簧-阻尼”系统的思想设计了一个虚拟弹簧模型。模型构想 想象在机器人和目标人之间连接着一根虚拟的弹簧。这根弹簧有两个作用长度方向当实际距离d_act大于预设的期望跟随距离d_ref时e d_act - d_ref 0弹簧被拉长产生一个拉力驱使机器人向前加速以缩短距离。反之如果太近了e 0则产生一个推力让机器人减速或后退。角度方向如果人不在机器人正前方α ≠ 0这根“弹簧”会产生一个扭转力矩驱使机器人转向将人“拉回”到自己的正前方视野内。控制律设计 我们为距离偏差e和角度偏差α分别设计了PID控制器在实际调试中可能只用PI或PD就够了v_d Kp_e * e Ki_e * ∫e dt Kd_e * de/dtω_d Kp_α * α Ki_α * ∫α dt Kd_α * dα/dt其中Kp,Ki,Kd分别是比例、积分、微分系数。积分项用于消除稳态误差微分项用于抑制超调、增加系统阻尼。针对差分驱动机器人的特殊处理——Brockett约束 纯反馈的连续控制律对于像我们这样的非完整约束系统不能横向移动的机器人在某些殊位形下会失效。例如当目标人就在机器人正后方α ≈ 180°时虚拟弹簧的拉力方向与机器人能运动的方向垂直机器人会“卡住”无法仅通过平滑控制转向目标。我们的解决方案——引入“转向模式” 设定一个度阈值例如45°。当目标偏离机器人正前方的角度|α| 45°时我们认为直接使用虚拟弹簧控制律效率低下或可能失效。此时我们让机器人停止前进v_d 0并原地旋转ω_d K * α直到|α|小于阈值再切换回正常的跟踪模式。这个策略简单粗暴但非常有效保证了机器人永远不会丢失侧后方或正后方的目标。整个控制算法的结构如图4所示形成了一个完整的反馈闭环。图4从位置信息到轮速指令的跟踪控制算法框图5. 系统集成、调试与实测效果算法和模型最终都要在真实的机器人上跑起来。这一部分分享从软硬件联调到实际测试的全过程以及遇到的典型问题和解决方案。5.1 系统集成与联调步骤分模块验证UWB模块单独测试每个锚点与标签的测距功能确认通信正常数据刷新率达标通常10-20Hz。电机驱动编写STM32测试程序分别控制左右轮正反转确认PWM输出、电机驱动板、编码器反馈如果有工作正常。串口通信确保A33与STM32之间的UART通信协议稳定能正确收发速度指令和数据反馈。算法仿真在电脑上用Python或MATLAB等工具模拟UWB数据加入噪声、实现定位算法、滤波器和控制器。用虚拟的目标轨迹测试整个控制逻辑是否合理初步整定PID参数。这一步能排除大量逻辑错误。开环测试将机器人架起轮子悬空。运行完整程序但将控制器计算出的轮速指令直接发送给电机。通过观察轮子转动和打印的日志确认定位算法输出的(x, y)坐标是否随标签移动而合理变化。控制器输出的(v_d, ω_d)是否符合预期人偏右ω_d是否为负值以右转。速度分解公式是否正确给定v_d, ω_d左右轮转速差是否明显。闭环静态测试将机器人放在地上让人持标签静止站在前方。启动跟踪程序预设一个较近的d_ref。观察机器人是否能自主运动到指定距离并停下。这是调试PID参数最关键的一步。重点调整距离环的Kp_e和Kd_e防止机器人冲过头或收敛过慢。闭环动态测试让人缓慢行走进行“走-停-转弯”等简单测试。重点调试角度环的Kp_α和转向模式的阈值、旋转速度增益K。目标是让机器人转向平滑不过冲能稳定跟上。5.2 实测效果与数据分析我们设计了三种典型场景进行测试并记录了机器人和人的轨迹坐标已滤波处理场景一静态接近目标行为人持标签静止不动。机器人行为从较远位置启动朝向目标加速接近过程中减速最终在预设距离d_ref处平稳停下。数据分析绘制“机器人与目标距离-时间”曲线应是一条从大变小最后稳定在d_ref附近的平滑曲线超调量小。这验证了距离控制环的有效性。场景二直线跟随目标行为人沿直线匀速前行。机器人行为启动后加速跟上随后保持与目标大致相同的速度维持d_ref距离。当人停下时机器人也减速停下。数据分析绘制两者在Y轴前进方向的位置-时间曲线两条曲线应几乎平行。距离曲线应围绕d_ref有小幅波动。这验证了系统对匀速目标的跟踪能力。场景三方形路径跟随目标行为人走一个近似的正方形路径。机器人行为在直道段能跟上在拐角处由于人突然转向导致角度偏差α迅速增大机器人会触发“转向模式”原地旋转对准目标后再加速跟上。轨迹会呈现明显的“切角”现象这是非完整约束和控制器策略决定的正常行为。数据分析绘制XY平面轨迹图。人的轨迹是一个方形机器人的轨迹是一个圆角方形且在拐角处会有明显的转向停顿点。这验证了系统在目标机动时的应对策略。5.3 常见问题与排查技巧实录在实际调试中我们遇到了不少坑这里总结成排查表希望能帮你节省时间问题现象可能原因排查步骤与解决方案定位坐标跳动剧烈1. UWB数据未校准。2. 锚点天线被遮挡或靠近金属。3. 电源噪声干扰。4. 滤波窗口太小或未启用。1. 执行严格的静态多点校准。2. 检查锚点安装位置确保天线朝向天空远离机器人金属本体和大面积金属物体。3. 用示波器检查UWB模块供电电压纹波加强滤波。4. 增大移动平均滤波的窗口大小N。机器人跟踪时严重震荡前后抖动1. 距离环PID参数不佳Kp_e太大或Kd_e太小。2. 定位数据更新频率远高于控制频率导致控制指令突变。3. 电机响应延迟大。1. 重新整定PID先调Kp_e使它能动再调Kd_e抑制震荡最后加很小的Ki_e消除静差。2. 让控制循环等待定位数据或对定位数据进行更低频率的采样。3. 检查电机驱动电流是否足够编码器反馈是否灵敏优化STM32的电机PID控制环。机器人转向迟钝或过冲1. 角度环Kp_α太小/太大。2. “转向模式”的阈值设置不合理。3. 机器人本体转动惯量大但角速度控制环未调好。1. 静态测试让人站在侧方观察机器人转向速度和超调调整Kp_α和Kd_α。2. 将转向阈值从45°调整为30°或60°观察对跟踪流畅度的影响。3. 在STM32层面对轮速控制增加加速度限制使转向更平滑。丢失侧后方目标“转向模式”的旋转速度太慢或目标移动太快。1. 提高转向模式下的角速度增益K。2. 考虑在转向模式中引入预测让人在机器人转向时其位置预测值也参与计算使机器人转向更“超前”。跟随距离始终有偏差距离环缺少积分项Ki_e或积分项被饱和限幅。1. 引入一个很小的Ki_e。2. 检查并合理设置积分限幅防止积分饱和导致控制失灵。系统偶尔死机或复位1. 电机启动瞬间电流过大导致电源电压跌落。2. 程序跑飞或堆栈溢出。1. 在电机电源入口处加大电容或采用软启动电路。2. 增加看门狗检查关键任务的执行时间优化代码效率。6. 总结与未来可拓展的方向走完整个项目从画电路板、焊元件到写驱动、调算法最后看着机器人稳稳地跟在身后这种成就感是纯粹的。基于UWB的追踪方案其最大的优势就是稳定。在走廊、办公室、甚至半户外的树荫下它都不会因为光线变化而“失明”这种可靠性是视觉方案短期内难以企及的。当然这个系统还有很大的优化空间。如果让我继续做下去我会从这几个方向深入传感器融合UWB怕什么怕金属环境下的严重多径干扰怕非视距NLOS场景。可以融合惯性测量单元IMU。当UWB信号质量差时用IMU进行短时间的位置和姿态推算航迹推算等UWB信号恢复后再进行融合定位。卡尔曼滤波或互补滤波是实现这种融合的经典工具。更智能的控制器现在的虚拟弹簧模型PID在平稳场景下不错但应对突然的加速、急转弯还是有点吃力。可以探索模型预测控制MPC它能在考虑机器人运动约束最大速度、加速度的前提下预测未来几步的状态给出更优的控制序列让跟踪更顺滑、更安全。动态目标预测目前机器人是“看到哪走到哪”永远比人的动作慢一拍。可以建立简单的人体运动模型如恒定速度模型根据历史位置预测人下一时刻可能的位置让机器人提前向预测点移动实现更“贴心”的跟随。引入轻量级视觉辅助UWB提供精准的“在哪”但不知道“是谁”。可以加一个低功耗的摄像头用轻量化的深度学习模型如MobileNet SSD做简单的人体检测和身份确认。这样可以在多人场景下锁定特定目标避免跟错人。视觉还可以提供丰富的场景信息辅助避障。这个项目就像搭了一个坚实的地基。UWB提供了可靠的空间感知能力而如何在此基础上建造更智能、更拟人化的交互行为还有无数的可能性等待探索。希望这篇详尽的分享能为你启动自己的机器人追踪项目提供一份可靠的蓝图和避坑指南。
