Abstract

A variety of motion planning algorithms has been developed over the past decades. For robotics, the transformation of the problem from the robot Cartesian space (workspace) to the configuration space (C-space) has been crucial - converting the problem of collision-checking between 3D objects to simpler, point-like, tests in the high-dimensional C-space. However, the C-space map-making is both computationally and memory-intensive and the complexity grows with the C-space dimension. With time-varying environments and many robot DoFs, this soon becomes intractable, as newly appearing and possibly moving obstacles need to be remapped online into the high-dimensional C-space. Therefore, we present a fast heuristic planning method designed for a humanoid robot that employs the sampling-based RRT* algorithm directly in the Cartesian space and in a hierarchical fashion: first, a collision-free path is planned for the end-effector; second, corresponding collision-free points for every via-point are searched for the robot elbow. The resulting path consists of straight-line segments and is only approximate, but the details - a kinematically feasible smooth trajectory for the robot - is offloaded to an online Cartesian solver and controller that is available on our platform. The results demonstrate, first, that our solution delivers real-time performance (plans faster than 1s on a standard PC) in the vast majority of cases in a significantly cluttered environment. Second, the results are suggestive of the fact that asymptotic optimality of the plans is preserved even for the additional control points. Third, a test of state-of-the-art algorithms on the same scenario shows that solutions cannot be found in reasonable time (less than 10s).

Results

Run-time batch experiment

Summary of a batch experiment with 1000 trials

In this experiment, 1000 trials were conducted successively one-by-one with new randomly generated obstacle positions. In these experiments, we fixed the time any of these modular planners can take to 0.1s. However, the number of calls to the individual planners is not known in advance and depends on the environment: the number of via points is determined dynamically by the planner. Furthermore, if the local planners for forearm midpoint or the final elbow collision checking fails, replanning from the top-level is triggered. Thus, the total computation time is the sum of all the individual planners’ execution.

The bar graph summarizes the results of the batch experiment in terms of this total planning time. It shows that 73.50% of trials complete with computation time of less than half of second, and 18.80% trials finish with from 0.5s to 1s of computation time. The percentage for computation time form 1s to 2s is 5.5%. In summary, a majority of trials (97.8%) can finish within 2s, which is suitable for our setting. The fact that all percentages sum up to 100% confirms that a solution was found in all cases.

Asymptotic optimality experiments

Cost of End-effector control point as a function of the defined planning time of modular planners (horizontal axis). The vertical bars show the standard deviation over 50 trial while the stars indicate the mean values of motion cost.
Cost of Elbow control point as a function of the defined planning time of modular planners (horizontal axis). The vertical bars show the standard deviation over 50 trial while the stars indicate the mean values of motion cost.

For this experiment, the obstacles are randomly created, but their positions kept unchanged from then on. Here, the timeout for every individual planner is systematically varied: after every 50 trials, the planning time is increased by 0.2 s. After each trial, the planning results, namely total computation time and cost of motion plan are stored. The experiments are finished when the simulator completes all 50 trials at the setting of 1.5s timeout for each modular planner. The results in figure indicate that the motion costs reduce when the modular planning time is increased, for both of 2 representative points of the forearm (end-effector and elbow). Please note that only the end-effector uses a unique global planner (RRT*) while the elbow path (calculated through the forearm midpoint) is a result of application of local planners to the end-effector via points. Yet, the results suggest that the asymptotic optimality of the original RRT* algorithm is transferred to the additional control points. A formal proof will, however, remain the subject of future work

Citation

@INPROCEEDINGS{7803377, 
author={P. D. H. Nguyen and M. Hoffmann and U. Pattacini and G. Metta}, 
booktitle={2016 IEEE-RAS 16th International Conference on Humanoid Robots (Humanoids)}, 
title={A fast heuristic Cartesian space motion planning algorithm for many-DoF robotic manipulators in dynamic environments}, 
year={2016}, 
pages={884-891}, 
keywords={end effectors;humanoid robots;manipulator kinematics;path planning;sampling methods;time-varying systems;C-space map-making;collision-checking;collision-free path planning;configuration space;end-effector;heuristic Cartesian space motion planning algorithm;high-dimensional C-space;humanoid robot;kinematically feasible smooth trajectory;many-DoF robotic manipulators;online Cartesian solver;robot Cartesian space;sampling-based RRT* algorithm;time-varying environments;Aerospace electronics;Collision avoidance;Elbow;Kinematics;Manipulators;Planning}, 
doi={10.1109/HUMANOIDS.2016.7803377}, 
month={Nov},}

Source code

Open-source code of the research is published via github