基于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 測試圖