基于姿態枚舉算法的混凝土布料機械臂軌跡控制

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

(浙江大學 流體動力與機電系統國家重點實驗室，浙江 杭州 310027)

引言

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

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

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

1 混凝土布料機械臂結構與運動學模型

1.1 混凝土布料機械臂結構

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

1.回轉臺 2.1#關節 3.1#臂 4.2#關節 5.2#臂 6.3#關節 7.3#臂 8.末端出料口 9.末端軟管圖1 混凝土布料機械臂結構示意圖

1.2 混凝土布料機械臂運動學正解模型

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

圖2 混凝土布料機械臂D-H坐標系示意圖

(1)

1.3 混凝土布料機械臂運動學逆解模型

T=[N5N6N7N8]

(2)

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

圖3 圓柱系與布料參數示意圖

(3)

式中,R3×3——末端旋轉矩陣

P3×1——末端位置矢量

經過降維簡化處理后，根據末端旋轉矩陣、末端位置矢量的數值意義可建立二維平面內3個關節角與4個布料參數的關系如下：

即:

(5)

整理可得混凝土布料機械臂逆運動學求解方程：

(6)

得到3個關節角和設定的4個布料參數之間的關系：

(7)

2 基于姿態枚舉算法的混凝土布料機械臂軌跡控制方法

2.1 姿態枚舉算法的思想及步驟

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

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

表1 13m-3Z型混凝土布料機械臂參數

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

(8)

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

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

(9)

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

Δy——各離散點在笛卡爾坐標系y軸的差值

Δz——各離散點在笛卡爾坐標系z軸的差值

n——軌跡路徑上的離散點個數

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

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

布料傾角與各關節角的關系如下：

θ=θ1+θ2+θ3

(10)

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

(11)

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

(12)

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

當求解得到的θ1，θ2，θ3不滿足表1所示關節角范圍條件時，按照枚舉方向對θ進行枚舉直到其逆運動學求解結果符合范圍條件。其中，目標軌跡起點到第2個離散點的求解過程流程圖如圖4所示。令所有離散點均按此方法求解得到相應的關節角度值從而生成預設路徑，即完成了本研究所提出的基于姿態枚舉算法的混凝土布料機械臂軌跡控制。

圖4 基于姿態枚舉算法末端軌跡控制程序流程圖

2.2 姿態枚舉算法的參數選取

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

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

表2 基于姿態枚舉算法的軌跡控制參數

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

圖5 不同參數下3#臂關節角變化曲線圖

3 基于姿態枚舉算法的混凝土布料機械臂軌跡控制聯合仿真

3.1 AMESim-MATLAB-ADAMS聯合仿真模型

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

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

圖6 聯合仿真模型各模塊及信息傳遞路徑

其中，軌跡的控制基于插值法進行，實時軌跡控制模塊在每個仿真步中讀取ADAMS模塊經運動學求解得到的關節角度值。在控制精度允許的范圍內，當關節角度達到目標位置時，應用基于姿態枚舉算法的軌跡控制方法，實時地更新各關節目標角度，以供控制信號產生模塊計算閥控信號值。

3.2 聯合仿真結果及分析

首先，以本研究所提出的姿態枚舉算法作為軌跡控制策略對一弧形目標軌跡進行仿真，其中的主要參數設置如表3所示。

表3 基于姿態枚舉算法的聯合仿真主要參數設置

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

圖7 末端軌跡曲線對比

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

從圖7可以看出，在布料機械臂運動學逆解模型的基礎上，2種不同的控制策略均能實現布料機械臂按照預定目標路線運動。

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

4 結論

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

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

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

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