⟳In review— alignat{N} parse fix; velocity-phase (Eq 6–8) and cubic-coeff (Eq 13–16) blocks split into per-row equations for KaTeX compatibility.· updated 2026-04-18

Theoretical Background: Trajectory Control

Module 4 Theory: Trajectory Control

4.3 Path vs. Trajectory

4.3.1 Fundamental Distinction

Path: A geometric curve in C-space with no timing information. Formally, $p : [0, 1] \to C_{free}$ , where $s \in [0, 1]$ is the path parameter.

Trajectory: A path with timing — it specifies where the robot should be at each instant. Formally, $q : [0, T] \to C_{free}$ , where $t \in [0, T]$ is time.

The distinction is critical: a path planner (A*, RRT) produces geometry, while a trajectory generator adds the temporal dimension needed for control.

4.3.2 Time Parameterization

Given a geometric path $p (s)$ , a time parameterization assigns $s (t)$ , yielding:

q (t) = p (s (t)) (1)

\overset{q}{˙} (t) = p^{'} (s) \overset{s}{˙} (t) (2)

\overset{q}{¨} (t) = p^{''} (s) \overset{s}{˙}^{2} + p^{'} (s) \overset{s}{¨} (3)

The simplest approach is linear parameterization with constant speed along the path:

s (t) = \frac{t}{T} (4)

\overset{s}{˙} = \frac{1}{T} = const (5)

Limitation: Linear parameterization produces discontinuous velocity at the start and end (instantaneous jump from zero to cruise speed), which is physically unrealizable and generates infinite acceleration demands.

4.4 Velocity and Acceleration Profiles

4.4.1 Trapezoidal Velocity Profile

The trapezoidal profile is the most common in industrial robotics. It consists of three phases:

Acceleration Phase ( $0$ to $t_{a}$ ): Constant acceleration $a_{m a x}$ ramps velocity from zero to $v_{m a x}$ .

Cruise Phase ( $t_{a}$ to $T - t_{a}$ ): Constant velocity $v_{m a x}$ with zero acceleration.

Deceleration Phase ( $T - t_{a}$ to $T$ ): Constant deceleration $- a_{m a x}$ brings velocity back to zero.

The velocity equations for each phase are:

v (t) = a_{m a x} t, 0 \leq t < t_{a} (6)

v (t) = v_{m a x}, t_{a} \leq t < T - t_{a} (7)

v (t) = a_{m a x} (T - t), T - t_{a} \leq t \leq T (8)

The acceleration time and total distance are:

t_{a} = \frac{v _{m a x}}{a _{m a x}} (9)

D = v_{m a x} (T - t_{a}) (10)

Short Distance Constraint: If the distance is too short for full acceleration to $v_{m a x}$ , the cruise phase vanishes and the profile becomes triangular:

v_{peak} = a_{m a x} D (11)

4.4.2 S-Curve (7-Segment) Profile

The S-curve profile is smoother than trapezoidal because it additionally limits jerk (the derivative of acceleration). It consists of seven segments: increasing acceleration, constant acceleration, decreasing acceleration, constant velocity (cruise), increasing deceleration, constant deceleration, and decreasing deceleration.

Advantage: Reduced vibration and mechanical stress because acceleration transitions are smooth rather than instantaneous.

Disadvantage: More complex to compute and requires specifying a jerk limit $j_{m a x}$ in addition to $a_{m a x}$ and $v_{m a x}$ .

4.5 Smooth Trajectory Generation

4.5.1 Cubic Polynomial Trajectories

A cubic polynomial satisfies four boundary conditions: position and velocity at the start and end of the motion segment.

Given $q (0) = q_{0}$ , $q (T) = q_{f}$ , $\overset{q}{˙} (0) = v_{0}$ , $\overset{q}{˙} (T) = v_{f}$ :

q (t) = a_{0} + a_{1} t + a_{2} t^{2} + a_{3} t^{3} (12)

The coefficients are determined by solving the boundary condition system:

a_{0} = q_{0} (13)

a_{1} = v_{0} (14)

a_{2} = \frac{3 ( q _{f} - q _{0} ) - ( 2 v _{0} + v _{f} ) T}{T ^{2}} (15)

a_{3} = \frac{2 ( q _{0} - q _{f} ) + ( v _{0} + v _{f} ) T}{T ^{3}} (16)

4.5.2 Quintic Polynomial Trajectories

A quintic polynomial satisfies six boundary conditions by additionally constraining acceleration at the start and end:

q (t) = a_{0} + a_{1} t + a_{2} t^{2} + a_{3} t^{3} + a_{4} t^{4} + a_{5} t^{5} (17)

Key Advantage: Quintic polynomials ensure continuous acceleration at segment boundaries, eliminating jerk discontinuities. This is important for manipulators where torque commands must be smooth.

4.5.3 Multi-Segment Trajectories (Via Points)

For trajectories passing through multiple waypoints $q_{0}, q_{1}, \dots, q_{N}$ , cubic spline interpolation fits a piecewise cubic polynomial ensuring position, velocity, and acceleration continuity at each waypoint. The result is a $C^{2}$ continuous trajectory.

Script Implementation (Cubic Spline Trajectory):

# Generate smooth trajectory through waypoints using cubic spline
        # Returns position, velocity, and acceleration at fine time resolution
        
        def generate_trajectory(waypoints, times):
        cs     = CubicSpline(times, waypoints, bc_type='clamped')
        t_fine = np.linspace(times[0], times[-1], 1000)
        q      = cs(t_fine)       # position
        q_dot  = cs(t_fine, 1)    # velocity  (first derivative)
        q_ddot = cs(t_fine, 2)    # acceleration (second derivative)
        return t_fine, q, q_dot, q_ddot

4.6 Trajectory Following Controllers

4.6.1 PID Trajectory Tracking

For each joint, a PID controller computes the required torque based on the tracking error:

τ_{i} = K_{p} (q_{d, i} - q_{i}) + K_{d} (\overset{q}{˙}_{d, i} - \overset{q}{˙}_{i}) + K_{i} \int (q_{d, i} - q_{i}) d t (18)

Limitations of PID: Does not account for robot dynamics (mass, inertia, Coriolis effects). Performance degrades at high speeds where dynamic effects become significant. Cross-coupling between joints is ignored.

4.6.2 Computed Torque Control (Inverse Dynamics)

Computed torque control uses the full dynamic model for feedforward compensation, achieving linear decoupled error dynamics:

τ = M (q) [\overset{q}{¨}_{d} + K_{d} \overset{e}{˙} + K_{p} e] + C (q, \overset{q}{˙}) \overset{q}{˙} + g (q) (19)

where $e = q_{d} - q$ is the tracking error, $M (q)$ is the inertia matrix, $C (q, \overset{q}{˙})$ is the Coriolis/centrifugal matrix, and $g (q)$ is the gravity vector.

Substituting into the robot dynamics equation $M \overset{q}{¨} + C \overset{q}{˙} + g = τ$ yields the closed-loop error system:

\overset{e}{¨} + K_{d} \overset{e}{˙} + K_{p} e = 0 (20)

This is a linear, decoupled second-order system. The gains are chosen for desired transient response:

K_{p} = ω_{n}^{2} I (21)

K_{d} = 2 ζ ω_{n} I (22)

where $ω_{n}$ is the natural frequency and $ζ$ is the damping ratio (typically $ζ = 1$ for critical damping).

4.6.3 Feedback Linearization for Mobile Robots

For a differential drive robot following a reference trajectory $(x_{d} (t), y_{d} (t))$ , the tracking error is expressed in the body frame:

e_{x} = cos θ (x_{d} - x) + sin θ (y_{d} - y) (23)

e_{y} = - sin θ (x_{d} - x) + cos θ (y_{d} - y) (24)

e_{θ} = θ_{d} - θ (25)

Kanayama Controller computes the linear and angular velocity commands:

v = v_{d} cos (e_{θ}) + k_{x} e_{x} (26)

ω = ω_{d} + v_{d} (k_{y} e_{y} + k_{θ} sin (e_{θ})) (27)

where $k_{x}, k_{y}, k_{θ} > 0$ are control gains. This controller is proven to be globally asymptotically stable for tracking smooth reference trajectories.

4.7 Error Handling and Recovery

4.7.1 Types of Tracking Errors

Position Error: Caused by wheel slip or drift. Detected by comparing odometry to reference. Recovered by increasing gains or re-planning.

Velocity Error: Caused by actuator saturation. Detected by comparing commanded vs. actual velocity. Recovered by reducing the speed profile.

Following Error: Caused by large disturbances. Detected when error exceeds a threshold. Recovered by pausing and re-planning from the current pose.

Collision: Caused by mapping error or dynamic obstacles. Detected by bumper or force sensor. Recovered by emergency stop followed by re-planning.

4.7.2 Error Monitoring

Script Implementation (Tracking Error Monitor):

# Monitor tracking error and trigger re-planning if threshold exceeded
        # Counts consecutive steps with excessive error before acting
        
        class TrajectoryTracker(Node):
        def __init__(self):
        super().__init__('trajectory_tracker')
        self.max_position_error = 0.3   # metres
        self.max_heading_error  = 0.5   # radians
        self.error_count        = 0
        self.max_error_count    = 50    # consecutive steps before re-plan
        
        def check_tracking_error(self, error):
        if (abs(error.x)     > self.max_position_error or
        abs(error.y)     > self.max_position_error or
        abs(error.theta) > self.max_heading_error):
        self.error_count += 1
        if self.error_count > self.max_error_count:
        self.trigger_replan()   # error persists, re-plan
        else:
        self.error_count = 0        # reset on good tracking

4.7.3 Recovery Strategies

Slow Down: Reduce trajectory speed to improve tracking accuracy. The tracking error in Equation (20) decreases with lower reference velocities because the feedforward demands on the controller are reduced.

Re-plan: Generate a new trajectory from the current pose to the goal. This is essential when the robot has deviated too far from the original path for the controller to recover.

Backtrack: Return to the last known good position on the trajectory and resume from there.

Stop and Localize: Halt all motion and run localization algorithms to improve state estimate accuracy before continuing.

Escalate: Request human intervention via teleoperation when autonomous recovery fails.

Integration: Theory to Practice

In ROS 2, the Nav2 controller server implements trajectory tracking using the concepts from this module. The global planner produces a geometric path (Section 4.3.1), the controller server applies time parameterization (Equations (1)–(5)) and velocity profiling (Equations (6)–(11)), and the local controller tracks the resulting trajectory. The Kanayama controller (Equations (26)–(27)) maps directly to Twist messages published on cmd_vel. Velocity constraints ( $v_{m a x}$ , $ω_{m a x}$ ) are enforced as ROS parameters, ensuring the generated trajectory respects hardware limits. The error monitoring pattern from Section 4.7.2 maps to the controller server’s progress checker, which triggers re-planning when the robot deviates beyond configured thresholds.

ArbiterROS mapping. The course platform exposes the two halves of this pipeline on separate pages. Reference-trajectory generation (Equations (1)–(11)) lives in frontend/lib/Robot/FigureEightTrajectory.js: given $a$ , $T$ , and arc-length parameter $s$ , it emits the time-sampled $(x_{d}, y_{d}, θ_{d}, v_{d}, ω_{d})$ tuples that /figure-eight-nav feeds to the tracker. The tracking half (Equations (20)–(27)) lives in frontend/composables/Robot/useControlAlgorithms.ts, which ships four interchangeable strategies — proportional, PID, Pure Pursuit, and a state-machine go-to-goal — all publishing to /cmd_vel. Lab 3 compares them on three scenarios; the Kanayama form above is the theoretical sibling of the Pure Pursuit implementation with $ℓ$ (lookahead) taking the role that ( $k_{y}, k_{θ}$ ) play here.

Theoretical Design Choices

Quintic polynomials are preferred over cubic for manipulator trajectories because the additional acceleration boundary conditions eliminate torque discontinuities at segment boundaries, reducing mechanical wear. The trapezoidal profile is chosen for industrial applications because it maximises the time spent at $v_{m a x}$ (maximising throughput) while requiring only two parameters ( $v_{m a x}$ , $a_{m a x}$ ). Computed torque control is preferred over PID for manipulators because PID tracking error grows with the square of velocity (the uncompensated dynamic terms scale as $\overset{q}{˙}^{2}$ ), while computed torque achieves velocity-independent error dynamics. The Kanayama controller is chosen for mobile robots because it operates in the body frame, naturally handling the nonholonomic constraint without requiring coordinate transformations, and its stability proof holds globally for smooth reference trajectories. The consecutive-step threshold in error monitoring (rather than a single-sample trigger) prevents false alarms from sensor noise while ensuring persistent deviations are detected within a bounded time.