基于SLAM的機器人在車間運送中的應用*

黃原有，陽 韜，張楊志，許立蕩，張明劍

（華南理工大學廣州學院，廣州 510800）

0 引言

SLAM運送機器人主要采用ROS系統的SLAM技術，對機器人所在位置的地圖重建和定位，并基于SLAM 技術，采用AMCL 導航定位算法和路徑規劃算法，實現機器人的自主運送。現如今，該技術主要應用于掃地機器人。相比掃地機器人的后輪兩電機差速控制轉向，SLAM運送機器人采用的是舵機控制轉向。除此之外，無人駕駛汽車也涉及到該技術，可以說該技術在國內外都非常地流行。但許多行業、許多場所都未能享受該技術所帶來的人工智能。比如說，在工業生產中，生產車間用人力運送材料和人力推車的現象。因此本文分析工業生產技術，結合目前主流的SLAM 技術與嵌入式技術，設計一種能夠應用于生產車間的智能運送機器人，能夠實現車間點對點的材料運輸，并且能夠擁有自動避障、自主導航、路徑規劃等功能，增加車間的安全性，減少車間材料運輸的成本，加快車間的生產效率。

1 系統整體設計

圖1 所示為系統框架圖，系統分別由智能運送機器人、生產機械檢測、管理后臺3個部分構成。而智能運送機器人又由3 個部分組成，分別是小車底盤、樹莓派、PC 端或云服務器的ROS系統。

系統通過檢測生產機械的生產材料是否用完或者成品是否已經制作完成，若制作完成或生產材料用完，則發送信號給管理后臺。管理后臺則發送目標位置給智能運送機器人的計算機端或者云服務器上的ROS 系統（這個項目在研究過程中選擇的是在計算機端安裝ROS系統），讓其通過無線網絡通信將目標位置的坐標傳送到樹莓派的ROS 系統上并且遠程編譯運行文件，再根據樹莓派上運行的激光雷達數據建立地圖，進行路徑規劃。最后通過串口通信將ROS 系統發布的機器人速度指令傳給機器人的底座，導航機器人運動到目標位置，最終實現機器人的智能運送功能。

圖1 系統框架圖

2 機器人機械結構

機器人的機械結構主要由小車底盤和儲物箱組成。

2.1 底盤

小車的底盤如圖2 所示，小車底盤分成3層。第一層是由普通的小車輪胎、2 個帶編碼器的電機、1 個轉向軸和1 塊鐵板構成，使用4個電機驅動則不需要轉向軸，可以通過控制電機的轉速來控制小車的轉向，能夠提高機器人的運載重量，小車的輪胎則使用麥克納姆輪的效果更好；第二層是在小車底板上面安裝小車的驅動板和電源等設備；第三層可用一個擋板隔開，然后將樹莓派和激光雷達放在上面，但激光雷達除了安裝在第三層，還可以安裝在儲物箱上面，激光需要不斷地掃描周圍環境的物體，以此安裝得高一點的位置比較合適。

圖2 實驗小車底盤實物圖

2.2 儲物箱

儲物箱可安裝在小車的底盤上面，用于運載物品，并且儲物箱的大小可根據機器人應用的場所以及小車的運載能力所決定。

3 機器人硬件系統設計

機器人的硬件系統設計主要由Stm32f407、樹莓派、激光雷達、電機構成。

3.1 嵌入式控制器

因為上層ROS系統中用move_base算法做路徑規劃需要小車的角速度和線速度的參數，所以小車底盤需安裝由Acorn公司開發的32位人工智能微控制器——Stm32f407微控制器，其低成本、高效率、高性能的特點適合應用到智能運送機器人上。因此采用這種性能相對較高的微控制器來驅動九軸角度傳感器去獲取小車運動過程中的IMU 信息（機器人的位置和姿態）。除此之外，Stm32f407 開發板還驅動帶編碼器的電機（用于讀取編碼器的讀數，轉化成速度，進而換算為里程計信息）、穩壓芯片（將電池的12 V 電壓降壓至5 V 電壓）、舵機（控制小車的轉向）。

3.2 樹莓派

樹莓派是一個微型電腦裝置，具備計算機的基本功能。而研發過程中使用的是樹莓派3B+，其擁有64 位四核處理器，使得ROS 程序能夠在上面運行。樹莓派3B+的系統是基于Linux，在本實驗中安裝的是ubuntu 18.04，并在ubuntu 命令窗口通過命令安裝ROS（實驗中安裝的是ROS melodic 版本）。除此之外，需要通過樹莓派與底層的Stm32微控制器通信，接受從底層傳送過來的機器人線速度、角速度、位置信息以及imu等信息，并且在ROS系統中move_base包中通過對底層的信息處理和結合路徑規劃算法，將會在cmd_vel主題上發布Twist的機器人速度指令消息，控制機器人底座的運動。

3.3 激光雷達

激光雷達(rplidar a1)是由SLAMTEC 公司開發的一種激光測距儀，其采用紅外激光測距信號，主要通過發射激光和障礙物反射回來的時間計算距離。rplidar a1 是采用三角測距技術對二維環境進行激光掃描，獲取點云信息進而形成點云地圖（即雷達掃描到的物體用一個個點在地圖顯示）。但是驅動激光雷達需要在樹莓派上的ROS 系統安裝rplidar_ROS 驅動包，配置激光雷達的參數，進而使ROS 系統獲取激光雷達的深度信息。如圖3 所示，在ROS 系統驅動rplidar a1 并用Rviz將數據變成可視化的圖像，從圖像可看出，在柵格上紅色的點是激光雷達掃描出來的障礙物。

圖3 激光雷達掃描圖

4 系統軟件設計

4.1 Stm32的軟件功能

本實驗Stm32f407 微控制器的開發環境是keil uVision5 和Stm32 cubemax。Stm32 cubemax是C語言代碼的生成工具，在軟件中可以圖形化的點擊選擇使用哪個GPIO、定時器的開啟等，最終生成配置好的代碼，加快了機器人的開發速度。而keil uVision5則是將生成的C代碼導入進軟件，讓開發人員編寫程序代碼和進行編譯調式，最后通過JLINK 仿真器燒錄進Stm32微控制器。 圖4所示為Stm32的控制流程圖，軟件具體功能如下。

（1）驅動九軸角度傳感器，讀取IMU 信息。Stm32f407給九軸角度傳感器5 V 電壓，驅動傳感器，再通過GPIO 讀取傳感器的IMU 信息。Stm32 微控制器將IMU 信息，傳給上層的ROS 系統，進行信息的解算，從而確定機器人的位姿信息。

（2）通過PID 算法控制小車的舵機和左右輪的電機。Stm32f407 通過PID 控制算法[1]輸出PWM 值給小車底盤的舵機和左右輪電機，控制電機和舵機的轉速，從而控制小車的速度和轉向。算法的主要表達式[2]為：

式中：Kp、Ki、Kd分別為通過不斷串口調試，得到適合控制機器人底座的比例系數、積分系數、微分系數，控制輸出PWM 值；e[n]為PWM 和上次PWM 值的誤差值；e[n-1]為上次PWM值和上上次PWM的誤差值；e[n-2]則為上上次PWM和其上次的誤差值。

因此，通過以上PID算法的公式得到Δu增量輸出，傳給左右輪電機和舵機，控制轉速。

（3）讀取編碼器的數值并轉化為速度、位姿信息。將獲取的編碼器讀數需解算成odometry 信息(即機器人底座的位置和速度信息)。實驗的小車是使用舵機來控制小車的轉向，而非通過小車左右輪的速度差來控制轉向，但這種控制方式在機器人轉向時，左右輪編碼器讀數轉換成的左右輪速度也會有差異，因此，可認為和差分機器人類似，運用運動公式得出小車的線速度和角速度。圖4 所示為差分機器人運動分析圖，得出公式如下：

圖4 Stm32的控制流程圖

圖5 差分機器人分析圖

假設運送機器人從圖5的點B運動到點A，其線速度則為左右輪速度的平均值，角速度則為左右輪速度的差值與機器人左右輪距離的商值。而圖4 機器人從點A 運動到點B 的過程可近似看作繞點O 做圓周運動，半徑r 可由線速度v 和角速度w 的商求出。圓周角α 可由相似三角形得出α=β，β則為左右輪運動距離的差值h 除以左右輪之間的距離l。求出r 和圓周角α后，可根據下面公式求出機器人相對于初始位置O在x和y方向上的偏移為：

式中：Δ Y為小車相對點O在Y方向的位移；Δ X則相對點O在X方向的位移。

（4） 發布里程計信息和IMU 信息和接收cmd_vel 話題上的Twist 消息（即為對小車左右電機的期望值）。ROS 系統上編寫的Serial串口通信功能包，實現底層的嵌入式控制器與樹莓派上的ROS通信，將小車底座的里程計信息和IMU信息傳入到ROS中。與此同時，move_base路徑規劃包會發布機器人的速度指令，然后Serial功能包將ROS對機器人的期望速度指令傳給底層。Stm32控制板將根據差分機器人的運動學公式對指令進行結算，將其轉換為小車左右輪電機的速度和舵機的轉速。

4.2 ROS系統

ROS是一種基于Linux的機器人操作系統，擁有很多已經封裝好的代碼庫，有很多功能包的算法已經編寫好，只需要根據開發的實際情況，稍作修改就能實現功能。如圖6所示，SLAM 智能運送機器人運用了Amcl 實現機器人的自主定位導、Move_base 實現機器人運送路徑的規劃、Slam_gmapping實現機器人環境的地圖重建。

機器人底層的里程計信息和IMU 信息通過Serial 包的base_controller 節點傳入，并將底層得到的線速度和角速度信息進行計算得到最終的里程計信息。而底層傳入的IMU 原始數據（raw_imu）通過imu_tools 過濾器得到imu/data_raw。對于底層的激光雷達通過ROS 安裝的驅動包獲取激光雷達的深度信息，并且機器人底座上方的激光雷達坐標（base_laser）和機器人底盤坐標(base_link)通過tf 軟件包將這兩個坐標加入到tf樹中，實現這兩個坐標之間的轉換，因為這2個坐標之間存在一些的誤差。底層的信息獲取并處理后，將會傳入到Slam_gmapping、Amcl、Move_base功能包。

圖6 ROS系統的架構圖

對于SLAM智能運送機器人的地圖重建的[3]Slam_gmapping[4]則需訂閱imu/data_raw 信息、里程計信息、激光雷達的深度信息，收到的imu/data_raw 會通過濾波[5]和里程計信息進行融合[6]，最終發布一個tf。最終根據激光雷達的深度信息和tf 坐標，使用SLAM算法實現機器人的即時定位與地圖構建，并發布map主題（獲取地圖數據，將其鎖定并定期更新）。除此之外，Amcl[7]功能包也是需要訂閱scan（激光雷達的深度信息）、tf（里程計信息中的機器人底盤坐標）、map主題，最終發布tf(顯示機器人的位姿，并將base_link，odom，map 都加入到tf樹中，使其能夠相互轉換)。最后將map數據、Amcl的tf、Odometry里程計信息、激光雷達信息輸入到move_base[8]功能包運用Dijkstra 最優路徑規劃算法進行路徑規劃，并通過cmd_vel 話題上的Twist 消息將機器人的速度指令發出去。其中，要將imu信息和里程計信息融合是考慮到輪胎打滑時，里程計的位姿可能會有誤差。

4.3 SLAM算法

圖7 所示為Slam_gmapping 流程圖，SLAM 智能運送機器人在建圖功能開發時，使用了gmapping 算法，其源碼結構組織是Open_Slam，具體流程分述如下。

圖7 Slam_gmapping 流程圖

4.3.1 創建原始地圖

在Slam_gmapping[9]首先通過laserCallback回調函數去調用其他函數進行數據的采集和處理，并重建地圖。接著laser-Callback 回調函數會調用initMapper 初始化函數，接收激光雷達的點云scan數據[5]，并創建一個原始地圖。

4.3.2 數據處理

等獲取激光數據和底層的Odometry 信息后，則調用processScan函數對激光雷達的數據處理，具體如下。

（1）讀取預測粒子的位姿，調用drawFromMotion函數。

（2）調用scanMatch函數來獲得更優位姿。首先，在scan-Match 函數里首先會用optimize 進行6 次位姿的修正和調用score 函數運用下面高斯函數的得分公式獲得每個粒子的得分：并判斷匹配得分是否滿足要求。 其次，調用likelihoodAndSocre 函數得出每個粒子的權重。最后，調用computeActiveArea 計算每個粒子相應的位姿所掃描到的區域并用Bresenham算法得到非障礙物格點的集合，最后形成可活動區域的范圍。

圖8 地圖重建的效果圖

（3）調用Resample（）函數對數據重采樣。在函數里，首先會判斷有效粒子數是否小于閾值。如果是，則需要進行重采樣。然后調用registerScan（），使用Bresenham 算法得到激光雷達已掃描的單元格，并標記有障礙物的單元格。最后delete 舊的粒子，add 新的粒子，并將粒子位姿和激光雷達的數據實時更新到局部地圖。但如果判斷得到無需重新采用，則創建新的節點，add到tf樹中。

4.3.3 更新地圖

根據Slam_gmapping功能包使機器人進行地圖重建的效果圖如圖8所示。

5 結束語

本文研究SLAM 技術與機器人技術，結合車間生產的實際情況，通過設計運送機器人，完成測試機，可以實現機器人的地圖重建、自動避障、自主定位導航等功能。后臺管理可以將材料運送的位置發往ROS 系統，系統則將運送指令發送給機器人，從而將生產車間材料運送到指定地點，實現無人配送，節約勞動成本，加快車間的生產效率。此外，SLAM運送機器人還可以應用在日常的生活中，例如農業生產過程中收割后的糧食運送，代替馬車、人力驅車、人力運送等。