Dynamics Basilio Bona DAUIN-Politecnico di Torino 2009 Basilio - - PowerPoint PPT Presentation

Dynamics

Basilio Bona

DAUIN-Politecnico di Torino

2009

Basilio Bona (DAUIN-Politecnico di Torino) Dynamics 2009 1 / 30

Dynamics - Introduction

In order to determine the dynamics of a manipulator, it is necessary to introduce the force or the inertia concept. The fundamental law of mechanics, established by Newton, is expressed as a vector valued equality f = ma i.e.,   fx fy fz   = m   ax ay az   where a = ˙ v is the acceleration and f is the applied force. This relation is applicable to a point mass (or point particle) m, where the applied force f and the acceleration a acting on the particle may alternatively play the role of cause or effect.

Basilio Bona (DAUIN-Politecnico di Torino) Dynamics 2009 2 / 30

Dynamics - Introduction

Similar to the relation f = ma valid for linear motions, there is another

• ne, valid for rotation motions.

This one, due to Euler, establishes a relation among the torque applied to a rigid body, its angular velocity and acceleration, and the body inertia moment: τ = Γ ˙ ω +ω ×Γω where τ is the applied torque, ω and ˙ ω il momento applicato al corpo, sono la velocit` a e l’accelerazione angolare del corpo e Γ ` e il momento d’inerzia del corpo rispetto al suo baricentro.

Basilio Bona (DAUIN-Politecnico di Torino) Dynamics 2009 3 / 30

Dynamics - Introduction

A system composed by N mass points or else a rigid body is in dynamic equilibrium when the sum of all forces, including inertial forces, is zero. the sum of all angular torques (with respect to the body mass center), including inertial torques, is zero. The first condition allows to write differential linear equations of equilibrium, called Newton equations, while the second condition allows to write differential angular equations of equilibrium, called Euler equations. Equations are vectorial, i.e., they relate vectorial quantities; every vectorial equations implies three scalar equations, one for each component. In n links manipulators, the number of vectorial equations is 2n.

Basilio Bona (DAUIN-Politecnico di Torino) Dynamics 2009 4 / 30

Newton-Euler Equations

Consider the generic i-th robot arm bi, with its center of mass ci, and DH conventions applied.

Figure: Arm i with applied forces and torques.

Basilio Bona (DAUIN-Politecnico di Torino) Dynamics 2009 5 / 30

Newton Equations

If: fi−1,i resultant of the forces applied from arm i −1 to arm i fi+1,i resultant of the forces applied from arm i +1 to arm i gi local gravity field acceleration vector aci total acceleration of the center-of-mass We can write the i-th Newton equation as: fi−1,i +fi+1,i +migi −miaci = 0

Basilio Bona (DAUIN-Politecnico di Torino) Dynamics 2009 6 / 30

Euler Equations

If: Ni−1,i resultant of the torques applied from arm (i −1) to arm i Ni+1,i resultant of the forces applied from arm (i +1) to arm i Γi inertia matrix of arm i with respect to its center-of-mass rci,i−1 position of the (i −1) BRF origin with respect to center-of-mass rci,i position of the (i +1) BRF with respect to center-of-mass BRF Body Reference Frame We can write the i-th Euler equation as Ni−1,i +Ni+1,i +rci,i−1 ×fi−1,i +rci,i ×fi+1,i

• moment of the forces f

−Γi ˙ ωi −ωi ×Γiωi = 0

Basilio Bona (DAUIN-Politecnico di Torino) Dynamics 2009 7 / 30

Newton-Euler Equations

The 2n vectorial Newton-Euler (N-E) equations are difficult to deal with, at least symbolically, since the internal constraints between the arms appear explicitly. These constraints are due to the forces f transmitted by an arm to the other. These constraints have no interest in determining the dynamical behavior and the motion laws of the multi-body structure, and their determination is useless. From this point of view, Lagrange equations are much more immediate and easy to deal with, as we shall see in the following. Nonetheless, from a purely numerical/algorithmic viewpoint N-E equations are easier to solve than Lagrange, due to the several recursive solutions found in literature.

Basilio Bona (DAUIN-Politecnico di Torino) Dynamics 2009 8 / 30

Lagrange Equations

Lagrange equations are scalar differential equations, unlike N-E equations; they are derived from the definition of a Lagrangian function given by the difference of the total kinetic co-energy and the total potential energy of the entire manipulator. Kinetic Co-energy The total kinetic co-energy C is a non-negative scalar function of the joint coordinates q(t) and velocities ˙ q(t); it is an additive function, i.e., C (q(t), ˙ q(t)) =

n

∑

i=1

Ci(q(t), ˙ q(t)) where Ci is the kinetic co-energy of arm i.

Basilio Bona (DAUIN-Politecnico di Torino) Dynamics 2009 9 / 30

Lagrange Equations

Potential Energy In mechanical systems the total potential energy P is a function of the joint coordinates q(t) alone; it is additive, i.e., P(q(t)) =

n

∑

i=1

Pi(q(t)) where Pi is the i-th arm potential energy.

Basilio Bona (DAUIN-Politecnico di Torino) Dynamics 2009 10 / 30

Lagrange Equations

Lagrangian function The Lagrangian (or Lagrangean) function is a state-function defined as L (q(t), ˙ q(t)) = C −P =

n

∑

i=1

Ci(q(t), ˙ q(t))−

n

∑

i=1

Pi(q(t)) “State-function” means that the value of the function depends only on the state of the system at time (t). Once the lagrangian is known, the system dynamics is described by n scalar differential equations: d dt ∂L ∂ ˙ qi

• − ∂L

∂qi = Fi i = 1,...,n where Fi is the i-th generalized scalar force.

Basilio Bona (DAUIN-Politecnico di Torino) Dynamics 2009 11 / 30

Lagrange Equations

From a dimensional point of view, Lagrange equations are differential equations whose terms are scalar forces or torques. Approximately we can say that these forces/torques are the sum of all the effects of the virtual work associated with the i-th coordinate qi(t). If among these forces there are also dissipative forces associated to a dissipative function D, where D(˙ q) =

n

∑

i=1

Di(˙ q) = 1 2

n

∑

i=1

βi ˙ q2

i

Lagrange equation can be written as d dt ∂L ∂ ˙ qi

• − ∂L

∂qi + ∂D ∂ ˙ qi = Fi i = 1,...,n

Basilio Bona (DAUIN-Politecnico di Torino) Dynamics 2009 12 / 30

Kinetic Energy

Since the energy is an additive scalar quantity, it is convenient to provide

• nly the value of the i-th arm kinetic energy:

Ci(q, ˙ q) = 1 2 vT

ci(q, ˙

q)mi vci(q, ˙ q)+ 1 2 ωT

i (q, ˙

q)Γi ωi(q, ˙ q) where vci is the “total” linear velocity of the i-th body center-of-mass (total means that is the velocity of the body with respect to an inertial RF), where ωi is the total angular velocity of the i-th body and Γi the inertia matrix with respect to the center-of-mass. Hence kinetic energy of a roto-translating arm is the sum of two terms: the kinetic energy due to translation plus the kinetic energy due to

• rotation. The former depends on the square value of the center-of-mass

velocity, while the latter depends on the square value of the angular velocity of the arm.

Basilio Bona (DAUIN-Politecnico di Torino) Dynamics 2009 13 / 30

Kinetic Energy

To express the kinetic energy (KE) directly in function of the generalized coordinates q and velocities ˙ q it is necessary to know the Jacobian matrices relative to vci and ωi. If we define J(i)

L =

• J(i)

L1

J(i)

L2

··· J(i)

Li

···

• J(i)

A =

• J(i)

A1

J(i)

A2

··· J(i)

Ai

···

• we have

vci = J(i)

L1 ˙

q1 +J(i)

L2 ˙

q2 +···+J(i)

Li ˙

qi = J(i)

L ˙

q ωi = J(i)

A1 ˙

q1 +J(i)

A2 ˙

q2 +···+J(i)

Ai ˙

qi = J(i)

A ˙

q

• ⇔ ˙

pci = Jci ˙ q

Basilio Bona (DAUIN-Politecnico di Torino) Dynamics 2009 14 / 30

Kinetic Energy

We can therefore write the KE as 1 2 ∑

i

˙ qT J(i)

L (q)T mi J(i) L (q)

• ˙

q+ ˙ qT J(i)

A (q)T Γi J(i) A (q)

• ˙

q

• r else

1 2 ˙ qT    ∑

i

• J(i)

L (q)T mi J(i) L (q)

• +
• J(i)

A (q)T Γi J(i) A (q)

• Hi(q)

    ˙ q and C = 1 2 ˙ qT∑

i

(Hi(q)) ˙ q = 1 2 ˙ qTH(q)˙ q

Basilio Bona (DAUIN-Politecnico di Torino) Dynamics 2009 15 / 30

Kinetic Energy

Calling Hij(q) the generic term ij of matrix H(q), we observe that Hij(q) =

n

∑

k=1

{Hij(q)}k where {Hij(q)}k is the ij-th element of the inertia matrix of the k-th arm. Therefore, another formulation of the kinetic co-energy is the following: C = 1 2

n

∑

i=1 n

∑

j=1

Hij(q)˙ qi ˙ qj

Basilio Bona (DAUIN-Politecnico di Torino) Dynamics 2009 16 / 30

Potential Energy

Usually potential energy is stored in elastic elements; if the manipulator is composed of purely rigid arms, there are no elements capable to store potential energy. Nonetheless, we can also consider another form of kinetic energy, i.e., the “position energy”, due to the presence of a gravitational force field, that, locally, has field lines parallel to the local acceleration vector G.

Figure: Potential energy has equi-potential surfaces orthogonal to the local gravity vector G.

Basilio Bona (DAUIN-Politecnico di Torino) Dynamics 2009 17 / 30

Potential Energy

Two alternative approaches are available to include the potential energy into Lagrange equations:

1

forces and torques due to the gravitational field are considered as external generalized forces Fi acting on the arms;

2

position energy, due to gravitational effects, is considered a potential energy term included into the total potential energy. In the following, the second approach will be developed; in any case it produces the same results as the first one.

Basilio Bona (DAUIN-Politecnico di Torino) Dynamics 2009 18 / 30

Potential Energy

In a gravitational field, the potential energy is expressed as: Pi(q) = −mi GT r0,ci(q) where r0,ci(q) is the vector that rep- resents the position of the center-of- mass of the i-th arm in RF R0. Usually we assume that potential en- ergy is zero when the center-of-mass is at the same level of the origin of R0; is positive if the center-of-mass is “above” the origin of R0, is nega- tive if the center-of-mass is “below” the origin of R0.

Figure: Potential energy is positive if GTr0,ci < 0.

Basilio Bona (DAUIN-Politecnico di Torino) Dynamics 2009 19 / 30

Generalized Forces

To compute the generalized forces, we neglect the effect of the weight forces, whose causes (gravity field) has been already included in the potential energy. Apart from the possible friction forces, that are expressed including the dissipation energy terms Di, all the other generalized forces are commonly due to the action of actuators moving the joints, and to the generalized interaction forces exchanged with the environment. In general, the generalized forces are the sum of all the effects associated with the virtual work of the i-th coordinate qi(t). In prismatic joints the total virtual work is the sum of virtual works done by all linear forces, while in rotational joints it is the sum of virtual works done by all angular torques. δW =

Nf

∑

k=1

fk ·δrk

• lineari

+

Nτ

∑

k=1

τk ·δαk

• angolari

= 0

Basilio Bona (DAUIN-Politecnico di Torino) Dynamics 2009 20 / 30

Generalized Forces and Virtual Works

The previous equation can also be expressed as: δW =

Nf

∑

k=1

fk ·δrk +

Nτ

∑

k=1

τk ·δαk =

Nf

∑

k=1

fk ·

• n

∑

i=1

∂rk ∂qi δqi

• +

Nτ

∑

k=1

τk ·

• n

∑

i=1

∂τk ∂qi δαi

• =

n

∑

i=1

• Nf

∑

k=1

fk · ∂rk ∂qi

• linear g.f. Fℓi

δqi +

n

∑

i=1

• Nτ

∑

k=1

τk · ∂αk ∂qi

• angular g.f. Fαi

δαi =

n

∑

i=1

Fℓiδqi +

n

∑

i=1

Fαiδαi by inspection of the equation terms it is possible to recover the exact expression of the i-th components of the generalized forces.

Basilio Bona (DAUIN-Politecnico di Torino) Dynamics 2009 21 / 30

Generalized Forces

More clearly, assuming that the joint actuators apply active command torques τc = τc1 ··· τcn T and that, at the manipulator TCP an external generalized force Fe is applied by or to the environment. Then the i-th component of the generalized total forces is Fi = τci +τei where we have called τei the i-th component of JTFe of the environment interaction torques. From now on we will use the symbol τi instead of Fi

Basilio Bona (DAUIN-Politecnico di Torino) Dynamics 2009 22 / 30

Final Equations

For simplicity, the friction forces are neglected and we consider only the terms d dt ∂L ∂ ˙ qi

• − ∂L

∂q = τi After the differentiation we have

∑

j

Hij(q)¨ qj +∑

j

• ∑

k

hijk(q)˙ qk

• cij(q, ˙

q)

˙ qj +gi(q) = τi where the hijk terms are called Christoffel symbols of the first type. hijk(q) = 1 2 ∂Hij(q) ∂qk + ∂Hik(q) ∂qj − ∂Hjk(q) ∂qi

• = hikj(q)

∀k

Basilio Bona (DAUIN-Politecnico di Torino) Dynamics 2009 23 / 30

Final Equations

Setting cij (q, ˙ q) = ∑

k

hijk(q) ˙ qk, we obtain:

∑

j

Hij(q)¨ qj +∑

j

cij(q, ˙ q)˙ qj +gi(q) = τi The n arm equations may be collected in a single matrix equation H(q)¨ q(t)+C(q, ˙ q) ˙ q(t)+g(q) = τ where τ = τc +τe, is the sum of the command torques provided by the actuators τc and of the possible external interaction torques τe. The elements of C(q, ˙ q) matrix are the terms cij (q, ˙ q) indicated above.

Basilio Bona (DAUIN-Politecnico di Torino) Dynamics 2009 24 / 30

Friction terms

If viscous dissipative phenomena (proportional to velocities) are present, these can be included in the left hand side of the equation, as: H(q)¨ q(t)+C(q, ˙ q)˙ q(t)+B˙ q+g(q) = τ where B is usually a diagonal matrix with constant elements, that are the constant viscous friction coefficients βi: B = diag(βi)

Basilio Bona (DAUIN-Politecnico di Torino) Dynamics 2009 25 / 30

Properties of the Lagrange Equation

Consider again the equation (without dissipative terms for brevity) H(q)¨ q(t)

• n terms

+C(q, ˙ q)˙ q(t)

• n terms

+ g(q)

• n terms

= τ

• n terms

it is always possible to define a set of np parameters, some geometric, some inertial ones, but all depending on the physical structure of the manipulator, that can be collected in a np parameter vector θ =   θ1 . . . θnp  

T

The robot dynamics is linear in the parameters, i.e., H(q)¨ q(t)+C(q, ˙ q)˙ q(t)+g(q) = Φ(¨ q(t), ˙ q(t),q(t))

• n×np structural matrix

θ

Basilio Bona (DAUIN-Politecnico di Torino) Dynamics 2009 26 / 30

Properties of the Lagrange Equation

Therefore, we can write Φ(¨ q(t), ˙ q(t),q(t)) θ = τ(t) where the Φ(¨ q, ˙ q,q) ∈ Rn×np matrix is called regression matrix or else regressor This linear equation allows to directly relate the generalized torques τ with the parameters θ; we can use this relation to obtain the numerical values

• f the parameters from the torques measurements.

Basilio Bona (DAUIN-Politecnico di Torino) Dynamics 2009 27 / 30

Properties of the Lagrange Equation

Let us we define a new matrix N(q, ˙ q) as N(q, ˙ q) = ˙ H(q)−2C(q, ˙ q) this matrix is “structurally” (i.e., always) antisymmetrical. As we have seen, the properties of antisymmetrical matrices allows to state that the quadratic form associated with N(q, ˙ q) is always zero: ˙ qT(t)N(q, ˙ q) ˙ q(t) ≡ 0, ∀ ˙ q(t) This property is useful in the when we are looking for Lyapunov functions in the study of the stability of nonlinear control algorithms. A last property of H(q) matrix is that, under very general conditions is positive definite, therefore invertible, for each value of q.

Basilio Bona (DAUIN-Politecnico di Torino) Dynamics 2009 28 / 30

State Equations

Starting from the manipulator matrix equation: H(q)¨ q(t)+C(q, ˙ q)˙ q(t)+g(q) = τ we can obtain the state equations of this system choosing as state vector x(t) the following one: x(t) ≡

• x1(t)

x2(t)

• =
• q(t)

˙ q(t)

• This choice produce the first order nonlinear differential state equations:

˙ x1(t) = x2(t) ˙ x2(t) = −H−1(x1){C(x1,x2)x2(t)+g(x1)}+H−1(x1)τ(t)

Basilio Bona (DAUIN-Politecnico di Torino) Dynamics 2009 29 / 30

State Equations

Sometimes it is necessary or useful to approximately linearize the state equations, in order to use classical linear techniques for control design. The inertia matrix can be approximated as H(q) ≈ ˆ H ˆ H =   H11 ... Hnn   i.e., diagonal and independent on q. When joint velocities are low, we can also set C ≈ 0, and the equation reduces to: H¨ q(t) = τ −g(q) which represents n decoupled double integrators, driven by external torques, including gravitational ones.

Basilio Bona (DAUIN-Politecnico di Torino) Dynamics 2009 30 / 30