Self-position estimation device and self-position estimation method, and camera relative position adjustment method

Abstract

To provide a self-position estimation device and a self-position estimation method that can improve the accuracy of self-position estimation without using a wide-angle lens.SOLUTION: A self-position estimation device 50 comprises: an image acquisition unit 60 that acquires a plurality of images obtained by photographing target groups 14A, 14B, by using a plurality of cameras 20A, 20B mounted on a probe head 12 and having different directions from each other as photographing directions; and a self-position estimation unit 62 that detects the position and posture of the probe head 12 on the basis of the plurality of images.SELECTED DRAWING: Figure 1

Description

[Technical Field]

[0001] The present invention relates to a self-position estimation device and method capable of estimating the self-position of a probe head, and a camera relative position adjustment method. [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] In a method for detecting the self-position from an image of a target captured using a camera, if there is an error in the coordinates (target coordinates) indicating the position of the target on the image captured by the camera, the error will cause variation in the principal point of the camera (the origin of the camera coordinate system), which will affect the accuracy of the estimation of the self-position, as will be explained below.

[0008] 9 to 11 are diagrams showing how a target 100 in three-dimensional space is projected onto the image plane 102 of the camera. FIG. 9 shows a case where there is no error in the target coordinates on the image plane 102 (coordinates indicating the position of the target 104 projected onto the image plane 102). FIG. 10 shows a case where there is an error in the target coordinates on the image plane 102. FIG. 11 shows how the use of a wide-angle lens reduces the error in the target coordinates on the image plane 102.

[0009] In the above method, the camera principal point 106 can be found from the correspondence between the coordinates (three-dimensional coordinates) indicating the position of the target 100 in three-dimensional space and the target coordinates on the image plane 102. In this case, as shown in Fig. 9, if there is no error in the target coordinates on the image plane 102, the three-dimensional coordinates indicating the position of the camera principal point 106 are also determined to be a single point without error.

[0010] However, as shown in Fig. 10, if there is an error in the target coordinates on the image plane 102, the position of the camera principal point 106 obtained from the above correspondence will vary depending on the range of the possible error. That is, due to the influence of the error in the target coordinates on the image plane 102, the position of the camera principal point 106 will vary within a principal point error range 108 shown in the hatched area in Fig. 9. In particular, the shooting direction of the camera (optical axis direction) will be significantly affected.

[0011] On the other hand, if a wide-angle lens is used, the distance between targets 100 can be increased, as shown in Fig. 11. In this case, if the error in the target coordinates on the image plane 102 is about the same as in Fig. 10, the range that the camera principal point 106 can take can be made smaller. In other words, the principal point error range 108 can be made smaller than in the case shown in Fig. 10.

[0012] In this way, in the above method, the principal point error range 108 is reduced by using a wide-angle lens, and therefore it is possible to improve the accuracy of estimating the self-position.

[0013] However, wide-angle lenses generally have large aberrations (distortion aberration, magnification aberration, etc.), making it difficult to achieve high accuracy. Furthermore, it is necessary to use large, expensive lenses with well-corrected aberrations. Therefore, there is a demand for improving the accuracy of self-localization without using wide-angle lenses.

[0014] The present invention has been made in consideration of these circumstances, and aims to provide a self-position estimation device, a self-position estimation method, and a camera relative position adjustment method that can improve the accuracy of self-position estimation without using a wide-angle lens. [Means for solving the problem]

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

[0016] The self-position estimation device according to the first aspect includes an image acquisition unit that acquires multiple images of a group of targets using multiple cameras mounted on a probe head with different shooting directions, and a self-position estimation unit that detects the position and attitude of the probe head based on the multiple images.

[0017] In the self-location estimation device of the second aspect, in the first aspect, the self-location estimation unit has a feature point detection unit that detects feature points indicating the position of each target in a target group from a plurality of images, and an optimization calculation unit that uses each feature point detected by the feature point detection unit to determine, by optimization calculation, the position and attitude of the probe head that minimizes the reprojection error of each target in the target group.

[0018] In the self-location estimation device according to the third aspect, in the first or second aspect, the imaging directions of the multiple cameras are orthogonal to each other.

[0019] A self-location estimation device according to a fourth aspect is any one of the first to third aspects, and has two cameras.

[0020] The self-position estimation method according to the fifth aspect includes an image acquisition step of acquiring a plurality of images of a group of targets using a plurality of cameras mounted on a probe head and having different shooting directions, and a self-position estimation step of detecting the position and attitude of the probe head based on the plurality of images.

[0021] The camera relative position adjustment method according to the sixth aspect includes an image acquisition step of acquiring a plurality of images of a target group using a plurality of cameras mounted on a probe head and having different shooting directions, a relative position detection step of detecting the relative positions of the plurality of cameras based on the plurality of images, and an adjustment step of adjusting the positions and attitudes of the plurality of cameras based on the relative positions detected in the relative position detection step. [Effects of the Invention]

[0022] According to the present invention, it is possible to improve the accuracy of self-position estimation without using a wide-angle lens. [Brief explanation of the drawings]

[0023] [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. 10 is an explanatory diagram for explaining a camera projection model. [Figure 4] FIG. 2 is a diagram for explaining an imaging system of a probe head. [Figure 5] FIG. 5 is an exploded view of the imaging system shown in FIG. [Figure 6] FIG. 1 is a diagram showing a model of the positional relationship between the probe head, the camera, and the target group. [Figure 7] 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 8] FIG. 10 is a diagram showing a modified example of the present embodiment. [Figure 9] FIG. 10 is a diagram showing a case where there is no error in the target coordinates on the image plane. [Figure 10] FIG. 10 is a diagram illustrating a case where there is an error in the target coordinates on the image plane. [Figure 11] FIG. 10 illustrates how using a wide-angle lens reduces errors. DETAILED DESCRIPTION OF THE INVENTION

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

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

[0026] As shown in FIGS. 1 and 2, the self-localization system 10 includes a probe head 12, target groups 14A and 14B (sometimes referred to as "first target group 14A" and "second target group 14B"), 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 have at least some of the functions of the self-localization device 50. When there is no need to distinguish between the target groups 14A and 14B, the alphabet may be omitted and they may be referred to as "target group 14."

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

[0028] The probe head 12 has a self-position estimation function. To realize the self-position estimation function, the probe head 12 is equipped with two cameras 20A and 20B (sometimes referred to as "first camera 20A" and "second camera 20B"). The probe head 12 captures an image of a first target group 14A with the first camera 20A and captures an image of a second target group 14B with the second camera 20B, thereby enabling the self-position (position and attitude) of the probe head 12 to be estimated by a self-position estimation device 50, which will be described later. When the cameras 20A and 20B are not to be distinguished from each other, they may be referred to as "camera 20" without the alphabet.

[0029] As shown in FIG. 2, the target group 14 (14A, 14B) is a group of multiple (large numbers of) targets 24 arranged two-dimensionally. Each target 24 is formed in a dot or point shape, and is arranged with a gap between each target 24. Each target 24 may be formed of a small point light source (point light source) such as an LED. The relative positions of the targets 24 in the target group 14 are known. The shape and size of each target 24 in the target group 14 are also known.

[0030] The first target group 14A is positioned opposite the first camera 20A. The second target group 14B is positioned opposite the second camera 20B. This allows the cameras 20A and 20B to simultaneously capture images of the target groups 14A and 14B that are positioned opposite each other. Note that various fixing or supporting members may be used for the target groups 14A and 14B as long as they can be fixed or supported in predetermined positions.

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

[0032] The storage unit 54 stores control programs and various data. The storage unit 54 is configured, for example, by a hard disk drive (HDD: 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] 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 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 a control program stored in the storage unit 54, thereby functioning as an image acquisition unit 60, a self-position estimation unit 62, and a determination processing unit 64. The self-position estimation unit 62 includes a feature point detection unit 66 and an optimization calculation unit 68.

[0036] The image acquisition unit 60 is an example of an image acquisition unit of the present invention. The self-location estimation unit 62 is an example of a self-location estimation unit of the present invention. The feature point detection unit 66 is an example of a feature point detection unit of the present invention. The optimization calculation unit 68 is an example of an optimization calculation unit of the present invention.

[0037] [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. 3 is an explanatory diagram for explaining the camera projection model.

[0038] As shown in Figure 3, the world coordinate system Σ w is a coordinate system that represents a position in three-dimensional space (real space), and its origin is O w and X w Axis, Y w axis, Z w The world coordinate system Σ is a three-dimensional Cartesian coordinate system with the axes as its coordinate axes. w Any coordinate system may be used as long as it can identify a position in three-dimensional space (three-dimensional position). c is 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 orthogonal coordinate system with the axes s is the camera coordinate system Σ c Origin of 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.

[0039] First, the world coordinate system Σ of a point P (object point) in three-dimensional space w Coordinates (x w , yw , z w ) is expressed 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): c can be converted to coordinates (x, y, z) in

[0040]

number

[0041] where [R|t] w,c is the world coordinate system Σ w From the camera coordinate system Σ c is the transformation matrix (external parameter matrix) for transforming coordinates into the world coordinate system Σ w [R|t] represents the orientation and position of the camera 20 at the time of the w,c Each component r 11 , r 12 , …, r 33 , t x , t y , t z are called the extrinsic parameters of the camera.

[0042] Next, the camera coordinate system Σ c When point P, which is located at (x, y, z) as viewed from the left, is projected onto image plane IP, and the coordinates (pixel coordinates) of projected point Q are (u, v), the following relationships are established as shown in equations (2) to (7).

[0043]

number

[0044]

number

[0045]

number

[0046]

number

[0047]

number

[0048]

number

[0049] where (x´, y´) is the camera coordinate system Σ c represents the coordinates of the projected point obtained by projecting point P, which is located at (x, y, z) as seen from the camera 20, onto the normalized image plane (z=1). Also, (x″, y″) represents 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.

[0050] Also, f x , f y are the focal lengths in the x and y directions in pixels, and c x , c y is the image coordinate system Σ s where k1, k2, and k3 are radial distortion coefficients, and p1 and p2 are tangential distortion coefficients. 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.

[0051] [Probe head imaging system] Fig. 4 is a model diagram of the imaging system of the probe head 12 according to this embodiment. Fig. 5 is an exploded view of the imaging system shown in Fig. 4. In Figs. 4 and 5, image planes 30A and 30B indicate the image planes of cameras 20A and 20B, respectively.

[0052] The probe head 12 according to this embodiment includes an imaging system 22 made up of two cameras 20A and 20B whose imaging directions (optical axis directions) are orthogonal to each other. That is, as shown in FIG. 4, the imaging system 22 in the probe head 12 is divided into two image planes 30A and 30B (sometimes referred to as the "first image plane 30A" and the "second image plane 30B"), and the image planes 30A and 30B are orthogonal to each other. While the present embodiment illustrates a case in which the imaging directions of the cameras 20A and 20B are orthogonal to each other, the present invention is not limited to this. For example, as in a modified example described later (see FIG. 8), it is sufficient that the imaging directions of the cameras 20A and 20B are at least different from each other.

[0053] 5, when there is an error in the target coordinates on the first image plane 30A (coordinates indicating the position of the target 34A projected onto the first image plane 30A), the range that the camera principal point 32A can take is defined as a first principal point error range 36A. Also, when there is an error in the target coordinates on the second image plane 30B (coordinates indicating the position of the target 34B projected onto the second image plane 30B), the range that the camera principal point 32B can take is defined as a second principal point error range 36B. In this case, when the relative positions of the cameras 20A and 20B are adjusted so that at least a portion of the principal point error ranges 36A and 36B corresponding to the cameras 20A and 20B overlap, as shown in FIG. 4, an overlap range 38, which is the overlapping range of the first principal point error range 36A and the second principal point error range 36B, becomes the range that the head origin (the reference point of the probe head 12) can take. This allows the range of possible positions of the head origin to be smaller than when self-position estimation is performed using one camera.

[0054] However, in order to overlap the principal point error ranges 36A and 36B of the cameras 20A and 20B, it is important to understand in advance the relationship between the camera coordinate systems (rotation matrix R and translation vector t) based on the cameras 20A and 20B, and to adjust the relative positions of the cameras 20A and 20B. A method for adjusting the relative positions of the cameras 20A and 20B will be described below.

[0055] [Camera relative position adjustment method] Fig. 6 is a diagram modeling the positional relationship between the probe head 12, cameras 20A and 20B, and target groups 14A and 14B. Note that for ease of explanation, the positional relationship between the cameras 20A and 20B has been changed in Fig. 6, but this can also be applied to the positional relationship shown in Fig. 2 (a positional relationship in which the shooting directions of the cameras 20A and 20B are perpendicular to each other). Furthermore, for simplicity of explanation, it is assumed below that the cameras 20A and 20B only shoot images of the target groups 14A and 14B, respectively.

[0056] First, as shown in Figure 6, the world coordinate system Σ w The three-dimensional coordinates in the first camera coordinate system (coordinate system based on the first camera 20A) Σ c1 The transformation matrix for transforming coordinates into three-dimensional coordinates in the world coordinate system Σ w from the probe head coordinate system (coordinate system based on the probe head 12) Σ h via the first camera coordinate system Σ c1 and the world coordinate system Σ w to the first target group coordinate system (coordinate system based on the first target group 14A) Σ t1 via the first camera coordinate system Σ c1 There are two ways to do this:

[0057] Similarly, in the world coordinate system Σ w The three-dimensional coordinates in the second camera coordinate system (coordinate system based on the second camera 20B) Σ c2 The transformation matrix for transforming coordinates into three-dimensional coordinates in the world coordinate system Σ w from the probe head coordinate system Σ h via the second camera coordinate system Σ c2 and the world coordinate system Σ w to the second target group coordinate system (coordinate system based on the second target group 14B) Σ t2 via the second camera coordinate system Σ c2 There are two ways to do this:

[0058] These coordinate transformations can be expressed using coordinate transformation matrices (homogeneous transformation matrices) as shown in the following equations (8) and (9).

number

number

[0059] Here, the parameters (coordinate transformation matrices) of equations (8) and (9) are defined as follows (see FIG. 6): Due to limitations on notation in the specification, matrices and vectors may be written in thin type. T t :World coordinate system Σ w from the probe head coordinate system Σ h Coordinate transformation matrix to C c1 : Probe head coordinate system Σ h From the first camera coordinate system Σ c1 Coordinate transformation matrix to C c2 : Probe head coordinate system Σ h to the second camera coordinate system Σ c2 Coordinate transformation matrix to P p1 :World coordinate system Σ w to the first target group coordinate system Σ t1 Coordinate transformation matrix to P p2 :World coordinate system Σ w to the second target group coordinate system Σ t2 Coordinate transformation matrix to A c1,p1,t : First target group coordinate system Σ t1 From the first camera coordinate system Σ c1 Coordinate transformation matrix to A c2,p2,t : Second target group coordinate system Σ t2 to the second camera coordinate system Σ c2 Coordinate transformation matrix to

[0060] The subscripts attached to the parameters are as follows: where n represents the number assigned to the camera 20, which is 1 or 2 in this example. Subscript C n indicates that the parameter is dependent on the n-th camera 20. Subscript P n indicates that it is a parameter dependent on the nth target group 14. The subscript t indicates that the parameter is time- or scene-dependent.

[0061] In addition, 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 c1 , K. c2 is assumed to be known.

[0062] When the first target group 14A is photographed by the first camera 20A, the point (target point) of the target 24 of the first target group 14A projected onto the first image plane 30A is defined as x c1,p1,t (vector), and the point (target point) indicating the position of the corresponding target 24 on the first target group 14 is X p1 (vector), the relationship shown in the following equation (10) holds.

number

number

[0063] Here, when formula (8) is transformed, it becomes as shown in formula (12) below.

number

[0064] Substituting equation (12) into equation (10) gives the following equation (13).

number

number

[0065] Similarly, the following formula (15) holds true for the second camera 20B.

number

number

[0066] Here, the first target group coordinate system Σ t1 Target point X at p1 , the second target group coordinate system Σ t2 Target point X at p2 , the camera matrix K of the first camera 20A c1 , and the camera matrix K of the second camera 20B c2 is known. In addition, the target point x^ on the images captured by the first camera 20A and the second camera 20B is known. c1,p1,t , x^ c2,p2,t (vector)(target point x on image plane 30A, 30B) c1,p1,t , x c2,p2,t In the formulas below, the character with a "^" above the "x" is x^ c1,p1,t , x^ c2,p2,t is.

[0067] In equations (13) and (15), the world coordinate system Σ w from the probe head coordinate system Σ h Coordinate transformation matrix T to t , probe head coordinate system Σ h From the first camera coordinate system Σ c1 Coordinate transformation matrix C to c1 , world coordinate system Σ w to the first target group coordinate system Σ t1 Coordinate transformation matrix P to p1 Inverse matrix P p1 -1 , probe head coordinate system Σh to the second camera coordinate system Σ c2 Coordinate transformation matrix C to c2 , and the world coordinate system Σ w to the second target group coordinate system Σ t2 Coordinate transformation matrix P to p2 Inverse matrix P p2 -1 is unknown.

[0068] Since what is desired here is the relative position of the cameras 20A and 20B in the probe head 12, the unknown parameter (T t , C c1 , P p1 -1 , C c2 , P p2 -1 ) is calculated by an optimization calculation. Typically, various nonlinear solution methods such as the Newton-Raphson method or the nonlinear conjugate gradient method can be used for the optimization calculation, but since these methods are generally well known, they will not be described in detail here. Note that this error function E indicates the reprojection error of each target 24 in the target groups 14A and 14B onto the image planes 30A and 30B of the cameras 20A and 20B.

number

[0069] By performing the above optimization calculation, the probe head coordinate system Σ h From the first camera coordinate system Σ c1 Coordinate transformation matrix C to c1 and the probe head coordinate system Σ h to the second camera coordinate system Σ c2 Coordinate transformation matrix C to c2 These coordinate transformation matrices C c1 , C c2 are the probe head coordinate system Σ h 2 shows the positions and attitudes of the cameras 20A and 20B in the image plane.

[0070] In the above optimization calculation, intuitively, the probe head coordinate system Σ h , that is, the position and orientation of the probe head 12 are closer to each other (preferably, the probe head coordinate system Σ h The positions and orientations of the cameras 20A and 20B are adjusted so that the probe head coordinate system Σ h This allows the range of possible positions to be narrower than when one camera is used, which leads to improved accuracy in self-position estimation, which will be described later.

[0071] Furthermore, by the optimization calculation, the world coordinate system Σ w from the probe head coordinate system Σ h Coordinate transformation matrix T to t This coordinate transformation matrix T t is the world coordinate system Σ w This indicates the self-position (position and attitude) of the probe head 12 in the image. Therefore, it is possible to simultaneously detect the self-position of the probe head 12 and the relative positions of the cameras 20A and 20B.

[0072] [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. 7 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, the cameras 20A and 20B have already been calibrated, and the camera matrix K including the internal parameters (focal length, optical center) and distortion parameters (distortion coefficients) of the cameras 20A and 20B has already been calculated. c1 , K. c2 is assumed to be known.

[0073] 2, the target groups 14A and 14B are photographed by the cameras 20A and 20B mounted on the probe head 12, respectively (step S10). The images (target images) photographed by the cameras 20A and 20B are transmitted to the self-position estimation device 50. When the target images are transmitted to the self-position estimation device 50, the image acquisition unit 60 acquires the target images and stores them in the storage unit 54.

[0074] Next, the self-position estimation unit 62 estimates the position and attitude (self-position) of the probe head 12 based on the target images captured by the cameras 20A and 20B. The processing performed by the self-position estimation unit 62 will be described below.

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

[0076] Specifically, the feature point detection unit 66 reads the target image from the storage unit 54. Then, the feature point detection unit 66 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 planes 30A, 30B; 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. The feature point on the target image detected at this time is the target point x^ on the image described above. c1,p1,t , x^ c2,p2,t (vector).

[0077] Next, the optimization calculation unit 68 uses each feature point detected by the feature point detection unit 66 to find the position and orientation of the probe head 12 that minimizes the reprojection error of each target 24 in the target groups 14A and 14B by optimization calculation (step S14). Specifically, the optimization calculation unit 68 finds the unknown parameters (T t , C c1 , P p1 -1, C c2 , P p2 -1 ) is calculated by optimization calculation. As a result, the world coordinate system Σ w from the probe head coordinate system Σ h Coordinate transformation matrix T to t is detected.

[0078] The self-position estimation unit 62 stores information indicating the position and posture of the probe head 12 obtained as described above (probe head self-position information) in the storage unit 54. The 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.

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

[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 64 determines whether the self-position estimation process of the probe head 12 is completed (NO in step S16), and ends this flowchart.

[0081] The self-position estimation unit 62 can detect the relative positions of the cameras 20A and 20B as well as the self-position of the probe head 12 by determining the unknown parameters through the optimization calculation in the optimization calculation unit 68.

[0082] In this case, the self-position estimation unit 62 may determine whether or not an abnormality has occurred in the relative positions of the cameras 20A and 20B when detecting the self-position of the probe head 12 in step S14. For example, the self-position estimation unit 62 may compare the detected relative positions of the cameras 20A and 20B with a relative reference position of the cameras 20A and 20B stored in advance in the storage unit 54, and if the difference between the detected relative positions exceeds a predetermined threshold (tolerance), output abnormality notification information to the user indicating that an abnormality has occurred in the relative positions of the cameras 20A and 20B. This makes it possible to prompt the user to perform a detailed inspection of the cameras 20A and 20B mounted on the probe head 12.

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

[0084] According to this embodiment, the probe head 12 is provided with a plurality of cameras 20 arranged such that the imaging directions (optical axis directions) of the cameras 20 are different from one another. Then, the self-position (position and attitude) of the camera 20 is estimated using images (target images) of the target group 14 captured by each camera 20 of the probe head 12. This makes it possible to accurately estimate the self-position of the probe head 12 without using a wide-angle lens.

[0085] Furthermore, according to this embodiment, the relative positions of the cameras 20 are detected from the images of the target group 14 captured by each camera 20, and the cameras 20 are arranged to satisfy the desired positional relationship based on the detection results (i.e., the probe head coordinate system Σ h This leads to an improvement in the accuracy of estimating the self-position of the probe head 12.

[0086] Furthermore, in this embodiment, the number of cameras 20 mounted on the probe head 12 is two, but the present invention is not limited to this, and three or more cameras 20 may be mounted on the probe head 12.

[0087] In addition, in this embodiment, the imaging directions of the multiple cameras 20 mounted on the probe head 12 are perpendicular to each other, but this is not limited to this. For example, as shown in FIG. 8, the imaging directions of the cameras 20 may intersect at an oblique angle other than perpendicular.

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

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

[0090] Furthermore, in this embodiment, the target group 14 is divided and provided for each camera 20, but the target group 14 does not necessarily have to be divided.

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

[0092] DESCRIPTION OF SYMBOLS 10...self-position estimation system, 12...probe head, 14...target group, 14A...target group (first target group), 14B...target group (second target group), 18...probe, 20...camera, 20A...camera (first camera), 20B...camera (second camera), 24...target, 30A...first image plane, 30B...second image plane, 32A...camera principal point, 32B...camera principal point, 36A...first principal point error range, 36B...second principal point error range, 38...overlap range, 50...self-position estimation device, 52...arithmetic processing unit, 54...memory unit, 60...image acquisition unit, 62...self-position estimation unit, 64...determination processing unit, 66...feature point detection unit, 68...optimization calculation unit

Claims

1. an image acquisition unit that acquires a plurality of images of the target group using a plurality of cameras that are mounted on the probe head and have imaging directions different from each other; a self-position estimation unit that detects the position and orientation of the probe head based on the plurality of images; A self-location estimation device comprising:

2. The self-location estimation unit a feature point detection unit that detects feature points indicating the positions of each target of the target group from the plurality of images; an optimization calculation unit that uses each of the feature points detected by the feature point detection unit to determine, by optimization calculation, the position and orientation of the probe head that minimizes a re-projection error of each of the targets in the target group; The self-location estimation device according to claim 1 , further comprising:

3. The shooting directions of the multiple cameras are orthogonal to each other. The self-position estimation device according to claim 1 or 2.

4. The number of cameras is two. The self-position estimation device according to claim 3 .

5. an image acquisition step of acquiring a plurality of images of the target group using a plurality of cameras mounted on the probe head and having imaging directions different from each other; a self-position estimation step of detecting the position and orientation of the probe head based on the plurality of images; A self-location estimation method including:

6. an image acquisition step of acquiring a plurality of images of the target group using a plurality of cameras mounted on the probe head and having imaging directions different from each other; a relative position detection step of detecting relative positions of the plurality of cameras based on the plurality of images; an adjustment step of adjusting positions and attitudes of the plurality of cameras based on the relative positions detected in the relative position detection step; A camera relative position adjustment method including:

Images