基于ROS的自主跟隨智能小車的設(shè)計

周旭華，賴理智，鐘承志

（河源職業(yè)技術(shù)學院，廣東 河源 517000）

隨著科技技術(shù)的發(fā)展，代替人類工作的各種智能化小車出現(xiàn)在工廠、車間以及家庭服務(wù)等場合。目前，智能小車的跟隨技術(shù)主要有超聲波、紅外、攝像頭傳感器、UWB技術(shù)等，這些定位和測距技術(shù)都較為成熟［1-3］。目前自主跟隨小車智能化需求不斷提高，且系統(tǒng)的可移植性要求也越來越強，針對這一需求，2010年Willow Garage公司發(fā)布了開源機器人操作系統(tǒng)ROS，該系統(tǒng)能夠大大縮短開發(fā)的時間，且極大提高了智能小車軟件上的移植性和復(fù)用性，同時也提高了軟件的兼容性。本文設(shè)計了以樹莓派為上位機，Arduino UNO為下位機，ROS為核心通信架構(gòu)的目標自主跟隨小車［4］。

1 智能小車整體設(shè)計

以樹莓派為上位機主控制板，Arduino UNO板為下位機輔控制板，搭建上位機和下位機的通信架構(gòu)，激光雷達在未知的環(huán)境中獲得自身位置信息、目標物體位置信息，上位機獲取處理的目標位置信息，發(fā)送命令到Arduino UNO板，Arduino通過接收樹莓派傳來的信號來給予電機驅(qū)動板信號，控制小車實現(xiàn)自動跟隨目標物體。其系統(tǒng)結(jié)構(gòu)框圖如圖1所示。

圖1 系統(tǒng)結(jié)構(gòu)框圖

2 硬件設(shè)計

2.1 硬件結(jié)構(gòu)設(shè)計

小車主要由樹莓派3b、Rplidar-A1激光雷達、Arduino UNO控制板、微型直流減速電機、直流電機驅(qū)動模塊、降壓模塊、電池等部件組成，其結(jié)構(gòu)設(shè)計如圖2所示。

圖2 小車結(jié)構(gòu)設(shè)計圖

2.2 下位機硬件設(shè)計

2.2.1 電機驅(qū)動模塊

電機驅(qū)動模塊接收到Arduino UNO發(fā)送過來的指令可以單獨控制左右電機的速度，電機驅(qū)動模塊的IN1、IN2、IN3、IN4分別與Arduino UNO的7、8、9、10號端口連接，控制電機正反轉(zhuǎn)、制動，+5V端口可接5V或3.3V為信號端提供電 源，ENA1、ENA2為電機使能端，連接PWM端子6、5來調(diào)節(jié)電機速度。ENA1控制左邊的電機，ENA控制右邊的電機。其NO與電機驅(qū)動模塊、電機的連接如圖3所示。

圖3 ArduinoUNO與電機驅(qū)動模塊、電機的連接電路圖

2.2.2 電動機編碼電路

電動機運用編碼器，可以由PID部分控制輪速，讓小車在自動跟隨目標物體運動時能夠更加平穩(wěn)地行駛跟隨，圖4為Arduino UNO與電機編碼器連接電路圖。

圖4 Arduino UNO與電機編碼器連接圖

2.3 上位機硬件設(shè)計

2.3.1 傳感器電路

樹莓派與雷達之間的連接是通過USB配置器的，VMOTO是雷達電機的供電引腳，MOTOCTL是雷達電機的啟閉引腳（高電平有效），輸入PWM信號可實現(xiàn)電機調(diào)速，VS.0是雷達測距核心的供電信號，TX是雷達測距核心的TTL串口輸出，RX是雷達測距核心的TTL串口輸入。USB配置器再通過USB線與樹莓派連接，實現(xiàn)樹莓派與雷達之間的通信，如圖5所示。

圖5 雷達與樹莓派的連接電路圖

2.3.2 上位機與下位機通信電路

樹莓派和Arduino UNO板均由同一個鋰離子電池供電，樹莓派的供電壓為5V，為了保護樹莓派不被燒壞，電源供給樹莓派的電壓必須通過降壓模塊降至5V，如圖6所示。

圖6 電源與電機驅(qū)動模塊、UNO的連接電路圖

2.3.3 整體設(shè)計電路圖

激光雷達在未知的環(huán)境中獲得自身位置信息、目標物體位置信息，上位機獲取處理的目標位置信息，發(fā)送命令到Arduino UNO板，Arduino通過接收樹莓派傳來的信號來給予電機驅(qū)動板信號，控制小車實現(xiàn)自動跟隨目標物體。整體設(shè)計電路如圖7所示。

圖7 整體設(shè)計電路圖

3 軟件設(shè)計

搭建ROS小車軟件由Unbuntu 16.04平臺、ROS系統(tǒng)平臺、ros_arduino_bridge通信程序、電機控制程序、主程序、rplidar_follower跟隨程序等部分構(gòu)成。軟件系統(tǒng)設(shè)計如圖8所示。

圖8 軟件設(shè)計

3.1 下位機的搭建

在Arduino上完成驅(qū)動電機的PID控制，以及與上位機通信程序的實現(xiàn)。然后在上位機安裝ROS ArduinoBridge庫，再配置ros_arduino_python節(jié)點，實現(xiàn)下位機與上位機之間的通信搭建，這個ROS功能包集用于實現(xiàn)ROS與Arduino的通信，負責接收ROS系統(tǒng)發(fā)送的速度控制話題/cmd_vel，并將話題信息進行解析，轉(zhuǎn)化為底盤電機的速度信息。同時該功能包還負責接收Arduino上發(fā)送的編碼器信息，并將相關(guān)信息轉(zhuǎn)化為里程計信息，并以/odom話題的形式發(fā)送出來供ROS使用。

3.2 跟隨程序的設(shè)計

基于ROS系統(tǒng)，用激光雷達掃描并公布最近物體的位置，按照距離排序以檢查較近點是否真實，返回控制信號，讀取角度的距離，然后獲取到目標物體，最終輸出角速度和線速度給小車，追蹤目標物體。圖9為程序設(shè)計流程圖。

圖9 程序設(shè)計流程圖

3.2.1 跟隨功能程序分析

首先，跟隨功能包節(jié)點laser_follower.launch文件加載啟動rplidar.launch和follower.launch和LaserTracker.launch三個launch文件，其中rplidar.launch文件主要用于啟動激光雷達傳感器；LaserTracker.launch文件主要進行當前目標離小車的最近距離測量；follower.launch此文件各參數(shù)可以改變跟隨目標的距離，設(shè)置小車的最大速度（線速度、角速度），且通過加載PID_param.yaml，可以調(diào)節(jié)pid讓小車行駛得更平穩(wěn)快速達到目標速度，跟隨目標物體。

3.2.2 激光雷達掃描程序分析

利用Python語言進行編程，Python語言不需要編譯，可以在ROS框架下編寫后直接在Linux終端執(zhí)行即可［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））：#如果我們已經(jīng)有了最后一次掃描：

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'）#發(fā)布警告說我們沒有找到任何東西

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 智能下車驅(qū)動程序

同樣，該程序用python編寫，小車對追蹤對象的當前位置（角度、距離），進行處理分析，調(diào)用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'］）#第一個參數(shù)是角度目標，第二個參數(shù)是目標距離

rospy.on_shutdown（self.controllerLoss）#當進程被Ctrl+C終止時調(diào)用此方法

def trackerInfoCallback（self，info）：#目前不處理來自對象跟蹤器的任何信息

rospy.logwarn（info.data）

def positionUpdateCallback（self，position）：#每當接新數(shù)據(jù)。然后它將更新電機

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

angleX=position.angleX

distance=position.distance

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

#調(diào)用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 測試結(jié)果

啟動小車底盤、小車雷達后，在小車雷達前方站一個人，距離為根據(jù)程序設(shè)置的距離，開啟小車自動跟隨功能，小車可跟隨前方的行人向前行駛，行人在改變方向行駛，小車也可以跟隨行人改變行駛方向跟隨行駛，所以本設(shè)計符合設(shè)計要求。但人不能過快地移動，一旦超出程序設(shè)置的距離，小車將選擇跟隨在其范圍內(nèi)的其他物體或人。測試圖如圖11所示。

圖11 測試圖