基于UWB 的智能跟随小车的系统设计

赵剑川，陈榕福，兰小海，何 威，张 宇，吴志威

(广东机电职业技术学院电子与通信学院,广东 广州 510550)

1 引言

目前，由于智能跟随技术在工农业生产、科学技术、智慧生活等各方面都有着广泛的应用需求[1]，故它已经成为了定位领域的一个研究热点。在各种定位方案中，UWB（Ultra-Wide Band）定位技术与其它定位技术相比具有精度高、功耗低、抗干扰能力强等显著特点[2,3]，使得它在精确定位领域得到了广泛的应用[4,5]。在短距离的智能目标跟随中使用UWB 技术，可以实现对目标进行精确的定位和跟踪。因此，开发基于UWB技术的智能跟随小车，对于研究UWB 跟随技术具有重要意义。

2 系统组成及其原理

智能跟随小车的控制系统如图1 所示。由图可见，整机控制系统由电源1、电源2、基站模块1、基站模块2、微处理器、电机驱动器组成。其中，电源1 为微处理器、基站模块1 和基站模块2 供电；电源2 单独为电机供电。UWB 标签接在被跟踪的移动物体上，基站1 和基站2 各自与UWB 标签进行消息通信并计算出各自与UWB 标签的距离后，基站1 把计算所得距离通过无线方式发送给基站2，再由基站2 通过串口转发给微处理器，基站2 把自己计算所得距离通过串口发送给微处理器。微处理器根据基站1 和基站2 所测得的距离解算出标签方位，再根据标签方位通过PWM 脉冲驱动电机1 和电机2，从而达到跟踪标签的目的。

图1 智能跟随小车的系统框图

2.1 UWB 模块设计

基站1、基站2 和标签电路都是一样，只是因为内部程序不同导致实现的功能也不同，所以名称不同，但本质上都属于UWB 模块，该模块的电路如图2 所示。UWB 模块主要包含Decawave 公司生产的DW1000射频芯片和STM32F4 单片机[6]，两者通过SPI 接口进行连接。其中DW1000 用于发出和接收无线信号，STM32用于对收发信号进行处理并获知两片相互通信的DW1000 之间的距离。

图2 UWB 模块原理图

2.2 电机驱动单元设计

本系统的电机驱动采用东芝公司的TB6612 FNG 直流电机驱动芯片来驱动，该芯片具有大电流MOSFET-H 桥结构，双通道电路输出，可以同时驱动2 个电机。TB6612 FNG 芯片有4 种电机控制模式：正转、反转、制动、停止，其PWM 频率可达100KHz。本系统的电机驱动设计原理图如图3 所示：

图3 电机驱动电路图

3 跟踪算法的设计

3.1 跟踪目标的定位

实要想对目标进行准确跟踪，首先要准确计算目标的方位。本设计通过建立参考坐标系后再确定被跟踪目标方位的方式来进行方位解算，其过程如下:

1、建立参考坐标系

如图4所示，基站1和基站2放置于小车的车头，他们之间的距离为lcm，标签挂在跟踪目标上。接下来，以基站1和基站2所在直线为x轴，基站1和基站2的中点o为原点，建立如图4所示的直角坐标系。

图4 整机坐标图

2、方位解算

假设基站1、基站2 和标签的坐标分别为(x1,y1)、(x2,y2)和(x,y)，基站1 和基站2 测得自身与跟踪目标的距离分别是d1和d2，则由图4 可知，这些物理量之间具有如下关系

求解以上方程组，可得到方程组的关于被跟踪目标的(x,y)坐标有两个解，且两个解关于x轴是对称的。我们假设目标一直在车头的前方，则可知y＞0 的那个解是目标的实际位置。将基站1 和基站2 的坐标值代入式(1)和(2)所组成的方程组，可得跟踪目标位置坐标如下：

另有以下结论:

（1）当x＜0 时，目标在车的左侧；

（2）当x＞0 时，目标在车的右侧；

（3）当x=0 时，目标在车的正前方；

（4）目标与车的偏离角为arctg(x/y)；

（5）目标与车的垂直距离为y。

3.2 跟踪目标定位精度的优化

在实际的定位测量中，由于各种因素的影响，DW1000 测得的距离值存在着较大的偏差。为了提高测量精度，本系统在采用双向测距的到达时间差进行定位[7]过程中，对测量到的距离值采用滑动平均值法进行滤波，滤波前后解算得到的位置分布图如图5 所示。其中图5(a)是没有使用滑动平均值滤波的情况，图5(b)是使用了滑动平均值滤波的情况。由图可以看到，滤波前y在-94.5cm～-99.5cm 之间变化约4cm 时，x的变化从-15cm 到10cm，整个变化达到25cm，而滤波后，y在-96.2cm 到-97.8 之间变化约1.6cm 时，x的变化只有6cm左右。可以发现，采用滑动平均值法后定位精度得到了明显的提高。

图5 使用滑动平均值法后标签位置分布对比

进一步研究发现，两个基站之间的距离大小对定位精度也有影响。图6 的(a)图和(b)图分别给出了基站间距为50cm 和基站间距为20cm 时跟踪目标的位置分布图，对比这两个图可以发现，基站间的距离对定位精度有着重要的影响。所以在实际应用中，需要对该距离进行不断的调整优化，在本系统中，经过调整基站间距设置为40cm 时定位效果相对较佳。

图6 基站间距对标签位置的影响

3.3 目标跟踪控制算法

在确定跟踪对象的位置后，接下来采用位置式PID 算法进行目标跟踪控制。其差分方程如下式所示[8]

其中，ek为k时刻设置值与实际值的差，uk为计算得到的控制量。

对于本设计，需要考虑两个问题：

(1) 距离的控制，通过控制小车的速度来控制小车与目标的距离。

假设保持的距离设置值为d，则标签实际距离和设定的保持距离的偏差为(d-y)，把(d-y)代入公式(5)计算得到udk，用udk调整小车前进速度，当udk较大时，加快小车速度，当udk较小时，减小小车速度，当udk＜0时，小车停止。

(2) 偏离角的控制，通过控制小车的旋转角度来控制偏离角。

偏离角大小可以用x/y来衡量，把x/y代入公式(5)得到uak，用uak控制旋转角度，当uak较大时，大幅旋转；当uak较小时，小幅旋转；uak为正，右转；uak为负，左转。

以上两个目标实际上都通过对小车车轮转动的控制来实现，具体可以采用融合控制的方法。该方法是先分别计算得到距离控制量udk和偏离角控制量uak，再把两个控制量相加得到总输出控制量，即

以上两式中，kLu用于作为控制左轮的PWM信号的脉宽参数，kRu用于作为控制右轮的PWM信号的脉宽参数。对小车进行融合控制后，其灵敏度和平滑度改善非常明显。

4 结论

本文设计了一种智能跟随小车控制系统，对系统的跟随控制进行了详细的研究。研究结果表明，使用滑动平均值滤波法能够减少UWB 测量过程中的噪声干扰，有效提高对跟踪目标的定位精度。在得到相对精准的测量数据后，采用距离与偏离角融合的PID 控制算法可以提高跟随过程的平稳性和灵敏度。实验研究还表明，定位基站的间距太小会降低方位解算的精度，实际应用中应根据具体场景不断优化选择定位基站的间距。