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-