AA 241X: Introduction to Optimization and Mission Planning Marco Pavone Department of Aeronautics and Astronautics Stanford University May 1, 2013 Prof. M. Pavone AA 241X, Mission Planning 1 Focus of this lecture is on the deterministic setup (some stochastic problems can be reduced to this setup) Mission Planning Objective: Compute a dynamically feasible and collision-free trajectory that minimizes a given performance index Assumes a low-level control module capable of tracking trajectories • Two main setups: Deterministic • Stochastic • Prof. M. Pavone AA 241X, Mission Planning 2 Mission Planning Objective: Compute a dynamically feasible and collision-free trajectory that minimizes a given performance index Assumes a low-level control module capable of tracking trajectories • Two main setups: Deterministic • Stochastic • Focus of this lecture is on the deterministic setup (some stochastic problems can be reduced to this setup) Prof. M. Pavone AA 241X, Mission Planning 2 Focus of the lecture is on optimization approaches to deterministic, “discrete-time” mission planning Tools: Linear programming • Quadratic programming • Non-linear programming • Mixed integer linear programming (MILP) • Model predictive control (open-loop feedback control) • Dynamic programming (closed-loop control) • Mission Planning in Deterministic Environments Tools: Calculus of variations (geared toward continuous-time formulations) and mathematical optimization (geared toward “discrete-time” formulations) Prof. M. Pavone AA 241X, Mission Planning 3 Mission Planning in Deterministic Environments Tools: Calculus of variations (geared toward continuous-time formulations) and mathematical optimization (geared toward “discrete-time” formulations) Focus of the lecture is on optimization approaches to deterministic, “discrete-time” mission planning Tools: Linear programming • Quadratic programming • Non-linear programming • Mixed integer linear programming (MILP) • Model predictive control (open-loop feedback control) • Dynamic programming (closed-loop control) • Prof. M. Pavone AA 241X, Mission Planning 3 Linear programming Tools: Huge number of applications (e.g., production planning, pattern • classification, multi-commodity flow problems, path planning) Can be solved very efficiently on huge problem instances (millions of • variables and constraints) Popular solvers: CPLEX, MATLAB (linprog), GLPK • The problem: min cT x x n 2R subject to A x b ≥ T where x =(x1,...,xn) Prof. M. Pavone AA 241X, Mission Planning 4 Linear programming Important points: Feasible set is a polyhedron • The search for optimal solutions can be restricted to corner points • (if they exist...) Tools: X = linprog(f,A,b,Aeq,beq,LB,UB): • solves the problem min f’x subject to: A*x <= b CPLEX (we will see it later) • Two main solution methods: Simplex method: solves the problem by visiting extreme points, on • the boundary of the feasible set, each time improving the cost Interior point methods: find an optimal solution while moving in the • interior of the feasible set Interior point methods are polynomial-time and outperform the simplex method for large, sparse problems Prof. M. Pavone AA 241X, Mission Planning 5 Simplex method Figure: Geometric interpretation of simplex iterations. Image from MIT Open Courseware Prof. M. Pavone AA 241X, Mission Planning 6 Quadratic programming The problem: 1 min cT x + xT Qx x n 2 2R subject to A x = b x 0 ≥ where Q is an n n positive semidefinite matrix ⇥ Quadratic cost with linear constraints convex optimization • ! Used frequently in MPC • Solved efficiently with interior point methods • X = quadprog(H,f,A,b) • solves the problem: min 0.5*x’*H*x + f’*x subject to: A*x <= b Prof. M. Pavone AA 241X, Mission Planning 7 Convex programming Models large class of control/mission planning problems • Can be solved very efficiently (some of them even online!) • Important example: second-order cone programming1 • Tools Open-source software for convex optimization: SeDuMi:Primal-dual • interior-point method http://sedumi.ie.lehigh.edu/ Yalmip: User-friendly MATLAB interface • http://control.ee.ethz.ch/~joloef/wiki/pmwiki.php 1http://stanford.edu/~boyd/papers/pdf/socp.pdf Prof. M. Pavone AA 241X, Mission Planning 8 Mixed-integer linear programming (MILP) The problem: min cT x + d T y subject to Ax + By b x, y 0 ≥ x integer Same as the linear programming problem except that some of the • variables are restricted to take integer values Captures non-convexity and logic • Very powerful framework used in a variety of applications, e.g., task • assignment, mission planning Much more difficult problem than linear programming • Prof. M. Pavone AA 241X, Mission Planning 9 Modeling techniques with MILP: general Examples: Binary choice • Forcing constraints • Relations between variables • Disjunctive constraints • Restricted range of values • Arbitrary piecewise linear cost functions • Prof. M. Pavone AA 241X, Mission Planning 10 Modeling techniques with MILP: general Binary choice:Abinaryvariablex can be used to encode a choice between two alternatives: x 0, 1 j 2 { } Example: n items each with weight wj ; total allowable weight K. Formulation: n w x K j j j 1 X− x 0, 1 j =1,...,n j 2 { } Forcing constraints: Certain decisions are dependent. Example: decision A can be made only if decision B is made. Formulation: associate A (respectively B)withbinaryvariablexA (respectively xB ). Then pose the constraint: x y x, y 0, 1 2 { } Prof. M. Pavone AA 241X, Mission Planning 11 Modeling techniques with MILP: general Relations between variables: A constraint of the form n x 1 j Xj=1 x 0, 1 j =1,...,n j 2 { } i.e., at most one of the variables can be one Disjunctive constraints: Big-M method: assume we want either aT x b or cT x d. Formulation (M big): aT x b + My 1 cT x d + My 2 y + y 1, y , y 0, 1 1 2 1 2 2 { } Prof. M. Pavone AA 241X, Mission Planning 12 Modeling techniques with MILP: general Restricted range of values:Assumewewanttorestrictx to take values in a ,...,a . Formulation: { 1 m} m x = aj yj Xj=1 m yj =1 Xj=1 y 0, 1 j =1,...,n j 2 { } Prof. M. Pavone AA 241X, Mission Planning 13 Modeling techniques with MILP Arbitrary piecewise linear cost function:Assumewewanttorestrictx to take values in a ,...,a . Formulation: { 1 m} a1 a2 a3 a4 Prof. M. Pavone AA 241X, Mission Planning 14 Modeling techniques with MILP Modeling of minimization of piecewise linear cost function: k min λi f (ai ) Xi=k k subject to λi =1 Xi=1 λ y 1 1 λi yi 1 + yi i =2,...,k 1 − − λk yk 1 − k 1 − yi =1 Xi=1 λ 0 i ≥ y 0, 1 i 2 { } Prof. M. Pavone AA 241X, Mission Planning 15 Modeling techniques with MILP: mission planning Collision avoidance:wewanttoensure x x or x y or x x or y y l l ≥ u ≥ u Formulation: x x + My l 1 y y + My l 2 x x My ≥ u − 3 y y My ≥ u − 4 4 y 3 y 0, 1 i i 2 { } Xi=1 Prof. M. Pavone AA 241X, Mission Planning 16 Modeling techniques with MILP: mission planning Minimum time of arrival: Assume we want to arrive at target xf in minimum time. Formulation: x M(1 y ) x(k) x + M(1 y ) f − − k f − k N yk =1 Xk=1 y 0, 1 k 2 { } and the objective function would be N min kyk Xk=1 Prof. M. Pavone AA 241X, Mission Planning 17 Modeling techniques with MILP: mission planning Task assignment: Want to optimally assign n tasks to n UAVs. Formulation: n n min cij yij Xi=1 Xj=1 n subject to yij =1, j =1,...,n i=1 Xn yij =1, i =1,...,n Xj=1 y 0, 1 ij 2 { } Special case: it can be solved with the simplex method! Prof. M. Pavone AA 241X, Mission Planning 18 Modeling techniques with MILP: mission planning Speed limits:Define 2⇡k 2⇡k T i := cos , sin k n n ✓ ◆ ✓ ◆ maximum speed • v i v , k =1,...,n · k max minimum speed (harder) • v i v M(1 y ), k =1,...,n · k ≥ min − − k n y 1 k ≥ Xk=1 y 0, 1 k 2 { } Prof. M. Pavone AA 241X, Mission Planning 19 MILP modeling: summary This collection of examples is by no means an exhaustive list • Formulations are not unique, look for one with “few” variables and • constraints Big-M method very handy but usually complicates the optimization • process Modeling reference: Christodoulos A Floudas. Nonlinear and • Mixed-Integer Optimization: Fundamentals and Applications: Fundamentals and Applications. 1995. Prof. M. Pavone AA 241X, Mission Planning 20 MILP solution: Branch & Bound Divide and conquer approach to explore the set of feasible integer • solutions Instead of exploring the entire feasible set, it uses bounds on the • optimal cost to avoid exploring certain parts of the set Let F be the set of feasible solutions to the problem min cT x subject to x F 2 Key 1:ThesetF is partitioned into a collection of subsets F1, F2,...,Fk , and each of the following subproblems is solved separately: min cT x subject to x F 2 i Prof. M. Pavone AA 241X, Mission Planning 21 MILP solution: Branch & Bound F F1 F2 F3 F4 Figure: Tree of subproblems Key 2:Thereisafairlyefficient algorithm to compute a lower bound b(Fi ) to the optimal cost, i.e.,: T b(Fi ) min c x x Fi 2 Prof. M. Pavone AA 241X, Mission Planning 22 MILP solution: Branch & Bound Key 3:LetU be an upper bound on the optimal cost. If b(Fi ) U,then no need to consider this problem further ≥ Algorithm: 1 Select an active subproblem Fi 2 If infeasible delete it, otherwise compute b(Fi ) 3 If b(F ) U delete the subproblem i ≥ 4 If b(Fi ) < U either obtain an optimal solution or break the sub problem into further subproblems and add them to the list of active subproblems Main “free parameters”: Di↵erent ways of choosing the subproblems (e.g., breadth-first • versus depth-first) Di↵erent ways of obtaining b(F ) • i Several ways of breaking the problem • Prof.