ADMM-MCBF-LCA: A Layered Control Architecture for Safe Real-Time Navigation

TL;DR

The paper tackles safe real-time navigation for a robot with input saturation and moving obstacles by proposing a layered control architecture (LCA) that combines an offline feasible path library with an online safety layer. The offline layer generates a library of nominal controllers, reference trajectories, and time-varying gains via ADMM-based decomposition and iLQR, while the online layer uses an on-manifold Modulated CBF (MCBF) safety filter and a nearest-neighbor path selector to ensure safety and task completion at 100 Hz. Key contributions include the offline path library generation, ADMM-based gain computation, MCBF-QP safety filtering, and an online path selection mechanism that avoids local minima yet preserves nominal behavior. Experiments in Gazebo and on Fetch demonstrate that ADMM-MCBF-LCA reaches goals safely and with feasible inputs, outperforming layered, end-to-end, and reactive baselines. The approach offers a practical framework for robust real-time navigation in cluttered and dynamic environments with arbitrarily shaped obstacles.

Abstract

We consider the problem of safe real-time navigation of a robot in a dynamic environment with moving obstacles of arbitrary smooth geometries and input saturation constraints. We assume that the robot detects and models nearby obstacle boundaries with a short-range sensor and that this detection is error-free. This problem presents three main challenges: i) input constraints, ii) safety, and iii) real-time computation. To tackle all three challenges, we present a layered control architecture (LCA) consisting of an offline path library generation layer, and an online path selection and safety layer. To overcome the limitations of reactive methods, our offline path library consists of feasible controllers, feedback gains, and reference trajectories. To handle computational burden and safety, we solve online path selection and generate safe inputs that run at 100 Hz. Through simulations on Gazebo and Fetch hardware in an indoor environment, we evaluate our approach against baselines that are layered, end-to-end, or reactive. Our experiments demonstrate that among all algorithms, only our proposed LCA is able to complete tasks such as reaching a goal, safely. When comparing metrics such as safety, input error, and success rate, we show that our approach generates safe and feasible inputs throughout the robot execution.

Figures (4)

• Figure 1: We show a Fetch robot successfully reaching a goal in a dynamic environment running our proposed algorithm to avoid a human walking around and other static obstacles. Blue and green shades in \ref{['fig:rviz']} are obstacles, while the green trajectories are ADMM-generated feasible paths.
• Figure 2: We present a schematic of our approach showing the offline path library generation on the top in blue and online execution in green. We generate a total of $N$ nominal controllers by using a fixed set of waypoints to sufficiently cover the initial known safe set. When commanding the robot, we use nearest neighbor search over predicted next states to choose a path where ties are broken by choosing the one closer to the center. We construct our nominal feedback input and apply a safety filter using a on-Manifold control barrier function. Finally, this is applied to the robot in real-time.
• Figure 3: In \ref{['fig:multi-path-admm']}, we show the feasible trajectories from our path library in green visualizing the computed offline solutions for the initially unknown environment in \ref{['fig:hospital']}. The pink dots indicate the feasible next states to choose from. The nearby detected obstacle boundaries are visualized as blue point clouds.
• Figure 4: In \ref{['fig:env approx']}, we show boundaries of the obstacle inside hospital Gazebo simulations that are learned using GPDF choi2024towards, and the ellipse approximations used by MPC-CBF solver. In \ref{['fig:hospital-traj']}, we plot the results from Gazebo simulations of our approach and $4$ baselines for the indoor environment with unknown obstacles. We observe that ours shown in blue is the only approach that reaches the goal while ensuring safety.