Mobile Robot Navigation & Control

Dynamic Navigation · Predictive Occupancy · MPPI · Embedded Control

Research Assistant · Prof. Mingyi Liu · GTIIT
Sep. 2026 – Present

Can short-horizon occupancy prediction help a robot navigate around moving obstacles when predictions are uncertain, stale, and imperfect?

I built a ROS 2/Gazebo testbed for the omnidirectional ROSMASTER M1 to investigate this question, separating predictive accuracy from downstream navigation outcomes. The platform connects local guidance and Nav2 MPPI to controlled evaluation; a parallel STM32/FreeRTOS workstream addresses low-level motion control.

My Contributions

Built the ROS 2/Gazebo navigation and controlled-evaluation platform for an omnidirectional M1 robot.
Developed the Nav2 controller wrapper combining kinodynamic A* local guides, MPPI, asynchronous replanning, and global-plan fallback.
Integrated a frozen pretrained occupancy predictor through a custom C++ local-costmap layer and designed rosbag and prediction-on/off evaluations.
Refactored STM32/FreeRTOS motion control into a single 10 ms owner task with command arbitration, timeout braking, and optional anti-windup wheel-speed PI.

NavFn supplies the global path. When the hybrid controller is enabled, a worker asynchronously generates kinodynamic local guides from a local-costmap snapshot; MPPI tracks the accepted guide, with a global-plan fallback if no fresh guide is available. The wrapper is optional: the default configuration retains native MPPI.

Source-derived navigation architecture with sensing, optional SCOPE, kinodynamic guidance, MPPI, command filters, and a separate embedded-control workstream — Current source-derived navigation and control architecture. Dashed paths are optional; the embedded strip represents a separate firmware workstream. Controller source ↗

Measured obstacles and optional SCOPE costs feed the local costmap used by guidance and control. Navigation commands pass through the velocity smoother, collision monitor, and final watchdog before reaching the Gazebo base. Planning, prediction, and final command authority therefore remain inspectable at separate interfaces.

Predictive Occupancy

SCOPE predicts occupancy from aligned scan history and odometry. I reproduced, integrated, and evaluated a frozen pretrained checkpoint; I did not author or retrain the model. A custom C++ Nav2 layer adds uncertainty-aware, coordinate-consistent prediction costs to the local costmap without clearing measured obstacles. Stale outputs withdraw only the prediction layer’s contribution. The integration remains off by default.

Eight-panel SCOPE example showing aligned occupancy history, predictions, and future scan-derived occupancy at 0.5 and 1.0 seconds — Recorded phase-03 example, selected by future dynamic-ROI occupancy rather than prediction score. Black marks are scan endpoints; aggregate results follow below.

**0.5 s prediction evaluation** · occupied IoU on 200 deterministic windows from one Gazebo bag
Method	Whole grid	Dynamic ROI
SCOPE	0.500	0.202
copy-last	0.589	0.177

Grouped bars comparing SCOPE and copy-last whole-grid and dynamic-ROI occupied IoU at a 0.5-second horizon — At 0.5 s, SCOPE improves occupied IoU inside the defined dynamic ROI while copy-last performs better over the whole grid. These are offline prediction metrics.

The localized advantage disappears at 1.0 s: dynamic-ROI occupied IoU is 0.061 for SCOPE versus 0.079 for copy-last. The results favor evaluating prediction by both region and horizon before using it in control.

Local-costmap integration details

Measured obstacles, optional additive SCOPE risk costs, inflation, and MPPI in the local costmap — Implemented plugin order: obstacle layer → SCOPE layer → inflation. The predictor modifies local costs; it does not publish velocity commands.

The online entry uses ten 10 Hz history frames and a 0.5 s horizon. Paired prediction and uncertainty grids must share their timestamp and geometry. Full fusion thresholds and expiry behavior are documented in the integration specification ↗.

Closed-Loop Evaluation

The next question is whether prediction improves navigation. I compared prediction-on and prediction-off runs with the same seed, start, and goal, keeping bag-based accuracy separate from the navigation outcome.

SCOPE off

6.855 s to complete the goal, with zero recoveries.

SCOPE on

Timeout · 20 recoveries under the enabled risk-layer configuration.

In this single fixed-seed trial pair, the enabled layer added persistent high-cost regions while MPPI repeatedly failed to find an executable trajectory. Excessive risk costs followed by inflation are a plausible explanation for the lost feasible space, motivating a fusion-policy ablation. This is a hypothesis from the recorded behavior, not an isolated causal result.

These historical SCOPE comparisons predate the hybrid controller and evaluate the earlier Nav2 MPPI configuration. They are not a benchmark of the architecture’s newer local-guidance path.

Recorded top-down Rosmaster M1 trajectory through a Gazebo scene with static and moving obstacles — Recorded trajectory from a separate dynamic-obstacle Gazebo run, included as testbed context. The SCOPE on/off pair is documented in the closed-loop report ↗.

From Navigation Commands to Wheel Control

High-level navigation and low-level actuation have different timing and ownership requirements. In the vendor STM32/FreeRTOS firmware, I assigned motion state and motor-output ownership to a single 10 ms task so asynchronous UART, CAN, and remote-control handlers submit commands through one arbitration boundary.

The owner selects the active command, converts body motion into mecanum wheel references, and applies timeout braking when commands expire. I added an optional anti-windup PI wheel-speed loop; the default retains legacy PID. This structure makes command selection, feedback, saturation, and braking part of one periodic control path.

Command arbitration feeding a single 10 millisecond wheel-control task, with encoder feedback and timeout braking — Single-owner firmware design. The wheel-speed PI shown is optional; firmware verification and physical deployment status are summarized below.

Engineering Diagnostics

LiDAR renderer failure isolation · command-authority tracing · odometry slip

LiDAR failure isolation

I traced recurring whole-frame -Inf scans to raw Gazebo output before the ROS bridge. In three 300 s runs per condition, the historical OGRE2 single-360° path latched in all three; the Ogre1 dual-180° alternative recorded zero whole-frame -Inf frames and zero NaNs.

Three-run A/B summary of OGRE2 single-360-degree GPU LiDAR latching and Ogre1 dual-180-degree merged scans — Three-run renderer A/B under the tested Gazebo condition. Timing and scan statistics are preserved in the LiDAR report ↗.

Command authority and alternative modes

I inspected the final command publisher rather than inferring actuation from a healthy controller topic. The Nav2 chain ends at the watchdog; default Imperative publishes directly to /cmd_vel, while localized Imperative uses its own watchdog. The modes run separately.

Earlier diagram comparing Nav2 and Imperative planning interfaces in the Gazebo testbed — Earlier Nav2/Imperative interface comparison; it does not depict the new kinodynamic controller wrapper.

Odometry and ground truth

The slip simulator separates ground-truth pose from the odometry supplied to localization and control. This makes odometry error an explicit test condition rather than silently giving the planner simulator truth.

Earlier overview of Gazebo sensing, odometry-slip simulation, localization, separate planner modes, and base actuation — Earlier overview of the shared simulation interfaces and available modes. The current selected control path is shown in System Architecture above.

Scope & Next Experiments

The bag-specific prediction study and single navigation pair do not establish generalized navigation performance. Matching a seed, start, and goal does not freeze obstacle timing, scheduling, or inference latency; similar minimum scan distances are not a safety margin or collision-rate estimate.

Gazebo remains the primary evaluation environment. Hardware bringup has not replicated the full simulation benchmark. The current firmware source is host-tested, but its changes have not been built with Keil/ARM or validated on physical M1 hardware, so the firmware diagrams do not imply physical closed-loop accuracy or a completed end-to-end hardware loop.

Next experiments will repeat matched scenarios across seeds and goals, control obstacle timing, and vary one fusion factor at a time—risk mapping, uncertainty weighting, inflation, or prediction age. The newer local-guidance path requires its own controlled comparisons. SCOPE remains frozen and zero-shot; the results currently support an experimentation platform and specific engineering findings.

View public platform repository