feedback controller motion planner parameterized control policy (pid, lqr, …) one (of many)...

Title of the Presentation, on Multiple Lines if Necessary

Feedback controller

Motion planner

Parameterized

control policy

(PID, LQR, )

One (of many) Integrated Approaches: Gain-Scheduled RRT

Core Idea:

Basic approach: decoupling tractable

Integrated approach: use feedback to shortcut the planning phase

x

goal

x

init

* Maeda, G.J; Singh, S.P.N; Durrant-Whyte, H.

Feedback Motion Planning Approach for Nonlinear Control using Gain Scheduled RRTs, IROS 2010

Introduction Synthesis Correction Analysis Conclusion

1

1

The motivation during my 1st year was, and has been,

Illustrate the application

Gain-Scheduled RRT

Holonomic case

q(m)

q(m/s)

Under differential constraints

x

init

s(m)

r(m)

x

goal

x

rand

Rapidly Exploring Random Trees (RRT) background


2

2

Gain-Scheduled RRT

q(m)

q(m/s)

Under differential constraints

Rapidly Exploring Random Trees (RRT) background

4. Works poorly under differential constraints

1. Solve a control problem

2. Scalable

3. Constrained environment

Connection gap

(good enough?)

5. Hard to avoid the connection gap


3

3

A RRT solution rarely reaches the goal (or connect the two trees) with zero error

How large?

Gain-Scheduled RRT: RRT Connection Gap

Connection gap


4

4

Region search

RRT connection is relaxed

Gain-Scheduled RRT: Search

Backwards tree

Forward tree

goal

Single state search:

Extensive exploration

Feedback system


5

5

GS-RRT: RoA & Verification

Find a candidate

Maximize candidate ( )

Verify candidate

**R. Tedrake, LQR-Trees: Feedback Motion Planning on Sparse Randomized Trees, RSS 2009

V(x) =

In the LQR case: J: optimal cost-to-go S: Algebraic Ricatti Eq.

Sum of squares relaxation

goal

(Thank you Russ )


6

6

Gain-Scheduled Verify V(x) as a Sum of Squares and Increase the RoA**

Gain-Scheduled RRT: Algorithm

Initialize tree(s)

Design feedback at the goal

Estimate the RoA

Extend tree

Try to reach RoA

Finish

yes

no


7

7

Region size Exploration Planning efficiency

Gain-Scheduled RRT

goal


8

8

9

Gain-Scheduled RRT: Result

Same initial and final conditions.

Every solution is different due to the random sampling

obstacle

Cart stopper

Cart stopper

Cart and pole in a cluttered workspace


9

Gain-Scheduled RRT

bi-RRT cart

Result:

cart and pole in state-space

bi-RRT pole

GS-RRT cart

GS-RRT pole


10

ACFR-E: GS-RRT Testbed

Agile: Operates at the performance limits

Perceptive Control/Senor Integration

Use expert action(s) to get better information (the feedback is a senor)

Non-linear!!

ACFR-E


11

AKA Darth-Wendy. Nonlinear and under-actuated tests agility. It is

It even sits in the Argo bay no wonder its cursed!

Manipulation under Large Disturbances


12

12

Models

Excavator:(Koivo et al., 1996)

Terrain(McKyes, 1989)


13

Viewing the Excavator as an Arm on Tracks

Arm

Compensation

Linear controller

X

ref

X

Linear behaviour achieved by compensation

PD

Arm

Compensation

Linear controller

X

ref


Spring/damper behaviour

X

Industrial robots

Anthropomorphic arms

Meka A2 Compliant arm

Fanuc arm

High to low gain spectrum

Field manipulators

?


14

14

Ex: Case of unknown viscous friction

The Problem of Uncertainty

Prediction assumes stationary world

Thus can have errors


15

15

Ex: Case of unknown viscous friction & fast control


Prediction assumes stationary world

Thus can have errors


16

16

Prediction is unreliable

Planning can be expensive to track with feedback

Actuator saturation

Problem of uncertainty:

Update the model

*LaValle, S. M. & Kuffner, J. J., Randomized Kinodynamic Planning , 2001

Unknown viscous friction



17

17

Planning/Control with Model Uncertainty

Feedback - estimation

SysID estimator

Model Uncertainty (parametric)


18

18

What kind of uncertainty?

Parameter estimation

Structured model

1. Holonomic biRRT

3. Smoothing + discretization

2. Solution

Model Updating RRTBack to the car example

Step 1/2: holonomic search


19

19

estimated friction

true friction

Holonomic heuristic

Model Updating RRTBack to the car example

Step 2/2: kinodynamic search on initial heuristic

20


20

Disturbances

Overcoming large disturbances with a PD

Use higher gains

Let the spring extendby increasing error

1. Exploration of tracking actions: fast if desired trajectory is a heuristic

2. Model predictive implementation: forward simulation

From a PD to a RRT

PD controller

Model Updating GS-BiRRT


21

21

Manipulation under Large Disturbances


22

Digging your own grave

22

Gain Scheduled RRT: More Broadly

Stability of a nonlinear system given by a linear controller

RRT prediction

Controller design

Estimation of RoA

Model based

Convex RoA vs actual stabilizable region

Limitations:

What about:

Multiple cases

People


23

23

The lengths of the links are: 1a = 0.07 m; 2a = 1.638 m; 3a = 0.819 m; 4a = 0.527 m. The distances between points described by the subscripts are: ABd = 0.812 m; AHd = 0.174 m; AId = 0.980 m; AGd = 0.938 m; CFd = 0.265 m; CId = 0.926 m; CJd = 0.215 m; CLd = 0.730 m; CQd = 0.310 m; DGd = 0.123 m; DRd = 0.16 m; DLd = 0.090 m; DJd = 0.932 m; DKd = 0.208 m; EHd =0.095 m; %[m]; JLd = 0.823 m; KGd = 0.193 m; KLd = 0.208 m. The moments of inertia are 2OI = 72.5901

2kgm ; 3OI = 6.0486 2kgm ; 4OI = 1.8055

2kgm . The link masses are 2m = 64.761 kg; 3m = 31.197 kg; 4m =37.213 kg. The angles in the digging plane: 4 = 0.4749 rad; 5 = 0.4059 rad; 6 = 0.0173 rad; 7 = 1.8285 rad; 8 = 2.7044 rad; 9 = 0.0147 rad; 10 = 0.5201 rad; 11 = 0.2883 rad; 4312 OPO= = 1.6005 rad;

41 6 = rad; 32 = rad; 44 2 = rad; 26 45 = rad; 6 =dg rad; 1 = 3.0654 rad; b = 0.9111 rad;

]})cos(/[)]sin({[tan 1121121

2 EHABABAH dddd ++++= .

The lengths of the links are:

1

a= 0.07 m;

2

a

= 1.638 m;

3

a= 0.819 m;

4

a

= 0.527 m. The distances

between points described by the subscripts are:

AB

d

= 0.812 m;

AH

d

= 0.174 m;

AI

d

= 0.980 m;

AG

d

=

0.938 m;

CF

d

= 0.265 m;

CI

d

= 0.926 m;

CJ

d

= 0.215 m;

CL

d

= 0.730 m;

CQ

d

= 0.310 m;

DG

d

= 0.123

m;

DR

d

= 0.16 m;

DL

d

= 0.090 m;

DJ

d

= 0.932 m;

DK

d

= 0.208 m;

EH

d

=0.095 m; %[m];

JL

d

= 0.823

m;

KG

d

= 0.193 m;

KL

d

= 0.208 m. The moments of inertia are

2O

I

= 72.5901

2

kgm

;

3O

I

= 6.0486

2

kgm

;

4O

I

= 1.8055

2

kgm

. The link masses are

2

m

= 64.761 kg;

3

m

= 31.197 kg;

4

m

=37.213 kg.

The angles in the digging plane:

4

s

= 0.4749 rad;

5

s

= 0.4059 rad;

6

s

= 0.0173 rad;

7

s

= 1.8285 rad;

8

s

= 2.7044 rad;

9

s

= 0.0147 rad;

10

s

= 0.5201 rad;

11

s

= 0.2883 rad;

4312

OPO=s

= 1.6005 rad;

41

6qpg-=

rad;

32

qpg-=

rad;

44

2qpe-=

rad;

26

45

qpe-=

rad;

6pq=

dg

rad;

1

n= 3.0654 rad;

b

q= 0.9111 rad;

]})cos(/[)]sin({[tan

112112

1

2 EHABABAH

dddd -++++=

-

sqsqqr

.

We have the following parameters: w= 43cm, l=52.7cm. Assume that h= 20cm, =30deg, and that soil has 3^/2.1deg,35,20deg,3.23 mtkPac ==== (sandy loam). From 30,35,0 ===

18.1,52.0 == NNc . From 30,35,35 === 05.2,40.2 == NNc . For deg3.23= cN =0.52+(2.40-0.52)23.3/35=1.77, N =1.18+(2.05-1.18)23.3/35=1.76 then the cutting

force is calculated from (*) as P=(1.2*9.8*0.2^2*1.76 + 20*0.2*1.77)*0.43=3.4 kN at angle (90-30-23.3)=36.7 deg. Two components of the force P (the measurements of the load pin):

cos ,sin PPPP YX == give XP =3.4*sin23.3=1.34 kN, YP =3.4*cos23.3=3.12 kN.

We have the following parameters: w= 43cm, l=52.7cm. Assume that h= 20cm,

a

=30deg, and that soil

has

3^/2.1deg,35,20deg,3.23 mtkPac ==== gfd

(sandy loam). From 30,35,0===afd

18.1,52.0==

g

NN

c

. From 30,35,35===afd

05.2,40.2==

g

NN

c

. For

deg3.23=d

c

N

=0.52+(2.40-0.52)23.3/35=1.77,

g

N

=1.18+(2.05-1.18)23.3/35=1.76 then the cutting

force is calculated from (*) as P=(1.2*9.8*0.2^2*1.76 + 20*0.2*1.77)*0.43=3.4 kN at angle (90-30-

23.3)=36.7 deg. Two components of t he force P (the measurements of the load pin):

ddcos ,sinPPPP

YX

==

give

X

P

=3.4*sin23.3=1.34 kN,

Y

P=3.4*cos23.3=3.12 kN.

P =(gh2N + chNc + qhNq

)w

Px Py h P x

Px

Py h

P x

d

a

ArmCompensationLinear controllerXref


05101520253035404550

-6

-4

-2

0

2

4

6

x position (m)

y position (m)

Iterations: 457 Nodes: 248 / 1

01020304050

-10

-5

0

5

10

Iterations: 2415 Nodes: 225 / 226

x car pos. (m)

y car pos. (m)

-5

0

5

10

-10

-8

-6

-4

-2

0

2

4

6

8

10

x(1)

x(2)

feedback controller motion planner parameterized control policy (pid, lqr, …) one (of many)...

Documents

relaxedgainscheduled

rrt solution

gain scheduled rrts

random trees rrt background4

gsrrt testbedagile

gainscheduled rrtcore

nonlinear control

sparse randomized trees