Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

Safe Planning for Articulated Robots Using Reachability-based Obstacle Avoidance With Spheres

Jonathan Michaux, Adam Li, Qingyi Chen, Che Chen, Bohao Zhang, Ram Vasudevan

TL;DR

SPARROWS presents a real-time, certifiably safe motion-planning framework for articulated robots in cluttered environments by overapproximating the robot's reachability with a novel Spherical Forward Occupancy ($\mathcal{SFO}$) built from Polynomial Zonotopes and enforcing safety via an exact signed distance to 3D zonotopes. The method combines forward-kinematics-based $\mathbf{FK}$ reachability, sphere-based obstacle primitives, and a receding-horizon optimization to produce collision-free trajectories efficiently. Its three main contributions are the $\mathcal{SFO}$ representation, an exact $s_d$ computation for zonotopes, and empirical demonstrations that SPARROWS outperforms state-of-the-art baselines (e.g., ARMTD) in dense clutter while maintaining safety. The work suggests practical impact for real-time, model-based robotics and points to future work integrating neural scene representations and uncertainty handling. All mathematical constructs are used to provide rigorous safety guarantees within a tractable optimization framework.

Abstract

Generating safe motion plans in real-time is necessary for the wide-scale deployment of robots in unstructured and human-centric environments. These motion plans must be safe to ensure humans are not harmed and nearby objects are not damaged. However, they must also be generated in real-time to ensure the robot can quickly adapt to changes in the environment. Many trajectory optimization methods introduce heuristics that trade-off safety and real-time performance, which can lead to potentially unsafe plans. This paper addresses this challenge by proposing Safe Planning for Articulated Robots Using Reachability-based Obstacle Avoidance With Spheres (SPARROWS). SPARROWS is a receding-horizon trajectory planner that utilizes the combination of a novel reachable set representation and an exact signed distance function to generate provably-safe motion plans. At runtime, SPARROWS uses parameterized trajectories to compute reachable sets composed entirely of spheres that overapproximate the swept volume of the robot's motion. SPARROWS then performs trajectory optimization to select a safe trajectory that is guaranteed to be collision-free. We demonstrate that SPARROWS' novel reachable set is significantly less conservative than previous approaches. We also demonstrate that SPARROWS outperforms a variety of state-of-the-art methods in solving challenging motion planning tasks in cluttered environments. Code, data, and video demonstrations can be found at \url{https://roahmlab.github.io/sparrows/}.

Safe Planning for Articulated Robots Using Reachability-based Obstacle Avoidance With Spheres

TL;DR

SPARROWS presents a real-time, certifiably safe motion-planning framework for articulated robots in cluttered environments by overapproximating the robot's reachability with a novel Spherical Forward Occupancy () built from Polynomial Zonotopes and enforcing safety via an exact signed distance to 3D zonotopes. The method combines forward-kinematics-based reachability, sphere-based obstacle primitives, and a receding-horizon optimization to produce collision-free trajectories efficiently. Its three main contributions are the representation, an exact computation for zonotopes, and empirical demonstrations that SPARROWS outperforms state-of-the-art baselines (e.g., ARMTD) in dense clutter while maintaining safety. The work suggests practical impact for real-time, model-based robotics and points to future work integrating neural scene representations and uncertainty handling. All mathematical constructs are used to provide rigorous safety guarantees within a tractable optimization framework.

Abstract

Generating safe motion plans in real-time is necessary for the wide-scale deployment of robots in unstructured and human-centric environments. These motion plans must be safe to ensure humans are not harmed and nearby objects are not damaged. However, they must also be generated in real-time to ensure the robot can quickly adapt to changes in the environment. Many trajectory optimization methods introduce heuristics that trade-off safety and real-time performance, which can lead to potentially unsafe plans. This paper addresses this challenge by proposing Safe Planning for Articulated Robots Using Reachability-based Obstacle Avoidance With Spheres (SPARROWS). SPARROWS is a receding-horizon trajectory planner that utilizes the combination of a novel reachable set representation and an exact signed distance function to generate provably-safe motion plans. At runtime, SPARROWS uses parameterized trajectories to compute reachable sets composed entirely of spheres that overapproximate the swept volume of the robot's motion. SPARROWS then performs trajectory optimization to select a safe trajectory that is guaranteed to be collision-free. We demonstrate that SPARROWS' novel reachable set is significantly less conservative than previous approaches. We also demonstrate that SPARROWS outperforms a variety of state-of-the-art methods in solving challenging motion planning tasks in cluttered environments. Code, data, and video demonstrations can be found at \url{https://roahmlab.github.io/sparrows/}.
Paper Structure (42 sections, 5 theorems, 56 equations, 7 figures, 15 tables, 3 algorithms)

This paper contains 42 sections, 5 theorems, 56 equations, 7 figures, 15 tables, 3 algorithms.

Table of Contents

  1. Introduction
  2. Related Work
  3. Contributions
  4. preliminaries
  5. Notation
  6. Polynomial Zonotopes
  7. Polynomial Zonotope Operations
  8. Interval Conversion
  9. Slicing a PZ
  10. Bounding a PZ
  11. Distance Functions and Projections onto Sets
  12. Arm and Environment Modeling
  13. Environment
  14. Arm Kinematics
  15. Arm Occupancy
  16. ...and 27 more sections

Key Result

Lemma 7

The parameterized trajectory polynomial zonotopes $\mathbf{ q_j }(\mathbf{T_i};\mathbf{K})$ are overapproximative, i.e., for each $j \in N_q$ and $k\in \mathbf{K_j},$ One can similarly define $\mathbf{ \dot{q}_j }(\mathbf{T_i};\mathbf{K})$ that are also overapproximative.

Figures (7)

  • Figure 1: This paper presents SPARROWS, a method that is capable of generating safe motion plans in dense and cluttered environments for single- and multi-arm robots. Here, both arms have start and goal configurations shown in blue and green, respectively. Prior to planning, SPARROWS is given full access to polytope overapproximations of obstacles to compute an exact signed distance function of the scene geometry. At runtime, SPARROWS combines the signed distance function with the novel Spherical Forward Occupancy as obstacle-avoidance constraints for SPARROWS to generate safe trajectories between the start and goal in a receding horizon manner. Each trajectory is selected by solving a nonlinear optimization problem such that an overapproximation (purple) of the swept volume of its entire motion remains collision-free. Note in this figure, a small, thin barrier is placed to prevent unwanted collisions between the two arms.
  • Figure 2: A visualization of the robot arm and its environment. The obstacles are shown in red and the robot is shown in grey (translucent). The volume of each joint is overapproximated by a sphere, shown in purple, in the workspace. Each link volume is overapproximated by a tapered capsule formed by the convex hull shown in light purple of two consecutive joint spheres (Assum. \ref{['assum:joint_link_occupancy']}).
  • Figure 3: A visualization of the Spherical Forward Occupancy construction for a robotic arm in 3D. (First column) The planning time horizon is partitioned into a finite set of polynomial zonotopes (Sec. \ref{['subsubsec:pz_time_horizon']}) before computing the parameterized trajectory polynomial zonotopes shown in purple (Sec. \ref{['subsubsec:pz_desired_traj']}). The time horizon consists of a planning phase $[0,t_p)$ and a braking phase $[t_p, t_\text{f}]$. A single desired trajectory for each joint is shown in black. (Second column) The polynomial zonotope forward kinematics algorithm (Sec. \ref{['subsubsec:pzfk']}) computes an overapproximation of the joint positions for each time interval. (Third column) The joint position zonotopes are first overapproximated by axis-aligned boxes and (Fourth column) subsequently overapproximated by the smallest sphere that circumscribes the box (Sec. \ref{['subsubsec:sjo']}). For the sake of clarity, the insets in columns 2 through 4 depict the polynomial zonotope joint positions, overapproximating boxes, and the circumscribed spheres as larger. (Fifth column) The radii of the circumscribed spheres are then added to the radii of the nominal joint spheres (Assum. \ref{['assum:joint_link_occupancy']}) to produce the Spherical Joint Occupancy (Lem. \ref{['lem:sjo']}). (Sixth column) Finally, the Spherical Forward Occupancy is computed for each link corresponding to the desired trajectory. (Seventh column) Note that SPARROWS generates link occupancies that are less conservative than ARMTD.
  • Figure 4: Subset of Hard Scenarios where SPARROWS succeeds. The start, goal, and intermediate poses are shown in blue, green, and grey (transparent), respectively. Obstacles are shown in red (transparent). The tasks include reaching from one side of a wall to another, between two small bins, from below to above a shelf, around one vertical post, and between two horizontal posts.
  • Figure 5: A comparison of SPARROWS' and ARMTD's reachable sets. (Top row) Both SPARROWS and ARMTD can generate trajectories that allow the arm to fit through a narrow passage when the maximum acceleration is limited to $\frac{\pi}{24}$ rad/s$^2$. (Bottom row) However, when the maximum acceleration is $\frac{\pi}{6}$ rad/s$^2$ ARMTD's reachable set collides with the obstacles while SPARROWS' remains collision-free.
  • ...and 2 more figures

Theorems & Definitions (10)

  • Definition 1
  • Definition 2
  • Definition 6: Trajectory Parameters
  • Lemma 7: Parmaeterized Trajectory PZs
  • Lemma 8: PZ Forward Kinematics
  • Lemma 9: Spherical Joint Occupancy
  • Theorem 10: Spherical Forward Occupancy
  • Lemma 11
  • proof
  • proof