- Email: [email protected]
Inverse kinematics of variable geometry parallel manipulator
Mechanism and Machine Theory
Mechanism and Machine Theory 40 (2005) 141–155
www.elsevier.com/locate/mechmt
Inverse kinematics of variable geometry parallel manipulator Yu-Xin Wang a
a,b,*
, Yi-Ming Wang
b
Department of Mechanical Engineering, Tongji University, Shanghai 200092, PR China Department of Mechanical Engineering, Tianjin University, Tianjin 300072, PR China
b
Received 11 May 2003; received in revised form 26 June 2004; accepted 6 July 2004 Available online 11 September 2004
Abstract The variable geometry parallel manipulator (VGPM) is a kind of manipulator that is suitable to change the spatial distribution of a variable geometry body (VGB). In the VGPM, the Stewart platform is utilized as a driving mechanism, and a number of spatial RSRR kinematic chains, which connect with the base frame and the top platform through revolute pairs, respectively, guide the rigid plates with spatially shaped inner surfaces to form the envelope of the VGB. The inverse kinematics problem of the VGPM is to determine every actuator displacement of the Stewart platform for given the outlet area and deflection angle of the VGB. In this paper, we present a method to solve this kind of inverse kinematics problem. At first, we present an assumption of the double semi-ellipses approximate distribution model of the VGB to determine the approximate position and orientation of the top platform. Through analysis of the deviations between the approximate distribution model and the given distribution parameters of the VGB, after making the compensation of the deviations, we obtain the corrected inverse kinematic solution of the VGPM with acceptable tolerance. Ó 2004 Elsevier Ltd. All rights reserved.
1. Introduction Quite recently, parallel manipulators have received a great deal of attention from many researchers [1]. This popularity is a result of the fact that the parallel manipulators have more advantages in comparison to serial robots in many aspects, such as stiffness in mechanical *
Corresponding author. Address: Department of Mechanical Engineering, Tongji University, Shanghai 200092, PR China. Tel.: +86 21 65984185; fax: +86 21 65984186. E-mail address: [email protected] (Y.-X. Wang). 0094-114X/$ - see front matter Ó 2004 Elsevier Ltd. All rights reserved. doi:10.1016/j.mechmachtheory.2004.07.003
142
Y.-X. Wang, Y.-M. Wang / Mechanism and Machine Theory 40 (2005) 141–155
structure, position accuracy, etc. The Stewart platform [2] is a typical parallel manipulator, which consists of two platforms connected by six extensible legs (actuators) with spherical joints at either end. The fixed platform is called the ‘‘base frame’’, and the movable platform is called the ‘‘top platform’’, which has 6 degrees of freedom relative to the base frame. Parallel manipulators generally perform the task of controlling the top platform with respect to the base frame. To achieve this goal, the position analysis of parallel manipulators, the forward and inverse kinematics problems, should be solved at first. The forward problem, which is the problem of finding the poses (positions and orientations) of the top platform when every actuator displacement is given, is challenge. In contrast to this, the inverse kinematics problem, which consists in finding the set of joint variables to achieve a desired configuration of the top platform, is easy in contrast to serial chain manipulators where the opposite is true. Many researchers [3–7] have studied the forward displacement analysis of parallel manipulators. The task of the variable geometry parallel manipulator (VGPM) [8] is quite different from that of parallel manipulators. Here, the objective is to change the spatial distribution of the variable geometry body (VGB), which is described by two parameters: the outlet area and the deflection angle. In the VGPM, the Stewart platform is utilized as a driving mechanism, and a number of spatial RSRR kinematic chains guide the rigid plates with spatially shaped inner enveloping surfaces X, which are fixed to the SR links of the RSRR kinematic chains, to form a variable geometry body. Here, the RSRR kinematic chain connects with the base frame and the top platform through revolute pairs respectively. In accord with parallel manipulators, the VGPM also has its inverse and forward kinematics problems. Figuring out the spatial distribution of the VGB when each actuator displacement of the Stewart platform is given is called the forward kinematics problem of the VGPM. The inverse kinematics problem of the VGPM consists in determining every actuator displacement of the Stewart platform when the spatial distribution of the VGB is given. Since the VGB is enveloped by a series of surfaces X, and the relationship between the distribution parameters of the VGB and the poses of the top platform is constrained by a set of implicit nonlinear equations, the inverse kinematics problem of the VGPM is quite difficult in contrast with its forward kinematics problem. The goal of this paper is to present a method for solving the inverse kinematics problem of the VGPM. For the given outlet area and the deflection angle of the VGB, the inverse kinematics problem of the VGPM can be divided into two steps: determining the poses of the top platform and calculating the inverse kinematics problem of the Stewart platform. The second step is straightforward, and it will not be presented in this paper. This work only focuses on the first step, i.e. for the given outlet area and the deflection angle of the VGB determining the poses of the top platform.
2. Double semi-ellipses distribution model of the VGB 2.1. Structure of the VGPM As shown in Fig. 1, the VGPM consists of two parts, the driving Stewart platform mechanism and a number of spatial RSRR kinematic chains (AiBiCiDi). The RSRR kinematic chain connects with the base frame and the top platform through the revolute pair at point Ai and the revolute pair at point Di, respectively. The rigid plate with a spatially shaped inner enveloping surface X is
Y.-X. Wang, Y.-M. Wang / Mechanism and Machine Theory 40 (2005) 141–155
Bi
S
Pi π
Z
nθ
δi
Top Platform
143
Ai R
E
R
R
ωi Ci Di vi
O
y
φi
Base Frame
x ζ
Fig. 1. Structure of the VGPM.
fixed to the SR links (BiCi) in the RSRR chain. By a number of surfaces X, the variable geometry body (VGB) will be enveloped. The variation of the pose of the top platform will make the RSRR kinematic chains change their spatial positions. In this situation, the distribution of the VGB, which is enveloped by a number of surfaces X, will change synchronously. There are two approximate terminal areas in the VGB. One is formed by the middle endpoints of the surfaces X, Pi, which are far from the base frame. We call it the outlet area, denoted by An. The opposite terminal area is called the inlet area. The angle between the connecting line from the inlet area center to the outlet area center G and the center normal of the base frame is defined as deflection angle, denoted by a. The fixed reference coordinate Oxyz is attached to the base frame, and its origin is located at the center point O of the base frame. The z axis is the normal line of the based frame through the center point O. Points Ai are distributed symmetrically along the circle with radius R2 on the top platform, and points Di are distributed symmetrically along the circle with radius R3 on the base frame. The other two joints in the RSRR kinematic chain are Ci, Di, respectively. The location of the kinematic RSRR chain is expressed by the angle /i (/i = 2pi/n) (i = 1, 2, . . . ,n) relative to the x axis. 2.2. Inverse kinematics problem of the VGPM 2.2.1. Given outlet area An The given outlet area An of the VGB is defined as a sum of the triangles DPiGPi+1 N X An ¼ DP i GP iþ1 i
ð1Þ
144
Y.-X. Wang, Y.-M. Wang / Mechanism and Machine Theory 40 (2005) 141–155
2.2.2. Given deflection angle a When link DiCi is short enough relative to link CiBi we suppose that the inlet area locates in the base frame approximately. Due to the symmetry of the VGPM, the center of the inlet area should be the coordinate origin O. Based on this assumption the deflecting angle a of the VGB is the angle between the line from the origin O to the center G of the outlet area An and the z axis Gx a ¼ cos1 qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi G2x þ G2y þ G2z
ð2Þ
2.2.3. Deflection azimuth In the VGB, there is a plane passing through the origin O and the center G of the outlet area An, the kinematic parameters and the spatial positions of the RSRR chains are distributed symmetrically relative to this plane. We call this plane here the deflection plane. The angle between the deflection plane and the yoz plane is called the deflection azimuth, which is denoted by b, and b 2 [0,2p]. 2.2.4. Inverse kinematics problem of the VGPM The outlet area An, the deflection angle a, and the deflection azimuth b of the VGB are three important parameters to describe the spatial distribution of the VGB. The main inverse kinematics problem of the VGPM is to find the way to determine the pose of the top platform of the Stewart platform for a group of the given parameters, which are the outlet area An, the deflection angle a, and the deflection azimuth b of the VGB.
Fig. 2. Distribution of surfaces X on the VGB.
Y.-X. Wang, Y.-M. Wang / Mechanism and Machine Theory 40 (2005) 141–155
145
2.3. RSRR kinematic chains in the particular positions 2.3.1. Kinematic chains in the deflection plane As shown in Fig. 2, when /i = b, the RSRR kinematic chain, in which the parameters are denoted by subscript ‘‘0S’’, is located in the deflection plane. In this RSRR kinematic chain, the axis of the revolute pair at A0S is parallel to the axis of the revolute pair at D0S, and the middle endpoint of the surface X guided by this RSRR kinematic chain is P0S. When /i = b + p, the RSRR kinematic chain, in which the parameters are denoted by subscript ‘‘2S’’, is also located in the deflection plane. In this RSRR kinematic chain, the axis of the revolute pair at A2S is parallel to the axis of the revolute pair at D2S, and the middle endpoint of the surface X guided by this RSRR kinematic chain is P2S. Because of the symmetry of the VGPM, the center of the VGB, G, must be on the connecting line P0SP2S. 2.3.2. Kinematic chain in the symmetrical position The RSRR kinematic chain relative to /i = b + p/2 and the RSRR kinematic chain relative to /i = b + 3p/2 are symmetrical about the deflection plane. The parameters in these kinematic chains are denoted by subscript ‘‘1S’’ and ‘‘3S’’, respectively. In Fig. 2, NS is the intersection point of line P0SP2S and line P1SP3S. When /i5b, no actual kinematic chain falls into the deflection plane. In this case, we can assume that there are two imaginary RSRR chains in the deflection plane at /i = b, /i = b + p, respectively, and that there are two imaginary RSRR chains in the symmetrical position at /i = b + p/2, /i = b + 3p/2, respectively. The final result in finding the approximate pose of the top platform of Stewart platform will not be affected. 2.4. Double semi-ellipses distribution model of the VGB When the VGB is deflected, the outlet area of the VGB is symmetrical with respect to the deflection plane. The numerical analysis shows that the middle endpoints Pi are distributed on two semi-ellipses approximately. One of the semi-ellipse passes through P1S, P2S, P3S, the other passes through P3S, P0S, P1S. We use these two semi-ellipses to express the distribution of the VGB approximately and call this distribution model of the VGB the double semi-ellipses distribution model, or the approximate distribution model. The outlet area An1 in the approximate distribution model is An1 ¼ paðb1 þ b2 Þ=2 where
qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi T a ¼ 0:5 ðP1S P3S Þ ðP1S P3S Þ qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi T b1 ¼ ðP0S N S Þ ðP0S N S Þ qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi T b2 ¼ ðP2S N S Þ ðP2S N S Þ
146
Y.-X. Wang, Y.-M. Wang / Mechanism and Machine Theory 40 (2005) 141–155
and NS is an intersection point of line P0SP2S and line P1SP3S. The center of the outlet area of the VGB in the approximate distribution model, GS, is 4ðb2 b1 Þ ðP2S P0S Þ þ N S GS ¼ 3pðb2 þ b1 Þ Accordingly, the outlet area in the approximate distribution model, An1, should equal the given area An, so one can set up the following equation f1 ¼ An An1 ¼ 0 ð3Þ Moreover, note that the approximate deflection angle determined by GS in the approximate distribution model should be equal to the given deflection angle aS. Thus one has f2 ¼ S 2 aS G2Sx C 2 aS G2Sy C 2 aS G2Sz ¼ 0 ð4Þ where C 2 aS ¼ cos2 aS , S 2 aS ¼ sin2 aS : 3. Approximate inverse kinematics 3.1. Pose of the top platform There are six parameters to identify the pose (position and orientation) of the top platform of the Stewart platform. In order to improve the capacity of the VGPM to bear the side-load and to prevent the interferences between surfaces X, the center of the top platform should be on the zaxis. In this situation, the center coordinate of the top platform is E (0, 0, l0), where l0 is the distance from the top platform center E to the base frame center O, as shown in Fig. 1. Moreover, in order to prevent collisions between surfaces X, the rotational degree of freedom of the top platform about z-axis is restricted. In this case, the two parameters h and n, as shown in Fig. 1, can be used to represent the orientation of the platform. Here h is the angle between the normal vector ~ n of the top platform and z-axis, and n is the angle between the projection of the normal vector ~ n on the xOy plane and x-axis. Since the VGB is symmetrical about its deflection plane, we know that n = b. Therefore, to the inverse kinematic problem of the VGPM, the three unknown parameters describing the pose of the top platform are ð5Þ fl0 ; h; ng
3.2. Surfaces X in general positions Let di denote the rotation angle of link AiBi at the revolute joint Ai, xi denote the rotation angle of link BiCi at the revolute joint Ci, and mi denote the rotation angle of link CiDi at the revolute joint Di. Then the coordinate of the middle endpoint Pi of surface X can be written as 2 3 2 3 lCP ðCui Smi Cxi Sui Sxi Þ þ lCD Cui Smi þ R3 Cui P xi Pi ¼ 4 P yi 5 ¼ 4 lCP ðSui Smi Cxi þ Cui Sxi Þ þ lCD Sui Smi þ R3 Sui 5 ð6Þ P zi lCP Cmi Cxi þ lCD Cmi
Y.-X. Wang, Y.-M. Wang / Mechanism and Machine Theory 40 (2005) 141–155
147
where C ¼ cos, S ¼ sin. If the set of the joint variables [vi,xi,di]T of the RSRR chain is known, the position of surface X can be determined through the Eq. (6). Considering rBi ¼ rAi þ LBi Ai ¼ rDi þ LCi Di þ LBi Ci we can write the following equations qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi g1i ¼ ½lCB ðS/i Sxi þ C/i Smi Cxi Þ þ lCD C/i Smi þ R3 C/i 1 S 2 bS 2 h lAB ðC/i ChSdi þ CbShCdi Þ R2 C/i Ch lEF CbSh ¼ 0
ð7aÞ
qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi g2i ¼ ½lCB ðC/i Sxi þ S/i Smi Cxi Þ þ lCD S/i Smi þ R3 S/i 1 S 2 bS 2 h lAB ½C/i SbCbS 2 hSdi þ S/i ð1 S 2 bS 2 hÞSdi þ SbShChCdi þ R2 ½C/i SbCbS 2 h S/i ð1 S 2 bS 2 hÞ lEF SbShCh ¼ 0
ð7bÞ
g3i ¼ lCB Cmi Cxi þ lCD Cmi þ lAB ðC/i CbShSdi þ S/i SbShSdi ChCdi Þ þ R2 ðC/i CbSh þ S/i SbShÞ l0 lEF Ch ¼ 0
ð7cÞ
Since the VGB is enveloped by a series of surfaces X, for the given distribution parameters a, b, An, we do not know the real distributions of all surface X on the VGB. Therefore, in Eq. (7), the rotation angles mi, xi, di of each RSRR kinematic chain in the general position are unknown. To obtain these unknown joint variables we should make use of the characteristics of surfaces X in the particular positions. 3.3. Surfaces X in the particular positions Due to n = b, without loss of the universality, we analyze the characteristics of the RSRR kinematic chain in the deflection plane when the deflection azimuth b = 0. In this case, the deflection plane of the VGB is the xOz plane, and the pose of the top platform can be expressed by the parameters h = hS and l0 = l0S, respectively. In the deflection plane, for the RSRR kinematic chain with respect to the location angle /i = 0, the rotation angle at the joint Ci is xi = x0S = 0. In this case, letting mi = m0S, di = d0S and substituting these parameters into Eq. (7), we obtain f3 ¼ ðlCD þ lCB ÞSm0S þ R3 lAB ðChS Sd0S þ ShS Cd0S Þ R2 ChS lEF ShS ¼ 0
ð8Þ
f4 ¼ ðlCD þ lCB ÞCm0S þ lAB ðShS Sd0S ChS Cd0S Þ þ R2 ShS lEF ChS l0S ¼ 0
ð9Þ
Moreover, for the RSRR kinematic chain with respect to the location angle /i = p, the rotation angle of the RSRR kinematic chain at joint Ci is xi = x2S = 0. Then, letting mi = m2S, di = d2S and substituting these parameters into Eq. (7), we have
148
Y.-X. Wang, Y.-M. Wang / Mechanism and Machine Theory 40 (2005) 141–155
f5 ¼ ðlCD þ lCB ÞSm2S þ R3 lAB ðChS Sd2S ShS Cd2S Þ R2 ChS þ lEF ShS ¼ 0
ð10Þ
f6 ¼ ðlCD þ lCB ÞCm2S lAB ðShS Sd2S þ ChS Cd2S Þ R2 ShS lEF ChS l0S ¼ 0
ð11Þ
For the kinematic chain in the symmetrical position with respect to the location angle /i = p/2, letting mi = m1S, xi = x1S, di = d1S, and substituting these parameters into Eq. (7), we obtain f7 ¼ lCB Sx1S þ lAB ShS Cd1S þ lEF ShS ¼ 0
ð12Þ
f8 ¼ lCB Sm1S Cx1S þ lCD Sm1S þ R3 lAB Sd1S R2 ¼ 0
ð13Þ
f9 ¼ lCB Cm1S Cx1S þ lCD Cm1S lAB ChS Cd1S l0S lEF ChS ¼ 0
ð14Þ
Uniting Eqs. (3) and (4) and Eqs. (8)–(14), we obtain the approximate inverse kinematics model of the VGPM as follows: T
F ðx; yÞ ¼ ½f1 ; f2 ; f5 ; . . . ; f9 ¼ 0T where (
ð15Þ
T
x ¼ ½m0S ; d0S ; m2S ; d2S ; m1S ; x1S ; d1S ; hS ; l0S T
y ¼ ½a; b ¼ 0; An
In Eq. (15) there are nine unknown variables and nine equations. When giving the distribution parameters of the VGB, a, b, An, through Eq. (15) we can solve these unknown variables with the Newton–Raphson method. Then the pose of the top platform in the double semi-ellipses distribution model is ½0; 0; l0S ; hS ; b ¼ 0; 0T 3.4. Reducing the dimensions of Eq. (15) Solving nine-dimensional nonlinear equation (15) with Newton–Raphson method is inefficient and time-consuming, and thus is not suitable for real-time control. In order to increase the calculating efficiency of Eq. (15), we should reduce the dimensions of Eq. (15). Letting hS and l0S express m0S and d0S in Eqs. (8) and (9), respectively, one obtains 8 1 c04 > > < d0S ¼ tg c03 hS pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi c205 þ c206 c207 c c þ M c > 06 07 0s 05 > : m0S ¼ cos1 c205 þ c206 On the other hand, letting hS and l0S express m2S and d2S in Eqs. (10) and (11), it follows
Y.-X. Wang, Y.-M. Wang / Mechanism and Machine Theory 40 (2005) 141–155
149
8 1 c24 > > < d2S ¼ hS tg c23
pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi c225 þ c226 c227 > 1 c26 c27 þ M 2s c25 > : m2S ¼ cos c225 þ c226 Finally, letting hS and l0S express m1S and x1S in Eqs. (12) and (14), leads to 8 c17 > m1S ¼ cos1 > > c16 > < 8qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi9 < l2 S 2 hS ðlAB Cd1S þ lEF Þ2 = BC > 1 > > x ¼ cos > : 1S : ; l BC
where c0i, c1i, c2i are the coefficients relative to hS, l0S, and M0S = ±1, M1S = ±1, M2S = ± 1. The mathematical sign before ‘‘1’’ can be either positive or negative, depending on the assembly configuration of the RSRR kinematic chain. Substituting m0S, d0S, m2S, d2S, m1S, x1S into Eqs. (3), (4), and (13), we obtain three equations for determining the pose of the top platform 8 > < f1 ¼ An An1 ¼ 0 f2 ¼ S 2 aG2Sx C 2 aG2Sy C 2 aG2Sz ¼ 0 ð16Þ > : f8 ¼ lCB Sm1s Cx1s þ lCD Sm1s lAB Sd1s R2 þ R3 ¼ 0 n pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi o l2BC S 2 hS ðlAB Cd1S þlEF Þ2 1 : where xS ¼ cos lBC In Eq. (16), An1, GSx, GSy, GSz, m1S are implicit functions of the three unknown parameters d1S, hS and l0S, as shown in the Appendix A. For the given area An and the deflection angle a of the VGB, through solving Eq. (16), we can calculate the position and the orientation of the top platform in the double semi-ellipses distribution model, i.e. parameters hS and l0S. 3.5. Approximate outlet area and deflection angle In Eq. (16), the determination of the parameters hS and l0S (the orientation and position of the top platform) is based on the assumption that the middle endpoints of surfaces X are distributed along the double semi-ellipses. However, once the orientation and position of the top platform, i.e. hs and l0s, respectively, are given, the actual positions of the middle endpoints of surfaces X, Pi, can be calculated through the following procedure. 1. Substitute h = hS, l0 = l0S in Eq. (7). 2. Solve Eq. (7) for the joint variables mi, xi, di. 3. Substitute the joint variables mi, xi, di in Eq. (6), yielding the middle endpoints Pi. Hence, then all surfaces X are computed.
150
Y.-X. Wang, Y.-M. Wang / Mechanism and Machine Theory 40 (2005) 141–155
3.5.1. Actual outlet area of the VGB Given pose [0, 0, l0 = l0S, h = hS, b = 0,0]T of the top platform, the actual outlet area of the VGB, which is calculated by a series of triangles DPiGSPi + 1, is n n qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi X X A21i þ A22i þ A23i ð17Þ Ai ¼ Aa ¼ i¼1
i¼1
where A1i ¼ ðN Sy P zi þ P yi P zi1 þ N Sz P yi1 N Sy P zi1 N Sz P yi P zi P yi1 Þ=2 A2i ¼ ðN Sz P xi þ P zi P xi1 þ N Sx P zi1 N Sz P xi1 N Sx P zi P xi P zi1 Þ=2 A3i ¼ ðN Sx P yi þ P xi P yi1 þ N Sy P xi1 N Sx P yi1 N Sy P xi P yi P xi1 Þ=2 and i1 = i + 1. 3.5.2. Actual deflection angle of the VGB The actual deflection angle of the VGB with respect to the top platformÕs pose [0, 0, l0 = l0S, h = hS, b = 0, 0]T is Gax aa ¼ cos1 qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi G2ax þ G2ay þ G2az
ð18Þ
where n P
Gax ¼ i¼1
n P
Ai Gxi Aa
;
Gay ¼
n P
Ai Gyi
i¼1
Aa
;
Gaz ¼ i¼1
Ai Gzi Aa
and 1 Gxi ¼ ðP xi þ P xi1 þ N Sx Þ; 3
1 Gyi ¼ þ ðP yi þ P yi1 þ N Sy Þ; 3
1 Gzi ¼ ðP zi þ P zi1 þ N Sz Þ 3
4. Corrected inverse kinematics 4.1. Deviations of the deflection angle and the outlet area Since the pose [0, 0, l0 = l0S, h = hS, b = 0, 0]T of the top platform is obtained based on the double semi-ellipses approximate distribution model, a deviation between the given outlet area An and the actual outlet area Aa will exist when the top platform takes the pose [0, 0, l0S, hS, 0, 0]T. Te deviation of the outlet area is n n X X oAi oAi oAi dAi ¼ dA1i þ dA2i þ dA3i DAn ¼ Aa An ¼ ð19Þ oA1i oA2i oA3i i¼1 i¼1
Y.-X. Wang, Y.-M. Wang / Mechanism and Machine Theory 40 (2005) 141–155
151
Due to the symmetry of the VGPM, when b = 0, we know dGax = 0. Then the deviation between the given deflection angle a and the actual given deflection angle aa with respect to the pose [0, 0, l0S, hS, 0, 0]T is Gay dGay þ Gaz dGaz Da ¼ aa a ¼ qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi ðG2ax þ G2ay þ G2az ÞðG2ay þ G2az Þ
ð20Þ
4.2. Correction of the pose Taking partial derivatives of functions g1i, g2i, g3i in Eq. (7) with respect to the joint variables mi, xi, di, h and l0, we get the increments dmi, dxi, ddi of the joint variables expressed as the functions of dh and dl0 8 > < dmi ¼ m11i dh þ m12i dl0 dxi ¼ m21i dh þ m22i dl0 ð21Þ > : ddi ¼ m31i dh þ m32i dl0 Substituting Eq. (21) into Eq. (7), and taking the total differential of the middle endpoint Pi with respect to dh and dl0, we obtain for the differential expression of Pi 2 3 qi11 qi12 6 7 dh dP i ¼ 4 qi21 qi22 5 ð22Þ dl0 qi31 qi32 Substituting Eq. (22) into Eq. (17), we obtain the differential expressions of A1i, A2i, A3i expressed by dh and dl0. Considering Eq. (22) and taking differentials of Gxi, Gyi, Gzi in Eq. (18) with respect to dh and dl0, one obtains 8 > < dGxi ¼ s11i dh þ s12i dl0 dGyi ¼ s21i dh þ s22i dl0 ð23Þ > : dGzi ¼ s31i dh þ s32i dl0 Taking the total differentials of Gax, Gay and Gaz in Eq. (18), we have 8 n P > > ðGyi Aa dAi þ Ai Aa dGyi Ai Gyi DAn Þ > > > i¼1 > > < dGay ¼ A2 a
n P > > > ðGzi Aa dAi þ Ai Aa dGzi Ai Gzi DAn Þ > > > i¼1 > dG ¼ : az A2a
where qikj, m11i, m12i, m21i, m22i, m31i, m32i, s11i, s12i, s21i, s22i, s31i, s32i are coefficients.
ð24Þ
152
Y.-X. Wang, Y.-M. Wang / Mechanism and Machine Theory 40 (2005) 141–155
Meanwhile, we take the strategy that the VGB must fulfill the given deflection angle. In this case, Da in Eq. (20) should be equal to zero. From Eqs. (20) and (24), we can then deduce the following corrected deviations of the pose of the top platform 8 d 11 d 23 d 21 DAn > > < dl0 ¼ d 11 d 12 d 12 d 21 ð25Þ > d 22 DAn d 12 d 23 > : dh ¼ d 11 d 12 d 12 d 21 where dij are given coefficients.
With the double semi - ellipses approximate distribution model discribing An1 ,α while β = 0 Through Eq. (3) - (4) and Eq. (8) - (14), or Eq. (15) determing the pose of the top platform based on the double semi - ellipses distibution model of the VGB
Solving nonlinear equations with Newton Raphson method and obtaining the appoximate pose of the top platform : l0 S and θ S
Based on the double semi-ellipse model to determine the approximate pose of the top platform
Input An , α , β
Determing the actual outlet area Aa and the actual deflecting angle α a while the top platform with the pose l0 S , θ S
as well as the deviation ∆α n between α and α a
Letting ∆α = 0, and solving the corrected increments of the pose of the top platform dl0 , dθ through Eq. (24) The corrected pose of the top platform is [0,0, l0 S + dl0 ,θ S + dθ , β = 0,0]T
While β ≠ 0, letting the deflecting plane turn β angle relative to the z axis, the corrected pose of
Adjusting the pose of the top platform to make up for the deviationsofthe approximate model
Computing the deviation ∆An between An and Aa
the top platform is [0,0, l0 S + dl0 ,θ S + dθ , β ,0]T
Calculation the compensated outlet area Ac and deflecting angle α c
Fig. 3. The flow chart solving the inverse kinematics problem of the VGPM.
Y.-X. Wang, Y.-M. Wang / Mechanism and Machine Theory 40 (2005) 141–155
153
The case b50 is the same as the case when the deflection plane rotates by an angle b relative to the z axis. Therefore, the corrected pose of the top platform of the Stewart platform at any deflection azimuth is ½0; 0; l0s þ dl0 ; hs þ dh; b; 0T
ð26Þ
To the corrected pose of the top platform, we can obtain the compensated outlet area Ac and deflection angle ac straightforwardly using equations similar to Eqs. (17) and (18).
Deviation of the outlet area (mm2 )
∆A 0
Corrected Model Ac
− 500 − 1000
− 1500
Approximate Model A a
− 2000
− 2500 0
5
10
15
20
25
Deflecting angle α (deg)
Fig. 4. Deviation of the outlet area DA.
Deviation of the Deflecting angle (deg)
∆α 1 .0 0.75
Approximate Model α a 0. 5 0.25 0 − 0.25 − 0. 5
Corrected Model α c 0
15 10 5 Deflecting angle α (deg)
20
Fig. 5. Deviation of the deflection angle Da.
25
154
Y.-X. Wang, Y.-M. Wang / Mechanism and Machine Theory 40 (2005) 141–155
5. Numerical results Fig. 3 shows the flow chart for solving the inverse kinematics problem of the VGPM. The variation of the outlet area deviation DAc between the given area An and the compensated outlet area Ac along with the deflection angle of VGB is shown in Fig. 4. For the approximate inverse kinematics model, the maximum relative error of the outlet area An1 is less than 2%, and the relative error of the outlet area An1 displays a decreasing tendency along with the increase of the deflection angle a of VGB. However, for the corrected inverse kinematics model, the relative error of the compensated outlet area is less than 0.1%. For the approximate inverse kinematics model, the deviation D a of the deflection angle has almost linear relationship with the deflection angle when the outlet area is given, as shown in Fig. 5. Comparatively, for the corrected inverse kinematics model, the error of the deviation Da of the VGB is less than 0.12%. During the derivation of Eq. (24), we have assumed that the deviation Da should be equal to zero. But in this figure we find it is not equal to zero. This kind of error may be caused by the linear approximation in Eq. (21), where we have only considered the linear increments dmi, dxi, ddi of the joint variables. Although this kind of simplification produces only an approximation, it has really enhanced the computing efficiency of the inverse kinematics problem of the VGPM.
6. Conclusion This paper presents an approach for solving the inverse kinematics problem of a variable geometry parallel manipulator (VGPM). In the VGPM, a Stewart platform is utilized as a driving mechanism, while a number of spatial RSRR kinematic chains guide rigid plates with spatial shaped inner enveloping surfaces to form a variable geometry body. The inverse kinematics problem of the VGPM consists in determining every actuator displacement of the Stewart platform when the distribution parameters of the VGB are given. In this paper, we present a universal method for solving the inverse kinematics problem of a VGPM. The method consists of four steps: (1) utilizing a double semi-ellipses distribution model to describe the approximate distribution of the VGB; (2) calculating the approximate pose of the top platform of Stewart platform based on the double semi-ellipses distribution model; (3) analyzing the mathematical deviations between the double semi-ellipses distribution model and the given parameters of the VGB; (4) making the compensation of these mathematical deviations. Through these four steps, we have solved the inverse kinematics problem of a VGPM with acceptable tolerance efficiently. Acknowledgement The authors gratefully acknowledge the support provided by the Teaching and Research Award Program for Outstanding Young Teachers in Higher Education Institutions of MOE, P.R. China (1999076). The authors also gratefully acknowledge the support provided by the Natural Science Foundation Committee, China (50375301).
Y.-X. Wang, Y.-M. Wang / Mechanism and Machine Theory 40 (2005) 141–155
155
Appendix A The expressions of joint variables with d1S, hS and l0S. 8 1 c04 > > < d0S ¼ tg c03 hS pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi c205 þ c206 c207 c c þ M c 06 07 0s 05 > 1 > m0S ¼ cos : 2 c05 þ c206 where c01 ¼ R4 þ R2 Shs L1 Chs l0s ;
c02 ¼ R2 Chs L1 Shs þ R4
c03 ¼ ðlBC lCD ÞCm0s H 1 Sm0s þ c01 ; c05 ¼ 2ðlBC lCD Þc02 2H 1 c01 ; 2
c07 ¼ ðlBC lCD Þ þ
H 21
þ
c201
þ
c04 ¼ ðlBC lCD ÞSm0s þ H 1 Cm0s þ c02
c06 ¼ 2ðlBC lCD Þc01 þ 2H 1 c02 c202
l2AB
8 1 c24 > > < d2S ¼ hS tg c23
pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi c225 þ c226 c227 c c þ M c > 26 27 2s 25 > : m2S ¼ cos1 2 c25 þ c226 where c21 ¼ R4 R2 Shs L1 Chs l0s ;
c22 ¼ R2 Chs L1 Shs R4
c23 ¼ ðlBC lCD ÞCm2s H 1 Sm2s þ c21 ; c25 ¼ 2ðlBC lCD Þc22 2H 1 c21 ;
c24 ¼ ðlBC lCD ÞSm2s H 1 Cm2s þ c22
c26 ¼ 2ðlBC lCD Þc21 2H 1 c22
2
c27 ¼ ðlBC lCD Þ þ H 21 þ c221 þ c222 l2AB 8 c17 > m1S ¼ cos1 > > c16 > < 8qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi9 < l2 S 2 hS ðlAB Cd1S þ lEF Þ2 = BC > 1 > > x ¼ cos > : 1S : ; l BC
where c15 = H1, c16 = lABCx1s + lCD, c17 = R4lABChsCd1sL1Chsl0s.
References [1] B. Dasgupta, T.S. Mruthyunjaya, The Stewart platform manipulator: a review, Mechanism and Machine Theory 35 (2000) 15–40. [2] D. Stewart, A platform with six degree of freedom, in: Proceedings of the conference on the Institution of Mechanical Engineering, London, 1965. [3] M. Griffis, J. Duffy, A forward displacement analysis of a class of Stewart Platforms, Journal of Robotic Systems 6 (6) (1989) 703–720.
Mechanism and Machine Theory 40 (2005) 141–155
www.elsevier.com/locate/mechmt
Inverse kinematics of variable geometry parallel manipulator Yu-Xin Wang a
a,b,*
, Yi-Ming Wang
b
Department of Mechanical Engineering, Tongji University, Shanghai 200092, PR China Department of Mechanical Engineering, Tianjin University, Tianjin 300072, PR China
b
Received 11 May 2003; received in revised form 26 June 2004; accepted 6 July 2004 Available online 11 September 2004
Abstract The variable geometry parallel manipulator (VGPM) is a kind of manipulator that is suitable to change the spatial distribution of a variable geometry body (VGB). In the VGPM, the Stewart platform is utilized as a driving mechanism, and a number of spatial RSRR kinematic chains, which connect with the base frame and the top platform through revolute pairs, respectively, guide the rigid plates with spatially shaped inner surfaces to form the envelope of the VGB. The inverse kinematics problem of the VGPM is to determine every actuator displacement of the Stewart platform for given the outlet area and deflection angle of the VGB. In this paper, we present a method to solve this kind of inverse kinematics problem. At first, we present an assumption of the double semi-ellipses approximate distribution model of the VGB to determine the approximate position and orientation of the top platform. Through analysis of the deviations between the approximate distribution model and the given distribution parameters of the VGB, after making the compensation of the deviations, we obtain the corrected inverse kinematic solution of the VGPM with acceptable tolerance. Ó 2004 Elsevier Ltd. All rights reserved.
1. Introduction Quite recently, parallel manipulators have received a great deal of attention from many researchers [1]. This popularity is a result of the fact that the parallel manipulators have more advantages in comparison to serial robots in many aspects, such as stiffness in mechanical *
Corresponding author. Address: Department of Mechanical Engineering, Tongji University, Shanghai 200092, PR China. Tel.: +86 21 65984185; fax: +86 21 65984186. E-mail address: [email protected] (Y.-X. Wang). 0094-114X/$ - see front matter Ó 2004 Elsevier Ltd. All rights reserved. doi:10.1016/j.mechmachtheory.2004.07.003
142
Y.-X. Wang, Y.-M. Wang / Mechanism and Machine Theory 40 (2005) 141–155
structure, position accuracy, etc. The Stewart platform [2] is a typical parallel manipulator, which consists of two platforms connected by six extensible legs (actuators) with spherical joints at either end. The fixed platform is called the ‘‘base frame’’, and the movable platform is called the ‘‘top platform’’, which has 6 degrees of freedom relative to the base frame. Parallel manipulators generally perform the task of controlling the top platform with respect to the base frame. To achieve this goal, the position analysis of parallel manipulators, the forward and inverse kinematics problems, should be solved at first. The forward problem, which is the problem of finding the poses (positions and orientations) of the top platform when every actuator displacement is given, is challenge. In contrast to this, the inverse kinematics problem, which consists in finding the set of joint variables to achieve a desired configuration of the top platform, is easy in contrast to serial chain manipulators where the opposite is true. Many researchers [3–7] have studied the forward displacement analysis of parallel manipulators. The task of the variable geometry parallel manipulator (VGPM) [8] is quite different from that of parallel manipulators. Here, the objective is to change the spatial distribution of the variable geometry body (VGB), which is described by two parameters: the outlet area and the deflection angle. In the VGPM, the Stewart platform is utilized as a driving mechanism, and a number of spatial RSRR kinematic chains guide the rigid plates with spatially shaped inner enveloping surfaces X, which are fixed to the SR links of the RSRR kinematic chains, to form a variable geometry body. Here, the RSRR kinematic chain connects with the base frame and the top platform through revolute pairs respectively. In accord with parallel manipulators, the VGPM also has its inverse and forward kinematics problems. Figuring out the spatial distribution of the VGB when each actuator displacement of the Stewart platform is given is called the forward kinematics problem of the VGPM. The inverse kinematics problem of the VGPM consists in determining every actuator displacement of the Stewart platform when the spatial distribution of the VGB is given. Since the VGB is enveloped by a series of surfaces X, and the relationship between the distribution parameters of the VGB and the poses of the top platform is constrained by a set of implicit nonlinear equations, the inverse kinematics problem of the VGPM is quite difficult in contrast with its forward kinematics problem. The goal of this paper is to present a method for solving the inverse kinematics problem of the VGPM. For the given outlet area and the deflection angle of the VGB, the inverse kinematics problem of the VGPM can be divided into two steps: determining the poses of the top platform and calculating the inverse kinematics problem of the Stewart platform. The second step is straightforward, and it will not be presented in this paper. This work only focuses on the first step, i.e. for the given outlet area and the deflection angle of the VGB determining the poses of the top platform.
2. Double semi-ellipses distribution model of the VGB 2.1. Structure of the VGPM As shown in Fig. 1, the VGPM consists of two parts, the driving Stewart platform mechanism and a number of spatial RSRR kinematic chains (AiBiCiDi). The RSRR kinematic chain connects with the base frame and the top platform through the revolute pair at point Ai and the revolute pair at point Di, respectively. The rigid plate with a spatially shaped inner enveloping surface X is
Y.-X. Wang, Y.-M. Wang / Mechanism and Machine Theory 40 (2005) 141–155
Bi
S
Pi π
Z
nθ
δi
Top Platform
143
Ai R
E
R
R
ωi Ci Di vi
O
y
φi
Base Frame
x ζ
Fig. 1. Structure of the VGPM.
fixed to the SR links (BiCi) in the RSRR chain. By a number of surfaces X, the variable geometry body (VGB) will be enveloped. The variation of the pose of the top platform will make the RSRR kinematic chains change their spatial positions. In this situation, the distribution of the VGB, which is enveloped by a number of surfaces X, will change synchronously. There are two approximate terminal areas in the VGB. One is formed by the middle endpoints of the surfaces X, Pi, which are far from the base frame. We call it the outlet area, denoted by An. The opposite terminal area is called the inlet area. The angle between the connecting line from the inlet area center to the outlet area center G and the center normal of the base frame is defined as deflection angle, denoted by a. The fixed reference coordinate Oxyz is attached to the base frame, and its origin is located at the center point O of the base frame. The z axis is the normal line of the based frame through the center point O. Points Ai are distributed symmetrically along the circle with radius R2 on the top platform, and points Di are distributed symmetrically along the circle with radius R3 on the base frame. The other two joints in the RSRR kinematic chain are Ci, Di, respectively. The location of the kinematic RSRR chain is expressed by the angle /i (/i = 2pi/n) (i = 1, 2, . . . ,n) relative to the x axis. 2.2. Inverse kinematics problem of the VGPM 2.2.1. Given outlet area An The given outlet area An of the VGB is defined as a sum of the triangles DPiGPi+1 N X An ¼ DP i GP iþ1 i
ð1Þ
144
Y.-X. Wang, Y.-M. Wang / Mechanism and Machine Theory 40 (2005) 141–155
2.2.2. Given deflection angle a When link DiCi is short enough relative to link CiBi we suppose that the inlet area locates in the base frame approximately. Due to the symmetry of the VGPM, the center of the inlet area should be the coordinate origin O. Based on this assumption the deflecting angle a of the VGB is the angle between the line from the origin O to the center G of the outlet area An and the z axis Gx a ¼ cos1 qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi G2x þ G2y þ G2z
ð2Þ
2.2.3. Deflection azimuth In the VGB, there is a plane passing through the origin O and the center G of the outlet area An, the kinematic parameters and the spatial positions of the RSRR chains are distributed symmetrically relative to this plane. We call this plane here the deflection plane. The angle between the deflection plane and the yoz plane is called the deflection azimuth, which is denoted by b, and b 2 [0,2p]. 2.2.4. Inverse kinematics problem of the VGPM The outlet area An, the deflection angle a, and the deflection azimuth b of the VGB are three important parameters to describe the spatial distribution of the VGB. The main inverse kinematics problem of the VGPM is to find the way to determine the pose of the top platform of the Stewart platform for a group of the given parameters, which are the outlet area An, the deflection angle a, and the deflection azimuth b of the VGB.
Fig. 2. Distribution of surfaces X on the VGB.
Y.-X. Wang, Y.-M. Wang / Mechanism and Machine Theory 40 (2005) 141–155
145
2.3. RSRR kinematic chains in the particular positions 2.3.1. Kinematic chains in the deflection plane As shown in Fig. 2, when /i = b, the RSRR kinematic chain, in which the parameters are denoted by subscript ‘‘0S’’, is located in the deflection plane. In this RSRR kinematic chain, the axis of the revolute pair at A0S is parallel to the axis of the revolute pair at D0S, and the middle endpoint of the surface X guided by this RSRR kinematic chain is P0S. When /i = b + p, the RSRR kinematic chain, in which the parameters are denoted by subscript ‘‘2S’’, is also located in the deflection plane. In this RSRR kinematic chain, the axis of the revolute pair at A2S is parallel to the axis of the revolute pair at D2S, and the middle endpoint of the surface X guided by this RSRR kinematic chain is P2S. Because of the symmetry of the VGPM, the center of the VGB, G, must be on the connecting line P0SP2S. 2.3.2. Kinematic chain in the symmetrical position The RSRR kinematic chain relative to /i = b + p/2 and the RSRR kinematic chain relative to /i = b + 3p/2 are symmetrical about the deflection plane. The parameters in these kinematic chains are denoted by subscript ‘‘1S’’ and ‘‘3S’’, respectively. In Fig. 2, NS is the intersection point of line P0SP2S and line P1SP3S. When /i5b, no actual kinematic chain falls into the deflection plane. In this case, we can assume that there are two imaginary RSRR chains in the deflection plane at /i = b, /i = b + p, respectively, and that there are two imaginary RSRR chains in the symmetrical position at /i = b + p/2, /i = b + 3p/2, respectively. The final result in finding the approximate pose of the top platform of Stewart platform will not be affected. 2.4. Double semi-ellipses distribution model of the VGB When the VGB is deflected, the outlet area of the VGB is symmetrical with respect to the deflection plane. The numerical analysis shows that the middle endpoints Pi are distributed on two semi-ellipses approximately. One of the semi-ellipse passes through P1S, P2S, P3S, the other passes through P3S, P0S, P1S. We use these two semi-ellipses to express the distribution of the VGB approximately and call this distribution model of the VGB the double semi-ellipses distribution model, or the approximate distribution model. The outlet area An1 in the approximate distribution model is An1 ¼ paðb1 þ b2 Þ=2 where
qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi T a ¼ 0:5 ðP1S P3S Þ ðP1S P3S Þ qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi T b1 ¼ ðP0S N S Þ ðP0S N S Þ qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi T b2 ¼ ðP2S N S Þ ðP2S N S Þ
146
Y.-X. Wang, Y.-M. Wang / Mechanism and Machine Theory 40 (2005) 141–155
and NS is an intersection point of line P0SP2S and line P1SP3S. The center of the outlet area of the VGB in the approximate distribution model, GS, is 4ðb2 b1 Þ ðP2S P0S Þ þ N S GS ¼ 3pðb2 þ b1 Þ Accordingly, the outlet area in the approximate distribution model, An1, should equal the given area An, so one can set up the following equation f1 ¼ An An1 ¼ 0 ð3Þ Moreover, note that the approximate deflection angle determined by GS in the approximate distribution model should be equal to the given deflection angle aS. Thus one has f2 ¼ S 2 aS G2Sx C 2 aS G2Sy C 2 aS G2Sz ¼ 0 ð4Þ where C 2 aS ¼ cos2 aS , S 2 aS ¼ sin2 aS : 3. Approximate inverse kinematics 3.1. Pose of the top platform There are six parameters to identify the pose (position and orientation) of the top platform of the Stewart platform. In order to improve the capacity of the VGPM to bear the side-load and to prevent the interferences between surfaces X, the center of the top platform should be on the zaxis. In this situation, the center coordinate of the top platform is E (0, 0, l0), where l0 is the distance from the top platform center E to the base frame center O, as shown in Fig. 1. Moreover, in order to prevent collisions between surfaces X, the rotational degree of freedom of the top platform about z-axis is restricted. In this case, the two parameters h and n, as shown in Fig. 1, can be used to represent the orientation of the platform. Here h is the angle between the normal vector ~ n of the top platform and z-axis, and n is the angle between the projection of the normal vector ~ n on the xOy plane and x-axis. Since the VGB is symmetrical about its deflection plane, we know that n = b. Therefore, to the inverse kinematic problem of the VGPM, the three unknown parameters describing the pose of the top platform are ð5Þ fl0 ; h; ng
3.2. Surfaces X in general positions Let di denote the rotation angle of link AiBi at the revolute joint Ai, xi denote the rotation angle of link BiCi at the revolute joint Ci, and mi denote the rotation angle of link CiDi at the revolute joint Di. Then the coordinate of the middle endpoint Pi of surface X can be written as 2 3 2 3 lCP ðCui Smi Cxi Sui Sxi Þ þ lCD Cui Smi þ R3 Cui P xi Pi ¼ 4 P yi 5 ¼ 4 lCP ðSui Smi Cxi þ Cui Sxi Þ þ lCD Sui Smi þ R3 Sui 5 ð6Þ P zi lCP Cmi Cxi þ lCD Cmi
Y.-X. Wang, Y.-M. Wang / Mechanism and Machine Theory 40 (2005) 141–155
147
where C ¼ cos, S ¼ sin. If the set of the joint variables [vi,xi,di]T of the RSRR chain is known, the position of surface X can be determined through the Eq. (6). Considering rBi ¼ rAi þ LBi Ai ¼ rDi þ LCi Di þ LBi Ci we can write the following equations qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi g1i ¼ ½lCB ðS/i Sxi þ C/i Smi Cxi Þ þ lCD C/i Smi þ R3 C/i 1 S 2 bS 2 h lAB ðC/i ChSdi þ CbShCdi Þ R2 C/i Ch lEF CbSh ¼ 0
ð7aÞ
qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi g2i ¼ ½lCB ðC/i Sxi þ S/i Smi Cxi Þ þ lCD S/i Smi þ R3 S/i 1 S 2 bS 2 h lAB ½C/i SbCbS 2 hSdi þ S/i ð1 S 2 bS 2 hÞSdi þ SbShChCdi þ R2 ½C/i SbCbS 2 h S/i ð1 S 2 bS 2 hÞ lEF SbShCh ¼ 0
ð7bÞ
g3i ¼ lCB Cmi Cxi þ lCD Cmi þ lAB ðC/i CbShSdi þ S/i SbShSdi ChCdi Þ þ R2 ðC/i CbSh þ S/i SbShÞ l0 lEF Ch ¼ 0
ð7cÞ
Since the VGB is enveloped by a series of surfaces X, for the given distribution parameters a, b, An, we do not know the real distributions of all surface X on the VGB. Therefore, in Eq. (7), the rotation angles mi, xi, di of each RSRR kinematic chain in the general position are unknown. To obtain these unknown joint variables we should make use of the characteristics of surfaces X in the particular positions. 3.3. Surfaces X in the particular positions Due to n = b, without loss of the universality, we analyze the characteristics of the RSRR kinematic chain in the deflection plane when the deflection azimuth b = 0. In this case, the deflection plane of the VGB is the xOz plane, and the pose of the top platform can be expressed by the parameters h = hS and l0 = l0S, respectively. In the deflection plane, for the RSRR kinematic chain with respect to the location angle /i = 0, the rotation angle at the joint Ci is xi = x0S = 0. In this case, letting mi = m0S, di = d0S and substituting these parameters into Eq. (7), we obtain f3 ¼ ðlCD þ lCB ÞSm0S þ R3 lAB ðChS Sd0S þ ShS Cd0S Þ R2 ChS lEF ShS ¼ 0
ð8Þ
f4 ¼ ðlCD þ lCB ÞCm0S þ lAB ðShS Sd0S ChS Cd0S Þ þ R2 ShS lEF ChS l0S ¼ 0
ð9Þ
Moreover, for the RSRR kinematic chain with respect to the location angle /i = p, the rotation angle of the RSRR kinematic chain at joint Ci is xi = x2S = 0. Then, letting mi = m2S, di = d2S and substituting these parameters into Eq. (7), we have
148
Y.-X. Wang, Y.-M. Wang / Mechanism and Machine Theory 40 (2005) 141–155
f5 ¼ ðlCD þ lCB ÞSm2S þ R3 lAB ðChS Sd2S ShS Cd2S Þ R2 ChS þ lEF ShS ¼ 0
ð10Þ
f6 ¼ ðlCD þ lCB ÞCm2S lAB ðShS Sd2S þ ChS Cd2S Þ R2 ShS lEF ChS l0S ¼ 0
ð11Þ
For the kinematic chain in the symmetrical position with respect to the location angle /i = p/2, letting mi = m1S, xi = x1S, di = d1S, and substituting these parameters into Eq. (7), we obtain f7 ¼ lCB Sx1S þ lAB ShS Cd1S þ lEF ShS ¼ 0
ð12Þ
f8 ¼ lCB Sm1S Cx1S þ lCD Sm1S þ R3 lAB Sd1S R2 ¼ 0
ð13Þ
f9 ¼ lCB Cm1S Cx1S þ lCD Cm1S lAB ChS Cd1S l0S lEF ChS ¼ 0
ð14Þ
Uniting Eqs. (3) and (4) and Eqs. (8)–(14), we obtain the approximate inverse kinematics model of the VGPM as follows: T
F ðx; yÞ ¼ ½f1 ; f2 ; f5 ; . . . ; f9 ¼ 0T where (
ð15Þ
T
x ¼ ½m0S ; d0S ; m2S ; d2S ; m1S ; x1S ; d1S ; hS ; l0S T
y ¼ ½a; b ¼ 0; An
In Eq. (15) there are nine unknown variables and nine equations. When giving the distribution parameters of the VGB, a, b, An, through Eq. (15) we can solve these unknown variables with the Newton–Raphson method. Then the pose of the top platform in the double semi-ellipses distribution model is ½0; 0; l0S ; hS ; b ¼ 0; 0T 3.4. Reducing the dimensions of Eq. (15) Solving nine-dimensional nonlinear equation (15) with Newton–Raphson method is inefficient and time-consuming, and thus is not suitable for real-time control. In order to increase the calculating efficiency of Eq. (15), we should reduce the dimensions of Eq. (15). Letting hS and l0S express m0S and d0S in Eqs. (8) and (9), respectively, one obtains 8 1 c04 > > < d0S ¼ tg c03 hS pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi c205 þ c206 c207 c c þ M c > 06 07 0s 05 > : m0S ¼ cos1 c205 þ c206 On the other hand, letting hS and l0S express m2S and d2S in Eqs. (10) and (11), it follows
Y.-X. Wang, Y.-M. Wang / Mechanism and Machine Theory 40 (2005) 141–155
149
8 1 c24 > > < d2S ¼ hS tg c23
pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi c225 þ c226 c227 > 1 c26 c27 þ M 2s c25 > : m2S ¼ cos c225 þ c226 Finally, letting hS and l0S express m1S and x1S in Eqs. (12) and (14), leads to 8 c17 > m1S ¼ cos1 > > c16 > < 8qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi9 < l2 S 2 hS ðlAB Cd1S þ lEF Þ2 = BC > 1 > > x ¼ cos > : 1S : ; l BC
where c0i, c1i, c2i are the coefficients relative to hS, l0S, and M0S = ±1, M1S = ±1, M2S = ± 1. The mathematical sign before ‘‘1’’ can be either positive or negative, depending on the assembly configuration of the RSRR kinematic chain. Substituting m0S, d0S, m2S, d2S, m1S, x1S into Eqs. (3), (4), and (13), we obtain three equations for determining the pose of the top platform 8 > < f1 ¼ An An1 ¼ 0 f2 ¼ S 2 aG2Sx C 2 aG2Sy C 2 aG2Sz ¼ 0 ð16Þ > : f8 ¼ lCB Sm1s Cx1s þ lCD Sm1s lAB Sd1s R2 þ R3 ¼ 0 n pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi o l2BC S 2 hS ðlAB Cd1S þlEF Þ2 1 : where xS ¼ cos lBC In Eq. (16), An1, GSx, GSy, GSz, m1S are implicit functions of the three unknown parameters d1S, hS and l0S, as shown in the Appendix A. For the given area An and the deflection angle a of the VGB, through solving Eq. (16), we can calculate the position and the orientation of the top platform in the double semi-ellipses distribution model, i.e. parameters hS and l0S. 3.5. Approximate outlet area and deflection angle In Eq. (16), the determination of the parameters hS and l0S (the orientation and position of the top platform) is based on the assumption that the middle endpoints of surfaces X are distributed along the double semi-ellipses. However, once the orientation and position of the top platform, i.e. hs and l0s, respectively, are given, the actual positions of the middle endpoints of surfaces X, Pi, can be calculated through the following procedure. 1. Substitute h = hS, l0 = l0S in Eq. (7). 2. Solve Eq. (7) for the joint variables mi, xi, di. 3. Substitute the joint variables mi, xi, di in Eq. (6), yielding the middle endpoints Pi. Hence, then all surfaces X are computed.
150
Y.-X. Wang, Y.-M. Wang / Mechanism and Machine Theory 40 (2005) 141–155
3.5.1. Actual outlet area of the VGB Given pose [0, 0, l0 = l0S, h = hS, b = 0,0]T of the top platform, the actual outlet area of the VGB, which is calculated by a series of triangles DPiGSPi + 1, is n n qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi X X A21i þ A22i þ A23i ð17Þ Ai ¼ Aa ¼ i¼1
i¼1
where A1i ¼ ðN Sy P zi þ P yi P zi1 þ N Sz P yi1 N Sy P zi1 N Sz P yi P zi P yi1 Þ=2 A2i ¼ ðN Sz P xi þ P zi P xi1 þ N Sx P zi1 N Sz P xi1 N Sx P zi P xi P zi1 Þ=2 A3i ¼ ðN Sx P yi þ P xi P yi1 þ N Sy P xi1 N Sx P yi1 N Sy P xi P yi P xi1 Þ=2 and i1 = i + 1. 3.5.2. Actual deflection angle of the VGB The actual deflection angle of the VGB with respect to the top platformÕs pose [0, 0, l0 = l0S, h = hS, b = 0, 0]T is Gax aa ¼ cos1 qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi G2ax þ G2ay þ G2az
ð18Þ
where n P
Gax ¼ i¼1
n P
Ai Gxi Aa
;
Gay ¼
n P
Ai Gyi
i¼1
Aa
;
Gaz ¼ i¼1
Ai Gzi Aa
and 1 Gxi ¼ ðP xi þ P xi1 þ N Sx Þ; 3
1 Gyi ¼ þ ðP yi þ P yi1 þ N Sy Þ; 3
1 Gzi ¼ ðP zi þ P zi1 þ N Sz Þ 3
4. Corrected inverse kinematics 4.1. Deviations of the deflection angle and the outlet area Since the pose [0, 0, l0 = l0S, h = hS, b = 0, 0]T of the top platform is obtained based on the double semi-ellipses approximate distribution model, a deviation between the given outlet area An and the actual outlet area Aa will exist when the top platform takes the pose [0, 0, l0S, hS, 0, 0]T. Te deviation of the outlet area is n n X X oAi oAi oAi dAi ¼ dA1i þ dA2i þ dA3i DAn ¼ Aa An ¼ ð19Þ oA1i oA2i oA3i i¼1 i¼1
Y.-X. Wang, Y.-M. Wang / Mechanism and Machine Theory 40 (2005) 141–155
151
Due to the symmetry of the VGPM, when b = 0, we know dGax = 0. Then the deviation between the given deflection angle a and the actual given deflection angle aa with respect to the pose [0, 0, l0S, hS, 0, 0]T is Gay dGay þ Gaz dGaz Da ¼ aa a ¼ qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi ðG2ax þ G2ay þ G2az ÞðG2ay þ G2az Þ
ð20Þ
4.2. Correction of the pose Taking partial derivatives of functions g1i, g2i, g3i in Eq. (7) with respect to the joint variables mi, xi, di, h and l0, we get the increments dmi, dxi, ddi of the joint variables expressed as the functions of dh and dl0 8 > < dmi ¼ m11i dh þ m12i dl0 dxi ¼ m21i dh þ m22i dl0 ð21Þ > : ddi ¼ m31i dh þ m32i dl0 Substituting Eq. (21) into Eq. (7), and taking the total differential of the middle endpoint Pi with respect to dh and dl0, we obtain for the differential expression of Pi 2 3 qi11 qi12 6 7 dh dP i ¼ 4 qi21 qi22 5 ð22Þ dl0 qi31 qi32 Substituting Eq. (22) into Eq. (17), we obtain the differential expressions of A1i, A2i, A3i expressed by dh and dl0. Considering Eq. (22) and taking differentials of Gxi, Gyi, Gzi in Eq. (18) with respect to dh and dl0, one obtains 8 > < dGxi ¼ s11i dh þ s12i dl0 dGyi ¼ s21i dh þ s22i dl0 ð23Þ > : dGzi ¼ s31i dh þ s32i dl0 Taking the total differentials of Gax, Gay and Gaz in Eq. (18), we have 8 n P > > ðGyi Aa dAi þ Ai Aa dGyi Ai Gyi DAn Þ > > > i¼1 > > < dGay ¼ A2 a
n P > > > ðGzi Aa dAi þ Ai Aa dGzi Ai Gzi DAn Þ > > > i¼1 > dG ¼ : az A2a
where qikj, m11i, m12i, m21i, m22i, m31i, m32i, s11i, s12i, s21i, s22i, s31i, s32i are coefficients.
ð24Þ
152
Y.-X. Wang, Y.-M. Wang / Mechanism and Machine Theory 40 (2005) 141–155
Meanwhile, we take the strategy that the VGB must fulfill the given deflection angle. In this case, Da in Eq. (20) should be equal to zero. From Eqs. (20) and (24), we can then deduce the following corrected deviations of the pose of the top platform 8 d 11 d 23 d 21 DAn > > < dl0 ¼ d 11 d 12 d 12 d 21 ð25Þ > d 22 DAn d 12 d 23 > : dh ¼ d 11 d 12 d 12 d 21 where dij are given coefficients.
With the double semi - ellipses approximate distribution model discribing An1 ,α while β = 0 Through Eq. (3) - (4) and Eq. (8) - (14), or Eq. (15) determing the pose of the top platform based on the double semi - ellipses distibution model of the VGB
Solving nonlinear equations with Newton Raphson method and obtaining the appoximate pose of the top platform : l0 S and θ S
Based on the double semi-ellipse model to determine the approximate pose of the top platform
Input An , α , β
Determing the actual outlet area Aa and the actual deflecting angle α a while the top platform with the pose l0 S , θ S
as well as the deviation ∆α n between α and α a
Letting ∆α = 0, and solving the corrected increments of the pose of the top platform dl0 , dθ through Eq. (24) The corrected pose of the top platform is [0,0, l0 S + dl0 ,θ S + dθ , β = 0,0]T
While β ≠ 0, letting the deflecting plane turn β angle relative to the z axis, the corrected pose of
Adjusting the pose of the top platform to make up for the deviationsofthe approximate model
Computing the deviation ∆An between An and Aa
the top platform is [0,0, l0 S + dl0 ,θ S + dθ , β ,0]T
Calculation the compensated outlet area Ac and deflecting angle α c
Fig. 3. The flow chart solving the inverse kinematics problem of the VGPM.
Y.-X. Wang, Y.-M. Wang / Mechanism and Machine Theory 40 (2005) 141–155
153
The case b50 is the same as the case when the deflection plane rotates by an angle b relative to the z axis. Therefore, the corrected pose of the top platform of the Stewart platform at any deflection azimuth is ½0; 0; l0s þ dl0 ; hs þ dh; b; 0T
ð26Þ
To the corrected pose of the top platform, we can obtain the compensated outlet area Ac and deflection angle ac straightforwardly using equations similar to Eqs. (17) and (18).
Deviation of the outlet area (mm2 )
∆A 0
Corrected Model Ac
− 500 − 1000
− 1500
Approximate Model A a
− 2000
− 2500 0
5
10
15
20
25
Deflecting angle α (deg)
Fig. 4. Deviation of the outlet area DA.
Deviation of the Deflecting angle (deg)
∆α 1 .0 0.75
Approximate Model α a 0. 5 0.25 0 − 0.25 − 0. 5
Corrected Model α c 0
15 10 5 Deflecting angle α (deg)
20
Fig. 5. Deviation of the deflection angle Da.
25
154
Y.-X. Wang, Y.-M. Wang / Mechanism and Machine Theory 40 (2005) 141–155
5. Numerical results Fig. 3 shows the flow chart for solving the inverse kinematics problem of the VGPM. The variation of the outlet area deviation DAc between the given area An and the compensated outlet area Ac along with the deflection angle of VGB is shown in Fig. 4. For the approximate inverse kinematics model, the maximum relative error of the outlet area An1 is less than 2%, and the relative error of the outlet area An1 displays a decreasing tendency along with the increase of the deflection angle a of VGB. However, for the corrected inverse kinematics model, the relative error of the compensated outlet area is less than 0.1%. For the approximate inverse kinematics model, the deviation D a of the deflection angle has almost linear relationship with the deflection angle when the outlet area is given, as shown in Fig. 5. Comparatively, for the corrected inverse kinematics model, the error of the deviation Da of the VGB is less than 0.12%. During the derivation of Eq. (24), we have assumed that the deviation Da should be equal to zero. But in this figure we find it is not equal to zero. This kind of error may be caused by the linear approximation in Eq. (21), where we have only considered the linear increments dmi, dxi, ddi of the joint variables. Although this kind of simplification produces only an approximation, it has really enhanced the computing efficiency of the inverse kinematics problem of the VGPM.
6. Conclusion This paper presents an approach for solving the inverse kinematics problem of a variable geometry parallel manipulator (VGPM). In the VGPM, a Stewart platform is utilized as a driving mechanism, while a number of spatial RSRR kinematic chains guide rigid plates with spatial shaped inner enveloping surfaces to form a variable geometry body. The inverse kinematics problem of the VGPM consists in determining every actuator displacement of the Stewart platform when the distribution parameters of the VGB are given. In this paper, we present a universal method for solving the inverse kinematics problem of a VGPM. The method consists of four steps: (1) utilizing a double semi-ellipses distribution model to describe the approximate distribution of the VGB; (2) calculating the approximate pose of the top platform of Stewart platform based on the double semi-ellipses distribution model; (3) analyzing the mathematical deviations between the double semi-ellipses distribution model and the given parameters of the VGB; (4) making the compensation of these mathematical deviations. Through these four steps, we have solved the inverse kinematics problem of a VGPM with acceptable tolerance efficiently. Acknowledgement The authors gratefully acknowledge the support provided by the Teaching and Research Award Program for Outstanding Young Teachers in Higher Education Institutions of MOE, P.R. China (1999076). The authors also gratefully acknowledge the support provided by the Natural Science Foundation Committee, China (50375301).
Y.-X. Wang, Y.-M. Wang / Mechanism and Machine Theory 40 (2005) 141–155
155
Appendix A The expressions of joint variables with d1S, hS and l0S. 8 1 c04 > > < d0S ¼ tg c03 hS pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi c205 þ c206 c207 c c þ M c 06 07 0s 05 > 1 > m0S ¼ cos : 2 c05 þ c206 where c01 ¼ R4 þ R2 Shs L1 Chs l0s ;
c02 ¼ R2 Chs L1 Shs þ R4
c03 ¼ ðlBC lCD ÞCm0s H 1 Sm0s þ c01 ; c05 ¼ 2ðlBC lCD Þc02 2H 1 c01 ; 2
c07 ¼ ðlBC lCD Þ þ
H 21
þ
c201
þ
c04 ¼ ðlBC lCD ÞSm0s þ H 1 Cm0s þ c02
c06 ¼ 2ðlBC lCD Þc01 þ 2H 1 c02 c202
l2AB
8 1 c24 > > < d2S ¼ hS tg c23
pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi c225 þ c226 c227 c c þ M c > 26 27 2s 25 > : m2S ¼ cos1 2 c25 þ c226 where c21 ¼ R4 R2 Shs L1 Chs l0s ;
c22 ¼ R2 Chs L1 Shs R4
c23 ¼ ðlBC lCD ÞCm2s H 1 Sm2s þ c21 ; c25 ¼ 2ðlBC lCD Þc22 2H 1 c21 ;
c24 ¼ ðlBC lCD ÞSm2s H 1 Cm2s þ c22
c26 ¼ 2ðlBC lCD Þc21 2H 1 c22
2
c27 ¼ ðlBC lCD Þ þ H 21 þ c221 þ c222 l2AB 8 c17 > m1S ¼ cos1 > > c16 > < 8qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi9 < l2 S 2 hS ðlAB Cd1S þ lEF Þ2 = BC > 1 > > x ¼ cos > : 1S : ; l BC
where c15 = H1, c16 = lABCx1s + lCD, c17 = R4lABChsCd1sL1Chsl0s.
References [1] B. Dasgupta, T.S. Mruthyunjaya, The Stewart platform manipulator: a review, Mechanism and Machine Theory 35 (2000) 15–40. [2] D. Stewart, A platform with six degree of freedom, in: Proceedings of the conference on the Institution of Mechanical Engineering, London, 1965. [3] M. Griffis, J. Duffy, A forward displacement analysis of a class of Stewart Platforms, Journal of Robotic Systems 6 (6) (1989) 703–720.