BoundPlanner: A convex-set-based approach to bounded manipulator trajectory planning

1. Problem Statement

Modern robot manipulators operating in unstructured and dynamic environments (e.g., household tasks) require online trajectory planning capabilities to react quickly to changing obstacles and tasks.

• The Challenge: Existing online planners often fail in challenging scenarios where they must respect strict kinematic limits and avoid collisions simultaneously.
• Limitations of Current Methods:
  • Offline planners are too slow for dynamic environments.
  • Sampling-based methods (e.g., RRT) can get stuck in local minima or fail to find feasible paths in tight spaces.
  • Potential field methods do not guarantee collision avoidance and scale poorly with the number of obstacles.
  • Joint-space convex sets are computationally expensive to compute because mapping task-space obstacles to joint space is difficult.
  • Strict path following (following a Cartesian path exactly) often leads to infeasible trajectories due to kinematic constraints.

The core problem is to develop a framework that allows for fast, reactive online replanning while ensuring kinematic feasibility and collision avoidance for the entire robot chain, not just the end-effector.

2. Methodology

The proposed framework consists of two tightly integrated components: BoundPlanner (a Cartesian path planner) and BoundMPC (an online trajectory planner).

A. BoundPlanner (Cartesian Path Planner)

BoundPlanner computes a reference path $\pi(\phi)$ with associated collision-free bounds in Cartesian space.

• Collision-Free Convex Sets: Instead of modeling obstacles directly, the algorithm explores the free space by computing convex sets (polytopes defined by half-spaces).
  • Generation Methods: It uses Maximum Volume Inscribed Ellipsoid (MVIE) optimization (iteratively expanding ellipsoids and their bounding convex sets) and Convex Hull Sets (creating sets around specific points while separating them from obstacles).
• Graph of Convex Sets (GCS): A graph $G$ is constructed where vertices are intersection sets of convex sets, and edges represent connectivity.
  • Cost Function: Edge costs penalize distance and set transitions but favor larger convex sets. This is crucial for manipulators, as larger sets allow the trajectory planner more freedom to deviate from the reference path to satisfy kinematic constraints.
• Path Generation:
  • A shortest path is found through the graph using Dijkstra's algorithm.
  • A piecewise linear position path and a geodesic orientation path are generated through the selected sequence of convex sets.
  • Refinement: The path is iteratively refined by adding new convex sets around projection points until convergence.
• Handling End-Effector Extent: The planner ensures that the entire convex hull of the end-effector (not just a single point) remains within the computed convex sets.

B. BoundMPC (Online Trajectory Planner)

BoundMPC is a Model Predictive Control (MPC) framework that computes the optimal joint-space trajectory online.

• Convex Sets as Bounds: Unlike previous versions that used polynomial bounds, this work uses the convex sets generated by BoundPlanner as hard constraints. The end-effector position $p_{tcp}(t)$ must satisfy $A_{path,i} p_{tcp}(t) \leq b_{path,i}$ within specific time intervals.
• Horizon Splitting: To handle the discontinuity of switching between different convex sets, the MPC horizon is split at specific indices ($m_s$) where the trajectory transitions from one set to the next. This ensures continuous optimization.
• Full-Body Collision Avoidance: A novel formulation ensures collision avoidance for the entire kinematic chain, not just the end-effector.
  • Key points along the robot's links are monitored.
  • For each point, a collision-free convex set is computed based on the current position and the predicted position at the end of the horizon.
  • This formulation is independent of the number of obstacles, scaling well to dense environments (e.g., point clouds).
• Replanning Strategy: When a new goal is detected or the environment changes:
  1. The current state is projected onto the existing path.
  2. A new "replanning set" is constructed connecting the current position to the furthest reachable point in the current horizon.
  3. BoundPlanner rapidly computes a new reference path from this set to the new goal, and BoundMPC executes the new trajectory.

3. Key Contributions

1. BoundPlanner: A novel, fast Cartesian path planner that explores collision-free space using convex sets. It specifically favors large sets to accommodate the kinematic constraints of manipulators, enabling significant path deviations.
2. Extended BoundMPC: An adaptation of the MPC framework to utilize Cartesian convex sets as path deviation bounds via horizon splitting, allowing the robot to follow a reference path while respecting kinematics.
3. Novel Full-Body Collision Avoidance: A convex-set-based formulation for the entire robot kinematic chain that is independent of the obstacle count and considers the prediction horizon, preventing the robot from passing through thin obstacles.
4. Reactive Online Replanning: The combination allows for rapid replanning in dynamic environments without getting stuck in local minima.

4. Experimental Results

The framework was tested on a 7-DoF KUKA LBR iiwa 14 R820 robot in two scenarios:

• Scenario 1: Reach into a Box (Constrained Environment)

  • Task: The robot must reach into a box with an obstacle above it.
  • Comparison: BoundPlanner + BoundMPC was compared against Graph-of-Convex-Sets (GCS), VP-STO, and RRT+MPC.
  • Results:
    • Planning Time: BoundPlanner was significantly faster (0.11s vs. 233s for GCS and 9.3s for VP-STO), enabling true online replanning.
    • Success Rate: Achieved 0 failures across 8 different box placements. RRT+MPC failed 4 times (stuck in local minima), and GCS failed once due to connectivity issues.
    • Trajectory Quality: Produced shorter Cartesian trajectories compared to GCS.

• Scenario 2: Grasp from Shelf with Replanning (Dynamic Environment)

  • Task: The robot grasps an object whose position changes mid-motion, requiring replanning.
  • Comparison: BoundPlanner vs. RRT+MPC.
  • Results:
    • Replanning Speed: BoundPlanner averaged 0.1s per replanning event, whereas RRT+MPC averaged 0.54s (max 2.06s).
    • Safety: RRT+MPC resulted in 3 collisions in 20 trials because it prioritized minimizing tracking error over collision avoidance. BoundPlanner achieved 0 collisions because the convex set constraints strictly enforce safety.

5. Significance

This work bridges the gap between global path planning and local trajectory execution for complex manipulators.

• Scalability: By using convex sets in Cartesian space, the method scales efficiently to environments with many obstacles (e.g., point clouds), unlike joint-space methods.
• Safety & Feasibility: The separation of path planning (global guidance) and trajectory execution (local kinematic optimization) allows the robot to deviate from the reference path to satisfy kinematic limits without violating collision constraints.
• Real-Time Applicability: The extremely low planning times (<0.2s) make this framework suitable for high-frequency online control in dynamic, unstructured environments, a critical requirement for next-generation service and industrial robots.
• Robustness: The method avoids local minima common in potential field methods and RRT, ensuring reliable task completion in cluttered spaces.