五自由度搖擺臺位置正解方法研究及MATLAB實現

李輝，宋詩

(1.安徽建筑工業學院機電學院，安徽合肥230601;2.中國科技大學信息科學技術學院，安徽合肥230088)

搖擺臺實際上是一種并聯機構，與串聯機構相比具有剛度大、結構穩定、承載能力強、精度高、運動慣性較小、運動學位置反解易求和便于實時控制等優點［1］，具有廣闊的應用前景。因此，國內外許多學者開始研究并聯機構，從最初的六自由度并聯機構到后來的少自由度并聯機構，在理論和應用方面都取得了豐富的成果。

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

在工程應用中，如何快速的找到合理的解遠比找到全部的解有意義，并且文獻 ［3-4］指出由桿長驅動的并聯機構在其工作空間內僅存在單解。基于此，作者著重討論數值解法，提出一種粒子群優化算法和Newton迭代法相結合的數值解法，并簡單介紹了如何在MATLAB中實現該方法。

1 基于粒子群優化算法的五自由度并聯機構位置正解

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

式中:ω為慣性權重，c1、c2為學習因子，r1、r2為區間［0，1］上的隨機數，pbi為第i個粒子的個體極值，gb為全局極值。此外，粒子的運動還受到最大速度的限制，當vi＞vmax時，取vi=vmax。

文中所討論的并聯機構上、下平臺以5個分支相連，每個分支中間為移動副，與上平面用球鉸相連，與下平面中心點以轉動副相連，其余用球鉸相連。機構示意圖如圖1所示。

圖1 五自由度搖擺臺機構示意圖

由位置反解的分析可知，并聯機構每支桿的長度li(i=0，1，…，4)由動坐標系相對于定坐標系的3個獨立轉角θx，θy，θz以及動坐標系原點A0在定坐標系中的坐標P(xA0，yA0，zA0)決定，寫成函數表達式:

因為定坐標系的原點處為轉動副，所以RPS分支只能在zA0y平面內運動，即xA0≡0，故不予考慮。將式(3)改寫成以下形式:

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

令:

則有:

向量表示:

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

求解方程 (7)的問題可以轉化為等價的函數優化問題

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

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

步驟一，設置種群規模N，在并聯機構可達空間內隨機初始化各個粒子的位置xi0和速度vi0，并確定最大進化代數Q和與全局極值相匹配的粒子占全部粒子的比例P。

步驟二，根據適應度函數計算每個粒子的適應度。

步驟三，由適應度的大小確定個體最優位置pbi及全局最優位置gb。

步驟四，判斷全局最優是否滿足精度要求，如果不滿足精度要求轉到步驟第三;反之，以當前的全局最優位置gb為要尋找的位姿輸出。

步驟五，看是否到達最大進化代數Q，如果到達最大進化代數，則以當前的全局最優位置gb為要尋找的合適初始位姿輸出。否則，繼續步驟六。

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

2 并聯機構位置正解方法的改進

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

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

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

通過大量的仿真試驗，發現當種群中有一半以上的粒子與當前全局最優粒子匹配的時候，可以認為PSO算法進入到后期搜索，此時以當前全局最優解為Newton迭代法的初始值。

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

即:

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

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

圖2 算法流程

3 并聯機構位置正解的MATLAB實現

3.1 MATLAB粒子群優化算法工具箱簡介

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

PSO算法工具箱中的核心函數是pso_Trelea_ vectorized()，該函數實現了整個粒子群的初始化、個體最優和全局最優的計算與更新、個體速度和位置的更新。在實際操作過程中，只需要用pso_Trelea_ vectorized()調用已經編寫好的目標函數，該函數會自動實現粒子群優化算法的尋優。

3.2 位置正解的PSOt實現

通過上面的分析，以式 (8)為目標函數。根據指標給出約束條件:

3.3 試驗結果比較

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

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

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

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

4 結束語

重點探討了某實用五自由度搖擺臺的位置正解，采用粒子群優化算法和Newton迭代法相結合的方法，該方法利用粒子群優化算法的全局搜索能力和前期快速收斂的特點，在整個解空間里快速搜索，并且能夠很快搜索到目標值附近。然而，越靠近目標值，PSO算法的收斂速度就越低，且容易陷入局部最優。所以作者在搜索后期直接摒棄了PSO算法，取而代之的是經典的Newton迭代法，這種經典的迭代法求解精度高，可以使最終的解達到理想的精度。然而，Newton迭代法也有計算量大的缺點，不過沒有關系，因為在搜索前期，PSO算法已經將搜索結果鎖定在目標值附近很小的范圍內，所以采用Newton迭代法只要再迭代很少的幾次就可以滿足精度要求了。試驗表明:這種改進有很高的精度和較好的實時性，對多數基于桿長驅動的搖擺臺的位置正解具有借鑒意義。

【1】馮志友，李永剛，張策，等.并聯機器人機構運動與動力分析研究現狀及展望［J］.中國機械工程，2006，17(9): 979-984.

【2】黃真，趙永生，趙鐵石.高等空間機構學［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粒子群算法工具箱求解水電站優化調度問題［J］.中國農村水利水電，2009(1):114-116.