JP4505299B2 - Control device and control method for movable mechanism driven by elastic actuator - Google Patents

Description

  The present invention relates to a control device and a control method for an elastic actuator-driven movable mechanism that controls a movable mechanism driven by an elastic actuator that operates by deformation of an elastic body, such as a fluid pressure drive actuator.

  In recent years, home robots such as pet robots have been actively developed, and it is expected that more practical home robots such as housework support robots will be put to practical use in the future. Since the home robot needs to enter the home and live together with humans, the required specifications differ from those of conventional industrial robots.

  In an industrial robot, an electric motor and a speed reducer are used to drive a drive mechanism, and high hand position accuracy such as a repeat accuracy of 0.1 mm is realized by high gain feedback control. However, the movable mechanism driven by such an electric motor has high rigidity and often lacks softness, and has many problems in terms of safety.

  On the other hand, a home robot does not necessarily require a high accuracy such as a repeat accuracy of 0.1 mm, and safety is emphasized such that no harm is caused when it comes into contact with a human. Therefore, a movable mechanism driven by an electric motor like a conventional industrial robot is not suitable for a field where safety is important such as a home robot, and a flexible and safe robot arm is required. ing.

  In response to such a problem, for example, a robot arm using a McKibben type pneumatic actuator has been proposed. The Macchiben type pneumatic actuator has a structure in which a restraining means made of a fiber cord is disposed on the outer surface of a tubular elastic body made of a rubber material, and both ends of the tubular elastic body are hermetically sealed with a sealing member. It has become. When an internal pressure is applied to the inner space of the tubular elastic body by a compressive fluid such as air through the fluid injecting / extracting means, the tubular elastic body tends to expand mainly in the radial direction, but the center of the tubular elastic body is caused by the action of the restraining means. It is converted into axial motion and the entire length contracts. Since this McKibben type actuator is mainly composed of an elastic body, it has the characteristics of being a flexible, safe and lightweight actuator.

  Furthermore, this McKibben type actuator has a large output weight ratio, and is characterized by high output while being lightweight. Therefore, when driving a movable mechanism such as a robot arm, it is possible to drive directly by a link mechanism or the like without using a speed reduction mechanism, and there is no rigidity of the joint that comes from the presence of a speed reducer, Combined with the flexibility of the actuator, a flexible movable mechanism can be realized.

  However, fluid actuators that operate with fluid pressure such as air, such as McKibben actuators, have poor responsiveness due to the effects of elastic properties due to fluid compressibility and flow resistance. I have a difficult task.

  To deal with such problems, as a conventional technique, Patent Document 1 discloses a control device that can draw a desired trajectory by providing a delay circuit for a robot arm that is driven by a combination of a servo motor and a fluid pressure drive actuator. is doing.

  In addition, when driven directly by a link mechanism or the like without using a speed reduction mechanism, the influence of gravity, inertial force, centrifugal force, Coriolis force, etc. acting on the structural material such as the arm cannot be ignored and control accuracy deteriorates. Therefore, it is necessary to control by a control method in consideration of dynamics such as a calculation torque method, and further, it is necessary to perform joint torque feedback control or the like in order to realize an accurate joint torque.

  For such problems, as a conventional technique, in Patent Document 2, a torque control value calculated based on a torque deviation that is a difference between a torque target value and a current torque value is used for a robot arm driven by a motor. Control that enables the robot arm to perform the desired motion by realizing torque feedback control that supplies the motor with a command value obtained by subtracting the value based on the current angular velocity of the motor from the motor. An apparatus is disclosed.

Japanese Patent Publication No. 2583272 Japanese Patent Publication No. 3324298

  However, in the control device including the delay circuit disclosed in Patent Document 1, a delay with respect to the target operation always occurs, so that the responsiveness is poor and it is impossible to execute a work that requires real-time performance. In addition, the effect is exhibited only in the case of a combination of a servo motor and a fluid drive actuator, and the effect cannot be exhibited in a robot arm constituted only by a fluid drive actuator.

  Further, the control device including the torque feedback control system disclosed in Patent Document 2 is effective for a robot arm driven by a motor, and as it is, it can be applied to a robot arm driven by an elastic actuator. Can not.

  Therefore, the object of the present invention is to solve the above-mentioned conventional problems, and to move a movable mechanism such as a robot arm driven by an elastic actuator with good responsiveness and without the influence of gravity, inertial force, centrifugal force, Coriolis force, etc. An object of the present invention is to provide a control device and control method for an elastic actuator-driven movable mechanism capable of controlling the position and force with high accuracy.

  In order to achieve the above object, the present invention is configured as follows.

According to a first aspect of the present invention, there is provided a control device for an elastic actuator-driven movable mechanism that is antagonistically driven by a set of elastic actuators,
An internal state measuring means for measuring an internal state of the elastic body actuator that is changed by driving the elastic body actuator and outputting a measurement value of the internal state;
Driving force measuring means for measuring the driving force generated by the elastic actuator and outputting the measured value of the driving force;
Output measuring means for measuring the output of the movable mechanism driven by the elastic actuator and outputting the measured value of the output;
The target value of the output of the movable mechanism driven by the elastic actuator and the measured value of the output measured by the output measuring means are input and output error compensation information is output so as to compensate the output error. Output error compensation means;
The output of the output error compensation information from the output error compensation means and the output of the measurement value of the driving force from the driving force measurement means are input and the driving force error compensation information is output so as to compensate for the driving force error. Driving force error compensation means for
Target internal state determining means for determining a target value of the internal state of the elastic actuator and outputting the target value of the internal state;
The output of the driving force error compensation information from the driving force error compensation unit, the output of the target value of the internal state from the target internal state determination unit, and the measurement value of the internal state from the internal state measurement unit An internal state error compensation means for outputting internal state error compensation information so as to compensate for the internal state error as the output is input;
An elastic actuator that controls the measured value of the output of the movable mechanism driven by the elastic actuator based on the internal state error compensation information output by the internal state error compensation means to be a target value of the output Provided is a control device for a drive-type movable mechanism.

According to a second aspect of the present invention, there is provided a control device for an elastic actuator-driven movable mechanism that is antagonistically driven by a set of elastic actuators,
An internal state measuring means for measuring an internal state of the elastic body actuator that is changed by driving the elastic body actuator and outputting a measurement value of the internal state;
Driving force measuring means for measuring the driving force generated by the elastic actuator and outputting the measured value of the driving force;
Output measuring means for measuring the output of the movable mechanism driven by the elastic actuator and outputting the measured value of the output;
The target value of the output of the movable mechanism driven by the elastic actuator and the measured value of the output measured by the output measuring means are input and output error compensation information is output so as to compensate the output error. Output error compensation means;
The output of the output error compensation information from the output error compensation means and the output of the measurement value of the driving force from the driving force measurement means are input and the driving force error compensation information is output so as to compensate for the driving force error. Driving force error compensation means for
Target internal state determining means for determining a target value of the internal state of the elastic actuator and outputting the target value of the internal state;
Internal state error compensation so that the output of the target value of the internal state from the target internal state determination means and the output of the measurement value of the internal state from the internal state measurement means are input and the internal state error is compensated. An internal state error compensation means for outputting information,
The output of the movable mechanism driven by the elastic actuator based on the internal state error compensation information output by the internal state error compensation unit and the driving force error compensation information compensated by the driving force error compensation unit. Provided is a control device for an elastic actuator-driven movable mechanism that controls a measured value to be a target value of the output.

According to a ninth aspect of the present invention, there is provided a method for controlling an elastic actuator-driven movable mechanism that is antagonistically driven by a set of elastic actuators,
Measure the internal state of the elastic actuator that changes due to the drive of the elastic actuator to obtain the measured value of the internal state,
Measure the driving force generated by the elastic actuator to obtain the measured value of the driving force,
Measure the output of the movable mechanism driven by the elastic body actuator to obtain the measured value of the output,
Obtaining output error compensation information so as to compensate the output error from the target value of the output of the movable mechanism driven by the elastic actuator and the measured value of the measured output,
Obtaining the driving force error compensation information so as to compensate the driving force error from the output of the output error compensation information and the output of the measurement value of the driving force,
Determining the target value of the internal state of the elastic actuator to obtain the target value of the internal state;
The internal state error compensation information is obtained so as to compensate the internal state error from the output of the driving force error compensation information, the output of the target value of the internal state, and the output of the measurement value of the internal state,
Provided is a control method for an elastic actuator-driven movable mechanism that controls the measured value of the output of the movable mechanism driven by the elastic actuator based on the internal state error compensation information to be a target value of the output. .

According to a tenth aspect of the present invention, there is provided a method for controlling an elastic actuator-driven movable mechanism that is antagonistically driven by a set of elastic actuators,
Measure the internal state of the elastic actuator that changes due to the drive of the elastic actuator to obtain the measured value of the internal state,
Measure the driving force generated by the elastic actuator to obtain the measured value of the driving force,
Measure the output of the movable mechanism driven by the elastic body actuator to obtain the measured value of the output,
Obtaining output error compensation information so as to compensate the output error from the target value of the output of the movable mechanism driven by the elastic actuator and the measured value of the output,
Obtaining the driving force error compensation information so as to compensate the driving force error from the output of the output error compensation information and the output of the measurement value of the driving force,
Determining the target value of the internal state of the elastic actuator to obtain the target value of the internal state;
Obtain internal state error compensation information so as to compensate the internal state error from the output of the target value of the internal state and the output of the measured value of the internal state,
Based on the internal state error compensation information and the driving force error compensation information, an elastic actuator drive type movable that controls the measured value of the output of the movable mechanism driven by the elastic actuator to be the target value of the output. A mechanism control method is provided.

Further, according to the control device of the first aspect of the present invention, a driving force error compensation means is disposed to constitute a driving force feedback control system that feeds back the driving force generated by the elastic actuator, and An internal state feedback control system that provides internal state error compensation means to feed back the internal state of the elastic actuator is configured inside the driving force feedback control system, and target internal state determination means is provided. Thus, by feeding forward the target internal state, it is possible to control a highly accurate elastic actuator driven movable mechanism with good responsiveness, less dynamic influence, and small steady-state deviation.

Further, according to the control device of the second aspect of the present invention, a driving force error compensation means is provided to constitute a driving force feedback control system that feeds back the driving force generated by the elastic actuator, and An internal state feedback control system that feeds back the internal state of the elastic actuator is configured independently of the driving force feedback control system, and a target internal state determination unit is disposed. Thus, by feeding forward the target internal state, it is possible to control a highly accurate elastic actuator driven movable mechanism with good responsiveness, little dynamic influence, and small steady-state deviation.

According to the control method of the ninth aspect of the present invention, a driving force error compensation means is provided to constitute a driving force feedback control system that feeds back the driving force generated by the elastic actuator, and An internal state feedback control system that provides internal state error compensation means to feed back the internal state of the elastic actuator is configured inside the driving force feedback control system, and target internal state determination means is provided. Thus, by feeding forward the target internal state, it is possible to control a highly accurate elastic actuator driven movable mechanism with good responsiveness, less dynamic influence, and small steady-state deviation.

Further, according to the control method of the tenth aspect of the present invention, a driving force error compensation means is arranged to constitute a driving force feedback control system that feeds back the driving force generated by the elastic actuator, and An internal state feedback control system that feeds back the internal state of the elastic actuator is configured independently of the driving force feedback control system, and a target internal state determination unit is disposed. Thus, by feeding forward the target internal state, it is possible to control a highly accurate elastic actuator driven movable mechanism with good responsiveness, little dynamic influence, and small steady-state deviation.

  Before describing embodiments of the present invention, various aspects of the present invention will be described first.

According to a first aspect of the present invention, there is provided a control device for an elastic actuator-driven movable mechanism that is antagonistically driven by a set of elastic actuators,
An internal state measuring means for measuring an internal state of the elastic body actuator that is changed by driving the elastic body actuator and outputting a measurement value of the internal state;
Driving force measuring means for measuring the driving force generated by the elastic actuator and outputting the measured value of the driving force;
Output measuring means for measuring the output of the movable mechanism driven by the elastic actuator and outputting the measured value of the output;
The target value of the output of the movable mechanism driven by the elastic actuator and the measured value of the output measured by the output measuring means are input and output error compensation information is output so as to compensate the output error. Output error compensation means;
The output of the output error compensation information from the output error compensation means and the output of the measurement value of the driving force from the driving force measurement means are input and the driving force error compensation information is output so as to compensate for the driving force error. Driving force error compensation means for
Target internal state determining means for determining a target value of the internal state of the elastic actuator and outputting the target value of the internal state;
The output of the driving force error compensation information from the driving force error compensation unit, the output of the target value of the internal state from the target internal state determination unit, and the measurement value of the internal state from the internal state measurement unit An internal state error compensation means for outputting internal state error compensation information so as to compensate for the internal state error as the output is input;
An elastic actuator that controls the measured value of the output of the movable mechanism driven by the elastic actuator based on the internal state error compensation information output by the internal state error compensation means to be a target value of the output Provided is a control device for a drive-type movable mechanism.

According to a second aspect of the present invention, there is provided a control device for an elastic actuator-driven movable mechanism that is antagonistically driven by a set of elastic actuators,
An internal state measuring means for measuring an internal state of the elastic body actuator that is changed by driving the elastic body actuator and outputting a measurement value of the internal state;
Driving force measuring means for measuring the driving force generated by the elastic actuator and outputting the measured value of the driving force;
Output measuring means for measuring the output of the movable mechanism driven by the elastic actuator and outputting the measured value of the output;
The target value of the output of the movable mechanism driven by the elastic actuator and the measured value of the output measured by the output measuring means are input and output error compensation information is output so as to compensate the output error. Output error compensation means;
The output of the output error compensation information from the output error compensation means and the output of the measurement value of the driving force from the driving force measurement means are input and the driving force error compensation information is output so as to compensate for the driving force error. Driving force error compensation means for
Target internal state determining means for determining a target value of the internal state of the elastic actuator and outputting the target value of the internal state;
Internal state error compensation so that the output of the target value of the internal state from the target internal state determination means and the output of the measurement value of the internal state from the internal state measurement means are input and the internal state error is compensated. An internal state error compensation means for outputting information,
The output of the movable mechanism driven by the elastic actuator based on the internal state error compensation information output by the internal state error compensation unit and the driving force error compensation information compensated by the driving force error compensation unit. Provided is a control device for an elastic actuator-driven movable mechanism that controls a measured value to be a target value of the output.

According to a third aspect of the present invention, in the first or second aspect, the target internal state determining means determines the target value of the internal state while receiving the target value of the output. Provided is a control device for an elastic actuator driving movable mechanism.

According to a fourth aspect of the present invention, the target internal state determination means approximates a relationship between the output of the elastic actuator and the internal state of the elastic actuator by a polynomial, and the elastic actuator by the polynomial The elastic actuator-driven movable mechanism control device according to the first or second aspect, wherein the target value of the internal state of the elastic actuator is calculated and determined from the target value of the output. To do.

According to a fifth aspect of the present invention, the target internal state determination means further includes a memory that stores a relationship between the output of the elastic actuator and the internal state of the elastic actuator as a table, The elastic actuator-driven movable mechanism control device according to the first or second aspect, wherein a target value of the internal state of the elastic actuator is determined from a target value of the output of the elastic actuator. provide.

According to a sixth aspect of the present invention, the elastic actuator is a fluid pressure driven actuator driven by fluid pressure. Control of the elastic actuator driven movable mechanism according to the first or second aspect Providing equipment.

According to the seventh aspect of the present invention, the fluid pressure driven actuator includes a hollow elastic body, a pair of sealing members that hermetically seal the hollow elastic body, and a hollow interior of the hollow elastic body. An elastic actuator-driven movable mechanism control device according to a sixth aspect is provided, which is an elastic expansion / contraction structure having a fluid passage member capable of injecting or dispensing fluid.

According to an eighth aspect of the present invention, the internal state of the elastic actuator is a fluid pressure, and the internal state measuring means is a pressure measuring means for measuring the fluid pressure of the elastic actuator. A control device for an elastic actuator-driven movable mechanism according to a sixth aspect is provided.

According to a ninth aspect of the present invention, there is provided a method for controlling an elastic actuator-driven movable mechanism that is antagonistically driven by a set of elastic actuators,
Measure the internal state of the elastic actuator that changes due to the drive of the elastic actuator to obtain the measured value of the internal state,
Measure the driving force generated by the elastic actuator to obtain the measured value of the driving force,
Measure the output of the movable mechanism driven by the elastic body actuator to obtain the measured value of the output,
Obtaining output error compensation information so as to compensate the output error from the target value of the output of the movable mechanism driven by the elastic actuator and the measured value of the measured output,
Obtaining the driving force error compensation information so as to compensate the driving force error from the output of the output error compensation information and the output of the measurement value of the driving force,
Determining the target value of the internal state of the elastic actuator to obtain the target value of the internal state;
The internal state error compensation information is obtained so as to compensate the internal state error from the output of the driving force error compensation information, the output of the target value of the internal state, and the output of the measurement value of the internal state,
Provided is a control method for an elastic actuator-driven movable mechanism that controls the measured value of the output of the movable mechanism driven by the elastic actuator based on the internal state error compensation information to be a target value of the output. .

According to a tenth aspect of the present invention, there is provided a method for controlling an elastic actuator-driven movable mechanism that is antagonistically driven by a set of elastic actuators,
Measure the internal state of the elastic actuator that changes due to the drive of the elastic actuator to obtain the measured value of the internal state,
Measure the driving force generated by the elastic actuator to obtain the measured value of the driving force,
Measure the output of the movable mechanism driven by the elastic body actuator to obtain the measured value of the output,
Obtaining output error compensation information so as to compensate the output error from the target value of the output of the movable mechanism driven by the elastic actuator and the measured value of the output,
Obtaining the driving force error compensation information so as to compensate the driving force error from the output of the output error compensation information and the output of the measurement value of the driving force,
Determining the target value of the internal state of the elastic actuator to obtain the target value of the internal state;
Obtain internal state error compensation information so as to compensate the internal state error from the output of the target value of the internal state and the output of the measured value of the internal state,
Based on the internal state error compensation information and the driving force error compensation information, an elastic actuator drive type movable that controls the measured value of the output of the movable mechanism driven by the elastic actuator to be the target value of the output. A mechanism control method is provided.

Embodiments according to the present invention will be described below in detail with reference to the drawings.
(First embodiment)
FIG. 1 is a block diagram showing the concept of a control device for an elastic actuator-driven movable mechanism according to a first embodiment of the present invention. In FIG. 1, reference numeral 101 denotes target output generation means, which outputs target values of outputs such as displacement and generated force of an elastic actuator driving movable mechanism 102 driven by an elastic actuator composed of the elastic expansion / contraction structure 1. Generate. Reference numeral 104 denotes output measuring means connected to the elastic actuator-driven movable mechanism 102, which measures the output value of the elastic actuator-driven movable mechanism 102 and inputs the measured value to the output error compensating means 103. . Reference numeral 103 denotes an output error compensation unit to which a target value from the target output generation unit 101 is input so that the measured value of the output of the elastic actuator driving movable mechanism 102 measured by the output measuring unit 104 follows the target value. To control. Reference numeral 105 denotes driving force error compensation means for outputting driving force error compensation information so as to compensate for the driving force error generated by the elastic actuator. The output of the output error compensation means 103 and the elastic actuator driving movable mechanism 102 are output. The difference from the driving force measured by the driving force measuring means 107 connected to is input. Reference numeral 106 denotes an internal state error compensation unit that receives the difference between the output information of the driving force error compensation information from the driving force error compensation unit 105 and the internal state measurement value from the internal state measurement unit 108 and outputs the internal state error compensation information. Yes, output information of the internal state error compensation information from the internal state error compensation means 106 is input to each elastic actuator 1 so that the internal state measurement value of each elastic actuator 1 follows each target value. Take control. Reference numeral 108 denotes an internal state measuring means connected to the elastic expansion / contraction structure 1 which is each elastic actuator constituting the elastic actuator driving movable mechanism 102, and each elastic expansion / contraction structure of the elastic actuator driving movable mechanism 102. The internal state measurement values that are internal pressures of the body 1 are measured, and the internal state measurement values are input to the internal state error compensation means 106.

  Next, regarding a specific example of the control device for the elastic actuator driving movable mechanism 102 according to the first embodiment, a control device for the robot arm 10 which is an example of the elastic actuator driving movable mechanism 102 is taken as an example of control. Take and explain.

  FIG. 2 is a diagram illustrating a configuration of the robot arm 10 to be controlled by the control device of the elastic actuator driving movable mechanism 102 according to the first embodiment of the present invention. In FIG. 2, 1-1a, 1-1b, 1-2a, 1-2b, 1-3a, 1-3b, 1-4a, 1-4b, 1-5a, 1-5b, 1-6a, 1- Reference numeral 6b (these are reference numerals for individual elastic expansion / contraction structures, which are typically indicated by reference numeral 1 when referring to the elastic expansion / contraction structure) is an elastic expansion / contraction structure. As shown in FIG. 3, the elastic expansion / contraction structure 1 is formed of a resin or metal fiber cord which is made of a rubber material and functions as a driving portion on the outer surface of a tubular hollow elastic body 2 which is difficult to stretch. The radial deformation due to the expansion of the tubular elastic body 2 is converted into the contraction of the axial length, while the radial deformation due to the contraction of the tubular elastic body 2 becomes the expansion of the axial length. A deformation direction restricting member 3 configured to be converted is disposed, and both ends of the tubular elastic body 2 are hermetically sealed with a sealing member 4. A compressive fluid such as air is passed through a tubular fluid passage member 5 having a fluid flow path through which the compressive fluid passes and capable of injecting or dispensing the fluid into the hollow interior of the elastic body 2. When the internal pressure is supplied to the inner space of the tubular elastic body 2 by the supplied compressive fluid, the tubular elastic body 2 tends to expand mainly in the radial direction, but the deformation of the deformation direction regulating member 3 causes the tubular elasticity. Since it is converted into a motion in the direction of the central axis of the body 2 and the entire length contracts, it can be used as an elastic actuator-driven movable mechanism 102 of linear motion drive.

  In the robot arm 10 of FIG. 2, one set of the elastic expansion / contraction structures 1, 1 are arranged so that the joint shaft faces the fulcrum, and either one of the pair of elastic expansion / contraction structures 1, 1 is arranged. When the elastic expansion / contraction structure 1 is contracted and the other elastic expansion / contraction structure 1 is expanded, a force is applied via a fulcrum to rotate the joint axis, whereby the joint axis rotates. Forward / reverse rotational motion can be realized. Specifically, the elastic expansion / contraction structure 1-1a and the elastic expansion / contraction structure 1-1b (the elastic expansion / contraction structure 1-1b is not shown because it is located behind the elastic expansion / contraction structure 1-1a). ), The first joint shaft 6-1 is rotated forward and reverse, and the second joint shaft 6-2 is rotated forward and backward by the antagonistic drive of the elastic expansion / contraction structure 1-2a and the elastic expansion / contraction structure 1-2b. The third joint shaft 6-3 is driven to rotate forward and backward by antagonistic driving of the elastic expansion / contraction structure 1-3a and the elastic expansion / contraction structure 1-3b, and the elastic expansion / contraction structure 1-4a is elastically expanded. The fourth joint shaft 6-4 is driven to rotate forward and backward by the antagonistic drive of the contraction structure 1-4b, and the fifth joint shaft 6 is driven by the antagonistic drive of the elastic expansion / contraction structure 1-5a and the elastic expansion / contraction structure 1-5b. -5 is rotated in forward and reverse directions, and an elastic expansion / contraction structure 1-6a and an elastic expansion / contraction structure The antagonistic driving of -6b has a structure in which the sixth joint axis 6-6 drives forward and reverse rotation.

  9-1a, 9-1b, 9-2a, 9-2b, 9-3a, 9-3b, 9-4a, 9-4b, 9-5a, 9-5b, 9-6a, 9-6b in FIG. Are elastic expansion and contraction structures 1-1a, 1-1b, 1-2a, 1-2b, 1-3a, 1-3b, 1-4a, 1-4b, 1-5a, 1-5b, 1-6a , 1-6b are pressure sensors that are examples of internal state measuring means, and are disposed at each fluid passage member 5 (fluid injection outlet) to measure the pressure in each elastic expansion / contraction structure.

  Specifically, the robot arm 10 is a six-degree-of-freedom robot arm, and a first joint shaft 6-1 that rotates forward and backward in a plane along the horizontal direction along the vertical axis with respect to the fixed wall 301. The third joint shaft 6-2 that rotates forward and backward in a plane along the vertical direction, and the third joint shaft 6-2 that rotates forward and backward between the second arm 308 and the first arm 311 in the plane along the vertical direction. A joint shaft 6-3, a fourth joint shaft 6-4 that rotates forward and backward in an axial direction orthogonal to the third joint shaft 6-3 between the second arm 308 and the first arm 311; A fifth joint shaft 6-5 that rotates forward and backward in a plane along the vertical direction between 311 and the hand 313, and a fifth joint shaft 6-5 between the first arm 311 and the hand 313. It is comprised from the 6th joint axis | shaft 6-6 which carries out forward / reverse rotation to the orthogonal | vertical axial direction.

  In the first joint shaft 6-1, the circular supports 302, 302 are rotatably connected to both sides of the rotating shaft 303 whose upper and lower ends are rotatably supported by bearings 304 and 305 and along the vertical direction, and Each end of the elastic expansion / contraction structure 1-1a and 1-1b (however, the elastic expansion / contraction structure 1-1b is not shown because it is disposed behind the elastic expansion / contraction structure 1-1a). While being connected to the fixed wall 301, each other end is connected to the support shaft 314 of each circular support 302. Therefore, the first arm 311 of the robot arm in the plane along the vertical axis Z around the rotation axis 303 of the first joint shaft 6-1 is driven by the antagonistic drive of the elastic expansion / contraction structures 1-1a and 1-1b. The second arm 308 and the hand 313 can be rotated forward and backward integrally. The upper bearing 305 is supported on the fixed wall 301 by a support bar 306.

  In the second joint shaft 6-2, two circular supports 302, 302 fixed on both sides of the rotating shaft 303 and fixed to the fixing wall 301 side of the rotating shaft 303 perpendicular to the longitudinal direction of the rotating shaft 303. The elastic expansion / contraction structures 1-2a and 1-2b are connected between the supports 307 and 307, and the second joint shaft 6 is driven by the antagonistic driving of the elastic expansion / contraction structures 1-2a and 1-2b. The first arm 311, the second arm 308, and the hand 313 of the robot arm 10 are integrally rotated forward and backward in the plane along the vertical direction around the horizontal axis of the support shaft 314 of −2.

  In the third joint shaft 6-3, the support members 309 and 309 are on the second arm link 308 on the circular support member 302 side of the second arm link 308 whose one end is fixed to the two circular support members 302 and 302. The first arm link 311 is fixed to one end of the first arm link 311 at a distal end side of the second arm link 308 and fixed to the first arm link 311 at a right angle. The body 310 is rotatably connected. Elastic expansion and contraction structures 1-3a and 1-3b are connected between the supports 309 and 309 of the second arm link 308 and the support 310 fixed to one end of the first arm link 311. Then, by the antagonistic drive of the elastic expansion / contraction structures 1-3a and 1-3b, the first arm 311 of the robot arm 10 and the first arm 311 of the robot arm 10 in the vertical direction around the horizontal axis of the support shaft 315 of the third joint shaft 6-3. The second arm 308 is rotated forward and backward relatively.

  In the fourth joint shaft 6-4, the second arm link 308, one end of which is fixed to the two circular supports 302, 302, is on the side of the circular support 302, and the support members 309, 309 and the second arm link 308 are connected. Supports 325 and 325 are respectively fixed orthogonal to the longitudinal direction, and an elastic expansion / contraction structure is provided between the supportes 325 and 325 and the support 310 fixed to one end of the first arm link 311. The bodies 1-4a and 1-4b are connected, and the robot rotates around the fourth joint axis 6-4 orthogonal to the third joint axis 6-3 by the antagonistic drive of the elastic expansion / contraction structures 1-4a and 1-4b. The first arm 311 and the second arm 308 of the arm 10 are rotated forward and backward relatively.

  In the fifth joint shaft 6-5, there is an elastic expansion / contraction between the support 310 of the first arm 311 and the support 312 fixed to one end of the hand 313 and rotatably connected to the first arm 311. The structures 1-5a and 1-5b are connected, and the elastic expansion and contraction structures 1-5a and 1-5b are driven in an antagonistic manner so as to be along the horizontal axis of the support shaft 326 of the fifth joint shaft 6-5 along the vertical direction. The hand 313 is rotated forward and backward with respect to the first arm 311 in the plane.

  In the sixth joint shaft 6-6, there is an elastic expansion / contraction between the support 310 of the first arm 311 and the support 312 fixed to one end of the hand 313 and rotatably connected to the first arm 311. The elastic expansion / contraction structures 1-6a and 1-6b are coupled with the structures 1-5a and 1-5b different in phase by 90 degrees, and the elastic expansion / contraction structures 1-6a and 1-6b are antagonistically driven. Thus, the hand 313 is rotated forward and backward with respect to the first arm 311 around the sixth joint axis 6-6 orthogonal to the fifth joint axis 6-5.

  Elastic expansion / contraction structures 1-1a and 1-1b, elastic expansion / contraction structures 1-2a and 1-2b, elastic expansion / contraction structures 1-3a and 1-3b, elastic expansion / contraction structures 1-4a and 1- 4b, elastic expansion and contraction structures 1-5a and 1-5b, and elastic expansion and contraction structures 1-6a and 1-6b are connected to a flow proportional solenoid valve 18 as will be described later. The electromagnetic valve 18 is connected to the control computer 19, and under the control of the control computer 19, the elastic expansion / contraction structures 1-1a and 1-1b, the elastic expansion / contraction structure 1-2a, and 1-2b, elastic expansion / contraction structures 1-3a and 1-3b, elastic expansion / contraction structures 1-4a and 1-4b, elastic expansion / contraction structures 1-5a and 1-5b, elastic expansion / contraction structures 1- 6a and 1-6b To control the respective contraction and expansion behavior. Each joint shaft has encoders 8-1, 8-2, 8-3, 8-4, 8-5, 8-6 as examples of displacement measuring means, which are examples of output measuring means, and driving force measurement. Torque sensors 7-1, 7-2, 7-3, 7-4, 7-5, and 7-6 as examples of drive torque measuring means that are examples of the means are arranged, and an encoder 8 (encoder 8 -1, 8-2, 8-3, 8-4, 8-5, and 8-6), the joint angle of the joint shaft is determined by the torque sensor 7 (torque sensors 7-1, 7-2, 7- 3, 7-4, 7-5, 7-6)), the driving torque generated by the antagonistic driving of the elastic expansion / contraction structure can be measured. Each elastic expansion / contraction structure 1 includes a pressure sensor 9 (pressure sensors 9-1a, 9-1b, 9-2a, 9-2b, 9) as an example of pressure measuring means which is an example of the internal state measuring means 107. -3a, 9-3b, 9-4a, 9-4b, 9-5a, 9-5b, 9-6a, 9-6b. The internal pressure (an example of the internal state) of the elastic expansion / contraction structure 1 that changes as the body 1 is driven can be measured.

  With the structure as described above, it is possible to realize basic functions as the robot arm 10 such as grasping and transporting an object by making use of multiple degrees of freedom.

  FIG. 4 is a diagram showing a configuration of an air pressure supply system for driving the robot arm 10 to be controlled by the control device of the elastic actuator drive type movable mechanism according to the first embodiment of the present invention. In FIG. 4, only the portion for driving the third joint axis of the robot arm 10 to rotate forward and reverse is shown, and the other portions are omitted. In FIG. 4, 16 is an air pressure source such as a compressor, and 17 is an air pressure adjusting unit in which an air pressure filter 17a, an air pressure reducing valve 17b, and an air pressure lubricator 17c are combined. Reference numeral 18 denotes a 5-port flow rate control electromagnetic valve that controls the flow rate by driving a spool valve or the like with the force of an electromagnet, for example. Reference numeral 19 denotes a control computer constituted by, for example, a general personal computer as an example of a control unit, which is equipped with a D / A board 20 and outputs a voltage command value to the 5-port flow control solenoid valve 18. The flow rate of each air flowing through each fluid passage member 5 can be controlled independently. In addition, the control computer 19 is equipped with a memory 19a for storing an operation program for the robot arm 10 in advance.

  Next, the operation of the air pressure supply system shown in FIG. 4 will be described. The high-pressure air generated by the air pressure source 16 is depressurized by the air pressure adjusting unit 17, adjusted to a constant pressure of, for example, 600 kPa, and supplied to the 5-port flow rate control electromagnetic valve 18. The opening degree of the 5-port flow rate control electromagnetic valve 18 is controlled in proportion to the voltage command value output from the control computer 19 via the D / A board 20. When a positive voltage command value is input from the control computer 19 to the 5-port flow rate control solenoid valve 18, the state indicated by A of the pneumatic circuit symbol is entered, and the elastic expansion / contraction structure 1-3a from the pneumatic pressure source 16 side. The flow path to the side is opened, and air having a flow rate proportional to the absolute value of the voltage command value is supplied to the elastic expansion / contraction structure 1-3a side. On the elastic expansion / contraction structure 1-3b side, the flow path to the atmospheric pressure side is opened, and an air flow having a flow rate proportional to the absolute value of the voltage command value is generated in the atmosphere from the elastic expansion / contraction structure 1-3b side. Is exhausted. Therefore, as shown in FIG. 4, the entire length of the elastic expansion / contraction structure 1-3a is shortened and the total length of the elastic expansion / contraction structure 1-3b is extended, so that the third speed is proportional to the absolute value of the voltage command value. The joint shaft 6-3 performs a right rotational motion. On the other hand, when a negative voltage command value is input from the control computer 19 to the 5-port flow rate control solenoid valve 18, the state indicated by B of the pneumatic circuit symbol is obtained, and the operation of the elastic expansion / contraction structure is reversed ( That is, when the entire length of the elastic expansion / contraction structure 1-3a is extended and the total length of the elastic expansion / contraction structure 1-3b is contracted), the third joint shaft 6-3 performs a counterclockwise rotation.

  That is, the air flow supplied from the five-port flow control electromagnetic valve 18 to the elastic expansion / contraction structure 1 side passes through the sealing member 4 by the fluid passage member 5, reaches the inside of the tubular elastic body 2, and has a tubular elasticity. The internal pressure of the body 2 is generated. Although the tubular elastic body 2 expands due to the generated internal pressure, the deformation in the radial direction due to the expansion is restricted by the restraining action (regulation action) of the fiber cords assembled in a mesh shape of the deformation direction restricting member 3, and the axial length is increased. The total length of the elastic expansion / contraction structure 1 is shortened as shown in the lower side of FIG. On the other hand, if the air is exhausted from the five-port flow control solenoid valve 18 to the atmosphere and the internal pressure of the tubular elastic body 2 is reduced, the elastic force of the tubular elastic body 2 restores and the expansion is eliminated, so that the elastic expansion / contraction structure The entire length of the body 1 extends as shown on the upper side of FIG. As a result, in FIG. 3, if it is assumed that it is fixed at the right end, there is a difference in distance d at the left end of the tubular elastic body 2 due to the expansion and contraction. Therefore, the elastic expansion / contraction structure 1 in the first embodiment can function as a linear displacement actuator by controlling supply of air pressure. Since the expansion / contraction amount is approximately proportional to the internal pressure of the elastic expansion / contraction structure 1, the air supplied to the elastic expansion / contraction structure 1 by controlling the 5-port flow rate control electromagnetic valve 18 by the control computer 19 as shown in FIG. By controlling the flow rate, the entire length of the elastic expansion / contraction structure 1 can be controlled.

  In the robot arm 10 shown in FIG. 2, the antagonistic drive by the elastic expansion / contraction structures 1-1a and 1-1b, the antagonistic drive by the elastic expansion / contraction structures 1-2a and 1-2b, and the elastic expansion / contraction structure 1-3a Antagonistic driving by 1-3b, antagonistic driving by elastic expansion / contraction structures 1-4a and 1-4b, antagonistic driving by elastic expansion / contraction structures 1-5a and 1-5b, elastic expansion / contraction structures 1-6a and 1- For the antagonistic drive by 6b, as shown in FIG. 5, a 5-port flow rate control electromagnetic valve 18 is arranged for each elastic expansion / contraction structure 1 that antagonizes, and a similar air pressure supply system is configured. In accordance with voltage command values output from the control computer 19 to the respective 5-port flow rate control solenoid valves 18 via the D / A board 20, all joint axes of the robot arm 10 are rotated forward and backward simultaneously. It has become possible way.

FIG. 6 is a diagram showing a configuration of a control device for an elastic actuator driving movable mechanism according to the first embodiment of the present invention provided in the control computer 19, for example. However, in FIG. 6, reference numeral 10 denotes an object to be controlled by the control device for the elastic actuator drive type movable mechanism and an example of the elastic actuator drive type movable mechanism, which is the robot arm shown in FIG. From the robot arm 10, the current value (joint angle vector) q = [q ₁ , q ₂ , q ₃ , q ₄ , q ₅ , q ₆ ] ^T measured by the encoder 8 of each joint axis; A drive torque generated at a current value τ = [τ ₁ , τ ₂ , τ ₃ , τ ₄ , τ ₅ , τ ₆ ] ^T measured by a torque sensor 7 disposed on each joint axis; The internal pressure P = [P _1a , P _1b , P _2a , P _2b , P _3a , P _3b , P _4a , P _4b measured by the pressure sensor 9 of each elastic expansion / contraction structure 1 , P _5a , P _5b , P _6a , P _6b ] ^T. However, q ₁ , q ₂ , q ₃ , q ₄ , q ₅ , q ₆ are respectively the first joint axis 6-1, the second joint axis 6-2, the third joint axis 6-3, and the fourth joint. This is the joint angle of the shaft 6-4, the fifth joint shaft 6-5, and the sixth joint shaft 6-6. Further, τ ₁ , τ ₂ , τ ₃ , τ ₄ , τ ₅ , and τ ₆ are the first joint axis 6-1, the second joint axis 6-2, the third joint axis 6-3, and the fourth joint, respectively. This is a driving torque for rotating the shaft 6-4, the fifth joint shaft 6-5, and the sixth joint shaft 6-6. P _1a , P _1b , P _2a , P _2b , P _3a , P _3b , P _4a , P _4b , P _5a , P _5b , P _6a , and P _6b are elastic expansion / contraction structures 1-1a, 1 -1b, 1-2a, 1-2b, 1-3a, 1-3b, 1-4a, 1-4b, 1-5a, 1-5b, 1-6a, 1-6b.

Reference numeral 13 denotes a pressure difference calculation means for inputting the internal pressure P of the elastic expansion / contraction structure 1 measured by the pressure sensor 9 output from the robot arm 10, that is, the measurement value P. From the value P, the pressure difference ΔP = [ΔP ₁ , ΔP ₂ , ΔP ₃ , ΔP ₄ , ΔP ₅ , ΔP ₆ ] ^T = [P _1a −P _1b , P _2a −P _2b , P _3a −P _3b , P _4a -P _4b , P _5a -P _5b , P _6a -P _6b ] ^T is calculated by the pressure difference calculating means 13 and output from the pressure difference calculating means 13.

  Reference numeral 21 denotes a forward kinematic calculation means to which a joint angle vector q, which is a current value q of the joint angle measured by the encoder 8 of each joint axis output from the robot arm 10, is input. The forward kinematics calculation means 21 performs geometrical scientific calculation of conversion from the vector q to the hand position / posture vector r.

11 is a target track generation unit, hand position and orientation target vector r _d for realizing the operation of the robot arm 10 to the target is output from the target trajectory generation section 11.

As an example, the output error compensation unit 103 includes a position error compensation unit 12 and an inverse dynamics calculation unit 24 so as to compensate the output position error. The position error compensation unit 12 outputs the hand position / posture vector r calculated by the forward kinematics calculation unit 21 from the current value q of the joint angle vector measured by the robot arm 10 and the target trajectory generation unit 11. is the error r _e is input to the hand position and orientation target vector r _d, the position error correction output u _p output from the positional error compensating unit 12 toward the inverse dynamic calculation means 24. The inverse dynamics calculation unit 24, the equation based on the equation of motion of the robot arm represented by the following formula (1), the position error correction torque tau _p for correcting the hand position and orientation error r _e is computed, reverse Output from the dynamics calculation means 24 as output error compensation information.

ただし、Ｍ（ｑ）は慣性行列、

Where M (q) is the inertia matrix,

は遠心力とコリオリ力を表す項、ｇ（ｑ）は重力負荷を表す項、Ｊ_ｒ（ｑ）はヤコビ行列である。

Is a term representing centrifugal force and Coriolis force, g (q) is a term representing gravity load, and J _r (q) is a Jacobian matrix.

Reference numeral 25 denotes drive torque error compensation means, which is an example of the drive force error compensation means 105, and the position error correction torque τ _p output from the inverse dynamics calculation means 24 and the current value τ of torque measured by the torque sensor 7. Torque error τ _e, which is the difference between the torque error correction pressure difference ΔP _τ and the drive torque error compensation means 25, so that the torque error correction pressure difference ΔP _τ compensates for the drive torque error. Output as compensation information.

Reference numeral 15 denotes a pressure difference error compensation means, which is an example of the internal state error compensation means 106, and the current error output from the pressure difference calculation means 13 from the torque error correction pressure difference ΔP _τ output from the drive torque error compensation means 25. A value obtained by subtracting the pressure difference ΔP (pressure difference error ΔP _e ) is input to the pressure difference error compensation means 15, and the pressure difference error compensation means 15 outputs a pressure difference error correction output so as to correct the error in the internal pressure state. u is output toward the robot arm 10. The pressure difference error correction output u is given as a voltage command value to each 5-port flow rate control solenoid valve 18 via the D / A board 20, and each joint shaft 6 is driven to rotate forward and backward to operate the robot arm 10. To do.

  The principle of the control operation relating to the control device configured as described above will be described.

Basic control operation, the position error compensation means 12 by the hand position and orientation feedback control of the error r _e (position control). As the position error compensation means 12, for example, by using the PID compensator acts is controlled to the hand position and orientation error r _e is converged to 0, the operation of the robot arm 10 to the target is realized.

  In addition, since the inverse dynamics calculation means 24 is provided, dynamic influences such as inertia force and centrifugal force generated in the robot arm 10 are compensated, so that the robot arm 10 shown in FIG. Even when a direct drive driving method that does not use a speed reducer is used to drive the joint, it is possible to realize a highly accurate operation.

  However, in order for compensation of the dynamic influence by the inverse dynamics calculation means 24 to function effectively, a mechanism for generating a desired torque as a joint driving force by the elastic actuator 1 is necessary.

A means for coping with such a problem is torque feedback control by the drive torque error compensation means 25. A torque error τ _e that is the difference between the position error correction torque τ _p and the current torque value τ measured by the torque sensor 7 is input to the drive torque error compensation means 25, and the torque error τ _e becomes zero. Thus, the drive torque error compensation means 25 operates. That is, the drive torque error compensation means 25 accurately realizes the position error correction torque τ _p that is a torque necessary for correcting the position error, and the compensation of the dynamic influence by the inverse dynamics calculation means 24 is effectively performed. Function.

  However, when driven by an elastic actuator, for example, the actuator 1 operated by a fluid such as air shown in FIG. Therefore, the torque feedback control alone cannot be performed with high accuracy.

A means for dealing with such a problem is feedback control of the pressure difference ΔP by the pressure difference error compensating means 15. Because the pressure difference error compensation means 15 for the torque error correction output [Delta] P _tau inputted from driving torque error compensation unit 25 operates the pressure difference error compensation means 15 when the torque error tau _e occurs, the torque error tau _e 0 The pressure difference is controlled so as to converge. Therefore, by forming an internal pressure feedback loop that controls the pressure difference inside the torque feedback loop that performs torque control as in the control system shown in FIG. 6, the poor responsiveness is compensated, and the torque control performance is improved. Improvements can be realized.

  The actual operation steps of the control program based on the above principle will be described based on the flowchart of FIG.

  In step 1, joint angle data (joint variable vector or joint angle vector q) measured by the encoder 8 of each joint axis of the robot arm 10 is taken into the control device.

Next, in Step 2, the Jacobian matrix _{Jr and the} like necessary for kinematics calculation of the robot arm 10 are calculated by the inverse dynamics calculation means 24, and then in Step 3, the joint angle data (joint) from the robot arm 10 is calculated. The current hand position / posture vector r of the robot arm 10 is calculated by the forward kinematics calculation means 21 from the angle vector q) (processing in the forward kinematics calculation means 21).

Then, in step 4, based on the operation program of the robot arm 10 that has been previously stored in the memory 19a in the control computer 19 of the control apparatus, the hand position and orientation target vector r _d of the robot arm 10 by the target track calculation unit 11 Calculated.

Then, in step 5, the error r _e of the hand position and orientation is the difference between the hand position and orientation target vector r _d and the current hand position and orientation vector r is calculated, then, in step 6, the hand position and orientation error r _e from the position error correction output u _p is computed by the position error compensation means 12 (process in the positional error compensating unit 12).

As a specific example of the position error compensating means 12, a PID compensator can be considered. For PID compensator, in step 6, a value obtained by multiplying the proportional gain to the error r _e of the hand position and orientation, the value obtained by multiplying the derivative gain to the differential value of the error r _e of the hand position and orientation, and the hand position and orientation the sum of the three values of the integral gain by multiplying the integral value of the error r _e is the position error correction output u _p of. Control is performed so that the position error converges to 0 by appropriately adjusting three gains of proportionality, differentiation, and integration, which are constant diagonal matrices.

  Next, in step 7, the inertia matrix M (q), centrifugal force / Coriolis force term necessary for dynamic calculation

、重力項ｇ（ｑ）の計算が行われ、次いで、ステップ８で、上記式（１）を使い、位置誤差修正トルクτ_ｐが計算される（逆動力学計算手段２４での処理）。

Then, the gravity term g (q) is calculated, and then in step 8, the position error correction torque τ _p is calculated using the above equation (1) (processing in the inverse dynamics calculation means 24).

Next, at step 9, the torque error τ _e is calculated by subtracting the measured value τ of the torque sensor 7 from the position error correction torque τ _p .

Next, at step 10, the torque error correction pressure difference ΔP _τ is calculated from the torque error τ _e by the drive torque error compensation means 25 (processing by the drive torque error compensation means 25). As a specific example of the drive torque error compensation means 25, a PID compensator can be considered. In the case of the PID compensator, in step 10, the value obtained by multiplying the torque error τ _e by the proportional gain, the value obtained by multiplying the differential value of the torque error τ _{e by} the differential gain, and the integral value of the torque error τ _e are multiplied by the integral gain. The sum of the three values is the torque error correction pressure difference ΔP _τ .

  Next, at step 11, the internal pressure values of the elastic actuators 1 respectively measured by the pressure sensors 9, which are an example of the internal state measuring means 107, are taken into the control device, and the insides of the elastic actuators 1 that are antagonistically driven are stored. The current pressure difference ΔP between the pressures is calculated by the pressure difference calculating means 13.

Next, at step 12, the pressure difference error obtained by subtracting the current pressure difference ΔP calculated by the pressure difference calculation means 13 at step 11 from the torque error correction pressure difference ΔP _τ calculated by the drive torque error compensation means 25 at step 10. ΔP _e is calculated by the pressure difference error compensating means 15 (processing by the pressure difference error compensating means 15). Next, at step 13, the pressure difference error correction output u is calculated from the pressure difference error ΔP _e by the pressure difference error compensation means 15 (processing by the pressure difference error compensation means 15). As the pressure difference error compensation means 15, for example, a PID compensator can be considered.

  Next, at step 14, the pressure difference error correction output u is sent from the pressure difference error compensation means 15 to the robot arm 10, specifically through the D / A board 20 in the control computer 19 of the robot arm 10. Each of the flow control valves 18 is supplied to the valve 18 and changes the pressure in the elastic actuator 1, so that forward / reverse rotational movements of the joint axes of the robot arm 10 are generated.

  Control of the operation of the robot arm 10 is realized by repeatedly executing the above steps 1 to 14 as a control calculation loop.

  As described above, according to the control device of the first embodiment, the drive torque error compensation means 25 is provided (the drive torque error compensation means is actually described as a part of the control program, And a torque feedback control system for feeding back the driving torque generated by the elastic actuator driving movable mechanism 102, and the pressure difference error compensating means 15 and the driving torque error compensating means 25. By further disposing between the elastic body actuator driven movable mechanism 102 and constituting an internal pressure control system that feeds back the internal state of each elastic expansion / contraction structure 1 of the elastic body actuator driven movable mechanism 102. Therefore, it is possible to control the robot arm 10 with high responsiveness and less dynamic influence.

Further, according to the control method of the first embodiment, the drive torque error compensation means 25 is provided to constitute a torque feedback control system that feeds back the drive torque generated by the elastic actuator drive movable mechanism 102. In addition, a pressure difference error compensating means 15 is further disposed between the driving torque error compensating means 25 and the elastic actuator driving movable mechanism 102, so that each elastic expansion / contraction of the elastic actuator driving movable mechanism 102 is performed. By configuring an internal pressure control system that feeds back the internal state of the structure 1, it is possible to control the robot arm 10 with high responsiveness and less dynamic influence, with high accuracy.

(Second Embodiment)
FIG. 8 is a block diagram showing the concept of a control device for an elastic actuator-driven movable mechanism according to the second embodiment of the present invention. In FIG. 8, reference numeral 101 denotes target output generation means for generating a target value of output such as displacement and generated force of the elastic actuator driving movable mechanism 102 driven by the elastic actuator 1. Reference numeral 104 denotes output measuring means connected to the elastic actuator-driven movable mechanism 102, which measures the output value of the elastic actuator-driven movable mechanism 102 and inputs the measured value to the output error compensating means 103. . Reference numeral 103 denotes output error compensation means for receiving output error between the output target value from the target output generation means 101 and the output measurement value from the output measurement means 104 and outputting output error compensation information. By outputting the output error compensation information so as to compensate for the output error, control is performed so that the measured value of the output of the elastic actuator driving movable mechanism 102 measured by the output measuring means 104 follows the target value. Reference numeral 105 denotes driving force error compensation means for compensating for an error in the driving force generated by the elastic actuator 1. The driving force error compensation means 105 is connected to the output of the output error compensation information from the output error compensation means 103 and to the elastic actuator drive movable mechanism 102. The difference from the driving force measured by the driving force measuring means 107 is input to the driving force error compensating means 105, and driving force error compensation information for compensating the driving force error is output. Reference numeral 109 denotes a target internal state determination unit to which an output target value generated by the target output generation unit 101 is input. Based on the output target value, the internal state target value of the elastic actuator 1 of the elastic actuator driving movable mechanism 102 To decide. 106 is a difference between the sum of the driving force error compensation information output from the driving force error compensation unit 105 and the output information output from the target internal state determination unit 109 and the internal state measurement value output from the internal state measurement unit 108. The internal state error compensation means is inputted and outputs the internal state error compensation information, and the output information of the internal state error compensation information from the internal state error compensation means 106 is the respective elastic actuators of the elastic actuator driving movable mechanism 102. 1, and control is performed so that the internal state measurement value of each elastic actuator 1 follows the target value. Reference numeral 108 denotes an internal state measuring means connected to each elastic actuator 1 of the elastic body actuator driven movable mechanism 102. The internal state measuring value 108, which is the internal pressure of each elastic actuator 11, is measured by the internal state measuring means. Thus, the internal state measurement value is input to the internal state error compensation means 106.

  Next, regarding a specific example of the control device for the elastic actuator drive movable mechanism 102 according to the second embodiment, the control device for the robot arm 10 which is an example of the elastic actuator drive movable mechanism 102 is taken as an example of control. Take and explain. Details of the robot arm 10 are the same as those in the first embodiment, and thus details thereof are omitted.

  FIG. 9 is a diagram more specifically showing the configuration of the control device of the elastic actuator driving movable mechanism 102 according to the second embodiment of the present invention.

In FIG. 9, reference numeral 26 denotes a target pressure difference determining unit as an example of the target internal state determining unit 109. A target joint angle vector q _d output from the target trajectory generating unit 11 is input to the target pressure difference calculating unit 26, and a target pressure difference (target value of pressure difference) ΔP _d = [ΔP from the target joint angle vector q _{d. 1d} , ΔP _2d , ΔP _3d , ΔP _4d , ΔP _5d , ΔP _6d ] ^T is calculated by the target pressure difference calculation means 26, and the target of the pressure difference is transferred from the target pressure difference calculation means 26 to the pressure difference error compensation means 15. The value is output. However, [Delta] P _1d, the _{ΔP 2d, ΔP 3d, ΔP 4d} , ΔP 5d, respectively [Delta] P _6d, the elastic expansion contraction structures 1-1a and 1-1b, the elastic expansion contraction structures 1-2a and 1-2b, Elastic expansion and contraction structures 1-3a and 1-3b, elastic expansion and contraction structures 1-4a and 1-4b, elastic expansion and contraction structures 1-5a and 1-5b, elastic expansion and contraction structures 1-6a and 1b This is the target value of the pressure difference of −6b.

The pressure difference error compensation means 15, which is an example of the internal state error compensation means 106, has a target pressure difference ΔP _d output from the target pressure difference calculation means 26 and a torque error correction pressure difference ΔP from the drive torque error compensation means 105. A value (pressure difference error ΔP _e ) obtained by adding _τ and subtracting the current pressure difference ΔP from the pressure difference calculating means 13 is input, and the pressure difference error compensating means 15 corrects the error in the internal pressure state. In addition, a pressure difference error correction output u is output toward the robot arm 10. The pressure difference error correction output u is given as a voltage command value to each 5-port flow rate control solenoid valve 18 via the D / A board 20, and each joint shaft 6 is driven to rotate forward and backward to operate the robot arm 10. To do.

  The principle of the control operation relating to the control device configured as described above will be described.

Basic control operation, the position error compensation means 12 by the hand position and orientation feedback control of the error r _e (position control). As the position error compensation means 12, for example, by using the PID compensator acts is controlled to the hand position and orientation error r _e is converged to 0, the operation of the robot arm 10 to the target is realized.

  In addition, since the inverse dynamics calculation means 24 is provided, dynamic influences such as inertia force and centrifugal force generated in the robot arm 10 are compensated, so that the robot arm 10 shown in FIG. Even when a direct drive driving method that does not use a speed reducer is used to drive the joint, it is possible to realize a highly accurate operation.

  However, in order for compensation of the dynamic influence by the inverse dynamics calculation means 24 to function effectively, a mechanism for generating a desired torque as a joint driving force by the elastic actuator 1 is necessary.

A means for coping with such a problem is torque feedback control by the drive torque error compensation means 25. A torque error τ _e that is the difference between the position error correction torque τ _p and the current torque value τ measured by the torque sensor 7 is input to the drive torque error compensation means 25, and the torque error τ _e becomes zero. Thus, the drive torque error compensation means 25 operates. That is, the drive torque error compensation means 25 accurately realizes the position error correction torque τ _p that is a torque necessary for correcting the position error, and the compensation of the dynamic influence by the inverse dynamics calculation means 24 is effectively performed. Function.

  However, when driven by an elastic actuator, for example, an actuator operated by a fluid such as air shown in FIG. 2, the responsiveness is poor due to the elastic elements of the elastic actuator, that is, the compressibility of the fluid, the flow resistance, etc. The torque feedback control alone cannot be accurately controlled.

A means for dealing with such a problem is feedback control of the pressure difference ΔP by the pressure difference error compensating means 15. Because the pressure difference error compensation means 15 for the torque error correction output [Delta] P _tau inputted from driving torque error compensation unit 25 operates the pressure difference error compensation means 15 when the torque error tau _e occurs, the torque error tau _e 0 The pressure difference is controlled so as to converge. Therefore, by constructing an internal pressure feedback loop that controls the pressure difference inside the torque feedback loop that performs torque control as in the control system shown in FIG. 9, the poor response is compensated, and the torque control performance is improved. Improvements can be realized.

However, although the responsiveness is improved only by providing the pressure difference error compensating means 15, there is a possibility that a stationary position deviation occurs and the positioning accuracy cannot be improved. This is due to that it does not the pressure difference required to achieve the desired joint angle vector q _d input to the pressure difference error compensation means 15 as a target value.

を使うことができる。ただし、Ａ、ｂは係数であり、図１０の測定結果より求めることができる。したがって、目標圧力差計算手段２６において、式（４）により目標関節角度ベクトルｑ_ｄから目標圧力差ΔＰ_ｄを計算して圧力差誤差補償手段１５に入力することにより、定常偏差の小さい高精度な位置制御を実現することができる。 A means for dealing with such a problem is the target pressure difference calculating means 26. When performing forward / reverse rotation driving of the joint shaft by antagonistic driving of the set of elastic expansion / contraction structures 1 and 1 shown in FIG. 3, the relationship between the joint angle and the internal pressure difference of the set of elastic expansion / contraction structure 1 is as follows: For example, as shown in FIG. FIG. 10 shows a result when an elastic expansion / contraction structure (Mackiben type pneumatic artificial muscle) having a total length of 250 mm and an inner diameter of 10 mm is used as an example. As shown in FIG. 10, the measurement result can be approximated by a substantially straight line. Therefore, a linear equation representing a straight line as an expression for calculating the desired pressure difference [Delta] P _d

Can be used. However, A and b are coefficients and can be obtained from the measurement results of FIG. Therefore, the target pressure difference calculating means 26 calculates the target pressure difference ΔP _d from the target joint angle vector q _d by the equation (4) and inputs it to the pressure difference error compensating means 15, thereby achieving high accuracy with a small steady deviation. Position control can be realized.

  FIG. 11 is a diagram illustrating another configuration of the control device for the elastic actuator drive-type movable mechanism according to the second embodiment of the present invention.

In the control device of FIG. 11, the target internal state determination unit 109 includes a target pressure difference calculation unit 26 and an approximate inverse kinematics calculation unit 27 as an example different from FIG. 9. The approximate inverse kinematics calculation means 27 outputs the hand position / posture vector r calculated by the forward kinematics calculation means 21 from the current value q of the joint angle vector measured in the robot arm 10 and the target trajectory generation means 11. an error r _e between the tip unit position and orientation target vector r _d that, the current value q is entered in the joint angle vector, the error r _e, from the Jacobian matrix J _r calculated from the current value q of the joint angle vector error _{q e} of the joint angle vector is calculated approximately by the calculation formula for ^{_{q e = J r -1 r e}} . The target pressure difference calculation means 26 includes a target joint based on the addition of the current value q of the joint angle vector measured by the robot arm 10 and the joint angle vector error q _e calculated by the approximate inverse kinematics calculation means 27. as angle vector _{q d, q d = q +} J r (q) -1 r e is inputted, from the target joint angle vector _{q d,} (target value of the pressure difference) target pressure difference _{ΔP d = [ΔP 1d, ΔP} 2d, ΔP _3d , ΔP _4d , ΔP _5d , ΔP _6d ] ^T is calculated by the target pressure difference calculating means 26, and the target value of the pressure difference is output from the target pressure difference calculating means 26 to the pressure difference error compensating means 15. .

Be configured as shown in this FIG. 11, similar to the arrangement of FIG. 9, the target pressure difference [Delta] P _d calculated by inputting the pressure difference error compensation means 15, small highly accurate position of the steady-state deviation Control can be realized.

  FIG. 15 is a diagram illustrating still another configuration of the control device for the elastic actuator driving movable mechanism according to the second embodiment of the present invention.

Controller of FIG. 15 is different from the controller of FIG. 9 is a specific example of the control device of FIG. 8, a point target trajectory generation section 11 for outputting the target joint angle vector q _d, a forward kinematics calculation means 21 without the joint angle vector q is fed back, the target joint angle vector q _d and joint angle point error q _e of the joint angle vector which is the difference between the vector q is input to the positional error compensating unit 12, the target pressure difference calculation means 26 except that the target joint vector q _d is input, the other parts are the same configuration. Therefore, according to the control device of FIG. 15, by the target joint vector q _d is given, the joint angle vector q is controlled so that the desired joint vector q _d.

FIG. 14 shows experimental results for verifying the performance of the control device according to the present invention. The experimental results in FIG. 14 show only the sixth joint 6-6 of the robot arm 10 shown in FIG. 2 by the control device shown in FIG. 2 in order to make the effects of taking the configuration of the control device according to the present invention easier to understand. is operated, the result of control was performed in the rotation angle q ₆ of the sixth joint 6-6. In the elastic expansion / contraction structure 1 for driving the sixth joint 6-6, the total length of the tubular elastic body 2 is 17 cm and the inner diameter is 1 cm. The distance between the intersection between the joint axes 6-3 and 6-4 and the intersection between the joint axes 6-5 and 6-6 was 28 cm. A weight was added to the hand portion so that the moment of inertia around the joint axis 6-6 was 0.0045 kgm ² . The high-pressure air supplied from the air pressure source 16 is adjusted to a pressure of 0.6 MPa by the air pressure adjusting unit 17 and supplied to the 5-port flow rate control electromagnetic valve 18.

  The result shown by the solid line in FIG. 14 is the control result by the control device of the second embodiment of FIG. 8, more specifically, the control device shown by the configuration of FIG. The target value was set to a target value that changes stepwise every 2 seconds between 0 ° and 35 °.

On the other hand, the current value q of the joint angle vector output from the robot arm 10 and the target joint angle vector output from the target trajectory generating means 11 as shown by the broken line in FIG. This is a result in the case of a configuration with only simple position control in which control is performed such that the error q _e of the joint angle vector, which is a difference from q _d , becomes zero.

  As can be seen from FIG. 14, according to the configuration of the control device of the second embodiment, it was confirmed that the control accuracy was improved as compared with the case of simple position control alone.

  As described above, according to the control device of the second embodiment, the torque feedback control that feeds back the driving torque generated by the elastic actuator driving movable mechanism 102 by providing the driving torque error compensating means 25. And a pressure difference error compensating means 15 is disposed between the elastic actuator driving movable mechanism 102 and the driving torque error compensating means 25 so that the inside of the elastic actuator driving movable mechanism 102 is provided. An internal pressure control system that feeds back the state is configured, and a target pressure difference calculation means 26 is further provided, and the target pressure difference is input between the drive torque error compensation means 25 and the pressure difference error compensation means 15. Feed-forward for high accuracy robots with good responsiveness, little dynamic influence, and small steady-state deviation. Control arm 10 is possible.

  Further, according to the control method of the second embodiment, the drive torque error compensating means 25 is disposed in the elastic actuator drive movable mechanism 102, and the drive generated by the elastic actuator drive movable mechanism 102 is achieved. A torque feedback control system that feeds back torque is configured, and a pressure difference error compensation means 15 is disposed between the elastic actuator drive movable mechanism 102 and the drive torque error compensation means 25 so that the elastic actuator An internal pressure control system that feeds back the internal state of the drive-type movable mechanism 102 is configured, and a target pressure difference calculating means 26 is further provided to convert the target pressure difference into the drive torque error compensating means 25 and the pressure difference error compensating means. By inputting between 15 and feed forward, the responsiveness is good and the dynamic influence is small. Et al in, it is possible to control the steady-state deviation is small highly accurate robotic arm 10.

In the control system of FIG. 11, the input to the desired internal state determining unit 109 and the error r _e of the hand position and orientation error r _e is the difference between the target value and the measured value measured in the robot arm 10 error r _e between the current value and the hand position and orientation vector r calculated by the forward kinematics calculation means 21 than q, the hand position and orientation target output from the target track generation unit 11 vector r _d of the joint angle vectors Since the block diagram can be transformed so that is input to the approximate inverse kinematics calculation means 23a, there is no essential difference between the case where the target value is input and the case where the error is input.

(Third embodiment)
FIG. 12 is a block diagram showing the concept of a control device for an elastic actuator-driven movable mechanism according to the third embodiment of the present invention.

  The components of the control device of the elastic actuator drive movable mechanism 102 according to the third embodiment shown in FIG. 12 are the same as those of the control device of the elastic actuator drive movable mechanism 102 according to the second embodiment shown in FIG. Yes, but the input / output relationship of each block is different. In the control device of FIG. 12, the loop in which the internal state is fed back to the internal state error compensating unit 106 is not inside the loop in which the driving force is fed back to the driving force error compensating unit 105, but exists independently. The elastic actuator driven movable mechanism 102 is configured by a signal obtained by adding the output of the internal state error compensation information from the internal state error compensation unit 106 and the output of the driving force error compensation information from the driving force error compensation unit 105. It is the structure which drives.

  Next, regarding a specific example of the control device for the elastic actuator driving movable mechanism 102 according to the third embodiment, the control device for the robot arm 10 which is an example of the elastic actuator driving movable mechanism 102 is taken as an example of control. Take and explain. Details of the robot arm 10 are the same as those in the first embodiment, and thus details thereof are omitted.

  FIG. 13 is a diagram more specifically showing the configuration of the control device for the elastic actuator drive-type movable mechanism according to the third embodiment of the present invention. In the control device shown in FIG. 13, a signal obtained by adding the output of the pressure difference error compensating means 15 and the output of the driving torque error compensating means is input to the robot arm 10.

With such a configuration, the target pressure difference calculating means 26 calculates the target pressure difference ΔP _d from the target joint angle vector q _d by, for example, the equation (4), and the target pressure difference calculating means 26 calculates the pressure difference error. by inputting the target pressure difference [Delta] P _d to the compensation means 15, it is possible as in the case of the control apparatus of the second embodiment, to realize a highly accurate position control small steady state error.

  FIG. 16 is a diagram showing still another configuration of the control device for the elastic actuator driving movable mechanism according to the third embodiment of the present invention.

Controller of FIG. 16 is different from the controller of FIG. 13 is a specific example of the control device of FIG. 12, the point target trajectory generation section 11 for outputting the target joint angle vector q _d, a forward kinematics calculation means 21 without the joint angle vector q is fed back, the target joint angle vector q _d and joint angle point error q _e of the joint angle vector which is the difference between the vector q is input to the positional error compensating unit 12, the target pressure difference calculation means 26 except that the target joint vector q _d is input, the other parts are the same configuration. Therefore, according to the control device of FIG. 16, by the target joint vector q _d is given, the joint angle vector q is controlled so that the desired joint vector q _d.

  FIG. 14 shows experimental results for verifying the performance of the control device according to the present invention. Since the details of the experiment have been described in the second embodiment, a description thereof will be omitted.

  In FIG. 14, the result shown by the alternate long and short dash line is the control result by the control device of the third embodiment of FIG. 12, more specifically by the control device shown by the configuration of FIG.

As can be seen from FIG. 14, according to the configuration of the control device of the third embodiment, as in the case of the second embodiment, the control accuracy can be improved compared to the case of simple position control alone. confirmed.

  As described above, according to the control device of the third embodiment, the drive torque error compensating means 25 is disposed on the elastic actuator drive movable mechanism 102 and is generated by the elastic actuator drive movable mechanism 102. And a target pressure difference calculating means 26 and a pressure difference error compensating means 15 separately from the driving torque error compensating means 25, the elastic actuator driving type movable mechanism. The pressure control system independent of the torque feedback control system that feeds back the internal state of the elastic actuator-driven movable mechanism 102 is arranged in the control mechanism 102 so that the response is good and the dynamic influence In addition, it is possible to control the robot arm 10 with high accuracy and a small steady deviation.

４０２は力誤差抽出手段であり、手先が発生する力ベクトルＦと、目標軌道生成手段１１より出力される手先が発生する目標力ベクトルＦ_ｄとの誤差である力誤差Ｆ_ｅが入力され、力制御を行う方向（ｘ_ｃ方向）の力誤差Ｆ_ｅｘのみを抽出し、力誤差補償手段４０３へと出力する。力制御を行う方向の力誤差の抽出は、図１９中に示した座標軸ｘとｘ_ｃ、ｙとｙ_ｃ、ｚとｚ_ｃがそれぞれ平行の場合、下記の式（６）により実行される。

力誤差補償手段４０３からは力誤差修正出力ｕ_Ｆが力−トルク変換手段４０４に向けて出力される。力−トルク変換手段４０４では、下記の式（７）で示される式により、力誤差Ｆ_ｅを修正するための力誤差修正トルクτ_Ｆが計算され出力される。

出力誤差補償手段１０３からは位置誤差修正トルクτ_ｐと力誤差修正トルクτ_Ｆが加算された値が出力誤差補償情報として出力される。
制御動作の基本は、位置誤差補償手段１２による手先位置・姿勢誤差ｒ_ｅのフィードバック制御（位置制御）と、力誤差補償手段４０３による手先が発生する力Ｆのフィードバック制御（力制御）を同時に実現する位置と力のハイブリッド制御である。位置誤差補償手段１２として、例えば、ＰＩＤ補償器を使用し、力誤差補償手段４０３として、例えばＰＩ補償器を使用すれば、手先位置・姿勢誤差ｒ_ｅが０に収束するように制御が働き、かつ、手先が発生する力誤差Ｆ_ｅが０に収束するように制御が働くため、目標とするロボットアーム４００の動作が実現する。
以上のように、駆動トルク誤差補償手段２５、および、圧力差誤差補償手段１５を備えることにより、応答性良く、精度の高いトルクフィードバック制御が可能となるため、応答性良く、精度の高い力制御が実現する。したがって、図１９に示した治具４０７の固定面４０６である壁面への押しつけ制御を様々な作業に応用することにより、例えば、窓拭き動作や、机の天面拭き動作等、実用的な作業を、安全性高く実現することが可能となる。
Further, according to the control method of the third embodiment, the drive torque error compensating means 25 is disposed in the elastic actuator drive movable mechanism 102, and the drive generated by the elastic actuator drive movable mechanism 102 is achieved. A torque feedback control system for feeding back torque is configured, and the target pressure difference calculation means 26 and the pressure difference error compensation means 15 are arranged in the elastic actuator drive movable mechanism 102 separately from the drive torque error compensation means 25. By providing a pressure control system independent of the torque feedback control system that feeds back the internal state of the elastic actuator drive movable mechanism 102, the responsiveness is good and the dynamic influence is small. Furthermore, it is possible to control the robot arm 10 with high accuracy with a small steady deviation.

(Fourth embodiment)
FIG. 18 shows a control apparatus for the elastic actuator-driven movable mechanism shown in FIG. 8, in which the jig 407 is gripped by the robot arm 400 as shown in FIG. 19 and in the direction perpendicular to the fixed surface 406 ( _xc direction). simultaneously performing force control of pressing with a force F, which is a diagram specifically illustrating the configuration of a control device when applied to the work to perform position control in a direction parallel (y _c, z c direction) to the fixing surface 406. 18, parts having the same configuration as those of the control device shown in FIG. 11 which is one of the specific examples of FIG. In FIG. 19, reference numeral 405 denotes a force sensor disposed on the hand 313, and a force vector F = [F _x , F _y , F _z , M _x , M _y , M _z ] ^T generated by the hand, that is, The force with which the jig 407 is pressed against the fixed surface 406 is measured. Where F _x , F _y , and F _z are translation forces in the _x , _y , and z directions, respectively, and M _x , M _y , and M _z are force moments about the x axis and forces about the y axis, respectively. Moment, a force moment around the z-axis. Since the other structure is the same as that of the robot arm 10 shown in FIG. 2, detailed description thereof is omitted.
The control device shown in FIG. 18 differs from the control device shown in FIG. 11 in the configuration of the output error compensation means 103, and the robot arm 400 generates the hand of the robot arm 400 in addition to the joint angle vector q as an output from the robot arm 400. The difference is that the force vector F to be output is output by the force sensor 405.
18, 401 is a position error extracting means, the position error _{r e} is inputted, the direction of controlling the position _(y c, _{z c} direction) to extract only the positional error _{r ex} of the positional error compensating unit 12 Is output. When the coordinate axes x and x _c , y and y _c , and z and z _c shown in FIG. 19 are parallel, the extraction of the position error in the direction in which the position control is performed is executed by the following equation (5).

A force error extraction unit 402 receives a force error F _e that is an error between the force vector F generated by the hand and the target force vector F _d generated by the hand output from the target trajectory generating unit 11. Only the force error F _{ex in the} control direction ( _xc direction) is extracted and output to the force error compensation means 403. Extraction of force error in the direction in which force control is performed is executed by the following equation (6) when the coordinate axes x and x _c , y and y _c , and z and z _c shown in FIG. 19 are parallel.

A force error correction output u _F is output from the force error compensation unit 403 toward the force-torque conversion unit 404. The force-torque conversion means 404 calculates and outputs a force error correction torque τ _F for correcting the force error F _{e according} to the following equation (7).

A value obtained by adding the position error correction torque τ _p and the force error correction torque τ _F is output from the output error compensation means 103 as output error compensation information.
Basic control operation, the position error compensation means 12 by the hand position and orientation error r _e of the feedback control (position control), the feedback control of the force F which the hand is generated by the force error compensation means 403 (force control) at the same time achieve Position and force hybrid control. As the position error compensation means 12, for example, using a PID compensator, as a force error compensation means 403, for example, when using a PI compensator acts is controlled to the hand position and orientation error r _e is converged to 0, and, since the acts controlled such force error F _e of the hand occurs converges to 0, the operation of the robot arm 400 to the target can be realized.
As described above, by providing the drive torque error compensation means 25 and the pressure difference error compensation means 15, torque feedback control with high responsiveness and high accuracy is possible, so that force control with high responsiveness and high accuracy is possible. Is realized. Accordingly, by applying the pressing control to the wall surface which is the fixed surface 406 of the jig 407 shown in FIG. 19 to various work, for example, practical work such as window wiping operation, desk top wiping operation, etc. Can be realized with high safety.

(Fifth embodiment)
Next, as a fifth embodiment of the present invention, a case where a conductive polymer actuator is used as an elastic actuator for another specific example of the control device for the elastic actuator driving movable mechanism shown in FIG. Take an example and explain. The details of the robot arm 10 are the same as those in the first embodiment except that a conductive polymer actuator is used as the elastic actuator, and the details are omitted.

  FIG. 19 is a cross-sectional view schematically showing an artificial muscle actuator 301 as an example of the conductive polymer actuator according to the fifth embodiment of the present invention. FIG. 20 shows an outline view thereof.

In FIG. 19, reference numeral 311 denotes an elastic plate made of a conductive polymer rectangular elastic body that expands and contracts in accordance with the oxidation-reduction reaction, and is a space surrounded by a cylindrical case 321 and a disk-shaped lid 322. It is arrange | positioned in the approximate center part in the liquid of the electrolyte solution 313 which is the electrolyte housing layer which satisfy | fills. As an electrolytic _solution, NaPF 6, TBAPF electrolytes such as ₆ or as dissolved in an organic solvent such as water or propylene carbonate, ionic liquids such as BMIPF ₆ are available. In particular, an electrolyte containing PF ₆ as an anion is desirable because a large displacement can be obtained in combination with polypyrrole, which is a conductive polymer.

  Polypyrrole, polyaniline, polymethoxyaniline, or the like can be used as the conductive polymer constituting the conductive polymer stretchable plate 311. Polypyrrole is desirable because of its large displacement. The thickness of the conductive polymer stretchable plate 311 is preferably about several tens of μm. If it is thinner than that, it will be weak in strength, and if it is thicker than that, ions will not be able to sufficiently enter and exit the conductive polymer stretchable plate 311, which is not desirable.

  Rods 323a and 323b are connected to both ends in the longitudinal direction of the conductive polymer stretchable plate 311. The rod 323a passes through a seal member 324a provided on the lid 322, and the rod 323b is connected to the case 321. And protrudes to the outside of the lid 322 and the case 321 respectively.

  The wiring connected to the conductive polymer stretchable plate 311 is connected to the power source 303 via the seal member 324 c provided in the lid 322, the switch 304, and the current measuring device 326. A counter electrode 325 is connected to the other pole of the power supply 303. The counter electrode 325 is in contact with the electrolytic solution 313 filled in the space in the case 321 through the seal member 324 d provided in the lid 322.

  The power supply 303 and the switch 304 are appropriately adjusted and turned on and off based on information from the current measuring device 326 by a control device (not shown), and the operation of the artificial muscle actuator 301 is controlled.

  Next, the operation of the artificial muscle actuator 301 will be described.

  Causes of the contraction of the conductive polymer stretchable plate 311 include anion (anion) in and out, cation (cation) in and out, change in polymer structure, and the like. FIG. 19A and FIG. In the explanation of the operation principle according to B) and FIG. 19C, since the main deformation mechanism is anion doping and undoping in a material system such as polypyrrole, the entry and exit of anions will be described.

  FIG. 19A shows a state in which no voltage is applied to the conductive polymer stretch plate 311 in a switched-off state, and FIG. 19B shows a positive potential applied to the conductive polymer stretch plate 311. Shows the case. When a voltage is applied to the conductive polymer stretch plate 311, the anion that is homogeneously present in the electrolyte solution 313, which is the electrolyte housing layer when no voltage is applied, becomes the conductive polymer stretch plate 311 side on the positive electrode side. To be drawn into the conductive polymer stretchable plate 311. With this oxidation process, the conductive polymer stretchable plate 311 expands, and the rods 323a and 323b can take out the displacement in the extension direction of the conductive polymer stretchable plate 311. FIG. 19C shows the case where a negative voltage is applied to the conductive polymer stretchable plate 311. The anions present on the conductive polymer stretchable plate 311 are attracted toward the counter electrode 325 facing each other, and are released into the electrolyte solution 313 which is an electrolyte enclosure layer. The polymer stretchable plate 311 contracts, and the displacement in the contraction direction can be taken out by the rods 323a and 323b.

  Since it is known that the displacement amount or generated force of the conductive polymer actuator that operates by the entry / exit of the anion as described above is approximately proportional to the amount of injected electric charge, in the fifth embodiment, the internal state The amount of charge is used as the internal state quantity when measuring.

  FIG. 21 is a diagram illustrating a configuration of a power supply system for driving the robot arm 10 which is a control target of the control device of the elastic actuator driving movable mechanism 102 according to the fifth embodiment of the present invention. In FIG. 21, only the part for driving the third joint shaft 6-3 of the robot arm 10 to rotate forward and backward is shown, and the other parts are omitted. In FIG. 21, reference numerals 351-3a and 351-3b denote drive power supplies, which are constituted by voltage variable power supplies 352-3a and 352-3b, and ammeters 353-3a and 353-3b. 3a, 301-3b (an example of the elastic expansion / contraction structure 1-3a and the elastic expansion / contraction structure 1-3b), and a voltage is applied. Reference numeral 19 denotes a control computer constituted by, for example, a general personal computer as an example of a control unit, which is equipped with a D / A board 20 and outputs voltage command values to the voltage variable power supplies 351-3a and 351-3b. By doing so, it is possible to independently control the voltage value applied to each of the conductive polymer actuators 301-3a and 301-3b, whereby the conductive polymer actuators 301-3a and 301-3b are controlled. Is driven. Further, an A / D board 354 is mounted on the control computer 19, and the current value measured by the ammeters 353-3 a and 353-3 b can be taken into the control computer 19.

FIG. 18 is a diagram showing the configuration of the control device of the elastic actuator drive movable mechanism 102 according to the fifth embodiment of the present invention. In FIG. 18, reference numeral 201 denotes a charge amount error compensation unit which is an example of the internal state error compensation unit 106. The charge amount error compensation means 201, the torque error correction charge amount c _tau from the driving torque error compensation unit 25 is an example of a driving torque error compensation unit 105, when the driving power source (conductive polymer actuator 301-3a Is the current charge amount c obtained by integrating in the charge amount calculation means 202 the current i measured by the drive power supply 351-3a, and in the case of the conductive polymer actuator 301-3b, the drive power supply 351-3b). (A charge amount error c _e ) is input, and the charge amount error compensation unit 201 calculates an error in the amount of charge injected into the conductive polymer actuator 301-3a or the conductive polymer actuator 301-3b. The charge amount error correction output u is output toward the robot arm 10 so as to be corrected. The charge amount error correction output u is given as a voltage command value to each power supply via the D / A board 20, and the third joint shaft 6-3 is driven to rotate forward and backward to operate the robot arm 10. Also in the other joint shafts 6-1 to 6-2 and 6-4 to 6-6, the robot arm 10 is operated by forward and reverse rotation of each joint shaft by the same control of the control device. To do.

According to the control device of the fifth embodiment, the drive torque error compensation means 25 is provided to constitute a torque feedback control system that feeds back the drive torque generated by the elastic actuator drive movable mechanism 102. In addition, the charge amount error compensating means 201 is disposed between the elastic actuator driving movable mechanism 102 and the driving torque error compensating means 25 to feed back the internal state of the elastic actuator driving movable mechanism 102. By configuring the control system, it is possible to control the robot arm 10 with high responsiveness and high precision and less dynamic influence.

  In the second and third embodiments, the target pressure difference calculation means 26 approximates the relationship between the joint angle and the pressure difference with a linear equation. However, the present invention is not limited to this, and a multidimensional such as a quadratic equation is used. It goes without saying that polynomials can be approximated. As described above, when the relationship between the output of the elastic actuator 1 and the internal state of the elastic actuator 1 is approximated by a polynomial, the target internal state determining means 105 uses the polynomial to output the target of the output of the elastic actuator 1. The target value of the internal state of the elastic actuator 1 is calculated and determined from the value. In addition, the target internal state determination means 105 controls the relationship between the output of the elastic body actuator 1 and the internal state of the elastic body actuator 1 (for example, the relationship between the joint angle and the pressure difference) instead of being approximated by a polynomial. A memory 19a (see FIG. 4) in the control computer 19 of the apparatus is stored as a table, and based on the table, from the target value of the output of the elastic actuator 1 (for example, the target value of the joint angle), the elastic actuator 1 It is also possible to adopt a configuration in which a target value (for example, a target value of the pressure difference) of the internal state is derived.

  In the third embodiment, the target internal state determination means 109 is the target pressure difference calculation means 26. However, the present invention is not limited to this, and similar to the case of the configuration of FIG. 11, approximate inverse kinematic calculation means. The same applies to the case of the configuration composed of 27 and the target pressure difference calculating means 26.

  In each of the above embodiments, the output is a joint angle. However, the present invention is not limited to this. The output measuring means 104 is a displacement speed sensor as an example of the displacement speed measuring means, and the output value of the elastic actuator is The same applies to the case where displacement speed control is performed as the displacement speed of the elastic actuator.

  In each of the above embodiments, the output is a joint angle. However, the present invention is not limited to this, and the output measuring means 104 is a force sensor as an example of a force measuring means, and the output value of the elastic actuator is the elasticity. The same applies to the case where force control is performed as the force exerted by the body actuator.

  In each of the above embodiments, the sensor is provided as an example of the internal state measuring unit 108. However, the same effect can be obtained even when an observer (observer) is provided to estimate the internal state and use the estimated value of the internal state. Demonstrate.

  In each of the above embodiments, the torque sensor 7 is provided as an example of the driving force measuring means 107. However, the same applies to the case where an observer (observer) is provided to estimate the driving force and use the estimated value of the driving force. Demonstrate the effect.

  In the fifth embodiment, the elastic actuator is described as an example of a conductive polymer actuator as an actuator driven by electrical stimulation. However, the present invention is not limited to this, and a dielectric polymer and various gels are used. Even in the case of an actuator that drives an elastic body such as an electric stimulus by electrical stimulation, the same effect is exhibited by adopting a potential or a charge amount as an internal state.

  It is to be noted that, by appropriately combining arbitrary embodiments of the various embodiments described above, the effects possessed by them can be produced.

  The control device and control method for the elastic actuator-driven movable mechanism of the present invention are useful as a control device and control method for performing position control such as trajectory control of the hand position of a robot arm operated by an elastic actuator. Further, the present invention is not limited to a robot arm, and can be applied to a control device and control method of a rotation mechanism using an elastic actuator in a production facility or the like, and a control device and control method of a linear motion mechanism using an elastic actuator such as a linear slider or a press device. Is possible.

It is a block diagram which shows the concept of the control apparatus of the elastic body actuator drive type movable mechanism concerning 1st Embodiment of this invention. It is a figure which shows the structure of the robot arm which is the control object of the control apparatus of the elastic body actuator drive type movable mechanism concerning 1st Embodiment of this invention. It is a figure which shows the structure and operation | movement of the elastic expansion-contraction structure which drives the robot arm which is the control object of the control apparatus of the elastic body actuator drive type movable mechanism concerning 1st Embodiment of this invention, Comprising: An upper side is a pressure reduction state, The lower side is a diagram showing a pressurized state. It is a figure which shows operation | movement of the air pressure supply system for driving the robot arm which is the control object of the control apparatus of the elastic body actuator drive type movable mechanism concerning 1st Embodiment of this invention with the air which is a compressible fluid. It is a figure which shows the structure of the pneumatic pressure supply system for driving the robot arm which is a control object of the control apparatus of the elastic body actuator drive type movable mechanism concerning 1st Embodiment of this invention with the air which is a compressive fluid. It is a figure which shows the structure of the control apparatus of the elastic body actuator drive-type movable mechanism concerning 1st Embodiment of this invention. It is a flowchart of the actual operation | movement step of the control program of the control apparatus of the elastic body actuator drive type movable mechanism concerning 1st Embodiment of this invention. It is a block diagram which shows the concept of the control apparatus of the elastic body actuator drive type movable mechanism concerning 2nd Embodiment of this invention. It is a figure which shows the structure of the control apparatus of the elastic body actuator drive-type movable mechanism concerning 2nd Embodiment of this invention. It is a figure which shows the relationship between the joint angle and internal pressure difference in the antagonistic drive by the elastic expansion-contraction structure of the elastic body actuator concerning 2nd Embodiment of this invention. It is a figure which shows the other structure of the control apparatus of the elastic body actuator drive-type movable mechanism concerning 2nd Embodiment of this invention. It is a block diagram which shows the concept of the control apparatus of the elastic body actuator drive type movable mechanism concerning 3rd Embodiment of this invention. It is a figure which shows the structure of the control apparatus of the elastic body actuator drive-type movable mechanism concerning 3rd Embodiment of this invention. It is a figure which shows the experimental result which verified the performance of the control apparatus which concerns on 2nd and 3rd embodiment of this invention. It is a block diagram which shows the more concrete structure of the control apparatus of 2nd Embodiment of FIG. It is a block diagram which shows the more concrete structure of the control apparatus of 3rd Embodiment of FIG. It is a block diagram which shows the structure only of simple position control concerning the result shown with the broken line in FIG. It is a figure which shows the structure of the control apparatus of the elastic body actuator drive-type movable mechanism concerning 4th Embodiment of this invention, It is a figure which shows the structure of the robot arm which is a control object of the control apparatus of the elastic body actuator drive type movable mechanism concerning 4th Embodiment of this invention, It is a figure which shows the structure of the control apparatus of the elastic body actuator drive-type movable mechanism concerning 5th Embodiment of this invention. It is sectional drawing which showed the outline of the conductive polymer actuator concerning 5th Embodiment of this invention. It is the external view which showed the outline of the conductive polymer actuator concerning 5th Embodiment of this invention. It is a figure which shows the structure of the power supply system for driving the robot arm which is the control object of the control apparatus of the elastic body actuator drive type movable mechanism concerning 5th Embodiment of this invention.

Explanation of symbols

1, 1-1a, 1-1b, 1-2a, 1-2b, 1-3a, 1-3b, 1-4a, 1-4b, 1-5a, 1-5b, 1-6a, 1-6b Elasticity Expansion and contraction structure 2 Tubular elastic body 3 Deformation direction regulating member 4 Sealing member 5 Fluid passage member 6, 6-1, 6-2, 6-3, 6-4, 6-5, 6-6 Joint shaft 8 Encoder (Example of displacement measuring means as an example of output measuring means)
9 Pressure sensor (an example of pressure measuring means which is an example of internal state measuring means)
DESCRIPTION OF SYMBOLS 10 Robot arm 11 Target track | orbit production | generation means 12 Position error compensation means 13 Pressure difference calculation means 15 Pressure difference error compensation means 16 Air pressure source 17 Air pressure adjustment unit 18 5 port flow control solenoid valve 19 Control computer 19a Memory 20 D / A board 21 Order Kinematics calculation means 22 Inverse kinematics calculation means 23a, 23b Approximate inverse kinematics calculation means 24 Inverse dynamics calculation means 25 Drive torque error compensation means 26 Target pressure difference calculation means 27 Approximate inverse kinematics calculation means 101 Target output generation means 102 Elastic actuator driven movable mechanism 103 Output error compensation means 104 Output measurement means 105 Driving force error compensation means 106 Internal state error compensation means 107 Driving force measurement means 108 Internal state measurement means 109 Target internal state determination means

Claims (10)

A control device for an elastic actuator-driven movable mechanism that is antagonistically driven by a set of elastic actuators,
  An internal state measuring means for measuring an internal state of the elastic body actuator that is changed by driving the elastic body actuator and outputting a measurement value of the internal state;
  Driving force measuring means for measuring the driving force generated by the elastic actuator and outputting the measured value of the driving force;
  Output measuring means for measuring the output of the movable mechanism driven by the elastic actuator and outputting the measured value of the output;
  The target value of the output of the movable mechanism driven by the elastic actuator and the measured value of the output measured by the output measuring means are input and output error compensation information is output so as to compensate the output error. Output error compensation means;
  The output of the output error compensation information from the output error compensation means and the output of the measurement value of the driving force from the driving force measurement means are input and the driving force error compensation information is output so as to compensate for the driving force error. Driving force error compensation means for
  Target internal state determination means for determining a target value of the internal state of the elastic actuator and outputting the target value of the internal state;
  The output of the driving force error compensation information from the driving force error compensation unit, the output of the target value of the internal state from the target internal state determination unit, and the measurement value of the internal state from the internal state measurement unit An internal state error compensation means for outputting internal state error compensation information so as to compensate for the internal state error as the output is input;
  An elastic actuator that controls the measured value of the output of the movable mechanism driven by the elastic actuator based on the internal state error compensation information output by the internal state error compensation means to be a target value of the output Control device for drive-type movable mechanism. A control device for an elastic actuator-driven movable mechanism that is antagonistically driven by a set of elastic actuators,
  An internal state measuring means for measuring an internal state of the elastic body actuator that is changed by driving the elastic body actuator and outputting a measurement value of the internal state;
  Driving force measuring means for measuring the driving force generated by the elastic actuator and outputting the measured value of the driving force;
  Output measuring means for measuring the output of the movable mechanism driven by the elastic actuator and outputting the measured value of the output;
  The target value of the output of the movable mechanism driven by the elastic actuator and the measured value of the output measured by the output measuring means are input and output error compensation information is output so as to compensate the output error. Output error compensation means;
  The output of the output error compensation information from the output error compensation means and the output of the measurement value of the driving force from the driving force measurement means are input and the driving force error compensation information is output so as to compensate for the driving force error. Driving force error compensation means for
  Target internal state determination means for determining a target value of the internal state of the elastic actuator and outputting the target value of the internal state;
  Internal state error compensation so that the output of the target value of the internal state from the target internal state determination means and the output of the measurement value of the internal state from the internal state measurement means are input and the internal state error is compensated. An internal state error compensation means for outputting information,
  The output of the movable mechanism driven by the elastic actuator based on the internal state error compensation information output by the internal state error compensation unit and the driving force error compensation information compensated by the driving force error compensation unit. A control device for an elastic actuator-driven movable mechanism that controls a measured value to be a target value of the output. 3. The control device for an elastic actuator-driven movable mechanism according to claim 1, wherein the target internal state determination unit determines the target value of the internal state while the target value of the output is input. . The target internal state determining means approximates a relationship between the output of the elastic actuator and the internal state of the elastic actuator by a polynomial, and the elastic body is calculated from a target value of the output of the elastic actuator by the polynomial. 3. The control device for an elastic actuator-driven movable mechanism according to claim 1, wherein a target value of the internal state of the actuator is calculated and determined. The target internal state determination means further includes a memory for storing a relationship between the output of the elastic actuator and the internal state of the elastic actuator as a table, and the target value of the output of the elastic actuator by the table. 3. The control device for an elastic actuator-driven movable mechanism according to claim 1, wherein a target value of the internal state of the elastic actuator is determined. 3. The control device for an elastic actuator-driven movable mechanism according to claim 1, wherein the elastic actuator is a fluid pressure driving actuator driven by fluid pressure. The fluid pressure drive actuator can inject or pour fluid into a hollow elastic body, a pair of sealing members for hermetically sealing the hollow elastic body, and a hollow inside of the hollow elastic body. 7. The control device for an elastic actuator-driven movable mechanism according to claim 6, wherein the control device is an elastic expansion / contraction structure having a fluid passage member. 7. The elastic actuator according to claim 6, wherein the internal state of the elastic actuator is a fluid pressure, and the internal state measuring unit is a pressure measuring unit that measures the fluid pressure of the elastic actuator. Control device for drive-type movable mechanism. A method for controlling an elastic actuator-driven movable mechanism that is antagonistically driven by a set of elastic actuators,
  Measure the internal state of the elastic actuator that changes due to the drive of the elastic actuator to obtain the measured value of the internal state,
  Measure the driving force generated by the elastic actuator to obtain the measured value of the driving force,
  Measure the output of the movable mechanism driven by the elastic body actuator to obtain the measured value of the output,
  Obtaining output error compensation information so as to compensate the output error from the target value of the output of the movable mechanism driven by the elastic actuator and the measured value of the measured output,
  Obtaining the driving force error compensation information so as to compensate the driving force error from the output of the output error compensation information and the output of the measurement value of the driving force,
  Determining the target value of the internal state of the elastic actuator to obtain the target value of the internal state;
  The internal state error compensation information is obtained so as to compensate the internal state error from the output of the driving force error compensation information, the output of the target value of the internal state, and the output of the measurement value of the internal state,
  A control method for an elastic actuator-driven movable mechanism that controls the measured value of the output of the movable mechanism driven by the elastic actuator based on the internal state error compensation information to be a target value of the output. A method for controlling an elastic actuator-driven movable mechanism that is antagonistically driven by a set of elastic actuators,
  Measure the internal state of the elastic actuator that changes due to the drive of the elastic actuator to obtain the measured value of the internal state,
  Measure the driving force generated by the elastic actuator to obtain the measured value of the driving force,
  Measure the output of the movable mechanism driven by the elastic body actuator to obtain the measured value of the output,
  Obtaining output error compensation information so as to compensate the output error from the target value of the output of the movable mechanism driven by the elastic actuator and the measured value of the output,
  Obtaining the driving force error compensation information so as to compensate the driving force error from the output of the output error compensation information and the output of the measurement value of the driving force,
  Determining the target value of the internal state of the elastic actuator to obtain the target value of the internal state;
  Obtain internal state error compensation information so as to compensate the internal state error from the output of the target value of the internal state and the output of the measured value of the internal state,
  Based on the internal state error compensation information and the driving force error compensation information, an elastic actuator drive type movable that controls the measured value of the output of the movable mechanism driven by the elastic actuator to be the target value of the output. Mechanism control method.

Images