Sequential Quadratic Programming (SQP)

At its core, SQP solves a sequence of Quadratic Programming (QP) subproblems. Each iteration approximates the original nonlinear problem by:

• Linearizing the constraints around the current iterate.
• Creating a quadratic approximation of the Lagrangian function (using the Hessian or an approximation thereof) for the objective.
• Solving this QP subproblem to find a search direction, then taking a step to update the solution estimate. This transforms a complex, non-convex NLP into a series of simpler, convex QPs.

Unlike penalty or barrier methods that move constraints into the objective, SQP treats nonlinear equality and inequality constraints explicitly within each QP subproblem. This direct handling is critical for robotics, where constraints are fundamental:

• Dynamic constraints (e.g., equations of motion from Lagrangian dynamics).
• Collision avoidance constraints (e.g., using Signed Distance Fields).
• Actuator torque and velocity limits. The linearized constraints in each QP ensure the solution progresses toward feasibility and optimality simultaneously.

When initialized near a solution, SQP exhibits superlinear or quadratic convergence rates, making it exceptionally fast for local refinement. This performance stems from its use of second-order information (the Hessian of the Lagrangian). This is vital for Model Predictive Control (MPC) applications, where an optimal trajectory must be re-computed in real-time (often < 1 sec) as the robot's state and environment change.

SQP's performance is heavily dependent on precise first and second-order derivative information. This necessitates:

• Analytical gradients and Jacobians of the objective and constraints, often derived via symbolic tools or automatic differentiation.
• An approximation of the Hessian of the Lagrangian (e.g., using BFGS quasi-Newton updates). Inaccurate derivatives can lead to poor QP approximations, causing the algorithm to fail or converge slowly. This makes SQP more complex to implement than derivative-free or first-order methods.

The solution to each QP subproblem satisfies the Karush–Kuhn–Tucker (KKT) conditions for the approximated problem. As the sequence converges, these solutions satisfy the KKT conditions for the original NLP. Therefore, SQP can be viewed as Newton's method applied directly to the KKT optimality conditions. This theoretical foundation guarantees that a converged solution is a local optimum (satisfying necessary conditions) of the constrained trajectory optimization problem.

Real-world SQP implementations use sophisticated techniques to ensure robustness:

• Merit Functions & Line Searches: A merit function (like an L1 penalty) is used to ensure each iteration provides sufficient progress toward both optimality and feasibility before accepting the step.
• Active-Set vs. Interior-Point QP Solvers: The QP subproblem can be solved using active-set methods (efficient for warm-starting) or interior-point methods (better for large, dense problems).
• Dense vs. Sparse Formulations: For long-horizon trajectory problems with thousands of variables, exploiting the block-tridiagonal sparsity structure of the Hessian and Jacobians is essential for computational tractability.

Nonlinear Programming (NLP) is the class of optimization problems that SQP is designed to solve. An NLP involves minimizing or maximizing a scalar objective function subject to a set of equality and inequality constraints, where at least one of these functions is nonlinear.

• General Form: Minimize ( f(x) ) subject to ( g_i(x) = 0 ) and ( h_j(x) \leq 0 ).
• Core Challenge: The nonlinearity makes finding a global optimum computationally intractable for most problems; solvers like SQP aim for efficient convergence to a local optimum.
• Robotics Applications: Trajectory optimization, inverse kinematics, and parameter estimation often naturally formulate as NLPs due to complex dynamics and collision-avoidance constraints.

The Karush–Kuhn–Tucker (KKT) conditions are first-order necessary conditions for a solution to a nonlinear programming problem to be optimal. SQP can be viewed as a Newton-type method applied directly to the KKT system of the original NLP.

• Components: The conditions combine stationarity of the Lagrangian, primal feasibility, dual feasibility, and complementary slackness.
• SQP Connection: Each SQP iteration constructs a Quadratic Programming (QP) subproblem whose solution satisfies the KKT conditions for a local quadratic model of the original problem. Solving this sequence of QPs drives the full solution toward satisfying the KKT conditions for the original NLP.
• Significance: They provide the fundamental optimality criteria that SQP and other solvers are designed to satisfy.

Quadratic Programming (QP) is the optimization of a quadratic objective function subject to linear constraints. It is the central subproblem solved at every iteration of the SQP algorithm.

• SQP Subproblem: SQP approximates the original NLP by constructing a QP where the objective is a local quadratic model (using the Hessian of the Lagrangian) and constraints are linearized approximations.
• Efficient Solvability: Unlike general NLPs, convex QPs can be solved to global optimality efficiently and reliably using active-set or interior-point methods. This makes them ideal building blocks for an iterative algorithm.
• Example: Minimize ( \frac{1}{2} x^T H x + c^T x ) subject to ( A_{eq} x = b_{eq} ) and ( A_{ineq} x \leq b_{ineq} ).

Model Predictive Control (MPC) is a real-time, receding-horizon control strategy that repeatedly solves a finite-horizon optimal control problem. SQP is a leading algorithm for solving the nonlinear optimization problem at the core of Nonlinear MPC (NMPC).

• Online Optimization: At each control time step, MPC solves a trajectory optimization problem to determine optimal control inputs, implements the first step, and then re-solves at the next state.
• SQP's Role: The need for fast, reliable online solutions makes efficient solvers like real-time SQP variants (e.g., acados, FORCES Pro) critical for NMPC deployment on robots and autonomous vehicles.
• Use Case: A self-driving car uses NMPC with an SQP solver to continuously replan a smooth, collision-free trajectory while obeying dynamic constraints.

Interior-Point Methods (IPMs) are an alternative class of algorithms for solving constrained optimization problems, including NLPs. While SQP is an active-set method that works on the boundary of constraints, IPMs approach the optimum from the interior of the feasible region.

• Mechanism: IPMs replace inequality constraints with a logarithmic barrier term in the objective and solve a sequence of unconstrained or equality-constrained subproblems.
• Comparison with SQP: IPMs often demonstrate more robust convergence for large-scale problems with many inequality constraints, while SQP can be more efficient for problems where the optimal active set is identified quickly. Modern solvers like IPOPT often use a primal-dual interior-point method.
• Application: Frequently used in large-scale trajectory optimization and economic dispatch problems.

Trajectory Optimization is the direct application domain in robotics where SQP is most prominently used. It is the process of computing a state and control trajectory that minimizes a cost (e.g., energy, time) while satisfying system dynamics and constraints.

• Mathematical Form: Typically discretized into a large, sparse NLP. Direct transcription methods convert the continuous-time problem directly into an NLP that SQP can solve.
• SQP Advantages: Efficiently handles the nonlinear dynamics and non-convex constraints (like obstacle avoidance) inherent to robotics. Its iterative QP structure allows for warm-starting, which is crucial for MPC.
• Tools: Frameworks like CasADi with IPOPT (an interior-point method) or custom SQP solvers are industry standards for prototyping and deploying trajectory optimization solutions.