基于内三角形质心算法的超宽带室内定位

魏 培，姜 平，贺晶晶，张会猛

(1.南通大学 电气工程学院，江苏 南通 226019； 2.南通大学 电子信息学院，江苏 南通 226019)

(*通信作者电子邮箱jiang.p@ntu.edu.cn)

基于内三角形质心算法的超宽带室内定位

魏 培1，姜 平1*，贺晶晶1，张会猛2

(1.南通大学 电气工程学院，江苏 南通 226019； 2.南通大学 电子信息学院，江苏 南通 226019)

(*通信作者电子邮箱jiang.p@ntu.edu.cn)

针对工业现场自动导引运输车(AGV)定位方式灵活性差的问题，应用DW1000射频(RF)芯片，设计实现了一种超宽带(UWB)室内定位系统。首先，提出了高效的多基站测距和多标签调度机制，解决标签冲突和组网问题。其次，针对三角形质心算法实际应用中测距极大误差引起的定位精度低、稳定差的问题，引入可信度概念，提出了内三角形质心算法，通过可信度算子削弱测距极大误差的影响，提高算法性能。最后，将该系统应用于工业车间，当系统容纳20个标签时，单个标签的坐标平均更新频率达到24 Hz，标准差为3 Hz；静态平均定位误差为11.7 cm，标准差为2.5 cm；动态最大误差小于30 cm。实验结果表明，该系统具有高实时性、高精度和高稳定性的特点。

自动导引运输车；超宽带；室内定位系统；三角形质心算法；定位性能

0 引言

随着智能制造技术的进一步发展，自动导引运输车(Automated Guided Vehicle, AGV)作为一种车间运输设备，其室内自主定位技术的研究也进入到崭新的发展阶段[1]。实际工业应用中，AGV定位主要有预设路径法和非预设路径法两种方式。预设路径法利用电磁、光电等技术，通过在预设路径上铺设电线、喷涂导引线实现定位功能，这种定位方式使工业车间不美观、小车控制灵活性不够，且难以实现多移动机器人复杂路径的控制。非预设路径法主要采用激光、视觉技术，实现成本较高，同时易受环境因素的制约，因而不能广泛应用于实际工业现场[2]。超宽带(Ultra WideBand, UWB)技术是一种利用亚纳秒级超窄脉冲的无载波通信技术，具有高分辨率、抗多径效应、穿透力强、硬件结构简单、频谱利用率高等优点，成为目前室内高精度定位应用最佳物理层技术，能够实现对移动物体的精确定位[3-4]。

三角形质心算法是一种基于三边测量[5]的定位算法，在室内定位系统中被广泛使用。文献[6]将三角形质心算法应用到无线传感器网络中，但由于普通三角形质心算法未能体现信标节点对节点位置影响力的问题，算法稳定性差。文献[7]针对具体实验环境，提出一种新型的加权方法，使用测试距离倒数之和作为权重因子，同时使用修正系数，避免信息淹没现象，提高算法定位精度，但该方法过于依赖实际环境，适应能力较差。文献[8]在此基础上，考虑环境因素对算法精度的影响，寻找能够更精确地估算未知节点最终位置的坐标集合，有效地减小了多次估算质心产生的累积误差，提高了算法的性能，但算法流程较多，实现比较复杂。

本文基于UWB技术，采用Decawave公司的DW1000射频芯片，应用基于非对称双边双向测距(Asymmetric Double Sided Two-Way Ranging, ADS-TWR)技术的定位机制，针对三角形质心算法在实际应用中出现的定位精度低、稳定性差问题，引入可信度概念，提出内三角形质心算法，设计并实现一种高实时性、高精度和高稳定性的室内AGV定位系统。

1 室内无线定位系统

图1为UWB室内定位系统示意图，模拟在具有4个生产工位的工业车间，AGV实现工位间自动搬运原料进行加工的场景。为了实现对AGV的位置等状态信息进行有效监控，系统设为三级分布式控制结构(设备层-数据层-应用层)，各层之间通过WiFi或Internet网络连接。

图1 室内无线定位系统示意图

设备层包括AGV、安装在车间顶部的基站和安装在AGV上的标签。基站分为通信基站(基站1)和普通基站(基站2、基站3和基站4)，与普通基站相比，通信基站安装有WiFi模块，用于连接设备层和数据层。硬件设计上，基站和标签均由单片机和DW1000芯片组成的通信模块构成，且由软件设置模块工作模式实现基站或标签功能。通过参数配置，DW1000可以精确地测量到UWB信号接收和发送的时间，在系统定位机制下，标签与基站间应用ADS-TWR测距技术计算距离，并利用UWB信号具有的通信功能，将距离值等数据经通信基站上传至数据层。标签(普通基站，通信基站另外设有WiFi功能模块)、AGV控制原理如图2所示，其中标签与AGV之间使用串口进行通信。

图2 标签(基站)和AGV控制原理

数据层由定位引擎构成，利用定位算法计算标签坐标。应用层使用C#技术开发基于Windows平台的监控软件，供用户查询AGV位置等状态信息和设置各种系统参数。

2 基于测距技术的UWB定位机制

2.1 ADS-TWR技术

实时定位系统中，为了计算标签的位置坐标，必须准确地测量标签与基站间的距离。ADS-TWR技术是一种异步测距技术，通过在两个不同节点间依次完成双向测距减小节点两侧时钟偏置引起的测量误差，计算信号在节点间的无线传播时延(Time Of Flight, TOF)，从而估算节点之间的距离[9-11]。ADS-TWR测距过程如图3所示，ttof表示UWB信号在标签与基站间的无线传播时延，tsp、trr、tsf、trp、tsr、trf代表UWB信号离开或到达基站(标签)天线的时间，测距过程如下：首先标签主动向基站发送请求帧触发测距时序，基站接收到请求帧后启动计时，在应答时间treplyB后向标签发送应答帧，标签接收到应答帧后启动计时，同时将treplyA和troundA值写入终止帧，并在应答时间treplyA后将终止帧发送给基站，基站接收到终止帧表示测距时序结束。基站计算距离如式(1)～(2)所示:

(1)

d=ttof×c

(2)

其中：c表示UWB信号传播速率;d表示标签与基站间距离。

图3 ADS-TWR测距原理

测距误差如式(3)所示:

error=ttof×(1-(ka+kb)/2)

(3)

其中：ka、kb为节点侧的时钟偏置系数(接近1)。可以看出，ADS-TWR技术在保证测量精度的同时，对两侧节点应答时间的对称性无要求，因而具有很强的实用性。本文设计基于DW1000的基站和标签，在室内视距(LineofSight,LoS)环境下可实现10cm测距精度。

2.2 多基站测距机制

基于ADS-TWR技术的室内定位要求标签至少与3个基站完成测距，为了减少标签测距的时间，保证定位系统的实时性要求，本文采用如图4所示优化的多基站测距机制。

请求帧包含4个基站的应答时间，标签以广播方式向所有基站发送请求帧触发多基站测距时序，各基站解析请求帧信息，并按设置的应答时间依序向标签发送应答帧，标签在接收到基站应答帧后，将用于计算TOF的参数写入终止帧，以广播方式发送给所有基站，基站接收终止帧，表示一轮多基站测距时序结束。根据式(1)～(2)，各基站计算与标签的距离值。

应用多基站测距机制，标签与4个基站间仅需发送6组帧包即可完成测距工作。通过调整DW1000工作参数，优化UWB帧包结构，实际应用中可控制单个帧包传输时间小于250μs，最终实现单次多基站测距时序时间小于2ms，从而满足定位系统对实时性的要求。

图4 多基站测距时序示意图

2.3 多标签调度机制

定位系统实际应用需要解决标签组网和多标签防冲突算法设计问题[12]，通过引入计算机领域“注册注销”和“按时间片轮转”思想，设计一种多标签调度机制，有效保证定位系统高效运行。

系统调度机制如图5所示，当标签通电后，首先向通信基站(基站1)发送注册帧，请求一个合法身份，在接收并解析通信基站注册应答帧后，标签获得基站坐标等信息，并得到一个时间片序号，该时间片序号决定标签何时可以触发测距时序。为了优化性能，通信基站会定期扫描所有标签，注销一段时间内没有消息的标签，更新标签时间片序号。

3 定位算法

基于ADS-TWR测距技术的定位方法总结为：在系统调度机制下，标签依序与基站完成ADS-TWR测距过程，对距离值进行滤波去噪[13-15]后，运用定位算法计算标签的位置坐标。如何改进定位算法，对提高系统定位精度和稳定性有着重要的作用。

3.1 三角形质心算法模型

三角形质心算法模型如图6所示，标签T与基站Ai(i=1,2,3)的距离为di，以基站位置为圆心，相应的测距值为半径画圆，得到标签的3个近似位置特征点B、C、D，则△BCD质心即为标签坐标，如式(4)所示:

(4)

三角形质心算法通过将标签坐标确定在△BCD内，并利用三角形质心估算标签坐标，实现对标签的定位。当标签与各基站测距误差均匀分布时，该算法可以获得较高的定位精度。但实际应用中UWB信号传播易受多径和非视距(NonLineofSight,NLoS)现象影响，同时，又由于室内环境复杂多变，难以通过建立一个具有普遍适用性的UWB信道模型来估计测距误差范围，这使得算法稳定性得不到保证[16]。针对三角形质心算法存在的不足，下面提出一种基于“可信度算子”的内三角形质心算法，即通过引入可信度算子，削弱测距极大误差的影响，从而提高算法的定位精度和稳定性。

图5 多标签系统调度流程

图6 三角形质心算法模型

3.2 内三角形质心算法

3.2.1 可信度算子

由图6可知，当标签与某一个基站测距误差较大时，相应的特征点明显远离其他点，使得三角形质心严重偏离标签实际位置，定位精度下降。本文提出可信度概念用于体现特征点与标签实际位置的接近程度，即特征点越接近标签实际位置，可信度越高，对标签坐标计算的影响力越大；反之，可信度越低，影响力越小。可信度通过可信度算子K表示，如式(5)所示：

K=1/LN

(5)

其中：L表示特征点与标签实际位置间的距离；N为可信度修正系数，用于分配特征点间权重主次关系，可根据实际环境选取合适的值。

3.2.2 算法模型

算法思想如下：对于具有3个基站的定位算法模型，假设某一特征点为标签实际位置，计算其余特征点的可信度算子K，运用加权平均公式求取这些特征点的等效位置；依次对3个特征点作上述处理，可以求得3个等效特征点，以等效特征点为顶点的内三角形质心即为标签坐标。如图6所示，以特征点B为例，LBC和LBD分别代表特征点C、D与参考点B间距离，设N=1，则KC=1/LBC、KD=1/LBD，根据式(6)可以得到加权特征点B′，同理以C、D为参考点可得C′、D′，则△B′C′D′质心为标签坐标。

(6)

4 实验及结果分析

4.1 实验环境

实验地点为室内18m×16m×6m的工业车间，该环境下多径和NLoS干扰现象较为严重。在车间四角布置4个基站，高度4m，基站硬件结构如图7所示，其中最左边为通信基站，安装有WiFi功能模块。同时，为了对系统中的数据进行有效监控，使用如图8所示的监控软件界面。

图7 基站(标签)实物图

图8 应用层监控软件

4.2 定位算法性能比较

为了验证三角形质心算法在多径和NLoS环境下的定位精度和稳定性，在车间中选取10个具有代表性的位置(标签与1～2个基站之间有遮挡物，遮挡物为厚度1cm的钢板)，将同一个标签依次静止放置在上述位置，分别使用三角形质心算法和内三角形质心算法进行对比实验。实验1使用三角形质心算法，实验2、3、4使用内三角形质心算法，分别设置可信度修正系数N=1、N=2和N=3。通过19次测量取中位值的方法减小误差，并利用监控软件记录实验数据，如表1所示。

可以看出，在室内多径和NLoS环境下，三角形质心算法定位精度较低，且对于标签位于定位区域边缘时，定位误差明显增大，算法稳定性较差。而内三角形质心算法通过引入可信度算子，明显提高了定位精度和稳定性，相比实验1，实验2、实验3、实验4使用内三角质心算法定位误差平均减小9.2cm、16.4cm和12.9cm，标准差减小2.4cm、5.1cm和3.8cm。同时发现，根据实际环境调整可信度修正系数N，可有效减小标签位于定位区域边缘时的定位误差，进一步提高定位算法的性能。由表1数据可知，针对当前实验环境，选取N=2，定位误差稳定在20cm以内。

表1 定位误差

4.3 系统性能验证

进一步，根据车间中AGV实际使用情况，将标签数量扩大到20个，检验系统的实时性和定位精度。实验过程如下：从系统中随机选取一个标签，保持高度不变，以0.5m/s的速度沿着设定的路线移动，监控软件记录该标签移动过程中所有的定位数据；同时选择30个时间点(各时间点间隔大约为3min)，记录该时间点起1s中标签坐标更新的次数。实验结果如图9～10所示。

图9反映了标签在30个采样时间点上的坐标更新频率。当系统容纳20个标签时，单个标签的坐标平均更新频率f=24Hz，标准差σ=3Hz，满足了实际车间环境下对AGV快速定位的要求。

图10描述了标签沿目标轨迹移动时，系统所记录的标签位置信息。可以看出，标签偏离目标轨迹的最大距离为28cm，相比标签静止时所计算的定位误差，由于标签移动过程中存在的各种干扰因素，实际动态误差偏大，但仍能保证一定的定位精度和稳定性。

5 结语

本文针对工业现场AGV定位方式灵活性差的问题，使用UWB技术，设计实现一种室内定位系统。一方面，优化系统定位机制，提高定位频率；另一方面，针对传统三角形质心算法定位精度低和稳定性差的问题，提出了基于可信度算子内三角形质心算法。实验结果表明，系统具有高实时性、高精度和高稳定性的特点。

图9 标签坐标更新频率

图10 标签动态轨迹

本文系统未来可应用在社会生产的各个方面，但作为商用定位系统还存在以下问题：1)系统定位功能实现需要标签与基站之间使用UWB脉冲信号进行多轮通信，功耗较大，而实际应用中，标签一般使用电池供电，标签低功耗设计问题有待解决；2)标签使用系统分配的时间片序号进行定位，随着标签数量的增多，标签的定位频率会明显下降，系统标签容量受到制约；3)实际工业环境复杂多变，需要进一步优化定位算法，使系统达到更高的定位精度和稳定性。

)

[1] 王健.AGV的研制与运动控制实验研究[D].哈尔滨:哈尔滨工业大学,2012:1-5.(WANGJ.Researchondesignandmotioncontrolexperimentforautomatedguidedvehicle[D].Harbin:HarbinInstituteofTechnology, 2012: 1-5.)

[2] 胡克维.自动导引小车AGV的导航和避障技术研究[D].杭州:浙江大学,2012:2-5.(HUKW.Researchonnavigationandobstacleavoidancetechnologyforautomatedguidedvehicle[D].Hangzhou:ZhejiangUniversity, 2012: 2-5.)

[3] 郭建广,郑紫微,杨任尔.基于改进变尺度法的超宽带定位新算法[J].计算机应用,2014,34(12):3395-3399.(GUOJG,ZHENGZW,YANGRE.NewUWBlocalizationalgorithmbasedonmodifiedDFPalgorithm[J].JournalofComputerApplications, 2014, 34(12): 3395-3399.)

[4]MONICAS,FERRARIG.Aswarm-basedapproachtoreal-time3Dindoorlocalization:experimentalperformanceanalysis[J].AppliedSoftComputing, 2016, 43: 489-497.

[5] 彭宇,王丹.无线传感器网络定位技术综述[J].电子测量与仪器学报,2011,25(5):389-399.(PENGY,WANGD.Areview:wirelesssensornetworkslocalization[J].JournalofElectronicMeasurementandInstrument, 2011, 25(5): 389-399.)

[6] 林玮,陈传峰.基于RSSI的无线传感器网络三角形质心定位算法[J].现代电子技术,2009,32(2):180-182.(LINW,CHENCF.RSSI-basedtriangleandcentroidlocationinwirelesssensornetwork[J].ModernElectronicsTechnique, 2009, 32(2): 180-182.)

[7]LIJ,LIUHP.AnewweightedcentroidlocalizationalgorithmincoalMinewirelesssensornetworks[C]//Proceedingsofthe2011 3rdInternationalConferenceonComputerResearchandDevelopment.Piscataway,NJ:IEEE, 2011: 106-109.

[8] 李虎俊,郭蓝天,卢军,等.基于二次质心的无线传感器网络定位算法[J].现代电子技术,2014,37(23):5-8, 12.(LIHJ,GUOLT,LUJ,etal.Twicecentroidbasedlocalizationalgorithmforwirelesssensornetwork[J].ModernElectronicsTechnique, 2014, 37(23): 5-8, 12.)

[9]JIANGY,LEUNGVCM.Anasymmetricdoublesidedtwo-way-rangingforcrystaloffset[C]//ISSSE’07:Proceedingsofthe2007InternationalSymposiumonSignals,SystemsandElectronics.Piscataway,NJ:IEEE, 2007: 525-528.

[10] 段翠翠,王瑞荣,王建中,等.基于无线传感器网络的高危生产区人员定位系统[J].传感技术学报,2012,25(11):1599-1602.(DUANCC,WANGRR,WANGJZ,etal.Locationsystemforstaffinhigh-riskproductionareabasedonwirelesssensornetworks[J].ChineseJournalofSensorsandActuators, 2012, 25(11): 1599-1602.)

[11] 常华伟,王福豹,严国强,等.无线传感器网络的TOF测距方法研究[J].现代电子技术,2011,34(1):35-38.(CHANGHW,WANGFB,YANGQ,etal.TOFrangingmethodforwirelesssensornetworks[J].ModernElectronicsTechnique, 2011, 34(1): 35-38.)

[12] 韦子辉,冯正和,王志峰,等.脉冲超宽带定位单元设计及应用测试[J].电视技术,2014,38(23):60-63.(WEIZH,FENGZH,WANGZF,etal.DesignandapplicationtestofIR-UWBpositioningunit[J].VideoEngineering, 2014, 38(23): 60-63.)

[13] 肖如良,李奕诺,江少华,等.基于卡尔曼滤波与中位加权的定位算法[J].计算机应用,2014,34(12):3387-3390,3394.(XIAORL,LIYN,JIANGSH,etal.IndoorpositioningbasedonKalmanfilterandweightedmedian[J].JournalofComputerApplications, 2014, 34(12): 3387-3390, 3394.)

[14] 李奕诺,肖如良,倪友聪,等.基于动态环境衰减的粒子滤波室内定位算法[J].计算机应用,2015,35(9):2465-2469.(LIYN,XIAORL,NIYC,etal.Indoorpositioningalgorithmwithdynamicenvironmentattenuationbasedonparticlefiltering[J].JournalofComputerApplications, 2015, 35(9): 2465-2469.)

[15] 刘洞波,杨高波,肖鹏,等.移动机器人自适应抗差无迹粒子滤波定位算法[J].仪器仪表学报,2015,36(5):1131-1137.(LIUDB,YANGGB,XIAOP,etal.Mobilerobotadaptiverobustunscentedparticlefilterlocalizationalgorithm[J].ChineseJournalofScientificInstrument, 2015, 36(5): 1131-1137.)[16] 樊甫华,阮怀林.低信噪比下非凸化压缩感知超宽带信道估计方法[J].电子学报,2014,42(2):353-359.(FAN F H, RUAN H L.Non-convex compressive sensing Ultra-Wide Band channel estimation method in low SNR conditions [J].Acta Electronica Sinica, 2014, 42(2): 353-359.)

This work is supported by the National Natural Science Foundation of China (61371111).

WEI Pei, born in 1992, M.S.candidate.His research interests include wireless sensor network localization, embedded system.

JIANG Ping, born in 1962, M.S., professor.His research interests include detection technology, intelligent control.

HE Jingjing, born in 1992, M.S.candidate.Her research interests include mobile communication system.

ZHANG Huimeng, born in 1992, M.S.candidate.His research interests include remote control of Internet of things.

Ultra wideband indoor localization based on inner triangle centroid algorithm

WEI Pei1, JIANG Ping1*, HE Jingjing1, ZHANG Huimeng2

(1.SchoolofElectricalEngineering,NantongUniversity,NantongJiangsu226019,China;2.SchoolofElectronicInformation,NantongUniversity,NantongJiangsu226019,China)

Aiming at the poor flexibility of Automated Guided Vehicle (AGV) localization method in industrial working field, an Ultra WideBand (UWB) indoor localization system by using DW1000 Radio Frequency (RF) chip was designed and implemented.Firstly, to solve the problem of conflicts and networking of tags, the efficient mechanisms for multi-station ranging and multi-tag scheduling were proposed.Secondly, concerning the low accuracy and poor stability of the triangle centroid localization algorithm caused by maximal ranging errors, a concept of credibility was introduced and the inner triangle centroid algorithm was proposed, which could weaken the influence of maximal ranging errors through credibility coefficient to improve the algorithm performance.Finally, the proposed system was applied to the industrial workshop with 20 tags.For a single tag, the average frequency of coordinate updating reached 24 Hz and its standard deviation was 3 Hz; the static average localization error was 11.7 cm and its standard deviation was 2.5 cm; the dynamic maximum error was within 30 cm.The experimental results show that the proposed localization system has the characteristics of high real-time performance, high precision and high stability.

Automated Guided Vehicle (AGV); Ultra WideBand (UWB); indoor localization system; triangle centroid algorithm; localization performance

2016-06-24;

2016-08-30。 基金项目:国家自然科学基金资助项目(61371111)。

魏培(1992—)，男，安徽池州人，硕士研究生，主要研究方向：无线传感器网络定位、嵌入式系统； 姜平(1962—)，男，江苏南通人，教授，硕士，主要研究方向：检测技术、智能控制； 贺晶晶(1992—)，女，河南郑州人，硕士研究生，主要研究方向：移动通信系统； 张会猛(1992—)，男，山东济宁人，硕士研究生，主要研究方向：物联网远程控制。

1001-9081(2017)01-0289-05

10.11772/j.issn.1001-9081.2017.01.0289

TP212.6

A