IROS2019国际学术会议论文集 2523

Abstract Recent studies on concentric tube robots CTRs have shown that they are well suited for minimally invasive endoscopic surgeries However typical surgical procedures require the use of multiple tools simultaneously which has led to the development of dual arm CTRs that are susceptible to self collision In this paper a closed loop control system for dual arm CTRs is proposed to detect and avoid the inter collision between arms along their entire body The collision detection module finds the minimum distance between the manipulators in the Cartesian space To avoid self collision the proposed control system using differential Jacobian based inverse kinematics is developed with three tasks with different priorities physical constraints self collision avoidance and end effector tracking The performance of the proposed control scheme is investigated through implementation for Cartesian control of a dual arm CTR to reach pre defined target points in a simulated scenario similar to epilepsy surgery The self collision detection module successfully predicted all 359 self collision cases in the target region The experimental results demonstrated the efficacy of the controller in handling inter collision between arms over their entire body by keeping the minimum distance between arms at 1 544 mm I INTRODUCTION Minimally invasive surgery MIS has gained popularity due to its clinical benefits including low infection rates less post operative pain faster recovery time and small incision size As a result there has been an exponential increase in the development of dexterous tools for MISs 1 Miniaturized continuum robots such as concentric tube robots CTRs are potentially suitable for an endoscopic minimally invasive approach 2 3 CTRs consist of pre curved telescoping and superelastic nitinol a nickel titanium alloy tubes that are concentrically arranged Each tube can be translated and rotated to create a final robot shape allowing full tip position control Thus each tube has two degrees of freedom DOF using two actuators at its proximal end The small size typically 1 3 mm of CTRs also favors safer minimally invasive interventions at many sites in the human body Epilepsy affects more than 50 million people worldwide with 30 potentially being surgical candidates The This Research supported by the Hospital for Sick Children and the University of Toronto S Sabetian T Looi and J M Drake M D are with the Institute for Biomaterials and Biomedical Engineering University of Toronto 164 College Street Toronto ON M5S 3E2 Canada Phone 416 813 7122 e mail saba sabetian mail utoronto ca S Sabetian T Looi and J M Drake M D are with the Centre for Image Guided Innovation and Therapeutic Intervention in the Hospital for Sick Children 555 University Avenue Toronto ON M5G 1X8 Canada e mail thomas looi sickkids ca james drake sickkids ca E Diller is with the Department of Mechanical and Industrial Engineering University of Toronto 5 King s College Road Toronto ON M5S3G8 Canada e mail ediller mie utoronto ca neurosurgical treatment of an epileptic focus involves large skin incisions extensive bone removal and excision of brain tissue 1 A recent study on epilepsy surgery demonstrated that non linear trajectories of CTRs cover a greater percentage of a target hippocampus than a linear trajectory Thus this study showed that a major open neurosurgical procedure would be technically feasible to perform in a minimally invasive style using dexterous CTRs 4 In general endoscopic surgeries with a large variety of aims ranging from navigation to operation through diagnosis require the use of two tools simultaneously Also redeploying the robot several times to change the end effector of CTRs during the MIS is time consuming These two problems necessitate dual arm CTRs in the surgical development Moreover current path planners for single arm CTRs can generate two non colliding paths for tip positions of a dual arm CTR However the resulting paths do not necessarily guarantee inter collision avoidance along the entire body of CTR manipulators 5 7 This inter collision between arms is referred to as self collision Self collision along the body of arms in dual arm CTRs can potentially occur due to lack of sensors on the body of manipulators and overlapping reachable workspace of two arms Although some groups have shown the ability to create shape sensors using fiber Bragg grating FBG sensors 8 this method does not provide the position of entire body of a CTR in Cartesian space Thus the challenge for the design and development of a control system for dual arm CTR is to detect and avoid inter collision between arms A Multi Arm Standard Robots Multi arm robots have gained popularity in both industry and clinical applications due to their cost and time effective capabilities to perform multiple co operative tasks 12 The focus of the controller of multi arm robots is to develop collision free motions primarily in the context of obstacle avoidance and self collision avoidance Collision avoidance Self Collision Detection and Avoidance for Dual Arm Concentric Tube Robots Saba Sabetian Thomas Looi Member IEEE Eric Diller Member IEEE and James M Drake Figure 1 Dual arm concentric tube robots a self collision case b collision free case Trocar Arm 1 Arm 2 Trocar Arm 2 Arm 1a b IEEE Robotics and Automation Letters RAL paper presented at the 2019 IEEE RSJ International Conference on Intelligent Robots and Systems IROS Macau China November 4 8 2019 Copyright 2019 IEEE is a significant phenomenon since it could result in damaging the robot itself damaging the surrounding environment and loss of stability in control 13 In addition self collision arises in multi arm robots where the manipulators share a small common workspace 14 To avoid self collision most multi arm robots have a centralized control system in which one central unit computes the end effecter motions for each arm 15 In some industrial robots the motion planner generates an offline configuration for each arm and keeps them in synch in the workspace to avoid self collision 16 Moreover a study on a humanoid manipulator has introduced a self collision approach by applying smooth repulsive forces at potential collision points 17 B Obstacle Avoidance for Concentric Tube Robots Previous studies on single arm CTRs have proposed fast and interactive path planners to avoid collision with critical anatomy in minimally invasive surgeries A recent study on CTRs has shown the feasibility of rapidly avoiding anatomical obstacles by combing offline pre computation of collision free road maps with online position control 9 Moreover some groups have presented a multi node computational framework for online calculation of collision free path plans for telemanipulation of single arm CTRs using computationally intense parallel programming techniques 10 11 C Dual Arm Concentric Tube Robots Prior works on collision avoidance with obstacles for redundant manipulators are based on task priority concept 6 18 19 To avoid collision with obstacles a set of kinematic inequality constraints is represented to be satisfied during the desired end effector motion 18 Although previous studies on motion control of single arm concentric tube robots have been developed significantly over the past ten years the first dual arm CTRs was introduced only in 2018 This recent study on dual arm CTRs proposed a self collision approach based on task priority concept by keeping the distance between tip positions of arms at a pre set value to minimize collision risks To be more specific one arm follows the other arm motion to perform collision free collaborative tasks 20 This study did not consider inter collision avoidance between arms along their entire body explicitly The follow the leader state of art proposed by Burgner Kahrs et al 20 limits the motions of dual arm CTRs compared to the motions of two individual single arm CTRs This limitation would be arguably insufficient for teleoperation in MISs 20 To address this gap in the development of dual arm CTRs for endoscopic surgeries we propose a novel control system which detects and avoids self collision over the entire body of manipulators with trajectory error of less than 1 of total robot s length Ultimately we aim to recreate the independency in manipulator s motion of two individual single arm CTRs for a dual arm concentric tube robotic platform while performing cooperatives tasks safely with two tools in an endoscopic MIS II SELF COLLISION DETECTION FOR DUAL ARM CTRS Existing kinematic models for single arm CTRs are used to simulate their performance in a Cartesian control system Two independent control systems for two arms are developed to calculate two sets of joint values The proposed self collision detection module takes those joint values to predict self collision The performance of the self collision detection module was characterized on a test workspace A Control System for Single Arm CTRs Two individual control systems have been developed for two 6 DOF CTR arms to calculate the joint values of each arm These two controllers are based on existing differential Jacobian based inverse kinematics with two tasks with different priorities 5 The primary task in the control system of single arm CTRs is joint level constraints that accounts for unstable configurations The secondary task is end effector tracking The tele operative controller of the single arm CTRs with inequality constraints reads the desired tip position as well as inequality joint constraints 8 and solves for augmented joint velocity vector 14 as shown in Fig 2 7 The joint values are 1 1 2 2 3 3 where denotes rotation angles denotes translation lengths and 1 2 3 refers to outer middle and inner tubes respectively The notation refers to time derivative of joint values joint velocity The physical constraints 8 are as follows 1 2 1 2 3 2 3 4 2 The joint level inequality constrained problem becomes an equality problem by defining a set of slack variables accordingly to satisfy Equation 6 8 a set of joint constraints can be found using Equation 7 where refers to slack variables 1 8 and is a constant 0 1 6 The augmented joint velocities can be found by Equation 8 where 2 diag 8 14 is the augmented constraint Jacobian matrix of the physical constraints and 0 3 14 is the augmented joint Jacobian matrix and are the control Figure 2 Control system of single arm concentric tube robots Inequality Constraint Jacobian Tip Position Tracking Joint Level Constraints gains for the primary and secondary tasks that were selected empirically and are the joint errors and end effector position errors respectively B Self Collision Detection for Dual Arm CTRs The self collision detection module was designed to take joint values of each arm and discretize the arm body into segments as shown in Fig 3 a Each discretized point is denoted as where subscript refers to the point number and superscript refers to the arm The Cartesian position of each discretized point is calculated using torsion free forward kinematics 7 These Cartesian positions are with respect to the proximal end of the corresponding arm Since the base of Arm 2 is 5 mm apart from the base of Arm 1 in the X direction in trocar channels as illustrated in Fig 4 the position of points along Arm 2 is mapped to the base of Arm 1 This way both arms are defined in the same coordinate system with the base of Arm 1 sets as the origin as demonstrated in Fig 3 To detect self collision a set of Euclidean distances was computed between a specific point on Arm 1 to all other points on Arm 2 as shown in Fig 3a This is repeated for all points along Arm 1 Then the minimum distance among all sets of distances was found Self collision is predicted if the minimum distance between arms for all sets are maintained as a positive value less than a threshold distance We set the threshold value to be the sum of outer radii of inner tubes with a safety factor To advance the self collision detection module a pruning approach was implemented based on 13 For each configuration of dual arm CTRs potential collision points of two arms were pre calculated by finding a mathematical model of CTR manipulators Each arm was divided into one to three segments with different curvatures depending on the arm configuration each segment was modeled as an arc using equation of a circle and the intersection points of these sets of equations were found The potential collision points differ based on the curvature material properties and length of the selected nitinol tubes The proposed pruning approach discretized the arm into segments only in a restricted region active region instead of its entire body Active region is a specific part of CTR manipulators that collisions potentially occur and was pre determined according to the pre calculated collision points in each configuration of CTRs as shown in Fig 3b Thus sets of distances were calculated between all pairs of points in the active region to predict self collision A test workspace was generated with 725 different joint values for Arm 2 while Arm 1 remained stationary at specific joint values of 1 0 10 0 20 0 40 The rotation angles are in radians while the translation lengths are in millimeters The inner tube rotation angle of Arm 2 was set to The tube design parameters were recorded in Table I where and refer to outer and inner diameters of tubes respectively is the elastic modulus is the tube curvature and represents the length of each tube constituted of only curved section The 3D shape of dual arm CTRs was estimated by fitting the body of each arm into a discrete number of cylinders with the height of 0 1 mm and radius equals to the outer radius of the corresponding nitinol tubes Then the outline points of cylinders were used to create a volume of polyhedra using alphaShape object provided by MATLAB geom3d toolbox shown in Fig 3c The self collision cases in the generated workspace was determined if the polyhedra volumes of Arm 1 and Arm 2 were inside each other The self collision cases in the test workspace were found to be 546 in total The accuracy of the proposed collision detection module was evaluated based on two different parameters 1 the distance Figure 3 Illustration of self collision detection module methods a no pruning detection b pruning detection c 3D model of CTRs Arm 1 Arm 1 Arm 2 Arm 2 a b c Active Region of CTRs Arm 1 Arm 2 TABLE I TUBE DESIGN PARAMETERS Tube outer middle inner mm 2 31 1 85 1 47 mm 2 11 1 70 1 29 GPa 31 4 28 6 26 6 mm 1 0 0147 0 0147 0 0226 mm 0 20 0 20 0 40 Figure 4 Trocar cross section dimensions Right Instrument Channel Left Instrument Channel Dc 2 55 mm S 5 mm x y z Dtrocar 9 5 mm Water Channel Endoscope Channel Figure 5 Self collision detection performance for various distance between points along each arm Blue bars indicate the cases that self collision module successfully predicted collision Orange bars indicate the failed cases of detection module where self collision was predicted by 3D models of CTRs between discretized points along each arm as shown in Fig 5 2 safety factor as illustrated in Fig 6 Also the computation times of detection module for both no pruning and pruning and the computation times of the self collision detection based on geometric 3D models of dual arm CTRs were measured using the timeit function in MATLAB on a 2 50 GHz Intel Core i7 6500U Laptop with 8 GB RAM The reported computation times for pruning algorithm in Table II do not include the computational cost of pre calculating the potential collision points for each configuration since the active regions corresponding to all possible configurations of CTR are only identified once with any sets of nitinol tubes The computation times reported in Table II were averaged over 100 samples III SELF COLLISION AVOIDANCE CONTROL SYSTEM The flowchart of the proposed controller for dual arm CTRs is illustrated in Fig 7 The desired positions of both arms are fed into two single arm controllers to calculate joint values as shown in Fig 8a Then the resultant joint values are fed into the self collision detection module to predict the collision Next the self collision avoidance module is triggered in case of collision detection To avoid self collision two individual motion controllers were developed based on three prioritized tasks The primary task is joint level constraints that accounts for joint limits and unstable configurations The secondary task is self collision avoidance and the tertiary task is end effector tracking as illustrated in Fig 7 The collision avoidance module is shared between two motion controllers of each arm as demonstrated in Fig 8b This shared control system takes the desired tip positions of each arm and calculates the minimum distance between CTR manipulators as described in section II Ultimately the self collision avoidance controller tends to keep the minimum distance between arms along their entire body at a pre defined threshold value Therefore the proposed controller for each arm is a closed form Cartesian controller in terms of differential kinematics with three main constraints to satisfy 1 inequality constrained problem for joint limits 2 equality constrained problem for self collision avoidance and 3 equality constrained problem for tip position tracking A Task Priority Formulation The task priority formulation for implemented Jacobian based kinematics is based on a recursive projection to null space of the prior task solution 1 1 1 0 1 Figure 7 Flowchart of the proposed controller for dual arm CTR Desired tip positions Inverse kinematics Forward Kinematics Calculation of Cartesian position of discretized points along each arm Self Collision Joint values calculated by collision avoidance Joint values calculated by individual controllers Single arm joint values YesNo i Physical constraints ii Self collision avoidance iii Tip position tracking TABLE II COMPUTATION TIME OF SELF COLLISION DETECTI