基于ROS的自主跟随智能小车的设计

周旭华，赖理智，钟承志

（河源职业技术学院，广东 河源 517000）

随着科技技术的发展，代替人类工作的各种智能化小车出现在工厂、车间以及家庭服务等场合。目前，智能小车的跟随技术主要有超声波、红外、摄像头传感器、UWB技术等，这些定位和测距技术都较为成熟［1-3］。目前自主跟随小车智能化需求不断提高，且系统的可移植性要求也越来越强，针对这一需求，2010年Willow Garage公司发布了开源机器人操作系统ROS，该系统能够大大缩短开发的时间，且极大提高了智能小车软件上的移植性和复用性，同时也提高了软件的兼容性。本文设计了以树莓派为上位机，Arduino UNO为下位机，ROS为核心通信架构的目标自主跟随小车［4］。

1 智能小车整体设计

以树莓派为上位机主控制板，Arduino UNO板为下位机辅控制板，搭建上位机和下位机的通信架构，激光雷达在未知的环境中获得自身位置信息、目标物体位置信息，上位机获取处理的目标位置信息，发送命令到Arduino UNO板，Arduino通过接收树莓派传来的信号来给予电机驱动板信号，控制小车实现自动跟随目标物体。其系统结构框图如图1所示。

图1 系统结构框图

2 硬件设计

2.1 硬件结构设计

小车主要由树莓派3b、Rplidar-A1激光雷达、Arduino UNO控制板、微型直流减速电机、直流电机驱动模块、降压模块、电池等部件组成，其结构设计如图2所示。

图2 小车结构设计图

2.2 下位机硬件设计

2.2.1 电机驱动模块

电机驱动模块接收到Arduino UNO发送过来的指令可以单独控制左右电机的速度，电机驱动模块的IN1、IN2、IN3、IN4分别与Arduino UNO的7、8、9、10号端口连接，控制电机正反转、制动，+5V端口可接5V或3.3V为信号端提供电 源，ENA1、ENA2为电机使能端，连接PWM端子6、5来调节电机速度。ENA1控制左边的电机，ENA控制右边的电机。其NO与电机驱动模块、电机的连接如图3所示。

图3 ArduinoUNO与电机驱动模块、电机的连接电路图

2.2.2 电动机编码电路

电动机运用编码器，可以由PID部分控制轮速，让小车在自动跟随目标物体运动时能够更加平稳地行驶跟随，图4为Arduino UNO与电机编码器连接电路图。

图4 Arduino UNO与电机编码器连接图

2.3 上位机硬件设计

2.3.1 传感器电路

树莓派与雷达之间的连接是通过USB配置器的，VMOTO是雷达电机的供电引脚，MOTOCTL是雷达电机的启闭引脚（高电平有效），输入PWM信号可实现电机调速，VS.0是雷达测距核心的供电信号，TX是雷达测距核心的TTL串口输出，RX是雷达测距核心的TTL串口输入。USB配置器再通过USB线与树莓派连接，实现树莓派与雷达之间的通信，如图5所示。

图5 雷达与树莓派的连接电路图

2.3.2 上位机与下位机通信电路

树莓派和Arduino UNO板均由同一个锂离子电池供电，树莓派的供电压为5V，为了保护树莓派不被烧坏，电源供给树莓派的电压必须通过降压模块降至5V，如图6所示。

图6 电源与电机驱动模块、UNO的连接电路图

2.3.3 整体设计电路图

激光雷达在未知的环境中获得自身位置信息、目标物体位置信息，上位机获取处理的目标位置信息，发送命令到Arduino UNO板，Arduino通过接收树莓派传来的信号来给予电机驱动板信号，控制小车实现自动跟随目标物体。整体设计电路如图7所示。

图7 整体设计电路图

3 软件设计

搭建ROS小车软件由Unbuntu 16.04平台、ROS系统平台、ros_arduino_bridge通信程序、电机控制程序、主程序、rplidar_follower跟随程序等部分构成。软件系统设计如图8所示。

图8 软件设计

3.1 下位机的搭建

在Arduino上完成驱动电机的PID控制，以及与上位机通信程序的实现。然后在上位机安装ROS ArduinoBridge库，再配置ros_arduino_python节点，实现下位机与上位机之间的通信搭建，这个ROS功能包集用于实现ROS与Arduino的通信，负责接收ROS系统发送的速度控制话题/cmd_vel，并将话题信息进行解析，转化为底盘电机的速度信息。同时该功能包还负责接收Arduino上发送的编码器信息，并将相关信息转化为里程计信息，并以/odom话题的形式发送出来供ROS使用。

3.2 跟随程序的设计

基于ROS系统，用激光雷达扫描并公布最近物体的位置，按照距离排序以检查较近点是否真实，返回控制信号，读取角度的距离，然后获取到目标物体，最终输出角速度和线速度给小车，追踪目标物体。图9为程序设计流程图。

图9 程序设计流程图

3.2.1 跟随功能程序分析

首先，跟随功能包节点laser_follower.launch文件加载启动rplidar.launch和follower.launch和LaserTracker.launch三个launch文件，其中rplidar.launch文件主要用于启动激光雷达传感器；LaserTracker.launch文件主要进行当前目标离小车的最近距离测量；follower.launch此文件各参数可以改变跟随目标的距离，设置小车的最大速度（线速度、角速度），且通过加载PID_param.yaml，可以调节pid让小车行驶得更平稳快速达到目标速度，跟随目标物体。

3.2.2 激光雷达扫描程序分析

利用Python语言进行编程，Python语言不需要编译，可以在ROS框架下编写后直接在Linux终端执行即可［5-7］，智能小车通过激光雷达扫描到最近的物体，并公布就近物体的位置信息，按照距离去检查较近点目标是否真实，如果真实，这个点将和最后一次扫描点进行比较，从最近距离检查所有距离的测量，检查是否有相似距离扫描，进行分析处理（至少找到一个点的合理距离，至少一点接近），计算对象位置的角度，来确定目标物体位置信息，如果没有找到，将会重新扫描直至找到目标物体，图10为智能小车激光雷达扫描流程图。部分程序如下：

图10 激光雷达扫描流程图

def registerScan（self，scan_data）：#记录激光扫描并公布最近物体的位置

ranges=np.array（scan_data.ranges）#按距离排序以检查距离较近点是否真实

sortedIndices=np.argsort（ranges）

minDistanceID=None

minDistance=float（'inf'）

if（not（self.lastScan is None））：#如果我们已经有了最后一次扫描：

for i in sortedIndices：#从最近的距离开始检查所有距离测量

tempMinDistance=ranges［i］

#检查是否有相似距离的扫描，在最后一次扫描中，就不会有索引超出范围windowIndex=np.clip（［i-self.winSize，i+self.winSize+1］，0，len（self.lastScan））

window=self.lastScan［windowIndex［0］：windowIndex［1］］

with np.errstate（invalid='ignore'）：

#检查窗口中的任何扫描（在上次扫描中）是否与当前扫描的距离足够近

if（np.any（abs（window-tempMinDistance）＜=self.deltaDist））：

#this will also be false for all tempMinDistance=NaN or inf

minDistanceID=i

minDistance=ranges［minDistanceID］

break#at least one point was equally close

self.lastScan=ranges

if（minDistance＞scan_data.range_max）：#没有找到一个合理的目标

rospy.logwarn（'laser no object found'）#发布警告说我们没有找到任何东西

self.infoPublisher.publish（StringMsg（'laser：nothing found'））

else：#计算对象位置的角度

minDistanceAngle=scan_data.angle_min+minDistanceID*scan_data.angle_increment

self.positionPublisher.publish（PositionMsg（minDistanceAngle，42，minDistance））

3.2.3 智能下车驱动程序

同样，该程序用python编写，小车对追踪对象的当前位置（角度、距离），进行处理分析，调用pid控制器进行更新速度，这些速度限制在上面指定最大速度，返回控制信息，进行目标物体跟随。部分程序分析如下：

#PID parameters first is angular，dist

targetDist=rospy.get_param（'～targetDist'）

PID_param=rospy.get_param（'～PID_controller'）

self.PID_controller=simplePID（［0，targetDist］，PID_param［'P'］，PID_param［'I'］，PID_param［'D'］）#第一个参数是角度目标，第二个参数是目标距离

rospy.on_shutdown（self.controllerLoss）#当进程被Ctrl+C终止时调用此方法

def trackerInfoCallback（self，info）：#目前不处理来自对象跟踪器的任何信息

rospy.logwarn（info.data）

def positionUpdateCallback（self，position）：#每当接新数据。然后它将更新电机

#if（not（self.active））：#如果不活动，将立即返回，不做任何事情

angleX=position.angleX

distance=position.distance

rospy.loginfo（'Angle：{}，Distance：{}，'.format（angleX，distance））

#调用PID控制器来更新它并获得新的速度

［uncliped_ang_speed，uncliped_lin_speed］=self.PID_controller.update（［angleX，distance］）

#将这些速度限制在小于上面指定的最大速度

angularSpeed =np.clip （-uncliped_ang_speed，-self.max_speed，self.max_speed）

linearSpeed =np.clip （-uncliped_lin_speed，-self.max_speed，self.max_speed）

4 测试结果

启动小车底盘、小车雷达后，在小车雷达前方站一个人，距离为根据程序设置的距离，开启小车自动跟随功能，小车可跟随前方的行人向前行驶，行人在改变方向行驶，小车也可以跟随行人改变行驶方向跟随行驶，所以本设计符合设计要求。但人不能过快地移动，一旦超出程序设置的距离，小车将选择跟随在其范围内的其他物体或人。测试图如图11所示。

图11 测试图