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

朱齊丹， 張錚， 紀勛

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

Stewart并聯機構以其剛度大、承載力強等特點彌補了串聯機器人的不足，在工業領域具有廣闊的應用前景。根據執行器的分布不同大致分為3-3、5-4、6-5和6-6等構型[1]。并聯機構運動學作為機器人領域的重點問題得到了國內外學者的廣泛研究，實時精確的運動學正解算法對于并聯機構的控制影響顯著。

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

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

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

1 并聯機構正解的混合算法

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

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

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

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

算法可分為4個步驟：

1)建立運動學模型：根據各執行器初始狀態時的上下連接點坐標建立運動學反解方程，在此基礎上建立非線性方程組作為運動學正解求解公式。

2)神經網絡訓練：獲取若干組工作空間內的位姿與對應的執行器長度，交換數據集映射關系作為BP神經網絡的訓練樣本，使用訓練完畢的神經網絡輸出估計值作為后續計算的初始值。

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

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

2 運動學模型

Stewart平臺存在多種構型，本文以6-6構型平臺為例建立坐標系并分析其運動學問題，同樣的正解思路對其他構型的并聯機構也可移植應用。

2.1 坐標系建立與運動學反解

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

圖3 Stewart平臺坐標系Fig.3 Axis of Stewart platform

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

(1)

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

(2)

動坐標系位姿表示為：

(3)

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

(4)

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

(5)

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

2.2 運動學正解

2.2.1 BP神經網絡

BP神經網絡對非線性映射有很好的逼近能力，能實現從平臺桿長變量空間到工作變量空間的映射，從而求得運動學正解[13]，6-6構型平臺所用神經網絡基本結構如圖4所示。

圖4 BP神經網絡結構Fig.4 Structure of BP neural network

本文BP神經網絡用于提供初始估計值，精度要求不高，為節約神經網絡訓練時間，采用三層神經網絡。輸入向量為執行器長度，輸出向量為正解位姿向量，輸入層、輸出層各由6個神經元組成。根據下式確定隱藏層節點數：

(6)

式中：i、o分別為輸入層、輸出層節點數。

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

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

由于樣本數量有限，神經網絡輸出結果一般不能達到精確度要求，需要用牛頓迭代法得出軌跡運動初始采樣點的位置正解，作為后續速度迭代的初始基準值。

2.2.2 線性近似與速度迭代

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

(7)

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

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

(8)

式中J為雅克比矩陣。

(9)

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

2.2.3 牛頓迭代法

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

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

(10)

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

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

(11)

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

迭代的目標函數為：

(12)

用向量符號為：

(13)

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

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

(14)

其中：

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

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

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

3 實例驗證

本文以MOOG公司生產的MB-E-6DOF并聯機構為驗證平臺，實物圖如圖6。

3.1 BP神經網絡訓練

由Stewart并聯機構機械參數得到執行器伸長量為零時各執行器鉸點位置坐標見表1，A為上鉸點，B為下鉸點，其中上鉸點坐標相對于動坐標系，下鉸點坐標相對靜坐標系。

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

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

為使執行器遠離正負限位，各執行器在開始運動前伸長量設置為16.23 cm。

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

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

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

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

該神經網絡訓練時間為5 s，在不同構型的并聯平臺移植時能節約大量時間。

3.2 算法對比

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

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

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

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

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

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

4 結論

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

2)本文算法可應用于并聯平臺的多線程任務控制，為其他耗時較長的任務，如視覺處理、與其他設備的通信節約計算資源。

由于條件有限，未在更多構型的并聯平臺上驗證，后續研究將在其他構型的并聯平臺上應用驗證。由于姿態矩陣采用歐拉角方式描述，為防止并聯平臺失控，在軌跡規劃階段避免了奇異情況，該情況有待在后續工作中研究。