JP7529599B2 - robot - Google Patents

Description

This disclosure relates to a robot having an arm and a load body that is displaceable relative to the arm.

A wire-driven robot hand is known (Patent Document 1). This robot hand has three links (loads) from the base of the drive system to the tip of the hand, and each link is connected to the adjacent link by a joint. The base (drive section) is provided with three drive actuators equipped with motors for driving three wires wound around pulleys fixed to each link. Each drive actuator is controlled by a corresponding motor controller. Each motor controller is connected to a collision monitoring device, which is a higher-level controller, and operates according to commands from the collision monitoring device. Signals from contact sensors attached to the surfaces of each link, signals from torque sensors that monitor the torque of each link, and signals from encoders attached to each motor are transmitted to the collision monitoring device. The collision monitoring device monitors the collision state of each link based on these signals, and changes the limit torque of each joint by controlling a solenoid that can change the wire spacing to adjust the tension of the wire depending on the collision state.

Japanese Patent Application Publication No. 10-249777

However, in conventional robot hands, the tension of the wire was controlled by a collision monitoring device, which was a higher-level controller, which required a long processing time to control the tension of the motor, and the movement of the load (link) was not smooth.

In view of this background, the present invention aims to provide a robot that can smoothly move a load.

In order to solve such problems, one embodiment of the present invention provides a robot (1) having an arm (31) and a load (32) displaceably provided with respect to the arm, the robot (1) comprising: a drive source (23) provided on the arm; a power transmission system (33) including a wire (36) for transmitting power generated by the drive source to the load; a load position sensor (44) for acquiring a position (θ _L ) and a velocity (ω _L ) of the load; a tension sensor (39) for acquiring a tension (Fa ₍ Ta)) of the wire; and a controller (13, 25) for controlling an output (ω _M ) of the drive source so as to realize a given target position (θ _{L t} ) of the load, the controller comprising: a first control device (13) for setting a target output (Tt) to be applied to the load based on the target position (θ L t) and the position (θ _{L )} of the load; and a second control device (25) that controls the output of the drive source based on a position of the load body directly obtained from the load position sensor and calculates the velocity based on the position.

According to this configuration, the second control device, which controls the output of the drive source based on the target output, obtains the position of the load directly from the load position sensor, calculates the speed based on the position, and controls the output of the drive source based on the tension of the wire and the speed of the load. Therefore, the second control device can calculate the speed of the load in a short time and bring the output of the drive source closer to the target output, making the operation of the load smoother.

Preferably, the second control device includes a force control unit (53) that sets a target additional speed (ωadd) of the driving source based on the target output (Tt) and the tension of the wire (Fa(Ta)), and sets a target speed ( _ωMt ) of the driving source based on the target additional speed and a driven driving source speed (ωf) obtained by converting the speed ( _ωL ) of the load, and a control unit (54) that drives the driving source based on the target speed.

According to this configuration, the second control device controls the output of the drive source using the target speed of the drive source as the control medium, based on the target output set by the first control device and the tension of the wire. This allows the control device to achieve conventional passive compliance control that provides compliance to the load, even if the power transmission system includes a wire that is a flexible element.

Preferably, the arm comprises an arm base (9) and a hand portion (14) that is connected to the arm base via a joint (12) and supports the load body, the wire is arranged to pass through the joint, and the second control device (25) and the tension sensor (39) are provided at the arm base.

With this configuration, the second control device and tension sensor are both provided at the base of the arm, thereby reducing the time required for the force control process by the second control device. This makes the movement of the load body smoother. In addition, it is possible to avoid an increase in the size of the hand unit due to the implementation of devices and sensors.

Preferably, the device further includes a base (2) that supports the arm, the first control device is provided on the base, and the second control device is provided on the arm.

This configuration makes it possible to avoid the arm becoming larger due to the implementation of the first control device.

In this way, the present invention can provide a robot that can smoothly move a load.

1 is a schematic diagram of a robot according to an embodiment of the present invention; Schematic diagram of the robot's hand Model diagram of the first actuator in the hand Model diagram of the second actuator in the hand System configuration diagram of the hand actuator Schematic functional block diagram of the actuator in the hand A functional block diagram of a main part of a controller for a first actuator. A functional block diagram of a main part of a controller for a second actuator. (A) Conventional technology; (B) The present invention. 1 is a Bode diagram showing the effect of responsiveness due to the control according to the embodiment; Time chart showing the response of the second actuator 1 is a time chart showing a response by a control system according to an embodiment; FIG. 13 is a model diagram of a first actuator according to another embodiment. FIG. 13 is a model diagram of a second actuator according to another embodiment.

The following describes in detail an embodiment of the present invention with reference to the drawings.

Figure 1 is a schematic diagram of a robot 1 according to an embodiment. As shown in Figure 1, the robot 1 is a humanoid robot. The robot 1 comprises a base 2, a head 3 arranged above the base 2, left and right arms 4 extending from the upper part of the base 2, hands 5 provided at the tips of the arms 4, left and right legs 6 extending from the lower part of the base 2, and feet 7 provided at the tips of the legs 6. In the following description, the front-to-rear direction of the robot 1 is referred to as the X-axis, the left-to-right direction as the Y-axis, and the up-to-down direction as the Z-axis.

The base 2 is composed of an upper and lower part that are connected vertically so that they can rotate relatively around the Z axis. The head 3 can move, such as by rotating around the Z axis, relative to the base 2.

The arm 4 includes an upper arm link 8 and a forearm link 9. The upper arm link 8 is connected to the base 2 via a shoulder joint 10, the upper arm link 8 and the forearm link 9 are connected to the elbow joint 11, and the forearm link 9 is connected to the hand 5 via a wrist joint 12. The shoulder joint 10 has degrees of freedom of rotation about the X-axis, Y-axis, and Z-axis, the elbow joint 11 has degrees of freedom of rotation about the Y-axis, and the wrist joint 12 has degrees of freedom of rotation about the X-axis, Y-axis, and Z-axis. The base 2 is provided with a first control device 13 that controls the overall operation of the robot 1.

As shown in FIG. 2, the hand 5 includes a palm 14 and a plurality of finger sections 15 extending from the palm 14. Each of the finger sections 15 includes a first finger link 16, a second finger link 17, and a third finger link 18. The palm 14 and the first finger link 16 are connected via a first finger joint 19, the first finger link 16 and the second finger link 17 are connected via a second finger joint 20, and the second finger link 17 and the third finger link 18 are connected via a third finger joint 21. The first finger joint 19 to the third finger joint 21 have a degree of freedom of rotation about the Y axis. The first finger joint 19 of the finger section 15 corresponding to the thumb further has a degree of freedom of rotation about the X axis. The forearm link 9, the palm 14, the first finger link 16 to the third finger link 18, the carpal joint 12, and the first finger joint 19 to the third finger joint 21 are covered by an outer case 22.

The forearm link 9 is provided with a plurality of motors 23 (driving sources) for driving the joints of each finger, and a second control device 25 for controlling the operation of these motors 23. In this embodiment, the third finger joint 21 is configured to be linked with the second finger joint 20, and two motors 23 are used for each finger portion 15: a motor 23 for driving the first finger joint 19, and a motor 23 for driving the second finger joint 20 and the third finger joint 21. The second control device 25 drives the motors 23 of all finger portions 15, and is common to the plurality of actuators 30 (30A, 30B).

The second control device 25 receives commands from the first control device 13 (Figure 1) mounted on the base 2, and drives all of the fingers 15 of the hand 5 by controlling the operation of the motor 23 based on the commands. The motor 23 operates with power supplied from a battery (not shown) mounted on the robot 1. Each of the fingers 15 constitutes an actuator 30. In other words, the portion from the upper arm link 8 to the palm 14 forms an arm 31 supported on the base 2 via the shoulder joint 10, and the palm 14 is connected to the forearm link 9, which forms the base of the arm 31, via the carpal joint 12, forming a hand that supports the fingers 15.

In this embodiment, the finger portion 15 has a plurality of joints (19 to 21) driven by a plurality of motors 23. In other words, it can be said that the finger portion 15 in this embodiment includes a plurality of actuators 30. Hereinafter, the actuator 30 that drives the first finger joint 19 is referred to as the first actuator 30A (see FIG. 3), and the actuator 30 that drives the second finger joint 20 is referred to as the second actuator 30B (see FIG. 4). The first actuator 30A drives the entire finger portion 15 to rotate around the Y axis relative to the palm portion 14. The second actuator 30B drives the second finger link 17 and the third finger link 18 to rotate around the Y axis relative to the first finger link 16. Hereinafter, the finger portion 15, which is the driving target in the first actuator 30A, and the second finger link 17 and the third finger link 18 in the second actuator 30B are simply referred to as the load body 32 (see FIG. 3 and FIG. 4). Each load body 32 is provided so as to be displaceable with respect to the arm 31 directly or indirectly via another load body 32. In other embodiments, the fingers may be driven by only one motor 23.

Figure 3 is a model diagram of the first actuator 30A of the hand section 5. As shown in Figure 3, the first actuator 30A includes a motor 23 provided on the forearm link 9, a load 32 driven by the motor 23, and a power transmission system 33 that transmits the power generated by the motor 23 to the load 32. The power transmission system 33 includes a drive pulley 34 that is driven to rotate by the motor 23, a driven pulley 35 that is integrally formed with the load 32 around the rotation axis of the load 32, and a wire 36 wound around the drive pulley 34 and the driven pulley 35. An idler pulley 37 and a tensioner 38 are provided in appropriate positions on the wire 36.

The wire 36 is an endless loop and transmits torque in both rotational directions of the drive pulley 34, i.e., bending torque Tb in the direction to bend the load body 32 and extension torque Te in the direction to extend the load body 32, to the driven pulley 35. In other words, the wire 36 is arranged so that it can drive the load body 32 in both directions, and connects the drive pulley 34 and the driven pulley 35 so that the bending torque Tb and extension torque Te can be transmitted.

In this embodiment, two tensioners 38 are provided at the portion of the wire 36 that transmits the bending side tension Fb and the portion that transmits the extension side tension Fe. Between the driven pulley 35 on the wire 36 and the two tensioners 38, tension sensors 39 (39A, 39B) are provided to acquire the tension F of the wire 36. One of the tension sensors 39 is a first tension sensor 39A that detects the bending side tension Fb of the wire 36, and the other is a second tension sensor 39B that detects the extension side tension Fe of the wire 36. The tension sensors 39 (39A, 39B) are provided on the forearm link 9 (Figure 2). The portion of the wire 36 between the tension sensor 39 and the driven pulley 35 passes through the carpal joint 12 (Figure 2). An outer tube (not shown) is provided at the portion of the wire 36 that passes through the carpal joint 12.

A reducer 40 having a predetermined reduction ratio RR is provided between the output shaft of the motor 23 and the drive pulley 34. Here, the radius of the drive pulley 34 and the radius of the driven pulley 35 are the same, and the drive pulley 34 and the driven pulley 35 rotate at the same speed. In other embodiments, a reduction mechanism based on the radius ratio of the drive pulley 34 and the driven pulley 35 may be added. The power transmission system 33 as a whole has a flexibility coefficient (1/Kspr). The flexibility coefficient is the reciprocal of the spring stiffness Kspr (spring constant) of the power transmission system 33.

Figure 4 is a model diagram of the second actuator 30B of the hand portion 5. As shown in Figure 4, the second actuator 30B includes a motor 23 provided on the forearm link 9, a load 32 driven by the motor 23, and a power transmission system 33 that transmits the power generated by the motor 23 to the load 32. The power transmission system 33 includes a drive pulley 34 that is driven to rotate by the motor 23, a driven pulley 35 that is integrally formed with the load 32 around the rotation axis of the load 32, and a wire 36 wound around the drive pulley 34 and the driven pulley 35. The wire 36 connects the drive pulley 34 and the driven pulley 35 so as to transmit the bending side tension Fb.

The wire 36 extends from one end fixed to the drive pulley 34 and is wound around the driven pulley 35, and the other end is fixed to the palm portion 14 (hand portion, FIG. 2). A biasing member 42 that constantly biases the wire 36 toward the palm portion 14 is provided on the other end side of the wire 36 relative to the driven pulley 35. The biasing member 42 may be, for example, a tension coil spring or a helical spring. When the wire 36 is pulled by the drive pulley 34, the bending side tension Fb transmits torque to the driven pulley 35 in a direction that bends the load body 32. When the wire 36 is biased by the biasing member 42 in a direction opposite to the direction in which the bending side tension Fb from the drive pulley 34 acts, the biasing force transmits torque to the driven pulley 35 in a direction that extends the load body 32.

In this embodiment, a tension sensor 39 for acquiring the tension F of the wire 36, i.e., a first tension sensor 39A for detecting the bending side tension Fb of the wire 36, is provided between the driving pulley 34 and the driven pulley 35 on the wire 36. The first tension sensor 39A is provided on the forearm link 9 (Figure 2). The portion of the wire 36 between the first tension sensor 39A and the driven pulley 35 passes through the carpal joint 12 (Figure 2). An outer tube (not shown) is provided at the portion of the wire 36 that passes through the carpal joint 12.

A reducer 40 having a predetermined reduction ratio RR is provided between the motor 23 and the drive pulley 34. Here, the radius of the drive pulley 34 and the radius of the driven pulley 35 are set to be the same. The output shaft of the motor 23 is directly connected (rigidly connected) to the drive pulley 34. The power transmission system 33 includes the wire 36, and thus has a flexibility coefficient 1/Kspr, which is the reciprocal of the wire stiffness.

Figure 5 is a system configuration diagram of the actuator 30 of the hand 5. As shown in Figure 5, the actuator system of the hand 5 provided in the robot 1 includes a first control device 13, a second control device 25, a plurality of motors 23, a plurality of motor angle sensors 43, a plurality of tension sensors 39, and a plurality of joint angle sensors 44. The second control device 25 is connected to the first control device 13 via a communication line 45. The plurality of motors 23, the motor angle sensors 43, and the joint angle sensors 44 are connected to the second control device 25 via the communication line 45.

The first control device 13 is an electronic control device composed of a CPU, a ROM, a RAM, an I/O, an analog circuit, etc. The first control device 13 executes various motion controls by executing arithmetic processing according to a program with the CPU. The first control device 13 may be configured as a single piece of hardware, or may be configured as a unit consisting of multiple pieces of hardware. A "motion control program" for causing the first control device 13 to function as a control device for the robot 1 may be stored in a storage device such as a ROM in advance. Alternatively, this program may be distributed from a server via a network or broadcast at any time and stored in the storage device of the first control device 13, or may be used by the first control device 13 via a network or broadcast in a state saved on the server. The first control device 13 executes arithmetic processing according to the motion control program to set the motion target value of the load body 32 of each actuator 30, and set the target torque Tt of the load body 32 based on the motion target value.

The second control device 25 is an electronic control device composed of a programmable logic device, a motor driver, an I/O, an analog circuit, etc. The programmable logic device controls the operation of the actuator 30 by executing arithmetic processing according to a program, and may be, for example, an FPGA (field-programmable gate array). An "operation control program" for causing the second control device 25 to function as a control device for the actuator 30 is stored in advance in the programmable logic device. The second control device 25 operates the actuator 30 by controlling the output of the motor 23 according to a command for the target torque Tt of the load 32 received from the first control device 13. That is, the second control device 25 cooperates with the first control device 13 to control the output of the motor 23 so as to realize a given target position of the load 32.

The motor angle sensor 43 is an angle sensor that detects the motor angle _θM to obtain the motor angle _θM (position) and motor angular velocity _ωM (speed), which are the angles of the output shaft of the corresponding motor 23. In other words, the motor angle sensor 43 is a drive position sensor that detects the position of the drive source to obtain the position and speed of the drive source. The motor angle sensor 43 may be, for example, an encoder, and outputs a signal corresponding to the motor angle _θM .

The tension sensor 39 is configured as a first tension sensor 39A (see FIG. 6) that detects the bending side tension Fb of the wire 36, or a second tension sensor 39B (see FIG. 6) that detects the extension side tension Fe of the wire 36. The tension sensor 39 outputs a signal corresponding to the tension F of the wire 36.

The joint angle sensor 44 is an angle sensor that detects a joint angle _θL in order to obtain the angular position of the load 32 driven by the motor 23 relative to the support member, i.e., the joint angle _θL (position) and joint angular velocity _ωL (velocity) of the joint. In other words, the joint angle sensor 44 is a load position sensor that detects the position of the load 32 in order to obtain the position and velocity of the load 32. The joint angle sensor 44 may be, for example, an encoder, and outputs a signal corresponding to the joint angle _θL .

As described above, the first control device 13 is disposed on the base 2 of the robot 1, and the second control device 25 and the multiple motors 23 are disposed on the forearm link 9 of the robot 1. Therefore, the shoulder joint 10 and elbow joint 11 are interposed between the first control device 13 and the second control device 25.

Fig. 6 is a schematic functional block diagram of the actuator 30 of the hand portion 5. As shown in Fig. 6, the first control device 13 has a joint target value setting unit 51 and a target tension setting unit 52. The joint target value setting unit 51 sets a motion target value of the load body 32 by executing calculation processing according to the motion control program. The motion target value of the load body 32 includes a target joint angle θ _L t and a target joint angular velocity ω _L t. The motion target value of the load body 32 may include a joint torque command.

The target tension setting unit 52 receives the target joint angle θ _Lt , the target joint angular velocity ω _Lt , and the joint angle θ _L and joint angular velocity ω _L acquired from the tension sensor 39. Based on the deviation of these input values, the target tension setting unit 52 performs impedance control to set a target torque Tt as a target output of the load 32. In this way, the target tension setting unit 52 sets a target output to be applied to the load 32 based on at least the target joint angle θ _Lt (target position) and the joint angle θ _L (position of the load 32).

The second control device 25 has a tension control unit 53, a motor control unit 54, a current control unit 55, and a sensor data acquisition unit 56. The sensor data acquisition unit 56 is equipped with a differentiator 57. The sensor data acquisition unit 56 acquires signals output from the tension sensors 39 (39A, 39B), the motor angle sensor 43, and the joint angle sensor 44, performs necessary processing, and distributes these signals to each functional unit that requires them. The differentiator 57 calculates the motor angular velocity _ωM by differentiating a signal corresponding to the motor angle _θM acquired from the motor angle sensor 43, and calculates the joint angular velocity _ωL by differentiating a signal corresponding to the joint angle _θL acquired from the joint angle sensor 44.

The sensor data acquisition unit 56 transmits the motor angle _θM to the motor control unit 54, transmits the tension F (Fb, Fe) of the wire 36 and the joint angular velocity _ωL to the tension control unit 53, and transmits the joint angle _θL and the joint angular velocity _ωL to the target tension setting unit 52 of the first control device 13. Note that in the first actuator 30A, both the flexion-side tension Fb and the extension-side tension Fe are transmitted to the tension control unit 53, and in the second actuator 30B, only the flexion-side tension Fb is transmitted to the tension control unit 53.

The tension control unit 53 sets a target motor angular velocity ω Mt based on the target torque Tt set by the target tension setting unit ₅₂ , the tension F (Fb, Fe) transmitted from the sensor data acquisition unit 56 , and the joint angular velocity ω _L.

The motor control unit 54 sets a target current It to be supplied to the motor 23 based on the target motor angular velocity _ωMt set by the tension control unit 53 and the motor angular velocity _ωM transmitted from the sensor data acquisition unit 56. The current control unit 55 controls the current I flowing from the battery to the motor 23 so that the target current It set by the motor control unit 54 is supplied to the motor 23. In this manner, the second control unit 25 controls the motor angular velocity _ωM , which is the output of the drive source. Hereinafter, the first control unit 13 and the second control unit 25 will be collectively referred to as the controller.

Figure 7 is a functional block diagram of the main parts of the controller for the first actuator 30A, and Figure 8 is a functional block diagram of the main parts of the controller for the second actuator 30B. First, the functions of the controller for the first actuator 30A will be described with reference to Figure 7.

The target tension setting unit 52 of the first control device 13 includes a first subtractor 61, a second subtractor 62, an integrator 63, and an adder 64. The first subtractor 61 calculates a joint angle difference ΔθL by subtracting the actual joint angle _θL detected by the joint angle sensor 44 from the target joint angle _θLt set by the joint target value setting unit 51. The second subtractor 62 calculates an angular velocity difference _ΔωL by subtracting the actual joint angular velocity _ωL detected by the joint angle sensor 44 from the target joint angular velocity _ωLt set by the joint target value setting unit 51. The integrator 63 integrates the joint angle _{difference ΔθL} . The target tension setting unit 52 multiplies the joint angle difference Δθ _L by a proportional gain Kp to convert it into a torque value of the load 32, multiplies the joint angular velocity ω _L by a differential gain Kd to convert it into a torque value of the load 32, and multiplies the integral value of the joint angle difference Δθ _L by an integral gain Ki to convert it into a torque value of the load 32. The adder 64 adds these three values to calculate a torque target value T _L t to be applied to the load 32.

The target tension setting unit 52 also includes a driving torque conversion unit 65 and a limiting unit 66. The driving torque conversion unit 65 converts the torque target value T _L t to be applied to the load 32 into torque on the driving side. In this embodiment, there is no speed reduction mechanism between the load 32 and the wire 36, and the torque of the load 32 and the torque of the wire 36 of the power transmission system 33 coincide with each other. Therefore, the driving torque conversion unit 65 outputs the torque target value T _L t as the target torque Tt directly to the limiting unit 66. The limiting unit 66 limits the target torque Tt so that −C1<Tt<C1. Here, C1 is a positive value and is the upper limit of the torque at which the wire 36 does not break in the power transmission system 33. After executing the limiting process, the limiting unit 66 outputs the target torque Tt to the tension control unit 53.

The target tension setting unit 52 can change the characteristics of the first actuator 30A by adjusting the mechanical impedance (spring stiffness Kspr) of the first actuator 30A by changing the proportional gain Kp, the differential gain Kd, and the integral gain Ki. Specifically, even if the power transmission system 33 includes a wire 36, the position responsiveness of the load 32 can be improved by increasing these gains. In addition, by reducing these gains, the flexibility of the load 32 of the hand portion 5 can be increased, and for example, the shock absorption performance can be improved.

The tension control unit 53 of the second control device 25 includes a subtractor 71, a drive side torque conversion unit 72, a subtractor 73, and an adder 74. The subtractor 71 subtracts the extension side tension Fe acquired from the second tension sensor 39B from the bending side tension Fb acquired from the first tension sensor 39A to calculate the actual torque tension Fa of the wire 36 acting as a torque on the driven pulley 35. The drive side torque conversion unit 72 multiplies the actual torque tension Fa calculated by the subtractor 71 by the radius of the drive pulley 34 to calculate the actual torque Ta of the wire 36 part of the power transmission system 33. The subtractor 73 subtracts the actual torque Ta from the target torque Tt set by the target tension setting unit 52 to calculate the torque deviation Terr to be applied to the load body 32.

The tension control unit 53 calculates a target additional angular velocity ωadd, which is an additional velocity command (target additional velocity) of the drive source, by multiplying the torque deviation Terr by a force control gain Ktp. The target additional angular velocity ωadd is input to an adder 74. Here, the force control gain Ktp is obtained by multiplying the flexibility coefficient (1/Kspr) of the power transmission system ₃₃ by a proportional gain Kp2, which is a speed gain for converting an angle, which is a position command, into an angular velocity, which is a speed command. The force control gain Ktp will be described in detail below.

9A and 9B are explanatory diagrams of the control of the conventional technology and the present invention, respectively. In the conventional passive compliance control shown in Patent Document 1, the flexible element intervening in the power transmission system 33 has a linear flexibility characteristic. Therefore, as shown in FIG. 9A, the angle command value θt, which is a position command, is calculated by multiplying the target torque Tt, which is a torque command, by the flexibility coefficient (1/Kspr) of the flexible element. In addition, the angle difference Δθ is calculated by subtracting the angle between the drive side and the load side (angle difference "θ _M - θ _L ") from the angle command value θt. Then, the angular velocity command is calculated by multiplying this angle difference Δθ by a proportional gain Kp ₂ for converting the angle command into an angular velocity command.

In contrast, in this embodiment, since the power transmission system 33 includes the wire 36, the 33 characteristic of the power transmission system 33 becomes nonlinear. Therefore, in the tension control unit 53, the actual torque Ta between the drive side and the load side, which is obtained by multiplying the angle between the drive side and the load side (angle difference "θ _M - θ _L ") by the spring stiffness Kspr of the power transmission system 33, is acquired from the detection value of the tension sensor 39. In addition, the torque deviation Terr is calculated by subtracting the actual torque Ta from the target torque Tt, which is the torque command. Then, the torque deviation Terr is multiplied by the flexibility coefficient (1/Kspr) of the power transmission system 33 to calculate a value equivalent to the conventional angle difference Δθ, and this value is multiplied by the proportional gain _Kp2 to calculate the angular velocity command. In this way, the tension control unit 53 performs similar processing using force (actual torque Ta) rather than position through equivalent exchange, and can calculate the speed command (target angular velocity ωt) without modeling the spring characteristics of the power transmission system 33, which has hysteresis.

Returning to Fig. 7, the description will be continued. The tension control unit 53 further includes a drive-side speed conversion unit 75. The drive-side speed conversion unit 75 converts the joint angular velocity _ωL acquired by the joint angle sensor 44 into an angular velocity on the drive side. Specifically, the drive-side speed conversion unit 75 multiplies the joint angular velocity _ωL by the reduction ratio RR of the reducer 40 to calculate a driven motor angular velocity ωf corresponding to the joint angular velocity _ωL . Here, the driven motor angular velocity ωf is an angular velocity equivalent to a driven drive source velocity obtained by converting the joint angular velocity _ωL of the load 32 that rotates following the drive of the motor 23 into a motor angular velocity _ωM .

The tension control unit 53 multiplies the driven motor angular velocity ωf by a control gain Kvff to optimize the driven motor angular velocity ωf. The control gain Kvff is usually set to 1, and is also set to 1 in this embodiment. The driven motor angular velocity ωf is input to an adder 74 as a feedforward term. The adder 74 adds the driven motor angular velocity ωf to the target additional angular velocity ωadd to calculate a target motor angular velocity _ωMt corresponding to the target speed of the drive source. The target motor angular velocity _ωMt is supplied to the motor control unit 54, and as described above, the motor control unit 54 executes angular velocity control to set the target current It based on the motor angular velocity _ωM .

In this way, the second control device 25 and the control method using it multiply the torque deviation Terr, which is the deviation between the actual torque Ta corresponding to the actual torque tension Fa of the wire 36 and the target torque Tt, by the force control gain Ktp to set the target additional angular velocity ωadd, which is the target angular velocity ωt of the drive source. Then, the second control device 25 controls the motor angular velocity _ωM , which is the output of the drive source, based on the target additional angular velocity ωadd, thereby achieving passive compliance control that imparts compliance to the load 32 in the same manner as in the conventional method.

Here, as described above, the force control gain Ktp is a multiplication value of the flexibility coefficient (1/Kspr) of the power transmission system 33 and the proportional gain _Kp2 as a speed gain. In other words, since the force control gain Ktp includes the flexibility coefficient (1/Kspr), a value obtained by multiplying the torque deviation Terr by the flexibility coefficient (1/Kspr) is calculated as an angle (angle difference Δθ in FIG. 9 ) between the drive side and the load side corresponding to the displacement difference between the actual displacement of the wire 36 and the target displacement of the wire 36. Then, the target additional angular velocity ωadd of the drive source is calculated by multiplying this displacement difference of the wire 36 (angle difference Δθ in FIG. 9 ) by the proportional gain _Kp2 .

Furthermore, the second control device 25 sets a target motor angular velocity _ωMt based on the driven motor angular velocity ωf obtained by converting the joint angular velocity _ωL of the load 32 acquired from the joint angle sensor 44 and the target added angular velocity ωadd. Then, the second control device 25 controls the motor angular velocity _ωM to realize the target motor angular velocity _ωMt . Therefore, the second control device 25 can control the motor angular velocity ωM using the target motor angular velocity _ωMt as a control medium based on the target torque Tt and the actual torque Ta corresponding to the actual _torque tension Fa of the wire 36.

3, in the first actuator 30A, the wire 36 is disposed so as to be able to drive the load 32 in both directions, and tension sensors 39 (39A, 39B) are provided on both sides of the load 32 to obtain the tension F of the wire 36 acting on the load 32 in both directions. With this configuration, the tension F of the wire 36 can be obtained in both directions, so that the second control device 25 can control the motor angular velocity _ωM so that the target torque Tt acts on the load 32 in both directions.

Next, the function and effect of the controller of the second actuator 30B will be described with reference to FIG. 8. Regarding the second actuator 30B, only the differences from the first actuator 30A will be described.

In the second actuator 30B, the target tension setting unit 52 of the first control device 13 further includes a biasing force compensation unit 67. On the other hand, the tension control unit 53 of the second control device 25 does not include a subtractor 71. The bending side tension Fb acquired from the first tension sensor 39A is input to the driving side torque conversion unit 72. The driving side torque conversion unit 72 regards the input bending side tension Fb as the actual torque tension Fa of the wire 36, and calculates the actual torque Ta of the wire 36 portion of the power transmission system 33 by multiplying the actual torque tension Fa by the radius of the driving pulley 34. However, this actual torque tension Fa includes a force that counteracts the biasing force by the biasing member 42 in FIG. 4.

Therefore, in the target tension setting unit 52 of the first control device 13, the biasing force compensation unit 67 calculates an opposing torque Tc corresponding to the opposing force to the biasing force of the biasing member 42 by multiplying the joint angle _θL corresponding to the extension displacement of the biasing member 42 by a compensation coefficient corresponding to the spring constant of the biasing member 42. The target tension setting unit 52 multiplies the opposing torque Tc by a torque feedforward gain Ktff to convert it into a torque value of the load 32. This value is input to the adder 64 and added to the other three values. As a result, the biasing force of the biasing member 42 is offset.

The limiting section 66 of the second actuator 30B limits the target torque Tt so that -C2<Tt<C1. Here, C2 is a positive value smaller than C1, and is the lower limit at which the wire 36 does not become loose. The rest is the same as the controller of the first actuator 30A shown in FIG. 7, and the controller of the second actuator 30B also provides the same effects as the controller of the first actuator 30A.

As shown in FIG. 4, in the second actuator 30B, the power transmission system 33 includes a biasing member 42 having linear characteristics that constantly biases the wire 36 in a direction opposite to the direction in which the tension F of the wire 36 acts. As shown in FIG. 8, the first control device 13 adds a counter torque Tc equivalent to the biasing force of the biasing member 42 to set the target torque Tt.

The conventional passive compliance control shown in Patent Document 1 is based on the premise that the power transmission system 33 is continuous via a flexible element such as a metal spring and can transmit forces in both positive and negative directions. Therefore, if the wire 36 is arranged to transmit tension F in only one direction and the force in the other direction is provided by the biasing member 42, conventional control cannot be used.

In this embodiment, even if the wire 36 is arranged to transmit the tension F in only one direction and a force to the other direction is applied by the biasing member 42 having a linear characteristic, the controller can control the motor angular velocity _ωM so that the target torque Tt acts on the load 32. In addition, since there is no need to arrange the wire 36 so as to drive the load 32 in both directions, the actuator 30 can be prevented from becoming large.

10 is a Bode diagram showing the effect of responsiveness by the control according to the embodiment. The dashed line in the graph shows the frequency characteristics when the target tension setting unit 52 of the first control device 13 does not add to the adder 64 the torque value of the load 32 converted by multiplying the integral value of the joint angle difference Δθ _L by the integral gain Ki, that is, when PD control is performed. The solid line shows the frequency characteristics of the control according to the embodiment. When the target tension setting unit 52 performs PD control using the joint angle θ _L , as shown by the dashed line, a resonance point occurs in the frequency range of 0.5 to 1 Hz, and while the gain increases, a phase delay occurs, and responsiveness decreases. In contrast, in this embodiment, resonance in this frequency range is suppressed, and responsiveness is improved.

11 is a time chart showing the response of the second actuator 30B. The vertical axis represents the joint angle _θL . The dashed line in the chart represents the command value of the joint angle _θL (target joint angle _θLt ), the solid line represents the actual measurement value (joint angle _θL ) of the present invention, and the dashed line represents the actual measurement value of the comparative example. The comparative example shows a case in which the target tension setting unit 52 (FIG. 8) does not convert the secondary side joint angular velocity _ωL to the primary side (driving side) driven motor angular velocity ωf in the driving side speed conversion unit 75 and add the value multiplied by the control gain Kvff as the feedforward term in the adder 74.

11, in the comparative example, when the command value for the joint angle _θL changes slightly (when a rotation command is issued to the extension side), the joint angle _θL returns slowly toward 0°. In contrast, in the present invention, the target tension setting unit 52 adds a feedforward term related to the driven motor angular velocity _{ωf based on the joint angular velocity ωL} to calculate the target motor angular velocity _ωMt , so that the joint angle _θL changes following the command value in the same way as when a command is issued to the flexion side, and the joint angle _θL returns quickly toward 0°. In other words, even if the wire 36 is arranged to transmit the tension F only to one side and a force to the other side is applied by the biasing member 42, the second control device 25 sets the target motor angular velocity _ωMt based on the driven motor angular velocity ωf obtained by converting the joint angular velocity _ωL of the load 32 acquired from the joint angle sensor 44 and the target added angular velocity ωadd, thereby improving the responsiveness of the load 32.

12 is a time chart showing the response by the control system according to the embodiment. The vertical axis represents the joint angle θ _L. The dashed line in the chart represents the command value of the joint angle θ _L (target joint angle θ _L t), the solid line represents the actual measured value (joint angle θ _L ) of the present invention, and the dashed line represents the actual measured value of the comparative example.

The comparative example is based on the configuration of a conventional system. Here, the configuration of the conventional system will be described in order to clarify the difference from the present invention. In the conventional system, the second control device 25 only had the function of a motor driver that controlled the output of the motor 23. In other words, the tension control unit 53 that controls the motor angular velocity _ωM , which is the output of the motor 23, was provided in the first control device 13, and the first control device 13 obtained the joint angle _θL of the load body 32 from the joint angle sensor 44, calculated the joint angular velocity _ωL from the joint angle _θL , and provided it to the tension control unit 53.

In contrast, in the present invention, as shown in Fig. 6, a tension control unit 53 is integrally provided in the second control device 25 serving as a motor driver for controlling the motor angular velocity _ωM . The tension control unit 53 of the second control device 25 directly obtains the joint angle _θL representing the position of the load 32 from the joint angle sensor 44 without going through the first control device 13, and calculates the joint angular velocity _ωL of the load 32 based on the joint angle _θL . Then, the second control device 25 controls the joint angular velocity _ωL , which is the output of the motor 23, based on the actual torque Ta corresponding to the tension F of the wire 36 and the joint angular velocity _ωL of the load 32. As a result, the second control device 25 can calculate the joint angular velocity _ωL of the load 32 in a short time and bring the motor angular velocity _ωM closer to the target motor angular velocity _ωMt , thereby making the operation of the load 32 smoother.

7 and 8, in the second control device 25, the tension control unit 53 sets a target additional angular velocity ωadd of the motor 23 based on the target torque Tt and the actual torque Ta of the wire 36 portion of the power transmission system 33, which corresponds to the actual torque tension Fa of the wire 36. The tension control unit 53 sets a target motor angular velocity _ωMt based on the target additional angular velocity ωadd and the driven motor angular velocity ωf obtained by converting the joint angular velocity _ωL of the load 32. Then, the motor control unit 54 drives the motor 23 based on the target motor angular velocity _ωMt . In this way, the second control device 25 controls the output of the motor 23 using the target motor angular velocity _ωMt as a control medium based on the target torque Tt and actual torque Ta set by the first control device 13. As a result, even if the power transmission system 33 includes the wire 36, which is a flexible element, the controller can realize a conventional passive compliance control that provides compliance to the load 32.

In addition, in the arm 31 shown in FIG. 2, the palm portion 14 forming the hand portion supporting the load body 32 is connected to the forearm link 9 forming the base of the arm via the carpal joint 12. Therefore, the wire 36 shown in FIG. 3 and FIG. 4 is arranged to pass through the carpal joint 12. Meanwhile, since the second control device 25 and the tension sensor 39 (39A, 39B) are both provided on the forearm link 9 as described above, the time required for tension control processing by the second control device 25 is shortened. This makes the movement of the load body 32 smoother. Also, the palm portion 14 is prevented from becoming larger due to the implementation of devices and sensors.

In other words, the tension sensor 39 (39A, 39B) is disposed on the motor 23 side relative to the carpal joint 12. This eliminates the need to provide the tension sensor 39 (39A, 39B) on the load body 32 side relative to the carpal joint 12, thereby preventing the actuator 30 from becoming larger in size on the load body 32 side.

As shown in FIG. 1, the robot 1 includes a base 2 that supports the arm 31, the first control device 13 is provided on the base 2, and the second control device 25 is provided on the arm 31. This prevents the arm 31 from becoming larger due to the implementation of the first control device 13.

This concludes the explanation of the specific embodiment, but the present invention is not limited to the above embodiment and can be modified in a wide range of ways. For example, in the above embodiment, the present invention was explained as being applied to the actuator 30 of the hand 5 of the robot 1 as an example, but the present invention can be widely applied to parts other than the hand 5 and to robots 1 other than humanoid robots.

In the above embodiment, the first actuator 30A is provided with two tension sensors 39 as shown in FIG. 3. In another embodiment, the first actuator 30A may be configured as shown in FIG. 13. In this first actuator 30A, a spring element 81 is provided between the motor 23 and the drive pulley 34. The spring element 81 may be, for example, a torsion coil spring. Instead of two tension sensors 39 being provided on the wire 36 as in FIG. 3, in this embodiment, a displacement sensor 82 is provided that measures the displacement of the spring element 81. The displacement sensor 82 can obtain the driving force of the motor 23 against the load 32 in both directions by measuring the displacement of the spring element 81.

In this embodiment, the power transmission system 33 includes a spring element 81 having linear characteristics interposed between the motor 23 and the wire 36, and a displacement sensor 82 is provided to measure the displacement of the spring element 81 as a sensor for acquiring the torque generated in the drive pulley 34 due to the tension difference between the bending side tension Fb and the extension side tension Fe of the wire 36. The second control device 25 multiplies the displacement measured by the displacement sensor 82 by the spring constant of the spring element 81 to acquire the torque (= (Fb-Fe) x r, where r is the radius of the drive pulley 34) generated in the drive pulley 34 due to the tension difference between the bending side tension Fb and the extension side tension Fe of the wire 36. With this configuration, the desired compliance can be given to the movement of the load body 32 not only by the wire 36 but also by the spring element 81. In addition, the torque generated in the drive pulley 34 due to the tension difference between the bending side tension Fb and the extension side tension Fe of the wire 36 can be measured from the displacement of the spring element 81, so only one displacement sensor 82 is required, and there is no need to provide two sensors such as the tension sensor 39 provided on the wire 36, making it possible to miniaturize the configuration around the wire 36 and the sensor.

In another embodiment, the second actuator 30B may be configured as shown in FIG. 14. In this second actuator 30B, an idler pulley 37 and a tensioner 38 are provided at appropriate positions on the wire 36 of the power transmission system 33. In the illustrated example, one idler pulley 37 and one tensioner 38 are provided in the portion that transmits the bending side tension Fb of the wire 36. In this manner, an idler pulley 37 and a tensioner 38 may be added to the power transmission system 33.

In addition, in the above embodiment, the first control device 13 is disposed on the base 2 of the robot 1, but the first control device 13 may be disposed on the forearm link 9 of the robot 1, and the arrangement of each device is not limited to this. In addition, the specific configuration, arrangement, quantity, angle, procedure, etc. of each member or part can be changed as appropriate within a range that does not deviate from the spirit of the present invention. On the other hand, not all of the components shown in the above embodiment are necessarily required, and can be selected as appropriate.

1: Robot 2: Base 12: Wrist joint 13: First control device 14: Palm (hand)
23: Motor (drive source)
25: Second control device 30: Actuator 30A: First actuator 30B: Second actuator 31: Arm 32: Load 33: Power transmission system 36: Wire 39: Tension sensor 39A: First tension sensor (bending side)
39B: Second tension sensor (extension side)
44: Joint angle sensor (load position sensor)
53: Tension control unit (force control unit)
54: Motor control unit F: Tension Fa: Actual torque Tension Fb: Bending side tension Fe: Extension tension Ta: Actual torque Tt: Target torque θ _L : Joint angle (position)
θ _L t: Target joint angle (target position)
ω _L : Joint angular velocity (velocity)
ω _M : Motor angular velocity (speed, output of driving source)
ω _M t: Target motor angular speed (target speed)
ωadd: target additional angular velocity (target additional velocity)
ωf: Angular velocity of driven motor (speed of driven drive source)

Claims (4)

A robot having an arm and a load body displaceably provided with respect to the arm,
A drive source provided on the arm;
a power transmission system including a wire that transmits the power generated by the driving source to the load;
A load position sensor for acquiring a position and a velocity of the load;
A tension sensor for acquiring the tension of the wire;
a controller for controlling an output of the drive source so as to realize a given target position of the load body;
The controller:
a first control device that sets a target output to be applied to the load based on the target position and the position of the load;
a second control device that controls the output of the drive source based on the target output, the tension of the wire, and the speed of the load,
A robot in which the second control device obtains the position of the load directly from the load position sensor and calculates the velocity based on the position. The second control device,
a force control unit that sets a target additional speed of the driving source based on the target output and the tension of the wire, and sets a target speed of the driving source based on the target additional speed and a driven driving source speed obtained by converting the speed of the load;
The robot according to claim 1 , further comprising a control unit that drives the drive source so as to control the output based on the target speed. the arm includes an arm base and a hand portion connected to the arm base via a joint and supporting the load body;
3. The robot according to claim 1, wherein the wire is provided to pass through the joint, and the second control device and the tension sensor are provided at a base of the arm. Further comprising a base supporting the arm,
The robot according to any one of claims 1 to 3, wherein the first control device is provided on the base, and the second control device is provided on the arm.

Images