周旭華，賴?yán)碇牵姵兄?/p>

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

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

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

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

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

2 硬件設(shè)計(jì)

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

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

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

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

2.2.1 電機(jī)驅(qū)動(dòng)模塊

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

圖3 ArduinoUNO與電機(jī)驅(qū)動(dòng)模塊、電機(jī)的連接電路圖

2.2.2 電動(dòng)機(jī)編碼電路

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

圖4 Arduino UNO與電機(jī)編碼器連接圖

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

2.3.1 傳感器電路

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

圖5 雷達(dá)與樹(shù)莓派的連接電路圖

2.3.2 上位機(jī)與下位機(jī)通信電路

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

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

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

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

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

3 軟件設(shè)計(jì)

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

圖8 軟件設(shè)計(jì)

3.1 下位機(jī)的搭建

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

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

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

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

3.2.1 跟隨功能程序分析

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

3.2.2 激光雷達(dá)掃描程序分析

利用Python語(yǔ)言進(jìn)行編程，Python語(yǔ)言不需要編譯，可以在ROS框架下編寫(xiě)后直接在Linux終端執(zhí)行即可［5-7］，智能小車通過(guò)激光雷達(dá)掃描到最近的物體，并公布就近物體的位置信息，按照距離去檢查較近點(diǎn)目標(biāo)是否真實(shí)，如果真實(shí)，這個(gè)點(diǎn)將和最后一次掃描點(diǎn)進(jìn)行比較，從最近距離檢查所有距離的測(cè)量，檢查是否有相似距離掃描，進(jìn)行分析處理（至少找到一個(gè)點(diǎn)的合理距離，至少一點(diǎn)接近），計(jì)算對(duì)象位置的角度，來(lái)確定目標(biāo)物體位置信息，如果沒(méi)有找到，將會(huì)重新掃描直至找到目標(biāo)物體，圖10為智能小車激光雷達(dá)掃描流程圖。部分程序如下：

圖10 激光雷達(dá)掃描流程圖

def registerScan（self，scan_data）：#記錄激光掃描并公布最近物體的位置

ranges=np.array（scan_data.ranges）#按距離排序以檢查距離較近點(diǎn)是否真實(shí)

sortedIndices=np.argsort（ranges）

minDistanceID=None

minDistance=float（'inf'）

if（not（self.lastScan is None））：#如果我們已經(jīng)有了最后一次掃描：

for i in sortedIndices：#從最近的距離開(kāi)始檢查所有距離測(cè)量

tempMinDistance=ranges［i］

#檢查是否有相似距離的掃描，在最后一次掃描中，就不會(huì)有索引超出范圍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'）：

#檢查窗口中的任何掃描（在上次掃描中）是否與當(dāng)前掃描的距離足夠近

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）：#沒(méi)有找到一個(gè)合理的目標(biāo)

rospy.logwarn（'laser no object found'）#發(fā)布警告說(shuō)我們沒(méi)有找到任何東西

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

else：#計(jì)算對(duì)象位置的角度

minDistanceAngle=scan_data.angle_min+minDistanceID*scan_data.angle_increment

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

3.2.3 智能下車驅(qū)動(dòng)程序

同樣，該程序用python編寫(xiě)，小車對(duì)追蹤對(duì)象的當(dāng)前位置（角度、距離），進(jìn)行處理分析，調(diào)用pid控制器進(jìn)行更新速度，這些速度限制在上面指定最大速度，返回控制信息，進(jìn)行目標(biāo)物體跟隨。部分程序分析如下：

#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'］）#第一個(gè)參數(shù)是角度目標(biāo)，第二個(gè)參數(shù)是目標(biāo)距離

rospy.on_shutdown（self.controllerLoss）#當(dāng)進(jìn)程被Ctrl+C終止時(shí)調(diào)用此方法

def trackerInfoCallback（self，info）：#目前不處理來(lái)自對(duì)象跟蹤器的任何信息

rospy.logwarn（info.data）

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

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

angleX=position.angleX

distance=position.distance

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

#調(diào)用PID控制器來(lái)更新它并獲得新的速度

［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 測(cè)試結(jié)果

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

圖11 測(cè)試圖