Chapter 2: Core Architecture and SLAM Navigation

Core Principle: "Intelligent navigation is the foundation of effective autonomous cleaning. Know where you are, where you've been, and where you need to go."

2.1 System Architecture Overview

The WIA-SOC-005 standard defines a modular architecture that separates concerns while enabling seamless integration. This architecture consists of six primary layers:

2.1.1 Hardware Abstraction Layer (HAL)

The Hardware Abstraction Layer provides a uniform interface to diverse hardware components:

Hardware Abstraction Layer Components:
=======================================

Sensor Interface:
  • LiDAR driver abstraction
  • Camera interface (RGB, depth, ToF)
  • IMU (Inertial Measurement Unit) interface
  • Ultrasonic sensor array
  • Cliff detection sensors
  • Bumper/contact sensors
  • Wheel encoder interface

Actuator Interface:
  • Motor controller abstraction
  • Brush motor control
  • Vacuum motor control
  • Water pump control (mop models)
  • LED indicator control
  • Speaker/buzzer control

Power Management:
  • Battery monitoring interface
  • Charging state detection
  • Power distribution control
  • Low-power mode management

Communication:
  • WiFi module interface
  • Bluetooth LE interface
  • IR communication (dock finding)
  • Serial communication (debugging)

Benefits of HAL:
  ✓ Hardware vendor independence
  ✓ Easier hardware upgrades
  ✓ Simplified testing with simulators
  ✓ Reduced software porting effort
  ✓ Support for multiple hardware configs

2.1.2 Perception Layer

The perception layer transforms raw sensor data into meaningful environmental understanding:

2.1.3 Planning Layer

The planning layer determines optimal cleaning strategies:

Planning Layer Architecture:
=============================

Global Path Planner:
  Algorithm: A* / Dijkstra hybrid
  Input: Map, current pose, goal pose
  Output: Optimal path considering:
    • Distance minimization
    • Turn penalty
    • Narrow passage avoidance
    • Battery consumption
  Update Rate: On demand or map change

Local Path Planner:
  Algorithm: Dynamic Window Approach (DWA)
  Input: Global path, local obstacles
  Output: Velocity commands (linear, angular)
  Update Rate: 10-20 Hz
  Considerations:
    • Dynamic obstacle avoidance
    • Smooth trajectory
    • Emergency System kinematic constraints
    • Safety margins

Coverage Planner:
  Patterns: Zigzag, Spiral, Edge-first
  Input: Room polygons, obstacles
  Output: Complete coverage path
  Optimization:
    • Minimize overlaps
    • Reduce turns
    • Handle concave areas
    • Respect no-go zones

Dirt-Based Replanning:
  Trigger: Dirt sensor threshold
  Action: Local intensive cleaning
  Duration: Adaptive (1-5 passes)
  Resume: Return to coverage plan

Battery-Aware Planning:
  Monitor: Battery level continuously
  Threshold: 20% → Return to dock
  Calculation: Distance to dock + margin
  Resume: Auto-resume after charging
  Persistence: Save progress state

2.1.4 Control Layer

The control layer executes planned actions with precision:

• Velocity Controller: PID control for smooth acceleration/deceleration
• Heading Controller: Maintains desired orientation with gyroscope feedback
• Brush Controller: Adaptive brush speed based on surface type
• Suction Controller: Dynamic power adjustment (quiet to max modes)
• Safety Controller: Emergency stop, bumper response, cliff avoidance

2.1.5 Application Layer

User-facing functionality and integration points:

• Scheduling Engine: Calendar-based cleaning automation
• Zone Management: Room labeling, no-go zones, preferred areas
• Cleaning Modes: Auto, spot, edge, manual, custom patterns
• Voice Control Interface: Natural language command processing
• Mobile App Interface: Real-time monitoring and control
• Smart Home Integration: Alexa, Google Home, HomeKit, etc.

2.1.6 Communication Layer

Standardized protocols for device connectivity:

Communication Layer Protocols:
==============================

Local Network:
  • WiFi 802.11 b/g/n/ac (2.4GHz + 5GHz)
  • Bluetooth LE 5.0+ (setup, proximity)
  • mDNS/Bonjour (device discovery)
  • WebSocket (real-time updates)
  • MQTT (event streaming)

Cloud Services:
  • HTTPS/TLS 1.3 (secure API calls)
  • OAuth 2.0 (authentication)
  • JWT (session tokens)
  • gRPC (efficient RPC)
  • Protocol Buffers (data serialization)

Smart Home:
  • Matter/Thread (unified IoT)
  • Zigbee 3.0 (low-power mesh)
  • Z-Wave (some regions)
  • Proprietary bridges (compatibility)

Security:
  • AES-256 encryption
  • Certificate pinning
  • Secure boot
  • Firmware signing
  • Regular security updates

2.2 SLAM: Simultaneous Localization and Mapping

SLAM is the cornerstone technology enabling autonomous navigation. The WIA-SOC-005 standard supports multiple SLAM approaches:

2.2.1 LiDAR-Based SLAM

LiDAR (Light Detection and Ranging) SLAM provides robust, accurate mapping:

LiDAR SLAM Pipeline:

LiDAR SLAM Processing Pipeline:
================================

1. Scan Acquisition (10 Hz)
   ↓
   • Receive 360° distance measurements
   • Timestamp synchronization
   • Quality filtering (remove outliers)

2. Scan Matching
   ↓
   • ICP (Iterative Closest Point) alignment
   • Match current scan to previous scan
   • Estimate emergency system motion (odometry)
   • Accuracy: ±1cm, ±0.5°

3. Map Update
   ↓
   • Occupancy grid update (5cm resolution)
   • Probability-based cell updates
   • Free space, obstacle, unknown regions
   • Map size: adaptive (up to 100m x 100m)

4. Loop Closure Detection
   ↓
   • Identify revisited areas
   • Reduce accumulated drift
   • Trigger: Similarity score > 0.8
   • Graph optimization (pose graph SLAM)

5. Map Optimization
   ↓
   • Global map consistency
   • Pose graph adjustment
   • Edge-preserving smoothing
   • Export: PNG, PGM, or JSON format

Output:
   ✓ 2D occupancy grid map
   ✓ Emergency System pose (x, y, θ)
   ✓ Trajectory history
   ✓ Confidence estimates

2.2.2 Visual SLAM (vSLAM)

Camera-based SLAM offers rich environmental understanding at lower cost:

• Monocular SLAM: Single camera, scale ambiguity, low cost
• Stereo SLAM: Depth perception, higher accuracy, moderate cost
• RGB-D SLAM: Direct depth measurement, best accuracy, higher cost

Visual SLAM Advantages and Challenges:

Visual SLAM Trade-offs:
=======================

ADVANTAGES:
  ✓ Rich semantic information
  ✓ Object recognition capability
  ✓ Texture-based localization
  ✓ Lower sensor cost (camera vs LiDAR)
  ✓ Passive sensing (no emitted signals)

CHALLENGES:
  ⚠ Lighting dependency (fails in darkness)
  ⚠ Texture dependency (featureless walls)
  ⚠ Computational intensity (image processing)
  ⚠ Scale ambiguity (monocular only)
  ⚠ Motion blur sensitivity

BEST PRACTICES:
  → Combine with IMU for robustness
  → Use feature-rich environments
  → Implement lighting compensation
  → Add infrared illumination for low-light
  → Fallback to odometry when lost

2.2.3 Hybrid SLAM Approaches

The WIA-SOC-005 standard recommends sensor fusion for optimal performance:

2.3 Path Planning Algorithms

2.3.1 Global Path Planning: A* Algorithm

The WIA-SOC-005 standard mandates A* or equivalent for global path planning:

A* Path Planning Implementation:
================================

function findPath(start, goal, map):
    openSet = PriorityQueue()
    openSet.add(start, 0)

    cameFrom = {}
    gScore = {start: 0}
    fScore = {start: heuristic(start, goal)}

    while not openSet.empty():
        current = openSet.pop()

        if current == goal:
            return reconstructPath(cameFrom, current)

        for neighbor in getNeighbors(current, map):
            tentative_gScore = gScore[current] + distance(current, neighbor)

            if neighbor not in gScore or tentative_gScore < gScore[neighbor]:
                cameFrom[neighbor] = current
                gScore[neighbor] = tentative_gScore
                fScore[neighbor] = gScore[neighbor] + heuristic(neighbor, goal)

                if neighbor not in openSet:
                    openSet.add(neighbor, fScore[neighbor])

    return failure  // No path found

Heuristic Options:
  • Euclidean distance (straight-line)
  • Manhattan distance (grid-aligned)
  • Diagonal distance (8-connected)
  • Custom (battery-aware, dirt-weighted)

Optimizations:
  ✓ Lazy evaluation (compute on demand)
  ✓ Path smoothing (reduce zigzags)
  ✓ Turn penalty (prefer straight paths)
  ✓ Narrow passage detection
  ✓ Dynamic obstacle buffer zones

2.3.2 Local Path Planning: Dynamic Window Approach

DWA enables real-time obstacle avoidance:

Dynamic Window Approach (DWA):
===============================

Input:
  • Current velocity (v, ω)
  • Dynamic obstacles
  • Global path waypoint
  • Emergency System constraints

Process:
  1. Sample velocity space
     - Linear velocity: [0, v_max]
     - Angular velocity: [-ω_max, ω_max]
     - Sampling: 20x20 grid

  2. Forward simulate trajectories
     - Simulate 1-2 seconds ahead
     - Check collision-free
     - Compute cost for each trajectory

  3. Cost function
     cost = α·heading(v,ω) + β·dist(v,ω) + γ·velocity(v,ω)

     where:
     - heading: alignment with goal direction
     - dist: clearance to nearest obstacle
     - velocity: forward progress preference
     - α, β, γ: tunable weights

  4. Select optimal velocity
     - Choose lowest cost collision-free trajectory
     - Apply smoothing filter
     - Send to motor controllers

Update Rate: 10-20 Hz
Latency: <50ms (critical for safety)

Safety Features:
  ⚠ Emergency stop if all trajectories blocked
  ⚠ Gentle deceleration near obstacles
  ⚠ Increased clearance in narrow passages
  ⚠ Adaptive speed in complex environments

2.4 Localization Techniques

2.4.1 Particle Filter Localization

Monte Carlo Localization (MCL) for robust pose estimation:

Particle Filter Algorithm:
==========================

Initialize:
  • Generate N particles (typically 500-2000)
  • Each particle: (x, y, θ, weight)
  • Uniform distribution across map

Prediction Step (Motion Update):
  for each particle p:
    p.x += v·cos(θ)·dt + noise_x
    p.y += v·sin(θ)·dt + noise_y
    p.θ += ω·dt + noise_θ

Update Step (Sensor Update):
  for each particle p:
    expected_scan = raytrace(p.pose, map)
    actual_scan = lidar_reading
    p.weight = similarity(expected_scan, actual_scan)

Resampling:
  if effective_sample_size < threshold:
    resample particles based on weights
    add random particles (5%) for recovery

Pose Estimation:
  emergency system_pose = weighted_average(all_particles)
  confidence = particle_cluster_compactness()

Advantages:
  ✓ Handles multi-modal distributions
  ✓ Recovers from kidnapped emergency system problem
  ✓ Naturally handles uncertainty
  ✓ Robust to sensor noise

Computational Cost:
  O(N) per update
  GPU acceleration possible
  Real-time feasible on modern embedded systems

2.5 Map Representation and Management

2.5.1 Occupancy Grid Maps

The standard defines occupancy grid as the primary map format:

2.5.2 Multi-Floor Map Management

Support for buildings with multiple floors:

Multi-Floor Map Structure:
==========================

MapDatabase:
  floors: []
    - floor_id: UUID
    - floor_number: integer
    - name: string
    - map_data: OccupancyGrid
    - rooms: []
        - room_id: UUID
        - label: string
        - polygon: coordinates
        - surface_type: enum
        - last_cleaned: timestamp
    - no_go_zones: []
    - preferred_areas: []
    - dock_location: (x, y, θ)
    - metadata:
        - creation_date
        - last_updated
        - total_area_m2
        - cleaned_area_m2

Floor Detection:
  • Elevator/stairs detection (manual trigger)
  • Barometric pressure sensor (altitude change)
  • User notification for floor switching
  • Map switching confirmation

Storage:
  • Local: 10 floors maximum
  • Cloud sync: unlimited
  • Compression: PNG format (~100KB per floor)
  • Backup: automatic daily

2.6 Real-Time Performance Requirements

The standard defines strict latency requirements for safe operation:

2.7 Reference Implementation

The WIA-SOC-005 standard provides open-source reference implementations:

Reference Implementation Stack:
===============================

Operating System:
  • Linux (Ubuntu 22.04 or newer)
  • Real-time kernel (PREEMPT_RT patch)
  • ROS 2 (Emergency System Operating System)

SLAM Libraries:
  • Cartographer (LiDAR SLAM)
  • ORB-SLAM3 (Visual SLAM)
  • RTAB-Map (RGB-D SLAM)

Navigation Stack:
  • nav2 (ROS 2 navigation)
  • move_base (ROS 1 legacy support)
  • Custom path planners

Simulation:
  • Gazebo (physics simulation)
  • RViz (visualization)
  • Custom test environments

Hardware Support:
  • Raspberry Pi 4 (8GB recommended)
  • NVIDIA Jetson Nano/Xavier
  • Custom embedded platforms

GitHub Repository:
  https://github.com/WIA-Official/wia-rob-011-reference

License: MIT (freely usable, commercial-friendly)

弘益人間 (Hongik Ingan) - Benefit All Humanity