基于SOA?Newton迭代的六自由度平臺正解算法

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

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

0 引言

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

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

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

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

1 運動學正解數學模型

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

圖2 六自由度運動平臺結構簡圖Fig.2 Schematic diagram of 6-DOF motion platform

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

確定旋轉矩陣和平移矩陣后，上鉸點Bi坐標由動坐標系變換到靜坐標系的坐標變換公式為

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

根據物理意義可知，函數fi（x）的最小值為零，因此可構建運動學正解的優化模型

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

2 SOA?Newton算法的基本思想

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

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

2.1 SOA算法基本原理

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

（1）步長更新

SOA搜索算法的步長更新規則為：如果適應度函數值小，表明結果靠近最優點，則搜索步長也??；如果適應度函數值較大，則表明位置不理想，應采用較大步長跳出當前位置［9］。搜索步長變量采用Guass函數來描述

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

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

根據不確定推理隸屬度函數，可以確定步長為

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

（2）方向更新

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

（3）位置更新

當搜索步長和方向確定后，位置可根據式（10）確定

2.2 Newton-Raphson迭代算法原理

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

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

代入六自由度平臺的桿長計算公式并展開可得六自由度運動平臺的正解迭代模型，如式（13）所示。可以看出，上平臺位置和姿態的變化量與桿長的伸縮量存在著對應關系。

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

圖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 六自由度平臺的參數設定

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

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

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

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

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

表1 四組桿長下對應的SOA算法正解結果Table 1 Calculation results of SOA algorithm corresponding to four rod lengths

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

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

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

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

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

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

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

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

4 結論

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