[Download] rev chapter-7-june26th

Chapter 7 Non-holonomicmotionplanning

Introduction

Controllabilityand FrobeniusTheorem

Examples ofNonholo-nomicSystems

NonholonomicMotionPlanning

Chapter 7 Nonholonomic motion planning

1

Lecture Notes for

A Geometrical Introduction to

Robotics and Manipulation

Richard Murray and Zexiang Li and Shankar S. Sastry

CRC Press

Zexiang Li1 and Yuanqing Wu1

1ECE, Hong Kong University of Science & Technology

July 29, 2010


Introduction





2


1 Introduction

2 Controllability and Frobenius�eorem

3 Examples of Nonholonomic Systems

4 Nonholonomic Motion Planning


Introduction




7.1 IntroductionChapter 7 Nonholonomic motion planning

3

◻ Examples of nonholonomic system:

θ

φ

(x, y)

Figure 6.2: Disk rolling on a plane.


Introduction





4

◻ Examples of nonholonomic system:


Introduction





5

Consider a rolling disk of radius ρ, as shown in Fig. 6.2

θ

φ

(x, y)

Figure 6.2: Disk rolling on a plane.

No slippage constraints:x − cos θρϕ = 0y − sin θρϕ = 0

Configuration:q = (x, y, θ , ϕ) ∈ E

Subspace of Permissible velocities∆q = {q ∈ TqE∣aTi (q)q = 0, i = 1, 2}


Introduction





6

Pfaffian constraints A(q)q = 0[ 1 0 0 −ρ cos θ0 1 0 −ρ sin θ ]´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶

A(q)

⎡⎢⎢⎢⎢⎢⎣xyθϕ

⎤⎥⎥⎥⎥⎥⎦= 0 (∗)

Q1: Is it possible to move between any two points in E while

satisfying the constraints (∗)?Q2: Is it possible to �nd two constraint functions

hi(q) = 0, i = 1, 2 s.t kerA(q) = kerDqh?


Introduction





7

Assume a1(q) = ∂h1∂q = [ ∂h1∂x

∂h1∂y

∂h1∂θ

∂h1∂ϕ ] = [1 0 0 − ρ cos θ],

⇒ ∂h1∂ϕ= −ρ cos θ , ∂h1

∂θ= 0, ∂2h1

∂θ∂ϕ= ∂2h1∂ϕ∂θ

= 0Since we would have ρ sin θ = 0 which is in general impossible, thisappears to be impossible.

◻ Nonholonomic constraints:Consider the previous example, let f1(q), f2(q) be a basis of ∆q, then

q = f1(q)u1 + f2(q)u2parameterizes a feasible path of the system. For example, letu1 = ϕ, u2 = θ, then⎡⎢⎢⎢⎢⎢⎣

xyθϕ

⎤⎥⎥⎥⎥⎥⎦=⎡⎢⎢⎢⎢⎢⎣

ρ cos θρ sin θ

01

⎤⎥⎥⎥⎥⎥⎦´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶f1(q)

u1 +⎡⎢⎢⎢⎢⎣0010

⎤⎥⎥⎥⎥⎦²f2(q)

u2

Note: A(q)fi(q) = 0, i = 1, 2


Introduction





8

◻ Flows of differential equations:

{ q = f (q)q(0) = q0 ⇒ �ow: ϕ

ft(q), ddt ϕf

t(q) = f (ϕft(q))

Taylor series expansion:

q(ε) = ϕfε(q0) = q0 + εq(0) + 1

2ε2q(0) +O(ε3)

◻ Lie bracket of vector fields f1, f2:

q(4ε) = ϕ−f2ε ○ ϕ−f1ε ○ ϕf2ε ○ ϕf1

ε (q0)q(ε) = q0 + εq(0) + ε2

2q(0) +O(ε3)

= q0 + εf1(q0) + ε2

2

∂f1∂q∣q0

f1(q0) +O(ε3)

q(4ε)

f1f2

εf2

−εf1

−εf2

εf1


Introduction





9

q(2ε) = ϕf2ε ○ ϕf1

ε (q0)= ϕf2

ε (q0 + εf1(q0) + ε2

2

∂f1∂q∣q0

f1(q0) +O(ε3))

= q0 + εf1(q0) + ε2

2

∂f1∂q

f1(q) + εf2(q0 + εf1(q0))

+ 1

2ε2∂f2∂q

f2(q0) +O(ε3)= q0 + εf1(q0) + 1

2ε2∂f1∂q

f1(q0) + εf2(q0) + ε2 ∂f2∂q

f1(q0)+ 1

2ε2∂f2∂q

f2(q0) +O(ε3) = q0 + ε(f1(q0) + f2(q0))+ 1

2ε2(∂f1

∂qf1(q0) + 2∂f2

∂qf1(q0) + ∂f2

∂qf2(q0)) +O(ε3)


Introduction





10

q(3ε) = ϕ−f1ε (q0 + ε(f1(q0) + f2(q0))+ 1

2ε2(∂f1

∂qf1(q0) + ∂f2

∂qf2(q0) + 2∂f2

∂qf1(q0)) +O(ε3))

= q0 + ε(f1(q0) + f2(q0))+ 1

2ε2(∂f1

∂qf1(q0) + ∂f2

∂qf2(q0) + 2∂f2

∂qf1(q0))

− εf1(q0)(q0 + ε(f1(q0) + f2(q0))) + 1

2ε2∂f1∂q

f1(q0) +O(ε3)= q0 + εf2(q0) + 1

2ε2(∂f2

∂qf2(q0) + 2∂f2

∂qf1(q0) − 2∂f1

∂qf2(q0)) +O(ε3)


Introduction





11

q(4ε) = ϕ−f2ε (q0 + εf2(q0) + 1

2ε2(∂f2

∂qf2(q0)

+ 2∂f2∂q

f1(q0) − 2∂f1∂q

f2(q0)) +O(ε3))= q0 + εf2(q0) + 1

2ε2(∂f2

∂qf2(q0) + 2∂f2

∂qf1(q0) − 2∂f1

∂qf2(q0))

− εf2(q0 + εf2(q0)) + 1

2ε2∂f2∂q

f2(q0) +O(ε3)= q0 + ε2(∂f2

∂qf1 − ∂f1

∂qf2) = q0 + ε2[f1, f2](q0) +O(ε3)

Definition: Lie bracket of two vector fields f1(q), f2(q)[f1 , f2](q0) = ∂f2

∂q∣q0

f1(q0) − ∂f1∂q∣q0

f2(q0) (∗)


Introduction





12

◇ Example: Lie bracket of two linear vectorfieldsLet f1(q) = Aq, f2(q) = Bq,A,B ∈ Rn×n, then

[f1 , f2](q) = (AB − BA)q


Introduction




7.2 Controllability and Frobenius TheoremChapter 7 Nonholonomic motion planning

13

Definition:A distribution ∆ is involutive if ∀f1 , f2 ∈ ∆, [f1, f2] ∈ ∆.Theorem 1 (Frobenius Theorem):

If ∆ is an involutive distribution, and rank∆p = m, then toeach point p ∈ E, there exists an maximal integrable manifoldM through p such that TpM = ∆p.

Theorem 2 (Chow’s Theorem):Consider

q = f1(q)u1 +⋯ + fm(q)um , q ∈ Rm (∗)If ∆ = {f1 , f2, [f1 , f2], [f1 , [f1, f2]], [f2, [f1 , f2]], . . . }, thecontrollability Lie algebra generated by ∆, has rank m near p,then the set of points, reachable from p by (∗) contains anopen neighborhood of p.


Introduction




7.3 Examples of Nonholonomic SystemsChapter 7 Nonholonomic motion planning

14

◇ Example: A rolling disk

q =⎡⎢⎢⎢⎢⎢⎣ρ cos θρ sin θ

01

⎤⎥⎥⎥⎥⎥⎦´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶f2(q)

u1 +⎡⎢⎢⎢⎢⎣0010

⎤⎥⎥⎥⎥⎦²f2(q)

u2

f3 = ∂f2∂q

f1 − ∂f1∂q

f2 =⎡⎢⎢⎢⎢⎢⎣

ρ sin θ−ρ cos θ00

⎤⎥⎥⎥⎥⎥⎦


Introduction





15

[f1 , f3] = ∂f3∂q

f1 − ∂f1∂q

f3 =⎡⎢⎢⎢⎢⎢⎣0 0 ρ cos θ 00 0 ρ sin θ 00 0 0 00 0 0 0

⎤⎥⎥⎥⎥⎥⎦⎡⎢⎢⎢⎢⎢⎣ρ cos θρ sin θ

01

⎤⎥⎥⎥⎥⎥⎦−⎡⎢⎢⎢⎢⎢⎣0 0 ρ cos θ 00 0 ρ sin θ 00 0 0 00 0 0 0

⎤⎥⎥⎥⎥⎥⎦⎡⎢⎢⎢⎢⎢⎣ρ cos θρ sin θ

01

⎤⎥⎥⎥⎥⎥⎦=⎡⎢⎢⎢⎢⎣0000

⎤⎥⎥⎥⎥⎦−⎡⎢⎢⎢⎢⎣0000

⎤⎥⎥⎥⎥⎦=⎡⎢⎢⎢⎢⎣0000

⎤⎥⎥⎥⎥⎦,

[f2, f3] =⎡⎢⎢⎢⎢⎢⎣0 0 ρ cos θ 00 0 ρ sin θ 00 0 0 00 0 0 0

⎤⎥⎥⎥⎥⎥⎦⎡⎢⎢⎢⎢⎣0010

⎤⎥⎥⎥⎥⎦=⎡⎢⎢⎢⎢⎢⎣ρ cos θρ sin θ

00

⎤⎥⎥⎥⎥⎥⎦= f4


Introduction





16

◇ Example: A Hopping robot

Angular momentum:I θ +m(l + d)2(θ + ψ) = 0Let u1 = ψ, u2 = l, q = (ψ, l, θ)q =⎡⎢⎢⎢⎢⎣

10

− m(l+d)2

I+m(l+d)2

⎤⎥⎥⎥⎥⎦´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶f1(q)

u1 + [ 010]

²f2(q)

u2 l

θ

ψ

d

Figure 7.5: A simple hopping robot.

[f1 , f2](q) = ∂f2∂q

f1 − ∂f1∂q

f2 =⎡⎢⎢⎢⎢⎣

00

mI(l+d)(I+m(l+d)2)2

⎤⎥⎥⎥⎥⎦≜ f3

Since rank∆ = rank[f1 , f2 , f3]= rank

⎡⎢⎢⎢⎢⎣1 0 00 1 0

− m(l+d)2

I+m(l+d)2 02mI(l+d)

(I+m(l+d)2)2

⎤⎥⎥⎥⎥⎦= 3

the system is controllable.


Introduction





17

◇ Example: Kinematic Car

q = (x, y, θ , ϕ) ∈ ENo-slippage constraint:

sin(θ + ϕ)x − cos(θ + ϕ)y− l cos ϕ ⋅ θ = 0sin θx − cos θy = 0⎡⎢⎢⎢⎢⎢⎣

xyθϕ

⎤⎥⎥⎥⎥⎥⎦=⎡⎢⎢⎢⎢⎢⎣

cos θsin θ

1l + tanϕ

0

⎤⎥⎥⎥⎥⎥⎦´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶f1(q)

u1 +

⎡⎢⎢⎢⎢⎣0001

⎤⎥⎥⎥⎥⎦²f2(q)

u2

y

x

l

φ

θ

Figure 7.8: Kinematic model of an automobile.


Introduction





18

u1 ∶Driving velocity

u2 ∶Steering velocity

f3 = [f1 , f2] =⎡⎢⎢⎢⎢⎢⎣

00

−1

l cos2 ϕ0

⎤⎥⎥⎥⎥⎥⎦, f4 = [f1, f3] =

⎡⎢⎢⎢⎢⎢⎢⎣

−sin θl cos2 ϕ

−cos θcos2 ϕ00

⎤⎥⎥⎥⎥⎥⎥⎦∆ = {f1, f2, f3, f4} has rank 4⇒ controllable.


Introduction





19

◇ Example: A ball rolling on a plane

q = (uf , vf , u0 , v0, ϕ)u1 = ωx , u2 = ωy

q =⎡⎢⎢⎢⎢⎢⎢⎣

0sec uf−ρ sin ϕ−ρ cos ϕ− tanuf

⎤⎥⎥⎥⎥⎥⎥⎦u1 +

⎡⎢⎢⎢⎢⎢⎢⎣

−10

−ρ sin ϕρ cos ϕ

0

⎤⎥⎥⎥⎥⎥⎥⎦u2

= f1(q)u1 + f2(q)u2

F

O

f3 = [f1 , f2] =⎡⎢⎢⎢⎢⎢⎢⎢⎣

0tanuf secuf− tanuf sinψ− tanuf cosψ− sec2 uf

⎤⎥⎥⎥⎥⎥⎥⎥⎦, f4 = [f1 , f3] =

⎡⎢⎢⎢⎢⎢⎢⎣

00

− cos ϕsin ϕ0

⎤⎥⎥⎥⎥⎥⎥⎦


Introduction





20

f5 = [f2, f3] =⎡⎢⎢⎢⎢⎢⎢⎢⎢⎣

0−(1 + sin2 uf ) sec2 uf

2 sin ϕ sec2 uf2 cos cos ϕ sec2 uf2 tanuf sec

2 uf

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎦∆q = {f1 , f2, . . . , f5}(q) has rank 5⇒ controllable.

◇ Example: A falling cat


Introduction





21

(r1(t), r2(t),A1(t),A2(t)) ⊂ R3×R

3× SO(3) × SO(3)

K = 1

2[ω1 ,ω2]T [ J1 J12

JT12 J2] [ω1 ,ω2] + 1

2m∥r∥2

Ji = Ii + εSTi SiJ12 = εS1AT

1 A2S2

Ii ∈ R3∶ intertia tensor of body i

m =m1 +m2,ωi = ATi Ai ,A = AT

1 A2 ∶ shape space

ε = m1m2/m ∶ reduced mass

Si ∈ R3∶ hinge position vector relative to Ci

mr = [ 00g]⇒ T = rz(0)/2g


Introduction





22

Equations of Motion:

[ J1 J12JT12 J2

] [ ω1ω2] + [ ω1 × J1ω1 + εS1Aω2S2ω2

ω2 × J2ω2 + εS2AT ω1S1ω1

] = [ −AI ] τAngular momentum:

µ = (A1J2 +A2JT12)ω1 + (A2J2 +A1J12)ω

q(t) = (A1(t),A2(t)) ∈ Q = SO(3) × SO(3)[A1J1 + A2J

T12 ,A2J2 + A1J12] [ ω1ω2

] = 0ω = ATA ∶ Relative velocity

IlA1ω1 = −(A2J2 +A1J12)ωIlA2ω2 = (A1J1 + A2J

T12)ω

Il = A1J1AT1 + A2J2A

T2 + A2J

T12A

T1 + A1J12A

T2

∶ locked body inertia tensor at shape A


Introduction





23

Cayley parameters: α ∈ R3

A1 = A1(α),A2 = A2(β), q = (α, β) ∈ R6 , u = ω ∈ R3

⇒ ω1 = U(α), ω2 = U(β)B1(q)q = B2(q)u,B1 = [ IlA1U(α) 0

0 IlA2U(β) ]⇒ q = B−11 (q)B2(q)u, u ∈ R3

◇ Example: A falling cat with a universal joint model

A = [ cos θ1 0 sin θ10 1 0− sin θ1 0 cos θ1

] [ 0 1 0cos θ2 0 sin θ2sin θ2 0 − cos θ2 ]

ω = [ 010] θ2 + [ cos θ2

0sin θ2

] θ1 = b1u1 + b2u2⇒ B1(q)q = B2(q)u, u ∈ R2 , q = (θ1 , θ2 , αT)T ⇒ q = B(q)u


Introduction




7.4 Nonholonomic Motion PlanningChapter 7 Nonholonomic motion planning

24

◇ Example: Brockett System

q1 = u1q2 = u2q3 = q1u2 − q2u1

⇒ f1 = [ 10−q2] , f2 = [ 0

1q1]

f3 = [f1 , f2] = f1 = [ 002]⇒ rank∆q = {f1, f2, f3} = 3

Problem 1: Nonholonomic Motion PlanningFind u ∶ [0, 1]↦ R

2 s.t. q(0) = q0 ,q(1) = qt andmin ∥u∥2 = ∫ 1

0∥u∥2dt


Introduction





25

Property 1: The optimal solutions have the form of sinusoids,with integrally related frequencies.

Proof :L(q, q) = (q21 + q22) + λ(q3 − q1q2 + q2q1)

d

dt(∂L(q, q)

∂qi)

⇒⎧⎪⎪⎪⎪⎨⎪⎪⎪⎪⎩

q1 + λq2 = 0q2 − λq1 = 0

λ = 0⇒ λ = const.[ u1u2] = [ 0 −λ

λ 0 ] [ u1u2 ] ≜ Λuu(t) = eΛtu(0)q(0) = (0, 0, 0)⇒ q(1) = (0, 0, a)

[ q1(t)q2(t) ] = (eΛt − I)Λ−1u(0)


Introduction





26

q1(1) = q2(1) = 0⇒ eΛ0 = I ⇒ λ = 2nπ , n = 0,±1,±2, . . .q3(1) = ∫ 1

0(q1u2 − q2u1)dt = − 1

λ(u21 (0) + u22(0)) ≜ a

∫1

0∥u∥2dt = ∥u(0)∥2 = −λa⇒ n = −1, ∥u(0)∥2 = 2πa

In general, forq = B(q)u, u ∈ Rm, q ∈ Rn

Find:

u0 = N∑i=1

αieit(t), α = (α1, . . . , αN) ∈ RN

min J(α , γ) = N∑i=1

α2i + γ∥f (α) − xf ∥2

Lett ∶ RN ↦ R

n∶ α ↦ q(1)

A = ∂t

∂α∈ R

n×N ,Hi = ∂2fi∂α2

,


Introduction





27

ak+1 = ak − µ[σI +ATA]−1[σαk +AT(f (αk) − xf )] ∗


Introduction





28


Introduction





29

◻ Other Techniques:*) Converting into chained form

q1 = u1q2 = u2q3 = q2u1q4 = q3u2qn = qn−1u1

*) Steering with Piecewise Constant inputs*) Differential flatness

Education

[Download] rev chapter-7-june26th