基于SSD的碼垛機(jī)械臂關(guān)節(jié)速度糾偏運(yùn)動(dòng)規(guī)劃

王清清，奚 琳

(1.安徽信息工程學(xué)院機(jī)械工程學(xué)院，安徽蕪湖 241000；2.安徽工程大學(xué)機(jī)械工程學(xué)院，安徽蕪湖 241000)

1 引言

隨著全球經(jīng)濟(jì)的蓬勃發(fā)展，人們對(duì)產(chǎn)品的精度要求也越來(lái)越高，加上人工成本的不斷提升，機(jī)器人加工已成為大中型企業(yè)首選的加工方式[1-3]。隨著機(jī)器人的逐漸興起，碼垛成為包裝過(guò)程中必不可少的流程，是物流自動(dòng)化技術(shù)的新興技術(shù)。碼垛作業(yè)在現(xiàn)企業(yè)物流管理中占據(jù)十分重要的地位。早在80年代初期，一些工業(yè)發(fā)達(dá)的國(guó)家已經(jīng)在自動(dòng)化、機(jī)械化碼垛方面取得了較好的發(fā)展，實(shí)現(xiàn)自動(dòng)化碼垛能夠提升物流速度，保障員工安全，減少物料浪費(fèi)[4]。為了確保碼垛機(jī)械臂保質(zhì)保量完成工作任務(wù)，機(jī)械臂需要根據(jù)一定運(yùn)動(dòng)軌跡進(jìn)行操作，軌跡規(guī)劃是碼垛機(jī)械臂完成任務(wù)和運(yùn)行性能的基礎(chǔ)[5]。為此相關(guān)研究人員進(jìn)行了很多研究，并取得了一定成果。

余亮等人[6]提出研究局部閉鏈碼垛機(jī)器人運(yùn)動(dòng)學(xué)分析及軌跡規(guī)劃方法。該方法利用D-H法獲取局部閉鏈碼垛機(jī)器人運(yùn)動(dòng)學(xué)正逆解，根據(jù)任務(wù)需要，通過(guò)可達(dá)工作空間完成碼垛機(jī)器人路徑規(guī)劃，采取將規(guī)劃結(jié)果與運(yùn)動(dòng)學(xué)逆解相結(jié)合的方式，確定機(jī)器人各關(guān)節(jié)位移和速度之間的時(shí)變關(guān)系，達(dá)到縮短運(yùn)行周期，提升工作效率，但該方法針對(duì)碼垛機(jī)械臂關(guān)節(jié)速度糾偏存在的影響因素分析甚少，存在一定局限性。張玲[7]提出基于三次樣條曲線的碼垛機(jī)器人平滑軌跡規(guī)劃方法。針對(duì)碼垛機(jī)器人運(yùn)行軌跡，采用三次樣條曲線進(jìn)行規(guī)劃，將軌跡曲線的參數(shù)連續(xù)性進(jìn)行結(jié)合，實(shí)現(xiàn)碼垛機(jī)器人平滑運(yùn)行軌跡，保證其工作過(guò)程中的運(yùn)動(dòng)平穩(wěn)性和連續(xù)性。該方法可有效控制碼垛機(jī)器人的穩(wěn)定運(yùn)行，但該方法在遇到障礙物時(shí)壁障性能較差。

基于此，本文提出基于SSD的碼垛機(jī)械臂關(guān)機(jī)速度糾偏運(yùn)動(dòng)規(guī)劃方法。借助SSD算法對(duì)研究目標(biāo)進(jìn)行自動(dòng)識(shí)別，并通過(guò)利用直流電機(jī)PWM糾偏調(diào)速控制模型和PID糾偏控制算法有效實(shí)現(xiàn)速度偏差的校正。與傳統(tǒng)方法相比，本文方法有效提升碼垛機(jī)械臂關(guān)節(jié)運(yùn)動(dòng)精度和平穩(wěn)性，成功實(shí)現(xiàn)任務(wù)的軌跡規(guī)劃。

2 碼垛機(jī)械臂關(guān)節(jié)速度糾偏運(yùn)動(dòng)規(guī)劃

2.1 基于SSD算法的目標(biāo)識(shí)別

SSD算法通過(guò)不同比例候選框和特征圖預(yù)測(cè)目標(biāo)物體，利用全卷積代替全連接層，有效提升算法精度和速度[8]。正樣本為準(zhǔn)確與真實(shí)框相匹配的設(shè)定，負(fù)樣本為其它框設(shè)定，其位置、置信度和反向傳播三種損失的加權(quán)總和為算法的損失值，網(wǎng)絡(luò)結(jié)構(gòu)如圖1所示。

圖1 SSD網(wǎng)絡(luò)結(jié)構(gòu)圖

以VGG 16為算法基準(zhǔn)網(wǎng)絡(luò)，將該全連接層轉(zhuǎn)變成卷積層。對(duì)參數(shù)為2×2的最大池化層轉(zhuǎn)變?yōu)?×3的最大池化，池化層的轉(zhuǎn)變能夠帶動(dòng)感受野發(fā)生改變，通過(guò)帶孔卷積對(duì)卷積核進(jìn)行膨脹處理，使感受野同時(shí)增大，豐富卷積之后的圖像信息。

輸入分辨率為300×300的圖片，3×3的卷積核，滑動(dòng)步長(zhǎng)(水平方向和垂直方向)為1，輸出150×150×64，該輸出通過(guò)對(duì)2×2的池化層進(jìn)行池化操作獲取。

匹配真實(shí)框與默認(rèn)框，建立真實(shí)框與默認(rèn)框的關(guān)系。設(shè)定兩框之間的閾值比默認(rèn)框高，促使產(chǎn)生若干個(gè)重疊的默認(rèn)框，便于不再只針對(duì)最大重疊默認(rèn)框進(jìn)行選擇，保證置信度的精準(zhǔn)。

假設(shè)第i個(gè)默認(rèn)框匹配p類別的第j個(gè)真實(shí)框，其中，大于默認(rèn)框的閾值有若干個(gè)，位置損失加上置信損失為目標(biāo)損失函數(shù)，即

(1)

其中，默認(rèn)框數(shù)量為N，位置損失為L(zhǎng)loc(x，l，g)，置信損失為L(zhǎng)conf(x，c)，α為1，位置損失表示真實(shí)框與損失框之間的損失，即

(2)

置信度損失為

(3)

SSD算法不受單尺度檢測(cè)的限制，對(duì)于邊框類別分?jǐn)?shù)和位置偏移的預(yù)測(cè)是通過(guò)小卷積預(yù)測(cè)器完成，可顯著提升檢測(cè)精度。同時(shí)，在單個(gè)網(wǎng)絡(luò)中封鎖住所有計(jì)算，確保在保證精度的基礎(chǔ)上對(duì)整體實(shí)時(shí)性具有提升作用。

2.2 碼垛機(jī)械臂關(guān)節(jié)速度糾偏PID控制

2.2.1 直流電機(jī)調(diào)速控制

以SSD算法識(shí)別結(jié)果為基礎(chǔ)，采用PLC為控制器，針對(duì)碼垛機(jī)械臂的電機(jī)轉(zhuǎn)速和電流采用PWN調(diào)速方式進(jìn)行PID控制[9]。調(diào)速模型如圖2所示。

圖2 直流電機(jī)動(dòng)態(tài)PWM調(diào)速模型

對(duì)于外界干擾需要進(jìn)行及時(shí)處理，不然會(huì)引起輸出值變化產(chǎn)生誤差，無(wú)刷直流電機(jī)調(diào)速模型能夠通過(guò)PID控制實(shí)現(xiàn)一定程度的干擾調(diào)節(jié)，有效減少轉(zhuǎn)速因干擾而產(chǎn)生的影響[10]。

2.2.2 PID糾偏實(shí)現(xiàn)方法

采用左輪和右輪轉(zhuǎn)速偏差，用以表示碼垛機(jī)械臂的運(yùn)動(dòng)過(guò)程，PID糾偏控制公式為

(4)

其中，u表示驅(qū)動(dòng)電壓控制電壓，e(k)表示系統(tǒng)偏差，ec表示系統(tǒng)變化變化率，Kp表示比例系數(shù)，KI表示積分系數(shù)，KD表示微分系數(shù)。

由于當(dāng)前狀態(tài)決定本次輸出值，在計(jì)算過(guò)程中，疊加系統(tǒng)偏差進(jìn)行求和，導(dǎo)致工作量偏大，為了減少偏大的瞬間微分值，通過(guò)監(jiān)看測(cè)定值進(jìn)行PID指令控制，如圖3所示。

圖3 PID指令控制框圖

PID表示公式為：

(5)

其中，u(k)表示輸出值，uPV(k)表示測(cè)定值，uSV(k)表示目標(biāo)值。

在PID控制器中，輸入SV偏差和PV偏差，分別設(shè)置比例系數(shù)參數(shù)、積分系數(shù)參數(shù)和微分系數(shù)參數(shù)，對(duì)直流電機(jī)進(jìn)行調(diào)速。根據(jù)測(cè)定值PV的微分狀況，測(cè)定值變化量與車輪速度變化量成正比，當(dāng)測(cè)定值變化量與輸出值成反比時(shí)，利用PWM波形式對(duì)電機(jī)進(jìn)行調(diào)速。

3 碼垛機(jī)械臂關(guān)節(jié)運(yùn)動(dòng)軌跡規(guī)劃實(shí)現(xiàn)

碼垛機(jī)械臂關(guān)節(jié)角利用運(yùn)動(dòng)學(xué)逆解的方式進(jìn)行目標(biāo)位姿與預(yù)定軌跡的求解，結(jié)合2.2小節(jié)的PID機(jī)械臂關(guān)節(jié)速度糾偏方式，共同完成碼垛機(jī)械臂關(guān)節(jié)運(yùn)動(dòng)軌跡規(guī)劃。

3.1 軌跡規(guī)劃法

利用“5-3-5”軌跡規(guī)劃法控制抓取放置過(guò)程中碼垛機(jī)械臂的速度、加速度和精度。具體步驟為：①起點(diǎn)→抬升點(diǎn)，采用五次多項(xiàng)式進(jìn)行規(guī)劃；②抬升點(diǎn)→下降點(diǎn)，采用三次多項(xiàng)式進(jìn)行規(guī)劃；③下降點(diǎn)→終點(diǎn)，采用采用五次多項(xiàng)式進(jìn)行規(guī)劃。各個(gè)關(guān)節(jié)的目標(biāo)軌跡多項(xiàng)式方程為

(6)

其中，θ1、θ2和θ3分別表示Ⅰ軸、Ⅱ軸和Ⅲ軸的關(guān)節(jié)變量，ζ表示軌跡。

完全滿足以下四個(gè)條件才能體現(xiàn)四線運(yùn)動(dòng)連續(xù)性，即：

①軌跡中任何一段初始和末端兩個(gè)位置需要與四個(gè)(A、B、C、D)關(guān)節(jié)角位置保持一致；

②軌跡第一段初始和第三段末端的角速度和角加速度相同，且為0；

③軌跡前后段之間角位置、角加速度和角加速度保持一致；

④根據(jù)關(guān)節(jié)軸的關(guān)節(jié)變量，設(shè)置軌跡ζ1f、ζ2f和ζ3f和所需時(shí)間，分別為2s、4s和2s。

3.2 軌跡規(guī)劃動(dòng)力學(xué)分析

碼垛機(jī)械臂每個(gè)關(guān)節(jié)運(yùn)行需要具有較大的力和力矩，保證關(guān)節(jié)速度和加速度滿足預(yù)期位置精度。利用拉格朗日構(gòu)建系統(tǒng)動(dòng)力學(xué)方程，碼垛機(jī)械臂關(guān)節(jié)速度糾偏運(yùn)動(dòng)力矩公式為

(7)

展開式(7)得

(8)

其中，F(xiàn)ii表示有效慣量，F(xiàn)i表示重力，F(xiàn)iji和Fijj分別表示關(guān)節(jié)i、j之間的耦合慣量和耦合力矩，表示在關(guān)節(jié)i上關(guān)節(jié)j處速度產(chǎn)生的向心力。

每個(gè)關(guān)節(jié)驅(qū)動(dòng)力矩方程為

(9)

(10)

重力項(xiàng)為

(11)

其中，m2、m3分別表示大臂小臂質(zhì)量。li表示相應(yīng)臂的長(zhǎng)度。

設(shè)質(zhì)心在l的中心，將慣性項(xiàng)、科里奧利力和向心力項(xiàng)、重力項(xiàng)的方程帶入式(9)中計(jì)算各關(guān)節(jié)驅(qū)動(dòng)力矩方程，即

(12)

(13)

根據(jù)上述分析，完成了碼垛機(jī)械臂關(guān)節(jié)速度糾偏運(yùn)動(dòng)規(guī)劃。

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

4.1 實(shí)驗(yàn)環(huán)境和參數(shù)設(shè)計(jì)

采用實(shí)物碼垛作業(yè)進(jìn)行速度糾偏運(yùn)動(dòng)規(guī)劃實(shí)驗(yàn)，碼垛機(jī)械臂為某大型公司工程樣機(jī)實(shí)驗(yàn)平臺(tái)，如圖4所示。

圖4 碼垛機(jī)械臂實(shí)驗(yàn)對(duì)象

實(shí)驗(yàn)條件：抓取對(duì)象重量為35 kg，直線上升高度385 mm，下降高度185 mm，機(jī)械臂運(yùn)行參數(shù)如表1所示。

表1 運(yùn)動(dòng)參數(shù)約束

在碼垛機(jī)械臂的運(yùn)行軌跡上設(shè)置磁條軌跡，并在運(yùn)動(dòng)軌跡上安放四個(gè)大小為200 mm×200 mm的白色標(biāo)記作為監(jiān)測(cè)點(diǎn)，保證白色記號(hào)與磁條各自中心相重合，根據(jù)實(shí)際運(yùn)行軌跡在白色標(biāo)記上留下的痕跡作為驗(yàn)證偏差情況。

4.2 結(jié)果分析

為驗(yàn)證本文方法的有效行，實(shí)驗(yàn)中設(shè)置不同偏差比例系數(shù)進(jìn)行實(shí)驗(yàn)，以磁條中心為界，偏左側(cè)為正值，偏右側(cè)為負(fù)值，測(cè)試點(diǎn)與標(biāo)準(zhǔn)軌跡間的偏差如表2所示。

表2 偏差距離表

由表2可知，當(dāng)偏差比例系數(shù)為4000時(shí)，本文方法能夠?qū)⑵羁刂圃凇?mm之內(nèi)，根據(jù)多次實(shí)驗(yàn)過(guò)程中實(shí)際運(yùn)行軌跡，基本與標(biāo)準(zhǔn)軌跡能夠保持一致，證明本文方法在碼垛機(jī)械臂關(guān)機(jī)速度糾偏運(yùn)動(dòng)中的控制效果較好，具有較好的應(yīng)用效果和適用性。

針對(duì)Ⅰ軸和Ⅱ軸關(guān)節(jié)角、角速度和角加速度的軌跡曲線，通過(guò)四線運(yùn)動(dòng)連續(xù)性條件和MATLAB軟件編程繪制，具體軌跡曲線如圖5所示。

圖5 Ⅰ軸和Ⅱ軸軌跡曲線

由圖5可知，在運(yùn)動(dòng)控制過(guò)程中所有關(guān)節(jié)角、角速度和角加速度軌跡曲線均連續(xù)且光滑，可有效避免關(guān)節(jié)變向運(yùn)動(dòng)的沖擊，確保碼垛機(jī)械臂平穩(wěn)運(yùn)行，有效提升各關(guān)節(jié)運(yùn)行學(xué)性能，提升碼垛機(jī)械臂工作效率。

為了進(jìn)一步驗(yàn)證所提方法的有效性，實(shí)驗(yàn)對(duì)比本文方法、文獻(xiàn)[6]局部閉鏈碼垛機(jī)器人運(yùn)動(dòng)學(xué)分析及軌跡規(guī)劃方法和文獻(xiàn)[7]基于三次樣條曲線的碼垛機(jī)器人平滑軌跡規(guī)劃方法，對(duì)目標(biāo)進(jìn)行識(shí)別。在碼垛機(jī)械臂運(yùn)動(dòng)軌跡前置放規(guī)格為600×300×300mm的50個(gè)紙箱和一個(gè)400×350×280mm的紙箱作為識(shí)別對(duì)象，以成功抓取400×350×280mm的紙箱為目標(biāo)，目標(biāo)識(shí)別速度對(duì)比如圖6所示。

圖6 目標(biāo)識(shí)別速度對(duì)比

由圖6可知，在碼垛機(jī)械臂目標(biāo)識(shí)過(guò)程中本文方法的平均識(shí)別時(shí)間是±0.04秒，文獻(xiàn)[6]方法的平均識(shí)別時(shí)間為±0.12秒，文獻(xiàn)[7]方法的平均識(shí)別時(shí)間為±0.10秒，根據(jù)識(shí)別時(shí)間驗(yàn)證了本文方法在目標(biāo)識(shí)別速度具有較明顯優(yōu)勢(shì)，表明本文方法控制下的碼垛機(jī)械臂的任務(wù)完成度和工作效率得到有效提升。

采用三種方法進(jìn)行對(duì)600×300×300mm的紙箱進(jìn)行碼垛實(shí)驗(yàn)，三種方法碼垛用時(shí)和抓取精準(zhǔn)度對(duì)比結(jié)果如表3所示。

表3 碼垛用時(shí)和抓取精準(zhǔn)度對(duì)比結(jié)果統(tǒng)計(jì)表

由表3可知，在堆垛過(guò)程中，本文方法與兩種對(duì)比方法相比效率分別提升了33.6%和26.8%，抓取精度分別提高6.7%和10.6%，表明采用本文方法對(duì)碼垛機(jī)械臂關(guān)節(jié)進(jìn)行糾偏運(yùn)動(dòng)規(guī)劃能夠有效提高該機(jī)器人的工作效率，具有較高的實(shí)際應(yīng)用價(jià)值。

5 結(jié)論

本文提出基于SSD的碼垛機(jī)械臂關(guān)節(jié)速度糾偏運(yùn)動(dòng)規(guī)劃方法。利用SSD算法進(jìn)行對(duì)目標(biāo)障礙物進(jìn)行自主智能識(shí)別，為后期碼垛機(jī)械臂工作提供運(yùn)動(dòng)方向，完成碼垛機(jī)械臂關(guān)節(jié)速度糾偏運(yùn)動(dòng)規(guī)劃。采用本文方法能夠?qū)⑵羁刂圃凇?mm之內(nèi)，且碼垛耗時(shí)較短。