IROS2019國際學(xué)術(shù)會(huì)議論文集 2157

qpSWIFT A Real time Sparse Quadratic Program Solver for Robotic Applications Abhishek Pandala1 Yanran Ding1 and Hae Won Park2 Abstract In this paper we present qpSWIFT a real time Quadratic Program QP solver Motivated by the need for a robust embedded QP solver in robotic applications qp SWIFT employs standard Primal Dual Interior Point Method PD IPM along with Mehrotra predictor corrector steps and Nesterov Todd scaling The sparse structure of the resulting KKT linear system in the QP formulation is exploited and sparse direct methods are utilized to solve the linear system of equations To further accelerate the factorization process we only modify the corresponding rows of the matrix factors that change during iterations and cache the nonzero Cholesky pattern qpSWIFT is library free written in ANSI C and its performance is benchmarked through standard problems that could be cast as QP Numerical results show that qpSWIFT outperforms state of the art solvers for small scale problems To evaluate the performance of the solver a real time implementa tion of the solver in the MPC framework through experiments on a quadrupedal robot are presented I INTRODUCTION In the robotics community there has been increasing use of optimization based techniques particularly ones based on Quadratic Programming QP The guarantees for fi nding an optimal solution in a tractable solve time and the fact that many robotic problems could be formulated as QPs has led to its widespread usage Many robot control frameworks for mulated as QPs can reason about system dynamics actuator limits and contact constraints while minimizing the task based objective function at hand 1 2 With these advan tages in mind several researchers have deployed Quadratic Programming for motion planning and control synthesis of autonomous systems For instance QP based Convex Model Predictive Control MPC 1 has been used to compute stabilizing control law for MIT Cheetah 3 3 Aaron et al 4 have used control barrier functions based QP for adaptive cruise control Feng et al 5 have developed inverse dynamics based full body controller and footstep planner for ATLAS humanoid both through QPs By adjusting the gait and approach velocities with a QP based MPC 6 MIT Manuscript received February 24 2019 Revised May 15 2019 Ac cepted June 17 2019 This paper was recommended for publication by Editor Dezhen Song upon evaluation of the Associate Editor and Reviewers comments This work was supported by NAVER LABS Corp under grant 087387 and Air Force Offi ce of Scientifi c Research under grant FA2386 17 1 4665 1Abhishek Pandala and Yanran Ding are with Department of Me chanicalScienceandEngineeringattheUniversityofIllinoisat Urbana Champaign IL 61801 USA pandala2 illinois edu yding35 illinois edu 2Hae Won Park is with the Department of Mechanical Engineering at the Korea Advanced Institute of Science and Technology Daejeon 34141 South Korea haewonpark kaist ac kr Digital Object Identifi er DOI see top of this page Cheetah 2 has shown the ability to jump over obstacles which are 80 of its leg length Scott et al 2 have developed custom active set based QP solver to implement whole body control law Exploiting sparsity and temporal structure of the optimal control law their solver achieved control frequencies of 1kHz Using simple Linear Inverted Pendulum Dynamics several researchers 5 7 8 have developed whole body control laws and footstep planners all of which rely on Quadratic Programs for solutions Further QPs arise as either approximation or as sub problems for complex optimization problems For example when planning motions or developing control strategies for legged robots it is necessary to ensure that the Ground Reaction Forces GRF abide by the friction cone These constraints are naturally encoded via the Quadratically Con strained Quadratic Programs QCQP or through second order cone programs SOCP 9 However approximations like linearized friction cone 10 enable reformulation in terms of a quadratic program For solving Inverse Kinematics and Inverse Dynamics problems several researchers have proposed cascaded approaches 11 12 9 for which QP arises as a subproblem For these hierarchical problems Escande et al 13 have developed an effi cient QP solver using a unifi ed active set strategy There exists many state of the art solvers to solve QP effi ciently These include the desktop solvers like GUROBI 14 MOSEK 15 CVXOPT 16 and embedded solvers like qpOASES 17 ECOS 18 and OSQP 19 In this work we mainly focus on developing embedded solvers for robotic applications A Related Work in Embedded Optimization Embedded applications typically involve solving the same problem instance repeatedly over different sets of data with a hard real time constraint To do this the solver must effi ciently take advantage of the problem structure At the same time it should be of low footprint library free and light weight In addition the choice of algorithm used for solving the Quadratic Programming problem has a signifi cant impact on execution rates Interior Point Methods Active set methods and fi rst order methods are among the popular algorithm choices Active set method and fi rst order methods can be easily warm started leading to faster execution rates Nevertheless the worst time complexity of Active set meth ods grows exponentially with the number of inequality con straints We also notice during numerical experiments Sec tion V C that solvers with active set methods qpOASES IEEE Robotics and Automation Letters RAL paper presented at the 2019 IEEE RSJ International Conference on Intelligent Robots and Systems IROS Macau China November 4 8 2019 Copyright 2019 IEEE tend to make very large number of iterations without warm start and in certain situations see Section IV in 2 Interior Point Methods on the other hand iteratively solve an unconstrained minimization problem with varying barrier function penalty function on the set of constraints param eters Interior Point Methods are also known to converge in a fi xed number of iterations almost independent of problem size conditioning and data type 20 As a result the time needed to solve the optimization problem can be accurately estimated beforehand a feature which enhances the reliabil ity for a solver used in embedded platforms Existing interior point embedded solver ECOS is designed for second order cone programming and is not well suited for QP since re formulating a QP into SOCP results in a QP formulation which is not very effi cient computationally 21 For better performance on embedded platforms re searchers have also investigated code generation techniques CVXGEN 22 is a well known small scale code generation solver for convex optimization problems By eliminating loops function calls branches and the associated overhead CVXGEN performs orders of magnitudes better than general purpose solvers for small scale problems Among other existing C code generators are AO MPC 23 and FiOrdOs 24 with fi rst order optimization techniques FORCES 25 26 is a second order optimization framework based on Primal Dual Interior Point Method capable of solving the Quadratically Constrained Quadratic Program and Linear Programs with multistage problems The major disadvantage of code generation solvers is that code size grows quickly with the problem dimension making them intractable for embedded applications On the other hand HPMPC 27 combines the code generation process with optimized BLAS libraries to achieve better performance Employing register blocking vectorization the optimized BLAS library BLAS FEO 28 achieves high performance for small to medium scale matrices However these dense linear algebra routines do not exploit sparsity B Contribution The necessity for an effi cient QP solver stems from our need to quickly compute control solutions for a quadrupedal robot in the MPC framework 29 In this work we present a Quadratic Programming solver targeted for embedded appli cations We employ the Primal Dual Interior Point Method with Mehrotra predictor corrector steps along with Nesterov Todd scaling Each iteration of the Primal Dual Interior Point Method requires the solution of the Karush Kuhn Tucker KKT linear system We notice that in most applications the formulated QP results in an extremely sparse KKT linear system Therefore instead of using factorization methods based on dense matrices we resort to sparse methods particularly sparse Cholesky factorization 30 for solving the KKT linear system To reduce fi ll in introduction of new nonzero s in the matrix factorizations we precompute the permutation matrix offl ine using approximate minimum degree heuristic 31 Dynamic regularization is used to factorize symmetric positive semi defi nite matrices and avoid fatal errors like division by zero We notice that during iterations the structure of KKT linear system remains unchanged while only elements in the lower diagonal change We exploit these two facts and propose two novel modifi cations to the matrix factorization process 30 First instead of performing a graph traversal to determine the nonzero row pattern of Cholesy factor we compute the transpose of Cholesky factor LT once and reuse it to obtain nonzero row pattern for subsequent iterations Section IV B Second instead of computing the complete factorization of KKT linear system we only fac torize the rows that change during iterations Section IV C With these modifi cations we are able to exploit the problem structure effi ciently and minimize redundant computations The paper is organized as follows Section II introduces the QP formulation Section III outlines the Primal Dual Interior Point algorithm Section IV describes symmetric positive semi defi nite sparse KKT factorization Section V bench marks the proposed solver with various problem instances on a desktop platform Section VI shows results of embedded robotic applications with trotting and bounding experiment on a quadruped robot Section VII provides the concluding remarks II QP FORMULATION qpSWIFT solves Quadratic Programs with linear con straints in the following form 32 minimize 1 2x TPx cTx 1a subject to A x b 1b G x s h 1c s 0 1d where x Rnare the primal variables s Rmdenote the slack variables P PT 0 Rn n c Rn A Rp n b Rp G Rm n h Rmare the problem data It is assumed that A has full row rank The Karush Kuhn Tucker KKT optimality conditions are necessary and suffi cient for optimality of convex problems The KKT conditions for the QP 1 are Px c ATy GTz 0 2a Ax b 0 2b Gx h s 0 2c s z 0 2d si zi 0 i 1 2 m 2e where 0 is a vector of zeros y Rpand z Rm are dual variables The existence of a primal dual pair of variables x y z s that satisfi es the conditions in 2 corresponds to the optimal solution of the QP problem 1 III INTERIOR POINTMETHOD Interior Point Methods IPM are a widespread method for solving convex optimization problems that include inequal ity constraints IPMs formulate the inequality constrained problem as equality constrained problem by incorporating a barrier function in the objective Then the unconstrained problem is iteratively solved with varying barrier function parameters using Newton s method until convergence to the optimal point The algorithm presented in this paper is based on coneqp in CVXOPT 33 for positive orthant cone Here we present the central ideas of the algorithm A Primal Dual IPM and Central Path In this work the following log barrier function is em ployed to replace the inequality constraints in the optimiza tion problem 1 t m X i 1 log t 0 3 where is the barrier function parameter and t is an arbitrary vector The corresponding KKT optimality conditions for this parameterized optimization problem also called the central path equations are given by Px c ATy GTz 0 4a Ax b 0 4b Gx h s 0 4c s z 0 4d si zi i 1 2 m 4e For each 0 the above system has a unique solution called the center The set of all centers forms a ho motopy path called the central path In the limit 0 the central path equations are equivalent to KKT optimality conditions By varying in each iteration the central path equations are solved by taking Newtons step This generates a sequence X k 1 X k k X k k 0 1 2 5 thatlooselytracksthecentralpath whereX k xk yk zk sk kis the step length found by line search X k is the search direction B Nesterov Todd NT Scaling Nesterov and Todd have shown that the self scaled prop erty of the barrier function 34 enables the construction of symmetric Primal Dual scalings Exploiting this scaling Interior Point Methods tend to make longer steps in the search direction Notably the Primal Dual scaling matrix W is a linear transformation given as s W Ts z Wz 6 that leaves the central path 4 and the corresponding positive orthant cone invariant s z 1 s z 1 s 0 s 0 z 0 z 0 7 where 1 is a column vector of ones and the operator is defi ned as u v u1v1 unvn u v Rn 8 Following 33 35 we adopt the following Nesterov Todd NT scaling given by the symmetric factorization of the Hessian of the barrier function This factorization is done at the unique scaling point 0 that satisfi es 2 z s 9 where u v denotes the inverse of u v and the NT scaling is obtained by the factorization 2 1 WTW Here we defi ne the scaling vector as W T s W z 10 The central path equations after NT scaling become 0 0 s PATGT A00 G00 x y z c b h s z 0 Wz W Ts k 1 k 0 11 C Initialization The initial condition x0 y0 z0 s0 for the Primal Dual IPM is obtained by solving a closely related optimization problem of the following form minimize 1 2x TPx cTx 1 2 ksk2 2 12a subject to A x b 12b G x s h 12c The above problem 12 is an equality constrained QP and the solution is obtained by solving the following linear system of equations PATGT A00 G0 I x0 y0 z c b h 13 where I is an identity matrix s0is computed as follows s0 z p 0 z 1 p 1 otherwise 14 where p inf z 1 0 z0is calculated as z0 z d 0 2 0 are the tolerances IV KKT FACTORIZATION The most computationally expensive part of the Interior Point algorithm is solving the KKT linear system 20 In this work we resort to sparse LDLT 30 factorization for solving the linear system of equations K LDLT 22 where L D are lower triangular and diagonal matrices respectively The advantages of using sparse matrix factor ization techniques are that the resulting code is concise and the number of fl oating point operations only depends on the number of nonzeros in the matrix K Compressed Column Storage CCS format was employed to represent sparse matrices Direct factorization of the KKT matrix leads to fi ll in in the factors L and D A fi ll in is a nonzero entry that appears in L but not in K To reduce fi ll in a permutation matrix M obtained from approximate minimum degree heuristic 31 is utilized and the new system to solve is MKMT LDLT 23 Sparse LDLT factorization involves two steps In the fi rst step referred to as symbolic factorization given a symmetric matrix K a useful data structure called the elimination tree is determined For example Fig 1 a b and c shows a sample matrix K its elimination tree and the corresponding Cholesky factor L respectively This symbolic factorization step requires only the sparsity pattern of the matrix K and is done once for all the iterations The elimination tree 37 is constructed iteratively from the sparsity pattern of K and it guides the second step in factorization process The second step referred to as the numerical factorization step the values of nonzero elements in L and D are determined For an overview of direct methods for sparse linear systems the readers are referred to 38 39 A Dynamic Regularization Since the KKT matrix in general is positive semi defi nite the matrix factorization is not guaranteed to exist To re solve this issue we perform dynamic regularization 22 by performing perturbations to the diagonal entries Whenever the entries of the diagonal matrix become close to zero we perturb them as follows If Dii Dii sgn Dii 24 where sgn x is the signum function and are chosen to be 10 14and 10 7respectively With this regularization the algorithm can factorize symmetric positive semi defi nite matrices and avoid fatal errors like division by zero Since these tolerances are very tight the solution obtained by dynamic regularization for the linear system of equations is close to the original solution Furthermore the termination criterion in 21 ensures that we obtain the optimal solution to the QP a Matrix K 1 2 3 4 5 6 7 8 9 10 11 c Cholesky Factor L 1 2 3 4 5 6 7 8 9 10 11 d Modified Cholesky Factor 1 3 5 6 7 8 9 2 4 10 11 Constant Elements Changed Elements b Elimination Tree 1 2 3 4 56 7 8 9 10 11 Path from 2 to the root Fig 1 a An example KKT matrix K with nonzero entries represented by blue dots b The elimination tree corresponding to K where the path from node 2 to the root of the tree is highlighted in red c The Cholesky Factor L of K d The modifi ed L with red dots representing entries that change through iterations B Caching Transpose of the Cholesky factor We use Sparse Cholesky package 30 that implements an up looking Cholesky factorization This method computes each row of L one at a time starting from the fi rst row to the last During the numerical factorization phase the nonzero pattern of each row of L is determined using the graph traversal in the elimination tree followed by sparse triangular solves The fact that the nonzero row pattern of L does not change during iterations could be exploited to accelerate the numerical factorization step Instead of performing graph traversals in elimination tree for each iteration we can cache the nonzero pattern of the transpose of Cholesky factor and reuse it for all the iterations C Modifying Rows that change During every iteration of the Interior Point algorithm the KKT Matrix changes only in the lower diagonal cor responding to WTW 20 Consequently some rows of L and D remain unchanged during the iterations To reduce the computational effort we can only compute matrix factorizations for rows of