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

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

The present invention relates to a self-position estimation device and method capable of estimating the self-position of a probe head, as well as a camera relative position adjustment method.

Traditionally, portable three-dimensional coordinate measuring machines (CMMs) have been mounted on robots to automatically measure large workpieces. Portable CMMs are beginning to meet requirements for accuracy of several tens of microns, but currently bridge-type CMMs are used for high-precision requirements of 10 microns or less.

Measuring methods using laser trackers and markers are also known as technologies for achieving accuracy of several tens of microns (see, for example, Patent Document 1). These methods are equipped with a base station that can swivel to adjust elevation and azimuth angles, and are also equipped with a distance sensor or camera to calculate the distance and attitude of the measuring head. With this measurement method, the movable range of the swivel mechanism and the measurement range of the distance sensor are long, making it possible to measure over a wide range.

However, with measurement methods using laser trackers or markers, 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.

Meanwhile, there is a known method for detecting a camera's own position (position and orientation) 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 correlate the position of each target in three-dimensional space with the position of each target in the captured image.

Japanese Patent Application Laid-Open No. 2020-148515 Japanese Patent Application Laid-Open No. 2022-30807

However, in a method of detecting 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, as explained below, the error will cause variation in the camera's principal point (the origin of the camera coordinate system), which will affect the accuracy of self-position estimation.

Figures 9 to 11 show how a target 100 in three-dimensional space is projected onto the image plane 102 of the camera. Figure 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). Figure 10 shows a case where there is an error in the target coordinates on the image plane 102. Figure 11 shows how using a wide-angle lens reduces the error in the target coordinates on the image plane 102.

With 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 Figure 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 will also be determined to be a single point without error.

However, as shown in Figure 10, if there is an error in the target coordinates on the image plane 102, the position of the camera principal point 106 calculated from the above correspondence will vary depending on the range of possible errors. In other words, due to the influence of errors in the target coordinates on the image plane 102, the position of the camera principal point 106 will vary within the principal point error range 108 shown in the hatched area in Figure 9. The camera's shooting direction (optical axis direction) is particularly affected.

On the other hand, using a wide-angle lens allows the targets 100 to be spaced farther apart, as shown in Figure 11. In this case, if the error in the target coordinates on the image plane 102 is the same as in Figure 10, the range of possible camera principal points 106 can be reduced. In other words, the principal point error range 108 can be made smaller than in the case shown in Figure 10.

In this way, in the above method, the principal point error range 108 is reduced by using a wide-angle lens, making it possible to improve the accuracy of self-position estimation.

However, wide-angle lenses generally have large aberrations (such as distortion and magnification aberration), 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-location estimation without using wide-angle lenses.

The present invention was 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.

To achieve the above objective, the present invention comprises the following aspects.

The self-location estimation device according to the first aspect includes an image acquisition unit that acquires multiple images of a target group 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 orientation of the probe head based on the multiple images.

In the self-location estimation device according to the second aspect, in the first aspect, the self-location estimation unit includes a feature point detection unit that detects feature points indicating the position of each target in the target group from multiple images, and an optimization calculation unit that uses each feature point detected by the feature point detection unit to perform optimization calculations to determine the position and orientation of the probe head that minimizes the reprojection error of each target in the target group.

The self-localization estimation device according to the third aspect is the first or second aspect, in which the shooting directions of the multiple cameras are orthogonal to each other.

The self-location estimation device according to the fourth aspect is any one of the first to third aspects, but has two cameras.

The self-location estimation method according to the fifth aspect includes an image acquisition step of acquiring multiple images of a target group using multiple cameras mounted on a probe head with imaging directions different from each other, and a self-location estimation step of detecting the position and orientation of the probe head based on the multiple images.

The camera relative position adjustment method according to the sixth aspect includes an image acquisition step of acquiring multiple images of a target group using multiple cameras mounted on a probe head and with imaging directions different from each other, a relative position detection step of detecting the relative positions of the multiple cameras based on the multiple images, and an adjustment step of adjusting the positions and attitudes of the multiple cameras based on the relative positions detected in the relative position detection step.

This invention makes it possible to improve the accuracy of self-position estimation without using a wide-angle lens.

1 is a block diagram showing a self-location estimation system according to an embodiment of the present invention; 1 is a schematic diagram illustrating a schematic configuration of a self-position estimation system according to an embodiment of the present invention. FIG. 10 is an explanatory diagram for explaining a camera projection model. FIG. 2 is a diagram for explaining an imaging system of a probe head. FIG. 5 is an exploded view of the imaging system shown in FIG. 4. FIG. 1 is a diagram showing a model of the positional relationship between the probe head, the camera, and the target group. 10 is a flowchart illustrating an example of a self-position estimation process executed by the self-position estimation device of the present embodiment. FIG. 10 is a diagram showing a modified example of the present embodiment. FIG. 10 is a diagram illustrating a case where there is no error in the target coordinates on the image plane. FIG. 10 is a diagram illustrating a case where there is an error in the target coordinates on the image plane. FIG. 10 illustrates how using a wide-angle lens reduces errors.

Embodiments of the present invention will be described below with reference to the accompanying drawings.

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

As shown in Figures 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. Note that when there is no need to distinguish between the target groups 14A and 14B, they may be referred to as "target group 14" without the alphabet.

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 contact (touch probe) or non-contact (laser, optical) as long as it is capable of measuring the three-dimensional coordinates of the workpiece. Examples of non-contact probes include laser scanners, point lasers, and line lasers. A user can hold the probe head 12 and perform measurements to obtain the three-dimensional coordinates of the workpiece at the measurement point.

The probe head 12 has a self-position estimation function. To achieve this function, the probe head 12 is equipped with two cameras 20A and 20B (sometimes referred to as the "first camera 20A" and the "second camera 20B"). The probe head 12 captures an image of the first target group 14A with the first camera 20A and an image of the second target group 14B with the second camera 20B, allowing the self-position (position and attitude) of the probe head 12 to be estimated by the self-position estimation device 50, described below. When there is no need to distinguish between the cameras 20A and 20B, they may be referred to as "camera 20" without the alphabet.

As shown in FIG. 2, the target group 14 (14A, 14B) is a two-dimensional arrangement of multiple (large numbers of) targets 24. 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 also be composed 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.

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 each camera 20A, 20B to simultaneously capture images of the target groups 14A, 14B, which are positioned opposite each other. Note that various fixing or supporting members may be used for the target groups 14A, 14B, as long as they can be fixed or supported in their respective predetermined positions.

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

The memory unit 54 stores control programs and various data. The memory unit 54 is configured, for example, by a hard disk drive (HDD) or a semiconductor memory device (SSD: Solid State Drive). The memory unit 54 may also include a temporary storage element configured, for example, by RAM (Random Access Memory) such as DRAM (Dynamic Random Access Memory) or SRAM (Static Random Access Memory), and may function as a working area for the arithmetic processing unit 52.

The memory unit 54 stores target images, which will be described later. The memory unit 54 also stores information (target group information) regarding the shape, size, and arrangement of each target 24 that makes up the target group 14.

The arithmetic processing unit 52 executes various arithmetic processes performed by the self-location estimation device 50. The arithmetic processing unit 52 has an arithmetic circuit composed of various processors, memory, 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)). 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.

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

Note that the image acquisition unit 60 is an example of an image acquisition unit of the present invention. The self-position estimation unit 62 is an example of a self-position 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.

[Camera projection model]
Before describing the self-location estimation process executed by the self-location estimation device 50 of this embodiment, a 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.

As shown in FIG. 3 , the world coordinate system (global coordinate system) _Σw is a coordinate system that represents a position in a three-dimensional space (real space). It is a three-dimensional Cartesian coordinate system with an origin _Ow and mutually orthogonal _Xw , _Yw , and _Zw axes as coordinate axes. Note that any coordinate system may be used for the world coordinate system _Σw as long as it can identify a position in three-dimensional space (three-dimensional position). The camera coordinate system _Σc is a three-dimensional Cartesian coordinate system with an origin _Oc at the optical axis center of the camera 20, an _Xc axis extending to the right of the origin _Oc , a _Yc axis extending downward, and a _{Zc axis} extending in the optical axis direction. The image coordinate system _Σs is a two-dimensional Cartesian coordinate _system (pixel coordinate system) with an origin at the upper left of the image plane IP, which is a focal length f away from the _origin Oc of the camera coordinate system Σc in the _Zc direction, and a U axis and a V axis parallel to the _Xc axis and the Yc axis, respectively.

First, the coordinates (xw, yw, _zw ) of a point P (object point) in three-dimensional space in the world coordinate system _Σw can be transformed into coordinates ( _x , _y , z) in the camera coordinate system _Σc using the rotation matrix R and translation vector t of the camera 20, as shown in the following equation (1).

Here, [R|t] _w,c is a transformation matrix (extrinsic parameter matrix) for coordinate transformation from the world coordinate system _Σw to the camera coordinate system _Σc , and represents the attitude and position of the camera 20 in the world coordinate system _Σw . The components _r11 , _r12 , ..., _r33 , _tx , _ty , and _tz of [R|t] _w,c are called extrinsic parameters of the camera.

Next, when a point P at (x, y, z) viewed from the camera coordinate system _Σc is projected onto the image plane IP, and the coordinates (pixel coordinates) of the projected point Q are (u, v), the following relational expressions (2) to (7) hold:

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 _Σc , 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.

Furthermore, _fx and _fy indicate focal lengths in the x and y directions expressed in pixel units. Furthermore, _cx and _cy indicate the optical center in the image coordinate system _Σs (the position where the optical axis of the camera 20 intersects with the image plane IP, the optical center in pixel units). Furthermore, _k1 , _k2 , and _k3 are radial distortion coefficients, and _p1 and _p2 are tangential distortion coefficients. In this specification, the focal lengths _fx and _fy and the optical centers _cx and _cy are referred to as internal parameters of the camera 20, and the distortion coefficients _k1 , _k2 , _k3 , _p1 , and _p2 are referred to as distortion parameters of the camera 20.

[Probe head imaging system]
Fig. 4 is a diagram showing a model of the imaging system of the probe head 12 according to this embodiment. Fig. 5 is a diagram showing 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.

The probe head 12 according to this embodiment is equipped with an imaging system 22 consisting of two cameras 20A, 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, 30B (sometimes referred to as the "first image plane 30A" and the "second image plane 30B"), with the image planes 30A, 30B being perpendicular to each other. While this embodiment illustrates a case in which the imaging directions of the cameras 20A, 20B are perpendicular to each other, this is not limiting. It is sufficient that at least the imaging directions of the cameras 20A, 20B are different from each other, as in the modified example described below (see FIG. 8).

Here, as shown in FIG. 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 the 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 the second principal point error range 36B. In this case, as shown in FIG. 4, 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, the overlapping range 38, which is the range where the first principal point error range 36A and the second principal point error range 36B overlap, 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 head origin positions to be smaller than when self-position estimation is performed using a single camera.

However, in order to overlap the principal point error ranges 36A and 36B of each camera 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 each camera 20A and 20B, and to adjust the relative positions of each camera 20A and 20B. Below, we will explain how to adjust the relative positions of each camera 20A and 20B.

[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 (where the shooting directions of the cameras 20A and 20B are orthogonal 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.

First, as shown in FIG. 6 , there are two types of transformation matrices for transforming three-dimensional coordinates in the world coordinate system _Σw into three-dimensional coordinates in the first camera coordinate system _Σc1 (a coordinate system based on the first camera 20A): one for transforming from the world coordinate system _Σw to _the first camera coordinate system _Σc1 via the probe head coordinate system (a coordinate system based on the probe head 12) Σh, and one for transforming from the world coordinate system _Σw to the first camera coordinate system _Σc1 via the first target group coordinate system (a coordinate system based on the first target group 14A) _Σt1 .

Similarly, there are two types of transformation matrices for transforming three-dimensional coordinates in the world coordinate system _Σw into three-dimensional coordinates in the second camera coordinate system _Σc2 (a coordinate system based on the second camera 20B): one for transforming from the world coordinate system _Σw to the second camera coordinate system _Σc2 via the probe head coordinate system _Σh , and one for transforming from the world coordinate system _Σw to the second camera coordinate system _Σc2 via the second target group coordinate system (a coordinate system based on the second target group 14B) _Σt2 .

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

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 _t : coordinate transformation matrix from world coordinate system _Σw to probe head coordinate system _Σh C _c1 : coordinate transformation matrix from probe head coordinate system _Σh to first camera coordinate system _Σc1 C _c2 : coordinate transformation matrix from probe head coordinate system _Σh to second camera coordinate system _Σc2 P _p1 : coordinate transformation matrix from world coordinate system _Σw to first target group coordinate system _Σt1 P _p2 : coordinate transformation matrix from world coordinate system _Σw to second target group coordinate system _Σt2 A _c1,p1,t : coordinate transformation matrix from first target group coordinate system _Σt1 to first camera coordinate system _Σc1 A _c2,p2,t : coordinate transformation matrix from second target group coordinate system _Σt2 to second camera coordinate system _Σc2

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.
The subscript C _n indicates that the parameter is dependent on the nth camera 20 .
The subscript P _n indicates that the parameter is dependent on the nth target group 14 .
The subscript t indicates that the parameter is time or scene dependent.

It is also assumed that the camera 20 has already been calibrated, and the camera matrices K _c1 and K _c2 including the internal parameters (focal length, optical center) and distortion parameters (distortion coefficients) of the camera 20 are known.

When the first target group 14A is photographed by the first camera 20A, if the point (target point) at which a target 24 in the first target group 14A is 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 defined as X _p1 (vector), then the relationship shown in the following equation (10) holds.
However, the following relationship exists:

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

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

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

Here, target point _Xp1 in the first target group coordinate system _Σt1 , target point Xp2 in the second target group coordinate system _Σt2 , camera matrix _Kc1 of first camera 20A, and camera matrix _Kc2 of second camera 20B are known. Also, target points x^ _{c1 ,p1,t} and x^ _c2,p2,t (vectors) on the images captured by first camera 20A and second camera 20B, respectively, are known (equivalent to target points xc1 _,p1,t and xc2 _,p2,t on image planes 30A and 30B). In the formulas below, the character with a "^" above "x" represents x^ _c1,p1,t and x^ _c2,p2,t .

In equations (13) and (15), the coordinate transformation matrix T _t from the world coordinate system _Σw to the probe head coordinate system _Σh , the coordinate transformation matrix C _c1 from the probe head coordinate system _Σh to the first camera coordinate system _Σc1 , the inverse matrix P _p1 ^-1 of the coordinate transformation matrix P _p1 from the world coordinate system _Σw to the first target group coordinate system Σt1, the coordinate transformation matrix C _c2 from the probe head coordinate system _Σh to the second camera coordinate system _Σc2 , and the inverse matrix P _{p2 -1} of the coordinate transformation matrix P _p2 from the world coordinate system _Σw to the second target group coordinate ^system _Σt2 are unknown.

Since what is desired here is the relative positions of the cameras 20A and 20B in the probe head 12, the unknown parameters (T _t , C _c1 , P _p1 ^-1 , C _c2 , P _p2 ^-1 ) that minimize the value of the error function E shown in the following equation (17) are found by optimization calculation. Typically, various nonlinear solution methods such as the Newton-Raphson method and the nonlinear conjugate gradient method can be used for optimization calculation, but since these are generally well-known methods, 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.

By performing the above optimization calculation, a coordinate transformation matrix _Cc1 from the probe head coordinate system Σh to the first camera coordinate system _Σc1 and a coordinate transformation matrix _Cc2 from the probe head coordinate system _Σh to the second camera coordinate system _Σc2 are obtained. These coordinate transformation matrices _Cc1 and _Cc2 indicate the positions and orientations of the cameras 20A and 20B in the probe head coordinate system _{Σh ,} respectively.

Intuitively, in the optimization calculation, the positions and orientations of the cameras 20A and 20B are adjusted based on the positions and orientations of the cameras 20A and 20B so that the probe head coordinate systems Σ _h calculated back from each of the cameras 20A and 20B, i.e., the position and orientation of the probe head 12, approach each other (preferably so that the probe head coordinate systems Σ _h calculated back from each of the cameras 20A and 20B approximately match). This makes it possible to narrow the range of the probe head coordinate system Σ _h compared to when one camera is used, which leads to improved accuracy of self-position estimation, which will be described later.

Furthermore, the optimization calculation can also determine the coordinate transformation matrix _Tt from the world coordinate system _Σw to the probe head coordinate system _Σh , along with the unknown parameters. This coordinate transformation matrix _Tt indicates the self-position (position and orientation) of the probe head 12 in the world coordinate system _Σw . Therefore, it becomes possible to simultaneously detect the self-position of the probe head 12 and the relative positions of the cameras 20A and 20B.

[Self-position estimation method]
Next, a description will be given of a processing procedure (an example of a self-location estimation method) for 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, it is assumed that the cameras 20A and 20B have already been calibrated, and the camera matrices _Kc1 and _Kc2 including the internal parameters (focal length, optical center) and distortion parameters (distortion coefficients) of the cameras 20A and 20B are known.

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

Next, the self-position estimation unit 62 estimates the position and orientation (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 is described below.

First, the feature point detection unit 66 performs a feature point detection process to detect multiple feature points from the target image (step S12).

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 (such as grayscale conversion) on the read target image, and then detects feature points (image points) indicating the position of each target 24 from the target image, and determines the coordinates (pixel coordinates) of each feature point on the target image (image planes 30A and 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 points on the target image detected at this time correspond to the target points x^ _c1,p1,t and x^ _c2,p2,t (vectors) on the image described above.

Next, the optimization calculation unit 68 uses each feature point detected by the feature point detection unit 66 to determine, 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, 14B (step S14). Specifically, the optimization calculation unit 68 determines, by optimization calculation, the unknown parameters (T _t , C _c1 , P _p1 ^-1 , C _c2 , P _p2 ^-1 ) that minimize the error function E shown in the above-mentioned equation (17). As a result, a coordinate transformation matrix T _t from the world coordinate system Σ _w to the probe head coordinate system Σ _h is detected as information indicating the self-position of the probe head 12.

The self-position estimation unit 62 stores information indicating the position and orientation of the probe head 12 determined as described above (probe head self-position information) in the memory unit 54. The probe head self-position information stored in the memory unit 54 is used when calculating the three-dimensional coordinates of the workpiece using the probe head 12.

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 the self-position estimation process of the probe head 12 should be continued (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.

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 has ended (NO in step S16), and ends this flowchart.

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 calculating the unknown parameters described above through the optimization calculation in the optimization calculation unit 68.

In this case, when detecting the self-position of the probe head 12 in step S14, the self-position estimation unit 62 may also determine whether an abnormality has occurred in the relative position of the cameras 20A and 20B. 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 pre-stored in the memory unit 54, and if the difference between the two exceeds a pre-set threshold (tolerance) as a result of the comparison, it may output abnormality notification information to the user indicating that an abnormality has occurred in the relative position 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.

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

According to this embodiment, the probe head 12 is equipped with multiple cameras 20 arranged so that the imaging directions (optical axis directions) of the cameras 20 are different from each other. Images (target images) of the target group 14 captured by each camera 20 of the probe head 12 are then 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.

Furthermore, according to this embodiment, the relative positions of the cameras 20 can be detected from the images of the target group 14 captured by each camera 20, and the positions and attitudes of the cameras 20 can be adjusted based on the detection results so that the cameras 20 satisfy the desired positional relationship (i.e., so that the probe head coordinate system Σ _h calculated back from each camera 20 approaches each other). This leads to an improvement in the estimation accuracy of the self-position of the probe head 12.

Furthermore, in this embodiment, the number of cameras 20 mounted on the probe head 12 is two, but this is not limited thereto, and three or more cameras 20 may be mounted on the probe head 12.

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

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 motion of the robot arm, thereby measuring the three-dimensional coordinates of the workpiece using the probe head 12.

In addition, in this embodiment, the target group 14 is configured as a dot pattern in which multiple targets 24 formed in a dot or point shape are arranged two-dimensionally. However, this is not limited to this. For example, the target group 14 may be configured as a grid pattern or checkered pattern as disclosed in 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).

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.

The above describes embodiments of the present invention, but the present invention is not limited to these examples, and various improvements and modifications may be made without departing from the spirit of the present invention.

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... Computation 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 (6)

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: 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: The shooting directions of the multiple cameras are orthogonal to each other.
The self-position estimation device according to claim 1 or 2. The number of cameras is two.
The self-position estimation device according to claim 3 . 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: 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: