Randomized KinoDynamic Planning Steven LaValle James Kuffner

Randomized KinoDynamic Planning To determine the sequence of control inputs to drive a robot from an initial state to an end state while obeying physically based dynamic models and avoiding obstacles in the robot’s environment Approach: tailored form of randomization (RRT’s) specially suited to high dimensional state spaces

Introduction and Related Research Randomized Potential Field

Introduction and Related Research Randomized Potential Field – A heuristic potential function is defined on the configuration space to steer robot towards goal through gradient descent

Introduction and Related Research Randomized Potential Field – A heuristic function is defined on the configuration space to steer robot towards goal through gradient descent – Random walks are used to escape local minimum traps.

Introduction and Related Research Randomized Potential Field – PRO: Drives exploration with gradient descent – PRO: Works well for holonomic planning – CON: Depends heavily on choice of good potential heuristic function – CON: Hard to do to accommodate obstacles and differential constraints – CON: Local Minima

Introduction and Related Research Probabilistic Roadmap – A graph is constructed on configuration space by generating random configurations and attempting to connect pairs of nearby configurations with a local planner.

Introduction and Related Research Probabilistic Roadmap – PRO: Uniform exploration – PRO: Local planning step is efficient for holonomic and steerable nonholonomic systems (simple) – CON: For complex nonholonomic and dynamic systems, local planning is HARD, like designing non-linear controller – CON: 1000’s of local planning steps typically – CON: designed for many queries (large pre-comp)

Problem Formulation Differential Constraints in State Space

Problem Formulation Handling Obstacles in State Space

Problem Formulation Solution Trajectory Unique Challenges: State Space has twice the dimension of Configuration; worsens the curse Momentum considerations cause drift, overshooting, oscillations, which potential fields and probabilistic roadmaps can’t handle

RRT based planner Rapidly exploring Random Trees- State Space Strategy… – Initialize tree with vertex – Repeatedly, select state at random in state space Select its nearest neighbor in the tree Choose a control and ensuing output that pushes neighbor state towards random state Create new vertex and add to tree

RRT based planner

Rapidly exploring Random Trees Outcome…

RRT based planner Rapidly exploring Random Trees – PRO: works for high degrees of freedom – PRO: no steering required – PRO: biased toward unexplored space – PRO: probabilistic completeness – CON: no well defined metric

RRT based planner Bidirectionnal planning algorithm Strategy: – Grow two RRT’s at initial and goal states – At each growth step, check for intersection – Either halt once path exists, or continue to accumulate best paths

RRT based planner Bidirectionnal planning algorithm Formally…

Spacecraft and Hovercraft tests Model – State

Spacecraft and Hovercraft tests Model – Control

Spacecraft and Hovercraft tests Model – Metric (Euclidean) – Weight vector is normalized – Dot product represents cosine of angle

Spacecraft and Hovercraft tests Model – Controls consists of a fixed set U in each example – Each set includes a ‘no control’ control – Each control is applied over a fixed timestep eg. dt = 0.01 sec – Control timestep is independent of RRT timestep Video

Spacecraft and Hovercraft tests Case 1: Planar Translating Body in X-Z plane 4 DOF: x,z,x’,z’ 4 Controls:

Spacecraft and Hovercraft tests Case 2: Planar Body with Rotation – 6 DoF: x,y,θ,x’,y’, θ’ – 3 controls: Translate forward Rotate clockwise Rotate counterCW

Spacecraft and Hovercraft tests Case 2: Planar Body with Rotation – ~ 5 minutes – 13,600 nodes

Spacecraft and Hovercraft tests Case 3: Translating 3-D body – 6 DoF: x,y,z,x’,y’,z’ – 6 controls: Opposing forces In each of the 3 Principal directions

Spacecraft and Hovercraft tests Case 3: Translating 3-D body – ~1 min – 16,300 nodes

Spacecraft and Hovercraft tests Case 3: Translating 3-D body – ~1 min – 16,300 nodes

Spacecraft and Hovercraft tests Case 4: 3-D body with rotation – Cylindrical satellite object – 12 DoF: x,y,z,Rx,Ry,Rz and derivatives – 5 controls: translate along Cylindirical axis, rotates arbitrarily Simulates satellite docking

Spacecraft and Hovercraft tests Case 4: 3-D body with rotation – ~6 minute – 23,800 nodes

Spacecraft and Hovercraft tests Case 4: 3-D body with rotation – ~6 minute – 23,800 nodes

Spacecraft and Hovercraft tests Case 4: 3-D body with rotation – 12 DoF – 5 controls – Forward, up-down – Clockwise roll

Spacecraft and Hovercraft tests Case 4: 3-D body with rotation – ~11 minute – ??? nodes

Broader Applications Broad applications in Humanoid Robotics – Generalized to many high DoF problems subject to various constraints Integrated Grasp Planning (Vahrenkamp, Do et al) [6] Full Body Motion (S Kagami, J Kuffner et al) [7]