JP4222338B2 - Adaptive visual feedback control method - Google Patents

Description

  The present invention relates to a control method for a link mechanism having multiple joints such as a robot hand arm, and more particularly to an adaptive visual feedback control method suitable for performing control based on an image acquired by an imaging apparatus.

  A control method called visual feedback control is known that feedback-controls the operation of a mechanical device having a link mechanism such as a robot hand based on image information taken by a camera such as a robot eye (see, for example, Patent Document 1).

In the technique described in Patent Document 1, the local two-dimensional velocity required when converting the motion (two-dimensional velocity vector) of the feature portion of the target object on the image coordinate system into the motion of the feature portion on the work coordinate system. A vector coordinate change matrix (image Jacobian matrix) is estimated from the movement of the target object on the work coordinate system to control the operation of the mechanical device with respect to the target object.
JP-A-11-85235

  However, with this technique, the movement of the target object on the work coordinate system is limited to a movement on a predetermined constraint plane, and cannot cope with movement in a three-dimensional space. In a visual feedback control system, a sensor that captures an image at a video frame rate, such as a television camera, and an image processing system are usually used as a visual sensor, but until the actual movement of the work object is transmitted to the control system. However, since it is inevitable that the delay is higher than the video frame rate, there is a problem that the feedback loop becomes slow, the control input becomes excessive and becomes unstable, and the tracking error tends to increase.

  SUMMARY OF THE INVENTION Accordingly, it is an object of the present invention to provide a visual feedback control method for a link mechanism that can respond to a movement in a three-dimensional space, can follow a target trajectory with high accuracy, and does not require complicated parameters. .

In order to solve the above problems, a visual feedback control method according to the present invention is a method for performing feedback control of a link mechanism having multiple joints based on information obtained by performing image processing on an image acquired by an imaging device. Based on the target position and orientation of the link mechanism, the feedforward control term and the predictive control term are used, and the Jacobian matrix describing the relationship between the variation of the position and orientation of the link mechanism and the variation of the joint angle is estimated from the tracking error And an adaptive visual feedback control method for obtaining a control command value by each control term set using the estimated Jacobian matrix and its pseudo inverse matrix .
It is preferable that the Jacobian matrix is estimated based on a modified formula obtained by adding a constant smaller than 1 to the denominator of the time difference term of the Jacobian matrix, using the time differential value of the position and orientation of the link mechanism and the joint angular velocity.

The deviation from the target trajectory is corrected by adding a feedforward term and a preview control term in addition to the feedback control term during control. Also, complicated parameter identification is avoided by estimating the Jacobian matrix from the tracking error and obtaining the control command value.

According to the present invention, by using the feedforward term and the predictive control term, it is possible to suppress the control input from becoming excessive and to realize stable control, thereby improving the accuracy of followability to the target trajectory. it can. In addition, by performing online estimation of the Jacobian matrix and avoiding complicated parameter identification, calculation of the control input is facilitated and application to motion in a three-dimensional space is also possible.

  DESCRIPTION OF EXEMPLARY EMBODIMENTS Hereinafter, preferred embodiments of the invention will be described in detail with reference to the accompanying drawings. In order to facilitate the understanding of the description, the same reference numerals are given to the same components in the drawings as much as possible, and duplicate descriptions are omitted.

  FIG. 1 is a schematic configuration diagram of a robot having a robot hand that realizes the adaptive visual feedback control method of the robot hand according to the present invention, and FIG. 2 is a block configuration diagram of a control device.

  As shown in FIG. 1, robot hands 1R, 1L are attached to the body 8 of the robot 100 via both left and right arms 7L, 7R. The body 8 is attached to a movable or fixed base. The head of the body 8 is provided with a camera eye 3 that is a visual sensor. A motor for driving the joint and an encoder potentiometer for detecting the joint angle are arranged at each joint of the hand 1 and the arm 7.

  Here, each arm 7 has seven degrees of freedom up to the wrist portion of the hand 1. The hand 1 has a thumb and three fingers facing it, and the thumb 10 is a four-degree-of-freedom link system in which a motor is arranged for each of four joints. In the other three fingers, a motor is arranged for each of the three joints excluding the most advanced joint, and the most advanced joint is configured to interlock with the adjacent joint. Therefore, each of the four joints constitutes a link system having three degrees of freedom. That is, the degree of freedom of the entire robot hand 1 is 13.

  The control device is mainly configured by a control ECU 2 including a RAM, a ROM, a CPU, and the like. An image recognition unit 20 that recognizes the position / posture of the robot hand 1, a position / posture calculation unit 21 that obtains a target value of the gripping position / griping posture based on the image recognition result, and a movement of the robot hand 1 according to the target value And a robot control unit 22 for controlling. The robot controller 22 receives the output signals of the encoder potentiometers 16 and the tactile sensors 15 arranged at the fingertips of the fingers, and controls the motor driver 4 to control the movement of the motors 14. Then, the movements of the robot hand 1 and the arm 7 are controlled.

  The control device is not limited to such a configuration, and a control unit that controls the movement of the robot hand 1 and the arm 7 so as to have an instructed posture, and an image recognition device and a gripping posture calculation unit are separately provided. It is good. In addition, these calculation units / control units may be divided by hardware or software.

A block diagram showing a control system of control (adaptive visual feedback control) performed by the control ECU 2 is shown in FIG. Here, the current time is represented by t _k , the time of the previous time step is represented by t _k−1 , and the time of the next time step is represented by t _{k + 1} .

The target trajectory setting unit 201 outputs the current and future (next time step) target hand (arm) positions r _d (t _k ) and r _d (t _{k + 1} ), respectively. Of these, the future target hand position r _d (t _{k + 1} ) is input to the trajectory planning unit 202, and the future target value q _d (t _{k + 1} ) of the joint angle is output. On the other hand, the current arm joint angle q (t _k ) is detected by the encoder potentiometer 16, and the current hand position r (q _k ) is output from the image recognition unit 20 based on image processing. (Representing a symbol indicating & over q of q (t _k).) Arm joint driving angular velocity obtained by time differentiation in the position and orientation calculation unit 21 to the arm joint angle q (t _k).

The command value u (t _k ) for each motor includes three terms, a feedback control term, a feedforward control term, and a preview control term.

The feedback control term multiplies the difference between the target hand position r _d (t _k ) output from the target trajectory setting unit 201 and the actual hand position r (q _k ) by a predetermined coefficient K to estimate a Jacobian matrix that will be described later. It is defined as a value obtained by multiplying a pseudo inverse matrix (represented by adding the superscript + to the estimated value of the Jacobian matrix) of the value (represented by adding the symbol ^ on Jk). This term ensures the stability of the control closed loop.

The feedforward control term was _obtained by time-differentiating the target hand position r _d (t _k ) output from the target trajectory setting unit 201 and multiplying the pseudo inverse matrix of the estimated value of the Jacobian matrix in the same manner as the feedback control term. Defined as a value. This term is a term for causing the movement of the hand 1 and arm 7 to follow the target trajectory.

The preview control term is an estimation of a Jacobian matrix to be described later from the unit matrix with respect to the difference between the target joint angle q _d (t _{k + 1} ) output from the trajectory planning unit 202 and the actual current joint angle q (t _k ). It is defined as a value obtained by multiplying a matrix obtained by subtracting a matrix obtained by multiplying a pseudo inverse matrix of values by an estimated value matrix of a Jacobian matrix. This term improves the following accuracy and suppresses the control input to realize stable control.

Expression (1) is an expression representing the command value u (t _k ), and the first to third terms on the right side correspond to a feedback control term, a feedforward control term, and a preview control term, respectively.

  Next, estimation of the Jacobian matrix will be described. Let e be a tracking error and define e by equation (2).

At this time, the Jacobian matrix is expressed by Equation (3).

By applying the Brauden method and deducing an estimation expression from Expression (2) and Expression (3), Expression (4) is derived.

When formula (4) is arranged, the following formula (5) is obtained.

If the equation (5) remains as it is, the estimated value diverges when the differential value of q _k is approximately 0. Therefore, using a positive constant ε smaller than 1, equation (5) is expressed as equation (6). Modified to use.

  The Jacobian matrix estimation unit 203 performs online estimation of the Jacobian matrix based on Equation (6), and outputs it.

  Next, the process of the control method according to the present invention will be described with reference to the flowchart of FIG. First, the image recognition unit 20 recognizes the position of the target object by performing image processing on the image acquired by the camera eye 3 (step S1). Next, the position / orientation calculation unit 21 plans an operation path between the arm 7 and the hand 1 (step S2), and stores the planned path (step S3). The plan of this motion path is to set the position coordinates of the contact point at the time of gripping, calculate the positions and postures of the hand 1 and the arm 7 in reverse kinematics, and then set the target of each hand 1 and arm 7. A trajectory may be set.

  After the setting, the driving of the hand 1 and the arm 7 is controlled using the control system described above (step S4), and the arm 7 and the hand 1 are moved along the set path to hold the object. . Further, a predetermined operation (for example, moving the object from one place to another) is performed while holding the object (step S5). When the predetermined operation is completed, the process is terminated.

  The inventors have performed a simulation for confirming the effect of improving the followability to the target trajectory by the control method of the present invention, and the results will be described below. FIG. 5 shows a target trajectory targeted in the simulation. In the XYZ coordinate system, the arm is moved by a straight path from an initial position of (379.1, −311.5, 714.7) mm to a target position of (529.1, −161.5, 864.7) mm think of. Here, the movement time from the initial position to the target position is set to 10 seconds, the sampling time of the control system is set to 40 ms, and the arm of the two types of control methods without foresight control and with foresight control is used. The trajectories were compared. The following initial values were used as initial values of the Jacobian matrix.

  6A to 6C show the X, Y, and Z coordinate values of the movement trajectory in each control method compared with the target trajectory. As shown in the figure, in visual feedback control that does not use preview control, the deviation from the target locus is large. In this simulation, this deviation does not reach divergence, but it is at most 100 mm away from the target position, and there is a possibility of collision if there is an obstacle or the like nearby.

  In contrast, in visual feedback control using preview control, a deviation from the target trajectory occurs at the beginning of the motion, but converges early and hardly sees any deviation thereafter. The initial shift is caused by the shift of the initial value of the Jacobian matrix, and it is considered that it can be further reduced by appropriately setting according to the movement.

  From the above simulation results, the effect of improving the followability to the target trajectory by the control method of the present invention was confirmed. In addition, by estimating the Jacobian matrix online, it was possible to confirm the effect of adapting to the actual movement while avoiding a complicated identification process.

  Here, the robot arm is verified by simulation, but the same effect can be obtained for the robot hand. The scope of application of the present invention is not limited to such robot hands and arms, but can be widely used as control of a link mechanism having multiple joints.

It is a schematic block diagram of the robot which has the robot hand which implement | achieves the adaptive visual feedback control method of the robot hand which concerns on this invention. It is a block block diagram of the control apparatus of the robot of FIG. It is a block diagram which shows the control system of FIG. It is a flowchart which shows the control processing of FIG. It is a figure which shows the target track | orbit made into object by simulation. The X, Y, and Z coordinate values of the movement trajectory in the two controls with and without preview control are shown in comparison with the target trajectory.

Explanation of symbols

  DESCRIPTION OF SYMBOLS 1 ... Robot hand, 3 ... Camera eye, 4 ... Motor driver, 7 ... Arm, 8 ... Body, 14 ... Motor, 16 ... Encoder potentiometer, 15 ... Tactile sensor, 20 ... Image recognition part, 21 ... Position and orientation calculation Unit 22, robot control unit 100, robot 201, target trajectory setting unit 202, trajectory plan unit 203, Jacobian matrix estimation unit

Claims (2)

In a method of performing feedback control of a link mechanism having a multi-joint based on information obtained by performing image processing on an image acquired by an imaging device,
Based on the target position and orientation of the link mechanism, the feedforward control term and the predictive control term are used, and the Jacobian matrix describing the relationship between the variation of the position and orientation of the link mechanism and the variation of the joint angle is estimated from the tracking error. An adaptive visual feedback control method characterized in that a control command value is obtained by each control term set using an estimated Jacobian matrix and its pseudo inverse matrix .   Using the time differential value of the position and orientation of the link mechanism and the driving angular velocity of the joint, and estimating the Jacobian matrix based on a modified formula obtained by adding a constant smaller than 1 to the denominator of the time difference term of the Jacobian matrix. The adaptive visual feedback control method according to claim 1.

Images