基于UWB技术的小车自动跟随系统的研究

张家僮，王洪源

(沈阳理工大学 信息科学与工程学院，沈阳110159)

随着当今社会人工智能和物联网的快速发展，自动跟随定位技术已经成为当前科技的研究热点，并且越来越多的普及到人们的生活当中。自动跟随定位技术的出现成为影响当今通信时代的一个关键因素。在过去的十几年里，人们已经感受到无线定位技术为生活带来的巨大便利，因此无论是在工作还是日常生活中，人们对自动跟随定位技术的需求也是越来越多。虽然当前室外定位技术的发展需求可以被4大全球定位系统(GPS、北斗系统、GLONASS系统、伽利略卫星导航系统)满足，但在GPS信号极其微弱甚至接收不到信号时，人们将无法从卫星广播中提取获得位置服务和有效导航数据，所以人们专门研究设计了应用于室内的定位技术。现今，这些先进的室内定位技术中，非接触式近距离无线通信技术定位已经成为热点。但这些无线定位技术技术都会存相应的短板。例如：IEEE.802.11a虽然传输距离长，速度快，适用于公司企业中距离较大的无线网络，但是它的成本较高，且功耗比较大；蓝牙技术同样也存在成本高、传输距离短、传输速率低、技术还不够成熟等缺点，因此应用并不广泛；Home RF[1]虽然代替了需要铺设昂贵传输线的有线家庭网络，成为网络中的设备，但是Home RF占据了与蓝牙相同的2.4G频率段，并且在功能上过于局限家庭的应用。因此实用性并不强。并且这些室内的定位技术，在实际环境的应用下，这些利用接受到的信号强度进行定位的技术容易收到干扰，且测量的数据误差较大，测量精度不能满足人们日常生活的需要。而UWB(Ultra Wide Band)超宽带技术虽然属于无线通信技术，但通信速率极高[2-3]。UWB超宽带技术所需要的设备价格并不昂贵，因此设计一个系统包含UWB定位模块、嵌入式驱动模块、电机驱动模块、电源模块以及汽车模组，其中UWB定位模块是搭建UWB超宽带自动跟随定位系统的重要组成部分，由一个移动标签和两个相对固定基站组成，是目前适用性最高的室内跟随定位系统。

1 UWB自动跟随定位技术原理

1.1 跟随原理

小车的自动跟随功能是基于三个无线连接的嵌入式定位系统开发而成。三个无线连接的嵌入式定位系统是指驱动两个基站和一个标签组成的UWB定位系统，其实验所需的标签到两个基站的位置数据测定采用飞行时间TOF(Time of Flight)算法。通过数学推导公式将两个定位基站与移动标签的距离信息转变为标签在小车坐标系中的相对坐标信息，经过其相关数据进行融合后通过无线通信方式传输到车辆控制系统，最终实现小车跟随标签移动。

1.2 定位算法

在自动跟随系统中，测量标签到基站的距离是最重要的，是系统的构架基础。因此本系统测距使用TOF飞行时间的测距方法[4]。TOF测距是基于双向测距的方法，经由信号在标签和基站之间往返的飞行时间来测量节点间的距离，要求移动标签和定位基站必须始终同步，并且定位基站接收信号的传输时间要在一定时间内。TOF测距原理图如图1所示。设备A主动发送数据，设备B接收到之后记录接受时间标记；延时Treply之后，设备B发送数据，同时记录发送时间标记；设备A接受数据，同时记录接收时间标记。所以可以得到两个时间差Treply。

图1 TOF测距原理图

最后得到无线信号的飞行时间Tprop如下。

Tprop=(Tround-Treply)/2

(1)

则设备A、B两点的距离为

d=c×Tprop

(2)

式中c是电磁波传播速度。

两个差值时间都是基于本地的时钟计算得到的，本地时钟误差可以抵消，但不同设备之间会存在微小的时钟偏移，假设设备A和B的时钟分别为tA和tg，因此得到的飞行时间会随着Treply的增加而增加，测距误差的方程如下。

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

(3)

在得到移动标签到两个定位基站之间距离值后，设令基站A为相对坐标原点，移动标签的相对坐标为(X，Y)。

当标签的相对位置在两个定位基站中，如图2所示。

图2 标签在两基站中间示意图

结合数学推导，得到移动标签位置。

X2+Y2=P2

(4)

(S-X)2+Y2=Q2

(5)

式中：X、Y代表标签到纵、横轴的距离；P、Q表示标签到基站A、B的距离；S表示两基站的距离。

当标签的相对位置在基站A的左侧，如图3所示。

图3 标签在基站A左侧示意图

结合数学推导，得到移标签位置。

X2+Y2=P2

(6)

(S+X)2+Y2=Q2

(7)

当标签的相对位置在基站B的右侧，如图4所示。

图4 标签在基站B右侧基站示意图

结合数学推导，得到移动标签位置。

X2+Y2=P2

(8)

(X-S)2+Y2=Q2

(9)

1.3 误差分析

根据DWM1000参考官方提供的数据手册[5]，判断系统的误差来源是DWM1000的天线误差和多径误差。天线误差的产生是由芯片内部决定的一个固定值，而此值会影响到实际距离的测量；多径误差的产生是源于UWB信号接收端的机制，一般接收端接收达到设定阈值的首个信号并将其作为信号的到达时间，但在实际环境中由于障碍物导致的信号能量损失，可能会使得首个到达信号被错误识别。

因此通过结合实际的系统测试，发现对该部分的校准可以结合其芯片自带的存储功能，仅需要初次使用时校准；并将基站安装在远离障碍物的地方，使得标签在室内大部分活动范围内为视距条件，就可以解决这两种误差，使误差在可以控制的范围内。

UWB自动跟随系统中，整体设计由移动标签、定位基站、主控制器和电机驱动模块构成，如图5所示。移动标签由室内中的测试人员携带以及小车对其定位跟随；定位基站被布置在小车的两侧。测试过程中，通过获取标签和定位基站之间往返的飞行时间来得到标签的位置信息，结合数学推导得到小车与标签的位置信息。通过主控制器上传到小车的电机模块，实现室内中小车对移动标签的自动跟随。

图5 系统结构图

2 UWB自动跟随测试系统设计

2.1 移动标签与定位基站硬件设计

系统中移动标签主要包括嵌入式驱动模块、定位模块、电机驱动模块以及电源模块。定位基站在移动标签的基础上多加了一个接口模块。系统的主控芯片采用意法半导体公司生产的STM32[6]芯片，该款MCU基于ARM Cortex-M3内核，主频32MHz，包含4个通用的16位定时器，多达3个SPI接口，电源工作电压为2.0～3.6V；该芯片拥有高性能、低成本和低功耗等优点。

定位模块中采用Deca Wave公司推出的DWM 1000芯片[7]来实现测距功能。该模块是一款基于IEEE 802.15.4-2011通信标准的无线信号收发模块。DWM 1000芯片定位控制器开启定位功能命令。如果接受到主控制器的定位命令，会分别向各基站发送测距请求，且精度可达厘米级别，支持高数据数率通信，可以根据实验测试的需求采用110Kb/s、850Kb/s和6.8Mb/s的数据传输速率。DWM 1000芯片采用相干接收技术，支持3.5～6.5GHz的6组中心频率带宽，通讯范围高达300m，抗多径衰落。

2.2 电机模块与电源模块硬件设计

电机驱动模块采用SGS公司生产的L298N芯片[8]。L298N芯片最为方便的是其输入端可以直接与STM32相联，不需要其他复杂的设计，就可以直接对电机驱动模块进行控制。

整个跟踪定位系统的电源使用12V的锂电池，由于STM32控制器使用的电压为3.3V，电机驱动模块电压为5V，所以系统需要将电源模块设计成稳压电路，将电压降到各模块需要的电压值，使其实现供电功能。

2.3 系统软件设计

小车自动跟随系统软件以双核STM32为处理器，双核协调配合完成小车的路径规划。系统软件设计主要针对标签、基站子程序进行。首先标签完成初始化后，等待主命令，并等待基站返回测距结果。实验通过TOF算法得到标签到基站的距离，然后将标签坐标发送给主控制器。采集数据伪代码如下。

getdistance()

creatpulse()//生成脉冲

time<-now()//存储当前时间

while get pulse()

do time=now()-time//获取时间差

end

time<-time / 2//获取单程时间

distance<-time * c//c为光速，distacne为测量的距离

return distance

然后开启基站接收功能，等待标签的测距命令。如成功接收到标签命令，基站会向标签发送一帧数据，记录相应的收发时间。通过数学推导可计算出小车与标签的距离。每完成一次测距，会自动清除时间的缓存信息，并等待下一次测距命令。

在距离测量算法中，由于各种因素的影响，每次测量的结果不同，这里面有包括小车的随动误差。就一次测量值而言，真实值和小车的随动误差都是未知的，所以系统需要对数据进行多次测量，通过数学推导公式取一定时间内或循环次数内的算数平均距离，作为当前的测量距离。经实验证明，该算法可以有效地减小误差，解决所测数据和真实值之间的误差问题。计算数据伪代码如下。

while getdistance()//如果获取到数据

do d1，distance2<-getdistance()//获取两个基站到标签的距离

fori<-0 to 10

sumd1 <-sumd1+d1

sumd2 <-sumd2+d2

sumd1 <-sumd1/10

sumd2 <-sumd2/10

x<-(sumd1*sumd1-sumd2*sumd2+250000)/1000//计算坐标x值

y<-sqrt(abs(sumd1*sumd1-x*x))//计算坐标y值

end

3 UWB自动跟随系统实验

3.1 实验方案设计

为验证本系统自动跟随小车的实际运动行驶状态，选择在室内区域进行小车自动跟随测试。首先将小车放置起点处，将实验采用的两个UWB定位基站分别放置于小车的两端，将可移动的标签任意放置在实验的规定区域内。系统设定小车拥有四种行驶情况：前进、左转、右转和停止。标签无论如何移动都可与两个定位基站形成稳固的三角形位置，如图6所示。

图6 标签基站位置摆放图

实验过程中，通过不断更改移动标签在规定实验场地中的位置，然后通过TOF测距算法与数学推导来确定移动标签每一次移动的相对位置坐标；再通过检测、判断移动标签与小车的的相对位置与距离，来实现小车对于移动标签的自动跟随功能，实现小车的自主运动(前进、左转、右转和停止)。实验流程如图7所示。

图7 系统实验流程图

3.2 实验测试数据及结果

实验中，小车车身的长度是50cm，因此默认两个定位基站的距离为50cm，并且由小车携带两个定位基站进行实验。在误差允许的情况下，默认移动标签的中心位置为25cm±15cm范围内。在这个设定的区间范围内，系统默认小车的行驶方向没有偏离。实验设定小车左侧携带的基站A为小车坐标系中的中点，基站A和基站B的连线为小车坐标系中的横轴，并且过原点与纵轴垂直，小车车向设为该小车坐标系的纵轴。本实验设计通过判断标签到纵轴的距离与标签到横轴的距离的数量关系来判定移动标签相对于小车的位置，进而控制小车的行驶状态，并通过取八次数据的平均值来使测量数据接近于真实值，减少实验误差。测距数据如表1所示。

实验预设当Y≥1m时，小车的行驶状态为跟随移动标签向前行驶；当Y<1m时，小车的行驶状态为停止行使(保持停留原地)。当X>40cm时，小车的行驶状态为跟随移动标签向右行驶；当X<10cm时，小车的行驶状态为跟随移动标签向左行驶。当Y<1m，但X>40cm或X<30cm时，小车的行驶状态为在原地旋转。实验数据如表2所示。

表1 TOF测距离结果

表2 实验测试结果

4 结论

通过结合TOF定位算法和数学公式推导来实现移动标签定位功能，该算法可以有效地减小误差，使处理后的结果能达到预期的实验要求。在较长的一段时间下，小车的行驶状态良好，并没有出现随意更改行驶方向等问题，一直都保持跟随移动标签的位置进行运动。