Self-position estimation device and self-position estimation method

Abstract

To provide a self-position estimation device and a self-position estimation method that can accurately estimate the self-position of a probe head, while facilitating association of targets in a three-dimensional space with targets on an image.SOLUTION: A self-position estimation device 50 comprises: a relative position information detection unit (target group position information detection unit 56 and probe head position information detection unit 58) that detects relative position information indicating the relative positional relationship between a target group 14 having a plurality of targets 24 and a probe head 12; an image acquisition unit 60 that acquires an image obtained by a camera 20 mounted on the probe head 12 photographing the target group 14; and a high-accuracy self-position estimation unit 62 that detects, by using the relative position information and the image, the position and posture of the probe head 12 relative to the target group 14 with higher accuracy than the relative position information.SELECTED DRAWING: Figure 1

Description

[Technical Field]

[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. [Background technology]

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

[0003] Furthermore, measurement methods using laser trackers and markers are known as technologies for achieving accuracy of the order of several tens of micrometers (see, for example, Patent Document 1). These have a base station equipped with a tilt function for adjusting elevation and azimuth angles, and are 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. [Prior art documents] [Patent documents]

[0005] [Patent Document 1] Japanese Patent Publication No. 2020-148515 [Patent Document 2] Japanese Patent Publication No. 2022-30807 Summary of the Invention [Problem to be solved by the invention]

[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. [Means for solving the problem]

[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 a plurality of targets and a probe head, an image acquisition unit that acquires an image of the target group captured 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 includes 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] A self-localization estimation device according to a 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 ,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.

number

[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-position estimation device according to a sixth aspect is any one of the first to third aspects, in which the probe head is equipped with a SLAM function unit, and the relative position information is information detected based on the 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. [Effects of the Invention]

[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. [Brief explanation of the drawings]

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

[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 also 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 of 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 holds the probe head 12 and performs 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 when the camera 20 mounted on the probe head 12 photographs the target group 14, the self-position (position and attitude) of the probe head 12 can 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, 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 something other than a tripod. For example, the target group 14 may be fixed to a wall member or the like.

[0026] 2, laser tracker 16 is supported on a tracker support member 28 that is installed on the floor. Tracker support member 28 is configured, for example, by 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 the direction of the irradiated laser light is controlled by a two-axis motor to follow the reflector, and the direction of the reflector can be measured using a two-axis encoder attached to the motor. The laser tracker 16 is equipped with a camera, and can automatically track a moving reflector within the camera's field of view and automatically irradiate the reflector with laser light.

[0028] The reflector is made up of three mirrors (not shown) that are arranged at right angles to each other, and reflects light incident from an opening (not shown) in the same direction as the incident light, 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-localization 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-localization 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-position 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 self-location estimation device 50 is also connected to the camera 20 mounted on the probe head 12 and the laser tracker 16. There are no particular limitations on the method of connection, and these 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: Hard Disk Drive) 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 processing operations 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., simple programmable logic device (SPLD), complex programmable logic device (CPLD), and 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 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 judgment 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-position 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 the reflector attached to the target group 14 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 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 Σ headIn FIG. 6, 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 is the rotation matrix and t is the 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 estimated by the high-precision self-position estimation unit 62 described later (information indicating the position and attitude of the probe head 12 with respect to the target group 14).

[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 memory unit 54.

[0048] Next, the high-accuracy self-position estimation unit 62 performs processing to estimate the position and attitude of the probe head 12 with high accuracy based on the above 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 calculates 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 coordinates (u i ,v i ) lens distortion correction is applied.

[0050] Each feature point corresponds to an object point P in three-dimensional space. 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.

number

[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).

number

[0053] The error function E(i,j) defined in equation (2) is the error function of the feature point U i and an object point P in three-dimensional space j (points indicating the position of each target 24), and the error function E(i,j) reaches its minimum value when the combination of these points is such that they are in a projective 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 From the combinations of , we select N combinations {U i ,P j} where N is a natural number greater than or equal to 2.

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

number

[0056] The transformation matrix [R|t] obtained in the optimization calculation unit 68 head,haindicates 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 posture 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 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] 〔effect〕 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 posture) of the probe head 12 based on the target image. Therefore, the target 24 (object point P j ) and target 24 (feature point U i ), and it is possible to estimate the self-position of the probe head 12 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] Another example of a low-accuracy measurement device is a distance measurement device using a camera. In this case, the position and orientation of the object to be measured may be detected by recognizing the three-dimensional position of each marker (e.g., ArUco) placed on the object to be measured from an image captured by the camera. Also, a distance measurement device using infrared rays or ultrasonic waves may be used.

[0065] Furthermore, the probe head 12 may 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, improving user convenience.

[0067] Furthermore, the low-accuracy measurement device is not limited to a non-contact type such as the laser tracker 16 that performs measurements without contacting the object to be measured, but may 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 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, 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. [Explanation of symbols]

[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...arithmetic 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 relative position information detection unit that detects relative position information indicating a relative positional relationship between a target group having a plurality of targets and the probe head; an image acquisition unit that acquires images of the target group captured by a camera mounted on the probe head; a high-accuracy self-position estimation unit that detects the position and orientation of the probe head relative to the target group based on the relative position information and the image; A self-location estimation device comprising:

2. The high-accuracy self-location estimation unit a feature point detection unit that detects feature points indicating the positions 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 that indicates a correspondence relationship between each of the feature points detected by the feature point detection unit and an object point that indicates a position of each of the targets in three-dimensional space is minimized; an optimization calculation unit that calculates the position and orientation of the probe head relative to the target group by optimization calculation based on the combinations of the feature points and the object points extracted by the combination extraction unit; The self-location estimation device according to claim 1 , further comprising:

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 self-position estimation device according to claim 2 , wherein the error function E(i, j) is expressed by the following equation: [Equation 1]

4. the relative position information is information detected based on a measurement result of a measurement device separate from the probe head; The self-position estimation device according to claim 1 .

5. the measurement device is a laser tracker; The self-position estimation device according to claim 4 .

6. the probe head is equipped with a SLAM function unit, The relative position information is information detected based on the measurement results of the SLAM function unit. The self-position estimation device according to claim 1 .

7. a relative position information detecting step of detecting relative position information indicating a relative positional relationship between the probe head and the target group; an image acquisition step of acquiring images of the target group by a camera mounted on the probe head; a high-precision self-position estimation step of detecting the position and orientation of the probe head relative to the target group based on the relative position information and the image; A self-location estimation method including:

Images