A Smooth Penalty-Based Feedback Law for Reactive Obstacle Avoidance with Convergence Guarantees

TL;DR

This work tackles safe autonomous navigation in unknown, obstacle-rich environments using only local sensing. It introduces Safe Penalty-based Feedback (SPF), a smooth, closed-form control law that blends a nominal input with a state-dependent projection toward obstacle boundaries via a penalty function, ensuring forward invariance of a safety margin and avoiding maps or switching. When the nominal controller is a gradient-descent on a potential $V$, the closed-loop system achieves almost global asymptotic stability (AGAS) under a simple curvature condition that compares obstacle-boundary curvature to the potential's level-set curvature, with undesired equilibria shown to be unstable under this mild assumption. The method is demonstrated through 2D and 3D simulations, using local distance and bearing to obstacles to achieve safe, convergent navigation with low computational overhead, offering a lightweight alternative to more complex or optimization-based safety schemes.

Abstract

This paper addresses the problem of safe autonomous navigation in unknown obstacle-filled environments using only local sensory information. We propose a smooth feedback controller derived from an unconstrained penalty-based formulation that guarantees safety by construction. The controller modifies an arbitrary nominal input through a closed-form expression. The resulting closed-form feedback has a projection structure that interpolates between the nominal control and its orthogonal projection onto the obstacle boundary, ensuring forward invariance of a user-defined safety margin. The control law depends only on the distance and bearing to obstacles and requires no map, switching, or set construction. When the nominal input is a gradient descent of a navigation potential, we prove that the closed-loop system achieves almost global asymptotic stability (AGAS) to the goal. Undesired equilibria are shown to be unstable under a mild geometric curvature condition, which compares the normal curvature of the obstacle boundary with that of the potential level sets. We refer to the proposed method as SPF (Safe Penalty-based Feedback), which ensures safe and smooth navigation with minimal computational overhead, as demonstrated through simulations in complex 2D and 3D environments.

Figures (10)

• Figure 1: Illustration of Assumptions \ref{['assumption:smoothBoundaries']}--\ref{['assumption:uniqeProjection']}. The (gray) shapes represent the obstacles, while the (blue) disk denotes the robot, modeled as a ball of radius $R$ centered at $x$. The (red) layer around the obstacles conceptually indicates the region where the distance function $d_{\complement\mathcal{X}}$ is of class $\mathcal{C}^k$, as required in Assumption \ref{['assumption:smoothBoundaries']}. The (light blue) region shows where the projection $\mathbf{P}_{\partial\mathcal{X}}(x)$ is guaranteed to be unique, as stated in Assumption \ref{['assumption:uniqeProjection']}. The (dashed) annular layer illustrates that the robot's radius $R$ is small enough to ensure that its center remains in the regions satisfying the assumptions.
• Figure 2: (\ref{['fig:2D psi vizualization']}) Visualization of the penalty scaling function $\psi(d(x), s(x))$ with respect to $d(x)$ and $s(x)$. The left subfigure shows level sets of the distance function $d(x)$ (in blue) and the nominal control alignment $s(x)$ (in red), depicting their spatial variation around an obstacle. The right subfigure displays the corresponding values of $\psi(d(x), s(x))$, revealing its anisotropic behavior. As designed, $\psi(d(x), s(x)) = 0$ whenever $d(x) \ge \mu$ or $s(x) \ge \nu$, and grows unbounded as $d(x) \to 0$ with $s(x) \le 0$. In all other regions, $\psi$ takes smooth values in $(0, +\infty)$. This structure ensures that $\psi$ acts like a directional shield, activating only when the robot faces the obstacle-rather than surrounding it completely-effectively shaping the avoidance response as seen in subfigure (\ref{['fig:3D psi vizualization']}.)
• Figure 3: An example of the functions $\phi_\mu(d(x))$, $\phi_\nu(s(x))$ and $\psi(d(x),s(x))$. (\ref{['fig:d']}) represents $\phi_\mu(d(x))$, where $\mu=0.6$. (\ref{['fig:s']}) represents $\phi_\nu(s(x))$, where $\nu=1$. (\ref{['fig:psiofds']}) represents the $\psi(d(x),s(x))$, and shows that it blows up when $d(x)\to 0$ and $s(x)\to 0$ and vanishes when $d(x)\ge\mu$ or $s(x)\ge\nu$.
• Figure 4: Block diagram illustrating the feedback avoidance controller proposed Theorem \ref{['theorem:convergence']}. The proposed simple control strategy in \ref{['eq:smoothControl']} smoothly projects the nominal gradient descent controller \ref{['eq:nominalControl']} onto the tangent space to the obstacle's boundary as the robot moves closer to the obstacle.
• Figure 5: An illustration of the undesired equilibria in case of 2D obstacles. The (gray) regions $\mathcal{O}_1$ and $\mathcal{O}_2$ represent distinct obstacles, each associated with an undesired equilibrium point $\bar{x}_1$ and $\bar{x}_2$, respectively. The sets $L_{\bar{x}_1}$ and $L_{\bar{x}_2}$ are the level sets passing through these equilibria. At each $\bar{x}_i$, the gradient $\nabla V_1$ normal to the level set, collinear and point in the same direction with $\eta_1$, the normal to the boundary of the obstacle.

Theorems & Definitions (15)

• Definition 1: $\mathcal{C}^l$-Penalty Scaling Function
• Proposition 1
• proof
• Remark 1
• Theorem 1
• proof
• Theorem 2
• proof
• Definition 2: Normal Curvature
• Proposition 2