JP3208900B2 - Method and apparatus for recognizing three-dimensional position and orientation based on vision - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention is based on vision that recognizes the three-dimensional position and orientation of a monocular camera based on image information of a known object captured by a monocular camera mounted on an autonomous mobile robot or the like. The present invention relates to a three-dimensional position and orientation recognition method and apparatus.

[0002]

2. Description of the Related Art In recent years, active operations such as moving an object such as an autonomous mobile robot so as to approach a target object having a known shape and absolute coordinate position and grasping the target object by an arm are performed. There's something we did. In such autonomous mobile robots, CCDs
It is necessary to mount imaging means such as a camera and recognize its own three-dimensional relative position with respect to the target object based on the two-dimensional image information. In this case, in an apparatus for performing position recognition, it is required to perform calculations with high accuracy and speed to obtain three-dimensional position data, and a configuration that can be realized at low cost is desired.

[0003] Conventionally, as a relatively simple device, for example, a device as disclosed in Japanese Patent Application Laid-Open No. Hei 3-166072 or Japanese Patent Application Laid-Open No. Hei 3-166073 has been considered. That is, in these, a special geometric shape (referred to as a marker) having a known shape and dimensions fixed to the target device is photographed by a camera, and the center of gravity is determined based on the two-dimensional image information of the marker. The relative three-dimensional positional relationship of the camera with respect to the target device is obtained by a method such as calculation.

[0004]

However, according to the above-described conventional method, since it is necessary to provide a marker on the target device, it is necessary to prepare for the installation each time, and the marker must be provided for the marker. Since it is necessary to always set the optical axis of the camera vertically, there is a problem that it is difficult to apply the camera to an arbitrary posture.

As a theoretical method capable of solving such a problem, a method of inversely calculating parameters representing the position and orientation of a camera from components of a perspective transformation matrix estimated by least squares in a perspective n-point problem is generally used in computer vision. It exists as a method. According to this method, even when the relationship between the optical axis of the camera and the origin of the absolute coordinates describing the target object is unknown, it is possible in principle to obtain parameters representing the position and orientation of the camera. .

However, in such a computer vision method, when an explicit calculation is actually performed using a computer, the applied formula becomes extremely complicated and the amount of calculation is enormous. Therefore, rapid arithmetic processing becomes difficult, and practical application becomes difficult.

SUMMARY OF THE INVENTION The present invention has been made in view of the above circumstances, and an object of the present invention is to provide a camera which can be used even when the relationship between the optical axis of the camera and the origin of absolute coordinates describing a target object having a known shape is unknown. Based on the two-dimensional image information taken by
It is an object of the present invention to provide a method and apparatus for recognizing a three-dimensional position and orientation based on visual information, which can relatively easily and quickly obtain a parameter representing the position and orientation of a camera.

[0008]

According to the present invention, there is provided a method for recognizing a three-dimensional position and orientation based on vision, based on image information obtained by photographing a target object whose shape has known three-dimensional absolute coordinates by an image pickup means. An image processing step of extracting two-dimensional image feature points indicating characteristics of the shape of the target object; and an image feature point extracted in the image processing step based on the approximate position and orientation of the imaging means, and a storage means. A correspondence list of feature points between the pattern of the appearance of the target object stored in advance is selected, and based on the correspondence list, the appearance of the target object at the current position and orientation of the imaging unit is determined. A least-squares estimation step of obtaining an estimated perspective transformation matrix by performing a least-squares estimation of the camera matrix to be represented, and corresponding to a required calculation accuracy according to a rough estimated distance between the imaging unit and the target object. A search area setting step of setting a search area having a quantization space point of a degree, and a two-dimensional image feature point to be obtained when the target object is photographed by the imaging means at the quantization space point of the search area. A perspective transformation matrix for calculation is calculated based on an estimated position of the quantization space point with respect to the three-dimensional absolute coordinates of the target object stored in advance in the storage unit and a posture parameter indicating an imaging posture of the imaging unit. A perspective transformation matrix operation step,
An evaluation error value indicating the degree of conformity between the perspective transformation matrix calculated in the perspective transformation matrix calculation step and the estimated perspective transformation matrix calculated in the least square estimation step is calculated based on a predetermined error evaluation function. Error calculating step, and comparing the evaluation error value obtained in the error calculating step with a required error precision set corresponding to the search area, and the evaluation error value becomes smaller than the required error precision. As a determination condition, an error determination step in which the search area setting step, the perspective transformation matrix calculation step, and the error calculation step are repeatedly executed, and when the determination condition is satisfied in the error determination step, the quantum at that time is determined. It is characterized in that it comprises a data output step of outputting the three-dimensional absolute coordinates of the image space point and the attitude parameter of the imaging means.

In addition, the apparatus for recognizing a three-dimensional position and orientation based on vision according to the present invention comprises: an image pickup means for outputting image information;
Image processing means for extracting a two-dimensional image feature point indicating a characteristic of the shape of the target object based on image information when the target object whose three-dimensional absolute coordinates indicating the shape is known is photographed by the imaging means, A correspondence list of feature points between the image feature points extracted by the image processing means and the pattern of the appearance of the target object stored in advance in the storage means is selected based on the approximate position and orientation of the imaging means. And a least-squares estimating means for performing a least-squares estimation of a camera matrix representing the appearance of the target object at the current position and orientation of the imaging means based on the correspondence list to obtain an estimated perspective transformation matrix; Search area setting means for setting a search area having a quantized space point having a density corresponding to a required calculation accuracy according to a rough estimated distance between the means and the target object; The perspective transformation matrix for calculating the two-dimensional image feature points to be obtained when photographing the object by the imaging unit in the quantization space point range,
Perspective transformation matrix computing means for performing computation based on the estimated position of the quantization space point with respect to the three-dimensional absolute coordinates of the target object stored in advance in the storage means and a posture parameter indicating the imaging posture of the imaging means; Error calculation for calculating an evaluation error value indicating a degree of conformity between the perspective transformation matrix obtained by the transformation matrix calculation means and the estimated perspective transformation matrix obtained by the least-squares estimation means based on a predetermined error evaluation function Means, and comparing the evaluation error value obtained by the error calculation means with a required error accuracy set corresponding to the search area, and determining that the evaluation error value is smaller than the required error accuracy. Error determination means for causing the search area setting means, the perspective transformation matrix calculation means, and the error calculation means to repeatedly execute the calculation; When the judgment condition is satisfied by the step, characterized in was constructed from the data output means for outputting a three-dimensional absolute coordinates of the quantized spatial points at that time and attitude parameters of the imaging means.

[0010]

According to the method for recognizing a three-dimensional position and orientation based on vision of the present invention, the three-dimensional absolute coordinates are known by the imaging means mounted on a device or the like which moves and approaches the target object. When the target object is photographed, in the image processing step, a two-dimensional image feature point indicating the shape characteristic of the target object is extracted based on the image information.

Subsequently, in the least squares estimation step, the image feature points extracted in the image processing step and the appearance of the target object stored in the storage means in advance based on the approximate position and orientation of the imaging means. A correspondence list of feature points between the pattern and the corresponding pattern is selected. Thereafter, based on the correspondence list, the minimum of a camera matrix as a transformation matrix representing the appearance of the target object at the current position and orientation of the imaging unit is selected. Square estimation is performed to obtain an estimated perspective transformation matrix.

Next, in a search area setting step, a search area having quantized spatial points of a density corresponding to a required calculation accuracy is set according to the approximate estimated distance between the imaging means and the target object, and then the search area is set. In the perspective transformation matrix calculation step, a perspective transformation matrix for calculating a two-dimensional image feature point to be obtained when the imaging unit is arranged at the quantization space point of the search area and the target object is photographed, The calculation is performed based on the estimated position of the quantization space point with respect to the three-dimensional absolute coordinates of the target object stored in advance in the storage unit and a posture parameter indicating the imaging posture of the imaging unit.

In the error calculation step, an evaluation error value indicating the degree of conformity between the perspective transformation matrix obtained in the perspective transformation matrix calculation step and the estimated perspective transformation matrix obtained in the least square estimation step is calculated. The calculation is performed based on a predetermined error evaluation function. This calculates the shift of the corresponding element in each matrix, and indicates that the smaller the evaluation error value, the higher the degree of matching between the two matrices.

Next, in the error judging step, the evaluation error value obtained in the above-described error calculating step is compared with a required error accuracy set corresponding to the search area, and the evaluation error value The search area setting step, the perspective transformation matrix calculation step, and the error calculation step are repeatedly executed, with the determination condition that the difference is smaller than the error precision,
Finally, when the above condition is met in the error determination step, in the data output step, the three-dimensional absolute coordinates of the quantization space point at that time and the posture parameter of the imaging unit are output. Thus, the quantization space point closest to the three-dimensional absolute coordinates of the actual imaging unit can be obtained as an approximate point, and the posture parameter representing the shift of the optical axis of the imaging unit can be detected.
Based on these data, movement control of the apparatus with respect to the target object can be performed accurately.

Further, according to the apparatus for recognizing a three-dimensional position and orientation based on vision of the present invention, the same operation can be performed by means for performing the above-mentioned steps, and the three-dimensional position of the actual imaging means can be controlled. The quantization space point closest to the absolute coordinates can be obtained as an approximate point, and the posture parameter representing the shift of the optical axis of the imaging means can be detected. This makes it possible to accurately control the movement of the device.

[0016]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS An embodiment in which the present invention is applied to a three-dimensional position recognition apparatus mounted on an autonomous mobile robot running in a factory will be described below with reference to the drawings. In FIG. 2 showing an outline of the entire configuration, an autonomous mobile robot 1
For example, it moves between work areas 2 along a track provided on a floor surface in a factory or the like, and is equipped with a three-dimensional position recognition device (see FIG. 1) of the present invention. An arm 3 is provided on the upper part of the autonomous mobile robot 1, and a camera 4 as an imaging means and a hand 6 for performing work such as grasping a work 5 as a target object are provided at a tip of the arm 3. Have been. In this case, work area 2
The work 5 having a predetermined shape is placed at a predetermined position on the bottom surface 2 a of the work area 2.

Now, the three-dimensional position recognition apparatus is configured as shown in FIG. Image processing unit 7 as image processing means
Comprises a pre-processing calculation unit 8 and a feature point extraction unit 9. The pre-processing calculation unit 8 inputs an image signal of a scene including the work 5 captured by the camera 4 and performs pre-processing, and the feature point extraction unit 9 performs processing based on the image information calculated by the pre-processing calculation unit 8. As will be described later, a two-dimensional image feature point corresponding to the feature point of the work 5 is extracted.

When the image feature points extracted by the image processing unit 7 are given, the least squares estimation unit 10 as a least squares estimation unit, based on the image feature points,
, The appearance pattern to be obtained corresponding to the position and orientation of the camera 4 when the work 5 is photographed is selected, and the least square estimation of the camera matrix for the current position and orientation of the camera 4 is performed. The resulting estimated perspective transformation matrix Ms is calculated and obtained.

The quantization space search section 11 includes a search area setting section 12 as search area setting means, a perspective transformation matrix operation section 13 as perspective transformation matrix operation means, an error operation section 14 as error operation means, and an error determination means. Error determination unit 1 as
5 and a data output unit 16 as data output means.

In this case, as the search area setting step, the search area setting section 12 performs a space search having a plurality of quantization space points E having a density based on the approximate position information of the autonomous mobile robot 1 with respect to the work 5 in the work area 2. An area S is set. The perspective transformation matrix calculation unit 13 performs, as a perspective transformation matrix calculation step, image feature points in an image obtained from an image signal when the camera 4 photographs the work 5 at the quantization space point E in the set space search area S. Is calculated in accordance with the approximate position information of the autonomous mobile robot 1 and the posture parameter of the camera 4.

The error calculating section 14 performs an error calculating step as follows.
Estimated perspective transformation matrix M obtained by least-squares estimator 10
The calculation is performed based on an error evaluation function that calculates an error between s and the perspective transformation matrix M computed by the perspective transformation matrix computation unit 13 as a sum of square errors of each matrix element. The error determination unit 15
As an error determination step, it is determined whether or not the calculated sum of squared errors is smaller than the required error precision ε. If smaller, the data of the position and orientation of the camera 4 at that time is output to the output terminal 17. Search area setting unit 12
Then, the calculation is executed in each of the above-described units. Then, as a data output step, the data output unit 16 outputs the quantization space point E in the space search area S at that time based on the result of the above determination.

The quantization space search unit 11 has a memory 18 as storage means for storing various data described later.
Are connected, and write and read operations of the various data can be performed. An external information input terminal 19 is provided, and a control unit (not shown) for controlling the traveling of the autonomous mobile robot 1 or a position detection sensor (not shown) is provided. , A signal indicating the approximate position of the autonomous mobile robot 1 is input. In this case, the data stored in advance in the memory 18 include a correspondence list of feature points, three-dimensional absolute coordinate data of the work 5 and the work area 2, parameters of the camera 4, a list of quantization space points E and their coordinate data, and the like. It is.

Next, the operation of this embodiment will be described with reference to FIGS. The three-dimensional position recognition device
The existence position of the autonomous mobile robot 1 is recognized in accordance with the steps shown in FIG. First, in the state of FIG. 2, when the autonomous mobile robot 1 moves on the track and approaches the work area 2, the camera 4
When the work area 2 enters the shooting scene and is shot,
The image processing section 7 receives the image signal of the camera 4 (step T1). In this case, the autonomous mobile robot 1
Is moving in a predetermined direction on a predetermined trajectory, and a rough stop position is known from internal information of a control unit for controlling the movement. Can be set so as to fall within the field of view.

Next, a work 5 photographed by the camera 4
Of the image signal of the image processing unit 7, the preprocessing calculation unit 8 of the image processing unit 7 executes a noise removal process (step T2) as a preprocess and a cutout process of the image of the work 5 (step T3), and thereafter, the feature point extraction The unit 9 calculates and outputs a set Q of m image feature points Qj based on the image signal as follows (step T4). Here, the image feature point Q
The set Q of j is defined as follows. Q = {Qj; j = 1, 2,..., M} (1) Qj = (uj, vj) (2) u, v; each element value of two-dimensional coordinates in pixel units on the display screen

In extracting the image feature point Qj, the following calculation is performed based on the approximate position information provided from the input terminal 19 as external information. That is, as the approximate position information, for example, a work plan in which the moving path of the autonomous mobile robot 1 is stored, history information of the spatial movement moved by the travel control, and the like are input. As approximate three-dimensional absolute coordinates of the center position of No. 1, approximate positions A0, A0 = (X0, Y0, Z0) (3) expressed in absolute coordinates can be obtained. Note that the three-dimensional absolute coordinates of the approximate position A0 represent a predetermined position in the work area 2 as the origin Op.

Next, from various appearance patterns (see FIGS. 4A to 4F) of the work 5 placed on the work area 2 stored in the memory 18 in advance, the approximate position A0 is determined. The appearance pattern corresponding to the value is selected (step T5). Now, for example, when the appearance pattern corresponding to the approximate position A0 corresponds to FIG. 4B, a set of three-dimensional feature points Pi of the work 5 appearing in the appearance pattern, P = ｛Pi I = 1, 2,..., N} (4) Image feature points Qj corresponding to The extracted image feature points Qj are stored in the memory 18. This is, for example, as shown in FIG. 9A, the memory 18 in which the coordinates of the feature points Pi of the work area 2 and the work 5 are described.
Is selected in correspondence with the above-mentioned appearance pattern (see FIG. 4 (b)), and the correspondence between the coordinate data of (1) and the image feature point Qj obtained by the image signal shown in FIG. The correspondence list of the feature points is selected based on the.

The least-squares estimating unit 10 performs least-squares estimation of the camera matrix C for the current position and orientation of the camera 4 based on the correspondence list selected as described above (step T6). In this case, the camera matrix C is set based on the following equations (5), (6), and (7).
(U, v, w) represents one point on the image display surface in the homogeneous coordinate system, (U, V) is a coordinate value represented by a coordinate axis of the image display surface, and (x, y, z) is a tertiary coordinate. These are the coordinates of one corresponding point in the original absolute coordinate space. That is, the camera matrix C is a parameter for performing conversion between the coordinates in the three-dimensional absolute space and the image display surface. Note that w represents a scale factor by the camera 4.

[0028]

(Equation 1)

From the equations (5) and (6), u-Uw = 0, v-Vw = 0 (8) is derived. Further, considering the properties of the homogeneous coordinates, the following equation is obtained. The following equations (9), (10), and (11) are obtained.

[0030]

(Equation 2)

Next, when the equations (9), (10) and (11) are substituted into the equation (8), the following equations (12) and (13) are obtained.

[0032]

(Equation 3) From these equations, w as the scale factor can be deleted and transformed as in the following equations (14) and (15).

[0033]

(Equation 4) From this result, an evaluation function represented by the following equations (16) and (17) can be derived, and under the conditions represented by the following equations (18) and (19) that minimize Eu and Ev. Then, a linear equation using each element of the camera matrix C as a coefficient is obtained to finally determine the camera matrix C.

[0034]

(Equation 5) Then, the estimation result of the camera matrix C (perspective transformation matrix in a broad sense) obtained as described above is stored as the estimated perspective transformation matrix Ms.

Next, in the quantization space search unit 11,
The search area setting unit 12 sets the approximate position A0 (X0, Y0
, Z0) are set as the space search areas Sc, Sc (c = 1, 2,..., C) (20) (step T7). This space search area Sc
Are, as shown in FIG. 5, a plurality of quantized spatial points E (k, c), E (k, c) (k = 1, 2,... K, c = 1, 2,..., C) (21)

In this case, the camera 4 faces the work 5 in the plane of the coordinate value X = XL1 in the X-axis direction (the direction facing the work area 2) in the absolute coordinate system based on the origin Op of the work area 2. Can be assumed. Here, XL1 is an estimated value of the X coordinate value indicating the position of the viewpoint of the camera 4, and this value is obtained from the coordinate value X0 of the approximate position A0 of the autonomous mobile robot 1 in the X direction and the relative coordinates of the camera 4 on the robot. Can decide. Further, in the case of the autonomous mobile robot 1 in the present embodiment, since the coordinate value Z0 in the height direction of the approximate position A0 is assumed not to largely deviate from a certain value, the coordinate value of the camera 4 in the Z direction is assumed. Can be narrowed down to a substantially narrow range.

Next, a search range of various parameters of the camera 4 is set (step T8). Here, as the parameters of the camera 4, as shown in a correspondence relationship in FIG. 6, (a) a posture parameter CP (α, β, γ) of the camera 4, (b) a focal length characteristic f (c) of the camera 4 There is a position (r, θ, φ) of the camera 4. In this case, since the focal length characteristic f in (b) is determined in advance by the characteristic of the camera 4, it can be stored as data in the memory 18, for example. Further, the value of the position (r, θ, φ) of the camera 4 in (c) is a value that can be determined in the subsequent calculation steps.

Therefore, here, the search range of the posture parameter CP (α, β, γ) representing the deviation of the optical axis from the center position of the work 5 of the camera 4 may be determined.
That is, the search range of the posture parameter CP of the camera 4 is, for example, as shown in FIG.
Image position Ops of origin Op as predetermined position of work 5
(If it is not included in the image information, it is converted into a coordinate system with the feature point included as the origin.) In pixel-based two-dimensional coordinates (u, v) on the image display screen, for example, (Na , Nb), the estimated values of α and β can be calculated from the following equations.

Α = 2 × arctan ｛(Na / NH) × tan (wH / 2)｝ (22) β = 2 × arctan ｛(Nb / NV) × tan (wV / 2)｝ (23) , NH; the horizontal image size of the image on the display screen NV; the vertical image size of the image on the display screen wH; the horizontal field angle of the camera wV; the vertical field angle of the camera.

In this case, if the required error accuracy is not very high, then, as a linear approximation, α = (Na / NH) × wH (24) β = (Nb / NV) × wV (25) It can be substituted.

If γ = 0 is not satisfied, the work 5
Annular region Sa (Op including the image position Ops at the center of
Since it is considered that Opsc when γ is corrected exists in s), the quantization space point Ops in Sa (Ops) is considered.
Equation c (22), (23) or equation (24)
Based on (25), α and β are determined, and γ = γpsc (the angle between Ops and Opsc), and the subsequent search step is performed. In the present embodiment, the autonomous mobile robot 1 can move on a substantially horizontal floor and adjust the rotation angle of the camera 4 with high accuracy using the coordinate value of the approximate position A0. Among the attitude parameters (α, β, γ) for the optical axis shift, the one that requires the widest search capability is α representing the horizontal angle shift. Therefore, it is sufficient to search for a minute range for β and γ.

Next, the perspective transformation matrix operation unit 13 sets the quantized spatial points E as initial search points in the spatial search area S1.
A quantization space point E (1,1) which is one point of (k, 1) (k = 1, 2,..., K) is selected (step T9).
The three-dimensional absolute coordinates and the posture parameter CP of the camera 4
Using the initial values of (α, β, γ), the perspective transformation matrix M (k, c) (k = 1, 2,..., K, c = 1, 2,..., C) (26) An operation is performed (step T10).

In this case, the perspective transformation matrix M is calculated as follows. That is, first, the origin Op of the work 5
Three-dimensional absolute coordinates (X, Y, Z) set for
, The initial search area S1 is set within the plane of X = X0, which is the X coordinate value of the approximate position A0, and the three-dimensional absolute coordinates (X, Y, Z) in the initial search area S1 are converted to the three-dimensional polar coordinates (r , Θ, φ). Based on the posture parameter CP (α, β, γ) of the camera 4 and the viewpoint position of the camera 4 obtained from the focal length characteristic f and the distance d to the projection plane, the perspective transformation matrix M is obtained based on the relationship shown in FIG. (K, c) is obtained.

Here, the perspective transformation matrix M (k, c) can be expressed as follows. M (k, c) = T ・ M1 ・ M2 ・ M3 (27) where T: a transformation matrix for rotation, parallel movement, enlargement, reduction, etc. (here, the rotation matrix represented by the posture parameter CP of the camera 4) Calculation) M1: Perspective transformation of the work being photographed into the projection space M2: Perspective transformation from the plane in the projection space to the projection plane M3: Coordinate transformation from the projection plane to the display surface of the image The product of the perspective transformation matrices M1 and M2 for calculating the perspective transformation between the surface and the work 5 and the coordinate transformation matrix M3 are given by the following equations (28) and (29), respectively.
Can be expressed as

[0045]

(Equation 6) Also, the posture parameter C corresponding to the optical axis shift of the camera 4
The transformation matrix T for performing the rotation operation in accordance with P (α, β, γ) is given by the following equations (30), (31), and (32) based on the relationship shown in FIGS. Can be expressed as At this time, in the conversion of the optical axis shift, assuming that the distance from the workpiece 5 does not change, T0 = T1.
Therefore, the following equation (33) holds.

[0046]

(Equation 7) Next, the error calculation unit 14 calculates an error evaluation function D () between the perspective transformation matrix M (k, r) obtained by the calculation in this way and the estimated perspective transformation matrix Ms obtained by the least square estimation unit 10. (1, 1) is calculated (step T11). In this case, the error evaluation function D (k, c) calculates the sum of the square errors between the matrix elements of the perspective transformation matrix M and the estimated perspective transformation matrix Ms.

Subsequently, the current quantization space point E (1, 1)
In the posture parameters CP (α, β, γ), α and β are changed while γ remains unchanged, and the error evaluation function D (nE, 1) (nE = 1, 2,. N
E) (Steps T12, 13, 10, 1)
1). Thereafter, DE1 (αE, βE, γE) is calculated as an optimum value from these error evaluation functions D (nE, 1) (step T14). Subsequently, in the error determination unit 15, the calculated value of the optimum value DE1 obtained from the value of the calculated error evaluation function D (k, 1) is used as the value of the required error accuracy ε as a preset detection condition. It is determined whether or not the following determination condition is satisfied: DE k <ε (34)
15).

If the above-mentioned determination condition expression (37) is not satisfied, the above operation is similarly performed for the other quantization space points E (k, 1) in the initial search area S1 ( After steps T16 and T17, step T
9 to T15). Further, other quantization space points E
If the determination condition expression (34) is not satisfied for (k, 1), the quantization space point E (k, 1) at which a value closest to the required error accuracy ε is obtained from the optimum value DE k at that time. 1)
A new space search area S2 is set based on the position (steps T18 and T19), and the above-described calculation process is executed again (steps T5 to T15).

On the other hand, when the condition of the criterion condition (34) is satisfied (step T15), the data output unit 16 outputs the quantization space point E corresponding to the optimum value DE k at that time.
The value of the position (rE, θE, φE) of (k, c) and the value of the posture parameter (αE, βE, γE) of the camera 4 are output to the output terminal 17 as detection data (step T2).
0), end the search program.

If the determination condition expression (34) is not satisfied despite the setting of the final space search area SC (step T18), the quantization space point E satisfying the required error accuracy ε is obtained.
After outputting a message that (k, c) does not exist (step T21), the optimum value D calculated up to that time is output.
The position (rE, θE, φE) of the quantization space point E (k, c) at which the value closest to the required error precision ε is obtained from Ek
And the posture parameters of the camera 4 (αE, βE, γ
The value of E) is output to the output terminal 17 as detection data (step T20), and the search program ends.

In the above case, at a certain distance (X = XL1) from the origin Op of the work 5 in the work area 2, the quantization space point E (k, 1) satisfying the required error accuracy ε is obtained.
Is set to S1, S2,
.. Are shown. For example, FIG.
Executing a search process of setting a spatial search area S (k, c) according to the position of the autonomous mobile robot 1 as it approaches (X <XL1) to the work 5 as the autonomous mobile robot 1 advances. Can also.

That is, for example, in the course of the movement of the autonomous mobile robot 1, one space search area S1 is set corresponding to the distance X from the origin Op, and the quantization space point E (k, 1) in the space search area S1 is set. Among them, the one closest to the required error accuracy ε is obtained as temporary detection data indicating the position and orientation parameters of the camera 4, the amount of movement of the autonomous mobile robot 1 is controlled based on the detection data, and the distance X from the origin Op is calculated. Becomes smaller, a new space search area S2 is set, and the quantization spatial point E closest to the required error accuracy ε is further set.
(K, 2) is calculated, and similarly, when the autonomous mobile robot 1 is closest to the workpiece 5, the search process may be set so as to finally satisfy the required error accuracy ε. good.

As described above, in the present embodiment, the work area 2 and the work 5 as a target object having a known shape are imaged by the camera 4 and the image feature points Qj are obtained from the image information.
Is extracted by the least-squares estimating unit 10, and a correspondence list of feature points between the extracted image feature points and the appearance pattern of the work 5 is selected, and the appearance of the work 5 at the current position and orientation of the camera 4 is selected. The estimated perspective transformation matrix Ms is obtained by performing least-squares estimation of the camera matrix representing the two directions, while a spatial search area Sc having a plurality of quantized spatial points is set, and the posture parameters CP (α, β , Γ), and calculates a perspective transformation matrix M (k, c) for obtaining an image feature point of the work 5 to be obtained when the work 5 is imaged at the quantization space point E (k, c). The position and orientation parameters C of the camera 4 are obtained by determining whether or not the required error accuracy ε is satisfied by the error evaluation function D (k, c) by obtaining the correspondence between them and performing matching.
Since P is estimated, the following effects can be obtained.

First, unlike the related art, there is no need to provide a special marker, and no moving time for marker recognition is required, so that the calculation time can be reduced and the speed can be increased. Second, instead of calculating parameters from the components of the perspective transformation matrix, a spatial search area Sc having quantized spatial points E arranged at a predetermined density is set to obtain a perspective transformation matrix,
Since the degree of conformity with the estimated perspective transformation matrix is evaluated, arithmetic processing can be executed quickly without an enormous amount of arithmetic.
Third, the posture parameter CP (α, β, γ) of the camera 4
Is applicable even if is unknown. Fourth, the accuracy of the calculation for obtaining the position of the camera 4 can be adjusted by how to obtain the quantization space point E. That is, for example, in consideration of the work content and the actual circumstances of the environment, it is set so that high-speed calculation is performed with coarse accuracy at long distances, and accurate calculation is performed with fine accuracy when approaching the work 5. It can be set to an appropriate accuracy according to the situation. Fifth, since the calculation is performed in this manner, when the camera 4 is actively moving with respect to the workpiece 5 as the target object, the optical axis alignment at a long distance (coarse estimation) can be used for a short distance. Until highly accurate estimation, it can be used in multiple stages in conjunction with the mobile system.

[0055]

As described above, according to the method for recognizing a three-dimensional position and orientation based on vision according to the present invention, the image processing means captures an object whose three-dimensional absolute coordinates indicating its shape are known by the image processing step. The two-dimensional image feature points indicating the characteristics of the shape of the target object are extracted based on the image information obtained when the image is captured, and in the least square estimation step, the extracted image is extracted based on the approximate position and orientation of the imaging means. A correspondence list between the feature points and the appearance pattern of the target object stored in the storage means is selected, an estimated perspective transformation matrix is obtained by least-squares estimation of the camera matrix, a search area setting step, a perspective transformation matrix calculation step, An error calculation step, an error determination step, and a data output step set a search area corresponding to the approximate estimated position of the imaging means, and set a plurality of quantized spatial points in the search area. When the perspective transformation matrix for the elephant object is calculated and the error with the estimated perspective transformation matrix is evaluated by an error evaluation function and the required error accuracy is satisfied, the quantization space point at that time is used as the three-dimensional absolute coordinate position of the imaging means. In addition to the estimation, the posture parameter of the imaging unit is output as the posture. Therefore, even if there is an optical axis deviation of the imaging unit, the position and posture of the imaging unit can be quickly and accurately determined without using a special marker. Recognition can be performed, and the balance between the calculation speed and the calculation accuracy can be appropriately set according to the distance depending on the method of setting the required error accuracy. Based on these data, the movement control of the apparatus with respect to the target object can be performed. And the like can be accurately performed.

Further, according to the apparatus for recognizing three-dimensional position and orientation based on vision of the present invention, the same operation can be performed by the means for executing the above-described steps, and the three-dimensional operation of the actual imaging means can be performed. The quantization space point closest to the absolute coordinates can be obtained as an approximate point, and the posture parameter representing the shift of the optical axis of the imaging means can be detected. There is an excellent effect that the movement control of the device can be accurately performed.

[Brief description of the drawings]

FIG. 1 is a block diagram of a functional configuration showing an embodiment of the present invention.

FIG. 2 is a diagram showing a positional relationship between an autonomous mobile robot and a work area.

FIG. 3 is a flowchart illustrating a process flow.

FIG. 4 Various appearance patterns

FIG. 5 is an operation explanatory view showing a positional relationship between a space search area and a work area.

FIG. 6 is a diagram showing a relationship between absolute coordinate axes and coordinate axes of a camera and an image.

FIG. 7 is an explanatory diagram showing a relationship between a workpiece on an image display unit and a posture parameter of a camera.

FIG. 8 is an operation explanatory diagram showing a relationship between coordinate axes for calculating a perspective transformation matrix.

FIGS. 9A and 9B are operation explanatory diagrams showing a correspondence relationship between (a) a definition of a feature point of a work area and a work and (b) an image feature point based on an image signal.

FIG. 10 is an operation explanatory diagram when a search area is set according to a distance from a workpiece;

[Explanation of symbols]

1 is an autonomous mobile robot, 2 is a work area, 4 is a camera (imaging means), 5 is a work (target object), 7 is an image processing unit, 8 is a pre-processing calculation unit, 9 is a feature point extraction unit, and 10 is a minimum. A square estimating unit (least square estimating means), 11 is a quantization space searching unit, 12 is a search area setting unit (search area setting means), 1
3 is a perspective transformation matrix computing unit (perspective transformation matrix computing means), 1
Reference numeral 4 denotes an error calculation unit (error calculation unit), 15 denotes an error determination unit (error determination unit), 16 denotes a data output unit (data output unit), 18 denotes a memory (storage unit), and 19 denotes an external information input terminal.

Continuation of the front page (58) Field surveyed (Int.Cl. ⁷ , DB name) G01B 11/00-11/30 102 B25J 9/10-9/22 B25J 13/00-13/08 G06T 1/00

Claims (2)

(57) [Claims] 1. An image for extracting a two-dimensional image feature point indicating a feature of a shape of a target object based on image information obtained by photographing the target object having a known three-dimensional absolute coordinate indicating a shape by an imaging unit. Processing step, based on the approximate position and orientation of the imaging means, the feature points between the image feature points extracted in the image processing step and the pattern of the appearance of the target object stored in advance in the storage means A least-squares estimation step of selecting a correspondence list and performing a least-squares estimation of a camera matrix representing the appearance of the target object at the current position and orientation of the imaging means based on the correspondence list to obtain an estimated perspective transformation matrix A search area setting step of setting a search area having a quantized spatial point having a density corresponding to a required calculation accuracy according to a rough estimated distance between the imaging unit and the target object; A perspective transformation matrix for calculating a two-dimensional image feature point to be obtained when the target object is photographed by the imaging unit at the quantization space point of the search area is stored in the storage unit in advance. A perspective transformation matrix calculation step of calculating based on the estimated position of the quantization space point with respect to the three-dimensional absolute coordinates and an attitude parameter indicating the imaging attitude of the imaging means; An error calculation step of calculating an evaluation error value indicating the degree of conformity between the perspective transformation matrix and the estimated perspective transformation matrix obtained in the least square estimation step based on a predetermined error evaluation function; Compare the obtained evaluation error value with the required error accuracy set corresponding to the search area,
An error determining step of repeatedly executing the search area setting step, the perspective transformation matrix calculating step, and the error calculating step, on the condition that the evaluation error value is smaller than the required error accuracy, A method for recognizing a three-dimensional position and orientation based on vision, comprising a data output step of outputting the three-dimensional absolute coordinates of the quantized space point at that time and the orientation parameter of the imaging means when the determination condition is satisfied. 2. An image pickup means for outputting image information, and a characteristic of the shape of the target object based on image information obtained by photographing the target object whose three-dimensional absolute coordinates indicating the shape is known by the image pickup means. Image processing means for extracting two-dimensional image feature points; image feature points extracted by the image processing means based on the approximate position and orientation of the imaging means; and the appearance of the target object stored in advance in storage means. And selecting a correspondence list of feature points between the two patterns and performing a least-squares estimation of a camera matrix representing the appearance of the target object at the current position and orientation of the imaging unit based on the correspondence list. A least-squares estimating unit for obtaining an estimated perspective transformation matrix; and a search having a quantized spatial point having a density corresponding to a required calculation accuracy according to a rough estimated distance between the imaging unit and the target object. Search area setting means for setting a search area; and a perspective transformation matrix for calculating a two-dimensional image feature point to be obtained when the target object is photographed by the imaging means at a quantization space point of the search area. A perspective transformation matrix calculating unit that calculates based on an estimated position of the quantization space point with respect to the three-dimensional absolute coordinates of the target object stored in advance in the storage unit and a posture parameter indicating an imaging posture of the imaging unit; An error for calculating an evaluation error value indicating a degree of conformity between the perspective transformation matrix calculated by the perspective transformation matrix calculation unit and the estimated perspective transformation matrix calculated by the least square estimation unit based on a predetermined error evaluation function. Calculating means, comparing the evaluation error value obtained by the error calculating means with a required error accuracy set corresponding to the search area,
An error determining unit that causes the search area setting unit, the perspective transformation matrix calculating unit, and the error calculating unit to repeatedly execute the calculation, based on a determination condition that the evaluation error value is smaller than the required error accuracy; When the determination condition is satisfied by the determination means, the three-dimensional absolute coordinate of the quantization space point at that time and the data output means for outputting the attitude parameter of the imaging means. Recognition device.