趙晉秀，尹志榮，張鑫，王羲，岳洪志

（太原工業(yè)學(xué)院機械工程系，山西 太原 030000）

0 引言

機器人研究領(lǐng)域，智能化、微型化是機器人的重要發(fā)展方向，具有自動搬運功能的分揀機器人一直是機器人研究領(lǐng)域的熱點，在軍用和民用方面前景廣闊。隨著時代發(fā)展，為解放多余勞動力、提高分揀搬運效率、減少生產(chǎn)成本，智能分揀搬運機器人應(yīng)運而生[1-3]。

傳統(tǒng)分揀搬運機器人通過巡線、超聲波測距、視覺監(jiān)測等方法實現(xiàn)分揀區(qū)與貨物堆放區(qū)之間的移動，此方法用時長、魯棒性小、對環(huán)境標志物依賴大[4]。

本文設(shè)計了一種通過編碼器和陀螺儀自主定位、TCS3200模塊識別顏色、垂直機械臂抓取的分揀搬運機器人。使用基于STM32F407ZGT6芯片的主控對傳感器信號進行識別處理并控制電機到達預(yù)定坐標和姿態(tài)。通過實驗解出了顏色識別算法，使得顏色識別受環(huán)境影響程度大大降低。

1 硬件系統(tǒng)設(shè)計

機器人硬件系統(tǒng)分為全向底盤和帶顏色識別模塊的機械臂兩部分。全向底盤負責(zé)物料分揀區(qū)和堆放區(qū)移動，機械臂負責(zé)抓取和搬運。機器人使用基于STM32F407ZGT6芯片的主控進行傳感器數(shù)值分析、電機驅(qū)動和機械臂控制。

圖1 系統(tǒng)設(shè)計

1.1 全向底盤

全向底盤，顧名思義是可以全方位移動的底盤，一般使用三個以上全向輪或四個麥克納姆輪，通過改變每個輪子的轉(zhuǎn)速和轉(zhuǎn)向?qū)崿F(xiàn)全向移動。

相較于傳統(tǒng)輪式底盤，全向底盤可以全方位移動、轉(zhuǎn)向靈活、適合狹窄區(qū)域作業(yè)，故本文使用全向底盤。為保證動力充足，本文使用四直流電機搭配四全向輪結(jié)構(gòu)。使用兩個L298N驅(qū)動進行控制，L298N內(nèi)含2個H橋的高電壓大電流全橋式驅(qū)動器，可以用來驅(qū)動直流電機[5]。

1.1.1 全向輪

全向輪輪轂上有若干個小輪轂，小輪可以釋放來自輪子法線方向的受力。

在實際中，為得到電機占空比和實際轉(zhuǎn)速的關(guān)系，使用編碼器電機作為驅(qū)動電機。為保證四個輪子電機驅(qū)動力相同，驅(qū)動輪使用四個編碼器電機。由于主控芯片定時器的編碼器功能口有限，僅使用一個電機的編碼器功能。

1.1.2 編碼器

編碼器又名碼盤，常用作測位移、測速傳感器。本方案采用四輪全向結(jié)構(gòu)，使用兩無動力的歐姆龍E6A2-CW5C編碼器通過聯(lián)軸器連接兩個小全向輪測定位移，兩編碼器正交安放，分別測定Χ軸和Y軸位移。

1.1.3 陀螺儀

陀螺儀常用于航海和航空領(lǐng)域，為航行人員和電子儀器指明航向。本方案使用MPU6050陀螺儀作為機器人角速度傳感器，對角速度積分可以得到航向角。

1.2 機械臂結(jié)構(gòu)

機械臂結(jié)構(gòu)使用舵機進行驅(qū)動和角度控制，機械臂使用的舵機較多，工作時電流較大，直接連接到主控會導(dǎo)致芯片供電不穩(wěn)，影響機器人平穩(wěn)運行，故采用維特智能16路舵機板對舵機進行控制，并且使用獨立電源供電。舵機板使用串口與主控通信，可以節(jié)約主控I/O口資源，也使得控制多個舵機變得更便捷。

使用編碼器電機驅(qū)動絲杠實現(xiàn)機械抓上下移動，絲杠與編碼器電機使用梅花聯(lián)軸器聯(lián)結(jié)，機械臂底部使用餐桌轉(zhuǎn)盤和舵機實現(xiàn)機械臂與底盤的聯(lián)結(jié)和整體轉(zhuǎn)動。

圖2 機械臂結(jié)構(gòu)

1.3 顏色識別模塊

顏色識別模塊可以準確、快速地識別物體顏色。本方案使用TCS3200顏色識別模塊，通過程序模數(shù)轉(zhuǎn)化輸出R、G、B三個數(shù)字量可以驅(qū)動TTL或CMOS邏輯輸入，與主控相連將數(shù)據(jù)傳回STM32芯片。實驗中，顏色識別模塊傳回數(shù)據(jù)受環(huán)境光照影響較大，因此在顏色識別模塊兩側(cè)使用兩恒流LED燈作為補光燈，使得實驗數(shù)據(jù)趨于平穩(wěn)。

圖3 顏色識別及補光燈

1.4 主控

主控是機器人的大腦，通過對傳感器信號進行處理并做出判斷，實現(xiàn)各部件協(xié)調(diào)運動以達到最終的動作和運動目標。本方案使用STM32F407ZGT6芯片作為主控芯片，對編碼器、陀螺儀、顏色識別模塊信號等進行分析和處理，驅(qū)動各模塊相互結(jié)合工作[6-8]。

STM32F407ZGT6芯片提供了足夠多的I/O口和定時器，可以支撐各個模塊的功能需求，為機器人開發(fā)和功能實現(xiàn)提供了更多的可能性，其中定時器特設(shè)的編碼器模式使得對編碼器的控制更加便捷。

1.5 減震系統(tǒng)

本文采用的四全向輪結(jié)構(gòu)對場地平整度要求較高，為防止某一驅(qū)動輪或編碼器計數(shù)輪騰空造成誤差，必須采用減震系統(tǒng)。

由于機器人體積較小，電機及電機支架之間距離較近，不宜使用垂直減震，底板使用玻纖板，通過機器人自重對底板造成輕微變形達到減震效果。編碼器處結(jié)構(gòu)較少，可以使用垂直彈簧減震。

圖4 編碼器及減震（彈簧已省略）

圖5 底板俯視圖

2 全向定位算法

2.1 建立坐標系

開電，陀螺儀初始化完成后設(shè)陀螺儀當前角度即機器人當前所指方向角度為0°，此時以初始坐標為直角坐標系原點，設(shè)逆時針偏移角度為正，順時針偏移角度為負。以俯視方向機器人的Y軸為正方向，順時針旋轉(zhuǎn)90°所指方向為Χ軸正向建立直角坐標系。

2.2 顏色識別算法

顏色識別模塊在使用前需要進行白平衡以得到R、G、B顏色因子。由于補光燈使得每次檢測時的光照環(huán)境基本相同，在程序中直接對顏色因子賦值，此方法可以省去白平衡步驟。

根據(jù)實驗值確定出一個較為合理顏色因子值，以便寫出顏色識別算法。圖6為顏色識別算法。

圖6 顏色識別算法

2.3 全場定位算法

全場定位算法使用閉環(huán)的PID算法，陀螺儀和編碼器分別監(jiān)測底盤的偏航角和位移。

陀螺儀測得的偏航角用于反饋底盤當前所處姿態(tài)的角度信息θ。正交擺放的編碼器，測得的信號進入主控計數(shù)器，經(jīng)過主控程序處理可以得到底盤在坐標系中的速度及底盤當前坐標：

主控程序給定目標坐標：

芯片執(zhí)行過程中，約5ms記錄一次數(shù)據(jù)，因此可將整個路程微分成無數(shù)個小的路程，微分后的單個小路程可以看作直線，此時Χ、Y軸編碼器記錄的位移為：

Χlast、Ylast為上個微分小段末的編碼器計值，即當前微分小段編碼器計值的初值，Χnow、Ynow為當前微分小段編碼器計值的末值，兩者取差值即為當前微分小段的位移。

圖7 位移正交分解

由圖可得：

其中dx，dy為分解到坐標系的微分小段位移，即機器人在坐標系的實際位移。此時，機器人在坐標系的位移為：

得到底盤當前姿態(tài)（Χnow，Ynow，θnow），將之與目標坐標姿態(tài)做差可得偏離位移，即距目標點剩余的位移和角度：

得到的剩余坐標和角度（Χerr，Yerr，θerr），使用PID算法對坐標和角度分別進行相應(yīng)計算，將此三個值疊加至電機占空比即可使底盤進行全向移動。由于PID算法本身會產(chǎn)生震蕩使得誤差較大，故算法根據(jù)剩余距離設(shè)定不同區(qū)間，改變PID權(quán)重，將震蕩帶來的影響減少到最小。

3 實驗測試

開電后，程序和各傳感器初始化完成，坐標系隨之建立，此時機器人處于坐標（0，0）點，且計初始角度為0°。

機器人根據(jù)設(shè)定坐標前往物料分揀區(qū)，啟動絲桿降下機械臂抓取物料，開啟補光燈并使用顏色識別模塊識別物料顏色，根據(jù)識別到的顏色前往對應(yīng)物料堆放區(qū)放下物料，回到出發(fā)區(qū)或繼續(xù)前往分揀區(qū)。

實驗過程中，通過調(diào)整剩余距離分區(qū)設(shè)定和各分區(qū)PID值，最終達到的最高精度為5mm/m。

為得到電機加速度能力以便調(diào)節(jié)PID的值，繪制電機時間-速度曲線。當給電機60%的速度時，得到時間-速度曲線圖如圖8。

圖8 時間-速度曲線圖

定位誤差主要由以下原因造成：

（1）PID算法本身ID是由誤差積累進行運轉(zhuǎn)，對系統(tǒng)的矯正有延遲。

（2）由于自身重量和電機扭力限制，電機占空比小于某值時底盤無法移動，使得電機對底盤坐標調(diào)節(jié)有較大的滯后性。

為改善原因（1），本方案采用分級PID控制底盤，即根據(jù)剩余位移（Χerr，Yerr）劃分不同區(qū)間，每個區(qū)間有各自的PID系數(shù)，每進入新區(qū)間，將I和D的累積值清空。此方法顯著緩解底盤在特殊點附近震蕩的問題，同時提高了定位精準度和效率；使用分級PID算法，定位精度可以達到5mm/m，但遠距離移動仍會產(chǎn)生較大誤差。

實驗中，正常運動情況下，給定P的權(quán)重較大時，機器人在即將抵達目標點的最后極短距離時，會由于P的系數(shù)過大而導(dǎo)致系統(tǒng)震蕩，實際表現(xiàn)為：在目標點處抖動，始終達不到給定的停止區(qū)間（此即為設(shè)定定點精度），因此使用分級調(diào)節(jié)，在最后的一段區(qū)域通過改變權(quán)重，將P調(diào)小，I調(diào)大，會產(chǎn)生如圖9所示的變化。

圖9 PID不同權(quán)重所產(chǎn)生的影響

對于遠距離移動，本方案采用多次定點的方法，在每個定點處設(shè)置黑線作為定位校準基準，每次到達基準點，機器人使用光電傳感器進行校準。此方法可以有效減小因遠距離移動帶來的累計誤差對定位精度的影響。使用紅外對管模塊，進行黑線校準，其精度高于僅靠定位算法的精度，多次定點，多次黑線校準的方法，使得機器人在遠距離移動時定位精度能夠得到保障。

4 結(jié)論

使用基于STM32F407ZGT6芯片的全向底盤與帶云臺的機械臂，結(jié)合顏色識別模塊能夠快速識別、抓取、搬運帶顏色的物料，對機器人定位準確度、顏色識別算法、機械臂設(shè)計與控制進行了研究。通過對RGB顏色因子的軟件賦值，顏色識別模塊可以精準識別黑、白、紅、藍、綠顏色。機器人自主定位精度可以達到5mm/m，滿足室內(nèi)搬運和機器人競賽的要求，由于系統(tǒng)誤差和PID算法本身的缺陷，難以進行更精準的定位。對于長遠距離，尤其需要拐彎的路徑，需要使用多個坐標或黑線標志才能使定位精度略為提高，但仍有待提高。