Trajectory Optimization

Nonlinear Programming (NLP) is the mathematical foundation of trajectory optimization. It frames the problem as finding a sequence of states and controls that minimizes a scalar cost function (e.g., energy, time, jerk) subject to nonlinear constraints (e.g., system dynamics, collision avoidance, actuator limits).

• Core Formulation: min J(x,u) subject to g(x,u)=0, h(x,u)≤0, where x is state, u is control.
• Challenge: The non-convex nature of robotics problems (e.g., obstacle avoidance) means solvers find local minima, not necessarily the global best path.
• Real-World Use: This is the standard form ingested by solvers like IPOPT and SNOPT to compute trajectories for robotic arms and autonomous vehicles.

Trajectory optimization algorithms are broadly classified by how they handle the system's differential equations.

• Direct Methods (Transcription): Discretize the continuous-time problem first, transforming it into a large, sparse NLP. The dynamics become nonlinear equality constraints.
  • Shooting Methods: Optimize only control inputs; simulate dynamics forward (single shooting) or handle segments (multiple shooting).
  • Collocation Methods: Simultaneously optimize states and controls at discretized points, enforcing dynamics via polynomial interpolation (e.g., Hermite-Simpson). More stable for unstable systems.
• Indirect Methods: Apply the Pontryagin's Maximum Principle to derive first-order optimality conditions, resulting in a boundary value problem. While theoretically elegant, they are sensitive to initial guesses and harder to implement with complex constraints.

The cost function quantitatively defines what constitutes a "good" trajectory. It is a weighted sum of terms penalizing undesirable properties.

• Common Penalties:
  • Control Effort: Σ uᵀ R u minimizes energy consumption.
  • State Deviation: Σ (x - x_des)ᵀ Q (x - x_des) tracks a desired path.
  • Final State Error: (x_N - x_goal)ᵀ Q_f (x_N - x_goal) ensures goal achievement.
  • Smoothness (Jerk): Σ ‖d³x/dt³‖² produces human-like, comfortable motion for arms and vehicles.
• Engineering Trade-off: Tuning the weights (Q, R matrices) balances competing objectives like speed vs. energy vs. accuracy.

A feasible trajectory must satisfy hard limits imposed by physics and the environment.

• Dynamic Constraints: The core equality constraints defined by the system's equations of motion: ẋ = f(x, u). For a quadrotor, this includes Newton-Euler equations; for a manipulator, Lagrangian dynamics.
• Path Constraints: Inequality constraints that must hold along the entire trajectory:
  • Actuator Limits: u_min ≤ u(t) ≤ u_max (motor torque, steering angle).
  • State Bounds: x_min ≤ x(t) ≤ x_max (joint limits, velocity caps).
  • Collision Avoidance: distance(robot(t), obstacle) ≥ d_safe. Often formulated using Signed Distance Fields (SDFs).
• Handling Methods: Constraints are enforced at discretization points in direct methods, turning the continuous problem into a constrained NLP.

Sequential Quadratic Programming (SQP) is a leading iterative algorithm for solving the NLP at the heart of trajectory optimization. It approximates the complex problem with simpler, convex subproblems.

• Mechanism: At each iteration k, SQP constructs a Quadratic Programming (QP) subproblem that locally approximates the original NLP:
  1. Approximate the Lagrangian (cost + constraints) with a 2nd-order Taylor expansion.
  2. Linearize the constraints.
• Solution: The QP subproblem is solved efficiently to find a search direction (Δz). The full solution is updated (z_{k+1} = z_k + αΔz) with a step size α.
• Why it's Preferred: SQP exhibits superlinear convergence near the solution and handles large, sparse problems common in robotic trajectory optimization effectively.

Model Predictive Control (MPC) is a receding-horizon application of trajectory optimization for real-time feedback control. It solves a finite-horizon problem at each control step and executes only the first control input.

• The Loop:
  1. Measure the current system state.
  2. Solve a trajectory optimization over a short future horizon (e.g., 1 second), from the current state.
  3. Apply only the first control command from the optimized plan.
  4. Repeat at the next time step with new state measurements.
• Key Benefit: MPC automatically re-plans to compensate for model inaccuracies and unexpected disturbances.
• Computational Demand: Requires extremely fast NLP solvers (often exploiting problem structure) to run at control frequencies (10-1000 Hz).

Self-driving cars use trajectory optimization to compute smooth, collision-free paths in real-time. The algorithm must consider:

• Vehicle dynamics (e.g., the bicycle model) to ensure physically feasible steering and acceleration.
• Hard constraints like road boundaries, traffic laws, and other vehicles (modeled as moving obstacles).
• Soft objectives such as passenger comfort (minimizing jerk), travel time, and energy efficiency.

Frameworks like Model Predictive Control (MPC) are dominant here, solving a rolling-horizon optimization to react to dynamic environments. The output is a sequence of steering angles and throttle/brake commands.

Industrial robotic arms performing tasks like welding, painting, or precise assembly rely on trajectory optimization for speed and accuracy. Key considerations include:

• Minimizing jerk to reduce mechanical wear and vibration, producing smooth motion (often using minimum-jerk or minimum-snap trajectories).
• Avoiding singularities and joint limits by incorporating kinematic constraints directly into the optimization problem.
• Optimizing for cycle time to maximize throughput on a production line.

This is often formulated as a Nonlinear Programming (NLP) problem, where the states are joint angles and the controls are joint torques.

For walking, running, or hopping robots (e.g., humanoids, quadrupeds), trajectory optimization plans center-of-mass motions and footstep placements. This is a hybrid optimal control problem because:

• The system dynamics change discretely when a foot makes or breaks contact with the ground.
• The sequence and timing of these contact events (the gait) must also be optimized.
• Stability criteria like the Zero Moment Point (ZMP) or Capture Point are enforced as constraints.

Model Predictive Control (MPC) is used online to adjust the plan based on state estimation, while larger-scale nonlinear programming offline can discover novel gaits.

Multirotor drones use trajectory optimization for tasks ranging from smooth cinematography to racing through complex courses. The formulation accounts for:

• Underactuated dynamics: A drone's 4 rotors control 4 degrees of freedom (thrust and 3 torques), making it incapable of instantaneous arbitrary acceleration.
• Minimum-time problems for racing, where the drone must push its physical limits of thrust and attitude.
• Payload delivery with swing-free trajectories for suspended loads.

Solutions often parameterize the trajectory using polynomials or B-splines, and the optimization solves for the spline coefficients that satisfy dynamics and obstacle constraints.

Trajectory optimization is used as a predictive tool in biomechanics to understand human and animal movement, and to control advanced prosthetics or exoskeletons. Applications include:

• Predictive simulation: Solving an optimal control problem to predict how a human will walk, given physiological cost functions (e.g., minimizing metabolic cost).
• Prosthetic joint planning: Computing optimal knee/ankle trajectories for a robotic leg that mimic natural gait and improve user stability.
• Rehabilitation robotics: Generating assistive force profiles for exoskeletons that encourage proper movement patterns.

These problems use detailed musculoskeletal models with muscle activation dynamics, making them high-dimensional and computationally intensive.

Sequential Quadratic Programming (SQP) is a dominant iterative algorithm for solving Nonlinear Programming (NLP) problems, which are central to trajectory optimization. At each iteration, it approximates the original problem as a Quadratic Programming (QP) subproblem—with a quadratic objective and linear constraints—and solves it to find a step toward the optimum.

• Mechanism: Linearizes constraints and uses a quasi-Newton method (like BFGS) to approximate the Hessian of the Lagrangian.
• Why it's preferred: Efficiently handles the dense, structured constraints common in trajectory optimization problems.
• Implementation: Found in solvers like SNOPT and IPOPT, which are standard in robotics research and industry.

These are the two principal families of numerical approaches for solving trajectory optimization problems.

• Direct Methods (Transcription): Discretize the continuous-time problem first, transforming it into a large, sparse Nonlinear Programming (NLP) problem. The states and controls at discretization points become the optimization variables.

  • Examples: Direct single shooting, direct multiple shooting, direct collocation.
  • Advantage: More robust to initial guesses and easier to handle complex constraints.

• Indirect Methods: Apply the calculus of variations first to derive necessary optimality conditions (like the Pontryagin's Maximum Principle), resulting in a boundary value problem that is then solved.

  • Advantage: Can yield highly accurate, mathematically optimal solutions.
  • Disadvantage: Sensitive to initial guesses and harder to implement with constraints.

Collocation methods are a powerful class of direct transcription techniques for trajectory optimization. The state and control trajectories are approximated by polynomials (e.g., Lagrange polynomials) over finite elements. The dynamics constraints are enforced by requiring the derivative of the polynomial to equal the system dynamics at specific collocation points within each element.

• Direct Collocation: The most common variant, where both state and control discretization points are optimization variables, and dynamics are enforced as nonlinear constraints at collocation points.
• Benefits: Produces a sparse, large-scale NLP well-suited for modern solvers. Provides a dense output trajectory.
• Applications: The workhorse method for complex systems like humanoid robots, spacecraft, and acrobatic drones.