Kinodynamic Planning

Unlike purely kinematic planning in Configuration Space (C-Space), kinodynamic planning operates in the state space, which includes velocity dimensions. A state vector for a quadrotor, for example, might be [x, y, z, roll, pitch, yaw, vx, vy, vz, ωx, ωy, ωz]. The planner must find a trajectory through this higher-dimensional space that satisfies both geometric constraints and the system's equations of motion.

The core distinction is the enforcement of nonholonomic constraints and dynamic constraints:

• Nonholonomic: Velocity-level restrictions that cannot be integrated into position constraints (e.g., a car cannot move sideways).
• Dynamic: Force and torque limits imposed by actuator saturation and mass properties. These constraints are typically expressed as ẋ = f(x, u), where x is the state, u is the control input, and f is the system dynamics model.

Standard Rapidly-exploring Random Trees (RRT) can be extended to kinodynamic domains by replacing the straight-line connection heuristic with a forward propagation of the system dynamics. When sampling a new state, the algorithm applies a control input u for a duration Δt and integrates the equations of motion to generate a dynamically feasible edge. This ensures every branch in the tree is a physically valid motion primitive.

An alternative to sampling-based methods, lattice planners discretize the state space into a regular grid and precompute a set of motion primitives—short, kinodynamically feasible path segments generated by applying constant control inputs. Online planning reduces to a graph search (e.g., A*) over this lattice. This approach provides deterministic guarantees on completeness and is widely used in autonomous vehicle navigation.

Kinodynamic planning often serves as a front-end to Trajectory Optimization. A sampling-based planner finds a coarse, feasible solution that is then refined by a numerical optimizer like Direct Collocation or Differential Dynamic Programming (DDP). The optimizer minimizes a cost function (e.g., control effort, time) while strictly enforcing the system dynamics as equality constraints, producing a smooth, locally optimal trajectory.

For reactive execution in dynamic environments, kinodynamic planning is embedded within a Model Predictive Control (MPC) loop. At each control cycle, the MPC solver computes a short-horizon kinodynamic plan from the current estimated state, applies the first control input, and then replans. This closed-loop approach handles disturbances and moving obstacles that an open-loop plan cannot anticipate.

At highway speeds, a vehicle cannot be treated as a simple geometric shape. Kinodynamic planners incorporate velocity, acceleration, and steering limits to generate trajectories that respect tire friction circles and engine torque curves.

• Emergency lane changes: Plans a path that stays within the vehicle's lateral acceleration limits to prevent rollover
• Merging maneuvers: Simultaneously solves for a path and velocity profile that matches traffic flow
• Friction constraints: Explicitly models the Kamm circle to prevent understeer or oversteer

Drones performing aerobatic maneuvers or navigating cluttered forests must plan trajectories that respect thrust-to-weight ratios, motor saturation limits, and aerodynamic effects. A purely geometric path would be physically impossible to track.

• Perching maneuvers: Plans a trajectory that bleeds kinetic energy to land precisely on a vertical surface
• Minimum-snap trajectories: Optimizes the fourth derivative of position to generate smooth motor commands
• Differential flatness: Exploits the quadrotor's mathematical property to plan in a lower-dimensional output space

Industrial manipulators moving heavy payloads cannot change direction instantaneously. Kinodynamic planning respects joint velocity limits, motor torque saturation, and actuator jerk constraints to prevent premature wear and vibration.

• Time-optimal trajectories: Finds the fastest path that stays within each joint's torque-speed curve
• Payload-aware planning: Adjusts acceleration limits based on the mass and inertia of the grasped object
• Vibration suppression: Minimizes jerk to avoid exciting structural resonances in the arm

Satellites and space robots operate under reaction wheel saturation limits, thruster fuel budgets, and conservation of angular momentum. Kinodynamic planning ensures maneuvers are feasible within these strict physical constraints.

• Zero-g free-flyers: Plans trajectories that respect momentum conservation while capturing tumbling objects
• Minimum-fuel slews: Finds attitude trajectories that minimize propellant consumption
• Singularity avoidance: Steers reaction wheels away from configurations where they lose control authority

Bipedal robots must plan footsteps and whole-body trajectories that satisfy zero-moment point (ZMP) constraints, joint torque limits, and foot friction cones. A kinodynamic planner simultaneously reasons about where to step and how to shift momentum.

• Push recovery: Generates a sequence of steps that brings the robot back to a stable stance without falling
• Stair climbing: Plans center-of-mass trajectories that keep the ZMP within the support polygon on each step
• Whole-body coordination: Coordinates arm swing with leg motion to counteract angular momentum

Minimally invasive surgical robots must navigate through tissue with velocity limits to prevent thermal damage, force limits to avoid tissue perforation, and tremor filtering constraints. Kinodynamic planning ensures patient safety.

• Needle steering: Plans insertion trajectories that account for tissue deformation and needle bending dynamics
• Virtual fixtures: Enforces forbidden regions and velocity limits as hard constraints in the planner
• Active constraints: Dynamically adjusts allowable forces based on real-time tissue impedance measurements