nonholonomic constraints in roboticstorres/wp-content/uploads/2018/02/nonholono… · leonardo a....

Classification and Examples Robot Kinematics: Pfaffian Constraints Dynamics with Nonholonomic Pfaffian Constraints

Nonholonomic Constraints in Robotics

Leonardo A. B. Torres

June, 2016.




Section 1

Classification and Examples




Constraints in Mechanical Systems

Constraints are algebraic and/or differential relations that were notincluded in the Robot canonical dynamical model, and that must besatisfied by ~q(t) and ~q(t), during the temporal evolution of themechanism. They can be categorized as:

1 Reonomic constraints: depend explicitly on time t.

φr(t, ~q, ~q) = 0.

2 Scleronomic constraints: do not depend explicitly on time.

φe(~q, ~q) = 0.

Our emphasis will be on scleronomic contraints.




Scleronomic Constraints: types

1 Holonomic contraints:h(~q) = 0;

with h(·) : Q → R. A set of p holonomic constraints is equivalent tothe determination of a nq − p dimensional manifold M in theConfiguration Space; i.e. M ⊂ Q, where nq is the dimension of Q.

2 Nonholonomic constraints:

r(

~q, ~q)

= 0,

with r(·, ·) : Q× TQ → R. Such constraints do not necessarilyimply that there will be a manifold to which the Robot configuration~q is confined during all the time. On the other hand, this constraintmeans that it should always have a very specific relation between~q(t) and the possible generalized velocities ~q(t) at each time instantt.




Holonomic Constraints in Robotics

In principle, all holonomic constraints should have already beenincluded in the description of the Configuration Space Q, such that~q becomes an independent variable to be chosen arbitrarily.

That is, to have additional holonomic constraints points to the factthat something was not done right in the determination of Q.

Example: A Robot moving in a circular ring with radius a. Suppose one

has chosen Q ≡ R× R, i.e. ~q = [x y]⊤.

x

y

θ

h(~q) = 0 ⇒ x2 + y2 − a2 = 0. In this case,the true Configuration Space has dimension1, i.e. ~q = θ ∈ [0, 2π) and Qtrue ≡ S1.




Holonomic Constraints in Robotics

Sometimes, due to the problem complexity, a constraint initially thoughtto be nonholonomic reveals to be instead a holonomic one. In this case,one says that the constraint is integrable. For example:

r(

~q, ~q)

=d

dt(h(~q)) = 0,

=∂h

∂q1q1 +

∂h

∂q2q2 + . . .+

∂h

∂qnq

qnq= 0; → Known constraint.

∴ h (~q)− constant = 0 → Hidden holonomic constraint.

Example (cont.): in the previous example of a robot confined to acircular ring, the following could have been found:

xx + yy = 0.





Usually nonholonomic constraints are associated with:

Rolling without slipping condition. E.g.: unicycle with zero lateralspeed.

(q1,q2) q3

~q = [x y θ]⊤

Constraint: yx= senθ

cos θ ⇒xsenθ − y cos θ = 0.

Angular momentum conservation. E.g.: jumping robot.

q3

q2

q1 Assuming zero initial angular momentum,one has that: Iq1 + m(q3)

2(q1 + q2) = 0,with I the moment of inertia and m the leg’smass concentrated at the foot.




Pfaffian Constraints

Commonly found nonholonomic constraints in Robotics have a morespecific structure characterizing them as Pfaffian constraints:

r(

~q, ~q)

= [a(~q)]⊤~q = 0,

where a(~q) : Q 7→ Rnq . A set of p Pfaffian constraints can be combined

in the same mathematical expression as

A(~q)~q = 0, (1)

such that

A(~q) =

a⊤1 (~q)a⊤2 (~q)

...a⊤p (~q)

.




Pfaffian Constraints and Virtual Work

Assuming Pfaffian constraints one can show that the forces associatedwith their existence do not perform virtual work. To see this, considerthat

~τ const. = A⊤(~q)~λ (2)

is the total generalized constraint force, which is formed by the linearcombination of vector fields associated with each one of the Pfaffianconstraints:

~τ const. = λ1a1(~q) + λ2a2(~q) + . . .+ λpap(~q);

with ~λ = [λ1 λ2 . . . λp]⊤ the vector of Lagrange Multipliers.




Pfaffian Constraints and Virtual Work

The Virtual Work δW done by ~τ const. will be

δW =[~τ const.

]⊤δ~q =

[

~λ⊤A(~q)]

δ~q.

Assuming that δ~q = ~q δt, one has that

[

~λ⊤A(~q)]

δ~q = ~λ⊤

✟✟✟✟✯

0[

A(~q)~q]

δt = 0.

And, therefore, constraint forces given by (2) do not perform work for the

special variation δ~q = ~q δt that corresponds to the actual movement.




Section 2

Robot Kinematics: Pfaffian Constraints




Kinematic Modelling

By considering Pfaffian constraints,

A(~q)~q = 0;

one can see that the generalized velocity ~q is a vector belonging to a setcharacterized by the property that every vector in this set, whenpre-multipled by A(~q), results in the zero vector. By definition such set isthe so-called Null Space or Kernel of the matrix A(~q):

N (A(~q)) = {~x ∈ Rnq ;A(~q)~x = 0} ,

~q ∈ N (A(~q)) .

N (A(~q)) is a linear subspace, and therefore there exists a vector basisfrom which every vector in this set can be generated. That is, N (A(~q))corresponds to the linear combination of vector fields gj(~q).




Kinematic Modelling

By assuming that the set of p < nq Pfaffian constraints is a linearlyindependent set of constraints, the Image of the matrix A(~q) ∈ R

p×nq

will have dimension p, and the N (A(~q)) will be m = nq − p dimensional.

Therefore, every generalized velocity vector ~q satisfying the Pfaffianconstraints can be written as

~q = g1(~q)u1 + g2(~q)u2 + . . .+ gm(~q)um ⇒

~q = G(~q)~u, (3)

with ~u = [u1 u2 . . . um]⊤ a vector of real numbers to be determined;and gj(~q) : Q → TQ, j = 1, 2, . . . ,m, are vector fields (columns ofG(~q)), such that:

A(q)gj(~q) = 0, j = 1, 2, . . . , nq − p ⇒ A(~q)G(~q) = 0




Kinematic Modelling – Example

Consider a unicycle where one can arbitrarily set the translational speed u1 andalso the rotational speed u2. The nonholonomic constraint is given by

[

senθ − cos θ 0]

x

y

θ

= 0.

Notice that dim {N (A(~q))} = 3− 1 = 2. Therefore, it is possible to find 2Linearly Independent (L.I.) vector fields g1(~q) and g2(~q), which are a basis forthe Null Space of A(~q), such that A(~q)g1(~q) = 0 and A(~q)g2(~q) = 0. Forexample:

x

y

θ

=

cos θ 0senθ 00 1

[

u1

u2

]

.




Trajectory Planning × Underactuated Systems

Even when only the kinematic model of the Robot withnonholonomic constraints is considered; i.e.

~q = G(~q)~u,

with A(~q)G(~q) = 0; it is a nontrivial task to design appropriatecontrol actions ~u that will drive the Robot from an arbitrary initialconfiguration ~qstart to a final configuration ~qgoal.

Since the dimension of G(~q) is m = nq − p (with m the number oflinearly independent columns of G(~q), and p the number ofnonholonomic Pfaffian constraints), the above system will alwayshave a number of control variables smaller than the number ofdegrees of freedom nq, i.e. dim{~u} = m < nq, and therefore thesystem will be underactuated.




Brockett’s Theorem

Theorem

Let f(~x, ~u) be a smooth function, with ~x ∈ Rnq and ~u ∈ R

m.

~x = f(~x, ~u)

is a system that can be rendered Lyapunov stable by means of an

appropriate state feedback control strategy, which is smooth and

time-invariant, i.e. u = κ(~x). In other words, the point ~x = 0, for ~u = 0,is a stable equilibrium point of

~x = f(~x, κ(~x)) ≡ fκ(~x).

In this case, necessarily, the mapping f(~x, ~u) 7→ Rnq should have the

following property: for every point ~x ∈ Vf , with Vf a neighborhood of

~x = 0, there should be a corresponding pair (~x, ~u) ∈ V , with V a

neighborhood of (0, 0), such that ~x = f(~x, ~u).Leonardo A. B. Torres



Brockett’s Theorem – Consequence

From Brockett’s theorem one can conclude that:

It is not possible to make the configuration ~qgoal a stable equilibriumpoint, by means of a smooth and time-invariant state feedback

law ~u = κ(~q), with κ(·) : Q → Rm, when considering the dynamical

system~q = G(~q)~u.

with G(·) : Q× Rm → R

nq , and m = nq − p, p > 0.

This claim can be verified by noticing that there will always be a nonzerovector ~q ∈ TQ, belonging to a neighborhood of the “null velocity vector”~q = 0, that cannot be obtained as a linear combination of m < nq

columns of G(~q). Notice that this is a consequence of the underactuationassociated with the nonholonomic constraints.




Brockett’s Theorem – Consequence

From the previous conclusion one naturally should search for controlstrategies leading to control laws u = κ(~x, t) that are:

Time-varying ones, and/or;

Discontinuous.




Section 3

Dynamics with Nonholonomic Pfaffian Constraints




Method 1: Explicit Computation of Constraint Forces

Notice that, for Pfaffian constraints, one has that:

{M(~q)~q + C(~q, ~q)~q + g(~q) = ~τ ;

A(~q)~q = 0.(4)

From the last equation, it is true that

A(~q)~q + A(~q)~q = 0. (5)

Moreover, consider that

~τ = ~u︸︷︷︸

control

− b(~q, ~q)︸︷︷︸

non-conservative forces

+ ~τ const.︸︷︷︸

nonhol. constraint forces

, (6)

with ~τ const. = A⊤(~q)~λ.





Using (6) in (4):

M~q + C~q + g(~q) + b(~q, ~q)︸︷︷︸

N(~q,~q)

= ~u+A⊤~λ,

⇒ ~q = M−1[

~u− C~q −N +A⊤~λ]

.

Substituting this last expression in (5), one has that:

AM−1[

~u− C~q −N +A⊤~λ]

+ A~q = 0,

from which the vector of Lagrange multipliers ~λ can be obtained:

~λ =(AM−1A⊤

)−1[

−AM−1(

~u− C~q −N)

− A~q]

(7)





Therefore, from (7),

1 Given ~u, ~q e ~q it is possible to obtain ~λ and to compute thenonholonomic constraint forces by considering

~τ const. = A⊤(~q)~λ.

2 Using the computed ~τ const. it is possible to determine the dynamicalevolution of the Robot given by the expression

M(~q)~q + C(~q, ~q)~q +N(~q, ~q) = ~u+ ~τ const..

Notice that the Pfaffian constraint forces are explicitly computed duringthe Robot simulation, at each integration time step.




Method 2: Order Reduction via Projection Matrix

Instead of explicitly computing ~λ and ~τ const., expressions (5) and (7) can bedirectly substituted in the Robot canonical equations:

M~q =(

~u− C~q −N)

+

A⊤

(

AM−1

A⊤

)−1[

−AM−1

(

~u− C~q −N)

− A~q]

;

M~q =

[

I − A⊤

(

AM−1

A⊤

)

−1

AM−1

]

(

~u−C~q −N)

+

A⊤

(

AM−1

A⊤

)−1

A~q;

[

I −A⊤

(

AM−1

A⊤

)−1

AM−1

]

M~q =

[

I −A⊤

(

AM−1

A⊤

)

−1

AM−1

]

(

~u− C~q −N)

.





From the previous mathematical development, one can define the matrix

Pu = I −A⊤(AM−1A⊤

)−1AM−1,

whose rank is m = nq − p, with p the number of Pfaffian constraints. Pu

is indeed a Projection Matrix, because:

P 2u = Pu.

This property means that, if ~y = Pu~x is the projection of ~x on a linearsubspace, by applying the projection operator again, the same vector isobtained, i.e. Pu~y = ~y.





Similarly, the matrixP = M−1PuM,

is also a projection matrix, i.e. P 2 = P . In this case,

PuM~q = Pu

(

~u− C~q −N)

;

M−1PuM~q = M−1Pu

(

~u− C~q −N)

;

P~q = PM−1(

~u− C~q −N)

(8)

whereP = I −M−1A⊤

(AM−1A⊤

)−1A

︸︷︷︸

M−1PuM

.





Since rank(P ) = rank(Pu) = m = nq − p, only m independent equationscan be obtained from (8).

To complete the set of differential equations needed to characterize theRobot dynamics, one should include p differential equations provided bythe Pfaffian constraints, such that

{

P~q = PM−1(

~u− C~q −N)

;

A~q = −A~q;

and one will have nq independent differential equations.




Method 3: Modified Euler-Lagrange Equation

From the assumption that nonholonomic constraint forces do not performvirtual work, we have that

A(~q)δ~q = 0 ⇒[

Aα(~q) | Aβ(~q)]

[

δ~qαδ~qβ

]

=

[

00

]

,

where

Aα(~q) ∈ Rp × R

nq−p,

Aβ(~q) ∈ Rp × R

p,

δ~qα ∈ Rnq−p

,

δ~qβ ∈ Rp,

with Aβ chosen as an invertible partition of A.





In this case, one can write that

δ~qβ = −(

A−1

β Aα

)

δ~qα (9)

From the above expression, and relying on D’Alembert principle, it follows that

(

d

dt

[

∂L

∂~q

]

−∂L

∂~q− ~τ

)⊤

δ~q = 0 ⇒

(

d

dt

[

∂L

∂~qα

]

−∂L

∂~qα− ~τα

)⊤

δ~qα +

(

d

dt

[

∂L

∂~qβ

]

−∂L

∂~qβ− ~τβ

)

⊤

δ~qβ = 0

The idea is to consider that δ~qα is free/independent, but δ~qβ is determined byδ~qα.





Substituting (9) in the last expression, and assuming that δ~qα is free, one hasthe so-called Modified Euler-Lagrange Equation

d

dt

[

∂L

∂~qα

]

−∂L

∂~qα− ~τα −

(

A−1

β Aα

)⊤

{

d

dt

[

∂L

∂~qβ

]

−∂L

∂~qβ− ~τβ

}

= 0. (10)

To eliminate ~qβ and ~qβ from the above expression, one can make use of thefollowing expressions from (9):

~qβ = −(

A−1

β Aα

)

~qα, (11)

~qβ = −(

A−1

β Aα

)

~qα +d

dt

[

−A−1

β Aα

]

~qα.

From (10) and (11) the full set of differential equations describing the Robotdynamical behavior, considering Pfaffian nonholonomic constraints, can beobtained.




Method 4: Pseudo-velocity approach I

1 Use the matriz G(~q) ∈ Rnq×(nq−p) whose columns span the kernel

of A(~q), i.e. A(~q)G(~q) = 0p×(nq−p), in order to represent modifieddynamical equations considering nonholonomic constraints [1]:

G⊤

{

M~q + C~q + g}

= G⊤

{

~u+A⊤~λ}

G⊤

{

M~q + C~q + g}

= G⊤~u

~q = G~v

G⊤MG~v +G⊤MG~v +G⊤ [CG~v + g] = G⊤~u.




Method 4: Pseudo-velocity approach II

From the previous development, we have that

~q = G⊤~v,

MG~v + CG~v +G⊤g = G⊤~u,

with

MG(~q) = G⊤(~q)M(~q)G(~q),

CG(~q, ~v) = G⊤(~q)M(~q)G(~q, ~v) +G⊤(~q)C(~q, ~v)G(~q).

Therefore we get a new dynamical description using theconfiguration ~q and the so-called pseudo-velocity ~v as the new statevariables.




Method 4: Pseudo-velocity approach III

Notice also that, by definition, G(~q) has full column rank; i.e.rank{G(~q)} = nq − p; and M(~q) ∈ R

nq×nq is a positive definitematrix. Therefore MG(~q) = G⊤(~q)M(~q)G(~q) is also a positivedefinite matrix, such that its inverse exists and one can write that

~q = G⊤(~q)~v,

~v = −MG(~q)−1

[−CG(~q, ~v)~v −G⊤(~q)g(~q) +G⊤(~q)~u

].



Proof 1

Prove that matrix

Pu = I −A⊤(AM−1A⊤

)−1AM−1

is a projection matrix.

P2

u =

[

I − A⊤

(

AM−1

A⊤

)

−1

AM−1

] [

I − A⊤

(

AM−1

A⊤

)

−1

AM−1

]

,

=I − A⊤

(

AM−1

A⊤

)−1

AM−1 − A

⊤

(

AM−1

A⊤

)−1

AM−1+

A⊤

(

AM−1

A⊤

)

−1

✘✘✘✘✘AM

−1A

⊤

✘✘✘✘✘✘✘(

AM−1

A⊤

)

−1

AM−1

,

=Pu.



Giuseppe Oriolo.Control of nonholonomic systems.http://www.dis.uniroma1.it/~oriolo/cns/cns_slides.pdf,2015.



http://www.dis.uniroma1.it/~oriolo/cns/cns_slides.pdf

nonholonomic constraints in roboticstorres/wp-content/uploads/2018/02/nonholono… · leonardo a....

Documents