基于STM 32的電動6-DOF加載系統(tǒng)設計*

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

（大連理工大學機械工程學院，遼寧大連 116024）

0 引言

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

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

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

1 電動6-DOF加載系統(tǒng)的體系結構

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

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

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

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

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

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

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

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

（1）坐標系規(guī)定：分別以動平臺中心A0和固定平臺中心B0為坐標系原點，建立右手規(guī)則的動坐標系{A}和靜坐標系{B}，如圖2所示。

圖2 加載裝置的俯視圖

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

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

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

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

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

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

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

結合式（1）、（3）得到第6根軸的逆運動學數(shù)學模型：

同理，從單根軸推廣到6根軸，可以寫出整個加載裝置的逆運動學數(shù)學模型。B L為6根軸的矢量表示，A W為動平臺上6個球鉸中心坐標，B X為平移矩陣，B Q為固定平臺上6個球鉸中心坐標，整個加載裝置的逆運動學數(shù)學模型為：

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

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

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

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

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

圖5 軸6伺服驅(qū)動器接收的PWM信號

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

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

3 多自由度加載實驗

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

圖7 多自由度加載的實驗裝置

為了得到動平臺的運動位移，在X、Y、Z每一個方向都安裝了一個高精度激光位移傳感器；為了得到動平臺中心受到的六維力，在動平臺底部中心位置安裝了高精度六維力傳感器。

3.1 位移控加載

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

圖8 YZ軸的位移階躍加載

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

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

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

3.2 力控加載

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

圖10Z軸的力位移加載

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

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

4 結束語

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