張家僮，王洪源

(沈陽理工大學(xué) 信息科學(xué)與工程學(xué)院，沈陽110159)

隨著當(dāng)今社會人工智能和物聯(lián)網(wǎng)的快速發(fā)展，自動跟隨定位技術(shù)已經(jīng)成為當(dāng)前科技的研究熱點，并且越來越多的普及到人們的生活當(dāng)中。自動跟隨定位技術(shù)的出現(xiàn)成為影響當(dāng)今通信時代的一個關(guān)鍵因素。在過去的十幾年里，人們已經(jīng)感受到無線定位技術(shù)為生活帶來的巨大便利，因此無論是在工作還是日常生活中，人們對自動跟隨定位技術(shù)的需求也是越來越多。雖然當(dāng)前室外定位技術(shù)的發(fā)展需求可以被4大全球定位系統(tǒng)(GPS、北斗系統(tǒng)、GLONASS系統(tǒng)、伽利略衛(wèi)星導(dǎo)航系統(tǒng))滿足，但在GPS信號極其微弱甚至接收不到信號時，人們將無法從衛(wèi)星廣播中提取獲得位置服務(wù)和有效導(dǎo)航數(shù)據(jù)，所以人們專門研究設(shè)計了應(yīng)用于室內(nèi)的定位技術(shù)?，F(xiàn)今，這些先進(jìn)的室內(nèi)定位技術(shù)中，非接觸式近距離無線通信技術(shù)定位已經(jīng)成為熱點。但這些無線定位技術(shù)技術(shù)都會存相應(yīng)的短板。例如：IEEE.802.11a雖然傳輸距離長，速度快，適用于公司企業(yè)中距離較大的無線網(wǎng)絡(luò)，但是它的成本較高，且功耗比較大；藍(lán)牙技術(shù)同樣也存在成本高、傳輸距離短、傳輸速率低、技術(shù)還不夠成熟等缺點，因此應(yīng)用并不廣泛；Home RF[1]雖然代替了需要鋪設(shè)昂貴傳輸線的有線家庭網(wǎng)絡(luò)，成為網(wǎng)絡(luò)中的設(shè)備，但是Home RF占據(jù)了與藍(lán)牙相同的2.4G頻率段，并且在功能上過于局限家庭的應(yīng)用。因此實用性并不強(qiáng)。并且這些室內(nèi)的定位技術(shù)，在實際環(huán)境的應(yīng)用下，這些利用接受到的信號強(qiáng)度進(jìn)行定位的技術(shù)容易收到干擾，且測量的數(shù)據(jù)誤差較大，測量精度不能滿足人們?nèi)粘Ｉ畹男枰６鳸WB(Ultra Wide Band)超寬帶技術(shù)雖然屬于無線通信技術(shù)，但通信速率極高[2-3]。UWB超寬帶技術(shù)所需要的設(shè)備價格并不昂貴，因此設(shè)計一個系統(tǒng)包含UWB定位模塊、嵌入式驅(qū)動模塊、電機(jī)驅(qū)動模塊、電源模塊以及汽車模組，其中UWB定位模塊是搭建UWB超寬帶自動跟隨定位系統(tǒng)的重要組成部分，由一個移動標(biāo)簽和兩個相對固定基站組成，是目前適用性最高的室內(nèi)跟隨定位系統(tǒng)。

1 UWB自動跟隨定位技術(shù)原理

1.1 跟隨原理

小車的自動跟隨功能是基于三個無線連接的嵌入式定位系統(tǒng)開發(fā)而成。三個無線連接的嵌入式定位系統(tǒng)是指驅(qū)動兩個基站和一個標(biāo)簽組成的UWB定位系統(tǒng)，其實驗所需的標(biāo)簽到兩個基站的位置數(shù)據(jù)測定采用飛行時間TOF(Time of Flight)算法。通過數(shù)學(xué)推導(dǎo)公式將兩個定位基站與移動標(biāo)簽的距離信息轉(zhuǎn)變?yōu)闃?biāo)簽在小車坐標(biāo)系中的相對坐標(biāo)信息，經(jīng)過其相關(guān)數(shù)據(jù)進(jìn)行融合后通過無線通信方式傳輸?shù)杰囕v控制系統(tǒng)，最終實現(xiàn)小車跟隨標(biāo)簽移動。

1.2 定位算法

在自動跟隨系統(tǒng)中，測量標(biāo)簽到基站的距離是最重要的，是系統(tǒng)的構(gòu)架基礎(chǔ)。因此本系統(tǒng)測距使用TOF飛行時間的測距方法[4]。TOF測距是基于雙向測距的方法，經(jīng)由信號在標(biāo)簽和基站之間往返的飛行時間來測量節(jié)點間的距離，要求移動標(biāo)簽和定位基站必須始終同步，并且定位基站接收信號的傳輸時間要在一定時間內(nèi)。TOF測距原理圖如圖1所示。設(shè)備A主動發(fā)送數(shù)據(jù)，設(shè)備B接收到之后記錄接受時間標(biāo)記；延時Treply之后，設(shè)備B發(fā)送數(shù)據(jù)，同時記錄發(fā)送時間標(biāo)記；設(shè)備A接受數(shù)據(jù)，同時記錄接收時間標(biāo)記。所以可以得到兩個時間差Treply。

圖1 TOF測距原理圖

最后得到無線信號的飛行時間Tprop如下。

Tprop=(Tround-Treply)/2

(1)

則設(shè)備A、B兩點的距離為

d=c×Tprop

(2)

式中c是電磁波傳播速度。

兩個差值時間都是基于本地的時鐘計算得到的，本地時鐘誤差可以抵消，但不同設(shè)備之間會存在微小的時鐘偏移，假設(shè)設(shè)備A和B的時鐘分別為tA和tg，因此得到的飛行時間會隨著Treply的增加而增加，測距誤差的方程如下。

error=Tprop-T2prop≈1/2(tB-tA)×Treply

(3)

在得到移動標(biāo)簽到兩個定位基站之間距離值后，設(shè)令基站A為相對坐標(biāo)原點，移動標(biāo)簽的相對坐標(biāo)為(X，Y)。

當(dāng)標(biāo)簽的相對位置在兩個定位基站中，如圖2所示。

圖2 標(biāo)簽在兩基站中間示意圖

結(jié)合數(shù)學(xué)推導(dǎo)，得到移動標(biāo)簽位置。

X2+Y2=P2

(4)

(S-X)2+Y2=Q2

(5)

式中：X、Y代表標(biāo)簽到縱、橫軸的距離；P、Q表示標(biāo)簽到基站A、B的距離；S表示兩基站的距離。

當(dāng)標(biāo)簽的相對位置在基站A的左側(cè)，如圖3所示。

圖3 標(biāo)簽在基站A左側(cè)示意圖

結(jié)合數(shù)學(xué)推導(dǎo)，得到移標(biāo)簽位置。

X2+Y2=P2

(6)

(S+X)2+Y2=Q2

(7)

當(dāng)標(biāo)簽的相對位置在基站B的右側(cè)，如圖4所示。

圖4 標(biāo)簽在基站B右側(cè)基站示意圖

結(jié)合數(shù)學(xué)推導(dǎo)，得到移動標(biāo)簽位置。

X2+Y2=P2

(8)

(X-S)2+Y2=Q2

(9)

1.3 誤差分析

根據(jù)DWM1000參考官方提供的數(shù)據(jù)手冊[5]，判斷系統(tǒng)的誤差來源是DWM1000的天線誤差和多徑誤差。天線誤差的產(chǎn)生是由芯片內(nèi)部決定的一個固定值，而此值會影響到實際距離的測量；多徑誤差的產(chǎn)生是源于UWB信號接收端的機(jī)制，一般接收端接收達(dá)到設(shè)定閾值的首個信號并將其作為信號的到達(dá)時間，但在實際環(huán)境中由于障礙物導(dǎo)致的信號能量損失，可能會使得首個到達(dá)信號被錯誤識別。

因此通過結(jié)合實際的系統(tǒng)測試，發(fā)現(xiàn)對該部分的校準(zhǔn)可以結(jié)合其芯片自帶的存儲功能，僅需要初次使用時校準(zhǔn)；并將基站安裝在遠(yuǎn)離障礙物的地方，使得標(biāo)簽在室內(nèi)大部分活動范圍內(nèi)為視距條件，就可以解決這兩種誤差，使誤差在可以控制的范圍內(nèi)。

UWB自動跟隨系統(tǒng)中，整體設(shè)計由移動標(biāo)簽、定位基站、主控制器和電機(jī)驅(qū)動模塊構(gòu)成，如圖5所示。移動標(biāo)簽由室內(nèi)中的測試人員攜帶以及小車對其定位跟隨；定位基站被布置在小車的兩側(cè)。測試過程中，通過獲取標(biāo)簽和定位基站之間往返的飛行時間來得到標(biāo)簽的位置信息，結(jié)合數(shù)學(xué)推導(dǎo)得到小車與標(biāo)簽的位置信息。通過主控制器上傳到小車的電機(jī)模塊，實現(xiàn)室內(nèi)中小車對移動標(biāo)簽的自動跟隨。

圖5 系統(tǒng)結(jié)構(gòu)圖

2 UWB自動跟隨測試系統(tǒng)設(shè)計

2.1 移動標(biāo)簽與定位基站硬件設(shè)計

系統(tǒng)中移動標(biāo)簽主要包括嵌入式驅(qū)動模塊、定位模塊、電機(jī)驅(qū)動模塊以及電源模塊。定位基站在移動標(biāo)簽的基礎(chǔ)上多加了一個接口模塊。系統(tǒng)的主控芯片采用意法半導(dǎo)體公司生產(chǎn)的STM32[6]芯片，該款MCU基于ARM Cortex-M3內(nèi)核，主頻32MHz，包含4個通用的16位定時器，多達(dá)3個SPI接口，電源工作電壓為2.0～3.6V；該芯片擁有高性能、低成本和低功耗等優(yōu)點。

定位模塊中采用Deca Wave公司推出的DWM 1000芯片[7]來實現(xiàn)測距功能。該模塊是一款基于IEEE 802.15.4-2011通信標(biāo)準(zhǔn)的無線信號收發(fā)模塊。DWM 1000芯片定位控制器開啟定位功能命令。如果接受到主控制器的定位命令，會分別向各基站發(fā)送測距請求，且精度可達(dá)厘米級別，支持高數(shù)據(jù)數(shù)率通信，可以根據(jù)實驗測試的需求采用110Kb/s、850Kb/s和6.8Mb/s的數(shù)據(jù)傳輸速率。DWM 1000芯片采用相干接收技術(shù)，支持3.5～6.5GHz的6組中心頻率帶寬，通訊范圍高達(dá)300m，抗多徑衰落。

2.2 電機(jī)模塊與電源模塊硬件設(shè)計

電機(jī)驅(qū)動模塊采用SGS公司生產(chǎn)的L298N芯片[8]。L298N芯片最為方便的是其輸入端可以直接與STM32相聯(lián)，不需要其他復(fù)雜的設(shè)計，就可以直接對電機(jī)驅(qū)動模塊進(jìn)行控制。

整個跟蹤定位系統(tǒng)的電源使用12V的鋰電池，由于STM32控制器使用的電壓為3.3V，電機(jī)驅(qū)動模塊電壓為5V，所以系統(tǒng)需要將電源模塊設(shè)計成穩(wěn)壓電路，將電壓降到各模塊需要的電壓值，使其實現(xiàn)供電功能。

2.3 系統(tǒng)軟件設(shè)計

小車自動跟隨系統(tǒng)軟件以雙核STM32為處理器，雙核協(xié)調(diào)配合完成小車的路徑規(guī)劃。系統(tǒng)軟件設(shè)計主要針對標(biāo)簽、基站子程序進(jìn)行。首先標(biāo)簽完成初始化后，等待主命令，并等待基站返回測距結(jié)果。實驗通過TOF算法得到標(biāo)簽到基站的距離，然后將標(biāo)簽坐標(biāo)發(fā)送給主控制器。采集數(shù)據(jù)偽代碼如下。

getdistance()

creatpulse()//生成脈沖

time<-now()//存儲當(dāng)前時間

while get pulse()

do time=now()-time//獲取時間差

end

time<-time / 2//獲取單程時間

distance<-time * c//c為光速，distacne為測量的距離

return distance

然后開啟基站接收功能，等待標(biāo)簽的測距命令。如成功接收到標(biāo)簽命令，基站會向標(biāo)簽發(fā)送一幀數(shù)據(jù)，記錄相應(yīng)的收發(fā)時間。通過數(shù)學(xué)推導(dǎo)可計算出小車與標(biāo)簽的距離。每完成一次測距，會自動清除時間的緩存信息，并等待下一次測距命令。

在距離測量算法中，由于各種因素的影響，每次測量的結(jié)果不同，這里面有包括小車的隨動誤差。就一次測量值而言，真實值和小車的隨動誤差都是未知的，所以系統(tǒng)需要對數(shù)據(jù)進(jìn)行多次測量，通過數(shù)學(xué)推導(dǎo)公式取一定時間內(nèi)或循環(huán)次數(shù)內(nèi)的算數(shù)平均距離，作為當(dāng)前的測量距離。經(jīng)實驗證明，該算法可以有效地減小誤差，解決所測數(shù)據(jù)和真實值之間的誤差問題。計算數(shù)據(jù)偽代碼如下。

while getdistance()//如果獲取到數(shù)據(jù)

do d1，distance2<-getdistance()//獲取兩個基站到標(biāo)簽的距離

fori<-0 to 10

sumd1 <-sumd1+d1

sumd2 <-sumd2+d2

sumd1 <-sumd1/10

sumd2 <-sumd2/10

x<-(sumd1*sumd1-sumd2*sumd2+250000)/1000//計算坐標(biāo)x值

y<-sqrt(abs(sumd1*sumd1-x*x))//計算坐標(biāo)y值

end

3 UWB自動跟隨系統(tǒng)實驗

3.1 實驗方案設(shè)計

為驗證本系統(tǒng)自動跟隨小車的實際運動行駛狀態(tài)，選擇在室內(nèi)區(qū)域進(jìn)行小車自動跟隨測試。首先將小車放置起點處，將實驗采用的兩個UWB定位基站分別放置于小車的兩端，將可移動的標(biāo)簽任意放置在實驗的規(guī)定區(qū)域內(nèi)。系統(tǒng)設(shè)定小車擁有四種行駛情況：前進(jìn)、左轉(zhuǎn)、右轉(zhuǎn)和停止。標(biāo)簽無論如何移動都可與兩個定位基站形成穩(wěn)固的三角形位置，如圖6所示。

圖6 標(biāo)簽基站位置擺放圖

實驗過程中，通過不斷更改移動標(biāo)簽在規(guī)定實驗場地中的位置，然后通過TOF測距算法與數(shù)學(xué)推導(dǎo)來確定移動標(biāo)簽每一次移動的相對位置坐標(biāo)；再通過檢測、判斷移動標(biāo)簽與小車的的相對位置與距離，來實現(xiàn)小車對于移動標(biāo)簽的自動跟隨功能，實現(xiàn)小車的自主運動(前進(jìn)、左轉(zhuǎn)、右轉(zhuǎn)和停止)。實驗流程如圖7所示。

圖7 系統(tǒng)實驗流程圖

3.2 實驗測試數(shù)據(jù)及結(jié)果

實驗中，小車車身的長度是50cm，因此默認(rèn)兩個定位基站的距離為50cm，并且由小車攜帶兩個定位基站進(jìn)行實驗。在誤差允許的情況下，默認(rèn)移動標(biāo)簽的中心位置為25cm±15cm范圍內(nèi)。在這個設(shè)定的區(qū)間范圍內(nèi)，系統(tǒng)默認(rèn)小車的行駛方向沒有偏離。實驗設(shè)定小車左側(cè)攜帶的基站A為小車坐標(biāo)系中的中點，基站A和基站B的連線為小車坐標(biāo)系中的橫軸，并且過原點與縱軸垂直，小車車向設(shè)為該小車坐標(biāo)系的縱軸。本實驗設(shè)計通過判斷標(biāo)簽到縱軸的距離與標(biāo)簽到橫軸的距離的數(shù)量關(guān)系來判定移動標(biāo)簽相對于小車的位置，進(jìn)而控制小車的行駛狀態(tài)，并通過取八次數(shù)據(jù)的平均值來使測量數(shù)據(jù)接近于真實值，減少實驗誤差。測距數(shù)據(jù)如表1所示。

實驗預(yù)設(shè)當(dāng)Y≥1m時，小車的行駛狀態(tài)為跟隨移動標(biāo)簽向前行駛；當(dāng)Y<1m時，小車的行駛狀態(tài)為停止行使(保持停留原地)。當(dāng)X>40cm時，小車的行駛狀態(tài)為跟隨移動標(biāo)簽向右行駛；當(dāng)X<10cm時，小車的行駛狀態(tài)為跟隨移動標(biāo)簽向左行駛。當(dāng)Y<1m，但X>40cm或X<30cm時，小車的行駛狀態(tài)為在原地旋轉(zhuǎn)。實驗數(shù)據(jù)如表2所示。

表1 TOF測距離結(jié)果

表2 實驗測試結(jié)果

4 結(jié)論

通過結(jié)合TOF定位算法和數(shù)學(xué)公式推導(dǎo)來實現(xiàn)移動標(biāo)簽定位功能，該算法可以有效地減小誤差，使處理后的結(jié)果能達(dá)到預(yù)期的實驗要求。在較長的一段時間下，小車的行駛狀態(tài)良好，并沒有出現(xiàn)隨意更改行駛方向等問題，一直都保持跟隨移動標(biāo)簽的位置進(jìn)行運動。