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

Abstract

PROBLEM TO BE SOLVED: To provide a robot control method that can operate while a trace of an end effector does not generate vibration following a command trace when the command trace accompanying an abrupt change is imparted, and to provide a robot control device.SOLUTION: A trace program is created at the most suitable link position attitude among link position attitudes of a manipulator. When the axial acceleration of an end effector in the most suitable link position attitude is larger than a first determination value in the trace program, it is determined whether or not the angle acceleration of each joint axis is larger than a second determination value, in a speed control loop of a control part, only proportional control based on a speed difference is performed when the angle acceleration of each joint axis is larger than the second determination value, and when the axis acceleration of the end effector in the most suitable position attitude is not larger than the first determination value, and also when the axis acceleration of the end effector is not larger than the first determination value, the proportional control and integration control are performed on the basis of the speed difference.

Description

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

  Conventionally, in multi-joint robots, when the robot is operated according to the taught operation program when accelerating when the robot starts operation from the stopped state or when decelerating after stopping from the operating state, the angular acceleration of each joint of the robot is excessive. It is known that this causes vibration (Patent Document 1). In Patent Document 1, in order to suppress this vibration, the angular acceleration of each joint is compared with a preset maximum angular acceleration, and if each angular acceleration is greater than the set maximum angular acceleration, each angular acceleration is It has been proposed to change to limit to the set maximum angular acceleration.

  Further, in Patent Document 2, in the case of an abrupt movement, the torque margin rate of each joint axis is calculated in consideration of the reaction force of other joints, and the minimum value among the torque margin rates of all the joint axes is calculated. The maximum allowable angular acceleration of each joint axis is calculated by the inertia or the like. During operation, control is performed so that the angular acceleration of each joint axis does not exceed the maximum allowable angular acceleration. This prevents the generated torque of the joint shaft from increasing the maximum allowable torque of the joint shaft.

Patent No. 2762789 Specification Paragraph 0003 Japanese Patent Laid-Open No. 09-204216

  By the way, in an articulated robot, for example, when the movement of the end effector suddenly changes due to a change in the command trajectory such that the end effector bends at right angles, the end effector greatly deviates from the command trajectory and the vibration trajectory is centered. A vibrating phenomenon appears. Such a phenomenon also occurs in an articulated robot having redundant degrees of freedom with respect to work degrees of freedom.

  Patent Document 1 has a problem of suppressing the occurrence of vibration in an operating state during acceleration or deceleration in a robot that does not have redundant degrees of freedom with respect to work degrees of freedom. However, the technique proposed in Patent Document 1 cannot cope with the change in the command trajectory in the robot having the redundancy degree of freedom with respect to the work degree of freedom.

The technique proposed in Patent Document 2 cannot be said to correspond to the change of the command trajectory in the robot having the redundancy degree of freedom with respect to the work degree of freedom.
It is an object of the present invention to provide a command trajectory for an end effector even when a command trajectory to bend is given to a robot having a redundancy degree of freedom with respect to work freedom from a straight movement of the end effector by the command trajectory. It is an object to provide a robot control method and a robot control apparatus that can draw and operate without generating vibration.

  In order to solve the above problems, the invention of claim 1 is directed to a robot control method in which a rotation unit actuator for each joint axis of a seven-axis manipulator is controlled by a control unit having a speed control loop. Among the link positions and postures of the manipulator allowed by the characteristics, a trajectory plan is created at a link position and posture that minimizes the average angular acceleration of each joint axis (hereinafter referred to as the optimal link position and posture). It is determined whether or not the axial acceleration of the end effector at the optimum link position and orientation is larger than a first determination value. When the axial acceleration of the end effector is larger than the first determination value on the trajectory plan, the angle of each joint axis It is determined whether or not the acceleration is greater than a second determination value. In the speed control loop of the control unit, the angular acceleration of each joint axis is based on the second determination value. In the case where it is large, only proportional control based on the speed deviation is performed, and when the axial acceleration of the end effector at the optimum link position and orientation is not larger than the first determination value and when the axial acceleration of the end effector is not larger than the first determination value The gist of the robot control method is to perform at least proportional control and integral control based on the speed deviation.

  According to a second aspect of the present invention, there is provided a robot control apparatus for controlling a rotation system actuator for each joint axis of a seven-axis manipulator by a control unit having a speed control loop. Of the postures, a trajectory planning unit that creates a trajectory plan at a link position and orientation (hereinafter referred to as an optimal link position and orientation) that minimizes the average angular acceleration of each joint axis, and the optimal link position and orientation on the trajectory plan A first determination unit that determines whether or not the axial acceleration of the end effector is greater than a first determination value; and when the axial acceleration of the end effector is greater than a first determination value in the trajectory plan, the angle of each joint axis A second determination unit that determines whether or not the acceleration is greater than a second determination value; and in the speed control loop of the control unit, When the degree is larger than the second determination value, only proportional control based on the speed deviation is performed. When the axial acceleration of the end effector at the optimum link position and orientation is not larger than the first determination value, the axial acceleration of the end effector is the first determination. The gist of the present invention is a robot control device that performs at least proportional control and integral control based on the speed deviation when the value is not larger than the value.

  According to the first aspect of the present invention, even when a command trajectory to bend is given to the robot having a redundancy degree of freedom relative to the degree of freedom of operation from the straight movement of the end effector by the command trajectory, the trajectory of the end effector Can provide a robot control method that can draw and operate according to the command trajectory without generating vibration.

  According to the second aspect of the present invention, even when a command trajectory to bend is given to the robot having a redundancy degree of freedom relative to the degree of freedom of operation from the straight movement of the end effector by the command trajectory, the trajectory of the end effector Can provide a robot control device that can draw and operate according to the command trajectory without generating vibration.

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 at the time of reading the work program which the robot control apparatus of one Embodiment performs. The flowchart of actuator average angular acceleration minimization orbit calculation. The block diagram of a position control loop and a speed control loop.

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 a rotary actuator (hereinafter simply referred to as an actuator), but is not limited thereto.

  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. In the work program, parameters such as a teaching point, an operation command, or various operation conditions are described for each teaching step (a line of a program described by teaching).

  The robot of the present embodiment is a robot that operates in a teaching playback system, and the manipulator 10 operates by executing the work program. 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 trajectory planning unit, a first determination unit, a second determination 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.

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

The flowchart of FIG. 4 is a process executed by the CPU 91 for each teaching step in which the teaching points of the work program are described.
(S10)
In S10, as shown in FIG. 4, the CPU 91 reads the teaching step described in the work program.

(S20)
In S20, the CPU 91 performs the actuator average angular acceleration minimized trajectory calculation for the teaching point read in S10. S20 is a trajectory plan creation process.

(Actuator average angular acceleration minimized trajectory calculation)
FIG. 5 is a flowchart of the actuator average angular acceleration minimized trajectory calculation executed by the CPU 91.

(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 with reference to FIG.
The posture parameter Φ indicates a link position / posture permitted by the redundancy degree of freedom when the manipulator 10 having the degree of freedom of freedom fixes the hand position, that is, when the hand position / posture is constrained. Note that the position and orientation of the tool are referred to as position and orientation.

  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. 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. 3, 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 S <b> 22, the CPU 91 performs an inverse conversion calculation to obtain the joint angle from the hand position / posture. Here, assuming that the joint angles θ1, θ2, θ3,..., Θ7 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 θ The hand position / posture X is 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. In this step, the joint angles θ1, θ2, θ3,..., Θ7 are calculated from the hand position / posture X and the posture parameter Φ using Equation (2).

(S23)
In S23, the CPU 91 calculates each angular acceleration for each joint axis in the posture parameter Φ, and stores each angular acceleration in the posture parameter Φ as a calculation result in the storage unit 94.

  The angular acceleration is calculated by first calculating the angular velocity by differentiating the joint angles θ1, θ2, θ3,..., Θ7 obtained by the equation (2) with, for example, a minute time of the calculation cycle. Further, it is obtained by differentiating with a minute time of the calculation cycle.

(S24)
In S24, the CPU 91 adds and updates a predetermined value as the attitude parameter Φ. That is, the position and orientation of the link are changed by virtually increasing the orientation parameter Φ.

(S25)
In S25, if the posture parameter Φ is equal to or smaller than the preset upper limit value Φmax, the CPU 91 returns to S22. If the posture parameter Φ is larger than the preset upper limit value Φm, the CPU 91 proceeds to S26. This upper limit value Φm is set in advance by a test or the like.

  In this way, when returning from S25 to S22, since the processing of S22 to S24 is repeated thereafter, the angular acceleration of each joint axis at different posture parameters Φ is stored in the storage unit 94.

(S26)
In S26, the CPU 91 calculates the average value by adding the angular accelerations of all joint axes for each different posture parameter Φ calculated every time the posture parameter Φ is updated in S23, and the average value becomes the minimum value. The attitude parameter Φ is derived. That is, the CPU 91 calculates the link position / posture having the smallest average angular acceleration of all joint axes as the optimum link position / posture, and the CPU 91 takes the derived link position / posture for each joint axis of the manipulator 10. The servo motor is driven and moved to the teaching point.

(S30)
Returning to FIG. 4, in S <b> 30, the CPU 91 performs an interpolation calculation with the teaching point read in S <b> 10 as a target value. Since the interpolation time in the interpolation calculation is generally 10 to 50 msec in general, the joint angle of each joint axis for each interpolation time is calculated, and the angular velocity of each joint axis is divided by the interpolation time. Then, the angular acceleration of each joint axis (hereinafter referred to as the acceleration of the actuator) is calculated, and the axial acceleration that is the acceleration in the axial directions of the X, Y, and Z axes in the tool coordinate axis system of the end effector is calculated. In addition, since the calculation method of the axial acceleration is publicly known, the description thereof is omitted.

(S40)
In S40, the CPU 91 determines whether or not the accelerations in the axial directions of the X, Y, and Z axes calculated in S30 are larger than a first threshold value as a first determination value. The first threshold value (first determination value) is used to assign a speed control method to be performed later when the axial acceleration of the end effector is large or small. The first threshold value is a value obtained in advance by a test or simulation so that the end effector moves along a command trajectory (a trajectory drawn by a plurality of teaching points).

  In S40, when the acceleration in the axial direction of each of the X, Y, and Z axes is larger than the first threshold value as the first determination value, the process proceeds to S50, and the axial direction of each of the X, Y, and Z axes is determined. When the acceleration is equal to or less than the first threshold value as the first determination value, the process proceeds to S70.

(S50)
In S50, the CPU 91 determines whether or not the calculated acceleration of each actuator is greater than a second threshold value as a second determination value by the interpolation calculation in S30. The second threshold value (second determination value) is used to assign a speed control method to be performed later when the acceleration of each actuator is large or small. The second threshold value is a value obtained in advance so that the end effector moves along the command locus (trajectory drawn by the teaching point) through a test or simulation.

  In S50, if the calculated acceleration of each actuator is larger than the second threshold value as the second determination value, the process proceeds to S60, and the calculated acceleration of each actuator is equal to or less than the second threshold value as the second determination value. In this case, the speed control unit 120 described later is set to PID control as a default, and the process proceeds to S70.

(S60)
In S60, the CPU 91 sets the speed control unit 120 of the actuator related to the joint axis that is larger than the second threshold value to only proportional control, and proceeds to S70.

(S70-S90)
In S70 to S90, as shown in FIG. 6, 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. 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, the rotation of each servo motor obtained by the CPU 91 by performing an inverse transformation calculation 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.

  Furthermore, if the speed deviation between the speed command ωs * and the actual speed ω of each of the servomotors 41 to 47 obtained from the actual position θk is not set to only proportional control in S60 by the speed control unit 120, In PID control, a current command iq * for each of the servo motors 41 to 47 is calculated by multiplying a predetermined speed gain K1 set in advance, 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. Here, a speed control loop in which the actual speed ω is fed back is configured.

  Further, in the case of an actuator of a joint axis that is set to only proportional control in S60, a current command iq * is calculated only with proportional control with the proportional gain KV shown in FIG. Output to the control unit 130.

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

(S100)
In S100, the CPU 91 determines whether or not the teaching point read in S10, that is, the target position has been reached. If the target position has not been reached, the process returns to S70. The flowchart ends.

  Thus, in the present embodiment, in the determination of S40, the acceleration in the axial direction of each of the X, Y, and Z axes of the end effector (tool 49) is larger than the first threshold value. In the determination, if the acceleration of the actuator is larger than the second threshold value, it is determined that the operation suddenly changes due to the change of the command locus (trajectory drawn by the teaching point), and the speed of the actuator of the joint axis is determined. In the control unit, only proportional control is performed. As a result, even when a command trajectory to bend is given from the straight movement of the end effector by the command trajectory, the end effector trajectory can be suppressed from generating vibrations according to the command trajectory.

This embodiment has the following features.
(1) The robot control method according to the present embodiment is a link position / posture (optimal link position / posture) that minimizes the average angular acceleration of each joint axis among the link positions / postures of the manipulator 10 permitted for redundancy for each teaching point. ) Is created (S20). Further, it is determined whether or not the axial acceleration of the tool 49 (end effector) at the optimum link position and orientation is larger than a first threshold value (first determination value) on the trajectory plan, and the tool 49 (end effector) is determined on the trajectory plan. When the axial acceleration is greater than the first threshold (first determination value), it is determined whether the angular acceleration of each joint axis is greater than the second threshold (second determination value). In the speed control loop of the CPU 91 (control unit), when the angular acceleration of each joint axis is larger than the (second determination value), only proportional control based on the speed deviation is performed, and the tool 49 (end effector) at the optimum link position and orientation When the axial acceleration of the tool 49 (end effector) is not greater than the first threshold value and when the axial acceleration of the tool 49 (end effector) is not greater than the first determination value, proportional control and integral control are performed based on the speed deviation. To do. As a result, according to the present embodiment, even when a command trajectory to bend is given to the robot having a redundancy degree of freedom relative to the degree of freedom of operation from the straight movement of the end effector by the command trajectory, The trajectory can be drawn and operated according to the command trajectory without generating vibration.

  Further, according to the present embodiment, the angular accelerations of all joint axes for each posture parameter Φ are added corresponding to the command trajectory to calculate the average value, and the optimal link position and posture with the average value being the minimum value, Since the trajectory is planned, the manipulator can be operated with an optimum link position and orientation along a command trajectory that draws a trajectory that curves from a straight trajectory.

  (2) The CPU 91 of the robot control apparatus according to the present embodiment serves as a trajectory plan unit, and for each teaching point, among the link positions and orientations of the manipulator 10 permitted for redundancy, a link that minimizes the average angular acceleration of each joint axis. Create a trajectory plan in position and orientation (optimum link position and orientation). In addition, the CPU 91 determines, as a first determination unit, whether or not the axial acceleration of the tool 49 (end effector) at the optimum link position and orientation is larger than a first threshold value (first determination value) in the trajectory plan.

  In addition, the CPU 91 serves as a second determination unit, and when the axial acceleration of the tool 49 (end effector) is larger than the first threshold (first determination value) in the trajectory plan, the angular acceleration of each joint axis is the second threshold (first 2) It is determined whether or not it is larger. In the speed control loop of the CPU 91 (control unit), when the angular acceleration of each joint axis is larger than the (second determination value), only proportional control based on the speed deviation is performed, and the tool 49 (end effector) at the optimum link position and orientation is performed. When the axial acceleration of the tool 49 (end effector) is not greater than the first threshold value and when the axial acceleration of the tool 49 (end effector) is not greater than the first determination value, proportional control and integral control are performed based on the speed deviation. To do. As a result, according to the present embodiment, even when a command trajectory to bend is given to the robot having a redundancy degree of freedom relative to the degree of freedom of operation from the straight movement of the end effector by the command trajectory, The trajectory can be drawn and operated according to the command trajectory without generating vibration.

  As a result, in the robot control device according to the present embodiment, even when a command trajectory to bend is given to the robot having a redundancy degree of freedom relative to the work freedom from the straight movement of the end effector by the command trajectory, the end control The trajectory of the effector can be drawn and operated according to the command trajectory without generating vibration. Further, according to the present embodiment, the angular accelerations of all joint axes for each posture parameter Φ are added corresponding to the command trajectory to calculate the average value, and the optimal link position and posture with the average value being the minimum value, Since the trajectory is planned, the manipulator can be operated with an optimum link position and orientation along a command trajectory that draws a trajectory that curves from a straight trajectory.

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.

  In the embodiment, the speed control unit 120 performs PID control (proportional integral derivative control) or P control (proportional control), but performs PI control (proportional integral control) or P control (proportional control). You may do it.

  In the above-described embodiment, the flowchart of FIG. 4 is executed at all teaching points. However, if the command trajectory can be predicted to change rapidly between specific teaching points in advance in the work program, In addition to excluding specific teaching points, instructions are given to the teaching steps of the work program so as to shift to S20 so that the processing of S20 shown in FIG. 4 is performed only at specific teaching points. At the teaching point, after jumping from S10 to S20, the process of S30 may be performed, and then jumping to S70.

RC: Controller (robot controller),
10 ... Manipulator, 11-18 ... Link,
41-47 ... Servo motor (rotary actuator),
91 ... CPU (trajectory planning unit, first determination unit, second determination unit, control unit).

Claims (2)

In a robot control method for controlling a rotation system actuator of each joint axis of a seven-axis manipulator by a control unit including a speed control loop,
For each teaching point, create a trajectory plan at the link position / posture (hereinafter referred to as the optimal link position / posture) that minimizes the average angular acceleration of each joint axis among the link position / posture of the manipulator allowed by redundancy,
Determining whether or not the axial acceleration of the end effector at the optimum link position and orientation on the trajectory plan is greater than a first determination value;
When the axial acceleration of the end effector is greater than a first determination value on the trajectory plan, determine whether the angular acceleration of each joint axis is greater than a second determination value;
In the speed control loop of the control unit, only proportional control based on speed deviation is performed when the angular acceleration of each joint axis is greater than a second determination value, and the axial acceleration of the end effector at the optimal link position and orientation is determined by the first determination. A robot control method comprising performing at least proportional control and integral control based on a speed deviation when the value is not greater than the value and when the axial acceleration of the end effector is not greater than the first determination value. In a robot control apparatus that controls a rotation system actuator of each joint axis of a seven-axis manipulator by a control unit including a speed control loop,
A trajectory for creating a trajectory plan at the link position and orientation (hereinafter referred to as the optimal link position and orientation) that minimizes the average angular acceleration of each joint axis among the link positions and orientations of the manipulator allowed for redundancy for each teaching point Planning department,
A first determination unit that determines whether or not the axial acceleration of the end effector at the optimum link position and orientation is greater than a first determination value on the trajectory plan;
A second determination unit that determines whether or not the angular acceleration of each joint axis is greater than a second determination value when the axial acceleration of the end effector is greater than a first determination value on the trajectory plan;
In the speed control loop of the control unit, only proportional control based on speed deviation is performed when the angular acceleration of each joint axis is greater than a second determination value, and the axial acceleration of the end effector at the optimal link position and orientation is determined by the first determination. A robot control device that performs at least proportional control and integral control based on a speed deviation when the value is not greater than the value and when the axial acceleration of the end effector is not greater than the first determination value.