Local Control Policies for Global Control and Navigation

Local Control Policies for Global Control and Navigation David C. Conner dcconner@cmu. edu Robotics Institute Carnegie Mellon University Center for the Foundations of Robotics Matthew T. Mason, Regent 16 July. Sensor 2003 Based Planning Lab Microdynamic Systems Laboratory

Theme Develop collections of local control policies – Control policies respect local constraints • Obstacles • Dynamical constraints – Convergence guaranteed over limited domain – Global performance guaranteed by composition – Deployment is automatic Systems: • Kinematic : • Dynamical : 16 July 2003 Center for the Foundations of Robotics 2

Overview • GOLF: A Metaphor – Good OLd Fashioned Sequential Composition – Potential Fields • Task and Spatial Decompositions – Convex Cellular Decompositions – Focus on for clarity • Local Control Policy Design – Kinematic Systems – Constrained Dynamical Systems • Future Directions and Conclusion 16 July 2003 Center for the Foundations of Robotics 3

GOLF : A Metaphor • Open Loop – System: – Not robust to: • Modeling error • External disturbance 16 July 2003 Center for the Foundations of Robotics 4

GOLF : A Metaphor • Feedback System: – Disturbance rejection – Modeling uncertainty • Lyapunov Methods 16 July 2003 Center for the Foundations of Robotics 5

GOLF : A Metaphor • Constraints – Potential Field Methods – Lyapunov Methods? 16 July 2003 Center for the Foundations of Robotics 6

GOLF : A Metaphor • Sequential Composition – Limited domain – Partial order – Prepares 16 July 2003 Center for the Foundations of Robotics 7

Potential Field Methods • Potential Fields – Dynamical Performance – Topological Considerations • Saddle Points • Local minima • Navigation Functions – Free of local minima (large k) – Numerically difficult to implement 16 July 2003 Center for the Foundations of Robotics 8

Opportunities • • • Control Policy Design Control Policy Deployment Global Performance Local Minima Planning Abstraction 16 July 2003 Center for the Foundations of Robotics 9

Task Decomposition Abstraction: Planning vs. Control – Plan discrete goals – Control policy generates continuous “behaviors” Example: “Get me to CFR on time” – Conventional version: Plan : Time Scaling : Control : – Our version: 16 July 2003 Plan : Out of Smith - across road - NSH-… Control : Center for the Foundations of Robotics 10

Spatial Decomposition • Cellular decomposition – Collection of cells Exact Approximate – Computational geometry • NP-hard in general • Known algorithms for polygonal • Know maps a priori • Adjacency graph – Dijkstra’s Algorithm – Spanning Tree (Partial order) 16 July 2003 Center for the Foundations of Robotics Spanning Tree 11

Control Policy Preconditions Must guarantee that the system: • respects – the boundaries of the cells – the dynamical constraints • converges – to the goal located inside the cell – exits the cell within the desired outlet zone Each cell is conditionally positive invariant 16 July 2003 Center for the Foundations of Robotics 12

Our Approach • Decompose free space – Convex polytopes in – Open sets – Shared boundaries • Partial order over cells – Adjacency graph – Spanning Tree • Generic Control Policy – – Map to unit ball Potential field in ball Pull back to cell Control policy in cell 16 July 2003 Center for the Foundations of Robotics 13

Mapping to Solution Space Contours of Constant Disk Radius • Map cell to unit ball • Ck continuous on interior • Full rank on interior • Solve Laplace’s Equation on disk Contours of Constant Potential • Pull potential solution back to cell 16 July 2003 Center for the Foundations of Robotics 14

Vector Field Definition Negative Normalized Gradient Vector Field • Orthogonal to cell boundary (a. e. ) • Inward pointing along inlet boundary • Outward pointing along outlet boundary • Potential function is a C k smooth function on the interior of the polygon • No local minima or saddle points • Flow along integral curves of behavior within a cell 16 July 2003 induces desired Center for the Foundations of Robotics 15

Kinematic System Constraint Goal Control Start Automated generation of polygonal decomposition [Keil, ’ 85] Automated deployment of controllers based on adjacency graph Controller switching is automatic based on region boundaries 16 July 2003 Center for the Foundations of Robotics 16

Dynamical System Constraints Control Integral Curves of Specification of the adjacency relationships induces a globally convergent controller for sufficiently high gain K. 16 July 2003 Center for the Foundations of Robotics 17

Constrained Dynamical Systems is not sufficient. Let Spectral Norm 16 July 2003 Center for the Foundations of Robotics 18

Constrained Dynamical Systems • Hybrid control policies within cell – Save – Align – Join – Flow • Savable set 16 July 2003 Center for the Foundations of Robotics 19

Constrained Dynamical Systems • Save control policy and Domain : Goal Set : 16 July 2003 Center for the Foundations of Robotics 20

Constrained Dynamical Systems • Align control policy Proof: Evaluate 16 July 2003 on boundary, Center for the Foundations of Robotics 21

Constrained Dynamical Systems • Join control policy Want Where K* > 0 chosen such that 16 July 2003 Center for the Foundations of Robotics 22

Constrained Dynamical Systems • Join control policy Brake Turn Bend Steer : Accelerate : 16 July 2003 Center for the Foundations of Robotics 23

Constrained Dynamical Systems • Flow control policy Set of zero measure, need to “fatten” Given , it is always true that (by Lemma 4. 2 and IVT) ( by Lemma 4. 3) (by Lemma 4. 4) 16 July 2003 Center for the Foundations of Robotics 24

Constrained Dynamical Systems • Simulation Results Hey! I have to give you some reason to come to my proposal : -) 16 July 2003 Center for the Foundations of Robotics 25

Future Work • Velocity Constraint back-chaining – Flow leaving cell enters in savable set of neighbor – Look for less conservative velocity scalings • Extend methods to – systems with non-holonomic constraints – underactuated systems • Develop tools to allow behavior design – parallel parking – doorway navigation • Interface with higher level AI reasoning 16 July 2003 Center for the Foundations of Robotics 26

Conclusions • Presented methods to automatically deploy local control policies – Local control policies respect local constraints – Composition guarantees global convergence – Fully actuated systems in • Planning vs. control abstraction • Future extensions – Non-holonomic constraints – Underactuated systems 16 July 2003 Center for the Foundations of Robotics 27

References 16 July 2003 28

16 July 2003 29

Convergent Control Policy • Map cell to ball • Map goal point in ball to origin of ball • Potential 16 July 2003 Center for the Foundations of Robotics 30

Polygon to Disk Mapping 16 July 2003 Center for the Foundations of Robotics 31

Mapping Comparison Our Mapping Alternate Mapping m is number of faces Schwarz. Christoffel Conformal Mapping 16 July 2003 Linear Retraction Mapping Center for the Foundations of Robotics 32

Mapping Comparison Our Mapping Alternate Mapping m is number of faces Schwarz. Christoffel Conformal Mapping 16 July 2003 Linear Retraction Mapping Center for the Foundations of Robotics 33

Laplace’s Equation on Disk Steady State Heat Equation a 1 a 0 Boundary Condition Solution 16 July 2003 Center for the Foundations of Robotics 34

Additional Examples 16 July 2003 Center for the Foundations of Robotics 35

C 2 Fillet Curve Approximation 16 July 2003 Center for the Foundations of Robotics 36

Polygonal Approximation • C 2 continuity of mapping needed • is singular at the vertices • Approximate at the vertex by C 2 fillet curve 16 July 2003 Center for the Foundations of Robotics 37