JP5758777B2 - robot - Google Patents

Description

  The present invention relates to a robot that moves by a moving body such as a carriage.

  As a robot that moves by such a moving body, Patent Document 1 describes an industrial robot in which an arm, a vertical drive unit, and a hand unit are provided via an index unit on an XY stage as a moving body. In this industrial robot, by providing each unit on the XY stage, position control is performed by the same simple method as the Cartesian coordinate robot, while ensuring a wide operating range that cannot be obtained with the Cartesian coordinate robot. Yes.

  Patent Document 2 describes a mobile robot that includes a moving mechanism and performs various inspections and operations by remote control. This mobile robot controls an auxiliary device provided in the middle of the movement path for assisting a movement operation, an inspection operation, etc., and changes its direction, moves to the upper floor, and the like. As a result, the function of the mobile robot itself is reduced and the size is reduced.

Japanese Patent Laid-Open No. 60-80576 JP-A-61-139806

  However, according to the industrial robot of Patent Document 1, since each unit is provided on the XY stage, the range in which the industrial robot can move is limited to the movable range of the XY stage.

  On the other hand, according to the mobile robot of Patent Document 2, since the mobile robot is provided with a moving mechanism having wheels, the mobile robot can move over a wide range by the mobile mechanism. However, since the arm-like mechanism composed of the arm and the joint mechanism is fixed as it is on the moving mechanism, even if the mobile robot moves to the vicinity of the operation target by the moving mechanism, the length of the arm-like mechanism is slightly Since there are not enough, the arm-shaped mechanism may not reach the operation target, and the operation target may not be operated.

  In addition, when an end effector at the tip of the arm-shaped mechanism or an operation target that requires fine and accurate operation is employed, the positioning and operation of the end effector with respect to the operation target can be performed only by the operation of the arm-shaped mechanism. It can be difficult.

  An object of the present invention is to provide a robot having a wide application range in view of the problems of the prior art.

A robot according to the present invention includes a robot arm having a joint mechanism and a link member connected via the joint mechanism, and is a robot that moves by a moving body. A displacement mechanism for displacing a stage to which ends are connected; a force detector for detecting a force acting between the stage and a base end of the robot arm; and the robot arm by controlling the displacement mechanism Control means for controlling the center of gravity position of the robot arm, and the control means is configured to control the displacement mechanism based on the input target motion trajectory of the robot arm and the target center of gravity position trajectory of the robot arm and the stage. A target stage position trajectory is determined, and an estimated center of gravity position trajectory of the robot arm is generated based on a detection output trajectory of the force detector. A stage position compensation trajectory obtained by multiplying the difference between the position trajectory and the estimated center of gravity position trajectory by a proportional gain coefficient, a differential gain coefficient or the sum thereof is added to the target stage position trajectory to generate a corrected target stage position trajectory. According to this, the stage is driven .

  According to the present invention, even when the robot arm is only driven and moved by the moving body, the end effector at the tip of the robot arm does not reach the operation target, and the operation target cannot be operated. In some cases, the end effector can be brought close to a position where the operation target can be operated. Therefore, the access range by the robot can be expanded.

  In addition, the end effector can be positioned and operated with respect to the operation target with high accuracy by using the end effector while simultaneously driving the robot arm by the joint mechanism and the displacement of the robot arm by the displacement mechanism. . Thereby, it is possible to operate the end effector and the operation target that require high positioning accuracy and operation accuracy without any trouble. Therefore, according to the present invention, a robot with a wide application range can be provided.

In the present invention, robot includes a force detector for detecting a force acting between the displacement of mechanism and the base end portion of the robot arm, the displacement of mechanism based on a detection result of the force detector as described above to controlling Therefore that have a control means for controlling the position of the center of gravity of the robot arm.

  According to this, by controlling the position of the center of gravity of the robot arm appropriately by the control means, it is possible to ensure the stability of the robot and to prevent the vehicle from falling over while traveling on rough terrain by the moving body or operating the operation target by the end effector. be able to.

  In the present invention, the control means controls the position of the center of gravity of the robot arm so as to be located at a predetermined stable position by controlling the displacement mechanism. When the center of gravity position cannot be controlled so as to be positioned at the stable position, further controlling the joint mechanism on the most proximal side of the robot arm, or by controlling both the joint mechanism and the moving body, The center of gravity of the robot may be controlled so as to be positioned at the stable position.

  According to this, even when the center of gravity position of the robot arm cannot be positioned at the stable position by controlling only the displacement mechanism, the center of gravity position of the robot arm is further controlled by additionally controlling the joint mechanism and the moving body. It can be located in a stable position. Therefore, it is possible to more reliably realize the stability of the robot and the prevention of falling during the movement by the moving body and the operation of the operation target.

It is explanatory drawing which shows the structure of the robot which concerns on one Embodiment of this invention. It is a block diagram which shows the structure of the control apparatus in the robot of FIG. It is a control block diagram showing control by the control apparatus of FIG.

  Hereinafter, embodiments of the present invention will be described with reference to the drawings. As shown in FIG. 1, the robot 1 includes a robot arm 2 as a manipulator. The robot arm 2 includes a plurality of joint mechanisms and a plurality of link members connected via the plurality of joint mechanisms.

The plurality of joint mechanisms include a first joint mechanism J ₁ disposed on the distal end side of the robot arm 2, a second joint mechanism J ₂ disposed on the proximal end side of the robot arm 2, and a first joint mechanism. Two intermediate joint mechanisms I ₁ and I ₂ arranged between J ₁ and the second joint mechanism J ₂ are included. The intermediate joint mechanism may be omitted. The number of intermediate joint mechanisms can be arbitrarily changed.

The rotational degree of rotation of the first joint mechanism J ₁ is 3 (roll, pitch and yaw), and the rotational degree of freedom of the second joint mechanism J ₂ is 2 (pitch and yaw). The rotational freedom degree of the first intermediate joint mechanism I ₁ is 1 (roll), and the rotational freedom degree of the second intermediate joint mechanism I ₂ is 2 (pitch and yaw). The robot arm 2 includes joint angle sensors S _i (i = 1, 2,...) Such as a rotary encoder that outputs a signal corresponding to a rotation angle (joint angle) within each rotation degree of freedom of each joint mechanism. (See FIG. 2). Note that the degree of freedom of rotation of each joint mechanism may be arbitrarily changed.

The tip of the robot arm 2 is connected to the jig X ₁ through the first joint mechanism J ₁ and the elastic element X ₀ in order. The jig X ₁ has an appropriate structure for engaging with a handle of the valve, for example, for performing a task of opening and closing the valve. At least one of the elastic element X ₀ and the jig X ₁ may be a component of the robot arm 2.

The base end portion of the robot arm 2 is connected to the XY stage X ₂ via the second joint mechanism J ₂ . XY stage X ₂ supports the robot arm 2 on carriage X _3, parallel to carriage X ₃ with respect to a predetermined reference plane based, it moves the robot arm 2. The cart X ₃ is a movable body that can be remotely controlled.

The robot arm 2 includes a first six-axis force sensor F ₁ and a first gyro sensor G ₁ arranged at the distal end thereof. The first 6-axis force sensor F ₁ outputs a signal corresponding to the relative force in the three axes (roll axis, pitch axis and yaw axis) between the robot arm 2 and the jig X ₁ and the moment about the three axes. Configured to output. The first gyro sensor G ₁ is configured to output a signal corresponding to the posture of the jig X ₁ in the world coordinate system (reference coordinate system) (such as an inclination angle with respect to the horizontal direction).

The robot arm 2 includes a second six-axis force sensor F ₂ and a second gyro sensor G ₂ arranged at the base end thereof. The second six-axis force sensor F ₂ is a signal corresponding to the relative force in the three axes (roll axis, pitch axis and yaw axis) between the robot arm 2 and the XY stage X ₂ and the moment about the three axes. Is configured to output. The second gyro sensor G ₂ is configured to output a signal corresponding to the attitude of the XY stage X ₂ in the world coordinate system (reference coordinate system).

An imaging device is attached to at least one of the robot arm 2, the jig X _1, and the cart X ₃ , and an image captured by the imaging device is displayed on an image device installed at a location away from the robot arm 2. The The operator can remotely control the operations of the XY stage X ₂ , the cart X ₃ , the robot arm 2, the jig X _1, etc. by operating the remote control device while viewing this image.

At that time, even when the carriage X ₃ is brought close to a position where it can be operated by the jig X ₁ and the robot arm 2 is extended, the jig X ₁ does not reach the operation target and cannot be accessed by the jig X _1. In this case, the jig X ₁ can be brought closer to the operation target by moving the XY stage X ₂ in the direction of the operation target. Thus, it is possible to operate according to the jig X ₁ to expand the possible range.

In addition, high operation accuracy may be required when operating the operation target with the jig X ₁ . In this case, the jig X ₁ cannot be accurately positioned with respect to the operation target only by driving the robot arm ₂ by the joint mechanisms J ₁ , J ₂ , I ₁ , and I ₂ , or linked to the operation of the jig X _1. As a result, the robot arm 2 cannot be driven accurately, which may hinder the operation of the operation target.

In such a case, the operation with the jig X _{1 is performed} while using both the driving of the robot arm 2 by the joint mechanisms J ₁ , J ₂ , I ₁ and I ₂ and the movement of the robot arm 2 by the XY stage X _2. By performing, the positioning of the jig X ₁ with respect to the operation target and the operation with the jig X ₁ can be performed with high accuracy. This is because the robot arm 2 and the XY stage X ₂ differ in highly accurate driving direction, operation content, and the like. Thereby, it is possible to perform processing without any trouble even on an operation target for which high operation accuracy is required.

  The robot arm 2 includes a control device 3 shown in FIG. The control device 3 is configured by a programmable computer. The target motion trajectory of the robot arm 2 is input to the control device 3 from the remote control device. The “trajectory” of a variable means a time-series variable value representing a time change mode of the variable.

In addition to the first six-axis force sensor F ₁ , the second six-axis force sensor F ₂ , the first gyro sensor G _1, and the second gyro sensor G ₂ , the control device 3 includes each joint mechanism. A signal output from the joint angle sensor S _{i of} J ₁ , J ₂ , I ₁ , I ₂ is input.

Based on the input signal, the control device 3 generates operation control command signals for the joint mechanisms J ₁ , J ₂ , I ₁ , I ₂ , the XY stage X ₂ , and the carriage X ₃ of the robot arm 2. A control process of outputting the signal to the actuator A _i to be operated is configured to be executed.

  Here, the control device 3 is configured to execute the control process. The CPU (central processing unit) constituting the control device 3 reads necessary software and data from a memory (storage device), and the software It is programmed that the said arithmetic processing is performed according to.

Further, based on the detection result of the six-axis force sensor F ₂ or the like, the control device 3 detects the XY stage X so that the deviation is compensated when the position of the center of gravity of the robot arm 2 is deviated from a predetermined stable position. Control ₂ At this time, when only the compensation control for the XY stage X ₂ is not sufficient, the compensation control is additionally performed for the second joint mechanism J ₂ and the carriage X ₃ .

As shown in FIG. 3, the control device 3 generates an estimated center-of-gravity position trajectory gc_act of the robot arm 2 as a configuration for performing compensation control of the XY stage X ₂ , the second joint mechanism J ₂ , and the carriage X _3. The estimated center-of-gravity position trajectory generating element 31, compensation control defining elements 32 and 33 for defining the case where compensation control is performed for each of the second joint mechanism J ₂ and the cart X _3, and the compensation amount according to the position from the compensation control defining element 32 It comprises a track and angle conversion element 34 for converting the angular compensation track θ_comp by rotation angle of the second joint mechanism J ₂ (yaw axis and the angle around the pitch axis).

  The compensation control defining element 32 includes a limiter 321 and a first-order lag element 322 connected to the output side of the limiter 321. The compensation control defining element 33 includes a limiter 331 and a first-order lag element 332 connected to the output side of the limiter 331.

In this configuration, the control device 3 uses the target motion trajectory of the robot arm 2 input to the control device 3 when controlling the XY stage X ₂ , the second joint mechanism J ₂ , and the cart X _3. Target center of gravity position trajectory gc_cmd which is the trajectory of the target center of gravity position, target stage position trajectory sp_cmd which is the trajectory of the target position of the XY stage X ₂ , and target angle trajectory which is the trajectory of the target rotation angle of the second joint mechanism J ₂ θ_cmd and a cart target position trajectory cp_cmd which is a trajectory of the target position of the cart X ₃ are determined.

Target barycentric position trajectory gc_cmd is represented as a track position on a predetermined reference plane relative to the carriage X _3, is set as the trajectory of the center of gravity of the robot arm 2, such as the posture of the robot 1 is most stable. Reference plane of the carriage X ₃ is a plane as a horizontal when the carriage X ₃ is positioned on a horizontal floor. Along this plane, XY stage X ₂ is moved. Therefore, the target stage position trajectory sp_cmd is represented as a position along the reference plane of the carriage X _3. The target angle trajectory θ_cmd is expressed as an angle around the pitch and yaw axis of the second joint mechanism J ₂ with reference to the cart X ₃ or the XY stage X ₂ .

The estimated center-of-gravity position trajectory generation element 31, generates an estimated center-of-gravity position trajectory gc_act of the robot arm 2 on the basis of the second six-axis force sensor F ₂ detection output trajectory F_act. At this time, for example, when four 6-axis force sensors that support the XY stage X ₂ at four points are employed as the second 6-axis force sensor F ₂ , the Z-axis detected by each 6-axis force sensor. it is possible to calculate the center of gravity of the robot arm 2 based on the force in the direction (the direction perpendicular to the moving surface of the XY stage X _2).

The centroid position is calculated as a position on the reference plane of the carriage X ₃ above. In the calculation, the inclination of the XY stage X ₂ (cart X ₃ ) obtained based on the second gyro sensor G ₂ is taken into consideration.

Based on the deviation between the target center-of-gravity position trajectory gc_cmd and the estimated center-of-gravity position trajectory gc_act, a stage position compensation trajectory sp_comp is generated according to the relational expression (1). Stage position compensation trajectory sp_comp is the deviation from a stable position of the center of gravity of the robot arm 2, a trajectory for compensating the control of the XY stage X _2. “Kp” is a proportional gain coefficient, and “Kd” is a differential gain coefficient. One of Kp and Kd may be 0.

  sp_comp = (Kp + Kds) (gc_cmd-gc_act) .. (1).

  The target stage position trajectory sp_cmd is corrected according to the relational expression (2) based on the stage position compensation trajectory sp_comp, thereby generating a corrected target stage position trajectory sp_cmd_mdfd.

  sp_cmd_mdfd = sp_cmd + sp_comp .. (2).

When the XY stage X ₂ is driven according to the corrected target stage position trajectory sp_cmd_mdfd, the center of gravity position of the robot arm 2 deviates from the target center of gravity position trajectory gc_cmd which is a stable position on the carriage X _3. deviation amount only, XY stage X ₂ is, the deviation direction is moved in the opposite direction. Thereby, the position of the center of gravity of the robot arm 2 is controlled so as to be always located at a stable position on the carriage X ₃ , so that the posture of the robot 1 is stabilized.

Only control of the XY stage X _2, when the deviation from the stable position of the center of gravity of the robot arm 2 can not be compensated, further the angle and yaw axis of the angle of the second pitch axis of the joint mechanism J _2, The position of the carriage X ₃ is controlled.

  That is, when the deviation between the target center-of-gravity position trajectory gc_cmd and the estimated center-of-gravity position trajectory gc_act exceeds the upper limit value Lmax of the limiter 321 or falls below the lower limit value Lmin, the first order lags according to the relational expression (3a) or (3b), respectively. Through element 322, a joint mechanism compensation trajectory j_comp is generated. When the deviation is not more than the upper limit value Lmax and not less than the lower limit value Lmin, the value of the joint mechanism compensation trajectory j_comp is 0 (zero).

  j_comp = (gc_cmd-gc_act-Lmax) K1 / (T1s + 1) .. (3a).

  j_comp = (gc_cmd-gc_act-Lmin) K1 / (T1s + 1) .. (3b).

The joint mechanism compensation trajectory j_comp compensates the angle of the second joint mechanism J ₂ so that the position of the center of gravity of the robot arm 2 moves in the opposite direction to the movement corresponding to the movement of the carriage X ₃ described later. Orbit for. T1 is a time constant and K1 is a gain constant.

Joint mechanism compensating track j_comp passes through an angle conversion element 34 is converted into angular compensation track θ_comp a trajectory compensation amount, expressed in angle and the angle of yaw axis of the second pitch axis of the joint mechanism J _2. This conversion is performed by ordinary geometric calculation. Based on the angle compensation trajectory θ_comp, the target angle trajectory θ_cmd is corrected according to the relational expression (4), and a corrected target angle trajectory θ_cmd_mdfd is generated.

  θ_cmd_mdfd = θ_cmd + θ_comp .. (4).

  On the other hand, when the deviation between the target center-of-gravity position trajectory gc_cmd and the estimated center-of-gravity position trajectory gc_act exceeds the upper limit value Lmax of the limiter 331 or falls below the lower limit value Lmin, the first-order lags according to the relational expression (5a) or (5b), respectively. Through element 332, a carriage position compensation trajectory cp_comp is generated. When the deviation is not more than the upper limit value Lmax and not less than the lower limit value Lmin, the value of the carriage position compensation trajectory cp_comp is 0 (zero).

  cp_comp = (gc_cmd-gc_act-Lmax) K2 / (T2s + 1) .. (5a).

  cp_comp = (gc_cmd-gc_act-Lmin) K2 / (T2s + 1) .. (5b).

The cart position compensation trajectory cp_comp is a trajectory for compensating the deviation of the center of gravity position of the robot arm 2 from the stable position in cooperation with the angle compensation of the second joint mechanism J ₂ described above under the control of the cart X _3. is there. T2 is a time constant, and K2 is a gain constant. The magnitude relationship between the time constant T1 and the time constant T2 in the relational expressions (3a) and (3b) described above may be any of T1 = T2, T1> T2, or T1 <T2.

  The cart target position trajectory cp_cmd is corrected according to the relational expression (6) based on the cart position compensation trajectory cp_comp to generate a corrected target cart position trajectory cp_cmd_mdfd.

  cp_cmd_mdfd = cp_cmd-cp_comp .. (6).

Then, the actuator of the carriage X ₃ is driven by the corrected target carriage position trajectory cp_cmd_mdfd, and the actuator of the second joint mechanism J ₂ is driven by the corrected target angular path θ_cmd_mdfd.

Thus, the carriage X ₃ moves in the direction of the deviation by an amount corresponding to the deviation amount from the stable position of the center of gravity position of the robot arm 2 (target gravity center position gc_cmd) (the carriage X ₃ is moving). In some cases, compensation control is performed so that the amount of movement in the direction of the deviation increases. In addition, the second joint mechanism J ₂ is controlled to have an angle around the pitch axis and an angle around the yaw axis so that the position of the center of gravity of the robot arm 2 moves in the direction opposite to the direction of the shift. Thereby, the shift | offset | difference from the stable position of the gravity center position of the robot arm 2 is compensated more reliably.

In addition to the control of the center of gravity, the control device 3 includes six-axis force sensors F ₁ and F ₂ , gyro sensors G ₁ and G ₂ , and joints of the joint mechanisms J ₁ , J ₂ , I ₁ , and I ₂ . By controlling the actuators A _i of the joint mechanisms J ₁ , J ₂ , I ₁ , and I ₂ based on the output from the angle sensor S _i , deflection compensation and compliance compensation can be performed.

As described above, according to the present embodiment, since the XY stage X ₂ is provided between the carriage X ₃ and the robot arm 2, the range in which the jig X ₁ can be operated can be expanded.

In addition, the jig X ₁ can be positioned and operated with high accuracy by performing the operation with the jig X ₁ while using both the driving of the robot arm 2 and the movement of the robot arm 2 with the XY stage X _2. Can do. Therefore, it is possible to perform processing without any trouble even on an operation target that requires high operation accuracy with the jig X ₁ .

Further, when the center of gravity of the robot arm 2 deviates from a predetermined stable position (target center of gravity position gc_cmd) on the carriage X ₃ , the XY stage X ₂ is moved in the direction opposite to the direction of deviation by the amount of the deviation. Therefore, it is possible to control the center of gravity of the robot arm 2 so that it is always located at the stable position.

Thereby, when the robot 1 is traveling by the carriage X ₃ or when the robot 1 is operating the operation target, the stability of the robot 1 can be ensured and the robot 1 can be prevented from falling. .

Moreover, the only movement of the XY stage X _2, if the deviation from the stable position of the center of gravity of the robot arm 2 persists, further, as the center of gravity of the robot arm 2 is positioned in the stable position, the second joint Since the rotation angle of the mechanism J ₂ and the position of the carriage X ₃ are controlled, the position of the center of gravity of the robot arm 2 can be surely positioned at the stable position.

In addition, this invention is not limited to the above-mentioned embodiment. For example, in the above description, the carriage X ₃ is used as a moving body for the robot 1 to move. However, instead of the carriage X ₃ , another moving body such as a traveling means that travels on a rail may be used. Good.

In the above, although using a jig X ₁ as an end effector, alternatively, it may be used those having a passive functions such as measuring instruments.

In the above, although using the XY stage X ₂ as a displacement mechanism, instead of this, it is also possible to use other displacement mechanism such as a linear stage or a two-dimensional linear stage.

In the above description, the second six-axis force sensor F ₂ is used as the force detector, but another force sensor such as a three-component force detector may be used instead.

In the above, if only the control of the XY stage X ₂ can not be controlled so as to position the center of gravity of the robot arm 2 in a stable position, furthermore both the operation of the second joint mechanism J ₂ and carriage X ₃ However, instead of additionally controlling both of them, only the operation of the second joint mechanism J ₂ may be additionally controlled.

1 ... robot, 2 ... robot arm, 3 ... controller, J ₁ ... first joint mechanism, J ₂ ... second joint mechanism, F ₁ ... 6 axis force sensor, F ₂ ... 6-axis force sensor (force detector) , G ₁ ... first gyro sensor, G ₂ ... gyro sensor, X ₀ ... elastic element, X ₁ ... jig, X ₂ ... XY stage (displacement mechanism), X ₃ ... cart (moving body).

Claims (2)

A robot having a robot arm having a joint mechanism and a link member connected via the joint mechanism, and moving by a moving body,
On the movable body, and a displacement mechanism for displacing the stage base end of the robot arm is coupled,
A force detector for detecting a force acting between the stage and the base end of the robot arm;
Control means for controlling the position of the center of gravity of the robot arm by controlling the displacement mechanism;
The control means, when controlling the displacement mechanism,
Based on the input target motion trajectory of the robot arm, a target center-of-gravity position trajectory of the robot arm and a target stage position trajectory of the stage are determined,
Generating an estimated center of gravity position trajectory of the robot arm based on the detection output trajectory of the force detector;
A corrected target stage position trajectory is obtained by adding a stage position compensation trajectory obtained by multiplying the difference between the target gravity center position trajectory and the estimated gravity center position trajectory by a proportional gain coefficient, a differential gain coefficient, or a sum thereof to the target stage position trajectory. Is generated, and the stage is driven in accordance therewith. The control means includes
By controlling the displacement mechanism, the position of the center of gravity of the robot arm is controlled to be positioned at a predetermined stable position,
If it is impossible to control the position of the center of gravity of the robot arm at the stable position only by controlling the displacement mechanism, the joint mechanism on the most proximal side of the robot arm is further controlled, or the joint mechanism and The robot according to claim 1 , wherein the center of gravity of the robot is controlled to be positioned at the stable position by controlling both of the moving bodies.

Images