Trajectory Generation and Motion Planning

Introduction

A robot does not become useful merely by reaching a desired pose. It must move there safely, smoothly, accurately, and within the limits of its actuators, joints, sensors, and environment. Motion planning determines a feasible path through the robot's configuration space, while trajectory generation assigns time to that path and specifies position, velocity, acceleration, and sometimes jerk as functions of time.

The distinction is important. A geometric path may avoid obstacles but demand impossible accelerations. Conversely, a time trajectory may be dynamically feasible but intersect an obstacle. Practical robotic systems therefore combine configuration-space modeling, collision detection, path-search or sampling-based planning, time parameterization, and feedback control.

Learning Objectives

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

1. Distinguish a path from a time-parameterized trajectory.
2. Formulate robot motion in configuration space.
3. Generate smooth joint-space and Cartesian trajectories.
4. Explain collision detection and configuration-space obstacles.
5. Apply graph-search methods such as A* and sampling-based methods such as PRM and RRT.
6. Recognize constraints involving joint limits, velocity, acceleration, torque, and obstacle clearance.
7. Evaluate and improve a planned motion using optimization and practical validation.

1. Paths, Trajectories, and Motion-Planning Layers

For an n-degree-of-freedom robot, a configuration is represented by

q = [q₁, q₂, …, q_n]^T. The configuration space, or C-space, is the set of all configurations permitted by the robot's mechanical structure. A path is a geometric curve q(s), where s is a path parameter. A trajectory is a time-dependent motion q(t). Time parameterization relates the two through s = s(t) and therefore q(t) = q(s(t)).

A complete planning system commonly contains these layers:

• Task specification: desired end-effector poses, tolerances, and timing.
• Geometric planning: a collision-free path.
• Time parameterization: velocity and acceleration profiles.
• Dynamic validation: torque, power, and actuator checks.
• Feedback execution: tracking using measured joint states.

A path planner should not be expected to solve all control and mechanical problems. Planning and control are related but distinct: planning predicts a feasible reference, whereas control compensates for modeling errors, disturbances, and tracking errors.

2. Configuration-Space Modeling

2.1 Joint-space representation

In joint space, each coordinate is a joint variable. Revolute-joint coordinates are angles, measured in radians, and prismatic-joint coordinates are displacements, measured in metres. A typical joint-limit constraint is

where q_i^min and q_i^max are mechanical or software limits. The feasible configuration set must also exclude self-colliding configurations and configurations that place the robot in collision with its environment.

For a two-link planar manipulator, forward kinematics are

where l₁ and l₂ are link lengths. These equations map joint configurations to an end-effector position. In general, forward kinematics are written x = f(q), where x may include position and orientation.

2.2 Workspace and configuration space

The workspace describes where the end effector can be located. The configuration space describes the complete robot state required to determine collision and mechanical feasibility. Two different joint configurations can produce the same end-effector position but different link locations; consequently, workspace planning alone can miss collisions.

An obstacle in physical space becomes a configuration-space obstacle, denoted C_obs. It is the set of robot configurations for which any part of the robot intersects an obstacle. The free configuration space is

where C is the admissible configuration space. A valid geometric path must remain in C_free.

2.3 Orientation and Cartesian configurations

A Cartesian pose includes position and orientation. Position may be represented by p = [x,y,z]^T, while orientation may be represented by a rotation matrix, Euler angles, or a unit quaternion. Rotation matrices satisfy

A unit quaternion must satisfy ||r|| = 1. Quaternions avoid some singularities associated with Euler angles, although quaternion interpolation requires attention to sign: r and -r represent the same physical orientation.

Cartesian planning is intuitive for tasks such as welding or painting, but it must ultimately be converted to joint motion through inverse kinematics. Multiple inverse-kinematic solutions may exist, and some may violate joint limits or approach singular configurations.

3. Smooth Joint-Space Trajectories

3.1 Boundary conditions

A trajectory should normally specify initial and final position, velocity, and sometimes acceleration. For one joint, the basic boundary conditions are

q(0) = q₀, q(T) = q_f, q̇(0) = v₀, and q̇(T) = v_f.

Here T is the motion duration. Smoothness is important because discontinuous velocity implies impulsive acceleration, while discontinuous acceleration produces theoretically unbounded jerk.

3.2 Cubic polynomial interpolation

A cubic polynomial is sufficient when position and velocity are specified at both endpoints:

The coefficients satisfy the four boundary conditions. For the common case of zero endpoint velocities, the result is

and

The position has continuous velocity, but acceleration changes abruptly at the start and end if the robot is connected to stationary segments. Cubic interpolation is therefore useful for basic point-to-point motion but may be insufficient where vibration or high precision matters.

3.3 Quintic polynomial interpolation

A quintic polynomial can impose zero position, velocity, and acceleration at both endpoints. With normalized time τ = t/T, the zero-velocity, zero-acceleration form is

This trajectory has continuous acceleration at the endpoints and a smoother jerk profile than the cubic alternative. Its use assumes that the resulting velocity and acceleration remain within actuator limits.

Worked Example: Cubic Joint Motion

A revolute joint moves from 0 to 1.0 rad in 2 s with zero endpoint velocities. The cubic trajectory is

q(t) = 3(t/2)² - 2(t/2)³.

At t = 1 s, q = 3(0.5)² - 2(0.5)³ = 0.75 - 0.25 = 0.50 rad. The velocity is

q̇(1) = 6(1)/2² - 6(1)²/2³ = 1.5 - 0.75 = 0.75 rad/s.

The acceleration is q̈(1) = 6/4 - 12/8 = 0 rad/s². The midpoint velocity is largest for this symmetric motion, whereas acceleration changes sign at the midpoint.

3.4 Trapezoidal and S-curve velocity profiles

Polynomial interpolation fixes endpoint conditions but does not automatically enforce limits. A trapezoidal velocity profile contains constant acceleration, constant velocity, and constant deceleration phases. If the acceleration and deceleration are bounded by a_max and the cruise velocity by v_max, the profile is selected according to displacement and duration.

An S-curve profile limits jerk, defined as

and is preferable when flexible transmissions, heavy payloads, or sensitive equipment make abrupt acceleration changes undesirable. In all cases, profile feasibility must be checked against velocity, acceleration, jerk, and torque limits.

4. Cartesian Trajectory Generation

A Cartesian trajectory may interpolate position and orientation directly. For straight-line translation between p₀ and p_f,

where σ(0)=0 and σ(T)=1. A cubic or quintic time-scaling function can supply σ(t). The path is straight in Cartesian position, but the corresponding joint path usually is not straight in joint space.

Orientation interpolation can use spherical linear interpolation (SLERP). For unit quaternions [[MATH(r_0)]] and [[MATH(r_1)]], define

where Ω is the angle satisfying cos Ω = r₀ · r₁, after selecting the quaternion signs so that the shorter rotation is used. This expression assumes non-identical orientations with sin Ω ≠ 0; near zero angle, normalized linear interpolation is commonly used as a numerical approximation.

At each time, inverse kinematics gives q(t). The solution must be checked for continuity, joint limits, singularities, and collisions. Near a singularity, small Cartesian velocities can require very large joint velocities. The differential relationship is

where J(q) is the Jacobian. When an inverse exists, q̇ = J^-1ẋ; otherwise, a pseudoinverse or constrained optimization method is used. The inverse formula is valid only away from singularities and when dimensions permit a suitable inverse.

5. Collision Detection and Feasibility

Collision checking tests whether robot geometry intersects obstacles or other robot links. Exact geometry may be represented by meshes, but collision detection often uses bounding volumes such as spheres, axis-aligned bounding boxes, or oriented bounding boxes for speed. A hierarchical strategy uses simple volumes first and detailed geometry only when necessary.

A planned edge between configurations q_a and q_b must be checked continuously or at sufficiently fine resolution. Checking only its endpoints is unsafe because the interpolated motion may collide between them. The required resolution depends on object geometry, curvature, robot dimensions, and maximum allowable penetration risk.

Important feasibility checks include:

• joint position, velocity, acceleration, and jerk limits;
• self-collision and environment collision;
• actuator torque and motor-speed limits;
• singularity proximity and numerical conditioning;
• payload, gravity, and friction effects;
• required clearance and sensor uncertainty.

For a manipulator, inverse dynamics can be represented abstractly as

where τ is actuator torque, M(q) is the inertia matrix, C(q,q̇)q̇ represents Coriolis and centrifugal effects, g(q) is gravity torque, τ_f represents friction, and τ_ext represents external loads. This model is useful for validation, but its accuracy depends on identified masses, inertias, friction, and payload conditions.

6. Graph-Search Motion Planning

Graph methods discretize the feasible space into nodes and edges. A grid planner is practical in low-dimensional spaces. Each free cell becomes a node, and neighboring cells are connected if motion between them is collision-free.

Dijkstra's algorithm finds a minimum-cost path when all edge costs are nonnegative, but it explores broadly. A* uses a heuristic:

where g(n) is the known cost from the start to node n, and h(n) estimates the remaining cost. If the heuristic never overestimates the true remaining cost, it is admissible; under standard conditions, A* returns an optimal path on the given graph.

A common cost combines length and risk:

where L is path length, R is a risk or clearance penalty, T is time, and the weights w_L,w_R,w_T reflect application priorities. Units must be normalized before adding unlike quantities.

Graph methods are transparent and can produce optimal solutions on their discretization. However, grid resolution grows rapidly with dimension, making them less suitable for high-degree-of-freedom manipulators.

7. Sampling-Based Planning

7.1 Probabilistic Roadmaps

A Probabilistic Roadmap (PRM) samples configurations in C_free, connects nearby collision-free samples, and searches the resulting graph. It is effective for repeated queries in a fixed environment: roadmap construction is performed once, then many start-goal queries can use it.

PRM performance depends on sample density, connection radius, collision checking, and narrow passages. Uniform random samples may rarely enter a narrow passage; bridge tests, obstacle-biased sampling, or adaptive sampling can improve performance.

7.2 Rapidly-exploring Random Trees

An RRT grows a tree from the start by sampling a random configuration, selecting the nearest existing node, and extending toward the sample by a limited step. The procedure explores large continuous spaces without explicit discretization.

Basic RRT is probabilistically complete under ideal assumptions: as the number of samples approaches infinity, the probability of finding a path approaches one if a path with suitable clearance exists. This does not guarantee a good path quickly or guarantee optimality. RRT* adds rewiring and is asymptotically optimal under appropriate conditions, although it generally requires more computation.

Sampling-based planners still require collision checking, goal-bias selection, a suitable distance metric, and post-processing. A Euclidean joint-space metric may be inappropriate when joints have different ranges, inertias, or safety importance; weighted metrics are often better.

8. Optimization Under Constraints

A feasible path can often be improved by shortening it, increasing clearance, reducing time, or lowering energy. A general constrained planning problem can be written

The objective may include terms such as path length, execution time, squared acceleration, torque, and inverse clearance. For example, a smoothness penalty can be expressed as

where the norm sums the squared joint accelerations. Such a penalty discourages abrupt motion but does not by itself guarantee collision avoidance or actuator feasibility.

After a planner produces a waypoint sequence, shortcut smoothing can test whether a direct connection between two nonconsecutive waypoints is collision-free. Optimization-based planners can adjust waypoints while maintaining constraints. The danger is that unconstrained smoothing may cut through obstacles or violate joint limits; every candidate must be revalidated.

9. Worked Planning Example

Consider a two-link planar arm with a fixed obstacle forming a rectangular forbidden region in workspace. The start and goal configurations are selected so that both are collision-free, but a direct joint-space interpolation causes the elbow to sweep through the obstacle.

A suitable procedure is:

1. Construct forward kinematics for both links.
2. Represent the obstacle and links with conservative geometric primitives.
3. Sample configurations within joint limits.
4. Reject samples that cause collision or insufficient clearance.
5. Connect nearby samples using interpolated joint motion and continuous collision checks.
6. Use A* to search the resulting roadmap.
7. Smooth the resulting waypoint sequence.
8. Apply time parameterization and test velocity, acceleration, and torque.
9. Execute with feedback while monitoring emergency-stop conditions.

Suppose the initial search gives a path length of 2.8 rad in a normalized joint metric and a smoothed path of 2.4 rad. The reduction is 0.4 rad, or approximately 14.3%. This comparison is meaningful only if both lengths use the same metric and the smoothed path remains feasible. A shorter path is not automatically preferable if its clearance is smaller or its required torque is greater.

10. Illustrative Case: Pick-and-Place Near a Human Work Zone

Background

A six-axis collaborative manipulator transfers components from a feeder to a fixture located near an operator. The robot must avoid the operator's monitored zone, preserve a minimum clearance from fixtures, and complete the transfer within a specified cycle time.

Problem

A straight Cartesian path is attractive because it gives predictable tool motion, but inverse kinematics produces two candidate joint branches. One branch approaches a joint limit and the other passes close to a fixture. A fast trajectory also produces excessive acceleration at the start of the move.

Analysis

The engineering team models the robot and fixture in configuration space, adds conservative safety margins, and rejects configurations near joint limits. A PRM is used because the workcell geometry is fixed and many pick-and-place queries are expected. The cost function penalizes time, joint travel, and low clearance. Candidate paths are then time-parameterized with an S-curve profile.

Approach and Solution

The selected path uses a raised intermediate waypoint, maintaining clearance from the fixture while avoiding the less favorable inverse-kinematic branch. Collision checks are performed along every edge, not merely at waypoints. Inverse dynamics verifies peak torque under the maximum payload. The controller tracks the reference trajectory using joint feedback, while safety-rated monitoring can stop or reduce the robot's motion when a person enters the defined zone.

Outcome

The final motion is slightly longer than the initial straight-line proposal but has greater clearance, lower peak acceleration, and no joint-limit violations. Its cycle time meets the production requirement after time scaling. The example illustrates that the best path is application-dependent: safety margin and repeatability may be more important than minimum geometric distance.

Learning Points

• Cartesian intuition does not replace configuration-space analysis.
• Inverse-kinematic branch selection is a planning decision.
• Safety constraints should be included before optimization, not added afterward.
• A path must be validated after smoothing and time parameterization.
• Human–robot applications require documented risk assessment and safety functions, not merely a mathematically collision-free path.

11. Implementation Best Practices and Limitations

Use consistent units, explicit joint conventions, and version-controlled robot models. Separate collision geometry from visual meshes when possible. Use conservative tolerances that reflect calibration error, perception uncertainty, and compliance. Test nominal and worst-case payloads. Log planned and measured trajectories so that tracking errors can be diagnosed.

No planner is universally superior. Grid search is simple but scales poorly. PRM is useful for repeated queries but can struggle with narrow passages. RRT explores high-dimensional spaces but may generate irregular, inefficient paths. Optimization can produce high-quality motion but is sensitive to initial guesses and local minima. Hybrid systems commonly combine sampling-based exploration with graph search and trajectory optimization.

Artificial intelligence can assist with learned heuristics, motion prediction, and experience-based sampling. Nevertheless, learned components should not replace collision validation, constraint checking, or safety-rated control. Training data may not cover unusual obstacles or payloads, and explainability is important when decisions affect people or expensive equipment.

Chapter Summary

Motion planning determines a feasible geometric route, while trajectory generation assigns time and produces position, velocity, and acceleration references. Configuration space is essential because collision depends on the complete robot configuration rather than only the end-effector position. Cubic and quintic polynomials provide smooth joint trajectories; trapezoidal and S-curve profiles provide explicit control of velocity, acceleration, and jerk. Cartesian trajectories require inverse kinematics and singularity analysis.

Collision checking must examine the motion between waypoints. Graph search methods such as Dijkstra's algorithm and A* are effective in discretized spaces, while PRM and RRT address higher-dimensional continuous spaces. Optimization can improve length, clearance, smoothness, time, and energy, but all optimized solutions must be rechecked against geometric, kinematic, dynamic, and safety constraints. In professional practice, the final motion is selected by balancing efficiency, robustness, clearance, actuator capability, and human safety.

Key Terms

Configuration: A complete specification of robot joint variables.

Configuration space: The space of all possible robot configurations.

C-space obstacle: Configurations that cause collision or violate a forbidden condition.

Free configuration space: The collision-free subset of configuration space.

Path: A geometric route independent of time.

Trajectory: A time-parameterized path.

Joint-space planning: Planning directly in joint coordinates.

Cartesian planning: Planning in end-effector position and orientation.

Forward kinematics: Mapping from joint configuration to end-effector pose.

Inverse kinematics: Finding joint configurations for a desired pose.

Jacobian: Matrix relating joint velocity to Cartesian velocity.

Singularity: A configuration where the Jacobian loses rank or becomes ill-conditioned.

Time parameterization: Assigning time to a geometric path.

Jerk: The time derivative of acceleration.

A*: A heuristic graph-search algorithm.

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

RRT: A rapidly exploring random tree for continuous-space planning.

Clearance: Distance between the robot and an obstacle.

Trajectory optimization: Improving a trajectory subject to an objective and constraints.

Probabilistic completeness: The property that success probability approaches one with increasing samples under stated assumptions.

Review Questions

Short Answer Questions

1. What is the difference between a path and a trajectory?
2. Why is configuration space more useful than workspace alone for collision checking?
3. State the boundary conditions commonly used for cubic interpolation.
4. What role does the Jacobian play in Cartesian trajectory generation?
5. Why must collision checking be performed along an edge between two configurations?

Long Answer Questions

1. Explain how a Cartesian trajectory is converted into a joint trajectory, including inverse kinematics, branch selection, and singularity concerns.
2. Compare cubic, quintic, trapezoidal, and S-curve trajectory profiles.
3. Explain A* search and the meaning of an admissible heuristic.
4. Compare PRM and RRT in terms of construction, repeated queries, exploration, and limitations.
5. Describe a complete industrial motion-planning workflow from task specification to feedback execution.

Analytical / Application Questions

1. A joint moves 1.2 rad in 3 s with zero endpoint velocities. Derive its cubic trajectory and calculate position and velocity at 1.5 s.
2. Design a planning strategy for a six-axis robot operating around a human worker. Identify geometric, dynamic, sensing, and safety constraints.
3. A smoothed path is shorter but has lower obstacle clearance and higher peak torque than the original path. Develop a cost-based decision and justify the selected motion using engineering criteria.

Multiple Choice Questions with Answers

Question 1

Which statement best distinguishes a path from a trajectory?

A. A path includes time, but a trajectory does not

B. A path is geometric, whereas a trajectory is time-parameterized

C. A path applies only to Cartesian motion

D. A trajectory cannot include velocity information

Correct Answer: B. A path is geometric, whereas a trajectory is time-parameterized

Question 2

For zero endpoint velocities, which polynomial provides the standard cubic point-to-point interpolation?

A. q(t) = q₀ + (q_f − q₀)(t/T)

B. q(t) = q₀ + (q_f − q₀)[3(t/T)² − 2(t/T)³]

C. q(t) = q₀ + (q_f − q₀)[10(t/T)³ − 15(t/T)⁴ + 6(t/T)⁵]

D. q(t) = q₀ + (q_f − q₀)(t/T)⁴

Correct Answer: B. q(t) = q₀ + (q_f − q₀)[3(t/T)² − 2(t/T)³]

Question 3

What does the configuration-space obstacle represent?

A. Only obstacles visible to a camera

B. Configurations in which the robot intersects an obstacle

C. The robot's reachable workspace

D. The set of actuator torque limits

Correct Answer: B. Configurations in which the robot intersects an obstacle

Question 4

In the relationship ẋ = J(q)q̇, what does J(q) represent?

A. The inertia matrix

B. The collision matrix

C. The robot Jacobian

D. The gravity vector

Correct Answer: C. The robot Jacobian

Question 5

What condition makes a heuristic admissible for A*?

A. It always overestimates the remaining cost

B. It is always zero

C. It never overestimates the true remaining cost

D. It is larger than the path cost already accumulated

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

Question 6

Which planner is particularly suitable when many start-goal queries occur in a fixed environment?

A. Probabilistic Roadmap

B. Single-step gradient descent

C. Cubic interpolation alone

D. Inverse dynamics

Correct Answer: A. Probabilistic Roadmap

Question 7

What is the main purpose of an S-curve velocity profile?

A. To remove all collision checks

B. To limit jerk and reduce abrupt acceleration changes

C. To guarantee the shortest geometric path

D. To eliminate inverse kinematics

Correct Answer: B. To limit jerk and reduce abrupt acceleration changes

Question 8

Why can a Cartesian path become infeasible during inverse-kinematic conversion?

A. Cartesian paths contain no position information

B. The resulting joint motion may violate limits or encounter singularities

C. Inverse kinematics changes the robot's link lengths

D. Cartesian paths cannot be time-parameterized

Correct Answer: B. The resulting joint motion may violate limits or encounter singularities

Question 9

Which term in the inverse-dynamics equation represents gravity torque?

A. M(q)q̈

B. C(q,q̇)q̇

C. g(q)

D. τ_f

Correct Answer: C. g(q)

Question 10

What is a fundamental limitation of basic RRT?

A. It can operate only on one-dimensional paths

B. It cannot use collision detection

C. It explores continuous spaces but does not generally guarantee an optimal path

D. It requires a complete grid of configuration space

Correct Answer: C. It explores continuous spaces but does not generally guarantee an optimal path