文献标识码：A
DOI：10.16157/j.issn.0258-7998.183260
中文引用格式：胡海兵，张文达，郑希鹏，等. 基于UWB的智能跟随车导航定位算法研究[J].电子技术应用，2019，45(3)：80-83，87.
英文引用格式：Hu Haibing，Zhang Wenda，Zheng Xipeng，et al. Research on navigation and positioning algorithm for intelligent following vehicle based on UWB[J]. Application of Electronic Technique，2019，45(3)：80-83，87.
0 引言
随着科学技术突飞猛进的发展，人们开始解放双手，更加注重设备的智能化和信息化。行李箱虽有轮子辅助可以替人们省力，但也已经不能满足人们对智能化生活的更高需求，而智能跟随车使箱体具有自动跟随的功能，可以极大地减轻人们的负担。目前虽已出现一些智能跟随车的控制方法[1-4]和定位方法[5-9]，但由于控制系统和定位精度的局限，基本都面临跟随效果不理想、功能单一等问题。
本设计提供了一种智能跟随装置，具有更好的控制方法及定位算法，使智能跟随车可以更加安全有效地跟随主人，不会出现丢失目标或者路径紊乱的情况。本设计提出了一种定位算法，在智能跟随车上方的两角分别固定一个基站，人手持移动标签在两基站的前方，先由到达时间算法(Time Of Arrival，TOA)[10]得出标签到两个基站的距离，数据经过卡尔曼滤波处理，再利用三角函数进行相关计算，以两个基站的中点为基准点，两个基站所在直线为基准线，得到手持标签到基准点的距离以及偏离基准线的角度，再将所得的距离和角度测量值与实际值相比较并分析误差，预期结果为：距离误差不超过10 cm，角度误差不超过10°。
1 系统总体设计
实验初期进行了一系列的方案选择，有单蓝牙方案、单红外方案、单433 MHz无线方案和单超声波方案，均被验证效果不理想。最终采用了无线和蓝牙结合的方案。
系统总体设计及工作原理为：由手环发射信号，射频模块接收此信号，用低通滤波算法滤除噪声和不可靠信号,完成数据预处理工作。图1所示为智能跟随车初步定位示意图，预处理信号通过TOA算法可以确定手环到两射频模块的距离，两射频模块之间的距离固定，利用海伦公式和三角形面积相等原理，即得到手环与两基站所在直线的垂直距离h，并由高度补偿传感器得到手环与射频模块的垂直误差X，由勾股定理即可算出手环与智能跟随车水平方向的直线距离L。至此完成对智能跟随车的初步定位工作，整个过程中用低通滤波算法和卡尔曼滤波算法对数据融合，以尽可能减小误差。
初步定位信息结合九轴平衡传感模块和深度传感器不断反馈数据误差，经离散化比例积分微分(Proportion Integration Differentiation，PID)算法[11]处理间隔为50 ms的数据，使智能跟随车始终处于稳定状态，完成跟随车的精确定位。精确定位的信息结合避障模块、重力传感器的反馈信息在控制单元STM32模糊PID算法的控制下，对手环实际位置和智能跟随车的实际路况信息进行融合、滤波以及规则分析，完成最优路径规划，此处采用的路径规划算法是最优解求解过程的粒子群优化算法(Particle Swarm Optimization，PSO)[12]。
各模块信息最后也都要进行低通滤波和卡尔曼滤波处理，防止数据丢失，保证各模块的数据信息能够同步交给控制模块处理。控制模块根据各模块反馈的数据信息来合理分配两电机的脉冲宽度调制值(Pulse Width Modulation，PWM)，以不断地实时控制电机的行进方向和速度，最终实现跟随车安全高效的跟随功能。
2 硬件设计
智能跟随车的目标物为具有信号收发功能的手环。智能跟随车具有控制单元、避障模块、两个射频模块、高度补偿传感器、重力传感器、深度传感器、平衡模块和驱动模块。车底盘的四角共设有4个避障模块，前后两端共设有2个深度传感器，底盘的前端设有2个射频模块和1个高度传感器，底盘的后端设有1个平衡模块。其他部件也都设置在底盘上。硬件总体结构如图2所示。
2.1 射频模块
两个射频模块采用的是UWB Mini 3s模块，以STM32F103T8U6为主控芯片，外围电路包括DWM1000模块、电源模块、LED指示模块和复位电路等。该模块基站标签为一体，通过拨码开关切换基站和标签功能。其作用是接收手环发来的信息，并传送给控制模块。
2.2 高度补偿传感器
高度补偿传感器用于确定手环与智能跟随车之间的垂直距离，将所收集到的信息反馈到控制模块。其作用是优化箱子的位置信息，使定位更加准确。
2.3 重力传感器
重力传感器用来测量智能跟随车所装行李的重量。当智能跟随车内装载有不同重量的行李时，相应调节电机转速，保证跟随车在装不同重量的行李下都可以跟上手环持有者的速度。
3 软件设计
3.1 主程序设计
智能跟随车上电后，首先进行系统的初始化，包括时钟配置、定时器初始化以及调试程序时所需串口的初始化。接着进行DWM1000模块初始化。初始化工作完成后，判断基站是否收到了标签发送的中断信号，若是，则基站将信息传给单片机计算出所需的距离和角度数据，经由PID控制算法调节电机驱动模块的PWM值，从而控制电机的转速和转向；否则返回继续进行该判断。本设计中智能跟随车的软件设计流程如图3所示。
3.2 定位原理与算法描述
本节算法设计的目的为计算出手持标签到两个基站中点的距离，以及偏离两基站所在直线的角度，并通过串口输出在显示屏上，便于观测和记录。具体算法描述如下：图4所示为实验定位算法原理图，图中点A为手持标签，点B和点C为智能跟随车上的两个固定基站，且两基站之间距离固定为33 cm，点D为两个基站的中点，线段
边上的高。手持标签A到两个基站B和C的距离通过TOA方法得到。
海伦公式为：
3.3 卡尔曼滤波程序设计
卡尔曼滤波是一种利用线性系统状态方程；通过系统输入输出观测数据对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，因此也可看作是滤波过程。首先引入一个线性随机微分方程：
4 测试数据及误差分析
4.1 实验设计
为了测试本设计中智能跟随车的定位精度，设计了如下实验，实验示意图如图5所示。图中右侧智能跟随车为俯视图，其中A和B为智能跟随车上方两脚固定的两个基站，C为可移动的手持标签，D为A、B两个基站的中点。
在本实验设计中，共选取了9个点进行测试，在图5中分别编号为1～9。其中点1～3的实际距离均为50 cm，实际角度分别为45°、60°和90°；点4～6的实际距离均为100 cm，实际角度分别为45°、60°和90°；点7～9的实际距离均为150 cm，实际角度分别为45°、60°和90°。
利用3.2中所介绍的算法，可以求解出图中
的距离以及∠CDB的角度，并通过串口输出显示出来。在实验中对每个点均选取100组连续的测试数据，并对所有数据取几何平均值作为每个点的测量值，以避免均值受极端值的较大影响。最后，将测量值与实际值进行对比，分析误差。
4.2 测量数据
经过实验，测出实验数据并在极坐标中画出每个点对应的距离和角度，得到如图6所示的测量数据点图。
图6中点1～3为实际距离50 cm，实际角度分别是45°、60°和90°对应点的测量值；点4～6为实际距离100 cm，实际角度分别是45°、60°和90°对应点的测量值；点7～9为实际距离150 cm，实际角度分别为45°、60°和90°对应点的测量值。
将每个点测量值的距离和角度分别与实际值的距离和角度作差，得到各个点对应的距离误差和角度误差数据，并绘制得到如图7所示的距离误差数据图和如图8所示的角度误差数据图。
4.3 误差分析
由图7和图8可得，本实验中距离的误差在-2 cm～+9 cm之间，角度的误差在-8°～+10°之间，满足设计和定位的需求。
从实验的过程来看，实验数据误差的来源可能有：
(1)手持标签不稳定。由于实验过程中可移动标签由人手持，在实验中可能会有轻微的不稳定，会导致数据有偏差。
(2)实际距离和角度标定的误差。两个基站的距离和九个测试点在实际操作中要用卷尺测距进行定点，在此过程中可能会有测量误差存在。
(3)手持标签和基站所在平面不水平。当手持标签与基站所在平面不水平时，标签到基站的距离会比实际值偏大，导致距离测量值偏大，角度测量值也会偏大。
(4)数据位数的丢失。由于在软件部分程序的计算中，数据的小数部分在运算中会被截断，并不能得到精确的数据，也会导致数据的误差。
5 结束语
本文设计了一种可以有效融合各个功能模块，集定位、避障、平衡、跟随、网络于一体的安全、可靠、高效的智能跟随装置、控制方法及定位方法，通过实验验证，本设计中智能跟随车定位的距离误差不超过9 cm，角度误差不超过10°，满足设计需求。在后续的实验设计中，会继续优化软件部分的设计，使实验误差进一步缩小。本设计系统相比于现有技术的控制和定位方法，可以使智能跟随车更加安全有效地跟随主人，不会出现丢失目标或者路径紊乱的情况，在市场上也有广泛的应用价值，对国内智能跟随领域的成长与进步也具有重要的意义。
参考文献
[1] 丁柏文，金琦淳，任俊，等.基于IAP15F2K61S2单片机的红外与无线搬运小车[J].仪表技术与传感器，2017(10)：110-114.
[2] 王志铠.基于UWB无线定位技术的AGV室内导航与控制技术研究[D].成都：电子科技大学，2018.
[3] 赵昊宁，聂宪波，关立强，等.自动跟随运输系统设计[J].自动化应用，2015(12)：69-71.
[4] 刘磊，孙晓菲，张煜.基于STM32的可遥控智能跟随小车设计[J].电子测量技术，2015(6)：31-33.
[5] 王飞.基于UWB的室内移动机器人导航定位技术的研究[D].哈尔滨：哈尔滨工程大学，2017.
[6] 贺晶晶，姜平，冯晓荣.基于UWB的无人运输车的导航定位算法研究[J].电子测量与仪器学报，2016，30(11)：1743-1749.
[7] 蔡磊，周亭亭，郭云鹏，等.基于超声波定位的智能跟随小车[J].电子测量技术，2013，36(11)：76-79.
[8] 卢靖宇，余文涛，赵新，等.基于超宽带的移动机器人室内定位系统设计[J].电子技术应用，2017，43(5)：25-28.
[9] IBRAHEEM M.Gyroscope-enhanced dead reckoning localization system for an intelligent Walker[C].2010 International Conference on Information Networking and Automation(ICINA).IEEE，2010：67-72.
[10] 李俊杰，王世练，路军，等.基于TOA的中心车辆定位系统的算法研究[J].通信技术，2001(5)：23-25.
[11] 薛辉，王丛岭.基于单片机采用PID算法的电机运行控制系统设计[J].机床电器，2006，33(1)：51-52.
[12] 李炳宇，萧蕴诗，汪镭.PSO算法在工程优化问题中的应用[J].计算机工程与应用，2004，40(18)：74-76.
作者信息:
胡海兵1，2，张文达1，2，郑希鹏1，2，曾贵苓3
（1.合肥工业大学 光电技术研究院 特种显示技术国家工程实验室，安徽 合肥230009；
2.合肥工业大学 光电技术研究院 现代显示技术省部共建国家重点实验室，安徽 合肥230009；
3.芜湖职业技术学院 电气工程学院，安徽 芜湖241006）