Joint mechanism control device

Abstract

The present invention makes it possible to exert large torque on a joint being driven by a wire while preventing the wire from being excessively loosened. The present invention involves controlling by a control unit by using: a joint mechanism; a first wire; a first actuator that performs winding or feeding of the first wire and turns a joint in a first direction by the winding; a second wire; and a second actuator that performs winding or feeding of the second wire and turns the joint in a second direction opposite to the first direction by the winding. The control unit temporally alternately executes a first mode of driving the first actuator and the second actuator so as to turn the joint, and a second mode of driving the first actuator and the second actuator so as not to loosen the first wire and the second wire.

Description

Joint mechanism control device

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

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

[0003] In order to control the angle of a 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 a 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 while generating a large joint torque, the wire does not loosen.

[0004] In such a joint mechanism using 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 via the pulley by a motor (actuator) is also known. However, it has not been easy to suppress the occurrence of wire loosening while generating a large joint torque.

[0005] Japanese Unexamined Patent Application Publication No. 2010 - 240834

[0006] In view of the above problems, the joint mechanism control device of the present disclosure provides a joint mechanism control device that can prevent the wire from being excessively loosened and at the same time enable the wire to exert a large torque on the joint being driven.

[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 makes it possible to generate a large torque in the joint being driven by the wire while preventing the wire from becoming excessively loose.

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

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

[0012] This shows an example of a wire-driven joint mechanism control device according to the first embodiment. This is a schematic diagram when the joint mechanism JM has only one joint. This is a block diagram showing an example of the detailed configuration of the controller C. This shows a control block diagram of the first mode, which is joint velocity-based joint angle control, and the second mode, which is joint torque-based joint angle control. This is a graph showing the change in torque of the 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. This is a schematic diagram illustrating the operation of changing the column component of the pulley matrix corresponding to a joint to 0 for a joint among two or more joints where an angular deviation from the target angle exists steadily due to an external force, in a wire-driven joint mechanism control device according to the second embodiment. This is a schematic diagram illustrating the operation of the wire-driven joint mechanism control device according to the second embodiment. The operation and effects of the wire-driven joint mechanism control device according to the second embodiment are explained. This is a block diagram showing an example of the detailed configuration of the controller C of the wire-driven joint mechanism control device according to the third embodiment. This shows a control block diagram of the wire-driven joint mechanism control device according to the third embodiment. The problems of the prior art are explained.

[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 interpreted as limiting it to this embodiment.

[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 installed 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, and the other end is controlled to be wound up or fed out by a motor (actuator) 40 (40A to 40C) mounted on the base 50. The motor 40 is given torque and rotation direction according to a control signal from the controller C to control the winding / feeding up of the wires W. 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 up 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, in joint J1, two wires (W1 and W2) are involved in its operation, and in joint J2, three wires W (W1, W2, and W3) are involved in its operation. Focusing on joint J1, of the two motors 40A and 40B, motor 40A winds up or feeds out wire W1, and winding rotates joint J1 in a first direction. On the other hand, motor 40B winds up or feeds out wire W2, and winding rotates joint J1 in a second direction opposite to the first direction. The controller C is required to control the wire W so that it does not become excessively loose, preventing it from derailing from the 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] The motor 40 winds and feeds out the wire W by means of a motor pulley (not shown in FIG. 1) attached to the drive shaft of the motor 40. When the rotational angles of the plurality of motors 40 are →δθ Ｍ and the wire displacement amount (wire displacement vector) is →δl, the relationship between the two is the motor pulley matrix R in the following [Equation A] Ｍ (r １ to r ｍ are the radii of the motor pulleys) and can be expressed as in [Equation B] (when a gear is attached to the drive shaft of the motor 40, the rotational angle of the output shaft of the gear becomes the rotational angle of the motor 40). Note that the rotational angle →δθ Ｍ represents the rotational amounts (δθ １ , δθ ２ …) of each of the plurality of motors 40 as a vector quantity. The wire displacement amount →δl represents the displacement amounts (δl １ , δl ２ …) of each of the plurality of wires W as a vector quantity.

[0022]

[0023] If there is no slack in the wire W, the wire displacement amount →δl is uniquely determined with respect to the joint angle deviation →δθ ｑ . The mapping from →δθ ｑ to →δl is linear, and →δl and δθ ｑ can be expressed as in [Equation 2] by means of a pulley matrix T as in the following [Equation 1].

[0024]

[0025]

[0026] This pulley matrix T is a pulley matrix when driving n joints (m > n) with m wires. The coefficient a ｉｊ represents the winding direction of the wire W. When the wire W is pulled and the joint rotates in the positive direction, a ｉｊ = 1, and when the wire W is pulled and the joint rotates in the negative direction, a ｉｊ = -1. Also, r ｉｊThis indicates the radius of pulley 30 at the j-th joint through which the i-th wire passes. For example, the pulley matrix T when two wires drive one joint is 2 rows x 1 column. The pulley matrix when three wires drive two joints is 3 rows x 2 columns. From [Math B] and [Math 2] → δθ Ｍ = R Ｍ －１ ・T・→δθ ｑ The following relationship is obtained. For convenience in writing out the motor control law, R Ｍ －１ ・T to T Ｗ Defined as, →δθ Ｍ = T Ｗ ・→δθ ｑ This is sometimes done. This Tw is also called a Pulley matrix. The difference between this and the Pulley matrix T in Mathematics 1 is distinguished by the context.

[0027] Controller C, as described later, provides a joint mechanism that can apply a large torque while suppressing the loosening of the wire W by alternately executing two control modes over time (here, a joint velocity-based joint angle control mode as the first mode and a joint torque bail joint angle control mode as the second mode). Figure 3 shows an example of the configuration of this controller C.

[0028] The first mode, joint velocity-based joint angle control, involves joint angle deviation → δθ ｑ Obtain the joint angle deviation → δθ ｑ The equivalent wire displacement →δl is determined based on [Equation 2]. Then, the motor rotation angle deviation →δθ is equivalent to the wire displacement →δl. Ｍ This is calculated based on [Mathematics B], and further, the motor rotation angle deviation → δθ Ｍ Compensating motor command torque → τ Ｍ The control is executed by calculating [the value].

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

[0030] Referring to Figure 3, the details of the configuration of controller C will be explained. 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). ｑ The programmable controller 72 calculates this joint angle deviation → δθ. ｑ 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). Ｍ 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 →τ ｑ , wire tension → F ｌ If we set it as follows, then by the principle of virtual work, the following equation holds:

[0035]

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

[0037]

[0038] This is wire tension → F ｌ From joint torque → τ ｑ This indicates that it is uniquely determined. However, joint torque → τ ｑ From wire tension → F ｌ 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 → τ ｑ In contrast, wire tension → F ｌ 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 ｌ This can be calculated. The calculated wire tension → F ｌ By multiplying this by the pulley radius, the motor command torque can be calculated from the joint command torque.

[0039] Next, the first mode, joint velocity-based joint angle control, will be described. By repeatedly executing this first mode and the second mode at high speed over time, 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 difference in speed, 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 ｐｐ _K ｖｐ _K ｖｉ 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 ｍａｘ The lower limit is Int ｍｉｎ 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 back drive 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 feeding 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 (for example, motors 40-2 and 40-3) generate torque in the feeding direction (negative direction).

[0044] Referring to Figure 11, the effects of rapidly and repeatedly executing the first mode (joint velocity-based joint angle control) and the second mode (joint torque-based joint angle control) 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 required that (1) each joint in the wire drive system exerts the necessary 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 mode and the second mode 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, a wire-driven joint mechanism control device according to the second embodiment will be described with reference to Figures 6 to 7. The joint mechanism control device of this 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 of the two joints, joint 20A 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, and at that 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, a large tension (the maximum tension that can be exerted if integral control is working) may be applied to at least one wire Wa of the wires W that drive the joint 20A 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 joint velocity-based joint angle control is performed, 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 and 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. ｑ Accordingly, the torque that motors M1 to M3 exert on wires W1 to W3 is →τ Ｍ The torque τ at joint 20A is being calculated. Ｊ１ If you want to exert the desired effect, the torque of joint 20A by motor M1 should be τ Ｊ１Ｍ１ , the torque of joint 20A by motor M2 is τ Ｊ１Ｍ２ Therefore, torque τ Ｊ１ =τ Ｊ１Ｍ１ -τ Ｊ１Ｍ２ This is the result.

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

[0057] At joint 20B, this tension f Ｍ１ and the tension f due to motor M2 Ｍ２ Motor M3 needs to pull wire W3 with a tension exceeding τ, 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 (τ Ｊ１Ｍ１ =τ Ｊ１Ｍ２ = 0) As a result, f Ｍ１ = f Ｍ２ Since this becomes 0, motor M3 will be able to 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 the joint torque to zero, 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 contents of the first mode and the second mode differ from those 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 → δθ ｑ The Pulley matrix Tw (=R Ｍ －１ The motor angle deviation obtained by multiplying by ×T is →δθ Ｍ 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 for the purpose of explaining this disclosure clearly, 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. Furthermore, 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.

[0065] 10...Link 20...Joint 30...Pulley 40...Motor 50...Base 71...Differential calculator 72...Programmable controller C...Controller JM...Joint mechanism W...Wire

Claims

1. A joint mechanism control device comprising: 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, wherein the control unit alternately executes in time a first mode that drives the first actuator and the second actuator to rotate the joint, and a second mode that drives the first actuator and the second actuator to prevent the first wire and the second wire from loosening.

2. The joint mechanism control device according to claim 1, wherein 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 a commanded angle, and the 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 is at a 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 joint mechanism control device according to claim 1, wherein the first mode is a mode in which the wire length required to be wound up or fed out 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 tension of the first wire and the second wire related to driving the joint to be driven is controlled so that the joint torque becomes zero without driving the joint.

5. The joint mechanism control device according to claim 4, 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.

6. The joint mechanism control device according to claim 1, wherein the first mode is a mode in which the wire length required to be wound up or fed out 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 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.

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 joint mechanism control device according to claim 7, wherein the joint mechanism has a plurality of joints, and the control unit changes the components of at least some of the columns of the pulley matrix to zero.

Images