基于關(guān)節(jié)位置約束的倉儲運輸機械臂分揀控制

張 潔

（國網(wǎng)北京物資公司，北京 100054）

近年來，國網(wǎng)物資公司不斷探索數(shù)字化倉儲管理新模式，以高標準建設(shè)智慧倉儲工程，提高儲備物資“共享共用、統(tǒng)籌調(diào)配、庫存合理”水平，努力實現(xiàn)倉庫智能化運營，驅(qū)動供應(yīng)鏈業(yè)務(wù)數(shù)字化、智慧化轉(zhuǎn)型[1]。隨著國網(wǎng)物資公司業(yè)務(wù)的增長，其倉儲運輸?shù)墓ぷ髁恳策_到了超高水平[2-3]。傳統(tǒng)的人工分揀和整理包裹的模式已不能適應(yīng)日益增長的倉儲運輸分揀需要，因此，電力設(shè)備的自動分揀技術(shù)的研究、控制和應(yīng)用顯得尤為重要[4]。要實現(xiàn)高品質(zhì)的運輸服務(wù)，必須提高貨物倉儲運輸?shù)耐掏铝縖5]、分揀速度和精確度。為滿足廣大用戶的精確配送要求，使運輸服務(wù)更好地贏得顧客青睞，必須對配送分揀體系進行優(yōu)化[6]，并對智能分揀包裹以及機器臂的控制實行研究和優(yōu)化設(shè)計，以減少成本，增加安全、穩(wěn)定，完成模式識別與智能技術(shù)相結(jié)合的運輸配送自動化控制系統(tǒng)，實現(xiàn)了對物流系統(tǒng)的自動分揀控制[7]。

例如，文獻[8]利用差分反饋技術(shù)對分揀控制系統(tǒng)的彈性振動進行抑制，給出了一種全局收斂的分段控制方法，使其擴展到全局，解決了Lyapuov 函數(shù)逆解控制中存在的系統(tǒng)初始誤差在收斂區(qū)內(nèi)的缺點，通過RBN 技術(shù)消除由于未知因素和模型不確定造成的誤差，從而實現(xiàn)分揀控制的軌跡追蹤和輸出約束；文獻[9]提出一種改進的基于增強學(xué)習(xí)的DDPG算法適用于六自由度煤矸石分類機器臂。在煤矸石進入機器臂作業(yè)空間后，基于各傳感器反饋結(jié)果可以對煤矸石判斷，并將一系列的關(guān)節(jié)角度狀態(tài)控制值輸出給相應(yīng)的運動控制器，通過對煤矸石的位置和關(guān)節(jié)角度來控制機器臂移動，從而達到對煤矸石分揀目的。

但上述方法的機械臂控制效果較差，分揀準確性有待提升。因此，本文提出了基于關(guān)節(jié)位置約束的倉儲運輸機械臂分揀控制。通過建立倉儲運輸機械臂模型，優(yōu)化控制矢量，采樣倉儲運輸貨物特征，識別各包裹信息，利用關(guān)節(jié)位置約束控制機械臂，實現(xiàn)精準地分揀，保證工作效率。

1 倉儲運輸機械臂分揀運動建模

首先構(gòu)建三連桿平面機械臂模型，如圖1 所示。

圖1 三連桿平面機械臂Fig.1 Three link planar robotic arm

圖1 中，在平面中具有一個冗余度。OiXiYi（i=1，2，3）是由Denavit-Hartenberg 建立的各個連接坐標系統(tǒng)，每個連接桿的長度分別是l1、l2、l3，終端位置表示為

式中：C、S 分別表示機械臂余弦函數(shù)之和、正弦函數(shù)之和，即：

其雅各比矩陣為

雅各比矩陣對時間的導(dǎo)數(shù)為

在此基礎(chǔ)上，利用雅各比矩陣的偽逆方法計算出機械臂運動公式，基于PD 控制器的負反饋，得出了加速度的反演結(jié)果：

2 倉儲運輸貨物特征采樣

為了實現(xiàn)對倉儲運輸貨物機械臂分揀控制，建立了一個基于SLP（服務(wù)定位協(xié)議）和RFID（射頻標識技術(shù)）的運輸包裹出庫量的時間序列模型，并利用二者獲得包裹特征，從而智能識別包裹與控制機械臂，建立特征采樣模型。

式中：D 表示包裹采樣特征；s 表示包裹；λ 表示識別參數(shù)；Lm表示包裹形狀；Km表示包裹體積；n 表示不為0 的正整數(shù)。

利用標識技術(shù)，對產(chǎn)品的運輸和流通進行了分析，得到了延遲特征量表達式為

運用模糊辨識法對倉庫系統(tǒng)各功能模塊進行模糊增益K=ΔK·Km，ΔK＞0 計算，建立倉儲包裹分揀的規(guī)劃模型：

獲得包裹分揀的出庫數(shù)據(jù)序列為u（n）。在此基礎(chǔ)上，利用模糊追蹤法對倉儲運輸貨物進行分揀，得出了該系統(tǒng)的輸入和輸出之間的關(guān)系：

式中：x（n）表示輸出數(shù)據(jù)序列；an、bn表示約束參量。

3 關(guān)節(jié)位置約束下倉儲運輸機械臂分揀控制

倉儲運輸機械臂的物理關(guān)節(jié)位置約束公式為

式中：qi描述的是關(guān)節(jié)i 的位置描述的是關(guān)節(jié)i 的正與負約束。在機器臂分揀過程中，通常要把關(guān)節(jié)位置層次上的關(guān)節(jié)位置約束轉(zhuǎn)換為關(guān)節(jié)速度水平約束，為此本文提出了連續(xù)系統(tǒng)在關(guān)節(jié)速度水平上的位置約束：

從定理可以看出，在一個連續(xù)的機械臂運行體系中，式（16）完全約束式（15）。機器臂的關(guān)節(jié)約束可以按照原理轉(zhuǎn)換成關(guān)節(jié)速度約束。對于一個離散系統(tǒng)，它的關(guān)節(jié)極限約束用增量的形式來表達：

式中：Δqi描述的是關(guān)節(jié)i 在無窮時間Δt 位置增量，聯(lián)立公式（17），能夠得到：

式中：ci=1/Δt。只要離散系統(tǒng)滿足ci≤1/T，就能保證機械臂關(guān)節(jié)的位置約束，并且離散系統(tǒng)的約束和連續(xù)約束是一樣的。

在關(guān)節(jié)速度約束的臨界點中，由于受時間變化的限制，不能應(yīng)用WLN 算法（加權(quán)最小二乘法）進行求解。必須找到一種將關(guān)節(jié)速度映射到空間中的非齊次映射，并且在空間中關(guān)節(jié)速度約束的表達式是不變的，因此使用WLN 法進行求解。本文提出了一種改進的關(guān)節(jié)概念，即真正的關(guān)節(jié)空間是由以下轉(zhuǎn)換實現(xiàn)的：

用時間變化的門限關(guān)節(jié)速度約束代替公式（20），能夠獲得修改的關(guān)節(jié)速度約束：

在實際的關(guān)節(jié)空間中，將具有時間變化的關(guān)節(jié)速度約束的逆運動學(xué)問題，轉(zhuǎn)化為具有固定門限的關(guān)節(jié)速度約束。利用WLN 法可以解決這個問題。二階閉環(huán)的分揀逆運動學(xué)公式為

式中：W1描述的是對角權(quán)值矩陣；T 表示周期表示期望加速度。假設(shè)加權(quán)矩陣表達式為

式中：ε1描述的是關(guān)節(jié)避障區(qū)域?qū)挾?，?＞0，在修正關(guān)節(jié)速度逼近臨界點時，可以降低對應(yīng)關(guān)節(jié)的重量，從而可以有效地抑制修正關(guān)節(jié)的速度。在修正關(guān)節(jié)轉(zhuǎn)速偏離修正關(guān)節(jié)速度極限時，通過調(diào)節(jié)重量消除對應(yīng)關(guān)節(jié)的動作極限，以保證關(guān)節(jié)位置約束是正確的。將公式（20）微分代入公式（23），能夠獲得實際的真實關(guān)節(jié)空間閉環(huán)控制規(guī)律：

將式（25）轉(zhuǎn)換，可得系統(tǒng)誤差公式：

此時系統(tǒng)誤差分揀指數(shù)為收斂狀態(tài)。

在無冗余的機器臂中，因為關(guān)節(jié)約束，W1會發(fā)生缺秩，所以雅克比矩陣是非奇異性時，仍然會引起算法的奇異，從而使得JW1JT不滿足的秩增加，公式（25）得出的關(guān)節(jié)加速度達到無限大，超過了電機的有效輸出。針對機械臂在雅克比近奇異性或近似奇異性的情況，采用帶有阻尼項的閉環(huán)控制方法：

添加Caccavale 方法來設(shè)定衰減項：

式中：σmin描述的是極小奇異數(shù)；′o 描述的是避障范圍；λmax描述的是極大衰減系數(shù)。

4 實驗分析

實驗以國網(wǎng)北京物資公司為例，對本文提出的倉儲運輸機械臂分揀控制性能進行分析。物資倉儲運輸任務(wù)為將50 套配電箱運輸至某配電網(wǎng)施工現(xiàn)場，倉庫需進行自動揀貨、出庫裝車、物資發(fā)運等工作。

為了驗證所提方法對機械臂分揀控制有效性，進行實驗。機械臂主要物理參數(shù)如表1 所示。

表1 機械臂及目標物理參數(shù)Tab.1 Mechanical arm and target physical parameters

4.1 關(guān)節(jié)位置約束對機械臂動力學(xué)特性的影響

驗證倉儲運輸機械臂關(guān)節(jié)位置在約束和無約束條件下的分揀動態(tài)，采用-0.06 m/s 的勻速分揀方案，關(guān)節(jié)參數(shù)：100 N·m/rad、800 N·m/rad；動力學(xué)特性參數(shù)：接觸碰撞剛性系數(shù)為103 N/m，接觸碰撞衰減為0，關(guān)節(jié)速度衰減為0.1；初始參數(shù)：所有關(guān)節(jié)角、關(guān)節(jié)角速度都是0，分揀對象初始速度是0。

不約束與約束的分揀碰撞力曲線，如圖2 所示。所謂的碰撞力，就是在慣性作用下，機器手臂末端和目標對象之間的撞擊力。比較可以看出，當機器臂的末端抓手與目標對象發(fā)生碰撞時，約束的撞擊次數(shù)比不約束要少，并且總體上的沖撞強度要小。

圖2 不約束與約束分揀碰撞力Fig.2 Unconstrained and constrained sorting collision forces

分揀時關(guān)節(jié)角度不受約束和約束的電機角度，如圖3 所示。通過比較可以看出，在分揀初期曲線是比較平穩(wěn)的；在分揀最后，曲線就會出現(xiàn)抖動，這表明不進行關(guān)節(jié)位置約束分揀終端的振動干擾很大。但總體來看關(guān)節(jié)位置約束的振動相對較小，控制效果更好。

圖3 不約束與約束關(guān)節(jié)角度Fig.3 Unconstrained and constrained joint angles

4.2 不同分揀控制策略影響

在考慮關(guān)節(jié)位置約束的條件下，對機器臂的分揀動態(tài)進行了研究，得到了勻速速率為0.06 m/s 和加速度為-0.024 m/s2，機器臂分揀跟蹤曲線變化。

在加速度和勻速度兩種分揀控制方案中目標的相對位置，如圖4 所示。比較可以看出，兩種分揀控制機器手臂的末端抓手都與目標對象發(fā)生了多次碰撞，但是，在加速中，第一次碰撞比勻速碰撞的次數(shù)和振幅都很小，沒有出現(xiàn)過多波動，相對控制較好。

圖4 不同分揀速度策略下目標相對位置Fig.4 Relative position of targets under different sorting speed strategies

在加速度分揀控制下的碰撞圖，如圖5 所示。當機器臂的末端抓與分揀對象發(fā)生碰撞時，加速運動的分揀控制會延遲大約0.5 s 的時間，而更大的碰撞力峰值則會降低，但是總體的撞擊力會更小。

圖5 加速分揀控制碰撞圖Fig.5 Accelerated sorting control collision diagram

在使用加速度和勻速度分揀的情況下的關(guān)節(jié)變形角度，如圖6 所示。比較可以看出，機械臂的加速分揀控制的關(guān)節(jié)變形角峰值接近于勻速分揀控制，但總體上來說加速度更小，并且振動頻率更低。

圖6 不同分揀速度策略下關(guān)節(jié)變形角度Fig.6 Joint deformation angle under different sorting speed strategies

5 結(jié)語

國網(wǎng)物資公司倉儲運輸量較大，為減少運輸成本，提高運營效率，必須從運輸中心的內(nèi)部分揀入手，因此本文提出了基于關(guān)節(jié)位置約束的倉儲運輸機械臂分揀控制。通過倉庫分揀機器人手臂建模，對其進行最優(yōu)控制，將待運輸物資設(shè)備特性取樣識別，并利用關(guān)節(jié)位置約束對機械臂控制，以確保分揀作業(yè)效率。實驗結(jié)果表明該方法能夠較好實現(xiàn)控制分揀，保證分揀的準確性。