Motion Planning

Sensor-Based Task-Constrained Motion Planning using Model Predictive Control

MPC-TCMP algorithm

Consider a robotic system that must perform a task, e.g., move its end-effector along an assigned trajectory. Assume that the workspace is populated by moving obstacles whose geometry and motion is not known in advance: there is however a sensory system that provides a current view of the obstacles surrounding the robot. The considered problem consists in using the available sensory information in order to generate real-time motion commands that allow the robot to execute the assigned task while avoiding all collisions. This can be regarded as a sensor-based version of the problem called Task-Constrained Motion Planning with Moving Obstacles (TCMP_MO), in turn an extension of the basic TCMP problem.

To solve the above problem, we propose in [1] a real-time planner based on model predictive control (MPC). In our approach, MPC is used to compute short-term joint velocities that guarantee task execution while optimizing an objective function that combines two integral terms computed along the planning horizon: the first is aimed at keeping the robot away from the obstacles, while the second penalizes control effort. In particular, the first term is computed on the basis of the closest obstacle points O_1,...,O_p to a set of p control points distributed along the robot structure. Constraints are added so as to guarantee the kinematic feasibility of the generated motion. As customary in MPC, only the first control action is actually sent out for execution, and a new optimization problem will be set up and solved at the next time instant.

The distances d_1,...,d_p computed by the sensory system are also used to perform an emergency stop of the robot if any of then goes below a certain threshold indicating an imminent risk of collision.


Below we report some simulation and experimental results obtained by applying the proposed approach to a UR10 manipulator coexisting with moving humans. Two Kinect sensors are used to monitor the robot workspace and to compute in real time human-robot distances while discriminating actual collisions from false positives due to camera occlusions; to this end, we use an extension of the parallel algorithm presented in [2].


We simulated the proposed sensor-based planner in C++ as an add-on for V-REP, a software development kit for robotic simulations. We have assigned an orientation task to the end-effector of the robot; in particular, the robot must keep its terminal tool vertical. We use two control points along the robot structure: one placed at the elbow and one at the wrist. This is an appropriate choice for the mechanical structure of the UR10. The sample rate of the robot controller is 8 msec. The planner requires only 3 msec to compute velocity inputs over a planning horizon equal to 10 control intervals, while the time needed to run the collision detection module is about 2,5 msec. The algorithm runs therefore in real-time with an adequate margin with respect to the controller sampling time of 8 msec. The clip below shows a simulation where a human acts as a moving obstacle.


The experiment shown in the following clip replicates the simulation scenario and confirms the reactivity and effectiveness of the proposed approach. 


[1] M. Cefalo, E. Magrini, G. Oriolo, Sensor-Based Task-Constrained Motion Planning using Model Predictive Control. Submitted to 12th IFAC Symposium on Robot Control (SYROCO 2018) [pdf].

[2] M. Cefalo, E. Magrini, G. Oriolo, Parallel Collision Check for Sensor Based Real-Time Motion Planning. 2017 IEEE International Conference on Robotics and Automation (ICRA 2017) [pdf].

DIAG Robotics Lab