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

TL;DR

This paper introduces BoundPlanner, a convex-set based Cartesian path planner, and BoundMPC, an online MPC that follows the planner's bounded path while accounting for the robot's kinematics. BoundPlanner builds a graph of collision-free convex sets in Cartesian space to generate a bounded, collision-free reference path, which BoundMPC then uses to compute a feasible joint trajectory with allowed path deviations. The approach includes a novel, obstacle-count-independent convex-set collision-avoidance formulation for the entire manipulator and uses MVIE, convex hull sets, and a graph-based path search with iterative refinement. Experiments on a 7-DoF manipulator demonstrate fast planning times and robust online replanning in constrained environments, outperforming several state-of-the-art baselines in both planning speed and safety. Overall, the work enables responsive, reliable manipulation in unstructured settings and lays groundwork for integration with higher-level planning frameworks.

Abstract

Online trajectory planning enables robot manipulators to react quickly to changing environments or tasks. Many robot trajectory planners exist for known environments but are often too slow for online computations. Current methods in online trajectory planning do not find suitable trajectories in challenging scenarios that respect the limits of the robot and account for collisions. This work proposes a trajectory planning framework consisting of the novel Cartesian path planner based on convex sets, called BoundPlanner, and the online trajectory planner BoundMPC. BoundPlanner explores and maps the collision-free space using convex sets to compute a reference path with bounds. BoundMPC is extended in this work to handle convex sets for path deviations, which allows the robot to optimally follow the path within the bounds while accounting for the robot's kinematics. Collisions of the robot's kinematic chain are considered by a novel convex-set-based collision avoidance formulation independent on the number of obstacles. Simulations and experiments with a 7-DoF manipulator show the performance of the proposed planner compared to state-of-the-art methods. The source code is available at github.com/Thieso/BoundPlanner and videos of the experiments can be found at www.acin.tuwien.ac.at/42d4

Figures (6)

• Figure 1: Schematic of the path planning using BoundPlanner to plan a Cartesian path $\pi(\phi)$ from the start point $\boldsymbol{p} _{0}$ to the end point $\boldsymbol{p} _{\mathrm{f}}$. (a) The graph $\mathcal{G}$ is built using the convex sets $\mathcal{S}_{0}, \mathcal{S}_{1}, \mathcal{S}_{2}, \mathcal{S}_\mathrm{f}$ around $\boldsymbol{p} _{0}$, $\boldsymbol{p} _{\mathrm{f}}$, and the green sample points, further visualized in (b). (c) A sequence of convex sets $\mathcal{S}_{\mathrm{path}, 0}, \ldots, \mathcal{S}_{\mathrm{path}, 2}$ is found using $\mathcal{G}$, connecting the starting set $\mathcal{S}_{0}$ with the ending set $\mathcal{S}_{\mathrm{f}}$. A path $\pi(\phi)$ is constructed inside these sets connecting $\boldsymbol{p} _{0}$ with $\boldsymbol{p} _{\mathrm{f}}$. (d) BoundMPC oelerichBoundMPCCartesianTrajectory2024 uses the path $\pi(\phi)$ of BoundPlanner to find a suitable joint trajectory $\mathcal{T}(t)$ such that the end-effector stays within the convex sets, thus avoiding the obstacles $\mathcal{O}_{\mathrm{1}}, \ldots, \mathcal{O}_{\mathrm{4}}$.
• Figure 2: Graph building procedure of BoundPlanner. The set $\mathcal{S}_{1}$ intersects with the starting set $\mathcal{S}_{0}$ and the final set $\mathcal{S}_{\mathrm{f}}$. In order to compute the costs $c_{\mathrm{0, 1, f}}$ from \ref{['eq:graph_cost']} for the edge between the intersection sets $\mathcal{S}_{1, 0}$ and $\mathcal{S}_{\mathrm{1, f}}$, the projections $\boldsymbol{p} _{1, 0}$ and $\boldsymbol{p} _{\mathrm{1, f}}$ are computed using \ref{['eq:projection']}.
• Figure 3: Schematic drawing of the replanning procedure where the replanning happens in $\mathcal{S}_{\mathrm{cur}}$. The replanning set $\mathcal{S}_{\mathrm{replan}}$ is computed to include $\boldsymbol{p} _{\mathrm{horizon}, 1}$ and $\boldsymbol{p} _{\mathrm{horizon, max}}$.
• Figure 4: Open-box scenario: The robot has to turn around and reach in the box with its end-effector. The convex obstacles $\mathcal{O}_{k}$, $k = 1, \ldots, 5$, are depicted in red and the convex planning sets $\mathcal{S}_{\mathrm{path}, i}$, $i = 0, \ldots, 2$, in blue. The end-effector trajectories $\boldsymbol{p} _{\mathrm{tcp}}(t)$ of different methods as well as the reference path $\boldsymbol{\pi} _{\mathrm{p}}(\phi)$ of BoundPlanner are shown.
• Figure 5: Open-box scenario: Collision avoidance sets for the point $\boldsymbol{p} _{\mathrm{c}, 5}$ on the robot' s kinematic chain. The set $\mathcal{S}_{\mathrm{c}, 5}$ for the initial time $t_{0}$ and the final time $t_{\mathrm{f}}$ is shown in blue and at intermediate time steps in grey. The grey lines illustrate the evolution of $\mathcal{S}_{\mathrm{c}, 5}$ over time with increasing line width.

Theorems & Definitions (3)

• Remark
• Remark
• Remark