張家僮，王洪源

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

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

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

1.1 跟隨原理

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

1.2 定位算法

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

圖1 TOF測(cè)距原理圖

最后得到無線信號(hào)的飛行時(shí)間Tprop如下。

Tprop=(Tround-Treply)/2

(1)

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

d=c×Tprop

(2)

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

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

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

(3)

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

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

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

結(jié)合數(shù)學(xué)推導(dǎo)，得到移動(dòng)標(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)簽的相對(duì)位置在基站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)簽的相對(duì)位置在基站B的右側(cè)，如圖4所示。

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

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

X2+Y2=P2

(8)

(X-S)2+Y2=Q2

(9)

1.3 誤差分析

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

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

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

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

2 UWB自動(dòng)跟隨測(cè)試系統(tǒng)設(shè)計(jì)

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

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

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

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

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

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

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

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

getdistance()

creatpulse()//生成脈沖

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

while get pulse()

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

end

time<-time / 2//獲取單程時(shí)間

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

return distance

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

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

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

do d1，distance2<-getdistance()//獲取兩個(gè)基站到標(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//計(jì)算坐標(biāo)x值

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

end

3 UWB自動(dòng)跟隨系統(tǒng)實(shí)驗(yàn)

3.1 實(shí)驗(yàn)方案設(shè)計(jì)

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

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

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

圖7 系統(tǒng)實(shí)驗(yàn)流程圖

3.2 實(shí)驗(yàn)測(cè)試數(shù)據(jù)及結(jié)果

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

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

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

表2 實(shí)驗(yàn)測(cè)試結(jié)果

4 結(jié)論

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