Dynamic Global/Local multi-layer motion planner architecture for autonomous Robots

This paper presents a novel dynamic motion planner designed to provide safe motions in the context of the Smart Autonomous Robot Assistant Surgeon (SARAS) surgical platform. SARAS is a multi-robot autonomous platform designed to execute auxiliary tasks in Minimally Invasive Surgeries (MIS) with a high degree of autonomy. The development of robotic systems with a high level of autonomy and reliability requires to perceive the workspace and human actions, to contextualize them with the surgical workflow, and, finally, plan and dynamically control the required motions. The autonomous control relies on a multi-level hierarchical Finite State Machine (hFSM) that decides and supervises all robot actions and their transitions. This approach requires multi-granularity decomposition of the surgical procedure and defines different motion profiles to preserve and safely interacts with the patients' anatomy. The motion planner is developed under the minimally invasive surgery context since it is an extreme use case where the environment is complex, dynamic and unstructured. Moreover, in the SARAS platform the autonomous robots share workspace as well as collaborate with other human-guided robotic instruments. This creates an even more complex working environment and defines a set of hierarchical relationships in which auxiliary instruments have a lower priority. The presented motion planner acts at two levels: Global and Local. The Global Planner generates an initial spline-based trajectory that, defined by a set of Control Points, follows a certain profile determined by the ongoing surgical action and the interaction with the patient's anatomy. Then, during the execution of the motion, the Local Planner observes the workspace (anatomy and other tools) and applies different virtual potential fields to the control points to dynamically modify their position to avoid potential collisions or tool blocking while maintaining trajectory coherence. At this level, it reactively modifies the trajectory between the tool position and the next control point applying Dynamical Systems based obstacle avoidance. This approach ensures collision free connections between the spline control points. The proposed motion planner is validated in a realistic surgical scenario. The experimental results are analysed from data collected during various Robotic- Assisted Radical Prostatectomy surgeries on manikins, performed with the SARAS SOLO-SURGERY platform: the main surgeon teleoperates a daVinci Research Kit and two robotic arms autonomously perform different auxiliary surgical tasks.