SYSTEM AND METHOD FOR ROBOTIC SURGERY SIMULATION Cross-Reference to Related Application

This application claims the benefit of priority to U.S. provisional patent application serial number 61/035,594, filed on March 11, 2008, now pending, which application is hereby incorporated by reference.

Field of the Invention

The present invention relates generally to robotic surgery simulators and in particular, to a system and method for simulating the kinematics of a surgical robot.

Background of the Invention Robotic surgery is becoming increasingly popular due to its many benefits over traditional open surgeries, including quicker recovery time, less pain, and less scarring. A robotic surgery system, such as the da Vinci® Surgical System ("dVSS") from Intuitive Surgical, Inc., typically consists of two main components; master console and a slave robot. A surgeon provides input through a manipulator of the master console which, in turn, controls a slave robot to perform the necessary motions at the patient.

Many surgeons are reluctant to switch to surgical robots, however, because of the differences in the skill set of the surgeon. For example, in traditional open surgeries and laparoscopic surgical procedures, the forces encountered by the tools used in the surgery are felt by the surgeon. In contrast, most robotic master consoles do not provide any force feedback to the surgeon. Generations of surgeons have been trained to perform surgical procedures using tactile sensation as a key sensory input, and performing a procedure without this sense is seen as a paradigm shift requiring extensive re-training before a surgeon may be allowed to perform procedures using robotic systems.

The acquisition costs for surgical robots is high — as high as several million dollars. Because of the expense of the equipment, it is most cost effective to devote as much of the surgical robot's time to performance of actual procedures. Therefore, the availability of such expensive equipment for training is usually low.

Accordingly, there exists a need for a less expensive system for training surgeons in robotic procedures. Such a system should: (1) provide articulated input devices such that the kinesthetics of working with the master console is preserved; (2) provide accurate training

simulations to minimize training time; (3) be compact and easy to assemble and disassemble so that the system may be transported to any location convenient for training; and (4) be inexpensive to manufacture.

Brief Summary of the Invention A system according to the invention may include a frame, a computer, a display, and two input devices. The frame may be adjustable, may be made from a lightweight material, and may fold for easier portability. The display and the computer may be in communication with each other and each may be attached to the frame. The display may be a binocular display, or may be a touchscreen display. Additional displays may be used. Two input devices may be used to simulate the master console of a surgical robot. The input devices may be articulated armature devices suitable for providing 3D input. The input devices may be attached to the frame in an "upside-down" configuration wherein a base of each input device is affixed to the frame such that a first joint of an arm is below the base. The input devices may be in communication with the computer and may provide positional signals to the computer. The positional signals may correspond to a position of an arm of each input device.

The system may also include a foot-operated input such as, for example, a foot pedal, which may provide additional functions such as engaging/disengaging tools, cauterization, cutting, turn lights on or off, changing a camera position, etc. More than one foot-operated device may be used.

The input devices may provide a position signal to the computer. The display may show a computer-generated depiction of a virtual surgical tool which may correspond to the position of the input device. The computer may be programmed to use a mathematical transform function to alter the relationship between movement of the input device in real space and movement of the virtual surgical tool in virtual space. The computer may be programmed with a Jacobian transform function to substantially mimic the relationship of a position of a manipulator of a master console to a position of a slave robot in a surgical robot. In this manner, a system according to the invention may be programmed to replicate the kinematics of a particular surgical robot.

A Robotic Surgical Simulator (RoSS) may be built according to the invention. Using commercially available input devices, computers, and displays, a system according to the invention may be built which satisfies the above-listed objectives of a surgical robot simulator. An example is provided of constructing a RoSS to simulate a dVSS.

The present invention may also be embodied as a method for simulating the kinematics of a robotic surgery system wherein a robot simulation system is provided; a robotic surgery system is provided; a first 4X4 transformation matrix may be determined; a second 4X4 transformation matrix may be determined; a simulation transformation matrix may be determined by multiplying the first and the second 4X4 transformation matrices; and the simulation transformation matrix may be used to cause a virtual surgical tool depicted on a display of the robot simulation system to respond to a positional change in an input device which substantially simulates a response of a slave robot to a positional change in a manipulator.

Description of the Drawings For a fuller understanding of the nature and objects of the invention, reference should be made to the following detailed description taken in conjunction with the accompanying drawings, in which:

Figure IA is a perspective view of a system according to an embodiment of the present invention; Figure IB is a perspective view of a system according to another embodiment of the present invention showing a binocular display;

Figure 2 is a perspective view of the system of Figure IA being operated by a person; Figure 3 A is screen view of a display showing two virtual surgical tools; Figure 3B is another screen view of a display showing two virtual surgical tools; Figure 4 A is a line diagram of a dVSS manipulator;

Figure 4B is a line diagram of a RoSS input device;

Figure 5 A is a diagram showing parameters in Denavit-Hartenberg notation; Figure 5B is another diagram showing DH parameters in Denavit-Hartenberg notation; Figure 6A is a graph showing the dVSS manipulator workspace map; Figure 6B is a graph showing the RoSS input device workspace map;

Figure 6C is a graph showing the dVSS slave robot map; Figure 6D is another graph showing the RoSS input device workspace map; Figure 7A is a diagram of a dVSS slave robot; Figure 7B is a diagram of the end-effector of a dVSS slave robot; Figure 8A is a graph showing the intermediate configurations of three joints of the dVSS slave robot with inverse kinematics of a known RoSS input device position; Figure 8B is a graph showing the intermediate positions of the dVSS slave robot with inverse kinematics of the same RoSS input device position as in Figure 8 A;

Figure 9 is a graph showing the limits of the RoSS input device;

Figure 1OA is a graph showing the intermediate configurations of three joints of the dVSS slave robot with inverse kinematics of a known RoSS input device position; Figure 1OB is a graph showing the intermediate positions of the dVSS slave robot with inverse kinematics of the same RoSS input device position as in Figure 1OA; and

Figure 11 is a method according to another embodiment of the present invention.

Detailed Description of the Invention

Figure IA depicts a system 10 according to an embodiment of the invention which may include a frame 12, a computer 14, a display 16, and two input devices 20 each having an end- effector 30. The frame 12 may be adjustable so that it may be more comfortable for an operator or so that it more accurately replicates the configuration of an actual surgical robot master console. The frame 12 may be made from a lightweight material and designed to fold substantially flat for easier portability. The display 16 and computer 14 may be attached to the frame 12.

The display 16 may be in communication with the computer 14. In this way, the computer 14 may provide the data shown on the display 16. The display 16 may comprise a second display. In this way, a stereoscopic image may be displayed to provide 3 dimensional ("3D") viewing. The display 16 may be a binocular display 32; Figure IB shows one such binocular display 32 which may be used in a system 34 according the invention. The display 16 may be affixed to the frame 12 or worn by a user (e.g. head-mounted). The display 16 may be a touchscreen or may further comprise a second display which may be a touchscreen display for controlling the computer 14.

The input devices 20 may be articulated armature devices suitable for providing 3D input. Each input device 20 may comprise a base 22, an arm 24, a plurality of joints 26, and an end-effector 30. The input devices 20 may provide at least six degrees of freedom. For example, a PHANTOM Omni® device from Sensable Technologies, Inc. may be used. While the Omni® device is depicted in the drawings, any 2 link mechanism having one free rotation at the base 22, two rotations at the links and three rotations of the wrist can be used. Two input devices 20 may be used to simulate the master console of a surgical robot. The input devices 20 may be attached to the frame 12 in an "upside-down" configuration wherein the base 22 of each input device 20 is affixed to the frame 12 and the first joint 28 of the arm 24 is positioned below the base 22. The end-effector of the input device may comprise a pinch input to provide a seventh degree of

freedom. The pinch input may cause a clasping motion of the jaws of a virtual surgical tool. The input devices 20 may be in communication with the computer 14, and each input device 20 may provide a position signal corresponding to a position of the arm 24 of the input device 20 to the computer 14. The position of the arm 24 may be provided by sensing the position of the joints 26.

The computer 14 may be programmed to provide an image of one or more virtual surgical tools 120 to the display 16 (see, e.g., Figures 2, 3 A, and 3B). The computer 14 may be additionally programmed to accept a position signal from at least one of the input devices 20 and transform the position signal into a position of the virtual surgical tool 120 depicted on the display 16. In this manner, changes in the position of at least one of the input devices 20 will be reflected as similar changes in the position of a virtual surgical tool 120 depicted on the display 16. The computer 14 may be programmed to use a mathematical transform function to alter the relationship between movement of the input device 20 in real space and movement of the virtual surgical tool 120 in virtual space. In this way, the virtual surgical tool 120 may move, for example, twice as fast as the input device 20, half as fast as the input device 20, the virtual surgical tool 120 may filter out a high frequency signal in the input (tremor), or any other relationship may be mapped.

The system 10 may also include a foot-operated input 40 such as, for example, a foot pedal. The foot-operated input 40 may provide functions such as engaging/disengaging tools, cauterization, cutting, turn lights on or off, changing a camera position, etc. More than one foot- operated input 40 may be used. The foot-operated input 40 may also enable additional virtual surgical tools 120 to be controlled by the input devices 20. For example, a three virtual surgical tools 120 may be controlled by two input devices 20 by using a foot-operated input 40 to selectively change the virtual surgical tools being controlled at any given time.

The computer 14 may be programmed with a Jacobian transform function wherein the

Jacobian transform is calculated to substantially mimic the relationship of a position of a manipulator to a position of a slave robot in a surgical robot. In this manner, the system 10 may be programmed to replicate the kinematics of a particular surgical robot, such as, for example, a dVSS. As such, the system 10 may easily be reconfigured to mimic the feel of various surgical robots by providing a particular Jacobian transform to mimic the desired surgical robot.

The present invention may also be embodied as a method for simulating the kinematics of a robotic surgery system 500 (see, e.g., Figure 11). Such a method 500 may comprise the

steps of providing 510 a robot simulation system. The robot simulation system may comprise a computer, a display in communication with the computer, and an input device in communication with the computer. The robot simulation system may be similar to that described above. The input device may have an input device workspace which is defined by the range of motion of the input device. The method 500 may further comprise the step of providing 520 a robotic surgery system, such as, for example, a dVSS. The robotic surgery system may comprise a master console having a manipulator. The manipulator may have a manipulator workspace defined by the range of motion of the manipulator. The robotic surgery system may further comprise a slave robot having a slave robot workspace defined by the range of motion of the slave robot.

A first 4X4 transformation matrix may be determined 530, the first 4X4 transformation matrix relating the input device workspace and the manipulator workspace. A second 4X4 transformation matrix may be determined 540, the second 4X4 transformation matrix relating the manipulator workspace and the slave workspace. A simulation transformation matrix may be determined 550 by multiplying the first 4X4 transformation matrix and the second 4X4 transformation matrix. The simulation transformation matrix may relate the input device workspace and the slave robot workspace. The simulation transformation matrix may be used 560 to cause a virtual surgical tool depicted on the display of the robot simulation system to respond to a positional change in the input device which substantially simulates a response of the slave robot to a positional change in the manipulator. In this way, the kinematics of the input device as related to the virtual surgical tool may substantially simulate the kinematics of the manipulator as related to the slave robot.

The invention is further described through example 1 below.

Example 1

In the following non-limiting example, a dVSS was simulated by a robotic surgical simulator (RoSS) system embodying the current invention.

Kinesthetic Mapping

Kinesthetic mapping may be used to determine the transformation matrix between the simulator console workspace and the virtual slave workspace. This would help in transforming the RoSS input device positions to the virtual surgical tool positions which in turn will result in the virtual surgical tool motion. It may be done in two parts: (1) the workspace mapping of the simulator console and the manipulator of the surgical robot master console, and (2) the

workspace mapping of the manipulator of the surgical robot master console and the surgical robot slave.

Workspace Mapping of RoSS Input Device and dVSS Manipulator using Forward Kinematics

Forward kinematics may be used to calculate the end-effector position and orientation given the angles of all the robot joints. Sensors may be located at the joints to measure the joint angles. This may result in a unique end-effector solution in 3 dimensional space. Matrix algebra is used to describe three dimensional rotations and translations. The complexity of expressions grows exponentially with number of joints (degrees of freedom).

Figure 4A depicts a line diagram of the manipulator of a dVSS master console; Figure 4B depicts a line diagram of the RoSS input device. The dVSS manipulator may be viewed as an arm and a wrist. The arm of the dVSS manipulator may have three degrees of freedom and comprise a shoulder and an elbow — the shoulder having two degrees of freedom, and the elbow having one degree of freedom. The five degrees of freedom of the wrist of the dVSS manipulator were collapsed and mapped to the three degrees of freedom of the RoSS input device. Due to the redundant degrees of freedom of the wrist of the manipulator, the 5 degrees of freedom could be collapsed to 3 degrees of freedom. The roll motion of the wrist of the manipulator was mapped to the roll motion of the wrist of the RoSS input device. The yaw motion of the jaws of the wrist of the manipulator was mapped to the yaw motion of the end- effector of the RoSS input device and the clasping of jaws was mapped to the clasping action of the pinch of the custom wrist of the RoSS input device.

Modified Denavit-Hartenberg ("DH") notation was used to kinematically model the manipulator and the input device. DH parameters were calculated for the dVSS manipulator and the RoSS input device. DH notation is a systematic notation for assigning orthonormal coordinate frames to the joints. The following steps were required in order to assign coordinate frames to the j oints :

(1) Assign a coordinate frame L ₀ to the dVSS manipulator base;

(2) Align Z _k with the axis of joint k + 1 ;

(3) Locate the origin of L _k at the intersection of Zk and Zk-i. When there is no intersection, use the intersection of Z _k with a common normal between Zk and Zk-I ;

(4) Select Xk to be orthogonal to Z _k and Z _k-1 . If Z _k and z _k-1 are parallel, point X _k away from Z^ ₁ ; and

(5) Select yk to form a right handed orthonormal coordinate frame;

After assigning coordinate frames, the DH parameters may be calculated based on the following conventions (see Figures 5A and 5B):

(1) θ _k is the angle of rotation from X _k -i to X _k measured about Z _{k- 1} ;

(2) dk is the distance measured along zk-1;

(3) a _k is the distance measured along X _k ; and

(4) oik is the angle of rotation from Z _k-1 to Z _k about X _k .

Each homogeneous transformation T may be represented as a product of four basic transformations associated with joints i andy (/-link length, α-link twist, d-\mk offset, and θ-joint angle) and /is a 4x4 identity matrix. The position and orientation of the end-effector was denoted by a position vector P and the 3x3 rotation matrix R. Based on the above DH parameters, a homogeneous transformation matrix was constructed which maps frame i coordinates into z-1 coordinates as follows:

cosθ, - sinα, cosθ, smα, sinθ, a, cosθ, sinθ, - cosα, cosθ, SUIa ₁ cosθ,

T ^a , sinθ, x i ¹ -l - (1)

0 sinα, cosa, d,

0 0 0 1

After calculating the homogeneous transformation matrix for each link, the composite transformation matrix was calculated. This matrix maps the tool coordinates to the base coordinates. This yields the transformation matrix as:

γT-I tool T-i wrist . . ηπtool

^■ ' ^■ base ^"" ^base -"- wrist (3)

This final composite transformation matrix was calculated with respect to the base frame. The DH parameters for the dVSS manipulator are shown in Table 1.

Table 1: DH Parameters of dVSS Manipulator

The DH parameters for the RoSS input device are shown in Table 2.

Table 2 : DH Parameters of RoSS Input Device

Based on these DH parameters, the individual transformation matrix for each link may be calculated and the composite transformation matrix may be constructed after multiplying each of the individual transformation matrices as follows

T ₀ ⁶ = T ₀ ¹ x T ₁ ² x T ₂ ³ x T ₃ ⁴ x T ₄ ⁵ x T ₅ ⁽ (4)

To find the overall workspaces of the RoSS input device and dVSS manipulator, the range of angles of all the joints was found.

The range of each of the joint angles of RoSS input device was:

Joint 1 : -1.45 < O ₁ < 1.05 (radians) Joint 2: 0.0 < θ ₂ < 1.727 (radians) Joint 3 : 1.0 < θ ₃ < 2.1 (radians)

Joint 4: 0.0 < O ₄ < 4.71 (radians)

Joint 5: 0.0 < θ ₅ < 3.0 (radians)

Joint 6: 0.0 < θ ₆ < 4.71 (radians) The range of each of the joint angles of dVSS manipulator was:

Joint 1 : -0.53 < G ₁ < 1.57 (radians)

Joint 2: 0.265 < θ ₂ < 0.785 (radians)

Joint 3 : 0.0 < O ₃ < 1.03 (radians)

Joint 4 : -3.14 < θ ₄ < 1.57 (radians) Joint 5: -1.57 < θ ₅ < 3.14 (radians)

Joint 6: -0.707 < θ ₆ < 0.707 (radians)

Each of the joint angles was varied incrementally to yield the end-effector positions in the workspace (Figure 6A and 6B). The end-effector position matrix was homogenized by adding a fourth column to the x, y and z columns. The workspace positions for both the RoSS input device and the dVSS manipulator were calculated. The 4X4 transformation matrix between the two workspaces was calculated by:

T = pinv(P ₀ ) * P _M (5)

where: Po is the set of homogenized positions for RoSS input device; and P _M is the set of homogenized positions for dVSS manipulator.

Since the end-effector encoder position values from the RoSS input device were spatially transformed to the calculated position values of RoSS input device from DH notation, these positions may either be transformed to the RoSS input device workspace or transformed to the dVSS manipulator workspace. Therefore, a set of device positions consisting of a large number of 3D spatial position values (9261 in number) and the end-effector positions were homogenized

by adding a fourth column to x, y and z columns. Then the 4X4 transformation matrix was found between the two workspaces.

Workspace Mapping of dVSS Master Console and dVSS Slave Robot

The manipulator of the dVSS controls a highly articulated laparoscopic tool of the dVSS slave robot shown in Figures 7 A and 7B, the end-effector of which has seven degrees of freedom. In the RoSS console, the three degrees of freedom of the shoulder and elbow of the input devices were mapped such that the virtual surgical tool is pivoted about the site of entry and has two rotational degrees of freedom and one translational degree of freedom along its axis. Two of the three degrees of freedom of the wrist were mapped to the jaws to allow rotational movement of the virtual surgical tool and the clasping of the jaws was mapped to a clasping action of the pinch of the custom wrist. The virtual surgical tool has the same degrees of freedom as the tool which is used with the dVSS. In order to map the RoSS input device with the virtual surgical tool, the dVSS manipulator was mapped to the dVSS slave robot.

Modified Denavit-Hartenberg ("DH") notation was again used to kinematically model the dVSS slave robot. The corresponding controlled parameters of the arm are summarized in Table 3. The kinematic equations of the arm for the instrument and the endoscope are represented by a total of 12 degrees of freedom (2 translational and 10 rotational).

Table 3 DH Parameters of dVSS Slave Robot

The corresponding transformation matrices between links 1 to 6, links 7 to 9, and links 10 to 12 are T ₀ ⁶ , T ₇ ⁹ and T ₁₀ ¹² . Links 1 to 6 correspond to the robot base, links 7 to 9 to the robot mechanism, and links 10 to 12 to the instruments. The robot base includes five revolute joints and one prismatic joint. A double parallelogram linkage mechanism is formed on link 8. The robot arm of the mechanism has three degrees of freedom, with two revolute joints and a prismatic joint.

Based on these DH parameters, the individual transformation matrix for each link was calculated as described above and the composite transformation matrix between the slave robot base and the end-effector of the slave robot (instrument) was constructed after multiplying each of the individual transformation matrices as follows

T ₀ ¹² = TJ X T ₁ ² X T ₂ ³ X T ₃ ⁴ X T ₄ ⁵ X T ₅ ⁶ X T ₆ ⁷ X T ₇ ⁸ X T ₈ ⁹ X T ₉ ¹⁰ X T/J X T, ¹² .... ₍₆ )

Table 4: DH Parameters of dVSS Manipulator

The first six degrees of freedom of dVSS slave robot were initially set to a particular configuration. The next three degrees of freedom of dVSS slave robot (two revolute and one prismatic joint) were mapped to first three degrees of freedom (three revolute joints) of the

manipulator. The last three degrees of freedom of dVSS slave robot and dVSS manipulator were also set to a particular configuration.

To find the overall workspaces of dVSS manipulator and dVSS slave robot, the range of joint angles of all the slave robot links was found. The range of each of the joint angles of dVSS slave robot was as follows:

Joint 1 : ti = 9.0 (inches)

Joint 2: θ ₂ =0.0 (radians)

Joint 3: θ ₃ = 0.0 (radians)

Joint 4: O ₄ = 0 (radians) Joint 5: O ₅ = 1.05 (radians)

Joint 6: O ₆ = 1.05 (radians)

Joint 7: -1.57 < O ₇ < 1.57 (radians)

Joint 8: -1.05 < O ₈ < 1.05 (radians)

Joint 9: 0.0 < t ₉ < 9.0 (inches) Joint 10: 0.0 < O ₁₀ < 7.33 (radians)

Joint 11 : 0.0 < O ₁₁ < 3.14 (radians)

Joint 12: 0.0 < θ ₎₂ < 3.14 (radians)

The range of each of the joint angles of the dVSS manipulator was:

Joint 1 : -0.53 < O ₁ < 1.57 (radians)

Joint 2: 0.265 < θ ₂ < 0.785 (radians)

Joint 3 : 0.0 < O ₃ < 1.03 (radians)

Joint 4: -3.14 < O ₄ < 1.57 (radians)

Joint 5: -1.57 < O ₅ < 3.14 (radians) Joint 6: -0.707 < θ ₆ < 0.707 (radians)

Each of the joint angles was varied incrementally to yield the end-effector positions in the workspace. Then the end-effector positions were homogenized by adding a fourth column to x, y and z columns. The workspace positions for both the dVSS manipulator (Figure 6A) and dVSS slave robot (Figure 6C) was calculated. The 4X4 transformation matrix between the two workspaces was calculated by

T = pinv(P _M ) * P _s (7)

where: Ps is the set of homogenized positions for dVSS slave robot; and P _M is the set of homogenized positions for dVSS manipulator.

Inverse Kinematics Using Jacobian-Based Control

Inverse kinematics may be used to find a set of joint configurations of an articulated structure based upon a desirable end-effector location. Inverse kinematics was used to determine a set of joint angles in an articulated structure based upon the position of the end-effector of the slave robot. This results in multiple joint angle solutions and infinite solutions at singularities. It may be generally used in software to control the joints. Control software should be able to perform the necessary calculations in near real time.

The mathematical representation of the inverse kinematics technique is defined as

θ = r'(x) (8)

Inverse kinematics may be implemented based upon the Jacobian technique. This technique incrementally changes joint orientations from a stable starting position towards a joint configuration that will result in the end-effector being located at the desired position in absolute space. The amount of incremental change on each iteration is defined by the relationship between the partial derivatives of the joint angles, θ, and the difference between the current location of the end-effector, X, and the desired position, X _d . The link between these two sets of parameters leads to the system Jacobian, J. This is a matrix that has dimensionality (m x n) where m is the spatial dimensional of X and n is the size of the joint orientation set, q.

X = f(θ) (9)

The Jacobian was derived from Equation 9 as follows. Taking partial derivatives of Equation 9:

dX = J(θ)dθ (10)

Where:

j = *L GD

^1J 6Q ₁

Rewriting Equation 10 in a form similar to inverse kinematics (Equation 9), results in Equation 12. This form of the problem transforms the under-defined system into a linear one that can be solved using iterative steps.

dθ = r'dX (12)

The problem now is that Equation 12 requires the inversion of the Jacobian matrix.

However because of the under-defined problem that the inverse kinematics technique suffers from, the Jacobian is very rarely square. Therefore, the right-hand generalized pseudo-inverse may be used to overcome the non-square matrix problem, as given in equation 14.

Generating the pseudo-inverse of the Jacobian in this way can lead to inaccuracies in the resulting inverse that need to be reduced. Any inaccuracies of the inverse Jacobian can be detected by multiplying it with the original Jacobian then subtracting the result from the identity matrix. A magnitude error can be determined by taking the second norm of the resulting matrix multiplied by dP, as outlined in Equation 15. If the error proves too big then dP can be decreased until the error falls within an acceptable limit.

An overview of the algorithm used to implement an iterative inverse kinematics solution is as follows:

(1) Calculate the difference between the goal position and the actual position of the end- effector. dP = X _g - X _p (13) (2) Calculate the Jacobian matrix using the current joint angles.

' ^{■ ■} f <">

(3) Calculate the pseudo-inverse of the Jacobian.

(4) Determine the error of the pseudo-inverse error: error = I - (jJ-' )dP (16)

(5) If error > e then dP = dP / 2 restart at step (4)

(6) Calculate the updated values for the joint orientations and use these as the new current values. Check the bounds for theta values.

lowerbound if θ + J dP < lowerbound θ = upperbound if θ + J ^~ 'dP > upperbound (17) θ + J dP if otherwise

(7) Using forward kinematics determine whether the new joint orientations position the end- effector close enough to the desired absolute location. If the solution is adequate then terminate the algorithm otherwise go back to step (1).

The time to complete the Inverse Kinematics algorithm for a given end-effector is an unknown quantity due to an arbitrary number of iterations required. However, the time to complete a single iteration is constant with respect to the dimensionality of X and θ which is unchanged under a complete execution of the algorithm. Therefore by placing an upper limit on the number of iterations we can set a maximum time boundary for the algorithm to return in. If the solver reaches the limit then the algorithm returns the closest result it has seen.

Jacobian-Based Inverse Kinematics Applied to Simulation

The dVSS slave robot consists of 12 degrees of freedom and the inverse kinematics were used to control only the three degrees of freedom. The first six degrees of freedom were fixed to suitable joint angles. These joint angles were calculated on the basis of a particular dVSS configuration during a surgical procedure in the operating room. The next three degrees of freedom are being considered for inverse kinematics control. These were two revolute joints and one prismatic joint. The last three degrees of freedom were also constrained as part of wrist at a particular configuration, (pi/2, pi/2, 0 radians).

The algorithm requires that the three joint angles which correspond to a particular configuration of dVSS are initialized. Then using forward kinematics, the end-effector position of the dVSS surgical tool (slave robot) was found which correspond to the initial position of the slave robot. Once the initial position is found the difference between the goal and initial position was calculated. The Jacobian was calculated by differentiating the end-effector position with respect to the three slave robot joint angles (two revolute joints and one prismatic joint) using the following equation:

dPx/dθ ₇ dPx /dθ ₈ dPx/dt ₉ ^'

J = dPy/dθ ₇ dPy/dθ _g dPy/dt ₉ (18) dPz/dθ ₇ dPz/dθ ₈ dPz/dt ₉

Once the Jacobian was known, its pseudo inverse was calculated. The pseudo inverse was used to reduce the singularities in the matrix. Then the three joint angles are updated using the following equation:

lowerbound if < lowerbound

θ ₇ upperbound if > upperbound (19)

if otherwise

Using forward kinematics equations, the three joint orientations are used to calculate the end-effector position and the difference between the goal position and the current end-effector position was checked. If the two were close enough then the algorithm is terminated.

Figures 8 A and 8B shows a complete cycle of an inverse kinematics algorithm for a particular RoSS input device position. The initial input device position was [-24.7935,-47.8608,- 47.7267] mm and when it was transformed to the dVSS slave robot position which was the goal position. The initial dVSS slave robot position was calculated using the initial joint configuration of the dVSS slave robot (using DH notation) where the joint values of the 7 ^th , 8 ^th , and 9 ^th joint are taken as -pi/6 radians, pi/4 radians, and 1 inch respectively. The algorithm took 8 iterations and the intermediate joint configuration and position values are shown in Figures 8A and 8B.

The motions of the virtual surgical tool were performed using inverse kinematics which was essentially controlled by the RoSS input device. The input device positions were transformed to the positions of the virtual surgical tool and then the inverse kinematics was performed to calculate the link parameters (joint angles) of the virtual surgical tool.

Figure 9 is a graph of RoSS input device positions. The positions were homogenized. The transformation matrix between the RoSS input device positions and dVSS manipulator workspace was calculated by taking the pseudo inverse of device positions and multiplying by dVSS manipulator homogeneous positions using Equation 5 (as described above).

Similarly the transformation matrix between dVSS manipulator and dVSS slave robot was obtained as described above.

The transformation matrix between the RoSS input device workspace and dVSS slave robot workspace was found by multiplying the two transformation matrices. Figures 6C depicts the workspace mapping of the dVSS slave robot and Figure 6D depict the workspace mapping of the RoSS input device (using device positions).

Once the transformation matrix was obtained, the RoSS input device position was transformed to the dVSS slave robot position. After the dVSS slave robot position was obtained, the inverse kinematics was performed to get the joint angles of 7 ^th , 8 ^th and 9 ^th links as described above. The l ^u and 8 ^l links are revolute joints and 9 ^th link is a prismatic joint. The accuracy of the algorithm is 10 ^~5 mm and the computational time is 25-35 ms for computing each of the three sets of joint angles using inverse kinematics algorithm. The number of iterations was set to 40 for a single run of algorithm. The motion of the virtual surgical tool was manipulated to get the desired motion of dVSS slave robot.

Figure 1OA shows joint configurations of the RoSS input device, and Figure 1OB shows the workspace of the RoSS input device using inverse kinematics for a set of input device positions.

The bounds on the 7 ^th , 8 ^th and 9 ^th Joint Configurations for the dVSS slave robot using inverse kinematics algorithm were found to be:

Joint 7: -1.15 < θ ₇ < -0.75 (radians) Joint 8: 0.35 < θ ₈ < 0.60 (radians) Joint 9: 7.5 < t ₉ < 9.0 (inches)

These joint values were used to calculate the end-effector boundaries of the dVSS slave robot. The dVSS slave robot was treated as a six degrees of freedom robot (3 degrees of freedom for arm and 3 degrees of freedom for wrist) so that the axis orientations of the dVSS slave robot and the RoSS input device were coincident.

The workspace boundaries of the dVSS slave robot from the above joint range using DH notation was found to be:

170 < x < 370 mm

-450 < y < -250 mm -120 < z < -5 mm

The workspace boundaries of end-effector of the RoSS input device position were:

-108 < x < 117 mm -110 < y < 240 mm -200 < z < -10 mm

Since the axis orientations of the dVSS slave robot and the RoSS input device are the same, the x, y, z axes of RoSS input device were mapped onto the x, y, z axes of dVSS slave robot. Then the two rotations along the x and z axes and one translation along the y axis were calculated to give the actual dVSS slave robot motions.

Although the present invention has been described with respect to one or more particular embodiments, it will be understood that other embodiments of the present invention may be made without departing from the spirit and scope of the present invention. Hence, the present invention is deemed limited only by the appended claims and the reasonable interpretation thereof.