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.
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.
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.
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.
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.
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.
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.
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°. 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.
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.
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.
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.
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.
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.
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:
and
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.
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.
Referring to
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.
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.
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.
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.
With reference to
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.
θ2−π/2
θ5−π/2
Further, the Homogenous transform for movement in the six DOF is as follows:
where cθcos θ and sθsin θ, this convention will be used throughout this disclosure. Thus, the final pose of frame 6 in the base frame 0 is defined as:
6
0
T=
1
0
T
2
1
T
3
2
T
3
4
T
5
4
T
6
5
T (2)
With reference to
As shown in
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 θ6 and the joint angle θ5 are in a range of about −π/2 to about π/2. As used herein, angles are expressed in radians where it is equal to 180°.
In the second case modeled in
In the third case modeled in
Referring back to the first case shown in
and the rotation 60R which are both defined in the Base Frame, the homogenous transform of the “TCP” Frame in the Base Frame can be expressed as:
To determine the joint angles, the Base Frame is expressed in the TCP Frame as:
Based on the property that 06T=[60T]−1, 06R=[60R]−1=60RT it can be determined that:
Referring now to
The projection of the trocar point “O” in the {circumflex over (x)}6, ŷ6 plane is point “O′” and the projection of point “O′” on the ŷ6 axis is “O″”.
Applying the right hand rule, the relative sign and quarter locations are as follows:
Taking into account the relative sign and quarter location, as detailed above, in Equation 6:
θ6=a tan 2(v′,−u′) (7)
With reference to
θ5=a tan 2(w′,−u′ cos θ6+v′ sin θ6−a6) (8)
In addition, the distance d4 between the trocar point “O” and the pitch point “P” is:
d
4
=w′ sin θ5+(−u′ cos θ6+v′ sin θ6−a6)cos θ5 (9)
Now that the joint angle θ6, the joint angle θ5, and the distance d4 have been determined, the joint angle θ6, the joint angle θ5, and the distance d4 can be used to determine the joint angle θ1, the joint angle θ2, and the joint angle θ3. In light of Equation 2 above, a rotation matrix chain can be expressed as:
6
0
R=
1
0
R
2
1
R
3
2
R
4
3
R
5
4
R
6
5
R (10)
Since joint angle θ5 and joint angle θ6 are now known, in view of Equation 1, 65R can be expressed as:
In addition, 54R can be expressed as:
The transpose of the product of the above two equations is expressible as:
Then multiplying Equation 13 from the right side to Equation 10:
6
0
R(54R65R)T=60R(54R65R)−1=10R21R32R43R54R65R(54R65R)−1=40R (14)
Further, by defining
provides that:
Referring to
θ1=a tan 2(−
In addition, joint angle θ2 can be expressed as:
θ2=a tan 2(
It can be observed that when θ2=±π/2, shaft OP of the arm 12 or tool 20 (
With reference to
θ3=a tan 2(−
Using the previous derivation of the rotation matrix above, if the unit vector of the frame is defined as {circumflex over (x)}, ŷ, {circumflex over (z)}, the equation 15 can be expressed as:
With respect to the second case, shown in
θ6−θ6′=π (20)
Thus, to determine the joint angles in the second case, the joint angle θ6 is calculated in a manner similar to the first case detailed above and then Equation 20 is used to determine the joint angle θ6′ for the second case. Once the joint angle θ6′ for the second case is determined, the rest of the joint angles θ1, θ2, θ3, and θ5 as well as the distance d4 can be determined using the above method.
Now turning to the third case, as shown in
If the singularity of the third case is detected, the previous joint angle θ6 can be used. Thus, with θ6=θ6,previous, the rest of the joint angles θ1, θ2, θ3, and θ5 as well as the distance d4 can be determined using the above method.
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 θ5 and θ6 are usually in the range of about −π/2 to about π/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 π/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 θ5 approaches ±π/2, a small rotation of the input pose along the axis x6 would cause a dramatic change in joint angle θ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 θ3. It will be appreciated that such sudden motions are undesirable in clinical settings.
To avoid an unrealizable configuration or a singularity condition, the joint angle θ5 is restricted in a range of about −5π/12 to about 5π/12 and the joint angle θ6 is restricted in a range of about −π/2 to about π/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.
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.
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.
With reference to
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 a6 of the yoke “PT”, the joint angle θ5 will approach ±π/2. Thus, any small orientation change of the 26 axis will cause a large change in the joint angle θ6 and the joint angle θ2.
Continuing to refer to
Referring to
u′<u
0 (22)
The coordinate u0 is chosen to meet the physical limits of joint angle θ5 and joint angle θ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”.
With particular reference to
(r″ cos θ5,max+a6)2+(r″ sin θ5,max)2=r02 (23)
From Equation 23, r″ can be calculated, and coordinate u0 can be expressed as
u
0=−(a6+r″ cos θ5,max) (24)
where θ5, max is a desirable limit (i.e., mechanical constraint) on joint angle θ5 (e.g., 75° or 5π/12).
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.
Referring to
During this rotation, the orientation of the vector {right arrow over (T)}O in the {circumflex over (x)}6, {circumflex over (z)}6 plane remains constant. In accordance with the projection rule
Where the linear constant k is given by:
Referring to
ϕ=2a sin(l/2r0) (27)
where l=√{square root over ((u′−u0)2+(√{square root over (r02−u′2)}−√{square root over (r02−u02)})2)}.
Referring now to
0
6
T=
7
6
T
0
7
T (28)
and the Rotated Base Frame expressed from Frame 6 through Frame 7′ is:
0
6
T=
7′
6
T
0
7′
T (29)
During rotation, the Base Frame in Frame 7 is equal to the Rotated Base Frame in Frame 7′ such that:
0
7
T=
0
7′
T (30)
Thus, the Rotated Base Frame in Frame 6 can be calculated as:
0
6
T=
7
6
T[76T]−106T (31)
From Equation 31 we have:
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:
where 0′6R is from the homogenous transform 0′6T in Equation 31.
Referring to
(r″ cos(π−θ5,extreme)−a6)2+(r″ sin(π−θ5,extreme))2=r02 (35)
Solving for r″ from the above equation 35 provides:
u
extreme
=r″ cos(π−θ5,extreme)−a6 (36)
In light of the above, the steps of the complete inverse kinematics algorithm for the arm 12 and the tool 20 (
In the inverse kinematics algorithm above, checking if the input pose needs correction is accomplished by choosing u0 in Equation 22 as a function of the yaw point “T” and a maximum joint angle θ5, max. For example, the maximum joint angle θ5, max may be selected as 5π/12 (i.e., 75°).
As detailed above, the tool 20 (
When the tool 20 is a straight instrument, the joint angle θ5 and the joint angle θ6 are about zero such that the straight instrument is treated as a wristed instrument. Accordingly, Equation 23 can be modified as follows:
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 θ6 is substantially zero. Thus, Equation 23 can be modified as follows:
where
and the rest of the algorithm detailed above continues to operate properly with a stapler instrument as the tool 20.
When the tool 20 is another type of instrument with a single DOF the joint angle θ5 may be substantially zero. Similar to the stapler instrument above, Equation 23 can be modified such that:
where
and the rest of the algorithm detailed above continues to operate properly with such an instrument.
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.
When tool 20 is an instrument with long jaws 29 (e.g., a bowel grasper), an additional frame may be needed. As shown in
Referring to
(r″ cos θ5,max+a6+a7)2+(r″ sin θ5,max)2=r02 (40)
Solving for r″ in Equation 40, coordinate u0 can be expressed as:
u
0=−(a6+a7+r″ cos θ5,max) (41)
Following the projection method as detailed above, Equation 3 becomes:
Expressing the Base Frame in the End Effector Frame provides:
and the position of the trocar point “O” in the End Effector Frame is:
Referring to
As shown in
l=√{square root over ((u′−u0)2+(√{square root over (r02−u′2)}−√{square root over (r02−u02)})2)} (45)
and the distance between the trocar point “O” and the yaw point “T” is:
r
0′=√{square root over (r02−u′2+(u′+a7)2)}(46)
and the distance between the yaw point “T” and the rotated trocar point “M” is:
r
0′=√{square root over (r02−u02+(−u0−a7)2)}(47)
The rotation angle ϕ′ can be expressed as:
Referring to
and Equation 33 becomes the following:
and further:
e
0
T=
6
0
T
e
6
T (51)
and 60T can be expressed as:
6
0
T=
e
0
T[e6T]−1 (52)
Where e6T is the End Effector Frame in the TCP frame and is expressed as:
By substituting the above equations into Equation 31:
Finally, by applying the relative motion between the TCP frame and the Base
Frame, the Rotated TCP Frame can be expressed in the Base Frame as:
6′
0
T=[60T]−1 (55)
With reference to
(r″ cos(π−θ5,extreme)−a6−a7)2+(r″ sin(π−θ5,max))2=r02 (56)
Solving r″ from Equation 56 provides:
u
extreme
=r″ cos(π−θ5,extreme)−a6−a7 (58)
Thus, the steps of the complete inverse kinematics algorithm for a long-jaw instrument are as follows:
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′>uextreme, if the angle approaches the extreme angle θexterme (e.g., π/6 or 30°), then the {circumflex over (x)}6 axis may be pointing back to the trocar point “O” such that the motion of the {circumflex over (x)}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 (
Referring briefly back to
During a medical procedure, when a correction of the pose of the arm 12 is desirable, the user interface 40 (
{circumflex over (τ)}=f(ϕ){right arrow over (T)}M×{right arrow over (T)}O/|{right arrow over (T)}M×{right arrow over (T)}O| (59)
In Equation 42, f(ϕ) 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(ϕ) 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.
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.
Filing Document | Filing Date | Country | Kind |
---|---|---|---|
PCT/US2017/020567 | 3/3/2017 | WO | 00 |
Number | Date | Country | |
---|---|---|---|
62303437 | Mar 2016 | US |