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

基于UWB的自主跟随机器人定位系统的设计
徐庆坤; 宋中越
【期刊名称】《《计算机工程与设计》》
【年(卷),期】2019(040)011
【总页数】5页(P3337-3341)
【关键词】超宽带; 自主跟随; 移动机器人; 定位; 卡尔曼滤波
【作者】徐庆坤; 宋中越
【作者单位】天津中德应用技术大学新能源系天津300350
【正文语种】中文
【中图分类】TP242.6
0 引言
移动机器人定位技术一直被人们广泛研究[1,2]，相比于主要应用于结构化环境下
的定点机器人，如何在非结构化环境下获取自身状态与周围环境信息的技术难点成为当前移动机器人研究的热点问题之一[3,4]。

牛群峰等[5]构建了一种基于TW-TOF的UWB信号定位系统，采用三边测量法实现室内定位，针对误差噪声问题，采用IMM卡尔曼滤波对测量数据进行优化处理，实验结果显示该系统有效降低了定位数据的噪声干扰。

符世琛等[6]提出了一种基
于UWB信号的TW-TOF测距的掘进机位姿检测方法，并分别在室内与室外3个
实验场地设计了测距测试实验，分别从环境变化、测距范围等方面对定位精度的影
响进行了分析。

徐永立等[7]针对移动机器人的自主跟随问题，采用超声波测距模
块实现障碍检测，采用UWB定位技术实现目标定位，分别从目标坐标解算方法、移动机器人控制策略等方面进行了分析，测试结果可看出该系统取得了良好的定位效果。

基于此，本文设计了一种基于UWB技术的自主跟随机器人定位系统。

选用DecaWave公司开发的DW1000芯片作为UWB无线收发模块，采用基于TOF
飞行时间方法进行测距，针对室内环境下由于非视距、多径传播等不利因素所造成的噪声干扰问题，采用卡尔曼滤波算法对测距数据进行优化处理，最后经过三边测量算法获得目标的定位数据，从而提高系统的定位精度与可靠性。

1 定位系统总体设计
自主跟随机器人定位系统由机器人定位单元、跟随目标、控制终端3部分构成，
系统如图1所示。

机器人定位单元主要由定位基站A0、A1、A2与主控制器组成，其中定位基站之间通过UWB无线通讯技术实现数据传递；主控制器通过UART
串行通讯方式接收定位基站A0的定位数据，定位数据为标签与各基站之间的实测距离；跟随目标为机器人定位单元所跟随的对象，通常为人员或车等运动对象；控制终端为一台移动手机终端，通过WiFi与机器人定位单元的主控制器实现数据交互，完成机器人定位系统的数据监视与控制。

图1 跟随机器人定位系统
机器人定位单元选用树莓派3B+作为主控制器，它具有以下特性：①1 GB SDRAM内存；②千兆以太网；③2.4 GHz 和5 GHz双频Wi-Fi；④蓝牙4.2&低功耗蓝牙(BLE)；⑤Micro-SD存储。

UWB定位基站A0、A1、A2与标签由DWM1000芯片与STM32F103T8U6单
片机构成，STM32F103T8U6单片机通过SPI总线对DWM1000模块实现数据交换。

定位基站A0与树莓派3B+主控制器之间通过UART串行通讯实现数据交换，
机器人定位单元硬件结构框架如图2所示。

图2 定位单元硬件结构框架
2 定位系统的定位方法
2.1 基于TOF的测距法
目前按照定位方法是否利用测距可划分为基于测距和非测距的定位方法[8,9]。

其
中基于非测距的定位方法比较典型的有：DV-HOP、K最近邻居法、K-N法等，
这种方法从现实角度来讲，绝大部分定位精度都要低于基于测距的定位方法；基于测距的定位方法定位原理都很类似，比较典型的有：基于信号到达时间TDOA定位、基于信号到达角度AOA定位、基于测距的TOF定位[10]等，其中TDOA定
位需要严格的时间同步并且求解较复杂；AOA定位由于角度分辨率的问题，在远
距离情况下定位精度较差，一般需与其它定位方式配合使用；TOF定位不需要时
钟同步并且求解简单，成本低。

因此，本系统采用基于TOF测距的定位方法。

2.2 卡尔曼滤波算法
在实际定位过程中由于存在不可避免的测量误差，因此需采用卡尔曼滤波算法对测距数据进行处理。

卡尔曼滤波算法是一种利用线性系统状态方程，通过实时观测系统输入输出数据，对系统状态进行最优估计。

由于观测数据中包含噪声干扰的影响，因此卡尔曼算法可看作是一个滤波过程,已经成为一种应用于传感器定位系统的标
准算法。

卡尔曼滤波是一种高效率的递归算法[11]，适用于估计一个动态系统的最优状态。

它的实质是通过测量值重构系统的状态向量，再经“预测-校准”的递推过程消除
随机干扰所造成的误差，进而得到系统的最优估计值[12]。

其中预测过程主要的作用为：滤波器根据上一状态的估计值，对系统的当前状态作出预测；而校正过程的主要作用为：滤波器通过当前状态观测值对预测过程中获得的预测值进行修正，进而获得更接近真实值的新估计值。

这两大过程可概括为以下5个计算方程：
(1)状态预测方程
X(k|k-1)=AX(k-1|k-1)+BU(k)
(1)
其中， X(k|k-1) 表示k-1时刻的状态预测值， A表示状态转移矩阵， X(k-1|k-1) 表示k-1时刻的状态估计最优值， B是系统参数， U(k) 是k时刻的控制量。

(2)协方差预测方程
P(k|k-1)=AP(k-1|k-1)AT+Q
(2)
其中， P(k|k-1) 是k-1时刻状态预测值对应的协方差矩阵，同理P(k-1|k-1) 是k-1时刻状态估计最优值对应的协方差矩阵， Q是系统过程中噪声(高斯白噪声)的协方差矩阵。

(3)计算卡尔曼增益
(3)
其中， K为卡尔曼增益， H为观测矩阵， R为测量噪声(高斯白噪声)对应的协方差矩阵。

(4)状态更新方程
X(k|k)=X(k|k-1)+K(Z(k)-HX(k|k-1))
(4)
其中， Z(k) 为k时刻系统的观测值，根据前两个预测方程可得到当前状态的预测值，然后再通过当前状态的观测值以及增益，就可计算出当前状态的最优状态估计值X(k|k)。

(5)协方差更新方程
P(k|k)=(I-KH)P(k|k-1)
(5)
其中， I为1的单位矩阵。

由于系统为线性离散系统，为了保证递推下去，当系统进入k+1时刻时， P(k|k) 对应着式(2)中的P(k-1|k-1)。

最后，通过卡尔曼递推方程可看出，在计算时需要确定参数的初始状态，即系统的初值。

初值的选取影响着递归过程是否能很好的收敛，如选择偏差较大，可能导致滤波结果含有较大误差，因此要根据实际情况合适选择，一般通过近似法确定。

2.3 三边测量算法
三边测量(trilateration)[13-15]是一种建立在水平控制网的方法，通过测量三角形的边长，再根据已知控制点的坐标、起始边的边长和坐标方位角，解算各三角形顶点水平位置，它是一种常用的定位算法。

如果将已知参考节点用P1、P2、P3表示，未知目标用T表示，目标T与P1、P2、P3之间的距离分别为R1、R2、R3,由几何关系可知，分别以P1、P2、P3为圆心，以R1、R2、R3为半径画圆，3个圆将相较于一点，即目标点T点，解算原理如图3所示。

图3 三边测量原理
假设参考点P1坐标分别为 (x1,y1), P2坐标为 (x2,y2), P3坐标为 (x3,y3), 目标点T坐标为 (x,y)，那么可根据以下方程求解
(6)
式(6)中将第一个方程减去第二、三个方程并转换为矩阵形式得
(7)
将式(7)重写为矩阵形式
AX=B
(8)
对于式(8)可用最小二乘法求解，则目标位置为
(x,y)=(ATA)-1ATB
(9)
2.4 混合算法步骤
本系统采用简单易于实现和计算相对简单的三边测量算法实现机器人的相对定位，由于在实际定位过程中存在测量误差的影响，无法按照三边测量的理论推导方法解算到3个圆唯一的交点，因此采用卡尔曼滤波算法首先将UWB模块所测得的测距数据进行滤波处理，然后再利用三边测量算法进行定位。

算法步骤如下：
步骤1 UWB基站A0基于TOF测距方法测量100组距离信息，其中每一组距离信息包括基站A0与各基站、标签之间的距离；
步骤2 采用卡尔曼滤波算法对以上所获得的100组距离信息进行滤波处理；
步骤3 将以上进行滤波处理后的距离信息，通过三边测量算法得到100组定位估计值，再将100组定位估计值求算数平均值，最终得到目标定位坐标 (x,y)。

3 跟随机器人定位测试
3.1 测试环境搭建
为了完成跟随机器人定位系统的测试，检验定位系统的精度与可靠性，在天津中德应用技术大学工业中心E1-5楼车间搭建了一个定位测试环境。

首先选取一个4000mm*4000mm的方形区域，设置区域中心为坐标原点，将机器人定位单元的基站A0、A1、A2坐标分别设定为(0,0)，(400,0)，(0,400)，测试场景示意图如图4所示。

为了简化定位测试过程，提高测试结果的准确性，测试时将标签固定在三脚架上，要求标签高度不变，测试场景实物图如图5所示。

图4 测试场景示意图
图5 测试场景实物图
3.2 测试结果
定位测试在测试区域内的4个区域(4个象限)内进行，测试过程选用Leica DISTO D1激光测距仪作为测距仪器，它在理想条件下测量精度为2 mm，测程为0.2 m～40 m，它的技术参数满足系统的测试需求，从而减小由于人工测量对定位测
试结果所造成的影响。

为了更好地验证定位系统的定位性能，分别在静态与动态两种测试方案下进行测试。

静态测试是在4个区域内随机选择4个点进行测试实验，为了更好验证系统的定
位效果，在坐标轴上分别随机选取一个点进行测试，每个位置点分别采集100次
定位数据，将100次的定位数据求算术平均值后得到的结果作为该点的实测结果。

表1为在静态测试环境下标签实测坐标与真实坐标的对比统计数据，其中式(10)为误差的计算公式。

通过表1中真实坐标与实测坐标的对比数据可以看出：定位系
统的定位精度控制在20 cm以内，满足自主跟随机器人定位系统的定位精度的要
求
(10)
其中，e为误差，xz与yz为真实坐标值，xc与yc为实测坐标值。

表1 静态点测试数据(mm)真实坐标实测坐标误差真实坐标实测坐标误差
(212,236)(265,355)130(-1587,-1678)(-1557,-
1567)115(531,638)(710,672)182(-1498,-1245)(-1408,-
1150)131(1147,1567)(1296,1688)192(209,-398)(239,-
323)81(1515,560)(1652,691)190(489,-654)(548,-514)152(-604,457)(-
496,540)136(976,-703)(1050,-581)143(-1023,658)(-921,667)102(1389,-1508)(1487,-1367)172(-1202,860)(-1157,920)75(1276,6)(1363,172)187(-898,1376)(-838,1433)83(8,1456)(136,1587)183(-876,-567)(-724,-497)167(-1676,7)(-1568,49)116(-1023,-1489)(-932,-1469)93(8,-1457)(60,-1396)80
动态测试是通过人为将固定在三脚架上的标签以约 0.5 m/s 的速度在测试区域内
移动进行测试，动态测试结果如图6所示，其中实线为真实位置，虚线为实测位置，从图中可看出动态测试误差控制在30 cm以内，可满足自主跟随机器人的动
态定位需求。

图6 动态测试结果
3.3 控制终端
图7为定位系统的测试移动终端界面，移动终端与定位系统主控制器之间通过WiFi连接，实现数据的交换，完成本系统的测试任务，为了便于测试，移动终端
可远程遥控跟随机器人。

图7 控制终端界面
4 结束语
本文设计了一种基于UWB技术的自主跟随机器人定位系统，采用基于TOF的测
距方法保证了定位系统的测距精度，融合卡尔曼滤波算法对测距数据进行优化处理，从而有效地抑制了由于非视距、多径传播所引起的干扰影响，提高了自主跟随机器人定位精度，采用三边测量算法对滤波后的测距数据进行解算定位。

测试结果表明，该定位系统提高了跟随机器人定位的精度与可靠性，可满足自主跟随机器人定位系统的要求。

参考文献：
【相关文献】
[1]LI Mengyuan,ZHOU Fengyu,TIAN Tian,et al.Design of indoor WiFi cloud positioning system based on GAN and its application to service robots[J].Robot,2018,40(5):119-129(in Chinese).[栗梦媛,周风余,田天,等.基于GAN的服务机器人室内WiFi云定位系统设计与实现[J].机
器人,2018,40(5):119-129.]
[2]LIU Huaping,ZHENG Xiangmei,SUN Fuchun.Orientationestimate of indoor mobile robot using laser scans[J].Journalof Tsinghua University(Science and
Technology),2018,58(7):609-613(in Chinese).[刘华平,郑向梅,孙富春.基于雷达信息的室内移动机器人的方位估计[J].清华大学学报(自然科学版),2018,58(7):609-613.]
[3]YANG Ping,LIU Yuankai,ZHAO Meixiang.Design and Reserch of the walkiing mechanism of the stair climbing robot based on the deformable wheel structure[J].Journal of Mechanical Transmission,2016,40(6):110-114(in Chinese).[杨萍,刘远凯,赵梅香.基于可变形车轮结构的阶梯攀爬机器人行走机构的设计与研究[J].机械传动,2016,40(6):110-114.]
[4]WANG Xiaowei,HE Lile,ZHAO Tao.Mobile robot for SLAM research based on lidar and binocular vision fusion[J].Chinese Journal of Sensors and Actuators,2018,31(3):394-399(in Chinese).[王消为,贺利乐,赵涛.基于激光雷达与双目视觉的移动机器人SLAM研究[J].传感技术学报,2018,31(3):394-399.]
[5]NIU Qunfen,CAO Yifan,WANG Li,et al.Study of UWB indoor positioning technology and optimization algorithm based on TW-TOF[J].Automation & Instrumentation,2018,33(1):5-9(in Chinese).[牛群峰,曹一帆,王莉,等.基于TW-TOF的UWB室内定位技术与优化算法研究[J].自动化与仪表,2018,33(1):5-9.]
[6]FU Shichen,LI Yiming,ZHANG Minjun,et al.Accuracy testing experiment in narrow roadway based on TW-TOF ranging technique of UWB signals[J].Coal
Technology,2017,36(3):246-248(in Chinese).[符世琛,李一鸣,张敏骏,等.基于UWB信号的TW-TOF测距技术在狭长巷道中的精度测试实验研究[J].煤炭技术,2017,36(3):246-248.]
[7]XU Yongli.Design of autonomous following robot based on UWB
technology[D].Xi’an:Chang’an University,2018(in Chinese).[徐永立.基于UWB技术的自主跟随机器人设计[D].西安:长安大学,2018.]
[8]YUAN Shuai.Research on smart car positioning and path planning based on ultra-wideband[D].Xi’an:Xidian University,2018(in Chinese).[袁帅.基于超宽带的智能小车定位与路径规划技术研究[D].西安:西安电子科技大学,2018.]
[9]LIU Tao,XU Aigong,SUI Xin.Adaptive robust Kalman filtering for UWB indoor positioning[J].Chinese Journal of Sensors and Actuators,2018,31(4):567-572(in Chinese).[刘韬,徐爱功,隋心.基于自适应抗差卡尔曼滤波的UWB室内定位[J].传感技术学报,2018,31(4):567-572.]
[10]Fujiwara R,Mizugaki K,Nakagawa T,et al.TOA/TDOA hybrid relative positioning system based on UWB-IR technology[J].Ieice Transactions on Communications,2011,94(4):1016-1024.
[11]WEI Heng,LYU Qiang,WANG Guosheng,et al.Trajectory tracking control for heterogeneous mobile robots based on UWB ranging[J].Journal of Beijing University of Aeronautics and Astronautics,2018,44(7):1461-1471(in Chinese).[卫恒,吕强,王国胜,等.基于超
宽带测距的异构移动机器人轨迹跟踪控制[J].北京航空航天大学学报,2018,44(7):1461-1471.] [12]Xiaojun S,Guangming Y.Multi-sensor optimal weighted fusion incremental Kalman smoother[J].Journal of Systems Engineering and Electronics,2018,29(2):262-268.
[13]Safavi S,Khan U A,Kar S,et al.Distributed localization:A linear theory[J].Proceedings of the IEEE,2018,106(7):1204-1223.
[14]Luo R C,Hsiao T J.Dynamic wireless indoor localization incorporating with an autonomous mobile robot based on an adaptive signal model fingerprinting approach[J].IEEE Transactions on Industrial Electronics,2019,66(3):1940-1951.
[15]FAN Zeyuan.Study on train speed fusion method by fuzzy adaptive federated kalman filter algorithm[D].Lanzhou:Lanzhou Jiaotong University,2018(in Chinese).[樊泽园.基于模糊自适应联合卡尔曼的列车速度融合方法研究[D].兰州:兰州交通大学,2018.]。