Motion Planning

Motion planning searches a configuration space (C-Space), where every possible state of the robot (e.g., all joint angles for an arm) is a single point. For a 6-DOF arm, this is a 6D space; for a mobile robot with position and orientation, it's 3D. The curse of dimensionality makes exhaustive search impossible, necessitating sampling-based methods like RRT and PRM.

• Key Challenge: Exploring vast, continuous spaces efficiently.
• Example: Planning for a robotic arm amidst shelves requires searching a 7+ dimensional space.

A valid motion plan must satisfy multiple, often competing, constraints simultaneously. These are hard requirements that cannot be violated.

• Kinodynamic Constraints: Include robot dynamics (e.g., acceleration limits) and kinematics (e.g., nonholonomic constraints like a car's inability to move sideways).
• Collision Avoidance: The primary constraint, requiring continuous checking against environmental geometry.
• Task Constraints: Specific requirements like maintaining an object's orientation or end-effector pose.

Failure to satisfy any constraint renders a plan invalid.

These are two key theoretical properties used to classify planning algorithms.

• Completeness: An algorithm is complete if it will always find a solution if one exists, and report failure otherwise. Probabilistically Complete algorithms (like RRT*) guarantee success as runtime approaches infinity.
• Optimality: An algorithm is optimal if it guarantees the solution minimizes a specified cost function (e.g., path length, energy, time). Asymptotically Optimal algorithms (like RRT*) converge to an optimal solution over time.

There is often a trade-off between these properties and computational speed.

Unlike grid-based AI, motion planning operates in continuous spaces. The robot's state (position, velocity) and control inputs (joint torques, wheel velocities) are real-valued vectors. This requires:

• Geometric Representations: Using Signed Distance Fields (SDFs) or polygon meshes for collision checking.
• Continuous Optimization: Framing the problem as Nonlinear Programming (NLP) or using trajectory optimization to refine a coarse path into a smooth, dynamically feasible trajectory.
• Integration with Control: The output is a sequence of states and control inputs, directly feedable to a low-level controller.

Practical systems decompose planning into a hierarchical structure to manage complexity.

• Global Planning (High-Level): Uses a simplified world model to compute a coarse, long-term path from start to goal. Algorithms like A* or PRM are used here, often ignoring fine dynamics.
• Local Planning (Low-Level): Also called replanning or collision avoidance. Operates on a short time horizon, reacting to unexpected obstacles (dynamic or newly perceived) and refining the global path for dynamic feasibility. Methods include Model Predictive Control (MPC) and the Dynamic Window Approach (DWA).

This hierarchy balances long-term goal-directedness with real-time reactivity.

Motion planning is not a standalone module. It is deeply integrated with the perception stack and must account for uncertainty.

• Perception-Dependence: Plans are generated from a world model built by sensor fusion and state estimation. Inaccurate maps or localization cause plan failure.
• Planning Under Uncertainty: The real world is partially observable. Advanced frameworks like POMDPs explicitly model uncertainty in robot state, obstacle positions, and action outcomes to compute robust policies.
• Replanning Frequency: In dynamic environments, the planner must constantly re-evaluate and update the plan based on new sensor data.

Self-driving cars use hierarchical motion planning to navigate complex urban environments. A high-level route planner charts a course using map data, while a local behavioral planner handles lane changes, merges, and intersections. The lowest-level trajectory planner (often using Model Predictive Control) generates a smooth, collision-free path that obeys vehicle dynamics and traffic rules. This stack must react in milliseconds to pedestrians, cyclists, and other vehicles, making real-time performance critical.

Industrial robot arms rely on motion planning for precise tasks like welding, painting, and assembling electronics. Planners must solve inverse kinematics to position the end-effector while avoiding self-collision between the arm's own links. For bin picking or assembly in clutter, planners use grasp planning algorithms to find feasible approach trajectories that avoid obstacles and align the gripper correctly. Key challenges include dealing with high-dimensional configuration spaces (6-7+ joints) and tolerances measured in millimeters.

In minimally invasive surgery, robotic systems like the da Vinci use constrained motion planning to operate through small incisions. The planner must account for:

• Remote Center of Motion (RCM) constraint: The tool must pivot around the incision point, a nonholonomic constraint.
• Avoidance of critical anatomy: Planning occurs with respect to a 3D model of the patient's anatomy from pre-op scans.
• Tremor filtering and motion scaling: Human surgeon commands are translated into ultra-precise, smooth robotic trajectories. Safety and precision are paramount, with sub-millimeter accuracy required.

For robots like quadrupeds (Boston Dynamics Spot) or bipeds (Boston Dynamics Atlas), motion planning is tightly coupled with dynamic control. Planners generate:

• Footstep planning: Determining where and when to place feet on uneven terrain.
• Center of Mass (CoM) trajectories: Planning the body's motion to maintain dynamic balance, often using simplified models like the Linear Inverted Pendulum.
• Whole-body trajectory optimization: Coordinating all joint motions to achieve a dynamic gait, jump, or recovery maneuver while respecting torque limits and contact forces. This often happens in a receding-horizon fashion.

Drones use motion planning for autonomous inspection, delivery, and aerial cinematography. Applications include:

• Obstacle-rich navigation: Using *RRT or similar algorithms to plan through forests or urban canyons, often with a Signed Distance Field (SDF) from onboard sensors.
• Energy-optimal trajectories: Planning paths that minimize battery consumption over long distances.
• Expressive motion planning: For film, planning smooth, dynamically feasible camera paths that frame a subject artistically while avoiding obstacles. This requires planning in the SE(3) space (3D position + orientation).

The Configuration Space (C-Space) is a fundamental mathematical abstraction where every possible state of a robot is represented as a single point. This transforms complex 3D collision checking into a simpler problem of checking if a point lies within a forbidden region.

• Key Insight: A robot's physical geometry and joint limits define the boundaries of C-Space.
• Obstacle Mapping: Real-world obstacles are inflated and mapped into C-Obstacles within this space.
• Dimensionality: For an articulated arm with n joints, C-Space is an n-dimensional manifold. Planning a path for the robot becomes the task of finding a curve for a point through free space.

Trajectory Optimization is the mathematical process of finding not just a feasible path, but an optimal sequence of states and control inputs. It frames motion planning as a constrained optimization problem.

• Cost Function: Minimizes criteria like time, energy, jerk (rate of change of acceleration), or control effort.
• Constraints: Must satisfy robot dynamics, actuator limits, and collision avoidance.
• Methods: Often solved via Nonlinear Programming (NLP) techniques like Sequential Quadratic Programming (SQP). It is the computational core of Model Predictive Control (MPC) for real-time re-planning.

Sampling-based planners are algorithms designed to efficiently explore high-dimensional C-Spaces by randomly sampling points, rather than explicitly modeling the entire space.

• Rapidly-exploring Random Tree (RRT): Builds a tree incrementally from the start configuration, biasing growth toward unexplored regions. Highly effective for single-query problems in complex spaces.
• Probabilistic Roadmap (PRM): Creates a graph (roadmap) of collision-free configurations in a pre-processing phase. Efficient for multi-query problems in static environments.
• Probabilistic Completeness: These algorithms guarantee finding a solution if one exists, given enough sampling time.

Search-based planners discretize the planning space into a graph (e.g., a grid) and apply graph search algorithms to find the optimal path.

• Dijkstra's Algorithm: Finds the shortest-cost path from a start node to all nodes in a graph with non-negative edge weights. Exhaustive but guarantees optimality.
• A Algorithm*: Enhances Dijkstra by using a heuristic function (e.g., Euclidean distance to goal) to prioritize exploring promising nodes. Optimal if the heuristic is admissible (never overestimates cost).
• Application: Dominant in 2D grid-based navigation (e.g., video games, warehouse AMRs) and forms the basis for more complex planners in discretized state spaces.

Collision Detection is the real-time geometric computation to determine if a robot's proposed configuration intersects with obstacles. Signed Distance Fields (SDFs) are a powerful representation to accelerate this.

• SDF Definition: A 3D grid where each cell stores the distance to the nearest object surface. The sign indicates inside (negative) or outside (positive).
• Efficient Queries: Checking collision for a point becomes a simple grid lookup and sign check. Gradient of the SDF provides the direction to the nearest surface, crucial for gradient-based optimization.
• Use Case: Essential for real-time motion planning in dynamic environments and a key component in physics simulators.

A Nonholonomic Constraint is a kinematic restriction on a system's possible velocities that is not integrable into a constraint on position alone. It limits how a robot can move, not necessarily where it can go.

• Classic Example: A car cannot move directly sideways; it must steer. The constraint is expressed as a differential equation: dy/dx - tan(θ) = 0.
• Planning Implication: Algorithms must plan in the space of feasible trajectories (e.g., Reeds-Shepp curves for cars), not just points in C-Space. Sampling-based planners like RRT must be extended (e.g., to Kinodynamic RRT) to respect these constraints.
• Other Examples: Differentially-driven robots, airplanes, and boats.