Trajectory Optimization

The cost function (or objective function) quantifies the quality of a trajectory. The optimizer's goal is to find the trajectory that minimizes this function. Common formulations include:

• Minimum Time: Minimize total traversal duration.
• Minimum Energy: Minimize integrated control effort or torque.
• Minimum Jerk/Snap: Penalize high derivatives of acceleration for smooth, human-like motion (critical for passenger comfort in autonomous vehicles or fluid robotic arm movements).
• Path Length: Minimize the total geometric distance traveled.

Engineers often use weighted sums to create multi-objective cost functions, e.g., J = w1 * time + w2 * energy.

System dynamics are the differential equations that govern how the robot's state changes over time in response to control inputs. They form the core equality constraint of the optimization: the computed trajectory must be physically feasible.

• For a simple wheeled robot, dynamics might model velocity and acceleration.
• For a quadrotor drone, dynamics involve complex equations relating rotor thrusts to orientation and translational acceleration.
• For a robotic arm, dynamics are derived using the Lagrangian or Newton-Euler formulations, accounting for masses, inertias, and Coriolis forces.

Ignoring dynamics yields a purely geometric path plan; incorporating them produces a dynamically feasible trajectory.

Constraints are hard limits the trajectory must satisfy. They are categorized as:

• Equality Constraints: Equations that must hold exactly. The primary example is the system dynamics: x_dot = f(x, u).
• Inequality Constraints: Limits that must not be violated.
  • Control Limits: Bounds on motor torque, voltage, or rotor speed (u_min <= u(t) <= u_max).
  • State Limits: Bounds on joint angles, velocities, or vehicle pitch/roll.
  • Path Constraints: Requirements that must hold along the entire trajectory, such as keeping a cup level.
• Boundary Conditions: Constraints on the start and end states, e.g., starting from rest and ending at a precise position and orientation.
• Collision Avoidance: Often formulated as inequality constraints requiring a minimum distance between the robot and obstacles at all times.

The optimization directly solves for two key time-series:

• State Variables (x(t)): Describe the complete configuration of the system at time t. For a robotic arm, this includes all joint angles and joint velocities. For a car, it includes position, orientation, velocity, and steering angle.
• Control Variables (u(t)): The inputs commanded to the system's actuators. For a robot arm, this is joint torque. For a car, it's throttle/brake and steering rate.

The optimizer searches the space of possible u(t) to produce a x(t) that minimizes cost while satisfying all constraints. The problem's dimensionality is the number of state and control variables multiplied by the number of discrete time steps.

Trajectory optimization problems are solved using two principal numerical approaches:

• Direct Methods (Transcription): The most common approach in robotics. They transcribe the continuous-time problem into a finite-dimensional Nonlinear Program (NLP) by discretizing both state and control trajectories. Popular variants include:
  • Direct Collocation: Discretizes states and controls, enforcing dynamics via constraint equations at collocation points.
  • Direct Shooting: Only discretizes controls; states are computed by forward simulation. Less stable but smaller problem size.
• Indirect Methods: Apply the calculus of variations and Pontryagin's Maximum Principle to derive necessary optimality conditions, resulting in a boundary value problem. They are highly accurate but sensitive to initial guesses and harder to implement with complex constraints.

Direct methods are favored for complex, constrained robotics problems due to their robustness and availability of powerful NLP solvers like IPOPT and SNOPT.

Trajectory optimization does not operate in isolation. It is typically the lower layer in a hierarchical planning stack:

1. Task Planning (High-Level): A symbolic planner (e.g., using PDDL) decides what to do: "Pick up the blue block from the table and place it on the shelf."
2. Motion Planning (Mid-Level): A geometric planner (e.g., RRT*) finds a collision-free path in configuration space.
3. Trajectory Optimization (Low-Level): Takes the geometric path as an initial guess and optimizes it for dynamics, smoothness, and time to produce an executable, time-parameterized trajectory.

This decomposition, often called Task and Motion Planning (TAMP), separates logical reasoning from continuous optimization, making complex problems tractable.

In industrial automation, trajectory optimization computes paths for robotic arms to perform tasks like pick-and-place, welding, and precision assembly. Key considerations include:

• Dynamic Constraints: Ensuring computed velocities and accelerations do not exceed motor torque limits.
• Collision Avoidance: Generating paths that avoid static fixtures, other robots, and the robot's own body (self-collision).
• Smooth Motion: Minimizing jerk to reduce wear on mechanical components and ensure product quality, especially in tasks like painting or dispensing adhesives.

For self-driving cars and drones, trajectory optimization plans smooth, passenger-comfortable, and legally compliant paths through dynamic environments.

• Lane Keeping & Merging: Optimizing for smooth lateral and longitudinal acceleration while adhering to traffic rules.
• Obstacle Interaction: Computing evasive maneuvers or safe stopping trajectories for pedestrians and other vehicles.
• Energy Efficiency: For electric vehicles, optimizing for regenerative braking and minimizing total energy consumption over a route. Frameworks like Model Predictive Control (MPC) are often used for this real-time, receding-horizon optimization.

For bipedal and quadrupedal robots, trajectory optimization is used to generate stable, dynamic walking and running gaits.

• Centroidal Dynamics: Optimizing the trajectory of the robot's center of mass and the timing of footstep placements (footstep planning) to maintain balance.
• Contact Force Optimization: Ensuring planned motions result in feasible contact forces that prevent slipping and comply with friction cone constraints.
• Whole-Body Control: The optimized centroidal trajectory is then used to solve for full-body joint motions via inverse kinematics and quadratic programming.

Trajectory optimization generates physically plausible motion for digital characters and analyzes human movement.

• Character Animation: Creating natural-looking motion by optimizing for principles like least action or matching keyframe poses while obeying physical dynamics.
• Sports Science: Analyzing athlete performance by optimizing a biomechanical model to achieve a goal (e.g., maximizing jump height or throw distance) and comparing the result to actual motion capture data.
• Prosthetics & Exoskeletons: Designing control strategies that minimize the user's metabolic cost or maximize stability.

Beyond single robot motion, trajectory optimization coordinates complex, time-sensitive processes.

• Coordinated Multi-Robot Systems: Optimizing the trajectories of multiple robots sharing a workspace to avoid collisions and minimize total cycle time for a task like car body welding.
• Machine Tool Pathing: For CNC mills and lathes, optimizing the tool path to minimize machining time, reduce tool wear, and avoid damaging the workpiece, subject to spindle speed and feed rate limits.
• Dynamic Scheduling: When combined with temporal planning, it can optimize the precise timing of operations in a flexible manufacturing cell.

The broader algorithmic field of computing a sequence of valid configurations for a robot to move from a start to a goal. Trajectory optimization is often the final step, adding time and dynamics to a geometric path. Core approaches include:

• Sampling-based planning (e.g., RRT, PRM) for exploring high-dimensional spaces.
• Graph search methods (e.g., A*) for discrete grid-based navigation.
• Geometric path planning, which finds a collision-free route without considering speed or acceleration.

A specific subset of motion planning concerned solely with finding a collision-free geometric route through the environment. It answers "where to go?" but not "how fast?" or "with what forces?". The output is a curve in configuration space. Trajectory optimization then takes this path and adds the time dimension, optimizing for smoothness, dynamics, and actuator limits to create an executable motion profile.

A real-time, receding-horizon control technique that repeatedly solves a trajectory optimization problem over a short time window. It uses a dynamic model to predict future states, computes optimal control inputs for that window, executes the first step, and then re-optimizes from the new state. This closed-loop approach makes it robust to disturbances and model inaccuracies, bridging the gap between planning and low-level control.

The geometric computation of the joint angles required to position a robot's end-effector at a desired pose (position and orientation). It is a fundamental prerequisite for many trajectory optimization problems. An optimizer might use IK solutions as candidate waypoints or incorporate IK constraints directly into its cost function to ensure the entire motion sequence is kinematically feasible for the manipulator's structure.

The process of creating a time-parameterized path. While sometimes used interchangeably with optimization, generation can be simpler, such as fitting a smooth polynomial (e.g., a quintic or cubic spline) between defined waypoints. Trajectory optimization is a more sophisticated form of generation that actively minimizes a cost function (energy, time, jerk) subject to hard constraints like torque limits and obstacle avoidance, rather than just interpolating points.

A foundational representation in robotics where every possible pose of a robot is mapped to a single point. The dimensionality equals the number of degrees of freedom. Obstacles in the physical world become forbidden regions in C-Space. Trajectory optimization typically operates within this abstract space, searching for a continuous curve from the start point to the goal point that remains within the free C-Space while satisfying dynamic constraints.