○Draft — not verified

Theoretical Background: Control Algorithms and Obstacle Avoidance

Module 5 Theory: Control Algorithms and Obstacle Avoidance

Learning Objectives

Understand the four control algorithms implemented in useControlAlgorithms.ts
Analyse the PID anti-windup mechanism and its necessity in mobile robot control
Understand the State Machine controller’s explicit alignment phase
Study the four obstacle avoidance modes in useObstacleAvoidance.ts
Apply the useAlgorithmComparison composable to benchmark controller performance

5.4.1 Control Algorithm Framework

All four algorithms share the same input vector and output structure, enabling runtime switching without restarting the simulation:

// useControlAlgorithms.ts
export type ControlAlgorithm =
  'proportional' | 'pid' | 'pure_pursuit' | 'state_machine'

interface ControlOutput {
  linear:    number            // m/s
  angular:   number            // rad/s
  algorithm: ControlAlgorithm
  timestamp: number            // Date.now()
}

// Tuned defaults for the 12-obstacle simulation field
const params = reactive<AlgorithmParams>({
  proportional: { kp_linear: 0.6,  kp_angular: 2.0 },
  pid: {
    kp_linear: 0.8,  ki_linear: 0.05,  kd_linear: 0.15,
    kp_angular: 2.5, ki_angular: 0.03, kd_angular: 0.2,
    integral_limit: 0.5,
  },
  pure_pursuit: { lookahead_distance: 0.5, kp_linear: 0.6 },
  state_machine: { arrival_tolerance: 0.15, heading_tolerance: 0.12 },
})

const maxLinearVel  = ref(0.7)   // m/s
const maxAngularVel = ref(1.5)   // rad/s

Common Input Computation:

All algorithms receive the same pre-computed error quantities:

d = (x_{g} - x)^{2} + (y_{g} - y)^{2} (1)

θ_{d} = atan2 (y_{g} - y, x_{g} - x) (2)

e_{θ} = θ_{d} - θ, e_{θ} \in [- π, π] (3)

// useControlAlgorithms.ts — compute() dispatcher
const compute = (goalX, goalY, robotX, robotY, robotTheta, dt) => {
  const dx    = goalX - robotX
  const dy    = goalY - robotY
  const distance     = Math.hypot(dx, dy)
  const desiredAngle = Math.atan2(dy, dx)
  let   headingError = desiredAngle - robotTheta
  while (headingError >  Math.PI) headingError -= 2*Math.PI
  while (headingError < -Math.PI) headingError += 2*Math.PI

  switch (currentAlgorithm.value) {
    case 'proportional':  return computeProportional(dx, dy, distance, headingError)
    case 'pid':           return computePID(dx, dy, distance, headingError, dt)
    case 'pure_pursuit':  return computePurePursuit(dx, dy, distance, headingError)
    case 'state_machine': return computeStateMachine(dx, dy, distance, headingError)
  }
}

5.4.2 Scheme 1: Proportional Controller

Direct proportional mapping from error to velocity command:

ω = sat_{[- ω_{max}, ω_{max}]} (K_{p, ω} e_{θ}) (4)

v = {min (v_{max}, K_{p, v} d) 0.3 v_{max} ∣ e_{θ} ∣ < 0.3 rad otherwise (5)

Platform defaults: $K_{p, v} = 0.6$ , $K_{p, ω} = 2.0$ , $v_{max} = 0.7$ m/s, $ω_{max} = 1.5$ rad/s.

const computeProportional = (dx, dy, distance, headingError) => {
  const p       = params.proportional
  const linear  = _clamp(p.kp_linear  * distance,
                         -maxLinearVel.value, maxLinearVel.value)
  const angular = _clamp(p.kp_angular * headingError,
                         -maxAngularVel.value, maxAngularVel.value)
  return { linear, angular, algorithm: 'proportional', timestamp: Date.now() }
}

5.4.3 Scheme 2: PID Controller

General PID Law:

u (t) = K_{p} e (t) + K_{i} \int_{0}^{t} e (τ) d τ + K_{d} \frac{d e}{d t} (6)

Discrete-Time Implementation with Anti-Windup:

I_{k} = clamp (I_{k - 1} + e_{k} \cdot Δ t, - I_{lim}, I_{lim}) (7)

ω_{k} = K_{p, ω} e_{θ} + K_{i, ω} I_{ω} + K_{d, ω} \frac{e _{θ} - e _{θ, prev}}{Δ t} (8)

Platform defaults: $K_{p, ω} = 2.5$ , $K_{i, ω} = 0.03$ , $K_{d, ω} = 0.2$ , $I_{lim} = 0.5$ .

const computePID = (dx, dy, distance, headingError, dt) => {
  const p = params.pid

  // Angular PID
  pidState.integral_angular = _clamp(
    pidState.integral_angular + headingError * dt,
    -p.integral_limit, p.integral_limit)

  const d_angular = (headingError - pidState.prev_error_angular) / dt
  const angular   = _clamp(
    p.kp_angular * headingError +
    p.ki_angular * pidState.integral_angular +
    p.kd_angular * d_angular,
    -maxAngularVel.value, maxAngularVel.value)

  pidState.prev_error_angular = headingError

  // Linear PID (distance error)
  pidState.integral_linear = _clamp(
    pidState.integral_linear + distance * dt,
    -p.integral_limit, p.integral_limit)

  const d_linear = (distance - pidState.prev_error_linear) / dt
  const linear   = _clamp(
    p.kp_linear  * distance +
    p.ki_linear  * pidState.integral_linear +
    p.kd_linear  * d_linear,
    0, maxLinearVel.value)

  pidState.prev_error_linear = distance
  return { linear, angular, algorithm: 'pid', timestamp: Date.now() }
}

Anti-Windup Bound:

K_{i} \int e d t \leq I_{lim} = 0.5 (9)

Without clamping, the integral accumulates during prolonged large errors (e.g. when the robot is blocked) and produces large overshoots when the path clears.

5.4.4 Scheme 3: Pure Pursuit (Goal-Seeking)

For single-goal navigation, Pure Pursuit uses the goal itself as the lookahead target. The steering curvature and angular velocity:

κ = \frac{2 sin ( e _{θ} )}{L} (10)

ω = sat_{[- ω_{max}, ω_{max}]} (v_{speed} \cdot κ) (11)

Platform default: $L = 0.5$ m (lookahead distance).

const computePurePursuit = (dx, dy, distance, headingError) => {
  const p      = params.pure_pursuit
  const kappa  = 2 * Math.sin(headingError) / p.lookahead_distance
  const linear = _clamp(p.kp_linear * distance,
                        0, maxLinearVel.value)
  const angular = _clamp(linear * kappa,
                         -maxAngularVel.value, maxAngularVel.value)
  return { linear, angular, algorithm: 'pure_pursuit', timestamp: Date.now() }
}

Note: this is the goal-seeking variant. The curved-path variant used by useFigureEightNavigation selects a moving lookahead point on a precomputed trajectory — see Module 5.5 for that derivation.

5.4.5 Scheme 4: State Machine Controller

Explicit two-phase control: align first, then drive. This prevents forward motion while the heading error is large, eliminating the lateral deviation that proportional control produces when the robot initially faces away from the goal.

State Transitions:

// States: 'aligning' | 'driving' | 'arrived'
ALIGNING  --[|e_theta| < heading_tolerance (0.12 rad)]--> DRIVING
DRIVING   --[|e_theta| > heading_tolerance * 2       ]--> ALIGNING
DRIVING   --[d < arrival_tolerance (0.15 m)          ]--> ARRIVED

const computeStateMachine = (dx, dy, distance, headingError) => {
  const p = params.state_machine

  if (distance < p.arrival_tolerance)
    return { linear: 0, angular: 0, algorithm: 'state_machine',
             timestamp: Date.now() }

  if (Math.abs(headingError) > p.heading_tolerance) {
    // ALIGNING: rotate only
    const angular = _clamp(2.0 * headingError,
                            -maxAngularVel.value, maxAngularVel.value)
    return { linear: 0, angular, algorithm: 'state_machine',
             timestamp: Date.now() }
  }

  // DRIVING: forward + gentle heading correction
  const linear  = _clamp(0.6 * distance, 0, maxLinearVel.value)
  const angular = _clamp(1.5 * headingError,
                          -maxAngularVel.value, maxAngularVel.value)
  return { linear, angular, algorithm: 'state_machine', timestamp: Date.now() }
}

5.4.6 Controller Comparison

Property	P	PID	Pure Pursuit	State Machine
Steady-state error	Possible	Eliminated	Path-dependent	Low
Oscillation risk	Low	Med. (tuning)	Low	Low
Requires precomputed path	No	No	No (goal variant)	No
Tuning parameters	2	7	2	2
Smooth trajectory	Moderate	Moderate	Smooth	Stepped
Heading alignment	Threshold	Threshold	Curvature-based	Explicit phase

5.4.7 Obstacle Avoidance Integration

useObstacleAvoidance runs orthogonally to the controller — its output overrides the controller when a threat is detected.

Safety Zones (default values from codebase):

const safetyZones = reactive<SafetyZones>({
  critical:  0.3,   // m — emergency stop / max avoidance
  warning:   0.7,   // m — avoidance algorithm activates
  detection: 2.0,   // m — obstacle tracking begins
})

5.4.7.1 Potential Fields

F_{att} = k_{att} (p_{g} - p) (12)

F_{rep, i} = k_{rep} (\frac{1}{d _{i}} - \frac{1}{d _{detect}}) \frac{r ^ _{i}}{d _{i}^{2}}, d_{i} < d_{detect} (13)

The proximity factor scales avoidance strength linearly between warning and critical:

f_{prox} = max (0, \frac{d _{min} - d _{crit}}{d _{warn} - d _{crit}}) \in [0, 1] (14)

5.4.7.2 Dynamic Window Approach (DWA)

(v^{*}, ω^{*}) = ar g (v, ω) \in W max [α G (\cdot) + β V (\cdot) + γ C (\cdot)] (15)

The avoidance direction is selected by comparing left vs. right sector clearance from the LiDAR data:

// Scale factor proportional to distance from critical zone
const scaleFactor = Math.max(0,
  (minFront - safetyZones.critical) / safetyZones.warning)
const leftClear = leftObstacles.every(o => o.range > safetyZones.warning)
const omega     = leftClear ? scaleFactor * maxAngularVel
                            : -scaleFactor * maxAngularVel

5.4.7.3 Vector Field Histogram (VFH)

Builds a polar histogram of obstacle density. Steering command selects the valley sector closest to the goal bearing.

5.4.7.4 Emergency Stop

Publishes zero velocity immediately when safetyStatus === ’critical’. Increments metrics.emergencyStops counter.

5.4.8 Algorithm Comparison

useAlgorithmComparison.ts provides a benchmarking composable that runs two algorithms sequentially on the same goal set and records their performance metrics side by side for display on the /control page analytics panel.

// useAlgorithmComparison.ts
const runComparison = async (algorithms: ControlAlgorithm[], goals: Goal[]) => {
  const results: ComparisonResult[] = []
  for (const algo of algorithms) {
    controlAlgos.setAlgorithm(algo)
    analytics.startCollection()
    await runGoalSequence(goals)
    analytics.stopCollection()
    results.push({ algorithm: algo, stats: analytics.stats.value })
  }
  return results
}

Integration: Theory to Practice

The /control page wires useControlAlgorithms, useObstacleAvoidance, useMobileRobotSimulation, and useAnalyticsEngine together in a

single 10 Hz control loop. The obstacle avoidance result is checked first; if avoidance is needed, its angular velocity overrides the controller output. The final {linear, angular} pair is sent to either the simulation physics engine or the ROS 2 /cmd_vel topic depending on the connection mode.

Theoretical Design Choices

Why $K_{p, ω} = 2.0$ for proportional control? At 2.0 rad/s per radian of heading error, a 90° error produces the maximum angular velocity of $1.5$ rad/s (clamped). This gives a natural settling time of approximately one second for heading corrections, matching the 10 Hz control frequency well.

Why integral_limit = 0.5? The integral contribution is capped so that $∣ K_{i} \int e d t ∣ \leq 0.5$ rad/s, which is one-third of $ω_{max}$ . This allows the integral to correct persistent errors without dominating the controller output during transient large errors.

Why heading_tolerance = 0.12 rad for the State Machine? $0.12$ rad is approximately 7°. This is tight enough to prevent visible lateral deviation during driving ( $< 5$ cm at 0.4 m/s for 0.1 s), while loose enough to avoid hunting around the exact heading in the alignment phase.

Tune the four controllers live

Proportional / PID / Pure Pursuit / State Machine — all selectable from the control panel.

/material-handling