Own-position estimating device, own-position estimating method, and camera relative position adjusting method

Abstract

Provided are an own-position estimating device and an own-position estimating method capable of improving the precision of own-position estimation without using a wide-angle lens. An own-position estimating device (50) comprises: an image acquiring unit (60) that acquires a plurality of images obtained by imaging target groups (14A, 14B) using a plurality of cameras (20A, 20B), respectively, that are mounted on a probe head (12) and that have mutually different imaging directions; and an own-position estimating unit (62) that detects the position and orientation of the probe head (12) on the basis of the plurality of images.

Description

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

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

[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] 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 determined 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, when 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 reduced. 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 (such as distortion and magnification aberration), making it difficult to achieve high accuracy. Furthermore, they require the use of 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.

[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 a plurality of images of a group of targets using a plurality of cameras mounted on a probe head and having different imaging directions, and a self-position estimation unit that detects the position and attitude of the probe head based on the plurality of images.

[0017] In the self-position estimation device of the second aspect, in the first aspect, the self-position estimation unit has a feature point detection unit that detects feature points indicating the position of each target in the 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] A self-location estimation device according to a third aspect is the first or second aspect, wherein the imaging directions of the plurality of cameras are orthogonal to each other.

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

[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 imaging directions different from each other, 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 imaging directions different from each other, 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.

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

[0023] 5 is a block diagram showing a self-location estimation system according to the present embodiment. FIG. 6 is a schematic configuration diagram showing a schematic configuration of the self-location estimation system according to the present embodiment. FIG. 7 is an explanatory diagram for explaining a camera projection model. FIG. 8 is a diagram for explaining an imaging system of a probe head. FIG. 9 is an exploded view of the imaging system shown in FIG. 4. FIG. 10 is a diagram modeling the positional relationship between the probe head, the camera, and a group of targets. FIG. 11 is a flowchart showing an example of a self-location estimation process executed by the self-location estimation device of the present embodiment. FIG. 12 is a diagram showing a modified example of the present embodiment. FIG. 13 is a diagram showing a case where there is no error in the target coordinates on the image plane. FIG. 14 is a diagram showing a case where there is an error in the target coordinates on the image plane. FIG. 15 is a diagram showing how errors are reduced when a wide-angle lens is used.

[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-location estimation 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-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 have at least some of the functions of the self-location estimation 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 photographs the first target group 14A with the first camera 20A and the 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-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. 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) or a semiconductor storage device (SSD: Solid State Drive). The storage unit 54 may include a temporary storage element configured, for example, by a random access memory (RAM) such as a dynamic random access memory (DRAM) or a static random access memory (SRAM), and may function as a work area for the arithmetic processing unit 52.

[0033] The storage unit 54 stores target images, 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 14.

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

[0035] The calculation processing unit 52 reads and executes the 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 describing the camera projection model.

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

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

[0040]

[0041] Here, [R | t] w,c is the world coordinate system Σ ｗ From the camera coordinate system Σ ｃ is the transformation matrix (external parameter matrix) for transforming coordinates into the world coordinate system Σ ｗ [R|t] represents the posture and position of the camera 20 at 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 Σ ｃ When a point P at (x, y, z) viewed from the left is projected onto an image plane IP, and the coordinates (pixel coordinates) of the projected point Q are (u, v), the following relationships shown in equations (2) to (7) hold.

[0043]

[0044]

[0045]

[0046]

[0047]

[0048]

[0049] Here, (x', y') is the camera coordinate system Σ ｃ represents the coordinates of the projected point obtained by projecting point P, which is located at (x, y, z) as viewed 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 ｘ , f ｙdenotes the focal length in the x and y directions expressed in pixels, and c ｘ , c ｙ is the image coordinate system Σ ｓ , where the optical axis of the camera 20 intersects with the image plane IP, i.e., 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.

[0051] [Imaging System of Probe Head] 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 the cameras 20A and 20B, respectively.

[0052] The probe head 12 according to this embodiment includes an imaging system 22 including two cameras 20A and 20B whose imaging directions (optical axis directions) are perpendicular 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 perpendicular to each other. While the present embodiment illustrates a case in which the imaging directions of the cameras 20A and 20B are perpendicular 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 can be taken by the camera principal point 32A 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 can be taken by the camera principal point 32B 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 can be taken by the head origin (the reference point of the probe head 12). 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] [Method for Adjusting Relative Camera Positions] 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 method can be similarly 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 FIG. 6, the world coordinate system Σ ｗ The three-dimensional coordinates in the first camera coordinate system (a coordinate system based on the first camera 20A) Σ ｃ１The transformation matrix for transforming coordinates into three-dimensional coordinates in the world coordinate system Σ ｗ from the probe head coordinate system (coordinate system based on the probe head 12) Σ ｈ via the first camera coordinate system Σ ｃ１ and the world coordinate system Σ ｗ to the first target group coordinate system (a coordinate system based on the first target group 14A) Σ ｔ１ via the first camera coordinate system Σ ｃ１ There are two ways to do this:

[0057] Similarly, in the world coordinate system Σ ｗ The three-dimensional coordinates in the second camera coordinate system (a coordinate system based on the second camera 20B) Σ ｃ２ The transformation matrix for transforming coordinates into three-dimensional coordinates in the world coordinate system Σ ｗ from the probe head coordinate system Σ ｈ via the second camera coordinate system Σ ｃ２ and the world coordinate system Σ ｗ to the second target group coordinate system (coordinate system based on the second target group 14B) Σ ｔ２ via the second camera coordinate system Σ ｃ２ 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).

[0059] Here, the parameters (coordinate transformation matrices) of equations (8) and (9) are defined as follows (see FIG. 6). Note that due to limitations on notation in the specification, matrices and vectors may be written in thin type. T ｔ : World coordinate system Σ ｗ from the probe head coordinate system Σ ｈ Coordinate transformation matrix C ｃ１ : Probe head coordinate system Σ ｈ From the first camera coordinate system Σ ｃ１ Coordinate transformation matrix C ｃ２ : Probe head coordinate system Σ ｈ From the second camera coordinate system Σ ｃ２Coordinate transformation matrix P ｐ１ : World coordinate system Σ ｗ From the first target group coordinate system Σ ｔ１ Coordinate transformation matrix P ｐ２ : World coordinate system Σ ｗ From the second target group coordinate system Σ ｔ２ Coordinate transformation matrix A ｃ１,ｐ１,ｔ : First target group coordinate system Σ ｔ１ From the first camera coordinate system Σ ｃ１ Coordinate transformation matrix A ｃ２,ｐ２,ｔ : Second target group coordinate system Σ ｔ２ From the second camera coordinate system Σ ｃ２ Coordinate transformation matrix to

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

[0061] Furthermore, 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 ｃ１ , K. ｃ２ 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 ｃ１,ｐ１,ｔ (vector), and the point (target point) indicating the position of the corresponding target 24 on the first target group 14 is defined as X ｐ１ (vector), the relationship shown in the following equation (10) holds. However, the following relationship exists:

[0063] Here, when equation (8) is transformed, it becomes as shown in the following equation (12).

[0064] Substituting equation (12) into equation (10) yields the following equation (13). However, the following relationship exists:

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

[0066] Here, the first target group coordinate system Σ ｔ１ Target point X at ｐ１ , the second target group coordinate system Σ ｔ２ Target point X at ｐ２ , the camera matrix K of the first camera 20A ｃ１ , and the camera matrix K of the second camera 20B ｃ２ is known. In addition, the target point x^ on the images captured by the first camera 20A and the second camera 20B is known. ｃ１,ｐ１,ｔ , x^ ｃ２,ｐ２,ｔ (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^ ｃ１,ｐ１,ｔ , x^ ｃ２,ｐ２,ｔ is.

[0067] In equations (13) and (15), the world coordinate system Σ ｗ from the probe head coordinate system Σ ｈ Coordinate transformation matrix T to ｔ , probe head coordinate system Σ ｈ From the first camera coordinate system Σ ｃ１ Coordinate transformation matrix C to ｃ１ , world coordinate system Σ ｗ From the first target group coordinate system Σ ｔ１ Coordinate transformation matrix P to ｐ１ Inverse matrix P ｐ１ －１ , probe head coordinate system Σ ｈ From the second camera coordinate system Σ ｃ２ Coordinate transformation matrix C to ｃ２ , and the world coordinate system Σ ｗ From the second target group coordinate system Σｔ２ Coordinate transformation matrix P to ｐ２ Inverse matrix P ｐ２ －１ is unknown.

[0068] Since what is desired to be found here is the relative position of the cameras 20A and 20B in the probe head 12, the unknown parameter (T ｔ , C ｃ１ , P ｐ１ －１ , C ｃ２ , P ｐ２ －１ ) 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.

[0069] By performing the above optimization calculation, the probe head coordinate system Σ ｈ From the first camera coordinate system Σ ｃ１ Coordinate transformation matrix C to ｃ１ and the probe head coordinate system Σ ｈ From the second camera coordinate system Σ ｃ２ Coordinate transformation matrix C to ｃ２ These coordinate transformation matrices C ｃ１ , C ｃ２ are the probe head coordinate system Σ ｈ 2 shows the positions and orientations of the cameras 20A and 20B at

[0070] In the optimization calculation, intuitively, the probe head coordinate system Σ ｈ , that is, the position and orientation of the probe head 12 are closer to each other (preferably, the probe head coordinate system Σ ｈ The positions and orientations of the cameras 20A and 20B are adjusted so that the probe head coordinate system Σｈ 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 Σ ｗ from the probe head coordinate system Σ ｈ Coordinate transformation matrix T to ｔ This coordinate transformation matrix T ｔ is the world coordinate system Σ ｗ 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-Location Estimation Method] Next, a process procedure (one 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. 7 is a flowchart showing one 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 a 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. ｃ１ , K. ｃ２ 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. ｃ１,ｐ１,ｔ , x^ ｃ２,ｐ２,ｔ (vector).

[0077] Next, the optimization calculation unit 68 uses each feature point detected by the feature point detection unit 66 to find, by optimization calculation, 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 (step S14). Specifically, the optimization calculation unit 68 finds the unknown parameters (T ｔ , C ｃ１ , P ｐ１ －１ , C ｃ２ , P ｐ２ －１ ) is calculated by optimization. As a result, the world coordinate system Σ ｗ from the probe head coordinate system Σ ｈ Coordinate transformation matrix T to ｔ is detected.

[0078] The self-position estimation unit 62 stores information indicating the position and attitude 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, the self-position estimation process of the probe head 12 is continuously and repeatedly performed while the probe head 12 is measuring the three-dimensional coordinates of the workpiece.

[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 described above 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 value) as a result of the comparison, 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] [Effects] 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 so that the imaging directions (optical axis directions) of the cameras 20 are different from one another. Images (target images) of the target group 14 captured by each camera 20 of the probe head 12 are used to estimate the self-position (position and attitude) of the camera 20. 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 a desired positional relationship based on the detection results (i.e., the probe head coordinate system Σ ｈ 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 this is not limitative, 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. However, the invention 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 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.

[0089] In addition, 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.

[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...Overlapping range, 50...Self-position estimation device, 52...Calculation 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. A self-location estimation device comprising: an image acquisition unit that acquires multiple images of a group of targets using multiple cameras mounted on a probe head with imaging directions different from each other; and a self-location estimation unit that detects the position and attitude of the probe head based on the multiple images.

2. The self-position estimation device according to claim 1, wherein the self-position estimation unit comprises: a feature point detection unit that detects feature points indicating the position of each target in the target group from the plurality of images; and 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 attitude of the probe head that minimizes the reprojection error of each target in the target group.

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

4. The self-location estimation device according to claim 3, wherein the number of cameras is two.

5. A self-location estimation method comprising: 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 with imaging directions different from each other; and a self-location estimation step of detecting the position and attitude of the probe head based on the plurality of images.

6. A camera relative position adjustment method comprising: 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 imaging directions different from each other; 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.

Images