11/27/2006 Massachusetts Institute of Technology Motivation Would - PDF document

11/27/2006 Massachusetts Institute of Technology Motivation • Would like robotic manipulators to be able to Manipulator Path Planning with Obstacles operate in cluttered environments using Disjunctive Programming ACC06 Short Paper Lars Blackmore and Brian Williams November 27, 2006 MIT Cooperative JPL LEMUR: In-space Construction Testbed Inspection and Assembly 2 Problem Statement Current Approaches • Current manipulator path planning methods plan in configuration space 1. Convert feasible region from workspace to configuration space offline • Given an initial configuration, find the optimal path θ elbow that avoids obstacles and ensures that Configuration space manipulator endpoint ends at goal position Workspace LB1 0 y x θ shoulder 0 2. Solve planning problem using existing methods • Potential field methods (Not optimal or complete) • Probabilistic Roadmaps (Optimal, complete in probabilistic sense) 3 4 New Approach: Key idea Assumptions • Recent methods have posed path planning problem for • Manipulator in 2D or 3D with arbitrary number aircraft as constrained optimization of rotational joints • Key ideas: 1. Very simple models of plant are ‘sufficient’ • Plan in discrete time – Double integrator with constraints on velocity and acceleration – Assume low-level controller can achieve anything within these constraints – So the state at any future time is a linear function of control inputs 2. Obstacle avoidance can be posed as satisfaction of disjunctive • Low velocity, accelerations linear constraints � Solve efficiently using disjunctive linear programming – Dynamics can be ignored • Novel approach for manipulators : • Key challenge: highly nonlinear kinematics – Plan directly in workspace using constrained optimization approach • No pre-computation necessary • Optimal and complete 5 6 1

Slide 3 LB1 Note that this is not 'find joint angles to follow an endpoint trajectory' Lars Blackmore, 6/6/2006

11/27/2006 Workspace Planning Approach Kinematic Constraints • At any time step i, the desired joint positions must be • Instead of planning joint angles, design finite trajectory for each joint in workspace kinematically feasible k ( 1 ) ( 2 ) ( ) – Joint 1: , ,..., – Manipulator must be able to achieve the joint positions x x x 1 1 1 k ( 1 ) ( 2 ) ( ) – Joint 2 (endpoint): x , x ,..., x 2 2 2 • Joint 1 must be distance l 1 from joint 0 ( 4 ) ( 5 ) ( 3 ) x x x • Joint 2 must be distance l 2 from joint 1 ( 2 ) 2 2 x 2 2 ( 2 ) ( 1 ) ( 3 ) x x x ( 4 ) x ( 5 ) ( 1 ) 1 Joint 2 2 1 x x 1 1 Joint 2 1 l 2 l 1 Joint 1 Joint 0 Joint 1 • Solving for joint angles given all joint locations straightforward • Assume can achieve given joint angle using low level controller • These are quadratic constraints on the desired joint LB3 positions 7 8 Kinematic Constraints Joint Angle Constraints • Joint 1 must be distance l 1 from joint 0 • Not all joint angles are feasible: − α t T t ( ) ( ) = 2 l x x 1 1 1 θ + α 2 θ ( t ) ( t ) x 1 • Joint 2 must be distance l 2 from joint 1 x 2 1 ( ) ( ) T t − t t − t = ( ) ( ) ( ) ( ) l 2 • Many joint angle constraints can be expressed as x x x x 2 1 2 1 2 quadratic constraints also, for example: ( ) ( ) • These are quadratic equality constraints on the T t − t t − t ≥ α ( ) ( ) ( ) ( ) l l x x x x cos desired joint positions 2 1 2 1 1 2 9 10 Obstacle Constraints Dynamic Constraints • Disjunctive linear constraints on joint positions prevents • Use linear constraints to encode highly simplified collision of joints with obstacles dynamics T = a x b – Cartesian velocity constraints 1 1 ( t ) x 2 − ( t ) − ( t 1 ) x x j j ≤ v ( t ) T = ∆ x a x b t max 1 2 2 ∨ = T > t ( ) a x b i 1 ... N i j i • Encode goal constraint in terms of endpoint position ∀ j , t k = ( ) goal x 2 • Disjunctive linear constraints on intermediate points prevent collision of links with obstacles • Encode initial configuration constraint ( ) T ∨ = λ t + − λ t > ( ) ( ) a x ( 1 ) x b i N i j j i 1 ... 11 12 2

Slide 7 LB3 Highlight example is 2 joint but general works Lars Blackmore, 6/9/2006

11/27/2006 Optimality Summary • Path planning for manipulators with obstacles expressed as constrained optimization : • Minimum control effort can be expressed piecewise linearly Model Component Constraint type Limited dynamics Linear, inequality • Minimum time can be expressed piecewise linearly Obstacles Disjunctive linear, inequality Kinematics Quadratic, equality • Minimum energy can be expressed quadratically Joint limits Quadratic, inequality • Minimum deviation of joint position from centre of • Now show problem can be approximated as Disjunctive workspace can be expressed quadratically Linear Program – Globally optimal solution can be guaranteed 13 14 DLP Solution DLP Solution • Approximating quadratic constraints • Disjunctive Linear Programming encoding [ ] y x t T t = ( ) ( ) l 2 1 x x Model Component Constraint type 1 1 1 Circumscribing � Limited dynamics Linear, inequality polygon Obstacles Disjunctive linear, � l inequality 1 Kinematics Quadratic, equality ? [ ] x ? Joint limits Quadratic, inequality x 1 Inscribing polygon • Main challenge: quadratic constraints – Can be approximated using disjunctive linear constraints • Challenge: adding edges increases # disjunctions 15 16 Preliminary Results Preliminary Results 3 2.5 • 3-D intractable 2 – Approximate spherical constraints introduce very large number of disjunctive linear constraints 1.5 1 y(m) 0.5 • 2-D solution to global optimum tractable for 0 relatively small problems −0.5 – Length of horizon and # joints drive complexity −1 −1.5 • Will show typical results from 2-D −2 −2 −1 0 1 2 3 4 x(m) 17 18 3

11/27/2006 Preliminary Results Preliminary Results 3 3 2.5 2.5 2 2 1.5 1.5 1 1 y(m) y(m) 0.5 0.5 0 0 −0.5 −0.5 −1 −1 −1.5 −1.5 −2 −2 −2 −1 0 1 2 3 4 −2 −1 0 1 2 3 4 x(m) x(m) 19 20 Preliminary Results Preliminary Results 3 3 2.5 2.5 2 2 1.5 1.5 1 1 y(m) y(m) 0.5 0.5 0 0 −0.5 −0.5 −1 −1 −1.5 −1.5 −2 −2 −2 −1 0 1 2 3 4 −2 −1 0 1 2 3 4 x(m) x(m) 21 22 Preliminary Results Preliminary Results 3 3 2.5 2.5 2 2 1.5 1.5 1 1 y(m) y(m) 0.5 0.5 0 0 −0.5 −0.5 −1 −1 −1.5 −1.5 −2 −2 −2 −1 0 1 2 3 4 −2 −1 0 1 2 3 4 x(m) x(m) 23 24 4

11/27/2006 Preliminary Results Preliminary Results 3 3 2.5 2.5 2 2 1.5 1.5 1 1 y(m) y(m) 0.5 0.5 0 0 −0.5 −0.5 −1 −1 −1.5 −1.5 −2 −2 −2 −1 0 1 2 3 4 −2 −1 0 1 2 3 4 x(m) x(m) 25 26 Preliminary Results Preliminary Results 3 3 2.5 2.5 2 2 1.5 1.5 1 1 y(m) y(m) 0.5 0.5 0 0 −0.5 −0.5 −1 −1 −1.5 −1.5 −2 −2 −2 −1 0 1 2 3 4 −2 −1 0 1 2 3 4 x(m) x(m) 27 28 Preliminary Results Preliminary Results 3 3 2.5 2.5 2 2 1.5 1.5 1 1 y(m) y(m) 0.5 0.5 0 0 −0.5 −0.5 −1 −1 −1.5 −1.5 −2 −2 −2 −1 0 1 2 3 4 −2 −1 0 1 2 3 4 x(m) x(m) 29 30 5

11/27/2006 Preliminary Results Preliminary Results 3 3 2.5 2.5 2 2 1.5 1.5 1 1 y(m) y(m) 0.5 0.5 0 0 −0.5 −0.5 −1 −1 −1.5 −1.5 −2 −2 −2 −1 0 1 2 3 4 −2 −1 0 1 2 3 4 x(m) x(m) 31 32 Preliminary Results Preliminary Results 3 3 2.5 2.5 2 2 1.5 1.5 1 1 y(m) y(m) 0.5 0.5 0 0 −0.5 −0.5 −1 −1 −1.5 −1.5 −2 −2 −2 −1 0 1 2 3 4 −2 −1 0 1 2 3 4 x(m) x(m) 33 34 Preliminary Results Preliminary Results 3 3 2.5 2.5 2 2 1.5 1.5 1 1 y(m) y(m) 0.5 0.5 0 0 −0.5 −0.5 −1 −1 −1.5 −1.5 −2 −2 −2 −1 0 1 2 3 4 −2 −1 0 1 2 3 4 x(m) x(m) 35 36 6

11/27/2006 Preliminary Results Conclusion • Novel approach for manipulator path planning with obstacles 3 2.5 • Constrained optimization formulation appealing 2 – Guarantees of optimality, completeness 1.5 1 • In practice, solution intractable with existing approaches for y(m) 0.5 large problems 0 −0.5 • Advances in quadratic disjunctive programming may make this approach practically effective −1 −1.5 • Receding horizon formulation can reduce complexity −2 −2 −1 0 1 2 3 4 x(m) 37 38 Questions? 39 7