A Systematic Approach for Designing Analytical Dynamics and Servo Control of Constrained Mechanical Systems

382

IEEE/CAA JOURNAL OF AUTOMATICA SINICA, VOL. 2, NO. 4, OCTOBER 2015

A Systematic Approach for Designing Analytical Dynamics and Servo Control of Constrained Mechanical Systems Xiaoli Liu, Shengchao Zhen, Kang Huang, Han Zhao, Ye-Hwa Chen, and Ke Shao

Abstract—A systematic approach for designing analytical dynamics and servo control of constrained mechanical systems is proposed. Fundamental equation of constrained mechanical systems is first obtained according to Udwadia-Kalaba approach which is applicable to holonomic and nonholonomic constrained systems no matter whether they satisfy the D’Alember’s principle. The performance specifications are modeled as servo constraints. Constraint-following servo control is used to realize the servo constraints. For this inverse dynamics control problem, the determination of control inputs is based on the Moore-Penrose generalized inverse to complete the specified motion. Secondorder constraints are used in the dynamics and servo control. Constraint violation suppression methods can be adopted to eliminate constraint drift in the numerical simulation. Furthermore, this proposed approach is applicable to not only fully actuated but also underactuated and redundantly actuated mechanical systems. Two-mass spring system and coordinated robot system are presented as examples for illustration. Index Terms—Constrained system, fundamental equation, servo constraint, numerical drift, underactuated system, redundantly actuated system

I. I NTRODUCTION

I

N order to control the constrained discrete mechanical system, the dynamic model should first be obtained. Obtaining equations of motion for constrained discrete mechanical systems is one of the central issues in multi-body dynamics. The problem has been energetically and continuously worked on by many scientists, engineers and mathematicians since constrained motion was initially described by Lagrange[1] . He invented the special Lagrange multiplier method to deal with constrained motion. However, the Lagrange multiplier method relies on problem-specific approaches to the determination of

Manuscript received August 13, 2014; accepted May 17, 2015. This work was supported by Natural Science Foundation of Anhui Province (1508085SME221). Recommended by Associate Editor Changyin Sun. Citation: Xiaoli Liu, Shengchao Zhen, Kang Huang, Han Zhao, Ye-Hwa Chen, Ke Shao. A systematic approach for designing analytical dynamics and servo control of constrained mechanical systems. IEEE/CAA Journal of Automatica Sinica, 2015, 2(4): 382−393 Xiaoli Liu, Kang Huang, Han Zhao, and Ke Shao are with Hefei University of Technology, Hefei 230009, China (e-mail: [email protected]; [email protected]; [email protected]; [email protected]). Shengchao Zhen is with Hefei University of Technology, Hefei 230009, China, and also with Georgia Institute of Technology, GA 30332, USA (email: [email protected]). Ye-Hwa Chen is with Georgia Institute of Technology, GA 30332, USA (e-mail: [email protected]).

the multipliers and it is often very difficult to find the multipliers to obtain the explicit equations of motion for systems which have large number of degrees of freedom and a mass of non-integrable constraints. Gauss[2] introduced a general, new principle of mechanics for handling constrained motion. Gauss’s principle gives a clear description of the general nature of constrained motion in terms of the minimization of a function of the accelerations of the particles of a system. Formulations of the equations of constrained motion, when the constraints satisfy D’Alember’s principle, were independently offered by Gibbs[3] and Appell[4] . Pars[5] in his treatise on the analytical dynamics refers to the Gibbs-Appell equations as “probably the most comprehensive equations of motion so far discovered”. But Gibbs-Appell equations require a felicitous choice of problem-specific quasi-coordinates and suffer from similar problems in dealing with systems with a large number of degrees of freedom and many non-integrable constraints. Dirac[6] developed, using Poisson brackets, a recursive scheme for determining the Lagrange multipliers for singular Hamiltonian systems where the constraints do not exactly depend on time. Udwadia and Kalaba[7−8] obtained a concise explicit set of equations of motion for constrained discrete dynamical systems which lead to a simple and new fundamental view of Lagrangian mechanics. They derived the fundamental equation of motion that describes the dynamics of constrained systems derived from Gauss’s principle which seems somewhat less popular than the principles of Lagrange, Hamilton, Gibbs, and Appell. The equations can deal with holonomic and also non-holonomic constraints. Udwadia and Kalaba[9−10] observed that all the above researches have used D’Alember’s principle as their starting point which indicates that the forces of constraints are considered to be ideal and the total work done by the forces of constraints under virtual displacement is always zero. This assumption works well in many situations and is regarded as the core of classical analytical dynamics, but it is not applicable when the constraints are nonideal. Thus, Udwadia and Kalaba generalized their previous equations to constrained mechanical systems that may not satisfy D’Alember’s principle. In this paper, we use fundamental equation of constrained system proposed by Udwadia and Kalaba to get the dynamic equations of motion of the constrained mechanical system. According to this approach, the unconstrained system is first considered whose equations of motion can be obtained by

LIU et al.: A SYSTEMATIC APPROACH FOR DESIGNING ANALYTICAL DYNAMICS AND SERVO CONTROL OF · · ·

ngian mechanics in terms of the generalized coordinates. Then constraint equations are written in the form of second order equation. The dynamic equations can be obtained by imposing the additional generalized forces of constraint obtained from the second order constraint equations upon the unconstrained system. Based on the obtained dynamic model, constraintfollowing servo control is used to realize the performance requirement modeled as servo constraints (also called active constraints, program constraints or prescribed constraints). That is to use a set of servo controls to generate the appropriate constraint force for the system to obey the constraints. We know that Newton-Euler mechanics divides forces into internal and external force and the motion is structureless. While analytical mechanics divides forces into impressed (or given) and constraints. The motion is under constraints and hence has a structure. As illustrated above, over the past century and a half, key developments in Lagrangian mechanics include the Maggi equation[11−12] , the Boltzmann and Hamel equation[12−14] , the Gibbs and Appell equation[3−5, 14] , and the Udwadia and Kalaba equation[8] . However, the emphasis of all these developments mainly falls into the passive constraint (also called material constraint) problem in which the constraint is followed in a passive manner. That is, as the constraint is determined, the environment (such as the structure of the machine) can generate the required constraint force automatically. Taking a particle constrained to move on a table surface as an example, the table surface can automatically generate the constraint force to support the particle. One is not necessary to consider how to generate the constraint force. So, the generation of constraint force is often considered to be a design task: design the machine structure to generate the constraint force automatically. We consider the issue of generation of constraint forces as a control task (that is servo constraint problem), rather than a design task. The servo constraint problem has rarely been studied in analytical mechanics, perhaps because the concept of servos is relatively new in analytical mechanics. The past efforts on this problem focused on individual examples (e.g., [15−16]) rather than systematic framework. In the article of Chen[17] , a systematic approach for the servo constraint problem is proposed. A system is equipped with a set of servo controls to provide the required constraint force. Then, the system obeys a set of required constraints. Through the use of servo controls, the constraint force is generated, not through the passive environment. The main contributions of this paper are fivefold. First, Udwadia-Kalaba approach is described in detail. The procedure of obtaining fundamental equation of constrained motion is shown. Second, based on the dynamic model obtained by Udwadia-Kalaba approach, constraint-following servo control is proposed to realize the performance requirement modeled as servo constraints. Third, the inverse dynamics control problem based on Moore-Penrose generalized inverse, second-order constraints and constraint violation suppression utilized in the proposed approach are explained. Fourth, the approach is novel and systematic. It is applicable to not only fully actuated but also underactuated and redundantly actuated mechanical systems. Fifth, two-mass spring system and coordinated robot

383

system are presented as examples to specifically illustrate the approach incorporating Udwadia-Kalaba approach and constraint-following servo control. A. Fundamental Equation of Constrained Systems According to Udwadia-Kalaba approach, first we should consider an unconstrained discrete dynamical system whose configuration is described by the n generalized coordinates q := [q1 , q2 , . . . , qn ]T . Its equation of motion can be obtained, using Newtonian or Lagrangian mechanics, by the relation M (q, t)¨ q = Q(q, q, ˙ t),

(1)

n×n

where M (q, t) ∈ R is symmetric and positive definite inertia matrix, q˙ ∈ Rn is the velocity and q¨ ∈ Rn is the acceleration, Q(q, q, ˙ t) ∈ Rn is the force imposed on the system whose constraints are released. The imposed forces could include centrifugal force, gravitational force and control input. The generalized acceleration of the unconstrained system, which we denote by a(q, q, ˙ t), is thus given by q¨ = M −1 (q, t)Q(q, q, ˙ t) = a(q, q, ˙ t).

(2)

Second, constraints present in the system should be considered. We shall assume that the system is subjected to h holonomic constraints of the form ϕi (q, t) = 0,

i = 1, 2, . . . , h,

(3)

and m − h nonholonomic constraints of the form ϕi (q, q, ˙ t) = 0,

i = h + 1, h + 2, . . . , m.

(4)

We can get the differentiation of the usual constraint equations in Lagrangian mechanics which are usually in Pfaffian form, that is to say, under the assumption of sufficient smoothness, take time derivative once on non-holonomic constraints and twice on holonomic constraints, then it will provide the constraint equations in the matrix form: ˆ q, A(q, ˙ t) = ˆb(q, q, ˙ t), (5) ˆ q, where A(q, ˙ t) is referred to as constraint matrix and ˆb(q, q, ˙ t) is a m-vector. The final step is to form the explicit equations of motion with constraints. Due to the presence of constraints, additional “generalized forces of constraints” should be imposed on the system. So, the actual explicit equation of motion of the constrained system could be assumed to take the form: M (q, t)¨ q = Q(q, q, ˙ t) + Qc (q, q, ˙ t), c

(6)

n

where Q (q, q, ˙ t) ∈ R , arising by virtue of the holonomic and non-holonomic constraints, is the additional term of forces imposed on the system. In Lagrangian mechanics, Qc (q, q, ˙ t) is considered to be ideal and it is governed by the usual D’Alember’s principle which indicates that forces of constraint do zero work under virtual displacements. However, the constraints can also be nonideal. Ideal constraints generate ideal constraint forces which are subject to D’Alember’s principle, while nonideal constraints generate nonideal constraint forces such as friction force, electro-magnetic force and so on. If there exist both ideal and nonideal constraints in the system, Udwadia and Kalaba put Qc (q, q, ˙ t) in the form of Qc (q, q, ˙ t) = Qcid (q, q, ˙ t) + Qcnid (q, q, ˙ t),

(7)

384

IEEE/CAA JOURNAL OF AUTOMATICA SINICA, VOL. 2, NO. 4, OCTOBER 2015

where Qcid (q, q, ˙ t) is the ideal constraint force and Qcnid (q, q, ˙ t) is the nonideal constraint force. Udwadia and Kalaba generalize D’Alember’s principle to include forces of constraint that may do positive, negative, or zero work under virtual displacement at any instant of time during the motion of the constrained system, that is to say, they extend the Lagrange’s form of D’Alember’s principle to include nonideal constraints. Now, we denote constraint force as c(q, q, ˙ t) ∈ Rn . The constraint force does the work W = v T c (the same as the work done by Qc (q, q, ˙ t) in any ˆ q, displacement v which subjects to A(q, ˙ t)v = 0. So we write it down in the form of (we will omit arguments of functions where no confusions may arise from now on)

nonideal constraint forces by using a new fundamental principle governing the motion of constrained systems. The equation of motion obtained by using Udwadia and Kalaba’s approach is general, simple and understandable. Remark 2. The advantage of determining the explicit equations of motion for this constrained coordinated robot system through Udwadia-Kalaba equation is obvious. Compared to the Lagrange multiplier method, there is no need for this new approach to determine the multiplier which is often very difficult to obtain for systems having a large number of degrees of freedom and many non-integrable constraints. Also, if using the formulations offered by Gibbs, Volterra, Appell and Boltzmann, we will require a proper quasi-coordinates, but we do not have this problem using the new initiated approach.

W = v T Qc = v T c,

(8)

which is the extended Lagrange’s form of D’Alembert’s principle. The work done by the ideal constraint force Qcid under virtual displacements is: v T Qcid = 0,

(9)

while the work done by nonideal constraint force is: v T Qcnid 6= 0.

(10)

Udwadia and Kalaba have proved that the ideal constraint force takes the form Qcid

ˆ = M B (ˆb − AM 1 2

ˆ+

−1

Q),

(11)

and non-ideal constraint force can be written in the form 1 − 12 ˆ + B)M ˆ Qcnid = M 2 (I − B c,

(12)

ˆ = AM ˆ where B and the superscript “+” denotes the Moore-Penrose generalized inverse[18−19] . From (6), (7), (11) and (12), the explicit equation of motion that governs the evolution of the constrained system (including both ideal and nonideal constraints) is − 12

1 − 12 ˆ + (ˆb − AM ˆ −1 Q) + M 21 (I − B ˆ + B)M ˆ M q¨ = Q + M 2 B c, (13) where the vector c is determined by the mechanician and could be obtained by experimentation and/or observation. When c is always zero, (13) reduces to the D’Alember’s principle, which means the total work done under virtual displacement is zero, the constraints are ideal, the constraint force is 1 ˆ + (ˆb − AM ˆ −1 Q), Qc = Qcid = M 2 B (14)

and the explicit equation of motion of the constrained system (only including ideal constraints) is ˆ + (ˆb − AM ˆ −1 Q), M q¨ = Q + M 2 B 1

(15)

which will be applied in the coordinated robot problem for there is no nonideal constraints in the system. Remark 1. Udwadia and Kalaba have provided explicit general equations of motion for constrained discrete dynamical systems with their new initiated approach which is applicable to all holonomic and nonholonomic constrained systems no matter whether they satisfy the D’Alember’s principle. They generalize Lagrangian mechanics to include both ideal and

II. C ONSTRAINT- FOLLOWING SERVO CONTROL APPROACH According to Chen’s constraint-following servo control approach, the system is now equipped with a set of servo controls which generate the active external forces to manipulate the behavior of the system. Suppose that the system is to follow a set of constraints which are determined by system performance requirements. The constraints are realized by servo controls instead of the passive environment. So, the equation of motion (6) of the system is now described as M q¨ = Q + Qc + Qcs ,

(16)

Qcs

where is the constraint force provided by the active servo controls which cause the system to move while observe the constraints as time evolves. This is the essence of the servo constraint problem and it is main difference from the passive constraint problem. The constraint force is provided by the active servo controls. Based on the available controls, the structure of the constraint force is predetermined as Qcs = Bu,

(17)

where B is determined by the structure of the available servo controls and u is the actual servo control. Consider the following constraints which the system is to follow: n X Ali (q, t)q˙i + Al (q, t) = 0, l = 1, . . . , m, (18) i=1

where m ≥ 1 and Al (·) are both C l in q and t. These constraints imply restrictions on the velocities as well as the displacement, and are the first order form of the constraints. Now the constraints are converted into second order form. Differentiating constraint equation (6) with respect to t yields n n X X d d Ali (q, t)q˙i + Ali (q, t)q¨i + Al (q, t) = 0, (19) dt dt i=1 i=1 where n

X ∂Ali (q, t) d ∂Ali (q, t) Ali (q, t) = q˙k + , dt ∂qk ∂t i=1 and

(20)

n

X ∂Al (q, t) d ∂Al (q, t) Al (q, t) = q˙k + . dt ∂q ∂t k i=1

(21)

LIU et al.: A SYSTEMATIC APPROACH FOR DESIGNING ANALYTICAL DYNAMICS AND SERVO CONTROL OF · · ·

Equation (19), the second order form of the constraints, can be rewritten as n n X X d d Ali (q, t)q¨i = − Ali (q, t)q˙i − Al (q, t) dt dt i=1 i=1 =: bi (q, ˙ q, t), l = 1, . . . , m,

(22)

or, in matrix form, A(q, t)¨ q = b(q, ˙ q, t),

(23)

where A = [Ali ]m×n and b = [b1 , b2 , . . . , bm ]T . Equation (23), the second order form of the constraints[20−21] , is linear with respect to q¨, and so contains the acceleration q¨j explicitly and linearly. The possible acceleration component q¨ is governed by (23) when q˙ is the possible velocity. In fact, the second order form constraint equation (23) is a very general form. It includes typical constraints as illustrated in [22−23]. It also includes a number of standard control problems such as stabilization, trajectory following and optimality[20−21] . The servo constraint problem can now be stated as follows: Determine the servo control u such that when constraint force Qcs is applied to system equation (6), the resulting controlled system observes constraint (23). We see that, in general, there is no relationship between B and A. B is determined by the available servo controls and A is based on the performance requirement. Actually, in the passive constraint problem, one can still use (16) and (23) by introducing B = A. The constraint force is generated by the environment not the servo control. Thus, passive constraint problem is reduced to a special case. Assumption 1. For each q, M (q) > 0. The assumption of the inertia matrix’s positive definiteness is vital for this constraint-following servo approach. In the past, this assumption was often believed to be a fact. However, this belief has been shown to be untrue in Chen et al.[24] . Definition 1. For given A and b, the constraint equation (23) is said to be consistent if there exists at least one solution q¨. The consistency is a necessary but not a sufficient condition to the servo constraint problem. That is, if the constraint equation (23) is not consistent, then there is no solution u to the servo constraint problem. Assumption 2. Considering constraint equation (23), for each q, t, rank A(q, t) ≥ 1. Also, the constraint is consistent. (This assumption ensures the existence of the Moore-Penrose inverse of A.) Let 1

x ¨ = M 2 q¨,

1

a := M − 2 (Q + Qc ),

1

Fsc = M − 2 Qcs . (24)

x ¨=a+

Fsc

=a+M

− 12

Qcs

¯ = a + Bu,

(25)

where B = [Bil ]n×p , u = [u1 , . . . , up ]. The servo constraint problem is then to design u such that system equation (25) obeys the constraint Cx ¨ = b, (26) 1

where C = AM − 2 .

Lemma 1. Subject to Assumption 2, (23) is consistent if and only if AA+ b = b. (27) Lemma 2. Subject to Assumption 1, “for given A and b, (23) is consistent” is equivalent to “for given C and b, (26) is consistent”. Lemma 3. Subject to Assumptions 1 and 2, the solution of (26) can be given by (see proof in [17]) x ¨ = C + b + (I − C + C)c,

(28)

where c ∈ Rn is an arbitrary vector that may depend on q, q˙ and t. Lemma 4. Subject to Assumptions 1 and 2, the constraint force Fsc is given by (see proof in [17]) Fsc = C + (b − Ca) + (I − C + C)c,

(29)

where r ∈ Rn is an arbitrary vector that may depend on q, q˙ and t. Because r is arbitrary, we have min kFsc k = C + (b − Ca), r

(30)

where k·k is the Euclidean norm. The minimum norm choice of Fsc occurs when r = 0. Reference [8] considered a passive constraint problem and the constraint force is given by x ¨ = a + C + (b − Ca),

(31)

where the second term on the right hand is actually the constraint force which is generated by the passive environment. This is derived by either Lagrange’s form of D’Alembert’s principle[25] or Gauss’s principle[8] . Comparing this result with (29), we see that the constraint force passively generated by the environment is actually a special case of Fsc . Now we focus on the design of u. Definition 2. The system equation (25) is said to be servo constraint controllable if there is a control u such that the system under this control observes the constraint equation (26). ¯ ¯b, consider the equation Let ¯b := b − Ca. For given C, B, ¯ = ¯b. (C B)u

(32)

This equation can be seen as a constraint on u. ¯ ¯b, (32) is consistent and Assumption 3. Considering C, B, hence ¯ ¯ +¯b = ¯b. (C B)(C B) (33) Theorem 1. Subject to Assumptions 1-3, the system equation (25) is servo constraint controllable if and only if ¯ ≥ 1. rank[C B]

System equation (16) can be represented as

385

(34)

Furthermore, the servo control u is given by (see proof in [17]) ¯ +¯b + [I − (C B) ¯ + (C B)]s, ¯ u = (C B)

(35)

where s ∈ Rm is an arbitrary vector that may depend on q, q˙ and t. We summarize the constraint-following servo control procedure as follows.

386

IEEE/CAA JOURNAL OF AUTOMATICA SINICA, VOL. 2, NO. 4, OCTOBER 2015

1) Based on Udwadia-Kalaba approach, impose the additional generalized constraint forces Qc obtained from the second order constraint equations upon the unconstrained system to obtain Udwadia-Kalaba equation. 2) Determine the servo structure by choosing B to get Qcs in (17). Then, the dynamic system is formulated in the form of (16). 3) Determine the desirable system performance in the task space and formulate it in the form of (23). ¯ ¯b by C = AM − 12 , B ¯ = M − 12 B, ¯b = 4) Obtain C, B, 1 b − Ca, a = M − 2 (Q + Qc ). 5) Construct the feedback control u using (35).

not only for further dynamic analysis but also control design. In the constraint-following servo control design, the constraint is also in the form of second order. Historically, the second-order constraint has been rarely mentioned perhaps because of the lack of physical correspondence. Pars[5] utilized this aspect to explore a number of interesting applications. By adopting the third form of fundamental equation (constraints of large acceleration changes), one can analyze discontinuous acceleration problems (e.g., a ball rolling off the table). This third form was also used to prove the Gauss’s principle. With second-order constraint, UdwadiaKalaba equation and constraint-following servo control were able to be developed. The advantage of using the second-order constraint lies in the fact that it is linear in the acceleration. One may think that the differentiation of the constraint has resulted in the loss of information (e.g., a constant). However, the initial condition of the state satisfies the zeroth-order or holonomic constraint which means the missing information is, in fact, retained in the initial condition. The solution of differential equations of second-order constraints satisfies an analytical relation which is a corollary of the differential equations but which is unknown to the automatic computer. When differential equations are integrated by numerical and automatic integration, the computed numerical values of the solution satisfy the analytic relations with poor accuracy. This is called constraint drift. Direct numerical integration of the second-order constraint generally leads to error accumulation due to constraint drift. In order to enable stable numerical simulation without error accumulation and ensure that the small numerical error made at each integration step does not accumulate along the solution process, Baumgarte’s constraint stabilization is used[27] . This constraint violation suppression method is one of the most popular methods in engineering practice and does not require iterative constraint corrections. We use this method to conduct the numerical simulation. Instead of using the original secondorder constraint, φ = A¨ q − b = 0, we use a corresponding 2 ¨ ˙ equation, φ+2α φ+β φ = 0, α = β > 0 which has a globally asymptotically stable solution approaching zero (φ = 0) over time.

III. D ISCUSSIONS ON THE PROPOSED SYSTEMATIC APPROACH

The proposed approach is novel and systematic because it uses constraint-following servo control based on the dynamic model obtained through Udwadia-Kalaba approach. In this section, we give more discussions. A. Inverse Dynamics Based on Moore-Penrose Generalized Inverse The concept of generalized inverse was first introduced by Moore[18] . He defined a unique inverse for all rectangular and square matrices and systematically investigated properties of this inverse. Bjerhammar[26] then noted the least squares property which was not mentioned by Penrose[19] sharpened and extended Bjerhammer’s work. Due to Moore and Penrose’s important and fruitful discovery, this unique inverse is commonly called Moore-Penrose generalized inverse (MP inverse). More recently, MP inverse, which plays a role in leastsquares problem, has found renewed applicability in the field of analytical dynamics[7] . Based on MP inverse, UdwadiaKalaba equation[9] is derived. MP inverse also finds its advantage in the inverse dynamics control problem. The inverse dynamics control problem is to determine the control inputs that force the system to realize the specified motion. MP inverse overcomes the limitation of dimensionality and rank that are related to matrix’s inversion. The proposed novel approach incorporates Udwadia-Kalaba approach and constraintfollowing servo control (one method of inverse dynamics control) that are both based on MP inverse. Thus, the proposed approach is expected to solve underactuated and redundantly actuated (also called overactuated) problems. This will be detailed in the following section. B. Second-order Constraints and Constraint Drift Suppression The constraint used in Lagrangian’s equation of motion, Maggi equation, Boltzmann and Hamel equation as well as Gibbs and Appell equation is either in the zeroth- or first-order form. While, the constraint (holonomic or nonholonomic) of Udwadia-Kalaba equation is first converted into a secondorder form, which is a critical step. Due to the mathematical conformity between the system dynamics and constraint, it is much more flexible to manipulate the equations. Secondorder constraints are believed to be the most suitable form,

C. Underactuated and Redundantly Actuated Problems Underactuated problem is actually partly specified motion problem, in which the number of control inputs, equal to the number of desired system outputs, is smaller than the number of degrees of freedom. The related inverse dynamics control problem, i.e., the determination of control inputs that force the system to complete the partly specified motion, is a challenging task. Examples of underactuated systems are mobile robots, legged robots, space robots, flexible robots and so on. The difficulty of the control problem for underactuated systems is due to the reduced dimension of the input space (some joints are not equipped with actuators). When the number of actuators is greater than the mobility (this situation usually happens in a closed-chain system), the system is called redundantly actuated system. Redundant actuation prevails in general biomechanical systems (e.g., human body). It can also

LIU et al.: A SYSTEMATIC APPROACH FOR DESIGNING ANALYTICAL DYNAMICS AND SERVO CONTROL OF · · ·

be found in many robotic applications including multiple arms, multi-fingered arms, walking machines and so on. Redundant actuation provides flexible manipulation, high reliability, high driving force and so on, while the principle drawback is the complexity of singularity analysis. However, this proposed approach is expected to solve underactuated and redundantly actuated problems mainly because of MP inverse (overcoming the limitation of dimensionality) and second-order constraints (mathematical conformity). IV. T WO - MASS SPRING SYSTEM A simple representative of underactuated problems is the two-mass spring system shown in Fig. 1, in which the actuating force F is applied to mass m1 , and the motion specification is a desired relation between position x1 of mass m1 and position x2 of mass m2 . This is a two-degree-of-freedom system, n = 2, and the number of control inputs/outputs is m = 1. First, note that, T =

1 1 1 m1 x˙ 21 + m2 x˙ 22 , V = k(x2 − x1 − l)2 , 2 2 2

(36)

where k is spring stiffness and l is the length between the center of mass m1 and the center of mass m2 when the spring is under no force. We then have d ∂L ∂L − = m1 x ¨1 − k(x2 − x1 − l) = F, dt ∂ x˙ 1 ∂x1 ∂L d ∂L − = m2 x ¨2 + k(x2 − x1 − l) = 0. dt ∂ x˙ 2 ∂x2

Fig. 1.

(37) (38)

The system can be put into the form of (16) by letting · ¸ · ¸ x1 m1 0 q= , M= , (39) x2 0 x2 · ¸ k(x2 − x1 − l) Qc = , (40) −k(x2 − x1 − l) · ¸ · ¸ 0 1 Q= , Qcs = F. (41) 0 0 Furthermore, we can convert it into the form of (25) with

x ¨=

1

" a=

1

m12 x ¨1 ¨2 m22 x

or, after taking the derivation twice with respect to t, · ¸ £ ¤ x ¨1 −1 1 = |{z} 0 ¨2 | {z } x | {z } b A

#

" ¯= , B

− 12

m1 0

−1 m1 2 k(x2 − x1 − l) −1 −m2 2 k(x2 − x1 − l)

# ,

(42)

h |

−1 −m1 2

{z

−1 m2 2

C

i

"

1

1

}

#

m12 x ¨1 m22 x ¨2 | {z }

= |{z} 0 .

(46)

b

x ¨

¯ = −m−1 6= 0, For the design of control F , since C B 1 we note that the system is servo constraint controllable. The control is thus given by ¯ +¯b + [1 − (C B) ¯ + (C B)]S ¯ ¯ +¯b + 0 F = (C B) = (C B) = −(1 + m1 m−1 2 )k(x2 − x1 − l).

(47)

V. C OORDINATED ROBOT SYSTEM In this system, two coordinated robots (each robot has s links) handle the common load. Because the common load or object is rigidly held by the two end effectors (i.e., fixed together, no sliding or rotation), the two end effectors and the common object are considered as one link. The whole system is considered as a closed chain. According to Udwadia and Kalaba’s approach, first we should obtain the equations of motion of the unconstrained dynamical system (serial manipulators of n = 2s − 1 links). Then, get the constraint (the end link hinged to the ground) and rewrite the constraint equations in the form of second-order. In the end, impose the additional generalized constraint forces obtained from the second-order constraint equations upon the unconstrained system to get Udwadia-Kalaba equation which is in analytical form. As we know, the closed-form dynamic equations of any serial manipulators n links can be obtained through the systematical Lagrangian formulation. The dynamics is strongly nonlinear and can be written in the general form (48)

where q (n-vector generalized coordinates) denotes the joint angles, M (q) is the n × n manipulator inertia matrix, C(q, q) ˙ q˙ is a n-vector of centripetal and Coriolis torques (with C(q, q) ˙ a n × n matrix), G(q) is the n vector of gravitational torques and τ is the impressed (or given) force. According to the systematical derivation of [28] (pp. 104−110), we get n ³ ´ X (i)T (i) (i)T (i) M (q) = mi JL JL + JA Ii JA , (49) i=1

where mi is the mass of the link, Ii is the inertia tensor at (i) (i) the centroid, JL and JA are Jacobian matrices for linear and angular velocities of link i, respectively. Namely, (i)

(i)

(i)

(i)

(i)

(i)

vci = JL1 q˙1 + · · · + JLi q˙i = JL q, ˙

# .

(43)

The servo constraint to be realized is x2 − x1 = 2l,

(45)

q¨

or

M (q)¨ q + C(q, q) ˙ q˙ + G(q) = τ,

Two-mass spring system.

"

387

(44)

ωi = JA1 q˙1 + · · · + JAi q˙i = JA q, ˙

(50)

where vci and ωi are the velocity vector of the centroid and angular velocity vector of link i with reference to the inertial (i) (i) reference frame, JLj and JAj are the j-th column vectors of (i) (i) Jacobian matrices JL and JA . Note that, since the motion of

388

IEEE/CAA JOURNAL OF AUTOMATICA SINICA, VOL. 2, NO. 4, OCTOBER 2015

link i depends on only joints 1 through i, the column vectors are set to zero for j ≥ i. Namely,

is to say, servo control of the coordinated robot is actually a redundantly actuated problem. We will show that the proposed approach applies to this redundantly actuated problem.

(i)

(i)

(i)

JL = [JL1 . . . JLi 0 . . . 0], (i)

(i)

(i)

JA = [JA1 . . . JAi 0 . . . 0].

(51)

For the n-link manipulators, the Coriolis and centripetal torque vector C q˙ can be uniquely defined. But this does not define the matrix C uniquely. However, according to [29], a particular choice of the matrix C which guarantees the matrix M˙ − 2C to be skew-symmetric is given by ¶ n n µ 1 X ∂mik ∂mjk 1 X ∂mij q˙k + − q˙k , (52) cij = 2 ∂qk 2 ∂qj ∂qi k=1

k=1

where cij is the ij-th element of matrix C. According to the systematical derivation [28] (pp. 104−110), we also get n X

of

(53)

Fig. 2. Model of two coordinated planar robot manipulators handling the common load.

where Gi is the i-th element of G and g is the vector representing the acceleration of gravity with reference to the inertial reference frame. The closed-form dynamic equations of the n-link serial manipulators is obtained. This is considered as the equation of motion of the unconstrained dynamical system. Then, we consider the second order constraint equation and constraint force. The end link hinged to the ground is considered as the constraint. So we can get the holonomic constraints. Taking time derivation twice on the holonomic constraints, we can get the second-order constraint equations which can be written in the matrix form

First, we do not consider the constraint of the end link hinged to the ground. For this unconstrained dynamical system (the five-link manipulator), we know

Gi =

(j)

mj g T JLi ,

j=1

ˆ q, A(q, ˙ t)¨ q = ˆb(q, q, ˙ t).

(54)

According to (14), we can get the constraint force Qc . Then, we impose the additional generalized constraint force Qc obtained from the second-order constraint equations upon the unconstrained system to get Udwadia-Kalaba equation which is in analytical form. We can rewrite (48) as M (q)¨ q = Q,

(55)

where Q = τ − C q˙ − G. The Udwadia-Kalaba equation is M (q)¨ q = Q + Qc .

(56)

Remark 3. In (56), M, Q, Qc have already been obtained. It provides explicit general equations of motion for the closed chain of two coordinated robots. We have shown the equation of motion obtained by using Udwadia and Kalaba’s approach is simple and understandable. We take two coordinated planar robot manipulators (each robot has s = 3 links) to detail the approach. In this problem, s = 3, n = 2s − 1 = 5 and q = [θ1 θ2 θ3 θ4 θ5 ]T . Fig. 2 clearly shows the structure of the whole robot system. The two coordinated robots have 6 joints and every joint has one actuator. However, we can obtain the system dynamic equation (16) using 5 generalized coordinates (θ1 , θ2 , θ3 , θ4 , θ5 ) and the degree of freedom of the coordinated robot system is 3. That

l1 sin θ1 , 2 l1 x˙ 1 = − l21 sin θ1 θ˙1 , y˙ 1 = cos θ1 θ˙1 , (57) 2 where x1 , y1 , x˙ 1 , y˙ 1 are the coordinates and velocities of the centroid of the first link. So, we can get  ˙  θ1 · ¸  θ˙2  ¸ · l1  x˙ 1 sin θ1 0 0 0 0   θ˙3  , (58) vc1 = = l21   y˙ 1 cos θ 0 0 0 0 1  θ˙4  2 θ˙5 x1 =

l1 2

cos θ1 ,

y1 =

(1)

which means JL1 = [ l21 sin θ1 l21 cos θ1 ]T . Similarly, we can represent the coordinates and velocities of the centroid of the other four links by the generalized coordinates (2) (3) (4) (5) θ1 , θ2 , θ3 , θ4 , θ5 . Then, JL , JL , JL , JL can be obtained (shown in (A1)). For the angular velocity, we know £ ¤ ω1 = θ˙1 = 1 0 0 0 0 q, £ ¤ ω2 = θ˙2 = 0 1 0 0 0 q, £ ¤ ω3 = θ˙3 = 0 0 1 0 0 q, £ ¤ ω4 = θ˙4 = 0 0 0 1 0 q, £ ¤ ω5 = θ˙5 = 0 0 0 0 1 q, (59) (1)

(2)

(3)

(4)

(5)

which means JA1 = JA2 = JA3 = JA4 = JA5 = 1 and the others are zero. According to (49)-(51), we get the matrix M (shown in (A2)). According to (52), we get the matrix C (shown in (A3)). According to (53), we know g = [0 g]T and get the matrix G (shown in (A4)). The impressed force τ is 0. So, equations of motion (48) of 5-link serial manipulators (unconstrained dynamic system) is obtained through the systematical Lagrangian formulation.

LIU et al.: A SYSTEMATIC APPROACH FOR DESIGNING ANALYTICAL DYNAMICS AND SERVO CONTROL OF · · ·

Considering the constraint of the end link hinged to the ground, we get l1 cos θ1 + l2 cos θ2 + l3 cos θ3 + l4 cos θ4 + l5 cos θ5 = L, l1 sin θ1 + l2 sin θ2 + l3 sin θ3 + l4 sin θ4 + l5 sin θ5 = 0, (60) where L is the horizontal distance of the two robots. Taking time derivation twice on the holonomic constraints, we get − l1 sin θ1 θ¨1 − l2 sin θ2 θ¨2 − l3 sin θ3 θ¨3 − l4 sin θ4 θ¨4 − l5 sin θ5 θ¨5 = l1 cos θ1 θ˙12 + l2 cos θ2 θ˙22 + l3 cos θ3 θ˙32 + l4 cos θ4 θ˙2 + l5 cos θ5 θ˙2 , 4

5

l1 cos θ1 θ¨1 + l2 cos θ2 θ¨2 + l3 cos θ3 θ¨3 + l4 cos θ4 θ¨4 + l5 cos θ5 θ¨5

4

Differentiating the above equation once and twice, we get l3 sin θ3 θ˙3 , 2 l3 y˙ 3 = l1 cos θ1 θ˙1 + l2 cos θ2 θ˙2 + cos θ3 θ˙3 , 2 l3 ¨ ¨ x ¨3 = −l1 sin θ1 θ1 − l2 sin θ2 θ2 − sin θ3 θ¨3 2 l3 2 2 ˙ ˙ − l1 cos θ1 θ1 − l2 cos θ2 θ2 − cos θ3 θ˙32 , 2 l3 ¨ ¨ y¨3 = l1 cos θ1 θ1 + l2 cos θ2 θ2 + cos θ3 θ¨3 2 l 3 − l1 sin θ1 θ˙12 − l2 sin θ2 θ˙22 − sin θ3 θ˙32 . (66) 2 These second order constraints can be represented in the form of matrix as (recalling (64)) x˙ 3 = −l1 sin θ1 θ˙1 − l2 sin θ2 θ˙2 −

A¨ q = b, where

= l1 sin θ1 θ˙12 + l2 sin θ2 θ˙22 + l3 sin θ3 θ˙32 + l4 sin θ4 θ˙2 + l5 sin θ5 θ˙2 . 5

(61)

The above second-order constraint equations can be written in the matrix form ˆ q, A(q, ˙ t)¨ q = ˆb(q, q, ˙ t),

(62)

389

(67)



 −l1 sin θ1 −l2 sin θ2 − l23 sin θ3 0 0 l3 A =  l1 cos θ1 l2 cos θ2 0 0 , 2 cos θ3 0 0 1 0 0   l3 2 2 ˙ ˙ l1 cos θ1 θ1 + l2 cos θ2 θ2 + 2 cos θ3 θ˙32 b =  l1 sin θ1 θ˙12 + l2 sin θ2 θ˙22 + l23 sin θ3 θ˙32 + 2l cos(t)  . 0 (68)

We should determine the servo structure by choosing B in (17). Based on the available servo controls (see Fig. 2), we get

where Aˆ = · −l1 sin θ1 −l2 sin θ2 −l3 sin θ3 −l4 sin θ4 l1 cos θ1 l2 cos θ2 l3 cos θ3 l4 cos θ4   2 2 ˙ ˙ l1 cos θ1 θ1 + l2 cos θ2 θ2 + l3 cos θ3 θ˙32   +l4 cos θ4 θ˙42 + l5 cos θ5 θ˙52 ˆb =    l1 sin θ1 θ˙12 + l2 sin θ2 θ˙22 + l3 sin θ3 θ˙32  . +l4 sin θ4 θ˙42 + l5 sin θ5 θ˙52

¸

−l5 sin θ5 , l5 cos θ5 (63)

According to (14), we can get the constraint force Qc . We know Q = τ − C q˙ − G. So, Udwadia-Kalaba equation (56) is obtained. The two coordinated planar robots system is considered as a closed kinematic chain and the dynamic equation has already been obtained. Now we focus on the servo control of the system. We first describe the performance requirement of the object in the task space. For simulation, we choose

x ¨3 = 0, y˙ 3 =

cos(t), θ˙3 = 0.

(64)

This performance requirement is considered as constraints. We also know that x3 = l1 cos θ1 + l2 cos θ2 + y3 = l1 sin θ1 + l2 sin θ2 +

l3 2 l3 2

cos θ3 , sin θ3 .

where

   B=  

 1 −1 0 0 0 0 0 1 −1 0 0 0   0 0 1 1 0 0  , 0 0 0 −1 1 0  0 0 0 0 −1 1

(65)

(69)

(70)

u is the actual available control u1 , u2 , u3 , u4 , u5 , u6 . Then, ¯ ¯b by C = AM − 21 , B ¯ = M − 21 B, ¯b = we can obtain C, B, 1 b − Ca, a = M − 2 (Q + Qc ). So, the feedback control u can be constructed using (35). VI. N UMERICAL S IMULATION Control of this two coordinated planar robots system is a redundantly actuated control. According to (35), we know the solution of control u is not unique. For simulation, we take ¯ +¯b, u = (C B)

x3 = 1.5l, y3 = 2l (1 − cos(t)), θ3 = 0, x˙ 3 = 0, y˙ 3 = l sin(t), θ˙3 = 0, 2 l 2

Qcs = Bu,

(71)

which means s = 0. This is the minimum norm choice from all possible choices of u. For simulation, we choose m1 = m2 = m3 = m4 = m5 = 1, I1 = I2 = I3 = I4 = I5 = 1, l1 = l2 = l3 = l4 = l5 = l = 1, g = 9.8. The initial configuration of the system is presented in Fig. 3. The initial position, velocity and acceleration are shown in Table I. The second-order constraint equation does not result in the loss of of information. Because the initial condition satisfies the zeroth-order (holonomic) and

390

IEEE/CAA JOURNAL OF AUTOMATICA SINICA, VOL. 2, NO. 4, OCTOBER 2015

first-order constraint. The missing information is retained in the initial condition.

Fig. 3.

The initial configuration of the system.

TABLE I INITIAL CONDITION OF THE COORDINATED ROBOT SYSTEM i θi θ˙i θ¨i

π 3

1 0

2 − π3 0

1 2

1 2

3 0 0 0

4 π 3

0 − 21

Fig. 6.

θ3 − t.

Fig. 7.

θ4 − t.

Fig. 8.

θ5 − t.

5 − π3 0 − 12

We can see the simulation results in Figs. 4-14. From Fig. 6, we know θ3 , θ˙3 , θ¨ are all zero, which means the third link always remains horizontal. Figs. 4 and 8 show that the first and fifth link move from the initial configuration to the upwardly vertical position (where the object is in the highest vertical position y3 = l = 1) and then come back to the initial position.

Fig. 4.

Fig. 5.

θ1 − t.

θ2 − t.

Figs. 5 and 7 show that the second and fourth link move from the initial configuration to the horizontal position (where the object is in the highest vertical position) and then come back to the initial position. So link 1, 2, 4, 5’s movements are periodic which leads to the periodic moving of the object (the third link) from y3 = 0 (the lowest position) to y3 = l = 1 (the highest position) and then back to the lowest position. So the desirable performance (x3 is always 1.5l, y3 = 2l (1 − cos(t)), θ3 is always zero) is realized by the servo controls which can be seen in Figs. 9-14. For the actuators of the first robot,

LIU et al.: A SYSTEMATIC APPROACH FOR DESIGNING ANALYTICAL DYNAMICS AND SERVO CONTROL OF · · ·

391

in Fig. 9, u1 fluctuates from 0 to 10. Fig. 10 shows that u2 ’s magnitude is from about 3 to 5. From Fig. 11, we see u3 runs from minimum −8 to maximum −5.5. For the other robot, Fig. 12 shows u4 ’s amplitude is from about 4.5 to 10 while amplitude of u5 in Fig. 13 is from −4.5 to 0.5 and u6 in Fig. 14 shows its magnitude runs from minimum −5 to maximum 0.

Fig. 9.

Fig. 10.

Fig. 12.

u4 − t.

Fig. 13.

u5 − t.

Fig. 14.

u6 − t.

u1 − t.

u2 − t.

VII. C ONCLUSION

Fig. 11.

u3 − t.

We propose a novel and systematic approach for dynamic modeling and servo control of constrained mechanical systems. In this approach, Udwadia-Kalaba equation is applied to obtain the additional generalized constraint force and then get the system’s dynamic equation. Second-order constraint equations are used, which do not result in the loss of information because of the initial condition satisfying the zeroth-order

392

IEEE/CAA JOURNAL OF AUTOMATICA SINICA, VOL. 2, NO. 4, OCTOBER 2015

(holonomic) and first-order constraint. For the control part, we propose to adopt the constraint-following servo control design approach which can solve fully actuated, underactuated and redundantly actuated systems. According to this control design approach, we first represent the desirable performance as a set of second order equations. Then we consider the performance requirement as the constraint and use a set of servo controls to generate appropriate constraint forces for the system to obey the constraints. Detailed discussions of the proposed approach are presented. Two-mass spring system and coordinated robots system are presented as examples for illustration.

· =

(3) JL1

· (2)

JL2 =

=

(4) JL1

(5) JL1

= ¸

− l22 sin θ2 l2 2 cos θ2

=

−l1 sin θ1 l1 cos θ1

M14 M15 M22 M23 M24 M25 M33 M34 M35 M44 M45

C14 C15 C21

,

C25 C31 C32 C34 C35 C41 C42 C43 C45 (A1)

C51

´ + m2 + m3 + m4 + m5 l1 2 + I1 , 4 ³m ´ 2 + m3 + m4 + m5 l1 l2 cos(θ1 − θ2 ), = M21 = ´ ³ m2 3 + m4 + m5 l1 l3 cos(θ1 − θ3 ), = M31 = ³ m2 ´ 4 = M41 = + m5 l1 l4 cos(θ1 − θ4 ), 2 m5 = M51 = l1 l5 cos(θ1 − θ5 ), 2 ³m ´ 2 = + m3 + m4 + m5 l2 2 + I2 , 4 ³m ´ 3 = M32 = + m4 + m5 l2 l3 cos(θ2 − θ3 ), ´ ³ m2 4 + m5 l2 l4 cos(θ2 − θ4 ), = M42 = 2 m5 = M52 = l2 l5 cos(θ2 − θ5 ), 2 ³m ´ 3 = + m4 + m5 l3 2 + I3 , 4 ³m ´ 4 = M43 = + m5 l3 l4 cos(θ3 − θ4 ), 2 m5 = M53 = l3 l5 cos(θ3 − θ5 ), 2 ´ ³m 4 = + m5 l4 2 + I4 , 4 m5 l4 l5 cos(θ4 − θ5 ), = M54 = 2

C52

M11 =

M13

C13 =

C24

¸

, ·

¸ −l2 sin θ2 (3) (4) (5) JL2 = JL2 = JL2 = , l2 cos θ2 · l3 ¸ − 2 sin θ3 (3) JL3 = , l3 2 cos θ3 · ¸ −l3 sin θ3 (4) (5) (5) JL3 = JL3 = JL2 = , l3 cos θ3 ¸ · l4 − 2 sin θ4 (4) , JL4 = l4 cos θ4 · 2 ¸ −l4 sin θ4 (5) JL4 = , l4 cos θ4 ¸ · l5 − 2 sin θ5 (5) . JL5 = l5 2 cos θ5

M12

C12 =

C23

A PPENDIX (2) JL1

M55 =

³m

1

C53 C54 C11

³m

m5 2 l5 + I5 . 4

(A2)

´ + m3 + m4 + m5 l1 l2 sin(θ1 − θ2 )θ˙2 , ´ + m4 + m5 l1 l3 sin(θ1 − θ3 )θ˙3 , ´ + m5 l1 l4 sin(θ1 − θ4 )θ˙4 ,

2

³ m2

3

³ m2 4 = 2 m5 = l1 l5 sin(θ1 − θ5 )θ˙5 , ³2m ´ 2 = + m3 + m4 + m5 l1 l2 sin(θ2 − θ1 )θ˙1 , ³ m2 ´ 3 = + m4 + m5 l2 l3 sin(θ2 − θ3 )θ˙3 , ´ ³ m2 4 + m5 l2 l4 sin(θ2 − θ4 )θ˙4 , = 2 m5 = l2 l5 sin(θ2 − θ5 )θ˙5 , ³2m ´ 3 = + m4 + m5 l1 l3 sin(θ3 − θ1 )θ˙1 , ³ m2 ´ 3 + m4 + m5 l2 l3 sin(θ3 − θ2 )θ˙2 , = ³ m2 ´ 4 = + m5 l3 l4 sin(θ3 − θ4 )θ˙4 , 2 m5 l3 l5 sin(θ3 − θ5 )θ˙5 , = ³2m ´ 4 = + m5 l1 l4 sin(θ4 − θ1 )θ˙1 , ³ m2 ´ 4 = + m5 l2 l4 sin(θ4 − θ2 )θ˙2 , ³ m2 ´ 4 + m5 l3 l4 sin(θ4 − θ3 )θ˙3 , = 2 m5 l4 l5 sin(θ4 − θ5 )θ˙5 , = 2 m5 = l1 l5 sin(θ5 − θ1 )θ˙1 , 2 m5 = l2 l5 sin(θ5 − θ2 )θ˙2 , 2 m5 l3 l5 sin(θ5 − θ3 )θ˙3 , = 2 m5 = l4 l5 sin(θ5 − θ4 )θ˙4 , 2 = C22 = C33 = C44 = C55 = 0. (A3)

G1 = G2 =

³m

1

³ m2

2

´ + m2 + m3 + m4 + m5 gl1 cos θ1 , ´ + m3 + m4 + m5 gl2 cos θ2 , ´ + m4 + m5 gl3 cos θ3 , ´ + m5 gl4 cos θ4 ,

³ m2 3 G3 = 2 ³m 4 G4 = 2 m5 G5 = gl5 cos θ5 . 2

(A4)

R EFERENCES [1] Lagrange J L. Mechanique Analytique. Paris: Mme ve Courcier, 1787. [2] Gauss C F. Uber ein neues allgemeines grundgsetz der mechanik. Journal fur die Reine und Angewandte Mathematik, 1829, 4: 232−235 [3] Gibbs J W. On the fundamental formulae of dynamics. American Journal of Mathematics, 1879, 2(1): 49−64 [4] Appell P. Sur une forme generale des equations de la dynamique. Comptes Rendus de l’Academie des Sciences, 1899, 129: 459−460

LIU et al.: A SYSTEMATIC APPROACH FOR DESIGNING ANALYTICAL DYNAMICS AND SERVO CONTROL OF · · ·

[5] Pars L A. A Treatise on Analytical Dynamics. Connecticut: Ox Bow Press, 1979.

393

[29] Slotine J -J E, Li W P. Applied Nonlinear Control. New Jersey: Prentice Hall, 1991.

[6] Dirac P A M. Lectures on Quantum Mechanics. New York: Yeshiva University Press, 1964. [7] Udwadia F E, Kalaba R E. A new perspective on constrained motion. Proceedings of the Royal Society, 1992, 439(1906), DOI: 10.1098/rspa.1992.0158 [8] Udwadia F E, Kalaba R E. Analytical Dynamics: A New Approach. Cambridge, UK: Cambridge University Press, 1996.

Xiaoli Liu Master student at Hefei University of Technology, China. Her research interests include analytical mechanics, multi-agent, and robotics.

[9] Udwadia F E, Kalaba R E. Explicit equations of motion for mechanical systems with nonideal constraints. Journal of Applied Mechanics, 2001, 68(3): 462−467 [10] Udwadia F E. On constrained motion. Applied Mathematics and Computation, 2005, 164(2): 313−320 [11] Maggi G A. Di alcune nuove forme delle equazioni della dinamica, applicabili ai sistemi anolonomi. Atti della Reale Accademia dei Lincei, Rendiconti, Classe di Scienze Fisiche, Matematiche e Naturali, 1901, 10: 287−291 (in Italian) [12] Neimark J I, Fufaev N A. Dynamics of Nonholonomic Systems. Phode Island, Providence: American Mathematical Society, 1972.

Shengchao Zhen Lecturer at Hefei University of Technology, China and a visiting scholar in Georgia Institute of Technology, USA. His research interests include analytic mechanics, dynamics of multi-body systems, optimal control, robust control, adaptive control, fuzzy engineering, and uncertainty management. Corresponding author of this paper.

[13] Hamel G. Die lagrange-eulersche gleichungen der mechanik. Zeitschrift fr Mathematik und Physik, 1904, 50: 1−57 (in German) [14] Hamel G. Theoretische Mechanik. Berlin: Springer-Verlag, 1949. (in German) [15] Cabannes H. General Mechanics (Second edition). Massachusetts, Waltham: Blaisdell/Ginn, 1968. (translated from the original French)

Kang Huang Professor at Hefei University of Technology. His research interests include mechanical transmission, vehicles, dynamics and control.

[16] Kirgetov V I. The motion of controlled mechanical systems with prescribed constraints (servoconstraints). Journal of Applied Mathematics and Mechanics, 1967, 31(3): 465−477 (translated from original Russian) [17] Chen Y H. Constraint-following servo control design for mechanical systems. Journal of Vibration and Control, 2009, 15(3): 369−389 [18] Moore E H. On the reciprocal of the general algebraic matrix. Bulletin of the American Mathematical Society, 1920, 26: 294−395 [19] Penrose R. A generalized inverse for matrices. Proceedings of the Cambridge Philosophical Society, 1955, 51: 404−413 [20] Chen Y H. Second-order constraints for equations of motion of constrained systems. IEEE/ASME Transaction on Mechatronics, 1998, 3(3): 240−248 [21] Chen Y H. Equations of motion of constrained mechanical systems: given force depends on constraint force. Mechatronics, 1999, 9(4): 411−428 [22] Rosenberg R M. Analytical Dynamics of Discrete Systems. New York: Plenum Press, 1977. [23] Papastavridis J G. Analytical Mechanics. New York: Oxford University Press, 2002. [24] Chen Y H, Leitmann G, Chen J S. Robust control for rigid serial manipulators: a general setting. In: Proceedings of the 1998 American Control Conference. Philadelphia, PA, USA: IEEE, 1998. 912−916 [25] Udwadia F E, Kalaba R E. On motion. Journal of the Franklin Institute, 1993, 330(3): 571−577 [26] Bjerhammar A. Rectangular reciprocal matrices with special reference to geodetic calculations. Bulletin Godsique, 1951, 20(1): 188−220 [27] Baumgarte J. Stabilization of constraints and integrals of motion in dynamical systems. Computer Methods in Applied Mechanics and Engineering, 1972, 1(1): 1−16 [28] Asada H, Slotine J J E. Robot Analysis and Control. New York: John Wiley and Sons, 1985.

Han Zhao Professor at Hefei University of Technology. His research interests include mechanical transmission, magnetic machine, vehicles, digital design and manufacturing, information system, dynamics and control.

Ye-Hwa Chen received the Ph. D. degree from University of California, Berkely, USA in 1985. He is currently a professor at Georgia Institute of Technology, USA. He is a member of IEEE, ASME and Sigma Xi. His research interests include advanced control methods for mechanical manipulators, neural networks and fuzzy engineering, adaptive robust control of uncertain systems, and uncertainty management. Ke Shao Ph. D. candidate at Hefei University of Technology, China. His research interests include analytical mechanics, multi-agent, and robotics.