Reactive Robot-Centric Safety for Autonomous Navigation in Constrained and Dynamic Environments

Abstract: In this work, we address the problem of ensuring real-time safety in autonomous robot navigation, in spatially constrained dynamic environments, by utilizing only onboard sensors. We present a real-time control architecture that integrates a 3D LIDAR perception-based composite control barrier function(CBF)-based safety filter directly into the autonomy pipeline. The proposed perception-driven framework enforces collision avoidance constraints dynamically from onboard point cloud data, thus allowing a large number of constraints to be handled at the control frequency, while remaining minimally invasive to nominal task execution. The safety region is defined as an ellipsoid in the body-frame, consistent with the geometry of the platform, which induces time-varying constraints in the world frame as the robot rotates; this effect is handled through a dedicated formulation of time-varying (CBF) for each LIDAR point. We validate the system through multiple field experiments in underground environments by utilizing a quadruped platform performing a visual inspection task, demonstrating reliable operation in the presence of dynamic obstacles, unsafe high-level references, abrupt localization anomalies, and while traversing through narrow corridors.

• The paper introduces body-frame ellipsoidal safety envelopes and a smooth composite control barrier function that converts hundreds of LiDAR constraints into one real-time quadratic program.
• Field tests on a Boston Dynamics Spot show safe narrow-passage traversal, collision avoidance after an odometry jump exceeding 100 meters, and reactions to unmapped dynamic obstacles.
• The filter scales to 800–1300 simultaneous constraints with mean solver times below 12 microseconds and 99.6% of calls completing within 20 microseconds, though robustness to tracking errors and moving obstacles remains open.

Overview and motivation

This paper presents a real-time, perception-driven safety architecture for autonomous navigation in spatially constrained, dynamic environments, using only onboard sensing. The work is motivated by inspection missions in subterranean and industrial settings where robots must operate in close proximity to infrastructure, pedestrians, and dynamic obstacles, while internal uncertainties—perceptual aliasing, geometric degeneracy in repetitive corridors, abrupt odometry drift—can corrupt the mapping, localization, and planning layers that conventional safety mechanisms rely upon. The central claim is that safety should be enforced reactively from instantaneous LIDAR point clouds at control frequency, as a minimally invasive filter on nominal task execution, rather than being delegated to higher-level planners operating over stale or inconsistent maps.

The authors identify a specific gap: although CBF-based safety filtering is well established theoretically, systematic experimental demonstrations of perception-driven safety filtering at control frequency, with geometry-consistent body-frame envelopes and dense point cloud data, under genuinely adverse field conditions remain limited.

Robot-centric constraint formulation

The core methodological contribution is a shift from exterior, obstacle-centric constraints to robot-centric ones. Conventional reactive navigation formulations impose a spherical clearance region around each observed obstacle point. For anisotropic platforms such as quadrupeds, humanoids, or aerial manipulators, this is overly conservative: it does not preserve directional clearance margins relative to the body geometry.

The paper instead defines the safe region as an axis-aligned ellipsoid in the body frame with semi-axes $a_x, a_y, a_z$ chosen to enclose the platform footprint. Each LIDAR point $p^B_i(t)$ must satisfy:

$$\left (\frac{x^B_i}{a_x} \right )^2 + \left (\frac{y^B_i}{a_y} \right )^2 + \left (\frac{z^B_i}{a_z} \right )^2 \geq 1$$

Expressed in the world frame via the time-varying rotation matrix $R^W_B(t)$, the shape matrix becomes orientation-dependent, so even static obstacles induce time-varying constraints whenever the robot rotates. This dual interpretation—ellipsoids centered on each obstacle point but axis-aligned with the robot's body frame—is handled through a dedicated time-varying CBF formulation per point, building on time-varying CBF theory for signal temporal logic tasks (Collaboration, 2018) and time-varying soft-maximum barrier constructions [safari2024time]. A planar specialization (restricting to the $(x,y)$ subspace) is used for the quadruped experiments.

The authors are explicit about a trade-off here: because the ellipsoid's corners lie closer to its boundary than its inscribed sphere would, the major axis must be inflated slightly ($a_x = 0.9$ m versus a 0.8 m spherical radius in their comparison), yet the resulting envelope remains substantially less conservative overall in narrow passages.

Composite CBF safety filter

Enforcing one nonconvex quadratic constraint per LIDAR point within an MPC framework yields large nonlinear, nonconvex programs unsuitable for onboard real-time execution. The paper instead recasts each point constraint as a CBF under a first-order kinematic model $\dot{p}^W_r = u^W$, and aggregates all $N$ individual barriers into a single smooth composite function via a log-sum-exp soft-min operator [molnar2023composing]:

$$H(p,t) = -\frac{\gamma}{\kappa} \log \left( \sum_{i=1}^{N} \exp \left( -\kappa \tanh\!\left(\frac{h_i(p,t)}{\gamma}\right) \right) \right)$$

A key property established in the paper is set inclusion: since the soft-min under-approximates the pointwise minimum, nonnegativity of $H(p,t)$ implies satisfaction of every individual constraint, i.e., $p^B_i(t)$0. The parameters $p^B_i(t)$1 and $p^B_i(t)$2 govern sharpness and saturation of the aggregation. Because $p^B_i(t)$3 is continuously differentiable whenever each $p^B_i(t)$4 is, the time-varying CBF condition of Definition 1 can be imposed directly on the composite function, yielding a single affine constraint in $p^B_i(t)$5 inside a quadratic program solved against a proportional go-to-goal nominal controller. This reduces the entire dense-constraint problem to one QP per control cycle—a decisive computational simplification compared to nonlinear MPC formulations with per-point constraints [falanga2018pampc].

The full architecture is multi-loop: a high-level reactive local-view inspection planner [viswanathan2024surface] supplies references, a nominal position controller generates velocity commands, the CBF-QP filters them, and a platform-specific low-level controller tracks the filtered velocity. The low-level layer is deliberately abstracted and assumed to track velocity commands perfectly—an assumption whose implications are discussed below.

Experimental evaluation

Validation is conducted on a Boston Dynamics Spot quadruped equipped with an Ouster OS-0 3D LIDAR, IMU, and stereo camera, running ROS Noetic on an Intel NUC, with Voxblox for 3D mapping and CVXPY for QP construction. All points within 3.5 m of the instantaneous scan serve as constraints. Three field scenarios in underground environments are reported.

Scenario 1 — narrow passage traversal. With a standard distance-based constraint ($p^B_i(t)$6 m), overlapping spherical unsafe regions around clustered obstacle points completely occlude a physically passable gap between a misplaced object and a doorway, producing a deadlock even when the reference lies in free space. With the elliptical envelope ($p^B_i(t)$7 m, $p^B_i(t)$8 m), the robot traverses the same bottleneck: the filter intervenes only when the planner issues references near obstacles, pushes the robot laterally away, and re-aligns with the nominal command once clear. This directly demonstrates the conservatism argument motivating the robot-centric design.

Scenario 2 — odometry failure. In long, self-similar corridors, perceptual aliasing causes an odometry jump exceeding 100 m between consecutive frames. The nominal controller saturates toward a falsely perceived goal, driving the robot toward a wall; the CBF filter, relying solely on onboard scans, prevents collision and steers the robot parallel to the wall and safely through a doorway. Notably, before the jump the filter interferes minimally with the nominal input—the intended minimal-invasiveness property—and only activates when $p^B_i(t)$9 transitions negative after the jump. The authors concede that small boundary oscillations occur when the reference lies inside the unsafe region, attributed to modeling inaccuracies and mechanical limitations; forward invariance is thus maintained empirically rather than exactly at the boundary.

Scenario 3 — dynamic obstacles. A chair moved toward the robot (~9 s) and a door opened across its path (~20 s)—obstacles absent from the precomputed map—cause immediate drops in $$\left (\frac{x^B_i}{a_x} \right )^2 + \left (\frac{y^B_i}{a_y} \right )^2 + \left (\frac{z^B_i}{a_z} \right )^2 \geq 1$$0 and corresponding deviations of the filtered input from the nominal command, with convergence back to nominal once clearance is restored.

Computational performance. The strongest numerical result concerns solver timing: mean solver time remained below 12 microseconds throughout Scenario 3, with 99.6% of solver calls completing within 20 microseconds and a single outlier at 30 microseconds. Even when simultaneously enforcing 800–1300 active constraints, the QP solves fast enough for control-frequency operation. These figures substantiate the scalability claim of the composite CBF approach on resource-constrained embedded hardware.

Metric	Value
Mean solver time	< 12 µs
Solver calls within 20 µs	99.6%
Worst-case solver call	30 µs
Simultaneous constraints enforced	800–1300
Odometry jump tolerated	> 100 m

Limitations and open questions

Several limitations are stated or implicit in the work. First, the safety guarantee rests on a first-order kinematic model with perfect velocity tracking by the low-level controller; unmodeled dynamics, actuation limits, and tracking error are acknowledged only qualitatively as sources of boundary oscillations, and no formal robustness margin (e.g., input-to-state safety) is quantified. Second, the ellipsoid semi-axes were hand-tuned per scenario ($$\left (\frac{x^B_i}{a_x} \right )^2 + \left (\frac{y^B_i}{a_y} \right )^2 + \left (\frac{z^B_i}{a_z} \right )^2 \geq 1$$1 m in the constricted corridor versus 0.6 m in wider tunnels), leaving open how these parameters should be selected or adapted automatically. Third, the vertical dimension was ignored in the planar specialization used for the quadruped, so full 3D enforcement is demonstrated in formulation but not in experiment. Fourth, the theoretical treatment assumes static obstacles; dynamic obstacles are handled implicitly by re-deriving constraints from fresh scans each cycle, without accounting for obstacle motion between scans. Finally, whether the composite soft-min's approximation gap (controlled by $$\left (\frac{x^B_i}{a_x} \right )^2 + \left (\frac{y^B_i}{a_y} \right )^2 + \left (\frac{z^B_i}{a_z} \right )^2 \geq 1$$2 and $$\left (\frac{x^B_i}{a_x} \right )^2 + \left (\frac{y^B_i}{a_y} \right )^2 + \left (\frac{z^B_i}{a_z} \right )^2 \geq 1$$3) introduces any practical loss of strictness relative to enforcing the true minimum is not analyzed quantitatively.

Conclusion

This paper contributes a complete, field-validated pipeline for perception-driven safety filtering: body-frame ellipsoidal safety envelopes inducing time-varying world-frame constraints, aggregated via a smooth soft-min composite CBF into a single lightweight QP solvable in microseconds aboard embedded hardware. The experimental evidence—narrow-passage traversal where distance-based constraints deadlock, resilience to a >100 m odometry jump, and reaction to unmapped dynamic obstacles at 800–1300 simultaneous constraints—supports the central thesis that robot-centric, sensor-level safety filtering provides a robust last line of defense independent of higher-level autonomy layers. The main open questions concern formal robustness guarantees under low-level tracking imperfections, principled selection of the ellipsoid geometry and soft-min parameters, and extension to fully three-dimensional enforcement on anisotropic platforms.