Own-position estimating device and own-position estimating method

Abstract

Provided are an own-position estimating device and an own-position estimating method capable of performing own-position estimation of a camera over a wide range. An own-position estimating device (50) comprises: an image acquiring unit (60) that acquires an image, captured by a camera (20), of a target plate (14) having a plurality of target groups; a feature point detecting unit (62) that detects a feature point indicating the position of each target of the target groups from the image; and an own-position estimating unit (64) that detects the position and orientation of the camera (20) on the basis of an error function indicating a correspondence relationship between each feature point detected by the feature point detecting unit (62) and a target point indicating the position of each target of the target groups.

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 can estimate the self-position of a camera.

[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] 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 in which the self-position (position and attitude) of a camera is detected 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.

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

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

[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 the first aspect in 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 equal to or less than N, and N is the number of targets), and 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) (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]

[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 any one of the first to third aspects, and includes a relative position estimation unit that detects target group relative position information indicating the relative positions of target groups based on an error function.

[0016] The 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-location estimation device according to a seventh aspect is the self-location estimation device according to any one of the first to sixth aspects, wherein each target is point-shaped or dot-shaped.

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

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

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

[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 a 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-location estimation system 10 includes a probe head 12, a target plate 14, and a self-location estimation device 50. The self-location estimation device 50 is an example of a self-location estimation device of the present invention. In this embodiment, the self-location estimation 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-location estimation 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 the camera 20 mounted on the probe head 12 photographs the target plate 14 (one or more target groups 16), allowing the self-position (position and attitude) of the probe head 12 to be estimated by the self-position estimation device 50 described below.

[0027] As shown in FIG. 2 , the target plate 14 has a main surface (target placement 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 (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 has a plurality (a large number) of targets 24 provided therein (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 are known. The shape and size of each target 24 in the target group 16 are also known.

[0030] [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 is also 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) 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] The storage unit 54 stores target images, which will be described later. 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 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.

[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 the 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 describing the camera projection model.

[0037] As shown in FIG. 4, the world coordinate system is a coordinate system that represents a position in a three-dimensional space (real space), and has an origin O ｗ and X ｗ Axis, Y ｗ axis, Z ｗ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 a coordinate system with the center O of the optical axis of the camera 20. ｃ is the origin, and the origin O ｃ From there, turn right and press X. ｃ axis, downward direction is Y ｃ axis, optical axis direction Z ｃ The image coordinate system is a three-dimensional Cartesian coordinate system with the axes O and O. ｃ From Z ｃ The upper left corner of the image plane IP, which is a focal distance f away in the X direction, is set as the origin. ｃ axis and Y ｃ It is a two-dimensional orthogonal coordinate system (pixel coordinate system) having a U axis and a V axis in directions parallel to the axes.

[0038] First, the coordinates (x) of point P (object point) in the world coordinate system in three-dimensional space are ｗ , y ｗ , z ｗ ) 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]

[0040] Here, [R | t] w,c is a transformation matrix (external 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 a projection point Q obtained by projecting a point P located at (x, y, z) in the camera coordinate system onto an image plane IP are (u, v), the following relationship shown in equations (2) to (7) holds.

[0042]

[0043]

[0044]

[0045]

[0046]

[0047]

[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 the camera 20 is taken into consideration.

[0049] Also, f ｘ , f ｙ denotes the focal length in the x and y directions expressed in pixels, and c ｘ , c ｙ indicates 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). １ , k ２ , k ３ is the radial distortion coefficient, and p １ , p ２ is the tangential distortion coefficient. In this specification, the focal length f ｘ , f ｙ , optical center c ｘ , c ｙ are called the internal parameters of the camera 20, and the distortion coefficient k １ , k ２ , k ３ , p １ , p ２ are called 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-Location Estimation Method] Next, 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 will be described. 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 calibration of the camera 20 has already been performed, 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 storage 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 (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 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, in order to distinguish between the multiple target groups 16, a hyphenated number will be added to each reference symbol. For example, the sth target group 16 will be denoted 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 will be denoted 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 above-mentioned feature point detection process are designated as 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 The coordinates (u i,s ,v i,s ) lens distortion correction is applied.

[0057] Furthermore, 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 relationship between can be expressed as the following equation (8).

[0059]

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

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

[0062] As shown in FIG. w,c is a transformation matrix for transforming coordinates from the world coordinate system to the camera coordinate system, and the rotation matrix R w,c and the translation vector t w,c Also, [R|t] w,s is a 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 the 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 the translation vector t a,b a and b are natural numbers equal to or less than N, and a≠b.

[0064] In this embodiment, the target plate 14 is divided into a plurality of target groups 16. Therefore, while the relative positions of the individual target groups 16 may vary due to factors such as installation errors and the weight of each target group 16, the relative positions of the targets 24 within each 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]

[0067] In this case, [R|t] is a 4x4 matrix, but in reality it is a translation with three degrees of freedom and a rotation with three degrees of freedom, so the parameters are (x, y, z, r １ , r ２ , r ３ ), the transformation of rotation and translation can be suppressed by expressing it as in the following equation (10) (Rodrigues rotation formula).

[0068]

[0069] The error function E defined in this way is 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 takes 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 (step S14).

[0071] The transformation matrix [R|t] obtained by 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 targets 16 using the following equation (11) (step S16).

[0074]

[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). ａ,ｂ 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, the abnormality determination unit 68 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, such as 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] [Effects] 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 targets 16 in addition to estimating 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 groups 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 illuminating 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 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 the probe head 12 may be moved by controlling the operation of the robot arm, thereby measuring the three-dimensional coordinates of the workpiece.

[0088] Second Embodiment In the second embodiment, a 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., a charge-coupled device (CCD) or a complementary metal oxide semiconductor (CMOS)) 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 the 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 defocus positions at which the images of the targets 24A and 24B are out of focus, the defocus position further behind the in-focus position F1 (on the opposite side from the lens 120) is designated F2, and the defocus 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 figure indicate the front and rear 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 preferably 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 Ｃ 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 (example) 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, thereby improving the accuracy of the self-location estimation process of the camera 20.

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

[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. A self-position estimation device comprising: 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 of the feature points detected by the feature point detection unit and target points indicating the position of each target in the target group.

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 2. The self-location estimation device according to claim 1, wherein, when 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 error function E is expressed by the following equation:

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

4. A self-position estimation device according to any one of claims 1 to 3, comprising a relative position estimation unit that detects target group relative position information indicating the relative positions of the target groups based on the error function.

5. The self-position estimation device according to claim 4, 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.

6. A self-position estimation device according to any one of claims 1 to 3, wherein the image acquired by the image acquisition unit is an image of each of the targets in the target group photographed in a defocused state.

7. The self-location estimation device according to claim 1, wherein each of the targets is point-shaped or dot-shaped.

8. A self-position estimation method comprising: 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 of the feature points detected in the feature point detection step and target points indicating the position of each target in the target group.

Images