（精選）基于matlab-simulink的六自由度運動平臺的位置反解實現(xiàn)與仿真研究(初稿1)--鐘有博

1、六自由度運動平臺位置逆解實現(xiàn)與仿真研究陳云軍 1, 鐘有博2CHEN Yun jun1, ZHONG You bo21.天津工業(yè)大學 電氣與自動化工程學院,天津 300387 2.天津工業(yè)大學 電子與信息工程學院,天津 3003871. School of Electrical Engineering and Automation, Tianjin Polytechnic University, Tianjin 300387, China2. School of Electronics and Information Engineering, Tianjin Polytechnic Unive

2、rsity, Tianjin 300387, ChinaE-mail:zhongyouboCHEN Yun jun, ZHONG You bo. Simulation research and realization of position inverse solution of 6-Dof motion platform Abstract：Firstly, use D-H method to analyze the kinematics of the 6-Dof motion platform, and establish its position inverse solution mode

3、l base on Matlab/Simulink simulation integrated environment. Then, build up the physics model of the 6-Dof motion platform by the SimMechancis tool sets under Simulink, and establish the simulation and verification system in Simulink. Finally, input the desired reference trajectory to simulate and v

4、erify, and obtain the experimental results that the error is small and results match. Therefore, verify the correctness of the position inverse solution model.Key words：6-Dof parallel platform; D-H method; position inverse solution;Matlab/Simulink摘 要：首先，應(yīng)用D-H法對六自由度運動平臺的運動學進行分析，并在Matlab/Simulink仿真集成環(huán)

5、境中建立其位置逆解模型；然后利用Simulink下的SimMechanics工具集搭建好六自由度運動平臺物理模型，并在Simulink下搭建起仿真驗證系統(tǒng)；最后輸入期望參考運動軌跡，加以仿真驗證，誤差較小，結(jié)果吻合，從而驗證位置逆解模型的正確性。 關(guān)鍵詞： 六自由度運動平臺; D-H法; 位置逆解; Matlab/Simulink 文獻標識碼: A 中圖分類號:TN913 1. 引言近年來，由于六自由度運動平臺具有極為廣泛的應(yīng)用前景，引起了國內(nèi)外科研機構(gòu)、研究院校廣泛的研究。六自由度運動平臺，又稱Stewart并聯(lián)平臺，stewart平臺的研究始于1965年，德國工程師stewart提出六自由

6、度的并聯(lián)機構(gòu)用來作為飛行模擬器1。目前經(jīng)典stewart運動平臺的機構(gòu)主要由上下兩個平臺和六個可以伸縮的桿件及它們和上下平臺相連接的鉸鏈構(gòu)成。其中，上平臺為負載平臺，可以移動；下平臺通常為底座，固定在地面。Stewart并聯(lián)平臺可以通過六條支桿的伸縮運動使得負載平臺可在工作范圍內(nèi)實現(xiàn)空間六個自由度的聯(lián)合運動（即俯仰、側(cè)傾、航偏轉(zhuǎn)動以及前后、左右、上下平動），并具有剛度好，精度高，承載能力強，動態(tài)特性好等優(yōu)點2。六自由度運動平臺的位置逆解的求解和仿真是進行機構(gòu)運動控制和結(jié)構(gòu)優(yōu)化的基礎(chǔ)，它對六自由度運動平臺進行位姿控制時的各種應(yīng)用場合均有直接的實用價值。而Matlab是一種高精度的科學計算工具，它

7、將計算、可視化和編程集成在一個容易使用的環(huán)境中，其典型功能包括數(shù)學計算、建模和仿真、數(shù)據(jù)分析、研究和可視化、創(chuàng)建圖形用戶接口。simulink是matlab中的一個重要部分，是一個用來對動態(tài)系統(tǒng)進行建模、仿真和分析的軟件包。Simulink下的工具集SimMechanics3是matlab6.5新增的機構(gòu)系統(tǒng)模塊集，它可以建立起機構(gòu)的物理模型，并可以通過相應(yīng)模塊與Simulink模塊或子系統(tǒng)相連接，進行聯(lián)合測試仿真，為機構(gòu)的運動學分析與建模實現(xiàn)提供了良好的技術(shù)基礎(chǔ)。本文利用Simulink/Matlab集成開發(fā)環(huán)境實現(xiàn)位置逆解算法模型，并且通過在其環(huán)境下建立的六自由度運動平臺物理模型進行了仿真

8、驗證。2. 位置逆解分析 六自由度運動平臺的運動學分析主要包括位置分析、速度和加速度分析。位置分析是求輸入輸出構(gòu)件之間的位置關(guān)系，它包括位置正解和位置逆解4。本文主要應(yīng)用D-H法對六自由度運動平臺的位置逆解進行分析并建模。D-H法5是由Denavit和Hartenberg共同提出來的一種通用方法，運用D-H法建立運動學模型的大體步驟是：首先按照一定規(guī)律建立坐標系，再建立坐標系間齊次坐標變換矩陣，最后建立機構(gòu)的運動學方程。2.1坐標系建立及坐標點的確定六自由度運動平臺由上、下平臺，六支運動桿件以及其與上下平臺相連接的運動鉸構(gòu)成，借助六條運動桿的伸縮運動，完成上平臺在三維空間六個自由度的運動，從而

9、模擬出各種空間姿態(tài)。為了清楚地描述平臺的運動，選取兩個坐標系，即動坐標P（OXYZ）和靜坐標系下G（OXYZ）。其中靜坐標系固定在下平臺中，靜坐標原點與下平臺的質(zhì)心重合；動坐標系固定在上平臺中，動坐標系原點與上平臺的質(zhì)心重合，如圖1所示。圖1 靜坐標系與動坐標系位置示意圖圖2 上、下平臺各鉸支點的位置分布示意圖如圖2，為上、下平臺各鉸點的位置分布示意圖。其中用矩陣B來表示支桿與下平臺相連接的鉸點Bi（i=1，2，3，4，5，6）在靜坐標系中的坐標向量。矩陣B的第一列的第一行至第三行元素分別表示B1點在動坐標系中的X軸、Y軸、Z軸的坐標量，其余列的意義與第一列意義類似。則可得出： B=Rbcos

10、(-b)Rbcos(b)Rbcos(23-b)Rbsin(-b)Rbsin(b)Rbsin(23-b)-hb-hb-hb Rbcos(23+b)Rbcos(43-b)Rbcos(43+b)Rbsin(23+b)Rbsin(43-b)Rbsin(43+b)-hb-hb-hb (1)式中：Rb為支桿與下平臺相連接的鉸點（下鉸點）分布圓的半徑；b為B1點相對OX軸的偏移角度，即下鉸點分布角；hb為下鉸點離下平臺質(zhì)心的高度。用矩陣A來表示液壓氣缸下端各鉸支點Ai（i=1，2，3，4，5，6）在動坐標系中的坐標向量。矩陣A的第一列的第一行至第三行元素分別表示A1點在動坐標系中的OX軸、OY軸、OZ軸的坐

11、標量，其余列的意義與第一列意義類似。則可得出： A=Rtcos(53+a)Rtcos(3-a)Rtcos(3+a)Rtsin(53+a)Rtsin(3-a)Rtsin(3+a)hahaha Rtcos(-a)Rtcos(+a)Rtcos(53-a)Rtsin(-a)Rtsin(+a)Rtsin(53-a)hahaha (2)式中：Rt為支桿與上平臺相連接的鉸點（上鉸點）分布圓的半徑；a為A1點相對OX軸的偏移角度，即上鉸點分布角；ha為上鉸點離上平臺質(zhì)心的高度。2.2坐標變換及次坐標變換矩陣剛體的位置可以用它在某個坐標系中的向量來描述，而動坐標系固定在上平臺中，其位置矢量為： P=x y zT

12、剛體的方位也稱剛體的姿態(tài)，而描述轉(zhuǎn)動剛體相對參考坐標系的方位都可以用三個角度作為廣義坐標，這三個角稱為歐拉角。它們可以描述剛體相對于參考坐標系的姿態(tài)，由三個歐拉角構(gòu)成的旋轉(zhuǎn)矩陣6為： R=cc-sc+cssss+cscsccc+sss-cs+ssc-scscc (3)式中：c表示余弦cos，s表示正弦sin。動作坐標系相對于經(jīng)坐標系的位置和姿態(tài)可以用廣義坐標Q來表示，Q的分量為qi；其中q1、q2、q3為動坐標系與靜坐標系下的三個姿態(tài)角（，），q4、q5、q6為動坐標系原點O在靜坐標系下OX、OY、OZ三軸上的坐標(x，y，z)，即記Q=（q1，q2，q3 ，q4、q5，q6）=（，x，y，z

13、）。齊次坐標系是用（n+1）維坐標來描述n維空間中的位置，齊次變換矩陣是4×4矩陣，它能把一個以其次坐標表示的位置矢量由一個坐標系映射到另外一個坐標系。若三維空間的位置矢量P表示成齊次坐標系，即P=x，y，z，1T，那么利用變換矩陣的概念，對轉(zhuǎn)動3×3旋轉(zhuǎn)矩陣可擴展成4×4齊次坐標變換矩陣，該齊次變換矩陣為：T=R3x3P3x1O1x31 （4）2.3平臺的位置逆解當已知上平臺的位置和姿態(tài)而求解機構(gòu)輸入桿件的位置時，稱為機構(gòu)的位置逆解。運動平臺的位置逆解，即給定上平臺在空間中的位置和姿態(tài)時求解六條支桿的伸縮量。首先，將上、下鉸點的坐標位置用齊次坐標的形式表示，則支

14、桿與下平臺相連接的鉸點為：Bi=Bi1 (i=1，2，3，4，5，6) （5）下鉸點位置坐標矩陣為：B=B11×6 (i=1，2，3，4，5，6) （6）支桿與下平臺相連接的鉸點為：Ai=Ai1 (i=1，2，3，4，5，6) （7）上鉸點位置坐標矩陣為：A=A11×6 (i=1，2，3，4，5，6) （8）根據(jù)前文所述，可以得出動坐標系與靜坐標系之間的變換關(guān)系為： Li=T*Ai-Bi （9）運動桿的伸縮量可由上、下鉸點間的距離與運動桿初始長度來確定，即： Li=Li-Li0=T*Ai-Bi-Li0 （i=1,2,3,4,5,6） （10）Li表示各運動桿的伸縮量，Li0

15、為各運動桿的初始長度。 3. 運動平臺位置逆解模型的建立3.1位置逆解模型的整體結(jié)構(gòu)根據(jù)上述反解過程分析，在Simulink中建立六自由度運動平臺逆解仿真模型。位置逆解模型機構(gòu)如圖3所示。圖3 位置逆解模型3.2 上平臺參考軌跡模塊上平臺參考軌跡模塊用于上平臺期望位資參數(shù)的輸入，該模塊中輸入的上平臺期望運動參數(shù)是動坐標系相對于經(jīng)坐標系的旋轉(zhuǎn)及平移變換量。輸出參數(shù)Xang、Yang、Zang為上平臺旋轉(zhuǎn)姿態(tài)角，輸出參數(shù)Xpos、Ypos、Zpos為上平臺的位置坐標。上平臺參考軌跡模塊是一個子系統(tǒng)模塊，打開該模塊后，其結(jié)構(gòu)如圖4所示。這里信號源均設(shè)為正弦波信號，用Sine Wave模塊，輸出參考軌

16、跡可以根據(jù)實際需求設(shè)置。圖4 上平臺輸入?yún)⒖架壽E模塊3.3 旋轉(zhuǎn)矩陣模型旋轉(zhuǎn)矩陣模塊是由9個函數(shù)封裝而成的子系統(tǒng)模塊，其內(nèi)部機構(gòu)框圖如圖5所示。每一個函數(shù)都是有Fcn模塊構(gòu)成，其相應(yīng)的函數(shù)表達式與旋轉(zhuǎn)矩陣R中的9個元素相對應(yīng)。最后用Reshape模塊將生成的9×1列向量轉(zhuǎn)換成3×3矩陣，即為空間坐標的旋轉(zhuǎn)變換矩陣。圖5 旋轉(zhuǎn)矩陣模型3.4 桿長向量計算模塊桿長向量計算也是子系統(tǒng)模塊，此子系統(tǒng)的作用是根據(jù)各桿件向量及初始位置桿長，計算輸出各桿件的伸縮量，其內(nèi)部機構(gòu)框圖如圖6所示。再打開桿長1子系統(tǒng)模塊，其內(nèi)部結(jié)構(gòu)框圖如圖7所示，該子系統(tǒng)模塊通過桿件序號用Selector模塊選

17、擇桿件向量，然后經(jīng)過運算得到相應(yīng)桿件的伸縮量。各桿件桿長向量的計算方法相同，只是輸入時加上不同的桿件序號，并根據(jù)所選取的桿件序號進行相應(yīng)的桿長向量計算。圖6 桿長向量計算模塊圖7 桿長1子系統(tǒng)4. 仿真驗證 4.1位置逆解仿真驗證系統(tǒng)搭建為了更好地驗證位置逆解算法模型的正確性，特在Simulink集成開發(fā)環(huán)境下搭建位置逆解模型仿真驗證系統(tǒng)，如圖8所示。該系統(tǒng)由逆解模型模塊、微分模塊、PID控制器模塊、六自由度運動平臺物理模型模塊、慣性位置傳感器模塊、示波器顯示模塊6個模塊組成。其中，逆解模型模塊是由上述所建立的位置逆解模型打包封裝而成的子系統(tǒng)，作用是實現(xiàn)位置逆解，將上平臺輸入的參考軌跡轉(zhuǎn)化成6

18、支桿的伸縮量，即6支桿的期望位移。微分模塊的作用是將6支桿的期望位移轉(zhuǎn)換成期望運動速度。PID控制器模塊用于對六自由度運動平臺的運動的控制。六自由度運動平臺的物理模型模塊是由Simulink下的工具集SimMechanics建立的物理模型7。慣性位置傳感器模塊目的是為了對上平臺的運動進行實時監(jiān)測。示波器模塊有4個scope block，用以顯示期望桿長位置、實際桿長位置、桿長位置誤差以及上平臺質(zhì)心位置。圖8 位置逆解模型仿真驗證系統(tǒng)SimMechanics提供了各種模塊來代表機械系統(tǒng)中的實體部件、接頭、約束及力等等，同時又緊密結(jié)合了Matlab/Simulink，具有建模靈活性高、參數(shù)自定義化

19、、結(jié)構(gòu)易調(diào)整、效率高等特點，另外還提供了3D可視化視圖，可以很直觀的查看所建立的模型，這為仿真模型的構(gòu)建和進一步研究帶來了極大的方便。如圖9，為所建立的六自由度運動平臺物理模型的可視化視圖，在接下來的仿真驗證過程中，我們可以在該模型瀏覽器中觀察仿真演示過程。圖9平臺物理模型可視化圖形 4.2仿真驗證分析在本節(jié)中，主要通過一個位置逆解實例來驗證逆解的正確性。首先，設(shè)置平臺物理模型相關(guān)參數(shù)，設(shè)置平臺模型結(jié)構(gòu)參數(shù)如下：上平臺半徑為30cm，厚度為2cm，上鉸點分布圓的半徑為Ra=18cm，分布角為a=15°；下平臺半徑為40cm，厚度為8cm，下鉸點分布圓半徑為Rb=28cm，分布角為b=

20、15°；6支桿的初始長度為L0=31.52cm，萬向鉸高度為3cm，可知ha=4cm，hb=7cm，上平臺的初始高度為H0=38.52cm，具體模型如圖9所示。其次，對上平臺的運動軌跡進行規(guī)劃，使上平臺上升一定高度后繞著中心位置做圓周運動；設(shè)置上升高度為5cm，圓周運動半徑為2cm，運動軌跡方程為： x=2sin(2)y=2cos(2)z=H0+5=43.52 （7）最后，綜合考慮代價和控制效果調(diào)整PID控制8參數(shù)進行仿真驗證，選取PID參數(shù)為：Kp=7.5e4，Ki=3.5e4，Kd=1.5e3。仿真結(jié)果如圖10、圖11、圖12所示，其中圖10為桿長位置誤差，誤差精度達到10-3c

21、m，圖11為上平臺質(zhì)心位置運動變化，平臺實際高度為43.5cm，與期望高度誤差為2×10-3cm，誤差較小，圖12為上平臺質(zhì)心的實際運動軌跡，與期望輸入?yún)⒖架壽E相符。圖10 桿長位置誤差圖11 上平臺質(zhì)心位置圖12 上平臺質(zhì)心實際運動軌跡5. 結(jié)論本文首先運用D-H法分析了六自由度運動平臺的運動學位置逆解，建立了機構(gòu)的位置逆解數(shù)學模型，并基于Matlab/Simulink仿真集成環(huán)境建立其Simulink模型，實現(xiàn)了平臺位姿逆解實時解算。其次，通過Simulink下的SimMechanics工具集建立平臺的物理模型，并在Simulink中搭建了仿真驗證系統(tǒng)。最后進行實驗驗證，由所建立的反