陳澤棟，盧明濤，閔躍軍，馬建明，丁祝順，王宏建

（北京航天控制儀器研究所，北京100039）

0 引言

六自由度并聯(lián)平臺采用Stewart結(jié)構(gòu)，相較于傳統(tǒng)的串聯(lián)平臺，其具有剛度大、精度高、運動空間廣、承載能力強、響應(yīng)速度快、誤差不累積等優(yōu)點，被廣泛地應(yīng)用于運動模擬、機械加工和精密定位等領(lǐng)域［1?2］。六自由度平臺位姿正解是設(shè)計和研究運動學(xué)、動力學(xué)和軌跡規(guī)劃的前提，也是后續(xù)實現(xiàn)高精度位姿控制的基礎(chǔ)。與串聯(lián)機構(gòu)相比，由于并聯(lián)機構(gòu)各個電動缸之間存在著強耦合關(guān)系，運動學(xué)正解需要求解一組含有六個未知數(shù)的非線性方程組，因此其運動學(xué)正解相對困難［3］。

國內(nèi)外學(xué)者采用了大量方法對Stewart平臺運動學(xué)正解進(jìn)行了研究。天津工業(yè)大學(xué)鐘有博［4］在Simulink軟件中利用Newton迭代的方法實現(xiàn)了運動學(xué)正解，但是由于每次迭代時都需要對方程組的Jacobian矩陣進(jìn)行求導(dǎo)，導(dǎo)致了算法的求解時間較長，難以用于實時系統(tǒng)。為了改進(jìn)這一問題，耿明超等［5］采用擬Newton法來進(jìn)行求解，利用當(dāng)前函數(shù)值代替Jacobian矩陣，從而避免了矩陣運算，降低了計算量。但是無論采用何種方式進(jìn)行迭代，都需要人為的提供一個迭代初值給算法，迭代初值會一定程度的影響求解的精度和速度。為了解決這一問題，陳莉等［6］、弓瑞等［7］采用智能計算的思想，將粒子群算法和遺傳算法應(yīng)用在六自由度并聯(lián)機器人的正解問題上，利用粒子群算法的全局搜索能力來進(jìn)行正解，在6?SGP機構(gòu)上驗證了該算法的可行性。但是，粒子群算法和遺傳算法在后期會出現(xiàn)收斂速度慢的現(xiàn)象，偶爾還會收斂到局部最優(yōu)點，難以應(yīng)用在工程實際中。

為了解決智能算法后期搜索效率降低和Newton法對初始點敏感的缺陷，本文提出一種基于人群搜索算法（Seeker Optimization Algorithm，SOA）的Newton迭代混合算法用于六自由度平臺運動學(xué)正解，算法的實現(xiàn)原理如圖1所示。已知六個電動缸長度后，利用人群搜索算法進(jìn)行運動學(xué)正解，將得到的位姿作為Newton迭代的初始值，然后利用Newton迭代算法進(jìn)行進(jìn)一步的求解，從而得到更加精確的位姿。最后，利用運動學(xué)反解求得該位姿對應(yīng)下的電動缸長度，與已知的電動缸長度進(jìn)行對比驗證。

圖1 SOA-Newton算法正解原理圖Fig.1 Schematic diagram of SOA-Newton hybrid algorithm

1 運動學(xué)正解數(shù)學(xué)模型

六自由度位姿平臺基于Stewart機構(gòu)，如圖2所示，主要由上平臺、下平臺以及連接上下平臺的6個電動缸構(gòu)成。電動缸與上下平臺之間通過虎克鉸連接，運動平臺工作時，上位機通過控制六個電動缸的長度來改變上平臺姿態(tài)，進(jìn)而實現(xiàn)空間六個自由度的運動。為了便于建模，對位姿平臺進(jìn)行結(jié)構(gòu)簡化，在上平臺建立動坐標(biāo)系Ob?XbYbZb，在下平臺建立靜坐標(biāo)系 Oa?XaYaZa。設(shè)電動缸與上平臺的鉸點坐標(biāo)為 Bi（i＝1，2，…，6），與下平臺的鉸點坐標(biāo)為 Ai（i＝1，2，…，6），鉸點為120°對稱分布，上平臺外接圓半徑為Ra，鉸點最短距離為ha，下平臺外接圓半徑為Rb，鉸點最短距離為hb。

圖2 六自由度運動平臺結(jié)構(gòu)簡圖Fig.2 Schematic diagram of 6-DOF motion platform

上平臺在運動過程中可以用六個變量來表示α、β、γ、x、y、z的姿態(tài)。其中，α、β、γ為繞 X軸、Y軸、Z軸分別旋轉(zhuǎn)的橫滾角、俯仰角、偏航角，旋轉(zhuǎn)的方向遵循右手定則；x、y、z表示沿 X軸、Y軸、Z軸的平移量，根據(jù)空間坐標(biāo)旋轉(zhuǎn)規(guī)則可以確定動坐標(biāo)系到靜坐標(biāo)系變換的旋轉(zhuǎn)矩陣R為

確定旋轉(zhuǎn)矩陣和平移矩陣后，上鉸點Bi坐標(biāo)由動坐標(biāo)系變換到靜坐標(biāo)系的坐標(biāo)變換公式為

六自由度位姿平臺正解問題是已知電動缸上下鉸點之間的長度去求解上平臺位姿。設(shè)未知變量 x＝［x1x2x3x4x5x6］T＝［α β γ x y z］T，已知變量為給定桿長L＝［L1L2L3L4L5L6］T，實際桿長與初始桿長的桿長差計算公式為

根據(jù)物理意義可知，函數(shù)fi（x）的最小值為零，因此可構(gòu)建運動學(xué)正解的優(yōu)化模型

模型的約束條件為上平臺允許的最大運動空間。

2 SOA?Newton算法的基本思想

SOA?Newton混合算法的基本流程為：首先進(jìn)行種群初始化，設(shè)置種群規(guī)模、最大迭代次數(shù)、邊界條件等；接著計算個體適應(yīng)度，尋找個體最優(yōu)和全局最優(yōu)；確定搜索的方向和步長，進(jìn)行種群位置更新，當(dāng)進(jìn)化到一定代數(shù)后，將SOA算法的結(jié)果作為初始迭代位姿進(jìn)行Newton迭代；設(shè)置Newton迭代求解精度和最大迭代次數(shù)進(jìn)行Newton迭代，滿足輸出條件后迭代的值即為平臺的位姿正解?；旌纤惴ɡ肧OA算法的全局搜索能力對Newton迭代初值進(jìn)行補償，其算法的基本思想及實現(xiàn)流程如圖3所示。

圖3 SOA-Newton混合迭代算法流程圖Fig.3 Flow chart of SOA-Newton algorithm

2.1 SOA算法基本原理

SOA算法模擬人的隨機搜索行為，即在連續(xù)的搜索過程中，當(dāng)搜尋者所處位置較優(yōu)時，應(yīng)該在較小鄰域內(nèi)進(jìn)行搜索；當(dāng)搜尋者位置較差時，應(yīng)該在較大的鄰域內(nèi)搜索。SOA算法將這種策略應(yīng)用于解決優(yōu)化模型最優(yōu)解的問題，其搜索方向和步長更新規(guī)則如下［8］：

（1）步長更新

SOA搜索算法的步長更新規(guī)則為：如果適應(yīng)度函數(shù)值小，表明結(jié)果靠近最優(yōu)點，則搜索步長也?。蝗绻m應(yīng)度函數(shù)值較大，則表明位置不理想，應(yīng)采用較大步長跳出當(dāng)前位置［9］。搜索步長變量采用Guass函數(shù)來描述

式（5）中，uA為Guass隸屬度，x為輸入變量，u、σ為隸屬度函數(shù)參數(shù)。當(dāng)輸入變量超出3σ時可以忽略，故設(shè)定最小隸屬度為0.0111，最大隸屬度為1。為了使目標(biāo)函數(shù)值的排列順序成正比，采用線性隸屬函數(shù)

式（6）中，ui為目標(biāo)函數(shù)值i的隸屬度，Ii為種群降序排列后的序列編號。

根據(jù)不確定推理隸屬度函數(shù)，可以確定步長為

式（7）中，αij為j維空間中的搜索步長，ω為慣性權(quán)值。為了提高SOA算法的全局搜索能力，采用非線性動態(tài)慣性權(quán)重。

（2）方向更新

搜索方向需要綜合利己行為、利他行為和預(yù)動行為來確定，更新規(guī)則如下

（3）位置更新

當(dāng)搜索步長和方向確定后，位置可根據(jù)式（10）確定

2.2 Newton-Raphson迭代算法原理

Newton?Raphson迭代法是一種用來求解復(fù)雜多元非線性方程組f（x）＝0的簡單數(shù)值解法，只要初值選的合理，經(jīng)過一定的迭代，總會達(dá)到收斂精度［10］。下面給出 Newton?Raphson 迭代在六自由度正解中應(yīng)用的方法，設(shè)定方程的初始解為x0，在x0處對方程組f（x）作一階Taylor展開

式（11）中，σ（x）為高階無窮小量，在此可以忽略，可得

代入六自由度平臺的桿長計算公式并展開可得六自由度運動平臺的正解迭代模型，如式（13）所示?？梢钥闯觯掀脚_位置和姿態(tài)的變化量與桿長的伸縮量存在著對應(yīng)關(guān)系。

對于六自由度并聯(lián)平臺來說，其初值x0＝［x1x2x3x4x5x6］T＝［α β γ x y z］T為上平臺的姿態(tài)和位置信息，ΔL為電動缸長度的變化值，ΔX為位置和姿態(tài)的變化值。在Simulink軟件中建立了利用Newton迭代法求解六自由度平臺正解的仿真模型，如圖4所示，inverse motion模塊中電動缸長度計算模型如圖5所示。模型的終止條件為：達(dá)到最大迭代次數(shù)N或最大誤差max（Δx）＝xk-xk-1＜ ε，此時求解的結(jié)果即為上平臺位姿。

圖4 Newton-Raphson迭代法Simulink仿真模型Fig.4 Simulink model of Newton-Raphson iteration method

圖5 電動缸長度計算模塊Fig.5 Calculation module of electric cylinder length

3 SOA?Newton算法的正解實例

3.1 六自由度平臺的參數(shù)設(shè)定

以實驗室研制的車載位姿平臺為例，如圖6所示，該平臺能夠根據(jù)外界反饋到的車體位姿調(diào)整上平臺姿態(tài)，使其保證在水平狀態(tài)。對該系統(tǒng)進(jìn)行簡化，得到其上平臺半徑為Ra＝0.4m，下平臺半徑為Rb＝0.54m，上平臺相鄰連點間的最短距離為ha＝0.1m，下平臺相鄰連點間的最短距離為hb＝0.2m，平臺處于工作零位時電動缸上下鉸點之間距離為0.72m。系統(tǒng)選用滾珠絲杠型電動缸，最大行程為150mm，能夠?qū)崿F(xiàn)上平臺三個線位移±0.2m、三個角位移±15°的運動。

圖6 車載位姿平臺Fig.6 Vehicle attitude platform

3.2 基于SOA迭代算法正解仿真

首先，使用SOA算法對六自由度平臺進(jìn)行位置正解，設(shè)定種群規(guī)模為150，最大進(jìn)化代數(shù)為300，最大隸屬度值為 0.95，最小隸屬度值為0.0111，權(quán)重最大值為0.9，最小值為0.1。由于該平臺的最大位移為±0.2m，因此將粒子群算法中的粒子空間約束在±15°內(nèi)，留有一定的余量。接著，均勻選取上平臺由初始點（0m，0m，0m，0°，0°，0°）運動到 （0.2m，0.2m，0.2m，15°，15°，15°）過程中的4組位姿，將這4組姿態(tài)進(jìn)行運動學(xué)反解，得到對應(yīng)的4組桿長。

利用SOA算法對這4組桿長進(jìn)行運動學(xué)正解，求取上平臺位置和姿態(tài)，計算結(jié)果如表1所示?？梢钥闯?，空間位置的最大誤差在4mm以內(nèi)，空間姿態(tài)的最大誤差在0.3°以內(nèi)，與位姿平臺精度要求還有一定的距離。一般情況下，可以通過改進(jìn)粒子群算法或者提高種群數(shù)量和迭代次數(shù)來提高求解精度，但是六自由度正解模型具有高度的耦合性、非線性，以上方法對精度的提高效果有限。以第4組桿長正解為例，SOA正解過程中適應(yīng)度曲線變化如圖7所示。由適應(yīng)度變化曲線可以看出，在進(jìn)化70代之后，函數(shù)的適應(yīng)度值變化并不明顯，這表明此時求得的值已經(jīng)很接近真實值，繼續(xù)進(jìn)行迭代只會導(dǎo)致計算時間大幅延長，對于提高求解精度意義不大。

表1 四組桿長下對應(yīng)的SOA算法正解結(jié)果Table 1 Calculation results of SOA algorithm corresponding to four rod lengths

求解過程中，姿態(tài)和位置的最大誤差變化曲線如圖8所示。由變化曲線可以看出，在進(jìn)化的前1/3過程中，位置誤差和姿態(tài)誤差都曾下降到一個比較小的值，表明種群已經(jīng)搜索到一個比較接近真實值的解。而且整個進(jìn)化過程中三個位置方向的最大誤差基本能保證在0.06m以內(nèi)，最小值能夠達(dá)到0.002m以下；姿態(tài)誤差基本能夠保證在2.5°以內(nèi)，最小的誤差值在0.05°左右，仍有提高的空間。

圖7 求解過程中的適應(yīng)度變化曲線Fig.7 Fitness curve during the solution process

圖8 迭代過程中的最大誤差變化曲線Fig.8 Maximum error curves during the solution process

為了提高結(jié)果的精度，需要利用 Newton?Raphson算法進(jìn)行進(jìn)一步求解，設(shè)置 Newton?Raphson算法的迭代精度為1×10-6m，最大迭代次數(shù)為15次，將表1中SOA算法的計算結(jié)果作為迭代初值進(jìn)行Newton迭代求解。同時，為了研究不同迭代初值對計算精度和速度的影響，對比了初值為平臺工作初始位置時Newton迭代算法的計算結(jié)果，如表2所示。可以看出，在達(dá)到同等的求解精度情況下，使用SOA算法迭代的結(jié)果作為初始值進(jìn)行Newton正解，能夠明顯減小迭代次數(shù)，從而縮短計算時間，為位姿平臺后續(xù)的實時控制打下基礎(chǔ)。

表2 不同初值下Newton算法迭代次數(shù)Table 2 Number of iterations under different initial values

3.3 SOA-Newton算法正解的實際驗證

為了驗證該算法在實際使用過程中的計算精度和速度，以車載位姿調(diào)節(jié)平臺為對象進(jìn)行混合算法正解實驗。設(shè)定上平臺的運動軌跡使其在工作空間內(nèi)繞X軸（橫滾）和Z軸（偏航）方向作一個復(fù)合的正弦運動，運動幅值為2°，頻率為 0.5Hz，初始相位差為180°，運動軌跡如圖9所示。為了兼顧求解的效率和精度，設(shè)置SOA算法的最大迭代次數(shù)為30次，Newton迭代算法的最大迭代次數(shù)為8次，桿長迭代精度為1×10-6m。實驗結(jié)果如圖10所示，可以看出由于六自由度平臺六個自由度之間存在著耦合關(guān)系，算法正解的精度在上平臺運動到最大位姿時會出現(xiàn)較大的波動，存在一定的周期性，這表明計算精度與上平臺所處位姿存在一定的聯(lián)系。除此之外，整個求解過程中姿態(tài)誤差基本保持在0.0005°以內(nèi)，位置精度基本在0.01mm以內(nèi)，而且每次求解都保證在10ms以內(nèi)，驗證了該算法應(yīng)用在六自由度并聯(lián)位姿平臺正解上的求解精度和求解效率，能夠滿足工程實際使用。

圖10 正解結(jié)果與理論值的誤差曲線Fig.10 Error curves of the forward solution value and the theoretical value

4 結(jié)論

本文首先對六自由度并聯(lián)位姿平臺的運動學(xué)進(jìn)行了分析，建立了六自由度平臺正解數(shù)學(xué)優(yōu)化模型。針對Newton?Raphson算法求解精度受迭代初值影響大的問題，提出了一種基于SOA算法的Newton迭代混合算法，并在Simulink中搭建了相應(yīng)的仿真模型。接著均勻選取了上平臺由平臺工作初始位置運動到極限位置過程中的4組姿態(tài)下對應(yīng)的4組缸長，對其進(jìn)行運動學(xué)正解，驗證了混合算法的求解精度和速度比單純使用Newton迭代算法和SOA算法要好。最后以實驗室研制的六自由度位姿平臺為例，對其進(jìn)行實時正解實驗，計算的結(jié)果表明，該算法在滿足精度要求的同時能夠兼顧求解速度，可應(yīng)用于六自由度并聯(lián)平臺的測試開發(fā)和實時控制。