Optimization of Surgical Robotic Instrument Mounting in a Macro–Micro Manipulator Setup for Improving Task Execution

In minimally invasive robotic surgery, the surgical instrument is usually inserted inside the patient’s body through a small incision, which acts as a remote center of motion (RCM). Serial-link manipulators can be used as macro robots on which microsurgical robotic instruments are mounted to increase the number of degrees of freedom of the system and ensure safe task and RCM motion execution. However, the surgical instrument needs to be placed in an appropriate configuration when completing the motion tasks. The contribution of this article is to present a novel framework that preoperatively identifies the best base configuration, in terms of Roll, Pitch, and Yaw angles, of the microsurgical instrument with respect to the macro serial-link manipulator’s end effector in order to achieve the maximum accuracy and dexterity in performing specified tasks. The framework relies on hierarchical quadratic programming for the control, genetic algorithm for the optimization, and on a resilience to error strategy to make sure deviations from the optimum do not affect the system’s performance. Simulation results show that the mounting configuration of the surgical instrument significantly impacts the performance of the whole macro–micro manipulator in executing the desired motion tasks, and both the simulation and experimental results demonstrate that the proposed optimization method improves the overall performance.


Optimization of Surgical Robotic Instrument
Mounting in a Macro-Micro Manipulator Setup for Improving Task Execution

M INIMALLY invasive robotic surgery (MIRS) has been
introduced in surgical operations due to the need to improve precision and augment the surgeon's capabilities, and therefore, reduce the patient's trauma.
In robotically assisted surgical procedures, the surgical instrument is inserted inside the patient's body through small incisions. This, however, restricts the motion of the robot, which is not allowed to move tangentially to the hole plane, but can only pivot and translate in the insertion direction about the insertion point. One of the challenges for the control of surgical robots is guaranteeing safe execution of the constrained motion at the insertion point, also known as remote center of motion (RCM). This constraint leads the robotic system to lose two degrees of freedom (DOFs) [1].
Different approaches have been developed to overcome the issue of loss of mobility due to the RCM motion, and they can typically be divided into design-based and control-based strategies [2]. Design-based strategies focus on constraining the RCM mechanically, by designing robotic structures, like parallel mechanisms [3], [4], to guarantee RCM. The drawback of these devices is that, since the mechanical RCM is at a fixed location with respect to the manipulator, they are usually specific for a certain type of operation and have complex structures [5], [6].
Control-based techniques, instead, allow RCM motion by properly tuning the control algorithm. These approaches simply require the kinematic model of the robot and can be employed in different situations to ensure that the instrument is directed through the RCM at all times. In order to achieve that, the robotic system must have enough DOFs. Therefore, a common approach is to use traditional serial-link manipulators on which the microsurgical instrument is mounted [7], [8] by choosing a suitable configuration for the base of the surgical instrument with respect to the macrorobot's end effector. Various approaches have been employed to control surgical robots under RCM constraint, such as the augmented Jacobian [1], [9], [10], projection in the null space method [11], and multiobjective optimization with constraints [12], which exploit the system redundancy by considering different subtasks. This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ The main drawbacks of these approaches are that they either may not always guarantee that the different subtasks are always satisfied, e.g., due to conflicts such as in the augmented Jacobian method [13], or do not allow to include additional constraints. Moreover, they only target surgical robots with few DOFs. In addition, when serial-link articulated robots are employed, the surgical instrument needs to be attached in a proper configuration in order to guarantee correct and safe task execution. Both in teleoperation and in autonomous control, the whole robotic structure needs to accurately fulfil the commanded motion task, satisfying the RCM and additional constraints, but to also allow as much dexterity as possible.
Different works have focused on finding the optimal placement of a robotic system, in order to guarantee a good performance in solving a given motion task. This problem has been of great interest especially in the manufacturing industry. In [14], the authors look for optimal robot-workpiece placement of a redundant manufacturing robot for a welding process through particle swarm optimization. Their approach, however, yielded an optimal configuration residing within a small region. This may lead to a deterioration of the performance if the placement is not accurate. Spensieri et al. [15] used the Nelder-Mead method for finding the optimal base placement of a manufacturing robot. Yet, the converge properties have not been proved, except for small problems [16].
Usually robots in manufacturing are required to complete the same single specified task over time, therefore, finding the optimal robot base placement may suffice, since it is required only once. In robotic surgery, instead, the task may vary depending on the surgical procedures to complete, and thus, the placement may need to change accordingly. In the field of robotic surgery, a few researches have focused on the optimal placement of a robot. For example, in [17], the authors focused on seeking optimal base positioning of a robot with an ultrasound probe for radiotherapy. However, their work did not require any consideration about RCM constraint. Konietschke et al. [18] utilized the genetic algorithm (GA) to optimally design a robot manipulator to perform a surgical task; the main drawback is that different tasks may require different new designs.
Other works, instead of optimizing for robot design or placement, concentrated on the optimal insertion point position. In [19], the authors used CT images to manually identify the set of possible port locations, and then, applied a search space method among the possible port locations to find the best one. In this work, however, the RCM motion was achieved thanks to the robot's mechanical design. In [2], instead, the robotic setup consists of a serial-link manipulator and a surgical instrument. This work is concerned with the problem of choosing a location for the RCM relative to the manipulator in order to maximize the system's performance. Their optimization only consisted of two position parameters along x and z and used a grid of 121 × 121 points as a search space.
The limitation of focusing on the optimal placement of the base of the robot is that it requires a large effort to move the whole robot around. This might also require a redesign of the whole operating room for each surgical procedure. Optimizing the robot design [20] on the other hand, would probably require a new design when a new task is assigned. The works focusing on the placement of the insertion point, instead, were mostly employing robots like the DaVinci, where the RCM motion is obtained mechanically. Moreover, the optimization of the placement of the insertion point should be able to include information provided by the surgeons, who take their decisions according to preoperative surgical requirements and patient's anatomy [21]. In addition, most of these works took into account only the system dexterity, without consideration of motion accuracy or resilience to placement errors. Configurations with high dexterity are not guaranteed to achieve good tip tracking performance, and viceversa, configurations leading to small tracking errors may not lead to high dexterity. Because of the RCM constraint, also the accuracy in tracking the RCM motion needs to be taken into account. Furthermore, the optimal configurations found do not take into account placement errors, and therefore, may reside in regions where small deviations from the optimum lead to large deterioration of the performance.
This manuscript is thus intended to provide a novel framework to identify the optimal mounting setup, by choosing the best base configuration of the microsurgical robotic instrument with respect to its macro serial-link manipulator's end-effector frame, to ensure the maximum accuracy and dexterity in performing specified tasks. The surgical robotic instrument's base configuration is here described by means of Roll, Pitch, and Yaw (RPY) angles, which are chosen as optimization variables. Optimizing only for the connection between the two robots makes the whole macro-micro manipulator more flexible and easily adaptable to different surgical procedures with minimum effort. This work focuses on the use of a KUKA robotic arm as a macrorobot, and the Micro-IGES surgical robotic instrument [22] (see Fig. 1) as a microrobot, to simulate a tumor resection task. However, the framework can be generalized to any other robotic structures and tasks. To the best of the authors' knowledge, there is no other published work analyzing the effects of the connection between a surgical robotic instrument and a serial-link manipulator, and focusing on finding the optimal connection in order to improve the task execution. To address this problem, we propose a novel framework consisting of an optimization algorithm, an optimal control strategy, and a resilience to error approach to limit the loss of performance due to possible assembly errors.
The optimization algorithm searches through all possible base configurations of the surgical robotic instrument and finds the optimal one, while the optimal control strategy is used to ensure proper task execution and acts as a testbench to compare the different configurations. Fig. 2 shows the roadmap of the proposed framework.
For the control problem, the hierarchical quadratic programming (HQP) [23] approach is adopted, since it allows to optimally solve multiple prioritized subtasks while satisfying additional constraints. With regards to the optimization of the surgical robotic instrument's base configuration, the GA is used. The choice of an heuristic global optimization approach like GA over nonheuristic approaches is dictated by the fact that nonheuristic approaches are generally based on some knowledge about the robotic structure and are more model-specific, thus not as generalizable as the heuristic ones. Among other heuristic approaches such as particle swarm optimization or Bayesian optimization, GA was chosen thanks to its ability to explore large search spaces, more likely find global minima, and to its capability to better deal with discrete variables. An appropriate fitness function is introduced, presenting a novel approach for computing the robot dexterity measure when multiple prioritized subtasks are assigned. However, the optimal configuration identified by the solver may reside in a region where small deviations from the optimal may lead to much worse performance. Consequently, a strategy to find an optimal solution in a neighborhood that guarantees a good performance is also introduced.
The main contributions of this work are as follows: 1) Analyzing how the mounting configuration between a macro serial-link robot and a microsurgical robotic instrument affects the performance of the whole system in performing a specified task.
2) Proposing a framework to identify the optimal mounting configuration in order to maximize the accuracy and dexterity in performing a desired task. 3) Proposing a fitness function and a proper resilience to error strategy to be included in the framework in order to find the optimal solution and guarantee that deviations from the optimum do not deteriorate the system's performance in fulfilling the desired task. Our simulation results show that the configuration of the surgical robotic instrument with respect to the serial-link manipulator significantly impacts the performance of the whole macro-micro manipulator in executing the desired motion tasks, thus justifying the importance of finding optimal robot's configurations. In addition, our proposed strategy for increasing resilience to errors is shown to be effective in finding optimal configurations, which lie in regions where small variations in the tracking errors and dexterity occur, thus ensuring that assembly errors will not deteriorate the system's performance.
This article is structured as follows. In Section II, the Micro-IGES surgical robotic instrument is described and the kinematic model of the whole macro-micro manipulator composed by the KUKA arm and the Micro-IGES robot is derived. Section III describes state-of-the-art approaches for controlling redundant manipulators and the chosen HQP approach. Additionally, an overview of the GA for function optimization is presented. Section IV presents the proposed framework, describing the control strategy to guarantee the execution of the desired motion task, while satisfying the RCM and additional constraints, and formulating the optimal configuration problem to be solved with the GA. Results are then shown in Section V, and finally, Section VI concludes this article.

II. ROBOTIC SYSTEM DESCRIPTION
In this section, an overview of the robotic system setup is presented, describing the Micro-IGES surgical robotic instrument and the KUKA LBR IIWA robotic arm.  The shaft is responsible for the roll and translation DOFs. The articulated end, instead, consists of two elbows for pitch and yaw, with each elbow made of a pair of coupled joints, a 1-DOF revolute joint for the wrist pitch and the jaws. The jaws provide two more DOFs: one for the wrist yaw and one for the gripper's opening/closing. Each joint of the articulated part is driven by an antagonistic pair of tendons, with each pair being connected to the corresponding driving capstan at the proximal drive unit. The coupling of the two pairs of joints of the elbows occurs at the driving unit: the two capstans that drive the two serial joints for each DOF of the elbow (pitch and yaw) are coupled by a series of gears with 1:2 ratio. Due to the current setup, in this work, only 5 DOF are considered (Roll, Elbow 1, Elbow 2, Wrist Pitch, and Wrist Yaw), since the translation is provided by the KUKA arm and gripper opening/closing is irrespective here.
2) Robot Kinematic Model: The nonlinearities in tendon transmission make the mathematical derivation of the system kinematics and dynamics tedious. In addition, tendon-driven systems for minimally invasive surgery usually lack sensors at the distal side, therefore, joint values cannot be measured and controlled directly. Because of the general nonlinear mapping between motors and joints q u = f (θ u ), being θ u the vector of motor positions and q u the vector of joint positions, the kinematic model of the robot, with respect to its base frame describe the Cartesian end-effector pose in terms of joint and motor positions, respectively, b v u ∈ R 6 the Cartesian end-effector twist (linear and angular velocities vector), b J q is the Cartesian task Jacobian with respect to the joint variables, and b J u the Jacobian with respect to the motor values. The matrix L, with L i,j = ∂q u,i ∂θ u,j , is the motor to joint differential matrix [25]. In this work, we employed the same motor to joint mapping as in [26]. The control problem for the Micro-IGES can, therefore, be formulated as a function of the motor values, which can be directly measured and controlled [27]. As described in Section II-A1, the system's state is expressed by for the Roll, Elbow 1, Elbow 2, Wrist Pitch, and Wrist Yaw motion.

B. Macro Robotic Manipulator
The KUKA LBR IIWA is an articulated industrial robot with seven joints (q k ∈ R 7 .). Thanks to its serial-link manipulator design and direct transmission (the motors are placed directly at the joints), the kinematic model of this robot can be easily computed by means of the Denavit-Hartenberg convention. Therefore, the kinematics of the KUKA can be expressed with respect to its base reference frame (2)

C. Macro-Micro Manipulator Model
The macro-micro manipulator is composed of the Micro-IGES and the KUKA robots (see Fig. 4). The Micro-IGES motor package is directly attached to the KUKA end effector. In our simulation environment, however, to simplify the visualization, the Micro-IGES is considered to be directly attached to the KUKA, just neglecting a vertical translation corresponding to the height of the Micro-IGES motor pack. In total, the system has 12 DOF (seven for the KUKA and five for the Micro-IGES). The robot state is then described by q = [q k θ u ] T . To compute the kinematics of the whole system, one approach could be to generate a single model of the whole manipulator and use standard modeling techniques, such as the Denavit-Hartenberg convention. In this work, however, we propose a more flexible approach that consists in combining the two models of the robots.
The whole macro-micro manipulator end-effector's pose, with respect to the KUKA base, can thus be computed as where 0 T k is the KUKA end-effector pose, k T b ∈ R 4×4 is a fixed transformation matrix from the KUKA end-effector to the Micro-IGES base frame, and b T u is the Micro-IGES pose with respect to its base. Decomposing (3) into position and orientation, results in where 0 P k is the KUKA end-effector position in its base frame, k P b,k is the Micro-IGES's base position with respect to the KUKA end-effector frame, b P u,b is the Micro-IGES's end-effector position with respect to its base frame, 0 R k is the KUKA end-effector rotation matrix in its base frame, k R b the Micro-IGES's base rotation matrix in KUKA's end-effector frame, and b R u the Micro-IGES end-effector rotation matrix with respect to its base frame. By deriving (4a) with respect to time, it yieldsṖ = 0Ṗ being the skew-symmetric matrix of cross-product and ω k ∈ R 3 KUKA's end-effector angular velocity. Being ω k = J o,kqk , with J o,k ∈ R 3×7 the KUKA orientation Jacobian, and from the definition of the cross product, it results thaṫ where J l,k ∈ R 3×7 and J l,u ∈ R 3×5 are the Jacobians of for the linear velocity of each robot, andĴ l, With regards to the orientation, by deriving (4b), we By combining (5) and (6), the macro-micro manipulator's endeffector twist can be computed as (7) with J KUKA ∈ R 6×7 and J IGES R 6×5 describing the contribution of each robot to the motion.

III. PROBLEM FORMULATION
In this section, a brief overview of the methods for the redundant robot control is presented, along with the description of the GA, employed for the optimization of the robot configuration.

A. Redundancy Resolution
The goal of inverse kinematics is to compute the proper joint angles q ∈ R n j and velocitiesq that allow the system to execute a desired motion task. The linearity at the velocity level is usually exploited to compute joint velocities froṁ where J is the Jacobian matrix andḣ ∈ R m p is a desired task.
In case of redundant robots, the number of available joints n J to execute the trajectory is larger than the number of constraints imposed by the desired task dimension m p . Therefore, the solution to problem (8), if it exists, is not unique. Different techniques have been developed to solve this problem, such as the extended Jacobian technique [28], [29] or the augmented Jacobian [30]. Some additional tasks are introduced so as to solve a determined single system of linear equations.
Those approaches, however, do not allow to consider task prioritization. In fact, all tasks are given the same level of priority and this may lead to conflicting tasks. In many cases, some tasks have lower priority than others, meaning that their achievement is less important. The gradient projection method allows to take task prioritization into consideration. Considering the case of control at velocity level, the general solution of (8) is [31] where J † is the pseudoinverse of J , P = I − J † J is the projector onto the null space of J , andq null is an arbitrary vector, responsible for the self-motions of the system: the robot can move without affecting the execution of the main motion task. Low priority tasks can be executed by being projected into the null space of higher priority tasks, without affecting the higher priority tasks [32]. Nevertheless, the gradient projection method does not permit to include system constraints. Recently, especially in the field of humanoid robotics, where researchers have to deal with high level of redundancy, a common approach to solve stacks of prioritized tasks subjected to different constraints (both equality and inequality) consists in applying the HQP framework introduced by [23], where the control problem is formulated as where N is the number of prioritized subtasks,ḣ n is a desired task to achieve, A in,n , b in,n describe additional inequality and equality constraints for each motion task. This formulation allows to find the optimal solutionq n , without violating the higher priority tasks and the imposed constraints. A in,0 , b in,0 , J h,0 can be set to zero or empty, as only used for initialization. λ is a constant coefficient for regularization. If only equality constraints are used, the solution of (10) coincides with (9).

B. Genetic Algorithm (GA)
The GA [33] belongs to evolutionary optimization methods, which try to find global optima of a cost function by mimicking the mechanisms of natural selection and genetic evolution. The main advantages of the GA are as follows:. 1) it is derivative free, thus it does not require to compute the derivatives of the cost function; 2) it finds the cost function optima in a trial and error way, by using multiple individuals per iteration; 3) it allows to have very large populations, and thus, effectively explore large search spaces. Due to its capabilities, the GA has been widely used to identify optimal design configurations [34]. Different works employed the GA for optimal robotic design [35]- [37].
In this work, MATLAB Optimization Toolbox has been used and the following steps are performed.
1) Initialization: The algorithm begins by creating an initial population of P opSize individuals. This initial population can be assigned by the user or generated by means of the CreationF unction. The ScaledV alues are then exploited by the SelectionF unction to choose the appropriate parents for the next generation. 4) Children Generation: From the P arents, Children are generated in different ways. A certain number of individuals are passed directly onto the next generation (Elite Children). A certain percentage of the Children is instead obtained by means of the Crossover F unction, and some others are randomly obtained through the M utation F unction. 5) Migrate Population: Finally, the obtained Children are used to replace the current P opulation, and thus, obtain a new population. Steps 2-5 are repeated until the stopping criteria are met. Generally, the algorithm stops if the fitness value is lower than a specific threshold, if the maximum number of generations is reached, or if the best fitness value does not change for a certain number of generations (Stall Generations). At the end of the procedure, the population of the latest generation is returned.

IV. METHOD
In this section, the framework to find the best surgical robotic instrument's base configuration in order to perform a desired task is presented, describing the chosen control method and formulating the optimization problem.

A. Motion Control
The robotic system comprised of the KUKA and the Micro-IGES robots is highly redundant. In fact, the total number of DOFs is 12 here. In order to optimally exploit the hyperredundancy of the system, different prioritized subtasks can be specified and the HQP framework (10) adopted. In a surgical motion task like tumor resection, at least two prioritized subtasks can be specified: keeping the RCM fixed and follow a desired path to remove the tumor.

1) Remote Center of Motion (RCM):
The RCM is a point, usually the surgical instrument's insertion point, where only rotations and insertion motions can be achieved. During laparoscopic procedures, for instance, the RCM coincides with the location of the hole on the patient's body where the surgical instrument is then inserted, as shown in Fig. 5. Therefore, the motion at the RCM needs to be constrained along the surface tangential directions (y and z in Fig. 5). Consequently, the primary subtask is to constrain the RCM and the corresponding cost function can be specified as ||ḣ RCM − J RCMq || 2 . J RCM allows to compute the velocity of the RCM based on the robot kinematics.
The position of the RCM with respect to the macromicro manipulator's base is computed similarly as in (4a) as P RCM = 0 P k + 0 R k k P RCM,k , with k P RCM,k being the RCM position with respect to the KUKA's end effector, in the endeffector's frame. By deriving with respect to time, as in (5), it turns out thaṫ The aforementioned equation shows that the contribution of the surgical robotic instrument to the RCM is null, as the RCM occurs on the shaft. To simplify the control problem, in this work, the RCM motion is expressed with respect to the insertion point frame in Fig. 5. Therefore, T the rotation matrix of the hole frame with respect to the KUKA base frame, and 0 J l,RCM and 0 J o,RCM the linear and orientation part of the RCM Jacobian. Thanks to this transformation, assuming the hole fixed, the RCM motion to consider is only along the y and z components. Consequently, the desired RCM velocity is set toḣ RCM = [0, 0] T in order to guarantee the RCM motion in the tangential directions to be fixed and J RCM ∈ R 2×12 . In cases where the RCM is not stationary (like patient breathing), it is still possible to track the RCM by properly specifying the velocityḣ RCM . In these cases, however, the RCM velocity needs to be known either a priori or by measurements with external sensors. For a more detailed description on how the kinematics of the RCM is computed, please refer to Appendix A.
2) Path Tracking: The secondary subtask is executing a desired motion, which, in cases like tumor resection, could be having the total system's end-effector follow a desired path. Given a reference Cartesian pathh p ∈ R m p , and its corresponding desired velocityḣ p , with m p ≤ 6, from (7), the cost function associated to the secondary subtask can be expressed as ||ḣ p − J totq ||. In this work, the path tracking is expressed in terms of the Cartesian position and orientation, therefore, m p = 6.

B. Prioritized Motion Control
In order to guarantee accurate execution of the two prioritized subtasks, the HQP problem (10) is modified as follows: for n = 1, 2 where n = 1 corresponds to guaranteeing RCM motion (J h,1 = J RCM ) and n = 2 is for the path tracking (J h,2 = J tot ). For the primary RCM subtask, J h,0 = 0 and only joint limits are considered. Λ = diag(λ k , λ u ) is a diagonal weighting matrix, which is used to reduce joint velocities when the systems are close to singularity. The entries of λ k ∈ R 7 and λ u ∈ R 5 are chosen such that with σ m being the smallest eigenvalue of the corresponding robot Jacobian. σ tresh and λ M are user specified values.
To reduce the tracking errors of both the primary and secondary subtasks, the desired tracking velocities are modified according to the closed-loop inverse kinematics (CLIK) [38] method as ḣ 1 =ḣ RCM + K RCM e RCṀ h 2 =ḣ P + K P e P (14) with e RCM ∈ R 2 and e p ∈ R 6 being the RCM and the tip position and orientation (expressed in angle-axis formulation) tracking errors. The tracking errors can be measured by external sensors, if present, or estimated from the kinematic model, otherwise. The matrices K RCM and K P are positive definite diagonal matrices of user's defined gains. Fig. 6 shows the flow chart of the procedure to find the optimal surgical robotic instrument's base configuration. The GA algorithm presented in Section III-B is employed to find the optimal Micro-IGES pose with respect to the KUKA robot. Therefore, the optimization variable is the Micro-IGES base to KUKA robot transformation matrix k T b , as described in Section II-C and shown in Fig. 7, which is defined as

C. Configuration Optimization
Since the Micro-IGES shaft is rigid, it follows that k P b,k = k R b r, with r = [0, 0, 0.27] T in meters, due to the length of the shaft. Consequently, the optimization problem can be formulated in terms of k R b only. At each iteration, the GA generates a population of possible configurations for the surgical robotic instrument. At each configuration, a desired motion task is solved. In this work, we are specifying three motion tasks, defined by μ = 1, 2, 3 and simulating the resection of three circular tumors at different locations, but using the same insertion point. Fig. 8 shows some snapshots of the different motion tasks, for two different base configurations. The purpose of using three different tumors is to make the optimization framework generalizable to different motion tasks. If only one motion task is assigned, the optimal configuration may not be suitable for a different task. As described in Section IV-A, each motion task is divided into two subtasks to guarantee the RCM motion and accurate path tracking, and solved at discrete time instants by means of (12). At each instant, the cost and dexterity measures are computed, as described in the following. These values will then be used in the fitness function for the GA. Once a motion task is completed, the system moves on to the next one, and when all the motion tasks are completed, the total fitness function is computed and passed to the GA for the generation of the next population. The procedure keeps running until the GA meets the stopping criteria.
1) GA Specifications: For the optimal configuration problem, the P opulation consists of individuals described by a set of RP Y angles, which define k R b . Because of the robot design, the Roll and Pitch are limited between ±90 • , whereas the yaw can vary between ±180 • . Moreover, each RPY angle could be considered to be continuous, meaning RP Y ∈ R 3 . However, fractions of angles may be difficult to achieve in a real world due to manufacturing tolerances. Therefore, in this work, we consider each angle to be a discrete integer number and RP Y ∈ Z 3 , which guarantees a resolution of 1 • . The optimization problem then becomes a mixed-integer GA.
Due to this discretization, the optimal configuration may be retrieved by simply searching throughout the domain of the optimization variables. This, however, would require 181 2 × 361 = 11 826 721 explorations, which might be unreasonable to solve. The GA, instead, may find the best solution by evaluating smaller sets, since at each generation, a new P opulation is created. If available, a priori knowledge about the robot model could be exploited to reduce the search space. Table I reports the values of the properties used to set up the GA optimizer in MATLAB, according to the description presented in Section III-B. For the generation of the population, Uniform function is used, which creates a random initial population with a uniform distribution, with the population always within the imposed range.
MATLAB Rank and T ournament Selection functions are used to rank each individual based on the value of its fitness function and to select the parents from the population. The use of the selection function is imposed by MATLAB, given the mixed-integer nature of the optimization.
For generating the children from the individuals in the current populations, 20% of all the individuals are chosen to be passed directly to the next population as Elite Children. Among the remaining individuals, 80% are obtained from the Crossover F unction, using the Heuristic function, and 20% from the M utation F unction, using the Gaussian function to generate random children.
Finally, for the stopping criteria, the maximum number of generations is set to 100, and the maximum number of Stall Generations is set to 10.
2) Fitness Function: The GA will find the optimal RP Y that minimize a certain fitness function. This function should take into consideration different factors such as the RCM motion, the tip positioning, and the robot dexterity in performing the desired motion tasks.
To perform the path tracking for each motion task μ = 1, 2, 3 by using the stack of tasks described in Section IV-A, the path is discretized in N t time steps, and (12) is used to find the joint commands at each time step. It is worth mentioning that the number of time steps is kept the same for each mounting configuration. The tracking accuracy can, therefore, be expressed in terms of the value of the cost function achieved by solving (12). Assuming thatq * is the optimal joint commands from (12) when both the RCM and the path tracking subtasks are solved, the cost at each time step t = 0 . . . N t is computed as with each c t ∈ [0, ∞]. This formulation allows to also take into account the positioning errors at each instant, and at best, each c t = 0. For each motion task and for each RP Y set, the averagec μ and the maximum c μ M cost can be obtained as t=0 c t and c μ M = max(c 0 , . . . , c N t ). On each motion task μ, the best RP Y configuration is the one that minimizes bothc μ and c μ M . In fact, just considering the average cost may not be enough, as some configurations may lead to high error peaks. Therefore, the cost fitness function, for each configuration, is defined as which tends to penalize configurations with high cost peaks. Beside the motion accuracy, it is also important to have configurations that improve the dexterity of the system. Generally, the manipulability ellipsoid [39] is used in robotics to assess the dexterity of a system. An ellipsoid is defined as v T (H) −1 v ≤ 1, with the singular values of H being the square of the length of its semiaxes. The manipulability ellipsoid is defined as the set of joint velocities such thatq Tq ≤ 1, which from the solution of (8), leads toḣ T (JJ T ) −1ḣ ≤ 1. Different measures exist to assess the dexterity of a robot, as reported in [40]. This derivation, however, applies only if one single subtask is specified and does not consider joint limits. Therefore, in this work, the following modification is carried out.
If joint limits were not imposed, the solution of (12) would be the same as the one obtained from the gradient projection method (as in Section III-A) and would be defined by [41] with P 1 = I − J † 1 J 1 , andḣ 1 , J 1 correspond to the RCM subtask, andḣ 2 , J 2 to the path tracking subtask. In order to take into account joint limits, a penalization function is used [40]. Each Jacobian column j = 1 . . . 12 is penalized for each subtask n = 1, 2 such that J n,j = s j J n,j where K is a positive constant. This allows the contribution of a specific joint to be zero when it reaches its joint limits. From the definition of the manipulability ellipsoid, following the Jacobian penalization, and from (18), the multisubtask manipulability ellipsoid can be defined aṡ with H ∈ R 8×8 , since it includes the 2-D RCM subtask and the 6-D path tracking subtask. If only one single subtask is specified, this formulation reduces to the traditional manipulability ellipsoid formulation. To assess the system dexterity, the inverse condition number is used [40]. Due to the time discretization of the motion, at each time step, the dexterity measure is defined as with δ t ∈ [0, ∞] and σ H,m , σ H,M are the minimum and maximum singular values of H. For each motion task and for Fig. 9. Example of the cost and dexterity computation for each time step t = 0, . . . N t of each motion task, for a given RP Y robot configuration.
t=0 δ t and δ μ m = min(δ 0 , . . . , δ N t ) are computed. To choose the RP Y with best dexterity, the dexterity fitness function is formulated as This function is again computed on each motion task μ and for each configuration. Configurations with high dexterity, will lead to highδ, δ m , and f d . Fig. 9 shows two examples of the cost and dexterity computation for a given motion task and a given RP Y configuration.
In order to find the RP Y configuration with the best dexterity and smallest cost, thus maximizing both f μ c and f μ d on each motion task, the overall motion task fitness function is defined Finally, the total fitness function that the GA will minimize is set to be with the negative sign being added to ensure the minimization of the fitness function.
3) Resilience to Errors: One issue that may arise from this optimization is that the optimal configuration identified by the solver may reside in a region where small deviations from the optimal may lead to much worse performance, yielding higher costs and lower dexterities. This is what could happen in case of positioning errors during the assembly on the real system. In order to overcome it, the following adjustment is made.
Given a robot configuration RP Y 0 , a neighboring region to this configuration is considered. This region is defined by a minimum (l m ) and maximum (l M ) size along each dimension. Due to the discretization of the angles, the size of the region is computed as l = l m + n l , with n l ∈ N ∩ [0, N l ], N l = l M − l m ∈ N + being the maximum number of intervals. This means that to each initial configuration corresponds a region R of N l * 2 dim neighboring configurations RP Y s , with dim = 1, 2, 3, depending on how many angles are optimized. Fig. 10 shows an example of the neighboring region when dim = 1 (only one angle optimized) and dim = 3.
For each neighboring configuration RP Y s in R, each motion task μ is solved andc, c M ,δ, and δ m are computed. Then, the deviation of the costs and dexterities of each neighboring configuration with respect to the initial configuration RP Y 0 is computed. The initial configuration is associated Fig. 10. Neighboring region R for a given configuration RP Y 0 when optimizing for one angle (dim = 1) and for all RPY angles (dim = 3).

Algorithm 1: Algorithm for Penalization of the Costs and
Dexterities Based on Neighboring Region. This is Implemented for Each Motion Task μ.
Since the optimal RP Y configuration should reside in a region with small costs and dexterities variations, the maximum deviations Δ μ 1 , Δ μ 2 , Δ μ 3 , Δ μ 4 in the region are computed and used to penalize the cost and dexterity values of the current initial configuration. The penalized costs and dexterities are then used to compute the fitness functions F for the GA. In this way, configurations leading to large deviations will be associated with higher penalized costs and smaller penalized dexterities. Algorithm 1 describes the penalization process.

V. RESULTS
In this section, the results of the proposed method are reported, both for a simulated test and real-world experiments.

A. Simulation Results
The proposed approach is employed to find the optimal configuration when only optimizing for one angle and when optimizing for the whole RPY set of angles. In the simulation test, the macro-micro manipulator needs to cut three circular tumors of 15 mm in radius in different locations with respect to the KUKA base frame. For the tracking subtask, the end-effector tip is required to be always perpendicular to the tumors' plane, and for all the tumors, the same insertion point is used, specified with respect to the KUKA base frame.
1) Roll Optimization: If only one single angle is required to be optimized, the GA optimization may be redundant. For this test, only the Roll angle is optimized, with the pitch and yaw set to 0 • . The Roll angle about the x-axis of KUKA end-effector's frame, due to the robot design, needs to be limited in the range [−90, 90] • . If a resolution of 1 • is wanted, then the Roll angle can be discretized in 181 points. A simple space search would be able to find the optimal solution. Using the GA, with a population of 181 individuals, the algorithm still finds the same optimal solution, yet in more iterations (11 in total), since it needs to meet the stopping criteria, such as reaching the maximum number of Stall Generations. Fig. 11 shows the optimization results for costs (c, c M ) and dexterities (δ, δ m ) on the three motion tasks and Fig. 12 shows the distribution of the fitness function, in logarithmic scale ,  TABLE II  COST AND DEXTERITY VALUES FOR THE OPTIMAL ROLL CONFIGURATIONS  WHEN PERFORMING THE THREE MOTION TASKS µ = 1, 2, 3 for the different Roll angles. In this scenario, the size of the neighboring region is chosen to range from 1 • to 5 • . The optimal Roll angle, allowing to have the best performance over the three motion tasks, results to be Roll opt = −76 • to which corresponds a fitness function value of −log 10 (|F |) = −7.45. Table II reports the costs and dexterities when using the optimal configuration Roll opt . The low values of the dexterity measures are due to the constraint imposed by the RCM: if the same motion task was specified without taking into account tracking the RCM, larger dexterity values would be achieved. The optimal configuration Roll opt found by the GA optimizer does not always lead to optimal costs or dexterities. This is mainly due to two reasons. First, the fitness function is a combination of all the measures used (average and minimum dexterity, average and maximum cost), and minimizing for one measure may lead to a deterioration of another measure. A second reason is the penalizations introduced as described in Section IV-C3. For instance, in motion task 1, the Roll angle of 5 • leads to the highest average dexterity. However, this configuration is not resilient to errors: small deviations from it will lead to great dexterity losses. The same applies to the other measures. The optimal configuration, instead, always resides within a region with small deviations. If the resilience to the errors was not considered in the optimization framework, the optimal Roll angle would be −56 • . Even though this configuration might have higher dexterities in each of the motion tasks, compared to Roll opt , it does not reside within a safe region.
It is interesting to notice that the configurations with highest dexterities and smaller costs are those with negative Roll angles. Indeed, Fig. 13 shows that the configurations with positive Roll, have larger number of joints reaching their limits during the motion, thus limiting the system's capabilities in performing the desired motion tasks.
2) RPY Optimization: If the whole set of optimal RPY angles is sought, the GA becomes more useful. In fact, a space search would require to go through millions of configurations, thus requiring long computational time. For the optimal RP Y , a population of 500 individuals was used and the size of the neighboring region was set to 5 • . The GA converged to the optimal configuration RP Y opt = [−85, −67, 87] • , corresponding to −log 10 (|F |) = −8.20, in 33 iterations. Table III reports the cost and dexterities associated with the optimal RPY configuration on the three different motion tasks, whereas Fig. 14 shows the approximated distribution of the costs and dexterities for different RPY configurations in the different motion tasks, and Fig. 15 the absolute value (in logarithmic  scale) of the fitness function F . The plots were created using around 10 000 configurations and interpolating the values of the measures within these samples. Even though this is an approximation to visualize the results, the solution of the GA is still a more optimal configuration among the interpolated ones. This is indeed given by the fact that over the whole process, the GA explored 13 300 configurations, given that it ran for 33 iterations generating 400 new individuals at each iteration. Also in this scenario, the optimal configuration identified by the GA algorithm does not always minimize each singular measure. However, the penalizations allow retrieving configurations with small costs and acceptable dexterities, lying in regions with small variations, thus being resilient to errors. Table IV reports the maximum deviations in the neighboring region for RP Y opt .

B. Experimental Results
In the real case scenario, the macro-micro manipulator needs to cut a single circular tumor of 10 mm in radius located at [39,26,10]mm from the KUKA base with a constant orientation, perpendicular to the tumor's plane, and with the RCM at [39,26,17]mm with respect to the KUKA base frame. The simulated environment was used as a testbench for our proposed framework to identify the optimal configuration for this new desired motion task. The purpose of the experiment is to compare the results with the original, currently set, surgical robotic instrument's base configuration RP Y = [−90, 0, −180] • , and the optimized one. Fig. 16 shows the setup for the real-world experiments and the two connections tested. A human plastic phantom was used to simulate a human body and place the desired insertion point. Two 6-DOF NDI AURORA electromagnetic (EM) trackers, providing both 3-D position and 3-D orientation data, have been used: one at the surgical robotic instrument's base, to track the shaft; one at the robot's end effector to track the robot tip. The accuracy of the sensors is of 0.01 mm. The EM tracker on the surgical robotic instrument's shaft is used to estimate the  pose of the shaft, and from that, to measure the RCM tracking error along perpendicular directions of the shaft, as described in Appendix A. For the real test, we optimized only for the Roll and Pitch angles, since the Yaw angle can be achieved with the last joint of the KUKA robot, specifying a population of 500 individuals. Fig. 17 shows the convergence of the GA algorithm to the optimal solution RP Y opt = [127, 41, 0] • . The presence of model uncertainties in the Micro-IGES and some manual initial setting operations that need to be carried out (like bringing it back to its home configuration or the initial alignment with the hole) affect the performance of the system, even when repeating the same task. Therefore, the experiment was run five times in order to compare the macro-micro manipulator's performance when using the original and the optimized configurations.  and P,max are the root mean squared error (RMSE) and the maximum value of the norm of the tip positioning error; (¯ θ ) is the RMSE of the orientation error (expressed in angle axis notation), and ( θ,max ) the maximum norm of the angular error; ( RCM ) the RMSE in tracking the RCM; andδ and δ m the average and minimum dexterity respectively. Because of the calibration errors, inaccuracies in the kinematic models, and other nonlinearities not fully compensated (such as hysteresis and backlash in the Micro-IGES surgical robotic instrument), each test leads to slightly different results and to worse performance than in the simulated scenario. However, overall both configurations yield good tracking accuracy, with errors in the order of few millimeters. In terms of path tracking, the original configuration RP Y = [−90, 0, −180] • achieves smaller average tip positioning and orientation errors, yet larger errors occur in tracking the RCM, compared to the optimal configuration RP Y = [127, 41, 0] • . With regards to the dexterity, the optimal configuration ensures higher dexterity measures, resulting also from a lower number of joints reaching their limits. Due to the chosen fitness function, the optimal configuration ensures a good tradeoff between tracking accuracy and dexterity. Our testbench simulation environment would predict a fitness function F = −11.7 × 10 6 for the optimized configuration, compared to F = −18.8 · 10 2 for the original one. Yet, the inherent modeling errors do not allow achieving those performance.

VI. CONCLUSION
Achieving safe and accurate execution of a specified surgical task, while satisfying the RCM and additional constraints, is of uttermost importance in MIRS. The control technique should be capable of ensuring high accuracy, while not violating the imposed constraints. The proposed HQP framework can be implemented as it allows solving prioritized subtasks while additional constraints are imposed.
In order to guarantee RCM motion, macro serial-link manipulators can be employed in conjunction with microsurgical robotic instruments. However, the connection and placement of the surgical instrument on the macromanipulator may affect the performance of the system, while performing the specified tasks. Therefore, an optimal configuration for the connection should be sought.
In this article, we presented a novel framework to identify the optimal configuration of the base of a surgical robotic instrument with respect to the serial-link manipulator's end-effector frame preoperatively. This frameworks exploited the capabilities of the HQP control strategy that allowed a highly redundant robotic system, consisting of a macro serial-link KUKA manipulator and the Micro-IGES surgical robotic instrument, to effectively perform specified motion tasks, while guaranteeing the RCM and additional constraints on the robot joint limits. To identify the optimal base configuration, the GA was employed, thanks to its capabilities to explore large search spaces.
Simulation results showed that some mounting configurations do not allowed achieving a high performance in terms of dexterity and accuracy, thus justifying the need to find an optimal configuration for the connection between the two robotic systems. The proposed framework proved effective in finding optimal configurations, both when optimizing only for the Roll angle and for the whole RPY angles, which guarantee small tracking errors of the desired paths, satisfaction of the RCM motion constraint, and maximization of the dexterity of the whole macro-micro manipulator. Moreover, the proposed strategy to make the optimal solution resilient to possible assembly errors was shown to be essential to ensure that the optimal configuration resides within a region where small variations in the tracking costs and dexterities occur.
Experimental results were carried out to test the performance on the real robotic system. Even though the simulation environment was predicting good performance for the optimized configuration, the inherent calibration and modeling errors in the system still affected the resulting performance.
In this work, the proposed framework was employed to only optimize for the surgical robotic instrument's base configuration, while considering the base of the serial-link manipulator, the insertion point, and the RCM fixed. However, the framework can be generalized to find the optimal insertion point placement or to include the serial-link manipulator's base configuration in the optimization. Moreover, for the RPY optimization, a population of 500 individuals was used. A larger number of individuals may lead to a better optimal solution, even though the computational time will be highly increased. The main limitation of the framework was that it is task-specific, therefore, a new optimal configuration may need to be recomputed and a new   connection between the two robots designed when the surgical task changes. However, on one hand, since the optimization was run offline and thanks to the modern 3-D printing techniques, a new design can be easily made and used to improve the system's performance. On the other hand, the framework can be used to assess the capabilities of the macro-micro manipulator based on the available connections between the two robots for a given surgical task. To make the framework more generalizable, different surgical tasks can be specified so that the optimal configuration is the one that performs the best overall.
The simulation analysis and the experimental tests will be considered to design a new reconfigurable attachment for the correct assembly of the Micro-IGES surgical robotic instrument on the KUKA robot that could be applied to different tasks.

APPENDIX A RCM KINEMATICS
In order to compute the kinematics of the RCM, given the kinematic model of the whole macro-micro manipulator, it is sufficient to know the desired pose of the RCM, 0T RCM , with respect to the macro-micro manipulator base frame. The estimated position of the RCM, however, needs to be projected onto the shaft of the surgical robotic instrument. Given the pose of the base of the surgical robotic instrument k T b with respect to the KUKA end effector, the pose of the shaft can be computed as 0 T b = 0 T k k T b and the pose of the RCM in the shaft frame will be b T RCM = 0 T −1 b 0T RCM . In our case, z b , the z-axis of the surgical robotic instrument's base, is aligned with the shaft, and since the RCM needs to be on the shaft, it is sufficient to project the actual RCM position on to the shaft to extract the expected RCM position as: b P RCM = z T b bP RCM z b . The expected position of the RCM in the macro-micro manipulator's base frame will then be 0 P RCM = 0 P k + 0 R k k R b b P RCM . Fig. 18 shows a representation for the computation of the expected position of the RCM.
Thanks to this formulation, it is also possible to easily compute the error in tracking the RCM as e RCM =P RCM − P RCM , which is then used in the control scheme to improve the tracking accuracy. In the real experiments, the shaft pose is measured by the EM tracker at the surgical robotic instrument's base.

ACKNOWLEDGMENT
Data supporting this study are openly available on the Robot Intelligence Lab website https://www.imperial.ac.uk/ robot-intelligence/. For the purpose of open access, the author has applied a Creative Commons Attribution (CC BY) licence to any Author Accepted Manuscript version arising.