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 perform self-position estimation of a camera over a wide range.SOLUTION: A self-position estimation device 50 comprises: an image acquisition unit 60 that acquires an image obtained by photographing, with a camera 20, a target plate 14 having a plurality of target groups; a feature point detection unit 62 that detects, from the image, feature points indicating the positions of the targets in the target groups; and a self-position estimation unit 64 that detects the position and posture of the camera 20 on the basis of an error function indicating the correspondence between the feature points detected by the feature point detection unit 62 and target points indicating the positions of the targets in the target groups.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 can estimate the self-position of a camera. [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] 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.

[0005] Meanwhile, a method is known for detecting the camera's own position (position and attitude) from an image captured by the camera of a target whose position in three-dimensional space is known (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 that associate the position of each target in three-dimensional space with the position of each target in the captured image. [Prior art documents] [Patent documents]

[0006] [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]

[0007] However, although the method of detecting the self-position from an image of a target captured using a camera can estimate the self-position with high accuracy, there is a problem in that it is difficult to obtain a wide measurement range, as will be described below.

[0008] That is, in order to obtain a wide measurement range in the above-mentioned method, it is necessary to enlarge the target group having multiple targets. In this case, it is necessary to determine the position of each target in three-dimensional space in advance, which requires a large measuring device for measuring the three-dimensional position of each target. In addition, the deflection of the target group due to its own weight increases the error in the three-dimensional coordinates of each target, which causes a deterioration in the accuracy of the self-position estimation. Therefore, it is difficult to support a wide area with a single target group, and there are limitations to estimating the self-position over a wide area.

[0009] The present invention has been made in view of the above circumstances, and has an object to provide a self-position estimation device and a self-position estimation method that are capable of estimating the self-position of a camera over a wide range. [Means for solving the problem]

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

[0011] The self-position estimation device according to the first aspect includes an image acquisition unit that acquires an image of a target plate having a group of multiple targets photographed by a camera, a feature point detection unit that detects feature points indicating the position of each target in the target group from the image, and a self-position estimation unit that detects the position and attitude of the camera based on an error function that indicates the correspondence between each feature point detected by the feature point detection unit and a target point indicating the position of each target in the target group.

[0012] The self-localization estimation device according to the second aspect is the same as that according to the first aspect, except that the transformation matrix from the world coordinate system to the s-th target group coordinate system is [R|t] w,s (where s is a natural number less than or equal to N, and N is the number of targets.) The transformation matrix from the world coordinate system to the camera coordinate system is [R|t] w,c The coordinates of the feature points on the image are (u i,s ,v i,s ) (where i is a natural number equal to or less than m, and m is the number of feature points), and the coordinates of the target point in the sth target group are (x pj,s ,y pj,s ,z pj,s ) (j is a natural number equal to or less than n, and n is the number of target points), and the error function is E, the error function E is expressed by the following equation.

[0013]

number

[0014] A self-location estimation device according to a third aspect is the second aspect, wherein the self-location estimation unit calculates a transformation matrix [R|t] when the error function is minimized. w,c is calculated as the position and orientation of the camera.

[0015] A self-position estimation device according to a fourth aspect is the self-position estimation device of any one of the first to third aspects, further comprising a relative position estimation unit that detects target group relative position information indicating relative positions of target groups based on an error function.

[0016] A self-position estimation device according to a fifth aspect is the fourth aspect, further comprising an abnormality determination unit that determines whether or not there is an abnormality in the target plate based on the target group relative position information.

[0017] A self-position estimation device according to a sixth aspect is any one of the first to fifth aspects, wherein the images acquired by the image acquisition unit are images of each target in the target group photographed in a defocused state.

[0018] A self-position estimation device according to a seventh aspect is the self-position estimation device according to any one of the first to sixth aspects, wherein each target is point-like or dot-like.

[0019] The self-position estimation method according to the eighth aspect includes an image acquisition step of acquiring an image of a target plate having a group of multiple targets photographed by a camera, a feature point detection step of detecting feature points indicating the position of each target in the target group from the image, and a self-position estimation step of detecting the position and attitude of the camera based on an error function indicating the correspondence between each feature point detected in the feature point detection step and a target point indicating the position of each target in the target group. [Effects of the Invention]

[0020] According to the present invention, it is possible to estimate the self-position of the camera over a wide range. [Brief explanation of the drawings]

[0021] [Figure 1] 1 is a block diagram showing a self-location estimation system according to a first embodiment. [Figure 2] 1 is a schematic configuration diagram showing a schematic configuration of a self-position estimation system according to a first embodiment. [Figure 3] FIG. 2 is a plan view showing a group of targets. [Figure 4] FIG. 10 is an explanatory diagram for explaining a camera projection model. [Figure 5] 4 is a flowchart showing an example of a self-position estimation process executed by the self-position estimation device of the first embodiment. [Figure 6] FIG. 2 is an explanatory diagram showing the relationship between the coordinate systems. [Figure 7] FIG. 10 is a diagram for explaining a modified example of the first embodiment. [Figure 8] FIG. 10 is a diagram showing an example of a group of targets photographed by a camera. [Figure 9] 1A and 1B are diagrams showing images of targets (Example and Comparative Example). DETAILED DESCRIPTION OF THE INVENTION

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

[0023] First Embodiment [Self-location estimation system] Fig. 1 is a block diagram showing a self-location estimation system 10 according to the first embodiment. Fig. 2 is a schematic diagram showing the schematic configuration of the self-location estimation system 10 according to the first embodiment.

[0024] 1 and 2, the self-localization system 10 includes a probe head 12, a target plate 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.

[0025] 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 is equipped with a contact or non-contact probe 18. The probe 18 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 holds the probe head 12 and performs measurements to obtain the three-dimensional coordinates of the measurement point on the workpiece.

[0026] The probe head 12 has a self-position estimation function, and by using a camera 20 mounted on the probe head 12 to photograph the target plate 14 (one or more target groups 16), the self-position (position and attitude) of the probe head 12 can be estimated by a self-position estimation device 50 described below.

[0027] As shown in Fig. 2, the target plate 14 has a main surface (target arrangement surface) 14a on which a plurality of target groups 16 are arranged. That is, the main surface 14a of the target plate 14 is partitioned into a plurality of regions, and a target group 16 is provided in each region. In other words, the target plate 14 has a structure in which a plurality of target groups 16 are arranged two-dimensionally adjacent to each other (closely spaced) on the main surface 14a. Each target group 16 is formed in a hexagonal shape (honeycomb shape), and the center-to-center distance between adjacent target groups 16 is constant. Furthermore, each target group 16 has a common structure, and as described below, each target group 16 is provided with a plurality (a large number) of targets 24 (see Fig. 3).

[0028] In this embodiment, as an example, the planar shape of each target group 16 is hexagonal (honeycomb-shaped), but the shape is not limited to this, and polygonal shapes such as triangle, rectangle, trapezoid, rhombus, pentagon, etc. may also be used as appropriate. From the viewpoints of strength and ease of fabrication, it is preferable that each target group 16 has a hexagonal shape.

[0029] FIG. 3 is a plan view showing the target group 16. As shown in FIG. 3, the target group 16 includes a plurality (a large number) of targets 24 arranged two-dimensionally. Each target 24 is formed in a point or dot shape. Each target 24 may also be configured as a small point light source (point light source) such as an LED. In the target group 16, each target 24 is arranged at intervals, and the relative positions of the targets 24 to each other are known. The shape and size of each target 24 in the target group 16 are also known.

[0030] [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. A camera 20 mounted on the probe head 12 is connected to the self-location estimation device 50. The method of connection with the camera 20 is not particularly limited, and the connection may be via a cable, or via a wired or wireless network.

[0031] 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.

[0032] Target images, which will be described later, are stored in the storage unit 54. The storage unit 54 also stores information (target group information) regarding the shape, size, and arrangement of each target 24 that constitutes the target group 16.

[0033] 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.

[0034] The calculation processing unit 52 functions as an image acquisition unit 60, a feature point detection unit 62, a self-position estimation unit 64, a relative position estimation unit 66, an abnormality judgment unit 68, and a judgment processing unit 70 by reading and executing a control program stored in the memory unit 54.

[0035] The image acquisition unit 60 is an example of an image acquisition unit of the present invention. The feature point detection unit 62 is an example of a feature point detection unit of the present invention. The self-position estimation unit 64 is an example of a self-position estimation unit of the present invention. The relative position estimation unit 66 is an example of a relative position estimation unit of the present invention. The abnormality determination unit 68 is an example of an abnormality determination unit of the present invention.

[0036] [Camera projection model] Before describing the self-location estimation process executed by the self-location estimation device 50 of this embodiment, the camera projection model that is the premise of the process will be described. Fig. 4 is an explanatory diagram for explaining the camera projection model.

[0037] As shown in Figure 4, the world coordinate system is a coordinate system that represents positions in three-dimensional space (real space), and its origin is O wand X w Axis, Y w axis, Z w The world coordinate system is a three-dimensional Cartesian coordinate system with the axes being the coordinate axes. Note that any coordinate system may be used as the world coordinate system as long as it can identify a position in three-dimensional space (three-dimensional position). The camera coordinate system is based on the optical axis center O of the camera 20. c is the origin, and the origin O c From there, turn right and press X c axis, downward direction is Y c axis, the optical axis direction is Z c The image coordinate system is a three-dimensional Cartesian coordinate system with the axis O. c From Z c The origin is the upper left corner of the image plane IP, which is a focal length f away in the X direction. c axis and Y c It is a two-dimensional Cartesian coordinate system (pixel coordinate system) with the U axis and V axis in directions parallel to the y-axis and y-axis, respectively.

[0038] First, the coordinates of point P (object point) in the three-dimensional space in the world coordinate system (x w , y w , z w ) can be converted into coordinates (x, y, z) in the camera coordinate system using the rotation matrix R and translation vector t of the camera 20, as shown in the following equation (1).

[0039]

number

[0040] where [R|t] w,c is a transformation matrix (extrinsic parameter matrix) for transforming coordinates from the world coordinate system to the camera coordinate system, and represents the orientation and position of the camera 20 in the world coordinate system. [R|t] w,c Each component r 11 , r 12 , …, r 33 , t x , t y , t z are called the extrinsic parameters of the camera.

[0041] Next, if the coordinates (pixel coordinates) of the projection point Q, which is formed by projecting point P, which is located at (x, y, z) in the camera coordinate system, onto the image plane IP, are (u, v), the following relationships are established as shown in equations (2) to (7).

[0042]

number

[0043]

number

[0044]

number

[0045]

number

[0046]

number

[0047]

number

[0048] Here, (x', y') represent the coordinates of the projected point obtained by projecting point P, which is located at (x, y, z) when viewed from the camera coordinate system, onto the normalized image plane (z=1). Also, (x'', y'') represent the coordinates of the projected point (distorted point) obtained by projecting point P onto the normalized image plane when the lens distortion of camera 20 is taken into consideration.

[0049] Also, f x , f y are the focal lengths in the x and y directions in pixels, and c x , c yindicates the optical center in the image coordinate system (the position where the optical axis of the camera 20 intersects with the image plane IP, the optical center in pixel units). Also, k1, k2, and k3 are radial distortion coefficients, and p1 and p2 are tangential distortion coefficients. In this specification, the focal length f x , f y , optical center c x , c y are called the internal parameters of the camera 20, and the distortion coefficients k1, k2, k3, p1, and p2 are called the distortion parameters of the camera 20.

[0050] The self-position of the camera 20 obtained in the self-position estimation process of this embodiment is expressed as a matrix [R|t] indicating the orientation and position of the camera 20 in the world coordinate system in the above-mentioned camera projection model. w,c This corresponds to obtaining the external parameters of the camera 20.

[0051] [Self-position estimation method] Next, a description will be given of a 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. 5 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. 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.

[0052] 2, the target plate 14 (one or more target groups 16) is photographed by the camera 20 mounted on the probe head 12 (step S10). 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.

[0053] Next, the feature point detection unit 62 executes a feature point detection process to detect a plurality of feature points from the target image (step S12).

[0054] Specifically, the feature point detection unit 62 reads the target image from the storage unit 54. Then, the feature point detection unit 62 performs predetermined image processing (such as grayscale conversion) 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 IP; see FIG. 4). Note that the position of the center of gravity of the target 24 is detected as the feature point on the target image.

[0055] Here, to distinguish between the multiple target groups 16, a hyphenated number is added to each reference symbol. For example, the sth target group 16 is expressed as 16-s, where s is a natural number equal to or less than N, and N is the number of target groups 16 included in the target plate 14. Similarly, each target 24 in the target group 16-s is expressed as 24-s by adding a hyphenated number to each reference symbol. Furthermore, when the subscript "s" is added to each coordinate described below, it indicates the coordinate corresponding to the sth target group 16-s.

[0056] The feature points on the target image detected in the feature point detection process described above are called Q i,s and its coordinates (two-dimensional coordinates) are (u i,s ,v i,s ) where i is a natural number equal to or less than m, and m is the number of feature points detected by the feature point detection unit 62. Note that, if necessary, each feature point Q i,s coordinates (u i,s ,v i,s ) has lens distortion correction applied.

[0057] Also, a target point indicating the position of each target 24-s in the target group 16-s (i.e., the position of each target 24-s in the s-th target group coordinate system) is denoted by P pj ,s and its coordinates (three-dimensional coordinates) are (x pj,s ,y pj,s ,z pj,s) where j is a natural number equal to or less than n, and n is the number of targets 24 included in target group 16-s.

[0058] Each feature point Q on the target image i,s and each target point P in the target group 16-s pi,s The correspondence between these can be expressed as in the following equation (8).

[0059]

number

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

[0061] Fig. 6 is an explanatory diagram showing the relationship between the coordinate systems. Fig. 6 shows, as an example, a case where the shooting range of camera 20 includes a partial area of ​​each of three target groups 16-s (s=1, 2, 3). For convenience, Fig. 6 shows a simplified illustration of camera 20.

[0062] As shown in Figure 6, [R|t] w,c is the transformation matrix for transforming coordinates from the world coordinate system to the camera coordinate system, and the rotation matrix R w,c and translation vector t w,c Also, [R|t] w,s is the transformation matrix for coordinate transformation from the world coordinate system to the sth target group coordinate system (a coordinate system based on the sth target group 16-s), and the rotation matrix R w,s and translation vector t w,s is defined as:

[0063] In addition, in FIG. 6, the transformation matrix for coordinate transformation from the target group coordinate system based on the a-th target group 16-a to the target group coordinate system based on the b-th target group 16-b is [R|t] a,b In addition, [R|t] a,b is the rotation matrix R a,b and translation vector ta,b where a and b are natural numbers less than or equal to N, and a≠b.

[0064] In this embodiment, the target plate 14 is divided into multiple target groups 16. Therefore, while the relative position of each target group 16 may vary due to factors such as installation error and the weight of each target group 16, the relative positions of targets 24 within a target group 16 are always constant. The self-position estimation process utilizes these characteristics to estimate the self-position of the camera 20 based on an error function E, which will be described later.

[0065] Here, the error function E is defined as follows:

[0066]

number

[0067] In this case, [R|t] is a 4x4 matrix, but in reality it is a 3-degree-of-freedom translation and 3-degree-of-freedom rotation, so there are six parameters (x, y, z, r1, r2, r3). ​​Therefore, by expressing it as in the following equation (10), the transformation of rotation and translation can be suppressed (Rodriguez rotation formula).

[0068]

number

[0069] The error function E defined in this way is the error function for each feature point Q on the target image. i,s and a target point P indicating the position of each target 24 in the target group 16-s. j,s (points indicating the position of each target 24 in the sth target group coordinate system), and the error function E reaches its minimum value when the combination of these points is such that they are in a projective relationship with each other.

[0070] Therefore, the self-position estimation unit 64 calculates the transformation matrix [R|t] that minimizes the error function E. w,c , [R|t] w,1 , [R|t] w,2 , …, [R|t] w,N is calculated by nonlinear optimization calculation (step S14).

[0071] The transformation matrix [R|t] obtained in the self-position estimation unit 64 w,c indicates the self-position (position and orientation) of the camera 20 in the world coordinate system.

[0072] The self-position estimation unit 64 stores information indicating the position and attitude of the camera 20 obtained as described above (camera self-position information) in the storage unit 54. The camera 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.

[0073] Next, the relative position estimation unit 66 calculates the relative positions of the target groups 16 using the following equation (11) (step S16).

[0074]

number

[0075] In addition, the self-position estimation unit 64 calculates [R|t] by nonlinear optimization calculation. w,c , [R|t] w,1 , [R|t] w,2 , …, [R|t] w,N is obtained, the relative position estimation unit 66 uses the calculation result to obtain the transformation matrix [R|t] as information indicating the relative positions of the a-th target group 16-a and the b-th target group 16-b, as shown in the above-mentioned equation (11). a,b can be obtained.

[0076] The relative position estimation unit 66 stores information indicating the relative positions of the target groups 16 obtained as described above (target group relative position information) in the storage unit 54. The storage unit 54 also stores reference information (reference relative position information) indicating the relative positions of the target groups 16, which serves as a reference for comparison in the abnormality determination unit 68 described below. The reference relative position information may be predetermined based on design information for the target plate 14, or may be target group relative position information detected during the previous self-position estimation process.

[0077] Next, the abnormality determination unit 68 reads the target group relative position information and the reference relative position information from the storage unit 54. Then, the abnormality determination unit 68 compares these pieces of information to determine whether there is a change between the target group relative position information and the reference relative position information (step S18). For example, it determines whether the difference in the relative distance or relative angle between the targets 16 exceeds a predetermined threshold (tolerance value).

[0078] If the abnormality determination unit 68 determines that there is a change in the target group relative position information (YES in step S18), the abnormality determination unit 68 outputs abnormality notification information indicating that an abnormality has occurred in the target plate 14 to an output unit (not shown) such as a monitor (step S20). This allows the user to individually determine whether there is an abnormality in the target plate 14 and the details of the abnormality. For example, this can be used for daily inspection of the target plate 14 to check whether the frame supporting the target group 16 has been distorted due to a collision or the like. Thereafter, the self-position estimation process is stopped, and this flowchart ends.

[0079] On the other hand, if the abnormality determination unit 68 determines that there is no change in the target group relative position information (NO in step S18), it determines whether or not to repeat the self-position estimation process of the camera 20 (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 camera 20 (YES in step S22), and repeats the processes from step S10 to step S22. As a result, the self-position estimation process of the camera 20 is continuously and repeatedly performed while the probe head 12 is measuring the three-dimensional coordinates of the workpiece.

[0080] 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 camera 20 is completed (NO in step S22), and ends this flowchart.

[0081] 〔effect〕 Next, the effects of this embodiment will be described.

[0082] According to this embodiment, the self-position (position and attitude) of the camera 20 is estimated using the target plate 14 divided into a plurality of target groups 16, so it is possible to estimate the self-position of the camera 20 without knowing the position (three-dimensional position) of each target 24 in three-dimensional space, as long as the relative position of each target 24 within the target group 16 is known. This makes it possible to increase the size of the target plate 14 by increasing the number of target groups 16 included in the target plate 14, and makes it possible to estimate the self-position of the camera 20 over a wide range.

[0083] Furthermore, according to this embodiment, it is possible to estimate the relative positions of the target group 16 as well as the self-position of the camera 20. This makes it possible to detect abnormalities in the target plate 14 early during daily management, improving maintainability and leading to improved accuracy in estimating the self-position of the camera 20.

[0084] In this embodiment, a preferred mode is shown in which both the self-position of the camera 20 and the relative positions of the target group 16 are estimated, but it is also possible to estimate only one of these.

[0085] Furthermore, in this embodiment, when the targets 24 of each target group 16 are configured with point light sources, the self-position estimation device 50 may control the point light sources via a light source control unit (not shown) so that only the area being photographed by the camera 20 is illuminated when photographing with the camera 20, as shown in Fig. 7. In this way, even if the target plate 14 is enlarged, the thermal effect can be reduced and energy can be saved by selectively lighting up the target group 16 according to the area being photographed by the camera 20.

[0086] Furthermore, in the present embodiment, an example has been shown in which each target group 16 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 this is not limiting, and for example, each target group 16 may be configured with a grid pattern or a checkered pattern as disclosed in the above-mentioned Patent Document 2. Furthermore, each target group 16 may be various two-dimensional patterns including an AR marker (Augmented Reality Marker) or a QR code (Quick Response code, registered trademark), etc.

[0087] Furthermore, the probe head 12 is not limited to one that is 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 the probe head 12 may be moved by controlling the operation of the robot arm, thereby measuring the three-dimensional coordinates of the workpiece with the probe head 12.

[0088] Second Embodiment In the second embodiment, the self-position estimation process of the camera 20 is performed based on target images of each target 24 of the target group 16 captured by the camera 20 in a defocused state. Fig. 8 is a diagram showing an example of the target group 16 captured by the camera 20.

[0089] As shown in FIG. 8, the camera 20 includes a lens (objective lens) 120 and an image sensor (e.g., including a CCD (Charge Coupled Device) or a CMOS (Complementary Metal Oxide Semiconductor)) 122. For simplicity, the example shown in FIG. 8 illustrates only two targets 24A and 24B as representatives of the multiple targets 24 on the target group 16. Reference symbols PX1 and PX2 in FIG. 8 denote optical axes of light from the targets 24A and 24B, respectively, toward the image sensor 122. In the example shown in FIG. 8, the optical axis PX1 coincides with the optical axis AX of the lens 120. Furthermore, in FIG. 8, the in-focus position at which the images of the targets 24A and 24B are focused via the lens 120 is designated F1. Among the de-focus positions at which the images of the targets 24A and 24B are out of focus, the de-focus position further behind the in-focus position F1 (on the opposite side from the lens 120) is designated F2, and the de-focus position further in front of the in-focus position F1 (toward the lens 120) is designated F3. 8 shows an example in which the image sensor 122 is positioned at a defocus position F2. The symbols f′ and f in the drawing indicate the front and back focal positions of the lens 120, respectively.

[0090] The targets 24 on the target group 16 are tiny dots. The shape of these dots in a plan view is approximately circular. Here, the size (diameter) of the targets 24 is determined by the relationship with at least one of, for example, the focal length of the camera 20 (the focal length of the lens 120), the distance between the camera 20 and the targets 24 (the distance along the optical axis between the lens 120 and the targets 24), the pixel size of the image sensor 122 of the camera 20, and the allowable error (pixel error) for feature point detection. Note that the targets 24 are not limited to tiny dots, and may also be tiny point light sources.

[0091] It is preferable that the size (diameter) of the target 24 on the target group 16 is such that when the target 24 is photographed by the image sensor 122 at the in-focus position F1, the size of the area occupied by the image of the target 24 on the image sensor 122 is one pixel or less. For example, when the focal length (rear focal length) of the lens 120 is 8 mm, the center-to-center distance between the camera 20 and the target 24 is 300 mm, and the pixel size of the image sensor 122 is 2.74 μm, the diameter φ of the target 24 is 102.75 μm or less. However, depending on the tolerance for feature point detection, the size of the target 24 may be larger than the above example.

[0092] When the lens 120 is an appropriately designed and manufactured lens (specifically, when the optical axis AX of the lens 120 coincides with the center of the aperture stop AP of the lens 120), when the target 24 is photographed, the height of the incident light at the position of the lens 120 is Δ1 = Δ2. In this case, the image height at the defocus position F2 (Y of the defocused image of the target 24) is C The distance δ1 to the center of the image (dimension in the direction) is also δ1=δ2. Although not shown, the same applies to the defocus position F3 as to the defocus position F2.

[0093] IXA in Fig. 9 shows an image (embodiment) of the target 24 captured by the image sensor 122 positioned at the defocus position F2. In IXA in Fig. 9, as described in Fig. 8, the center line of the image of the target 24 overlaps and substantially coincides with the optical axis PX of the light from the target 24. The same is true when the target 24 is captured by the image sensor 122 positioned at the defocus position F3.

[0094] IXB in Fig. 9 shows an image (comparison example) of a dot (a non-minimal target) whose image captured in an in-focus state is about the same size as IXA in Fig. 9. In this case, as shown in the enlarged view in IXC in Fig. 8, the center line L1 of the dot image is shifted from the line L2 passing through the center of gravity of the dot image.

[0095] In contrast, when an extremely small target 24 is photographed in a defocused state, the center line of the image of the target 24 coincides with the optical axis (AX or AX1), so the center (feature point) of the target 24 can be detected with high accuracy.

[0096] In the second embodiment, the targets 24 on the target group 16 are minimized, so that even if the target group 16 is photographed at an angle with respect to the camera 20, the positions of the feature points (the positions of the centers of gravity of the targets 24) can be detected with high accuracy without being affected by a non-telecentric optical system. On the other hand, minimizing the targets 24 makes them more susceptible to the influence of luminance noise, but in this embodiment, the targets 24 are photographed in a defocused state, so that the positions of the feature points can be detected with the influence of luminance noise effectively suppressed.

[0097] In the second embodiment, the minute targets 24 are substantially circular (dot-shaped), but are not limited to this. Any shape may be used as long as the center of gravity of the target 24 coincides with the center position in the defocused image, and the target 24 may be, for example, a shape that is line-symmetrical in two directions orthogonal to each other, such as an ellipse, or a shape that is point-symmetrical.

[0098] In the second embodiment, processing is performed in the same manner as in the first embodiment except for the points mentioned above, and therefore a description thereof will be omitted here to avoid duplication.

[0099] According to the second embodiment, the self-location estimation process is performed based on a target image of a target group 16 including extremely small targets 24, captured by the camera 20 in a defocused state. Therefore, even if the target group 16 is captured at an angle with respect to the camera 20, the positions of the feature points (the positions of the centers of gravity of the targets 24) can be detected with high accuracy without being affected by a non-telecentric optical system. Furthermore, by capturing the targets 24 in a defocused state, the positions of the feature points can be detected while effectively suppressing the influence of luminance noise associated with the miniaturization of the targets 24. Therefore, it is possible to accurately detect the feature points of the target image, and the accuracy of the self-location estimation process of the camera 20 can be improved.

[0100] 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]

[0101] 10...self-position estimation system, 12...probe head, 14...target plate, 16...target group, 18...probe, 20...camera, 24...target, 50...self-position estimation device, 52...arithmetic processing unit, 54...memory unit, 60...image acquisition unit, 62...feature point detection unit, 64...self-position estimation unit, 66...relative position estimation unit, 68...abnormality judgment unit, 70...judgment processing unit, 120...lens, 122...imaging element, F1...in-focus position, F2...defocus position, F3...defocus position

Claims

1. an image acquisition unit that acquires an image of a target plate having a group of multiple targets taken by a camera; a feature point detection unit that detects feature points indicating the positions of each target of the target group from the image; a self-position estimation unit that detects the position and orientation of the camera based on an error function that indicates a correspondence between each of the feature points detected by the feature point detection unit and a target point that indicates the position of each of the targets in the target group; A self-location estimation device comprising:

2. The transformation matrix from the world coordinate system to the sth target group coordinate system is [R|t] w,s (where s is a natural number equal to or less than N, and N is the number of targets in the group), and the transformation matrix from the world coordinate system to the camera coordinate system is [R|t] w,c and the coordinates of the feature points on the image are (u i,s ,v i,s ) (i is a natural number equal to or less than m, and m is the number of feature points), and the coordinates of the target point in the s-th target group are (x pj,s ,y pj,s ,z pj,s ) (j is a natural number equal to or less than n, and n is the number of the target points), and the error function is E, The self-position estimation device according to claim 1 , wherein the error function E is expressed by the following equation: [Equation 1]

3. The self-position estimation unit calculates the transformation matrix [R|t] when the error function is minimized. w,c is calculated as the position and orientation of the camera. The self-position estimation device according to claim 2 .

4. a relative position estimation unit that detects target group relative position information indicating relative positions of the target groups based on the error function; The self-position estimation device according to claim 1 .

5. an abnormality determination unit that determines whether or not there is an abnormality in the target plate based on the target group relative position information; The self-position estimation device according to claim 4 .

6. The image acquired by the image acquisition unit is an image obtained by photographing each of the targets in the target group in a defocused state. The self-position estimation device according to claim 1 .

7. Each of the targets is point-shaped or dot-shaped. The self-position estimation device according to claim 1 .

8. an image acquisition step of acquiring an image of a target plate having a plurality of target groups by a camera; a feature point detection step of detecting feature points indicating the positions of each target of the target group from the image; a self-position estimation step of detecting a position and an attitude of the camera based on an error function indicating a correspondence between each of the feature points detected in the feature point detection step and a target point indicating a position of each of the targets in the target group; A self-location estimation method including:

Images