基于ROS的自動(dòng)分揀機(jī)器人系統(tǒng)設(shè)計(jì)

孫小飛，余 濤，陳 銳

（重慶大學(xué)，重慶 400044）

0 引言

電子商務(wù)的迅猛發(fā)展，為倉(cāng)儲(chǔ)物流行業(yè)帶來(lái)了新的需求和挑戰(zhàn)[1]。中國(guó)快遞行業(yè)的整體規(guī)模也在電子商務(wù)的迅猛發(fā)展下迅速壯大，2019年業(yè)務(wù)量已超過(guò)630億件，如此巨大業(yè)務(wù)量的快遞分揀工作也消耗著大量的人力物力資源。例如，在勞動(dòng)密集型的配送中心，從事分揀作業(yè)的人員占配送人員總數(shù)的50%，花費(fèi)在分揀作業(yè)上的時(shí)間占配送中心總工作時(shí)間的30%～40%[2]，這種傳統(tǒng)的人工手動(dòng)模式效率低、錯(cuò)誤率高；而立體倉(cāng)庫(kù)等自動(dòng)分揀系統(tǒng)雖然分揀效率較高，但占地面積大，而且，一旦建成就不可改變，柔性和靈活性較差[3]。

針對(duì)目前倉(cāng)儲(chǔ)物流行業(yè)存在的問(wèn)題，本文以中國(guó)機(jī)器人大賽“自動(dòng)分揀”項(xiàng)目為依托，設(shè)計(jì)了一款具有自主導(dǎo)航、自動(dòng)避障以及自動(dòng)分揀功能的自動(dòng)分揀機(jī)器人，并且，在實(shí)驗(yàn)過(guò)程中，針對(duì)DWA算法存在的問(wèn)題進(jìn)行了相應(yīng)的優(yōu)化。

1 自動(dòng)分揀機(jī)器人的硬件系統(tǒng)設(shè)計(jì)

1.1 本體結(jié)構(gòu)設(shè)計(jì)

本文中的自動(dòng)分揀機(jī)器人本體結(jié)構(gòu)由四驅(qū)麥克納姆輪底盤、二自由度機(jī)械臂以及自動(dòng)分揀倉(cāng)庫(kù)組成。其中，四驅(qū)麥克納姆輪底盤采用四組1000線的光電式編碼器直流減速電機(jī)（減速比1:27）驅(qū)動(dòng)麥克納姆輪的驅(qū)動(dòng)形式，選用的編碼器可以精確地測(cè)出機(jī)器人四個(gè)輪子的角速度數(shù)據(jù)，作為里程計(jì)數(shù)據(jù)的重要參考；電機(jī)固定板采用雙層玻纖板結(jié)構(gòu)，兩層玻纖板靠銅柱支撐，中間放置電機(jī)驅(qū)動(dòng)器、STM32控制板以及超聲波傳感器等元器件，其中兩個(gè)超聲波位于底盤前端，兩個(gè)超聲波位于底盤后端，固定板四周則由一圈較薄的玻纖板將其密封，防塵的同時(shí)，保證美觀。二自由度機(jī)械臂的大臂由步進(jìn)電機(jī)驅(qū)動(dòng)，小臂由舵機(jī)驅(qū)動(dòng)，小臂末端為舵機(jī)驅(qū)動(dòng)的撥片，撥片上安有顏色傳感器，對(duì)目標(biāo)物體進(jìn)行顏色識(shí)別。自動(dòng)分揀倉(cāng)庫(kù)由滑道，分揀擋板、分揀通道組成，分揀擋板會(huì)根據(jù)顏色傳感器的顏色反饋進(jìn)行擺動(dòng)，將目標(biāo)顏色的球體分別放置于不同倉(cāng)庫(kù)中。另外，機(jī)器人的前端放置有深度相機(jī)Kinect，分揀倉(cāng)庫(kù)上端安裝有激光雷達(dá)Rplidar，機(jī)械臂側(cè)面為MinpiPC。圖1為該機(jī)器人的結(jié)構(gòu)示意圖。

圖1 機(jī)器人結(jié)構(gòu)示意圖

1.2 運(yùn)動(dòng)學(xué)模型

該機(jī)器人的移動(dòng)底盤采用四驅(qū)麥克納姆輪[4]驅(qū)動(dòng)，該方式具有轉(zhuǎn)彎半徑小、運(yùn)動(dòng)靈活等特點(diǎn)。根據(jù)ROS下坐標(biāo)系的規(guī)定，建立其運(yùn)動(dòng)學(xué)模型示意圖：

圖2 四驅(qū)麥克納姆輪運(yùn)動(dòng)學(xué)模型示意圖

對(duì)下位機(jī)上傳的四個(gè)編碼器數(shù)據(jù)進(jìn)行速度解算得到機(jī)器人的速度：

其中ω1、ω2、ω3、ω4為四個(gè)輪子的角速度，Rω是輪轂半徑，2α為橫向兩輪中心間距，2b為前后輪軸的中心距，麥克納姆輪輪轂軸線和輥?zhàn)虞S線的夾角為α=45°；

若在初始時(shí)刻機(jī)器人坐標(biāo)系（xroryr）與里程計(jì)坐標(biāo)系（xoooyo）重合，則利用累加的方式進(jìn)行機(jī)器人定位：

式(3)采用了近似的計(jì)算方法，其假定在采樣間隔tΔt內(nèi)速度是恒定的，因此tΔt應(yīng)該盡可能小，計(jì)算精度才會(huì)越高。

但是，由于麥克納姆輪運(yùn)動(dòng)過(guò)程中，打滑現(xiàn)象嚴(yán)重，所以會(huì)造成機(jī)器人在運(yùn)動(dòng)，尤其是轉(zhuǎn)彎時(shí)，根據(jù)編碼器數(shù)據(jù)計(jì)算得到的里程計(jì)會(huì)存在較大誤差，為了保證里程計(jì)的精確性，采用下面兩種方法進(jìn)行優(yōu)化：

用IMU的yaw軸角速度替換由四個(gè)編碼器數(shù)據(jù)計(jì)算得到的角速度值；

對(duì)機(jī)器人直線和自轉(zhuǎn)運(yùn)動(dòng)進(jìn)行里程計(jì)精度測(cè)量，在相應(yīng)的里程計(jì)算公式中增加校正系數(shù)：

得到新的里程計(jì)計(jì)算公式：

1.3 硬件控制系統(tǒng)

自動(dòng)分揀機(jī)器人硬件系統(tǒng)由上位機(jī)與下位機(jī)兩部分組成。上位機(jī)為MiniPC，裝載Ubuntu 16.04+ROS kinetic，其與深度相機(jī)Kinect、激光雷達(dá)RplidarA2、慣性傳感器Razor_imu_9dof等通過(guò)USB串口直接相連，進(jìn)行環(huán)境信息以及自身位姿數(shù)據(jù)獲取。下位機(jī)為STM32，其與直流電機(jī)驅(qū)動(dòng)器RoboModule之間通過(guò)CAN通信，實(shí)現(xiàn)對(duì)四個(gè)底盤電機(jī)的驅(qū)動(dòng)；與步進(jìn)電機(jī)驅(qū)動(dòng)器TB6600直連控制大臂步進(jìn)電機(jī)動(dòng)作；與舵機(jī)驅(qū)動(dòng)器PCA9625之間通過(guò)IIC通信，實(shí)現(xiàn)對(duì)多個(gè)舵機(jī)的控制；顏色傳感器與STM32直連，通過(guò)IIC通信，獲取目標(biāo)物體的RGB值；超聲波傳感器與STM32直連，獲取距離數(shù)據(jù)。上位機(jī)與下位機(jī)通過(guò)串口線連接，實(shí)現(xiàn)數(shù)據(jù)通信。該自動(dòng)分揀機(jī)器人的硬件框架如圖3所示。

圖3 硬件框架

2 自動(dòng)分揀機(jī)器人的軟件系統(tǒng)設(shè)計(jì)

自動(dòng)分揀機(jī)器人的軟件系統(tǒng)包括上位機(jī)導(dǎo)航控制系統(tǒng)和下位機(jī)底層驅(qū)動(dòng)控制系統(tǒng)，其中，上位機(jī)基于ROS實(shí)現(xiàn)了機(jī)器人的自主導(dǎo)航功能[5]，下位機(jī)通過(guò)STM32實(shí)現(xiàn)對(duì)底層傳感器的驅(qū)動(dòng)控制，兩者通過(guò)串口通信實(shí)現(xiàn)數(shù)據(jù)傳輸。圖4為上位機(jī)與下位機(jī)的通信框圖。

圖4 通信框圖

2.1 下位機(jī)底層驅(qū)動(dòng)控制系統(tǒng)設(shè)計(jì)

機(jī)器人的下位機(jī)底層驅(qū)動(dòng)系統(tǒng)主要通過(guò)STM32實(shí)現(xiàn)電機(jī)的底層驅(qū)動(dòng)以及顏色傳感器、超聲波傳感器數(shù)據(jù)獲取。STM32通過(guò)與超聲波直連，獲取機(jī)器人前后500mm距離范圍內(nèi)的障礙物信息，并通過(guò)串口將距離數(shù)據(jù)上傳到上位機(jī)，進(jìn)行避障、距離矯正；同時(shí)，下位機(jī)根據(jù)上位機(jī)下發(fā)的速度數(shù)據(jù)控制底盤運(yùn)動(dòng)，并將機(jī)器人的實(shí)際運(yùn)動(dòng)速度上傳給上位機(jī)；當(dāng)機(jī)器人運(yùn)動(dòng)到目標(biāo)位置后，上位機(jī)對(duì)目標(biāo)點(diǎn)標(biāo)志位進(jìn)行賦值，下發(fā)給下位機(jī)，下位機(jī)根據(jù)標(biāo)志位的數(shù)據(jù)控制機(jī)械臂、自動(dòng)分揀倉(cāng)庫(kù)動(dòng)作，實(shí)現(xiàn)對(duì)目標(biāo)物體的分揀、放置工作；當(dāng)工作完成后，下位機(jī)對(duì)工作完成標(biāo)志位進(jìn)行賦值，并通過(guò)串口上傳到上位機(jī)，發(fā)布相應(yīng)目標(biāo)點(diǎn)任務(wù)。機(jī)器人下位機(jī)底層驅(qū)動(dòng)控制系統(tǒng)框架如圖5所示。

圖5 下位機(jī)底層驅(qū)動(dòng)控制系統(tǒng)框架

2.2 上位機(jī)導(dǎo)航控制系統(tǒng)設(shè)計(jì)

機(jī)器人的上位機(jī)導(dǎo)航控制系統(tǒng)基于ROS實(shí)現(xiàn)機(jī)器人的自主導(dǎo)航[6,7]功能。上位機(jī)利用下位機(jī)上傳的編碼器數(shù)據(jù)以及IMU的yaw軸角速度數(shù)據(jù)進(jìn)行里程計(jì)計(jì)算，并結(jié)合Rplidar、Kinect采集的外部環(huán)境進(jìn)行信息，利用Gmapping算法進(jìn)行建圖，利用AMCL算法實(shí)現(xiàn)定位。另外，上位機(jī)根據(jù)下位機(jī)上傳的工作完成標(biāo)志位的數(shù)據(jù)發(fā)布相應(yīng)的目標(biāo)點(diǎn)任務(wù)，利用A*算法進(jìn)行全局路徑規(guī)劃避開(kāi)所有障礙物，利用DWA算法在全局路徑的基礎(chǔ)上，產(chǎn)生一系列的速度下發(fā)到下位機(jī)，控制底盤運(yùn)動(dòng)。當(dāng)機(jī)器人運(yùn)動(dòng)到指定位置后，上位機(jī)利用IMU的yaw軸姿態(tài)數(shù)據(jù)結(jié)合超聲波數(shù)據(jù)進(jìn)行姿態(tài)矯正；矯正完成后，上位機(jī)對(duì)目標(biāo)點(diǎn)標(biāo)志位進(jìn)行賦值，下發(fā)給下位機(jī)，完成相應(yīng)的抓取、放置工作。機(jī)器人上位機(jī)導(dǎo)航控制系統(tǒng)框架如圖6所示。

圖6 上位機(jī)導(dǎo)航控制系統(tǒng)控制框架

2.3 機(jī)器人邏輯控制框架

自動(dòng)分揀機(jī)器人的主要功能是自主完成多點(diǎn)導(dǎo)航以及目標(biāo)物體的自動(dòng)分揀放置工作。首先，啟動(dòng)機(jī)器人，機(jī)器人向抓取區(qū)運(yùn)動(dòng)，當(dāng)運(yùn)動(dòng)到目標(biāo)位置后，會(huì)進(jìn)行姿態(tài)矯正，保證抓取的可靠性；然后，機(jī)械臂擺動(dòng)到工作狀態(tài)，通過(guò)顏色傳感器對(duì)目標(biāo)物體進(jìn)行顏色識(shí)別，分選到指定倉(cāng)庫(kù)中；接著，機(jī)器人先后運(yùn)動(dòng)到紅、藍(lán)色倉(cāng)庫(kù)區(qū)，打開(kāi)自動(dòng)分揀倉(cāng)庫(kù)門，將不同顏色的目標(biāo)物體放置到指定倉(cāng)庫(kù)；最后，機(jī)器人回到初始位置。

機(jī)器人的邏輯控制框架如圖7所示。

圖7 邏輯控制框架

3 DWA算法優(yōu)化

由于該機(jī)器人在工作區(qū)域內(nèi)多處需要拐彎、掉頭，所以減少拐彎、掉頭時(shí)行走的路徑，從而縮短機(jī)器人的運(yùn)行時(shí)間，對(duì)提高機(jī)器人的工作效率很有必要。同時(shí)，工作場(chǎng)地上存在著各種靜態(tài)、動(dòng)態(tài)障礙物，所以，良好的避障效果也十分重要。該自動(dòng)分揀機(jī)器人的路徑規(guī)劃算法采用A*和DWA算法，其中，DWA算法具有較好的實(shí)時(shí)性，但是避障效果較差，因此，針對(duì)機(jī)器人的工作需求以及DWA算法存在的問(wèn)題，對(duì)DWA算法做了相應(yīng)的優(yōu)化。

sim_period越長(zhǎng)，速度越大，規(guī)劃出的路徑越長(zhǎng)，越能更早的避開(kāi)障礙物，但是路徑末端端點(diǎn)偏離全局路徑的方位差越大。當(dāng)算法對(duì)局部路徑與全局路徑進(jìn)行貼合程度打分時(shí)，會(huì)以局部路徑的末端端點(diǎn)為參考，如果選擇的局部路徑與全局路徑末端端點(diǎn)位姿偏差越大，機(jī)器人在掉頭、拐彎或者避障時(shí)，需要運(yùn)動(dòng)一個(gè)比較大的軌跡才能調(diào)整到局部路徑末端對(duì)應(yīng)的全局路徑的位姿，從而，也增加了機(jī)器人的運(yùn)動(dòng)時(shí)間。

為了得到最優(yōu)的整體效果，最終采用增加仿真時(shí)間，裁剪局部路徑的方式對(duì)DWA算法進(jìn)行優(yōu)化。如圖8所示，適當(dāng)增加仿真時(shí)間sim_period，進(jìn)而增加DWA算法規(guī)劃出的路徑長(zhǎng)度，當(dāng)算法進(jìn)行避障評(píng)價(jià)時(shí)，可以更早的做出避障判斷；當(dāng)對(duì)局部路徑與全局路徑的貼合程度進(jìn)行評(píng)價(jià)時(shí)，對(duì)路徑進(jìn)行裁剪，去掉局部路徑末端偏離全局路徑的部分，保證局部路徑和全局路徑的貼合程度，同時(shí)，充分發(fā)揮麥克納姆輪的運(yùn)動(dòng)特性，使機(jī)器人在轉(zhuǎn)彎、掉頭、避障時(shí)傾向于并且可以通過(guò)自轉(zhuǎn)的方式調(diào)整機(jī)器人位姿，從而減少運(yùn)行路徑，縮短運(yùn)行時(shí)間，優(yōu)化避障效果。

圖8 路徑選擇評(píng)價(jià)圖

4 實(shí)驗(yàn)

自動(dòng)分揀機(jī)器人的主要功能是自主完成多點(diǎn)導(dǎo)航以及目標(biāo)物體的自動(dòng)分揀放置工作。并且，根據(jù)DWA算法原理以及麥克納姆輪的運(yùn)動(dòng)特性，對(duì)DWA算法進(jìn)行了優(yōu)化。

4.1 多點(diǎn)導(dǎo)航實(shí)驗(yàn)

研究過(guò)程中，根據(jù)工作需求，提前設(shè)置好目標(biāo)點(diǎn)坐標(biāo)，自動(dòng)分揀機(jī)器人按順序從啟動(dòng)區(qū)依次運(yùn)動(dòng)到藍(lán)（紅）色抓取區(qū)、藍(lán)（紅）色倉(cāng)庫(kù)區(qū)、紅（藍(lán)）色倉(cāng)庫(kù)區(qū)、啟動(dòng)區(qū)，并且完成相應(yīng)的抓取分揀工作、藍(lán)（紅）球放置工作、紅（藍(lán)）球放置工作。機(jī)器人在工作過(guò)程中的工作狀態(tài)圖如下：

圖9 工作狀態(tài)圖

4.2 分層避障實(shí)驗(yàn)

機(jī)器人工作環(huán)境中存在著各種靜態(tài)、動(dòng)態(tài)障礙物，并且障礙物的高度、位置各不相同，所以傳感器檢測(cè)障礙物時(shí)，需要有較大的檢測(cè)范圍。因此，該機(jī)器人障礙物檢測(cè)傳感器采用分層布置方式。其中，底盤上的超聲波通過(guò)控制邏輯，進(jìn)行高度較低、距離較近物體避障；Rplidar、Kinect將檢測(cè)到的障礙物投到代價(jià)地圖Costmap上進(jìn)行避障，其中，Rplidar進(jìn)行高度較高物體避障，Kinect進(jìn)行超生波以及Rplidar中間層的障礙物避障，從而保證了機(jī)器人的避障能力。圖10為機(jī)器人障礙物檢測(cè)區(qū)域分布圖，其中，深色三角區(qū)域?yàn)槌暡z測(cè)范圍，深色矩形區(qū)域?yàn)镵inect檢測(cè)范圍，Rplidar檢測(cè)范圍為高度為350mm的360°圓周范圍，圖中未標(biāo)出。圖11為Kinect與Rplidar的避障效果圖，其中，左圖為Kinect拍攝圖像，右圖為Kinect圖像中高、低障礙物在Costmap中投影，并且，機(jī)器人規(guī)劃出的路徑完全避開(kāi)了障礙物。

圖10 障礙物檢測(cè)區(qū)域分布圖

圖11 避障效果圖

4.3 DWA優(yōu)化效果實(shí)驗(yàn)

4.3.1 掉頭實(shí)驗(yàn)

本研究選取了不同的sin_period，對(duì)算法優(yōu)化前后，10次試驗(yàn)中，機(jī)器人旋轉(zhuǎn)180°運(yùn)行總時(shí)間以及相對(duì)起點(diǎn)位置（該坐標(biāo)系為右手笛卡爾坐標(biāo)系，并且，機(jī)器人正前方為X軸正方向），機(jī)器人在X軸、Y軸方向的偏移量進(jìn)行記錄，求取相應(yīng)平均值，實(shí)驗(yàn)結(jié)果如圖12、圖13所示。由于優(yōu)化后的算法對(duì)局部路徑進(jìn)行了裁剪，使局部路徑與全局路徑更加貼合，機(jī)器人在掉頭、拐彎時(shí)，傾向于通過(guò)自轉(zhuǎn)的方式進(jìn)行，所以行走的路徑更短，消耗時(shí)間更少。當(dāng)選取sim_period=2.5時(shí)，算法優(yōu)化后，機(jī)器人掉頭完成時(shí)的Y軸偏移量是優(yōu)化前Y軸偏移量的51%，優(yōu)化后的運(yùn)行時(shí)間也比優(yōu)化前縮短了30%，并且，隨著sim_period的增加，優(yōu)化后的算法仍能使機(jī)器人保持很好的掉頭效果。由此可見(jiàn)，優(yōu)化后的算法可以使機(jī)器人具有更好的掉頭效果。

圖12 掉頭運(yùn)行時(shí)間消耗圖

圖13 掉頭運(yùn)動(dòng)偏移量

4.3.2 避障實(shí)驗(yàn)

本研究在機(jī)器人正前方1 m處設(shè)置一長(zhǎng)寬高為64×23×42cm 的長(zhǎng)方體障礙物，將機(jī)器人目標(biāo)點(diǎn)設(shè)置在機(jī)器人正前方2.5m處，選取不同的sim_period，運(yùn)行程序，記錄10次試驗(yàn)中，機(jī)器人越過(guò)障礙物運(yùn)行的時(shí)間。由于優(yōu)化后算法規(guī)劃的局部路徑與全局路徑有更好的貼合性，使機(jī)器人在避障時(shí)行走路徑緊貼全局路徑（全局路徑已避開(kāi)所有障礙物），因此可以更好的避開(kāi)障礙物。另外，即使障礙物離機(jī)器人很近，或者是突然出現(xiàn)的障礙物，機(jī)器人也可以通過(guò)自轉(zhuǎn)的方式，調(diào)整到避開(kāi)障礙物的姿態(tài)，從而減少機(jī)器人在障礙物前猶豫、卡頓、減速尋找路徑的時(shí)間。如圖14中實(shí)驗(yàn)結(jié)果，在sim_period所有的選取值下，算法優(yōu)化后，機(jī)器人避障消耗的時(shí)間都少于算法優(yōu)化前，并且在實(shí)驗(yàn)過(guò)程中，算法優(yōu)化后，機(jī)器人避障成功率更高。由此可見(jiàn)，算法優(yōu)化后，機(jī)器人具有更好的避障效果。

圖14 機(jī)器人避障時(shí)間消耗圖

5 結(jié)語(yǔ)

該自動(dòng)分揀機(jī)器人具有自主導(dǎo)航、自動(dòng)避障以及自動(dòng)分揀功能。設(shè)置好多個(gè)目標(biāo)點(diǎn)，機(jī)器人可以依次運(yùn)動(dòng)到相應(yīng)坐標(biāo)點(diǎn)，自主完成物體的抓取、分揀、放置工作。另外，該機(jī)器人自主導(dǎo)航功能是基于ROS實(shí)現(xiàn)的，具有很強(qiáng)的可移植性，可以快速移植到各種輪式自主導(dǎo)航底盤，同時(shí)，只需根據(jù)工作需求，更換相應(yīng)的機(jī)械臂、抓手，就可以完成應(yīng)用于不同場(chǎng)景的自動(dòng)分揀機(jī)器人系統(tǒng)設(shè)計(jì)。該研究還對(duì)DWA算法進(jìn)行了優(yōu)化，提高了機(jī)器人的運(yùn)動(dòng)效率以及避障效果。后續(xù)將會(huì)結(jié)合G2O[10]圖優(yōu)化算法對(duì)DWA算法進(jìn)行優(yōu)化，進(jìn)一步提高機(jī)器人的動(dòng)態(tài)避障效果。