This project enhances the autonomy of a grass-cutting robot (Clearpath Moose) to operate on uneven, hilly terrain. The system addresses slope hazards through three core software components:
- Extended Kalman Filter (EKF): 6-state filter
[x, y, z, roll, pitch, yaw]fusing wheel odometry with GPS, IMU and compass measurements for accurate localization. - 3D LiDAR Mapping: Builds an OctoMap-based occupancy grid to represent complex terrain.
- Slope-Aware Navigation: Uses an A* planner with a custom cost function to identify and avoid hazardous gradients (>20°) while maintaining coverage.
The core logic for this project is found in the following file:
.../moose/moose/controllers/moose_path_following/moose_path_following.c
- Webots R2022a
| Metric | Value |
|---|---|
| EKF localization accuracy | RMSE = 4 mm |
| LiDAR frames processed | 4,735+ (800K+ points) |
| Elevation grid coverage | 86.6 % |
| Navigation goals reached | 6+ |
| Tip-over hazard threshold | 20.05° (0.35 rad roll/pitch, 32 ms safety loop) |
| Traversability cost weights | slope × 200 + roughness × 55 |
Watch on YouTube: https://youtu.be/MwyOcCSr9X8 · Download the mp4
- Modified Hardware Interface: Custom
Moose.protowith 8-wheel PositionSensors enabled closed-loop odometry. - Differential Drive Logic: Converts raw encoder ticks into metric state estimates (x, y, θ) using skid-steering kinematics.
- State Prediction: Propagates system uncertainty (Covariance Matrix P) based on motion model.
- Update Step: Fuses GPS position (x, y, z), IMU roll/pitch and magnetometer (compass) yaw with the prediction to eliminate odometric drift.
- Auto-Calibration: Solves coordinate frame mismatch by aligning odometry θ with the initial compass reading.
- LiDAR Pipeline: Filters point clouds (0.3m < d < 50m) and downsamples on a 0.1 m voxel grid (200³ cells).
- Occupancy Grid: Probabilistic log-odds update model with dynamic map recentring for unbounded exploration.
- Traversability Analysis: Converts 3D data into a 2D cost map, penalizing surface roughness and slopes.
- Slope-Aware A:* Global planner that treats gradients > 20° as obstacles.
- Hazard Detection: High-frequency (32ms) safety layer that monitors IMU Roll/Pitch to prevent tipping.
- Stuck Recovery: Monitors displacement over 5s windows; triggers reversing manoeuvres if the robot is physically stuck.
This project was a collaborative effort. The specific contributions for assessment purposes are listed below.
Role: Robot Modelling, Low-level Control & Kinematics
File: protos/Moose.proto, controllers/moose_path_following/moose_path_following.c
Key Contributions:
- Modified
Moose.prototo add 8-wheel feedback sensors. - Implemented low-level C driver for motor/sensor interfacing.
- Developed Differential Drive Odometry and EKF Prediction step.
Role: Sensor Fusion, Calibration & Validation
File: controllers/moose_path_following/moose_path_following.c
Key Contributions:
- Implemented EKF Update step (Compass fusion).
- Developed the "Auto-Alignment" routine to fix coordinate frame inversion.
- Built the GPS Ground Truth validation system.
Role: 3D Mapping & Environment Reconstruction, Sensor Fusion (EKF)
File: controllers/moose_path_following/moose_path_following.c
Key Contributions:
- Built the LiDAR processing pipeline (Filtering, Voxel Grid, Transform).
- Implemented OctoMap-style occupancy grid and ICP Scan Matching.
- Developed the Traversability Cost function (Slope + Roughness).
- Co-developed the 6-state EKF sensor fusion (GPS + IMU + compass).
Role: Path Planning, Hazard Detection & Evaluation
File: controllers/moose_path_following/moose_path_following.c
Key Contributions:
- Implemented the slope-aware A* global planner.
- Developed the IMU-based Hazard Detection safety layer.
- Integrated the Navigation Controller (Path Following + Performance Metrics).