基于麥克納姆輪的視覺(jué)識(shí)別與搬運(yùn)系統(tǒng)設(shè)計(jì)

陳煒琦，周騫煜，吳巖

（杭州電子科技大學(xué) 電子信息學(xué)院，浙江杭州，310018）

0 引言

工業(yè)機(jī)器人自誕生后就迅速進(jìn)入產(chǎn)業(yè)化發(fā)展，并且在工業(yè)生產(chǎn)中得到廣泛應(yīng)用[1]。麥克納姆輪移動(dòng)平臺(tái)因其獨(dú)特的全向運(yùn)動(dòng)特性能夠在狹小的空間靈活運(yùn)動(dòng)而備受關(guān)注[2]。機(jī)器視覺(jué)技術(shù)融合了計(jì)算機(jī)科學(xué)、應(yīng)用數(shù)學(xué)等多個(gè)交叉學(xué)科的知識(shí)，已經(jīng)成為人工智能應(yīng)用領(lǐng)域的研究熱點(diǎn)[3]。本文旨在研究結(jié)合視覺(jué)識(shí)別、目標(biāo)檢測(cè)和麥克納姆輪以更好地完成復(fù)雜的搬運(yùn)任務(wù)。

1 系統(tǒng)總體方案設(shè)計(jì)

本系統(tǒng)總體方案設(shè)計(jì)如圖1 所示，主要分為六大模塊：

圖1 系統(tǒng)總體方案框圖

主控模塊，采集傳感器模塊、視覺(jué)處理模塊、輔助調(diào)試模塊的信息，做出控制決策，驅(qū)動(dòng)電機(jī)控制模塊。

電源模塊，負(fù)責(zé)為主控模塊和相關(guān)外設(shè)提供穩(wěn)定的電源電壓。

傳感器模塊，獲取系統(tǒng)與外界環(huán)境的各類信息供主控模塊分析計(jì)算。

電機(jī)控制模塊，直流電機(jī)實(shí)現(xiàn)系統(tǒng)運(yùn)動(dòng)，舵機(jī)完成機(jī)械臂運(yùn)行，電磁鐵實(shí)現(xiàn)抓取和放置物品。

視覺(jué)處理模塊，獲取物品位置信息和分類結(jié)果。

輔助調(diào)試模塊，監(jiān)視系統(tǒng)狀態(tài)，觀察變量。

2 系統(tǒng)硬件電路設(shè)計(jì)

系統(tǒng)采用NXP RT1064 為核心MCU，整套硬件系統(tǒng)包括主控模塊、電源模塊、傳感器模塊、電機(jī)控制模塊、視覺(jué)處理模塊、輔助調(diào)試模塊。

2.1 主控模塊設(shè)計(jì)

考慮合理分配RT1064 的硬件資源以確保其余模塊正常使用，原理圖如圖2 所示。

圖2 主控模塊原理圖

2.2 電源模塊設(shè)計(jì)

電源選擇8.4V 電池，通過(guò)buck[4]電路完成降壓，其中5V 電源采用TPS54328DDAR，3.3V 電源選擇TPS7333。原理圖如圖3 所示。

圖3 電源模塊原理圖

2.3 傳感器接口設(shè)計(jì)

傳感器模塊包括陀螺儀、藍(lán)牙模塊和編碼器，原理圖如圖4 所示。

圖4 傳感器模塊原理圖

2.4 電機(jī)控制模塊設(shè)計(jì)

電機(jī)控制模塊包括直流電機(jī)、舵機(jī)以及電磁鐵，直流電機(jī)模塊采用H 橋[5]的設(shè)計(jì)。舵機(jī)和電磁鐵工作電壓均為8.4V。原理圖如圖5 所示。

圖5 電機(jī)控制模塊原理圖

2.5 視覺(jué)處理模塊設(shè)計(jì)

視覺(jué)處理模塊包括三個(gè)圖像識(shí)別視覺(jué)模塊，均使用UART 通訊，視覺(jué)處理模塊原理圖如圖6 所示。

圖6 視覺(jué)處理模塊原理圖

2.6 輔助調(diào)試模塊設(shè)計(jì)

輔助調(diào)試模塊包括OLED 屏、按鍵、無(wú)線串口，輔助調(diào)試模塊原理圖如圖7 所示。

圖7 輔助調(diào)試模塊原理圖

3 系統(tǒng)結(jié)構(gòu)設(shè)計(jì)

系統(tǒng)結(jié)構(gòu)設(shè)計(jì)部分包括系統(tǒng)底盤和機(jī)械臂的設(shè)計(jì)。經(jīng)過(guò)Solidworks 軟件建模，使用激光雕刻技術(shù)和3D 打印技術(shù)加工實(shí)物并完成組裝。

3.1 系統(tǒng)底盤設(shè)計(jì)

系統(tǒng)底盤作為整個(gè)系統(tǒng)的支撐結(jié)構(gòu)，所有模塊均直接或間接安裝在底盤上，設(shè)計(jì)時(shí)主要考慮機(jī)械臂和麥克納姆輪的安裝。

機(jī)械臂與底盤之間通過(guò)螺栓螺母連接，為解決重心偏移問(wèn)題，機(jī)械臂安裝在底盤前端，電池等電子元件均安裝在底盤后端。

底盤采用四輪獨(dú)立驅(qū)動(dòng)的麥克納姆輪全向底盤結(jié)構(gòu)，每個(gè)電機(jī)驅(qū)動(dòng)一個(gè)麥克納姆輪[6]，安裝方式為螺栓螺母，驅(qū)動(dòng)電機(jī)與編碼器通過(guò)齒輪配合，底盤設(shè)計(jì)如圖8 所示，安裝效果如圖9 所示。

圖8 底盤設(shè)計(jì)建模圖

圖9 麥克納姆輪安裝效果

3.2 機(jī)械臂設(shè)計(jì)

機(jī)械臂是系統(tǒng)的執(zhí)行結(jié)構(gòu)，負(fù)責(zé)將物品搬離或放回地面。機(jī)械臂采用2 自由度結(jié)構(gòu)，驅(qū)動(dòng)電機(jī)為舵機(jī)。為簡(jiǎn)化機(jī)械臂設(shè)計(jì)，在物品表面貼有圓鐵片，機(jī)械臂頂部安裝電磁鐵實(shí)現(xiàn)物品的搬運(yùn)。

4 系統(tǒng)軟件設(shè)計(jì)

系統(tǒng)軟件設(shè)計(jì)部分包括系統(tǒng)運(yùn)動(dòng)、目標(biāo)檢測(cè)、物品分類、物品搬運(yùn)、系統(tǒng)定位五大模塊。發(fā)出出發(fā)命令后，系統(tǒng)開(kāi)始遍歷。若未發(fā)現(xiàn)物品，系統(tǒng)將遍歷場(chǎng)地；若發(fā)現(xiàn)物品，系統(tǒng)運(yùn)動(dòng)到物品附近，識(shí)別物品類別后將物品搬運(yùn)到指定區(qū)域，然后再運(yùn)動(dòng)到原先位置繼續(xù)遍歷。多個(gè)物品則根據(jù)就近原則搬運(yùn)。任務(wù)進(jìn)行過(guò)程中，根據(jù)傳感器信息計(jì)算當(dāng)前系統(tǒng)坐標(biāo)。系統(tǒng)程序流程框圖如圖10 所示。

圖10 系統(tǒng)程序流程框圖

4.1 系統(tǒng)運(yùn)動(dòng)

以場(chǎng)地為固定坐標(biāo)系，規(guī)定向右為x 軸正方向，向前為y 軸正方向，垂直地面向上為z 軸正方向。

4.1.1 麥克納姆輪運(yùn)動(dòng)學(xué)模型解算

式中(v xv yvz)T為系統(tǒng)的實(shí)際速度，R 為輪子半徑，k 為常數(shù)，(v0v1v2v3)T為輪子轉(zhuǎn)速。

為更好地求解四個(gè)輪子的轉(zhuǎn)速，引入式(2)，由線性變換可解得式(3)。

兩式中所有符號(hào)均與式(1)相同。

式(1)和式(3)即麥克納姆輪正逆向運(yùn)動(dòng)學(xué)模型解算[7]。

4.1.2 PID 控制算法

但是，在實(shí)際系統(tǒng)中，需要結(jié)合編碼器構(gòu)建PID 速度環(huán)控制系統(tǒng)[8]，PID 算法框圖如圖11 所示。

圖11 PID 算法框圖

數(shù)學(xué)公式為：

式中Kp為比例時(shí)間系數(shù)，Ki為積分時(shí)間系數(shù)，Kd為微分時(shí)間系數(shù)，t為從0 時(shí)刻開(kāi)始到輸出當(dāng)前控制量時(shí)經(jīng)過(guò)的時(shí)間間隔。

The velocity amplification coefficient of the open-loop system is

本文使用三個(gè)不同參數(shù)且相互獨(dú)立的PID 控制環(huán)計(jì)算系統(tǒng)當(dāng)前速度的理論值，使用一個(gè)PID 控制環(huán)確保系統(tǒng)當(dāng)前速度為理論速度。完整的PID 串級(jí)環(huán)程序流程框圖如圖12 所示。

圖12 PID 串級(jí)環(huán)程序流程框圖

第一個(gè)PID 環(huán)控制系統(tǒng)沿著x 軸方向平移。給定系統(tǒng)的目標(biāo)坐標(biāo)(x0y00)，主控模塊實(shí)時(shí)計(jì)算當(dāng)前坐標(biāo)(x y Yaw)，系統(tǒng)當(dāng)前位置與目標(biāo)位置在x軸方向上的距離差值e x=x-x0，將其代入式(4)得此刻目標(biāo)速度分量vx。

第二個(gè)和第三個(gè)PID 環(huán)分別控制系統(tǒng)沿著y 軸運(yùn)動(dòng)和繞著z 軸旋轉(zhuǎn)，計(jì)算方式與第一個(gè)PID 環(huán)僅有參數(shù)上的差別，可得目標(biāo)速度分量vy和vz。

將三個(gè)速度代入式(3)即得到四個(gè)輪子的目標(biāo)速度v0、1v、v2、v3。

由四個(gè)編碼器可得四個(gè)輪子的實(shí)際速度，把四個(gè)速度差值作為第四個(gè)PID 環(huán)的輸入就可得到輸出，這個(gè)輸出可作為驅(qū)動(dòng)每個(gè)輪子的PWM 占空比。

4.2 目標(biāo)檢測(cè)

目標(biāo)檢測(cè)采用輕量化的YOLO 模型，將實(shí)拍且不影響訓(xùn)練的圖片組成數(shù)據(jù)集1，將數(shù)據(jù)集1 通過(guò)OPENCV 進(jìn)行擴(kuò)充數(shù)據(jù)集得到數(shù)據(jù)集2，對(duì)數(shù)據(jù)集2 的每一張圖片人工添加標(biāo)簽確保訓(xùn)練數(shù)據(jù)正確，訓(xùn)練結(jié)束后使用量化、采樣的方式得到輕量化模型。

系統(tǒng)在遍歷時(shí)，圖像識(shí)別視覺(jué)模塊使用OPENMV 庫(kù)函數(shù)讀取該模型，將物品在圖像識(shí)別視覺(jué)模塊拍攝的圖片中的像素坐標(biāo)轉(zhuǎn)化為場(chǎng)地內(nèi)真實(shí)的距離坐標(biāo)，就得到了物品的位置信息。

4.3 物品分類

為提高分類準(zhǔn)確率和系統(tǒng)魯棒性，在物品表面貼有卡片用于指示物品類別。物品分類采用輕量化的CNN 神經(jīng)網(wǎng)絡(luò)模型，將實(shí)拍物品所屬類別的卡片得到數(shù)據(jù)集1，將數(shù)據(jù)集1 中的所有圖片進(jìn)行數(shù)據(jù)增強(qiáng)并縮放至64＊64 像素點(diǎn)，得到數(shù)據(jù)集2，并放入TensorFlow 框架中，共包括6 層卷積層，6 層池化層，6 層隨機(jī)失活層，卷平層，全連接層，激活函數(shù)[9]。訓(xùn)練結(jié)束時(shí)測(cè)試集正確率84.1%，經(jīng)過(guò)量化、采樣后對(duì)卡片實(shí)際驗(yàn)證正確率，達(dá)到80.8%。

將所有錯(cuò)誤的卡片再次進(jìn)行數(shù)據(jù)增強(qiáng)得到數(shù)據(jù)集3，調(diào)整網(wǎng)絡(luò)結(jié)構(gòu)和學(xué)習(xí)率、batch 等參數(shù)，將數(shù)據(jù)集3 放入新的網(wǎng)絡(luò)中訓(xùn)練，訓(xùn)練結(jié)束后再次實(shí)際驗(yàn)證正確率并不斷重復(fù)上述過(guò)程，最終實(shí)際驗(yàn)證正確率為99.2%，每一類別正確率如圖13 所示。

圖13 測(cè)試集類別正確率圖

圖14 舵機(jī)狀態(tài)1-物品搬運(yùn)

4.4 物品搬運(yùn)

卡片搬運(yùn)依靠機(jī)械結(jié)構(gòu)完成，機(jī)械臂驅(qū)動(dòng)電機(jī)為舵機(jī)，只需改變信號(hào)的占空比即可控制舵機(jī)。

機(jī)械臂自動(dòng)處于復(fù)位狀態(tài)，當(dāng)準(zhǔn)備搬運(yùn)物品時(shí)進(jìn)入狀態(tài)1，當(dāng)物品離開(kāi)地面后系統(tǒng)運(yùn)動(dòng)到指定區(qū)域，運(yùn)動(dòng)結(jié)束后電磁鐵斷電，機(jī)械臂復(fù)位。

在實(shí)際控制過(guò)程中，由于舵機(jī)精度不夠?qū)е略跈C(jī)械臂運(yùn)動(dòng)時(shí)物品并不能被很好地吸合，因此采用積分分離PID 算法[10]，用運(yùn)動(dòng)軌跡的控制量進(jìn)行快速準(zhǔn)確處理，計(jì)算PWM占空比完成對(duì)舵機(jī)的控制。

4.5 系統(tǒng)定位

為更好地完成物品搬運(yùn)任務(wù)，準(zhǔn)確而實(shí)時(shí)的位置信息能夠顯著提高效率。本文通過(guò)處理陀螺儀和編碼器信息，采用數(shù)據(jù)融合方式完成定位。

4.5.1 陀螺儀數(shù)據(jù)處理

陀螺儀可直接得到系統(tǒng)的三軸加速度和三軸角速度，但是陀螺儀原始信號(hào)存在很大噪聲，即使通過(guò)濾波也無(wú)法完成去噪甚至?xí)淖冇杏眯盘?hào)，因此本文采用四元數(shù)進(jìn)行姿態(tài)解算得到歐拉角[11]。

四元數(shù)解算，先對(duì)重力加速度歸一化，將四元數(shù)換成方向余弦中的三個(gè)元素，對(duì)六軸數(shù)據(jù)向量叉積得姿態(tài)誤差，對(duì)誤差進(jìn)行積分再濾波修正陀螺儀誤差，通過(guò)一階龍格庫(kù)塔法更新四元數(shù)并歸一化就可以得到最終的偏航角Yaw。

4.5.2 編碼器數(shù)據(jù)處理

利用一階互補(bǔ)濾波算法[12]處理編碼器的原始數(shù)據(jù)，即得四個(gè)輪子的旋轉(zhuǎn)速度，將其代入式（1）可得速度，但是，由于偏航角的變化，需引入坐標(biāo)變換進(jìn)行速度修正得到系統(tǒng)以場(chǎng)地為坐標(biāo)系的速度，對(duì)修正后的速度經(jīng)過(guò)積分得到系統(tǒng)的坐標(biāo)。由三角函數(shù)的計(jì)算可得速度修正公式為式(5)。

式中()T為修正后的速度二維分量，a 為偏航角的余弦值，b 為偏航角的正弦值，(v xvy)T為修正前的速度二維分量。

5 結(jié)語(yǔ)

本文設(shè)計(jì)了一種基于麥克納姆輪的視覺(jué)識(shí)別與搬運(yùn)系統(tǒng)，經(jīng)過(guò)綜合設(shè)計(jì)硬件、結(jié)構(gòu)、軟件三個(gè)部分，參照buck電路、H 橋等經(jīng)典電路，使用PID 控制算法、目標(biāo)檢測(cè)算法、CNN 神經(jīng)網(wǎng)絡(luò)，結(jié)合Solidworks、OPENMV IDE 等軟件進(jìn)行設(shè)計(jì)。經(jīng)過(guò)測(cè)試，本系統(tǒng)完成對(duì)所有物品的搬運(yùn)任務(wù)，可用于物流分揀領(lǐng)域，具有較高的運(yùn)行效率以及較高的分類正確率。