基于SOA?Newton迭代的六自由度平臺(tái)正解算法

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

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

0 引言

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

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

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

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

1 運(yùn)動(dòng)學(xué)正解數(shù)學(xué)模型

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

圖2 六自由度運(yùn)動(dòng)平臺(tái)結(jié)構(gòu)簡圖Fig.2 Schematic diagram of 6-DOF motion platform

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

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

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

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

模型的約束條件為上平臺(tái)允許的最大運(yùn)動(dòng)空間。

2 SOA?Newton算法的基本思想

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

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

2.1 SOA算法基本原理

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

（1）步長更新

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

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

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

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

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

（2）方向更新

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

（3）位置更新

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

2.2 Newton-Raphson迭代算法原理

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

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

代入六自由度平臺(tái)的桿長計(jì)算公式并展開可得六自由度運(yùn)動(dòng)平臺(tái)的正解迭代模型，如式（13）所示。可以看出，上平臺(tái)位置和姿態(tài)的變化量與桿長的伸縮量存在著對應(yīng)關(guān)系。

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

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

圖5 電動(dòng)缸長度計(jì)算模塊Fig.5 Calculation module of electric cylinder length

3 SOA?Newton算法的正解實(shí)例

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

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

圖6 車載位姿平臺(tái)Fig.6 Vehicle attitude platform

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

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

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

表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)誤差都曾下降到一個(gè)比較小的值，表明種群已經(jīng)搜索到一個(gè)比較接近真實(shí)值的解。而且整個(gè)進(jìn)化過程中三個(gè)位置方向的最大誤差基本能保證在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算法的計(jì)算結(jié)果作為迭代初值進(jìn)行Newton迭代求解。同時(shí)，為了研究不同迭代初值對計(jì)算精度和速度的影響，對比了初值為平臺(tái)工作初始位置時(shí)Newton迭代算法的計(jì)算結(jié)果，如表2所示?？梢钥闯?，在達(dá)到同等的求解精度情況下，使用SOA算法迭代的結(jié)果作為初始值進(jìn)行Newton正解，能夠明顯減小迭代次數(shù)，從而縮短計(jì)算時(shí)間，為位姿平臺(tái)后續(xù)的實(shí)時(shí)控制打下基礎(chǔ)。

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

3.3 SOA-Newton算法正解的實(shí)際驗(yàn)證

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

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

4 結(jié)論

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