Nonholonomic Constraints Examples - LaDiSpe - Politecnico di Torino

34
Nonholonomic Constraints Examples Basilio Bona DAUIN – Politecnico di Torino July 2009 B. Bona (DAUIN) Examples July 2009 1 / 34

Transcript of Nonholonomic Constraints Examples - LaDiSpe - Politecnico di Torino

Nonholonomic ConstraintsExamples

Basilio Bona

DAUIN – Politecnico di Torino

July 2009

B. Bona (DAUIN) Examples July 2009 1 / 34

Example 1

Given qT =[x y

]Tcheck that the constraint

φ(q) = (2x + sin y − ye−x)dx

dt+ (x cos y + cos y + e−x)

dy

dt= 0

is holonomic.

We have, by inspection

a1(q) = 2x + sin y − ye−x a2(q) = x cos y + cos y + e−x

B. Bona (DAUIN) Examples July 2009 2 / 34

We have to test the following conditions

∂a1(q)

∂y≡

∂a1(q)

∂q2

?=

∂a2(q)

∂q1≡

∂a2(q)

∂x

but since

∂a1(q)

∂y= cos y − e−x

∂a2(q)

∂x= cos y − e−x

the two terms are equal, and the constraint is holonomic

φ(q) = x2 + x sin x + ye−x + sin y = c

i.e.,x2 + x sin x + ye−x + sin y − c = 0

B. Bona (DAUIN) Examples July 2009 3 / 34

Example 2

Given qT =[x y θ

]T, check that the constraint

x sin θ − y cos θ = 0

[sin θ − cos θ 0

]

dxdydθ

= 0

is nonholonomic.

Computing the mixed partial derivative we obtain

∂a1(q)

∂y= 0

∂a1(q)

∂θ= cos θ

∂a2(q)

∂x= 0

∂a2(q)

∂θ= − sin θ

∂a3(q)

∂x= 0

∂a3(q)

∂y= 0

B. Bona (DAUIN) Examples July 2009 4 / 34

Hence the constraint is nonholonomic, since

∂a1(q)

∂y=

∂a2(q)

∂x

∂a1(q)

∂θ6=

∂a3(q)

∂x∂a2(q)

∂θ6=

∂a3(q)

∂y

Using the integrating factor approach, we have to compute the terms in

∂µa1

∂q2= sin q3

∂µ

∂q2

∂µa2

∂q1= − cos q3

∂µ

∂q1

∂µa1

∂q3= sin q3

∂µ

∂q3+ µ cos q3

∂µa3

∂q1= 0

∂µa2

∂q3= − cos q3

∂µ

∂q3µ sin q3

∂µa3

∂q2= 0

B. Bona (DAUIN) Examples July 2009 5 / 34

and write the system:

sinq3∂µ

∂q2= − cos q3

∂µ

∂q1

sinq3∂µ

∂q3= −µ cos q3

cos q3∂µ

∂q3= µ sin q3

Squaring and adding the two last equations yields∂µ

∂θ= ±µ; substitution

in the system above gives

µ cos θ = µ sin θ

µ sin θ = −µ cos θ

Since the only possible solution is µ = 0, the constraints are nonholonomic.

B. Bona (DAUIN) Examples July 2009 6 / 34

Example 3

Figure: Example of nonholonomic constrained system: a disk rotating on a planewithout sliding.

B. Bona (DAUIN) Examples July 2009 7 / 34

The previous Figure represents a rigid wheel, with radius ρ and negligiblethickness, rolling on a plane πc with a single contact point Oc(t), thatchanges in time according to the wheel motion.

The plane πr of the wheel is orthogonal to the axis of the wheel, describedby the unit vector jr . This plane forms an angle α(t) wrt the normal of theplane πc , that, for simplicity is horizontal.

The plane πr forms an angle β(t) wrt to a conventional direction on πc ,e.g., the direction of the unit vector i0 of the fixed reference frameR0(i0, j0, k0). This angle may vary with time.

The motion constraint implies in the first place that the contact point Oc

must in any circumstance belong to πc (no jumps allowed).

B. Bona (DAUIN) Examples July 2009 8 / 34

In the second place, the non sliding motion implies two condition:

(Transversal sliding) the impossibility for the wheel to movetransversally; the wheel velocity has no components along the unitvector jc on πc .

(Longitudinal sliding) the wheel displacement d(t) on the plane alongic must always be equal to the length ρθ(t) of the arc under θ. If notso, a sliding or slipping condition arise: the wheel moves, but doesnot rotate, or the wheel rotates but does not move.

These conditions imply that the instantaneous velocity of Oc or Or mustbe aligned with the unit vector i′, by construction itself aligned with ic .

Rotation around kr are allowed, since the rotation takes place around anideal geometric point Oc , that do not imply a sliding effect.

B. Bona (DAUIN) Examples July 2009 9 / 34

In conclusion, the wheel translates along a local direction given by i′ = icand rotates around the instantaneous rotation axis kr = k′

The homogeneous matrix T0r between the fixed reference frame R0 and

the local reference frame Rr is

from R0 to Rc : T0c = Trasl(dc(t)) Rot(−k, β(t)), where

dc =[xc yc 0

]Trepresents Oc in R0:

from Rc to Rr : Tcr = Rot(i, α(t)) Trasl(ρ) Rot(j, θ(t)), where

ρ =[0 0 ρ

]T

Without constraints, the generalized coordinates describing the system arethe three components of dc(t) and the three angles α(t), β(t), e θ(t):

qT =[xc yc zc α β θ

]

B. Bona (DAUIN) Examples July 2009 10 / 34

It seems convincing that the planar motion reduces the dof from six tofive, considering the additional constraint

dc · k0 = 0; → q3(t) = 0,∀t

Therefore the new generalized coordinates are

qTc =

[xc yc α β θ

]

The non sliding/slipping condition generates a relation between thecontact point velocity and the wheel velocity; when we represent all thevectors in a common frame R0, we have:

dc(t) = ρθ(t)ic

B. Bona (DAUIN) Examples July 2009 11 / 34

Using the quantities in Figure, we write

ρθ(t)

−sβ0

−d

dt

xc

yc

0

= 0

or

dxc = ρθ(t)cβdt

dyc = −ρθ(t)sβdt

hencedxc

dyc

= −1

tanβ

B. Bona (DAUIN) Examples July 2009 12 / 34

Replacing the infinitesimal displacements with the virtual ones, we have

tanβ δxc + δyc = 0 or sinβ δxc + cos β δyc = 0 (1)

i.e.,

[sin q4 cos q4 0 0 0

]

δq1

δq2

δq3

δq4

δq5

= 0

This represent a constraint in Pfaffian form witha1 = sinβ, a2 = cos β, a3 = a4 = a5 = 0. Computing the partial derivativeswe have

∂a1(q)

∂q4= cos q4;

∂a4(q)

∂q1= 0;

∂a2(q)

∂q4= − sin q4;

∂a4(q)

∂q2= 0

B. Bona (DAUIN) Examples July 2009 13 / 34

It appears immediately that equality relations among partial derivatives arenot satisfied, so the constrain is non holonomic.

We could ask how many degrees of freedom has the system; relation (1)introduces a constraint that links the virtual displacement δxc to δyc .Recalling the dof’s definition as the number of virtual displacementsindependent and complete, the system has only four of suchdisplacements, i.e., δxc , δα, δβ e δθ, while δyc is connected to δxc by (1)and δzc = 0 due to the plane motion.

The accessibility space however has dimension 5, since, it is possible, witha suitable motion, to bring the state from any initial point q0 ∈ R

5 to anyfinal point qf ∈ R

5.

B. Bona (DAUIN) Examples July 2009 14 / 34

Example: the unicycle

The unicycle is illustrated in Figure 2.

Figure: The unicycle scheme.

B. Bona (DAUIN) Examples July 2009 15 / 34

The unicycle generalized coordinates are

q =[q1 q2 q3

]T=

[x y θ

]T

As seen in a previous example, the non sliding motion on the planedetermines the following single (m = 1) constraint

xsθ − ycθ =[sθ −cθ 0

]

︸ ︷︷ ︸

A

q1

q2

q3

= Aq = 0

Now we compute the vector fields forming a basis of the null space of A;the null space has dimension n − m = 2; moreover, since the null spaceconditions are expressed by the following equation

[sθ −cθ 0

]

g11 g21

g12 g22

g13 g23

=[sθ −cθ 0

] [g1 g2

]

we obtain

g1 =

sθ0

; g2 =

001

B. Bona (DAUIN) Examples July 2009 16 / 34

Hence the kinematic model of the unicycle becomes

xy

θ

= Gu =

sθ0

u1 +

001

u2

Physical considerations allows to define the following inputs: u1 = v ,where v is the driving velocity of the unicycle, obtained as the product ofthe wheel radius ρ and the wheel angular speed α, and u2 = ω, whereω = θ is the steering velocity around the vertical axis.

B. Bona (DAUIN) Examples July 2009 17 / 34

The Lie bracket of the two input vector fields is

[g1,g2] =∂g2

∂qg1 −

∂g1

∂qg2 =

0 0 00 0 00 0 0

g1 −

0 0 −sθ0 0 cθ

0 0 0

g2 =

sθ−cθ

0

We can see that [g1,g2] ≡ g3 is linearly independent from g1 and g2. Wecan also compute the next term [g1, [g1,g2]] as

[g1,g3] =∂g3

∂qg1−

∂g1

∂qg3 =

0 0 cθ

0 0 sθ0 0 0

001

0 0 −sθ0 0 cθ

0 0 0

sθcθ

0

=

sθ0

that is no more independent from the previous fields.

B. Bona (DAUIN) Examples July 2009 18 / 34

If we consider the iterative procedure ∆1,∆2, · · · ,∆κ = ∆κ+1 to obtainthe involutive closure, i.e, the accessibility distribution ∆A, and recall thatκ is called the degree of nonholonomy, we have

∆1 = ∆ = span {g1,g2}∆2 = span {g1,g2, [g1,g2]}∆3 = span {g1,g2, [g1,g2], [g1, [g1,g2]]} = ∆2

Hence ∆2 = ∆3, the accessibility distribution ∆A is

∆A = ∆2 = span {g1,g2, [g1,g2]}

with dim∆A = 3 and κ = 2.

B. Bona (DAUIN) Examples July 2009 19 / 34

Chained form: the unicycle

Recalling the kinematic model of the unicycle

xy

θ

= Gu =

sθ0

v +

001

ω

with the following coordinates change

z1 = θ; z2 = xcθ + ysθ; z3 = xsθ − ycθ

and the following input change

v ≡ u1 = v2 + z3v1

ω ≡ u2 = v1

we obtain the chained form

z1 = v1

z2 = v2

z3 = z2v1

Notice that the new coordinates z2, z3 give the position of the unicycle ina reference frame that rotates with the robot body.

B. Bona (DAUIN) Examples July 2009 20 / 34

Example: the bicycle

The bicycle is illustrated in Figure 3, where C is the instantaneous rotationcenter, θ the body orientation, φ the steering wheel angle, and x , y thecartesian coordinates of the rear (passive) wheel contact point.

Figure: The bicycle scheme.

B. Bona (DAUIN) Examples July 2009 21 / 34

Possible bicycle generalized coordinates are

q =[q1 q2 q3 q4

]T=

[x y θ φ

]T

The vehicle motion is constrained by the two non-sliding constraints at thecontact points of the posterior and anterior wheels:

xas34 − yac34 = 0xs3 − yc3 = 0

where xa, ya are the cartesian coordinates of the anterior wheel, s3 = sin θ,c3 = cos θ s34 = sin(θ + φ), and c34 = cos(θ + φ). The bicycle body fixthe relation between the position of the two wheels

xa = x + ℓc3 xa = x − ℓs3ya = y + ℓs3 ya = y + ℓc3

B. Bona (DAUIN) Examples July 2009 22 / 34

The constrains can be rewritten as

xs34 − yc34 − ℓθc4 = 0xs3 − yc3 = 0

The associated A(q) matrix is

A(q) =

[s3 −c3 0 0s34 −c34 −ℓc4 0

]

whose rank is ρ(A) = 2; the null space N (A) dimension is ν = n − ρ = 2.A basis of the null space can be found setting A

[g1 g2

]= 0. Consider

g1 =

g11

g12

g13

g14

g2 =

g21

g22

g23

g24

G =

[g1 g2

]

B. Bona (DAUIN) Examples July 2009 23 / 34

The condition AG = O gives the following constraints

s3g11 − c3g12 = 0 s3g21 − c3g22 = 0s3g11 − c3g12 − ℓc4g13 = 0 s3g21 − c3g22 − ℓc4g23 = 0

Since we have eight unknowns, but only four equations, we apply some

heuristics: we set g2 =[0 0 0 1

]Tand g14 = 0, so we reduce to two

equations in three unknowns.

We can verify that the choice

g1(q) =

c3c4

s3c4

s4/ℓ0

g2(q) =

0001

satisfies the constraints, yielding to the kinematic model of the bicycle

xy

θ

φ

=

c3c4

s3c4

s4/ℓ0

u1 +

0001

u2

B. Bona (DAUIN) Examples July 2009 24 / 34

Consider the geometric relations among the command variables, if thebicycle is driven by the anterior wheel, we set u2 as the steering velocity ω,while u1 is the anterior driving velocity v . In this case

xy

θ

φ

=

c3c4

s3c4

s4/ℓ0

v +

0001

ω

If, on the contrary, the driving velocity v ′ is applied by the rear wheel, wehave the geometric relation u1c4 = v ′, and consequently

xy

θ

φ

=

c3c4

s3c4

s4/ℓ0

u1 +

0001

ω ⇒

xy

θ

φ

=

c3

s3s4/c4ℓ

0

v ′ +

0001

ω

B. Bona (DAUIN) Examples July 2009 25 / 34

Chained form: the bicycle

Assume the rear wheel drive bicycle model

xy

θ

φ

=

c3

s3t4ℓ0

v ′ +

0001

ω

where t4 = tanq4. Introducing the following coordinates change

z1 = x ; z2 =1

ℓsec3 θ tanφ; z3 = tanφ; z4 = y

B. Bona (DAUIN) Examples July 2009 26 / 34

and the following input transformation

v =v1

cos θ

ω = −3

ℓv1 sec θ sin2 φ +

1

ℓv2 cos3 θ cos2 φ

we obtain the chained form

z1 = v1

z2 = v2

z3 = z2v1

z4 = z3v1

B. Bona (DAUIN) Examples July 2009 27 / 34

Example: a planar space robot

The planar satellite in Figure 4 consists of a free-flying main body, with tworotating arms; for simplicity their mass is concentrated at end of the arm.

Figure: The planar satellite scheme.B. Bona (DAUIN) Examples July 2009 28 / 34

Consider the following generalized coordinates: x , y the position ofcenter-of-mass (com), θ its orientation wrt a fixed frame, φ1, φ2 theorientation of the two arms wrt to the body frame.

Henceq =

[x y φ1 φ2 θ

]T

Other parameters are: r the distance of the arm revolute joints from comand ℓ the length of the two arms; M is the main body mass, Γ its inertiamoment wrt to the com, and m are the equal masses of the two arms.

With the usual angle convention (positive if anti-clockwise), the twomasses have coordinates

x1 = x + r cos θ + ℓ cos(θ + φ1); y1 = y + r sin θ + ℓ sin(θ + φ1)x2 = x − r cos θ − ℓ cos(θ + φ1); y2 = y − r sin θ − ℓ sin(θ + φ1)

B. Bona (DAUIN) Examples July 2009 29 / 34

The Lagrangian function, assuming no gravity, is simply the kinetic energy

T =1

2[M(x2 + y2) + Γ θ2 + m(x2

1 + y21 ) + m(x2

2 + y22 )]

but it can also be written in simplified form as

T =1

2(M + 2m) ‖x‖2 +

1

2

[

φ1 φ2 θ]M

φ1

φ2

θ

; M ∈ R3×3

where x is the com velocity vector and M is diagonal, with the followingelements mij :

m11 = mℓ2 m12 = 0 m13 = mℓ2 + mr cos φ1

m22 = mℓ2 m23 = mℓ2 + mr cos φ2

m33 = Γ + 2m(r2 + ℓ2) + 2mℓr(cos φ1 + cos φ2)

We notice that L does not depend on θ, and we can reduce the

generalized coordinates to the following q =[φ1 φ2 θ

]T.

B. Bona (DAUIN) Examples July 2009 30 / 34

If the satellite is free-floating and no other generalized force/torque isapplied, and assuming that the satellite com remains in (0, 0) for t ≥ t0,then T reduces to

T =1

2

[

φ1 φ2 θ]M

φ1

φ2

θ

; M ∈ R3×3

and the Lagrange equations reduce to

d

dt

∂L

∂θ−

∂L

∂θ︸︷︷︸

=0

= 0

since we know (from Hamilton equations) that∂L

∂θis the angular

momentum p

B. Bona (DAUIN) Examples July 2009 31 / 34

We have

p =∂L

∂θ= m13φ1 + m23φ2 + m33θ

If the initial angular momentum is zero, we have the constraint

m13(q)q1 + m23(q)q2 + m33(q)q3 = 0

Since the actuated coordinates are u1 = φ1 and u2 = φ2, we have

q = g1(q)u1 + g2(q)u2

with

q1

q2

q3

=

10

−m13

m33

u1 +

01

−m23

m33

u2

B. Bona (DAUIN) Examples July 2009 32 / 34

It is sufficient to compute

∂m13

∂q2= 0

∂m23

∂q1= 0

∂m13

∂q3= 0

∂m33

∂q1= −2mℓr sin q1

∂m23

∂q3= 0

∂m33

∂q2= −2mℓr sin q2

to verify that the constraints are nonholonomic.

B. Bona (DAUIN) Examples July 2009 33 / 34

References

[01] F. Bullo, A.D. Lewis, Geometric Control of Mechanical Systems,Springer, 2005.

[02] H.K. Khalil, Nonlinear Systems, III Ed. Springer, 2002.

[03] R.M. Murray, Z. Li, S. Sastry, A Mathematical Introduction toRobotic Manipulation, CRC Press, 1994.

[04] S. Sastry, Nonlinear Systems: Analysis, Stability, and Control,Springer, 1999.

[05] B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo, Robotics: Modelling,Planning and Control, Springer, 2009.

[06] Z. Qu, Cooperative Control of Dynamical Systems: Applications toAutonomous Vehicles, Springer, 2009.

B. Bona (DAUIN) Examples July 2009 34 / 34