3-DOF Delta并聯(lián)手控器逆運(yùn)動(dòng)學(xué)研究及MATLAB/SimMachenics仿真

（河南工業(yè)大學(xué) 機(jī)器人研究所，鄭州 450000）

0 引言

人機(jī)交互是遙操作技術(shù)的顯著特點(diǎn)之一，手控器作為力覺(jué)人機(jī)交互重要設(shè)備，成為近年來(lái)遙操作及力覺(jué)臨場(chǎng)感技術(shù)中重要的研究課題之一[1]。手控器按結(jié)構(gòu)類(lèi)型可以分為串聯(lián)式、并聯(lián)式和復(fù)合式。由于串聯(lián)式手控器存在較多奇異點(diǎn)，為此研究者提出采用并聯(lián)結(jié)構(gòu)的手控器，這種結(jié)構(gòu)具有較強(qiáng)的承載力以及高運(yùn)動(dòng)精度，因此逐漸成為目前研究熱點(diǎn)。并聯(lián)結(jié)構(gòu)出現(xiàn)自二十世紀(jì)30年代，在80年代洛桑理工學(xué)院Revmond Clavel教授提出了Delta并聯(lián)機(jī)器人的概念。由于其優(yōu)異的性能，目前Delta結(jié)構(gòu)已經(jīng)被廣泛應(yīng)用于各種手控器結(jié)構(gòu)中。并且國(guó)內(nèi)外眾多學(xué)者對(duì)其運(yùn)動(dòng)學(xué)分析及建模等方面也都進(jìn)行了積極的探索[2,3]。

典型的三自由度Delta結(jié)構(gòu)手控器由三個(gè)對(duì)稱(chēng)的運(yùn)動(dòng)支鏈、動(dòng)平臺(tái)和靜平臺(tái)組成。支鏈與靜平臺(tái)通過(guò)電機(jī)相連，電機(jī)轉(zhuǎn)動(dòng)帶動(dòng)支鏈運(yùn)動(dòng)從而使動(dòng)平臺(tái)運(yùn)動(dòng)，同時(shí)通過(guò)控制動(dòng)平臺(tái)運(yùn)動(dòng)可以通過(guò)支鏈將相應(yīng)運(yùn)動(dòng)傳遞給電機(jī)。對(duì)手控器進(jìn)行有效控制首先要研究其正/逆運(yùn)動(dòng)學(xué)特性。并聯(lián)機(jī)構(gòu)手控器逆運(yùn)動(dòng)學(xué)所研究的問(wèn)題是操作動(dòng)平臺(tái)運(yùn)動(dòng)時(shí)求解相應(yīng)驅(qū)動(dòng)支鏈的輸入角度。Mustafa等人采用解析法對(duì)Delta結(jié)構(gòu)并聯(lián)機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)正解研究，分析了三種典型關(guān)節(jié)向量下的動(dòng)平臺(tái)位姿[4]。文獻(xiàn)[5]對(duì)并聯(lián)手控器進(jìn)行運(yùn)動(dòng)學(xué)分析并引入重力補(bǔ)償模型，從而提高了機(jī)構(gòu)操作舒適度。Lipkin和Duffy等人采用基于扭矩的對(duì)偶性關(guān)系提出對(duì)扭矩控制的新方法[6]。雖然此類(lèi)方法很多，但是由于并聯(lián)Delta結(jié)構(gòu)手控器逆運(yùn)動(dòng)學(xué)模型復(fù)雜，在線計(jì)算量大，控制效果不理想，因此在實(shí)際中應(yīng)用較少[7]。

以MATLAB/SimMachenics工具箱作為開(kāi)發(fā)工具，對(duì)Delta并聯(lián)手控器進(jìn)行建模仿真并分析其逆運(yùn)動(dòng)學(xué)規(guī)律。根據(jù)寧祎提出的手控器性能評(píng)價(jià)方法[8]，為了避免設(shè)計(jì)上的盲目性，在設(shè)計(jì)過(guò)程中可以通過(guò)使用MATLAB/SimMachenics仿真的方法驗(yàn)證設(shè)計(jì)的正確性，為進(jìn)一步研究提供方便。

1 3-DOF并聯(lián)手控器構(gòu)型介紹與坐標(biāo)系建立

由于并聯(lián)型手控器具有結(jié)構(gòu)剛度大，操作靈活并且能夠?qū)Σ僮髡弋a(chǎn)生較大的反饋力，適用于面向特殊復(fù)雜情況下的遙操作系統(tǒng)。Delta并聯(lián)型手控器由靜平臺(tái)、三組支鏈、動(dòng)平臺(tái)組成。其結(jié)構(gòu)如圖1所示。靜平臺(tái)與動(dòng)平臺(tái)之間由三條運(yùn)動(dòng)支鏈相連，設(shè)支鏈與靜平臺(tái)之間三個(gè)連接點(diǎn)分別為A1、A2、A3，三個(gè)連接點(diǎn)均勻分布在以O(shè)為圓心，半徑為r0的A1A2A3外接圓上。對(duì)靜平臺(tái)建立坐標(biāo)系，以O(shè)為原點(diǎn)建立O-XYZ固定坐標(biāo)系，其中O-X經(jīng)過(guò)A1點(diǎn)，O-Ai與X軸夾角為αi(i=1,2,3)，以A1為中心建立坐標(biāo)系A(chǔ)i-XYZ，坐標(biāo)系X軸與O-X同向。θi為轉(zhuǎn)動(dòng)副Ai的轉(zhuǎn)動(dòng)角度。動(dòng)平臺(tái)與運(yùn)動(dòng)支鏈之間的連接點(diǎn)分別為C1、C2、C3，與靜平臺(tái)相同，動(dòng)平臺(tái)上的三條運(yùn)動(dòng)支鏈同樣均勻分布在以P為圓心，半徑為rp的三角形C1C2C3外接圓上。建立P-XYZ坐標(biāo)系，P-X軸過(guò)C1點(diǎn)，P-X與P?Ci(i=1,2,3)之間夾角為αi(i=1,2,3)。Bi為連接兩條支鏈的轉(zhuǎn)動(dòng)副，支鏈AiBi的長(zhǎng)度為l，BiCi的長(zhǎng)度為m。同時(shí)建立動(dòng)坐標(biāo)系Oi?XYZ(i=1,2,3)，Oi與O重合。在運(yùn)動(dòng)平臺(tái)以Pi為原點(diǎn)建立動(dòng)坐標(biāo)系Pi?XYZ(i=1,2,3)，Pi-X過(guò)Ci點(diǎn)[9]。

圖1 并聯(lián)手控器運(yùn)動(dòng)學(xué)分析圖

2 逆運(yùn)動(dòng)學(xué)模型建立

逆運(yùn)動(dòng)學(xué)所研究的問(wèn)題是已知?jiǎng)悠脚_(tái)OP的運(yùn)動(dòng)情況，求靜平臺(tái)機(jī)構(gòu)的輸入角θ。根據(jù)上文中建立的手控器模型坐標(biāo)系，對(duì)第i個(gè)支鏈建立矢量閉環(huán)：

上式變換得：

化簡(jiǎn)得：

且：

由此可知，只需求出Bi、Ci點(diǎn)在Oi-XYZ坐標(biāo)系中的點(diǎn)矢量，再根據(jù)式(4)即可求得輸入角θi。設(shè)：P點(diǎn)在O-XYZ坐標(biāo)系中的向量為：

則P點(diǎn)在Oi-XYZ中的點(diǎn)矢量為：

其中ROOi為坐標(biāo)系O-XYZ到Oi-XYZ的轉(zhuǎn)換矩陣。根據(jù)手控器模型中設(shè)定O-Ai到X軸的夾角為αi(i=1,2,3)，得：

Ai點(diǎn)在Oi-XYZ中的矢量為：

Bi點(diǎn)在Ai-XYZ中的矢量為：

則，Bi點(diǎn)在O-XYZ中的矢量為：

在運(yùn)動(dòng)平臺(tái)的動(dòng)坐標(biāo)系Pi-XYZ中，Cpi為Ci點(diǎn)在Pi-XYZ坐標(biāo)系上的矢量，且：

CO為Ci點(diǎn)在O-XYZ上的矢量，因此得到：

RPiOi為坐標(biāo)系Pi-XYZ到坐標(biāo)系Oi-XYZ的轉(zhuǎn)換矩陣，其中：

因此將式(6)、式(7)代入式(3)即可求得BiCi的值，再由式(4)建立的方程得：

則方程(8)化簡(jiǎn)為：

由方程(10)解得：

至此便可得到靜平臺(tái)與支鏈連接的運(yùn)動(dòng)副的輸入角：

因此，當(dāng)給定運(yùn)動(dòng)平臺(tái)的位姿時(shí)，通過(guò)上式便可求得電機(jī)的輸入即驅(qū)動(dòng)臂的張角。

3 系統(tǒng)仿真

SimMechanics是MATLAB仿真的一個(gè)工具箱，其結(jié)合Simulink與MATLAB的功能于一體，通過(guò)一系列的關(guān)聯(lián)模塊組成系統(tǒng)的模型，同時(shí)提供可視化仿真工具將機(jī)械系統(tǒng)簡(jiǎn)化為直觀顯示的機(jī)構(gòu)。本文使用SimMechanics建立并聯(lián)手控器仿真平臺(tái)。SimMechanics工具箱為用戶(hù)提供了剛體模塊組（Bodies）、運(yùn)動(dòng)副模塊組（Joints）、約束和驅(qū)動(dòng)模塊組（Constraints&Drivers）、傳感器和驅(qū)動(dòng)器模塊組（Sensors&Actuators）、機(jī)械仿真輔助模塊組（Utilities）等機(jī)構(gòu)模塊，能夠?qū)Ω鞣N運(yùn)動(dòng)副連接的多剛體機(jī)構(gòu)進(jìn)行建模與仿真，從而實(shí)現(xiàn)對(duì)機(jī)構(gòu)系統(tǒng)動(dòng)態(tài)性能的分析[9]。

使用SimMechanics對(duì)系統(tǒng)建模首先需要選擇Ground、Body、Joint模塊。并且建立完整的仿真模型還需要Machine Environment模塊和Ground模塊。通過(guò)Machine Environment模塊可以設(shè)定系統(tǒng)的機(jī)械環(huán)境變量。通過(guò)Ground模塊可以將系統(tǒng)固定在慣性系統(tǒng)中，就是仿真系統(tǒng)中的定點(diǎn)，代表在世界坐標(biāo)系中一個(gè)靜止不動(dòng)的點(diǎn)。需要注意的是在連接中不能將傳感器和驅(qū)動(dòng)器連接在Ground上。Body模塊表示一個(gè)剛體構(gòu)件，包括質(zhì)量和慣性張量、重心坐標(biāo)以及一個(gè)或多個(gè)坐標(biāo)系統(tǒng)。Joint模塊是運(yùn)動(dòng)副模塊組。給定3-DOF手控器結(jié)構(gòu)尺寸參數(shù)如表1所示。

表1 3-DOF手控器結(jié)構(gòu)尺寸參數(shù)

本文根據(jù)并聯(lián)手控器的機(jī)械結(jié)構(gòu)在MATLAB/Simulink中建立仿真模型如圖2所示[10]。

圖2 并聯(lián)手控器的SimMechanics框圖

運(yùn)行后得到并聯(lián)手控器仿真模型，如圖3所示。

圖3 并聯(lián)手控器仿真模型

SimMechanics還帶有傳感器與執(zhí)行器模塊組（Sensor&Actuators）。該模塊組中的模塊是用來(lái)與Simulink模塊進(jìn)行數(shù)據(jù)交換的。運(yùn)動(dòng)學(xué)逆解要解決的問(wèn)題是給定動(dòng)平臺(tái)的位姿、速度、加速度，求解各驅(qū)動(dòng)關(guān)節(jié)的位置、速度、加速度。因此在系統(tǒng)搭建過(guò)程中需要在底部三個(gè)轉(zhuǎn)動(dòng)副處連接檢測(cè)模塊Joint Sensor。

仿真初始環(huán)境如圖3所示，設(shè)此時(shí)三個(gè)轉(zhuǎn)動(dòng)副的角度為0。設(shè)置仿真時(shí)間為10s，動(dòng)平臺(tái)受重力作用向下運(yùn)動(dòng)，到達(dá)底部后復(fù)位，重復(fù)此動(dòng)作兩次。仿真運(yùn)行后得到三個(gè)轉(zhuǎn)動(dòng)副的轉(zhuǎn)角大小如圖4所示。

圖4 轉(zhuǎn)動(dòng)副Ai(i=1,2,3)的角度曲線

從圖中可以明顯看到完整運(yùn)行一次所用時(shí)間為5s，因此分析前5s三個(gè)轉(zhuǎn)動(dòng)副的輸入角變化曲線。從圖中可以看出動(dòng)平臺(tái)在一個(gè)周期內(nèi)移動(dòng)時(shí)三個(gè)轉(zhuǎn)動(dòng)副的轉(zhuǎn)動(dòng)角度的模是相同的，并且只有一個(gè)解。結(jié)果符合實(shí)際要求。

SimMachenics的另一個(gè)優(yōu)勢(shì)在于在仿真過(guò)程中可以觀看機(jī)構(gòu)運(yùn)行的實(shí)時(shí)動(dòng)畫(huà)。如圖5所示為某一時(shí)刻機(jī)構(gòu)運(yùn)行動(dòng)畫(huà)截圖。

圖5 SimMachenics運(yùn)行動(dòng)畫(huà)截圖

4 結(jié)束語(yǔ)

并聯(lián)型手控器結(jié)構(gòu)的復(fù)雜性及各桿件支鏈的耦合問(wèn)題，給系統(tǒng)控制帶來(lái)了困難。本文對(duì)Dleta型手控器進(jìn)行了逆運(yùn)動(dòng)學(xué)分析，采用SimMechanics仿真的方法，可快速得出與動(dòng)平臺(tái)位姿對(duì)應(yīng)的各運(yùn)動(dòng)支鏈應(yīng)輸入的角度。在理論上，Dleta型手控器的運(yùn)動(dòng)學(xué)逆解存在多值性，在實(shí)際結(jié)構(gòu)中，各支鏈的輸入角受到幾何約束，其運(yùn)動(dòng)學(xué)逆解是唯一的。為了驗(yàn)證運(yùn)動(dòng)學(xué)逆解，本文采用SimMechanics仿真，對(duì)并聯(lián)手控器進(jìn)行建模和分析，可快速得到運(yùn)動(dòng)學(xué)逆解，為手控器的機(jī)構(gòu)設(shè)計(jì)提供了便

【】【】利。SimMechanics工具箱采用可視化模塊將機(jī)械系統(tǒng)簡(jiǎn)化為直觀顯示的機(jī)構(gòu)模型，使原本看似復(fù)雜的機(jī)構(gòu)建模，通過(guò)SimMechanics工具箱可容易解決。研究表明，用Matlab/SimMechanics平臺(tái)對(duì)Dleta型手控器進(jìn)行仿真分析便捷高效，為并聯(lián)手控器的機(jī)構(gòu)設(shè)計(jì)和控制研究提供了一種有效的技術(shù)手段。

[1]倪濤,朱厚文,張紅彥,黃玲濤,鄭幻飛,鞏明德.基于手控器觸覺(jué)共享控制的主從遙操作[J].農(nóng)業(yè)機(jī)械學(xué)報(bào),2016,(12):367-372+423.

[2]楊斌久,蔡光起,朱春霞.少自由度并聯(lián)機(jī)器人的研究現(xiàn)狀[J].機(jī)床與液壓,2006(5):201-205.

[3]Mustafa M, MisuariR, Daniyal H. Forward kinematics of 3 degree of freedom delta robot[J].Research and Development,2007(5):1-4.

[4]H. Lipkin, J. Duffy, Hybrid twist and wrench control for a robotic manipulator[J].MachineDesign,2002(6):138-144.

[5]余昆.并聯(lián)手控器運(yùn)動(dòng)學(xué)分析及重力補(bǔ)償研究[D].華中科技大學(xué),2014.

[6]Brogardh Torgny. Present and Future Robot Control Development-An Industrial Perspective[J].Annual Reviews in Control,2007(31):69-79.

[7]許禮進(jìn),劉有余,劉陽(yáng).DELTA機(jī)器人逆運(yùn)動(dòng)學(xué)的分析與控制[J].制造業(yè)自動(dòng)化,2014,(17):66-69.

[8]寧祎. 手控器的性能和評(píng)價(jià)方法[J].機(jī)器人,2000,(04):282-288.

[9]宮赤坤,熊吉光,黃成林.Delta并聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)仿真分析[J].制造業(yè)自動(dòng)化,2013,(05):5-7+14.

[10]王英波,黃其濤,鄭書(shū)濤,韓俊偉,許宏光.Simulink和SimMechanics環(huán)境下并聯(lián)機(jī)器人動(dòng)力學(xué)建模與分析[J].哈爾濱工程大學(xué)學(xué)報(bào),2012,(01):100-105.