- Email: [email protected]
Trajectory tracking control of robots with flexible links
Mechanism and Machine Theory 37 (2002) 1377–1394 www.elsevier.com/locate/mechmt
Trajectory tracking control of robots with flexible links S. Kemal Ider a
a,*
€ zg€ , M. Kemal O oren a, Volkan Ay
b
Mechanical Engineering Department, Middle East Technical University, Ankara 06531, Turkey b Mercedes-Benz T€urk, Hadımk€ oy, I_ stanbul, 34002 Turkey Received 17 July 2000; accepted 1 March 2002
Abstract A new method is developed for the end-effector trajectory tracking control of robots with flexible links. In order to cope with the non-minimum phase property of the system, the closed-loop poles are placed at desired locations using full state feedback. The dynamic equations are linearized about the rigid motion. A composite control law is designed to track the desired trajectory while at the same time the internal dynamics is stabilized. The proposed method is valid for all types of manipulators with any degree of freedom. A two link manipulator is simulated to illustrate the performance of the method. Ó 2002 Elsevier Science Ltd. All rights reserved.
1. Introduction Light-weight flexible arms will most likely constitute the next generation robots due to their large payload carrying capacities at high speeds and less power demand. However, because of the vibrations during the motion, the control problem of robots with flexible members are more complex compared to rigid robots. Some researchers considered point-to-point position control or regulation of single link and multilink robots in which the main task is to suppress the residue vibrations [1–3]. Book et al. [1] designed a full state feedback law for a two link manipulator. Pham et al. [2] developed an inverse dynamics controller with a joint PD action which is stable for positioning but unstable for trajectory tracking. Chaloub and Ulsoy [3] studied robust pole assignment and optimal regulator controls using a linearized model of the structure. In many applications, however, tip trajectory tracking is required. Tip tracking control of flexible link manipulators has the difficulty of the non-minimum phase property due to the finite *
Corresponding author. Tel.: +90-312-210-5289; fax: +90-312-210-1266. E-mail address: [email protected] (S.K. Ider).
0094-114X/02/$ - see front matter Ó 2002 Elsevier Science Ltd. All rights reserved. PII: S 0 0 9 4 - 1 1 4 X ( 0 2 ) 0 0 0 4 3 - 5
1378
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
speed of propagation of the mechanical wave along the links since the joint actuators are noncolocated. The transfer function between the input (torque) and the output (end-effector position) is non-minimum phase, i.e. it has positive real zeros. These zeros become poles of the inverse dynamic system transfer function. Cannon and Schmidtz [4] applied a linear quadratic optimal control approach to a one link flexible robot arm in which the non-minimum phase property was first demonstrated. After that, many researchers considered different approaches to the control of one link flexible arms [5–8]. Siciliano and Book [5] utilized singular perturbation approach where a slow control for the slow rigid subsystem and a fast stabilizing control for the fast flexible system are considered, assuming that the links of the manipulator are not too flexible so that the perturbation parameter is sufficiently small. Chen and Yang [6] considered the rigid body mode and the first flexible mode as the nominal model and the other modes as unmodeled dynamics. Kwon and Book [7] separated the inverse system into its causal and anticausal parts, and obtained the feedforward torque history by integrating the causal part of the equations forward in time and the anticausal part backward in time. Zhu et al. [8] lumped the flexible link into a simple spring-mass unit and applied a two-loop PD type controller. For the trajectory tracking control of multilink manipulators various approaches have been considered [9–16]. Asada et al. [9] developed an iterative inverse dynamics algorithm using a virtual link coordinate system. Bayo et al. [10] presented an inverse dynamics solution procedure to obtain non-causal force trajectories by an iterative frequency domain approach. Paden et al. [11] described a control law making use of the feedforward torques as obtained by Bayo et al. [10] and a passive joint control. Zhao and Chen [12] utilized a feedforward plus feedback control structure where the feedforward signal is generated by a stable inversion procedure which is based on an iterative solution of a two point boundary value problem. Moallem et al. [13] and Damaren [14] investigated inversion techniques based on output redefinition in which suitable outputs which render the system minimum phase are utilized. Siciliano et al. [15] and Moallem et al. [16] applied singular perturbation approach to multilink manipulators. In this paper a controller is designed such that while the end-effector tracks the desired trajectory, the internal system dynamics are stabilized. The non-linear dynamic equations are linearized about the rigid motion (the motion that would occur if all links were rigid) and full state feedback is utilized. For the closed-loop system to be asymptotically stable it must be slowly varying as defined in Refs. [21,22]. The stability condition is achieved by placing the poles at a sufficient distance from the imaginary axis [22]. The bias term that arises due to linearizing about the rigid motion is compensated by partitioning the control input and separating the related subsystem. A two link robot is simulated to illustrate the performance of the algorithm.
2. Dynamic equations and linearization The deformation of each link is defined relative to a link reference frame which follows the global motion of the link in a manner consistent with the boundary conditions. Let h ¼ ½h1 ; . . . ; hn T be the vector of joint variables where n is the degree of freedom of the manipulator T when it is assumed rigid, and let g ¼ ½g1 ; . . . ; gm be the vector of elastic modal variables of all links. The dynamic equations of the manipulator can be written as below [17]:
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
r 0 T Mrr Mre h€ Q ¼ þ e þ ee sym M Q Kg 0 g€
where
Mrr Mre Mðh; gÞ ¼ sym Mee
1379
ð1Þ
is the inertia matrix, r Q _ ðQðh; g; h; g_ Þ ¼ Qe contains Coriolis, centrifugal and gravitational terms, superscripts r and e refer to rigid body (joint) and elastic degrees of freedom respectively, K is a block diagonal matrix whose diagonal submatrices are the stiffness matrices of the links in terms of modal variables and T ¼ ½T1 ; . . . ; Tn T is the vector of joint actuator torques and forces. The equations will be linearized about the rigid motion, which is the motion of the manipulator when all links are assumed rigid, corresponding to the desired trajectory of the end-effector. To this end h, g and T can be written as h ¼ h þ Dh
ð2aÞ
g ¼ Dg
ð2bÞ
ð2cÞ
T ¼ T þ DT
where superscript refers to the rigid motion (g ¼ 0), i.e. T is the vector of torques required by the rigid manipulator and h is the joint variable vector of the rigid manipulator, and Dh, g and DT are the variations from the rigid motion. Substituting Eqs. (2a,c) into Eq. (1), neglecting higher order variations and canceling the terms corresponding to the rigid system, the linear equations are obtained in the following form: ML ðh Þ€z þ CL ðh ; h_ Þ_z þ KL ðh ; h_ ; h€ Þz ¼ WL ðh ; h_ ; h€ Þ þ JDT T T ,
T
ð3Þ
T
J ¼ ½ In 0 and In is n n identity matrix. where z ¼ ½ Dh g The output of the system is the n dimensional vector of the end-effector position and orientation variables, RE ðh; gÞ. The difference between the end-effector position and orientation of the flexible and rigid manipulators is DRE ¼ RE ðh; gÞ RE ðh ; 0Þ
ð4Þ
Using Eqs. (2a,c), Eq. (4) can be expressed as DRE ¼ Dðh Þz
ð5Þ
Defining the 2ðn þ mÞ dimensional state vector as x ¼ ½ DhT
gT
Dh_T
g_ T
T
the state space representation of the model is x_ ¼ Ax þ Bu þ Z y ¼ Cx
ð6Þ ð7Þ ð8Þ
1380
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
where u ¼ DT represents the control vector, A is the 2ðn þ mÞ 2ðn þ mÞ system matrix, B is the 2ðn þ mÞ n input matrix, y ¼ DRE is the output vector, C is the n 2ðn þ mÞ output matrix and Zðh ; h_ ; h€ Þ is a known bias term. A, B, C and Z are 0 Inþm ð9Þ A¼ M 1 M 1 L KL L CL 0 B¼ ð10Þ M 1 L J C ¼ ½D 0 0 Z¼ M 1 L WL
ð11Þ ð12Þ
Since A, B, C and Z depend on the rigid motion variables h , h_ and h€ , they are time dependent. The appearance of the bias term Z in Eq. (7) is due to the fact that the rigid motion about which the equations of the flexible manipulator (Eq. (1)) are linearized does not totally satisfy Eq. (1).
3. Control law design The control torques should be designed in order to reduce the deviation of the end-effector position and orientation from the desired trajectory, and at the same time yield the internal system dynamics to be stable. To this end, first the known bias term Z should be eliminated from the state space equations. This can be done by separating the original system into two subsystems. Denoting the first and the second subsystems with overscripts ^ and ~ respectively, x, u and y can be written as ^þx ~ x¼x
ð13aÞ
u¼^ uþ~ u
ð13bÞ
y¼^ yþ~ y
ð13cÞ
Using Eqs. (13a–c), Eqs. (7) and (8) become ^_ þ x ~_ ¼ Að^ ~Þ þ Bð^ x xþx uþ~ uÞ þ Z
ð14Þ
^ ~Þ yþ~ y ¼ Cð^ xþx
ð15Þ
Separation of the relevant terms in Eqs. (14) and (15) yield the two subsystems as ^_ ¼ A^ x x þ B^ uþZ
ð16Þ
^ y ¼ C^ x
ð17Þ
~_ ¼ A~ x x þ B~ u
ð18Þ
~ y ¼ C~ x
ð19Þ
and
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
1381
In order to obtain a stable solution to Eq. (16), the first part of the control, ^u is set as bx ^ ^ u ¼ K b is a n 2ðn þ mÞ gain matrix. Using Eq. (20), Eq. (16) can be written as where K b Þ^ ^_ ¼ ðA B K x xþZ
ð20Þ
ð21Þ
b has to ^. The vector of initial values x ^o can be arbitrarily taken as zero. K which can be solved for x be chosen such that the poles of Eq. (21) are placed properly for stability. Then ^y can be computed from Eq. (17). Since the end-effector tracking error is desired to be zero (i.e. y ¼ 0), the desired ~y is ~ y ¼ C^ x y ¼ ^
ð22Þ
The second part of the control, ~ u is designed to stabilize the second subsystem dynamics and to track the desired trajectory, as ex ~ ~ þ Hð~ yÞ ð23Þ u ¼ K y ~ e is n 2ðn þ mÞ feedback gain matrix and H is n n end-effector position error gain where K matrix. Substitution of Eq. (19) into Eq. (23) and the resulting equation into Eq. (18) yields ~_ ¼ ðA BKÞ~ x x þ BH~ y
ð24Þ
where e þ HC K¼K
ð25Þ
The matrix K will be chosen in order to place the poles properly for stability. For finding a ~_ ¼ 0 and x ~¼x ~ , at which Eq. (24) suitable H, one can consider the equilibrium conditions where x yields ~ ¼ ðA BKÞ 1 BH~ y x
ð26Þ
Premultiplying both sides of Eq. (26) by C, one obtains ~ y ¼ C~ x ¼ CðA BKÞ 1 BH~ y
ð27Þ
H is found from Eq. (27) as H ¼ ½ CðA BKÞ 1 B
1
ð28Þ
Note that, since Eqs. (21) and (24) have similar characteristics, it is convenient to use the same b and K the same as each other. After obtaining H pole locations for both subsystems, i.e. to use K e from Eq. (25). from Eq. (28) one can compute the feedback gain K From Eq. (2c), the total control torque vector required for trajectory tracking is uþ~ u T ¼ T þ ^
ð29Þ
b and K that correspond to the selected pole locations, a pole placement In order to determine K method shown by Brogan [18] is utilized. For a brief explanation of this method, consider the eigenvalue problem of Eqs. (21) and (24) written as below.
1382
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
ðA BKÞwi ¼ ki wi
ð30Þ
where ki , i ¼ 1; . . . ; 2ðn þ mÞ are the eigenvalues (poles) and wi are the corresponding eigenvectors of A BK. Eq. (30) can be written as wi ¼ 0; i ¼ 1; . . . ; 2ðn þ mÞ ð31Þ ½ ðki I AÞ B Kwi wi For each ki chosen, n solution vectors for can be found which form a basis for the null Kwi space of the 2ðn þ mÞ 2ðn þ mÞ þ n matrix ½ ðki I AÞ B . To obtain a unique K, a linear combination of the solution vectors is selected. This means an arbitrary selection of the n dimensional vector bi ¼ Kwi . Denote the matrix formed by the selected null space vectors for all ki ’s by G, which can be partitioned as w G¼ ð32Þ b where w is 2ðn þ mÞ 2ðn þ mÞ matrix whose columns are the corresponding eigenvectors wi , and b is n 2ðn þ mÞ matrix whose columns are the selected bi , i.e. Kw ¼ b. Hence K can be obtained as K ¼ bw 1
ð33Þ
Since multiple input systems allow an infinite number of choices for the gain matrix K (for a given set of eigenvalues), the designer can make choices. It is desired to make w as nearly orthogonal as possible because this improves its invertibility robustness and tends to give less interaction between modes of the closed-loop system. This orthogonality usually improves system sensitivity to parameter variations [18]. b are obtained by Eq. (33), H and K e are computed from Eqs. (28) and (25) reOnce K and K b, K e and H are updated during the motion at selected time intervals. In fact spectively. The gains K b e the computation of K , K and H at different intervals can be done off-line for any desired endeffector trajectory. Furthermore, the numerical integration of Eq. (21) can also be done off-line. ^ and from Eq. (20) ^ Hence x u can be obtained for each control sampling time. Consequently the ~ is obtained by computations which must be done on-line are only Eqs. (23) and (29). In Eq. (23) x ^ from the measured deviations of the states from the rigid motion, ~y is computed by subtracting x Eq. (19) and ~ y is computed by Eq. (22). To investigate the stability of the linear time varying control system, consider the homogeneous equation associated with Eqs. (21) and (24), which can be written as n_ ¼ CðtÞn, where C ¼ A BK. It is known that stability is not ensured by having only r0 > 0 such that [21] Re ki ðtÞ 6 r0 < 0
8i; 8t P 0
ð34Þ
However it can be shown that such a system is asymptotically stable when the variation of C is sufficiently slow as described in the Refs. [20–22]. Desoer [22] showed that a time varying system is asymptotically stable at large if r2 C_ M 6 0 4 3m
ð35Þ
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
1383
where r0 is as given by Eq. (34), C_ M ¼ supt P 0 kC_ ðtÞk and m is a constant that satisfies kesCðtÞ k 6 m e sðr0 =2Þ
8t P 0; 8s P 0
ð36Þ
To see how r0 affects Eq. (35) consider how C_ M varies with r0 . K is found from Eq. (31) by choosing ki and bi arbitrarily, which can also be written in the form, ðA ki IÞwi ¼ Bbi , where bi ¼ ðKwi Þ, i ¼ 1; . . . ; 2ðn þ mÞ. K can then be expressed as K ¼ fðk; b; tÞ where f is a function such that the effect of k on K is in the order of k. In order to express C_ M , one can write of of of K_ ¼ k_ þ b_ þ ok ob ot Like k, k_ is also arbitrary, hence it can be chosen so that its dependence on k is in the order of k. Also since b and b_ are arbitrary they can be chosen independently of k. Therefore the effect of k on K_ is also in the order of k. Then, C_ ¼ A_ B_ K BK_ implies that the effect of k on C_ is in the order of k. On the other hand r0 ¼ mint P 0 jRe k1 j, where k1 is the most dominant eigenvalue. Therefore the effect of r0 on C_ M is in the order of r0 , while the effect of r0 on the right hand side of Eq. (35) is in the order of r20 . Hence by choosing a sufficiently large r0 , one can achieve asymptotic stability in the large. 4. Numerical example A 2R planar manipular shown in Fig. 1 is considered. The tip point C is required to track a desired trajectory. m1 and m2 are the masses, L1 and L2 are the lengths, and T1 and T2 are the motor
Fig. 1. 2R planar manipulator.
1384
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
torques of the upper arm and the forearm, respectively. The upper arm is assumed rigid while the forearm is a uniform Euler–Bernoulli beam modeled in accordance with the Ritz approach. The manipulator moves in the horizontal plane so gravity is not considered. The body reference frames Ið1Þ and Ið2Þ of the upper arm and the forearm are located at A and B respectively as shown and I is the fixed reference frame. Fixed-free boundary conditions are assumed for the forearm, hence ð2Þ the unit vector I1 is always tangent to the beam centerline at B. h1 and h2 are the joint angles. The deformation of the forearm is described by the first two bending modes since the higher bending modes and the longitudinal deformation were observed to be negligible in open loop simulations of the system performed using the rigid torques. Hence in this system n ¼ 2 and m ¼ 2. The dynamic equations are derived in Appendix A where Mrr , Mre , Mee , Qr , Qe and K in Eq. (1) and ML , CL , KL and WL of the linearized system (Eq. (3)) are given. The output RE ðh; gÞ is ð1Þ ð2Þ ð2Þ the position vector of the tip point C which is L1 I1 þ L2 I1 þ ½/1 ðL2 Þg1 þ /2 ðL2 Þg2 I2 , where /1 and /2 are the mode shapes given in Appendix A. Hence RE can be expressed as L1 cos h1 þ L2 cosðh1 þ h2 Þ ½/1 ðL2 Þg1 þ /2 ðL2 Þg2 sinðh1 þ h2 Þ ð37Þ RE ðh; gÞ ¼ L1 sin h1 þ L2 sinðh1 þ h2 Þ þ ½/1 ðL2 Þg1 þ /2 ðL2 Þg2 cosðh1 þ h2 Þ The output of the linear system DRE is obtained from Eq. (4) yielding D in Eq. (5) to be L1 s1 L2 s12 L2 s12 /1 ðL2 Þs12 /2 ðL2 Þs12 Dðh Þ ¼ L1 c1 þ L2 c12 L2 c12 /1 ðL2 Þc12 /2 ðL2 Þc12
ð38Þ
where s1 ¼ sin h1 , c1 ¼ cos h1 , s12 ¼ sinðh1 þ h2 Þ and c12 ¼ cosðh1 þ h2 Þ. The data used are m1 ¼ m2 ¼ 1 kg, L1 ¼ 1 m, L2 ¼ 1:5 m and EI ¼ 50 Nm2 . The desired trajectory of the end point C is a straight line along I1 starting from rest at xCo ¼ 1 m and ending at rest at xCf ¼ 1 m, with yC ¼ 0:75 m. It is composed of an acceleration part, a constant velocity part (with 0.56 m/s velocity) and a deceleration part. For the acceleration and deceleration parts Hermite polynomials are used so that the spline functions are continuous up to the jerk. The acceleration and deceleration periods are 1.5 s each and the constant velocity period is 2 s. The desired trajectory along I1 is shown in Fig. 2. From the desired trajectory the rigid motion and torques h1 , h2 , T1 and T2 are obtained by inverse dynamic analysis of the rigid manipulator. The closed-loop poles must be placed sufficiently away from the imaginary axis for asymptotic stability. After a few trials, the following poles yielded a satisfactory performance: k1 ¼ 30, b ¼ K are k2 ¼ 50, k3 ¼ 70, k4 ¼ 100, k5 ¼ 140, k6 ¼ 190, k7 ¼ 250 and k8 ¼ 320. K obtained from Eq. (33) where G is formed by selecting the first column of the null space basis set of ½ ðki I AÞ B for each ki . The required control torques are computed from Eq. (29) as described in Section 3. The control sampling frequency is 1000 Hz. The control sampling frequency is chosen not to be close to the frequencies of the unmodeled higher modes in order to avoid their excitation. The tip position error components are shown in Figs. 3 and 4. The simulation results of the proposed method are compared with those of the computed torque method where it is assumed that the links are rigid and the control torques are computed as T ¼ Mrr ½h€ þ C1 ðh_ h_Þ þ C2 ðh hÞ þ Qr
ð39Þ
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
Fig. 2. Desired position of the tip-point C along I1 .
Fig. 3. Tip position error along I1 .
1385
1386
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
Fig. 4. Tip position error along I2 .
where in Mrr and Qr , gi ’s are set to zero. C1 and C2 are the velocity and position feedback gain diagonal matrices respectively. The plant is again the flexible system, hence the link flexibility represents unmodelled dynamics. The two poles of the computed torque method for each error component are selected as the same as the two poles of the proposed method which are closest to the imaginary axis. When poles which are further away from the imaginary axis are used, the resulting higher control torques cause larger vibrations, hence increase the position error further. It is seen from the figures that the end point tracking performance is greatly improved when the link flexibility is considered in the control law by the proposed approach. The time histories of the applied control torques T1 and T2 are given in Figs. 5 and 6, which show that the required torques are within reasonable levels. Figs. 7 and 8 show the joint variables h1 and h2 and Figs. 9 and 10 show the modal coordinates g1 and g2 .
5. Conclusions In this study, a controller design methodology is developed for end-effector trajectory tracking of flexible manipulators. The non-linear dynamic equations are linearized about the states and torques that correspond to the operation of the manipulator when the manipulator is assumed rigid. In order to cope with the non-minimum phase property of the system, the internal dynamics is made stable by placing the closed-loop poles at desired locations using full state feedback. The composite control torques are designed also to track the desired trajectory at the same time.
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
Fig. 5. Control torque applied at A, T1 .
Fig. 6. Control torque applied at B, T2 .
1387
1388
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
Fig. 7. Joint variable of the upperarm, h1 .
Fig. 8. Joint variable of the forearm, h2 .
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
Fig. 9. First modal coordinate.
Fig. 10. Second modal coordinate.
1389
1390
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
In order to have asymptotic stability in the large the closed-loop system must be slowly varying [21,22]. It is shown that the stability condition is satisfied by placing the poles sufficiently away from the imaginary axis. Since the rigid motion and torques, and the feedback gain matrices can be computed off-line as functions of time together with the solution of the subsystem corresponding to the bias term, only a small amount of on-line computations are needed for the calculation of the control torques.
Appendix A The generalized coordinate vector is q ¼ ½ hT gT T , where h ¼ ½ h1 h2 T and g ¼ ½ g1 g2 T . Let / denote the mode shape vector, i.e. / ¼ ½ /1 ðxÞ /2 ðxÞ T where x represents xð2Þ in Fig. 1 and the fixed-free flexural deflection modes are [19]. ar x ar x ar x ar x sinh þ ðcos ar þ cosh ar Þ cos cosh ðA:1Þ /r ¼ ðsin ar sinh ar Þ sin L2 L2 L2 L2 for r ¼ 1, 2, where a1 ¼ 1:875 and a2 ¼ 4:694. The position vector from B to an arbitrary point in the forearm using components in frame Ið2Þ is Rx x 12 gT ½ 0 /0 /0T dng r¼ ðA:2Þ /T g Rx ð2Þ where /0 ðnÞ ¼ d/=dn, /T g is deflection along I2 and the displacement ð1=2ÞgT ½ 0 /0 /0T dng ð2Þ along I1 is the geometrically non-linear coupling term which is necessary to account for the effect of the axial forces on bending [17,23]. The velocity of the arbitrary point is v ¼ vB þ r_ þ T x r where x r ¼ ðh_1 þ h_2 Þ½ r2 r1 . This yields 2 3 h_1 T T T / g g K 4 _ 5 L1 sin h2 / g v ¼ Jq_ ¼ ðA:3Þ h2 /T L1 cos h2 þ x 12 gT Kg x 12 gT Kg g Rx where KðxÞ is the 4 4 matrix KðxÞ ¼ 0 /0 /T0 dn. UsingR Kane’s equations, the generalized inertia force vector due to the forearm is of the arbitrary point. Since f ð2Þ ¼ qJT a dV , where a ¼ aB þ v_ þ x v is the acceleration
T y þ Q, where f ð1Þ ¼ ð1=3Þm1 L1 h€1 0 0 , one obtains M and Q as f ð1Þ þ f ð2Þ ¼ M€ Z L2 h 2 i rr 2 1 M11 ¼ 3m1 L1 þ qA2 ðL1 sin h2 /T gÞ2 þ L1 cos h2 þ x 12gT Kg dx ðA:4Þ rr ¼ qA2 M12
rr M22 ¼ qA2
0
Z Z
L2 0 L2 0
ð/T g L1 sin h2 Þð/T gÞ þ L1 cos h2 þ x 12gT Kg x 12gT Kg dx
h
2 i /T gð/T gÞ þ x 12gT Kg dx
ðA:5Þ ðA:6Þ
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
" re
M ¼
qA2
ee
M ¼ qA2 Qr1
¼ qA2
Qr2 ¼ qA2
R L2 0
Z
//T dx
L2 0 L2
ðL1 sin h2 /T gÞp1 þ L1 cos h2 þ x 12gT Kg p2 dx
ðA:8Þ
Z
L2
ðA:9Þ
/T gp1 þ x 12gT Kg p2 dx
ðA:10Þ
/T gp1 þ /p2 dx
ðA:11Þ
0
Qe ¼ qA2
ðA:7Þ
0
Z Z
L2
KT gðL1 sin h2 /T gÞ þ / L1 cos h2 þ x 12 gT Kg dx RL qA2 0 2 KT gð/T gÞ þ / x 12 gT Kg dx
1391
#
0
where q is the mass density, A2 is the cross-section area of the forearm and p1 and p2 are the Ið2Þ components of the Coriolis and centripetal acceleration vector of the forearm, p1 ¼ L1 cos h2 h_21 x 12gT Kg ðh_1 þ h_2 Þ2 2/T g_ ðh_1 þ h_2 Þ g_ T Kg_ ðA:12Þ p2 ¼ L1 sin h2 h_21 /T gðh_1 þ h_2 Þ2 2gT Kg_ ðh_1 þ h_2 Þ
ðA:13Þ
At this step simplifying assumptions valid for the motion treated in the example will be made. First, since the elastic deflections are small, quadratic terms of gi and their time derivatives will be omitted. Second, the stress stiffening of bending due to the coupling between the longitudinal and transverse motions is significant only if the axial force is sufficiently large, since a tensile force stiffens bending while a compressive force softens it. Stress stiffening is essential for high speed rotating beams, however it doesn’t have a significant effect in a deployment maneouver such as the one considered here [17]. Hence the terms involving K will be omitted. When the space integrals are evaluated after the time dependent variables are separated, one obtains M and Q as below. m1 L21 þ m2 L22 þ m2 L21 þ m2 L1 L2 cos h2 þ m2 L1 ð4:757g1 þ 47:414g2 Þ sin h2 3 m2 L22 rr rr M12 ¼ M21 ¼ þ 0:5m2 L1 L2 cos h2 þ m2 L1 ð2:378g1 þ 23:707g2 Þ sin h2 3 m2 L22 rr ¼ M22 3 1:728m 4:959m2 L2 23:707m2 L1 cos h2 2 L2 2:378m2 L1 cos h2 re M ¼ 1:728m2 L2 4:959m2 L2 9:226m2 0 Mee ¼ 0 2984:5m2 rr ¼ M11
ðA:14Þ ðA:15Þ ðA:16Þ ðA:17Þ ðA:18Þ
Qr1 ¼ m2 L1 ð2:378g1 h_22 þ 4:757g1 h_1 h_2 þ 23:707g2 h_22 Þ cos h2 þ m2 L1 ð 47:414g_ 2 ðh_1 þ h_2 Þ þ 0:5L2 h_22 þ L2 h_1 h_2 Þ sin h2
ðA:19Þ
1392
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
h
i Qr2 ¼ m2 L1 ð2:378g1 h_21 þ 23:707g2 h_21 Þ cos h2 0:5L2 h_21 sin h2 h i Qe1 ¼ m2 9:226g1 ðh_21 þ h_22 Þ 18:452g1 h_1 h_2 2:378L1 h_21 sin h2 h i Qe2 ¼ m2 2984:5g2 ðh_21 þ h_22 Þ 5969:0g2 h_1 h_2 23:707L1 h_21 sin h2
ðA:20Þ ðA:21Þ ðA:22Þ
The stiffness matrix K is as below. K¼
Z
L2
EI /00 /00T dx ¼
0
EI 114:1 0 L32
0 1:449 106
ðA:23Þ
The matrices ML , CL , KL and WL of the linear system are m1 L21 þ m2 L22 þ m2 L21 þ m2 L1 L2 cos h2 3 m2 L22 þ 0:5m2 L1 L2 cos h2 ¼ ML21 ¼ 3 ¼ ML31 ¼ 1:728m2 L2 2:378m2 L1 cos h2
ML11 ¼
ðA:24Þ
ML12
ðA:25Þ
ML13
ML14 ¼ ML41 ¼ 4:959m2 L2
23:707m2 L1 cos h2
m2 L22 3 ¼ ML32 ¼ 1:728m2 L2
ðA:26Þ ðA:27Þ
ML22 ¼
ðA:28Þ
ML23
ðA:29Þ
ML24 ¼ ML42 ¼ 4:959m2 L2
ðA:30Þ
ML33 ¼ 9:226m2
ðA:31Þ
ML44 ¼ 2984:5m2
ðA:32Þ
ML34 ¼ ML43 ¼ 0
ðA:33Þ
CL11 ¼ m2 L1 L2 h_2 sin h2 CL ¼ m2 L1 L2 ðh_ þ h_ Þ sin h
ðA:34Þ
CL13 ¼ 4:757m2 L1 ðh_1 þ h_2 Þ sin h2 CL ¼ 47:414m2 L1 ðh_ þ h_ Þ sin h
ðA:36Þ
1
12
2
1
14
CL21 ¼
m2 L1 L2 h_1
ðA:35Þ
2
2
2
sin h2
ðA:37Þ ðA:38Þ
CL31 ¼ 4:757m2 L1 h_1 sin h2 CL ¼ 47:414m2 L1 h_ sin h
ðA:39Þ
CL22 ¼ CL23 ¼ CL24 ¼ CL32 ¼ CL33 ¼ CL34 ¼ CL42 ¼ CL43 ¼ CL44 ¼ 0 2 _ € _ € _ KL12 ¼ m2 L1 L2 0:5ðh2 cos h2 þ h2 sin h2 Þ ðh1 h2 cos h2 þ h1 sin h2 Þ
ðA:41Þ
41
1
2
ðA:40Þ
ðA:42Þ
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
2 KL13 ¼ m2 L1 2:378ðh_2 cos h2 þ h€2 sin h2 Þ 4:757ðh_1 h_2 cos h2 þ h€1 sin h2 Þ
KL14 ¼ m2 L1
2 _ € _ € _ 23:707ðh2 cos h2 þ h2 sin h2 Þ 47:414ðh1 h2 cos h2 þ h1 sin h2 Þ
2 KL22 ¼ 0:5m2 L1 L2 ðh_1 cos h2 h€2 sin h2 Þ
1393
ðA:43Þ ðA:44Þ ðA:45Þ
2
KL23 ¼ KL32 ¼ 2:378m2 L1 ðh_1 cos h2 h€1 sin h2 Þ
ðA:46Þ
2
KL24 ¼ KL42 ¼ 23:707m2 L1 ðh_1 cos h2 h€1 sin h2 Þ EI KL33 ¼ 114:1 3 9:226m2 ðh_1 þ h_2 Þ2 L2 EI KL44 ¼ 1:449 106 3 2984:5m2 ðh_1 þ h_2 Þ2 L2
ðA:47Þ ðA:48Þ ðA:49Þ
KL11 ¼ KL21 ¼ KL31 ¼ KL41 ¼ KL34 ¼ KL43 ¼ 0
ðA:50Þ 2
WL3 ¼ 1:728m2 L2 ðh€1 þ h€2 Þ þ 2:378m2 L1 ðh€1 cos h2 þ h_1 sin h2 Þ 2
ðA:51Þ
WL4 ¼ 4:959m2 L2 ðh€1 þ h€2 Þ þ 23:707m2 L1 ðh€1 cos h2 þ h_1 sin h2 Þ
ðA:52Þ
WL1 ¼ WL2 ¼ 0
ðA:53Þ
References [1] W.J. Book, O. Maizza-Neto, D.E. Whitney, Feedback control of two beam, two joint systems with distributed flexibility, Journal of Dynamic Systems, Measurement and Control 97 (1975) 424–431. [2] C.M. Pham, W. Khalil, Chevallereau, A nonlinear model-based control of flexible robots, Robotica 11 (1993) 73–82. [3] N.G. Chaloub, A.G. Ulsoy, Control of a flexible robot arm: experimental and theoretical results, Journal of Dynamic Systems, Measurement and Control 109 (1987) 299–310. [4] R.H. Cannon, E. Schmitz, Initial experiments on the end-point control of flexible one-link robot, International Journal of Robotics Research 3 (1984) 62–75. [5] B. Siciliano, W.J. Book, A singular perturbation approach to control lightweight flexible manipulators, International Journal of Robotics Research 7 (1998) 79–90. [6] B.-S. Chen, T.-Y. Yang, Robust optimal model matching control design of flexible manipulators, Journal of Dynamic Systems, Measurement and Control 115 (1993) 173–178. [7] D.-S. Kwon, W.J. Book, A time-domain inverse dynamic tracking control of a single-link flexible manipulator, Journal of Dynamic Systems, Measurement and Control 116 (1994) 193–200. [8] G. Zhu, S.S. Ge, T.H. Lee, Simulation studies of tip tracking control of a single-link flexible robot based on a lumped model, Robotica 17 (1999) 71–78. [9] H. Asada, Z.-D. Ma, H. Tokumaru, Inverse dynamics of flexible robot arms: modeling and computation for trajectory control, Journal of Dynamic Systems, Measurement and Control 112 (1990) 177–185. [10] E. Bayo, P. Papadopulos, J. Stubbe, M.A. Serna, Inverse dynamics and kinematics of multi-link elastic robots: an iterative frequency approach, International Journal of Robotics Research 8 (1989) 49–62. [11] B. Paden, D. Chen, R. Ledesma, E. Bayo, Exponentially stable tracking control for multijoint flexible-link manipulators, Journal of Dynamic Systems, Measurement and Control 115 (1993) 53–59. [12] H. Zhao, D. Chen, Tip trajectory tracking for multilink flexible manipulators using stable inversion, Journal of Guidance, Control and Dynamics 21 (1998) 313–320.
1394
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
[13] M. Moallem, R.V. Patel, K. Khorasani, An inverse dynamics control strategy for tip position tracking of flexible multi-link manipulators, Journal of Robotic Systems 14 (1997) 649–658. [14] C.J. Damaren, Modal properties and control system design for two-link flexible manipulators, International Journal of Robotics Research 17 (1998) 667–678. [15] B. Siciliano, J.V. Prasad, A.J. Calise, Output feedback two time scale control of multilink flexible arms, Journal of Dynamic Systems, Measurement and Control 114 (1992) 70–77. [16] M. Moallem, K. Khorasani, R.V. Patel, An integral manifold approach for tip position tracking of flexible multilink manipulators, IEEE Transactions on Robotics and Automation 13 (1997) 823–837. [17] S.K. Ider, F.M.L. Amirouche, Influence of geometric nonlinearities in the dynamics of flexible treelike structures, Journal of Guidance, Control and Dynamics 12 (1989) 830–837. [18] L.W. Brogan, Modern Control Theory, third ed., Prentice Hall, 1991. [19] L. Meirovitch, Elements of Vibration Analysis, second ed., McGraw-Hill, 1986. [20] T.E. Fortmann, K.L. Hitz, An Introduction to Linear Control Systems, Marcel Dekker, 1977. [21] H.H. Rosenbrock, The stability of linear time-dependent control systems, Journal of Electronics and Control 15 (1963) 73–80. [22] C.A. Desoer, Slowly varying systems x_ ¼ AðtÞx, IEEE Transactions on Automatic Control AC-13 (1968) 742–743. [23] T.R. Kane, R.R. Ryan, A.K. Banerjee, Dynamics of a cantilever beam attached to a moving base, Journal of Guidance Control and Dynamics 10 (1987) 139–151.
Trajectory tracking control of robots with flexible links S. Kemal Ider a
a,*
€ zg€ , M. Kemal O oren a, Volkan Ay
b
Mechanical Engineering Department, Middle East Technical University, Ankara 06531, Turkey b Mercedes-Benz T€urk, Hadımk€ oy, I_ stanbul, 34002 Turkey Received 17 July 2000; accepted 1 March 2002
Abstract A new method is developed for the end-effector trajectory tracking control of robots with flexible links. In order to cope with the non-minimum phase property of the system, the closed-loop poles are placed at desired locations using full state feedback. The dynamic equations are linearized about the rigid motion. A composite control law is designed to track the desired trajectory while at the same time the internal dynamics is stabilized. The proposed method is valid for all types of manipulators with any degree of freedom. A two link manipulator is simulated to illustrate the performance of the method. Ó 2002 Elsevier Science Ltd. All rights reserved.
1. Introduction Light-weight flexible arms will most likely constitute the next generation robots due to their large payload carrying capacities at high speeds and less power demand. However, because of the vibrations during the motion, the control problem of robots with flexible members are more complex compared to rigid robots. Some researchers considered point-to-point position control or regulation of single link and multilink robots in which the main task is to suppress the residue vibrations [1–3]. Book et al. [1] designed a full state feedback law for a two link manipulator. Pham et al. [2] developed an inverse dynamics controller with a joint PD action which is stable for positioning but unstable for trajectory tracking. Chaloub and Ulsoy [3] studied robust pole assignment and optimal regulator controls using a linearized model of the structure. In many applications, however, tip trajectory tracking is required. Tip tracking control of flexible link manipulators has the difficulty of the non-minimum phase property due to the finite *
Corresponding author. Tel.: +90-312-210-5289; fax: +90-312-210-1266. E-mail address: [email protected] (S.K. Ider).
0094-114X/02/$ - see front matter Ó 2002 Elsevier Science Ltd. All rights reserved. PII: S 0 0 9 4 - 1 1 4 X ( 0 2 ) 0 0 0 4 3 - 5
1378
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
speed of propagation of the mechanical wave along the links since the joint actuators are noncolocated. The transfer function between the input (torque) and the output (end-effector position) is non-minimum phase, i.e. it has positive real zeros. These zeros become poles of the inverse dynamic system transfer function. Cannon and Schmidtz [4] applied a linear quadratic optimal control approach to a one link flexible robot arm in which the non-minimum phase property was first demonstrated. After that, many researchers considered different approaches to the control of one link flexible arms [5–8]. Siciliano and Book [5] utilized singular perturbation approach where a slow control for the slow rigid subsystem and a fast stabilizing control for the fast flexible system are considered, assuming that the links of the manipulator are not too flexible so that the perturbation parameter is sufficiently small. Chen and Yang [6] considered the rigid body mode and the first flexible mode as the nominal model and the other modes as unmodeled dynamics. Kwon and Book [7] separated the inverse system into its causal and anticausal parts, and obtained the feedforward torque history by integrating the causal part of the equations forward in time and the anticausal part backward in time. Zhu et al. [8] lumped the flexible link into a simple spring-mass unit and applied a two-loop PD type controller. For the trajectory tracking control of multilink manipulators various approaches have been considered [9–16]. Asada et al. [9] developed an iterative inverse dynamics algorithm using a virtual link coordinate system. Bayo et al. [10] presented an inverse dynamics solution procedure to obtain non-causal force trajectories by an iterative frequency domain approach. Paden et al. [11] described a control law making use of the feedforward torques as obtained by Bayo et al. [10] and a passive joint control. Zhao and Chen [12] utilized a feedforward plus feedback control structure where the feedforward signal is generated by a stable inversion procedure which is based on an iterative solution of a two point boundary value problem. Moallem et al. [13] and Damaren [14] investigated inversion techniques based on output redefinition in which suitable outputs which render the system minimum phase are utilized. Siciliano et al. [15] and Moallem et al. [16] applied singular perturbation approach to multilink manipulators. In this paper a controller is designed such that while the end-effector tracks the desired trajectory, the internal system dynamics are stabilized. The non-linear dynamic equations are linearized about the rigid motion (the motion that would occur if all links were rigid) and full state feedback is utilized. For the closed-loop system to be asymptotically stable it must be slowly varying as defined in Refs. [21,22]. The stability condition is achieved by placing the poles at a sufficient distance from the imaginary axis [22]. The bias term that arises due to linearizing about the rigid motion is compensated by partitioning the control input and separating the related subsystem. A two link robot is simulated to illustrate the performance of the algorithm.
2. Dynamic equations and linearization The deformation of each link is defined relative to a link reference frame which follows the global motion of the link in a manner consistent with the boundary conditions. Let h ¼ ½h1 ; . . . ; hn T be the vector of joint variables where n is the degree of freedom of the manipulator T when it is assumed rigid, and let g ¼ ½g1 ; . . . ; gm be the vector of elastic modal variables of all links. The dynamic equations of the manipulator can be written as below [17]:
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
r 0 T Mrr Mre h€ Q ¼ þ e þ ee sym M Q Kg 0 g€
where
Mrr Mre Mðh; gÞ ¼ sym Mee
1379
ð1Þ
is the inertia matrix, r Q _ ðQðh; g; h; g_ Þ ¼ Qe contains Coriolis, centrifugal and gravitational terms, superscripts r and e refer to rigid body (joint) and elastic degrees of freedom respectively, K is a block diagonal matrix whose diagonal submatrices are the stiffness matrices of the links in terms of modal variables and T ¼ ½T1 ; . . . ; Tn T is the vector of joint actuator torques and forces. The equations will be linearized about the rigid motion, which is the motion of the manipulator when all links are assumed rigid, corresponding to the desired trajectory of the end-effector. To this end h, g and T can be written as h ¼ h þ Dh
ð2aÞ
g ¼ Dg
ð2bÞ
ð2cÞ
T ¼ T þ DT
where superscript refers to the rigid motion (g ¼ 0), i.e. T is the vector of torques required by the rigid manipulator and h is the joint variable vector of the rigid manipulator, and Dh, g and DT are the variations from the rigid motion. Substituting Eqs. (2a,c) into Eq. (1), neglecting higher order variations and canceling the terms corresponding to the rigid system, the linear equations are obtained in the following form: ML ðh Þ€z þ CL ðh ; h_ Þ_z þ KL ðh ; h_ ; h€ Þz ¼ WL ðh ; h_ ; h€ Þ þ JDT T T ,
T
ð3Þ
T
J ¼ ½ In 0 and In is n n identity matrix. where z ¼ ½ Dh g The output of the system is the n dimensional vector of the end-effector position and orientation variables, RE ðh; gÞ. The difference between the end-effector position and orientation of the flexible and rigid manipulators is DRE ¼ RE ðh; gÞ RE ðh ; 0Þ
ð4Þ
Using Eqs. (2a,c), Eq. (4) can be expressed as DRE ¼ Dðh Þz
ð5Þ
Defining the 2ðn þ mÞ dimensional state vector as x ¼ ½ DhT
gT
Dh_T
g_ T
T
the state space representation of the model is x_ ¼ Ax þ Bu þ Z y ¼ Cx
ð6Þ ð7Þ ð8Þ
1380
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
where u ¼ DT represents the control vector, A is the 2ðn þ mÞ 2ðn þ mÞ system matrix, B is the 2ðn þ mÞ n input matrix, y ¼ DRE is the output vector, C is the n 2ðn þ mÞ output matrix and Zðh ; h_ ; h€ Þ is a known bias term. A, B, C and Z are 0 Inþm ð9Þ A¼ M 1 M 1 L KL L CL 0 B¼ ð10Þ M 1 L J C ¼ ½D 0 0 Z¼ M 1 L WL
ð11Þ ð12Þ
Since A, B, C and Z depend on the rigid motion variables h , h_ and h€ , they are time dependent. The appearance of the bias term Z in Eq. (7) is due to the fact that the rigid motion about which the equations of the flexible manipulator (Eq. (1)) are linearized does not totally satisfy Eq. (1).
3. Control law design The control torques should be designed in order to reduce the deviation of the end-effector position and orientation from the desired trajectory, and at the same time yield the internal system dynamics to be stable. To this end, first the known bias term Z should be eliminated from the state space equations. This can be done by separating the original system into two subsystems. Denoting the first and the second subsystems with overscripts ^ and ~ respectively, x, u and y can be written as ^þx ~ x¼x
ð13aÞ
u¼^ uþ~ u
ð13bÞ
y¼^ yþ~ y
ð13cÞ
Using Eqs. (13a–c), Eqs. (7) and (8) become ^_ þ x ~_ ¼ Að^ ~Þ þ Bð^ x xþx uþ~ uÞ þ Z
ð14Þ
^ ~Þ yþ~ y ¼ Cð^ xþx
ð15Þ
Separation of the relevant terms in Eqs. (14) and (15) yield the two subsystems as ^_ ¼ A^ x x þ B^ uþZ
ð16Þ
^ y ¼ C^ x
ð17Þ
~_ ¼ A~ x x þ B~ u
ð18Þ
~ y ¼ C~ x
ð19Þ
and
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
1381
In order to obtain a stable solution to Eq. (16), the first part of the control, ^u is set as bx ^ ^ u ¼ K b is a n 2ðn þ mÞ gain matrix. Using Eq. (20), Eq. (16) can be written as where K b Þ^ ^_ ¼ ðA B K x xþZ
ð20Þ
ð21Þ
b has to ^. The vector of initial values x ^o can be arbitrarily taken as zero. K which can be solved for x be chosen such that the poles of Eq. (21) are placed properly for stability. Then ^y can be computed from Eq. (17). Since the end-effector tracking error is desired to be zero (i.e. y ¼ 0), the desired ~y is ~ y ¼ C^ x y ¼ ^
ð22Þ
The second part of the control, ~ u is designed to stabilize the second subsystem dynamics and to track the desired trajectory, as ex ~ ~ þ Hð~ yÞ ð23Þ u ¼ K y ~ e is n 2ðn þ mÞ feedback gain matrix and H is n n end-effector position error gain where K matrix. Substitution of Eq. (19) into Eq. (23) and the resulting equation into Eq. (18) yields ~_ ¼ ðA BKÞ~ x x þ BH~ y
ð24Þ
where e þ HC K¼K
ð25Þ
The matrix K will be chosen in order to place the poles properly for stability. For finding a ~_ ¼ 0 and x ~¼x ~ , at which Eq. (24) suitable H, one can consider the equilibrium conditions where x yields ~ ¼ ðA BKÞ 1 BH~ y x
ð26Þ
Premultiplying both sides of Eq. (26) by C, one obtains ~ y ¼ C~ x ¼ CðA BKÞ 1 BH~ y
ð27Þ
H is found from Eq. (27) as H ¼ ½ CðA BKÞ 1 B
1
ð28Þ
Note that, since Eqs. (21) and (24) have similar characteristics, it is convenient to use the same b and K the same as each other. After obtaining H pole locations for both subsystems, i.e. to use K e from Eq. (25). from Eq. (28) one can compute the feedback gain K From Eq. (2c), the total control torque vector required for trajectory tracking is uþ~ u T ¼ T þ ^
ð29Þ
b and K that correspond to the selected pole locations, a pole placement In order to determine K method shown by Brogan [18] is utilized. For a brief explanation of this method, consider the eigenvalue problem of Eqs. (21) and (24) written as below.
1382
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
ðA BKÞwi ¼ ki wi
ð30Þ
where ki , i ¼ 1; . . . ; 2ðn þ mÞ are the eigenvalues (poles) and wi are the corresponding eigenvectors of A BK. Eq. (30) can be written as wi ¼ 0; i ¼ 1; . . . ; 2ðn þ mÞ ð31Þ ½ ðki I AÞ B Kwi wi For each ki chosen, n solution vectors for can be found which form a basis for the null Kwi space of the 2ðn þ mÞ 2ðn þ mÞ þ n matrix ½ ðki I AÞ B . To obtain a unique K, a linear combination of the solution vectors is selected. This means an arbitrary selection of the n dimensional vector bi ¼ Kwi . Denote the matrix formed by the selected null space vectors for all ki ’s by G, which can be partitioned as w G¼ ð32Þ b where w is 2ðn þ mÞ 2ðn þ mÞ matrix whose columns are the corresponding eigenvectors wi , and b is n 2ðn þ mÞ matrix whose columns are the selected bi , i.e. Kw ¼ b. Hence K can be obtained as K ¼ bw 1
ð33Þ
Since multiple input systems allow an infinite number of choices for the gain matrix K (for a given set of eigenvalues), the designer can make choices. It is desired to make w as nearly orthogonal as possible because this improves its invertibility robustness and tends to give less interaction between modes of the closed-loop system. This orthogonality usually improves system sensitivity to parameter variations [18]. b are obtained by Eq. (33), H and K e are computed from Eqs. (28) and (25) reOnce K and K b, K e and H are updated during the motion at selected time intervals. In fact spectively. The gains K b e the computation of K , K and H at different intervals can be done off-line for any desired endeffector trajectory. Furthermore, the numerical integration of Eq. (21) can also be done off-line. ^ and from Eq. (20) ^ Hence x u can be obtained for each control sampling time. Consequently the ~ is obtained by computations which must be done on-line are only Eqs. (23) and (29). In Eq. (23) x ^ from the measured deviations of the states from the rigid motion, ~y is computed by subtracting x Eq. (19) and ~ y is computed by Eq. (22). To investigate the stability of the linear time varying control system, consider the homogeneous equation associated with Eqs. (21) and (24), which can be written as n_ ¼ CðtÞn, where C ¼ A BK. It is known that stability is not ensured by having only r0 > 0 such that [21] Re ki ðtÞ 6 r0 < 0
8i; 8t P 0
ð34Þ
However it can be shown that such a system is asymptotically stable when the variation of C is sufficiently slow as described in the Refs. [20–22]. Desoer [22] showed that a time varying system is asymptotically stable at large if r2 C_ M 6 0 4 3m
ð35Þ
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
1383
where r0 is as given by Eq. (34), C_ M ¼ supt P 0 kC_ ðtÞk and m is a constant that satisfies kesCðtÞ k 6 m e sðr0 =2Þ
8t P 0; 8s P 0
ð36Þ
To see how r0 affects Eq. (35) consider how C_ M varies with r0 . K is found from Eq. (31) by choosing ki and bi arbitrarily, which can also be written in the form, ðA ki IÞwi ¼ Bbi , where bi ¼ ðKwi Þ, i ¼ 1; . . . ; 2ðn þ mÞ. K can then be expressed as K ¼ fðk; b; tÞ where f is a function such that the effect of k on K is in the order of k. In order to express C_ M , one can write of of of K_ ¼ k_ þ b_ þ ok ob ot Like k, k_ is also arbitrary, hence it can be chosen so that its dependence on k is in the order of k. Also since b and b_ are arbitrary they can be chosen independently of k. Therefore the effect of k on K_ is also in the order of k. Then, C_ ¼ A_ B_ K BK_ implies that the effect of k on C_ is in the order of k. On the other hand r0 ¼ mint P 0 jRe k1 j, where k1 is the most dominant eigenvalue. Therefore the effect of r0 on C_ M is in the order of r0 , while the effect of r0 on the right hand side of Eq. (35) is in the order of r20 . Hence by choosing a sufficiently large r0 , one can achieve asymptotic stability in the large. 4. Numerical example A 2R planar manipular shown in Fig. 1 is considered. The tip point C is required to track a desired trajectory. m1 and m2 are the masses, L1 and L2 are the lengths, and T1 and T2 are the motor
Fig. 1. 2R planar manipulator.
1384
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
torques of the upper arm and the forearm, respectively. The upper arm is assumed rigid while the forearm is a uniform Euler–Bernoulli beam modeled in accordance with the Ritz approach. The manipulator moves in the horizontal plane so gravity is not considered. The body reference frames Ið1Þ and Ið2Þ of the upper arm and the forearm are located at A and B respectively as shown and I is the fixed reference frame. Fixed-free boundary conditions are assumed for the forearm, hence ð2Þ the unit vector I1 is always tangent to the beam centerline at B. h1 and h2 are the joint angles. The deformation of the forearm is described by the first two bending modes since the higher bending modes and the longitudinal deformation were observed to be negligible in open loop simulations of the system performed using the rigid torques. Hence in this system n ¼ 2 and m ¼ 2. The dynamic equations are derived in Appendix A where Mrr , Mre , Mee , Qr , Qe and K in Eq. (1) and ML , CL , KL and WL of the linearized system (Eq. (3)) are given. The output RE ðh; gÞ is ð1Þ ð2Þ ð2Þ the position vector of the tip point C which is L1 I1 þ L2 I1 þ ½/1 ðL2 Þg1 þ /2 ðL2 Þg2 I2 , where /1 and /2 are the mode shapes given in Appendix A. Hence RE can be expressed as L1 cos h1 þ L2 cosðh1 þ h2 Þ ½/1 ðL2 Þg1 þ /2 ðL2 Þg2 sinðh1 þ h2 Þ ð37Þ RE ðh; gÞ ¼ L1 sin h1 þ L2 sinðh1 þ h2 Þ þ ½/1 ðL2 Þg1 þ /2 ðL2 Þg2 cosðh1 þ h2 Þ The output of the linear system DRE is obtained from Eq. (4) yielding D in Eq. (5) to be L1 s1 L2 s12 L2 s12 /1 ðL2 Þs12 /2 ðL2 Þs12 Dðh Þ ¼ L1 c1 þ L2 c12 L2 c12 /1 ðL2 Þc12 /2 ðL2 Þc12
ð38Þ
where s1 ¼ sin h1 , c1 ¼ cos h1 , s12 ¼ sinðh1 þ h2 Þ and c12 ¼ cosðh1 þ h2 Þ. The data used are m1 ¼ m2 ¼ 1 kg, L1 ¼ 1 m, L2 ¼ 1:5 m and EI ¼ 50 Nm2 . The desired trajectory of the end point C is a straight line along I1 starting from rest at xCo ¼ 1 m and ending at rest at xCf ¼ 1 m, with yC ¼ 0:75 m. It is composed of an acceleration part, a constant velocity part (with 0.56 m/s velocity) and a deceleration part. For the acceleration and deceleration parts Hermite polynomials are used so that the spline functions are continuous up to the jerk. The acceleration and deceleration periods are 1.5 s each and the constant velocity period is 2 s. The desired trajectory along I1 is shown in Fig. 2. From the desired trajectory the rigid motion and torques h1 , h2 , T1 and T2 are obtained by inverse dynamic analysis of the rigid manipulator. The closed-loop poles must be placed sufficiently away from the imaginary axis for asymptotic stability. After a few trials, the following poles yielded a satisfactory performance: k1 ¼ 30, b ¼ K are k2 ¼ 50, k3 ¼ 70, k4 ¼ 100, k5 ¼ 140, k6 ¼ 190, k7 ¼ 250 and k8 ¼ 320. K obtained from Eq. (33) where G is formed by selecting the first column of the null space basis set of ½ ðki I AÞ B for each ki . The required control torques are computed from Eq. (29) as described in Section 3. The control sampling frequency is 1000 Hz. The control sampling frequency is chosen not to be close to the frequencies of the unmodeled higher modes in order to avoid their excitation. The tip position error components are shown in Figs. 3 and 4. The simulation results of the proposed method are compared with those of the computed torque method where it is assumed that the links are rigid and the control torques are computed as T ¼ Mrr ½h€ þ C1 ðh_ h_Þ þ C2 ðh hÞ þ Qr
ð39Þ
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
Fig. 2. Desired position of the tip-point C along I1 .
Fig. 3. Tip position error along I1 .
1385
1386
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
Fig. 4. Tip position error along I2 .
where in Mrr and Qr , gi ’s are set to zero. C1 and C2 are the velocity and position feedback gain diagonal matrices respectively. The plant is again the flexible system, hence the link flexibility represents unmodelled dynamics. The two poles of the computed torque method for each error component are selected as the same as the two poles of the proposed method which are closest to the imaginary axis. When poles which are further away from the imaginary axis are used, the resulting higher control torques cause larger vibrations, hence increase the position error further. It is seen from the figures that the end point tracking performance is greatly improved when the link flexibility is considered in the control law by the proposed approach. The time histories of the applied control torques T1 and T2 are given in Figs. 5 and 6, which show that the required torques are within reasonable levels. Figs. 7 and 8 show the joint variables h1 and h2 and Figs. 9 and 10 show the modal coordinates g1 and g2 .
5. Conclusions In this study, a controller design methodology is developed for end-effector trajectory tracking of flexible manipulators. The non-linear dynamic equations are linearized about the states and torques that correspond to the operation of the manipulator when the manipulator is assumed rigid. In order to cope with the non-minimum phase property of the system, the internal dynamics is made stable by placing the closed-loop poles at desired locations using full state feedback. The composite control torques are designed also to track the desired trajectory at the same time.
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
Fig. 5. Control torque applied at A, T1 .
Fig. 6. Control torque applied at B, T2 .
1387
1388
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
Fig. 7. Joint variable of the upperarm, h1 .
Fig. 8. Joint variable of the forearm, h2 .
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
Fig. 9. First modal coordinate.
Fig. 10. Second modal coordinate.
1389
1390
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
In order to have asymptotic stability in the large the closed-loop system must be slowly varying [21,22]. It is shown that the stability condition is satisfied by placing the poles sufficiently away from the imaginary axis. Since the rigid motion and torques, and the feedback gain matrices can be computed off-line as functions of time together with the solution of the subsystem corresponding to the bias term, only a small amount of on-line computations are needed for the calculation of the control torques.
Appendix A The generalized coordinate vector is q ¼ ½ hT gT T , where h ¼ ½ h1 h2 T and g ¼ ½ g1 g2 T . Let / denote the mode shape vector, i.e. / ¼ ½ /1 ðxÞ /2 ðxÞ T where x represents xð2Þ in Fig. 1 and the fixed-free flexural deflection modes are [19]. ar x ar x ar x ar x sinh þ ðcos ar þ cosh ar Þ cos cosh ðA:1Þ /r ¼ ðsin ar sinh ar Þ sin L2 L2 L2 L2 for r ¼ 1, 2, where a1 ¼ 1:875 and a2 ¼ 4:694. The position vector from B to an arbitrary point in the forearm using components in frame Ið2Þ is Rx x 12 gT ½ 0 /0 /0T dng r¼ ðA:2Þ /T g Rx ð2Þ where /0 ðnÞ ¼ d/=dn, /T g is deflection along I2 and the displacement ð1=2ÞgT ½ 0 /0 /0T dng ð2Þ along I1 is the geometrically non-linear coupling term which is necessary to account for the effect of the axial forces on bending [17,23]. The velocity of the arbitrary point is v ¼ vB þ r_ þ T x r where x r ¼ ðh_1 þ h_2 Þ½ r2 r1 . This yields 2 3 h_1 T T T / g g K 4 _ 5 L1 sin h2 / g v ¼ Jq_ ¼ ðA:3Þ h2 /T L1 cos h2 þ x 12 gT Kg x 12 gT Kg g Rx where KðxÞ is the 4 4 matrix KðxÞ ¼ 0 /0 /T0 dn. UsingR Kane’s equations, the generalized inertia force vector due to the forearm is of the arbitrary point. Since f ð2Þ ¼ qJT a dV , where a ¼ aB þ v_ þ x v is the acceleration
T y þ Q, where f ð1Þ ¼ ð1=3Þm1 L1 h€1 0 0 , one obtains M and Q as f ð1Þ þ f ð2Þ ¼ M€ Z L2 h 2 i rr 2 1 M11 ¼ 3m1 L1 þ qA2 ðL1 sin h2 /T gÞ2 þ L1 cos h2 þ x 12gT Kg dx ðA:4Þ rr ¼ qA2 M12
rr M22 ¼ qA2
0
Z Z
L2 0 L2 0
ð/T g L1 sin h2 Þð/T gÞ þ L1 cos h2 þ x 12gT Kg x 12gT Kg dx
h
2 i /T gð/T gÞ þ x 12gT Kg dx
ðA:5Þ ðA:6Þ
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
" re
M ¼
qA2
ee
M ¼ qA2 Qr1
¼ qA2
Qr2 ¼ qA2
R L2 0
Z
//T dx
L2 0 L2
ðL1 sin h2 /T gÞp1 þ L1 cos h2 þ x 12gT Kg p2 dx
ðA:8Þ
Z
L2
ðA:9Þ
/T gp1 þ x 12gT Kg p2 dx
ðA:10Þ
/T gp1 þ /p2 dx
ðA:11Þ
0
Qe ¼ qA2
ðA:7Þ
0
Z Z
L2
KT gðL1 sin h2 /T gÞ þ / L1 cos h2 þ x 12 gT Kg dx RL qA2 0 2 KT gð/T gÞ þ / x 12 gT Kg dx
1391
#
0
where q is the mass density, A2 is the cross-section area of the forearm and p1 and p2 are the Ið2Þ components of the Coriolis and centripetal acceleration vector of the forearm, p1 ¼ L1 cos h2 h_21 x 12gT Kg ðh_1 þ h_2 Þ2 2/T g_ ðh_1 þ h_2 Þ g_ T Kg_ ðA:12Þ p2 ¼ L1 sin h2 h_21 /T gðh_1 þ h_2 Þ2 2gT Kg_ ðh_1 þ h_2 Þ
ðA:13Þ
At this step simplifying assumptions valid for the motion treated in the example will be made. First, since the elastic deflections are small, quadratic terms of gi and their time derivatives will be omitted. Second, the stress stiffening of bending due to the coupling between the longitudinal and transverse motions is significant only if the axial force is sufficiently large, since a tensile force stiffens bending while a compressive force softens it. Stress stiffening is essential for high speed rotating beams, however it doesn’t have a significant effect in a deployment maneouver such as the one considered here [17]. Hence the terms involving K will be omitted. When the space integrals are evaluated after the time dependent variables are separated, one obtains M and Q as below. m1 L21 þ m2 L22 þ m2 L21 þ m2 L1 L2 cos h2 þ m2 L1 ð4:757g1 þ 47:414g2 Þ sin h2 3 m2 L22 rr rr M12 ¼ M21 ¼ þ 0:5m2 L1 L2 cos h2 þ m2 L1 ð2:378g1 þ 23:707g2 Þ sin h2 3 m2 L22 rr ¼ M22 3 1:728m 4:959m2 L2 23:707m2 L1 cos h2 2 L2 2:378m2 L1 cos h2 re M ¼ 1:728m2 L2 4:959m2 L2 9:226m2 0 Mee ¼ 0 2984:5m2 rr ¼ M11
ðA:14Þ ðA:15Þ ðA:16Þ ðA:17Þ ðA:18Þ
Qr1 ¼ m2 L1 ð2:378g1 h_22 þ 4:757g1 h_1 h_2 þ 23:707g2 h_22 Þ cos h2 þ m2 L1 ð 47:414g_ 2 ðh_1 þ h_2 Þ þ 0:5L2 h_22 þ L2 h_1 h_2 Þ sin h2
ðA:19Þ
1392
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
h
i Qr2 ¼ m2 L1 ð2:378g1 h_21 þ 23:707g2 h_21 Þ cos h2 0:5L2 h_21 sin h2 h i Qe1 ¼ m2 9:226g1 ðh_21 þ h_22 Þ 18:452g1 h_1 h_2 2:378L1 h_21 sin h2 h i Qe2 ¼ m2 2984:5g2 ðh_21 þ h_22 Þ 5969:0g2 h_1 h_2 23:707L1 h_21 sin h2
ðA:20Þ ðA:21Þ ðA:22Þ
The stiffness matrix K is as below. K¼
Z
L2
EI /00 /00T dx ¼
0
EI 114:1 0 L32
0 1:449 106
ðA:23Þ
The matrices ML , CL , KL and WL of the linear system are m1 L21 þ m2 L22 þ m2 L21 þ m2 L1 L2 cos h2 3 m2 L22 þ 0:5m2 L1 L2 cos h2 ¼ ML21 ¼ 3 ¼ ML31 ¼ 1:728m2 L2 2:378m2 L1 cos h2
ML11 ¼
ðA:24Þ
ML12
ðA:25Þ
ML13
ML14 ¼ ML41 ¼ 4:959m2 L2
23:707m2 L1 cos h2
m2 L22 3 ¼ ML32 ¼ 1:728m2 L2
ðA:26Þ ðA:27Þ
ML22 ¼
ðA:28Þ
ML23
ðA:29Þ
ML24 ¼ ML42 ¼ 4:959m2 L2
ðA:30Þ
ML33 ¼ 9:226m2
ðA:31Þ
ML44 ¼ 2984:5m2
ðA:32Þ
ML34 ¼ ML43 ¼ 0
ðA:33Þ
CL11 ¼ m2 L1 L2 h_2 sin h2 CL ¼ m2 L1 L2 ðh_ þ h_ Þ sin h
ðA:34Þ
CL13 ¼ 4:757m2 L1 ðh_1 þ h_2 Þ sin h2 CL ¼ 47:414m2 L1 ðh_ þ h_ Þ sin h
ðA:36Þ
1
12
2
1
14
CL21 ¼
m2 L1 L2 h_1
ðA:35Þ
2
2
2
sin h2
ðA:37Þ ðA:38Þ
CL31 ¼ 4:757m2 L1 h_1 sin h2 CL ¼ 47:414m2 L1 h_ sin h
ðA:39Þ
CL22 ¼ CL23 ¼ CL24 ¼ CL32 ¼ CL33 ¼ CL34 ¼ CL42 ¼ CL43 ¼ CL44 ¼ 0 2 _ € _ € _ KL12 ¼ m2 L1 L2 0:5ðh2 cos h2 þ h2 sin h2 Þ ðh1 h2 cos h2 þ h1 sin h2 Þ
ðA:41Þ
41
1
2
ðA:40Þ
ðA:42Þ
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
2 KL13 ¼ m2 L1 2:378ðh_2 cos h2 þ h€2 sin h2 Þ 4:757ðh_1 h_2 cos h2 þ h€1 sin h2 Þ
KL14 ¼ m2 L1
2 _ € _ € _ 23:707ðh2 cos h2 þ h2 sin h2 Þ 47:414ðh1 h2 cos h2 þ h1 sin h2 Þ
2 KL22 ¼ 0:5m2 L1 L2 ðh_1 cos h2 h€2 sin h2 Þ
1393
ðA:43Þ ðA:44Þ ðA:45Þ
2
KL23 ¼ KL32 ¼ 2:378m2 L1 ðh_1 cos h2 h€1 sin h2 Þ
ðA:46Þ
2
KL24 ¼ KL42 ¼ 23:707m2 L1 ðh_1 cos h2 h€1 sin h2 Þ EI KL33 ¼ 114:1 3 9:226m2 ðh_1 þ h_2 Þ2 L2 EI KL44 ¼ 1:449 106 3 2984:5m2 ðh_1 þ h_2 Þ2 L2
ðA:47Þ ðA:48Þ ðA:49Þ
KL11 ¼ KL21 ¼ KL31 ¼ KL41 ¼ KL34 ¼ KL43 ¼ 0
ðA:50Þ 2
WL3 ¼ 1:728m2 L2 ðh€1 þ h€2 Þ þ 2:378m2 L1 ðh€1 cos h2 þ h_1 sin h2 Þ 2
ðA:51Þ
WL4 ¼ 4:959m2 L2 ðh€1 þ h€2 Þ þ 23:707m2 L1 ðh€1 cos h2 þ h_1 sin h2 Þ
ðA:52Þ
WL1 ¼ WL2 ¼ 0
ðA:53Þ
References [1] W.J. Book, O. Maizza-Neto, D.E. Whitney, Feedback control of two beam, two joint systems with distributed flexibility, Journal of Dynamic Systems, Measurement and Control 97 (1975) 424–431. [2] C.M. Pham, W. Khalil, Chevallereau, A nonlinear model-based control of flexible robots, Robotica 11 (1993) 73–82. [3] N.G. Chaloub, A.G. Ulsoy, Control of a flexible robot arm: experimental and theoretical results, Journal of Dynamic Systems, Measurement and Control 109 (1987) 299–310. [4] R.H. Cannon, E. Schmitz, Initial experiments on the end-point control of flexible one-link robot, International Journal of Robotics Research 3 (1984) 62–75. [5] B. Siciliano, W.J. Book, A singular perturbation approach to control lightweight flexible manipulators, International Journal of Robotics Research 7 (1998) 79–90. [6] B.-S. Chen, T.-Y. Yang, Robust optimal model matching control design of flexible manipulators, Journal of Dynamic Systems, Measurement and Control 115 (1993) 173–178. [7] D.-S. Kwon, W.J. Book, A time-domain inverse dynamic tracking control of a single-link flexible manipulator, Journal of Dynamic Systems, Measurement and Control 116 (1994) 193–200. [8] G. Zhu, S.S. Ge, T.H. Lee, Simulation studies of tip tracking control of a single-link flexible robot based on a lumped model, Robotica 17 (1999) 71–78. [9] H. Asada, Z.-D. Ma, H. Tokumaru, Inverse dynamics of flexible robot arms: modeling and computation for trajectory control, Journal of Dynamic Systems, Measurement and Control 112 (1990) 177–185. [10] E. Bayo, P. Papadopulos, J. Stubbe, M.A. Serna, Inverse dynamics and kinematics of multi-link elastic robots: an iterative frequency approach, International Journal of Robotics Research 8 (1989) 49–62. [11] B. Paden, D. Chen, R. Ledesma, E. Bayo, Exponentially stable tracking control for multijoint flexible-link manipulators, Journal of Dynamic Systems, Measurement and Control 115 (1993) 53–59. [12] H. Zhao, D. Chen, Tip trajectory tracking for multilink flexible manipulators using stable inversion, Journal of Guidance, Control and Dynamics 21 (1998) 313–320.
1394
S.K. Ider et al. / Mechanism and Machine Theory 37 (2002) 1377–1394
[13] M. Moallem, R.V. Patel, K. Khorasani, An inverse dynamics control strategy for tip position tracking of flexible multi-link manipulators, Journal of Robotic Systems 14 (1997) 649–658. [14] C.J. Damaren, Modal properties and control system design for two-link flexible manipulators, International Journal of Robotics Research 17 (1998) 667–678. [15] B. Siciliano, J.V. Prasad, A.J. Calise, Output feedback two time scale control of multilink flexible arms, Journal of Dynamic Systems, Measurement and Control 114 (1992) 70–77. [16] M. Moallem, K. Khorasani, R.V. Patel, An integral manifold approach for tip position tracking of flexible multilink manipulators, IEEE Transactions on Robotics and Automation 13 (1997) 823–837. [17] S.K. Ider, F.M.L. Amirouche, Influence of geometric nonlinearities in the dynamics of flexible treelike structures, Journal of Guidance, Control and Dynamics 12 (1989) 830–837. [18] L.W. Brogan, Modern Control Theory, third ed., Prentice Hall, 1991. [19] L. Meirovitch, Elements of Vibration Analysis, second ed., McGraw-Hill, 1986. [20] T.E. Fortmann, K.L. Hitz, An Introduction to Linear Control Systems, Marcel Dekker, 1977. [21] H.H. Rosenbrock, The stability of linear time-dependent control systems, Journal of Electronics and Control 15 (1963) 73–80. [22] C.A. Desoer, Slowly varying systems x_ ¼ AðtÞx, IEEE Transactions on Automatic Control AC-13 (1968) 742–743. [23] T.R. Kane, R.R. Ryan, A.K. Banerjee, Dynamics of a cantilever beam attached to a moving base, Journal of Guidance Control and Dynamics 10 (1987) 139–151.