JP2014161937A - Posture detection device, position detection device, robot, robot system, posture detection method and program - Google Patents

Abstract

PROBLEM TO BE SOLVED: To facilitate posture estimation of an object while keeping accuracy of the posture estimation of the object.SOLUTION: A posture detection device includes: a first difference degree calculation unit 151 for calculating first difference degrees indicating degrees of the differences between an image obtained by imaging an object by a first imaging device, and a model image of the object, for each posture of the object in the model image; a posture acquisition unit 152 for, on the basis of conversion rules, acquiring multiple postures of the object in second virtual space corresponding to multiple postures of the object in first virtual space; a second difference degree calculation unit 153 for calculating second difference degrees indicating degrees of the differences between an image obtained by imaging the object by a second imaging device, and the model image of the object, for each posture of the object in the model image; and a posture estimation unit 154 for, on the basis of the multiple first difference degrees and the multiple second difference degrees, estimating the posture of the object in a coordinate system of the first imaging device.

Description

  The present invention relates to a posture detection device, a position detection device, a robot, a robot system, a posture detection method, and a program.

  2. Description of the Related Art Conventionally, there has been disclosed a robot technology that detects the position and orientation of an object from an image captured by a camera and controls the object to a predetermined position and orientation. In the prior art, for the position detection of the camera in the optical axis direction in the camera coordinates, the position is detected from the size of the object in the image, but the degree of change in the size of the object with respect to the change in position is There was a problem that it was small and detection accuracy was low. Furthermore, there is a problem in that the object in the image becomes smaller depending on the posture change of the object, and the posture detection accuracy is lowered.

  To solve this problem, a conventional stereo camera is used to detect the position on the plane perpendicular to the optical axis of the camera at each camera coordinate, and the position of the camera in the optical axis direction is determined using the principle of triangulation. A technique for detecting with high accuracy is disclosed (for example, see Patent Document 1). The technology discloses that the posture detection accuracy is further prevented by using the posture detected at each camera coordinate.

JP-T-2004-508954

  However, in order to increase the detection accuracy of the position and orientation, it is necessary to estimate the relative position and orientation of the cameras constituting the stereo camera with high accuracy, and it is necessary to increase the number of corresponding points of each coordinate. This is complicated and time consuming. On the other hand, if the number of corresponding points is reduced to shorten the time, there is a problem that the position and orientation cannot be detected with high accuracy.

  Accordingly, one aspect of the present invention has been made in view of the above problems, and a posture detection device and position detection that can simplify the posture estimation processing of an object while maintaining the estimation accuracy of the posture of the object. It is an object to provide an apparatus, a robot, a robot system, a posture detection method, and a program.

(1) According to one aspect of the present invention, a first captured image obtained by capturing an image of an object by the first imaging device and a first virtual space corresponding to the space captured by the first imaging device. A first dissimilarity calculating unit that calculates a first dissimilarity indicating a dissimilarity between the object and the model image for each of a plurality of different postures of the object in the first virtual space; In a second virtual image corresponding to a second captured image obtained by imaging a target by a second imaging device arranged at a position different from the imaging device, and a space imaged by the second imaging device A second dissimilarity calculating unit that calculates a second dissimilarity indicating a dissimilarity with the model image of the object for each of a plurality of postures of the object in the second virtual space; and the first dissimilarity A plurality of first differences calculated by the degree calculator and a plurality of differences calculated by the second difference calculator And posture estimation unit that estimates a posture of said object in said first imaging coordinate system based on the second difference degree is a posture detection device comprising a.
Thereby, since the attitude | position detection apparatus can estimate the attitude | position of a target object based on each difference obtained from the several different viewpoint, it can improve the estimation precision of an attitude | position.

  (2) One aspect of the present invention is the posture detection device described above, wherein the posture estimation unit includes the plurality of first differences calculated by the first difference calculation unit and the second differences. The degree of the object in the first imaging coordinate system is estimated based on a value obtained by multiplying each of the plurality of second dissimilarities calculated by the degree calculating unit by a weighting factor and multiplying the weight coefficient. To do. Accordingly, the posture detection device sets the weighting coefficient so as to neglect the degree of difference obtained from the captured image of the imaging device with the worse visibility of the target object between the first imaging device and the second imaging device. By multiplying, it is possible to reduce the influence of the difference included in the captured image of the imaging device with the worse visibility when estimating the posture of the object on the posture estimation of the object. , Posture estimation accuracy can be improved.

(3) One aspect of the present invention is the posture detection device described above, and when the first imaging device has better visibility of the object than the second imaging device, the posture estimation unit includes: When the object is in a predetermined position range from the target position, the weighting coefficient by which the first difference is multiplied is made larger than the weighting coefficient by which the second difference is multiplied.
Accordingly, the posture estimation unit weights the second degree of difference obtained from the captured image of the second imaging device with poorer visibility when the object is in a predetermined position range from the target position. Therefore, the influence of the error included in the second difference degree on the posture estimation of the object can be reduced. As a result, the posture estimation unit can improve the accuracy of posture estimation of the object. In addition, since it is not necessary to calculate the relative positional relationship between the imaging devices, it is possible to easily estimate the posture of the object.

(4) Moreover, one aspect of the present invention is the posture detection device described above, and when the first imaging device has better visibility of the object than the second imaging device, the posture estimation unit When the object is in a predetermined position range from the target position, the object is moved from the movement start time of the object so that a weighting factor to be multiplied by the second difference is within the predetermined range. The weighting factor by which the second difference is multiplied is changed until the time when the object reaches the predetermined position range from the target position.
Thereby, when the target object is in a predetermined position range from the target position, the weight related to the second difference obtained from the captured image of the second imaging device with poor visibility is determined in advance. By making it fall within the range, it is possible to further reduce the influence of the error included in the second dissimilarity on the posture estimation of the object. As a result, the posture estimation unit can improve the accuracy of posture estimation of the object.

(5) Moreover, one aspect of the present invention is the posture detection device described above, and when the first imaging device has better visibility of the object than the second imaging device, the posture estimation unit Is a period from the movement start time of the target object to the time when the target object reaches a predetermined position range from the target position, and the weight coefficient to be multiplied by the second difference is reduced with the passage of time. .
As a result, as the time elapses, the weight related to the second difference obtained from the captured image of the second imaging device with poorer visibility becomes smaller, so that the error included in the second difference becomes smaller. The influence on the posture estimation of the object can be reduced. As a result, the posture estimation unit can improve the accuracy of posture estimation of the object.

(6) One embodiment of the present invention is the posture detection device described above, in which the first imaging device has better visibility of the object than the second imaging device, and the position of the object. When the pose moves, the posture estimation unit decreases the weight coefficient by which the second difference is multiplied as the amount of movement of the object per unit time decreases.
Here, the movement amount of the object per unit time is the largest when the object starts to move, and then gradually decreases with the passage of time. For this reason, as the time elapses, the weight related to the second dissimilarity decreases, so that the influence of the error included in the second dissimilarity on the posture estimation of the object can be reduced. As a result, the posture estimation unit can improve the accuracy of posture estimation of the object.

(7) According to another aspect of the present invention, in the posture detection device described above, when the first imaging device has better visibility of the object than the second imaging device, the posture estimation unit Decreases the weighting factor by which the second difference is multiplied as the object approaches the target position.
Thereby, as the object approaches the target position, the weight related to the second dissimilarity decreases, so that the influence of the error included in the second dissimilarity on the posture estimation of the object can be reduced. As a result, the posture estimation unit can improve the accuracy of posture estimation of the object.

  (8) Moreover, 1 aspect of this invention is the above-mentioned attitude | position detection apparatus, Comprising: The said attitude | position estimation part calculates the weighting coefficient by which the said 1st difference is multiplied, and the weighting coefficient by which the said 2nd difference is multiplied. , And changing according to a noise component mixed in each of the first captured image and the second captured image. As a result, the posture estimation unit can relatively reduce the weighting factor for an image with a large amount of noise components, and can relatively reduce the contribution of the dissimilarity corresponding to an image with a large amount of noise components to posture estimation. Therefore, the accuracy of posture estimation of the object can be improved.

  (9) Moreover, one aspect of the present invention is the posture detection apparatus described above, wherein the posture estimation unit multiplies the weight coefficient by which the first difference is multiplied and the weight coefficient by which the second difference is multiplied. Is the reciprocal of the variance of the normalized random noise mixed in each of the first captured image and the second captured image. As a result, the greater the variance of the normalized random noise, the more noise, but the greater the variance of the normalized random noise, the smaller the weighting factor. Therefore, the greater the noise, the smaller the weighting factor. As a result, the weighting factor for a noisy image can be made relatively small, and the contribution of the dissimilarity corresponding to the noisy image to the posture estimation can be made relatively small. The accuracy of estimation can be improved.

  (10) Moreover, one aspect of the present invention is the posture detection device described above, wherein the posture estimation unit calculates a difference between images captured by the first imaging device between different frames. Calculated as the variance of normalized random noise mixed in the image, and the difference in the image between different frames captured by the second imaging device as the variance of normalized random noise mixed in the second captured image calculate. Thus, by calculating the image dissimilarity between the frames, it is possible to calculate the difference in the amount of noise included between the different frames. Can be approximated. Thereby, the larger the image dissimilarity between frames, the larger the variance of the normalized random noise and the smaller the weighting factor. As a result, the weighting factor for a noisy image can be made relatively small, and the contribution of the dissimilarity corresponding to the noisy image to the posture estimation can be made relatively small. The accuracy of estimation can be improved.

  (11) Moreover, one aspect of the present invention is the posture detection device described above, in which coordinates when the posture of the object is expressed in a first imaging coordinate system with the first imaging device as a reference are expressed. A plurality of different objects in the first virtual space based on a conversion rule for converting to coordinates when the posture of the object is expressed in a second imaging coordinate system based on the second imaging device. A posture acquisition unit that acquires each of a plurality of postures of the object in the second virtual space corresponding to the posture is further provided, and the second difference calculation unit is arranged at a position different from that of the first imaging device. A second captured image obtained by capturing the target object by the second image capturing apparatus and a model image of the target object in the second virtual space corresponding to the space captured by the second image capturing apparatus. The posture acquisition unit calculates a second degree of difference indicating the degree of difference. Calculated for each of the plurality of orientation of the target object in serial the second virtual space. As a result, the degree of difference can be calculated for each model image of the object viewed from different viewpoints in the first virtual space and the second virtual space. As a result, since the posture can be estimated by placing importance on the posture detected from the captured image captured by the imaging device suitable for posture detection, the accuracy of the posture estimation of the target can be improved.

(12) One embodiment of the present invention is the posture detection device described above, wherein the first difference calculation unit is a first obtained by the first imaging device imaging an object. A third dissimilarity indicating the dissimilarity between the captured image and the model image of the object in the first virtual space is calculated for each of a plurality of different positions of the object in the first virtual space. The second dissimilarity calculation unit includes a second captured image obtained by the second imaging device capturing an object and a model image of the object in the second virtual space. A fourth dissimilarity indicating a dissimilarity is calculated for each of a plurality of different positions of the object in the second virtual space, and is based on the plurality of third dissimilarities and the plurality of fourth dissimilarities. A position estimation unit that estimates the position of the object in the first imaging coordinate system; Provided.
As a result, the position is estimated from the third difference calculated using the first captured image and the fourth difference calculated using the second captured image. The position estimation accuracy can be improved compared to the position estimation from the image.

(13) Moreover, one aspect of the present invention is the posture detection device described above, wherein the position estimation unit refers to a plurality of third differences calculated by the first difference calculation unit, Estimating the position of the first plane orthogonal to the optical axis of the imaging device in the first imaging coordinate system, and referring to the plurality of fourth differences calculated by the second difference calculator; With reference to the plane position estimation unit that estimates the position of the second plane orthogonal to the optical axis of the imaging apparatus in the second imaging coordinate system, the position of the first plane, and the position of the second plane And a depth calculation unit that calculates the depth of the object in the first imaging coordinate system.
Thereby, since the depth is calculated from the third difference calculated using the first captured image and the third difference calculated using the second captured image, the depth estimation accuracy is improved. Can be made. Further, the first imaging device is used to calculate the position of the first imaging device in the optical axis direction with reference to the position of the first plane and the position of the second plane estimated by the plane position estimation unit. The estimation accuracy of the position in the optical axis direction can be improved.

  (14) One aspect of the present invention is the posture detection device described above, in which the position estimation unit is configured to perform the first estimation based on the plurality of third differences and the plurality of fourth differences. The depth of the object in one imaging coordinate system is estimated. As a result, the depth is estimated from the third difference calculated using the first captured image and the fourth difference calculated using the second captured image. It is possible to improve the accuracy of estimating the depth than estimating the depth from the image.

(15) Moreover, one aspect of the present invention is the posture detection apparatus described above, with reference to a difference between the posture of the object estimated by the posture estimation unit and the posture of the object estimated before that. A determination unit that determines whether or not the estimation has converged, and when the determination unit determines that the estimation has not converged, the first difference calculation unit determines the orientation of the model image of the object in advance The plurality of first dissimilarities are calculated based on the model image after being changed as determined and the first captured image, and the second dissimilarity calculating unit is configured to calculate the model image of the object. The plurality of second dissimilarities are calculated based on the model image after the pose is changed as determined in advance and the second captured image, and the posture estimating unit is configured to calculate the first dissimilarity. The plurality of first differences calculated by the calculation unit and the second difference calculation unit calculated by the calculation unit Based on the second difference degree number, and estimating the pose of the object, the determining unit again performs the determination using the attitude of the object in which the posture estimating unit has estimated.
As a result, the posture estimation unit updates the posture of the object until the difference between the posture of the target object and the posture of the target object previously estimated becomes small, so that the accuracy of the posture estimation of the target object is improved. Can do.

(16) One embodiment of the present invention is the posture detection apparatus described above, wherein the position or posture of the target object changes before and after the target object is within a predetermined range from the target position. The amount of change between the two images acquired by the first imaging device is the difference between the two images acquired by the second imaging device before and after the position or orientation of the object changes. Greater than the amount of change.
Thereby, the first imaging device has good visibility of the object when the object is in a predetermined range from the target position, and when the object is in the predetermined range from the target position. In addition, since the ratio that the first difference contributes to the detection of the posture is relatively large, the posture accuracy of the target when the target is in a predetermined range from the target position can be improved.

(17) Further, one aspect of the present invention is the posture detection device described above, wherein the first imaging device captures the target when the target is at a target position and posture. A goal image acquisition unit that acquires a goal image and a second goal image captured by the second imaging device, and an object that exists in a first virtual space that reproduces the space where the first goal image is captured A fifth image indicating a degree of difference between the first goal image and an image captured by moving the model as determined in advance around or in a predetermined axis from the position and orientation of the model The degree of difference is calculated and predetermined around the axis or in the axial direction based on the position and orientation of the model of the object existing in the second virtual space that reproduces the space where the second goal image was captured. The model as A sixth dissimilarity indicating a dissimilarity between the image taken by moving and the second goal image is calculated, and the fifth dissimilarity in the moving direction when the fifth dissimilarity is calculated is calculated. By comparing the gradient and the gradient of the sixth dissimilarity in the moving direction when the sixth dissimilarity is calculated, an imaging device serving as a reference for posture estimation is determined, and the determined imaging device And a positioning reference imaging device determination unit that newly sets the first imaging device as the first imaging device.
As a result, in the vicinity of the target position and posture, the ratio that the degree of difference derived from the image captured by the imaging device serving as a reference for the determined posture estimation contributes to the posture estimation increases. At that time, the change in the dissimilarity becomes larger in the vicinity of the target position and posture than when another imaging device is used as a reference, so that the target can be reached earlier in the target position and posture.

  (18) Further, according to one aspect of the present invention, in the posture detection device described above, when there are a plurality of the objects, the first difference calculation unit performs position and posture estimation in the first virtual space. The image areas of all objects other than the target object are specified, the specified image area is excluded from the calculation target, the first difference is calculated, and the second difference calculation unit is In this virtual space, the image areas of all the objects other than the object to be subjected to position / orientation estimation are specified, and the specified image area is excluded from the calculation target, and the second difference is calculated. Thus, even if there are a plurality of objects, the image area of the object other than the object that is the object of posture estimation does not contribute to the degree of difference, so that the image area of the object that is the object of posture estimation The degree of difference can be calculated only from the above. As a result, the posture can be estimated for each of the plurality of objects.

(19) One aspect of the present invention is the posture detection apparatus described above, based on the estimated position and posture of the object and the position and posture when the object is at the target position. A target position / posture determination unit that determines the amount of change in position and posture in the robot coordinate system after a predetermined time has elapsed from the current time, and the target position / posture determination unit for each time interval for controlling the robot. A robot controller that determines the position and posture of the robot after the time interval based on the latest amount of change among the determined amounts of change, and controls the robot to be the determined position and posture; and The robot control unit performs a robot until the time determined based on the shortest time from which the own apparatus determines the amount of change from the first captured image and the second captured image within the predetermined time. DOO determines the position and orientation of the robot for movement acceleration and deceleration, the subsequent time to determine the position and orientation of the robot to move the robot at a constant speed.
Thereby, even if there is a fluctuation in the time until the change amount is determined from the first captured image and the second captured image, the change amount is reduced from the start of imaging of the first captured image and the second captured image. When a series of image processing until the determination is completed, the robot always moves at a constant speed. Thereby, the robot controller can start acceleration control or deceleration control of the robot with the speed when moving at the constant speed as the initial speed. Therefore, the robot control unit can control the robot using the amount of change obtained by the image processing at the same time as the image processing is completed, so the time until the object is set to the target position and target posture is shortened. be able to.

(20) Further, according to one aspect of the present invention, the first difference degree is calculated for each of a plurality of different postures of the object in the first virtual space corresponding to the space captured by the first imaging device. The second difference for each of the plurality of postures of the object in the second virtual space corresponding to the space imaged by the second imaging device arranged at a different position from the first imaging device. A first imaging coordinate based on the first imaging device with reference to a second degree-of-difference calculation unit that calculates a degree, and the plurality of first differences and the plurality of second differences And a posture estimation unit that estimates the posture of the object in the system.
Thereby, since the attitude | position detection apparatus can estimate the attitude | position of a target object based on each difference obtained from the several different viewpoint, it can improve the estimation precision of an attitude | position.

(21) Further, according to one embodiment of the present invention, a difference between a first captured image obtained by the first imaging device capturing an image of the target object and a model image of the target object in the first virtual space. A third dissimilarity calculating unit that calculates a first dissimilarity indicating a degree for each of a plurality of different positions of the object in the first virtual space, and the second imaging device images the object. The first difference indicating the difference between the second captured image obtained in step S3 and the model image of the object in the second virtual space is a plurality of different objects of the object in the second virtual space. A second dissimilarity calculator calculated for each position; a plurality of first dissimilarities calculated by the first dissimilarity calculator; and a plurality of second dissimilarities calculated by the second dissimilarity calculator. A position estimation unit that estimates the position of the object in the first imaging coordinate system based on the degree of difference; A position detecting device comprising a.
Thereby, since the position detection apparatus can estimate the position of the object based on each of the degrees of difference obtained from a plurality of different viewpoints, the position estimation accuracy can be improved.

(22) Further, according to one embodiment of the present invention, a first virtual image corresponding to a first captured image obtained by imaging a target by the first imaging device and a space captured by the first imaging device. A first dissimilarity calculating unit that calculates a first dissimilarity indicating a dissimilarity with a model image of the object in space for each of a plurality of different postures of the object in the first virtual space; And a second virtual image corresponding to a space captured by the second imaging device and a second captured image obtained by imaging the object by the second imaging device arranged at a different position from the first imaging device. A second dissimilarity calculating unit that calculates a second dissimilarity indicating a dissimilarity with the model image of the object in space for each of a plurality of postures of the object in the second virtual space; The plurality of first difference degrees calculated by the difference degree calculation unit and the second difference degree calculation unit calculate A robot and a posture estimation unit that estimates a posture of said object in said first imaging coordinate system based on a plurality of second difference degree.
Thereby, since the posture of the object can be estimated based on the degrees of difference obtained from a plurality of different viewpoints, the accuracy of posture estimation can be improved.

(23) Further, according to one embodiment of the present invention, a robot having a movable part, a first captured image obtained by imaging a target by the first imaging device, and a space captured by the first imaging device. A first difference that calculates a first degree of difference indicating a degree of difference from the model image of the object in the first virtual space corresponding to each of a plurality of different postures of the object in the first virtual space. A second captured image obtained by imaging the object by the second imaging device arranged at a position different from the degree calculating unit and the first imaging device, and a space captured by the second imaging device 2nd difference which calculates the 2nd difference which shows the difference with the model image of the above-mentioned object in the 2nd virtual space corresponding to each for a plurality of postures of the object in the above-mentioned 2nd virtual space A plurality of first differences calculated by the calculation unit and the first difference calculation unit; And posture estimation unit that estimates a posture of said object in said first imaging coordinate system based on a plurality of second difference degree of dissimilarity calculating unit 2 has been calculated, a robotic system comprising a.
Thereby, since the posture of the object can be estimated based on the degrees of difference obtained from a plurality of different viewpoints, the accuracy of posture estimation can be improved.

(24) In addition, according to one aspect of the present invention, the first dissimilarity calculating unit captures a first captured image obtained by the first imaging device capturing an image of an object, and the first imaging device captures an image. Calculating a first dissimilarity indicating a dissimilarity with the model image of the object in the first virtual space corresponding to the space for each of a plurality of different postures of the object in the first virtual space; A second captured image obtained by the second imaging device having a second dissimilarity calculating unit imaging a target by a second imaging device arranged at a position different from the first imaging device; and the second imaging A second dissimilarity indicating a dissimilarity with the model image of the object in the second virtual space corresponding to the space imaged by the device is calculated for each of a plurality of postures of the object in the second virtual space. A plurality of first dissimilarities calculated by the first dissimilarity calculating unit by the procedure and the posture estimating unit; A step of serial second difference calculation unit estimates the posture of the object in the first image acquisition coordinate system based on a plurality of second difference degree calculated, a position detection method having.
Thereby, since the posture of the object can be estimated based on the degrees of difference obtained from a plurality of different viewpoints, the accuracy of posture estimation can be improved.

(25) In addition, according to one embodiment of the present invention, a computer may include a first captured image obtained by imaging a target by the first imaging device, and a first space corresponding to the space captured by the first imaging device. A first dissimilarity calculating step for calculating a first dissimilarity indicating a dissimilarity with the model image of the object in one virtual space for each of a plurality of different postures of the object in the first virtual space; , A second captured image obtained by imaging a target object by a second imaging device arranged at a position different from that of the first imaging device, and a second corresponding to a space captured by the second imaging device. A second degree of difference indicating a degree of difference from the model image of the object in the second virtual space for each of a plurality of postures of the object in the second virtual space calculated in the posture acquisition step. 2 difference degree calculation steps and the first difference degree calculation step. Of the object in the first imaging coordinate system based on the plurality of first dissimilarities calculated in step 2 and the plurality of second dissimilarities calculated in the second dissimilarity calculating step. And a posture estimation step for estimating a posture.
By executing this program, the posture of the object can be estimated based on the degrees of difference obtained from a plurality of different viewpoints, so that the posture estimation accuracy can be improved.

It is a schematic block diagram which shows the structure of the robot in 1st Embodiment. It is a figure for demonstrating the coordinate used when detecting the position and attitude | position of a target object. It is a figure which shows an example of the image image | photographed with the 1st camera and the 2nd camera. It is a schematic block diagram which shows the structure of the control apparatus in 1st Embodiment. It is a schematic block diagram which shows the structure of the position and orientation detection part in 1st Embodiment. It is a flowchart which shows an example of the flow of the whole process of the control apparatus in 1st Embodiment. 5 is a flowchart illustrating an example of a position and orientation detection process of an object 104 used in steps S105 and S106 of FIG. FIG. 8 is a flowchart continued from FIG. 7. FIG. 9 is a flowchart continued from FIG. 8. FIG. The weight w _A of the first camera which is an example of a temporal change of the weight w _B of the second camera. It is a schematic block diagram which shows the structure of the control apparatus in 2nd Embodiment. It is a flowchart which shows an example of the flow of the whole process of the control apparatus in 2nd Embodiment. It is a schematic block diagram which shows the structure of the control apparatus in 3rd Embodiment. It is an example of the time change of the x coordinate of the hand position of the robot main body in a robot coordinate system. It is a flowchart which shows an example of the flow of the whole process of the control apparatus in 3rd Embodiment. It is a flowchart which shows an example of the process of the robot control part in 3rd Embodiment. It is a figure for demonstrating a mask image.

<First Embodiment>
Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.
As an example, the robot 1 according to the first embodiment uses a stereo camera to estimate the position and orientation of an object that is a target for position and orientation estimation. Then, as an example, the robot 1 detects the position and orientation of the object using a weighted average of the degrees of difference between an image captured by each camera and an image captured from a CG (Computer Graphics) model of the object. As an example, the robot 1 controls the target to a predetermined position and posture using visual servo.

  FIG. 1 is a schematic block diagram showing the configuration of the robot in the first embodiment. The robot 1 includes a control device (attitude detection device) 101, a robot body 102, a gripper 103, a first camera (first imaging device) 105, and a second camera (second imaging device) 106. The robot body 102 is a vertical articulated robot as an example. The first camera 105 and the second camera 106 constitute a stereo camera. The figure shows the robot 1 when the gripper 103 is holding the object 104. In the drawing, an example of the position of the robot hand (hereinafter also referred to as a robot hand position) P1 is shown.

In the figure, sigma _R are the coordinates of the robot, the rotation matrix indicating the orientation of the coordinates of the first camera 105 to be described later in sigma _R and R (hat) _RA. Here, R (hat) is obtained by adding the symbol ^ on R.
The control device 101 controls the robot body 102.

The robot body 102 grasps the object 104 with the gripper 103 according to the control of the control device 101. Then, the robot body 102 changes the position (also referred to as the robot hand position) and posture of the gripper 103 to the target position and target posture of the gripper 103 so that the target object 104 takes a target position and posture.
The robot body 102 includes six joints as an example. The robot body 102 may have 5 or less joints or 7 or more joints. In the present embodiment, as an example, a description will be given of an example in which the control apparatus 101 conveys the object 104 by controlling the robot body 102 so that the object 104 takes a target position and posture. The assembly work of the object 104 can be performed by the same process.

  In this embodiment, rather than arranging the stereo cameras in such a way that the orientations of the coordinate axes of the respective cameras coincide with each other as in the case of parallel stereo, the axes of the respective camera coordinates are arranged so as to complement each other. It is good to arrange so that the directions do not match (arrange so that the object 104 is photographed from different directions).

FIG. 2 is a diagram for explaining coordinates used when detecting the position and orientation of the object 104. In the figure, Σ _A is a coordinate based on the first camera 105 (hereinafter also referred to as a first camera coordinate), and Σ _B is a coordinate based on the second camera 106 (hereinafter referred to as a second camera coordinate). and is also referred to), the at T (arrow) _BA and R (hat) _BA is the position vector indicating the position of the first camera coordinate sigma _a in the second camera coordinate sigma _B respectively, the second camera coordinate sigma _B is a rotation matrix indicating the orientation of the first camera coordinate sigma _a. Here, T (arrow) is obtained by adding a symbol → to T. T (Arrow) _A and R (hat) _A is the position vector indicating the position of the object 104 in the first camera coordinate sigma _A respectively, at a rotation matrix indicating the orientation of the object 104 in the first camera coordinate sigma _A is there. T _{(Arrow) B} and R _{(hat) B} is the position vector indicating the position of the object 104 in the sigma _B respectively, a rotation matrix indicating the orientation of the object 104 in the sigma _B.

FIG. 3 is a diagram illustrating an example of images taken by the first camera 105 and the second camera 106. In the figure, an image G301 photographed by the first camera 105 and an image G302 photographed by the second camera 106 are shown. The coordinates of the image G301 photographed by the first camera 105 are u _{A on} the horizontal axis and v _{A on} the vertical axis with the upper left corner of the image G301 as the origin. Coordinates of the image G302 taken by the second camera 106 _{u B} the horizontal axis at the upper left corner of the image G302 as the origin, the longitudinal axis and _{u B.} Note that a rectangular parallelepiped drawn with a solid line in the image 301 photographed by the first camera 105 and the image 302 photographed by the second camera 106 represents the image area of the current object 104.

FIG. 4 is a schematic block diagram illustrating the configuration of the control device 101 according to the first embodiment. The control device 101 includes an input unit 110, a processing unit 120, and a storage unit 190.
The input unit 110 receives input from the user (for example, items related to setting items), and outputs input information indicating the received input to the processing unit 120.
The storage unit 190 stores various programs for the processing unit 120 to read and execute.

  The processing unit 120 stores the input information input from the input unit 110 in the storage unit 190. The processing unit 120 reads and executes a program stored in the storage unit 190. The processing unit 120 includes a conversion rule determination unit 130, a goal image acquisition unit 140, a current image acquisition unit 145, a position and orientation detection unit 150, a robot control unit 160, and a first determination unit 170.

  The conversion rule determination unit 130 uses the second imaging coordinate system with the second camera 106 as a coordinate when the posture of the object is represented in the first imaging coordinate system with the first camera 105 as a reference. A conversion (for example, a rotation matrix) to the coordinates when the posture of the object 104 is expressed is determined. The conversion rule determination unit 130 outputs conversion rule information indicating the determined conversion rule to the position / orientation detection unit 150.

  In the present embodiment, as an example, the visibility of the object 104 when the object 104 is in a predetermined range from the target position is better for the first camera 105 than for the second camera 106. To do. Here, visibility is the amount of change in the image when the position or orientation of the object 104 changes. A camera with good visibility has a large amount of change between images before and after the change even if the position or orientation of the object 104 changes minutely. Thereby, a camera with good visibility can capture a minute change in the position or orientation of the object 104 as a difference in images. From the above, in the present embodiment, as an example, when the object 104 is within a predetermined range from the target position, the first camera 105 is moved before and after the position or posture of the object 104 changes. The amount of change between the two images obtained by imaging is larger than the amount of change between the two images obtained by the second camera 106 before and after the position or orientation of the object 104 changes.

The goal image acquisition unit 140 generates an image (hereinafter also referred to as a goal image) captured by the first camera 105 when the object 104 is in the target position and posture. The goal image acquisition unit 140 outputs goal image information indicating the generated goal image to the position / orientation detection unit 150.
The current image acquisition unit 145 causes the first camera 105 to image the current object 104 and acquires a first image obtained by imaging. In addition, the current image acquisition unit 145 causes the second camera 106 to image the current object 104 and acquires a second image obtained by imaging. The current image acquisition unit 145 outputs the acquired first image information indicating the first image and second image information indicating the second image to the position and orientation detection unit 150.

The position / orientation detection unit 150 refers to the conversion rule determined by the conversion rule determination unit 130 and the goal image generated by the goal image acquisition unit 140 when the target object 104 is at the target position and orientation (goal state). _A position vector T (arrow) ^g _A and a posture R (hat) ^g _A indicating the position of the object 104 are detected.
Further, the position / orientation detection unit 150 refers to the conversion rule determined by the conversion rule determination unit 130, the first image information and the second image information input from the current image acquisition unit 145, and the current position of the object 104. A position vector T (arrow) ^c _A and an attitude R (hat) ^c _A are detected. The position / orientation detection unit 150 includes information indicating a position vector T (arrow) ^g _A and an attitude R (hat) ^g _A indicating the position of the detected target object 104 in the goal state, and a position indicating the current position of the object 104. Information indicating the vector T (arrow) ^c _A and the posture R (hat) ^c _A is output to the robot controller 160.

The robot control unit 160 receives a position vector T (arrow) ^g _A indicating the position of the target object 104 in the goal state input from the position and orientation detection unit 150 and a position vector T (arrow) indicating the current position of the target object ^c. Referring to _A , the gripper 103 is moved in the translation direction. Further, the robot controller 160 refers to the posture R (hat) ^g _A of the target object 104 in the goal state and the posture R (hat) ^c _{A of the} current target object 104 to move the gripper 103 in the rotation direction. .

The first determination unit 170 determines whether the current position of the target object 104 has reached the position of the target object 104 in the goal state, and the current posture of the target object 104 is set to the posture of the target object 104 in the goal state. It is determined whether it has been reached.
The first determination unit 170 determines that the current position of the target object 104 has reached the position of the target object 104 in the goal state, and the current posture of the target object 104 has reached the position of the target object 104 in the goal state It is determined that the object 104 has reached the goal state. On the other hand, in other cases, the first determination unit 170 determines that the goal state has not been reached, and the position vector T (arrow) ^c _A and the posture indicating the current position of the object 104 in the first camera coordinates. The position / orientation detection unit 150 detects a rotation matrix R (hat) ^c _A indicating, and the process is continued.

  FIG. 5 is a schematic block diagram illustrating a configuration of the position / orientation detection unit 150 according to the first embodiment. The position / orientation detection unit 150 includes a first dissimilarity calculation unit 151, an attitude acquisition unit 152, a second dissimilarity calculation unit 153, an attitude estimation unit 154, a position estimation unit 156, and a second determination unit 159. The position estimation unit 156 includes a plane position estimation unit 157 and a depth update unit 158. First, after describing an outline of processing of each unit included in the position and orientation detection unit 150, detailed processing of each unit will be described using a flowchart described later.

  The first dissimilarity calculation unit 151 corresponds to a first captured image obtained by the first camera 105 capturing an image of the object 104 to be posture estimated and a space captured by the first camera 105. A first dissimilarity indicating a dissimilarity with the model image of the object 104 in the first virtual space is calculated for each of a plurality of different postures of the object in the first virtual space. In the present embodiment, as an example, the model image is a computer graphic image (CG image). The first dissimilarity calculation unit 151 outputs information indicating the plurality of calculated first dissimilarities to the posture acquisition unit 152 and the posture estimation unit 154. Here, the greater the amount of change in the degree of difference in a certain axial direction or around a certain axis, the higher the visibility.

  The posture acquisition unit 152, based on the conversion rule determined by the conversion rule determination unit 130, a plurality of postures of the object 104 in the second virtual space corresponding to a plurality of different postures of the object 104 in the first virtual space. Get each. The posture acquisition unit 152 outputs information indicating the calculated plurality of postures to the second difference calculation unit 153.

  The second dissimilarity calculation unit 153 includes a second captured image obtained by the second camera 106 capturing an image of the object 104 and a second virtual space corresponding to the space captured by the second camera 106. A second dissimilarity indicating a dissimilarity between the target object 104 and the model image is calculated for each of a plurality of postures of the target object 104 in the second virtual space calculated by the posture acquisition unit 152. The second dissimilarity calculation unit 153 outputs information indicating the calculated plurality of second dissimilarities to the posture estimation unit 154.

The posture estimation unit 154 refers to the plurality of first differences calculated by the first difference calculation unit 151 and the plurality of second differences calculated by the second difference calculation unit 153. The posture of the object 104 is estimated. In addition, when the first camera 105 has better visibility of the object 104 than the second camera 106, the posture estimation unit 154 is in a position range determined in advance from the target position ( For example, when the object 104 is at the target position), the weight related to the first dissimilarity in the estimation of the posture of the object 104 is made larger than the weight related to the second dissimilarity. Here, “good visibility” means that the amount of change in the image when the position or orientation of the object 104 changes is large, and the first camera 105 is more visible than the second camera 106. The case where is good means that the amount of change of the first image when the position or orientation of the object 104 changes is larger than the amount of change of the second image. Here, when the object 104 is within a predetermined position range from the target position, a state in which the object 104 does not stop completely is also included. The posture estimation unit 154 outputs information indicating the estimated posture of the target object 104 to the second determination unit 159.
The first dissimilarity calculation unit 151 calculates the dissimilarity between the first captured image obtained by the first camera 105 capturing an image of the object and the model image of the object 104 in the first virtual space. The third degree of difference shown is calculated for each of a plurality of different positions of the object 104 in the first virtual space. Then, the first dissimilarity calculation unit 151 outputs information indicating the calculated fourth dissimilarities to the plane position estimation unit 157 and the depth update unit 158.

  The second degree-of-difference calculation unit 153 calculates the degree of difference between the second captured image obtained by the second camera 106 capturing an object and the model image of the object 104 in the second virtual space. The fourth degree of difference shown is calculated for each of a plurality of different positions of the object 104 in the second virtual space. Then, the second dissimilarity calculation unit 153 outputs information indicating the calculated fourth dissimilarities to the depth update unit 158.

  The planar position estimation unit 157 refers to the plurality of third differences calculated by the first difference calculation unit 151 and updates the horizontal position and the vertical position of the object 104 in the first imaging coordinate system. The plane position estimation unit 157 outputs information indicating the updated horizontal position and vertical position to the depth update unit 158, for example.

  The depth update unit 158 includes a plurality of third differences calculated by the first difference calculation unit 151, a plurality of fourth differences calculated by the second difference calculation unit 153, and a plane position estimation unit. With reference to the updated horizontal position and vertical position input from 157, the depth of the object 104 in the first imaging coordinate system is updated. The depth update unit 158 outputs information indicating the updated depth and information indicating the updated horizontal position and vertical position input from the plane position estimation unit 157 to the second determination unit 159.

  The second determination unit 159 refers to the difference between the posture of the target object estimated by the posture estimation unit 154 and the posture of the target object 104 estimated before that to determine whether the estimation has converged. Further, the second determination unit 159 also refers to the respective differences between the updated horizontal position and vertical position input from the depth update unit 158 and the horizontal position and vertical position of the object 104 estimated before that, It is determined whether the estimation has converged. Further, the second determination unit 159 determines whether or not the estimation has converged with reference to each difference between the updated depth input from the depth update unit 158 and the depth estimated before that.

  When the second determination unit 159 determines that it has not converged, each unit executes the following processing. The first dissimilarity calculation unit 151 calculates a plurality of first dissimilarities based on the model image and the first captured image after changing the posture of the model image of the object 104 as determined in advance. calculate. Furthermore, the second dissimilarity calculation unit 153 generates a plurality of second differences based on the model image and the second captured image after changing the posture of the model image of the object 104 as determined in advance. Calculate the degree. Then, the posture estimation unit 154 is based on the plurality of first differences calculated by the first difference calculation unit 151 and the plurality of second differences calculated by the second difference calculation unit 153. The posture of the object 104 is updated. The second determination unit 159 performs determination again using the posture of the object 104 updated by the posture estimation unit 154. Details of these processes will be described with reference to flowcharts described later.

On the other hand, when it is determined that the second determination unit 159 has converged, the position vector T (arrow) ^g _A indicating the position of the target object 104 in the goal state and the rotation matrix R (hat) ^g _A indicating the posture In this case, the second determination unit 159 outputs the posture of the target object 104 estimated by the posture estimation unit 154 to the robot control unit 160 as a rotation matrix R (hat) ^g _A indicating the posture of the target object 104 in the goal state. Further, the updated horizontal position and vertical position output from the plane position estimation unit 157 and the updated depth output from the depth update unit 158 are combined as a position T (arrow) ^g _A of the target object 104 in the goal state. And output to the robot controller 160.

When it is determined that the second determination unit 159 has converged and the estimation target is the position T (arrow) ^c _{A of the} current object 104 and the rotation matrix R (hat) ^c _A indicating the posture, the second determination unit 159 Outputs the posture of the target object 104 estimated by the posture estimation unit 154 to the robot control unit 160 as a rotation matrix R (hat) ^c _A indicating the current posture of the target object 104. Further, the updated horizontal position and vertical position output by the plane position estimation unit 157 and the updated depth output by the depth update unit 158 are combined as the current position T (arrow) ^c _A of the target object 104. Output to the robot controller 160.

Next, FIG. 6 is a flowchart illustrating an example of the overall processing flow of the control apparatus 101 according to the first embodiment.
(Step S101) First, the processing unit 120 estimates the rotation matrix R _{(hat) RA} indicating the posture of the first camera coordinate sigma _A in the robot coordinate sigma _R. Specifically, for example, an easy to detect markers by gripping by the gripper 103, is moved in the axial direction of the robot coordinate sigma _R, processing unit 120, the same processing as the position detection processing of the object 104 to be described later using, for detecting the position of the marker in the first camera coordinate sigma _a. Processing unit 120 estimates the rotation matrix R (hat) _RA indicating the posture of the first camera coordinate sigma _A in the robot coordinate sigma _R using the detected position of the marker.

(Step S102) Next, the conversion rule determination unit 130 estimates the position vector T _{(arrow) BA} indicating the position of the origin of the first camera coordinate sigma _A in the second camera coordinate sigma _B. The conversion rule determination unit 130 estimates the rotation matrix R (hat) _BA indicating the posture of the first camera coordinate sigma _A in the second camera coordinate sigma _B. Specifically, for example, the conversion rule determination unit 130 causes the first camera 105 and the second camera 106 to photograph the object 104 on which a known pattern is drawn, and uses two images obtained by the imaging to be described later. The position vector T (arrow) _BA and the rotation matrix R (hat) _BA may be estimated by the position and orientation detection processing shown in FIGS.

(Step S103) Next, the goal image acquisition unit 140 generates an image captured by the first camera 105 when the object 104 is at the target position and posture. Specifically, in this embodiment, since the conveyance work of the target object 104 is assumed as an example, the goal image acquisition unit 140 is, for example, the first operation when the target object 104 is in the position and orientation at the transfer destination. An image captured by one camera 105 may be generated. At this time, the goal image acquisition unit 140 may acquire the image after actually moving the object 104 in the real space, or after moving the CG model of the object 104 in the virtual space. An image may be acquired.
Note that in the case of assembly work, an image in the first camera 105 when the object 104 is in the position and posture after assembly may be generated.

  In the figure, steps S101 and S102 are calibration operations and may be performed at least once. Step S103 is a teaching process, and it may be performed at least once according to the change of the processing content. Steps S104 to S108 to be described next are processes executed every time.

(Step S <b> 104) Next, the robot control unit 160 moves the object 104 within the field of view of the first camera 105 and the second camera 106.
(Step S105) Next, the position / orientation detection unit 150 indicates a target position (also referred to as a goal position) of the object 104 in the first camera coordinates from the image created in step S103 using a detection method described later. A position vector T (arrow) ^g _A and a rotation matrix R (hat) ^g _A indicating a target posture (also referred to as goal posture) are detected.
In step S103, when the goal image acquisition unit 140 acquires the goal image by moving the CG model of the object 104 in the virtual space, the position and orientation detection unit 150 is known because the position and orientation in the virtual space are known. May set them as the target position and orientation.

(Step S106) Next, using the first camera 105 and the second camera 106, a position vector T (arrow) ^c _A indicating the position of the object 104 in the current first camera coordinates and rotation indicating the posture The matrix R (hat) ^c _A is detected. This processing method will be described in detail later.
(Step S107) Next, the robot control unit 160 uses the rotation matrix R _{(hat) RA} indicating the posture of the first camera coordinate sigma _A in the robot coordinate sigma _R obtained in step S101, for the hand of the robot The movement amount in the translation direction and the movement amount in the rotation direction are calculated. Then, the robot control unit 160 moves the hand of the robot in the translation direction by the calculated translation amount, and moves it in the rotation direction by the calculated rotation amount Δθ (arrow) _R. For example, the robot control unit 160 calculates the translation amount ΔT (arrow) _{R in} the translation direction and the movement amount Δθ (arrow) _R in the rotation direction according to the following equation (1). However, the robot control unit 160 calculates, for example, each element Δθ _x , Δθ _y , Δθ _z of the movement amount Δθ (arrow) _{R in} the rotation direction by using Expression (5) described later.

Here, θ (arrow) ^c _R and θ (arrow) ^g _R are rotation matrix R (hat) _A (θ (arrow) of angles θ (arrow) ^g _A and θ (arrow) ^c _A obtained by estimation, respectively. ) ^C _A ) and R (hat) _A (θ (arrow) ^g _A ), R (hat) ^c _R = R (hat) _RAR (hat) _A (θ (arrow) ^c _A ) and R (hat) ) according to ^g _R = R _{(hat) RA} R _{(hat) a} (theta ^(Arrow) _{g a),} and converted to the robot coordinate, an angle calculated by using the equation (5) described later.
(Step S108) Next, the first determination unit 170 determines in advance the movement amount ΔT (arrow) _R in the translation direction and the movement amount Δθ (arrow) _{R in the} rotation direction calculated by the robot control unit 160 in step S107. It is determined whether it is smaller than the value. On the other hand, if it is smaller than the predetermined value (YES), the first determination unit 170 determines that the object 104 has reached the position and orientation of the object 104 in the image created in step S103, and ends the process. . When the value is equal to or greater than the predetermined value (NO), the first determination unit 170 determines that the target position and posture have not been reached, and returns the process to step S106. Above, description of this flowchart is complete | finished.

FIG. 7 is a flowchart illustrating an example of the position and orientation detection processing of the object 104 used in steps S102, S105, and S106 of FIG. FIG. 8 is a flowchart subsequent to FIG. FIG. 9 is a flowchart subsequent to FIG.
(Step S201) First, the position / orientation detection unit 150 includes a position vector T (arrow) _A [x _A y _A z _A ] indicating an initial detection position of the object 104 in the first camera coordinates and the second camera coordinates, T (Arrow) _B [x _B y _B z _B ] is set. When the processing flow of the transfer work is determined in advance, for example, the input unit 110 receives an input of the initial detection position from the user. For example, the position / orientation detection unit 150 sets the above-described position vector at the position indicated by the initial detection position received by the input unit 110.

(Step S202) Next, the posture acquisition unit 152 calculates a rotation matrix R (hat) _A indicating the initial detection posture of the object 104 at the first camera coordinates from the rotation angle in each axial direction. Specifically, for example, the posture acquisition unit 152 calculates the rotation matrix R (hat) _{A according} to the following equation (2) using the xyz Euler angle representation as the posture representation of the object 104.

(Step S203) Next, the attitude acquisition unit 152, for example, a rotation matrix R (hat) _B (hat) indicating the attitude at the second camera coordinates from the detected attitude R (hat) _A of the object 104 at the first camera coordinates. = R (Hat) _BA R (Hat) _A ) is calculated according to the following equation (3).

(Step S204) Next, the first dissimilarity calculation unit 151 positions the CG model of the object 104 at a position T (arrow) in the first virtual space that reproduces the space photographed by the first camera 105, for example. an image captured by moving Δ minute each axis positive direction from _a and the attitude R _{(hat) a,} dissimilarity ^E ₊ xA ^a of the image taken by the first camera ^{_{105, E + yA a, E}} + zA a, E ^{+ ΘxA} _A , E ^{+ θyA} _A , and E ^{+ θzA} _A are calculated. Here, the dissimilarities E ^{+ θxA} _A , E ^{+ θyA} _A , and E ^{+ θzA} _A are the first dissimilarities, and the dissimilarities E ^{+ xA} _A , E ^{+ yA} _A , and E ^{+ zA} _A are the third dissimilarities. When Sum of Squared Difference (SSD) is adopted as an example of the degree of difference, the first degree-of-difference calculation unit 151 calculates each degree of difference according to the following equation (4).

Here, W is the number of pixels in the horizontal direction of the image, H is the number of pixels in the vertical direction of the image, I (u, v) is the gradation at the coordinates (u, v) of one image, J (u, v) Is the gradation at the coordinates (u, v) of the other image.
(Step S205) Next, the first dissimilarity calculation unit 151 positions the CG model of the object 104 at the position T (arrow) _A and the first virtual space that reproduces the space photographed by the first camera 105. Degree of difference E ^−xA _A , E ^−yA _A , E ^−zA _A , between the image captured by Δ minute movement in each axis negative direction from the posture R (hat) _A and the image captured by the first camera 105 E ^−θxA _A , E ^−θyA _A , and E ^−θzA _A are calculated. Here, the dissimilarities E ^−θxA _A , E ^−θyA _A , and E ^−θzA _A are the first dissimilarities, and the dissimilarities E ^−xA _A , E ^−yA _A , and E ^−zA _A are the third dissimilarities. is there.

(Step S206) Next, the second dissimilarity calculation unit 153, for example, positions the CG model of the object 104 at the position T (arrow) in the second virtual space that reproduces the space photographed by the second camera 106. Δ is slightly moved in the positive direction of each axis from _B. In addition, the second dissimilarity calculation unit 153 converts, for example, the minute movement amount of the posture R (hat) _B in step S204 into the minute movement amount in the second camera coordinates according to Expression (3). Then, the second difference degree calculation unit 153, for example, a difference between an image photographed by minutely moving the CG model in each axial direction according to the converted minute movement amount and an image photographed by the second camera 106. degrees ^E ₊ xB _^B, calculates _{^{E + yB B, E + zB}} a, E + θxB B, E + θyB B, the ^{E +} θzB _B. Here, the dissimilarity ^{_{E + θxB B, E + θyB}} B, E + θzB B in the second difference degree dissimilarity ^{_{E + xB B, E + yB}} B, the ^{E + zB} _A is a fourth difference degree. Further, the second dissimilarity calculation unit 153 calculates, for example, the rotation angle in each axial direction according to the following equation (5).

Here, [R (hat)] _ij represents an i-row j-column component of the rotation matrix R (hat).
(Step S207) Next, the second dissimilarity calculation unit 153 positions the CG model of the object 104 at a position T (arrow) in the second virtual space that reproduces the space photographed by the second camera 106, for example. Δ is slightly moved from _{B in the} negative direction of each axis. Also, the second dissimilarity calculation unit 153 converts, for example, the minute movement of the posture R (hat) _A in S205 into the minute movement amount in the second camera coordinates. Then, the second difference degree calculation unit 153, for example, a difference between an image photographed by minutely moving the CG model in each axial direction according to the converted minute movement amount and an image photographed by the second camera 106. degrees ^E _-xB ^B, calculates ^{_{E -yB B, E -zB a,}} E -θxB B, E -θyB B, the ^{E -θzB} _B. Here, the dissimilarities E ^−θxB _B , E ^−θyB _B , and E ^−θzB _B are the second dissimilarities, and the dissimilarities E ^−xB _B , E ^−yB _B , and E ^−zB _A are the fourth dissimilarities. is there.

(Step S208) Next, the planar position estimating section 157, for example, in a first camera 105, the _{x A} and _{y A} directional gradients for each dissimilarity when the object 104 is in the T _{(arrow) A,} It calculates according to following Formula (6).

(Step S209) Next, the plane position estimation unit 157 updates the detected positions of the object 104 in the first camera 105 in the x _A and y _A axis directions, for example, according to the following equation (7). Here, Expression (7) represents the update process by the steepest descent method.

Here, C is a constant and is set as appropriate.
(Step S210) Next, the planar position estimating section 157, for example, in the second camera 106, the _{x B} and _{y B-axis} gradient of the respective dissimilarity when the object 104 is in the T _{(Arrow) B} It calculates according to following Formula (8).

(Step S211) Next, the planar position estimating section 157, for example, to update the detection position of the _{x B} and _{y B-axis} direction of the object 104 in the second camera 106 according to the following equation (9). Here, Expression (9) represents the update process by the steepest descent method.

Here, C is a constant and is set as appropriate. Incidentally, when updating the detection position of the x _A-axis direction is constant C, for example, it may be represented by the following equation (10).

Here, delta is the distance in the case where is minutely moving the object in x _A-axis direction. In this case, the value obtained by multiplying the constant C by the gradient of the dissimilarity E in the x-axis direction falls within the range of −Δ to Δ from the following equation (11).

  Thereby, since the update amount is in the range of −Δ to Δ, the plane position estimation unit 157 can reduce the vibration of the horizontal position along with the update when the horizontal position is updated.

(Step S212) Next, the depth update unit 158, a detection position _{z A} of _{z A-axis} direction of the object 104 in the first camera 105 is calculated according using triangulation example, the following equation (12) .

The processing described above is summarized as follows. The plane position estimation unit 157 refers to the plurality of third differences calculated by the first difference calculation unit, and the position of the first plane orthogonal to the optical axis of the camera in the first imaging coordinate system Estimate (x _A , y _A ). The plane position estimation unit 157 refers to the plurality of fourth differences calculated by the second difference calculation unit, and the position of the second plane orthogonal to the optical axis of the camera in the second imaging coordinate system Estimate (x _B , y _B ). The depth updating unit 158 refers to the first plane position (x _A , y _A ) and the 21st plane position (x _B , y _B ) estimated by the plane position estimation unit 157, The depth z _A of the object in the imaging coordinate system is calculated.
The depth updating unit 158 may calculate the detection position z _B in the z _B- axis direction of the object 104 in the second camera 106 according to the following equation (13).

Then, the depth update unit 158 combines the update result and x _A and y _A which are the calculation results of step S209 to determine the detection position in the first camera 105 as T (arrow) _A = [x _A y _A z _A ]. Set to ^T. Here, the only z _A-axis direction is reflected at the detection position of the three-dimensional position obtained by restoring this step in S212 3D. This uses a rotation matrix R (hat) _BA indicating the attitude of the second camera coordinate Σ _A in the second camera coordinate Σ _B when performing the three-dimensional reconstruction, but the rotation matrix R (hat) _BA is There is a high possibility that it contains errors. Therefore, by reflecting the detection position T (arrow) _A in that also improve the accuracy by considering the error is expected z _A-axis direction only the first camera 105, it is possible to prevent a decrease in detection accuracy.

(Step S213) Next, the depth update unit 158 converts the _{z A-axis} direction of the detection positions _{z A} of the object 104 in the first camera 105 set in step S212 to the second camera coordinate. Depth update unit 158 updates the value of the converted detected positions z _B of z _B-axis direction. Then, the depth updating unit 158 determines the detection position in the second camera 106 by combining the update result and the calculation result x _B and y _{B in} step S211 with T (arrow) _B = [x _B y _B z _B ] ^T And set.

(Step S214) Next, the posture estimation unit 154 evaluates, for example, the weighted average of the dissimilarities in the respective axis directions when the posture of the CG model of the object 104 calculated in steps S204 to S207 is slightly moved. function _{^{E + θxA a ', E -θxA}} a', E + θyA a ', E -θyA a', E + θzA a ', E -θzA a' as, is calculated according to the following equation (14) to (16) .

Here, the weight _{w A} of the first camera 105 in the weighted average, determine the weights _{w B} of the second camera 106 for example as in the following Expression (17) and (18).

Here, D is a constant and is appropriately set. By setting the weights as in Expression (17) and Expression (18), for example, the first camera 105 is compared with the second camera 106, and the first camera 105 is more visually recognizing the object 104. When the difference is small and the degree of dissimilarity is small, the weight w _A of the first camera 105 in the weighted average is larger than the weight w _B of the second camera 106 in Expression (17).

On the other hand, when the second camera 106 has better visibility of the object 104 and the dissimilarity is smaller, the weight w _B of the second camera 106 in the weighted average is higher than the first camera 105 from the equation (18). It is greater than the weight _{w a.} As a result, since the ratio of the degree of difference based on the image captured by the camera with the better visibility to the evaluation function increases, even if the posture of the object 104 changes, the change in the posture becomes a change in the evaluation function. Since it is reflected, the posture can be detected robustly.

In step S214, when the image of the first camera 105 when the object 104 is at the target position and orientation is generated, the first camera 105 is brought into a position and orientation with good visibility of the object 104. Assume that it is set. In this case, in the posture detection of the object 104 near the target position and posture, the proportion of the difference degree in the evaluation function based on the image captured by the first camera 105 is high according to the equation (17). As previously described, the rotation matrix R (hat) _BA indicating the posture of the first camera coordinate sigma _A in the second camera coordinate sigma _B estimated in step S202 it is likely to contain errors. However, by setting as described above, in the posture detection of the target object 104 when the target object 104 is in the vicinity of the target position and posture, the degree of difference based on the image captured by the first camera 105 is an evaluation function. The percentage of Therefore, the influence of the error included in the rotation matrix R (hat) _BA is eliminated, and the posture can be detected with high accuracy.

  Note that the processing of the posture estimation unit 154 is not limited to the above example. The posture estimation unit 154 may increase the rate at which the first difference degree contributes to posture estimation as the first difference degree decreases. Similarly, the posture estimation unit 154 may increase the rate at which the second dissimilarity contributes to posture estimation as the second dissimilarity decreases.

  Next, the posture estimation unit 154 calculates the gradient in each axial direction according to the following equation (19).

  (Step S215) Next, the posture estimation unit 154 updates the posture of the object 104 in the first camera 105, for example, according to the following equation (20). Here, Formula (20) represents the update process by the steepest descent method.

Here, C is a constant and is set as appropriate.
(Step S216) The second determination unit 159 performs convergence determination. Specifically, for example, the second determination unit 159 determines the difference between the position and orientation of the object 104 in the first camera 105 calculated in steps S209, S212, and S215 from the previous value. It is determined whether it is smaller. If the difference from the previous value is smaller than the predetermined value (YES), the second determination unit 159 determines that it has converged and ends the process. On the other hand, if the difference from the previous value is greater than or equal to a predetermined value (NO), the second determination unit 159 determines that it has not converged and returns the process to step S203.

  The above is the description of the position and orientation detection processing of the object 104. This detection method is used in steps S102, S105, and S106 in FIG. 6, but in step S106, the convergence determination in step S216 is not performed, and the detection is continued.

  As described above, in the control device 101 according to the present embodiment, the conversion rule determination unit 130 is different from the first camera 105 having good visibility of the target object 104 when the target object for posture estimation is at the target position. A rotation matrix indicating the orientation of the first camera coordinates relative to the first camera 105 in the second camera coordinates relative to the second camera 106 arranged at the position is estimated. Then, when the target 104 is in a predetermined range from the target position (for example, when the target 104 has finished moving to the target position), the posture estimation unit 154 determines that the first degree of difference is the target. The ratio that contributes to the estimation of the posture of the object 104 is set higher than the ratio that the second dissimilarity contributes to the estimation of the posture of the object 104. Then, the posture estimation unit 154 refers to the plurality of first differences calculated by the first difference calculation unit 151 and the plurality of second differences calculated by the second difference calculation unit 153. Thus, the posture of the object 104 is estimated.

Thereby, the posture estimation unit 154 has a ratio in which the second degree of difference affected by the error included in the conversion rule contributes to posture detection when the object 104 is within a predetermined range from the target position. Since it becomes relatively small, it is possible to reduce the influence of the error included in the conversion rule on the posture estimation of the object.
As a result, the posture estimation unit 154 can improve the accuracy of posture estimation of the target object.
In addition, the first camera 105 has good visibility of the object 104 when the object 104 is at the target position, and the first camera 105 when the object 104 is in a predetermined range from the target position. Since the ratio that the degree of difference contributes to posture detection is relatively large, the posture accuracy of the target object when the target object 104 is in a predetermined range from the target position can be improved.

  In addition, posture estimation using the conventional stereo camera described in the background art requires calculation by calculating the relative positional relationship between the cameras many times. There was a problem of becoming. Since the control apparatus 101 in this embodiment does not require a process of calculating the relative positional relationship between the cameras, the control apparatus 101 can easily estimate the posture of the object.

  In addition, in the conventional technology using the stereo camera described in the background art, a technique used when detecting the position and orientation of an object from an image photographed by the camera is appropriately selected according to the shape of the object, the state of the surface, and the like. There is also a problem that it is necessary to select and cannot be applied to various objects.

On the other hand, the control device 101 according to the present embodiment performs the three-dimensional reconstruction from the position of the target object 104 detected by each camera, and thereby the position z of the target object 104 in the first camera 105 in the optical axis direction of the first camera 105. _A is calculated. Here, if the position in the optical axis direction of the first camera 105 is estimated from the degree of difference in the optical axis direction of the first camera 105, the change in the degree of difference in the optical axis direction of the first camera 105 is small. The estimation error of the axial position is larger than the error included in the conversion rule. On the other hand, the control apparatus 101 according to the present embodiment refers to the position of the first plane estimated by the plane position estimation unit 157 and the position of the second plane estimated by the plane position estimation unit 157. Since the estimation of the position of the first camera 105 in the optical axis direction is calculated, the estimation accuracy of the position in the optical axis direction can be improved.

The posture estimation unit 154 determines a weight related to the second dissimilarity (for example, the weight w _B of the second camera 106) when the object 104 is within a predetermined position range from the target position. Even if the weighting factor by which the second difference is multiplied is changed from the movement start time of the target object 104 to the time when the target object 104 reaches the predetermined position range from the target position so as to fall within the determined range. Good. Specifically, for example, the posture estimation unit 154 moves the object 104 so that the weight related to the second dissimilarity becomes 0 when the object 104 is in a predetermined position range from the target position. The weight related to the second difference may be changed from the start time to the time when the target object 104 reaches a predetermined position range from the target position. Specifically, for example, the posture estimating unit 154, as shown in FIG. 10, the weight w _B the weight w _A and the second camera 106 of the first camera 105 may be temporally varied. Figure 10 is an example of time change of the weight _{w B} the weight _{w A} and the second camera 106 of the first camera 105. The vertical axis is the weight w, and the horizontal axis is the elapsed time. In the figure, the weight w _A of the first camera 105 is constant and 1 regardless of time. On the other hand, the weight w _B of the second camera 106 is 1 at the elapsed time 0 (that is, at the start of movement of the robot hand position), but decreases with the passage of time. When the target position and posture are reached, the weight w _A of the first camera 105 remains 1, whereas the weight w _B of the second camera 106 is zero. In this way, the posture estimation unit 154 is a weighting factor that multiplies the second degree of difference in a period from the movement start time of the object 104 to the time when the object 104 reaches a predetermined position range from the target position. May be reduced over time.

As a result, when the work related to the object is completed, the weight related to the second dissimilarity affected by the error included in the rotation matrix R (hat) _BA can be brought close to 0 or 0, so that the rotation matrix R ( (Hat) The influence of the error included in the _{BA on} the posture estimation of the object 104 can be further reduced. As a result, the posture estimation unit 154 can estimate the posture of the object with high accuracy.

In addition, the posture estimation unit 154 may decrease the weight related to the second degree of difference (for example, the weight w _B of the second camera 106) as time passes, as illustrated in FIG. As a result, as time elapses (for example, as the object 104 approaches the target position), the second degree of difference affected by the error included in the rotation matrix R (hat) _BA is used for posture detection. Since the contribution ratio is small, the influence of the error included in the rotation matrix R (hat) _{BA on} the posture estimation of the object 104 can be reduced. As a result, the posture estimation unit 154 can estimate the posture of the object 104 with high accuracy.

In addition, when the first camera 105 has better visibility of the object 104 than the second camera 106 and the position of the object 104 moves, the posture estimation unit 154 moves the object 104 per unit time. The weight related to the second dissimilarity (for example, the weight w _B of the second camera 106) may be decreased as becomes smaller. Here, the amount of movement of the object 104 per unit time is the largest when the object 104 starts to move, and then gradually decreases with the passage of time. Therefore, as time elapses (for example, as the object 104 approaches the target position), the second difference degree affected by the error included in the rotation matrix R (hat) _BA is used for posture detection. Since the contribution ratio is small, the influence of the error included in the rotation matrix R (hat) _{BA on} the posture estimation of the object 104 can be reduced. As a result, the posture estimation unit 154 can estimate the posture of the object with high accuracy.

Further, when the first camera 105 has better visibility of the object 104 than the second camera 106, the posture estimation unit 154, as shown in FIG. The weight w _B ) of the camera 106 may be reduced as the object approaches the target position. As a result, as the work on the object approaches, the ratio of the second dissimilarity influenced by the error included in the rotation matrix R (hat) _BA contributes to the detection of the posture becomes smaller. (Hat) The influence of the error included in the _{BA on} the posture estimation of the object 104 can be reduced. As a result, the posture estimation unit 154 can estimate the posture of the object with high accuracy.

<Second Embodiment>
Next, the second embodiment will be described. In the robot 1b of the second embodiment, a camera that captures an image from which an evaluation function having a steeper gradient of the evaluation function near the target position is derived is used as a reference camera for determining the position and orientation. Set. The configuration of the robot 1b of the second embodiment is such that the control device 101 is changed to a control device (attitude detection device) 101b with respect to the robot 1 of the first embodiment.

  FIG. 11 is a schematic block diagram illustrating a configuration of the control device 101b in the second embodiment. In this figure, elements common to those in FIG. 4 are denoted by the same reference numerals, and detailed description thereof is omitted. The configuration of the control device 101b in FIG. 11 is a configuration in which the processing unit 120 is changed to a processing unit 120b with respect to the configuration of the control device 101 in FIG. In the configuration of the processing unit 120b in FIG. 11, a positioning reference camera determination unit 180 is added to the configuration of the processing unit 120 in FIG. 4, the goal image acquisition unit 140 is added to the goal image acquisition unit 140b, and the position and orientation detection unit 150. Is changed to the position / orientation detection unit 150b.

  The goal image acquisition unit 140b has the same function as the goal image acquisition unit 140 in the first embodiment, but differs in the following points. The goal image acquisition unit 140b includes information indicating an image (hereinafter also referred to as a first goal image) taken by the first camera 105 when the object 104 is in the target position and orientation, and the first image. Information indicating an image (hereinafter also referred to as a second goal image) obtained by photographing the object 104 by the second camera 106 is output to the positioning reference camera determination unit 180.

The positioning reference camera determination unit 180 is required to have a predetermined accuracy based on the position and orientation of the CG model of the object 104 existing in the first virtual space that reproduces the space in which the first goal image is captured, for example. The degree of difference E _A ⁺ between the image taken by moving the CG model Δ slightly in the positive angular direction around the axis and the first goal image is calculated.
In addition, the positioning reference camera determination unit 180 has a predetermined accuracy based on the position and orientation of the CG model of the object 104 existing in the first virtual space that reproduces the space where the first goal image is captured, for example. The degree of difference E _A ⁻ between the image taken by moving the CG model Δ slightly in the negative angular direction around the required axis and the first goal image is calculated.

In addition, the positioning reference camera determination unit 180 has a predetermined accuracy based on the position and orientation of the CG model of the object 104 existing in the second virtual space that reproduces the space where the second goal image is captured, for example. The degree of difference E _B ⁺ between the image captured by moving the CG model Δ slightly in the positive angular direction around the required axis and the second goal image is calculated.
In addition, the positioning reference camera determination unit 180 has a predetermined accuracy based on the position and orientation of the CG model of the object 104 existing in the second virtual space that reproduces the space where the second goal image is captured, for example. The degree of difference E _B ⁻ between the image taken by moving the CG model by Δ minutely around the required axis in the negative angle direction and the second goal image is calculated.

The positioning reference camera determining unit 180, for example, degree-of-difference _E ^{A +} and dissimilarity _{E A} ^- gradient ^{_{G A (= (E A +}} -E A -) / 2Δ) is calculated. The positioning reference camera determining unit 180, for example, degree-of-difference _E ^{B +} and dissimilarity _{E B} ^- gradient ^{_{G B (= (E B +}} -E B -) / 2Δ) is calculated. The positioning reference camera determining unit 180, for example, of the gradient G _A and gradient G _B, it is set in the camera as a reference in determining the position and orientation of the camera used to slope to derive a better steep.
Then, the positioning reference camera determination unit 180 outputs reference camera information indicating the set reference camera to the position / orientation detection unit 150b.

  The position / orientation detection unit 150b has the same function as the position / orientation detection unit 150 in the first embodiment, but differs in the following points. The position / orientation detection unit 150b performs the same process as in the first embodiment when the reference camera indicated by the reference camera information is the first camera 105. When the reference camera indicated by the reference camera information is the second camera 106, the position / orientation detection unit 150b performs a process of replacing the first camera 105 and the second camera 106 in the first embodiment.

FIG. 12 is a flowchart illustrating an example of the overall processing flow of the control apparatus 101 according to the second embodiment.
(Step S301) First, the conversion rule determination unit 130 obtains a position vector T _{(arrow) BA} indicating the position of the origin of the first camera coordinate sigma _A in the second camera coordinate sigma _B. The conversion rule determining unit 130 uses a vector obtained by multiplying the acquired position vector T (arrow) _BA by a minus value as a position vector T (arrow) indicating the position of the origin of the second camera coordinate Σ _B in the first camera coordinate Σ _A. ) Calculated as _AB .
The conversion rule determination unit 130 obtains the rotation matrix R (hat) _BA indicating the posture of the first camera coordinate sigma _A in the second camera coordinate sigma _B. The conversion rule determining unit 130 calculates the inverse matrix of the acquired rotation matrix R (hat) _BA as a rotation matrix R (hat) _AB indicating the attitude of the second camera coordinates Σ _B in the first camera coordinates Σ _A. .
The conversion rule determination unit 130 obtains the rotation matrix R (hat) _RA indicating the posture of the first camera coordinate sigma _A in the robot coordinate sigma _R. Then, the conversion rule determining unit 130 multiplies the obtained rotation matrix R (hat) _RA by the rotation matrix R (hat) _AB calculated from the left to obtain the posture of the second camera coordinate Σ _B in the robot coordinate Σ _R. A rotation matrix R (hat) _{RB shown} is calculated.
(Step S302) Next, the goal image acquisition unit 140b captures the first goal image captured from the first camera 105 and the second camera 106 when the object 104 is at the target position and orientation. The second goal image is acquired.
(Step S303) Next, the positioning reference camera determination unit 180 determines which camera is used as a reference for positioning.
(Step S304) Next, the position / orientation detection unit 150b determines whether or not the determination by the positioning reference camera determination unit 180 is a determination to position the first camera 105 as a reference.
If the position / orientation detection unit 150b determines to position the first camera 105 as a reference (YES), the process proceeds to step S305, and the processing unit 120b executes the processes of steps S305 to S309. On the other hand, if the position / orientation detection unit 150b determines to position the second camera 106 as a reference (NO), the process proceeds to step S310, and the processing unit 120b executes the processes of steps S310 to S314.

(Step S <b> 305) The robot control unit 160 moves the object 104 within the field of view of the first camera 105 and the second camera 106.
(Step S306) Next, the position / orientation detection unit 150b indicates a position vector T (arrow) ^g _A indicating the position of the object 104 in the first camera coordinates from the first goal image acquired in step S301, and the attitude. A rotation matrix R (hat) ^g _A is detected.

(Step S307) Next, the position / orientation detection unit 150b uses the first camera 105 and the second camera 106, and a position vector T (arrow) indicating the position of the object 104 in the current first camera coordinates. ^c _A and a rotation matrix R (hat) indicating a posture ^c _A are detected.
(Step S308) Next, the robot control unit 160 uses the rotation matrix R _{(hat) RA} indicating the posture of the first camera coordinate sigma _A in the robot coordinate sigma _R obtained in step S304, for the hand of the robot The movement amount in the translation direction and the movement amount in the rotation direction are calculated. Then, the robot controller 160 moves the hand of the robot in the translation direction by the calculated translational movement amount ΔT (arrow) _R, and moves it in the rotational direction by the calculated rotational movement amount Δθ (arrow) _R.

(Step S309) Next, the first determination unit 170 determines in advance the movement amount ΔT (arrow) _R in the translation direction and the movement amount Δθ (arrow) _{R in the} rotation direction calculated by the robot control unit 160 in step S309. It is determined whether it is smaller than the value. If it is smaller than the predetermined value (YES), the first determination unit 170 determines that the object 104 has reached the position and orientation of the object 104 in the first goal image created in step S301, and performs the processing. finish. If the value is equal to or greater than a predetermined value (NO), the first determination unit 170 determines that the target position and posture have not been reached, and returns the process to step S307. Note that the robot control unit 160 calculates, for example, each element Δθ _x , Δθ _y , Δθ _z of the movement amount Δθ (arrow) _{R in} the rotation direction using Expression (5).

(Step S <b> 310) Next, the robot control unit 160 moves the object 104 within the field of view of the first camera 105 and the second camera 106.
(Step S311) Next, the position / orientation detection unit 150b indicates a position vector T (arrow) ^g _B indicating the position of the object 104 in the second camera coordinates from the second goal image acquired in step S301, and the attitude. A rotation matrix R (hat) ^g _B is detected.

(Step S312) Next, the position / orientation detection unit 150b uses the first camera 105 and the second camera 106, and a position vector T (arrow) indicating the position of the object 104 in the current second camera coordinates. ^c _B and a rotation matrix R (hat) indicating the posture ^c _B are detected.
(Step S313) Next, the robot control unit 160 uses the rotation matrix R _{(hat) RB} indicating the posture of the second camera coordinate sigma _B in the robot coordinate sigma _R obtained in step S311, for the hand of the robot The movement amount in the translation direction and the movement amount in the rotation direction are calculated. Then, the robot controller 160 moves the hand of the robot in the translation direction by the calculated translational movement amount ΔT (arrow) _R, and moves it in the rotational direction by the calculated rotational movement amount Δθ (arrow) _R.

(Step S314) Next, the first determination unit 170 determines in advance the movement amount ΔT (arrow) _R in the translation direction and the movement amount Δθ (arrow) _{R in the} rotation direction calculated by the robot control unit 160 in step S315. It is determined whether it is smaller than the value. If it is smaller than the predetermined value (YES), the first determination unit 170 determines that the object 104 has reached the position and orientation of the object 104 in the second goal image created in step S301, and performs the processing. finish. When the value is equal to or greater than a predetermined value (NO), the first determination unit 170 determines that the target position and posture have not been reached, and returns the process to step S312. Above, description of this flowchart is complete | finished. Note that the robot control unit 160 calculates, for example, each element Δθ _x , Δθ _y , Δθ _z of the movement amount Δθ (arrow) _{R in} the rotation direction using Expression (5).

Above, the control device 101 in the second embodiment, the positioning reference camera determining unit 180 calculates the gradient G _A and gradient G _B, of the gradient G _A and gradient G _B, gradient derives towards steep The camera used for this is set as a reference camera for determining the position and orientation.
Thereby, in addition to the effect in 1st Embodiment, there exist the following effects. In the vicinity of the target position and orientation, the ratio at which the degree of difference derived from the image captured by the set reference camera contributes to the evaluation function increases. At that time, since the change in the dissimilarity becomes larger near the target position and posture than when another camera is used as a reference, the change in the evaluation function also becomes larger, and the change in the position and posture of the object 104 to be updated. Since the amount increases, the object 104 can be made to reach the target position and posture earlier.

When calculating the degree of difference, the positioning reference camera determination unit 180 calculates Δ from the position and orientation of the CG model of the object 104 in a positive or negative angular direction around an axis that requires a predetermined accuracy. Although it was taken with a small amount of movement, it is not limited to this. The positioning reference camera determining unit 180 may perform shooting by moving Δ slightly in the positive or negative direction with respect to the axial direction requiring a predetermined accuracy.
Further, according to the work content of the robot 1b, the input unit 110 inputs which axis or which axis the Δ minute movement is to be performed, the input unit 110 receives the input, and the processing unit 120b. May be set by the one received by the input unit 110.

Therefore, the positioning reference camera determination unit 180 has a predetermined accuracy based on the position and orientation of the CG model of the object 104 existing in the first virtual space that reproduces the space in which the first goal image is captured. A fifth dissimilarity indicating a dissimilarity between the first goal image and an image captured by moving the CG model as determined in advance around or in the required axis is calculated.
The positioning reference camera determination unit 180 is an axis that requires a predetermined accuracy from the position and orientation of the CG model of the object 104 existing in the second virtual space that reproduces the space in which the second goal image is captured. A sixth dissimilarity indicating the dissimilarity between the image captured by moving the CG model as determined in advance around or in the axial direction and the second goal image is calculated.
In addition, the positioning reference camera determination unit 180 has a gradient of the fifth difference in the movement direction when the fifth difference is calculated, and a sixth difference in the movement direction when the sixth difference is calculated. By comparing with the gradient, a camera serving as a reference for posture estimation is determined. At that time, the positioning reference camera determination unit 180 may newly set the determined reference camera as the first camera.

<Third Embodiment>
Subsequently, a third embodiment will be described. There is fluctuation in the time required for the process of determining the amount of change in the position and posture of the robot body 102 from the first captured image and the second captured image before a predetermined time. Therefore, in consideration of the fluctuation, a series of processes from when the apparatus starts capturing the first captured image and the second captured image to determining the amount of change in the position and orientation of the robot main body 102 are involved. When the control device starts to control the robot body 102 after waiting for the maximum time (for example, 15 milliseconds), it takes a long time for the object 104 to reach the target position and posture. There's a problem. Therefore, in the present embodiment, it is an object of the present embodiment to shorten the time required for the object 104 to reach the target position and posture.

  In this embodiment, as an example, an average time required for a series of processes from when the apparatus starts capturing the first captured image and the second captured image to determining the amount of change in the position and orientation of the robot body 102 In the following description, it is assumed that the average time for image processing (hereinafter also referred to as image processing time) is 10 milliseconds, and the time required for the series of processing (hereinafter also referred to as image processing time) is ± 5 milliseconds. In the present embodiment, as an example, it is assumed that the position and orientation control of the robot is performed every 1 millisecond.

  FIG. 13 is a schematic block diagram illustrating the configuration of the control device 101c according to the third embodiment. In this figure, elements common to those in FIG. 4 are denoted by the same reference numerals, and detailed description thereof is omitted. The configuration of the control device 101c in FIG. 13 is different from the configuration of the control device 101 in FIG. 4 in that the processing unit 120 is changed to a processing unit 120c. The configuration of the processing unit 120c in FIG. 13 is obtained by adding a target position / posture determination unit 200 to the configuration of the processing unit 120 in FIG. 4 and changing the robot control unit 160 to a robot control unit 160c. .

  The target position / posture determination unit 200 is determined in advance from the current time based on the position and posture of the object estimated by the position / posture detection unit 150 and the position and posture when the target is at the target position. The amount of change in position and orientation in the robot coordinate system after elapse of time (for example, 10 milliseconds) is determined. Here, the predetermined time is, for example, an average time from the start of capturing the first captured image and the second captured image to the determination of the change amount.

Specifically, for example, the target position / posture determination unit 200 determines a translation amount of movement ΔT (arrow) _R and a rotation amount of movement Δθ (arrow) _R by the same processing as in the first embodiment. Target position posture determination unit 200, a movement amount [Delta] T (arrow) _R translational direction, each time determining the amount of movement [Delta] [theta] (arrow) _R rotation direction, the movement amount [Delta] T (arrow) _R translational direction, the direction of rotation Movement amount Δθ (arrow) _R is output to the robot controller 160 c and the first determination unit 170.

  The robot control unit 160c, based on the latest change amount among the change amounts determined by the target posture determination unit, for each time interval (for example, 1 millisecond) for controlling the robot body 102, The position and posture of the robot body 102 after (milliseconds) are determined, and the robot body 102 is controlled so as to be the determined position and posture. At that time, the robot control unit 160c determines that the change amount is determined from the first captured image and the second captured image within the predetermined time (here, 10 milliseconds as an example). The position and posture of the robot body 102 are determined so that the robot moves with acceleration and deceleration until a time determined based on the shortest time, and the robot body 102 so that the robot body 102 moves at a constant speed during the subsequent time. The position and orientation of 102 are determined.

  Here, in this embodiment, the time determined based on the shortest time until the apparatus determines the change amount from the first captured image and the second captured image is an example. This is the time obtained by adding the time to capture the first captured image and the second captured image to the shortest time from the captured image and the second captured image until the change amount is determined. Since the imaging time of the first captured image and the second captured image is constant, it is determined based on the shortest time until the device determines the amount of change from the first captured image and the second captured image. As an example, the obtained time is the shortest time from the start of capturing the first captured image and the second captured image until the change amount is determined.

An example of the processing for controlling the position and orientation of the robot main body 102 by the robot control unit 160c will be described with reference to FIG. FIG. 14 is an example of the time change of the x coordinate of the hand position of the robot body 102 in the robot coordinate system. In the figure, the robot body moves while accelerating along the x coordinate of the hand of the robot body 102 in the period T1, moves at a constant speed along the x coordinate of the hand of the robot body 102 in the period T2, and moves to the robot body in the period T3. It is shown that the robot moves while decelerating along the x-coordinate of the hand of 102, and moves while accelerating along the x-coordinate of the hand of the robot body 102 in the period T4. Further, after 10 milliseconds, the second translational movement amount ΔT (arrow) _R and the rotational movement amount Δθ (arrow) _R are determined, and after 15 milliseconds, the third translational movement amount ΔT ( (Arrow) _R and the movement amount Δθ (arrow) _{R in the} rotation direction are determined.

In the example of FIG. 14, in the period T1 and the period T2, the robot control unit 160c uses the translation amount ΔT (arrow) _R in the translation direction and the movement amount Δθ (arrow) _R in the rotation direction, which are determined first, as 1 The x coordinate of the hand position of the robot body 102 after milliseconds is determined. Thereby, the x coordinate in the period T1 and the period T2 is obtained every 1 millisecond. The robot control unit 160c moves the hand of the robot main body 102 to the x coordinate of the determined hand position after 1 millisecond over 1 millisecond. The robot control unit 160c repeats the determination of the x coordinate and the movement process of the hand of the robot body 102 every time 1 millisecond elapses. Accordingly, the bot control unit 160c moves the hand position of the robot body 102 while accelerating along the x coordinate in the first half of the period T1, and in the second half of the period T2, the robot The hand position of the main body 102 can be moved at a constant speed along the x coordinate.

At time 10 milliseconds, the robot controller 160c receives the second translation amount ΔT (arrow) _R and the rotational amount Δθ (arrow) _R from the target position / posture determination unit 200. Using the second translational movement amount ΔT (arrow) _R and the rotational movement amount Δθ (arrow) _R , the x coordinate of the hand position of the robot body 102 after 1 millisecond is determined. Thereby, the x coordinate in the period T3 is obtained every 1 millisecond. The robot control unit 160c moves the hand of the robot main body 102 to the x coordinate of the determined hand position after 1 millisecond over 1 millisecond. The robot control unit 160c repeats the determination of the x coordinate and the movement process of the hand of the robot body 102 every time 1 millisecond elapses. Thereby, the robot control unit 160c can move the hand position of the robot body 102 while decelerating along the x-coordinate in the period T3.

At time 15 milliseconds, the robot controller 160c receives the third translational movement amount ΔT (arrow) _R and the rotational movement amount Δθ (arrow) _R from the target position and orientation determination unit 200. Using the third movement amount ΔT (arrow) _{R in} the translation direction and the movement amount Δθ (arrow) _R in the rotation direction, the x coordinate of the hand position of the robot body 102 after 1 millisecond is determined. Thereby, the x coordinate in the period T4 is obtained every 1 millisecond. The robot control unit 160c moves the hand of the robot main body 102 to the x coordinate of the determined hand position after 1 millisecond over 1 millisecond. The robot control unit 160c repeats the determination of the x coordinate and the movement process of the hand of the robot body 102 every time 1 millisecond elapses. Thereby, the robot control unit 160c can move the position of the hand of the robot body 102 while accelerating along the x coordinate in the period T4.

  Next, details of the x coordinate determination process of the hand position of the robot body 102 will be described. The robot control unit 160c determines the x coordinate of the hand position of the robot body 102 according to the following equation (21).

Here, t is the time, T is the average image processing time, v ₁ is the velocity along the x coordinate of the hand position of the robot body 102, and v ₀ is the hand position of the robot body 102 at the image processing average time T. in initial velocity along the x coordinate, x ₀ is the initial position of the x coordinate in the image processing average time T.
Here, _{v 1} is expressed by the following equation (22).

Δx is the x-coordinate component of the translation amount ΔT (arrow) _{R in} the translation direction, n is a positive integer (here, 1 as an example), and ΔT is a robot control cycle (in this embodiment, 1 millisecond as an example) is there.
The robot control unit 160c determines the y-coordinate, z-coordinate, and rotation angles θx, θy, and θz of the hand position of the robot body 102 by the same process.

FIG. 15 is a flowchart illustrating an example of the overall processing flow of the control apparatus 101c according to the third embodiment. Steps S401 to S406 are the same as steps S101 to S106 in FIG. Also, step S409 is the same as step S108 in FIG.
(Step S407) Next, the target position posture determination unit 200 uses the rotation matrix R _{(hat) RA} indicating the posture of the first camera coordinate sigma _A in the robot coordinate sigma _R obtained in step S101, the hand of the robot A translation amount ΔT (arrow) _{R in} the translation direction and a movement amount Δθ (arrow) _R in the rotation direction are determined. The robot control unit 160c outputs the determined translation amount ΔT (arrow) _R in the translation direction and rotation amount Δθ (arrow) _R in the rotation direction to the first determination unit 170 and the robot control unit 160c.

FIG. 16 is a flowchart illustrating an example of processing of the robot control unit 160c in the third embodiment.
(Step S501) The robot control unit 160c determines whether or not the translational movement amount ΔT (arrow) _R and the rotational movement amount Δθ (arrow) _R are newly determined by the target position and orientation determination unit 200. If it is newly determined (YES), the process proceeds to step S502. If it has not been newly determined (NO), the process proceeds to step S503.
(Step S502) The robot control unit 160c uses the new translational movement amount ΔT (arrow) _R and the rotational movement amount Δθ (arrow) _R to determine the robot hand position and the robot hand position after 1 millisecond. (Hereinafter also referred to as robot hand posture) is determined.
(Step S503) The robot controller 160c uses the translation amount ΔT (arrow) _{R in} the translational direction and the movement amount Δθ (arrow) _R in the rotational direction so far to determine the robot hand position and the robot hand position after 1 millisecond. Determine posture.
(Step S504) Next, the robot control unit 160c moves the robot hand so that the robot hand position and the robot hand posture become 1 ms after the robot hand is determined.
(Step S505) Next, in the robot controller 160c, the first determination unit 170 determines that the latest translational movement amount ΔT (arrow) _R and rotational movement amount Δθ (arrow) _R are smaller than predetermined values. Is determined. If it is determined that the value is smaller than the predetermined value (YES), the robot control unit 160c ends the process. If it is not determined that the value is smaller than the predetermined value (NO), the robot control unit 160c returns to the process of step S501. Above, the process of this flowchart is complete | finished.

As described above, in the third embodiment, the robot controller 160c updates the translational movement every time the translational movement amount ΔT (arrow) _R and the rotational movement amount Δθ (arrow) _R are updated. Using the amount ΔT (arrow) _R and the amount of movement Δθ (arrow) _R in the rotation direction, the robot hand position and the robot hand posture are determined at the robot control cycle interval. Then, the robot control unit 160c controls the robot main body 102 so that the determined robot hand position and robot hand position are obtained in the robot control cycle. At that time, the robot control unit 160c accelerates and decelerates the robot's hand until the shortest time required for image processing (for example, 5 milliseconds) out of the predetermined average image processing time (for example, 10 milliseconds). The robot hand position and the robot hand posture are determined so as to move, and the robot hand position and the robot hand posture are determined so that the robot hand moves at a constant speed during the subsequent time (for example, 5 to 10 milliseconds). As a result, even if the image processing time fluctuates by ± 5 milliseconds and the time required for image processing varies between 5 milliseconds and 10 milliseconds, the robot's hand moves at a constant speed during that time. The acceleration control or deceleration control of the robot can be started with the initial speed as the speed when moving at the constant speed. As a result, the robot control unit 160c controls the robot using the translational movement amount ΔT (arrow) _R and the rotational movement amount Δθ (arrow) _R obtained by the image processing upon completion of the image processing. Therefore, in addition to the effect of the first embodiment, it is possible to shorten the time until the object 104 is set to the target position and the target posture.

  Note that in each embodiment, the posture estimation unit 154 mixes a weighting factor by which the first difference is multiplied and a weighting factor by which the second difference is multiplied into each of the first captured image and the second captured image. The noise component may be changed according to the noise component. Here, the noise component is a noise component resulting from a noise signal mixed after the process of photoelectric conversion by the image sensor of the imaging apparatus. The noise component is, for example, a noise component derived from a random noise signal mixed in an image signal after photoelectric conversion when the image sensor of the imaging apparatus performs photoelectric conversion. Specifically, for example, the posture estimation unit 154 applies a weighting factor to be multiplied by the first difference degree and a weighting factor to be multiplied by the second difference degree to each of the first captured image and the second captured image. It is good also as the reciprocal of dispersion | distribution of the normalization random noise which is mixed. At that time, the posture estimation unit 154 determines, for example, an inter-frame dissimilarity, which is an image dissimilarity between different frames of the first camera 105 and the second camera 106, from the first captured image and the second captured image. You may calculate as a substitute of dispersion | distribution of the normalization random noise mixed in each of the captured image. More preferably, the posture estimation unit 154 mixes the degree of difference between the image of the previous frame of each camera and the image of the current frame in each of the first captured image and the second captured image. It may be calculated as a substitute for noise dispersion. This is because the variance is proportional to the difference between frames. Here, the difference between frames is calculated, for example, by calculating the absolute value of the difference between the pixel value of the image one frame before and the pixel value of the pixel corresponding to the pixel value in the current frame image, This is the sum of the values obtained by calculation.

  In addition, in each embodiment, when there are a plurality of objects, the following may be performed. The first dissimilarity calculation unit 151 identifies the image areas of all objects other than the object that is the target of position / orientation estimation in the first virtual space, and removes the identified image area from the calculation target to perform the first operation. The degree of difference may be calculated. In addition, the second dissimilarity calculation unit 153 identifies the image areas of all the objects other than the object that is the position and orientation estimation target in the second virtual space, and excludes the identified image area from the calculation target. A second degree of difference may be calculated.

  Specifically, for example, the first difference calculation unit 151 and the second difference calculation unit 153 may perform the following processing. For example, the first dissimilarity calculation unit 151 and the second dissimilarity calculation unit 153 each include all the objects other than the target object for position and orientation estimation in each of the first and second virtual spaces. When all the images slightly moved in the positive and negative directions are arranged, a union of image areas is extracted for each object, and a mask image in which all the unions are masked is acquired. The first dissimilarity calculation unit 151 and the second dissimilarity calculation unit 153, for example, use the acquired mask image to remove objects other than the object that is the target of position and orientation estimation, and are different from each other. Calculate the degree.

  For the sake of simplicity, the above-described processing will be described by taking as an example the case where the CG image is a planar image. FIG. 17 is a diagram for explaining the mask image. In the figure, in the first virtual space, a CG image G131-1 in which both the first object and the second object are moved by Δ minute from the current position, and CG that has both moved Δ by minute from the current position. An example of an image G131-2, a CG image G131-3 that is Δ-moved to the right from the current position, and a CG image G131-4 that is Δ-moved to the left from the current position are shown. In the CG image G131-1, an image area R132-1 of the first object and an image area R133-1 of the second object are shown. Here, for the sake of simplicity, the image areas R132-2 to R132-2 and the image areas R133-2 to R133-2 of the first object in the other CG images G131-2 to G131-2 are not shown. In the mask image G134, a mask area R135 of the second object which is an union of the image areas R133-1 to R4 of the second object in the CG images G131-1 to G13-4 described above is shown. In the mask image G135, a mask area R137 of the first object that is an union of the image areas R132-1 to R4 of the first object in the CG images G131-1 to G13-4 described above is shown. Yes.

In the case of the example in the figure, the first difference calculation unit 151 acquires, for example, the mask image G134 in FIG. Then, the first dissimilarity calculation unit 151 calculates the first dissimilarity regarding the first object, for example, excluding the region of the mask image G134. The first difference calculation unit 151 acquires, for example, the mask image G136 of FIG. Then, the first dissimilarity calculation unit 151 calculates the first dissimilarity regarding the second object, for example, excluding the region of the mask image G136.
Similarly, the second difference calculation unit 153 acquires, for example, the mask image G134 of FIG. And the 2nd dissimilarity calculation part 153 calculates the 2nd dissimilarity regarding a 1st target object except the field of mask image G134, for example. For example, the second dissimilarity calculation unit 153 acquires the mask image G136 of FIG. Then, the second dissimilarity calculation unit 153 calculates the second dissimilarity regarding the second object, excluding the region of the mask image G136.

In each embodiment, the depth update unit 158 of the position estimation unit 156 calculates the detection position z _A in the z _A axis direction of the object 104 in the first camera 105 using triangulation. It is not limited to. The position estimation unit 156 performs the first imaging based on the plurality of third differences calculated by the first difference calculation unit and the plurality of fourth differences calculated by the second difference calculation unit. You may estimate the depth of the target object in a coordinate system.

Note that a system including a plurality of devices may process each process of the control device (101 or 101b) of each embodiment in a distributed manner by the plurality of devices.
Also, a program for executing each process of the control device (101 or 101b) of each embodiment is recorded on a computer-readable recording medium, and the program recorded on the recording medium is read into a computer system and executed. By doing so, you may perform the various processes mentioned above which concern on a control apparatus (101 or 101b).

  Here, the “computer system” may include an OS and hardware such as peripheral devices. Further, the “computer system” includes a homepage providing environment (or display environment) if the WWW system is used. The “computer-readable recording medium” means a flexible disk, a magneto-optical disk, a ROM, a writable nonvolatile memory such as a flash memory, a portable medium such as a CD-ROM, a hard disk built in a computer system, etc. This is a storage device.

  Further, the “computer-readable recording medium” means a volatile memory (for example, DRAM (Dynamic) in a computer system that becomes a server or a client when a program is transmitted via a network such as the Internet or a communication line such as a telephone line. Random Access Memory)) that holds a program for a certain period of time is also included. The program may be transmitted from a computer system storing the program in a storage device or the like to another computer system via a transmission medium or by a transmission wave in the transmission medium. Here, the “transmission medium” for transmitting the program refers to a medium having a function of transmitting information, such as a network (communication network) such as the Internet or a communication line (communication line) such as a telephone line. The program may be for realizing a part of the functions described above. Furthermore, what can implement | achieve the function mentioned above in combination with the program already recorded on the computer system, what is called a difference file (difference program) may be sufficient.

  As mentioned above, although embodiment of this invention was explained in full detail with reference to drawings, a specific structure is not restricted to this embodiment. Each configuration in each embodiment, a combination thereof, and the like are examples, and the addition, omission, replacement, and other changes of the configuration can be made without departing from the spirit of the present invention. Further, the present invention is not limited by the embodiments, and is limited only by the scope of the claims.

101, 101b Control device (attitude detection device)
DESCRIPTION OF SYMBOLS 102 Robot main body 103 Gripper 104 Target object 105 1st camera (1st imaging device)
106 Second camera (second imaging device)
110 Input unit 120 Processing unit 120, 120b
130 Conversion Rule Determination Unit 140, 140b Goal Image Acquisition Unit 145 Current Image Acquisition Unit 150, 150b Position / Orientation Detection Unit 151 First Dissimilarity Calculation Unit 152 Posture Acquisition Unit 153 Second Dissimilarity Calculation Unit 154 Posture Estimation Unit 156 Position Estimator 157 Plane position estimator 158 Depth updater 159 Second determination unit 160, 160c Robot controller 170 First determination unit 180 Positioning reference camera determination unit 190 Storage unit 200 Target position / posture determination unit

Claims (25)

The degree of difference between the first captured image obtained by imaging the object by the first imaging device and the model image of the object in the first virtual space corresponding to the space imaged by the first imaging device A first dissimilarity calculating unit that calculates a first dissimilarity for each of a plurality of different postures of the object in the first virtual space;
A second captured image obtained by imaging a target object by a second imaging device arranged at a position different from that of the first imaging device, and a second corresponding to a space captured by the second imaging device. A second dissimilarity calculating unit that calculates a second dissimilarity indicating a dissimilarity with the model image of the object in the virtual space for each of a plurality of postures of the object in the second virtual space;
In the first imaging coordinate system based on the plurality of first differences calculated by the first difference calculation unit and the plurality of second differences calculated by the second difference calculation unit. A posture estimation unit for estimating the posture of the object;
An attitude detection device comprising:   The posture estimation unit assigns a weight coefficient to each of the plurality of first differences calculated by the first difference calculation unit and the plurality of second differences calculated by the second difference calculation unit. The posture detection device according to claim 1, wherein the posture of the object in the first imaging coordinate system is estimated based on a value obtained by multiplication. When the first imaging device has better visibility of the object than the second imaging device,
The posture estimation unit, when the object is in a predetermined position range from a target position, makes a weighting factor by which the first difference is multiplied larger than a weighting factor by which the second difference is multiplied. The posture detection apparatus according to 2. When the first imaging device has better visibility of the object than the second imaging device,
The posture estimation unit starts moving the object so that a weighting factor by which the second difference is multiplied falls within a predetermined range when the object is in a predetermined position range from a target position. The posture detection device according to claim 2 or 3, wherein a weighting factor by which the second difference is multiplied is changed from time to time when the object reaches a predetermined position range from a target position. When the first imaging device has better visibility of the object than the second imaging device,
The posture estimation unit is configured to calculate a weighting factor for multiplying the second dissimilarity by a period of time from a movement start time of the object to a time when the object reaches a predetermined position range from a target position. The posture detection device according to claim 1, wherein the posture detection device is made smaller as time passes. When the first imaging device has better visibility of the object than the second imaging device and the position of the object moves,
The posture detection apparatus according to any one of claims 1 to 3, wherein the posture estimation unit decreases a weighting factor by which the second difference is multiplied as the movement amount of the target object per unit time decreases. When the first imaging device has better visibility of the object than the second imaging device,
The posture detection apparatus according to any one of claims 1 to 4, wherein the posture estimation unit decreases a weighting factor to be multiplied by the second degree of difference as the object approaches a target position.   The posture estimation unit mixes a weighting factor to be multiplied by the first difference degree and a weighting factor to be multiplied by the second difference degree into each of the first captured image and the second captured image. The attitude detection apparatus according to claim 3, wherein the attitude detection apparatus is changed according to a noise component.   The posture estimation unit mixes a weighting factor to be multiplied by the first degree of difference and a weighting factor to be multiplied by the second degree of difference into each of the first captured image and the second captured image. The posture detection device according to claim 8, wherein the posture detection device is an inverse of the variance of the normalized random noise.   The posture estimation unit calculates the degree of difference between images captured by the first imaging device between different frames as a variance of normalized random noise mixed in the first captured image, and the second imaging The posture detection apparatus according to claim 9, wherein the degree of difference between images captured by the apparatus between different frames is calculated as a variance of normalized random noise mixed in the second captured image. The coordinates when the posture of the object is expressed in the first imaging coordinate system with the first imaging device as a reference are the coordinates of the object in the second imaging coordinate system with the second imaging device as a reference. The plurality of postures of the object in the second virtual space corresponding to the plurality of different postures of the object in the first virtual space are acquired based on the conversion rule for converting the coordinates to A posture acquisition unit;
The second dissimilarity calculation unit includes a second captured image obtained by capturing an image of an object by a second image capturing device arranged at a position different from the first image capturing device, and the second image capturing. The second dissimilarity indicating the dissimilarity with the model image of the object in the second virtual space corresponding to the space imaged by the device is obtained by the object in the second virtual space acquired by the posture acquisition unit. The posture detection device according to claim 1, wherein the posture detection device calculates the posture for each of a plurality of postures. The first dissimilarity calculator calculates a difference between a first captured image obtained by the first imaging device capturing an object and a model image of the object in the first virtual space. Calculating a third dissimilarity indicating a degree for each of a plurality of different positions of the object in the first virtual space;
The second degree-of-difference calculation unit is configured such that a difference between a second captured image obtained by the second imaging device capturing an object and a model image of the object in the second virtual space Calculating a fourth dissimilarity indicating a degree for each of a plurality of different positions of the object in the second virtual space;
A position estimation unit that estimates a position of the object in the first imaging coordinate system based on the plurality of third dissimilarities and the plurality of fourth dissimilarities;
The posture detection device according to claim 1, comprising: The position estimation unit
With reference to a plurality of third dissimilarities calculated by the first dissimilarity calculating unit, a position of a first plane orthogonal to the optical axis of the imaging device in the first imaging coordinate system is estimated, Plane position estimation for estimating the position of the second plane orthogonal to the optical axis of the imaging device in the second imaging coordinate system with reference to the plurality of fourth differences calculated by the second difference calculation unit And
A depth calculator that calculates the depth of the object in the first imaging coordinate system with reference to the position of the first plane and the position of the second plane;
The posture detection device according to claim 12.   The position estimation unit estimates the depth of the object in the first imaging coordinate system based on the plurality of third dissimilarities and the plurality of fourth dissimilarities. The attitude detection device according to claim 1. A determination unit that determines whether or not the estimation has converged with reference to a difference between the posture of the object estimated by the posture estimation unit and the posture of the object estimated before;
With
When it is determined that the determination unit has not converged, the first dissimilarity calculation unit changes the posture of the model image of the object as previously determined and the first imaging The plurality of first dissimilarities are calculated based on the image, and the second dissimilarity calculator calculates the model image after changing the posture of the model image of the object as determined in advance, and Calculating the plurality of second differences based on the second captured image;
The posture estimation unit is configured based on the plurality of first differences calculated by the first difference calculation unit and the plurality of second differences calculated by the second difference calculation unit. Estimate the posture of the object,
The posture detection device according to claim 1, wherein the determination unit performs the determination again using the posture of the object estimated by the posture estimation unit.   When the object is within a predetermined range from the target position, the amount of change between the two images obtained by the first imaging device before and after the position or orientation of the object is changed is The posture detection device according to any one of claims 1 to 14, wherein the posture detection device is larger than a change amount between two images obtained by the second imaging device before and after the position or posture of the object changes. . When the object is at a target position and orientation, a first goal image obtained by photographing the object with the first imaging device and a second goal image obtained by photographing with the second imaging device are acquired. A goal image acquisition unit;
From the position and orientation of the model of the object existing in the first virtual space that reproduces the space in which the first goal image was captured, the model is determined in advance around a predetermined axis or in an axial direction. Calculating a fifth dissimilarity indicating a dissimilarity between the image taken by moving and the first goal image;
From the position and orientation of the model of the object existing in the second virtual space that reproduces the space in which the second goal image was photographed, the model is determined in advance around a predetermined axis or in an axial direction. Calculating a sixth dissimilarity indicating a dissimilarity between the image taken by moving and the second goal image;
The gradient of the fifth difference in the movement direction when the fifth difference is calculated and the gradient of the sixth difference in the movement direction when the sixth difference is calculated By comparing, an imaging device serving as a reference for posture estimation is determined, and a positioning reference imaging device determination unit that newly sets the determined imaging device as the first imaging device;
The posture detection device according to any one of claims 1 to 15. When there are a plurality of the objects, the first dissimilarity calculation unit identifies an image region of all objects other than the object to be a position and orientation estimation target in the first virtual space, and the identified image Calculating the first difference by excluding the region from the calculation target;
The second dissimilarity calculation unit specifies image regions of all objects other than the target object for position and orientation estimation in the second virtual space, and removes the specified image region from the calculation target. The posture detection apparatus according to claim 1, wherein the degree of difference between the two is calculated. Based on the estimated position and orientation of the object and the position and orientation when the object is at the target position, the position and orientation in the robot coordinate system after a predetermined time has elapsed from the current time A target position and orientation determination unit that determines the amount of change in
For each time interval for controlling the robot, the position and posture of the robot after the time interval are determined based on the latest change amount among the change amounts determined by the target position and posture determination unit, and the determined position and posture A robot controller that controls the robot to become
Further comprising
The robot control unit performs the robot until the time determined based on the shortest time until the apparatus determines the amount of change from the first captured image and the second captured image among the predetermined time. 15. The posture according to claim 12, wherein the position and posture of the robot are determined so that the robot moves with acceleration and deceleration, and then the position and posture of the robot are determined so that the robot moves at a constant speed. Detection device. A first dissimilarity calculating unit that calculates a first dissimilarity for each of a plurality of different postures of an object in a first virtual space corresponding to a space imaged by the first imaging device;
A second difference is calculated for each of a plurality of postures of the object in the second virtual space corresponding to the space imaged by the second imaging device arranged at a position different from the first imaging device. The difference calculation unit of
A posture estimation unit that estimates a posture of the object in a first imaging coordinate system based on the first imaging device based on the plurality of first differences and the plurality of second differences. ,
An attitude detection device comprising: The first difference indicating the difference between the first captured image obtained by the first imaging device capturing the object and the model image of the object in the first virtual space is the first difference. A third dissimilarity calculator for calculating each of a plurality of different positions of the object in one virtual space;
The first difference indicating the difference between the second captured image obtained by the second imaging device capturing the object and the model image of the object in the second virtual space is the first difference. A second dissimilarity calculator for calculating each of a plurality of different positions of the object in two virtual spaces;
Based on the plurality of first differences calculated by the first difference calculation unit and the plurality of second differences calculated by the second difference calculation unit, the first imaging coordinate system in the first imaging coordinate system A position estimator for estimating the position of the object;
A position detection device comprising: The degree of difference between the first captured image obtained by imaging the object by the first imaging device and the model image of the object in the first virtual space corresponding to the space imaged by the first imaging device A first dissimilarity calculating unit that calculates a first dissimilarity for each of a plurality of different postures of the object in the first virtual space;
A second captured image obtained by imaging a target object by a second imaging device arranged at a position different from that of the first imaging device, and a second corresponding to a space captured by the second imaging device. A second dissimilarity calculating unit that calculates a second dissimilarity indicating a dissimilarity with the model image of the object in the virtual space for each of a plurality of postures of the object in the second virtual space;
The first imaging device coordinate system based on the plurality of first differences calculated by the first difference calculation unit and the plurality of second differences calculated by the second difference calculation unit. A posture estimation unit for estimating the posture of the object in
Robot equipped with. A robot having moving parts;
The degree of difference between the first captured image obtained by imaging the object by the first imaging device and the model image of the object in the first virtual space corresponding to the space imaged by the first imaging device A first dissimilarity calculating unit that calculates a first dissimilarity for each of a plurality of different postures of the object in the first virtual space;
A second captured image obtained by imaging a target object by a second imaging device arranged at a position different from that of the first imaging device, and a second corresponding to a space captured by the second imaging device. A second dissimilarity calculating unit that calculates a second dissimilarity indicating a dissimilarity with the model image of the object in the virtual space for each of a plurality of postures of the object in the second virtual space;
The first imaging device coordinate system based on the plurality of first differences calculated by the first difference calculation unit and the plurality of second differences calculated by the second difference calculation unit. A posture estimation unit for estimating the posture of the object in
A robot system comprising: The first dissimilarity calculation unit includes a first captured image obtained by imaging a target by the first imaging device, and the first virtual space corresponding to the space imaged by the first imaging device. A procedure for calculating a first degree of difference indicating a degree of difference from the model image of the object for each of a plurality of different postures of the object in the first virtual space;
A second captured image obtained by imaging a target by a second imaging device arranged at a position different from that of the first imaging device, and the second imaging device; Calculating a second dissimilarity indicating a dissimilarity with the model image of the object in the second virtual space corresponding to the space imaged for each of a plurality of postures of the object in the second virtual space When,
Based on the plurality of first differences calculated by the first difference calculation unit and the plurality of second differences calculated by the second difference calculation unit by the posture estimation unit. A procedure for estimating the posture of the object in the imaging coordinate system of
A posture detection method comprising: On the computer,
The degree of difference between the first captured image obtained by imaging the object by the first imaging device and the model image of the object in the first virtual space corresponding to the space imaged by the first imaging device A first dissimilarity calculating step for calculating a first dissimilarity for each of a plurality of different postures of the object in the first virtual space;
A second captured image obtained by imaging a target object by a second imaging device arranged at a position different from that of the first imaging device, and a second corresponding to a space captured by the second imaging device. A second dissimilarity calculating step of calculating a second dissimilarity indicating a dissimilarity with the model image of the object in the virtual space for each of a plurality of postures of the object in the second virtual space;
The first imaging coordinates based on the plurality of first dissimilarities calculated in the first dissimilarity calculating step and the plurality of second dissimilarities calculated in the second dissimilarity calculating step A posture estimation step for estimating a posture of the object in the system;
A program for running

Images