Proactive Motion Planning for Human-Robot Cooperation

Proactive Motion Planning for Human-Robot Cooperation Elena Basei2,3, Edoardo Lamon2,3, Matteo Saveriano1,3, Daniele Fontanelli1,3, Luigi Palopoli2,3 Abstract—This abstract addresses the incorporation of human motion prediction into proactive and dynamic human-aware motion planning, with the goal of enabling safe collaboration between humans and robots. A deep learning, graph-based model is used to forecast human motion and is integrated into a planning framework. This framework employs a static roadmap along with a time-variant A* algorithm to modify the trajectory of a UR5e manipulator. This method greatly improves humanrobot interaction and enables proactive collision avoidance by combining precise motion forecasts with adaptive trajectory planning. Index Terms—Human-Robot Cooperation (HRC), Human Motion Prediction (HMP), Human-Aware Motion Planning I. INTRODUCTION Human-aware motion planning (HAMP) addresses the challenge of generating robot trajectories in shared workspaces while ensuring safety, efficiency, and explainability. Early solutions mainly relied on deliberative approaches [1], where robots computed complete paths in advance using graphbased or optimisation techniques. While effective in structured environments, these methods struggle in human-robot cooperation due to the dynamic nature of human presence, which can constantly alter the workspace. This often renders precomputed plans invalid and necessitates costly replanning. In response, reactive planning [2] emerged as an alternative by utilising real-time sensory feedback to adapt trajectories. Although this approach enhances safety, it treats humans as unpredictable obstacles, resulting in abrupt stops, jerky motions, and significant reductions in productivity. Proactive planning [3], [4] addresses these limitations by considering both current and predicted future human movements. By anticipating intent, robots can move more fluidly and safely, improving efficiency and operator comfort. Our work integrates motion forecasting into a human-aware planning framework, using a deep learning graph-based method to forecast human motion in real time and generate optimal, collision-free plans for robots. The contributions are the following: 1) a real-time proactive planning method that utilises predicted human motion in conjunction with a time-variant A* algorithm; 2) a parallelised version of the A* algorithm, optimised for real-time pathfinding in complex environments. We acknowledge the support of the MUR PNRR project FAIR - Future AI Research (PE00000013), the project INVERSE (Grant Agreement n. 101136067), the project Magician (Grant Agreement n. 101120731) and the contribution of VRT foundation. 1Department of Industrial Engineering, Universit` a di Trento, Trento. 2Department of Information Engineering and Computer Science, Universit` a di Trento, Trento, Italy. 3Interdepartmental Robotics Labs (IDRA), University of Trento, Trento, Italy {name.surname}@unitn.it The data collection and experiments were conducted with the approval of the ethics committee at the Authors’ Institution under application N.2025-003. II. METHODOLOGY The proposed architecture for proactive motion planning includes three components: (i) online prediction of future human movements, (ii) an algorithm for generating collisionfree robot trajectories, and (iii) a monitoring strategy that identifies potential collisions and replans as necessary. This work will focus on the last two components. A. Parallelised Time-Varying A* Our approach was inspired by the T-PRM framework by H¨ uppi et al. [4] for planar holonomic robots. We extended the algorithm to address dynamic obstacles and to operate in real-time. First, we construct a graph G= (V, E)from deterministic samples in 3D space based on end-effector positions, avoiding static obstacles. To optimise computation time, we reduce the configuration space from 6 DoFs to 3 by fixing the end-effector orientation. In the querying phase, we implement a modified A* algorithm that calculates the shortest, time-sensitive, collision-free paths. We introduce the arrival time map (AT(v)), and the joint configuration map (Cfg(v)) for each node. As we expand the nodes, a closedform inverse kinematics (IK) solver is used to produce potential robot configurations, prioritising configurations based on distance from the previous one. Valid configurations must adhere to distance constraints and avoid collisions with the human position and predicted motion at the arrival time, in line with ISO/TS 15066. Invalid configurations are discarded. Once we identify a valid successor, we update AT and record the configuration in Cfg. After reaching the goal, we reconstruct the trajectory, resulting in a time-parametrised, collision-free motion plan πT(t)via polynomial spline interpolation. To enhance computation efficiency, we developed a multithreaded A* algorithm. Instead of sequentially processing a queue of open nodes, our method makes use of Nthreads that extract and evaluate nodes in a cycle while guaranteeing secure updates to shared resources. B. Plan Handling and Replan Strategy Although the multithreaded A* technique increases efficiency, it requires a significant amount of computing time, often more than inter-frame time. Moreover, unpredictable human movements can cause predictions to be inaccurate, leading to unrealistic plans or longer computation times. Lower speeds are also required by safety standards near critical limits. To address these issues, we use a state machine for execution oversight and parallel plan generation. When a new data item arrives at time Ti, we start two processes: one generates a backup plan πTi(t)and the other creates an emergency plan ˆπTi(t)for quick recovery from unexpected system halts. To generate πTi(t), we run the 2025 I-RIM Conference October 17-19, Rome, Italy ISBN: 9788894580570 10.5281/zenodo.17629744 117 Fig. 1. Test environment with Universal Robots UR5e and ZED2 camera. modified A* algorithm using the HMP hTi(t)with a deadline Dfor plan delivery. If the deadline is missed, no plan is produced. If met, the plan is valid and can be used starting from time Ti+D. The process begins with start position s=F K (πcurr(Ti+D)), with πcurr(t)denoting the current plan in execution. Since Dexceeds the inter-frame time F, multiple instances (up to D F) can process different data items simultaneously. Once a plan πTi(t)is successfully generated, it is stored for future use. If the robot needs to initiate motion, the starting position sis set to the robot’s current position. The emergency plan ˆπTi(t)is generated using the modified A* while keeping the human’s position fixed and stopping once a thread reaches a position at time F+D. This approach reduces computation time, allowing us to generate ˆπTi(t) within 0.01 seconds. The F+Dtiming horizon ensures there is enough time for a new proactive plan to be developed. During execution, when new data arrives at time Ti, a decision process evaluates if the current plan πcurr(t)is still applicable based on the updated positions of the human. The process can switch between two states: Nominal Execution and Collision Avoidance. In the Nominal Execution state, the robot adjusts its speed based on its distance to the human, following ISO/TS 15066 guidelines. If the robot violates the stop distance or a collision is predicted at Ti, it transitions to Collision Avoidance, where for violations predicted after Ti+D, it schedules a transition to πTi(t)at Ti+D. For violations before Ti+D, the robot immediately stops and sets the current plan to the emergency plan ˆπTi(t). This replanning approach ensures safe cooperation by allowing the robot to adapt its trajectory based on human actions. III. RESULTS AND DISCUSSION We conducted a preliminary 5-minute evaluation of a cooperative assembly task using a Universal Robots UR5e and a ZED2 RGB-D camera to track human skeletons at 30 Hz. Ten subjects (4 females, 6 males, aged 23-31) with limited experience in robot manipulators performed the assembly task while the robot executed pick-and-place operations in the same area. Participants received blocks for assembly without specific instructions to test our replanning approach, and were advised to minimise movements in front of the robot to avoid triggering safety protocols. The setup included four drop TABLE I COMPARISON OF PROACTIVE AND REACTIVE PLANNING. Method Completed Tasks Avg. Planning Time (s) Avg. Stop & Re-plan Avg. Online Re-plan Reactive 12.7 0.18 0.41 0.49 Proactive 15.5 0.21 0.20 2.66 stations and a conveyor belt pickup station (Fig. 1). We used the Graph-Mixer [5] network to predict human motion for the next second without fine-tuning it on our data, reflecting the typical constraints of small-medium enterprises where quick reconfiguration is often necessary. General applicability without task-specific fine-tuning is crucial in such scenarios. We compared our proactive HAMP methods against a baseline provided by a reactive method, inspired by [2], which only considers the current position of the human for replanning. Table I displays the number of tasks completed, the average planning time, and the average number of stops and re-planning instances for each task. The initial results suggest that the proactive method outperforms the reactive approach. Specifically, the proactive approach showed a 22% increase in task completion on average. Additionally, the average planning times for both methods appeared similar. Proactive methods also tended to reduce the number of stopand-replan events by 53%. However, the proactive approach resulted in a higher online re-plan count, as it continuously updates the plan in the background, reflecting its anticipatory nature. Conversely, the reactive method exhibited non-zero re-plan counts (0.49), as it allowed for transitions between feasible plans whenever potential future collisions with the current human configuration were identified. To obtain a preliminary indication of the relevance of these findings, we conducted a statistical analysis on the differences between the methods using either Wilcoxon signed-rank tests or repeated measures ANOVA, as appropriate, with a significance threshold at p < 0.05. These analyses suggested significant performance differences among the methods. Notably, there were differences in the completed tasks, with the proactive method tending to outperform the reactive baseline. Additionally, the mean online replanning test showed a highly significant result (p= 1.15 ×10−7), supporting the potential of the proactive approach. While these results are promising, further investigation is needed to draw definitive conclusions. REFERENCES [1] S. M. LaValle, Planning algorithms. Cambridge university press, 2006. [2] K. Merckaert, B. Convens, M. M. Nicotra, and B. Vanderborght, “Real-time constraint-based planning and control of robotic manipulators for safe human–robot collaboration,” Robotics and Computer-Integrated Manufacturing, vol. 87, p. 102711, 2024. [Online]. Available: https: //www.sciencedirect.com/science/article/pii/S0736584523001862 [3] W. Liu, K. Eltouny, S. Tian, X. Liang, and M. Zheng, “Integrating uncertainty-aware human motion prediction into graph-based manipulator motion planning,” IEEE/ASME Transactions on Mechatronics, 2024. [4] M. H¨ uppi, L. Bartolomei, R. Mascaro, and M. Chli, “T-prm: Temporal probabilistic roadmap for path planning in dynamic environments,” in 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2022, pp. 10 320–10 327. [5] S. Yang, H. Li, C.-M. Pun, C. Du, and H. Gao, “Adaptive spatial-temporal graph-mixer for human motion prediction,” IEEE Signal Processing Letters, vol. 31, pp. 1244–1248, 2024. 118