1 Introduction↩︎

Safety is a fundamental requirement in robotics, often formulated as a set invariance problem: keeping the system state within a safe set that does not intersect the failure set [1]. Control Barrier Functions (CBFs) provide a principled framework for synthesizing controllers that render safe sets forward invariant, typically via a quadratic program (CBF-QP) [1]–[3]. Standard CBFs assume perfect state information [2], [3], but real-world robotic systems operate under state uncertainty arising, e.g., from noisy and incomplete measurements. An effective way to reason about such uncertainty is through Bayesian inference, which provides a belief: a probability distribution over possible states [4]. Building on this idea, recent work has introduced belief CBFs (BCBFs), which ensure safety using the beliefs produced by Kalman filters (KFs) and particle filters (PFs) [5]–[7].

These BCBFs assume the environment has a known and fixed number of objects. In practice this is restrictive: limited sensor range and occlusions cause objects to enter and leave the scene. We therefore consider settings having an unknown and time-varying number of indistinguishable moving objects. This multi-object setting introduces two new challenges: the number of objects varies, and measurement-to-object associations are ambiguous. Both challenges are better handled by a belief over a random finite set (RFS), whose cardinality and elements are random. A commonly used RFS-based estimator is the probability hypothesis density (PHD) filter. It approximates the multi-object belief as a Poisson point process (PPP) and propagates its first-order moment [8]. In particular, the sequential Monte Carlo implementation of the PHD (SMC-PHD) filter [9], [10] has proven effective in nonlinear, non-Gaussian, and complex robotic environments [11]–[13].

Despite the advantages of the SMC-PHD filter as a state estimator, incorporating its beliefs into a CBF framework remains challenging. First, state space safety specifications do not directly translate to belief space, where safety should be reasoned about probabilistically over distributions of states. This requires us to design a CBF in belief space with risk-aware guarantees. Second, the belief space dynamics are hybrid in nature, combining continuous prediction with discrete update, which makes the analysis of forward invariance difficult. Third, SMC-PHD filters typically represent the belief by a large number of particles [9]–[11], yielding a high-dimensional representation that complicates the synthesis of computationally efficient controllers.

To address these challenges, we leverage the PPP structure of the PHD belief to construct a risk-aware BCBF directly on the particles. We then derive the corresponding nonsmooth CBF conditions [3], establish forward invariance under continuous prediction, and give an explicit condition for certifying safety across discrete filter updates. To handle the high-dimensional particle representation, we reformulate the CBF conditions into a more tractable form and exploit parallelism in constructing the BCBF-QP, achieving an average control computation time of less than \(5\) ms. We validate our approach in simulation on two safe robot control applications: field-of-view (FOV) maintenance and obstacle avoidance in unstructured dynamic environments. We further demonstrate the latter on underwater hardware experiments, as shown in Fig. 1.

2 Related Work↩︎

2.1 CBFs under State Uncertainty↩︎

Existing work characterizes measurement noise from samples and develops CBFs that are robust to it, using distributionally robust optimization (DRO) [14] or learning-based approaches [15]. These methods handle noisy sensing but assume access to the full state, which is typically only partially observed in practice. For instance, the CBF condition may depend on velocity while the robot only measures position. A natural remedy is to estimate the full state and account for the resulting state uncertainty in CBFs. Following this idea, prior works extend CBFs to handle bounded state estimation errors [16], [17]. Observer-based CBFs derive these bounds from the error dynamics [16], whereas measurement-robust CBFs obtain them from data [17]. However, both methods enforce safety against the worst-case error within the bound, which can be conservative compared to incorporating unbounded stochastic uncertainty [18]. Recent work on BCBFs [5]–[7] accounts for this stochastic uncertainty but remains limited to a known, fixed number of objects in the environment.

2.2 PHD Filters in Robotics↩︎

The PHD filter has been applied to robot localization [19], mapping [11], simultaneous localization and mapping [20], and multi-object search and tracking [12], [13]. For multi-object state estimation, unlike KFs and PFs, the RFS-based PHD filter does not require explicit measurement-to-object association. This is particularly advantageous when the number of objects is large [11], [13]. The trade-off is that individual object identities are not maintained. This is not restrictive for the safe control tasks we target, where the safety specification treats all objects identically.

3 Preliminaries↩︎

We consider a robot modeled by the control-affine dynamics of the form \[\label{eq:sys95robot} \dot{\boldsymbol{x}} = \boldsymbol{f}(\boldsymbol{x}) + \boldsymbol{g}(\boldsymbol{x})\boldsymbol{u},\tag{1}\] where \(\boldsymbol{x}\in \mathcal{X} \subseteq \mathbb{R}^{n_x}\) is the robot state, \(\boldsymbol{u}\in \mathcal{U} \subseteq \mathbb{R}^{n_u}\) is the control input, and the drift and input vector fields are given by \(\boldsymbol{f}:\mathbb{R}^{n_x}\to\mathbb{R}^{n_x}\) and \(\boldsymbol{g}:\mathbb{R}^{n_x}\to\mathbb{R}^{n_x \times n_u}\).

The environment contains an unknown number of objects, sharing the same dynamical and measurement models: \[\begin{align} \tag{2} \dot{\boldsymbol{o}} &= \boldsymbol{\xi}(\boldsymbol{o}), \\ \tag{3} \boldsymbol{z}&= \boldsymbol{\ell}(\boldsymbol{x}, \boldsymbol{o}, \boldsymbol{\nu}). \end{align}\] We let \(\boldsymbol{o}\in \mathcal{O} \subseteq \mathbb{R}^{n_o}\) denote the object state, with dynamics \(\boldsymbol{\xi}:\mathbb{R}^{n_o}\to\mathbb{R}^{n_o}\). The observation is \(\boldsymbol{z}\in \mathcal{Z} \subseteq \mathbb{R}^{n_z}\), generated by the measurement function \(\boldsymbol{\ell}:\mathcal{X} \times \mathcal{O} \times \mathbb{R}^{n_\nu} \to \mathbb{R}^{n_z}\) under measurement noise \(\boldsymbol{\nu} \in \mathbb{R}^{n_\nu}\).

3.1 Nonsmooth Control Barrier Functions↩︎

Consider the dynamics defined in (1 ) and (2 ). Let the set \({\mathcal{C} \subseteq \mathbb{R}^{n_x} \times \mathbb{R}^{n_o}}\) be the closed zero super-level set of a locally Lipschitz function \(h:\mathbb{R}^{n_x} \times \mathbb{R}^{n_o} \to \mathbb{R}\), defined as \[\begin{align} \mathcal{C} &:= \{ (\boldsymbol{x},\boldsymbol{o}) \in \mathcal{X} \times \mathcal{O} \mid h(\boldsymbol{x},\boldsymbol{o}) \geq 0 \}, \\ \partial \mathcal{C} &:= \{ (\boldsymbol{x},\boldsymbol{o}) \in \mathcal{X} \times \mathcal{O} \mid h(\boldsymbol{x},\boldsymbol{o}) = 0 \}. \end{align} \label{eq:safe95set}\tag{4}\]

If the systems 1 and 2 are safe with respect to \(\mathcal{C}\), we refer to \(\mathcal{C}\) as the safe set and the function \(h\) as a safety function. To synthesize a controller that keeps the system safe, we employ nonsmooth CBFs.

3.2 Random Finite Sets and PHD Filters↩︎

When the robot operates in an unknown and dynamic environment, neither the number of objects in the environment nor their states are known a priori. We therefore employ RFSs to represent multi-object states. Formally, we let \(\mathbb{O}\) denote the object RFS, and let \(\boldsymbol{o}^{(i)} \in \mathcal{O}\) denote the state of its \(i\)-th element, then we have \(\mathbb{O}=\{\boldsymbol{o}^{(1)},\boldsymbol{o}^{(2)},\ldots,\boldsymbol{o}^{(n)}\}\). The cardinality \(n\in\mathbb{N}\) is itself a random variable. The first moment of a distribution over an RFS is called the PHD, denoted by \(D(\boldsymbol{o})\). As a density over the single-object state space, the PHD satisfies, for any subset \(\mathcal{F} \subseteq \mathcal{O}\), \[\label{eq:phd95inte} \int_{\boldsymbol{o}\in \mathcal{F}} D(\boldsymbol{o})\, d\boldsymbol{o}= \mathbb{E}\bigl[|\mathbb{O} \cap \mathcal{F}|\bigr],\tag{5}\] where \(|\cdot|\) denotes set cardinality, i.e., the integral of the PHD over \(\mathcal{F}\) equals the expected number of objects in \(\mathcal{F}\).

The robot receives a set of sensor measurements \(\mathbb{Z}=\{\boldsymbol{z}^{(1)},\boldsymbol{z}^{(2)},\ldots,\boldsymbol{z}^{(m)}\}\), where the number of measurements \(m\in\mathbb{N}\) varies over time. Using these measurements, the PHD filter [8] recursively estimates the multi-object posterior. Analogous to how a Gaussian distribution underlies the KF, the PHD filter approximates the multi-object posterior by a PPP and assumes the objects are independent. Since a PPP is fully characterized by its first order moment, the multi-object belief reduces to the PHD. In the SMC implementation [9], [10], the PHD is approximated by weighted particles: \[\label{eq:phd95smc} D(\boldsymbol{o}) \approx \sum_{i=1}^{L} w^{(i)} \delta(\boldsymbol{o}- \tilde{\boldsymbol{o}}^{(i)}),\tag{6}\] where \(L\) is the number of particles, \(w^{(i)}\) and \(\tilde{\boldsymbol{o}}^{(i)}\) are the estimated weight and state of the \(i\)-th particle, and \(\delta(\cdot)\) denotes the Dirac delta function. As measurements arrive only at discrete time steps \(t_1,\ldots,t_k,\ldots\), the PHD filter consists of continuous prediction over the interval \(t\in [t_{k-1},t_k)\) and a discrete update at \(t=t_k\). The discrete update incorporates new measurements, followed by particle birth, weight update, and resampling. After resampling, we obtain a particle set \(\{(\tilde{\boldsymbol{o}}^{(i)}, w)\}_{i=1}^L\) with a uniform weight \(w := \sum_{i=1}^{L} w^{(i)} / L\). We refer the reader to [9], [10] for further details.

3.3 Poisson Point Process↩︎

We now introduce the PPP and its key properties [21].

Setting \(m=0\) in ?? gives the void probability of the PPP: \[\label{eq:void95probability} \Pr[N(\mathcal{F})=0]=e^{-\Lambda(\mathcal{F})}.\tag{7}\] This is the probability that \(\mathcal{F}\) contains no objects, which we use to reformulate the safety specification.

4 Problem Formulation↩︎

We consider a robot governed by 1 operating in an environment containing an unknown and time-varying number of indistinguishable objects, each evolving according to 2 . The robot state is assumed known for simplicity, so that the focus is on the objects’ state uncertainty. The multi-object state is modeled as an RFS \(\mathbb{O}\) with the standard assumption that the objects are independent [8]–[10].

In the single-object setting, safety with respect to an object \(\boldsymbol{o}\) is specified by a single-object smooth safety function \(h_o\), with the safe set \(\mathcal{C}_o := \{(\boldsymbol{x}, \boldsymbol{o}) \in \mathcal{X} \times \mathcal{O} : h_o(\boldsymbol{x}, \boldsymbol{o}) \geq 0\}\). In the multi-object setting, we use the same \(h_o\) for every object in \(\mathbb{O}\). The robot is therefore safe if \(h_o(\boldsymbol{x}, \boldsymbol{o}^{(i)}) \geq 0\) for every \(\boldsymbol{o}^{(i)} \in \mathbb{O}\). Since \(\mathbb{O}\) is an RFS, we require this safety specification to hold probabilistically, i.e., in a risk-aware sense at level \(\tau\), formalized as the following problem.

To deal with the chance constraint in Problem 1, we leverage the SMC-PHD filter, which represents the multi-object belief by a set of weighted particles. The probability in ?? is thus evaluated under the PHD belief. Using this representation, we cast Problem 1 as a set invariance problem in belief space. Specifically, we aim to design a safe set over the particle-based belief with risk-aware guarantees and synthesize a safe controller that renders this set forward invariant.

5 Risk-Aware Safe Set in Belief Space↩︎

5.1 Particle-Based Safety Specification↩︎

Under the SMC-PHD filter, the multi-object belief is parameterized by a weighted particle set \(\{(\tilde{\boldsymbol{o}}^{(i)}, w)\}_{i=1}^L\), where all particles share a uniform weight \(w\) after resampling. From this we define the belief state: \[\label{eq:belief95state} \boldsymbol{b}:= \begin{bmatrix} \tilde{\boldsymbol{o}}^{(1)} & \dots & \tilde{\boldsymbol{o}}^{(L)} \end{bmatrix}^\top \in \mathcal{B} \subseteq \mathbb{R}^{L \cdot n_o}.\tag{8}\]

To express the safety specification in belief space, we exploit the PPP structure of the multi-object belief via its void probability 7 . We first specify the relevant region by introducing the single-object failure set \[\label{eq:unsafe95region} \mathcal{F}(\boldsymbol{x}) := \left\{ \boldsymbol{o}\in \mathcal{O} \;\middle|\; h_o(\boldsymbol{x}, \boldsymbol{o}) < 0 \right\},\tag{9}\] the set of object states that render the robot unsafe. Because the same \(h_o\) applies to every object, \(\mathcal{F}\) is shared across all \(\boldsymbol{o}^{(i)} \in \mathbb{O}\). We denote \(N(\mathcal{F}(\boldsymbol{x}))\) as the number of objects in the failure set, and the two probabilities are equal: \[\Pr\!\big[\,h_o(\boldsymbol{x}, \boldsymbol{o}^{(i)}) \geq 0,\;\forall \boldsymbol{o}^{(i)} \in \mathbb{O}\,\big] = \Pr\!\big[N(\mathcal{F}(\boldsymbol{x}))=0\big].\] Under the PPP belief, applying 7 yields the belief space safety specification: \[\label{eq:problem95count95constraint} e^{-\Lambda(\mathcal{F}(\boldsymbol{x}), \boldsymbol{b})} \ge 1-\tau.\tag{10}\] Since the intensity of the PPP is estimated by the particle set \(\{(\tilde{\boldsymbol{o}}^{(i)}, w)\}_{i=1}^L\) as described in Remark 1, we have \[\label{eq:lambda95indicator} \Lambda(\mathcal{F}(\boldsymbol{x}), \boldsymbol{b}) \approx \sum_{i=1}^L w \mathbb{1}\{h_o(\boldsymbol{x}, \tilde{\boldsymbol{o}}^{(i)}) < 0\}.\tag{11}\] By further simplifying 10 , we obtain the specification \[\label{eq:problem95weight95fail} \ln\!\left(\frac{1}{1-\tau}\right) - \Lambda(\mathcal{F}(\boldsymbol{x}), \boldsymbol{b}) \geq 0.\tag{12}\] Intuitively, 12 requires that the total weight of the particles in the failure set not exceed a threshold determined by the risk level \(\tau\).

5.2 Belief Control Barrier Function↩︎

Although the safety specification 12 is straightforward to derive, its left-hand side cannot directly serve as a safety function, as the indicator in 11 is discontinuous in \((\boldsymbol{x},\boldsymbol{b})\), so its gradient is undefined. We therefore reformulate 12 into an equivalent form from which we construct a locally Lipschitz BCBF. To start, we define \(s_i(\boldsymbol{x},\boldsymbol{b}) := h_o\!\left(\boldsymbol{x}, \tilde{\boldsymbol{o}}^{(i)}\right)\) for \(i=1,\dots,L\), so that particle \(i\) lies in the safe set when \(s_i \ge 0\) and in the failure set when \(s_i < 0\). We let \(M(\boldsymbol{x},\boldsymbol{b}) := \sum_{i=1}^L \mathbb{1}\{s_i < 0\}\) count the particles in the failure set, so that specification 12 reads \(wM \leq \ln(1/(1-\tau))\). Since \(M\) is integer-valued and \(w\) is a uniform weight, the specification is equivalent to \(M \le k_\tau\), where \[\label{eq:k95threshold} k_\tau := \min\!\left\{ L,\; \left\lfloor \frac{1}{w}\ln\!\left(\frac{1}{1-\tau}\right) \right\rfloor \right\},\tag{13}\] or, equivalently, to requiring at least \(L - k_\tau\) safe particles: \[\label{eq:count95form} \sum_{i=1}^L \mathbb{1}\{s_i \ge 0\} \ge L - k_\tau.\tag{14}\] We let \(\mathcal{S} := \bigl\{ \mathcal{I} \subseteq \{1,\dots,L\} \mid |\mathcal{I}| = L - k_\tau \bigr\}\) denote the collection of all index subsets of cardinality \(L - k_\tau\). Condition 14 holds if and only if at least one subset \(\mathcal{I} \in \mathcal{S}\) has all of its particles in the safe set, which we can write as \[\label{eq:safety95spec95reform} \max_{\mathcal{I} \in \mathcal{S}} \;\min_{i \in \mathcal{I}} s_i(\boldsymbol{x},\boldsymbol{b}) \ge 0.\tag{15}\] We let \(s_{(1)} \le \cdots \le s_{(L)}\) denote the order statistics of \(\{s_i\}_{i=1}^L\). Since the inner \(\min\) in 15 increases with each \(s_i\), the outer \(\max\) is attained by any subset \(\mathcal{I}^\star\) indexing the top-\((L - k_\tau)\) values \(s_{(k_\tau+1)},\dots,s_{(L)}\). To remove the nonsmoothness of the inner \(\min\), we replace it with the smooth soft minimum [22] and define our BCBF: \[\label{eq:belief95cbf} h_b(\boldsymbol{x},\boldsymbol{b}) := -\frac{1}{\kappa} \ln\!\left( \sum_{i\in \mathcal{I}^\star} e^{-\kappa s_{i}(\boldsymbol{x},\boldsymbol{b})} \right),\tag{16}\] where \(\kappa > 0\). Here, \(h_b\) is an under-approximation of \(\min_{i \in \mathcal{I}^\star} s_i\) and recovers it as \(\kappa \to \infty\). The corresponding belief space safe set is: \[\label{eq:belief95safe95set} \begin{align} \mathcal{C}_b &:= \bigl\{\, (\boldsymbol{x},\boldsymbol{b}) \in \mathcal{X} \times \mathcal{B} \,\big|\; h_b(\boldsymbol{x},\boldsymbol{b}) \ge 0 \bigr\}, \\ \partial \mathcal{C}_b &:= \bigl\{\, (\boldsymbol{x},\boldsymbol{b}) \in \mathcal{X} \times \mathcal{B} \,\big|\; h_b(\boldsymbol{x},\boldsymbol{b}) = 0 \bigr\}. \end{align}\tag{17}\] In the following lemma, we show that \(h_b\) is locally Lipschitz.

Since \(h_o\) is locally Lipschitz, each \(s_i\) is locally Lipschitz in \((\boldsymbol{x},\boldsymbol{b})\). The sorting operation \((s_1,\dots,s_L)\mapsto(s_{(1)},\dots,s_{(L)})\) is locally Lipschitz  [23], so selecting its top-\((L-k_\tau)\) values \((s_{(k_\tau+1)},\dots,s_{(L)})\) is also locally Lipschitz. As the soft minimum is smooth, \(h_b\) is locally Lipschitz as a composition of locally Lipschitz functions.

To enforce forward invariance of \(\mathcal{C}_b\), there remain two challenges. First, although \(h_b\) is locally Lipschitz, it could be nonsmooth when particles tie at the threshold value \(s_{(k_\tau+1)}\). We therefore adopt the nonsmooth CBF framework [3] to synthesize a safe controller. Second, the belief evolves through continuous prediction and discrete updates, which we analyze separately in the next section.

6 Safe Controller Synthesis↩︎

6.1 Belief Dynamics↩︎

Between filter updates, each particle propagates according to the object dynamics: \[\dot{\boldsymbol{b}} = \begin{bmatrix} \boldsymbol{\xi}(\tilde{\boldsymbol{o}}^{(1)})^\top & \cdots & \boldsymbol{\xi}(\tilde{\boldsymbol{o}}^{(L)})^\top \end{bmatrix}^\top := \boldsymbol{\Xi}(\boldsymbol{b}).\] The particle weight \(w\) remains constant during the continuous prediction since no new information is incorporated. We denote the joint dynamics of the robot and the objects in the belief space as \[\label{eq:belief95dynamics} \boldsymbol{F}(\boldsymbol{x},\boldsymbol{b},\boldsymbol{u}) := \begin{bmatrix} \boldsymbol{f}(\boldsymbol{x})+\boldsymbol{g}(\boldsymbol{x})\boldsymbol{u}\\ \boldsymbol{\Xi}(\boldsymbol{b}) \end{bmatrix}.\tag{18}\]

At discrete update times \(\{t_k\}_{k \in \mathbb{N}}\), the SMC-PHD filter incorporates new observations \(\mathbb{Z}_k\), inducing a discrete update: \[\label{eq:belief95update} \boldsymbol{b}(t_k^+) = \boldsymbol{\Delta}_k\!\left(\boldsymbol{b}(t_k^-), \mathbb{Z}_k\right),\tag{19}\] where \(\boldsymbol{\Delta}_k\) can be discontinuous, as resampling can duplicate or remove particles, and birth introduces new particles depending on the measurements. The weight \(w\) is also updated at \(t_k\) but is omitted here for brevity. The overall belief dynamics form a hybrid system as follows: \[\label{eq:belief95hybrid} \begin{cases} \dot{\boldsymbol{b}} = \boldsymbol{\Xi}(\boldsymbol{b}), & t \in [t_{k-1}, t_{k}), \\ \boldsymbol{b}^+ = \boldsymbol{\Delta}_k\!\left(\boldsymbol{b}^-, \mathbb{Z}_k\right), & t = t_k. \end{cases}\tag{20}\] Here, \(\boldsymbol{b}^-=\boldsymbol{b}(t_k^-)\) and \(\boldsymbol{b}^+=\boldsymbol{b}(t_k^+)\) denote the belief states immediately before and after the update at \(t_k\), respectively.

6.2 Safety under Continuous Prediction↩︎

During the prediction phase, the belief evolves continuously according to 18 . When particles tie at the threshold value \(s_{(k_\tau+1)}\), the selection \(\mathcal{I}^\star\) may become non-unique, and where it does, the time derivative of \(h_b\) differs across these selections, which makes \(h_b\) nonsmooth. We therefore partition the indices into those strictly above the threshold, \(\mathcal{H}:= \{ \eta \mid s_\eta > s_{(k_\tau+1)} \}\), and those tied at it, \(\mathcal{T}:= \{ \mu \mid s_\mu = s_{(k_\tau+1)} \}\). Every \(\mathcal{I}^\star\) retains all of \(\mathcal{H}\) and completes it with a subset \(\mathcal{J} \subseteq \mathcal{T}\) of cardinality \(|\mathcal{J}| = L - k_\tau - |\mathcal{H}|\), i.e., \(\mathcal{I}^\star = \mathcal{H} \cup \mathcal{J}\). An illustration is shown in Fig. 2. We collect all such selections in the active set \[\mathcal{A} := \{\, \mathcal{H} \cup \mathcal{J} \mid \mathcal{J} \subseteq \mathcal{T},\;|\mathcal{J}| = L - k_\tau - |\mathcal{H}| \,\}.\] To render \(\mathcal{C}_b\) forward invariant, the following CBF condition should hold for every active realization: \[\label{eq:cbf95condition} \nabla h_b^{(\mathcal{I}^\star)\top} \boldsymbol{F}(\boldsymbol{x}, \boldsymbol{b}, \boldsymbol{u}) \;\geq\; -\gamma h_b, \quad \forall\, \mathcal{I}^\star \in \mathcal{A},\tag{21}\] where \(\gamma\) is a positive constant. Enforcing 21 directly requires one constraint for each realization \(\mathcal{I}^\star\), but the number of such realizations grows exponentially with the number of tied particles \(|\mathcal{T}|\). Therefore, we exploit the structure of the tie to obtain a more conservative but tractable set of conditions whose size scales linearly in \(|\mathcal{T}|\). We begin by expanding the gradient of each realization as \[\label{eq:gradient95expansion} \nabla h_b^{(\mathcal{I}^\star)} = \sum_{i \in \mathcal{I}^\star} c_i \nabla s_i = \sum_{\eta \in \mathcal{H}} c_\eta \nabla s_\eta + \sum_{j \in \mathcal{J}} c_j \nabla s_j.\tag{22}\] where the coefficient \(c_i := e^{-\kappa s_i} / \sum_{\ell \in \mathcal{I}^\star} e^{-\kappa s_\ell}\). Our key observation is that the tied particles share a common coefficient. Since \(s_\mu = s_{(k_\tau+1)}\) for every \(\mu \in \mathcal{T}\), each \(c_\mu\) equals the same value \(\bar{c}\). Consequently, \(\sum_{j \in \mathcal{J}} c_j \nabla s_j = \bar{c} \sum_{j \in \mathcal{J}} \nabla s_j\) in 22 . This motivates us to introduce a single auxiliary variable \(\rho \in \mathbb{R}\) that lower-bounds each tied gradient term, replacing 21 with the following sufficient conditions: \[\tag{23} \begin{empheq}[left=\empheqlbrace\,]{align} &\sum_{\eta \in \mathcal{H}} c_\eta \nabla s_\eta^\top \boldsymbol{F}(\boldsymbol{x},\boldsymbol{b},\boldsymbol{u}) + \bar{c} |\mathcal{J}| \rho \ge -\gamma h_b, \tag{24} \\ &\nabla s_\mu^\top \boldsymbol{F}(\boldsymbol{x},\boldsymbol{b},\boldsymbol{u}) \ge \rho, \quad \forall \mu \in \mathcal{T}. \tag{25} \end{empheq}\] We note that 23 contains \(|\mathcal{T}|\) constraints in 25 plus one constraint 24 . The following lemma establishes that 23 is sufficient for 21 .

Since \(\bar{c}>0\) and 25 gives \(\nabla s_\mu^\top \boldsymbol{F}\ge\rho\) for all \(\mu\in\mathcal{T}\), for any \(\mathcal{J}\subseteq\mathcal{T}\) we have \[\sum_{j \in \mathcal{J}} c_j \nabla s_j^\top \boldsymbol{F} = \bar{c}\sum_{j \in \mathcal{J}} \nabla s_j^\top \boldsymbol{F} \ge |\mathcal{J}| \bar{c} \rho.\] Combining this with 24 yields \[\sum_{i \in \mathcal{H}} c_i \nabla s_i^\top \boldsymbol{F} + \sum_{j \in \mathcal{J}} c_j \nabla s_j^\top \boldsymbol{F} \ge -\gamma h_b, \quad \forall \mathcal{J} \subseteq \mathcal{T}.\] By 22 , the left-hand side equals \(\nabla h_b^{(\mathcal{I}^\star)\top} \boldsymbol{F}\). Since every \(\mathcal{I}^\star \in\mathcal{A}\) is of this form, 21 holds.

Building on this, the next theorem establishes that satisfying conditions in 23 renders \(\mathcal{C}_b\) forward invariant during continuous prediction.

By Lemma 1, \(h_b\) is locally Lipschitz. Away from tie points, \(\mathcal{A}\) contains a single subset and \(h_b\) is smooth, therefore \(\partial h_b = \{\nabla h_b\}\). When tie points occur, by [3], the generalized gradient of \(h_b\) satisfies \[\label{eq:gen95gradient} \partial h_b(\boldsymbol{x},\boldsymbol{b}) = \operatorname{co}\!\bigl\{ \nabla h_b^{(\mathcal{I}^\star)}(\boldsymbol{x},\boldsymbol{b}) \mid \mathcal{I}^\star \in \mathcal{A} \bigr\},\tag{26}\] where \(\operatorname{co}\) denotes the convex hull. Given that 23 holds, by Lemma 2, 21 holds. By [3], \(h_b\) is a valid nonsmooth CBF for 18 . Thus, \(\mathcal{C}_b\) is forward invariant.

6.3 Safety under Discrete Updates↩︎

At time steps when sensor measurements become available, the belief state is updated according to 19 . This update may introduce particles into the failure set \(\mathcal{F}\). The resulting belief change cannot in general be fully compensated for by the control input at the update time. For example, under limited FOV or occlusion, a previously unobserved object may enter the FOV and trigger particle birth inside \(\mathcal{F}\). Nevertheless, we mitigate this issue by constructing 16 with a tightened risk level \(\tau-\epsilon\), where \(0<\epsilon<\tau\). We write \(\mathcal{C}_b^{\tau-\epsilon}\) and \(\mathcal{C}_b^{\tau}\) for the safe sets constructed with the tightened and original risk levels, respectively. The resulting safety margin permits a bounded increase in the total weight of particles in \(\mathcal{F}\), as quantified in the following proposition.

If the update satisfies ?? , then 12 holds with risk level \(\tau\), indicating that 15 is satisfied. Therefore, when \(\kappa\) approaches \(\infty\), 16 remains non-negative, which means the state \((\boldsymbol{x}, \boldsymbol{b})\) remains within \(\mathcal{C}_b^{\tau}\) under the update.

6.4 Controller Synthesis via BCBF-QP↩︎

The final risk-aware safe controller is synthesized via the following QP: \[\begin{align} \boldsymbol{u}^* = &\arg\min_{\boldsymbol{u}\in \mathcal{U}, \rho} (\boldsymbol{u}- \boldsymbol{u}_\mathrm{ref})^\top \boldsymbol{Q} (\boldsymbol{u}- \boldsymbol{u}_\mathrm{ref}) \\ \text{s.t.} \;\; & \eqref{eq:final95cbf95condition}\\ \end{align}\] Here, \(\boldsymbol{Q} \succ 0\) is the symmetric weighting matrix. We note that the controller synthesis reduces to a QP with \(n_u+1\) decision variables and \(|\mathcal{T}|+1\) constraints, which remains tractable even when the particle-based belief state is high-dimensional. The CBF condition 23 is enforced using the tightened risk level \(\tau-\epsilon\). The following remark discusses how the state is maintained in \(\mathcal{C}_b\) under the belief dynamics.

7 Experiments and Applications↩︎

We validate our approach in two example applications ⁶, where the robot needs to estimate the multi-object state to satisfy the safety specification.

7.1 Implementation Details↩︎

For runtime efficiency, we exploit a structural property of 23 : computing each \(s_i\) and evaluating \(c_i \nabla s_i^\top \boldsymbol{F}\) are independent across particles and therefore parallelizable. We implement this in JAX [25] using vectorization and automatic differentiation. A further difficulty arises when evaluating \(c_i\) in 22 , since \(e^{-\kappa s_i}\) can underflow for large \(\kappa\). To avoid this, we rescale the numerator and denominator by a common factor, which preserves \(c_i\) while ensuring numerical stability.

We solve the QPs with OSQP [26]. To guarantee that a control input is always available, we relax the CBF constraint with a heavily penalized slack variable. In simulation, we enable the relaxation only as a fallback when the strict QP is infeasible. On hardware, we instead keep it always active to ensure real-time performance. Throughout, we use the same \(\gamma\) for our method and the baselines, since a smaller \(\gamma\) induces more conservative behavior across all CBFs.

All computations are performed on a laptop with an Intel Core i7-13700H CPU, 32 GB of RAM, and an NVIDIA RTX 4070 GPU with 8 GB of memory.

7.2 Multi-Object FOV Maintenance↩︎

7.2.1 Setup↩︎

We consider a planar robot modeled as a unicycle with state \(\boldsymbol{x}= [p_x, p_y, \theta]^\top \in \mathbb{R}^2 \times \mathrm{S}^1\) and dynamics \[\dot{p}_x = u_v\cos\theta, \quad \dot{p}_y = u_v\sin\theta, \quad \dot{\theta} = u_\omega,\] where \([p_x,p_y]^\top\) and \(\theta\) denote the position and heading of the robot, respectively, and \(u_v\) and \(u_\omega\) are the linear and angular velocity inputs. The robot is equipped with a forward-facing sensor with a limited FOV that returns a set of range-and-bearing measurements. The robot should keep the moving objects within its FOV, whereas a reference controller attempts to stabilize the robot at its initial pose \([0,0,\pi/2]^\top\). An unknown number of objects are initialized in the FOV, sharing the dynamics and measurement model: \[\begin{align} &\dot{\boldsymbol{q}} = \boldsymbol{v}, \quad \dot{\boldsymbol{v}} = \boldsymbol{0}, \\ &\boldsymbol{z}= [\sqrt{{}^p q_x^2 + {}^p q_y^2}, \;\mathrm{arctan2}({}^p q_y, {}^p q_x)]^\top + \boldsymbol{\nu}, \end{align}\] where \(\boldsymbol{q}=[q_x, q_y]^\top\) and \(\boldsymbol{v}=[v_x, v_y]^\top\) are the object position and velocity, and \([{}^p q_x, {}^p q_y]^\top\) denotes the object position in the robot’s local frame. \(\boldsymbol{\nu} \sim \mathcal{N}(\boldsymbol{0}, \mathbf{R})\) is the measurement noise with \(\mathbf{R} = \mathrm{diag}(\sigma_r^2, \sigma_b^2)\), where \(\sigma_r = \SI{1}{\meter}\) and \(\sigma_b = \SI{1}{°}\). The sensor measurement rate is \(\SI{10}{\hertz}\). Since the velocity is not directly observed and the measurement model is nonlinear, we use the SMC-PHD filter [10] to estimate the full multi-object state. We resample to a fixed \(L=3000\) particles at each discrete update step.

We model the FOV as a bounded angular sector with aperture \(\beta=\SI{50}{°}\), as illustrated in Fig. 3. Following [27], we define a pair of single-object safety functions in the state space, one for each edge of the FOV sector: \[h^{\mathrm{R/L}}_o(\boldsymbol{x},\boldsymbol{o}) := \tan(\beta/2)\cdot{}^p q_x \pm {}^p q_y.\] We set the true number of objects to \(N_o=4\). The objects’ initial positions are randomly sampled along the central axis of the angular FOV sector, and their velocities are constant, with magnitudes uniform in \([0.5, 1]\), and directions uniform in \([-\pi/3, -\pi/6]\) relative to the positive \(x\)-axis.

7.2.2 Baselines↩︎

Given the particles from the SMC-PHD filter, a common way to extract object state estimates is to first cluster the particles and then take the mean of each cluster [9], [10]. We then evaluate the state space safety function \(h^{\mathrm{R/L}}_o\) for each object state estimate and aggregate them into a single CBF [22], which we refer to as Mean-CBF. Alternatively, one can take the highest-weight particle in each cluster as a maximum a posteriori (MAP) estimate. The CBF synthesized from these estimates is referred to as MAP-CBF.

7.2.3 Results↩︎

We evaluate our method against the baselines over 100 simulations. The statistical results are reported in Table 1. At each time \(t\), we let \[h^{\mathrm{gt}}_o(t) \;:=\; \min_{i \in \{1,\dots,N_o\}} \; \min\!\big\{\, h_o^{\mathrm{L}}(\boldsymbol{x}, \boldsymbol{o}^{(i)}),\; h_o^{\mathrm{R}}(\boldsymbol{x}, \boldsymbol{o}^{(i)}) \,\big\}\] denote the smallest ground-truth value of the state space safety function over both FOV edges and all \(N_o\) objects. If \(h^{\mathrm{gt}}_o\) becomes negative, the objects are not within the FOV. As shown in the table, our BCBF keeps \(h^{\mathrm{gt}}_o\) nonnegative when the risk level is sufficiently low, whereas both Mean-CBF and MAP-CBF can be unsafe. This is because they reduce the particle-based belief to a single estimate per object, discarding the spatial distribution of the particles. This leads to safety violations when the PHD belief is imperfect. In contrast, our BCBF is synthesized from the full belief and therefore remains robust to state uncertainty. Moreover, decreasing the risk level enlarges the safety margin, as the BCBF drives \(h^{\mathrm{gt}}_o\) further from zero, yielding stronger robustness. The computation time \(t_c\) includes both QP construction and solution. In all cases the average \(t_c\) remains below \(5\,\mathrm{ms}\), demonstrating the real-time performance of our method.

We visualize one simulation run of our BCBF with tightened risk level (\(\tau=0.05\), \(\epsilon=0.04\)) in Fig. 3. The objects move from the upper-left toward the lower-right. To maintain the objects within the FOV, the BCBF controller rotates the robot and drives it backward. We also plot the evolution of \(\min \{ h^{\mathrm{L}}_b, h^{\mathrm{R}}_b \}\). The observed spikes are mainly caused by the discrete PHD filter update. Notably, the BCBF value evaluated at the tightened level \(\tau - \epsilon\) may become negative, yet the BCBF value evaluated at the original risk level (\(\tau=0.05\)) remains nonnegative throughout. This illustrates that tightening the risk level can permit moderate belief changes caused by the discrete updates.

In practice, sensor measurements could contain false alarms caused by complex environmental effects, such as sand ripples in underwater scenes [28]. To further demonstrate the robustness of our framework, we repeat the simulation with false alarms, modeled as a PPP with uniform intensity. The results are shown in Fig. 4. The false alarms induce additional particle birth, causing more frequent spikes in the BCBF values, occasionally driving them negative. Nevertheless, the PHD filter is still able to estimate the objects’ states, and the safe controller keeps \(h^{\mathrm{gt}}_o\) nonnegative throughout, maintaining the objects within the FOV.

7.3 Obstacle Avoidance in Dynamic Environments↩︎

7.3.1 Simulation Setup and Baselines↩︎

We consider the robot to be a 3D single integrator with state \(\boldsymbol{x}= [p_x, p_y, p_z]^\top \in \mathbb{R}^3\) denoting its position, and dynamics \(\dot{\boldsymbol{x}} = \boldsymbol{u}\), where \(\boldsymbol{u}\) is the input velocity. The robot carries a depth sensor that returns position point clouds of the environment, with no prior knowledge of the obstacles’ shapes. We build on the SMC-PHD in [11], which treats the point cloud directly as a set of objects and estimates their positions and velocities, yielding a unified representation of the environment and its motion. The trade-off is a large, time-varying object set, since points appear and disappear under sensor motion and occlusion — precisely the setting where the RFS formulation is effective [11]. Following this approach, the object dynamics and measurement model are \[\dot{\boldsymbol{q}} = \boldsymbol{v}, \quad \dot{\boldsymbol{v}} = \boldsymbol{0}, \quad \boldsymbol{z}= {}^p\boldsymbol{q} + \boldsymbol{\nu},\] where \(\boldsymbol{q}=[q_x, q_y, q_z]^\top\) and \(\boldsymbol{v}=[v_x, v_y, v_z]^\top\) are the object position and velocity, \({}^p\boldsymbol{q}\) is the position in the robot’s local frame, and \(\boldsymbol{\nu} \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{R})\) is the measurement noise. We use PyBullet [29] to simulate the robot and the obstacles. The point cloud measurement is generated by ray-casting uniformly around the robot at a 10 Hz rate. In simulation, we assume that the robot has a full spherical FOV. We resample to a fixed \(L=8000\) particles for the SMC-PHD filter.

The single-object state space safety function is defined as \[h_o(\boldsymbol{x},\boldsymbol{o}) := \|\,[p_x - q_x, p_y - q_y, p_z - q_z]\|_2 - r_{\mathrm{safe}},\] where \(r_{\mathrm{safe}}\) is the safe distance. We benchmark our BCBF against a CBF that uses the soft minimum distance between the robot and the point cloud [30], which considers point positions but not their motion, i.e., it implicitly treats \(\boldsymbol{v} = \boldsymbol{0}\). Both methods use \(\kappa=100\) for the soft minimum.

2pt

7.3.2 Simulation Results↩︎

We evaluate both methods on three test cases shown in Fig. 5. In particular, case A presents a highly unstructured environment, mixing arbitrarily shaped static structures with moving obstacles. For cases A and B, we randomize the obstacles’ initial poses and velocity directions. For case C, we only keep the randomness in the evolution of the PHD filter. We run 100 simulations for each case, and the results are reported in Table 2.

In cases A and B, our BCBF achieves a higher success rate and a lower collision rate while maintaining a reasonable time \(t_n\) to reach the goal, reflecting the additional caution from anticipating obstacle motion. In contrast, the baseline [30] ignores obstacle velocities and thus incurs more collisions. In case C, our BCBF again attains a lower collision rate. As shown by Fig. 6, our method reacts earlier and avoids the obstacle, whereas the baseline collides.

7.3.3 Hardware Validation↩︎

We validate our method on a BlueROV underwater robot equipped with a 3D sonar, with state estimation from motion capture measurements in a tank arena [31]. The sonar provides point cloud measurements at \(\SI{5}{\hertz}\) within a \(\SI{90}{°} \times \SI{40}{°}\) (vertical \(\times\) horizontal) FOV, while the safe controller runs at \(\SI{50}{\hertz}\). We follow the reduced-order hierarchical control framework of [1], where a low-level controller tracks the input velocities, enabling us to give safe velocity commands at the kinematic level.

We first recreate the head-on collision setting of case C in hardware, shown in Fig. 7. As in simulation, our method reacts early enough by anticipating the obstacle’s motion, whereas the baseline [30] becomes unsafe. Then, we test our method under adversarial teleoperation toward the obstacles at about \(\SI{0.2}{\meter\per\second}\), as shown in Fig. 1. Our safe controller modifies the reference to keep a safe distance from both static and moving obstacles, demonstrating real-world applicability in dynamic unstructured environments.

8 Conclusion↩︎

In this work, we proposed a risk-aware BCBF framework for the particle-based beliefs produced by the SMC-PHD filter. The resulting safe controller remains computationally efficient even when the particle-based belief state is high-dimensional. We validated its robustness and adaptability in both simulation and hardware experiments. For future work, we intend to account for the mismatch between the estimated PPP belief and the true multi-object posterior, for example, through distributionally robust formulations [7], [14]. We also plan to move the computation onboard, enabling untethered safe operation in underwater environments.

Acknowledgement↩︎

We thank Matti Vahs, Ming Li, and Luzia Knoedler for the helpful discussions, and Julian Valdez for the hardware experiments.

References↩︎

---

1. *Corresponding author: shaohang@kth.se↩︎

2. \(^{1}\)Department of Robotics, Perception and Learning, KTH Royal Institute of Technology, Stockholm, Sweden↩︎

3. \(^{2}\)Department of Cognitive Robotics, Delft University of Technology, Delft, The Netherlands↩︎

4. \(^{3}\)Division of Aerospace, Moveability and Naval Architecture, KTH Royal Institute of Technology, Stockholm, Sweden↩︎

5. This work was partially supported by the Wallenberg AI, Autonomous Systems and Software Program (WASP) funded by the Knut and Alice Wallenberg Foundation. This work was also partially supported by Digital Futures, Vinnova and FMV through the SHARCEX grant. The authors are also affiliated with Digital Futures.↩︎

6. Code will be released upon acceptance.↩︎