Host-position estimation device and host-position estimation method

Abstract

The present invention provides a host-position estimation device and a host-position estimation method with which it is possible to estimate the host position of a camera with high accuracy without being affected by a refraction error caused by a light-transmitting part arranged in front of a target. A host-position estimation device (50) comprises: a host-position estimation unit (62) that estimates the host position of a camera (20) on the basis of an image obtained by capturing, with the camera (20), a target group (16) in which a transparent window material (30) is arranged in front of a plurality of targets (24); a line-of-sight detection unit (68) that detects a line of sight (L) from the principal point of the camera (20) toward the targets (24) with the transparent window material (30) interposed therebetween on the basis of the host position of the camera (20) estimated by the host-position estimation unit (62); and a correction unit (70) that corrects the host position of the camera (20) on the basis of the distance between the line of sight (L) and the targets (24).

Description

Self-position estimation device and self-position estimation method

[0001] The present invention relates to a self-position estimation device and a self-position estimation method capable of estimating the self-position of a camera.

[0002] Traditionally, portable three-dimensional coordinate measuring machines (CMMs) have been mounted on robots to automatically measure large workpieces. While portable CMMs are meeting accuracy requirements of several tens of micrometers, bridge-type CMMs are currently being used to address high-precision requirements of 10 micrometers or less.

[0003] Furthermore, measurement methods using laser trackers or markers are known as techniques to achieve an accuracy of several tens of micrometers (see, for example, Patent Document 1). These methods include a base station equipped with an elevation and azimuth swivel function, and further equipped with a distance sensor or camera to calculate the distance and orientation of the measurement head. With this measurement method, because the movable range of the swivel mechanism and the measuring range of the distance sensor are long, measurements can be taken over a wide area.

[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 the limitations of the angle accuracy of the swivel mechanism.

[0005] On the other hand, a method is known for estimating the camera's own position (position and orientation) from images taken with a camera of targets whose positions in three-dimensional space are known (see, for example, Patent Document 2). This method has the advantage of being able to easily obtain high accuracy (5 μm or less) with a relatively simple mechanism by relating the position of each target in three-dimensional space with the position of each target in the captured image and performing calculations.

[0006] Japanese Patent Publication No. 2020-148515 Japanese Patent Publication No. 2022-30807

[0007] Incidentally, in the target plate (target group composed of a plurality of targets) used in the above method, a transparent window material (light transmission part) such as a firing material may be arranged on the front surface of the target for antifouling or the like. In this case, an error occurs in the position of the target (target coordinates) on the image captured by the camera due to the influence of refraction by the transparent window material. As a result, there is a problem that the estimation accuracy of the self-position of the camera decreases.

[0008] FIG. 7 is a diagram for explaining the refraction of light. As shown in FIG. 7, when light is incident on the boundary of media having different refractive indices, the traveling direction of the light changes due to refraction. Here, the incident angle with respect to the normal line of the boundary surface is θ Ａ and the refraction angle is θ Ｂ Let it be. Also, the absolute refractive index of the medium on the incident side is n Ａ and the absolute refractive index of the medium on the transmission side is n Ｂ Let it be. Also, the speed of light in the medium on the incident side is v Ａ and the speed of light in the medium on the transmission side is v Ｂ Let it be. Also, since the frequency f of the wave does not change before and after refraction, from the formula v = fλ representing the speed of the wave using the wavelength λ, for the speed of light in the media with refractive indices n Ａ , n Ｂ , v Ａ = fλ Ａ , v Ｂ = fλ Ｂ holds. Then, from Snell's law, the following formula (1) holds. Note that n ＡＢ is the ratio of the absolute refractive indices of the two media (referred to as "relative refractive index").

[0009] FIG. 8 is a graph showing the relationship between the incident angle θ Ａ and the refraction angle θ Ｂ . In FIG. 8, the horizontal axis represents the incident angle θ Ａ and the vertical axis represents the difference between the refraction angle θ Ｂ and the incident angle θ Ａ . Here, as an example, a graph in the case where the medium on the incident side is air (n Ａ ≈ 1.00) and the medium on the transmission side is glass (n Ｂ ≈ 1.44) is shown. As shown in FIG. 8, the incident angle θ ＡThe larger the angle of refraction θ becomes (that is, the further the incident light on the interface is from perpendicular to the surface), the greater the angle of refraction θ becomes. Ｂ and the angle of incidence θ Ａ The difference (absolute value) between the two points increases, indicating that the error due to refraction increases.

[0010] Figure 9 is a schematic diagram showing the line of sight from camera 120 to each target 124. As shown in Figure 9, when camera 120 photographs multiple targets 124 through the transparent window material 130, the amount of refraction by the transparent window material 130 (the difference between the angle of refraction and the angle of incidence) differs depending on the direction of the line of sight from camera 120 to each target 124.

[0011] Figure 10 is a diagram illustrating the effect of refraction error caused by a transparent window material. In Figure 10, Image 100A is an image of the target plate without a transparent window material. Image 100B is an image of the target plate with a transparent window material (in this example, a 5 mm thick glass material). Image 100C is a composite image obtained by superimposing Images 100A and 100C, each made semi-transparent.

[0012] As shown in Figure 10, when the camera is tilted relative to the target plate during shooting, the direction and amount of shift in the target coordinates differ for each line of sight to each target due to differences in the direction and angle of incidence to the transparent window material. In other words, if a transparent window material (light-transmitting part) that causes refraction is placed between the camera and the target, the amount of error due to refraction differs depending on the direction and angle of the line of sight (light) from the camera to each target. As a result, errors occur in the position of the targets in the image captured by the camera, which reduces the accuracy of the camera's self-position estimation.

[0013] This invention has been made in view of these circumstances, and aims to provide a self-position estimation device and a self-position estimation method that can perform high-precision self-position estimation of a camera without being affected by refraction errors caused by a light-transmitting part placed in front of the target.

[0014] To achieve the above objective, the present invention comprises the following embodiments.

[0015] The self-position estimation device according to the first embodiment includes: a self-position estimation unit that estimates the self-position of a camera based on an image taken by a camera of a group of targets in which light-transmitting parts are arranged in front of a plurality of targets; a line-of-sight detection unit that detects a line of sight from the principal point of the camera toward the target through the light-transmitting parts based on the self-position of the camera estimated by the self-position estimation unit; and a correction unit that corrects the self-position of the camera based on the distance between the line of sight and the target.

[0016] In the second embodiment of the self-position estimation device, in the first embodiment, the line-of-sight detection unit detects a first line of sight passing through the principal point of the camera and the feature point of the target image on the image, a second line of sight that refracts with respect to the optical axis of the first line of sight and passes through the light-transmitting part, and a third line of sight that refracts with respect to the optical axis of the second line of sight and passes through the space in which the target is placed.

[0017] In the third embodiment of the self-position estimation device, in the first or second embodiment, the gaze detection unit detects multiple gazes for each target, and the correction unit corrects the camera's self-position so that the sum of the squares of the distances obtained for each gaze is minimized.

[0018] The self-position estimation method according to the fourth embodiment comprises: a self-position estimation step of estimating the self-position of a camera based on an image taken by a camera of a group of targets in which light-transmitting parts are arranged in front of multiple targets; a line-of-sight detection step of detecting a line of sight from the principal point of the camera toward the targets through the light-transmitting parts based on the self-position of the camera estimated in the self-position estimation step; and a correction step of correcting the self-position of the camera based on the distance between the line of sight and the targets.

[0019] According to the present invention, it is possible to accurately estimate the camera's own position without being affected by refraction errors caused by a light-transmitting part placed in front of the target.

[0020] This is a block diagram showing the self-position estimation system according to this embodiment. This is a schematic diagram showing the general configuration of the self-position estimation system according to this embodiment. This is an explanatory diagram for explaining the camera projection model. This is a flowchart showing an example of the self-position estimation process performed by the self-position estimation device of this embodiment. This is a diagram modeling the positional relationship between the camera and the target group. This is a diagram showing the relationship between the direction vectors (line of sight vectors) that constitute the first line of sight, the second line of sight, and the third line of sight, respectively. This is a diagram for explaining the refraction of light. This is a graph showing the relationship between the angle of incidence and the angle of refraction. This is a schematic diagram showing the line of sight from the camera toward each target. This is a diagram for explaining the effect of refraction error due to transparent window material.

[0021] Embodiments of the present invention will be described below with reference to the attached drawings.

[0022] [Self-Position Estimation System] Figure 1 is a block diagram showing the self-position estimation system 10 according to this embodiment. Figure 2 is a schematic diagram showing the general configuration of the self-position estimation system 10 according to this embodiment.

[0023] As shown in Figures 1 and 2, the self-position estimation system 10 comprises a probe head 12, a target group 16, and a self-position estimation device 50. The self-position estimation device 50 is an example of the self-position estimation device of the present invention. In this embodiment, the self-position 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-position estimation device 50.

[0024] 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-type or non-contact-type probe 18. The probe 18 may be a contact-type (touch probe type) or a non-contact-type (laser type, optical type) probe, 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. By holding the probe head 12 and performing measurements, the three-dimensional coordinates at the measurement points of the workpiece can be obtained.

[0025] The probe head 12 has a self-position estimation function, and when the camera 20 mounted on the probe head 12 photographs the target group 16, the self-position estimation device 50 described later can estimate the probe head 12's own position (position and orientation).

[0026] As shown in Figure 2, the target group 16 consists of multiple (many) targets 24 arranged in a two-dimensional manner. Each target 24 is formed as a point or dot, and is spaced apart from each other. Each target 24 may also be composed of a small point-shaped light source (point light source), such as an LED. The relative positions of the targets 24 are known. The shape and size of each target 24 are also known.

[0027] In this embodiment, the target group 16 has a transparent window material 30 (see Figure 5) made of, for example, glass, placed in front of (on the surface of) a plurality of targets 24. The transparent window material 30 constitutes a light-transmitting portion that transmits light to which the camera 20 is sensitive (visible light in this example), and causes refraction of light at the interface of the transparent window material 30. Note that the transparent window material 30 is an example of a light-transmitting portion of the present invention.

[0028] [Self-Position Estimation Device] Next, the self-position estimation device 50 will be described. As shown in Figure 1, the self-position estimation device 50 is composed of, for example, a personal computer and includes a calculation processing unit 52 and a storage unit 54. A camera 20 mounted on the probe head 12 is connected to the self-position estimation device 50. The method of connection to the camera 20 is not particularly limited and may be connected via a cable, or via a wired or wireless network.

[0029] The storage unit 54 stores control programs and various data. The storage unit 54 is composed of, for example, a hard disk drive (HDD) or a solid state drive (SSD). The storage unit 54 may also include temporary memory elements composed of, for example, DRAM (Dynamic Random Access Memory) or SRAM (Static Random Access Memory), which are RAM (Random Access Memory), and may function as a work area for the arithmetic processing unit 52.

[0030] The memory unit 54 stores the 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 make up the target group 16.

[0031] The arithmetic processing unit 52 executes various arithmetic processes performed by the self-position estimation device 50. The arithmetic processing unit 52 comprises an arithmetic circuit composed of various processors and memory. The various processors include CPUs (Central Processing Units), GPUs (Graphics Processing Units), ASICs (Application Specific Integrated Circuits), and programmable logic devices [for example, SPLDs (Simple Programmable Logic Devices), CPLDs (Complex Programmable Logic Devices), and FPGAs (Field Programmable Gate Arrays)]. 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.

[0032] The arithmetic processing unit 52 functions as an image acquisition unit 60, a self-position estimation unit 62, a gaze detection unit 68, a correction unit 70, and a judgment processing unit 72 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 64 and an optimization calculation unit 66.

[0033] Note that the image acquisition unit 60 is an example of the image acquisition unit of the present invention. The self-position estimation unit 62 is an example of the self-position estimation unit of the present invention. The gaze detection unit 68 is an example of the gaze detection unit of the present invention. The correction unit 70 is an example of the correction unit of the present invention.

[0034] [Camera Projection Model] Before describing the self-position estimation process performed by the self-position estimation device 50 of this embodiment, we will now explain the camera projection model that serves as its premise. Figure 3 is an explanatory diagram for illustrating the camera projection model.

[0035] As shown in Figure 3, the world coordinate system (world coordinate system) Σ ｗ O is a coordinate system that represents a position in three-dimensional space (real space), with the origin being O. ｗ Let X be mutually orthogonal. ｗ Axis, Y ｗ axis, Z ｗ This is a three-dimensional Cartesian coordinate system with the axes as coordinate axes. Note that this is the world coordinate system Σ. ｗ Any coordinate system can be used as long as the position in three-dimensional space (three-dimensional position) can be determined. Camera coordinate system Σ ｃ The optical axis center of camera 20 is O ｃ Let O be the origin. ｃ From there, turn right X ｃ Axis, downward direction is Y ｃ The axis, the optical axis direction, is Z ｃ This is a three-dimensional Cartesian coordinate system. Image coordinate system Σ ｓ is the camera coordinate system Σ ｃ The origin of O ｃ From Z ｃ The origin is the upper left corner of the image plane IP, which is at a focal distance f in the direction X ｃ Axis and Y ｃ This is a two-dimensional Cartesian coordinate system (pixel coordinate system) with a U-axis and a V-axis running parallel to the other axes.

[0036] First, the world coordinate system Σ of point P (object point) in three-dimensional space ｗ Coordinates (x) ｗ , y ｗ , z ｗ The camera coordinate system Σ is obtained using the rotation matrix R and translation vector t of camera 20, as shown in equation (2) below.ｃ It can be converted to coordinates (x, y, z) in a given space.

[0037]

[0038] Here, [R|t] w,c is the world coordinate system Σ ｗ From camera coordinate system Σ ｃ This is a transformation matrix (external parameter matrix) for transforming coordinates to the world coordinate system Σ ｗ This shows the orientation and position of camera 20. [R|t] w,c Each component r 11 , r 12 , ..., r 33 t x t y t z These are called external camera parameters. Note that in the following explanation, "R" and "R(R)" will be used interchangeably. ｘ , R ｙ , R ｚ ) is used synonymously. ｘ , R ｙ , R ｚ This represents the rotation components in the x, y, and z directions of the rotation matrix R. Also, the transformation matrix [R | t] w,c This is sometimes simply written as [R|t].

[0039] Next, the camera coordinate system Σ ｃ If point P, located at (x, y, z) as viewed from the image plane IP, has coordinates (pixel coordinates) of the projected point Q as (u, v), then the following relationships shown in equations (3) to (8) hold.

[0040]

[0041]

[0042]

[0043]

[0044]

[0045]

[0046] Here, (x', y') is the camera coordinate system Σ ｃThis represents the coordinates of the projected point when point P, located at (x, y, z) as viewed from the camera, is projected onto the normalized image plane (z=1). Furthermore, (x'', y'') represents the coordinates of the projected point (distorted point) when point P is projected onto the normalized image plane, taking into account the lens distortion of the camera 20.

[0047] Also, yf ｘ , f ｙ This indicates the focal length in the x and y directions, expressed in pixels. Also, c ｘ , c ｙ This is the image coordinate system Σ ｓ This indicates the optical center in (the position where the optical axis of camera 20 intersects the image plane IP, the optical center on a pixel-by-pixel basis). Also, k １ , k ２ , k ３ is the radial strain coefficient, and p １ , p ２ is the tangential distortion coefficient. In this specification, the focal length f ｘ , f ｙ , optical center c ｘ , c ｙ These are called the internal parameters of camera 20, and the distortion coefficient k １ , k ２ , k ３ , p １ , p ２ This is called the distortion parameter of camera 20.

[0048] [Self-Position Estimation Method] Next, the processing procedure (an example of a self-position estimation method) of the self-position estimation process performed by the self-position estimation device 50 of this embodiment will be described. Figure 4 is a flowchart showing an example of the self-position estimation process performed by the self-position estimation device 50 (arithmetic processing unit 52) ​​of this embodiment. At the start of this flowchart, it is assumed that the camera 20 has already been calibrated, and that the camera matrix K, including the intrinsic parameters (focal length, optical center) and distortion parameters (distortion coefficient) of the camera 20, is known.

[0049] First, as shown in FIG. 2, the target group 16 is photographed by the camera 20 mounted on the probe head 12 (step S10). The image (target image) photographed by the camera 20 is transmitted to the self-position estimation device 50. When the target image is transmitted to the self-position estimation device 50, the image acquisition unit 60 acquires the target image and stores it in the storage unit 54.

[0050] Next, the self-position estimation unit 62 estimates the self-position (position and orientation) of the camera 20 based on the target image photographed by the camera 20. Hereinafter, the processing performed by the self-position estimation unit 62 will be described.

[0051] First, the feature point detection unit 64 executes a feature point detection process for detecting a plurality of feature points from the target image (step S12).

[0052] Specifically, the feature point detection unit 64 reads the target image from the storage unit 54. Then, after performing predetermined image processing (such as grayscale conversion) on the read target image, the feature point detection unit 64 detects feature points (image points) indicating the positions of each target 24 (target images) from the target image, and obtains the coordinates (pixel coordinates) of each feature point on the target image (image plane IP; see FIG. 3). Note that, as the feature point on the target image, the position of the center of gravity of the target 24 is detected.

[0053] The feature points on the target image detected in the above-described feature point detection process are denoted by Q ｉ and its coordinates (two-dimensional coordinates) are set to (u ｉ , v ｉ ) (where i is a variable (a number uniquely assigned to each feature point) and is a natural number of 2 or more). Note that, if necessary, lens distortion correction is applied to the coordinates (u ｉ , v ｉ , v ｉ ) of each feature point Q

[0054] Also, the coordinates indicating the object point P j (the position of each target 24 in the target group 14) in the three-dimensional space are (x ｗｊ , y ｗｊ , z ｗｊWhen it is set as (where j is a variable (a number uniquely assigned to each object point) and is a natural number of 2 or more), from the camera projection model shown in FIG. 3, it can be formulated as the following equation (9).

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

[0056] Here, the error function E is defined as follows in equation (10).

[0057] The error function E defined by the equation (10) is an error function showing the correspondence between the feature point Q on the target image ｉ and the object point P in the three-dimensional space j (the point indicating the position of each target 24). In these combinations, when the combination is such that each point has a projective relationship with each other, the error function E becomes the minimum value.

[0058] The optimization calculation unit 66 obtains the transformation matrix [R|t] at which the error function E becomes the minimum value by non-linear optimization calculation (step S14). The transformation matrix [R|t] obtained in the optimization calculation unit 66 is the world coordinate system Σ[[ID=I9]] ｗ It indicates the self-position (position and orientation) of the camera 20.

[0059] The self-position estimation unit 62 stores information (camera self-position information) indicating the position and orientation of the camera 20 obtained as described above in the storage unit 54. The camera self-position information stored in the storage unit 54 is used when calculating the three-dimensional coordinates of the workpiece using the probe head 12.

[0060] Here, the transformation matrix [R|t] indicating the self-position of the camera 20 is obtained based on the coordinates (target coordinates) of each feature point Q on the target image ｉ Therefore, in order to accurately obtain the self-position of the camera 20, it is necessary that the coordinates of each feature point Q ｉ are accurately obtained.

[0061] However, when a transparent window material 30 is placed in front of the target, the refraction effect of the transparent window material 30 affects each feature point Q on the target image. ｉ An error (hereinafter also referred to as "refraction error") occurs in the coordinates. In particular, the greater the tilt angle of the line of sight from camera 20 toward each target 24, the greater the refraction error, which is a factor that significantly reduces the accuracy of the camera 20's self-position estimation.

[0062] Therefore, in this embodiment, after the self-position estimation unit 62 determines the self-position of the camera 20, the gaze detection unit 68 and the correction unit 70 perform processing to reduce the effect of refraction error caused by the transparent window material 30. The processes performed by the gaze detection unit 68 and the correction unit 70, respectively, will be described in detail below.

[0063] [Gaze Detection Processing] The gaze detection unit 68 performs gaze detection processing to detect the gaze direction from the camera 20 to each target 24, based on the self-position of the camera 20 determined by the self-position estimation unit 62 (step S16).

[0064] Figure 5 is a diagram modeling the positional relationship between camera 20 and target group 16. In the model shown in Figure 5, the self-position of camera 20 is defined by the transformation matrix [R|t]. Also, feature point Q on the image (target image) captured by camera 20. ｉ The coordinates (target coordinates) are (u ｉ , v ｉ ) and the corresponding object point P in three-dimensional space. ｉ The coordinates of (Target 24) are (x ｗｉ , y ｗｉ , z ｗｉ ) Furthermore, the processing performed in the self-position estimation unit 62 described above results in the feature point Q on the target image. ｉ and object point P in three-dimensional space ｉ The correspondence with (Target 24) has already been determined and is known. Furthermore, in order to simplify the formulation in the following explanation, feature point Q on the target image is used. ｉ The coordinates are assumed to be corrected for lens distortion in camera 20.

[0065] As shown in Figure 5, the principal point of camera 20 (the origin O shown in Figure 3) ｃ (equivalent to) line of sight L ｉ Q is a feature point on the target image (image plane IP). ｉ Passing through object point P ｉ (Target 24) This line of sight L ｉ This refers to the principal point of camera 20 and feature point Q on the target image. ｉ The first line of sight L passing through ｒ，ｉ And, the first line of sight L ｒ，ｉ The second line of sight L refracts relative to the optical axis and passes through the inside of the transparent window material 30. ｍ，ｉ And, the second line of sight L ｍ，ｉ The third line of sight L refracts relative to the optical axis and passes through the space (medium) where the target 24 is located. ｋ，ｉ It consists of the following. Note that the first line of sight L ｒ，ｉ , second line of sight L ｍ，ｉ , and the third line of sight L ｋ，ｉ Figure 6 shows the relationship between the direction vectors (line of sight vectors) that constitute each of these. The following describes the detection method for each line of sight.

[0066] (Detection of the first line of sight) Position of the principal point of camera 20 (first line of sight L) ｒ，ｉ The starting point of the first line of sight L is defined by the "t" (translation vector) of the transformation matrix [R|t] that indicates the self-position of the camera 20. ｒ，i The direction vector of (hereinafter referred to as the "first line of sight vector") is r ｉ In that case, the first line of sight L ｒ，ｉ As shown in equation (11) below, the variable s ｒ It can be obtained as a function of . For the sake of explanation, "L ｒ，ｉ (s ｒ ) and "L ｒ，ｉ This is used synonymously with "".

[0067] First line-of-sight vector r ｉ This is a direction vector indicating the direction of the line of sight from camera 20 to transparent window material 30. As shown in Figure 5, feature point Q on the target image ｉ Coordinates (u ｉ , v ｉ ) From this, the first line of sight vector r ｉ (=(xｉ , y ｉ , z ｉ )) can be expressed as shown in equation (12) below.

[0068] (Detection of the second line of sight) First line of sight L ｒ，i The intersection of the transparent window material 30 and the surface 30a (second line of sight L) ｍ，i (corresponding to the starting point) to x ｒ，i And the second line of sight L ｍ，i The direction vector (hereinafter referred to as the "second line of sight vector") is m ｉ In that case, the second line of sight L ｍ，i As shown in equation (13) below, the variable s ｍ It can be obtained as a function of . For the sake of explanation, "L ｍ，ｉ (s ｍ ) and "L ｍ，ｉ This is used synonymously with "".

[0069] As shown in Figure 6, the second line of sight vector m ｉ This is the second line of sight L that enters from the surface 30a of the transparent window material 30, is refracted, and passes through the transparent window material 30. ｍ，i This is the direction vector. Second line-of-sight vector m ｉ This can be obtained using Snell's Law by the following equation (14). Since the derivation method of equation (14) is publicly known, a detailed explanation will be omitted. Note that h １ The normal vector of the surface 30a of the transparent window material 30 is shown. Also, n １ n is the refractive index of the space between the camera 20 and the transparent window material 30, ２ This indicates the refractive index of the transparent window material 30.

[0070] (Detection of the third line of sight) Second line of sight L ｍ，i The intersection of the transparent window material 30 and the back surface 30b (third line of sight L) ｋ，i (corresponding to the starting point) to x ｍ，i And the third line of sight L ｋ，i The direction vector of (hereinafter referred to as the "third line of sight vector") is k ｉ In that case, the third line of sight L ｋ，i As shown in equation (15) below, the variable s ｋ It can be obtained as a function of . For the sake of explanation, "Lｋ，ｉ (s ｋ ) and "L ｋ，ｉ This is used synonymously with "".

[0071] As shown in Figure 6, the third line of sight vector k ｉ The light exits from the back surface 30b of the transparent window material 30, is refracted, and reaches object point P ｉ Third line of sight L towards (Target 24) ｋ，i This is the direction vector. The third line-of-sight vector k ｉ This can be obtained using Snell's Law by the following equation (16). Since the derivation method of equation (16) is publicly known, a detailed explanation will be omitted. Note that h ２ The normal vector of the back surface 30b of the transparent window material 30 is shown. However, the normal vector here is in the direction from the outside to the inside of the transparent window material. Also, n ３ This is the refractive index of the space (medium) in which target 24 exists.

[0072] Note that the intersection x ｒ，i , x ｍ，i The calculation method is not particularly limited, and conventionally known methods can be used. Furthermore, the thickness of the transparent window material 30 is assumed to be known.

[0073] The gaze detection unit 68 detects the first gaze L by the method described above. ｒ，ｉ , second line of sight L ｍ，ｉ , and the third line of sight L ｋ，ｉ By determining this, the line of sight L from camera 20 toward each target 24 is determined. ｉ It detects.

[0074] The gaze detection unit 68 performs the gaze detection process described above on multiple feature points Q on the target image. ｉ This process is repeated. This results in multiple feature points Q on the target image. ｉ The corresponding line of sight L ｉ The gaze detection process detects all feature points Q on the target image. ｉ You can also do this for some of the feature points Q. ｉ This may be done for all feature points Q on the target image. ｉWhen performing gaze detection processing on certain feature points Q, ｉ Compared to performing gaze detection processing on the target image, the correction process described later makes it possible to correct the camera 20's own position with high accuracy. In addition, some feature points Q on the target image ｉ When performing gaze detection processing on all feature points Q, ｉ Compared to performing gaze detection processing on the subject, it becomes possible to reduce the time required for the correction processing described later.

[0075] [Correction process] Next, the correction unit 70 corrects the line of sight L detected in the line of sight detection process. ｉ Based on this, a correction process is performed to correct the self-position of the camera 20 (step S18).

[0076] The transformation matrix [R|t] indicating the self-position of the camera 20, obtained by the self-position estimation unit 62, includes the refraction error caused by the transparent window material 30 mentioned above. Therefore, the first line-of-sight vector r, which is determined based on the self-position of the camera 20, is ｉ , second line of sight vector m ｉ , and the third line of sight vector k ｉ This will also include an error. In that case, the line of sight L detected by the line of sight detection unit 68 will be included. ｉ As shown in Figure 5, its line of sight L ｉ The corresponding object point Q ｉ It passes a position far from the actual position of (Target 24). And the larger the error in the camera 20's own position, the greater the line of sight L ｉ and the corresponding object point Q ｉ distance d ｉ On the other hand, the closer the camera 20's own position is to its correct position, the greater the line of sight L ｉ and the corresponding object point Q ｉ distance d ｉ It becomes smaller.

[0077] Therefore, all or some of the feature points Q on the target image ｉ Multiple lines of sight L passing through each of them ｉ And each feature point Q ｉ The corresponding object point P ｉ Distance d from (Target 24) ｉThe self-position [R|t] of camera 20 when the sum of the squares of the values ​​is minimized can be substantially considered as the true self-position.

[0078] In this embodiment, focusing on these characteristics, the gaze line L detected by the gaze detection process is considered. ｉ and the corresponding object point Q ｉ Based on the distance, the self-position estimation unit 62 corrects the self-position of the camera 20.

[0079] Here, the error function E(R ｘ , R ｙ , R ｚ t) is defined as shown in equation (17) below. N is the number of gazes L detected in the gaze detection process, and Q is a feature point on the target image. ｉ The value is less than or equal to the number of items.

[0080] The correction unit 70 uses the error function E(R ｘ , R ｙ , R ｚ The transformation matrix [R|t] that minimizes t) is determined by nonlinear optimization. The transformation matrix [R|t] obtained here becomes the correction value for the camera 20's own position. The correction unit 70 then updates the camera 20's own position stored in the memory unit 54 with the above correction value.

[0081] Furthermore, various nonlinear solution methods such as the steepest descent method, the conjugate gradient method, and Adam can be used for the optimization calculations performed by the self-position estimation unit 62 (optimization calculation unit 66) and the correction unit 70. Since these methods are well known, their explanation will be omitted here.

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

[0083] 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 72 determines that the self-position estimation process of the camera 20 has finished (NO in step S20), and terminates this flowchart.

[0084] [Effects] Next, the effects of this embodiment will be described.

[0085] According to this embodiment, after a self-position estimation process is performed to estimate the camera 20's own position based on an image of the target group 16 captured by the camera 20, a line of sight L is directed from the principal point of the camera 20 towards the target 24 through the transparent window material 30. ｉ A gaze detection process is performed to detect each of the 24 targets. Then, each gaze L ｉ A correction process is performed to correct the camera 20's own position (position and orientation) so that the sum of the squares of the distances to the corresponding target 24 is minimized. This makes it possible to estimate the camera 20's own position with high accuracy without being affected by the refraction error caused by the transparent window material 30 in front of the target.

[0086] The camera 20 is not limited to a visible light camera; for example, it may be an infrared light camera. In this case, the transparent window material 30 only needs to be transparent to the wavelength range of infrared light. That is, the transparent window material 30 is not limited to being transparent to the wavelength range of visible light, but needs to be transparent to the wavelength range of light to which the camera 20 is sensitive.

[0087] Furthermore, in this embodiment, one example is shown where the target group 16 is composed of a dot pattern in which a plurality of targets 24 formed as dots or points are arranged in a two-dimensional manner. However, the embodiment is not limited to this, and for example, the target group 16 may be composed of a grid pattern or a checker pattern as disclosed in Patent Document 2. In addition, each target group 16 may be various two-dimensional patterns including AR markers (Augmented Reality Markers) or QR codes (Quick Response code, registered trademark).

[0088] Furthermore, the probe head 12 is not limited to one that the user grasps and uses to measure the workpiece. For example, the probe head 12 may be attached to the tip (end effector) of a multi-joint robot arm, and the probe head 12 may be moved by controlling the movement of the robot arm, thereby measuring the three-dimensional coordinates of the workpiece with the probe head 12.

[0089] Although 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.

[0090] 10...Self-position estimation system, 12...Probe head, 16...Target group, 18...Probe, 20...Camera, 24...Target, 30...Transparent window material, 50...Self-position estimation device, 52...Calculation processing unit, 54...Storage unit, 60...Image acquisition unit, 62...Self-position estimation unit, 64...Feature point detection unit, 66...Optimization calculation unit, 68...Look-line detection unit, 70...Correction unit, 72...Decision processing unit

Claims

1. A self-position estimation device comprising: a self-position estimation unit that estimates the self-position of a camera based on an image taken by a camera of a group of targets in which light-transmitting parts are arranged in front of multiple targets; a line-of-sight detection unit that detects a line of sight from the principal point of the camera toward the targets via the light-transmitting parts based on the self-position of the camera estimated by the self-position estimation unit; and a correction unit that corrects the self-position of the camera based on the distance between the line of sight and the targets.

2. The self-position estimation device according to claim 1, wherein the line of sight detection unit detects a first line of sight passing through the principal point of the camera and the feature point of the target image on the image, a second line of sight that is refracted with respect to the optical axis of the first line of sight and passes through the light-transmitting part, and a third line of sight that is refracted with respect to the optical axis of the second line of sight and passes through the space in which the target is located.

3. The self-position estimation device according to claim 1 or 2, wherein the gaze detection unit detects a plurality of gazes for each target, and the correction unit corrects the self-position of the camera so that the sum of the squares of the distances obtained for each gaze is minimized.

4. A self-position estimation method comprising: a self-position estimation step of estimating the self-position of a camera based on an image taken by a camera of a group of targets in which light-transmitting parts are arranged in front of multiple targets; a line-of-sight detection step of detecting a line of sight from the principal point of the camera toward the targets through the light-transmitting parts based on the self-position of the camera estimated in the self-position estimation step; and a correction step of correcting the self-position of the camera based on the distance between the line of sight and the targets.

Images