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

Abstract

PROBLEM TO BE SOLVED: To provide a robot control method and a robot control device capable of suppressing the falling of a link of a load side connected to a joint axis affected by gravity acceleration when brake turning-off and servo locking are performed from a stop held state by a mechanical brake in a robot having the redundancy of a 7-axis manipulator.SOLUTION: In a state that a servo motor of each joint axis of a manipulator is controlled so as to set an optimal link position posture, and the joint axis is stopped and held at a stop position and in a stop posture, an integral gain of a speed control loop of the servo motor concerning a gravity axis is set larger than the integral gain at the time of servo control during the rotational operation of the gravity shaft to servo-lock the servo motor concerning the gravity axis (S50). The stop held state of the mechanical brake is released with the passage of a prescribed time when a motor current increases after the servo lock (S60).

Description

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

  Conventionally, in multi-joint robots, when the servo motor provided on each joint axis of the manipulator is servo controlled by the servo control system and the manipulator is stopped and held, the servo is turned off and held by the mechanical brake so that the gravity axis does not fall I am doing so.

  Patent Document 1 proposes a method in which the manipulator takes a specific posture when the manipulator is stopped and held, and Patent Document 2 proposes a frictional resistance in the movable part when the manipulator is in a standby state. There has been proposed a method for reducing the torque of the motor by using the above-mentioned.

Japanese Patent No. 3125946 Japanese Patent No. 3552076

By the way, conventionally, when the servo is turned on after being stopped and held by the mechanical brake, the brake of the mechanical brake and the servo lock are operated simultaneously.
However, depending on the link position and orientation of the manipulator, when the brake is turned off at the same time as the servo lock, the link on the load side connected to the joint axis of the joint axis that rotates in the same direction as the direction of gravitational acceleration is affected. May fall due to gravitational acceleration. Due to this fall, the tool or the like at the tip of the manipulator may collide with surrounding objects such as a workpiece, and the workpiece and the tool may be damaged.

In Patent Document 1 and Patent Document 2, there is no proposal of a method and an apparatus for solving the above-described problems when performing servo lock and brake off from a stopped holding state.
It is an object of the present invention to provide a robot having redundancy of a seven-axis manipulator on a load side connected to a joint shaft that is affected by gravitational acceleration when performing brake-off and servo-locking from a stop holding state by a mechanical brake. It is an object of the present invention to provide a robot control method and a robot control apparatus that can suppress the dropping of a link.

  In order to solve the above-described problems, the invention of claim 1 is directed to a state in which each joint of a manipulator having seven joint axes is held together with a brake off from a stop holding state in which the joint shaft is held at a stop position and a stop posture by a mechanical brake. In the robot control method for servo-locking the servo motor of the shaft, the joint position among the plurality of link position / posture allowed by the redundancy degree of freedom of the manipulator while the hand position / posture of the manipulator is held at the stop position and the stop posture. A first stage for obtaining an optimum link position and orientation at which a load torque caused by gravity is minimized, and a servo motor for each joint axis of the manipulator so as to be the optimum link position and orientation; The joint shaft is stopped and held by the mechanical brake at the stop position and the stop posture. And a second step of servo-locking the servo motor related to the gravity axis by making the integral gain of the speed control loop of the servo motor related to the gravity axis larger than the integral gain during servo control during rotation of the gravity axis. The gist of the robot control method includes a step and a fourth step of releasing the stop holding of the mechanical brake after a lapse of a predetermined time when the motor current increases after the servo lock.

  According to a second aspect of the present invention, in the first aspect, in the third stage, the integral gain of the speed control loop of the servo motor related to the gravity axis is set to the magnitude of the load torque of the gravity axis at the optimum link position / posture. Accordingly, the servo motor related to the gravity axis is servo-locked by making it larger than an integral gain at the time of servo control during rotation operation of the gravity axis.

  According to a third aspect of the present invention, there is provided a control unit that performs a servo control including a speed control loop that controls a speed of a servo motor of the joint shaft of a manipulator having seven joint shafts, and a mechanical brake provided for each joint shaft. A plurality of link positions and postures that are allowed by the redundancy degree of freedom of the manipulator while the hand position and posture of the manipulator are held at the stop position and the stop posture of the gravitational axis included in the joint axis. A calculating unit that calculates an optimum link position and orientation that minimizes a load torque caused by gravity of at least the servo motor, and the control unit includes a servo motor for each joint axis of the manipulator so as to be the optimum link position and orientation; A first control unit that controls and holds the joint shaft with a mechanical brake at the stop position and the stop posture; and the gravity A second control unit for servo-locking the servo motor related to the gravity axis by making the integral gain of the speed control loop of the servo motor greater than the integral gain during servo control during rotation of the gravity axis, and the servo lock The gist of the present invention is a robot control device including a third control unit that releases the stop holding of the mechanical brake after a predetermined time when the motor current increases later.

  According to a fourth aspect of the present invention, in the third aspect, the second control unit determines the integral gain of the speed control loop of the servo motor related to the gravity axis, and the magnitude of the load torque of the gravity axis at the optimum link position / posture. Accordingly, the servo motor related to the gravity axis is servo-locked by setting the gain larger than the integral gain during servo control during rotation of the gravity axis.

  According to the first aspect of the present invention, in the robot having the redundancy of the seven-axis manipulator, when the brake is turned off and the servo is locked from the stop holding state by the mechanical brake, the robot is connected to the joint shaft affected by the gravitational acceleration. It is possible to provide a robot control method capable of suppressing the drop of the load side link.

  According to the second aspect of the present invention, the integral gain of the speed control loop of the servo motor related to the gravity axis is determined according to the magnitude of the load torque of the gravity axis at the optimum link position and orientation during rotation of the gravity axis. Since it is larger than the integral gain at the time of servo control, it is possible to provide a robot control method that can output a motor current according to the load torque and can apply a servo lock according to the load torque.

  According to the invention of claim 3, in the robot having the redundancy of the seven-axis manipulator, when the brake is turned off and the servo is locked from the stop holding state by the mechanical brake, the robot is connected to the joint shaft affected by the gravitational acceleration. It is possible to provide a robot control device capable of suppressing the drop of the load side link.

  According to the fourth aspect of the present invention, the integral gain of the speed control loop of the servo motor related to the gravity axis is determined according to the magnitude of the load torque of the gravity axis at the optimum link position and orientation during the rotation operation of the gravity axis. Since it is larger than the integral gain at the time of servo control, it is possible to provide a robot controller capable of outputting a motor current according to the load torque and applying a servo lock according to the load torque.

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. The schematic block diagram of the servomotor and mechanical brake of one Embodiment. Explanatory drawing of the attitude | position parameter of one Embodiment. The flowchart of the program which the robot control apparatus of one Embodiment performs. The flowchart of the program which the robot control apparatus of one Embodiment performs. The block diagram of a position control loop and a speed control loop. The flowchart of the program which a robot control apparatus performs, when canceling | releases the stop holding | maintenance by a mechanical brake.

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. That is, the J2 axis, the J4 axis, and the J6 axis are joint axes (gravity axes) having a rotation direction in which gravitational acceleration acts.

  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.

  Moreover, the 2nd servomotor 42 is attached to the 2nd joint 22, and when the electric power is supplied, the 3rd link 13 is rotated with respect to the 2nd link 12 via the reduction gear which is not shown in figure. 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 is used as the servo motor, 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 joint angles with respect to the links 11 to 17 adjacent to each of the links 12 to 18 (note that the rotation angles of the joint shafts). And the detection signal is transmitted to the controller RC. In this way, the rotation angle of the joint shaft detected by the rotary encoders 71 to 77 at that time is stored in the storage unit 94 as current position data at that time.

  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 joint 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 rotation angle of the joint shaft may be attached.

  As shown in FIG. 3, brakes 141 to 147 as mechanical brakes are provided at the rotating portions of the servomotors 41 to 47, respectively. In FIG. 3, for convenience of explanation, the PWM generators 51 to 57, the servo amplifiers 61 to 67, the brakes 141 to 147, and the later-described brake circuits 101 to 107 are shown as one block.

  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 calculation unit, a first control unit, a second control unit, a third control unit, and a control unit.
Further, the brake circuits 101 to 107 are electrically connected to the CPU 91. When the CPU 91 outputs a stop signal, which will be described in the operation of the embodiment described later, to one of the brake circuits 101 to 107, the brake circuit that has input the stop signal operates the brake to activate the servo motor. It is possible to brake the operation independently.

  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.

(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.

5 and 6 are flowcharts executed by the CPU 91 when the manipulator 10 is stopped and held at the stop position described in the work program.
(S10)
The CPU 91 sets stop position data (stop position and stop posture) described in the work program.

(S20)
In S20, the CPU 91 performs a determination process of the load torque minimum link position and orientation of the gravity axis. Details of the processing of S20 will be described with reference to the flowchart of FIG. (S21)
In S21, the CPU 91 initializes the attitude parameter Φ. In this embodiment, the CPU 91 initializes Φ = 0, but the initial value is not limited to zero. The attitude parameter Φ will be described.

  The posture parameter Φ is allowed by the redundant degree of freedom when the manipulator 10 having the redundant degree of freedom holds the hand position and posture at the stop position and the stopped posture, that is, when the hand position and posture are restricted. This indicates the link position and orientation. Specifically, as shown in FIG. 4, the fourth joint 24 of the manipulator 10 is centered on the second joint 22 (hereinafter referred to as the first reference point W), and the link lengths of the third link 13 to the fourth link 14. And a sphere A2 having a radius that is the sum of the link lengths of the fifth link 15 to the sixth link 16 around the sixth joint 26 (hereinafter referred to as the second reference point K). Movement on the intersection circle E to be formed is possible. Therefore, in the present embodiment, the link position / posture changes so that the fourth joint 24 is positioned on the intersecting circle E.

  As shown in FIG. 4, the central axis O passing through the center of the intersecting circle E is an axis 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). is there. Since the fourth joint 24 is located on the cross circle E, the posture parameter Φ can be expressed as a parameter indicating 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.

(S22)
In S22, the CPU 91 updates the posture parameter Φ by adding a predetermined value. That is, the position and orientation of the link are changed by virtually increasing the orientation parameter Φ. The predetermined value is not limited, but may be an appropriate angle. For example, the predetermined value may be updated from several degrees to several tens degrees.

(S23)
In S23, an inverse transformation calculation is performed to obtain the joint angle from the hand position / posture.
Here, assuming that the joint angles q1, q2, q3,..., Q7 of the first joint 21 to the seventh joint 27 are the hand coordinates (x, y, z) and the hand posture (a, b, c), the vector q The hand position / posture X is expressed as follows.

Each joint angle q1, q2, q3,..., Q7 can be expressed as shown in Equation (2), and these equations are inverse transformation equations. In this step, the joint angles q1, q2, q3,..., Q7 are calculated from the hand position / posture X and the posture parameter Φ according to equation (2).

(S24)
In S <b> 24, the CPU 91 calculates the load torque of the motor provided on each joint shaft using the formula (3) and stores it in the storage unit 94.

  In the right side of Equation (3), the first term is the inertia force, the second term is the torque derived from the centrifugal force / Coriolis force, the third term is the torque derived from the gravity load, the fourth term is the friction torque, and the fifth term The term is the actuator inertia torque. Among these equations, the first term to the third term on the right side are equations of motion derived by the Lagrangian method and are known equations. The fourth and fifth terms on the right side are terms that take into account the effect of the actuator.

The angular velocity and angular acceleration of the joint i are calculated based on the obtained joint angle qi.

  The inertial force of the first term is as follows.

It should be noted that the parameter “known” or the like is stored in the storage unit 94.

When the calculation of the load torque of the motor provided on each joint axis of each axis is completed, the CPU 91 proceeds to S25.
(S25)
In S25, the CPU 91 returns to S22 if the posture parameter Φ is less than or equal to the preset upper limit value Φmax, and proceeds to S26 if the posture parameter Φ exceeds the preset upper limit value Φm. This upper limit value Φm is set in advance by a test or the like.

(S26)
In S26, the CPU 91 determines the posture parameter Φ which is the link position and posture with the minimum load torque among the gravity axes (J2 axis, J4 axis and J6 axis in the present embodiment) of the motor load torque calculated in S24. Is derived. The method for deriving the posture parameter Φ is, for example, calculating the average value by summing the load torque of the motor of the gravity axis obtained for each update parameter, and determining the posture parameter that minimizes the average value as the optimum link position posture Is derived (calculated) as a posture parameter Φ.

In this embodiment, the load torque of all the joint axes is calculated. However, in S26, the load torque of only the gravity axis may be calculated.
Next, returning to the flowchart of FIG.

(S30)
In S30, the CPU 91 performs an interpolation operation so that the link position / posture becomes the optimum link position / posture when the hand position / posture of the manipulator 10 reaches the stop position / stop posture set in S10, and then the stop position and Each joint axis is moved based on the result of the interpolation calculation toward the stop posture.

(Normal servo control)
Here, the control of the CPU 91 during normal servo control when the joint axes are moved will be described.

  As illustrated in FIG. 7, the CPU 91 performs position control, speed control, and current control as the position control unit 110, the speed control unit 120, and the current control unit 130, respectively. The position control, speed control, and current control will be described with reference to FIG.

In FIG. 7, 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. 7, the rotation of each servo motor obtained by the CPU 91 by performing an inverse conversion operation based on the target position and target posture of the end effector (tool 49) described in the work program stored in the RAM 93. The position is set as a 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. The speed gain K1 here is a total of the proportional gain KV of proportional control in PID control, the integral gain KI of integral control, and the differential gain KD of differential control, and each gain is set in advance. Note that the integral gain KI in this normal servo control is set to a value smaller than the integral gain tabulated.

As described above, the 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.

  That is, 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. 7, 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 positions and speeds of the servomotors 41 to 47, and servos the rotational positions of the servomotors 41 to 47 and the position of the tool 49. Control.

  The CPU 91 performs normal servo control as described above, and when the manipulator 10 reaches the stop position and the stop posture, the brake circuits 101 to 107 as the mechanical brakes operate the brakes 141 to 147 (brake on ) To stop. When the manipulator 10 reaches the stop position and the stop posture, the link position / posture is stopped and held by the mechanical brake in the state where the link position / posture is the optimum link position / posture derived in S26.

  Next, with reference to the flowchart of FIG. 8, the release and stop of the mechanical brake and the servo lock will be described in a state where the manipulator 10 is stopped and held in the optimum link position and posture state.

The flowchart of FIG. 8 is a flowchart of a program executed when releasing the stop by the mechanical brake.
(S40)
In S40, the CPU 91 loads the load torque at the optimum link position and orientation obtained by calculating the integral gain KI of the speed control loop of the gravity axis (in this embodiment, the J2, J4, and J6 axes) in S24. Accordingly, the predetermined number of times is set as compared with normal servo control to prepare for a servo lock in S50 described later.

  Note that, depending on the load torque, the larger the load torque, the larger the CPU 91 sets the integral gain KI. The relationship between the load torque and the magnitude of the integral gain KI is obtained in advance by a test or the like, and this relationship is stored in the storage unit 94 as a table of the load torque and the integral gain KI, for example.

(S50)
In S50, the CPU 91 applies a servo lock to the servo motors 42, 44, and 46 related to the gravity axes (J2, A4, and J6) with the integral gain KI set in S40, and the remaining joint axes. Servo locks are applied to servo motors 41, 43, and 45 (J1 axis, J3 axis, and J5 axis) with an integral gain in normal servo control.

(S60)
In S60, the CPU 91 releases the brakes 141 to 147 (brake off) in the brake circuits 101 to 107 when a predetermined time elapses after the servo lock.

Here, the predetermined time will be described.
In the state where the mechanical brake is stopped and held, since the servo lock is started, distribution with the current position as the target position starts and the position deviation is small, but it is not a large value because it is simply a proportional gain. On the other hand, since the integral gain of the speed control loop is increased, the current command increases, and the motor current output to the gravitational axis servomotor increases while the mechanical brake is stopped and held for a specified time. To do. In the present embodiment, an overcurrent determination threshold value (hereinafter referred to as a first threshold value) for determining whether or not the motor current detected by the current detection circuit (not shown) is an overcurrent, than the first threshold value. A small second threshold value is set in advance, and the time until the second threshold value is reached is set as a predetermined time.

  As a result, in this embodiment, the gravitational axis has a small positional deviation due to a state in which the gravity gain is stopped and held by the mechanical brake because the integral gain is set to a value that is a predetermined multiple larger than that during normal servo control. However, the current command (torque command) can be increased, and as a result, the torque for servo locking can be increased. Even after the mechanical brake is released, the torque at the time of servo lock is increased, so that the gravity axis can be prevented from dropping.

CPU91 complete | finishes the process of this flowchart, after complete | finishing cancellation | release of the stop holding | maintenance by the said mechanical brake.
This embodiment has the following features.

  (1) The robot control method according to the present embodiment is included in the joint axis among a plurality of link position / postures permitted by the redundancy degree of freedom of the manipulator 10 while the hand position / posture of the manipulator 10 is held at the stop position and the stop posture. The first stage (S24, S26) for obtaining the optimum link position and orientation at which the load torque including the torque due to gravity of the servo motors of the J2 axis, J4 axis, and J6 axis (gravity axis) is minimized. In addition, a second stage (S30) is provided in which the servo motors of the joint axes of the manipulator 10 are controlled so as to obtain the optimum link position and posture, and the joint shaft is stopped and held by the mechanical brake at the stop position and the stop posture. The robot control method is such that the integral gain of the servo motor speed control loop with respect to the J2, J4, and J6 axes (gravity axis) is larger than the integral gain during servo control during rotation of the gravity axis. And a third stage (S50) of servo-locking the servo motor related to the gravity axis. Then, the present control method includes a fourth stage (S60) for releasing the stop holding of the mechanical brake after a predetermined time when the motor current increases after the servo lock. As a result, according to the present embodiment, the robot having the redundancy of the seven-axis manipulator is connected to the joint shaft that is affected by the gravitational acceleration when the brake is turned off and the servo is locked from the stop holding state by the mechanical brake. The fall of the link on the load side can be suppressed.

  (2) In the robot control method of the present embodiment, in the third step (S50), the integral gain of the servo motor speed control loop for the J2, J4, and J6 axes (gravity axis) is set at the optimum link position and orientation. In accordance with the magnitude of the load torque of the gravity axis, the servo gain related to the gravity axis is servo-locked by making it larger than the integral gain at the time of servo control during rotation of the gravity axis. As a result, according to the present embodiment, the integral gain of the speed control loop of the servo motor related to the gravity axis is determined during rotation of the gravity axis according to the magnitude of the load torque of the gravity axis at the optimum link position and orientation. Therefore, the motor current can be output according to the load torque, and the servo lock can be applied according to the load torque.

  (3) The CPU 91 of the robot control device according to the present embodiment has a plurality of link position / postures that allow the redundancy degree of freedom of the manipulator 10 as the control unit while holding the hand position / posture of the manipulator 10 at the stop position and the stop position. Among them, the optimum link position / posture that minimizes the load torque including the torque caused by gravity of the servo motors of the J2 axis, J4 axis, and J6 axis (gravity axis) included in the joint axis is calculated.

  Further, the CPU 91 controls the servo motors of the joint axes of the manipulator 10 as the first control unit so as to obtain the optimum link position and posture, and stops and holds the joint shaft with the mechanical brake at the stop position and the stop posture. Further, as a second control unit, the CPU 91 determines the integral gain of the servo motor speed control loop with respect to the J2 axis, J4 axis, and J6 axis (gravity axis) from the integral gain during servo control during rotation of the gravity axis. Servo-lock the servo motor about the gravity axis. Further, as a third control unit, the CPU 91 releases the stop holding of the mechanical brake after a predetermined time when the motor current increases after the servo lock. As a result, according to the present embodiment, the robot having the redundancy of the seven-axis manipulator is connected to the joint shaft that is affected by the gravitational acceleration when the brake is turned off and the servo is locked from the stop holding state by the mechanical brake. The fall of the link on the load side can be suppressed.

  (4) In the robot control apparatus of the present embodiment, the CPU 91 (second control unit) determines the integral gain of the speed control loop of the servo motor related to the gravity axis, and the magnitude of the load torque of the gravity axis at the optimum link position and orientation. Accordingly, the servo motor for the J2 axis, J4 axis, and J6 axis (gravity axis) is servo-locked by setting it larger than the integral gain during servo control during rotation of the gravity axis. As a result, according to the present embodiment, the integral gain of the servo motor speed control loop with respect to the J2, J4, and J6 axes (gravity axis) is set to the magnitude of the load torque of the gravity axis at the optimum link position and orientation. Accordingly, since it is larger than the integral gain at the time of servo control during the rotation of the gravity axis, a motor current corresponding to the load torque can be output, and a servo lock can be applied according to the load torque.

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

In the embodiment, the gravity axis is the J2 axis, the J4 axis, and the J6 axis. However, the gravity axis is not limited to these axes, and another joint axis may be the gravity axis.
In the above-described embodiment, the load torque is calculated by the equation (3). However, at least the load torque derived from the gravity load shown in the third term of the equation (3) is calculated, and the load due to the gravity load is calculated. A posture parameter Φ that minimizes the torque may be derived.

  The present invention may be applied to, for example, a manipulator having a scalar axis, that is, a joint axis that moves horizontally and a gravity axis.

RC: Controller (robot controller),
10 ... Manipulator, 11-18 ... Link,
41-47 ... Servo motor,
91... CPU (calculation unit, first control unit, second control unit, third control unit and control unit).

Claims (4)

In a robot control method for servo-locking a servo motor of each joint axis together with a brake off from a stop holding state in which the joint axis of a manipulator having seven joint axes is held at a stop position and a posture by a mechanical brake,
Among the plurality of link positions and postures allowed by the redundancy degree of freedom of the manipulator while maintaining the hand position and posture of the manipulator at the stop position and the stop posture, at least the gravity of the servo motor of the gravity axis included in the joint axis A first stage for obtaining an optimum link position and orientation at which the resulting load torque is minimized;
A second stage of controlling the servo motor of each joint axis of the manipulator so as to be the optimum link position and posture and stopping and holding the joint shaft with a mechanical brake at the stop position and the stop posture;
A third stage in which the integral gain of the speed control loop of the servo motor related to the gravity axis is made larger than the integral gain during servo control during rotation of the gravity axis to servo-lock the servo motor related to the gravity axis;
A robot control method comprising: a fourth step of releasing stop holding of the mechanical brake after a predetermined time when the motor current increases after the servo lock.   In the third stage, the integral gain of the speed control loop of the servo motor related to the gravity axis is set according to the magnitude of the load torque of the gravity axis at the optimum link position and orientation during the rotation operation of the gravity axis. The robot control method according to claim 1, wherein the servo motor related to the gravity axis is servo-locked by making it larger than an integral gain at the time of control. In a robot control device including a control unit that performs a servo control including a speed control loop that controls a speed of a servo motor of the joint axis of a manipulator having seven joint axes, and a mechanical brake provided for each of the joint axes.
Of the plurality of link positions and postures allowed by the redundant degree of freedom of the manipulator while the hand position and posture of the manipulator are held at the stop position and the stop posture, at least due to gravity of the servo motor of the gravity axis included in the joint axis A calculation unit that calculates an optimal link position and orientation that minimizes the load torque to be
The control unit controls a servo motor of each joint shaft of the manipulator so as to be the optimum link position and posture, and the first control unit stops and holds the joint shaft with a mechanical brake at the stop position and the stop posture; A second control unit for servo-locking the servo motor related to the gravity axis by making an integral gain of the speed control loop of the servo motor related to the gravity axis larger than an integral gain during servo control during rotation of the gravity axis; A robot control apparatus comprising: a third control unit for releasing the mechanical brake stop and holding after a lapse of a predetermined time during which the motor current increases after the servo lock.   The second control unit determines the integral gain of the speed control loop of the servo motor related to the gravity axis in accordance with the magnitude of the load torque of the gravity axis at the optimum link position and orientation during rotation of the gravity axis. 4. The robot control apparatus according to claim 3, wherein the servo motor related to the gravity axis is servo-locked by making it larger than an integral gain at the time of servo control.