Motion Planning, Autonomy, and AI Robotics

Introduction

A robot is useful not merely because it can move, but because it can select safe, efficient, and purposeful actions in an uncertain environment. Motion planning connects geometric models of robots with algorithms that generate feasible trajectories. Autonomy extends this process by allowing a robot to perceive, reason, act, and recover with limited human intervention. Artificial intelligence adds learning, semantic interpretation, and adaptation, but also introduces concerns about robustness, verification, explainability, and responsibility.

This chapter develops a unified view of planning for mechatronic systems. It begins with configuration spaces and collision detection, then examines graph-search and sampling-based planners, local obstacle avoidance, task planning, and behavior architectures. It concludes with machine learning, reinforcement learning, foundation models, and an integrated warehouse-robot case.

Learning Objectives

By the end of this chapter, a student should be able to:

1. Model a robot and its obstacles in configuration space.
2. Explain collision detection, obstacle inflation, and free-space connectivity.
3. Select and implement graph-search or sampling-based motion planners.
4. Distinguish global planning from local obstacle avoidance.
5. Relate task planning, motion planning, and behavior architectures.
6. Evaluate learning-based and foundation-model approaches using safety, robustness, and explainability criteria.
7. Analyse an autonomous robotic application and justify an appropriate planning architecture.

1. From Geometric Motion to Autonomous Behaviour

A planning problem has four essential elements:

• State: the information needed to describe the robot and relevant environment conditions.
• Action: a command or control input that changes the state.
• Goal: a desired state, task condition, or set of acceptable outcomes.
• Constraints: limits imposed by geometry, actuators, dynamics, safety, or regulation.

For a robot manipulator, the state may include joint positions and velocities. For a mobile robot, it may include planar position and orientation. A planner must account for more than whether a geometric path exists. A path may be too long, pass through narrow clearances, demand excessive acceleration, or require an impossible change in orientation.

A useful hierarchy is:

These levels interact. A task planner may request that a mobile manipulator pick an object, but motion planning must determine whether the arm can reach it without collision. Conversely, a motion planner may report that a requested task is infeasible, causing task-level replanning.

2. Configuration-Space Modeling

2.1 Configuration and configuration space

A robot configuration is a minimal set of coordinates that uniquely specifies its pose or joint arrangement. For a planar rigid body, a configuration is commonly

[[M:q = (x, y, θ)]]

where [[M:x]] and [[M:y]] are position coordinates in metres and [[M:θ]] is orientation in radians.

The set of all possible configurations is the configuration space, or C-space, denoted [[M:C]]. The subset in which the robot does not intersect an obstacle is the free configuration space, [[M:C_free]].

Here, [[M:Robot(q)]] is the physical region occupied by the robot at configuration [[M:q]]. The equation assumes a known geometric model and a defined collision criterion. In practice, uncertainty margins are often added, so that nominally free configurations must also maintain a specified safety clearance.

A path is a continuous mapping [[M:σ:[0,1]→C_free]] joining start configuration [[M:q_s]] to goal configuration [[M:q_g]]. A trajectory additionally specifies time, [[M:q(t)]], and must satisfy velocity, acceleration, and actuator constraints.

2.2 Obstacles and Minkowski sums

For a translating robot whose shape is represented by a set [[M:R]], collision checking can be converted into point planning by expanding obstacles. If an obstacle is [[M:O]], the configuration-space obstacle is

where [[M:⊕]] denotes the Minkowski sum and [[M:−R]] is the reflection of the robot about the reference point. A reference point is collision-free exactly when it does not lie in the expanded obstacle, under the assumptions of rigid bodies, exact geometry, and translation-only motion.

For a circular robot with radius [[M:r]], obstacle inflation by [[M:r]] is geometrically straightforward. For a rectangular robot, the exact configuration-space obstacle depends on orientation; using a circumscribed circle is simpler but conservative. Conservative models reduce collision risk but may eliminate feasible routes.

For articulated robots, each configuration is a vector of joint coordinates:

The C-space dimension is normally the number of independent degrees of freedom. A six-joint industrial arm therefore has a six-dimensional joint space, even though its end effector moves in three-dimensional physical space. Obstacles become complex regions in joint space, and self-collision must be checked in addition to collision with the environment.

2.3 Constraints and metrics

A planning metric measures path length or cost. For a planar robot, a weighted distance may be written as

where [[M:λ]] has units of length per radian and expresses the relative cost of rotation. The function [[M:wrap(·)]] returns the smallest equivalent angular difference, usually in [[M:(−π,π]]]. This metric is not universal: energy, risk, time, clearance, and actuator wear may be more important than geometric distance.

A general planning objective can combine several terms:

Here [[M:L]] is path length, [[M:T]] travel time, [[M:R]] risk, and [[M:E]] energy; the weights must be selected so that terms are dimensionally normalized or have compatible units. A planner that minimizes distance alone may produce a path with poor safety or excessive control effort.

3. Collision Detection and Validation

Collision detection determines whether two geometric bodies intersect or whether their separation is below a safety threshold. Common representations include exact meshes, convex decompositions, bounding volumes, occupancy grids, signed-distance fields, and voxel maps.

Broad-phase detection quickly rejects pairs that are far apart, often using axis-aligned bounding boxes or bounding-volume hierarchies. Narrow-phase detection performs detailed intersection tests only for potentially colliding pairs. This separation is important because a robot may contain many links and obstacles, making exhaustive exact testing computationally expensive.

A practical collision checker should address:

• environment collision;
• self-collision;
• attached objects and tools;
• joint limits and workspace limits;
• continuous collision along an edge, not only at its endpoints;
• uncertainty in localization, geometry, and actuation.

Checking only the endpoints of a path segment is unsafe. A segment between two collision-free configurations may pass through an obstacle. Therefore, planners recursively subdivide an edge or use conservative continuous-collision methods. The sampling resolution must be related to obstacle size and robot motion; excessively coarse checking can miss thin obstacles, whereas excessively fine checking increases computation.

4. Graph-Search Motion Planning

Graph planners discretize the free space into nodes and edges. A grid planner represents a mobile robot’s position, and possibly orientation, on a lattice. A roadmap represents collision-free connections among selected configurations.

4.1 Dijkstra’s algorithm and A*

Dijkstra’s algorithm expands the lowest-cost unvisited node and is optimal for non-negative edge costs, but it explores many nodes that are irrelevant to the goal. A* adds a heuristic [[M:h(n)]]:

where [[M:g(n)]] is the cost from the start to node [[M:n]], and [[M:h(n)]] estimates the remaining cost to the goal. If the heuristic never overestimates the true remaining cost, it is admissible; with suitable consistency conditions, A* returns an optimal path on the given graph.

For a four-connected unit-cost grid, Manhattan distance is admissible when diagonal moves are not allowed:

For an eight-connected grid, the heuristic must match the movement costs. A* is complete on a finite graph when implemented correctly, but its optimality applies to the discretized graph, not necessarily to the continuous world.

4.2 Grid resolution and trade-offs

Fine grids represent obstacles and clearance more accurately but require more memory and computation. Coarse grids are faster but can erase narrow passages or generate paths that are not executable by the real robot. A grid planner should therefore include robot footprint inflation and a post-planning path-smoothing and validation step.

Worked Example: A* Cost Comparison

Suppose two candidate routes have costs based on distance and risk:

• Route A: distance 12 m, risk penalty 4;
• Route B: distance 15 m, risk penalty 1.

Let the objective be [[M:J = L + 2R]], where the coefficient 2 is expressed in equivalent metres per risk unit. Then:

[[M:J_A = 12 + 2(4) = 20]]

[[M:J_B = 15 + 2(1) = 17]]

Route B is selected even though it is geometrically longer. This illustrates why the cost function must reflect operational priorities rather than distance alone.

5. Sampling-Based Planning

Grid methods become impractical as dimension increases. Sampling-based planners avoid constructing a complete discretization of high-dimensional C-space.

5.1 Probabilistic Roadmaps

A Probabilistic Roadmap (PRM) samples configurations, retains collision-free samples, and connects nearby samples when the connecting local path is collision-free. It is especially suitable for repeated queries in a largely static environment. A roadmap has a learning phase and a query phase.

PRM is probabilistically complete under standard assumptions: as the number of samples approaches infinity, the probability of finding a path approaches one if a robust path exists. This does not mean that a finite roadmap guarantees success, nor does it guarantee the shortest path.

5.2 Rapidly-exploring Random Trees

RRT grows a tree from the start. A random configuration is selected, the nearest tree node is found, and the tree is extended toward the sample by a limited step. RRT is useful for single-query problems and systems with differential constraints.

RRT adds rewiring to improve path quality. Under appropriate assumptions, RRT is asymptotically optimal: as samples increase, the best path cost converges toward the optimum. Convergence may be slow, and narrow passages remain difficult.

A simplified RRT procedure is:

1. Initialize a tree with [[M:q_s]].
2. Sample [[M:q_rand]] from C-space.
3. Find the nearest node [[M:q_near]].
4. Move a limited distance toward the sample to obtain [[M:q_new]].
5. Add [[M:q_new]] if the edge is collision-free.
6. Stop when a valid connection to the goal is found or a computation limit is reached.

Sampling strategy matters. Goal bias can accelerate discovery, while obstacle-aware or informed sampling can improve efficiency. However, excessive goal bias may cause repeated failure in a constrained region.

6. Global Planning, Local Avoidance, and Replanning

A global planner uses a map to compute a route over a relatively large region. A local planner reacts to current sensor observations and generates short-horizon commands. Combining both is usually more reliable than using either alone.

The Dynamic Window Approach (DWA), for example, evaluates admissible velocity commands over a short horizon. It considers the robot’s current velocity limits, braking capability, obstacle clearance, and progress toward a global path. The velocity search must be restricted to commands that can be safely stopped before collision.

Potential-field methods define attractive and repulsive influences. A basic attractive potential can be written as

where [[M:k_att]] is a positive gain and [[M:q_g]] is the goal. A repulsive potential is applied near obstacles. The command is often related to the negative gradient, [[M:−∇U(q)]]. Potential fields are computationally simple but may produce local minima, oscillations, or narrow-passage failure. Randomized escapes, navigation functions, or a global planner can reduce these problems.

Replanning is required when the map changes, the robot deviates, an obstacle appears, or a prediction becomes invalid. A robust system separates fast safety reactions from slower global optimization. Emergency braking and protective stops should not depend on a computationally heavy planner.

7. Task Planning and Behaviour Architectures

Motion planning answers “how to move,” whereas task planning answers “which actions should be performed and in what order.” A symbolic task planner may represent actions using preconditions and effects. For example, a “pick” action may require that an object is visible, the gripper is empty, and the robot is near a grasp pose. Its effects may include “object held” and “gripper occupied.”

A hierarchical task network decomposes a high-level objective such as “prepare an order” into navigation, perception, grasping, verification, and delivery. Task planners are effective when symbolic state descriptions are reliable, but real environments contain uncertainty and unmodeled events.

Three common behaviour architectures are:

The subsumption architecture is a reactive design in which lower-level behaviours, such as collision avoidance, can suppress higher-level behaviours. A finite-state machine explicitly represents modes and transitions. Behaviour trees organize conditions and actions in a readable, modular structure. A hybrid system commonly places safety-critical reflexes below a task executive and a global planner.

8. Learning-Based Robotics

Machine learning can support perception, grasp selection, terrain classification, dynamics prediction, cost estimation, and policy generation. It is most valuable where hand-designed models are difficult or incomplete. However, a learned model may fail under distribution shift, sensor degradation, unusual objects, or adversarial conditions.

8.1 Supervised and self-supervised learning

Supervised learning estimates a mapping from labelled examples, such as images to object poses. Performance depends on representative data and correct labels. Self-supervised methods obtain labels from robot interaction, geometric consistency, or temporal prediction, reducing manual annotation but not eliminating bias.

Learning can also estimate a planning cost or a collision probability. Such estimates should be treated as uncertain predictions rather than facts. A safety layer can reject actions with insufficient confidence or require additional sensing.

8.2 Reinforcement learning

In reinforcement learning, an agent selects action [[M:a_t]] in state [[M:s_t]] and receives reward [[M:r_t]]. A policy [[M:π(a|s)]] specifies action selection. The discounted return is

where [[M:γ]] is the discount factor, normally satisfying [[M:0 ≤ γ < 1]] for an infinite-horizon discounted problem. The formulation assumes rewards are bounded or otherwise well behaved. A high reward does not automatically imply safety: reward design can create unintended shortcuts.

Safe reinforcement learning may impose constraints on collision probability, energy, or human exposure. In deployment, learned policies are often combined with model-predictive control, control-barrier constraints, action shielding, or a verified fallback controller. Training in simulation requires attention to the sim-to-real gap; randomized textures alone do not guarantee transfer of contact dynamics, latency, friction, or sensor noise.

9. Foundation Models and Embodied AI

Vision-language models can interpret instructions, identify objects, describe scenes, and propose task decompositions. Language models can translate a command such as “place the blue container on the inspection table” into symbolic subtasks. These capabilities may reduce programming effort and improve human-robot interaction.

Nevertheless, foundation models are not inherently reliable motion planners or safety controllers. They may hallucinate object properties, misunderstand references, produce invalid action sequences, or be manipulated by misleading visual or textual inputs. A responsible architecture therefore uses a foundation model as a high-level assistant, while grounding its outputs in:

• a verified action library;
• explicit precondition and postcondition checks;
• geometric feasibility tests;
• access control and audit logs;
• uncertainty thresholds and human approval for consequential actions;
• deterministic emergency and safety mechanisms.

Explainability has several forms. A planner can provide a geometric path and clearance values; a task executive can provide the selected action sequence and failed precondition; a learned policy may provide saliency or value estimates, but these are not necessarily faithful causal explanations. Engineers should distinguish an explanation that is understandable from one that accurately reflects the system’s internal decision process.

10. Robustness, Safety, and Evaluation

A robotics system should be evaluated beyond average success rate. Relevant measures include:

• task success probability;
• collision and near-collision frequency;
• path length and execution time;
• minimum obstacle clearance;
• computation time and replanning latency;
• energy consumption;
• recovery success after sensor or actuator faults;
• calibration and localization sensitivity;
• performance across environments not seen during development.

If [[M:N_s]] successful trials occur in [[M:N]] independent tests, an empirical success rate is

This is a descriptive estimate, not proof of future performance. Statistical confidence intervals should be used when making safety or deployment claims, and tests should include rare but consequential failure modes.

Safety engineering uses defense in depth. Examples include speed limits near humans, redundant sensing, force and torque monitoring, geofencing, protective stops, watchdog timers, safe-state transitions, and manual override. The appropriate regulatory framework depends on jurisdiction and application, but risk assessment, traceability, cybersecurity, and human accountability are central professional obligations.

11. Illustrative Case Study: Autonomous Warehouse Mobile Manipulator

Background

Consider an illustrative warehouse robot that receives a natural-language request to retrieve a boxed component, navigate aisles, grasp the box, and deliver it to an inspection station. The warehouse contains dynamic human workers, pallets, shelving, and narrow passages.

Problem

The robot must combine semantic perception, task planning, navigation, manipulation, and recovery. A shortest route may be unsafe if it passes close to workers. A grasp pose may be visually plausible but unreachable because of shelf geometry.

Analysis

The system uses a two-dimensional occupancy map for global navigation and a three-dimensional representation near shelves. The robot footprint is inflated by the required safety margin before grid planning. A* generates a global route with costs for distance, predicted congestion, and clearance. A local velocity planner responds to moving people.

The manipulator uses joint-space sampling-based planning. Candidate grasp configurations are filtered by inverse-kinematics feasibility, joint limits, self-collision, shelf collision, and gripper reach. A task executive represents states such as “object located,” “robot positioned,” “grasp verified,” and “delivery complete.”

A vision-language model interprets the request and identifies candidate objects, but it cannot directly command motors. Its proposed object identity is checked against inventory data, and its proposed grasp is validated by geometric and force criteria.

Approach and solution

1. Parse the request into a verified task graph.
2. Localize the robot and object using sensor fusion.
3. Inflate mapped obstacles by robot footprint and uncertainty margin.
4. Use A* for a global route.
5. Use local avoidance with braking-safe velocity limits.
6. Generate and validate arm trajectories using a sampling-based planner.
7. Verify grasp force and object identity.
8. Replan if a worker blocks the route or grasp verification fails.
9. Move to a safe state and request human assistance after repeated failures.

Outcome

The architecture is more robust than a single end-to-end policy because each stage has explicit interfaces and checks. Its disadvantages are increased engineering effort, computational overhead, and the need to maintain maps, models, and recovery logic.

Learning points

The case demonstrates that autonomy is a systems problem. No single algorithm solves perception uncertainty, geometric feasibility, task sequencing, dynamic avoidance, and safety simultaneously. Learning components are most valuable when integrated with explicit models, validation, and fallback behaviour.

12. Best-Practice Design Workflow

A practical workflow is:

1. Define the operational design domain, including surfaces, lighting, people, payloads, and speed limits.
2. Select state variables and model uncertainty explicitly.
3. Build accurate robot and obstacle geometry.
4. Choose a global planner appropriate to dimensionality and map structure.
5. Validate continuous collision, dynamics, and actuator limits.
6. Add a local safety layer independent of high-level learning.
7. Define task states, preconditions, postconditions, and recovery actions.
8. Evaluate nominal, boundary, fault, and adversarial cases.
9. Log decisions, sensor data, planner versions, and overrides.
10. Deploy gradually with monitoring and a rollback plan.

The best planner is not necessarily the theoretically most sophisticated. It is the planner whose assumptions match the environment, whose failure modes are understood, and whose outputs can be monitored and safely rejected.

Chapter Summary

Motion planning transforms robot geometry, constraints, and goals into feasible movement. Configuration space provides a powerful abstraction: collision checking becomes the problem of avoiding configuration-space obstacles. Graph-search methods such as A* are effective in discretized spaces, while PRM and RRT methods address higher-dimensional continuous spaces. Their completeness and optimality claims depend on assumptions and should not be confused with guaranteed real-world success.

Global planning should be complemented by local avoidance, continuous collision validation, and replanning. Task planning determines action sequences, while finite-state machines, behaviour trees, reactive layers, and hybrid architectures organize autonomous behaviour. Machine learning and reinforcement learning offer adaptability, but require careful treatment of data quality, distribution shift, reward design, and sim-to-real transfer. Foundation models can assist with semantic interpretation and task decomposition, but their outputs must be grounded by explicit preconditions, geometric checks, safety mechanisms, and human accountability.

A professional autonomous robot is therefore a layered system: model-based where guarantees matter, reactive where time is critical, and learning-based where uncertainty and complexity make hand-designed solutions inadequate.

Key Terms

Configuration space: The set of coordinates describing all robot configurations.

Free configuration space: Configurations that satisfy collision and other constraints.

Minkowski sum: A geometric operation used to expand obstacles for point-based planning.

Collision detection: Determining whether robot and obstacle geometries intersect or violate clearance.

Path: A continuous geometric route through configuration space.

Trajectory: A path parameterized by time.

Graph search: Planning over nodes and edges in a discrete representation.

A*: A heuristic graph-search algorithm using path cost plus estimated remaining cost.

Admissible heuristic: A heuristic that never overestimates the true remaining cost.

Probabilistic roadmap: A sampling-based graph for repeated motion-planning queries.

RRT: A sampling-based tree planner that rapidly explores configuration space.

Probabilistic completeness: Probability of finding a path approaches one as samples increase, under stated assumptions.

Local planner: A short-horizon planner reacting to current surroundings.

Task planning: Selection and ordering of symbolic actions to achieve a goal.

Behaviour architecture: Organization of reactive, deliberative, and executive robot behaviours.

Reinforcement learning: Learning action policies through interaction and reward.

Sim-to-real transfer: Applying policies trained in simulation to physical robots.

Foundation model: A broadly trained model adaptable to multiple tasks or modalities.

Explainability: The degree to which a system’s decisions can be understood and justified.

Defence in depth: Use of multiple independent safety mechanisms.

Review Questions

Short Answer Questions

1. What is the difference between configuration space and physical workspace?
2. Why are obstacles inflated when planning for a translating robot?
3. What property must an A* heuristic have to preserve optimality on a graph?
4. State one advantage and one limitation of sampling-based planning.
5. Why should a foundation model not directly command safety-critical robot actuators?

Long Answer Questions

1. Explain how a mobile robot’s physical collision problem can be transformed into point planning in configuration space.
2. Compare Dijkstra’s algorithm, A*, PRM, and RRT in terms of representation, efficiency, and applicability.
3. Explain why global planning and local obstacle avoidance are normally combined in autonomous robots.
4. Describe how task planning, motion planning, trajectory generation, and control interact in a robotic manipulation task.
5. Discuss the benefits and risks of reinforcement learning for physical robots, including reward design and sim-to-real transfer.

Analytical / Application Questions

1. Design a planning architecture for a hospital delivery robot operating among people. Specify its map representation, global planner, local safety layer, task executive, and recovery actions.
2. A robot has two feasible routes: one is shorter but passes close to a human work area, while the other is longer with greater clearance. Define a cost function and explain how you would select and validate the route.
3. A vision-language model proposes that a box is graspable, but the motion planner reports that every collision-free grasp configuration violates a joint limit. Explain the correct system response and identify which layer should make the final decision.

Multiple Choice Questions with Answers

Question 1

What is the free configuration space?

A. All positions occupied by obstacles

B. Configurations satisfying collision and other specified constraints

C. The physical workspace of the robot only

D. The set of actuator voltages

Correct Answer: B. Configurations satisfying collision and other specified constraints

Question 2

For a translating robot, obstacle inflation is used primarily to convert robot-obstacle collision checking into what problem?

A. Point planning around expanded obstacles

B. Image classification

C. Motor identification

D. Symbolic task decomposition

Correct Answer: A. Point planning around expanded obstacles

Question 3

In A*, the evaluation function is [[M:f(n) = g(n) + h(n)]]. What does [[M:g(n)]] represent?

A. Estimated distance from the goal to the start

B. The heuristic error

C. Cost accumulated from the start to node n

D. Robot acceleration at node n

Correct Answer: C. Cost accumulated from the start to node n

Question 4

Which statement about an admissible A* heuristic is correct?

A. It must always overestimate the remaining cost

B. It never overestimates the true remaining cost

C. It ignores the goal state

D. It can assign negative edge costs

Correct Answer: B. It never overestimates the true remaining cost

Question 5

Which planner is especially suitable for repeated queries in a largely static configuration space?

A. Probabilistic roadmap

B. Emergency braking controller

C. Finite-state sensor filter

D. Single-step potential-field command

Correct Answer: A. Probabilistic roadmap

Question 6

What is a major limitation of basic potential-field navigation?

A. It cannot represent a goal

B. It always requires a precomputed roadmap

C. It can become trapped in local minima

D. It cannot use distance measurements

Correct Answer: C. It can become trapped in local minima

Question 7

What is the main role of a local planner?

A. Compile robot firmware

B. React to nearby obstacles and generate short-horizon commands

C. Label all training images

D. Define the robot’s mechanical joints

Correct Answer: B. React to nearby obstacles and generate short-horizon commands

Question 8

Which statement best describes reinforcement learning?

A. Learning a policy through interaction and reward

B. Computing only exact inverse kinematics

C. Inflating obstacles by a robot radius

D. Searching a fixed grid without a cost function

Correct Answer: A. Learning a policy through interaction and reward

Question 9

Why is sim-to-real transfer difficult?

A. Simulation cannot represent any geometry

B. Physical dynamics, sensing, latency, and friction may differ from simulation

C. Real robots do not require controllers

D. Simulation always uses continuous collision checking

Correct Answer: B. Physical dynamics, sensing, latency, and friction may differ from simulation

Question 10

Which is the most appropriate use of a foundation model in a safety-critical robot architecture?

A. Directly generate unrestricted motor currents

B. Replace all collision checking

C. Assist with semantic interpretation while verified layers validate actions

D. Disable human override to improve autonomy

Correct Answer: C. Assist with semantic interpretation while verified layers validate actions