Safe Navigation using Density Functions

TL;DR

This work addresses safe navigation in cluttered and high-dimensional spaces by formulating a dual, density-based approach to controller synthesis. It analytically constructs navigation density functions that encode obstacle geometry and target reachability, and proves almost-everywhere convergence to the goal while avoiding unsafe regions. The method accommodates complex obstacle shapes, scales to fully actuated robotic systems, and remains robust under control saturation and bounded noise, offering a practical alternative to traditional navigation functions and hierarchical planning. Overall, the density-based framework provides a geometrically flexible, analytically tractable means to synthesize safe controllers with occupancy-based guarantees for diverse robotic applications.

Abstract

This paper presents a novel approach for safe control synthesis using the dual formulation of the navigation problem. The main contribution of this paper is in the analytical construction of density functions for almost everywhere navigation with safety constraints. In contrast to the existing approaches, where density functions are used for the analysis of navigation problems, we use density functions for the synthesis of safe controllers. We provide convergence proof using the proposed density functions for navigation with safety. Further, we use these density functions to design feedback controllers capable of navigating in cluttered environments and high-dimensional configuration spaces. The proposed analytical construction of density functions overcomes the problem associated with navigation functions, which are known to exist but challenging to construct, and potential functions, which suffer from local minima. Application of the developed framework is demonstrated on simple integrator dynamics and fully actuated robotic systems.

Figures (8)

• Figure 1: Navigation framework using density where (a) defines the navigation problem, (b) shows the density for navigation, and (c) shows occupancy measure, which physically denotes the duration of system trajectories occupying the set.
• Figure 2: $\Psi(x)$ for (a) ${\mathbf X}_u$ as a circle ($h({\mathbf x}) = ||{\mathbf x}||^2 - r_1^2 \leq 0$) and transition region boundary as an ellipse ($s({\mathbf x}) = ||\mathbf{a}{\mathbf x}||^2-r_2^2=0$ where $r_2 > r_1$ and $\mathbf{a}$ is a scaling vector), (b) ${\mathbf X}_u$ as a rounded square ($h({\mathbf x}) = ||{\mathbf x}||^4 - r_1^4 \leq 0$) and a transition region $(s({\mathbf x}) = a^2x_1^2 + b^2x_2^2c^{x_1} - r_2^2$ where $r_2 > r_1$; $a, b, c$ are parameters) defined using equation \ref{['density_fun']}, (c-d) 3D view of (a) and (b) respectively.
• Figure 3: (a) Trajectories converge to the target set (green) while avoiding the unsafe set (gray) with a.e. convergence, (b) Initial conditions along the zero-measure set (black) converge to a saddle point (purple), (c) Trajectories starting at A ($s({\mathbf x})>0$) and B (in ${\mathbf X}_{s_k}$) converge to the target set, (d) Trajectories starting from A and B follow the same path near the boundary of $s({\mathbf x})$.
• Figure 4: (a) Trajectories converge to the target set (green) while avoiding arbitrary obstacles (gray), (b) Trajectory finding a narrow feasible region around obstacles, (c) Navigation in a spherical grid, (d) Navigation through two tori, unbounded cylinder, and sphere.
• Figure 5: Comparison of density functions and NFs for random initial conditions. The sensing region for the density function is defined by $s({\mathbf x}) = a^2x_1^2 + b^2x_2^2c^{x_1} - r^2$ ($r$, $a, b, c$ are parameters). For (a) $r = 2.5$, trajectories don't converge, while setting (c) $r = 4.5$ leads to all trajectories converging. NFs with their corresponding tuning parameter for convergence (b) $\kappa=1$ and (d) $\kappa=10$ lead to trajectories not converging.

Theorems & Definitions (5)

• Remark 1
• Remark 2
• Theorem 1
• Remark 3
• Lemma 1