五自由度搖擺臺(tái)位置正解方法研究及MATLAB實(shí)現(xiàn)

李輝，宋詩(shī)

(1.安徽建筑工業(yè)學(xué)院機(jī)電學(xué)院，安徽合肥230601;2.中國(guó)科技大學(xué)信息科學(xué)技術(shù)學(xué)院，安徽合肥230088)

搖擺臺(tái)實(shí)際上是一種并聯(lián)機(jī)構(gòu)，與串聯(lián)機(jī)構(gòu)相比具有剛度大、結(jié)構(gòu)穩(wěn)定、承載能力強(qiáng)、精度高、運(yùn)動(dòng)慣性較小、運(yùn)動(dòng)學(xué)位置反解易求和便于實(shí)時(shí)控制等優(yōu)點(diǎn)［1］，具有廣闊的應(yīng)用前景。因此，國(guó)內(nèi)外許多學(xué)者開(kāi)始研究并聯(lián)機(jī)構(gòu)，從最初的六自由度并聯(lián)機(jī)構(gòu)到后來(lái)的少自由度并聯(lián)機(jī)構(gòu)，在理論和應(yīng)用方面都取得了豐富的成果。

位置分析是并聯(lián)機(jī)構(gòu)分析和研究的基礎(chǔ)，一切后續(xù)的應(yīng)用都是從位置分析開(kāi)始的。所謂位置分析是指求解機(jī)構(gòu)運(yùn)動(dòng)平臺(tái)的位置和姿態(tài)與輸入桿長(zhǎng)度之間的關(guān)系。機(jī)構(gòu)的位置分析同時(shí)也是機(jī)構(gòu)的速度分析、加速度分析、受力分析以及誤差分析等的基礎(chǔ)，位置分析包括兩個(gè)問(wèn)題，位置正解和位置反解。機(jī)構(gòu)位置正解的任務(wù)就是在給定各個(gè)移動(dòng)副位移的情況下，求出上平臺(tái)在空間的位置和姿態(tài);反之，則為位置反解。位置反解的問(wèn)題比較簡(jiǎn)單，利用預(yù)先規(guī)劃好的位姿軌跡再結(jié)合機(jī)構(gòu)的尺寸采用坐標(biāo)變換的方法可以很快求出移動(dòng)副的位移。而位置正解卻比較麻煩，其核心是求解一組維數(shù)較多、耦合性強(qiáng)的非線性方程組。目前，位置正解主要有兩種解法，封閉解法和數(shù)值解法。這兩種方法各有優(yōu)缺點(diǎn):封閉解法的優(yōu)點(diǎn)是能夠得到方程的全部解，缺點(diǎn)是求解難度很大，并且一種機(jī)構(gòu)一種解法，沒(méi)有通用性［2］;數(shù)值解法的優(yōu)點(diǎn)是能夠方便迅速地對(duì)任何機(jī)型機(jī)構(gòu)求解，缺點(diǎn)是不能求出所有的位置解，并且最終的結(jié)果與所選的初值有直接的關(guān)系。

在工程應(yīng)用中，如何快速的找到合理的解遠(yuǎn)比找到全部的解有意義，并且文獻(xiàn) ［3-4］指出由桿長(zhǎng)驅(qū)動(dòng)的并聯(lián)機(jī)構(gòu)在其工作空間內(nèi)僅存在單解?；诖?，作者著重討論數(shù)值解法，提出一種粒子群優(yōu)化算法和Newton迭代法相結(jié)合的數(shù)值解法，并簡(jiǎn)單介紹了如何在MATLAB中實(shí)現(xiàn)該方法。

1 基于粒子群優(yōu)化算法的五自由度并聯(lián)機(jī)構(gòu)位置正解

粒子群優(yōu)化算法(PSO)是KENNEDY和EBERHART于1995年提出的一種基于群集智能的演化計(jì)算技術(shù)。該算法具有參數(shù)設(shè)置少、搜索能力強(qiáng)、并行性好、魯棒性強(qiáng)等特點(diǎn)，且搜索前期的收斂速度快，計(jì)算效率比傳統(tǒng)的隨機(jī)方法高，非常適合工程應(yīng)用。PSO算法中的每個(gè)粒子都是解空間的一個(gè)假想解，它根據(jù)自身的飛行經(jīng)驗(yàn)和同伴的飛行經(jīng)驗(yàn)來(lái)調(diào)整自己的飛行。其算法核心在于粒子的位置xi和速度vi更新公式:

式中:ω為慣性權(quán)重，c1、c2為學(xué)習(xí)因子，r1、r2為區(qū)間［0，1］上的隨機(jī)數(shù)，pbi為第i個(gè)粒子的個(gè)體極值，gb為全局極值。此外，粒子的運(yùn)動(dòng)還受到最大速度的限制，當(dāng)vi＞vmax時(shí)，取vi=vmax。

文中所討論的并聯(lián)機(jī)構(gòu)上、下平臺(tái)以5個(gè)分支相連，每個(gè)分支中間為移動(dòng)副，與上平面用球鉸相連，與下平面中心點(diǎn)以轉(zhuǎn)動(dòng)副相連，其余用球鉸相連。機(jī)構(gòu)示意圖如圖1所示。

圖1 五自由度搖擺臺(tái)機(jī)構(gòu)示意圖

由位置反解的分析可知，并聯(lián)機(jī)構(gòu)每支桿的長(zhǎng)度li(i=0，1，…，4)由動(dòng)坐標(biāo)系相對(duì)于定坐標(biāo)系的3個(gè)獨(dú)立轉(zhuǎn)角θx，θy，θz以及動(dòng)坐標(biāo)系原點(diǎn)A0在定坐標(biāo)系中的坐標(biāo)P(xA0，yA0，zA0)決定，寫成函數(shù)表達(dá)式:

因?yàn)槎ㄗ鴺?biāo)系的原點(diǎn)處為轉(zhuǎn)動(dòng)副，所以RPS分支只能在zA0y平面內(nèi)運(yùn)動(dòng)，即xA0≡0，故不予考慮。將式(3)改寫成以下形式:

于是，位置正解的問(wèn)題就轉(zhuǎn)化成求解非線性方程組(4)的問(wèn)題了。

令:

則有:

向量表示:

式中:a=(θx，θy，θz，yA0，zA0)T，

求解方程 (7)的問(wèn)題可以轉(zhuǎn)化為等價(jià)的函數(shù)優(yōu)化問(wèn)題

也就是說(shuō)求解方程 (7)的解的問(wèn)題就是尋找一向量a0(θx，θy，θz，yA0，zA0)使得P(a0)=0成立的問(wèn)題。式(8)同時(shí)也是采用粒子群優(yōu)化 (PSO)算法搜索最優(yōu)解時(shí)的適應(yīng)度函數(shù)。

以方程 (7)為例，搜索位姿參數(shù)的具體步驟如下:

步驟一，設(shè)置種群規(guī)模N，在并聯(lián)機(jī)構(gòu)可達(dá)空間內(nèi)隨機(jī)初始化各個(gè)粒子的位置xi0和速度vi0，并確定最大進(jìn)化代數(shù)Q和與全局極值相匹配的粒子占全部粒子的比例P。

步驟二，根據(jù)適應(yīng)度函數(shù)計(jì)算每個(gè)粒子的適應(yīng)度。

步驟三，由適應(yīng)度的大小確定個(gè)體最優(yōu)位置pbi及全局最優(yōu)位置gb。

步驟四，判斷全局最優(yōu)是否滿足精度要求，如果不滿足精度要求轉(zhuǎn)到步驟第三;反之，以當(dāng)前的全局最優(yōu)位置gb為要尋找的位姿輸出。

步驟五，看是否到達(dá)最大進(jìn)化代數(shù)Q，如果到達(dá)最大進(jìn)化代數(shù)，則以當(dāng)前的全局最優(yōu)位置gb為要尋找的合適初始位姿輸出。否則，繼續(xù)步驟六。

步驟六，根據(jù)PSO迭代公式(1)、(2)分別對(duì)群體的位置xi和速度vi進(jìn)行更新。更新完以后轉(zhuǎn)到步驟二。

2 并聯(lián)機(jī)構(gòu)位置正解方法的改進(jìn)

由于PSO算法在搜索后期的收斂速度低，且容易陷入局部最優(yōu)，既耗時(shí)又不精確。所以在搜索后期作者用經(jīng)典的具有較高精度的Newton迭代法取而代之，雖然Newton迭代法計(jì)算量大，但是由于之前的PSO算法已經(jīng)將搜索結(jié)果限定在離目標(biāo)值很近的范圍內(nèi)，所以Newton迭代法只需要很少的幾步迭代就可以使結(jié)果在誤差范圍以內(nèi)，對(duì)于實(shí)時(shí)性的影響并不大，可以同時(shí)滿足精度和實(shí)時(shí)性的雙重要求。

為了說(shuō)明問(wèn)題的方便，文中提出匹配的概念，解空間的任意兩個(gè)粒子xi和xj之間的距離σ(xi，xj)為:

對(duì)于給定的常數(shù)ε＞0，如果有σ(xi，xj)＜ε，則稱粒子xi和xj匹配。

通過(guò)大量的仿真試驗(yàn)，發(fā)現(xiàn)當(dāng)種群中有一半以上的粒子與當(dāng)前全局最優(yōu)粒子匹配的時(shí)候，可以認(rèn)為PSO算法進(jìn)入到后期搜索，此時(shí)以當(dāng)前全局最優(yōu)解為Newton迭代法的初始值。

對(duì)方程(7)采用Newton迭代法可得:

即:

在利用粒子群優(yōu)化算法搜索到合適的初始位姿a0后，將a0代入式 (11)，反復(fù)利用上式進(jìn)行迭代計(jì)算可以很快得到符合精度要求的位置解。

上述這種方法的流程圖如圖2所示。

圖2 算法流程

3 并聯(lián)機(jī)構(gòu)位置正解的MATLAB實(shí)現(xiàn)

3.1 MATLAB粒子群優(yōu)化算法工具箱簡(jiǎn)介

PSOt為PSO的工具箱，該工具箱將PSO算法的核心部分封裝起來(lái)，用戶只需要定義好自己需要優(yōu)化的函數(shù)，并設(shè)置好函數(shù)自變量的取值范圍、每步迭代允許的最大變化量等，即可自行優(yōu)化。

PSO算法工具箱中的核心函數(shù)是pso_Trelea_ vectorized()，該函數(shù)實(shí)現(xiàn)了整個(gè)粒子群的初始化、個(gè)體最優(yōu)和全局最優(yōu)的計(jì)算與更新、個(gè)體速度和位置的更新。在實(shí)際操作過(guò)程中，只需要用pso_Trelea_ vectorized()調(diào)用已經(jīng)編寫好的目標(biāo)函數(shù)，該函數(shù)會(huì)自動(dòng)實(shí)現(xiàn)粒子群優(yōu)化算法的尋優(yōu)。

3.2 位置正解的PSOt實(shí)現(xiàn)

通過(guò)上面的分析，以式 (8)為目標(biāo)函數(shù)。根據(jù)指標(biāo)給出約束條件:

3.3 試驗(yàn)結(jié)果比較

如圖3是使用粒子群優(yōu)化算法得到的誤差曲線，可以看出，執(zhí)行300次搜索以后誤差在0.001 6左右(當(dāng)然這個(gè)誤差不是一定的，幾乎每次搜索的誤差都不一樣，這個(gè)數(shù)據(jù)是筆者在做了很多次搜索以后得出的平均值)。對(duì)于精度要求不高的系統(tǒng)這個(gè)誤差是允許的，但是由于每次搜索的誤差都不一樣，并不能保證每次都能搜索到這么高的精度，所以對(duì)于精度要求比較高的系統(tǒng)，這樣做是不能滿足精度要求的。而且由于搜索時(shí)間較長(zhǎng)，對(duì)于實(shí)時(shí)性要求較高的系統(tǒng)而言，這個(gè)速度不一定能滿足要求。此外還可以看出在搜索初期粒子收斂的速度相當(dāng)快，通過(guò)大約50次搜索就可以使誤差限定在1以內(nèi);然而，再往后的搜索速度就明顯變慢了許多，在這種情況下如果還采用PSO算法，那么性價(jià)比就會(huì)比較低。

圖3 PSO算法求解的誤差曲線圖

圖4是采用粒子群優(yōu)化算法和 Newton迭代法相結(jié)合的方式得到的誤差曲線，可以看出這種算法具有很高的精度(0.001)，幾乎可以滿足所有的工程要求，并且速度較快。

圖4 PSO結(jié)合Newton迭代法求解的誤差曲線圖

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

重點(diǎn)探討了某實(shí)用五自由度搖擺臺(tái)的位置正解，采用粒子群優(yōu)化算法和Newton迭代法相結(jié)合的方法，該方法利用粒子群優(yōu)化算法的全局搜索能力和前期快速收斂的特點(diǎn)，在整個(gè)解空間里快速搜索，并且能夠很快搜索到目標(biāo)值附近。然而，越靠近目標(biāo)值，PSO算法的收斂速度就越低，且容易陷入局部最優(yōu)。所以作者在搜索后期直接摒棄了PSO算法，取而代之的是經(jīng)典的Newton迭代法，這種經(jīng)典的迭代法求解精度高，可以使最終的解達(dá)到理想的精度。然而，Newton迭代法也有計(jì)算量大的缺點(diǎn)，不過(guò)沒(méi)有關(guān)系，因?yàn)樵谒阉髑捌?，PSO算法已經(jīng)將搜索結(jié)果鎖定在目標(biāo)值附近很小的范圍內(nèi)，所以采用Newton迭代法只要再迭代很少的幾次就可以滿足精度要求了。試驗(yàn)表明:這種改進(jìn)有很高的精度和較好的實(shí)時(shí)性，對(duì)多數(shù)基于桿長(zhǎng)驅(qū)動(dòng)的搖擺臺(tái)的位置正解具有借鑒意義。

【1】馮志友，李永剛，張策，等.并聯(lián)機(jī)器人機(jī)構(gòu)運(yùn)動(dòng)與動(dòng)力分析研究現(xiàn)狀及展望［J］.中國(guó)機(jī)械工程，2006，17(9): 979-984.

【2】黃真，趙永生，趙鐵石.高等空間機(jī)構(gòu)學(xué)［M］.北京:高等教育出版社，2006.

【3】LEE T Y，SHIM J K.Improved Dialytic Elimination Algorithm for the Forward Kinematic of the General Stewart-Gough Platform［J］.Mech Theory，2003，3(8):562-577.

【4】CODOUREY Alain.Dynamic Modeling of Parallel Robots for Computed-Torque Control Implementation［J］.The International Journal of Robotics Research，1998，17(12): 1325-1336.

【5】芮鈞，陳守倫.Matlab粒子群算法工具箱求解水電站優(yōu)化調(diào)度問(wèn)題［J］.中國(guó)農(nóng)村水利水電，2009(1):114-116.