Stewart平臺實時位置正解通用方法

朱齊丹， 張錚， 紀(jì)勛

(哈爾濱工程大學(xué) 智能科學(xué)與工程學(xué)院，黑龍江 哈爾濱 150001)

Stewart并聯(lián)機(jī)構(gòu)以其剛度大、承載力強(qiáng)等特點彌補(bǔ)了串聯(lián)機(jī)器人的不足，在工業(yè)領(lǐng)域具有廣闊的應(yīng)用前景。根據(jù)執(zhí)行器的分布不同大致分為3-3、5-4、6-5和6-6等構(gòu)型[1]。并聯(lián)機(jī)構(gòu)運動學(xué)作為機(jī)器人領(lǐng)域的重點問題得到了國內(nèi)外學(xué)者的廣泛研究，實時精確的運動學(xué)正解算法對于并聯(lián)機(jī)構(gòu)的控制影響顯著。

Stewart并聯(lián)機(jī)構(gòu)位置正解需要求解一組強(qiáng)耦合非線性方程組，尚無公認(rèn)正解解法能兼顧計算的高效與精確，而逆解相對容易。傳統(tǒng)正解方法分為數(shù)值法和解析法，數(shù)值法[2-3]核心為迭代計算，計算量較大，計算的結(jié)果對初值敏感，迭代初值選取不當(dāng)則結(jié)果可能發(fā)散；解析法通過建立約束方程[4-5]、消元[6-7]和幾何分析[8]等方法建立了幾種特定結(jié)構(gòu)并聯(lián)平臺的封閉解，該類算法無需初值且能避免位置奇異，但是公式推導(dǎo)復(fù)雜，僅適用于特定結(jié)構(gòu)的并聯(lián)平臺，具有局限性。

近年來，學(xué)者們開始借鑒智能算法解決位置正解問題，以此彌補(bǔ)傳統(tǒng)位置正解算法缺陷，神經(jīng)網(wǎng)絡(luò)無需推導(dǎo)輸入輸出的顯性表達(dá)式，但是得到精確解需要大量樣本數(shù)據(jù)進(jìn)行學(xué)習(xí)訓(xùn)練[9]。神經(jīng)網(wǎng)絡(luò)算法中，BP神經(jīng)網(wǎng)絡(luò)研究最為普遍，不能同時滿足求解的精確性和快速性，眾多學(xué)者基于該算法進(jìn)行改進(jìn)[10-13]。此外，Morell等[14]使用支持向量回歸的方法求解，思路新穎，但該方法模型訓(xùn)練時間過長。劉偉銳等[15]基于PSO算法，吳小勇等[16]、謝志江等[17]改進(jìn)了蟻群算法應(yīng)用于正解問題，未能驗證求解耗時。Mahmoodi等[18]通過安裝旋轉(zhuǎn)傳感器實時采集位姿數(shù)據(jù)，擺脫了平臺構(gòu)型的限制，通用性高，但該方法精確度依賴于傳感器靈敏度，且信號處理占用了大量運算資源。

為了克服以上算法思路的不足，本文提出一種混合算法，結(jié)合非線性系統(tǒng)線性化的思路和計算機(jī)的計算流程，使用速度迭代減少了總迭代次數(shù)，縮短了計算時間，最后通過試驗對比驗證了本文算法的精確性與實時性。

1 并聯(lián)機(jī)構(gòu)正解的混合算法

不同于以往學(xué)者孤立地求解單個點的位置正解，本文算法融合傳統(tǒng)算法與智能算法的優(yōu)點，基于軌跡運動過程實時求解。由于計算機(jī)解算位置正解時以極短的采樣周期對運動數(shù)據(jù)進(jìn)行采樣，之后對此進(jìn)行解算，根據(jù)這一特性將計算過程離散化，利用上一采樣點數(shù)據(jù)進(jìn)行速度迭代得出當(dāng)前時刻的位置正解以節(jié)約運算資源。

混合算法由3個部分組成：BP神經(jīng)網(wǎng)絡(luò)、雅克比速度迭代和牛頓迭代法。計算機(jī)采樣點序列為k，其中，初始采樣點(k=1)的位置正解由BP神經(jīng)網(wǎng)絡(luò)和牛頓迭代法處理得出，如圖1所示。其中，BP神經(jīng)網(wǎng)絡(luò)僅在位姿初始值未知時執(zhí)行一次，用于提供后續(xù)計算的初始估計值。若位姿初始值已知，則不啟用BP神經(jīng)網(wǎng)絡(luò)。k=2和之后的位置正解過程如圖2，其中速度迭代的作用是通過線性化計算減少總迭代次數(shù)。

圖1 初始采樣點解算過程Fig.1 Initial sample point process

圖2 k=2之后的計算過程Fig.2 Process after k=2

算法可分為4個步驟：

1)建立運動學(xué)模型：根據(jù)各執(zhí)行器初始狀態(tài)時的上下連接點坐標(biāo)建立運動學(xué)反解方程，在此基礎(chǔ)上建立非線性方程組作為運動學(xué)正解求解公式。

2)神經(jīng)網(wǎng)絡(luò)訓(xùn)練：獲取若干組工作空間內(nèi)的位姿與對應(yīng)的執(zhí)行器長度，交換數(shù)據(jù)集映射關(guān)系作為BP神經(jīng)網(wǎng)絡(luò)的訓(xùn)練樣本，使用訓(xùn)練完畢的神經(jīng)網(wǎng)絡(luò)輸出估計值作為后續(xù)計算的初始值。

3)速度迭代：依據(jù)運動的線性近似求得位姿變化速度，與前一采樣周期累加得到速度迭代的結(jié)果，并更新雅可比矩陣。

4)速度迭代的結(jié)果代入誤差判斷公式，若不滿足精度要求，則繼續(xù)牛頓迭代直至滿足精度要求。

2 運動學(xué)模型

Stewart平臺存在多種構(gòu)型，本文以6-6構(gòu)型平臺為例建立坐標(biāo)系并分析其運動學(xué)問題，同樣的正解思路對其他構(gòu)型的并聯(lián)機(jī)構(gòu)也可移植應(yīng)用。

2.1 坐標(biāo)系建立與運動學(xué)反解

6-6構(gòu)型Stewart平臺由6個執(zhí)行器連接基座和上平臺，動坐標(biāo)系與上平臺固連，靜坐標(biāo)系建立在基座中心位置。運動學(xué)反解的基本思路是：根據(jù)動靜坐標(biāo)系以及歐拉角旋轉(zhuǎn)變換公式推導(dǎo)出動坐標(biāo)系中的點到靜坐標(biāo)系的位姿轉(zhuǎn)換矩陣，求出6個執(zhí)行器上鉸點在靜坐標(biāo)系中的坐標(biāo)，根據(jù)空間向量二范數(shù)公式求出對應(yīng)執(zhí)行器的長度，得到上平臺位姿反解公式[19]。根據(jù)本文并聯(lián)機(jī)構(gòu)機(jī)械參數(shù)，為方便矩陣換算，將靜坐標(biāo)系建立在各執(zhí)行器伸長量為零時對應(yīng)的動坐標(biāo)系位置，與該位置動坐標(biāo)系重合。平臺坐標(biāo)系如圖3。

圖3 Stewart平臺坐標(biāo)系Fig.3 Axis of Stewart platform

執(zhí)行器上鉸點Ai(i=1,2,…,6)在動坐標(biāo)系中的矢量為：

(1)

執(zhí)行器下鉸點Bi(i=1,2,…,6)在靜坐標(biāo)系中的矢量為：

(2)

動坐標(biāo)系位姿表示為：

(3)

式中：Op為動坐標(biāo)系原點在靜坐標(biāo)系中的位置矢量；θ為動坐標(biāo)系繞x、y、z軸的歐拉角矢量；AiBi由編號為i的執(zhí)行器連接。動坐標(biāo)系相對靜坐標(biāo)系的旋轉(zhuǎn)矩陣為：

(4)

式中s表示sin函數(shù)，c表示cos函數(shù)。上平臺運動時，上鉸點在動坐標(biāo)系中的向量可變換為該點在靜坐標(biāo)系中的向量，各執(zhí)行器長度，即運動學(xué)反解公式可表示為：

(5)

式中Li為第i個執(zhí)行器的長度，i=1,2，…，6。

2.2 運動學(xué)正解

2.2.1 BP神經(jīng)網(wǎng)絡(luò)

BP神經(jīng)網(wǎng)絡(luò)對非線性映射有很好的逼近能力，能實現(xiàn)從平臺桿長變量空間到工作變量空間的映射，從而求得運動學(xué)正解[13]，6-6構(gòu)型平臺所用神經(jīng)網(wǎng)絡(luò)基本結(jié)構(gòu)如圖4所示。

圖4 BP神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)Fig.4 Structure of BP neural network

本文BP神經(jīng)網(wǎng)絡(luò)用于提供初始估計值，精度要求不高，為節(jié)約神經(jīng)網(wǎng)絡(luò)訓(xùn)練時間，采用三層神經(jīng)網(wǎng)絡(luò)。輸入向量為執(zhí)行器長度，輸出向量為正解位姿向量，輸入層、輸出層各由6個神經(jīng)元組成。根據(jù)下式確定隱藏層節(jié)點數(shù)：

(6)

式中：i、o分別為輸入層、輸出層節(jié)點數(shù)。

訓(xùn)練神經(jīng)網(wǎng)絡(luò)需要合適的訓(xùn)練樣本。根據(jù)平臺的運動空間和執(zhí)行器硬件參數(shù)，為保證神經(jīng)網(wǎng)絡(luò)的魯棒性，在6個自由度上隨機(jī)取值，由式(5)得到平臺反解數(shù)據(jù)的映射為{αβγxyz}→{L1L2L3L4L5L6}，再將2組數(shù)據(jù)集映射方向逆轉(zhuǎn)即得到位置正解的神經(jīng)網(wǎng)絡(luò)學(xué)習(xí)樣本。

軌跡運動經(jīng)采樣后得到離散點序列，實時計算位置正解時計算機(jī)以采樣周期T采集執(zhí)行器長度，求解對應(yīng)時刻位置正解。

由于樣本數(shù)量有限，神經(jīng)網(wǎng)絡(luò)輸出結(jié)果一般不能達(dá)到精確度要求，需要用牛頓迭代法得出軌跡運動初始采樣點的位置正解，作為后續(xù)速度迭代的初始基準(zhǔn)值。

2.2.2 線性近似與速度迭代

由于計算機(jī)采樣周期足夠短，且軌跡運動時執(zhí)行器速度不會產(chǎn)生較大突變，執(zhí)行器瞬時速度計算公式為：

(7)

式中：T為采樣周期；k為采樣點序列。

雅克比矩陣將關(guān)節(jié)空間速度映射到笛卡爾空間速度，可用于求解位姿變化速度。式(5)兩端求導(dǎo)得到位姿變化速度與執(zhí)行器速度的關(guān)系：

(8)

式中J為雅克比矩陣。

(9)

由于第k個點的正解精確度依賴上一周期的正解，且將采樣間隔的執(zhí)行器理想化為勻速運動，此時求得正解有微小偏差，為避免誤差累積，式(9)得出的數(shù)據(jù)需進(jìn)一步驗證。

2.2.3 牛頓迭代法

牛頓迭代法求解精確，但收斂效果依賴初值選取。式(9)單次所求結(jié)果已經(jīng)逼近真實值，不會造成迭代的離散。

設(shè)置允許的誤差閾值為ε，將式(9)結(jié)果代入式(5)，以此求得的執(zhí)行器長度與實際長度對比：

(10)

式中Liy為式(9)求得的位姿經(jīng)反解得到的各執(zhí)行器長度。

若式(9)結(jié)果使式(10)成立，則求得位置正解足夠精確，以此次結(jié)果計算雅克比矩陣并作為下一采樣周期的累加值；若不滿足精度要求，則用牛頓迭代法校正。為表示方便，將式(3)重寫為：

(11)

式中：q1、q2、q3為歐拉角變量;q4、q5、q6為位置變量。

迭代的目標(biāo)函數(shù)為：

(12)

用向量符號為：

(13)

將式(9)得到的解代入下式:

P(n+1)=P(n)-H′(P(n))-1H(P(n))

(14)

其中：

n為牛頓迭代的次數(shù)。P(0)=P(k)；n=0，1，…。

求得結(jié)果代入反解公式，若求得執(zhí)行器長度與采集長度對比滿足式(10)，則停止迭代，否則繼續(xù)迭代直到對應(yīng)反解滿足精度要求[20]。軌跡中的位置正解計算流程如圖5。

圖5 獲得初始值后的處理流程Fig.5 Process after the initial value is obtained

3 實例驗證

本文以MOOG公司生產(chǎn)的MB-E-6DOF并聯(lián)機(jī)構(gòu)為驗證平臺，實物圖如圖6。

3.1 BP神經(jīng)網(wǎng)絡(luò)訓(xùn)練

由Stewart并聯(lián)機(jī)構(gòu)機(jī)械參數(shù)得到執(zhí)行器伸長量為零時各執(zhí)行器鉸點位置坐標(biāo)見表1，A為上鉸點，B為下鉸點，其中上鉸點坐標(biāo)相對于動坐標(biāo)系，下鉸點坐標(biāo)相對靜坐標(biāo)系。

圖6 平臺實物Fig.6 Picture of the platform

表1 鉸點位置Table 1 Position of hinge points cm

為使執(zhí)行器遠(yuǎn)離正負(fù)限位，各執(zhí)行器在開始運動前伸長量設(shè)置為16.23 cm。

由式(6)可得隱藏層含有6個神經(jīng)元。設(shè)定上平臺平移范圍為±20 cm，旋轉(zhuǎn)范圍±20 rad，在該范圍內(nèi)隨機(jī)生成1 100組映射，其中1 000組作為學(xué)習(xí)樣本，其余100組作為測試樣本。經(jīng)實驗可知，激勵函數(shù)選為tansig時訓(xùn)練時間最短，誤差下降曲線如圖7。測試樣本輸出誤差如圖8。

圖7 誤差下降曲線Fig.7 Error reduction curves

由圖7可知，神經(jīng)網(wǎng)絡(luò)在經(jīng)過220次迭代后達(dá)到輸出誤差要求。由圖8知，BP神經(jīng)網(wǎng)絡(luò)輸出誤差在6%以內(nèi)，滿足作為迭代初值的要求。在初始值未知的情況下可由BP神經(jīng)網(wǎng)絡(luò)輸出正解估計值，再經(jīng)牛頓迭代得到軌跡運動初始采樣點正解的精確值。

圖8 測試樣本誤差Fig.8 Error of test samples

該神經(jīng)網(wǎng)絡(luò)訓(xùn)練時間為5 s，在不同構(gòu)型的并聯(lián)平臺移植時能節(jié)約大量時間。

3.2 算法對比

為驗證混合算法的效果，本文從計算時間和迭代次數(shù)2個方面與牛頓迭代法進(jìn)行對比。程序在Windows 7，Matlab 2012a環(huán)境下運行，計算機(jī)配置為Intel i3CPU，內(nèi)存4 GB，采用Matlab自帶的秒表計時器統(tǒng)計計算時間。

使平臺1號、4號執(zhí)行器長度按正弦函數(shù)變化，幅值5.08 cm，頻率0.3 Hz，其余執(zhí)行器長度不變，在軌跡運動某一時刻開始每隔10 ms采集各執(zhí)行器實際長度，共采集4 666組執(zhí)行器長度數(shù)據(jù)。設(shè)定誤差閾值ε為0.001 cm，分別使用牛頓迭代法和混合算法解算位置正解，牛頓迭代法初始采樣點迭代初值選取為BP神經(jīng)網(wǎng)絡(luò)的輸出結(jié)果，其余點迭代初值選取為上一采樣點經(jīng)牛頓迭代的位置正解。每組數(shù)據(jù)解算時間和迭代次數(shù)如圖9。

圖9 正解計算耗時與迭代次數(shù)對比Fig.9 Comparision of time-consuming and iterative steps

由圖9可知，混合算法求取位置正解能有效縮短解算時間，減少迭代次數(shù)。允許誤差上限相同時，新算法較牛頓迭代法減少耗時約50%。由于2種算法設(shè)定誤差閾值相同，故計算精確度相同。

值得注意的是，混合算法在初始采樣點發(fā)生了3次迭代，之后的采樣點牛頓迭代次數(shù)為零，說明經(jīng)速度迭代計算后得出的位置正解誤差已經(jīng)符合要求，且速度迭代的收斂速度更快。盡管如此，考慮到計算機(jī)傳輸數(shù)據(jù)失幀等特殊情況，為避免誤差累積，混合算法中的牛頓迭代法部分需要保留。

平臺機(jī)械參數(shù)已知，即位置反解公式已知，由本文算法可快速得出相應(yīng)的雅克比矩陣，在不同構(gòu)型的平臺間移植簡單易行。

4 結(jié)論

1)本文針對Stewart平臺位置正解問題提出了結(jié)合傳統(tǒng)算法與智能算法的混合算法，以6-6 Stewart平臺為例對算法進(jìn)行了驗證。在相同精度要求下，新算法較牛頓迭代法耗時更短，具有更強(qiáng)實時性。

2)本文算法可應(yīng)用于并聯(lián)平臺的多線程任務(wù)控制，為其他耗時較長的任務(wù)，如視覺處理、與其他設(shè)備的通信節(jié)約計算資源。

由于條件有限，未在更多構(gòu)型的并聯(lián)平臺上驗證，后續(xù)研究將在其他構(gòu)型的并聯(lián)平臺上應(yīng)用驗證。由于姿態(tài)矩陣采用歐拉角方式描述，為防止并聯(lián)平臺失控，在軌跡規(guī)劃階段避免了奇異情況，該情況有待在后續(xù)工作中研究。