robot modeling and control - ulisboa · modeling dynamics control architectural concerns practical...

ModelingDynamics

ControlArchitecturalConcerns

Practical ControlStrategies

Finalremarks

Robot Modeling and ControlPhD Course on Advanced Robotics

João Silva Sequeira1

[email protected]

Fall 2010

ModelingDynamics



Finalremarks

References I

Patrick MuirModeling and Control of Wheeled Mobile RobotsPhD Thesis, 1988

A.M. BlochNonholonomic Mechanics and ControlSpringer, 2003.

V.I. ArnoldMathematical Methods of Classical MechanicsSpringer, 1989

Marcio Queiroz, Michael Malisoff, Peter Wolenski (Eds)Optimal Control, Stabilization and Nonsmooth AnalysisSpringer, 2004

Georgi SmirnovIntroduction to the Theory of Differential InclusionsAmerican Mathematical Society, 2002

Alexey Pavlov, Nathan v.d. Wouw, Henk NijmeijerUniform Output Regulation of Nonlinear SystemsBirkhäuser, 2006

ModelingDynamics



Finalremarks

References II

Hassan KhalilNonlinear SystemsPrentice Hall

M.R. FlanneryThe enigma of nonholonomic constraintsAm. J. Physics, 73(3), March 2005

Harris McClamroch, Danwei WangFeedback Stabilization and Tracking of Constrained RobotsIEEE Trans, on Automatic Control, 33(5), May 1988

Jean-Claude LatombeRobot Motion PlanningKluwer Academic, 1992

Robert Herman, Arthur KrenerNonlinear Controllability and ObservabilityIEEE Trans. Automatic Control, AC-22(5):728-740, 1977

Z. Li, J. Canny (eds)Nonholonomic Motion PlanningKluwer, 1993

ModelingDynamics



Finalremarks

References III

A. Di Carlo, A. PaoluzziFast computation of inertia through affinely extended Euler tensorJournal of Computer-Aided Design, 38:1145-1153, 2006

John CraigIntroduction to RoboticsAddison-Wesley, 1989

Lorenzo Sciavicco, Bruno SicilianoModeling and Control of Robot ManipulatorsMcGraw-Hill, 1996

Jean-Pierre MerletParallel RobotsKluwer Academics Publishers, 2000

Philippe Sardain, Guy BessonnetForces Acting on a Biped Robot. Center of Pressure - Zero Moment PointIEEE Trans. on Systems, Man, and Cybernetics-Part A, 34(5):630-637,September 2004

A. GoswamiPostural stability of biped robots and the foot rotation indicator pointInt. Journal of Robotics Research 18(6):523-533, 1999

ModelingDynamics



Finalremarks

References IV

H. Olsson, K.J. Aström, C. Canudas de Wit, M. Gäfvert, P. LischinskyFriction Models and Friction CompensationAvailable on web at ...

Frank L. Lewis, D.M. Dawson, Chaudki T. AbdallahRobot Manipulator Control: Theory and PracticeCRC Press, 2nd edition, 2003

R. RajagopalanA Generic Kinematic Formulation for Wheeled Mobile RobotsJournal of Robotic Systems 14(2):77-91, 1997

Peter J. OlverApplications of Lie Groups to Differential EquationsSpringer-Verlag, 1993

M. PostnikovLie Groups and Lie AlgebrasNauka, 1994

Ignacy DulebaOn the use of Campbell-Baker-Hausdorff-Dynkin formulas in nonholonomicmotion planningProcs. of the SYROCO’99, 1999

ModelingDynamics



Finalremarks

References V

Wulf RossmannLie GroupsOxford Mathematics, 2006

Ole Jakob SordalenFeedback Control of Nonholonomic Mobile RobotsPhD thesis, NTH, Norway, 1993

Bruno Siciliano, Oussama Khatib (eds)Handbook of RoboticsSpringer, 2008

Guy Campion, Georges Bastin, Brigitte D’Andrea-NovelStructural Properties and Classification of Kinematic and Dynamics Modelsof Wheeled Mobile RobotsIEEE Trans. on Robotics and Automation, 12(1):47-62, Feb 1996

J. Denavit, R.S. HartenbergA Kinematic Notation for Lower Pair Mechanisms Based on MatricesJournal of Applied Mechanics 77(2):215-221, June 1955

P. N. Sheth, J.J. Uicker Jr.A Generalized Symbolic Notation for MechanismsJournal of Engineering for Industry, Series B, 93(70):102-112, 1971

ModelingDynamics



Finalremarks

References VI

M. Barbosa, N. Ramos, P. LimaMermaid - Multiple-Robot Middleware for Intelligent Decision-MakingProcs. of the 6th IFAC Symp. on Intelligent Autonomous Vehicles, France,2007 Toulouse, France.

Dong Hun Shin, Kyung Hoon ParkVelocity Kinematic Modeling for Wheeled Mobile RobotsProcs. of the Int. Conf. on Robotics and Automation, Korea, 2001

ModelingDynamics



Finalremarks

Introduction I

I Study how forces and torques influence the motion of a robot

I Take into account the geometry of the robot, mass, and inertia properties

I Classical Mechanics apply (of course)

I Two kinds of problems can be formulated (as usual)

I Given a goal/reference trajectory, what are the forces/torques that

need to be applied by each actuator such that the robot

executes/follows the trajectory ?

I What is the motion that corresponds to a given set of forces/toques

applied by the actuators ?

ModelingDynamics



Finalremarks

Introduction II

I (Not surprisingly) robot dynamics is becoming increasingly important in ...

computer game design

I Some authors argue that kinematic models for land mobile robots operating

at velocities below 10 km/h and not undergoing frequent

accelerations/decelerations, [Rajagopalan,1997]

ModelingDynamics



Finalremarks

Forces

I Represent the capability of having an unconstrained body moving in straight

line

I A force is a vector quantity in which each coordinate generates linear motion

along an axis of the corresponding reference frame

I Newton’s law

F = m a

The key physical quantity here is the mass m

ModelingDynamics



Finalremarks

Moments / Torques

I Represent the ability to generate rotation motion

τ = d× f

Right hand rule

I A torque is vector quantity; each coordinate represents the ability to

generate rotation motion around an axis of the corresponding reference

frame

I Euler’s law

τ = ω + ω × (Iω)

The key physical quantity here is the inertia tensor, I, that expresses the

mass distribution along the body of the robot

ModelingDynamics



Finalremarks

Generic Taxonomy for Robot DynamicsBodies

Rigid

Flexible

-

-

Free

Constrained

-

Open

Closed

-

In this course it is assumed that no flexible bodies are used

ModelingDynamics



Finalremarks

The Least Action Principle / Hamilton’sPrinciple

I Dynamics of mechanical systems follow the LAP / Hamilton’s principle

The action

S =

∫ t2

t1L(q, q, t)dt

is to be minimized over all paths with fixed time, that is δS = 0

L(q, q, t) = K (q, q, t)− V (q, t) is called the Lagrangian and expresses an

energy balance

I Some authors point to slight differences between the Least Action and

Hamilton’s principles; see for instance [Bloch, 2003], pp 8-9

ModelingDynamics



Finalremarks

Lagrangian Formulation

I Dynamics of mechanical systems follow the LAP / Hamilton’s principle

I The variations δS yield (avoiding the dependence on t)

δS =

∫ t2

t1

(∂L∂qδq +

∂L∂qδq)

dt

Integrating by parts,

ddt∂L∂q−∂L∂q

= 0

if it is assumed that no external forces act on the system

I If there are external forces F

ddt∂L∂q−∂L∂q

= F

ModelingDynamics



Finalremarks

Newton-Euler formulation

Newton’s law F = m a

Euler’s law N = CM I ω + ω × CM I ω

where

F Force acting on the CM

m Mass

a Linear acceleration

N Torque acting on the CM

CM I Inertia tensor described in a reference frame with origin at the CM

ω Angular velocity (described in a frame with orientation identical orienta-tion of that of inertia )

ModelingDynamics



Finalremarks

Mass distribution - Inertia tensor

The inertia tensor in frameFA is a matrix

AI =

Ixx −Ixy −Ixz

−Ixy Iyy −Iyz

−Ixz −Iyz Izz

where ρ is the density of thematerial forming the body

Ixx =∫

V (y2 + z2)ρdV

Iyy =∫

V (x2 + z2)ρdV

Izz =∫

V (x2 + y2)ρdV

Inertia moments

Ixy =∫

V xyρdV

Ixz =∫

V xzρdV

Iyz =∫

V yzρdV

Inertia products

I The point mass assumption is often used in robotics, meaning that all themass is concentrated at the CM

I Under the point mass assumption, the inertia matrix written in a frame withorigin at the CM is a null matrix

ModelingDynamics



Finalremarks

Mass distribution - Inertia tensor

AI depends of the referenceframe where it is computed

In the frame of the Center of Mass (CM),

AIzz =∫ h/2−h/2

∫ l/2−l/2

∫ w/2−w/2 ρ(x2 + y2)dxdydz = ρwlh

12 (w2 + l2)

AIyy =∫ h/2−h/2

∫ l/2−l/2

∫ w/2−w/2 ρ(x2 + z2)dxdydz = ρwlh

12 (w2 + h2)

AIxx =∫ h/2−h/2

∫ l/2−l/2

∫ w/2−w/2 ρ(y2 + z2)dxdydz = ρwlh

12 (h2 + l2)

AIxy =∫ h/2−h/2

∫ l/2−l/2

∫ w/2−w/2 ρxydxdydz = 0

AIxz =∫ h/2−h/2

∫ l/2−l/2

∫ w/2−w/2 ρxzdxdydz = 0

AIyz =∫ h/2−h/2

∫ l/2−l/2

∫ w/2−w/2 ρyzdxdydz = 0

ModelingDynamics



Finalremarks

Mass Distribution - Inertia Tensor

I Difficult to compute exactly for arbitrary shapes from the base definitionbecause of their likely complex description

I Computer-Aided programs use special techniques (see for instance[Di Carlo,Paoluzzi,2006]

I Bodies are approximated by polyhedra and special algorithms forvolume integration are used

I Often inertia computation can be simplified without leading to major errors inthe performance of the models

I Point mass assumption is commonly used in small rigid bodies with anapproximate uniform distribution of mass

I Mass is assumed to be totally concentrated at the center of mass(CM) and hence

ρ(x , y , z) = mδ(x , y , z)

I The inertia tensor is described in the a reference frame with origin atthe CM hence

Iij = Iij =∫

V ijmδ(i, j, k)dV = 0, i 6= j

Iii = Iii =∫

V (j2 + k2)mδ(i, j, k)dV = 0

ModelingDynamics



Finalremarks

Lagrangian Formulation for UnconstrainedBodies I

L(θ, θ)︸︷︷︸ = K (θ, θ)︸︷︷︸ − U(θ)︸︷︷︸Free Lagrangian Kinetic energy Potential energy

I The dynamics equation is

ddt

(∂L∂θ

)−∂L∂θ

= τ orddt

(∂K∂θ

)−∂K∂θ

+∂U∂θ

= τ

I τ stands for the generalized torques/forces due to the actuators, dissipative

effects, disturbances, etc

I Generalized forces imposed by other constraints may be included additively

ModelingDynamics



Finalremarks

Lagrangian Formulation for UnconstrainedBodies II

I In case of robots composed by n rigid bodies

I K stands for the sum of the kinetic energy of all the rigid bodies

K =∑n

i=1 Ki with Ki = 12 mi vT

civci + 1

2iωT

iCi Ii iωi

I U stands for the sum of the potential energy of all the bodies and Uref

is a constant that sets the minimal potential energy

U =∑n

i=1 Ui with Ui = −mi0gT 0Pci + Uref

I Positional (holonomic) constraints between the rigid bodies forming

the robot amount to coordinate transformations and hence do not

influence the Lagrangian

ModelingDynamics



Finalremarks

NE formulation for articulated bodies

I Serial kinematic chains are suitable to be described by iterative algorithms

for dynamics modeling

I Propagate the velocities and accelerations from the base (static point) of the

chain towards the other extremity (the end-effector)

I Propagate the forces and torques from the end-effector towards the base

(see [Craig,1989] for details)

ModelingDynamics



Finalremarks

Iterative NE - Rotation Joints

i+1ωi+1 = i+1i R iωi + θi+1

i+1Zi+1

i+1ωi+1 = i+1i R i ωi + i+1

i R iωi × θi+1i+1Zi+1 + θi+1

i+1Zi+1

i+1vi+1 = i+1i R

[i ωi × i Pi+1 +i ωi ×(iωi × i Pi+1

)+i vi

]i+1vci+1 =i+1 ωi+1 × i+1Pci+1 +i+1 ωi+1 ×

(i+1ωi+1 × i+1Pci+1

)+i+1 vi+1

i+1Fi+1 = mi+1i+1vci+1

i+1Ni+1 =ci+1 Ii+1i+1ωi+1 +i+1 ωi+1 ×ci+1 Ii+1

i+1ωi+1

I Outward iterations, i = 0 −→ n − 1

I i Zi is the joint axis in link i

I Fci+1 has the same orientation as Fi+1

ModelingDynamics



Finalremarks

Iterative NE - Rotation Joints

i fi = ii+1R i+1fi+1 +i Fi

iηi =i Ni + ii+1R i+1ηi+1 + i Pci ×i Fi + i Pi+1 × i

i+1R i+1fi+1

τi =i ηTi

i Zi

I Inward iterations, i = n −→ 1

I In free space (end-effector not constrained),

n+1fn+1 = 0

n+1ηn+1 = 0

ModelingDynamics



Finalremarks

Iterative NE - Linear Joints

i+1ωi+1 = i+1i R iωi + θi+1

i+1Zi+1

i+1ωi+1 = i+1i R i ωi + i+1

i R iωi × θi+1i+1Zi+1 + θi+1

i+1Zi+1

i+1vi+1 = i+1i R

[i ωi × i Pi+1 +i ωi ×(iωi × i Pi+1

)+i vi

]+ 2 i+1ωi+1 × di+1

i+1Zi+1 + di+1i+1Zi+1

i+1vci+1 =i+1 ωi+1 × i+1Pci+1 +i+1 ωi+1 ×(i+1ωi+1 × i+1Pci+1

)+i+1 vi+1

i+1Fi+1 = mi+1i+1vci+1

i+1Ni+1 =ci+1 Ii+1i+1ωi+1 +i+1 ωi+1 ×ci+1 Ii+1

i+1ωi+1

I Outward iterations, i = 0 −→ n − 1

I i Zi is the joint axis in link i

I Fci+1 has the same orientation as Fi+1

ModelingDynamics



Finalremarks

Iterative NE - Linear Joints

i fi = ii+1R i+1fi+1 +i Fi

iηi =i Ni + ii+1R i+1ηi+1 + i Pci ×i Fi + i Pi+1 × i

i+1R i+1fi+1

τi =i f Ti

i Zi

I Inward iterations, i = n −→ 1

I In free space (end-effector not constrained),

n+1fn+1 = 0

n+1ηn+1 = 0

ModelingDynamics



Finalremarks

Example: 2-Link Serial Manipulator

Manipulator RR planar(horizontal plane)

i αi−1 ai−1 di θi

1 0 0 0 θ1

2 0 a1 0 θ2

3 0 a2 0 0

I F3 only defines thegripper

I The CM of each link islocated at the extremityof the link

I The mass of each link isa point (located at theCM)

03T =

c1 −s1 0 0s1 c1 0 00 0 1 00 0 0 1

c2 −s2 0 a1

s2 c2 0 00 0 1 00 0 0 1

1 0 0 a2

0 1 0 00 0 1 00 0 0 1

=

c12 −s12 0 c12a2 + c1a1s12 c12 0 s12a2 + s1a10 0 1 00 0 0 1

L =2∑

i=1

( 1

2m1

0vTci

0vci +1

20ω

Ti I0ci

ωi

)=

2∑i=1

( 1

2m1

0vTci

0vci

)

I Potential energy is constant and hence it does not affects the dynamic model

I Ici = 0

ModelingDynamics



Finalremarks


I Assuming that CM2 is at 2Pc2 = [a2, 0, 0]′ then 0vc2 ≡ 0v3

I From the direct kinematics the origin of F3 is

0vc2 =[

(−s12a2 − s1a1) θ1 − s12a2θ2, (c12a2 + c1a1) θ1 + c12a2θ2, 0]′

I Similarly, if CM1 is at 1Pc1 = [a1, 0, 0]′ then

0vc1 ≡ 0v10vc1 =

[−s1a1θ1, c1a1θ1, 0

]′I Yields for the kinetic energies

K1 = 12 m1a2

1θ21

K2 = 12 m2

(a2

2θ22 +

(a2

2 + a21 + 2c2a1a2

)θ2

1 + 2(a2

2 + c2a1a2)θ1θ2

)L = K1 + K2

ModelingDynamics



Finalremarks


The terms in the dynamics equation are

∂L∂θ

=

0

−2s2a1a2θ21 − 2s2a1a2θ1θ2

∂L∂θ

=

m1θ1a21 + m2

(a2

2 + a21 + 2c2a1a2

)θ1 + m2

(a2

2 + c2a1a2)θ2

m2a22θ2 + m2

(a2

2 + c2a1a2)θ1

ddt

(∂L∂θ

)=

m1a2

1θ1 − 2m1s2a1a2θ1θ2 + m2(a2

2 + a21 + 2c2a1a2

)θ1−

−m2s2a1a2θ22 + m2

(a2

2 + c2a1a2)θ2

m2a22θ2 −m2s2a1a2θ1θ2 + m2

(a2

2 + c2a1a2)θ1

ModelingDynamics



Finalremarks


τ1 =m2a22

(θ1 + θ2

)+ m2a1a2c2

(2θ1 + θ2

)+ (m1 + m2) a2

1θ1

−m2a1a2s2θ22 − 2m2a1a2s2θ1θ2

τ2 =m2a1a2c2θ1 + m2a22

(θ1 + θ2

)−m2a1a2s2θ1θ2

in vector form[τ1

τ2

]=

[m2a2

2 + 2m2a1a2c2 + (m1 + m2) a21 m2a2

2 + 2m2a1a2c2

m2a22 + 2m2a1a2c2 m2a2

2

][θ1

θ2

]

−[

m2a1a2s2θ22 + 2m2a1a2s2θ1θ2

m2a1a2s2θ1θ2

]

I What if the end-effector is constrained to be in contact with a surface ?

I How to include such constraints in the Lagrangian (or Newton-Euler)model ?

ModelingDynamics



Finalremarks

Algebraic Complexity of a n-dof CommonRobot

I The state vector is of dimension n

ddt

(∂K (θ, θ)

∂θ

)−∂U∂θ

=

[∂K∂θi

+∂2K∂ωi∂θj

ωi

]+

[∂2K∂ωi∂ωj

ωi

]−∂U∂θ

I M(θ) is n × n and is obtained from

M =

[∂2K∂ωi∂ωj

]

I V (θ, θ) and G(θ) are n × 1 and are obtained from

V =

∂K∂θ1

+ ∂2K∂ω1∂θ1

ω1 + . . .+ ∂2K∂ω1∂θn

ωn

...

∂K∂θn

+ ∂2K∂ωn∂θ1

ω1 + . . .+ ∂2K∂ωn∂θn

ωn

G = −

∂U∂θ1

...

∂U∂θn

ModelingDynamics



Finalremarks

Algebraic Complexity of a n-dof GenericRobot

I Complexity measured by the number of operations needed to obtain all the

structural terms required to represent a robot

I n2 + n2 = 2n2 second derivatives

I n + n = 2n first derivatives

I Total of 2n(n + 1) derivatives

Robot dof

Common industrial manipulator 5

Common biped 6

Quadruped 4*2

I The algebraic complexity of each of the derivatives (involving terms in θ2i ,

θiθj , θi θj , and θi θj is upper bounded by

O(n + n2 + n2)

I Conclusion: Total complexity in the order of O(n2) ... which is a lot !

ModelingDynamics



Finalremarks

Unicycle Dynamics: (Highly) SimplifiedLagrangian Formulation

I Unicycle robot with circular body

I Mass is uniformly distributed

I The free Lagrangian is computed as ifthere were no constraints

L =12

m [v , 0, 0]

[v00

]+

12

[0, 0, ω] Ic

[00ω

]=

12

mv2 +12

Iczzω2 =

12

mv2 +112

H3D3

144πω2

∂L∂θ≡[

00

],

∂L∂θ≡[

mvIczzω

],

ddt

(∂L∂θ

)≡[

mvIczz ω

]and then

ddt

(∂L∂θ

)= τ −→

[FN

]=

[mv

Iczz ω

]

ModelingDynamics



Finalremarks

Unicycle Dynamics: (Highly) SimplifiedLagrangian Formulation

I The free Lagrangian can also be written as

L =12

m(

hωd + ωe

2

)2+

12

Iczz

(hωd − ωe

2d

)2

∂L∂θ≡[

00

],

∂L∂θ≡[

h2

2 m (ωd + ωe) + h2

2d2 Iczz (ωd − ωe)h2

2 m (ωd + ωe)− h2

2d2 Iczz (ωd − ωe)

]

ddt

(∂L∂θ

)≡[

h2

2 m (ωd + ωe) + h2

2d2 Iczz (ωd − ωe)h2

2 m (ωd + ωe)− h2

2d2 Iczz (ωd − ωe)

]I The motor torques are thus given by

Nright = h2

2

(m +

Iczzd2

)ωd + h2

2

(m − Iczz

d2

)ωe

Nleft = h2

2

(m − Iczz

d2

)ωd + h2

2

(m +

Iczzd2

)ωe

I ... but the robot would have to be ... flying ...

ModelingDynamics



Finalremarks

General Form of the UnconstrainedDynamics Equation I

I The dynamics of compositions of rigid bodies follows the general description

M(θ)θ + N(θ, θ) = τ

M(θ)θ + V (θ, θ) + G(θ) = τ

M(θ)θ + C(θ, θ) + N(θ) + G(θ) = τ

I M(θ) is symmetric and positive definite

ModelingDynamics



Finalremarks

General Form of the UnconstrainedDynamics Equation II

I The following are useful expressions when manipulating the dynamics

equation

0 < λmin(M(θ)) < ‖M(θ)‖ ≤ λmax(M(θ))

∀x ∈ Rn, xT(

˙M(θ)− 2C(θ, θ))

x = 0

∀θ, x , ν ∈ Rn, C(θ, x)ν = C(θ, ν)x

∀θ, θ ∈ Rn, 0 < Cm‖θ‖2 ≤ ‖C(θ, θ)‖ ≤ CM‖θ‖2

∀θ ∈ Rr , ‖G(θ)‖ ≤ κg

ModelingDynamics



Finalremarks

General Form of the UnconstrainedDynamics Equation III

I The equivalence theorem between norms in Rn may be useful when

manipulating the above properties

Let ‖ · ‖a and ‖ · ‖b be any two norms in Rn. Then there exists constants

γ1, γ2 > 0 such that

∀v ∈ Rn, γ1‖v‖a ≤ ‖v‖b ≤ γ2‖v‖a

Example: For v ∈ Rn, ‖v‖∞ ≤ ‖v‖2 ≤√

n‖v‖∞

Note: This is not be valid in general metric spaces

ModelingDynamics



Finalremarks

Constrained Dynamics I

I Holonomic constraints, g(θ) = 0, can be included in the free Lagrangian

simply as

L?(θ, θ) = L(θ, θ) + λg(θ)

I Holonomic constraints arise often in closed kinematic chains subject to

positional constraints

I Generic nonholonomic constraints, g(θ, θ) = 0, can not be included in the

same way (see [Flannery,2005])

When using variational calculus in Hamilton and D’Alembert principles it is

necessary to ensure that the displaced paths, q + δq, are geometrically

feasible so that the Lagrange multiplier rule can be used

However, for a generic nonholonomic constraint the following holds

g(q, q, t) = 0 6⇒ g(q + δq, q + δq, t) = 0

ModelingDynamics



Finalremarks

Constrained Dynamics II

meaning that not all displaced paths are geometrically possible

For linear constraints of the form A(q, t)q + B(q, t) = 0 that are also

semiholonomic, i.e., they are an exact differential of some function f (q, t)

(see [Flannery,2005])

A(q, t)q + B(q, t) =ddt

f (q, t) =∂f∂q

q +∂f∂t

the geometric feasibility holds, i.e.

g(q, q, t) = 0 ⇒ g(q + δq, q + δq, t) = 0

I Nonholonomic constraints arise often in closed kinematic chains subject to

velocity constraints

ModelingDynamics



Finalremarks

Dynamics Under Holonomic Constraints

I Replacing the free Lagrangian in the Euler-Lagrange equations by the

constrained Lagrangian yields (see for instance [Flannery,2005],

[McClamroch,Wang,1988], [Bloch, 2003])

ddt∂L∂q −

∂L∂q = τ + λJ(q)

subject to

g(q) = 0

J(q)q = 0

where

I L is the free Lagrangian

I λ if the vector of Lagrange multipliers

I J(q) =[∂g∂qi

]is the Jacobian associated to the holonomic constraints

g(q) = 0

ModelingDynamics



Finalremarks

Dynamics Under Nonholonomic Constraints

I D’Alembert’s principle is valid only for linear semi-holonomic constraints of

the form

A(q)q + B(q) = 0

(see [Flannery,2005])

I Replacing the free Lagrangian in the Euler-Lagrange equations by the

constrained Lagrangian yields

ddt∂L∂q −

∂L∂q = τ + λA(q)

subject to

A(q)q + B(q) = 0

ModelingDynamics



Finalremarks

NE Constrained Dynamics

I The terms n+1fn+1, n+1ηn+1 in the iterative N-E can be used to to account

for any force/torque constraints the end-effector is subject to

I Jacobians are maps between velocity space or between generalized

torques,[Craig,1989]

I The relation τe = J(θ)T fexternal expresses how the external forces, fexternal ,

are mapped in torques through the geometry of the robot

I This component can be added directly in the dynamics equations

M(θ)θ + V (θ, θ) + G(θ) = τ + τe

ModelingDynamics



Finalremarks

Examples of Nonholonomic ConstraintsDue to Contact

I Unicycle rolling constraint

x sin(θ)− y cos(θ) = 0

I A generic sliding constraint

K x + y = 0

For a robot with model

[xy

]=

[1 1/KK 1

] [vx

vy

]

ModelingDynamics



Finalremarks

Unicycle Constrained Dynamics

I The robot is in contact with the floorand hence subject to contact forcesthat do not allow side slipping

I The unicycle motion constraint is

x sin(θ)− y cos(θ) = 0

which yields for the main equation

[m 0 00 m 00 0 Iczz

] xyθ

=

[FxFyNz

]+

[sin(θ)− cos(θ)

0

]λ

ModelingDynamics



Finalremarks

Unicycle Constrained Dynamics

I If the free Lagrangian (and hence the dynamics) is written in terms of the

wheel angular velocities

Let ρ be the curvature radius of the trajectory; the unicycle constraint can

also be written as

v = ωρ

and hence

ωr

(1−

ρ

d

)+ ωl

(1 +

ρ

d

)= 0

τc =

1− ρd

1 + ρd

λ

ModelingDynamics



Finalremarks

Constrained robot dynamics - Example(Lara Croft’s ponytail)

Credits:news.bbc.co.uk/cbbcnews/hi/sci_tech/newsid_2364000/2364099.stm Credits: www.laracroft.com

I Model as a multi-link serial manipulator, the tip of the ponytail correspondsto the end-effector, moving either

I In free space, or

I With tip constrained to a fixed position / slide along a surface

ModelingDynamics



Finalremarks

A Differential-Algebraic Solution forConstrained Robot Dynamics I

I Given

M(θ)θ + N(θ, θ) = τ + τc

subject to

A(θ) = 0

I One can write

A(θ) = 0

A(θ)θ = J(θ)θ = 0

A(θ) = J(θ)θ + J(θ)θ = 0

θ = M−1 (−N + τ + τc) = M−1 (−N + τ + JTλ)

ModelingDynamics



Finalremarks

A Differential-Algebraic Solution forConstrained Robot Dynamics II

I Substituting above

JM−1JTλ = −J θ − JM−1 (−N + τ)

λ =(

JM−1JT)−1 (

−J θ − JM−1(τ − N))

= −J−T MJ−1J θ − J−T (τ − N)

ModelingDynamics



Finalremarks

Example: Rolling disk

I Dynamics of the wheel angular velocity rolling on a horizontal plane, without

side slipping and not being steered

I Thin wheel disk model for wheel inertia

mr2

2 ω = u + rλ

m = 1, r = 1, u = 1

v − ωr = 0

I The solution of the DAE system is

ω = 4

λ = 1

ModelingDynamics



Finalremarks

Example: The RIOL robot I

I Legged locomotion mode using

1. Rigid grasping

2. Compliant grasping, during sliding along the line

3. Unconstrained motion

ModelingDynamics



Finalremarks

Example: The RIOL robot II

ModelingDynamics



Finalremarks

Example: The RIOL robot III

One kinematic loop

One kinematic loop Two independent kinematic loops

I Total of 3 models

I No dissipative terms included

ModelingDynamics



Finalremarks

Example: The RIOL robot - Model 1

I No obstacles in the cable and the robot moves straight along the cable by

using the combined (synchronized) motion by joints 2 and 6, with the pulleys

1 and 3 always in contact with the line.

I When claw 1 grabs the line rigidly, claw 3 is sliding along the line, and

vice-versa.

I Full free Lagrangian

L =12

7∑i=1,i 6=4

mi0vi

T 0vi +12

7∑i=1,i 6=4

0ωiT

Ii 0ωi (1)

I Full model

M1(θ)θ + N1(θ, θ) = Q + JT1 (θ)λ1

J1(θ)θ − (0,−v , 0) = 0

ModelingDynamics



Finalremarks

Example: The RIOL robot - Model 1 I

a) Unconstrained model c) Position constrained model e) Velocity constrained model

I

Initial conditions are set to(0.4,−π/2, 0, 0, 0, 0) and (0.4,−π/2, 0, 0)

respectively for the real and the simplifiedmodel in all the tests

I For the velocity constrained tests the end-effector is constrained to move

along the line with velocity (0,−0.1, 0) m/s

ModelingDynamics



Finalremarks

Example: The RIOL robot - Model 1 II

I Plot a) shows a typical n-pendulum behavior with relatively small

displacements by the joints given that the starting configuration is close to

equilibrium

I Plot b) is coherent with having claw 3 rigidly grasping a fixed point; gravity

induces the sin like motion by all of the rotation joints

I Plot c) is coherent with the fact that claw 3 is sliding with linear motion; note

the linear velocity offsetting the sin like motion

ModelingDynamics



Finalremarks

Example: The RIOL robot - Model 2I The loop is closed by the intermediate claw and includes also a two link

serial open chain

I The free Lagrangian includes the intermediate link with the linear actuator,

L =12

7∑i=1

mi0Vi

T 0Vi +12

7∑i=1

0ωiT

Ii 0ωi

I The constraint imposed by the intermediate claw when in a rigid graspsituation is of the type

J2(θ)θ = 0

where J2 is the Jacobian of the chain composed by links 1-4

The free serial chain composed by links 5, 6 and 7 is a serial chain thatoperates unconstrained

I For the situation in which the claw is allowed to slide along the line theconstraint is similar to that used in model 1,

J2(θ)θ = (0,−v , 0)

ModelingDynamics



Finalremarks

Example: The RIOL robot - Model 2 I

a) Middle claw position constrained full model b) Middle claw unconstrained full model

I

The starting configuration is

(θ1, θ2, θ3, d4, θ5, θ6, θ7) =

(0.4,−π/2, 0, 0.6, 0, π/2, 0),

with all the velocities set to zero

ModelingDynamics



Finalremarks

Example: The RIOL robot - Model 2 II

I Plot a) is coherent with having (i) a closed chain, formed by joints 1-4,

moving under the action of gravity (in a sin like motion), and (ii) an open

chain, formed by joints 4-7, moving under the action of gravity

The amplitude of the motion of joints 6 and 7 is bigger than the other joints,

which is coherent with the fact that they are not constrained

I Plot b) shows the unconstrained motion of the linear joint 4 under the action

of gravity; note that the motion is close to parabolic

ModelingDynamics



Finalremarks


I Two closed chains; the righthand claw grabs the line rigidly whereas the

middle either grabs the line rigidly or is allowed to slide along the cable

I All the links are involved and hence the free Lagrangian is identical to that of

model 2

I The constraints for the middle and righthand claws are

J2(θ)θ = 0

J2(θ)θ = (0,−v , 0)

for the rigid and loose grasp of the middle claw, and

J1(θ)θ = (0,−v , 0)

for the righthand claw

ModelingDynamics



Finalremarks


a) Middle claw position constrained full model b) Middle claw unconstrained full model

I The starting configuration identical to that of the test of model 2

I Plot a) is coherent with having all the claws grabbing rigidly the line; in such

situation the robot forms a single rigid body

I The unconstrained situation amounts to the same dynamics as that for

model 2

ModelingDynamics



Finalremarks

Swarms I

I

Modeling using Or-

dinary Differential

Equations (ODE)

qi = Ai qi +Bj qj +Bi ui , i, j = 1, . . . , n, i 6= j

The ui inputs can

be interpreted as

artificial "social"

forces

I The v.f Ai qi + Bi ui expresses the individual dynamics of the i-th robot

I The v.f Bj qj is included ad-hoc to constrain the motion of the i-th robot as a

function of the motion of the j-th robot

ModelingDynamics



Finalremarks

Swarms II

I If the swarm is to be modeled as a set of smaller size teams of robots then

an alternative modeling can use Partial Differential Equations (PDE) of the

diffusion type

∂φ

∂t+∇ · f = τ

to account for the transfer of robots between the teams in the swarm (φ may

represent the density of robots in some region)

ModelingDynamics



Finalremarks

Including dissipative effects in robotdynamic models I

I Models of dissipative effects are expressed in terms of forces and torques;

hence they can be included directly in the dynamics equations

I Classical Models include different components with each taking care of a

specific aspect of the friction

I a) - Coulomb friction

I b) - Coulomb plus viscous friction

I c) - Stiction (static friction) plusCoulomb and viscous friction

I d) - The friction force may decreasecontinuously from the static frictionlevel

ModelingDynamics



Finalremarks

Including dissipative effects in robotdynamic models II

I Static models (Coulomb + viscous)

F =

Fe if v = 0 and |Fe| < FS

FS sgn(Fe) if v = 0 and |Fe| ≥ FS

I Static models (Stribeck)

F =

F (v) if v 6= 0Fe if v = 0 and |Fe| < FS

FS sgn(Fe) otherwise

where F (v) is an arbitrary function, e.g.,

F (v) = FC + (FS − FC)e−|v/vS |δS

+ Fv v

where vS is called the Stribeck velocity.

ModelingDynamics



Finalremarks

Including dissipative effects in robotdynamic models III

I Dynamic models (Dahl)

dF/dx = σ(1− F/Fc sgn(v))α

where x is a displacement, F the friction force, Fc the Coulomb friction

force, σ is the stiffness coefficient and α is a parameter that determines the

shape of the stress-strain curve (a value of 1 is commonly used)

ModelingDynamics



Finalremarks

Architectural Concepts in Robot Control

I High level control

Decision making, path planning, and eventually also trajectory generation

I Middleware

Roughly, it encompasses all the interfacing between high and low levels and

infrastructure services needed for any of the components in high and low

levels to operate properly, e.g., communications

I Low level control

Everything that relates to path following and actuator control

ModelingDynamics



Finalremarks

Classes of Robot Models to Control

Models

Fixed structure

Variable structure

-

Continuous/Discrete?

Hybrid

Continuous/Discrete

-

x = f (x, u)

orxk+1 = f (xk , uk )

?xc = fc(xc , xd , uc)

xdk+1 = fd (xd , xc , udk )

?x ∈ F (x, u)

orxk+1 ∈ F (xk , uk )

?

ModelingDynamics



Finalremarks

Key issues in Robot Control I

I Stability

Mission goals are usually required to be stable equilibria

I Reaching a configuration and staying in a close neighborhood

I Reaching a region centered at a given configuration and staying

inside the region

I Reaching a given path and keep following it

I Robustness to uncertainties in the dynamics model

In general models are not exact; often uncertainties can be estimated and

bounded

I Robustness to external disturbances

Observation noise, input noise, etc

ModelingDynamics



Finalremarks

Key issues in Robot Control II

I Dependability

Besides handling the decision making adequate to the objectives of the

mission, the high level control system must be prepared to handle gracefully

whatever contingencies may arise

It is key that the framework that integrates every component used to control

a robot be prepared to handle exception conditions (see for instance

[Barbosa et.al, 2007])

ModelingDynamics



Finalremarks

Stability

I Standard Lyapunov theory for systems modeled by fixed structure systems

(smooth differential equations)

I Generalized Lyapunov theory for variable structure systems (hybrid systems

and differential inclusions with upper semi-continuous righthand side)

Upper semi-continuous models often arise in real life (see Corollary 4.4 in

[Smirnov, 2002] on the existence of solutions of differential inclusions

x ∈ F (x) when F (x) is USC)

ModelingDynamics



Finalremarks

Stability in Fixed Structure Systems II Stability of equilibrium states - Lyapunov stability, [Khalil,]

x = 0 is stable if

∀ε > 0,∃δ(ε) > 0, : ∀t ≥ 0, ‖x(0)‖ < δ(ε)⇒ ‖x(t)‖ < ε

and asymptotically stable if it is stable and δ can be chosen such that

‖x(0)‖ < δ(ε)⇒ limt→∞

x(t) = 0

I Input-to-state stability - A bounded input leads to a bounded state

I Input-to-output stability - A norm of the input-output map is bounded

ModelingDynamics



Finalremarks

Stability in Fixed Structure Systems II

I Lyapunov theorem for continuous systems, (Theorem 4.1 in [Khalil,])

Let x = 0 be an equilibrium point for x = f (x) and D ⊂ Rn be a domain

containing x = 0. Let V : 7→ R be a continuously differentiable function such

thatV (0) = 0,

∀x ∈ D − 0, V (x) > 0

∀x ∈ D, V (x) ≤ 0

Then, x = 0 is locally stable

If

∀x ∈ D − 0, V (x) < 0

then x = 0 is locally asymptotically stable

The extension to global requires the additional condition

‖x‖ → ∞⇒ V (X)→∞

ModelingDynamics



Finalremarks

Stability in Fixed Structure Systems III

I La Salle invariance principle (Theorem 4.4 in [Khalil,])

A set M is said invariant with respect to x = f (x) if

∀t ∈ R, x(0) ∈ M ⇒ x(t) ∈ M and positively invariant if t is constrained to

R+

Let Ω ⊂ D be a compact set that is positively invariant with respect to

x = f (x). Let V : D 7→ R be a continuously differentiable function such that

∀x ∈ Ω, V (x) ≤ 0. Let E be the set of all points in Ω where V (x) = 0. Let

M be the largest invariant set in E . Then every solution starting in Ω

approaches M as t →∞

ModelingDynamics



Finalremarks

Stability in Variable Structure Robots I

I Differential systems with discontinuous righthand side can be formed when

the robot behaves according to different models and each model is activated

according to specific operational conditions

F (x , u) =

f1(x , u) if condition 1

......

fm(x , u) if condition m

Without loosing generality, for Robotics purposes one can assume that the

switching conditions are such that F (x , u) is upper semi-continuous

Often the switching conditions are defined by the high level control

ModelingDynamics



Finalremarks

Stability in Variable Structure Robots III Lyapunov theorem for discontinuous systems, (see Theorem 8.2 in

[Smirnov, 2002])

Let x = 0 be an equilibrium point for x ∈ F (x) and assume that there exists

η > 0, a positive definite function V : Rn 7→ R and a negative definite

function W : Rn × R 7→ R such that

D+V (x)(v) ≤ W (x)

for all v ∈ F (x), |x | ≤ η

Then the equilibrium x = 0 is asymptotically stable

D+V (x)(v) is the Dini upper derivative defined as

D+V (x)(v) = limh → 0+,v′ → v

supV (x + hv ′)− V (x)

h

ModelingDynamics



Finalremarks

Stability in Variable Structure Robots III

I Robots with variable kinematic structure, e.g., open kinematic chains that

are transformed into closed chains and vice-versa, may require a practical

stability index that controls the switching between models

For legged robots stability is tightly tied to the locomotion gait

I Zero Moment Point (ZMP), or Center of Pressure (CoP)

[Sardain,Bessonnet,2004, Goswami,1999]

Point on the ground where (i) the resultant of the ground reaction

forces is zero, or, equivalently, (ii) the total moment due to gravity and

inertia equals zero, or, equivalently (iii) the moment generated by the

reaction torque has null horizontal projection

Stability requires that the ZMP point does not leave the convex hull of

the foot support area

ModelingDynamics



Finalremarks

Stability in Variable Structure Robots IV

I Ground projection of the center of mass (GpCM), [Goswami,1999]

Point on the ground where the center of mass is projected

Stability requires that the GpCM point does not leave the convex hull

of the foot support area

I Foot rotation Indication (FRI), [Goswami,1999]

Point on the ground at which the resultant moment of the force/torque

acting on the foot is normal to the surface

Stability requires that the FRI point be contained inside the convex

hull of the foot support area

FRI indicates the occurrence of foot rotation and its direction;

ModelingDynamics



Finalremarks

Control and Controllability

I The controllability theorem (see [Hermann,Krener,1977]) provides a

constructive way to build generic controllers for nilpotent systems

Given a set of v.f.

Generate the corresponding Lie alge-bra

?- Use the Phillip Hall proce-

dure

Find a basis at each configuration?

Use the right sequence of controlsthat yield the goal motion direction

?- Check if the controller is

feasible

ModelingDynamics



Finalremarks

Brocket’s Theorem I

I Necessary conditions for feedback stabilization of nonlinear systems (see

[Bloch, 2003])

Consider the nonlinear system x = f (x , u) with f (x0, 0) = 0 and f (·, ·)

continuously differentiable in a neighborhood of (x0, 0)

Necessary conditions for the existence of a continuously differentiable

control law for asymptotically stabilizing (x0, 0) are:

I (i) The linearized system has no uncontrollable modes associated

with eigenvalues with positive real part

I (ii) There exists a neighborhood N of (x0, 0) such that for each ξ ∈ N

there exists a control uξ(t) defined for all t > 0 that drives the solution

of x = f (x , uξ) from the point x = ξ at t = 0 to x = x0 at t =∞

ModelingDynamics



Finalremarks

Brocket’s Theorem III (iii) The mapping γ : N × Rm 7→ Rn, N a neighborhood of the origin,

defined by γ : (x , u) 7→ f (x , u) should be onto an open set of the

origin

I Consider a generic system x = f (x , u) subject to nonholonomic constraints

g(x , x) = 0

The nonholonomic constraints imply that points in a neighborhood of the

origin do not need to yield velocities covering an open set of the origin

hence γ is not onto

It is not possible to stabilize a nonholonomic system by a smooth time

invariant feedback law

ModelingDynamics



Finalremarks

Brocket’s Theorem III

I A simple intuition for this result can be obtained directly from the geometric

interpretation of the Lie bracket

To move along an arbitrary motion direction in the Control Lie Algebra (CLA)

requires the switching on/off of the primitive v.f in the right sequence

This means that a feedback control law u = γ(x) does not need to be a

smooth or even continuous mapping; nonholonomic constraints tend to

impose complex maneuvering

ModelingDynamics



Finalremarks

Model Inversion / Exact Linearization II Given the generic model

M(θ)θ + C(θ, θ) + G(θ) = τ

Given that M(θ) is always nonsingular, it is algebraically possible to choose

τ = M(θ)τ ′ + C(θ, θ) + G(θ) such that

θ = τ ′

that is, a system similar to a particle with unit mass

This technique corresponds to an exact linearization at each point θ

I Standard linear control can be used

I Easy to obtain control laws that yield system robust to bounded

uncertainties in the model (see for instance [Sciavicco,Siciliano,1996])

ModelingDynamics



Finalremarks

Model Inversion / Exact Linearization II

I Multiple variations are possible,

τ = τ ′ + G(θ) Compensate for the gravity

τ = τ ′ + G(θ) + V (θ, θ) Compensate for gravity + centrifugal andCoriolis terms

τ ′ = −Kpe − Kv e −∫ t

0Kp(s)ds

I In general

τ = M(θ)τ ′ + N(θ, θ) + w

with M and N approximations for M and N and w an additional input

ModelingDynamics



Finalremarks

Extended PID Approaches

I The underlying idea is to add ad-hoc terms to the standard PID law to shape

its behavior

I Terms using the sign of a function of the error (sliding mode control)

τ = −Kpe − Kv e − Kesgn(f (e))− Ki

∫ t

0e(s)ds

where f (e) is some function of the error

I Generalized integral terms

τ = −Kpe−Kv e−Kesgn(f (e)+ e)−Kf f (e)−∫ t

0

(Kip f (e(s)) + Kiv f (s)

)ds

where f (x) =

1 if x ≥ π/2sin(x) if |x | < π/2−1 if x ≤ −π/2

ModelingDynamics



Finalremarks

Sliding Mode - An Informal Statement

I See [Khalil,] on SM control (and other control approaches)

I Define a sliding surface to where the system must converge (and stay)

I Compute a Lyapunov candidate for it

I Analyze all the possibilities for the sign of the terms in the derivative of the

Lyapunov function

Each possibility corresponds to a single mode

I Check that there is the adequate upper bound for the variations imposed by

the switching between modes in order to ensure stability

I A key feature of SM approaches is that the control laws are naturally robust

to dynamics uncertainties (as the control laws can mostly be defined using

only the sign features of the terms in the Lyapunov function)

ModelingDynamics



Finalremarks

Sliding Mode - A Generalized VersionI Given a robot

M(θ)θ + N(θ, θ) = τ

and a sliding surface S =θ, : g(θ, θ) = 0

compute τ such that

min d(θ,K ) The global control objective

min d(θ,S) The path to goal control objective

subject to

M(θ)θ + N(θ, θ) = τ

where d(θ,A) stands for the distance between a point θ and a set A

I Choose a candidate for Lyapunov function; a common choice is

V (s) =12

s2

with

s = g(θ, θ)

ModelingDynamics



Finalremarks

Sliding Mode - Basic ExampleI Control to x = 0

x = x3 + u

Using a sliding surface αx = 0, for some α > 0

I Lyapunov stability

V (s) =12

s2

V (s) = αx(x3 + u)

= αx4 + αxu < 0

Choosing u =

−β1x3 x > 0

−β2x x < 0, with β1, β2 adequate constants,

yields a stable system

I Clearly this is not the best control for this system but it is useful to illustrate

the freedom in the controller design using the SM strategy

ModelingDynamics



Finalremarks

Sliding Mode - A Path Following Examplefor a Unicycle

I Have a unicycle robot starting in an arbitrary configuration (x(0), y(0), θ(0))

and move such that it approaches and follows a path defined by

s ≡ y = sin(rx)

with r a constant

I Two modes of operation

I Approach the manifold representing the path

I Once there, move in such a way that it stays there

ModelingDynamics



Finalremarks

Sliding Mode - A Path Following Examplefor a Unicycle

ModelingDynamics



Finalremarks

A Roadmap for the Study of Robot Modeland Control

Choose a C-space

Get a model forthe robot

?- Check controllability

Variable structure Select a controller

?- Fixed structure

-

Stability?

Stability?

GeneralizedLyapunov

?Lyapunov

?

Iterate if needed-

robot modeling and control - ulisboa · modeling dynamics control architectural concerns practical...

Documents