Stewart 型六自由度平臺正反解研究

中國科技論文在線

Stewart型六自由度平臺正反解研究

蔡保富，廖傳書

武漢理工大學 電信系，武漢（430070）

E-mail: cbf65627972@

摘 要：本文研究了Stewart型六自由度平臺的正解和反解。根據Stewart型六自由度平臺

的結構的特點，為了達到高精度的實時控制，設計出具有算法簡單、效率高，并具有易于編

程的正反解算法。本算法已經在MATLAB下仿真模擬過，并嵌入到實際平臺上VxWorks

系統下實際實現。本文介紹這一算法的實現思想。

關鍵詞：Stewart型六自由度平臺；正解；反解；實時控制；VxWorks

1引 言

六自由度運動平臺是一種重要的仿真實驗設備，其應用范圍非常廣泛。因其結構簡單、

高剛度、高精度和高負載能力等優點，六自由度運動平臺已成為飛機、艦船、宇航和車載設

備進行動態可靠性研究的重要模擬試驗裝置。這種系統普遍采用Stewart平臺及其變形機構，

平臺有上下兩個平臺和6個并聯的、可獨立自由伸縮的杠桿組成，伸縮杠和平臺之間通過球

鉸鏈聯接，通過改變伸縮杠的長度可以實現上動臺面的空間多自由度運行

[1]

。

對于并聯機構的六自由度平臺在運動過程中，要保證運動的實時性和正確性，就需要通

過對伸縮杠的精確控制來實現，這就需要引入六自由度平臺的實時位置正反解算法。所謂六

自由度平臺的位置反解，是指由運動平臺的空間姿態求六個伸縮杠的伸縮量。而六自由度的

位置正解，是指有六個伸縮杠的伸縮量來求運動平臺的空間姿態

[2]

本課題就是通過對Stewart型六自由度運動平臺的研究，建立一種具有運動學正反解的

數學模型，通過計算機程序實現該數學模型，仿真并實際運行。

2運動平臺空間位置分析

對于一種機構的運動分析包括位置分析、速度分析和加速度分析三部分，位置分析是運

動學分析最基本的任務。機構的位置分析是求解機構的輸出和輸入構件之間的位置關系。對

于Stewart型六自由度平臺就是六個輸入杠的長度和作為輸出的運動平臺的姿態和位置之間

的關系

[3]

。

位置解有封閉解法和數值解法兩種。封閉解法通常是采用多種方法從約束方程組中消去

未知數，以得到單參數的多項式后再求解。封閉解法有許多的方法，包括矢量代數法、幾何

法、矩陣法、螺旋代數法等等。其優點是在得到解析表達式后理論上還有很多應用，而且能

夠得到全部解，其缺點是難度很大，沒有通用性。而數值法通常采用跌代法、連續法等等方

式求方程組的代數值。優點是可以比較迅速方便的得到機構的位置解，缺點是一般得不到全

部的解，不適合做理論上的研究

[4]

。

本課題中六自由度運動平臺的反解是通過封閉解法得到的，而正解采用的數值解法。

3六自由度運動平臺反解的研究

如圖1，分別為空間的3個方向，對于與圖中的和，在驅動器

x,y,z

X',Y',Z'X,Y,Z

的作用下改變伸縮杠的長度，使上平臺的位置和狀態發生變化。若給定了上平臺的空間位置

資助來源：中國科學院聲學所研究基金

-1-

中國科技論文在線

的位置反解

[5][6]

。

和姿態，分別為方向上的轉角值和平移量，求各個杠長，即各缸的位移，這就是平臺

x,y,z

圖 1 Stewart型六自由度平臺

在上平臺建立一個坐標系中初始狀態下，動坐標系

X'Y'Z'

建立在上平臺上的中心位

置，這樣上下臺面的12個點都對應一個初始坐標值。當上臺面改變時可以根據平面與平

[7]

面上點的關系求出此時新點的坐標值。

P

P=T*P0+P'

（1）

其中為上臺面姿態的方向余旋， （2）

T

T=T1*T2*T3

001cos0sincos?sin0

??????

ββγγ

??????

T1=0cos?sin,T2=010,T3=sincos0

??????

ααγγ

（3）

??????

0sincos?sin0cos001

ααββ

??????

αβγ

,,T1,T2,T3

分別為上平臺在方向上的轉角，得到的分別是在坐標系下的3

x,y,z

個方向余旋。為初始點的坐標值，為上平臺在方向上的3個位移量。這樣有空

P0

P'

x,y,z

間點的幾何關系可以得到平臺的位置反解計算方程

6

∑

L=(P?B)+(P?B)+(P?B)

iixixiyiyiziz

i=1

222

（4）

其中

L

i

為6個缸長值，為上臺面的6個動點的坐標值，為下臺面

P,P,PB,B,B

ixiyizixiyiz

6個靜點的初始坐標值。

這樣就可以得到6個獨立的顯式方程，當已知機構的基本尺寸和上臺面的位置和姿態

后，就可以利用上式來求出6個缸的位移了。達到反解出杠長的目的。

4六自由度正解的研究

由于并聯機構結構的復雜性，六自由度運動平臺的位置正解難度比較大。普通的解析法

雖然適合做理論上的研究，但是因實現難度太大，不適合工程上的應用，因而這里我們采用

數值解法來求解，從而求得缸長對應的上平臺的位置和姿態，即X，Y，Z方向的轉角值和

平移量

[7]

。

在這里我選擇在數值解法中比較適用的牛頓跌代法來求解。

4.1牛頓跌代法

-2-

中國科技論文在線

跌代法是一種重要的逐次逼近的求根方法，它是利用某個固定的公式，把選定的方程的

根的初值反復代入，以校正根的近視值，使之逐步精確化，最后得到滿足精度要求的解。

牛頓跌代法是一種常用的跌代方法，其基本思想是把非線性方程

f(x)0

=

轉化為某個

線性方程求解。

則函數在附近可以用一階泰勒多項的一個近似根為，

f(x)f(x)0

xx

00

設已知方程

=

式表示并化簡的

f(x0)f'(x0)(xx0)0

+?=

（5）

可以得到跌代公式

x=x?fxfx

nnnn

+1

()/'()

（6）

由導數的定義我們可以得

x=x?fxfx+??fx?

nnnnn

+1

()[(()())]

（7）

跌代是個無限的過程，而實際計算過程中只能跌代有限次，為了節省計算時間，就要求

跌代的收斂速度快。牛頓跌代法的收斂速度較快。在實際的跌代中，只要前后兩次跌代之差

的絕對值不超過允許的誤差

ε

，即滿足

||

x?x<

nn

?1

ε

（8）

即的真實根的誤差不會超過，我們就取為方程的根。

xx

nn

ε

4.2正解方程

正解方程可以由反解方程變化而來。只是由于正解方程用解析式表示起來過于麻煩，

所以這里我們通過向量或矩陣的方式來表示，那樣無論是理解還是實現起來都要容易一些。

由式子（1），（2），（3）得

P=TP+P

*0'

（9）

L=TP+P?BTP+P?B

2

(*0')*(*0')

T

（10）

上式中為初始點坐標的一個3*1的行列式，為包含3個角度變量的3*3余旋矩陣，

P

0

T

P'P

為包含3個位移變量的3*1的行列式，為新的空間坐標點的一個3*1的行列式B為下

臺面相應基點的坐標，L為上臺面點離對應基點的距離（即缸長）

令

∑

F(x,y,z,l,m,n)(T*P0P'B)*(T*P0P'B)L

=+?+??

iiiiiiiiii

i

=0

5

T

2

（11）

解非線性方程組（10）就可以得到姿態

(x,y,z,l,m,n)

。

4.3正解方程的求解方法和步驟

首先令初始點，其中為上運動平臺在中位時Z

(,,,,,)(0,0,,0,0,0)

xyzlmn=zz

midmid

向的升高值。

將

FPP

iii

()

00

(i1,2...,6)

=

在附近進行泰勒展開，并取其線性部分得：

F(P)+(q?q)=0

iikk

00

∑

k=1

6

?F(P)

ii0

（12）

(i1,2...,6)

=

?q

k

式（11）可以看成以為未知數的線性方程組，其雅可比系數矩陣為：

P

i

-3-

中國科技論文在線

??

?F/?q?F/?q

1116

K

??

MOM

J=

??

（13）

??

?F/?q?F/?q

L

??

6166

通過初等行變換求雅可比矩陣的逆矩陣，這樣就完全轉化為求矩陣的乘法和加減運算

了。

其中

(q,q,q,q,q,q)=(x,y,z,l,m,n)

123456

（14）

當

6

∑

(P?P)<

ii

0

i=1

6

ε

（為所需要的精度），則可以把當作所求的正解。否則把

ε

∑

P

i

i

=

1

6

∑

P

當作初始點，重復（10）~（13）直到滿足求解精度為止。因為六自由度運動平臺一

i

i

=

1

般在中位附近運動，在算法上把正解的初始值設為運動平臺的中位，保證了算法的收斂。在

實際過程中還可以把上次的平臺狀態記住，當作此次運算的初始值，這樣跌代的次數還會進

一步減少。

5計算實例

算法實現后，我們把它加入到控制六自由度平臺運動的下位機控制程序中。下位機是實

時操作系統Vxworks構成的，這樣也保證了六自由度運動平臺的實時和精確性。

5.1反解實例

當六自由度運動平臺的姿態為x向轉角0度，平移0mm；y向轉角0度，平移0mm；z

向轉角0度，平移0mm時，此時對應的6缸的長度都是1039.576mm。這種情況就是六自

由度運動平臺的初始狀態。

下面是試驗中獲得的幾組數據：

表1 六自由度反解實驗數據

1 2 3 4

0 1 10 3

0 5 5 4

0 8 -5 -5

0 -10 20 20

0 -10 50 50

0 100 100 50

1039.576 1119.345 1066.379 1040.350

1039.576 1047.186 1080.882 1048.098

1039.576 1208.643 1197.602 1081.208

1039.576 1125.544 1276.527 1177.008

1039.576 1183.212 1058.081 1058.083

1039.576 1094.014 1097.886 1106.593

組別

x轉角( )

o

y轉角( )

o

z轉角( )

o

x平移(mm)

y平移(mm)

z平移(mm)

1號缸長(mm)

2號缸長(mm)

3號缸長(mm)

4號缸長(mm)

5號缸長(mm)

6號缸長(mm)

5.2正解實例

因為正解的跌代算法運行起來比較的耗時，如果把跌代精度設的過高，那樣跌代的次數

也會增加，這對要求實時控制的六自由度運動平臺肯定是不合適的，所以我們把精度設置為

-4-

中國科技論文在線

，

10（我們是為了表現其差別才把精度設高點，實際的精度還可以進一步降低點），并在算

-5

法上面也做了優化處理，這樣運行時間會顯著降低。

下面是試驗中測得的幾組數據

組別

1號缸長(mm)

2號缸長(mm)

3號缸長(mm)

4號缸長(mm)

5號缸長(mm)

6號缸長(mm)

x轉角( )

o

y轉角( )

o

z轉角( )

o

x平移(mm)

y平移(mm)

z平移(mm)

表2 六自由度正解實驗數據

1 2 3 4

1039.576 1153.257 1066.379 1040.350

1039.576 1102.212 1080.882 1048.098

1039.576 1153.257 1197.602 1081.208

1039.576 1102.213 1276.527 1177.008

1039.576 1153.258 1058.081 1058.083

1039.576 1102.212 1097.886 1106.593

0.00000 0.00000 10.00002 3.00001

0.00000 0.00001 4.99985 3.99986

0.00000 4.99998 -5.00008 5.00007

0.00024 0.00024 19.99895 19.99859

0.00000 0.00001 49.99898 49.99855

0.00009 99.99982 199.99964 49.99954

試驗所用的系統配置為CPU1.61G，內存512M，運行所花時間最少為0.08ms（只跌代

一次就得到滿足精度的值），最慢需要0.36ms（跌代5次），實際過程中我們選定上一次

紀錄的姿態作為正解初始值，這樣基本上只需要1次就可以跌代出結果來，運算時間上可以

控制在0.1ms內了，完全可以滿足工業上面所要求的控制要求了。

6結論

本文對Stewart型六自由度平臺的正反解算法進行了研究。對該類并聯機構建立數學模

型，通過封閉解法和數值解法分別對六自由度運動平臺的姿態反解和正解進行了分析求解。

并通過把算法嵌入到VxWorks操作系統中去，來實際控制運動平臺的多自由度度運動。結

果表明該算法能夠很好的實現六自由度運動平臺的正反解功能，因其具有很高的精度和較快

的運算速度，能夠在自動控制中得到實際應用。

參考文獻

[1] 賀利樂，六自由度并聯桿系機床位置正解的研究 [J]， 機床與液壓，..112。

[2] Liu Dejun, Huang Qingcheng, Che Rensheng et al. A measuring model study of a new coordinate - measuring

machine bad on the parallel kinematic mechanism[J] .Measurement Science and Technology ,1999.10 (11) :1020

– 1024 .

[3] Ilian A. Bonev , Jeha Ryu , A new method for solving the direct kinematics of general 6 - 6 Stewart Platforms

using three linear extra nsor [J] , Mechanism and Machine Theory. 35 (2000) 423～436 .

[4] Tae - Young Lee and Jae - Kyung Shim1Algebraic Elimination -Bad Real - Time Forward Kinematics of the

6 - 6 Stewart Platform with Planar Ba and proc. IEEE Int. Conf . t. 2001. pp. 1301～

1306 .

[5] 李維嘉，六自由度并聯運動機構正向解的研究 [J]，華中理工大學學報,1997.25 (4) : 38～401。

[6] 張尚盈.趙 慧.韓俊偉.六自由度運動平臺實時控制的正_反解算法 [J] .哈爾濱工業大學.2003。

[7] 黃真，趙永生，趙鐵石，高等空間結構學 [M] .高等教育出版，2005.10。

[8] 陳智育，溫彥軍，陳琪， VxWorks程序開發實踐 [M] ，人民郵電出版，2004.3。

[9] 薛定宇，陳陽泉，基于MATLAB/Simullink的系統仿真技術和應用 [M] ，清華大學出版社，2002.3。

-5-

中國科技論文在線

Study for positive solutions and anti-solution of Six-DOF

Stewart platform

Cai Baofu, Liao Chuanshu

Department of communication, Wuhan University of Technology, Wuhan（430070）

Abstract

This article studies the Six-DOF Stewart platform-type positive solutions and anti-solution. according

to the structural characteristics of Six-DOF Stewart platform, in order to achieve high precision

real-time control and design a positive and negative solution algorithm which is simple, efficient, and

has easy-to-programming . The algorithm has been simulated under MATLAB, and embedded it into

the VxWorks system to run. This article describes the implementation of this design idea.

Keyword：Six-DOF Stewart platform;positive solutions;anti-positive solutions;Real-time control;

VxWorks

作者簡介：蔡保富，男，1985年出生，碩士研究生，主要研究方向是模式識別、智能控制

和嵌入式系統。

-6-