SAFE NAVIGATION USING DENSITY FUNCTIONS
Clemson University
This work develops a density-function based framework for safe robot navigation. Instead of designing a potential field that directly pulls the robot to a goal, we construct a smooth density function over the environment: density is shaped to vanish near unsafe obstacle regions and to concentrate around desired target sets. Moving in the positive direction of this density, or enforcing its discrete transport inside a safety filter or MPC, gives controllers that can navigate cluttered spaces while respecting safety constraints. The examples here turn the theory into small reproducible simulations: single-integrator robots, unicycles with nonholonomic steering, and autonomous-racing. Overall, this research provides analytical density construction for safe navigation, extensions to time-varying obstacles and targets, and use as a high-level planner for more complex robots such as quadrupeds.
Density Function Construction
The plot shows how the density changes with the obstacle and target parameters. The displayed obstacle is centered at the origin and the goal is at \((1.8,1.4)\).
Density Feedback Controller
The density function can be used to design safe and stabilizing feedback controllers. For a single-integrator system, we use the positive gradient of the density function as the controller. The obstacle terms push density away from unsafe sets, while the target term increases density near the goal. See the holonomic navigation examples for static obstacles, local sensing, and safety-filter comparisons.
$$
u_k=\nabla\rho(x_k)
$$
In the dynamic-obstacle closing-in setup, a unicycle robot moves toward its goal while circular obstacles approach from multiple directions. Collision-cone density feedback uses both relative position and relative velocity, so the robot reacts to obstacles that are on a future collision path rather than only to nearby obstacle centers. The dynamic-obstacle experiments compare reactive, collision-cone, and velocity-obstacle density constructions.
On the Unitree Go1, the same dynamic-obstacle framework identifies nearby pedestrians online and updates a safe unicycle motion plan at 10 Hz. Backstepping converts the plan into desired forward-speed and yaw-rate commands, which a separate baseline locomotion controller tracks on the quadruped. See the social navigation experiments for the complete hardware setup and results.
Density Safety Filter
Consider the nonlinear system \(\dot{x}=F(x,u)\). The safety filter keeps a nominal controller in the loop and only changes it when needed. At each step, it solves a small quadratic program that stays close to \(u_{\mathrm{nom}}\) while enforcing the one-step density condition for the discrete dynamics.
$$
\begin{aligned}
\min_{u_k,s}\quad
&\frac{1}{2}\|u_k-u_{\mathrm{nom}}\|_W^2+\frac{1}{2}w_s s^2\\
\mathrm{s.t.}\quad
&\rho(x_{k+1})-\rho(x_k)
+\Delta t\,\nabla\!\cdot(F_d\rho)(x_k,u_k)+s\ge 0,\\
&u_{\min}\le u_k\le u_{\max},\qquad s\ge 0.
\end{aligned}
$$
For a unicycle, the filter applies the same one-step density condition through the nonholonomic dynamics. In the multi-obstacle setup, each obstacle contributes a density constraint, and the optimizer adjusts the forward-speed and yaw-rate command just enough to keep the next predicted state in a safe high-density direction. See the non-holonomic examples for the complete feedback, filter, MPC, and baseline comparisons.
The sensing radius controls how early an obstacle begins shaping the density. Increasing this radius makes the controller react earlier to unsafe regions, producing a more conservative safety margin. This is useful for achieving a desired degree of safety with a physically intuitive tuning parameter.
In the multi-agent swap-8 setup, eight unicycle agents exchange positions through a shared workspace. The nominal commands point each agent toward its own goal, while the density filter applies velocity-obstacle constraints so agents adjust their forward speed and yaw rate before their relative motion becomes unsafe. The multi-agent navigation page compares the controllers across crossing and position-swap scenarios.
Comparison With CBF
For the unicycle model, a vanilla one-step CBF filter encodes obstacle safety only through the robot's \(x,y\) position. The filter can reduce forward speed as the robot approaches the unsafe set, but the CBF constraint has no direct control authority over the yaw-rate input that would steer the robot around the obstacle and toward the goal. In this example, the vanilla CBF filter stays safe but fails to make progress. See the unicycle CBF discussion for the full setup.
Density MPC
In the Density MPC we enforce the density transport condition along the predicted trajectory. The optimization balances task objectives, input effort, subject system constraints, and the density transport condition at each step.
$$
\begin{aligned}
\min_{x,u,c}\quad
&\sum_{j=0}^{N-1}
\ell(x_j,u_j,c_j)+\ell_f(x_N)\\
\mathrm{s.t.}\quad
&x_0=x_{op},\\
&x_{j+1}=F_d(x_j,u_j)\\
&\rho(x_{j+1})-\rho(x_j)
+\Delta t\,\nabla\!\cdot\!\left(F_d\rho\right)(x_j,u_j)
-\Delta t\,c_j\rho(x_j)\ge 0,\\
&u_j\in\mathcal U, \\
&c_j\ge 0,\qquad
\end{aligned}
$$
For the unicycle model, density MPC plans over forward-speed and yaw-rate inputs so the robot can safely steer around obstacles. The MPC activates the density constraints for obstacles inside the current field of view while continuing to optimize progress toward the goal.
In the car racing example, Density MPC uses a unicycle-style vehicle model to predict the ego car’s motion over a short horizon. Each surrounding car defines its own density function, which suppresses unsafe regions around that vehicle. At every step, the MPC solves for the best steering and throttle/brake inputs that make progress along the track while satisfying the density constraints, allowing the ego car to overtake other cars in a safe and controlled manner. See the autonomous-racing page for the vehicle model, track formulation, and complete results.
In the cluttered zig-zag maze, the robot follows sparse A* waypoints through staggered wall blocks. Density MPC reasons over a short future horizon, which lets it trade immediate waypoint tracking against future clearance and steer through the narrow turns without relying on a purely local correction. The maze navigation page compares density feedback, filtering, and MPC across the full set of layouts.
Recent Publications
Feedback Controller
Analytical density functions are used directly as feedback controllers or as high-level motion planners with applications to quadruped motion planning and navigation in dynamic environments.
- Zheng, Andrew*, Sriram SKS Narayanan*, and Umesh Vaidya. "Safe Navigation using Density Functions." IEEE Robotics and Automation Letters, vol. 8, no. 12, 2023, pp. 8502-8509.
- Narayanan, Sriram SKS, Andrew Zheng, and Umesh Vaidya. "Safe Motion Planning for Quadruped Robots Using Density Functions." 2023 Ninth Indian Control Conference (ICC), 2023, pp. 263-268.
- Narayanan, Sriram SKS, Joseph Moyalan, and Umesh Vaidya. "Safe Navigation in Dynamic Environments using Density Functions." 2024.
Density Filters
Density functions are used inside one-step safety filters. These filters minimally modify a nominal command while enforcing the density condition.
- Moyalan, Joseph, Sriram SKS Narayanan, Andrew Zheng, and Umesh Vaidya. "Synthesizing Controller for Safe Navigation using Control Density Function." 2024 American Control Conference (ACC), 2024, pp. 3397-3402.
- Narayanan, Sriram SKS, Joseph Moyalan, Andrew Zheng, and Umesh Vaidya. "Density Functions for Dynamic Safe Navigation of Robotic Systems." 2025 American Control Conference, 2025, pp. 3694-3699.
Density MPC
The density transport condition is enforced along a predicted trajectory in an MPC setting. This enables safe finite-horizon predictive control with density constraints.
- Narayanan, Sriram SKS, Sajad Ahmadi, Javad Mohammadpour Velni, and Umesh Vaidya. "Safety Critical Model Predictive Control Using Discrete-Time Control Density Functions." ASME Letters in Dynamic Systems and Control, vol. 6, no. 1, 2025, article 011014.