基于UWB定位的緝毒犬訓練跟蹤錄像小車設計

馬興錄，唐亞男

(青島科技大學 信息科學技術學院，山東 青島 266061)

0 引言

一直以來，各國政府嚴厲打擊毒品犯罪活動，但全球范圍內的毒品犯罪活動仍舊活躍，在某些特定區域尤為猖獗，甚至以先進的科技技術作為犯罪武器，傳統的犯罪形式逐漸向技術化、智能化、集團化轉變[1]。在這種嚴峻的形勢下，為有效打擊毒品犯罪，各國政府采取了多種新型稽查毒品的方式，其中不乏智能儀器，例如模擬動物嗅覺與視覺器官開發出的電子眼、電子鼻等[1]。但是，緝毒犬作為傳統緝毒方式之一，不但沒有因現代科技的高度發展而被社會所拋棄，而且越來越受到各國的重視[2]，并將其視為開展緝毒工作中不可或缺的一環。

較早時期人們訓練緝毒犬對傳送帶上行李進行搜毒，并取得良好效果。國內外對于緝毒犬的訓練還沒有采用智能小車進行定位跟蹤進行輔助訓練的先例。機場緝毒犬的工作場所多在行李箱轉盤，因此需要訓練緝毒犬在行李箱轉盤上進行毒物檢查[3-5]。日常訓練中訓導人員需要時刻陪伴在緝毒犬身邊，并手動記錄緝毒犬每次的訓練成果，人工投入成本大，訓練效率低。在當前的緝毒犬訓練工作過程中，存在人力資源有限、任務完成時限緊迫和訓練信息不完全等問題，為有效提高訓導人員訓練緝毒犬的效率，實時記錄與保存緝毒犬的訓練過程并自動導出訓練評測結果，目前亟需一套系統的緝毒犬訓練智能化自動解決方案。

這款輔助訓練的系統，使用樹莓派為核心主板，結合UWB室內定位模塊快速定位跟蹤緝毒犬的位置，搭載USB攝像頭實時向終端傳輸視頻。在提高緝毒犬效率與減少緝毒犬的訓練時間成本方面有較好效果。

1 測距原理

現如今存在的室內定位技術主要有紅外線定位技術、超聲波定位技術、藍牙定位技術、WiFi定位技術、超寬帶(UWB)定位技術[6-7]、射頻識別定位技術以及地磁定位技術[8]。基于輔助設備的室內定位技術主要是通過在室內部署設備進行測距，再由后臺進行定位計算[8]。作為首部應用了UWB技術的iPhone手機，iPhone11除了擁有基礎的定位跟蹤功能，還可以檢測物品在現實世界的位置信息。

作為無線載波通信技術之一的超寬帶(UWB)定位技術[9],利用極高的射頻頻率達到高帶寬的效果。超寬帶定位技術在傳輸信號使產生極短脈沖的信號源，在短時間內以極低的功率實現數據的高速傳播。

在 UWB定位技術中，基于距離的定位算法[10]是最常用的，其中包括飛行時間定位算法(TOF)[11]、到達時間差定位算法(TDOA)以及到達角定位算法(AOA)。

TOF定位算法利用信號收發及終端收發設備之間的數據傳輸時間，基于一定的計算得到收發設備之間的距離[12]。TDOA定位算法利用記錄接收同一個發送設備的不同信號時間差，計算得到設備之間的距離。AOA定位算法利用計算同一組發送設備和接收設備之間的相對方位，得到設備之間的距離。

該系統使用的UWB定位部分采用單邊雙向測距原理，其測距原理如圖1所示。在此基礎上再增加一次通訊，兩次通訊的時間因時鐘偏移引入的誤差可相互彌補[13-14]，得到更高的測距精度，如圖2所示。

圖1 單邊雙向測距原理示意圖

圖2 改進單邊雙向測距原理示意圖

假設設備A和設備B的時鐘精度是20 ppm，ka和kb分別是設備A、B時鐘的實際頻率和預期頻率的比值，那么ka和kb分別是0.999 98或者1.000 02。設備A和B相距100 m，電磁波的飛行時間是333 ns[15]。

(1)

(2)

根據公式(1)和公式(2)計算，因時鐘引入的誤差為20*333*10-9s，則測距誤差為2.2 mm，在實際情況下，由于誤差過小，可以對誤差忽略不計。

2 系統結構

2.1 總體設計

緝毒犬訓練監督輔助系統采用樹莓派4 B電腦為核心，以UWB定位模塊為主要定位部件，結合可見光攝像頭，編寫相關軟件及算法，實現了攝像機定位、智能車跟隨、實時視頻回傳、自動警報提醒以及監控界面可視化等功能。如圖3所示。

圖3 系統整體結構

其中定位部分采用的是UWB定位模塊，由三個基站模塊與一個標簽模塊組成，實現二維平面內定位功能[16]。其中一個主基站通過USB線連接到樹莓派4 B上，負責定位數據的粗處理及傳輸功能。其余兩個基站分別布置在空曠的位置，緝毒犬攜帶標簽活動在基站范圍[17](即以基站為圓心，直徑15 m內)，如果標簽不在基站范圍內或者距離基站過近，系統自動發出警報，提醒工作人員檢查緝毒犬狀態。實時視頻傳輸部分采用通訊協議，由樹莓派將視頻實時傳回到客戶終端。

2.2 硬件結構設計

設計中系統硬件組成結構圖如圖4所示，包括樹莓派4 B、四個D-DWM-PG1.7型UWB定位模塊、三個移動電源、CSI端口攝像頭、115 200 bps串行通訊接口和音箱。

圖4 系統硬件結構

使用樹莓派4 B為核心控制板，其操作系統基于linux，安裝Python運行環境。運用樹莓派網絡探針可以測量吞吐量、延遲以及丟包率[18]，通過分析網絡穩定性、可靠性來保障網絡服務質量。且樹莓派的HDMI高清視頻輸出接口提供1 080 p全高清影像輸出功能。

D-DWM-PG1.7型UWB定位模塊基于 DW1000定位芯片開發，搭載CP2102方案的USB串口，硬件串口更加穩定。主基站和樹莓派連接，兩個次基站與主基站呈等邊三角模式，標簽活動在等邊三角范圍內，分布如圖5所示。該設計測距精度為10 cm，定位精度[19]為30 cm。

圖5 定位模塊放置模擬示意圖

模塊與模塊之間采用標準MODBUS-RTU通訊協議，通訊步驟如表1所示，且配有電源指示燈與信號指示燈。

表1 定位模塊之間通訊步驟圖

D-DWM-PG1.7型UWB定位模塊與樹莓派之間通過TTL串口進行通信。本設計中定位模塊連接樹莓派需要使用TTL串口轉USB的模塊，串口波特率為115 200 bps。四個定位模塊分為三個基站模塊和一個標簽模塊，三個基站中以其中一個基站作為主基站[20]，與樹莓派通過TTL串口轉USB的模塊連接，進行模塊之間的數據粗處理以及將粗處理的數據傳給樹莓派。另外兩個基站由移動電源供電，分別放置在應用場景的邊緣，為減少定位誤差需保證周圍30 cm內沒有物體遮擋。

視頻傳輸部分攝像部分采用MIPI CSI-3協議，它是一種高速雙向協議，主要用于多層對等基于UniPro的M-PHY設備網絡中攝像機和主機之間的圖像和視頻傳輸。攝像頭與樹莓派連接使用常見的USB接口。報警部分選擇的是適配樹莓派4B的基礎音箱，音箱與樹莓派之間使用3.5 mm耳機接口連接。

采用該方案的最終設計成品在成品功耗控制方面表現優異，在此基礎之上實現了組織架構成本的最小化并且具備體積小、易集成、便于攜帶組裝等優點，能夠應用于多種場景。

3 系統軟件設計

3.1 系統功能概述

緝毒犬訓練監督輔助系統軟件總體架構如圖6所示，系統的功能包括實時定位、自動跟隨、語音報警、實時傳輸以及同步顯示五個功能。其中服務端實現對UWB模塊主基站數據解析與處理功能，通過穩定的TCP網絡協議不間斷獲取主基站發來的粗數據，并且對數據進行精處理，分析數據變化，將其轉換成運動指令，控制智能車運動，實現自動跟隨功能。

圖6 軟件功能架構圖

如圖7所示，硬件設備上電并自動開機后，用戶可使用電腦或者移動設備搜索智能車的熱點。連接該熱點后，進入系統界面，開啟攝像，即可看到訓練場的畫面。一旦緝毒犬攜帶標簽越出基站覆蓋范圍，系統將自動發起語音警報。

圖7 總體功能流程圖

3.2 定位追蹤功能模塊設計

3.2.1 定位追蹤流程

系統以定位模塊為輸入點，其軟件功能流程圖如圖8所示。當攜帶標簽的緝毒犬首次出現在基站覆蓋范圍內時，三個基站將采用不間斷測量模式，首先計算各自距離標簽的距離，主基站在此基礎之上匯總其他兩個基站的數據一并發送給樹莓派，通過樹莓派的數據處理驅動調整智能車的初始位置，保證緝毒犬時刻處在可攝像范圍內；隨后主基站不間斷將緝毒犬的位置更新發送至樹莓派，如果當前位置數據較前位置數據呈增大趨勢，則智能車向左移動相應的距離，反之智能車向右移動相應的距離，保證緝毒犬時刻處在可攝像范圍內。一旦位置數據異常，系統自動發出警報提醒緝毒犬運動軌跡偏離工作范圍。

圖8 基于UWB的智能車定位追蹤流程圖

3.2.2 基于移動目標的定位追蹤算法

3.2.2.1 定位數據獲取

根據定位模塊的通訊協議，主機先向模塊發送一段定位指令數據，主基站返回一串8位字符數據串表示接收到指令并開始測距定位，然后主機每隔一段時間發送一次查詢指令數據，主基站返回一串31位字符數據串表示當前測量的數據。然后將其中表示三個基站各自與標簽的距離相關的數據切分出來，每四個為基站到標簽的距離數據(每個基站的距離值是由兩個字節組成，高8位在左，低8位在右)，然后將距離數據轉換成距離。距離的計算公式(3)如下：

L=hex2D(Lh)×256+hex2D(Ll)

(3)

其中:Lh是模塊返回的距離數據的高八位，Ll為模塊返回的距離數據的低八位，均以16進制的形式呈現。使用hex2D函數(Hex To Decimal)將Lh和Ll轉換之后進行計算獲得距離值L。三個基站的距離都通過解析運算得到，然后存到變量參數中用于后面的數據比較。

3.2.2.2 移動目標追蹤校準算法

該設計考慮到緝毒犬工作時趨于集中在小范圍活動的特點，根據時間差模型[21]，定位模組不間斷測量距離，智能車頻繁的移動嚴重影響視頻畫面的穩定性，因此提出了在此背景環境下基于移動目標的定位追蹤算法。

該算法的思路是：設備開機后，首先定位緝毒犬的初始位置，智能車移動到視頻畫面里能看到緝毒犬的位置；然后緝毒犬開始工作，定位模組不斷更新緝毒犬的位置，智能車實時跟隨拍攝，如檢測到定位數據異常，則發起警報，提醒工作人員檢查緝毒犬的工作狀態及當前位置。

算法具體實現：

關于魯迅的短篇小說《藥》，已經有不少學者從影響研究的層面探討了俄蘇作家，如果戈理、契訶夫、屠格涅夫、安特萊夫等對魯迅的影響。老師可以選取一二篇有代表性的學術論文，讓學生閱讀，并體會影響研究的方法和步驟。

設備在開機后定位緝毒犬的初始位置，初始化時間內將每次的獲取到的距離數據進行緩慢平均，一次獲取存儲10組距離數據點，分別進行相加之后，除以10獲取到的平均值即作為緝毒犬的初始位置。

初始位置穩定后，定位模組不間斷工作，主機首先間隔0.1 s獲取一組距離數據，連續獲得幾次，分別相加，求取平均值作為當前的距離數據組a，如公式(4)，之后每獲取三次距離數據，加入距離數據組a，形成新的求平均組值，得到新的平均值作為新的距離數據組a1，如公式(5)，如此無限循環求取am，如公式(6)：

(4)

am(LA,LB,LC)=

(5)

am(LA,LB,LC)=

(6)

m=1,m∈∞

其中:LA、LB、LC分別代表不斷更新的三個基站相對標簽的距離值，距離單位為cm，a(LA,LB,LC)為初始化之后第一次求取的距離數據組。O(LA,LB,LC)為每間隔0.1 s獲取到的距離數據。n表示每間隔0.1 s的當前獲取次數，m表示當前求取的平均距離組值的次數。計算a與a1的各部分距離值的差值，獲得位置突變組(突變組指在限定一個距離差值閾值后，每超出了這個閾值的距離差值的當前距離組即判定為突變組)。獲得突變組之后，就可以判斷緝毒犬的移動軌跡，并且經過多次測試，每次連續間隔0.1 s取4次距離數據，得到的距離數據組計算差值判斷出來的緝毒犬移動軌跡與實際緝毒犬移動軌跡重合率最高。獲得平均距離組值之后，根據目標追蹤校準算法公式(7)計算位置突變組lisc：

lisc(l,r,s)=am(LA,LB,LC)-am-1(LA,LB,LC)

(7)

得到的位置突變組lisc，里面的l，r，s用來判定智能車的運動狀態。

3.2.3 核心算法偽代碼

Input：當前時刻緝毒犬定位數據Pist，包含三個基站相對于標簽的距離數據LA、LB、LC。

Process2：將獲得的初始位置保存，收集三次位置數據組，將這三組和初始位置相加求平均值即當前校準后的位置數據組，再次保存當前校準后的位置數據組，再收集三次位置數據組，與上一個校準后的位置數據組相加求平均，無限循環。

Process3：通過Process2過程中獲得的校準后的當前時刻位置數據組。將當前的位置數據組與前一組校準的位置數據組各部分相減，得到相對距離差值la，lb，lc:

if(LA==0):break；

elif(LA>p):

if(lb*lc==0):Carstop()

elif(lb*lc>0):Carturnleft()

elif(lb*lc<0):Carturnright()

得到一個控制智能車運動的指令。

Final Output：智能車運動CarRun系列指令。

3.3 實時視頻傳輸功能及自動報警

設備開機之后，開啟攝像功能。攝像頭將捕捉到的圖像信息傳回到主機中進行數據處理，實時視頻傳輸流程圖如圖9所示。主機運行socket服務端程序，向終端電腦傳送視頻幀信號；雙方約定好傳輸分辨率，傳輸幀率，傳輸圖片編碼格式，傳輸端口，向服務器端建立連接，并實時傳輸圖像編碼。終端運行socket客戶端程序接收視頻幀，解碼實時展示并保存，然后將圖像由二進制數據，轉化成圖像矩陣，并利用openCV庫函數，進行展示和保存，方便工作人員查看記錄。

圖9 實時視頻傳輸及保存流程圖

自動報警功能基于定位追蹤算法，主機處理基站發送過來的位置數據組，一旦發現數據異常，則蜂鳴報警，工作人員查看緝毒犬是否遠離工作范圍，若數據再次正常，則報警停止。

4 測試與分析

在完成對系統的軟硬件設計后需要進行實地檢測該設備系統能否完成預期的功能，以及測試設備在連續不間斷的工作狀態下運行的穩定性。如圖10所示首先確定兩個次基站的位置，并將其布置在平穩的地面，且保證周圍30 cm內無障礙物遮擋，兩個基站分別由移動電源供電。然后把主基站插在帶有攝像頭的智能車上，將智能車放置在固定滑軌上，將攜帶標簽的緝毒犬牽入場內待定。

圖10 系統測試模擬圖

樹莓派創建軟連接，編寫腳本實現開機自啟動中間件和wifi，進入樹莓派系統并自啟動中間件后，會有一個網絡初始化，通過函數open_server_socket()實現，打開服務器的socket并且返回新的文件描述符，小車作為客戶端，樹莓派上的中間件為服務端，中間件通過函數listen_and_-connect_to_client()實現了對客戶端、端口號的監聽。接收到新的連接并且與新連接相連，返回新客戶端連接的socket文件描述符。中間件為實現與小車進行數據傳送，設置了兩個線程：接收數據線程與發送數據線程。service_client()函數主要實現創建并啟動發送數據線程，以及向客戶端發送傳感器數據，創建和啟動接收數據線程來接收來自客戶端的控制小車的命令，當線程結束后再斷開與客戶端的連接，實現樹莓派對智能車的指令控制。

設備運行過程中緝毒犬的運動頻率很快，且活動范圍長時間處在較小的一個變化區間內，UWB定位模組的定位精度為30 cm，數據變化過于頻繁，導致攝像機拍攝的畫面不穩。為了使視頻畫面更加穩定可以將原本的取值求平均周期延長至8組，并且后期對核心算法進行了調整，提高了智能車運動的靈敏度：參照移動目標定位追蹤算法，將原本的突變點閾值M、N調整為M1、N1，確保緝毒犬運動軌跡判斷準確的同時提高了拍攝畫面的穩定性。

算法調整前后所得到的距離數據組變化曲線圖如圖11所示，圖11(a)中為修改平均周期之前，圖11(b)為修改平均周期之后的曲線，可以看出圖11(b)中的曲線較圖11(a)更加能篩選出有效的突變點。

圖11 定位追蹤數據穩定度分析

對設備進行實際場地布置測試，通過替換移動電源保證設備正常供電并運行，連續運行一周后，系統穩定可靠，沒有出現卡機或崩潰現象，硬件可靠性得到驗證，測試反饋總體效果良好。

5 結束語

該設計的緝毒犬訓練監督輔助系統利用樹莓派D-DWM-PG1.7型UWB定位模組，基于移動目標的定位追蹤算法實現了在多種應用環境中定位追蹤錄像緝毒犬工作。在設備開始工作后，緝毒犬攜帶標簽進入基站覆蓋范圍內可對緝毒犬進行自動追蹤錄像并且能夠在緝毒犬遠離工作區域時及時發出警報，準確率達99%以上。

后期選擇性能更好，定位精度更高的UWB定位模組，可以實現更精確的定位追蹤。系統后續可添加更多的功能，通過目標檢測算法檢測視頻里緝毒犬和行李的位置，輔助顏色識別技術，系統智能判斷緝毒犬是否識別出攜帶毒品的行李物品，提高訓練效率，節省人力，培訓出更多優秀的緝毒犬，提高緝毒工作的效率。

綜上該設計可以實現有效定位跟蹤錄像緝毒犬訓練過程，提高訓練效率，在緝毒工作領域具有較大應用前景。