基于Udwadia-Kalaba方法的Delta機(jī)器人動力學(xué)建模及軌跡跟蹤控制

中圖分類號：TP242 文獻(xiàn)標(biāo)志碼：A

Modeling Trajectory Tracking Control Delta Robots Based on Udwadia-Kalaba Method

YANG Lei， LI Qin-sheng ， ZHANG Guo-zheng （ ， Colege ，Wuhu 2410oo，， ）

Abstract：To address the dynamic modeling challenges Delta robots，a novel approach that combines the Udwadia-Kalaba （U-K） method with Lagrange equations to establish a dynamic model is proposed in this paper. Initiall， the Lagrange equations are employed to derive an unconstrained system. Subsequently， kinematic constraints are used to describe the physical connections between the systems. Analytical solutions for the constraint forces are obtained through the U-K method，which are then applied to the unconstrained system，resulting in a comprehensive dynamic model Delta robots. In the design the trajectory tracking controller，the target trajectory is considered as a virtual constraint. The U-K method is employed to determine the output forces torques，aiming to satisfy the constraints the trajectory. The effectiveness the modeling approach trajectory tracking has been validated through numerical simulations.

Key words：Udwadia-Kalaba method;Delta robot； dynamics modeling; constraints; trajectory tracking

0 引言

有精度高、剛度大、承載能力強(qiáng)和結(jié)構(gòu)緊湊等優(yōu)勢，在食品、精密加工與測量和農(nóng)業(yè)等領(lǐng)域得到了廣泛的應(yīng)用[1-3].然而Delta機(jī)器人是一個(gè)多自由

Delta機(jī)器人是并聯(lián)機(jī)器人中的重要成員，具度、多變量、高度非線性和多參數(shù)耦合的復(fù)雜系統(tǒng)，傳統(tǒng)的動力學(xué)建模方法繁瑣，

Delta機(jī)器人常見的動力學(xué)方法有牛頓歐拉法、拉格朗日方程法、虛功原理法[4和凱恩方法等.Dasgupta[5基于牛頓歐拉法提出了并聯(lián)機(jī)器人動力學(xué)建模的通用法制，推導(dǎo)過程相對復(fù)雜.孫志偉等[6]通過對Delta機(jī)器人的運(yùn)動學(xué)分析，得到了Delta機(jī)器人的動力學(xué)模型.王剛等[基于拉格朗日乘子法對Delta并聯(lián)機(jī)器人簡化并建模，缺點(diǎn)是該方法的建模過程復(fù)雜.李曉麗[8采用虛功原理建立了Delta機(jī)器人的動力學(xué)模型，需要先計(jì)算出每個(gè)運(yùn)動部件的速度和加速度.劉國軍[]利用凱恩方程對三自由度Delta并聯(lián)機(jī)器人建立了動力學(xué)模型.陳艷娟9針對Delta機(jī)器人軌跡跟蹤控制，分別建立PID控制系統(tǒng)和模糊PID控制系統(tǒng).通過仿真結(jié)果發(fā)現(xiàn)，模糊PID控制效果要優(yōu)于PID控制，

在上述方法的基礎(chǔ)上，本文利用UdwadiaKalaba（U-K）方法[10-1]，在不引入拉格朗日乘子的情況下，將受到約束的機(jī)械系統(tǒng)的約束關(guān)系融入到系統(tǒng)的動力學(xué)方程中.這種方法應(yīng)用在平面冗余并聯(lián)機(jī)器人[12]和雙移動機(jī)械臂動力學(xué)建模中.U-K方法首先將預(yù)定的運(yùn)動路徑轉(zhuǎn)化為一個(gè)虛構(gòu)的約束條件，并以此作為控制輸入，從而實(shí)現(xiàn)機(jī)械系統(tǒng)沿預(yù)期路徑的精確移動；然后通過對Delta機(jī)器人運(yùn)動學(xué)分析，基于拉格朗日方程得到了無約束的動力學(xué)模型；其次將Delta機(jī)器人各支鏈的固有結(jié)構(gòu)視為一種約束，基于U-K方法得到約束力的解析解；最后將獲得的約束力施加到無約束系統(tǒng)中，獲得了Delta機(jī)器人的動力學(xué)模型.在仿真的初始階段，末端動平臺軌跡與主動關(guān)節(jié)轉(zhuǎn)角之間的關(guān)系并不匹配，通過修正后的方程能夠使它們之間相匹配.將期望軌跡抽象為虛擬約束，通過Pfaffian標(biāo)準(zhǔn)微分形式和U-K方法計(jì)算出所需的輸出力矩以滿足軌跡約束.

Udwadia-Kalaba方法

U-K方法是一種求解受約束系統(tǒng)約束力解析解的方法.在穩(wěn)定無約束系統(tǒng)的動力學(xué)方程的基礎(chǔ)上，系統(tǒng)同時(shí)受到平衡力與約束力的作用，使

其穩(wěn)定且始終滿足給定的約束條件.根據(jù)U-K理論的一般應(yīng)用方法，建立具有 n 個(gè)狀態(tài)變量的動力學(xué)系統(tǒng)模型的步驟如下：

第1步，建立無約束的動力學(xué)方程

式中， M 為系統(tǒng)的 n×n 維慣性矩陣； Q 為施加在系統(tǒng)上的廣義力； q 為廣義坐標(biāo); 是廣義速度； 是廣義加速度， t 為時(shí)間

第2步，構(gòu)建約束方程

Σ_m 個(gè)約束方程被分為兩類，一類是不顯含 的完整約束，即

另一類是顯含 的非完整約束，即

i=h+1，h+2，…，m.

對于不顯含 的完整約束進(jìn)行二次求導(dǎo)，對于顯含 的非完整約束進(jìn)行一次求導(dǎo)，得到約束方程的二階標(biāo)準(zhǔn)微分形式為

式中， A 為 m×n 維矩陣； b 為 Ψ_m 維向量.

第3步，基于U-K方程得到約束力的解析解為

M^1/2（AM^-1/2）+（b-AM^-1Q），

式中， Q^c 為約束力；Moore-Penrose廣義逆矩陣，記為 A⁺ .在描述約束運(yùn)動的過程中，M-P廣義逆矩陣有重要作用.假設(shè)矩陣 A 的M-P廣義逆矩陣為 A⁺ ，那么 A⁺ 滿足以下條件：

AA⁺A=A，

A⁺AA⁺=A⁺，

AA⁺=（AA⁺）^T，

A⁺A=（A⁺A）^T.

第4步，將第3步中得到的約束力施加到第1步的無約束系統(tǒng)，從而得到系統(tǒng)的動力學(xué)模型為

從上述使用U-K方法過程中可以看出，約束可以是完整約束或非完整約束且不需要引入額外的輔助變量，如拉格朗日乘子.

此外，在進(jìn)行數(shù)值仿真時(shí)，系統(tǒng)的初始狀態(tài)可能并不滿足運(yùn)動要求.如果對這個(gè)狀態(tài)不及時(shí)調(diào)整，則會出現(xiàn)仿真結(jié)果發(fā)散，與實(shí)際不相同.為了解決這類問題，Udwadia創(chuàng)造性地提出了一種方法.

首先，將約束方程（2）修正為

式中， F（Φ，t，β） 表示包含 P 向量參數(shù) β 的 Ψ_m 向量．式（9）必須滿足以下要求：

（a）Φ=0 是約束方程的一個(gè)平衡點(diǎn)；

（b）平衡點(diǎn)必須是全局漸近穩(wěn)定的（GAS）.

約束方程（2）重新被改寫為

式中， η_igt;0，ν_igt;0 ，平衡點(diǎn)漸近穩(wěn)定時(shí)滿足

為了避免仿真結(jié)果發(fā)散，約束方程（2）重新被改寫為

因此，基于U-K方法得到約束力的解析解被改寫為

2 Delta機(jī)器人動力學(xué)模型

與所有的并聯(lián)機(jī)器人一樣，Delta機(jī)器人由靜平臺、動平臺和3個(gè)支鏈組成，結(jié)構(gòu)如圖1所示.Delta機(jī)器人的獨(dú)特之處在于：支鏈由主動臂和從動臂組成，從動臂由平行四邊形機(jī)構(gòu)組成.這種結(jié)構(gòu)限制了動平臺的自由度，從而使動平臺只能以3個(gè)純平動自由度進(jìn)行運(yùn)動

笛卡爾坐標(biāo)系結(jié)構(gòu)如圖2所示，其中 P₁ 、P₂，P₃ 構(gòu)成了靜平臺，并以靜平臺中心點(diǎn) O 建立OXYZ坐標(biāo)系. B₁、B₂、B₃ 構(gòu)成了動平臺，動平臺中心點(diǎn) O^′（x，y，z） 為末端執(zhí)行點(diǎn). OP₁A₁B₁O^′ 構(gòu)成了支鏈 1，OP₂A₂B₂O^′ 構(gòu)成了支鏈2，OP₃A₃B₃O^′ 構(gòu)成了支鏈3.

2.1 無約束系統(tǒng)動力學(xué)方程的建立

為了更好地描述Delta機(jī)器人的動力學(xué)模型，使 q=[x，y，z，θ₁，θ₂，θ₃]^T 為機(jī)器人的廣義坐標(biāo)，其中 x?y?z 是末端執(zhí)行點(diǎn)位置， θ₁，θ₂，θ₃

是機(jī)器人主動關(guān)節(jié)轉(zhuǎn)角.首先，根據(jù)拉格朗日方程構(gòu)建Delta機(jī)器人的無約束動力學(xué)模型為

式中， L 為拉格朗日函數(shù)； K 為動能； P 為勢能； τ_i

為使 q 運(yùn)動的力或力矩.

系統(tǒng)的動能包括主動臂轉(zhuǎn)動，從動臂平動、轉(zhuǎn)動和動平臺的平動.其中主動臂的動能 K_Λpi 可表示為

從動臂的動能 K_ai 包含平動動能和繞質(zhì)心轉(zhuǎn)動動能，從動臂的動能可表示為

式中， v_2i 為質(zhì)心速度； ω_2i 為從動臂相對于質(zhì)心的轉(zhuǎn)動速度，質(zhì)心速度為

式中， e 為沿著從動臂軸線方向的單位矢量， v_ri 為主動臂與從動臂連接處的速度，即

將等式（16）寫成矩陣的形式，可得

v_ei 從動臂與動平臺連接處的速度：

V_ei=Rot（Z，α_i）V_o^′=

式中， 為動平臺質(zhì)心處的速度；Rot（Z，α_i） 為繞 Z 軸旋轉(zhuǎn)的旋轉(zhuǎn)矩陣，且

動平臺動能 K_bi 可表示為

結(jié)合等式（14）、（15）和（17），Delta機(jī)器人系統(tǒng)總動能可表示為

本文將靜平臺定為零勢能面，系統(tǒng)的勢能包括主動臂勢能、從動臂勢能和動平臺勢能.其中主動臂的勢能 V_Φpi 可表示為

從動臂的勢能 V_ai 可表示為

結(jié)合等式（18）和（22），得到拉格朗日函數(shù) L =K_pi+K_ai+K_bi-（V_pi+V_ai+V_bi） ，即

對等式（23）做以下運(yùn)算：

結(jié)合等式 （26）～（29） ，可得到一般動力學(xué)方程為：

再將等式（27）轉(zhuǎn)換成（1）的形式，即：

式中， ，通過上述步驟，得到Delta機(jī)器人的無約束系統(tǒng).

2.2 約束方程的建立

靜平臺、主動臂、從動臂和動平臺構(gòu)成一組支鏈，共3組.它們之間需要滿足一定的結(jié)構(gòu)要求.在本文中，將這種結(jié)構(gòu)要求視為一種約束，3組支

鏈 （i=1，2，3） 約束方程可表示為

結(jié)構(gòu)約束方程（32）不含 .首先，對其關(guān)于時(shí)間 t 求一次導(dǎo)，得到

接下來，再對約束等式（33）關(guān)于時(shí)間 Ψ_t 求一次導(dǎo)，得到：

考慮到Delta機(jī)器人在仿真的初始階段廣義坐標(biāo)之間的關(guān)系可能并不匹配，因此將約束方程（32）轉(zhuǎn)化為

根據(jù)U-K方法和式（11），得到約束力 ，可表示為

最后，將所求的約束力施加到無約束系統(tǒng)中，可表示為

即可得到完整的Delta機(jī)器人動力學(xué)模型.整個(gè)建模過程清晰且不需要引入額外的輔助變量.

3 數(shù)值仿真及結(jié)果分析

通過仿真軟件驗(yàn)證所建立的動力學(xué)模型是否正確，以及采用一種基于U-K方法的Delta機(jī)器人軌跡跟蹤控制方法.將要跟蹤的位置（或速度）軌跡抽象為虛擬約束，并將其轉(zhuǎn)化為Pfaffian標(biāo)準(zhǔn)形式，再通過U-K方程計(jì)算機(jī)器人所需的驅(qū)動力矩，以實(shí)現(xiàn)機(jī)器人的軌跡跟蹤控制.

假設(shè)末端執(zhí)行點(diǎn)實(shí)現(xiàn)螺旋上升的運(yùn)動軌跡：

將運(yùn)動軌跡也視為一種約束，對等式（38）關(guān)于時(shí)間 Ψ_t 求兩次導(dǎo)，得

再將等式（39）轉(zhuǎn)化為

b_d=

[-0.4π²cos（2πt），-0.4π²sin（2πt），0]^T.

矩陣 A_d 和 b_d 與之前得到的約束矩陣 A_m 和 進(jìn)行整合，形成全新的約束矩陣 A 和 接著，將這些新的約束矩陣 A 和 代人系統(tǒng)動力學(xué)模型（34）中.系統(tǒng)動力學(xué)參數(shù)如表2所列.

此外，在進(jìn)行數(shù)值仿真之前，還需要知道末端執(zhí)行點(diǎn)和速度的初始條件分別是：

x（0）=0.1，y（0）=0，z（0）=-1

主動關(guān)節(jié)轉(zhuǎn)角初始角度和角速度分別是：

θ₁（0）=-0.6，θ₂（0）=-0.6，θ₃（0）=-0.6

由Delta機(jī)器人的運(yùn)動學(xué)分析可知[14]，主動關(guān)節(jié)轉(zhuǎn)角與末端動平臺執(zhí)行點(diǎn)之間存在一定關(guān)系：

0=

根據(jù)等式（42）計(jì)算出的結(jié)果與本文給定的初始主動關(guān)節(jié)轉(zhuǎn)角并不相同.實(shí)際上，當(dāng)Delta機(jī)器人軌跡跟蹤時(shí)，可能存在著末端執(zhí)行點(diǎn)與主動關(guān)節(jié)轉(zhuǎn)角不匹配的現(xiàn)象.通過修正方程（11），不匹配問題在一段時(shí)間后得以解決，仿真結(jié)果如圖3～ 圖6所示.

Delta機(jī)器人末端執(zhí)行點(diǎn)各分量的運(yùn)行軌跡如圖3所示.圖3（a）是Delta機(jī)器人在 x 分量的運(yùn)動，滿足期望軌跡，圖3（b）是Delta機(jī)器人在y分量上的運(yùn)動，也是滿足期望軌跡.圖3（c）是Delta機(jī)器人在 z 分量的運(yùn)動，同樣滿足期望軌跡.因此，本文所提出動力學(xué)建模方法和軌跡跟蹤控制方法是有效的.

末端執(zhí)行點(diǎn)的運(yùn)動軌跡誤差如圖4所示.圖4（a）、圖4（b）和圖4（c）分別表示在 x，y 和 z 上的軌跡誤差.其中，在 x 分量上的軌跡誤差在 6～ 10m ，在 y 分量上的軌跡誤差也在 6～10m ，在 z 分量上的軌跡誤差僅有 10～14m .通過誤差分析，可以看到這種控制方法能夠高效地使系統(tǒng)穩(wěn)定跟蹤期望軌跡.

Delta機(jī)器人末端執(zhí)行點(diǎn)各分量復(fù)合運(yùn)動軌跡如圖5所示.在空間中以螺旋上升軌跡.可以看出，Delta機(jī)器人能夠?qū)崿F(xiàn)期望軌跡.

Delta機(jī)器人在運(yùn)動過程中主動關(guān)節(jié)轉(zhuǎn)角變化情況如圖6所示.在之前仿真研究中，需要精確知道末端執(zhí)行點(diǎn)下的關(guān)節(jié)轉(zhuǎn)角.然而，在Delta機(jī)器人初始擺放時(shí)，主動關(guān)節(jié)的轉(zhuǎn)角并不是最理想狀態(tài).針對關(guān)節(jié)轉(zhuǎn)角不理想的情況，應(yīng)用本文所提出的修正方程，經(jīng)過一段時(shí)間后，主動關(guān)節(jié)轉(zhuǎn)角與末端執(zhí)行點(diǎn)相匹配，到達(dá)理想狀態(tài).

圖7（a）是末端動平臺各分量運(yùn)動所需要的力，圖7（b）是主動關(guān)節(jié)旋轉(zhuǎn)所需要的力矩.圖 3～ 圖7的仿真結(jié)果表明基于U-K方程的動力建模與軌跡跟蹤控制方法是有效的.

4結(jié)語

本文針對Delta機(jī)器人的動力學(xué)建模與軌跡跟蹤控制問題，提出了一種基于U-K方法的建模方法和控制方法.應(yīng)用拉格朗日方程，得到了一個(gè)無約束條件下的動力學(xué)模型.通過U-K方法得到相應(yīng)的約束力表達(dá)式.使用Pfaffian標(biāo)準(zhǔn)微分形態(tài)和U-K方程，計(jì)算得到了必要的輸出扭矩以滿足軌跡限制，將約束力應(yīng)用到無約束模型上，從而建立了Delta機(jī)器人的完整動力學(xué)模型.通過仿真實(shí)驗(yàn)，解決了末端執(zhí)行點(diǎn)與主動關(guān)節(jié)轉(zhuǎn)角不匹配的問題，使主動關(guān)節(jié)轉(zhuǎn)角達(dá)到理想狀態(tài).實(shí)驗(yàn)驗(yàn)證了本文方法運(yùn)用在Delta機(jī)器人動力學(xué)建模及軌跡跟蹤控制的有效性.

參考文獻(xiàn)：

[1］畢憲東，王振，李朝龍.基于Delta機(jī)器人的食品生產(chǎn)線動態(tài)目標(biāo)抓取方法[J].食品與機(jī)械，2022，38（6）：117-122.

[2]李云仕，方華，石安，等.Delta機(jī)器人在精密3C制造領(lǐng)域中的應(yīng)用[J].中國高新科技，2021（10）：45-46.

[3]靳合琦.基于Delta并聯(lián)機(jī)構(gòu)的移栽補(bǔ)苗機(jī)器人設(shè)計(jì)研究［D].鎮(zhèn)江：江蘇大學(xué)，2017.

[4]ASADI F，HEYDARI A. Analytical dynamic model-ing Delta robot with experimental verification[J].Proceedings the Institution Engi-neers，Part K：Journal Multi-body Dynamics，2020，234（3）：623-630.

[5]DASGUPTA B，CHOUDHURY P.A general strate-gy based on the Newton-Euler approach for the dy-namic formulation parallel manipulators[J].Mech-anism Machine Theory，1999，34（6） ：801-824.

[6]孫志偉，李亞洲，武志華.基于拉格朗日方程的Delta機(jī)器人動力學(xué)分析[J].機(jī)電工程技術(shù)，2020，49（9）：120-123.

[7]王剛，劉延杰，吳明月，等.基于拉格朗日乘子法的Delta并聯(lián)機(jī)器人簡化剛體動力學(xué)建模方法[J].機(jī)械與電子，2015（7）：68-72，76.

[8]李曉麗.三自由度DELTA機(jī)器人控制方法的研究[D].鞍山：遼寧科技大學(xué)，2019.

[9]陳艷娟.Delta機(jī)器人軌跡跟蹤控制仿真研究[D].蘭州：蘭州理工大學(xué)，2017.

[10] FIRDAUS E，UDWADIA，et al. A new perspectiveon constrained motion[J]. Proceedings the RoyalSociety A： Mathematical， Physical Sciences，1992，439：407-410.

[11]UDWADIA F E.A new perspective on tracking con-trol nonlinear structural mechanical systems[J].Proceedings the Royal Society A，2003，459 ：1783-1800.

[12]韓江，汪鵬，董方方，等.基于Udwadia-Kalaba方法的平面冗余并聯(lián)機(jī)器人建模與軌跡跟蹤控制[J].應(yīng)用數(shù)學(xué)和力學(xué)，2020，41（11）：1183-1196.

[責(zé)任編輯：李 嵐]