Kinematics of wristed laparoscopic instruments

Abstract

Systems, methods, and computer program products for determining joint angles of a wristed laparoscopic tool of a robotic surgical instrument are disclosed. The method includes providing a first robotic arm where the robotic arm includes surgical instrument having a proximal end and a distal end and the tool is disposed at the distal end of the instrument. The tool has an elbow joint, a wrist joint, and a distal-most end. A three-dimensional position of the wrist joint is determined based on a three-dimensional position and orientation of the distal-most end. A three dimensional position of the elbow joint is determined based on the three-dimensional position of the wrist joint. A first angle corresponding to a twist of the tool, a second angle corresponding to an elbow joint angle of the tool, a third angle corresponding to a wrist angle, and a fourth angle corresponding to an open/close angle of the tool are determined.

1 .- 45 . (canceled) 
     
     
         46 . A method, comprising:
 (a) inserting a robotic arm into a trocar positioned at an incision site of a patient, wherein the robotic arm comprises a surgical instrument and a tool disposed thereon, wherein the tool comprises an elbow joint and a wrist joint;   (b) determining (i) a three-dimensional (3D) position of the wrist joint based on a three-dimensional position and orientation of the tool and (ii) a three-dimensional position of the elbow joint based on the three-dimensional position of the wrist joint;   (c) determining (iii) a first angle corresponding to a twist of the tool, (iv) a second angle corresponding to an elbow angle of the tool, (v) a third angle corresponding to a wrist angle of the tool, and (vi) a fourth angle corresponding to an open/close angle of the tool, based at least in part on the three-dimensional position of the wrist joint and the three dimensional position of the elbow joint; and   (d) generating a virtual representation of a surgical scene that is accessible via the incision site, wherein the virtual representation comprises a virtual model of the surgical instrument and a virtual anatomy of one or more anatomical structures in the surgical scene, wherein the virtual representation provides a real-time visualization of a procedure involving the surgical instrument and the one or more anatomical structures.   
     
     
         47 . The method of  claim 46 , wherein the real-time visualization provides a field of view for visualizing the procedure without seeing a physical tool or instrument. 
     
     
         48 . The method of  claim 46 , wherein the virtual anatomy comprises a 3D anatomical atlas or a 3D reconstruction based on one or more images of the one or more anatomical structures. 
     
     
         49 . The method of  claim 46 , further comprising, prior to (a), positioning the trocar at the incision site of the patient, wherein the position of the trocar is (i) specified by a surgeon or (ii) determined using a trajectory planning algorithm. 
     
     
         50 . The method of  claim 46 , wherein at least one of the first angle, the second angle, the third angle, and the fourth angle is determined based on a three-dimensional position of the trocar and a three-dimensional position and orientation of a distal end of the tool. 
     
     
         51 . The method of  claim 46 , further comprising, subsequent to (d), planning and executing one or more surgical maneuvers for the procedure using the virtual representation, wherein the procedure comprises a fully or partially automated surgical procedure. 
     
     
         52 . The method of  claim 46 , wherein (c) further comprises determining a pose of the tool based at least in part on a three-dimensional position of the trocar and at least one of the first angle, the second angle, the third angle, or the fourth angle. 
     
     
         53 . The method of  claim 52 , further comprising planning one or more surgical maneuvers based on the pose of the tool. 
     
     
         54 . The method of  claim 46 , wherein the three-dimensional position of the wrist joint is determined based on a three-dimensional position of the trocar and a pose of a distal end of the tool. 
     
     
         55 . The method of  claim 46 , wherein the three-dimensional position of the wrist joint is determined using a three-dimensional position of the distal end of the tool, a linear distance between the distal end of the tool and the wrist joint, and a directional unit vector pointing from the distal end of the tool to the wrist joint. 
     
     
         56 . The method of  claim 46 , wherein the three-dimensional position of the elbow joint is determined based on an intersection of a circle centered at the three-dimensional position of the wrist joint and a plane defined by a first vector representing a rotation axis of the wrist joint and a second vector representing a line connecting the wrist joint to the incision site, wherein the circle comprises a radius equal to a linear distance between the three-dimensional position of the wrist joint and the three-dimensional position of the elbow joint. 
     
     
         57 . The method of  claim 56 , wherein the intersection of the circle and the plane defines two or more solutions for the three-dimensional position of the elbow joint, wherein the method further comprises selecting at least one solution of the two or more solutions that does not violate one or more joint limits of the tool. 
     
     
         58 . The method of  claim 46 , further comprising generating a planned surgical path for the surgical instrument using the three-dimensional position of the wrist joint, the three-dimensional position of the elbow joint, the first angle, the second angle, the third angle, and the fourth angle. 
     
     
         59 . The method of  claim 46 , further comprising determining whether the surgical instrument will collide with an anatomical structure or another robotic arm, using the three-dimensional position of the wrist joint, the three-dimensional position of the elbow joint, the first angle, the second angle, the third angle, and the fourth angle. 
     
     
         60 . The method of  claim 46 , wherein the first angle is determined based on a transformation matrix of the elbow joint with respect to a distal end of the tool. 
     
     
         61 . The method of  claim 46 , wherein the second angle is determined based on the three-dimensional position of the wrist joint, the three-dimensional position of the elbow joint, and a three-dimensional position of the trocar. 
     
     
         62 . The method of  claim 46 , wherein the third angle is determined based on (i) an angle between a line defined from the three-dimensional position of the elbow to the three-dimensional position of the wrist and a first gripping member of the tool, and (ii) an angle between a line defined from the three-dimensional position of the elbow to the three-dimensional position of the wrist and a second gripping member of the tool. 
     
     
         63 . The method of  claim 46 , wherein the fourth angle is determined based on (i) an angle between a line defined from the three-dimensional position of the elbow to the three-dimensional position of the wrist and a first gripping member of the tool, and (ii) an angle between a line defined from the three-dimensional position of the elbow to the three-dimensional position of the wrist and a second gripping member of the tool. 
     
     
         64 . The method of  claim 46 , further comprising providing (i) positional information for the elbow or wrist joints, (ii) the first, second, third, and/or fourth angles, and (iii) a pose of the tool to a control system and using the control system to move at least one of the elbow or wrist joints to a target angle to actuate the surgical instrument. 
     
     
         65 . The method of  claim 64 , further comprising using the control system to move the wrist joint, the elbow joint, or the tool to a target position or pose to perform one or more steps of the procedure.