基于超宽带定位的自动巡航小车设计

付仁杰，杨亚莉

(上海工程技术大学 机械与汽车工程学院，上海201620)

0 引 言

随着机器人技术和自动化技术的发展，具有循迹功能的自动导引运输车在工业生产过程中得到了大量的应用，其中应用最为广泛的有光电式循迹小车以及电磁式循迹小车。这2种方式的循迹小车在实际运用中部署都相对复杂。光电式循迹小车需要预先在地面上印刷黑色引导线，而电磁式循迹小车需要在地下铺设磁针，一旦轨迹设定好就不能变动，因此应用的灵活性较差。曾贵苓［1］等利用定位算法获取小车实时位置，采用传感器实现小车的传感单元，实现小车各种模式下的智能搬运工作；陈雷［2］等对RFID定位系统以及车间RSSI算法模型进行了系统研究，开发车间定位原型系统，对实验测试结果分析较好；王世峰［3］等以AVR单片机为控制枢纽，利用硬件及电路设计对小车运动控制良好。

UWB是一种短距离无线通信技术，安全性能高，定位精度准确，被广泛应用于精确的室内定位［4、5］。UWB定位系统采用三维空间坐标来表示具体的空间位置，因此在UWB有效定位范围内的空间点都可以用坐标来表示［6］。同理，预先铺设的小车行驶路径也可用一系列的坐标点来表示，通过修改坐标点的集合就可以实现小车行驶路径的重新设定［7］。

定位技术随着科技的发展，广泛应用在汽车、无人机以及先进设备领域当中。将GPS与通讯技术相结合，能够实现实时监测、报警、调度等功能［8］。

本设计中引入了位置坐标信息，采用一系列的坐标点构成的轨迹作为小车的引导路径，运用UWB定位技术来指引小车按照指定路径行驶。

1 系统硬件设计

1.1 总体设计方案

系统主要由定位基站以及自动巡航小车主体两个部分组成。定位基站由3个UWB定位基站构成，能够实现2D空间上的定位。自动巡航小车以车模为载体，主要功能模块包括:电源模块、电机驱动模块、UWB定位标签、姿态传感器模块、避障模块、树莓派3B模块。其中电源模块以及电机控制模块同属于扩展板。树莓派3B模块是小车的控制核心，通过UWB标签以及姿态传感器获得小车的当前的位置信息以及姿态信息；根据给定的轨迹信息控制小车上电机的转速，从而实现对小车的行驶轨迹的控制，最终实现小车根据指定轨迹巡航的功能。自动巡航小车系统的结构框图如图1所示。图1中MPU9250为姿态传感器，可以测量小车转角、加速度、以及角速度等；HY-SRF05是超声波传感器模块，系统中用作避障模块，实现小车前方障碍物的检测。

图1 自动巡航小车系统框图Fig.1 Block diagram of automatic cruising car system

1.2 电源模块

小车采用12V的锂电池供电。本设计中选用SY8008B稳压芯片将12 V的输入电压降到5 V，供树莓派模块使用。SY8008B稳压芯片能够提供高达1.2 A的输出电流，能够满足树莓派模块的供电要求。直流电机直接采用12 V输入电压供电。电源模块的电路设计如图2所示。

图2 电源模块电路图Fig.2 Circuit diagram of power module

1.3 电机驱动模块

为增强小车整体的驱动能力，本设计中选用4个直流电机来驱动小车；另一方面，为降低硬件控制的成本，小车同侧的两个直流电机采用同一路PWM来控制。综合上述因素，本设计选用L289电机驱动芯片，该芯片可提供两路电机驱动输出，通过对使能引脚以及PWM输入引脚的控制可以实现电机的启停以及转速的控制。L289芯片的输入引脚与树莓派GPIO相连，树莓派3B模块内置有PWM发生器，最多可同时提供两路PWM输出，直流电机驱动电路图如图3所示。

图3 直流电机驱动电路图Fig.3 Dc motor drive circuit diagram

2 系统软件设计

2.1 软件的总体设计

软件设计的主要任务包括:

（1）通过UART接口从UWB标签读取其到UWB各基站之间的距离信息，并根据距离信息计算出小车的位置；

（2）通过I2C接口从MPU9250模块中读取小车的车身方位角、车速等信息；

（3）实现自动加载路径的功能，并结合PID控制算法实现自动巡航的功能；

（4）通过超声波测距模块实现避障功能；

（5）完成软件的整体设计。

本设计中存在着大量的延时操作，因此不适合在主线程中同时处理上述操作。为保证整个系统的实时性和稳定性，软件设计采用多线程方式，本设计中共创建了两个子线程分别完成各自的任务。主线程负责完成小车定位、路径加载以及紧急控制等任务，子线程1主要完成PID控制算法以及小车的控制等任务，子线程2负责时刻检测小车前方障碍物的任务。软件的整体设计流程图如图4所示。

图4 软件整体设计流程图Fig.4 Flow chart of overall software design

2.2 巡航功能设计

2.2.1 UWB定位

UWB定位通常采用3边测量法，原理如图5所示，以3个节点A、B、C为圆心作圆，坐标分别为（Xa，Ya），（Xb，Yb），（Xc，Yc），这3个圆周相交于一点D，交点D即为移动节点，A、B、C即为参考节点，A、B、C与交点D的距离分别为da、db、dc。

图5 三边定位法原理图Fig.5 Schematic diagram of three-side positioning method

由上述几何关系可以得出公式（1）:

采用最小二乘法求解上述方程可求得D点坐标公式（2）:

2.2.2 PID控制算法

本设计中小车以路标点为指引向前行驶，但行驶的过程中存在不确定的因素使小车偏离原定方向，因此需要引入闭环控制来时刻修正小车的行驶方向。PID控制器是自动控制系统中应用最为广泛的闭环控制器，而在计算机控制系统中通常采用数字PID控制器。数字型PID控制器的算法分为位置式PID控制算法和增量式PID控制算法。

本设计首先通过UWB定位标签获取小车的位置，再结合目标位置计算得到小车的目标位置角，以目标位置角作为输入，以位置传感器测得的位置角作为反馈数据，采用增量式PID控制算法控制小车两侧的PWM输出值，使小车能够跟随目标点行驶。整个闭环控制系统的系统框图如图6所示。

图6 PID控制系统框图Fig.6 PID control system block diagram

图中r（k）为输入的目标位置角，c（k）为当前测得的位置角，u（k）经过PID环节得出的小车两侧的PWM修正值。PID算法公式（3）为:

其中，e（k）为第k个采样时刻的误差角；T为采样周期；kp为比例环节系数；Ti为积分环节系数；Td为微分环节系数。

2.2.3 巡航功能的实现

本设计中引入位置坐标信息，采用坐标信息表示实际的物理位置，采用一系列的位置坐标构成的轨迹作为小车的指定行驶路径，通过UWB定位技术来获取小车的位置信息。巡航功能的实现过程:

（1）获取小车的当前位置并找到距离最近的路径点作为下一个目标点；

（2）计算小车车身的目标方位角，计算公式（4）为:

其中，θ是目标方位角；xt、yt为目标位置坐标；xc、yc为当前位置的坐标；

（3）获取当前小车车身方位角，并通过PID算法控制小车前进；

（4）判断是否到达目标点，若到达则加载下一个路径点并重复执行（2）（3）过程，否则重复执行（4）。具体的流程如图4中（a）主线程和（b）子线程1所示。

2.3 避障功能设计

由于不能保证小车在巡航过程中其行进路径上不存在障碍物，设计中需添加避障功能来保证小车的行驶安全。本设计中采用超声波传感器来进行障碍物检测，小车在运行过程中通过超声波传感器时刻检测小车前方物体，当小车前方安全距离内出现障碍物，避障功能模块就会向主控模块发出避障提示，主控模块响应避障操作。避障功能的流程设计如图4中（c）子线程2。

3 实验测试

本实验主要对小车的定轨巡航功能进行测试，主要测试了“O”型和“S”型2种指定轨迹下小车的运行情况。首先，通过小车上的位置记录程序记录小车所经过空间点坐标；其次，将上述空间点导入到二维平面地图上；最后，将所有空间点连接绘制成轨迹图，如图7和图8所示。

图7 “O”型轨迹测试结果图Fig.7 Test results of the＂O＂trajectory

图8 “S”型轨迹测试结果图Fig.8 Test results of the＂S＂trajectory

图7和图8中虚线表示目标轨迹，实线表示实际运行轨迹。对比目标轨迹和实际运行轨迹可以看出所设计的小车能够实现定轨巡航的功能，但局部有较大的波动出现，其产生的主要原因有两点:

（1）UWB定位时引入噪声干扰使得所获得的位置信息存在着波动，因此需要对UWB的定位算法进行优化；

（2）PID控制算法的比例、积分、微分环节系数选择不当，可考虑用模糊PID控制算法来取代增量式PID控制算法。

4 结束语

本文针对传统循迹小车系统在部署和应用中存在的不足，提出并设计了一种能够按照指定路径自动巡航的小车系统。设计中引入了位置坐标信息，采用一系列位置坐标构成的轨迹作为小车的引导路径，通过UWB定位技术来引导小车追踪指定路径。本设计采用树莓派3B模块做为主控单元，运用增量式PID控制算法来控制小车追踪路径，通过MPU9250模块获取小车的速度、方向等信息，通过HY-SRF05超声波传感器模块实现避障功能。测试结果表明，所设计小车系统能够实现定轨巡航的功能，通过各个模块和算法控制，可以实现设计的目标，该小车系统具有一定实际应用价值，能够自动准确灵活的按指定路径达到指定位置。