基于姿態(tài)枚舉算法的混凝土布料機(jī)械臂軌跡控制

方梓帆，錢劍勇，蘇 琦，徐 兵

(浙江大學(xué) 流體動力與機(jī)電系統(tǒng)國家重點實驗室，浙江 杭州 310027)

引言

混凝土布料機(jī)具有布料范圍大、機(jī)動性高、靈活性強(qiáng)的特點，是建筑行業(yè)中至關(guān)重要的工程機(jī)械之一[1]。如今，我國混凝土布料臂架廣泛采用操作工人單關(guān)節(jié)獨立遙控的控制方式。為減少勞動力投入，提高澆筑質(zhì)量，避免因布料機(jī)械臂支撐體系坍塌等極端情況導(dǎo)致的人員傷亡事故，建筑施工上對全自動混凝土布料機(jī)器人的需求日漸提高，其核心問題就是如何實現(xiàn)末端軌跡控制，即全自動布料[2]。

國內(nèi)外已經(jīng)有諸多學(xué)者對混凝土布料機(jī)械臂的末端軌跡控制進(jìn)行了研究，均取得了較好的成果。KEATING S J等[3]實現(xiàn)了泡沫式建筑材料澆筑噴頭的末端軌跡控制；AYTEN等[4]基于能量消耗最少的原則進(jìn)行了最優(yōu)軌跡規(guī)劃方面的研究，提出了效率高且復(fù)雜度低的優(yōu)化算法；黃罡等[5]基于正解模型推導(dǎo)了油缸行程和關(guān)節(jié)角變化的關(guān)系，并通過位移傳感器測量液壓缸的伸出長度，運用前饋補(bǔ)償?shù)目刂撇呗?，?yōu)化了臂架的控制系統(tǒng)；周淑文等[6-8]基于正解模型采用插補(bǔ)算法實現(xiàn)對臂架出料口的智能化控制，并進(jìn)行了仿真與實驗；杜冰暉等[9]依據(jù)最優(yōu)控制原理制定了臂架運動過程中的關(guān)節(jié)角限制條件，并進(jìn)行了避障控制的研究；王珊[10]將臂架的整體形態(tài)與臂架逆解相結(jié)合，使臂架保持流動阻力較小的弧形；張思奇等[11]運用最小范數(shù)法使關(guān)節(jié)運動連續(xù)平穩(wěn)，并進(jìn)行了聯(lián)合仿真優(yōu)化；郭立新等[12]采用以各執(zhí)行構(gòu)件變化時間最大者最小的最優(yōu)化控制原理進(jìn)行運動學(xué)逆解；俞志鵬[13]基于遍歷算法對上個離散點關(guān)節(jié)角附近的角度進(jìn)行遍歷，以各關(guān)節(jié)角絕對值之和最小作為限制條件；朱云峰等[14]基于數(shù)值迭代算法，依次固定所求夾角中的3個而轉(zhuǎn)動另一角，使得末端逐步逼近目標(biāo)點，該方法適用于PLC控制，能夠明顯使臂架運動快于人工手動調(diào)試。但是上述研究多是針對布料機(jī)剛性臂架，而在實際應(yīng)用中，布料機(jī)臂架末端常通過布料軟管對混凝土進(jìn)行導(dǎo)向，若臂架進(jìn)行大幅度擺動，容易導(dǎo)致布料軟管出現(xiàn)甩動，影響混凝土出料落點的位置。因此，在進(jìn)行軌跡控制時，還應(yīng)考慮最后一節(jié)臂的擺動對布料軟管產(chǎn)生的影響。

針對混凝土布料機(jī)的軌跡控制，提出一種基于姿態(tài)枚舉算法的機(jī)械臂軌跡控制方法，采用該方法對某13m-3Z型混凝土布料機(jī)進(jìn)行了末端軌跡控制，并基于AMESim-MATLAB-ADAMS模型對其進(jìn)行了聯(lián)合仿真分析，得到了末端軌跡以及布料傾角的變化規(guī)律。該方法以布料機(jī)械臂的運動學(xué)逆解模型為基礎(chǔ)，求解得到目標(biāo)關(guān)節(jié)角度，適用于具有多自由度的布料機(jī)械臂的PLC控制。

1 混凝土布料機(jī)械臂結(jié)構(gòu)與運動學(xué)模型

1.1 混凝土布料機(jī)械臂結(jié)構(gòu)

混凝土布料機(jī)械臂通常是一個多自由度機(jī)械臂系統(tǒng)，本研究以具有三節(jié)臂的混凝土布料機(jī)械臂為例，闡述其基本結(jié)構(gòu)及運動學(xué)逆解模型的建立過程。該混凝土布料機(jī)械臂結(jié)構(gòu)如圖1所示，其主要組成部分為回轉(zhuǎn)臺、1#關(guān)節(jié)、1#臂、2#關(guān)節(jié)、2#臂、3#關(guān)節(jié)、3#臂、末端軟管。末端軟管基本不具有混凝土承載能力，僅為末端混凝土出料口起導(dǎo)向作用，在施工過程中，基本保持垂直朝向工作平面，可暫忽略其自由度，因此可將該系統(tǒng)視為回轉(zhuǎn)臺+三節(jié)臂組成的四自由度系統(tǒng)。

1.回轉(zhuǎn)臺 2.1#關(guān)節(jié) 3.1#臂 4.2#關(guān)節(jié) 5.2#臂 6.3#關(guān)節(jié) 7.3#臂 8.末端出料口 9.末端軟管圖1 混凝土布料機(jī)械臂結(jié)構(gòu)示意圖

1.2 混凝土布料機(jī)械臂運動學(xué)正解模型

為研究關(guān)節(jié)補(bǔ)償角對末端出料口位置的影響，以D-H矩陣法為理論基礎(chǔ)，建立各臂段關(guān)節(jié)角度變化和臂架末端空間位姿的關(guān)系。在該混凝土布料機(jī)械臂上建立的D-H坐標(biāo)系如圖2所示。

圖2 混凝土布料機(jī)械臂D-H坐標(biāo)系示意圖

(1)

1.3 混凝土布料機(jī)械臂運動學(xué)逆解模型

T=[N5N6N7N8]

(2)

以基坐標(biāo)原點為原點設(shè)置圓柱坐標(biāo)系如圖3所示。在該坐標(biāo)系中，為便于建立末端位姿與關(guān)節(jié)角的函數(shù)關(guān)系，設(shè)定4個布料參數(shù)如下：布料高度H為末端坐標(biāo)系原點在圓柱坐標(biāo)系z軸方向的尺度；布料半徑R為末端坐標(biāo)系原點在圓柱坐標(biāo)系的徑向尺度；布料傾角θ為末端坐標(biāo)系x軸與水平面之間的夾角；此時θ0也稱布料轉(zhuǎn)角。

圖3 圓柱系與布料參數(shù)示意圖

(3)

式中,R3×3——末端旋轉(zhuǎn)矩陣

P3×1——末端位置矢量

經(jīng)過降維簡化處理后，根據(jù)末端旋轉(zhuǎn)矩陣、末端位置矢量的數(shù)值意義可建立二維平面內(nèi)3個關(guān)節(jié)角與4個布料參數(shù)的關(guān)系如下：

即:

(5)

整理可得混凝土布料機(jī)械臂逆運動學(xué)求解方程：

(6)

得到3個關(guān)節(jié)角和設(shè)定的4個布料參數(shù)之間的關(guān)系：

(7)

2 基于姿態(tài)枚舉算法的混凝土布料機(jī)械臂軌跡控制方法

2.1 姿態(tài)枚舉算法的思想及步驟

本研究所提出的基于姿態(tài)枚舉算法的混凝土布料機(jī)械臂軌跡控制方法，其目的在于減小布料傾角θ的變化量，進(jìn)而減少末端軟管相對于3#臂架鉸點的轉(zhuǎn)角變化量，降低混凝土出料時因末端軟管徑向甩動產(chǎn)生的離心力。

現(xiàn)以某13m-3Z型混凝土布料機(jī)械臂為例對該控制方法進(jìn)行闡述，部分參數(shù)如表1所示。

表1 13m-3Z型混凝土布料機(jī)械臂參數(shù)

令笛卡爾坐標(biāo)系原點、z軸、x軸分別與圓柱坐標(biāo)系原點、z軸、方位角基準(zhǔn)軸重合，可建立笛卡爾坐標(biāo)系中布料參數(shù)與臂架末端位置的關(guān)系如下：

(8)

當(dāng)給定一段布料機(jī)械臂末端運動軌跡時，利用插值法可將該目標(biāo)運動軌跡離散化為不同的離散點。通過控制臂架末端依次跟隨離散點，最終實現(xiàn)臂架末端從目標(biāo)起點經(jīng)目標(biāo)軌跡抵達(dá)目標(biāo)終點的布料操作。離散化過程中，各離散點之間的距離應(yīng)根據(jù)實際工程需要來確定。若距離過小，產(chǎn)生的離散點則過多，容易導(dǎo)致控制器運算量較大，末端頻繁動作而不穩(wěn)定等問題；若距離過大，則離散點過少，會使末端軌跡跟蹤的精度降低。

以控制末端按照某一直線軌跡進(jìn)行運動為例，給定離散點之間的距離，可求得離散點個數(shù)，并進(jìn)一步確定相鄰離散點坐標(biāo)差值如下：

(9)

式中, Δx——各離散點在笛卡爾坐標(biāo)系x軸的差值

Δy——各離散點在笛卡爾坐標(biāo)系y軸的差值

Δz——各離散點在笛卡爾坐標(biāo)系z軸的差值

n——軌跡路徑上的離散點個數(shù)

(xt,yt,zt)——目標(biāo)軌跡終點坐標(biāo)

(x0,y0,z0)——目標(biāo)軌跡起點坐標(biāo)

布料傾角與各關(guān)節(jié)角的關(guān)系如下：

θ=θ1+θ2+θ3

(10)

對于目標(biāo)軌跡上任一對正向運行的相鄰離散點而言，布料半徑的變化量滿足下式：

(11)

根據(jù)ΔR的正負(fù)判斷相鄰2點布料半徑的變化方向，作為末端姿態(tài)即布料傾角枚舉方向的判定依據(jù)：

(12)

設(shè)定初始布料傾角θ以及每次枚舉時的傾角變化量Δθ。為使混凝土澆筑出料順暢，通常令θ=-90°，同時將各離散點笛卡爾坐標(biāo)轉(zhuǎn)換為柱坐標(biāo)，即計算各離散點坐標(biāo)對應(yīng)R,H,θ0的值代入布料機(jī)械臂逆運動學(xué)模型，可求解得到各關(guān)節(jié)角度值。

當(dāng)求解得到的θ1，θ2，θ3不滿足表1所示關(guān)節(jié)角范圍條件時，按照枚舉方向?qū)Ζ冗M(jìn)行枚舉直到其逆運動學(xué)求解結(jié)果符合范圍條件。其中，目標(biāo)軌跡起點到第2個離散點的求解過程流程圖如圖4所示。令所有離散點均按此方法求解得到相應(yīng)的關(guān)節(jié)角度值從而生成預(yù)設(shè)路徑，即完成了本研究所提出的基于姿態(tài)枚舉算法的混凝土布料機(jī)械臂軌跡控制。

圖4 基于姿態(tài)枚舉算法末端軌跡控制程序流程圖

2.2 姿態(tài)枚舉算法的參數(shù)選取

應(yīng)用姿態(tài)枚舉算法對多自由度混凝土布料機(jī)械臂進(jìn)行軌跡控制的過程中，對預(yù)設(shè)路徑產(chǎn)生影響的參數(shù)，包括離散點個數(shù)n、初始布料傾角θ及傾角枚舉步長Δθ。其中，初始布料傾角θ對預(yù)設(shè)路徑的精度以及平滑性影響較小。

現(xiàn)以表1所述布料機(jī)械臂為對象，設(shè)置3組不同的參數(shù)如表2所示，采用基于姿態(tài)枚舉算法的控制策略對軌跡進(jìn)行控制，得到3#臂關(guān)節(jié)角θ3的變化曲線如圖5所示。其中，設(shè)置初始傾角θ=-90°，起始點坐標(biāo)：R=7000，H=0，θ0=0°，終點坐標(biāo)：R=127000，H=0，θ0=30°。

表2 基于姿態(tài)枚舉算法的軌跡控制參數(shù)

分析圖5可得，在該條件下，當(dāng)傾角枚舉步長保持在1°時，通過逆解模型求得的3#臂關(guān)節(jié)角波動在5°以上，且增加離散點個數(shù)對于預(yù)設(shè)軌跡的平滑性改善并不明顯；當(dāng)傾角枚舉步長設(shè)為0.1°時，關(guān)節(jié)角波動減少至2°左右，預(yù)設(shè)軌跡的平滑性和穩(wěn)定性大大改善。因此，在傾角傳感器分辨率足夠的情況下，采用姿態(tài)枚舉算法對布料機(jī)械臂軌跡進(jìn)行控制時，應(yīng)盡可能減小傾角枚舉步長Δθ的設(shè)置。

圖5 不同參數(shù)下3#臂關(guān)節(jié)角變化曲線圖

3 基于姿態(tài)枚舉算法的混凝土布料機(jī)械臂軌跡控制聯(lián)合仿真

3.1 AMESim-MATLAB-ADAMS聯(lián)合仿真模型

為了驗證基于姿態(tài)枚舉算法的混凝土布料機(jī)械臂軌跡控制方法在實際工程應(yīng)用中的可行性，以表1所述某13m-3Z型混凝土布料機(jī)械臂為對象，建立了基于AMESim-MATLAB-ADAMS的混凝土布料機(jī)械臂聯(lián)合仿真模型。

通過在AMESim中建立閥控缸模型，在ADAMS中建立布料機(jī)械臂機(jī)械結(jié)構(gòu)模型，在MATLAB/Simulink環(huán)境下建立機(jī)械臂控制模型并將三者結(jié)合進(jìn)行聯(lián)合仿真的方式，使仿真模型更加貼合實際。該聯(lián)合仿真模型以MATLAB/Simulink為主體，并添加了AMESim與ADAMS的聯(lián)仿接口。各個模塊以及模塊間的信息傳遞路徑如圖6所示。

圖6 聯(lián)合仿真模型各模塊及信息傳遞路徑

其中，軌跡的控制基于插值法進(jìn)行，實時軌跡控制模塊在每個仿真步中讀取ADAMS模塊經(jīng)運動學(xué)求解得到的關(guān)節(jié)角度值。在控制精度允許的范圍內(nèi)，當(dāng)關(guān)節(jié)角度達(dá)到目標(biāo)位置時，應(yīng)用基于姿態(tài)枚舉算法的軌跡控制方法，實時地更新各關(guān)節(jié)目標(biāo)角度，以供控制信號產(chǎn)生模塊計算閥控信號值。

3.2 聯(lián)合仿真結(jié)果及分析

首先，以本研究所提出的姿態(tài)枚舉算法作為軌跡控制策略對一弧形目標(biāo)軌跡進(jìn)行仿真，其中的主要參數(shù)設(shè)置如表3所示。

表3 基于姿態(tài)枚舉算法的聯(lián)合仿真主要參數(shù)設(shè)置

其次，對于具有多自由度的機(jī)械臂軌跡控制而言，遍歷算法是一種簡單且通用的控制算法。該算法和枚舉算法具有相似性，兩者均按一定步長對機(jī)械臂逆運動學(xué)進(jìn)行求解結(jié)果的搜索，并依據(jù)附加的限定條件從結(jié)果解集中篩選出最優(yōu)解。本研究以遍歷算法作為軌跡控制策略進(jìn)行仿真對比，布料傾角θ的遍歷范圍設(shè)置為[-70°，-1°]，遍歷步長設(shè)置為0.1°，初始傾角設(shè)置為-60°，最優(yōu)解約束條件設(shè)置為各關(guān)節(jié)角變化量之和最小。得到2種不同控制策略下的末端軌跡曲線如圖7所示，布料傾角隨布料半徑變化曲線如圖8所示。

圖7 末端軌跡曲線對比

圖8 布料傾角變化曲線圖對比

從圖7可以看出，在布料機(jī)械臂運動學(xué)逆解模型的基礎(chǔ)上，2種不同的控制策略均能實現(xiàn)布料機(jī)械臂按照預(yù)定目標(biāo)路線運動。

從圖8可以看出，當(dāng)采用基于遍歷算法的軌跡控制策略時，布料傾角θ隨布料半徑增加而增加，最小值為-60.14°，最大值為-1.33°，傾角變化量為58.81°。當(dāng)采用基于枚舉算法的軌跡控制策略時，在布料半徑為7000～10100 mm的運動段里，布料傾角最小值為-60.50°，最大值為-60.19°，波動僅為0.31°，基本穩(wěn)定在-60°左右，證明在該運動段內(nèi)軟管-3#臂鉸點與3# 臂一起以近似平移的方式移動；在布料半徑大于10100 mm的運動段里，由于臂架伸展距離較遠(yuǎn)，布料傾角不能保持在-60°，并隨布料半徑增加而增加；但得益于姿態(tài)枚舉的控制策略，從起點運行到終點的過程中，布料傾角最大值為-19.83°，傾角變化量為40.67°，與基于遍歷算法的控制策略相比，變化量減少了30.8%。結(jié)果表明，盡管采用以各關(guān)節(jié)角變化量之和最小作限制條件的遍歷算法有利于縮短油缸行程，減少能耗，但與混凝土出料口直接相關(guān)的3#臂擺動幅度較大。本研究提出的基于姿態(tài)枚舉算法的混凝土布料機(jī)械臂軌跡控制方法限制了3#臂的擺動，減少了末端軟管的徑向甩動，增加了混凝土出料的穩(wěn)定性。

4 結(jié)論

本研究以13m-3Z型混凝土布料機(jī)械臂為例，建立了該機(jī)械臂的運動學(xué)正、逆解模型，提出了基于姿態(tài)枚舉算法的軌跡控制方法，分析了該方法不同參數(shù)對控制結(jié)果的影響規(guī)律，搭建了聯(lián)合仿真模型，對比分析姿態(tài)枚舉算法和遍歷算法的仿真結(jié)果，得到以下結(jié)論：

(1) 傾角變化量反映布料機(jī)最后一節(jié)臂與水平面間夾角的波動情況。傾角變化量越小，則波動越小，末端軟管隨最后一節(jié)臂擺動的幅度越小，混凝土出料落點的控制精度越高;

(2) 與遍歷算法相比，采用本研究提出的姿態(tài)枚舉算法，可使布料傾角變化量減少30.8%。證明該算法可減少末端布料軟管相對最后一節(jié)臂的轉(zhuǎn)動，從而降低末端布料軟管的徑向甩動;

(3) 選取姿態(tài)枚舉算法的參數(shù)時，減小傾角枚舉步長Δθ可改善軌跡控制的平滑性和穩(wěn)定性。