Autonomous correction of alignment error in a master-slave robotic system

Abstract

In some embodiments, correcting an alignment error between an end effector of a tool associated with a slave and a master actuator associated with a master in a robotic system involves receiving at the master, master actuator orientation signals (RMCURR) representing the orientation of the master actuator relative to a master reference frame and generating end effector orientation signals (REENEW) representing the end effector orientation relative to a slave reference frame, producing control signals based on the end effector orientation signals, receiving an enablement signal for selectively enabling the control signals to be transmitted from the master to the slave, responsive to a transition of the enablement signal from not active state to active state, computing the master-slave misalignment signals (RΔ) as a difference between the master actuator orientation signals (RMCURR) and the end effector orientation signals (REENEW), and adjusting the master-slave misalignment signals (RΔ) to reduce the alignment difference.

Description

BACKGROUND OF THE INVENTION

1. Field of Invention

This invention relates to master-slave robotic systems such as used for laparoscopic surgery and more particularly to autonomous correction of alignment error between the master and slave in such systems.

2. Description of Related Art

During operation of a teleoperated robotic minimally invasive surgical system where a slave instrument is intended to follow the motion of a master input controller held by a user, it is possible for the master and slave to become misaligned with respect to each other's base frames of reference. In instances of misalignment, the slave end-effector no longer points in the direction that the user is expecting, which may result in less than optimal controllability of the slave instrument.

U.S. Pat. No. 7,806,891 entitled “Repositioning and reorientation of master slave relationship in minimally invasive telesurgery” describes a system but the system disrupts motion of the slave when there is misalignment between the master and slave. The system also requires a master controller with the ability to actively control, or lock orientation degrees of freedom.

U.S. Pat. No. 8,423,186 entitled “Ratcheting for master alignment of a teleoperated minimally invasive surgical instrument” describes a system that only reduces alignment error when the motion of the master handle is in a direction such that it would reduce the misalignment if the slave were not moved.

SUMMARY OF THE INVENTION

The disclosure describes a method of correcting an alignment error between an end effector of a tool associated with a slave and a master actuator associated with a master in a master-slave robotic system in which an orientation of the end effector is remotely controlled by an orientation of the master actuator by producing and transmitting control signals at the master for controlling the slave. The method involves causing a processor associated with the master to receive master actuator orientation signals (R_MCURR) representing the orientation of the master actuator relative to a master reference frame and causing the processor to generate end effector orientation signals (R_EENEW) representing the end effector orientation relative to a slave reference frame, in response to the master actuator orientation signals (R_m), master base orientation signals (R_MBASE) representing previous-saved values of the master actuator orientation signals (R_MCURR) and slave base orientation signals (R_EEBASE) representing previous-saved values of the end effector orientation signals (R_EENEW). The method also involves causing the processor to produce control signals based on the end effector orientation signals, for transmission from the master to the slave and causing the processor to receive an enablement signal for selectively enabling the control signals to be transmitted from the master to the slave whereby the master transmits the control signals to the slave when the enablement signal is active and does not transmit the control signals to the slave when the enablement signal is not active and such that when the enablement signal is active, changes in the orientation of the master actuator cause corresponding changes in the orientation of the end effector and such that when the enablement signal is not active, changes in the orientation of the master actuator do not cause corresponding changes in the orientation of the end effector. The method also involves, when the enablement signal transitions from the not active state to the active state, causing the processor to save the values of the master actuator orientation signals (R_MCURR) as the master base orientation signals (R_MBASE) to create the previous-saved values of the master actuator orientation signals (R_MCURR) and save the values of the end effector orientation signals (R_EENEW) as the slave base orientation signals (R_EEBASE) to create the previous-saved values of the end effector orientation signals (R_EENEW). The method also involves causing the processor to detect a difference, between the master actuator orientation signals (R_MCURR) and the end effector orientation signals (R_EENEW), the difference representing a difference in physical alignment between the tool and the master relative to their respective reference frames and in response to detecting the difference, causing the processor to adjust the saved slave base orientation signals (R_EEBASE) to ultimately have the same values as the saved master base orientation (R_MBASE) values so that subsequent generations of the end effector orientation signals (R_EENEW) cause the control signals to cause the tool to satisfy an alignment criterion.

The disclosure also describes a method of correcting an alignment error between an end effector of a tool associated with a slave and a master actuator associated with a master in a master-slave robotic system in which an orientation of the end effector is remotely controlled by an orientation of the master actuator by producing and transmitting control signals at the master for controlling the slave. The method involves causing a processor associated with the master to receive master actuator orientation signals (R_MCURR) representing the orientation of the master actuator relative to a master reference frame and causing the processor to generate end effector orientation signals (R_EENEW) representing the end effector orientation relative to a slave reference frame, in response to the master actuator orientation signals (R_MCURR) and master-slave misalignment signals (R_Δ), representing a product of previously saved values of the master actuator orientation signals (R_MCURR) and the end effector orientation signals (R_EENEW). The method also involves causing the processor to produce control signals based on the end effector orientation signals, for transmission from the master to the slave and causing the processor to receive an enablement signal for selectively enabling the control signals to be transmitted from the master to the slave whereby the master transmits the control signals to the slave when the enablement signal is active and does not transmit the control signals to the slave when the enablement signal is not active and such that when the enablement signal is active, changes in the orientation of the master actuator cause corresponding changes in the orientation of the end effector and such that when the enablement signal is not active, changes in the orientation of the master actuator do not cause corresponding changes in the orientation of the end effector. The method also involves, when the enablement signal transitions from the not active state to the active state, causing the processor to compute the master-slave misalignment signals (R_Δ) as a difference between the master actuator orientation signals (R_MCURR) and the end effector orientation signals (R_EENEW), the misalignment signals representing a difference in physical alignment between the tool and the master relative to their respective reference frames. The method further involves causing the processor to detect a second difference between the master actuator orientation signals (R_MCURR) and the end effector orientation signals (R_EENEW) the second difference representing a difference in physical alignment between the tool and the master relative to their respective reference frames and in response to detecting the second difference, causing the processor to adjust the master-slave misalignment signals (R_Δ) to reduce the alignment difference to satisfy an alignment criterion so that subsequent generations of the end effector orientation signals (R_EENEW) cause the control signals to cause the tool to be physically aligned with the master within the alignment criterion.

The disclosure also describes an apparatus for correcting an alignment error between an end effector of a tool associated with a slave and a master actuator associated with a master in a master-slave robotic system in which an orientation of the end effector is remotely controlled by an orientation of the master actuator by producing and transmitting control signals at the master for controlling the slave. The apparatus includes means associated with the master for receiving master actuator orientation signals (R_MCURR) representing the orientation of the master actuator relative to a master reference frame and means for generating end effector orientation signals (R_EENEW) representing the end effector orientation relative to a slave reference frame, in response to the master actuator orientation signals (R_m), master base orientation signals (R_MBASE) representing previous-saved values of the master actuator orientation signals (R_MCURR) and slave base orientation signals (R_EEBASE) representing previous-saved values of the end effector orientation signals (R_EENEW). The apparatus also includes means for producing control signals based on the end effector orientation signals, for transmission from the master to the slave and means for receiving an enablement signal for selectively enabling the control signals to be transmitted from the master to the slave whereby the master transmits the control signals to the slave when the enablement signal is active and does not transmit the control signals to the slave when the enablement signal is not active and such that when the enablement signal is active, changes in the orientation of the master actuator cause corresponding changes in the orientation of the end effector and such that when the enablement signal is not active, changes in the orientation of the master actuator do not cause corresponding changes in the orientation of the end effector. The apparatus also includes means responsive to a transition of the enablement signal from the not active state to the active state, for saving the values of the master actuator orientation signals (R_MCURR) as the master base orientation signals (R_MBASE) to create the previous-saved values of the master actuator orientation signals (R_MCURR) and saving the values of the end effector orientation signals (R_EENEW) as the slave base orientation signals (R_EEBASE) to create the previous-saved values of the end effector orientation signals (R_EENEW). The apparatus also includes means for detecting a difference, between the master actuator orientation signals (R_MCURR) and the end effector orientation signals (R_EENEW), the difference representing a difference in physical alignment between the tool and the master relative to their respective reference frames means for causing the processor to adjust the saved slave base orientation signals (R_EEBASE) to ultimately have the same values as the saved master base orientation (R_MBASE) values so that subsequent generations of the end effector orientation signals (R_EENEW) cause the control signals to cause the tool to satisfy an alignment criterion, in response to detecting the difference.

The disclosure also describes an apparatus for correcting an alignment error between an end effector of a tool associated with a slave and a master actuator associated with a master in a master-slave robotic system in which an orientation of the end effector is remotely controlled by an orientation of the master actuator by producing and transmitting control signals at the master for controlling the slave. The apparatus includes means associated with the master for receiving master actuator orientation signals (R_MCURR) representing the orientation of the master actuator relative to a master reference frame and means for generating end effector orientation signals (R_EENEW) representing the end effector orientation relative to a slave reference frame, in response to the master actuator orientation signals (R_MCURR) and master-slave misalignment signals (R_Δ), representing a product of previously saved values of the master actuator orientation signals (R_MCURR) and the end effector orientation signals (R_EENEW). The apparatus also includes means for producing control signals based on the end effector orientation signals, for transmission from the master to the slave and means for receiving an enablement signal for selectively enabling the control signals to be transmitted from the master to the slave whereby the master transmits the control signals to the slave when the enablement signal is active and does not transmit the control signals to the slave when the enablement signal is not active and such that when the enablement signal is active, changes in the orientation of the master actuator cause corresponding changes in the orientation of the end effector and such that when the enablement signal is not active, changes in the orientation of the master actuator do not cause corresponding changes in the orientation of the end effector. The apparatus also includes means responsive to a transition of the enablement signal from the not active state to the active state, for computing the master-slave misalignment signals (R_Δ) as a difference between the master actuator orientation signals (R_MCURR) and the end effector orientation signals (R_EENEW), the misalignment signals representing a difference in physical alignment between the tool and the master relative to their respective reference frames, means for detecting a second difference, between the master actuator orientation signals (R_MCURR) and the end effector orientation signals (R_EENEW) the second difference representing the difference in physical alignment between the tool and the master relative to their respective reference frames and means responsive to detecting the second difference, for causing the processor to adjust the master-slave misalignment signals (R_Δ) to reduce the alignment difference to satisfy an alignment criterion so that subsequent generations of the end effector orientation signals (R_EENEW) cause the control signals to cause the tool to be physically aligned with the master within the alignment criterion.

The disclosure also describes an apparatus for correcting an alignment error between an end effector of a tool associated with a slave and a master actuator associated with a master in a master-slave robotic system in which an orientation of the end effector is remotely controlled by an orientation of the master actuator by producing and transmitting control signals at the master for controlling the slave. The apparatus includes a processor associated with the master operably configured to receive master actuator orientation signals (R_MCURR) representing the orientation of the master actuator relative to a master reference frame. The processor is further configured to generate end effector orientation signals (R_EENEW) representing the end effector orientation relative to a slave reference frame, in response to the master actuator orientation signals (R_m), to generate master base orientation signals (R_MBASE) representing previous-saved values of the master actuator orientation signals (R_MCURR) and to generate slave base orientation signals (R_EEBASE) representing previous-saved values of the end effector orientation signals (R_EENEW). The process is also configured to produce control signals based on the end effector orientation signals, for transmission from the master to the slave and to receive an enablement signal for selectively enabling the control signals to be transmitted from the master to the slave whereby the master transmits the control signals to the slave when the enablement signal is active and does not transmit the control signals to the slave when the enablement signal is not active and such that when the enablement signal is active, changes in the orientation of the master actuator cause corresponding changes in the orientation of the end effector and such that when the enablement signal is not active, changes in the orientation of the master actuator do not cause corresponding changes in the orientation of the end effector. The processor is also configured to, when the enablement signal transitions from the not active state to the active state, save the values of the master actuator orientation signals (R_MCURR) as the master base orientation signals (R_MBASE) to create the previous-saved values of the master actuator orientation signals (R_MCURR) and save the values of the end effector orientation signals (R_EENEW) as the slave base orientation signals (R_EEBASE) to create the previous-saved values of the end effector orientation signals (R_EENEW). The processor is also configured to detect a difference, between the master actuator orientation signals (R_MCURR) and the end effector orientation signals (R_EENEW), the difference representing a difference in physical alignment between the tool and the master relative to their respective reference frames and the processor is configured to adjust the saved slave base orientation signals (R_EEBASE) to ultimately have the same values as the saved master base orientation (R_MBASE) values so that subsequent generations of the end effector orientation signals (R_EENEW) cause the control signals to cause the tool to satisfy an alignment criterion, in response to detecting the difference.

The disclosure also describes an apparatus for correcting an alignment error between an end effector of a tool associated with a slave and a master actuator associated with a master in a master-slave robotic system in which an orientation of the end effector is remotely controlled by an orientation of the master actuator by producing and transmitting control signals at the master for controlling the slave. The apparatus includes a processor associated with the master and operably configured to receive master actuator orientation signals (R_MCURR) representing the orientation of the master actuator relative to a master reference frame. The processor is further configured to generate end effector orientation signals (R_EENEW) representing the end effector orientation relative to a slave reference frame, in response to the master actuator orientation signals (R_MCURR) and master-slave misalignment signals (R_Δ), representing a product of previously saved values of the master actuator orientation signals (R_MCURR) and the end effector orientation signals (R_EENEW). The processor is also configured to produce control signals based on the end effector orientation signals, for transmission from the master to the slave and receive an enablement signal for selectively enabling the control signals to be transmitted from the master to the slave whereby the master transmits the control signals to the slave when the enablement signal is active and does not transmit the control signals to the slave when the enablement signal is not active and such that when the enablement signal is active, changes in the orientation of the master actuator cause corresponding changes in the orientation of the end effector and such that when the enablement signal is not active, changes in the orientation of the master actuator do not cause corresponding changes in the orientation of the end effector. The processor is also configured to, in response to a transition of the enablement signal from the not active state to the active state, compute the master-slave misalignment signals (R_Δ) as a difference between the master actuator orientation signals (R_MCURR) and the end effector orientation signals (R_EENEW), the misalignment signals representing a difference in physical alignment between the tool and the master relative to their respective reference frames. The processor is also configured to detect a second difference, between the master actuator orientation signals (R_MCURR) and the end effector orientation signals (R_EENEW) the second difference representing the difference in physical alignment between the tool and the master relative to their respective reference frames and responsive to detecting the second difference, the processor adjusts the master-slave misalignment signals (R_Δ) to reduce the alignment difference to satisfy an alignment criterion so that subsequent generations of the end effector orientation signals (R_EENEW) cause the control signals to cause the tool to be physically aligned with the master within the alignment criterion.

Other aspects and features of the present invention will become apparent to those ordinarily skilled in the art upon review of the following description of specific embodiments of the invention in conjunction with the accompanying figures.

BRIEF DESCRIPTION OF THE DRAWINGS

In drawings which illustrate embodiments of the invention,

FIG. 1 is a pictorial representation of a laparoscopic surgery system according to one embodiment of the invention;

FIG. 2 is an oblique view of an input device of a master subsystem of the laparoscopic surgery system shown in FIG. 1;

FIG. 3 is a block diagram illustrating certain functionality and certain signals produced and used by the system shown in FIG. 1;

FIG. 4 is an oblique view of a tool positioning device with a tool in the form of an end effector held thereby, in an insertion tube of the laparoscopic surgery system shown in FIG. 1;

FIG. 5 is a schematic representation of current and previous value buffers maintained by a master apparatus of the laparoscopic surgery system shown in FIG. 1 and updated according to the functions shown in FIGS. 6, 8A and 8B;

FIG. 6 is a block diagram of a storage routine executed by the master apparatus in response to detection of a signal transition of an enablement signal produced in response to user input;

FIG. 7 is an oblique view of the input device shown in FIG. 2 and the tool positioning device shown in FIG. 4 showing relationships between base axes of the input device and the end effector;

FIGS. 8A-8E are successive portions of a flowchart representing codes executed by the master apparatus of the laparoscopic surgery system shown in FIG. 1, to provide for computation of an alignment difference between the input device shown in FIG. 2 and the end effector shown in FIG. 4 and for controlling translational movement of the end effector and for controlling the type of control signals sent to a slave subsystem of the laparoscopic surgery system shown in FIG. 1, based on the computed alignment difference; and

FIG. 9 is a flowchart of an alternative block of code optionally replacing the block of code shown at 204 and 206 in FIG. 8B.

DETAILED DESCRIPTION

Referring to FIG. 1, a robotic control system in the form of a laparoscopic surgery system is shown generally at 50. The system includes a master subsystem 52 and a slave subsystem 54. The master subsystem 52 may be located anywhere in the world, but for the purposes of this description it will be considered to be in an operating room. The slave subsystem 54 is located in the operating room.

In the embodiment shown, the master subsystem 52 comprises a workstation 56 having first and second input devices 58 and 60 and a viewer 62 in communication with a master apparatus 64 comprising at least one processor. The first and second input devices 58 and 60 are operable to be actuated by respective hands of an operator such as a surgeon, for example, who will perform the laparoscopic surgery by manipulating the first and second input devices of the master subsystem 52 to control corresponding laparoscopic tools 66 and 67 on the slave subsystem 54.

The viewer 62 may include an LCD display 68, for example, for displaying images acquired by a camera 70 on the slave subsystem 54, to enable the user to see the laparoscopic tools 66 and 67 inside the patient while manipulating the first and second input devices 58 and 60 to cause the tools to move in desired ways to perform the surgery. The first and second input devices 58 and 60 produce position and orientation signals that are received by the master apparatus 64 and the master apparatus produces slave control signals that are transmitted by wires 72 or wirelessly, for example, from the master subsystem 52 to the slave subsystem 54.

The slave subsystem 54 includes a slave computer 74 that receives the slave control signals from the master subsystem 52 and produces motor control signals that control motors 76 on a drive mechanism of a tool controller 78 of the slave subsystem 54, to extend and retract wires (not shown) of respective tool positioning devices 79 and 81 to position and to rotate the tools 66 and 67. Exemplary tool positioning devices and tools for this purpose are described in PCT/CA2013/001076, which is incorporated herein by reference. The tool positing devices 79 and 81 extend through an insertion tube 61, a portion of which is inserted through a small opening 63 in the patient to position end effectors 71 and 73 of the tools 66 and 67 inside the patient, to facilitate the surgery.

In the embodiment shown, the workstation 56 has a support 80 having a flat surface 82 for supporting the first and second input devices 58 and 60 in positions that are comfortable to the operator whose hands are actuating the first and second input devices 58 and 60.

In the embodiment shown, the slave subsystem 54 includes a cart 84 in which the slave computer 74 is located. The cart 84 has an articulated arm 86 mechanically connected thereto, with a tool holder mount 88 disposed at a distal end of the articulated arm.

In the embodiment shown, the first and second input devices 58 and 60 are the same, but individually adapted for left and right hand use respectively. In this embodiment, each input device 58 and 60 is an Omega.7 haptic device available from Force Dimension, of Switzerland. For simplicity, only input device 60 will be described, it is being understood that input device 58 operates in the same way.

Referring to FIG. 2, generally input device 60 includes a base plate 90 that supports a control unit 92 having arms 94, 96, 98 connected to a gimbal-mounted handle 102 that can be grasped by the hand of the operator user and rotated about orthogonal axes x₁, y₁ and z₁ of a first Cartesian reference frame having an origin at a point midway along the axis of a cylinder that forms part of the handle 102. This first Cartesian reference frame may be referred to as the handle reference frame. The origin may be referred to as the handle position 104.

The arms 94, 96, 98 facilitate translational movement of the handle 102 and hence the handle position 104, in space, and confine the movement of the handle position within a volume in space. This volume may be referred to as the handle translational workspace.

The handle 102 is mounted on a gimbal mount 106 having a pin 108. The base plate 90 has a calibration opening 110 for receiving the pin 108. When the pin 108 is received in the opening 110, the input device 60 is in a calibration position that is defined relative to a fixed master Cartesian reference frame comprising orthogonal axes x_r, y_r, z_r generally in the center of the handle translational workspace. In the embodiment shown, this master reference frame has an x_r-z_r plane parallel to the flat surface 82 and a y_r axis perpendicular to the flat surface.

In the embodiment shown, the z_r axis is parallel to the flat surface 82 and is coincident with an axis 112 passing centrally through the control unit 92 so that pushing and pulling the handle 102 toward and away from the center of the control unit 92 along the axis 112 in a direction parallel to the flat surface 82 is movement in the z_r direction.

The control unit 92 has sensors (not shown) that sense the positions of the arms 94, 96, 98 and the rotation of the handle 102 and produces signals representing the handle position 104 (i.e. the center of the handle 102) in the workspace and the rotational orientation of the handle 102 relative to the fixed master reference frame x_r, y_r, z_r. In this embodiment, these position and orientation signals are transmitted on wires 111 of a USB bus to the master apparatus 64. More particularly, the control unit 92 produces current handle position signals and current handle orientation signals that represent the current position and orientation of the handle 102 by a current handle position vector P_MCURR and a current handle rotation matrix R_MCURR, relative to the fixed master reference frame x_r, y_r, z_r.

For example, the current handle position vector {right arrow over (P)}_MCURR is a vector

$\left\{\begin{array}{c}{x}_1\\ y_1\\ z_1\end{array}\right\},$ where x₁, y₁, and z₁ represent coordinates of the handle position 104 within the handle workspace relative to the fixed master reference frame, x_r, y_r, z_r.

The current handle rotation matrix R_MCURR is a 3×3 matrix

$\left[\begin{array}{ccc}{x}_{1x}& y_{1x}& z_{1x}\\ x_{1y}& y_{1y}& z_{1y}\\ x_{1z}& y_{1z}& z_{1z}\end{array}\right],$ where the columns of the matrix represent the axes of the handle reference frame x₁, y₁, z₁ written in the fixed master reference frame x_r, y_r, z_r. R_MCURR thus defines the current rotational orientation of the handle 102 in the handle translational workspace, relative to the x_r, y_r, z_r master reference frame.

The current handle position vector {right arrow over (P)}_MCURR and current handle rotation matrix R_MCURR are transmitted in the current handle position and orientation signals on wires 111 of the USB bus, for example, to the master apparatus 64 in FIG. 1.

In addition, in the embodiment shown, referring to FIG. 1, the master apparatus 64 is coupled to a footswitch 170 actuable by the operator (surgeon) to provide a binary enablement signal to the master apparatus 64. When the footswitch 170 is not activated, i.e. not depressed, the enablement signal is in an active state and when the footswitch 170 is depressed the enablement signal is in an inactive state. The footswitch 170 thus controls the state of the enablement signal. As will be seen below, the enablement signal allows the user to cause the master apparatus 64 to selectively enable and disable movement of the end effectors in response to movement of the handles 102.

Referring now to FIG. 7, the end effector 73 and related structures are described. The fixed slave reference frame has axes x_s, y_s and z_s which intersect at a point referred to as the slave fixed base position 128, lying on the longitudinal axis 136 of the insertion tube 61 and contained in a plane perpendicular to the longitudinal axis 136 and containing a distal edge 103 of the insertion tube 61. The z_s axis is coincident with the longitudinal axis 136 of the insertion tube 61. The x_s−z_s plane thus contains the longitudinal axis 136 of the insertion tube 61 and the x_s and y_s axes define a plane perpendicular to the longitudinal axis 136 of the insertion tube 61.

In the embodiment shown, end effector 73 includes a pair of gripper jaws. Orthogonal axes x₂, y₂ and z₂ of an end effector Cartesian reference frame have an origin on the end effector axis, for example, at the intersection at the tip of the gripper jaws of the end effector 73. The origin of the end effector reference frame may be referred to as the end effector position 150 relative to the fixed slave reference frame x_s, y_s, z_s. Due to the mobility of the tool positioning device 81 and the mobility of the end effector 73 itself, the end effector position 150 can be placed at discrete positions within a volume in space. This volume may be referred to as the end effector translational workspace.

New end effector positions and end effector orientations are calculated by an end effector position and orientation calculation block 116 encoded in the master apparatus 64 shown in FIG. 3, in response to the current handle position signals {right arrow over (P)}_MCURR and current handle orientation signals R_MCURR and are represented by a new end effector position vector {right arrow over (P)}_EENEW and a new end effector rotation matrix R_EENEW, relative to the x_s, y_s, z_s fixed slave reference frame.

For example, the new end effector position vector {right arrow over (P)}_EENEW is a vector

$\left\{\begin{array}{c}{x}_2\\ y_2\\ z_2\end{array}\right\},$ where x₂, y₂, and z₂ represent coordinates of the end effector position 150 within the end effector translational workspace relative to the x_s, y_s, z_s fixed slave reference frame.

The end effector rotation matrix R_EENEW is a 3×3 matrix

$\left[\begin{array}{ccc}{x}_{2x}& y_{2x}& z_{2x}\\ x_{2y}& y_{2y}& z_{2y}\\ x_{2z}& y_{2z}& z_{2z}\end{array}\right],$ where the columns of the R_EENEW matrix represent the axes of the end effector reference frame x₂, y₂, z₂ written in the fixed slave reference frame x_s, y_s, z_s. R_EENEW thus defines a new orientation of the end effector 73 in the end effector translational workspace, relative to the x_s, y_s, z_s reference frame.

Referring back to FIG. 1, in the embodiment shown, the master apparatus 64 is controlled by program codes stored on a non-transitory computer readable medium such as a disk drive 114. The codes direct the master apparatus 64 to perform various functions. Referring to FIGS. 1 and 3, these functions may be grouped into categories and expressed as functional blocks of code including an end effector position and orientation calculation block 116, a kinematics block 118, a motion control block 120, and a base setting block 216, all stored on the disk drive 114 of the master apparatus 64. For ease of description, these blocks are shown as functional blocks within the master apparatus 64 in FIG. 3. These functional blocks are executed separately but in the same manner for each input device 58 and 60. The execution of these functional blocks for only input device 60 and end effector 73 will be described, it being understood they are separately executed in the same way for input device 58 and end effector 71 to achieve control of end effectors 73 and 71 by right and left hands respectively of the operator.

Generally, the end effector position and orientation calculation block 116 includes codes that direct the master apparatus 64 to produce the new end effector position and orientation signals, referred to herein as {right arrow over (P)}_EENEW and R_EENEW respectively.

The kinematics block 118 includes codes that direct the master apparatus 64 to produce configuration variables in response to the newly calculated end effector position and orientation signals {right arrow over (P)}_EENEW and R_EENEW.

The motion control block 120 includes codes that direct the master apparatus 64 to produce the slave control signals, in response to the configuration variables.

The base setting block 216 is executed asynchronously, whenever the enablement signal transitions from an inactive state to an active state, such as when the user releases the footswitch 170. The base setting block 216 directs the master apparatus 64 to set new reference positions and orientations for the handle 102 and end effector 73, respectively as will be described below.

Referring back to FIG. 1, in the embodiment shown, the slave control signals represent wire length values indicating how much certain wires of a given tool positioning device 81 of the slave subsystem 54 must be extended or retracted to cause the end effector 73 of the tool 67 to be positioned and rotated in a manner determined by positioning and rotating the corresponding input device 60.

Referring to FIGS. 1 and 3, the slave control signals representing the wire length values are transmitted to the slave computer 74, which has its own computer readable medium encoded with a communication interface block 124 including codes for directing the slave computer to receive the slave control signals from the master apparatus 64. The computer readable medium is also encoded with a motor control signal generator block 126 including codes for causing the slave computer 74 to generate motor control signals for controlling the motors 76 on the tool controller 78 to extend and retract the wires controlling the attached tool positioning device 81 according to the wire length values represented by the slave control signals from the master apparatus 64.

The kinematics block 118 receives newly calculated end effector position and orientation signals {right arrow over (P)}_EENEW and R_EENEW each time the end effector position and orientation calculation block 116 is executed. In response, the kinematics block 118 produces the configuration variables described below.

Referring to FIGS. 3 and 4, generally, the codes in the kinematics block 118 direct the master apparatus 64 to calculate values for the above configuration variables in response to the end effector position and rotation signals {right arrow over (P)}_EENEW and R_EENEW produced by the end effector position and orientation calculation block 116 and these calculated configuration values generally define a tool positioning device pose required to position end effector 73 at a desired location and at a desired orientation in the end effector translational workspace.

Referring to FIG. 4, the tool positioning device 81 has a first articulated segment 130, referred to as an s-segment and a second articulated segment 132 referred to as a distal segment. The segments 130 and 132 each include a plurality of “vertebra” 324. The s-segment 130 begins at a distance from the insertion tube 61, referred to as the insertion distance gins, which is the distance between the fixed slave base position 128 defined as the origin of the slave fixed base reference frame x_s, y_s, z_s and a first position 330 at the origin of a first position reference frame x₃, y₃, and z₃. The insertion distance gins represents an unbendable portion of the tool positioning device 81 that extends out of the end of the insertion tube 61. In the embodiment shown, the insertion distance gins may be about 10-20 mm, for example. In other embodiments, the insertion distance gins may be longer or shorter, varying from 0-100 mm, for example.

The s-segment 130 extends from the first position 330 to a third position 334 defined as an origin of a third reference frame having axes x₅, y₅, and z₅ and is capable of assuming a smooth S-shape when control wires (not shown) inside the s-segment 130 are pushed and pulled. The s-segment 130 has a mid-point at a second position 332, defined as the origin of a second position reference frame having axes x₄, y₄, z₄. The s-segment 130 has a length L₁, which in the embodiment shown may be about 65 mm, for example.

The distal segment 132 extends from the third position 334 to a fourth position 336 defined as an origin of a fourth reference frame having axes x₆, y₆, z₆. The distal segment 132 has a length L₂, which in the embodiment shown may be about 23 mm, for example.

The tool 67 also has an end effector length, which in the embodiment shown is a gripper length L₃ that extends from the fourth position 336 to the end effector position 150 defined as the origin of axes x₂, y₂, and z₂. The gripper length L₃, in this embodiment, may be about 25 mm, for example. The slave base position 128, first position 330, second position 332, third position 334, fourth position 336 and end effector position 150 may collectively be referred to as tool reference positions.

As explained in PCT/CA2013/001076, hereby incorporated herein by reference in its entirety, by pushing and pulling on certain control wires inside the tool positioning devices 79 and 81, the s-segment 130 can be bent into any of various degrees of an S-shape, from straight as shown in FIG. 7 to a partial S-shape as shown in FIG. 4 to a full S-shape. The s-segment 130 is sectional in that it has a first section 320 and a second section 322 on opposite sides of the second position 332. The first and second sections 320 and 322 lie in a first bend plane containing the first position 330, second position 332, and third position 334. The first bend plane is at an angle δ_prox to the x_s-z_s plane of the fixed slave reference frame. The first section 320 and second section 322 are bent in the first bend plane through opposite but equal angles θ_prox such that no matter the angle θ_prox or the bend plane angle θ_prox, the z₅ axis of the third position 334 is always parallel to and aligned in the same direction as the z₅ axis of the fixed slave base position 128. Thus, by pushing and pulling on the control wires within the tool positioning device 81, the third position 334 can be placed at any of a number of discrete positions within a cylindrical volume in space. This volume may be referred to as the s-segment workspace.

In addition, the distal segment 132 lies in a second bend plane containing the third position 334 and the fourth position 336. The second bend plane is at an angle δ_dist to the x_s-z_s plane of the fixed slave reference frame. The distal segment 132 is bent in the second bend plane at an angle θ_dist. Thus, by pushing and pulling the control wires within the tool positioning device 81, the fourth position 336 can be placed within another volume in space. This volume may be referred to as the distal workspace. The combination of the s-segment workspace plus the distal workspace can be referred to as the tool positioning device workspace, as this represents the total possible movement of the tools 66 and 67 as effected by the respective tool positioning devices 79 and 81.

The distance between the fourth position 336 and the end effector position 150 is the distance between the movable portion of the distal segment 132 and the tip of the gripper end effector 73 in the embodiment shown, i.e. the length L₃. Generally, the portion of the gripper between the fourth position 336 and the end effector position 150 (L₃) will be unbendable.

In the embodiment shown, the end effector 73 is a gripper jaw tool that is rotatable about the z₂ axis in the x₂-y₂ plane of the end effector reference frame, the angle of rotation being represented by an angle γ relative to the positive x₂ axis. Finally, the gripper jaws may be at any of varying degrees of openness from fully closed to fully open (as limited by the hinge). The varying degrees of openness may be defined as the “gripper”.

In summary therefore, the configuration variables provided by the kinematic block 118 codes are as follows:

• • q_ins: represents a distance from the slave base position 128 defined by axes x_s, y_s, and z_s to the first position 330 defined by axes x₃, y₃ and z₃ where the s-segment 130 of the tool positioning device 81 begins;
  • δ_prox: represents a first bend plane in which the s-segment 130 is bent relative to the x_s−y_s plane of the fixed slave reference frame;
  • θ_prox: represents an angle at which the first and second sections 320 and 322 of the s-segment 130 is bent in the first bend plane;
  • δ_dist: represents a second bend plane in which the distal segment 132 is bent relative to the x_s−y_s plane of the fixed slave reference frame;
  • θ_dist: represents an angle through which the distal segment 132 is bent in the second bend;
  • γ: represents a rotation of the end effector 73 about axis z₂; and
  • Gripper: represents a degree of openness of the gripper jaws of the end effector 73. (This is a value which is calculated in direct proportion to a signal produced by an actuator (not shown) on the handle 102 indicative of an amount of pressure the operator exerts by squeezing the handle).

To calculate the configuration variables, it will first be recalled that the end effector rotation matrix R_EENEW is a 3×3 matrix:

$\left[\begin{array}{ccc}{x}_{2x}& y_{2x}& z_{2x}\\ x_{2y}& y_{2y}& z_{2y}\\ x_{2z}& y_{2z}& z_{2z}\end{array}\right].$

Since the last column of R_EENEW is the z-axis of the end effector reference frame written relative to the fixed slave reference frame x_s, y_s and z_s, the values exist, δ_dist, and γ associated with the distal segment 132 can be calculated according to the relations:

$\begin{array}{cc}{\theta }_{\mathrm{dist}}=\frac{\pi }{2}-a\tan 2\left(\sqrt{z_{2x}^2+z_{2y}^2},z_{2z}\right)& \left(2\right)\\ {\delta }_{\mathrm{dist}}=-a\tan 2\left(z_{2y},z_{2x}\right)\mathrm{If}{\delta }_{\mathrm{dist}}>\frac{\pi }{2}& \left(3\right)\\ \gamma =a\tan 2\left(-y_{2z},x_{2z}\right)-{\delta }_{\mathrm{dist}}+\pi \mathrm{else}& \left(4a\right)\\ \gamma =a\tan 2\left(y_{2z},-x_{2z}\right)-{\delta }_{\mathrm{dist}}& \left(4b\right)\end{array}$

These values can then be used to compute the locations of the third position 334, the fourth position 336, and the end effector position 150 relative to the fixed slave base position 128. The locations may be expressed in terms of vectors p_3/s from the fixed slave base position 128 to the first position 330, p_4/3 from the third position 334 to the fourth position 336, and p_5/4 from the fourth position 336 to the end effector position 150. p_3/s is then calculated from {right arrow over (P)}_EENEW as follows:

$\begin{array}{cc}{\stackrel{\_}{p}}_{3/s}={\stackrel{\_}{p}}_{\mathrm{EENEW}}-{\stackrel{\_}{p}}_{4/3}-{\stackrel{\_}{p}}_{5/4},\mathrm{where}:& \left(5\right)\\ {\stackrel{\_}{p}}_{4/3}·\stackrel{\_}{i}=\frac{-L_2\cos {\delta }_{\mathrm{dist}}\left(\sin {\theta }_{\mathrm{dist}}-1\right)}{\frac{\pi }{2}-{\theta }_{\mathrm{dist}}}& \left(6a\right)\\ {\stackrel{\_}{p}}_{4/3}·\stackrel{\_}{j}=\frac{L_2\sin {\delta }_{\mathrm{dist}}\left(\sin {\theta }_{\mathrm{dist}}-1\right)}{\frac{\pi }{2}-{\theta }_{\mathrm{dist}}}& \left(6b\right)\\ {\stackrel{\_}{p}}_{4/3}·\stackrel{\_}{k}=\frac{L_2\cos \left({\theta }_{\mathrm{dist}}\right)}{\frac{\pi }{2}-{\theta }_{\mathrm{dist}}}& \left(6c\right)\\ {\stackrel{\_}{p}}_{5/4}·\stackrel{\_}{i}=L_3\cos \left({\delta }_{\mathrm{dist}}\right)\cos \left({\theta }_{\mathrm{dist}}\right)& \left(7a\right)\\ {\stackrel{\_}{p}}_{5/4}·\stackrel{\_}{j}=-L_3\sin \left({\delta }_{\mathrm{dist}}\right)\cos \left({\theta }_{\mathrm{dist}}\right)& \left(7b\right)\\ {\stackrel{\_}{p}}_{5/4}·\stackrel{\_}{k}=L_3\sin \left({\theta }_{\mathrm{dist}}\right),& \left(7c\right)\end{array}$

where:

• • ī is a unit vector in the x direction;
  • j is a unit vector in the y direction; and
  • k is a unit vector in the z direction.

Once the vector from the fixed slave base position 128 to the third position 334 (p_3/s) is known, the configuration variables, δ_prox and θ_prox, for the s-segment 130 can be found. The configuration variable Γ_prox associated with the s-segment 130 is calculated by solving the following two equations for δ_prox:

$\begin{array}{cc}{\stackrel{\_}{p}}_{3/s}·\stackrel{\_}{i}=\frac{-L_1\cos {\delta }_{\mathrm{prox}}\left(\sin {\theta }_{\mathrm{prox}}-1\right)}{\frac{\pi }{2}-{\theta }_{\mathrm{prox}}}& \left(8a\right)\\ {\stackrel{\_}{p}}_{3/s}·\stackrel{\_}{j}=\frac{L_1\sin {\delta }_{\mathrm{prox}}\left(\sin {\theta }_{\mathrm{prox}}-1\right)}{\frac{\pi }{2}-{\theta }_{\mathrm{prox}}}.& \left(8b\right)\end{array}$

The ratio of (8b) and (8a) givesδ_prox=a tan 2(−p_3/s·j,p_3/s·ī),  (9)where ī and j are unit vectors in the x and y directions respectively.

A closed form solution cannot be found for θ_prox, thus θ_prox must be found with a numerical equation solution to either of equations (8a) or (8b). A Newton-Raphson method, being a method for iteratively approximating successively better roots of a real-valued function, may be employed, for example. The Newton-Raphson method can be implemented using the following equations:

$\begin{array}{cc}f\left({\theta }_{\mathrm{prox}}\right)=\frac{L_1}{\frac{\pi }{2}-{\theta }_{\mathrm{prox}}}\cos {\delta }_{\mathrm{prox}}\left(1-\sin {\theta }_{\mathrm{prox}}\right)-{\stackrel{\_}{p}}_{3/s}·\stackrel{\_}{i}=0,& \left(10\right)\end{array}$ where ī is the unit vector in the x direction.

The equation (10) is equation (8a) rewritten in the form f(θ_prox)=0. The Newton-Raphson method tends to converge very quickly because in the range 0<θ<_prox<π, the function has a large radius of curvature and has no local stationary points. Following the Newton-Raphson method, successive improved estimates of θ_prox can be made iteratively to satisfy equation (10) using the following relationship:

$\begin{array}{cc}{\theta }_{n+1}={\theta }_n-\frac{f\left({\theta }_n\right)}{f^{\prime }\left({\theta }_n\right)}& \left(11\right)\end{array}$

Finally, upon determination of θ_prox, the following equation can be used to find q_ins,

$\begin{array}{cc}{q}_{\mathrm{ins}}=-{\stackrel{\_}{p}}_{3/s}·\stackrel{\_}{k}-\frac{L_1\cos {\theta }_{\mathrm{prox}}}{\frac{\pi }{2}-{\theta }_{\mathrm{prox}}},& \left(12\right)\end{array}$ where:

• • k is the unit vector in the z direction;
  • p_3/s·k is the dot product of the vector p_3/s and the unit vector k.

The codes in the kinematics block 118 shown in FIG. 6 direct the master apparatus 64 to calculate values for the above configuration variables in response to the end effector position and orientation signals {right arrow over (P)}_EENEW and R_EENEW produced by the end effector position and orientation calculation block 116 and these calculated configuration variables generally define a tool positioning device pose required to position the end effector 73 at a desired location and at a desired orientation in the end effector workspace.

It will be appreciated that configuration variables are produced for each end effector 71 and 73 and therefore in the embodiment shown, two sets of configuration variables which will be referred to as left and right configuration variables respectively are produced and forwarded or otherwise made available to the motion control block 120.

Referring to FIG. 5, the master apparatus 64 queries the control unit 92 for the current handle position vector {right arrow over (P)}_MCURR and current handle rotation matrix R_MCURR periodically, at a sample rate of about 1 kHz. These values are stored by the master apparatus 64 in a first “current” buffer 140 having a first store 142 storing the three values representing the currently acquired handle position vector {right arrow over (P)}_MCURR and a second store 144 storing the nine values representing the acquired handle rotation matrix R_MCURR.

Referring to FIGS. 2 and 5, the master apparatus 64 also stores values x_mb, y_mb, z_mb representing a definable master base position represented by a base position vector {right arrow over (P)}_MBASE in a third store 146 and stores values representing a definable master base rotation matrix R_MBASE in a fourth store 148. The master apparatus 64 initially causes the definable master base position vector {right arrow over (P)}_MBASE to be set equal to the current handle position vector {right arrow over (P)}_MCURR on startup of the system and causes the definable master base rotation matrix R_MBASE to define an orientation that is the same as the orientation defined by the current handle rotation matrix R_MCURR associated with the current handle rotation, on startup of the system.

Initially, therefore:{right arrow over (P)}_MBASE={right arrow over (P)}_MCURR; andR_MBASE=R_MCURR.

In other words, the master base reference frame and the handle reference frame coincide at startup.

Thereafter, the master base position vector {right arrow over (P)}_MBASE and the master base rotation matrix R_MBASE are maintained at the same values as on startup until the enablement signal is activated, such as by inactivation of the footswitch (170 in FIGS. 1 and 3), which causes the enablement signal to transition from the inactive state to the active state. In response, the base setting block 216 shown in FIG. 3 and more in detail in FIG. 6 is executed to change the master base position vector {right arrow over (P)}_MBASE and master base rotation matrix R_MBASE to the currently acquired handle position vector {right arrow over (P)}_MCURR and currently acquired handle orientation matrix R_MCURR respectively.

Referring to FIGS. 5 and 7, the master apparatus 64 further stores values x_sb, y_sb, z_sb representing a definable slave base position vector {right arrow over (P)}_EEBASE in a fifth store 152 and stores values representing a definable slave base rotation matrix R_EEBASE in a sixth store 154. The master apparatus 64 initially causes the definable slave base position vector {right arrow over (P)}_EEBASE to be set equal to the new end effector position vector {right arrow over (P)}_EENEW and causes the definable slave base rotation matrix R_EEBASE to define an orientation that is the same as the orientation defined by the new end effector rotation matrix R_EENEW, on startup of the system.

Initially, therefore:{right arrow over (P)}_EEBASE={right arrow over (P)}_EENEW; andR_EEBASE=R_EENEW.

In other words, the slave base reference frame and the end effector reference frame coincide at startup.

The slave base position {right arrow over (P)}_EEBASE and slave base rotation matrix R_EEBASE are maintained at the same values as on startup until the enablement signal is activated such as by inactivation of the footswitch (170 in FIGS. 1 and 3), which causes the enablement signal to transition from the inactive state to the active state. In response, the base setting block 216 in FIG. 6 directs the master apparatus 64 to change the slave base position vector {right arrow over (P)}_EEBASE and slave base rotation matrix R_EEBASE to the newly calculated end effector position vector {right arrow over (P)}_EENEW and newly calculated end effector rotation matrix R_EENEW.

Referring to FIGS. 3 and 8A to 8D, the end effector position and orientation calculation block 116 is executed each time a set of new values for {right arrow over (P)}_MCURR and R_MCURR are acquired from the control unit 92. It begins with a first block 161 that directs the master apparatus 64 to check the state of the enablement signal to determine whether it is active or inactive. If the enablement signal is active, then the process continues at block 160 shown in FIG. 8B. Block 160 directs the master apparatus 64 to produce and store, in a seventh store 162 in FIG. 5, values representing the new end effector position vector {right arrow over (P)}_EENEW and to produce and store, in an eighth store 164 in FIG. 5, values representing the new end effector rotation matrix R_EENEW.

To produce new end effector position signals {right arrow over (P)}_EENEW and new end effector orientation signals R_EENEW representing a desired end effector position 150 and desired end effector orientation, relative to the slave base position 128 and the slave base rotation, the new end effector position signals {right arrow over (P)}_EENEW and new end effector orientation signals R_EENEW are calculated according to the following relations:{right arrow over (P)}_EENEW=A({right arrow over (P)}_MCURR−{right arrow over (P)}_MBASE)+{right arrow over (P)}_EEBASE andR_EENEW=R_EEBASER_MBASE⁻¹R_MCURR

• Where: {right arrow over (P)}_EENEW is the new end effector position vector that represents the new desired end effector position 150 of the end effector 73 in the end effector workspace, relative to the slave base reference frame;
  • A is a scalar value representing a scaling factor in translational motion between the master and the slave;
  • {right arrow over (P)}_MCURR is the current representation of the handle position vector stored in the first store 142, the handle position vector being relative to the fixed master reference frame;
  • {right arrow over (P)}_MBASE is the last-saved position vector {right arrow over (P)}_MCURR for the handle 102 that was saved upon the last inactive to active state transition of the enablement signal such as by release of the footswitch 170 or on system initialization or by operation of a control interface by the operator;
  • {right arrow over (P)}_EEBASE is the last-saved position vector {right arrow over (P)}_EENEW for the end effector 73 that was saved upon the last inactive to active state transition of the enablement signal;
  • R_EENEW is the new end effector rotation matrix representing the current orientation of the end effector 73 relative to the slave reference frame;
  • R_EEBASE is the rotation matrix representing the last-saved orientation of the end effector 73 saved upon the last inactive to active state transition of the enablement signal;
  • R_MBASE⁻¹ is the inverse of rotation matrix R_MBASE, where R_MBASE is a rotation matrix representing the last-saved orientation of the handle 102 saved upon the last inactive to active state transition of the enablement signal;
  • R_MCURR is the currently acquired rotation matrix representing the orientation of the handle 102 relative to the fixed master reference frame.

The following describes how the master apparatus 64 is controlled by the codes in the end effector position and orientation calculation block 116 to effect autonomous alignment of the orientation of the end effector 73 with the handle 102 after clutching and to effect autonomous alignment of the z-axes of the handle 102 and end effector 73 for wrist roll management.

Referring to FIG. 8B, after values representing the desired end effector position and orientation signals {right arrow over (P)}_EENEW and R_EENEW are calculated by the master apparatus 64 executing block 160, the master apparatus 64 is directed to block 202 which directs the master apparatus to determine whether the system 50 is configured to allow wrist roll management. A simple binary wrist roll management signal selectively set by the operator is used to indicate to the master apparatus 64 whether the system 50 is configured to allow wrist roll management or not. If the system is not configured for wrist roll management, block 204 directs the master apparatus 64 to compute a rotation matrix that carries the newly calculated end effector orientation into the current handle orientation (R_{EE_TO_MASTER}) by the relation:R_{EE_TO_MASTER}=R_EENEW⁻¹R_MCURR

• Where: R_EENEW⁻¹ is the inverse matrix of the end effector rotation matrix R_EENEW represented by a 3×3 matrix stored in the eighth store 164 in FIG. 3; and
  • R_MCURR is the current handle rotation matrix represented by the 3×3 matrix stored in the second store 144 in FIG. 3.

Then, block 206 directs the master apparatus 64 to compute an angle of rotation associated with R_{EE_TO_MASTER} (ϕ_{EE_TO_MASTER}) by the relation:ϕ_{EE_TO_MASTER}=a cos(0.5 trace(R_{EE_TO_MASTER})−1)

This angle of rotation (ϕ_{EE_TO_MASTER}) represents the alignment difference between the orientation of the handle 102 and the newly calculated end effector orientation.

In a special case, applicable to the embodiment described here, it is desirable that to be aligned, only the z-axes of the reference frames described by R_EENEW and R_MCURR be coincident. In this case the handle 102 and the end effector 73 point in the same direction relative to their respective fixed reference frames (x_r y_r, z_r and x_s, y_s, z_s, respectively) and the roll about their z-axis is not considered.

In this special case therefore, blocks 204 and 206 shown in FIG. 8B are replaced with block 205 shown in FIG. 9 which involves the following computationϕ_{EE_TO_MASTER}=a cos(R_EENEW(1,3)*R_{M_CURR}(1,3)+R_EENEW(2,3)*R_{M_CURR}(2,3)+R_EENEW(3,3)*R_MCURR(3,3))

• • Where (i,j) represent matrix row (i) and column (j) indices.

This computation represents the angle obtained from the dot product of the z-axes of the handle reference frame and end effector reference frame.

Referring back to FIG. 8B, alternatively, if at block 202 it is determined that the system is configured to allow wrist roll management, block 208 directs the master apparatus 64 to compute an offset angle between the handle reference frame and the end effector reference frame z-axes (ϕ_{EE_TO_MASTER}) by the relation:

${\varphi }_{\mathrm{EE}\_\mathrm{TO}\_\mathrm{MASTER}}=\mathrm{acos}\left(\left\{\begin{array}{c}{R}_{\mathrm{EENEW}1,3}\\ R_{\mathrm{EENEW}2,3}\\ R_{\mathrm{EENEW}3,3}\end{array}\right\}·\left\{\begin{array}{c}{R}_{\mathrm{MCURR}1,3}\\ R_{\mathrm{MCURR}2,3}\\ R_{\mathrm{MCURR}3,3}\end{array}\right\}\right)$

After executing either block 206 or block 208, the angle of rotation by which the handle 102 and end effector 73 are out of alignment, i.e. the alignment error, is given by ϕ_{EE_TO_MASTER}.

The master apparatus 64 is then directed to block 210 which causes it to determine whether the alignment error ϕ_{EE_TO_MASTER} meets a criterion, such as being above a threshold value. If the alignment error is not above the threshold value, the current handle orientation R_MCURR and new end effector orientation R_EENEW are considered to be aligned.

Then, block 215 directs the master apparatus 64 to signal the motion control block 120 of FIG. 3 to indicate that slave control signals based on the newly calculated values for {right arrow over (P)}_EENEW and R_EENEW are to be sent to the slave computer 74. This causes the end effector 73 to assume a position and orientation determined by the current position and current orientation of the handle 102 when the alignment difference meets the criterion.

Block 159 then directs the master apparatus 64 to copy the newly calculated end effector position vector {right arrow over (P)}_EENEW and end effector rotation matrix R_EENEW into an eleventh and twelfth stores 147 and 149 of a previous buffer 141 of FIG. 5. The newly calculated end effector position vector {right arrow over (P)}_EENEW and newly calculated end effector rotation matrix R_EENEW are thus renamed as “previously calculated end effector position vector” {right arrow over (P)}_EEPREV and “previously calculated end effector rotation matrix” R_EEPREV. By storing the newly calculated end effector position vector {right arrow over (P)}_EENEW and newly calculated end effector rotation matrix R_EENEW, as previously calculated end effector position vector {right arrow over (P)}_EEPREV and previously calculated end effector rotation matrix R_EEPREV, a subsequently acquired new end effector position vector {right arrow over (P)}_EENEW and subsequently acquired new end effector rotation matrix R_EENEW can be calculated from the next current handle position vector {right arrow over (P)}_MCURR and next current handle position matrix R_MCURR.

The end effector position and orientation calculation block 116 is thus completed, and the calculated {right arrow over (P)}_EENEW and R_EENEW values stored in the seventh and eighth stores 162 and 164 are available for use by the kinematics block 118.

If at block 210 the alignment error is above the threshold value, block 214 directs the master apparatus 64 to produce a rotation matrix that carries the previous handle orientation into current handle orientation (R_DIFF), according to the relation:R_DIFF=R_MPREV⁻¹R_MCURR

• • Where: R_MPREV⁻¹ is the inverse of the previous handle rotation matrix stored in the tenth store 145 of FIG. 3; and
    • R_MCURR is the current handle rotation matrix stored in the second store 144 of FIG. 3.

Referring to FIG. 8C, the master apparatus 64 is then directed by block 230 to determine a unit vector in the direction of an axis of rotation (e_DIFF) associated with R_DIFF by the relation:

$\stackrel{\_}{p}=0.5\left\{\begin{array}{c}{R}_{\mathrm{DIFF}3,2}-R_{\mathrm{DIFF}2,3}\\ R_{\mathrm{DIFF}1,3}-R_{\mathrm{DIFF}3,1}\\ R_{\mathrm{DIFF}2,1}-R_{\mathrm{DIFF}1,2}\end{array}\right\}$ ${\stackrel{\_}{e}}_{\mathrm{DIFF}}=\frac{\stackrel{\_}{p}}{\stackrel{\_}{p}}$

Then, block 216 directs the master computer 64 to compute an angle of rotation (ϕ_DIFF) associated with R_DIFF by the relation:ϕ_DIFF=a cos(0.5 trace(R_DIFF)−1)

Then, block 218 directs the master computer 64 to compute an angular speed of the rotation (ω_DIFF) associated with R_DIFF by the relation:ω_DIFF=sr·ϕ_DIFF

• • Where: sr=sample rate in Hz at which {right arrow over (P)}_MCURR and R_MCURR values are acquired from the control unit (92).

Then, block 220 directs the master apparatus 64 to determine whether the angular speed ω_DIFF meets a second criterion such as being above a threshold speed to initiate auto alignment. (This may avoid any automated motion when the user is performing slow fine movements that may be undesirable.)

If not, the master apparatus 64 is directed to block 222 which corresponds to location “E” on FIG. 8B to execute blocks 215 and 159 and then exit the end effector position and orientation calculation block 116. The saved {right arrow over (P)}_EENEW and R_EENEW values in the seventh and eighth stores 162 and 164 of FIG. 3 are then available for use by the kinematics block 118.

If at block 220 the angular speed ω_diff is above the threshold speed, block 224 directs the master computer 64 to determine whether the system is configured to allow wrist roll misalignment by reading the status of the wrist roll management signal set by the operator. If the wrist roll management signal is not active, block 226 directs the master apparatus 64 to determine a misalignment axis e_ERR, i.e. the axis of rotation associated with R_{EE_TO_MASTER} by the relation

$\stackrel{\_}{q}=0.5\left\{\begin{array}{c}{R}_{\mathrm{EE}\_\mathrm{TO}\_\mathrm{MASTER}3,2}-R_{\mathrm{EE}\_\mathrm{TO}\_\mathrm{MASTER}2,3}\\ R_{\mathrm{EE}\_\mathrm{TO}\_\mathrm{MASTER}1,3}-R_{\mathrm{EE}\_\mathrm{TO}\_\mathrm{MASTER}3,1}\\ R_{\mathrm{EE}\_\mathrm{TO}\_\mathrm{MASTER}2,1}-R_{\mathrm{EE}\_\mathrm{TO}\_\mathrm{MASTER}1,2}\end{array}\right\}$ ${\stackrel{\_}{e}}_{\mathrm{ERR}}=\frac{\stackrel{\_}{q}}{\stackrel{\_}{q}}$

Alternatively, if the system is configured to allow wrist roll misalignment, block 228 directs the master apparatus 64 to determine a unit vector in the direction of the misalignment axis e_ERR by the relation:

$e_{\mathrm{ERR}}=\frac{1}{\sin \left({\varphi }_{\mathrm{EE}\_\mathrm{TO}\_\mathrm{MASTER}}\right)}\left\{\begin{array}{c}{R}_{\mathrm{EENEW}1,3}\\ R_{\mathrm{EENEW}2,3}\\ R_{\mathrm{EENEW}3,3}\end{array}\right\}\times \left\{\begin{array}{c}{R}_{\mathrm{MCURR}1,3}\\ R_{\mathrm{MCURR}2,3}\\ R_{\mathrm{MCURR}3,3}\end{array}\right\}$

Now referring to FIG. 8D, block 232 directs the master apparatus 64 to compute the component of the handle rotation in the misalignment plane L by the relation:L=|ē_DIFF·ē_ERR|

Then, block 234 directs the master apparatus 64 to compute a correction angle ϕ_C, as a function of the angle of the master rotation ϕ_DIFF and the component of the master rotation in the misalignment plane L by the relation:ϕ_C=Bϕ_DIFFf(L)

• Where: B is a scaling factor that defines the fraction of ϕ_DIFF that should be used in the correction angle ϕ_c, for example B=0.5.
  • f(L) is a function that alters the profile of how the correction angle ϕ_c changes with the plane of orientation change, for example f(L)=L³.

Then, block 236 directs the master apparatus 64 to compute a correction matrix R_C(ϕ_C, e_ERR) to rotate by the correction angle ϕ_c about the misalignment axis e_ERR.

The correction matrix R_c is determined by the relation:R_C(ϕ_C,e_ERR)=e_ERRe_ERR^T+cos(ϕ_C)*(I−e_ERRe_ERR^T)+sin(ϕ_C)E_ERR

Where: E_ERR=the cross product matrix of vector e_ERR

$E_{\mathrm{ERR}}=\left[\begin{array}{ccc}0& -e_{\mathrm{ERR}3}& e_{\mathrm{ERR}2}\\ e_{\mathrm{ERR}3}& 0& -e_{\mathrm{ERR}1}\\ -e_{\mathrm{ERR}2}& e_{\mathrm{ERR}1}& 0\end{array}\right]$ $I=\mathrm{the}3\times 3\mathrm{identity}\mathrm{matrix}\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$

Then, block 238 directs the master apparatus 64 to modify the end effector base matrix using the correction matrix, R_c by the relation:R_EEBASE′=R_EEBASER_C

The R_EEBASE′ value calculated by block 238 is then saved in the sixth store 154, of FIG. 3 as R_EEBASE.

Then, block 244 directs the master apparatus 64 to re-compute the end effector rotation matrix R_EENEW using the new end effector base rotation matrix R_EEBASE and store this new end effector orientation as R_EENEW in store 164, of FIG. 3.R_EENEW=RR_EEBASE

• Where: R is a rotation matrix describing the rotation between the master base rotation matrix and R_MBASE the current handle rotation matrix R_MCURR.R=R_MCURRR_MBASE⁻¹

Then, block 240 directs the master apparatus 64 to location “E” in FIG. 8B to execute blocks 215 and 159 and the end effector positioning and orientation calculation block 116 is ended.

Alternatively referring to FIG. 8E blocks 250 and 252 can replace blocks 238 and 244 in FIG. 8D and the use of base frames R_MBASE and R_EEBASE to calculate R_MCURR and R_EENEW can be avoided, requiring consideration of only the reference frames x_r, y_r, z_r and x_s, y_s, z_s for the master and slave, respectively, and the task frames x₁, y₁, z₁ and x₂, y₂, z₂ for the master and slave, respectively. In this alternative, the misalignment R_Δ between R_m and R_ee is calculated upon system activation using the relation R_Δ=R_m⁻¹ R_ee. The master-slave misalignment (R_Δ) is then used to account for the misalignment as the new values of R_ee are computed using the relation R_EENEW=R_MCURRR_Δ. A correction matrix (R_c) is then used in block 250 to adjust R_Δ each time step using the relation R_Δ′=R_ΔR_C until the alignment reaches the alignment criterion.

Referring back to FIG. 3, the kinematics block 118 then directs the master apparatus 64 to use the saved values for {right arrow over (P)}_EENEW and {right arrow over (R)}_EENEW to determine the configuration variables. The configuration variables are made available to the motion control block 120 and the motion control block 120 produces the slave control signals representing wire lengths. Referring back to FIG. 8A, block 161 has the effect that the slave control signals based on {right arrow over (P)}_EENEW and R_EENEW are transmitted to the slave computer 74 when the footswitch 170 is not depressed. When the footswitch 170 is depressed the process continues at block 163, and the slave control signals based on {right arrow over (P)}_EEPREV and R_EEPREV are transmitted to the slave computer 74 when the footswitch 170 is depressed.

It will be appreciated that the above routine is executed by the end effecter position and orientation calculation block 116 after each sample of the {right arrow over (P)}_MCURR and R_MCURR values is acquired from the control unit 92. Suitable values for the scaling factor B at block 234 and suitable choices for the correction angle function f(L) at block 234 will cause the R_EEBASE saved in store 154 to be updated each time the routine is executed, i.e. each time new {right arrow over (P)}_MCURR and R_MCURR values are acquired, until the alignment error ϕ_{EE_TO_MASTER} is below the alignment threshold at which time the handle 102 and the end effector 73 are considered to be aligned and no further modification of the R_EEBASE value occurs.

Generally, when the enablement signal is in the inactive state, the handle 102 on input device 61 can be moved and rotated and the calculations of {right arrow over (P)}_EENEW and R_EENEW will still be performed, but there will be no movement of the end effector 73. This allows “clutching” or repositioning the handle 102 without corresponding movement of the end effector 73, to enable the end effector 73 to have increased range of movement and to allow the operator to reposition their hands to a more comfortable position within the master translational workspace. For example, referring to FIG. 7, the operator could push the handle 102 in the z_r direction toward the control unit 92 while the enablement signal is active, whereby the end effector 73 is moved in the z_s direction corresponding to the movement of the handle 102. Then, the operator can actuate the footswitch 170 to set the enablement signal inactive and while the footswitch is actuated, withdraw the handle 102 along the z_r axis in the opposite direction, away from the control unit 92, while the end effector 73 remains stationary due to the enablement signal being inactive. Then, the operator can release the footswitch 170 to set the enablement signal active and then continue pushing the handle 102 in the z_r direction toward the control unit 92 while the end effector 73 is moved in the z_s direction corresponding to the movement of the handle 102. Similar effects are experienced with rotational movements of the handle 102 and the end effector 73, when the enablement signal is active and inactive, to achieve a clutching effect in rotation.

The above clutching effect is achieved by causing movements of the handle 102 and movements of the end effector 73 to be made relative to the last-saved master base position {right arrow over (P)}_MBASE and orientation R_MBASE and the last saved slave base position {right arrow over (P)}_EEBASE and orientation R_EEBASE respectively. The master computer 64 stores the current values of the current handle position {right arrow over (P)}_MCURR and current handle orientation R_MCURR signals as new values of the master base position signals {right arrow over (P)}_MBASE and new values of the master base orientation signals R_MBASE respectively, and stores the current values of the end effector position signals {right arrow over (P)}_EENEW and new end effector orientation R_EENEW signals as new values of the slave base position signals {right arrow over (P)}_EEBASE and new values of the slave base orientation signals R_EEBASE respectively, in response to the enablement signal transitioning from the “not active” state to the “active” state. Otherwise, upon release of the footswitch 170, the end effector 73 would “snap” to the absolute position directly mapped to the position and orientation of the handle 102 and this could be dangerous if it were to occur inside a patient because the end effector 73 could tear into tissue or internal organs of the patient with possibly life-threatening effects. In addition, the surgeon would feel somewhat out of control of the end effectors 71 and 73.

While the above described clutching effect is desirable to match the range of translational movement of the end effector 73 with the range of movement of the handle 102 and to reposition the hands of the operator to a comfortable position, it is not desirable for clutching to result in reorientation of the handle within the master rotational workspace because orientation control can become unnatural or unintuitive to the operator when there is a misalignment between the handle 102 and end effector 73. In the absence of a mechanical means to maintain the orientation of the handle 102 it would be difficult for the operator to rotate the handle 102 to cause it to be exactly aligned with the end effector 73 on release of the footswitch 170 so that normal operation can be resumed. In this regard, the codes of the end effector position and orientation calculation block 116 direct the master apparatus 64 to detect a difference, between the current handle orientation signals R_MCURR and the new end effector orientation signals R_EENEW, the difference representing a difference in physical alignment between the end effector 73 and the handle 102 relative to their respective fixed reference frame. In response to detecting the difference, the codes cause the master apparatus 64 to adjust the saved slave base orientation signals R_EEBASE to ultimately have values close to the same values as the saved master base orientation signals R_MBASE so that subsequent generations of the end effector orientation signals R_EENEW cause the slave control signals produced by the motion control block 120 to cause the end effector 73 to be physically aligned with the handle 102 relative to their respective fixed reference frames.

This technique of adjusting the saved slave base orientation signals R_EEBASE also has applications in providing a wrist-roll management feature, where wrist roll is measured as variations of orientation of the handle 102 relative to only the z-axis. The wrist roll management feature would have the effect of correcting only for misalignment for the direction in which the end effector 73 and the handle 102 are pointing and not the rotation about the axis 134 of the end effector 73.

Generally, the above described system may cause smooth autonomous motion of the end effector 73 toward alignment with the handle 102, when there is a misalignment between the handle 102 and the end effector 73 without compromising control of the end effector 73 for the operator. In addition, if the alignment error ϕ_{EE_TO_MASTER} exceeds the threshold value, the alignment error will always be reduced no matter what direction the handle 102 is moving, unless e_DIFF and e_ERR are parallel and f(A) is such that f(A)=0 when A=0.

While specific embodiments of the invention have been described and illustrated, such embodiments should be considered illustrative of the invention only and not as limiting the invention as construed in accordance with the accompanying claims.

Claims

1. A method of correcting alignment between a slave end effector and a master actuator in a robotic system, the method comprising, by a processor: receiving signals representing an orientation of the master actuator within a master reference frame;determining a state of an enablement signal that permits movement of the end effector responsive to movement of the master actuator;while the enablement signal is in an active state, successively producing control signals for controlling an orientation of the end effector with respect to a slave reference frame in response to changes in orientation of the master actuator;in response to the enablement signal being in an inactive state, facilitating repositioning of the master actuator without corresponding movement of the end effector;in response to the enablement signal transitioning from the inactive state to the active state, determining an alignment error between an orientation of the master actuator with respect to the master reference frame and an orientation of the end effector with respect to the slave reference frame; andwhile the alignment error satisfies a threshold and the enablement signal is in the active state, making successive adjustments to successively produced control signals, the successive adjustments causing a reduction of the alignment error, the successive adjustments being made until an alignment criterion between the slave reference frame and the master reference frame is satisfied.

2. The method of claim 1 wherein determining the alignment error comprises computing an offset angle between the master reference frame and the slave reference frame.

3. The method of claim 2 wherein computing the offset angle is performed while permitting a roll alignment error about the respective z-axes between the slave reference frame and the master reference frame.

4. The method of claim 2 wherein computing the offset angle comprises generating a rotation matrix that carries the end effector orientation at the time of the enablement signal transition into the master actuator orientation at the time of the enablement signal transition and determining the offset angle associated with the rotation matrix.

5. The method of claim 2 wherein the alignment criterion is satisfied in response to the offset angle between the slave reference frame and the master reference frame being less than a threshold value.

6. The method of claim 1 wherein making the successive adjustments comprises, for each successive adjustment: determining a misalignment axis associated with the alignment error;determining an axis and an angle of rotation associated with the change in orientation of the master actuator;computing a component of rotation associated with the changes in orientation of the master actuator in the direction of the misalignment axis; andcomputing a correction angle for the adjustment based on the angle of rotation of the master actuator and the computed component of rotation.

7. The method of claim 1 further comprising computing an angular speed of rotation associated with each change in orientation of the master actuator and, while the angular speed of rotation does not satisfy a threshold speed associated with reducing the alignment error, discontinuing making the successive adjustments to the successively produced control signals.

8. The method of claim 7 further comprising resuming making the successive adjustments to the successively produced control signals in response to the determined angular speed of rotation satisfying the threshold speed.

9. The method of claim 7 wherein computing the angular speed of rotation comprises: generating a rotation matrix that carries a previous master actuator orientation into a current master actuator orientation;computing an angle of rotation associated with the rotation matrix; andcomputing the angular speed of rotation based on the angle of rotation and a sampling rate for the master actuator.

10. The method of claim 1 wherein making successive adjustments to successively produced control signals comprises successively adjusting an orientation of the slave reference frame until the slave reference frame meets the alignment criterion.

11. An apparatus for correcting alignment between a slave end effector and a master actuator in a robotic system, the apparatus comprising: a communication interface; anda processor configured to: receive, over the communication interface, signals representing an orientation of the master actuator within a master reference frame;determining a state of an enablement signal that permits movement of the end effector responsive to movement of the master actuator;successively produce control signals for controlling an orientation of the end effector with respect to a slave reference frame in response to changes in orientation of the master actuator while the enablement signal is in an active state;facilitate repositioning of the master actuator without corresponding movement of the end effector when the enablement signal is in an inactive state;determine an alignment error between an orientation of the master actuator with respect to the master reference frame and an orientation of the end effector with respect to the slave reference frame in response to the enablement signal transitioning from the inactive state to the active state; andmake successive adjustments to successively produced control signals while the alignment error satisfies a threshold and the enablement signal is in the active state, the successive adjustments causing a reduction of the alignment error, the processor configured to make the successive adjustments until an alignment criterion between the slave reference frame and the master reference frame is satisfied.

12. The apparatus of claim 11 wherein the processor is configured to determine the alignment error by being further configured to compute an offset angle between the master reference frame and the slave reference frame.

13. The apparatus of claim 12 wherein the processor is configured to compute the offset angle by being further configured to compute an offset angle that causes a z-axis of the slave reference frame and a z-axis of the master reference frame to point in the same direction while permitting a roll alignment error about the respective z-axes between the slave reference frame and the master reference frame.

14. The apparatus of claim 12 wherein the processor is configured to compute the offset angle by being further configured to generate a rotation matrix that carries the end effector orientation at the time of the enablement signal transition into the master actuator orientation at the time of the enablement signal transition and determine the offset angle associated with the rotation matrix.

15. The apparatus of claim 12 wherein the alignment criterion is satisfied in response to the offset angle between the slave reference frame and the master reference frame being less than a threshold value.

16. The apparatus of claim 11 wherein the processor is configured to make the successive adjustments by being further configured to, for each successive adjustment: determine a misalignment axis associated with the alignment error;determine an axis and an angle of rotation associated with the change in orientation of the master actuator;compute a component of rotation associated with the changes in orientation of the master actuator in the direction of the misalignment axis; andcompute a correction angle for the adjustment based on the angle of rotation of the master actuator and the computed component of rotation.

17. The apparatus of claim 11 wherein the processor is further configured to compute an angular speed of rotation associated with each change in orientation of the master actuator and, while the angular speed of rotation is does not satisfy a threshold speed associated with the reduction of the alignment error, discontinue making the successive adjustments to the successively produced control signals.

18. The apparatus of claim 17 wherein the processor is further configured to resume making the successive adjustment to the successively produced control signals in response to the determined angular speed of rotation satisfying the threshold speed.

19. The apparatus of claim 17 wherein the processor is configured to compute the angular speed of rotation by being configured to: generate a rotation matrix that carries a previous master actuator orientation into a current master actuator orientation;compute an angle of rotation associated with the rotation matrix; andcompute the angular speed of rotation based on the angle of rotation and a sampling rate for the master actuator.

20. The apparatus of claim 11 wherein the processor is configured to make the successive adjustments to the successively produced control signals by being further configured to successively adjust an orientation of the slave reference frame until the slave reference frame meets the alignment criterion.