移動機械臂牽引卷裝紗線的動態建模與控制

許高平, 孫以澤

(東華大學 機械工程學院, 上海 201620)

隨著機器人技術與紡織工業智能化的不斷發展,利用工業機器人替代人工完成各類典型的紡織加工技能作業已成為紡織行業的新趨勢,比如紡紗領域的物料轉運機器人、細紗自動落紗機器人、紗線整理打包入庫機器人[1];織造領域的筒子架換運機器人[2]、經軸上落機器人[3];編織領域的芯模牽引機器人[4]。以上機器人應用多為固定點位間的搬運或對剛性物體的抓取、牽引等,而將剛性的工業機器人適配于紡織柔性體加工制造,完成技能性更強的擬人化作業仍存在挑戰。

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

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

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

1 卷裝紗線牽引動力學

移動機械臂對紗線牽引操縱的控制框架如圖1所示,其中,紗線的動力學建模是實現機器人對紗線精確控制的基礎。

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

1.1 卷裝表面紗線的數學描述

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

圖2 卷裝表面紗線的數學描述示意圖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

圓柱坐標系與卷裝基坐標系的轉換關系為

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

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

1.2 運動紗線單元的ANCF模型

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

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

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

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

通過對單元節點左邊進行三次Hermite插值,可得到P點在t時刻的全局位置坐標:

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

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

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

1.3 紗線單元廣義黏彈性力

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

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

1.4 紗線單元廣義外力

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

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

1.4.1 廣義空氣阻力

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

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

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

式中:Cr≈1,為空氣阻力系數;ρair≈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 紗線運動控制方程

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

可得紗線系統的動力學微分代數方程:

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

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

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

2 紗線牽引機器人避障策略

2.1 AGG-RRT算法

AGG-RRT算法是在基本RRT算法中引入了自適應目標引導分量,其原理如圖7所示,圖中圓形是障礙物。

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

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

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

2) 在全局可行域內生成隨機點Prand,并搜索Pnodes中離Prand最近的節點Pnear;

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

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

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

AGG-RRT算法在節點Pnear處擴展新節點Pnew的生成公式為

對應機械臂末端位置的速度公式為

式中:ρv=ρ/Δt;λ為目標引導分量系數,其被定義為

2.2 紗線軸向應變約束

2.3 碰撞檢測

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

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

由機械臂末端位移逆解關節轉動角可得到:

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

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

由關節轉動正解關節末端位移可得到:

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

進一步由范數相容性有:

因此得到:

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

2.4 反向路徑修正

機械臂沿路徑Γ牽引紗線運動至Pj時,若機械臂或紗線與障礙物發生碰撞,則需對路徑進行反向修正,如圖8所示。首先舍棄碰撞點Pj前一點Pj-1至目標點Pgoal間的路徑段;然后以Pj-1為新起點P′start至Pgoal重新逐步搜索新的無碰撞路徑Γ′。需要注意的是,為防止修正路徑Γ′在碰撞點Pj附近振蕩,修正路徑搜索時應將目標引導因子λ的初始值設為0。

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

3 模擬仿真試驗

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

3.1 移動機械臂建模

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

表1 移動機械臂MDH參數表Tab.1 MDH parameter table of mobile manipulator

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

3.2 移動機械臂避障規劃

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

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

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

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

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

圖13 機械人關節運動曲線Fig.13 Joint motion curves of robot

3.3 機器人牽引紗線避障路徑規劃

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

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

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

紗線無碰撞牽引路徑的自動搜索結果如圖14所示,對搜索的末端無碰撞路徑進行8次遍歷碰撞檢測與修正,最終獲得機器人關節空間完全無碰撞的紗線牽引路徑。機器人移動底盤的平移運動曲線和機械人的關節運動曲線分別如圖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 紗線牽引路徑機械人關節運動曲線Fig.16 Joint motion curves of robot for yarn-pulling path

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

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

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

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

4 結 論

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

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