On the Kinematics of Robotic-assisted Minimally Invasive Surgery

Minimally invasive surgery is characterized by the insertion of the surgical instruments into the human body through small insertion points called trocars, as opposed to open surgery which requires substantial cutting of skin and tissue to give the surgeon direct access to the operating area. To avoid damage to the skin and tissue, zero lateral velocity at the insertion point is crucial. Entering the human body through trocars in this way thus adds constraints to the robot kinematics and the end-effector velocities cannot be found from the joint velocities using the simple relation given by the standard Jacobian matrix. We therefore derive a new Jacobian matrix which gives the relation between the joint variables and the end-effector velocities and at the same time guarantees that the velocity constraints at the insertion point are always satisfied. We denote this new Jacobian the Remote Center of Motion Jacobian Matrix (RCM Jacobian). The main contribution of this paper is that we address the problem at a kinematic level and that we through the RCM Jacobian can guarantee that the insertion point constraints are satisfied which again allows for the controller to be implemented in the end-effector workspace. By eliminating the kinematic constraints from the control loop we can derive the control law in the end-effector space and we are therefore able to apply Cartesian control schemes such as compliant or hybrid control.


Introduction
The use of robots for surgical procedures has grown into one of the most promising applications of robotic technology in health care.One of the reasons for this is the use of robots in minimally invasive surgery (MIS), which is characterized by the insertion of the tools through small holes in the patient's body.MIS leads to less patient trauma, shorter recovery times and lower overall risk compared to conventional open surgery.
Robotic-assisted minimally invasive surgery (RAMIS) is performed with a robotic manipulator whose end effector is attached to a long and thin shaft used to penetrate the skin through a trocar.To avoid damaging the patients' tissues at the insertion point, it is vital that the lateral displacements at this point is kept to a minimal.This adds a kinematic constraint to the structure, often referred to as the Remote Center of Motion (RCM).
These kinematic constraints can be implemented either mechanically or in the software.Mechanical constraints can be imposed by designing a parallel kinematic structure for which the shaft pivots about the RCM (Locke and Patel, 2007;Sun and Yeung, 2007;Hannaford et al., 2013).Alternatively, the kinematic constraints can be implemented in the controller.For standard open-chain manipulators these constraints cannot be added through the mechanical design, so the only way to guarantee that the constraints are satisfied is through the controller.The main advantage of this approach is more flexibility when it comes to changing the RCM quickly, and it also allows the robot to follow the motion of a moving RCM, for example if the patient is breathing.The main disadvantage is that additional safety systems need to be implemented to guarantee that the RCM constraints are not violated in case of system breakdown.In both cases, whether the constraint is passive or active, the mapping from joint space to the Cartesian space is needed for control purposes.
Several researchers have addressed the problem of imposing the RCM constraints on the robot motion by modifying the controller.Early results solved the motion constraints as an optimization problem, for example in Funda et al. (1996) and Li et al. (2005).In Ortmaier and Hirzinger (2000) the RCM kinematics is derived and used to estimate the position of the entry point for a robot with passive joints.The passive joints guarantee that no forces are exerted to the entry point.In Locke and Patel (2007) the kinematic model is used to derive an optimization technique that allows isotropy of the surgical tool to be evaluated subject to the RCM constraint.Trocar kinematics is also discussed in Lenarčič and Galletti (2004).Azimian et al. (2010) and Azimian (2012) use the concept of task priority and restricted Jacobian to derive the constrained motion in terms of the trocar and manipulator geometry.The end-effector motion is found in the standard way from the manipulator Jacobian, which is taken from the null space of the constraint Jacobian of the entry point.The constraint Jacobian is found in the normal way by the mapping from the joint space velocities to the lateral linear velocities of the RCM point.The constraints at the insertion point are given first priority and the end-effector motion is given a secondary priority as this is taken from the null space of the first Jacobian (Nakamura, 1991).The approach depends on the kinematics of both the robotic manipulator and the trocar.
In this paper we take a somewhat different approach in that we impose the constraints on the velocity twist of the last link of the robot directly and rewrite the mapping from the end-effector twists to the robot twists so that it is guaranteed to satisfy the RCM constraints.The control problem is thus formulated in the end-effector space and the velocities are then mapped to the robot twist, which is effectuated in the normal way by the low-level robot controller.We thus obtain a formulation that is independent of the robot kinematics which allows for simple implementation as we can use existing low-level controllers both for the robot and the wrist/end-effector.
It is well known that the location of the trocar and the pose of the robot are critical factors when it comes optimizing the manipulability, dexterity, reachability, and visibility of the robot Sun and Yeung (2007).As these factors can significantly enhance the overall performance of the system, it is desirable to obtain a system in which the trocar location and the robot pose can be chosen actively both before and during the surgery.We therefore opt for an analytical approach, as opposed to the null-space approach discussed above in order to be able to control these factors more actively.
Interaction control during robotic surgery is a critical procedure due to the risk of damaging the patient's tissue and organs.The complexity of surgical operations often requires hybrid control schemes applying a combination of motion and force control to allow for both stiff or direct force control in the directions associated with the task and compliant and indirect force control in the other directions.This will result in an efficient and at the same time a safe control scheme well suited for robotic surgery.
The proposed control scheme is developed so that the control law can be implemented in the Cartesian space.This allows for efficient implementation of hybrid control schemes (Cho et al., 2012) while the insertion point constraints are taken care of by the modified manipulator Jacobian denoted the Remote Center of Motion Jacobian Matrix (RCMJ).We are therefore able to apply the standard approaches for Cartesian control used for conventional robotic manipulators found in literature.
The paper is organized as follows: A system overview of the robotic system discussed and the problem formulation are presented in Section 2. In Section 3 the kinematics of the system subject to the Remote Center of Motion constraints are derived.The dynamics of the system is discussed briefly in Section 4 and some comments regarding the control problem are presented in Section 5. Concluding remarks are presented in Section 6.

System Overview and Problem Formulation
The system discussed in this paper consists of a standard or customized 6-DoF robotic manipulator with a shaft, i.e., a thin long link used for inserting the endeffector into the body through the trocar.At the end of the shaft there is a wrist with two or more additional degrees of freedom and a tool.At the insertion point we will require that the sideways velocities are eliminated to prevent the robot from damaging the patient's tissue.This requirement imposes a 2-DoF constraint on the shaft so that the end of the shaft has 4 DoF of motion.The additional degrees of freedom in the wrist give the end effector a full 6-DoF motion.Most endoscopic wrists, such as the one used in the Intuitive Surgical's da Vinci robots, have two or more additional degrees of freedom.This paper is not concerned with redundancy, so we consider the case where the wrist has 2 DoF.The system setup and the configuration spaces used in this paper are shown in Figure 1.
The problem considered in this paper consists of deriving the kinematics of the robotic system subject to the kinematic constraints at the entry point.This can then be used to obtain a stiff control of zero velocity at the insertion point while allowing for a combination of stiff and compliant control at the end-effector.This kind of Cartesian control schemes require the state space to be written in terms of the end-effector variables and the kinematic constraints need to be eliminated from the control law.In this paper we thus endeavor to convey a mapping from the end-effector space, in which the high-level controller is derived, to the joint space of the low-level control, which satisfies the Remote Center of Motion constraints.

Constraint Kinematics
The main objective of this section is to find a workspace representation suited for controlling the end-effector motion and which at the same time guarantees zero lateral velocity at the insertion point.To obtain a unified approach satisfying both control objectives will require both the velocities at the insertion point as well as a well-defined set of end-effector velocities to be included in the state space formulation.We will see that due to the kinematic constraints at the insertion point, it is not straight forward to find a state space representation suited for control.Because the mapping from the joint variables to the end-effector space also needs to take the constraints into account we cannot use the Jacobian in the standard way.We will therefore introduce new velocity variables that do not have a straight forward geometric interpretation, but allow us to include the constraints in the Jacobian and find a mapping from the joint velocity space to the constrained end-effector velocity space.

Notation
We will denote a mapping from the inertial frame F 0 to a moving frame F a by the homogeneous transformation matrix where R 0a is the rotation matrix that gives the orientation of F a with respect to F 0 , and p 0a is the vector from F 0 to F a .The velocity of F a with respect to F 0 is given in body coordinates as a twist where ωB 0a ∈ R 3×3 is the skew-symmetric representation of the angular velocities ω B 0a ∈ R 3 and v B 0a ∈ R 3 is the linear velocity.V B 0a ∈ R 4×4 is thus the matrix representation of an element of the Lie algebra.The vector representation of the same twist is denoted V B 0a ∈ R 6 .For a third frame F b that relates to F a through the homogeneous transformation matrix g ab we can find its motion with respect to the inertial frame F 0 by g 0b = g 0a g ab .The body velocity V B 0b of the frame F b with respect to F 0 is given by where Ad g ba is the Adjoint map given by We refer to From et al. (2013) for more details on this topic.

Robot Arm Motions
We will attach a frame F r to the last link of the 6-DoF robotic manipulator, as illustrated in Figure 1.The body velocities of this frame with respect to a fixed inertial frame F 0 is represented by The robot velocities can be found from the joint velocities by the Jacobian in the standard way as V B 0r = J B r (q r ) qr where J B r (q r ) is the geometric Jacobian relating the joint velocities and the body twist of the end effector.One of the control objectives is to maintain zero translational velocity at the entry point.We will thus also define a reference frame F p at this point with the same orientation as F r , i.e., R rp = I.We denote the body velocities of this frame as The reference frames are illustrated in Figure 1.The relation between the velocity at the last link of the robot and the entry point, i.e., the point p rp = 0 0 a T in frame F r is given by the simple relation Robot Frame -F r Motion space: SE(3) Insertion Point Frame -F p Constraints: R 2 Motion space: End-effector Frame -F e Motion space: We also attach a wrist frame F w at the end of the shaft, i.e., the part that is located inside the body, and denote the body velocities as Similarly the velocities at the end effector is given by adding the motion of the shaft and the last joints of the end effector and are denoted x ω e y ω e z T . (9)

Constrained End-effector Motion
We can study the end-effector motions under the assumption that the insertion point constraints are satisfied.In this section we will derive the end-effector motion assuming the insertion point constraints are satisfied, and in the next section we will constrain the end-effector velocity by including the insertion point constraints.The results in this section are thus useful when analyzing and deriving control laws for mechanically constrained manipulators such as the da Vinci and Raven surgical robots (??).
The insertion point constraints can be satisfied either through a control law, for example a simple position control law, or by a mechanical device.In any case the velocity at the insertion point can be written in terms of the velocities at F r as At the end of the shaft we attach the wrist frame F w .
The wrist frame has only four degrees of freedom and can thus be written in terms of the velocities at the last robot link (or alternatively the entry point) as Figure 1: System setup for Robotic-assisted Minimally Invasive Surgery.The configuration spaces at the different frames are shown in terms of linear motion R and rotational motion S.
while the angular velocities are identical: ω B 0p = ω B 0r .We also attach a wrist frame F w at the end of the shaft, i.e., the part that is located inside the body, and denote the body velocities as Similarly the velocities at the end effector is given by adding the motion of the shaft and the last joints of the end effector and are denoted x ω e y ω e z T . (9)

Constrained End-effector Motion
We can study the end-effector motions under the assumption that the insertion point constraints are satisfied.In this section we will derive the end-effector motion assuming the insertion point constraints are satisfied, and in the next section we will constrain the end-effector velocity by including the insertion point constraints.The results in this section are thus useful when analyzing and deriving control laws for mechanically constrained manipulators such as the da Vinci and Raven surgical robots (Sun and Yeung, 2007;Hannaford et al., 2013).
The insertion point constraints can be satisfied either through a control law, for example a simple position control law, or by a mechanical device.In any case the velocity at the insertion point can be written in terms of the velocities at F r as At the end of the shaft we attach the wrist frame F w .
The wrist frame has only four degrees of freedom and can thus be written in terms of the velocities at the last robot link (or alternatively the entry point) as where b is the distance from the trocar to the end of the shaft located inside the body.
Finally the velocity of the end-effector frame F e is found by adding the velocity of the end effector with respect to the wrist frame to the velocity of the wrist frame with respect to the inertial frame, where both need to be expressed in the end-effector frame, as we want the body velocities: To simplify the expressions we assume that the last two joints revolute about the x-axis and we set l 8 = 0. We first write which gives where we have used the short hand notations s and c for sin and cos respectively, and that l 7 cos q 7 cos q 78 + l 7 sin q 7 sin q 78 = l 7 cos (q 7 + q 8 − q 7 ) = l 7 cos q 8 (15) and −l 7 cos q 7 sin q 78 +l 7 sin q 7 cos q 78 = −(l 7 sin q 78 cos q 7 − l 7 cos q 78 sin q 7 ) = −l 7 sin (q 7 + q 8 − q 7 ) = −l 7 sin q 8 .
We can now write The contribution of the last two links of the wrist can be found straight forward by observing that (recall that we assume l 8 = 0) q8 only contributes to angular motion and the q7 contributes to the angular velocity in the same way as q8 and linear velocity which needs to take into account the orientation of the last link.We can find this by the body geometric Jacobian: Here X 7 7 and X 8 8 are the constant velocity twists of joint 7 and 8, respectively, as seen from the joint frames (From et al., 2013).
The body velocity at the end effector is then given by Equation ( 19) on the next page where q 78 = q 7 + q 8 .For most telesurgical systems the wrist is close to a spherical joint so we can assume that l 7 , l 8 << b and we get

Complete State Space Representation
In the previous section we looked at the end-effector motion assuming the insertion point constraints were satisfied.In this section we will include the insertion point velocities in the state space formulation to be able to cancel this motion to zero.The state space can then be written as a vector in R 8 : This formulation leads to the kinematic and dynamic separations as shown in Figure 2.This choice of state 1 0 0 0 l 7 cos q 7 l 7 sin q 7 0 cq 78 sq 78 −l 7 cos q 8 0 0 0 −sq 78 cq 78 l 7 sin q 8 0 0 b + l 7 cos q 7 l 7 sin q 7 sin q 78 −b cos q 78 − l 7 cos q 8 0 0 cos q 78 b sin q 78 + l 7 sin q 8 0 0 0 1 0 0 0 0 cos q 78 sin q 78 0 0 b + l 7 cos q 7 l 7 sin q 7 0 0 sin q 78 −b cos q 78 − l 7 cos q 8 0 0 −l 7 cos q 8 0 cos q 78 b sin q 78 + l 7 sin q 8 0 0 l 7 sin q 8 0 0 1 0 0 1 1 0 0 cos q 78 sin q 78 0 0 0 0 − sin q 78 cos q 78 0 0 bω r y + l 7 cos q 7 ω r y + l 7 sin q 7 ω r z sin q 78 v r z − (b cos q 78 + l 7 cos q 8 )ω r x − l 7 cos q 8 q7 cos q 78 v r z + (b sin q 78 + l 7 sin q 8 )ω r x + l 7 sin q 8 q7 ω r x + q7 + q8 cos q 78 ω r y + sin q 78 ω r z − sin q 78 ω r y + cos variables is very useful when controlling the velocity at the entry point to zero.It is also convenient because it can be found directly from the robot kinematics (first 6 variables) and the end-effector kinematics (last 2 variables).Furthermore, the kinetic energy can be estimated to T and q w = q 7 q 8 T .In general there are some coupling terms, but considering the relatively low velocities normally used during robotic surgery and the low weight of the wrist compared to the robot, this dynamic decoupling is a good approximation.In this sense this is a good choice of state variables.
On the other hand, the end-effector velocities as written in this way are not particularly useful because these are not the velocities that we want to control.A more appropriate choice of state variables for our problem is therefore found as (23) Here we have separated the entry point velocities-for which we want zero velocity-and the velocity of the end-effector-on which we want to derive our control law.This representation of the velocity vector is therefore suitable for both stiff control at the entry point and for example hybrid control of the end effector.
We need to find the entry-point and end-effector velocities in terms of the robot and wrist velocities, i.e., (24) The contribution of the wrist is identical to the previous section while the robot velocity relates to the insertion point and wrist frame velocities by Equation ( 7), which, when we remove the constraints at the insertion 0r is convenient as the robot and end-effector kinematics and dynamics are separated, but the formulation is not suited for control because the end-effector variables are not present.The last formulation of the workspace variables is more suitable for control as it separates the entry point velocities and the end-effector velocity variables, and these can be treated independently.point, becomes The body velocity at the end effector when no constraints are present are found by The details are found in Equation ( 27).We will denote the relation found in Equation ( 27) as We note that if we want to use our model to control both the entry point and end-effector velocity, we need to use the transformation in Equation ( 27), while for analysis of the end-effector motion assuming the constraints are satisfied, it is sufficient to use Equation (18).
For control purposes it is convenient to know the mapping from the end-effector workspace to the joint space.The mapping between the workspace velocities and the joint velocities are then found by the geometric Jacobian J B g,r : We will denote this matrix J eq and write Whenever the entry point velocities are zero we can leave these out and write For our purpose, however, it is more convenient to use the robot frame F r as a reference for the controller instead of the joint velocities directly, as this becomes computationally faster and we can use the conversion to the low-level controllers already available in the robot controller.This is discussed more in Section 5.

Minimal Representation with Insertion Point Constraints
From Equation ( 7) we see that the velocities at the entry point can be written in terms of the robot velocities as and the constraint of zero velocity can therefore be cast into the following simple form where we need to know the distance from the last link of the robot to the entry point, which may be time varying.We can incorporate these constraints in the kinematics by introducing new variables v 1 and v 2 such that Substituting this into Equation ( 27) gives Equation (38).
Now that we have guaranteed that the velocities at the entry point are zero we can remove these and get the representation well suited for workspace control given in Equation ( 39).This representation is suitable for workspace control and at the same time guarantees that the entry point velocity constraints are satisfied.We will denote the matrix in Equation ( 39) that gives us the minimum representation of the end-effector workspace as J m ep and this important transformation as

Implementation
The first step is to find the new joint velocity variables from the desired end-effector motion.This is found by the inverse of the transformation in Equation (39).
Once the new variables in Equation (39) are found, these need to be transformed into a reference for the last robot link F r .
In the controller v 1 and v 2 are realized through the expressions found in Equations ( 36) and (37).This relation can be written as a matrix This expression, together with Equation (39) thus gives a mapping from the desired end-effector velocities to the robot and end-effector velocities.These are easily implemented either by the standard control interface of the robots or by finding the joint velocities through the inverse Jacobian and feeding this to the low-level controllers.
It is in fact this implementation of the new velocity variables v 1 and v 2 into the robot velocities V B 0r that guarantees that the insertion point constraints are always satisfied.It is important to remember that a(t) and b(t) are time varying and should thus be calculated from the manipulator forward kinematics.

Dynamic Equations
To find the dynamic equations we first write the kinetic energy of each of the rigid bodies in the system as 6+2) given as representing the inertia of each link and the total inertia matrix is given by M (q) = 8 i=1 M i (q).Further denote by C(q, q) the Coriolis matrix of the system.The dynamics of the whole system can now be written as M r (q r ) M rw (q) M T rw (q) M w (q w ) qr qw (a + b) + l 7 cos q 7 l 7 sin q 7 0 0 0 cos q 78 sin q 78 −(a + b) cos q 78 − l 7 cos q 8 0 0 −l 7 cos q 8 0 0 − sin q 78 cos q 78 (a + b) sin q 78 + l 7 sin q 8 0 0 l 7 sin q 8 0 0 0 0 1 0 0 1 1 0 0 0 0 cos q 78 sin q 78 0 0 0 0 0 0 − sin q 78 cos q 78 0 0 (a + b) + l 7 cos q 7 l 7 sin q 7 0 0 0 cos q 78 sin q 78 −(a + b) cos q 78 − l 7 cos q 8 0 0 −l 7 cos q 8 0 0 − sin q 78 cos q 78 (a + b) sin q 78 + l 7 sin q 8 0 0 l 7 sin q 8 0 0 0 0 1 0 0 1 1 0 0 0 0 cos q 78 sin q 78 0 0 0 0 0 0 − sin q 78 cos q 78 0 0 − 1 a l 7 cos q 7 l 7 sin q 7 0 0 0 cos q 78 sin q 78 − 1 a (a + b) cos q 78 − 1 a l 7 cos q 8 0 0 −l 7 cos q 8 0 0 − sin q 78 cos q 78 1 a (a + b) sin q 78 + 1 a l 7 sin q 8 0 0 l 7 sin q 8 0 0 0 0 − 1 a l 7 cos q 7 0 0 l 7 sin q 7 0 0 0 cos q 78 − 1 a (a + b) cos q 78 − 1 a l 7 cos q 8 sin q 78 0 −l 7 cos q 8 0 0 − sin q 78 + 1 a (a + b) sin q 78 + 1 a l 7 sin q 8 cos q 78 0 l 7 sin q 8 0 0 a cos q 78 0 0 sin q 78 0 0 1 a sin q 78 0 0 cos q 78 0 0 − 1 a (b cos q 78 + l 7 cos q 8 ) sin q 78 0 −l 7 cos q 8 0 0 1 a (b sin q 78 + l 7 sin q 8 ) cos q 78 0 l 7 sin q 8 0 0 − 1 a (b + l 7 cos q 7 ) 0 0 l 7 sin q 7 0 0 0 − 1 a (b cos q 78 + l 7 cos q 8 ) sin q 78 0 −l 7 cos q 8 0 0 1 a (b sin q 78 + l 7 sin q 8 ) cos q 78 0 l 7 sin q 8 0 0 We see that we have a total of 8 variables (6 for the robotic manipulator and 2 for the wrist).However, for control purposes we want to define only the 6 variables representing the end-effector velocity.We thus need the transformation in the form To find an explicit expression we use Equation ( 2) and find We now write and get the dynamics These equations give us the motion of the end effector in R 6 and the constraint forces that reduce the workspace from R 8 to R 6 .If we are only interested in the motion of the end effector we obtain this by (48) At this stage it is interesting to look at how the motion in R 8 is reduced to a motion in R 6 , as this is not a redundant system with internal motion.For the system at hand we restrict the motion at the entry point so that the two redundant degrees of freedom are reduced by imposing a restriction on the velocity at this point.This constraint is seen in Equation ( 40) which reduced the degrees of freedom of the system from eight to six by imposing the constraints in Equation (36-37).The resulting equations in the end-effector variables V B 0e are then given by Equation ( 48).
We also note that the reformulation of the dynamics in this way requires the manipulator Jacobian.Thus, a low-level control scheme of this kind is dependent of the manipulator kinematics, which is of course always the case, while the kinematic formulation presented in the previous section, and also studied in the next section, becomes a high-level controller and is thus independent of the manipulator kinematics, as we will see.In this case the inverse kinematics is handled by the robot controller.

Cartesian Control
In Section 3 we found a state space representation well suited for implementing different control schemes at the entry point and the end effector by separating the workspace variables.This can for example be used to obtain a stiff behavior at the entry point and compliant or hybrid control at the end effector.In this section we will briefly look at how the kinematic formulation derived in this paper can be used in control.

Position Control
We will first look at a simple position control scheme, i.e., we want the end effector to follow a pre-defined path, in addition to satisfying the constraints at the insertion point.Stiff control of this kind is necessary in many applications where the end effector is to follow a reference path as closely as possible.
The easiest way to use the RCM Jacobian is to simply transform the desired end-effector motion into a joint trajectory which is guaranteed to satisfy the entry point constraints.This is obtained through A joint space controller can then be implemented in the normal way.This control law is shown in Figure 3.For trajectory following an outer loop should be added for robustness purposes.For teleoperation, however, this is normally not required, as the operator compensates for drift in the position.

Hybrid Control
If a force sensor is available at the end effector we can simply design a controller that minimizes the interaction forces, like the Natural Admittance Controller (NAC) presented by Newman and Zhang (1994) or other end-effector force control schemes.Using the force measurements we can then generate one control signal to minimize the interaction forces and one signal to follow the desired trajectory (Newman and Zhang, 1994).Combining this with the kinematic constraints at the entry point will result in an extension of the 4 DoF approach presented in Deal et al. ( 2012) to a complete 6-DoF representation of the end-effector workspace.Alternatively an impedance controller can be applied, i.e., we define the desired characteristics for the interaction forces in terms of a mass-spring-damper system.
Similarly, a standard hybrid control law can be implemented as in Figure 4. Several tasks in robotic telesurgery require the end effector to interact with the environment, for example in cutting and suturing.From, "Kinematics of Minimally Invasive Robotic Surgery" by the use of the RCM Jacobian matrix joint trajectory which is guaranteed to satisfy the entry point constraints.This is obtained through A joint space controller can then be implemented in the normal way.This control law is shown in Figure 3.For trajectory following an outer loop should be added for robustness purposes.For teleoperation, however, this is normally not required, as the operator compensates for drift in the position.

Hybrid Control
If a force sensor is available at the end effector we can simply design a controller that minimizes the interaction forces, like the Natural Admittance Controller (NAC) presented by ? or other end-effector force control schemes.Using the force measurements we can then generate one control signal to minimize the interaction forces and one signal to follow the desired trajectory (?).Combining this with the kinematic constraints at the entry point will result in an extension of the 4 DoF approach presented in ? to a complete 6-DoF representation of the end-effector workspace.Alternatively an impedance controller can be applied, i.e., we define the desired characteristics for the interaction forces in terms of a mass-spring-damper system.
Similarly, a standard hybrid control law can be implemented as in Figure 4. Several tasks in robotic telesurgery require the end effector to interact with the environment, for example in cutting and suturing.This will in many cases require the combination of stiff and compliant control in the different directions of the end-effector frame.
For this kind of control schemes we first define a set of constraints, which correspond to the natural constraints of ? and ?.These restrict the allowed motion of the end-effector with the objective not to harm the patient.Then, the reference generated by the operator is treated as an artificial constraint, i.e., the motion is free but guaranteed to always satisfy the natural constraints.One example of hybrid control is shown in Figure 4. We refer to ? and ?for more details.

Impedance Control with Insertion Point Constraints
Impedance control for minimally invasive surgery is challenging because the impedance control needs to be implemented in the end-effector space while the constraints on the robot motion needs to be so that the velocities at the insertion point are zero.One solution to this problem is shown in Figure 5.The desired motion is given by the master velocities V B 0e,d .For impedance control to be useful it needs to be implemented in the workspace variables.We will define a compliant frame F c which gives the position and orientation of the end effector when it is in contact with the environment, i.e., the deviation from the desired frame F d due to the sensed end-effector forces (?).When the end effector is in contact with the environment it will thus follow the frame F c which relates to the desired trajectory frame F d by where p dc is the position of F c with respect to F d and M c , D c , and K c define the mass, spring, and damper system for a force F e .This gives a new desired motion given by the frame F c when the robot is in contact with the environment.We see that when there is no contact we have p dc = 0 and get F d = F c .as expected.
We also need to guarantee that the velocities at the insertion point are zero.This is guaranteed by introducing the variables v 1 and v 2 as in Equation ( 39).The matrix (J m ep ) −1 thus gives us the motion of the manipulator arm for which the constraints are satisfied.We now give this as input to the robot arm, together with the wrist motion, also found by Equation ( 39) Note that we have separated the feedback loops for the manipulator arm and the wrist, as shown in Figure 5.We have obtained compliant control in the endeffector workspace which also guarantees that the insertion point constraints are satisfies, as required.
Note also that we only use the velocity variables in the controller.This is not a problem in teleoperation, as the position variables are normally compensated for by the user, and we are mainly interested in following the velocity reference.In the impedance controller, For this kind of control schemes we first define a set of constraints, which correspond to the natural constraints of Craig and Raibert (1979) and Mason (1981).These restrict the allowed motion of the end-effector with the objective not to harm the patient.Then, the reference generated by the operator is treated as an artificial constraint, i.e., the motion is free but guaranteed to always satisfy the natural constraints.One example of hybrid control is shown in Figure 4. We refer to Craig and Raibert (1979) and Mason (1981) for more details.

Impedance Control with Insertion Point Constraints
Impedance control for minimally invasive surgery is challenging because the impedance control needs to be implemented in the end-effector space while the constraints on the robot motion needs to be so that the velocities at the insertion point are zero.One solution to this problem is shown in Figure 5.The desired motion is given by the master velocities V B 0e,d .For impedance control to be useful it needs to be implemented in the workspace variables.We will define a compliant frame F c which gives the position and orientation of the end effector when it is in contact with the environment, i.e., the deviation from the desired frame F d due to the sensed end-effector forces (Natale (2003)).When the end effector is in contact with the environment it will thus follow the frame F c which relates to the desired trajectory frame F d by where p dc is the position of F c with respect to F d and M c , D c , and K c define the mass, spring, and damper system for a force F e .This gives a new desired motion given by the frame F c when the robot is in contact with the environment.We see that when there is no contact we have p dc = 0 and get F d = F c .as expected.
We also need to guarantee that the velocities at the insertion point are zero.This is guaranteed by introducing the variables v 1 and v 2 as in Equation ( 39).The matrix (J m ep ) −1 thus gives us the motion of the manipulator arm for which the constraints are satisfied.We now give this as input to the robot arm, together with the wrist motion, also found by Equation ( 39) Note that we have separated the feedback loops for the manipulator arm and the wrist, as shown in Figure 5.We have obtained compliant control in the endeffector workspace which also guarantees that the insertion point constraints are satisfies, as required.
Note also that we only use the velocity variables in the controller.This is not a problem in teleoperation, as the position variables are normally compensated for by the user, and we are mainly interested in following the velocity reference.In the impedance controller, however, we need both the acceleration and position variables.We therefore need to include a memory in the impedance controller so that the position can be recovered whenever spring forces are required.

Conclusion
This paper derived the kinematics of robotic manipulators subject to remote center of motion (RCM) constraints.The control law is derived in the Cartesian end-effector space which allows for conventional control schemes such as compliant and hybrid control to be implemented also in the presence of RCM constraints.We solved the constraints at a kinematic level, i.e., we find a Jacobian matrix that maps the end-effector velocities into a new set of velocity variables on which we impose a structure that guarantees that the remote center of motion constraints are satisfied.This modified Jacobian matrix, denoted the RCM Jacobian, can thus be used to find the desired joint velocities from a desired end-effector velocity and the trocar location.Because the new velocity variables are easily mapped to the robot and wrist joint space the desired trajectory can be implemented using standard controllers available for most commercially available manipulators.
Figure 4: Compliant control by the use of the RCM Jacobian matrix however, we need both the acceleration and position variables.We therefore need to include a memory in the impedance controller so that the position can be recovered whenever spring forces are required.

Conclusion
This paper derived the kinematics of robotic manipulators subject to remote center of motion (RCM) constraints.The control law is derived in the Cartesian end-effector space which allows for conventional control schemes such as compliant and hybrid control to be implemented also in the presence of RCM constraints.We solved the constraints at a kinematic level, i.e., we find a Jacobian matrix that maps the end-effector velocities into a new set of velocity variables on which we impose a structure that guarantees that the remote center of motion constraints are satisfied.This modified Jacobian matrix, denoted the RCM Jacobian, can thus be used to find the desired joint velocities from a desired end-effector velocity and the trocar location.Because the new velocity variables are easily mapped to the robot and wrist joint space the desired trajectory can be implemented using standard controllers available for most commercially available manipulators.

Future Work
The algorithms presented in the paper have been implemented and we have verified that the insertion point velocities are kept at zero.It would be interesting to look more into the hybrid control law and see if and how the RCM constraints reduce the manipulability and thus also the performance of the hybrid control in the end-effector frame.We should also look at how the singularities are affected and whether a redundant system is required in order to guarantee that singularities do not arise in the workspace.Figure 5: An impedance control scheme which first imposes the desired impedance control on the end-effector variables and then, through the constrained Jacobian matrix (J m ep ) −1 guarantees that the insertion point constraints are satisfied.Note that both the robot and wrist can be controlled using the standard controllers available with most industrial manipulators.
Figure 5: An impedance control scheme which first imposes the desired impedance control on the end-effector variables and then, through the constrained Jacobian matrix (J m ep ) −1 guarantees that the insertion point constraints are satisfied.Note that both the robot and wrist can be controlled using the standard controllers available with most industrial manipulators.

Future Work
The algorithms presented in the paper have been implemented and we have verified that the insertion point velocities are kept at zero.It would be interesting to look more into the hybrid control law and see if and how the RCM constraints reduce the manipulability and thus also the performance of the hybrid control in the end-effector frame.We should also look at how the singularities are affected and whether a redundant system is required in order to guarantee that singularities do not arise in the workspace.

Figure 1 :
Figure 1: System setup for Robotic-assisted Minimally Invasive Surgery.The configuration spaces at the different frames are shown in terms of linear motion R and rotational motion S.

Figure 2 :
Figure 2: Separation of the workspace variables.The first representation with V B0r is convenient as the robot and end-effector kinematics and dynamics are separated, but the formulation is not suited for control because the end-effector variables are not present.The last formulation of the workspace variables is more suitable for control as it separates the entry point velocities and the end-effector velocity variables, and these can be treated independently.

Figure 3 :
Figure 3: Stiff control by the use of the RCM Jacobian matrix

Figure 4 :
Figure 4: Compliant control by the use of the RCM Jacobian matrix