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
Introduction Synthesis Correction Analysis Conclusion
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
Introduction Synthesis Correction Analysis Conclusion
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
Introduction Synthesis Correction Analysis Conclusion
4
Region search
RRT connection is relaxed
Gain-Scheduled RRT: Search
Backwards tree
Forward tree
goal
Single state search:
Extensive exploration
Feedback system
Introduction Synthesis Correction Analysis Conclusion
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 )
Introduction Synthesis Correction Analysis Conclusion
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
Introduction Synthesis Correction Analysis Conclusion
7
Region size Exploration Planning efficiency
Gain-Scheduled RRT
goal
Introduction Synthesis Correction Analysis Conclusion
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
Introduction Synthesis Correction Analysis Conclusion
Gain-Scheduled RRT
bi-RRT cart
Result:
cart and pole in state-space
bi-RRT pole
GS-RRT cart
GS-RRT pole
Introduction Synthesis Correction Analysis Conclusion
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
Introduction Synthesis Correction Analysis Conclusion
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
Introduction Synthesis Correction Analysis Conclusion
12
Models
Excavator:(Koivo et al., 1996)
Terrain(McKyes, 1989)
Introduction Synthesis Correction Analysis Conclusion
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
Linear behaviour achieved by compensation
Spring/damper behaviour
X
Industrial robots
Anthropomorphic arms
Meka A2 Compliant arm
Fanuc arm
High to low gain spectrum
Field manipulators
?
Introduction Synthesis Correction Analysis Conclusion
14
Ex: Case of unknown viscous friction
The Problem of Uncertainty
Prediction assumes stationary world
Thus can have errors
Introduction Synthesis Correction Analysis Conclusion
15
Ex: Case of unknown viscous friction & fast control
The Problem of Uncertainty
Prediction assumes stationary world
Thus can have errors
Introduction Synthesis Correction Analysis Conclusion
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
The Problem of Uncertainty
Introduction Synthesis Correction Analysis Conclusion
17
Planning/Control with Model Uncertainty
Feedback - estimation
SysID estimator
Model Uncertainty (parametric)
Introduction Synthesis Correction Analysis Conclusion
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
Introduction Synthesis Correction Analysis Conclusion
19
estimated friction
true friction
Holonomic heuristic
Model Updating RRTBack to the car example
Step 2/2: kinodynamic search on initial heuristic
20
Introduction Synthesis Correction Analysis Conclusion
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
Introduction Synthesis Correction Analysis Conclusion
21
Manipulation under Large Disturbances
Introduction Synthesis Correction Analysis Conclusion
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
Introduction Synthesis Correction Analysis Conclusion
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
d
a
ArmCompensationLinear controllerXref
Linear behaviour achieved by compensation
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)