JP2778430B2 - Three-dimensional position and posture recognition method based on vision and three-dimensional position and posture recognition device 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 a three-dimensional position and orientation recognition device based on vision.

[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 a device for performing position recognition, it is required to perform calculation 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 calculation 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
Provided are a vision-based three-dimensional position and orientation recognition method and a vision-based three-dimensional position and orientation recognition device that enable relatively simple and quick determination of parameters representing the position and orientation of a camera. It is in.

[0008]

According to the present invention, a method for recognizing a three-dimensional position and orientation based on vision is based on an image signal 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 a two-dimensional image feature point indicating a characteristic of the shape of the target object, and a plurality of parameters indicating the position and orientation of the imaging unit with respect to the target object as search parameters, and estimation provided by external information. A search area setting step of setting a parameter search start point according to the distance and setting a spatial search area having a parameter search point quantized with a parameter search width corresponding to the estimated distance; and setting in the search area setting step Obtained when the target object is imaged by the imaging means from a position determined by the parameter search point of the space search area. A perspective transformation matrix for calculating a two-dimensional estimated image feature point to be calculated is obtained by estimating an estimated position of the quantization space point with respect to three-dimensional absolute coordinates of the target object stored in advance in the storage means and an initial search of the imaging means. Calculating a perspective transformation matrix based on the parameters and
A perspective transformation calculation step of calculating the two-dimensional estimated image feature point of the target object at the parameter search point using the perspective transformation matrix obtained in the perspective transformation matrix calculation step; and The correspondence between the extracted two-dimensional image feature points of the target object and the two-dimensional estimated image feature points calculated in the perspective transformation calculation step is determined, and the image feature points from which the correspondences are obtained. An error calculation step of calculating an evaluation error value indicating the degree of conformity between the search parameter and the estimated image feature point based on a predetermined error evaluation function; and performing the search for a specified search parameter of the plurality of search parameters. In the region, the parameter search point such that the evaluation error value of the image feature point with respect to each of the estimated image feature points is minimized is determined as a local optimum parameter. A local optimum parameter calculation step to be newly set as a parameter search point, and this local optimum parameter calculation step, while sequentially executing all of the plurality of parameters in a predetermined order, and repeatedly executing the predetermined number of times; When the evaluation error value at that time is smaller than a predetermined required error accuracy, an optimal position and orientation data determination step of determining the obtained plurality of parameter search values as a plurality of parameters of the position and orientation vector. It is characterized by

Further, 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 an image signal;
Storage means for storing three-dimensional absolute coordinate data indicating the shape of the target object and data specifying the search order; and characteristics of the shape of the target object based on an image signal when the target object is photographed by the imaging means. Image processing means for extracting a two-dimensional image feature point indicating, and a plurality of parameters indicating the position and orientation of the imaging means with respect to the target object as search parameters, and a parameter search start point corresponding to an estimated distance given by external information And a search area setting means for setting a space search area having a parameter search point quantized with a parameter search width corresponding to the estimated distance, and a parameter search for the space search area set by the search area setting means A two-dimensional estimated image to be obtained when the target object is photographed by the imaging means from a position determined by a point A perspective transformation matrix for calculating a feature point 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 an initial search parameter of the imaging unit. Perspective transformation matrix calculation means, and perspective transformation calculation means for calculating the two-dimensional estimated image feature point of the target object at the parameter search point using the perspective transformation matrix determined by the perspective transformation matrix calculation means, While obtaining the correspondence between the two-dimensional image feature points of the target object extracted by the image processing means and the two-dimensional estimated image feature points calculated by the perspective transformation calculation means, Error calculating means for calculating an evaluation error value indicating a degree of conformity between the obtained image feature point and the estimated image feature point based on a predetermined error evaluation function; For the specified search parameter among the search parameters, the parameter search point that minimizes the evaluation error value of the image feature point with respect to each of the estimated image feature points in the search area is determined as a local optimum parameter. A local optimum parameter calculating means to be newly set as a parameter search point; and a calculating process by the local optimum parameter calculating means are sequentially executed for all of the plurality of parameters in a predetermined order. An optimal position and orientation data determining means for repeatedly executing, and when the evaluation error value at that time is smaller than a predetermined required error accuracy, determining a plurality of obtained parameter search values as a plurality of parameters of the position and orientation vector This is characterized in that it is provided with

[0010]

According to the method for recognizing a three-dimensional position and orientation based on vision, the position and orientation of the imaging means with respect to the target object are recognized as follows. That is, when an image signal of a target object photographed by the imaging means is input, a two-dimensional image feature point indicating a shape characteristic or the like of the target object is extracted based on the image signal in the image processing step. Become.

In the search area setting step, a plurality of parameters indicating the position and orientation of the imaging means with respect to the target object are set as search parameters, and a parameter search start point for the search parameters is set according to an estimated distance given by external information. At the same time, a spatial search area having a parameter search point quantized with a parameter search width determined according to the estimated distance is set.

In the perspective transformation matrix calculation step, a two-dimensional image to be obtained when the object is photographed by the imaging means from a position determined by the parameter search point in the space search area set in the search area setting step described above. Is calculated by calculating a perspective transformation matrix for calculating the estimated image feature point. At this time, the perspective transformation matrix is calculated based on the estimated position of the parameter search point with respect to the three-dimensional absolute coordinates of the target object stored in advance in the storage means and the parameter search start point of the imaging means.

In the error calculation step, the correspondence between the two-dimensional image feature points of the target object extracted in the image processing step and the two-dimensional estimated image feature points calculated in the perspective transformation calculation step is determined. In addition to the evaluation error value, the evaluation error value indicating the degree of conformity between the image feature point and the estimated image feature point for which the correspondence has been obtained is calculated based on a predetermined error evaluation function.

In the local optimum parameter calculating step, the evaluation error value of the image feature point for each estimated image feature point is minimized within the search area set for the specified search parameter among the plurality of search parameters. Is obtained as a local optimum parameter, and is newly set as a parameter search point.

In the optimum position and orientation data determining step, the above-described local optimum parameter calculating step is sequentially executed for all of the plurality of search parameters in a predetermined order, and this is repeated a predetermined number of times. When the evaluation error value is smaller than a predetermined required error accuracy, the obtained plurality of parameter search values are determined as a plurality of parameters indicating the position and orientation of the imaging unit.

In this case, in a state in which the object whose known three-dimensional absolute coordinates are known is photographed by the image pickup means within a range of a position and orientation of one view, the execution of each of the above-mentioned steps causes the image pickup means to execute the above steps. For the position and orientation vector representing the position and orientation by a plurality of parameters, searching for each parameter sequentially and independently is performed a predetermined number of times on a multi-scale cyclically, so that an optimal position and orientation is searched. The probability of being able to do so is extremely high, and the position and orientation of the imaging means can be recognized at high speed and with high accuracy while the number of searches is small.

According to the apparatus for recognizing a three-dimensional position and orientation based on vision according to the second aspect of the present invention, since the means for executing the above-described processing steps are provided, the above-described processing steps are executed by the respective means. As a result, the position and orientation of the imaging unit can be recognized at high speed and with high accuracy while the number of searches is small.

[0018]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS A first embodiment in which the present invention is applied to a three-dimensional position recognition apparatus mounted on an autonomous mobile robot running on a track in a factory or the like will be described below with reference to FIGS. I do.

2, an autonomous mobile robot 1 moves between work areas 2 along a trajectory provided on a floor surface in a factory, for example. (See FIG. 1). An arm 3 is provided above the autonomous mobile robot 1
And a hand 6 for performing operations such as gripping a camera 4 as an imaging unit and a work 5 as a target object. In this case, the work area 2 is provided in a recess formed so as to be depressed from the wall surface, and the work 5 having a predetermined shape is placed at a predetermined position on the bottom surface 2a of the work area 2.

Now, the three-dimensional position recognition device 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 image signals of a scene including the work 5 captured by the camera 4 and performs pre-processing calculations such as binarization processing. Based on the obtained image information, a two-dimensional image feature point corresponding to the feature point of the work 5 is extracted as described later.

The quantization space search section 10 includes a search area determination section 11 as search area setting means, a perspective transformation matrix calculation section 12 as perspective transformation matrix calculation means, a perspective transformation section 13 as perspective transformation calculation means, and error calculation. An error evaluation function calculator 14 as means, a local optimum parameter calculator, an optimum value determiner 15 as optimum position and orientation data determiner, and a final output unit 16 for outputting the determined optimum position and orientation data. .

In this case, the search area determination section 11 determines the position and orientation vector s [γ, γ of the camera 4 based on the approximate position information of the autonomous mobile robot 1 with respect to the work 5 in the work area 2.
θ, φ, ρ] (h = 1, 2, 3, 4) as search parameters, the parameter search start point s (1, 1), the parameter search width δ (k, h), and Set the parameter search area (k (= 1, 2, ...,
K) indicates the number of repetitive searches). The parameters γ, θ, φ, and ρ will be described in detail in the description of the operation.

The perspective transformation matrix calculator 12 calculates the image feature points in the image obtained from the image signal when the camera 4 photographs the work 5 at the parameter search point s (k, h) in the set parameter search area. A perspective transformation matrix M for obtaining coordinates is calculated according to the approximate position information of the autonomous mobile robot 1 and the posture parameters of the camera 4. The perspective transformation calculator 13 calculates the perspective transformation matrix M
Is used to calculate a two-dimensional estimated image feature point to be obtained corresponding to the feature point when the camera 4 captures the work 5 at each position of the parameter search point s (k, h).

The error evaluation function calculator 14 is provided with the image processor 7
A correspondence between the extracted image feature points and the estimated image feature points is established, and an error value of a position between them is calculated based on an error evaluation function D for obtaining a sum of squared errors. The optimum value determining unit 15 determines an optimum value for one search parameter as a local optimum parameter, and sequentially calculates the local optimum parameter for each parameter of the position and orientation vector in a predetermined order, and calculates this predetermined value. A plurality of parameters of the position and orientation vector are determined by repeatedly executing the number of times.

The quantization space search unit 10 is connected to a memory 17 as a storage means so that various data can be written and read.
In this case, the data stored in advance in the memory 17 include a feature point matching list, three-dimensional absolute coordinate data of the work 5 and the work area 2, a list of the focal length f of the camera 4, a parameter search point, and a parameter search width. The information includes coordinate data and search order designation information for position and orientation parameters.

The external position information output unit 18 outputs data indicating the external position information of the camera 4 to the quantization space search unit 10.
A signal indicating the approximate position of the autonomous mobile robot 1 is input from a control device (not shown) for controlling the travel of the autonomous mobile robot 1 or a position detection sensor (not shown).

Next, the operation of the present embodiment will be described with reference to FIGS. The three-dimensional position recognition device
As the flow of the operation, the position of the autonomous mobile robot 1 is recognized in accordance with each processing step of the flowchart as 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 work area 2 enters the shooting scene of the camera 4, and the image is processed. 7 comes to input the image signal of the camera 4 (step T1).

In this case, the autonomous mobile robot 1 is moving on a predetermined trajectory in a predetermined direction. When approaching the work area 2, the autonomous mobile robot 1 determines a predetermined position based on a signal from a sensor indicating a stop position, movement control information, and the like. Is stopped in the stop area E, so that when the camera 4 captures the work 5 in the work area 2 with the camera 4 stopped in the stop area E, the work 5 is sufficiently contained within the field of view. Is set to

Next, the 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 in equations (1) and (2).

[0030]

(Equation 1)

In extracting the image feature points Qj, the following calculation is performed based on the approximate position information provided from the external information output unit 19. 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. The approximate three-dimensional absolute coordinates of the center position can be obtained. The relative position of the camera 4 with respect to the center position of the autonomous mobile robot 1 can be obtained based on the control position information of the arm 3, so that the approximate position of the camera 4 can be determined. Based on the obtained approximate position information of the camera 4, the initial search area S (1) can be set as a reference search area in the subsequent search process.

Next, in steps T5 and T6, the search area determination section 11 sets search ranges of various parameters of the camera 4. Here, as the parameters indicating the state of the camera 4, as shown in the correspondence relationship in FIGS. 2 and 4, (a) the posture parameter CP (α,
β, γ), (b) the focal length characteristic f of the camera 4, and (c) the position vector (r, θ, φ) of the camera 4.

In this case, the focal length f of the camera 4 shown in (b) is data determined in advance according to the characteristics of the camera 4 and is stored in advance in the memory 17 as focal length data. The parameters α, β, and γ of the posture parameter CP are a rotation angle γ and a rotation angle around the z-axis when the optical axis of the camera 4 is the z-axis, the horizontal direction is the x-axis, and the vertical direction is the y-axis. The rotation angle α and the rotation angle β about the y-axis. The position parameters of the camera 4 indicate the position of the viewpoint of the camera 4 with respect to the origin of the three-dimensional absolute coordinate system in polar coordinates, where r is a distance and θ and φ are values indicating the inclination angle of r.

Then, of the posture parameters CP of the camera 4, the posture parameters α and β indicating the deviation of the optical axis can be obtained based on the coordinate data on the photographing screen of the center position of the object. That is, as shown in FIG. 5, the image position Ops on the shooting screen (the feature included when not included in the image information) with respect to the origin Op of the coordinate system of the workpiece 5 on the shooting screen. If the coordinates are converted to a coordinate system with the point as the origin, for example, (Na, Nb) in the two-dimensional coordinates (u, v) expressed in pixel units (pixel values), the following equation (3), (4) The estimated values of the posture parameters α and β can be calculated from (4).

In this case, the value of α shown in equation (3) is cos β
Is contained in the two-dimensional coordinate data Na of the image position Ops on the photographing screen when the origin Op of the coordinate system of the workpiece 5 moves away from the work 5 when the work is first rotated around the y axis by the angle β. Is corrected.

[0036]

(Equation 2)

As a result, the camera parameters indicating the position and orientation of the camera 4 include a four-dimensional position and orientation vector s [γ, θ, φ] using the remaining four unknown parameters γ, θ, φ, r as elements. , R] and these parameters may be set as search parameters. In the following description, in order to avoid confusion in the notation of the sign between the parameter γ and the parameter r, the notation in the present specification is indicated by the sign “ρ” instead of the sign “r”. Position and orientation vector s [γ, θ, φ,
r] is replaced with the position and orientation vector s [γ, θ, φ, ρ]. (In FIGS. 2, 4 and 6,
Indicated as “r”. Next, the variable h for designating the search parameter to be searched is set to "1" (step T7), the first search parameter is designated to "γ" (step T8), and the search area S (1) Is a parameter search start point s (1, 1) for a search parameter γ.
Set as This parameter search start point s (1,
1) corresponds to the initial value of the parameter search point s (k, h), where k indicates the number of repetitive searches.

The set parameter search start point s
In (1,1), a parameter search width δ (1,1) for the search parameter γ is set (step T).
10, T11), a search process is performed. At this time, the change of the value of the search parameter within the search range is changed by the parameter search width δ by sequentially specifying the variable I.

Next, in the perspective transformation matrix calculation unit 12,
The three-dimensional position coordinates of the parameter search start point s (1, 1) and the posture parameter α obtained by the above equations (3) and (4)
Then, the perspective transformation matrix M (1, 1) is calculated using the initial postures formed by the values of .beta. And the initial setting values of the search parameter .gamma. (Step T12).

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
Is set as an initial search area S (1). The initial search area S (1) is set based on the stop area E of the autonomous mobile robot 1, and the approximate position A0 (X0, Y0, Z0), which is the scheduled imaging position of the camera 4, is represented by polar coordinates. Centering on the approximate position A0 (ρ0, θ0, φ0), a position search area corresponding to stop position accuracy corresponds to the range of ρ0−Δρ1 ≦ ρ ≦ ρ0 + Δρ2 θ0−Δθ1 ≦ θ ≦ θ0 + Δθ2 φ0−Δφ1 ≦ φ ≦ φ0 + Δφ2 Is set to For the posture search area, the parameters α and β are set using the results obtained by the equations (3) and (4), and the parameter γ is set so as to correspond to the range of −Δγ1 ≦ γ ≦ Δγ2. Is done.

Then, the posture parameter CP of the camera 4
Camera 4 calculated from (α, β, γ) and focal length characteristic f
FIG. 6 based on the viewpoint position and the distance d to the projection plane.
(“Ρ” is represented as “r”) based on the relationship shown in FIG. Here, the perspective transformation matrix M
Can be expressed as in the following equation (5).

[0042]

(Equation 3)

Of these, the product of the perspective transformation matrices M1 and M2 for calculating the perspective transformation between the image display surface and the work 5 and the coordinate transformation matrix M3 are given by the following equations (6) and (7), respectively. ).

[0044]

(Equation 4)

The conversion matrix T for calculating the rotation according to the posture parameter CP (α, β, γ) corresponding to the optical axis shift of the camera 4 is shown in FIG. 4 (“ρ” is “r ) And the relationship shown in FIG.
Equations (8), (9), and (10) can be used as a rotation transformation matrix that rotates in the order of α and β.

[0046]

(Equation 5)

In the above case, in the process of calculating the perspective transformation matrix M, the amount of calculation can be reduced as follows. That is, the equation (8) is substituted into the transformation matrix T in the equation (5). Here, when the element represented by (T1, M1, M2) is set to MT, the equation (5A) is obtained. At this time, the value of the element MT can be transformed into a simple expression such as Expression (5B) when calculated from Expressions (6) and (8). Therefore, when the equation (5B) is substituted into the equation (5A), the perspective transformation matrix M becomes as shown in the equation (5C). In the actual calculation, the calculation amount is reduced by performing the calculation based on the equation (5C). Thus, the calculation speed can be improved.

[0048]

(Equation 6)

Next, in step T13, based on the perspective transformation matrix M (1, 1) calculated in this way, the work area 2 and the work area 2 are placed by the perspective transformation calculation unit 13. For each feature point Pi of the set P of n feature points Pi of the workpiece 5, the perspective coordinate transformation of the three-dimensional absolute coordinate data is performed to obtain the coordinates of the two-dimensional image display unit, and the equation (1)
1) A set Qe of estimated image feature points Qei for each feature point Pi as shown in equation (12) is obtained. When the homogeneous coordinates are used, the conversion from the set P of the feature points Pi to the set Qe of the estimated image feature points Qei is performed by using the equation (5) or the simplified equation (5C). It can be expressed as follows.

[0050]

(Equation 7)

Next, a set Q of image feature points Qj of the work 5 photographed by the camera 4 extracted by the image processing unit 7
A feature point correspondence list in which a correspondence between each image feature point Qj in (formula (1)) and a three-dimensional feature point Pi (see formula (4)) of the work 5 stored in advance is selected. (Step T14). In this case, when each image feature point Qj is associated with the position of the feature point Pi of the work 5 shown in FIG. 7A and the various appearance patterns shown in FIGS. 8A to 8F, As shown in FIG. 7B, the correspondence between the image feature point Qj on the shooting screen and the corresponding feature point Pi is obtained as the image information. Then, a pattern of how the work 5 looks is determined based on the result of the above association.

For example, when the feature point correspondence list corresponding to the appearance pattern (b) shown in FIG. 8 is obtained, the feature point correspondence list shown in FIG. a) to (c), the image feature points Qj
Various states are supposed, such as a case where the position appears small with respect to the estimated feature point Qei (a), a case where the position appears biased (b), and a case where the position appears inclined (c). In other words, in the following search process, a shift in the shape of the work 5 in the same appearance pattern state as shown in FIGS. 9A to 9C is searched.

Subsequently, the error evaluation function calculator 14 calculates
An error evaluation function D (1,1) between the image feature point Qj for which the correspondence has been obtained and the set Qe (= P · M) of the estimated image feature points Qei is calculated (step T15). In this case, the error evaluation function D (k, h) calculates a value corresponding to the sum of square errors of the distance between the image feature point Qj and the estimated image feature point Qei.

Subsequently, through steps T16 and T17,
The search process proceeds by repeatedly executing the above-described steps T11 to T15 for each parameter search point quantized at intervals of the parameter search width δ (1, 1) within the parameter search range in which the search parameter γ is set.

Then, the value D of the obtained error evaluation function is obtained.
Local optimal value Dmin (1, 1) which is the minimum value of (1, 1)
Search parameter γ at the parameter search point corresponding to
, The parameter search point in the parameter search range at this time is set to s (1, 2) (step T18). In this calculation process,
Other than the search parameter γ, the parameters θ and
The search is executed as an independent search in which the values of φ and ρ are fixed with the parameter initial setting values unchanged.

Subsequently, the next search parameter is set to θ (h = 2) in steps T19 and T20 using the parameter search point s (1,2) of the position and orientation just obtained as a new parameter search start point. , The above-described steps T8 to T1
7, the local optimum value Dmin (1,2) for the search parameter θ is calculated.

In this case, the value of the search parameter θ is such that the parameter search width is δ (1,1) within the parameter search range of θ.
The calculation is performed by performing the above-described steps T8 to T17 for the parameter search point quantized as 2). Then, in step T18, the search parameter θ
When the local optimal value Dmin (1,2) for is obtained,
The corresponding parameter search point is s (1,3).

Hereinafter, steps T8 to T18 described above are repeatedly executed for search parameters φ and ρ. And the local optimum value Dmi of the search parameter φ
After the parameter search point s (1,4) for n (1,4) is obtained, the local optimum value Dmin of the search parameter ρ
When (2,1) is obtained, the parameter search point s (2,1) at this time is determined by all search parameters γ, θ,
This means that one local optimum Dmin has been obtained for φ and ρ.

Therefore, when the process goes to step T19, the process goes to step T21. The process goes to step T22 until the value of the number of repetition searches k reaches the set value K. ~ T2
Step 2 is repeated.

In this case, the search area S (k) is set from the search area S (1) to a new search area S (2). That is, each search parameter γ,
By executing one search process for θ, φ, and ρ, the search range is narrowed in the second time, and a narrow parameter search width δ (2, h) is set within the range. It becomes.

Further, all search parameters γ, θ,
One local optimum Dmin (2,1) for φ and ρ
The parameter search point s (2,1) obtained as s (2,1) is used as the parameter search start point s (2,1) in the second search process.
And a similar calculation process is executed. Therefore, each search parameter γ, θ,
The search process is performed sequentially and independently for φ and ρ,
A calculation is performed such that this is cyclically repeated a predetermined number of times K.

By the way, the concept of such a search process is as follows:
This will be briefly described with reference to FIG. In the present embodiment, since the position and orientation vector s [γ, θ, φ, ρ] has four parameters, the search area space becomes four-dimensional, which is unreasonable in the drawing. Here, for convenience, the position and orientation vector s is set to three search parameters θ,
An example in which the position and orientation vector s [θ, φ, ρ] having φ and ρ is set will be described.

That is, in FIG. 10, each search parameter θ, φ, ρ of the position and orientation vector s [θ, φ, ρ]
Are the search axes θ axis, φ axis, and ρ axis, respectively, and are shown as coordinate axes orthogonal in a three-dimensional space. The origin of this coordinate system is defined as a parameter search start point s (1, 1).

First, the search parameters φ and ρ are fixed at the values of the initial search points, and the search process is independently executed only for the search parameter θ to obtain the error evaluation value D at that time.
The parameter search point s (1,2) of the position and orientation vector when min becomes minimum is calculated. That is, an error evaluation value Dmin is obtained for a parameter search point set at a predetermined parameter search width δ (1, 1) along the θ-axis direction (indicated by an arrow (a) in the figure), and the search parameter θ at that time is obtained. A new parameter search point s (1,2) using the value of is obtained, and the first independent search for the search parameter θ is completed.

Next, the parameter search point s obtained just now is
A similar search process is executed for search parameter φ using (1, 2) as a parameter search start point. This time, the search process is executed only for the search parameter φ in accordance with the parameter search width δ (1, 2) while the other search parameters θ and ρ are fixed. Therefore, the search is performed in the direction along the φ axis with the parameter search point s (1, 2) as the search start point (indicated by an arrow (b) in the figure), and the local optimum obtained within a predetermined search range is obtained. The value Dmin is obtained, and a new parameter search point s (1, 3) is obtained using the value of the search parameter φ at that time.

Next, when the same is performed for the search parameter ρ, the parameter search point s (2, 1) obtained at that time becomes the parameter search start point in the second search process. By repeatedly performing the search process K times, the parameter search points s (k, h) are sequentially searched for optimal values as shown by arrows (c) to (f) in the figure, and finally, The obtained parameter search point s (K + 1, 1) becomes the optimum position and orientation data that gives the optimum position and orientation vector s [θ, φ, ρ].

When the search process for each search parameter is executed once as described above, in the next search process, the search range is narrowed cyclically as a multiple scale. Since the process can be executed, the detection accuracy can be improved and the target detection accuracy can be increased as the value of the number of searches K is set larger.

When the above-described steps T7 to T22 are repeatedly executed by the number of repetitions K (for example, K = 5) as described above, the process proceeds from step T21 to step T23.
Will be transferred to. Here, the optimal value determination unit 15
It is determined whether or not the value of the local optimum value Dmin for the finally obtained parameter search point s (6, 1) is smaller than the target error accuracy ε. The position / posture vector s [γ, θ, φ, ρ] corresponding to the parameter search point s (6, 1) at that time and the values α and β calculated in step T5 as posture parameters are output to the final output unit 16 as optimal position / posture data. (Step T24).

If the value of the local optimum value Dmin is greater than or equal to the error accuracy ε and does not satisfy the condition of step T23 as a result of a series of search processes, the process proceeds from step T23 to step T25, where the required error accuracy After outputting a message that there is no search point s that satisfies ε, the parameter search point s for which a value closest to the required error precision ε was obtained from the local optimum values Dmin calculated up to that time.
The value of the position and orientation vector s [γ, θ, φ, ρ] of (k, h) and the value of the orientation parameter (α, β) of the camera 4 are set as new parameter search start points, and a new search area S Is set, the process returns to step T6, and the search process is repeatedly executed in the same manner as described above.

By executing the search process as described above, it is necessary to set a parameter search width δ smaller than the final target detection accuracy ε when the value of the number of repetitions k is the last K, for example. For the number of repetitions k (k <K) before that, a parameter search width δ larger than the target detection accuracy can be set in accordance with the detection accuracy at that time. Makes it possible to quickly narrow down the search area by roughly calculating the position and orientation by setting the parameter search width δ to a wide width, and to set the parameter search width δ to the target detection accuracy in the final stage. . In other words, since the search process can be executed with the parameter search width being a multiple scale, highly accurate calculation processing can be performed efficiently while performing a rough calculation in which the calculation time of the search process is reduced.

According to the experimental results of the inventors, by executing the search process on multiple scales as described above,
Even when the value of the number of repetitive searches K is set to “5”, the detection error accuracy is set to the distance accuracy ± 1.0 mm and the posture accuracy ± 0.5.
It can be seen that it can sufficiently cope with the determination of the position and orientation of °, and it has been confirmed that the position and orientation can be robustly recognized with high speed and high accuracy.

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, image feature points Qj are extracted from the image information, and the quantized spatial points are extracted. A plurality of spatial search areas Sc are set, and the position and orientation vector s of the camera 4 is set.
[Γ, θ, φ, ρ] is set and the work 5 is moved to its search point s.
The estimated image feature point Qei of the work 5 to be obtained when the image is taken at (k, h) is transformed into a perspective transformation matrix M (k, h).
Is obtained by calculating the corresponding relationship, calculating the local optimum value Dmin for the error evaluation function D, and executing the sequential independent search for each search parameter by the number of repetitions K. Position and orientation vector s corresponding to search point s when error accuracy ε is satisfied
Since [γ, θ, φ, ρ] and the posture parameter (α, β) are output as optimal position / posture data, 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, since it is not a method of calculating parameters from components of a perspective transformation matrix that is not explicitly solved at present, there is no need for least-squares estimation of a perspective transformation matrix that can be explicitly solved but requires a large amount of calculation. Therefore, calculation processing can be executed quickly. Third, the present invention can be applied even when the posture parameter of the camera 4 is unknown.

Fourth, the position and orientation vector s of the camera 4
In the calculation for obtaining [γ, θ, φ, ρ], the sequential independent search for each search parameter is cyclically and repeatedly performed on multiple scales, so that the optimum position and orientation can be obtained with high accuracy and robustness with a small number of searches. Will be able to recognize.

Fifth, since the actual calculation is performed by simplifying the expression (5) of the perspective transformation matrix M as shown in the expression (5C), a large calculation is performed as compared with the case where the calculation is performed without changing the expression (5). Since the amount can be reduced, the calculation speed can be improved.

In the above embodiment, the search order of the search parameters γ, θ, φ, and ρ is specified in advance so as to be in this order, and the sequential independent search process is executed. The present invention is not limited to this, and a function of flexibly changing the search order according to the approximate position and orientation of the camera 4 with respect to the workpiece 5 may be provided. That is, for example, if the parameter ρ with respect to the distance r is at a position that is a large element that determines the search range, the search order is determined such that the search parameter ρ is designated as the first search order. By setting the judgment criteria, it is possible to execute a more efficient search process.

FIG. 11 shows a second embodiment of the present invention. Hereinafter, portions different from the first embodiment will be described. In other words, this embodiment improves the robustness against the measurement error caused by the calculation of the origin on the photographing screen by executing the steps shown in FIG. 11 in addition to the contents of the program executed in the first embodiment. And to improve the recognition accuracy.

In the first embodiment, the values of α and β, which are the posture parameters of the camera, are set based on the image information at the initial stage of the search process, and these are handled as known data. This is because the six parameters α, β, γ, θ, φ, and ρ to be searched are narrowed down to four parameters in the actual calculation stage to reduce the amount of calculation and increase the speed. I have.

However, in this case, if an error is included in the two-dimensional coordinates of the origin of the object coordinate system extracted from the image data of the photographing screen, the error also occurs in the values of the posture parameters α and β obtained as initial settings. Has occurred. Therefore, when performing the cyclic independent search for the other four search parameters based on this initial setting, the posture parameters α, β
In some cases, it becomes impossible to achieve high-accuracy position and orientation recognition that exceeds the search accuracy of the search.

Therefore, in this embodiment, the above-mentioned disadvantages are eliminated, and the increase in the amount of calculation is minimized as compared with the case where the cyclic independent search process is performed using all six position and orientation parameters as search parameters. In order to prevent the calculation speed from decreasing.

First, during the search process in the first embodiment, when step T6 is completed,
Step T25 shown in FIG. Here, it is determined whether or not the value of the number k of repeated searches is equal to or more than the number m of times of starting the search for the preset camera posture parameters α and β. Here, when the determination is "NO", the process directly proceeds to step T7 to execute the same search process as in the first embodiment, and thereafter, while repeatedly executing the search process, the process proceeds to step T25. If "YES" is determined, the process branches to step T26.

That is, in step T26, the search width of the posture parameters α and β as the search parameters is set,
When the process proceeds to step T27, a search for the posture parameter β is performed, and subsequently, a search for the posture parameter α is performed in step T28, and thereafter, the process returns to step T7 and proceeds to the subsequent steps in the same manner as in the first embodiment. Perform the search process. In this case, as the number of searches increases, the set value of the search width in step T26 becomes smaller,
The search accuracy is increased.

Therefore, according to the second embodiment, in addition to the effects of the first embodiment, the camera posture parameters α and β obtained based on the origin Op on the photographing screen at the initial stage are obtained. Since the search process is executed by adding the values to the search parameters in the latter half of the search process, the errors in the initial values of the camera posture parameters α and β due to the detection errors included in the captured image origin coordinate Op data in the initial stage. This can be corrected, and the actual detection accuracy in the latter half of the search process can be further improved. Further, compared with the case where the search is executed for all parameters from the first half of the search process, the detection accuracy can be improved without lowering the calculation speed.

Note that, in the above case, the inventors set the value of the number of repetitive searches K to 12 times, which is more than the number of times 5 in the first embodiment, and performed a highly accurate search. In this case, it has been found that when the number m of search start times of the camera posture parameters α and β is set to seven, good results can be obtained with high accuracy.

In this case, setting the value of the number of times K of repetitive search to a large value generally increases the detection accuracy, but on the other hand, restricts the search time to be long. The number of repetitive searches is set in consideration of the detection accuracy and the search time. Therefore, when it is required to improve detection accuracy in a short time, the search process in the present embodiment is an effective method.

In the above-described second embodiment, a case has been described in which the number of repetitive searches K is set to 12 and the number of searches m for starting the camera attitude parameters α and β is set to 7 in an experiment. However, the present invention is not limited to this. , The number of repetitive searches K and the number of searches start m (K>m> 2) can be set to appropriate values in consideration of detection accuracy and search time.

[0087]

As described above, according to the present invention,
The following effects can be obtained. That is, in the method for recognizing a three-dimensional position and orientation based on vision according to claim 1, a two-dimensional image feature point is extracted in an image processing step with respect to an image signal input of a target object photographed by an imaging unit. In the spatial search area setting step, a spatial search area having a quantized spatial point having a density corresponding to a required calculation accuracy is set according to a rough estimated distance between the imaging means and the target object based on external information or the like, and the search is performed. In the parameter setting step, an initial value of a parameter search value, a search point, and a search range are set.

Then, in the perspective transformation matrix calculation step, a perspective transformation matrix for calculating a two-dimensional estimated image feature point is calculated, and in the perspective transformation calculation step, a two-dimensional estimated image feature point of the target object is calculated. In the error calculation step, the correspondence between the image feature point and the estimated image feature point is obtained, and the evaluation error value between them is calculated based on the error evaluation function.In the local optimum parameter calculation step and the optimum position / posture data determination step, The local independent parameters are sequentially and independently searched for all of the plurality of parameters in a predetermined order, and the search is repeated a predetermined number of times to determine the plurality of parameters of the position and orientation vector.

Thus, for the position and orientation vector representing the position and orientation of the image pickup means by a plurality of parameters, it is possible to repeatedly and independently search for each of the parameters a predetermined number of times on a multiple scale. This provides an excellent effect that the position and orientation of the imaging means can be recognized robustly at high speed and with high accuracy with a small number of searches.

According to the apparatus for recognizing a three-dimensional position and orientation based on vision according to the second aspect of the present invention, since the means for executing each of the above-mentioned processing steps is provided, it is possible to achieve high-speed and high-accuracy robustness despite a small number of searches. This has an excellent effect that the position and orientation of the image pickup means can be recognized.

[Brief description of the drawings]

FIG. 1 is a schematic block diagram showing a first 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 for schematically explaining a process flow;

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

FIG. 5 is an explanatory diagram showing a relationship between a work on a shooting screen and a posture parameter of a camera.

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

FIG. 7A is an operation explanatory view showing a correspondence relationship between (a) the definition of the feature points of the work area and the work and (b) the image feature points by the image signal.

FIG. 8: Various appearance patterns of a workpiece

FIG. 9 is an explanatory diagram of states of different appearances in one appearance pattern.

FIG. 10 is a conceptual explanatory diagram of a search process.

FIG. 11 is an explanatory diagram showing additional steps of a flowchart for schematically explaining a process flow according to the second embodiment of the present invention;

[Explanation of symbols]

1 is an autonomous mobile robot, 4 is a camera (imaging means), 5 is a work (target object), 7 is an image processing unit (image processing means), 8 is a pre-processing calculation unit, 9 is a feature point extraction unit, 10 is a quantum A space search unit, 11 is a search area determination unit (search area setting means), 12 is a perspective transformation matrix calculation unit (perspective transformation matrix calculation means), 13 is a perspective transformation unit (perspective transformation calculation means), 14
Is an error evaluation function calculation unit (error calculation means), 15 is an optimum value determination unit (local optimum parameter calculation means, optimum position and orientation data determination means), 16 is a final output unit, 17 is a memory (storage means), and 18 is an external unit. It is a position information output unit.

Continuation of front page (72) Inventor Tetsuya Konan 1-1-1 Showa-cho, Kariya-shi, Aichi Japan Inside Denso Co., Ltd. (58) Field surveyed (Int. Cl. ⁶ , DB name) G01B 11/00-11/30 B25J 19/04 G05B 19/19 G05D 3/12

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 an image signal obtained by photographing the target object of which the three-dimensional absolute coordinate indicating the shape is known by an imaging unit. Processing step, a plurality of parameters indicating the position and orientation of the imaging unit with respect to the target object are set as search parameters, a parameter search start point is set according to an estimated distance given by external information, and a parameter search is performed according to the estimated distance. A search area setting step of setting a spatial search area having a parameter search point quantized by a width; and the object by the imaging means from a position determined by the parameter search point of the space search area set in the search area setting step. A perspective transformation matrix for calculating a two-dimensional estimated image feature point to be obtained when shooting the object,
Calculating a perspective transformation matrix based on an estimated position of the parameter search point with respect to the three-dimensional absolute coordinates of the target object stored in advance in the storage means and a parameter search start point of the imaging means; A perspective transformation calculation step of calculating the two-dimensional estimated image feature point of the target object at the parameter search point using the perspective transformation matrix obtained in the step, and the object extracted in the image processing step The correspondence between the two-dimensional image feature point of the object and the two-dimensional estimated image feature point calculated in the perspective transformation calculation step is obtained, and the image feature point and the estimated image feature point from which the correspondence is obtained. An error calculation step of calculating an evaluation error value indicating a degree of conformity between the plurality of search parameters based on a predetermined error evaluation function. In the search area for the specified search parameter, the parameter search point that minimizes the evaluation error value of the image feature point for each of the estimated image feature points is obtained as a local optimum parameter, and A local optimal parameter calculating step to be set as a parameter search point, and sequentially executing the local optimal parameter calculating step for all of the plurality of parameters in a predetermined order, and repeatedly executing the predetermined number of times. And determining an obtained plurality of parameter search values as a plurality of parameters of the position and orientation vector when the evaluation error value is smaller than a predetermined required error accuracy. A three-dimensional position and orientation recognition method based on vision. 2. An image pickup means for outputting an image signal; a storage means for storing three-dimensional absolute coordinate data indicating a shape of a target object and data for specifying a search order; Image processing means for extracting a two-dimensional image feature point indicating a feature of the shape of the target object based on the image signal at the time, a plurality of parameters indicating the position and orientation of the imaging means with respect to the target object as search parameters, Search area setting means for setting a parameter search start point according to an estimated distance given by external information and setting a spatial search area having a parameter search point quantized with a parameter search width according to the estimated distance; The object is set by the imaging unit from a position determined by a parameter search point of the space search area set by the area setting unit. A perspective transformation matrix for calculating a two-dimensional estimated image feature point to be obtained when capturing the image, the estimated position of the quantized space point with respect to the three-dimensional absolute coordinates of the target object stored in advance in the storage means. A perspective transformation matrix calculating means for calculating based on an initial search parameter of the imaging means; andthe two-dimensional object of the target object at the parameter search point using the perspective transformation matrix obtained by the perspective transformation matrix calculating means. A perspective transformation calculating unit that calculates an estimated image feature point; a two-dimensional image feature point of the target object extracted by the image processing unit; and a two-dimensional estimated image feature point calculated by the perspective transformation calculation unit. And the evaluation error value indicating the degree of conformity between the image feature point from which the correspondence is obtained and the estimated image feature point is determined by a predetermined error. An error calculating means for calculating based on a valence function, wherein an evaluation error value of an image feature point with respect to each of the estimated image feature points is minimized in the search area for a specified search parameter of the plurality of search parameters. A local optimum parameter calculating means for obtaining the parameter search point as a local optimum parameter and setting the parameter search point as a new parameter search point; and performing the calculation processing by the local optimum parameter calculating means in a predetermined order. Are sequentially executed for all of the parameters, and this is repeatedly executed a predetermined number of times. When the evaluation error value at that time is smaller than a predetermined required error accuracy, the obtained plurality of parameter search values are converted to the position and orientation. And an optimal position and orientation data determining means for determining as a plurality of parameters of the vector. A three-dimensional position and posture recognition device based on the visual sense of the sign.