8. Trajectory Generation and Motion Planning

Introduction and Context

A robot is useful not merely because it can reach a position, but because it can move through space safely, smoothly, accurately, and at an acceptable cost. The problem of deciding where a robot should move is commonly divided into two related problems. Motion planning determines a collision-free geometric route from an initial configuration to a goal configuration. Trajectory generation assigns time to that route, producing position, velocity, and acceleration profiles that satisfy the robot’s physical and control constraints.

The distinction is important. A geometrically valid path may contain sharp corners, excessive curvature, or velocities that a motor cannot achieve. Conversely, a dynamically smooth trajectory may be unsafe if the planned route intersects an obstacle. Industrial robot programming, autonomous vehicles, mobile manipulators, warehouse systems, surgical robots, and collaborative robots all require an integrated treatment of geometry, time, dynamics, sensing, and control.

This chapter develops methods for joint-space and Cartesian trajectory generation, configuration-space reasoning, graph-search and sampling-based planning, optimization-based planning, collision avoidance, scheduling, and multi-robot coordination. The emphasis is on selecting an appropriate method rather than treating any single algorithm as universally superior.

Learning Objectives

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

1. Distinguish a path from a time-parameterized trajectory.

2. Generate smooth polynomial, spline, and Cartesian trajectories.

3. Explain configuration space and the role of the robot’s body geometry.

4. Compare graph-search, sampling-based, and optimization-based planners.

5. Select collision-checking and replanning strategies for practical systems.

6. Formulate scheduling and coordination problems for multiple robots.

7. Evaluate trajectories using safety, smoothness, time, energy, and robustness criteria.

8. Recognize implementation risks, ethical responsibilities, and emerging digital methods.

Path, Trajectory, and Configuration

A robot configuration is a complete specification of its pose and internal variables. For an n-joint manipulator, the configuration is represented by

q = [q₁, q₂, …, qₙ]ᵀ.

The set of all allowable configurations is the configuration space, or C-space. Joint limits, mechanical restrictions, and self-collision constraints define the feasible portion of this space. A path is a continuous curve q(s), where s is a path parameter, usually normalized from zero to one. A trajectory is a path with a time law:

q(t) = q(s(t)).

Velocity and acceleration follow from the chain rule:

q̇ = (dq/ds)ṡ,

q̈ = (d²q/ds²)ṡ² + (dq/ds)s̈.

This separation is useful because a geometric planner can first find a collision-free path and a time-scaling method can subsequently enforce velocity, acceleration, and sometimes jerk limits. In demanding applications, however, geometry and timing cannot be completely separated: dynamic obstacles, torque limits, energy consumption, and coordinated robots require a coupled formulation.

A trajectory is judged by more than endpoint accuracy. Typical criteria include execution time, maximum velocity, acceleration and jerk, path length, energy, clearance from obstacles, sensitivity to modelling error, and computational effort. A short path with poor clearance may be inferior to a slightly longer path that is robust to uncertainty.

Smooth Joint-Space Trajectories

Joint-space planning specifies the motion directly in the variables controlled by the actuators. It is computationally convenient and naturally respects joint limits. It is particularly suitable for point-to-point tasks in which the end effector need not follow a prescribed spatial curve.

Polynomial interpolation

For a move between q₀ and qf over duration T, a cubic polynomial can be written as

q(t) = a₀ + a₁t + a₂t² + a₃t³.

If the initial and final velocities are specified, the four coefficients are determined by q(0), q(T), q̇(0), and q̇(T). Cubic interpolation provides continuous position and velocity, but acceleration generally changes abruptly at the boundaries.

A quintic polynomial adds two degrees of freedom:

q(t) = a₀ + a₁t + a₂t² + a₃t³ + a₄t⁴ + a₅t⁵.

It can satisfy position, velocity, and acceleration conditions at both endpoints. Consequently, it is commonly used when smooth acceleration is required. Jerk, the derivative of acceleration, may still be discontinuous at segment transitions.

For a rest-to-rest quintic motion with normalized time τ = t/T, the familiar smoothstep form is

q(τ) = q₀ + (qf − q₀)(10τ³ − 15τ⁴ + 6τ⁵).

The duration T must be selected so that all joints remain within their velocity and acceleration limits. Applying the same duration to every joint is simple; selecting a common duration from the most restrictive joint preserves synchronization.

Trapezoidal and S-curve velocity profiles

A trapezoidal profile accelerates at a constant rate, travels at constant velocity, and decelerates at a constant rate. It is easy to implement and provides explicit velocity and acceleration limits. However, acceleration changes instantaneously at the phase boundaries, producing theoretically infinite jerk.

An S-curve profile smooths the acceleration transitions by limiting jerk. It is preferable for fragile payloads, high-speed mechanisms, precision assembly, and systems in which vibration must be controlled. The additional planning complexity is justified when structural resonance, actuator wear, or payload stability is important.

Via points and splines

Tasks such as welding, painting, and inspection often require the robot to pass through intermediate via points. Independent polynomial segments can produce discontinuities in velocity or acceleration. Cubic splines impose continuity across segments, while higher-order splines can impose continuity of acceleration or jerk. The designer must avoid excessive interpolation points: closely spaced noisy points can create unnecessary oscillation and high actuator activity.

Cartesian Trajectories and Inverse Kinematics

When tool orientation and position must follow a specified path, planning in Cartesian space is more natural. A pose is represented by

x = [pᵀ, orientation]ᵀ,

where p is position and orientation may be expressed by a rotation matrix, quaternion, axis-angle vector, or another minimal representation.

Linear interpolation of position is straightforward:

p(s) = (1 − s)p₀ + spf.

Orientation interpolation must preserve valid rotations. Direct interpolation of rotation-matrix elements is generally inappropriate. Unit quaternions with spherical linear interpolation, or SLERP, provide constant-speed interpolation on the unit quaternion sphere and avoid many singularities associated with Euler angles.

A Cartesian trajectory is converted to joint motion through inverse kinematics. If x = f(q), then locally

ẋ = J(q)q̇,

where J is the manipulator Jacobian. Away from singularities, q̇ = J⁻¹ẋ for a square nonsingular Jacobian. For redundant or near-singular robots, the pseudoinverse and null-space methods are used:

q̇ = J⁺ẋ + (I − J⁺J)q̇₀.

The second term allows secondary objectives such as joint-limit avoidance, posture selection, or obstacle clearance. Near singularities, damped least squares is safer than an unregularized pseudoinverse because it limits excessive joint velocities.

Cartesian planning does not automatically guarantee a feasible joint trajectory. Inverse kinematics may have multiple branches, discontinuities, or no solution for some poses. Therefore, solutions must be checked for joint limits, collision, continuity, singularity proximity, and actuator constraints.

Configuration-Space Reasoning and Collision Avoidance

Collision avoidance becomes conceptually simpler when obstacles are transformed into configuration space. Instead of moving a finite-size robot around a point obstacle, one can imagine expanding the obstacle by the robot’s geometry and moving a representative point. The expanded region is the configuration-space obstacle. A path is valid if it remains in the free configuration space.

For a high-degree-of-freedom manipulator, explicitly constructing C-space obstacles is usually impractical. Collision detection is therefore performed implicitly using geometric models such as bounding spheres, capsules, convex hulls, voxel maps, or signed-distance fields. Broad-phase tests eliminate obviously separated pairs, while narrow-phase tests calculate accurate distances or intersections.

A safety margin is essential because models, calibration, sensing, and control are imperfect. If the estimated obstacle position has uncertainty, the forbidden region should be inflated by an appropriate margin. Excessive inflation reduces accessibility; insufficient inflation creates unacceptable risk. The margin should reflect speed, stopping distance, sensor uncertainty, control error, and the consequences of contact.

A useful trajectory cost may combine several objectives:

J = w₁T + w₂L + w₃E + w₄R + w₅C,

where T is time, L path length, E energy, R a risk or clearance penalty, and C a smoothness measure. The weights express application priorities. In safety-critical systems, collision risk is normally treated as a hard constraint rather than merely a low-weight cost.

Planning Methods: From Discrete Search to Continuous Optimization

Graph-search methods

Grid-based planning discretizes the workspace or configuration space into cells and uses algorithms such as breadth-first search, Dijkstra’s algorithm, or A. A combines the accumulated cost with a heuristic estimate of the remaining cost. With an admissible heuristic, it can find a minimum-cost path on the graph. Grid methods are transparent and effective in low-dimensional spaces, but memory requirements grow rapidly with dimension and grid resolution. Coarse grids may miss narrow passages; fine grids may be computationally prohibitive.

For manipulators, planning can also use a lattice of feasible joint configurations or motion primitives. This improves feasibility but requires careful construction of the primitive set. Graph search is especially effective when the environment is structured, repeatedly encountered, or represented by a reliable occupancy map.

Sampling-based methods

Sampling-based planners avoid explicit decomposition of high-dimensional free space. Probabilistic Roadmaps (PRM) sample collision-free configurations, connect nearby samples with local planners, and search the resulting graph. PRM is well suited to repeated queries in a largely static environment.

Rapidly-exploring Random Trees (RRT) grow a tree from the start by sampling a configuration and extending from the nearest existing node. RRT is effective for single-query problems and difficult geometries. RRT-Connect grows trees from both the start and goal and attempts to join them. RRT* rewires the tree to improve path cost and is asymptotically optimal under suitable assumptions, although convergence may be slow.

Sampling methods are probabilistically complete: if a solution exists and sampling continues under appropriate conditions, the probability of finding one approaches one. This does not mean that a particular finite run will find the best path. Post-processing through shortcutting, spline fitting, or trajectory optimization is often needed to produce a smooth executable result.

Optimization-based planning

Optimization methods represent a trajectory by waypoints, spline coefficients, or control inputs and minimize a cost subject to constraints. A typical formulation is

minimize J(q, q̇, q̈)

subject to dynamics, boundary conditions, joint limits, collision constraints, and actuator limits.

Sequential quadratic programming, nonlinear programming, CHOMP-like functional optimization, covariant gradient methods, and stochastic optimizers are examples of approaches used in practice. Optimization can produce smooth, efficient trajectories and incorporate many application-specific objectives. Its principal weakness is local minima: the solution may remain trapped on the wrong side of an obstacle or fail because the initial guess is infeasible.

A practical architecture often combines methods. A sampling or graph planner supplies a collision-free initial path; shortcutting and smoothing reduce unnecessary motion; trajectory optimization then enforces dynamics, clearance, and smoothness. This hybrid strategy balances global exploration with local refinement.

Time Parameterization, Scheduling, and Execution

Once a geometric path is selected, time parameterization determines how quickly it is traversed. The procedure must enforce limits on q̇, q̈, jerk, torque, and sometimes motor temperature. Time-optimal motion is not always desirable: aggressive motion can increase vibration, wear, energy consumption, and collision risk.

In a production cell, trajectory generation is linked to scheduling. Tasks may involve loading, machining, inspection, tool changing, and unloading. A schedule assigns operations to resources and determines precedence relationships, while motion planning determines whether each scheduled movement is feasible. A theoretically optimal schedule is useless if its robot motions are mutually blocking or require impossible accelerations.

For multiple robots, collision avoidance includes both workspace and temporal conflicts. Two robots may use the same region at different times, so coordination can be achieved through time windows, priorities, reservations, velocity adjustment, or centralized multi-agent planning. Centralized planning captures interactions accurately but can become computationally expensive. Decentralized methods scale better but require communication, conflict-resolution rules, and guarantees against deadlock.

A reservation-table approach divides shared workspace resources into spatial regions and time intervals. A robot reserves a region before entering it; another robot must wait, reroute, or negotiate. Priority rules should be designed carefully, since fixed priorities can cause starvation. Deadlock detection and recovery are essential in tightly coupled cells.

An Illustrative Industrial Case: Coordinated Bin-Picking Cell

Background

Consider an illustrative automated warehouse cell containing two six-axis manipulators, a conveyor, a depth camera, and a shared packing zone. Each robot picks randomly oriented parts from a bin and places them in cartons. The robots have overlapping reachable regions near the conveyor.

Situation and problem

Independent point-to-point joint trajectories were initially generated for each pick-and-place cycle. Although each individual path was collision-free against static equipment, the robots occasionally entered the shared zone simultaneously. The system also experienced abrupt wrist motions because inverse-kinematic solutions changed branches between successive parts.

Analysis

The problem contained three coupled issues: uncertainty in object pose, continuity of inverse-kinematic solutions, and temporal coordination. Treating the robots independently ignored dynamic obstacles created by the other robot. Increasing speed worsened the risk because braking distance and prediction error increased.

Approach

A voxelized environment model was used for coarse collision checking, followed by accurate mesh checks near the shared zone. Each robot generated several candidate grasps and inverse-kinematic solutions. A PRM supplied collision-free global routes around the bin and equipment. Candidate paths were smoothed and time-parameterized with jerk-limited profiles.

The shared zone was represented as a reservable resource. A scheduling layer assigned entry windows according to carton demand and current robot state. If a robot could not obtain a safe time window, it selected an alternative path or waited at a designated staging pose. A null-space posture objective discouraged joint-limit proximity, while a continuity penalty favored the inverse-kinematic branch closest to the previous configuration.

Outcome

The resulting system accepted a modest increase in average path length in exchange for greater clearance and fewer abrupt reversals. The important improvement was architectural: perception, global planning, trajectory smoothing, and coordination were treated as connected stages rather than independent programs.

Learning points

The case demonstrates that collision avoidance is not only a geometric problem. Robust performance requires uncertainty margins, continuous kinematic branch selection, time-aware coordination, and recovery policies. It also illustrates why a slightly slower trajectory may produce higher throughput when interruptions and faults are reduced.

Implementation Challenges and Common Errors

Several errors recur in practical systems. Planning only the end-effector point can allow the wrist, elbow, cable bundle, or payload to collide. Checking collision only at discrete waypoints can miss intersection between them; continuous or sufficiently dense segment checking is required. Treating a planned path as a trajectory ignores velocity and acceleration limits. Using Euler angles without considering representation singularities can create artificial orientation jumps.

Other failures arise from stale environmental data, poor calibration, unmodelled tool geometry, and inconsistent coordinate frames. A planner may report success while the controller cannot track the result because the commanded motion exceeds torque or bandwidth limits. Computational time must also be considered: a planner that takes longer than the available cycle time is not operationally useful.

Recovery is part of planning. A system should define safe stopping, retreat, replanning, human notification, and restart procedures. It should distinguish sensor uncertainty from genuine obstacle motion and should log planner decisions for diagnosis and validation.

Professional, Ethical, and Governance Considerations

Robot motion affects worker safety, product quality, property, and sometimes patient welfare. Safety functions should not depend solely on a probabilistic planner. Independent protective measures—such as guarded zones, certified safety-rated monitoring, emergency stops, speed limits, and validated separation distances—remain necessary where regulations and risk assessments require them.

Engineers must document assumptions, model fidelity, validation tests, operating envelopes, and known failure modes. In collaborative applications, the robot’s speed and force limits should be selected from a formal risk assessment rather than convenience. Data-driven planners also raise governance issues: sensor data may reveal worker behaviour or facility layouts, and automated decisions should be auditable. Human override, clear responsibility, and cybersecurity controls are important whenever planning software is connected to production networks.

Digital Planning and Emerging Directions

Modern systems increasingly combine 3-D perception, digital twins, GPU-accelerated collision checking, and cloud or edge computation. Signed-distance representations allow planners to estimate clearance and gradients efficiently. Learning-based methods can predict good initial paths, grasp choices, or cost functions, but they should generally be constrained by verified collision and safety layers.

Model predictive control provides a rolling-horizon alternative to one-shot planning. It repeatedly predicts future motion, optimizes a short horizon, executes a portion, and replans as new sensor information arrives. This is valuable for moving obstacles and uncertain dynamics, although computational deadlines and stability must be managed.

Multi-robot systems are moving toward distributed optimization, negotiation protocols, and task-and-motion planning, in which symbolic task decisions and continuous geometric motions are solved together. The principal research challenge is combining the adaptability of learning with the predictability required for safety-critical automation.

Best-Practice Design Procedure

A robust planning workflow is as follows:

1. Define start and goal states, task tolerances, payload, and operating constraints.

2. Build consistent robot, tool, workspace, and obstacle models.

3. Select joint-space or Cartesian representation according to task requirements.

4. Choose a global planner appropriate to dimension, obstacle structure, and query frequency.

5. Check the complete robot and payload for collision with uncertainty margins.

6. Smooth and time-parameterize the path under velocity, acceleration, jerk, torque, and synchronization limits.

7. Validate singularities, joint-limit proximity, tracking feasibility, and stopping behaviour.

8. Coordinate shared resources and define priority, waiting, rerouting, and recovery rules.

9. Test nominal, uncertain, degraded-sensor, and emergency conditions in simulation and on the real system.

10. Monitor execution and replan when environmental or robot-state assumptions become invalid.

Chapter Summary

Trajectory generation assigns time to robot motion, whereas motion planning determines a feasible route through configuration space. Joint-space interpolation is convenient for point-to-point movement; Cartesian interpolation is preferable when tool position or orientation must follow a specified path. Cubic and quintic polynomials, splines, trapezoidal profiles, and jerk-limited S-curves provide different smoothness and implementation characteristics.

Collision avoidance requires considering the whole robot, payload, uncertainty, and dynamic obstacles. Graph-search methods are effective in structured low-dimensional representations, sampling-based planners handle high-dimensional geometry, and optimization methods refine smoothness and efficiency but may encounter local minima. Hybrid planning is often most practical. Time parameterization, scheduling, and multi-robot coordination extend geometric planning into operational decision-making. Safe deployment requires validation, independent protective measures, recovery strategies, documentation, and careful use of digital and learning-based tools.

Key Terms

Configuration space: Space whose coordinates represent all robot configurations.

Configuration-space obstacle: Region of configurations that produce collision.

Path: Geometric curve connecting initial and goal configurations.

Trajectory: A path together with a time parameterization.

Via point: Intermediate state through which a trajectory is required to pass.

Jerk: Time derivative of acceleration.

Time scaling: Assignment of time to a geometric path.

Jacobian: Matrix mapping joint velocities to task-space velocities.

Singularity: Configuration in which the manipulator loses or severely limits motion authority in some direction.

Pseudoinverse: Generalized inverse used to obtain least-squares joint velocities.

Null-space motion: Robot motion that does not change the primary task output.

Configuration-space obstacle inflation: Geometric expansion accounting for robot or uncertainty size.

PRM: Sampling-based planner that builds a reusable roadmap.

RRT: Sampling-based planner that incrementally grows a tree through free space.

A*: Graph-search algorithm combining path cost and a heuristic estimate.

Trajectory optimization: Numerical optimization of a parameterized motion subject to constraints.

Clearance: Distance between the robot and an obstacle.

Reservation table: Spatial-temporal record of resources reserved by robots.

Model predictive control: Receding-horizon optimization with repeated replanning.

Task-and-motion planning: Integrated planning of symbolic tasks and continuous robot motions.

Review Questions

Short Answer Questions

1. Distinguish a path from a trajectory.

2. Why is configuration space useful for collision avoidance?

3. State one advantage and one limitation of quintic interpolation.

4. What is the purpose of the Jacobian pseudoinverse?

5. Why are jerk-limited profiles used in industrial robots?

Long Answer Questions

1. Explain joint-space and Cartesian trajectory generation, including inverse-kinematic and singularity issues.

2. Compare graph-search, sampling-based, and optimization-based motion planners.

3. Describe a complete collision-checking and trajectory-validation workflow.

4. Explain how scheduling and reservation methods support multi-robot coordination.

5. Discuss the role of uncertainty margins, safety systems, and recovery planning in robot deployment.

Analytical / Application Questions

1. A six-axis robot must move a heavy payload between two stations while avoiding a narrow obstacle. Select a planning and time-parameterization strategy and justify it.

2. Two robots repeatedly deadlock in a shared assembly zone. Formulate a coordination policy involving priorities, reservations, waiting, and rerouting.

3. A Cartesian path produces rapidly changing joint angles near a singularity. Diagnose the cause and propose a revised planning and control approach.

22. Multiple Choice Questions with Answers

Question 1

Which statement best distinguishes a path from a trajectory?

A. A path includes actuator torque, whereas a trajectory does not

B. A path is geometric, whereas a trajectory also specifies timing

C. A path is always collision-free, whereas a trajectory is not

D. A path applies only to mobile robots

Correct Answer: B. A path is geometric, whereas a trajectory also specifies timing

Question 2

Why is a quintic polynomial often preferred to a cubic polynomial for rest-to-rest motion?

A. It requires no boundary conditions

B. It guarantees global collision avoidance

C. It can impose position, velocity, and acceleration conditions at both endpoints

D. It eliminates the need for inverse kinematics

Correct Answer: C. It can impose position, velocity, and acceleration conditions at both endpoints

Question 3

A robot must keep its tool orientation and position on a prescribed curve. Which planning representation is most appropriate initially?

A. Cartesian space

B. Only actuator-current space

C. A random joint configuration

D. A scheduling graph without geometry

Correct Answer: A. Cartesian space

Question 4

What is the principal purpose of inflating obstacles in configuration-space reasoning?

A. To increase robot speed

B. To represent the finite size of the robot and safety uncertainty

C. To remove joint limits

D. To guarantee optimal energy use

Correct Answer: B. To represent the finite size of the robot and safety uncertainty

Question 5

Which planner is especially suited to repeated queries in a mostly static environment?

A. Probabilistic Roadmap

B. Emergency stop controller

C. Single cubic interpolation

D. Fixed-priority scheduler alone

Correct Answer: A. Probabilistic Roadmap

Question 6

A sampling-based planner finds a collision-free but unnecessarily long path. Which subsequent operation is most appropriate?

A. Remove all collision checks

B. Apply shortcutting, smoothing, or trajectory optimization

C. Increase joint velocities without changing the path

D. Replace the robot model with a point

Correct Answer: B. Apply shortcutting, smoothing, or trajectory optimization

Question 7

Near a manipulator singularity, why can an unregularized Jacobian inverse be dangerous?

A. It always stops the robot

B. It can produce excessively large joint velocities

C. It prevents Cartesian motion completely in every direction

D. It changes obstacle geometry

Correct Answer: B. It can produce excessively large joint velocities

Question 8

Two robots need the same workspace region at different times. Which method directly represents this conflict?

A. A spatial-temporal reservation table

B. Independent endpoint interpolation only

C. Removing the region from both workspaces permanently

D. Increasing camera resolution alone

Correct Answer: A. A spatial-temporal reservation table

Question 9

Which is a major limitation of local trajectory optimization?

A. It cannot represent smooth trajectories

B. It may become trapped in a local minimum or require a good initial path

C. It cannot include joint limits

D. It is applicable only to one-dimensional robots

Correct Answer: B. It may become trapped in a local minimum or require a good initial path

Question 10

Why should a planned trajectory be validated against actuator limits?

A. Geometric feasibility does not guarantee executable velocity, acceleration, torque, or tracking requirements

B. Actuator limits determine the colour of the robot

C. Collision checking is unnecessary when torque is limited

D. Actuator limits replace the need for scheduling

Correct Answer: A. Geometric feasibility does not guarantee executable velocity, acceleration, torque, or tracking requirements