JP5699697B2 - Robot device, position and orientation detection device, position and orientation detection program, and position and orientation detection method - Google Patents

Description

  The present invention relates to a robot apparatus, a position / orientation detection apparatus, a position / orientation detection program, and a position / orientation detection method.

Conventionally, a technique for estimating the position and orientation of a three-dimensional space of an object expressed by a planar image is known. For example, a recognition device is known that accurately recognizes an object whose appearance changes depending on at least one of position and direction (see, for example, Patent Document 1).
This recognition apparatus sets a reference coordinate system in advance based on the degree of similarity between an object and each of a plurality of template information. Then, the recognition apparatus obtains a weighted similarity from a weighting factor and a similarity that are higher as the similarity is higher at each position in the reference coordinate system, and recognizes an object using the weighted similarity. To do.

JP 2009-301382 A

  However, this recognition device is a device intended to operate an electronic device in a non-contact manner, particularly by recognizing an action of an object (for example, a human hand). The method of recognizing an object based on the weighted similarity applied to this recognition apparatus is a method of recognizing an object probabilistically, and therefore recognizes the movement of a person's hand or foot that changes roughly. That is, it is suitable for applications in which the range of spatial recognition is relatively wide. However, for example, in the visual feedback control of the manipulator, it is necessary to recognize the position and posture of the manipulator with high accuracy, and thus the above recognition device cannot be applied to the robot device. Note that the visual feedback control is to control the operation of the robot apparatus, for example, the position and posture of the manipulator based on visual information acquired by a visual sensor such as an imaging apparatus.

  Therefore, the present invention has been made to solve the above problem, and a robot apparatus, a position / orientation detection apparatus, which detects a position and an attitude of a target object expressed by a planar image in a three-dimensional space with high accuracy, An object is to provide a position and orientation detection program and a position and orientation detection method.

[1] In order to solve the above-described problem, a robot apparatus according to an aspect of the present invention includes an imaging unit that captures an image of a target object and generates image data, a robot body that movably supports the imaging unit, For each line-of-sight direction from the imaging unit with respect to the reference of the target object, template image data of the surface with respect to the line-of-sight direction, information representing the position and orientation of the surface in the robot coordinate system, the position of the imaging unit in the robot coordinate system, and A template information storage unit that stores template information having information representing a posture; and a surface that is visible from the image of the target object included in the image data generated by the imaging unit; and from the template information storage unit The template information corresponding to the visible surface is read, and the robot is read based on the template information. And a position and orientation data generating unit that generates position and orientation data representing the position and orientation of the target object in a coordinate system, the position and orientation data generating unit, for each template image data, a plurality extracted from the template image data Extracting a plurality of feature points from the image data generated by the imaging unit, and a template feature data storage unit that stores template feature data in which feature quantities in each of the feature points are associated with positional information of the feature points; A feature data generation unit that generates feature data in which the feature amount for each feature point and the position information of the feature point are associated with each other, and the feature data generated by the feature data generation unit and stored in the template feature data storage unit On the basis of the template feature data thus obtained, the feature amount in the feature data and the template The degree of correlation with the feature quantity in the feature data is calculated, the position information of the feature point corresponding to the matching feature quantity is detected, and based on the position information, the surface that is visible in the image of the target object and the visible And a first position / orientation calculation unit that detects a reference surface that is the surface having the highest degree of similarity with the template image data corresponding to the surface .
Here, the robot body is, for example, an articulated robot. The reference is, for example, a non-defective sample of the target object. Further, the line-of-sight direction is, for example, directions orthogonal to each other, and may be six directions orthogonal to each other, and among these six directions, the five directions excluding the vertically downward when the reference is placed on a horizontal surface in a normal use state. Good. In addition, information representing the position and orientation is expressed by a translation vector and a rotation matrix.
With this configuration, according to one aspect of the present invention, the position and orientation of the target object are calculated based on the template information corresponding to the visible surface obtained from the target object image. The position and orientation of the target object in the three-dimensional space can be detected with high accuracy.
In addition, according to one aspect of the present invention, since the position and orientation of the target object are obtained using all the visible surfaces from the current viewpoint, the position and orientation of the target object represented by the planar image are accurately determined. Can be detected.
[2] In the robot apparatus according to [1], the position / orientation data generation unit is stored in the position information of the feature point detected by the first position / orientation calculation unit, the reference plane, and the template information storage unit. A combined model image generating unit that generates combined model image data based on the template information, and a converted combined model obtained by projective conversion of the combined model image data generated by the combined model image generating unit using a homography matrix A second position / orientation calculation unit that calculates similarity between image data and the image data, and generates the position / orientation data based on the homography matrix and the template information that cause the similarity to exceed a threshold; It is characterized by providing .
[3] In the robot apparatus according to [1], the position / orientation data generation unit converts the template image data obtained by projective conversion of each of the template image data using a homography matrix and the image. The degree of coincidence with the data is calculated, and a coincidence degree evaluation unit that detects a visible surface from the image of the target object based on the degree of coincidence, and corresponds to the visible surface detected by the coincidence degree evaluation unit The similarity between the converted image data obtained by projective conversion of the template image data using the homography matrix and the image data is calculated, and the template information is based on the homography matrix corresponding to the calculation result that gives the highest similarity and the template information. And a position / orientation calculation unit that generates the position / orientation data, and the first position in the position / orientation data generation unit In the posture calculation unit, when the similarity corresponding to the reference plane is below a threshold, to function the position and orientation data generating unit, characterized in that.
Here, the degree of coincidence is, for example, the sum of absolute values of luminance differences between image data and converted image data or the square sum of luminance differences. The similarity is, for example, normalized cross correlation.
With this configuration, according to one aspect of the present invention, the position and orientation of the target object can be obtained with high accuracy based on the most reliable surface among the visible surfaces. The position and orientation of the target object in the three-dimensional space can be detected with high accuracy. In addition, according to one aspect of the present invention, since the position and orientation are obtained using only one representative surface among the visible surfaces, the load on the calculation is light.
Further, according to one aspect of the present invention, when the degree of distortion or deformation of the reference surface is relatively low, processing based on one surface is performed, while when the degree of distortion or deformation of the reference surface is relatively high , Processing based on all visible surfaces can be performed. That is, according to the state of the visible surface (for example, distortion or deformation), it is possible to switch between the detection processing of the position and orientation of the target object based on one surface and the detection processing based on all the visible surfaces, and the detection accuracy based on the viewpoint is improved. Variation can be suppressed.

[4] In order to solve the above-described problem, a position / orientation detection apparatus according to an aspect of the present invention includes, for each line-of-sight direction with respect to a reference of a target object, template image data of the surface with respect to the line-of-sight direction and the surface in the reference coordinate system. A template information storage unit for storing template information having information representing the position and orientation of the image and information representing the position and orientation of the viewpoint in the reference coordinate system; and the target object included in image data obtained based on a predetermined viewpoint A visible surface from the image, and reading template information corresponding to the visible surface from the template information storage unit, and based on the template information, the position and orientation of the target object in the reference coordinate system and position and orientation data generating unit that generates position and orientation data representing, wherein the position and orientation data A generating unit that stores, for each template image data, template feature data in which template feature data in which a feature amount in each of a plurality of feature points extracted from the template image data is associated with position information of the feature points is stored; A feature data generation unit that extracts a plurality of feature points from the image data and generates feature data in which feature amounts of the feature points are associated with position information of the feature points; and the feature data generation unit generates Based on the feature data and the template feature data stored in the template feature data storage unit, the degree of correlation between the feature quantity in the feature data and the feature quantity in the template feature data is calculated and matched. Detecting position information of the feature point corresponding to the quantity, and based on the position information, an image of the target object A first position and orientation calculation unit that detects a visible surface and a reference surface that is a surface having the highest degree of similarity between the visible surface and the template image data corresponding to the surface. Features.
With this configuration, according to one aspect of the present invention, the position and orientation of the target object are calculated based on the template information corresponding to the visible surface obtained from the target object image. The position and orientation of the target object in the three-dimensional space can be detected with high accuracy.

[5] In order to solve the above-described problem, a position / orientation detection program according to an aspect of the present invention provides a computer with a template image data and a reference coordinate system of a surface with respect to a line-of-sight direction for each line-of-sight direction with respect to a reference of a target object. Included in image data obtained based on a predetermined viewpoint, and a template information storage section that stores template information having information indicating the position and orientation of the surface in the reference information and information indicating the position and orientation of the viewpoint in the reference coordinate system A visible surface is detected from the image of the target object, template information corresponding to the visible surface is read from the template information storage unit, and the position of the target object in the reference coordinate system based on the template information And a position / orientation data generation unit that generates position / orientation data representing the attitude; Is to function, the position and orientation data generating unit, for each template image data, the template feature data associated with the position information of the feature amount and the feature points in a plurality of feature points respectively extracted from the template image data A feature data generation unit that extracts a plurality of feature points from the image data and generates feature data in which feature amounts for each feature point are associated with positional information of the feature points And a feature amount in the feature data and a feature amount in the template feature data based on the feature data generated by the feature data generation unit and the template feature data stored in the template feature data storage unit Calculating the degree, detecting the position information of the feature point corresponding to the matching feature quantity, the position Based on the information, a first position and orientation for detecting a surface that is visible in the image of the target object and a reference surface that has the highest degree of similarity between the surface that is visible and the template image data corresponding to the surface It functions as a calculation unit.

[6] In order to solve the above-described problem, the position / orientation detection method according to one aspect of the present invention is such that the position / orientation data generation unit is visible from an image of a target object included in image data obtained based on a predetermined viewpoint. For each line-of-sight direction with respect to the reference of the target object, a template image data of the surface with respect to the line-of-sight direction, information indicating the position and orientation of the surface in the reference coordinate system, and the position and posture of the viewpoint in the reference coordinate system are detected. The template information corresponding to the surface that is visible is read from the template information storage unit that stores the template information having the information indicating the position, and the position and orientation of the target object in the reference coordinate system are represented based on the template information position and orientation detection step of generating the position and orientation data, have a, the position and orientation data generating unit, A plurality of feature points extracted from the image data, a feature data generation step for generating feature data in which the feature amount for each feature point is associated with the position information of the feature point, and the feature data generation step A template feature data storage unit that stores, for each of the feature data and template image data, template feature data in which feature quantities at each of a plurality of feature points extracted from the template image data are associated with positional information of the feature points Based on the template feature data stored in the table, the degree of correlation between the feature quantity in the feature data and the feature quantity in the template feature data is calculated, and the position information of the feature point corresponding to the matching feature quantity is detected. Then, based on the position information, the surface that is visible in the image of the target object and the possible Characterized by chromatic between a first position and orientation calculation step of similarity between the template image data corresponding to said surface to detect the reference plane is the highest plane, the one surface is.

  Therefore, according to each aspect of the present invention, the position and orientation of the target object represented by the planar image in the three-dimensional space can be detected with high accuracy.

1 is a schematic external view of a robot and a target object in an inspection system to which a robot apparatus according to a first embodiment of the present invention is applied. It is a block diagram showing the schematic function structure of the inspection system to which the robot apparatus which is the embodiment is applied. It is a block diagram showing the functional composition of the position and orientation detection device in the embodiment. It is a conceptual diagram showing the template position and orientation data for one surface. It is a figure showing the data structure of a correspondence table. It is a flowchart showing the procedure of the external appearance inspection process of the inspection system in the embodiment. 5 is a flowchart illustrating a procedure of detection processing of a position and orientation of a target object executed by a position and orientation detection apparatus in the embodiment. 5 is a flowchart illustrating a procedure of detection processing of a position and orientation of a target object executed by a position and orientation detection apparatus in the embodiment. 5 is a flowchart illustrating a procedure of detection processing of a position and orientation of a target object executed by a position and orientation detection apparatus in the embodiment. In the same embodiment, it is the figure which represented typically the one part content of the process by a position and orientation calculation part. It is a block diagram showing the schematic function structure of the inspection system to which the robot apparatus which is 2nd Embodiment of this invention is applied. It is a block diagram showing the functional composition of the position and orientation detection device in the embodiment. 5 is a flowchart illustrating a procedure of detection processing of a position and orientation of a target object executed by a position and orientation detection apparatus in the embodiment. 5 is a flowchart illustrating a procedure of detection processing of a position and orientation of a target object executed by a position and orientation detection apparatus in the embodiment.

Hereinafter, embodiments for carrying out the present invention will be described in detail with reference to the drawings.
[First Embodiment]
FIG. 1 is a schematic external view of a robot and a target object in an inspection system to which a robot apparatus according to a first embodiment of the present invention is applied. As shown in FIG. 1, the robot 10 is configured by providing an imaging device (imaging unit) 11 on a robot body 12.

  The robot body 12 supports the imaging device 11 in a movable manner. Specifically, the robot body 12 includes a support base 12a fixed to the ground, an arm part 12b connected to the support base 12a so as to be able to turn and bend, and an arm part 12b that can be pivoted and swingable. And a connected hand portion 12c. The robot body 12 is, for example, a 6-axis vertical articulated robot. The robot body 12 has a 6-axis degree of freedom by a coordinated operation of the support base 12a, the arm part 12b, and the hand part 12c. It can be changed freely in the three-dimensional space.

  Note that the robot body 12 may be configured to change the imaging device 11, tools, components, and the like according to the purpose of work. Further, the degree of freedom of the robot body 12 is not limited to six axes. Specifically, for example, the robot body 12 may have a degree of freedom by seven axes. Moreover, you may install the support stand 12a in the place fixed with respect to the grounds, such as a wall and a ceiling. The robot body 12 includes an arm portion and a hand portion (not shown) that support tools and parts in addition to the arm portion 12b and the hand portion 12c that support the imaging device 11, and the plurality of arm portions and hand portions are independent of each other. It is good also as a structure which moves in conjunction or linked.

As shown in FIG. 1, a target object 5 that is an inspection target object for visual inspection is placed on a table (not shown) within the movable range of the tip of the hand portion 12c.
The inspection system in the present embodiment is an apparatus that observes the appearance of the target object 5 and inspects the state of the inspection site.

FIG. 2 is a block diagram illustrating a schematic functional configuration of an inspection system to which the robot apparatus according to the present embodiment is applied. As shown in FIG. 2, the inspection system 1 includes a robot 10, a position / orientation detection device 20, an inspection device 30, and a robot control device 40.
As shown in FIG. 1, the robot 10 includes an imaging device 11 and a robot body 12.

The imaging device 11 is a camera device that can selectively capture still images and moving images. The imaging device 11 captures a still image of the target object 5 in accordance with a still image capturing request signal supplied from the inspection device 30, and outputs image data that is data of the still image. Further, the imaging device 11 shoots the target object 5 in a moving image at a frame rate of 30 frames / second (fps; frame per second), for example, in accordance with the moving image shooting start request signal supplied from the inspection device 30. Image data that is data is output continuously. Further, the imaging device 11 stops the moving image shooting operation in accordance with the moving image shooting stop request signal supplied from the control device 30.
As described above, the robot body 12 is a device for moving the attached imaging device 11 in the three-dimensional space.

  The position / orientation detection device 20 takes in the image data of the still image supplied from the imaging device 11, and based on the planar image (target object image) of the target object 5 included in the image data, the target object 5 in the three-dimensional space. The position and orientation are calculated, and the position and orientation data is supplied to the inspection device 30. The three-dimensional space in this embodiment is a three-dimensional space by a robot coordinate system that is a reference coordinate system. The detailed configuration of the position / orientation detection apparatus 20 will be described later.

  The robot apparatus according to the present embodiment includes a robot 10 and a position / orientation detection apparatus 20.

When acquiring the position and orientation data from the position and orientation detection device 20, the inspection device 30 transmits a still image capturing request signal to the imaging device 11. In addition, the inspection device 30 transmits a moving image shooting start request signal and a moving image shooting stop request signal to the imaging device 11 in the execution of the appearance inspection process.
The inspection device 30 takes in the position and orientation data supplied from the position and orientation detection device 20, and manages the position and orientation of the target object 5 in the three-dimensional space of the robot coordinate system. Further, the inspection apparatus 30 takes in the image data of the moving image supplied from the imaging apparatus 11 sequentially or every several frames, and generates a robot control command for controlling the posture of the robot body 12 according to a known target tracking process. Then, this robot control command is supplied to the robot controller 40.
The inspection process 30 performs an appearance inspection process on the target object image included in the image data. The appearance inspection process is, for example, a process of detecting an abnormality in appearance by a template matching process that is a known technique.

  The robot control device 40 takes in the robot control command supplied from the inspection device 30 and controls the posture of the robot body 12, that is, the position and posture of the imaging device 11 based on the robot control command.

FIG. 3 is a block diagram illustrating a functional configuration of the position / orientation detection apparatus 20. As illustrated in FIG. 3, the position / orientation detection apparatus 20 includes an image data acquisition unit 201, an image data storage unit 202, a template information storage unit 203, a matching degree evaluation unit 204, a correspondence table storage unit 205, And an attitude calculation unit 206.
The template information storage unit 203, the matching score evaluation unit 204, the correspondence table storage unit 205, and the position / orientation calculation unit 206 are position / orientation data generation units.

The image data acquisition unit 201 takes in image data of a still image supplied from the imaging device 11 of the robot 10 and stores it in the image data storage unit 202.
The image data storage unit 202 stores the image data captured by the image data acquisition unit 201.

  The template information storage unit 203 stores template information about the reference of the target object 5 in advance. The reference is, for example, a non-defective sample of the target object 5 and is in a state where there are no abnormalities such as scratches or missing parts in its appearance. The template information includes a plurality of template image data obtained by the imaging device 11 imaging a reference from each of a plurality of line-of-sight directions, and template position and orientation data for each of the plurality of template images. The plurality of line-of-sight directions are, for example, directions orthogonal to each other and may be six directions orthogonal to each other, or may be five directions excluding the vertical downward direction when the reference is placed on a horizontal surface in a state of normal use. . By removing the vertically lower part, the template image on the bottom side of the reference can be omitted. The template position and orientation data includes information representing the position and orientation of the surface, information representing the relative position and orientation of the reference reference point (origin) and the surface, information representing the position and orientation of the imaging device 11, Information representing the relative position and orientation of the imaging device 11 and the surface.

  Note that the information indicating the relative position and orientation between the imaging device 11 and the surface is not stored in advance. For example, the coincidence evaluation unit 204 determines that the information indicating the position and orientation of the surface and the imaging device 11 You may make it obtain | require by calculating from the information showing a position and an attitude | position. Similarly, information indicating the relative position and orientation between the reference reference point (origin) and the surface may be calculated and obtained.

The template position / orientation data will be described in detail.
FIG. 4 is a conceptual diagram showing template position and orientation data for one surface. FIG. 4 shows a case where the reference of the target object 5 is imaged from one direction (upper side) of six directions orthogonal to each other.
In FIG. 4, Σ represents a robot coordinate system (XYZ coordinate system) which is a reference coordinate system. P ₁ indicates a first surface corresponding to the first template image. Sigma _C1 is the case of capturing a first surface _{P 1,} representing the camera coordinate system of the imaging device 11 _{(X C1 Y C1 Z C1} coordinate system). Origin O (O) _C1 of the camera coordinate system sigma _C1 is that penetrated to the optical axis of the imaging device 11 for imaging the first plane P _1. Σ _Obj represents an object coordinate system (X _Obj Y _Obj Z _Obj coordinate system) provided for the reference. For example, _ΣObj is provided by making the origin O (O) _Obj coincide with one corner (vertex) of the reference. Sigma _S1 represents the first surface _{P 1} origin O _{(O) S1} coordinate system provided to include a _{(X S1 Y S1 Z S1} coordinate system).

Further, in FIG. 4, t _{(bold) S1} and R _{(bold) S1} is a translation vector and the rotation matrix from the origin O of the robot coordinate system sigma with respect to the origin _{O S1} of the first surface _{P 1} of the coordinate system sigma _S1 It is. Note that the description “(bold)” indicates that the character immediately before it is expressed in bold, and that the character is a vector or a matrix. Delta] t _{(bold) S1Obj} and [Delta] R _{(bold) S1Obj} is a translation vector and the rotation matrix with respect to the origin _{O Obj} of the object coordinate system sigma _Obj from the origin _{O S1} of the coordinate system sigma _S1. t _{(bold) C1} and R _{(bold) C1} is a translation vector and the rotation matrix from the origin O of the robot coordinate system sigma with respect to the origin _{O C1} of the camera coordinate system _{sigma C1.} Delta] t _{(bold) C1S1} and [Delta] R _{(bold) C1S1} is a translation vector and the rotation matrix from the origin _{O C1} of the camera coordinate system _{sigma C1} with respect to the origin _{O S1} of the coordinate system sigma _S1.

For example, when storing template information for six surfaces, the template information storage unit 203 stores template image data and template position and orientation data for six surfaces. Specifically, the template information storage unit 203 stores n-th template image data corresponding to the plane P _n (the subscript n is a positive integer of 6 or less, the same applies hereinafter). Further, the template information storage unit 203 stores a translation vector t (bold body) _Sn and a rotation matrix R (bold body) _Sn as information representing the position and orientation of the surface. Further, the template information storage unit 203 stores a translation vector Δt (bold body) _SnObj and a rotation matrix ΔR (bold body) _SnObj as information representing the relative position and orientation of the reference reference point (origin) and the surface. To do. The template information storage unit 203 stores a translation vector t (bold body) _Cn and a rotation matrix R (bold body) _Cn as information representing the position and orientation of the imaging device 11. The template information storage unit 203 stores a translation vector Δt (bold body) _CnSn and a rotation matrix ΔR (bold body) _CnSn as information representing the relative position and orientation between the imaging device 11 and the surface.

Returning to the description of FIG. 3, the matching degree evaluation unit 204 and the image data stored in the image data storage unit 202 and all the template image data stored in the template information storage unit 203 (for example, the first surface P _1). To 6th plane image P6) to _6th plane P6) for each template image data, conversion that is an image obtained by changing the line-of-sight direction in the template image data to the line-of-sight direction in the image data Image data is generated, the degree of coincidence between the image data and the converted image data is calculated and evaluated according to a predetermined criterion. The converted image data is obtained when the matching degree evaluation unit 204 performs a calculation process of multiplying the template image data by the homography matrix. The degree of coincidence is obtained when the coincidence degree evaluation unit 204 performs image matching processing (pattern matching processing) between the image data and the converted image data. The degree of coincidence is, for example, the sum of absolute values of luminance differences between image data and converted image data or the square sum of luminance differences.

The coincidence degree evaluation unit 204 outputs an evaluation result that the converted image data is valid when the coincidence degree exceeds the threshold value, and that the converted image data is invalid when the coincidence degree is equal to or less than the threshold value. Output the evaluation results. Valid and invalid indicate, for example, whether the surface is visible or invisible. For example, the coincidence degree evaluation unit 204 represents the coincidence degree evaluation result by the state of the valid flag. Specifically, the coincidence degree evaluation unit 204 represents, for example, an evaluation result that the converted image data is valid by an effective flag = “ON” (“1”), and an evaluation that the converted image data is invalid. The result is represented by a valid flag = “OFF” (“0”).
The coincidence degree evaluation unit 204 generates a correspondence table storing coincidence degree evaluation results for all template image data and various related information described later, and stores the correspondence table in the correspondence table storage unit 205.

Here, the homography matrix will be described. The coordinate system of the imaging device in the three-dimensional space is F ^*, and the image of the point A in the image obtained by imaging the arbitrary point A by this imaging device is p (bold body) ^* = [u ^* v ^* 1] ^T To do. An image of the point A in an image obtained by moving the image pickup device and imaging the point A by the image pickup device at the movement destination is F (bold body) = [u v 1] ^T. Also, a translation vector representing the relative distance between F ^* and F is t (bold body), and a rotation matrix representing the posture change is R (bold body).

Consider a case where the following expression (1) is established as an expression representing the relationship between the point p (bold body) ^* and the point p (bold body) when the point A exists on the plane π. However, s is a value determined by the ratio of the distance between the point A and the two coordinate systems F ^* and F. G (bold) is a homography matrix.

  The homography matrix G (bold body) is a matrix of 3 rows and 3 columns, and is represented by the following formula (2).

  In addition, the homography matrix G (bold body) can be expressed as the following equation (3). Here, d is a distance between the imaging device and the plane π, and n (bold body) is a normal vector of the plane π.

  If the homography matrix G (bold body) can be estimated, the translation vector t (bold body), the rotation matrix R (bold body), the normal vector n (bold body) of the plane π, and the imaging device and the plane π The distance d between can be calculated.

  A set of coordinates of projection points on the captured image of each point at all points existing on the plane π is expressed as follows using Expression (1). First, the value s is defined as the following formula (4).

The following equation (5) is established by the equations (1) and (4). However, w (bold body) is a function of the homography matrix G (bold body) and is a perspective projection transformation matrix. Converting the point p (bold body) ^* to the corresponding point p (bold body) using this perspective projection transformation matrix is called perspective projection transformation.

According to Equation (5), for a point existing on the plane π, if the homography matrix of the plane π is known, a point on the other captured image corresponds to a point on the other captured image. Can be obtained uniquely.
Therefore, by obtaining the homography matrix, it is possible to obtain how much the image of interest is translated and how much it is rotated with respect to the original image, in other words, the region of interest can be tracked.

Next, the correspondence table generated by the matching score evaluation unit 204 will be described.
FIG. 5 is a diagram illustrating a data configuration of the correspondence table. As shown in FIG. 5, the correspondence table is a data table in which the items “template image number”, “valid flag”, “matching start position”, and “homography matrix element” are associated with each other. The “template image number” is an item of information for identifying template image data stored in the template information storage unit 203. The “valid flag” is an item of the evaluation result of coincidence. “ON” (“1”) is an evaluation result that the converted image data is valid, and “OFF” (“0”) is an evaluation result that the converted image data is invalid. The “matching start position” is an item of the image matching start position on the image data when the coincidence evaluation unit 204 evaluates the coincidence. This start position is specifically a coordinate value, for example, the coordinate value of image data corresponding to the position of the upper left end of the converted image data to be matched. The “homography matrix element” is an item of the element of the homography matrix when the coincidence evaluation unit 204 evaluates the coincidence.

  Returning to the description of FIG. 3, the correspondence table storage unit 205 stores the correspondence table newly generated or updated by the matching degree evaluation unit 204.

  The position / orientation calculation unit 206 detects one surface having the highest reliability from the effective surfaces in the image data, obtains the position and orientation of the target object 5 based on this surface, and obtains the position / orientation data. Supply to the inspection device 30. Specifically, the position / orientation calculation unit 206 reads image data from the image data storage unit 202, reads a correspondence table from the correspondence table storage unit 205, and reads template position / orientation data from the template information storage unit 203. Then, the position / orientation calculation unit 206 calculates the similarity between the image data and the converted image data for each template image data indicating the evaluation result that the converted image data is valid. Select a homography matrix. The degree of similarity is obtained by the position / orientation calculation unit 206 calculating, for example, normalized cross-correlation.

  Note that the position / orientation calculation unit 206 may obtain the similarity by calculating SSD (Sum of Squared Difference) or SAD (Sum of Absolute Difference) in addition to the normalized cross-correlation.

  The position / orientation calculation unit 206 calculates a translation vector and a rotation matrix from the origin of the coordinate system corresponding to the selected surface to the origin of the coordinate system of the converted image based on the selected homography matrix. Specifically, the position / orientation calculation unit 206 obtains a translation vector and a rotation matrix by performing singular value decomposition on the selected homography matrix. In addition, the position / orientation calculation unit 206 acquires information (translation vector and rotation matrix) on the current position and orientation of the imaging device 11 from, for example, the robot body 12 or the robot control device 40. Then, the position / orientation calculation unit 206 calculates a translation vector and a rotation matrix with respect to the origin of the coordinate system of the converted image from the origin of the robot coordinate system, and further, based on the calculated translation vector and rotation matrix, the origin of the robot coordinate system Then, a translation vector and a rotation matrix with respect to the origin of the target object 5 are calculated, and the calculation result is supplied to the inspection apparatus 30 as position and orientation data. The calculation method here will be described later.

  In the position / orientation detection apparatus 20, the image data storage unit 202, the template information storage unit 203, and the correspondence table storage unit 205 are realized by, for example, a semiconductor storage device, a magnetic hard disk device, or a combination thereof.

Next, operation | movement of the test | inspection system 1 in this embodiment is demonstrated.
FIG. 6 is a flowchart showing the procedure of the appearance inspection process of the inspection system 1. Before starting the processing of the flowchart of FIG. 6, the inspection device 30 of the inspection system 1 transmits a still image shooting request signal to the imaging device 11 in order to acquire position / orientation data from the position / orientation detection device 20. .
First, in step S1, the position / orientation detection apparatus 20 executes position and orientation detection processing of the target object, generates position and orientation data representing the position and orientation of the target object 5 in the robot coordinate system, and this position and orientation data. Is supplied to the inspection device 30. Details of the detection processing of the position and orientation of the target object will be described later.

Next, in step S <b> 2, the inspection apparatus 30 captures and stores the position / orientation data supplied from the position / orientation detection apparatus 20.
Next, for example, the inspection apparatus 30 receives an input of a shooting position and orientation of the imaging apparatus 11 by an operation by an operator, generates a robot control command having contents for specifying the position and orientation, and uses the robot control command as a robot. This is supplied to the control unit 40.
Next, the robot control unit 40 takes in the robot control command supplied from the inspection apparatus 30 and controls the operation of the robot body 12 in accordance with the robot control command. Thereby, the imaging device 11 is set to the position and posture specified by the operator.

Next, in step S <b> 3, the inspection device 30 transmits a moving image shooting start request signal to the imaging device 11 in order to execute an appearance inspection process.
Next, the inspection device 30 takes in the image data of the moving image supplied from the imaging device 11 sequentially or every several frames.
Next, in step S4, the inspection device 30 performs the appearance inspection by detecting the target object 5 from the image data by executing a template matching process.
Next, the inspection device 30 transmits a moving image shooting stop request signal to the imaging device 11. And the imaging device 11 which received the moving image shooting stop request signal stops the shooting operation.

Next, the position and orientation detection processing of the target object executed by the position and orientation detection apparatus 20 will be described.
FIG. 7 to FIG. 9 are flowcharts illustrating the procedure of the target object position and orientation detection processing executed by the position and orientation detection apparatus 20.
First, in step S <b> 101 of FIG. 7, the image data acquisition unit 201 takes in image data of a still image supplied from the imaging device 11 of the robot 10 and stores the image data in the image data storage unit 202.
Next, in step S <b> 102, the coincidence degree evaluation unit 204 reads image data from the image data storage unit 202, and reads one page of template image data from the template information storage unit 203.

Next, in step S103, the degree-of-match evaluation unit 204 sets initial values for the elements of the homography matrix. The initial value here is a value indicating a lower limit or an upper limit in each of the translation range and the rotation range.
Next, in step S104, the coincidence degree evaluation unit 204 performs projective transformation on the template image data to generate converted image data. Specifically, the degree-of-matching evaluation unit 204 performs an operation of multiplying the template image data by the homography matrix to generate converted image data.

In step S105, the coincidence degree evaluation unit 204 sets an initial value at the start position on the image data for the image matching process between the image data and the converted image data. For example, the coincidence degree evaluation unit 204 sets the upper left coordinate value of the image data as the start position.
Next, in step S106, the coincidence degree evaluation unit 204 performs image matching processing between the image data and the converted image data to calculate the coincidence degree. The coincidence degree evaluation unit 204 calculates, for example, the sum of absolute values of luminance differences or the square sum of luminance differences between the image data and the converted image data, and uses this value as the degree of coincidence.

  Next, in step S107 of FIG. 8, when the degree of coincidence exceeds the threshold (S107: YES), the process proceeds to step S108, and when the degree of coincidence is equal to or less than the threshold (S107: NO), the process proceeds to step S110. .

  Step S107: In step S108 following YES, the matching score evaluation unit 204 associates the valid flag = “ON”, the image matching start position, and the element of the homography matrix with the template image number of the template image data. And stored in the correspondence table, and this correspondence table is stored in the correspondence table storage unit 205. When the correspondence table is already stored in the correspondence table storage unit 205, the matching degree evaluation unit 204 adds data to the correspondence table.

  In step S109 following step S108 or step S112, if there is other template image data (S109: YES), the process returns to step S102, and if there is no other template image data (S109: NO), the process of step S115. Move on.

  In step S110 following step S107: NO, if the tracking of the entire image in the image data is completed (S110: YES), the process proceeds to step S111. If not completed (S110: NO), the process proceeds to step S113. Move.

  In step S111 following step S110: YES, when the elements of the homography matrix have reached the limit values in the range of rotation and the range of translation (S111: YES), the process proceeds to step S112, and the limit value has been reached. If not (S111: NO), the process proceeds to step S114.

In step S112 following step S111: YES, the matching score evaluation unit 204 stores the valid flag = “OFF” in the correspondence table in association with the template image number of the template image data, and this correspondence table is stored in the correspondence table storage unit. 205 stores the result. When the correspondence table is already stored in the correspondence table storage unit 205, the matching degree evaluation unit 204 adds data to the correspondence table.
Next, the process proceeds to step S109.

Step S110: In step S113 following NO, the coincidence degree evaluation unit 204 changes the start position on the image data for the image matching process between the image data and the converted image data. For example, the coincidence degree evaluation unit 204 shifts the start position by one pixel or a plurality of pixels in the horizontal direction and / or vertical direction of the image.
Next, the process returns to step S106.

In step S114 following step S111: NO, the coincidence degree evaluation unit 204 changes the element of the homography matrix. For example, the coincidence degree evaluation unit 204 changes the elements of the homography matrix so that the rotation component and the translation component are changed by a predetermined step within the rotation range and the translation range.
Next, the process returns to step S104.

In step S115 of FIG. 9 following step S109: NO, the position / orientation calculation unit 206 reads image data from the image data storage unit 202, reads a correspondence table from the correspondence table storage unit 205, and template information from the template information storage unit 203. Is read.
Next, the position / orientation calculation unit 206 calculates the image data, the converted image data, and the template image data indicating the evaluation result that the converted image data is valid, that is, for each template image data whose valid flag is “ON”. Calculate the similarity of. However, the converted image data is obtained by the position / orientation calculation unit 206 performing an operation of multiplying the template image data by the homography matrix. The position / orientation calculation unit 206 obtains the similarity between the image data and the converted image data by, for example, calculating a normalized cross-correlation.

  Next, in step S116, the position / orientation calculation unit 206 selects a surface and a homography matrix having the highest similarity.

Next, in step S117, the position / orientation calculation unit 206 calculates a translation vector and a rotation matrix from the origin of the coordinate system corresponding to the selected surface to the origin of the coordinate system of the converted image based on the selected homography matrix. To do. Specifically, the position / orientation calculation unit 206 obtains a translation vector and a rotation matrix by performing singular value decomposition on the homography matrix.
Next, the position / orientation calculation unit 206 acquires information on the current position and orientation of the imaging device 11 (translation vector and rotation matrix) from the robot body 12 or the robot control device 40, for example.

  In step S118, the position / orientation calculation unit 206 calculates a translation vector and a rotation matrix from the origin of the robot coordinate system to the origin of the coordinate system of the converted image, and further, based on the calculated translation vector and rotation matrix. Then, a translation vector and a rotation matrix with respect to the reference point (origin) of the target object 5 are calculated from the origin of the robot coordinate system, and the calculation result is supplied to the inspection apparatus 30 as position and orientation data.

FIG. 10 is a diagram schematically showing the contents of the processing of step S117 and step S118 by the position / orientation calculation unit 206. Figure 10 is a diagram showing a case surface to be the highest degree of similarity selected by the position and orientation calculation unit 206 is the first surface P _1. In FIG. 10, Δt (bold body) _S1 and ΔR (bold body) _S1 are first surfaces P ₁ obtained by performing singular value decomposition of the homography matrix by the position and orientation calculation unit 206 in the process of step S117. _Is a translation vector and a rotation matrix from the origin of the coordinate system to the origin O _S1 of the coordinate system Σ _S1 of the transformed image. t (bold) _C and R (bold) _C is a translation vector and the rotation matrix with respect to the origin O (O) _C of the current camera coordinate system sigma _C from the origin O of the robot coordinate system sigma. t _{(bold) S1} and R _{(bold) S1} is a translation vector and the rotation matrix with respect to the origin _{O S1} of the coordinate system sigma _S1 converted image from the origin O of the robot coordinate system sigma. Δt (bold body) _C1S1 and ΔR (bold body) _C1S1 are a translation vector and a rotation matrix from the origin O _C1 of the camera coordinate system Σ _{C1 to} the origin of the coordinate system Σ of the first plane P ₁ when the template image is acquired. It is. t _{(bold) Obj} and R _{(bold) Obj} is a translation vector and the rotation matrix with respect to the origin _{O Obj} of the coordinate system sigma _Obj of the object 5 from the origin O of the robot coordinate system sigma, the position in the process of step S118 This is the position and orientation data calculated by the orientation calculation unit 206.

As shown in FIG. 10, the position / orientation calculation unit 206 includes a translation vector t (bold body) _Sm and a rotation matrix R (bold body) _Sm (where the subscript m indicates the surface with the highest degree of similarity. The same applies hereinafter). ) Is calculated by the following equation (6).

Next, the position / orientation calculation unit 206 calculates t (bold body) _Obj and R (bold body) _Obj by the following equation (7).

  As described above in detail, in the first embodiment of the present invention, the position / orientation detection device 20 captures the template image data obtained by imaging the reference of the target object 5 for each line-of-sight direction from the imaging device 11, and the surface. Information indicating the relative position and orientation of the reference reference point (origin) and the surface, information indicating the position and orientation of the imaging device 11, and the imaging device 11 and the surface. Information indicating relative position and posture was retained. The position / orientation detection device 20 detects an effective surface (for example, a visible surface) from the image data based on the image data including the planar image of the target object 5 supplied from the imaging device 11 and all the template image data. Further, the most reliable surface is detected from the effective surfaces, the position and orientation of the target object are obtained based on this surface, and supplied to the inspection apparatus 30 as position and orientation data. The inspection apparatus 30 takes in the position and orientation data supplied from the position and orientation detection apparatus 20 and manages the position and orientation of the target object 5 in the three-dimensional space of the robot coordinate system.

With this configuration, according to the present embodiment, in order to obtain the position and orientation of the target object based on the most reliable surface among the visible surfaces, the three-dimensional space of the target object expressed by the planar image The position and orientation at can be detected with high accuracy.
In addition, according to the present embodiment, since the position and orientation are obtained using only one representative surface of the visible surfaces, the calculation load is light.

[Second Embodiment]
In the second embodiment of the present invention, the position / orientation detection apparatus 20 in the first embodiment described above is changed to another form.
FIG. 11 is a block diagram showing a schematic functional configuration of an inspection system to which the robot apparatus according to the second embodiment of the present invention is applied. As shown in FIG. 11, the inspection system 1a has a configuration in which the position / orientation detection device 20 of the inspection system 1 in the first embodiment is changed to a position / orientation detection device 20a. Therefore, in the present embodiment, the position / orientation detection apparatus 20a having a configuration different from that of the first embodiment will be specifically described, and description of other configurations that are common to the first embodiment will be omitted.

  The robot apparatus according to the present embodiment includes a robot 10 and a position / orientation detection apparatus 20a.

  The position / orientation detection device 20a detects the position and orientation of each surface of the target object 5 from the image data acquired from the imaging device 11 through matching processing based on the feature amount of the image, and based on the detection result, the current imaging device. All surfaces (visible surfaces) that are visible from 11 viewpoints (current viewpoints) are detected. Then, the position / orientation detection apparatus 20a selects a surface having the highest similarity with the template image from all visible surfaces (the first position / orientation detection process). Then, the position / orientation detection apparatus 20a generates composite model image data using all visible surfaces, and evaluates and calculates the position and orientation of the target object 5 in the three-dimensional space based on the composite model image data ( Second position and orientation detection process), and supplies the position and orientation data to the inspection apparatus 30.

FIG. 12 is a block diagram illustrating a functional configuration of the position / orientation detection apparatus 20a. As shown in FIG. 12, the position / orientation detection apparatus 20a includes an image data acquisition unit 211, an image data storage unit 212, a feature data generation unit 213, a template feature data storage unit 214, and a first position / orientation calculation unit 215. A template information storage unit 216, a combined model image generation unit 217, and a second position and orientation calculation unit 218.
The feature data generation unit 213, the template feature data storage unit 214, the first position / orientation calculation unit 215, the template information storage unit 216, the synthesized model image generation unit 217, and the second position / orientation calculation unit 218 The position and orientation data generation unit.

The image data acquisition unit 211 captures image data of a still image supplied from the imaging device 11 of the robot 10 and stores it in the image data storage unit 212.
The image data storage unit 212 stores the image data captured by the image data acquisition unit 211.

  The feature data generation unit 213 reads image data from the image data storage unit 212, extracts a plurality of feature points from the image data, and features data that associates feature amounts at the feature points with positional information of the feature points. And the feature data is supplied to the first position and orientation calculation unit 215. A feature point is a point at which, for example, a luminance value or a color changes suddenly, and corresponds to, for example, a vertex or an intersection of patterns. The feature amount is a feature amount of the image. For example, the feature data generation unit 213 performs a well-known SIFT (Scale-Invariant Feature Transform) process for examining the state of a Gaussian distribution of luminance for each small region including a plurality of pixels and extracting a feature amount, thereby performing a SIFT feature amount. And the SIFT feature value is used as the feature value. Moreover, the feature data generation unit 213 may apply SURF (Speed-Up Robust Features). The position information of the feature point is a coordinate value or a position vector of the feature point obtained using, for example, the position of the upper left end of the image data as the origin.

  The template feature data storage unit 214 stores template feature data for the reference of the target object 5. The template feature data is data in which a feature amount at each of a plurality of feature points extracted from reference template image data is associated with position information of the feature points. The template feature data storage unit 214 stores, for each line of sight, template feature data corresponding to each of a plurality of lines of sight from around the reference in the three-dimensional space.

  The first position / orientation calculation unit 215 executes a first position / orientation detection process. That is, the first position / orientation calculation unit 215 takes in the feature data supplied from the feature data generation unit 213 and reads the template feature data from the template feature data storage unit 214. Then, the first position / orientation calculation unit 215 executes a matching process between the feature amount in the feature data and the feature amount in the template feature data, and extracts the position information of the feature point corresponding to the matching feature amount. Then, the first position / orientation calculation unit 215 detects the position and orientation of each surface of the target object 5 based on the position information. Specifically, the first position / orientation calculation unit 215 performs homography conversion on the template image so that at least four feature points belonging to the same plane match the plane of the current image, and performs matching processing. The position and orientation of each surface of the target object 5 are calculated by singular value decomposition of the homography matrix. Then, the first position / orientation calculation unit 215 detects all visible surfaces from the current viewpoint based on the calculation result. The first position / orientation calculation unit 215 selects the position and orientation of the surface (reference surface) having the highest similarity between the image of the visible surface and the template image from all the visible surfaces. This reference surface is, for example, a surface with less distortion and inclination with respect to the template image at the current viewpoint.

  The template information storage unit 216 stores template information about the reference of the target object 5 in advance. As in the first embodiment, the template information includes a plurality of template image data obtained by imaging a reference from each of a plurality of line-of-sight directions (for example, six directions orthogonal to each other), and template positions for each of the plurality of template images. Attitude data. That is, the template position / orientation data includes information representing the position and orientation of the surface, information representing the relative position and orientation of the reference reference point (origin) and the surface, and information representing the position and orientation of the imaging device 11. And information representing the relative position and orientation of the imaging device 11 and the surface. Furthermore, in the present embodiment, the template information includes template position / orientation relationship data representing the relationship between the positions and orientations of a plurality of template images corresponding to a plurality of surfaces.

  Note that information representing the relative position and orientation between the imaging device 11 and the surface is not stored in advance. For example, the composite model image generation unit 217 may include information representing the position and orientation of the surface and the imaging device 11. It may be obtained by calculating from information indicating the position and orientation of Similarly, information indicating the relative position and orientation between the reference reference point (origin) and the surface may be calculated and obtained.

The template position / orientation relationship data is a relative translation vector and rotation matrix of all template images. Specifically, with respect to the template image data for six surfaces, if the translation vector and the rotation matrix from the i-th template image to the j-th template image are (t (bold body) _ij , R (bold body) _ij ), The template position / orientation relationship data is, for example, (t (bold body) ₁₂ , R (bold body) ₁₂ ), (t (bold body) ₁₃ , R (bold body) ₁₃ ), (t (bold body) ₁₄ , R (Bold body) ₁₄ ), (t (bold body) ₁₅ , R (bold body) ₁₅ ), (t (bold body) ₁₆ , R (bold body) ₁₆ ), (t (bold body) ₂₃ , R (bold body) _body) 23), (t _(bold) 24, R _(bold) 24), (t _(bold) 25, R _(bold) 25), (t _{(bold) 26} R _{(bold) 26),} (t _(bold) 34, R _{(bold) 34),} (t _(bold) 35, R _{(bold) 35),} (t _(bold) 36, R ( Bold body) ₃₆ ), (t (bold body) ₄₅ , R (bold body) ₄₅ ), (t (bold body) ₄₆ , R (bold body) ₄₆ ), and (t (bold body) ₅₆ , R (bold body) Body) ₅₆ ).

The synthesized model image generation unit 217 and the second position / orientation calculation unit 218 execute a second position / orientation detection process.
First, the synthesized model image generation unit 217 takes in the position information of feature points corresponding to all visible surfaces and the positions and orientations of the reference surfaces from the first position / orientation calculation unit 215, and receives template information from the template information storage unit 216. Is read. Then, the composite model image generation unit 217 generates composite model image data that is visible at the current viewpoint based on the captured position information of the feature points, the position and orientation of the reference plane, and the template information. The data is supplied to the second position / orientation calculation unit 218.

In addition, the synthesized model image generation unit 217 takes in the similarity supplied from the second position / orientation calculation unit 218, and if the similarity is equal to or less than a threshold value, the synthesized model image is generated by slightly changing the position and orientation of the reference plane. Model image data is generated, and this combined model image data is supplied to the second position and orientation calculation unit 218.
In addition, when the similarity supplied from the second rank posture calculation unit 218 is a value exceeding the threshold, the composite model image generation unit 217 displays status information indicating completion of fine adjustment as the second position posture calculation unit 218. To supply.

  The second position / orientation calculator 218 evaluates and obtains the position and orientation of the target object 5 based on the image data and the synthesized model image data, and supplies the obtained position and orientation as position / orientation data to the inspection apparatus 30. To do. Specifically, the second position / orientation calculation unit 218 receives the composite model image data supplied from the composite model image generation unit 217 and reads the image data from the image data storage unit 212. Then, the second position and orientation calculation unit 218 calculates the similarity between the converted combined model image data obtained by projective conversion of the combined model image data and the image data, and supplies the similarity to the combined model image generation unit 217. The similarity is obtained by the second position / orientation calculation unit 218 calculating, for example, normalized cross-correlation.

  Note that the second position and orientation calculation unit 218 may calculate the similarity by calculating SSD (Sum of Squared Difference), SAD (Sum of Absolute Difference), or the like in addition to the normalized cross-correlation.

  In addition, when the second position / orientation calculation unit 218 fetches the status information supplied from the composite model image generation unit 217, the second position / orientation calculation unit 218 obtains the current position and orientation information (translation vector and rotation matrix) of the imaging device 11, for example, the robot body 12 or from the robot controller 40. Then, the second position / orientation calculation unit 218 calculates a translation vector and a rotation matrix from the origin of the robot coordinate system to the origin of the coordinate system of the transformed composite model image, and supplies the calculation result to the inspection apparatus 30 as position / orientation data. To do.

  In the position / orientation detection apparatus 20a, the image data storage unit 212, the template feature data storage unit 214, and the template information storage unit 216 are realized by, for example, a semiconductor storage device, a magnetic hard disk device, or a combination thereof.

Next, the position and orientation detection processing of the target object executed by the position and orientation detection apparatus 20a will be described.
FIG. 13 and FIG. 14 are flowcharts illustrating a procedure of target object position and orientation detection processing executed by the position and orientation detection apparatus 20a.
First, in step S <b> 201 of FIG. 13, the image data acquisition unit 211 takes in image data of a still image supplied from the imaging device 11 of the robot 10 and stores this image data in the image data storage unit 212.
Next, in step S202, the feature data generation unit 213 reads the image data from the image data storage unit 212, extracts a plurality of feature points from the image data, and calculates feature amounts (for example, SIFT feature amounts) at each feature point. Feature data in which the position information (position vector) of the feature point is associated is generated, and the feature data is supplied to the first position and orientation calculation unit 215.

Next, in step S <b> 203, the first position / orientation calculation unit 215 takes in the feature data supplied from the feature data generation unit 213 and reads the template feature data from the template feature data storage unit 214.
Next, the first position and orientation calculation unit 215 executes a matching process between the feature amount in the feature data and the feature amount in the template feature data, and extracts the position information of the feature point corresponding to the matching feature amount. In this matching process, the first position / orientation calculation unit 215 calculates the degree of correlation. For example, the first position / orientation calculation unit 215 calculates the Euclidean distance between the feature amount A in the feature data and the feature amount B in the template feature data, and matches the feature amount A with which the Euclidean distance is equal to or less than a threshold. Detect as.
Next, the first position / orientation calculation unit 215 detects the position and orientation of each surface of the target object 5 based on the extracted position information.
Next, the first position / orientation calculation unit 215 detects all visible surfaces from the current viewpoint based on the detection result, and the surface having the highest degree of similarity with the template image among all the visible surfaces, that is, the reference surface Select the position and posture.

Next, in step S <b> 204, the composite model image generation unit 217 takes in the position information of the feature points supplied from the first position / orientation calculation unit 215.
Next, the synthesized model image generation unit 217 selects one surface that is visible in the model obtained from the position information of the feature points, and the template information storage unit stores the template image data and the template position / orientation data corresponding to the selected surface. Read from H.216.

  Next, in step S205, the composite model image generation unit 217 calculates a homography matrix for projective transformation from the read template image data to the one surface. Specifically, the synthesized model image generation unit 217 calculates a homography matrix based on the position and orientation of the template image in the robot coordinate system and the position information of one surface.

  Next, in step S206, the composite model image generation unit 217 performs projective conversion on the template image data to generate converted image data, and stores the converted image data. Specifically, the composite model image generation unit 217 performs an operation of multiplying the template image data by the homography matrix to generate converted image data, and stores the converted image data in an internal storage area.

  Next, in step S207, if there is another surface that is visible (S207: YES), the process proceeds to step S204. If there is no other surface that is visible (S207: NO), the process proceeds to step S208. .

Step S207: In step S208 following YES, the composite model image generation unit 217 reads the converted image data from the internal storage area, and reads the template position / orientation relationship data from the template information storage unit 216.
Next, the synthesized model image generation unit 217 generates synthesized model image data by synthesizing the converted image data based on the template position / orientation relationship data, and supplies the synthesized model image data to the second position / orientation calculation unit 218. .

Next, in step S209 of FIG. 14, the second position / orientation calculation unit 218 takes in the synthesized model image data supplied from the synthesized model image generation unit 217 and reads the image data from the image data storage unit 212.
Next, the second position / orientation calculation unit 218 sets initial values for the elements of the homography matrix. The initial value here is determined based on the position and orientation of the reference plane acquired by the first position and orientation calculation unit 215 by the matching processing in the processing of step S203 described above.

  Next, in step S210, the second position / orientation calculation unit 218 performs projective transformation on the combined model image data to generate converted model image data. Specifically, the second position / orientation calculation unit 218 performs an operation of multiplying the synthesized model image data by the homography matrix to generate converted model image data.

  Next, in step S211, the second position / orientation calculation unit 218 acquires the similarity between the image data and the converted combined model image data, for example, by calculating a normalized cross-correlation, and the similarity is acquired by the combined model image. This is supplied to the generation unit 217.

Next, in step S <b> 212, the composite model image generation unit 217 captures the similarity supplied from the second position / orientation calculation unit 218. If the similarity exceeds the threshold (S212: YES), the composite model image generation unit 217 supplies status information indicating completion of fine adjustment to the second position / orientation calculation unit 218, and then performs the process of step S213. Move.
On the other hand, when the similarity is equal to or less than the threshold (S212: NO), the process proceeds to step S215.

  In step S213 following step S212: YES, when the second position / orientation calculation unit 218 fetches the status information supplied from the synthesized model image generation unit 217, information on the current position and orientation of the imaging device 11 (translation vector and Rotation matrix) is acquired from, for example, the robot body 12 or the robot controller 40.

  Next, in step S214, the second position / orientation calculation unit 218 calculates a translation vector and a rotation matrix from the origin of the robot coordinate system to the origin of the coordinate system of the transformed composite model image using the above equation (6). Further, based on the calculated translation vector and rotation matrix, a translation vector and a rotation matrix from the origin of the robot coordinate system to the reference point (origin) of the target object 5 are calculated using the equation (7). The calculation result is supplied to the inspection apparatus 30 as position and orientation data. And the process of this flowchart is complete | finished.

  Step S212: In step S215 following NO, the composite model image generation unit 217 generates composite model image data in a state in which the position and orientation of the reference plane are slightly changed, and this composite model image data is subjected to second position and orientation calculation. To the unit 218. Steps S209, S210, S211, S212: NO, S215, S210,..., S212: YES, a convergence method based on a known ESM (Efficient Second-Order Minimization Method) can be applied.

  As described above in detail, in the second embodiment of the present invention, the position / orientation detection device 20a determines the position and orientation of the target object 5 by performing matching processing based on the feature amount of the image data acquired from the imaging device 11. Then, based on the detection result, synthetic model image data using all visible surfaces from the current viewpoint is generated. The position / orientation detection apparatus 20a calculates the position and orientation of the target object 5 in the three-dimensional space based on the synthesized model image data, and supplies the calculation result to the inspection apparatus 30 as position / orientation data. The inspection apparatus 30 takes in the position and orientation data supplied from the position and orientation detection apparatus 20a, and manages the position and orientation of the target object 5 in the robot coordinate system.

  With this configuration, according to the present embodiment, since the position and orientation of the target object are obtained using all visible surfaces from the current viewpoint, the target object is more accurately compared to the first embodiment. The position and posture in the three-dimensional space can be detected.

  As described above, according to the first embodiment and the second embodiment, based on one or a plurality of template image data and template position / orientation data corresponding to a surface that is seen as a planar image of a target object obtained by imaging. Since the position and orientation of the target object are calculated, the position and orientation of the target object represented by the planar image in the three-dimensional space can be detected with high accuracy.

  As another embodiment based on the first embodiment and the second embodiment, the position / orientation detection apparatus includes both configurations of the position / orientation detection apparatus 20 and the position / orientation detection apparatus 20a, and the first position / orientation calculation unit 215. The function of the position / orientation detection apparatus 20 and the function of the position / orientation detection apparatus 20a may be switched according to the similarity calculated by the above. Specifically, the position / orientation detection apparatus according to another embodiment includes, for example, the similarity between the image of the visible surface calculated by the first position / orientation calculation unit 215 of the position / orientation detection apparatus 20a for each visible surface and the template image. If all of the above are below the threshold, in other words, if the similarity corresponding to the reference plane is below the threshold, the processing is switched to the configuration on the position and orientation detection device 20a side.

  By configuring in this way, when the degree of distortion and deformation of the reference surface is relatively low, processing based on one surface is performed, while when the degree of distortion and deformation of the reference surface is relatively high, the visible surface Processing based on all can be performed. That is, according to the state of the visible surface (for example, distortion or deformation), it is possible to switch between the detection processing of the position and orientation of the target object 5 based on one surface and the detection processing based on all the visible surfaces, and the detection accuracy based on the viewpoint The variation of can be suppressed.

  In the first embodiment and the second embodiment, the robot 10 is configured such that the robot body 12 supports the imaging device 11 movably, and the target object 5 and the reference are fixedly installed. In addition to this, for example, the robot 10 may be configured such that the robot body 12 movably supports either the target object 5 or the reference, and the imaging device 11 may be fixedly installed.

In addition to the imaging device 11 (first imaging device), another imaging device (second imaging device) that captures an image including the target object 5 in the field of view may be fixedly installed. In this case, the imaging device 11 is applied only to the process of performing an appearance inspection by changing the relative position and orientation of the imaging device 11 and the target object 5, and another imaging device detects the position and orientation of the target object 5. Applies only to processes that The position and orientation detection device 20,20a is the translation vector t _{(bold) C} and the rotation matrix R _(bold) from the origin O of the robot coordinate system sigma with respect to the origin _{O C} current camera coordinate system sigma _{C C} beforehand Calculate and remember.
With this configuration, it is not necessary to calculate the translation vector t (bold body) _C and the rotation matrix R (bold body) _C each time when calculating the above equation (6).

  In the first embodiment and the second embodiment, the template image data is an example of captured image data obtained by imaging a reference, for example. However, other than this, CAD (Computer Aided Design) data or CG (Computer Graphics) data can also be used. The format of CAD data or CG data is not limited, and it is sufficient that the shape and pattern are visible as in the case of a template image obtained by imaging.

Further, even if the camera coordinate system sigma _C matches the robot coordinate system sigma, first and second embodiments can be applied.

  Moreover, you may make it implement | achieve a part of function of the position and orientation detection apparatuses 20 and 20a in 1st Embodiment mentioned above and 2nd Embodiment with a computer. In this case, the position / orientation detection program for realizing the control function is recorded on a computer-readable recording medium, and the position / orientation detection program recorded on the recording medium is read by the computer system and executed. May be. Note that the “computer system” here includes an OS (Operating System) and hardware of peripheral devices. The “computer-readable recording medium” refers to a portable recording medium such as a flexible disk, a magneto-optical disk, an optical disk, and a memory card, and a storage device such as a magnetic hard disk built in the computer system. Furthermore, a “computer-readable recording medium” dynamically holds a program for a short time, like a communication line when transmitting a program via a network such as the Internet or a communication line such as a telephone line. In this case, it is possible to include a server that holds a program for a certain period of time, such as a volatile memory inside a computer system that serves as a server device or a client. In addition, the above program may be for realizing a part of the above-described functions, and further, may be realized by combining the above-described functions with a program already recorded in the computer system. .

  As mentioned above, although embodiment of this invention was explained in full detail with reference to drawings, the specific structure is not restricted to that embodiment, The design of the range which does not deviate from the summary of this invention, etc. are included.

1, 1a Inspection system 5 Target object 10 Robot 11 Imaging device (imaging unit)
DESCRIPTION OF SYMBOLS 12 Robot main body 12a Support stand 12b Arm part 12c Hand part 20, 20a Position and orientation detection apparatus 30 Inspection apparatus 40 Robot control apparatus 201 Image data acquisition part 202 Image data storage part 203 Template information storage part 204 Matching degree evaluation part 205 Correspondence table storage Unit 206 position and orientation calculation unit 211 image data acquisition unit 212 image data storage unit 213 feature data generation unit 214 template feature data storage unit 215 first position and orientation calculation unit 216 template information storage unit 217 synthetic model image generation unit 218 second position and orientation Calculation part

Claims (6)

An imaging unit that images a target object and generates image data;
A robot body that movably supports the imaging unit;
For each line-of-sight direction from the image capturing unit with respect to the reference of the target object, template image data of the surface with respect to the line-of-sight direction, information representing the position and orientation of the surface in the robot coordinate system, and the position of the image capturing unit in the robot coordinate system And a template information storage unit for storing template information having information representing the posture;
A visible surface is detected from the image of the target object included in the image data generated by the imaging unit, template information corresponding to the visible surface is read from the template information storage unit, and the template information is read. A position and orientation data generation unit that generates position and orientation data representing the position and orientation of the target object in the robot coordinate system,
Equipped with a,
The position and orientation data generation unit
A template feature data storage unit that stores, for each template image data, template feature data in which a feature quantity at each of a plurality of feature points extracted from the template image data is associated with position information of the feature points; and the imaging unit A feature data generation unit that extracts a plurality of feature points from the image data generated by the image data and generates feature data in which feature amounts of the feature points are associated with position information of the feature points; and the feature data generation unit And calculating the degree of correlation between the feature amount in the feature data and the feature amount in the template feature data based on the feature data generated by the template feature data stored in the template feature data storage unit. Detecting position information of a feature point corresponding to a feature quantity to be detected, and based on the position information, the object A first position and orientation calculation unit that detects the reference plane is the highest face similarity to the template image data corresponding to said surface of the surfaces is a surface and the visible visible in the image,
A robot apparatus comprising:   The position and orientation data generation unit
A synthesized model image generating unit that generates synthesized model image data based on the position information of the feature points detected by the first position and orientation calculation unit, the reference plane, and the template information stored in the template information storage unit; ,
The similarity between the combined model image data generated by the combined model image generation unit and the image data obtained by projective conversion using a homography matrix is calculated, and the similarity exceeds a threshold value. A second position and orientation calculation unit that generates the position and orientation data based on the homography matrix and the template information,
  The robot apparatus according to claim 1, further comprising:   The position and orientation data generation unit
  For each of the plurality of template image data, the degree of coincidence between the image data and the converted image data obtained by projective transformation of the template image data using a homography matrix is calculated, and based on the degree of coincidence, the image of the target object A degree-of-matching evaluation unit that detects a visible surface;
  The template image data corresponding to the visible surface detected by the coincidence degree evaluation unit is subjected to projective transformation using a homography matrix to calculate the similarity between the image data and the highest similarity. A position and orientation calculation unit that generates the position and orientation data based on the homography matrix corresponding to the calculation result and the template information;
  With
  In the first position and orientation calculation unit in the position and orientation data generation unit, when the similarity corresponding to the reference plane is equal to or less than a threshold, the position and orientation data generation unit is caused to function.
  The robot apparatus according to claim 1 or 2, wherein For each line-of-sight direction with respect to the reference of the target object, it has template image data of the surface with respect to the line-of-sight direction, information representing the position and orientation of the surface in the reference coordinate system, and information representing the position and orientation of the viewpoint in the reference coordinate system A template information storage unit for storing template information;
A visible surface is detected from the image of the target object included in the image data obtained based on a predetermined viewpoint, template information corresponding to the visible surface is read from the template information storage unit, and the template information is read. A position and orientation data generation unit that generates position and orientation data representing the position and orientation of the target object in the reference coordinate system,
Equipped with a,
The position and orientation data generation unit
A template feature data storage unit that stores, for each template image data, template feature data in which a feature amount in each of a plurality of feature points extracted from the template image data is associated with position information of the feature points;
A feature data generating unit that extracts a plurality of feature points from the image data and generates feature data in which feature amounts for each feature point are associated with position information of the feature points;
Based on the feature data generated by the feature data generation unit and the template feature data stored in the template feature data storage unit, the degree of correlation between the feature amount in the feature data and the feature amount in the template feature data is calculated. Calculate and detect position information of feature points corresponding to matching feature quantities, and based on the position information, a template corresponding to the surface among the visible surface and the visible surface in the target object image A first position and orientation calculation unit that detects a reference surface that is a surface having the highest degree of similarity to image data;
The position and attitude sensing apparatus according to claim Rukoto equipped with. Computer
For each line-of-sight direction with respect to the reference of the target object, it has template image data of the surface with respect to the line-of-sight direction, information representing the position and orientation of the surface in the reference coordinate system, and information representing the position and orientation of the viewpoint in the reference coordinate system a template information storage unit that stores the template information,
A visible surface is detected from the image of the target object included in the image data obtained based on a predetermined viewpoint, template information corresponding to the visible surface is read from the template information storage unit, and the template information is read. A position and orientation data generation unit that generates position and orientation data representing the position and orientation of the target object in the reference coordinate system,
To function as,
The position and orientation data generation unit
A template feature data storage unit that stores, for each template image data, template feature data in which a feature amount in each of a plurality of feature points extracted from the template image data is associated with position information of the feature points;
A feature data generating unit that extracts a plurality of feature points from the image data and generates feature data in which feature amounts for each feature point are associated with position information of the feature points;
Based on the feature data generated by the feature data generation unit and the template feature data stored in the template feature data storage unit, the degree of correlation between the feature amount in the feature data and the feature amount in the template feature data is calculated. Calculate and detect position information of feature points corresponding to matching feature quantities, and based on the position information, a template corresponding to the surface among the visible surface and the visible surface in the target object image A first position and orientation calculation unit that detects a reference surface that is a surface having the highest degree of similarity to image data;
Position and orientation detection program to function as A position and orientation data generation unit detects a visible surface from an image of a target object included in image data obtained based on a predetermined viewpoint, and a template image of the surface with respect to the line-of-sight direction for each line-of-sight direction with respect to the reference of the target object Corresponding to the visible surface from a template information storage unit that stores data and template information having information representing the position and orientation of the surface in the reference coordinate system and information representing the position and orientation of the viewpoint in the reference coordinate system A position and orientation detection step of reading template information to be generated and generating position and orientation data representing the position and orientation of the target object in the reference coordinate system based on the template information;
I have a,
The position and orientation data generation unit
A feature data generation step of extracting a plurality of feature points from the image data and generating feature data in which feature amounts for each feature point are associated with positional information of the feature points;
Template feature data in which the feature data generated in the feature data generation step and the feature amount in each of a plurality of feature points extracted from the template image data are associated with the position information of the feature points for each template image data Based on the template feature data stored in the template feature data storage unit for storing the feature amount, the degree of correlation between the feature amount in the feature data and the feature amount in the template feature data is calculated to correspond to the matching feature amount A surface having the highest similarity between the surface that is visible in the image of the target object and the template image data corresponding to the surface among the surfaces that are visible based on the position information A first position / orientation calculation step for detecting a reference plane that is
Position and orientation detecting method characterized by have a.

Images