Estimation of a position and orientation of a frame used in controlling movement of a tool

Abstract

A robotic system includes a camera having an image frame whose position and orientation relative to a fixed frame is determinable through one or more image frame transforms, a tool disposed within a field of view of the camera and having a tool frame whose position and orientation relative to the fixed frame is determinable through one or more tool frame transforms, and at least one processor programmed to identify pose indicating points of the tool from one or more camera captured images, determine an estimated transform for an unknown one of the image and tool frame transforms using the identified pose indicating points and known ones of the image and tool frame transforms, update a master-to-tool transform using the estimated and known ones of the image and tool frame transforms, and command movement of the tool in response to movement of a master using the updated master-to-tool transform.

We claim: 
     
       1. A robotic system comprising:
 a processor; 
 a master control device having a master frame; 
 an image capturing system having an image frame whose position and orientation relative to a fixed frame is determinable through one or more image frame transforms; and 
 a tool disposed within a field of view of the image capturing system and having a tool frame whose position and orientation relative to the fixed frame is determinable through one or more tool frame transforms; 
 all but one of the image and tool frame transforms being a known frame transform, because information for determining all dimensions and angles that are required for directly calculating the known frame transform is available for processing by the processor; and 
 the one of the image and tool frame transforms being an unknown frame transform from a first frame to a second frame, because information for determining at least one dimension or angle required for directly calculating the unknown frame transform is not available for processing by the processor; and 
 the processor being programmed to perform operations including:
 identifying pose indicating points of the tool from information in one or more images captured by the image capturing system; 
 determining an estimated transform for the unknown frame transform by using the identified pose indicating points and each known image and tool frame transform, the estimated transform being characterized by a chain of frame relationships between the first frame and the second frame with each known image and tool frame transform relating its corresponding frames to one another and with the identified pose indicating points relating the tool frame to the image frame; 
 updating a master-to-tool transform by using the estimated transform for the unknown frame transform and each known image and tool frame transform; and 
 commanding movement of the tool in response to movement of the master control device by using the updated master-to-tool transform. 
 
 
     
     
       2. The robotic system of  claim 1 , further comprising:
 a display that is viewable by an operator wherein an eye frame is defined at the eyes of the operator; and 
 a vision system configured to display information of images captured by the image capturing system in the display; 
 wherein the processor updates the master-to-tool transform by adjusting an offset which aligns an eye-master transform with an image-tool transform, wherein the eye-master transform transforms points in the master frame to corresponding points in the eye frame, and wherein the image-tool transform transforms points in the tool frame to corresponding points in the image frame. 
 
     
     
       3. The robotic system of  claim 1 , wherein the unknown frame transform is one of the one or more tool frame transforms. 
     
     
       4. The robotic system of  claim 3 , wherein the tool moves relative to a pivot point having a pivot point frame, the tool has an end effector which has an end effector frame, and the unknown frame transform transforms points in the pivot point frame to corresponding points in the end effector frame. 
     
     
       5. The robotic system of  claim 4 , wherein the unknown frame transform identifies a tool guide through which the tool extends so that the end effector extends out of a distal end of the tool guide, and wherein the identified tool guide is one of two or more candidate tool guides each having a different pivot point to end effector transform than another of the two or more candidate tool guides. 
     
     
       6. The robotic system of  claim 1 , wherein the unknown frame transform is one of the one or more image frame transforms. 
     
     
       7. The robotic system of  claim 6 , wherein the image capturing system is tethered to a mechanical element having a mechanical element frame, and wherein the unknown frame transform transforms points in the mechanical element frame to corresponding points in the image frame. 
     
     
       8. The robotic system of  claim 1 , further comprising:
 a first structure having a first robotic arm coupled to the image capturing system, the first structure having a first structure frame; and 
 a second structure having a second robotic arm coupled to the tool, the second structure having a second structure frame; 
 wherein one of the first and second structure frames is the fixed frame and the unknown frame transform transforms points in the other of the first and second structure frames to corresponding points in the fixed frame. 
 
     
     
       9. The robotic system of  claim 8 , wherein the first or second structure corresponding to the other of the first and second structure frames is a movable structure. 
     
     
       10. The robotic system of  claim 8 , wherein the first or second structure corresponding to the fixed frame is a movable structure. 
     
     
       11. The robotic system of  claim 1 , wherein one of the known frame transforms of the image frame transforms is determined by using a sensor positioned along a kinematic chain extending to the image capturing system. 
     
     
       12. The robotic system of  claim 1 , wherein one of the known frame transforms of the image frame transforms is determined by applying trigonometry to known dimensions of one or more structures mechanically coupled to the image capturing system. 
     
     
       13. The robotic system of  claim 1 , wherein one of the known frame transforms of the tool frame transforms is determined by using a sensor positioned along a kinematic chain extending to the tool. 
     
     
       14. The robotic system of  claim 1 , wherein one of the known frame transforms of the tool frame transforms is determined by applying trigonometry to known dimensions of one or more structures mechanically coupled to the tool. 
     
     
       15. The robotic system of  claim 1 , wherein the processor smoothes the estimated transform for the unknown frame transform to generate a smoothed estimated transform and updates the master-to-tool transform using the smoothed estimated transform. 
     
     
       16. The robotic system of  claim 15 , wherein the processor is programmed to: estimate the position and orientation of the tool frame relative to the image frame by periodically updating a prior estimate of the position and orientation of the tool frame relative to the image frame, determine the estimated transform for the unknown frame transform by updating a prior determination for the estimated transform for the unknown frame transform, and smooth the estimated transform for the unknown frame transform by averaging current and prior estimated transforms for the unknown frame transform. 
     
     
       17. A method implemented in a robotic system comprising a master control device having a master frame, an image capturing system having an image frame whose position and orientation relative to a fixed frame is determinable through one or more image frame transforms, a tool disposed within a field of view of the image capturing system and having a tool frame whose position and orientation relative to the fixed frame is determinable through one or more tool frame transforms, all but one of the image and tool frame transforms being a known frame transform, because information for determining all dimensions and angles that are required for directly calculating the known frame transform is available for processing by a processor, and the one of the image and tool frame transforms being an unknown frame transform from a first frame to a second frame, because information for determining at least one dimension or angle required for directly calculating the unknown frame transform is not available for processing by the processor; the method comprising:
 identifying pose indicating points of the tool from information in one or more images captured by the image capturing system; 
 determining an estimated transform for the unknown frame transform by using the identified pose indicating points and each known image and tool frame transform, the estimated transform being characterized by a chain of frame relationships between the first frame and the second frame with each known image and tool frame transform relating its corresponding frames to one another and with the identified pose indicating points relating the tool frame to the image frame; 
 updating a master-to-tool transform by using the estimated transform for the unknown transform and each known image and tool frame transform; and 
 commanding movement of the tool in response to movement of the master control device by using the updated master-to-tool transform. 
 
     
     
       18. The method of  claim 17 , wherein the robotic system further comprises a display that is viewable by an operator wherein an eye frame is defined at the eyes of the operator and a vision system configured to display information of images captured by the image capturing system in the display, and wherein the updating of the master-to-tool transform comprises:
 adjusting an offset which aligns an eye-master transform with an image-tool transform, wherein the eye-master transform transforms points in the master frame to corresponding points in the eye frame, and wherein the image-tool transform transforms points in the tool frame to corresponding points in the image frame. 
 
     
     
       19. The method of  claim 17 , wherein the unknown frame transform is one of the one or more tool frame transforms. 
     
     
       20. The method of  claim 19 , wherein the tool moves relative to a pivot point having a pivot point frame, the tool has an end effector which has an end effector frame, and the unknown frame transform transforms points in the pivot point frame to corresponding points in the end effector frame. 
     
     
       21. The method of  claim 20 , wherein the unknown frame transform identifies a tool guide through which the tool extends so that the end effector extends out of a distal end of the tool guide, and wherein the identified tool guide is one of two or more candidate tool guides each having a different pivot point to end effector transform than another of the two or more candidate guides. 
     
     
       22. The method of  claim 17 , wherein the unknown frame transform is one of the one or more image frame transforms. 
     
     
       23. The method of  claim 22 , wherein the image capturing system is tethered to a mechanical element having a mechanical element frame, and wherein the unknown frame transform transforms points in the mechanical element frame to corresponding points in the image frame. 
     
     
       24. The method of  claim 17 , wherein the robotic system further comprises a first structure having a first robotic arm coupled to the image capturing system, the first structure having a first structure frame, and a second structure having a second robotic arm coupled to the tool, the second structure having a second structure frame; and wherein one of the first and second structure frames is the fixed frame and the unknown frame transform transforms points in the other of the first and second structure frames to corresponding points in the fixed frame. 
     
     
       25. The method of  claim 24 , wherein the first or second structure corresponding to the other of the first and second structure frames is a movable structure. 
     
     
       26. The method of  claim 24 , wherein the first or second structure corresponding to the fixed frame is a movable structure. 
     
     
       27. The method of  claim 17 , further comprising:
 determining one of the known frame transforms of the image frame transforms by using a sensor positioned along a kinematic chain extending to the image capturing system. 
 
     
     
       28. The method of  claim 17 , further comprising:
 determining one of the known frame transforms of the image frame transforms by applying trigonometry to known dimensions of one or more structures mechanically coupled to the image capturing system. 
 
     
     
       29. The method of  claim 17 , further comprising:
 determining one of the known frame transforms of the tool frame transforms by using a sensor positioned along a kinematic chain extending to the tool. 
 
     
     
       30. The method of  claim 17 , further comprising:
 determining one of the known frame transforms of the tool frame transforms by applying trigonometry to known dimensions of one or more structures mechanically coupled to the tool. 
 
     
     
       31. The method of  claim 17 , further comprising:
 smoothing the estimated transform for the unknown frame transform to generate a smoothed estimated transform; and 
 wherein the updating of the master-to-tool transform uses the smoothed estimated transform. 
 
     
     
       32. The method of  claim 31 , wherein
 the estimating of the position and orientation of the tool frame relative to the image frame comprises periodically updating a prior estimate of the position and orientation of the tool frame relative to the image frame; 
 the determining of the estimated transform for the unknown frame transform comprises updating a prior determination for the estimated transform for the unknown frame transform; and 
 the smoothing of the estimated transform for the unknown frame transform comprises averaging current and prior estimated transforms for the unknown frame transform.