Block Condensing with qpdunes

Transcription

1 Block Condensing with qpdunes Dimitris Kouzoupis Rien Quirynen, Janick Frasch and Moritz Diehl Systems control and optimization laboratory (SYSCOP) TEMPO summer school August 5, 215 Dimitris Kouzoupis Block Condensing with qpdunes August 5, 215

2 1 QPs in nonlinear MPC Table of Contents 1. QPs in nonlinear MPC 2. Block Condensing 3. Results Dimitris Kouzoupis Block Condensing with qpdunes August 5, 215

3 1 QPs in nonlinear MPC QPs in nonlinear MPC Multistage QP from direct multiple shooting: with Z = [x u... x N ]. minimize Z subject to A ez = b i 1 2 Z H Z + h Z (1a) A iz b i (1b) (1c) Dimitris Kouzoupis Block Condensing with qpdunes August 5, / 16

4 1 QPs in nonlinear MPC QPs in nonlinear MPC Multistage QP from direct multiple shooting: with Z = [x u... x N ]. minimize Z subject to A ez = b i 1 2 Z H Z + h Z (1a) A iz b i (1b) (1c) Sparsity of Hessian matrix nz = 836 Dimitris Kouzoupis Block Condensing with qpdunes August 5, / 16

5 1 QPs in nonlinear MPC QPs in nonlinear MPC Multistage QP from direct multiple shooting: with Z = [x u... x N ]. minimize Z subject to A ez = b i 1 2 Z H Z + h Z (1a) A iz b i (1b) (1c) Sparsity of Hessian matrix Sparsity of Equality constraints nz = nz = 186 Dimitris Kouzoupis Block Condensing with qpdunes August 5, / 16

6 1 QPs in nonlinear MPC Condensed approach Eliminate equality constraints and solve smaller, dense QP: 1 > U Hc U + hc> U 2 subject to Ac U bc (2a) minimize U (2b) > > Solve for U = [u> u1>... un 1 ]. Condensed QP data are calculated online. Solution must be expanded for the next SQP iteration. Dimitris Kouzoupis Block Condensing with qpdunes August 5, / 16

7 1 QPs in nonlinear MPC Condensed approach Eliminate equality constraints and solve smaller, dense QP: 1 > U Hc U + hc> U 2 subject to Ac U bc (2a) minimize U (2b) > > Solve for U = [u> u1>... un 1 ]. Condensed QP data are calculated online. Solution must be expanded for the next SQP iteration. Good idea for: Short horizons Many states Few controls Dimitris Kouzoupis Block Condensing with qpdunes August 5, / 16

8 1 QPs in nonlinear MPC Sparse approach Alternative: Solve directly the sparse QP minimize Z subject to A ez = b i 1 2 Z HZ + h Z (3a) A iz b i (3b) (3c) Keep both states and controls as optimization variables. Use a QP solver with linear algebra that exploits sparsity. Software License Method FORCES Pro Commercial Primal-dual Interior Point, ADMM, FGM HPMPC LGPL Primal-Dual Interior Point qpdunes LGPL Non-smooth dual Newton method. 1 1 Frasch, Sager, and Diehl, A Parallel Quadratic Programming Method for Dynamic Optimization Problems. Dimitris Kouzoupis Block Condensing with qpdunes August 5, / 16

9 1 QPs in nonlinear MPC Sparse approach Alternative: Solve directly the sparse QP minimize Z subject to A ez = b i 1 2 Z HZ + h Z (3a) A iz b i (3b) (3c) Keep both states and controls as optimization variables. Use a QP solver with linear algebra that exploits sparsity. Software License Method FORCES Pro Commercial Primal-dual Interior Point, ADMM, FGM HPMPC LGPL Primal-Dual Interior Point qpdunes LGPL Non-smooth dual Newton method. 1 1 Frasch, Sager, and Diehl, A Parallel Quadratic Programming Method for Dynamic Optimization Problems. Dimitris Kouzoupis Block Condensing with qpdunes August 5, / 16

10 1 QPs in nonlinear MPC The dual Newton strategy Block-banded QP: minimize z N k= 1 2 z k H k z k + h k z k subject to E k+1 z k+1 = C k z k + c k,k =,..., N 1, d l k D k z k d u k, k =,..., N. Dimitris Kouzoupis Block Condensing with qpdunes August 5, / 16

11 1 QPs in nonlinear MPC The dual Newton strategy Block-banded QP: minimize z N k= 1 2 z k H k z k + h k z k subject to E k+1 z k+1 = C k z k + c k,k =,..., N 1, (λ k+1 ) d l k D k z k d u k, k =,..., N. Key idea: Dualize equality constraints and apply Newton method to solve dual problem. maximize λ minimize z N k= [ ] [ ] 1 2 z k H k z k + hk λk Ek z k + z λ k+1 C k + λ k+1c k k subject to: d l k D k z k d u k, k =,..., N. Dimitris Kouzoupis Block Condensing with qpdunes August 5, / 16

12 1 QPs in nonlinear MPC The dual Newton strategy Block-banded QP: minimize z N k= 1 2 z k H k z k + h k z k subject to E k+1 z k+1 = C k z k + c k,k =,..., N 1, (λ k+1 ) d l k D k z k d u k, k =,..., N. Key idea: Dualize equality constraints and apply Newton method to solve dual problem. maximize λ minimize z N k= Separable in primal variables [ ] [ ] 1 2 z k H k z k + hk λk Ek z k + z λ k+1 C k + λ k+1c k k subject to: d l k D k z k d u k, k =,..., N. Dimitris Kouzoupis Block Condensing with qpdunes August 5, / 16

13 1 QPs in nonlinear MPC The dual Newton strategy Minimization and summation can be interchanged: maximize λ N fk (λ) k= with λ R Nnx and stage QPs: fk 1 (λ) = min z k 2 z k H k z k + ( s.t. d l k D k z k d u k h k + [ ] [ ]) λk Ek z λ k+1 C k + λ k+1c k k Sub-problems can be solved in parallel by any QP solver. qpdunes supports two options: Clipping (for diagonal hessian and box constraints). qpoases (for the general case). Dimitris Kouzoupis Block Condensing with qpdunes August 5, / 16

14 1 QPs in nonlinear MPC The dual Newton strategy Minimization and summation can be interchanged: maximize λ N fk (λ) k= { Concave PW Quadratic Once cont. differentiable with λ R Nnx and stage QPs: fk 1 (λ) = min z k 2 z k H k z k + ( s.t. d l k D k z k d u k h k + [ ] [ ]) λk Ek z λ k+1 C k + λ k+1c k k Sub-problems can be solved in parallel by any QP solver. qpdunes supports two options: Clipping (for diagonal hessian and box constraints). qpoases (for the general case). (Unconstrained) Dual problem is solved using a non-smooth Newton method. Dimitris Kouzoupis Block Condensing with qpdunes August 5, / 16

15 1 QPs in nonlinear MPC Sparse vs condensed formulation Figure: Comparison taken from 2 2 Vukov et al., Auto-generated Algorithms for Nonlinear Model Predictive Control on Long and on Short Horizons. Dimitris Kouzoupis Block Condensing with qpdunes August 5, / 16

16 1 QPs in nonlinear MPC Sparse vs condensed formulation Can we combine the two approaches? Figure: Comparison taken from 2 2 Vukov et al., Auto-generated Algorithms for Nonlinear Model Predictive Control on Long and on Short Horizons. Dimitris Kouzoupis Block Condensing with qpdunes August 5, / 16

17 2 Block Condensing Table of Contents 1. QPs in nonlinear MPC 2. Block Condensing 3. Results Dimitris Kouzoupis Block Condensing with qpdunes August 5, 215

18 2 Block Condensing Block condensing Main idea: 3 Gather M successive stages: [ z k z k+1... z k+m 1 ] [ { x }}{{}}{ k u k x k+1 u k+1... { x }}{ k+m 1 u k+m 1 ] Eliminate all but the first state variable in each stage: Stage index n runs from to N/M. Last stage variable: z N/M = z N = x N. z n = [x k u k u k+1... u k+m 1 ] Sparse QPs with different levels of sparsity. 3 Axehill, Controlling the level of sparsity in MPC. Dimitris Kouzoupis Block Condensing with qpdunes August 5, / 16

19 2 Block Condensing Block condensing Sparsity of Hessian matrix Sparsity of Equality constraints nz = nz = 186 n x = 6, n u = 2, N = 2 Dimitris Kouzoupis Block Condensing with qpdunes August 5, / 16

20 2 Block Condensing Block condensing Sparsity of Hessian matrix Sparsity of Equality constraints nz = nz = 414 n x = 6, n u = 2, N = 2 Block size M = 5 Dimitris Kouzoupis Block Condensing with qpdunes August 5, / 16

21 2 Block Condensing Block condensing Sparsity of Hessian matrix Sparsity of Equality constraints nz = nz = 414 n x = 6, n u = 2, N = 2 Block size M = 5 Solve a smaller yet denser QP with the same sparse solver. Complexity still scales linearly with N (for fixed M). Cost of condensing quadratic in M. Dimitris Kouzoupis Block Condensing with qpdunes August 5, / 16

22 2 Block Condensing Contributions Block Condensing for Fast Nonlinear MPC with the Dual Newton Strategy D. Kouzoupis, R. Quirynen, J.V. Frasch and M. Diehl Proceedings of the 5th IFAC Conference on Nonlinear Model Predictive Control Investigate benefits of block condensing in NMPC. Efficient, code generated block condensing in ACADO. Support for both FORCES Pro and qpdunes. Benchmark example that illustrates speedups up to an order of magnitude. Dimitris Kouzoupis Block Condensing with qpdunes August 5, / 16

23 2 Block Condensing Benchmark example NMPC on a chain of masses (scalable). One point of the chain fixed, the other controlled. Reach the equilibrium position without hitting the wall. Code available online z [m] x [m] y [m] Dimitris Kouzoupis Block Condensing with qpdunes August 5, / 16

24 3 Results Table of Contents 1. QPs in nonlinear MPC 2. Block Condensing 3. Results Dimitris Kouzoupis Block Condensing with qpdunes August 5, 215

25 3 Results Benchmark example Effect of block condensing with qpdunes on worst-case RTI timings: Different block sizes, wide range of horizons. Performance improves up to M = 1. Total cpu time [ms] M = 1 M = 2 M = 5 M = 1 M = Horizon N Dimitris Kouzoupis Block Condensing with qpdunes August 5, / 16

26 3 Results Benchmark example Comparison of sparse QP solvers. Speedup in worst-case total cpu time. For block size M = 1: FORCES t max = ms qpdunes t max = ms Fewer Newton iterations. Speedup factor FORCES qpdunes Block size FORCES VS qpdunes (N=1) Dimitris Kouzoupis Block Condensing with qpdunes August 5, / 16

27 3 Results Benchmark example Increasing the block size: More expensive block condensing. Condensing of fewer blocks. Stage problems: Need longer preparation. Are more difficult to solve. Contain more information. Fewer Newton iterations. Computational load is moved from the Newton steps to the QP sub-problems. Number of iterations Computation time [ms] Average number of iterations Computation time [ms] Average time per iteration, per block (Average time per iteration, per block)x(average number of iterations)x(number of blocks) Block size Choice of optimal block size (N=2) Dimitris Kouzoupis Block Condensing with qpdunes August 5, / 16

28 3 Results Benchmark example Comparison for different number of masses. Each mass introduces 6 states. Benefit of block condensing increases n m = 3 n m = 4 n m = 5 Optimal block size changes. Effect on feedback times stronger as condensing is part of the preparation step. Speedup factor Block size Different number of masses (N=6) Dimitris Kouzoupis Block Condensing with qpdunes August 5, / 16

29 3 Results Try it yourself Condensing: mpc.set( QP_SOLVER, QP_QPOASES ); mpc.set( SPARSE_QP_SOLUTION, FULL_CONDENSING_N2 ); Sparse: mpc.set( QP_SOLVER, QP_QPDUNES ); % or QP_FORCES mpc.set( SPARSE_QP_SOLUTION, SPARSE_SOLVER ); Block condensing: mpc.set( QP_SOLVER, QP_QPDUNES ); % or QP_FORCES mpc.set( SPARSE_QP_SOLUTION, BLOCK_CONDENSING_N2 ); mpc.set( CONDENSING_BLOCK_SIZE, M); qpdunes-dev on github: Benchmark of the paper: Dimitris Kouzoupis Block Condensing with qpdunes August 5, / 16