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

中圖分類號：TP242 文獻標志碼：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 引言

有精度高、剛度大、承載能力強和結構緊湊等優勢，在食品、精密加工與測量和農業等領域得到了廣泛的應用[1-3].然而Delta機器人是一個多自由

Delta機器人是并聯機器人中的重要成員，具度、多變量、高度非線性和多參數耦合的復雜系統，傳統的動力學建模方法繁瑣，

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

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

Udwadia-Kalaba方法

U-K方法是一種求解受約束系統約束力解析解的方法.在穩定無約束系統的動力學方程的基礎上，系統同時受到平衡力與約束力的作用，使

其穩定且始終滿足給定的約束條件.根據U-K理論的一般應用方法，建立具有 n 個狀態變量的動力學系統模型的步驟如下：

第1步，建立無約束的動力學方程

式中， M 為系統的 n×n 維慣性矩陣； Q 為施加在系統上的廣義力； q 為廣義坐標; 是廣義速度； 是廣義加速度， t 為時間

第2步，構建約束方程

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

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

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

對于不顯含 的完整約束進行二次求導，對于顯含 的非完整約束進行一次求導，得到約束方程的二階標準微分形式為

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

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

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

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

AA⁺A=A，

A⁺AA⁺=A⁺，

AA⁺=（AA⁺）^T，

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

第4步，將第3步中得到的約束力施加到第1步的無約束系統，從而得到系統的動力學模型為

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

此外，在進行數值仿真時，系統的初始狀態可能并不滿足運動要求.如果對這個狀態不及時調整，則會出現仿真結果發散，與實際不相同.為了解決這類問題，Udwadia創造性地提出了一種方法.

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

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

（a）Φ=0 是約束方程的一個平衡點；

（b）平衡點必須是全局漸近穩定的（GAS）.

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

式中， η_igt;0，ν_igt;0 ，平衡點漸近穩定時滿足

為了避免仿真結果發散，約束方程（2）重新被改寫為

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

2 Delta機器人動力學模型

與所有的并聯機器人一樣，Delta機器人由靜平臺、動平臺和3個支鏈組成，結構如圖1所示.Delta機器人的獨特之處在于：支鏈由主動臂和從動臂組成，從動臂由平行四邊形機構組成.這種結構限制了動平臺的自由度，從而使動平臺只能以3個純平動自由度進行運動

笛卡爾坐標系結構如圖2所示，其中 P₁ 、P₂，P₃ 構成了靜平臺，并以靜平臺中心點 O 建立OXYZ坐標系. B₁、B₂、B₃ 構成了動平臺，動平臺中心點 O^′（x，y，z） 為末端執行點. OP₁A₁B₁O^′ 構成了支鏈 1，OP₂A₂B₂O^′ 構成了支鏈2，OP₃A₃B₃O^′ 構成了支鏈3.

2.1 無約束系統動力學方程的建立

為了更好地描述Delta機器人的動力學模型，使 q=[x，y，z，θ₁，θ₂，θ₃]^T 為機器人的廣義坐標，其中 x?y?z 是末端執行點位置， θ₁，θ₂，θ₃

是機器人主動關節轉角.首先，根據拉格朗日方程構建Delta機器人的無約束動力學模型為

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

為使 q 運動的力或力矩.

系統的動能包括主動臂轉動，從動臂平動、轉動和動平臺的平動.其中主動臂的動能 K_Λpi 可表示為

從動臂的動能 K_ai 包含平動動能和繞質心轉動動能，從動臂的動能可表示為

式中， v_2i 為質心速度； ω_2i 為從動臂相對于質心的轉動速度，質心速度為

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

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

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

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

式中， 為動平臺質心處的速度；Rot（Z，α_i） 為繞 Z 軸旋轉的旋轉矩陣，且

動平臺動能 K_bi 可表示為

結合等式（14）、（15）和（17），Delta機器人系統總動能可表示為

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

從動臂的勢能 V_ai 可表示為

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

對等式（23）做以下運算：

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

再將等式（27）轉換成（1）的形式，即：

式中， ，通過上述步驟，得到Delta機器人的無約束系統.

2.2 約束方程的建立

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

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

結構約束方程（32）不含 .首先，對其關于時間 t 求一次導，得到

接下來，再對約束等式（33）關于時間 Ψ_t 求一次導，得到：

考慮到Delta機器人在仿真的初始階段廣義坐標之間的關系可能并不匹配，因此將約束方程（32）轉化為

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

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

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

3 數值仿真及結果分析

通過仿真軟件驗證所建立的動力學模型是否正確，以及采用一種基于U-K方法的Delta機器人軌跡跟蹤控制方法.將要跟蹤的位置（或速度）軌跡抽象為虛擬約束，并將其轉化為Pfaffian標準形式，再通過U-K方程計算機器人所需的驅動力矩，以實現機器人的軌跡跟蹤控制.

假設末端執行點實現螺旋上升的運動軌跡：

將運動軌跡也視為一種約束，對等式（38）關于時間 Ψ_t 求兩次導，得

再將等式（39）轉化為

b_d=

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

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

此外，在進行數值仿真之前，還需要知道末端執行點和速度的初始條件分別是：

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

主動關節轉角初始角度和角速度分別是：

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

由Delta機器人的運動學分析可知[14]，主動關節轉角與末端動平臺執行點之間存在一定關系：

0=

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

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

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

Delta機器人末端執行點各分量復合運動軌跡如圖5所示.在空間中以螺旋上升軌跡.可以看出，Delta機器人能夠實現期望軌跡.

Delta機器人在運動過程中主動關節轉角變化情況如圖6所示.在之前仿真研究中，需要精確知道末端執行點下的關節轉角.然而，在Delta機器人初始擺放時，主動關節的轉角并不是最理想狀態.針對關節轉角不理想的情況，應用本文所提出的修正方程，經過一段時間后，主動關節轉角與末端執行點相匹配，到達理想狀態.

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

4結語

本文針對Delta機器人的動力學建模與軌跡跟蹤控制問題，提出了一種基于U-K方法的建模方法和控制方法.應用拉格朗日方程，得到了一個無約束條件下的動力學模型.通過U-K方法得到相應的約束力表達式.使用Pfaffian標準微分形態和U-K方程，計算得到了必要的輸出扭矩以滿足軌跡限制，將約束力應用到無約束模型上，從而建立了Delta機器人的完整動力學模型.通過仿真實驗，解決了末端執行點與主動關節轉角不匹配的問題，使主動關節轉角達到理想狀態.實驗驗證了本文方法運用在Delta機器人動力學建模及軌跡跟蹤控制的有效性.

參考文獻：

[1］畢憲東，王振，李朝龍.基于Delta機器人的食品生產線動態目標抓取方法[J].食品與機械，2022，38（6）：117-122.

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

[3]靳合琦.基于Delta并聯機構的移栽補苗機器人設計研究［D].鎮江：江蘇大學，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].機電工程技術，2020，49（9）：120-123.

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

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

[9]陳艷娟.Delta機器人軌跡跟蹤控制仿真研究[D].蘭州：蘭州理工大學，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方法的平面冗余并聯機器人建模與軌跡跟蹤控制[J].應用數學和力學，2020，41（11）：1183-1196.

[責任編輯：李 嵐]