Kinodynamic Planning

Kinodynamic planning explicitly models the differential constraints of an agent, which are mathematical equations governing its motion. These constraints define the relationship between an agent's state (e.g., position, velocity) and its control inputs (e.g., acceleration, steering angle).

• Kinematic Constraints: Govern the geometry of motion (e.g., a car's turning radius).
• Dynamic Constraints: Govern the forces and torques required for motion (e.g., maximum acceleration due to motor torque).

Unlike geometric planners that treat agents as points, kinodynamic planners must search in a state space that includes derivatives like velocity and acceleration.

A State Lattice is the fundamental data structure for discrete kinodynamic planning. It is a graph where each node represents a feasible kinematic state (e.g., [x, y, θ, v]) and edges represent motion primitives.

• Motion Primitives: Short, pre-computed trajectory segments that connect lattice states while exactly satisfying the agent's differential constraints. They are the 'building blocks' of a full path.
• This representation transforms the continuous planning problem into a graph search, allowing the use of algorithms like A* but in a high-dimensional state space that respects dynamics.

The search space for kinodynamic planning is significantly more complex than for geometric path finding. The state of an agent is defined by its pose and its derivatives.

• Example State Vector: For a differential-drive robot, this could be [x, y, heading, left_wheel_velocity, right_wheel_velocity].
• This high dimensionality leads to the curse of dimensionality, making exhaustive search intractable. Planners must use efficient sampling (e.g., RRT*) or heuristic-guided search within a state lattice to find solutions.

Solutions are not just sequences of positions, but sequences of control inputs (e.g., throttle, brake, steering command) over time. The planner must ensure that every commanded input is within the agent's physical capabilities.

• Actuator Limits: Commands must respect maximum torque, force, or velocity outputs.
• Stability Constraints: For vehicles, inputs must maintain dynamic stability (e.g., prevent rollover).
• The resulting trajectory is a time-parameterized path that the agent's low-level controller can execute directly, without requiring further smoothing or conversion.

In Heterogeneous Fleet Orchestration, kinodynamic planning is crucial for coordinating agents with different dynamic models.

• A forklift and an autonomous mobile robot (AMR) have vastly different acceleration profiles and turning radii. A kinodynamic planner generates agent-specific trajectories that account for these differences.
• This enables accurate spatio-temporal scheduling, ensuring that a slower, bulky agent's path does not create a bottleneck for faster agents, and that all reservations in a Conflict Avoidance Table (CAT) are dynamically feasible.

Kinodynamic planning solves a more constrained and realistic problem than pure geometric path finding.

Kinodynamic planning is essential for any physical system where inertia and control limits cannot be ignored.

Kinodynamic planning generates trajectories that respect a vehicle's non-holonomic constraints (e.g., a car cannot move sideways) and dynamic limits (e.g., maximum acceleration and tire friction).

• Plans must account for steering curvature, velocity profiles, and actuator saturation.
• Used for lane changes, merging, and navigating complex intersections where simple geometric paths are insufficient.
• Ensures generated plans are not just collision-free but also dynamically executable and comfortable for passengers.

For industrial and service robots, kinodynamic planning finds joint-space trajectories that move a payload from point A to point B while obeying torque limits, joint velocity caps, and energy constraints.

• Critical for avoiding actuator damage and ensuring smooth, vibration-free motion.
• Must consider the full dynamics model, including inertial forces and coupling between joints.
• Enables high-speed pick-and-place, assembly, and precise toolpath following where dynamics dominate performance.

Planning for walking, running, or climbing robots requires strict adherence to balance dynamics (e.g., Zero Moment Point), footstep placement forces, and contact sequencing.

• Kinodynamic planners generate center-of-mass trajectories and footstep plans that are dynamically stable.
• Must handle underactuation and hybrid dynamics (switching between flight and contact phases).
• Essential for robots like Boston Dynamics' Atlas to perform complex maneuvers without falling.

Quadrotors and fixed-wing UAVs have complex dynamics involving thrust, drag, pitch, and roll. Kinodynamic planning produces aggressive, acrobatic flight trajectories.

• Plans specify full state trajectories (position, velocity, orientation, angular rates) that are feasible given propeller thrust limits.
• Enables high-speed flight through narrow windows, dynamic obstacle avoidance, and precise payload delivery.
• Algorithms often use differential flatness to simplify the planning problem for these specific dynamics.

In the microgravity of space, spacecraft motion is governed by orbital mechanics and limited propellant. Kinodynamic planning finds fuel-optimal trajectories for docking or station-keeping.

• Must respect the non-linear dynamics of orbital motion (e.g., Clohessy-Wiltshire equations) and thruster impulse limits.
• Plans are propellant-constrained, making optimality crucial for mission longevity.
• Used for autonomous satellite servicing, International Space Station resupply, and constellation management.

Modern video games use kinodynamic planning to generate physically plausible character motion, such as parkour, climbing, or recovering from a shove.

• Planners work with a simplified dynamics model of the character's body to find trajectories that look natural and obey momentum.
• Enables procedural animation that adapts to unpredictable level geometry and player interactions.
• Balances physical realism with computational speed for real-time, 60 FPS performance.

A State Lattice is a discrete graph representation used in motion planning where nodes represent feasible kinematic states (e.g., position, velocity, orientation) and edges represent motion primitives—short, dynamically feasible trajectory segments that connect these states. It provides a searchable structure that inherently respects the agent's kinematic and dynamic constraints, forming a foundational discretization for many kinodynamic planners.

• Key Feature: Encodes continuous dynamics into a discrete graph for graph-search algorithms like A*.
• Example: For a differential-drive robot, motion primitives might include arcs of turning at specific curvatures while accelerating.

A Motion Primitive is a short, pre-computed trajectory segment that satisfies the system's differential constraints. Primitives are the building blocks for lattice-based planners, connecting discrete states in the state lattice. They are typically generated by applying a set of control inputs (e.g., acceleration, steering angle) over a fixed duration.

• Purpose: Enables search-based planning in high-dimensional state spaces by providing locally feasible actions.
• Generation: Often derived from solving the system's equations of motion or via numerical integration of control samples.

Differential Constraints are mathematical equations that define the relationship between a system's state variables and their derivatives (e.g., velocity is the derivative of position). They model the fundamental physics limiting how an agent can move, distinguishing kinodynamic planning from pure geometric pathfinding.

• Kinematic Constraints: Limit possible velocities and orientations (e.g., a car's non-holonomic constraint preventing lateral movement).
• Dynamic Constraints: Limit forces, torques, acceleration, and higher-order derivatives.
• Representation: Often expressed as ẋ = f(x, u), where x is the state and u is the control input.

Trajectory Optimization is a complementary approach to search-based kinodynamic planning. It formulates the problem as minimizing a cost function (e.g., energy, time) subject to differential constraints, collision avoidance, and boundary conditions. Methods like Direct Collocation or Model Predictive Control (MPC) solve this numerically.

• Relation to Kinodynamic Planning: Often used to refine a coarse path from a lattice search into a smooth, optimal trajectory.
• Characteristic: Operates in the continuous domain, providing high-quality solutions but requiring good initial guesses and dealing with local minima.

Nonholonomic Planning is a subset of kinodynamic planning focused on systems with nonholonomic constraints—differential constraints that are non-integrable, meaning they restrict the set of achievable velocities but not necessarily the set of reachable positions. This is typical for wheeled vehicles.

• Classic Example: A car cannot move sideways instantaneously; it must reorient its wheels.
• Planning Implication: The planner must reason about maneuvers (like multi-point turns) to reach goals, as straight-line geometric paths may be dynamically infeasible.

Sampling-Based Planners like RRT* (Rapidly-exploring Random Tree) and PRM (Probabilistic Roadmap) can be extended to handle kinodynamic constraints. These algorithms randomly sample points in the state space and attempt to connect them with locally feasible trajectories, often using a steering function.

• Kinodynamic RRT*: Grows a tree of dynamically feasible trajectories by randomly sampling a state, finding the nearest tree node, and extending toward it using a control input that respects dynamics.
• Advantage: Effective for high-dimensional state spaces where lattice discretization becomes infeasible.