8803 - Mobile Manipulation: Control Mike Stilman Robotics & - - PDF document

1

Mike Stilman (RIM@GT) 8803 Lecture 13 1

• Mike Stilman
• Robotics & Intelligent Machines @ GT
• Georgia Institute of Technology
• Atlanta, GA 30332-0760
• February 19, 2008

8803 - Mobile Manipulation: Control

Mike Stilman (RIM@GT) 8803 Lecture 13 2

How do we make the robot move?

Today: Control of Dynamical Systems & Robots Thursday: Strategies for Combining Position & Force

2

Mike Stilman (RIM@GT) 8803 Lecture 13 3

The Robot is a Dynamic System

What is a Dynamic System?

– A system whose variables are time-dependent – Inputs and Outputs vary with time

How can we describe a dynamic system?

– Input-output differential equations – Free-body diagrams – State-variable matrix – Transfer functions – Block diagrams

Mike Stilman (RIM@GT) 8803 Lecture 13 4

Control

What is Control?

Given the system state, choose input to get a desired output.

3

Mike Stilman (RIM@GT) 8803 Lecture 13 5

Car Cruise Control

m¨ x x b˙ x f

m¨ x + b ˙ x = f

¨ x = £ − b

m

¤ ˙ x + £ 1

m

¤ f

¨ x + b m ˙ x = f m

State Variable Form:

Mike Stilman (RIM@GT) 8803 Lecture 13 6

Car Cruise Control (Open Loop)

m¨ x x b˙ x f

m¨ x + b ˙ x = f

Steady State → Let

0 + b ˙ xss = f ˙ xr = ˙ xss = f b u = f = b ˙ xr ¨ x = 0

¨ x = £ − b

m

¤ ˙ x + £ 1

m

¤ f

State Variable Form:

4

Mike Stilman (RIM@GT) 8803 Lecture 13 7

Car Cruise Control (Open Loop)

m¨ x x b˙ x f

m¨ x + b ˙ x = f 0 + b ˙ xss = f ˙ xr = ˙ xss = f b f = b ˙ xr

¨ x = £ − b

m

¤ ˙ x + £ 1

m

¤ f

State Variable Form: Ctrl Plant

˙ xr

˙ x

f

¨ x = £ − b

m

¤ ˙ x + £ b

m

¤ ˙ xr

Steady State → Let ¨

x = 0

Mike Stilman (RIM@GT) 8803 Lecture 13 8

Car Cruise Control (Open Loop)

m¨ x x b˙ x f

m¨ x + b ˙ x = f

State Variable Form: Ctrl Plant

˙ xr

˙ x

f

¨ x = £ − b

m

¤ ˙ x + £ b

m

¤ ˙ xr

100 200 300 10 20 30 time (s) velocity (m/s)

m = 1500kg b = 50Ns/m vr = 27m/s

5

Mike Stilman (RIM@GT) 8803 Lecture 13 9

Car Steering

θ

J ¨ θ τ J ¨ θ + b ˙ θ = τ ¨ θ = − b J ˙ θ + 1 J τ b ˙ θ · ˙ θ ¨ θ ¸ = ·0 1 − b

J

¸ ·θ ˙ θ ¸ + ·0

1 J

¸ τ

State Variable Form:

Mike Stilman (RIM@GT) 8803 Lecture 13 10

Car Steering (Proportional Control)

θ

J ¨ θ τ

Ctrl Plant

θr

θ

u

+ −

e

State Variable Form:

b ˙ θ

τ = −Kp(θ − θr)

· ˙ θ ¨ θ ¸ = · 1 − Kp

J

− b

J

¸ ·θ ˙ θ ¸ + · 0

Kp J

¸ θr J ¨ θ + b ˙ θ = τ ¨ θ = − b J ˙ θ + 1 J τ ¨ θ = − b J ˙ θ − Kp J θ + Kp J θr

6

Mike Stilman (RIM@GT) 8803 Lecture 13 11

Car Steering (Proportional Control)

θ

J ¨ θ τ

State Variable Form:

· ˙ θ ¨ θ ¸ = · 1 − Kp

J

− b

J

¸ ·θ ˙ θ ¸ + · 0

Kp J

¸ θr b ˙ θ J ¨ θ + b ˙ θ = τ ¨ θ = − b J ˙ θ + 1 J τ

τ = −Kp(θ − θr)

¨ θ = − b J ˙ θ − Kp J θ + Kp J θr

Ctrl Plant

θr

θ

u

+ −

e

2 4 6 8 10 0.2 0.4 0.6 0.8 1 time (s) position (rad)

J = .1kgm2 b = .1Nms θr = .5rad

Mike Stilman (RIM@GT) 8803 Lecture 13 12

Car Steering (Proportional Control)

θ

J ¨ θ τ b ˙ θ

Stability? Exponential Convergence? Limitations? Ctrl Plant

θr

θ

u

+ −

e

2 4 6 8 10 0.2 0.4 0.6 0.8 1 time (s) position (rad)

¨ θ = − b J ˙ θ − Kp J θ + Kp J θr

τ = −Kp(θ − θr)

7

Mike Stilman (RIM@GT) 8803 Lecture 13 13

Car Steering (PD Control)

θ

J ¨ θ τ

Ctrl Plant

θr

θ

u

+ −

e

State Variable Form:

b ˙ θ J ¨ θ + b ˙ θ = τ ¨ θ = b J ˙ θ + 1 J τ ˙ θr = 0

+ −

˙ θ

· ˙ θ ¨ θ ¸ = · 1 − Kp

J

− b+Kd

J

¸ ·θ ˙ θ ¸ + · 0

Kp J

¸ θr ¨ θ = − b J ˙ θ − Kd J ˙ θ − Kp J θ + Kp J θr

τ = −Kd( ˙ θ − ˙ θr) − Kp(θ − θr)

2 4 6 8 10 0.1 0.2 0.3 0.4 0.5 0.6 time (s) position (rad)

Mike Stilman (RIM@GT) 8803 Lecture 13 14

PD Control

Under-damped Over-damped Critically damped (Just Right)

2 4 6 8 10 0.1 0.2 0.3 0.4 0.5 0.6 time (s) position (rad) 2 4 6 8 10 0.1 0.2 0.3 0.4 0.5 0.6 time (s) position (rad) 2 4 6 8 10 0.1 0.2 0.3 0.4 0.5 0.6 time (s) position (rad)

8

Mike Stilman (RIM@GT) 8803 Lecture 13 15

PD Control (Selecting Gains)

¨ θ = −b + Kd J ˙ θ − Kp J (θ − θr) ¨ θ = −K0

d ˙

θ − K0

p(θ − θr)

¨ θ + K0

d ˙

θ + K0

pθ = Kpθr

Characteristic Polynomial: Standard Form:

s2 + K0

ds + K0 p

s2 + 2ξωns + ω2

n

ξ ωn

Damping Ratio: Natural Frequency:

K0

p = ω2 n

K0

d = 2ξωn

ξ = 1

K0

p = ω2 n

K0

d = 2ωn = 2

q K0

p

Mike Stilman (RIM@GT) 8803 Lecture 13 16

PD Control (Selecting Gains)

K0

p = ω2 n

K0

d = 2ωn = 2

q K0

p

Starting Point: What is ? Gains are limited by:

• structural flexibility
• time delay
• sampling rate
• actuator saturation

ωn

Optimal Control – Linear Quadratic Regulator Tuning!

9

Mike Stilman (RIM@GT) 8803 Lecture 13 17

Car Cruise Control (Closed Loop)

m¨ x x b˙ x f

m¨ x + b ˙ x = f

100 200 300 10 20 30 time (s) velocity (m/s) 5 10 15 20 10 20 30 time (s) velocity (m/s)

Open Loop:

f = −bKp( ˙ x − ˙ xr)

K=20

Mike Stilman (RIM@GT) 8803 Lecture 13 18

Car Cruise Control (Closed Loop)

m¨ x x b˙ x f

m¨ x + b ˙ x = f

100 200 300 10 20 30 time (s) velocity (m/s) 5 10 15 20 10 20 30 time (s) velocity (m/s)

Open Loop:

f = −bKp( ˙ x − ˙ xr)

K=20

10

Mike Stilman (RIM@GT) 8803 Lecture 13 19

Another Example

m¨ x x b˙ x f

m¨ x + b ˙ x = f

m

mg − Kd˙ z − Kp(z − zr)

f = −bKp( ˙ x − ˙ xr)

Mike Stilman (RIM@GT) 8803 Lecture 13 20

Manipulator Control (Gravity Compensation)

d1

θ1 τ1

m1 z

¨ θ1 = −d1 cos(θ1)m1g + τ1

x1 = d1 cos(θ1)

x

11

Mike Stilman (RIM@GT) 8803 Lecture 13 21

Manipulator Control (Gravity Compensation)

d1

θ1 τ1

m1 z

¨ θ1 = −d1 cos(θ1)m1g + τ1

x1 = d1 cos(θ1)

x Acceleration due to gravity: How do we do position control?

τ1 = d1c1m1g − Kp(θ1 − θ1d) − Kd ˙ θ1

τg = −d1 cos(θ1)m1g

Mike Stilman (RIM@GT) 8803 Lecture 13 22

Manipulator Control (Gravity Compensation)

l1

θ1 θ2

z x

d1

m1 m2

d2

x1 = d1c1 x2 = l1c1 + d2c2

Torque due to gravity:

12

Mike Stilman (RIM@GT) 8803 Lecture 13 23

Manipulator Control (Gravity Compensation)

l1

θ1 θ2

z x

d1

m1 m2

d2

x1 = d1c1 x2 = l1c1 + d2c2

Torque due to gravity:

τ2 = −d2c12m2g τ1 = −d1c1m1g − (l1c1 + d2c12)m2g

Mike Stilman (RIM@GT) 8803 Lecture 13 24

Manipulator Control (Gravity Compensation)

l1

θ1 θ2

z x

d1

m1 m2

d2

τ2 = −d2c12m2g τ1 = −d1c1m1g − (l1c1 + d2c12)m2g x1 = d1c1 x2 = l1c1 + d2c12

Torque due to gravity: How do we do position control?

· τ1 τ2 ¸ = · d2c12m2 d1c1m1 + (l1c1 + d2c12)m2 ¸ g − Kp · θ1 − θ1d θ2 − θ2d ¸ − Kd · ˙ θ1 ˙ θ2 ¸

Gains are typically diagonal matrices

13

Mike Stilman (RIM@GT) 8803 Lecture 13 25

Manipulator Control (Full Model)

M(q)¨ q + C(q, ˙ q) ˙ q + G(q) = τ

Mike Stilman (RIM@GT) 8803 Lecture 13 26

Manipulator Control (Full Model)

Computed Torque:

M(q)(¨ qd − Kd(˙ q − ˙ qd) − Kp(q − qd)) + C(q, ˙ q)˙ q + G(q) = τ

Augmented PD:

M(q)¨ qd + C(q, ˙ q)˙ qd + G(q) − Kd(˙ q − ˙ qd) − Kp(q − qd) = τ

M(q)¨ q + C(q, ˙ q) ˙ q + G(q) = τ

14

Mike Stilman (RIM@GT) 8803 Lecture 13 27

More Jacobians (Virtual Work)

δx = Jδq W = FTx δW = FTδx δW = τ Tδq FTδx = τ Tδq FTJδq = τ Tδq τ = JTF

Open Loop Force Control

Mike Stilman (RIM@GT) 8803 Lecture 13 28

More Jacobians (Virtual Work)

δx = Jδq W = FTx δW = FTδx δW = τ Tδq FTδx = τ Tδq FTJδq = τ Tδq τ = JTF

Open Loop Force Control

JT

vig =?

Jvi

Velocity Jacobian for Link Mass i:

G = −(JT

v1(m1g) + JT v2(m2g)...JT vn(mng))

τ = G(q) + JTF

15

Mike Stilman (RIM@GT) 8803 Lecture 13 29

Manipulator Workspace Control

M(q)¨ q + C(q, ˙ q) ˙ q + G(q) = τ ¯ M(q)¨ x + ¯ C(q, ˙ q)˙ x + ¯ G(q) = F

¯ M = J−T MJT ¯ C = J−T (CJ−1 + M ˙ J−1) ¯ G = J−T G F = J−T τ

τ = JTF

Khatib ’80, Murray ‘94

F

Mike Stilman (RIM@GT) 8803 Lecture 13 30

Manipulator Workspace Control

τ = JTF

Khatib ’80, Murray ‘94 Computed Torque:

τ = JTF M(q)¨ q + C(q, ˙ q) ˙ q + G(q) = τ ¯ M(q)¨ x + ¯ C(q, ˙ q)˙ x + ¯ G(q) = F

¯ M = J−T MJT ¯ C = J−T (CJ−1 + M ˙ J−1) ¯ G = J−T G F = J−T τ

F = ¯ M(q)(¨ xd − Kd( ˙ x − ˙ xd) − Kp(x − xd)) + ¯ C(q, ˙ q) ˙ x + ¯ G(q)

16

Mike Stilman (RIM@GT) 8803 Lecture 13 31

Manipulator Workspace Control

τ = JTF

Computed Torque:

F = ¯ M(q)(¨ xd − Kd( ˙ x − ˙ xd) − Kp(x − xd)) + ¯ C(q, ˙ q) ˙ x + ¯ G(q)

τ = JTF

Gravity Augmented PD:

F = ¯ G(q) − Kd(˙ x − ˙ xd) − Kp(x − xd)

τ = JTF M(q)¨ q + C(q, ˙ q) ˙ q + G(q) = τ ¯ M(q)¨ x + ¯ C(q, ˙ q)˙ x + ¯ G(q) = F

¯ M = J−T MJT ¯ C = J−T (CJ−1 + M ˙ J−1) ¯ G = J−T G F = J−T τ

Mike Stilman (RIM@GT) 8803 Lecture 13 32

Joint Space vs. Work Space (Step Response)

Joint Trajectory Work Space Trajectory Joint Space Control Work Space Control

17

Mike Stilman (RIM@GT) 8803 Lecture 13 33

Summary

• Dynamic Systems change over time
• Open Loop Control
• Closed Loop PID Control
• Gravity Compensation & Linearization
• Joint Space Position Control
• Work Space Position Control
• Open Loop Force Control

Mike Stilman (RIM@GT) 8803 Lecture 13 34

Thursday

• How do we close the loop on force control?
• What is the relationship between force/position control?
• Can we do both?