Download on a new approach to motion control of constrained mechanical

The 40th International Conference on
Mechanics of Solids, Acoustics and Vibrations &
The 6th International Conference on
“Advanced Composite Materials Engineering”
ICMSAV2016& COMAT2016
Brasov, ROMANIA, 24-25 November 2016
ON A NEW APPROACH TO MOTION CONTROL OF CONSTRAINED
MECHANICAL SYSTEMS
Dumitru D. Nicoara1
Transilvania University, City Brasov, ROMANIA, [email protected]
Abstract: In this paper is proposed a new approach for motion control of constrained mechanical systems. One of the aims
of this paper is to introduce to the motion control design community in this new and novel approach which is
simple yet extremely useful. The general approach of controlling complex multi-body systems was first developed
by Udwadia (2002 & 2003). The Udwadia-Kalaba’s equations of motion are more adequate to model constrained
mechanical systems rather than the Lagrange’s equations of motion at least for control purpose. The proposed approach
covers most of constraints including holonomic and nonholonomic constraints.
Keywords: control motion, constrained mechanical systems, Udwadia-Kalaba’s equations.
1. INTRODUCTION
Control of mechanical systems with nonholonomic constraints is investigated somewhat recently and relatively
small number of papers is found in literature [3], [4], [10], [11].
The motion of complex mechanical systems is often mathematically modeled by what we call their equations of
motion. Several formalisms Lagrange’s equations, Gibbs–Appell equations, generalized inverse equations
Udwadia and Kalaba [16], have been developed for obtaining the equations of motion for such structural and
mechanical systems. Though these formalisms do not all afford the same ease of use in any given practical
situation, they are equivalent to one another.
They all rely on D’Alembert’s principle. D’Alembert’s principle is equivalent to a principle that was first stated
by Gauss [8] and is referred to nowadays as Gauss’s principle of least constraint. In fact, like D’Alembert’s
principle, Gauss’s principle can be thought of as a starting point from which the machinery of analytical
dynamics can be developed [14]. For example, it has been used in Udwadia and Kalaba [15], in conjunction with
the concept of the Moore-Penrose inverse of a matrix [9], to obtain a simple and general set of equations for
holonomically and nonholonomically constrained mechanical systems when the forces of constraint are ideal.
Control of mechanical systems with nonholonomic constraints and control of mechanical systems with
kinematics constraints are investigated recently [2], [10], [11]. The last decades have shown an increasing
interest in the control of underactuated mechanical systems. Underactuated mechanical systems generate
interesting control problems which require fundamental nonlinear approaches. The linear approximation around
equilibrium points may, in general, not be controllable and the feedback stabilization problem, in general, can
not be transformed into a linear control problem. Therefore linear control methods can not be used to solve the
feedback stabilization problem, not even locally. Also, the tracking control problem can not be transformed into
a linear control problem. However, it turns out that, under certain conditions, the tracking control problem can be
solved by linear time-varying control. There are several methodologies that have been developed to date for the
control of nonlinear systems that have tracking requirements and kinematics constraints [12], [13],. The
methodology that we propose to present in this paper is inspired by a central result related to the analytical
dynamics of constrained motion [14], [15], [16]. This powerful new approach, which is obtained by using
Gauss’s principle, does not distinguish between holonomic or nonholonomic constraints.
2. GAUSS PRINCIPLE OF LEAST CONSTRAINT
In his epochal paper of 1829, Gauss [8] began by remarking that the D’Alembert principle reduced all of
dynamics to statics and that the principle of virtual works reduced all of statics to a mathematical problem.
151
2.1 Minimization form of Gauss Principle
It is well known that a 6 x 6 so-called generalized mass matrix or operational space inertia matrix) M represents
the linear and angular components of the rigid body’s inertia. This generalized mass matrix allows to write the
“acceleration energy” to be minimised in Gauss’ Principle as
E G 
1
(   Ma c )T M 1(   Ma c )
2
(1)
 g is the six-dimensional vector containing the concatenation of the three-dimensional force F and moment M
used in (1). a c is any constraint-compatible, six-dimensional acceleration vector ( r, ω ) of the rigid body. The
minimization of the “energy” in (1) takes place over all possible accelerations a c while the current positions
and velocities are given. The motion constraints give rise to a linear constraint on the accelerations, and the
following minimization procedure results:
min
(   Ma c )T M 1(   Ma c )
c
(2)
a
subject to Aa c  b
2.2. Gauss’ principle of least constraints
“The objects’ constrained accelerations are the closest possible accelerations to their unconstrained ones”
Formally, the accelerations minimize the distance
G(a) 
1
1
( a  a u )M ( a  a u )  a  a u
2
2
2
M
(3)
over the set of possible accelerations.
The matrix A and the vector b are determined by the geometry of the constraint. The solution to such a
constrained minimization problem is well known [4], [7] and uses the so-called weighted generalized inverse:

a c  AM
b  M 1 AT ( AM 1 AT )1b
(4)
Note that since mass matrix M is symmetric positive definite,
M
is well-defined non-Euclidian norm. By
1 T
v Mv of the system, this norm is usually called kinetic norm.
2
Thus, Gauss’ principle amounts to minimize kinetic distance between the generalized accelerations a and au,
over the set of possible accelerations.
Gauss principle can be stated even more simply: at any moment constrained acceleration is the closest possible
acceleration to its unconstrained accelerations.
Gauss’ Principle can be proven to be equivalent to d’ Alembert’s Principle for holonomic (“driftless”)
constraints; for non-holonomic constraints, d’Alembert’s Principle gives incorrect results, while Gauss’ Principle
remains physically meaningful.
analogy with the kinetic energy E 
3. UDWADIA-KALABA FORMULATION (UKF) AND GAUSS PRINCIPLE.
In a recent series of papers [15] and in a textbook [14] Udwadia and Kalaba, starting from the Gauss Principle
of Least Constraint [8] deduced a new formulation of the dynamics equations of motion for a system of
constrained particles or rigid bodies. The main features of this formulation are [5]:
- the equations of motion can be reduced to a system of ordinary differential equations (ODE), even when a
redundant set of coordinates is used;
- variations of degrees-of-freedom due to the change of topology or other causes are allowed and do not require
special effort in computer programming;
- rheonomic and scleronomic constraints are treated in the same way;
- forward and inverse dynamics problems can be solved within the same tool;
- easy computer implementation, provided that a subroutine for computing the pseudoinverse matrix is available.
The above features make the formulation very attractive. In fact, one of the shortcomings associated with the use
of a redundant set of coordinates is the integration of a mixed system of differential-algebraic equations (DAE).
These systems are different from ordinary differential equations (ODE) and require specialized numerical
152
methods for their solution. A fundamental step of the Udwadia-Kalaba (UKF) formulation is the computation of
the Moore-Penrose [6], [7], [9], [16]. generalized inverse or pseudoinverse matrix. D. de Falco, E. Pennestri and
L. Vita, [5], Arabyan and Wu [1] investigated numerically the advantages of this formulation.
Main results of the method of Udwadia and Kalaba are briefly reviewed in this section for later use.
Consider first an unconstrained multibody system whose configuration is described by the n generalized
coordinates
q  q1 , q2 ,..., qn T
(5)
The system equation of motion can be obtained, using Newtonian or Lagrangian mechanics, by the relations
M (q, t )q  Q(q, q , t )
(6)
where the n  n matrix M is symmetric and positive definite. The matrix M and the generalized force n-vector
( n  1 matrix) Q( q , q ,t ) are known.
The generalized acceleration of the unconstrained system, which we denote by the n-vector a, is then given by
q  M 1Q  a( q, q ,t ) .
Suppose that the system is subjected to h holonomic constraints of the form
i ( q ,t )  0, i  1,2,.., h,
(7)
And m  h nonholonomic constraints of the form
i ( q , q ,t )  0, i  h  1, h  2,.., m .
(8)
These constraints encompass all the usual holonomic and nonholonomic constraints (or combinations thereof) to
which the multibody system may be subjected. We note that the constraints may be also explicit functions of the
time and that the nonholonomic constraints may be nonlinear in the velocity components. Under the assumption
of sufficient smoothness, we can differentiate equations (7) twice with respect to time and equations (8) once
with respect to time to obtain the consistent equation set
A(q, q, t )q  b(q, q, t )
(9)
where the constrain matrix A is a known m  n matrix and b is a known m- vector. It can be seen that (9)
includes holonomic, nonholonomic, and many other kinds of constraints. The derivation of the Udwadia–Kalaba
equations utilizes the acceleration form of the constraint equations (9).
The presence of the constraints (9) imposes additional constraint forces on the multibody system that alter its
acceleration, so that the explicit equation of motion of the constrained system becomes
M ( q,t )q  Q( q, q ,t )  Qc ( q, q ,t )
(10)
where n -vector Qc ( q , q ,t ) represents the generalized constraint forces. The additional term Qc on the right-hand
side arises by virtue of the imposed constraints prescribed by equations (9).
The main result of Udwadia and Kalaba [14] is the derivation of the explicit equations of motion under
constraints (9) as
q  a  M 1 2 ( AM 1 2 ) ( b  Aa )
(11)
Mq  Q  K( b  AM 1Q )
(12)
or
where the n  m matrix K is
K( q, q ,t )  M 1 2 ( AM 1 2 )
(13)
and ( AM 1 2 ) denotes pseudoinverse of the matrix AM 1 2 or Moore- Penrose generalized inverse.
From the equation (11) we can be that at each instant of time, the motion of constrained dynamical system
  a  M 1 K( b  Aa )  K1e of constrained generalized accelerations, q( t ) ,
evolves so that the deviation a  q
from the unconstrained acceleration, a, at each instant is direct proportional to the error e  b  Aa at that
instant, the matrix of proportional it begin K1  M 1 K  M 1 / 2 ( AM 1 / 2 )  . We shall refer to the matrix K 1
as the weighted Moore-Penrose generalized invers of the weighted constraint matrix A.
153
The equation of motion (12) yields from the unic solutions of the constrained minimization problem, Gauss
principle, [15]
min G  [ q  a ] T M [ q  a ]
q
(14)
s.t .Aq  b
where G is the Gaussian functions.
Now, we can derive the Gauss principle of last constraint from formula (11). In what follows, it is convenient to
use the ‘scaled’ acceleration ,
qs  M 1 / 2 q, a s  M 1 / 2 a  M 1 / 2 Q,
(15)
Then, Eq. (11) becomes
qs  a s  B  e
(16)
But B  e is the solution of the variational problem
min x T x
(17)
x
s .t . B x  e
Thus, we obtain
min qs  a s T qs  a s 
qs

(18)

s. t AM 1 / 2 qs  a s   b  Aa
Rewriting now the original variables, we see that the variational problems becomes (10).
The second main result is the derivation of the force of constraint Qc by the equation
Qc  K( b  AM 1Q )
(19)
Equation (10)-(13) and (19) give the explicit equation of motion of the constrained system in terms of the
quantities M, A, Q, and b.
4. EXTENDED GAUSS PRINCIPLE AND GENERAL EQUATIONS OF MOTIONS
In this section the formulation of general equation of motions is deduced following the approach of UdwadiaKalaba [15]. We wish to determine all the equations of motion that are compatible with the constrain
A( q , q , t )q  b( q , q , t ) with no physical assumptions being made at all. We rewrite the equation of constrain in
the form


AM 1 / 2 M 1 / 2 q  b
(20)
The theory of generalized inverses shows us that the general solution of this set of linear algebraic equations for
q is
q  a  M 1 2 ( AM 1 2 )  ( b  Aa )

 M 1 2  I  AM 1 2

 AM  M

1 2
1 2
(21)
C
where a is the free motion acceleration vector and C is an arbitrary vector, both being of the dimension 3n 1 .
Eq.(14) can be rewritten as equation of motion
Mq  Ma  M 1 2 ( AM 1 2 )  ( b  Aa )

 M 1 2  I  AM 1 2

 AM  M

1 2
1 2
(22).
C
154
Equation (22) is the most general possible equation of motion that is compatible with the constraint relation
Aq  b , of course assuming that the matrix M is nonsingular.
Note that only two essential mathematical ideas have entered the analysis: the chain rule of differentiation and
generalized inverses of matrices.
Modern computing environments, such MATLAB, can be used for calculating the generalized inverse of a
matrix, so it makes the approach highly suitable for numerical study.
The Udwadia-Kabala formulation, (UKF), of the dynamic equation is discussed from the point of view of
numerical efficiency in the papers [ 5], [6].
5. MOTION CONTROL OF NONLINEAR MECHANICAL SYSTEMS - A NEW MODELING
METHOD
We consider in this paper an n- degree-of freedom mechanical system described by the Lagrange’s equations as
M (q, t )q  D(q, q, t )  
(23)
where n—vector  is the external forcing term.
Suppose that the system is under the constraints
g( q, q , t )  0
(24)
where g is an m-vector, g(  ) : R n  R n  R  R m . Is assumed that the functions g i ( q , q , t ) are C1 . The set
(24) may contain relation that is integrable and/or non-integrable.
The external forces,  , are used to control the system. The basic problem is to find a function  in some class so
that solutions to equation (11) have some desired properties, while observing the constraint for all t  t 0 .
Physically one may not always be able to measure the full state of the mechanical system. In this situation the
function  must only depend on the observable variables. For now, we will consider the case when all variables
may be observed. This is referred to as full state feedback control.
The constrain (24) can be expressed in form (9) by differentiating it with respect to time. We obtain
g
g
g
.
q  
q 
q
q
t
(25)
Therefore from equations (9) and (25) we give
A( q , q , t )  
g
g
g
q 
and b( q , q , t )  
.
q
q
t
(26)
In order to use the Udwadia - Kalaba formulation (UKF) we rewritten the constrained mechanical system (23)(24) in the form (6) and (9).
The unconstrained equation (23) can be rewritten as
M ( q , t )q    D( q , q , t ) .
(27)
From (6) and (27) we give
Q( q , q , t )    D( q , q , t ) .
(28)
So, in the Udwadia-Kalaba formulation (UKF), a mechanical system (23) which is to be controlled under the
constraints (24) is represented like equations (26) and (27).
From equations (12), (26) and (27) we obtain
Mq  D  M 1 2 ( AM 1 2 ) ( b  AM 1 D )  ( I  M 1 2 ( AM 1 )  AM 1 ) .
(29)
Mq  D  M 1 2 B  ( b  AM 1 D )  ( I  M 1 2 B  AM 1 )
(30)
or
where B  AM 1 2 .
The equation (30) represents the new explicit dynamics equation of the constrained mechanical system. The
right-hand side is the control with new input matrix.
155
Application of (19) to the system (26)-(27) gives
Q C  M 1 2 ( AM 1 2 )  ( b  AM 1 D )  M 1 2 ( AM 1 2 )  AM 1
(31)
Q C  M 1 2 B  ( b  AM 1 D )  M 1 2 B  AM 1 .
(32)
or
From equation (32) we can control the generalized constraint force.
6. CONCLUSION
Based on the recent results from analytical dynamics, Udwadia-Kalaba approach, this paper present a new
method for motion control of constrained mechanical systems. We present a rederivation of the Gauss principle
and then an extension of his principle to cases in which the standard principle of virtual work is not applicable.
For to do this we used the Udwadia–Kalaba equation and the acceleration form of the constraint equations,
together with the generalized Moore–Penrose inverse of a scaled constraint matrix. By this methodology we can
control mechanical systems with nonholonomic constraints and non-ideal constraints, too. We obtain the explicit
equation of controlled motion and we can see explicitly how the input is affected due to presence of constraints.
The approach requires very little computation compared with standard approaches. So, this control method is
useful for online real-time control of nonlinear systems.
REFERENCES
[1] Ara Arabyan, Fei Wu, An Improved Formulation for Constrained Mechanical Systems, Multibody System
Dynamics, vol.2, 1998, pp.49-69.
[2] Bloch, A. M., Reyhanoglu, M., and McClamroch, N.H., “Control and Stabilization of Nonholonomic
Dynamic Systems,” IEEE Trans. on Automatic Control, Vol. 37, No. 11, pp. 1746-1757, 1992.
[3] Bloch, A. M., and McClamroch, N. M., “Control of Mechanical Systems with Classical Nonholonomic
Constraints,” Proc. of the 28th Conf. on Decision and Control, Tampa, FL, pp. 201-205, 1989.
[4] Campion, G., d′ Andrea-Novel, B., and Bastin, G..,Controllability and State Feedback Stabilizability on
Nonholonomic Mechanical Systems, in Advanced Robot Control, New York: Springer-Verlag, pp. 106- 124,
1991.
[5] De Falco, D., Pennestri, Vita, L., the Udwadia –Kalaba Formulation: A Report on its Numerical Eficiency in
Multibody Dynamics Simulations and on its teaching Effectivness, Multibody Dinamics 2005, ECCOMAS
Thematic Conference, Madrid, Spain, 21-24 Junie 2005.
[6] Itiki, C., Neto, J.J., Complete automation of the generalized inverse method for constrained
mechanical systems of particles, Applied Mathematics and Computation, 2004, pp.561-580.
[7] Moore,E.H.,On the reciprocal of the general algebraic matrix. Abstract, Bull. Am. Math. Soc. 26,394-395,
1920.
[8] C. Gauss, Uber ein neues allgemeines grundgesetz der mechanik, J. Reine Angew. Math. 4, 232-235, 1829.
[9] Penrose, R., “A Generalized Inverse for Matrices,” Proc. of the Cambridge philosophical Society, Vol. 51,
Part 3, pp. 406-413, 1955.
[10] Su, C., Stepanenko, Y., “Robust Motion/Force Control of Mechanical Systems with Classical
Nonholonomic Constraints,” IEEE Trans. On Automatic Control, Vol. 39, No. 3, pp. 609-614, 1994.
[11] Su,C., Leung, T. P., Zhou, Q.J.,: Force/motion control of constrained robot using sliding mode, IEEE Trans.
Automatic control, Vol. 37, pp. 668-672,1992.
[12] Sastry, S., Nonlinear systems analysis, stability, and control. Springer, 1999.
[13] Slotine, J.-J. E., Li, W. Applied nonlinear control. Englewood Cliffs, NJ: Prentice-Hall, 1991.
[14] Udwadia, F. and Kalaba, R. Analytical Dynamics a New Approach. Cambridge University Press,
Cambridge, 1996.
[15] Udwadia, F. E., and Kalaba, R. E., “A New Perspective on Constrained Motion,” Proc. of Royal Society of
London, Series A, Vol. 439, pp. 407-410, 1992.
[16] Udwadia, F.E., Kalaba, R.E., A Unified Approach for the Recursive Determination of Generalized Inverses,
Computers and Mathematics with Applications, vol.37, 1999, pp.125-130.
156