Closing the Perception-Action Loop for Semantically Safe Navigation in Semi-Static Environments

TL;DR

The paper tackles robust long-term navigation in semi-static environments by closing the perception-action loop: it builds an object-level, semantically annotated TSDF map, estimates object consistency, and encodes this rich map into a discrete-time CBF. An MPC-CBF controller then plans safe trajectories that adapt to potential scene changes, balancing safety with feasibility. Key contributions include a unified object-map-to-CBF pipeline, the integration of semantic/consistency cues via Slope and Bias heuristics, and validated performance in both simulation and real-world experiments demonstrating dynamic behavior adjustment in response to scene changes. This approach enhances practical safety and adaptability for autonomous robots operating beyond static environments.

Abstract

Autonomous robots navigating in changing environments demand adaptive navigation strategies for safe long-term operation. While many modern control paradigms offer theoretical guarantees, they often assume known extrinsic safety constraints, overlooking challenges when deployed in real-world environments where objects can appear, disappear, and shift over time. In this paper, we present a closed-loop perception-action pipeline that bridges this gap. Our system encodes an online-constructed dense map, along with object-level semantic and consistency estimates into a control barrier function (CBF) to regulate safe regions in the scene. A model predictive controller (MPC) leverages the CBF-based safety constraints to adapt its navigation behaviour, which is particularly crucial when potential scene changes occur. We test the system in simulations and real-world experiments to demonstrate the impact of semantic information and scene change handling on robot behavior, validating the practicality of our approach.

Figures (7)

• Figure 1: Our system takes in semantically annotated RGB-D frames to localize and construct an object-aware volumetric map. Each object holds a semantic label and a consistency score. The map is transformed into a CBF, which is utilized by an MPC to plan safe paths around objects. In our framework, objects that are likely-static, such as walls, and those having higher consistency scores, will have smaller unsafe regions around them.
• Figure 2: Our closed-loop pipeline (Section \ref{['sec:problem_formulation']}) for semantically safe navigation in semi-static environments. The system maintains an object library and takes in semantically segmented RGB-D frames at each timestep. A modified ORB-SLAM3 provides pose estimates using features from likely-static objects. From the estimated pose, observations are associated to mapped objects and the consistency is updated for each object (Section \ref{['sec:bayesian_update']}). From the joint object map, an object-aware CBF is constructed, that takes object semantics and consistency into account (Section \ref{['sec:construct_cbf']}). Finally, an MPC leverages the CBF-based safety constraints to compute actions for the robot (Section \ref{['sec:mpc_cbf']}).
• Figure 3: Visualization of the proposed heuristics for CBFs around objects. The Slope Heuristic scales down the slope of the CBF for uncertain objects, inflating both the unsafe region (red) and the cautious region (orange), causing the robot to act more conservatively. The Bias Heuristic only inflates the unsafe region around likely-dynamic objects to create a larger safety buffer, and the robot's behavior is unaffected outside. The two heuristics can be combined to achieve the desired behaviours.
• Figure 4: Visualization of the object-aware CBFs around the drawer object under different stationarity labels and consistency estimates.
• Figure 5: Ground-truth robot trajectories with different values of $\bar{\gamma}$ in the MPC-CBF safety constraint (\ref{['subeq:cbf-constraint-in-mpc']}).