Theoretical Background: Navigation Control Algorithms

Module 4 Theory: Navigation Control Algorithms

Learning Objectives

• Understand the hybrid reactive-deliberative navigation system
• Master the behaviour-based control architecture implementation
• Analyse the proportional control algorithms for goal-seeking
• Learn obstacle avoidance using potential field principles
• Implement real-time motion control with ROS integration

4.14 Navigation Architecture Overview

4.14.1 Behaviour-Based Navigation System

The script implements a behaviour-based navigation architecture with three primary modes:

// Navigation state machine
this.navigationMode    = 'manual';  // 'manual', 'navigating', 'avoiding'
this.currentGoal       = null;
this.obstacleThreshold = 0.8;       // metres

Control Flow:

1. Manual Mode: Direct user control via keyboard or buttons
2. Navigating Mode: Autonomous goal-seeking behaviour
3. Avoiding Mode: Reactive obstacle avoidance (highest priority)

4.14.2 Navigation Control Loop

startNavigationLoop() {
    this.navigationInterval = setInterval(() => {
        if (this.navigationMode === 'navigating' &&
            this.currentGoal && this.connected) {
            this.updateNavigation(); // Main control algorithm
        }
    }, 100); // 10 Hz navigation updates
}

Key Characteristics:

• Real-time: 10 Hz update rate (100 ms cycle time)
• Event-driven: Responds to sensor data and user-set goals
• Fault-tolerant: Handles connection failures gracefully

4.15 Goal-Seeking Control Algorithm

4.15.1 Proportional Control Implementation

The script uses proportional control for both linear and angular motion. The complete updateNavigation() function implements a six-step control pipeline:

updateNavigation() {
    if (!this.currentGoal || !this.laserScan) return;


```javascript
const goalX    = this.currentGoal.x;
const goalY    = this.currentGoal.y;
const robotX   = this.robotPose.x;
const robotY   = this.robotPose.y;
const robotTheta = this.robotPose.theta;

// STEP 1: Calculate distance to goal
const goalDistance = Math.sqrt(
    Math.pow(goalX - robotX, 2) + Math.pow(goalY - robotY, 2)
);

// STEP 2: Check goal reached condition
if (goalDistance < this.goalTolerance) { // default: 0.3 m
    this.goalReached();
    return;
}

// STEP 3: Calculate desired heading (goal-seeking)
const desiredAngle = Math.atan2(goalY - robotY, goalX - robotX);
let angleError = desiredAngle - robotTheta;

// STEP 4: Normalise angle error to [-PI, PI]
while (angleError >  Math.PI) angleError -= 2 * Math.PI;
while (angleError < -Math.PI) angleError += 2 * Math.PI;

// STEP 5: Check for obstacles (behaviour arbitration)
const obstacleAvoidance = this.calculateObstacleAvoidance();

let linearVel  = 0;
let angularVel = 0;

if (obstacleAvoidance.needsAvoidance) {
    // PRIORITY 1: Obstacle avoidance behaviour
    this.navigationMode = 'avoiding';
    this.updateNavigationStatus('avoiding', 'Avoiding obstacle');

    angularVel = obstacleAvoidance.angularVel * this.currentSpeed;
    linearVel  = Math.max(0.1, this.currentSpeed * 0.3); // Reduce speed

} else {
    // PRIORITY 2: Normal goal-seeking behaviour
    this.navigationMode = 'navigating';
    this.updateNavigationStatus('navigating',
        `Distance: ${goalDistance.toFixed(2)}m`);

    // PROPORTIONAL CONTROL: angular velocity
    angularVel = Math.max(-1.0,
                 Math.min(1.0, angleError * 2.0)) * this.currentSpeed;

    // CONDITIONAL LINEAR VELOCITY CONTROL
    if (Math.abs(angleError) < 0.3) { // ~17 degrees
        // Good heading alignment -- move forward
        linearVel = Math.min(this.currentSpeed, goalDistance);
    } else {
        // Poor heading alignment -- slow down while turning
        linearVel = this.currentSpeed * 0.3;
    }
}

// STEP 6: Execute motion command
this.publishVelocity(linearVel, angularVel);

}

### 4.15.2 Control Algorithm Analysis

### Goal Distance and Desired Heading:



$$
d_{\mathrm{goal}} = \sqrt{(x_g - x)^2 + (y_g - y)^2} \tag{1}
$$





$$
\theta_d = \operatorname{atan2}(y_g - y,\; x_g - x) \tag{2}
$$





$$
e_\theta = \theta_d - \theta, \quad e_\theta \in [-\pi, \pi] \tag{3}
$$



### Angular Control (Heading Control):



$$
\omega = \mathrm{sat}_{[-1,\,1]}\!\bigl(K_{p,\omega}\, e_\theta\bigr)
             \cdot v_{\mathrm{speed}}, \qquad K_{p,\omega} = 2.0 \tag{4}
$$



### Linear Control (Speed Control):



$$
v =
    \begin{cases}
        \min\!\bigl(v_{\mathrm{speed}},\; d_{\mathrm{goal}}\bigr)
            & \text{if } |e_\theta| < 0.3\,\text{rad} \\[4pt]
        0.3\, v_{\mathrm{speed}}
            & \text{otherwise}
    \end{cases} \tag{5}
$$



### Control Properties:

- $K_{p,\omega} = 2.0$ rad/s per radian error — higher gain for responsive turning

- Heading threshold $= 0.3$ rad ($\approx 17°$) — balance between precision and
  efficiency

- Speed reduction $= 0.3$ — conservative turning to prevent overshoot

## 4.16 Obstacle Avoidance Algorithm

### 4.16.1 Potential Field-Based Avoidance

The script implements artificial potential fields for obstacle avoidance using a six-step
detection and response pipeline:

``` {language=""}
calculateObstacleAvoidance() {
    if (!this.laserScan) return { needsAvoidance: false, angularVel: 0 };


```javascript
const ranges         = this.laserScan.ranges;
const angleIncrement = this.laserScan.angle_increment;

// STEP 1: Define front sector for collision detection
const frontSectorAngle = Math.PI / 6; // +/-30 degrees
const centerIndex      = Math.floor(ranges.length / 2);
const sectorSize       = Math.floor(frontSectorAngle / angleIncrement);

let minDistance      = Infinity;
let avoidanceDirection = 0;

// STEP 2: Scan front sector for obstacles
for (let i = centerIndex - sectorSize;
         i <= centerIndex + sectorSize; i++) {
    if (i >= 0 && i < ranges.length) {
        const range = ranges[i];
        if (range > this.laserScan.range_min &&
            range < this.laserScan.range_max) {
            minDistance = Math.min(minDistance, range);
        }
    }
}

// STEP 3: Check if avoidance is needed
if (minDistance < this.obstacleThreshold) { // default: 0.8 m

    let leftDistance  = Infinity;
    let rightDistance = Infinity;

    // STEP 4a: Sample left side clearance
    for (let i = centerIndex + sectorSize;
             i < Math.min(ranges.length, centerIndex + sectorSize * 2); i++) {
        const range = ranges[i];
        if (range > this.laserScan.range_min &&
            range < this.laserScan.range_max) {
            leftDistance = Math.min(leftDistance, range);
        }
    }

    // STEP 4b: Sample right side clearance
    for (let i = Math.max(0, centerIndex - sectorSize * 2);
             i < centerIndex - sectorSize; i++) {
        const range = ranges[i];
        if (range > this.laserScan.range_min &&
            range < this.laserScan.range_max) {
            rightDistance = Math.min(rightDistance, range);
        }
    }

    // STEP 5: Choose direction with maximum clearance
    avoidanceDirection = leftDistance > rightDistance ? 1.0 : -1.0;

    // STEP 6: Calculate repulsive force magnitude
    const repulsiveForce = 1.0 - minDistance / this.obstacleThreshold;

    return {
        needsAvoidance: true,
        angularVel:     avoidanceDirection * repulsiveForce
    };
}

return { needsAvoidance: false, angularVel: 0 };

}

### 4.16.2 Obstacle Avoidance Analysis

### Repulsive Force and Direction Selection:



$$
F_{\mathrm{rep}} = 1 - \frac{d_{\min}}{d_{\mathrm{threshold}}} \tag{6}
$$





$$
\sigma = \begin{cases} +1 & \text{if } d_{\mathrm{left}} > d_{\mathrm{right}}
                                \quad\text{(turn left)} \\
                           -1 & \text{otherwise}
                                \quad\text{(turn right)} \end{cases} \tag{7}
$$





$$
\omega_{\mathrm{avoid}} = \sigma \cdot F_{\mathrm{rep}} \cdot v_{\mathrm{speed}} \tag{8}
$$



### Sensor Processing:

- **Front sector:** $\pm 30°$ collision detection zone

- **Side sectors:** $\pm 30°$ to $\pm 60°$ for avoidance direction selection

- **Range filtering:** Excludes readings outside
  $[r_{\min},\, r_{\max}]$

### Control Characteristics:

- **Threshold $= 0.8$ m:** Conservative safety margin

- **Binary direction:** Simple left/right decision based on maximum clearance

- **Force scaling:** Stronger avoidance response for closer obstacles
  ($F_{\mathrm{rep}} \to 1$ as $d_{\min} \to 0$)

## 4.17 Motion Control and Command Generation

### 4.17.1 Velocity Command Publishing

All motion commands are packaged as ROSLIB `Twist` messages and published to the
`/cmd_vel` topic:

``` {language=""}
publishVelocity(linear, angular) {
    if (!this.connected || !this.cmdVelPub) return;


```javascript
// Create ROS Twist message
const twist = new ROSLIB.Message({
    linear: {
        x: linear,  // Forward/backward velocity (m/s)
        y: 0,        // Lateral velocity (always 0 -- differential drive)
        z: 0         // Vertical velocity (always 0)
    },
    angular: {
        x: 0,        // Roll rate  (always 0)
        y: 0,        // Pitch rate (always 0)
        z: angular   // Yaw rate (rad/s) -- turning
    }
});

// Publish to ROS /cmd_vel topic
this.cmdVelPub.publish(twist);

}

### 4.17.2 Manual Control Integration

Manual input overrides autonomous navigation immediately. Keyboard mappings follow the
standard WASD/arrow convention:

``` {language=""}
// Manual control overrides autonomous navigation
startMove(linear, angular) {
    if (!this.connected || !this.cmdVelPub) return;


```javascript
const linVel = linear  * this.currentSpeed;  // +/-1 * speed
const angVel = angular * this.currentSpeed;  // +/-1 * speed

this.publishVelocity(linVel, angVel);
this.navigationMode = 'manual';
document.getElementById('nav-mode').textContent = 'Manual';

}

// Keyboard control mapping handleKeyDown(e) { if (!this.connected) return;

switch (e.key.toLowerCase()) {
    case 'w': case 'arrowup':
        this.cancelNavigation();
        this.startMove(1, 0);    // Forward
        break;
    case 's': case 'arrowdown':
        this.cancelNavigation();
        this.startMove(-1, 0);   // Backward
        break;
    case 'a': case 'arrowleft':
        this.cancelNavigation();
        this.startMove(0, 1);    // Turn left
        break;
    case 'd': case 'arrowright':
        this.cancelNavigation();
        this.startMove(0, -1);   // Turn right
        break;
    case ' ':
        this.cancelNavigation();
        this.stopMove();         // Emergency stop
        break;
}

}

## 4.18 Behaviour Arbitration and Mode Switching

### 4.18.1 Priority-Based Control Architecture

The priority hierarchy, from highest to lowest, is: manual control, obstacle avoidance, and
goal-seeking navigation.

``` {language=""}
cancelNavigation() {
    if (this.navigationMode !== 'manual') {
        this.navigationMode = 'manual';
        this.currentGoal    = null;

// Remove goal visualisation
if (this.goalMarker) {
    this.scene.remove(this.goalMarker);
    this.goalMarker = null;
}

// Update UI status
this.updateNavigationStatus('connected', 'Ready');
document.getElementById('nav-mode').textContent    = 'Manual';
document.getElementById('current-goal').textContent = 'None';

        this.log('[X] Navigation cancelled');
    }
}

// Emergency stop (highest priority)
emergencyStop() {
    this.cancelNavigation();
    this.stopMove();
    this.log('[!] EMERGENCY STOP');
}

4.18.2 Goal Management

setGoal(x, y) {
    this.currentGoal    = { x: x, y: y };
    this.navigationMode = 'navigating';

    // Visual feedback
    this.createGoalMarker(x, y);
    this.updateNavigationStatus('navigating',
        `Navigating to (${x.toFixed(1)}, ${y.toFixed(1)})`);
    this.log(`[*] Goal set: (${x.toFixed(1)}, ${y.toFixed(1)})`);
}

goalReached() {
    // Success state
    this.navigationMode = 'manual';
    this.currentGoal    = null;

// Stop robot this.publishVelocity(0, 0);

// Clean up visualisation if (this.goalMarker) { this.scene.remove(this.goalMarker); this.goalMarker = null; }

// UI feedback this.updateNavigationStatus('connected', 'Goal reached!'); this.log('! Goal reached!');

    // Auto-reset status after 3 seconds
    setTimeout(() => {
        this.updateNavigationStatus('connected', 'Ready');
    }, 3000);
}

4.19 User Interface and Goal Setting

4.19.1 Interactive Goal Selection

Goal positions are set by clicking on the 3D viewport. A raycaster projects the mouse click through the camera onto the ground plane ($y=0$):

handleClick(event) {
    const rect  = this.renderer.domElement.getBoundingClientRect();
    const mouse = new THREE.Vector2();


```javascript
// Convert screen coordinates to normalised device coordinates
mouse.x =  ((event.clientX - rect.left) / rect.width)  * 2 - 1;
mouse.y = -((event.clientY - rect.top)  / rect.height) * 2 + 1;

// Raycast from camera through mouse position
const raycaster = new THREE.Raycaster();
raycaster.setFromCamera(mouse, this.camera);

// Intersect with ground plane (y = 0)
const groundPlane      = new THREE.Plane(new THREE.Vector3(0, 1, 0), 0);
const intersectionPoint = new THREE.Vector3();

if (raycaster.ray.intersectPlane(groundPlane, intersectionPoint)) {
    // Set navigation goal at clicked location
    this.setGoal(intersectionPoint.x, intersectionPoint.z);
}

}

### 4.19.2 Speed Control Interface

``` {language=""}
// Dynamic speed adjustment via slider
document.getElementById('speed-slider').addEventListener('input', (e) => {
    this.currentSpeed = parseFloat(e.target.value);
    document.getElementById('speed-value').textContent =
        `${this.currentSpeed} m/s`;
});

// Speed affects all motion commands
startMove(linear, angular) {
    const linVel = linear  * this.currentSpeed;  // Scale by user setting
    const angVel = angular * this.currentSpeed;
    this.publishVelocity(linVel, angVel);
}

4.20 Control Algorithm Performance Analysis

4.20.1 Control Loop Timing

// Navigation control frequency
const CONTROL_FREQUENCY = 10;   // Hz
const CONTROL_PERIOD    = 100;  // ms

// Performance characteristics:
// - Response time:    ~100--200 ms (1--2 control cycles)
// - Settling time:    ~1--2 s for heading corrections
// - Steady-state error: +/-0.3 m position, +/-0.3 rad heading

The real-time constraint requires:

4.20.2 Stability and Robustness

Proportional Control Stability:

• No integral term: Prevents windup but allows steady-state position error
• No derivative term: Reduces noise sensitivity but gives slower response
• Saturation limits: Prevent actuator overdriving ($|\omega |\le 1.0\cdot v_{\mathrm{speed}}$)

Obstacle Avoidance Robustness:

• Conservative threshold: 0.8 m safety margin prevents collisions
• Sensor filtering: Invalid laser readings are excluded before processing
• Graceful degradation: Falls back to manual control on sensor failure

// Error handling and fault tolerance
if (!this.laserScan) {
    // No sensor data -- disable autonomous navigation
    return { needsAvoidance: false, angularVel: 0 };
}

if (!this.connected) {
    // Communication failure -- disable all motion
    this.disableControls();
    return;
}

4.21 Navigation Performance Metrics

4.21.1 Real-Time Performance Monitoring

// Goal tracking metrics
if (this.currentGoal) {
    const distance = Math.sqrt(
        Math.pow(this.currentGoal.x - this.robotPose.x, 2) +
        Math.pow(this.currentGoal.y - this.robotPose.y, 2)
    );
    document.getElementById('goal-distance').textContent =
        `${distance.toFixed(2)} m`;
}

// Obstacle detection metrics
const validRanges = this.laserScan.ranges.filter(range =>
    range > this.laserScan.range_min &&
    range < this.laserScan.range_max
);
const nearestObstacle = Math.min(...validRanges);
document.getElementById('nearest-obstacle').textContent =
    `${nearestObstacle.toFixed(2)} m`;

4.21.2 Navigation Success Criteria

Goal Reaching:

• Position tolerance: 0.3 m (configurable via this.goalTolerance)
• Success rate: Depends on environment complexity and obstacle density
• Path efficiency: No explicit path planning — direct line-of-sight approach

Collision Avoidance:

• Safety margin: 0.8 m obstacle threshold
• Reaction time: 100 ms (one control cycle)
• False positive tolerance: Conservative bias toward safety

Summary

The script implements a hybrid reactive-deliberative navigation system with the following key characteristics.

Control Algorithms:

1. Proportional Control (Equations (1)–(5)): Simple, stable goal-seeking based on distance and heading error
2. Potential Fields (Equations (6)–(8)): Reactive obstacle avoidance with repulsive force scaling
3. Behaviour Arbitration: Priority-based switching between manual, avoiding, and navigating modes

Architecture Benefits:

• Real-time performance: 10 Hz control loop satisfying Equation (9)
• User-friendly: Interactive goal setting and immediate manual override
• Fault-tolerant: Graceful handling of sensor and communication failures
• Modular design: Clear separation between behaviours

Performance Trade-offs:

• Simplicity vs. Optimality: Direct approach rather than optimal path planning; may fail in maze-like environments
• Reactivity vs. Planning: Immediate response to obstacles but susceptible to local minima
• Safety vs. Efficiency: Conservative 0.8 m threshold reduces collision risk at the cost of navigable corridor width

This navigation system is ideal for teleoperation, demonstration, and educational applications where simplicity, responsiveness, and user interaction are more important than optimal path planning or complex behaviour coordination.