Reactive Motion Generation With Particle-Based Perception in Dynamic Environments

Xiyuan Zhao; Huijun Li; Lifeng Zhu; Zhikai Wei; Xianyi Zhu; Aiguo Song

Fig. 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.

Fig. 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.

Fig. 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.

Fig. 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.

Fig. 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).

Fig. 6: The real-world and simulation environments for mapping performance comparison. (a) The labeled object is moving in the fixed FOV. We consider the synchronized velocity from the NOKOV motion capture system as ground truth. (b) For the simulated occupancy estimation scenario, multiple models are placed on cabinet or the table. The manipulator moves along a predefined trajectory between the initial position and target configuration back and forth. Since the mapping performance is compared here, we ensure that there is no collision between obstacles and the manipulator.

Fig. 7: The real-world velocity estimation curves of the object on conveyor belt. The orange and blue shadows indicate the standard deviation of estimation results from DSP map and G-DSP map, respectively. From about

t=3.4

s to

t=4.6

s, the parcel is pulled by human partner.

Metrics	Case
Method	Constant-Velocity		Random-Velocity
	RMSE $\downarrow$	Standard Dev. $\downarrow$	RMSE $\downarrow$	Standard Dev. $\downarrow$
Munkres-Diff	0.142	—	0.167	—
DSP map	0.079	0.107	0.110	0.109
G-DSP map	0.035	0.082	0.075	0.083

TABLE I: Velocity Estimation Comparison in Manipulation Tasks With Voxel Size Of 0.05 m.

Metrics	Voxel Size [m]
Map	0.02		0.03		0.05
	AUC $\uparrow$	F1 $\uparrow$	AUC $\uparrow$	F1 $\uparrow$	AUC $\uparrow$	F1 $\uparrow$
Ewok map [ 45 ]	0.845	0.915	0.848	0.918	0.894	0.943
K3DOM map [ 16 ]	0.242	0.357	0.307	0.426	0.411	0.550
DSP map [ 15 ]	0.757	0.882	0.909	0.927	0.913	0.924
G-DSP map (Ours)	0.910	0.924	0.943	0.958	0.951	0.964

TABLE II: Occupancy Estimation Comparison in Manipulation Tasks. Averaged AUC and best F1 score are reported.

Fig. 8: Perception result comparison with a fixed camera when the voxel resolution is

0.02

m at three time steps. To maintain the dynamic characteristics for manipulation, we only map obstacles in current FOV. The colors of point cloud and voxels indicate their

z

-axis height in the world frame

\mathcal{F}_{w}

. Meanwhile, the FOV and camera frame

\mathcal{F}_{c}

are also visualized. Three translucent purple models represent current real positions of three dynamic objects. The red dashed ellipses show trail noise for probabilistic inertia, while red dashed boxes suggest small objects missing of K3DOM. The red solid ellipses in DSP map show lags resulting from relatively long map latency. (a) Simulation scenario. (b) Raw point cloud. (c) Ewok [ 45 ] . (d) K3DOM [ 16 ] . (e) DSP map [ 15 ] . (f) Ours.

Fig. 9: Computation time for four perception methods using different voxel sizes in dynamic environment of Fig. 6 (b). The averaged step time is reported.

Obs.	Obs.	Success Rate $\uparrow$			Round-Trip Time [s]			Path Length [rad]			MPC Frequency [Hz] $\uparrow$
Size	Max Vel	STORM	DS-MPPI	Ours	STORM	DS-MPPI	Ours	STORM	DS-MPPI	Ours	STORM	DS-MPPI	Ours
	0.0 m/s	1.00	1.00	1.00	10.59 (0.86)	4.97 (0.17)	11.75 (1.02)	10.75 (0.99)	11.66 (0.26)	12.16 (0.50)	44.0	9.9	43.8
2	0.1 m/s	1.00	0.99	1.00	11.49 (2.02)	6.25 (1.05)	11.85 (1.11)	12.43 (1.18)	13.86 (2.05)	12.17 (0.63)	43.1	9.9	42.4
	0.2 m/s	0.88	0.96	0.99	11.53 (2.00)	5.67 (1.13)	12.45 (2.01)	12.40 (1.50)	12.39 (1.60)	12.32 (0.96)	40.5	9.8	41.0
	0.0 m/s	1.00	1.00	1.00	12.76 (1.21)	5.31 (0.24)	14.28 (1.11)	12.25 (0.30)	11.77 (0.60)	12.44 (0.27)	29.4	9.7	29.8
4	0.1 m/s	0.93	1.00	1.00	13.50 (1.78)	5.54 (1.74)	14.89 (2.10)	12.89 (1.26)	11.84 (0.93)	12.80 (0.97)	28.8	9.9	29.8

TABLE III: Planning Method comparison of three methods in known static and dynamic environments. Mean (Standard Deviation)

Fig. 10: The planning benchmark in dynamic obstacle avoidance. Two-, four-, and six-spheres size cross-shaped obstacle in comparison. Start, intermediate, and goal joint configuration of robot from left to right in goal reaching task. Different obstacle velocities (from 0.0 to 0.2 m/s) are set to evaluate reactive planning performance.

Fig. 11: Comparison of success rate, trajectory execution time and path length for sensor-based planning in simulated unstructured dynamic environments. Cases with time exceeding 14 s or path length exceeding 18 rad are clipped.

Fig. 12: Sensor-based planning comparison with two cameras when the maximum velocity is

0.1

m/s and

0.2

m/s in scenario 1. The top two lines show the planning procedure of baseline, while the bottom two lines indicate snapshots of ours. (a)-(c):

\mathbf{v}_{\mathrm{m}}=0.1

m/s. Although the baseline framework perceives the moving obstacle and generate an avoidance trajectory, it fails to anticipate the obstacle’s motion and consequently results in collision. (d)-(f):

\mathbf{v}_{\mathrm{m}}=0.2

m/s. The raycast-based map suffers from probabilistic inertia for fast-moving obstacles, making robot failed to perceive the approach of the T-shaped obstacle in time (see red dashed ellipses of Line 2). By explicitly incorporating dynamic characteristics, SMART consistently achieves collision-free motion.

Fig. 13: Sensor-based planning in scenario 2 with multiple obstacles. (a)-(c): Under baseline method, the robot could not respond sufficiently fast to moving multi-parcel, leading to near-collision contacts, and collides with a dynamic bunny model approaching from above (red solid ellipses of Line 1). Red dashed ellipses indicate delayed mapping using raycast-based perception. (d)-(f): In contrast, SMART consistently successfully avoids both dynamic obstacles.

Fig. 14: Sensor-based planning in real-world tasks. The six-DOF robot reactively avoids parcels that are transported on conveyor belt using SMART. (a)-(c): The robot avoids small parcel and continues to the target position. (d)-(f): More complex-shaped multi-parcel moves at 0.1 m/s, which is perceived by sensors and avoided by robot. The onboard camera can be used for grasping when reaching target positions in future applications.

Fig. 15: The experiment of pHRI. A six-DOF robot reactively avoids the human using SMART in the collaborative workspace. Robot avoids the raised arm to reach the target configuration.

Reactive Motion Generation With Particle-Based Perception in Dynamic Environments

Abstract

Figures & tables

Explore similar work

Clustering-Embedded Model Predictive Path Integral Control: Avoiding Averaging-Induced Failure and Enabling Efficient Cluster Selection for Dynamic Obstacles

RAY-TOLD: Ray-Based Latent Dynamics for Dense Dynamic Obstacle Avoidance with TDMPC

Real-Time Motion Planning with Dynamic Hazards: Classical vs. Learning-Based Methods