楊 藝，王宏彬，張栗寅，劉 超,，盛鑫軍*

(1.上海交通大學(xué)機(jī)械與動力工程學(xué)院，上海 200240；2.海安上海交通大學(xué)智能裝備研究院，海安 226601)

中國白酒源遠(yuǎn)流長，在千年的發(fā)展過程中，白酒釀造工藝不斷開拓創(chuàng)新，形成了包含多種香型的釀酒理論[1-2]。上甑工藝作為釀酒過程核心工藝環(huán)節(jié)之一，直接決定了出酒率和白酒品質(zhì)。在上甑過程中，工人師傅將原料分層、均勻、疏松地鋪撒在甑鍋內(nèi)，使蒸汽從底部一層一層往上滲透將酒甑中的酒精蒸餾出來，并在鋪撒過程中保證不跑汽、不壓汽[3]。在現(xiàn)有的絕大部分白酒酒廠中，上甑過程仍需要大量人工完成，通常在上甑過程中，需要兩名上甑工人配合通過一個小簸箕將容積接近3 m3的甑鍋用甑料填滿，其中一個工人負(fù)責(zé)用鐵锨將料堆中的甑料鏟入簸箕中，另一個工人將簸箕中的甑料分層均勻地鋪灑到甑鍋中，在35～40 min的工藝過程中工人們往往要重復(fù)成百上千次鏟料、鋪料的動作，搬運(yùn)的甑料總質(zhì)量超過2 t。近年來，隨著高精度、高負(fù)載工業(yè)機(jī)器人的發(fā)展，工業(yè)機(jī)器人替代人工完成上甑過程成為了可能。

機(jī)器人在釀造行業(yè)的應(yīng)用目前正處于起步階段，相關(guān)研究成果較少，現(xiàn)有的部分釀造上甑機(jī)器人往往根據(jù)鋪滿每層甑料設(shè)計鋪料結(jié)構(gòu)，忽視了人工上甑工藝的傳承。例如，文獻(xiàn)[4-5]中研發(fā)的新型鋪料機(jī)構(gòu)以及專利[6]中的上甑機(jī)器人，上甑動作按照預(yù)定的軌跡執(zhí)行，二者均能完成基本上甑功能，然而同人工上甑工藝區(qū)別較大，在部分區(qū)域由于姿態(tài)限制難以完成漏汽補(bǔ)料任務(wù)。

基于此，在上甑工藝環(huán)節(jié)，從傳承人工工藝手法出發(fā)，通過對人工上甑過程進(jìn)行動作數(shù)據(jù)采集，提出一種基于樣條曲線-高斯概率模型的泛化方法，得到人工上甑工藝的標(biāo)準(zhǔn)軌跡。通過對軌跡和實際人工上甑過程的分析，規(guī)劃更加貼近人工上甑過程的上甑軌跡，設(shè)計并制作甑料流速可控的末端料斗機(jī)構(gòu)，選用埃夫特ER-210D六軸工業(yè)機(jī)器人作為執(zhí)行器，實現(xiàn)仿人上甑軌跡的機(jī)器人自動上甑過程。在上甑過程中，利用工業(yè)機(jī)器人的動力學(xué)特性得到軌跡最大速度，配合末端料斗機(jī)構(gòu)的甑料流速控制實現(xiàn)每層的給定厚度鋪灑，在保證“輕撒勻鋪”工藝要求的同時，盡可能提高上甑效率。

1 人工上甑工藝泛化與提取

1.1 基于動態(tài)時間規(guī)整算法的時間對齊

在人工上甑的過程中(圖1)，將三顆紅外反射小球固定在簸箕邊緣上，利用如圖2所示的Nokov紅外動作捕捉系統(tǒng)采集上甑過程中簸箕的空間運(yùn)動軌跡。

圖1 上甑動作數(shù)據(jù)采集Fig.1 Motion data acquisition of the caldron

圖2 Nokov紅外動作捕捉系統(tǒng)Fig.2 The Nokov infrared motion capture system

對于人工來說，同一上甑手法的鋪灑動作無法做到每次的軌跡和速度一致，在同樣采樣頻率下采集得到的簸箕數(shù)據(jù)往往存在一定的隨機(jī)性和變形。對于空間上的變形，定義為在空間約束下允許的變形滿足某種概率分布，因此屬于示教學(xué)習(xí)系統(tǒng)模型需要泛化的一部分。對于時間上的變形，其會導(dǎo)致各次示教數(shù)據(jù)的時間序列非完全同步，若不經(jīng)預(yù)處理，直接將時間顯性作為訓(xùn)練數(shù)據(jù)進(jìn)行模型提取，會嚴(yán)重影響模型的精確性。故而，在進(jìn)行空間上的軌跡泛化前，需要對同一手法的多次簸箕軌跡進(jìn)行時序規(guī)整，其本質(zhì)是尋找若干組時序映射使多次示教數(shù)據(jù)中的點對建立對應(yīng)關(guān)系。

令Q={q1,q2,…,qn}為長度為n的參考數(shù)據(jù)，其中qi對應(yīng)參考軌跡某時刻的采樣數(shù)據(jù)，總時長為TQ；令P={p1,p2,…,pm}為長度為m的待規(guī)整數(shù)據(jù)，其中pi對應(yīng)待規(guī)整軌跡某時刻的采樣數(shù)據(jù),總時長為TP。qi與pj的維數(shù)應(yīng)保持一致，兩者在采樣過程中保持采樣頻率恒定。時序統(tǒng)一規(guī)整方法即是尋找限制下的某時序映射關(guān)系：

TP=f(TQ)

(1)

使待規(guī)整軌跡映射后，所有對應(yīng)點的數(shù)據(jù)差在某度量下最?。?/p>

(2)

對于三維空間的位置點，采用位置誤差作為度量標(biāo)準(zhǔn)，則函數(shù)g(x,y)的表達(dá)為

(3)

動態(tài)事件規(guī)整算法(dynamic time warping,DTW)[7]是一種建立時序映射關(guān)系的算法，其算法原理如圖3所示。

圖3 DTW算法原理示意圖Fig.3 The schematic diagram of DTW algorithm

對于兩條長度分別為m和n的軌跡數(shù)據(jù)q1和q2，可構(gòu)建m×n階矩陣M，矩陣中各元素含義為

M(i,j)=g[q1(i),q2(j)]

(4)

即M(i,j)表示q1中第i個數(shù)據(jù)與q2中第j個數(shù)據(jù)在三維空間中距離，而時序規(guī)整的目的是從M(1,1)起，找出一條最短的連續(xù)路徑s。

s=[M(i1,j1),M(i2,j2),…,M(ik,jk)]

(5)

(6)

對于路徑中的任一點M(i,j)，為防止數(shù)據(jù)丟失即保證路徑連續(xù)且方向遞增，則下一組對應(yīng)點有三種可能：M(i+1,j)、M(i,j+1)和M(i+1,j+1)，從i=j=1開始，每次選擇移動到路徑增長量最小的下一個點，一直到i=j=max(m,n)。圖中經(jīng)過的點對代表最后得到的新的配對序列s(k)。動態(tài)時間規(guī)整的物理意義是對于兩條路徑上的點，從第一個點依次開始找出三維空間上距離最短的下一組相鄰點作為新的序列在該時刻的對齊點，其依據(jù)是對于同一種上甑手法的多次動作數(shù)據(jù)，在消除工人上甑速度不一致導(dǎo)致的時序變形后，新的對齊序列在空間上的變形應(yīng)盡量小。

DTW使用的簡單度量函數(shù)g僅假設(shè)距離較近的點有配對關(guān)系，忽視了速度、加速度等信息，在應(yīng)用于振蕩嚴(yán)重的動作數(shù)據(jù)時，容易產(chǎn)生點對錯誤匹配導(dǎo)致奇點問題[8]，還會造成連續(xù)性問題。如圖4所示，A曲線為原始示教軌跡對，B為基于動態(tài)時間歸整算法的軌跡匹配結(jié)果。圖中可知，簡單度量函數(shù)g在振蕩嚴(yán)重的軌跡上引起點對錯誤匹配，而錯誤匹配又引起某軌跡單點對應(yīng)其他軌跡多點，帶來不連續(xù)性問題影響后續(xù)模型提取過程。

圖4 DTW算法缺陷Fig.4 The defects of DTW algorithm

為增加規(guī)整算法對形狀的感知，采用一階導(dǎo)數(shù)替代簸箕軌跡的空間位置數(shù)據(jù)，利用形狀上特征與特征的對應(yīng)來尋找時間規(guī)整軌跡(derivative dynamic time warping,DDTW)[9]。對于長度為nq的動作數(shù)據(jù)序列q，其導(dǎo)數(shù)序列Cq為

2

(7)

Cq(0)=Cq(1),Cq(m)=Cq(m-1)

(8)

為測試算法性能，使用式(9)生成仿真數(shù)據(jù)：

(9)

圖5 第三維噪聲下時序規(guī)整效果Fig.5 The result of time warping under the influence of noise in the 3rd dimension

1.2 基于高斯概率模型的軌跡泛化方法

對于時序規(guī)整后的簸箕軌跡數(shù)據(jù)，假設(shè)每個時序?qū)?yīng)的數(shù)據(jù)組在空間中符合高斯概率模型，多維高斯概率模型表達(dá)式為

(10)

式(10)中：d為變量維度；μ為n維向量，代表各維變量的均值；Σ為協(xié)方差矩陣，描述各維變量之間的相關(guān)度。假設(shè)泛化提取后的簸箕軌跡為X=F(t)，其中X為6維向量，前三維為每時刻高斯概率模型的均值，其物理意義為泛化后軌跡曲線的空間坐標(biāo)，后三維表示每時刻高斯概率模型的方差，對于k組對齊后的動作數(shù)據(jù)，同一時刻的數(shù)據(jù)組yt為同一高斯概率模型，高斯概率模型的似然概率為

g{yt|u[X1∶3(t)],σ[X4∶9(t)]}=

(11)

假設(shè)對齊后的數(shù)據(jù)序列長度為N，若存在軌跡X使得各時刻高斯概率模型的似然概率和達(dá)到最大，則定義該軌跡為上甑工藝的標(biāo)準(zhǔn)軌跡，泛化的最優(yōu)化問題可表述為

(12)

為使擬合的泛化軌跡光滑柔順，以三次B樣條曲線替代軌跡X進(jìn)行優(yōu)化。B樣條曲線的表達(dá)式為

(13)

式(13)中：Ni,k(u)為基函數(shù)；Pi為第i個控制點。Ni,k(u)利用遞推公式定義為

(14)

(15)

式(15)中：ti為節(jié)點值；T=[t0,ti,…,tL+2k+1]為節(jié)點矢量；L=n-k且節(jié)點是非減數(shù)列。均勻B樣條曲線指節(jié)點沿參數(shù)軸均勻分布，即

用權(quán)重矩陣和控制點矩陣簡化B樣條模型可得

本文提出的脆弱性評估的思想和方法，有一定的創(chuàng)新意義，但在實際操作中還有很多需要改進(jìn)和完善的地方，更重要的是，它還需要與其它方法結(jié)合使用，進(jìn)一步的擴(kuò)展到整個國家受氣候變化影響的脆弱性評估中。由于干預(yù)原因具有不確定性，我們采取的模型單一，可能對結(jié)果造成誤差。

x(t)=A(t)·θ

(16)

式(16)中：x∈RN×d，N表示示教軌跡的點數(shù)，d表示B樣條曲線維度，x表示任意時刻高斯模型參數(shù)的擬合值；A∈RN×p，p表示控制點數(shù)量，A(i,j)表示控制點j在i時刻的權(quán)重，即

A(i,j)=Nj,3(i)

(17)

優(yōu)化問題轉(zhuǎn)化為

(18)

函數(shù)σ(x)將向量轉(zhuǎn)為對稱矩陣，定義為

(19)

優(yōu)化函數(shù)代入式(11)取自然對數(shù)后可等效為

(20)

求偏導(dǎo)：

σT[A(t)·θ4∶9]

(21)

(22)

式(22)中：σ*[A(t)·θ4∶9]表示矩陣σ[A(t)·θ4∶9]的伴隨矩陣，根據(jù)偏導(dǎo)數(shù)求出下降方向，利用最速下降法求解loss函數(shù)最小值，即原優(yōu)化函數(shù)最大值，最終得到泛化軌跡如圖6所示。

圖6 人工上甑軌跡泛化結(jié)果Fig.6 The generalization results of the artificial caldron-feeding trajectories

從圖6可知，人工上甑時由于工人臂展限制，在進(jìn)行遠(yuǎn)端鋪料時，工人往往通過拋灑的方式以離心力將甑料灑到遠(yuǎn)端。各條上甑軌跡泛化結(jié)果在形狀上相似，均為以工人站立點為圓心的類圓弧型軌跡，各條軌跡距離人工站立點的距離不同，距離越遠(yuǎn)代表鋪灑距離越遠(yuǎn)。

2 末端料斗機(jī)構(gòu)與上甑軌跡規(guī)劃

2.1 末端料斗機(jī)構(gòu)設(shè)計

采用機(jī)器人上甑時，末端料斗機(jī)構(gòu)容積、質(zhì)量與人工上甑時使用的簸箕差距大，機(jī)器人的拋灑的結(jié)果與人工相差很大，故本文采用垂直鋪灑的方式實現(xiàn)機(jī)器人上甑，甑料從機(jī)器人末端料斗機(jī)構(gòu)垂直下落鋪灑，并帶動末端料斗機(jī)構(gòu)覆蓋整個上甑平面。從圖7中近端軌跡特征結(jié)合工人實際上甑流程可以看出，外層溜邊鋪灑為以甑鍋中心為圓心的圓弧形軌跡鋪灑，內(nèi)層按距工人的遠(yuǎn)近分為近、中、遠(yuǎn)三層，每次鋪灑的甑料都是以工人站立點為圓心的圓環(huán)區(qū)域，由此，本文將甑鍋內(nèi)任意上甑平面分為如圖7所示8個區(qū)域。機(jī)器人末端料斗機(jī)構(gòu)如圖8所示。

區(qū)域①和⑤為甑鍋外側(cè)鋪灑，區(qū)域②～④和區(qū)域⑥～⑧為內(nèi)層鋪灑。區(qū)域②～④和⑥～⑧的鋪灑方向不同，②～④為逆時針鋪灑，⑥～⑧為順時針鋪灑圖7 甑鍋內(nèi)上甑平面分割Fig.7 The area separation result of the working plane in caldron

圖8 末端料斗機(jī)構(gòu)Fig.8 The hopper mechanism

裝置外側(cè)框架有方形管焊接組成，內(nèi)側(cè)由不銹鋼板包圍形成包圍盒容納甑料，底部為電機(jī)控制的主動傳送帶，料倉內(nèi)安裝電動滾筒焊接形成的釘耙滾筒。甑料密度約為1 000 kg/m3，整體物理性狀為粘稠泥沙狀物料，料斗機(jī)構(gòu)底部與水平方向呈30°，此時甑料與傳送帶間的黏性摩擦力與重力接近抵消，整個機(jī)構(gòu)通過底部主動傳送帶控制甑料的下料速度，滾筒釘耙的作用是打散被壓實的甑料，保證下料順暢、均勻、松散。末端料斗機(jī)構(gòu)總質(zhì)量76 kg，滿載能容納甑料質(zhì)量為35 kg，上甑系統(tǒng)采用埃夫特ER-210D機(jī)器人(最大負(fù)載為210 kg)，末端總質(zhì)量符合機(jī)器人負(fù)載要求。

2.2 末端料斗機(jī)構(gòu)流速控制

末端料斗機(jī)構(gòu)的下料情況由傳送帶轉(zhuǎn)速ω1和滾筒釘耙轉(zhuǎn)速ω2控制。根據(jù)料斗機(jī)構(gòu)的工作原理，末端料斗機(jī)構(gòu)流出的甑料完全由底部傳送帶從料倉中送出，因此單位時間內(nèi)下料總量由ω1控制。甑料從出料口流出時，甑料脫離傳送帶后在重力作用下在近端墜落。滾筒釘耙的主要作用是將壓實的甑料打散。設(shè)料斗機(jī)構(gòu)的下料速度控制函數(shù)為f(ω1)，并通過實驗測定。

進(jìn)行實驗時，控制機(jī)器人將末端裝置以上甑姿態(tài)移動到稱重傳感器和亞克力板組成的測量平臺上方，改變傳送帶電機(jī)轉(zhuǎn)速ω1，記錄不同轉(zhuǎn)速下裝置單位時間內(nèi)下料質(zhì)量如圖9所示。

圖9 下料質(zhì)量流速與傳送帶電機(jī)轉(zhuǎn)速關(guān)系Fig.9 The relationship between the mass flow rate of the hopper mechanism and the speed of the conveyor motor

由實驗結(jié)果可知，料斗機(jī)構(gòu)的下料質(zhì)量流速與傳送帶電機(jī)轉(zhuǎn)速的關(guān)系可擬合為一條直線，直線表達(dá)式為

vm=f(ω1)=1.19ω1+0.29

(23)

2.3 上甑軌跡規(guī)劃

為得到每條鋪灑路徑的笛卡爾姿態(tài)軌跡，以區(qū)域①的鋪灑為例，工作空間的俯視圖及各坐標(biāo)系位置關(guān)系如圖10所示。

圖10 工作空間各坐標(biāo)系相對關(guān)系示意圖Fig.10 Schematic diagram of the relative relationship of each coordinate system in the workspace

坐標(biāo)系{xOy}、{x1O1y1}、{x2O2y2}、{x3O3y3}分別為機(jī)器人基座坐標(biāo)系、機(jī)器人末端法蘭坐標(biāo)系、固結(jié)于末端料斗機(jī)構(gòu)出料口中點的物體坐標(biāo)系和固結(jié)于甑鍋底面圓心的物體坐標(biāo)系。末端料斗機(jī)構(gòu)通過螺釘固定于機(jī)器人末端法蘭盤，坐標(biāo)系{x1O1y1}、{x2O2y2}的相對位姿始終保持不變，三個坐標(biāo)系間的轉(zhuǎn)換關(guān)系為

(24)

(25)

在上甑工作時，末端料斗機(jī)構(gòu)在豎直方向投影為圖10中左圖所示長h寬d的矩形，在進(jìn)行區(qū)域①鋪灑時，末端料斗機(jī)構(gòu)出料口中點O2的運(yùn)行軌跡為與甑鍋同圓心半徑為r的左半圓。為防止碰撞，以末端料斗機(jī)構(gòu)長度h為弦長的圓弧半徑R應(yīng)略小于當(dāng)前甑料面半徑。甑鍋形狀為上直徑2 m、下直徑1.8 m、高度1.2 m的圓錐臺，設(shè)當(dāng)前鋪料面在甑鍋內(nèi)高度為H，甑鍋底面圓心O3在機(jī)器人基座標(biāo)系{xOy}下位置為[XYZ]。從坐標(biāo)系{xOy}到坐標(biāo)系{x2O2y2}的坐標(biāo)變換分為三步，第一步將坐標(biāo)系{xOy}平移到運(yùn)動路徑的圓弧r上，第二步坐標(biāo)系{xOy}繞x軸旋轉(zhuǎn)180°使z軸向下，第三步坐標(biāo)系{xOy}繞z軸旋轉(zhuǎn)α使料斗不與甑鍋碰撞，則坐標(biāo)轉(zhuǎn)換矩陣可表示為

θ∈[0,π]

(26)

(27)

(28)

(29)

3 機(jī)器人上甑厚度控制策略

上甑機(jī)器人末端料斗機(jī)構(gòu)由滾筒和底部傳送帶配合送料，鋪料的定厚控制有兩種策略，第一種是在鋪料過程中充分發(fā)揮機(jī)器人性能，使機(jī)器人在鋪料軌跡中達(dá)到時間最優(yōu)，通過不斷調(diào)節(jié)末端料斗機(jī)構(gòu)下料速度配合機(jī)器人實現(xiàn)定厚鋪灑。第二種是機(jī)器人在執(zhí)行各條鋪料軌跡時采用固定的笛卡爾速度勻速鋪灑，對該過程進(jìn)行動力學(xué)分析，在滿足機(jī)器人載荷的條件下找出最大的笛卡爾速度，然后依據(jù)該笛卡爾速度結(jié)合末端料斗機(jī)構(gòu)的出料控制函數(shù)找到合適的出料速度，在不需要頻繁改變末端機(jī)構(gòu)的電機(jī)轉(zhuǎn)速也可在較短時間內(nèi)實現(xiàn)給定軌跡給定厚度的甑料鋪灑。在實際上甑使用中，第一種策略中，雖然傳送帶和滾筒速度可調(diào)，但由于加減速過程的存在無法做到及時矯正下料速度，難以配合機(jī)器人不斷變化的笛卡爾速度實現(xiàn)給定厚度的鋪灑，因此本文采用第二種策略進(jìn)行作業(yè)研究。

基于2.3節(jié)計算的機(jī)器人笛卡爾鋪灑軌跡，現(xiàn)首先建立機(jī)器人的運(yùn)動學(xué)模型，通過機(jī)器人逆運(yùn)動學(xué)將笛卡爾軌跡轉(zhuǎn)化為關(guān)節(jié)空間軌跡，然后參照機(jī)器人物理參數(shù)建立機(jī)器人的動力學(xué)模型，從各關(guān)節(jié)的軌跡導(dǎo)出各關(guān)節(jié)的力矩曲線，通過與各關(guān)節(jié)最大力矩限制對比，計算出符合各關(guān)節(jié)力矩限制條件的最大笛卡爾速度。

3.1 上甑機(jī)器人正逆運(yùn)動學(xué)模型

機(jī)器人上甑系統(tǒng)采用埃夫特ER-210D機(jī)器人作業(yè)，其尺寸參數(shù)如圖11所示。

圖11 埃夫特ER-210D機(jī)器人尺寸參數(shù)Fig.11 The parameters of EFORT ER-210D robot

該機(jī)器人為六軸工業(yè)機(jī)器人，末端三傳動軸軸線相交形成球腕。采用改進(jìn)的D-H參數(shù)法[10]建立機(jī)器人各連桿坐標(biāo)系，完成機(jī)器人的運(yùn)動學(xué)建模。坐標(biāo)系建立方法如圖12所示，在此參數(shù)法中使用扭轉(zhuǎn)角αi-1描述關(guān)節(jié)i-1的軸線zi-1到關(guān)節(jié)i的軸線zi繞坐標(biāo)軸xi-1的轉(zhuǎn)角；連桿長度αi-1描述沿著坐標(biāo)軸xi-1將zi-1移動到zi的距離；關(guān)節(jié)偏距di表示沿著軸線zi-1使得xi-1與xi重合的移動距離；關(guān)節(jié)轉(zhuǎn)角θi表示關(guān)節(jié)i繞著zi軸將xi-1旋轉(zhuǎn)到xi的轉(zhuǎn)角。

圖12 改進(jìn)的D-H參數(shù)法Fig.12 The improved D-H parameter method

相鄰連桿坐標(biāo)的齊次轉(zhuǎn)換矩陣可表示為

(30)

(31)

圖13 埃夫特ER-210D機(jī)器人D-H坐標(biāo)模型Fig.13 The D-H coordinate model of EFORT ER-210D robot

表1 埃夫特ER-210D機(jī)器人D-H參數(shù)Table 1 The D-H parameters of ER-210D robot

埃夫特ER-210D機(jī)器人后三軸為軸線相交的球腕模型，圖13中點P4-6為軸線交點，其在空間中的位置完全由機(jī)器人前三軸角度確定。若某時刻機(jī)器人末端在空間中的位置為[xt,yt,zt]，機(jī)械臂末端坐標(biāo)系相對于基座坐標(biāo)系的旋轉(zhuǎn)矩陣為Rt，則后三軸軸線通過的同一點P4-6的位置，可知：

(32)

(33)

(34)

式中：si=sinθ1，ci=cosθ1，sij=sin(θ1+θ1)，cij=cos(θ1+θ1)。由式(32)和式(34)前兩項可求出：

(35)

若sinθ1非0，將θ1代入至y4-6方程兩邊同除sinθ1，若sinθ1為0，則將θ1代入至x4-6方程兩邊同除cosθ1，與z4-6方程組成方程組后可化簡為

Asinθ2+Bcosθ2=C

(36)

(37)

B=2.2z4-7-1.482 8

(38)

0.417 6

(39)

式中：

由輔助角公式可得

(40)

式(40)中：θ2∈[-80°,60°]。將θ2代入式(32)和式(34)中z4-6方程，同理，由輔助角公式可得

(41)

式(41)中：θ3∈[-83°,90°]。該機(jī)器人后三軸旋轉(zhuǎn)矩陣為

(42)

對應(yīng)項連立為9個方程組，參考文獻(xiàn)[11-12]的求解方法，其解為

θ4=atan2[R4-6(1,3),-R4-6(2,3)]

(43)

(44)

θ6=atan2[R4-6(3,1),-R4-6(3,2)]

(45)

式中：R4-6(i,j)表示取R4-6矩陣的i行j列。函數(shù)atan2的定義如下：

(46)

由式(35)～式(46)可知，當(dāng)機(jī)器人末端位姿可知時，可反解出機(jī)器人各關(guān)節(jié)角度，實現(xiàn)機(jī)器人運(yùn)動學(xué)逆解。

3.2 基于機(jī)器人動力學(xué)的機(jī)器人最大速度求解

機(jī)器人牛頓-歐拉動力學(xué)狀態(tài)方程[13]為

(47)

在SolidWorks中，通過對機(jī)器人三維模型的質(zhì)量仿真，得到機(jī)器人的各連桿參數(shù)如表2所示。

表2中，mi為連桿i的質(zhì)量其中連桿6為末端法蘭盤和滿載的料斗機(jī)構(gòu)固定形成的等效連桿，[rx,i,ry,i,rz,i]表示連桿i的質(zhì)心在連桿坐標(biāo)系{i}中的向量，[Ix,I,Iy,I,Iz,i]表示連桿i在質(zhì)心坐標(biāo)系中的慣性矩。牛頓-歐拉動力學(xué)逆解的過程[14]為先從基座到末端工具坐標(biāo)系推導(dǎo)各連桿質(zhì)心的角速度/角加速度、速度/線加速度，根據(jù)牛頓第二定律建立各連桿的受力方程，然后從末端工具坐標(biāo)系向基座逆推，依次求解各關(guān)節(jié)向下一連桿所需提供的力矩。借助MATLAB中的Robotics Toolbox工具箱[15]完成機(jī)器人的動力學(xué)逆解問題。

表2 埃夫特ER-210D工業(yè)機(jī)器人連桿物理參數(shù)Table 2 The physical parameters of each link in ER-210D robot

基于2.3節(jié)中求出的上甑機(jī)器人在不同區(qū)域鋪灑作業(yè)時機(jī)器人末端工具坐標(biāo)系的笛卡爾位姿軌跡，根據(jù)位姿軌跡和機(jī)器人的運(yùn)動學(xué)逆解模型求解出各軌跡對應(yīng)的關(guān)節(jié)空間軌跡。以鋪灑區(qū)域①為例，機(jī)器人的末端的位姿軌跡方程可由式(24)～式(29)表述，對該軌跡中的各位姿進(jìn)行反解，各關(guān)節(jié)的角度變化曲線如圖14(d)所示。設(shè)定末端料斗機(jī)構(gòu)鋪灑時的速度恒定為0.14 m/s，通過動力學(xué)反解模型解出該軌跡上甑過程中各關(guān)節(jié)的力矩曲線如圖14(b)和圖14(c)所示。

圖14 速度0.14 m/s動力學(xué)逆解結(jié)果Fig.14 The inverse solution of robot dynamics at the speed of 0.14 m/s

機(jī)器人各關(guān)節(jié)軸線，即各連桿坐標(biāo)系的z軸，從圖13的D-H模型中可以看出，機(jī)器人1軸軸線為豎直方向，主要承受后續(xù)結(jié)構(gòu)的重力彎矩，在上甑工作時其力矩在前三軸中屬于較低水平。機(jī)器人的2軸提供機(jī)器人在豎直平面內(nèi)運(yùn)動的大部分扭矩，通常在前三軸中為最大。而3軸提供了后續(xù)結(jié)構(gòu)在豎直平面的部分扭矩，其扭矩位于二者之間。機(jī)器人4～6軸的主要作用是調(diào)節(jié)末端姿態(tài)，力臂較小，因此在姿態(tài)變化并不劇烈的作業(yè)中所提供的扭矩往往遠(yuǎn)小于前3軸。圖14中曲線與實際情況相符，說明了機(jī)器人動力學(xué)反解模型的合理性。

從埃夫特ER-210D機(jī)器人手冊可知，各軸的最大可提供力矩(N·m)為

(48)

針對不同的末端鋪料速度，計算出每個軸的最大負(fù)載比，即過程中所需最大力矩與最大可提供力矩的比值，各軸的最大負(fù)載比與末端速度的關(guān)系如圖15所示。

圖15 區(qū)域1鋪灑速度與各軸最大負(fù)載比關(guān)系圖Fig.15 The relationship between the maximum load ratio of each axis and spreading speed in area 1

以最大負(fù)載比不超過80%為安全限制條件，可以得到區(qū)域一上甑軌跡的最大速度為0.4 m/s。按照上述過程對其余7個區(qū)域的上甑軌跡進(jìn)行計算，得到所有軌跡末端料斗機(jī)構(gòu)的最大速度(m/s)為

(49)

3.3 給定厚度鋪料控制策略

對于軌跡i，設(shè)末端料斗機(jī)構(gòu)的速度為v，末端出料口寬度為l=0.4 m，甑料密度ρ≈1 000 kg/m3，則在時間dt內(nèi)，由出料平衡可得

vdtlhρ=f(ω1)dt

(50)

(51)

鋪料厚度h的控制函數(shù)如式(23)所示，每條鋪料軌跡的最大末端料斗機(jī)構(gòu)速度v由式(49)可知。兩式聯(lián)立，即可解出每個區(qū)域?qū)崿F(xiàn)任意鋪料厚度所需的末端料斗機(jī)構(gòu)傳送帶電機(jī)轉(zhuǎn)速。

以所需厚度25 mm為例，計算各軌跡所需傳送帶電機(jī)轉(zhuǎn)速(rad/s)分別為

(52)

機(jī)器人按照給定軌跡和速度，末端料斗機(jī)構(gòu)按照給定轉(zhuǎn)速進(jìn)行鋪灑，完成完整一層甑料鋪灑之后，通過固定在末端料斗機(jī)構(gòu)上的紅外測距傳感器測量該層甑料的鋪灑厚度，結(jié)果如圖16所示。

圖16 定厚25 mm實際鋪灑效果Fig.16 The actual paving effects with a fixed thickness of 25 mm

由于末端料斗的流速控制關(guān)系為擬合函數(shù)、料斗的泥沙狀物理特性影響，從圖中可以看出鋪灑出的甑料厚度波動較大，但從整體來看，波動范圍未超過5 mm，厚度平均值為24 mm，厚度控制策略效果顯著。

4 機(jī)器人上甑實驗及數(shù)據(jù)分析

經(jīng)過15次機(jī)器人單機(jī)上甑實驗，每次上甑時間分布如圖17所示。

圖17 上甑實驗時間數(shù)據(jù)記錄圖Fig.17 The record chart of time cost in all caldron feeding experiments

從實驗數(shù)據(jù)可以看出，機(jī)器人單機(jī)完整上甑時間(min)分布在[26.2，31.7]區(qū)間內(nèi)，平均上甑時間為29.04 min，相比與人工上甑時間35 min縮短了18%，證明了機(jī)器人自動上甑效率相對人工上甑效率更高。

以下層甑料鋪灑前鍋內(nèi)甑料高度測量值減去上層甑料鋪灑前鍋內(nèi)甑料高度，可測量每層甑料厚度，取當(dāng)次上甑過程中每層厚度的平均值作為當(dāng)次實驗每層甑料厚度，15次實驗內(nèi)每層甑料厚度變化如圖18所示。

圖18 上甑實驗每層厚度記錄圖Fig.18 The record chart of the thickness of each layer in all caldron feeding experiments

從實驗數(shù)據(jù)可以看出，在15次上甑實驗中平均每層上甑厚度為23.94 mm，與期望厚度25 mm誤差很小。同時，每次實驗的每層甑料厚度也均在人工上甑工藝所要求的20～30 mm范圍內(nèi)。綜上所述，15次上甑實驗證明了該機(jī)器人上甑系統(tǒng)不僅在符合人工上甑工藝要求的條件下提高了上甑效率，而且每層上甑厚度也具有良好的穩(wěn)定性。

5 總結(jié)

面向白酒釀造過程中的上甑環(huán)節(jié)，通過分析上甑師傅典型作業(yè)手法，設(shè)計開發(fā)了一套面向上甑工藝的機(jī)器人作業(yè)系統(tǒng)，并對其關(guān)鍵技術(shù)進(jìn)行了重點研究分析。首先，提出了一種基于樣條曲線-高斯模型的上甑軌跡泛化編碼算法，該算法假設(shè)上甑過程中的重復(fù)動作在任意時刻的數(shù)據(jù)點符合高斯分布，利用三次均勻B樣條曲線表征所有時刻的高斯分布參數(shù)，使用最大似然估計構(gòu)造優(yōu)化得到最終的樣條曲線參數(shù).并通過采集人工上甑的動作數(shù)據(jù)，結(jié)合泛化模型得到人工上甑工藝在空間中的軌跡信息。其次，重點分析人工上甑的工藝特性，不僅對機(jī)器人上甑區(qū)域進(jìn)行了分割，而且設(shè)計了甑料流速可控的末端料斗機(jī)構(gòu)，結(jié)合現(xiàn)場布局對每個區(qū)域的鋪灑軌跡進(jìn)行計算。之后，通過建立機(jī)器人動力學(xué)逆解模型，根據(jù)機(jī)器人各關(guān)節(jié)負(fù)載限制得到每個區(qū)域的最大速度，結(jié)合末端料斗機(jī)構(gòu)的伺服控制制定上甑作業(yè)時鋪灑厚度的控制策略。最后，通過15次上甑實驗數(shù)據(jù)驗證了本文所設(shè)計的機(jī)器人上甑作業(yè)系統(tǒng)的有效性，在保證每層上甑厚度符合工藝的前提下，將每次的上甑時間相比人工縮短了18%。