廖連杰，桑 勇，郭聯(lián)龍，蔣路明

（大連理工大學(xué)機(jī)械工程學(xué)院，遼寧大連 116024）

0 引言

并聯(lián)機(jī)器人也叫做并聯(lián)機(jī)構(gòu)，其特點(diǎn)是在基座和末端執(zhí)行器之間至少存在兩個(gè)運(yùn)動(dòng)鏈?？梢赃M(jìn)行縱移、橫移、升降、滾轉(zhuǎn)、俯仰、偏航運(yùn)動(dòng)的六自由度并聯(lián)機(jī)構(gòu)被稱為6-DOF并聯(lián)平臺(tái)，該類平臺(tái)擁有結(jié)構(gòu)強(qiáng)度高、精度高、響應(yīng)速度快等優(yōu)勢(shì)，是運(yùn)動(dòng)模擬、精密對(duì)接、醫(yī)療手術(shù)等設(shè)備[1-3]的關(guān)鍵組成部分，廣泛應(yīng)用于工業(yè)與生活當(dāng)中。

位移、力加載實(shí)驗(yàn)在工程中十分常見(jiàn)，比如研究材料壓縮特性時(shí)，常用單自由度位移、力拉壓實(shí)驗(yàn)機(jī)來(lái)進(jìn)行壓縮試驗(yàn)[4]；研究土的強(qiáng)度和本構(gòu)特性時(shí)，常用動(dòng)靜三軸實(shí)驗(yàn)儀進(jìn)行單自由度的位移、力加載測(cè)試[5]。單自由度加載設(shè)備應(yīng)用已經(jīng)很廣泛，近年來(lái)一些學(xué)者對(duì)多自由度加載設(shè)備進(jìn)行了研究，許國(guó)山等[6]研究了電液伺服三自由度加載系統(tǒng)的考慮幾何非線性的控制方法，但是針對(duì)一些需要進(jìn)行高精度多自由度的位移、力加載的場(chǎng)合，目前大部分設(shè)備難以實(shí)現(xiàn)。

6-DOF并聯(lián)平臺(tái)能完成復(fù)雜的多自由度組合運(yùn)動(dòng)[7-8]，另外機(jī)電伺服系統(tǒng)具有結(jié)構(gòu)緊湊、響應(yīng)快、節(jié)能環(huán)保、精度高等特點(diǎn)[9-10]，綜合以上兩點(diǎn)本文研制了基于Stewart平臺(tái)構(gòu)型的電動(dòng)6-DOF加載系統(tǒng)，它能實(shí)現(xiàn)多自由度高精度位移、力加載功能。本文首先介紹加載系統(tǒng)的體系結(jié)構(gòu)，詳細(xì)闡述各系統(tǒng)硬件之間的關(guān)系，之后建立了加載系統(tǒng)的逆運(yùn)動(dòng)學(xué)數(shù)學(xué)模型。針對(duì)加載裝置實(shí)現(xiàn)力控功能的難點(diǎn)問(wèn)題，提出了基于位移環(huán)的閉環(huán)力控的解決方案。最后設(shè)計(jì)多自由度加載實(shí)驗(yàn)，測(cè)試了6-DOF加載系統(tǒng)。

1 電動(dòng)6-DOF加載系統(tǒng)的體系結(jié)構(gòu)

電動(dòng)6-DOF加載裝置、控制柜和上位機(jī)組成加載系統(tǒng)的體系結(jié)構(gòu)，體系結(jié)構(gòu)的核心是基于Stewart平臺(tái)構(gòu)型的電動(dòng)6-DOF加載裝置。如圖1所示，加載裝置由固定平臺(tái)2、上下球鉸3、伺服電機(jī)4、電缸5、稱重傳感器6、動(dòng)平臺(tái)7、高精度六維力傳感器8以及其他連接件組成。加載裝置在地面組裝完成后，固定平臺(tái)2與固定架1之間用高強(qiáng)度螺釘連接，使得整個(gè)加載裝置呈現(xiàn)倒掛姿態(tài)。系統(tǒng)加載時(shí)伺服電機(jī)4旋轉(zhuǎn)帶動(dòng)電缸5伸縮，同時(shí)上下球鉸3同步旋轉(zhuǎn)，從而改變動(dòng)平臺(tái)7的位姿（位置和姿態(tài)）。本文自主開(kāi)發(fā)的加載系統(tǒng)控制程序運(yùn)行在工控機(jī)13上，工控機(jī)通過(guò)高速網(wǎng)線12連接到控制柜，控制柜存放除加載裝置和上位機(jī)以外的所有配件，包括伺服驅(qū)動(dòng)器9、六維力放大器10以及STM32控制器11?？刂乒駛?cè)面留有一個(gè)孔，加載系統(tǒng)的所有接線都接入該孔，這使得現(xiàn)場(chǎng)布置整潔有序。

圖1 加載系統(tǒng)的硬件組成

從圖1可以看出加載系統(tǒng)各硬件之間的關(guān)系。用戶在上位機(jī)中輸入加載指令，隨后上位機(jī)發(fā)送數(shù)據(jù)給STM32控制器。控制器有兩個(gè)功能：一是產(chǎn)生指定的PWM脈沖；二是通過(guò)TCP協(xié)議與上位機(jī)通信。除此之外控制器不進(jìn)行任何其他操作，確保系統(tǒng)能夠穩(wěn)定運(yùn)行。伺服驅(qū)動(dòng)器解析控制器產(chǎn)生的PWM脈沖后，驅(qū)動(dòng)伺服電機(jī)旋轉(zhuǎn)，從而改變動(dòng)平臺(tái)位姿。

此外，高精度六維力傳感器安裝在動(dòng)平臺(tái)的中心，將動(dòng)平臺(tái)所受六維力轉(zhuǎn)化為電信號(hào)，隨后由六維力放大器采集、放大電信號(hào)，通過(guò)UDP協(xié)議將六維力信息傳送至上位機(jī)。工控機(jī)有兩個(gè)網(wǎng)線口，一個(gè)網(wǎng)口控制STM32控制器產(chǎn)生脈沖，另一個(gè)網(wǎng)口接收六維力信息，兩網(wǎng)口同時(shí)工作時(shí)互不干擾。

2 加載系統(tǒng)的數(shù)學(xué)模型和控制體系

根據(jù)控制系統(tǒng)的設(shè)計(jì)步驟，在完成完電動(dòng)6-DOF加載系統(tǒng)的搭建后需要建立系統(tǒng)輸入與輸出之間的關(guān)系，為此本文對(duì)加載系統(tǒng)的逆運(yùn)動(dòng)學(xué)數(shù)學(xué)模型進(jìn)行了建立，并完成了位移加載和力控加載的控制體系設(shè)計(jì)。

2.1 加載系統(tǒng)的數(shù)學(xué)模型建立

逆運(yùn)動(dòng)學(xué)對(duì)加載系統(tǒng)的作業(yè)軌跡規(guī)劃和軌跡跟蹤控制具有重要意義，為研究動(dòng)平臺(tái)的位姿與伺服電缸伸縮量之間的數(shù)學(xué)關(guān)系，需要建立加載系統(tǒng)的逆運(yùn)動(dòng)學(xué)數(shù)學(xué)模型，該模型映射了動(dòng)平臺(tái)的位姿和各軸長(zhǎng)度之間的關(guān)系[11]。在建模中作以下規(guī)定。

（1）坐標(biāo)系規(guī)定：分別以動(dòng)平臺(tái)中心A0和固定平臺(tái)中心B0為坐標(biāo)系原點(diǎn)，建立右手規(guī)則的動(dòng)坐標(biāo)系{A}和靜坐標(biāo)系{B}，如圖2所示。

圖2 加載裝置的俯視圖

（2）軸編號(hào)規(guī)定：A1~A6是動(dòng)平臺(tái)上各球鉸的旋轉(zhuǎn)中心，B1~B6是固定平臺(tái)上各球鉸的旋轉(zhuǎn)中心，A i B i表示加載系統(tǒng)的第i根軸。

（3）空間位姿規(guī)定：p=[φθψx y z]T為動(dòng)平臺(tái)的位姿，φ、θ、ψ被稱之為橫滾自由度、俯仰自由度和偏航自由度，x、y、z被稱之為縱移自由度、平移自由度、升降自由度，動(dòng)平臺(tái)初始位姿為[0 0 0 0 0 0]T，初始位姿時(shí)動(dòng)平臺(tái)與固定平臺(tái)之間的高度用常量h表示。

上述加載系統(tǒng)是基于Stewart平臺(tái)構(gòu)型建立，6根軸的逆運(yùn)動(dòng)學(xué)數(shù)學(xué)模型相同，為了圖示簡(jiǎn)潔，本文只畫(huà)出第6根軸的逆運(yùn)動(dòng)學(xué)數(shù)學(xué)模型，如圖3所示。圖中B w6為6個(gè)動(dòng)平臺(tái)球鉸旋轉(zhuǎn)中心在{B}系下的坐標(biāo)，B l6為第6根軸在{B}系下的矢量表示，B q6為6個(gè)固定平臺(tái)球鉸旋轉(zhuǎn)中心在{B}系下的坐標(biāo)。由于各矢量都是在靜系{B}下的表示，根據(jù)同一坐標(biāo)系內(nèi)矢量的疊加原理，可得出：

圖3 加載系統(tǒng)第6根軸的逆運(yùn)動(dòng)學(xué)數(shù)學(xué)模型

式中的角標(biāo)B代表相關(guān)矢量是在{B}下的表示。動(dòng)平臺(tái)的各球鉸中心在{A}下的表示是不變的，通過(guò)坐標(biāo)變換，{A}下的向量能夠在{B}下表示出來(lái)。本文采用Z-Y-X歐拉角變換[12]表示動(dòng)平臺(tái)的姿態(tài)，其中涉及的旋轉(zhuǎn)轉(zhuǎn)換矩陣具體表達(dá)為：

式中：Rot(φ,θ,ψ)表示{A}繞著{A}的Z軸旋轉(zhuǎn)ψ，再繞新Y軸旋轉(zhuǎn)θ，最后繞新X軸旋轉(zhuǎn)φ的變換過(guò)程。

動(dòng)平臺(tái)和固定平臺(tái)之間的位置關(guān)系中心的位置關(guān)系表示為pt=[x y(z+h)]T。根據(jù)Z-Y-X歐拉角變換，將{A}下的A w6變換到{B}下，得到：

結(jié)合式（1）、（3）得到第6根軸的逆運(yùn)動(dòng)學(xué)數(shù)學(xué)模型：

同理，從單根軸推廣到6根軸，可以寫(xiě)出整個(gè)加載裝置的逆運(yùn)動(dòng)學(xué)數(shù)學(xué)模型。B L為6根軸的矢量表示，A W為動(dòng)平臺(tái)上6個(gè)球鉸中心坐標(biāo)，B X為平移矩陣，B Q為固定平臺(tái)上6個(gè)球鉸中心坐標(biāo)，整個(gè)加載裝置的逆運(yùn)動(dòng)學(xué)數(shù)學(xué)模型為：

假設(shè)各軸的初始長(zhǎng)度為l0i(i=1,2,…,6)，最終各伺服電缸伸縮量Δl i表示為：

2.2 加載系統(tǒng)的控制體系設(shè)計(jì)

本文設(shè)計(jì)了加載系統(tǒng)的位移控制系統(tǒng)，能夠?qū)崿F(xiàn)位移加載控制，其原理如圖4所示。

圖4 位移控制系統(tǒng)的原理

如圖5所示，輸入的位移加載指令可以是6個(gè)自由度的任意組合，用戶在上位機(jī)中輸入位移加載指令后，加載系統(tǒng)控制程序便規(guī)劃動(dòng)平臺(tái)的運(yùn)行軌跡，然后將運(yùn)行軌跡分成n小段。上位機(jī)將n小段數(shù)據(jù)逐次發(fā)送給STM32控制器，之后控制器產(chǎn)生PWM脈沖。為了觀察位移控制系統(tǒng)的PWM脈沖分布是否均勻，本文使用示波器在伺服驅(qū)動(dòng)器的PWM信號(hào)接收端進(jìn)行測(cè)試。圖5顯示動(dòng)平臺(tái)進(jìn)行勻速位移加載時(shí)軸6的伺服驅(qū)動(dòng)器接收到的PWM脈沖信號(hào)，圖中每?jī)蓚€(gè)脈沖對(duì)應(yīng)一小段運(yùn)行軌跡，脈沖分布十分均勻，從而驗(yàn)證了上述的位移控制系統(tǒng)。

圖5 軸6伺服驅(qū)動(dòng)器接收的PWM信號(hào)

本文提出基于位移環(huán)的閉環(huán)力控加載方案，能夠?qū)崿F(xiàn)動(dòng)平臺(tái)沿某個(gè)空間方位的動(dòng)態(tài)力加載。力控加載的流程如圖6所示。用戶輸入力加載指令Fi(t)時(shí)，加載系統(tǒng)的控制程序首先實(shí)時(shí)比較Fi(t)和Fu(t)，F(xiàn)u(t)為高精度六維力傳感器測(cè)出的六維力數(shù)值，然后控制程序?qū)⒘Φ钠盍喀(t)通過(guò)模糊PID控制器轉(zhuǎn)化為位移ΔS(t)。ΔS(t)作為位移控制系統(tǒng)的輸入指令，使得動(dòng)平臺(tái)輸出一定位移，這位移使得Fu(t)更加趨近于Fi(t)，從而降低了力的偏差。

圖6 閉環(huán)力控的流程

3 多自由度加載實(shí)驗(yàn)

電動(dòng)6-DOF加載系統(tǒng)能夠?qū)崿F(xiàn)多自由度的位移、力加載，加載方案組合眾多，為了實(shí)驗(yàn)方便本文以YZ軸為代表進(jìn)行了位移階躍加載，以Z軸為代表進(jìn)行了位移三角波加載，以Z軸為代表進(jìn)行了力正弦加載和力正弦加載，軸的規(guī)定和實(shí)驗(yàn)裝置如圖7所示。

圖7 多自由度加載的實(shí)驗(yàn)裝置

為了得到動(dòng)平臺(tái)的運(yùn)動(dòng)位移，在X、Y、Z每一個(gè)方向都安裝了一個(gè)高精度激光位移傳感器；為了得到動(dòng)平臺(tái)中心受到的六維力，在動(dòng)平臺(tái)底部中心位置安裝了高精度六維力傳感器。

3.1 位移控加載

動(dòng)平臺(tái)在工作空間的中心時(shí)有最大的位移加載范圍，故控制動(dòng)平臺(tái)下降100 mm到p=[0 0 0 0 0 100]T處，在此位置進(jìn)行YZ軸的位移階躍加載，加載的位移大小為30 2 mm，加載速度分別為1 mm/s、2 mm/s、5 mm/s。YZ軸的實(shí)驗(yàn)結(jié)果如圖8所示。從圖中的實(shí)驗(yàn)結(jié)果來(lái)看，加載系統(tǒng)完成了Y、Z軸的階躍加載，Y和Z兩個(gè)方向都加載了30 mm左右的位移，通過(guò)計(jì)算得到Y(jié)、Z軸的穩(wěn)態(tài)加載誤差分別小于0.25%和0.20%。

圖8 YZ軸的位移階躍加載

另外還進(jìn)行了3組不同幅值不同頻率的X軸的位移三角波加載，第1組幅值為5 mm，頻率為0.004 Hz；第2組幅值為5 mm，頻率為0.02 Hz；第3組幅值為30 mm，頻率為0.004 Hz。X軸的實(shí)際位移波形如圖9所示。從圖中的實(shí)驗(yàn)結(jié)果來(lái)看，加載系統(tǒng)實(shí)現(xiàn)了不同頻率、幅值的X軸三角波加載，3組實(shí)驗(yàn)的誤差小于0.49%。

圖9 X軸的位移三角波加載

由以上位移控加載的實(shí)驗(yàn)結(jié)果知：電動(dòng)6-DOF加載系統(tǒng)能夠?qū)崿F(xiàn)多自由度的高精度位移加載。

3.2 力控加載

加載系統(tǒng)的閉環(huán)力控方案基于位移環(huán)，前文通過(guò)實(shí)驗(yàn)已經(jīng)驗(yàn)證了多自由度的高精度位移加載功能，為此驗(yàn)證單自由度的高精度力加載功能，間接證明多自由度的高精度力加載功能。以Z軸為代表進(jìn)行了力正弦加載，實(shí)驗(yàn)結(jié)果如圖10所示。圖中的曲線1為理論力，曲線2為實(shí)際加載力，不難發(fā)現(xiàn)每次加載出現(xiàn)一個(gè)波峰之后，曲線2都能很快趨近曲線1，而且穩(wěn)定時(shí)實(shí)際加載力的偏差控制在±0.5 N以內(nèi)。

圖10Z軸的力位移加載

另外進(jìn)行了3組不同幅值不同頻率的Z軸的正弦加載，第1組幅值為80 N，頻率為0.02 Hz；第2組幅值為50 N，頻率為0.05 Hz；第3組幅值為10 N，頻率為0.1 Hz，實(shí)驗(yàn)結(jié)果如圖11所示。圖示結(jié)果表明力正弦加載具有平滑的波形，3組實(shí)驗(yàn)的最大誤差小于2.3%。

由以上力控加載的實(shí)驗(yàn)結(jié)果知：電動(dòng)6-DOF加載系統(tǒng)同樣能夠?qū)崿F(xiàn)多自由度的高精度力加載。

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

本文基于STM32研制了電動(dòng)6-DOF加載系統(tǒng)，該加載系統(tǒng)實(shí)現(xiàn)了高精度的位移、力加載，其中力加載一直是Stewart平臺(tái)構(gòu)型加載系統(tǒng)的難點(diǎn)問(wèn)題。本文詳細(xì)介紹了6-DOF加載系統(tǒng)的體系結(jié)構(gòu)，以及各硬件之間的關(guān)系，然后采用Z-Y-X歐拉角方法描述加載系統(tǒng)動(dòng)平臺(tái)的空間位姿，探討了動(dòng)平臺(tái)的空間位姿與伺服電缸伸縮量之間的數(shù)學(xué)關(guān)系，建立了加載系統(tǒng)的逆運(yùn)動(dòng)學(xué)數(shù)學(xué)模型。針對(duì)加載系統(tǒng)實(shí)現(xiàn)力控功能的難點(diǎn)問(wèn)題，提出了基于位移環(huán)的閉環(huán)力控的解決方案。最后設(shè)計(jì)了多自由度加載實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果表明YZ軸的位移階躍加載誤差小于0.25%，X軸的位移三角波加載誤差小于0.49%，Z軸的力正弦加載偏差控制在±0.5 N以內(nèi)，Z軸的力正弦加載誤差小于2.3%，客觀的實(shí)驗(yàn)結(jié)果驗(yàn)證了加載系統(tǒng)的多自由度高精度位移、力加載功能。