Reactive Motion Generation With Particle-Based Perception in Dynamic Environments

TL;DR

The paper addresses reactive motion generation for high-DOF manipulators operating in dynamic, unstructured environments. It introduces SMART, a framework that tightly couples a global, tensorized particle-based perception module (G-DSP map) with a dynamic obstacle-aware MPPI planner (D-STORM) to propagate robot-obstacle dynamics under uncertainty. Key contributions include the G-DSP map with explicit obstacle velocities and covariance, batch tensor-based mapping for real-time updates, and the D-STORM planner that jointly propagates robot and obstacle dynamics within an MPC-based stochastic optimization. Empirical results from simulations and real UR5 experiments show improved safety, reactivity, and robustness to dynamic obstacles compared with state-of-the-art perception-planning baselines. The framework has practical impact for real-time manipulation in uncertain environments and pHRI, with potential extensions to active viewpoint planning and multi-view sensing.

Abstract

Reactive motion generation in dynamic and unstructured scenarios is typically subject to essentially static perception and system dynamics. Reliably modeling dynamic obstacles and optimizing collision-free trajectories under perceptive and control uncertainty are challenging. This article focuses on revealing tight connection between reactive planning and dynamic mapping for manipulators from a model-based perspective. To enable efficient particle-based perception with expressively dynamic property, we present a tensorized particle weight update scheme that explicitly maintains obstacle velocities and covariance meanwhile. Building upon this dynamic representation, we propose an obstacle-aware MPPI-based planning formulation that jointly propagates robot-obstacle dynamics, allowing future system motion to be predicted and evaluated under uncertainty. The model predictive method is shown to significantly improve safety and reactivity with dynamic surroundings. By applying our complete framework in simulated and noisy real-world environments, we demonstrate that explicit modeling of robot-obstacle dynamics consistently enhances performance over state-of-the-art MPPI-based perception-planning baselines avoiding multiple static and dynamic obstacles.

Figures (15)

• Figure 1: The pipeline of our proposed sensor-based planning framework SMART, including the realtime perception (dynamic occupancy mapping), dynamics propagation and reactive motion generation components.
• Figure 2: Visualization of G-DSP map. Since realtime planning for serial rigid robots involves the whole body, we consider the fixed base as the origin of world frame $O$. Thus, our map is global in the limited operation space. (a) Two obstacles of arbitrary shape are represented by point objects (blue solid circle). The manipulator starts from the blue initial configuration and must reach the green goal position while avoiding static and dynamic obstacles. (b) A fixed sensor setup. The obstacles are approximated by weighted particles (hollow circles) $\tilde{\bm{x}}_{t}$ with estimated positions and velocities. The mapping space are divided into two subspaces, i.e. voxel subspaces $\mathbb{V}$ and pyramid subspaces $\mathbb{P}$. (c) The dual-view setup with a fixed and an onboard camera for arm manipulation. The observations $\bm{z}_{t}$ from onboard camera are point clouds, which divides the pyramid space $\mathbb{P}$ into visible and occluded spaces. In narrow cases, a depth sensor fixed on $\mathbf{x}_C^1$ and an onboard camera mounted on robot $\mathbf{x}_C^2$ broadens perception for robots. (d) Another unstructured setup is to use two fixed cameras at $\mathbf{x}_C^1$, $\mathbf{x}_C^2$ to mitigate occlusion and realize whole-body collision avoidance. Our parallel global map works with common multi-view manipulation setups.
• Figure 3: Trilevel model of the G-DSP map. The mapping process of particle-based maps can be divided into three levels, including object (cluster) level, subspace level and particle level. The object level utilizes the finite difference of cluster center positions at time step $t + 1$ and $t$ to estimate rough velocities. The pyramid and voxel subspaces play the significant role in particle weight update and occupancy estimation. The core of G-DSP map is the stochastic dynamics embedded to particle representation, which is realized by parallel tensor operation. For planning, voxels with velocity and uncertainty estimation are considered as obstacle primitives.
• Figure 4: Illustration of the system dynamics propagation in D-STORM. The robot considers uncertainty by sampling $K$ control sequences with importance weighting, while obstacle voxels explicitly propagate their uncertainties by covariance. In D-STORM, the horizon of obstacles are aligned with the robot by time step $\mathbf{dt}$, which is used to rollout in $H$. Then the immediate cost can be calculated by spatial positions of robot-obstacle dynamics in the horizon.
• Figure 5: The motions generated from the baseline (top) and SMART (bottom) in the task space. (a)-(c) show cases of robot planning with an onboard sensor, while (d)-(e) visualize a case with a fixed camera. In (a), a 3-DOF manipulator can avoid the static obstacle using both baseline and SMART. However, for fast-moving obstacles in (b) and (d), STORM controller with raycast-based perception loses to capture their dynamic characteristics, leading to a local minima and collision. SMART leverages more complete dynamics estimation and prediction, realizing repulsive reaction to dynamic environments. (c), (d) and (e) details the map and top ten control trajectories in MPC controller at a certain moment. A 3-DOF planar robot (blue) traverses the task space avoiding the red obstacle from initial configuration (gray) to the target configuration (green), modeling the surroundings with voxels (light red grids).