基于內三角形質心算法的超寬帶室內定位

魏 培，姜 平，賀晶晶，張會猛

(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