Motion Planning, Navigation and Task Planning

Introduction

A robot is useful only when it can transform a goal—such as reaching a workstation, grasping a component, or coordinating with another robot—into safe physical motion. This transformation is the concern of planning. Motion planning determines where a robot may move and which path it should follow. Navigation applies planning to movement through an environment, often under uncertainty and with incomplete information. Task planning determines what sequence of actions should be performed to achieve a goal.

These levels are closely related. A task planner may decide that a mobile manipulator must collect a part, transport it, and place it in a fixture. A motion planner must then find collision-free configurations for the base and arm. A trajectory generator converts the geometric path into time-dependent commands that respect velocity, acceleration, and actuator limits. During execution, a navigation system continually updates the plan as sensors reveal moving obstacles or modelling errors.

This chapter develops configuration-space models, graph-search and sampling-based planners, trajectory generation, obstacle avoidance, scheduling, and multi-robot coordination. The emphasis is on the assumptions behind each method and on the engineering trade-offs between completeness, computational cost, path quality, and robustness.

Learning Objectives

After studying this chapter, the reader should be able to:

1. Model robot motion in configuration space and explain obstacle inflation.
2. Formulate planning as a search problem over a continuous or discrete state space.
3. Select and apply graph-search and sampling-based planning methods.
4. Generate time-parameterized trajectories subject to kinematic and dynamic limits.
5. Design local obstacle-avoidance and navigation strategies under uncertainty.
6. Explain task planning, scheduling, and resource constraints.
7. Analyse coordination, safety, and performance in multi-robot systems.
8. Evaluate practical limitations, ethical responsibilities, and current developments in robot planning.

1. The Planning Problem

Let a robot configuration be represented by a vector q. For a planar mobile robot, q may be q = (x,y,θ), where x and y are position coordinates in metres and θ is heading in radians. For a six-joint manipulator, q contains six joint angles. The configuration space, or C-space, is the set of all possible q values.

A planning problem normally contains:

• an initial configuration qₛ;
• a goal region Qg rather than necessarily one exact configuration;
• an obstacle region in the workspace;
• robot kinematic and mechanical constraints; and
• an objective, such as minimum distance, time, energy, or risk.

A feasible path is a continuous function q(s), with 0 ≤ s ≤ 1, such that q(0) = qₛ, q(1) ∈ Qg, and every configuration on the path is collision-free. A trajectory adds time: q(t), with t ∈ [0,T]. Thus, a path describes geometry, whereas a trajectory describes geometry and timing.

A useful general cost functional is:

Here L is path length, T is execution time, E is energy or control effort, R is a risk measure, and wL, wT, wE, and wR are non-negative weights. The terms must be scaled consistently or normalized before combining them. The best path is therefore application-dependent: a warehouse vehicle may prioritize time, while a surgical robot may prioritize clearance and smoothness.

Configuration-space obstacles

Let O be an obstacle in workspace and R(q) the robot occupied region at configuration q. The forbidden configuration set is

The free configuration space is

where C is the complete configuration space. Planning becomes the search for a continuous route through C_free. This abstraction is powerful because collision checking is expressed as membership in C_obstacle, regardless of whether the robot is a point, rigid body, or articulated mechanism.

For a circular robot of radius r moving among planar obstacles, the robot can be treated as a point if every obstacle is expanded outward by r. This is a Minkowski-sum construction:

where B_r is a disk of radius r and ⊕ denotes the Minkowski sum. If a safety margin m is required, the effective radius becomes r + m. The approximation is exact for a circular robot translating without rotation; it is not generally exact for a non-circular or rotating robot.

For a rectangular robot, obstacle inflation depends on orientation. A common engineering approximation uses a bounding circle or a set of orientation-specific inflated maps. These simplify computation but can be conservative or unsafe if the approximation is not chosen carefully.

Degrees of freedom and constraints

The dimension of C-space equals the number of independent configuration variables. A planar rigid body has three degrees of freedom: x, y, and θ. A differential-drive robot has three pose variables but only two instantaneous control inputs, linear velocity v and angular velocity ω. Its nonholonomic constraint prevents arbitrary sideways motion. In ideal form:

where ẋ and ẏ are translational velocity components. A path planner that ignores this constraint may produce a geometrically valid route that the robot cannot follow.

2. Discrete Graph Search

A continuous planning problem can be approximated by a graph G = (V,E), where V is a set of states or configurations and E is a set of feasible connections. Each edge has a cost c(u,v), often distance or estimated travel time.

Grid and visibility representations

In a grid map, free space is divided into cells. Four-connected grids allow movement north, south, east, and west; eight-connected grids also allow diagonal movement. Diagonal edges should have cost √2 times the cell width when the grid spacing is equal in both directions. Corner-cutting must be prohibited when a diagonal move would pass through an obstacle corner.

A visibility graph represents polygonal obstacle vertices and start and goal points as nodes. Two nodes are connected if the straight segment between them is collision-free. For polygonal obstacles and Euclidean path length, a shortest path can be found through obstacle vertices, although the graph may become large and sensitive to localization error.

Dijkstra’s algorithm and A*

Dijkstra’s algorithm expands the lowest-cost unvisited node and is optimal when edge costs are non-negative. A* improves efficiency by adding a heuristic h(n) estimating the remaining cost:

Here g(n) is the cost from the start to node n, and h(n) is the estimated cost from n to the goal. If h(n) never overestimates the true remaining cost, it is admissible, and A* returns an optimal solution for the graph. A consistent heuristic satisfies h(n) ≤ c(n,n') + h(n') for every edge and supports efficient closed-set handling.

For a grid with four-connected movement and unit edge cost, Manhattan distance is admissible:

For unrestricted Euclidean movement, Euclidean distance is appropriate:

The heuristic must match the permitted motion and cost model. An excessively optimistic heuristic remains correct but may expand many nodes; an overestimating heuristic may produce a faster but non-optimal result.

Worked example: A* cost comparison

Suppose three frontier nodes have the following values:

A* expands node B first because f(B) = 6 + 3 = 9 is smallest. This does not mean B is physically closest to the goal; it means B has the lowest estimated total cost under the selected heuristic.

Limitations of graph search

Resolution affects both quality and cost. A fine grid represents narrow passages better but increases memory and computation. A coarse grid is efficient but may eliminate feasible routes. Graph search also depends on map accuracy. Unknown or dynamic obstacles require replanning, such as repeated A, D Lite, or other incremental methods.

3. Sampling-Based Motion Planning

Graph discretization is difficult in high-dimensional configuration spaces. Sampling-based planners avoid constructing a complete grid by testing selected configurations and connecting nearby samples.

Probabilistic Roadmaps

A Probabilistic Roadmap (PRM) is generally used for repeated queries in a mostly static environment. During a learning phase, collision-free samples are generated in C_free and connected to nearby samples using a local planner. During a query phase, the start and goal are connected to the roadmap and a graph search finds a route.

PRM is probabilistically complete under suitable conditions: if a path exists with sufficient clearance and the number of samples approaches infinity, the probability of finding a path approaches one. This is not the same as deterministic completeness, which guarantees success after finite systematic exploration.

Rapidly-exploring Random Trees

RRT grows a tree from the initial configuration. A random sample qrand is generated, the nearest existing node qnear is selected, and a new state qnew is created by moving a limited distance toward qrand. The edge is retained only if it is collision-free.

RRT is effective for single-query problems and high-dimensional systems. Basic RRT is not generally optimal. RRT adds rewiring: nearby nodes are reconsidered so that lower-cost parent connections can be established. Under standard assumptions, RRT is asymptotically optimal, meaning that the best path cost approaches the optimum as the number of samples tends to infinity.

Sampling bias is important. Uniform sampling may waste effort in large open areas, while goal biasing increases the probability of approaching the goal. Narrow passages remain difficult because their volume in C-space may be very small. Collision checking is often the computational bottleneck.

Comparing planning methods

4. From Path to Executable Trajectory

A geometric path is not directly suitable for motors. The robot requires q(t), q̇(t), and often q̈(t), while respecting limits such as

and

where i identifies a joint or coordinate. Units must be consistent: angular velocity is measured in rad/s, translational velocity in m/s, and acceleration in m/s² or rad/s².

Polynomial interpolation

For one scalar coordinate, a cubic polynomial can satisfy specified initial and final position and velocity:

Given q(0)=q₀, q(T)=qf, q̇(0)=v₀, and q̇(T)=vf, the coefficients are determined by these four boundary conditions. For the common case v₀ = vf = 0:

This trajectory has zero velocity at both endpoints. Its velocity is

and its acceleration is

The cubic has continuous position, velocity, and acceleration within the segment, but acceleration changes abruptly at the boundaries if adjacent segments are not coordinated. Industrial systems often use quintic polynomials, trapezoidal velocity profiles, or S-curve profiles to control jerk.

Time scaling and retiming

A path q(s), 0 ≤ s ≤ 1, can be time-scaled by choosing s(t). This separates geometric planning from velocity planning. Retiming must account for all joints simultaneously: a trajectory is feasible only if every joint remains within its limits. A path with high curvature may also require lower speed to respect vehicle lateral acceleration or manipulator dynamic constraints.

Smoothness and feasibility

Shortest paths may contain sharp corners. A real robot must decelerate before a corner or follow a smoothed curve. Smoothing must never move the robot into an obstacle. The correct order is therefore: generate a candidate path, smooth it, re-check collision clearance, and retime it under kinematic limits.

5. Navigation and Obstacle Avoidance

Navigation combines localization, mapping, global planning, local control, and feedback. A typical architecture uses a global planner for long-range route selection and a local planner for short-horizon collision avoidance.

Potential fields

In a potential-field method, the robot experiences an attractive influence toward the goal and repulsive influences from obstacles. A simple attractive potential is

where k_att > 0 is a gain and ||·|| is Euclidean distance. A repulsive potential may be defined only within influence distance ρ₀:

and U_rep(q)=0 when ρ(q)>ρ₀. Here ρ(q) is distance to the nearest obstacle, and krep is a gain. The control direction is often proportional to −∇U(q). Potential fields are computationally simple but can suffer from local minima, oscillation, and narrow-passage failure.

Velocity obstacles and dynamic window methods

For moving obstacles, a robot must reason about relative velocity, not merely current position. A velocity obstacle is the set of robot velocities that would lead to collision with an obstacle within a specified time horizon. Reciprocal methods allow multiple agents to select compatible velocities.

The Dynamic Window Approach evaluates reachable velocities over a short interval, rejects those that cause collision, and scores the remainder according to heading, clearance, and speed. It is practical for differential-drive robots, but its result depends on sensing range, prediction assumptions, sampling resolution, and scoring weights.

Uncertainty and safety margins

Sensor noise, localization error, actuation delay, and unmodelled geometry reduce actual clearance. A safety margin should reflect uncertainty and braking distance, not be selected arbitrarily. If a robot travels at speed v and can brake with constant deceleration magnitude a > 0, the ideal stopping distance is

This relation assumes immediate braking, constant deceleration, level motion, and no reaction delay. With reaction time τ, the distance becomes

The margin must also include perception and localization uncertainty. A planner that treats a noisy obstacle boundary as exact may be formally collision-free in simulation but unsafe in operation.

6. Task Planning and Scheduling

Motion planning answers “how can the robot move?” Task planning answers “what should it do, and in what order?” A task plan may include symbolic actions such as Navigate(room A), Pick(part), Inspect(part), and Place(part, fixture).

An action can be described by:

• preconditions: facts that must hold before execution;
• effects: facts made true or false after execution;
• resources: tools, robots, workspace, or energy required; and
• duration or temporal constraints.

For example, Pick(part) may require that the robot is at the part, the gripper is empty, and the part is reachable. Its effects may include gripper holding part and part no longer on the table.

Ordering constraints

If action A must precede B, then a precedence constraint is written A → B. A directed acyclic graph can represent these constraints. A topological ordering provides a feasible sequence when no cycle exists. If A requires an object produced by B, the precedence is mandatory; otherwise, the order may be chosen to minimize travel or setup cost.

Scheduling

Scheduling assigns actions to times and resources. For a single machine with non-overlapping tasks, completion time is the sum of durations:

where pi is the processing time of task i. In realistic robot cells, tasks may overlap: one robot moves while a conveyor operates, but two robots may not occupy the same restricted zone simultaneously.

A schedule must consider setup time, battery charging, tool changes, precedence, deadlines, and collision constraints. A mathematically short schedule may be operationally poor if it leaves no recovery time or repeatedly approaches safety limits.

Task and motion planning (TAMP) integrates symbolic actions with geometric feasibility. A symbolic plan may fail because a grasp is unreachable, a corridor is too narrow, or a required placement configuration is in collision. TAMP therefore alternates between discrete reasoning and continuous feasibility checks, or searches over both levels jointly.

7. Multi-Robot Planning

Multiple robots introduce shared-space, communication, and coordination problems. A valid plan must ensure that robots do not collide with one another and that shared resources are allocated consistently.

A simple strategy is prioritized planning: assign an order to robots, plan the first robot, and treat its trajectory as a moving obstacle for later robots. This is fast but incomplete; a poor priority order may make a solvable problem appear impossible.

Centralized planning searches the joint configuration space:

where Qi is the configuration space of robot i. This approach can represent interactions accurately, but dimensionality grows rapidly with N. Decentralized methods are more scalable but require communication, prediction, and conflict-resolution rules.

Coordination can be implemented using reservations, time-expanded graphs, velocity-obstacle methods, or traffic-management zones. Safety-critical systems should define right-of-way rules, emergency stopping behaviour, and communication-loss responses before deployment.

Performance should be evaluated using more than throughput. Relevant measures include collision and near-miss rate, deadlock frequency, makespan, energy use, fairness of task allocation, recovery time, and the number of manual interventions.

8. Illustrative Case: Warehouse Mobile Manipulators

Background

Consider an illustrative warehouse containing two autonomous mobile manipulators. Each robot collects a container from storage, transports it through aisles, and places it at an inspection station. The environment contains static shelving, pedestrians, charging points, and a narrow shared intersection.

Problem

A shortest-path planner produced collisions in simulation because the robot body was represented as a point. It also generated routes that were infeasible for the differential-drive base and allowed both robots to enter the shared intersection simultaneously.

Analysis

The engineering team first modelled each robot as a rectangle and inflated obstacles by the robot footprint plus a 0.15 m uncertainty margin. The global planner used A* on an occupancy grid. Diagonal motion was allowed only when the swept rectangular footprint remained collision-free. The local controller used a dynamic-window method to respond to pedestrians and temporary obstructions.

The base’s nonholonomic constraint was included in local trajectory evaluation. Candidate velocities were rejected if predicted motion violated stopping-distance clearance. At the task level, the system represented pickup, transport, inspection, and return-to-charge actions with preconditions and durations.

The shared intersection was declared a resource with capacity one. A reservation scheduler assigned time intervals to robots. If a robot was delayed, the reservation was released and recomputed rather than allowing it to continue under an obsolete schedule.

Approach and solution

The complete pipeline was:

1. localize the robot and update the occupancy map;
2. inflate static and detected obstacles by footprint and uncertainty margin;
3. compute an A* route to the next workstation;
4. generate a feasible local trajectory with velocity and acceleration limits;
5. reserve the shared intersection before entry;
6. monitor clearance, localization confidence, and actuator status;
7. replan if an obstacle, delay, or communication fault invalidates assumptions.

Outcome

The revised system produced slightly longer routes but fewer emergency stops and no planned intersection conflicts during validation trials. The principal improvement was not a single algorithm; it was the integration of geometry, timing, uncertainty, and resource constraints.

Learning points

A warehouse planner must model the actual robot footprint, not only its reference point. Global optimality is less important than robust execution when people and vehicles create uncertainty. Multi-robot scheduling must be connected to motion execution because a delayed robot can invalidate a time-based plan.

9. Validation, Reliability, and Professional Practice

Planning software should be tested at several levels. Unit tests verify collision geometry, coordinate transformations, heuristic calculations, and trajectory limits. Scenario tests evaluate narrow passages, blocked goals, sensor dropouts, and moving obstacles. Hardware-in-the-loop tests expose timing and communication problems. Field trials should begin with low speed and large clearances, followed by controlled increases in complexity.

Important metrics include:

• success probability over repeated trials;
• planning time and replanning time;
• path length and execution duration;
• minimum obstacle clearance;
• maximum velocity, acceleration, and jerk;
• energy consumption;
• recovery and emergency-stop frequency; and
• human intervention rate.

A planner should report failure rather than silently return an unsafe or incomplete route. Logs should record map versions, sensor confidence, planner parameters, selected trajectory, and reasons for replanning. This supports debugging, maintenance, and accountability.

Ethically and legally, a robot operator or designer has a duty to prioritize human safety, provide understandable operating boundaries, and avoid deploying a planner outside its validated assumptions. Personal data from cameras and tracking systems must be collected proportionately and protected. Autonomous decisions should remain auditable, especially when robots operate near workers or make allocation decisions affecting people.

10. Contemporary Developments and Limitations

Learning-based planners can estimate costs, predict pedestrian motion, select sampling distributions, or imitate expert trajectories. However, learned components may fail under distribution shift and can be difficult to verify. A strong engineering pattern is to use learning for prediction or heuristic guidance while retaining explicit collision checking, constraint enforcement, and emergency control.

Optimization-based planners formulate motion as a constrained minimization problem. They can produce smooth trajectories and incorporate dynamics, but nonlinear optimization may converge to local minima and depends strongly on initialization. Hybrid systems increasingly combine graph search for global structure, sampling for difficult geometric feasibility, and optimization for smoothing and timing.

No planner is universally best. Grid methods are transparent and reliable in low-dimensional mapped environments; sampling methods handle high-dimensional geometry; optimization produces high-quality trajectories; reactive methods respond quickly to unforeseen events. System design should therefore match the method to environment structure, computational resources, safety requirements, and the consequences of failure.

Chapter Summary

Motion planning represents robot movement in configuration space, where collisions become forbidden configurations. Obstacle inflation converts finite robot geometry into point planning for suitable cases, but the robot footprint, orientation, and nonholonomic constraints must be respected.

Graph-search methods such as Dijkstra’s algorithm and A operate on discretized representations. A uses g(n)+h(n), and an admissible heuristic preserves graph optimality. Sampling-based methods such as PRM, RRT, and RRT* are valuable in high-dimensional spaces, with different strengths concerning repeated queries, single-query problems, and asymptotic path quality.

A path must be converted into a time-parameterized trajectory that satisfies velocity, acceleration, smoothness, and actuator constraints. Navigation combines global planning, local obstacle avoidance, localization, mapping, and feedback. Safety margins must account for uncertainty and stopping distance.

Task planning specifies action order, preconditions, effects, resources, and temporal constraints. Task-and-motion planning connects symbolic decisions with geometric feasibility. Multi-robot systems require collision avoidance, scheduling, communication, right-of-way rules, and recovery from delays. Effective professional practice validates planners systematically, records decisions, and treats safety, privacy, and accountability as design requirements.

Key Terms

Configuration space: The space of all robot configurations.

Free configuration space: The set of configurations that do not cause collision.

Configuration-space obstacle: The set of configurations producing contact with an obstacle.

Minkowski sum: A geometric operation used to inflate obstacles by a robot footprint.

Holonomic constraint: A constraint expressible solely in terms of configuration variables.

Nonholonomic constraint: A motion constraint involving velocities that limits instantaneous movement.

Graph search: Systematic exploration of nodes and edges representing possible states and transitions.

A*: A heuristic graph-search algorithm using the evaluation f(n)=g(n)+h(n).

Admissible heuristic: A heuristic that never overestimates remaining cost.

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

RRT: A sampling-based tree planner that explores configuration space toward random samples.

RRT*: An RRT variant with rewiring and asymptotic optimality under standard assumptions.

Path: A geometric sequence of configurations without explicit timing.

Trajectory: A time-parameterized path.

Retiming: Assigning time and velocity to a geometric path.

Potential field: A scalar field producing attractive and repulsive navigation influences.

Velocity obstacle: A set of velocities predicted to cause collision with another moving object.

Task-and-motion planning: Integrated planning over symbolic actions and continuous robot motion.

Makespan: The time at which the complete scheduled set of tasks finishes.

Prioritized planning: Multi-robot planning in which robots are planned sequentially according to priority.

Review Questions

Short Answer Questions

1. What is the difference between configuration space and workspace?
2. Why are obstacles inflated when planning for a circular robot?
3. What conditions make a heuristic admissible for A*?
4. Distinguish a geometric path from a trajectory.
5. Why can potential-field navigation become trapped?

Long Answer Questions

1. Explain how a differential-drive robot’s nonholonomic constraint affects motion planning.
2. Compare A, PRM, RRT, and RRT with respect to representation, completeness, computation, and application.
3. Derive and explain the cubic trajectory used for zero initial and final velocity.
4. Describe how uncertainty, reaction time, and braking limits should influence obstacle-clearance margins.
5. Explain why task-and-motion planning is necessary for manipulation tasks involving reachability and grasping.

Analytical / Application Questions

1. Design a planning architecture for a mobile robot operating in a hospital with static walls, moving people, narrow corridors, and restricted areas. Justify the global planner, local planner, safety checks, and replanning policy.
2. Two robots must use a single narrow loading bay. Develop a scheduling and motion-coordination strategy that prevents collision, handles delays, and avoids indefinite starvation of either robot.
3. A planned route is collision-free for a point robot but intersects a shelf when the robot footprint is included. Analyse the modelling error and propose at least two corrective methods, discussing their computational and operational trade-offs.

Multiple Choice Questions with Answers

Question 1

Which set best describes a robot configuration for a planar rigid body?

A. Only its mass and battery level

B. Its x-position, y-position, and orientation

C. Its wheel voltage and motor current only

D. The number of obstacles in the environment

Correct Answer: B. Its x-position, y-position, and orientation

Question 2

For a circular robot translating without rotation, what operation correctly represents obstacle inflation?

A. Minkowski sum of the obstacle with a disk

B. Division of the obstacle area by the robot mass

C. Rotation of the obstacle by 90°

D. Projection of the obstacle onto the time axis

Correct Answer: A. Minkowski sum of the obstacle with a disk

Question 3

In A*, what does f(n)=g(n)+h(n) represent?

A. Only the measured distance from the start

B. Only the estimated distance to the goal

C. Estimated total cost through node n

D. The robot’s maximum acceleration

Correct Answer: C. Estimated total cost through node n

Question 4

Which statement about an admissible A* heuristic is correct?

A. It always overestimates the remaining cost

B. It never overestimates the remaining cost

C. It must equal zero at every node

D. It is applicable only to moving obstacles

Correct Answer: B. It never overestimates the remaining cost

Question 5

What is a principal advantage of sampling-based planners in robotics?

A. They require no collision checking

B. They handle high-dimensional configuration spaces effectively

C. They always return the shortest path immediately

D. They eliminate all actuator constraints

Correct Answer: B. They handle high-dimensional configuration spaces effectively

Question 6

A robot travels at 2 m/s and can brake at 1 m/s², with zero reaction delay. What is its ideal stopping distance?

A. 1 m

B. 2 m

C. 4 m

D. 8 m

Correct Answer: B. 2 m

Question 7

Why must a geometric path be converted into a trajectory before execution?

A. Motors require time-dependent position and velocity commands

B. A path automatically specifies battery chemistry

C. A trajectory removes the need for collision checking

D. Geometric paths can be used only by stationary robots

Correct Answer: A. Motors require time-dependent position and velocity commands

Question 8

Which issue is characteristic of basic potential-field navigation?

A. It cannot represent a goal

B. It may become trapped in a local minimum

C. It requires a complete joint-space grid in every case

D. It guarantees a globally shortest path

Correct Answer: B. It may become trapped in a local minimum

Question 9

In task planning, what is a precondition?

A. A fact that must hold before an action can execute

B. The final battery percentage after execution

C. A random sample used by RRT

D. The robot’s maximum wheel diameter

Correct Answer: A. A fact that must hold before an action can execute

Question 10

What is the main weakness of prioritized multi-robot planning?

A. It cannot represent any obstacles

B. It always requires joint configuration-space search

C. A poor priority order can make a solvable problem appear impossible

D. It prevents robots from using schedules

Correct Answer: C. A poor priority order can make a solvable problem appear impossible