基于6-RSS并聯(lián)機(jī)器平臺(tái)的小球系統(tǒng)研究

劉艷梨 劉海瑞 鄒上元

（1.江蘇安全技術(shù)職業(yè)學(xué)院 電氣工程學(xué)院，徐州 221011；2.江蘇安全技術(shù)職業(yè)學(xué)院 交通與安全學(xué)院，徐州 210011）

6-RSS是一種新型并聯(lián)機(jī)器平臺(tái)，利用6個(gè)分支將上下兩個(gè)平臺(tái)連接起來[1]。6個(gè)短桿在電機(jī)的驅(qū)動(dòng)下可以自由轉(zhuǎn)動(dòng)。短桿和長(zhǎng)桿之間和長(zhǎng)桿與頂板之間通過球鉸連接。通過控制電機(jī)轉(zhuǎn)動(dòng)，實(shí)現(xiàn)平臺(tái)的6個(gè)自由度的運(yùn)動(dòng)。

本文研究基于6-RSS并聯(lián)機(jī)器平臺(tái)的小球系統(tǒng)，是一個(gè)典型的多變量和非線性控制系統(tǒng)，通過控制兩個(gè)相互垂直的旋轉(zhuǎn)軸運(yùn)動(dòng)來達(dá)到對(duì)小球定點(diǎn)和軌跡運(yùn)動(dòng)的控制[2-3]。其中，通過4線電阻屏采集小球在平臺(tái)上的位置，將位置信息反饋給控制系統(tǒng)；控制系統(tǒng)采用特定的控制策略得出兩個(gè)旋轉(zhuǎn)軸所需轉(zhuǎn)動(dòng)的角度，將角度信息反饋給電機(jī)，控制平臺(tái)旋轉(zhuǎn)[4-6]。

1 6-RSS并聯(lián)平臺(tái)

1.1 平臺(tái)逆運(yùn)動(dòng)學(xué)分析

為了方便建立6-RSS并聯(lián)機(jī)構(gòu)的位置逆解數(shù)學(xué)模型，在動(dòng)平臺(tái)和靜平臺(tái)上分別建立動(dòng)、靜兩個(gè)坐標(biāo)系，如圖1所示。動(dòng)坐標(biāo)系O′-X′Y′Z′建立在頂端分支桿和動(dòng)平臺(tái)聯(lián)接球鉸中心所在的平面內(nèi)，坐標(biāo)系原點(diǎn)為球鉸中心所在圓周的圓心。靜坐標(biāo)系O-XYZ建立在底端分支桿和靜平臺(tái)聯(lián)接轉(zhuǎn)動(dòng)副軸線的所在平面內(nèi)，坐標(biāo)系原點(diǎn)為轉(zhuǎn)動(dòng)副軸線內(nèi)切圓圓心。

圖1 6-RSS并聯(lián)平臺(tái)機(jī)構(gòu)簡(jiǎn)圖

圖1中，Ai表示與靜平臺(tái)聯(lián)接的轉(zhuǎn)動(dòng)副的軸線和底端分支桿的中心線交點(diǎn)在靜坐標(biāo)系下的位置矢量，有Ai={AiX AiYAiZ}T，i=[1,6]；Bi表示底端分支桿的中心線交點(diǎn)和頂端分支桿的中心線交點(diǎn)在靜坐標(biāo)系下的位置矢量，有Bi={BiXBiYBiZ}T，i=[1,6]；Ci表示與動(dòng)平臺(tái)聯(lián)接的球鉸中心在靜坐標(biāo)系下的位置矢量，有Ci={CiXCiYCiZ}T，i=[1,6]；Ci′表示與動(dòng)平臺(tái)聯(lián)接的球鉸中心在動(dòng)坐標(biāo)系下的位置矢量，有表示靜平臺(tái)上的 6 個(gè)轉(zhuǎn)動(dòng)副軸線在靜坐標(biāo)系下的矢量，有Di={DiXDiYDiZ}T，i=[1,6]。一般情況下，考慮機(jī)構(gòu)的對(duì)稱性，常取：

LDi分別表示6個(gè)底端分支桿的桿長(zhǎng)矢量，長(zhǎng)短不變，有：

式中：LDi分別表示6個(gè)頂端分支桿的桿長(zhǎng)矢量，其長(zhǎng)短不變；θi分別是6個(gè)轉(zhuǎn)動(dòng)副的轉(zhuǎn)動(dòng)角度，用底端分支桿的中心線相對(duì)其初始位置（底端分支桿中心線在XOY平面內(nèi)的狀態(tài)）轉(zhuǎn)過的角度來描述。

在建立上述坐標(biāo)系和定義相關(guān)參數(shù)以后，可以得到以下的數(shù)學(xué)關(guān)系：

式中：P為動(dòng)坐標(biāo)系原點(diǎn)在靜坐標(biāo)系下的位置矢量，P={OX′,OY′,OZ′}T；T為動(dòng)坐標(biāo)系的方向余弦矩陣，其第 1、2、3列分別為動(dòng)坐標(biāo)系三坐標(biāo)軸X′、Y′、Z′在定坐標(biāo)系下的方向余弦。T和P可共同表示動(dòng)平臺(tái)的位姿。

由式（10）～式（12），可得：

將式（14）展開，可得：

式中，LDi是θi的函數(shù)。由于 |LDi|=const、|LUi|=const，式（14）和式（15）實(shí)際上是關(guān)于θi的方程。在給定結(jié)構(gòu)的結(jié)構(gòu)幾何參數(shù)和動(dòng)平臺(tái)的位姿參數(shù)后，可以通過式（15）來求解θi，從而建立位置逆解的數(shù)學(xué)模型。

1.2 平臺(tái)轉(zhuǎn)角控制

控制平臺(tái)繞X軸、Y軸旋轉(zhuǎn)，即控制動(dòng)坐標(biāo)系的方向余弦矩陣?yán)@X軸旋轉(zhuǎn)α：

繞Y旋轉(zhuǎn)β：

2 小球系統(tǒng)控制

2.1 小球歸位物理分析（動(dòng)坐標(biāo)系內(nèi)）

設(shè)定初始小球位置Q(X0,Y0,Z0)，從電阻屏讀取初始位置Q，做PQ的垂線，以控制轉(zhuǎn)軸，平臺(tái)傾角為δ（如圖2所示），則小球的受力分析如圖3所示。圖3中，G為小球重力；N為斜面對(duì)小球的支撐力；Fs為斜面對(duì)小球的阻力，γ為斜面傾斜角。

圖2 小球在動(dòng)坐標(biāo)系下的轉(zhuǎn)動(dòng)位置

圖3 小球受力分析簡(jiǎn)圖

動(dòng)平臺(tái)與小球間的滾動(dòng)摩擦因數(shù)為μ，小球質(zhì)量為m，小球半徑為r，加速度為a，角加速度為a，則轉(zhuǎn)動(dòng)慣量為：

控制δ的大小使小球做純滾動(dòng)運(yùn)動(dòng)，動(dòng)力學(xué)方程為：

解得：

2.2 小球系統(tǒng)數(shù)學(xué)模型

利用拉格朗日方程對(duì)小球系統(tǒng)的數(shù)學(xué)模型進(jìn)行求解，求解過程中用到的參數(shù)如表1所示。

表1 小球系統(tǒng)的數(shù)學(xué)模型參數(shù)

小球動(dòng)能為：

平板動(dòng)能為：

小球繞支撐點(diǎn)的轉(zhuǎn)動(dòng)動(dòng)能為：

以坐標(biāo)原點(diǎn)為零勢(shì)能點(diǎn)，平板勢(shì)能和小球勢(shì)能為：

綜上得拉格朗日函數(shù)為：

對(duì)拉格朗日方程取下列4個(gè)變量：

定義：

可得：

2.3 小球的軌跡運(yùn)動(dòng)控制

2.3.1 平臺(tái)角加速度

由小球系統(tǒng)的數(shù)學(xué)模型可以得出，控制量為平臺(tái)的角加速度。因?yàn)橄到y(tǒng)是實(shí)現(xiàn)小球的定點(diǎn)和定軌跡運(yùn)動(dòng)，為保證小球純滾動(dòng)，平板傾斜角度不宜過大，接近于0°。所以，根據(jù)小量代換可將式（31）化簡(jiǎn)為：

其中：

2.3.2 LQR控制器

對(duì)于小球系統(tǒng)的LQR控制器，系統(tǒng)參數(shù)A、B已知，所以只需要確定Q、R兩個(gè)權(quán)值矩陣，就可通過式K=lqr(A,B,Q,R)求出狀態(tài)反饋矩陣K。

因?yàn)橄到y(tǒng)具有對(duì)稱性，且X、Y方向結(jié)構(gòu)一致，所以以X方向求解為例進(jìn)行說明，有：

本實(shí)驗(yàn)取R=2，Q=diag(Q11,Q22,Q33,Q44)。其中，Q11、Q22分別代表小球X軸的實(shí)際值與設(shè)定值差和小球X軸的速度權(quán)重；Q33、Q44分別代表Y軸方向平板傾斜角和Y軸方向平板傾斜角速度權(quán)重。這里取Q=diag(300,4,1 000,10)，得：

3 系統(tǒng)仿真

利用Creo軟件搭建系統(tǒng)仿真模型并模擬其工作空間，如圖4所示。

圖4 系統(tǒng)仿真模型及模擬的工作空間

搭建Simulink模型對(duì)LQR控制器進(jìn)行仿真（圖5），得到小球位置階躍響應(yīng)曲線和平板轉(zhuǎn)角階躍響應(yīng)曲線，表明小球在4 s內(nèi)可以達(dá)到穩(wěn)定狀態(tài)。

圖5 LQR控制器模型

4 結(jié)語

本文介紹了一種新型基于6-RSS并聯(lián)機(jī)器平臺(tái)的小球控制系統(tǒng)。系統(tǒng)利用6自由度并聯(lián)連桿驅(qū)動(dòng)結(jié)構(gòu)實(shí)現(xiàn)了穩(wěn)態(tài)維持、分析及控制功能，采用PID閉環(huán)控制系統(tǒng)和程序?qū)崿F(xiàn)了對(duì)6自由度并聯(lián)連桿傳動(dòng)機(jī)構(gòu)的運(yùn)動(dòng)控制。該系統(tǒng)在飛行器、船舶、車輛以及機(jī)器人等的運(yùn)動(dòng)過程穩(wěn)態(tài)維持及控制模擬中具有直接應(yīng)用價(jià)值。