Special Issue Article
Dynamic modeling of SCARA robot based on Udwadia–Kalaba theory
Advances in Mechanical Engineering 2017, Vol. 9(10) 1–12 Ó The Author(s) 2017 DOI: 10.1177/1687814017728450 journals.sagepub.com/home/ade
Yaru Xu and Rong Liu
Abstract With the aim of dynamic modeling of the SCARA robot subjected to space trajectory constraint to satisfy repetitive tasks with higher accuracy, a succinct and explicit equation of motion based on Udwadia–Kalaba theory is established. The trajectory constraint, which is regarded as the external constraints imposed on the system, is integrated into the dynamic modeling of the system dexterously. The explicit expression of constraint torques required to satisfy constraints and explicit dynamic equation of the system without Lagrange multiplier are obtained. However, constraint violation arises when the initial conditions are incompatible with the constraint equations. Baumgarte stabilization method is considered for constraint violation suppression. Simulations of the varying law of the generalized coordinate variables and the trajectories of the SCARA robot are performed to demonstrate the simplicity and accuracy of the method. Keywords SCARA robot, constraint, dynamic modeling, Udwadia–Kalaba theory, Baumgarte stabilization method
Date received: 4 April 2017; accepted: 26 July 2017 Academic Editor: Kai Bao
Introduction As a nonlinear dynamic system, the SCARA robot with four joints (e.g. three revolute and one prismatic) has better repeatability of robot movements and higher accuracy in their performance. The SCARA robot is required to follow the desired trajectory for many applications during the current stage of industrial development for productivity enhancement and quality improvement in manufactured products, such as pick and place, assembly, and packaging. In fact, dynamic modeling of the SCARA robot to realize the desired trajectory is within the dynamic modeling of constrained multibody system. One of the key issues to address in multibody dynamics is to obtain explicit equations of motion for constrained dynamical systems.1 Constrained dynamical systems are conventional model based on Lagrangian formulation in which Lagrangian multiplier is determined relied on problemspecific approaches.2 When the number of the constraint equations is not equal to the number of generalized variables, the traditional Lagrangian formulation is
invalidated. Moreover, there are many alternative approaches available to model constrained dynamical systems, which also need auxiliary variables. For example, Hamel’s equations require Quasi Velocity as auxiliary term; Kane’s3 equations require a set of PseudoGeneralized Speed as auxiliary terms; Gibbs–Appell formulation requires Pseudo-Velocity and PseudoAcceleration as auxiliary terms;4 and Poincare´ equations require a set of Pseudo-Velocity as auxiliary terms. It is worth noting that only the numerical solutions can be obtained by all of the aforementioned methods. An explicit equation of motion for constrained systems, called the Udwadia–Kalaba equation, was proposed by Firdaus E. Udwadia and Robert E. Kalaba5 in 1992, which opens up a new way of modeling Institute of Robotics, Beihang University, Beijing, China Corresponding author: Yaru Xu, Institute of Robotics, Beihang University, Xueyuan Road No. 37, Haidian District, Beijing 100191, China. Email:
[email protected]
Creative Commons CC-BY: This article is distributed under the terms of the Creative Commons Attribution 4.0 License (http://www.creativecommons.org/licenses/by/4.0/) which permits any use, reproduction and distribution of the work without further permission provided the original work is attributed as specified on the SAGE and Open Access pages (https://us.sagepub.com/en-us/nam/ open-access-at-sage).
2 complex multibody systems. This method provides a general structure for the explicit equations of motion for mechanical systems subjected to holonomic and nonholonomic equality constraints.6,7 Moreover, it is not an issue for Udwadia–Kalaba equation based on the Gauss’ principle of least constraint, and the optimal value of the generalized variables solution can still be obtained by the Moore–Penrose generalized inverse when the number of the constraint equations is not equal to the number of generalized variables. Udwadia and colleagues8–11 also effectively dealt with the dynamics and control of nonlinear uncertain systems to benefit the basic research and further application of this method. It has been studied in some application fields, such as industrial robot,12 parallel robot,13 flexible multibody systems,14 railway vehicles collision,15 machine fish,1 control of tethered satellites,16 rigid multibody systems,17 and mobile robots.18 Like many other robots, the SCARA robot is often required to achieve some specified space trajectories (i.e. trajectory constraint). Only a few scholars focus on the explicit dynamical equations for the SCARA robot subjected to constraints. In this article, a succinct and explicit equation of motion based on the Udwadia– Kalaba theory for the SCARA robot is established. However, constraint violation arises when the initial conditions are incompatible with the constraint equations. Due to the simplicity and easiness for computational implementation, Baumgarte stabilization method, proposed by Baumgarte19 and then developed in numerous contributions, is probably the most popular and attractive technique to implement the control of constraint violation.20 Therefore, Baumgarte stabilization method is considered for constraint violation suppression. The comparison of simulation results of the dynamic modeling shows that the derived equations can successfully address the dynamic modeling for the SCARA robot subjected to trajectory constraints to satisfy repetitive tasks with higher accuracy. Finally, conclusions of this work and future tasks are given.
Udwadia–Kalaba theory The explicit equation of motion for a constrained dynamical system proposed by Udwadia and Kalaba5 is derived in this section using a systematic general threestep procedure: (1) consideration of the so-called unconstrained system of equations obtained using Newtonian or Lagrangian mechanics, (2) description of the constraints required to model the given constrained system, and (3) expression of the constraint torques for system to satisfy the given constraints.
Unconstrained multibody dynamics Considering an unconstrained multibody system characterized by n generalized coordinates q :¼
Advances in Mechanical Engineering ½q1 q2 qn T 2 Rn , the dynamics of the system is described as _ t) M(q, t)€q = Q(q, q,
ð1Þ
n3n
denotes a symmetric positivein which M(q, t) 2 R definite mass matrix of the system related to q, t; _ t) 2 Rn collects the normal and Coriolis inertial Q(q, q, _ t; q 2 Rn is terms and the applied forces related to q, q, n the generalized coordinate n-vector; q_ 2 R is the generalized velocity; and €q 2 Rn is the generalized acceleration. The generalized acceleration of the unconstrained _ t), is of the form dynamical system, defined as a(q, q, _ t) _ t) :¼ M1 (q, t)Q(q, q, a(q, q,
ð2Þ
Constraint description It is assumed that the system is subjected to p sufficiently smooth control requirements as constraints, which include all the usual varieties of holonomic and/ or nonholonomic constraints and can be describes as9 _ t) = 0, i = 1, 2, . . . , p ui (q, q,
ð3Þ
Equation (3) can be simplified as F = ½u1 u2 up T = 0
ð4Þ
Differentiating the usual constraint equation (4) with respect to time t once (for nonholonomic constraints) or twice (for holonomic constraints) yields a set of constraint equations in matrix form as9 _ t)€q = b(q, q, _ t) A(q, q,
ð5Þ
_ t) 2 Rp 3 n denotes the constraint in which A(q, q, _ matrix, and b(q, q, t) 2 Rp is a p-vector.
Constrained multibody dynamics The intent needs to establish the explicit equations of motion with constraints. Additional generalized forces of constraints arise due to the presence of constraints. Accordingly, the actual explicit equation of motion of the constrained system can be assumed to take the form _ t) _ t) + Qc (q, q, M(q, t)€q = Q(q, q,
ð6Þ
_ t) 2 Rn is the constraint forces in which Qc (q, q, imposed on the system. Obtaining the constraint torques is a main goal in the modeling of constrained dynamical system. Udwadia and Kalaba7 proposed that the constraint torque vector is explicitly given by Qc = M1=2 B+ b AM1 Q
ð7Þ
Xu and Liu
3
_ t)M1=2 (q, t), and the superin which B(q, t) :¼ A(q, q, script ‘‘ + ’’ denotes the Moore–Penrose generalized inverse. The explicit dynamical equation of the constrained system called as the Udwadia–Kalaba theory is denoted in the following general form5,9 M€ q = Q + M1=2 B+ b AM1 Q
t = M(q) €q + C q_ + G(q)
in which M(q) 2 R4 3 4 is the mass matrix; C q_ 2 R4 3 1 is the centrifugal force and Coriolis force matrix; _ €q 2 R4 3 1 G(q) 2 R4 3 1 is the gravitational force; q, q, are the generalized position, velocity, and acceleration, respectively; t 2 R4 3 1 is the joint torque vector; and21
ð8Þ
Premultiplying both sides of equation (8) with M , the acceleration of the constrained system can be obtained € q = M1 Q + M1=2 B+ b BM1=2 Q
ð9Þ
M A
AT 0
in which Lagrange (b AM1 Q).
€q Q = l b
multiplier
ð10Þ
Cij ðqÞ =
Consider the SCARA robot illustrated in Figure 1. The generalized coordinates of the system are denoted as q = ½q1 q2 q3 q4 T .
Unconstrained dynamics Result. The equations of motion of the unconstrained dynamical system are established first using the traditional Lagrange method
ð12Þ
n ∂Mij 1X ∂Mik ∂Mkj + q_ k ∂qj ∂qi 2 k = 1 ∂qk _ qÞ = Gðq,
∂V ∂q
ð13Þ
ð14Þ
Proof. The coordinate frame relative to the base frame of the robot is represented as
l = (AM1 AT )1
Dynamic modeling of SCARA robot
jTi ATli M0l Alj jj
l = max (i, j)
The standard Lagrange multiplier formulation of the constrained dynamics is in the following form
n X
Mij (q) =
1
ð11Þ
^
^
gSCi (q) = ej1 q1 eji qi gSCi (0)
ð15Þ
in which Ci is attached to the mass center of the ith link of the SCARA robot; S is the base frame of the robot; gSCi (q) = (RSCi , tSCi ) 2 SE(3) is the transformation between Ci and S, and gSCi (0) is the transformation between Ci and S at q = 0; ^ji is referred to a skewsymmetric matrix. For the revolute joint, the joint vi 3 Li twists ji is expressed as ji = , in which vi vi 2 R3 is a unit vector which specifies the direction of rotation; Li 2 R3 is any point on the axis; for the prisv matic joint, the twist ji is expressed as ji = i , in 0 3 which vi 2 R is a unit vector in the direction of translation. The body velocity of the mass center of the ith link is described as VbSCi = J(q)q_
ð16Þ
in which J(q) is the body Jacobian corresponding to gSCi (q), and
J(q) = jy1 in which jyj = Ad1^jj qj (e
jyi
j ^ eji qi gSCi (0)) j
0
0
ð17Þ
ðj iÞ is the jth instan-
taneous joint twist relative to the ith link frame Ci . The total kinetic energy of the robot has the form _ qÞ = T ðq, Figure 1. Sketch map of the SCARA robot manipulator subject to constraint.
n X i=1 n X
_ qÞ = Ti ðq,
n X 1
2 i=1
VbSCi
T
Mi VbSCi
1 T T 1 q_ Ji ðqÞMi Ji ðqÞq_ = : q_ T MðqÞq_ = 2 2 i=1
ð18Þ
4
Advances in Mechanical Engineering
_ qÞ and Mi are the kinetic energy and the in which Ti ðq, generalized inertia matrix of the ith link, respectively. The total potential energy has the form V ðq Þ =
n X
V i ðq Þ =
i=1
n X
M0i = AdTg1 Mi Adg1 SC (0) SCi (0)
mi ghi ðqÞ
ð19Þ
i=1
in which Vi (q) is the potential energy of the ith link; mi is the mass of the ith link; g is the gravitational constant; and hi (q) is the height of the mass center of the ith link. The Lagrange function can be obtained as _ qÞ = Lðq,
n X
The inertia of the ith link reflected into the base frame of the manipulator, M0i , is given by
_ qÞ Vi ðqÞÞ ðTi ðq,
ð26Þ
i
Using the notation defined above, the inertia matrix for the manipulator can be obtained as21 n X
Mij (q) =
jTi ATli M0l Alj jj
ð27Þ
l = max (i, j)
2. Substituting equations (22) and (23) into equa_ ij in terms of partial tion (21) and expanding M derivatives, one can obtain
i=1
=
n 1 T 1 X q_ MðqÞq_ V ðqÞ = Mij ðqÞq_ i q_ j V ðqÞ 2 2 i, j = 1
ð20Þ So far, the equations of motion can be obtained by substituting into Lagrange’s equations d ∂L ∂L = Fi dt ∂q_ i ∂qi
ð21Þ
in which Fi represents the generalized forces acting on the ith joint, and !
n n X d ∂L d X _ ij q_ j ð22Þ = Mij q_ j = Mij €qj + M dt ∂q_ i dt j = 1 j=1 n ∂Mkj ∂L 1 X ∂V = q_ k q_ j ∂qi 2 j, k = 1 ∂qi ∂qi
ð23Þ
n X ∂Mij 1 ∂Mkj Mij ðqÞ€qj + q_ q_ q_ q_ ∂qk j k 2 ∂qi k j j=1 j, k = 1 n X
∂V + ðqÞ = Fi , ∂qi
ð28Þ
i = 1, 2, . . . , n
Rearranging all the terms, equation (28) can be rewritten as n X
Mij ðqÞ€qj +
j=1
n X
Gijk q_ j q_ k +
j, k = 1
i = 1, 2, . . . , n ∂M ðqÞ in which Gijk = 12 ∂qij k +
∂Mik ðqÞ ∂qj
∂V ðqÞ = Fi , ∂qi
∂Mkj ðqÞ ∂qi
ð29Þ
.
_ q) is considered to be defined in The matrix C(q, order to put equation (29) back into vector form21 n ∂Mij 1X ∂Mik ∂Mkj _ qÞ = Cij ðq, Gijk q_ K = + q_ k 2 k = 1 ∂qk ∂qj ∂qi k =1 n X
1. The matrix MðqÞ 2 Rn 3 n is obtained using the following procedure. In terms of the link Jacobians, Ji , the manipulator inertia matrix is defined as MðqÞ =
n X
ð30Þ in which jTi ATli M0l Alk
JTi ðqÞMi Ji ðqÞ
SCi (0)
Aii ji 0
n P
=
l = max (i, j)
½Aki ji , jk T ATlk M0l Alj jj +
h i Akj jj , jk .
ð24Þ
i=1
in which Ji (q) = Adg1 ½ Ai1 j1
∂Mij ∂qk
0 .
3. Finally, the gravitational forces on the manipulator are written as21
is referred to as the adjoint transformation Adg1 SC (0) i
Aij jj represents the jth colassociated with g1 SCi (0) . Adg1 SC i
umn of the body Jacobian for the ith link, in which Aij is the adjoint transformation given by 8 1 < Ad(e^jj + 1 qj + 1 e^ji qi ) i .j Aij = I i=j : 0 i \j
ð25Þ
_ qÞ = Gðq,
∂V ∂q
ð31Þ
For the SCARA robot, considering gST = (RST , tST ) 2 SE(3), the transformation between tool and base frames at q = 0 is given by
Xu and Liu
5 2
13 0 @ l1 + l2 A 7 7 5 l0 1 0
6I gST ð0Þ = 6 4 0
^
ejq = ð32Þ
v1 v2 (1 cq) v3 sq v22 (1 cq) + cq
v1 v3 (1 cq) v2 sq
v2 v3 (1 cq) + v1 sq
j2 = ½ l1
0 1
0
^ I evq ðv 3 vÞ + qvvT v 1
^ ^ +v ^ 2 (1 cq) = I + vsq evq 2 v21 (1 cq) + cq 6 = 4 v1 v2 (1 cq) + v3 sq
v23 (1 cq) + cq
The forward kinematic map of the SCARA robot is given by gST ðqÞ = e
^ j1 q1 ^ j2 q2 ^ j3 q3 ^ j4 q4
e
e
RðqÞ gST ð0Þ = 0
c1
s1
0
0
c1 0
0 1
07 7 7, 05
0 s2 c2
0 0 0
1
The inertia matrix M(q) 2 R4 3 4 for the SCARA robot can be derived as21
0 c2 6s ^ 6 2 ej2 q2 = 6 40
0
1
0 c3
0 s3
0 0
c3
0
j3 = ½ l1 + l2 j4 = ½ 0
0
0 1
2
0
0 0
0
a + b + 2gc2 6 b + gc2 MðqÞ = 6 4 d 0
0 0
T
1
T
b + gc2 b d 0
d d d 0
3 0 0 7 7 0 5 m4
2
g sin q2 q_ 2 6 g sin q2 q_ 1 Cðq, q_ Þ = 6 4 0 0
g sin q2 ðq_ 1 + q_ 2 Þ 0 0 0
0 0 0 0
3
0 07 7 05 0 ð35Þ
The gravitational forces on the SCARA robot is expressed as21 Gðq, q_ Þ = ½ 0
0
0 m4 g T
Constraint description The exponential of this twist is given by
2
2
ð34Þ
in which a = Iz1 + r12 m1 + l12 m2 + l12 m3 + l12 m4 , b = Iz2 + Iz3 + Iz4 + l22 m3 + l22 m4 + m2 r22 , g = l1 l2 m3 + l1 l2 m4 + l1 m2 r2 , and d = Iz3 + Iz4 . Note that ci = cos qi , si = sin qi , cij = cos (qi + qj ), and sij = sin (qi + qj ). The Coriolis matrix for the SCARA robot can be obtained as21
ð36Þ
3
6s ^ 6 1 ej1 q1 = 6 40
0
pðqÞ 1
in which the individual exponentials have the form 2
T
e
ð38Þ
T
0 1
3 v1 v3 (1 cq) + v2 sq 7 v2 v3 (1 cq) v1 sq 5
ð33Þ
0 0
ð37Þ
sq = sin q
For the three revolute joints, the direction of rotation is described as v1 = v2 = v3 = ½ 0 0 1 T , and the points on the axis are described as L 2 = ½ 0 l1 0 T , and L 1 = ½ 0 0 1 T , T L3 = ½ 0 l1 + l2 0 . For the prismatic joint, the direction of translation is described as v4 = ½ 0 0 1 . This yields twists 0 0
^ evq 0
in which
cq = cos q,
j1 = ½ 0
6s ^ 6 3 ej3 q3 = 6 40 2
0 1 0
60 1 ^ 6 ej 4 q 4 = 6 40 0 0 0
0 0
3 l1 s2 l1 ð1 c2 Þ 7 7 7 5 0 1 ðl1 + l2 Þs3
ðl1 + l2 Þð1 c3 Þ 7 7 7, 5 0
1 0 3 0 0 0 07 7 7 1 q4 5 0
3
1
1
Expanding the terms in the product of exponentials formula yields gST ðqÞ = 2
RðqÞ
pðqÞ
0
1 s123
0
l1 s1 l2 s12
3
c123 0
0 1
l1 c1 + l2 c12 l0 + q4
7 7 7 5
0
0
1
c123
6s 6 123 =6 4 0 0
ð39Þ
The end-effector of the SCARA robot moves along space trajectory to complete the given tasks. The desired space trajectory, which is regarded as constraints, is presented as follows.
6
Advances in Mechanical Engineering q1 + q2 + q3 = 0
The position constraints can be described as x = 0:05 sinð0:4ptÞ y = 0:35 + 0:05 cosð0:4ptÞ
ð40Þ
z = 0:02t The following elementary rotations about the x-, y-, and z-axis are defined as 2
1
0
3
0
sccg
ð41Þ
3
cusc 7 ð42Þ susc 5
scsg
r11 S 4 T R = r21 r31
r12 r22 r32
3 2 c124 r13 r23 5 = 4 s123 r33 0
s123 c123 0
3 0 05 1
Differentiating equation (48) with respect to time t twice yields the second-order constraints ð49Þ
in which 2
ð43Þ
ð44Þ
The solution of c is always in the range of 0 c 1808, p although solutions can be obtained ffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffitwo ffi 2 + r 2 . The above solutions degenerfrom sin c = r31 32 ate when c = 08 or 1808; at this point, only the sum or difference between u and g is obtained. u = 08 is usually selected. If c = 08, then 8 < a = 08 b = 08 : g = A tan 2ðr12 , r11 Þ
3
A€q = b
and sin c 6¼ 0, then 9 u = A tan 2 ðr23 , r13 Þ = > pffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi 2 + r2 , r c = A tan 2 r31 32 33 > ; g = A tan 2ðr32 , r31 Þ
u1
6 7 6 u2 7 6 7 Fðt Þ = 6 7 6 u3 7 4 5 u4 2 3 l1 s1 l2 s12 0:05 sinð0:4ptÞ 6 7 6 l1 c1 + l2 c12 0:35 0:05 cosð0:4ptÞ 7 6 7 =6 7=0 6 7 + q + q q 1 2 3 4 5 q4 + l0 0:02t
cc
Conversely, equivalent ZYZ Euler angles can be solved by the rotation matrix, as shown below. If 2
ð47Þ
ð48Þ
S ^zu ^yc ^zg T Rzyz ðu, c, g Þ = Rz ðuÞRy ðcÞRz ðg Þ = e e e
cuccsg sucg succsg + cucg
8 l1 s1 l2 s12 = 0:05 sinð0:4ptÞ > > > < l c + l c = 0:35 + 0:05 cosð0:4ptÞ 1 1 2 12 > q1 + q2 + q3 = 0 > > : q4 + l0 = 0:02t
2
ZYZ Euler angles are used to express the orientation of the end-effector of the SCARA robot. The equivalent rotation matrix is denoted as
cucccg susg 6 = 4 succcg + cusg
Now that there are the following four constraints in total
In general, the constraint equations may be thought of as a set of p (p = 4) constraints of the form
6 7 Rx (u) :¼ ex^u = 4 0 cos u sin u 5 0 sin u cos u 2 3 cos u 0 sin u 6 7 1 0 5 Ry (u) :¼ e^yu = 4 0 sin u 0 cos u 2 3 cos u sin u 0 6 7 Rz (u) :¼ e^zu = 4 sin u cos u 0 5 0 0 1
2
ð46Þ
3 l1 c1 l2 c12 l2 c12 0 0 6 l s l s l2 s12 0 0 7 2 12 6 1 1 7 A=6 7 4 1 1 1 05 0 0 0 1 3 2 2 2 q_ 1 l1 s1 ðq_ 1 + q_ 2 Þ l2 s12 0:05 3 (0:4p)2 sin (0:4pt) 7 6 2 6 q_ l1 c1 + ðq_ 1 + q_ 2 Þ2 l2 c12 0:05 3 (0:4p)2 cos (0:4pt) 7 1 7 b=6 7 6 0 5 4 0
Constraint torque description Constraint torques for the system to meet the desired trajectory are explicitly determined by equation (7) in the form of Qc = M1=2 (AM1=2 )+ (b AM1 Q)
ð45Þ
The orientation constraint equation combined with equation (43) can be derived as
ð50Þ
Finally, the dynamic equation is obtained by equations (6), (7), and (50) M€q = Q + Qc
ð51Þ
Xu and Liu
7
Figure 2. Comparisons of dynamic responses between numerical value and theoretical value: (a) q1, q2, q3 and (b) q4.
Figure 3. Comparison of the trajectories between numerical value and theoretical value.
Numerical simulations The constraints must be satisfied at each instant of time during the maneuver including the initial time.
In practice, however, it is usually quite difficult to meet these constraints at the initial time.22 In other words, the initial conditions are incompatible with the constraint equations. It is assumed that the initial configuration for simulation are as follows: q1 (0) = 308, q2 (0) 558, q3 (0) = 248, and q4 (0) = 0 m; q_ 1 (0) = 0:157rad=s, q_ 2 (0) =0:0001 rad=s, q_ 3 (0) =0:157rad=s, and q_ 4 (0) = 0:0195m=s. The parameter values for simulation are as follows: m1 = 20kg, m2 = m3 =15kg, m4 =0:5 kg, l0 =0 m, l1 =0:2 m, l2 =0:25m, r1 =0:1 m, r2 = 0:125 m, Iz1 =0:27kg m2 , Iz2 = 0:31kg m2 , Iz3 =0:02kg m2 , and Iz4 =0:0001 kg m2 . The simulation results are shown in Figures 2–5. Figure 2 represents the dynamic responses at each joint of the SCARA robot subjected to the space trajectory constraints, in which the solid curves and the dash curves represent the numerical value and the theoretical value, respectively. The numerical trajectories is slightly off the theoretical trajectory with time, as illustrated in Figure 3, in which the solid curve and the dash curve represent the numerical trajectory and the theoretical trajectory, respectively. However, the joint errors and
Figure 4. Sketch map of joint errors between numerical value and theoretical value: (a) q1 -error, q2 -error, q3 -error and (b) q4 -error.
8
Advances in Mechanical Engineering
Figure 5. Displacement errors between numerical value and theoretical value: (a) x-error, (b) y-error, and (c) z-error.
displacement errors in the satisfaction of the constraints tend to increase in time and spoil reliability of the simulation results, as shown in Figures 4 and 5, respectively. The largest displacement errors correspond to about 8 3 1024 m in x-axis, 7 3 1023 m in y-axis, and 0.01 m in z-axis, which should not be neglected. Therefore, it is necessary to consider the numerical method for reducing the errors.
form of equation (49).22 Thus, utilizing the Baumgarte stabilization method, the equations of motion for a system subjected to constraints are stated in the form
Error reducing
2
Constraint violation arises when the initial conditions are incompatible with the constraint equations. This makes the simulation results seriously different from real situations. Accordingly, it is necessary for the use of Baumgarte stabilization method to suppress constraint violation. In the standard formulation of the method, the € = 0, (open-loop) second-order differential constraints, F 23 are replaced with its stabilized (closed-loop) form € + aF _ + bF = 0 F
ð52Þ
in which a = diagfa1 , a2 , . . . , ap g and b = diag fb1 , b2 , . . . , bp g. If ai , bi .0, F approaches to zero asymptotically. Thus, from equation (52), a more general constraint equation is obtained, which nonetheless retains the
M A
AT 0
€q Q = _ l baFbF
ð53Þ
Then, equation (52) yields the following constraint equation l1 c1 l2 c12
6 6 l1 s1 l2 s12 6 6 1 4 2 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 6 4
0 q_ 21 l1 s1
l2 c12 l2 s12 1 0
0 0
32
€q1
3
76 7 0 0 76 €q2 7 76 7 = 6 7 1 07 54 €q3 5 0 1
€q4
2
ðq_ 1 + q_ 2 Þ l2 s12 0:05 3 (0:4p)2 sin (0:4pt)
3
7 a1 ðq_ 1 l1 c1 ðq_ 1 + q_ 2 Þl2 c12 0:05 3 (0:4p) cos (0:4pt)Þ 7 7 7 7 b1 ðl1 s1 l2 s12 0:05 sinð0:4ptÞÞ 7 7 7 q_ 21 l1 c1 + ðq_ 1 + q_ 2 Þ2 l2 c12 0:05 3 (0:4p)2 cos (0:4pt) 7 7 a2 ðq_ 1 l1 s1 ðq_ 1 + q_ 2 Þl2 s12 + 0:05 3 (0:4p) sinð0:4ptÞÞ 7 7 7 7 b2 ðl1 c1 + l2 c12 0:35 0:05 cosð0:4ptÞÞ 7 7 a3 ðq_ 1 + q_ 2 + q_ 3 Þ b3 ðq1 + q2 + q3 Þ 5 a4 ðq_ 4 0:02Þ b4 ðq4 + l0 0:02tÞ
ð54Þ
Xu and Liu
9
Figure 6. Comparisons of dynamic responses between modified numerical value and theoretical value: (a) q1, q2, q3 and (b) q4.
3.
4.
constraint trajectory obtained by modified numerical integration gets clear improvement. Comparing with the qi-error between numerical value and theoretical value, the amplitude of qi-error between modified numerical value and theoretical value is substantially lowered, as illustrated in Figure 8. Obviously, the errors are within an acceptable range that the constraint relations in x-, y-, and z-axis are being maintained well as desired, as depicted in Figure 9. Figure 10 represents the constraint torques to make the SCARA robot follow the space trajectory for precise application.
Figure 7. Comparison of the trajectories between modified numerical value and theoretical value.
Conclusion Note that an improper choice of a and b can lead to unacceptable results in the dynamic analysis of the multibody systems.24 Baumgarte19 highlighted that the suitable choice of a and b is performed by numerical experiments. For simplicity, we choose a1 = a2 = a3 = a4 = 0:5 and b1 = b2 = b3 = b4 = 200. The simulation results are presented in Figures 6–10. 1.
2.
Figure 6 represents the dynamic response curves as a function of time t between modified numerical value and theoretical value, in which the solid curves and the dash curves represent the modified numerical value and the theoretical value, respectively. The trajectories are almost coincident by comparing between modified numerical value and theoretical value, as illustrated in Figure 7, in which the solid curves and the dash curves represent the modified numerical value and the theoretical value, respectively. The overall effect of
Aiming at dynamic modeling of the SCARA robot subjected to constraint, this article establishes the dynamic equation based on the Udwadia–Kalaba theory and reduces the errors using Baumgarte stabilization method. The comparison of numerical simulations confirms the ease of implementation and the high numerical accuracy of this methodology. The research process reveals some conclusions as follows: 1.
2.
The desired space trajectory, which is regarded as constraints imposed on the system, is integrated into the dynamic modeling of the system dexterously. The explicit, closed-form expression for the constraint torques required to guarantee the endeffector of the SCARA robot to move along the desired space trajectory and the explicit dynamic equation of the system without Lagrange multiplier are obtained.
10
Advances in Mechanical Engineering
Figure 8. Comparisons of joint errors for-and-aft modification: (a) q1 -error, (b) q2 -error, (c) q3 -error, and (d) q4 -error.
Figure 9. Comparison of displacement errors between for-and-aft modification: (a) x-error, (b) y-error, and (c) z-error.
Xu and Liu
11
Figure 10. Constraint torques: (a) Qc1, Qc2, Qc3, and (b) Qc4.
3.
Baumgarte stabilization method is used for constraint violation suppression to realize trajectory motion with high accuracy.
Declaration of conflicting interests The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.
Funding The author(s) received no financial support for the research, authorship, and/or publication of this article.
References 1. Zhao H, Zhen S and Chen YH. Dynamic modeling and simulation of multi-body systems using the UdwadiaKalaba theory. Chin J Mech Eng 2013; 26: 839–850. 2. Braun DJ and Goldfarb M. Eliminating constraint drift in the numerical simulation of constrained dynamical systems. Comput Method Appl M 2009; 198: 3151–3160. 3. Kane TR. Dynamics of nonholonomic systems. J Appl Mech 1961; 28: 574–578. 4. Korayem MH and Shafei AM. A new approach for dynamic modeling of n-viscoelastic-link robotic manipulators mounted on a mobile base. Nonlinear Dynam 2015; 79: 2767–2786. 5. Udwadia FE and Kalaba RE. A new perspective on constrained motion. Proc R Soc Lond A Math Phys Sci 1992; 433: 407–410. 6. Udwadia FE and Kalaba RE. On the foundations of analytical dynamics. Int J Non Linear Mech 2002; 37: 1079–1090. 7. Udwadia FE and Kalaba RE. What is the general form of the explicit equations of motion for constrained mechanical systems? J Appl Mech 2002; 69: 335–339. 8. Udwadia FE and Koganti PB. Dynamics and control of a multi-body planar pendulum. Nonlinear Dynam 2015; 81: 845–866.
9. Udwadia FE and Wanichanon T. Control of uncertain nonlinear multibody mechanical systems. J Appl Mech 2014; 81: 041020. 10. Udwadia FE and Wanichanon T. A closed-form approach to tracking control of nonlinear uncertain systems using the fundamental equation. Earth Space 2012; 10: 1339–1348. 11. Udwadia FE, Wanichanon T and Cho H. Methodology for satellite formation-keeping in the presence of system uncertainties. J Guid Control Dynam 2014; 37: 1611–1624. 12. Liu J and Liu R. Simple method to the dynamic modeling of industrial robot subject to constraint. Adv Mech Eng 2016; 8: 1–9. 13. Huang J, Chen YH and Zhong Z. Udwadia-Kalaba approach for parallel manipulator dynamics. J Dyn Syst Meas Contr 2013; 135: 061003. 14. Pennestrı` E, Valentini PP and de Falco D. An application of the Udwadia–Kalaba dynamic formulation to flexible multibody systems. J Franklin Inst 2010; 347: 173–194. 15. Oprea RA. A constrained motion perspective of railway vehicles collision. Multibody Syst Dyn 2013; 30: 101–116. 16. Schutte AD and Dooley BA. Constrained motion of tethered satellites. J Aerospace Eng 2005; 18: 242–250. 17. Pappalardo CM. A natural absolute coordinate formulation for the kinematic and dynamic analysis of rigid multibody systems. Nonlinear Dynam 2015; 81: 1841–1869. 18. Sun H, Zhao H, Zhen S, et al. Application of the Udwadia–Kalaba approach to tracking control of mobile robots. Nonlinear Dynam 2016; 83: 389–400. 19. Baumgarte J. Stabilization of constraints and integrals of motion in dynamical systems. Comput Method Appl M 1972; 1: 1–16. 20. Blajer W. Methods for constraint violation suppression in the numerical simulation of constrained multibody systems—a comparative study. Comput Method Appl M 2011; 200: 1568–1576. 21. Murray RM, Li Z, Sastry SS, et al. A mathematical introduction to robotic manipulation. Boca Raton, FL: CRC Press, 1994. 22. Cho H and Udwadia FE. Explicit solution to the full nonlinear problem for satellite formation-keeping. Acta Astronautica 2010; 67: 369–387.
12 23. Lin ST and Chen MW. A PID type constraint stabilization method for numerical integration of multibody systems. J Comput Nonlin Dyn 2011; 6: 044501.
Advances in Mechanical Engineering 24. Nikravesh PE. Some methods for dynamic analysis of constrained mechanical systems: a survey. In: Haug EJ (ed.) Computer aided analysis and optimization of mechanical system dynamics. Berlin: Springer, 1984, pp.351–368.
