Indoor SLAM, 2D LiDAR Odometry, and Extended Kalman Filter Sensor Fusion for Search-and-Rescue
Hardware & Systems Takeaway
In GPS-denied disaster rubble, conventional mobile robots become blinded. The ADNR platform fuses 360-degree 2D LiDAR scans, wheel encoder odometry, and an 9-axis IMU through an Extended Kalman Filter (EKF) to construct real-time 2D occupancy grid maps.
Empirical Architecture Comparison: Sensor Fusion Modalities in the ADNR Disaster Platform
| Sensor Modality | Update Frequency | Measurement Contribution | Noise / Error Characterization |
|---|---|---|---|
| RPLiDAR A2 (Laser Scanner) | 10 Hz (8,000 samples/sec) | Range-bearing scans up to 12 meters | Gaussian beam noise $\sigma_r \approx 0.5$ cm; failure on mirrors/smoke |
| Optical Wheel Encoders | 50 Hz (Hardware interrupts) | Differential drive linear/angular velocity | Accumulates unbounded systematic wheel slippage drift |
| BNO055 9-DOF IMU | 100 Hz (I2C interface) | Angular rate (gyro) and linear acceleration | Gyro zero-bias drift; magnetic distortion from rebar debris |
| Fused EKF State Vector | 50 Hz real-time output | Estimates $[x, y, \theta, v, \omega]^T$ | Bounded covariance ellipse; sub-2cm localization drift |
1. Navigation in GPS-Denied Hostile Environments
During structural collapses, natural earthquakes, and industrial hazmat incidents, human first responders cannot safely enter damaged interiors. The Autonomous Disaster Navigation Robot (ADNR) is engineered for autonomous reconnaissance in rubble-strewn, dark, GPS-denied environments. The chassis utilizes a ruggedized tracked suspension with high-torque planetary DC gearmotors, providing traction over loose concrete fragments, stairs, and uneven terrain.2. The Extended Kalman Filter (EKF) Sensor Fusion Architecture
Wheel encoders provide high-frequency velocity measurements but suffer from severe drift due to wheel slip over debris. Conversely, LiDAR scan-matching (Hector SLAM / Cartographer) provides absolute spatial fixes but operates at a lower update rate (10 Hz). We couple these streams via an Extended Kalman Filter. The non-linear robot kinematic model is linearized via the Jacobian matrix $F_k$: $$\mathbf{x}_k = f(\mathbf{x}_{k-1}, \mathbf{u}_k) + \mathbf{w}_k, \quad \mathbf{z}_k = h(\mathbf{x}_k) + \mathbf{v}_k$$ $$P_k^- = F_k P_{k-1} F_k^T + Q_k$$ $$K_k = P_k^- H_k^T (H_k P_k^- H_k^T + R_k)^{-1}$$ The EKF innovation update continuously corrects the heading orientation $\theta$ using IMU gyro integration, preventing map warping during fast turns.3. ROS2 Nav2 Autonomous Path Planning & Obstacle Costmaps
Onboard computation is driven by a Raspberry Pi 5 running Ubuntu 24.04 and ROS2 Jazzy Jalisco. The Navigation 2 (Nav2) stack maintains two distinct costmaps:- Global Costmap: Ray-traces 2D occupancy grids (0 = Free space, 100 = Lethal obstacle, -1 = Unknown). Plans optimal trajectories using Smac Planner (Hybrid-A*).
- Local Costmap: 3x3 meter rolling window. Computes dynamic collision avoidance trajectories at 20 Hz using Model Predictive Path Integral (MPPI) control.