Density Construction · Safe Navigation using Density Functions

Run

Generate a minimal density-feedback rollout using the same density construction.

python examples/single_integrator/static_single_obstacle/density_feedback.py

Interactive Density Function

Tune the obstacle geometry and target exponent, then rotate the 3D surface to see how the density vanishes around unsafe states and concentrates near the target.

Obstacle shape Safety radius 0.55 Sensing radius 1.25 Goal exponent alpha 0.40

System and Sets

Consider the control-affine system $\dot{x}=f(x)+g(x)u$. Let $\mathcal{X}$ be the state space, $\mathcal{X}_u$ the unsafe set, and $\mathcal{X}_T$ the target set. The goal is to construct a density function $\rho:\mathcal{X}\setminus\mathcal{X}_u\to\mathbb{R}_{>0}$ that guides almost all initial conditions to $\mathcal{X}_T$ while avoiding $\mathcal{X}_u$.

Obstacle and Sensing Regions

Represent obstacle $i$ by a smooth function $h_i:\mathcal{X}\to\mathbb{R}$, with the unsafe set described by a sublevel set. A larger set $\mathcal{X}_{s,i}$ defines the region where the density transitions around the obstacle.

$$ \mathcal{X}_{u,i}=\{x\in\mathcal{X}:h_i(x)\le 0\}, \qquad \mathcal{X}_{s,i}=\{x\in\mathcal{X}:s_i(x)\le 0\}\setminus\mathcal{X}_{u,i}. $$

For a circular obstacle, $h_i(x)=\|x-c_i\|^2-r_{u,i}^2$ defines the unsafe set and $s_i(x)=\|x-c_i\|^2-r_{s,i}^2$ defines the larger sensing boundary, with $r_{s,i}>r_{u,i}$.

Encoding the Obstacles

The inverse bump gives a smooth transition from zero to one. The obstacle factor $\phi_i$ is zero on the unsafe set and smoothly transitions to one outside the sensing region.

$$ b(\tau)= \begin{cases} \exp(-1/\tau), & \tau>0,\\ 0, & \tau\le 0, \end{cases} \qquad \bar{b}(\tau)=\frac{b(\tau)}{b(\tau)+b(1-\tau)}. $$ $$ \phi_i(x)=\bar{b}\!\left(\frac{h_i(x)}{s_i(x)}\right), \qquad \Phi(x)=\prod_{i=1}^{M}\phi_i(x). $$

Encodring the Target

Choose a positive target function $V_T(x)$ that vanishes on the target set and is positive away from it. The density combines the obstacle product with the target factor.

$$ V_T(x)>0 \quad \text{for } x\notin\mathcal{X}_T, \qquad V_T(x)=0 \quad \text{on } \mathcal{X}_T. $$

Overall Density function

$$ \rho(x)=\frac{\Phi(x)}{V_T(x)^\alpha}, \qquad \alpha>0. $$

Density-Based Safety Condition

The density is used to synthesize safe feedback or to constrain a filter/MPC. For the continuous-time control-affine system, the density-based safety condition is written as

$$ \nabla\cdot\Big(f(x)\rho(x)+g(x)\rho(x)u(x)\Big)\ge 0. $$

Density Function Construction