ParaMaP: Parallel Mapping and Collision-free Motion Planning for Reactive Robot Manipulation

TL;DR

ParaMaP addresses real‑time, collision‑free motion planning for manipulation in unknown environments by tightly integrating GPU‑accelerated environment mapping with a sampling‑based MPC planner. The method introduces a gather‑then‑transform Euclidean Distance Field and a robot‑masked occupancy update to prevent self‑collision artifacts, plus a Lie‑algebra based pose error on $SE(3)$ within SMPC for geometrically consistent optimization. All components run on the GPU to enable high‑frequency replanning, and the approach is validated in extensive simulations and real‑world 7‑DoF experiments showing robust performance with dynamic obstacles. The work offers a practical, scalable solution for reactive manipulation in cluttered and unstructured settings.

Abstract

Real-time and collision-free motion planning remains challenging for robotic manipulation in unknown environments due to continuous perception updates and the need for frequent online replanning. To address these challenges, we propose a parallel mapping and motion planning framework that tightly integrates Euclidean Distance Transform (EDT)-based environment representation with a sampling-based model predictive control (SMPC) planner. On the mapping side, a dense distance-field-based representation is constructed using a GPU-based EDT and augmented with a robot-masked update mechanism to prevent false self-collision detections during online perception. On the planning side, motion generation is formulated as a stochastic optimization problem with a unified objective function and efficiently solved by evaluating large batches of candidate rollouts in parallel within a SMPC framework, in which a geometrically consistent pose tracking metric defined on SE(3) is incorporated to ensure fast and accurate convergence to the target pose. The entire mapping and planning pipeline is implemented on the GPU to support high-frequency replanning. The effectiveness of the proposed framework is validated through extensive simulations and real-world experiments on a 7-DoF robotic manipulator. More details are available at: https://zxw610.github.io/ParaMaP.

Figures (9)

• Figure 1: Illustration of the voxel projection method for occupancy updating. (a) Camera-based projection of 3D voxels onto the image plane. (b) LiDAR-based projection of 3D voxels and point clouds onto spherical coordinates.
• Figure 2: Robot geometry representations. (a) The original mesh model of the robot rendered in RViz. (b) The simplified collision model constructed by approximating each link with a set of $n_s$ spheres.
• Figure 3: Performance with respect to voxel size on the Flat Dataset. (a) Total mapping time, consisting of OGM and the EDT. (b) Average updating time of OGM and EDT. The EDT is computed within a fixed local volume of $8\,\text{m} \times 8\,\text{m} \times 3\,\text{m}$ centered on the robot.
• Figure 4: Performance with respect to voxel size on the Dynablox Dataset. (a) Total mapping time (OGM + EDT). (b) Average updating time of OGM and EDT. The EDT is computed in a fixed $20 \times 20 \times 6\,\text{m}^3$ local volume.
• Figure 5: Benchmark motion-planning scenario in the Gazebo simulation.