基于UWB 的实验室跟随机器人系统设计

钟世杰，李隆，李响，宋强

（成都工业学院，四川成都，610031）

0 引言

随着科技的快速发展，人们的生活水平逐渐提升，如何解放生产力，为人类带来更多便利，已成为未来发展的主题。尤其是进入21 世纪后，科技发展更加迅速，人工智能、无人驾驶、量子计算、5G 通信等技术都是具有代表性的创新技术。在“以国内大循环为主，国内国际双循环相互促进的新发展格局”的双循环指导下，未来，我国科技发展将更加迅速。

近代以来，我国机器人行业发展迅速，但是起步较晚，在机器人环境地图构建、路径规划以及障碍物闪避等技术方面还存在不少空白，适合用于工业现场的机器人更是十分稀少，智能型机器人代替人工已是大势所趋。如走进万千家庭的智能扫地机器人、工业中应用广泛的搬运车和国内某电商平台的无人快递车等。智能型机器人用途广泛，可应用于各个行业领域，如：物流仓储，医疗护理，工业制造等，还可以加快进行工业自动化，数字化的发展，提高我国国际影响力和竞争力。

当下，针对实验室、仓库等室内应用场景，跟随机器人的市场缺口巨大，是一片等待开发的蓝海，本文利用了激光雷达和UWB 技术，设计了一种小型化、可跟随、可避障、低延迟的跟随机器人，可以在实验室、仓库等室内环境下，实现地图构建、自主定位、跟随人体前行和同时躲避前方障碍物等功能。

1 系统总体设计方案

基于UWB 的实验室跟随机器人系统整体可分为硬件设计和软件设计两大部分。主要实现的功能有：地图建模、自动避障和人体跟随。机器人系统由树莓派4B 主控模块、UWB 模块、电机驱动模块、驱动控制模块、激光雷达模块和串口显示屏组成，系统总体框图如图1 所示。其中树莓派4B 负责逻辑主控，地图构建，网络连接，同时与外围传感器进行通信。STM32 单片机作为电机的驱动控制，通过控制电机驱动模块来对电机进行控制。UWB 模块通过发射超宽带窄波与基站进行定位，确定当前的位姿。激光雷达模块采用ToF 算法，识别道路前方的障碍物，将障碍物与自身的距离返回给树莓派4B 进行地图构建，从而实现避障和跟随功能。

图1 系统总体框图

2 系统硬件设计

■2.1 主控模块

树莓派4B 是由“Raspberry Pi 基金会”于2019 年上市的第4 代卡片式电脑。它是一款基于ARM 架构的微型电脑主板，以SD/MicroSD 卡为内存硬盘，拥有一个64 位的四核处理器，主频1.5GHz，内存容量从1GB 到8GB 不等，支持双频Wi-Fi、蓝牙5.0、Gigabit 以太网、2 个USB 3.0和2 个USB 2.0 接口、2 个HDMI 接口、MicroSD 卡槽等。同时具有运算速度快、接口丰富、体积小等优点，通过它连接各种传感器、模块和外设，可以很好地满足本系统设计的需求。对于本跟随机器人系统而言，需要时刻采集周围环境变量，对当前地图进行建模，分析定位，并驱动电机前进。由于整个系统工作量较大，如使用普通的单片机模块进行运算，整个系统超算力运行，将会导致行动迟缓等问题，为了获得更好的算力和功能扩展能力，选择性能强大的树莓派4B 作为主控模块是很好的选择。

■2.2 UWB 模块

UWB(超宽带无线载波通信)技术是一种具有高精度定位、高时间分辨率、高能效、高稳定性特点的技术。其定位原理为：UWB 定位利用发射方向控制的超短脉冲信号来测量目标与基站之间的时延，从而计算出目标的位置。

在本系统设计当中，关键一步是利用UWB 模块获取当前的位置和姿态[1]，即分别获取标签与3 个基站之间的x,y,z坐标。其中UWB 模块既可以做基站，又可以做标签。一般情况下，3 个UWB 基站和1 个UWB 标签就可以组合实现三维空间的定位[2]，但是却无法避免z 轴上有两个取值的问题。因此在设计算法时，需要将两个z 轴的值进行分析，舍去错误值。

具体地说，UWB 定位需要至少三个基站分别与目标进行通信，每个基站发送一个包含时间戳的超短脉冲信号。目标接收到这些信号后，计算出每个信号的到达时间，并将其与基站发送信号的时间戳相减，得到时延数据。由于时延与距离成正比，因此可以通过时延数据计算出目标与每个基站之间的距离。一旦确定了目标与三个或以上的基站之间的距离，就可以使用三角定位法计算出目标的位置。UWB 技术的应用场景大致可以分为三个方面[3]，分别是通信、雷达和定位，常应用于智能家庭、无线网络、战术组网电台、探地雷达、车辆避撞雷达以及军用民用等需要精确定位的系统中。在本系统设计中，经过资料查阅和实验比对，选择了可以带来更好定位效果的UWB 技术[4]，其特点如表1 所示。

表1 常见的室内定位技术特点

图2 UWB 信标

■2.3 驱动模块

2.3.1 驱动模块控制系统简介

驱动模块控制系统使用STM32F407VET6 单片机来进行控制，驱动部分则使用FOC（Field Oriented Control，磁场定向控制)三相电机控制器，由此驱动2 个200W 的无刷直流轮毂电机。FOC 三相电机控制技术是一种用于控制交流电机的技术，现主要用于驱动无刷直流电机，其特点包括：可以将电机磁场方向和电流方向分离控制，从而实现对电机的高精度控制；可以控制电机的功率因数，最大限度地提高电机效率，减少系统能耗，使电机在运行时产生更低的噪音；可以通过闭环控制降低电机的温升，延长电机寿命，提高电机系统的可靠性；可以实时监测电机状态，及时发现问题并进行调整，避免电机损坏和故障发生。无刷直流电机是一种新型的电动机，其可分为具有直流电机特性的无刷直流电机和具有交流电机特性的无刷直流电机。相较于传统的有刷直流电动机，它具有高效率、高功率密度、高可靠性和低噪声等特点。

在课时量限制的情况下采用传统的教师讲、学生听的方式，以理论讲授为主，很难调动学生的学习积极性。教学与科研分离现象较为突出，教学过程中未能有效地结合基础前沿理论与具有时代性的现实经济问题。目前案例教学仅用简单的经济学例子解释计量经济学模型，并非真正意义上的案例教学。多数计量经济学教材上每章提供的案例综合性较差、陈旧，未能根据不同专业特点进行设置，且对如何选题、如何建立和分析模型以解决实际经济问题等讨论少之甚少，导致学生在综合实证分析中，从模型设定、查阅资料、收集整理数据、模型估计和诊断等各个建模环节，仍存在大量的技术问题[2]。

2.3.2 电机驱动模块

电机驱动采用SimpleFOCShield v2.0.4。SimpleFOCShield v2.0.4 是一款带电流环的FOC 无刷电机驱动器，主要用于低功耗的FOC 应用，驱动无刷直流电动机。该驱动器功能丰富，支持开/闭环无刷速度/位置控制以及最大功率120W 输出，最大电流5A 输出，工作电压12～24V。此外，SimpleFOC v2.0.4 还可通过多种硬件平台进行控制，如Arduino、STM32和ESP32 等，并且可以与多种编码器进行通信，如ABI、I2C和SPI 等。

本文选择在STM32 平台上控制SimpleFOCShield 驱动器，方法是：先将电机的三根相线（U、V 和W)连接到SimpleFOCShield 上的相应引脚，然后将相位编码器的各个引脚（A、B 和I)连接到STM32 的外部中断引脚，最后将两个驱动板的TB_PWR 接口并联到12V 电源上，需要注意的是，SMT32、SimpleFOCShield 驱动器、无刷直流电机，三者需要共地连接。

■2.4 传感器模块

2.4.1 光电编码器

光电编码器是一种通过光电转换将输出轴上的机械几何位移量转换成脉冲或数字量的传感器。具体地说，光电编码器的信号输出可以是集电极开路NPN 输出、集电极开路PNP 输出、方波、正弦波和推挽式等多种形式。在本设计中，采用1000 线的高分辨率光电编码器来输出精准的里程计信息，可为后续的精准定位提供必要条件。其中1000 线是指编码器轴承在1倍频的条件下每旋转360°能提供1000个脉冲，线数越高，代表编码器的分辨率越高。高精度编码器的线数范围没有一个具体的界定，一般来说，1000 线及以上的编码器都可以被认为是高精度编码器，因为它们通常提供了较高的测量精度和较低的误差率。

图3 光电编码器原理示意图

2.4.2 ToF 激光雷达

机器人需要避开环境中的障碍物前进，就需要时刻获取与距离障碍物的位置距离。然而在室内环境下，障碍物多且分布杂乱，传统的测距方式难以实现。例如超声波测距，它是一种常用的非接触式测距方法，利用了超声波的回声来测量与其发射源之间的距离。然而超声波易受到环境的影响，例如温度、湿度、气压等因素都会影响超声波的传播速度和回声反射情况，从而影响测量精度。例如在实验室场景下，其测距的误差通常在数厘米至数十厘米之间，精度较低。

图4 激光雷达测距电路

■2.5 串口LCD 显示屏

显示屏是一种将一定的电子文件通过特定的传输设备显示到屏幕上的输出工具。在本设计中，串口显示屏用于显示当前跟随机器人的系统状态，例如当前电量、当前地图坐标和当前位姿等。显示屏采用串口通信协议连接LCD 液晶显示屏。相较于OLED 显示屏，LCD 显示屏具有寿命长、成本低等优点，如表所示。LCD 显示屏虽然寿命较长，但是在长时间使用后依旧会损坏，所以在安装显示屏时，应预留一定的拆卸空间，保障后续的更换操作能够顺利进行。

表2 显示屏特点

3 系统软件设计

本系统软件设计主要由Linux 环境下的VS code 软件进行程序编写。Linux 可以在各种硬件平台上运行，从小型嵌入式设备到大型服务器集群都可以使用。这种跨平台性使得Linux 成为很多领域的首选操作系统。除此以外，Linux 还提供了丰富的开发工具，如gcc 编译器、gdb 调试器、make 构建工具等，可以大大地缩短开发周期。基于以上原因，本系统软件设计选择在Linux 环境下进行。

本系统软件设计包括：机器人的初始化程序，激光测距程序，UWB 定位程序，避障跟随程序，电机驱动程序和显示模块程序。实验室跟随机器人的软件层核心任务是实现避障跟随。整个系统上电后，主程序首先对系统时钟以及串口等通信接口进行初始化，树莓派计算机通过读取激光雷达以及UWB 的定位信息，确定当前坐标和位姿以及障碍物的方向和距离，再通过STM32 驱动系统来驱动电机，实现机器人的路径规划、地图构建、自主定位、跟随人体前行的同时躲避前方障碍物等功能。

图5 主程序流程图

■3.1 UWB 定位算法

UWB 标签的常用定位方法有：单边双向测距法(SSTWR)，双边双向测距法（DS-TWR)，双向飞行时间法(TOF)，到达时间差法(TDOA)和到达时间算法(TOA)。本系统设计考虑到实现难度与实现效果，最终选择了到达时间差法定位。要使用到达时间差法，至少需要3 个UWB 基站与1 个UWB 标签。3 个UWB 基站和1 个UWB 标签就可以组合实现三维空间的定位，但是却无法避免z 轴上有两个取值的问题。在当前设计的应用场景中，载具车辆高度较低，约在10～20cm 高，而UWB 标签一般是戴在人的手上，距地面约80～100cm，所以在应用场景中可以始终选择正z 作为真实z 值[6]。

■3.2 地图构建子程序

机器人需要在室内进行移动，就需要进行地图构建。SLAM 算法(Simultaneous Localization and Mapping，同时定位与地图构建)是指在未知环境中，利用移动机器人的传感器信息，同时进行自我定位和环境地图构建的一类算法。该算法的主要思想是将机器人在未知环境中移动的过程中所获得的传感器数据和运动控制信息进行融合，同时实现自我定位和环境地图构建的过程。简单来说，就是机器人在未知环境中不断移动，通过传感器获取周围环境的信息，如激光雷达，同时根据机器人的运动状态更新自己在环境中的位置，并把所有的数据融合起来生成机器人所在的地图。其软件设计流程图如图6 所示。

图6 地图构建子程序流程图

■3.3 避障跟随子程序

在获得了自身定位与环境之间的关系后，工具车读取UWB 信息进行解码，确定自身的位置和姿态，方便调整角度来驱动电机越过障碍物或者跟随人体前行。其程序流程图如图7 所示。

图7 避障子程序流程图

4 结语

本系统以UWB 技术为基础，以树莓派4B 为主控，采用FOC 三相电机控制器，搭配霍尔传感器和1000 线光电编码器，实现了机器人的自身定位、环境地图构建、路径规划以及障碍物闪避等功能。

经测试，跟随机器人运行平稳，能够实现避障跟随等功能，同时在实验室、教室等复杂场景工作下，也有良好的表现。系统整机测试如图8 所示。

图8 UWB 基站与跟随机器人实物测试图

在本系统中，UWB 技术用于实现机器人的位置和姿态的确定，树莓派4B 作为主控负责控制机器人的运动，FOC三相电机控制器用于控制电机的转速和转向，霍尔传感器和1000 线光电编码器用于获取机器人的运动状态。通过这些技术的组合，本系统能够实现高精度的定位和运动控制，同时能够构建环境地图、规划路径并避开障碍物，具有较高的实用性和可靠性。通过实际测试，机器人在实验室环境下运行良好，能够自主定位与避障，提高工作效率，减少人力成本，保障人员安全等，是未来智能化，自动化的发展趋势，具有一定的实际应用价值和可观的市场前景。