Joint mechanism control device

Abstract

The wire is designed to prevent excessive loosening while still allowing it to exert significant torque on the joint it drives. [Solution] The joint mechanism is controlled by a control unit, using a joint mechanism, a first wire, a first actuator that winds or feeds out the first wire and rotates the joint in a first direction by winding, a second wire, and a second actuator that winds or feeds out the second wire and rotates the joint in a second direction opposite to the first direction by winding. The control unit alternately executes a first mode in time which drives the first actuator and the second actuator to rotate the joint, and a second mode which drives the first actuator and the second actuator to prevent the first wire and the second wire from loosening.

Description

Technical Field

[0001] The present disclosure relates to a joint mechanism control device that controls a joint mechanism having joints.

Background Art

[0002] In recent years, in factories, construction sites, etc., there has been an increasing demand for joint mechanism control devices such as robots having joint mechanisms capable of performing operations similar to those of humans. In such robots, etc., it is possible to change the posture of the joint mechanism by appropriately controlling the angle of the joint and perform a desired operation.

[0003] In order to control the angle of the joint, a joint mechanism control device configured to perform control for winding or feeding out a wire is also known. However, in a mechanism for changing the angle of the joint with a wire, there is a possibility that the wire may loosen. When the wire loosens, the control of the joint mechanism becomes inaccurate or the responsiveness deteriorates. Therefore, in such a joint mechanism using such a wire, control is required such that the wire does not loosen while generating a large joint torque.

[0004] In such a joint mechanism using such a wire, in order to realize control in which the wire does not loosen, a pulley is provided at the joint portion, and an operation of winding up / loosening a wire more than the number of joints by a motor (actuator) via the pulley is also known. However, it has not been easy to suppress the occurrence of wire loosening while generating a large joint torque.

Prior Art Documents

Patent Documents

[0005]

Patent Document 1

Summary of the Invention

Problems to be Solved by the Invention

[0006] In view of the above issues, the joint mechanism control device of this disclosure provides a joint mechanism control device that enables a large torque to be exerted on the joint being driven by the wire, while preventing the wire from becoming excessively loose. [Means for solving the problem]

[0007] To solve the above problem, the joint mechanism control device according to the present disclosure comprises a joint mechanism having a joint, a first wire, a first actuator that winds or feeds out the first wire and rotates the joint in a first direction by winding, a second wire, a second actuator that winds or feeds out the second wire and rotates the joint in a second direction opposite to the first direction by winding, and a control unit that controls the first actuator and the second actuator. The control unit alternately executes in time a first mode in which it drives the first actuator and the second actuator to rotate the joint, and a second mode in which it drives the first actuator and the second actuator so that the first wire and the second wire do not loosen.

[0008] According to this joint mechanism control device, the control of the first and second actuators is performed separately in the time axis direction, which prevents the wire from becoming excessively loose while still allowing a large torque to be exerted on the joint being driven by the wire.

[0009] As an example, the first mode is a mode in which an internal command speed is calculated from the joint angle deviation to be a speed command for the first actuator and the second actuator so that the joint reaches the commanded angle, and speed control of the first actuator and the second actuator is performed based on the internal command speed, and the second mode is a mode in which a joint command torque is calculated from the joint angle deviation to be applied to the joint, and the first actuator and the second actuator are controlled so that the joint reaches the commanded angle. In this case, the second mode may output actuator command torque to the first actuator and the second actuator so that the tension of all the first wires and the second wires involved in driving the joint is equal to or greater than the minimum tension.

[0010] As another example, the first mode is a mode in which the wire length required to be wound or fed is calculated from the angle deviation of the joint, and the wire lengths of the first wire and the second wire are controlled by controlling the first actuator and the second actuator so that the joint is at the commanded angle, and the second mode is a mode in which the tension of the first wire and the second wires related to driving the joint to be driven is controlled so that the joint is not driven and the joint torque becomes zero. In this case, the second mode can output actuator command torque to the first actuator and the second actuator so that the tension of the first wire and the second wire is equal to or greater than the minimum tension. [Effects of the Invention]

[0011] According to the joint mechanism control device of this disclosure, multiple motor controls are divided and executed in the time axis direction, making it possible to generate a large torque in the joint being driven by the wire while preventing the wire from becoming excessively loose. [Brief explanation of the drawing]

[0012] [Figure 1] An example of a wire-driven joint mechanism control device according to the first embodiment is shown. [Figure 2] This is a schematic diagram of a joint mechanism JM that has only one joint. [Figure 3] This is a block diagram showing an example of the configuration details of controller C. [Figure 4] The control block diagrams for the first mode, joint velocity-based joint angle control, and the second mode, joint torque-based joint angle control, are shown. [Figure 5] This graph shows the change in torque of motor 40 when the first mode (joint velocity-based joint angle control) and the second mode (joint torque-based joint angle control) are repeatedly executed at high speed over time. [Figure 6] This is a schematic diagram illustrating an operation in a wire-driven joint mechanism control device according to a second embodiment, in which, for a joint among two or more joints where an angular deviation from the target angle exists steadily due to an external force, the column component of the pulley matrix corresponding to that joint is changed to 0. [Figure 7] This is a schematic diagram illustrating the operation of a wire-driven joint mechanism control device according to a second embodiment. [Figure 8] The operation and effects of a wire-driven joint mechanism control device according to a second embodiment will be described. [Figure 9] This is a block diagram showing an example of the detailed configuration of the controller C of a wire-driven joint mechanism control device according to the third embodiment. [Figure 10] This shows a control block diagram of a wire-driven joint mechanism control device according to a third embodiment. [Figure 11] This explains the problems with conventional technology. [Modes for carrying out the invention]

[0013] This embodiment will be described below with reference to the attached drawings. In the attached drawings, functionally identical elements may be indicated by the same number. The attached drawings show embodiments and implementation examples in accordance with the principles of this disclosure, but they are for the purpose of understanding this disclosure and are not to be used in any way to restrict the interpretation of this disclosure. The descriptions in this specification are merely typical examples and do not limit the claims or applications of this disclosure in any way.

[0014] In each drawing, identical or substantially equivalent elements, components, and parts may be assigned the same reference numeral. Furthermore, dimensions and proportions in the drawings are exaggerated for illustrative purposes and may differ from actual proportions. Also, in the mathematical formulas (images) included in the following explanation, bolded notation represents matrices or vectors. To distinguish these matrices or vectors from scalar quantities, they may be represented by "→" instead of bold in the text.

[0015] While this embodiment is described in sufficient detail for those skilled in the art to implement the disclosure, it is important to understand that other implementations and forms are possible, and that the configuration and structure can be modified and various elements replaced without departing from the scope and spirit of the technical idea of ​​this disclosure. Therefore, the following description should not be construed as limiting to this.

[0016] [First Embodiment] Figure 1 shows an example of a wire-driven joint mechanism control device according to the first embodiment. As shown in Figure 1, the joint mechanism control device 1 of the first embodiment comprises a joint mechanism JM driven by the tension of a wire driven by a motor (actuator), and a controller C that controls it.

[0017] The joint mechanism JM comprises multiple links 10 (for example, three links 10A, 10B, and 10C), joints 20 (for example, two joints 20A and 20B), and pulleys 30 (for example, large pulleys 30A and 30C, and small pulleys 30B and 30D), and is, in one example, a robot arm. Adjacent links 10 are configured to be rotatable (angle changeable) around joint axes J (J1 and J2).

[0018] A pulley 30 is positioned around the joint axis J, and a number of wires W (for example, three wires W1 to W3) greater than the number of joints 20 (two in this case) are in contact with the outer circumference of the pulley 30. The joint mechanism JM is driven by a torque generated at the joint axis J, which is the tension of the wires W multiplied by the radius of the pulley 30. One end of the wires W is fixed to the link 10 or the pulley 30, while the other end is controlled to be wound up or unwound by a motor (actuator) 40 (40A to 40C) mounted on the base 50. The motor 40 controls the winding / unwinding of the wires W by receiving torque and rotation direction according to a control signal from the controller C. In this specification, the mechanical drive system of the joint, including the pulley 30 and the wires W, may be referred to as the wire drive system.

[0019] The configuration illustrated in Figure 1 is just one example, and the following disclosures are applicable to joint mechanisms JM employing a larger number of links and wires. Conversely, as shown in Figure 2, a joint mechanism may have only one joint, but two wires W1 and W2 (first wire and second wire), which is more than the number of joints, and motors 40A and 40B (first actuator and second actuator) that wind or unwind the wires W1 and W2. In any case, to prevent the wires W from loosening, a number of wires W greater than the number of joints N (N+1 or more) are provided. Furthermore, even in a joint mechanism JM having multiple joints, when focusing on one joint, at least two motors (actuators) are configured to be involved in the operation of that joint via at least two wires. For example, two wires (W1 and W2) are involved in the operation of joint J1, and three wires W (W1, W2, and W3) are involved in the operation of joint J2. Focusing on joint J1, of the two motors 40A and 40B, motor 40A winds or feeds out wire W1, and winding rotates joint J1 in a first direction. On the other hand, motor 40B winds or feeds out wire W2, and winding rotates joint J1 in a second direction opposite to the first direction. Controller C is required to control the wire W so that it does not become excessively loose, preventing it from derailing from pulley 30 and breaking.

[0020] In the case of a joint mechanism JM having one joint J, two wires W1 and W2 are provided, as shown in Figure 2. This allows for the design of mechanically elongated links (such as the fingers of a robot hand). While a large number of wires increases the possibility of wire slack, in this embodiment, the control described below can be used to suppress wire slack while still generating a large torque. In Figure 1, all links 10 are controlled by wires W, but some links may be directly controlled by motors 40. In addition, some links may include motors that impart linear motion to the links.

[0021] Motor 40 winds up and feeds out the wire W using a motor pulley (not shown in Figure 1) attached to the drive shaft of motor 40. The rotation angles of multiple motors 40 are →δθ M When the wire displacement (wire displacement vector) is denoted as →δl, the relationship between the two is given by the motor pulley matrix R in [Mathematics A] below. M (r1~r m The rotation angle can be expressed as shown in [Equation B], where δθ is the radius of the motor pulley. (Note that if a gear is attached to the drive shaft of motor 40, the rotation angle of the gear's output shaft becomes the rotation angle of motor 40.) Note that the rotation angle → δθ M The vectors represent the rotational amounts (δθ1, δθ2…) of each of the multiple motors 40. The wire displacement →δl represents the displacement amounts (δl1, δl2…) of each of the multiple wires W as vector quantities.

[0022]

number

number

[0023] If there is no slack in wire W, the joint angle deviation → δθ q For this, the wire displacement →δl is uniquely determined. →δθ q The mapping from →δl is linear, and by the Pulley matrix T shown in [Equation 1] below, →δl and δθ q This can be expressed as [Equation 2].

[0024]

number

[0025]

number

[0026] This pulley matrix T is the pulley matrix when driving n joints (m > n) with m wires. The coefficient a ij represents the winding direction of the wire W. When pulling the wire W causes the joint to rotate in the positive direction, a ij = 1, and when pulling the wire W causes the joint to rotate in the negative direction, a ij = -1. Also, r ij indicates the radius of the pulley 30 at the j-th joint through which the i-th wire passes. As an example, for instance, when driving one joint with two wires, the pulley matrix T is 2 rows × 1 column. When driving two joints with three wires, the pulley matrix is 3 rows × 2 columns. From [Number B] and [Number 2] → δθ M = R M -1 ·T·→δθ q The relationship of can be obtained. For the convenience of writing the control law of the motor, R M -1 ·T is defined as T W and →δθ M = T W ·→δθ q Sometimes. This Tw is also called a pulley matrix. The difference from the pulley matrix T of Equation 1 is distinguished in the context.

[0027] As will be described later, the controller C provides a joint mechanism that can apply a large torque while suppressing the occurrence of slack in the wire W by alternately executing two control modes (here, the joint angle control mode based on joint speed as the first mode and the joint angle control mode based on joint torque as the second mode) over time. FIG. 3 shows an example of the configuration of this controller C.

[0028] In the joint angle control based on joint speed, which is the first mode, the joint angle deviation →δθ q is obtained, and the wire displacement amount →δl equivalent to the joint angle deviation →δθ q is obtained based on [Number 2]. Then, the motor rotation angle deviation →δθ M equivalent to the wire displacement amount →δl is calculated based on [Number B], and further, the motor rotation angle deviation →δθ MCompensating motor command torque → τ M The control is executed by calculating [the value].

[0029] The second mode, joint torque-based joint angle control, involves joint angle deviation → δθ q Obtain the joint angle deviation → δθ q Joint torque to compensate for →τ q Calculate the joint torque → τ q Equivalent wire tension → F l Calculate the wire tension → F l Equivalent motor command torque → τ M The control is executed by calculating [the value].

[0030] Refer to Figure 3 to explain the details of the configuration of controller C. As an example, controller C is configured to include a difference calculator 71 and a programmable controller 72.

[0031] The difference calculator 71 calculates the joint angle deviation →δθ as the difference between the joint angle command obtained from an external control device (not shown) and the current angle of each joint (current joint angle). q The programmable controller 72 calculates this joint angle deviation → δθ. q Based on this, the motor command torque →τ is a command relating to the torque applied to the motor 40 according to one of two control methods (joint velocity-based joint angle control and joint torque-based joint angle control). M The motor 40 outputs the current rotation angle, which is the motor current angle, according to an encoder (not shown). This motor current angle is converted to the motor current speed by a differentiator (not shown) and input to the programmable controller 72. The current angle of each joint can also be measured by an encoder or potentiometer (not shown) mounted near the joint.

[0032] The two control modes executed by the programmable controller 72 are switched and executed at high speed. The timing of the switch and the execution period of each control vary depending on the system structure and load.

[0033] Figure 4 shows control block diagrams for the first mode, joint velocity-based joint angle control, and the second mode, joint torque-based joint angle control.

[0034] First, we will explain the second mode, joint torque-based joint angle control. As shown in Figure 4(b), joint torque-based joint angle control can be achieved by generating joint command torque from the joint angle deviation using a PID compensator or the like. In this embodiment, the joint mechanism is driven by a wire W, but joint torque-based joint angle control is possible. The joint torque is →τ q , wire tension → F l If we set it as follows, then by the principle of virtual work, the following equation holds:

[0035]

number

[0036] From [Equation 3] and [Equation 2], the following relationship can be obtained.

[0037]

number

[0038] This is wire tension → F l From joint torque → τ q This indicates that it is uniquely determined. However, joint torque → τ q From wire tension → F l It is not possible to uniquely determine this. Therefore, in the joint torque-based joint angle control, which is the second mode of the first embodiment, a minimum tension is set such that the wire W does not loosen, and the given joint torque → τ q In contrast, wire tension → F l By finding a combination of values ​​such that all elements have a tension greater than or equal to the minimum tension, the wire tension that prevents the wire from loosening is determined → F l This can be calculated. The calculated wire tension → F lBy multiplying this by the pulley radius, the motor command torque can be calculated from the joint command torque.

[0039] Next, we will explain the first mode, joint velocity-based joint angle control. By rapidly repeating the execution of this first and second modes, it becomes possible to generate a large torque at the joint being driven by the wire while preventing the wire from becoming excessively loose. As shown in Figure 4(a), in the first mode, joint velocity-based joint angle control, the internal command speed of the motor 40 is calculated based on the joint angle deviation, and the difference between the internal command speed and the current speed of the motor 40 is calculated. The cumulative error of the speed deviation, which is the speed difference, is calculated, and the motor command torque is calculated based on the value obtained by multiplying this integral by the speed integral gain. Note that in Figure 4(a), K pp , K vp , K vi J represents the angle-proportional gain, velocity-proportional gain, velocity-integral gain, and the inertia of the controlled object, respectively. The integral of the velocity deviation has an upper limit of Int max The lower limit is Int min This is used to saturate the system. Note that when switching between the first and second modes, the internal state of each mode is not reset, and control resumes from that internal state when the next cycle is executed.

[0040] The first mode, joint velocity-based joint angle control, is a control method intended to actively rotate motors that should be back-driven during the second mode, joint torque-based joint angle control, in the direction of back-driving. Specifically, the motors that should be back-driven are back-driven as motor speed control by setting the internal command speed, calculated from the joint angle deviation, to a negative value.

[0041] In the first mode, joint velocity-based joint angle control, the motor command speed (internal command speed) is generated directly from the joint angle deviation. Therefore, if joint angle deviations occur steadily, the wires of some motors 40 may be actively fed out in the backdrive direction for a long time, potentially causing the wires to become excessively loose. However, in this embodiment, the first mode and the second mode are switched and executed periodically, so it is possible to avoid some motors 40 excessively loosening their wires.

[0042] Furthermore, if the wire feeding effect of the first mode, which is joint velocity-based joint angle control, exceeds the winding effect of the second mode, which is joint torque-based joint angle control, the wire feeding state may occur for a long period of time. For this reason, it is preferable to control the switching timing so that the time average of the wire feeding control in the first mode and the wire winding control in the second mode is always positive (winding control is dominant), and the negative torque during the joint velocity-based joint angle control in the first mode is limited.

[0043] Figure 5 is a graph showing the change in torque of motor 40 when the first mode (joint velocity-based joint angle control) and the second mode (joint torque-based joint angle control) are repeatedly executed at high speed over time. The vertical axis represents the torque of motor 40, with the positive direction representing torque in the winding direction and the negative direction representing torque in the feed direction. In the second mode, the torque of all motors 40 (motors 40-1 to 40-3) is set to torque in the winding direction. On the other hand, in the first mode, some motors 40 (e.g., motors 40-2 and 40-3) generate torque in the feed direction (negative direction).

[0044] Referring to Figure 11, the effects of rapidly repeating the first mode (joint velocity-based joint angle control) and the second mode (joint torque-based joint angle control) over time will be explained.

[0045] In the joint mechanism JM, in order to bring the joint angle of one joint to the target angle, it is necessary that (1) each joint in the wire drive system exerts the required joint torque, and (2) all wires do not loosen to the point of derailing from the pulley. However, if only the second mode, joint torque-based joint angle control, is performed, the joint torque decreases, making it difficult to achieve control that satisfies both (1) and (2) above. That is, as shown in Figure 11, it is necessary to feed out the wire while applying minimum tension (back drive), but feeding out the wire and applying minimum tension are contradictory movements, and this is difficult to achieve by continuously performing the second mode, joint torque-based joint angle control. The reason for this is that the motor 40 has poor back drive properties due to the friction of the high reduction gear, and most of the tension for joint driving is wasted there, resulting in a decrease in actual joint torque. By repeatedly executing the first and second modes at high speed over time, control that satisfies both (1) and (2) becomes possible.

[0046] As described above, with the joint mechanism control device of this first embodiment, multiple motor controls are divided and executed in the time axis direction, making it possible to generate a large torque at the joint being driven by the wire while preventing the wire from loosening.

[0047] [Second Embodiment] Next, with reference to Figures 6 and 7, a wire-driven joint mechanism control device according to a second embodiment will be described. This joint mechanism control device of the second embodiment is suitable for controlling a joint mechanism JM having two or more joints. Specifically, for a joint among the two or more joints where an angular deviation from the target angle exists steadily due to an external force, an operation is performed to change the column component of the pulley matrix corresponding to that joint to 0 (see Figure 6).

[0048] As an example, we will describe a case where the joint mechanism JM is a hand mechanism for gripping an object as shown in Figure 7. Here, as an example, we will describe a joint mechanism JM that has three links 10A, 10B, and 10C, two joints 20A and 20B, and a finger F that extends from link 10B and can grip the workpiece WK together with link 10A. In Figure 7, wires, pulleys, and motors are omitted from the illustration. Figure 7 shows (1) the initial state before gripping the workpiece WK, (2) the state in which the workpiece WK is held, and (3) the state in which the joint 20B rotates in order to lift the workpiece WK while holding it.

[0049] Assume that joint 20A, one of the two joints, has a constant angular deviation from the target angle. When changing from the initial state (1) to the state of gripping the workpiece WK (2), joint 20A is rotated. At this time, the joint angular deviation, which is the difference between the target angle and the current angle, is calculated, and control is performed to eliminate the joint angular deviation.

[0050] During the time period in which joint velocity-based joint angle control in the first mode is performed, at least one wire Wa of the wires W that drive the joint 20A may be subjected to a large tension (the maximum tension that can be exerted if integral control is working) in order to eliminate the joint angle deviation. In this case, when moving another joint 20B, it may be necessary to displace wire Wa in the direction of feed (back-drive).

[0051] Because the tension in wire Wa is large, the tension that wire Wb must exert to move joint 20B is also large. However, since there is a limit to the tension that wire Wb can exert, the joint torque that can be generated at joint 20B is reduced by the large torque of wire Wa. In cases where a large torque is required at joint 20B, such as when moving against gravity, it may not be possible to generate the necessary joint torque, and the drive system may not be able to perform adequately.

[0052] To implement this type of control, the control device of the second embodiment performs joint velocity-based joint angle control using a new pulley matrix Tw' in which the components of the pulley matrix Tw corresponding to joint 20A are changed to zero (see Figure 6). The components of the pulley matrix Tw corresponding to joint 20B are not changed.

[0053] Changing the component corresponding to joint 20A in the pulley matrix to zero is equivalent to virtually treating the joint angle deviation of joint 20A as zero. As a result, when performing joint velocity-based joint angle control, the tension in wire Wa that originates from the joint angle deviation of joint 20A becomes zero. Therefore, the tension exerted by wire Wb to drive joint 20B is not reduced by the tension in wire Wa, and the joint torque of joint 20B can be increased.

[0054] Referring to Figure 8, the operation and effects of the second embodiment will be explained using as an example a joint mechanism having two joints 20A, 20B, three links 10A to 10C, and three wires W1 to W3, where joint 20B is rotated in the clockwise direction in Figure 8. Wires W1 to W3 are each controlled to be wound up or unwound by separate motors M1 to M3.

[0055] In the control of the first embodiment, the joint angle deviation → δθ is the difference between the current joint angle and the target joint angle of one joint 20. q Accordingly, the torque that motors M1-M3 exert on wires W1-W3 is →τ M The torque τ at joint 20A is being calculated. J1 If you want to exert the torque of joint 20A by motor M1, then τ J1M1 , the torque of joint 20A by motor M2 is τ J1M2 Therefore, torque τ J1 =τ J1M1 -τ J1M2 This is the result.

[0056] Torque τ that joint 20A can exert J1 Since this is the wire tension multiplied by the pulley radius of the joint, the tension of the wire W1 is f M1The radius of the pulley over which wire W1 is applied to joint 20A is r J1M1 Therefore, the torque τ produced by motor M1 of joint 20A J1M1 =f M1 r J1M1 This means that the tension f of the wire W1 that the motor M1 should pull is M1 is f M1 =τ J1M1 / r J1M1 This is the result.

[0057] At joint 20B, this tension f M1 and the tension f due to motor M2 M2 Motor M3 needs to pull wire W3 with a tension exceeding this, but in this embodiment, the torque applied to joint 20A can be reduced to zero by setting the component of the pulley matrix corresponding to joint 20A to 0 as described above (τ J1M1 =τ J1M2 =0) This means that f M1 =f M2 Since this becomes 0, motor M3 can generate enough torque to bend joint 20B.

[0058] The pulley matrix is ​​a matrix that lists the radii of the pulleys connected to joints 20A and 20B through which each wire W1 to W3 passes. By virtually setting the pulley radius of a specific joint used to determine joint torque to 0, it becomes possible to drive other joints in a joint mechanism with multiple joints, even if the tension in one joint increases.

[0059] [Third Embodiment] Referring to Figures 9 to 10, an example of a wire-driven joint mechanism control device of the third embodiment is shown. This joint mechanism control device of the third embodiment includes a joint mechanism JM and a controller C, similar to the first embodiment. Similar to the first embodiment, the controller C allows for high-speed switching between the first mode and the second mode. However, in this third embodiment, the control content of the first mode and the second mode differs from that of the first embodiment.

[0060] In this third embodiment, the first mode calculates the wire length required for winding or unwinding from the angular deviation of each joint 20, and controls the motor (actuator) to control the wire length so that the joint 20 is at the commanded angle (wire length-based joint angle control). The second mode controls the wire tension so as not to drive the joint 20 (wire tension control). By repeating these first and second modes at high speed over time, the same effects as in the first embodiment can be achieved.

[0061] Figure 10 shows control block diagrams for the first mode, wire length-based joint angle control, and the second mode, wire tension control.

[0062] First, we will explain the first mode, wire length-based joint angle control. As shown in Figure 10(b), in wire length-based joint angle control, the joint angle deviation → δθ q The Pulley matrix Tw(=R M -1 The motor angle deviation obtained by multiplying by ×T is →δθ M The motor command angle is calculated according to the current angle of the motor 40, and the motor command torque is calculated by PID control based on the motor command angle and the current angle deviation of the motor 40. If only this first mode is executed, the wire may loosen to the point of derailing from the pulley, and as with the first embodiment, it becomes difficult to achieve both the generation of the required torque and the prevention of wire loosening.

[0063] Therefore, in this embodiment, a second mode is implemented, which controls the wire tension but does not drive the joints 20. In the second mode, as shown in Figure 10(a), the torque of all joints 20A and 20B is set to zero, and motor torque is generated so that only the minimum tension of each wire W is obtained. By switching between the first mode and the second mode at high speed, the same effects as in the first embodiment can be obtained.

[0064] This disclosure is not limited to the embodiments described above, and includes various modifications. For example, the embodiments described above are described in detail to make this disclosure easier to understand, and are not necessarily limited to those having all the configurations described. Furthermore, it is possible to replace parts of the configuration of one embodiment with the configuration of another embodiment, and it is also possible to add configurations from other embodiments to the configuration of one embodiment. In addition, it is possible to add, delete, or replace parts of the configuration of each embodiment with other configurations. For example, the first embodiment and the second embodiment may be combined to perform a first mode in which the wire length required for winding or feeding is calculated from the angle deviation of each joint 20, and the motor (actuator) is controlled to control the wire length so that the joint 20 is at the commanded angle (wire length-based joint angle control), and a second mode in which joint torque-based joint angle control similar to the first embodiment is performed. [Explanation of Symbols]

[0065] 10…Link 20... joints 30...Pulley 40…motor 50...Base 71...Difference calculator 72…Programmable Controller C... Controller JM...Joint Mechanism W...wire

Claims

1. Joint mechanisms having joints, First wire and A first actuator that winds or unwinds the first wire and rotates the joint in a first direction by winding, The second wire and A second actuator that winds or unwinds the second wire, and by winding, rotates the joint in a second direction opposite to the first direction, A control unit that controls the first actuator and the second actuator, Equipped with, The control unit alternately executes, in time, a first mode in which it drives the first actuator and the second actuator to rotate the joint, and a second mode in which it drives the first actuator and the second actuator to prevent the first wire and the second wire from loosening. A joint mechanism control device characterized by the following:

2. The first mode is a mode in which an internal command speed is calculated from the joint angle deviation to be a speed command for the first actuator and the second actuator so that the joint is at the commanded angle, and speed control of the first actuator and the second actuator is performed based on the internal command speed. The joint mechanism control device according to claim 1, wherein the second mode is a mode in which the joint command torque to be applied to the joint is calculated from the joint angle deviation and the first actuator and the second actuator are controlled so that the joint is at the commanded angle.

3. The joint mechanism control device according to claim 2, wherein the second mode outputs actuator command torque to the first actuator and the second actuator so that the tension of the first wire and the second wire is equal to or greater than the minimum tension.

4. The first mode is a mode in which the wire length required to be wound or fed is calculated from the angle deviation of the joint, and the wire lengths of the first wire and the second wire are controlled by controlling the first actuator and the second actuator so that the joint is at the commanded angle. The second mode is a mode in which the tension of the first wire and the second wire related to driving the joint to be driven is controlled so that the joint is not driven and the joint torque becomes zero. The joint mechanism control device according to claim 1.

5. The second mode outputs actuator command torque to the first actuator and the second actuator so that the tension of the first wire and the second wire is equal to or greater than the minimum tension. The joint mechanism control device according to claim 4.

6. The first mode is a mode in which the wire length required to be wound or fed is calculated from the angle deviation of the joint, and the wire lengths of the first wire and the second wire are controlled by controlling the first actuator and the second actuator so that the joint is at the commanded angle. The second mode is a mode in which the joint command torque to be applied to the joint is calculated from the joint angle deviation, and the first actuator and the second actuator are controlled so that the joint is at the commanded angle. The joint mechanism control device according to claim 1.

7. The joint mechanism control device according to claim 1, wherein the control unit calculates a wire displacement vector by multiplying the joint angle deviation vector by a pulley matrix.

8. The aforementioned joint mechanism has multiple joints, The joint mechanism control device according to claim 7, wherein the control unit changes the components of at least some of the columns of the pulley matrix to zero.

Images