POSITION PLANNING FOR COLLABORATING ROBOTS AND ITS APPLICATION IN NEUROSURGERY

Original scientific paper Applications of robot manipulators in surgery are nowadays a very actual field of research. Still, there are a number of technical problems when setting and preparing robotical systems for various operation procedures. One of them is the robot-patient placement. When placing robots in respect to known target working positions it is crucial to assure feasible positioning where all required motions can be executed with no kinematic or collision problems. A planning method for robot placement suitable for neurosurgical operations is presented in this paper. The planning method is based on a multi-objective cost function which is composed of criteria that balance dexterity properties with a novel collision avoiding parameter. Use of the planning approach is implemented and validated on a dual arm robot setup.


Introduction (Introductory remarks)
Placement of robotic arms in respect to their working positions is a problem targeted since the early days of robotics.Over the years robot performance criteria based on the kinematic analysis have been established and demonstrated to be useful when coping with them, starting with the manipulability criterion by Yoshikawa [1] and the condition number by Salisbury and Craig [2], followed by the compatibility index [3].Also, a criterion for avoiding the limits of the manipulator joints is presented in [4].The manipulability performance criterion has been extended for multiple cooperating robots in [5].These criteria are solely dependent on the kinematic state of the robot's kinematic chain i.e. design parameters and joint angles.Multi-objective optimization for the placement problem based on some of these criteria has been presented in [6,7] respectively.Similarly, kinematic and time criteria for the optimization of the robot base position were used in the SMAR system that was developed for industrial use [8].Approaches that do not rely on the kinematic performance criteria have also been presented.One that does not require the computation of the inverse kinematic solution is presented in [9].It forces the workspace envelope with the use of a cost function under functionality constraints towards the working target points.
The base positioning problem can also be related to the path planning problem by inverting it.It is obvious that the robot's trajectory should be in its working range but assuring the optimal movements in the robot's kinematic space is a higher level problem.In [10] the optimal robot path is calculated based on the optimization of custom variable robot constraints which leads to an unconstrained minimization problem.Optimal trajectory poses found with the use of a genetic algorithm optimization of a pose ruled surface concept is presented in [11].
In many standard industrial applications it is sufficient to determine the robots optimal position in respect to its target positions only once.On the other hand the need for dynamic robot placement in respect to its working trajectories is pronounced in the field of medical robotics.Here, patient's placement is mostly defined for every operation case individually.
Regardless of this, not many papers are dealing with the problem of robot-to-patient positioning.However, some work has been shown.For example, a two-step optimization problem using different criteria crucial for the execution of a minimally invasive surgery (MIS) operation is set and implemented in [12].Further, also for MIS, a preoperative position planning strategy based on a dexterity map of the laparoscopic robot is given in [13].
Due to the application of robotic arms in new dynamic environments such as neurosurgical operations, it is important to develop mechanisms that enable fast and easy adaptation to surrounding conditions.Here, it is crucial to know the right placement of the robot in respect to the patient when the patient is positioned in the operating room.Planning strategies which are carried out before entering the operation room can be very useful for this.However, often, it is not possible to accurately predict the patient's position in the operation procedure or replicate the setup from the pre-operational plan.This implies the need for a time-efficient off-line planning solution which can be carried out as soon as the patient is positioned in the real operation position.The position of the surgical robots could therefore be set for each individual case specifically.As the final result, operation completion together with collision free operation has to be guaranteed.
Operation completion can be related with the robot's operation workspace.As mentioned, robot's kinematic performance indices can assure the robot to operate in its optimal part of its workspace.Considering this, the use of multi-objective optimization of kinematic performance criteria seems interesting in order to achieve a reliable solution for this problem.
Standard neurosurgical operations e.g.stereotactic procedures mainly require the surgeon to use both hands while being potentially assisted by scrub nurses.By analogy, robotic surgical systems use multiple robotic arms.Operation of these robots in same spatial target points is often required.For this purpose, besides the identification of suitable performance criteria for reliable motion execution, a custom collision avoidance criterion that enables adequate spatial separation of two robots while working in same target points will be incorporated into the positioning strategy.The robot positioning problem is then solved in one step by optimizing every parameter of the proposed objective function.The approach will be tested for two 6DOF revolute serial robots.The work presented in this paper is the result of research conducted on the neurosurgical platform RONNA [14].

Methods
In this section are presented performance criteria which have been identified as suitable for the development of the optimization method for the positioning problem of stereotactic robots.Besides giving a solution for the placement of one robot the method has to provide a strategy for two cooperating robots, operating in mutual physical target points without colliding with each other.

Condition number
The first kinematic performance index used in this application is the condition number c(J T ).It is used as a measure of dexterity of the robotic arm by quantifying the distance from singularity positions.A low condition number indicates a robot posture with high dexterity and far from singularity positions.High values of the condition number indicate the closeness of singularity positions in which high uncontrolled velocities of the end effector are possible.Singular positions can therefore be avoided when planning robot linear movements characteristic for stereotactic minimally invasive operations like bone drilling and insertion of surgical instruments.
The condition number is calculated based on the robot's Jacobian matrix.Containing both linear and angular velocities the Jacobian matrix of a robot is not homogenous in its original form.A number of different normalization methods were presented in [15].Here, the robot's characteristic length L is used as a normalization factor.The characteristic length is obtained by the procedure presented in [16].Normalization of the Jacobian was made using the form shown in [17]: .
The condition number of the normalized Jacobian in the normalized form can then be obtained as follows: ( ). (2)

Joint limit avoidance
The joint limit avoidance criterion presented by Pamanes [4] is designed with the idea of optimal robot posture when its joints are positioned in the middle of there working range.This prevents the robot from movements which can possibly not be performed because of joint boundaries.The criterion is defined as: k and σ k -the mean and standard deviations of the m×n values of the coefficient k: Δq ij -the deviation of the i th joint variable, with respect to the center of its permissible range, at the j th task range.Δq i max -the maximum deviation permissible of the i th joint variable.
For a six axis revolute robot, n=6 and m is the number of positions (solutions) which will be considered in the objective function for optimization.

Separation distance
Collision avoidance between robots is mostly referred to planning collision free paths while performing individual tasks or collaborative actions e.g.transporting an object together.This is done either in real time during the robots motion (online) or before the beginning of the motion (off-line).Real time approaches tend to require relevant processing power [18 and 19], mostly unnecessary in medical systems.A lot has been done on the subject of collision free offline path planning.In [20], the robot configuration space is searched for optimal collision free paths with a co-evolutionary genetic algorithm.In [21], trajectories of cooperating robots are represented as the n th order polynomials whose parameters are optimized with genetic algorithms under the robot's dynamic constraints.A two phase approach which uses an incremental A* algorithm to search the configuration space-time for the path planning of multiple robots is shown in [22].Collision avoidance for redundant manipulators has also been a special subject of different papers [23][24][25].
Considering the use in static unchanging environments during the robots motion, a simpler approach for collision avoidance of cooperating robots intended to work in same spatial positions is presented in this section and defined as a function.This approach takes the positions of the (n−1) th joints of cooperating manipulators and maximizes their spatial distance: This can be achieved in a controllable way by altering one of the unconstrained orientation parameters of the robot operation point -roll, pitch or yaw, depending on the robot tool configuration.For cooperative robots this means aligning both robot tools in the way they do not interfere with each other.In Fig. 1 an example situation is shown where both roll angles of the robots tools were corrected by the combined value of α in order to avoid collision.
For a practical application this can be implemented as the Euclidian distance calculated for each configuration of the robots.This function can be treated as a criterion which is suitable for the implementation into the final objective function.The goal of the function is to yield roll angles for both robots that maximize the distance between two joints and therefore minimize the parameter e.

Objective function
The objective function is composed of the three described criteria in a multi-objective function.It minimizes the criteria of the condition number, joint limit avoidance and the above described separation distance.The separation distance is implemented in the optimization cost function only for robot cooperation cases: Optimization parameters x r1 , y r1 , x r2 , y r2 are the Cartesian base coordinates of the first and second robot respectively relative to the common working points.Also, α 1 and α 2 are the tool roll angles defined from the initial positions of the two robots.Depending on the degrees of freedom of the robot bases, the parameters, z r1 and z r2 can also be incorporated.In situations where the robots are mounted on tables with fixed heights which can often be seen in medical applications, this is not needed.
It can be noted that for solving the optimization problem with these criteria both inverse and forward kinematic solutions have to be known for all robots.

Experiments and results
The optimization problem is set to minimize the objective function Q under the constraints of the inverse kinematics of both robots involved.The objective function is minimized with the Nelder-Mead algorithm that solves unconstrained nonlinear problems and is implemented with the fminsearch algorithm in MATLAB.It proved to be a satisfying solution in terms of speed and optimization results considering six optimization parameters in this application.Initial conditions for the minimization algorithm can be set arbitrarily.However, depending on the application, it is convenient to set the initial positions of robots in respect to the targets in real physical boundaries of the robot workspace.This assures a good starting position for fast execution of the optimization.Regarding Fig. 2: a) k 2 , k 3 = 0, the condition number  forces all target points in the position of maximal dexterity which lies relatively close to the origin of the robot base coordinate system.b) k 1 , k 3 =0, joint limit avoidance φ tends to put all the target locations far from the robot base origin and therefore forces the robot to operate in a stretched configuration.c) k 3 = 0, combining the two criteria (c and φ ) a more neutral pose of the robot is achieved while maintaining good dexterity values.
All parameters of the objective are set to influence the solution.The separation distance e does not affect the robots in respect to the target points like the other two criteria but adopts the robot configuration in respect to the cooperating robot.This is needed in dual arm cooperation to prevent eventual collisions during operation in mutual target points.It was shown that the objective function for the use with two robots yields most reliable results in terms of collision avoidance when the influence of these three criteria is balanced in a certain ratio.These parameters are identified experimentally and show a good benchmark for the scaling factors k 1 , k 2 , k 3 .The final cost function is then scaled as follows From the analysis above, it can be noted that the condition number, joint limit avoidance and separation distance each affect the manipulability of the robotic manipulator in different ways.It is assumed and theoretically confirmed that the condition number and the joint limit avoidance parameter try to increase the manipulability of the robot.However the impact of the separation distance criterion on the manipulability is unknown.As the condition number is here considered as the basic indicator of dexterity, the separation distance will be analyzed in respect to it.The global condition number of the robot without the separation distance criterion will be compared to the case when the separation distance is applied.This will be done for both robots in the dual arm configuration for more than 30 target trajectories.It is shown that the separation distance deteriorates the dexterity by around 35% in mean value.This however is not a reason to discard the criterion because the condition number stays relatively low in respect to the whole normalized value range.This means that the use of the separation distance used in the earlier mentioned ratio does not affect dexterity majorly.

Implementation and testing in stereotactic operations
The presented objective function is developed for position planning in situations where robots have to perform individual or cooperating tasks in specific predefined target positions.Relative relations of the target (operation) points are not altered or modified in the optimization process.Optimal locations of robot bases are obtained as well as configuration corrections for mutual target points.This assures movement execution in the optimal part of the robot workspace while solving the collision problem in critical points (when robots work at the same target).
As mentioned, the approach is designed for applications where robots have to operate in the same coordinate frame.Such an application is stereotactic surgery.The patient can typically have multiple trajectories in the patient's coordinate space (frame) which have to be performed through the operation.An important requirement here is that all trajectories can be performed in a single setup thus saving total operation time.
For the testing of the developed concept, planning for a dual arm stereotactic operation is simulated.The simulated setup uses two industrial robots with fixed base-heights in a global coordinate system.This is characteristic for systems that are mounted on mobile platforms.A typical procedure is performed as follows.Operation steps: One of the robots positions its guidance tool in entry points of the trajectories with the trajectory orientation and maintains a fixed position while the other robot performs a linear movement from the entry to the target point also under the trajectory orientation.This represents characteristic stereotactic operations like bone drilling or insertion of surgical tools into the intracranial space of the patient.
The mutual operating frame coordinate system with target points for which the optimization is to be tested is represented with a phantom with several random trajectories.Input parameters for the optimization algorithm are the phantom height in respect to both robots (z coordinate) and the phantom orientation in the robots base coordinate frames.Variables that are to be optimized in the objective function are the planar (horizontal) position of both robots and tool separation angles for the collision avoidance.The orientation of the robot base is not used as an optimization iparameter (it remains constant) because the first robot axis has practically no influence on the robot dexterity.
It can be noted that the condition number  and here presented separation distance e do not incorporate the number of target points in their definition.In other words, values of these parameters have to be obtained for every targeted point individually and by averaging, the current iteration in the optimization can be evaluated.This solution is used here.On the other hand, the joint limit avoidance index φ has this already implemented in its definition by evaluating all the robot joints in all configurations of the individual setup through the mean and standard deviation of the joint angles expressed in the coefficient k, so: where n is the number of configurations i.e. target points.
Typically neurosurgical systems can use dual arm configurations with two different robots with different characteristics.The presented approach does not depend on the type of the robot used.The only requirement is to know the inverse and forward kinematic models of the used robots.
For illustration of the optimization solutions, some of the simulated scenarios with the relations between the mutual working frame (phantom) and the robots are shown in Fig. 5.
After running the optimization for several random phantom heights and orientations a very high execution rate for all phantom trajectories was noted.By setting the parameters as in section 3.1, no collisions for executed trajectories occurred between robots.Also, the linear movement from the trajectory entry to target points was executed without problems.High dexterity is here very important because linear movements can bring problems regarding singularity positions in the movement.Eventual positioning of the working frame outside of the robots workspace would also appear with the lack of IK solutions.These were not reported during the testing.

Conclusion
The method for optimal robot placement in stereotactic surgery interventions was presented in this paper.Also, a separation criterion for collaborating robots was proposed with the purpose of collision avoidance in mutual target positions.It is an approach which can be used to determine the robots position immediately after the patient was set in its operation position.Here, the data about the patient's height and orientation can be given to the optimization algorithm via a stereovision or optical tracking system that is commonly used in surgical operating procedures.
The proposed cost function is constructed from three different criteria.Two of them are concerned with the optimal placement of the robot manipulator in respect to working trajectories.The third criterion is used to prevent colliding robot configurations when cooperating robots work at the same operation point.It is shown that with correct scaling of the single objective functions it is possible to achieve very reliable results of the placement optimization.Problems regarding robot singularities and joint limits did not occur in scope of the extensive testing carried out with the methodology described in section 3.2.The proposed Nelder-Meads optimization algorithm has showed to be able to find converging solutions in satisfying time lengths (5-30 sec).However, realistic initial conditions proved to bring faster solution convergence.This can be achieved by initially placing the target positions inside the robots physical working range and by putting the robots positions relative to each other in a manner which is likely to be suitable for the operating conditions and setup (e.g.robots opposite to each other).Also, when using this for real life clinical applications it is highly recommended to simulate the operation under the obtained optimized parameters.
Another, more general, approach of robot position planning is to have a predefined task-oriented layout where robots are positioned according to an off-line generated spatial plan.The criteria presented in this paper can be used for the creation of such layout plans, resulting in discrete dexterity maps.
The presented approach can also be used in applications with more than two robots.For such use, the collision criterion should be adjusted to calculate and value configurations of the distance vector between n robots.With more robots in use and less free space around target trajectories, an extra trajectory planning or scheduling method might be needed to avoid collisions while reaching target locations.
Future work will be concerned with utilizing a machine learning approach for the problem of robotpatient placement where training data will be provided using Monte Carlo simulation.

Figure 1
Figure 1 Robot cooperation at same target point with corrected roll angles

3. 1 Figure 2
Parameter scaling Different values of scaling factors k 1 , k 2 and k 3 will be shown in Fig. 2 in order to illustrate the effect of individual criteria in the objective function in respect to target positions contained in a coordinate frame of a test phantom.Optimal robot poses according to individual criteria: a) condition number, b) joint limit avoidance, c) combination of a) and b)

Figure 3
The effect of the separation distance criterion on collaborating robots: a) without separation distance, b) with separation distance a) k 3 = 0, a dual arm collaboration in the same target point with no influence of the separation distance .b) k 3 ≠ 0, a dual arm collaboration in the same target point with the influence of separation distance .

Figure 4
Figure 4 The effect of the separation distance criterion on the condition number In Fig. 4 C1 and C2 represent the pure condition number of the robot.C1d and C2d represent the case when the cost function involves the separation distance criterion.The condition number is scaled to the values from 0 to 1 and lower values indicate better dexterity.It is shown that the separation distance deteriorates the dexterity by around 35% in mean value.This however is not a reason to discard the criterion because the condition number stays relatively low in respect to the whole normalized value range.This means that the use of the separation distance used in the earlier mentioned ratio does not affect dexterity majorly.

Figure 5
Figure 5 Robots working on common target trajectories in different spatial positions and orientations with optimized position and avoidance parameters