基于UWB的自主跟随机器人系统设计

杨再标，易先军，彭 萌，耿翰夫，付 龙

（武汉工程大学 电气信息学院，武汉 430205）

随着人工智能技术和网络化通信技术的发展，人们开始解放双手，更加注重设备的智能化和信息化，自主跟随机器人也因此受到了社会各界的广泛关注。 目前，通过摄像头图像识别的方法运用在跟随目标对象移动范围较大的场合时，很容易丢失跟随目标对象，特别是在多人的场合中，很难实现单一目标的跟踪，从而导致跟随失败。

UWB 技术是近年来兴起的一种高速传输数据的无线通信技术，具有传输数据效率高的优点，可高达1000 Mb/s 以上。 UWB 具有抗干扰性强、传输效率高、能耗小等优势。 在边长检测中，能得到较为精确的结果，很好地运用于跟随机器人中[1-2]。 本文基于2011年提出的IEEE802.15.4-2011 标准中的IR-UWB 技术开发高精度射频测距系统， 采用Decawave 公司的DWM1000 芯片设计射频测距系统通信节点，应用SDS-TWR 算法实现测距，使自主跟随机器人系统可以更加安全有效地跟随目标对象[3]。本文旨在研究并设计一种基于UWB 的自主跟随机器人系统，在利用角度大小跟随中，通过减少跟随边长的误差，进而减小跟随角度误差，以提高自主跟随机器人的跟随精度。

1 跟随机器人原理

1.1 跟随边长检测原理

自主跟随机器人系统的跟随边长检测算法采用双边双向测距（SDS-TWR）[4-7]。 具体以基站0 和标签之间的通信为例。 基站0 向标签发送Poll 数据包，同时记录下发送Poll 数据包的时刻T1，并在一段时间后打开基站0 的串口RX 通道； 标签提前开启标签的串口RX 通道， 收到Poll 数据包后并记录下接收到Poll 数据包的时刻T2；标签等待先前设定好的延时处理时间Treply1后， 通过已经开启的串口TX 通道向基站0 发送Response 数据包， 同时记录下发送Response 数据包的时刻T3，并在发送完后打开设备B 的串口RX 通道； 基站0 接收到Response数据包后并记录下接收到Response 数据包的时刻T4； 基站0 等待先前设定好的延迟处理时间Treply2后，通过已经开启的串口TX 通道向标签发送Final数据包，同时记录下发送Final 数据包的时刻T5；标签接收到Final 数据包后并记录下接收到Final 数据包的时刻T6。 测距过程如图1 所示。

图1 SDS-TWR 算法示意图Fig.1 Schematic diagram of SDS-TWR algorithm

则电磁波飞行时间为

式中：Tround1=T4-T1；Tround2=T6-T3；Treply1=T3-T2；Treply2=T5-T4。

通过式（1）求得电磁波飞行时间Tprop，再乘以光速，可得到基站0 到标签之间的距离。

1.2 跟随机器人跟随原理

首先将基站0 和基站1 分别固定于机器人同一水平线上，并且基站0 和基站1 相距30 cm，人手持标签于两个基站的前方，具体如图2 所示。 基站0、基站1 和标签三者构成一个三角形。

图2 跟随原理Fig.2 Following principle

设三角形三边边长分别为a、b、c，b 边所对应的角为θ。 a=30 cm，由SDS-TWR 算法可测得b、c 的距离。 于是，由余弦定理有：

利用反三角函数可求得θ 的值。 计算得到的θ角度与控制器设定的角度相比较，进而控制机器人的运动。

2 跟随机器人系统总体设计

2.1 跟随机器人系统总体结构

跟随机器人系统总体结构主要由控制器（STM32F103）、声光报警器、直流减速电机驱动器、直流减速电机、 光电编码器、12 V 锂电池、OLED 显示屏、基站（基站0、基站1）、标签以及红外线超声波传感器共10 个模块电路组成。 跟随机器人系统总体结构如图3 所示。 跟随机器人的跟随目标对象为具有接收信号和发送信号功能的标签。 当人手持标签运行时，跟随机器人就会跟随标签运动。

2.2 跟随机器人硬件设计

图3 跟随机器人系统总体结构Fig.3 Overall structure of the follow robot system

主控制器选用STM32F103ZET6 芯片作为最小系统，主控制器主要是控制跟随机器人的运动和从控制器进行信息交互。12 V 锂电池作为整个跟随机器人的供电系统，提供了系统的动力来源。OLED 显示屏模块将电源电量和跟随机器人的运动状态呈现在屏幕上，当电源电量低于5 V 时，电量字体变为红色并且不停闪烁， 以提示用户及时给电源充电。声光报警器模块采用蜂鸣器和LED 灯制作而成，当电源电量不足或者跟随机器人系统出现故障时，蜂鸣器将发出警报声音，同时LED 灯不停闪烁，警告用户电源电量不足或需要重启系统。 直流减速电机驱动器、直流减速电机和光电编码器三者构成一个闭环回路， 通过增量PID 算法对电机进行速度控制。 红外线传感器和超声波传感器两者对外部环境信息进行采集，使跟随机器人能够检测并避开障碍物。

标签、基站0 和基站1 的硬件结构相同，都由控制器（STM32F103）、DWM1000 模块、OLED 显示屏和5 V 电源四部分组成。 DWM1000 模块是DecaWave公司生产的低功耗芯片， 该模块传输数据效率高，可高达6.8 Mb/s，可以实现双边双向测距，在跟随机器人中有很好的运用。

3 跟随机器人软件程序

跟随机器人软件设计包括运动控制程序和基站跟随边长检测程序。

3.1 跟随机器人运动控制

跟随机器人运动程序流程如图4 所示。 跟随机器人供电后， 首先进行的是全部硬件模块的初始化，包括通信串口初始化、定时器初始化以及OELD显示屏的初始化等。 接着进入判断程序，当接收到融合信息“R”时，机器人右转；当接收到融合信息“L”时，机器人左转；当机器人接收到融合信息“F”时，机器人前进；当机器人接收到融合信息“S”时，机器人停止前进。

图4 跟随机器人运动控制流程Fig.4 Motion control flow chart of the follow robot

3.2 基站跟随边长检测程序

基站跟随边长检测程序流程如图5 所示。 基站开机初始化后，发送Poll 数据包给标签，标签有响应后，进行跟随边长检测，执行双边双向检测步骤。当基站检测完成并没有响应时， 基站进入休眠状态，当收到指令时再次进行检测跟随长度。

图5 基站跟随边长检测程序流程Fig.5 Base station following side length detection procedure flow chart

4 系统功能测试

4.1 跟随边长检测

为了测试本设计中跟随机器人的测距精度，设计了如下实验，实验俯视示意图如图6 所示。 将基站0 和基站1 固定于一水平、相距30 cm 的直线上，并且基站0 用USB 转串口线与电脑相连；标签置于基站0 和基站1 的平行且相距100 cm 的直线上，基站1、基站0 和标签形成一个角度，通过改变该角度的大小来获得标签的测试点，在测试点上测取基站0 到标签的距离信息并将距离信息通过串口打印在电脑串口助手中。 本次实验测量的角度分别取了30°、45°、60°、90°、120°、135°、150°共7 个角度，所对应的的测试点分别为点A、B、C、D、E、F、G。 在测试点检测的边长距离结果如表1 所示。 实验数据表明，边长检测误差在±10 cm 之内，进而减少了跟随角度的误差，提高了跟随精度。

图6 实验设计Fig.6 Experimental design

表1 边长检测结果Tab.1 Side length test result

4.2 跟随测试

将跟随机器人硬件平台供电后，将标签0 通过USB 转串口连接线连接到电脑上，通过电脑串口调试助手和跟随机器人的实际运动来检测标签与基站0、 基站1 的角度和距离的融合信息是否已经传输到跟随机器人的主控制器上。 串口打印信息和跟随机器人运动信状态息如表2 所示。

表2 机器人跟随状态Tab.2 Robot following state

5 结语

上述所设计的硬件电路和相应的软件控制流程，实现了机器人的自主跟随。 在测取实验数据中，验证了基于UWB 的自主跟随机器人系统能够实现在人员移动密集的环境下对目标对象实时、准确的跟随。 通过减少跟随边长误差，进而减小跟随角度误差，以提高自主跟随机器人的跟随精度，完成跟随目标对象的任务。 但在边长检测精度上和跟随算法上还不够精确，还可以进一步提高。