JP6076745B2 - Link mechanism - Google Patents

Description

  The present invention includes one or a plurality of joint mechanisms provided between a base and a movable portion movable with respect to the base, and an actuator that outputs a driving force that displaces the one or more joint mechanisms. And each of the one or more joint mechanisms relates to a link mechanism configured to transmit power via an elastically deformable elastic element configured to be capable of variably controlling rigidity.

  Conventionally, a plurality of joint mechanisms provided between a base body and a movable part movable with respect to the base body, and an actuator that outputs a driving force for displacing the plurality of joint mechanisms, each of the plurality of joint mechanisms is A link mechanism configured to transmit power via an elastically deformable elastic element configured to be capable of variably controlling rigidity is known (Patent Document 1). This link mechanism is configured to be capable of changing the rigidity of the elastic element of the joint mechanism.

JP 2008-30296 A

  In the link mechanism in which the stiffness of the elastic element of the joint mechanism is variable as described in Patent Document 1, the stiffness with respect to the displacement of the movable portion is set to any one of one or more predetermined target stiffnesses. It is conceivable to control the rigidity of the elastic element of each joint mechanism so as to be equal to.

  However, conventionally, the setting range of the rigidity of the elastic element of the joint mechanism has not been sufficiently studied. For this reason, an appropriate setting range of the stiffness of the elastic element of the joint mechanism may be set to a range with a sufficient margin, that is, an excessively wide range.

  The present invention has been made in view of the above points, and includes one or a plurality of joint mechanisms provided between a base and a movable portion movable with respect to the base, and one or a plurality of joint mechanisms. Each of the one or more joint mechanisms is configured to transmit power via an elastically deformable elastic element configured to variably control rigidity. In the constructed link mechanism, the rigidity of the elastic element of the joint mechanism is appropriately set so that the rigidity against the displacement of the movable part is equal to any one of one or more predetermined target rigidity. An object is to provide a link mechanism that can be set within a range.

  Further, the present invention provides a link mechanism that can set the rigidity of the elastic element of the joint mechanism within an appropriate range in order to prevent each joint mechanism from being damaged due to a part of the link mechanism colliding with an external object. A further objective is to provide

The present invention, and an actuator that outputs driving force for displacing the multiple joint mechanism provided between the movable movable portion relative to the base body and the base body, the joint mechanism before Kifuku number, before each Kifuku number of joint mechanism is a link mechanism that is configured to perform power transmission via a variably controllable-configured elastically deformable elastic element stiffness, pre Kifuku number When a target stiffness value for the displacement of the movable part defined by the stiffness of each elastic element of each joint mechanism is defined as a target stiffness, a plurality of operations corresponding to each of a plurality of types of operations to be executed by the link mechanism wherein is the value of the target rigidity is prepared in advance, the rigidity of the elastic elements of each of the front Kifuku number of joint mechanisms, necessary to realize all of the plurality of target rigidity, the elastic of the joint mechanism Minimum and maximum element stiffness Is configured to be changed in a range between, the plurality of joint mechanisms each of said elastic element of stiffness minimum and maximum values of each, kj1_i, kj2_i (i: identification of each of the plurality of joint mechanisms No.), the minimum value kj1_i and the maximum value Kj2_i of the stiffness of the elastic element of each joint mechanism are values determined by the following expressions (a1) and (a2) .
kj1_i = min (kj_rmin_1, kj_rmin_2, ..., kj_rmin_K)
...... (a1)
kj1_i = max (kj_rmax_1, kj_rmax_2, ..., kj_rmax_K)
...... (a2)
However,
Kj_rmin_k (k = 1, 2,..., K. k: identification number of each of the plurality of types of work) in the equation (a1) is the value of the target stiffness ke_cmd_k corresponding to the kth work, Of the component corresponding to the i-th joint mechanism among the diagonal components of the matrix kten calculated by the following equation (a3) from the Jacobian matrix J defined according to the attitude of the link mechanism and its transposed matrix J ^T The minimum value of the component value range corresponding to the variable range of the posture of the link mechanism in the k-th work, kj_rmax_k (k = 1, 2,..., K in the equation (a2)) K: identification number of each of the plurality of types of work) from the target stiffness value ke_cmd_k corresponding to the k-th work, the Jacobian matrix J and its transpose matrix J ^{T according} to the following equation (a3) The i-th joint of the calculated diagonal components of the matrix kten A maximum value of the component corresponding to the structure, the maximum value of the range of values of the component corresponding to the variable range of the posture of the link mechanism in the working of the k-th.
kten = J ^T · ke_cmd_k · J (a3)

  When a predetermined work is performed by the operation of the movable part of the link mechanism (the tip part or the portion between the base and the tip part), the rigidity against the displacement of the movable part is an appropriate rigidity (target) according to the work. Rigidity). For example, when performing precise work, higher rigidity is desired as the rigidity with respect to the displacement of the movable part.

When various kinds of work are performed by the operation of the movable part of the link mechanism, the target rigidity corresponding to each work is set. That is, when any one of a variety of work is performed, the rigidity with respect to the displacement of the movable portion is controlled to be a target rigidity corresponding to the work among the plurality of target rigidity. For this reason, the target rigidity with respect to the displacement of the movable part is prepared in advance with a plurality of values corresponding to each of a plurality of types of work to be executed by the link mechanism .

According to the present invention, in each of the multiple joint mechanism, the rigidity of the elastic element of the joint mechanism is needed to implement all of the plurality of target stiffness, the minimum stiffness of the elastic element of the joint mechanism It is configured to be changeable between a value and a maximum value . In this case, the minimum value and the maximum value determined by the equations (a1) and (a2), respectively, are used as the minimum value and the maximum value.

Thereby , in each joint mechanism, the range in which the rigidity of the elastic element can be changed is set to a range necessary for realizing all of the plurality of target rigidity. Thus, it is not necessary to set the rigidity setting range of the elastic element of the joint mechanism to an excessively wide range. Accordingly, the rigidity against displacement of the movable portion can be set necessary for realizing all predefined target stiffness of multiple, the rigidity of the elastic element of the joint mechanism within an appropriate range.

  The present invention includes one or a plurality of joint mechanisms provided between a base and a movable portion movable with respect to the base, and an actuator that outputs a driving force that displaces the one or more joint mechanisms. Each of the one or more joint mechanisms is a link mechanism configured to transmit power via an elastically deformable elastic element configured to be variably controllable in rigidity, In each of the one or more joint mechanisms, when the elastic deformation amount of the elastic element of the joint mechanism is the maximum deformation amount of the elastic element of the joint mechanism, the elastic energy of the elastic element of the joint mechanism is When the joint mechanism is displaced, the joint mechanism when the moment of inertia or mass of the joint mechanism is a minimum value that the joint mechanism can take and is displaced by the actuator. The rigidity set to be equal to or higher than the kinetic energy of the joint mechanism when the joint mechanism is assumed to be displaced at the maximum realizable displacement speed is defined as the first rigidity of the elastic element of the joint mechanism, In each of the one or more joint mechanisms, when the joint mechanism is displaced, the inertia moment or the inertial mass of the joint mechanism becomes the maximum value that the joint mechanism can take, and the displacement of the joint mechanism is started. The maximum acceleration that the joint mechanism can realize by the actuator is the time from the point in time until the start of the displacement of the joint mechanism by the elastic energy of the elastic element of the joint mechanism accumulated by the displacement of the joint mechanism. Assuming that the joint mechanism is displaced, the displacement speed of the joint mechanism is changed by the maximum displacement speed of the joint mechanism. The stiffness set to be equal to or greater than the time of the case is defined as the second stiffness of the elastic element of the joint mechanism, and is defined by the stiffness of the elastic element of each of the one or more joint mechanisms When the target value of the stiffness with respect to the displacement of the movable part is defined as the target stiffness, a plurality of the target stiffnesses are prepared in advance, and the stiffness of the elastic element of each of the one or the plurality of joint mechanisms is the plurality of the plurality of target stiffnesses. Necessary for realizing all of the plurality of target stiffnesses and the minimum value of the stiffness of the elastic element of the joint mechanism and the smaller value of the first stiffness necessary for realizing all of the target stiffnesses The joint mechanism is configured to be changeable in a range between a maximum value of rigidity of the elastic element and a larger value of the second rigidity.

  When a predetermined work is performed by the operation of the movable part of the link mechanism (the tip part or the portion between the base and the tip part), the rigidity against the displacement of the movable part is an appropriate rigidity (target) according to the work. Rigidity). For example, when performing precise work, higher rigidity is desired as the rigidity with respect to the displacement of the movable part.

  When various kinds of work are performed by the operation of the movable part of the link mechanism, the target rigidity corresponding to each work is set. That is, when any one of a variety of work is performed, the rigidity with respect to the displacement of the movable portion is controlled to be a target rigidity corresponding to the work among the plurality of target rigidity. For this reason, multiple target rigidity with respect to the displacement of a movable part is prepared beforehand.

  Here, the amount of elastic deformation of the elastic element in the present invention means the relative amount of displacement between the side transmitting power to the elastic element and the side transmitting the power via the elastic element. Further, the inertia moment or inertia mass of the joint mechanism when the joint mechanism is displaced is the inertia moment when the joint mechanism is a rotary joint mechanism, and the inertia mass when the joint mechanism is a linear motion joint mechanism.

  Further, when the joint mechanism is displaced, the moment of inertia or the inertial mass of the joint mechanism changes in accordance with the displacement of each of the one or more joint mechanisms (hereinafter referred to as “link mechanism posture”). The maximum value or the minimum value of the moment of inertia or the inertial mass of the joint mechanism is the maximum value or the minimum value that can be taken within the range of postures that can be realized by the link mechanism.

  According to various studies by the inventor of the present application, when a “movable part or a part between the movable part and the base” collides with an external object (hereinafter referred to as a part that collides with an external object (movable part or movable part). In order to prevent each joint mechanism from being damaged, the portion 1 between the base and the base is 1 so as to satisfy the following two conditions. It is necessary to be able to control the rigidity of the elastic element of one or a plurality of joint mechanisms (hereinafter, such a joint mechanism is referred to as “target joint mechanism”).

(Condition 1)
In each of one or a plurality of target joint mechanisms, the kinetic energy (hereinafter referred to as “collision kinetic energy”) due to the displacement of the target joint mechanism resulting from the collision of the collision unit with the object is represented by the elasticity of the target joint mechanism. The condition that all the elastic energy of the element can be converted.

(Condition 2)
In each of one or a plurality of target joint mechanisms, the target joint mechanism accumulated by the displacement of the target joint mechanism from the start of the displacement of the target joint mechanism due to the collision unit colliding with the object. The time until the displacement of the target joint mechanism by the elastic energy of the elastic element is started, in other words, the time during which the elastic element of the target joint mechanism absorbs the impact (hereinafter referred to as “first time”). However, when the target joint mechanism is displaced at the maximum acceleration that the target joint mechanism can realize by the actuator, the displacement speed of the target joint mechanism is determined by the direction of the relative displacement speed of the target joint mechanism due to the collision. The condition that it is longer than the time (hereinafter referred to as “second time”) that can be changed by the speed obtained by reversing.

  In each of one or a plurality of target joint mechanisms, when the above two conditions are satisfied, an impact on the target joint mechanism due to the collision portion colliding with an object causes an elastic element of the target joint mechanism. (The effect of Condition 1), while the elastic element of the target joint mechanism absorbs the shock, the displacement speed of the target joint mechanism is determined relative to the target joint mechanism due to the collision. It is possible to change the direction of the displacement speed by a reverse speed, and it is possible to prevent the target joint mechanism from being forcibly displaced by the elastic energy of the elastic element of the target joint mechanism. ). By satisfying the above two conditions, the above-described effect can be obtained, so that the target joint mechanism can be prevented from being damaged by the collision portion colliding with the object.

  In the present invention, in order to satisfy the above conditions 1 and 2, the following configuration is adopted.

  First, in order to satisfy the above condition 1, the following configuration is adopted. That is, in each of the one or more joint mechanisms, the elastic energy of the elastic element of the joint mechanism when the elastic deformation amount of the elastic element of the joint mechanism is the maximum deformation amount of the elastic element of the joint mechanism is When the joint mechanism is displaced, the moment of inertia or the inertial mass of the joint mechanism becomes the minimum value that can be taken by the joint mechanism, and when the joint mechanism is displaced by the actuator, the joint mechanism has the maximum displacement speed that can be realized. The stiffness of the elastic element of the joint mechanism, which is equal to or higher than the kinetic energy of the joint mechanism when it is assumed to be displaced (hereinafter referred to as “joint minimum inertial collision kinetic energy”), is defined as the first stiffness.

  The collision kinetic energy increases as the moment of inertia or mass of the joint mechanism increases when the joint mechanism is displaced. The joint minimum inertial collision kinetic energy is the collision kinetic energy when the moment of inertia or the inertial mass of the joint mechanism is the minimum value that the joint mechanism can take when the joint mechanism is displaced. Therefore, under the condition that the rigidity of the elastic element of the joint mechanism and the displacement speed of the joint mechanism are constant, the joint minimum inertial collision kinetic energy is the minimum value that can be taken by the collision kinetic energy.

  When the stiffness of the elastic element of the joint mechanism is set to the first stiffness, when the joint mechanism is displaced, the inertia moment or inertia mass of the joint mechanism is any value that the joint mechanism can take. In addition, Condition 1 is satisfied regardless of any value that can be taken when changing the joint mechanism.

  In the present invention, the rigidity of the elastic element of the joint mechanism is the smaller one of the minimum rigidity value and the first rigidity of the elastic elements of the joint mechanism required to realize all of the plurality of target rigidity. Is set as the minimum value of the variable range of the stiffness of the elastic element of the joint mechanism.

  Therefore, in one or a plurality of joint mechanisms, the rigidity of the elastic element of the joint mechanism is set to the first rigidity, and the rigidity against the displacement of the movable part is necessary to realize all of the plurality of target rigidity. It can also be set to a high rigidity.

  In order to satisfy the above condition 2, the following configuration is adopted. That is, in each of one or a plurality of joint mechanisms, when the joint mechanism is displaced, the inertia moment or the inertial mass of the joint mechanism becomes the maximum value that can be taken by the joint mechanism, and the displacement of the joint mechanism is started. The time from the point in time until the point when the displacement of the joint mechanism is started by the elastic energy of the elastic element of the joint mechanism accumulated by the displacement of the joint mechanism (hereinafter referred to as “third time”) is the actuator Assuming that the joint mechanism is displaced at the maximum acceleration that can be realized by the joint mechanism, the time for changing the displacement speed of the joint mechanism by the maximum displacement speed of the joint mechanism (hereinafter referred to as “fourth”). The second rigidity is set so as to be equal to or greater than “time”.

  Here, the first time increases as the moment of inertia or mass of the joint mechanism increases when the joint mechanism is displaced under the condition that the rigidity of the elastic element of the joint mechanism is constant. The third time is the first time when the moment of inertia or inertial mass of the joint mechanism is the maximum value that the joint mechanism can take when the joint mechanism is displaced. Therefore, under the condition that the rigidity of the elastic element of the joint mechanism is constant, the third time is the maximum length that can be taken by the first time.

  The second time increases as the amount of change in the displacement speed of the joint mechanism increases under the condition that the rigidity of the elastic element of the joint mechanism is constant. The fourth time is a second time when the displacement speed of the joint mechanism is changed by the maximum displacement speed of the joint mechanism. Accordingly, under the condition that the rigidity of the elastic element of the joint mechanism is constant, the fourth time is the maximum length that can be taken by the second time.

  Thus, the second stiffness is set such that the third time, which is the maximum length of the first time, is equal to or greater than the fourth time, which is the maximum length of the second time. When the rigidity of the elastic element of the joint mechanism is set to the second rigidity, the inertial moment or inertial mass of the joint mechanism can be any value that the joint mechanism can take, and the displacement of the joint mechanism The first time is equal to or longer than the second time regardless of the value that can be taken when changing the joint mechanism, and the condition 2 is satisfied.

  In the present invention, the larger one of the maximum stiffness value and the second stiffness of the elastic elements of the joint mechanism, which is necessary to realize all of the plurality of target stiffnesses. Is set as the maximum value of the variable range of the stiffness of the elastic element of the joint mechanism.

  Therefore, in one or a plurality of joint mechanisms, the rigidity of the elastic element of the joint mechanism is set to the second rigidity, and the rigidity against the displacement of the movable part is necessary to realize all of the plurality of target rigidity. It can also be set to a high rigidity.

  As described above, the rigidity of the elastic element of the joint mechanism is set within an appropriate range in order to make the rigidity against the displacement of the movable part equal to any one of one or more predetermined target rigidity. In addition, the rigidity of the elastic elements of the joint mechanism can be set to an appropriate range in order to prevent the joint mechanisms from being damaged due to the collision portion colliding with an external object.

  In the present invention, when any one of the one or more joint mechanisms is defined as the i-th joint mechanism, the first rigidity of the elastic element of the i-th joint mechanism is expressed by the following formula ( It is preferable that it is set to be higher than the rigidity obtained according to 1).

However, I_min_i indicates the minimum moment of inertia or mass of the i-th joint mechanism that can be taken by the i-th joint mechanism when the i-th joint mechanism is displaced, and ωj_max_i indicates the i-th joint mechanism as described above. The maximum displacement speed that can be realized when the actuator is displaced is indicated by xj_lim_i, which indicates the maximum deformation amount of the elastic element of the i-th joint mechanism.

  Here, when the rigidity of the elastic element of the i-th joint mechanism is defined as kj_i, Expression (1) is kj_i that satisfies the following Expression (1-1).

In Formula (1-1), when the i-th joint mechanism is displaced, when the i-th joint mechanism is displaced, the left side indicates that the collision portion is an object when the i-th joint mechanism has a minimum value that can be taken by the i-th joint mechanism. The kinetic energy of the i-th joint mechanism in the case of colliding with (i.e., the joint minimum inertial collision kinetic energy) is shown. Further, the right side shows the first deformation when the elastic deformation amount of the elastic element of the i-th joint mechanism is the maximum deformation amount of the elastic element of the i-th joint mechanism when the rigidity of the elastic element of the i-th joint mechanism is kj_i. The elastic energy of the elastic element of i joint mechanism is shown.

  Accordingly, when the i-th joint mechanism is displaced, the moment of inertia or the inertial mass of the i-th joint mechanism is any value that the i-th joint mechanism can take, and the amount by which the displacement speed of the i-th joint mechanism is changed is The rigidity of the elastic element of the i-th joint mechanism can be set so as to satisfy the above-described condition 1 regardless of any value that can be taken when changing the i-th joint mechanism.

  In the present invention, when any one of the one or more joint mechanisms is defined as the i-th joint mechanism, the second rigidity of the elastic element of the i-th joint mechanism is expressed by the following formula ( It is preferably set to be equal to or less than the rigidity obtained according to 2).

However, I_max_i indicates the maximum value of the moment of inertia or inertial mass of the i-th joint mechanism that can be taken by the i-th joint mechanism when the i-th joint mechanism is displaced, π indicates the circumference, and Aj_max_i Represents the maximum acceleration that can be achieved when the i-th joint mechanism is displaced by the actuator, and ωj_max_i represents the maximum displacement speed that can be achieved when the i-th joint mechanism is displaced by the actuator.

  Here, when the rigidity of the elastic element of the i-th joint mechanism is defined as kj_i, Expression (2) is kj_i that satisfies the following Expression (2-1).

In equation (2-1), the left side indicates that the state of the i-th joint mechanism is the maximum value that can be taken by the i-th joint mechanism when the i-th joint mechanism is displaced. In the state and the stiffness of the elastic element of the i-th joint mechanism is kj_i, a quarter cycle of the period represented by the reciprocal of the natural frequency of the elastic element of the i-th joint mechanism is shown. Specifically, the left side of the elastic element of the i-th joint mechanism accumulated by the displacement of the i-th joint mechanism due to the collision from the time when the displacement of the i-th joint mechanism due to the collision portion colliding with the object starts. The time until the time when the displacement of the i-th joint mechanism by elastic energy is started is shown.

  The right side is the maximum acceleration that can be realized by the i-th joint mechanism. When the i-th joint mechanism is displaced by the actuator, the displacement speed of the i-th joint mechanism is realized when the i-th joint mechanism is displaced by the actuator. The time when changing only the maximum possible displacement speed (the shortest time that can be realized by the i-th joint mechanism) is shown.

  Since the time indicated by the left side becomes longer as the rigidity of the elastic element of the i-th joint mechanism is smaller, the i-th joint mechanism is displaced by setting the second rigidity to be equal to or less than the rigidity obtained by the equation (2). When the moment of inertia or the inertial mass of the i-th joint mechanism is any value that the i-th joint mechanism can take, and when the amount of change in the displacement speed of the i-th joint mechanism changes the i-th joint mechanism The rigidity of the elastic element of the i-th joint mechanism can be set so as to satisfy the condition 2 described above regardless of which value can be taken.

  In the present invention, each of the one or more joint mechanisms is configured to transmit power via the elastic element and a viscous element configured to variably control a viscosity coefficient. Alternatively, the viscosity coefficient of the viscous element of each of the plurality of joint mechanisms can be changed within a range of the first joint viscosity coefficient and the second joint viscosity coefficient set for each of the one or more joint mechanisms. The first joint viscosity coefficient of the viscous element of each of the one or more joint mechanisms is equal to a minimum value in a range in which the rigidity of the elastic element of the joint mechanism can be changed. And the second joint viscosity coefficient of the viscous element of each of the one or more joint mechanisms is set such that the behavior characteristic of the joint mechanism is a critical damping characteristic or an overdamping characteristic. The elasticity of the joint mechanism The rigidity of the element is, when it is assumed to be equal to the maximum value among the changeable range, it is preferable that the behavioral characteristics of the joint mechanism is configured such that the characteristics of critical damping or overdamping.

  Thereby, even if the rigidity of the elastic element of one or a plurality of joint mechanisms is changed to any rigidity within the variable range of the rigidity of the elastic element, The viscosity coefficient at which the behavior characteristic of the joint mechanism becomes the critical damping characteristic or the excessive damping characteristic is included in the range of the first joint viscosity coefficient and the second joint viscosity coefficient.

  Accordingly, it is possible to change the viscosity coefficient of the viscous element of each joint mechanism so that the behavior characteristic of each of the one or more joint mechanisms becomes a critical damping characteristic or an overdamping characteristic. Is prevented from becoming a characteristic of damped vibration, and the collision part can be prevented from vibrating unnecessarily.

The figure which shows the link mechanism of 1st Embodiment of this invention. The figure which shows the joint mechanism of the link mechanism of FIG. The figure which shows the transmission mechanism of the joint mechanism of FIG. The figure which shows the characteristic variable mechanism of the transmission mechanism of FIG. VV sectional view taken on the line of FIG. The figure which shows the link mechanism of 2nd Embodiment of this invention.

[1. First Embodiment]
(1-1. Configuration)
(1-1-1. Link mechanism)
With reference to FIGS. 1-5, the link mechanism of embodiment of this invention is demonstrated. As shown in FIG. 1, the link mechanism 1 includes a base body 2, a tip portion (corresponding to a movable portion of the present invention) 3, and a plurality of joint mechanisms Ji (i = 1, 2,..., N). And a plurality of link members Li (i = 0, 1,..., N) and an actuator 4 (see FIG. 2).

  “I” of the plurality of joint mechanisms Ji represents the order when counting from 1 toward the tip 3 from the base body 2. In addition, when explicitly specifying that the order of the joint mechanism is i-th, it is expressed as the i-th joint mechanism. For example, the j-th joint mechanism may be expressed as “j-th joint mechanism Jj”. Further, “i” of the plurality of link members Li represents the order when counting from 0 toward the tip 3 from the base body 2. In addition, when specifying explicitly that the order of a link member is i-th, it represents with an i-th link member. For example, the jth link member may be represented as “jth link member Lj”.

  The base 2 is fixed at a predetermined position such as on the floor surface. In addition, the base | substrate 2 is equipped with a motive power apparatus etc., and it may be comprised so that the position and attitude | position can be changed with the driving force of this motive power apparatus. The distal end portion 3 functions as an end effector in the link mechanism 1. The tip portion 3 is connected to the base body 2 by a plurality of link members Li that are connected via a plurality of joint mechanisms Ji.

  Two link members Li-1, Li are connected to each of the plurality of joint mechanisms Ji (i = 1,..., N).

  Each of the plurality of joint mechanisms Ji is configured to change the relative positions and postures of the two link members Li-1, Li connected to the joint mechanism Ji.

  In the present embodiment, all the joint mechanisms Ji are configured as rotary joint mechanisms. For this reason, in the following description, the relative position and posture of the two link members Li-1, Li connected to the predetermined joint mechanism Ji may be referred to as a joint angle ψj_i. Note that “the joint angle ψj_i changes” in the present embodiment corresponds to “the joint mechanism is displaced” in the present invention.

  In the present embodiment, all joint mechanisms Ji are configured as rotary joint mechanisms, but each joint mechanism Ji may be configured as either a rotary joint mechanism or a linear motion joint mechanism. Further, the number of joint mechanisms Ji may be an arbitrary number.

  The actuator 4 is provided in each of the plurality of joint mechanisms Ji, and outputs a driving force that displaces each joint mechanism Ji.

(1-1-2. Joint mechanism)
(1-1-2-1. Configuration of joint mechanism)
Next, details of the plurality of joint mechanisms Ji will be described with reference to FIGS. In the present embodiment, each joint mechanism Ji is a rotary joint mechanism having the same configuration.

  As shown in FIG. 2, the joint mechanism Ji includes a wire 11, a driving pulley 12a, a driven pulley 12b, and a transmission mechanism 8 that generates an elastic force and a viscous force between the pulleys 12a and 12b. .

  The reduction gear 7 connected to the actuator output shaft 4a of the actuator 4 is connected to the drive pulley 12a. The drive pulley 12a rotates in conjunction with the rotation of the actuator output shaft 4a by the rotational driving force (torque) applied from the actuator 4 via the speed reducer 7.

  Note that the speed reducer 7 may have an arbitrary structure, and for example, a speed reducer constituted by a harmonic drive (registered trademark) or a plurality of gears may be employed. Alternatively, the speed reducer 7 may include a mechanism that converts a linear motion into a rotational motion. In that case, as the actuator, for example, a linear motion actuator constituted by an electric motor and a ball screw, an electric linear motor, or the like may be employed.

  In the i-th joint mechanism Ji, the i-th link member Li rotatably supports the driving pulley 12a and the driven pulley 12b (not shown). In the i-th joint mechanism Ji, the i-1th link member Li-1 is fixed to the driven pulley 12b.

  The driven pulley 12b is juxtaposed on the side of the drive pulley 12a so that the rotation axis thereof is parallel to the rotation axis of the drive pulley 12a.

  The transmission mechanism 8 includes a characteristic variable mechanism 10 for changing the rigidity and viscosity between the pulleys 12a and 12b, and a wire 11 spanned between the driving pulley 12a and the driven pulley 12b.

  As shown in FIG. 3, the wire 11 has two extending portions 11a and 11b extending between the pulleys 12a and 12b, and portions other than the extending portions 11a and 11b are both pulleys 12a and 12b. Of the outer periphery of the outer periphery of the driving pulley 12a so that it does not slip to any place except the outer periphery of the driving pulley 12a (the portion facing the driven pulley 12b and the outer periphery of the driven pulley 12b facing the driving pulley 12a). It is wrapped around. Note that the wire 11 has some elasticity.

  The characteristic variable mechanism 10 is configured, for example, as shown in FIGS. That is, the variable characteristic mechanism 10 includes a rotating bar 14 having rollers 13a and 13b pivotally attached to both ends. The rotary bar 14 can rotate integrally with the rotary shaft 15 around the axis of the rotary shaft 15 fixed at the center thereof. The rotation shaft 15 is disposed in a position parallel to the rotation axis of the pulleys 12a and 12b at a position between the driving pulley 12a and the driven pulley 12b.

  The rollers 13a and 13b at both ends of the rotating bar 14 have their respective rotation axes oriented in parallel to the rotation axes of the drive pulley 12a and the driven pulley 12b.

  And the outer peripheral part of the inner end side (side facing the other roller 13b) of one roller 13a of the rollers 13a and 13b is one extending portion of the two extending portions 11a and 11b of the wire 11 The outer peripheral portion on the inner end side (the side facing one roller 13a) of the other roller 13b is in pressure contact with the other stretched portion 11b of the wire 11. In this case, each of the extending portions 11a and 11b of the wire 11 is curved at the press contact portion of the rollers 13a and 13b.

  The variable characteristic mechanism 10 further includes a gear (spur gear) 16 that is connected to the rotary bar 14 via a rotary shaft 15 so as to be rotatable integrally with the rotary bar 14, and a spring meshed with the gear 16. A worm 17, an electric motor 18 that rotationally drives the spring worm 17, and a cylinder 19 in which viscous oil is sealed are provided.

  The spring worm 17 is a spring member formed in a coil spring shape so as to be able to function as a worm gear, and is externally attached to the rotation drive shaft 18 a of the electric motor 18. One end of the spring worm 17 near the main body of the electric motor 18 is fixed to a spring seat member 20a fixed to the rotation drive shaft 18a. Accordingly, the spring worm 17 rotates integrally with the rotation drive shaft 18a of the electric motor 18, and the gear 16 rotates as the spring worm 17 rotates.

  The cylinder 19 has a cylindrical portion 21 disposed coaxially with the rotation drive shaft 18 a on the other end side of the spring worm 17. A rotation drive shaft 18a of the electric motor 18 passes through the inside of the cylinder portion 21, and the cylinder portion 21 is slidable in the axial direction along the rotation drive shaft 18a. The other end of the spring worm 17 is fixed to a spring seat member 20b fixed to the end face of the cylindrical portion 21 on the spring worm 17 side. Therefore, as the spring worm 17 expands and contracts, the cylinder portion 21 of the cylinder 19 slides in the axial direction of the rotation drive shaft 18 a of the electric motor 18.

  In addition, a piston 22 fixed to the rotary drive shaft 18 a is provided inside the cylinder portion 21, and an outer peripheral surface of the piston 22 is in sliding contact with an inner peripheral surface of the cylinder portion 21.

  Then, viscous oil is enclosed in two oil chambers 23 a and 23 b defined by the piston 22 inside the cylinder portion 21. These oil chambers 23 a and 23 b are communicated by a communication pipe 25 having an orifice portion 24. In this case, the orifice portion 24 can change the opening area by a valve mechanism or the like (not shown).

(1-1-2-2. Operation of joint mechanism)
The operation of the joint mechanism Ji having the above configuration will be described. When the spring worm 17 is rotationally driven by the electric motor 18, the rotating bar 14 is rotated via the gear 16 meshed with the spring worm 17. Therefore, the rotation angle (phase angle) of the rotating bar 14 can be controlled by the servo control of the electric motor 18. Here, in the following description, as shown in FIG. 3, the phase angle of the rotary bar 14 is the direction in which the rotary bar 14 extends (the interval between the rollers 13a and 13b), and the interval between the drive pulley 12a and the driven pulley 12b. Is defined as a rotation angle φ of the rotating bar 14 from a state perpendicular to the rotation bar 14.

  In a state where power transmission (transmission of rotational driving force) between the pulleys 12a and 12b is not performed, the phase angle φ of the rotary bar 14 is controlled to a predetermined angle value (for example, φ0 in FIG. 3). Assume a state in which the rotation of the rotation drive shaft 18a of the electric motor 18 (and hence the rotation of the spring worm 17) is stopped (hereinafter, this state is referred to as a reference state).

  In this reference state, when a rotational driving force (torque) is applied from the actuator 4 to the driving pulley 12a, a tension proportional to the rotational driving force is generated in one of the stretched portions 11a and 11b of the wire 11, and the tension The rotational driving force is transmitted from the driving pulley 12a to the driven pulley 12b.

  At the same time, due to the tension generated in one of the stretched portions 11a and 11b of the wire 11, both pulleys 12a and 13b are brought into contact with the roller 13a or 13b of the rollers 13a and 13b that are in contact with the stretched portion 11a or 11b. A translational force in a direction substantially orthogonal to the interval direction of 12b acts.

  For example, as shown in FIG. 3, when a torque τd in the counterclockwise direction is applied to the drive pulley 12a, a tension Te proportional to the torque τd (= τd / the effective rotation radius of the drive pulley 12a) ) Occurs, and the translation force F acts on the roller 13a by the tension Te. The magnitude of the translational force F is substantially proportional to the torque τd. Further, the tension Te in FIG. 3 indicates the tension acting on the roller 13a.

  When the phase angle of the rotating bar 14 in the reference state is not zero (for example, the situation shown in FIG. 3), the rotation is caused by the translational force acting on the roller 13a or 13b (hereinafter referred to as F). A rotational driving force (torque) acts on the bar 14. As a result, the drive pulley 12a rotates relative to the driven pulley 12b, and the rotating bar 14 rotates. As a result, the gear 16 meshing with the spring worm 17 rotates integrally with the rotating bar 14.

  In this case, the torque (hereinafter referred to as τa) acting on the rotating bar 14 due to the translational force F acting on the roller 13a or 13b is expressed by the following equation (1122-1) with respect to the translational force F. Have the relationship. As shown in FIG. 3, φ0 is the value of the phase angle φ of the rotating bar 14 in the reference state, and Ra is the rotation radius of the shaft center portion of the rollers 13a and 13b around the shaft center of the rotating shaft 15. .

τa = F · sin (φ0) · Ra (1122-1)
Since the spring worm 17 does not rotate in the situation where the torque τa is applied to the rotary bar 14 as described above, a part of the spring worm 17 (specifically, the meshed portion of the gear 16 and the electric motor 18 are rotated by the rotation of the gear 16. The portion between the side spring seat member 20a) is extended or shortened, and the spring worm 17 generates an elastic force corresponding to the amount of expansion / contraction.

  In this case, the amount of expansion and contraction of the spring worm 17 from the reference state, and hence the amount of rotation of the rotating bar 14 from the reference state (the amount of change in phase angle) is applied to the gear 16 by the elastic force (translation force) of the spring worm 17. The applied torque balances with the torque acting on the rotating bar 14 (= the torque acting on the gear 16) due to the translational force F acting on the roller 13 a or 13 b due to the tension of the wire 11. In this equilibrium state, the torque τd applied to the drive pulley 12a is transmitted to the driven pulley 12b via the transmission mechanism 8.

  The rotation amount of the rotating bar 14 from the reference state in the equilibrium state is Δφ [rad], the rotation radius of the gear 16 is Rb, the rigidity of the spring worm 17 (the elasticity generated per unit change amount of the expansion / contraction amount of the spring worm 17). If the force change amount is k_sp_w, the torque acting on the gear 16 by the elastic force of the spring worm 17 in the equilibrium state (hereinafter referred to as τb) is given by the following equation (1122-2). It is done.

τb = k_sp_w · sin (Δφ) · Rb≈k_sp_w · Δφ · Rb (1122-2)
From the equation (1122-2) and the equation (1122-1), the relationship between the translational force F in the equilibrium state and the rotation amount Δφ from the reference state of the rotating bar 14 is expressed by the following equation (1122-3) Given by.

F = (k_sp_w · Rb / (sin (φ0) · Ra)) · Δφ (1122-3)
Therefore, the translational force F acting on the roller 13a or 13b by the tension of the wire 11 is proportional to the rotation amount Δφ of the rotating bar 14 from the reference state.

  Here, the translational force F acting on the roller 13a or 13b by the tension of the wire 11 increases as the torque applied to the drive pulley 12a (and thus the torque transmitted to the driven pulley 12b) increases. Further, the relative rotation amount of the drive pulley 12a with respect to the driven pulley 12b (relative rotation amount from the reference state) increases as the rotation amount of the rotation bar 14 from the reference state in the equilibrium state increases.

  Therefore, in a steady state where the relative rotation amount between the pulleys 12a and 12b is kept constant, torque transmitted from the driving pulley 12a to the driven pulley 12b (torque applied to the driven pulley 12b) is expressed as τsp, When the relative rotation amount between the pulleys 12a and 12b is expressed as Δθ, a proportional relationship of the following equation (1122-4) is approximately established between τsp and Δθ.

τsp = k · Δθ (1122-4)
Therefore, the transmission mechanism 8 functions as a spring member that transmits power between the driving pulley 12a and the driven pulley 12b. The torque τsp corresponds to torque (hereinafter referred to as “elastic force torque”) τsp transmitted by the elastic force generated between the pulleys 12a and 12b by the transmission mechanism 8. In this case, k in the equation (1122-4) is the ratio of the change in the elastic force torque τsp to the change in the relative rotation amount Δθ between the pulleys 12a and 12b.

  This k indicates the rigidity between the pulleys 12a and 12b. The larger the value of k, the higher the rigidity between the pulleys 12a and 12b (the difference in the amount of rotation between the pulleys 12a and 12b is less likely to occur. Means). The value of the rigidity k of the transmission mechanism 8 basically corresponds to the phase angle φ0 of the rotating bar 14 in the reference state, and the value of the rigidity k decreases as φ0 increases.

  In order for a control device (not shown) configured by a CPU or the like to control the rigidity k of the transmission mechanism 8 to a desired rigidity, for example, the following is performed. First, a table in which the phase angle φ0 of the rotating bar 14 in the reference state is associated with the stiffness k is stored and held in a storage device such as a memory of the control device. Then, the control device acquires the phase angle φ0 of the rotating bar 14 in the reference state with respect to the desired rigidity according to the stored table, so that the phase angle φ0 of the rotating bar 14 becomes the phase angle φ0. The electric motor 18 is controlled. Thereby, the rigidity k of the transmission mechanism 8 is controlled to be the desired rigidity.

  Here, the electric motor 18, the spring worm 17, the gear 16, the rotating bar 14, and the wire 11 are collectively referred to as the elastic element 5. The rigidity k between the pulleys 12a and 12b is referred to as the rigidity kj of the elastic element 5. Further, the relative rotation amount Δθ between the pulleys 12a and 12b becomes the elastic displacement amount of the elastic element 5 (that is, corresponds to the elastic deformation amount of the elastic element in the present invention).

  Further, in the transmission mechanism 8 of the present embodiment, when the rotary bar 14 rotates from the reference state, the cylinder portion 21 of the cylinder 19 slides relative to the piston 22 as the spring worm 17 expands and contracts.

  At this time, when viscous oil flows between the oil chambers 23a and 23b through the communication pipe 25 having the orifice portion 24, a viscous force serving as a resistance force against the sliding of the cylindrical portion 21 is generated. As a result, the rotation of the rotating bar 14 from the reference state, and consequently, a viscous force that is a resistance force to the relative rotation of the driving pulley 12a with respect to the driven pulley 12b is generated between the pulleys 12a and 12b. And this viscous force will change by changing the opening area of the orifice part 24. FIG.

  In this case, the viscous force generated in the cylinder 19 when the opening area of the orifice portion 24 is kept constant is proportional to the moving speed of the cylinder portion 21 with respect to the piston 22, and consequently the expansion / contraction speed of the spring worm 17. The expansion / contraction speed of the spring worm 17 is substantially proportional to the rotation speed of the rotary bar 14.

  Further, the temporal change rate of the relative rotation amount between the pulleys 12a and 12b, that is, the relative rotation speed between the pulleys 12a and 12b (difference between the rotation angular velocities of the pulleys 12a and 12b) Depending on the rotational speed, the greater the rotational speed of the rotating bar 14, the greater the relative rotational speed between the pulleys 12a, 12b.

  Therefore, the relative rotational speed between the pulleys 12a and 12b is expressed as Δω_pulley [rad / s], and the torque (hereinafter referred to as viscous torque) applied to the driven pulley 12b by the viscous force between the pulleys 12a and 12b is τdp. Is expressed as the following equation (1122-5) approximately between Δω_pulley and τdp.

τdp = C · Δω_pulley (1122-2)
Therefore, the transmission mechanism 8 also has a function of generating a viscous force between the driving pulley 12a and the driven pulley 12b. In this case, C in the expression (1122-5) is a ratio of the change in the viscous torque τdp with respect to the change in the relative rotational speed Δω_pulley between the pulleys 12a and 12b, and is hereinafter referred to as a viscosity coefficient C.

  The viscosity coefficient C indicates the degree of viscosity between the pulleys 12a and 12b. The larger the value of C, the higher the viscosity between the pulleys 12a and 12b (viscosity generated between the pulleys 12a and 12b). Is likely to grow). The value of the viscosity coefficient C of the transmission mechanism 8 basically corresponds to the opening area of the orifice portion 24, and the value of C decreases as the opening area increases.

  In order to control the viscosity coefficient C of the transmission mechanism 8 to a desired viscosity coefficient by a control device (not shown) configured by a CPU or the like, for example, the following is performed. First, a table in which the opening area of the orifice portion 24 is associated with the viscosity coefficient C is stored and held in a storage device such as a memory of the control device. Then, the control device acquires the opening area of the orifice part 24 with respect to the desired viscosity coefficient according to the stored table, and controls the opening area of the orifice part 24 to be the acquired opening area. . Thereby, the viscosity coefficient C of the transmission mechanism 8 is controlled to be the desired viscosity coefficient.

  Hereinafter, the cylinder 19 and the orifice portion 24 are collectively referred to as the viscous element 6. The viscosity coefficient C between the pulleys 12a and 12b is referred to as the viscosity coefficient Cj of the viscous element 6.

(1-2. Rigidity against tip end displacement)
The stiffness ke with respect to the displacement of the distal end portion 3 (hereinafter simply referred to as “stiffness of the distal end portion 3”) is defined according to the stiffness kj of the elastic element 5 of each joint mechanism Ji. Therefore, when a stiffness ke_cmd that is a target value of the stiffness ke of the distal end portion 3 (hereinafter referred to as “target stiffness”) ke_cmd is set, the stiffness kj of the elastic element 5 of each joint mechanism Ji is set according to the target stiffness ke_cmd. By setting, the rigidity ke of the tip 3 can be set to be the target rigidity ke_cmd.

  Hereinafter, an example of how to set the stiffness kj of the elastic element 5 of each joint mechanism Ji when the target stiffness ke_cmd of the distal end portion 3 is given will be described.

  In the following description, in the case where the symbol relating to the i-th joint mechanism Ji (where i represents the order of the joint mechanism when counted from the base body 2 toward the tip portion 3) is explicitly expressed, Append "_i" to the end of.

  In the present embodiment, a coordinate system fixed to the base 2 (hereinafter referred to as “base coordinate system”) is used unless otherwise specified. When explicitly expressing values related to the x-axis, y-axis, and z-axis in the base body coordinate system, “_x”, “_y”, and “_z” are added to the end of the code.

  A force (hereinafter referred to as “tip force”) Fe (= (Fe_x, Fe_y, Fe_z)) acting on the distal end portion 3 is represented by the following equation (12-1).

Fe = ke · Δxe (12-1)
However, Δxe (= (Δxe_x, Δxe_y, Δxe_z)) is a change amount of the displacement of the tip portion 3 (hereinafter referred to as “tip portion displacement change amount”). Further, ke is a coefficient matrix that defines the relationship between the tip portion displacement change amount Δxe and the tip force Fe.

  The coefficient matrix ke is, for example, a diagonal matrix, and each diagonal component represents the rigidity ke_x, ke_y, ke_z with respect to the displacement of the tip 3 in the x, y, and z directions. In this case, when each diagonal matrix element el1, el2, ..., eln is used to represent the diagonal matrix as "diag (el1, el2, ..., eln)", ke is "diag ( ke_x, ke_y, ke_z) ". Note that the coefficient matrix ke need not be a diagonal matrix.

  Hereinafter, in this specification, the description “diag (...)” represents a diagonal matrix having the elements of “...” as diagonal components.

  Here, as the target force Fe and the tip portion displacement change amount Δxe, a three-element vector representing the three translational directions of x, y, and z is used, but there are three x-axis, y-axis, and z-axis. A six-element vector may be used by further adding three elements representing rotational components (roll, pitch, yaw) about the axis. In this case, the target rigidity ke and the like are appropriately set according to the elements of the target force Fe and the tip portion displacement change amount Δxe.

  Further, the tip portion displacement change amount Δxe is expressed by the following equation (12-2).

Δxe = J · Δψj (12-2)
However, J is a Jacobian matrix that defines the relationship between the displacement speed of the tip 3 and the joint displacement speed (hereinafter referred to as “joint displacement speed”) ωj_i of each joint mechanism Ji in the base coordinate system. Δψj (= (Δψj_1, Δψj_2,..., Δψj_N)) is a change amount of the joint angle ψj_i of each joint mechanism Ji (hereinafter referred to as “joint angle change amount”).

  Further, the torque (hereinafter referred to as “joint torque”) τj (= (τj_1, τj_2,..., Τj_N)) of each joint mechanism Ji is expressed by the following equation (12-3) using the tip force Fe. It is expressed in

τj = J ^T · Fe (12-3)
Here, J ^T is a transposed matrix of the Jacobian matrix J described above.

  Each joint torque τj is expressed by the following equation (12-4) using the joint angle change amount Δψj of each joint mechanism Ji.

τj = kten · Δψj (12-4)
Here, kten is a coefficient matrix representing the rigidity of the elastic element 5 of each joint mechanism Ji, representing the relationship of the joint torque τj of each joint mechanism Ji generated by the joint angle change amount Δψj_i of each joint mechanism Ji.

  Kten at this time is represented by the following equation (12-5) from the above-described equations (12-1) to (12-4).

kten = J ^T・ ke ・ J (12-5)
The Jacobian matrix J and its transpose matrix J ^T are determined according to the posture of the link mechanism 1 (that is, the joint angle ψj_i of each joint mechanism Ji). For this reason, kten is defined according to the joint angle ψj_i of each joint mechanism Ji and the rigidity ke of the tip 3 according to the equation (12-5). Hereinafter, each element of kten is represented by kten_a_b (a, b = 1, 2,..., N). N at this time is the number of joint mechanisms Ji. The first subscript (ie, a) represents the row number of the matrix, and the last subscript (ie, b) represents the column number of the matrix.

  Further, from the equation (12-4), the torque τj_i of the i-th joint mechanism Ji is represented by the following equation (12-6).

As described above, the joint torque τj_i of the i-th joint mechanism Ji includes not only the joint angle change amount Δψj_i of the i-th joint mechanism Ji but also other joint mechanisms Jj (j = 1, 2,..., I− 1, i + 1,..., N) is affected by the joint angle change amount Δψj_j.

  Therefore, in each joint mechanism Ji, first, a torque other than the torque (kten_i_i · Δψj_i) obtained by multiplying the joint angle change amount Δψj_i of the joint mechanism Ji and the rigidity kten_i_i of the elastic element 5 of the joint mechanism Ji is obtained. Considered as disturbance τr. This disturbance τr can be absorbed by the viscosity of the viscous element 6. For this reason, torques other than the torque (kten_i_i · Δψj_i) obtained by multiplying the stiffness kten_i_i of the elastic element 5 of the joint mechanism Ji may be ignored.

  Therefore, in kten obtained by the above equation (12-5), each diagonal component of the matrix kten_diag (= diag (kten_1_1, kten_2_2,..., Kten_N_N)) in which elements other than the diagonal component are set to 0, The stiffness kj_i of the elastic element 5 of each joint mechanism Ji is set. At this time, kten_i_i is the rigidity kj_i of the elastic element 5 of the i-th joint mechanism Ji. As a more specific example, kten_1_1 is the rigidity kj_1 of the elastic element 5 of the first joint mechanism J1, and kten_2_2 is the rigidity kj_2 of the elastic element 5 of the second joint mechanism J2.

(1-3. Characteristic variable range)
(1-3-1. Rigidity variable range)
When a predetermined work is performed by the operation of the movable part (the tip part 3 in the present embodiment) of the link mechanism 1, the rigidity ke against the displacement of the tip part 3 has an appropriate rigidity according to the work. For example, in the case of performing precise work, higher rigidity is desired as the rigidity ke against the displacement of the tip 3.

  When various kinds of work are performed by the operation of the distal end portion 3 of the link mechanism 1, the target rigidity ke_cmd_k corresponding to each work is set. That is, when performing any one of various work, the rigidity with respect to the displacement of the tip portion 3 is controlled so as to become the target rigidity ke_cmd_k corresponding to the work among the plurality of target rigiditys ke_cmd_k. The For this reason, a plurality of target stiffnesses ke_cmd for the displacement of the distal end portion 3 are prepared in advance.

  Further, the stiffness kj of the elastic element 5 of each joint mechanism Ji is the minimum value (hereinafter referred to as “first”) of the stiffness kj of the elastic element 5 of the joint mechanism Ji necessary to realize all of the plurality of target stiffnesses ke_cmd. It is configured to be changeable within a range between kj1_i (referred to as “joint stiffness”) and a maximum value (hereinafter referred to as “second joint stiffness”) kj2_i.

  Thereby, in each joint mechanism Ji, the range in which the rigidity of the elastic element 5 can be changed is set to a range necessary for realizing all of the plurality of target stiffnesses ke_cmd. Thus, it is not necessary to set the setting range of the rigidity kj of the elastic element 5 of each joint mechanism Ji to an excessively wide range. Accordingly, the rigidity kj of the elastic element 5 of the joint mechanism Ji necessary for realizing all of the plurality of target rigiditys ke_cmd defined in advance can be set within an appropriate range.

  Hereinafter, details of the setting range of the stiffness kj of the elastic element 5 of each joint mechanism Ji will be described.

  A plurality of target stiffnesses are represented as ke_cmd_k (k = 1, 2,..., K). That is, in the present embodiment, K target stiffnesses are set. Note that K is basically the same number as the above-described work, and can be set to an arbitrary number. These target stiffnesses ke_cmd_k are set to a large value when, for example, it is desired to precisely control the operation of the tip 3, and when the tip 3 collides with an obstacle or the like, A small value is set when it is desired to reduce the generated impact.

  As shown in the above equation (12-5), the stiffness kj_i of the elastic element 5 of each joint mechanism Ji depends on the target stiffness ke_cmd_k and the posture of the link mechanism 1 (that is, the joint angle ψj_i of each joint mechanism Ji). It is prescribed.

  In each of a plurality of operations, the posture of the link mechanism 1 that can be taken for the tip 3 to perform the operation is defined within a certain range (hereinafter referred to as “working posture range”). For this reason, in each work, the range of the rigidity kj_i of the elastic element 5 of each joint mechanism Ji is defined according to the target stiffness ke_cmd_k for the work and the work posture range for the work.

  This also means that the range of the stiffness kj_i of the elastic element 5 of each joint mechanism Ji for each work is defined.

  Therefore, in each joint mechanism Ji, the first joint rigidity (minimum value) kj1_i of the elastic element 5 of the joint mechanism Ji is the minimum value kj_rmin_i of the range kj_range_i of the rigidity kj_i of the elastic element 5 of the joint mechanism Ji for each work. The smallest value is set (see the following equation (131-1)).

kj1_i = min (kj_rmin_1, kj_rmin_2, ..., kj_rmin_K) (131-1)
However, “min (...)” represents a function for obtaining the minimum value among the elements included in “...”.

  The first joint stiffness kj1_i set by the equation (131-1) is the “minimum value of the stiffness of the elastic element of the joint mechanism necessary to realize all of the plurality of target stiffnesses” in the present invention. Equivalent to.

  In each joint mechanism Ji, the second joint stiffness (maximum value) kj2_i of the elastic element 5 of the joint mechanism Ji is the maximum value kj_rmax_i of the range kj_range_i of the stiffness kj_i of the elastic element 5 of the joint mechanism Ji for each work. The largest value is set (see the following equation (131-2)).

kj2_i = max (kj_rmax_1, kj_rmax_2, ..., kj_rmax_K) (131-2)
However, “max (...)” represents a function for obtaining the maximum value among the elements included in “...”.

  The second joint stiffness kj2_i set by the equation (131-2) is the “maximum value of the stiffness of the elastic element of the joint mechanism necessary for realizing all of the plurality of target stiffnesses” in the present invention. Equivalent to.

Note that the rigidity kj_i of the elastic element 5 of each joint mechanism Ji in an arbitrary work posture for each work is defined as follows. The Jacobian matrix J defined by the posture of the link mechanism 1 for each work (ie, the joint angle ψj_i of each joint mechanism Ji) and its transposition matrix J ^T, and the target stiffness ke_cmd_k corresponding to the target stiffness ke_cmd_k for each task, The coefficient matrix kten is defined according to the above-described equation (12-5). As a result, a matrix kten_diag in which elements other than the diagonal components of the coefficient matrix kten are set to 0 is defined. Each element kten_i_i of the diagonal component of the matrix kten_diag is the rigidity kj_i of the elastic element 5 of each joint mechanism Ji.

  In addition, the range of the stiffness kj_i of the elastic element 5 of each joint mechanism Ji defined as described above at any posture within the work posture range of the link mechanism 1 in any one work (the entire work posture range). The corresponding range) is the range of the stiffness kj_i of the elastic element 5 of the joint mechanism Ji corresponding to the one work.

  In this embodiment, in each joint mechanism Ji, the first joint stiffness kj1_i or more of the elastic element 5 of the joint mechanism Ji defined as described above, and the joint mechanism Ji defined as described above. The elastic element 5 of the joint mechanism Ji is configured so that the rigidity kj of the elastic element 5 of the joint mechanism Ji can be changed within the range of the second joint rigidity kj2_i or less of the elastic element 5.

(1-3-2. Viscosity coefficient variable range)
In each joint mechanism Ji, the viscosity coefficient Cj_i of the viscous element 6 is configured to be changeable within the range of the first joint viscosity coefficient Cj1_i and the second joint viscosity coefficient Cj2_i. Here, the first joint viscosity coefficient Cj1_i is the behavior characteristic of the i-th joint mechanism Ji when it is assumed that the stiffness kj_i of the elastic element 5 of the i-th joint mechanism Ji is equal to the first joint stiffness (minimum value) kj1_i. Is set to be critically damped or overdamped. Further, when it is assumed that the stiffness kj_i of the elastic element 5 of the i-th joint mechanism Ji is equal to the second joint stiffness (maximum value) kj2_i, the second joint viscosity coefficient Cj2_i is the behavior characteristic of the i-th joint mechanism Ji. It is set to have a characteristic of critical damping or overdamping.

  As a result, the stiffness kj_i of the elastic element 5 of each joint mechanism Ji is changed to any stiffness kj_i in the range of the first joint stiffness (minimum value) kj1_i and the second joint stiffness (maximum value) kj2_i. Even so, with respect to the changed stiffness kj_i, the viscosity coefficient Cj_i at which the behavior characteristic of the i-th joint mechanism Ji becomes a critical damping or overdamping characteristic is equal to or higher than the first joint viscosity coefficient Cj1_i and the second joint viscosity. It is included in the range below the coefficient Cj2_i.

  Therefore, for example, it becomes possible to change the viscosity coefficient Cj_i of the viscous element 6 of each joint mechanism Ji so that the behavior characteristic of each joint mechanism Ji becomes a critical damping characteristic or an overdamping characteristic by the control of the control device. It is possible to prevent the behavioral characteristics of each joint mechanism Ji from becoming a damped vibration characteristic. Thereby, it can prevent that the front-end | tip part 3 vibrates unnecessarily.

  Specifically, the first joint viscosity coefficient Cj1_i is determined according to the following expression (132-1), and the second joint viscosity coefficient Cj2_i is determined according to the following expression (132-2).

However, in Expressions (132-1) and (132-2), ζ represents an attenuation ratio. The damping ratio ζ is set so that the behavior characteristic of the target joint mechanism Ji becomes a critical damping characteristic or an excessive damping characteristic (that is, ζ is set to 1 or more).

  Further, I_min_i is the smallest moment of inertia among the moments of inertia I_t_i when the i-th target joint mechanism Ji is displaced in each of all possible combinations of “joint angles ψj_i that each joint mechanism Ji can take”. It is. Further, I_max_i is the largest moment of inertia among the moments of inertia I_t_i when the i-th target joint mechanism Ji is displaced in each of all possible combinations of “joint angles ψj_i that each joint mechanism Ji can take”. It is.

  Here, in each target joint mechanism Ji, the inertia moment I_t_i when the target joint mechanism Ji is displaced is, for example, from the position and posture of each link member Li obtained according to the joint angle ψj_i of each joint mechanism Ji. It calculates | requires according to Formula (132-3).

However, the symbol “x” represents an outer product. I_k is an inertia tensor for rotating the k-th joint mechanism Jk when focusing only on the k-th joint mechanism Jk in the base coordinate system. Further, rgc_k (= (rgc_k_x, rgc_k_y, rgc_k_z)) is a position vector indicating the gravity center position of the k-th link member Lk, and r_i (= (r_i_x, r_i_y, r_i_z)) is the i-th target joint mechanism Ji It is a position vector indicating the position of the center of rotation. m_k is the mass of the k-th link member Lk. Further, a_i (= (a_i_x, a_i_y, a_i_z)) is a rotation axis vector representing the rotation direction of the i-th target joint mechanism Ji. a_i is appropriately set to indicate the rotation axis of the i-th target joint mechanism Ji.

  “|| I_k · a_i ||” in the expression (132-3) represents the link member Lk (k = i, i + 1,..., N between the i-th target joint mechanism Ji and the tip 3. −1, N), the moment of inertia when the link member Lk rotates around an axis parallel to the rotation center axis of the i-th target joint mechanism Ji and passing through the center of gravity of the link member Lk , Referred to as “first moment of inertia”). Therefore, I_A in the expression (132-3) represents each link member Lk (k = i, i + 1,..., N-1, N) between the i-th target joint mechanism Ji and the tip 3. Is the sum of the first moments of inertia at.

In addition, “|| a_i × (rgc_k−r_i) || ² · m_k” in Expression (132-3) is the link member Lk (k = i, k) between the i-th target joint mechanism Ji and the distal end portion 3. i + 1,..., N−1, N), the moment of inertia when the link member Li is rotated around the rotation axis of the i-th target joint mechanism Ji (hereinafter referred to as “second moment of inertia”). ). Therefore, I_B in the expression (132-3) represents each link member Lk (k = i, i + 1,..., N-1, N) between the i-th target joint mechanism Ji and the tip 3. Is the sum of the second moments of inertia at.

  Note that, in each target joint mechanism Ji, the moment of inertia I_t_i when the target joint mechanism Ji is displaced is not limited to that calculated according to the equation (132-3), but may be calculated using another arithmetic equation or the like. .

  Note that rgc_k changes according to the joint angle ψj of each of the first joint mechanism J1 to the k-1th joint mechanism Jk-1. Moreover, r_i and a_i change according to each joint angle (psi) j of 1st joint mechanism J1-i-1th joint mechanism Ji-1. In addition, I_k and m_k are uniquely determined when the structure of the link mechanism 1 is determined.

(1-4. Control)
Next, in the link mechanism 1 configured as described above, an example of a method for controlling the rigidity kj_i of the elastic element 5 and the viscosity coefficient Cj_i of the viscous element 6 by the control device (not shown) will be described. To do.

(1-4-1. Rigidity)
The control device determines the target stiffness ke_cmd for the displacement of the distal end portion 3 in accordance with the operation of the link mechanism 1 or an external signal (for example, a signal output by an operator or the like operating the controller). To do. The target stiffness ke_cmd at this time is determined as any one of a plurality of target stiffnesses ke_cmd_k determined in advance.

The control device then detects the Jacobian matrix J from the detection result of the angle sensor (not shown) that detects the joint angle ψj_i of the joint mechanism Ji provided in each joint mechanism Ji (that is, the current posture of the link mechanism 1). And its transpose matrix J ^T. Then, the control device uses the calculated Jacobian matrix J, its transposed matrix J ^T , and the target stiffness ke_cmd to calculate the matrix kten according to the above equation (12-5).

  Then, the control device obtains a matrix kten_diag in which components other than the diagonal component of the matrix kten are set to 0, and each of the diagonal components kten_i_i of the matrix kten_diag is determined as the target joint stiffness kj_cmd_i of the elastic element 5 of each joint mechanism Ji. Set as.

  Then, the control device controls the rigidity kj_i of the elastic element 5 of each joint mechanism Ji so as to become the set target joint rigidity kj_cmd_i. Thereby, the control device can control the stiffness ke of the tip 3 defined by the stiffness kj of the elastic element 5 of each joint mechanism Ji so as to become the target stiffness ke_cmd of the tip 3.

(1-4-2. Viscosity coefficient)
In each joint mechanism Ji, the control device sets the target joint viscosity coefficient Cj_cmd_i, which is the target value of the viscosity coefficient of the viscous element 6 of the joint mechanism Ji, according to the target joint stiffness kj_cmd_i set as described above, as follows: Determine according to (142).

However, in the equation (142), ζ represents an attenuation ratio as in the equations (132-1) and (132-2). The damping ratio ζ is set so that the behavior characteristic of the target joint mechanism Ji becomes a critical damping characteristic or an excessive damping characteristic (that is, ζ is set to 1 or more).

  Further, I_t_i is obtained according to the above-described equation (132-3) according to information such as the joint angle ψj_i of each joint mechanism Ji at the present time.

  Then, the control device controls the viscosity coefficient Cj_i of the viscous element 6 of each joint mechanism Ji to be the determined target joint viscosity coefficient Cj_cmd_i.

  Thereby, the behavior characteristic of each joint mechanism Ji becomes a critical damping characteristic or an over-damping characteristic, and is prevented from becoming a damping vibration characteristic. Therefore, it is possible to prevent the tip portion 3 from vibrating unnecessarily.

[2. Second Embodiment]
Next, the link mechanism 101 of 2nd Embodiment of this invention is demonstrated.

  Since the structure of the link mechanism 101 of this embodiment is the same as the link mechanism 1 of 1st Embodiment, description is abbreviate | omitted.

(2-1. Characteristic variable range)
In the link mechanism 101 of the present embodiment, the rigidity kj of the elastic element 5 of each joint mechanism Ji is the minimum of the rigidity kj of the elastic element 5 of the joint mechanism Ji necessary to realize all of the plurality of target rigidity ke_cmd. At least a changeable range is set between the value (that is, the first joint stiffness) kj1_i and the maximum value (ie, the second joint stiffness) kj2_i, and any one between the base 2 and the tip 3 Even when an external object Obj collides with the link member Li, the rigidity kj_i of the elastic element 5 of each joint mechanism Ji can be changed so that each joint mechanism Ji can be prevented from being damaged. A changeable range of the rigidity kj_i of the elastic element 5 of the joint mechanism Ji is set.

  Details of these will be described below.

  Here, a link member that collides with the object Obj is defined as a collision link member Lcoll (see FIG. 6). One or more joint mechanisms Ji between the base 2 and the collision link member Lcoll are defined as target joint mechanisms Ji. Further, any of the link members Li (i = 1, 2,..., N) having one or a plurality of joint mechanisms Ji between the base body 2 and the object Obj may collide, Any of these link members Li (i = 1, 2,..., N) can be the collision link member Lcoll.

(2-1-1. Rigidity variable range)
According to various studies by the inventors of the present application, when the collision link member Lcoll collides with the object Obj, in order to prevent the entire joint mechanism Ji from being damaged, each target joint is satisfied so as to satisfy the following two conditions. It is necessary to be able to control the rigidity kj_i of the elastic element 5 of the mechanism Ji.

(Condition 1)
In each target joint mechanism Ji, the kinetic energy (hereinafter referred to as “collision kinetic energy”) due to the displacement of the target joint mechanism Ji caused by the collision link member Lcoll colliding with the object Obj is used as the elasticity of the target joint mechanism Ji. The condition that all the elastic energy of element 5 can be converted.

(Condition 2)
The condition that the first time is equal to or longer than the second time in each target joint mechanism Ji.

  Here, the first time is “accumulated by the displacement of the target joint mechanism Ji” from “the time when the displacement of the target joint mechanism Ji caused by the collision link member Lcoll colliding with the object Obj”. The time until the displacement of the target joint mechanism Ji by the elastic energy of the elastic element 5 of the target joint mechanism Ji is started. In other words, the first time is a time during which the elastic element 5 of the target joint mechanism Ji absorbs the collision kinetic energy.

  The second time refers to the joint displacement speed of the target joint mechanism Ji (hereinafter referred to as “joint” when the target joint mechanism Ji is displaced by the actuator 4 at the maximum acceleration Aj_max_i that the target joint mechanism Ji can achieve. This is the time required to change ωj_i (referred to as “displacement speed”) by a speed obtained by reversing the direction of the relative joint displacement speed Δωj_i of the target joint mechanism Ji caused by the collision link member Lcoll colliding with the object Obj.

  Here, the relative joint displacement speed Δωj_i represents the amount of change in the joint displacement speed ωj_i of each target joint mechanism Ji caused by the collision of the object Obj and the collision link member Lcoll with the object Obj. In the present embodiment, since the joint mechanism Ji is a rotary joint mechanism, the joint displacement speed ωj_i is an angular speed.

  In each target joint mechanism Ji, when the above two conditions are satisfied, the following two effects are obtained. Thereby, damage to each target joint mechanism Ji due to the collision link member Lcoll colliding with the object Obj can be prevented.

(Effect 1: Effect of Condition 1)
The effect that in each target joint mechanism Ji, the impact on the target joint mechanism Ji caused by the collision link member Lcoll colliding with the object Obj can be absorbed by the elastic element 5 of the target joint mechanism Ji.

(Effect 2: Effect of Condition 2)
In each target joint mechanism Ji, while the elastic element 5 of the target joint mechanism Ji absorbs the collision kinetic energy, the joint displacement speed ωj_i of the target joint mechanism Ji is set to be relative to the target joint mechanism Ji due to the collision. The target joint mechanism Ji can be prevented from being forcibly displaced by the elastic energy of the elastic element 5 of the target joint mechanism Ji. The effect of being able to do it.

  For example, when detecting that the collision link member Lcoll collides with the object Obj, a control device (not shown) including a CPU or the like is configured to prevent the target joint mechanism Ji from being damaged by the collision. The rigidity kj_i of the elastic element 5 of each target joint mechanism Ji is controlled so as to satisfy the conditions 1 and 2. Therefore, the change range (kj1_i or more and kj2_i) of the stiffness kj_i of the elastic element 5 of each target joint mechanism Ji is set so as to satisfy at least the above condition 1 and condition 2.

  Hereinafter, a method for setting the change range of the stiffness kj_i of the elastic element 5 will be described.

(2-1-1-1. First joint stiffness: minimum value)
In order to satisfy the condition 1, in each target joint mechanism Ji, “the first joint stiffness (that is, the minimum value of the variable range) kj1_i of the elastic element 5 of the i-th target joint mechanism Ji is the maximum joint elastic energy, The minimum stiffness among the stiffness kjc1_i ”which is equal to or higher than the inertial collision kinetic energy and the stiffness kj of the elastic element 5 of the joint mechanism Ji corresponding to each target stiffness ke_cmd. Is set to the smaller of the two values.

  Here, the joint maximum elastic energy is the i-th target joint when the elastic displacement Δθ_i of the elastic element 5 of the i-th target joint mechanism Ji is the maximum displacement Δθ_lim_i of the elastic element 5 of the i-th target joint mechanism Ji. This is the elastic energy of the elastic element 5 of the mechanism Ji.

  Further, the minimum joint inertial kinetic energy of the joint is the minimum value (hereinafter referred to as the following) that the inertia moment I_t_i of the i-th target joint mechanism Ji when the i-th target joint mechanism Ji is displaced can be taken. In the i-th target joint mechanism Ji, this minimum value is referred to as “i-th minimum moment of inertia” I_min_i, and the maximum displacement speed ωj_max_i that can be realized by the i-th target joint mechanism Ji when displaced by the actuator 4 Is the kinetic energy of the i-th target joint mechanism Ji when the i-th target joint mechanism Ji is assumed to be displaced.

  Here, in each target joint mechanism Ji, the inertia moment I_t_i when the target joint mechanism Ji is displaced is obtained according to the above-described equation (132-3), as in the first embodiment.

  The joint minimum inertial collision kinetic energy increases as the inertia moment I_t_i of the i-th target joint mechanism Ji when the i-th target joint mechanism Ji is displaced increases. The joint minimum inertial collision kinetic energy is the collision kinetic energy in a case where the inertia moment I_t_i of the i-th target joint mechanism Ji when the i-th target joint mechanism Ji is displaced is the i-th minimum inertia moment I_min_i. Therefore, under the condition that the rigidity kj_i of the elastic element 5 of the i-th target joint mechanism Ji and the joint displacement speed ωj_i of the i-th target joint mechanism Ji are constant, the joint minimum inertial collision kinetic energy is a value that the collision kinetic energy can take. It is the minimum value of.

  When the rigidity kj_i of the elastic element 5 of the i-th target joint mechanism Ji is set to be equal to or higher than the first joint rigidity kj1_i as described above, the i-th target joint mechanism Ji is displaced when the i-th target joint mechanism Ji is displaced. Is any value that the i-th target joint mechanism Ji can take, and the amount by which the joint displacement speed ωj_i of the i-th target joint mechanism Ji changes changes the i-th target joint mechanism Ji. Any value that can be taken from time to time, so that the condition 1 is satisfied (so that the collision kinetic energy can be completely converted into the elastic energy of the elastic element 5 of the i-th target joint mechanism Ji). The rigidity kj_i of the elastic element 5 of the joint mechanism Ji can be set, the rigidity kj_i of the elastic element 5 of the joint mechanism Ji, the rigidity ke with respect to the displacement of the distal end portion 3, and any of the plurality of target rigidity ke_cmd_k Even when selected, the rigidity kj_i necessary for realizing the selected target rigidity ke_cmd_k can be set.

  In each target joint mechanism Ji, more specifically, the first stiffness kjc1_i of the elastic element 5 of the i-th target joint mechanism Ji is set to be greater than or equal to the value obtained by the following equation (2111-1).

Here, the formula (2111-1) indicates the rigidity kj_i of the elastic element 5 of the i-th target joint mechanism Ji that satisfies the following formula (2111-2).

In the formula (2111-2), the left side indicates the joint minimum inertial collision kinetic energy described above. The right side indicates the joint maximum elastic energy described above.

  Further, the first joint stiffness kj1_i described above is determined in more detail according to the following equation (2111-3).

kj1_i = min (kjc1_i, kr_rmin),
kj_rmin = min (kj_rmin_1, kj_rmin_2, ..., kj_rmin_K)) (2111-3)
Here, kj_rmin_1, kj_rmin_2,..., Kj_rmin_K in Expression (2111-3) are the minimum values of the range kj_range_i of the stiffness kj_i of the elastic element 5 of each joint mechanism Ji for each work, as in the first embodiment. is there. That is, “min (kj_rmin_1, kj_rmin_2,..., Kj_rmin_K)” is “the minimum value of the stiffness of the elastic element of the joint mechanism required to realize all of the plurality of target stiffnesses” in the present invention. is there.

(2-1-1-2. Second joint stiffness: maximum value)
In order to satisfy the condition 2, in each target joint mechanism Ji, the second joint stiffness (that is, the maximum value of the variable range) kj2_i of the elastic element 5 of the i-th target joint mechanism Ji is equal to or longer than the fourth time. To a larger value of kjc2_i (hereinafter referred to as “second stiffness”) and the maximum stiffness of the stiffness kj of the elastic element 5 of the joint mechanism Ji corresponding to each target stiffness ke_cmd. Is set.

  Here, the third time is the maximum value (hereinafter referred to as the i th target joint mechanism Ji) that the moment of inertia I_t_i of the i th target joint mechanism Ji can take when the i th target joint mechanism Ji is displaced. In the target joint mechanism Ji, this maximum value becomes “I_max_i” (referred to as “i-th maximum moment of inertia”), and from the time when the displacement of the i-th target joint mechanism Ji is started, accumulation is performed by the displacement of the i-th target joint mechanism Ji. The time until the start of displacement of the i-th target joint mechanism Ji by the elastic energy of the elastic element 5 of the i-th target joint mechanism Ji.

  Note that the i-th maximum inertia moment I_max_i when the i-th target joint mechanism Ji is displaced is the i-th target joint mechanism in each of all possible combinations of “joint angles ψj_i that each joint mechanism Ji can take”. Of the moments of inertia I_t_i when Ji is displaced, the largest moment of inertia I_t_i is determined.

  The fourth time is the joint displacement of the i-th target joint mechanism Ji when it is assumed that the i-th target joint mechanism Ji is displaced at the maximum acceleration Aj_max_i that can be realized by the actuator 4 by the actuator 4. This is the time when the speed ωj_i is changed by the maximum joint displacement speed ωj_max_i of the i-th target joint mechanism Ji.

  Here, during the first time, the inertia of the i-th target joint mechanism Ji when the i-th target joint mechanism Ji is displaced under the condition that the stiffness kj_i of the elastic element 5 of the i-th target joint mechanism Ji is constant. The larger the moment I_t_i, the larger the moment. The third time is a first time when the inertia moment I_t_i of the i-th target joint mechanism Ji is the i-th maximum inertia moment I_max_i when the i-th target joint mechanism Ji is displaced. Therefore, under the condition that the rigidity kj_i of the elastic element 5 of the i-th target joint mechanism Ji is constant, the third time is the maximum length that can be taken by the first time.

  The second time increases as the amount of change in the joint displacement speed ωj_i of the i-th target joint mechanism Ji increases under the condition that the stiffness kj_i of the elastic element 5 of the i-th target joint mechanism Ji is constant. . The fourth time is a second time when the joint displacement speed ωj_i of the i-th target joint mechanism Ji is changed by the maximum joint displacement speed ωj_max_i of the i-th target joint mechanism Ji. Therefore, under the condition that the stiffness kj_i of the elastic element 5 of the i-th target joint mechanism Ji is constant, the fourth time is the maximum length that can be taken by the second time.

  Thus, the second stiffness kjc2_i is set so that the third time, which is the maximum length of the first time, is equal to or greater than the fourth time, which is the maximum length of the second time.

  By setting the rigidity kj_i of the elastic element 5 of the i-th target joint mechanism Ji to be equal to or less than the second joint rigidity kj2_i as described above, the inertia moment I_t_i of the i-th target joint mechanism Ji is changed to that of the i-th target joint mechanism Ji. Any value that can be taken, and any value that can be taken when changing the joint displacement speed ωj_i of the i-th target joint mechanism Ji when changing the i-th target joint mechanism Ji, The rigidity kj_i of the i-th target joint mechanism Ji can be set so that the condition 2 is satisfied (the first time is equal to or longer than the second time), and the rigidity kj_i of the elastic element 5 of the joint mechanism Ji is The stiffness ke for the displacement of 3 can be set to the stiffness kj_i necessary for realizing the selected target stiffness ke_cmd_k, regardless of which of the plurality of target stiffnesses ke_cmd_k is selected.

  In each target joint mechanism Ji, more specifically, the second stiffness kjc2_i of the elastic element 5 of the i-th target joint mechanism Ji is set to be equal to or greater than a value obtained by the following equation (21112-1).

However, (pi) shows a circumference ratio.

  Here, the equation (2112-1) is the rigidity kj_i of the elastic element 5 of the i-th target joint mechanism Ji that satisfies the following equation (2112-2).

In the formula (211-2), the left side is the third time described above. The right side is the fourth time described above.

  The second joint stiffness kj2_i described above is determined in more detail according to the following relational expression (2112-3).

kj2_i = max (kjc2_i, kj_rmax),
kj_rmax = max (kj_rmax_1, kj_rmax_2,..., kj_rmax_K)) (2112-3)
Here, kj_rmax_1, kj_rmax_2,..., Kj_rmax_K in Expression (2112-3) are the maximum values of the range kj_range_i of the stiffness kj_i of the elastic element 5 of each joint mechanism Ji for each work, as in the first embodiment. is there. That is, “min (kj_rmax_1, kj_rmax_2,..., Kj_rmax_K)” is “the maximum value of the stiffness of the elastic element of the joint mechanism necessary to realize all of the plurality of target stiffnesses” in the present invention. is there.

  As described above, the stiffness kj_i of the elastic element 5 of each joint mechanism Ji is set so that the stiffness ke with respect to the displacement of the distal end portion 3 is equal to any one of the plurality of target stiffnesses ke_cmd_k defined in advance. Can be set to an appropriate range, and each joint mechanism Ji can be prevented from being damaged by a part of the link mechanism 101 (that is, the collision link member Lcoll) colliding with the object Obj. The stiffness kj_i of the elastic element 5 can be set within an appropriate range.

(2-1-2. Viscosity coefficient variable range)
In each joint mechanism Ji, the viscosity coefficient Cj_i of the viscous element 6 is configured to be changeable within the range of the first joint viscosity coefficient Cj1_i and the second joint viscosity coefficient Cj2_i. Here, when it is assumed that the stiffness kj_i of the elastic element 5 of the i-th joint mechanism Ji is equal to the first joint stiffness kj1_i, the first joint viscosity coefficient Cj1_i is the critical damping or It is set to have overdamping characteristics. Further, when it is assumed that the stiffness kj_i of the elastic element 5 of the i-th joint mechanism Ji is equal to the second joint stiffness kj2_i, the second joint viscosity coefficient Cj2_i indicates that the behavior characteristic of the i-th joint mechanism Ji is critically damped or excessive. It is set to have attenuation characteristics.

  Thereby, even if the stiffness kj_i of the elastic element 5 of each joint mechanism Ji is changed to any stiffness kj_i within the range of the first joint stiffness kj1_i and the second joint stiffness kj2_i, the change is made. The viscosity coefficient Cj_i at which the behavior characteristic of the i-th joint mechanism Ji becomes a critical damping characteristic or an excessive damping characteristic with respect to the stiffness kj_i is included in the range of the first joint viscosity coefficient Cj1_i and the second joint viscosity coefficient Cj2_i. .

  Therefore, for example, it becomes possible to change the viscosity coefficient Cj_i of the viscous element 6 of each joint mechanism Ji so that the behavior characteristic of each joint mechanism Ji becomes a critical damping characteristic or an overdamping characteristic by the control of the control device. It is possible to prevent the behavioral characteristics of each joint mechanism Ji from becoming a damped vibration characteristic. Then, the occurrence of the vibration of the collision link member Lcoll corresponding to the damped vibration of each joint mechanism Ji is prevented, and the collision between the collision link member Lcoll and the object due to the elasticity of the elastic element 5 of each joint mechanism Ji is prevented. Is done. Thereby, damage to each joint mechanism Ji due to the collision link member Lcoll colliding with the object Obj can be prevented.

  As in the first embodiment, the first joint viscosity coefficient Cj1_i is determined according to the above-described equation (132-1), and the second joint viscosity coefficient Cj2_i is determined according to the above-described equation (132-2).

(2-2. Control)
Next, in the link mechanism 1 configured as described above, an example in which the control device (not shown) controls the rigidity kj_i of the elastic element 5 and the viscosity coefficient Cj_i of the viscous element 6 of each joint mechanism Ji will be described. To do.

  In the present embodiment, as described in the first embodiment, when the control device is set to the predetermined target rigidity ke_cmd_k, the rigidity ke of the distal end portion 3 becomes the predetermined target rigidity ke_cmd_k. The stiffness kj_i of the elastic element 5 and the viscosity coefficient Cj_i of the viscous element 6 of each joint mechanism Ji are controlled (normal control). In addition to this control, when the control device detects that any of the link members Li may collide with the object Obj, the joint mechanism Ji is not damaged by the collision. The stiffness kj_i of the elastic element 5 of each joint mechanism Ji and the viscosity coefficient Cj_i of the viscous element 6 are controlled (control during collision).

  Hereinafter, the description of the normal control similar to the first embodiment will be omitted, and the control at the time of collision will be described.

  Note that the control device detects that there is a possibility that any of the link members Li may collide with the object Obj based on information from an external sensor (such as a camera) (not shown). Then, when the possibility is higher than a certain value in any one of the link members Li, the control device uses the link member Li that may collide the link member Li with the possibility more than a certain value with the object Obj. It is estimated that there is.

(2-2-1. Rigidity)
In this case, the control device, in each target joint mechanism Ji, has a rigidity kj_i that satisfies the following expression (221-1) within a range in which the rigidity kj_i of the elastic element 5 of the target joint mechanism Ji can be changed (condition 1) Target stiffness that is a target value of the stiffness kj_i of the elastic element 5 of the target joint mechanism Ji so that the stiffness kj_i that satisfies the following equation (221-2) or less (so that the condition 2 is satisfied) is satisfied. kj_cmd_i is determined. In the equation (2211-1), the left side represents the above-described collision kinetic energy, and the right side represents the above-described joint maximum elastic energy. In the formula (221-2), the left side indicates the first time described above, and the right side indicates the second time described above.

Here, the relative joint displacement speed Δωj_i of the target joint mechanism Ji is the collision relative speed (relative between the collision link member Lcoll and the object Obj) output by the speed sensor (not shown) provided in the collision link member Lcoll. Is determined according to the following equation (221-3) according to ΔV (= (ΔV_x, ΔV_y, ΔV_z)).

Δω = Jcoll ⁺ · ΔV (221-3)
Here, Jcoll ⁺ is a pseudo inverse matrix of the Jacobian matrix Jcoll shown below. The Jacobian matrix Jcoll is a Jacobian matrix that defines the relationship between the displacement speed of the collision link member Lcoll and the joint displacement speed ωj_i of the target joint mechanism Ji in the base coordinate system. Further, when the relative joint displacement speed Δωj_i of the joint mechanism Jcoll closest to the collision link member Lcoll among the target joint mechanisms Ji is represented as Δωj_coll, Δω is represented by (Δωj_1, Δωj_2,..., Δωj_coll). . That is, Δω is a vector having all the relative joint displacement speeds Δωj_i of the target joint mechanisms Ji as elements.

  Then, the control device controls the rigidity kj_i of the elastic element 5 of each joint mechanism Ji so as to become the determined target rigidity kj_cmd_i. Thereby, even if it is a case where the collision link member Lcoll collides with the object Obj, damage to each joint mechanism Ji can be prevented.

(2-2-2. Viscosity coefficient)
In each joint mechanism Ji, the control device sets the target viscosity coefficient Cj_cmd_i, which is the target value of the viscosity coefficient of the viscous element 6 of the joint mechanism Ji, according to the target stiffness kj_cmd_i set as described above. In the same manner as above, it is determined according to the above-described equation (142).

  Then, the control device controls the viscosity coefficient Cj_i of the viscosity element 6 of each joint mechanism Ji to be the determined target viscosity coefficient Cj_cmd_i.

  Thereby, the behavior characteristic of each joint mechanism Ji becomes a critical damping characteristic or an over-damping characteristic, and is prevented from becoming a damping vibration characteristic. Therefore, the vibration of the collision link member Lcoll according to the damped vibration of the joint mechanism Ji is prevented, and the collision between the collision link member Lcoll and the object Obj due to the elasticity of the elastic element 5 of each joint mechanism Ji is prevented. Is done. Thereby, damage to each joint mechanism Ji due to the collision link member Lcoll colliding with the object Obj can be prevented.

(2-3. Modifications)
In the present embodiment, the first stiffness kjc1_i is determined to be a value equal to or greater than the stiffness kj_i obtained according to the equation (2111-1) described above, but the value may be determined by another arithmetic equation or the like.

  In the present embodiment, the second stiffness kjc2_i is determined to be a value equal to or less than the stiffness obtained according to the equation (21112-1) described above. However, the value may be determined by another arithmetic equation or the like. .

(3. Overall modification)
In the first and second embodiments, the target stiffness ke_cmd is not limited to the target value of the stiffness ke with respect to the displacement of the tip 3, but is the stiffness with respect to the displacement of a member between the base 2 and the tip 3. May be. Similarly, the movable part in the present invention is not limited to the tip part 3 but may be a member between the base 2 and the tip part 3.

  Further, the stiffness kj of the elastic element 5 of each joint mechanism Ji corresponding to the target stiffness ke_cmd of the distal end portion 3 follows the above-described formula (12-5) as shown in the first embodiment and the second embodiment. You may obtain by another aspect, not only what is obtained.

  Further, the joint mechanism Ji may be in an aspect not including the viscous element 6 as long as the joint mechanism Ji includes at least the elastic element 5.

  Further, when the joint mechanism Ji transmits power between the drive side link and the driven side link as long as the power is converted into an elastic force and a viscous force and transmitted, this embodiment It may be an aspect other than the aspect. For example, a conductive polymer actuator configured to be able to change the stiffness and viscosity coefficient may be used on a power transmission path between the drive side link and the driven side link.

  DESCRIPTION OF SYMBOLS 1 ... Link mechanism (1st Embodiment), 101 ... Link mechanism (2nd Embodiment), Obj ... Object, 2 ... Base | substrate, 3 ... Tip part, Ji ... Joint mechanism, 4 ... Actuator, 5 ... Elastic element, 6 ... viscous element, 11 ... wire (elastic element), 14 ... rotating bar (elastic element), 16 ... gear (elastic element), 17 ... spring worm (elastic element), 18 ... electric motor (elastic element), 19 ... cylinder (Viscous element), 24 ... orifice part (viscous element), Δθ ... elastic displacement amount (elastic deformation amount), Δθ_i ... elastic displacement amount (elastic deformation amount), Δθ_lim_i ... maximum displacement amount (maximum deformation amount), kj1_i ... 1 joint stiffness, kj2_i ... second joint stiffness, Cj1_i ... first joint viscosity coefficient, Cj2_i ... second joint viscosity coefficient, ωj_i ... joint displacement speed (displacement speed), Δωj_i ... relative joint displacement speed (relative displacement speed) ), I_t_i ... moment of inertia, Lcoll ... Collision link member (movable part), ke_cmd: target rigidity, kjc1_i: first rigidity, kjc2_i: second rigidity.

Claims (5)

And an actuator that outputs driving force for displacing the multiple joint mechanism provided between the joint mechanism before Kifuku number of movable movable portion relative to the base body and the base body, before Kifuku number of Each of the joint mechanisms is a link mechanism configured to transmit power via an elastically deformable elastic element configured to be capable of variably controlling rigidity,
When the stiffness target value of the displacement of the movable portion defined by the stiffness of the elastic elements of each of the front Kifuku number of joint mechanism defines the target stiffness,
A plurality of target stiffness values corresponding to each of a plurality of types of work to be executed by the link mechanism are prepared in advance ,
Rigidity of the elastic elements of each of the front Kifuku number of joint mechanisms, necessary to realize all of the plurality of target rigidity, between the minimum and maximum values of the stiffness of the elastic element of the joint mechanism It is configured to be changed in the range of,
When the minimum and maximum stiffness values of the elastic elements of each of the plurality of joint mechanisms are expressed as kj1_i and kj2_i (i: the identification number of each of the plurality of joint mechanisms), the elastic elements of the joint mechanisms The link mechanism is characterized in that the minimum value kj1_i and the maximum value Kj2_i of the rigidity are values determined by the following expressions (a1) and (a2) .

kj1_i = min (kj_rmin_1, kj_rmin_2, ..., kj_rmin_K)
...... (a1)
kj1_i = max (kj_rmax_1, kj_rmax_2, ..., kj_rmax_K)
...... (a2)

However,
Kj_rmin_k (k = 1, 2,..., K. k: identification number of each of the plurality of types of work) in the equation (a1) is the value of the target stiffness ke_cmd_k corresponding to the kth work, Of the component corresponding to the i-th joint mechanism among the diagonal components of the matrix kten calculated by the following equation (a3) from the Jacobian matrix J defined according to the attitude of the link mechanism and its transposed matrix J ^T The minimum value of the component value range corresponding to the variable range of the posture of the link mechanism in the k-th work, kj_rmax_k (k = 1, 2,..., K in the equation (a2)) K: identification number of each of the plurality of types of work) from the target stiffness value ke_cmd_k corresponding to the k-th work, the Jacobian matrix J and its transpose matrix J ^{T according} to the following equation (a3) The i-th joint of the calculated diagonal components of the matrix kten A maximum value of the component corresponding to the structure, the maximum value of the range of values of the component corresponding to the variable range of the posture of the link mechanism in the working of the k-th.

kten = J ^T · ke_cmd_k · J (a3) One or a plurality of joint mechanisms provided between a base and a movable part movable with respect to the base; and an actuator for outputting a driving force for displacing the one or the plurality of joint mechanisms, Each of the one or more joint mechanisms is a link mechanism configured to transmit power via an elastically deformable elastic element configured to be variably controllable in rigidity.
In each of the one or more joint mechanisms, when the elastic deformation amount of the elastic element of the joint mechanism is the maximum deformation amount of the elastic element of the joint mechanism, the elastic energy of the elastic element of the joint mechanism is When the joint mechanism is displaced, the moment of inertia or the inertial mass of the joint mechanism is a minimum value that can be taken by the joint mechanism, and the maximum displacement speed at which the joint mechanism can be realized when the joint mechanism is displaced by the actuator. The rigidity set to be equal to or higher than the kinetic energy of the joint mechanism when the joint mechanism is assumed to be displaced is defined as the first rigidity of the elastic element of the joint mechanism,
In each of the one or more joint mechanisms, when the joint mechanism is displaced, the inertia moment or the inertial mass of the joint mechanism becomes the maximum value that the joint mechanism can take, and the displacement of the joint mechanism is started. The maximum acceleration that the joint mechanism can realize by the actuator is the time from the point in time until the start of the displacement of the joint mechanism by the elastic energy of the elastic element of the joint mechanism accumulated by the displacement of the joint mechanism. When the joint mechanism is assumed to be displaced in the above, the rigidity set to be equal to or longer than the time required for changing the displacement speed of the joint mechanism by the maximum displacement speed of the joint mechanism is Defined as the second stiffness of the elastic element,
When a target stiffness value for the displacement of the movable part defined by the stiffness of the elastic element of each of the one or more joint mechanisms is defined as a target stiffness,
A plurality of the target stiffnesses are prepared in advance,
The rigidity of the elastic element of each of the one or more joint mechanisms is the minimum value of the rigidity of the elastic element of the joint mechanism and the first rigidity necessary to realize all of the plurality of target rigidity. Between the smaller one of the values and the maximum value of the stiffness of the elastic element of the joint mechanism and the larger value of the second stiffness required to realize all of the plurality of target stiffnesses A link mechanism configured to be changeable within a range. 3. The link mechanism according to claim 2, wherein when any one of the one or more joint mechanisms is defined as an i-th joint mechanism, the first of the elastic elements of the i-th joint mechanism. The link mechanism characterized in that the rigidity is set to be higher than the rigidity obtained according to the following equation (1).
However,
I_min_i: Minimum value of the moment of inertia or inertial mass of the i-th joint mechanism that can be taken by the i-th joint mechanism when the i-th joint mechanism is displaced ωj_max_i: Realized when the i-th joint mechanism is displaced by the actuator Maximum possible displacement speed
xj_lim_i: maximum deformation amount of the elastic element of the i-th joint mechanism The link mechanism according to claim 2 or 3, wherein when any one of the one or more joint mechanisms is defined as an i-th joint mechanism, the elastic element of the i-th joint mechanism is the link mechanism. The link mechanism characterized in that the second rigidity is set to be equal to or less than the rigidity obtained according to the following equation (2).
However,
I_max_i: Maximum moment of inertia or mass of the i-th joint mechanism that can be taken by the i-th joint mechanism when the i-th joint mechanism is displaced π: Circumferential ratio
Aj_max_i: Maximum acceleration realizable when the i-th joint mechanism is displaced by the actuator ωj_max_i: Maximum displacement speed achievable when the i-th joint mechanism is displaced by the actuator In the link mechanism according to any one of claims 1 to 4,
Each of the one or more joint mechanisms is configured to transmit power via the elastic element and a viscous element configured to variably control the viscosity coefficient,
The viscosity coefficient of the viscous element of each of the one or more joint mechanisms is greater than or equal to a first joint viscosity coefficient and less than or equal to a second joint viscosity coefficient set for each of the one or more joint mechanisms. It is configured to be changeable at
When the first joint viscosity coefficient of the viscous element of each of the one or more joint mechanisms is assumed that the rigidity of the elastic element of the joint mechanism is equal to a minimum value within a changeable range, The behavioral characteristics of the joint mechanism are set to be critical damping or overdamping characteristics,
When the second joint viscosity coefficient of the viscous element of each of the one or more joint mechanisms is assumed that the rigidity of the elastic element of the joint mechanism is equal to the maximum value in a changeable range, A link mechanism characterized in that the behavior characteristic of the joint mechanism is set to be a critical damping characteristic or an excessive damping characteristic.