基于關節位置約束的倉儲運輸機械臂分揀控制

張 潔

（國網北京物資公司，北京 100054）

近年來，國網物資公司不斷探索數字化倉儲管理新模式，以高標準建設智慧倉儲工程，提高儲備物資“共享共用、統籌調配、庫存合理”水平，努力實現倉庫智能化運營，驅動供應鏈業務數字化、智慧化轉型[1]。隨著國網物資公司業務的增長，其倉儲運輸的工作量也達到了超高水平[2-3]。傳統的人工分揀和整理包裹的模式已不能適應日益增長的倉儲運輸分揀需要，因此，電力設備的自動分揀技術的研究、控制和應用顯得尤為重要[4]。要實現高品質的運輸服務，必須提高貨物倉儲運輸的吞吐量[5]、分揀速度和精確度。為滿足廣大用戶的精確配送要求，使運輸服務更好地贏得顧客青睞，必須對配送分揀體系進行優化[6]，并對智能分揀包裹以及機器臂的控制實行研究和優化設計，以減少成本，增加安全、穩定，完成模式識別與智能技術相結合的運輸配送自動化控制系統，實現了對物流系統的自動分揀控制[7]。

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

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

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

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

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

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

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

其雅各比矩陣為

雅各比矩陣對時間的導數為

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

2 倉儲運輸貨物特征采樣

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

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

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

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

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

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

3 關節位置約束下倉儲運輸機械臂分揀控制

倉儲運輸機械臂的物理關節位置約束公式為

式中：qi描述的是關節i 的位置描述的是關節i 的正與負約束。在機器臂分揀過程中，通常要把關節位置層次上的關節位置約束轉換為關節速度水平約束，為此本文提出了連續系統在關節速度水平上的位置約束：

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

式中：Δqi描述的是關節i 在無窮時間Δt 位置增量，聯立公式（17），能夠得到：

式中：ci=1/Δt。只要離散系統滿足ci≤1/T，就能保證機械臂關節的位置約束，并且離散系統的約束和連續約束是一樣的。

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

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

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

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

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

將式（25）轉換，可得系統誤差公式：

此時系統誤差分揀指數為收斂狀態。

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

添加Caccavale 方法來設定衰減項：

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

4 實驗分析

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

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

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

4.1 關節位置約束對機械臂動力學特性的影響

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

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

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

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

圖3 不約束與約束關節角度Fig.3 Unconstrained and constrained joint angles

4.2 不同分揀控制策略影響

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

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

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

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

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

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

圖6 不同分揀速度策略下關節變形角度Fig.6 Joint deformation angle under different sorting speed strategies

5 結語

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