提綜臂輔助旋鉚機械手動力學(xué)建模及控制仿真

梁 棟，梁正宇，楊建成，徐振宇

（1.天津工業(yè)大學(xué) 機械工程學(xué)院，天津 300387；2.天津工業(yè)大學(xué) 天津市現(xiàn)代機電裝備技術(shù)重點實驗室，天津 300387；3.江蘇金龍科技股份有限公司，江蘇 蘇州 215500）

多臂開口織機（簡稱為多臂織機）通過多臂實現(xiàn)開口，其在劍桿織機、噴氣織機等無梭織機中應(yīng)用廣泛，常用于較為復(fù)雜的小花紋織物的織制，而提綜臂則是多臂織機上實現(xiàn)凸輪開口的一個核心部件[1-2]?，F(xiàn)階段對提綜臂的裝配加工，主要采用旋鉚工藝，由人工將提綜臂相關(guān)零部件放至旋鉚機上進(jìn)行鉚接。因人工操作效率低且誤差較大，故存在鉚接的產(chǎn)品一致性差、質(zhì)量不穩(wěn)定的問題。為實現(xiàn)該工藝的自動化，提高生產(chǎn)效率，保證加工質(zhì)量，本文關(guān)注多臂織機提綜臂的輔助旋鉚加工，并針對提綜臂的部分加工過程，采用先進(jìn)并聯(lián)機器人技術(shù)，提出新型輔助旋鉚并聯(lián)機器人。在此基礎(chǔ)上，重點關(guān)注機器人的高效動力學(xué)控制策略，旨在保證機械手實現(xiàn)高速、高精度地抓放動作。

相比于傳統(tǒng)串聯(lián)機器人，并聯(lián)機器人具有速度快、剛度大、結(jié)構(gòu)穩(wěn)定、運動慣性小、承載能力強、誤差累積小等特點[3-4]。然而，并聯(lián)機器人因其存在非線性閉環(huán)約束，其關(guān)節(jié)間的耦合作用較強。因此，并聯(lián)機器人也具有控制難度大的特點，簡單的電機位置控制（不考慮動力學(xué)因素）難以保證并聯(lián)機器人在高速下的運動精度。對此，李占賢[5]提出基于伺服電動機的控制模型，通過增加速度前饋等環(huán)節(jié)，實現(xiàn)對并聯(lián)機器人的準(zhǔn)確控制。除此之外，基于逆動力學(xué)模型的控制方法因其良好的控制性能而備受青睞，設(shè)計者可以基于數(shù)學(xué)模型設(shè)計其相應(yīng)的控制器來實現(xiàn)對機器人的運動控制，例如計算力矩控制[6-7]、神經(jīng)網(wǎng)絡(luò)控制[8]、模糊控制[9-10]、滑模控制[11]、模糊滑?？刂芠12]、模糊神經(jīng)控制[13]等。動力學(xué)模型是動態(tài)特性分析和控制的基礎(chǔ)，對于剛體動力學(xué)模型的建立，采用的方法則主要有牛頓-歐拉法[14-15]、虛功原理法[5-7]、拉格朗日方法[3-16]、KANE方法[17]等。在控制仿真實驗的方法選擇上，文獻(xiàn)[18]采用Simulink與SimMechanics相結(jié)合的方法進(jìn)行控制仿真實驗，文獻(xiàn)[19]采用Simulink與ADAMS聯(lián)合仿真的方法對控制參數(shù)進(jìn)行整定。

以提綜臂的高效自動化加工為背景，本文采用并聯(lián)機器人技術(shù)，提出一種輔助旋鉚機械手，實現(xiàn)將提綜臂零部件從傳送帶快速放至專用夾具，并對該機械手開展剛體動力學(xué)分析、控制策略設(shè)計及虛擬仿真實驗，為其樣機制造及實際應(yīng)用奠定基礎(chǔ)。首先，建立單支鏈的閉環(huán)矢量方程，并進(jìn)行位置和速度分析；其次，基于拉格朗日方法建立機構(gòu)的逆剛體動力學(xué)模型；接著，采用計算力矩控制方法，設(shè)計一種任務(wù)空間運動控制律；最后，借助ADAMS與Simulink平臺開展聯(lián)合虛擬仿真，整定控制參數(shù)。

1 輔助旋鉚機械手結(jié)構(gòu)簡介

為實現(xiàn)多臂織機提綜臂的自動化旋鉚加工，提出一種輔助旋鉚并聯(lián)機械手，其主體運動執(zhí)行機構(gòu)的SolidWorks虛擬樣機如圖1所示。為便于觀察，圖1僅示出主要運動執(zhí)行機構(gòu)，其余附件未示出。該機構(gòu)包含左、右兩條相同支鏈，各支鏈均由一組平行四連桿機構(gòu)驅(qū)動，保證了中間肘部關(guān)節(jié)擁有與驅(qū)動關(guān)節(jié)一致的運動規(guī)律，便于實現(xiàn)運動控制。兩條支鏈各由一臺伺服電動機驅(qū)動，且兩個驅(qū)動轉(zhuǎn)軸相互平行。各支鏈中肘部連接架與動平臺則由一組4U（U表示虎克鉸）子鏈相連接，其余均由轉(zhuǎn)動副連接。

圖1 機構(gòu)示意圖Fig.1 Diagram of mechanism

虎克鉸的布置：不失一般性，動平臺形狀設(shè)計為正方形，其對角線上的虎克鉸的固定轉(zhuǎn)軸兩兩平行，相鄰的固定轉(zhuǎn)軸兩兩垂直。動平臺上4個虎克鉸的空間排布形式如圖2所示。中間肘部關(guān)節(jié)上虎克鉸的固定軸線，兩兩相互垂直，與桿件另一端連接動平臺的虎克鉸上的固定軸線各自平行。由文獻(xiàn)[20]可知，該機構(gòu)具有y、z兩個方向的移動自由度。

圖2 動平臺上虎克鉸的布置Fig.2 Collocation of hook joints on moving platform

根據(jù)生產(chǎn)工藝要求，設(shè)計結(jié)果應(yīng)達(dá)到如下目標(biāo)：在最大加速度為3g的工況下，y、z2個方向的誤差小于1 mm。由此設(shè)計的輔助旋鉚機械手可以實現(xiàn)在多臂機提綜臂旋鉚加工過程中將零部件快速、精準(zhǔn)地放至專用夾具，進(jìn)而有效提高生產(chǎn)效率，保證加工質(zhì)量。

2 動力學(xué)模型的建立

2.1 運動學(xué)分析

由于機構(gòu)是依靠平行四連桿驅(qū)動，且相對布置的兩桿相互平行，具有相同的運動規(guī)律。為簡化分析，以驅(qū)動桿、肘部連接架、單被動桿和動平臺構(gòu)造閉環(huán)矢量鏈，如圖3所示。以靜平臺中心點O為原點，建立慣性坐標(biāo)系O-XYZ，設(shè)驅(qū)動桿與Y軸夾角為θi，G點坐標(biāo)為[0yz]T，i為支鏈序號，且當(dāng)i=1，sgn（i）=-1；i=2，sgn（i）=1，可得到以下矢量：

圖3 閉環(huán)矢量鏈Fig.3 Closed-loop vector chain

由圖3可建立閉環(huán)矢量方程，將所有桿件視為剛性桿，對向量DE求模，得到如下位置約束方程：

2.1 .1 位置逆解

對式（2）進(jìn)行化簡，可得

2.1 .2 位置正解

位置正解對于并聯(lián)機構(gòu)的控制具有重要意義，其通常需借助數(shù)值迭代法（如牛頓迭代法[21]）進(jìn)行求解。由于牛頓迭代法對初始點的要求較高，若給定的初始點位置與實際初始點位置偏離較大，則可能會導(dǎo)致無法求得正確解，甚至無法收斂。因本文機構(gòu)的結(jié)構(gòu)特殊，可以得到其解析正解，這將有助于后續(xù)基于任務(wù)空間的高效控制策略設(shè)計。

對式（2）展開，分離出變量y、z，得到以下方程：

當(dāng)i=1、i=2時，根據(jù)式（5）可得2個二元二次方程，聯(lián)立消元整理后可得

根據(jù)一元二次方程求根公式解得

2.1 .3 速度分析

對約束方程式（2）關(guān)于時間求導(dǎo)得

由此可得輸入與輸出之間的運動映射關(guān)系為

對式（10）做矩陣變換，得到

式中：J為機構(gòu)的運動雅可比矩陣，且

2.2 剛體動力學(xué)分析

剛體動力學(xué)建模旨在給出機構(gòu)的尺度和慣性參數(shù)的條件下，建立驅(qū)動關(guān)節(jié)輸入力（力矩）與末端動平臺輸出運動之間的映射關(guān)系，它是評價機構(gòu)動態(tài)操作性能、物理樣機設(shè)計、伺服電機選型以及運動控制策略設(shè)計的基礎(chǔ)。本文采用Lagrange方法建立機器人的剛體動力學(xué)模型。

2.2 .1 結(jié)構(gòu)簡化及能量計算

由于求解完整的動力學(xué)模型較為復(fù)雜，且考慮到后續(xù)控制器運算的高效性，對部分桿件做適當(dāng)簡化，對一些非線性項進(jìn)行忽略，在此利用以下假設(shè)：

（1）運動副為理想的，無因摩擦而引起的能量耗散；各桿件為剛體，不考慮在高速運動下慣性力引致的彈性變形。

（2）考慮到被動桿為輕質(zhì)桿，忽略其轉(zhuǎn)動時帶來的慣性力，其質(zhì)量按靜力等效原則，以其質(zhì)量的1/2分別等效到被動桿兩端。

取驅(qū)動桿轉(zhuǎn)軸平面為零勢能面，以下為對單條支鏈各部分的能量分析。動平臺的動能：

動平臺的勢能：

式中：m1為動平臺質(zhì)量；g為自然重力加速度（取9.8 m/s2）

主動桿組（包括驅(qū)動桿與輔助連桿）動能：由于驅(qū)動桿與輔助連桿具有相同的運動規(guī)律，故主動桿組的動能可寫為

式中：I21為驅(qū)動桿繞基座轉(zhuǎn)軸的轉(zhuǎn)動慣量；I22為輔助連桿繞基座轉(zhuǎn)軸的轉(zhuǎn)動慣量。

主動桿組的勢能：

式中：m21為驅(qū)動桿的質(zhì)量；m22為輔助連桿的質(zhì)量；r21為驅(qū)動桿質(zhì)心到其轉(zhuǎn)軸中心的距離；r22為輔助連桿質(zhì)心到其轉(zhuǎn)軸中心的距離；h為驅(qū)動桿轉(zhuǎn)軸中心與輔助連桿轉(zhuǎn)軸中心的z向垂直距離。

中間關(guān)節(jié)的動能：

中間關(guān)節(jié)的勢能：

式中：I3為中間關(guān)節(jié)的繞驅(qū)動軸的轉(zhuǎn)動慣量；r3為中間關(guān)節(jié)質(zhì)心到驅(qū)動軸的距離；m3為中間肘部關(guān)節(jié)的質(zhì)量。

2.2 .2 建立系統(tǒng)的逆剛體動力學(xué)模型

考慮到機器人機構(gòu)為含閉環(huán)約束的多體系統(tǒng)，采用第一類Lagrange方程：

式中：L為Lagrange能量函數(shù)；qi（i=1，2，…）為廣義坐標(biāo)；τi為廣義力；fj為第j個約束方程；λj為第j個Lagrange乘子。選取系統(tǒng)廣義坐標(biāo)向量為：

根據(jù)機器人閉環(huán)矢量方程，以桿長為約束，可列得如下約束方程：

將簡化的模型代入動能表達(dá)式，求取系統(tǒng)總動能為：

系統(tǒng)總勢能為：

將上述式子分別代入式（19），求得各廣義力：

廣義力τ1：

廣義力τ3：

因機構(gòu)僅有主動關(guān)節(jié)處安裝伺服電機，故τ3=0且τ4=0，將其代入式（26）和（27）得到方程組：

對式（28）方程組進(jìn)行消元，求得拉格朗日乘子，其可表征機構(gòu)的廣義約束力：

式中：|J1|表示矩陣J1的行列式。

將求得的拉格朗日乘子代入上面動力學(xué)方程組，整理得到：

式中：

3 動力學(xué)控制策略

與一般機械系統(tǒng)一樣，當(dāng)機器人結(jié)構(gòu)及其相關(guān)參數(shù)確定后，其動態(tài)特性可由系統(tǒng)動力學(xué)方程來描述。因此，可通過基于系統(tǒng)數(shù)學(xué)模型設(shè)計相應(yīng)的控制器來實現(xiàn)對機器人的運動控制。對于具有強耦合性、非線性的機械系統(tǒng)而言，PD（P表示比例系數(shù)，D表示微分系數(shù)）反饋控制是最為簡單且行之有效的控制方法[7]。為實現(xiàn)高速下的機器人運動控制，本文計及系統(tǒng)動力學(xué)因素，對輔助旋鉚機械手設(shè)計具有PD補償和非線性補償?shù)挠嬎懔乜刂破?。其中，PD補償是對慣性項進(jìn)行補償，非線性項是對重力項、離心力與科氏力項進(jìn)行補償。在實際應(yīng)用中，計算力矩實質(zhì)上是通過求出驅(qū)動電機的輸出力矩，進(jìn)而通過改變電機的輸出電壓（電流）實現(xiàn)對機械系統(tǒng)的精確控制。

在本文研究中，暫不考慮摩擦力帶來的影響，后期可在樣機調(diào)試中對這部分力矩進(jìn)行辨識/補償。

根據(jù)系統(tǒng)逆動力學(xué)方程，可以得到驅(qū)動力矩與末端執(zhí)行器運動的映射關(guān)系為：

在輸入力矩中，離心力與科氏力項以及重力項，均采用理論值作為補償，慣性項則采用PD進(jìn)行補償，設(shè)計任務(wù)空間控制輸入力矩為：

式中：e=xd-x；Kp為位置增益對角矩陣；Kd為速度增益對角矩陣；x為末端執(zhí)行器實際位置向量；xd為末端執(zhí)行器理論位置向量。

假設(shè)控制輸入力矩與驅(qū)動力矩相同，可以得到：

上式化簡后得到一個關(guān)于時間的常系數(shù)二階齊次線性微分方程：

根據(jù)常系數(shù)二階齊次微分方程解的形式可知，誤差函數(shù)可由指數(shù)函數(shù)線性表達(dá)。因此，通過調(diào)整PD參數(shù)，可使誤差漸進(jìn)趨近于0，即末端軌跡的控制精度可以得到保證?？刂圃砜驁D如圖4所示。

圖4 計算力矩控制原理框圖Fig.4 Diagram of computed torque control（CTC）

4 基于Simulink與ADAMS的聯(lián)合仿真實驗

為了驗證設(shè)計的動力學(xué)控制策略的合理性與有效性，以及便于直觀地觀察機器人控制效果與運動狀況，現(xiàn)結(jié)合控制策略與虛擬樣機技術(shù)進(jìn)行聯(lián)合虛擬仿真。通過聯(lián)合仿真，可以在虛擬環(huán)境下調(diào)試并發(fā)現(xiàn)設(shè)計中存在的問題，并及時對控制參數(shù)做出修正與調(diào)整，降低樣機研發(fā)成本，為未來樣機制造提供理論依據(jù)和技術(shù)儲備。

在此通過MATLAB軟件中的Simulink模塊與ADAMS軟件中的Control模塊實施聯(lián)合虛擬仿真實驗。主要思路是：在Simulink中按照圖4搭建控制模型，起到模擬控制器的作用；在ADAMS軟件中導(dǎo)入機器人的SolidWorks虛擬樣機，起到模擬機構(gòu)運動的作用。最終搭建的Simulink-Adams聯(lián)合控制仿真模型如圖5所示。

圖5 Simulink-ADAMS聯(lián)合控制仿真模型Fig.5 Simulink-ADAMSintegrated control simulation model

根據(jù)工作任務(wù)需求，設(shè)計“門字形”軌跡作為末端執(zhí)行器的運動軌跡，并采用修正梯形加速度運動規(guī)律[4]，取最大加速度amax＝3g。仿真中，給出機器人的尺度及質(zhì)量參數(shù)如下：l1=0.06 m，l2=0.44 m，l3=0.07 m，l4=0.09 m，l5=0.515 m，l6=l7=0.060 m，r21=0.185 m，r22=0.22 m，r3=0.465 m；m1=0.90 kg，m21=1.16 kg，m22=0.10 kg，m3=1.03 kg，I21=0.06 kg·m2，I21=0.11 kg·m2，I22=0.006 5 kg·m2，I3=0.22 kg·m2。通過試湊法，整定控制參數(shù)為：Kp=diag（5 000，5 000），Kd=diag（2 000，2 000）。需要說明的是，由于該模型忽略了系統(tǒng)中的摩擦力等影響，最佳控制參數(shù)還需在實際中進(jìn)行調(diào)試，在此僅是提供一個參考的范圍。

根據(jù)ADAMS輸出的仿真動畫，可以看到聯(lián)合仿真下，機構(gòu)在不同時刻的運動狀態(tài)如圖6所示。

圖6 機構(gòu)在不同時刻的運動位姿Fig.6 Poses of mechanism in different time

將仿真實驗結(jié)果導(dǎo)出，通過理想軌跡與仿真軌跡的對比，可得到機械手運動過程中末端執(zhí)行器位置跟蹤及誤差的變化情況，分別如圖7和圖8所示。主動關(guān)節(jié)控制輸入力矩如圖9所示。

結(jié)合圖7和圖8發(fā)現(xiàn)，機械手在y向的跟蹤誤差控制在0.3 mm以內(nèi)，在z向的跟蹤誤差也能控制在0.65 mm以內(nèi)。由于z向僅是控制機械手的上下移動，并不影響y向的定位，所以使用該控制策略，理論上滿足設(shè)計要求。另外由圖9可看出，控制力矩峰值大小為37.82 N·m，且力矩在軌跡跟蹤過程中波動平緩，無振蕩和突變，這將有利于實際控制。綜上，對于高速運動的輔助旋鉚并聯(lián)機械手而言，利用設(shè)計的任務(wù)空間計算力矩控制方案，可以獲得較高的運動精度以及較好的控制性能，同時亦為其他高速并聯(lián)機器人的運動控制提供了技術(shù)參考。

圖7 末端執(zhí)行器軌跡對比Fig.7 Comparison of trajectory of end-effector

圖8 末端執(zhí)行器位置跟蹤誤差Fig.8 Position tracking errorsof end-effector

圖9 主動關(guān)節(jié)控制力矩Fig.9 Control torques of active joints

5 結(jié)論

（1）在對輔助旋鉚機械手結(jié)構(gòu)分析基礎(chǔ)上，構(gòu)造了單支鏈的閉環(huán)矢量鏈。運用閉環(huán)矢量法，建立了單支鏈的位置約束方程，得到了位置正逆解的解析表達(dá)式；通過求導(dǎo)法，建立了速度映射模型。

（2）基于第一類Lagrange方程，并結(jié)合系統(tǒng)閉環(huán)約束方程，建立了機械手的剛體動力學(xué)模型，其中的Lagrange乘子可表征系統(tǒng)廣義約束力。通過消元，進(jìn)一步得到了驅(qū)動力矩與末端執(zhí)行器參考點運動之間的函數(shù)映射關(guān)系。

（3）設(shè)計了任務(wù)空間計算力矩控制策略，考慮了由PD補償來實現(xiàn)對慣性項的補償，由離心力、科氏力項以及重力項的理論值作為非線性補償。經(jīng)推導(dǎo)可知，選擇合適的PD參數(shù)，可使跟蹤誤差漸進(jìn)趨近于0。

（4）搭建了Simulink-Adams聯(lián)合虛擬仿真模型。仿真實驗結(jié)果表明，在最大加速度為3g時，設(shè)計的任務(wù)空間動力學(xué)控制策略可使末端執(zhí)行器的y向跟蹤誤差在0.3 mm以內(nèi)，z向的跟蹤誤差也能控制在0.65 mm以內(nèi)，且控制力矩波動平緩，符合設(shè)計預(yù)期。需指出，本文給出的PD參數(shù)僅為參考值，實際應(yīng)用時仍需結(jié)合實體樣機進(jìn)行調(diào)試試驗。