Internal access device, position attitude estimation system, position attitude estimation method, and program

Abstract

To provide an internal access device capable of accurately estimating a position and attitude.SOLUTION: An internal access device comprises: a main body including a tip part which contacts the surface of a subject from the outside of the subject or enters the inside of the subject via the surface of the subject from the outside of the subject, and a base part positioned outside the subject; an optical marker mounted on the base part and captured by a camera; an inertial measurement unit mounted on the base part and measuring information related to the attitude of the main body; and a distance detection unit mounted on the base part or on the surface of the subject and detecting the distance from a first position, which is the location of the distance detection unit, to a second position whose relative distance from the first position changes according to the position of the tip part.SELECTED DRAWING: Figure 3

Description

[Technical Field]

[0001] The present invention relates to an internal access device capable of accessing the inside of a subject, a position and orientation estimation system for an internal access device, a position and orientation estimation method for an internal access device, and a program for causing a computer to execute the position and orientation estimation method. [Background technology]

[0002] An example of such an internal access device is a surgical instrument used in laparoscopic surgery. In laparoscopic surgery, an endoscope and surgical instruments are inserted through a hole in the abdomen, and the lesion is removed using the surgical instruments. In laparoscopic surgery, surgery is performed while referring to two-dimensional endoscopic images. Therefore, the surgeon must be able to convert two-dimensional images into three-dimensional images. In addition, the surgeon must become accustomed to the mismatch between the coordinate systems between the eyes and hands during operation, as well as the reversal of surgical instrument operation.

[0003] One method to solve the above problem is to measure the position and orientation of surgical instruments using an optical motion capture system equipped with a camera and optical markers. Specifically, the position and orientation of surgical instruments can be measured by capturing images of optical markers attached to the surgical instruments with an external camera. This is expected to contribute to efficient surgical training by quantitatively analyzing the surgeon's surgical skills from the measured position and orientation of the surgical instruments and providing feedback to the surgeon.

[0004] However, in measurements using optical motion capture, there is a risk that at least a portion of the measurement data may be lost if an optical marker attached to a surgical instrument is obscured by a surgical assistant or the like. Therefore, a position estimation device using an inertial measurement unit is considered, as disclosed in Patent Documents 1 and 2. In the position estimation devices disclosed in Patent Documents 1 and 2, when at least a portion of the measurement data obtained by optical motion capture is lost, the position of the human body, which is the target of position estimation, is estimated using the measurement data from the inertial measurement unit. [Prior art documents] [Patent documents]

[0005] [Patent Document 1] Japanese Patent Application Laid-Open No. 2016-6415 [Patent Document 2] Japanese Patent Publication No. 2023-24042 Summary of the Invention [Problem to be solved by the invention]

[0006] The position estimation devices disclosed in Patent Documents 1 and 2 use an inertial measurement unit to estimate position. An inertial measurement unit is a device that measures the acceleration applied to an object. It measures the orientation of the object by measuring the gravitational acceleration applied to the object, and can calculate the movement acceleration by removing the component derived from the gravitational acceleration from the measured acceleration information. Therefore, in order to obtain position information using an inertial measurement unit, it is necessary to integrate the measured movement acceleration data twice. In this case, if an error occurs in the integration, the error will accumulate due to the two integrations. As a result, there is a risk that the accuracy of the estimated position will decrease.

[0007] SUMMARY OF THE INVENTION Therefore, an object of the present invention is to solve the above problems and to provide an internal access device capable of estimating position and orientation with high accuracy. [Means for solving the problem]

[0008] An internal access device according to one aspect of the present disclosure includes: a main body having a tip portion that contacts a surface of the subject from outside the subject or that enters the subject from outside the subject via the surface of the subject, and a base portion that is located outside the subject; an optical marker mounted on the base and imaged by a camera; an inertial measurement unit mounted on the base and configured to measure information relating to the attitude of the main body; and a distance detection unit mounted on the base or the surface of the subject, which detects the distance from a first position, which is the position of the distance detection unit, to a second position, which is a position outside the subject and whose relative distance to the first position changes depending on the position of the tip.

[0009] Furthermore, a position and orientation estimation system according to an aspect of the present disclosure includes: the internal access device; an optical motion capture system including the camera and the optical marker, the camera capturing an image of the optical marker to obtain position information of the optical marker; a computer that acquires position information from the optical motion capture system, acquires attitude information of the main body from the inertial measurement unit, and acquires distance information from the first position to the second position from the distance detection unit; The computer If the acquired position information is not missing, estimate the position and attitude of the internal access device based on the acquired position information; When at least a portion of the acquired position information is missing, the position and orientation of the internal access device are estimated based on the last position information before the missing information, the acquired orientation information, and the acquired distance information.

[0010] Furthermore, a position and orientation estimation method according to one aspect of the present disclosure includes: 1. A position and orientation estimation method for estimating a position and orientation of an internal access device, the method comprising: a main body having a tip portion that contacts a surface of a subject from outside the subject or that enters the inside of the subject from outside the subject via the surface of the subject, and a base portion located outside the subject; an optical marker mounted on the base and imaged by a camera; an inertial measurement unit mounted on the base and measuring information related to the orientation of the main body; and a distance detection unit mounted on the base or the surface of the subject, the distance detection unit detecting a distance from a first position, which is the position of the distance detection unit, to a second position, which is a position outside the subject and whose relative distance to the first position changes depending on the position of the tip portion, acquiring position information from an optical motion capture system that includes the camera and the optical marker, and obtains position information of the optical marker by capturing an image of the optical marker with the camera; acquiring attitude information of the main body from the inertial measurement unit; acquiring distance information from the distance detection unit to the second position; If the acquired position information is not missing, estimate the position and attitude of the internal access device based on the acquired position information; When at least a portion of the acquired position information is missing, the position and orientation of the internal access device are estimated based on the last position information before the missing information, the acquired orientation information, and the acquired distance information.

[0011] A program according to one aspect of the present disclosure causes a computer to execute the position and orientation estimation method. [Effects of the Invention]

[0012] According to the present disclosure, it is possible to provide an internal access device capable of estimating position and orientation with high accuracy. [Brief explanation of the drawings]

[0013] [Figure 1] 1 is a diagram showing the overall configuration of a position and orientation estimation system according to an embodiment of the present disclosure. [Figure 2] FIG. 1 is a block diagram of a position and orientation estimation system according to an embodiment of the present disclosure. [Figure 3] FIG. [Figure 4] FIG. 1 is a schematic perspective view showing an internal access device and a trocar. [Figure 5] FIG. 1 is a schematic side view showing an internal access device and a trocar. [Figure 6] 10 is a flowchart for explaining a position and orientation estimation process performed by a position and orientation estimation system according to an embodiment of the present disclosure. [Figure 7]FIG. 1 is a diagram showing an example of an algorithm for position and orientation estimation processing by a position and orientation estimation system according to an embodiment of the present disclosure. [Figure 8] FIG. 4 is a block diagram showing the data flow of the position and orientation estimation process. [Figure 9] 10 is a graph showing changes over time in coordinate values ​​of an internal access device estimated by a position and orientation estimation system according to an embodiment of the present disclosure. [Figure 10] 10 is a graph showing changes over time in coordinate values ​​of an internal access device estimated by a position and orientation estimation system of a comparative example. [Figure 11] 11 is a graph obtained by superimposing the graphs in FIGS. 9 and 10. DETAILED DESCRIPTION OF THE INVENTION

[0014] An example of the present disclosure will now be described with reference to the accompanying drawings. Note that the following description is merely illustrative in nature and is not intended to limit the present disclosure, its applications, or its uses. The drawings are schematic, and the ratios of dimensions and the like do not necessarily correspond to reality. Furthermore, in the following description, terms indicating specific directions or positions (e.g., terms including "upper," "lower," "right," "left," "front," and "rear") are used as necessary. However, the use of terms indicating specific directions or positions is intended to facilitate understanding of the present disclosure with reference to the drawings, and the meanings of these terms do not limit the technical scope of the present disclosure.

[0015] <Embodiment> The position and orientation estimation system estimates the position and orientation of the internal access device, for example, coordinate values ​​within the interior of the subject accessed by the internal access device.

[0016] For example, a position and orientation estimation system estimates the position of the tip of a surgical instrument such as forceps used in laparoscopic surgery. In this case, the surgical instrument such as forceps corresponds to the main body of the internal access device. In laparoscopic surgery, the tip of the surgical instrument penetrates the epidermis of a subject (e.g., a human body that is the surgical target) and enters the interior of the subject. The position and orientation estimation system estimates the position and orientation of the surgical instrument that has entered the interior of the subject.

[0017] The position and orientation estimation system is not limited to systems used in laparoscopic surgery. For example, the position and orientation estimation system may be one that estimates the position of the tip of a pointer used in brain surgery. In this case, the pointer corresponds to the main body of the internal access device. In brain surgery, the tip of the pointer comes into contact with the epidermis of the subject (e.g., a human body that is the surgical target). Navigation within the brain is performed by emitting ultrasound or the like from the tip of the pointer into the interior of the subject. The position and orientation estimation system estimates the position and orientation of the pointer that comes into contact with the surface of the subject.

[0018] The position and orientation estimation system is not limited to systems used in surgery. For example, the position and orientation estimation system may be applied to non-destructive testing, which examines the surface or interior of an object such as a building without destroying the object. In this case, an emission device that emits ultrasound, radiation, or the like used in non-destructive testing corresponds to the main body of the internal access device. In non-destructive testing, the tip of the emission device contacts the surface of an object (such as a building to be inspected) or penetrates the surface to enter the interior of the object. The position and orientation estimation system estimates the position and orientation of the emission device.

[0019] The position and orientation of the aforementioned tools and devices are estimated by an optical motion capture system included in the position and orientation estimation system. More specifically, this is done based on position information obtained by a camera included in the optical motion capture system capturing images of optical markers mounted on the internal access device. However, if an obstacle or the like is positioned between the camera and the optical marker, preventing capture of the image, at least a portion of the position information may be lost. In this case, the position and orientation estimation system estimates the position and orientation of the tool or device based on information measured by an inertial measurement unit included in the position and orientation estimation system and information detected by a distance detection unit included in the position and orientation estimation system.

[0020] Hereinafter, as one embodiment of the present disclosure, a configuration of a position and orientation estimation system 10 used in laparoscopic surgery will be described.

[0021] Fig. 1 is a diagram illustrating an overall configuration of a position and orientation estimation system according to an embodiment of the present disclosure. Fig. 2 is a block diagram of the position and orientation estimation system according to an embodiment of the present disclosure.

[0022] As shown in FIGS. 1 and 2, the position and orientation estimation system 10 includes an internal access device 20, an optical motion capture system 30, and a computer 40.

[0023] The internal-access device 20 and the computer 40 are wirelessly connected by known means such as Bluetooth (registered trademark) or Wi-Fi (registered trademark), and the optical motion capture system 30 and the computer 40 are wired by known means such as USB (registered trademark), and are capable of communicating with each other. Note that the internal-access device 20, the optical motion capture system 30, and the computer 40 may each be configured to be wired or wirelessly connectable.

[0024] In this embodiment, the main body 21 of the internal access device 20 is a forceps, a surgical instrument used in laparoscopic surgery. As shown by the dashed line in FIG. 1 , the main body 21 penetrates a surface 60A of the subject 60 to access the interior of the subject 60. In laparoscopic surgery, the subject 60 is typically a human body. In this embodiment, the subject 60 is a box that serves as a substitute for a human body for laparoscopic surgery training. Note that the subject 60 is not limited to the box. For example, the subject 60 may be a human body to be operated on or a building to be inspected, as described above, or a pig to be used as an experimental subject, as described below. In other words, the subject is a living organism (e.g., a person or an animal) or an object (e.g., the box or building) accessed by the internal access device 20 for an examination, experiment, or the like. Here, "accessed by the internal access device 20" refers to, for example, the internal access device 20 contacting the surface of the subject 60 or the internal access device 20 entering the interior of the subject 60.

[0025] The optical motion capture system 30 obtains position information by capturing images of optical markers using cameras. In this embodiment, the optical motion capture system 30 includes a housing 31, three cameras 32 mounted on the housing 31, and four optical markers 23 mounted on the main body 21 of the internal access device 20. In this embodiment, the cameras 32 are infrared cameras, but other cameras may be used. The number of cameras may be any number greater than or equal to two, and each camera may be independent. Each of the three cameras 32 captures images of the four optical markers 23. As a result, the optical motion capture system 30 obtains position information of the optical markers 23. The number of optical markers 23 may be any number greater than or equal to three. In addition to the three cameras 32, the housing 31 is equipped with a communication unit (not shown) for communicating with the outside. The position information is transmitted to a computer 40 by the communication unit.

[0026] The computer 40 is, for example, a personal computer. The computer 40 is not limited to a personal computer, and may be any computer that executes various calculations based on acquired information.

[0027] FIG. 3 is a side view showing the internal access device.

[0028] As shown in Figures 2 and 3, the internal access device 20 includes a main body 21, a storage section 22, an optical marker 23, an inertial measurement unit (IMU) 24, a distance detection section 25, a microcomputer 26, and a battery 27.

[0029] As described above, the main body 21 is a surgical instrument used in laparoscopic surgery. In this embodiment, the main body 21 is a pair of forceps. The main body 21 includes a base portion 211 and an insertion portion 212.

[0030] The base 211 is a portion of the main body 21 that is not inserted into the subject 60 but is located outside the subject 60. The base 211 includes a handle portion 211A. The handle portion 211A is a portion that is gripped and operated by a user of the internal-access device 20. The insertion portion 212 is a portion of the main body 21 that can enter the subject 60 from the outside through a surface 60A of the subject 60.

[0031] The insertion portion 212 is rod-shaped. The insertion portion 212 extends from the base portion 211 in the axial direction 101. A tip portion 212A of the insertion portion 212 is located on the opposite side of the base portion 211 in the axial direction 101. In other words, one end of the base portion 211 in the axial direction 101 is connected to the base portion 211, and the other end of the base portion 211 in the axial direction 101 is the tip portion 212A. The tip portion 212A is configured to be openable and closable.

[0032] Figure 4 is a schematic perspective view of the internal access device and trocar, and Figure 5 is a schematic side view of the internal access device and trocar.

[0033] As shown in FIGS. 4 and 5 , in this embodiment, a trocar 50 having a through-hole 50A is attached to a subject 60. The trocar 50 is inserted into a hole formed in the subject 60. This allows the outside and inside of the subject 60 to communicate via the through-hole 50A. The main body 21 of the internal access device 20 is inserted into the through-hole 50A provided in the trocar 50. This allows the tip 212A of the main body 21 of the internal access device 20 to enter the inside of the subject 60.

[0034] 3 to 5, the housing section 22 is attached to the base section 211 of the main body 21. The housing section 22 has optical markers 23, an inertial measurement device 24, a distance detection section 25, a microcomputer 26, and a battery 27 mounted therein.

[0035] The storage section 22 has four protrusions 221. The number of protrusions may be any number greater than or equal to three. To identify multiple surgical instruments, the arrangement of the protrusions is unique to each surgical instrument. The optical marker 23 described above is attached to the tip of each of the four protrusions 221. The optical marker 23 is part of the optical motion capture system 30 and also part of the internal access device 20. As shown in FIG. 2, the storage section 22 has an internal space 22A. The internal space 22A houses an inertial measurement unit 24, a distance detection unit 25, a microcomputer 26, and a battery 27.

[0036] The inertial measurement unit 24 is a measurement unit equipped with an acceleration sensor and a gyro sensor. The inertial measurement unit 24 is used to measure the attitude of the internal access device 20. In this embodiment, the inertial measurement unit 24 has a three-axis acceleration sensor, a three-axis gyro sensor, and a three-axis geomagnetic sensor. The inertial measurement unit 24 measures information related to the attitude of the internal access device 20. The information related to the attitude includes acceleration and angular velocity.

[0037] In this embodiment, the distance detection unit 25 is a ToF (Time of Flight) distance measurement sensor. In this embodiment, the distance detection unit 25 detects a distance D1 (see FIG. 5 ) from the distance detection unit 25 to the opposing surface 50B of the trocar 50. In this embodiment, the position of the distance detection unit 25 corresponds to a first position, and the position of the opposing surface 50B of the trocar 50 corresponds to a second position. Note that in this embodiment, the distance detection unit 25 is attached to the main body 21 of the internal-access device 20, but the distance detection unit 25 may be provided separately from the main body 21. For example, the distance detection unit 25 may be provided on the surface 60A of the subject 60. In this case, the distance detection unit 25 detects, for example, the distance from the surface 60A of the subject 60 on which the distance detection unit 25 is provided to the main body 21 of the internal-access device 20.

[0038] 4 and 5, the main body 21 of the internal access device 20 enters the inside of the subject 60 via the trocar 50. That is, the tip 212A penetrates the surface 60A of the subject 60. In this state, the movement of the main body 21 is restricted to four degrees of freedom: translational movement in the axial direction 101 of the main body 21 and movement around each of three three-dimensional coordinate axes.

[0039] Here, the translational movement is a sliding movement M1 along the axial direction 101. Furthermore, the movements around each of the three three-dimensional coordinate axes are a rotational movement M2 and rotational movements M3 and M4.

[0040] The rotational movement M2 is a movement that rotates in the circumferential direction around the axis of the main body 21. The axis of the main body 21 is an imaginary line that is parallel to the axial direction 101 and passes through the center of the insertion portion 212.

[0041] The rotational movement M3 is a rotational movement in a first direction perpendicular to the axial direction 101, centered on a portion of the main body 21 located on the surface 60A of the subject 60 (in this embodiment, the insertion point 50C of the trocar 50). The rotational movement M4 is a rotational movement in a second direction perpendicular to both the axial direction 101 and the first direction, centered on a portion of the main body 21 located on the surface 60A of the subject 60 (in this embodiment, the insertion point 50C of the trocar 50). Here, the first and second directions are not limited to the directions depicted in FIG. 4, but are any directions that satisfy the above-mentioned conditions. In other words, the rotational movements M3 and M4 are rotational movements in any direction, centered on a portion of the main body 21 located on the surface 60A of the subject 60 (in this embodiment, the insertion point 50C of the trocar 50).

[0042] Based on the distance D1 acquired from the distance detection unit 25, the microcomputer 26 measures the change in distance due to the slide movement M1 among the slide movement M1, rotational movement M2, and pivotal movements M3 and M4 described above. For example, as shown in FIG. 5, the distance D2 from the insertion point 50C of the trocar 50 to the tip 212A is constantly estimated from the movement trajectory of the surgical tool tip position measured by the optical motion capture system 30. Similarly, the distance D1 acquired from the distance detection unit 25 is constantly measured. If the distance D1 immediately before the acquired positional information is lost is D1i and the distance D2 is D2i, the distance D2 after the positional information is lost can be calculated using the distance D1 detected by the distance detection unit 25 as D2 = D2i - M1 and M1 = D1 - D1i.

[0043] The second position may be any position that satisfies the following conditions. That is, the second position may be any position outside the subject 60, where the relative distance from the first position changes depending on the position of the tip 212A. The facing surface 50B of the trocar 50 is outside the subject 60, and the distance from the tip 212A changes depending on the position of the tip 212A. Therefore, the facing surface 50B of the trocar 50 may correspond to the second position. As long as the above conditions are satisfied, the second position is not limited to the position of the facing surface 50B of the trocar 50. That is, the distance detection unit 25 may measure a distance other than the distance D1 shown in FIG. 5. For example, if the position and orientation estimation system 10 does not include a trocar 50, the distance detection unit 25 may measure the distance along the axial direction 101 from the distance detection unit 25 to the surface 60A of the subject 60. In this case, the position of the surface 60A of the subject 60 corresponds to the second position, and the position of the surface 60A of the subject 60 satisfies the above conditions.

[0044] The distance detection unit 25 may be a distance measurement sensor other than the ToF type. Furthermore, the distance detection unit 25 is not limited to a distance measurement sensor. For example, the distance detection unit 25 may detect the distance based on the amount of rotation of a ball. Specifically, a ball is placed on the inner surface constituting the through-hole 50A of the trocar 50, and the internal access device 20 inserted into the through-hole 50A moves in contact with the ball, thereby measuring the amount of rotation of the ball.

[0045] The microcomputer 26 has a calculation function and a communication function. The communication function is, for example, Bluetooth communication or Wi-Fi communication. The microcomputer 26 is powered by a battery 27. The microcomputer 26 transmits information from the inertial measurement unit 24 and the distance detection unit 25 to the computer 40 either directly or after calculation. At this time, the data may be received via a microcomputer or a wireless communication unit connected to the computer 40. In this embodiment, the microcomputer 26 transmits to the computer 40 attitude information of the internal access device 20 calculated from information related to the attitude, including the acceleration and angular velocity measured by the inertial measurement unit 24. The microcomputer 26 also transmits distance information detected by the distance detection unit 25 (information on distance D1 in this embodiment) directly to the computer 40.

[0046] The computer 40 acquires position information from a communication unit mounted on the housing 31 of the optical motion capture system 30 .

[0047] The computer 40 also acquires the following information from the inertial measurement unit 24 and the distance detection unit 25. The computer 40 acquires attitude information from the inertial measurement unit 24 and distance information regarding the distance D1 from the distance detection unit 25. In this embodiment, the computer 40 acquires the above information from the inertial measurement unit 24 and the distance detection unit 25 via the microcomputer 26. Of course, the computer 40 may also acquire the above information directly from the inertial measurement unit 24 and the distance detection unit 25.

[0048] The internal access device described above can achieve the following effects.

[0049] According to this embodiment, the distance detection unit 25 can detect distance information regarding the distance D1 from the first position to the second position. When distance information is sent from the internal access device 20 to the computer 40, the computer 40 does not need to calculate the position of the internal access device 20 by integrating twice the information regarding the acceleration measured by the inertial measurement unit 24. As a result, the position and attitude of the internal access device 20 can be estimated with high accuracy based on the distance D1 from the first position to the second position detected by the distance detection unit 25 and the attitude information measured by the inertial measurement unit 24.

[0050] The internal access device 20 is equipped with an optical marker 23. Therefore, position information regarding the position of the optical marker 23 is obtained by capturing an image of the optical marker 23 with the camera 32. When the position information is sent to the computer 40, the computer 40 can estimate the position and attitude of the internal access device 20 based on the position information.

[0051] Even if at least a portion of the position information is lost due to the presence of an obstacle between the optical marker 23 and the camera 32, the internal access device 20 can send information for estimating its position and attitude from the inertial measurement unit 24 and the distance detection unit 25 to the computer. The information for estimating the position and attitude is attitude information and distance information. The computer 40 can estimate the attitude of the internal access device 20 based on the attitude information acquired from the inertial measurement unit 24 of the internal access device 20. The computer 40 can also estimate the position of the internal access device 20 based on the distance information acquired from the distance detection unit 25 of the internal access device 20.

[0052] According to this embodiment, the movement of the internal access device 20 is limited to the sliding movement M1, the rotational movement M2, and the pivotal movements M3 and M4, which makes it easy to detect the distance by the distance detection unit 25. The internal access device 20, whose movement direction is limited as described above, is a suitable application example for using the distance detection unit 25 that detects the distance D1 from the first position to the second position.

[0053] According to this embodiment, the internal access device 20 can be used in laparoscopic surgery.

[0054] According to this embodiment, the configuration for detecting the distance D1 from the first position to the second position can be simplified, for example, the number of parts required for detecting the distance D1 from the first position to the second position can be reduced.

[0055] According to this embodiment, the inertial measurement unit 24 and the distance detection unit 25 are both housed in the housing unit 22 (one location). Therefore, compared to a configuration in which the inertial measurement unit 24 and the distance detection unit 25 are mounted in different locations (multiple locations) on the main body 21, it is possible to reduce the interference caused by the inertial measurement unit 24 and the distance detection unit 25 with the operation of the internal access device 20 by the operator.

[0056] FIG. 6 is a flowchart illustrating a position and orientation estimation process performed by a position and orientation estimation system according to an embodiment of the present disclosure.

[0057] A method for estimating the position and orientation of the internal access device 20 by the position and orientation estimation system 10 will be described below with reference to Fig. 6. In this embodiment, the estimation of the position and orientation of the internal access device 20 described below is realized by the computer 40 executing a program stored in a storage medium (not shown). The program may be stored in a read-only memory (ROM) or a random access memory (RAM), or may be stored in wired logic such as an application-specific integrated circuit (ASIC) or a field-programmable gate array (FPGA).

[0058] As described above, the computer 40 acquires position information from the optical motion capture system 30 (MoCap) (S10). The computer 40 also acquires attitude information and distance information from the internal access device 20 (S20, S30). Specifically, the computer 40 acquires attitude information from the inertial measurement unit 24 (IMU) (S20) and distance information from the distance detection unit 25 (S30). Note that the execution order of steps S10-S30 is not limited to the order shown in FIG. 6. In this embodiment, steps S10-S30 are executed in parallel.

[0059] In this embodiment, the optical motion capture system 30 performs measurements at 120 Hz, while the inertial measurement unit 24 and distance detection unit 25 perform measurements at 40 Hz. The computer 40 continuously acquires information at these frequencies. In steps S10-S30, the computer 40 acquires the same number of pieces of posture information and distance information, and acquires three times as many pieces of position information as the number of pieces of posture information and distance information.

[0060] The computer 40 determines whether at least a part of the position information acquired in step S10 is missing (S40).

[0061] Note that step S40 can be performed at any frequency. For example, if step S40 is performed at 40 Hz, the measurement values ​​acquired by the computer 40 in steps S10-S30 before step S40 is performed are as follows: In step S10, the computer 40 acquires three values ​​of position information from the optical motion capture system 30. In step S20, the computer 40 acquires one value of attitude information from the inertial measurement unit 24. In step S30, the computer 40 acquires one value of distance information from the distance detection unit 25.

[0062] If at least a part of the position information acquired in step S10 is missing (S40: YES), the computer 40 estimates the position and posture of the missing internal access device 20 based on the posture information acquired in step S20 and the distance information acquired in step S30 (S50). Note that a known method is used to estimate the position and posture of the internal access device 20 in step S50.

[0063] On the other hand, if there is no missing information in the position information acquired in step S10 (S40: NO), the computer 40 estimates the position and attitude of the internal access device 20 based on the position information acquired in step S10 (S60). Note that, to estimate the position and attitude of the internal access device 20 in step S60, a known method is used, as in step S50.

[0064] Furthermore, the computer 40 corrects the posture information acquired in step S20 based on the position information acquired in step S10 (S70). In this case, the next time step S50 is executed, the computer 40 interpolates the missing position information based on the posture information corrected in step S70 and the distance information acquired in step S30 (S50). Although not shown in the flowchart, the position information acquired in step S10 may be corrected based on the posture information acquired in step S20. For example, the computer 40 constantly calculates and records a difference value between the posture information of the internal access device 20 calculated based on the position information acquired from the optical motion capture system 30 in step S10 and the posture information of the internal access device 20 acquired from the inertial measurement unit 24 in step S20. If the change in the difference value from the previous calculation is equal to or greater than a preset threshold, the computer 40 performs a correction to exclude the corresponding position information from the position information acquired in step S10 as an outlier. That is, the computer 40 determines whether the posture information calculated based on the position information acquired from the optical motion capture system 30 is an abnormal value based on the fluctuation of the difference value.

[0065] If the measurement has not ended (S80: NO), the processes of steps S10 to S70 are repeated, and if the measurement has ended (S80: YES), the execution of the processes of steps S10 to S70 is stopped.

[0066] FIG. 7 is a diagram illustrating an example of an algorithm for position and orientation estimation processing by the position and orientation estimation system according to an embodiment of the present disclosure.

[0067] An example of an algorithm for executing the processing described in the flowchart of Fig. 6 will be described below with reference to Fig. 7. Note that this algorithm is just an example, and other algorithms may be used for the position and orientation estimation processing by the position and orientation estimation system.

[0068] First, the computer 40 calculates the position p of the tip 212A of the internal access device 20 measured by the optical motion capture system 30. MoCap and the insertion point p of the internal access device 20 pivot (insertion point 50C of the trocar 50) and the position p of the tip 212A MoCap The distance D2 to the target point is added to the distance information l, which is the measurement value obtained by the distance detection unit 25. ToF The sum (l old ) are constantly recorded (S10, S30).

[0069] There is an attitude angle offset between the attitude information of the internal access device 20 measured by the optical motion capture system 30 and the attitude information of the internal access device 20 measured by the inertial measurement unit 24. In the sixth and seventh lines of FIG. 7, in order to eliminate the attitude angle offset, the differential quaternion q diff is calculated, and q in the global coordinate system diff Rotation axis of G n diff is required.

[0070] However, the difference quaternion q diff Rotation axis of G n diffis expressed in the global coordinate system and therefore constantly changes with the change in the posture of the internal access device 20. Therefore, in the eighth line of FIG. G n diff is the rotation axis in the local coordinate system of the inertial measurement unit 24 L n diff Also, in the 9th line of Figure 7, the differential quaternion q diff Rotation angle θ diff The processing from lines 4 to 10 in Fig. 7 is executed only if there is no missing position information (S40: NO). Therefore, in the processing from lines 4 to 10 in Fig. 7, an attitude angle offset is constantly calculated based on the position information measured by the optical motion capture system 30, thereby correcting errors in the attitude information acquired from the inertial measurement unit 24 (S70).

[0071] 7 is executed when attitude information is received from the inertial measurement unit 24. At this time, the attitude angle offset rotation axis in the local coordinate system of the inertial measurement unit 24 obtained in the above-mentioned process is L n diff and rotation angle θ diff Based on this, the pose estimate q est is calculated (S50).

[0072] In the processing of lines 15 and 16 in FIG. 7, the position estimate value of the tip 212A of the internal access device 20 is G p est is calculated (S50). In the optical motion capture system 30, the local coordinate system of the internal access device 20 is defined as a right-handed coordinate system with the Y axis pointing upward and the X axis of which coincides with the sheath axis of the internal access device 20. Therefore, the position of the tip 212A of the internal access device 20 in the local coordinate system of the inertial measurement unit 24 is ML p est is defined as in the 15th line of Figure 7. Here, the distance information l ToF The sum of (l old) is calculated by multiplying the insertion length (distance D2) of the internal access device 20 most recently measured by the optical motion capture system 30 by the distance information l, which is the most recent measurement value of the distance detection unit 25. ToF Therefore, the distance information l, which is the current measurement value of the distance detection unit 25, is added. ToF The current insertion length is calculated by taking the difference from the

[0073] Then, on the 16th line of FIG. 7, the estimated position of the tip 212A in the global coordinate system is G p est In the calculation, the position p of the tip 212A of the internal-access device 20 last measured by the optical motion capture system 30 is acquired. last The difference in the change in the estimated value (q est ML p est q -1 est -p old ) ensures continuity of the values ​​at the interpolation start point. If there is no missing position information (S40: NO), q est =q MoCap and G p est =p last holds, the measurement values ​​obtained by the optical motion capture system 30 can be obtained as they are.

[0074] The above-described interpolation process has a problem in that it is susceptible to noise and abnormal values ​​in the position information immediately before the loss, since the interpolation process uses the position information before the loss as a reference. Noise is also contained in the inertial measurement unit 24 and the distance detection unit 25. For this reason, in this embodiment, abnormal values ​​are removed and smoothed for each measurement value (position information, attitude information, and distance information) prior to the interpolation process.

[0075] The outlier removal and smoothing will be described below with reference to Fig. 8. Fig. 8 is a block diagram showing the data flow of the position and orientation estimation process.

[0076] First, to remove abnormal values ​​from the position information, outlier removal based on the interval measurement rate and outlier removal using a Kalman filter are performed. In outlier removal based on the interval measurement rate, if the measurement rate within a moving window falls below a threshold, the frame below the threshold is removed as an abnormal value. For example, the number of windows is set to 60, and the measurement rate threshold is set to 25 (%). In outlier removal based on the Kalman filter, the Kalman filter estimated value is compared with the measured value, and if the deviation between the two exceeds a certain multiple of the standard deviation calculated by the Kalman filter, the measured value is deemed to be an abnormal value and removed. For example, the order of the Kalman filter is set to 2, and the threshold magnification is set to 7.

[0077] After that, smoothing is performed using a Savitzky-Golay filter (for example, order 3, window number 61). In this execution, in order to synchronize with the measurement values ​​(attitude information) from the inertial measurement unit 24, the data is delayed by 5 frames (for example, 41 (ms)) and then input to the position and attitude estimation process described above.

[0078] Similarly, noise is also contained in the detected value (distance information) by the distance detection unit 25. Therefore, outlier processing based on the measurement range is performed, and then smoothing is performed using a Savitzky-Golay filter (for example, order 3, number of windows 20) before being input to the position and orientation estimation process.

[0079] Furthermore, the attitude information from the inertial measurement unit 24 is not subjected to additional processing because the measurement values ​​are integrated and smoothed in the inertial measurement unit 24.

[0080] The estimated values ​​obtained by the position and orientation estimation process are obtained at 40 (Hz), so they are linearly interpolated to 120 (Hz) and then smoothed using a Savitzky-Golay filter.

[0081] In order to evaluate the measurement accuracy of the position and orientation estimation system 10 described above, an experiment was conducted to compare the position and orientation estimation system 10 of this embodiment (hereinafter also referred to as an example) with a comparative example.

[0082] The measurement accuracy was evaluated by measuring the position and posture of the internal access device 20 during lymph node dissection, which dissects and removes fatty tissue from the aorta, and during renal parenchymal suturing, which sutures an incision to the kidney. A cadaveric pig organ was used as the subject 60. The internal access devices 20 used included grasping forceps, scissor forceps, a clip applier, and a needle holder.

[0083] In the examples, the following equipment was used: An OptiTrack V120: Trio manufactured by NaturalPoint was used as the optical motion capture system 30; a Bosch BNO055 module, a 9-axis IMU sensor, was used as the inertial measurement unit 24; a VL53L0X ToF (Time of Flight) distance sensor was used as the distance detection unit 25; and an M5StickC Plus manufactured by M5Stack Technology was used as the microcomputer 26. As described above, the optical motion capture system 30 performed measurements at 120 Hz, and the inertial measurement unit 24 and distance detection unit 25 performed measurements at 40 Hz.

[0084] The following equipment was used in the comparative example. The comparative example does not use the inertial measurement unit 24 or the distance detection unit 25. In addition to the OptiTrack V120: Trio (optical motion capture system 30) also used in the example, the comparative example used six OptiTrack Prime 41, an optical motion capture system capable of more stable measurements.

[0085] As shown in FIG. 1, Prime41 measures from multiple cameras 80 (six in this comparison) surrounding the user of the internal access device 20, which has the advantage of making occlusion less likely to occur and enabling highly accurate measurements. On the other hand, Prime41 is difficult to use in actual operating rooms with many obstacles, and cannot replace this embodiment, which is intended for use in such environments. The measurement frequency was set to 120 (Hz), the same as V120: Trio. Note that the camera 80 is depicted with a dashed line in FIG. 1. This is to clarify that the camera 80 is a comparative example and not an embodiment.

[0086] For synchronization in the example and comparative example, an infrared LED light emitting device was used, and synchronization was performed manually after the experiment based on the timing when light emission was detected.In addition, the global coordinate systems in the example and comparative example were matched by placing the calibration jig used when setting the origin in a position where it could be seen by both systems, and defining the origin at the same time.

[0087] In this experiment, the measurement data of the comparative example (Prime41) was used as the true value, and the accuracy of the values ​​during the defect of the optical motion capture system 30 (V120: Trio) in the example was verified. The data to be verified was the data while the tip 212A of the internal-access device 20 was present inside the subject 60.

[0088] In the experiment, first, the measurement rate A is calculated, which is the ratio of the number of frames that can be measured by the optical motion capture system 30 to the total number of target frames N. trio Next, the average coordinate error Δp ​​from the measurement value of the comparative example was calculated using the following formula (1).

[0089]

number

[0090] In equation (1), Δp is IMU , Δp linear It is calculated in two sections. IMUis the average error of the interpolated value by the inertial measurement unit 24 in the example when the comparative example is taken as the true value. linear is the average error of the linearly interpolated value when the comparative example is taken as the true value. The horizontal line above Δp on the left side of equation (1) indicates the Δp and Δp in this paragraph. IMU , Δp linear It is omitted in.

[0091] The attitude angle error Δθ was calculated based on the following equation (2).

[0092]

number

[0093] In equation (2), q w (i) is the difference quaternion ΔQ(i)=Q -1 observed (i)Q ref The w element of (i). q w (i) is Δθ IMU , Δθ slerp It is calculated in two sections. IMU is the error of the interpolated value by the inertial measurement unit 24 in the embodiment when the comparative example is taken as the true value. slerp is the error of the spherical linear interpolation value when the comparative example is taken as the true value. Note that the horizontal line above Δθ on the left side of equation (2) indicates the Δθ and Δθ in this paragraph. IMU , Δθ slerp It is omitted in.

[0094] Subsequently, the interpolation errors calculated in categories (b) and (c) were subjected to a paired Wilcoxon signed rank sum test to verify whether the errors in the example were significantly smaller than those in the comparative example. Examples of the interpolation results for the tip position in the renal parenchyma suturing task are shown in Figures 9-11, and the accuracy verification results are shown in Tables 1 and 2 below.

[0095] [Table 1]

[0096] [Table 2]

[0097] Note that the number of data used for verification varies for some internal access devices 20 due to measurement failure or lack of interpolation, so the number of verification data (N) is shown next to the name of the internal access device 20 in the table (Grasping forceps, Scissors forceps, Clip applier, Needle holder).

[0098] Fig. 9 is a graph showing changes over time in the coordinate values ​​of an internal access device estimated by a position and orientation estimation system according to an embodiment of the present disclosure. Fig. 10 is a graph showing changes over time in the coordinate values ​​of an internal access device estimated by a position and orientation estimation system of a comparative example. Fig. 11 is a graph obtained by superimposing the graphs of Fig. 9 and Fig. 10.

[0099] As shown in Figures 9-11, it was confirmed that the embodiment can interpolate valid measurement values ​​even when data from the optical motion capture system 30 (MoCap) is missing. In Figures 9-11, the horizontal axis represents time, and the vertical axis represents the coordinates (X, Y, Z) of the tip 212A. In Figures 9 and 11, MoCap represents the measurement value (position information) from the optical motion capture system 30, IMU represents the value interpolated using posture information and distance information, and True Value represents the measurement data of the comparative example. Figure 10 also shows the measurement data of the comparative example.

[0100] Furthermore, it was confirmed from Tables 1 and 2 that the error of the interpolated values ​​according to the examples was smaller than the linear interpolated value, except for the clip applier. IMU is the average error of the interpolated value by the inertial measurement unit 24 in the embodiment, and Δp linear is the average error of the linearly interpolated value in the example. In Table 1, ΔpIMU is Δp linear Also, as mentioned above, in Table 2, Δθ IMU is the error of the interpolated value by the inertial measurement unit 24 in the embodiment, and Δθ slerp is the error of the spherical linear interpolation value in the example. In Table 2, Δθ IMU is Δθ slerp The reason why no difference was observed with the clip ligator is thought to be that the clip ligator was used for a short time and the interpolation time was extremely short.

[0101] The position and orientation estimation system, the position and orientation estimation method, and the program described above can achieve the following effects.

[0102] According to this system, the computer 40 acquires distance information from the distance detection unit 25 of the internal access device 20, and therefore there is no need to calculate distance by integrating twice the acceleration information included in the attitude information acquired from the inertial measurement unit 24 of the internal access device 20. As a result, the position and attitude of the internal access device 20 can be estimated with high accuracy.

[0103] According to this system, the computer 40 can estimate the position and posture of the internal-access device 20 based on the position information acquired from the optical motion capture system 30.

[0104] Furthermore, even if at least a portion of the position information is lost due to the presence of an obstacle between the optical marker 23 and the camera 32, the computer 40 can estimate the attitude of the internal access device 20 based on the attitude information acquired from the inertial measurement unit 24 of the internal access device 20. Furthermore, the computer 40 can estimate the position of the internal access device 20 based on the attitude information and distance information acquired from the distance detection unit 25 of the internal access device 20.

[0105] Due to the characteristics of the inertial measurement unit 24, there is a risk of drift occurring in the attitude information obtained by measurement by the inertial measurement unit 24. Drift is a phenomenon in which slight errors in the values ​​measured by the inertial measurement unit 24 increase over time. According to this system, if the acquired position information is not missing, the computer 40 corrects the acquired attitude information based on the acquired position information. This reduces the error in the attitude information. If at least a portion of the acquired position information is missing, the computer 40 estimates the position and attitude of the internal access device 20 using the acquired distance information and the corrected attitude information. This allows the position and attitude of the internal access device 20 to be estimated with high accuracy.

[0106] The position and orientation information obtained by measurement using the optical motion capture system 30 may contain abnormal values ​​depending on the measurement conditions. This system constantly calculates the difference between the orientation information obtained by the optical motion capture system 30 and the orientation information obtained from the inertial measurement unit 24, and by monitoring the fluctuations, it is possible to determine that any sudden changes in the difference value are abnormal. This allows the position and orientation information of the optical motion capture system 30 to be accurately corrected. Note that position and orientation information determined to be abnormal is estimated by this system in the same way as missing values.

[0107] According to this system, the position and orientation estimation system 10 can be used in laparoscopic surgery using the internal access device 20 as a surgical instrument.

[0108] According to this system, the second position can be defined by the trocar, so that the distance detection unit can easily detect the distance from the first position to the second position.

[0109] According to this method, since distance information is acquired from the distance detection unit 25 of the internal access device 20, it is not necessary to calculate distance by integrating twice the acceleration information included in the attitude information acquired from the inertial measurement unit 24 of the internal access device 20. As a result, the position and attitude of the internal access device 20 can be estimated with high accuracy.

[0110] According to this method, the position and orientation of the internal-access device 20 can be estimated based on the position information obtained from the optical motion capture system 30.

[0111] Furthermore, even if at least a portion of the position information is missing due to the presence of an obstacle between the optical marker 23 and the camera 32, the method can estimate the attitude of the internal access device 20 based on the attitude information acquired from the inertial measurement unit 24 of the internal access device 20. The method can also estimate the position of the internal access device 20 based on the attitude information and distance information acquired from the distance detection unit 25 of the internal access device 20.

[0112] By executing this program, the effects of the above-described method can be achieved.

[0113] The internal access device, the position and orientation estimation system, the position and orientation estimation method, and the program described above can be expressed as follows.

[0114] (1) An internal access device according to one aspect of the present disclosure includes: a main body having a tip portion that contacts a surface of the subject from outside the subject or that enters the subject from outside the subject via the surface of the subject, and a base portion that is located outside the subject; an optical marker mounted on the base and imaged by a camera; an inertial measurement unit mounted on the base and configured to measure information relating to the attitude of the main body; and a distance detection unit mounted on the base or the surface of the subject, which detects the distance from a first position, which is the position of the distance detection unit, to a second position, which is a position outside the subject and whose relative distance to the first position changes depending on the position of the tip.

[0115] (2) In the internal access device of (1), The body may extend axially; When the tip portion penetrates the surface of the subject, the main body may be capable of three types of movement: sliding movement along the axial direction, rotational movement in a circumferential direction around the axis of the main body, and rotational movement in which the main body rotates around a part of the main body located on the surface of the subject.

[0116] (3) In the internal access device of (2), The main body may be a surgical instrument used in laparoscopic surgery.

[0117] (4) In any one of the internal access devices (1) to (3), The distance detection unit may be a distance measurement sensor.

[0118] (5) In any one of the internal access devices (1) to (4), The main body may further include a storage portion having an internal space, The housing may house the inertial measurement unit and the distance detection unit, and may have the optical marker attached thereto.

[0119] (6) A position and orientation estimation system according to one aspect of the present disclosure includes: Any one of the internal access devices (1) to (5) above; an optical motion capture system including the camera and the optical marker, the camera capturing an image of the optical marker to obtain position information of the optical marker; a computer that acquires position information from the optical motion capture system, acquires attitude information of the main body from the inertial measurement unit, and acquires distance information from the first position to the second position from the distance detection unit, The computer If the acquired position information is not missing, estimate the position and attitude of the internal access device based on the acquired position information; When at least a portion of the acquired position information is missing, the position and orientation of the internal access device are estimated based on the last position information before the missing information, the acquired orientation information, and the acquired distance information.

[0120] (7) In the position and orientation estimation system of (6), The computer If the acquired position information is not missing, the acquired orientation information may be corrected based on the acquired position information; If at least a portion of the acquired position information is missing, the missing position information may be interpolated using the acquired distance information and corrected attitude information.

[0121] (8) In the position and orientation estimation system of (6) or (7), The computer may calculate a difference between posture information calculated from position information acquired from the optical motion capture system and posture information acquired from the inertial measurement unit, and determine whether the posture information in the optical motion capture system is an abnormal value based on the variation.

[0122] (9) In any one of the position and orientation estimation systems (6) to (8), The internal access device may be a surgical instrument used in laparoscopic surgery; The tip of the body of the internal access device may penetrate the surface of the subject and enter the interior of the subject.

[0123] (10) In any one of the position and orientation estimation systems (6) to (9), The position and orientation estimation system may further include a trocar; The second position may be the position of the trocar.

[0124] (11) A position and orientation estimation method according to one aspect of the present disclosure includes: 1. A position and orientation estimation method for estimating a position and orientation of an internal access device, the method comprising: a main body having a tip portion that contacts a surface of a subject from outside the subject or that enters the inside of the subject from outside the subject via the surface of the subject, and a base portion located outside the subject; an optical marker mounted on the base and imaged by a camera; an inertial measurement unit mounted on the base and measuring information related to the orientation of the main body; and a distance detection unit mounted on the base or the surface of the subject, the distance detection unit detecting a distance from a first position, which is the position of the distance detection unit, to a second position, which is a position outside the subject and whose relative distance to the first position changes depending on the position of the tip portion, acquiring position information from an optical motion capture system that includes the camera and the optical marker, and obtains position information of the optical marker by capturing an image of the optical marker with the camera; acquiring attitude information of the main body from the inertial measurement unit; acquiring distance information from the distance detection unit to the second position; If the acquired position information is not missing, estimate the position and attitude of the internal access device based on the acquired position information; When at least a portion of the acquired position information is missing, the position and orientation of the internal access device are estimated based on the last position information before the missing information, the acquired orientation information, and the acquired distance information.

[0125] (12) A program according to one embodiment of the present disclosure, (11) is a program for causing a computer to execute the position and orientation estimation method.

[0126] Any of the various embodiments described above may be combined appropriately to achieve the effects of each embodiment.

[0127] While the present invention has been fully described in connection with preferred embodiments, with appropriate reference to the drawings, various changes and modifications will become apparent to those skilled in the art, and it is to be understood that such changes and modifications are included within the scope of the present invention as defined by the appended claims unless they depart therefrom. [Explanation of symbols]

[0128] 10 Position and Pose Estimation System 20 Internal access devices 21 Main Unit 211 Base 212A Tip 22 Storage section 22A Internal space 23 Optical Marker 24 Inertial Measurement Unit 25 Distance detection unit 30 Optical motion capture system 40 Computer 50 Trocar 60 Subjects 60A Surface 101 Axial direction

Claims

1. a main body having a tip portion that contacts a surface of the subject from outside the subject or that enters the subject from outside the subject via the surface of the subject, and a base portion that is located outside the subject; an optical marker mounted on the base and imaged by a camera; an inertial measurement unit mounted on the base and configured to measure information relating to the attitude of the main body; an internal access device comprising: a distance detection unit mounted on the base or the surface of the subject, the distance detection unit detecting a distance from a first position, which is the position of the distance detection unit, to a second position, which is a position outside the subject and whose relative distance to the first position changes depending on the position of the tip.

2. The body extends axially, The internal access device of claim 1, wherein the main body is capable of three types of movement when the tip portion penetrates the surface of the subject: sliding movement along the axial direction, rotational movement in a circumferential direction around the axis of the main body, and rotational movement in which the main body rotates around a portion of the main body located on the surface of the subject.

3. The internal access device of claim 2 , wherein the body is a surgical instrument used in laparoscopic surgery.

4. The internal access device according to claim 1 , wherein the distance detection unit is a distance measurement sensor.

5. The main body further includes a storage portion having an internal space, The internal access device according to claim 1 , wherein the inertial measurement unit and the distance detection unit are housed in the housing portion, and the optical marker is attached to the housing portion.

6. An internal access device according to any one of claims 1 to 5; an optical motion capture system including the camera and the optical marker, the camera capturing an image of the optical marker to obtain position information of the optical marker; a computer that acquires position information from the optical motion capture system, acquires attitude information of the main body from the inertial measurement unit, and acquires distance information from the first position to the second position from the distance detection unit, The computer If the acquired position information is not missing, estimate the position and attitude of the internal access device based on the acquired position information; A position and attitude estimation system that, when at least a portion of the acquired position information is missing, estimates the position and attitude of the internal access device based on the last position information before the missing information, the acquired attitude information, and the acquired distance information.

7. The computer If the acquired position information is not missing, correct the acquired attitude information based on the acquired position information; The position and orientation estimation system according to claim 6 , wherein, when at least a portion of the acquired position information is missing, the missing position information is interpolated using the acquired distance information and the corrected orientation information.

8. The computer 7. The position and attitude estimation system according to claim 6, wherein a difference value is calculated between attitude information calculated from position information acquired from the optical motion capture system and attitude information acquired from the inertial measurement unit, and based on the variation, it is determined whether the attitude information acquired by the optical motion capture system is an abnormal value.

9. the internal access device is a surgical instrument used in laparoscopic surgery; The position and orientation estimation system according to claim 6 , wherein the tip of the main body of the internal access device penetrates the surface of the subject and enters the inside of the subject.

10. Further equipped with a trocar, The position and orientation estimation system according to claim 6 , wherein the second position is the position of the trocar.

11. 1. A position and orientation estimation method for estimating a position and orientation of an internal access device, the method comprising: a main body having a tip portion that contacts a surface of a subject from outside the subject or that enters the inside of the subject from outside the subject via the surface of the subject, and a base portion located outside the subject; an optical marker mounted on the base and imaged by a camera; an inertial measurement unit mounted on the base and measuring information related to the orientation of the main body; and a distance detection unit mounted on the base or the surface of the subject, the distance detection unit detecting a distance from a first position, which is the position of the distance detection unit, to a second position, which is a position outside the subject and whose relative distance to the first position changes depending on the position of the tip portion, acquiring position information from an optical motion capture system that includes the camera and the optical marker, and obtains position information of the optical marker by capturing an image of the optical marker with the camera; acquiring attitude information from the inertial measurement unit; acquiring distance information from the distance detection unit to the second position; If the acquired position information is not missing, estimate the position and attitude of the internal access device based on the acquired position information; A position and attitude estimation method that, when at least a portion of the acquired position information is missing, estimates the position and attitude of the internal access device based on the last position information before the missing information, the acquired attitude information, and the acquired distance information.

12. A program for causing a computer to execute the position and orientation estimation method according to claim 11.

Images