State Lattice

Each node in a state lattice represents a feasible kinematic state of the agent, not just a spatial position. This typically includes:

• Pose: (x, y) position and orientation (θ).
• Kinematic State: Often includes velocity (v) and sometimes curvature (κ) or higher-order derivatives.
• Discretization: The continuous state space is sampled into a discrete, searchable set of nodes. For example, a 2D grid might be extended with 16 angular orientations and 3 velocity bins, creating a high-dimensional but finite graph.

Edges are pre-computed, short trajectory segments called motion primitives that connect state nodes while obeying the agent's dynamic constraints. Key characteristics:

• Feasibility: Each primitive is a solution to the system's equations of motion (e.g., Dubins curves for cars, acceleration-limited arcs for robots).
• Library-Based: A finite set of primitives is generated offline, enabling fast online graph search.
• Connectivity: Primitives define how the agent can transition from one discrete state (e.g., (x, y, θ, v)) to a neighboring state.

The interconnection of state nodes via motion primitives forms the lattice graph. Its properties are critical for planning:

• Regularity: The graph often has a regular structure in the state space, though it can be irregular.
• Reachability: The set of primitives determines the reachable set from any given state within a fixed time or distance.
• Search Space: Planning becomes a graph search problem (e.g., using A*) over this lattice to find a sequence of primitives from start to goal.

The primary advantage of a state lattice is that any path found through the graph is inherently dynamically feasible. This is because:

• Constraint Embedding: Kinematic and dynamic limits (max steering angle, acceleration) are baked into the motion primitive generation process.
• Smooth Trajectories: Connecting pre-computed primitives produces a continuous trajectory the agent can physically execute, unlike a path from a geometric planner which may require post-processing.

The state lattice is a discretization method for solving the continuous kinodynamic planning problem. It transforms the problem of finding a feasible trajectory in a continuous state space into a discrete graph search. This makes complex planning tractable for systems like:

• Autonomous Vehicles: Planning smooth, drivable paths on roads.
• Mobile Robots: Navigating warehouses with non-holonomic constraints.
• UAVs: Generating agile flight paths respecting dynamics.

For Multi-Agent Path Finding (MAPF), state lattices can be used in centralized planners. The joint state space is the Cartesian product of individual agent lattices. Searching this space (e.g., with MAA*) finds collision-free, dynamically feasible paths for the entire fleet. However, dimensionality grows exponentially, leading to techniques like subdimensional expansion that expand the joint space only when conflicts occur.

For bipedal and quadruped robots (e.g., Boston Dynamics Atlas, ANYmal), state lattices model the complex dynamics of walking and running. Nodes can represent the robot's center-of-mass state and footstep locations, while edges correspond to dynamically stable stepping motions or gait cycles. Planning in this space allows the robot to navigate uneven terrain, climb stairs, or recover from pushes by selecting sequences of pre-validated dynamic actions.

• Key Constraint: Dynamic balance and foot placement feasibility.
• Example: Planning a sequence of footsteps across a rubble field while maintaining stability.

Quadrotors and fixed-wing UAVs use state lattices for agile, high-speed flight in cluttered environments. The lattice incorporates the full dynamic model of the drone, including thrust and attitude. Edges are short, aggressive maneuver primitives like barrel rolls or rapid ascents. This allows for planning trajectories that are not only collision-free but also dynamically feasible and energy-efficient, enabling autonomous drone racing or inspection in tight spaces.

• Key Constraint: Actuator limits (max thrust, torque).
• Example: Planning a high-speed flight path through a window or a forest.

For robotic arms in manufacturing and logistics, state lattices plan smooth, jerk-limited motions for pick-and-place or assembly tasks. Nodes represent the arm's joint space or task space states (including velocity), and edges are short, executable movement segments. This is superior to simple geometric planning because it directly accounts for the arm's torque limits, avoiding commands that would cause stalling or vibration, leading to higher precision and longer hardware life.

• Key Constraint: Joint velocity and acceleration limits.
• Example: Planning a fast, vibration-free motion to insert a peg into a hole.

In orbital mechanics, state lattices are used for planning the final approach and docking maneuvers of spacecraft. Nodes represent orbital states (relative position, velocity, attitude), and edges are short thruster burn primitives that obey conservation of momentum and fuel constraints. This allows for the generation of safe, fuel-optimal trajectories for rendezvous in micro-gravity, where dynamics are highly non-linear and counter-intuitive.

• Key Constraint: Conservation of momentum, limited propellant.
• Example: Planning a series of thruster pulses to dock with the International Space Station.

In Heterogeneous Fleet Orchestration, a state lattice provides the underlying motion model for each agent type within a Multi-Agent Path Finding (MAPF) framework. The high-level MAPF solver (e.g., Conflict-Based Search) plans discrete assignments, while the state lattice ensures the low-level trajectories for each robot are kinematically feasible. This decoupling is essential for coordinating mixed fleets of differential-drive AMRs, forklifts, and legged robots in a shared warehouse, as each agent's lattice is uniquely tailored to its dynamics.

• Key Benefit: Separates combinatorial coordination from continuous feasibility.
• Example: A CBS planner resolves a vertex conflict, and each agent uses its own lattice to compute a dynamically valid path that respects the imposed constraint.

Multi-Agent Path Finding (MAPF) is the core computational problem of finding collision-free paths for multiple agents from their start locations to their goal locations in a shared environment. It is the overarching challenge that State Lattices and other planning graphs help solve.

• Centralized vs. Decoupled: Algorithms can plan in the full joint state space of all agents (centralized) or plan for agents individually while managing interactions (decoupled).
• Optimality Criteria: Solutions are typically evaluated on metrics like Makespan (total completion time) or Sum of Costs (total distance traveled).
• Applications: Directly applicable to warehouse robotics, autonomous vehicle fleets, and video game character navigation.

Kinodynamic Planning is the problem of finding a trajectory that is both geometrically collision-free and satisfies the kinematic (e.g., turning radius) and dynamic (e.g., acceleration limits) constraints of a physical agent. A State Lattice is explicitly designed for this domain.

• Beyond Geometry: Unlike simple grid-based planning, it accounts for real vehicle dynamics, ensuring planned motions are executable.
• Motion Primitives: The edges in a State Lattice are typically short, feasible trajectories (primitives) pre-computed to obey these constraints.
• Connection to State Lattice: The lattice's nodes represent feasible kinematic states (pose, velocity), and its edges are kinodynamically feasible motions between them.

Conflict-Based Search (CBS) is a leading optimal, two-level algorithm for solving MAPF problems. It resolves conflicts between agents by imposing constraints and replanning individual paths, which can be planned over structures like State Lattices.

• High-Level Constraint Tree: Searches for a consistent set of constraints (e.g., "Agent A cannot be at vertex V at time T").
• Low-Level Planners: Each node in the constraint tree uses a single-agent planner (like A* on a State Lattice) to find paths satisfying its constraints.
• Efficiency: CBS avoids searching the enormous joint state space directly, making optimal MAPF more tractable for dozens of agents.

The Velocity Obstacle (VO) is a geometric formalism for local, reactive collision avoidance. Optimal Reciprocal Collision Avoidance (ORCA) is a distributed algorithm based on VO principles, often used in conjunction with global lattice planners.

• Velocity Space: VO defines the set of velocities an agent can choose that would cause a future collision with another moving object.
• Reactive vs. Planned: VO/ORCA agents react instantaneously to nearby agents, unlike planners that pre-compute full paths. They are often used for last-mile deviation from a State Lattice plan.
• Reciprocity: ORCA assumes all agents share responsibility for avoidance, leading to smoother, more efficient maneuvers than purely reactive methods.

Space-Time A* and Safe Interval Path Planning (SIPP) are algorithms for planning in dynamic environments where obstacles move along known trajectories. They extend search to the time dimension, a concept complementary to State Lattices.

• Time as a Dimension: Space-Time A* searches a 3D grid (x, y, time), treating time as an explicit axis to avoid moving obstacles.
• Efficiency of SIPP: SIPP improves on this by grouping time into safe intervals where a location is continuously free, drastically reducing the search space.
• Integration: A State Lattice can be used as the underlying spatial graph, with SIPP or Space-Time A* performing the temporal search over it.

Makespan and Sum of Costs (SOC) are the two primary metrics for evaluating the quality of a MAPF solution. They provide quantitative measures for comparing algorithms and lattice designs.

• Makespan: The total time from the start of execution until the last agent reaches its goal. Minimizing makespan is critical for overall system throughput.
• Sum of Costs (SOC): Also called Flowtime, this is the sum of the path costs (e.g., travel time) for all individual agents. Minimizing SOC reduces total fleet energy consumption or wear.
• Trade-off: Optimizing for one metric often leads to a suboptimal result for the other. The choice depends on the operational priority of the fleet.