Own-position estimating device and own-position estimating method

Abstract

Provided is an own-position estimating device and an own-position estimating method capable of estimating the own-position of a probe head with a high degree of precision, while facilitating association of a target in a three-dimensional space with a target in an image. An own-position estimating device (50) comprises: a relative position information detecting unit (target group position information detecting unit (56) and probe head position information detecting unit (58)) that detects relative position information indicating the relative positional relationship between a probe head (12) and a target group (14) including a plurality of targets (24); an image acquiring unit (60) that acquires an image of the target group (14) captured by a camera (20) mounted on the probe head (12); and a high-precision own-position estimating unit (62) that uses the relative position information and the image to detect the position and orientation of the probe head (12) with respect to the target group (14) with a higher degree of precision than the relative position information.

Description

Self-location estimation device and self-location estimation method

[0001] The present invention relates to a self-position estimation device and a self-position estimation method that are capable of estimating the self-position of a probe head.

[0002] Conventionally, portable three-dimensional coordinate measuring machines (CMMs) have been mounted on robots to automatically measure large workpieces. Portable three-dimensional coordinate measuring machines are gradually meeting the requirements for accuracy of several tens of micrometers, but currently bridge-type three-dimensional coordinate measuring machines are used for high accuracy requirements of 10 micrometers or less.

[0003] Furthermore, measurement methods using laser trackers or markers are known as technologies for achieving accuracy of the order of several tens of micrometers (see, for example, Patent Document 1). These methods have a base station equipped with a tilt function for adjusting the elevation and azimuth angles, and further equipped with a distance sensor or camera to calculate the distance and attitude of the measurement head. With this measurement method, the movable range of the tilt mechanism and the measurement range of the distance sensor are long, making it possible to measure over a wide range.

[0004] Meanwhile, a method is known in which a target whose position in three-dimensional space is known is detected from an image captured by a camera (see, for example, Patent Document 2). This method has the advantage of easily achieving high accuracy (5 μm or less) with a relatively simple mechanism by performing calculations by relating the position of each target in three-dimensional space to the position of each target in the captured image.

[0005] JP 2020-148515 A JP 2022-30807 A

[0006] However, in measurement methods using laser trackers or markers, the measurement accuracy tends to deteriorate as the distance between the base station and the measurement head increases, mainly due to limitations in the angular accuracy of the oscillating mechanism.

[0007] In addition, it is difficult to obtain a wide measurement range with the method of detecting the self-position from an image of the target captured with a camera.Furthermore, since the position of each target in three-dimensional space must be linked to the position of each target in the captured image, there is a disadvantage that it takes time to search for the correspondence.

[0008] The present invention has been made in consideration of these circumstances, and aims to provide a self-position estimation device and a self-position estimation method that can estimate the self-position of a probe head with high accuracy while facilitating the association of a target in three-dimensional space with a target on an image.

[0009] In order to achieve the above object, the present invention comprises the following aspects.

[0010] The self-position estimation device according to the first aspect includes a relative position information detection unit that detects relative position information indicating the relative positional relationship between a target group having multiple targets and a probe head, an image acquisition unit that acquires an image of the target group taken by a camera mounted on the probe head, and a high-precision self-position estimation unit that detects the position and attitude of the probe head relative to the target group based on the relative position information and the image.

[0011] In the self-location estimation device according to the second aspect, in the first aspect, the high-precision self-location estimation unit has a feature point detection unit that detects feature points that indicate the position of each target on an image, a combination extraction unit that extracts combinations of feature points and object points so that an error function that indicates the correspondence between each feature point detected by the feature point detection unit and an object point that indicates the position of each target in three-dimensional space is minimized, and an optimization calculation unit that determines the position and orientation of the probe head relative to the target group by optimization calculation based on the combinations of feature points and object points extracted by the combination extraction unit.

[0012] The self-localization estimation device according to the third aspect is the self-localization estimation device according to the second aspect, wherein the transformation matrix from the world coordinate system to the target group coordinate system is [R|t] target,la The transformation matrix from the world coordinate system to the head coordinate system is [R|t] head,la The coordinates of the feature points on the image are (u i ,vi ), and the coordinates of the object point are (x pj ,y pj ,z pj ) and the error function is E(i,j) (where i and j are variables), the error function E(i,j) is expressed by the following equation.

[0013] A self-location estimation device according to a fourth aspect is any one of the first to third aspects, wherein the relative position information is information detected based on a measurement result of a measurement device separate from the probe head.

[0014] A self-localization estimation device according to a fifth aspect is the fourth aspect, wherein the measurement device is a laser tracker.

[0015] A self-location estimation device according to a sixth aspect is any one of the first to third aspects, wherein the probe head is equipped with a SLAM function unit, and the relative position information is information detected based on measurement results of the SLAM function unit.

[0016] The self-position estimation method according to the seventh aspect includes a relative position information detection step of detecting relative position information indicating the relative positional relationship between a target group having a plurality of targets and a probe head, an image acquisition step of acquiring an image of the target group taken by a camera mounted on the probe head, and a high-precision self-position estimation step of detecting the position and attitude of the probe head relative to the target group based on the relative position information and the image.

[0017] According to the present invention, it is possible to easily associate a target in three-dimensional space with a target on an image, and to estimate the self-position of a probe head with high accuracy.

[0018] FIG. 1 is a block diagram showing a self-location estimation system according to the present embodiment; FIG. 2 is a schematic configuration diagram showing a schematic configuration of the self-location estimation system according to the present embodiment; FIG. 3 is a schematic plan view showing an example of a target group; FIG. 4 is a flowchart showing an example of a self-location estimation process executed by the self-location estimation device of the present embodiment; FIG. 5 is a diagram showing the relationship between measurement regions of a laser tracker and a probe head; FIG. 6 is a diagram showing a coordinate system model showing the relationship between a world coordinate system, a target group coordinate system, and a head coordinate system; and FIG. 7 is a block diagram showing a self-location estimation system according to another embodiment.

[0019] Hereinafter, an embodiment of the present invention will be described with reference to the accompanying drawings.

[0020] [Self-Location Estimation System] Fig. 1 is a block diagram showing a self-location estimation system 10 according to this embodiment. Fig. 2 is a schematic diagram showing the schematic configuration of the self-location estimation system 10 according to this embodiment.

[0021] 1 and 2 , the self-localization system 10 includes a probe head 12, a laser tracker 16, a group of targets 14, and a self-localization device 50. The self-localization device 50 is an example of a self-localization device of the present invention. In this embodiment, the self-localization device 50 is configured separately from the probe head 12, but the probe head 12 may include at least some of the functions of the self-localization device 50.

[0022] The probe head 12 is a portable three-dimensional coordinate measuring machine that measures the three-dimensional coordinates of a workpiece (not shown). The probe head 12 may be a contact type (touch probe type) or a non-contact type (laser type, optical type) as long as it is capable of measuring the three-dimensional coordinates of the workpiece. Examples of non-contact types include a laser scanner, a point laser, and a line laser. A user can hold the probe head 12 and perform measurements to obtain the three-dimensional coordinates of the measurement point on the workpiece.

[0023] The probe head 12 has a self-position estimation function, and the camera 20 mounted on the probe head 12 photographs the target group 14, allowing the self-position (position and attitude) of the probe head 12 to be estimated by the self-position estimation device 50 described below.

[0024] Fig. 3 is a schematic plan view showing an example of the target group 14. As shown in Fig. 3, the target group 14 is formed by two-dimensionally arranging a plurality (a large number) of targets 24 on a flat target member 22. Each target 24 is formed in a dot or point shape and is arranged with a gap between each target 24. Furthermore, each target 24 may be formed of a small point-like light source (point light source) such as an LED.

[0025] The target group 14 configured in this manner is supported by a target group support member 26 installed on the floor surface, as shown in Fig. 2 . The target group support member 26 is configured, for example, by a tripod. Note that Fig. 2 shows, as an example, a configuration in which two target groups 14 are supported by the target group support member 26 made of a tripod, but the number of target groups 14 is not limited to two and may be one, or three or more. Furthermore, the target group support member 26 may be configured as something other than a tripod. For example, the target group 14 may be fixed to a wall member or the like.

[0026] 2, the laser tracker 16 is supported on a tracker support member 28 that is installed on the floor. The tracker support member 28 is configured by, for example, a tripod.

[0027] The laser tracker 16 is configured to irradiate a reflector placed on the measurement object (the target group 14 and the probe head 12) with laser light, receive the laser light reflected by the reflector, and measure the time from irradiation to reception, thereby measuring the distance from the laser tracker 16 to the reflector. The laser tracker 16 is also configured to be rotatable about a vertical axis and a horizontal axis, and is capable of controlling the direction of the irradiated laser light with a two-axis motor to follow the reflector, and measuring the direction of the reflector using a two-axis encoder attached to the motor. The laser tracker 16 is equipped with a camera, and is capable of automatically tracking a moving reflector within the camera's field of view and automatically irradiating the reflector with laser light.

[0028] The reflector is constructed using a reflecting mirror (not shown) that uses three mirrors that are perpendicular to each other, and reflects light that enters through an opening (not shown) in the direction of incidence regardless of the angle of incidence.

[0029] In this way, the laser tracker 16 can measure the three-dimensional position of the object to be measured to which the reflector is attached by measuring the distance to the reflector and the direction of the reflector. The laser tracker 16 is connected to the self-location estimation device 50 via a cable or via a wired or wireless network, and the measurement results by the laser tracker 16 are input to the self-location estimation device 50.

[0030] In this embodiment, the target group 14 and the probe head 12 are each provided with a plurality of reflectors in order to measure the positions and orientations of the target group 14 and the probe head 12 using the laser tracker 16. The memory unit 54 of the self-position estimation device 50 stores in advance the relationship between the shapes of the target group 14 and the probe head 12 and the attachment positions of the plurality of reflectors. This allows the self-position estimation device 50 (a calculation processing unit 52 described later) to detect the positions and orientations of the target group 14 and the probe head 12 from the measurement results of the laser tracker 16.

[0031] [Self-Location Estimation Device] Next, the self-location estimation device 50 will be described. As shown in Fig. 1 , the self-location estimation device 50 is configured by, for example, a personal computer, and includes an arithmetic processing unit 52 and a storage unit 54. The camera 20 mounted on the probe head 12 and the laser tracker 16 are also connected to the self-location estimation device 50. The method of connection between these is not particularly limited, and they may be connected via a cable, or via a wired or wireless network.

[0032] The storage unit 54 stores control programs and various data. The storage unit 54 is configured, for example, by a hard disk drive (HDD) or a semiconductor storage device (SSD: Solid State Drive). The storage unit 54 may include a temporary storage element configured, for example, by a random access memory (RAM) such as a dynamic random access memory (DRAM) or a static random access memory (SRAM), and may function as a work area for the arithmetic processing unit 52.

[0033] The storage unit 54 stores target images, target group position information, probe head position information, etc., which will be described later. The storage unit 54 also stores information (target information) regarding the shape, size, and arrangement of each target 24 constituting the target group 14.

[0034] The arithmetic processing unit 52 executes various arithmetic processes performed by the self-position estimation device 50. The arithmetic processing unit 52 includes an arithmetic circuit configured with various processors, memories, etc. The various processors include a central processing unit (CPU), a graphics processing unit (GPU), an application specific integrated circuit (ASIC), and a programmable logic device (e.g., a simple programmable logic device (SPLD), a complex programmable logic device (CPLD), and a field programmable gate array (FPGA)). Note that the various functions of the arithmetic processing unit 52 may be implemented by a single processor or by multiple processors of the same or different types.

[0035] The calculation processing unit 52 reads out and executes the control program stored in the memory unit 54, thereby functioning as a target group position information detection unit 56, a probe head position information detection unit 58, an image acquisition unit 60, a high-precision self-position estimation unit 62, and a determination processing unit 70. The high-precision self-position estimation unit 62 includes a feature point detection unit 64, a combination extraction unit 66, and an optimization calculation unit 68.

[0036] The target group position information detection unit 56 and the probe head position information detection unit 58 are examples of a relative position information detection unit of the present invention. The image acquisition unit 60 is an example of an image acquisition unit of the present invention. The high-accuracy self-position estimation unit 62 is an example of a high-accuracy self-position estimation unit of the present invention. Furthermore, the feature point detection unit 64, the combination extraction unit 66, and the optimization calculation unit 68 are examples of a feature point detection unit, a combination extraction unit, and an optimization calculation unit of the present invention, respectively.

[0037] [Self-Location Estimation Method] The following describes the processing procedure (an example of a self-location estimation method) of the self-location estimation process executed by the self-location estimation device 50 of this embodiment. Fig. 4 is a flowchart showing an example of the self-location estimation process executed by the self-location estimation device 50 (arithmetic processing unit 52) ​​of this embodiment. Fig. 5 is a diagram showing the relationship between the measurement areas of the laser tracker 16 and the probe head 12. Note that at the start of this flowchart, it is assumed that the camera 20 has already been calibrated, and the camera matrix K including the internal parameters (focal length, optical center) and distortion parameters (distortion coefficients) of the camera 20 is known.

[0038] First, the laser tracker 16 and the target group 14 are installed (step S10). At this time, as shown in Fig. 5 , the laser tracker 16 is installed at a position separated from the target group 14 and the probe head 12 so that the measurement region (low-accuracy measurement region) R1 of the laser tracker 16 encompasses the measurement region (high-accuracy measurement region) R2 of the probe head 12. Note that the measurement region R1 of the laser tracker 16 is wider than the measurement region R2 of the probe head 12, and the laser tracker 16 can measure a wider range than the probe head 12.

[0039] Next, the position and orientation of the target group 14 are detected using a low-precision measurement method (step S12). Specifically, the laser tracker 16 measures the distance to and the direction of the reflectors attached to the target group 14, and transmits measurement data indicating the measurement results to the self-position estimation device 50. The measurement data transmitted to the self-position estimation device 50 is input to the target group position information detection unit 56.

[0040] The target group position information detection unit 56 acquires the measurement data transmitted from the laser tracker 16. Then, based on the acquired measurement data, the target group position information detection unit 56 detects information indicating the position and attitude of the target group 14 (hereinafter referred to as "target group position information"). The target group position information detection unit 56 stores the detected target group position information in the memory unit 54.

[0041] Next, the position and orientation of the probe head 12 are detected using a low-precision measurement method (step S14). Specifically, the laser tracker 16 measures the distance to the reflector attached to the probe head 12 and the direction of the reflector, and transmits measurement data indicating the measurement results to the self-position estimation device 50. The measurement data transmitted to the self-position estimation device 50 is input to the probe head position information detection unit 58.

[0042] The probe head position information detection unit 58 acquires the measurement data transmitted from the laser tracker 16. Then, based on the acquired measurement data, the probe head position information detection unit 58 detects information indicating the position and attitude of the probe head 12 (hereinafter referred to as "probe head position information"). The probe head position information detection unit 58 stores the detected probe head position information in the storage unit 54.

[0043] Here, the target group position information and the probe head position information will be explained. world and the target group coordinate system Σ target and the head coordinate system Σ head 6 is a diagram showing a coordinate system model showing the relationship between the world coordinate system Σ world is a coordinate system based on the laser tracker 16. The head coordinate system Σ head is a coordinate system based on the probe head 12. The target group coordinate system Σ target is a coordinate system based on the target group 14.

[0044] In this embodiment, the target group position information detected by the target group position information detection unit 56 is expressed as a transformation matrix [R|t] target,la This transformation matrix [R|t] can be defined as target,la is the world coordinate system Σ world and indicates the position and orientation of the target group 14 in the world coordinate system Σ world From the target group coordinate system Σ target where R denotes a rotation matrix and t denotes a translation vector (same below).

[0045] In this embodiment, the probe head position information detected by the probe head position information detection unit 58 is expressed as a transformation matrix [R|t] head,la This transformation matrix [R|t] can be defined as head,la is the world coordinate system Σ world and indicates the position and orientation of the probe head 12 in the world coordinate system Σ world From the head coordinate system Σ head is shown as a transformation matrix for transforming coordinates to

[0046] The target group position information and probe head position information detected as described above correspond to relative position information that indicates the relative positional relationship in three-dimensional space between the target group 14 and the probe head 12. This relative position information is found based on the measurement results of the laser tracker 16, and is less accurate than information indicating the self-position of the probe head 12 (information indicating the position and attitude of the probe head 12 with respect to the target group 14) estimated by the high-precision self-position estimation unit 62 described below.

[0047] Next, the target group 14 is photographed by the camera 20 mounted on the probe head 12 (step S16). The image (target image) photographed by the camera 20 is transmitted to the self-position estimation device 50. When the target image is transmitted to the self-position estimation device 50, the image acquisition unit 60 acquires the target image and stores it in the storage unit 54.

[0048] Next, the high-precision self-position estimation unit 62 performs processing to estimate the position and attitude of the probe head 12 with high precision based on the above-mentioned relative position information (target group position information and probe head position information) and the target image captured by the camera 20.

[0049] Specifically, the feature point detection unit 64 reads the target image from the storage unit 54. Then, the feature point detection unit 64 performs predetermined image processing (grayscale conversion, etc.) on the read target image, and then detects feature points (image points) indicating the position of each target 24 from the target image, and determines the coordinates (pixel coordinates) of each feature point on the target image (image plane M; see FIG. 6) (step S18). Note that the position of the center of gravity of the target 24 is detected as the feature point on the target image. The feature points on the target image detected at this time are referred to as U. i and its coordinates (two-dimensional coordinates) are (u i ,v i ) (where i is a variable and is a natural number equal to or greater than 2). If necessary, each feature point U i The coordinates (u i ,v i ) lens distortion correction is applied.

[0050] The object point P in the three-dimensional space corresponding to each feature point j The coordinates indicating the position of each target 24 in the target group 14 are (x pj ,y pj ,z pj ) (where j is a variable and is a natural number equal to or greater than 2), the following formula (1) can be obtained from the coordinate system model shown in FIG.

[0051] In equation (1), K is a camera matrix indicating the internal parameters of the camera 20, and is known.

[0052] Here, the error function E(i, j) is defined as the following equation (2).

[0053] The error function E(i, j) defined by equation (2) is the error function E(i, j) of the feature point U on the target image. i and an object point P in three-dimensional space j The error function E(i, j) is an error function that indicates the correspondence between the points (i, j) and the target 24 (points indicating the position of each target 24), and the error function E(i, j) reaches its minimum value when the points are in a projection relationship with each other.

[0054] Therefore, the combination extraction unit 66 extracts the feature points U i and an object point P in three-dimensional space j Among the combinations of i ,P j} is extracted, where N is a natural number of 2 or more.

[0055] Next, the optimization calculation unit 68 finds the N combinations {U i ,P j} to obtain the transformation matrix [R|t] of the following equation (3): head,ha is calculated by nonlinear optimization.

[0056] The transformation matrix [R|t] obtained in the optimization calculation unit 68 head,ha indicates the position and orientation of the probe head 12 relative to the target group 14, and is more accurate than the relative position information described above.

[0057] In this way, the high-accuracy self-position estimation unit 62 selects N combinations {U i ,P j This makes it easy to associate each feature point on the target image with each object point in three-dimensional space, and reduces the search time required for these associations.

[0058] The high-precision self-position estimation unit 62 stores information indicating the position and attitude of the probe head 12 (high-precision probe head self-position information) calculated by the optimization calculation unit 68 in the storage unit 54. The high-precision probe head self-position information stored in the storage unit 54 is used when calculating the three-dimensional coordinates of the workpiece using the probe head 12.

[0059] Next, it is determined whether or not to repeat the self-position estimation process of the probe head 12 (step S22). Specifically, while the probe head 12 is measuring the three-dimensional coordinates of the workpiece, the determination processing unit 70 determines whether or not to continue the self-position estimation process of the probe head 12 (YES in step S22), and repeats the processes from step S14 to step S22. As a result, the self-position estimation process of the probe head 12 is continuously and repeatedly performed while the probe head 12 is measuring the three-dimensional coordinates of the workpiece.

[0060] On the other hand, when the measurement of the three-dimensional coordinates of the workpiece by the probe head 12 is completed, the determination processing unit 70 determines whether the self-position estimation process of the probe head 12 is completed (NO in step S22), and ends this flowchart.

[0061] [Effects] Next, the effects of this embodiment will be described.

[0062] According to this embodiment, the relative position information (target group position information and probe head position information) obtained by wide-area measurement using a low-precision method is used to detect the self-position (position and attitude) of the probe head 12 based on the target image. Therefore, the target 24 (object point P j ) and the target 24 (feature point U i ) and the self-position of the probe head 12 can be estimated with high accuracy.

[0063] In this embodiment, a case where a distance measuring device using a laser (laser tracker 16) is applied has been described as an example of a low-precision measuring device, but other configurations can also be applied as long as they can detect the position and orientation of the object to be measured (probe head 12 and target group 14).

[0064] [Other Configuration Examples] As another example of a low-accuracy measurement device, for example, a distance measurement device using a camera may be used. In this case, the position and orientation of the measurement object may be detected by recognizing the three-dimensional position of each marker (e.g., ArUco) placed on the measurement object from an image captured by the camera. Furthermore, a distance measurement device using infrared rays or ultrasonic waves may be used.

[0065] The probe head 12 may also have a SLAM (Simultaneous Localization and Mapping) function.

[0066] FIG. 7 is a block diagram showing a self-localization system 10A according to another embodiment. As shown in FIG. 7 , in the self-localization system 10A according to another embodiment, the probe head 12 includes a SLAM function unit 30. In this case, the SLAM function unit 30 includes a sensor (not shown) such as a Lidar (Lithium Detection And Ranging; laser scanner), a camera, or a ToF (Time of Flight) sensor to realize the SLAM function. The sensor measures distance information to various objects (including the target group 14) in the surrounding environment. This allows the target group position information detection unit 56 and the probe head position information detection unit 58 to detect target group position information and probe head position information based on the measurement data obtained by the SLAM function unit 30. The self-localization system 10A configured in this manner eliminates the need for a distance measurement device such as a laser tracker 16, thereby improving user convenience.

[0067] Furthermore, the low-precision measuring device is not limited to a non-contact type that performs measurements without contacting the object to be measured, such as the laser tracker 16, but may also be a contact type that performs measurements by contacting the object to be measured.

[0068] Furthermore, the probe head 12 is not limited to being held by a user to measure a workpiece. For example, the probe head 12 may be attached to the tip (end effector) of an articulated robot arm, and moved by controlling the motion of the robot arm, thereby measuring the three-dimensional coordinates of the workpiece. In this case, the position and orientation of the probe head 12 may be detected based on the tool coordinate system of the robot arm, instead of the laser tracker 16 or the like described above. The tool coordinate system is a coordinate system set with the tip of the robot arm as the reference, and is determined based on the encoder values ​​of encoders attached to each joint of the robot arm.

[0069] Furthermore, in the present embodiment, the case where the target group 14 is configured with a dot pattern in which a plurality of targets 24 formed in a dot or point shape are arranged two-dimensionally has been shown as an example, but the present invention is not limited to this, and the target group 14 may be configured with, for example, a grid pattern or a checkered pattern as disclosed in the above-mentioned Patent Document 2. Furthermore, the target group 14 may be various two-dimensional patterns including an AR marker (Augmented Reality Marker) or a QR code (Quick Response code, registered trademark), etc.

[0070] Although the embodiments of the present invention have been described above, the present invention is not limited to the above examples, and various improvements and modifications may be made without departing from the spirit of the present invention.

[0071] 10... Self-position estimation system, 10A... Self-position estimation system, 12... Probe head, 14... Target group, 16... Laser tracker, 20... Camera, 22... Target member, 24... Target, 26... Target group support member, 28... Tracker support member, 30... SLAM function unit, 50... Self-position estimation device, 52... Calculation processing unit, 54... Memory unit, 56... Target group position information detection unit, 58... Probe head position information detection unit, 60... Image acquisition unit, 62... High-precision self-position estimation unit, 64... Feature point detection unit, 66... ​​Combination extraction unit, 68... Optimization calculation unit, 70... Determination processing unit

Claims

1. A self-position estimation device comprising: a relative position information detection unit that detects relative position information that indicates the relative positional relationship between a target group having multiple targets and a probe head; an image acquisition unit that acquires images of the target group taken by a camera mounted on the probe head; and a high-precision self-position estimation unit that detects the position and attitude of the probe head relative to the target group based on the relative position information and the images.

2. The self-position estimation device according to claim 1, wherein the high-precision self-position estimation unit comprises: a feature point detection unit that detects feature points indicating the position of each of the targets on the image; a combination extraction unit that extracts combinations of the feature points and the object points so that an error function indicating the correspondence between each of the feature points detected by the feature point detection unit and the object points indicating the position of each of the targets in three-dimensional space is minimized; and an optimization calculation unit that determines the position and orientation of the probe head relative to the group of targets by optimization calculation based on the combinations of the feature points and the object points extracted by the combination extraction unit.

3. The transformation matrix from the world coordinate system to the target group coordinate system is [R|t] target,la and the transformation matrix from the world coordinate system to the head coordinate system is [R|t] head,la and the coordinates of the feature points on the image are (u i ,v i ), and the coordinates of the object point are (x pj ,y pj ,z pj ) and the error function is E(i,j) (where i and j are variables), the error function E(i,j) is expressed by the following equation:

4. A self-position estimation device according to any one of claims 1 to 3, wherein the relative position information is information detected based on measurement results of a measurement device separate from the probe head.

5. The self-location estimation device according to claim 4, wherein the measurement device is a laser tracker.

6. A self-location estimation device according to any one of claims 1 to 3, wherein the probe head is equipped with a SLAM function unit, and the relative position information is information detected based on measurement results of the SLAM function unit.

7. A self-position estimation method comprising: a relative position information detection step of detecting relative position information indicating the relative positional relationship between a target group having a plurality of targets and a probe head; an image acquisition step of acquiring an image of the target group taken by a camera mounted on the probe head; and a high-precision self-position estimation step of detecting the position and attitude of the probe head relative to the target group based on the relative position information and the image.

Images