許高平, 孫以澤

(東華大學(xué) 機(jī)械工程學(xué)院, 上海 201620)

隨著機(jī)器人技術(shù)與紡織工業(yè)智能化的不斷發(fā)展,利用工業(yè)機(jī)器人替代人工完成各類典型的紡織加工技能作業(yè)已成為紡織行業(yè)的新趨勢,比如紡紗領(lǐng)域的物料轉(zhuǎn)運機(jī)器人、細(xì)紗自動落紗機(jī)器人、紗線整理打包入庫機(jī)器人[1];織造領(lǐng)域的筒子架換運機(jī)器人[2]、經(jīng)軸上落機(jī)器人[3];編織領(lǐng)域的芯模牽引機(jī)器人[4]。以上機(jī)器人應(yīng)用多為固定點位間的搬運或?qū)傂晕矬w的抓取、牽引等,而將剛性的工業(yè)機(jī)器人適配于紡織柔性體加工制造,完成技能性更強(qiáng)的擬人化作業(yè)仍存在挑戰(zhàn)。

在織造整經(jīng)工藝中,卷裝紗線作為織材投入織造加工前,從大量的卷裝表面理出線頭并牽引至固定點位便是一項技能性要求極高的工作,目前紡織工廠均采用人工操作,該操作過程繁瑣費時,人工成本高,生產(chǎn)效率低,且操作工直接接觸導(dǎo)致卷裝被污染的風(fēng)險較大。因此,本文在利用自動找線頭裝置[5]的基礎(chǔ)上,采用機(jī)械臂對紗架上的卷裝預(yù)留線頭進(jìn)行抓取與牽引,通過提出一種集成移動機(jī)械臂運動規(guī)劃策略的卷裝紗線牽引操縱框架,繞開環(huán)境障礙的同時防止紗線被過度拉伸,實現(xiàn)卷裝紗頭的自動化梳理。

紗線屬于可變形線性體(DLOs),當(dāng)前對DLOs的研究主要有2個方向:高效精確的形狀建模和穩(wěn)健通用的操縱規(guī)劃[6]。DLOs的動態(tài)建模是機(jī)器人對其精確控制的基礎(chǔ),目前針對DLOs的建模主要采用質(zhì)量彈簧模型[7]、多體模型[8]、動態(tài)樣條模型[9]、有限元模型[10]、離散彈性桿模型[11]與絕對節(jié)點坐標(biāo)方法 (ANCF)[12]等。其中,基于柔性離散體的ANCF能夠準(zhǔn)確描述大變形體的動力學(xué)特性,因此被廣泛應(yīng)用于繩索[13]、電纜[14]、紗線[15]等大變形柔性線形體的動力學(xué)建模。機(jī)器人對DLOs操縱控制的研究主要有機(jī)器人操縱繩子打結(jié)/解結(jié)[16]、機(jī)器人對DLOs形態(tài)的控制[17]、機(jī)器人操縱DLOs插入孔隙[18]、機(jī)器人布線[6]等。相比于其它類型的DLOs,紗線更加細(xì)長柔軟,機(jī)器人與單根紗線間的交互感知困難,因此機(jī)械臂對卷裝紗線的精確控制更具挑戰(zhàn)。

此外,紡織工廠環(huán)境往往比較復(fù)雜,設(shè)備緊湊,機(jī)器人運動空間局限,因此機(jī)器人牽引紗線運動路徑的避障規(guī)劃也是其成功應(yīng)用的關(guān)鍵。機(jī)器人避障路徑搜索的代表性算法主要有概率路標(biāo)法(PRM)[19]、A*算法[20]、人工勢能法[21]與快速擴(kuò)散隨機(jī)樹法(RRT)算法[22]等。其中,RRT算法因其快速擴(kuò)散性與概率完整性等優(yōu)點被廣泛應(yīng)用,但基本RRT存在著隨機(jī)性強(qiáng)、導(dǎo)向性差、耗時長、冗余結(jié)點多、尋跡平滑度差等不足。對此,一些研究者在其基礎(chǔ)上做了很多改進(jìn)[23-25],其中,Xu等[26]提出了自適應(yīng)引導(dǎo)快速擴(kuò)散隨機(jī)樹(AGG-RRT)算法,其在路徑搜索過程中根據(jù)環(huán)境中障礙物的分布情況自適應(yīng)調(diào)整偏置目標(biāo)因子大小,同時兼顧了導(dǎo)向性與全局隨機(jī)性,搜索時間短,成功率高,已成功應(yīng)用于復(fù)雜織造環(huán)境下機(jī)械臂的無碰撞軌跡規(guī)劃。

1 卷裝紗線牽引動力學(xué)

移動機(jī)械臂對紗線牽引操縱的控制框架如圖1所示,其中,紗線的動力學(xué)建模是實現(xiàn)機(jī)器人對紗線精確控制的基礎(chǔ)。

圖1 移動機(jī)械臂對卷裝紗線的牽引操縱控制框架Fig.1 Control framework for yarn hauling by mobile manipulator

1.1 卷裝表面紗線的數(shù)學(xué)描述

卷裝表面紗線的數(shù)學(xué)描述示意圖見圖2。卷裝基坐標(biāo)系位于卷裝大端底面中心,其基坐標(biāo)為{ex,ey,ez}。s為沿紗線弧長的拉格朗日坐標(biāo),紗線末端Pe為它的原點,r(s,t)為t時刻紗線上某一點的位置向量。為簡化紗線初始狀態(tài)方程的計算,建立圓柱坐標(biāo)系,其基坐標(biāo)為{er,eθ,ez},圓柱坐標(biāo)系內(nèi)任意點都可用(r,θ,z)唯一描述,因此P點的坐標(biāo)向量在圓柱坐標(biāo)系下可表示為

圖2 卷裝表面紗線的數(shù)學(xué)描述示意圖Fig.2 Schematic diagram of mathematical description for yarn on package surface

r(s,t)=r(s,t)er(θ(s,t),t)+z(s,t)ez

圓柱坐標(biāo)系與卷裝基坐標(biāo)系的轉(zhuǎn)換關(guān)系為

根據(jù)卷裝的絡(luò)筒原理求解卷裝表面紗線的初始狀態(tài)方程。令研究的紗線段在卷裝表面從Ps點纏繞到Pe點,紗線段弧長為L;Pe在圓柱坐標(biāo)系下的坐標(biāo)為(r0e(θ0),z0ez);V為絡(luò)筒時紗線相對卷裝的卷繞絕對速度。則紗線段上的任一點P的初始狀態(tài)方程為

式中:β為卷裝的錐角;φ為卷裝紗線的纏繞角;T為從P點開始卷繞的時長;0≤s≤L。

1.2 運動紗線單元的ANCF模型

對于長徑比和大柔性的紗線來說,其截面變形不是關(guān)注的重點,從而可忽略沿截面方向的斜率矢量,因此采用ANCF的梯度缺省索梁單元建立紗線動力學(xué)模型,以求解紗線被牽引脫離卷裝表面的運動控制方程。

如圖3所示,梯度缺省ANCF索梁單元的節(jié)點坐標(biāo)由節(jié)點位置r及其對軸向物質(zhì)坐標(biāo)(0≤≤)的導(dǎo)數(shù)r組成。對于P(si)與P(si+1)兩點間的第i(i=1,2,…,N)個紗線單元,其廣義節(jié)點坐標(biāo)可表示為

圖3 運動紗線單元的ANCF模型Fig.3 ANCF model of moving yarn element

第i個紗線單元軸線上的任意點P在卷裝基坐標(biāo)系下的位置坐標(biāo)可表示為關(guān)于物質(zhì)坐標(biāo)的三次多項式:

通過對單元節(jié)點左邊進(jìn)行三次Hermite插值,可得到P點在t時刻的全局位置坐標(biāo):

ri(,t)=S()qi(t)

式中,S()為插值基函數(shù),即單元形函數(shù),表示如下:

式中:I3為三階單元矩陣;為紗線單元長度。

1.3 紗線單元廣義黏彈性力

第i個紗線單元可通過軸向應(yīng)變εi和曲率κi描述其軸向和彎曲變形,具體表達(dá)式如下:

考慮紗線的黏彈性,紗線第i單元的廣義黏彈性力表示為

1.4 紗線單元廣義外力

其中,紗線單元的廣義重力求解較為簡單,由于紗線單元的每個點均受到分布載荷ρyarng的作用,ρyarn為紗線的線密度,因此紗線單元的廣義重力可表示為

后文將給出紗線單元的廣義空氣阻力與廣義接觸力的求解模型。

1.4.1 廣義空氣阻力

當(dāng)紗線在空氣中運動時,紗線單元所受到的空氣阻力模型如圖4所示。

圖4 紗線單元的空氣阻力模型Fig.4 Air resistance model of yarn element

其中,t=r/|r|,b=(r×r)/|r×r|,n=b×t。忽略空氣的流動速度,單位長度的空氣阻力計算公式[27]如下:

式中:Cr≈1,為空氣阻力系數(shù);ρa(bǔ)ir≈1 kg/m3,為空氣密度;dAcos?=πRdcos?,為紗線迎流橫截面的有效面積;?為v與n的夾角。

所有紗線單元均受到分布載荷dfair的作用,因此紗線單元的廣義空氣阻力為:

1.4.2 廣義接觸力

采用Hunt-Crossley模型[28]建立紗線與卷裝表面的接觸力模型,其可表示為

圖5 紗線與卷裝表面的接觸模型Fig.5 Contact model of yarn with package surface

式中:zaxis=(0,0,zp-rPtanβ);Rm為卷裝大端面的半徑。紗線單元中與卷裝表面接觸的點受到集中載荷力fci(p)的作用,因此紗線單元i的廣義接觸力為:

1.5 紗線運動控制方程

根據(jù)虛功原理建立紗線系統(tǒng)的運動控制方程。由系統(tǒng)各項虛功之和為零可得:

可得紗線系統(tǒng)的動力學(xué)微分代數(shù)方程:

對于圖6所示的末端點隨機(jī)械手牽引運動的卷裝紗線,利用牛頓法得到紗線系統(tǒng)的離散動力學(xué)運動方程。首先采用全隱式積分[29]在每個時間步上更新紗線系統(tǒng)的廣義速度:

圖6 機(jī)械手牽引紗線控制示意圖Fig.6 Control schematic of manipulator pulling yarn

若當(dāng)前時刻機(jī)械手末端的廣義坐標(biāo)為p(t)=(PE,-α)T(PE和α分別為機(jī)械手末端的位置坐標(biāo)與接近向量),則添加機(jī)械手運動約束后的紗線系統(tǒng)廣義修正速度與廣義坐標(biāo)更新公式為

2 紗線牽引機(jī)器人避障策略

2.1 AGG-RRT算法

AGG-RRT算法是在基本RRT算法中引入了自適應(yīng)目標(biāo)引導(dǎo)分量,其原理如圖7所示,圖中圓形是障礙物。

圖7 AGG-RRT算法示意圖Fig.7 Schematic diagram of AGG-RRT algorithm

AGG-RRT算法的具體步驟為:

1) 設(shè)定起點Pstart并存儲在Pnodes;

2) 在全局可行域內(nèi)生成隨機(jī)點Prand,并搜索Pnodes中離Prand最近的節(jié)點Pnear;

3) 沿分量PnearPrand與分量PnearPgoal的合成方向以一定步長ρ結(jié)合自適應(yīng)策略生成Pnew;

4) 對路徑PnearPnew進(jìn)行碰撞檢測,如果無碰撞,則將Pnew存儲在Pnodes;

5) 重復(fù)2)～4),直至Pnew與Pgoal間的距離小于設(shè)定的誤差值。

AGG-RRT算法在節(jié)點Pnear處擴(kuò)展新節(jié)點Pnew的生成公式為

對應(yīng)機(jī)械臂末端位置的速度公式為

式中:ρv=ρ/Δt;λ為目標(biāo)引導(dǎo)分量系數(shù),其被定義為

2.2 紗線軸向應(yīng)變約束

2.3 碰撞檢測

通過基于紗線軸向應(yīng)變約束的AGG-RRT算法得到機(jī)器人末端無碰撞路徑后,需進(jìn)一步對機(jī)械臂的空間構(gòu)型ΩRob和紗線的空間構(gòu)型Ωq與障礙物之間進(jìn)行碰撞檢測,以保證機(jī)械臂在牽引紗線運動過程中,機(jī)械臂體和紗線均不與環(huán)境中障礙物發(fā)生碰撞。首先使用膠囊體或球體包圍盒包絡(luò)不規(guī)則的機(jī)械臂和障礙物,實現(xiàn)模型簡化。然后利用包絡(luò)體之間的歐氏距離來評估機(jī)械臂與障礙物之間的空間占位關(guān)系。如果任何2個球體或膠囊之間的最小距離減去安全距離小于二者半徑之和,則認(rèn)為發(fā)生了碰撞。同理可評估紗線與障礙物之間的占位關(guān)系。

另外,為了保證碰撞檢測的有效性,在機(jī)械臂末端每個移動間隔ΔPE內(nèi),機(jī)械臂各連桿移動的最大距離應(yīng)小于最小障礙物的寬度δmax(即包絡(luò)體的直徑),因此機(jī)械臂末端路徑的搜索步長ρ需滿足以下條件。

由機(jī)械臂末端位移逆解關(guān)節(jié)轉(zhuǎn)動角可得到:

Δθr=Jv(θr)(3×n)-1ΔPET

式中:θr=[θr1,θr2,…,θrn]T,為機(jī)器人的廣義關(guān)節(jié)坐標(biāo);n為機(jī)器人的關(guān)節(jié)數(shù);ωE為機(jī)器人末端的角速度向量;J(θr)(6×n)=[Jv(θr)(3×n);Jω(θr)(3×n)],為機(jī)器人末端的雅可比矩陣,它由線速度雅可比矩陣分量Jv(θr)(3×n)和角速度雅可比矩陣分量Jω(θr)(3×n)組成。

由關(guān)節(jié)轉(zhuǎn)動正解關(guān)節(jié)末端位移可得到:

ΔPiT=Jvi(θri)(3×i)Δθri=Jvi(θri)(3×i)(Jv(θr)(3×i)-1ΔPET)

進(jìn)一步由范數(shù)相容性有:

因此得到:

式中:ΔPi和Jvi(θri)(3×i)分別為機(jī)械人第i個關(guān)節(jié)末端的位置變化量和線速度雅可比矩陣;θri=[θr1,θr2,…,θri]T,為機(jī)器人前i個關(guān)節(jié)的廣義關(guān)節(jié)坐標(biāo);Jv(θr)(3×i)為Jv(θr)(3×n)前i列的分量矩陣。

2.4 反向路徑修正

機(jī)械臂沿路徑Γ牽引紗線運動至Pj時,若機(jī)械臂或紗線與障礙物發(fā)生碰撞,則需對路徑進(jìn)行反向修正,如圖8所示。首先舍棄碰撞點Pj前一點Pj-1至目標(biāo)點Pgoal間的路徑段;然后以Pj-1為新起點P′start至Pgoal重新逐步搜索新的無碰撞路徑?！洹Ｐ枰⒁獾氖?為防止修正路徑?！湓谂鲎颤cPj附近振蕩,修正路徑搜索時應(yīng)將目標(biāo)引導(dǎo)因子λ的初始值設(shè)為0。

圖8 無碰撞路徑反向修正示意圖Fig.8 Collision-free path reverse correction diagram

3 模擬仿真試驗

搭建小型整經(jīng)紗架與移動機(jī)械臂實驗平臺,在SOLIDWORKS 2021內(nèi)構(gòu)建其URDF模型,導(dǎo)入MATLAB R2018a內(nèi),編寫基于紗線ANCF模型與軸向應(yīng)變約束AGG-RRT算法的機(jī)器人牽引紗線避障運動規(guī)劃代碼進(jìn)行數(shù)值仿真,驗證移動機(jī)械臂在復(fù)雜的紡織加工環(huán)境下對柔性紗線動態(tài)操縱規(guī)劃框架的可行性。仿真在配置AMD Ryzen 7 5800H with Radeon Graphics CPU、運行頻率3.20 GHz、16.00 GB內(nèi)存、WIN11系統(tǒng)的計算機(jī)上運行。

3.1 移動機(jī)械臂建模

采用的移動機(jī)械臂由移動底盤與其搭載的六關(guān)節(jié)機(jī)械臂組成。通過MDH法對其進(jìn)行運動學(xué)建模,其MDH模型與參數(shù)分別如圖9和表1所示。由于移動底盤具有2個平移和1個旋轉(zhuǎn)自由度,因此整個復(fù)合機(jī)械臂具有9個自由度,屬于冗余度機(jī)器人,逆運動學(xué)存在多解。

表1 移動機(jī)械臂MDH參數(shù)表Tab.1 MDH parameter table of mobile manipulator

圖9 移動機(jī)械臂MDH模型Fig.9 MDH model of mobile manipulator

3.2 移動機(jī)械臂避障規(guī)劃

以移動機(jī)械臂從紗架正面繞至紗架背面為例,進(jìn)行機(jī)器人避障路徑搜索仿真,測試面對大障礙時的避障能力,仿真參數(shù)如表2所示。無碰撞路徑的自動搜索結(jié)果如圖10所示,對搜索的末端無碰撞路徑進(jìn)行2次遍歷碰撞檢測與修正,最終獲得機(jī)器人關(guān)節(jié)空間完全無碰撞的路徑。機(jī)器人繞過紗架的運動過程如圖11所示。機(jī)器人移動底盤的平移運動曲線和機(jī)器人關(guān)節(jié)運動曲線分別如圖12、13所示。

表2 繞過紗架路徑搜索仿真參數(shù)Tab.2 Simulation parameters for path searching bypassing creel

圖10 繞過紗架路徑的自動搜索Fig.10 Path auto-search when bypassing creel

圖11 機(jī)器人繞過紗架的運動過程Fig.11 Movement process of robot bypassing creel

圖12 移動底盤的平移運動曲線Fig.12 Translational motion curves of moving chassis

圖13 機(jī)械人關(guān)節(jié)運動曲線Fig.13 Joint motion curves of robot

3.3 機(jī)器人牽引紗線避障路徑規(guī)劃

以移動機(jī)械臂夾取卷裝預(yù)留線頭越過障礙將其牽引至目標(biāo)點為例,進(jìn)行機(jī)器人攜紗路徑避障仿真。仿真參數(shù)中關(guān)于路徑搜索的起始與目標(biāo)位姿外參數(shù)見下,其它參數(shù)均與表2中相同,紗線及卷裝仿真參數(shù)見表3。

表3 機(jī)器人牽引紗線避障規(guī)劃紗線和卷裝仿真參數(shù)Tab.3 Yarn and package simulation parameters for obstacle avoidance planning of robot

另外紗線ANCF模型的單元數(shù)為172,每個單元未變形情況下的初始長度為11.8 mm,紗線的一側(cè)端點與卷裝固連。紗線所有單元的整體軸向應(yīng)變最大值εmax設(shè)為1 mm。

紗線無碰撞牽引路徑的自動搜索結(jié)果如圖14所示,對搜索的末端無碰撞路徑進(jìn)行8次遍歷碰撞檢測與修正,最終獲得機(jī)器人關(guān)節(jié)空間完全無碰撞的紗線牽引路徑。機(jī)器人移動底盤的平移運動曲線和機(jī)械人的關(guān)節(jié)運動曲線分別如圖15、16所示。

圖14 紗線牽引路徑自動搜索Fig.14 Automatic search of yarn-pulling path

圖15 紗線牽引路徑移動底盤的平移運動曲線Fig.15 Translational motion curves of moving chassis for yarn-pulling path

圖16 紗線牽引路徑機(jī)械人關(guān)節(jié)運動曲線Fig.16 Joint motion curves of robot for yarn-pulling path

進(jìn)一步對移動機(jī)械臂的無碰撞路徑在笛卡爾坐標(biāo)系內(nèi)進(jìn)行S型速度曲線規(guī)劃[26],得到如圖17所示的機(jī)器人攜紗運動的插補(bǔ)軌跡。然后根據(jù)紗線的運動控制方程,求解每個機(jī)器人運動時間步長下紗線的空間構(gòu)型,其結(jié)果如圖18所示。另外圖19示出紗線第40、80、120和160單元的整體軸向應(yīng)變,可以看出,各單元的整體軸向應(yīng)變的絕對值均小于εmax(1 mm),避免了紗線被過度拉伸,滿足了機(jī)器人對紗線的柔順牽引。

圖17 機(jī)器人牽引紗線路徑S型速度插補(bǔ)軌跡曲線Fig.17 S-shaped velocity interpolation trajectory curves for robot yarn-pulling path

圖18 機(jī)器人牽引紗線避障運動過程Fig.18 Obstacle avoidance motion process of robot pulling yarn

圖19 部分紗線單元的整體軸向應(yīng)變Fig.19 Overall axial strain of some yarn elements. (a) 40th yarn element; (b) 80th yarnelement; (c) 120th yarn element; (d) 160th yarnelement

4 結(jié) 論

本文針對機(jī)器人在復(fù)雜紡織加工環(huán)境中對柔性線形紗線直接操縱存在紗線形態(tài)感知困難和機(jī)器人運動空間受限的兩大難點,以移動機(jī)械臂對織造前整經(jīng)紗架上卷裝線頭的牽引操縱為例,提出了一種集成移動機(jī)械臂運動規(guī)劃策略的卷裝紗線牽引操縱控制框架。構(gòu)建了卷裝紗線系統(tǒng)的動力學(xué)模型;解析了機(jī)器人與紗線間的運動耦合關(guān)系和機(jī)器人牽引紗線的運動控制方程;提出了基于紗線軸向應(yīng)變約束的改進(jìn)自適應(yīng)引導(dǎo)快速擴(kuò)散隨機(jī)樹算法,以此對移動機(jī)械臂進(jìn)行了避障運動規(guī)劃,繞開環(huán)境障礙的同時防止紗線被過度拉伸。

在仿真實驗中,首先對移動機(jī)械臂繞過紗架進(jìn)行了模擬,驗證了避障算法面對大障礙物的避障能力,表明該算法在復(fù)雜的紡織加工環(huán)境下具有適用性。然后對移動機(jī)械臂牽引紗線繞過障礙物到達(dá)目標(biāo)點進(jìn)行了模擬,成功規(guī)劃出無碰撞軌跡,并有效控制了紗線被牽引運動過程中的軸向應(yīng)變,表明該控制框架的有效性,能夠滿足機(jī)械臂對卷裝紗線從起點到目標(biāo)點的無碰撞柔順牽引操縱需求。