基于動(dòng)力學(xué)的協(xié)作機(jī)械臂零力拖動(dòng)研究

劉鑌震，潘松峰，李先弘，王先月，伊永祺

(青島大學(xué) 自動(dòng)化學(xué)院，山東 青島 266000)

0 引言

隨著機(jī)械臂產(chǎn)業(yè)的發(fā)展及用人成本的上升，我國(guó)制造業(yè)向著智能化、無(wú)人化的方向快速發(fā)展。由于協(xié)作機(jī)械臂具有一定人機(jī)交互能力，因此其不僅出現(xiàn)在商場(chǎng)等民用領(lǐng)域，更在軍用、醫(yī)療等領(lǐng)域有了突飛猛進(jìn)的發(fā)展。零力拖動(dòng)，即由操作員直接拖動(dòng)機(jī)器人各關(guān)節(jié)運(yùn)動(dòng)至理想位姿并進(jìn)行記錄。作為協(xié)作機(jī)械臂與操作者共同工作的一種協(xié)作方案，具有重要的研究意義。

針對(duì)某公司一種基于開(kāi)環(huán)力矩控制方法的6軸協(xié)作機(jī)械臂開(kāi)發(fā)平臺(tái)進(jìn)行機(jī)械臂本體動(dòng)力學(xué)分析，并結(jié)合電機(jī)模型對(duì)機(jī)器人進(jìn)行關(guān)于零力拖動(dòng)的相關(guān)研究，利用實(shí)驗(yàn)對(duì)摩擦補(bǔ)償參數(shù)等進(jìn)行校準(zhǔn)，確保機(jī)械臂可在工作范圍內(nèi)實(shí)現(xiàn)零力拖動(dòng)。機(jī)械臂本體及相關(guān)尺寸數(shù)據(jù)如圖1所示。

圖1 機(jī)械臂本體及相關(guān)尺寸圖

1 動(dòng)力學(xué)建模

采用改進(jìn)D-H參數(shù)法對(duì)6自由度協(xié)作機(jī)械臂進(jìn)行建模，其機(jī)械臂結(jié)構(gòu)原理如圖2所示，D-H參數(shù)如表1所示。

由于機(jī)器人從關(guān)節(jié)4之后桿長(zhǎng)均為0，因此僅對(duì)協(xié)作機(jī)械臂的前3個(gè)關(guān)節(jié)進(jìn)行研究。

機(jī)械臂動(dòng)力學(xué)的機(jī)器人本體動(dòng)力學(xué)研究常見(jiàn)有Lagrange法[1-3]、Newton-Euler法[4-6]和Dalembert法[7-8]等。為了便于計(jì)算及具象化研究，本文采用Lagrange法進(jìn)行動(dòng)力學(xué)建模與分析。

圖2 6自由度機(jī)械臂結(jié)構(gòu)原理圖

表1 D-H參數(shù)表

Lagrange法的數(shù)學(xué)模型如式(1)所示。

L=K-P

(1)

式中：L為L(zhǎng)agrange函數(shù)；K為機(jī)械臂本體系統(tǒng)動(dòng)能；P為系統(tǒng)位能。在此基礎(chǔ)上動(dòng)力學(xué)方程可表示為[9]

(2)

結(jié)合式(1)、式(2)，機(jī)械臂的本體動(dòng)力學(xué)也可以寫作式(3)形式：

(3)

將式(3)中的哥氏力和離心力分離，機(jī)械臂的本體動(dòng)力學(xué)也可寫作式(4)形式：

(4)

式中C(q)和F(q)均為3×3的矩陣。

系統(tǒng)位能考慮慣性帶來(lái)的勢(shì)能，因此采用轉(zhuǎn)動(dòng)慣量法計(jì)算力矩。機(jī)械臂的質(zhì)量分布在伺服電機(jī)及連桿上，產(chǎn)生繞中心軸線旋轉(zhuǎn)(伺服電機(jī))和繞遠(yuǎn)端關(guān)節(jié)旋轉(zhuǎn)(連桿)兩種轉(zhuǎn)動(dòng)慣量，如圖3(a)、圖3(b)所示，參數(shù)示意圖如圖3(c)所示。

圖3 轉(zhuǎn)動(dòng)慣量及機(jī)械臂參數(shù)示意圖

中心軸線轉(zhuǎn)動(dòng)慣量由伺服電機(jī)形成，即Jm1、Jm3和Jm5，計(jì)算方式如式(5)所示；遠(yuǎn)端關(guān)節(jié)轉(zhuǎn)動(dòng)慣量由連桿形成，即Jm2、Jm4和Jm6，計(jì)算方式如式(6)所示。

(5)

(6)

其中各字符含義如圖3(a)、圖3(b)所示。圖3(c)中：l2表示關(guān)節(jié)1電機(jī)和關(guān)節(jié)2電機(jī)之間，連桿質(zhì)心與電機(jī)1質(zhì)心之間的距離；l3表示關(guān)節(jié)1電機(jī)和關(guān)節(jié)2電機(jī)之間，連桿質(zhì)心與關(guān)節(jié)2電機(jī)之間的距離；關(guān)節(jié)1和關(guān)節(jié)2之間距離很短，此處忽略，從而得到機(jī)械臂動(dòng)力學(xué)所需轉(zhuǎn)動(dòng)慣量的物理參數(shù)。

由此建立起機(jī)械臂本體的動(dòng)力學(xué)模型，其動(dòng)力學(xué)方程中的具體參數(shù)為：

m6l3l6)c2-2m6l5l6c3-2m6l3l6c23，

(m4l3l4+m5l3l5+m6l3l5)c2-2m6l5l6cosθ3-m6l3l6c23，

c11=0,

c12=-(m4l3l4+m5l3l5+m6l3l5)s2+m6l3l6s23，

c13=m6l5l6s3+m6l3l6s23，

g1=[m2l2+m3l3+(m4+m5+m6)l3]gc1+(m4l4+m5l5+m6l5)gc12+m6gl6c123。

推導(dǎo)結(jié)果較為復(fù)雜且篇幅有限，不再贅述。此處本體動(dòng)力學(xué)模型處于理想狀態(tài)下，而在實(shí)際模型中電機(jī)除輸出式(4)所得力矩外，還需考慮庫(kù)侖摩擦力矩τf及黏性摩擦力矩τg，其計(jì)算方式如式(7)、式(8)所示。

(7)

τg=N·sign(q)

(8)

式中：M為黏滯系數(shù)矩陣；N為庫(kù)侖力向量；q為關(guān)節(jié)輸出角度而非電機(jī)輸出角度。將式(7)、式(8)代入式(4)中，并加入機(jī)械臂各軸電機(jī)的減速比，得到各軸電機(jī)的理論輸出力矩如式(9)所示。

(9)

(10)

式中ri表示第i個(gè)關(guān)節(jié)電機(jī)的減速比，由電機(jī)固件決定。通過(guò)關(guān)節(jié)電機(jī)減速比矩陣T將關(guān)節(jié)力矩與電機(jī)實(shí)際輸出力矩結(jié)合。

2 電機(jī)模型

電機(jī)是機(jī)械臂關(guān)節(jié)力矩的提供者，因此需要將電機(jī)模型與零力模型相結(jié)合。本文采用永磁同步電機(jī)PMSM，數(shù)學(xué)模型如式(11)所示[10]。

(11)

式中：ud、uq分別為永磁同步電機(jī)d軸和q軸定子的電壓；id、iq分別為永磁同步電機(jī)d軸和q軸定子的電流；ω為轉(zhuǎn)子角速度；Φ為電機(jī)永磁體磁鏈；J為轉(zhuǎn)動(dòng)慣量；TL為電機(jī)負(fù)載轉(zhuǎn)矩；Te為電磁轉(zhuǎn)矩。由于在式(9)中已從能量觀點(diǎn)對(duì)轉(zhuǎn)動(dòng)慣量進(jìn)行運(yùn)算，電機(jī)側(cè)不需再對(duì)轉(zhuǎn)動(dòng)慣量進(jìn)行單獨(dú)運(yùn)算。

以單關(guān)節(jié)電機(jī)為例對(duì)電機(jī)模型進(jìn)行研究。如圖4所示，機(jī)械臂關(guān)節(jié)屬于多軸傳動(dòng)系統(tǒng)，電機(jī)與關(guān)節(jié)之間由減速器連接，ωS為電機(jī)側(cè)轉(zhuǎn)子轉(zhuǎn)速，qdi為電機(jī)輸出角度，ωL為關(guān)節(jié)側(cè)轉(zhuǎn)速，qi為關(guān)節(jié)實(shí)際輸出角度，四者關(guān)系如式(12)所示。

(12)

圖4 電機(jī)傳動(dòng)示意圖

(13)

(14)

其結(jié)構(gòu)圖如圖5所示，實(shí)現(xiàn)閉環(huán)控制方案。

圖5 零力拖動(dòng)模型結(jié)構(gòu)圖

3 零力控制模型

零力控制模型就是將考慮各類摩擦及慣性力的機(jī)器人本體動(dòng)力學(xué)方程與電機(jī)模型結(jié)合得到的包含式(9)和式(14)的綜合模型，人為拖動(dòng)過(guò)程力矩模型如式(15)所示。

(15)

式中P為力矩矩陣，其表達(dá)式為

(16)

4 實(shí)驗(yàn)與分析

利用6軸協(xié)作機(jī)械臂開(kāi)發(fā)平臺(tái)所自帶的運(yùn)動(dòng)傳感器及動(dòng)力學(xué)參數(shù)識(shí)別，以第2關(guān)節(jié)為例進(jìn)行相關(guān)實(shí)驗(yàn)。其實(shí)驗(yàn)驗(yàn)證圖如圖6所示，J2(2)為庫(kù)侖摩擦系數(shù)，對(duì)應(yīng)式(15)中的N，J2(3)為黏性摩擦系數(shù)，對(duì)應(yīng)式(15)中的M。

圖6 單關(guān)節(jié)零力拖動(dòng)驗(yàn)證圖

首先將模型拷入電機(jī)控制卡并對(duì)其參數(shù)進(jìn)行預(yù)設(shè)。預(yù)設(shè)M=N=10，圖6中J2(4)為動(dòng)力學(xué)補(bǔ)償參數(shù)，應(yīng)為1。預(yù)設(shè)參數(shù)的目的在于避免機(jī)械臂在零力狀態(tài)下因重力導(dǎo)致限位，保證其安全性。隨后通過(guò)對(duì)庫(kù)侖摩擦力矩和黏性摩擦力矩補(bǔ)償，得到M和N的最優(yōu)值，確保機(jī)械臂可進(jìn)行零力拖動(dòng)。找到最優(yōu)值的方法為：當(dāng)所給定M和N過(guò)小時(shí)，會(huì)因補(bǔ)償不足無(wú)法實(shí)現(xiàn)零力拖動(dòng)；當(dāng)所給定M和N過(guò)大時(shí)，可實(shí)現(xiàn)零力拖動(dòng)，但會(huì)因補(bǔ)償過(guò)度導(dǎo)致卡死或拖動(dòng)困難。機(jī)械臂廠商在只補(bǔ)償重力、庫(kù)侖摩擦力和黏性摩擦力的情況下給定M的最優(yōu)值為25，N的最優(yōu)值為42。最后通過(guò)比對(duì)M和N的方式驗(yàn)證模型準(zhǔn)確性。

通過(guò)驗(yàn)證可知，當(dāng)庫(kù)侖摩擦參數(shù)M>15、黏性摩擦參數(shù)N>30時(shí)有較好的零力拖動(dòng)效果。在此條件下通過(guò)人為拖動(dòng)機(jī)械臂，轉(zhuǎn)動(dòng)第二關(guān)節(jié)使其關(guān)節(jié)角從0°拖動(dòng)至30°位置，得到庫(kù)侖摩擦力矩、黏性摩擦力矩、動(dòng)力學(xué)力矩與時(shí)間關(guān)系如圖7所示。

圖7 庫(kù)侖摩擦力矩、黏性摩擦力矩、動(dòng)力學(xué)力矩與時(shí)間關(guān)系圖

由于零力拖動(dòng)需人為進(jìn)行操作，因此圖7中黏性摩擦力矩在1.5s后呈現(xiàn)出抖動(dòng)現(xiàn)象為人為操作影響。通過(guò)驗(yàn)證，當(dāng)庫(kù)侖摩擦參數(shù)M=20、黏性摩擦參數(shù)N=40時(shí)達(dá)到機(jī)械臂零力拖動(dòng)的最佳狀態(tài)。二者均小于廠商所給定的數(shù)值。原因是由于廠商給定M和N時(shí)并未對(duì)其他慣性力等補(bǔ)償，只能通過(guò)增大庫(kù)侖摩擦力矩和黏性摩擦力矩間接補(bǔ)償其他慣性力。由此證明模型的準(zhǔn)確性，并為零力拖動(dòng)準(zhǔn)確控制提供了依據(jù)。

5 結(jié)語(yǔ)

針對(duì)一種6自由度協(xié)作機(jī)械臂，利用動(dòng)力學(xué)方程、靜力學(xué)原理及電機(jī)轉(zhuǎn)動(dòng)軸參數(shù)關(guān)系對(duì)零力拖動(dòng)進(jìn)行研究。通過(guò)實(shí)驗(yàn)平臺(tái)驗(yàn)證的方法對(duì)模型參數(shù)進(jìn)行補(bǔ)償，當(dāng)庫(kù)侖摩擦參數(shù)M=20、黏性摩擦參數(shù)N=40時(shí)準(zhǔn)確地實(shí)現(xiàn)了機(jī)械臂的零力拖動(dòng)控制，證明了模型的準(zhǔn)確性。此模型較于傳統(tǒng)的重力補(bǔ)償零力拖動(dòng)控制而言，采用動(dòng)力學(xué)及摩擦力補(bǔ)償方式從電機(jī)角度實(shí)現(xiàn)了零力拖動(dòng)的準(zhǔn)確控制，解決了控制精度不足的問(wèn)題，也為日后研究工業(yè)協(xié)作機(jī)械臂的動(dòng)力學(xué)控制夯實(shí)了基礎(chǔ)。