Self-position estimation device and self-position estimation method

Abstract

The present invention provides a self-position estimation device and method that can perform highly accurate self-position estimation of a camera while suppressing the occurrence of periodic errors caused by a group of targets arranged periodically. [Solution] The self-position estimation device 50 includes an image acquisition unit 60 that acquires an image of a target group 16 in which a plurality of targets 24 are periodically arranged, captured by a camera 20; a feature point detection unit 64 that detects feature points indicating the position of each target 24 in the image; a feature point probability setting unit 66 that sets the probability of each feature point and makes the probability of feature points in the peripheral part of the image smaller than the probability of feature points in the central part of the image; and an optimization calculation unit 68 that calculates the self-position of the camera 20 that minimizes the reprojection error of each target 24 in the target group 16 based on the probabilities set for each feature point.

Description

[Technical Field]

[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. [Background technology]

[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 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 is known (see, for example, Patent Document 2). This self-position estimation 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. [Prior art documents] [Patent Documents]

[0006] [Patent Document 1] Japanese Patent Publication No. 2020-148515 [Patent Document 2] Japanese Patent Publication No. 2022-30807 [Overview of the project] [Problems that the invention aims to solve]

[0007] Incidentally, the self-localization method described above generally uses a group of targets arranged periodically. However, when the camera's self-localization is performed based on images taken while the camera moves over such a group of targets, the following problems arise.

[0008] Figure 12 is a graph illustrating the problems of the conventional approach. In Figure 12, the horizontal axis represents the camera's movement, and the vertical axis represents the camera's self-position estimation error. The camera's self-position estimation error is the difference between the estimated value and the true value of the camera's self-position.

[0009] When a group of targets arranged periodically is photographed while moving with a camera, the camera's self-position estimation error changes periodically with respect to the amount of camera movement, as shown in Figure 12. Specifically, the self-position estimation error changes discontinuously (abruptly) each time the camera moves by a certain amount (1 mm in this example), and in other parts, the self-position estimation error changes continuously (gradually) within a predetermined range (approximately -2 μm to 2 μm in this example). The interval at which the camera's self-position estimation error changes discontinuously (amount of camera movement) corresponds to the distance between targets aligned in the direction of camera movement in the target group.

[0010] When the camera's self-position estimation error changes periodically with respect to the camera's movement, it becomes difficult to estimate the camera's self-position with high accuracy. In the following explanation, the self-position estimation error that changes periodically with respect to the camera's movement will also be referred to as the "periodic error."

[0011] This invention has been made in view of these circumstances, and aims to provide a self-position estimation device and self-position estimation method that can perform high-precision self-position estimation of a camera while suppressing the occurrence of periodic errors caused by a group of targets arranged periodically. [Means for solving the problem]

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

[0013] The self-position estimation device according to the first embodiment includes: an image acquisition unit that acquires an image of a target group in which multiple targets are periodically arranged, captured by a camera; a feature point detection unit that detects feature points indicating the position of each target in the image; a feature point probability setting unit that sets probabilities for each feature point, wherein the probability for feature points in the peripheral part of the image is smaller than the probability for feature points in the central part of the image; and an optimization calculation unit that calculates the camera's own position that minimizes the reprojection error of each target in the target group based on the probabilities set for each feature point.

[0014] In the second embodiment of the self-localization device, in the first embodiment, the feature point probability setting unit sets the probability for a feature point according to probability distribution information that shows the correspondence between the position of the feature point and the probability.

[0015] In the third embodiment of the self-position estimation device, in the second embodiment, the probability distribution information is a probability distribution image showing a correspondence.

[0016] In the fourth embodiment, the self-localization device is such that, in the third embodiment, the probability distribution image is an image based on a Gaussian distribution.

[0017] In the fifth embodiment of the self-localization device, in any of the first to fourth embodiments, the optimization calculation unit performs an optimization calculation by weighting each feature point based on the probability for each feature point.

[0018] In the self-localization device according to the sixth embodiment, in any of the first to fourth embodiments, the optimization calculation unit determines whether or not to adopt each feature point in the optimization calculation based on the probability for each feature point, and performs the optimization calculation using the feature points that have been determined to be adopted.

[0019] The self-position estimation method according to the seventh embodiment comprises: an image acquisition step of acquiring an image of a target group in which multiple targets are periodically arranged, captured by a camera; a feature point detection step of detecting feature points indicating the position of each target in the image; a feature point probability setting step of setting a probability for each feature point, wherein the probability for feature points in the peripheral part of the image is smaller than the probability for feature points in the central part of the image; and an optimization calculation step of determining the self-position of the camera that minimizes the reprojection error of each target in the target group based on the probabilities set for each feature point. [Effects of the Invention]

[0020] According to the present invention, it is possible to perform highly accurate self-position estimation of a camera while suppressing the occurrence of periodic errors caused by a target group in which multiple targets are arranged periodically. [Brief explanation of the drawing]

[0021] [Figure 1] This is a block diagram showing the self-localization system according to the first embodiment. [Figure 2] This is a schematic diagram showing the general configuration of the self-localization system according to the first embodiment. [Figure 3] This is an explanatory diagram for describing the camera projection model. [Figure 4] This flowchart shows an example of the self-position estimation process performed by the self-position estimation device of the first embodiment. [Figure 5] This diagram explains the factors that cause periodic errors. [Figure 6] This figure shows an example of a probability distribution image according to the first embodiment. [Figure 7] This figure shows another example of a probability distribution image according to the first embodiment. [Figure 8] This graph illustrates the effects of the first embodiment. [Figure 9] This figure shows an example of a probability distribution image according to the second embodiment. [Figure 10] This figure shows another example of a probability distribution image according to the second embodiment. [Figure 11] This flowchart shows an example of the self-position estimation process performed by the self-position estimation device of the second embodiment. [Figure 12] This graph illustrates the problems with conventional methods. [Modes for carrying out the invention]

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

[0023] <First Embodiment> [Self-localization system] Figure 1 is a block diagram showing the self-localization system 10 according to the first embodiment. Figure 2 is a schematic diagram showing the general configuration of the self-localization system 10 according to the first embodiment.

[0024] As shown in Figures 1 and 2, the self-position estimation system 10 of this embodiment 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.

[0025] The probe head 12 is a portable three-dimensional coordinate measuring machine that measures the three-dimensional coordinates of a workpiece (not shown). The probe head 12 is equipped with a contact-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), 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.

[0026] 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).

[0027] As shown in Figure 2, the target group 16 consists of multiple (many) targets 24 arranged periodically 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.

[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. The 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 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), or other 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 image, 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. Furthermore, the memory unit 54 stores the probability distribution image, which will be described later.

[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 [e.g., 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, and a determination processing unit 70 by reading and executing the control program stored in the storage unit 54. The self-position estimation unit 62 includes a feature point detection unit 64, a feature point probability setting unit 66, and an optimization arithmetic unit 68.

[0033] Note that the image acquisition unit 60 is an example of the image acquisition unit of the present invention. The feature point detection unit 64 is an example of the feature point detection unit of the present invention. The feature point probability setting unit 66 is an example of the feature point probability setting unit of the present invention. The optimization arithmetic unit 68 is an example of the optimization arithmetic unit of the present invention.

[0034] 〔Camera Projection Model〕 Before explaining the self-position estimation process executed by the self-position estimation device 50 of the present embodiment, the camera projection model on which it is based will be explained. FIG. 3 is an explanatory diagram for explaining the camera projection model.

[0035] As shown in FIG. 3, the world coordinate system (world coordinate system) Σ w is a coordinate system representing positions in a three-dimensional space (real space), with the origin at O w and a three-dimensional orthogonal coordinate system with the X w axis, Y w axis, and Z w axis as the coordinate axes. Note that the world coordinate system Σ w may use any coordinate system as long as it can specify positions in a three-dimensional space (three-dimensional positions). The camera coordinate system Σ c has the optical axis center O c of the camera 20 as the origin, and a three-dimensional orthogonal coordinate system with the right direction from the origin O c as the X c axis, the downward direction as the Y c axis, and the optical axis direction as the Z c axis. The image coordinate system Σ s has the upper left of the image plane IP, which is at a focal distance f in the Z c direction from the origin O c of the camera coordinate system Σ c as the origin, and the X c axis and Y cThis is a two-dimensional Cartesian coordinate system (pixel coordinate system) with U-axis and V-axis running parallel to the other axes.

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

[0037]

number

[0038] Here, [R|t] w,c is the world coordinate system Σ w From camera coordinate system Σ c This is a transformation matrix (external parameter matrix) for transforming coordinates to the world coordinate system Σ w 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 parameters of the camera.

[0039] Next, the camera coordinate system Σ c 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 (2) through (7) hold.

[0040]

number

number

[0041]

number

[0042]

number

[0043]

number

[0044]

number

[0045] Here, (x', y') is the camera coordinate system Σ c 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 camera 20.

[0046] Also, PH x ,f y This indicates the focal length in the x and y directions, expressed in pixels. Also, c x , c y This is the image coordinate system Σ s This indicates the optical center (the position where the optical axis of camera 20 intersects the image plane IP, the optical center on a pixel-by-pixel basis). k1, k2, and k3 are radial distortion coefficients, and p1 and p2 are tangential distortion coefficients. In this specification, the focal length f x ,f y , optical center c x , c y These are called the internal parameters of camera 20, and the distortion coefficients k1, k2, k3, p1, and p2 are called the distortion parameters of camera 20.

[0047] [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 the first 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 the first 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.

[0048] First, as shown in Figure 2, the camera 20 mounted on the probe head 12 captures images of the target group 16 (step S10). The images (target images) captured by the camera 20 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.

[0049] Next, the self-position estimation unit 62 estimates the camera 20's own position (position and orientation) based on the target image captured by the camera 20. The following describes the processing performed by the self-position estimation unit 62.

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

[0051] Specifically, the feature point detection unit 64 reads the target image from the storage unit 54. Then, the feature point detection unit 64 performs predetermined image processing (such as grayscale conversion) on the read target image, detects feature points (image points) that indicate the position of each target 24 (target image) from the target image, and determines the coordinates (pixel coordinates) of each feature point on the target image (image plane IP; see Figure 3). The position of the centroid of the target 24 is detected as a feature point on the target image.

[0052] Next, the feature point probability setting unit 66 performs a probability setting process that sets a probability for each feature point according to its position on the target image (step S14). The probability represents the probability of the feature point being selected in the optimization calculation unit 68, which will be described later. Details of the probability setting process performed by the feature point probability setting unit 66 will be described later.

[0053] Next, the optimization calculation unit 68 uses each feature point detected by the feature point detection unit 64 to determine the position and orientation of the camera 20 that minimizes the reprojection error of each target 24 in the target group 16 through optimization calculation (step S16). In this embodiment, when this optimization calculation is performed, the optimization calculation is carried out based on the probability of each feature point set by the feature point probability setting unit 66.

[0054] Before describing the optimization calculation process performed in this embodiment, we will first describe the optimization calculation process when the probability setting process is not performed.

[0055] The feature points detected on the target image in the feature point detection process described above are called Q i This is shown, and its coordinates (2D coordinates) are (u i , v i ) (where i is a variable (a number uniquely assigned to each feature point) and is a natural number greater than or equal to 2). Note that, if necessary, each feature point Q i Coordinates (u i , v i Apply lens distortion correction to the image.

[0056] Also, an object point P in three-dimensional space j The coordinates indicating the position of each target 24 in the target group 16 are (x wj , y wj , z wj If we assume that (where j is a variable (a number uniquely assigned to each object point) and is a natural number greater than or equal to 2), then from the camera projection model shown in Figure 3, we can formulate it as follows: equation (8).

number

[0057] In equation (8), K is the camera matrix representing the intrinsic parameters of camera 20, and is known.

[0058] Here, the error function E is defined as shown in equation (9) below, where N is the number of feature points.

number

[0059] The error function E defined by equation (9) is given by the feature points Q on the target image. i and object point P in three-dimensional space j This error function shows the correspondence with (points indicating the positions of each target 24), and the error function E is minimized when the combinations of these points are projected onto each other.

[0060] The transformation matrix [R|t] that minimizes the error function E can be found by nonlinear optimization. The transformation matrix [R|t] obtained in this way is the world coordinate system Σ w This represents the self-position (position and orientation) of camera 20.

[0061] By the way, when the above optimization calculation process is performed (i.e., when the probability setting process is not performed), as mentioned above, there is a problem in that periodic errors occur in the self-position estimation process of camera 20 as the camera 20 moves (see Figure 12).

[0062] Figure 5 is a diagram illustrating the causes of periodic errors. As shown in Figure 5, when a target group 16 in which multiple targets 24 are arranged periodically is photographed while the camera 20 is moving, the multiple targets 24 may move in and out of the camera 20's field of view VF (target image) as a group as the camera 20 moves.

[0063] For example, as shown from VA to VB in Figure 5, when the camera 20 moves to the left, the field of view VF of the camera 20 also moves to the left. As a result, multiple targets 24 on the right side of the field of view VF (targets 24 located in the area enclosed by reference numeral 22A) disappear outside the field of view VF. On the other hand, multiple targets 24 on the left side of the field of view VF (targets 24 located in the area enclosed by reference numeral 22B) enter the field of view VF. In this case, in an extreme example, the camera calibration error may switch from being dominant on the right side of the field of view VF to being dominant on the left side. If there is a bias in the camera calibration values ​​(especially the distortion parameter) within the field of view VF at this time, the self-position estimation error of the camera 20 will change abruptly (discontinuously).

[0064] In other words, when the target group 16 is photographed while the camera 20 is moving, multiple targets 24 move in and out of the camera 20's field of view VF as a group. As a result, the error distribution of each feature point changes significantly within the field of view VF. Consequently, periodic errors occur in the camera 20's self-localization process.

[0065] To reduce such periodic errors, it is desirable that in the optimization calculation process of the optimization calculation unit 68, the contribution (probability of adoption) of feature points located in the peripheral part of the target image is reduced compared to the central part of the image. This reduces the influence of the target 24 entering and exiting the field of view VF of the camera 20, and effectively suppresses the occurrence of periodic errors.

[0066] Therefore, in this embodiment, before the optimization calculation process is performed by the optimization calculation unit 68, the feature point probability setting unit 66 performs a probability setting process in which a probability is set for each feature point according to the position of each feature point on the target image.

[0067] The memory unit 54 has a probability distribution image stored in it beforehand. The probability distribution image is used in the probability setting process by the feature point probability setting unit 66, and is an image (map) that represents the probability of a feature point at each position (pixel). As will be described in detail later, the probability distribution image has a probability distribution in which the probability is smaller in the peripheral part of the image compared to the central part of the image. The feature point probability setting unit 66 reads the probability distribution image from the memory unit 54 and sets a probability for each feature point on the target image according to that probability distribution image. The probability set for each feature point is a value between 0 and 1. The probability distribution image is an example of the probability distribution information of the present invention.

[0068] Figure 6 shows an example of a probability distribution image according to the first embodiment. As shown in Figure 6, the probability distribution image 30A is a grayscale image having the same size as the target image, and the density (pixel value) at the position corresponding to the feature point represents the probability that the feature point is assigned to the feature point. The probability distribution in the probability distribution image 30A is defined according to a Gaussian distribution in which the probability decreases concentrically from the center of the image to the periphery of the image. In the probability distribution image 30A shown in Figure 6, the white position corresponds to a probability of 1, the black position corresponds to a probability of 0, and the probability of the gray (intermediate color between white and black) position corresponds to a value proportional to the density (a value between 0 and 1).

[0069] According to the probability distribution image 30A shown in Figure 6, the probability assigned to each feature point gradually decreases as you move from the center of the image towards the periphery, and finally the probability becomes 0 at the outermost edge of the periphery. This reduces the contribution of feature points in the periphery of the image, which are a factor in periodic errors in the self-localization process.

[0070] Figure 7 shows another example of a probability distribution image according to the first embodiment. As shown in Figure 7, the probability distribution image 30B is similar to the probability distribution image 30A in that it is a grayscale image having the same size as the target image, but the probability distribution of the probability distribution image 30B is defined according to a distribution different from a Gaussian distribution.

[0071] In other words, in the probability distribution image 30B shown in Figure 7, compared to the probability distribution image 30A shown in Figure 6, the white area extends over a wide area from the center of the image, and the range in which the probability of a feature point is set to 1 has been expanded. Consequently, the gray and black areas outside the white area have become narrower, and the range in which the probability of a feature point is set to a value less than 1 has become limited.

[0072] According to the probability distribution image 30B shown in Figure 7, the overall contribution of feature points used in the self-localization process can be improved compared to the probability distribution image 30A. Therefore, the accuracy of the self-localization estimation of the camera 20 can be improved due to the averaging effect resulting from the increase in feature points.

[0073] In this embodiment, the probability distribution image is not limited to the examples shown in Figures 6 and 7. Any probability distribution image with an arbitrary probability distribution may be used, as long as it can reduce the contribution of feature points in the peripheral part of the image compared to the central part of the image during the self-localization process (optimization calculation process).

[0074] In the probability setting process by the feature point probability setting unit 66, the aforementioned probability distribution images (30A, 30B) are superimposed on the target image, the position (pixel) of the probability distribution image corresponding to the position of each feature point is identified, and the probability corresponding to the identified position in the probability distribution image is assigned to the feature point. This makes it possible to reduce the probability of feature points in the peripheral part of the image compared to the central part of the image. In other words, among the feature points detected on the target image, the probability assigned to feature points that are less affected by errors in camera calibration values ​​(especially lens distortion parameters) can be increased, and the probability assigned to feature points that are more affected by errors can be decreased.

[0075] The memory unit 54 may store multiple types of probability distribution images, each with a different probability distribution. For example, the memory unit 54 may store multiple types of probability distribution images, including the probability distribution image 30A shown in Figure 6 and the probability distribution image 30B shown in Figure 7. In this case, the feature point probability setting unit 66 selects the desired probability distribution image from among the multiple types of probability distribution images stored in the memory unit 54. The selection of the probability distribution image may be made possible by the user via an operation unit (not shown).

[0076] In this embodiment, a probability distribution image is shown as an example of the probability distribution information of the present invention, but the invention is not limited to this. Any information that shows the correspondence between the position of each feature point and its probability is acceptable, and may be expressed as a table or function. For example, a table or function that specifies that the probability decreases gradually or in stages at a constant rate as the distance from the center of the target image to the feature point increases may be used as the probability distribution information.

[0077] After the probability setting process is performed in the feature point probability setting unit 66 as described above, the optimization calculation unit 68 performs optimization calculations based on the probabilities set for each feature point. The optimization calculations in the optimization calculation unit 68 are performed as follows.

[0078] Feature point Q on the target image detected in the feature point detection process described above i The coordinates (u) that indicate the coordinates i , v i ) and the object point P in three-dimensional space. j (The coordinates (x) indicate the position of each target 24 in the target group 16) wj , y wj , z wj The relationship with ) is expressed by the following equation (10).

number

[0079] Note that in equation (10), K(f x ,f y , c x , cy ) is a camera matrix showing the internal parameters of the camera 20 (corresponding to the above-described camera matrix K) and is known. K(f x , f y , c x , c y ) are the internal parameters of the camera 20, the focal length f x , f y , and the optical center c x , c y expressed as a function of.

[0080] When the probabilities respectively set for each feature point Q i in the probability setting process in the feature point probability setting unit 66 are α(u i , v i ), and the weighting coefficient of each feature point Q i used in the optimization calculation process is w i , then w i = α(u i , v i ). That is, in the present embodiment, the probability α(u i , v i , v i ) of each feature point Q i is used as the weighting coefficient w p . And the error function E

Equation

[0081] Here, Equation (11) can be rewritten in vector notation as shown in Equation (12) below.

[0082]

Equation

[0083] Note that in Equation (12), Q i is the coordinate (pixel coordinate) indicating the position of the feature point of the target detected from the target image. Also, "P jCharacters with a "^" appended above the "」" are the coordinates (pixel coordinates) of the target points (object points P in three-dimensional space) calculated from the camera projection model and the parameter group (internal parameters), which are the points obtained by projecting the object point P j onto the image plane IP. Also, in Equation (12), to indicate that the error function E p is a function of the rotation matrix R and the translation vector t of the camera 20, it is denoted as "E p (R, t)". However, it has the same content as the error function E p shown in Equation (11), just with a different notation method.

[0084] In the probability setting process in the feature point probability setting unit 66, as described above, the probability set for each feature point on the target image is made smaller as it approaches the image periphery from the central part of the image. Therefore, the contribution degree (weight) of each feature point to the error function E p (or the error function E p (R, t) shown in Equation (12)) is smaller in the image periphery than in the central part of the image. This makes it possible to effectively suppress the occurrence of periodic errors in the self-position estimation process of the camera 20.

[0085] The optimization calculation unit 68 obtains the transformation matrix [R|t] for which the error function E p (or the error function E p (R, t) shown in Equation (12)) becomes the minimum value through non-linear optimization calculation (step S16). Specifically, using various non-linear solution methods such as the steepest descent method, conjugate gradient method, Adam, etc., the optimal parameters (each parameter defining the transformation matrix [R|t]) for which the gradient ▽E p shown in the following Equation (13) becomes 0 are obtained through iterative calculation. The transformation matrix [R|t] obtained in the optimization calculation unit 68 indicates the self-position (position and orientation) of the camera 20 in the world coordinate system Σ w .

Number

[0086] Note that in equation (13), R x , R y , R z This represents the rotation components in the x, y, and z directions of the rotation matrix R.

[0087] The self-position estimation unit 62 (optimization calculation unit 68) stores the information indicating the position and orientation of the camera 20 (camera self-position information) 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.

[0088] Next, the determination processing unit 70 determines whether or not to repeat the self-position estimation process of the camera 20 (step S18). Specifically, while the probe head 12 is measuring the three-dimensional coordinates of the workpiece, the determination processing unit 70 determines whether to continue the self-position estimation process of the probe head 12 (YES in step S18), and repeats the processes from step S10 to step S16. 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.

[0089] On the other hand, when the measurement of the three-dimensional coordinates of the workpiece by the probe head 12 is completed, the determination processing unit 70 determines that the self-position estimation process of the camera 20 has finished (NO in step S18), and terminates this flowchart.

[0090] 〔effect〕 Next, the effects of the first embodiment will be described.

[0091] Figure 8 is a graph illustrating the effects of the first embodiment. The graph in Figure 8 shows the self-position estimation error when the self-position estimation process is performed after the probability setting process is performed, and the way to interpret the graph is the same as in Figure 12.

[0092] In the first embodiment, the contribution (weight) of each feature point to the self-localization process (optimization calculation process) is smaller in the peripheral part of the image than in the central part, so the influence of the target entering and leaving the camera 20's field of view can be effectively suppressed. As a result, as shown in Figure 8, the self-localization error of the camera 20 does not undergo the periodic changes shown in the graph in Figure 12 (when probability setting processing is not performed), and moreover, the self-localization error is relatively small overall regardless of the amount of movement of the camera 20.

[0093] Therefore, according to the first embodiment, it is possible to perform high-precision self-position estimation of the camera 20 while suppressing the occurrence of periodic errors in the self-position estimation process.

[0094] <Second Embodiment> Next, a second embodiment will be described. In the first embodiment described above, the probability set for each feature point is a value between 0 and 1, and the probability of the feature point is used as a weighting coefficient in the optimization calculation process. In contrast, in the second embodiment, the probability set for each feature point is a value of 0 or 1, and the decision of whether to adopt or reject a feature point in the optimization calculation process is made based on the probability of the feature point. In the following description, the differences between the second embodiment and the first embodiment will be explained, and the explanation of the points common to the first embodiment will be omitted. Also, the same reference numerals will be used to describe the same configuration as in the first embodiment described above.

[0095] Figure 9 shows an example of a probability distribution image in the second embodiment. As shown in Figure 9, the probability distribution image 32A of the second embodiment has the same size as the target image and is an image (map) that represents the probability of a feature point at each position (pixel), similar to the probability distribution image 30A of the first embodiment. However, it differs in that the probability is either 0 or 1 (i.e., only two values). In Figure 9, white positions correspond to a probability of 1, and black positions correspond to a probability of 0.

[0096] The probability distribution image 32A of the second embodiment was created based on the probability distribution image 30A of the first embodiment. Specifically, in the probability distribution image 30A of the first embodiment (u i , v i The probability of being at position ) is α(u i , v i ) and in the probability distribution image 32A of the second embodiment, (u i , v i The probability at the position of ) is β(u i , v i In this case, the probability distribution of the probability distribution image 32A of the second embodiment is set according to the following equation (14).

number

[0097] Note that rand i is a uniformly distributed random number (uniform random number) in the interval [0.0, 1.0]. Equation (14) is (u i , v i Random number at the position of i and probability α(u i , v i ) compare with random number rand i The probability is α(u i , v i If the value is less than or equal to ), the probability β(u i , v i Set ) to 1, and a random number ran i The probability is α(u i , v i If it is greater than ), the probability β(u i , v i This represents setting ) to 0.

[0098] Thus, in the probability distribution image 32A of the second embodiment, a random value (0 or more and 1 or less) is set as the probability threshold, and each position (u i , v i The probability α(u i , v i Based on the comparison of the magnitude of the position (u i , v iThe probability β(u i , v i The probability β(u) is determined. Note that the probability distribution image 32A of the second embodiment is as shown in Figure 9, i , v i It is represented as a two-color image that has been binarized according to the value of (0 or 1).

[0099] Furthermore, as mentioned above, the probability distribution image 32A of the second embodiment is created based on the probability distribution image 30A of the first embodiment, and therefore inherits the properties of the probability distribution of the probability distribution image 30A of the first embodiment. The proportion of probabilities that are 1 is high in the center of the image, and the proportion of probabilities that are 0 gradually increases as you move from the center of the image towards the periphery. In addition, since the probabilities at each position (pixel) of the probability distribution image 32A are randomly distributed by random numbers, the accuracy of estimating the camera 20's own position can be improved by the overall averaging effect.

[0100] In Figure 9, due to drawing constraints, the probability distribution image 32A is shown as a black and white two-color image. However, any image that can represent the probability of each position as a value of 0 or 1 is acceptable, and of course, a two-color image consisting of colors other than black and white is also acceptable. The same applies to the probability distribution image 32B described later.

[0101] Figure 10 shows another example of a probability distribution image in the second embodiment. The probability distribution image 32B shown in Figure 10, like the probability distribution image 32A shown in Figure 9, has the same size as the target image and is an image (map) in which the probability of feature points at each position (pixel) is represented by a value of 0 or 1.

[0102] The probability distribution image 32B shown in Figure 10 was created based on the probability distribution image 30B of the first embodiment. The specific creation method is the same as that of the probability distribution image 32A, so to avoid repetition, the explanation is omitted here.

[0103] Furthermore, the probability distribution image 32B shown in Figure 10 inherits the properties of the probability distribution of the probability distribution image 30B of the first embodiment. Therefore, compared to the probability distribution image 32A shown in Figure 9, the area in the center of the image where the probability is 1 is wider, while the area in the peripheral part of the image where the probability is 0 is narrower and more limited.

[0104] Figure 11 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 the second embodiment.

[0105] As shown in Figure 11, in the self-localization process of the second embodiment, similar to the first embodiment, the feature point detection unit 64 performs feature point detection processing (step S12), and then the feature point probability setting unit 66 performs probability setting processing to set the probability of each feature point (step S14). At that time, the probability of each feature point is set to a value of 0 or 1 according to the probability distribution image 32A shown in Figure 9 (or the probability distribution image 32B shown in Figure 10).

[0106] Next, the optimization calculation unit 68 performs a judgment process for each feature point on the target image to determine whether or not to include it in the calculation of the error function E shown in equation (9) (step S15), before performing the process of calculating the error function E shown in equation (9) (optimization calculation process). Equation (9) is shown again below.

number

[0107] In this decision process, a decision is made whether to include or exclude a feature point in the calculation of the error function E, based on the probability set for each feature point in the probability setting process by the feature point probability setting unit 66. Specifically, if the probability of a feature point is 1, it is "included" in the calculation of the error function E. On the other hand, if the probability of a feature point is 0, it is "excluded" in the calculation of the error function E. As a result, among the feature points on the target image, the proportion of feature points that are "excluded" in the calculation of the error function E increases as you move closer to the periphery of the image compared to the center of the image.

[0108] Next, the optimization unit 68 uses only the feature points on the target image that have been determined to be "accepted" by the judgment process described above to perform the calculation of the error function E shown in equation (9) (step S16). Specifically, the optimization unit 68 obtains the transformation matrix [R|t] that minimizes the error function E shown in equation (9) by nonlinear optimization. In this case, the "N" above the summation symbol "Σ" represents the number of feature points that were determined to be "accepted" in the judgment process.

[0109] The transformation matrix [R|t] obtained in this way is the world coordinate system Σ w This represents the self-position (position and orientation) of camera 20. The subsequent processing is the same as in the first embodiment.

[0110] According to the second embodiment, before the optimization calculation is performed, a decision process is performed for each feature point on the target image to determine whether or not to include that feature point in the optimization calculation (summation of the error function E). This provides the same effect as the first embodiment and also reduces the computational cost of the optimization calculation.

[0111] Furthermore, in the second embodiment, the probability of each feature point is set to a value of 0 or 1, but it is not limited to this. Any value other than 0 and 1 is acceptable, as long as the decision processing in the optimization calculation unit 68 can determine whether or not to adopt each feature point, or any label (for example, a label indicating "adopted" or "rejected") may be used.

[0112] In the second embodiment, instead of the judgment process described above, the probability of feature points may be used as a weighting coefficient in the optimization calculation process, similar to the first embodiment.

[0113] Furthermore, in the first embodiment described above, the optimization calculation was performed by weighting each feature point based on its probability. However, similar to the second embodiment, it is also possible to perform a decision process before the optimization calculation is performed to determine whether or not to include each feature point in the optimization calculation based on its probability, without weighting the feature points. In that case, for example, if the probability of a feature point is greater than or equal to a predetermined probability threshold (fixed value), the feature point can be "included," and if the probability of a feature point is less than the probability threshold, the feature point can be "rejected."

[0114] <Other> In the embodiments described above, 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 invention 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. Furthermore, each target group 16 may be various two-dimensional patterns including AR markers (Augmented Reality Markers) or QR codes (Quick Response code, registered trademark).

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

[0116] 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. [Explanation of Symbols]

[0117] 10...Self-localization system, 12...Probe head, 16...Target group, 18...Probe, 20...Camera, 24...Target, 30A...Probability distribution image, 30B...Probability distribution image, 32A...Probability distribution image, 32B...Probability distribution image, 50...Self-localization device, 52...Calculation processing unit, 54...Storage unit, 60...Image acquisition unit, 62...Self-localization unit, 64...Feature point detection unit, 66...Feature point probability setting unit, 68...Optimization calculation unit, 70...Decision processing unit

Claims

1. An image acquisition unit that acquires images of a group of targets, in which multiple targets are arranged periodically, captured by a camera, A feature point detection unit that detects feature points indicating the positions of each of the targets in the aforementioned image, A feature point probability setting unit that sets a probability for each of the feature points, wherein the feature point probability setting unit makes the probability for the feature points in the peripheral part of the image smaller than the probability for the feature points in the central part of the image, An optimization calculation unit that, based on the probabilities set for each of the feature points, calculates the camera's own position that minimizes the reprojection error of each target in the target group; A self-position estimation device equipped with the following features.

2. The feature point probability setting unit sets the probability for the feature point according to probability distribution information that shows the correspondence between the location of the feature point and the probability. The self-position estimation device according to claim 1.

3. The aforementioned probability distribution information is a probability distribution image showing the aforementioned correspondence. The self-position estimation device according to claim 2.

4. The aforementioned probability distribution image is an image based on a Gaussian distribution. The self-position estimation device according to claim 3.

5. The optimization calculation unit performs the optimization calculation by weighting each feature point based on the probability for each feature point. A self-position estimation device according to any one of claims 1 to 4.

6. The optimization calculation unit determines whether to adopt each feature point in the optimization calculation based on the probability for each feature point, and performs the optimization calculation using the feature points that it has determined to adopt. A self-position estimation device according to any one of claims 1 to 4.

7. An image acquisition step involves capturing images of a group of targets, in which multiple targets are arranged periodically, using a camera. A feature point detection step of detecting feature points that indicate the position of each of the targets in the aforementioned image, A feature point probability setting step of setting a probability for each of the feature points, wherein the probability for the feature points in the peripheral part of the image is smaller than the probability for the feature points in the central part of the image; An optimization calculation step in which, based on the probabilities set for each feature point, the camera's own position in the target group is determined by optimization calculation to minimize the reprojection error of each target; A self-localization method comprising the following features.

Images