JP2007011978A - Motion controller for robot - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a motion controller for a robot, capable of performing control processing at high speed with high accuracy. <P>SOLUTION: In this motion controller, a forward kinematics arithmetic part 251 finds actual position/attitude information ξ from a joint angle θ, and a double feedback group wherein a feedback group of a second feedback control system performing visual feedback on the basis of a recognition result of an image recognition part 20 is present outside a feedback group of a first feedback control system 25 performing feedback control is configured. A present position/attitude or the like of a hand is estimated from the recognition result of the image recognition part 20 by a first motion estimation part 261, and delay of control of the visual feedback is compensated. <P>COPYRIGHT: (C)2007,JPO&INPIT

Description

  The present invention relates to a motion control apparatus that controls the position and posture of a robot.

  A robot hand that performs a predetermined operation such as holding an object and moving the object to another location is known. Rather than repeatedly performing the same action on the same type of object, trying to perform various actions on objects with various properties (sizes and shapes) complicates the movement, and not only the position / posture Since force control is also required, the control itself is complicated. Techniques have been proposed for accurately controlling such multipurpose robot hands (see, for example, Patent Document 1 and Patent Document 2).

  In the technique of Patent Document 1, a joint selected by multiplying a displacement error signal representing a difference between an actual displacement of a hand position of a robot hand and a desired displacement by a pseudo inverse matrix of a matrix product of a selection matrix and a Jacobian matrix. Calculate the displacement error signal. At the same time, a force error signal is generated from the difference between the actual force acting on the hand and the desired force, and similarly selected joint force error signal is calculated. A control signal is generated based on the calculated joint displacement error signal and joint force error signal, the hand is moved to a desired position, and a desired force is exhibited.

In the technique of Patent Document 2, a work object is previously photographed by a visual sensor and stored as a reference image, and the position and orientation are controlled so that the image of the work object photographed by the visual sensor during work is in the same state as the reference image. It performs visual feedback control.
JP-A-5-143161 JP 2003-305676 A

  However, the technique of Patent Document 1 may cause a deviation between the hand position grasped by the control unit and the actual position due to the manufacturing accuracy of the robot, the control error of the movable unit, and the like. You may not be able to guide your hand.

  Moreover, if the technique of patent document 2 is used, since position / posture information can be acquired from a visual sensor, it is possible to correct the deviation between the hand position grasped by the control unit and the actual position. At the frame rate of the camera used for the visual sensor, high-speed feedback control cannot be performed. A camera with a high frame rate that supports high-speed feedback control is expensive, and the image processing system needs to be sped up, so that it is not easy to apply to a robot hand. In addition, control cannot be performed when an image of an object cannot be normally acquired due to an obstacle or the like.

  SUMMARY OF THE INVENTION An object of the present invention is to provide a robot motion control apparatus that enables high-speed and accurate control processing.

  In order to solve the above-described problems, a robot motion control device according to the present invention is a control device that controls the motion of a robot having a part that can be mechanically moved by a drive mechanism, and the position of the part that can be moved without contact. And an external sensor for detecting the target position of the movable part, an internal sensor for acquiring the positional relationship between the movable parts according to the drive of the drive mechanism, and a movement for controlling the movement of the movable part toward the target position A deviation between the position of the movable part acquired using the control means and the external sensor and the position of the movable part acquired using the internal sensor is obtained, and the target position of the movable part used for control in the movement control means is obtained. And a position correcting means for correcting.

  As the external sensor, a non-contact type sensor such as a visual sensor, an ultrasonic sensor, or a radar is used. Further, as the internal sensor, a joint angle position sensor arranged at the joint of the robot, an angle sensor of a drive motor, or the like is used. The target position of the movable part used for the control is verified and corrected based on the position information of the movable part obtained by the external sensor and the inner sensor.

  If the external sensor has a longer sampling time than the position correction of the position correction means, the position information by the internal sensor based on the time change of the position of the movable part acquired using the external sensor It is preferable to further include position estimation means for estimating the position information of the movable part corresponding to the acquisition time point and outputting it to the position correction means.

  The position correction timing by the position correction means matches the control sampling time of the movement control means. It is preferable to shorten the control sampling period in order to perform a highly accurate and highly followable motion. On the other hand, when a visual sensor or the like is used as the external sensor, it takes a long time to acquire the position information, and the acquisition of the position information does not coincide with the sampling by the internal sensor, and every several times or more. Will be acquired. The position estimation means estimates the position information of the movable part based on the position information obtained by the external sensor at the time when the position information is obtained by the internal sensor.

  Here, it is preferable to further include prohibiting means for prohibiting correction by the position correcting means when it is determined whether or not the position information acquisition by the external sensor is successful and it is determined that the position information acquisition is not successful. This is to prevent the position correction unit from correcting the position based on the incorrect position information when the position information acquisition by the external sensor is not successful.

  The sampling cycle for acquiring the position information of the external sensor is preferably an integral multiple of the position information acquisition cycle of the internal sensor, which makes it easier to match the timing of the position information correction and control.

  According to the present invention, the difference between the position information of the movable part obtained based on the positional relationship of the drive mechanism using the inner world sensor and the position information of the movable part obtained without contact using the outer world sensor is used. Since the target position used for the control is corrected, the shift between the position of the movable part grasped by the control unit and the actual position of the movable part can be corrected, and the control can be performed with high accuracy.

  Further, when the sampling time of the external sensor is long, by providing a position estimation means capable of estimating position information with a shorter sampling time, for example, even when using a visual sensor with a low video acquisition rate, It is possible to perform control, and it is possible to cope with fast movement.

  Further, when it is determined that the position information acquisition by the external sensor has not been successful, the control disturbance is suppressed by prohibiting the position correction. If the sampling period for acquiring the position information of the external sensor is an integral multiple of the sampling period for acquiring the position information by the internal sensor, the positional information correction and control synchronization are facilitated.

  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 motion control device according to the present invention, and FIG. 2 is a block configuration diagram of the 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.

  The control device is mainly configured by a control ECU 2 including a RAM, a ROM, a CPU, and the like, and the object and the object are detected by an image recognition process from an output image of the camera eye 3 that captures an image of the object and the robot hand 1. 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 signal of each encoder potentiometer 16 and controls the motor driver 4 to control the movement of each motor 14 to control the movement of the robot hand 1 and the arm 7. .

  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.

  FIG. 3 is a block diagram showing a control system of control (composite visual feedback control) performed by the control ECU 2. This control system includes a first feedback control system 25 and a second feedback control system 26, and the feedback loop of the second feedback control system exists outside the feedback loop of the first feedback control system 25. A double feedback loop is configured.

First, the configuration of the first feedback control system 25 will be described. In the first feedback control system 25, the joint angle θ measured by each encoder potentiometer 16 arranged at the joint of the robot 100 is input to the forward kinematics computation unit 251, and the hand 1 arm 7 is obtained. A difference between the actual position / posture information ξ and a feature amount (target position / posture information) ξ ′ _d representing a target position / posture of the hand 1 / arm 7 which is an output of a second feedback control system 26 described later. The arithmetic unit 252 obtains the target angular velocity value of the joint servo system by multiplying the feedback control coefficient λ and the pseudo inverse matrix of the Jacobian matrix (in the figure, it is represented by adding “.” On the θ of θref). The difference between this angular velocity target and the actual joint angular velocity of the robot 100 (in the figure, a mark is added to θ) is input to the motor driver 4 which is a servo system, and the movement of each motor 14 is controlled. To do.

On the other hand, in the second feedback control system 26, the first motion estimation unit 261 obtains the current position / posture information ξ _c based on the position / posture information ξ _craw that is the recognition result of the image recognition unit 20. On the other hand, from the set target position / posture information ξ _draw , the second motion estimation unit 262 obtains the current target position / posture information ξ _d . By subtracting the difference Δξ _d between ξ _c and the actual position / posture information ξ, which is the calculation result of the forward kinematics calculation unit 251 described above, from the obtained target position / posture information ξ _d , the first feedback control system 25 is obtained. determine the target position and orientation information ξ _'d to output.

Here, the sampling time (period) T _s1 of the first feedback control system 25 is the sampling time (period) T _{s2 of} the second feedback control system 26 (the sampling time of the second feedback control system 26 here is The sampling time of the camera eye 3 and the image recognition unit 20 is different from the output time interval of the motion estimation units 261 and 262.) The imaging device used as the camera eye 3 usually acquires images at a video frame rate (about 33 ms). On the other hand, each element of the first feedback control system 25 can be controlled at a speed several to several tens of times higher than that. The output time interval of each motion estimation unit 261, 262 matches the sampling time of the first feedback control system 25.

  The feedback amount of the first feedback control system 25 constituting the inner feedback loop is calculated from the structure of the hand 1 and the arm 7, the joint portion, that is, the parameters and joint angles of each link.

  On the other hand, in the second feedback control system 26 constituting the external feedback loop, the motion of the target object, the hand 1 and the arm 7 is estimated in order to compensate for the delay in the acquisition timing of the camera eye 3 and to give an appropriate command. A mechanism is required. The first and second motion estimation units 261 and 262 in the second feedback control system 26 are mechanisms for that purpose.

Here, a case where linear interpolation is performed will be described as an example. FIG. 4 is a diagram for explaining motion estimation, where s (k) is the detection amount by the visual sensor at time t _k , and s (k−1) is the detection amount by the visual sensor at time t _k−1. . T ₂ is an integer multiple of T ₁ , and T ₂ = t _k −t _k−1 = t _{k + 1} −t _k . In this case, any, (expressed with the sign ^ on s (i).) Motion estimation value between t _k time and t _{k + 1} time is expressed by the following equation.

  With this interpolation formula, a motion estimation value can be output in synchronization with the sampling time in the first feedback control system, and a delay in the acquisition timing of the camera eye 3 can be compensated. Here, linear interpolation is used for motion estimation, but interpolation may be performed using a quadratic equation or other polynomials.

In the second feedback control system 26, the actual position calculated by the forward kinematics of ξ _C compensated for the delay of the acquisition timing from the position and orientation of the hand recognized by the camera eye 3-image recognition unit 20 A difference Δξ _d with respect to the posture information ξ is used as a feedback control amount. As a result, the deviation between the position and orientation of the hand 1 and arm 7 calculated by forward kinematics caused by errors in the joints of the robot, errors in the sensor parameters, the state of the joints, and the like, and the actual position and orientation are eliminated. Can be removed.

  Further, since the work object and the hand 1 can be simultaneously photographed by the same camera eye 3 and can be recognized by the image recognition unit 20, the identification error of the parameters of the camera eye 3 and the image recognition unit 20 and the variation of the parameters A recognition error of the relative position and orientation between the work target and the hand 1 can be avoided, and control with high accuracy is possible.

In addition, when there is an obstacle between the camera eye 3 and the object or the hand 1 or when there is no obstacle, the image recognition unit 20 fails to recognize the object or the hand 1 or the like. When the position / posture information ξ _craw is not obtained from 20 (when it is not normal, it includes a case where it is determined), the feedback amount Δξ _d is set to 0. As a result, the outer feedback loop does not function and only the inner feedback loop is executed. Therefore, malfunctions of the hand 1 and the arm 7 due to the abnormality of the position / posture information ξ _craw can be suppressed.

  Next, the process of the control method in the motion control apparatus according to the present invention will be described with reference to the flowcharts of FIGS. FIG. 5 is a flowchart of the gripping process when gripping a stationary object, and FIG. 6 is a flowchart of the gripping process when gripping a dynamic object.

  First, the gripping process of the stationary object will be described with reference to the flowchart of FIG. First, the image recognition unit 20 recognizes the position and orientation 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 determines the position / orientation of the arm 7 and the hand 1 for gripping the object (step S2).

  After setting, the hand 1 and the arm 7 are controlled using the above-described control system (visual servo system) (step S3), the arrival at the target position is determined (step S4), and if not reached, By returning to step S3, the drive control of the hand 1 and the arm 7 is continued until it reaches. If it is determined that the target position has been reached, the process proceeds to step S5, the target object is gripped, and the process ends. If necessary, a predetermined operation, for example, an operation of moving the object from one place to another may be performed while holding the object.

  Next, gripping processing when gripping a dynamic object will be described with reference to FIG. The dynamic object here includes not only the case where the object itself is moving, but also the case where the main body side of the robot 100 is moving, and the robot main body and the object move relatively. It is a concept that points to the case.

  First, the image recognition unit 20 recognizes the position and orientation of the target object by performing image processing on the image acquired by the camera eye 3 (step S11). Next, the position / orientation calculation unit 21 determines the position / orientation of the arm 7 and the hand 1 for gripping the object (step S12). After the setting, the hand 1 and the arm 7 are controlled using the control system (visual servo system) described above (step S13), and the arrival at the target position is determined (step S14).

  The basic processing so far is the same as the processing shown in FIG. 5, but in the case of an object that is stationary relative to the robot, the position and orientation of the object do not change during control. On the other hand, in the case of an object moving relative to the robot, the position and orientation of the object fluctuate from moment to moment even during control. Therefore, when it is determined that the target position has not been reached, the process returns to step S11 to execute again from recognition of the position and orientation of the object, and the drive control of the hand 1 and arm 7 is continued. If it is determined that the target position has been reached, the process proceeds to step S15, the target object is gripped, and the process ends. If necessary, a predetermined operation, for example, an operation of moving the object from one place to another may be performed while holding the object.

  The inventors conducted a simulation for confirming the effect of improving the followability of the hand to the target object by the control method of the present invention, and the results will be described below. Here, using the robot 100 shown in FIG. 7, a feature point for facilitating recognition of the position and orientation of the hand 1 in the camera eye 3-image recognition unit 20 on the back of the right hand 1 </ b> R that holds the object 200. Assume that A to C are arranged. Hereinafter, an orthogonal coordinate system is used with respect to the body 8 of the robot 100, where the front-rear direction is the X-axis, the left-right direction is the Y-axis, the vertical direction is the Z-axis, and the center of gravity of the robot 100 is the origin.

In the control system, the sampling time T1 of the _first feedback control system 25 constituting the inner feedback loop is ₁ ms, and the sampling time (video acquisition frame rate) T2 of the _second feedback control system 26 constituting the outer feedback loop. Is 40 ms.

  First, a case where a gripping of a static object is simulated will be described. The feature points A to C are (338.8, −306.5, 451.6) mm, (303.4, −306.5, 416.3) mm, and (268.1, −306.5, 451), respectively. .6) mm from the initial position (376.6, -622.5, 1106.0) mm, (398.2, -647.5, 1068.5) mm, (367.0, -625.8, Considering the case of moving the arm to a target position of 1036.0) mm, the trajectory of the arm is determined by the control method of the present invention (with visual feedback control including motion estimation) and the conventional control method not including visual feedback control. Compared. Here, it is assumed that the link length of the arm 7 includes an error of 0.01 m.

  FIG. 8 shows the simulation result. Here, only the locus change of the feature point A is shown. In the case of conventional control that does not include visual feedback control, movement ends at a position that is shifted by several millimeters in the X, Y, and Z directions from the target position, and gripping fails when the gripping target is small. there is a possibility. On the other hand, according to the present invention, the position and orientation error of the robot hand caused by the parameter error of the arm 7 can be corrected, and the arm 7 and the hand 1 can follow the target object with high accuracy. It could be confirmed.

  Next, a case where a dynamic object gripping simulation is simulated will be described. The initial positions of the feature points A to C are (338.8, −306.5, 451.6) mm and (303.4, −306.5, 416. 3) mm, and (268.1, -306.5, 451.6) mm. The initial target positions of the feature points are (429.1, −311.5, 764.7) mm, (429.1, −311.5, 714.7) mm, (379.1, −311.5, The target position after 10 seconds is (376.2, −622.5, 1106.0) mm, (398.2, −647.5, 1068.5) mm, (367). 0.0, −625.8, 1036.0) mm, and the target position moves at a constant speed. The trajectory of this target value is shown in FIG. Here, the trajectory of the arm was compared between the control method of the present invention and visual feedback control without motion estimation.

  FIG. 10 shows the simulation result. Here, only the locus change of the feature point A is shown. When motion estimation is not performed, a trajectory deviated by several millimeters in the X, Y, and Z directions from the target position is drawn, and the movement of the grasped object cannot be sufficiently followed. This is due to a delay in visual feedback control. In contrast, according to the present invention, it was confirmed that the delay in visual feedback control can be corrected by motion estimation, and the arm 7 and the hand 1 can follow the target object with high accuracy.

FIG. 11 shows the trajectory of the feature point A when the object is circularly moving in the YZ plane. The center coordinates (y _a , z _a ) = (− 311.5, 764.7) mm of the target locus of the circular motion of the point A, the radius of the circular motion was 100 mm, and the motion cycle was 5 seconds. The initial position of the feature point A was (338.8, −306.5, 451.6) mm.

  When motion estimation is not performed, followability is poor due to delay in visual feedback control. On the other hand, according to the present invention, since this delay is guaranteed by motion estimation, the tracking accuracy is improved. Thereby, it was confirmed that it was suitable about the grip control with respect to a dynamic object.

  In the embodiment described above, the composite visual feedback control system using the visual sensor (camera eye) has been described. However, instead of the visual sensor, an object or control target is contactlessly used by using an ultrasonic sensor or a radar. A sensor (external sensor) that determines the position of the moving part (movable part) of the robot may be used. Further, instead of the encoder potentiometer, various position sensors and sensors (inner world sensors) for acquiring the positional relationship of each drive mechanism from the control state of the drive mechanism may be combined.

  In the above description, the operation control of the robot hand / arm has been described as an example, but the present invention can also be applied to the control of the neck, fingers, torso, feet, etc., which are other robot drive parts. Further, the present invention can be similarly applied to various drive parts of a robot that is not humanoid.

It is a schematic block diagram of the robot which has the motion control apparatus of the robot which concerns on this invention. It is a block block diagram of the control apparatus (movement control apparatus of the robot which concerns on this invention) of FIG. It is a block diagram which shows the control system in the control apparatus of FIG. It is a figure explaining the motion estimation in the control system of FIG. FIG. 4 is a flowchart of gripping processing when a stationary object is gripped by the control system of FIG. FIG. 4 is a flowchart of gripping processing when a dynamic object is gripped by the control system of FIG. 3. It is a figure which shows the structure of the robot made into the object of simulation. It is a figure which shows the simulation result of a static object grip. It is a figure which shows the locus | trajectory of the target value in the simulation of dynamic object holding | grip. It is a figure which shows the simulation result of a dynamic object grip. It is a figure which shows the simulation result of a dynamic object grip.

Explanation of symbols

  DESCRIPTION OF SYMBOLS 1 ... Robot hand, 3 ... Camera eye, 4 ... Motor driver, 7 ... Arm, 8 ... Body, 14 ... Motor, 16 ... Encoder potentiometer, 20 ... Image recognition part, 21 ... Position and orientation calculation part, 22 ... Robot Control unit, 25 ... first feedback control system, 26 ... second feedback control system, 100 ... robot, 251 ... forward kinematics calculation unit, 252 ... calculation unit, 261 ... first motion estimation unit, 262 ... first 2 motion estimation units.

Claims (4)

A control device for controlling the movement of a robot having a part that can be mechanically moved by a drive mechanism,
An external sensor for detecting a position of the movable part and a target position of the movable part in a non-contact manner;
An internal sensor that acquires the positional relationship between the movable parts according to the driving of the driving mechanism;
Movement control means for controlling movement of the movable part toward the target position;
A deviation between the position of the movable part acquired using the external sensor and the position of the movable part acquired using the internal sensor is obtained, and the target position of the movable part used for control in the movement control means is determined. Position correcting means for correcting;
A robot motion control device comprising:   When the external sensor has a longer sampling time than the internal sensor, the positional information is acquired by the internal sensor based on the time change of the position of the movable part acquired using the external sensor. 2. The robot motion control apparatus according to claim 1, further comprising position estimation means for estimating position information of a movable part corresponding to a time point and outputting the position information to the position correction means.   The apparatus further comprises a prohibiting unit that prohibits correction by the position correcting unit when it is determined whether or not the acquisition of the position information by the external sensor is successful and it is determined that the acquisition of the positional information is not successful. The robot motion control apparatus according to 1 or 2.   The robot motion control method according to claim 2, wherein a sampling period for acquiring position information of the external sensor is an integral multiple of a position information acquisition sampling period of the internal sensor.

Images