Mathematical foundations for the autonomous mobile robot on the /control page. Differential-drive kinematics with RK4 integration, four navigation controllers, three obstacle avoidance methods, and simulated 72-ray LiDAR. Reference: MEC781 Modules 1–5.

1. Differential-Drive Kinematics

The unicycle model relates body-frame velocities (v, ω) to world-frame pose derivatives.

State vector: q = [ x, y, θ, v, ω ] — 5 states, 2 control inputs (v_cmd, ω_cmd)

Integration: 4th-Order Runge-Kutta

The state is advanced using RK4 for numerical accuracy:

q_n+1 = q_n + (dt / 6)( k₁ + 2k₂ + 2k₃ + k₄ )

(6)

k₁ = f(q_n) k₂ = f(q_n + ½ dt k₁) k₃ = f(q_n + ½ dt k₂) k₄ = f(q_n + dt k₃)

Why RK4 over Euler? Euler integration accumulates O(dt) error per step. RK4 achieves O(dt⁴) per step — at dt = 0.02 s (50 Hz), RK4 error is ~10⁸× smaller. Critical for accurate trajectory tracking and consistent algorithm comparison.

2. Navigation Controllers

All four controllers share the same error computation. Given robot pose (x, y, θ) and target (x_t, y_t):

d = √( (x_t − x)² + (y_t − y)² )

(7)

α = atan2(y_t − y, x_t − x) − θ

(8)

d = distance error, α = heading error (normalised to [−π, π]).

3. PID Controller

Independent PID loops for linear and angular channels with anti-windup clamping.

u(t) = K_p e(t) + K_i ∫₀ᵗ e(τ) dτ + K_d de(t)/dt

(9)

Linear Channel

e(t) = d (distance to target)

Kp0.1 – 5.0 (default 0.8)

Ki0.0 – 1.0 (default 0.05)

Kd0.0 – 1.0 (default 0.15)

Output clamped to [−v_max, v_max]. Integral clamped to ±0.5 for anti-windup.

Angular Channel

e(t) = α (heading error)

Kp0.5 – 8.0 (default 2.5)

Ki0.0 – 1.0 (default 0.03)

Kd0.0 – 1.0 (default 0.2)

Output clamped to [−ω_max, ω_max]. Same integral limit for anti-windup.

Derivative term. Computed as finite difference: d_err = (e[n] − e[n−1]) / dt. Kd dampens oscillation near the target. The integral term eliminates steady-state error from drag.

4. Pure Pursuit Controller

Geometric path-following algorithm. The robot steers toward a look-ahead point at distance L_d ahead on the path.

ω = 2 sin(α) / L_d

(10)

v = K_p · d (halved when d < L_d)

(11)

Pure Pursuit Params

L_d (lookahead distance)0.1 – 2.0 m (default 0.5)

Short L_d → tight tracking, oscillatory. Long L_d → smooth curves, cuts corners.

Kp linear0.1 – 3.0 (default 0.6)

Speed proportional to remaining distance. Automatically slows near target.

Geometry

Eq. (10) derives from the geometry of a circular arc passing through the robot and the look-ahead point. The curvature κ = 2 sin(α) / L_d gives the angular velocity ω = v · κ. For point-to-point navigation, the target itself serves as the look-ahead point. For trajectory following (figure-eight), L_d indexes ahead on the path.

5. Obstacle Avoidance

Three reactive methods that modify the controller's velocity command before it reaches the physics engine.

5.1 Potential Fields

F_rep = − ∑ (1 / r_i²) · r̂_i (for r_i < detection zone)

(12)

v_safe = v_cmd · proximity_factor ω_safe = ω_cmd + steer · F_rep angle

(13)

Each obstacle generates a repulsive force inversely proportional to distance². Forces are summed. Proximity factor linearly scales speed to zero at critical zone.

5.2 Dynamic Window Approach (DWA)

Evaluates forward, left, and right sectors. If front obstacles detected, scales linear velocity by clearance ratio and steers toward the clearer side:

v_safe = v_cmd · max(0, (r_front − r_crit) / r_warn)

(14)

ω_steer = ± 1.5 / max(r_front, 0.1) (sign: toward clearer side)

(15)

5.3 Vector Field Histogram (VFH)

Builds a 36-bin polar histogram of obstacle density. Finds the widest gap (lowest density) and steers toward it:

h[k] = ∑ (1 / r_i) for obstacles in sector k (k = 0..35, 10° bins)

(16)

ω = 0.8 · θ_gap v = 0.7 · v_cmd

(17)

θ_gap = center angle of widest gap. Speed reduced to 70% during avoidance manoeuvre.

6. Simulated LiDAR

72-ray simulated laser scanner via ray-circle intersection. Feeds obstacle detection for avoidance algorithms.

Scan Parameters

Rays72 (5° increment)

Angle range−π to +π (360°)

Min range0.1 m

Max range10.0 m

Rate (backend)10 Hz

Coordinate Transform

Each ray is cast in the sensor frame (aligned with robot body). Cartesian conversion to world frame:

x_w = x_r + r cos(θ_ray) cos(θ_r) − r sin(θ_ray) sin(θ_r)
y_w = y_r + r cos(θ_ray) sin(θ_r) + r sin(θ_ray) cos(θ_r)

7. Figure-Eight Trajectory

Lemniscate of Gerono parameterisation for the /figure-eight-nav page.

x(t) = a sin(t) y(t) = b sin(t) cos(t)

(18)

κ(t) = | ẋ ÿ − ẏ ẍ | / ( ẋ² + ẏ² )^3/2

(19)

Path Params

a (half-width)2.0 m

b (half-height)1.0 m

Resolution200 waypoints

Speed0.3 m/s

Derivatives

ẋ = a cos(t)

ẏ = b(cos²t − sin²t)

ẍ = −a sin(t)

ÿ = −4b sin(t) cos(t)

Curvature κ(t) is used for speed-adaptive control — robot slows at high-curvature crossover points.

8. Control Pipeline

Equation Reference

#	Equation	Description
(1)–(3)	ẋ = v cos θ, ẏ = v sin θ, θ̇ = ω	Unicycle kinematic model
(4)–(5)	v̇ = Kp(v_cmd − v) − β·v	First-order velocity dynamics with drag
(6)	q += (dt/6)(k₁ + 2k₂ + 2k₃ + k₄)	RK4 integration
(7)–(8)	d = ‖p_t − p‖, α = atan2(Δy,Δx) − θ	Distance and heading error
(9)	u = Kp·e + Ki·∫e + Kd·ė	PID control law
(10)–(11)	ω = 2 sin α / L_d	Pure pursuit steering
(12)–(13)	F_rep = −Σ(1/r²) r̂	Potential field repulsive force
(14)–(15)	v = v_cmd · clearance_ratio	DWA speed scaling
(16)–(17)	h[k] = Σ(1/r), ω → θ_gap	VFH polar histogram
(18)–(19)	x = a sin t, y = b sin t cos t	Lemniscate of Gerono (figure-eight)