基于DirectX的六維控制器虛擬仿真

張彥斐，陳 超，宮金良

(山東理工大學(xué) 機(jī)械工程學(xué)院, 山東 淄博 255049)

并聯(lián)機(jī)構(gòu)具有剛度大、精度高等諸多優(yōu)點(diǎn)，目前已經(jīng)逐漸應(yīng)用到各個(gè)領(lǐng)域，包括并聯(lián)機(jī)床、微操作機(jī)器人、誤差補(bǔ)償器、多維控制器等。并聯(lián)機(jī)構(gòu)的運(yùn)動(dòng)學(xué)建模是控制系統(tǒng)設(shè)計(jì)的前提條件，相應(yīng)建模方法較多，很多學(xué)者對(duì)此進(jìn)行了深入研究[1-3]。陳宇航等[4]基于旋量運(yùn)算得到子閉環(huán)中任意兩構(gòu)件的相對(duì)速度和加速度，解決了含有子閉環(huán)的并聯(lián)機(jī)構(gòu)動(dòng)學(xué)建模問(wèn)題。耿明超等[5]提出一種擬Newton法，提高了并聯(lián)機(jī)構(gòu)運(yùn)動(dòng)學(xué)正解的計(jì)算效率，并有效提高了實(shí)時(shí)性。孫堅(jiān)等[6]借助Matlab中SimMechanics模塊的系統(tǒng)動(dòng)態(tài)建模功能，對(duì)Stewart平臺(tái)進(jìn)行了運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)分析，避免了復(fù)雜的建模過(guò)程。另外，基于ADAMS軟件可以進(jìn)行虛擬樣機(jī)的試驗(yàn)和測(cè)試，也能夠有效簡(jiǎn)化并聯(lián)機(jī)構(gòu)的分析方法[7-8]。如張靜等[9]應(yīng)用Inventor軟件建立幾何模型，以Parasolid格式導(dǎo)入ADAMS環(huán)境，對(duì)3-UPS/UP并聯(lián)機(jī)構(gòu)進(jìn)行了運(yùn)動(dòng)學(xué)仿真;賈小剛等[10]結(jié)合ADAMS與Pro/E軟件對(duì)并聯(lián)機(jī)構(gòu)的虛擬樣機(jī)進(jìn)行了運(yùn)動(dòng)學(xué)分析。

本文提出一種基于宏動(dòng)3-URS并聯(lián)機(jī)構(gòu)的六維控制器，建立相應(yīng)的運(yùn)動(dòng)學(xué)模型，并利用DirectX技術(shù)和四元數(shù)表示的坐標(biāo)系旋轉(zhuǎn)變換，進(jìn)行六維控制器的虛擬仿真試驗(yàn)。

1 3-URS并聯(lián)機(jī)構(gòu)

圖1所示為一種基于宏動(dòng)3-URS并聯(lián)機(jī)構(gòu)，利用光電旋轉(zhuǎn)編碼器測(cè)量角位移的六維控制器。機(jī)構(gòu)利用3條完全相同且呈中心對(duì)稱(chēng)分布的支鏈，將靜平臺(tái)和動(dòng)平臺(tái)連接在一起。6套驅(qū)動(dòng)電機(jī)分成3組安裝在靜平臺(tái)上，分別為3個(gè)萬(wàn)向鉸呈正交布置的兩軸提供動(dòng)力輸入。六維控制器工作時(shí)，在力和力矩的作用下，動(dòng)平臺(tái)的位姿將會(huì)發(fā)生改變，各關(guān)節(jié)量也會(huì)發(fā)生相應(yīng)的變化。光電旋轉(zhuǎn)編碼器輸入改變的各關(guān)節(jié)變量，輸出經(jīng)過(guò)濾波、電平轉(zhuǎn)換等調(diào)理之后的差分電壓模擬量信號(hào)，再通過(guò)數(shù)據(jù)采集卡將其倍頻處理后轉(zhuǎn)換成供計(jì)算機(jī)分析處理的數(shù)字量信號(hào)。計(jì)算機(jī)把采集的各關(guān)節(jié)變量，利用運(yùn)動(dòng)學(xué)模型及控制算法進(jìn)行解算，控制虛擬機(jī)器人實(shí)現(xiàn)關(guān)聯(lián)運(yùn)動(dòng)和模擬動(dòng)態(tài)仿真。

1.動(dòng)平臺(tái)；2.上連桿；3.光電編碼器；4.下連桿；5.萬(wàn)向鉸；6.靜平臺(tái)；7.力矩電機(jī)；8.安裝基座；9.轉(zhuǎn)動(dòng)副；10.球鉸圖1 3-URS并聯(lián)機(jī)構(gòu)三維模型Fig.1 Three-dimensional model of 3-URS parallel mechanism

2 基于D-H方法的運(yùn)動(dòng)學(xué)建模

為便于建立3-URS并聯(lián)機(jī)構(gòu)的數(shù)學(xué)模型，將圖1所示的三維模型進(jìn)行簡(jiǎn)化處理，得到圖2所示的3-URS并聯(lián)機(jī)構(gòu)原理簡(jiǎn)圖，其中Ai(i=1,2,3)為萬(wàn)向鉸兩垂直交錯(cuò)軸的交點(diǎn)，Bi為每條支鏈末端球鉸的中心點(diǎn)。在靜平臺(tái)中心固結(jié)參考坐標(biāo)系{O}，Y軸正方向通過(guò)A1，Z軸垂直向上，X軸正方向由右手螺旋法則確定。在動(dòng)平臺(tái)中心固結(jié)動(dòng)坐標(biāo)系{O1}，初始位置各坐標(biāo)軸的方向與參考坐標(biāo)系一致。αi為繞X0軸的轉(zhuǎn)角，βi為繞Z0軸的轉(zhuǎn)角，γi為PiAi與PiBi之間的夾角，δ為PiAi與Z1軸之間的夾角，a為靜平臺(tái)的邊長(zhǎng)，b為動(dòng)平臺(tái)的邊長(zhǎng)。

圖2 3-URS并聯(lián)機(jī)構(gòu)原理簡(jiǎn)圖Fig.2 Schematic diagram of 3-URS parallel mechanism

2.1 動(dòng)平臺(tái)運(yùn)動(dòng)學(xué)正解

采用D-H矩陣變換法對(duì)并聯(lián)機(jī)構(gòu)進(jìn)行運(yùn)動(dòng)學(xué)建模。表1中給出了各連桿的D-H參數(shù)。

表1 D-H參數(shù)表
Tab.1 D-H Parameters

序號(hào)θdaα1π2+2i-1()3π0a3-π22-π2+βi()00π23-arctan(tanαicosβi)-l1cosδl1sinδπ24π2-γi-δ()0l20

相鄰連桿之間的位姿關(guān)系可用坐標(biāo)旋轉(zhuǎn)和平移的方法建立聯(lián)系，變換矩陣Ai為

式中，Sθi=sinθi，Cθi=cosθi。Bi點(diǎn)相對(duì)于參考坐標(biāo)系的變換矩陣為

Ti=A1iA2iA3iA4i

(1)

在矩陣Ti中用位置矢量Bi=[pxipyipzi]T表示球鉸中心Bi的位置，用旋轉(zhuǎn)矩陣R=[noa]表示末端連桿的空間姿態(tài)。將表1中各參數(shù)帶入式(1)，求得Bi的坐標(biāo)

(2)

式中：

為了便于在編程中實(shí)現(xiàn)六維控制器對(duì)虛擬仿真的控制，采用Z-Y-X型動(dòng)態(tài)歐拉角來(lái)描述上平臺(tái)相對(duì)于參考坐標(biāo)系的姿態(tài)。其中旋轉(zhuǎn)矩陣R=[noa]，可通過(guò)3條支鏈末端球鉸中心Bi的空間坐標(biāo)值進(jìn)行求解。各角的反正切表達(dá)式為

(3)

式中，

上平臺(tái)中心O1點(diǎn)的坐標(biāo)為

(4)

式(3)、式(4)即為并聯(lián)機(jī)構(gòu)位置正解，等號(hào)右邊是用結(jié)構(gòu)參數(shù)和關(guān)節(jié)變量表示的解析表達(dá)式來(lái)確定動(dòng)平臺(tái)的位姿。當(dāng)給定各關(guān)節(jié)變量的變化范圍時(shí)，便可求得3-URS并聯(lián)機(jī)構(gòu)的工作空間。

2.2 上連桿運(yùn)動(dòng)學(xué)正解

式(2)求得Bi的坐標(biāo)即為上連桿的位置坐標(biāo)。上連桿的姿態(tài)采用Z-Y-X型動(dòng)態(tài)歐拉角進(jìn)行描述。在齊次變換矩陣Ti中的旋轉(zhuǎn)矩陣R=[noa]，其各單位方向矢量在參考坐標(biāo)系上的分量為動(dòng)坐標(biāo)系各坐標(biāo)軸的方向余弦。向量n、o、a的表達(dá)式分別如下：

式中：

L1=sinθitanαicosβi-cosθisinβi

L2=cosθitanαicosβi+sinθisinβi

R1=cosθicosβi

R2=sinθicosβi

Q=sinβicosβitanαi

上連桿Z-Y-X型動(dòng)態(tài)歐拉角的反正切表達(dá)式為

(5)

式(2)、式(5)即為上連桿的空間姿態(tài)表達(dá)式。其空間位姿的正確性是3-URS并機(jī)構(gòu)能否實(shí)現(xiàn)在虛擬環(huán)境中關(guān)聯(lián)運(yùn)動(dòng)的關(guān)鍵。

3 MATLAB仿真實(shí)例

(a)位形1 (b)位形2 (c)位形3 (d)位形4圖3 不同位形參數(shù)下的機(jī)構(gòu)位形簡(jiǎn)圖Fig.3 Pose examples with different input variables

表2 機(jī)構(gòu)正解
Tab.2 Forward kinematic of the mechanism

[α1,α2,α3,β1,β2,β3,γ1,γ2,γ3][x,y,z,α,β,γ][0,0,0,-2.5,-2.5,-2.5,50,50,50][0,0,144.631,0,0,0][3.1,0.3,-23.2,-15.8,11.7,-3.2,57.8,71.3,64.6][-52.498,3.612,183.769,0.478,0.026,-0.110][-6.3,-9.9,20.1,10.4,-19.8,-4.3,67.7,49.8,59.2][53.945,0.531,168.535,-0.107,0.154,-0.024][18.3,-18.1,1.5,7.5,6.5,-27.5,70.7,70.2,52.1][1.897,73.049,181.047,-0.031,-0.010,-0.105]

采用數(shù)值驗(yàn)證的方法來(lái)驗(yàn)證運(yùn)動(dòng)學(xué)正解的可靠性。根據(jù)運(yùn)動(dòng)學(xué)正解，求得動(dòng)平臺(tái)的位姿參數(shù)，然后將其與表3三維建模軟件的標(biāo)定值進(jìn)行比較，結(jié)果表明誤差均≤5×10-3。由此驗(yàn)證了3-URS并聯(lián)機(jī)構(gòu)運(yùn)動(dòng)學(xué)正解的正確性以及結(jié)構(gòu)設(shè)計(jì)的合理性和可行性。

表3 機(jī)構(gòu)位姿標(biāo)定及誤差
Tab.3 Error and pose calibration

[x,y,z,α,β,γ]δ[0,0,144.631,0,0,0]≤5×10-3[-52.497,3.610,183.773,0.478,0.025,-0.106]≤5×10-3[53.945,0.534,168.530,-0.107,0.152,-0.025]≤5×10-3[1.894,73.052,181.050,-0.031,-0.012,-0.107]≤5×10-3

4 虛擬仿真系統(tǒng)

六維控制器的末端將外部受力與力矩直接和輸出的位移增量和轉(zhuǎn)動(dòng)增量建立關(guān)系，然后再作用于各關(guān)聯(lián)構(gòu)件反推出其位姿。數(shù)據(jù)采集卡即時(shí)采集光電旋轉(zhuǎn)編碼器監(jiān)測(cè)的各關(guān)節(jié)變量，在VS中利用DirectX技術(shù)把采集到的各關(guān)節(jié)變量，依據(jù)運(yùn)動(dòng)學(xué)正解模型進(jìn)行處理，計(jì)算出各關(guān)聯(lián)構(gòu)件的空間位姿，實(shí)現(xiàn)3-URS并聯(lián)機(jī)構(gòu)在虛擬三維場(chǎng)景中關(guān)聯(lián)運(yùn)動(dòng)并進(jìn)行模擬仿真分析，控制系統(tǒng)流程如圖4所示。

圖4 控制系統(tǒng)流程圖Fig.4 Flow chart of the control system

以上連桿為例進(jìn)行介紹。根據(jù)各關(guān)節(jié)編碼器所采集的關(guān)節(jié)變量，依據(jù)式(2)求得球鉸中心Bi的坐標(biāo)，依據(jù)式(5)求得上連桿的動(dòng)態(tài)歐拉角。由于在DirectX中旋轉(zhuǎn)變換是以世界坐標(biāo)系進(jìn)行變換，而求得的上連桿歐拉角是以其動(dòng)坐標(biāo)系進(jìn)行變換。所以為了解決利用DirectX技術(shù)重繪上連桿的空間位姿問(wèn)題，需要求得上連桿動(dòng)坐標(biāo)系在各次旋轉(zhuǎn)變換中的空間位姿。這樣就把繞上連桿動(dòng)坐標(biāo)系的旋轉(zhuǎn)變換轉(zhuǎn)換為繞世界坐標(biāo)系中的特定軸的旋轉(zhuǎn)變換。

點(diǎn)在左手直角坐標(biāo)系中繞過(guò)原點(diǎn)任意軸r=(rx，ry，rz)旋轉(zhuǎn)θ角的旋轉(zhuǎn)變換矩陣為

Rot(r,θ)=

式中，

初始情況下，上連桿的動(dòng)坐標(biāo)系與世界坐標(biāo)系完全重合。在世界坐標(biāo)系的原點(diǎn)位置，依據(jù)上式旋轉(zhuǎn)上連桿動(dòng)坐標(biāo)系的3個(gè)坐標(biāo)軸，變換到所需的姿態(tài)后，平移到Bi點(diǎn)，再進(jìn)行渲染操作。主要步驟如下：

(1)繞動(dòng)坐標(biāo)系的Z軸旋轉(zhuǎn)α角。此時(shí)動(dòng)坐標(biāo)系的X軸和Y軸在世界坐標(biāo)系下的矢量分別為[cosα -sinα 0]T、[sinα cosα 0]T。

(2)繞動(dòng)坐標(biāo)系的Y軸旋轉(zhuǎn)β角。局部坐標(biāo)系的X軸在世界坐標(biāo)系下的矢量為[cosαcosβ-sinαcosβsinβ]T。

(3)繞動(dòng)坐標(biāo)系的X軸旋轉(zhuǎn)γ角。此時(shí)得到的坐標(biāo)系即所需的姿態(tài)坐標(biāo)系。

在編程實(shí)現(xiàn)上連桿的姿態(tài)變化時(shí)，若中間旋轉(zhuǎn)變換的過(guò)程采用四元數(shù)Q(x,y,z,w)=[sin(w/2)x, sin(w/2)y, sin(w/2)z, cos(w/2)](其中[x，y，z]為旋轉(zhuǎn)軸，w為旋轉(zhuǎn)角度)來(lái)表示坐標(biāo)系的旋轉(zhuǎn)變換，則可以有效避免物體在旋轉(zhuǎn)變換的過(guò)程中出現(xiàn)歐拉角萬(wàn)向節(jié)死鎖、矩陣數(shù)據(jù)處理量大、物體旋轉(zhuǎn)變換出現(xiàn)不連續(xù)的問(wèn)題。因?yàn)樗脑獢?shù)具有不論剛體處于什么空間位姿都不會(huì)退化的優(yōu)點(diǎn)，并且參數(shù)的范圍不受限制，方程組線性化程度高，無(wú)奇異，只涉及代數(shù)運(yùn)算，所以避免了大量三角函數(shù)運(yùn)算。由四元數(shù)到旋轉(zhuǎn)矩陣的變換公式為

Rot(r,θ)=

式中，

四元數(shù)將三維空間的旋轉(zhuǎn)概念擴(kuò)展到四維空間，這為表示和處理三維空間中點(diǎn)的旋轉(zhuǎn)變換帶來(lái)了極大方便。在程序中若廣泛用四元數(shù)替換矩陣變換，不僅能節(jié)省內(nèi)存資源，提高數(shù)據(jù)運(yùn)算速度，同時(shí)也能降低計(jì)算處理成本。若需姿態(tài)變換矩陣，可依據(jù)上式把四元數(shù)轉(zhuǎn)換成姿態(tài)變換矩陣。圖5為3-URS并聯(lián)機(jī)構(gòu)虛擬仿真實(shí)例效果。

(a)效果1 (b)效果2

(c)效果3 (d)效果4圖5 虛擬仿真系統(tǒng)運(yùn)行效果圖Fig.5 Results of the virtual simulation system

5 結(jié)論

1) 提出了一種基于宏動(dòng)3-URS并聯(lián)機(jī)構(gòu)，利用光電旋轉(zhuǎn)增量式編碼器測(cè)量角位移的六維控制器設(shè)計(jì)方案。此并聯(lián)機(jī)構(gòu)利用3條完全相同且呈中心對(duì)稱(chēng)分布的URS支鏈，將靜平臺(tái)和動(dòng)平臺(tái)連接在一起，充分集成了并聯(lián)機(jī)構(gòu)、光電旋轉(zhuǎn)增量式編碼器、萬(wàn)向鉸等的優(yōu)點(diǎn)，為六維控制器的結(jié)構(gòu)設(shè)計(jì)提供了一種新思路。

2) 已知六維控制器的各關(guān)節(jié)變量，對(duì)并聯(lián)機(jī)構(gòu)的正向運(yùn)動(dòng)學(xué)進(jìn)行了封閉式完全耦合分析并利用MATLAB對(duì)數(shù)學(xué)模型進(jìn)行了有效驗(yàn)證，從而為實(shí)時(shí)測(cè)量動(dòng)平臺(tái)的位姿提供了有力保證，同時(shí)也為虛擬環(huán)境中并聯(lián)機(jī)器人的運(yùn)動(dòng)仿真分析提供了必要的理論依據(jù)。

3) 引入四元數(shù)表示坐標(biāo)系旋轉(zhuǎn)變換并進(jìn)行正交規(guī)范化處理，基于DirectX技術(shù)構(gòu)建了虛擬仿真系統(tǒng)，實(shí)現(xiàn)了六維控制器對(duì)虛擬機(jī)器人的實(shí)時(shí)控制，為機(jī)器人運(yùn)動(dòng)學(xué)驗(yàn)證提供了實(shí)時(shí)檢測(cè)平臺(tái)。