基于UWB定位的缉毒犬训练跟踪录像小车设计

2022-02-16马兴录唐亚男

计算机测量与控制 2022年1期

马兴录，唐亚男

(青岛科技大学信息科学技术学院，山东青岛 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)更加能筛选出有效的突变点。