betaFeedback

WorksComputer Science

FLASH-CBF-RRT: Fast Lipschitz-Adaptive Safe Horizon CBF Steering for Certified Motion Planning

Abstract

Fast motion planning requires both sample-efficient exploration and computationally efficient local steering. Conventional straight-line steering is inexpensive but wastes samples by rejecting colliding extensions. Quadratic program controllers with control barrier function (CBF) constraints can be used to achieve effective local steering but can increase the planning time due to the expensive sequential evaluations. We develop a method to accelerate CBF steering by computing a time horizon within which CBF-filtered velocity may be applied constantly using closed-form kinematic bounds on collision-clearance. Our method, FLASH-CBF-RRT, significantly reduces the number of evaluations compared to prior CBF-RRT approaches because the planner can take adaptive steering step sizes that are shorter near obstacles and longer in open space. We implement this steering procedure as an extension of RRT-Connect in OMPL. Across 689 MotionBenchMaker problems, FLASH-CBF-RRT matches RRT-Connect's 100% success rate while reducing median planning time by 3.82x, and is 3.88x faster than fixed-step CBF-RRT. Hardware experiments on a 17-DoF mobile manipulator demonstrate 2.2x faster planning and 4.2x fewer evaluations. Videos and results are available at: https://mamani828.github.io/FLASH-CBF-RRT/.

Introduction

Sampling-based motion planners such as rapidly exploring random trees (RRT) [1], RRT* [2], RRT-Connect (RRT-C) [3], and probabilistic roadmap (PRM) [4] are widely used for high-dimensional robot motion planning. Their efficiency depends critically on local steering. Conventional steering is inexpensive but follows straight configuration-space paths, rejecting extensions that encounter obstacles. In cluttered environments, this propose-and-reject process wastes samples and limits expansion progress.

Control barrier functions (CBFs) [5] can serve as an alternative steering mechanism because they provide local collision avoidance. CBF controllers use geometric information such as a signed distance field (SDF) [6] and its gradients to impose safety constraints on control inputs generated as solutions of quadratic programs (QPs). Rather than rejecting colliding extensions, CBF controllers can modify the steering command to avoid obstacles, as demonstrated in [7, 8, 9]. This can improve the sample efficiency of motion planning as samples near collision can be retained in the planning graph.

This advantage, however, comes with significant computational overhead [10]. For robot manipulators, each CBF steering step requires additional computation of forward kinematics (FK) Jacobians and a QP solution. Consequently, although CBF steering can produce better graph extensions, the computation can still be slower than conventional geometric planning in overall planning time.

We develop Fast Lipschitz-Adaptive Safe Horizon (FLASH)-CBF-RRT, a sampling-based planner designed to provide two advantages: sample-efficient exploration through CBF steering and low computational overhead through certified adaptive propagation of the CBF controls, illustrated in Figure 1. This certified adaptive propagation step builds on a key observation that a safe control input need not be recomputed at every small integration step but remains valid for a certain safe horizon. We adaptively compute the safe horizon in closed form by bounding the variation in collision clearance under constant joint velocity through Lipschitz continuity. The clearance variation bound is ob-

FLASH-CBF-RRT steering around obstacles

Figure 1. FLASH-CBF-RRT. (a) A CBF safety filter steers tree extensions around obstacles instead of rejecting them. Each filtered control is held for a certified duration, computed in closed form as the time at which a kinematic bound on link displacement under constant joint velocity would exhaust the current collision clearance. (b)–(c) Workspace- and configuration-space views.

This work was supported by the National Science Foundation (NSF) CAREER Award (grant No. 2047138) and the Technology Innovation Program (20018112, Development of autonomous manipulation and gripping technology using imitation learning based on visual and tactile sensing) funded by the Ministry of Trade, Industry & Energy (MOTIE), Korea. tained from a new closed-form bound on the robot link displacements throughout a constant joint-velocity motion. The same bounds are used to reduce the individual QP sizes by removing provably inactive CBF constraints. Because the bounds are certified, FLASH-CBF-RRT inherits continuous motion safety in addition to faster planning time.

We evaluate FLASH-CBF-RRT on UR5 in seven simulated MotionBenchMaker [11] environments, and on a real Reachy2 robot where we plan and execute motions using an SDF representation [6] estimated using an onboard RGB-D camera. The simulated experiments show that FLASH-CBF-RRT plans 3.82× faster than standard RRT-C [3] and 20.83× faster than a prior CBF-RRT baseline [10], which proposes to stop the steer without solving the QP when CBF constraints are active. Ablation experiments show that our certified adaptive propagation method reduces median planning time by 3.75× and CBF-QP solves by 3.76×.

Our contributions are summarized as follows.

  • We develop FLASH-CBF-RRT, a sampling-based planner that uses CBF-based steering to improve the effectiveness of planning graph expansions.

  • We derive a new certified adaptive propagation bound that determines how long a filtered control may be safely propagated, allowing FLASH-CBF-RRT to make long excursions with certified safety without intermediate geometric evaluations or CBF-QP solves. This new bound substantially reduces the computational overhead of CBF steering, as well as the number of constraints for the CBF-QP.

  • We provide an efficient implementation of FLASH-CBF-RRT within OMPL [12] and evaluate it on UR5 simulation benchmarks and a 17-DoF Reachy2 mobile manipulator with real RGB-D data. The results demonstrate faster median planning time than both geometric RRT-C and CBF-RRT without certified adaptive propagation, at an equal success rate.

Related Work

The computational efficiency of sampling-based planners such as RRT [1] and RRT-C [3] strongly depends on the time spent in collision-checking the candidate configurations. Thus, fast planning can be achieved by biasing the samples or speeding up each collision check. Approaches such as [13, 14] speed up collision checking by exploiting specialized data structures such as [15] that enable parallel computation. Here, we also consider speeding up individual geometric operations but do so through mathematical reformulations that are independent of compute hardware. Our approach reduces the overall number of geometric operations by biasing the samples toward free space with adaptive steering step sizes.

Many prior approaches also consider biasing the samples toward free space. Classical approaches such as [16] and [17] pre-process the obstacle geometry into discrete primitives that allow generating desirable samples. These approaches require an explicit representation of the obstacles, which is difficult to obtain from real sensor data. Instead, other approaches rely on continuous or sensor-driven obstacle representations [18] and bias candidate edges via artificial potential fields [19], local trajectory optimization [20, 21], or reinforcement learning [22]. However, these methods either require costly trajectory optimization for each candidate motion [21, 22], or do not certify safety of the local connections [19, 22]. A promising recent direction is to combine sampling-based planners with CBF-based local controllers [5] that guarantee safety in the limit of small time-steps. CBFs allow local collision avoidance by first representing the free space as the super-level set of a function, such as SDFs [6], and imposing a constraint on the rate of change of the function over the controlled trajectory, which is linear in the control action. The combination of CBFs with sampling-based planning was first considered in [7]. Subsequent work improves the computation time, by biasing samples using the cross-entropy method [8] or stopping without solving the QP when the nominal control violates the CBF constraint [10]. The work in [9] extends CBF-RRT to manipulators by learning a neural CBF candidate that takes perception information as input. These methods achieve progress around obstacles but repeated barrier evaluations and QP solves introduce computational overhead. In comparison, our work focuses on speeding up CBF-RRT by deriving certified adaptive step sizes within which QP solves may be skipped, and reducing the number of CBF constraints at each configuration.

Adaptive step sizes have been considered in the context of efficient collision checking. Early work in [23] uses adaptive step sizes derived from clearance information to expedite collision checking of a straight line in configuration space. Similarly, [24] uses clearance information to form reusable collision-free neighborhoods for collision checking. Similar benefits are observed in [25, 26]. Approaches such as [27, 28, 29] construct a locally safe polytopic corridor based on collision samples. Here, we use clearance information, and derive a new, tighter bound along a constant configuration velocity to reduce QP solves. The adaptive step size can be viewed from the perspective of sampled-data and self-triggered CBFs [30, 31, 32], which bound the interval between control updates to ensure continuous safety throughout the motion. However, no prior method has computed an explicit safe hold-time by exploiting both the kinematic structure of an articulated manipulator and the structure of an SDF-based safety constraint. We derive such a hold-time for steering, during which the filtered control can be applied without reevaluating the barriers or resolving the CBF-QP.

Problem Formulation

We consider a manipulator robot with nn joints moving in a static workspace W⊂R3\mathcal{W} \subset\mathbb{R}^{3} that contains obstacles O⊂W\mathcal{O} \subset\mathcal{W}. The robot has configuration q∈C\mathbf{q} \in\mathcal{C} and velocity input u∈Rn\mathbf{u} \in\mathbb{R}^{n}, so that q˙=u\dot{\mathbf{q}} = \mathbf{u}. The robot’s geometry is approximated by spheres indexed by S={1,…,N}\mathcal{S} = \{1,\ldots,N\}, which can be obtained through tools such as FOAM [33]. Each sphere s∈Ss \in\mathcal{S} has radius rs>0r_s > 0, and its world-frame center at configuration q\mathbf{q} is given by the FK map ps:C→R3\mathbf{p}_s : \mathcal{C} \to\mathbb{R}^{3}.

We assume that we are given an SDF representation d:R3→Rd : \mathbb{R}^{3} \to\mathbb{R} of the obstacles O\mathcal{O}, defined as the signed distance to the closest obstacle:

d(p)={min⁡p′∈∂O∥p−p′∥,p∉O,−min⁡p′∈∂O∥p−p′∥,p∈O.(1)d(\mathbf{p}) = \begin{cases} \min_{\mathbf{p}'\in\partial\mathcal{O}} \lVert\mathbf{p}-\mathbf{p}'\rVert, & \mathbf{p}\notin\mathcal{O},\\ -\min_{\mathbf{p}'\in\partial\mathcal{O}} \lVert\mathbf{p}-\mathbf{p}'\rVert, & \mathbf{p}\in\mathcal{O}. \end{cases} \tag*{(1)}

An SDF is 1-Lipschitz, i.e., ∣d(p′)−d(p)∣≤∥p′−p∥\lvert d(\mathbf{p}')-d(\mathbf{p})\rvert\le\lVert\mathbf{p}'-\mathbf{p}\rVert. Whenever differentiable, its gradient satisfies ∥∇d(p)∥=1\lVert\nabla d(\mathbf{p})\rVert=1. We assume that we have a differentiable SDF map from sensor data that satisfies the 1-Lipschitz property. This can be achieved using tools such as [6, 34].

Let P⊆{(a,b)∈S×S:a<b}\mathcal{P}\subseteq\{(a,b)\in\mathcal{S}\times\mathcal{S}:a<b\} denote sphere pairs on non-adjacent robot links that may self-collide. A configuration q\mathbf{q} is collision-free if

d(ps(q))≥rs,∀s∈S,d(\mathbf{p}_{s}(\mathbf{q}))\ge r_{s},\qquad\forall s\in\mathcal{S},
∥pa(q)−pb(q)∥≥ra+rb,∀(a,b)∈P.(2)\lVert\mathbf{p}_{a}(\mathbf{q})-\mathbf{p}_{b}(\mathbf{q})\rVert\ge r_{a}+r_{b},\qquad\forall(a,b)\in\mathcal{P}. \tag*{(2)}

Given two configurations qstart,qgoal\mathbf{q}_{\mathrm{start}},\mathbf{q}_{\mathrm{goal}}, we aim to find a continuous path π:[0,1]→C\pi:[0,1]\to\mathcal{C} that satisfies the collision constraints (2) and the boundary conditions π(0)=qstart\pi(0)=\mathbf{q}_{\mathrm{start}} and π(1)=qgoal\pi(1)=\mathbf{q}_{\mathrm{goal}}. As shown in Figure 2, our approach utilizes the SDF d(p)d(\mathbf{p}) and its gradients ∇d(p)\nabla d(\mathbf{p}) to: a) find collision-avoiding velocities to efficiently explore the free space; and b) compute certified adaptive horizons within which the velocities from a) can be used without recomputing.

Certified CBF steering

Figure 2. Certified CBF steering. (a) One extension from qnear\mathbf{q}_{\mathrm{near}} toward qrand\mathbf{q}_{\mathrm{rand}}. At each orange dot, the safety filter (4) computes a control held for the certified duration τhold\tau_{\mathrm{hold}}, yielding longer segments in open space and shorter segments near obstacles. (b) Control-space view at the circled configuration. Solid lines denote retained CBF constraints; the green polygon is the feasible set within the joint-velocity limits, onto which unom\mathbf{u}_{\mathrm{nom}} is projected to obtain u∗\mathbf{u}^{\ast}. Dashed lines denote constraints (h4h_{4}, h5h_{5}) certified redundant by Theorem IV.3. (c) Workspace view with a sphere-approximated mobile manipulator and an SDF cross-section. SDF queries at sphere centers provide distances and gradients for the safety filter. Spheres 1–3 generate the retained constraints in (b); spheres 4 and 5 generate the screened constraints.

FLASH-CBF-RRT

FLASH-CBF-RRT grows two trees, Ts\mathcal{T}_{s} from the start and Tg\mathcal{T}_{g} from the goal. Each iteration samples qrand\mathbf{q}_{\mathrm{rand}}, finds its nearest node qnear\mathbf{q}_{\mathrm{near}} in the active tree, and extends from qnear\mathbf{q}_{\mathrm{near}} toward qrand\mathbf{q}_{\mathrm{rand}} with the certified CBF steering of Algorithm 1, yielding qnew\mathbf{q}_{\mathrm{new}}. The other tree then steers toward qnew\mathbf{q}_{\mathrm{new}} until it is reached or progress stalls, after which the trees swap roles; planning ends once they connect. Unlike the straight-line steering and collision checking of RRT-C, each steering update bends around obstacles using a CBF-QP controller. The CBF steering is accelerated by certifying how long a control can be held without re-evaluation (FirstOrderBound and SecondOrderBound, Sec. IV-B) and removing redundant constraints (Screen, Sec. IV-C).

Algorithm 1: Certified CBF Steering

Input: q\mathbf{q} and qtarget\mathbf{q}_{\mathrm{target}}

Output: Steering status and qnew\mathbf{q}_{\mathrm{new}}

while q\mathbf{q} has not reached qtarget\mathbf{q}_{\mathrm{target}} do

unom←K(qtarget−q)\mathbf{u}_{\mathrm{nom}}\leftarrow K(\mathbf{q}_{\mathrm{target}}-\mathbf{q})

τtarget←∥qtarget−q∥∥unom∥\tau_{\mathrm{target}}\leftarrow\frac{\lVert\mathbf{q}_{\mathrm{target}}-\mathbf{q}\rVert}{\lVert\mathbf{u}_{\mathrm{nom}}\rVert}

IQP←Screen⁡(q)\mathcal{I}_{\mathrm{QP}}\leftarrow\operatorname{Screen}(\mathbf{q}) \hfill // Thm. IV.3

u∗←CBF-QP⁡(q,unom,IQP)\mathbf{u}^{\ast}\leftarrow\operatorname{CBF\text{-}QP}(\mathbf{q},\mathbf{u}_{\mathrm{nom}},\mathcal{I}_{\mathrm{QP}}) \hfill // Eq. (4)

if CBF-QP⁡\operatorname{CBF\text{-}QP} infeasible ∨max⁡j∣uj∗∣≤ϵu\lor\max_{j}\lvert u_{j}^{\ast}\rvert\le\epsilon_{u} then

&nbsp;&nbsp;return (TRAPPED⁡,q)(\operatorname{TRAPPED},\mathbf{q})

// Props. IV.1 and IV.2

foreach i∈Ii\in\mathcal{I} do

&nbsp;&nbsp;τi,Lip←FirstOrderBound⁡(hi,q,u∗)\tau_{i,\mathrm{Lip}}\leftarrow\operatorname{FirstOrderBound}(h_{i},\mathbf{q},\mathbf{u}^{\ast})

&nbsp;&nbsp;τi,Kin←SecondOrderBound⁡(hi,q,u∗)\tau_{i,\mathrm{Kin}}\leftarrow\operatorname{SecondOrderBound}(h_{i},\mathbf{q},\mathbf{u}^{\ast})

&nbsp;&nbsp;τi←max⁡{τi,Lip,τi,Kin}\tau_{i}\leftarrow\max\{\tau_{i,\mathrm{Lip}},\tau_{i,\mathrm{Kin}}\}

τhold←min⁡{τtarget,min⁡i∈Iτi}\tau_{\mathrm{hold}}\leftarrow\min\{\tau_{\mathrm{target}},\min_{i\in\mathcal{I}}\tau_{i}\} \hfill // Eq. (19)

if τhold\tau_{\mathrm{hold}} below threshold then

&nbsp;&nbsp;return (ADVANCED⁡,q)(\operatorname{ADVANCED},\mathbf{q})

q←q+τholdu∗\mathbf{q}\leftarrow\mathbf{q}+\tau_{\mathrm{hold}}\mathbf{u}^{\ast}

qnew←q\mathbf{q}_{\mathrm{new}}\leftarrow\mathbf{q}

return (REACHED⁡,qnew)(\operatorname{REACHED},\mathbf{q}_{\mathrm{new}})

CBF-Based Steering

Standard sampling-based planners steer from qnear\mathbf{q}_{\mathrm{near}} toward a target qtarget\mathbf{q}_{\mathrm{target}} along a straight line in configuration space and reject the segment if any intermediate state is in collision. We instead filter the steering command online. Let q∈C\mathbf{q}\in\mathcal{C} be the current configuration during an extension and define a nominal control unom=K(qtarget,q)\mathbf{u}_{\mathrm{nom}}=K(\mathbf{q}_{\mathrm{target}},\mathbf{q}) given by a controller. Using the collision model from Sec. III, define the barriers

hi(q):={d(pi(q))−ri,i∈S,∥pa(q)−pb(q)∥−ra−rb,i=(a,b)∈P.(3)h_{i}(\mathbf{q}) := \begin{cases} d(\mathbf{p}_{i}(\mathbf{q}))-r_{i}, & i\in\mathcal{S},\\ \lVert\mathbf{p}_{a}(\mathbf{q})-\mathbf{p}_{b}(\mathbf{q})\rVert-r_{a}-r_{b}, & i=(a,b)\in\mathcal{P}. \end{cases} \tag*{(3)}

where i∈I:=S∪Pi \in\mathcal{I} := \mathcal{S} \cup\mathcal{P} indexes the sphere–obstacle and self-collision barriers. Rather than executing unom\mathbf{u}_{\mathrm{nom}} directly, we filter it to satisfy the CBF constraint as:

u⋆=arg⁡min⁡u 12∥u−unom∥2\mathbf{u}^{\star} = \underset{\mathbf{u}}{\arg\min}\ \frac{1}{2}\lVert\mathbf{u} - \mathbf{u}_{\mathrm{nom}}\rVert^{2}
s.t.∇hi(q)Tu≥−κhi(q),∀i∈I,\text{s.t.}\quad\nabla h_i(\mathbf{q})^{\mathsf{T}}\mathbf{u} \geq-\kappa h_i(\mathbf{q}),\quad\forall i \in\mathcal{I},
umin⁡≤u≤umax⁡,(4)\mathbf{u}_{\min} \leq\mathbf{u} \leq\mathbf{u}_{\max}, \tag*{(4)}

where κ>0\kappa> 0 is the CBF gain bounding the admissible rate of barrier decrease. The problem (4) is a QP, and can be solved using methods such as [35]. We integrate the result through the first-order model q˙=u⋆\dot{\mathbf{q}} = \mathbf{u}^{\star}. Collision avoidance is thus enforced during the extension itself rather than checked afterward. Because the CBF-QP acts as a local safety filter, there may exist configurations for which the feasible control is substantially attenuated by the active barrier constraints, resulting in little or no progress toward qtarget\mathbf{q}_{\mathrm{target}}. We detect such cases using a minimum-control threshold; if max⁡j∣uj⋆∣≤ϵu\max_j |u_j^{\star}| \leq\epsilon_u, where ϵu\epsilon_u is the termination threshold, the current extension is terminated early.

Certified Adaptive Propagation

While CBF steering can provide sample efficiency, it has to solve a QP at every timestep. Meanwhile, the filtered control u⋆\mathbf{u}^{\star} can stay collision free for a longer period while making progress towards the node. We derive a certified adaptive horizon that the filtered control remains collision free, thus skipping QP solves and proximity queries.

Let q0\mathbf{q}_0 be the initial configuration at which the QP (4) was solved, and let τtarget>0\tau_{\mathrm{target}} > 0 denote the maximum propagation duration considered by the current steering update. Holding u⋆\mathbf{u}^{\star} under the first-order model introduced in Sec. IV-A:

q(τ)=q0+τu⋆,0≤τ≤τtarget.(5)\mathbf{q}(\tau) = \mathbf{q}_0 + \tau\mathbf{u}^{\star},\qquad0 \leq\tau\leq\tau_{\mathrm{target}}. \tag*{(5)}

The exact safe horizon of the held control is

τsafe⋆:=inf⁡{τ∈[0,τtarget]∣hi(q(τ))<0, ∃i∈I}.(6)\tau_{\mathrm{safe}}^{\star} := \inf\{\tau\in[0,\tau_{\mathrm{target}}] \mid h_i(\mathbf{q}(\tau)) < 0,\ \exists i \in\mathcal{I}\}. \tag*{(6)}

Here, τtarget\tau_{\mathrm{target}} is the nominal propagation time to the target and caps propagation under u⋆\mathbf{u}^{\star} to preserve RRT’s local exploration behavior. To bound τsafe⋆\tau_{\mathrm{safe}}^{\star}, we use the fact that all barriers hih_i are a form of geometric distance, and are hence 1-Lipschitz in the corresponding workspace sphere center pi(q)\mathbf{p}_i(\mathbf{q}). Therefore, the change in any hih_i is bounded by the displacement in the sphere center. With pi(τ)=pi(q(τ))\mathbf{p}_i(\tau) = \mathbf{p}_i(\mathbf{q}(\tau)) as a shorthand:

di(τ)=∥pi(τ)−pi(0)∥,(7)d_i(\tau) = \lVert\mathbf{p}_i(\tau) - \mathbf{p}_i(0)\rVert, \tag*{(7)}

and hi(q(τ))≥hi(q(0))−di(τ)h_i(\mathbf{q}(\tau)) \geq h_i(\mathbf{q}(0)) - d_i(\tau) by Lipschitz continuity. We derive two closed-form upper bounds on the displacement di(τ)d_i(\tau) to lower bound the barrier value at any τ\tau, and identify safe horizons within which the barrier values remain positive.

First-Order Bound

We first derive a linear upper bound on di(τ)d_i(\tau) using the Lipschitz continuity of the FK map ps(⋅)\mathbf{p}_s(\cdot). We use the following upper bound on the Lipschitz constant from [25]. For each joint jj, let ℓj\ell_j denote the length of joint jj, and for each sphere s∈Ss \in\mathcal{S}, let msm_s denote the joint the sphere ss belongs to. The Lipschitz constant for each sphere’s position ps\mathbf{p}_s with respect to joint jj is bounded by:

Λsj=∑k=jmsℓk,(8)\Lambda_{sj} = \sum_{k=j}^{m_s} \ell_k, \tag*{(8)}

so that di(τ)≤∑kΛik∥qk(τ)−qk(0)∥d_i(\tau) \leq\sum_k \Lambda_{ik}\lVert\mathbf{q}_k(\tau) - \mathbf{q}_k(0)\rVert for i∈Si \in\mathcal{S}. Intuitively, the constants Λsj\Lambda_{sj} corresponds to the largest possible radius of rotation of sphere ss around joint jj’s axis, assuming a fully extended manipulator [25]. For the barriers i∈Ii \in\mathcal{I}, the Lipschitz constants are:

Aik:={Λik,i∈S,Λak+Λbk,i=(a,b)∈P.(9)A_{ik} := \begin{cases} \Lambda_{ik}, & i \in\mathcal{S},\\ \Lambda_{ak} + \Lambda_{bk}, & i = (a,b) \in\mathcal{P}. \end{cases} \tag*{(9)}

which satisfy hi(q(τ))−hi(q(0))≥−∑kAik∣qk(τ)−qk(0)∣h_i(\mathbf{q}(\tau)) - h_i(\mathbf{q}(0)) \geq-\sum_k A_{ik}\lvert q_k(\tau) - q_k(0)\rvert for all barriers. This follows from the 1-Lipschitz property of the environment distance field for the environment barriers i∈Si \in\mathcal{S}, and from the triangle inequality for the self-collision barriers i∈Pi \in\mathcal{P}. Using the Lipschitz constants AikA_{ik}, the first-order bound is given as follows:

Proposition IV.1 (First-order horizon). For a given u⋆\mathbf{u}^{\star}, consider the first-order bound:

γi⋆:=∑k=1nAik∣uk⋆∣.(10)\gamma_i^{\star} := \sum_{k=1}^{n} A_{ik}|u_k^{\star}|. \tag*{(10)}

Suppose hi(q0)≥0h_i(\mathbf{q}_0) \geq0 for every i∈Ii \in\mathcal{I}, and define

τi,Lip:=hi(q0)γi⋆(11)\tau_{i,\mathrm{Lip}} := \frac{h_i(\mathbf{q}_0)}{\gamma_i^{\star}} \tag*{(11)}

with +∞+\infty for γi⋆=0\gamma_i^{\star} = 0. Then,

τLip:=min⁡{τtarget,min⁡i∈Iτi,Lip}≤τsafe⋆(12)\tau_{\mathrm{Lip}} := \min\left\{\tau_{\mathrm{target}},\min_{i\in\mathcal{I}}\tau_{i,\mathrm{Lip}}\right\} \leq\tau_{\mathrm{safe}}^{\star} \tag*{(12)}

is a certified hold time, i.e., hi(q(τ))≥0h_i(\mathbf{q}(\tau)) \geq0, ∀τ∈[0,τLip]\forall\tau\in[0,\tau_{\mathrm{Lip}}].

Proof. Substituting u⋆\mathbf{u}^{\star}, for every τ≤τLip\tau\leq\tau_{\mathrm{Lip}} and i∈Ii \in\mathcal{I}, hi(q(τ))−hi(q0)≥−τγi⋆h_i(\mathbf{q}(\tau)) - h_i(\mathbf{q}_0) \geq-\tau\gamma_i^{\star} by Lipschitz continuity. Since τ≤τLip\tau\leq\tau_{\mathrm{Lip}}, substituting (11) yields the claim. □\square

This bound is conservative for two reasons. First, the coefficients Λik\Lambda_{ik} are worst-case kinematic bounds over C\mathcal{C}. Second, (10) assumes that all joint contributions simultaneously reduce the same clearance. Consequently, the resulting horizon τLip\tau_{\mathrm{Lip}} may be substantially smaller than τsafe⋆\tau_{\mathrm{safe}}^{\star}.

Second-Order Bound

We now derive a second-order bound on the displacement. To do so, we invoke Taylor’s theorem with integral remainder as:

di(τ)≤τ∥p˙i(0)∥2+∫0τ(τ−s)∥p¨i(s)∥ ds.(13)d_i(\tau) \leq\tau\lVert\dot{\mathbf{p}}_i(0)\rVert_2 + \int_{0}^{\tau}(\tau-s)\lVert\ddot{\mathbf{p}}_i(s)\rVert\,\mathrm{d}s. \tag*{(13)}

The linear term can be computed exactly as ∥p˙i(0)∥=∥Js(q0)u⋆∥\lVert\dot{\mathbf{p}}_i(0)\rVert= \lVert J_s(\mathbf{q}_0)\mathbf{u}^{\star}\rVert, where Js(q)J_s(\mathbf{q}) is the position Jacobian of sphere ss at q\mathbf{q}. We derive a uniform bound on the second order term ∥p¨i(s)∥\lVert\ddot{\mathbf{p}}_i(s)\rVert over [0,τtarget][0,\tau_{\mathrm{target}}] so that (13) can be bounded uniformly. The acceleration of the sphere center evolves as:

p¨s(τ)=∑j=1msω˙j(τ)×rsj(τ)+ωj(τ)×(ωj(τ)×rsj(τ)),(14)\ddot{\mathbf{p}}_s(\tau) = \sum_{j=1}^{m_s}\dot{\boldsymbol{\omega}}_j(\tau) \times\mathbf{r}_{sj}(\tau) + \boldsymbol{\omega}_j(\tau) \times\left(\boldsymbol{\omega}_j(\tau) \times\mathbf{r}_{sj}(\tau)\right), \tag*{(14)}

where ×\times is the cross product, ωj(τ)\omega_j(\tau) is the angular velocity of link jj at τ\tau, and rsj(τ)r_{sj}(\tau) is the vector between: 1) for j<msj < m_s, the origins of joint jj and j+1j + 1 in the world frame, and 2) for j=msj = m_s, the origin of joint jj and the center ps\mathbf{p}_s.

To bound linear acceleration (14), we will bound the angular velocity ∥ωj(τ)∥\lVert\omega_j(\tau)\rVert and angular acceleration ∥ω˙j(τ)∥\lVert\dot{\omega}_j(\tau)\rVert uniformly over τ∈[0,τtarget]\tau\in[0,\tau_{\mathrm{target}}]. First, ωj=∑k=1juk⋆zk(τ)\omega_j = \sum_{k=1}^{j} u_k^{\star}z_k(\tau), where zk(τ)z_k(\tau) are unit vectors along each joint axis in the world frame. Suppose that c‾km≤zk(τ)Tzm(τ)≤c‾km\underline{c}_{km} \le z_k(\tau)^{\mathsf{T}}z_m(\tau) \le\overline{c}_{km} for all k<mk < m on the path and t∈[0,τtarget]t \in[0,\tau_{\mathrm{target}}], which we will derive soon. Expanding ∥ωj∥2\lVert\omega_j\rVert^2 gives

∥ωj∥2≤∑k=1j(uk⋆)2+∑1≤k<m≤j2max⁡{uk⋆um⋆c‾km,uk⋆um⋆c‾km}\lVert\omega_j\rVert^2 \le\sum_{k=1}^{j}(u_k^{\star})^2+\sum_{1\le k<m\le j}2\max\{u_k^{\star}u_m^{\star}\underline{c}_{km},u_k^{\star}u_m^{\star}\overline{c}_{km}\}
{#eq:15}

:=\Omega_j^2

Equation not extracted

For angular acceleration, we have ω˙j=∑k=1juk⋆(ωk−1×zk)\dot{\omega}_j=\sum_{k=1}^{j}u_k^{\star}(\omega_{k-1}\times z_k). The cross product term is bounded as ∥ωk−1×zk∥2=∥ωk−1∥2−(ωk−1Tzk)2\lVert\omega_{k-1}\times z_k\rVert^2=\lVert\omega_{k-1}\rVert^2-(\omega_{k-1}^{\mathsf{T}}z_k)^2. Using the bounds c‾km\underline{c}_{km} and c‾km\overline{c}_{km} on the dot-product terms from before, it can be shown that:

∥ω˙j∥≤∑k=1j∣uk⋆∣Ωk−12−(max⁡{0,ηk,−η‾k})2(15)\lVert\dot{\omega}_j\rVert\le\sum_{k=1}^{j}|u_k^{\star}|\sqrt{\Omega_{k-1}^2-\left(\max\{0,\eta_k,-\overline{\eta}_k\}\right)^2} \tag*{(15)}
:=Bj,:=B_j,

where ηk:=∑r=1k−1min⁡{uk⋆crk,uk⋆c‾rk}\eta_k:=\sum_{r=1}^{k-1}\min\{u_k^{\star}c_{rk},u_k^{\star}\overline{c}_{rk}\}, and η‾k\overline{\eta}_k sums over the maximum. The angular velocity bound (15) is understood with Ω0:=0\Omega_0:=0 and empty sums for η\eta are equal to zero. The radicand is nonnegative because it derives from bounding the dot product ωk−1Tzk\omega_{k-1}^{\mathsf{T}}z_k. Because the segment lengths ℓj=∥rsj(τ)∥\ell_j=\lVert r_{sj}(\tau)\rVert are fixed, the acceleration (14) can be bounded as:

∥p¨s(τ)∥≤∑j=1ms∥ω˙j(τ)∥ℓj+∥ωj(τ)∥2ℓj\lVert\ddot{\mathbf{p}}_s(\tau)\rVert\le\sum_{j=1}^{m_s}\lVert\dot{\omega}_j(\tau)\rVert\ell_j+\lVert\omega_j(\tau)\rVert^2\ell_j
{#eq:17}

\le\sum_{j=1}^{m_s}\ell_j(B_j+\Omega_j^2):=C_s.

Thisgivesoursecond−orderbound:This gives our second-order bound:

Proposition IV.2 (Second-order safe horizon). Suppose hi(q0)≥0h_i(\mathbf{q}_0)\ge0 for every i∈Ii\in\mathcal{I}. Let the per-barrier second-order bound be:

τi,Kin:=2hi(q0)∥Ji(q0)u⋆∥+∥Ji(q0)u⋆∥2+2Cihi(q0)(16)\tau_{i,\mathrm{Kin}}:=\frac{2h_i(\mathbf{q}_0)}{\lVert J_i(\mathbf{q}_0)\mathbf{u}^{\star}\rVert+\sqrt{\lVert J_i(\mathbf{q}_0)\mathbf{u}^{\star}\rVert^2+2C_i h_i(\mathbf{q}_0)}} \tag*{(16)}

Then, the second-order horizon defined as:

τKin:=min⁡{τtarget,min⁡i∈Iτi,Kin},(17)\tau_{\mathrm{Kin}}:=\min\left\{\tau_{\mathrm{target}},\min_{i\in\mathcal{I}}\tau_{i,\mathrm{Kin}}\right\}, \tag*{(17)}

is a certified horizon. I.e., hi(q(τ))≥0h_i(\mathbf{q}(\tau))\ge0, ∀i\forall i, ∀τ∈[0,τKin]\forall\tau\in[0,\tau_{\mathrm{Kin}}].

Proof. Since hih_i is 1-Lipschitz in pi\mathbf{p}_i for i∈Si\in\mathcal{S}, hi(q(τ))≥hi(q0)−∥Ji(q0)u⋆∥τ−12Ciτ2h_i(\mathbf{q}(\tau))\ge h_i(\mathbf{q}_0)-\lVert J_i(\mathbf{q}_0)\mathbf{u}^{\star}\rVert\tau-\frac{1}{2}C_i\tau^2 on [0,τtarget][0,\tau_{\mathrm{target}}] using the Taylor expansion (13) and the acceleration bound (17). Solving the quadratic yields the result. □\square

Speed-bound correction: The second-order horizon (17) yields generally longer hold-times than the first-order horizon (12). An illustration of the differences between the first-order and second-order horizons, along with their workspace configurations, can be seen in Figure 3.

Workspace and configuration-space view of the adaptive horizon bounds

Figure 3. Workspace and configuration-space view of the adaptive horizon bounds at q0\mathbf{q}_0. (a) The initial robot configuration, the first-order and second-order certified endpoints qLip\mathbf{q}_{\mathrm{Lip}} and qKin\mathbf{q}_{\mathrm{Kin}}, and the exact contact configuration qFK\mathbf{q}_{\mathrm{FK}} in the workspace. (b) A configuration-space view of (a), with regions showing potential hold times along other directions.

However, the second-order horizon can grow loose because the assumed linear speed grows linearly without bound. Meanwhile, the linear velocity p˙i=∑jωj×rij\dot{\mathbf{p}}_i=\sum_j\omega_j\times r_{ij} also admits a uniform bound Vi:=∑j=1miℓjΩj≥∥p˙s(τ)∥V_i:=\sum_{j=1}^{m_i}\ell_j\Omega_j\ge\lVert\dot{\mathbf{p}}_s(\tau)\rVert. For Ci>0C_i>0, the assumed speed under the acceleration bound (17) exceeds the bound ViV_i at Ti:=Vi−∥Ji(q0)u⋆∥CiT_i:=\frac{V_i-\lVert J_i(\mathbf{q}_0)\mathbf{u}^{\star}\rVert}{C_i}, after a displacement Di:=Vi2−∥Ji(q0)u⋆∥22CiD_i:=\frac{V_i^2-\lVert J_i(\mathbf{q}_0)\mathbf{u}^{\star}\rVert^2}{2C_i}. Thus, when τi,Kin>Ti\tau_{i,\mathrm{Kin}}>T_i, the safe horizon can be corrected with a constant-speed model as:

τi,Kin=min⁡{τtarget,Ti+hi(q0)−DiVi},(18)\tau_{i,\mathrm{Kin}}=\min\left\{\tau_{\mathrm{target}},T_i+\frac{h_i(\mathbf{q}_0)-D_i}{V_i}\right\}, \tag*{(18)}

where division by Vi=0V_i=0 is read as +∞+\infty. When τi,Kin≤Ti\tau_{i,\mathrm{Kin}}\le T_i, we retain the value from (16).

Combined hold time: Since τi,Lip\tau_{i,\mathrm{Lip}} and τi,Kin\tau_{i,\mathrm{Kin}} certify barrier ii, we take the larger per barrier and the smallest over barriers:

τi:=max⁡{τi,Lip,τi,Kin},τhold:=min⁡{τtarget,min⁡i∈Iτi}.(19)\tau_i:=\max\{\tau_{i,\mathrm{Lip}},\tau_{i,\mathrm{Kin}}\},\qquad\tau_{\mathrm{hold}}:=\min\left\{\tau_{\mathrm{target}},\min_{i\in\mathcal{I}}\tau_i\right\}. \tag*{(19)}

By Propositions IV.1 and IV.2, hi(q(τ))≥0h_i(\mathbf{q}(\tau))\ge0 for all i∈Ii\in\mathcal{I} and τ∈[0,τhold]\tau\in[0,\tau_{\mathrm{hold}}].

Self-collisions: To handle self-collisions between pairs (a,b)∈P(a,b)\in\mathcal{P}, we work in the moving frame of the lowest common ancestor (LCA) link of the two spheres. The term ∥p˙i(0)∥\lVert\dot{\mathbf{p}}_i(0)\rVert in (13) is replaced with the initial relative velocity between aa and bb in the LCA frame. The corresponding acceleration bound (17) follows from summing the acceleration bounds of each sphere, computed while treating the LCA as the base link of the manipulator. This construction replicates all our statements for the self-collision barrier.

Bounding the angle between joint axes: Let k<mk<m, and let θkm0:=arccos⁡(zk(0)Tzm(0))\theta_{km}^0:=\arccos(z_k(0)^{\mathsf{T}}z_m(0)). Only joints strictly between kk and mm change the angle between the two axes: proximal (≤k\le k) joints rotate both, whereas distal (≥m\ge m) joints rotate neither. Each intermediate joint rotates zmz_m relative to zkz_k by at most its own rotation angle from the motion u⋆τu^{\star}\tau. So the angle changes by at most δkm:=τtarget∑r=k+1m−1∣ur⋆∣\delta_{km}:=\tau_{\mathrm{target}}\sum_{r=k+1}^{m-1}|u_r^{\star}|. With the lower bound θkm−:=max⁡{0,θkm0−δkm}\theta_{km}^{-}:=\max\{0,\theta_{km}^0-\delta_{km}\} and the upper bound θkm+:=min⁡{π,θkm0+δkm}\theta_{km}^{+}:=\min\{\pi,\theta_{km}^0+\delta_{km}\}, the monotonicity of cosine on [0,π][0,\pi] gives c‾km:=cos⁡θkm+\underline{c}_{km}:=\cos\theta_{km}^{+} and c‾km:=cos⁡θkm−\overline{c}_{km}:=\cos\theta_{km}^{-}. Adjacent axes (m=k+1m=k+1) then keep constant angles directly.

Holonomic Mobile Bases

A holonomic mobile base adds additional degrees of freedom in linear velocity vbase∈R2\mathbf{v}_{\mathrm{base}}\in\mathbb{R}^2 and rotation ωbase∈R\omega_{\mathrm{base}}\in\mathbb{R}. Rotation ωbase\omega_{\mathrm{base}} can be added without any modification by treating it as another revolute joint at the mobile base link.

Translation adds to displacement. For the first-order bound, (10) is modified as γ˙i→γ˙i+∥vbase∥\dot{\gamma}_i\to\dot{\gamma}_i+\lVert\mathbf{v}_{\mathrm{base}}\rVert for environment barriers i∈Si\in\mathcal{S}. For the second-order bound, the initial velocity changes as ∥p˙i(0)∥→∥p˙i(0)+vbase∥\lVert\dot{\mathbf{p}}_i(0)\rVert\to\lVert\dot{\mathbf{p}}_i(0)+\mathbf{v}_{\mathrm{base}}\rVert, and the velocity bound as Vi→Vi+∥vbase∥V_i\to V_i+\lVert\mathbf{v}_{\mathrm{base}}\rVert.

Lipschitz QP Row Reduction

The Lipschitz coefficients used to certify the adaptive horizon also bound the decrease of each barrier over the entire admissible control set. This allows removing CBF constraints that are inactive over admissible velocities:

Theorem IV.3 (Certified CBF constraint removal). Let uˉk:=max⁡{∣umin⁡,k∣,∣umax⁡,k∣}\bar{u}_k := \max\{|u_{\min,k}|, |u_{\max,k}|\} be the maximum joint velocity from (4). For each barrier i∈Ii \in\mathcal{I}, the worst-case decrease rate is

rˉi:=∑k=1nAikuˉk.(20)\bar{r}_i := \sum_{k=1}^{n} A_{ik}\bar{u}_k. \tag*{(20)}

Now, for each barrier i∈Ii \in\mathcal{I}, hi(q)≥rˉiκh_i(\mathbf{q}) \ge\frac{\bar{r}_i}{\kappa} implies ∇hi(q)⊤u≥−κhi(q)\nabla h_i(\mathbf{q})^{\top}\mathbf{u} \ge-\kappa h_i(\mathbf{q}) for all u\mathbf{u} satisfying the limit umin⁡≤u≤umax⁡\mathbf{u}_{\min} \le\mathbf{u} \le\mathbf{u}_{\max}. Consequently, (4) may be solved using only

IQP(q):={i∈I:hi(q)<rˉiκ}.(21)\mathcal{I}_{\mathrm{QP}}(\mathbf{q}) := \left\{i \in\mathcal{I}: h_i(\mathbf{q}) < \frac{\bar{r}_i}{\kappa}\right\}. \tag*{(21)}

Proof. For every admissible u\mathbf{u} of (4), ∣uk∣≤uˉk|u_k| \le\bar{u}_k, and Lipschitz continuity implies:

∇hi(q)⊤u=∑k=1n∂hi∂qk(q)uk≥−∑k=1nAikuˉk,(22)\nabla h_i(\mathbf{q})^{\top}\mathbf{u} = \sum_{k=1}^{n} \frac{\partial h_i}{\partial q_k}(\mathbf{q})u_k \ge-\sum_{k=1}^{n} A_{ik}\bar{u}_k, \tag*{(22)}

where the right-hand side is −rˉi-\bar{r}_i. By precondition, −rˉi≥−κhi(q)-\bar{r}_i \ge-\kappa h_i(\mathbf{q}), so the CBF constraint for ii is satisfied for all u\mathbf{u} satisfying umin⁡≤u≤umax⁡\mathbf{u}_{\min} \le\mathbf{u} \le\mathbf{u}_{\max}. Such constraints therefore do not affect the feasible set or the solution.

For a mobile manipulator, Thm. IV.3 extends similarly by treating ωbase\omega_{\mathrm{base}} as an additional revolute joint, and after, for i∈Si \in\mathcal{S}, rˉi:=vˉbase+∑k=1nAikuˉk\bar{r}_i := \bar{v}_{\mathrm{base}} + \sum_{k=1}^{n} A_{ik}\bar{u}_k. Self-collision barriers are unaffected since base motion preserves pairwise distances.

Unlike fixed distance thresholds or nearest-obstacle heuristics, our screening is certified over the entire admissible control set umin⁡≤u≤umax⁡\mathbf{u}_{\min} \le\mathbf{u} \le\mathbf{u}_{\max}. Removing these constraints not only speeds up the QP solve, but also obviates the evaluation of gradients ∇hi(q0)\nabla h_i(\mathbf{q}_0).

Experiments

We evaluate all planners on 689 UR5 planning problems from MotionBenchMaker [11] across seven cluttered environments: Box, Cage, Small Bookshelf, Tall Bookshelf, Narrow Bookshelf, Tabletop Pick, and Under-Table Pick.

All planners are implemented in C++ using OMPL’s RRT-C implementation. The CBF variants are implemented as a subclass of ompl::base::StateSpace. During each RRT-C extension, this class invokes the CBF filter and records the resulting safe trajectory. The CBF QPs are solved using QPMAD [35] with κ=0.5\kappa= 0.5 with a proportional nominal controller. Fixed-step steering uses an integration step of 1 ms. All methods use a goal tolerance of 0.1 rad and a maximum extension range of 2.0 rad. A 10-second timeout was used for all plans. RRT-C uses collision-checking resolution of 0.01 rad along a straight line. All planners run on a computer with a 24-core Intel Core Ultra 9 285K CPU and 64 GiB of RAM.

We compare our (FLASH-CBF-RRT-C) method against 3 baselines and 3 ablations:

Baselines

  • RRT-C: RRT-C with collision checking.

  • CBF-RRT-C: CBF-RRT-C variant with fixed steps derived from [7].

  • No-QP: Fixed-step accept-or-stop steering derived from [10]. At each integration step, extension applies the nominal control unchanged if it satisfies every CBF constraint and otherwise terminates.

Ablations

  • Fixed: FLASH-CBF-RRT-C without horizon hops. It only uses the redundant QP constraint removal from Thm. IV.3.

  • L1: FLASH-CBF-RRT-C with the first-order horizon.

  • No-QP + L2: The same accept-or-stop controller, as No-QP, utilizing adaptive horizon from Proposition IV.2. Accepted nominal control is held for the second-order certified horizon.

Planner Comparison

Figure 4 and Table 1 compare RRT-C, CBF-RRT-C, and QP-free CBF planners. RRT-C and CBF-RRT-C solve all 689 problems, while No-QP and No-QP+L2 solve 601 and 631, respectively. All CBF-based paths pass collision auditing, whereas one RRT-C path fails. Compared with RRT-C, CBF-RRT-C reduces the median tree size from 20 to 11 vertices and path cost from 13.69 to 9.86 rad, but increases planning time from 17.00 to 17.24 ms. Thus, its sample-efficiency gains are offset by QP overhead, consistent with [10].

Figure 4. Comparison with geometric, CBF-QP, and QP-free planning. Dark-gray, blue, orange, pink, and purple curves denote RRT-C, CBF-RRT-C, No-QP, No-QP + L2, and Ours respectively. Plots show the percentage of problems solved within the planning-time, filter-evaluation, or tree-node budget.

Our method reduces the median planning time to 4.45 ms, corresponding to speedups of 3.82×3.82\times over RRT-C and 3.88×3.88\times over CBF-RRT-C. It requires a median of 298 filter evaluations, compared with 3,597 collision checks for RRT-C and 1,119 filter evaluations for CBF-RRT-C. These represent reductions by factors of 12.07×12.07\times and 3.76×3.76\times, respectively, although collision checks and filter evaluations have different computational costs. The median tree size is 11 vertices, and the median path cost is 10.015 rad, approximately 1.5% higher than CBF-RRT-C and 26.9% lower than RRT-C. The two QP-free variants isolate the benefit of adaptive propagation from that of correcting the nominal control. No-QP has 601 solutions, with medians of 92.69 ms, 90 vertices, and 10,639 evaluations. Adding the second-order time horizon increases the solution count to 631 and reduces the medians to 12.24 ms, 99 vertices, and 682 evaluations. This results in a 7.57× speedup and 15.60× fewer evaluations. Compared with No-QP+L2, our method reports 58 additional solutions and is 2.75× faster in median planning time. It also reduces the median evaluation count by a factor of 2.29× and the median path cost by 44.14%, from 17.93 to 10.01 rad. These results indicate that both adaptive propagation and CBF steering improve planning performance in a complementary manner. The certified horizons reduce reevaluation frequency, while the CBF-QP enables sustained progress around obstacles, leading to longer extensions and improving sample efficiency and progress toward the goal.

Adaptive-Horizon Ablation

Figure 5 compares the fixed-timestep, first-order, and second-order horizon variants of FLASH-CBF-RRT with identical constraint screening. This ablation shows how much each horizon bound affects planning. All three report successful solutions for all 689 problems, with a median of 11 vertices for all planners. The first-order horizon reduces the median filter evaluation count from 1,119 to 529 and the median planning time from 16.69 to 7.45 ms, corresponding to 2.12× fewer evaluations and a 2.24× speedup. The second-order horizon further reduces the evaluation count to 298 and planning time to 4.45 ms, yielding an additional 1.78× reduction in evaluations and 1.68× speedup. The second-order variant is 3.75× faster than the fixed step size variant in runtime, with 3.76× fewer CBF-QP solves. While the second-order variant reduces the CBF-QP solve count relative to the first-order variant by 43.67%, it reduces the runtime by 40.27%. This gap is due to the overhead of second-order certification.

Figure 5. Adaptive-step ablation on the UR5 dataset. Green, turquoise, and purple denote Fixed, L1, and Ours, respectively.

Real-Robot Planning Experiments

We evaluate FLASH-CBF-RRT on a 17-DoF Pollen Reachy2 robot in a tabletop environment with obstacles as shown in Figure 6. The SDF of the environment is estimated using Kernel-SDF [6] from RGB-D images from an Orbbec Gemini 336 RGB-D camera onboard the robot. All planning iterations were run onboard a SolidRun Bedrock V3000 industrial PC. Using the same map and the same start configuration, we drew 100 random valid goal configurations around the table. A ROS2 [36] interface was implemented to query the SDF, and to control the robot.

Reachy2 in the tabletop scene

Figure 6. Reachy2 in the tabletop scene. Ghost poses show configurations along one planned 17-DoF motion, where the mobile base approaches the table, and both arms reach between the obstacles.

MethodSolvedTime (ms)NodesEval.Motion (rad)QP size
Ours6894.451129810.01277.2
Baselines
RRT-C68917.0020359713.69–
CBF-RRT-C [7]68917.341111199.86342.0
No-QP [10]60192.69901063917.66–
Ablations
Fixed68916.691111199.86266.2
L16897.45115299.87275.7
No-QP + L263112.249968217.93–

Table I. Results across 689 scenarios. Solved: number of scenarios solved. The reported statistics are medians. Eval.: number of collision checks (RRT-C), integration steps for No-QP, or CBF-QP iterations. Nodes: number of vertices in the tree; Motion: path length in radians; QP size: number of constraints per CBF-QP solve. CBF variants all return collision-free paths, while one collision was found among RRT-C. Best values are boldfaced.

Table II reports median performance over 100 mobile-manipulator plans using identical instances and seeds. Both methods solve all goals, but FLASH-CBF-RRT is 2.22× faster, uses 4.15× fewer geometric evaluations, and produces 15.1% less arm motion, consistent with the UR5 results.

MethodSolvedTime (ms)NodesEval.Path Length (m)Motion (rad)
Ours10012.2110206.51.6481.675
RRT-C10027.0513856.51.6231.973

Table II. Median performance over 100 randomized onboard planning queries on a real Reachy2 mobile manipulator. Solved: no. of problems solved. Time: planning time. Eval: no. of collision checks or CBF-QP solves. Path length: base motion length. Motion: arm motion length. Goals are sampled over a mapped table, with base poses placed on a surrounding ring. Planning is performed in the full 17-DoF configuration space using a live SDF map.

Conclusion

We presented FLASH-CBF-RRT, a sampling-based planner that achieves fast, continuously safe motion planning by using a differentiable SDF representation in two complementary manners. It biases tree search toward free space using CBF-QP as a local collision-avoidance controller. The resulting control action is applied with a certified adaptive step size determined using two novel closed-form bounds derived from the robot kinematics and the SDF clearance information. This allows FLASH-CBF-RRT to make long excursions in open spaces and short steps near obstacles. The bounds also allow removing provably inactive CBF constraints to reduce the QP size for faster computation. Experiments show that FLASH-CBF-RRT achieves both sample-efficient exploration and lower overall planning time, in both simulation and hardware. For future work, we plan to extend the adaptive horizon computation to time-varying fields and to explore asymptotically optimal planning.

References

References

  1. [1]S. M. LaValle, “Rapidly-exploring random trees: A new tool for path planning,” Department of Computer Science, Iowa State University, Tech. Rep. TR 98-11, 1998.
  2. [2]S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimal motion planning,” The Int. J. of Robotics Research, vol. 30, no. 7, pp. 846–894, 2011.arxiv.org/abs/1105.1186
  3. [3]J. J. Kuffner and S. M. LaValle, “RRT-Connect: An efficient approach to single-query path planning,” in IEEE Int. Conf. on Robotics and Automation (ICRA), vol. 2, 2000, pp. 995–1001.DOI
  4. [4]L. E. Kavraki, P. Svestka, J.-C. Latombe, and M. H. Overmars, “Probabilistic roadmaps for path planning in high-dimensional configuration spaces,” Trans. on Robotics and Automation, vol. 12, no. 4, pp. 566–580, 1996.DOI
  5. [5]A. D. Ames, X. Xu, J. W. Grizzle, and P. Tabuada, “Control barrier function based quadratic programs for safety critical systems,” Trans. on Automatic Control, vol. 62, no. 8, pp. 3861–3876, 2017.DOI
  6. [6]Z. Dai, T. Fan, M. Amani, J. Seo, K. M. Lee, H. Oh, and N. Atanasov, “Kernel-SDF: An open-source library for real-time signed distance function estimation using kernel regression,” IEEE Robotics and Automation Lett., vol. 11, no. 10, pp. 11 054–11 061, 2026.
  7. [7]G. Yang, B. Vang, Z. Serlin, C. Belta, and R. Tron, “Sampling-based motion planning via control barrier functions,” in Int. Conf. on Automation, Control and Robots (ICACR), 2019, pp. 22–29.DOI
  8. [8]A. Ahmad, C. Belta, and R. Tron, “Adaptive sampling-based motion planning with control barrier functions,” in IEEE Conf. on Decision and Control (CDC), 2022, pp. 4513–4518.arxiv.org/abs/2206.00795
  9. [9]M. Yu, C. Yu, M.-M. Naddaf-Sh, D. Upadhyay, S. Gao, and C. Fan, “Efficient motion planning for manipulators with control barrier function-induced neural controller,” in IEEE Int. Conf. on Robotics and Automation (ICRA), 2024, pp. 14 348–14 355.arxiv.org/abs/2404.01184
  10. [10]G. Yang, M. Cai, A. Ahmad, A. Prorok, R. Tron, and C. Belta, “LQR-CBF-RRT*: Safe and optimal motion planning,” in American Control Conf. (ACC), 2025, pp. 3700–3705.arxiv.org/abs/2304.00790
  11. [11]C. Chamzas, C. Quentíin-Peña, Z. Kingston, A. Orthey, D. Rakita, M. Gleicher, M. Toussaint, and L. E. Kavraki, “Motionbenchmaker: A tool to generate and benchmark motion planning datasets,” IEEE Robotics and Automation Lett., vol. 7, no. 2, pp. 882–889, 2022.DOI
  12. [12]I. A. Şucan, M. Moll, and L. E. Kavraki, “The Open Motion Planning Library,” vol. 19, no. 4, pp. 72–82, December 2012.DOI
  13. [13]W. Thomason, Z. Kingston, and L. E. Kavraki, “Motions in microseconds via vectorized sampling-based planning,” in IEEE Int. Conf. on Robotics and Automation (ICRA), 2024, pp. 8749–8756.arxiv.org/abs/2309.14545
  14. [14]B. Sundaralingam, S. K. S. Hari, A. Fishman, C. Garrett, K. Van Wyk, V. Blukis, A. Millane, H. Oleynikova, A. Handa, F. Ramos, N. Ratliff, and D. Fox, “CuRobo: Parallelized collision-free robot motion generation,” in IEEE Int. Conf. on Robotics and Automation (ICRA), 2023, pp. 8112–8119.DOI
  15. [15]C. W. Ramsey, Z. Kingston, W. Thomason, and L. E. Kavraki, “Collision-affording point trees: SIMD-amenable nearest neighbors for fast collision checking,” in Robotics: Science and Systems (RSS), 2024.DOI
  16. [16]C. Holleman and L. Kavraki, “A framework for using the workspace medial axis in prm planners,” in IEEE Int. Conf. on Robotics and Automation (ICRA), vol. 2, 2000, pp. 1408–1413 vol.2.DOI
  17. [17]N. M. Amato, O. B. Bayazit, L. K. Dale, C. V. Jones, and D. Vallejo, “OBPRM: an obstacle-based PRM for 3D workspaces,” in Proc. of Workshop on Algorithmic Foundations of Robotics (WAFR’98), March 1998, pp. 155–168.
  18. [18]H. Oleynikova, Z. Taylor, M. Fehr, R. Siegwart, and J. Nieto, “Voxblox: Incremental 3D Euclidean signed distance fields for onboard MAV planning,” in IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS), Sep. 2017, pp. 1366–1373.DOI
  19. [19]A. H. Qureshi and Y. Ayaz, “Potential functions based sampling heuristic for optimal path planning,” Autonomous Robots, vol. 40, no. 6, pp. 1079–1093, 2016.DOI
  20. [20]S. Choudhury, J. D. Gammell, T. D. Barfoot, S. S. Srinivasa, and S. Scherer, “Regionally accelerated batch informed trees (RABIT*): A framework to integrate local information into optimal path planning,” in IEEE Int. Conf. on Robotics and Automation (ICRA), 2016, pp. 4207–4214.DOI
  21. [21]D. Kim, Y. Kwon, and S.-E. Yoon, “Dancing prm*: Simultaneous planning of sampling and optimization with configuration free space approximation,” in IEEE Int. Conf. on Robotics and Automation (ICRA), 2018, pp. 7071–7078.DOI
  22. [22]H.-T. L. Chang, J. Hsu, M. Fiser, L. Tapia, and A. Faust, “RL-RRT: Kinodynamic motion planning via learning reachability estimators from RL policies,” IEEE Robotics and Automation Lett., vol. 4, no. 4, pp. 4298–4305, 2019.
  23. [23]F. Schwarzer, M. Saha, and J.-C. Latombe, “Adaptive dynamic collision checking for single and multiple articulated robots in complex environments,” Trans. on Robotics, vol. 21, no. 3, pp. 338–353, 2005.DOI
  24. [24]J. Bialkowski, M. Otte, S. Karaman, and E. Frazzoli, “Efficient collision checking in sampling-based motion planning via safety certificates,” The Int. J. of Robotics Research, vol. 35, no. 7, pp. 767–796, 2016.DOI
  25. [25]A. Ademovic and B. Lacevic, “Path planning for robotic manipulators using expanded bubbles of free C-space,” in IEEE Int. Conf. on Robotics and Automation (ICRA), 2016, pp. 77–82.DOI
  26. [26]K. M. B. Lee, Z. Dai, C. L. Gentil, L. Wu, N. Atanassov, and T. Vidal-Calleja, “Safe bubble cover for motion planning on distance fields,” arXiv preprint arXiv:2408.13377, 2024.arxiv.org/abs/2408.13377
  27. [27]O. Arslan, V. Pacelli, and D. E. Koditschek, “Sensory steering for sampling-based motion planning,” in IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS), 2017, pp. 3708–3715.DOI
  28. [28]J. Huh, O. Arslan, and D. D. Lee, “Probabilistically safe corridors to guide sampling-based motion planning,” in International Symposium on Robotics Research (ISRR), 2022, pp. 311–327.
  29. [29]P. Werner, R. Cheng, T. Stewart, R. Tedrake, and D. Rus, “Super-fast configuration-space convex set computation on GPUs for online motion planning,” in Robotics: Science and Systems (RSS), 2025.
  30. [30]J. Breeden, K. Garg, and D. Panagou, “Control barrier functions in sampled-data systems,” IEEE Control Systems Lett., vol. 6, pp. 367–372, 2022.DOI
  31. [31]X. Tan, E. Daş, A. D. Ames, and J. W. Burdick, “Zero-order control barrier functions for sampled-data systems with state and input depen- dent safety constraints,” in American Control Conf. (ACC), 2025, pp. 283–290.
  32. [32]G. Yang, C. Belta, and R. Tron, “Self-triggered control for safety critical systems using control barrier functions,” in American Control Conf. (ACC), 2019, pp. 4454–4459.arxiv.org/abs/1903.03692
  33. [33]S. Coumar, G. Chang, N. Kodkani, and Z. Kingston, “Foam: A tool for spherical approximation of robot geometry,” arXiv preprint arXiv:2503.13704, 2025.arxiv.org/abs/2503.13704
  34. [34]G. Fiorillo and L. Béthune, “1-lipschitz neural distance fields,” Computer Graphics Forum, vol. 43, no. 5, p. e15128, 2024.
  35. [35]A. Sherikov, “QPMAD: Eigen-based C++ quadratic programming solver,” https://github.com/asherikov/qpmad, Software.
  36. [36]S. Macenski, T. Foote, B. Gerkey, C. Lalancette, and W. Woodall, “Robot operating system 2: Design, architecture, and uses in the wild,” Science Robotics, vol. 7, no. 66, p. eabm6074, 2022.DOI

Paper details

vlnc:W_n7huqaqf
Version
v1 · latest
Published
Fresh — added to the Hub within the last 30 days
Added
License
CC BY 4.0
Usage

views · 9

downloads · 0

A view counts once per browser per day, a download each time the PDF is requested.

  • Computer Science

Not recorded for this paper

View statement
Looking for related papers…
Contents