Motor control system, control method of motor control system, and robot system
阅读说明：本技术 马达控制系统、马达控制系统的控制方法以及机器人系统 (Motor control system, control method of motor control system, and robot system ) 是由 坪井信高 于 2018-05-17 设计创作，主要内容包括：本发明涉及马达控制系统,基于从上一级装置输入的位置指令控制向马达(11)供给的电流来控制马达(11),并且控制输入轴(13a)与马达(11)的输出轴(11a)连接并且输出轴(13b)与负载连接的减速机(13)的动作,包括：探测部(12),探测用于检测马达(11)的输出轴(11a)的旋转速度的事件；速度偏差生成部(31),基于位置指令生成速度指令,并且计算速度指令、与基于探测部(12)所探测到的事件检测出的马达(11)的输出轴(11a)的旋转速度的偏差即速度偏差；角度传递误差补偿部(34),推断马达(11)的输出轴(11a)的旋转角与减速机(13)的输出轴(13b)的旋转角之间的角度传递误差,并且基于推断出的角度传递误差,补正速度指令、速度偏差、或基于探测部(12)所探测到的事件检测出的马达(11)的输出轴(11a)的旋转速度；电流指令生成部(36),基于速度偏差生成电流指令；以及电流控制部(23),基于电流指令控制向马达(11)供给的电流。(The present invention relates to a motor control system for controlling the operation of a speed reducer (13) in which an input shaft (13a) is connected to an output shaft (11a) of a motor (11) and an output shaft (13b) is connected to a load, while controlling the motor (11) by controlling the current supplied to the motor (11) based on a position command input from a higher-order device, the motor control system including: a detection unit (12) that detects an event for detecting the rotational speed of the output shaft (11a) of the motor (11); a speed deviation generation unit (31) that generates a speed command on the basis of the position command, and calculates a speed deviation, which is a deviation between the speed command and the rotational speed of the output shaft (11a) of the motor (11) detected on the basis of the event detected by the detection unit (12); an angle transmission error compensation unit (34) that estimates an angle transmission error between the rotation angle of the output shaft (11a) of the motor (11) and the rotation angle of the output shaft (13b) of the speed reducer (13), and corrects a speed command, a speed deviation, or a rotation speed of the output shaft (11a) of the motor (11) detected based on an event detected by the detection unit (12), based on the estimated angle transmission error; a current command generation unit (36) that generates a current command on the basis of the speed deviation; and a current control unit (23) that controls the current supplied to the motor (11) on the basis of the current command.)
1. A motor control system for controlling a motor by controlling a current supplied to the motor based on a position command input from a higher-level device, and controlling an operation of a speed reducer having an input shaft connected to an output shaft of the motor and an output shaft connected to a load,
the motor control system includes:
a detection section that detects an event for detecting a rotation speed of an output shaft of the motor;
a speed deviation generating unit that generates a speed command based on the position command and calculates a speed deviation, which is a deviation between the speed command and a rotation speed of the output shaft of the motor detected based on the event detected by the detecting unit;
an angle transmission error compensation unit that estimates an angle transmission error between a rotation angle of an output shaft of the motor and a rotation angle of an output shaft of the speed reducer, and corrects the speed command, the speed deviation, or a rotation speed of the output shaft of the motor detected based on the event detected by the detection unit, based on the estimated angle transmission error;
a current command generation unit that generates a current command based on the speed deviation; and
and a current control unit that controls the current supplied to the motor based on the current command.
2. The motor control system of claim 1,
the speed command is a speed feedforward command obtained by time-differentiating the position command.
3. The motor control system of claim 2,
the angle transfer error compensation unit corrects the speed command by correcting the speed feedforward command based on the estimated angle transfer error.
4. The motor control system of claim 1,
the detection section detects an event for detecting a rotation angle and a rotation speed of an output shaft of the motor,
the speed command is a value obtained by adding a speed feedforward command obtained by time-differentiating the position command and a gain speed command obtained by multiplying a position proportional gain by a deviation between the position command and a rotation angle of the output shaft of the motor detected based on the event detected by the detection unit.
5. The motor control system of claim 4,
the angle transfer error compensation unit corrects the speed command by correcting the speed feedforward command based on the estimated angle transfer error.
6. The motor control system of claim 5,
the angle transfer error compensation unit further corrects the position command based on the estimated angle transfer error,
the speed command is a value obtained by adding a speed feedforward command obtained by time-differentiating the position command input from a device at a higher stage to a gain speed command obtained by multiplying a position proportional gain by a deviation between the corrected position command and a rotation angle of the output shaft of the motor detected based on the event detected by the detection unit.
7. The motor control system according to any one of claims 1 to 6,
the angle transfer error compensation unit estimates the angle transfer error based on a periodic function in which periodic variations of the angle transfer error are modeled.
8. The motor control system according to any one of claims 1 to 7,
the speed reducer is a wave gear device.
9. A control method of a motor control system for controlling a motor by controlling a current supplied to the motor based on a position command input from a higher-order device, and controlling an operation of a speed reducer having an input shaft connected to an output shaft of the motor and an output shaft connected to a load,
the motor control system includes:
a detection section that detects an event for detecting a rotation speed of an output shaft of the motor;
a speed deviation generating unit that generates a speed command based on the position command and calculates a speed deviation that is a deviation between the speed command and a rotation speed of the output shaft of the motor detected based on the event detected by the detecting unit;
an angle transmission error compensation unit that estimates an angle transmission error between a rotation angle of an output shaft of the motor and a rotation angle of an output shaft of the speed reducer, and corrects the speed command, the speed deviation, or a rotation speed of the output shaft of the motor detected based on the event detected by the detection unit, based on the estimated angle transmission error;
a current command generation unit that generates a current command based on the speed deviation; and
and a current control unit that controls the current supplied to the motor based on the current command.
10. A robot system, wherein,
the robot system includes:
a robot arm;
a motor that is a driving source of a joint of the robot arm;
a speed reducer having an input shaft connected to an output shaft of the motor and an output shaft connected to the joint of the robot arm;
an instruction unit that generates a position instruction; and
a motor control system for controlling the operation of the motor by controlling the current supplied to the motor based on the position command generated by the command unit,
the motor control system includes:
a detection section that detects an event for detecting a rotation speed of an output shaft of the motor;
a speed deviation generating unit that generates a speed command based on the position command and calculates a speed deviation that is a deviation between the speed command and a rotation speed of the output shaft of the motor detected based on the event detected by the detecting unit;
an angle transmission error compensation unit that estimates an angle transmission error between a rotation angle of an output shaft of the motor and a rotation angle of an output shaft of the speed reducer, and corrects the speed command, the speed deviation, or a rotation speed of the output shaft of the motor detected based on the event detected by the detection unit, based on the estimated angle transmission error;
a current command generation unit that generates a current command based on the speed deviation; and
and a current control unit that controls the current supplied to the motor based on the current command.
Technical Field
The invention relates to a motor control system, a control method of the motor control system, and a robot system.
Background
Conventionally, there is known a positioning system capable of correcting a positioning error caused by an angle transmission error of a reduction gear (for example, see patent document 1).
The positioning system includes an error correction unit that corrects a position command based on error correction data for correcting a positioning error of an output shaft of an actuator including a motor and a speed reducer, and supplies the corrected position command to a driver that drives the motor. This can correct the positioning error of the actuator.
Patent document 1: japanese patent laid-open publication No. 2003-223225
However, the positioning system described in patent document 1 may not appropriately suppress the fluctuation of the rotation speed of the output shaft of the actuator, and the behavior of the load connected to the output shaft of the actuator may become unstable.
Disclosure of Invention
In order to solve the above problem, a motor control system according to one aspect controls a current supplied to a motor based on a position command input from a higher-order device to control the motor, and controls an operation of a speed reducer having an input shaft connected to an output shaft of the motor and an output shaft connected to a load, the motor control system including: a detection unit that detects an event for detecting a rotational speed of an output shaft of the motor; a speed deviation generating unit that generates a speed command based on the position command and calculates a speed deviation, which is a deviation between the speed command and a rotation speed of an output shaft of the motor detected based on the event detected by the detecting unit; an angle transmission error compensation unit that estimates an angle transmission error between a rotation angle of an output shaft of the motor and a rotation angle of an output shaft of the speed reducer, and corrects the speed command, the speed deviation, or a rotation speed of the output shaft of the motor detected based on the event detected by the detection unit, based on the estimated angle transmission error; a current command generation unit that generates a current command based on the speed deviation; and a current control unit that controls a current to be supplied to the motor based on the current command.
According to this configuration, the angle transmission error can be compensated with a simple configuration, and unstable behavior of the load due to the angle transmission error can be suppressed.
The present invention has an effect of suppressing unstable behavior of the load due to the angle transmission error.
Drawings
Fig. 1 is a diagram schematically showing a configuration example of a robot system according to embodiment 1.
Fig. 2 is a block diagram schematically showing an example of the configuration of a control system of the robot system of fig. 1.
Fig. 3 is a block diagram schematically showing an example of the configuration of a control system of the servo control unit of the robot system of fig. 1.
Fig. 4 is an explanatory diagram of an angle transfer error.
Fig. 5 is a graph showing a relationship between a speed feedforward command value and a correction value of the speed feedforward command value in an operation example of the robot system according to embodiment 2.
Fig. 6 is a block diagram schematically showing an example of the configuration of a control system of a servo control unit of a robot system according to embodiment 3 of the present invention.
Fig. 7 is a block diagram schematically showing an example of the configuration of a control system of a servo control unit of the robot system according to embodiment 4.
Fig. 8 is a block diagram schematically showing an example of the configuration of a control system of a servo control unit of the robot system according to embodiment 5.
Fig. 9 is a block diagram schematically showing an example of the configuration of a control system of a servo control unit of the robot system according to embodiment 6.
Fig. 10 is a block diagram schematically showing an example of the configuration of a control system of a servo control unit of a robot system according to embodiment 7.
Detailed Description
A motor control system according to one aspect controls a current supplied to a motor based on a position command input from a higher-order device to control the motor, and controls an operation of a speed reducer having an input shaft connected to an output shaft of the motor and an output shaft connected to a load, the motor control system including: a detection unit that detects an event for detecting a rotational speed of an output shaft of the motor; a speed deviation generating unit that generates a speed command based on the position command and calculates a speed deviation, which is a deviation between the speed command and a rotation speed of an output shaft of the motor detected based on the event detected by the detecting unit; an angle transmission error compensation unit that estimates an angle transmission error between a rotation angle of an output shaft of the motor and a rotation angle of an output shaft of the speed reducer, and corrects the speed command, the speed deviation, or a rotation speed of the output shaft of the motor detected based on the event detected by the detection unit, based on the estimated angle transmission error; a current command generation unit that generates a current command based on the speed deviation; and a current control unit that controls a current to be supplied to the motor based on the current command.
According to this configuration, the angle transmission error can be compensated with a simple configuration, and unstable behavior of the load due to the angle transmission error can be suppressed.
The following may be configured: the speed command is a speed feedforward command obtained by time-differentiating the position command.
According to this configuration, the speed of the motor can be appropriately controlled in accordance with a speed feedforward command for controlling the rotation speed of the output shaft of the motor.
The following may be configured: the angle transfer error compensation unit corrects the speed command by correcting the speed feedforward command based on the estimated angle transfer error.
According to this configuration, by correcting the speed feedforward command for controlling the rotation speed of the output shaft of the motor, the speed command can be appropriately corrected, and the angle transmission error can be appropriately compensated.
The following may be configured: the detection unit detects an event for detecting a rotation angle and a rotation speed of the output shaft of the motor, the speed command is a value obtained by adding a speed feedforward command obtained by time-differentiating the position command to a gain speed command obtained by multiplying a position proportional gain by a deviation between the position command and the rotation angle of the output shaft of the motor detected based on the event detected by the detection unit.
According to this configuration, the speed of the motor can be appropriately controlled in accordance with the speed feedforward command for controlling the rotation speed of the output shaft of the motor, and the rotation angle of the motor can be appropriately controlled in accordance with the gain speed command.
The following may be configured: the angle transfer error compensation unit corrects the speed command by correcting the speed feedforward command based on the estimated angle transfer error.
According to this configuration, by correcting the speed feedforward command for controlling the rotation speed of the output shaft of the motor, the speed command can be appropriately corrected, and the angle transmission error can be appropriately compensated.
The following may be configured: the angle transmission error compensation unit may further correct the position command based on the estimated angle transmission error, wherein the speed command is a value obtained by adding a speed feedforward command obtained by time-differentiating the position command input from the higher-stage device to a gain speed command obtained by multiplying a position proportional gain by a deviation between the corrected position command and a rotation angle of the output shaft of the motor detected based on the event detected by the detection unit.
With this configuration, the angle transfer error can be more appropriately compensated.
The following may be configured: the angle transfer error compensation unit estimates the angle transfer error based on a periodic function in which periodic variation of the angle transfer error is modeled.
With this configuration, the angular transfer error represented as the periodic error can be appropriately compensated.
The following may be configured: the speed reducer is a wave gear device.
According to this configuration, unstable behavior of the load due to an angle transmission error of the wave gear device can be suppressed.
A control method of a motor control system according to an aspect of the present invention is a control method of a motor control system for controlling a motor by controlling a current supplied to the motor based on a position command input from a higher-order device, and controlling an operation of a speed reducer having an input shaft connected to an output shaft of the motor and an output shaft connected to a load, the motor control system including: a detection unit that detects an event for detecting a rotational speed of an output shaft of the motor; a speed deviation generating unit that generates a speed command based on the position command and calculates a speed deviation that is a deviation between the speed command and a rotation speed of an output shaft of the motor detected based on the event detected by the detecting unit; an angle transmission error compensation unit that estimates an angle transmission error between a rotation angle of an output shaft of the motor and a rotation angle of an output shaft of the speed reducer, and corrects the speed command, the speed deviation, or a rotation speed of the output shaft of the motor detected based on the event detected by the detection unit, based on the estimated angle transmission error; a current command generation unit that generates a current command based on the speed deviation; and a current control unit that controls a current to be supplied to the motor based on the current command.
According to this configuration, the angle transmission error can be compensated with a simple configuration, and unstable behavior of the load due to the angle transmission error can be suppressed.
A robot system according to one aspect includes: a robot arm; a motor that is a drive source of a joint of the robot arm; a speed reducer having an input shaft connected to an output shaft of the motor and an output shaft connected to the joint of the robot arm; an instruction unit that generates a position instruction; and a motor control system that controls an operation of the motor by controlling a current supplied to the motor based on the position command generated by the command unit, the motor control system including: a detection unit that detects an event for detecting a rotational speed of an output shaft of the motor; a speed deviation generating unit that generates a speed command based on the position command and calculates a speed deviation that is a deviation between the speed command and a rotation speed of an output shaft of the motor detected based on the event detected by the detecting unit; an angle transmission error compensation unit that estimates an angle transmission error between a rotation angle of an output shaft of the motor and a rotation angle of an output shaft of the speed reducer, and corrects the speed command, the speed deviation, or a rotation speed of the output shaft of the motor detected based on the event detected by the detection unit, based on the estimated angle transmission error; a current command generation unit that generates a current command based on the speed deviation; and a current control unit that controls a current to be supplied to the motor based on the current command.
According to this configuration, the angle transmission error can be compensated with a simple configuration, and vibration of the robot arm caused by the angle transmission error can be suppressed.
Hereinafter, embodiments will be described with reference to the drawings. The present invention is not limited to the present embodiment. In the following, the same or corresponding elements are denoted by the same reference numerals throughout the drawings, and redundant description thereof will be omitted.
(embodiment mode 1)
Fig. 1 is a diagram schematically showing an example of the configuration of a robot system 100 according to embodiment 1. Fig. 2 is a block diagram schematically showing an example of the configuration of the control system of the robot system 100.
As shown in fig. 1, the robot system 100 is used for industrial use, for example. The robot system 100 includes: a robot 1 having a robot arm; and a robot controller 2 for controlling the operation of the robot 1.
The robot 1 is an industrial robot (articulated robot) of an articulated robot, and has a plurality of joints 10 and a robot arm having a manipulator 14 at a distal end portion. As shown in fig. 2, each joint 10 is provided with a driving unit for driving the joint 10, and the driving unit includes a servomotor (motor) 11, an encoder 12, and a speed reducer 13. The robot 1 is not limited to the articulated robot. In the present embodiment, the robot arm of the robot 1 is configured such that 6 joints are arranged in 1 row.
The encoder (detecting section) 12 detects an event for detecting an actual rotation angle and an actual rotation speed of the output shaft 11a of the servomotor 11. In the present embodiment, the encoder 12 outputs information including the actual rotation angle of the output shaft 11a of the servomotor 11 based on the detected event. Then, an actual rotational speed of the output shaft 11a of the servo motor 11 is calculated by time differentiating an actual rotational angle of the output shaft 11a of the servo motor 11 by an encoding value differentiating unit 37 described in detail later.
The speed reducer 13 includes an input shaft 13a connected to the output shaft 11a of the servomotor 11, and an output shaft 13b connected to the joint 10 (load) of the robot 1. The input shaft 13a may be integrated with the output shaft 11a of the servomotor 11. The reduction gear 13 may be constituted by one device or a plurality of devices. The speed reducer 13 reduces the rotation of the output shaft 11a of the servomotor 11 input to the input shaft 13a at a predetermined reduction gear ratio R, and outputs the reduced rotation from the output shaft 13 b. The speed reducer 13 is, for example, a ripple gear device (hammermitaco drive (registered trademark)). But is not limited thereto.
A wave gear device is provided with a rigid gear, a flexible gear, and a wave generator. The rigid gear is a rigid internal gear, e.g. provided integrally with the housing. The flexible gear is an external gear having flexibility and is engaged with the rigid gear. The flexible gear has fewer teeth than the rigid gear and is connected to the output shaft 13 b. The wave generator is an elliptical cam that contacts the inside of the flexible gear, and is connected to the input shaft 13 a. Then, by rotating the input shaft 13a, the wave generator moves the meshing position of the flexible gear and the rigid gear, and the flexible gear rotates around the rotation shaft in accordance with the difference in the number of teeth between the rigid gear and the flexible gear, thereby rotating the output shaft 13 b. The wave gear device has characteristics suitable for a reduction gear of a drive mechanism of a robot due to characteristics such as small size and light weight, high reduction ratio, high torque capacity, no backlash, and the like.
[ example of configuration of robot control device ]
As shown in fig. 2, the robot control device 2 includes a command unit 21, a servo control unit 22 provided corresponding to each joint, and a servo amplifier 23 provided corresponding to each joint. The servo control section 22 and the servo amplifier 23 constitute a motor control system. The motor control system controls the operation of the servo motor 11 by controlling the current supplied to the servo motor 11 based on the position command input from the command unit 21 as the higher-stage device, and controls the operation of the speed reducer 13 in which the input shaft 13a is connected to the output shaft 11a of the servo motor 11 and the output shaft 13b is connected to the load. In the present embodiment, the load is a joint 10 of a robot arm of the robot 1, and the joint 10 is configured to be able to move a working end (an end portion where a manipulator is provided) of the robot arm by rotating the joint.
The command unit 21 generates and outputs a position command based on the operation program. The outputted position command is inputted to the servo control unit 22. In the present embodiment, the position command is a control amount for controlling the position of the output shaft 11a of the servomotor 11, and is a rotation angle of the output shaft 11a of the servomotor 11.
Fig. 3 is a block diagram schematically showing an example of the configuration of a control system of a servo control unit of the robot system. In addition, gravity compensation and dynamic compensation can be performed on the position command.
The servo control unit 22 generates a current command based on the position command generated by the command unit 21. As shown in fig. 3, the servo control unit 22 includes a velocity deviation generating unit 31, an angle transfer error compensating unit 34, and a current command generating unit 36.
The speed deviation generating unit 31, the angle transmission error compensating unit 34, and the current command generating unit 36 are functional blocks realized by executing a predetermined control program by a not-shown arithmetic unit. The arithmetic unit is configured by an arithmetic unit such as a microcontroller, a CPU, an ASIC, and a Programmable Logic Device (PLD) such as an FPGA. The arithmetic unit may be constituted by a single controller that performs centralized control, or may be constituted by a plurality of controllers that cooperate with each other to perform distributed control. The robot controller 2 includes a storage device (not shown) that stores various programs and data.
The speed deviation generating unit 31 generates a speed command based on the position command, and calculates a speed deviation, which is a deviation between the speed command and the actual rotational speed of the output shaft 11a of the servomotor 11 detected based on the event detected by the encoder 12.
The velocity deviation generating unit 31 includes a velocity feedforward command generating unit 32, a positional deviation calculating unit 41, a gain velocity command generating unit 42, a velocity deviation calculating unit 60, and a code value differentiating unit 37.
The speed feedforward command generating unit 32 generates a speed feedforward command based on the position command. The speed feedforward command generating unit 32 includes a differentiator that time-differentiates the position command, and generates the speed feedforward command by time-differentiating the position command.
The positional deviation calculation unit 41 subtracts the actual rotation angle of the output shaft 11a of the servomotor 11 from the position command, and calculates a positional deviation, which is a deviation between the position command and the actual rotation angle of the output shaft 11a of the servomotor 11 output from the encoder 12.
The gain speed command generation unit 42 calculates a value obtained by multiplying the positional deviation calculated by the positional deviation calculation unit 41 by the position proportional gain Kp. The calculated value constitutes a gain speed command. In this way, the gain speed command generating unit 42 performs P control (proportional control).
The encoder value differentiating unit 37 calculates the actual rotation speed of the output shaft 11a of the servo motor 11 based on the actual rotation angle of the output shaft 11a of the servo motor 11 output from the encoder 12. The code value differentiating section 37 includes a differentiator that time-differentiates the actual rotation angle of the output shaft 11a of the servo motor 11, and calculates the actual rotation speed of the output shaft 11a of the servo motor 11 by time-differentiating the actual rotation angle of the output shaft 11a of the servo motor 11.
The speed deviation calculation unit 60 generates a speed command by adding a correction speed feedforward command (details will be described later) obtained by correcting the speed feedforward command to the gain speed command. The speed deviation calculation unit 60 calculates a speed deviation, which is a deviation between the speed command and the rotational speed of the output shaft 11a of the servomotor 11 calculated by the code value differentiation unit 37. The order of addition and subtraction of the correction speed feedforward command, the gain speed command, and the rotation speed of the output shaft 11a of the servo motor 11 in the speed deviation calculation unit 60 is not limited to this.
The angle transmission error compensation unit 34 estimates an angle transmission error between a rotation angle of the output shaft 11a of the servo motor 11 (a rotation angle of the input shaft 13a of the reduction gear 13) and a rotation angle of the output shaft 13b of the reduction gear 13. The angle transfer error compensation unit 34 corrects the speed feedforward command based on the estimated angle transfer error, and generates a corrected speed feedforward command. The angle transfer error compensation unit 34 includes an angle transfer error estimation unit 51 and a correction unit 52.
However, as shown in fig. 4, in a reduction gear including a wave gear device, an angle transmission error, which is a difference between a theoretical output rotation angle obtained by multiplying an input rotation angle input to the reduction gear by a reduction gear ratio, and an actual output rotation angle, is generated due to a machining error or the like. The angle transmission error periodically changes with the rotation of the output shaft of the motor. The angular transfer error ATE of the output shaft of the speed reducer can be approximately expressed by a model using a function according to the following equation (1).
[ mathematical formula 1]
ATE＝A sin(fθ+φ)…(1)
Wherein the content of the first and second substances,
a is the amplitude of the angular transfer error model function,
f is the frequency of the angular transfer error model function (the number of waves of angular transfer error per rotation of the output shaft of the motor),
theta is a rotation angle of an output shaft of the servomotor (input shaft of the reducer),
is the phase of the angular transfer error model function.
The angle transmission error estimating unit 51 estimates an angle transmission error between the rotation angle of the output shaft 11a of the servo motor 11, which is the input rotation angle of the speed reducer 13, and the rotation angle of the output shaft 13b of the speed reducer 13, which is the output rotation angle of the speed reducer 13, based on a periodic function for modeling the periodic variation of the angle transmission error according to the above equation (1), and determines a compensation amount θ (·) comp to be applied to the output shaft 11a of the servo motor 11 to compensate the angle transmission error (to cancel the angle transmission error). Here θ (-) is a symbol with a mark over θ. In the present embodiment, the angle transmission error estimating unit 51 calculates a compensation amount θ (-) comp to be applied to the output shaft 11a of the servomotor 11 based on the following formula (2).
[ mathematical formula 2]
Wherein the content of the first and second substances,
a is the amplitude of the angular transfer error model function,
f is the frequency of the angular transfer error model function (the number of waves of angular transfer error per rotation of the output shaft of the motor),
theta is a value of the position command,
r is the reduction ratio.
The formula (2) is obtained by differentiating the formula (1) with time and multiplying the result by a minus sign at the reduction ratio, and the amplitude a and the phase difference of the formula (1) and the formula (2)Is predefined. For example, it was found that a component having a frequency of 2 has a particularly large influence on the angle transmission error of the wave gear device. Therefore, when the speed reducer 13 is a wave gear device, the frequency f may be set to 2, and a separately determined amplitude a and phase corresponding to the frequency f may be used
The correction unit 52 adds the compensation amount θ (·) comp to the velocity feedforward command value to generate a corrected velocity feedforward command.
In this way, the angle transfer error compensation unit 34 corrects the speed feedforward command value based on the angle transfer error estimated by the angle transfer error estimation unit 51, and generates a corrected speed feedforward command. Then, as described above, the speed deviation calculation unit 60 generates the speed command by adding the correction speed feedforward command to the gain speed command.
In this way, the angle transmission error compensation unit 34 indirectly corrects the speed command and indirectly corrects the correction deviation by correcting the speed feedforward command.
The current command generating unit 36 generates a current command based on the speed deviation generated by the speed deviation generating unit 31. The current command is a control amount for controlling the current supplied to the windings of the servomotor 11.
For example, the current command generation unit 36 includes a speed proportional gain unit 62, an integration unit 63, a speed integral gain unit 64, and an addition/subtraction unit 65. The speed proportional gain unit 62 calculates the value of the 1 st command obtained by multiplying the speed deviation calculated by the speed deviation calculation unit 60 by the speed proportional gain Kvp. The integrating section 63 integrates the value of the 1 st instruction. The speed integration gain unit 64 calculates the value of the 2 nd command obtained by multiplying the value integrated by the integration unit 63 by the speed integration gain Kvi. The addition/subtraction unit 65 calculates a value obtained by adding the value of the 1 st command calculated by the speed proportional gain unit 62 and the value of the 2 nd command calculated by the speed integral gain unit 64, and outputs the value as a current command. That is, the current command generating unit 36 is configured to perform PI control (proportional-integral control). The outputted current command is inputted to the servo amplifier 23.
The servo amplifier (current control unit) 23 controls the current supplied to the servo motor 11 based on the current command generated by the current command generating unit 36.
However, if the speed command is generated after the correction for compensating the angle transmission error for the position command, the correction is negated in the process of calculating the speed deviation of the next stage, and the angle transmission error cannot be appropriately compensated in some cases. However, in the present embodiment, the angle transmission error compensation unit 34 estimates the angle transmission error between the rotation angle of the output shaft 11a of the servo motor 11 and the rotation angle of the output shaft 13b of the reduction gear 13, and corrects the velocity command by correcting the velocity feed-forward command, which is a command at the next stage from the position command, thereby correcting the velocity deviation.
Thus, the structure is as follows: the angle transmission error compensation unit 34 corrects a speed feedforward command for controlling the rotational speed of the output shaft 11a of the servo motor 11, and controls the rotational speed of the output shaft 11a of the servo motor 11 based on the corrected speed feedforward command. That is, the robot controller 2 controls: the output shaft 11a of the servomotor 11 is rotated at a speed that compensates for the angle transmission error.
As described above, in the robot system 100, the angle transmission error compensation unit 34 corrects the speed feedforward command by correcting the speed feedforward command, thereby correcting the speed command and correcting the speed deviation, so that the angle transmission error can be compensated with a simple configuration, and unstable behavior (vibration or the like) of the load due to the angle transmission error can be suppressed.
In particular, in the robot 1 which is a vertical articulated robot, the variation in the rotational speed of the joints due to the angle transmission error appears as vibration of the hand 14. Therefore, by suppressing the fluctuation in the rotational speed of the joint due to the angle transmission error, the vibration of the manipulator 14 due to the angle transmission error can be suppressed, and the positioning accuracy can be improved.
Further, since the angle transmission error compensation unit 34 corrects the velocity command by correcting the velocity feedforward command to correct the velocity deviation, the velocity command can be appropriately corrected and the angle transmission error can be appropriately compensated in the robot system 100 that controls the servo motor 11 using the velocity feedforward command.
(embodiment mode 2)
The following describes the configuration and operation of embodiment 2, with respect to differences from embodiment 1.
Fig. 5 is a graph showing a relationship between a speed feedforward command and a correction value of the speed feedforward command in an operation example of the robot system according to embodiment 2.
In embodiment 1 described above, the angle transmission error estimating unit 51 calculates the compensation amount θ (·) comp to be applied to the output shaft 11a of the servo motor 11 using the value of the speed feedforward command as in equation (2).
In contrast, in the present embodiment, the angle transfer error estimator 51 first corrects the value of the velocity feedforward command θ (-) based on the expressions (3) to (5) to calculate θ (-) a.
[ mathematical formula 3]
Wherein the content of the first and second substances,
k is a coefficient that is specified by a number,
vlim is an upper limit value of the speed feedforward command value.
Fig. 5 is a graph showing equations (3) to (5), and when the speed feedforward command value θ (-) is equal to or less than vlim, the speed feedforward command value θ (-) is set as the correction value θ (-) a of the speed feedforward command value, as it is, based on equation (3).
When the speed feedforward command value θ (-) exceeds vlim, it is set based on equation (4) that the correction value θ (-) a of the speed feedforward command value becomes smaller as the speed feedforward command value θ (-) becomes larger.
In addition, if vlim exceeds the value of the correction value θ (-) a of the velocity feedforward command to 0 in equation (4), the correction value θ (-) a of the velocity feedforward command is set to 0 based on equation (5).
Next, the angle transmission error estimator 51 calculates a compensation amount θ (-) comp to be applied to the output shaft 11a of the servomotor 11, using the correction value θ (-) a of the velocity feedforward command value, based on the formula (6).
[ mathematical formula 4]
Wherein the content of the first and second substances,
a is the amplitude of the angular transfer error model function,
f is the frequency of the angular transfer error model function (the number of waves of angular transfer error per rotation of the output shaft of the motor),
theta is a value of the position command,
r is the reduction ratio.
That is, when the speed feedforward command value θ (-) is equal to or less than vlim, the compensation amount θ (-) comp in the present embodiment is configured to be the same as the compensation amount θ (-) comp in embodiment 1 described above.
On the other hand, when the velocity feedforward command value θ (-) exceeds vlim, the compensation amount θ (-) comp in the present embodiment is configured to be smaller than the compensation amount θ (-) comp in embodiment 1 described above, and particularly, as shown in equation (5), the compensation amount θ (-) comp is configured to be 0 when the velocity feedforward command value θ (-) becomes a predetermined value or more.
In this way, when the speed feedforward command value θ (-) exceeds a predetermined value, the compensation amount θ (-) comp is reduced or not compensated. This can prevent the compensation amount θ (-) comp from becoming excessively large, and can prevent a problem from occurring in the control. Further, it is possible to simplify control in a region where high-speed operation is not generally required with high accuracy.
In the present embodiment, the speed feedforward command value θ (-) is set so that the correction value θ (-) a of the speed feedforward command value becomes smaller as the speed feedforward command value θ (-) becomes larger within the range of the speed feedforward command value shown in equation (4), and therefore, when the speed feedforward command value θ (-) changes over vlim, it is possible to prevent the operation of the robot 1 from changing abruptly.
(embodiment mode 3)
The following describes the configuration and operation of embodiment 3, with respect to differences from embodiment 1.
Fig. 6 is a block diagram schematically showing an example of the configuration of the control system of the servo control unit 22 of the robot system according to embodiment 3.
In the present embodiment, the servo control unit 22 further includes a gravity compensation unit 337.
The gravity compensation unit 337 is a functional unit for performing compensation to eliminate the influence of gravity acting on the robot 1. The gravity compensation unit 337 includes a gravity compensation value calculation unit 341 and a correction unit 342. The gravity compensation value calculation unit 341 calculates a gravity compensation value G for canceling out the gravity torque acting on the joint 10 of the robot 1. The correction unit 342 adds the gravity compensation value G to the current command value.
However, the phase difference between the output shaft 11a of the servomotor 11 and the output shaft 13b of the reduction gear 13 changes in proportion to the torque applied to the output shaft 13b of the reduction gear 13. Therefore, the change can be approximately expressed by a model expressed as a function below.
[ math figure 5]
Wherein the content of the first and second substances,
a is a gravitational torque phase proportionality constant,
g is the torque of the force of gravity,
In the present embodiment, the angle transmission error estimating unit 51 calculates the compensation amount θ (·) comp to be applied to the output shaft 11a of the servomotor 11 based on the formula (2) using the gravity compensation value G calculated by the gravity compensation value calculating unit 341 based on the formula (7) described above and based on the formula (7)
(embodiment mode 4)
The following describes the configuration and operation of embodiment 4, with respect to differences from embodiment 1.
Fig. 7 is a block diagram schematically showing an example of the configuration of a control system of a servo control unit of the robot system according to embodiment 4.
In embodiment 1 described above, the angle transmission error compensation unit 34 indirectly corrects the speed command by correcting the speed feedforward command, thereby indirectly correcting the speed deviation. In contrast, in the present embodiment, as shown in fig. 7, the angle transfer error compensation unit 434 corrects the speed deviation.
That is, the speed deviation calculation unit 60 generates the speed command by adding the speed feedforward command and the gain speed command. The speed deviation calculation unit 60 calculates a speed deviation, which is a deviation between the speed command and the rotation speed of the output shaft 11a of the servomotor 11.
The correction unit 452 of the angle transmission error compensation unit 434 adds the compensation amount θ (-) comp to the velocity deviation to calculate the corrected velocity deviation.
The current command generating unit 36 generates a current command based on the corrected speed deviation generated by the angle transfer error compensating unit 434. That is, the velocity proportional gain unit 62 generates the 1 st command obtained by multiplying the corrected deviation calculated by the correction unit 452 of the angle transmission error compensation unit 434 by the velocity proportional gain Kvp.
(embodiment 5)
The following describes the structure and operation of embodiment 5, with respect to differences from embodiment 1.
Fig. 8 is a block diagram schematically showing an example of the configuration of a control system of a servo control unit of the robot system according to embodiment 5.
In embodiment 1 described above, the angle transmission error compensation unit 34 indirectly corrects the speed command by correcting the speed feedforward command, and indirectly corrects the speed deviation. In contrast, in the present embodiment, as shown in fig. 8, the angle transmission error compensation unit 534 indirectly corrects the velocity command by correcting the gain velocity command, and indirectly corrects the velocity deviation.
That is, the correction unit 552 of the angle transmission error compensation unit 534 adds the compensation amount θ (-) comp to the gain speed command to calculate the corrected gain speed command. Then, the speed deviation calculation unit 60 calculates a speed command by adding the speed feedforward command and the correction gain speed command. The speed deviation calculation unit 60 calculates a speed deviation, which is a deviation between the speed command and the rotation speed of the output shaft 11a of the servomotor 11.
(embodiment mode 6)
The following describes the configuration and operation of embodiment 6, with respect to differences from embodiment 1.
Fig. 9 is a block diagram schematically showing an example of the configuration of a control system of a servo control unit of the robot system according to embodiment 6.
In embodiment 1 described above, the angle transmission error compensation unit 34 indirectly corrects the speed command by correcting the speed feedforward command, and indirectly corrects the speed deviation. In contrast, in the present embodiment, as shown in fig. 9, the angular transfer error compensation unit 634 indirectly corrects the speed deviation by correcting the rotational speed of the output shaft 11a of the servomotor 11.
That is, the correction unit 652 of the angle transmission error compensation unit 634 calculates a corrected rotational speed by subtracting the compensation amount θ (-) comp from the rotational speed of the output shaft 11a of the servomotor 11.
The speed deviation calculation unit 60 calculates a speed deviation, which is a deviation between the speed command and the corrected rotation speed.
(embodiment 7)
The following describes the configuration and operation of embodiment 7, with respect to differences from embodiment 1.
Fig. 10 is a block diagram schematically showing an example of the configuration of a control system of a servo control unit of a robot system according to embodiment 7.
In embodiment 1 described above, the angle transmission error compensation unit 34 indirectly corrects the speed command by correcting the speed feedforward command, and indirectly corrects the speed deviation. In contrast, in the present embodiment, as shown in fig. 10, the angle transmission error compensation unit 734 corrects the position command and the speed feed-forward command based on the estimated angle transmission error. The angle transfer error compensation unit 734 includes an angle transfer error estimation unit 51, a 1 st correction unit 752, and a 2 nd correction unit 753.
The 1 st correcting unit 752 calculates a correction position command by adding the compensation amount θ (·) comp to the position command.
The 2 nd correcting unit 753 is the same as the correcting unit 52, and therefore, a detailed description thereof will be omitted.
The speed feedforward command generating unit 32 generates a speed feedforward command based on the position command before correction.
The positional deviation calculation unit 41 subtracts the rotation angle of the output shaft 11a of the servomotor 11 from the corrected position command, thereby calculating a positional deviation, which is a deviation between the position command and the rotation angle of the output shaft 11a of the servomotor 11.
(embodiment mode 8)
The following describes the configuration and operation of embodiment 8, with respect to differences from embodiment 1.
In embodiment 1 described above, when calculating the compensation amount θ (-) comp to be applied to the output shaft 11a of the servomotor 11 based on the formula (2), the angle transmission error estimating unit 51 of the angle transmission error compensating unit 34 calculates the compensation amount θ (-) comp using the predetermined frequency f that has a particularly large influence on the angle transmission error, the predetermined amplitude a corresponding to the frequency, and the predetermined phase difference Φ. Instead, the angle transmission error estimating unit 51 of the angle transmission error compensating unit 34 may calculate the compensation amount θ (·) comp to be applied to the output shaft 11a of the servomotor 11 based on the following equation (8).
[ mathematical formula 6]
Wherein the content of the first and second substances,
i is the pattern of the frequency and,
Aiis the amplitude of the angular transfer error model function corresponding to the mode of the frequency,
fiis the frequency of the angular transfer error model function corresponding to the pattern of frequencies (the number of waves of angular transfer error per rotation of the motor),
theta is a value of the position command,
r is the reduction ratio.
That is, the angle transmission error estimating unit 51 calculates the angle transmission error for each frequency mode based on the values of the position command and the speed feedforward command (time differential value of the position command), and uses the combined value for correction of the angle transmission error. This makes it possible to correct the angle transmission error more accurately.
Many modifications and other embodiments of the invention will come to mind to one skilled in the art to which this invention pertains having the benefit of the teachings presented in the foregoing descriptions. Therefore, the foregoing description is to be construed as illustrative only and is for the purpose of teaching those skilled in the art the best mode of carrying out the invention. The details of the structure and/or function may be substantially changed without departing from the spirit of the present invention.
Description of the reference numerals
1 … robot; 2 … robot control device; 10 … rotating shaft; 11 … servomotor; 11a … output shaft; 12 … encoder; 13 … speed reducer; 13a … input shaft; 13b … output shaft; 22 … servo control part; 23 … servo amplifier; 31 … a speed deviation calculating part; 34 … angle transfer error compensation part; 36 … current command generating part; 100 … robotic system.
- 上一篇：一种医用注射器针头装配设备
- 下一篇：用于封装光伏屋顶瓦片的系统和方法