U.S. patent application number 16/081773 was filed with the patent office on 2021-07-15 for inverse kinematic control systems for robotic surgical system.
The applicant listed for this patent is Covidien LP. Invention is credited to Jiqi Cheng.
Application Number | 20210212777 16/081773 |
Document ID | / |
Family ID | 1000005506493 |
Filed Date | 2021-07-15 |
United States Patent
Application |
20210212777 |
Kind Code |
A1 |
Cheng; Jiqi |
July 15, 2021 |
INVERSE KINEMATIC CONTROL SYSTEMS FOR ROBOTIC SURGICAL SYSTEM
Abstract
A method of using inverse kinematics to control a robotic system
includes receiving an input pose from a user interface to move an
arm of the robotic system, calculating a remote center of motion
for a desired pose from the input pose in a tool center-point
frame, checking when the desire pose needs correction, correcting
the desired pose of the arm, and moving the arm to the desired pose
in response to the input pose. The arm of the robotic system
including a tool having a jaw disposed at an end of the arm.
Checking when the desired pose needs correction includes verifying
that the remote center of motion is at or beyond a boundary
distance in the desired pose. Correcting the desired pose of the
arm occurs when the remote center of motion is within the boundary
distance.
Inventors: |
Cheng; Jiqi; (Cheshire,
CT) |
|
Applicant: |
Name |
City |
State |
Country |
Type |
Covidien LP |
Mansfield |
MA |
US |
|
|
Family ID: |
1000005506493 |
Appl. No.: |
16/081773 |
Filed: |
March 3, 2017 |
PCT Filed: |
March 3, 2017 |
PCT NO: |
PCT/US2017/020567 |
371 Date: |
August 31, 2018 |
Related U.S. Patent Documents
|
|
|
|
|
|
Application
Number |
Filing Date |
Patent Number |
|
|
62303437 |
Mar 4, 2016 |
|
|
|
Current U.S.
Class: |
1/1 |
Current CPC
Class: |
B25J 9/1689 20130101;
A61B 34/74 20160201; A61B 17/29 20130101; A61B 34/30 20160201; B25J
13/025 20130101; A61B 34/76 20160201 |
International
Class: |
A61B 34/30 20060101
A61B034/30; A61B 34/00 20060101 A61B034/00; A61B 17/29 20060101
A61B017/29; B25J 13/02 20060101 B25J013/02; B25J 9/16 20060101
B25J009/16 |
Claims
1. A method of controlling a robotic system, the method comprising:
receiving an input pose from a user interface to move an arm of the
robotic system, wherein a tool having a jaw is disposed at the end
of the arm, the jaw defining a jaw axis; calculating a remote
center of motion for a desired pose from the input pose in a tool
center-point frame; checking when the desired pose needs correction
by verifying the remote center of motion is at or beyond a boundary
distance in the desired pose; correcting the desired pose of the
arm when the remote center of motion is within the boundary
distance; and moving the arm to the desired pose in response to the
input pose.
2. The method according to claim 1, wherein during correcting the
desired pose of the arm, the jaw axis is held in position.
3. The method according to claim 1, wherein correcting the desired
pose of the arm includes moving the remote center of motion to the
boundary distance.
4. The method according to claim 1, wherein determining the
boundary distance as a function of a maximum joint angle of a pitch
joint between the tool and the distance between the origin of the
tool center-point and the pitch joint.
5. The method according to claim 4, wherein the maximum joint angle
of the pitch joint is 75.degree..
6. The method according to claim 4, wherein determining the
boundary distance includes taking the sum of the distance between
the origin of the tool center-point and the pitch joint and the
cosine of the maximum joint angle of the pitch joint.
7. The method according to claim 5, wherein the boundary distance
is taken along the jaw axis.
8. The method according to claim 1, further comprising providing
feedback when the remote center of motion approaches the boundary
distance.
9. The method according to claim 8, wherein providing feedback when
the remote center of motion approaches the boundary distance
includes increasing the feedback as the remote center of motion
approaches the boundary distance.
10. The method according to claim 9, wherein increasing the
feedback includes linearly increasing the feedback.
11. The method according to claim 9, wherein increasing the
feedback includes exponentially increasing the feedback.
12. The method according to claim 9, wherein increasing the
feedback includes linearly increasing the feedback as the remote
center of motion approaches the boundary distance and exponentially
increasing the feedback as the remote center of motion crosses the
boundary distance.
13. The method according to claim 1, further comprising determining
a check angle defined between the jaw axis and a vector between an
origin of the tool center-point frame and the remote center of
motion when the remote center of motion is within the boundary
distance.
14. The method according to claim 13, further comprising providing
feedback when the check angle is below a predefined extreme
angle.
15. A robotic surgical system comprising: a processing unit; a user
interface in communication with the processing unit and including
an input handle; and a robotic system in communication with the
processing unit and including an arm and a tool supported at an end
of the arm, the arm defining a remote center of motion and the tool
defining a tool center-point frame, the arm and the tool configured
to move to a desired pose in response to an input pose of the input
handle, wherein the processing unit is configured to verify the
remote center of motion being within the boundary distance in the
desired pose, and wherein the processing unit is configured to
correct the desired pose when the remote center of motion is within
the boundary distance.
16. The robotic surgical system according to claim 15, wherein the
processing unit is configured to verify that a check angle defined
between the jaw axis and a vector between an origin of the tool
center-point frame and the remote center of motion is below a
predefined extreme angle when the remote center of motion is within
the boundary distance in the desired pose.
17. The robotic surgical system according to claim 16, wherein the
user interface is configured to provide feedback to a clinician
when the check angle is below the predefined extreme angle.
18. The robotic surgical system according to claim 15, wherein the
input handle is configured to provide force feedback to a clinician
as the remote center of motion approaches the boundary distance in
the desired pose.
Description
BACKGROUND
[0001] Robotic surgical systems such as teleoperative systems are
used to perform minimally invasive surgical procedures that offer
many benefits over traditional open surgery techniques, including
less pain, shorter hospital stays, quicker return to normal
activities, minimal scarring, reduced recovery time, and less
injury to tissue.
[0002] Robotic surgical systems can have a number of robotic arms
that move attached instruments or tools, such as an image capturing
device, a stapler, an electrosurgical instrument, etc., in response
to movement of input devices by a surgeon viewing images captured
by the image capturing device of a surgical site. During a surgical
procedure, each of the tools is inserted through an opening, either
natural or an incision, into the patient and positioned to
manipulate tissue at a surgical site. The openings are placed about
the patient's body so that the surgical instruments may be used to
cooperatively perform the surgical procedure and the image
capturing device may view the surgical site.
[0003] During the surgical procedure, the tools are manipulated in
multiple degrees of freedom. When one or more of the degrees of
freedom align with one another a singularity is created. As the
tool approaches a singularity, the operation of the tool may become
unpredictable and the degrees of freedom of the tool may be
decreased. To prevent the tool from reaching a singularity, some
systems employ hard stops. Other systems allow the tool to reach a
singularity and use a comparison of a speed of an input controller
with a speed of movement of the tool to predict the movement of the
tool.
[0004] There is a continuing need to control a tool of a robotic
surgical system as the tool approaches and/or passes through a
singularity. Additionally or alternatively, there is a need to
control a tool of a robotic surgical system to avoid reaching a
singularity and to maintain each degree of freedom of the tool.
SUMMARY
[0005] This disclosure relates generally to correcting the pose of
an arm and a tool of a robotic system to avoid singularities
between joints and to maintain degrees of freedom of movement of
the arm and the tool. Specifically, when a remote center of motion
of the arm is within a boundary distance from an origin of a tool
center-point frame, the remote center of motion is moved to the
boundary distance while maintaining a position of a jaw axis of the
tool and rotating the remote center of motion according to the
rigid body kinematics. After the rotation, the tool center-point
frame is expressed in the rotated remote center of motion frame,
and treated as the corrected desired pose for inverse kinematics
calculation.
[0006] In an aspect of the present disclosure, a method of using
inverse kinematics to control a robotic system includes receiving
an input pose from a user interface to move an arm of the robotic
system, calculating a remote center of motion for a desired pose
from the input pose in a tool center-point frame, checking when the
desire pose needs correction, correcting the desired pose of the
arm, and moving the arm to the desired pose in response to the
input pose. The arm of the robotic system includes a tool having a
jaw disposed at an end of the arm. Checking when the desired pose
needs correction includes verifying that the remote center of
motion is at or beyond a boundary distance in the desired pose.
Correcting the desired pose of the arm occurs when the remote
center of motion is within the boundary distance.
[0007] In aspects, during correcting the desired pose of the arm,
the jaw axis is held in position. Correcting the desired pose of
the arm may include moving the remote center of motion to the
boundary distance.
[0008] In some aspects, determining the boundary distance occurs as
a function of a maximum joint angler of a pitch joint defined
between the tool and the distance between the origin of the tool
center-point and the pitch joint. The maximum joint angle of the
pitch joint may be about 75.degree.. Determining the boundary
distance may include taking the sum of the distance between the
origin of the tool center-point and the pitch joint and the cosine
of the maximum joint angle of the pitch point. The boundary
distance may be taken along the jaw axis.
[0009] In certain aspects, the method includes providing feedback
when the remote center of motion approaches the boundary distance
in the desired pose. Providing feedback when the remote center of
motion in the desired pose approaches the boundary distance may
include increasing the feedback as the remote center of motion
approaches the boundary distance. Increasing the feedback may
include linearly or exponentially increasing the feedback.
Alternatively, increasing the feedback may include linearly
increasing the feedback as the remote center of motion approaches
the boundary distance and exponentially increasing the feedback as
the remote center of motion crosses the boundary distance.
[0010] In particular aspects, the method includes determining a
check angle that is defined between the jaw axis and a vector
between an origin of the tool center-point frame and the remote
center of motion when the remote center of motion is within the
boundary distance. The method may include providing feedback when
the check angle is below a predefined extreme angle.
[0011] In another aspect of the present disclosure, a robotic
surgical system includes a processing unit, a user interface, and a
robotic system. The user interface is in communication with the
processing unit and includes an input handle. The robotic system is
in communication with the processing unit and including an arm and
a tool supported at an end of the arm. The arm defines a remote
center of motion and the tool defines a tool center-point frame.
The arm and the tool are configured to move to a desired pose in
response to an input pose of the input handle. The processing unit
is configured to verify when the remote center of motion is within
the boundary distance in the desired pose. The processing unit is
configured to correct the desired pose when the remote center of
motion is within the boundary distance.
[0012] In aspects, the processing unit is configured to verify that
a check angle defined between the jaw axis and a vector defined
between an origin of the tool center-point frame and the remote
center of motion is below a predefined extreme angle when the
remote center of motion is within the boundary distance in the
desired pose. The user interface may be configured to provide
feedback to a clinician when the check angle is below the
predefined extreme angle. Additionally or alternatively, the input
handle may be configured to provide force feedback to a clinician
when the remote center of motion approaches the boundary distance
in the desired pose.
[0013] Further, to the extent consistent, any of the aspects
described herein may be used in conjunction with any or all of the
other aspects described herein.
BRIEF DESCRIPTION OF THE DRAWINGS
[0014] Various aspects of the present disclosure are described
hereinbelow with reference to the drawings, which are incorporated
in and constitute a part of this specification, wherein:
[0015] FIG. 1 is a schematic illustration of a user interface and a
robotic system of a robotic surgical system in accordance with the
present disclosure;
[0016] FIG. 2 is a side view of an exemplary arm and tool of the
robotic system of FIG. 1;
[0017] FIG. 3 is a model of the arm and tool of FIG. 2 in a first
case;
[0018] FIG. 4 is a model of the arm and tool of FIG. 2 in a second
case;
[0019] FIG. 5 is a model of the arm and tool of FIG. 2 in a third
case;
[0020] FIG. 6 is the model of FIG. 3 illustrating a calculation of
a joint angle .theta..sub.6;
[0021] FIG. 7 is the model of FIG. 6 illustrating a calculation of
a joint angle .theta..sub.5;
[0022] FIG. 8 is the model of FIG. 7 illustrating a calculation of
joint angles .theta..sub.1 and .theta..sub.2;
[0023] FIG. 9 is the model of FIG. 8 illustrating a calculation of
a joint angle .theta..sub.3;
[0024] FIG. 10 is a plan view of the {circumflex over (x)}.sub.6,
y.sub.6 plane of the model of FIG. 3;
[0025] FIG. 11 is a model illustrating determination of
u.sub.0;
[0026] FIG. 12 is the plan view of FIG. 10 including a boundary
line B;
[0027] FIG. 13 is a model illustrating a projection method of
calculating a rotation of a trocar point O to a rotated trocar
point M;
[0028] FIG. 14 is a calculation of a rotation angle .PHI. from the
trocar point O to the rotated trocar point M of FIG. 13;
[0029] FIG. 15 is a calculation of an orientation matrix for the
rotation of the Base Frame {circumflex over (x)}.sub.0, y.sub.0,
{circumflex over (z)}.sub.0;
[0030] FIG. 16 is a model illustrating detection of an extreme
condition;
[0031] FIG. 17 is a model of a long jawed instrument with the arm
of the robotic system of FIG. 1 in accordance with the present
disclosure;
[0032] FIG. 18 is a calculation of a rotation angle .PHI. in a
Frame 6 of the trocar point O to the rotated trocar point M of FIG.
17;
[0033] FIG. 19 is a model illustrating determination of u.sub.0 for
the long jawed instrument of FIG. 17;
[0034] FIG. 20 is a projection of an End Effector Frame of FIG. 17
to the rotated trocar point M
[0035] FIG. 21 is a calculation of an orientation matrix for the
rotation of the Frame 6;
[0036] and
[0037] FIG. 22 is a model illustrating detection of an extreme
condition for the long jawed instrument of FIG. 17.
DETAILED DESCRIPTION
[0038] Embodiments of the present disclosure are now described in
detail with reference to the drawings in which like reference
numerals designate identical or corresponding elements in each of
the several views. As used herein, the term "clinician" refers to a
doctor, a nurse, or any other care provider and may include support
personnel. Throughout this description, the term "proximal" refers
to the portion of the device or component thereof that is closest
to the clinician and the term "distal" refers to the portion of the
device or component thereof that is farthest from the
clinician.
[0039] This disclosure relates generally to correcting the pose of
an arm and a tool of a robotic system to avoid singularities
between joints and to maintain degrees of freedom of movement of
the arm and the tool. Specifically, when a remote center of motion
of the arm is within a boundary distance from an origin of a tool
center-point frame, the remote center of motion is moved to the
boundary distance while maintaining a position of a jaw axis of the
tool and rotating the remote center of motion according to the
rigid body kinematics. After the rotation, the tool center-point
frame is expressed in the rotated remote center of motion frame,
and treated as the corrected desired pose for inverse kinematics
calculation.
[0040] Referring to FIG. 1, a robotic surgical system 1 in
accordance with the present disclosure is shown generally as a
robotic system 10, a processing unit 30, and a user interface 40.
The robotic system 10 generally includes linkages or arms 12 and a
robot base 18. The arms 12 moveably support an end effector or tool
20 which is configured to act on tissue. The arms 12 each have an
end 14 that supports an end effector or tool 20 which is configured
to act on tissue. In addition, the ends 14 of the arms 12 may
include an imaging device 16 for imaging a surgical site. The user
interface 40 is in communication with robot base 18 through the
processing unit 30.
[0041] The user interface 40 includes a display device 44 which is
configured to display three-dimensional images. The display device
44 displays three-dimensional images of the surgical site which may
include data captured by imaging devices 16 positioned on the ends
14 of the arms 12 and/or include data captured by imaging devices
that are positioned about the surgical theater (e.g., an imaging
device positioned within the surgical site, an imaging device
positioned adjacent the patient, imaging device 56 positioned at a
distal end of an imaging linkage or arm 52). The imaging devices
(e.g., imaging devices 16, 56) may capture visual images, infra-red
images, ultrasound images, X-ray images, thermal images, and/or any
other known real-time images of the surgical site. The imaging
devices transmit captured imaging data to the processing unit 30
which creates three-dimensional images of the surgical site in
real-time from the imaging data and transmits the three-dimensional
images to the display device 44 for display.
[0042] The user interface 40 also includes input handles 42 which
are supported on control arms 43 which allow a clinician to
manipulate the robotic system 10 (e.g., move the arms 12, the ends
14 of the arms 12, and/or the tools 20). Each of the input handles
42 is in communication with the processing unit 30 to transmit
control signals thereto and to receive feedback signals therefrom.
Additionally or alternatively, each of the input handles 42 may
include input devices (not shown) which allow the surgeon to
manipulate (e.g., clamp, grasp, fire, open, close, rotate, thrust,
slice, etc.) the tools 20 supported at the ends 14 of the arms
12.
[0043] Each of the input handles 42 is moveable through a
predefined workspace to move the ends 14 of the arms 12 within a
surgical site. The three-dimensional images on the display device
44 are orientated such that movement of the input handle 42 moves
the ends 14 of the arms 12 as viewed on the display device 44. It
will be appreciated that the orientation of the three-dimensional
images on the display device may be mirrored or rotated relative to
view from above the patient. In addition, it will be appreciated
that the size of the three-dimensional images on the display device
44 may be scaled to be larger or smaller than the actual structures
of the surgical site permitting a clinician to have a better view
of structures within the surgical site. As the input handles 42 are
moved, the tools 20 are moved within the surgical site as detailed
below. As detailed herein, movement of the tools 20 may also
include movement of the ends 14 of the arms 12 which support the
tools 20.
[0044] For a detailed discussion of the construction and operation
of a robotic surgical system 1, reference may be made to U.S. Pat.
No. 8,828,023, the entire contents of which are incorporated herein
by reference.
[0045] With reference to FIG. 2, the end 14 of an arm 12 of the
robotic system 10 is moveable about a Remote Center of Motion (RCM)
22 in four Degrees of Freedom (DOF) or joints. In addition, the
tool 20 is pivotal in two DOF about a first tool joint 24 and a
second tool joint 26, respectively. The arm 12 of the robotic
system 10 defines a frame x.sub.0, y.sub.0, z.sub.0 (the Base
Frame) positioned at the RCM. The first DOF or yaw joint of the arm
12 is represented by the frame x.sub.1, y.sub.1, z.sub.1 (Frame 1)
with the position of the arm 12 about the yaw joint represented as
joint angle .theta..sub.1. The second DOF or pitch joint of the arm
12 is represented by the frame x.sub.2, y.sub.2, z.sub.2 (Frame 2)
with the position of the arm 12 about the pitch joint represented
as joint angle .theta..sub.2. The third DOF or roll joint of the
arm 12 is represented by the frame x.sub.3, y.sub.3, z.sub.3 (Frame
3) with the position of the arm 12 about the roll joint represented
as joint angle .theta..sub.3. The fourth DOF or linear joint is
represented by the frame x.sub.4, y.sub.4, z.sub.4 (Frame 4) and
with the position of the first tool joint 24 from the RCM 22
represented as distance d.sub.4. Movement of the tool 20 about the
first tool joint 24 is defined in the fifth DOF or tool pitch joint
represented in the frame x.sub.5, y.sub.5, z.sub.5 (Frame 5) with
the position of the tool pitch joint represented as joint angle
.theta.5. Movement of the tool 20 about the second tool joint 26 is
defined in the sixth DOF or tool yaw joint in the frame x.sub.6,
y.sub.6, z.sub.6 (Frame 6) with the position in the tool yaw joint
represented as joint angle .theta..sub.6. The orientation of the
end effector 29 is represented by the {circumflex over (x)}.sub.6
axis.
[0046] Using a typical forward kinematics model based on the frame
attachments detailed above, the Denavit-Hartenberg (DH) parameters
of the intracorporeal chain is shown in Table 1 below. As used
herein "i" is the frame number, "a" is the length from the common
normal, "a" is the angle about the common normal, "D" is the offset
along previous z axis to the common normal, "0" is the angle about
previous z axis, and the joint type is the type of movement about
the joint. It will be appreciated that the common normal is along
the arm 12.
TABLE-US-00001 TABLE 1 i a.sub.i-1 .alpha..sub.i-1 D.sub.i
.theta..sub.i Joint Type 1 0 0 0 .theta..sub.1 Rotational 2 0
-.pi./2 0 .sup. .theta..sub.2-.pi./2 Rotational 3 0 .pi./2 0
.theta..sub.3 Rotational 4 0 0 d.sub.4 0 Linear 5 0 -.pi./2 0 .sup.
.theta..sub.5-.pi./2 Rotational 6 a.sub.6 -.pi./2 0 .theta..sub.6
Rotational
[0047] Further, the Homogenous transform for movement in the six
DOF is as follows:
i i - 1 .times. T = [ c .times. .times. .theta. i - s .times.
.times. .theta. i 0 a i - 1 s .times. .times. .theta. i .times. c
.times. .times. .alpha. i - 1 c .times. .times. .theta. i .times. c
.times. .times. .alpha. i - 1 - s .times. .times. .alpha. i - 1 - s
.times. .times. .alpha. i - 1 .times. d i s .times. .times. .theta.
i .times. s .times. .times. .alpha. i - 1 c .times. .times. .theta.
i .times. s .times. .times. .alpha. i - 1 c .times. .times. .alpha.
i - 1 c .times. .times. .alpha. i - 1 .times. d i 0 0 0 1 ] ( 1 )
##EQU00001##
where c.theta.cos .theta. and s.theta.sin .theta., this convention
will be used throughout this disclosure. Thus, the final pose of
frame 6 in the base frame 0 is defined as:
.sub.6.sup.0T=.sub.1.sup.0T.sub.2.sup.1T.sub.3.sup.2T.sub.3.sup.4T.sub.5-
.sup.4T.sub.6.sup.5T (2)
[0048] With reference to FIG. 3, an inverse kinematic model for the
arm 12 and the tool 20 in accordance with the present disclosure is
disclosed. With reference to FIG. 3 and used in the following
equations, the RCM 22 is represented by trocar point "O" and is the
point at which the end 14 of the arm 12 or the tool 20 passes
through a trocar (not shown) in to the body cavity of the patient.
The tool pitch joint 24 is represented by point "P" and the tool
yaw joint 26 is represented by yaw point "T". A yoke 25 (FIG. 2) of
the tool 20 may be represented by the line "PT". In this model a
frame of a jaw 29 of the tool 20 is represented by the Frame 6 and
may be referred to as the tool-center-point frame (TCP frame) with
the jaw direction positioned along the {circumflex over (x)}.sub.6
axis. In addition, the yaw point "T" may be referred to as the
"TCP" point herein.
[0049] As shown in FIG. 3, the jaw direction {right arrow over
(T)}x.sub.6 is known as the direction of the {circumflex over
(x)}.sub.6 axis. With the jaw direction {right arrow over
(T)}x.sub.6 fixed, the joint angle .theta..sub.6 can be determined
by placing the following two constraints on the pitch point "P".
First, the pitch point "P" is in a yaw plane "YP" defined by
rotation of the yoke "PT" around the {circumflex over (z)}.sub.6
axis, shown as the dashed circle "YP" in FIG. 3, with both the yoke
PT and the jaw direction {right arrow over (T)}x.sub.6 lying in the
yaw plane "YP". As shown, when joint angle .theta..sub.6 changes,
the pitch point "P" will be in the yaw plane "YP" a distance
a.sub.6 from the yaw point T. Second, the pitch point "P" is also
in a pitch plane "PP" which is perpendicular to the yaw plane "YP"
defined by the trocar point "O" and the {circumflex over (z)}.sub.6
axis.
[0050] With these two constraints placed on the pitch point "P",
three cases can be defined to determine the location of the pitch
point "P". In the first case modeled in FIG. 3, the pitch plane
"PP" is uniquely defined by the trocar point "O" and the 26 axis
and intersects the yaw plane "YP" at the pitch point "P" on a
proximal side (i.e., closer to the trocar point "O") of the yaw
point "T". Thus, in the first case, the joint angle .theta..sub.6
and the joint angle .theta..sub.5 are in a range of about -.pi./2
to about .pi./2. As used herein, angles are expressed in radians
where it is equal to 180.degree..
[0051] In the second case modeled in FIG. 4, the pitch plane "PP"
is uniquely defined by the trocar point "O" and the {circumflex
over (z)}.sub.6 axis and intersects the yaw plane "YP" at the pitch
point "P" on a distal side (i.e., away to the trocar point "O") of
the yaw point "T". Thus, in the second case, the joint angle
.theta..sub.6 and the joint angle .theta..sub.5 are outside of a
range of about -.pi./2 to about .pi./2.
[0052] In the third case modeled in FIG. 5, the 26 axis is directed
towards the trocar point "O" such that the trocar point "O" and the
{circumflex over (z)}.sub.6 axis fail to uniquely define the pitch
plane "PP". In the third case a singularity exists which makes it
difficult to determine a range of the joint angle .theta..sub.6 and
the joint angle .theta..sub.5. The third case can be modeled as
described in greater detail below.
[0053] Referring back to the first case shown in FIG. 3, the
determination of the joint angle .theta..sub.6, the joint angle
.theta..sub.5, and the distance d.sub.4 is detailed in accordance
with the present disclosure. First, assuming that the desired pose
of the "TCP" Frame is defined by the position
[ u v w ] ##EQU00002##
and the rotation .sub.6.sup.0R which are both defined in the Base
Frame, the homogenous transform of the "TCP" Frame in the Base
Frame can be expressed as:
6 0 .times. T = [ 6 0 .times. R [ u v w ] 0 1 ] ( 3 )
##EQU00003##
[0054] To determine the joint angles, the Base Frame is expressed
in the TCP Frame as:
6 0 .times. T = [ 6 0 .times. R [ u ' v ' w ' ] 0 1 ] ( 4 )
##EQU00004##
Based on the property that .sub.0.sup.6T=[.sub.6.sup.0T].sup.-1,
.sub.0.sup.6R=[.sub.6.sup.0R].sup.-1=.sub.6.sup.0R.sup.T it can be
determined that:
[ u ' v ' w ' ] = - R T 6 0 .function. [ u v w ] ( 5 )
##EQU00005##
[0055] Referring now to FIG. 6, the coordinate of the trocar point
"O" in the "TCP" Frame is
[ u ' v ' w ' ] ##EQU00006##
The projection of the trocar point "O" in the {circumflex over
(x)}.sub.6, y.sub.6 plane is point "O'" and the projection of point
"O'" on the y.sub.6 axis is "O''".
[0056] Applying the right hand rule, the relative sign and quarter
locations are as follows:
{ / 2 < .theta. 6 < , .times. when .times. .times. u ' > 0
, v ' > 0 ; 0 < .theta. 6 < / 2 , .times. when .times.
.times. u ' < 0 , v ' > 0 ; - / 2 < .theta. 6 < 0 ,
.times. when .times. .times. u ' < 0 , v ' < 0 ; - <
.theta. 6 < - / 2 , .times. when .times. .times. u ' > 0 , v
' < 0. ( 6 ) ##EQU00007##
[0057] Taking into account the relative sign and quarter location,
as detailed above, in Equation 6:
.theta..sub.6=a tan 2(v',-u') (7)
[0058] With reference to FIG. 7, the distance between the yaw point
"T" and point "O'" is given by TO'=-u' cos .theta..sub.6+v' sin
.theta..sub.6 and the distance between the pitch point "P" and
point "O'" is given by PO'=-u' cos .theta..sub.6+v' sin
.theta..sub.6-a.sub.6. Thus, the joint angle .theta..sub.5 is given
by:
.theta..sub.5=a tan 2(w',-u' cos .theta..sub.6+v' sin
.theta..sub.6-a.sub.6) (8)
[0059] In addition, the distance d.sub.4 between the trocar point
"O" and the pitch point "P" is:
d.sub.4=w' sin .theta..sub.5+(-u' cos .theta..sub.6+v' sin
.theta..sub.6-a.sub.6)cos .theta..sub.5 (9)
[0060] Now that the joint angle .theta..sub.6, the joint angle
.theta..sub.5, and the distance d.sub.4 have been determined, the
joint angle .theta..sub.6, the joint angle .theta..sub.5, and the
distance d.sub.4 can be used to determine the joint angle
.theta..sub.1, the joint angle .theta..sub.2, and the joint angle
.theta..sub.3. In light of Equation 2 above, a rotation matrix
chain can be expressed as:
.sub.6.sup.0R=.sub.1.sup.0R.sub.2.sup.1R.sub.3.sup.2R.sub.4.sup.3R.sub.5-
.sup.4R.sub.6.sup.5R (10)
[0061] Since joint angle .theta..sub.5 and joint angle
.theta..sub.6 are now known, in view of Equation 1, .sub.6.sup.5R
can be expressed as:
6 5 .times. R = [ c .times. .theta. 6 - s .times. .theta. 6 0 0 0 1
- s .times. .theta. 6 - c .times. .theta. 6 0 ] ( 11 )
##EQU00008##
In addition, .sub.5.sup.4R can be expressed as:
5 4 .times. R = [ s .times. .theta. 5 c .times. .theta. 5 0 0 0 1 c
.times. .theta. 5 - s .times. .theta. 5 0 ] ( 12 ) ##EQU00009##
The transpose of the product of the above two equations is
expressible as:
( 5 4 .times. R 6 5 .times. R ) T = [ s .times. .theta. 5 .times. c
.times. .theta. 6 - s .times. .theta. 6 c .times. .theta. 5 .times.
c .times. .theta. 6 - s .times. .theta. 5 .times. s .times. .theta.
6 - c .times. .theta. 6 - c .times. .theta. 5 .times. s .times.
.theta. 6 c .times. .theta. 5 0 - s .times. .theta. 5 ] ( 13 )
##EQU00010##
Then multiplying Equation 13 from the right side to Equation
10:
.sub.6.sup.0R(.sub.5.sup.4R.sub.6.sup.5R).sub.T=.sub.6.sup.0R(.sub.5.sup-
.4R.sub.6.sup.5R).sup.-1=.sub.1.sup.0R.sub.2.sup.1R.sub.3.sup.2R.sub.4.sup-
.3R.sub.5.sup.4R.sub.6.sup.5R(.sub.5.sup.4R.sub.6.sup.5R).sup.-1=.sub.4.su-
p.0R (14)
Further, by defining
4 0 .times. R .times. = .DELTA. .times. R _ = [ R _ 11 R _ 12 R _
13 R _ 21 R _ 22 R _ 23 R _ 31 R _ 32 R _ 33 ] ##EQU00011##
provides that:
[ R _ 11 R _ 12 R _ 13 R _ 21 R _ 22 R _ 23 R _ 31 R _ 32 R _ 33 ]
.times. = 6 0 .times. R .function. [ s .times. .theta. 5 .times. c
.times. .theta. 6 - s .times. .theta. 6 c .times. .theta. 5 .times.
c .times. .theta. 6 - s .times. .theta. 5 .times. s .times. .theta.
6 - c .times. .theta. 6 - c .times. .theta. 5 .times. s .times.
.theta. 6 c .times. .theta. 5 0 - s .times. .theta. 5 ] ( 15 )
##EQU00012##
[0062] Referring to FIG. 8, the projection of the pitch point "P"
in the {circumflex over (x)}.sub.0, y.sub.0 plane is point "P'" and
the projection of the point "P'" on the {circumflex over (x)}.sub.0
axis is "P''". Thus, P'P''=-R.sub.23OP and OP''=-R.sub.13OP.
Accordingly, the joint angle .theta..sub.1 can be expressed as:
.theta..sub.1=a tan 2(-R.sub.23,-R.sub.13) (16)
In addition, joint angle .theta..sub.2 can be expressed as:
.theta..sub.2=a tan 2(R.sub.33, {square root over
(R.sub.13.sup.2+R.sub.23.sup.2)}) (17)
[0063] It can be observed that when .theta..sub.2=.+-..pi./2, shaft
OP of the arm 12 or tool 20 (FIG. 2) is aligned with the rotation
axis of joint 1. This is a singularity where joint angle
.theta..sub.1 and joint angle .theta..sub.3 are not uniquely
defined. A robot can be designed in such a way that the joint angle
.theta..sub.2 is mechanically constrained to or in a range (e.g.,
about -70.degree. to about) 70.degree.. In such a range,
.theta..sub.2 will not reach the singularity. In normal operation,
this singularity is checked and rejected by the processing unit 30
(FIG. 1).
[0064] With reference to FIG. 9, a {circumflex over (z)}.sub.0'
axis is created at the pitch point "P" that is parallel to the
{circumflex over (z)}.sub.0 axis, the projection point z.sub.0' in
the plane {circumflex over (x)}.sub.4, y.sub.4 is point z.sub.0''.
Thus, the joint angle .theta..sub.3 can be expressed as:
.theta..sub.3=a tan 2(-R.sub.32,R.sub.31) (18)
[0065] Using the previous derivation of the rotation matrix above,
if the unit vector of the frame is defined as {circumflex over
(x)}, y, {circumflex over (z)}, the equation 15 can be expressed
as:
[ R _ 11 R _ 12 R _ 13 R _ 21 R _ 22 R _ 23 R _ 31 R _ 32 R _ 33 ]
= [ x ^ 4 x ^ 0 y ^ 4 x ^ 0 z ^ 4 x ^ 0 x ^ 4 y ^ 0 y ^ 4 y ^ 0 z ^
4 y ^ 0 x ^ 4 z ^ 0 y ^ 4 z ^ 0 z ^ 4 z ^ 0 ] ( 19 )
##EQU00013##
[0066] With respect to the second case, shown in FIG. 4, the
solution for the joint angle .theta..sub.6 is labeled
.theta..sub.6'. It will be appreciated that as shown in FIG. 4:
.theta..sub.6-.theta..sub.6'=.pi. (20)
[0067] Thus, to determine the joint angles in the second case, the
joint angle .theta..sub.6 is calculated in a manner similar to the
first case detailed above and then Equation 20 is used to determine
the joint angle .theta..sub.6' for the second case. Once the joint
angle .theta..sub.6' for the second case is determined, the rest of
the joint angles .theta..sub.1, .theta..sub.2, .theta..sub.3, and
.theta..sub.5 as well as the distance d.sub.4 can be determined
using the above method.
[0068] Now turning to the third case, as shown in FIG. 5, the third
case can be detected when
{ u ' .apprxeq. 0 ; v ' .apprxeq. 0 ( 21 ) ##EQU00014##
If the singularity of the third case is detected, the previous
joint angle .theta..sub.6 can be used. Thus, with
.theta..sub.6=.theta..sub.6,previous, the rest of the joint angles
.theta..sub.1, .theta..sub.2, .theta..sub.3, and .theta..sub.5 as
well as the distance d.sub.4 can be determined using the above
method.
[0069] As detailed above, the inverse kinematics solution provides
mathematically precise mapping between an input pose and joint
angles of the surgical robot 10. In the inverse kinematics
solution, other considerations such as joint limits and joint speed
limits are not taken into account. For example, due to mechanical
constraints of the design of the surgical robot 10, the joint
limits of joint angles .theta..sub.5 and .theta..sub.6 are usually
in the range of about -.pi./2 to about .pi./2. The mechanical
constraints of the surgical robot 10 prevent cases 2 and 3,
detailed above, from being physically realizable since one of the
joint angles will be greater than .pi./2. However, the poses
presented in cases 2 and 3 may be commanded by the input device 42
because the input device 42 and the surgical robot 10 have
different kinematic constructions and working spaces. For example,
case 3 is a singularity where the inverse kinematic solution is not
unique. In addition, when the joint angle .theta..sub.5 approaches
.+-..pi./2, a small rotation of the input pose along the axis
x.sub.6 would cause a dramatic change in joint angle .theta..sub.3.
Such a dramatic change may violate a joint speed limit and an
acceleration limit of the roll joint of the arm 12 represented by
joint angle .theta..sub.3. It will be appreciated that such sudden
motions are undesirable in clinical settings.
[0070] To avoid an unrealizable configuration or a singularity
condition, the joint angle .theta..sub.5 is restricted in a range
of about -5.pi./12 to about 5.pi./12 and the joint angle
.theta..sub.6 is restricted in a range of about -.pi./2 to about
.pi./2. One method of achieving this is in a brutal-force method by
clamping the solution provided by the inverse kinematics solution.
However, the brutal-force method may cause discontinuity in the
inverse kinematics solution which is not desirable in a clinical
setting.
[0071] Alternatively, as detailed below, a method may be used to
correct the input pose after it crosses from a desirable solution
space to an undesirable solution space. In the desirable solution
space, the joint limits and the joint speed limits are acceptable
and in the undesirable solution space at least one of the joint
limits or the joint speed limits are exceeded or undesirable. Once
the input pose is corrected, the corrected input pose can be solved
by using the inverse kinematics solution detailed above having
deniable joint angles. Such a method may avoid discontinuities in
the joint angles when the boundary between the desirable and
undesirable solution spaces is crossed. In addition, such a method
can realize the position component of the input pose by only
correcting an orientation component of the TCP frame to avoid
unintended motion of the TCP frame from being introduced into the
inverse kinematic solution.
[0072] As described below, a projection method that corrects an
orientation component of the TCP frame of a desired input pose
while maintaining a position component of the desired input pose is
disclosed in accordance with the present disclosure. In the
projection method, a boundary plane that separates a desirable and
undesirable solution space may be defined by locating the trocar
point on a spherical surface. A boundary plane then divides the
spherical surface into two sections. If the trocar point is located
on one side of the boundary (e.g., the right side), the input pose
does not require correction. If no correction is required, the
inverse kinematics solutions are desirable and within the joint
limits and the joint speed limits. When the trocar point is located
on the other side of the boundary (e.g., the left side), the input
pose is projected to the boundary plane. If correction is required,
the input pose is corrected by rotating the TCP frame to a
projection point located on the boundary plane according to rigid
body kinematics and expressing the corrected TCP frame in a rotated
base frame for the inverse kinematics solution. After the
correction of the TCP frame, the inverse kinematics solution for
the corrected TCP frame will be desirable and within the joint
limits and the joint speed limits.
[0073] With reference to FIGS. 10-15, a method of correcting an
input pose for the arm 12 and the tool 20 (FIG. 2) in the event of
the desired input pose being in an undesirable solution space
(i.e., case 2, case 3, or a singularity) will be described in
detail. Knowing that the joint angle .theta..sub.6 is physically
limited to about -.pi./2 to about .pi./2 and that the joint angle
.theta..sub.5 is physically limited to about -.pi./2 to about
.pi./2, the projection of the trocar point "O" in the plane
{circumflex over (x)}.sub.6, y.sub.6 lies in region "S" of FIG.
10.
[0074] When the projection point "O'" of the trocar point "O" is
close to a circle about the yaw point "T" with a radius equal to
the length a.sub.6 of the yoke "PT", the joint angle .theta..sub.5
will approach .+-..pi./2. Thus, any small orientation change of the
26 axis will cause a large change in the joint angle .theta..sub.6
and the joint angle .theta..sub.2.
[0075] Continuing to refer to FIG. 10, it will be appreciated that
the projection point "O'" cannot lie outside of the region "S" due
to the physical limits of joint angle .theta..sub.6 and/or joint
angle .theta..sub.5.
[0076] Referring to FIG. 12, during operation of the arm 12 and the
tool 20, it is desirable that the projection point "O'" is
positioned beyond, to the right as shown, of a dotted boundary line
"B" with a dimension u.sub.o from the y.sub.6 axis which is greater
than the length a.sub.6 of the yoke "PT" which requires that:
u'<u.sub.0 (22)
[0077] The coordinate u.sub.0 is chosen to meet the physical limits
of joint angle .theta..sub.5 and joint angle .theta..sub.6 to avoid
a singular condition. As described in detail below, the projection
point "O'" may be positioned on either side or on the boundary line
"B".
[0078] With particular reference to FIG. 11, a distance r'' is
defined between the projection point "O" and pitch point "P", such
that
(r'' cos .theta..sub.5,max+a.sub.6).sup.2+(r'' sin
.theta..sub.5,max).sup.2=r.sub.0.sup.2 (23)
From Equation 23, r'' can be calculated, and coordinate u.sub.0 can
be expressed as
u.sub.0=-(a.sub.6+r'' cos .theta..sub.5,max) (24)
where .theta..sub.5, max is a desirable limit (i.e., mechanical
constraint) on joint angle .theta..sub.5 (e.g., 75.degree. or
5.pi./12).
[0079] If the projection point "O'" lies on the left side of the
boundary line "B", the projection point "O'" is systematically
projected to the boundary line "B". In this manner, the yaw point
"T" is rotated according to the projection rule, as detailed below,
such that after the rotation of the yaw point "T", a new pose of
the "TCP" Frame is realized.
[0080] Referring to FIG. 13, the trocar point "O" and the yaw point
"T" are connected such that a distance between the yaw point "T"
and the trocar point "O" is distance r.sub.0. The distance r.sub.0
is equal to {square root over (u'.sup.2+v'.sup.2+w'.sup.2)}. With
the distance r.sub.0 held constant, the vector {right arrow over
(T)}O is rotated in the "TCP" Frame around the yaw point "T" such
that the trocar point "O" will travel on a sphere centered at the
yaw point "T". When the projection point "O'" lies on the left side
of the boundary line "B" as illustrated in FIG. 11, the yaw point
"T" will rotate until the trocar point "O" is rotated to the point
"M" such that the projection point "O'" lies on the boundary line
"B".
[0081] During this rotation, the orientation of the vector {right
arrow over (T)}O in the {circumflex over (x)}.sub.6, {circumflex
over (z)}.sub.6 plane remains constant. In accordance with the
projection rule
{ u '' = u 0 v '' = kv ' w '' = kw ' ( 25 ) ##EQU00015##
Where the linear constant k is given by:
k = r 0 2 - u 0 2 v ' .times. 2 + w ' .times. 2 ( 26 )
##EQU00016##
[0082] Referring to FIG. 13, the rotation angle ".PHI." of the yaw
point "T" can be determined as:
.PHI.=2a sin(l/2r.sub.0) (27)
where l= {square root over ((u'-u.sub.0).sup.2+( {square root over
(r.sub.0.sup.2-u'.sup.2)}- {square root over
(r.sub.0.sup.2-u.sub.0.sup.2)}).sup.2)}.
[0083] Referring now to FIG. 15, new frames x.sub.7, y.sub.7,
z.sub.7 (Frame 7) and x.sub.7', y.sub.7', and z.sup.7' (Frame 7')
are defined. Both Frames 7 and 7' originate at the yaw point "T".
For Frame 7, the {circumflex over (x)}.sub.7 axis is aligned with
the vector {right arrow over (T)}O and the {circumflex over
(z)}.sub.7 axis is along the direction of {right arrow over
(T)}M.times.{right arrow over (T)}O. For Frame 7', the {circumflex
over (x)}.sub.7' axis is aligned with the vector {right arrow over
(T)}.sub.M and the {circumflex over (z)}.sub.7' axis is along the
direction of {right arrow over (T)}M.times.{right arrow over (T)}O.
After the rotation, the Base Frame becomes frame x.sub.0',
y.sub.0', and z.sub.0' (Rotated Base Frame). Thus, when
.PHI..ltoreq..pi./2, {right arrow over (T)}z.sub.7'={right arrow
over (T)}M.times.{right arrow over (T)}O/|{right arrow over
(T)}M.times.{right arrow over (T)}O|, and when .PHI.>.pi./2,
{right arrow over (T)}z.sub.7'=-{right arrow over
(T)}M.times.{right arrow over (T)}O/|{right arrow over
(T)}M.times.{right arrow over (T)}O|, the Base Frame expressed from
Frame 6 through Frame 7 is:
.sub.0.sup.6T=.sub.7.sup.6T.sub.0.sup.7T (28)
and the Rotated Base Frame expressed from Frame 6 through Frame 7'
is:
.sub.0.sup.6T=.sub.7'.sup.6T.sub.0.sup.7'T (29)
During rotation, the Base Frame in Frame 7 is equal to the Rotated
Base Frame in Frame 7' such that:
.sub.0.sup.7T=.sub.0.sup.7'T (30)
Thus, the Rotated Base Frame in Frame 6 can be calculated as:
.sub.0.sup.6T=.sub.7.sup.6T[.sub.7.sup.6T].sup.-1.sub.0.sup.6T
(31)
From Equation 31 we have:
7 6 .times. T = [ x ^ 7 ' y ^ 7 ' z ^ 7 ' [ u 0 v '' w '' ] 0 1 ]
.times. .times. and ( 32 ) 7 6 .times. T = [ x ^ 7 y ^ 7 z ^ 7 [ u
' v ' w ' ] 0 1 ] ( 33 ) ##EQU00017##
[0084] During the above posing, it is assumed, for simplicity, that
the trocar point "O" and the Base Frame are rotated and that the
yaw point "T" and the "TCP" Frame are kept constant. In fact,
during the above movement, the trocar point "O", the yaw point "T",
and the Base Frame are kept constant and only the "TCP" Frame is
rotated. Due to the fact that the rotation is relative, if the
"TCP" Frame is rotated instead of the Base Frame, then the rotated
"TCP" Frame can be expressed in the Base Frame as:
6 0 .times. T = [ R T 0 ' 6 [ u v w ] 0 1 ] ( 34 ) ##EQU00018##
where .sub.0'.sup.6R is from the homogenous transform
.sub.0'.sup.6T in Equation 31.
[0085] Referring to FIG. 16, in some extreme conditions, when
u'>u.sub.extreme, another singularity condition is approached.
This singularity condition is unlikely due to the range of motion
of anatomical limits of the hands of a clinician that are engaged
with the input device 43 (FIG. 1). However, when
.theta..sub.5>.theta..sub.5,extreme (e.g., 150.degree.),
{circumflex over (x)}.sub.6 is pointing back to the trocar point
"O" and the motion of {circumflex over (x)}.sub.6 will be
significantly amplified. The motion from the inverse kinematic
solution may become less stable. Accordingly, the algorithm may
provide feedback to a clinician informing or warning the clinician
that the singularity condition is being approached. In addition, if
necessary, the robotic system 10 may decouple or stop the mapping
between the input device 43 and the arm 12 of the surgical robot
10. Similar to the determination of u.sub.0 in Equations 23 and 24,
the distance between trocar point "O" and pitch point "P" can be
expressed as:
(r'' cos(.pi.-.theta..sub.5,extreme)-a.sub.6).sup.2+(r''
sin(.pi.-.theta..sub.5,extreme)).sup.2=r.sub.0.sup.2 (35)
[0086] Solving for r'' from the above equation 35 provides:
u.sub.extreme=r'' cos(.pi.-.theta..sub.5,extreme)-a.sub.6 (36)
[0087] In light of the above, the steps of the complete inverse
kinematics algorithm for the arm 12 and the tool 20 (FIG. 2) are:
[0088] 1) For each given input pose .sub.6.sup.0T, calculate the
trocar point "O" position in the TCP frame using Equation 5; [0089]
2) Use Equation 22 to check if the input pose needs correction, if
correction is needed got to step 3, otherwise, skip to step 5;
[0090] 3) Check if u'>u.sub.extreme with Equation 35, if true
provide feedback, else got to Step 4; [0091] 4) Use Equations 23 to
31 to calculate a corrected pose .sub.6'.sup.0T, and replace the
original pose with the corrected pose; [0092] 5) Use Equations 6 to
18 to calculate the joint angles .theta..sub.1, .theta..sub.2,
.theta..sub.3, .theta..sub.5, and .theta..sub.6 as well as the
distance d.sub.4.
[0093] In the inverse kinematics algorithm above, checking if the
input pose needs correction is accomplished by choosing u.sub.0 in
Equation 22 as a function of the yaw point "T" and a maximum joint
angle .theta..sub.5, max. For example, the maximum joint angle
.theta..sub.5, max may be selected as 5.pi./12 (i.e.,
75.degree.).
[0094] As detailed above, the tool 20 (FIG. 2) has two DOF in
addition to a grasping function. It is contemplated that other
tools may be used with this inverse kinematic model by manipulating
constraints of the joint angles as detailed below.
[0095] When the tool 20 is a straight instrument, the joint angle
.theta..sub.5 and the joint angle .theta..sub.6 are about zero such
that the straight instrument is treated as a wristed instrument.
Accordingly, Equation 23 can be modified as follows:
{ u '' = k ' .times. u ' v '' = 0 w '' = k ' .times. w ' ( 37 )
##EQU00019##
[0096] When the tool 20 is a stapler instrument, a jaw (not shown)
of the stapler instrument is typically pivotal in one DOF such that
the joint angle .theta..sub.6 is substantially zero. Thus, Equation
23 can be modified as follows:
{ u '' = k ' .times. u ' v '' = 0 w '' = k ' .times. w ' ( 38 )
##EQU00020##
where
k ' = r 0 u '2 + w '2 , ##EQU00021##
and the rest of the algorithm detailed above continues to operate
properly with a stapler instrument as the tool 20.
[0097] When the tool 20 is another type of instrument with a single
DOF the joint angle .theta..sub.5 may be substantially zero.
Similar to the stapler instrument above, Equation 23 can be
modified such that:
{ u '' = k ' .times. u ' v '' = k ' .times. v ' w '' = 0 ( 39 )
##EQU00022## where
k '' = r 0 u '2 + v '2 , ##EQU00023##
and the rest of the algorithm detailed above continues to operate
properly with such an instrument.
[0098] It will be appreciated that Equations 37-39 are applied
after the correction Equations 25-34 have been applied. Thus, two
rounds of corrections are required.
[0099] When tool 20 is an instrument with long jaws 29 (e.g., a
bowel grasper), an additional frame may be needed. As shown in FIG.
17, a frame x.sub.e, y.sub.e, z.sub.e (End Effector Frame) is
defined orientated similar to Frame 6 and offset a distance a.sub.7
from Frame 6. As shown, the End Effector Frame is defined adjacent
a tip of the long jaws 29 along a centerline of the long jaws 29.
It is contemplated that the End Effector Frame may be on the
centerline of the long jaws 29, in a middle of the long jaws 29, or
any point along or between the long jaws 29.
[0100] Referring to FIGS. 18-20, the determination of u.sub.0 needs
to contemplate the distance a.sub.7. With particular reference to
FIG. 19, similar to the derivation of Equation 23
(r'' cos .theta..sub.5,max+a.sub.6+a.sub.7).sup.2+(r'' sin
.theta..sub.5,max).sup.2=r.sub.0.sup.2 (40)
Solving for r'' in Equation 40, coordinate u.sub.0 can be expressed
as:
u.sub.0=-(a.sub.6+a.sub.7+r'' cos .theta..sub.5,max) (41)
[0101] Following the projection method as detailed above, Equation
3 becomes:
6 0 .times. T = [ 6 0 .times. R [ u v w ] 0 1 ] ( 42 )
##EQU00024##
Expressing the Base Frame in the End Effector Frame provides:
6 0 .times. T = [ 6 0 .times. R [ u ' v ' w ' ] 0 1 ] ( 43 )
##EQU00025##
and the position of the trocar point "O" in the End Effector Frame
is:
[ u ' v ' w ' ] = - R T 6 0 .function. [ u v w ] ( 44 )
##EQU00026##
[0102] Referring to FIG. 18, u' is constrained according to
Equation 22. If u'>u.sub.0, where u.sub.0 can be dynamically
constrained by using Equation 40, the trocar point "O" is projected
to rotated trocar point "M" as shown in FIG. 20. The coordinates of
the rotated trocar point "M" is calculated according to Equation
25.
As shown in FIG. 18, the distance between trocar point "O" and
rotated trocar point "M" is:
l= {square root over ((u'-u.sub.0).sup.2+( {square root over
(r.sub.0.sup.2-u'.sup.2)}- {square root over
(r.sub.0.sup.2-u.sub.0.sup.2)}).sup.2)} (45)
and the distance between the trocar point "O" and the yaw point "T"
is:
r.sub.0'= {square root over
(r.sub.0.sup.2-u'.sup.2+(u'+a.sub.7).sup.2)}(46)
and the distance between the yaw point "T" and the rotated trocar
point "M" is:
r.sub.0'= {square root over
(r.sub.0.sup.2-u.sub.0.sup.2+(-u.sub.0-a.sub.7).sup.2)}(47)
The rotation angle .PHI.' can be expressed as:
.PHI. ' = a .times. .times. cos ( l 2 - r 0 '2 - r 0 ''2 2 .times.
r 0 ' .times. r 0 '' ) ( 48 ) ##EQU00027##
[0103] Referring to FIG. 21, following a similar derivation to that
of Equations 25-34, the rotation angle .PHI.' is instead of .PHI..
It will be appreciated that Equation 31 remains accurate; however,
Equation 32 changes to the following:
7 6 .times. T = [ x ^ 7 ' y ^ 7 ' z ^ 7 ' [ 0 0 0 ] 0 1 ] ( 49 )
##EQU00028##
and Equation 33 becomes the following:
7 6 .times. T = [ x ^ 7 y ^ 7 z ^ 7 [ 0 0 0 ] 0 1 ] ( 50 )
##EQU00029##
and further:
.sub.e.sup.0T=.sub.6.sup.0T.sub.e.sup.6T (51)
and .sub.6.sup.0T can be expressed as:
.sub.6.sup.0T=.sub.e.sup.0T[.sub.e.sup.6T].sup.-1 (52)
[0104] Where .sub.e.sup.6T is the End Effector Frame in the TCP
frame and is expressed as:
e 6 .times. T = [ I [ a 7 0 0 ] 0 1 ] ( 53 ) ##EQU00030##
[0105] By substituting the above equations into Equation 31:
.times. ( 54 ) 0 6 .times. T = [ x ^ 7 ' y ^ 7 ' z ^ 7 ' [ u 0 + a
7 v '' w '' ] 0 1 ] .function. [ x ^ 7 y ^ 7 z ^ 7 [ u ' + a 7 v '
w ' ] 0 1 ] - 1 .times. [ e 0 .times. T .function. [ I [ a 7 0 0 ]
0 1 ] - 1 ] - 1 ##EQU00031##
[0106] Finally, by applying the relative motion between the TCP
frame and the Base
[0107] Frame, the Rotated TCP Frame can be expressed in the Base
Frame as:
.sub.6'.sup.0T=[.sub.6.sup.0T].sup.-1 (55)
[0108] With reference to FIG. 22, an extreme condition for the
long-jaw instrument is shown such that the distance between the
trocar point :O: and the pitch point "P" is:
(r'' cos(.pi.-.theta..sub.5,extreme)-a.sub.6-a.sub.7).sup.2+(r''
sin(.pi.-.theta..sub.5,max)).sup.2=r.sub.0.sup.2 (56)
[0109] Solving r'' from Equation 56 provides:
u.sub.extreme=r'' cos(.pi.-.theta..sub.5,extreme)-a.sub.6-a.sub.7
(58)
[0110] Thus, the steps of the complete inverse kinematics algorithm
for a long-jaw instrument are as follows: [0111] 1) For each given
input pose .sub.6.sup.0T, calculate the trocar point "O" position
in the End Effector Frame using Equation 42; [0112] 2) Use Equation
40 to check if the input pose needs correction, if correction is
needed got to step 3, otherwise calculate .sub.6'.sup.0T from
Equation 52 and go to step 5; [0113] 3) Check if
u'>u.sub.extreme with Equation 57, if true provide feedback,
else got to Step 4; [0114] 4) Use Equations 49-55 to calculate a
corrected pose .sub.6'.sup.0T; and replace the original pose
.sub.6'.sup.0T with the corrected pose .sub.6'.sup.0T; [0115] 5)
Reapply Equations 3-18 to calculate the joint angles .theta..sub.1,
.theta..sub.2, .theta..sub.3, .theta..sub.5, and .theta..sub.6 as
well as the distance d.sub.4.
[0116] It is also within the scope of this disclosure to provide a
clinician with an indication or an alert when the arm 12 and the
tool 20 are approaching a singularity or may experience reduced
DOF. Thus, while checking whether u'>u.sub.extreme, if the angle
approaches the extreme angle .theta..sub.exterme (e.g., .pi./6 or
30.degree.), then the {circumflex over (x)}.sub.6 axis may be
pointing back to the trocar point "O" such that the motion of the
{circumflex over (x)}.sub.6 axis will be significantly amplified.
In such a condition, the inverse kinematic solution may become less
stable and the algorithm may provide a warning to the user such
that mapping between the input handle 43 (FIG. 1) and the tool 20
(FIG. 1) may be decoupled until the pose is corrected in Step 4 of
the algorithm detailed above.
[0117] Referring briefly back to FIG. 12, when the input pose
.sub.6'.sup.0T positions the trocar point "O" on the left side of
the boundary line "B" the tool 20 would be functioning in an
undesired configuration (e.g., with a possible decrease in DOF or
the desired pose of the tool 20 may not be reachable in light of
the current pose of the arm 12 or the desired pose of the tool 20
may be close to a singularity). In response to such an input pose,
it may be desirable to correct the pose of the arm 12 using the
projection method, as detailed above.
[0118] During a medical procedure, when a correction of the pose of
the arm 12 is desirable, the user interface 40 (FIG. 1) may provide
feedback to a clinician interfacing with the input handles 42. The
feedback may be visual, audible, or haptic. One form of haptic
feedback is force feedback that may inform the clinician of the
current status of the configuration of the arm 12 and the tool 20.
The force feedback would provide a tactile force through the input
handle 42 as the boundary line "B" is approached and/or crossed.
For example, the tactile force may have a first feedback force as
the boundary line B is approached and a second stronger feedback
force as the boundary line "B" is crossed. The feedback forces can
be generated by a feedback torque of a force feedback system (not
explicitly shown). With reference to FIG. 15, the feedback torque
can be expressed in the Base Frame as:
{circumflex over (.tau.)}=f(.PHI.){right arrow over
(T)}M.times.{right arrow over (T)}O/|{right arrow over
(T)}M.times.{right arrow over (T)}O| (59)
[0119] In Equation 42, f(.PHI.) is a scalar function that
determines the magnitude of the feedback torque {circumflex over
(t)} and the cross product of {right arrow over (T)}M and {right
arrow over (T)}O determines the direction of the feedback torque
{circumflex over (t)}. The f(.PHI.) may be a linear function or may
be an exponential function that is limited by a maximum torque. The
maximum torque may be set by torque that is realizable by the force
feedback system and/or may be set by torque that is physiologically
meaningful to a clinician interfacing with the input handles 42. It
will be appreciated that in Equation 42 the feedback torque
{circumflex over (t)} is expressed in the Base Frame and that when
displayed or applied by the input handles 42 of the user interface
40, the feedback torque {circumflex over (t)} would be expressed in
the proper frame of the user interface 40.
[0120] While several embodiments of the disclosure have been shown
in the drawings, it is not intended that the disclosure be limited
thereto, as it is intended that the disclosure be as broad in scope
as the art will allow and that the specification be read likewise.
Any combination of the above embodiments is also envisioned and is
within the scope of the appended claims. Therefore, the above
description should not be construed as limiting, but merely as
exemplifications of particular embodiments. Those skilled in the
art will envision other modifications within the scope of the
claims appended hereto.
* * * * *