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

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

Stewart并聯(lián)機(jī)構(gòu)以其剛度大、承載力強(qiáng)等特點(diǎn)彌補(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)運(yùn)動(dòng)學(xué)作為機(jī)器人領(lǐng)域的重點(diǎn)問題得到了國(guó)內(nèi)外學(xué)者的廣泛研究，實(shí)時(shí)精確的運(yùn)動(dòng)學(xué)正解算法對(duì)于并聯(lián)機(jī)構(gòu)的控制影響顯著。

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

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

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

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

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

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

圖1 初始采樣點(diǎn)解算過程Fig.1 Initial sample point process

圖2 k=2之后的計(jì)算過程Fig.2 Process after k=2

算法可分為4個(gè)步驟：

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

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

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

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

2 運(yùn)動(dòng)學(xué)模型

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

2.1 坐標(biāo)系建立與運(yùn)動(dòng)學(xué)反解

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

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

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

(1)

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

(2)

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

(3)

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

(4)

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

(5)

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

2.2 運(yùn)動(dòng)學(xué)正解

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

BP神經(jīng)網(wǎng)絡(luò)對(duì)非線性映射有很好的逼近能力，能實(shí)現(xiàn)從平臺(tái)桿長(zhǎng)變量空間到工作變量空間的映射，從而求得運(yùn)動(dòng)學(xué)正解[13]，6-6構(gòu)型平臺(tái)所用神經(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ò)用于提供初始估計(jì)值，精度要求不高，為節(jié)約神經(jīng)網(wǎng)絡(luò)訓(xùn)練時(shí)間，采用三層神經(jīng)網(wǎng)絡(luò)。輸入向量為執(zhí)行器長(zhǎng)度，輸出向量為正解位姿向量，輸入層、輸出層各由6個(gè)神經(jīng)元組成。根據(jù)下式確定隱藏層節(jié)點(diǎn)數(shù)：

(6)

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

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

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

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

2.2.2 線性近似與速度迭代

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

(7)

式中：T為采樣周期；k為采樣點(diǎn)序列。

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

(8)

式中J為雅克比矩陣。

(9)

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

2.2.3 牛頓迭代法

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

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

(10)

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

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

(11)

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

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

(12)

用向量符號(hào)為：

(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í)行器長(zhǎng)度與采集長(zhǎng)度對(duì)比滿足式(10)，則停止迭代，否則繼續(xù)迭代直到對(duì)應(yīng)反解滿足精度要求[20]。軌跡中的位置正解計(jì)算流程如圖5。

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

3 實(shí)例驗(yàn)證

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

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

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

圖6 平臺(tái)實(shí)物Fig.6 Picture of the platform

表1 鉸點(diǎn)位置Table 1 Position of hinge points cm

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

由式(6)可得隱藏層含有6個(gè)神經(jīng)元。設(shè)定上平臺(tái)平移范圍為±20 cm，旋轉(zhuǎn)范圍±20 rad，在該范圍內(nèi)隨機(jī)生成1 100組映射，其中1 000組作為學(xué)習(xí)樣本，其余100組作為測(cè)試樣本。經(jīng)實(shí)驗(yàn)可知，激勵(lì)函數(shù)選為tansig時(shí)訓(xùn)練時(shí)間最短，誤差下降曲線如圖7。測(cè)試樣本輸出誤差如圖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ì)值，再經(jīng)牛頓迭代得到軌跡運(yùn)動(dòng)初始采樣點(diǎn)正解的精確值。

圖8 測(cè)試樣本誤差Fig.8 Error of test samples

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

3.2 算法對(duì)比

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

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

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

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

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

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

4 結(jié)論

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

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

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