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

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

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 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

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

1. Holonomic biRRT 3. Smoothing + discretization 2. Solution Model Updating RRT Back to the car example… Step 1/2: holonomic search Introduction → Synthesis → Correction → Analysis → Conclusion 19

estimated friction true friction Holonomic heuristic Model Updating RRT Back 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 1.Use higher gains 2.Let the “spring extend“by 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

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