JP2014140941A - Robot control system, robot, robot control method, and program - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a robot control system that selects an appropriate relationship of association between an image sampling timing and joint angle sampling timing, a robot, a robot control method, and a program.SOLUTION: A robot control system includes: an image feature quantity information acquisition unit (feature quantity vector acquisition unit 20) that acquires image feature quantity information at plural image sampling timings; a joint angle information acquisition unit (joint angle vector acquisition unit 10) that acquires joint angle information of a robot at sampling timings that are plural joint angle sampling timings and are asynchronous with the image sampling timings; and a robot control unit 200 that controls the robot on the basis of image feature quantities and pieces of joint angle information acquired at the image sampling timings and joint angle sampling timings respectively which have a selective relationship of association selected from among plural relationships of association.

Description

  The present invention relates to a robot control system, a robot, a robot control method, a program, and the like.

  In robot applications, it is required to move the robot to the expected position more accurately. For example, in industrial applications, it is required to move the hand holding the part to the assembly position more accurately. In the past, this requirement has been realized by calibrating more rigid robots more accurately.

  However, no matter how high the rigidity is, it may be difficult to quickly perform work with high accuracy due to various factors such as thermal expansion due to heat generation. There is also a problem that accurate calibration work takes time.

  Therefore, feedback control that feeds back the difference between the target position (target state) and the current position (current state) to the movement of the robot is widely used. In this case, a method using a parameter called Jacobian for conversion processing for feeding back the difference between the target position and the current position to the movement of the robot is known.

  For example, Patent Document 1 uses an on-the-fly method that re-estimates the Jacobian using information on how much the actual motion differs from the assumed motion while operating the robot using the Jacobian. A robot control technique is disclosed.

JP 2006-318301 A

  A feature vector representing the target position and the current position is [f], and the joint angle vector of the robot is [θ]. In this case, the Jacobian [J] associates the difference [Δf] in the feature vector and the difference [Δθ] in the joint angle vector with [Δf] = [J] [Δθ]. In robot control using feedback processing, when [Δf] is obtained from a target value, an actual measurement value, and the like, it is necessary to calculate [Δθ] as a control parameter. Therefore, an inverse Jacobian that is an inverse matrix of Jacobian is used. Will be used.

  Patent Document 1 discloses a method of defining a tracking error e, which is the difference between a desired robot motion and an actual robot motion, and using that to estimate the Jacobian on-the-fly. According to this method, if we can obtain a set of data such as what value of the feature vector when the robot is in a certain posture (that is, in a state where there is a joint angle vector), the Jacobian can be calculated using that. Can be estimated.

  But here is the problem of synchronization. That is, in order to calculate the Jacobian by the method of Patent Document 1 or the like, data “what value the feature vector was when the joint angle vector was present” is necessary. If the temporal correspondence between the joint angle vector and the feature vector is not taken, that is, if the synchronization is not taken, the tracking error e cannot be accurately obtained by the method of Patent Document 1. As a result, the Jacobian cannot be determined accurately.

  In order to assume that there is no problem even if it is assumed that synchronization is achieved as in Patent Document 1, etc., it is assumed that image feature amount information acquired from a captured image is taken as a feature amount from the robot control system. It is conceivable to use a method of sending a synchronization signal and a shutter signal to However, this causes new cost problems and wiring problems.

  According to some aspects of the present invention, it is possible to provide a robot control system, a robot, a robot control method, a program, and the like that select an appropriate correspondence between an image sampling timing and a joint angle sampling timing.

  One aspect of the present invention is an image feature amount information acquisition unit that acquires image feature amount information at a plurality of image sampling timings, a plurality of joint angle sampling timings, and a robot that is asynchronous with the image sampling timings. A joint angle information acquisition unit for acquiring the joint angle information, and the image feature amount information and the joint in a selection correspondence selected from a plurality of correspondences between the image sampling timing and the joint angle sampling timing The present invention relates to a robot control system including a robot control unit that controls the robot based on angle information.

  In one aspect of the present invention, an appropriate correspondence (selection correspondence) is selected from a plurality of correspondences between the image sampling timing and the joint angle sampling timing, and the image feature amount information and the joint in the selection correspondence are selected. Robot control is performed using angle information. Therefore, it is possible to synchronize data without adding specific hardware or the like to the robot or camera, and it is possible to perform robot control with higher accuracy than in the case of asynchronous.

  In one aspect of the present invention, the robot control unit estimates a Jacobian in the selection correspondence based on the image feature information and the joint angle information in the selection correspondence, and the estimated Jacobian The robot may be controlled based on the above.

  This makes it possible to perform robot control using a Jacobian.

  In one aspect of the present invention, the robot control unit estimates the Jacobian in each of the plurality of correspondences between the image sampling timing and the joint angle sampling timing, and evaluates the estimated Jacobian A value may be obtained, and the selection correspondence may be selected from the plurality of correspondences based on the evaluation value comparison process.

  Thereby, it is possible to estimate the Jacobian in each correspondence relationship of a plurality of correspondence relationships, and to determine the selection correspondence relationship based on the estimated evaluation value of the Jacobian.

  In one aspect of the present invention, the robot control unit creates a normalization equation in each correspondence relationship of the plurality of correspondence relationships between the image sampling timing and the joint angle sampling timing, and the created normalization The Jacobian may be estimated by obtaining a least square solution that is a solution of the equation.

  This makes it possible to estimate the Jacobian as a least squares solution that is a solution of the normalized equation.

  In one aspect of the present invention, the robot control unit uses the estimated Jacobian to calculate the image feature amount information at a comparison timing that is a timing next to the timing at which the Jacobian is estimated. The evaluation value may be obtained based on the image feature amount information and the image feature amount information at the comparison timing acquired by the image feature amount information acquisition unit.

  This makes it possible to obtain an evaluation value of the Jacobian using the image feature amount information estimated using the Jacobian and the image feature amount information actually acquired.

  In the aspect of the invention, the robot control unit may obtain a square error of the normalization equation as the evaluation value using the estimated Jacobian.

  Accordingly, it is possible to obtain the evaluation value of the Jacobian by returning the Jacobian obtained as the least square solution to the equation for obtaining the square error.

In the aspect of the invention, the robot control unit may set the vector or matrix represented by the difference in the image feature information as F, the matrix represented by the difference in the joint angle information as B, and the Jacobian. A normalization equation that gives a least squares solution of F = BJ is created, where J is a vector or matrix representing, V is a matrix having the eigenvector of B ^T B as a column vector for the matrix B, and BB ^When the matrix having the eigenvector of ^{T as} the column vector is U and the matrix having the inverse of the eigenvalue λ of B ^T B as a diagonal element is diag [1 / λ], B ⁺ = Vdiag [1 / λ ] V ^T B ^T , or B ⁺ = B ^T Udiag [1 / λ] U ^{T is used} to determine the generalized inverse matrix B ⁺ of the matrix B, and from the determined generalized inverse matrix B ⁺ , J = B ⁺ Find the matrix J according to F The Jacobian may be estimated.

As a result, the generalized inverse matrix B ⁺ of the matrix B can be obtained quickly and stably.

  In one aspect of the present invention, the robot control unit obtains a robot Jacobian that associates the end point position of the robot with the joint angle information, and creates the normalization equation based on the obtained robot Jacobian. May be.

  Thus, by using the robot Jacobian, it is possible to estimate the image feature amount Jacobian with higher accuracy.

  In one embodiment of the present invention, the robot control unit may compare the image at the given timing based on a comparison process between the robot Jacobian at a reference timing and the robot Jacobian at a given timing. A weighting may be set for a set of the difference value of the feature amount information and the difference value of the joint angle information, and the normalization equation may be created using the set weighting.

  Accordingly, it is possible to estimate the image feature amount Jacobian with higher accuracy by creating a normalization equation using the weights obtained as a result of the comparison processing of the two robot Jacobians.

  In the aspect of the invention, the robot control unit may analytically obtain the robot Jacobian and create the normalization equation based on the obtained robot Jacobian.

  Accordingly, it is possible to estimate the image feature amount Jacobian with higher accuracy by using the robot Jacobian obtained analytically.

Moreover, in one aspect of the present invention, when the sampling frequency of the image sampling timing in the image feature amount information acquisition unit is f _A , and the sampling frequency of the joint angle sampling timing in the joint angle information acquisition unit is f _B In addition, the joint angle information acquisition unit may acquire the joint angle information at the sampling frequency that satisfies f _B ≧ f _A.

  This makes it possible to perform processing for a case where the sampling frequency of the joint angle sampling timing is larger than the sampling frequency of the image sampling timing.

  Another aspect of the present invention provides an image feature amount information acquisition unit that acquires image feature amount information at a plurality of image sampling timings, and joint angle information acquisition that acquires joint angle information of a robot at a plurality of joint angle sampling timings. And controlling the robot based on the image feature amount information and the joint angle information in a selection correspondence selected from a plurality of correspondences between the image sampling timing and the joint angle sampling timing. And a robot control unit.

  Another aspect of the present invention relates to a robot including the above robot control system.

  According to another aspect of the present invention, image feature amount information at a plurality of image sampling timings is acquired, and at a plurality of joint angle sampling timings, the robot joint angle information at a sampling timing asynchronous with the image sampling timings. And controlling the robot based on the image feature amount and the joint angle information in a selected correspondence selected from a plurality of correspondences between the image sampling timing and the joint angle sampling timing. Related to robot control method.

  Moreover, the other aspect of this invention is related with the program which functions a computer as said each part.

  According to another aspect of the present invention, image feature amount information at a plurality of image sampling timings is acquired, and a plurality of joint angle sampling timings, wherein the arm joint angle information is asynchronous with the image sampling timings. And controlling the arm based on the image feature quantity and the joint angle information in a selection correspondence selected from among a plurality of correspondences between the image sampling timing and the joint angle sampling timing. Related to robots.

1A to 1C are diagrams illustrating a Jacobian and a specific example thereof. The figure explaining the Jacobian estimation method of look-then-move type. FIG. 3A shows an example of synchronous data, and FIGS. 3B to 3D show examples of asynchronous data. A search example of a combination of a feature vector and a joint angle vector. The structural example of a robot control system. 6A and 6B are configuration examples of a robot including a robot control system. 7A and 7B are diagrams illustrating a Jacobian estimation method using a plurality of combinations. The system structural example of the block which calculates a generalized inverse matrix. The figure explaining an example of an evaluation process. The flowchart explaining the robot control processing of 1st Embodiment. The figure explaining the relationship between image feature-value Jacobian and robot Jacobian. An example of weighted simultaneous equations using the robot Jacobian. The flowchart explaining the robot control processing of 2nd Embodiment.

  Hereinafter, this embodiment will be described. In addition, this embodiment demonstrated below does not unduly limit the content of this invention described in the claim. In addition, all the configurations described in the present embodiment are not necessarily essential configuration requirements of the present invention.

1. First, the method of this embodiment will be described. In robot applications, it is required to move to the expected position more accurately. For example, in industrial applications, it is required to move the hand holding the part to the assembly position more accurately.

  In the past, this requirement has been realized by more accurately calibrating a stiffer robot. That is, since the low rigidity causes non-linearity that is difficult to control, a robot with higher rigidity has been required. In addition, more accurate calibration has been required to cope with various errors that remain even if the rigidity of the robot is high.

  However, there are many limitations and limitations to such a response. For example, it is inevitable that the motor that drives the robot generates heat, and an error in the order of several hundreds of microns occurs due to thermal expansion due to this. In order to avoid this, it is necessary to warm up in advance to bring the robot into a thermal equilibrium state. That is, there is a limitation that work cannot be started immediately when high accuracy is required. Also, the calibration work after warm-up requires a very long work time. In addition, it is necessary to calibrate each robot individually according to the installation status of each robot.

  As a method for dealing with these problems, there is a method of performing feedback control in which an output from any sensor is incorporated in a robot control process. For example, by detecting the difference between the target position (state) and the current position (state) with an imaging means such as a camera, and feeding it back to the posture and movement of the robot, it is possible to cope with the nonlinearity of the robot. is there. If this method works ideally, it is possible to easily cope with fluctuation factors such as thermal expansion, and it is possible to eliminate the need for calibration work.

  There are several types of conversion processing for “feeding back the difference between the target position and the current position” to the robot motion. Among them, the most direct conversion method is a method using a parameter called Jacobian.

A feature vector [f] is a set of feature amounts representing the target position and the current position, and a joint angle vector [θ] is a set of joint angles of the robot expressing the posture of the robot. These are generally related using a non-linear vector function [F]. The Jacobian or Jacobian matrix [J] is a first-order term obtained by expanding this vector function around a certain point [θ ₀ ]. In other words, as shown in FIG. 1A or FIG. 1B, the Jacobian maps a minute displacement in the joint angle space to the feature amount space. In this embodiment, since image feature amount information is used as a feature amount, the image feature amount Jacobian shown in FIG. 1B is used as the Jacobian. And as a formula showing Jacobian, the formula shown in Drawing 1 (C) can be considered, for example. However, as will be described later in the second embodiment, the robot Jacobian shown in FIG. 1A may be analytically obtained and used for processing in order to improve the estimation accuracy of the image feature amount Jacobian.

When the target feature vector is [f ₀ ] and the current feature vector is [f], the following equation (1) is obtained.

[F] − [f ₀ ] = [Δf] = [J] [Δθ] (1)
Therefore, if the generalized inverse matrix [J] ⁻¹ [J] ⁻¹ of [J] can be obtained, the following expression (2) is obtained.

[Δθ] = [J] ⁻¹ [Δf] (2)
A process of adding [Δθ] (or a value obtained by multiplying [Δθ] by a gain or the like) obtained by the above equation (2) to [θ] that is the current posture of the robot, that is, the robot by [Δθ]. [Δf] can be reduced by moving the joint angle, and the robot can be brought close to the target position and state.

  Conventionally, as a look-then-move type Jacobian estimation method, for example, an example using the method shown in FIG. 2 has been seen. That is, the change in the feature vector when the specific joint is changed minutely corresponds to the value of a specific column of the Jacobian matrix. If this is performed for all joints, a Jacobian matrix can be obtained. It is possible to actually control the robot with the Jacobian thus obtained. In addition, even if a specific joint is not operated independently as described above, even if a plurality of joints are operated simultaneously, the same processing can be performed as long as the operation is linearly independent.

  However, in such a method, there is a problem that the operation of estimating the Jacobian and the operation of actually moving the robot to the target position are separated. That is, the operation of stopping, watching, estimating the Jacobian, and operating the robot using the Jacobian is repeated. This has a problem that the operation speed of the robot is remarkably reduced.

  On the other hand, Patent Document 1 discloses a method of defining a tracking error e, which is a difference between desired robot motion and actual robot motion, and estimating the Jacobian on-the-fly using the error. . According to this method, if we can obtain a set of data such as what value of the feature vector when the robot is in a certain posture (that is, in a state where there is a joint angle vector), the Jacobian can be calculated using that. Can be estimated.

  That is, in the conventional method, as shown in FIG. 3A, image data (feature vector is acquired from the image data because visual servo or the like is assumed) and the image data is acquired. It is assumed that the robot posture (joint angle vector) is synchronized data that matches timing.

  By using a method of sending a synchronization signal and a shutter signal from the robot control system to the imaging system, it is considered that there is no problem even if synchronization is assumed as in the conventional method. However, this causes problems such as cost and wiring. Assuming that no synchronization signal, shutter signal, or the like is used, as will be described later with reference to FIGS. 3B to 3D, asynchronism between data occurs, and accurate Jacobian estimation is possible. It becomes difficult.

  Therefore, the present applicant proposes a method for accurately estimating the Jacobian by synchronizing the feature vector and the joint angle vector even when a high-cost and complicated device is not used. Specifically, as will be described later with reference to FIGS. 3B to 3D, the data becomes asynchronous due to origin deviation, fluctuation, or both. Therefore, here, a range where the origin deviation can occur and a range where the fluctuation can occur are set, and the Jacobian is estimated for a plurality of combinations between the feature vector and the joint angle vector considered in the range. For example, when fluctuation occurs such that four joint angle vectors (joint angle data) correspond to a feature amount vector (image data) at a certain timing, as shown in FIG. Are considered, the problem of searching for the optimum combination (in the example of FIG. 4, the combination represented by the arrow indicated by the thick line) is solved.

  Specifically, a plurality of Jacobians estimated by each correspondence relationship of a plurality of correspondence relationships are evaluated by a given evaluation method, and the optimum Jacobian is adopted as the Jacobian at the timing. In the present embodiment, the processing is performed assuming the periodicity of the image sampling timing and the periodicity of the joint angle sampling timing.

  However, in the method of this embodiment, in order to determine the Jacobian at a given timing, it is necessary to estimate the Jacobian for a certain number of combinations (for example, tens of thousands of combinations). For this reason, high speed of Jacobian estimation once is required. Therefore, the present applicant proposes a method for obtaining a generalized inverse matrix at high speed and stably (robustly) by using Equation (13) described later, and estimating the Jacobian using the obtained generalized inverse matrix. .

  Hereinafter, the first embodiment and the second embodiment will be described. In the first embodiment, a basic method will be described, including Jacobian estimation using the following equation (13). In the second embodiment, a modified example of the first embodiment that improves the accuracy of estimating the Jacobian (image feature amount Jacobian in a narrow sense) using the robot Jacobian obtained analytically will be described.

2. First Embodiment A first embodiment will be described. After describing a configuration example of the robot control system, a method for setting a plurality of correspondence relationships between image feature amount information and joint angle information will be described. Then, a method for estimating the Jacobian in each set correspondence relationship will be described, and a method for selecting an appropriate Jacobian from a plurality of estimated Jacobians (that is, selecting an appropriate correspondence relationship) will be described. Finally, details of the processing of this embodiment will be described with reference to FIG.

2.1 System Configuration Example FIG. 5 shows a system configuration example of the robot control system of this embodiment. The robot control system includes a joint angle vector acquisition unit (joint angle information acquisition unit) 10, a feature amount vector acquisition unit (image feature amount information acquisition unit) 20, a storage unit 30, and a robot control unit 200. However, the robot control system and each part constituting the robot control system are not limited to the configuration shown in FIG. 5, and various modifications such as omitting some of these components or adding other components are possible. Is possible.

The joint angle vector acquisition unit 10 acquires joint angle information of the robot. Although the joint angle information can be expressed in various formats, for example, even when a joint angle vector having a number of values corresponding to the degree of freedom (number of joints) of the robot as an element at one joint angle sampling timing is acquired. Good. Specifically, in the case of a seven-degree-of-freedom robot, one joint angle information is a joint angle vector having seven values of θ _{0 to} θ ₆ as elements.

  The feature quantity vector acquisition unit 20 acquires feature quantity information from sensor values and the like. The feature amount information expresses the position (or state) of the robot, but in this embodiment, since control using an image such as a visual servo is assumed, an image feature amount acquired from the captured image is obtained. This is feature amount information. It should be noted that some pre-processing may be required to acquire feature amount information, as may be the case where feature amount information is acquired by performing given image processing on the acquired image. The feature vector acquisition unit 20 of the present embodiment may perform such preprocessing. Alternatively, the image itself (for example, a set of pixel values of each pixel) may be acquired as image feature amount information.

  The storage unit 30 stores various information, and its function can be realized by a memory such as a RAM, an HDD (hard disk drive), or the like. The storage unit 30 includes a joint angle vector sequence storage unit 31, a feature amount vector sequence storage unit 33, and a target feature amount storage unit 35.

  The joint angle vector sequence storage unit 31 stores a plurality of joint angle vectors, for example, in the form of FIFO. In the present embodiment, it is assumed that the Jacobian is obtained by solving the equation shown in FIG. 7B (or an equation corresponding to the equation) shown in FIG. As shown in (A), at least seven (in a broad sense, only corresponding to the degree of freedom of the robot) is required. Therefore, here, the joint angle vector sequence storage unit 31 needs to store at least eight joint angle vectors.

  The feature vector sequence storage unit 33 stores a plurality of feature vectors, for example, in the form of FIFO. Similar to the joint angle vector sequence storage unit 31, the feature quantity vector sequence storage unit 33 needs to store at least eight feature quantity vectors.

  The target feature amount storage unit 35 stores a target feature amount. In this embodiment, since a robot control method for performing feedback processing is assumed, the feature amount information at each timing approaches the target feature amount stored in the target feature amount storage unit 35 (the difference is reduced). Control).

  The robot control unit 200 selects an appropriate correspondence between the image sampling timing and the joint angle sampling timing, and controls the robot based on the Jacobian in the selection correspondence. The robot control unit 200 includes a correspondence setting unit 210, a Jacobian estimation unit 220, an evaluation processing unit 230, an inverse Jacobian calculation unit 240, a target difference vector calculation unit 250, a matrix / vector product calculation unit 260, a control Data output unit 270.

  The correspondence setting unit 210 sets a plurality of correspondences between the image sampling timing and the joint angle sampling timing based on asynchronous factors between data (here, origin deviation and fluctuation).

  The Jacobian estimation unit 220 estimates a Jacobian in each correspondence relationship of the plurality of correspondence relationships set by the correspondence relationship setting unit 210. In the present embodiment, the Jacobian is estimated using the following equation (13), and the details will be described later.

  The evaluation processing unit 230 calculates an evaluation value for a plurality of Jacobians estimated by the Jacobian estimation unit 220 (in a narrow sense, the number corresponding to all the correspondences set by the correspondence relationship setting unit 210), and the optimum Jacobian is calculated. In addition, the correspondence between the data corresponding to the optimum Jacobian is selected as the selected correspondence, thereby synchronizing the data.

The inverse Jacobian calculating unit 240 calculates the optimal Jacobian inverse Jacobian determined by the evaluation processing unit 230 (corresponding to [J] ⁻¹ in the above equation (2)). Various methods are known for calculating the inverse Jacobian, and any of the conventional methods may be used in this embodiment. However, the process for obtaining the generalized inverse matrix (the following expression (13)) in the Jacobian estimation unit 220 can also be used for the process for obtaining the inverse Jacobian that is the generalized inverse matrix with the Jacobian as an input. That is, the inverse Jacobian calculating unit 240 of the present embodiment may calculate the inverse Jacobian using the following equation (13).

  The target difference vector calculation unit 250 calculates a difference between the image feature amount information at the processing timing and the target feature amount information stored in the target feature amount storage unit 35. The output of the target difference vector calculation unit 250 corresponds to [Δf] in the above equation (2).

The matrix / vector product calculation unit 260 calculates the product of the inverse Jacobian [J] ⁻¹ output from the inverse Jacobian calculation unit 240 and the target difference vector [Δf] output from the target difference vector calculation unit 250. That is, here, the process of obtaining [Δθ] in the above equation (2) is performed.

  The control data output unit 270 outputs the obtained [Δθ] or a value obtained by multiplying it by a gain or the like as control data. Specifically, an instruction to change each joint by an angle corresponding to the control data is output to the robot.

  FIG. 6A shows an example of a robot (robot system) including the robot control system of this embodiment. This robot system includes a control device 300 (corresponding to the above-described robot control system) and a robot main body 310 (a robot in a broad sense). The control device 300 performs control processing for the robot body 310. Specifically, an instruction to change a joint angle by an angle corresponding to the above [Δθ] to a drive unit (for example, a joint drive motor provided for each joint) included in the robot main body 310 is the control data. By outputting, the robot main body 310 performs control to shift to the target position (target state). The robot body 310 includes an arm 320 and a hand (gripping unit) 330. Then, it operates according to an operation instruction from the control device 300. For example, operations such as gripping or moving a workpiece placed on a pallet (not shown) are performed. Further, information such as the posture of the robot body 310 and the position of the workpiece is detected based on captured image information acquired by an imaging device (not shown), and the detected information is sent to the control device 300.

  The robot control system of this embodiment is provided in, for example, the control device 300 in FIG. 6A, and the robot control system is realized by, for example, the hardware or program of the control device 300. According to the robot control system of the present embodiment, it is possible to reduce performance requirements for control hardware such as the control device 300 and to operate the robot body 310 with high responsiveness.

  In FIG. 6A, the robot body 310 (robot) and the control device 300 (robot control system) are configured separately. However, as shown in FIG. 6B, the robot body 310 and the control device. 300 may be integrally formed. Specifically, in FIG. 6B, the control device 300 is stored in the base unit portion that supports the robot body 310. The base unit portion is provided with wheels and the like so that the entire robot can move. 6A shows an example of a single arm type robot, the robot of this embodiment may be a multi-arm type robot such as a double arm type as shown in FIG. 6B.

2.2 Correspondence Setting Processing The correspondence setting processing performed between the image sampling timing and the joint angle sampling timing performed by the correspondence setting unit 210 will be described. The image sampling timing here represents the timing at which image data (and image feature amount information based on the image data) is acquired, and the specific rate depends on the performance of the camera and the like, for example, about 10 milliseconds. It is conceivable that the period is. The joint angle sampling timing represents the timing at which joint angle information, which is posture data of the robot, is acquired, and has a period of about 1 millisecond, for example.

  In this embodiment, the periodicity of the image sampling timing and the periodicity of the joint angle sampling timing are assumed. In other words, as long as the same camera is used under the same conditions, the fluctuation of the image feature amount information acquisition interval of 9 milliseconds or 11 milliseconds does not occur. Considering this characteristic, it is difficult to think about this assumption.

  In the case of synchronized ideal data (synchronous data), the relationship between the image sampling timing and the joint angle sampling timing is as shown in FIG. 3A, and the image feature amount information at the timing connected by arrows. And joint angle information may be used for processing. On the other hand, in this embodiment, the possibility of origin deviation due to some delay is considered. Specifically, as shown in FIG. 3B, the image feature amount information of the image sampling timing at a point in time when the joint angle information at a certain joint angle sampling timing is not the same as or nearest to the joint angle sampling timing. It can be considered that In other words, the joint angle sampling timing corresponding to a given image sampling timing has a certain range (for example, if the joint angle sampling period is 1 millisecond and the origin deviation range is 50 milliseconds, 50 joints). ) As a candidate for correspondence.

  In addition, when the operation cycle of the camera and the robot does not have an appropriate integer ratio, it is necessary to consider asynchrony caused by fluctuations. For example, the frame rate of an inexpensive NTSC type camera is 29.97 frames per second, and the robot operation cycle may be 128 microseconds × 8, 1.024 milliseconds. Furthermore, fluctuations may occur due to hardware. Specifically, even when there is no deviation of the origin, as shown in FIG. 3C, the joint angle sampling timing corresponding to a given image sampling timing is not the same, but deviates before and after that. It is possible. For example, if the joint angle sampling period is 1 millisecond and the relative period fluctuation is ± 1 millisecond, it is necessary to set three samplings including one sample before and after as candidates for correspondence.

  In the present embodiment, as shown in FIG. 3D, the correspondence is set in consideration of both origin deviation and fluctuation. For example, since it is necessary to consider the origin deviation at the first processing timing, the correspondence between the image sampling timing and the joint angle sampling timing is a number corresponding to the origin deviation range (50 in the above example). Set. On the other hand, once an optimal Jacobian is estimated, the degree of origin deviation at that time can be known, and the value of the origin deviation can be used in the subsequent processing. In other words, once the origin is set, only fluctuations need to be considered after that, and the correspondence between the image sampling timing and the joint angle sampling timing is a number corresponding to the fluctuation range (three in the above example). Set.

  However, in this embodiment, as will be described later, at least seven sets of difference values of image feature amount information and difference values of joint angle information are used in the estimation of Jacobian. That is, in the correspondence setting unit 210, it is necessary to set at least eight sets of correspondence between the image sampling timing and the joint angle sampling timing (so as to cover all in a narrow sense).

In this case, since it is necessary to consider the origin deviation at the first processing timing, a number corresponding to the origin deviation (for example, 50 ways) is set for one of the 8 sets of correspondence, and the other 7 for the set corresponding relationship set the number of only corresponding to the fluctuation (e.g., 3 ⁷ kinds of triplicate respectively). That is, in terms of the specific numerical values to set the correspondence between the 50 × 3 ⁷ ways, and outputs a corresponding relationship set in Jacobian estimation unit 220. On the other hand, if the home position alignment is finished, it is sufficient to set the number of only correspond to fluctuations respectively (e.g. 3 ^eight triplicate respectively) for eight sets of correspondence.

2.3 Jacobian Calculation Processing A method for estimating the Jacobian in each correspondence relationship among a plurality of correspondence relationships set by the correspondence relationship setting unit 210 will be described. In the following description, a 6-dimensional vector composed of f _{0 to} f ₅ is considered as feature amount information (feature amount vector), and a 7-dimensional vector composed of θ ₀ to θ ₆ is considered as joint angle information (joint angle vector). However, the present invention is not limited to this, and the following method can be applied to arbitrary feature amount information and joint angle information.

  Since the Jacobian satisfies the above equation (1), the following equation (3) is established.

ここでΔｆ_０〜Δｆ_５は２つの（狭義には隣り合うタイミングでの）特徴量ベクトルの差分を表し、Δθ_０〜Δθ_６は２つの関節角ベクトルの差分を表すものである。つまり対応関係設定部２１０により、対応付けされた画像特徴量情報と関節角情報の組が２組設定されれば、２つの画像特徴量情報の差分をとることで１つのΔｆ_０〜Δｆ_５が求められ、２つの関節角情報の差分をとることで１つのΔθ_０〜Δθ_６が求められるため、上式（３）を設定することができる。

Here, Δf ₀ to Δf ₅ represent the difference between the two feature quantity vectors (at the timing adjacent to each other in a narrow sense), and Δθ _{0 to} Δθ ₆ represent the difference between the two joint angle vectors. That is, if two sets of associated image feature amount information and joint angle information are set by the correspondence setting unit 210, one Δf ₀ to Δf ₅ is obtained by taking the difference between the two image feature amount information. Since one Δθ ₀ to Δθ ₆ is obtained by obtaining the difference between the two joint angle information, the above equation (3) can be set.

  Here, the following equation (5) is established by considering transposition of both sides as in the following equation (4).

[Δf] ^T = [Δθ] ^T [J] ^T (4)

上式（５）に示したように、Δｆ_０〜Δｆ_５及びΔθ_０〜Δθ_６が既知の値として取得することができた場合には、それらの値を用いてＪを求めることになる。しかし、上式（５）では未知数がＪ_００〜Ｊ_５６の４２個あるのに対して、方程式の数は６個である。よって条件が十分でないため近似的に解くことしかできない。そのため、本実施形態では１次独立な成分を持つ式を７組以上まとめることで、より正確な解を求めるものとする。

As shown in the above equation (5), when Δf ₀ to Δf ₅ and Δθ _{0 to} Δθ ₆ can be acquired as known values, J is obtained using these values. However, in the above formula (5), there are 42 unknowns J _{00 to} J ₅₆ , while the number of equations is 6. Therefore, since the condition is not sufficient, it can only be solved approximately. Therefore, in this embodiment, a more accurate solution is obtained by collecting seven or more sets of equations having primary independent components.

Specifically, as described in the correspondence setting process, a correspondence that associates at least eight sets of image feature amount information and joint angle information is set. Then, seven sets of Δf _{0 to} Δf ₅ and Δθ _{0 to} Δθ ₆ can be obtained by obtaining the difference between adjacent image feature amount information and the difference between adjacent joint angle information.

This is shown in FIGS. 7A and 7B. As shown in (1) to (7) of FIG. 7A, a matrix of 7 rows having each set of Δf ₀ to Δf ₅ as row elements, and each set of Δθ _{0 to} Δθ ₆ as row elements. When the 7-row matrix is considered, these matrices satisfy the equation shown in FIG. In this case, since 42 equations (assuming that each is linearly independent as described above) can be set for 42 unknowns, the Jacobian can be accurately obtained. is there.

In addition, the correspondence setting unit 210 sets nine or more sets of associated image feature information and joint angle information, thereby acquiring eight or more sets of Δf _{0 to} Δf ₅ and Δθ _{0 to} Δθ _6. It may be a thing. In this case, since the conditional expression becomes excessive with respect to the unknown, there is a possibility that J that satisfies all the equations cannot be obtained. Here, when x is an n-dimensional vector, y is an m-dimensional vector, and A is an m × n matrix, a linear simultaneous equation of the following equation (6) is considered.

y = Ax (6)
In this case, since the least squares solution that minimizes the square error given by the following equation (7) is given as the solution of the normalized equation of the following equation (8), the least squares solution x ^* is expressed by the following equation (9): Become.

|| y−Ax || ² = (y−Ax) ^T (y−Ax) (7)
A ^T Ax = A ^T y (8)
x ^* = (A ^T A) ⁻¹ A ^T y (9)
The above equation (9) indicates that the inverse matrix (generalized inverse matrix) A ⁺ of A which is not generally a square matrix is given by the following equation (10).

A ⁺ = (A ^T A) ⁻¹ A ^T (10)
That is, although there is a difference between a matrix and a vector, if the matrix determined by Δf is y, the matrix determined by Δθ is A, and the matrix determined by the Jacobian is x, the equation in FIG. The Jacobian can be obtained as a least squares solution that is a solution of the normalization equation.

Here, the above equations (9) and (10) indicate that if the generalized inverse matrix A ⁺ of the matrix A can be obtained, the least squares solution can be obtained by x ^* = A ⁺ y. The generalized inverse matrix is not limited to the form of the above formula (10).

For example, an arbitrary m × n matrix A can be subjected to singular value decomposition in the form of the following equation (11): A = UΣV ^T (11)
Here, U is an m-dimensional orthonormal basis, V is an n-dimensional orthonormal basis, and Σ is a matrix having singular values in diagonal terms. Incidentally, U is a matrix having m eigenvectors of the square matrix AA ^T as a column vector, V is a matrix with n eigenvectors of a square matrix A ^{T A} as a column vector, sigma is shared by U and V In other words, it is a matrix having the square root of the eigenvalue as a diagonal term.

From this, it is possible to consider A ⁺ in the following equation (12) as a matrix having the same properties as the inverse matrix of A in the above equation (11).

A ⁺ = VΣ ⁻¹ U ^T (12)
That is, even if singular value decomposition of a matrix (hereinafter referred to as matrix B) determined by Δθ is performed as A in the above equation (11), a generalized inverse matrix B ⁺ of the matrix B is obtained according to the above equation (12). Good. Also in this case, the least square solution representing the Jacobian can be obtained by x ^* = A ⁺ y (specifically, [J] ^T = B ⁺ [Δf]).

However, the method for obtaining the generalized inverse matrix by the above equation (10) may cause a problem that the inverse Jacobian cannot be obtained stably. In the above equation (10), it is necessary to obtain the inverse matrix of ^{A T A (A T A)} -1. Here, although A ^T A is a square matrix, its rank (rank) depends on the value of a specific target matrix (here, the value of Δθ) and may not be a full rank in some cases. In that case, since the inverse matrix (A ^T A) ⁻¹ cannot be obtained appropriately, the generalized inverse matrix is not appropriate. In the control of the robot, since control with an inappropriate parameter can cause a serious problem such as a collision, the above equation (10) cannot be used for calculating the generalized inverse matrix.

  Further, in the method of obtaining the generalized inverse matrix by the above equation (12), since the normal inverse matrix processing is not performed, the generalized inverse matrix can be obtained stably (robustly), but the calculation amount is large and the speed is high. The problem that processing is difficult may arise. For example, the number of rows in the matrix B (m in the above example) corresponds to the number of sets of the associated image feature information and joint angle information, and the number of columns (n in the above example) is the freedom of the robot. Corresponds to the degree (for example, the number of joints). At this time, if the value of m is larger than n in order to increase the Jacobian estimation accuracy, it is necessary to obtain a huge matrix as U, and the calculation amount becomes enormous. .

Therefore, in this embodiment, the generalized inverse matrix A ⁺ of the matrix A is obtained by the following equation (13). In the following equation (13), diag [1 / λ] represents a matrix having the inverse of the eigenvalue λ as a diagonal value as shown in the following equation (14). However, when λ = 0, the reciprocal thereof is also 0.

A ⁺ = Vdiag [1 / λ] V ^T A ^T (13)

上式（１３）を用いれば、上式（１０）に示した例とは異なり、通常の逆行列処理を行う必要がない。よって、行列Ｂのランクによらず一般化逆行列Ｂ^＋を安定的に求めることが可能である。また、上式（１２）に示した例とは異なり、正規直交基底Ｕを求める必要がない。つまりｍの値が大きくなったとしても、ｍ次元の行列を計算する必要がなくなるため高速に一般化逆行列Ｂ^＋を求めることが可能である。

If the above equation (13) is used, unlike the example shown in the above equation (10), it is not necessary to perform normal inverse matrix processing. Therefore, the generalized inverse matrix B ⁺ can be obtained stably regardless of the rank of the matrix B. Further, unlike the example shown in the above equation (12), there is no need to obtain the orthonormal basis U. That is, even if the value of m becomes large, it is not necessary to calculate an m-dimensional matrix, so that the generalized inverse matrix B ⁺ can be obtained at high speed.

Note that the values of m and n differ depending on the setting of the robot to be used. Therefore, unlike the above example, in a case where n is much larger than m, the generalized inverse matrix may be obtained using the following equation (15). Also in this case, the generalized inverse matrix B ⁺ can be stably obtained, and since it is not necessary to handle a very large n-dimensional matrix, it is possible to perform processing at high speed.

A ⁺ = A ^T Udiag [1 / λ] U ^T (15)
That is, the Jacobian estimation unit 220 reads out eight or more joint angle vectors and feature amount vectors associated by the correspondence setting unit 210 from the joint angle vector sequence storage unit 31 and the feature amount vector sequence storage unit 33, and the difference between them. As a result, seven or more sets of Δf ₀ to Δf ₅ and Δθ _{0 to} Δθ ₆ are acquired. Then, a generalized inverse matrix B ⁺ of the matrix B determined from seven or more sets of Δθ ₀ to Δθ ₆ (θ ₀₀ to θ _{66 in} the example of FIG. 7B) is obtained.

  The Jacobian estimation unit 220 may include a generalized inverse matrix acquisition unit, and the generalized inverse matrix acquisition unit includes a variance matrix calculation unit 221, an eigenvalue calculation unit 222, and an inverse calculation unit 223 as illustrated in FIG. And eigenvector matrix calculation unit 224, transposed matrix calculation units 225-1 and 225-2, matrix / vector product calculation unit 226, and matrix product calculation units 227-1 and 227-2.

The variance matrix calculation unit 221 acquires the matrix B and obtains the variance matrix. The covariance matrix herein is a B ^{T B} in the case of using the above equation (13), a BB ^T in the case of using the above equation (15). The calculated dispersion matrix is output to the eigenvalue calculation unit 222 and the eigenvector matrix calculation unit 224.

  The eigenvalue calculation unit 222 calculates the eigenvalue λ of the variance matrix. The reciprocal calculation unit 223 calculates the reciprocal 1 / λ of the eigenvalue λ. However, when λ = 0, the reciprocal of the eigenvalue is also 0.

  The eigenvector matrix calculation unit 224 calculates an eigenvector of the variance matrix and calculates an eigenvector matrix composed of eigenvectors. The eigenvector matrix corresponds to V in the above equation (13) or U in the above equation (15).

  The matrix / vector product calculation unit 226 calculates a product of V or U output from the eigenvector matrix calculation unit 224 and diag [1 / λ] output from the reciprocal calculation unit 223. Specifically, Vdiag [1 / λ] in the above equation (13) or Udiag [1 / λ] in the above equation (15) is obtained.

The transposed matrix calculation unit 225-1 obtains the transpose matrix of the output of the eigenvector matrix calculation unit 224. Specifically, V ^T or U ^T is obtained. The matrix product calculation unit 227-1 obtains the product of the output of the matrix / vector product calculation unit 226 and the output of the transposed matrix calculation unit 225-1. Specifically, finding a Udiag [1 / λ] ^{U T} of the Vdiag [1 / λ] ^{V T} of the equation (13), or the above equation (15).

Further, the transposed matrix calculation unit 225-2 obtains a transposed matrix B ^T of the matrix B. The matrix product calculation unit 227-2 obtains the product of the output of the matrix product calculation unit 227-1 and the output of the transposed matrix calculation unit 225-2. Specifically, Vdiag [1 / λ] V ^T B ^T or B ^T Udiag [1 / λ] U ^T is obtained, and the output of the matrix product calculation unit 227-2 is the generalized inverse matrix of the matrix B. B ⁺ .

Once the generalized inverse matrix B ⁺ of the matrix B determined by Δθ can be obtained, the Jacobian can be obtained by multiplying the determined generalized inverse matrix B ⁺ from the left by the matrix determined by Δf. is there.

The 2.4 optimal for the corresponding selection process relationships above technique, Jacobian is estimated for each of the plurality of correspondence relationships set by the correspondence relationship setting unit 210 (e.g., 50 × 3 of ⁷ types correspondence). Next, an optimum Jacobian and an optimum correspondence (selective correspondence) corresponding to the Jacobian are selected from the plurality of Jacobians.

  Specifically, an evaluation value (error information) is obtained for each Jacobian, and an optimum Jacobian or the like is selected by performing comparison processing of the evaluation values. An example of the evaluation value calculation process is shown in FIG. As shown in FIG. 9, the latest feature amount information (A1 in FIG. 9) is used as evaluation information, and no correspondence setting or Jacobian estimation is performed. Then, the previous feature amount information is associated with the joint angle information (here, nine sets of image feature amount information and joint angle information associated with each other are set, so that Δf and Δθ Eight sets are set), and the Jacobian is estimated by the above-described method.

  When the Jacobian is estimated, the latest image feature amount is estimated from the data at the previous timing using the Jacobian. Specifically, if the Jacobian at that time is applied to the difference value of the previous joint angle information as shown in the above equation (1), the image feature amount information at the latest image sampling timing is obtained. (Strictly speaking, the difference between the latest image feature quantity and the image feature quantity one timing before is first obtained, and the latest image feature quantity is obtained therefrom). More specifically, the joint angle information at the joint angle sampling timing corresponding to the image sampling timing (A2) (for example, B2 depending on the setting in the correspondence setting unit 210) and the previous joint angle information ( What is necessary is just to make a Jacobian act on the difference value with B3) corresponding to A3. Here, as the estimated Jacobian is appropriate, the image feature amount information (estimated value) at the latest timing estimated using the Jacobian should be closer to the image feature amount information that is a measured value. Therefore, in this embodiment, for example, an error (e.g., the sum of squares of differences between components) between the estimated image feature information and the measured image feature information may be used as the evaluation value.

  This evaluation value calculation process is performed on multiple estimated Jacobians, and the one with the best evaluation value (here, the smallest value because it is error information) is selected as the optimum Jacobian, and the correspondence at that time is selected and supported It is related.

  Note that the Jacobian evaluation value is not limited to the above. The Jacobian obtained by the above method is a least squares solution that minimizes the square error of the above equation (7). Therefore, if the value is calculated by substituting the obtained Jacobian for x in the above equation (7), the value of the square error when the Jacobian is used can be obtained. Since the square error value is considered to be smaller as the Jacobian is appropriate, the square error value of the above equation (7) may be used as the evaluation value of the Jacobian. Also in this case, the one with the smallest evaluation value is selected as the optimum Jacobian, and the corresponding relationship at that time is set as the selected corresponding relationship.

  However, as shown in FIGS. 7A and 7B, only 7 sets of image feature amount information and joint angle information (a set corresponding to the degree of freedom of the robot in a broad sense) were used. In this case, the number of unknowns and the number of linearly independent equations coincide with each other, and the simultaneous equations represented by the equation of FIG. 7B can be solved as they are. In this case, since the equal sign of the above equation (6) holds, the square error of the above equation (7) is always zero. That is, when the conditions are set without excess or deficiency, the square errors according to the above equation (7) of a plurality of estimated Jacobians are all 0, and their validity cannot be compared. Therefore, as an evaluation value in this case, a value obtained by the method shown in FIG. 9 may be used instead of the square error of the above equation (7). On the other hand, if the number of equations is excessive, the equal sign of the above formula (6) is not generally established, and a significant value other than 0 is acquired as the square error value of the above formula (7). Can do. In this case, the evaluation value may be obtained by the method of FIG. 9, or the square error value of the above equation (7) may be used.

2.5 Processing Details The processing flow of this embodiment will be described with reference to the flowchart of FIG. When this process is started, first, a minimum evaluation value and an estimated Jacobian that is a Jacobian corresponding to the minimum evaluation value are initialized (step S101, step S102). Then, the study scope all correspondence (e.g. 50 × 3 of ⁷ types correspondence) set by the correspondence relationship setting unit 210 judges whether it has examined all of the examined range (step S103).

  If there is a correspondence relationship that has not been examined (No in step S103), one unconsidered correspondence relationship is selected (step S104), and image feature amount information and joint angle information in the selected correspondence relationship are selected. Is used to estimate the Jacobian by the method described above (step S105). Then, an evaluation value is calculated using the estimated Jacobian (step S106). Specifically, the square error may be obtained by the method of FIG. 9 or the above equation (7).

  Thereafter, a comparison process is performed between the obtained evaluation value and the minimum evaluation value held at that time (step S107). If the obtained evaluation value is smaller than the minimum evaluation value (Yes in step S107), the minimum evaluation value is updated with the evaluation value obtained in step S106 (step S108), and the estimated Jacobian is obtained in step S105. It is updated with the Jacobian estimated in (Step S109). On the other hand, when the obtained evaluation value is equal to or greater than the minimum evaluation value (No in step S107), the process returns to step S103 without updating the minimum evaluation value and the estimated Jacobian.

  If the processing of step S104 to step S109 is repeated and all correspondences are examined, the determination in step S103 is Yes, and it is determined whether the minimum evaluation value or the initial value has been updated (step S110). If the minimum evaluation value has not been updated, it is determined that an appropriate correspondence cannot be selected, and the process ends. On the other hand, if the minimum evaluation value has been updated, the estimated Jacobian that gives the minimum evaluation value at Step S110 is output as the optimal Jacobian, and the correspondence corresponding to the Jacobian is selected as the selection correspondence. (Step S111).

  In the above embodiment, as shown in FIG. 5, the robot control system includes an image feature amount information acquisition unit (feature amount vector acquisition unit) 20 that acquires image feature amount information at a plurality of image sampling timings, and a plurality of image feature amount information acquisition units. A joint angle information acquisition unit (joint angle vector acquisition unit) 10 that acquires joint angle information of the robot at the joint angle sampling timing, and a selection selected from a plurality of correspondences between the image sampling timing and the joint angle sampling timing A robot control unit 200 that controls a robot (for example, the robot main body 310 in FIG. 6A) based on the image feature amount information and the joint angle information in the correspondence relationship is included. Note that the plurality of joint angle sampling timings are assumed to be sampling timings asynchronous with the image sampling timing.

  Here, the image feature amount information is information representing the image feature amount, and may be, for example, a feature amount vector having a value representing the image feature amount as an element. In this case, the element of the feature vector may be various values acquired from image data, and may be information representing an edge, information representing a spatial frequency, a pixel value itself, or the like. The joint angle information is information representing the joint angle of the robot, and may be a joint angle vector having elements representing values of the angles of the respective joints. The joint angle vector has, as its elements, a value corresponding to the degree of freedom of the robot in a narrow sense.

  Accordingly, it is possible to select an appropriate correspondence between the joint angle sampling timing and the image sampling timing (that is, between the joint angle information and the image feature amount information) and to synchronize the data. If conditions such as using a high-performance camera are not satisfied, there is a high possibility that asynchronism will occur due to the origin deviation shown in FIG. 3B or the fluctuation shown in FIG. 3C. Appropriate robot control is difficult if the synchronization between data is assumed without considering the points. Therefore, in this embodiment, it is assumed that an appropriate correspondence relationship between data is selected, and then robot control is performed using image feature amount information and joint angle information in the selected correspondence relationship.

  Further, the robot control unit 200 may estimate the Jacobian in the selection correspondence based on the image feature amount information and the joint angle information in the selection correspondence, and control the robot based on the estimated Jacobian.

  This makes it possible to perform robot control using a Jacobian. As described above, the Jacobian maps a minute displacement (difference) in a given space as a minute displacement (difference) in another space, and therefore, image feature values using two or more pieces of image feature amount information. It is necessary to obtain a difference in information and obtain a difference in joint angle information using two or more pieces of joint angle information. Therefore, it is assumed here that a correspondence relationship that associates at least two sets of image feature amount information and joint angle information is selected.

  Further, the robot control unit 200 estimates a Jacobian in each of the plurality of correspondences between the image sampling timing and the joint angle sampling timing, obtains an estimated value of the estimated Jacobian, and based on the evaluation value comparison processing The selected correspondence relationship may be selected from a plurality of correspondence relationships.

  Accordingly, it is possible to select an optimum correspondence (the above-mentioned selection correspondence) by estimating the Jacobian in each correspondence and determining the validity of the estimated Jacobian. In robot control using the Jacobian, the robot control signal (specifically, information indicating how much the joint angle should be changed) is determined by the Jacobian. Therefore, whether or not the Jacobian is accurate is determined by the robot control. It is an important indicator for determining accuracy. Therefore, here, when a plurality of correspondence relationships are set, the Jacobians (for all of them in a narrow sense) are calculated, and the selected correspondence relationship is determined using the evaluation value of the Jacobian.

  Further, the robot controller 200 creates a normalization equation in each of the plurality of correspondences between the image sampling timing and the joint angle sampling timing, and obtains a least squares solution that is a solution of the created normalization equation. You may estimate the Jacobian.

  This makes it possible to estimate the Jacobian by solving the normalization equation. In the Jacobian, the above expression (1) is established from the definition, and the above expression (4) is established by changing the expression form. In this case, since it is known that the least square solution of the above equation (6) is given as a solution of the normalized equation, the Jacobian is estimated as the least square solution by solving the normalized equation in this embodiment as well. To do. The above equation (4) is obtained by obtaining the transpose matrix on both sides of the above equation (1), but the above equation (1) is modified by another form, and the simultaneous equations corresponding to the above equation (6) are obtained. You may ask for it.

  In addition, the robot control unit 200 uses the estimated Jacobian to calculate image feature amount information at a comparison timing that is a timing next to the timing at which the Jacobian is estimated, and obtains the calculated image feature amount information and image feature amount information. The evaluation value may be obtained based on the image feature amount information at the comparison timing acquired by the unit 20.

  Here, the comparison timing represents a timing next to the Jacobian estimation timing, and corresponds to, for example, A1 in FIG. By causing the estimated Jacobian to act on the difference value (Δθ) of the joint angle information at the timing corresponding to the Jacobian, the difference value (Δf) of the image feature amount information between the timing and the next timing is estimated. can do.

  Thereby, it is possible to obtain the evaluation value of the Jacobian estimated by the method shown in FIG. The image feature amount information acquired in A1 is an actual measurement value, and the image feature amount information obtained by applying Jacobian to the difference between the joint angle information of B2 and B3 is an estimated value. As the accuracy of the Jacobian is higher, the estimated value is considered to be closer to the actually measured value. Therefore, the evaluation value may be determined according to the difference between the actually measured value and the estimated value. For example, if an evaluation value is used such that the smaller the value is, the higher the evaluation is, the Jacobian evaluation value is a value representing the difference between the actual measurement value and the estimated value (for example, each element of the image feature amount information (feature amount vector)). The sum of squares of the differences may be used as the evaluation value, in this case, even if the conditional expression is not excessive or insufficient with respect to the number of unknowns (such as the case shown in FIG. 7B), the evaluation value is appropriately set. Can be requested.

  Further, the robot control unit 200 may obtain the square error of the normalized equation as an evaluation value using the estimated Jacobian.

  As a result, the square error expressed by the above equation (7) can be used as the Jacobian evaluation value. The plurality of Jacobians are all obtained as least squares solutions, but the value of the square error when the least squares solution is substituted into the above equation (7) differs depending on the Jacobian. In this case, it can be considered that the smaller the square error value, the higher the estimation accuracy of the Jacobian, and therefore the square error value of the above equation (7) can be used as the Jacobian evaluation value. In this case as well, the smaller the evaluation value, the higher the validity of the Jacobian.

Further, the robot control unit 200 sets F as a vector or matrix represented by the difference in image feature information, B as a matrix represented by the difference in joint angle information, and J as a vector or matrix representing Jacobian. = creates a normal equation which gives the least squares solution for BJ, for matrices B, and eigenvectors of B T ^B the matrix of a column vector and V, and a matrix of eigenvectors of BB ^T and the column vectors and U , Where B ⁺ = Vdiag [1 / λ] V ^T B ^T or B ⁺ = B ^T Udiag [, where diag [1 / λ] is a matrix whose diagonal element is the inverse of the eigenvalue λ of B ^T B the 1 / λ] U ^T, generalized determined inverse matrix B ⁺ of the matrix B, and the obtained generalized inverse matrix B ^+, may be estimated Jacobian by obtaining matrix J in accordance with J = B + ^F.

Thus, when obtaining the generalized inverse matrix B ⁺ , unlike the case of obtaining by the above equation (10), it is not necessary to perform a general inverse matrix operation, and it is stable (robustly) regardless of the rank of the matrix B. Processing can be performed. Further, when compared with the method of performing singular value decomposition of the above equation (12), in the above equation (12), it is necessary to use both orthonormal bases U and V for the calculation. Either one can perform inverse Jacobian operations. The sizes of U and V are determined according to how many sets of image feature amount information and joint angle information are used. According to the method of the present embodiment, a normal amount with a small amount of calculation according to the situation. Since the orthogonal basis can be appropriately selected, the generalized inverse matrix B ⁺ can be obtained at a higher speed than in the above equation (12).

When the sampling frequency of the image sampling timing in the image feature amount information acquisition unit 20 is f _A and the sampling frequency of the joint angle sampling timing in the joint angle information acquisition unit 10 is f _B , the joint angle information acquisition unit 10 The joint angle information may be acquired at a sampling frequency satisfying f _B ≧ f _A.

  As a result, it is possible to perform the processing of the present embodiment when the joint angle information is acquired more frequently than the image feature amount information. In this case, processing is efficiently performed by selecting an optimum correspondence by searching for a plurality of joint angle sampling timings that can have a correspondence with the image sampling timing with reference to a small number of image sampling timings. Is possible. When the range of fluctuation is narrow, the number of joint angle information corresponding to one piece of image feature amount information is small (in the above example of FIG. 3C, three pieces of information are obtained for one piece of image feature amount information. This is because it is possible to skip processing for joint angle sampling timings that do not need to be processed.

  Further, the present embodiment described above can be applied to a robot including the above robot control system.

  The robot (robot system) here may be constituted by a robot main body 310 and a control device 300 (corresponding to the robot control system described above) as shown in FIG. Note that the robot body 310 and the control device 300 may be configured integrally as shown in FIG. Further, the robot control in the present embodiment may be arm control in a narrow sense.

  Note that the robot control system or the like according to the present embodiment may realize part or most of the processing by a program. In this case, the robot control system or the like of the present embodiment is realized by a processor such as a CPU executing a program. Specifically, a program stored in a non-temporary information storage medium is read, and a processor such as a CPU executes the read program. Here, the information storage medium (computer-readable medium) stores programs, data, and the like, and functions as an optical disk (DVD, CD, etc.), HDD (hard disk drive), or memory (card type). It can be realized by memory, ROM, etc. A processor such as a CPU performs various processes according to the present embodiment based on a program (data) stored in the information storage medium. That is, in the information storage medium, a program for causing a computer (an apparatus including an operation unit, a processing unit, a storage unit, and an output unit) to function as each unit of the present embodiment (a program for causing the computer to execute processing of each unit) Is memorized.

3. Second Embodiment In the present embodiment, an example will be described in which a normalization equation is weighted based on analytically obtained robot Jacobian to improve Jacobian estimation accuracy.

3.1 Robot Jacobian As described in the first embodiment, for the estimation of the Jacobian, it is necessary to associate a plurality of sets of image feature amount information and joint angle information. In other words, when obtaining the Jacobian at a given timing, data of a certain amount of time (for example, data up to 7 timings before and 70 milliseconds when the image sampling timing is a 10 ms cycle) is used. It will be necessary. During this time, if the Jacobian is constant, there is no problem with the accuracy of the estimated Jacobian, but the Jacobian can change due to a change in the robot posture during the above time. If one Jacobian is to be obtained using data at a certain time and the Jacobian can vary between them, a problem may occur in the accuracy of the estimated Jacobian. However, this point is not particularly considered in the first embodiment. Therefore, in the second embodiment, accuracy is improved by using a robot Jacobian.

  First, the robot Jacobian will be described. Jacobian represents a first-order coefficient when a multivariable nonlinear vector function is expanded in the vicinity of a given point. That is, Jacobian can be said to give a linear approximation of how a small displacement in a certain coordinate system is mapped to a coordinate system mapped by a nonlinear vector function.

  Therefore, various nonlinear vector functions (and coordinate systems mapped thereby) can be considered, and as a Jacobian, a robot Jacobian (FIG. 1 (A)) that maps a minute displacement in a joint angle space to a hand position space can be considered. It is also possible to consider an image feature amount Jacobian (FIG. 1B) that maps a minute displacement in the joint angle space to the image feature amount space. Note that as long as image feature amount information is used as feature amount information, the Jacobian obtained by the method of the first embodiment is an image feature amount Jacobian.

Here, it is possible to control the hand posture of the robot based on the image feature amount information as in visual servo, etc., because conversion processing between the image feature amount information and the hand posture is possible (in other words, A function that maps to the other). In other words, when mapping the minute displacement in the joint angle space to the image feature amount space, direct mapping (conversion by the image feature amount Jacobian J _θ-f ) and the minute displacement in the joint angle space on the hand on mapped onto the pose space (converted by a robot Jacobian J _θ-p), equivalent to what a minute displacement in the hand pose space maps to the image feature amount space (referred to as convenience Jacobian J _p-f) The following formula (16) is established. That is, the image feature amount Jacobian can be decomposed as shown in FIG. 11 using the robot Jacobian.

_{J θ-f = J θ-} p J p-f ····· (16)
Here, in the visual servo or the like, the position of the camera is normally fixed, and it is general that the image feature amount information can be the same if the hand posture of the robot is the same. That is, when the image Jacobian J _θ-f changes during the acquisition period of information used for Jacobian estimation (70 milliseconds in the above example), it is considered that the change of the robot Jacobian J _θ-p is dominant.

It is known that the robot Jacobian J _θ-p can be obtained by analysis when the robot responds to the joint angle as a theoretical link diameter. That is, in the present embodiment, the degree of fluctuation of the image Jacobian J _θ-f is estimated based on the analytically obtained robot Jacobian, and the estimation accuracy of the image Jacobian J _θ-f is increased by weighting according to the estimation result. Improve. Since various methods for analytically obtaining the robot Jacobian are known, detailed description is omitted. An actual robot does not perform an ideal motion, and an error may occur in the robot Jacobian that is analytically determined. However, the robot Jacobian in this embodiment is for setting a weighting coefficient, and the robot Even if an error occurs in the Jacobian, it is not a problem.

3.2 Weighted Normalization Equation In this embodiment, a weighting coefficient is calculated using a robot Jacobian, and a normalization equation is created using the weighting coefficient. Specifically, when the correspondence relationship is set by the correspondence relationship setting unit 210, each difference vector (the difference between the joint angle vectors and the difference between the image feature amount vectors) is used using the difference value of the joint angle information. Find the corresponding robot Jacobian analytically.

  The robot Jacobian corresponding to the given difference vector is set as the reference robot Jacobian. The weighting for each difference vector may be set by the difference between the robot Jacobian corresponding to the difference vector and the reference robot Jacobian. The larger the difference is, the larger the variation of the robot Jacobian is. Therefore, the weight for the corresponding difference vector is set to be small (the contribution of the difference vector to the processing result is small).

  Specifically, when the robot Jacobian of (1) in FIG. 7A is used as a reference, the degree of difference is obtained by the following expression (17) for each of (1) to (7), and the following expression (18) To obtain a value for normalization. A value such as the following formula (19) may be used as the weighting factor. When comparing (1), the degree of difference is 0, so the weighting factor is 1. Further, the weight coefficient approaches 0 as the degree of difference between the robot Jacobians increases.

Err (J _i , J _j ) = Σ _k (abs (J _i (k) −J _j (k))) (17)
N = sqrt (Σ _j (Err (J ₀ , J _j )) ² ) (18)
w _i = 1.0−abs (Err (J ₀ , J _i )) / N (19)
In this way, the equation shown in FIG. 12 can be used instead of the simultaneous equations shown in FIG. After the equation of FIG. 12 is obtained, the least square solution of the equation may be obtained as a solution of the normalized equation as in the first embodiment. Specifically, a process for obtaining a generalized inverse matrix may be performed on the matrix B determined by Δθ after weighting.

3.3 Processing Details The processing flow of this embodiment will be described with reference to the flowchart of FIG. As is clear from FIG. 13, steps S201 to S204 are the same as steps S101 to S104 in FIG. 10, and steps S208 to S213 are the same as steps S106 to S111 in FIG. Is omitted.

  In this embodiment, after one correspondence is selected in step S204, the robot Jacobian is calculated analytically using the information at that time (step S205). Then, a weighting coefficient is calculated based on the robot Jacobian by a method such as the above formulas (17) to (19) (step S206). Then, as shown in FIG. 12, a normalization equation is created using the weighting coefficient, and a Jacobian is estimated as a least squares solution that is a solution of the normalization equation (step S207).

  The processing after the Jacobian is estimated is the same as in the first embodiment.

  In the present embodiment described above, the robot control unit 200 may obtain a robot Jacobian that associates the end point position of the robot with the joint angle information, and may create a normalization equation based on the obtained robot Jacobian. The robot control unit 200 may analytically obtain a robot Jacobian and create the normalization equation based on the obtained robot Jacobian.

  This makes it possible to create a normalization equation based on the obtained robot Jacobian (analytical in a narrow sense) and estimate the image feature amount Jacobian based on the normalization equation. As described above, it is necessary to use the data acquired for estimating the image feature amount Jacobian over a certain period, and when the image feature amount Jacobian has changed within the period, the timing of the change is used. The difference value corresponding to (the difference value between the image feature amount information and the joint angle information) may have an adverse effect such as a decrease in accuracy in the estimation of the image feature amount Jacobian. As described above, when the image feature amount Jacobian is expressed as a product of a plurality of terms including the robot Jacobian, the variation degree of the image feature amount Jacobian is mainly represented by the variation degree of the robot Jacobian. That is, the greater the fluctuation of the robot Jacobian, the smaller the contribution of the difference value in the image feature amount Jacobian estimation process at that time, thereby enabling more accurate processing.

  Further, the robot control unit 200 includes a robot Jacobian at a reference timing (for example, timing 0 in the above equation (19)) and a given timing (timing i in the above equation (19), i == in the narrow sense. 0-7) Based on the comparison processing with the robot Jacobian, the weighting is set for the set of the difference value (Δf) of the image feature amount information and the difference value (Δθ) of the joint angle information at the given timing. The normalization equation may be created using the set weighting.

  Accordingly, it is possible to improve the estimation accuracy of the image feature amount Jacobian by creating a normalization equation using weighting. As an example of weighting, the weighting coefficient obtained by the above equations (17) to (19) may be used. In that case, the Jacobian is obtained by solving the least squares solution of the equation shown in FIG. It should be noted that the larger the fluctuation of the robot Jacobian, the smaller the value of the weighting factor is, and the specific calculation method is not limited to the above equations (17) to (19).

10 joint angle vector acquisition unit, 20 feature vector acquisition unit, 30 storage unit,
31 joint angle vector sequence storage unit, 33 feature vector sequence storage unit,
35 target feature amount storage unit, 200 robot control unit, 210 correspondence setting unit,
220 Jacobian estimator, 221 variance matrix calculator, 222 eigenvalue calculator,
223 inverse calculation unit, 224 eigenvector matrix calculation unit,
225-1, 225-2 transpose matrix calculation unit, 226 matrix / vector product calculation unit,
227-1, 227-2 matrix product calculation unit, 230 evaluation processing unit,
240 inverse Jacobian calculation unit, 250 target difference vector calculation unit,
260 matrix / vector product calculation unit, 270 control data output unit, 300 control device,
310 robot body, 320 arms

Claims (16)

An image feature amount information acquisition unit for acquiring image feature amount information at a plurality of image sampling timings;
A joint angle information acquisition unit that acquires joint angle information of the robot at a plurality of joint angle sampling timings at a sampling timing asynchronous with the image sampling timing;
A robot control unit that controls the robot based on the image feature amount information and the joint angle information in a selection correspondence selected from a plurality of correspondences between the image sampling timing and the joint angle sampling timing When,
A robot control system comprising: In claim 1,
The robot controller is
A robot control characterized in that a Jacobian in the selection correspondence is estimated based on the image feature amount information and the joint angle information in the selection correspondence, and the robot is controlled based on the estimated Jacobian. system. In claim 2,
The robot controller is
Estimating the Jacobian in each of the plurality of correspondences between the image sampling timing and the joint angle sampling timing, and obtaining an estimated value of the estimated Jacobian,
The robot control system, wherein the selection correspondence is selected from a plurality of the correspondences based on the evaluation value comparison process. In claim 3,
The robot controller is
A normalization equation is created in each of the plurality of correspondence relationships between the image sampling timing and the joint angle sampling timing, and the Jacobian is obtained by obtaining a least square solution that is a solution of the created normalization equation. A robot control system characterized by estimating. In claim 4,
The robot controller is
Using the estimated Jacobian, the image feature amount information at a comparison timing that is a timing next to the timing at which the Jacobian is estimated is calculated, and the calculated image feature amount information and the image feature amount information acquisition unit acquire The robot control system characterized in that the evaluation value is obtained based on the image feature amount information at the comparison timing. In claim 4,
The robot controller is
A robot control system characterized in that a square error of the normalization equation is obtained as the evaluation value using the estimated Jacobian. In any one of Claims 4 thru | or 6.
The robot controller is
The vector or matrix represented by the difference between the image feature information is F, the matrix represented by the joint angle information difference is B, and the vector or matrix representing the Jacobian is J.
F = BJ Equation (1)
Create the normalized equation that gives the least squares solution of
With respect to the matrix B, and a matrix of the eigenvectors of B T ^B column vector and V, and matrix with the column vector of the eigenvectors of BB ^T and U, diagonal the inverse of the eigenvalue λ of B T ^B When the matrix as an element is diag [1 / λ],
B ⁺ = Vdiag [1 / λ] V ^T B ^T Equation (2)
Or B ⁺ = B ^T Udiag [1 / λ] U ^T (3)
To obtain the generalized inverse matrix B ⁺ of the matrix B, and from the obtained generalized inverse matrix B ⁺ and the equation (1),
J = B ⁺ F (4)
The Jacobian is estimated by obtaining the matrix J in accordance with the robot control system. In any of claims 4 to 7,
The robot controller is
A robot control system characterized in that a robot Jacobian that associates the end point position of the robot with the joint angle information is obtained, and the normalization equation is created based on the obtained robot Jacobian. In claim 8,
The robot controller is
Based on the comparison process between the robot Jacobian at the reference timing and the robot Jacobian at the given timing, the difference value of the image feature amount information and the difference value of the joint angle information at the given timing A robot control system characterized by setting a weight for a set and creating the normalization equation using the set weight. In claim 8 or 9,
The robot controller is
A robot control system characterized by analytically obtaining the robot Jacobian and creating the normalization equation based on the obtained robot Jacobian. In any one of Claims 1 thru | or 10.
When the sampling frequency of the image sampling timing in the image feature amount information acquisition unit is f _A , and the sampling frequency of the joint angle sampling timing in the joint angle information acquisition unit is f _B ,
The joint angle information acquisition unit
The robot control system, wherein the joint angle information is acquired at the sampling frequency satisfying f _B ≧ f _A. An image feature amount information acquisition unit for acquiring image feature amount information at a plurality of image sampling timings;
A joint angle information acquisition unit for acquiring joint angle information of the robot at a plurality of joint angle sampling timings;
A robot control unit that controls the robot based on the image feature amount information and the joint angle information in a selection correspondence selected from a plurality of correspondences between the image sampling timing and the joint angle sampling timing When,
A robot control system comprising:   A robot comprising the robot control system according to claim 1. Obtain image feature information at multiple image sampling timings,
It is a plurality of joint angle sampling timing, and at the sampling timing asynchronous with the image sampling timing, acquires the robot joint angle information,
The robot is controlled based on the image feature amount and the joint angle information in a selection correspondence selected from a plurality of correspondences between the image sampling timing and the joint angle sampling timing. Robot control method. An image feature amount information acquisition unit for acquiring image feature amount information at a plurality of image sampling timings;
A joint angle information acquisition unit that acquires joint angle information of the robot at a plurality of joint angle sampling timings at a sampling timing asynchronous with the image sampling timing;
As a robot control unit that controls the robot based on the image feature amount and the joint angle information in a selection correspondence selected from a plurality of correspondences between the image sampling timing and the joint angle sampling timing ,
A program characterized by operating a computer. Obtain image feature information at multiple image sampling timings,
It is a plurality of joint angle sampling timing, and at the sampling timing asynchronous with the image sampling timing, arm joint angle information is acquired,
The arm is controlled based on the image feature amount and the joint angle information in a selection correspondence selected from a plurality of correspondences between the image sampling timing and the joint angle sampling timing. Robot to do.

Images