JP2013220501A - Robot control method and robot control device - Google Patents

Abstract

【Task】
Provided are a robot control method and a robot control apparatus that can easily escape from the interference state manually while maintaining the position and orientation of the tool at the tip of the manipulator using the redundancy of the seven-axis manipulator.
[Solution]
With the current position and orientation of the hand of the manipulator 10 held, a rotation angle Φ for rotating the fourth joint 24 of the manipulator 10 around the central axis O connecting the second joint 22 and the sixth joint 26 of the manipulator 10 is set. The joint control axis can be changed manually, the integral gain of the speed control loop is set to 0, and the gain of the position control loop with respect to the joint axis is increased as the joint amount increases with the rotation of the fourth joint 24 around the central axis O. Set a small value to control the servo motor of the joint axis.
[Selection] Figure 3

Description

  The present invention relates to a robot control method and a robot control apparatus.

Conventionally, Patent Literature 1, Patent Literature 2, Patent Literature 3, and Patent Literature 5 propose a robot control device that controls a robot having redundant degrees of freedom to avoid an obstacle.
Further, Patent Document 4 discloses a technique for stopping a robot so as to avoid damage to a robot movable portion when a tool at the tip of a manipulator collides with an obstacle.

These techniques are techniques that are useful for avoiding collisions or preventing damage to tools, workpieces, and the like when a collision occurs.
Further, in Patent Document 6, the position control gain, the proportional gain of the speed control loop, and the output of the integrator are lowered to preset values, the servo motor is driven, and the driven body driven by the servo motor A servo control method has been proposed that can be moved manually.

JP-A-5-228864 JP-A-5-285863 JP-A-9-314487 JP 2002-283276 A JP-A-2005-14108 JP-A-6-332538

  Patent Documents 1 to 5 described above are techniques for avoiding a collision between a manipulator and an obstacle in advance or preventing damage to a tool or the like at the time of the collision. It has not been proposed how to return the manipulator from the sandwiched state. In particular, there is no disclosure on how to deal with a case where an object is sandwiched between a manipulator link and a fixed object.

  In Patent Document 6, the redundancy in the 7-axis robot is not used, and the manipulator cannot be manually moved in a state where the hand position / posture of the manipulator is constrained.

  An object of the present invention is to provide a robot control method and a robot control device that can easily escape from an interference state manually while maintaining the position and orientation of the tool at the tip of the manipulator using the redundancy of the seven-axis manipulator. There is to do.

  In order to solve the above problems, the invention of claim 1 is a control system including a position control loop and a speed control loop. Each joint axis of a seven-axis manipulator having a first joint to a seventh joint in order from the fixed side. In the robot control method for controlling the rotary actuator of the above-mentioned, around the axis connecting the second joint and the sixth joint of the manipulator (hereinafter referred to as the central axis) while maintaining the current position and orientation of the hand of the manipulator Thus, the rotation angle at which the fourth joint of the manipulator rotates can be manually changed, the integral gain of the speed control loop is set to 0, and the amount of rotation accompanying the rotation of the fourth joint around the central axis is large. The position of the joint axis is set to a smaller gain in the position control loop, and the rotation system actuator of the joint axis is controlled. It has a robot control method and gist.

  According to a second aspect of the present invention, in a control system including a position control loop and a speed control loop, robot control is performed for controlling a rotation system actuator for each joint axis of a seven-axis manipulator having first to seventh joints in order from the fixed side. In the apparatus, the control system includes the manipulator around an axis (hereinafter referred to as a central axis) connecting the second joint and the sixth joint of the manipulator while maintaining the current position and orientation of the hand of the manipulator. A first setting unit that sets the integral gain of the speed control loop to 0 and a rotation amount associated with the rotation of the fourth joint around the central axis is large so that the rotation angle of the fourth joint can be manually changed A second setting unit that sets a smaller gain of the position control loop for the joint axis, and a gain of the position control loop set by the second setting unit. Performs location control, are summarized as robot control apparatus characterized in that it comprises a control unit for controlling the rotation system actuators of the said articulation axis to perform proportional-plus-integral control with integral gain set by the first setting unit.

  A third aspect of the present invention includes the mode switching operation unit of the normal state mode and the emergency state mode according to the first aspect. In the normal state mode, the control unit is positioned at a preset gain of the position control loop. In addition to performing control, proportional integral control is performed with a preset integral gain and proportional gain to control the rotation system actuator of the joint axis. In the emergency state mode, the position control loop set by the second setting unit In addition, the position control is performed with the above-mentioned gain, and the proportional-integral control is performed with the preset proportional gain and the integral gain set by the first setting unit to control the rotation system actuator of the joint axis.

  According to the first aspect of the present invention, it is possible to provide a robot control method that can easily escape from the interference state manually while maintaining the position and orientation of the tool at the tip of the manipulator using the redundancy of the seven-axis manipulator. .

  According to the invention of claim 2, a robot control device can be provided that can easily escape from the interference state manually while maintaining the position and orientation of the tool at the tip of the manipulator using the redundancy of the seven-axis manipulator. .

  According to the invention of claim 3, the effect of claim 2 can be easily realized when the mode switching operation unit is switched to the emergency state mode.

The skeleton figure of the manipulator which has the redundancy degree of freedom of one Embodiment. The schematic block diagram of the robot control apparatus of one Embodiment. Explanatory drawing of the attitude | position parameter of one Embodiment. The flowchart of the program of the emergency mode which the robot control apparatus of one Embodiment performs. The block diagram of the position control loop and speed control loop at the time of emergency mode. The block diagram of the position control loop and speed control loop at the time of normal state mode.

A robot control apparatus and a robot control method for controlling a seven-axis manipulator according to an embodiment of the present invention will be described below with reference to FIGS.
First, a manipulator having a redundancy degree of freedom with respect to the work degree of freedom of the present embodiment will be described.

  As shown in FIG. 1, the manipulator 10 is formed by connecting eight links 11 to 18 in series by seven joints 21 to 27. The manipulator 10 which is an articulated robot is a robot having seven degrees of freedom (degrees of freedom n = 7) in which the links 12 to 18 can rotate at the seven joints 21 to 27, and the number of dimensions of the work space ( The number of dimensions m) is 6, which has a redundancy of 1 (= nm).

  One end of the first link 11 is fixed to the floor surface FL, and the other end is connected to one side of the first joint 21. One end of the second link 12 is connected to the other side of the first joint 21, and one side of the second joint 22 is connected to the other end of the second link 12. Similarly, the third link 13, the fourth link 14, the fifth link 15, the sixth link 16, the seventh link 17, and the eighth link 18 are respectively connected to the third joint 23, the fourth joint 24, and the fifth joint 25. The sixth joint 26 and the seventh joint 27 are connected in order.

  The other side of the first joint 21 is rotatable with respect to one side about an axis extending in the vertical direction in FIG. 1 as indicated by an arrow 31, whereby the second link 12 is adjacent to the second link 12. With respect to one link 11, it can turn in the direction of arrow 31 around the rotation axis (J1 axis) of the first joint 21.

  Further, the other side of the second joint 22 is rotatable with respect to one side about an axis (J2 axis) extending in a direction perpendicular to the paper surface in FIG. Accordingly, the third link 13 can rotate in the direction of the arrow 32 around the rotation axis of the second joint 22, that is, in the vertical direction with respect to the adjacent second link 12.

  Hereinafter, the third joint 23, the fourth joint 24, the fifth joint 25, the sixth joint 26, and the seventh joint 27 are also rotatable, and the fourth link 14, the fifth link 15, and the sixth link. 16, the seventh link 17 and the eighth link 18 can also turn in the directions of arrows 33 to 37 around the rotation axes (J3 axis to J7 axis) of the joints 23 to 27, respectively. Note that, throughout the present application, the links 11 to 18 connected through the first joints 21 to 27 are referred to as adjacent links 11 to 18. Further, the J1 axis to the J7 axis correspond to joint axes.

Note that the rotation directions of the J2, J4, and J6 axes coincide with the direction in which the gravitational acceleration acts as shown in FIG.
As shown in FIG. 1, a first servo motor 41 is attached to the first joint 21, and when power is supplied, the second link 12 is connected to the first link 11 via a reduction gear (not shown). And turn.

  Further, a second servo motor 42 is attached to the second joint 22, and when the electric power is supplied, the third link 13 is turned with respect to the second link 12 via a reduction gear (not shown). Similarly, servo motors 43 to 47 are attached to the third joint 23, the fourth joint 24, the fifth joint 25, the sixth joint 26, and the seventh joint 27, respectively, and are supplied with power. Each of the links 14 to 18 is turned through a reduction gear (not shown).

  In addition, although each motor is provided in each joint, in FIG. 1, for convenience of explanation, it is illustrated separately from the joint. In this embodiment, an AC motor, which is a servo motor, is used as the rotary actuator, but the present invention is not limited to this.

  A tool 49 as an end effector is attached to the tip of the eighth link 18. Along with the eighth link 18, the tool 49 can turn in the direction of the arrow 37 as shown in FIG. 1 around the rotation axis (J7 axis) of the seventh joint 27. The tool 49 is, for example, a hand that can grip a work or the like. The type of tool 49 is not limited because it is not related to the present invention.

  As described above, the manipulator 10 rotates the second link 12 to the eighth link 18 by driving the first servo motor 41 to the seventh servo motor 47 to rotate the second link 12 to the eighth link 18. Are accumulated and work on the tool 49 at the tip, so that the position and posture of the tip of the tool 49 can be matched with the target position and posture according to the work content.

Next, with reference to FIG. 2, an electrical configuration of an articulated robot centering on a controller RC as a robot control device for controlling the manipulator 10 will be described.
The controller RC includes a computer 90, PWM generators 51 to 57 electrically connected to the computer 90, and servo amplifiers 61 to 67 electrically connected to the PWM generators 51 to 57. The servo amplifiers 61 to 67 are electrically connected to the first servo motor 41 to the seventh servo motor 47, respectively.

  The computer 90 outputs a control command to the PWM generators 51 to 57, and the PWM generators 51 to 57 output a PWM signal to the servo amplifiers 61 to 67 based on the control command. The servo amplifiers 61 to 67 rotate the links 12 to 18 by operating the servo motors 41 to 47 according to the output.

  The servo motors 41 to 47 incorporate rotary encoders 71 to 77 and are connected to a computer 90 via an interface 80. The rotary encoders 71 to 77 detect the rotation angles of the servo motors 41 to 47, that is, the rotation angles of the links 12 to 18 with respect to the adjacent links 11 to 17 (the joint angles of the joint axes). And the detection signal is transmitted to the controller RC. The rotary encoders 71 to 77 correspond to a rotation angle detector. The rotation angle detector is not limited to a rotary encoder, and may be a resolver or a potentiometer.

  Instead of providing the rotary encoders 71 to 77 with respect to the first servo motor 41 to the seventh servo motor 47, the rotation angles of the links 11 to 18 are connected to the links 11 to 18 or the first joint 21 to the seventh joint 27 ( A sensor capable of directly detecting the joint angle of the joint axis may be attached.

  The computer 90 includes a CPU 91, ROM 92, RAM 93, a nonvolatile storage unit 94 such as a hard disk, an interface 95, and the like, and is electrically connected via a bus 96.

  The storage unit 94 stores various data, work programs for causing the robot to perform various operations, various parameters, and the like. That is, the robot of the present embodiment is a robot that operates in a teaching playback system, and the manipulator 10 operates when the work program is executed. The ROM 92 stores system programs for the entire system. The RAM 93 is a working memory for the CPU 91, and temporarily stores data when various calculations are executed. The CPU 91 corresponds to a first setting unit, a second setting unit, and a control unit.

  An input device 82 is connected to the controller RC via the interface 95. The input device 82 is an operation panel having a monitor screen (not shown) and various input keys. The user can input various data. The input device 82 is provided with a power switch for an articulated robot, and with respect to the computer 90, the final target position and final target posture of the tip of the tool 49 (hereinafter referred to as the hand) at the tip of the manipulator 10; It is possible to input the position and orientation at the interpolation point at the tip of the head and the jog operation for changing the posture of the manipulator 10 using redundancy. Further, the input device 82 includes a mode switching key (not shown), and the mode switching key can be selected between a normal state mode and an emergency state mode. The input device 82 corresponds to a mode switching operation unit.

(Operation of the embodiment)
Next, the operation of the controller RC of the articulated robot according to the present embodiment will be described with reference to FIGS.

(Normal mode)
First, when the normal mode is selected by a mode switching key (not shown) of the input device 82, the CPU 91 of the controller RC interpolates the teaching points (final target position and the tip of the tool 49) written in the work program. The servomotors 41 to 47 of each joint axis of the manipulator 10 are controlled so that the hand is positioned at the position at the point.

  That is, for each step in which the teaching point is described, the CPU 91 sets the teaching point (hand position), hand posture and speed data written in the work program in the storage unit 94 in a predetermined area of the RAM 93 and The servo motors 41 to 47 of each joint axis are controlled by this method. The hand position and hand posture are collectively referred to as the hand position and posture.

  The operation of the control system of the controller RC performed in the normal state mode will be described with reference to the control system block diagram shown in FIG. For convenience of explanation, the reference numerals of the constituent members are the same as those of the constituent members already described. In FIG. 6, the left side from the alternate long and short dash line represents the operation on the controller RC side, and the right side represents the operation on the servo motor side.

  As shown in FIG. 6, in the controller RC, the CPU 91 first obtains each servo motor obtained by performing an inverse conversion calculation based on the target position and target posture of the tool 49 described in the work program stored in the RAM 93. Is the position command θs *.

  A deviation between this position command θs * and the actual position θk of each of the servo motors 41 to 47 obtained by the rotary encoders 71 to 77 is calculated. The rotary encoders 71 to 77 detect the rotation angle (that is, the joint angle) at a detection cycle sufficiently shorter than the control cycle in the work program.

  Then, the position control unit 110 calculates the target speed (speed command ωs *) of the servo motor by multiplying the calculated position deviation by a preset position gain K0 in P control (proportional control). . The position gain K0 is a gain of the position control loop.

Further, the speed control unit 120 uses a predetermined speed that is set in advance by PID control to the speed deviation between the speed command ωs * and the actual speed ω of each servo motor 41 to 47 obtained from the actual position θk. The gain K1 is multiplied to calculate a current command iq * for each of the servo motors 41 to 47, and this current command iq * is output to the current control unit 130. Speed gains K1 herein is obtained by overall proportional gain Kv of the proportional control in the PID control, the differential gain K _D integral gain Ki and differential control of the integral control, the gain is one that is set in advance. In the normal mode, the integral gain Ki is a value different from zero. Here, a speed control loop in which the actual speed ω is fed back is configured.

  The current control unit 130 takes in the actual currents of the servomotors 41 to 47 detected by a current detection circuit (not shown) by A / D conversion, and outputs a control command so that the actual current iq becomes a current command. The PWM generators 51 to 57 shown in FIG. Here, a current control loop in which the actual current iq is fed back is configured.

  Specifically, the PWM signal is generated by multiplying the current deviation between the current command iq * and the actual current iq by a predetermined current gain K2 by PI control. The generated PWM signal is output to each of the servo amplifiers 61 to 67, and the energization current of each of the servo motors 41 to 47 is controlled. In FIG. 6, KA / D described in block P6 representing the procedure for taking in the actual current by A / D conversion represents a conversion constant for converting the actual current into a digital value.

  As a result, a drive voltage is applied to the motor windings of the servo motors 41 to 47 from the servo amplifiers 61 to 67 in accordance with the PWM signal. A voltage obtained by synthesizing the counter electromotive force obtained by multiplying the rotational angular velocity by the counter electromotive force constant Ke is obtained. A current (that is, an actual current) obtained by multiplying the terminal voltage by a coefficient {1 / (Ls + R)} having the motor inductance L and the motor resistance R as parameters flows through each motor winding.

  Further, when a current flows through the motor winding, in each servo motor 41 to 47, a motor torque TM determined by the actual current and the torque constant Kt is generated in the rotor, and a delay due to the inertia J of the motor shaft ( Rotational angular acceleration is generated with 1 / J), and is controlled to a rotational speed obtained by integrating (1 / S) the rotational angular acceleration. A rotational position obtained by integrating (1 / S) the rotational speed is detected by a sensor such as a rotary encoder provided in each servo motor 41 to 47, and the detection signal is fed back into the controller RC. In this way, the controller RC is configured as a servo control device that feedback-controls the rotational position and speed of each servo motor 41 to 47, and controls the rotational position of each servo motor 41 to 47 and the position of the tool 49. To do.

(Emergency mode)
Next, the emergency mode will be described.
When the manipulator 10 is operating in the normal state mode as described above, it is assumed that any part of the second to sixth joints interferes with an obstacle in the work area of the work or the manipulator 10. In the case of such interference, an interference sensor such as a collision detection sensor (not shown) provided in the manipulator 10 detects the interference and outputs an interference detection signal to the CPU 91.

  Based on this interference detection signal, the CPU 91 turns off the power supply of the servo amplifiers 61 to 67 for each joint axis and simultaneously operates a known brake control circuit (not shown). The brake control circuit controls and brakes a magnet brake (not shown) provided in the servomotors 41 to 47 so as to immediately apply a brake. By this braking, each joint axis of the manipulator 10 stops at the current position.

  In this state, the operator operates a mode switching key (not shown) of the input device 82. By this operation, the CPU 91 of the controller RC switches from the normal state mode to the emergency state mode, and executes the emergency state mode program shown in the flowchart of FIG.

Further, the CPU 91 stores the hand position / posture in the storage unit 94 when switching to the emergency mode.
After the execution of this program is started, the processing of S10 to S30 described later is first performed, and then the CPU 91 turns on the power of the servo amplifiers 61 to 67 of each joint axis, and simultaneously installs a brake control circuit (not shown). Operate to release the braking of a magnet brake (not shown).

(S10)
In S10, the CPU 91 sets the integral gain Ki of the speed control loop of the servo motor for each joint axis to 0 as shown in FIG.

(S20)
In S20, ΔΦ / Δθi of each joint axis at the current position of the manipulator 10 is calculated in a state where the hand position / posture is held (restrained). ΔΦ / Δθi is a ratio of a minute amount of rotation angle θi (i = 1 to 7) of each joint axis when a minute amount of posture parameter Φ described later moves.

Before describing the method of calculating ΔΦ / Δθi, the posture parameter Φ will be described.
As shown in FIG. 3, when the manipulator 10 having a redundant degree of freedom fixes the hand position, that is, when the hand position and posture are constrained, the link position and posture allowed by the redundant degree of freedom is shown. Is shown. As shown in FIG. 3, the fourth joint 24 of the manipulator 10 is centered on the second joint 22 (hereinafter referred to as a first reference point W), and the total link length of the third link 13 to the fourth link 14 is defined as a radius. The intersection circle E formed by the sphere A1 and the sphere A2 centered on the sixth joint 26 (hereinafter referred to as the second reference point K) and having the radius of the total link length of the fifth link 15 to the sixth link 16 It is possible to move up. Accordingly, the link position / posture changes so that the fourth joint 24 is positioned on the intersecting circle E.

  As shown in FIG. 3, the central axis O passing through the center of the intersecting circle E is a central axis O passing through the first reference point W (center of the second joint 22) and the second reference point K (center of the sixth joint 26). It is. Since the fourth joint 24 is located on the cross circle E, the posture parameter Φ can indicate the link position and posture on the cross circle E. Therefore, the angle from the appropriate position R on the intersecting circle E to the changed position is defined here as the posture parameter Φ. In the present embodiment, the position R is the current position of the fourth joint 24. The posture parameter Φ is a rotation angle around which the fourth joint 24 rotates around the central axis O. The posture parameter Φ is sometimes referred to as a rotation angle Φ.

(Calculation method of ΔΦ / Δθi)
In the above-described normal state mode, inverse transformation calculation is performed in order to obtain the joint angle of each joint axis from the hand position / posture, and the joint angles θ1, θ2, θ3,. Assuming that θ7 is the hand coordinates (x, y, z) and the hand posture (a, b, c), the vector θ and the hand position / posture X are expressed as follows.

Each joint angle θ1, θ2, θ3,..., Θ7 can be expressed as shown in equation (2), and these equations are inverse transformation equations.

  By the above equation (2), the joint angles θ1, θ2, θ3,..., Θ7 can be calculated from the hand position / posture X and the posture parameter Φ.

From these relationships, if differentiation is possible, the CPU 91 calculates Δθi in the assumed minute amount ΔΦ based on the equation (2). When differentiation cannot be performed, the CPU 91 obtains a difference using Expression (3) and Expression (4).

After calculating Δθi and ΔΦ as described above, the CPU 91 calculates ΔΦ / Δθi.

(S30)
In S30, instead of the gain K0, the CPU 91 calculates the gain of the position control loop of the servo motor of each joint axis using the equation (5) and sets it as shown in FIG.

The control period of this program is, for example, several milliseconds.

  As can be seen from this equation (5), ΔΦ is the same for each joint axis, but the gain Gi becomes smaller as the joint axis has a larger Δθi. As will be described later, this indicates that the larger the Δθi is, the larger the position deviation in the position control loop is. In order to cope with this, the larger the Δθi is, the smaller the gain Gi is.

  Therefore, the setting of the position control loop gain Gi and the speed control loop integral gain Ki to 0 by the CPU 91 of S10 to S30 is set very fast, and in this state, the operator Then, the manipulator 10 is moved by touching it directly.

  That is, in this state, the operator directly touches the manipulator 10 and moves it in the link position and orientation permitted by the redundancy degree of freedom, that is, in the link position and orientation indicated by the posture parameter Φ.

  At this time, if the operator manually moves the manipulator 10 so as to rotate at the link position and orientation represented by the orientation parameter Φ, the rotation angle θi differs for each joint axis.

  At this time, if the gain of the position control loop is the same as that in the normal mode, the current command in the servo motor of each joint axis increases as the position deviation with respect to the position command increases and should be held at that position. The motor torque (holding force) becomes stronger.

However, in the emergency mode, the gain Gi is made smaller as the position deviation is larger, so that the servo motor of any joint shaft can reduce the holding force.
Furthermore, since the integral gain Ki is set to 0 in the speed control loop, when the manipulator 10 is moved manually, even if the above-described positional deviation is large, the current command is not large, and the speed command is not large. The manipulator can be moved manually against the motor torque.

(S40)
In S40, after the process of S30, the CPU 91 determines whether or not the mode has been switched to the normal mode by the mode switch key. If there is no switch, the processes of S10 to S30 are repeated. When the switching is performed, the CPU 91 returns to the normal mode and returns the position gain of the position control loop and the integral gain Ki of the speed control loop from 0 to the original gain.

This embodiment has the following features.
(1) The robot control method according to the present embodiment is configured so that the manipulator around the central axis O connecting the second joint 22 and the sixth joint 26 of the manipulator 10 with the current position and orientation of the hand of the manipulator 10 maintained. The rotation angle Φ at which the tenth fourth joint 24 rotates can be manually changed, the integral gain Ki of the speed control loop is set to 0, and the rotation amount associated with the rotation of the fourth joint 24 around the central axis O increases. For the joint axis, the gain of the position control loop related to the joint axis is set smaller, and the servo motor (rotary actuator) of the joint axis is controlled.

  As a result, according to the present embodiment, it is possible to provide a robot control method that can easily and manually escape from the interference state while maintaining the position and orientation of the tool at the tip of the manipulator using the redundancy of the seven-axis manipulator. .

  In addition, since the link position and orientation can be changed in a state where the hand position of the manipulator is constrained, it is easy to continuously return to normal work after returning from the interference state.

  (2) In the robot control device (controller RC) of the present embodiment, the control system has the CPU 91 as the first setting unit with the second position of the manipulator 10 in the state where the current position and orientation of the hand of the manipulator 10 is held. The integral gain Ki of the speed control loop is set to 0 so that the rotation angle Φ of rotation of the fourth joint 24 of the manipulator 10 can be manually changed around the central axis O connecting the joint 22 and the sixth joint 26. Further, the CPU 91 sets the gain of the position control loop related to the joint axis as the second setting unit as the joint axis has a larger amount of rotation with the rotation of the fourth joint 24 around the central axis O. Further, as a control unit, the CPU 91 performs position control with the gain of the set position control loop, and performs proportional integral control with the set integral gain Ki to control the joint axis servo motor (rotary actuator). .

  As a result, according to the present embodiment, it is possible to provide a robot control device that can easily escape from the interference state manually while maintaining the position and orientation of the tool at the tip of the manipulator using the redundancy of the seven-axis manipulator. . In addition, since the link position and orientation can be changed in a state where the hand position of the manipulator is constrained, it is easy to continuously return to normal work after returning from the interference state.

  (3) The robot control device (controller RC) of the present embodiment includes an input device 82 (mode switching operation unit) in a normal state mode and an emergency state mode. Then, in the normal state mode, the CPU 91 (control unit) of the robot control device performs position control with a gain of a preset position control loop and performs proportional integral control with a preset integral gain and proportional gain. Go to control the servo motor (rotary actuator) of the joint axis.

  In the emergency state mode, the CPU 91 (control unit) performs position control with the gain of the set position control loop, and performs proportional integral control with the preset proportional gain and the set integral gain. Controls the servo motor (rotary actuator) of the joint shaft. As a result, according to the present embodiment, the effect (2) can be obtained when the mode switching operation unit is switched to the emergency state mode.

In addition, embodiment of this invention is not limited to the said embodiment, You may change as follows.
In the embodiment, the AC motor as the servo motor is used as the actuator. However, a DC motor or a stepping motor may be used.

RC: Controller (robot controller),
10 ... Manipulator, 11-18 ... Link,
22 ... 2nd joint, 24 ... 4th joint, 26 ... 6th joint, 27 ... 7th joint,
41-47 ... Servo motor (rotary actuator),
82 ... input device (mode switching operation unit),
91... CPU (first setting unit, second setting unit, control unit).

Claims (3)

In a control system including a position control loop and a speed control loop, a robot control method for controlling a rotation system actuator of each joint axis of a seven-axis manipulator having a first joint to a seventh joint in order from the fixed side,
A rotation angle at which the fourth joint of the manipulator rotates around an axis connecting the second joint and the sixth joint of the manipulator (hereinafter referred to as a central axis) while maintaining the current position and orientation of the hand of the manipulator The position control loop with respect to the joint axis can be changed as the joint axis has an integral gain of the speed control loop of 0 and the amount of rotation with the rotation of the fourth joint around the central axis increases. The robot control method is characterized in that the rotation system actuator of the joint axis is controlled by setting a small gain. In a control system including a position control loop and a speed control loop, in a robot control device for controlling a rotation system actuator of each joint axis of a seven-axis manipulator having a first joint to a seventh joint in order from the fixed side,
The control system includes
A rotation angle at which the fourth joint of the manipulator rotates around an axis connecting the second joint and the sixth joint of the manipulator (hereinafter referred to as a central axis) while maintaining the current position and orientation of the hand of the manipulator A first setting unit that sets the integral gain of the speed control loop to 0,
A second setting unit configured to set a gain of the position control loop with respect to the joint axis to be smaller as the joint axis has a larger rotation amount with the rotation of the fourth joint around the central axis;
A control unit that performs position control with the gain of the position control loop set by the second setting unit and performs proportional integral control with the integral gain set by the first setting unit to control the rotation system actuator of the joint axis. A robot control device comprising: It has a mode switching operation section for normal mode and emergency mode,
In the normal state mode, the control unit performs position control with a preset gain of the position control loop, and performs proportional integral control with a preset integral gain and proportional gain, thereby rotating the joint axis rotation system. Control the actuator,
In the emergency state mode, position control is performed with the gain of the position control loop set by the second setting unit, and proportional integral control is performed with the preset proportional gain and the integral gain set by the first setting unit. The robot control apparatus according to claim 2, wherein the rotation system actuator of the joint shaft is controlled.