Many-RRT*: Robust Joint-Space Trajectory Planning for Serial Manipulators

TL;DR

Many-RRT*: an extension of RRT*-Connect that plans to multiple goals in parallel that maintains robust convergence and asymptotic optimality is proposed.

Abstract

The rapid advancement of high degree-of-freedom (DoF) serial manipulators necessitates the use of swift, sampling-based motion planners for high-dimensional spaces. While sampling-based planners like the Rapidly-Exploring Random Tree (RRT) are widely used, planning in the manipulator's joint space presents significant challenges due to non-invertible forward kinematics. A single task-space end-effector pose can correspond to multiple configuration-space states, creating a multi-arm bandit problem for the planner. In complex environments, simply choosing the wrong joint space goal can result in suboptimal trajectories or even failure to find a viable plan. To address this planning problem, we propose Many-RRT*: an extension of RRT*-Connect that plans to multiple goals in parallel. By generating multiple IK solutions and growing independent trees from these goal configurations simultaneously alongside a single start tree, Many-RRT* ensures that computational effort is not wasted on suboptimal IK solutions. This approach maintains robust convergence and asymptotic optimality. Experimental evaluations across robot morphologies and diverse obstacle environments demonstrate that Many-RRT* provides higher quality trajectories (44.5% lower cost in the same runtime) with a significantly higher success rate (100% vs. the next best of 1.6%) than previous RRT iterations without compromising on runtime performance.

Figures (5)

• Figure 1: The trees created by RRT$^\star$-Connect (top) vs. Many-RRT$^\star$ (bottom) in the task-space (left) and joint-space (right). Despite having denser, more extensive trees, RRT$^\star$-Connect chooses a suboptimal goal joint state from the preimage of the robot's forward kinematics. Since our method samples multiple goal configurations, we can solve for many possible plans in parallel, improving success rate and decreasing path costs.
• Figure 2: A 2DoF Arm in an environment with obstacles that fully split the configuration space. A naive seed for Equation (\ref{['eq:ik-opt']}) would produce Goal 2, which is impossible to reach from the initial configuration. Sampling the optimal goal configuration is necessary for sampling-based planners to converge quickly. The configuration space $\mathcal{C}_\text{free}$ is represented in green.
• Figure 3: Many-RRT$^\star$ solves many bidirectional RRTs in parallel. (a) We sample many points from the preimage of $g = f(q)$, which is a continuous manifold for redundant manipulators. (b) Asynchronously and in parallel, we execute a separate RRT$^\star$ thread for each configuration sampled in (a). These configurations are the roots of these RRTs, and the start position is the goal. (c) We separately execute a RRT$^\star$ algorithm rooted at the start configuration that tries to connect to members of the trees in (b). This process is asynchronous, keeping track of the best solution yet given the state at each iteration.
• Figure 4: The median cost per iteration across trials for the Random environment with $500$ trials per method. Not only does Many-RRT$^\star$ converge to a feasible path before baselines, but it also finds lower-cost trajectories as the number of iterations increases. The performance difference widens as the dimension of the configuration-space increases: for the 7-DoF manipulator, Many-RRT$^\star$ finds a feasible solution in $6$ times fewer iterations.
• Figure 5: The collision representations of the environments used in our simulated experiments. From left to right, the environments increase in relative difficulty. In Table, the manipulator must move from one pose to another while avoiding the flat table in the center of the frame. In Wall, the manipulator must move from one side to another of the wall while avoiding both the table and the wall. Likewise, in Passage, the manipulator must move from one side to the other by navigating through the passage. In this environment, the strengths of bi-directional sampling-based planners becomes more evident, as methods like RRT$^\star$-Connect are able to explore both areas to attempt to connect the joint-space through the narrow passage. In the most challenging environment, Random, a random configuration of spherical objects are scattered throughout the environment. Across environments, we ensure that both the goal and start positions are collision-free and that a feasible motion plan exists before experimentation.