基于UWB双向飞行算法完成精准跟随实验的研究.pdf

wwwele169com|41实验研究基于 UWB 双向飞行算法完成精准跟随实验的研究刘卓娅1，周俊华1，胡树杰1，林凡舒1，张驰1，董映萍1，龙永运2（1.珠海科技学院，广东珠海，519041；2.珠海城市职业技术学院，广东珠海，519090）摘要：为了实现智能跟随搬运机器人在跟随过程中的跟随精准度要求，提出了一种基于UWB双向飞行算法的智能跟随系统，该跟随系统由stm32f103zet6主控板、蓝牙模块、驱动模块、UWB模块以及超声波模块构成，通过建立智能跟随搬运机器人上的三个UWB通信模块与人手环上的UWB模块之间的数学模型，然后运用UWB双向飞行算法得出基站与手环标签的直线距离，由此实现了智能跟随搬运机器人的定位跟随，并且通过US-015超声波模块作为辅助，有效实现了智能跟随搬运机器人在复杂环境中的精准跟随。最终通过标签与基站间水平高度差的精准度实验和手环与基站间偏转角度的精准度实验表明，基于UWB双向飞行算法能够使智能跟随搬运机器人在跟随过程中实现有效的精准跟随。关键词：UWB；双向飞行算法；智能跟随；超声波避障0 引言 随着智能时代的发展，各种智能机器人产品亦蜂拥而出，诸如自动跟随行李箱、智能跟随陪护机器人、智能跟随快递车、智能跟随搬运机器人等，它们均体现出了智能跟随技术在机器人领域中有着广泛的应用。智能跟随搬运机器人能有效解决人工劳动力，为人们的日常生活提供便利。本文研究设计了基于 UWB 技术的智能跟随系统。该系统采用基于 UWB 的双向飞行算法，能有效提高智能跟随精准度，同时考虑到现实应用场景中，跟随机器人执行定位跟随功能时会遇到各种障碍物，因此本系统设有基于超声波传感器的避障功能模块。1 总体设计 1.1 系统流程设计本系统总体分为两部分，第一部分为是智能手环，第二部分是小跟搬主体。其中智能手环的主要功能为接收基站发送的信号并返回信号给基站，而小跟搬的主要功能分为两个，一个是向智能手环发收信号并进行计算处理，实现跟随的功能，另一个是避障功能，当小跟搬在跟随的过程中判断到出现障碍物的情况，停止跟随进行避障操作，当避障完成后重新定位继续跟随。系统总体流程如图 1 所示，当系统初始化后，智能手环通过蓝牙模块与小跟搬进行同步后，首先小跟搬的基站发出信号给手环上的标签，通过预设跟随距离计算出跟随手环标签预基站之间的水平差，然后进行跟随模式。当小跟搬上的超声波模块检测到障碍物后，断开跟随模式进入避障模式，成功避障后，计算与标签的偏移角后进行修正，之后继续进入跟随模式。1.2 硬件结构为实现小跟搬达到精准跟随的目的，该方案采用了一个主控元件，四个超宽带模块，一个电机驱动模块，四个超声波模块和一个蓝牙模块。硬件总框图设计如图 2 所示。1.2.1 主控元件该系统采用的 MCU 单片机为 STM32F103ZET6。相较于STC 单片机有着更高的实用性，STM32 具有最高 72MHz 工作频率，串行单线调试（SWD)和 JTAG 接口可用于仿真，可以进行复杂的计算和精准的控制，2 个看门狗定时器防止程序跑飞，单片机自动复位。甚至可以用 IIC 通讯，而 STC 系列目前只能进行串口通讯，其他通讯方式不能精准稳定进行。1.2.2 UWB 超宽带小跟搬采用的 UWB 超声波模块的主控芯片的型号为图 1系统流程图图 2硬件总框图42|电子制作2023 年 7 月实验研究STM32F105，内置电路中包含 DW1000、电源、拨码开关等模块。其中，DW1000 是 UWB 的一个信号收发器。该模块具有许多优点，如穿透力强、安全性高、系统复杂度低、定位精度高等。此外，UWB 通信模块通过拨码开关的变化可以同时作为基站和标签。1.2.3 电机驱动模块小跟搬选用的电机驱动模块的型号为 L298N，是专用的驱动集成电路。由于 L298N 可通过主控芯片的 I/O 输入对电机的控制电平进行直接设定，因此能实现电机的正反转的变化。程序化控制简便、硬件操作易上手、稳定性优良，满足直流电机的大电流驱动条件，符合小跟搬运动的条件要求。1.2.4 超声波模块小跟搬选用的超声波模块的型号为 US-015，US-015 可实现 0.02m-4m 的非接触测距功能，探测精度为0.1cm+1%，分辨率高于 1mm，可达 0.5mm。与普通的超声波模块（HC-SR04）相比，其具有高分辨率、重复测量一致性高、工作性能稳定等特点。1.2.5 蓝牙模块系统应用的蓝牙模块为 jdy-30 是依据蓝牙 3.0 协议标准，工作频段为 2.4GHz 范围的片上系统，主要适用于蓝牙高速数据传输。数据传输比 BLE 蓝牙快、可达到 8K 每秒以上的速率，并且功率消耗低。利用这个优势可将蓝牙与MCU 进行串口通讯来实现标签，基站的时钟同步。2 智能跟随 2.1 跟随原理本次研究设计的小跟搬自动跟随系统是通过 UWB定位技术来实现的。其共使用到4个UWB单片机模块，小跟搬上装有 3 个 UWB 基站（A1、A2、A3），外加一个标签（B0）装备在手环中，模块示意图如图 3 所示。其基本的工作原理为：由小跟搬上固定位置的三个基站模块（A1、A2、A3）发射信号，位于手环上的标签（B0）接收到此信号后返回信息，用算术平均滤波法滤除出现的噪声和不可靠信号，并利用双向飞行时间法计算得到标签与三个基站之间的几何信息，完成距离数据的初步收集。可根据三个基站与标签 B0 的直线距离，利用勾股定理可得到手环和基站的水平高度差 H（单位：m）。计算出水平差后，可根据主基站A1与手环的直线距离x1（单位：m）实现小跟搬自动跟随功能，而 A2、A3 辅助基站与手环的直线距离 x2、x3（单位：m）则作为小跟搬的转向判定，小跟搬主芯片通过PID算法进行计算，调节控制电机，最终实现小跟搬的跟随和控制转向功能。跟随系统软件设计流程图如图 4 所示。图 4跟随系统软件设计流程图 2.2 UWB 定位算法2.2.1 测距算法UWB 技术的测距原理大致有两种，一种是飞行时间测距法，另一种是双向飞行时间法。飞行时间测距法：主要利用信号在两个异步收发机（Transceiver）之间飞行时间来测量节点间的距离。因为在视距视线环境下，基于 TOF 测距方法是随距离呈线性关系。双向飞行时间法：设备 A 首先向设备 B 发出一个数据包，并记录下发包时刻 Ta1，设备 B 收到数据包后，记下收包时刻 Tb1。之后设备 B 等待 Treply 时刻，在 Tb2（Tb2=Tb1+Treply）时刻，向设备 A 发送一个数据包，设备 A 收到数据包后记下时刻值 Ta2。飞行时间测距法虽然计算简单，但是测距精度有所欠缺，并且设备约束比较严格，需要设备之间时钟必须同步，所以采取双向飞行时间法。首先将固定在小跟搬上的三个基站通过有线连接将时钟同步，在手环上的标签 B0 利用蓝牙模块实现时钟同步，减小计算式的误差值。当小跟搬开机，并与智能手环匹配后，首先将各基站与标签时钟同步，之后 A1、A2、A3 基站在 Ta1 时刻发送信号，标签 BO 分别在 Tt1、Tt2、Tt3 时刻接收到，然后等待Treply（单位：s）时间后，标签 T0 再分别发送信号给基站，收到的时间为 Tb1、Tb2、Tb3。由此，根据公式（1）（2）（3）就可分别计算出各个基站与标签之间的直线距离 S1、S2、S3（单位：m）。1*(11/2SCTbTaTreply=)式(1)22*/2(1)SCTbTaTreply=式(2)3*b3a1)/2 (SCTTTreply=式(3)（C为光速，C 约为 3108m/s）图 3模块示意图wwwele169com|43实验研究2.2.2 定位算法当小跟搬成功将基站与标签之间的直线距离测算出来后，首先进行身高估算阶段。小跟搬通过 PID 算法调节小跟搬方向，使得 A2、A3 两个辅助基站与手环标签的直线距离相等，如图 5 定位算法所示。距离调节完毕后，因为已知小跟搬长度为 I（单位：m），宽度为 W（单位：m），预设的跟随距离为 L（单位：m），根据勾股定理可分别计算出各基站与标签的水平高度差 h（单位：m），最后通过算数平均法计算得到小跟搬与手环的水平高度差 H（单位：m）。图 5定位算法2211()hSIL=+式(4)22()2wXL=+式(5)2222hSX=式(6)2233hSX=式(7)1233hhhH+=式(8)公式(4)为已知小跟搬长度 I（单位：m）和预设跟随距离 L（单位：m），根据主基站 A1 与手环标签的直线距离 S1（单位：m），计算得出 A1 基站预估的基站与标签的水平高度差 h1（单位：m）；公式(5)为已知小跟搬宽度 W（单位：m）和预设跟随距离 L（单位：m），可算出辅助基站 A2、A3 与手环标签垂直投射的阴影距离 X（单位：m）；公式(6)(7)为已知基站 A2、A3 与手环标签垂直投射的阴影距离 X（单位：m），根据基站与手环标签的直线距离S2、S3（单位：m），计算得出两个辅助基站预估的基站与标签的水平高度差 h2、h3（单位：m）；公式(8)为将三个基站预估的水平高度差取其平均值，得到最终结果 H（单位：m）。已知水平高度差后，通过读取主基站 A1 与手环标签的直线距离来实现跟随功能，两个辅助基站的主要作用为计算标签与小跟搬的偏移角度，实现转向。2.2.3 角算法当从避障模式切换回跟随模式后，小跟搬与标签会出现较大的角度偏移，如果读取出标签与小跟搬的偏移角度，就能很快重新控制电机使小跟搬回归到跟随模式的路径中。如图 6 所示为定位的俯视图，A 为手环标签，BC 分别是两个辅助基站的位置，AD 为目前小跟搬与手环投影的直线距离，E 为两个辅助基站的中点。因为已经算出了基站与标签的水平高度差 H，根据勾股定理可算出 AB、AC 的长度，BC 为小跟搬的宽度，根据公式(9)(10)海伦公式就可以算出 ABC 的面积 S（单位：m），根据公式(11)则可以算出 AD 的距离，最后就可以计算出 BD 的距离，BD 再与 BE 作差可得 ED 的距离，以此类推可推导出 AE 的距离，利用反三角公式(12)可得出 EAD 的角度。()/2PABBCCA=+式(9)*()*()*()SPPABPBCPCA=式(10)2*(/)ADSBC=式(11)90(/EADarcsin ADAE=)式(12)通过得到夹角 EAD 的大小即可得到标签与小跟搬的偏移角度，从而控制小跟搬按规定进行偏转运动。3 实验测试结果 3.1 数据测试为了检测基站与标签之间的水平高度差和偏移角度的精度，设计了两个实验来对精度进行检验测试，然后分析实验中所得实际数据与真实测量数据出现的误差。第一个实验测试是在水平地面上进行，通过将预设跟随距离分为 0.5m、1m、1.5m，实际水平高度差分为 0.5m、1m、1.5m 后，在每种条件下进行五次实验，计算出的标签与基站之间的水平高度差，并记录下与实际水平高度差作差得到的实验误差量，得到表 1。然后将在每种条件下五次实验的数据算出平均值后取绝对值，得到每种条件下的误差数据，绘制出如图 7 所示的水平高度测量误差趋势图。图 6 定位的俯视图44|电子制作2023 年 7 月实验研究表1不同条件下的水平高度差测量误差量预设跟随距离（m）真实水平高度差（m）五次实际测量数据的偏差（mm）050050+321+474-346+103+222100+824+653+1249-049+732150-1423-1376-1633-989+044100050+1363+1412+1278-1258+1133100+2755+1901+1156-1137+1122150-3022-2891-2457-3035-2493150050+1637+1926+2746+966+1124100+4657+3061+4131+4946+3877150-8435-6188-7157-7957-9328 图 7水平高度测量误差趋势图第二个实验测试是将手环标签与基站放置在同一水平面上进行，通过将手环标签与两个辅助基站的垂直距离分为0.5m、1.0m、1.5m，实际偏移角度分为 30、45、60后，在每种条件下进行五次实验，计算出手环标签对于辅助基站的偏移角度，并记录下与实际偏移角度作差得到的实验误差量，得到表 2。然后将在每种条件下五次实验的数据算出平均值后取绝对值，得到每种条件下的误差数据，绘制出如图8 所示的偏移角度测量误差趋势图。表2不同条件下的偏移角度测量误差量垂直距离（m）实际偏移角度（）五次实际测量数据的误差（）0503000-12+05-17+13+214500+11+21+08-05-176000-15+06+20-10+111003000+24+19-28+16+244500-30-18+30-26+166000+30+29-19+31-291503000-41+52-24+40+474500-59+37+45-27+126000-43+16+57-42+38 3.2 误差分析本次实验可以看出在平面上，计算基站与标签之间的水平高度差的误差量会随着预设的跟随距离的增加而增加，其实际水平高度差对于误差量的影响较小，但在可以接受的范围内，并且对跟随的影响较小。而在同一水平面上的计算偏移角度在 6以内，在可接受范围以内，并搭配上 PID 算法进行调试，误差即可得到有效控制。图 8偏移角度测量误差趋势图4 总结本文在研究智能跟随搬运机器人技术的背景下，所设计出的小跟搬采用了近年热门的超宽带（UWB）定位技术，以此具备了抗干扰能力强，穿透力强等优势，同时运用了双向飞行时间算法（TW-TOF）大大缩小了由模块间的高度差所带来的误差，实现了更精准的跟随，此外超声波避障系统使小跟搬在复杂的地形情况能够自动避障达到了跟随的精度要求。通过试验机检验可知我们的技术方案是可行的，但对于该方案中的信号降噪技术仍需进一步的优化与改进。参考文献 1 刘金海,王勃凡,周龙,郑岳久.基于 UWB 技术实现自动跟随小车设计 J/OL.农业装备与车辆工程,2019(12):1-92020-02-07.2 刘晏宁.基于 UWB 测距的分布式室内定位系统 J.电子世界,2017(22):133-134.3 潘忠祥,江剑.基于 UWB 的三边优化算法研究 J.国外电子测量技术,2019,38(02):25-29.4 胡海兵,张文达,郑希鹏,曾贵苓.基于 UWB 的智能跟随车导航定位算法研究 J.电子技术应用,2019,45(03):80-83+87.5 张重阳,刘新月,赵欢,赵雪.基于 STM32 的智能小车防追尾系统 J.电子世界,2018(05):196-197+200.6 罗勃.基于 UWB 技术的 TDOA 定位算法的研究与实现 D.海南大学,2017.7Sakr Mostafa,Masiero Andrea,El-Sheimy Naser.LocSpeck:A Collaborative and Distributed Positioning System for Asymmetric Nodes Based on UWB Ad-Hoc Network and Wi-Fi Fingerprinting.J.Sensors(Basel,Switzerland),2019,20(1).（下转第 32 页）32|电子制作2023 年 7 月实验研究（上接第 44 页）性应变增加值的大小趋势，与其平均温度最大值与最小值极差的趋势基本一致，温度极差越大，累计等效塑性应变增长越快。3 结语本文主要介绍了功率模块系统仿真包含的内容：热仿真、寄生参数仿真、双脉冲仿真、载流能力仿真、应用电路仿真、绝缘耐压仿真、可靠性仿真、EMI/EMC仿真、工艺过程仿真等方面；强调系统级仿真需要不断提高其覆盖范围和准确性；以一功率模块（4 颗芯片并联，三项六单元）为例，简单演示了功率模块系统仿真的效果，包括热仿真、寄生参数仿真、载流能力仿真、绝缘耐压仿真、双脉冲仿真、应用电路仿真和可靠性仿真，并对各个仿真结果进行了简单分析。图 32不同键合线 Bond 点面平均温度随时间变化图 33 不同键合线 Bond 点平均累计等效塑性应变随时间变化图 34Bond 点最后三个周期平均累计等效塑性应变增加值与周期内平均温度最大值与最小值极差之间的关系 8 吴 杰,王 小 妮,刘 鹏,王 家 坡,孙 瑶.智 能 小 车 蓝 牙 通信模块设计与实现 J.北京信息科技大学学报(自然科学版),2019,34(06):64-69+75.9 张育人.基于 Android 的智能避障小车系统的设计与实现 J.机械制造与自动化,2019,48(05):147-149+156.10Petsiuk Aliaksei L,Pearce Joshua M.Low-Cost Open Source Ultrasound-Sensing Based Navigational Support for the Visually Impaired.J.Sensors(Basel,Switzerland),2019,19(17).11 吴瑞锐,朱晓峰,宋宗峰.基于 HC-SR04 多超声波避障技术J.智库时代,2020(03):281-283.12 王鹏飞,张映宏,王昊,关豪.基于 STM32F103 微控制器自动避障小车控制系统设计J.信息技术与信息化,2019(02):77-80.13 彭 笑,张 丹 红,熊 斌 宇,江 朝 朝.基 于 DW1000 的 室 内定位系统设计与稳定性优化 J.广西大学学报(自然科学版),2018,43(05):1803-1810.14 黄 梦 雨,秦 建 军,高 磊,鲁 增 辉.基 于 UWB 的 零 转 径跟随机器人控制系统设计 J.重庆理工大学学报(自然科学),2019,33(09):142-150.累计等效塑性应变增加值与周期内平均温度最大值与最小值极差之间的关系中，可以看到 Bond 点之间的累计等效塑参考文献 1 马建,刘晓东,陈轶嵩,汪贵平,赵轩,贺伊琳,许世维,张凯,张一西.中国新能源汽车产业与技术发展现状及对策 J.中国公路学报,2018,31(08):1-19.2 刘佳佳,刘英坤,谭永亮.SiC 电力电子器件研究现状及新进展 J.半导体技术,2017,42(10):744-753.