JP2903964B2 - 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 relates to a method of recognizing a three-dimensional position and orientation based on vision, which recognizes the three-dimensional position and orientation of the monocular camera based on image information taken by the monocular camera. The present invention relates to a three-dimensional position and orientation recognition device based on vision.

[0002]

2. Description of the Related Art In recent years, for example, an autonomous mobile robot or the like moves so as to approach a target object whose shape and absolute coordinate position are known, and moves the target object by a manipulator provided at the tip of a robot hand. In some cases, active operations such as grasping are performed. In such an autonomous mobile robot, it is necessary to mount imaging means such as a CCD 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 CCD camera with respect to the target device is obtained by a method such as calculating the following, and the movement of the robot hand is controlled based on this.

[0004]

However, in the above-described conventional configuration, the imaging of the marker by the CCD camera is performed such that the marker display surface is positioned in a plane perpendicular to the optical axis of the CCD camera. This is performed by moving the marker, and it is necessary to make the photographing screen of the marker have a one-to-one correspondence with the two-dimensional relative position of the marker display surface. is there.

[0005] That is, for example, when a camera is attached to a robot hand for gripping an object, the robot hand is moved to an appropriate position in order to vertically photograph a marker arranged near the object. However, at this time, when the manipulator at the tip of the robot hand is in a state of gripping an article, the movement range is restricted.

Further, when the marker is moved into the field of view by the CCD camera, if the area where the marker is installed is small, the robot hands come into contact with the working environment and cause damage to each other. It is necessary to set the installation area in a work environment in which the robot hand can move sufficiently, and there is a problem that the work environment is greatly restricted.

However, conventionally, in order to solve such a problem, by photographing from a position obliquely away from the marker, the position and orientation of the camera can be robustly and quickly and accurately (robust is generally called “robustness”). It is used as a qualitative term to indicate that the results of processing and recognition are not significantly affected by noise or changes in environmental conditions.)

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 make the image pickup means perpendicular to the plane marker when the three-dimensional position is recognized by photographing the plane marker by the image pickup means. Without facing each other, based on image information taken from an oblique direction, it is possible to perform high-speed, high-accuracy and robust position / posture recognition simply by performing simple binary image processing. It is an object of the present invention to provide a three-dimensional position and orientation recognition method and apparatus based on the three-dimensional position and orientation.

[0009]

According to the present invention, there is provided a method for recognizing a three-dimensional position and orientation based on vision, which comprises two line segments having predetermined dimensions and orthogonal to each other, and lines respectively parallel to the two line segments. A marker which is displayed on a plane by a change in grayscale information or color information with respect to a background portion of the figure enclosed including the minutes, and selects an image area of the marker based on image data from the imaging means. An image processing step of detecting a change point of the value of the image data in the selected image area and extracting a contour point position, and based on the data of the contour point position extracted in the image processing step. A straight line fitting processing step of calculating parameters of straight lines each including a line segment orthogonal to the marker and a line segment parallel to the marker on a shooting screen by a least squares estimation method; A unit direction vector calculation processing step of calculating a unit direction vector of the marker based on vanishing point data which is an intersection of the straight lines calculated in the straight line fitting processing step corresponding to the parallel line segments displayed in A marker coordinate system is set with the intersection of the line segments orthogonal to the marker as the origin, and the direction of each line segment and the normal direction perpendicular to the marker plane and passing through the origin as the reference direction, based on the unit direction vector. A position and orientation calculation processing step of calculating a parameter indicating the position and orientation of the marker; and a parameter calculation result indicating the position and orientation of the imaging means in the position and orientation calculation processing step, as viewed from a reference coordinate system of the imaging means. And a coordinate transformation matrix calculation processing step of calculating a coordinate transformation matrix representing the position and orientation of the marker. .

[0010] The marker is composed of at least two rectangular or square figures which are displayed by a change in shading information or color information with respect to the background part, and these figures are mutually represented by one vertex. It is good to arrange so that adjacent sides including the vertices that are in contact with each other and make an angle may form a right angle.

In the line fitting processing step, a plurality of straight lines corresponding to one line segment of the marker are detected based on the data of the contour point position extracted in the image processing step, It is preferable to obtain one straight line by averaging those straight lines and determine the straight line parameters.

Further, in the unit direction vector calculation processing step, three or more candidate values of the vanishing point are calculated using the law of invariance of cross ratio, and the candidate values are calculated based on the two-dimensional coordinate data of the candidate values. Preferably, a unit direction vector is calculated.

In the unit direction vector calculation processing step, for a straight line obtained corresponding to each of three or more parallel line segments of the marker, a unit direction vector with respect to an intersection point between the straight lines is quadratic with respect to a moment matrix. It is preferable to calculate the unit direction vector by solving as a formal minimization problem.

Further, the three-dimensional position and orientation recognition apparatus based on vision according to the present invention has at least two
A state in which two sets of parallel lines composed of two parallel line segments are arranged so as to be orthogonal to each other is a marker displayed on a plane by a change in density information or color information with respect to a background portion, and an image of a captured image. Imaging means for outputting data, storage means for storing marker data indicating the shape and dimensions of the marker, and selecting an image area of the marker based on image data from the imaging means, and selecting the selected image Image processing means for detecting a change point of the value of the image data in the area and extracting a contour point position, and a group of parallel lines orthogonal to the marker based on the data on the contour point position extracted by the image processing means Line fitting processing means for obtaining the parameters of the straight line including the respective line segments by the least squares estimation method, and the photographed image of the straight line including the respective line segments of the two sets of parallel lines. A unit direction vector calculation processing means for calculating a unit direction vector of the marker based on data of a vanishing point which is an intersection point above, and a method passing through two orthogonal straight lines obtained by the straight line fitting processing means and the intersection point thereof A position and orientation calculation processing means for setting a marker coordinate system having a line as an axis of a coordinate system and calculating the position and orientation of the marker based on the marker coordinate system and the unit direction vector; And a coordinate conversion matrix calculation unit that calculates a coordinate conversion matrix representing the position and orientation of the marker viewed from a reference coordinate system of the imaging unit based on a parameter calculation result indicating the position and orientation of the imaging unit. It has features.

Further, the marker is constituted by at least two rectangular or square figures which are displayed by a change in shading information or color information with respect to a background portion, and these figures are mutually formed by one vertex. It is preferable to arrange so that adjacent sides that touch and include the vertex form a right angle.

The straight line fitting processing means detects a plurality of straight lines corresponding to one line segment of the marker based on the data of the contour point position extracted by the image processing means. It is preferable that one straight line is obtained by averaging the straight lines, and the straight line parameters are determined.

Further, the unit direction vector calculation processing means calculates three or more candidate values of the vanishing point using the law of invariance of cross ratio, and calculates the unit value based on the two-dimensional coordinate data of the candidate values. It can be configured to calculate a direction vector.

Further, the unit direction vector calculation processing means may calculate a unit direction vector for a straight line corresponding to each of three or more parallel line segments of the marker with respect to their intersection by a quadratic form for a moment matrix. The unit direction vector may be calculated by solving as a minimization problem.

[0019]

According to the method for recognizing a three-dimensional position and orientation based on vision according to the first aspect, when the marker is photographed by the imaging means positioned obliquely in the three-dimensional space with respect to the marker, By executing each processing step, it becomes possible to recognize the three-dimensional position and orientation of the imaging means with respect to the marker.

That is, first, in the image processing step, the image data of the marker photographed by the image pickup means is obtained.
The image area of the marker is selected, and a change point of the value of the image data in the selected image area is detected to extract the contour point position. At this time, since each line segment of the marker is displayed as a change in shading information or color information, the detection of the line segment can be obtained from the outline of a figure having no line width, and image processing is simplified. At the same time, it is possible to extract the contour point position with high accuracy by minimizing the detection error.

Next, in the straight line fitting processing step, based on the data of the contour point position extracted in the above-described image processing step, the parameters of the straight line including the line segment orthogonal to the marker and the line segment parallel to the marker are calculated. It can be obtained by the least squares estimation method.

In the unit direction vector calculation processing step, since the marker is photographed from an oblique direction by the imaging means, the marker is calculated in the straight line fitting processing step corresponding to the parallel line segment displayed on the marker. Since the straight lines intersect at infinity, the two-dimensional coordinates of the vanishing point, which is the intersection, are calculated, and the unit direction vector is calculated from this.

Subsequently, in a position / orientation calculation processing step, a marker coordinate system with the intersection of the line segments orthogonal to the marker as the origin and the direction of each line segment and the normal direction perpendicular to the marker plane and passing through the origin as the reference direction. Is set, and a parameter indicating the position and orientation of the marker is calculated based on the unit direction vector obtained in the unit direction vector calculation processing step.

In the coordinate transformation matrix calculation processing step, a coordinate transformation matrix representing the position and orientation of the marker viewed from the reference coordinate system of the imaging means is calculated based on the parameter calculation result indicating the position and orientation of the imaging means. Become.

In this case, the three-dimensional position and orientation can be recognized by photographing the marker obliquely by the image pickup means, and simple calculation processing for the figure of the marker only needs to be executed. And a robust processing result can be obtained.

According to the method for recognizing a three-dimensional position and orientation based on vision according to the second aspect of the present invention, the markers are displayed as two or more simple rectangular or square figures. In addition to simplifying the posture recognition calculation process, for example, by arranging the figures so as to be arranged in the horizontal direction, even when the inclination angle of the marker with respect to the shooting screen of the imaging unit becomes large, It is possible to reduce the distance change in the depth direction of the line segment, reduce errors due to defocus, and perform accurate detection.

According to a third aspect of the present invention, in the line fitting processing step, one of the markers is determined based on the data of the contour point position extracted in the image processing step. By detecting a plurality of straight lines corresponding to the line segments and averaging the straight lines to obtain a single straight line and determine the straight-line parameters, for example, a marker photographed by the imaging unit is Even when a contour cannot be detected as an orthogonal line segment due to defocus, it can be detected as a more accurate straight line by averaging a plurality of straight lines obtained from the contour point positions.

According to the third aspect of the present invention, in the unit direction vector calculation processing step, three or more vanishing point candidate values are calculated using the bi-invariance rule. Then, since the unit direction vector is calculated based on the two-dimensional coordinate data of those candidate values, for example, a vanishing point obtained from the intersection point of the straight line detected corresponding to the parallel line segment has an error. Is more accurate, it is possible to detect a more accurate unit direction vector.

According to the method for recognizing a three-dimensional position and orientation based on vision, in the unit direction vector calculation processing step, straight lines obtained respectively corresponding to three or more parallel line segments of the marker are provided. Is calculated by solving the unit direction vector for those intersections as a quadratic form minimization problem for the moment matrix. Therefore, even if an error occurs in the straight line detected in the same manner as described above, a more accurate unit direction can be obtained. Vectors can be detected.

According to the apparatus for recognizing a three-dimensional position and orientation based on vision according to the sixth aspect, when the marker is photographed by the imaging means positioned obliquely in the three-dimensional space with respect to the marker, As a result, it becomes possible to recognize the three-dimensional position and orientation of the imaging means with respect to the marker.

That is, with respect to the marker image data obtained when the marker is photographed in the oblique direction of the three-dimensional space by the image pickup means, the image processing means selects the marker image area and the selected image area. A change point of the value of the image data in the image area is detected, and a contour point position is extracted. At this time, since each line segment of the marker is displayed as a change in shading information or color information, the line segment can be detected from the outline of a figure having no line width.
Image processing is simplified, and a detection error can be reduced as much as possible to accurately extract the contour point position.

Next, based on the extracted contour point position data, the straight line fitting processing means obtains the parameters of the straight line including the line segment orthogonal to the marker and the line segment parallel thereto by the least squares estimation method. Next, since the marker is photographed from an oblique direction by the imaging means,
Utilizing that the straight line calculated by the straight line fitting processing means corresponding to the parallel line segment displayed on the marker intersects at the infinity position, the unit direction vector calculation processing means makes use of the intersection point of the vanishing point. The two-dimensional coordinates are calculated, and the unit direction vector is calculated from this.

Subsequently, the position and orientation calculation processing means sets a marker coordinate system with the intersection of the line segments orthogonal to the marker as the origin, the direction of each line segment and the normal direction perpendicular to the marker plane and passing through the origin as the reference direction. Is set, and a parameter indicating the position and orientation of the marker is calculated based on the unit direction vector obtained by the unit direction vector calculation means. Then, based on the parameter calculation result indicating the position and orientation of the imaging unit, the coordinate conversion matrix calculation processing unit calculates a coordinate conversion matrix representing the position and orientation of the marker viewed from the reference coordinate system of the imaging unit.

In this case, the three-dimensional position and orientation can be recognized by photographing the marker obliquely by the imaging means, and simple calculation processing for the figure of the marker only needs to be performed. And a robust processing result can be obtained.

According to the apparatus for recognizing three-dimensional position and orientation based on vision according to the seventh aspect, the marker is displayed as two or more simple figures having a rectangular or square shape. In addition to simplifying the posture recognition calculation process, for example, by arranging the figures so as to be arranged in the horizontal direction, even when the inclination angle of the marker with respect to the shooting screen of the imaging unit becomes large, It is possible to reduce the distance change in the depth direction of the line segment, reduce errors due to defocus, and perform accurate detection.

According to the apparatus for recognizing a three-dimensional position and orientation based on vision according to claim 8, one of the markers is determined by the straight line fitting processing means based on the data of the contour point position extracted in the image processing step. A plurality of straight lines are detected corresponding to the line segments, and one straight line is obtained by averaging the straight lines to determine the straight line parameters. Even when the contour cannot be detected as an orthogonal line segment due to the displacement, it can be detected as a more accurate straight line by averaging a plurality of straight lines obtained from the contour point positions.

According to the apparatus for recognizing three-dimensional position and orientation based on vision according to the ninth aspect, the unit direction vector calculation processing means calculates three or more vanishing point candidate values using the law of invariance of cross ratio. Then, since the unit direction vector is calculated based on the two-dimensional coordinate data of those candidate values, for example,
Even when there is an error in the straight line detected corresponding to the parallel line segment and there are a plurality of vanishing points obtained from the intersections, a more accurate unit direction vector can be detected.

According to the apparatus for recognizing a three-dimensional position and orientation based on vision according to the tenth aspect, straight lines obtained by the unit direction vector calculation processing means corresponding to three or more parallel line segments of the marker, respectively. Is calculated by solving the unit direction vector for those intersections as a quadratic form minimization problem for the moment matrix. Therefore, even if an error occurs in the straight line detected in the same manner as described above, a more accurate unit direction can be obtained. Vectors can be detected.

[0039]

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 (not shown) travels on a track in a factory or the like, stops at a predetermined work position, and is controlled by a robot hand 1 attached to an upper part of the main body. An operation such as grasping or placing an object is performed, and the operation is stopped at a predetermined position based on a detection signal from a sensor, external information, or the like.

The robot hand 1 comprises an arm 2 rotatably held at the upper part of the main body of the autonomous mobile robot and a head 3 rotatably provided at the tip of the arm 2. A manipulator 4 for gripping an object is provided at 3. In the robot hand 1, the drive of the arm 2, head 3 and manipulator 4 is controlled in accordance with a control signal from a control device (not shown).

In this case, the arm portion 2 is rotatable in a horizontal plane with a vertical axis as a rotation axis at the base end 2a, and a horizontal axis as a rotation axis at a joint 2b provided at an intermediate portion. It is rotatable in a vertical plane. Also, a head unit 3 attached to the tip of the arm unit 2
Is rotatable about a horizontal axis as a rotation axis in a vertical plane, is rotatable about a vertical axis of the head section 3 as a rotation axis, and is rotated about an axis in a protruding direction of the head section 3. The shaft is rotatable in a twisting direction.

A small CCD camera 5 as an image pickup means is mounted on the upper part of the head 3 at a position where the operation of the manipulator 4 is not hindered. It has become. When the position and orientation of the CCD camera 5 are obtained by calculation based on the image data captured by the CCD camera 5 as described later, the operation of the robot hand 1 is controlled by the control device according to the result. The drive is controlled.

At the position where the manipulator 4 works, there is provided a marker 6 whose positional relationship with the object is arranged in a predetermined relationship. This marker 6
Has a configuration in which, for example, two square portions 7 and 8 displayed in black are arranged side by side on a white background portion as shown in FIG.

In this case, the square portions 7 and 8 are arranged so as to be in contact with one vertex, and four sides a1, a2, a3, a4 of the square portion 7 and four sides b of the square portion 8
1, b2, b3, and b4, sides a1 and b2;
The sides a2 and b1 are arranged so as to be located on a straight line. The point where the square portions 7 and 8 are in contact is defined as the origin O of the marker coordinate system. In addition, the size of the marker 6 is set so that when the image is taken by the CCD camera 5, the whole is within a field of view of the image taken with a sufficient size and without distortion.

The three-dimensional position / posture recognition device 9 is specifically composed of, for example, a computer for executing overall calculation control, and its internal functional block configuration is as shown in FIG. Is configured. That is,
In FIG. 1, the whole of a three-dimensional position and orientation recognition device 9 has, as a schematic configuration, an image processing unit 10 as image processing means,
A straight line fitting unit 11 as a straight line fitting unit, a vanishing point calculation unit 12 as a unit direction vector calculation unit, a position and orientation calculation unit 13 as a position and orientation calculation unit, a coordinate conversion matrix calculation unit 14 as a coordinate conversion matrix calculation unit, and storage It comprises a memory 15 as a means, and each executes a calculation process according to the flow of a flowchart described later.

The image processing section 10 includes a pre-processing calculation section 16 and a sample point extraction section 17. The pre-processing calculation section 16 converts the image data of the marker 6 captured by the CCD camera 5 into digital image data as binarized data as a pre-processing calculation, cuts out an image area where the marker 6 is present, and outputs it. The sample point extracting unit 17 extracts the contour points of the squares 7 and 8 from the coordinate data of the position where the marker image in the clipped image area exists, and outputs it as sample point coordinate data.

The straight-line fitting unit 12 includes the least-squares estimating unit 1
8 and a straight line parameter output unit 19. When the least square estimation unit 18 detects a straight line on the photographing screen from the sample point coordinate data as described later, the straight line parameter output unit 19 Parameters are output.

The vanishing point calculator 12 comprises a moment matrix calculator 20, a minimum eigenvalue calculator 21, a unit eigenvector calculator 22, an optimum vector calculator 23, and a vanishing point coordinate estimator 24. These are points at which the straight lines intersect at an infinity point on the shooting screen based on the detected straight line parameters based on the straight line parameters corresponding to the parallel line segments of the marker 6 as described later. The vanishing point is calculated, and is solved here as a quadratic form minimization problem.

The position and orientation calculator 13 includes a focal length calculator 2
5, a posture calculator 26 and a marker origin position calculator 27. These calculate an optimal vector, a focal length, a posture vector, and a marker origin position based on the obtained two-dimensional coordinate data of the vanishing point on the photographing screen and the marker coordinate system based on the origin O of the marker 6. It has become.

The memory 15 stores marker data indicating the shape and size of the marker 6 and the like, and also knows in advance information such as the angle of view of the CCD camera 5 and the aspect ratio (vertical and horizontal ratio of the screen). Is stored.

The coordinate transformation matrix unit 14 calculates the CC from the data representing the position and orientation of the marker 6 obtained as described above.
A coordinate transformation matrix representing the position and orientation of the marker 6 with respect to the coordinate system of the D camera 5 is calculated and obtained.
The calculation result is provided to a control device or the like for controlling the operation of the autonomous mobile robot via the data output unit 28.

Next, the operation of the present embodiment will be sequentially described with reference to FIGS. First, when approaching the vicinity of the work position, the autonomous mobile robot is stopped in a predetermined stop area by a separately provided sensor or the like, and is taught in advance so that the CCD camera 5 takes an image of the marker 6 at that position. ing. In this case, as shown in FIG. 5, the CCD camera 5 shoots the marker 6 obliquely from above with the square portions 7 and 8 arranged side by side. The whole image of the marker 6 is taken in the field of view without distortion.

Then, the three-dimensional position recognizing device 8 executes processing steps for recognizing the three-dimensional position and orientation in accordance with the flowchart of the recognition program shown in FIG.

(A) Image Processing Steps (S1 to S4) First, when the program is started, various initial value settings are executed (step S1), and then the preprocessing calculation unit 16 uses the CCD camera 5 The photographed image data of the marker 6 is input (step S2). Next, the pre-processing calculation unit 16 executes the pre-processing calculation in step S3. In this preprocessing calculation,
By binarizing the image data input from the CCD camera 5 with a predetermined threshold value, the analog data representing light and dark is converted into binary digital image data such as "1" or "0", and The image area where the marker 6 is photographed is cut out and output.

As a result, the digital image data at the positions corresponding to the square portions 7 and 8 drawn in black on the marker 6 and the digital image data at the positions corresponding to the white background portion are “1” and “0”, respectively. ] Is identified as any one of the digital image data.

Thereafter, in step S4, the sample extracting section 17 samples the contour lines corresponding to the sides a1 to a4, b1 to b4 of the square portions 7 and 8 of the marker 6 based on the digital image data. . As shown in the plan view of FIG. 6, there are six straight lines Lm1 to Lm6 as straight lines passing through the sides of the square portions 7 and 8 of the marker 6.

In sample extraction, these straight lines L
Straight lines L1 to L1 on the shooting screen corresponding to m1 to Lm6
In order to detect L6, as shown in FIG. 7, scanning is performed in the horizontal direction, that is, in the x-axis direction, on the shooting screen, and the coordinate data of the pixel at the position where the digital image data value changes is stored.
Accordingly, the sampling points on the sides a1 to a4 and b1 to b4 of the square portions 7 and 8 on the shooting screen are converted into, for example, s sample extraction points SPn [xn, yn] (where n = 1, 2,..., s).

(B) Straight-line fitting processing steps (S5 to S5)
S9) Next, proceeding to step S5, the straight-line fitting processing unit 11
Least squares estimation unit 18 and straight line parameter output unit 19
In, based on the obtained coordinate data of the s sample extraction points SPn, straight lines L1 to L6 on the shooting screen corresponding to the straight lines Lm1 to Lm6 of the marker 6 (see FIG. 8)
Is calculated by the least squares estimation method, and the parameters of the obtained straight line are output.

In this case, when performing the calculation by applying the least squares estimation method, in this embodiment, the straight line L1
To L6 are expressed in a Hessian standard form as shown in equation (1), and this equation includes a sampling point SPn [xn, y
n], and the calculation is executed by a derivation process as described in the appendix below to calculate the straight line parameters [θi, hi].

In the derivation process described in (Appendix) and the derivation in the following search process, “Camera calibration by computational projection geometry” by Onodera and Kanaya
(Information Processing Society of Japan Research Report 68-CV-1, 1990-0
9) was referred to.

[0061]

(Equation 1)

When the straight line is detected as described above, for example, if the image taken by the CCD camera 5 is out of focus, the image of the marker 6 may be blurred. As shown in FIG. 9, the outline may become ambiguous and a so-called gray zone GL may occur. In this embodiment, in response to such a focus shift, in step S6,
By detecting a plurality of straight lines Li1, Li2,... Corresponding to the gray zone GL of the portion where the straight line Li is to be detected, and averaging the straight line parameters, the parameters of one straight line Li are determined. Like that.

As described above, for example, the straight line L
Upon completion of the calculation for step S1, the parameters [θ1, h1] of the straight line are obtained in step S7.
Steps S5 to S7 are executed again after steps S8 and S9, and the parameters of the straight line [θ2,
h2], and similarly, the parameters of the straight lines L3 to L6 are calculated and calculated.

(C) Marker unit direction vector calculation processing steps (S10 to S17) Next, the unit direction vectors indicating the orientation of the marker 6 being photographed by the CCD camera 5 are obtained from the six unit direction vectors obtained as described above. The parameters [θi, hi] (i
= 1 to 6).

In this case, a parallel straight line L in the plane of the marker 6
With respect to the set of m1 to Lm3 and the set of parallel straight lines L1 to L3 and the set of L4 to L6 corresponding to the set of m1 to Lm3 and the set of straight lines Lm4 to Lm6, the marker 6 is photographed from the oblique direction by the CCD camera 5. Therefore, points corresponding to infinity in each set of parallel straight lines intersect at a certain point on the imaging screen.

At this time, assuming that these intersections are vanishing points V1 and V2, respectively, by setting the coordinate system of the marker 6, a two-dimensional image of the marker coordinate origin O and the vanishing points V1 and V2 on the photographing screen is obtained. When the geometric properties are used based on the coordinate data, the position and orientation of the CCD camera 5 can be uniquely determined.

However, when the vanishing point V1 or V2 is calculated for each set of the above-described straight lines, generally, FIG.
, A plurality of vanishing point candidates V1a, V1b,.
, And V2a, V2b,... Are obtained. In order to find the optimum vanishing point V1 or V2 from the data of these vanishing point candidates, the optimum vanishing point V1 or V2 is separately calculated by the following two methods, and the optimal vanishing point V1 is obtained from the results. , V2 are calculated.

That is, the first is (a) a method of calculating using the law of invariability of the compound ratio, and the second is (a) a method of calculating as a quadratic form minimization problem. In using these calculation methods, based on data obtained in advance through experiments and the like, how to combine and use the results of the two calculation methods is designated by an index α, and steps S10 and S10 are used. In S13, the index α
The selection is divided according to the value of.

In this case, the setting of the index α value is determined in the range of 0 ≦ α ≦ 1 based on data obtained in advance through experiments and the like. The value of the index α is used as an index when obtaining the optimum vectors mo1 and mo2 respectively corresponding to vanishing points V1 and V2, which will be described later. Unit direction vectors m1 ′ and m2 ′ obtained based on the law,
(A) Combining unit direction vectors m1 and m2 obtained as a quadratic form minimization problem to obtain optimal vectors mo1 and mo2
Is set for the combination ratio of the calculation formula (2).

When the value of the index α is α = 1, only the unit direction vectors m1 and m2 are used, and when the value of α = 0, only the unit direction vectors m1 ′ and m2 ′ are used. This corresponds to obtaining the vectors mo1 and mo2. When the value of the index α is 0 <α <1, the optimal vectors mo1, m
o2 is to be obtained.

[0071]

(Equation 2)

(A) Calculation process using the rule of invariance of compound ratio Here, the case where the value of the index α is not “1” corresponds to “1”, and “YES” is determined in step S10, and steps S11 and S12 are performed. Is executed when the calculation step is advanced.

The law of cross ratio invariance is that "projection transformation from a straight line to a straight line is uniquely determined by specifying how three different points on the straight line are mapped". It is a geometric law, and using this,
For example, as shown in FIG. 11, points A, B, C and D on a straight line L (corresponding to the straight lines Lm1 to Lm6 on the marker 6) are on a straight line L '(corresponding to the straight lines L1 to L6 on the photographing screen). Do)
Are mapped to the points A ', B', C ', and D', the relationship between the lengths of the line segments demarcated between these points is as follows:
Equation (3) is obtained.

[0074]

(Equation 3)

Then, the line segments AB and B on the straight line L
Assuming that the length dimension of C is "1" and the point Dn is located at a distance n from the point A, the left side of the equation (3) is as shown in the equation (4). Assuming that the length of the line segment A'Dn 'is dn, the right side of Expression (3) becomes Expression (5). From the relationship of Expression (3), since the values on the right sides of Expression (4) and Expression (5) are equal, Expression (6) is obtained.

[0076]

(Equation 4)

Then, when equation (6) is solved for the length dn on the straight line L ', equation (7) is obtained. Here, considering the case where the point Dn of the straight line L is at infinity, the corresponding point Dn 'on the straight line L' is defined by the length dn of n when the infinity in the equation (7) becomes infinite. Equation (8) is obtained because of the value. That is, the value dn of this length dimension is the distance of the position corresponding to the vanishing point.

[0078]

(Equation 5)

Therefore, the vanishing points V1 and V
12, each intersection of straight lines L1 to L6 on the photographing screen is extracted as feature points P1 to P9 as shown in FIG. 12, and these feature points and vanishing points V1 and V2 to be found are extracted. Table 1 shows that the relationship between the coordinates (xv1, yvi) and (xv2, yv2) corresponds to points A to C shown in Expression (8).
Thus, each coordinate value may be obtained using the relationship of the above equation (8).

[0080]

[Table 1]

That is, from the relationship of equation (8), each straight line L
1 to L6, the distance dv (t) between the feature points (t = 1
To 6) can be expressed as in equation (9), so that vanishing point V1,
The value of each coordinate value xv1, yv1, xv2, yv2 of V2 can be expressed as in equations (10) to (13).

[0082]

(Equation 6)

The two-dimensional coordinates of the vanishing points V1 and V2 on the photographing screen are represented by V1 (xv1, yv
1), V2 (xv2, yv2), CCD
The vector from the camera 5 to the vanishing points V1 and V2 is the straight line Lm1 of the actual marker 6 in the three-dimensional space.
It is a unit vector indicating the directions of Lm3 and Lm4 to Lm6.

Therefore, as shown in FIG. 13, the unit direction vector from the origin O of the marker 6 toward the vanishing point V1 is m1 '(x-axis direction), and the unit direction vector toward the vanishing point V2 is m2' (y-axis direction). ), The unit direction vectors m1 'and m2' can be expressed as in equations (14) and (15), where f (focal length) is the distance from the CCD camera 5 to the center point of the shooting screen. it can. That is, in step S12, the unit direction vectors m1 'and m2' are calculated based on the equations (14) and (15).

[0085]

(Equation 7)

(A) Calculation process as quadratic form minimization problem If the value of index α is not “0”, step S
When it becomes 13, it is determined "YES" here and step S
This calculation process is executed in 14 to S16.

First, in step S 14, the moment matrix calculator 20 calculates each straight line Lm 1 through Lm 1 of the marker 6.
In order to obtain the unit direction vectors m1 and m2 for Lm6, based on the straight line parameters [θi, hi] (i = 1 to 6) of the straight lines L1 to L6 calculated in step S7,
Moment matrix M defined by equations (16) and (17)
1, M2 is calculated.

In this case, if a unit normal vector of a plane including the viewpoint P of the CCD camera 5 and the straight line Lmi is defined as an N vector ni of the straight line Lmi with respect to the straight line Lmi in the three-dimensional space, the straight line Lmi becomes Since the above is expressed as a straight line Li as in equation (1), its N vector ni is as in equation (18). However, equation (1)
F 'in 8) is data indicating the temporary focal length of the CCD camera 5.

The value of the provisional focal length f 'is stored in the memory 15
In this embodiment, the nominal value of the specification of the CCD camera 5 or the already calculated result is used in order to improve the calculation accuracy.

[0090]

(Equation 8)

In the following steps S15 and S16, a set of three straight lines (Lm1 to Lm3, Lm4 to Lm
m6), the N-vectors m1 and m2 of vanishing points V1 and V2, which are the intersections of the straight lines in each set, are converted to the quadratic form minimization problem for the moment matrices M1 and M2, that is, mi = ｛[m, Mim]]. It solves as m｝ to be minimized, and calculates m1 and m2 as unit eigenvectors for the minimum eigenvalues of M1 and M2. As is well known, the quadratic form [m, Mm] (i = 1,
Unit vectors m1 and m2 that minimize 2) are eigenvectors for the minimum eigenvalues of the moment matrices of equations (16) and (17), respectively.

That is, in step S15, the minimum eigenvalue is calculated by the minimum eigenvalue calculator 21, and in subsequent step S16, the unit eigenvectors m1 and m2 are calculated by the unit eigenvector calculator 22.

In this case, if the point P whose N vector is m (the N vector of the point P is defined as a unit vector from the viewpoint to the point P in the space coordinate system) is on all the straight lines Li, Dot product (m, ni) = 0 for all i
Therefore, considering a unit vector that minimizes the following equation (19), it results in the quadratic form minimization problem as described above. Then, from the above results, the unit eigenvector m
1 and m2 are obtained as in equations (20) and (21). Note that f in Expressions (20) and (21) is the true focal length of the CCD camera 5.

[0094]

(Equation 9)

As described above, the unit direction vectors m1 'and m2' based on the law of invariability of the cross ratio (a) and the unit eigenvectors m1 and m1 obtained as the quadratic form minimization problem (a)
Based on m2, in step S17, the optimum vector calculation unit 23 calculates the optimum vectors mo1 and mo2 according to the value of the index α according to the above-described equation (2), and sets these as unit direction vectors m1 and m2. Will decide.

(D) Position and orientation calculation processing step (S18)
-S20) Next, when the process proceeds to step S18, the focal length calculator 25 calculates the true focal length f of the CCD camera 5 based on the result described above. That is, since the unit direction vectors m1 and m2 are orthogonal to each other, the inner product becomes 0 when the inner product thereof is obtained, and the equation (22) is established. When f is solved from this, the value of the true focal length f is obtained by the equation (23) It is obtained as follows.

[0097]

(Equation 10)

Subsequently, when the flow proceeds to step S19, the posture calculating section 26 calculates the posture vector [ms1, ms2, ms3] of the marker 6 shown in FIG. In this case, the attitude vector ms3 is equal to the attitude vectors ms1 and ms.
Since it is given as an outer product of 2, it becomes as shown in equation (24). Then, the posture vectors ms1 and ms2 can be calculated from the above-described optimum vectors mo1 and mo2 and the value of the true focal length f as in Expression (25).

[0099]

[Equation 11]

Next, at step S20, the marker origin position calculator 27 calculates the origin position O of the marker 6. That is, first, based on the parameters of the six straight lines L1 to L6 obtained in step S7 and the value of the true focal length f, the N vectors n1 to n of the straight lines L1 to L6 are determined.
6 is calculated according to the following equation (26). From these, the N vector mPk of each feature point Pk (k = 1 to 9) is calculated according to the equation (2).
Calculate according to 7). Note that the N vector m of the point P
P is defined as a unit direction vector from the viewpoint V of the CDD camera 5 toward the point P.

Then, from these, C for each point Pk
The distance between the viewpoint of the CD camera 5 and the origin O of the marker 6 is calculated and the average value thereof is calculated to calculate the distance R to the marker origin position according to equation (28).

[0102]

(Equation 12)

(E) Coordinate transformation matrix calculation processing step (S
21) In this way, when the origin position and orientation of the marker 6 are obtained,
Proceeding to step S21, the coordinate transformation matrix calculation unit 14
Based on the above result, a coordinate conversion matrix indicating a position and an orientation relative to the spatial coordinate system of the CCD camera 5 is calculated as a position parameter divided into a rotation conversion matrix and a translation conversion matrix by a known calculation method. become.

According to the present embodiment, as the marker 6, two simple square portions 7, 8 are displayed in black and white binarized information. Since the three-dimensional position and orientation can be recognized by photographing, the line segment of the marker 6 can be extracted as edge data of the square portions 7 and 8, and the line segment having no line width can be accurately extracted. Since the detection can be performed well and the CCD camera 5 can be detected without moving the CCD camera 5 to the front of the marker 6, the degree of freedom of the photographing position of the CCD camera 5 is increased and the movement range of the robot hand 1 is restricted. , And the position and orientation of the CCD camera 5 can be recognized at high speed, with high accuracy, and robustly while simplifying image processing.

According to the present embodiment, when the straight-line fitting processing unit 11 detects a straight line, the process proceeds to step S
In FIG. 6, a single straight line is determined by detecting a plurality of straight lines and averaging them.
Even when the gray zone GL occurs due to the defocus of the imaging screen of the marker 6 by the D camera 5, an accurate straight line detection process can always be performed.

In this embodiment, in the calculation of the unit direction vector, the vanishing points V1 and V2 are obtained by setting the index α by a method utilizing the law of invariance of the cross ratio, and the quadratic form minimization problem Vanishing point V1, V2
Since the optimum vector of the unit direction vector is calculated by obtaining the N vector of the above, an appropriate unit direction vector can be calculated as needed.

FIGS. 15 to 19 show the second to sixth embodiments of the present invention, and all show markers 29 to 33 instead of the markers 6 in the first embodiment. .

That is, in the second embodiment shown in FIG. 15, the marker 29 is replaced with the marker 6 used in the first embodiment.
Are formed by displaying square portions 34 and 35 in a state where the black and white of the square portions 7 and 8 with respect to the background portion are inverted.
With this, the same operation and effect as those of the first embodiment can be obtained.

In the third embodiment shown in FIG. 16, the marker 30 is replaced by squares 7 and 8 in which two rectangles 36 and 37 are arranged in a C shape so as to form a right angle. The teaching is performed so that the CCD camera 5 captures an image from the direction shown in the figure, and the position and orientation are recognized.

In this case, the same effect as that of the first embodiment can be obtained, and even if the inclination angle when the marker 29 is photographed by the CCD camera 5 is large, the rectangular portion 36 located on the far side is obtained. , 37 can be input as data having relatively long dimensions on the photographing screen, so that the accuracy of detecting straight lines can be improved.

In the fourth embodiment shown in FIG. 17, a marker 31 is represented by a small square 39 with different shading information inside a large square 38, and a position where the diagonal lines of the squares 38 and 39 overlap. The same operation and effect as in the first embodiment can be obtained by using such a marker 31.

In the fifth embodiment shown in FIG. 18, the marker 32 is displayed by arranging square portions 40 and 41 corresponding to the square portions 7 and 8 used in the marker 6 in two upper and lower stages. In the sixth embodiment shown in FIG. 19, the markers 33 are displayed by arranging three square portions 42 to 44 and a right-angled triangular portion 45 at equal intervals from each other. The first by the markers 32 and 33
The same operation and effect as those of the embodiment can be obtained.

In each of the above-described embodiments, the case where the markers 6, 29 to 33 are displayed using black and white shading information has been described. However, the present invention is not limited to this, and it is also possible to display the markers 6, 29 to 33 using color information. In this case, in addition to using a CCD camera capable of taking color images,
The preprocessing calculation unit may be configured to binarize the color signal.

Further, in each of the above embodiments, the case where the present invention is applied to the three-dimensional position recognition device of the autonomous mobile robot has been described. However, the robot itself is fixed, and the work target is located near the robot. The present invention can also be applied to a moving three-dimensional position recognition device or the like.

(Appendix) Detection of Straight Line by Least Square Estimation Method The derivation process for the detection of a straight line by the least square estimation method in the above-described straight line fitting processing step will be described. As described above, when the equation of a straight line is expressed in Hessian standard form, it is expressed as equation (a), and N extracted sample points P
The distance from i (xi, yi) is as shown in equation (b). Therefore, by obtaining the values of θ and h when minimizing the value of the function defined as in equation (c), equation (a) is obtained.
Is obtained.

[0116]

(Equation 13)

Here, various symbols used in the following derivation process are defined as in the following equations (d) to (f).

[0118]

[Equation 14]

By differentiating equation (c) with h and setting it to "0", h is obtained as in equation (g). Substituting the result of equation (g) into equation (c), differentiating by θ and setting it to “0” gives θ
Is obtained as in equation (h).

[0120]

(Equation 15)

In this case, the conditions for selecting h and θ may be expressed by equations (i) and (j), respectively.

[0122]

(Equation 16)

[0123]

As described above, the method for recognizing three-dimensional position and orientation based on vision and the apparatus for recognizing three-dimensional position and orientation based on vision according to the present invention have the following excellent effects. .

That is, according to the method for recognizing a three-dimensional position and orientation based on vision, two line segments having a predetermined dimension on a plane and orthogonal to each other, and the two line segments are respectively provided. In the image processing step, a marker displaying a figure surrounded by parallel line segments and displaying a background portion by a change in density information or color information is photographed by imaging means positioned in an oblique direction of the three-dimensional space. The contour point position is extracted, and in a straight line fitting processing step, a parameter of a straight line corresponding to the line segment of the marker is obtained by a least squares estimation method, a unit direction vector is calculated in a unit direction vector calculation processing step, and a position and orientation calculation step is performed. Calculating a parameter indicating the position and orientation of the marker and calculating a coordinate transformation matrix representing the position and orientation of the marker as viewed from the reference coordinate system of the imaging means. As a result, the orthogonal line segment displayed on the marker can be accurately detected, and the image processing is performed on a simple figure, so that the calculation processing is simplified, and the three-dimensional processing is performed at high speed, with high accuracy, and robustly. It has an excellent effect that the position and the posture can be recognized.

According to the three-dimensional position and orientation recognition method based on vision, the markers are displayed as two or more simple figures having a rectangular or square shape. In addition to simplifying the calculation process of the posture recognition, for example, by arranging the figures so as to be arranged in the horizontal direction, even if the inclination angle of the marker with respect to the photographing screen of the imaging means becomes large, the line segment There is an excellent effect that it is possible to reduce an error due to defocus by reducing a change in distance in the depth direction and to perform accurate detection.

According to the method for recognizing a three-dimensional position and orientation based on vision according to the third aspect, in the straight line fitting processing step, one of the markers is determined based on the data of the contour point position extracted in the image processing step. A plurality of straight lines are detected corresponding to the line segments, and one straight line is obtained by averaging the straight lines to determine the straight line parameters. Even when the outline cannot be detected as an orthogonal line segment due to the displacement, an excellent effect that an accurate straight line can be detected is obtained.

According to the method for recognizing a three-dimensional position and orientation based on vision, in the unit direction vector calculation processing step, three or more vanishing point candidate values are calculated using the law of invariance of cross ratio. Then, since the unit direction vector is calculated based on the two-dimensional coordinate data of those candidate values, for example, a vanishing point obtained from the intersection point of the straight line detected corresponding to the parallel line segment has an error. In the case where there are a plurality of, there is an excellent effect that a more accurate unit direction vector can be detected.

According to the three-dimensional position and orientation recognition method based on vision, in the unit direction vector calculation processing step, straight lines obtained respectively corresponding to three or more parallel line segments of the marker. Is calculated by solving the unit direction vector for those intersections as a quadratic form minimization problem for the moment matrix. Therefore, even if an error occurs in the straight line detected in the same manner as described above, a more accurate unit direction can be obtained. This has an excellent effect that a vector can be detected.

According to the apparatus for recognizing a three-dimensional position and orientation based on vision according to claim 6, since the marker according to claim 1 is used and each means for executing each processing step is provided. In addition to being able to accurately detect orthogonal line segments displayed on the marker and performing image processing on a simple figure, the calculation processing is simplified, and the three-dimensional position and orientation can be quickly, accurately, and robustly. It has an excellent effect that it can be recognized.

According to the apparatus for recognizing a three-dimensional position and orientation based on vision according to the seventh aspect, the markers are displayed by two or more simple figures having a rectangular or square shape. In addition to simplifying the calculation process of the posture recognition, for example, by arranging the figures so as to be arranged in the horizontal direction, even if the inclination angle of the marker with respect to the photographing screen of the imaging means becomes large, the line segment There is an excellent effect that it is possible to reduce an error due to defocus by reducing a change in distance in the depth direction and to perform accurate detection.

According to the three-dimensional position and orientation recognition apparatus based on vision, one of the markers is determined by the straight line fitting processing means based on the data of the contour point position extracted in the image processing step. A plurality of straight lines are detected corresponding to the line segments, and one straight line is obtained by averaging the straight lines to determine the straight line parameters. Even when the contour cannot be detected as an orthogonal line segment due to the displacement, an excellent effect is obtained in that it can be detected as a more accurate straight line by averaging a plurality of straight lines obtained from the contour point positions. .

According to the apparatus for recognizing three-dimensional position and orientation based on vision according to the ninth aspect, the unit direction vector calculation processing means calculates three or more vanishing point candidate values using the law of invariance of cross ratio. Then, since the unit direction vector is calculated based on the two-dimensional coordinate data of those candidate values, for example,
Even if there is an error in the straight line detected corresponding to the parallel line segment and there are a plurality of vanishing points obtained from the intersections, an excellent effect that a more accurate unit direction vector can be detected can be obtained. Play.

According to the apparatus for recognizing a three-dimensional position and orientation based on vision according to the tenth aspect, straight lines obtained by the unit direction vector calculation processing means respectively corresponding to three or more parallel line segments of the marker. Is calculated by solving the unit direction vector for those intersections as a quadratic form minimization problem for the moment matrix. Therefore, even if an error occurs in the straight line detected in the same manner as described above, a more accurate unit direction can be obtained. This has an excellent effect that a vector can be detected.

[Brief description of the drawings]

FIG. 1 is an overall block diagram showing a first embodiment of the present invention.

FIG. 2 is a diagram showing a relationship between a robot hand and a marker.

FIG. 3 is a plan view of a marker.

FIG. 4 is a schematic flowchart of a recognition program.

FIG. 5 is a photographing position and posture of a marker by a CCD camera.

FIG. 6 is an explanatory diagram showing each line segment of a marker and a straight line to be detected;

FIG. 7 is an explanatory diagram showing a scanning direction of a sample extraction point on a shooting screen;

FIG. 8 is an explanatory diagram showing a relationship between a square portion of a marker and a straight line to be detected.

FIG. 9 is an explanatory diagram of a case where compensation is performed when a focus shift has occurred;

FIG. 10 is an explanatory diagram when a plurality of vanishing points occur for three or more straight lines.

FIG. 11 is an explanatory diagram of the law of invariance of cross ratio.

FIG. 12 is an explanatory diagram of a relationship between a feature point on a shooting screen and a straight line.

FIG. 13 is an explanatory diagram of a marker posture vector and a coordinate system.

FIG. 14 is an explanatory diagram of an optimal vanishing point direction vector.

FIG. 15 is a view corresponding to FIG. 3, showing a second embodiment of the present invention.

FIG. 16 is a view corresponding to FIG. 3, showing a third embodiment of the present invention.

FIG. 17 is a view corresponding to FIG. 3, showing a fourth embodiment of the present invention;

FIG. 18 is a view corresponding to FIG. 3, showing a fifth embodiment of the present invention;

FIG. 19 is a view corresponding to FIG. 3, showing a sixth embodiment of the present invention;

[Explanation of symbols]

1 is a robot hand, 2 is an arm, 3 is a head, 4
Is a manipulator, 5 is a CCD camera (imaging means), 6
Is a marker, 7 and 8 are square parts, 9 is a three-dimensional position and orientation recognition device, 10 is an image processing unit (image processing means), 11 is a straight line fitting unit (straight line fitting means), and 12 is a vanishing point calculation unit (unit direction) Vector calculation means), 13 is a position and orientation calculation unit (position and orientation calculation means), 14 is a coordinate transformation matrix calculation unit (coordinate transformation matrix calculation means), 15 is a memory (storage means), 1
6 is a preprocessing calculation unit, 17 is a sample point extraction unit, 18 is a least squares estimation unit, 19 is a linear parameter output unit, 20 is a moment matrix calculation unit, 21 is a minimum eigenvalue calculation unit, 22 is a unit eigenvector calculation unit, 23 Is the optimal vector calculator,
24 is a vanishing point coordinate estimating unit, 25 is a focal length calculating unit, 26
Is a position and orientation calculation unit, 27 is a marker origin calculation unit, 28 is a data output unit, and 29 to 33 are markers.

──────────────────────────────────────────────────続 き Continuation of the front page (56) References JP-A-6-243236 (JP, A) JP-A-5-321521 (JP, A) JP-A-5-126527 (JP, A) JP-A-2- 38804 (JP, A) JP-A-1-242903 (JP, A) JP-A-63-133002 (JP, A) JP-A-1-133183 (JP, A) JP-A-5-2413 (JP, A) WO 93/15376 (WO, A1) (58) Fields investigated (Int. Cl. ⁶ , DB name) G01B 11/00-11/30 102 B25J 3/00-3/04 B25J 9/10-9 / 22 B25J 13/00-13/08 B25J 19/02-19/06 G05B 19/18-19/46 G05D 3/00-3/20 G06T 1/00-9/20

Claims (10)

(57) [Claims] 1. A figure surrounded by two line segments having a predetermined dimension and orthogonal to each other and a line segment parallel to each of the two line segments is represented by shading information or color information with respect to a background portion. Characterized by performing the following processing steps (a) to (e) based on image data obtained when the marker is photographed from an oblique direction by an image pickup means. 3D position and posture recognition method based on visual perception. (A) Image processing for selecting an image area of the marker based on image data from the imaging means, detecting a change point of a value of image data in the selected image area, and extracting a contour point position And (b) minimizing parameters of a straight line including a line segment orthogonal to the marker and a line segment parallel to the marker on the photographing screen based on the data of the contour point position extracted in the image processing step. A straight line fitting processing step calculated by a square estimation method; (c) based on vanishing point data which is an intersection of the straight lines calculated in the straight line fitting processing step corresponding to the parallel line segments displayed on the markers. A unit direction vector calculation processing step of calculating a unit direction vector of the marker; Position and orientation calculation processing step of setting a marker coordinate system with a direction normal to the marker plane and a normal direction passing through the origin as a reference direction, and calculating a parameter indicating the position and orientation of the marker based on the unit direction vector (E) based on a parameter calculation result indicating the position and orientation of the imaging means in the position and orientation calculation processing step,
A coordinate conversion matrix calculation processing step of calculating a coordinate conversion matrix representing the position and orientation of the marker as viewed from a reference coordinate system of the imaging means. 2. The method according to claim 1, wherein the marker includes at least two markers that are displayed based on a change in density information or color information with respect to a background portion.
A plurality of rectangular or square figures, which are arranged so that they contact at one vertex and adjacent sides including the vertex form a right angle. 3. The method for recognizing a three-dimensional position and posture based on vision according to 1. 3. In the straight line fitting processing step, a plurality of straight lines corresponding to one line segment of the marker are detected based on the data of the contour point position extracted in the image processing step, 3. A method for recognizing a three-dimensional position and orientation based on vision according to claim 1, wherein one straight line is obtained by averaging the straight lines and the straight line parameters are determined. 4. In the unit direction vector calculation processing step, three or more candidate values of the vanishing point are calculated using a law of invariance of cross ratio, and based on two-dimensional coordinate data of the candidate values. The method of recognizing a three-dimensional position and orientation based on vision according to claim 2 or 3, wherein a unit direction vector is calculated. 5. In the unit direction vector calculation processing step, for a straight line obtained corresponding to each of three or more parallel line segments of the marker, a unit direction vector with respect to an intersection point between the straight lines is quadratic with respect to a moment matrix. 5. The method according to claim 2, wherein the unit direction vector is calculated by solving the unit direction vector as a form minimization problem. 6. A state in which two parallel line groups each having at least two parallel line segments having a predetermined dimension are arranged so as to be orthogonal to each other on a plane due to a change in density information or color information with respect to a background portion. An imager that outputs image data of a captured image; a storage device that stores marker data indicating the shape and dimensions of the marker; an image of the marker based on image data from the imager Image processing means for selecting a region, detecting a change point of the value of the image data in the selected image region, and extracting a contour point position; data of the contour point position extracted by the image processing means A straight line fitting processing means for obtaining a parameter of a straight line including each line segment of the group of orthogonal parallel lines of the marker based on the least squares estimation method, A unit direction vector calculation processing unit that calculates a unit direction vector based on data of a vanishing point that is an intersection point on the shooting screen of a straight line including each line segment of the set of parallel line groups, A position and orientation for calculating a position and orientation of the marker based on the marker coordinate system and the unit direction vector, by setting a marker coordinate system using two orthogonal straight lines and a normal passing through the intersection point as axes of the coordinate system. Calculating a coordinate conversion matrix representing the position and orientation of the marker as viewed from a reference coordinate system of the imaging unit based on a parameter calculation result indicating the position and orientation of the imaging unit by the position and orientation calculation processing unit. An apparatus for recognizing a three-dimensional position and orientation based on vision, comprising: a coordinate transformation matrix calculation unit. 7. The image processing apparatus according to claim 1, wherein the marker is at least two markers displayed based on a change in density information or color information with respect to a background portion.
A plurality of rectangular or square figures, which are arranged so that they contact at one vertex and adjacent sides including the vertex form a right angle. 6. A three-dimensional position and orientation recognition apparatus based on vision according to 6. 8. The straight line fitting processing means detects a plurality of straight lines corresponding to one line segment of the marker, based on the data of the contour point position extracted by the image processing means, 8. A three-dimensional position and orientation based on vision according to claim 6, wherein a straight line is obtained by performing a straight line averaging process, and the straight line parameters are determined. Recognition device. 9. The unit direction vector calculation processing means,
The apparatus is configured to calculate three or more candidate values of the vanishing point using the law of invariance of cross ratio, and calculate the unit direction vector based on two-dimensional coordinate data of the candidate values. 9. The three-dimensional position and orientation recognition device based on vision according to claim 7 or 8. 10. The unit direction vector calculation processing means converts a unit direction vector with respect to an intersection point between straight lines obtained respectively corresponding to three or more parallel line segments of the marker into a quadratic form with respect to a moment matrix. The system according to claim 7, wherein the unit direction vector is calculated by solving as a minimization problem.
10. A three-dimensional position and orientation recognition apparatus based on vision according to 8 or 9.