A Virtual Work Based Algorithm for Solving Direct Dynamics Problem of a 3-RRP Spherical Parallel Manipulator

Transcription

1 J Intell Robot Syst (2011) 63:25 49 DOI /s A Virtual Work Based Algorithm for Solving Direct Dynamics Problem of a 3-RRP Spherical Parallel Manipulator Alireza Akbarzadeh Javad Enferadi Received: 29 March 2010 / Accepted: 23 September 2010 / Published online: 3 November 2010 Springer Science+Business Media B.V Abstract In this paper, we use principle of virtual work to obtain the direct dynamics analysis of a 3-RRP spherical parallel manipulator, also called spherical star-triangle (SST) manipulator (Enferadi et al., Robotica 27, 2009). This manipulator has good accuracy and relatively a large workspace which is free of singularities (Enferadi et al., Robotica, 2009). The direct kinematics problem of this manipulator has eight solution (Enferadi et al., Robotica 27, 2009). Given a desired actuated joint trajectories, we first present an algorithm for selecting the admissible solution. Next, direct velocity and direct acceleration analysis are obtained in invariant form. The concept of direct link Jacobian matrices is introduced. The direct link Jacobian matrix relates motion of any link to vector velocity of actuated joints. Finally, dynamical equations of the manipulator are obtained using the principle of virtual work and the concept of direct link Jacobian matrices. This method allows elimination of constraint forces and moments at the passive joints from motion equations. Two examples are presented and trajectory of moving platform are obtained. Results are verified using a commercial dynamics modeling package as well as inverse dynamics analysis (Enferadi et al., Nonlinear Dyn 63, 2010). Keywords Spherical parallel manipulator Direct link Jacobian matrix Direct dynamics Virtual work A. Akbarzadeh Center for Applied Research on Soft Computing and Intelligent Systems (CARSIS), Ferdowsi University of Mashhad, Mashhad, Iran Ali_Akbarzadeh_T@yahoo.com J. Enferadi (B) Department of Mechanical Engineering, Islamic Azad University-Mashhad Branch, Mashhad, Iran Javadenferadi@gmail.com

2 26 J Intell Robot Syst (2011) 63: Introduction Parallel manipulators are closed-loop mechanical chains, which generally have good performance in terms of accuracy, rigidity and ability to manipulate large loads. These mechanisms consist of two main bodies connected by several legs. One body is assumed to be fixed while the other is regarded as moveable and hence they are respectively called base and moving platform. Many industrial applications require orientating a rigid body around a fixed point such as: solar panels, space antenna and telescopic mechanisms, flight simulator mechanism, camera devices and orienting a tool or a workpiece in machine tools. A spherical manipulator is one in which the end-effector is moved on the surface of a sphere. In other words, the end-effector can rotate around any axis passing through a fixed point, center of sphere. Therefore, a spherical manipulator can be used as a device to orient the end-effector. Spherical manipulators can be either serial or parallel. Generally, a spherical parallel manipulator is composed of three legs connecting a moving platform (end-effector) to a fixed base. The end-effector has three degrees of freedom for rotation around a fixed point. The fixed point of rotation, denoted by O, is the center of spherical parallel manipulator. Several spherical parallel manipulators (SPMs) with different architectures have been reported in the literature, see for instance [4 7]. One typical SPM, called agile eye [7], is used for orienting cameras and antennas. The pioneering work of Gosselin and Angeles [7], an overconstrained mechanism with actuated revolute joints, has contributed significantly to the subsequent development of SPMs. Nowadays, the agile eye is a successful technical application of a SPM. However, it has a low lifting capacity. In Di Gregorio [5] a new SPM with RUU-type legs is introduced. Moreover, Di Gregorio [6] proposed a non-overconstrained SPM composed of three RRS-type legs. Also, Gallardo et al. [8] proposed a family of spherical parallel manipulators with a compact asymmetrical topology consisting of two legs and one spherical joint. While the kinematics has been studied extensively during the last two decades, fewer papers can be focused on the dynamics of parallel architectures. The dynamics analysis of parallel robots is usually implemented through analytical methods in classical mechanics [9] in which projection and resolution of equations on the reference axes are written in a large number of cumbersome and scalar relations. Furthermore, the solutions are rendered by large scale computation together with time consuming computer codes. Meanwhile, quite few special approaches have been conducted for dynamic modelling of parallel mechanism with special configurations. Sorli et al. [10] conducted the dynamics modelling for Turin parallel manipulator, though the mechanism has three identical legs, it has 6-DOFs. Geng et al. [11] developed Lagrange s equations of motion under some simplifying assumptions regarding the geometry and inertia distribution of the manipulator. Dasgupta and Mruthyunjaya [12] used the Newton-Euler approach to develop closed-form dynamic equations of Stewart platform, considering all dynamic and gravity effects as well as viscous friction at joints. The inverse dynamics algorithm solves the following problem; Given the desired trajectory of the end-effector as well as the mass distribution of each link, find actuator moments and/or forces required to generate this trajectory. The inverse dynamics is needed for

3 J Intell Robot Syst (2011) 63: Control: if one wants the robot to follow a specified trajectory, one has to convert the desired motion along that trajectory into joint forces that will generate this motion. Motion planning and optimization: when generating a desired motion for the robot end-effector, one can use the inverse dynamics of the robot to check whether the robot s actuators will be able to generate the joint forces needed to execute the trajectory. Further, one can examine the maximum speed the trajectory could be executed. The direct dynamics algorithm solves the following problem; Given the vector of initial actuated joint positions, vector of initial actuated joint velocities, applied actuated torques, applied external forces to end-effector, as well as the mass distribution of all links, find the resulting motion of end-effector. The direct dynamics is used for simulation and as feedforward in the robot s motion controller. The direct dynamics is used for simulation and as feedforward in the robot s motion controller. Therefore, under the assumption that all physical parameters in the dynamics model are accurate, the direct dynamics calculates what the robot does when specific joint torques are applied. Due to the closed-loop structure and kinematic constraints of parallel manipulators, the derivation of dynamic equations is quite complicated. There are three main formulation methods of the dynamical equations; namely, Newton-Euler laws [13, 14], the Euler-Lagrange formulation [15 17] and the principle of virtual work [18, 19]. The Newton-Euler formulation are obtained from the free-body diagrams. The approach is not suitable for motion simulation, as it finds the internal moments and forces that do not affect the motion of the system. The Euler-Lagrange equations results from the kinetic and potential energies of the system at hand. Euler-Lagrange equations give an independent set of equations of motion that is good for motion simulation, however, requires complex calculations for the partial derivatives. The principle of virtual work is the most efficient method for the dynamic analysis of parallel manipulators. This method allows elimination of all reaction forces and moments. Moreover, one can derive the equations of motions in terms of a set of independent generalized coordinates. In this paper, the principle of virtual work is employed for solving the direct dynamics of a 3-RRP spherical parallel manipulator. The manipulator allows the fixed base to be of any spherical triangular shape and the arcs making the moving spherical star (MSS), the end-effector, to have any angles between them. The concept of direct link Jacobian matrices is used to relate motion between any joints (active and/or passive) and actuators velocity vector. The method leads to a more compact form of the dynamical equations of motion. In what follows, first the geometry of the star-triangle spherical parallel manipulator is described [1]. An algorithm for selecting the admissible solution among eight possible solutions is presented. Next, the concept of direct link Jacobian matrices is introduced and the dynamics equations of motion are formulated. Two examples with different initial conditions and applied torques for actuators are presented and simulated. The resulting trajectory of MSS is calculated. An example is presented and results are verified with inputs of the inverse dynamics problem. A commercial dynamics modeling package is used to create a physical model of the manipulator. Finally, a commercial dynamic package

4 28 J Intell Robot Syst (2011) 63:25 49 is used to perform dynamic simulation and verify results of the derived analytical model. 2 Definition of Manipulator Structure The spherical star triangle (SST) parallel manipulator consists of a fixed spherical triangular base, P, and a moving platform which is shaped like a spherical star, S.The fixed base and the moving platform are connected via three legs. Each of the three moving legs is made of PRP (curved prismatic-revolute-curved prismatic) joints. The general model of this manipulator is depicted in Fig. 1. The first curved prismatic joint which is also the motorized joint moves along circular arc, P k P k+1, located on the surface of the sphere. In practice, it is difficult to manufacture an actuated curved prismatic joint which moves on a circular arc. However, by closer inspection, one can see that each of the motorized joints can also be viewed as a revolute joint with its axis passing through the origin of the sphere. See Figs. 2 and 3. Therefore, to physically construct this manipulator we will build its legs with RRP joints. The physical model of this manipulator is depicted in Fig. 2. To develop the mathematical model of the manipulator, first a sphere with center at O and a fixed spherical triangle, P 1 P 2 P 3,on its surface is considered. The unit vector v k is defined along OP k. Actuators stroke which can travel along the arc P k P k+1 are defined by angle γ k. See Figs. 1 and 3. The moving spherical star (MSS), S, is next considered. The star is made of three arcs which are located on a surface of a second sphere. The first and the second sphere have the same center but the radius of the second sphere is slightly larger due to intermediate revolute joint. This difference should be minimized in order to increase the structural stiffness of the manipulator. The three arcs of MSS intersect at point E. The angle between these arcs, α 1, α 2 and α 3 can be manually selected by the robot designer to obtain the desired performance. Position of point E defines end-effector position. Unit vector s is defined along OE. See Figs. 1 and 3. The arcs Fig. 1 General model of SST

5 J Intell Robot Syst (2011) 63: Fig. 2 Physical model of the SST manipulator with motors of the base triangle, P k P k+1, cross over the corresponding arcs of the moveable star platform ER k, at the point R k. Angular position of the actuators are defined by the unit vector r k. Direction of this unit vector is defined along OR k. Furthermore, R k is a joint which allows rotation about r k axis as well as a rotation about the axis that passes through center of sphere, O and is perpendicular to OER k plane. See Fig. 3. Fig. 3 Parameters description of kth leg

6 30 J Intell Robot Syst (2011) 63: Description of Notation and Coordinate Frames The following conventions are used for notations in this paper There are three legs, represented by k = 1,2,3. The trailing subscript of a vector, coordinate frame, matrix or a scalar quantity describes the corresponding number of leg. For example, r k describes angular position of the kth actuator. The leading superscript describes a vector, an axis, or a matrix with respect to a coordinate frame. For example, A r k describes angular position of the kth actuator with respect to coordinate frame {A}. Description of a vector without leading superscript means the vector is described in the fixed base coordinate frame {B}. For example, r k means B r k. The A B R k notation is a rotation matrix that describes rotation of coordinate frame {B} with respect to coordinate frame {A}. Next, we define coordinate frames that are required to solve inverse kinematics problem and link Jacobian matrices. These frames are used to show the relation between individual joints and the moving platform. For this purpose, we need to define a total of 11 coordinate frames which are made up: A fixed base coordinate frame, {B}, attached to fixed base. A moving coordinate frame, {E}, attached to end effector. A fixed, non moving, coordinate frame for each leg, {0k}, for k = 1,2,3. A moving coordinate frame for each leg, {1k}, for k = 1,2,3, attached to actuated joint. A moving coordinate frame for each leg, {2k}, for k = 1,2,3, attached to passive revolute joint. For example, ik r k where i = 0,1,2 and k = 1,2,3, is a unit vector which describes angular position of the kth actuator with respect to coordinate frame {ik}. Before we can define above coordinate frames, we need to define two unit vectors w k and t k. As stated before, the motion of the curved prismatic actuator can also be viewed as a revolution (revolute joint) with an axis that passes through origin of the sphere. This axis is defined by a unit vector, w k (Fig. 3). This unit vector is perpendicular to the plane OP k P k+1. Therefore w k = v k v k+1 v k v k+1 or w k = v k r k v k r k (1) The motion of the passive curved prismatic joint can also be viewed as a revolute joint with an axis that passes through the origin of the sphere. This axis is defined by a unit vector, t k (Fig. 3). This unit vector is perpendicular to the plane OBER k and passes through origin. Therefore t k = s r k s r k (2) We can now define 11 coordinate frames based on the unit vectors v k, s, w k and t k. Note that origins of all 11 coordinate frames are located at the center of the sphere.

7 J Intell Robot Syst (2011) 63: Fig. 4 The moving and base coordinate frames O u The first coordinate frame B(x,y,z) is attached to the fixed based and is called fixed base coordinate frame. The x and z axes of this frame are chosen along the unit vector v 1 and w 1, respectively. The y axis is chosen by the right hand rule. See Fig. 4. The second coordinate frame E(u,v,w) is attached to the moving spherical star (MSS). The w and v axes of this frame are chosen along s and t 1, respectively. The v axis is chosen by the right hand rule. See Fig. 4. The fixed, non moving, coordinate frame for each leg, {0k} = ( 0k x, 0k y, 0k z), is attached to the fixed base. The 0k x and 0k z axes of these coordinate frames are chosen along unit vectors v k and w k, respectively. The 0k y axis is chosen by the right hand rule. See Fig. 5. Fig. 5 Coordinate frame description of kth leg o r th motor

8 32 J Intell Robot Syst (2011) 63:25 49 The moving coordinate frames {1k} = ( 1k x, 1k y, 1k z) are attached to the actuated links, motors shaft. The 1k x and 1k z axes of these coordinate frames are chosen along unit vectors r k and w k, respectively. The 1k y axis is chosen by the right hand rule. See Fig. 5. The moving coordinate frames {2k} = ( 2k x, 2k y, 2k z) are attached to the revolute passive joints. To simplify mathematics, the 2k x axis is choosen to be along the rotation axis. Therefore, the 2k x and 2k z axes of these coordinate frames are chosen along the unit vectors r k and t k, respectively. The 2k y axis is chosen by the right hand rule. See Fig Kinematics Analysis As stated before, first coordinate frame B(x, y, z) is attached to the fixed based and the second coordinate frame E(u,v,w) is attached to the MSS. These coordinate frames as well as unit vectors defined in Section 3 will now allow us to describe the MSS orientation with respect to the fixed base. Using Z-Y-Z Euler angles, the rotation matrix is defined as B E R Z Y Z cos θ cos ϕ cos ψ sin θ sin ψ cos θ cos ϕ cos ψ sin θ sin ψ cos θ sin ψ = sin θ cos ϕ cos ψ + cos θ sin ψ sin θ cos ϕ sin ψ + cos θ cos ψ cos θ sin ψ (3) sin ϕ cos ψ sin ϕ sin ϕ cos ϕ Where angles θ, ϕ and ψ specify orientation of MSS. These angles are shown in Fig Selecting the Admissible Solution of the Direct Kinematics Analysis A solution to the direct kinematics problem is required for direct dynamic analysis. Therefore, direct kinematics problem of SST manipulator is the subject of this section. As shown in Fig. 4, the orientation of the MSS can be defined by the unit vector s and angle ψ (rotation angle about the unit vector s). Note that the unit vector s is defined by angles θ and ϕ. Therefore, the direct kinematics problem can be defined as: given actuator rotations, γ k, and other structural parameters of the SST manipulator, obtain orientation of the MSS (the unit vector s and angle ψ). The direct kinematics problem of the SST manipulator is solved by authors in [1]. The solution consists of obtaining the angles μ 1 and β 1 which together defined the orientation of the MSS. See Fig. 3. The direct kinematics problem of the SST manipulator has eight solutions (real and/or complex) [1]. Therefore, we have maximum of eight real values for the unit vector s and eight real values for the unit vector t 1. Given the physical structure of the manipulator, not all solutions can physically occur. Therefore, we must select the real solutions that are physically admissible. Referring to the Figs. 1 and3, two conditions are required and can be written as 1. β k β max, for example in isotropic design of SST manipulator, β max is equal to π Point E is located on base spherical triangle P 1 P 2 P 3.

9 J Intell Robot Syst (2011) 63: Fig. 6 Description of second condition The first condition can easily be checked by a computer program. For the second condition, consider a plane in space. See Fig. 6. The unit vector w 1 is defined normal to the plane 1. Also, consider an arbitrary unit vector, s,inspace.ifw T 1 s > 0,thenwe can conclude that the unit vector s is on top of the plane. Next, consider two arbitrary non-parallel planes in space. Two unit vectors w 1 and w 2 are defined normal to these planes. For any unit vector, s,inspace,ifw T 1 s > 0 and wt 2 s > 0, then we can conclude that the unit vector s is in top of the two planes or between the two planes. Finally, consider three planes OP 1 P 2,OP 2 P 3 and OP 3 P 1 of SST manipulator. These planes may be defined by three unit vectors w 1, w 2 and w 3. If point E is located on the base spherical triangle P 1 P 2 P 3, we must have w T k s > 0 fork = 1, 2, 3 (4) Therefore, we can determine the acceptable unit vector s among the candidates. Using this unit vector and Eq. 2, we can obtain the unit vector t 1. The rotation of MSS with respect to base coordinate frame can now be defined as follow B E R = [ uvw ] = [ t1 s t 1 s t 1 s This rotation matrix is equivalent with Eq. 3. Therefore, we can obtain the angles θ, φ and ψ as ϕ = cos 1 ( B E R (3, 3) ) 0 ϕ π (6) θ = Atan2 ( B E R (2, 3), B E R (1, 3)) 0 θ 2π (7) ψ = Atan2 ( B E R (3, 2), B E R (3, 1)) 0 ψ 2π (8) Where the B E R (m, n) represents mth row and nth column of the rotation matrix B E R. ] 3 3 (5)

10 34 J Intell Robot Syst (2011) 63: Direct Velocity Analysis The differential kinematics relations pertaining to parallel manipulators take on the form J 1 γ + J 2 ω = 0 (9) Where J 1 and J 2 are the direct kinematic and the inverse kinematic Jacobian matrices for the SST manipulator, respectively. Moreover, γ is the vector of actuator rates and ω is angular velocity of MSS in base coordinate frame. Referring to Fig. 3, the angular velocity ω of the end-effector defined in the base coordinate frame can be written as w k γ k r k μ k t k β k = ω fork = 1, 2, 3 (10) where β k is the equivalent angular velocity for the passive curved prismatic joint and μ k is angular velocity of the passive revolute joint. Note the two minus signs in Eq. 10 are because the rotations are defined opposite to positive rotation in righthand rule. The terms γ k, μ k and β k are scalar values and the three unit vectors w k, r k, t k represent their respective direction. Also note that μ k is the angle between planes OBER k and OP k P k+1, while β k is the angle between s and r k. The inner product of both sides of Eq. 10 with (r k t k ) leads to an equation free of passive joints rates, which simplifies to (r k t k ) T w k γ k (r k t k ) T ω = 0 (11) Equation 11,fork = 1,2,3, can be assembled and expressed in vector form as in Eq. 9. Therefore, we can define J 1 and J 2 as following c J 1 = 0 c 2 0 (12) 0 0 c 3 in which (r 1 t 1 ) T J 2 = (r 2 t 2 ) T (13) (r 3 t 3 ) T c k = (r k t k ) T w k fork = 1, 2, 3 (14) Therefore, we can write angular velocity of MSS using the differential kinematics Eq. 9 as ω = J 1 2 J 1 γ = J γ (15) Now, we obtain γ k, μ k and β k as function of angular velocity vector of actuated joints, γ.

11 J Intell Robot Syst (2011) 63: a) Angular velocity of the kth actuated joint, γ k The value of angular velocity for each actuator can be written as where γ k = d T k γ for k = 1, 2, 3 (16) d 1 = [1 0 0] T, d 2 = [0 1 0] T, d 3 = [0 0 1] T (17) b) Angular velocity of the kth passive revolute joint, μ k We can obtain μ k using inner product of both sides of Eq. 10 with r k. Therefore, we can write μ k = r T k ω for k = 1, 2, 3 (18) Substituting ω from Eq. 15 into above equation yields μ k = a T k γ for k = 1, 2, 3 (19) Where a k is a vector that can be written as a k = ( r T k J) T for k = 1, 2, 3 (20) c) Angular velocity of the kth passive curved prismatic joint, β k Finally, we can obtain β k using inner product of both sides of Eq. 10 with (w k r k ). Therefore, we can write β k = (w k r k ) T ω (21) (w k r k ) T t k Substituting ω from Eq. 15 into above equation yields β k = b T k γ for k = 1, 2, 3 (22) Where b k is a vector that can be written as ( ) b k = (w T k r k ) T J for k = 1, 2, 3 (23) (w k r k ) T t k Equations 16, 19 and 22 allows obtaining the γ k, β k and μ k when the angular velocity vector of the actuated joints are supplied. These variables will be used in the direct acceleration analysis and obtaining the direct link Jacobian matrices. 4.3 Direct Acceleration Analysis In this subsection, we relate the angular acceleration vector of actuators to the angular acceleration of the MSS and all joints. Consider Figs. 1 and 3. Taking derivative of Eq. 10, yields Note that w k γ k + ẇ k γ k r k μ k ṙ k μ k t k β k t k β k = ω for k = 1, 2, 3 (24) ẇ k = 0 (25)

12 36 J Intell Robot Syst (2011) 63:25 49 ṙ k = ( γ k w k r k ) (26) ṫ k = ( w k γ k r k μ k ) tk = ( w k γ k r k μ k t k β k ) tk = ω t k (27) Therefore, we can obtain the angular acceleration of MSS through any leg w k γ k r k μ k γ k μ k (w k r k ) t k β k β k (ω t k ) = ω for k = 1, 2, 3 (28) Where γ k is the angular acceleration of the actuator, μ k is angular acceleration of the passive revolute joint, β k is the equivalent angular acceleration for the passive curved prismatic joint and ω is the angular acceleration of the MSS in the base coordinate frame. Upon multiplication of two sides of Eq. 28 by (r k t k ) T and eliminating angular acceleration of the passive joints, μ k and β k,eq.28 canberewrittenas (r k t k ) T w k γ k (r k t k ) T ω γ k μ k (r k t k ) T (w k r k ) β k (r k t k ) T (ω t k )=0 (29) Substituting μ k and β k from Eqs. 19 and 22 into the above equation, we can write (r k t k ) T w k γ k (r k t k ) T ω γ k ( a T k γ ) (r k t k ) T (w k r k ) ( b T k γ) (r k t k ) T (ω t k ) = 0 (30) Furthermore, we can write (r k t k ) T (ω t k ) = ω T (t k (r k t k )) = ω T ( r k ( t T k t k ) tk ( t T k r k )) = ω T r k = t T k ω (31) (r k t k ) T (w k r k ) = w T k (r k (r k t k )) = w T ( ( ) ( )) k rk r T k t k tk r T k r k = t T k w k (32) Substituting above equations into Eq. 30,wecanwrite (r k t k ) T w k γ k (r k t k ) T ω γ k ( a T k γ )( t T k w k) + ( b T k γ )( r T k ω) = 0 (33) Substituting Eq. 15 into above equation, will yield (r k t k ) T w k γ k (r k t k ) T ω γ k ( a T k γ )( t T k w k) + ( b T k γ )( r T k J γ) = 0 (34) For k = 1,2,3, the above equation can be written in the matrix form as J 1 γ + J 2 ω + M γ + n = 0 (35) In which matrices J 1 and J 2 were defined by Eqs. 12 and 13, respectively. The matrix M and the vector n are defined as m M = 0 m 2 0 (36) 0 0 m 3 n 1 n = n 2 (37) n 3

13 J Intell Robot Syst (2011) 63: Where m k = ( a T k γ)( t T k w ) k for k = 1, 2, 3 (38) n k = ( b T k γ)( r T k J γ) for k = 1, 2, 3 (39) In the direct acceleration analysis, the angular velocity vector and angular acceleration vector of the actuated joints are known. Therefore, the angular acceleration of MSS can be obtained as follows ω = J γ J 1 2 (M γ + n) (40) In what follows, we will obtain relations between scalar values of the angular acceleration of any actuator, ÿ k any passive revolute joint, μ k and any equivalent passive curved prismatic joints, β k, with vectors γ, γ and γ. Note that the value of β k is not needed in dynamics equations, however its derivation is shown here for completeness. The scalar values of μ k and γ k will later be used in the dynamics equations. a) Angular acceleration of the kth actuated joint, γ k The angular acceleration value for each actuator can be written as γ k = d T k γ for k = 1, 2, 3 (41) Where d T k is defined in Eq. 17. The above equation provides angular acceleration of any actuator as a function of vectors γ. b) Angular acceleration of the kth passive revolute joint, μ k To obtain μ k, we product both sides of Eq. 28 with r T k. Therefore, we can write μ k = β k r T k (ω t k) r T k ω for k = 1, 2, 3 (42) or μ k = β k (t k r k ) T ω r T k ω for k = 1, 2, 3 (43) Substituting β k, ω and ω from Eqs. 22, 15 and 40 into the above equation, we can write where f k (γ, γ) is a scalar function, as μ k = ( r T k J) γ + f k (γ, γ) (44) f k (γ, γ) = ( b T k γ) (t k r k ) T (J γ) + r T k J 1 2 (M γ + n) for k = 1, 2, 3 (45) Equation 44 provides angular acceleration of any passive revolute joint as a function of vectors γ, γ and γ. c) Angular acceleration of the kth passive curved prismatic joint, β k To obtain β k, we product both sides of Eq. 28 with t T k. Therefore, we can write β k = t T k w k γ k γ k μ k t T k (w k r k ) t T k ω for k = 1, 2, 3 (46)

14 38 J Intell Robot Syst (2011) 63:25 49 Substituting γ k, μ k, ω and γ k from Eq. 16, 19, 40 and 41 into the above equation, we can write β k = u T k (γ) γ + u k (γ, γ) (47) where u k (γ) andu k (γ, γ) are vector and scalar functions, respectively. These two functions can be written as u T k (γ) = ( tt k wk d T k J) (48) u k (γ, γ) = t T k 4.4 Direct Link Jacobian Matrices ( J 1 2 (M γ + n) (w k r k ) ( d T k γ)( a T k γ)) (49) In this subsection, we introduce a new concept called direct link Jacobian matrix and will apply it in the solution process of the SST direct dynamics problem. To the best of authers knowledge this is first time this concept is used. The SST manipulator has three actuated links, three intermediate passive links and one MSS. Finding relations between motion of actuated joints and motion of all passive and actuated links is subject of this subsection. These relations will be used when solving direct dynamics problem utilizing virtual work method. The SST manipulator will have a total of six direct link Jacobian matrix. Three of which, will relate angular velocity vector of the actuated joints to angular velocity vector of any actuated link in the coordinate frame attached to it, {1k}. The other three will relate angular velocity vector of the actuated joints to angular velocity vector of any intermediate passive link in the coordinate frame attached to it, {2k}. To find direct link Jacobian matrix, we can use the coordinate frames described in Section 3. We previously defined the fixed coordinate frames for each leg as {0k}. Therefore, rotation matrix defining these coordinate frames with respect to base frame, {B}, can be defined by writing the axes of the frames {0k} in the base coordinate frame as [ ] B 0k R = w k v k v k w k v k w k for k = 1, 2, 3 (50) The coordinate frames {1k} are attached to the actuated joints. For each leg, a rotation matrix describing orientation of frame {1k} with respect to frame {0k} can be written as 0k 1k R = Rot ( 0k ) ( z, γ k = Rot 0k ) w k, γ k for k = 1, 2, 3 (51) The coordinate frames {2k} are attached to the passive revolute joints. Therefore, rotation matrix describing orientation of these coordinate frames with respect to coordinate frames {1k} can be written as 1k 2k R = Rot ( 1k ) ( x, μ k = Rot 1k ) r k, μ k for k = 1, 2, 3 (52) Now, we compute direct link Jacobian matrices of actuated links and intermediate passive links. a) Direct link Jacobian matrices of actuated links

15 J Intell Robot Syst (2011) 63: Consider Fig. 3, we will define angular velocity vector of the kth actuator in coordinate frame that is attached to it, ă 1k ω a,as 1k ω a = γ k 1k w k for k = 1, 2, 3 (53) By replacing the scalar value of any actuator angular velocity, γ k, with its equivalent inner product of two vectors, Eq. 16, the above equation can be written as where therefore where 1k ω a = 1k w k d T k γ for k = 1, 2, 3 (54) 1k w k = ( 0k 1k R ) T ( B0k R ) T wk for k = 1, 2, 3 (55) 1k ω a = 1k J a γ for k = 1, 2, 3 (56) 1k J a = ( 0k 1k R ) T ( B0k R ) T wk d T k for k = 1, 2, 3 (57) is a 3 3 matrix called the direct link Jacobian matrix of the kth actuator. This equation allows us to calculate angular velocity vector of the kth actuator link when the angular velocity vector of the actuated joints is given. b) Direct link Jacobian matrices of intermediate passive link Consider Fig. 3, we can write the angular velocity vector of the kth intermediate passive link (ipl) in coordinate frame that is attached to it as 2k ω ipl = 2k 1k R1k ω a μ 2k k r k for k = 1, 2, 3 (58) By replacing the scalar value of any intermediate passive revolute joint angular velocity, μ k, with its equivalent inner product of two vectors, Eq. 19, and substituting 1k ω a from Eq. 56 into the above equation will lead to 2k ω ipl = ( 2k 1k R 1k J a 2k r k a T ) k γ for k = 1, 2, 3 (59) Substituting Eq. 57 into above equation and simplifying will lead to 2k ω ipl = 2k J ipl γ for k = 1, 2, 3 (60) where 2k J ipl = 2k B R ( w k d T k r ka T ) k for k = 1, 2, 3 (61) 2k B R = ( 1k 2k R ) T ( 0k 1k R ) T ( B0k R ) T (62) Note that 2k J ipl is a 3 3 matrix and is called the link Jacobian matrix of the kth intermediate passive link. This equation allows us to compute angular velocity of the kth intermediate passive link when the angular velocity vector of the actuated joints is given.

16 40 J Intell Robot Syst (2011) 63: Dynamics The direct dynamics problem aims to find the response of a robot arm corresponding to given applied moments and/or forces. That is, given the vector of joint moments or forces, it computes the resulting motion of the manipulator as a function of time. For simplicity, we will assume that frictional forces at the joints are negligible. There are three main methods that may be utilized in order to compute the actuated torques. The first one uses the Newton-Euler classic procedure, the second uses the Lagrange s equations and the third approach is based on the principle of virtual work. Unlike serial manipulators, parallel manipulators always contain passive joints. Among these methods, virtual work allows elimination of internal forces and moments at the passive joints from motion equations. Therefore, virtual work method can offer certain advantages. The principle of virtual work states that a mechanism is under dynamic equilibrium if and only if the virtual work developed by all external, internal and inertia forces vanish during any virtual displacement that is compatible with the kinematics constraints. 5.1 Applied Torques and Inertia In the direct dynamics problem, the vector of initial actuated joint positions and vector of initial actuated joint velocities are given. Therefore, we need to formulate the dynamics equations in terms of the γ, γ, γ which are the actuator angle, rate, and acceleration vectors. The resultant applied torque on motors comes from Resultant torque due to applied external moment exerted at the point E and inertia of the MSS Resultant torque due to inertia of the actuated links Resultant torque due to inertia of the intermediate passive links (ipl) In this subsection, we obtain these resultant torques as function of angular position vector, γ, angular velocity vector, γ, and angular acceleration vector, γ,ofthe actuators. a) Resultant torque due to applied external moment and inertia of the MSS The resultant applied torques due to moment exerted at point E and inertia of the MSS in the base coordinate frame can be written as n e = n ext I s ω ω (I s ω) (63) where n ext is the external moment exerted at the point E and I s is the inertia matrix of MSS with respect to the base coordinate frame,{b}, and is defined as I s = B E RE I s E B R (64) where E I S is the inertia matrix of the MSS with respect to moving coordinate frame, {E}, attached to MSS. Substituting ω and ω from Eqs. 15 and 40 into Eq. 63, wecan write n e = Q (γ ) Rγ + q (γ, γ) (65) where Q(γ) isa3 3matrixandq (γ, γ) is a 3 1 vector. The matrix Q(γ )isa function of actuators angular position vector and the vector q (γ, γ) is a function

17 J Intell Robot Syst (2011) 63: of actuators angular position and velocity vectors. These two functions can be written as Q (γ ) = I s J (66) q (γ, γ) = I s J 1 2 (M γ + n) (J γ) (I s J γ) + n ext (67) b) The resultant torques due to inertia of the actuated links The inertia as result of motion of the actuated links will produce moment that can be expressed as 1k n a = 1k I a 1k ω a 1k ω a ( 1k I a 1k ω a ) for k = 1, 2, 3 (68) where Substituting Eq. 41 into above equation will lead to where E k is a 3 3 matrix and can be written as 1k ω a = γ k 1kw k for k = 1, 2, 3 (69) 1k ω a = E k γ for k = 1, 2, 3 (70) E k = 1k w k d T k for k = 1, 2, 3 (71) We can now obtain 1k n a by substituting Eqs. 56 and 70 into Eq. 68. Therefore 1k n a = L k (γ) γ + l k (γ, γ) for k = 1, 2, 3 (72) Where L k (γ )isa3 3matrixandl k (γ, γ) is a 3 1 vector. These can be defined as L k (γ) = 1k I a E k for k = 1, 2, 3 (73) l k (γ, γ) = ( 1k J a γ ) ( 1k I a 1k J a γ ) for k = 1, 2, 3 (74) c) The resultant torques due to inertia of the intermediate passive links The inertia as result of motion of the intermediate passive links will produce moment that can be expressed as 2k n ipl = 2k I ipl 2k ω ipl 2k ω ipl ( 2k I ipl 2k ω ipl ) for k = 1, 2, 3 (75) where 2k ω ipl = 2k 1k R1k ω a ( 2k 1k R 1k ) ( ω a 2k ) μk r k 2k μk r k for k = 1, 2, 3 (76) Substituting Eqs. 19, 44, 56 and 70 into above equation and simplifying will lead to 2k ω ipl = P k (γ) γ + p k (γ, γ) for k = 1, 2, 3 (77) where P k (γ)isa3 3matrixandp k (γ, γ) is a 3 3 vector. These can be defined as

18 42 J Intell Robot Syst (2011) 63:25 49 P k = 2k 1k RE k + 2k r k ( r T k J ) for k = 1, 2, 3 (78) p k = ( 2k 1k R 1k J a γ ) ( 2k r k a T k γ) f k (γ, γ) 2k r k for k = 1, 2, 3 (79) We can now obtain 2k n ipl by substituting Eqs. 60 and 77 into Eq. 75. Therefore 2k n ipl = K k (γ) γ + k k (γ, γ) for k = 1, 2, 03 (80) where K k (γ)isa3 3matrixandk k (γ, γ) is a 3 3 vector. These can be defined as K k = 2k I ipl P k (γ) for k = 1, 2, 3 (81) k k = 2k I ipl p k (γ, γ) ( 2k J ipl γ ) ( 2k I ipl 2k J ipl γ ) for k = 1, 2, 3 (82) 5.2 Equation of Motions Equation of motion using principle of virtual work can be stated as (δγ) T τ + (δλ s ) T n e + 3 ( (δ 1k ) T λ 1k a n a + ( δ 2k ) T ) λ 2k ipl n ipl = 0 (83) k=1 where δγ and δγ s are virtual rotation vectors of the actuated joints and MSS defined in the base coordinate frame {B}. Additionally, δ 1k λ a and δ 2k λ ipl are virtual rotation vectors of kth actuated links and kth intermediate passive links with respect to coordinate frame that is attached to them. τ is a vector that represents the three actuated torques. The axis of each actuator torque is defined along the unit vector w k which itself is defined in the base coordinate frame, {B}, see Fig. 5. Therefore, the directions of the three actuator torques are pre-defined in the base frame and will not need any transformations. The virtual rotations in equation of motion (Eq. 83) must be compatible with the kinematics constraints imposed by both active and passive joints. Therefore, it is necessary to relate the above virtual displacements to a set of independent generalized virtual displacements. Using Eqs. 15, 56 and 60,wecanwrite λ s = ω Eq. 15 δλ s = Jδγ (84) 1k λ a = 1k ω a Eq. 56 δ 1k λ a = 1k J a δγ (85) 2k λ ipl = 2k ω ipl Eq. 60 δ 2k λ ipl = 2k J ipl δγ (86) Substituting above equations into Eq. 83 will yield (δγ) T (τ + J T n e + 3 ( (1k ) T J 1k a n a + ( 2k ) T ) ) J 2k ipl n ipl = 0 (87) k=1

19 J Intell Robot Syst (2011) 63: Since Eq. 87 is valid for any δγ, it follows that τ + J T n e + 3 ( (1k ) T J 1k a n a + ( 2k ) T ) J 2k ipl n ipl = 0 (88) k=1 Substituting n e, 1k n a and 2k n ipl from Eqs. 65, 72 and 80 into above equation will result in dynamics equation in terms of rotation, angular velocity and angular acceleration vectors of the actuators. τ + J T Q (γ) γ + J T q (γ, γ) + 3 ( (1k ) T J a (Lk (γ) γ + l k (γ, γ)) + ( 2k ) T J ipl (Kk (γ) γ + k k (γ, γ))) = 0 (89) k=1 The above dynamics equation of SST manipulator can be expressed in the more familiar format as T 1 (γ) γ + T 2 (γ, γ) + τ = 0 (90) where τ is the 3 1 vector of applied actuated torques, T 1 (γ)isthe3 3 manipulator inertia matrix, T 2 (γ, γ) is the 3 1 vector of centrifugal and Coriolis terms. Additionally, γ, γ, γ are the 3 1 actuator angle, rate, and acceleration vectors, respectively. Therefore, T 1 (γ)andt 2 (γ, γ) are defined as 3 ( (1k T 1 (γ ) = J T ) T Q (γ ) + J a Lk (γ) + ( 2k ) T J ipl Kk (γ)) k=1 (91) 3 ( (1k T 2 (γ, γ) = J T ) T q (γ, γ) + J a lk (γ, γ) + ( 2k ) T J ipl kk (γ, γ)) k=1 (92) The procedure for solving direct dynamics solution of the SST manipulator is represented in a flow chart in Fig Numerical Examples As stated before, the solution outlined in this paper applies to a general model of SST manipulator. See Fig. 7. For simplicity and ease of illustration, we choose isotropic design of SST manipulator [1]. Based on the previoussection, a computer programis developed using MATLAB software. Two examples with different initial conditions and applied torques for actuators are simulated and trajectory of MSS is calculated. Results are verified in two ways. First, using inverse dynamics problem, a trajectory for the MSS is supplied and required motor torques as well as angular position of actuators as function of time are calculated [3]. Therefore, the initial conditions of actuator angular positions and velocities can be calculated. If these initial conditions along with torque trajectory, output of the inverse dynamics problem, are supplied to the direct dynamics problem then the same trajectory for MSS must be obtained. Secondly, results are also verified using a commercial dynamics modeling software.

20 44 J Intell Robot Syst (2011) 63:25 49 Direct Dynamics Inputs Applied torques to the actuators Initial angular velocity of the actuated joint Initial position of the actuated joints Direct kinematics problem Selecting admissible solution Direct velocity analysis Direct acceleration analysis Obtaining direct link Jacobian matrices Direct dynamics equations of the manipulator Direct Dynamics Outputs Fig. 7 Calculation of the direct dynamics of the SST manipulator 6.1 Specification of the SST Manipulator Consider Figs. 1 and 3. We assume the followings. a) Architecture parameters for fixed base: Radius is 0.35 m Planes OP 1 P 2,OP 2 P 3 and OP 3 P 1 of SST manipulator are located in x y, y z and z xof base coordinate frame, respectively. Therefore v 1 = [ 100 ] T, v 2 = [ 010 ] T, v 3 = [ 001 ] T

21 J Intell Robot Syst (2011) 63: Fig. 8 Position of the end-effector (point E) b) Architecture parameters for MSS: Radius is 0.4 m Angle between planes OER k and OER k+1 is 120 degree. Therefore c) Mass properties of MSS α 1 = α 2 = α 3 = 120 m s = 2.6 kg E I s = kg.m Fig. 9 The ψ angle (rotation about the unit vector s) versus time

22 46 J Intell Robot Syst (2011) 63:25 49 Fig. 10 Three-dimensional trajectory of point E d) Equal mass properties for each actuator link, k = 1,2,3 m a = 0.3 kg k I a = kg.m e) Equal mass properties for each intermediate passive link, k = 1,2,3 m ipl = 0.06 kg k I ipl = kg.m Fig. 11 Trajectory of point E versus time

23 J Intell Robot Syst (2011) 63: Fig. 12 The θ angle versus time 6.2 Input Torques and Initial Conditions Example 1 For the first simulation, the input torques and initial conditions of actuators are specified as γ k (0) = π rad for k = 1, 2, 3 4 γ k (0) = rad for k = 1, 2, 3 sec τ k (t) = 1.13 sin (12t) Nm for k = 1, 2, 3 where 0 t π / 6. This implies that position of point E, defining the endeffector position remains fixed in center of the base spherical triangle while MSS rotates about the unit vector s in a sinusoidal fashion. Also note that in specifying architecture parameters of this example, we intentionally choose parameters that resulted in a symmetrical base and symmetrical MSS. Motion parameters of MSS are calculated as function of time and are plotted in Figs. 8 and 9. To verify our result, the output of the inverse dynamics problem given in [3] is used. Results of the direct Fig. 13 The ϕ angle versus time

24 48 J Intell Robot Syst (2011) 63:25 49 Fig. 14 The ψ angle versus time dynamics problem presented in this example is confirmed with the input of inverse dynamics problem given in [3]. Example 2 For the second simulation, the input torques and initial conditions of actuators are specified as follow γ k = π rad 4 for k = 1, 2, 3 γ k = 0 for k = 1, 2, 3 τ 1 = 4sin(18t), τ 1 = 5cos(12t), τ 1 = 3cos(24t) Nm where 0 t 1. The MSS trajectory is calculated as function of time. Additionally, this example is simulated using a dynamics modeling commercial software. Results of the simulation is plotted in Figs. 10, 11, 12, 13, and14. As shown the results of the analytical and commercial software are very close. These results verify the correctness of our mathematical model. 7 Conclusion The need to study control strategies and perform simulation has motivated us to study direct dynamics problem of the 3-RRP spherical parallel manipulator. We contribute by presenting a methodology for solving direct dynamics problem of the manipulator using the principle of virtual work, as well as introducing the concept of direct link Jacobian matrix. Using the principle of virtual work, the constraint forces in passive joints are eliminated. The methodology involves four basic steps. First, we present an algorithm for selecting the admissible solution among possible direct kinematics solutions. Next, direct velocity and direct acceleration analysis using invariant form is performed. Velocity and acceleration of the passive joints are obtained. In the third step, concept of direct link Jacobian matrix is introduced and formulated using invariant form. In the forth step, dynamics analysis is presented. To demonstrate the methodology, two numerical examples are presented. Results are verified in two ways by a commercial dynamics modeling package as well as inverse dynamics problem. The study presented in this paper provides a framework for our

25 J Intell Robot Syst (2011) 63: future research in the areas of SST manipulator control. Furthermore, the presented solution methodology may be applied to some other spherical parallel manipulators. Acknowledgement The authors would like to thank Ferdowsi University of Mashhad for their financial support of this research project. References 1. Enferadi, J., Akbarzadeh Tootoonchi, A.: A novel spherical parallel manipulator: forward position problem, singularity analysis and isotropy design. Robotica 27, (2009) 2. Enferadi, J., Akbarzadeh Tootoonchi, A.: Accuracy and stiffness analysis of a 3-RRP spherical parallel manipulator. Robotica, published online by Cambridge University Press, 4 March Enferadi, J., Akbarzadeh Tootoonchi, A.: Inverse dynamics analysis of a general spherical startriangle parallel manipulator using principle of virtual work. Nonlinear Dyn. 63, (2010) 4. Alizade, R.I., Tagiyev, N.R., Duffy, J.: A forward and reverse displacement analysis of an inparallel spherical manipulator. Mech. Mach. Theory 29(1), (1994) 5. Di Gregorio, R.: A new parallel wrist using only revolute pairs: the 3-RUU wrist. Robotica 19, (2001) 6. Di Gregorio, R.: A new family of spherical parallel manipulators. Robotica 20, (2002) 7. Gosselin, C.M., Angeles J.: The optimum kinematic design of a spherical three-degree-offreedom parallel manipulator. ASME J. Mech. Transm. Autom. Des. 111(2), (1989) 8. Gallardo, J., Rodriguez, R., Caudillo, M., Rico, J.: A family of spherical parallel manipulators with two legs. Mech. Mach. Theory 43, (2008) 9. Li, Y-W., Wang, J., Wang, L-P., Liu, X-J.: Inverse dynamics and simulation of a 3-DOF spatial parallel manipulator. In: Proceedings of the IEEE International Conference on Robotics & Automation ICRA 2003, pp Taipei, Taiwan (2003) 10. Sorli, M., Ferarresi, C., Kolarski, M., Borovac, B., Vucobratovic, M.: Mechanics of Turin parallel robot. Mech. Mach. Theory 32(1), (1997) 11. Geng, Z., Haynes, L.S, Lee, J.D., Carroll, R.L.: On the dynamic model and kinematic analysis of a class of Stewart platforms. Robot. Auton. Syst. 9, (1992) 12. Dasgupta, B., Mruthyunjaya, T.S.: A Newton-Euler formulation for the inverse dynamics of the Stewart platform manipulator. Mech. Mach. Theory 34, (1998) 13. Dasgupta, B., Mruthyunjaya, T.S.: Closed-form dynamic equations of the general Stewart platform through the Newton Euler approach. Mech. Mach. Theory 33(7), (1998) 14. Riebe, S., Ulbrich, H.: Modeling and online computation of the dynamics of a parallel kinematic with six degrees-of-freedom. Arch. Appl. Mech. 72(11 12), (2003) 15. Bhattacharya, S., Nenchev, D.N., Uchiyama, M.: A recursive formula for the inverse of the inertia matrix of a parallel manipulator. Mech. Mach. Theory 33(7), (1998) 16. Leroy, N., Kokosy, A.M., Perruquetti, W.: Dynamic modeling of a parallel robot. Application to a surgical simulator. Proc. IEEE Int. Conf. Robot. Autom. 3, (2003) 17. Lee, K.M., Shah, D.K.: Dynamic analysis of a three-degrees-of-freedom in-parallel actuated manipulator. IEEE J. Robot. Autom. 4(3), (1988) 18. Staicu, S., Carp-Ciocardia, D.C.: Dynamic analysis of Clavel s delta parallel robot. Proc. IEEE Int. Conf. Robot. Autom. 3, (2003) 19. Khalil, W., Ibrahim, O.: General solution for the dynamic modeling of parallel robots. Proc. IEEE Int. Conf. Robot. Autom. 4, (2004)