NMPC-MP: Real-time Nonlinear Model Predictive Control for Safe Motion Planning in Manipulator Teleoperation

Motion control and planning for the manipulator are critical components in manipulator teleoperation. Online (real-time) motion control is challenging for active obstacle avoidance and often results in fluctuating and unsafe motion. Offline motion planning, on the other hand, generates precise and secure trajectories for complex manipulation. In this paper, a real-time nonlinear model predictive control based motion planner (NMPC-MP) is designed for teleoperated manipulation. In contrast to traditional NMPC-based approaches, our model considers a complex environment with dynamic obstacles. Our multi-threaded NMPC-MP allows for real-time planning, including dynamic objects. We evaluate our approach both in a simulated environment and with real-world experiments using the Kinova (R) Movo platform. The comparison to state-of-the-art approaches (e.g., RRT-Connect, CHOMP, and STOMP) shows a significant improvement in real-time motion planning using NMPC-MP. In real-world tests, the proposed planner was applied on a human-shaped dual manipulator setup. Our results show that the NMPC-MP runs in real-time and generates smooth and reliable trajectories. The experiments validate that the planner is able to precisely track active goals from the teleoperator while avoiding self-collision and obstacles.