Collision Avoidance, Sensor-based Control, Vision-Based Navigation.

1 INTRODUCTION↩︎

Autonomous vehicles, mobile industrial robots, and multi-agent systems need to respect safety requirements when navigating complex unknown environments. Control Barrier Functions (CBFs) [1] provide strong mathematical guarantee of satisfying the safety requirements, with promising applications on legged robots [2], autonomous vehicles [3], multi-robot systems [4], and fast aerial robots [5]. CBF can be combined with Control Lyapunov Functions (CLFs) in Quadratic Programs (QPs) to ensure safety and stability in safety-critical systems [6], or act as a QP-based safety filter that minimally modifies unsafe control inputs to prevent safety violations [1].

State-of-the-art CBFs define safe sets by reasoning about explicitly identified obstacles or build Signed Distance Functions (SDFs) of safe sets. BarrierNet [7] optimizes parameters of a CBF constructed from sensor data with respect to (w.r.t.) user-defined safe sets, while Liu et al. [8] extract elliptical obstacles from elevation grid maps. However, approximating obstacles with minimum bounding circles requires a separate constraint for each obstacle, and can be overly conservative for some environments. Then, SDFs provide a representation of the closest distance to a set’s surface, offering essential information for online obstacle avoidance and motion planning [9]. Li et al. [10] formulate an SDF-based CBF to maneuver around obstacles in a goal reaching scenario using local sensor measurements. Srinivasan et al. [11] learn to classify safe sets in LiDAR data using a Support Vector Machine and construct a single constraint CBF from the resulting SDF at the cost of exhaustive data collection. In [12], the computation complexity of [11] is addressed by approximating the SDF from LiDAR using an incremental training approach with replay memory, albeit employing one constraint per obstacle.

In a priori unknown environments, CBFs must rely on awareness of the surrounding unsafe regions and their changes to reach a quick and smooth generation of a safety certificate. However, practical mobile robots are hindered by the occlusion, range, and Field of View (FoV) of the available sensors such as cameras or solid state LiDARs. Therefore, at any time, a robot can only partially observe the environment, limiting its ability to reason about safety. Occupancy grid maps (OGMs) aggregate such partial observations to construct grid-based occupancy models where each cell carries the probability that its respective part of the environment is an obstacle [13]. OGMs are sensor-agnostic [14]–[16], and can be updated online during the deployment, utilizing a probabilistic approach to account for sensor uncertainty [17].

We propose OGM-CBF, a novel CBF formulation, that exploits the OGM’s persistency to retain memory of a priori observed obstacles as demonstrated in 1. The proposed CBF employs smoothly interpolated SDF to describe arbitrarily shaped obstacles into one CBF constraint in total in a QP. In particular, the CBF is of degree one, yielding a safety filter over the linear and angular velocity.

We highlight the contributions as follows.

• A CBF that employs OGM and smoothly interpolated SDF to efficiently describe obstacles of arbitrary amount and shape using a single CBF constraint in a QP problem.

• Integrating OGM’s capability to retain memory of a priori observed obstacles that can no longer be observed by the robot into the CBF.

• A sensor-agnostic safety guarantee method that has been integrated into a range of exteroceptive modalities such as depth cameras, laser scanners, and 3D LiDARs.

• A demonstration on how to employ image pyramid over the SDF to yield a multi-level OGM-CBF formulation and inject steering authority when facing degenerate SDF gradients of head-on approached obstacles.

• Quantitative and qualitative evaluation of the proposed approach against memory-unaware baselines.

• Deployment of the proposed approach on both a small, differential drive warehouse robot and a large, Articulated Frame Steering (AFS) autonomous wheel-loader.

The rest of this paper is organized as follows. 2 reviews the principles of the CBF, OGM, and SDF. 3 describes the proposed control methodology. 4 provides the quantitative and qualitative evaluations, and reports on the deployments on the warehouse robot and wheel loader. Finally, in 5, we conclude the paper and draw the future work.

2 PRELIMINARIES AND BACKGROUND↩︎

Let the robot’s motion be described by the non-linear control-affine system \[\label{eq:background:control95system} \begin{align} \dot{\boldsymbol{x}} = f(\boldsymbol{x}) + g(\boldsymbol{x})\boldsymbol{u},\\ \boldsymbol{z}= O_1(\boldsymbol{x},\boldsymbol{\boldsymbol{q}}), \\ (\boldsymbol{p},\boldsymbol{\hat{\boldsymbol{p}}}) = O_2(\boldsymbol{x}), \end{align}\tag{1}\] where \(\boldsymbol{x}\in \mathbb{X}\subseteq \mathbb{R}^{n}\) is the system state, \(\boldsymbol{u}=[\boldsymbol{v},\boldsymbol{\omega}]^T \in \mathbb{U}\subseteq \mathbb{R}^{m}\) is the control input, consisting of the linear and angular velocities in the body frame, and \(f : \mathbb{R}^n \to \mathbb{R}^n \quad\text{and}\quad g : \mathbb{R}^n \to \mathbb{R}^{n\times m}\) are locally Lipschitz continuous functions. The robot is equipped with the observation systems \(O_{1}\) and \(O_{2}\). The exteroceptive (e.g., camera, LiDAR, RADAR) \(O_{1} : \mathbb{R}^n \times \mathbb{R}^q \rightarrow \mathbb{R}^k\) is a function of the state and the environment variables \(\boldsymbol{q}\in \mathbb{R}^{q}\) that yields the observation \(\boldsymbol{z}\in \mathbb{R}^{k}\). Finally, \(O_{2} : \mathbb{R}^n \rightarrow \mathbb{R}^i \times \mathbb{R}^i\) is a function of states and provides the position \(\boldsymbol{p}\in \mathbb{R}^{i}\) and the robot heading unit vector \(\boldsymbol{\hat{\boldsymbol{p}}}\in \mathbb{R}^{i}\).

2.1 Control Barrier Function↩︎

Let the system 1 be categorized as safe if \(\boldsymbol{x}(t) \in \mathbb{X}_\text{s}\subseteq \mathbb{X}\), \(\boldsymbol{u}(t)\in \mathbb{U}_\text{s}\subseteq \mathbb{U}\), \(\forall t\geq 0\), where \(\mathbb{X}_\text{s}\) is the set of safe states, and \(\mathbb{U}_\text{s}\) is the set of admissible control inputs. \(\mathcal{S}\subseteq \mathbb{X}_\text{s}\) is the zero-superlevel set of a continuously differentiable function \(h(\boldsymbol{x}) : \mathbb{X}\rightarrow \mathbb{R}\), yielding \[\label{eq:background:safe95set} \mathcal{S}= \{\boldsymbol{x}\in \mathbb{X} \; | \; h(\boldsymbol{x}) \geq 0 \}.\tag{2}\]

Next, we reason about the safety of 1 in the terms of the forward invariant set. The set \(\mathcal{S}\) is forward invariant for the control system 1 if the state trajectories starting in \(\mathcal{S}\) will remain in \(\mathcal{S}\), i.e., if \(\boldsymbol{x}(0) \in \mathcal{S}\), there exists \(\boldsymbol{u}(t) \in \mathbb{U}_\text{s}\), such that \(\boldsymbol{x}(t) \in \mathcal{S}\), \(\forall t\geq 0\) [18]. Ames et al. [1] define that the function \(h(\boldsymbol{x})\) is a Control Barrier Function (CBF) if there exists an extended class \(\kappa\) function \(\alpha(.)\) such that for all \(\boldsymbol{x}\in \mathbb{X}\) \[\label{eq:background:dot95cbf} \sup_{\boldsymbol{u}\in \mathbb{U}_s}[\frac{\partial h(\boldsymbol{x})}{\partial \boldsymbol{x}} (f(\boldsymbol{x})+g(\boldsymbol{x})\boldsymbol{u})] \ge -\alpha(h(\boldsymbol{x})),\tag{3}\] where an extended class \(\kappa\) function is a function \(\alpha: \mathbb{R}\rightarrow \mathbb{R}\) that is strictly increasing and \(\alpha(0)=0\). Besides, for brevity, let \(\Dot{h}(\boldsymbol{x},\boldsymbol{u})=\frac{\partial h(\boldsymbol{x})}{\partial \boldsymbol{x}} (f(\boldsymbol{x})+g(\boldsymbol{x})\boldsymbol{u})\) be used further on.

2.2 Occupancy Grid Map↩︎

Let the environment be represented as the Occupancy Grid Map (OGM) \(\mathcal{M}\) [13] with cell size \(d_{\nu}\) that follows the update rule \[\label{eq:background:ogm} \text{update}(\mathcal{M}, \boldsymbol{p},\boldsymbol{\hat{\boldsymbol{p}}}, \boldsymbol{z}) \rightarrow \mathcal{M}'.\tag{4}\] Let \(\text{prob}(\nu)\) be the probability that the position \(\boldsymbol{p}\) corresponding to the cell \(\nu(\boldsymbol{p})\) is an obstacle. The safe and unsafe sets are defined by binarizing \(\mathcal{M}\) so that \[\label{eq:background:binary95ogm} \text{prob}_\text{bin}(\nu) = \begin{cases} 0 & \text{if}\;\text{prob}(\nu) < \text{prob}_\text{occ},\\ 1 & \text{if}\;\text{prob}(\nu) \geq \text{prob}_\text{occ}, \end{cases}\tag{5}\] where the risk threshold \(\text{prob}_\text{occ}\) is the probability at which a cell is considered occupied. Then, the unsafe set \(\mathbb{X}_\text{u}\) is the union of all states corresponding to the occupied cells \[\label{eq:background:unsafe} \mathbb{X}_\text{u}= \{ \boldsymbol{x}\in \mathbb{X}\mid (\boldsymbol{p},\boldsymbol{\hat{\boldsymbol{p}}}) = O_2(\boldsymbol{x}) : \text{prob}_\text{bin}(\nu(\boldsymbol{p})) = 1 \},\tag{6}\] and the safe set \(\mathbb{X}_\text{s}\) is given by \(\mathbb{X}\setminus \mathbb{X}_\text{u}\).

2.3 Signed Distance Function↩︎

Let the Signed Distance Function (SDF) at point \(\boldsymbol{p}\) yield the signed distance from \(\boldsymbol{p}\) to the closest point of a set \(\Omega\) in a metric space, where the sign indicates whether \(\boldsymbol{p}\) is inside or outside \(\Omega\) [19]. If \(\Omega\) is a subset of a metric space with metric \(d\), the SDF is \[\label{eq:background:sdf} \phi(\boldsymbol{p}, \Omega)= \begin{cases} d(\boldsymbol{p}, \partial \Omega) & \text{if}\;\boldsymbol{p}\in \Omega,\\ -d(\boldsymbol{p}, \partial \Omega) & \text{if}\;\boldsymbol{p}\notin \Omega, \end{cases}\tag{7}\] where \(\partial \Omega\) is the boundary of \(\Omega\) and \(d(\boldsymbol{p}, \partial \Omega)\) is the minimum distance from \(\boldsymbol{p}\) to a point on the boundary. Additionally, \(d(., \partial \Omega)\) is the desired distance metric to the set \(\partial \Omega\). If \(\Omega\) is a subset of the Euclidean space \(\mathbb{R}^n\) with a piecewise smooth boundary and \(d(., \partial \Omega)\) is the Euclidean distance metric, the SDF is differentiable almost everywhere. Moreover, the gradient of the function satisfies the eikonal equation \(|\nabla \phi| = 1\) at every point where it is differentiable; for additional details, the interested reader is referred to [20], [21].

3 CONTROL METHODOLOGY↩︎

The proposed method aims to ensure safety of robots in partially observable environments by formulating a CBF that employs OGM to retain memory of a priori observed obstacles that have left the robot’s field of view. The formulation comprises an SDF-based distance term that captures the effect of the robot’s distance to obstacles of arbitrary shape, and an alignment term that captures the alignment of the robot’s heading vector and the avoidance direction. The CBF is built as follows.

First, the OGM is refined into an SDF of the unoccupied space, see 2. Next, the SDF is processed by the image pyramid to retain steering authority when facing the degenerate gradient prevalent in head-ons with locally flat obstacles. The image pyramid transform yields CBF constraints at multiple scales, providing additional avoidance directions. Besides, since the OGM and thus also the SDF are discrete (i.e., in grid space), an interpolation is used over the SDFs at each scale to ensure the CBF is continuous. Finally, following the common safe-control practice, the CBF condition is enforced via a QP-based safety filter that minimally perturbs a nominal performance controller.

In the rest of this section, we describe the individual building blocks in detail.

3.1 Occupancy Grid Map-based Control Barrier Function↩︎

Let the OGM \(\mathcal{M}\) follow the online update rule 4 . Then, the safety is achieved using the CBF \[\label{eq:method:cbf} h(\boldsymbol{x}) = \phi'(\boldsymbol{p}; \mathbb{X}_\text{s}) + l_\text{s}+ l_\text{a}\boldsymbol{\hat{\boldsymbol{p}}}\cdot \nabla_{\boldsymbol{p}} \phi'(\boldsymbol{p}; \mathbb{X}_\text{s}),\tag{8}\] where

• \(\phi'(\boldsymbol{p}; \mathbb{X}_\text{s}) = T(\phi(\boldsymbol{p}, \mathbb{X}_\text{s}))\), and \(T: \mathbb{R}^j \rightarrow \mathbb{R}\) is an interpolation function, where \(j\) is the number of points from the OGM used for interpolation. Therefore, \(\phi'\) is a smoothed SDF of safe set \(\mathbb{X}_\text{s}\), evaluated at the robot’s body frame position \(\boldsymbol{p}\);

• \(\boldsymbol{\hat{\boldsymbol{p}}}\cdot \nabla_{\boldsymbol{p}} \phi'(\boldsymbol{p}; \mathbb{X}_\text{s})\) is the dot product of the robot heading unit vector \(\boldsymbol{\hat{\boldsymbol{p}}}\) and smooth SDF gradient \(\nabla_{\boldsymbol{p}} \phi'(\boldsymbol{p};\mathbb{X}_\text{s})\);

• \(l_\text{s}\), and \(l_\text{a}\) are scalar values such that \(0 < l_\text{a}\leq -l_\text{s}\).

Note that \(\boldsymbol{p}\), \(\boldsymbol{\hat{\boldsymbol{p}}}\), and thus also \(\phi'\) are defined in the same coordinate frame, e.g., the world frame, and \(\boldsymbol{p}\), \(\boldsymbol{\hat{\boldsymbol{p}}}\) are derived from system state \(\boldsymbol{x}\) as in 1 .

Intuitively, the proposed CBF comprises two distinct components. The larger these components are, the safer the robot is. The first component, \(\phi'(\boldsymbol{p}; \mathbb{X}_\text{s})\), captures the effect of the robot’s distance to obstacles, maintaining safety.

The second component, \(l_\text{s}+ l_\text{a}\boldsymbol{\hat{\boldsymbol{p}}}\cdot \nabla_{\boldsymbol{p}} \phi'(\boldsymbol{p}; \mathbb{X}_\text{s})\), captures the alignment of the robot’s heading vector \(\boldsymbol{\hat{\boldsymbol{p}}}\) to the SDF gradient \(\nabla_{\boldsymbol{p}} \phi'(\boldsymbol{p};\mathbb{X}_\text{s})\). In particular, the alignment component involves calculating the cosine similarity of the two vectors. Since \(\nabla_{\boldsymbol{p}} \phi'\) is the direction that \(\phi'\) increases the most, moving in that direction moves the robot away from obstacles as visualized by the gradient field in 2 (c). Without the second component, derivative of \(h(\boldsymbol{x})\) would be a function of linear velocity only, i.e., to maintain safety, the system could only stop. Therefore, similar to Toulkani et al. [22], the second component aims to enable steering without the additional complexity of a higher order CBF such as the one in [23].

The performance of the OGM-CBF is influenced by the design of the function \(\alpha(.)\), and the values of \(l_\text{s}\) and \(l_\text{a}\), all of which are hyperparameters to be adjusted as per control system 1 .

3.2 Necessary Properties of the Control Barrier Function↩︎

Next, we establish that the constructed function has the necessary properties to be a CBF.

3.3 SDF Pyramid Transform↩︎

Recall that in (11 ), \(\boldsymbol{\omega}\) can enter \(\dot{h}\) only through the term \(b(\boldsymbol{x})\). It follows that whenever \(\nabla_{\boldsymbol{p}} \phi'(\boldsymbol{p};\mathbb{X}_s)\) is collinear with \(\boldsymbol{\hat{\boldsymbol{p}}}\), then \(b(\boldsymbol{x})=0\), and therefore \(\dot{h}\) becomes (locally) insensitive to \(\boldsymbol{\omega}\). The collinearity of \(\nabla_{\boldsymbol{p}} \phi'(\boldsymbol{p};\mathbb{X}_s)\) with \(\boldsymbol{\hat{\boldsymbol{p}}}\) is prevalent when approaching locally orthogonal obstacle boundaries head on. In this gradient degeneracy regime, the CBF cannot exploit \(\boldsymbol{\omega}\) to steer away and must resort to reducing \(\boldsymbol{v}\), loosing the steering authority.

We mitigate this by constructing a multi-scale Gaussian pyramid of the SDF and enforce CBF constraints at each scale. Starting from the base SDF grid \(\phi_1\), we build levels \(k=2,\dots,K\) via smoothing and down sampling by the factor of two \[\phi_{k} \;=\; \mathcal{D}\!\left(\mathcal{G}_\sigma * \phi_{k-1}\right),\] where \(\mathcal{G}_\sigma\) is the Gaussian blur kernel, convolved at each level, followed by the pyramid down sampling \(\mathcal{D}\).

Finally, we define a level-wise barrier \(h_k(\boldsymbol{x})\) by replacing \(\phi\) and its gradient with \(\phi_k\) and \(\nabla\phi_k\), respectively, for \(k=1,\dots,K\), and impose one CBF, \(h_k(\boldsymbol{x})\), per level. The coarser but smoothed levels reduce local gradient degeneracy and provide additional, non-parallel avoidance directions, improving steering around locally orthogonal obstacle geometries.

3.4 Smooth Signed Distance Function Approximation↩︎

Since the occupancy grid map \(\mathcal{M}\) is calculated at discrete positions (centers of the cells), \(\phi(.; \mathbb{X}_\text{s})\) is initially also calculated at discrete grid points, and thus is not continuous. However, it is then smoothly interpolated to approximate the SDF at an arbitrary robot position \(\boldsymbol{p}\), using \(j\) grid points neighboring \(\boldsymbol{p}\). Let the function \(T\) represent this interpolation (smoothing) of the SDF; then, applying \(T\) ensures the continuity of \(h(\boldsymbol{x})\).

3.5 OGM-CBF-QP Control Synthesis↩︎

The safe control using the proposed OGM-CBF is implemented as a QP-based safety filter. At each control step, given a nominal/reference control input \(\boldsymbol{u}_{\mathrm{ref}}(\boldsymbol{x})\), a safe control input is computed by solving \[\label{eq:method:optimization95cbfqp} \begin{align} \boldsymbol{u}^{*}(\boldsymbol{x}) &= \underset{\boldsymbol{u}}{\mathrm{argmin}}\; J(\boldsymbol{u},\boldsymbol{x}) \\ \mathrm{s.t.}\quad \dot{h}_k(\boldsymbol{x},\boldsymbol{u}) &\ge -\alpha\!\left(h_k(\boldsymbol{x})\right),\quad \forall k\in\{1,\dots,K\}, \\ \boldsymbol{u}_{lb} \le \boldsymbol{u}&\le \boldsymbol{u}_{ub}. \end{align}\tag{12}\] Since the system is control-affine, each CBF constraint is affine in \(\boldsymbol{u}\), more specifically \[\label{eq:method:affine95hdot} \dot{h}_k(\boldsymbol{x},\boldsymbol{u}) = c_k(\boldsymbol{x}) + \boldsymbol{d}_k(\boldsymbol{x})^\top \boldsymbol{u}.\tag{13}\] We elect to use the standard quadratic objective that penalizes deviation from the nominal control input \(\boldsymbol{u}_{\mathrm{ref}}(\boldsymbol{x})\) \[\label{eq:method:cost} J(\boldsymbol{u},\boldsymbol{x}) = (\boldsymbol{u}-\boldsymbol{u}_{\mathrm{ref}}(\boldsymbol{x}))^\top \mathbf{P}\, (\boldsymbol{u}-\boldsymbol{u}_{\mathrm{ref}}(\boldsymbol{x})),\tag{14}\] with \(\mathbf{P}\succ 0\). Since \(J\) is quadratic and constraints are linear in \(\boldsymbol{u}\), optimization (12 ) is a Quadratic Program (QP), solved online to obtain \(\boldsymbol{u}^*\). Overall, the online operation of the proposed OGM-CBF-QP follows Algorithm 3.

4 Experimental Evaluation↩︎

To experimentally verify the proposed OGM-CBF, a quantitative and qualitative evaluation first compares the proposed approach against memory-unaware baselines. In rest of the presented results, we investigate the effect of the SDF pyramid and the resulting multi-level formulation, and report on deployments on a small warehouse robot and an Articulated Frame Steering (AFS) autonomous wheel loader.

A Note on Common Kinematics and Control Objectives

In [sc.effect_of_memory] [sc.effect_multi_level] [sc.mir], the robots’ employ a common system with the state \({\boldsymbol{x}= [x,y, \psi]^\top}\) (the coordinates of the vehicle’s center in the \(2\)D plane, and the heading), with \(\boldsymbol{u}=[v,w]^T\) (linear and angular speeds) as the control inputs. Safe navigation is deployed by Algorithm 3 and quadratic optimization framework (12 ), with the nominal control objective to maintain a target heading. In particular, the cost function is \(J(\boldsymbol{u},\boldsymbol{x}) =\frac{1}{2}\bigl(v-v_{\mathrm{ref}}\bigr)^2 +\frac{1}{2}\bigl(\omega-\omega_{\mathrm{ref}}\bigr)^2\) with reference velocities \(v_\mathrm{ref}\) and \(\omega_\mathrm{ref}= -k_{\psi}(\psi-\psi_\mathrm{ref})\). The kinematics of the AFS wheel loader in 4.4 differ, an thus are described therein.

4.1 Obstacle Memory in Limited FoV Operation↩︎

Two memory-unware baselines are considered for a quantitative and qualitative comparison in the CARLA autonomous driving simulator [24]. First, SDF-based variant of V-CBF [25] is selected as a representative of the memory unaware methods due its native support for depth cameras, which are representative of the limited FoV operation. Second, a memory-unaware modification of the OGM-CBF (MU-OGM-CBF) employs the current sensory input only to construct non-persistent grid maps, and is used to perform an ablation of the memory/map persistency.

Both the proposed approach and the baselines are provided with the same forward-facing 60 ° FoV depth camera and the vehicle odometry. The depth imagery is used natively in the V-CBF, while the OGM-CBF variants build an EKF-based elevation grid map [26] and refine it into an occupancy grid map with \(d_{\nu}= \SI{0.1}{\meter}\) cells. Steering and throttle commands are calculated using the standard kinematic bicycle model in a \(2\)D workspace [27], and the vehicle tracks the nominal target heading \(\psi_{\mathrm{ref}}\) while guaranteeing safety in a road scenario with static obstacles. Using the nominal common heading-tracking controller, we set \(v_{\mathrm{ref}}=\SI{3}{\meter\per\second}\) and the heading gain \(k_\psi=0.5\). During all runs, the (MU)-OGM-CBF were configured with \(\alpha{=}0.6\), \(v\!\in[0,3]\,\si{\meter\per\second}\), \(\omega\!\in[-4\pi,4\pi]\,\si{\radian\per\second}\), \(l_a{=}0.25\), \(l_s{=}-0.25\), obstacle inflation radius \(\SI{2.1}{\meter}\), and \(K=3\) levels of pyramid with \(\sigma=1\) Gaussian kernel; and V-CBF with \(\alpha{=}0.7\), and the same bounds on \(v,\omega\).

The quantitative evaluation is carried out in the two scenarios illustrated in 4.

1 overviews the behaviors in the two scenarios, each with \(19\) uniformly distributed starting positions. In the Open scenario, the vehicle faces spaced obstacles. While the proposed approach navigates the scenario safely with 100 % success rate, the reactive, memory-unaware baselines suffer from their limited FoV. In particular, the V-CBF succeeds in 53 % runs, while the MU-OGM-CBF has 84 % success rate. In the authors’ opinion, the MU-OGM-CBF benefits from the obstacle inflation in one-shot grid maps, whereas the V-CBF relies on image frame perception only. Moreover, the proposed approach exhibits higher average minimum distance to obstacle in the successful runs, indicating wider avoidance and thus better safety, albeit at the cost of loosing the inherent efficiency of tight avoidance.

The obstacles in the Trap scenario are tighter, and placed so that a vehicle relying only on its FoV may be prompted to steer away from one obstacle into another. The proposed approach leverages the grid map to avoid all obstacles, while both the MU-OGM-CBF and V-CBF succeed only in 37 % of the starting configurations. Again, the proposed approach exhibits larger average minimum distance to obstacle, suggesting wider avoidance. Moreover, 5 further visualizes how the memory-unaware methods enter states where avoidance of a one obstacle steers them into another despite it being in the FoV in a prior time step.

4.2 Effect of Multi-level CBF Formulation↩︎

A \(2\)D differential drive kinematic simulator is employed to investigate the effects of the SDF pyramid tranform when navigating obstacle surfaces locally orthogonal to robot heading. 6 highlights the SDF gradient of the individual levels, and the resulting safe avoidance trajectory in a U-shaped obstacle scenario, given the desired target heading \(\psi_{\mathrm{ref}}\). The baseline constructs OGM-CBF only w.r.t. the level \(1\) SDF (no pyramid transform). As seen, for a heading-orthogonal obstacle, the SDF gradient \(\nabla_{\boldsymbol{p}} \phi'_k (.), k=1\) is directly opposite to the heading, demonstrating the gradient degeneracy described in 3.3. Therefore, while the system stays in the safe set, stopping in front of the obstacle, the baseline is immobilized since it lacks steering authority. On the other hand, the proposed method employs up to level \(3\) pyramid (\(2\) pyramid transforms applied to the SDF of the OGM). In particular, the gradients of SDFs on levels \(2\) and \(3\) ((\(\nabla_{\boldsymbol{p}} \phi'_k (.), k=2,3\))) are not directly opposite to the heading, injecting the authority to navigate around the obstacle, see 6. However, it should be noted that there is only a particular subspace on the shape of the Gaussian kernel \(\mathcal{G}_\sigma\) and number of pyramid levels where one can prevent loss of information from high blur and keep efficacy. Besides, consider that Gaussian blurring should not be applied directly to the \(h_0\) as it smooths \(h_0\) around \(h_0=0\), shifting the zero-superlevel set \(\mathcal{S}\) of the primary CBF.

4.3 Deployment on a Warehouse Robot↩︎

The proposed approach has been deployed on MiR100, a warehouse differential drive platform with a front-left SICK S300 \(\SI{270}{°}\)-FOV \(2D\) laser scanner and integrated fusion odometry. The two scenarios in 7 are each repeated \(5\) times in \(2\) sensing configurations for \(20\) runs in total. The scenarios model clutter of small obstacles and a large concave obstacle, respectively, both using short and long range sensing, with the \(2D\) scanner limited to \(\SI{5}{\meter}\) and \(\SI{25}{\meter}\) range, respectively.

The proposed safe control approach is executed onboard an NVIDIA Jetson AGX Orin. During the experiments, the system is initialized with no prior map of the environment, and builds the occupancy grid map with \(d_{\nu}= \SI{0.05}{\meter}\) cells online during the navigation; the robot embodiment is represented by \(\SI{0.35}{\meter}\) obstacle inflation. The utilized safe control synthesis described in (12 ) is parametrized with \(\alpha=0.3\), \(l_a=-l_s=0.25\), control bounds \(v\!\in[-0.5,0.5]\,\si{\meter\per\second}\), \(\omega\!\in[-\frac{\pi}{4},\frac{\pi}{4}]\,\si{\radian\per\second}\), and \(K=3\) levels of pyramid with \(\sigma=1\) Gaussian kernel. The nominal heading-tracker controller uses \(v_{\mathrm{ref}}=\SI{0.5}{\meter\per\second}\) and the heading gain \(k_{\psi}=0.5\).

During the deployment, the system has successfully navigated both scenarios, following similar paths over the repeated runs. Besides, the development of the CBF function \(h(\boldsymbol{x})\) over the individual runs indicates that the proposed system successfully and safely repeatedly avoids obstacles in both scenarios, see 7

4.4 Deployment on an Articulated Wheel Loader↩︎

The proposed approach has been deployed on a \(5\,\mathrm{ton}\) AFS wheel loader. As described in [[28]][29], the AFS front body kinematic model is \[\begin{align} \dot{x}_f &= v_f\cos\theta_f, & \quad \dot{y}_f &= v_f\sin\theta_f, \\ \dot{\theta}_f &= \frac{v_f\sin\beta + l_r\dot{\beta}}{l_f\cos\beta + l_r}, & \quad \dot{\beta} &= u_{\dot{\beta}}, \end{align}\] with state \(\boldsymbol{x}=[x_f,\,y_f,\,\theta_f,\,\beta]^\top\) and control \(\boldsymbol{u}=[v_f,\,u_{\dot{\beta}}]^\top\), where \((x_f,y_f)\) is the front-body reference-point position, \(\theta_f\) the front-body heading, \(\beta\) the articulation (hinge) angle between front and rear bodies, \(l_f,l_r\) are the joint-to-front and -rear axle distances, respectively, \(v_f\) is the front-body forward speed, and \(u_{\dot{\beta}}\) is the commanded articulation rate (i.e., \(\dot{\beta}=u_{\dot{\beta}}\)).

The machine is equipped with a rear-facing Velarray M1600 LiDAR. Hence, the machine first maps the environment using the rear-facing LiDAR, creating a memory of the obstacles in an elevation grid map that is refined into \(d_{\nu}=\SI{0.1}{\meter}\) occupancy. Then, the map is used to navigate in forward direction. In particular, the machine is tasked to nominally track a target heading \(\psi_{\mathrm{ref}}\) while operating safely in the static obstacle scenario presented in 8. At each control step, the OGM-CBF-QP defined in (12 ) is solved [29]. Nominal motion is defined by \(u_{\mathrm{ref}}=[v_{\mathrm{ref}},\,u_{\dot{\beta},\mathrm{ref}}]^\top\), where \(v_{\mathrm{ref}}=\SI{1.0}{\meter\per\second}\) and \(\omega_{\mathrm{ref}} = -k_\psi\,(\theta_f-\psi_{\mathrm{ref}}), u_{\dot{\beta},\mathrm{ref}} = -\frac{v_{\mathrm{ref}}}{l_r}\sin\beta +(\frac{l_f}{l_r}\cos\beta+1)\omega_{\mathrm{ref}}.\)

In the described avoidance scenario, the hyperparameters were set to \(\alpha=0.15, l_a=1.5, l_s=-2, k_{\psi}=0.7\), and \(K=2\) levels of pyramid with \(\sigma=8.6\) Gaussian kernel. Due to low-speed tracking limits of the low-level linear velocity feed-forward controller on the machine, we restrict the commanded forward velocity to \(v_f\in[0.2,1]\,\si{\meter\per\second}\) during real-world experiments, and upper and lower bounds on \(u_{\dot{\beta}}\) are \(u_{\dot{\beta}}\in[-0.8,0.8]\,\si{\radian\per\second}\). Besides, since the linear velocity is strictly positive and the vehicle cannot turn in place, it cannot back up into an obstacle. Therefore, we assume that only the states where \(l_a \boldsymbol{\hat{\boldsymbol{p}}}\cdot \nabla_{\boldsymbol{p}} \phi'(.) \leq 0\) are relevant. Then, abusing the formulation, \(-l_s\) is considered to be equivalent to the inflation radius and is set to \(\SI{2}{\meter}\) to cover the front body embodiment.

During the deployment, the wheel loader have safely operated in the scenario (\(h\ge 0\)) as overviewed in 8. Hence, we conclude that despite coupling the proposed CBF to kinematic models only, the proposed approach is appropriate for safe control onboard both a warehouse differential drive robot and an articulated wheel-loader.

5 Conclusion↩︎

We present OGM-CBF, an SDF-based CBF that is coupled to an OGM to provide safety guarantee for mobile robots in environments with arbitrary shaped obstacles. When the environment can be only partially observed, the OGM-CBF leverages the OGM memory to avoid a priori observed obstacles that have left the robot’s field of view. The proposed approach outperforms memory-unaware baselines that may steer into out of field of view obstacles when prompted by other hazards or the nominal control inputs. Besides, the proposed approach exploits the grid representation to consider the SDF gradient w.r.t. multiple scales of local neighborhoods, promoting steering authority in cases when the SDF gradient degenerates due to obstacle surfaces orthogonal to the heading. The OGM-CBF is sensor-agnostic, and we have utilized depth imagery, \(2\)D laser scanner, and \(3\)D LiDAR to deploy the safe control to an autonomous driving simulator, a real warehouse robot, and an articulated frame steering autonomous wheel loader, respectively. The authors suggest that the future extensions can focus on higher order CBF to address complex vehicle dynamics or mitigate sensor sparsity at range by employing inpainting over the grid representation.

References↩︎