三自由度運(yùn)動(dòng)平臺(tái)的運(yùn)動(dòng)仿真及控制設(shè)計(jì)

聶良兵，王培俊，潘 璇，程宏釗

（西南交通大學(xué)機(jī)械工程學(xué)院，四川 成都 610031）

0 引言

在基于3－RPS的并聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)仿真相關(guān)研究工作中，主要存在以下不足:可視化程度低，難以提供逼真的三維仿真效果；運(yùn)算速度慢，所寫程序難以應(yīng)用到真實(shí)的運(yùn)動(dòng)控制中；不易實(shí)現(xiàn)參數(shù)化的動(dòng)態(tài)仿真；仿真內(nèi)容受到相應(yīng)軟件功能模塊的限制，程序擴(kuò)展性差［1］。針對(duì)以上問(wèn)題，自主研發(fā)了一套3－RPS機(jī)構(gòu)的參數(shù)化實(shí)時(shí)動(dòng)態(tài)的三維可視化仿真系統(tǒng)，并根據(jù)仿真結(jié)果，結(jié)合MPC2810運(yùn)動(dòng)控制卡，提出了一種機(jī)構(gòu)運(yùn)動(dòng)控制理論。

1 3－RPS機(jī)構(gòu)運(yùn)動(dòng)仿真系統(tǒng)功能模塊

仿真系統(tǒng)主要由以下幾個(gè)功能模塊組成:

a.三維可視化顯示模塊，實(shí)現(xiàn)對(duì)三自由度機(jī)構(gòu)的三維顯示、視角變換等。

b.運(yùn)動(dòng)仿真模塊，實(shí)現(xiàn)對(duì)運(yùn)動(dòng)平臺(tái)進(jìn)行逆解分析并對(duì)3個(gè)自由度進(jìn)行運(yùn)動(dòng)仿真，對(duì)仿真結(jié)果進(jìn)行整理保存。

c.機(jī)構(gòu)正解模塊，對(duì)運(yùn)動(dòng)平臺(tái)進(jìn)行正解分析，可根據(jù)用戶的輸入進(jìn)行解算。

d.機(jī)構(gòu)參數(shù)設(shè)置模塊，向用戶提供人機(jī)交互參數(shù)輸入界面，實(shí)現(xiàn)對(duì)機(jī)構(gòu)的動(dòng)態(tài)仿真。

2 3－RPS機(jī)構(gòu)仿真系統(tǒng)設(shè)計(jì)

2.1 三維模型的顯示

在進(jìn)行三維運(yùn)動(dòng)仿真前，必需完成三維模型的三維顯示，這是完成后續(xù)各項(xiàng)功能的基礎(chǔ)。OBJ文件是一種標(biāo)準(zhǔn)的3D模型文件格式，適合用于3D軟件模型之間的互導(dǎo)。目前，幾乎所有知名的主流3D軟件都支持OBJ文件的讀寫。在系統(tǒng)中，編寫了相應(yīng)的函數(shù)來(lái)對(duì)OBJ文件進(jìn)行解析和讀取。通過(guò)Pro／E或3ds Max等三維軟件，建立起三維基礎(chǔ)模型，然后將其導(dǎo)出為OBJ文件，最后對(duì)其讀取并繪制三維模型［2］。

2.2 3－RPS機(jī)構(gòu)逆解分析

圖1 3－RPS機(jī)構(gòu)

3－RPS機(jī)構(gòu)的典型形式如圖1所示［3］。為進(jìn)行三維運(yùn)動(dòng)仿真，首先應(yīng)進(jìn)行運(yùn)動(dòng)學(xué)逆解分析，以確定所要實(shí)現(xiàn)的運(yùn)動(dòng)。

A1，A2，A3表示電動(dòng)缸與固定平臺(tái)的銷軸聯(lián)接點(diǎn)，P1，P2，P3所形成的三角形表示運(yùn)動(dòng)平臺(tái)。為便于表達(dá)和計(jì)算，設(shè)置了2個(gè)坐標(biāo)系:固定不變的Oa－XYZ世界坐標(biāo)系和固接于運(yùn)動(dòng)平臺(tái)的Op－XYZ局部坐標(biāo)系［4］。對(duì)于運(yùn)動(dòng)平臺(tái)，應(yīng)實(shí)現(xiàn)繞其自身坐標(biāo)系Xp，Zp兩軸的旋轉(zhuǎn)運(yùn)動(dòng)和沿豎直方向的上下移動(dòng)。首先，以運(yùn)動(dòng)平臺(tái)為研究對(duì)象，設(shè)其繞Xp軸的旋轉(zhuǎn)角度為α，其對(duì)應(yīng)的旋轉(zhuǎn)矩陣為MRotx；繞Zp軸的旋轉(zhuǎn)角度為β，其對(duì)應(yīng)的旋轉(zhuǎn)矩陣為MRotz；沿豎直方向位移量為Height，其平移矩陣為MT，最終的模型變換矩陣為Mres，根據(jù)計(jì)算機(jī)圖形學(xué)［5］有:

將矩陣 Mres左乘P1，P2，P3的初始坐標(biāo)值，便可得到運(yùn)動(dòng)后各點(diǎn)的坐標(biāo)值［6］。

2.3 三維模型的繪制

在進(jìn)行了機(jī)構(gòu)運(yùn)動(dòng)學(xué)逆解后，還需要正確地繪制三維模型?，F(xiàn)以圖1中A2，P2處的電動(dòng)缸為例，闡述如何在OpenGL中正確繪制運(yùn)動(dòng)后的模型。

為動(dòng)態(tài)改變電動(dòng)缸的長(zhǎng)度并簡(jiǎn)化繪制過(guò)程，在此處將電動(dòng)缸分解為4部分進(jìn)行繪制。電動(dòng)缸的組成部件如圖2所示，其中A，C，D部分的模型為固不變部分，B部分的長(zhǎng)度則是可變的。電動(dòng)缸的總長(zhǎng)度應(yīng)由以上4部分組成，設(shè)其為L(zhǎng)，則有:

a，c，d為固定值；b為可變值。

圖2 電動(dòng)缸分解

按2.2節(jié)所述的方法，計(jì)算出運(yùn)動(dòng)后P2點(diǎn)的值，再假設(shè)用戶將電動(dòng)缸的總長(zhǎng)度設(shè)為L(zhǎng)，則可按以下流程進(jìn)行電動(dòng)缸模型的繪制:

a.假定一個(gè)電動(dòng)缸模型坐標(biāo)系，并在此坐標(biāo)系中進(jìn)行模型繪制。

b.在電動(dòng)缸坐標(biāo)系中的坐標(biāo)原點(diǎn)處繪制A部分。

c.在電動(dòng)缸坐標(biāo)系中，將C部分沿Y軸方向平移L－d并進(jìn)行繪制。

d.根據(jù)L，a，c，d的值計(jì)算出b的值，將B部分縮放至長(zhǎng)度b，并在電動(dòng)缸坐標(biāo)系中沿Y軸方向移動(dòng)L－c－d并繪制。

e.根據(jù)計(jì)算后的P2和A2的值，計(jì)算出P2A2的長(zhǎng)度length，同樣在電動(dòng)缸坐標(biāo)系中將D部分沿Y軸方向平移length并繪制。

f.根據(jù)A2和運(yùn)動(dòng)后點(diǎn)P2，計(jì)算出將電動(dòng)缸坐標(biāo)系轉(zhuǎn)換為世界坐標(biāo)系的變換矩陣［7］，并將此矩陣乘以O(shè)penGL當(dāng)前的模型變換矩陣。

機(jī)構(gòu)的其他部分按照同樣的方法進(jìn)行繪制，其中主要采用了坐標(biāo)系變換的方法。仿真最終效果如圖3所示。

圖3 仿真效果

用戶可以通過(guò)各參數(shù)面板設(shè)置機(jī)構(gòu)的尺寸，如座椅的長(zhǎng)、寬、高，電動(dòng)缸的安裝位置和初始高度等。

2.4 運(yùn)動(dòng)仿真設(shè)計(jì)

系統(tǒng)主要對(duì)3－RPS運(yùn)動(dòng)平臺(tái)3個(gè)自由度進(jìn)行仿真，即側(cè)翻、俯仰和豎直方向的運(yùn)動(dòng)。利用人機(jī)交互操作參數(shù)面板，可對(duì)各個(gè)自由度的加速度、最大速度以及運(yùn)動(dòng)的起止位置進(jìn)行設(shè)置。

當(dāng)按下仿真按鈕后，程序?qū)?huì)讀入相應(yīng)的參數(shù)，然后開啟一個(gè)線程，并在此線程內(nèi)設(shè)置一個(gè)while循環(huán)，在循環(huán)體內(nèi)不斷計(jì)算線程的運(yùn)行時(shí)間。通過(guò)此時(shí)間分別計(jì)算出α，β和豎直方向的運(yùn)動(dòng)量Height，并對(duì)運(yùn)動(dòng)平臺(tái)進(jìn)行更新，用戶可將運(yùn)動(dòng)平臺(tái)當(dāng)前的各狀態(tài)值保存于Excel中。

在主窗口區(qū)可以觀察到機(jī)構(gòu)的實(shí)時(shí)運(yùn)動(dòng)情況，電動(dòng)缸的實(shí)時(shí)狀態(tài)量也會(huì)顯示在屏幕左下方，仿真結(jié)束后可繪制出各個(gè)電動(dòng)缸的運(yùn)動(dòng)軌跡圖，如圖4所示。

圖4 運(yùn)動(dòng)軌跡

2.5 機(jī)構(gòu)正解分析

如圖1所示。建立與逆解相同的坐標(biāo)系，設(shè)各電動(dòng)缸銷軸副的安裝位置分別為A1，A2，A3，各電動(dòng)缸長(zhǎng)度分別為PA1，PA2和PA3，求此時(shí)P1，P2和P3的坐標(biāo)。根據(jù)相應(yīng)的約束條件可列出約束方程［8］，將其整理后可得方程組:

此為一個(gè)非線性方程組，可采用牛頓迭代法［9］進(jìn)行求解，則其Jacobi矩陣為:

設(shè)x＝［xp1，yp1，yp2，yp3，zp2］T，則由牛頓迭代公式x（k＋1）＝x（k）－（Df（x（k）））－1f（x（k）），通過(guò)一定次數(shù)的迭代計(jì)算，可得到滿足給定要求的結(jié)果。

用戶可在參數(shù)面板中設(shè)置各電動(dòng)缸的長(zhǎng)度、迭代初值以及迭代次數(shù)，若滿足各約束條件，則能求出在相應(yīng)電動(dòng)缸長(zhǎng)度下運(yùn)動(dòng)各點(diǎn)的坐標(biāo)值，實(shí)現(xiàn)對(duì)逆解法的驗(yàn)證。

3 運(yùn)動(dòng)控制設(shè)計(jì)

3.1 MPC2810運(yùn)動(dòng)控制卡

3－RPS運(yùn)動(dòng)平臺(tái)的控制實(shí)際上是對(duì)3個(gè)電動(dòng)缸相應(yīng)電機(jī)的控制。電機(jī)的控制方法主要有2類:通過(guò)PLC控制和通過(guò)運(yùn)動(dòng)控制卡控制。為使運(yùn)動(dòng)控制程序能夠與PC電腦進(jìn)行有效的通信，并集成于其他的PC虛擬軟件中，采用樂(lè)創(chuàng)MPC2810運(yùn)動(dòng)控制卡來(lái)進(jìn)行電機(jī)運(yùn)動(dòng)控制。

MPC2810運(yùn)動(dòng)控制卡可提供常速運(yùn)動(dòng)模式、梯形運(yùn)動(dòng)模式和S形曲線運(yùn)動(dòng)模式等，但其大多數(shù)運(yùn)動(dòng)模式都要求在運(yùn)動(dòng)控制前有明確的運(yùn)動(dòng)軌跡，且脈沖發(fā)送加速度在發(fā)送過(guò)程中通常是無(wú)法改變的。而在虛擬仿真環(huán)境中，具有許多隨機(jī)因素，無(wú)法在控制前確定運(yùn)動(dòng)軌跡。由圖4各電動(dòng)缸的運(yùn)動(dòng)軌跡圖也可看出，各電動(dòng)缸的運(yùn)動(dòng)軌跡是曲線，運(yùn)動(dòng)的加速度是隨時(shí)變化的，各電動(dòng)缸的運(yùn)動(dòng)也并非是獨(dú)立的。在MPC2810運(yùn)動(dòng)控制卡的各種運(yùn)動(dòng)模式中，也無(wú)法找到一種適合的運(yùn)動(dòng)模式來(lái)進(jìn)行控制。

3.2 以直代曲運(yùn)動(dòng)控制思想

在圖4中的運(yùn)動(dòng)軌跡曲線中，若在時(shí)間軸上取一時(shí)間微段Δt，則其運(yùn)動(dòng)軌跡曲線可看成由許多直線組成的，此直線運(yùn)動(dòng)過(guò)程恰好可以用常速運(yùn)動(dòng)模式來(lái)進(jìn)行控制。經(jīng)測(cè)試，MPC2810在1s內(nèi)大概可有效發(fā)送20條運(yùn)動(dòng)控制指命，則Δt可取0.05s，在此時(shí)間內(nèi)，將電動(dòng)缸的運(yùn)動(dòng)軌跡看作是以常速做直線運(yùn)動(dòng)。在每段運(yùn)動(dòng)開始時(shí)，可用仿真系統(tǒng)的方法計(jì)算出Δt，各電動(dòng)缸所需要的位移量Δs，則可得運(yùn)動(dòng)平均速度為:

然后以此為參數(shù)，調(diào)用MPC2810常速運(yùn)動(dòng)模式的相關(guān)函數(shù)，如下所述。

設(shè)置某軸常速度為v，即

啟動(dòng)某軸常速運(yùn)動(dòng)，行程為Δs，即

上面各函數(shù)相應(yīng)的參數(shù)可根據(jù)實(shí)際情況換算為脈沖單位，如此反復(fù)，便可完成所有要求運(yùn)動(dòng)軌跡。

3.3 誤差分析

在3.2節(jié)的運(yùn)動(dòng)控制方法中，直線運(yùn)動(dòng)的起始位置和終止位置都不存在位移誤差，誤差主要存在于運(yùn)動(dòng)過(guò)程中，以直代曲的位移偏差在直線運(yùn)動(dòng)結(jié)束時(shí)便會(huì)消除，并不會(huì)累積?，F(xiàn)取第3部分中的一段仿真數(shù)據(jù)進(jìn)行誤差分析，如表1所示。

表1 仿真數(shù)據(jù)

從表1和整個(gè)仿真數(shù)據(jù)可以發(fā)現(xiàn)，各個(gè)電動(dòng)缸在各種正常的運(yùn)動(dòng)過(guò)程中，在0.05s內(nèi)的位移量都沒(méi)有超過(guò)1mm。根據(jù)曲線的單調(diào)性可知，除了在波峰或波谷處，以直代曲的方式所產(chǎn)生的誤差都不會(huì)大于1mm，而這種誤差完全可以由機(jī)構(gòu)的安裝間隙所消除。

綜上所述，可采用如下方法提高機(jī)構(gòu)控制精度:采用高性能運(yùn)動(dòng)控制卡，提高每秒有效指令發(fā)送個(gè)數(shù)；盡量避免或減少運(yùn)動(dòng)軌跡出現(xiàn)波峰或波谷，使運(yùn)動(dòng)軌跡為單調(diào)曲線；采用高性能、高精度的電動(dòng)缸，減少電機(jī)的漏步現(xiàn)象。

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

仿真系統(tǒng)在對(duì)3－RPS機(jī)構(gòu)進(jìn)行正逆解的基礎(chǔ)上，對(duì)其進(jìn)行了三維動(dòng)態(tài)參數(shù)化仿真，不僅形象生動(dòng)地展示了運(yùn)動(dòng)平臺(tái)在各種運(yùn)動(dòng)情況下運(yùn)動(dòng)狀態(tài)，而且還確保了機(jī)構(gòu)運(yùn)動(dòng)學(xué)逆解的正確性，保證了運(yùn)動(dòng)控制程序算法的正確性，因?yàn)樗兴惴ǘ加蒀＋＋實(shí)現(xiàn)，所以可直接運(yùn)用于運(yùn)動(dòng)控制程序中。在仿真過(guò)程中，大量運(yùn)用坐標(biāo)系變換的思想，簡(jiǎn)化了Open－GL的繪制流程。最后根據(jù)仿真系統(tǒng)所提供的仿真結(jié)果，依據(jù)MPC2810運(yùn)動(dòng)控制卡，提出了以直代曲的運(yùn)動(dòng)控制思想。

［1］孫 濤，趙東標(biāo)，李曉慶.基于Open Inventor的六自由度鉆銑機(jī)加工仿真研究［J］.機(jī)械與電子，2012，（3）:35－40.

［2］黎 華，肖 偉.幾種三維模型文件在OpenGL中的輸入與處理［J］.物探化探計(jì)算技術(shù)，2007，29（1）:83－86.

［3］王旭永，王顯正，張 穎，等.三自由度并聯(lián)驅(qū)動(dòng)平臺(tái)機(jī)構(gòu)的位置逆解及其分析［J］.上海交通大學(xué)學(xué)報(bào)，1998，32（1）:102－104.

［4］毛泰祥，楊汝清，周 兵.三自由度并聯(lián)機(jī)器人結(jié)構(gòu)參數(shù)優(yōu)化與仿真方法研究［J］.機(jī)械與電子，2004，（3）:71－74.

［5］Hearn D D，Baker M P.Computer graphics with Open－GL［M］.Beijing:Publishing House of Electronics Industry，2006.

［6］張 敏，石秀華，杜向黨，等.三自由度機(jī)械手運(yùn)動(dòng)學(xué)分析［J］.機(jī)械與電子，2005，（1）:65－66.

［7］Shreiner Dave，the Khronos OpenGL ARB Working Group.OpenGL programming guide［M］.Beijing:China Machine Press，2010.

［8］崔國(guó)華，王國(guó)強(qiáng)，趙春江，等.空間轉(zhuǎn)動(dòng)三自由度并聯(lián)微調(diào)機(jī)構(gòu)設(shè)計(jì)與運(yùn)動(dòng)學(xué)分析［J］.農(nóng)業(yè)機(jī)械學(xué)報(bào)，2008，39（9）:144－148.

［9］徐士良.數(shù)值分析與算法［M］.北京:機(jī)械工業(yè)出版社，2007.