OMEGA++: Semantic Mapping & Kinodynamic Planning

• OMEGA++ Code is an open-source system for high-efficiency semantic occupancy mapping with integrated kinodynamic planning in dynamic environments using air-ground robots.
• The architecture features asynchronous ROS pipelines combining OccMamba for state-of-the-art semantic mapping and AGR-Planner for hybrid, ESDF-free path planning.
• Empirical results demonstrate state-of-the-art mIoU and planning speed, significantly outperforming previous AGR navigation systems.

OMEGA++ Code is an open-source, high-efficiency C++/Python system for semantic occupancy mapping and planning in dynamic environments with air-ground robots (AGRs). It integrates state-of-the-art 3D semantic-occupancy prediction (OccMamba) with an energy- and kinodynamic-aware hybrid planner (AGR-Planner), enabling real-time navigation under severe occlusions without reliance on ESDF or brute-force voxel traversals. OMEGA++ achieves state-of-the-art mean IoU (mIoU) in dynamic scenes and demonstrates superior planning speed and success compared to prior AGR navigation systems (Wang et al., 2024).

1. System Organization and Core Modules

OMEGA++ is structured as two asynchronously running ROS pipelines on robot hardware:

• OccMamba: Real-time 3D semantic-occupancy estimation from LiDAR or depth input, outputting a dense tensor $$O \in \mathbb{R}^{(C+1) \times L \times W \times H}$$, where $C$ is the number of semantic classes, with defaults $(L, W, H) = (256, 256, 32)$.
• AGR-Planner: Hybrid pathfinding and trajectory optimization operating on the rolling local occupancy grid generated by OccMamba, using ESDF-free, kinodynamic A* and gradient-based B-spline methods.

A zero-copy, query-based map update protocol, derived from AGRNav, interconnects these modules by exposing OccMamba's outputs as a local grid that can be efficiently queried for collision checking and cost-field construction. All communication is ROS-native, primarily around the /omega/local_map topic.

Key Architectural Components

Module	Function	Core Technique
OccMamba	Semantic+occupancy inference	Multi-scale voxelization, Mamba state-space models, BEV fusion
AGR-Planner	Pathfinding & optimization	Kinodynamic A*, B-spline, non-ESDF cost fields

OccMamba Architecture

OccMamba splits feature extraction into:

• Sem-Branch: Semantic Mamba blocks per scale, linear-complexity 3D state-space modeling.
• Geo-Branch: Geometry Mamba blocks, geometric free/occupied prediction.
• BEV Fusion: Channel-wise U-Net encoder/decoder with cross-scale, cross-branch feature fusion using SSC-RS modules in BEV space.

Multi-scale, sparse input voxelization at $s_1 = 0.2$ m, $s_2 = 0.4$ m, $s_3 = 0.8$ m yields three tensors, each feeding into separate Sem- and Geo-Mamba blocks. BEV fusion operates via learned upsampling and skip connections, outputting semantic-occupancy logits.

AGR-Planner Design

Planning comprises:

• Kinodynamic A* search over discrete motion primitives and hybrid modes (ground/air), optimizing control sequence $u_t$ for cost $c(x_t, u_t)$.
• Trajectory post-optimization via B-splines parameterized by control points $Q$, subject to smoothness, collision, and dynamic limits.

2. Mathematical Formulation and Algorithms

OccMamba State-Space Modeling

Each Mamba block uses a continuous-time state-space model, discretized as: $$h_k = \bar{A} h_{k-1} + \bar{B} x_k, \qquad y_k = C h_k$$ where

$$\bar{A} = \exp(\Delta A), \quad \bar{B} = (\Delta A)^{-1} \left( \exp(\Delta A) - I \right) \Delta B$$

A global convolution kernel $K$ applies via sequence-wise depthwise convolution for linear complexity. Semantic and geometric loss functions incorporate cross-entropy, BCE, and Lovász terms across all scales and BEV outputs for deep supervision.

Planner Optimization

Kinodynamic A* minimizes the aggregate cost $J_{A^*} = \sum_t c(x_t, u_t)$ under dynamic constraints $|v| \leq v_{\max}^{\text{mode}}$, $|\omega| \leq \omega_{\max}$, $x_{t+1} = f(x_t, u_t)$. B-spline trajectory $\Theta(u;Q)$ of order $p_b$ and $N_c$ control points is optimized: $$\min_Q J = \lambda_s J_{\text{smooth}} + \lambda_c J_{\text{coll}} + \lambda_d J_{\text{dyn}} + \lambda_t J_{\text{term}} + \lambda_n J_{\text{curv}}$$ with smoothness $J_{\text{smooth}}$, collision barrier $J_{\text{coll}}$, dynamic $J_{\text{dyn}}$, terminal $J_{\text{term}}$, and curvature $J_{\text{curv}}$ costs evaluated explicitly from B-spline basis derivatives.

3. Codebase Organization and Usage

OMEGA++ is distributed as a ROS-based software suite. The key directories and files are:

Directory/File	Purpose
occ_mamba/	Semantic-occupancy network code
planner/	Path planning and optimization
scripts/	Helper scripts for training/inference
launch/	ROS launch files for modular startup

Environment setup:

• Ubuntu 20.04, Conda, CUDA 11.x, and ROS Noetic are prerequisites.
• Specific Python packages (e.g., PyTorch 1.12.1+cu113, spconv v2.2) are required.
• Training and evaluation scripts interface with config files found under configs/.
• Real-time deployment is triggered via ROS launch files; OccMamba and Planner initialize in parallel, consuming /pointcloud and outputting /omega/local_map, /cmd_vel respectively.

Example workflow includes:

1. Editing YAML config files for batch size, learning rates, SSM hidden dimension, voxel resolution.
2. Launching training and inference with provided bash scripts.
3. Real-time operation by launching occmamba.launch and planner.launch with appropriate parameters.
4. For result replication, default hyperparameters and TensorRT-optimized OccMamba are used on embedded hardware (e.g., Jetson Xavier NX).

4. Empirical Results and Performance Comparison

OMEGA++ achieves significant improvements on SemanticKITTI and in planning benchmarks:

Method	IoU (%)	mIoU (%)	FPS	Params (M)	Mem (GB)
Monoscene	34.2	11.1	1.1	149.6	20.3
OccFormer	34.5	12.3	1.8	81.4	21.0
SSC-RS	59.7	24.2	16.7	∼60	20.0
OccMamba	59.9	25.0	22.1	23.8	3.5

For dynamic path planning in simulation (200 trials):

System	Success (%)	Avg. plan time (s)	Avg. move time (s)
TABV	72	7.1	22.4
AGRNav	89	6.5	18.9
OMEGA++	98	0.8	16.1

Ablation studies confirm the importance of Sem-Mamba and Geo-Mamba blocks: removing these leads to a decrease in both mIoU and planner success.

5. Implementation Considerations and Tuning

Efficient deployment considerations include:

• Voxel resolution can be increased (e.g., to 0.4 m) for faster inference in less cluttered domains.
• Mamba hidden dimension $N$ may be reduced to 32 to fit GPUs <2 GB, at a minor IoU penalty (–2%).
• For highly cluttered scenes, increase planner’s $\lambda_c$ to 50 to enhance obstacle margin.
• Curvature threshold $C_\text{max}$ should match platform geometry, typically $[0.1, 0.3]$ m⁻¹.
• Sensor extrinsics and calibration are tunable via configs/sensor.yaml.

Batch size (default 6), learning rate (default 1e-3, decayed at epoch 50), and Lovász/CE weights are set to balance training stability and accuracy. For most environments, less than 3.5 GB GPU memory is required for OccMamba inference.

6. Context and Significance

OMEGA++ is positioned as a tractable, high-performance AGR navigation system in dynamic, heavily occluded environments. Key advances include the linear-complexity Mamba blocks for semantic-occupancy mapping and the ESDF-free, hybrid kinodynamic planning pipeline. By unifying state-space models, efficient BEV fusion, and fast, on-device trajectory optimization, OMEGA++ represents a practical solution for real-time robotics scenarios requiring robust occlusion reasoning and dynamic pathfinding. This suggests direct applicability to surveillance, disaster response, and any environment where rapid, reliable navigation is critical despite perception uncertainties (Wang et al., 2024).