鐘有博-基于simulink的6-DoF平臺的反解仿真-20140403

1、基于Simulink/SimMechanics的6-DoF平臺的反解建模與仿真1 引言運動平臺按結(jié)構(gòu)可以分為串聯(lián)和并聯(lián)兩大類。與串聯(lián)形式相比，并聯(lián)形式具有剛度大、承載能力強、結(jié)構(gòu)簡單、運動負(fù)荷小、能實現(xiàn)包括橫移、縱移、深沉等多個自由度運動的特點。同時，與串聯(lián)形式的優(yōu)點也很明顯，其具有運動空間大，測量精度高，運動、受力分析相對簡單，控制、測量的實現(xiàn)相對容易，且每個自由度都能獨立運動等特點。六自由度運動平臺是由六條液壓氣缸通過萬向節(jié)鉸連（或球鉸鏈）將上、下兩個平臺連接而成，下平臺固定在基礎(chǔ)上，借助六條液壓氣缸的伸縮運動，完成上平臺在三維空間六個自由度（， X，Y，Z）的運動，從而可以模擬出各種空間

2、運動姿態(tài)?？臻g運動的目標(biāo)是實現(xiàn)平臺在空間運動的三個姿態(tài)角度和三個平動位移，即俯仰、滾轉(zhuǎn)、偏航、上下垂直運動、前后平移和左右平移，及六個姿態(tài)的復(fù)合運動姿態(tài)。Matlab中的Simulink是一個用來對動態(tài)系統(tǒng)進(jìn)行建模、仿真和分析的軟件包。SimMechanics是Matlab6.5新增的機構(gòu)系統(tǒng)模塊集，它可以對各種運動副連接的剛體進(jìn)行建模與仿真，實現(xiàn)對機構(gòu)系統(tǒng)進(jìn)行分析與設(shè)計的目的。它提供了一個可以在Simulink環(huán)境下直接使用的模塊集，可以將表示各種機構(gòu)的模塊在普通的Simulink窗口中繪制出來，并通過它自己提供的檢測與驅(qū)動模塊和普通的Simulink模塊連接起來，獲得整個系統(tǒng)的仿真結(jié)果。S

3、imMechanics動態(tài)仿真工具具有系統(tǒng)建模直觀方便、仿真功能強大，自動模型分析等強大優(yōu)勢，可以很好地對機械系統(tǒng)的各種運動進(jìn)行分析，從而為機械系統(tǒng)提供一個強大而方便的工具。通過該模型可以有效的對6-DoF運動平進(jìn)行位置反解分析。本文運用并聯(lián)機器人SimMechanics模型對6-DoF運動平臺進(jìn)行位置反解的方法，并設(shè)計PID控制器對平臺進(jìn)行仿真。2 基于Simulink的平臺的反解建模2.1 坐標(biāo)系建立及坐標(biāo)點的確定為了清楚地描述平臺的運動，選取兩個坐標(biāo)系，即動坐標(biāo)系OXYZ和靜坐標(biāo)系OXYZ。其中靜坐標(biāo)系固定在下平臺中，動坐標(biāo)系固定在上平臺中，初始位置時，上下平臺相互平行，且上平臺相對下平

4、臺偏移/3。靜坐標(biāo)系以下平臺六個鉸點所在圓的圓心為原點，以下平臺鉸點B1、B2連線的中垂線所在直線為X軸，Z軸垂直于平臺向上，如圖所示：OxZOXZYYTop platformBase platformB1B2B3B4B5B6A1A2A3A4A5A6圖2.1 靜坐標(biāo)系與動坐標(biāo)系位置示意圖XYXYabB1B2B3B4B5B6/3O(O)A1A2A3A4A5A6圖2.2 上、下平臺各鉸支點的位置分布示意圖用矩陣B來表示液壓氣缸下端各鉸支點Bi(i=1，2，3，4，5，6)在靜坐標(biāo)系中的坐標(biāo)向量。矩陣B的第一列的第一行至第三行元素分別表示B1點在動坐標(biāo)系中的X軸、Y軸、Z軸的坐標(biāo)量，其余列的意義與第

5、一列意義類似。則可得出：B=bij3X6=Rbcos-b Rbcosb Rbcos23-b Rbcos23+b Rbcos43-b Rbcos43+bRbsin-b Rbsinb Rbsin23-b Rbsin23+b Rbsin43-b Rbsin43+b0 0 0 0 0 0(2-1)將矩陣B寫成齊次坐標(biāo)的形式：B=bij4X6=bij4X611X64X6(2-2)式中：Rb - 液壓氣缸下端各鉸點分布圓的半徑；b - B1點相對OX軸的偏移角度，即下鉸點分布角；用矩陣A來表示液壓氣缸下端各鉸支點Ai(i=1，2，3，4，5，6)在動坐標(biāo)系中的坐標(biāo)向量。矩陣A的第一列的第一行至第三行元素分

6、別表示A1點在動坐標(biāo)系中的OX軸、OY軸、OZ軸的坐標(biāo)量，其余列的意義與第一列意義類似。則可得出：A=aij3X6=Rtcos53+a Rtcos3-a Rtcos3+a Rtcos-a Rtcos+a Rtcos53-aRtsin53+a Rtsin3-a Rtsin3+a Rtsin-a Rtsin+a Rtsin53-a0 0 0 0 0 0(2-3)將矩陣A寫成齊次坐標(biāo)的形式：A=aij4X6=aij4X611X64X6 (2-4)式中：Rt - 液壓氣缸下端各鉸支點分布圓的半徑；a - A1點相對OX軸的偏移角度，即上鉸點分布角；2.2坐標(biāo)變換及旋轉(zhuǎn)矩陣對于在三維空間里的一個參考系，

7、任何坐標(biāo)系的向，都可以用三個歐拉角來表現(xiàn)。動坐標(biāo)系固定在剛體中，隨著剛體的旋轉(zhuǎn)而旋轉(zhuǎn)。動坐標(biāo)系相對靜坐標(biāo)系的位置可以用廣義坐標(biāo)來表示，Q的分量為qi。其中q1、q2、q3為動坐標(biāo)系與靜坐標(biāo)系的三個姿態(tài)角，q4、q5、q6為動坐標(biāo)系原點O在靜坐標(biāo)系OX、OY、OZ三軸上的坐標(biāo)。動坐標(biāo)系與靜坐標(biāo)系之間，存在一個齊次變換矩陣。由靜坐標(biāo)系到動坐標(biāo)系的變換順序為：第一次沿OX軸方向平移q4，變換矩陣為：T1= 1 0 0 q4 0 1 0 0 0 0 1 0 0 0 0 1 (2-5)第二次沿 OY 向平移 q5，變換矩陣為:T2= 1 0 0 0 0 1 0 q50 0 1 0 0 0 0 1 (2-

8、6)第三次沿 OZ向平移q6，變換矩陣為:T3= 1 0 0 0 0 1 0 0 0 0 1 q6 0 0 0 1 (2-7)三次平移之后，接著進(jìn)行三次旋轉(zhuǎn)變換。第一次繞OZ旋轉(zhuǎn)偏航角q3，變換矩陣為T4= cosq3 -sinq3 0 0 sinq3 cosq3 0 0 0 0 1 0 0 0 0 1 (2-8)第二次繞OY旋轉(zhuǎn)縱搖角q2，變換矩陣為T5= cosq2 0 sinq2 0 0 1 0 0 -sinq2 0 cosq2 0 0 0 0 1(2-9)第三次繞OX旋轉(zhuǎn)橫搖角q1，變換矩陣為T6= 1 0 0 q40 cosq1 -sinq1 0 0 sinq1 cosq1 0 0

9、0 0 1 (2-10)經(jīng)過以上各個變換之后，則可以得到由靜坐標(biāo)系到動坐標(biāo)系的坐標(biāo)變換矩陣T為T=T1T2T3T4T5T6=cosq2cosq3 -cosq1sinq3+sinq1sinq2cosq3 sinq1sinq3+cosq1sinq2cosq3 q4 cosq2sinq3 cosq1cosq3+sinq1sinq2sinq3 -sinq1cosq3+cosq1sinq2sinq3 q5 -sinq2 sinq1cosq2 cosq1cosq2 q6 0 0 0 1 (2-11)2.3平臺的位置反解當(dāng)已知輸出件的位置和姿態(tài)而求解機構(gòu)輸入件的位置時，稱為機構(gòu)位置的反解。并聯(lián)機構(gòu)的運動分析

10、可以確定某些構(gòu)件運動所需的空間，校檢它們的運動是否干涉。平臺的位置反解，即當(dāng)給定上平臺在空間中的位置和姿態(tài)時求解各個液壓氣缸活塞桿的伸縮量（即位移量）。液壓汽缸活塞桿的伸縮量可以由液壓氣缸的上、下鉸支點之間的距離減去上、下鉸支點間的初始距離來確定。上、下鉸支點間的距離公式為li=k=13gki-bki2 i=1，2，3，4，5，6(2-12)式中：gki為液壓氣缸上端鉸支點在動坐標(biāo)系下的坐標(biāo)變換到靜坐標(biāo)系下所得各點相對應(yīng)的坐標(biāo)，其計算公式為G=gij4x6=TA(2-13)bki為液壓氣缸下鉸支點在靜坐標(biāo)系中各點相對應(yīng)的坐標(biāo)。液壓氣缸活塞桿伸縮量為l=li-l0 (i=1，2，3，4，5，6)

11、(2-14)2.4反解模型的建立Simulink是Matlab軟件的擴展，它是實現(xiàn)動態(tài)系統(tǒng)建模和仿真的一個軟件包，它與Matlab語言的主要區(qū)別在于，其與用戶的交互接口是基于Windows的模型化圖形輸入，其結(jié)果是使得用戶可以把更多的精力投入到系統(tǒng)模型構(gòu)建，而非語言編程上。為了創(chuàng)建動態(tài)系統(tǒng)模型，Simulink提供了一個建立模型方塊圖的圖形用戶接口(GUI)，這個創(chuàng)建過程只需要單擊和拖動鼠標(biāo)操作就能完成，它提供了一種更快捷、直接明了的方式，而用戶可以立即看到系統(tǒng)的仿真效果，以下是利用Simulink建立運動平臺反解模型的建模過程。轉(zhuǎn)換矩陣T模型，用MATLAB Function模塊實現(xiàn)矩陣運算

12、并將結(jié)果輸出，在MATLAB Function模塊中輸入T矩陣，輸入端是給定的參考姿態(tài)Q=(q1, q2, q3, q4, q5, q6)，如圖所示圖2.3矩陣T模型矩陣B模塊，使用MATLAB Function模塊，在模塊中輸入B矩陣公式。輸入端是上平臺的鉸支點所在圓的半徑Rb，和鉸支點的分布角b。如圖所示圖2.4矩陣B模型矩陣A模塊，使用MATLAB Function模塊，在模塊中輸入A矩陣公式。輸入端是上平臺的鉸支點所在圓的半徑Rt，和鉸支點的分布角a。如圖所示圖2.5 矩陣A模型根據(jù)計反解算公式（2-13）、（2-14）、（2-15）建立反解模型，l0為上、下平臺各鉸支點間的初始長度，

13、輸出為各個液壓氣缸的伸縮量（即移位量）li，如圖所示:圖2.6 反解過程模型其中Mod子系統(tǒng)模塊用來求上、下平臺各鉸支點間的距離，輸出為各液壓氣缸伸縮后的長度，即各個桿的長度，其模型如圖所示圖2.7 Mod子系統(tǒng)模型參考運動姿態(tài)模塊，主要由Top Plat Reference子系統(tǒng)和Mux模塊組成，如圖所示圖2.8 參考姿態(tài)模型其中Top Plat Reference子系統(tǒng)模型，可以根據(jù)平臺所要求的運動來規(guī)定，在動坐標(biāo)的初始位置時，動坐標(biāo)系原點在靜坐標(biāo)系下為q4q5q6=000+H，所以在q6中加上數(shù)值為H的常數(shù)模塊， 如圖所示圖2.9 Top Plat Reference子系統(tǒng)模型反解過程的

14、總體模型如圖2.10所示，圖2.10(b)是將反解算法過程封裝成子系統(tǒng)后的總體模型。圖2.10(a) 反解總體模型圖2.10(b) 封裝后的反解總體模型3 用SimMechanics模塊集構(gòu)建平臺SimMechanics機構(gòu)系統(tǒng)模塊集可以對各種運動副連接的剛體進(jìn)行建模與仿真，能實現(xiàn)對機構(gòu)系統(tǒng)進(jìn)行分析與設(shè)計的目的。它包括下面幾個模塊組：剛體模塊組、約束與驅(qū)動模塊組、輔助工具模塊組、運動副模塊組、檢測與驅(qū)動模塊組。驅(qū)動模塊用來添加Simulink輸入量，有3種驅(qū)動方式，即構(gòu)件驅(qū)動、鉸連接副驅(qū)動和驅(qū)動執(zhí)行器驅(qū)動。使用這些模塊可以方便的建立復(fù)雜的機械系統(tǒng)的圖形化模型，進(jìn)行機械系統(tǒng)的單獨分析或與任何Si

15、mulink設(shè)計的控制器及其它動態(tài)系統(tǒng)相連接進(jìn)行綜合仿真。本文首先根據(jù)6-DoF并聯(lián)結(jié)構(gòu)平臺的連接關(guān)系建立仿真模型，然后進(jìn)行調(diào)試及仿真。6-DoF并聯(lián)機構(gòu)運動平臺的SimMechanics的總體模型框圖如圖3.1所示，主要由上平臺、下平臺及6條驅(qū)動腿子系統(tǒng)組成，6條驅(qū)動腿子系統(tǒng)通過胡克鉸與上、下平臺相連接。下平臺固定不動，用ground模塊來表示，其參數(shù)設(shè)置比較簡單，只需填寫各鉸點在世界坐標(biāo)系中坐標(biāo)即可；上平臺是一個剛體，用Body模塊來表示，Body模塊的參數(shù)設(shè)置主要分兩部分：上部分設(shè)置剛體的質(zhì)量與慣量，下部分設(shè)置剛體的坐標(biāo)系。對上平臺來說，由于它與6條驅(qū)動腿相連接，所以除了中心點（質(zhì)心CG

16、），還需設(shè)置6條驅(qū)動腿與上平臺連接的鉸點處的坐標(biāo)系。上平臺安裝有傳感器用于檢測上平臺運動。圖3.1 6-DoF運動平臺的SimMechanics的模型驅(qū)動腿子系統(tǒng)結(jié)構(gòu)框圖如圖3.2所示，它由驅(qū)動腿的上部分剛體、下部分剛體以及它們與上下平臺相連接的上、下鉸點，中間用一移動副連接，該移動副連接有驅(qū)動器和傳感器。當(dāng)上平臺運動時，活塞在液壓氣缸中的移動導(dǎo)致驅(qū)動腿長度的變化，所以把驅(qū)動腿的上、下部分分別作為一個剛體用Body模塊來表示。對于連接部位，上平臺相對于驅(qū)動腿，驅(qū)動腿的上部分相對于下部分，下部分相對于下平臺都有相對運動，所以它們之間應(yīng)該分別用一個接頭連接。本模型用兩個2自由度的胡克鉸與上、下平臺

17、相連接；液壓氣缸中活塞的驅(qū)動導(dǎo)致驅(qū)動腿的伸縮，所以驅(qū)動腿上下兩部分連接處（即液壓氣缸處）有一個沿腿方向的平移自由度，加上胡克鉸處移動過來的一個旋轉(zhuǎn)自由度，可用一個Cylindrical（代表一個平移自由度和一個旋轉(zhuǎn)自由度）接頭來表示。這樣就實現(xiàn)了六個自由度的運動。根據(jù)建模所規(guī)定坐標(biāo)系及相應(yīng)需求正確定義兩個剛體的質(zhì)量、轉(zhuǎn)動慣量矩陣和幾何參數(shù)，正確定義三個鉸鏈連接點坐標(biāo)、鉸鏈轉(zhuǎn)動軸的方向矢量，便可得到運動平臺的六條驅(qū)動腿模型。圖3.2 驅(qū)動腿子系統(tǒng)SimMechanics模型通過驅(qū)動器（actuator）將運動施加到主動副上，進(jìn)行運動平臺機構(gòu)的正向運動分析；將運動施加到上平臺，可以進(jìn)行運動平臺機構(gòu)的反向運動分析。利用傳感器（sensor）可以測量機構(gòu)正向分析中上平臺的位移和機構(gòu)反向分析中主運動副的角位移、角速度和角加速度。所有的輸出都通過工作空間輸出為數(shù)組存盤，供后期處理。輸出也可以通過Scope實時顯示。4 仿真驗證系統(tǒng)的總體結(jié)構(gòu)運動平臺的仿真系統(tǒng)結(jié)構(gòu)框圖如圖4.1所示，該模型框圖共由6部分組成。反解過程模塊根據(jù)上平臺的軌跡來得到6條驅(qū)動腿的期望位移（伸縮量）；微分模塊用來將