JP5616290B2 - Control device for moving body - Google Patents

Description

  The present invention relates to a control device for a moving body including a plurality of movable links such as leg links.

  In motion control of a moving body such as a legged mobile robot that moves on the floor surface by movement of multiple leg links connected to the base (movement that repeats landing and leaving of each leg link), basically the movement The joints of the leg links are driven so that the actual movement follows the target movement of the body.

  The above target motion is generally generated so as to satisfy the dynamic requirements (requirements such as the presence of the ZMP in the support polygon) on the assumed floor surface set to represent the actual floor surface. However, the actual motion of the moving body may deviate from the target motion due to a shape error of the assumed floor surface used to generate the target motion with respect to the actual floor surface. In such a case, the posture of the moving body tends to collapse.

  As a technique for preventing this, conventionally, for example, a compliance control technique proposed by the present applicant in Patent Document 1 is known. In this compliance control technology, the actual floor reaction force moment generated around the center point of the total floor reaction force is observed while observing the floor reaction force that actually acts on the biped walking robot. The target position and posture of the tip portion (foot portion) of each leg link are corrected so as to follow the target moment for restoring the posture to the target posture, and depending on the corrected target position and posture, The displacement amount of the joint of the leg link is controlled.

Patent No. 3629133

  In order to cause a moving body such as a legged mobile robot to perform a variety of operations, not only the moving body is moved on the floor but also a plurality of contact target surfaces (for example, the external environment) of the moving body (for example, In some cases, it is required to perform the required movement of the moving body while bringing one or more movable links such as leg links or arm links of the moving body into contact with each of the floor surface and the wall surface.

  In such a case, unlike the case where the moving body is simply moved on the floor, an external force acts on the moving body from each contact target surface.

  For stable operation of the moving body, contact force (reaction force) that actually acts on the moving body from each contact target surface even in a situation where the moving body is in contact with a plurality of contact target surfaces as described above. It is considered desirable to appropriately adjust the displacement amount of the joint of the moving body so as to follow the required target contact force.

  In this case, in order to control the contact force that actually acts on the moving body from each contact target surface, it is conceivable to use a compliance control technique as disclosed in Patent Document 1.

  However, in the compliance control technique found in Patent Document 1, each leg link for causing the actual floor reaction force moment acting on the moving body (legged mobile robot) from the floor surface as the contact target surface to follow the target moment. The correction of the target position and posture of the distal end portion (foot portion) of each leg is made by changing the vertical positions of the distal end portions of both leg links in opposite directions (hereinafter referred to as the first correction in this section), It is assumed that the correction (hereinafter referred to as the second correction in this column) that changes the posture of the distal end portion of the leg link is combined.

  In this case, the correction amount of the position of the tip portion of each leg link in the first correction and the correction amount of the posture of the tip portion of each leg link in the second correction are determined separately.

  For this reason, in the conventional compliance control technique, the change in the floor reaction force caused by the first correction and the change in the floor reaction force caused by the second correction tend to interfere with each other. For this reason, the change in the floor reaction force moment actually caused by the correction of the position and posture of the tip portion of each leg link formed by combining the first correction and the second correction is excessive or insufficient with respect to the target moment. There is an inconvenience that it is easy to produce.

  Further, in order to suppress the occurrence of the excess or deficiency as much as possible, a gain for determining the correction amount of the position of the tip portion of each leg link by the first correction (the floor reaction to be generated by the first correction). A gain that defines the degree of the force moment with respect to the target moment) and a gain for determining the correction amount of the posture of each leg link by the second correction (the floor reaction force to be generated by the second correction) It is necessary to appropriately adjust the gain that defines the degree of the moment with respect to the target moment. The adjustment work generally has the disadvantage of requiring a large number of man-hours.

  The present invention has been made in view of the background of the present invention, and moves the moving body that performs an operation to contact a plurality of contact target surfaces so that the total contact force that actually acts from each contact target surface follows the target value. A moving body capable of appropriately controlling the body movement without requiring a process for determining a correction amount from the target movement of the position and posture of each contact portion with respect to each contact target surface of the moving body. An object of the present invention is to provide a control device.

  First, technical matters that form the basis of the present invention will be generalized and described below.

  As shown in FIG. 1, a moving body 101 in which a plurality of movable links 103 are connected to a base body 102 is assumed. Each movable link 103 of the moving body 101 includes a plurality of joints, and spatial movement is possible by displacement of these joints. Each joint is a rotary or direct acting joint.

  Then, in a certain motion state of the moving body 101, m (i) (m (i) ≧ 1) movable links 103 (103_1 to 103_m (i)) of the movable links 103 of the moving body 101 have their distal ends. Comes into contact with the i-th contact target surface, which is one contact target surface existing in the operating environment of the mobile object 101, and the mobile object 101 is in contact with the i-th contact target surface via these movable links 103_1-103_m (i). It is assumed that a contact force as a reaction force is received from the. In the following description, each of the movable links 103_1 to 103_m (i) is collectively referred to as a jth movable link 103_j (j = 1, 2,..., M (i)) or a movable link 103_j.

  At this time, the total contact force (vector) that is the total contact force acting on the moving body 101 from the i-th contact target surface is ↑ FMt (i), and the total translation force that is the translational force vector of the ↑ FMt (i). The contact force is expressed as ↑ Mt (i), and the total contact force moment which is a moment vector among ↑ Ft (i) and ↑ FMt (i).

These total translational contact forces ↑ Ft (i) and total contact force moments ↑ Mt (i) are respectively seen in an inertial coordinate system (a coordinate system fixed to the floor or wall of the operating environment of the moving body 101). It is assumed that it is expressed as a three-component vertical vector. The i-th total contact force ↑ FMt (i) is a six-component vertical vector (= [↑ Ft (i), ↑ Mt (i)) in which the components ↑ Ft (i) and ↑ Mt (i) are arranged. ] ^T ). Further, the point of action Pt (i) of the total contact force ↑ FMt (i) is a point on the i-th contact surface.

  In this specification, “↑” is used as a symbol representing a vector (vertical vector). The superscript “T” means transposition. Further, in this specification, for convenience of understanding, as an inertial coordinate system that expresses translational force, moment, position, posture, etc., for example, the horizontal axis in the front-rear direction of the moving body 101 is the X axis, and the vertical direction is the Z axis. It is assumed that a three-axis orthogonal coordinate system in which the direction perpendicular to the X axis and the Z axis (the horizontal direction of the moving body 101) is the Y axis is used.

  Further, a contact force (hereinafter referred to as a movable link contact force) acting on the j-th movable link 103_j (j = 1, 2,..., N) from the i-th contact target surface is represented by ↑ FM (i) _j, ↑ FM ( A translational contact force that is a translational force vector in i) _j is denoted as ↑ F (i) _j, and a contact force moment that is a moment vector in ↑ FM (i) _j is denoted as ↑ M (i) _j.

These translational contact forces ↑ F (i) _j and contact force moments ↑ M (i) _j are the same as the i-th total contact force ↑ FMt (i). It shall be expressed as a vector. The j-th movable link contact force ↑ FM (i) _j is a six-component vertical vector (= [↑ F (i) _j, ↑ F (i) _j, ↑ F (i) _j, ↑ M (i) _j] ^T ). Further, the action point P (i) _j of the j-th movable link contact force ↑ FM (i) _j is a point on the i-th contact target surface within the contact surface between the j-th movable link 103_j and the i-th contact target surface. To do.

  At this time, the total contact force ↑ FMt (i) (hereinafter, sometimes referred to as i-th total contact force ↑ FMt (i)) and the movable link contact force ↑ FM (i) _j (j = 1, 2,..., M The relationship between (i)) is generally expressed by the following equation (1).

なお、式（１）のただし書きにおいて、行列AA(i)_jの成分“Ｉ”、“０”はそれぞれ、単位行列、零行列である。また、“×”は外積（ベクトル積）を表す算術符号である。

In the proviso of equation (1), the components “I” and “0” of the matrix AA (i) _j are a unit matrix and a zero matrix, respectively. “×” is an arithmetic code representing an outer product (vector product).

Here, perturbation contact force ↑ ΔFM (i) _j (= [↑ ΔF (i) _j, ↑ ΔM (i)) at the action point P (i) _j of the j-th movable link contact force ↑ FM (i) _j. _j] ^T ), so that the perturbation contact force ↑ ΔFMt (i) (= [↑ ΔFt (i), ↑ ΔMt) is applied to the point of action Pt (i) of the i-th total contact force ↑ FMt (i). (i)] Assume that ^T ) is added. Note that adding the contact force ↑ ΔFM (i) _j for the perturbation to the action point P (i) _j means that the contact force acting on the action point P (i) _j is increased from ↑ FM (i) _j to ↑ FM. (i) _j + ↑ means change to ΔFM (i) _j. Similarly, adding the contact force ↑ ΔFMt (i) for the perturbation to the action point Pt (i) means that the total contact force acting on the action point Pt (i) is increased from ↑ FMt (i) to ↑ FMt (i). ) + ↑ means change to ΔFMt (i).

  Thereafter, ↑ ΔFM (i) _j is the j-th movable link perturbation contact force, and the translational force vector ↑ ΔF (i) _j and the moment vector ↑ ΔM (i) _j of the ↑ ΔFM (i) _j are respectively the j-th movable It is called a link perturbation translational contact force and a j-th movable link perturbation contact force moment. Also, ↑ ΔFMt (i) is the i-th perturbation total contact force (or perturbation total contact force), and the translation force vector ↑ ΔFt and moment vector ↑ ΔMt of ↑ ΔFMt (i) are respectively the i-th perturbation total translation contact force. (Or perturbation total translational contact force), i-th perturbation total contact force moment (or perturbation total contact force moment).

  The relationship between the perturbation total contact force ↑ ΔFMt (i) and the j-th movable link perturbation contact force ↑ ΔFM (i) _j (j = 1, 2,..., M (i)) ) Is given by the following equation (2).

この式（２）におけるAA(i)_j（ｊ＝１，２，…，ｍ(i)）は、式（１）のただし書きで定義したものと同じである。

AA (i) _j (j = 1, 2,..., M (i)) in this equation (2) is the same as that defined in the proviso of equation (1).

  Further, the j-th movable link perturbation contact force ↑ ΔFM (i) _j is the position of the contact surface between the j-th movable link 103_j and the i-th contact target surface (hereinafter referred to as the j-th movable link contact surface or the movable link contact surface). And assumed to be caused by a springy displacement of the posture. More specifically, the spring-like displacement (translational displacement) of the position of the jth movable link contact surface generates a jth movable link perturbation translational contact force ↑ ΔF (i) _j, and the spring of the posture of the jth movable link contact surface It is assumed that the j-th movable link perturbation contact force moment ↑ M (i) _j is generated by the directional displacement (rotational displacement). Note that the spring-like displacement of the position and posture of the j-th movable link contact surface is caused by the elastic deformation of the forming member of the i-th contact target surface at the j-th movable link contact surface or the elastic deformation of the contact portion of the j-th movable link 103_j. It corresponds to translational displacement and rotational displacement, respectively.

  The displacement amount of the position of the j-th movable link contact surface due to this spring displacement (vector of the translational displacement amount in the triaxial direction of the j-th movable link contact surface, hereinafter referred to as the spring-like translational displacement amount) and the displacement amount of the posture (the first i is a vector of rotational displacement amounts around the three axes of the movable link contact surface (hereinafter referred to as spring-like rotational displacement amount), and are denoted as ↑ Xorg (i) _j and ↑ Xrot (i) _j, respectively. At this time, the relationship between ↑ Xorg (i) _j and ↑ ΔF (i) _j and the relationship between ↑ Xrot (i) _j and ↑ ΔM (i) _j are respectively expressed by the following equations (3). , (4).

↑ ΔF (i) _j = Korg (i) _j ・ ↑ Xorg (i) _j (3)
↑ ΔM (i) _j = Krot (i) _j ・ ↑ Xrot (i) _j (4)

Korg (i) _j in equation (3) is a third-order diagonal composed of the spring constant of each component of the springy translational displacement amount ↑ Xorg (i) _j of the jth movable link contact surface as a diagonal component. Matrix (hereinafter referred to as translational spring constant matrix Korg (i) _j), Krot (i) _j in equation (4) is the amount of springy rotational displacement ↑ Xrot (i) _j of the jth movable link contact surface Is a cubic diagonal matrix (hereinafter referred to as a rotational spring constant matrix Krot (i) _j).

  Further, all contact surfaces between the moving body 101 and the i-th contact target surface (contact surfaces of all the movable links 103 (i) _1 to 103 (i) _m (i) in contact with the i-th contact target surface). Assuming an i-th representative contact surface as a single virtual contact surface that represents i, the i-th total contact force ↑ FMt (i) acts on the moving body 101 at this i-th representative contact surface. . The i-th representative contact surface corresponds to a so-called support polygon on the i-th contact target surface.

  Then, it is assumed that the i-th perturbation total contact force ↑ ΔFMt (i) is generated by the spring displacement of the position and posture of the i-th representative contact surface in the same way as the case of the j-th movable link contact surface. More specifically, the perturbation total translational contact force ↑ ΔFt (i) is generated by the spring-like displacement (translational displacement) of the position of the i-th representative contact surface, and the spring-like displacement (rotational displacement) of the posture of the i-th representative contact surface. Suppose that a perturbed total contact force moment ↑ ΔMt (i) is generated by

  Accordingly, the amount of spring translational displacement at the position of the i-th representative contact surface (vector of translational displacement in three axes due to the spring displacement) and the amount of posture rotational displacement (rotational displacement about the three axes due to the spring displacement). ↑ Xc_org (i) and ↑ Xc_rot (i) respectively, and the relationship between ↑ Xc_org (i) and ↑ ΔFt (i), and ↑ Xc_rot (i) and ↑ ΔMt (i) The relationship between is expressed by the following equations (5) and (6), respectively.

↑ ΔFt (i) = Kc_org (i) ・ ↑ Xc_org (i) (5)
↑ ΔMt (i) = Kc_rot (i) ・ ↑ Xc_rot (i) (6)

In equation (5), Kc_org (i) is a cubic diagonal matrix (hereinafter referred to as the spring constant for each component of the springy translational displacement amount ↑ Xc_org (i) of the i-th representative contact surface). , Translational spring constant matrix Kc_org (i)), and Kc_rot (i) in equation (6) is the diagonal component of the spring constant for each component of the i-th representative contact surface spring displacement ↑ Xc_rot (i) Is a cubic diagonal matrix (hereinafter referred to as a rotational spring constant matrix Kc_rot (i)).

  Supplementally, the i-th representative contact surface is the single movable link 103_j when only the single movable link 103_j of the moving body 101 is in contact with the i-th contact target surface. The contact surface with the i th contact target surface (jth movable link contact surface). In this state, the spring displacement of the position and posture of the i-th representative contact surface is the same as the spring displacement of the position and posture of the j-th movable link contact surface, and the i-th contact at the j-th movable link contact surface. This corresponds to a translational displacement and a rotational displacement due to elastic deformation of the member forming the contact target surface or elastic deformation of the contact portion of the j-th movable link 103_j, respectively.

  Further, in a state where two or more movable links 103 of the moving body 101 are in contact with the i-th contact target surface, the i-th representative contact surface is all the movable links 103 in contact with the i-th contact target surface. It has the meaning as a contact surface with the i-th contact object surface of the single virtual contact part which put together the contact part. The spring displacement of the position and orientation of the i-th representative contact surface in this state is caused by the elastic deformation of the forming member of the i-th contact target surface at the contact surface of the virtual contact portion or the virtual contact portion. This corresponds to the translational displacement and rotational displacement caused by the elastic deformation.

  In the above equation (2), the j-th movable link perturbation added to the action point P (i) _j of the j-th movable link contact force ↑ FM (i) _j (j = 1, 2,..., M (i)). Focusing on the perturbation total translational contact force ↑ ΔFt (i) added to the point of action Pt (i) of the total contact force ↑ FMt (i) by the translational contact force ↑ ΔF (i) _j, the following equation (7) is obtained. It is done.

↑ ΔFt (i) = ↑ ΔF (i) _1 + ↑ ΔF (i) _2 + …… + ↑ ΔF (i) _m (i) (7)

From this equation (7) and the above equations (3) and (5), the following equation (8) is obtained.

Kc_org (i) ・ ↑ Xc_org (i) = Korg (i) _1 ・ ↑ Xorg (i) _1 + Korg (i) _2 ・ ↑ Xorg (i) _2
＋ …… ＋ Korg (i) _m (i) ・ ↑ Xorg (i) _m (i) …… （8）

On the other hand, the point of action Pt (i) of the total contact force ↑ FMt (i) before the addition of the perturbation total contact force ↑ ΔFMt (i) is the total contact force center point, and the jth movable link perturbation contact force ↑ ΔFM (i ) _j before application of the jth movable link contact force ↑ FM (i) _j, where the point of action P (i) _j is the contact force center point of the jth movable link contact surface, the point of action Pt with respect to an arbitrary reference point The position vector of (i) (this is expressed as ↑ Pt (i)) and each position vector (this is ↑) of the action point P (i) _j (j = 1, 2,..., m (i)). P (i) _j) is given by the following equation (9).

  The center point of the total contact force is the point of action of the total contact force ↑ FMt (i), and the component parallel to the i-th contact target surface of the total contact force moment ↑ Mt (i) around that point is zero. It is such a point. Similarly, the contact force center point of the j-th movable link contact surface is the point of action of the j-th movable link contact force ↑ FM (i) _j, and the contact force moment ↑ M (i) _j around that point This is a point where the component parallel to the i-contact target surface is zero.

↑ Pt (i) = r (i) _1 ・ ↑ P (i) _1 + r (i) _2 ・ ↑ P (i) _2
＋ …… ＋ r (i) _m (i) ・ ↑ P (i) _m (i) …… （9）

In this equation (9), r (i) _j (j = 1, 2,..., M (i)) is a weighting coefficient defined by r (i) _j≡Fn (i) _j / Fnt (i). is there. Here, Fn (i) _j (j = 1, 2,..., M (i)) is a vertical drag perpendicular to the i-th contact target surface of the j-th movable link translational contact force ↑ F (i) _j. The absolute value of the component (hereinafter referred to as contact surface normal force component), Fnt (i) is the absolute value of the normal force component perpendicular to the i-th contact surface of the total translational contact force ↑ Ft (i) (contact surface vertical) Drag component), and Fnt (i) = Fn (i) _1 + Fn (i) _2 +... + Fn (i) _m (i). Therefore, the weight coefficient r (i) _j is the contact surface normal force component of the j-th movable link contact force ↑ F (i) _j with respect to the contact surface normal force component Fnt (i) of the total translational contact force ↑ Ft (i). This is the ratio of Fn (i) _j, and its value is a value within the range of 0 ≦ r (i) _j ≦ 1.

  The relationship similar to this equation (9) is that the springy translational displacement amount ↑ Xc_org (i) at the position of the i-th representative contact surface and the springy translational displacement amount ↑ Xorg (at the position of the j-th movable link contact surface. i) Assume that it holds also between _j. That is, it is assumed that the following formula (10) holds.

↑ Xc_org (i) = r (i) _1 ・ ↑ Xorg (i) _1 + r (i) _2 ・ ↑ Xorg (i) _2
＋ …… ＋ r_m (i) ・ ↑ Xorg (i) _m (i) …… （10）

At this time, the following expression (11) is obtained from the expression (10) and the expression (8).

Korg (i) _j = r (i) _j · Kc_org (i) (11)

In the present invention, the above equation (10) is obtained by converting the spring-like translational displacement amount ↑ Xc_org (i) of the i-th representative contact surface and the spring of the j-th movable link contact surface (j = 1, 2,..., M (i)). The basic equation representing the relationship between the directional translational displacement amount ↑ Xorg (i) _j and the equation (11) is obtained by converting the translation spring constant matrix Kc_org (i) of the i-th representative contact surface and the j-th movable link contact surface (j = 1, 2,..., M (i)) is a basic expression representing the relationship with the translation spring constant matrix Korg (i) _j.

  Accordingly, each diagonal component of the translation spring constant matrix Korg (i) _j of the j-th movable link contact surface (spring constant relating to each translational displacement in the three axial directions) is proportional to the weight coefficient r (i) _j, and the weight The larger the value of the coefficient r (i) _j (closer to “1”), that is, the jth movable link translational contact force ↑ with respect to the contact surface normal force component Fnt (i) of the total translational contact force ↑ Ft (i) ↑ The larger the ratio of the contact surface normal force component Fn (i) _j of F (i) _j is, the larger it is. In other words, the greater the ratio of Fn (i) _j to Fnt (i), the greater the required spring translational displacement amount ↑ Xorg (i) _j of the jth movable link contact surface with respect to the required jth movable link perturbation translational contact force. It is assumed that the sensitivity of the change of is increased.

  In addition, when only the single j-th movable leg link 103_j is in contact with the i-th contact target surface (when r (i) _j = 1), the j-th movable link contact surface and the i-th representative contact. Corresponding to the coincidence with the surface, the translation spring constant matrix Korg (i) _j of the j-th movable link contact surface and the translation spring constant matrix Kc_org (i) of the i-th representative contact surface coincide with each other. Further, in this case, since ↑ ΔF (i) _j = ↑ ΔFt (i), the spring-like translational displacement amount ↑ Xorg (i) _j of the j-th movable link contact surface and the spring-like translational displacement of the i-th representative contact surface The quantity ↑ Xc_org (i) matches each other.

  When two or more movable links 103 are in contact with the i th contact target surface, the springy translational displacement amount ↑ Xc_org (i) of the i th representative contact surface is in contact with the i th contact target surface. The weighted average value of the spring-like translational displacement amount ↑ Xorg (i) _j of the jth movable link contact surface corresponding to each movable link 103_j (weighted average value using r (i) _j as a weighting factor). .

  Next, in the above formula (2), the translational contact forces ↑ F (i) _1 of the first to m (i) movable link contact forces ↑ FM (i) _1 to ↑ FM (i) _m (i) ~ ↑ F (i) _m (i) is held constant, and the point P (i) of the j-th movable link contact force ↑ FM (i) _j (j = 1, 2,..., M (i)). ) _j is added to the operating point Pt (i) of the total contact force ↑ FMt (i) by adding the j-th movable link perturbation contact force moment ↑ ΔM (i) _j to ↑ _j. ), The following equation (12) is obtained.

↑ ΔMt (i) = ↑ ΔM (i) _1 + ↑ ΔM (i) _2 + …… + ↑ ΔM (i) _m (i) (12)

Then, the following equation (13) is obtained from the equation (12) and the equations (4) and (6).

Kc_rot (i) ・ ↑ Xc_rot (i) = Krot (i) _1 ・ ↑ Xrot (i) _1 + Krot (i) _2 ・ ↑ Xrot (i) _2
＋ …… ＋ Krot (i) _m (i) ・ ↑ Xrot (i) _m (i)
(13)

On the other hand, when the action point P (i) _j of the j-th movable link contact force ↑ FM (i) _j is the contact force center point of the j-th movable link contact surface, the first to m-th (i) movable link contact forces. Action point of jth movable link contact force ↑ FM (i) _j with the translational contact forces ↑ F_1 to ↑ F_N of ↑ FM (i) _1 to ↑ FM (i) _m (i) kept constant Adding the j-th perturbation contact force moment ↑ ΔM (i) _j to P (i) _j (contact force center point) means that the position of the contact force center point on the j-th movable link contact surface is i This is equivalent to shifting from the point P (i) _j on the contact target surface. Similarly, when the point of action Pt (i) of the total contact force ↑ FMt (i) is the center point of the total contact force, the perturbation total contact force moment ↑ is applied to the point of action Pt (i) of the total contact force ↑ FMt (i). The addition of ΔMt (i) is equivalent to the shift of the position of the center point of all contact forces from the point Pt (i) on the i-th contact target surface.

  In this case, the displacement amount (two-component displacement amount vector) in the direction parallel to the i-th contact target surface at the position of the contact force center point on the j-th movable link contact surface is ↑ ΔRpt (i) _j, the total contact force Assuming that the displacement in the direction parallel to the i-th contact target surface at the center point (two-component displacement vector) is ↑ ΔRptt (i), ↑ ΔRptt (i) · Fnt (i) = ↑ ΔMt (i ) Is a component around the axis parallel to the i-th contact target surface, and ↑ ΔRpt (i) _j · Fn (i) _j = ↑ ΔM (i) _j is a component around the axis parallel to the i-th contact target surface. Therefore, the following equation (14) is obtained from the above equation (12).

↑ ΔRptt (i) = r (i) _1 ・ ↑ ΔRpt (i) _1 + r (i) _2 ・ ↑ ΔRpt (i) _2
+ …… + r (i) _m (i) ・ ↑ ΔRpt (i) _m (i) (14)

Note that r (i) _j (j = 1, 2,..., M (i)) in the equation (14) is the above-described weight coefficient r (i) _j (≡Fn (i) _j / Fnt (i)). It is.

  In addition, since the first to m (i) movable link contact surfaces are portions on a common contact target surface (i-th contact target surface), each of the first to m (i) movable link contact surfaces. When the acting vertical drags Fn (i) _j are the same, the displacement amount ↑ ΔRpt (i) _j in the direction parallel to the j-th contact target surface at the position of the contact force center point of the j-th movable link contact surface The corresponding perturbation contact force moment (= ↑ ΔRpt (i) _j · Fn (i) _j) and the i th contact surface of the rotational displacement amount ↑ Xrot (i) _j due to the spring displacement of the j th movable link contact surface It can be considered that the relationship between the components around the parallel axis is the same for any j-th movable link contact surface.

  Therefore, when the contact surface normal force components Fn (i) _j acting on each of the first to m (i) movable link contact surfaces have the same value (this is referred to as Fna (i)), 15) is considered to hold.

↑ ΔRpt (i) _j ・ Fna (i) = Krot (i) ・ ↑ Xrot (i) _j_ab (15)

↑ Xrot (i) _j_ab is a biaxial component of the spring-like rotational displacement amount ↑ Xrot (i) _j of the jth movable link contact surface parallel to the i-th contact target surface, and Krot (i) is It is a quadratic diagonal matrix (a spring constant matrix related to rotation about two axes parallel to the i-th contact target surface).

  Assuming that the above equation (15) holds true for the i-th representative contact surface, the following equation (16) is obtained.

↑ ΔRptt (i) ・ Fna (i) ＝ Krot (i) ・ ↑ Xc_rot (i) _ab (16)

↑ Xc_rot (i) _ab is a biaxial component around the i-th contact target surface of the spring-like rotational displacement amount ↑ Xc_rot (i) of the i-th representative contact surface.

  The following formula (17) is obtained from the above formulas (14) to (16).

Krot (i) ・ ↑ Xc_rot (i) _ab
＝ r (i) _1 ・ Krot (i) ・ ↑ Xrot (i) _1_ab + r (i) _2 ・ Krot (i) ・ ↑ Xrot (i) _2_ab
＋ …… ＋ r (i) _m (i) ・ Krot (i) ・ ↑ Xrot (i) _m (i) _ab …… （17）

Furthermore, the following equation (18) is obtained from this equation (17).

↑ Xc_rot (i) _ab = r (i) _1 ・ ↑ Xrot (i) _1_ab + r (i) _2 ・ ↑ Xrot (i) _2_ab
＋ …… ＋ r (i) _m (i) ・ ↑ Xrot (i) _m (i) _ab …… （18）

Therefore, in this embodiment, between the spring-like rotational displacement amount ↑ Xc_org (i) of the i-th representative contact surface and the spring-like rotational displacement amount ↑ Xorg (i) _j of the j-th movable link contact surface, It is assumed that the same relationship as the above equation (18) holds for all components (three components). That is, it is assumed that the following formula (19) is established.

↑ Xc_rot (i) = r (i) _1 ・ ↑ Xrot (i) _1 + r (i) _2 ・ ↑ Xrot (i) _2
＋ …… ＋ r (i) _m (i) ・ ↑ Xrot (i) _m (i) …… （19）

At this time, the following equation (20) is obtained from the equation (19) and the equation (13).

Krot (i) _j = r (i) _i ・ Kc_rot (i) (20)

In the present embodiment, the above equation (19) is obtained from the spring-like rotational displacement amount ↑ Xc_rot (i) of the representative contact surface (i-th representative contact surface) and the j-th movable link contact surface (j = 1, 2,..., M (i)) spring-like rotational displacement amount ↑ Xrot (i) _j is a basic expression that represents the relationship between the rotational spring constant matrix Kc_rot (i) of the representative contact surface and the j-th movable link contact. This is a basic expression representing the relationship between the surface (j = 1, 2,..., M (i)) and the rotational spring constant matrix Krot (i) _j.

  Accordingly, each diagonal component of the rotational spring constant matrix Krot (i) _j of the j-th movable link contact surface (spring constant related to each rotational displacement around the three axes) is proportional to the weight coefficient r (i) _j, and the weight The larger the value of the coefficient r (i) _j (closer to “1”), that is, the jth movable link translational contact force ↑ with respect to the contact surface normal force component Fnt (i) of the total translational contact force ↑ Ft (i) ↑ The larger the ratio of the contact surface normal force component Fn (i) _j of F (i) _j is, the larger it is. In other words, the greater the ratio of Fn (i) _j to Fnt (i), the springy rotational displacement amount ↑ Xrot (i) _j of the jth movable link contact surface with respect to the required jth movable link perturbation contact force moment. It is assumed that the sensitivity of the change of is increased.

  In addition, when only the single j-th movable link 100_j is in contact with the i-th contact target surface (when r (i) _j = 1), the j-th movable link contact surface and the i-th representative contact surface. Corresponds to the rotation spring constant matrix Krot (i) _j of the j-th movable link contact surface and the rotation spring constant matrix Kc_rot (i) of the i-th representative contact surface. Further, in this case, ↑ ΔM (i) _j = ↑ ΔMt (i), so that the spring-like rotational displacement amount ↑ Xrot (i) _j of the j-th movable link contact surface and the spring-like rotational displacement of the i-th representative contact surface The quantity ↑ Xc_rot (i) will match each other.

  When two or more movable links 103 are in contact with the i-th contact target surface, the spring-like rotational displacement amount ↑ Xc_rot (i) of the representative contact surface is in contact with the i-th contact target surface. This is the weighted average value (weighted average value using r (i) _j as a weighting factor) of the spring-like rotational displacement amount ↑ Xrot (i) _j of the jth movable link contact surface corresponding to each movable link 103_j.

  Next, the following formula (21) is obtained by applying the formulas (3) to (6) to the formula (2) and further applying the formulas (11) and (20).

  The above formulas (11) and (20) indicate that the point of action Pt (i) of the total contact force ↑ FMt (i) is the center point of the total contact force and the point of action of the jth movable link contact force ↑ FM (i) _j. Since it is assumed that P (i) _j is the contact force center point of the j-th movable link contact surface, VV (i) _j (j = 1, 2,..., M (i)) in equation (21). Is a matrix defined as follows when the position vector of the contact force center point of the jth movable link contact surface with respect to the action point Pt (i) as the total contact force center point is again ↑ V (i) _j. It is.

VV (i) _j: VV (i) _j · ↑ F (i) _j = V (i) _j × ↑ F (i) _j matrix where ↑ V (i) _j is j Position vector of contact force center point of movable link contact surface

In this case, the total contact force center point is more specifically the total contact force center point corresponding to the total contact force ↑ FMt (i) before the perturbation total contact force ↑ ΔFMt (i) is added. The contact force central point of the j-th movable link contact surface is the contact force corresponding to the j-th movable link contact force ↑ FM (i) _j before the j-th movable link perturbation contact force ↑ ΔFM (i) _j is added. The central point.

  Further, the following equation (22) is obtained from the above equation (21).

Here, a generalized variable vector configured with the position (position in the three-axis direction) and posture (posture angle around three axes) of the moving body 101 and the displacement amount of each joint of the moving body 101 as components is represented. ↑ q, displacement vector (= [↑ Xc_org () consisting of springy translational displacement amount ↑ Xc_org (i) at the position of the representative contact surface (i-th representative contact surface) and springy rotational displacement amount ↑ Xc_rot (i) of the posture i), ↑ Xc_rot (i)] ^T ) is ↑ Xc (i), the spring-like translational displacement amount ↑ Xorg (i) _j at the position of the j-th movable link contact surface and the spring-like rotational displacement amount ↑ Xrot (i ) _j's displacement vector (= [↑ Xorg (i) _j, ↑ Xrot (i) _j] ^T ) is expressed as ↑ X (i) _j. Hereinafter, ↑ Xc (i) is referred to as the springiness translation / rotation displacement amount of the representative contact surface, and ↑ X (i) _j is referred to as the springiness translation / rotation displacement amount of the jth movable link contact surface. More specifically, the generalized variable vector ↑ q is a vertical vector in which six components of the position and orientation of the base body 102 and the displacement amount of each joint of the moving body 101 are arranged.

  In this case, if the amount of springy translational / rotational displacement ↑ Xc (i) of the representative contact surface (i-th representative contact surface) is regarded as the displacement per unit time (time change rate) of the position and orientation of the representative contact surface. The Jacobian matrix expressing the relationship between the displacement per unit time of the position and orientation of the representative contact surface and the variation per unit time (temporal rate of change) ↑ Δq of the generalized variable vector ↑ q is The relationship between ↑ Xc (i) and ↑ Δq is expressed as a matrix Jc (i) expressed by the following equation (23). Note that ↑ Δq is a vertical vector in which the amount of change per unit time of each component of the generalized variable vector ↑ q is arranged.

↑ Xc (i) = Jc (i) ・ ↑ Δq (23)

Similarly, if the springy translational / rotational displacement amount ↑ X (i) _j of the jth movable link contact surface is regarded as the displacement amount (time change rate) of the position and posture of the jth movable link contact surface per unit time. The Jacobian expressing the relationship between the displacement amount per unit time of the position and orientation of the jth movable link contact surface and the change amount (time change rate) ↑ Δq of the generalized variable vector ↑ q per unit time The matrix is expressed as a matrix J (i) _j that represents the relationship between ↑ X (i) _j and ↑ Δq by the following equation (24).

↑ X (i) _j = J (i) _j ・ ↑ Δq (24)

Supplementally, a Jacobian matrix (↑ Xc_org (i) and ↑ Δq between the displacements per unit time of the position of the i-th representative contact surface and ↑ Δq is expressed by the following equation (25a) Jc_org (i) and the Jacobian matrix (↑ Xc_rot and ↑ Δq) representing the relationship between the displacement per unit time of the posture of the i-th representative contact surface and ↑ Δq. When a matrix expressed by the following equation (25b) is expressed as Jc_rot (i), Jc (i) = [Jc_org (i), Jc_rot (i)] ^T as shown in equation (25c).

↑ Xc_org (i) = Jc_org (i) ・ ↑ Δq (25a)
↑ Xc_rot (i) = Jc_rot (i) ・ ↑ Δq ...... (25b)
Jc (i) = [Jc_org (i), Jc_rot (i)] ^T (25c)

Similarly, a Jacobian matrix (↑ Xorg (i) _j and ↑ Δq between the displacement amount per unit time of the position of the jth movable link contact surface and ↑ Δq is expressed by the following formula ( Jorg (i) _j, a Jacobian matrix (↑ Xrot (i) _j and ↑) that expresses the relationship between the displacement amount per unit time of the posture of the jth movable link contact surface and ↑ Δq If the relationship between Δq and the matrix expressed by the following equation (26b) is expressed as Jrot (i) _j, J (i) _j = [Jorg (i) _j, Jrot (i) as shown in equation (26c). _j] ^T

↑ Xorg (i) _j = Jorg (i) _j ・ ↑ Δq (26a)
↑ Xrot (i) _j = Jrot (i) _j ↑ Δq (26b)
J (i) _j = [Jorg (i) _j, Jrot (i) _j] ^T (26c)

The following equation (27) is obtained by differentiating both sides of the equation (22) and applying the equations (23) and (24).

従って、第ｉ代表接触面の位置及び姿勢の変位に関するヤコビアン行列Ｊc(i)（以降、第ｉ代表接触面ヤコビアン行列Ｊc(i)又は代表接触面ヤコビアン行列Ｊc(i)という）は、第ｉ接触対象面における各可動リンク接触面の位置及び姿勢の変位に関するヤコビアン行列Ｊ(i)_j（ｊ＝１，２，…，ｍ(i)）から、上記の式（２７）に基づいて決定できることとなる。

Therefore, the Jacobian matrix Jc (i) (hereinafter referred to as the i-th representative contact surface Jacobian matrix Jc (i) or the representative contact surface Jacobian matrix Jc (i)) regarding the displacement of the position and orientation of the i-th representative contact surface is It can be determined based on the above equation (27) from the Jacobian matrix J (i) _j (j = 1, 2,..., M (i)) regarding the displacement and position of each movable link contact surface on the contact target surface. It becomes.

  In this case, since the position and orientation of the j-th movable link contact surface are the position and orientation of the tip of the j-th movable link 103_j, the Jacobian matrix J (i) _j (hereinafter, the movable link Jacobian matrix J (i) _j In other words, is a Jacobian matrix expressing the relationship between the amount of change per unit time in the position and orientation of the tip of the j-th movable link 103_j and ↑ Δq. Such a Jacobian matrix J (i) _j can be specified based on the observed value of the actual displacement amount of each joint of the moving body 101 and its temporal change rate.

  Then, if each leg link Jacobian matrix J_i (i = 1, 2,..., N) is specified in this way, the representative contact surface Jacobian matrix Jc can be determined by the above equation (27) using those J_i. It becomes.

  Here, there are N (N ≧ 2) different contact target surfaces (first to Nth contact target surfaces) in the operating environment of the moving body 101, and the first of these in the motion state of the moving body 101. Assume that one or more movable links 103 of the moving body 1 are in contact with each of the Nth contact target surfaces.

  In this case, said matter regarding the i-th contact target surface is a matter established for each of the first to N-th contact target surfaces.

Thus, in the state where the movable link 103 of the moving body 101 is in contact with each of the N contact target surfaces, the i th contact target surface (i = When a perturbation total contact force ↑ ΔFMt (i) with respect to 1, 2,..., N) is given, this ↑ ΔFMt (i) is the i-th representative based on the equations (5) and (6). It can be converted into the required value of the spring-like translational / rotational displacement amount ↑ Xc (i) (= [↑ Xc_org (i), ↑ Xc_rot (i)] ^T ) of the contact surface. This conversion is given by the following equation (28).

さらに、第１〜第Ｎ接触対象面のそれぞれに対応する第ｉ代表接触面（ｉ＝１，２，…，Ｎ）のばね性並進・回転変位量↑Ｘc(i)を並べたベクトル（縦ベクトル）を、総合ばね性並進・回転変位量↑Ｘc（≡[↑Ｘc(1)，↑Ｘc(2)，……，↑Ｘc(N)]^T）、第１〜第Ｎ接触対象面のそれぞれに対応する第ｉ代表接触面ヤコビアン行列Ｊc(i)（ｉ＝１，２，…，Ｎ）を縦に並べて構成される行列を、総合ヤコビアン行列Ｊc（≡[Ｊc(1)，Ｊc(2)，……，Ｊc(N)]^Tとおくと、↑Ｘcと、一般化変数ベクトル↑ｑの時間的変化率↑Δｑとの間の関係は、次式（２９）により与えられる。

Furthermore, a vector (vertical) in which the springy translational / rotational displacement amount ↑ Xc (i) of the i-th representative contact surface (i = 1, 2,..., N) corresponding to each of the first to N-th contact target surfaces is arranged. Vector) is the total springiness translational / rotational displacement amount ↑ Xc (≡ [↑ Xc (1), ↑ Xc (2), ..., ↑ Xc (N)] ^T ), A matrix constituted by vertically arranging i-th representative contact surface Jacobian matrices Jc (i) (i = 1, 2,..., N) corresponding to the respective i-th representative contact surfaces Jc (i), Jc (≡ [Jc (1), Jc ( 2), ..., Jc (N)] ^T , the relationship between ↑ Xc and the temporal change rate ↑ Δq of the generalized variable vector ↑ q is given by the following equation (29).

そして、総合ヤコビアン行列Ｊcの擬似逆行列をＪc^-1とおくと、第１〜第Ｎ接触対象面のそれぞれに対応する第ｉ代表接触面（ｉ＝１，２，…，Ｎ）のばね性並進・回転変位量↑Ｘc(i)の要求値を並べてなる総合ばね性並進・回転変位量↑Ｘcの要求値から、次式（３０）により、第１〜第Ｎ接触対象面の摂動全接触力↑ΔＦＭt(i)（ｉ＝１，２，…，Ｎ）の要求値を実現するための、移動体１０１の各関節の変位量の修正量を決定できることとなる。

When the pseudo inverse matrix of the general Jacobian matrix Jc is Jc ⁻¹ , the spring property of the i-th representative contact surface (i = 1, 2,..., N) corresponding to each of the first to N-th contact target surfaces. From the required value of the total springiness translational / rotational displacement amount ↑ Xc obtained by arranging the required values of the translational / rotational displacement amount ↑ Xc (i), perturbation total contact of the first to Nth contact target surfaces according to the following equation (30) The correction amount of the displacement amount of each joint of the moving body 101 for realizing the required value of the force ↑ ΔFMt (i) (i = 1, 2,..., N) can be determined.

↑ Δq = Jc ⁻¹・ ↑ Xc …… Formula (30)

Therefore, the required value of the perturbation total contact force ↑ ΔFMt (i) is a required operation amount for controlling the total contact force acting on the mobile body 101 from the i-th contact target surface in an arbitrary motion state of the mobile body 101. When given, the i-th representative contact surface Jacobian matrix Jc (i) is obtained based on the equation (27), and the total Jacobian composed of Jc (i) (i = 1, 2,..., N) is obtained. A pseudo inverse matrix Jc ⁻¹ of the matrix Jc is determined. Further, based on the equation (28), the springy translational / rotational displacement amount ↑ Xc (i) of the i-th representative contact surface corresponding to the required value of ↑ ΔFMt (i) is obtained. Then, the perturbation total contact force ↑ ΔFMt (i) (i =) from the representative contact surface total spring translational / rotational displacement amount ↑ Xc composed of this ↑ Xc (i) and Jc ^{−1 according to} the equation (29b). The correction amount of the displacement amount of each joint for realizing all the required values of 1, 2, ..., N) can be determined collectively.

  As a result, the displacement amount of each joint can be determined collectively without determining the correction amount itself of the position and orientation of the tip of each movable link 103_j.

  The above is the technical matter on which the present invention is based.

  The present invention will be described below on the basis of what has been described above.

The present invention relates to the operation of a moving body including a base body, a plurality of movable links connected to the base body, and a joint actuator for driving a joint of each movable link. A target motion for causing the moving body to move while contacting one or more movable links with each of the first to Nth contact target surfaces (N: an integer of 2 or more), A control device that controls the moving body according to a target total contact force that is a target value of a total contact force to be applied to the movable body from each of the first to Nth contact target surfaces;
An observed value of the total contact force actually acting on the moving body from the i-th contact target surface (i = 1, 2,..., N), which is each of the first to N-th contact target surfaces, and the i-th contact The i-th total contact force, which is a required correction amount of the total contact force to be additionally applied to the moving body from the i-th contact target surface in order to bring the deviation from the target total contact force corresponding to the target surface close to zero. A total contact force required correction amount determining means for determining each of the required correction amounts (i = 1, 2,..., N) according to the deviation;
The i-th total contact force required correction amount is a spring property of the position and posture of the i-th representative contact surface as a single virtual contact surface that represents all the contact surfaces of the movable body and the i-th contact target surface. Assuming that this occurs due to displacement, the i-th total contact force required correction amount and the spring constant of the i-th representative contact surface determined in advance corresponding to the i-th contact target surface, Representative contact surface position / posture displacement amount calculating means for calculating a required displacement amount of each position and posture of the representative contact surface (i = 1, 2,..., N);
A temporal change rate of the position and orientation of the i-th representative contact surface, a temporal change rate of a generalized variable vector composed of the position and orientation of the base body and the displacement amount of each joint of the movable body as components; Of the i-th representative contact surface Jacobian matrix Jc (i) (i = 1, 2,..., N), which is a Jacobian matrix representing the relationship between the i-th contact surface and the movable link in contact with the i-th contact target surface. The temporal change rate of the position of the contact portion of each movable link of the i-th contact movable link group which is a group or the temporal change rate of the position and posture of the contact portion of each movable link of the i-th contact movable link group Each movable link Jacobian matrix J (i) _j, which is a Jacobian matrix representing the relationship between the temporal change rate of the activation variable vector, the spring constant of the i-th representative contact surface, and the i-th contact with the movable body Total contact force actually acting from the target surface The relative position of the actual contact force center point of the contact portion of each movable link of the i-th contact movable link group with respect to the total contact force center point as the action point, and actually acts on each movable link of the i-th contact movable link group Representative contact surface Jacobian matrix calculating means for calculating from the contact force value according to the equation (27);
The calculated first to Nth representative contact surface Jacobian matrix Jc (i) () is added to the total required displacement amount ↑ Xc obtained by arranging the calculated required displacement amounts of the first to Nth representative contact surfaces. The required displacement amounts of the respective positions and orientations of the first to Nth representative contact surfaces are realized by multiplying the pseudo inverse matrix Jc ⁻¹ of the general Jacobian matrix Jc formed by arranging i = 1, 2,..., N). A joint displacement correction amount determining means for determining a joint displacement correction amount that is a correction amount of a displacement amount of each joint of the moving body for
The target joint displacement amount, which is the displacement amount of each joint of the mobile body defined by the target motion of the mobile body, is corrected according to the corrected target joint displacement amount obtained by correcting the determined joint displacement correction amount. And a joint displacement control means for controlling the joint actuator (first invention).

  Here, the expression (27) and the meaning of the variable of the expression (27) are described again as follows.

なお、本発明において、前記移動体の動作環境に存在する互いに異なる複数の接触対象面というのは、その複数の接触対象面に含まれる任意の２つの接触対象面（又はその２つの接触対象面をそれぞれ包含する平面）が互いに交差するか、又は、互いに間隔を存して平行もしくはほぼ平行に対向するような複数の接触対象面を意味する。この場合、各接触対象面は、それに接触させる可動リンクの部位に比して広い面積を有する面（例えば、通常の床面の如き面）である必要はなく、例えば、該部位と同程度又はそれよりも小さい面積を有する局所的な面であってもよい。

In the present invention, the plurality of different contact target surfaces existing in the operating environment of the moving body refers to any two contact target surfaces (or the two contact target surfaces included in the plurality of contact target surfaces). Means a plurality of contact target surfaces that cross each other or face each other in parallel or almost in parallel with a space therebetween. In this case, each contact target surface does not need to be a surface (for example, a surface such as a normal floor surface) having a larger area than the portion of the movable link to be contacted with the contact target surface. It may be a local surface having a smaller area.

  According to the first aspect of the present invention, observation of the total contact force that actually acts on the moving body from the i-th contact target surface (i = 1, 2,..., N) (hereinafter sometimes referred to as actual total contact force). Each of the i-th total contact force request correction amounts (i = 1, 2,..., N) for making the deviation between the value and the target total contact force corresponding to the i-th contact target surface approach zero. It is determined by the contact force request correction amount determining means. That is, for each contact target surface, the i-th total contact force request correction amount is determined as a feedback operation amount (control input) for causing the actual total contact force to follow the target total contact force. This i-th total contact force request correction amount corresponds to the required value of the perturbation total contact force ↑ ΔFMt (i) (i = 1, 2,..., N) corresponding to the i-th contact target surface.

  Note that the total contact force that acts on the moving body from the i-th contact target surface (i = 1, 2,..., N) is that one or more movable links of the moving body come into contact with the i-th contact target surface. Means the total reaction force received by the moving body from the i th contact target surface.

  Further, the target total contact force in the present invention is set as a target value of a contact force (reaction force) acting on the moving body from an assumed contact target surface as a model of each actual contact target surface, for example. is there. In this case, as the target total contact force of each contact target surface, for example, the resultant force is a dynamic relationship of an appropriate dynamic model with respect to the target motion of the moving body (the resultant total contact force is It is possible to employ one created so as to establish a dynamic relationship (e.g., balanced with the resultant force of gravity acting on the moving body and the inertial force generated by the target motion of the moving body). Alternatively, the reference total contact force of each contact target surface created so that the above dynamic relationship is established with respect to the target motion of the mobile object is expressed as a predetermined state quantity (the overall center of gravity of the mobile object). Even if the deviation is corrected to be close to zero according to the deviation between the target value and the actual value of each position, the amount of momentum around the center of gravity, or the state quantity such as the rate of change thereof) Good.

  Basically, the target total contact force may be set in any manner as long as the target motion of the moving body is set in a form that can be dynamically realized.

  In the present invention, the required displacement amount of the position and orientation of the i-th representative contact surface (i = 1, 2,..., N) corresponding to the determined i-th total contact force required correction amount is the representative contact surface position / posture. It is calculated by the displacement amount calculation means.

  As a result, for each contact target surface, the i-th total contact force request correction amount as a feedback operation amount (control input) for causing the actual total contact force to follow the target total contact force corresponds to the i-th contact target surface. To the required displacement amount of the position and orientation of the i-th representative contact surface which is a single virtual surface.

  Here, the displacement amount of the position and orientation of the i-th representative contact surface corresponds to the spring translational / rotational displacement amount ↑ Xc (i), and the change per unit time of the generalized variable vector ↑ q. With respect to the quantity ↑ Δq, there is a relationship of the above equation (23) expressed by the i-th representative contact surface Jacobian matrix Jc (i).

  The total springiness translation / rotation displacement amount ↑ Xc obtained by arranging the displacement amounts (springiness translation / rotation displacement amount ↑ Xc (i)) of the respective positions and orientations of the first to Nth representative contact surfaces is the above general The relationship of the above formula (29) expressed by the general Jacobin matrix Jc in which the representative contact surface Jacobian matrices Jc (1) to Jc (N) are arranged with respect to the change amount ↑ Δq of the generalized variable vector ↑ q per unit time Have

Therefore, the total required displacement amount (required total springiness translational / rotational displacement amount ↑ Xc) in which the required displacement amounts of the position and orientation of the i-th representative contact surface respectively corresponding to the first to Nth total contact force required correction amounts are arranged. Value) is multiplied by the pseudo inverse matrix Jc ⁻¹ of the overall Jacobian matrix Jc (that is, according to the above equation (30)) to realize the entire total required displacement amount (and hence the first to Nth). The joint displacement correction amount of the moving body (for realizing the total contact force request correction amount) can be determined.

  Therefore, in the present invention, each of the i-th representative contact surface Jacobian matrix Jc (i) (i = 1, 2,..., N) is calculated by the representative contact surface Jacobian matrix calculating means. In this case, the i-th representative contact surface Jacobian matrix Jc (i) is the temporal position of the contact portion of each movable link in the i-th contact movable link group, which is a group of movable links in contact with the i-th contact target surface. Each movable link which is a Jacobian matrix representing a relationship between a change rate or a temporal change rate of the position and orientation of the contact portion of each movable link of the i-th movable link group and a temporal change rate of the generalized variable vector A Jacobian matrix J (i) _j, a spring constant of the i-th representative contact surface, and each of the i-th contact movable link group with respect to a total contact force center point as an action point of the total contact force actually acting on the movable body It is calculated by the equation (27) from the relative position of the actual contact force center point of the contact portion of the movable link and the value of the contact force actually acting on each movable link of the i-th contact movable link group.

  Here, the total contact force center point on the i-th contact target surface is the i-th contact target of the moment (contact force moment) generated around the point by the total contact force acting on the moving body from the i-th contact target surface. It means a point where the component around the axis parallel to the surface becomes zero. Further, the contact force central point of the contact portion of each movable link of the i-th contact movable link group is a moment (contact force moment) generated around the point by the contact force acting on the movable link from the i-th contact target surface. This means that the component around the axis parallel to the i-th contact target surface is zero.

  It should be noted that the value of the contact force actually acting on each movable link used for the calculation of Expression (27), the position of the contact force action point, and the position of all contact force action points are the forces mounted on the moving body. Obviously, the observed value measured by a sensor or the like may be used. However, if the actual value can be approximated accurately, the estimated value is approximately estimated or predicted based on the target contact force or an appropriate model. It may be a value.

  Further, supplementing with respect to the movable link Jacobian matrix J (i) _j, in the present invention, each movable link is a movable link having a structure capable of changing the contact force moment acting on the contact portion to the contact target surface (for example, The contact state of the contact portion with the contact target surface becomes a surface contact state, and the posture of the contact portion can be changed by an actuator. The movable link) and the contact force moment acting on the contact portion are changed. The movable link may have any structure such as a movable link having a structure that cannot be made (for example, a movable link in which the contact state between the contact portion and the contact target surface is a dot).

  In this case, when each movable link has a structure capable of changing the contact force moment at the contact portion, the movable link Jacobian matrix is temporally related to the position and orientation of the contact portion of the movable link. This is a Jacobian matrix that represents the relationship between the rate of change and the temporal rate of change of the generalized variable vector. In addition, when each movable link has a structure in which the contact force moment at the contact portion cannot be changed, the movable link Jacobian matrix is expressed as a rate of change with time of the position of the contact portion of the movable link. This is a Jacobian matrix that represents the relationship between the temporal change rate of the generalized variable vector.

  These movable link Jacobian matrices can be obtained by a known method on the basis of observation values of actual displacement amounts of the joints of the moving body.

In the present invention, the joint displacement correction amount determining means determines the pseudo inverse matrix Jc ⁻¹ of the total Jacobian matrix Jc obtained by arranging the i-th representative contact surface Jacobian matrix Jc (i) calculated as described above. The joint displacement correction amount is determined by multiplying the total required displacement amount obtained by arranging the required displacement amounts of the i representative contact surface and the posture. As a result, the joint displacement correction amount for realizing the first to Nth total contact force request correction amounts is determined.

  Further, the joint displacement control means corrects the target joint displacement amount, which is the displacement amount of each joint of the moving body, defined by the target motion of the moving body, using the joint displacement correction amount determined as described above. The joint actuator is controlled in accordance with the corrected target joint displacement amount.

  This allows the displacement of each joint of the moving body to be controlled so that the actual total contact force of each contact target surface follows the target total contact force in a moving state where the moving body contacts a plurality of contact target surfaces. In other words, so-called compliance control for all contact target surfaces (first to Nth contact target surfaces) is realized.

According to the first invention, the i-th total contact force required correction amount for each contact target surface is converted into the required displacement amount of the position and orientation of the i-th representative contact surface corresponding to the contact target surface, and these requests are made. By calculating the joint displacement correction amount of each joint of the moving body from the total required displacement amount in which the displacement amounts are arranged by the pseudo inverse matrix Jc ⁻¹ of the total Jacobian matrix, the contact portion of the movable link on each contact target surface Processing that determines the correction amount of the position and orientation of the contact part of the movable link with respect to each contact target surface in consideration of the relationship between the correction of the position and orientation of the robot and the change in the actual total contact force and their mutual relationship Without executing, it is possible to collectively determine the correction amount (joint displacement correction amount) of the displacement amount of each joint of the moving body for causing the actual total contact force for all the contact target surfaces to follow the target total contact force. Therefore, the process for determining the joint displacement correction amount can be performed efficiently and in a short time.

  In this case, the weight coefficient r (i) _j of each movable link (each movable link of the i-th contact movable link group) in the equation (27) indicates that the movable link has a relatively large contact surface normal force component of the contact force. Therefore, the share of each movable link of the i-th movable movable link group with respect to the i-th total contact force required correction amount is consequently a contact. A movable link having a relatively large surface normal force component becomes larger. Accordingly, the first to Nth total contact force required correction amounts can be reliably realized without unnecessarily correcting the position and posture of the contact portion of the movable link having a relatively small contact surface normal force component. A correct joint displacement correction amount can be determined.

  Moreover, since the weight coefficient r (i) _j of each movable link of the i-th contact movable link group changes continuously, the i-th representative contact surface Jacobian matrix Jc (i) may cause a discontinuous change. Absent. For this reason, the displacement amount of each joint of the moving body can be changed smoothly and continuously. As a result, the movement of the moving body can be performed smoothly.

  Therefore, according to the first aspect of the invention, the movement of the moving body is controlled so that the moving body that performs the operation of contacting the plurality of contact target surfaces causes the total contact force that actually acts from each contact target surface to follow the target value. This can be appropriately performed without requiring a process for determining a correction amount from the target motion of the position or posture of each contact portion with respect to each contact target surface of the moving body.

  In the first aspect of the invention, the total contact force request correction amount determination means calculates each of the i th total contact force request correction amount (i = 1, 2,..., N), for example, on the i th contact target surface. It is determined by integrating the deviation (second invention).

  More preferably, the total contact force request correction amount determining means determines each of the i th total contact force request correction amounts (i = 1, 2,..., N) at least on the deviation on the i th contact target surface. Is determined by synthesizing a proportional term proportional to the integral term and an integral term obtained by integrating the deviation (third invention).

  That is, the difference between the observed value of the total contact force (actual total contact force) actually acting on the moving body from each contact target surface and the target total contact force is assumed when the target motion is created. Steady deviation resulting from steady displacement of the actual position and orientation of the actual contact target surface relative to the contact target surface, local irregularities of the actual contact target surface that are not assumed by the assumed contact target surface, etc. There are temporary deviations due to

  According to the second invention, the former deviation can be compensated. According to the third aspect of the invention, the former deviation can be compensated by the integral term, and the latter deviation can be compensated by the proportional term.

  In the second invention or the third invention, in the i th contact target surface (i = 1, 2,..., N), the difference between the observed value of the actual total contact force and the target total contact force is integrated (hereinafter, , This is expressed as ↑ ΔFMt (i) _int) is the position and posture of the assumed i-th contact target surface that is the assumed contact target surface assumed in the target motion of the mobile object corresponding to the i-th contact target surface. And a steady difference between the actual position and orientation of the i-th contact target surface. For this reason, the integral value ↑ ΔFMt (i) _int of the deviation is calculated from the above equation (28) by ↑ Xc (i), that is, ↑ ΔFMt (i) _int. ↑ Xc (i) _int calculated by 1) corresponds to a steady error between the position and orientation of the assumed i th contact target surface and the actual position and orientation of the i th contact target surface. Conceivable.

なお、式（２８−１）においては、↑Ｘc(i)_intのうちの並進変位量の成分、回転変位量の成分をそれぞれ↑Ｘc_org(i)_int、↑Ｘc_rot(i)_intと表記し、↑ΔＦＭt(i)_intのうちの並進力成分、モーメント成分をそれぞれ↑ΔＦt(i)_int、↑ΔＭt(i)_intと表記している。

In Equation (28-1), the translational displacement component and the rotational displacement component of ↑ Xc (i) _int are represented as ↑ Xc_org (i) _int and ↑ Xc_rot (i) _int, respectively. The translational force component and moment component of ↑ ΔFMt (i) _int are respectively expressed as ↑ ΔFt (i) _int and ↑ ΔMt (i) _int.

  Therefore, the actual position and orientation of the i-th contact target surface can be estimated by correcting the position and orientation of the assumed i-th contact target surface with the above-mentioned ↑ Xc (i) _int.

  Therefore, in the second aspect of the invention, the contact assumed in the target motion corresponding to the h-th contact target surface that is a predetermined specific contact target surface among the first to N-th contact target surfaces. By correcting the position and orientation of the assumed h-th contact target surface, which is the target surface, according to the required displacement amount of the h-th representative contact surface calculated by the representative contact surface position / posture displacement amount calculation means, h A contact target surface estimation means for estimating the position and orientation of the contact target surface may be further provided (fourth invention).

  Moreover, in the said 3rd invention, the contact assumed by the said target motion corresponding to the hth contact target surface which is a predetermined specific contact target surface among the said 1st-Nth contact target surfaces. It further comprises contact target surface estimation means for estimating the actual position and orientation of the h-th contact target surface by correcting the position and orientation of the assumed h-th contact target surface, which is the target surface. The means includes the integral from the integral term that composes the h-th total contact force required correction amount corresponding to the h-th contact target surface and the spring constant of the h-th representative contact surface corresponding to the h-th contact target surface. The h-th representative contact surface steady displacement amount that is the displacement amount of the position and orientation of the h-th representative contact surface corresponding to the term is calculated, and the assumed h-th contact surface of the target contact surface is calculated according to the h-th representative contact surface steady displacement amount. The actual h-th contact pair by correcting the position and orientation It may be estimated the position and attitude of the surface (fifth aspect).

  In the fourth and fifth aspects of the invention, the h-th contact target surface may be one or a part of the first to N-th contact target surfaces. It may be all of the Nth contact target surface.

  In the fourth aspect of the invention, in the state where the compliance control of the moving body is performed as described above, the total contact force is related to any one of the first to n-th contact target surfaces, the h-th contact pair target surface. The h-th total contact force required correction amount determined by the required correction amount determination means is a value obtained by integrating the deviation between the observed value of the actual total contact force on the h-th contact target surface and the target total contact force (integral Therefore, the position of the h-th representative contact surface calculated by the representative contact surface position / posture displacement amount calculation means from the h-th total contact force required correction amount and the spring constant of the h-th representative contact surface, and The required displacement amount of the posture is constant between the position and posture of the assumed h-th contact target surface that is the h-th contact target surface assumed in the target motion and the actual position and posture of the h-th contact target surface. This is equivalent to a large difference. That is, the required displacement amount corresponds to ↑ Xc (k) _int in the equation (28-1).

  Therefore, in the fourth invention, the position and orientation of the assumed h-th contact target surface are corrected according to the required displacement amount of the h-th representative contact surface calculated by the representative contact surface position / posture displacement amount calculating means. Thus, the actual position and orientation of the h-th contact target surface can be estimated.

  In the fifth aspect of the invention, in the state in which the compliance control of the moving body is performed as described above, the total contact force request correction amount determining unit determines any one of the h-th contact pair target surface. The position of the hth representative contact surface calculated by the same calculation as the representative contact surface position / posture displacement amount calculating means from the integral term of the total contact force request correction amount and the spring constant of the hth representative contact surface. And the displacement amount of the posture (the position of the h-th representative contact surface and the displacement amount of the posture calculated using the integral term instead of the h-th total contact force request correction amount) are the position of the assumed h-th contact target surface. And a position corresponding to a steady difference between the position and the actual position and position of the h-th contact target surface, that is, corresponding to ↑ Xc (k) _int in the equation (28-1).

  Therefore, in the fifth aspect of the invention, the h-th representative contact surface steady displacement amount corresponding to the integral term is calculated from the integral term corresponding to the h-th total contact force required correction amount and the spring constant of the h-th representative contact surface. By calculating and correcting the position and orientation of the assumed h-th contact target surface according to the h-th representative contact surface steady displacement amount, the actual position and orientation of the h-th contact target surface can be estimated.

  In this case, the compliance control can determine the joint displacement correction amount that can surely realize the h-th total contact force required correction amount and control the operation of the moving body. The h total contact force required operation amount or the h-th representative contact surface steady displacement amount in the fifth invention is a steady state between the position and posture of the assumed h-th contact target surface and the actual position and posture of the h-th contact target surface. As a result, the reliability corresponding to the difference is high. Therefore, the actual position and orientation of the h-th contact target surface can be accurately estimated.

In the first to fifth inventions, the pseudo inverse matrix Jc ⁻¹ of the general Jacobian matrix Jc can be calculated by a known appropriate method.

However, the pseudo inverse matrix Jc ⁻¹ of the calculated total Jacobian matrix Jc is a matrix calculated by the following equation (31) from the preset weight matrix W and the calculated total Jacobian matrix Jc. Quasi-inverse matrix calculation parameter determining means for determining the value of k in the equation (31) so that the determinant DET represented by the following equation (32) is equal to or greater than a predetermined positive threshold value. And

^{Jc -1 = W -1 · Jc T} · (Jc · W -1 · Jc T + k · I) -1 ...... (31)
DET = det (Jc ・ W ⁻¹・ Jc ^T + k ・ I) (32)
W: Predetermined weight matrix (diagonal matrix)

The pseudo inverse matrix calculation parameter determining means sets the provisional value of k so as to increase stepwise from a predetermined initial value, and uses the set provisional values to determine the value of the determinant DET. Repeatedly calculating and determining whether or not the absolute value of the calculated determinant DET is equal to or greater than the predetermined threshold, and the provisional value of k when the determination result is affirmative , A means for determining the value of k used for calculating the pseudo inverse matrix by the equation (31), and the increase amount of the provisional value of k when the determination result is negative, Set to a value proportional to the nth root of the absolute value of the deviation between the absolute value of the determinant DET calculated using the provisional value and the predetermined threshold (n: the order of Jc · W ⁻¹ · Jc ^T ). Is preferred (sixth invention).

  In Equation (31), I is a unit matrix. In addition, the weighting coefficient matrix W takes into account the responsiveness of the change in the position and posture of the representative contact surface with respect to the change in the displacement amount of each joint, and the like to realize the required displacement amount of the position and posture of the representative contact surface. The degree of correction of the joint displacement amount is adjusted for each joint. The weight coefficient matrix W may be a unit matrix.

Further, k is an adjustment parameter for preventing the determinant of the matrix in parentheses on the right side of Expression (31), that is, the determinant DET shown in Expression (32) from becoming too small. A real value greater than or equal to zero. Supplementing the adjustment parameter k, the pseudo inverse matrix Jc ⁻¹ can be basically calculated by the above equation (31) when k = 0.

In this case, however, the determinant DET may be too small (a value close to zero). In such a case, the inverse matrix of the matrix in the parenthesis on the right side of Equation (31) diverges, making it impossible to determine an appropriate pseudo inverse matrix Jc- ¹ . In order to prevent this, in Equation (31), a matrix of k times the unit matrix I is added to the first term in the parenthesis on the right side.

  On the other hand, an appropriate value of the adjustment parameter k for preventing the determinant DET from becoming too small varies depending on Jc. Further, the magnitude of the determinant DET exhibits a non-linear change with respect to a change in the value of k.

  For this reason, in the third aspect of the invention, the pseudo-inverse matrix operation parameter determining means exploratively determines the value of k at which the absolute value of the determinant DET is equal to or greater than the predetermined threshold (does not become too small).

  Specifically, the provisional value of k is set so as to increase stepwise from a predetermined initial value, the value of the determinant DET is calculated using each set provisional value, It is repeatedly determined whether or not the absolute value of the determinant DET is equal to or greater than the predetermined threshold value, and the provisional value of k when the determination result is affirmative is expressed as the equation (31). To determine the value of k used to calculate the pseudo inverse matrix.

In this case, if the increase amount of the provisional value of k is constant in this processing, it takes too much time until the value of k is finally determined, or every control processing cycle of the control device of the mobile unit The value of k determined in (1) frequently fluctuates, and as a result, discontinuous fluctuations in the calculated pseudo inverse matrix Jc ⁻¹ tend to occur.

On the other hand, according to the knowledge of the present inventor, when the order of Jc · W ⁻¹ · Jc ^T is n, the value of the determinant DET changes in proportion to the value n of the k value.

Accordingly, in the sixth aspect of the present invention, in the processing of the pseudo inverse matrix calculation parameter determining means, a matrix obtained by calculating the increase amount of the provisional value of k when the determination result is negative using the provisional value before the increase. The absolute value of the deviation between the formula DET and the predetermined threshold is set to a value proportional to the nth root of the absolute value (n: the order of Jc · W ⁻¹ · Jc ^T ).

Thus, according to the sixth aspect of the present invention, an appropriate value of k used for calculating the pseudo inverse matrix Jc ⁻¹ (the absolute value of DET is equal to or greater than a predetermined threshold value) for each control processing cycle of the control device of the moving body. Can be determined efficiently and in a short time, and the pseudo inverse matrix Jc ^-1 can be changed smoothly. Eventually, the joint displacement correction amount can be determined so as to smoothly change the displacement amount of each joint of the moving body.

  According to the sixth aspect of the invention, the weight coefficient matrix W takes into account the responsiveness of the change in the position and posture of the representative contact surface with respect to the change in the displacement amount of each joint, and the like. The degree of correction of the displacement amount of each joint for realizing the required displacement amount can be adjusted for each joint.

The figure which generalizes and shows the moving body for demonstrating this invention, and the floor reaction force which acts on this. The figure which shows schematic structure of the moving body in one Embodiment of this invention. The figure which shows the structure of each movable link of the moving body of FIG. The block diagram which shows the structure regarding control of the moving body of FIG. The block diagram which shows the function of the control apparatus shown in FIG. The figure which shows the operation example of the moving body shown in FIG. The flowchart which shows the process regarding the joint displacement correction amount determination part shown in FIG. The figure which shows the example of the other structure form of the moving body to which this invention is applied. The figure which shows the example of the other structure form of the moving body to which this invention is applied.

  An embodiment of the present invention will be described with reference to FIGS.

  Referring to FIG. 2, a moving body 1 exemplified in the present embodiment includes a base 2 and a plurality of movable links 3 a to 3 d (hereinafter collectively referred to as a movable link 3) extending from the base 2. ). In the example of this embodiment, the number of movable links 3 is four.

  Each movable link 3 is a link mechanism that can function as a leg or an arm of the moving body 1. Each movable link 3 includes a plurality of element links 4a to 4c (hereinafter, generically referred to as element links 4), and a plurality of joint portions 5a to 5c that sequentially connect these element links 4 from the base 2 side. (Hereinafter, collectively referred to as joint part 5).

  In the example of the present embodiment, the number of the element links 4 and the joint portions 5 constituting each movable link 3 is three. Of the three element links 4a to 4c, the element link 4a is connected to the base body 2 via the joint part 5a, the element link 4b is connected to the element link 4a via the joint part 5b, and the element link 4c is It is connected to the element link 4b via the joint portion 5c.

  Of the element links 4a to 4c, the element link 4c constituting the distal end portion of the movable link 3 contacts a contact target surface such as a floor surface or a wall surface existing in the operating environment of the moving body 1 when the moving body 1 moves. It is a part to be made (that is, a part corresponding to a foot part or a palm part). The element link 4c (hereinafter sometimes referred to as the distal end portion 4c of the movable link 3) is formed in a substantially plate shape in the example of the present embodiment, and is brought into contact with the floor surface or wall surface of the element link 4c. The contact surface is a flat surface. An elastic member 10 made of rubber or the like is attached to the contact surface of the element link 4c. For this reason, the element link 4 c comes into contact with a contact target surface such as a floor surface or a wall surface via the elastic member 6.

  Each of the joint portions 5a to 5c is configured by one or a plurality of joints, for example, as shown in FIG. Specifically, the joint portion 5a includes, for example, three joints 7a, 7b, and 7c, and has a degree of freedom of rotation around three axes. Further, the joint portion 5b is constituted by, for example, one joint 7d and has a degree of freedom of rotation around one axis. Further, the joint portion 5c is constituted by, for example, two joints 7e and 7f, and has a degree of freedom of rotation around two axes. Accordingly, each movable link 3 is configured to have a total of 6 degrees of freedom of motion. Thereby, each movable link 3 can perform a spatial motion.

  In addition, the structure of each movable link 3 is not restricted to said structure. For example, the movable link 3 may include a direct acting joint in addition to the rotating joint. Each movable link may be configured to have a degree of freedom of movement of 7 degrees or more. Further, the structure of each movable link 3 (the size and number of element links 4 and the structure and number of joints 11) need not be the same for all the movable links 3. For example, when the movable links 3a and 3b are mainly used as legs and the movable links 3c and 3d are mainly used as arms, the movable links 3a and 3b and the movable links 3c and 3d have different structures. It may be a thing.

  Although not shown in FIGS. 2 and 3, the movable body 1 includes a plurality of electric motors as joint actuators that respectively drive the joints 7 a to 7 f (hereinafter, collectively referred to as joints 7). 8 (shown in FIG. 4) is mounted. Each electric motor 8 is connected to a corresponding joint 7 so as to transmit a driving force (rotational driving force) via a power transmission mechanism (not shown) including a speed reducer.

  The joint actuator may be an actuator other than the electric motor, for example, a hydraulic actuator.

  In the mobile body 1 having the above configuration, each movable link 3 is spatially moved by driving each joint 7 of each movable link 3 by an electric motor 8. By this movement, the movement (including movement) of the moving body 1 can be performed. For example, two movable links 3a and 3b among the movable links 3a to 3d are leg links, and the movable links 3a and 3b are exercised in the same form (gait) as a human walking motion. A walking motion can be performed. Further, by moving the movable links 3c and 3d as arm links, it is possible to perform an operation such as pushing a wall surface. Alternatively, the four movable links 3a to 3d may be leg links, and the movable links 3a to 3d may be moved on the floor surface by moving in the same manner as a quadruped animal.

  Supplementally, in addition to the movable link 3, a head or the like may be connected to the base body 2 of the moving body 1. Further, for example, the base 2 may be constituted by two halves, and these halves may be connected via a joint.

  In the present embodiment, in order to control the operation of the moving body 1 having the above structure, a control device 50 configured by an electronic circuit unit including a CPU, a RAM, a ROM, and the like, and various sensors are provided. .

  In this case, as shown in FIG. 4, the sensor measures the attitude angle (inclination angle with respect to the vertical direction or the azimuth angle around the yaw axis) of the moving body 1 and its temporal change rate (angular velocity). Therefore, in order to measure the contact force (reaction force) received from the contact target surface in a state where the posture sensor 51 mounted on the base body 2 and the distal end portion 4c of each movable link 3 are in contact with the external contact target surface. A force sensor 52 interposed between the joint 5c and the tip (element link) 4c of each movable link 3 is provided.

  The posture sensor 51 is composed of, for example, a gyro sensor that detects angular velocities around three axes and an acceleration sensor that detects accelerations in three axis directions. Each force sensor 52 is constituted by a six-axis force sensor that detects, for example, a translational force in the three-axis direction and a moment around the three axes.

  Although not shown in FIGS. 2 and 3, the moving body 1 is provided with, for example, a rotary encoder 53 (shown in FIG. 4) as a sensor for detecting the displacement amount (rotation angle) of each joint 7. Yes. In addition, as a sensor which detects the displacement amount (rotation angle) of each joint 7, you may use other sensors, such as a potentiometer.

  The output of each of the sensors 51, 52, 53 is input to the control device 50. Then, the control device 50 measures the observed values of the operating state of the moving body 1 recognized from these input values (measured values of the attitude angle of the base 2 and its temporal change rate (angular velocity), the moving speed of the base 2). Value, measured value of contact force acting on the distal end portion 4c of each movable link 3, and measured value of displacement of each joint 7 and its temporal change rate). A target value (hereinafter referred to as a joint displacement command) of the displacement amount of each joint 7 to be performed is determined, and an actual displacement amount (actual displacement amount) of each joint 7 is determined according to the joint displacement command. 8 to control.

  As shown in FIG. 5, the control device 50 that executes such control processing includes, as shown in FIG. 5, a reference gait generation unit 61, a posture stabilization compensation force determination unit 62, a target total contact force. Determination unit 63, total contact force request correction amount determination unit 64, representative contact surface translation / rotation displacement amount calculation unit 65, representative contact surface Jacobian matrix calculation unit 66, target joint displacement amount determination unit 67, joint displacement correction amount determination unit 68 A joint displacement control unit 69 and a contact target surface estimation unit 70.

  Then, the control device 50 includes the reference gait generator 61, the posture stabilization compensation force determination unit 62, the target total contact force determination unit 63, the total contact force request correction amount determination unit 64, the representative contact surface translation / rotation displacement amount. By sequentially executing the processing of the calculation unit 65, the representative contact surface Jacobian matrix calculation unit 66, the target joint displacement amount determination unit 67, the joint displacement correction amount determination unit 68, and the joint displacement control unit 69, a predetermined calculation processing cycle is performed. A joint displacement command for the joint is sequentially determined, and the electric motor 8 is controlled via a motor drive circuit (not shown) in accordance with the joint displacement command.

  Further, the control device 50 executes the process of the contact target surface estimation unit 70 in parallel with the control process described above, so that a specific contact among the actual contact target surfaces existing in the operating environment of the moving body 1 is obtained. Estimate the position and orientation of the target surface.

  Below, the process of the control apparatus 50 including the detailed process of each said function part is demonstrated by making into an example the case where the mobile body 1 performs exercise | movement as shown in FIG.

[Process of Reference Gait Generation Unit 61]
In the present embodiment, the control device 50 uses the reference gait generator 61 to generate a reference gait as a reference target gait of the moving body 1.

  This reference gait defines the target spatial position and spatial trajectory (spatial orientation) of each part of the mobile body 1 (and consequently the target displacement amount of each joint of the mobile body 1). The target motion (which defines the trajectory) and the target total external force which defines the trajectory of the total external force, which is the total external force to be applied to the moving body 1 from the contact target surface in order to realize the target motion. “Orbit” means a time series of instantaneous values.

  In the present embodiment, the target motion of the reference gait generated by the basic gait generator 61 is a target motion for moving the moving body 1 in an operating environment in which there are a plurality of contact target surfaces including the floor surface. . In this target motion, one or more movable links 3 are brought into contact with each of a plurality of contact target surfaces, whereby the mobile body 1 receives a contact force as a reaction force from the plurality of contact target surfaces. State is included.

  As an example of such a target exercise, the present embodiment exemplifies a target exercise that causes the moving body 1 to perform an operation as shown in FIG.

  In this target motion, the moving body 1 basically performs a walking motion in which the two movable links 3a and 3b are moved as legs on the floor surface FL as one contact target surface existing in the operating environment. Then, it moves on the floor surface FL along a movement route as indicated by a broken line A.

  In this case, the movement path A is a path that passes through the opening WLa of the wall WL to which the door D that can be pushed open is attached. And in the said target exercise | movement, the mobile body 1 performs the operation | movement which pushes and opens the door D, when passing the opening WLa. In this operation, the moving body 1 faces one of the movable links 3c, 3d, for example, the front end portion 4c of the movable link 3d in a state of facing the door D in the state of facing the door D. In this contact state, the door D is opened by pushing the door D by the movable link 3d. Thus, after opening the door D, the moving body 1 moves through the opening WLa.

  In such a target motion, when the door D is pushed open, the moving body 1 comes into contact with the floor surface FL as two different contact surfaces and the door surface Da, and these floor surface FL and the door surface. A contact force as a reaction force is received from Da.

  The target motion that causes the moving body 1 to perform the above-described motion matches the shape of the surface to be touched such as the actual floor surface existing in the operating environment of the moving body 1 (the position and posture of each part of the surface to be touched). Are generated so that the movable link 3 of the moving body 1 is brought into contact with an assumed contact target surface which is a contact target surface model whose position and orientation are determined in advance. In the present embodiment, the assumed contact target surface used for creating the target motion is a contact target surface (estimated contact target surface) estimated by the contact target surface estimation unit 70 as representing an actual contact target surface. In accordance with the frequency (for example, every one step or a plurality of steps when the moving body 1 moves, or every predetermined time).

  In the moving body 1 of the present embodiment, this target motion includes the target movable link position / posture trajectory which is the trajectory of the target position and target posture of the tip (element link) 4c of each movable link 3, the target position of the base 2 and The target body position / posture trajectory, which is the trajectory of the target posture, the target center-of-gravity position trajectory, which is the trajectory of the target position of the entire center of gravity of the moving body 1, and the trajectory of the target value of the angular momentum around the entire center of gravity of the moving body 1. It consists of a target angular momentum trajectory.

  It should be noted that the “position” of the distal end portion (element link) 4c of the movable link 3 is the position of the representative point of the distal end portion 4c arbitrarily determined as representatively showing the spatial position of the distal end portion 4c ( For example, it means the position of a specific position on the contact surface of the tip 4c. The same applies to the “position” of the substrate 2. Further, the “posture” of the distal end portion 4 c of the movable link 3 means the spatial orientation of the distal end portion 4 c. The same applies to the “posture” of the base 2.

  The target movable link position / posture and the target base body position / posture described above are the positions viewed in the global coordinate system as an inertial coordinate system fixed to an arbitrary stationary object such as a floor surface of the operating environment of the moving body 1 and Expressed as posture. As this global coordinate system, for example, a point on the grounding surface (contact surface with the floor surface) of one movable link 3 as a support leg of the mobile body 1 (a leg that supports the weight of the mobile body 1 on the floor) is used as an origin. A support leg coordinate system is used in which the front-rear horizontal axis of the movable link 3 (element link) 4c is the X-axis, the vertical axis is the Z-axis, and the horizontal axis perpendicular to these is the Y-axis. It is done.

  In this case, in the walking motion of the moving body 1, every time the support leg is switched, the position of the origin of the global coordinate system and the directions of the X axis and the Y axis are updated. However, the global coordinate system may be a coordinate system that is constantly fixed with respect to an arbitrary stationary object such as a floor surface. In the following description, for convenience of description, the X axis, the Y axis, and the Z axis mean the three axes of the support leg coordinate system unless otherwise specified.

  Supplementally, the target center-of-gravity position trajectory and the target angular momentum trajectory are subordinately determined according to the target movable link position / posture trajectory of each movable link 3 and the target base body position / posture trajectory. The amount of displacement of the joint 7 is not directly specified. Therefore, the target center-of-gravity position trajectory and the target angular momentum trajectory do not necessarily have to be components of the target motion of the basic gait generated by the reference gait generator 61. In the present embodiment, the reference gait generation unit 61 generates the target center-of-gravity position trajectory and the target angular momentum trajectory by the posture stabilization compensation force determination unit 62 described later. This is for use in processing.

  In the present embodiment, the reference gait generator 61 generates a target motion of the reference gait for operating the moving body 1 in the form as shown in FIG. 6, for example, as follows.

  In the target motion generation process for performing the walking motion of the moving body 1 before and after the door D is opened, the target movable link position / posture trajectory with respect to the movable links 3c and 3d is, for example, that of the movable links 3c and 3d with respect to the base body 2. The overall relative posture is determined so as to be constantly kept constant. The target movable link position / posture trajectory and the target base body position / posture trajectory relating to the movable links 3a and 3b as legs are generated by a method similar to the method proposed by the present applicant in Japanese Patent No. 3726081, for example. .

  The generation processing of the target movable link position / posture trajectory and the target base body position / posture trajectory of the movable links 3a, 3b in this case will be schematically described. Assumed floor surface of the distal end portion 4c corresponding to the foot portion of the movable links 3a, 3b. Parameters that define the target movable link position / posture trajectory, such as a planned landing position and a planned landing time with respect to the assumed contact target surface corresponding to the floor surface, are given to the control device 50 from the outside of the moving body 1. This is determined according to the request for the moving direction and the moving speed, or the moving plan. The parameters may be input to the control device 50 from the outside, or may be stored in advance in the storage device of the control device 50.

  The trajectory of the target ZMP, which is the target position of the ZMP (Zero Moment Point), is determined according to the target movable link position / posture trajectory of the movable links 3a and 3b defined by this parameter. The target ZMP trajectory fits as much as possible in a position near the center of the support polygon on the assumed floor where the target ZMP is defined according to the target movable link position / posture trajectory of the movable links 3a and 3b, and is as smooth as possible ( It is determined to move (without step change).

  Furthermore, using an appropriate dynamic model expressing the dynamics of the moving body 1 (the relationship between the floor reaction force as an external force and the movement of the moving body 1), the target ZMP is satisfied (the moving body 1). Target body position so that the horizontal component of the moment generated around the target ZMP (the components around the X axis and the Y axis) is zero) The attitude trajectory is determined.

  Furthermore, using the geometric model (rigid link model) of the moving body 1 from the target movable link position / posture trajectory and the target base body position / posture trajectory of each movable link 3, A surrounding target angular momentum trajectory is calculated. The rigid body link model of the moving body 1 is a model in which mass and inertia are set for each element link.

  Further, in the target motion generation process for performing the operation of pushing the door D open, the target movable link position / posture trajectory related to the movable links 3a and 3b as the legs is, for example, the tip portions 4c of the movable links 3a and 3b, It is determined so as to be kept stationary by contacting (grounding) the assumed floor surface before the door D. Further, the target ZMP trajectory is determined such that the target ZMP is within a position near the center of the support polygon on the assumed floor surface.

  Further, the target movable link position / posture trajectory with respect to the movable link 3c is determined, for example, so as to keep the relative posture of the entire movable link 3c with respect to the base 2 constant.

  Then, the target movable link position / posture trajectory related to the movable link 3d for pushing the door D open is such that the tip 4c of the movable link 3d is brought into contact with a predetermined portion of the door surface D, and then a predetermined opening operation is performed. Following the opening operation of the door D in the pattern (rotational motion around the hinge), the tip 4c of the movable link 3d is determined to move.

  Furthermore, the door D is opened using an appropriate dynamic model that expresses the relationship between the opening operation of the door D and the external force acting on the door surface Da (or the reaction force received by the moving body 1 from the door surface Da). An estimated value of the reaction force (contact force) received by the moving body 1 during operation is obtained.

  Then, the dynamics of the moving body 1 (relationship between the contact force (reaction force) acting on the moving body 1 from the floor surface FL as a contact target surface and the door surface Da and the movement of the moving body 1) are expressed. By using an appropriate dynamic model, the target ZMP is satisfied (the inertial force generated by the movement of the moving body 1, the gravity acting on the moving body 1, and the contact force received by the moving body 1 from the door surface Da). The target substrate position / posture trajectory is determined so that the horizontal component of the moment that the resultant force is generated around the target ZMP (the component around the X axis and the Y axis) becomes zero.

  Further, as in the case of the walking motion of the moving body 1, the moving body 1 is moved from the target movable link position / posture trajectory and the target base body position / posture trajectory using the geometric model (rigid body link model) of the moving body 1. A target center-of-gravity position trajectory of the body 1 and a target angular momentum trajectory around the target center-of-gravity position are calculated.

  Supplementally, the method for generating the target motion of the moving body 1 is not limited to the above-described method, but can generate a target motion that can be realized by the moving body 1 in an operating environment in which an assumed contact target surface is set. Any other method may be used.

  Further, during the walking motion of the moving body 2, the relative posture of the movable links 3c and 3d with respect to the base body 2 may be determined so as to be changed in a desired form as necessary. For example, both movable links 3c and 3d may be swung back and forth in accordance with the walking motion of the moving body 1. In that case, a dynamic model is used in consideration of the change in the position of the overall center of gravity of the movable body 1 due to the change in the relative posture of the movable links 3c and 3d with respect to the base 2, and the change in angular momentum around the overall center of gravity. Thus, the target substrate position / posture trajectory may be generated.

  Further, when the door D is opened, the target movable link position / posture trajectory of the movable links 3a and 3b is set so that the movable links 3a and 3b as legs approach the door D, for example, with the opening operation of the door D. It may be determined. Furthermore, the target motion may be determined so that both the movable links 3c and 3d are brought into contact with the door surface Da and the door D is opened.

  Further, the constituent elements of the target exercise are not limited to the above. For example, when the movable body 1 is provided with a movable part (head or the like) relative to the upper body 2 in addition to the movable link 3, the trajectory of the target position and target posture of the part is also a component of the target motion. Added. For example, when each movable link 3 has a degree of freedom of 7 degrees or more, in addition to the target movable link position / posture trajectory, for example, the target position / posture trajectory of the intermediate portion of each movable link 3 is moved to the target motion. You may add as a component of.

  The constituent elements of the target motion are not particularly limited as long as they can define the trajectory of the displacement amount of each joint of the moving body 1 and may be appropriately determined according to the structure of the moving body 1 and the like.

  The target total external force in the standard gait defines the trajectory of the total external force (excluding gravity) that must be dynamically applied to the moving body 1 in order to move the moving body 1 according to the target motion. To do. The target total external force is composed of a target total translational external force that is a target value of the total translational force applied to the moving body 1 and a target total moment external force that is a target value of the total moment applied to the mobile body 1. . More specifically, the target total moment external force is expressed as a moment around an arbitrary reference point in the inertial coordinate system (for example, the origin of the support leg coordinate system).

In the following description, the target total translational external force (vector) of the target total external force of the standard gait is expressed as ↑ Ft_cmd0, and the target total moment external force (vector) is expressed as ↑ Mt_cmd0. In this embodiment, ↑ Ft_cmd0 and ↑ Mt_cmd0 are vertical vectors composed of three components, an X-axis component, a Y-axis component, and a Z-axis component, respectively. Further, the target total external force composed of ↑ Ft_cmd0 and ↑ Mt_cmd0 is denoted as ↑ FMt_cmd0. In this case, ↑ FMt_cmd0 is a vertical vector (six-component vertical vector) in which the components ↑ Ft_cmd0 and ↑ Mt_cmd0 are arranged. That is, ↑ FMt_cmd0 = [↑ Ft_cmd0, ↑ Mt_cmd0] ^T.

  The target total external force ↑ Ft_cmd0 of the target total external force ↑ FMt_cmd0 in the basic gait is the gravity acting on the overall center of gravity of the moving body 1 (the product of the overall mass of the moving body 1 and the gravitational acceleration vector), Translational force that balances the resultant force of the inertial force generated by the movement (translational movement) of the entire center of gravity of the moving body 1 (reversed the sign of the product of the translational acceleration vector of the entire center of gravity of the moving body 1 and the entire mass). (Translation force obtained by reversing the sign of the resultant force). In this case, the inertial force is calculated using the target center-of-gravity position trajectory of the moving body 1 and the value of the total mass of the moving body 1.

  The target total moment external force ↑ Mt_cmd0 of the target total external force ↑ FMt_cmd0 is determined by the inertial force moment generated around the reference point of the inertial coordinate system by the translational motion of the entire center of gravity of the moving body 1 and the target motion of the moving body 1. It is determined as a balanced moment (a moment obtained by inverting the sign of the combined moment) that balances the combined moment with the inertial moment generated around the entire center of gravity of the moving body 1.

  In this case, the inertial moment generated around the reference point by the translational movement of the entire center of gravity of the moving body 1 uses the position vector of the target center of gravity position of the moving body 1 relative to the reference point and the value of the entire mass of the moving body 1. Is calculated. In addition, the inertial moment generated around the entire center of gravity of the moving body 1 by the target motion of the moving body 1 includes the target motion of the moving body 1 and the rigid body link model (each element link has mass and inertia). Model).

  The above is the process of the reference gait generator 61. Supplementally, the reference gait does not need to be generated during the moving operation of the moving body 1, but is created in advance before the moving body 1 starts moving, and is stored in advance in the storage device of the control device 50. You may make it memorize | store and hold | maintain or input to the control apparatus 50 from the outside suitably by radio | wireless communication. In this case, the control device 50 does not need to include the reference gait generator 61.

[Process of Target Joint Displacement Determining Unit 67]
The control device 50 calculates the target motion (more specifically, the target movable link position / posture trajectory and target base body position / posture trajectory of each movable link 3 in the target motion) of the reference gait generated as described above. The target joint displacement amount determination unit 67 is input, and the target joint displacement amount determination unit 67 performs processing. The target joint displacement amount determination unit 67 is a functional unit that calculates a reference target displacement amount that is a displacement amount of each joint of the moving body 1 defined by the target motion of the reference gait. In this case, the target joint displacement amount determination unit 67 calculates the target displacement amount of each joint of the moving body 1 from the input target motion by inverse kinematics calculation processing.

  When the reference gait is created in advance, the target displacement amount of each joint corresponding to the target motion of the reference gait may be created in advance. In this case, the control device 50 does not need to include the target joint displacement amount determination unit 67.

[Processing of Posture Stabilization Compensation Force Determination Unit 62]
In parallel with the processing of the target joint displacement amount determination unit 67 (or before or after the processing), the control device 50 corrects the posture stabilization compensation force determination unit 62, the target total contact force determination unit 63, and the total contact force request correction. The amount determination unit 64, the representative contact surface translation / rotation displacement amount calculation unit 65, the representative contact surface Jacobian matrix calculation unit 66, and the joint displacement correction amount determination unit 68 are executed. These processes are processes for determining an operation amount (control input) for compliance control, and are hereinafter referred to as a compliance operation amount determination process. And the control apparatus 50 determines the joint displacement correction amount for correcting the reference | standard target displacement amount of each joint corresponding to the said reference gait by this compliance operation amount determination process.

  In this compliance operation amount determination process, the control device 50 first executes the process of the posture stabilization compensation force determination unit 62. This posture stabilization compensation force determination unit 62 is added to the moving body 1 in order to bring the deviation between the target value of the required movement state quantity in the target movement of the moving body 1 and the value of the actual movement state quantity close to zero. This is a functional unit that determines the posture stabilization compensation force, which is an external force to be applied, as a correction amount for correcting the target total external force ↑ FMt_cmd0 of the reference gait.

The movement state quantity is a kind of state quantity that is predetermined as important for stabilizing the posture of the moving body 1. In this embodiment, the position of the entire center of gravity of the moving body 1 and the angular momentum around the entire center of gravity of the moving body 1 are adopted as the amount of motion state. Then, the posture stabilization compensation force determination unit 62 applies to the moving body 1 in order to bring the deviation between the target centroid position in the reference gait and the observed value of the actual centroid position (actual centroid position) of the moving body 1 close to zero. The deviation between the translational external force ↑ Fg_dmd to be additionally applied (hereinafter referred to as the compensated translational external force ↑ Fg_dmd) and the observed value of the angular momentum around the actual center of gravity of the moving body 1 in the reference gait is zero. A pair of moment external force ↑ Mg_dmd (hereinafter referred to as compensation moment external force ↑ Mg_dmd) to be additionally applied to the moving body 1 in order to approach is determined as the posture stabilization compensation force. In the following description, a vector in which the compensated translational external force ↑ Fg_dmd and the compensation moment external force ↑ Mg_dmd are arranged is denoted as ↑ FMg_dmd (= [↑ Fg_dmd, ↑ Mg_dmd] ^T ), and this is referred to as posture stabilization compensation force.

  The compensation translational external force ↑ Fg_dmd of the posture stabilization compensation force ↑ FMg_dmd is a predetermined feedback from the deviation between the target centroid position in the reference gait and the observed value of the actual centroid position (actual centroid position) of the moving body 1. Determined by the control law. As the feedback control law, for example, a PD law is used in the present embodiment.

  That is, in the present embodiment, as shown by the following equation (33), the target centroid position ↑ Pg_cmd of the reference gait generated by the reference gait generator 61 and the observed value of the actual centroid position ↑ Pg_act of the moving body 1 Observed value of the proportional term obtained by multiplying the deviation by a predetermined gain Kp, and the temporal change rate of the target center of gravity position ↑ Pg_cmd ↑ Pg'_cmd and the actual center of gravity position of the moving body 1 ↑ Pg_act ↑ Pg'_act The compensated translational external force ↑ Fg_cmd is determined by adding the differential term obtained by multiplying the deviation from (= ↑ the temporal change rate of the observed value of Pg_act) by a predetermined value gain Kv.

↑ Fg_dmd = Kp ・ (↑ Pg_cmd− ↑ Pg_act) + Kv ・ (↑ Pg'_cmd− ↑ Pg'_act)
(33)

Note that ↑ Fg_cmd, ↑ Pg_cmd, ↑ Pg_act, ↑ Pg′_cmd, and ↑ Pg′_act are vectors of any three components (components in three axial directions). Further, the observed value of the actual center of gravity position ↑ Pg_act and the temporal change rate thereof are the measured value of the actual displacement amount (actual displacement amount) of each joint 7 indicated by the output of the rotary encoder 53, and the geometry of the moving body 1. It is calculated using an academic model (rigid link model).

  The compensation moment external force ↑ Mg_dmd of the posture stabilization compensation force ↑ FMg_dmd is a predetermined feedback based on the deviation between the target angular momentum in the standard gait and the observed angular momentum around the actual center of gravity of the moving body 1. Determined by the control law. As the feedback control law, for example, a proportional law is used in this embodiment.

  That is, in the present embodiment, as shown by the following equation (34), the target angular momentum ↑ Lg_cmd of the reference gait generated by the reference gait generator 61 and the actual angle around the actual center of gravity position of the moving body 1 The compensation moment external force ↑ Mg_dmd is determined by multiplying the deviation of the momentum (actual angular momentum) ↑ Lg_act from the observed value by a predetermined gain KL.

↑ Mg_dmd = KL ・ (↑ Lg_cmd− ↑ Lg_act) …… （34）

Note that ↑ Mg_cmd, ↑ Lg_cmd, and ↑ Lg_act are all vectors of three components (components around three axes). The observed value of the actual angular momentum ↑ Lg_act is calculated by using the measured value of the actual displacement amount of each joint 7 indicated by the output of the rotary encoder 53 and the geometric model (rigid link model) of the moving body 1. Is done.

  Supplementally, the compensated translational external force ↑ Fg_dmd may be determined by a feedback control law (for example, a proportional law) other than the PD law. Further, the compensation moment external force ↑ Mg_dmd may be determined by a feedback control law (for example, PD law) other than the proportional law.

  In the present embodiment, the target centroid position ↑ Pg_cmd and the target angular momentum ↑ Lg_cmd are given to the posture stabilization compensation force determination unit 62 from the reference gait generator 61, but these target centroid positions ↑ Pg_cmd and The target angular momentum ↑ Lg_cmd may be calculated by the posture stabilization compensation force determination unit 62 from the target motion of the reference gait.

[Process of Target Total Contact Force Determination Unit 63]
Next, the control device 50 executes processing of the target total contact force determination unit 63. The target total contact force determination unit 63 corrects the target total external force ↑ FMt_cmd0 of the standard gait by the posture stabilization compensation force ↑ FMg_dmd determined as described above, and corrects the target total external force after the correction (hereinafter, the corrected target total force). In order to realize the external force ↑ FMt_cmd1), the target total contact force ↑ FMt (i) _cmd1 (i) to be applied to the moving body 1 from each of N (N ≧ 1) contact target surfaces with which the moving body 1 contacts. = 1, 2,..., N).

  Specifically, the target total contact force determination unit 63 adds the posture stabilization compensation force ↑ FMg_dmd determined by the posture stabilization compensation force determination unit 62 to the target total external force ↑ FMt_cmd0 generated by the reference gait generation unit 61. Is added to determine the corrected target total external force ↑ FMt_cmd1. That is, ↑ FMt_cmd0 + ↑ FMg_dmd is determined as ↑ FMt_cmd1.

In this case, the corrected target total external force ↑ FMt_cmd1 is a vector (= [↑ Ft_cmd1, ↑ Mt_cmd1] ^T ) composed of a translational external force ↑ Ft_cmd1 and a moment external force ↑ Mt_cmd1. Then, ↑ Ft_cmd1 (hereinafter referred to as the corrected target total translational external force) is the sum of the target total translational external force ↑ Ft_cmd0 and the compensation translational external force ↑ Fg_dmd of the standard gait (= ↑ Ft_cmd0 + ↑ Fg_dmd), ↑ Mt_cmd1 (hereinafter referred to as corrected target total moment external force) is the sum of the target total moment external force ↑ Mt_cmd0 of the standard gait and the compensation moment external force ↑ Mg_dmd (= ↑ Mt_cmd0 + ↑ Mg_dmd).

  Then, the target total contact force determination unit 63 sets the corrected target total external force ↑ FMt_cmd1 in advance to all contact target surfaces (first to Nth contact target surfaces) with which the mobile body 1 is currently in contact. Target total contact force ↑ FMt (i) _cmd1 (i-th target total contact force ↑ FMt (i) _cmd1) (i) to be distributed according to the rules and applied to the moving body 1 from each contact target surface (i-th contact target surface) = 1, 2,..., N), respectively.

Here, in detail, the i-th target total contact force ↑ FMt (i) _cmd1 is an assumed i-th contact target surface as a model of the i-th contact target surface (its spatial position and orientation is the actual i-th contact). It is determined as a target value of the contact force acting on the moving body 1 from a contact target surface that is predetermined or similar to the contact target surface. This i-th target total contact force ↑ FM (i) _cmd1 is a target that is a target value of the translational force (translational force component of the total contact force) acting on the moving body 1 from a certain point on the assumed i-th contact target surface. From the total translational contact force ↑ Ft (i) _cmd1 and the target total contact force moment ↑ Mt (i) _cmd1 that is the target value of the moment (moment component of the total contact force) acting on the moving body 1 around the point of action This is a constructed vector (= [↑ Ft (i) _cmd1, ↑ Mt (i) _cmd1] ^T ).

  In this embodiment, the action point of the i-th target total contact force ↑ FMt (i) _cmd1 is the total contact force center point (about the point around that point) related to the total contact force acting on the moving body 1 from the i-th contact target surface. The target position of the contact point of the total contact force such that the component around the axis parallel to the i-th contact target surface of the total contact force moment is zero, which is set on the assumed i-th contact target surface. The target total contact force center point.

  Therefore, the i-th target total contact force ↑ FMt (i) _cmd1 is zero when the contact force moment about the axis parallel to the assumed i-th contact surface of the i-th target total contact force moment ↑ Mt (i) _cmd1 is zero. The total contact force. In each determination process of the i-th target total contact force ↑ FMt (i) _cmd1 (i = 1, 2,..., N), the target total contact force center point (the first point) on each of the assumed i-th contact target surfaces. i target total contact force central point) is also determined.

  The first to Nth target total contact forces ↑ FMt (1) _cmd1 to ↑ FMt (N) _cmd1 are determined (modified target total external force ↑ FMt_cmd1 is distributed) in the present embodiment, for example, as follows. It is. That is, when the current contact target surface of the mobile body 1 is one (when N = 1), the corrected target total translational external force ↑ Ft_cmd1 remains as it is, and the target total corresponding to the one contact target surface remains unchanged. The target total translational contact force ↑ Ft (i) _cmd1 of the contact force ↑ FMt (i) _cmd1 is determined.

  Further, when this target total translational contact force ↑ Ft (i) _cmd1 acts on the target total contact force center point on the assumed contact target surface corresponding to the one contact target surface, the moment generated around the reference point (= Vector product of the position vector of the target total floor reaction force center point with respect to the reference point and the target total translational contact force ↑ Ft (i) _cmd1) The target total contact force center point is determined on the assumed target contact surface so as to coincide with the component around the axis parallel to the assumed contact target surface in the rear target total moment external force ↑ Mt_cmd1.

  The target total contact force ↑ FMt (i) _cmd1 of the target total contact force moment ↑ Mt (i) _cmd1 is zero in the component around the axis that is horizontal to the assumed contact target surface of ↑ Mt (i) _cmd1, In addition, the above-mentioned reference is determined by the component around the axis perpendicular to the assumed contact target surface in ↑ Mt (i) _cmd1 and the component around the axis parallel to the assumed contact target surface in the corrected target total translational external force ↑ Ft_cmd1. The moment generated around the point is determined so as to coincide with the component around the axis perpendicular to the assumed contact target surface in the corrected target total moment external force ↑ Mt_cmd1.

  Further, when there are two or more current contact target surfaces of the moving body 1 (when N ≧ 2), the first to Nth target total contact forces ↑ FMt (1) _cmd1 to ↑ FMt (N) For example, _cmd1 is determined as follows.

  That is, the contact surface normal force component of each of the first to Nth target total contact forces ↑ FMt (1) _cmd1 to ↑ FMt (N) _cmd1 (translation force component perpendicular to the assumed i-th contact target surface corresponding to each) So that the total translational force vector is equal to the component of the corrected target total translational external force ↑ Ft_cmd1 excluding the component parallel to all of the assumed i-th contact target surfaces (i = 1, 2,..., N). The contact surface normal force components of the first to Nth target total contact forces ↑ FMt (1) _cmd1 to ↑ FMt (N) _cmd1 are determined.

  Further, the sum of the frictional force components (translational force components parallel to the assumed i-th contact surface corresponding to each) of the first to Nth target total contact forces ↑ FMt (1) _cmd1 to ↑ FMt (N) _cmd1 Of the corrected target total translational external force ↑ Ft_cmd1 so as to coincide with the components parallel to all of the assumed i-th contact target surfaces (i = 1, 2,..., N). The frictional force components of the Nth target total contact force ↑ FMt (1) _cmd1 to ↑ FMt (N) _cmd1 are determined.

  In this case, the frictional force components of the first to Nth target total contact forces ↑ FMt (1) _cmd1 to ↑ FMt (N) _cmd1 are the i-th in which the magnitude of the contact surface normal force component is relatively small. The frictional force component of the target total contact force ↑ FMt (i) _cmd1 is determined to be relatively small.

  For example, when the contact surface normal force component of the first target total contact force ↑ FMt (1) _cmd1 is smaller than the contact surface normal force component of the second target total contact force ↑ FMt (2) _cmd1 Are such that the magnitude of the friction force component of the first target total contact force ↑ FMt (1) _cmd1 is smaller than the magnitude of the friction force component of the second target total contact force ↑ FMt (2) _cmd1. A frictional force component is determined.

  Then, the i-th target total translational contact force ↑ Ft (i) _cmd1 of the i-th target total contact force ↑ FMt (i) _cmd1 (i = 1, 2,..., N) It is determined as the combined translational force with the force component.

  Furthermore, each of the i-th target total translational contact force ↑ Ft (i) _cmd1 (i = 1, 2,..., N) determined as described above is the target total contact force center on the corresponding assumed i-th contact target surface. When acting on a point (i-th target total contact force central point), the total moment generated around the reference point (= position vector of the i-th target total floor reaction force central point with respect to the reference point and the i-th target total contact force) The first to Nth targets so that the translational contact force ↑ Ft (i) _cmd1 is the total moment vector) is almost equal to the corrected target total moment external force ↑ Mt_cmd1 or as close as possible. The center point of all contact forces is determined.

  Such first to Nth target total contact center points can be determined as follows, for example. That is, a provisional value of the i-th target total contact force center point is determined for each of the assumed i-th contact target surfaces (i = 1, 2,..., N), and the provisional i-th target all contact center point is determined. When the i-th target total contact force ↑ FMt (i) _cmd1 is applied, the total moment generated around the reference point is calculated. Then, the deviation between the total moment and the corrected target total moment external force ↑ Mt_cmd1 is calculated as an error moment.

  Further, a Jacobian matrix representing the sensitivity of the change in moment about the reference point with respect to the change in position of each of the i-th target total contact force central points (change from the provisional value) is calculated, and a pseudo inverse matrix of the Jacobian matrix is calculated. By multiplying the error moment, the correction amount of each position of the i-th target total contact force central point is determined.

  Then, the first to Nth target total contact force center points are respectively determined by correcting the respective positions of the i-th target total contact force center points from the provisional values based on the determined correction amount.

  Further, the contact force moments about the axis perpendicular to the assumed i-th contact target surface of the i-th target total contact moment are determined. In this case, when the i-th target total translational contact force ↑ Ft (i) _cmd1 is applied to the i-th target total contact force central point (i = 1, 2,..., N) determined as described above, the reference The combined moment of the total moment generated around the point and the contact force moment around the axis perpendicular to the assumed i th contact target surface (i = 1, 2,..., N) is the corrected target total moment external force ↑ Mt_cmd1 A contact force moment around an axis perpendicular to each of the assumed i th contact target surfaces (hereinafter referred to as torsional force moment) is determined so as to match.

  Each of the i-th target total contact force moment Mt (i) _cmd1 (i = 1, 2,..., N) is a moment (parallel to the assumed i-th contact target surface) that matches the torsional force moment determined as described above. The component around the correct axis is determined as a moment that becomes zero).

  The first to Nth target total contact forces ↑ FMt (1) _cmd1 to ↑ FMt (N) _cmd1 are used in order to match the total moment generated around the reference point with the corrected target total moment external force ↑ Mt_cmd1. Instead of determining the torsional force moment, the sum of the friction force components of the first to Nth target total translational contact forces Ft (i) _cmd1 is assumed to be the corrected target total translational external force ↑ Ft_cmd1 Friction of two or more i-th target total translational contact forces Ft (i) _cmd1 so as not to change from values corresponding to components parallel to all i-th contact target surfaces (i = 1, 2,..., N). The correction amount of the force component may be determined. In this case, when the corrected i-th target total translational contact force ↑ Ft (i) _cmd1 is applied to the i-th target total contact force central point (i = 1, 2,..., N), respectively, the reference If the correction amount of the friction force component of two or more i-th target total translational contact forces Ft (i) _cmd1 is determined so that the total moment generated around the point matches the corrected target total moment external force ↑ Mt_cmd1 Good.

  Alternatively, in order to make the total moment generated around the reference point by the first to Nth target total contact forces ↑ FMt (1) _cmd1 to ↑ FMt (N) _cmd1 coincide with the corrected target total moment external force ↑ Mt_cmd1. The torsional force moment of the i-th target total contact force FMt (i) _cmd1 (= ↑ Mt (i) _cmd1) and the correction amount of the friction force component of two or more i-th target total translational contact forces Ft (i) _cmd1 Both of them may be determined.

  In the present embodiment, as described above, the total translation force of the first to Nth target total translational contact forces ↑ Ft (i) _cmd1 matches the corrected total translational external force ↑ Ft_cmd1, and Nth target total contact force ↑ FMt (1) _cmd1 to ↑ FMt (N) _cmd1 so that the total moment generated around the reference point matches the corrected target total moment external force ↑ Mt_cmd1 Target total contact forces ↑ FMt (1) _cmd1 to ↑ FMt (N) _cmd1 are determined together with the first to Nth target total contact center points.

  In this case, each of the i-th target total contact force central points (i = 1, 2,..., N) is an area (so-called “so-called”) in which all the contact surfaces of the moving body 1 are connected to the assumed i-th contact target surface. Determined in the region corresponding to the support polygon).

  The above is the specific processing of the target total contact force determination unit 63 in the present embodiment.

  Here, the case where the moving body 1 is moved in the form shown in FIG. 6 will be described as an example. In the walking operation before and after the opening operation of the door D by the moving body 1, the contact target surface is Only the floor surface FL (hereinafter referred to as a first contact target surface). In this situation, the first target total contact force ↑ FMt (1) __ cmd1 corresponding to the target total floor reaction force is determined by the above processing. In this case, the first target total contact force center point is the target total floor reaction force center point, and is set within the support polygon on the assumed first contact target surface (assumed floor surface). In the state where only one of the movable links 3a and 3b serving as the legs is grounded (one-leg supported state), the support polygon is assumed to be the tip portion 4c of the one movable link 3a or 3b and the assumed first contact target surface. It is the area of the contact surface. Further, in a state where both the movable links 3a and 3b are grounded (supporting both legs), this is a region where the contact surfaces of the respective distal end portions 4c of the movable links 3a and 3b and the assumed first contact target surface are connected.

  Further, in the operation of pushing the door D open, the contact target surfaces are two contacts of the floor surface FL (first contact target surface) and the door surface Da (hereinafter referred to as the second contact target surface). It is a target surface. In this situation, the first target total contact force ↑ FMt (1) _cmd1 (target total floor reaction force) and the second target total contact force ↑ FMt (2) _cmd1 (target total contact force from the door surface Da) Are determined by the above processing.

  In this case, the first target total contact force center point corresponding to the target total floor reaction force center point is set in the region of the support polygon on the assumed first contact target surface (assumed floor surface). Further, the second target total contact center point is set in the area of the contact surface between the tip end portion 4c of the movable link 3d and the assumed second contact target surface corresponding to the door surface Da. In this case, the assumed second contact target surface is a surface that rotates around the hinge of the door D in a predetermined opening operation pattern of the door D (a contact target surface whose position and posture change with time).

  Supplementally, the above-mentioned method for determining the first to Nth target total contact forces ↑ FMt (1) _cmd1 to ↑ FMt (N) _cmd1 was previously proposed by the present applicant in Japanese Patent Application No. 2010-44712. The details are described in Japanese Patent Application No. 2010-44712. The present invention is not essentially based on how to determine each target total contact force ↑ FMt (i) _cmd1. For this reason, in order to avoid redundant description, in this specification, the description of the determination process of the first to Nth target total contact forces ↑ FMt (1) _cmd1 to ↑ FMt (N) _cmd1 is only a brief description. .

  Here, the first to Nth target total contact forces ↑ FMt (1) _cmd1 to ↑ FMt (N) _cmd1 related to the N contact target surfaces with which the moving body 1 is in contact are the first to Nth targets. The total translational force of the total translational contact force Ft (i) _cmd1 is equal to (or almost coincides with) the total translational external force after correction ↑ Ft_cmd1, and the first to Nth target total contact forces ↑ FMt (1) _cmd1 ~ ↑ The total moment generated around the reference point by FMt (N) _cmd1 matches (or almost matches) the corrected target total moment external force ↑ Mt_cmd1, and the target total contact force ↑ FMt ( i) The first to Nth target total contact forces ↑ FMt (1) _cmd1 to ↑ FMt (N) _cmd1 are determined as long as _cmd1 changes continuously (smoothly). The first to Nth target total contact force ↑ FMt (1) _cmd1 to ↑ FMt (N) _cmd1 may be determined.

[Process of Total Contact Force Request Correction Amount Determination Unit 64]
Next, the control device 50 executes processing of the total contact force request correction amount determination unit 64. The total contact force request correction amount determination unit 64 applies the i th contact target surface (i) to each of the first to Nth target total contact forces ↑ FMt (1) _cmd1 to ↑ FMt (N) _cmd1 determined as described above. = 1, 2,..., N) a function for determining the required correction amount for the i-th total contact force so as to follow the observed value of the actual total contact force (i-th total contact force) acting on the moving body 1 Part.

The required correction amount of the i-th total contact force corresponds to the required value of the i-th perturbation total contact force ↑ ΔFMt (i) of the equation (2), and the translational contact force of the i-th total contact force is Required value of i-th perturbation total translational contact force ↑ ΔFt (i) as required correction amount (3-component longitudinal vector) and required correction amount of total contact force moment around i-th target total contact force center point (3 components) Perturbation total contact force moment ↑ ΔMt (i) as a vertical vector). In the following description, the required value of i-th perturbation total translational contact force ↑ ΔFt (i) is ↑ ΔFt (i) _dmd, and the required value of i-th perturbation total contact force moment ↑ ΔMt (i) is ↑ ΔMt (i) _dmd. The required value of the i-th perturbation total floor reaction force ↑ ΔFMt (i) as a six-component vertical vector that combines these is expressed as ↑ ΔFMt (i) _dmd (= [↑ ΔFt (i) _dmd, ↑ ΔMt ( i) _dmd] ^T ).

Further, the observed translational contact value (vertical vector of three components) of the actual total contact force (actual total contact force) acting on the moving body 1 from the i-th contact target surface is represented by ↑ Ft (i) _act, actual Of the total contact force (i-th actual total contact force), the observed value (three-component vertical vector) of the contact force moment is expressed as ↑ Mt (i) _act, and these ↑ Ft (i) _act, ↑ ↑ FMt (i) _act (= [↑ Ft (i) _act, ↑ Mt (i) _act] ^T ) is the observed value of the i th real total contact force as a vertical vector of 6 components combined with Mt (i) _act. Is written.

  The total contact force request correction amount determination unit 64 performs a process described below to obtain the required value ↑ ΔFMt (i) _dmd (i = 1, 2,..., N) of the i th perturbation total contact force ↑ ΔFMt (i). Respectively.

  That is, the total contact force request correction amount determination unit 64 first sets the i-th target total contact force ↑ FMt (i) _cmd1 and the i-th target contact surface (i = 1, 2,..., N), i The deviation ↑ Dfmt (i) (= ↑ FMt (i) _cmd1− ↑ FMt (i) _act) from the observed value ↑ FMt (i) _act of the actual total contact force is calculated. In this case, the observed value ↑ FMt (i) _act of the i-th actual total contact force is the value of each movable link 3 of the i-th contact movable link group that is a group of the movable links 3 in contact with the i-th contact target surface. The observed value of the contact force (six-axis force) of the movable link 3 indicated by the output of the force sensor 52 is calculated by synthesizing the i-th target total contact force center point as an action point.

  However, in this embodiment, in order to prevent the deviation ↑ Dfmt from fluctuating excessively, the total contact force request correction amount determination unit 64 determines the output of the force sensor 52 for each of the i-th contact movable link groups. By applying a low pass characteristic filtering process to the value of the i th actual total contact force obtained by synthesizing the observed values of the contact forces (six axis forces) of the movable links 3 shown, the i th target total contact force center point The observed value ↑ FMt (i) _act of the i-th actual total contact force acting on is calculated. In this case, the i-th actual total contact force is obtained by combining the observation values of the contact forces (six-axis forces) of the movable links 3 of the i-th contact movable link group, which are respectively subjected to low-pass filtering. The observed value ↑ FMt (i) _act may be calculated.

  Next, the total contact force request correction amount determination unit 64 determines, for each of the i-th contact movable link groups, the proportional term (formula (41) determined according to the deviation ↑ Dfmt (i) as shown in the following formula (41). ) And the integral term (the second term on the right side of equation (41)) are added together to obtain the required value ↑ ΔFMt (i) _dmd of the i-th perturbation total contact force ↑ ΔFMt (i). decide.

  The proportional term is obtained by multiplying the deviation ↑ Dfmt (i) by a predetermined gain Kcmp. The integral term is obtained by integrating a value ↑ Dfmt (i) _filt obtained by subjecting the deviation ↑ Dfmt (i) to low-pass filtering and a value obtained by multiplying the predetermined value gain Kestm. Here, the filtering process of the low-pass characteristic for obtaining ↑ Dfmt (i) _filt is the high-frequency of the low-pass characteristic filtering process used when the cutoff frequency on the high-frequency side calculates the deviation ↑ Dfmt (i). This is a filtering process with a characteristic that the frequency becomes lower than the cutoff frequency on the side.

↑ ΔFMt (i) _dmd = Kcmp ・ ↑ Dfmt (i) + ∫ （Kestm ・ ↑ Dfmt (i) _filt)
...... (41)

The gains Kcmp and Kestm may be scalars, but may be diagonal matrices.

  Here, the integral term in the equation (41) corresponds to a steady deviation between the i-th target total contact force ↑ FMt (i) _cmd1 and the observed value ↑ FMt (i) _act of the i-th actual total contact force. Corresponds to an error in the position and orientation of the assumed i th contact target surface relative to the actual i th contact target surface.

  In the present embodiment, the required value ↑ ΔFMt (i) _dmd of the i-th perturbation total contact force ↑ ΔFMt (i) is calculated by the above equation (41), but the proportional term is omitted and the required value ↑ ΔFMt (i ) _dmd may be calculated. Also, in the calculation of the integral term, ↑ Dfmt (i) may be used as it is instead of ↑ ΔDfmt (i) _filt.

  The above is the processing of the total contact force request correction amount determination unit 64 in the present embodiment.

  Here, the case where the moving body 1 is moved in the form shown in FIG. 6 will be described as an example. In the walking operation before and after the opening operation of the door D by the moving body 1, the floor surface FL ( The required value ↑ ΔFMt (1) _dmd of the first perturbation total contact force ↑ ΔFMt (1) regarding the first contact target surface) is calculated by the above equation (41).

  Further, during the operation of pushing the door D open, the required value ↑ ΔFMt (1) _dmd of the first perturbation total contact force ↑ ΔFMt (1) related to the floor surface FL (first contact target surface) and the door surface Da (first) The required value ↑ ΔFMt (2) _dmd of the second perturbation total contact force ↑ FMt (2) for the two contact target surface) is calculated by the above equation (41).

[Processing of representative contact surface translation / rotation displacement calculation unit 65]
After executing the processing of the total contact force request correction amount determination unit 64 as described above, the control device 50 executes the processing of the representative contact surface translation / rotation displacement amount calculation unit 65. The representative contact surface translation / rotation displacement amount calculation unit 65 calculates the required value ↑ ΔFMt (i) _dmd (i = 1, 2,..., I-th perturbation total floor reaction force ↑ ΔFMt (i) determined as described above. N) is converted into a springy translational / rotational displacement amount ↑ Xc (i) (= [↑ Xc (i)) of a virtual i-th representative contact surface corresponding to the i-th contact target surface according to the equation (28). _org, ↑ Xc (i) _rot] ^T ).

  In this case, in this embodiment, the translation spring constant matrix Kc_org (i) of the i-th representative contact surface representing the contact surface of the moving body 1 with respect to each of the i-th contact target surfaces (i = 1, 2,..., N). Each diagonal component (spring constant of each component relating to translational displacement of the i-th representative contact surface in the three-axis direction) and each diagonal component (rotation coefficient i) of the rotational spring constant matrix Kc_rot (i) of the i-th representative contact surface. The spring constant of each component relating to the rotational displacement around the three axes of the representative contact surface is a constant value determined in advance for each i-th contact target surface (i = 1, 2,..., N). Yes.

  Then, the representative contact surface translation / rotation displacement amount calculation unit 65 uses these spring constant matrices Kc_org (i) and Kc_rot (i) to determine the spring property of the i-th representative contact surface based on the equation (28). The translational / rotational displacement amount ↑ Xc (i) is calculated. Specifically, according to the following equation (42) obtained by substituting ↑ ΔFMt (i) _dmd for ↑ ΔFMt (i) on the right side of equation (28), the springy translational / rotational displacement amount ↑ of the i-th representative contact surface ↑ Xc (i) is calculated respectively.

この式（４２）におけるＫc_org(i)^-1は、並進ばね定数行列Ｋc_org(i)の逆行列（Ｋc_org(i)の各対角成分の逆数値を対角成分とする行列）、Ｋc_rot(i)^-1は、回転ばね定数行列Ｋc_rot(i)の逆行列（Ｋc_rot(i)の各対角成分の逆数値を対角成分とする行列）である。これらのＫc_org(i)^-1，Ｋc_rot(i)^-1は、それぞれ、制御装置５０で並進ばね定数行列Ｋc_org(i)、回転ばね定数行列Ｋc_rot(i)から算出するようにしてもよいが、Ｋc_org(i)^-1，Ｋc_rot(i)^-1をあらかじめ制御装置５０の記憶装置に記憶保持しておくようにしてもよい。

In this equation (42), Kc_org (i) ⁻¹ is an inverse matrix of the translational spring constant matrix Kc_org (i) (a matrix in which the inverse value of each diagonal component of Kc_org (i) is a diagonal component), Kc_rot (i ^-1 is an inverse matrix of the rotation spring constant matrix Kc_rot (i) (a matrix having the inverse value of each diagonal component of Kc_rot (i) as a diagonal component). These Kc_org (i) ⁻¹ and Kc_rot (i) ⁻¹ may be calculated from the translation spring constant matrix Kc_org (i) and the rotation spring constant matrix Kc_rot (i) by the control device 50, respectively. Kc_org (i) ⁻¹ and Kc_rot (i) ⁻¹ may be stored in the storage device of the control device 50 in advance.

  The above is the processing of the representative contact surface translation / rotation displacement amount calculation unit 65 in the present embodiment.

  Here, the case where the moving body 1 is moved in the form shown in FIG. 6 will be described as an example. In the walking operation before and after the opening operation of the door D by the moving body 1, the floor surface FL ( The springy translational / rotational displacement amount ↑ Xc (1) of the first representative contact surface corresponding to the first contact target surface) is calculated by the above equation (42).

  Further, during the operation of pushing the door D open, the springy translational / rotational displacement amount ↑ Xc (1) of the first representative contact surface corresponding to the floor surface FL (first contact target surface) and the door surface Da (first surface) The springy translational / rotational displacement amount ↑ Xc (2) of the second representative contact surface corresponding to (2 contact target surface) is calculated by the above equation (42).

[Processing of Representative Contact Surface Jacobian Matrix Calculation Unit 66]
In the compliance operation amount determination process, the controller 50 calculates the spring-like translational / rotational displacement amounts ↑ Xc (1) to ↑ Xc (N) of the first to Nth representative contact surfaces as described above. The processing of the representative contact surface Jacobian matrix calculation unit 66 is executed (or before or after the calculation processing of ↑ Xc (1) to ↑ Xc (N)).

  The representative contact surface Jacobian matrix calculation unit 66 is a functional unit that calculates the i-th representative contact surface Jacobian matrix Jc (i) (i = 1, 2,..., N) that expresses the relationship of the equation (23). is there. More specifically, the generalized variable vector ↑ q in the present embodiment is a vertical vector in which six components of the position and posture of the base body 2 and the displacement amount of each joint 7 of the moving body 1 are arranged. A vertical vector in which the amount of change per unit time of each component of ↑ q is arranged is ↑ Δq in Expression (23).

  The representative contact surface Jacobian matrix calculation unit 66 calculates the i-th representative contact surface Jacobian matrix Jc (i) according to the equation (27). Here, the expression (27) and the meanings of the main variables of the expression (27) are described again as follows.

この場合、式（２７）の右辺の演算に必要な変数値は、次のように決定される。

In this case, the variable value necessary for the calculation of the right side of Expression (27) is determined as follows.

  Based on the observed value (measured value) of the actual displacement amount (actual joint displacement amount) of each joint 7 indicated by the output of the rotary encoder 53 with respect to the weight coefficient r (i) _j related to the i-th contact target surface. The observation value of the current actual posture of the tip portion 4c of each movable link 3 of the contact movable link group is determined. Further, based on the observed value of the actual posture of the tip end portion 4c of each movable link 3 (each of the first to m (i) th movable links 3) of the i-th contact movable link group and the output of the force sensor 52. , Current actual contact surface normal force component Fn (i) _j (actual contact surface normal force component Fn (i) _j) (j = 1, 2,..., M of each movable link 3 of the i-th contact movable link group. The observed value of (i)) is obtained. Then, from the observed value of the actual contact surface normal force component Fn (i) _j of each movable link 3 of the i-th contact movable link group, the weighting factor r (i) _j is determined according to the definition formula shown by the formula (27-1). (J = 1, 2,..., M (i)) is determined. In this case, in order to prevent the value of the weight coefficient r (i) _j from fluctuating frequently, a filtering process such as a low-pass filter may be applied to each observed value of Fn (i) _j.

  Supplementally, the weight coefficient r (i) _j may be determined based on the observed value of the actual contact surface normal force component Fn (i) _j of each movable link 3 of each movable link 3 of the i-th movable movable link group. Preferably, the weight coefficient r (i) _j may be determined using an approximate estimated value or predicted value of the actual contact surface normal force component Fn (i) _j instead of the observed value. . For example, when the target contact force of each movable link 3 of the i-th contact movable link group is set, and the actual contact force of each movable link 3 is considered to approximately match the target contact force. The weight coefficient r (i) _j may be determined using the target value instead of the observed value of the actual contact surface normal force component Fn (i) _j.

The coefficient matrix Rk (i) related to the matrix A (i) _j (j = 1, 2,..., M (i)) related to the i-th contact target surface is a translation spring constant determined in advance for the i-th representative contact surface. It is determined from the matrix Kc_org (i) and the rotational spring constant matrix Kc_rot (i) (or its inverse matrix Kc_rot (i) ⁻¹ ) according to the above equation (27-3). The coefficient matrix Rk (i) may be stored in advance in the storage device of the control device 50.

  Further, the position vector ↑ V (i) _j necessary for determining the matrix VV (i) _j related to the matrix A (i) _j is determined as follows, for example.

  Based on the observation value (measured value) of the current actual displacement amount of each joint 7 indicated by the output of the rotary encoder 53 and the output of the force sensor 52, the contact of each movable link 3 of the i-th contact movable link group. A position vector (hereinafter referred to as ↑ Va (i) _j) of the current actual contact force center point (actual contact force center point) on the surface (contact surface with the i-th contact target surface) is specified. . The reference point of this position vector may be an arbitrary point.

  Next, the position vector ↑ Vc (i) (= r) of the weighted average point corresponding to the weight coefficient r (i) _j of the actual contact force central point corresponding to each movable link 3 of the i-th movable movable link group. (i) _1 ・ ↑ Va (i) _1 + r (i) _2 ・ ↑ Va (i) _2 ＋ …… + r (i) _m (i) ・ ↑ Va (i) _m (i)) Calculated as the position vector of the force center point.

  Then, by ↑ V (i) _j = ↑ Va (i) _j− ↑ Vc (i) (j = 1, 2,..., M (i)), the i-th contact movable with respect to the i-th actual total contact force center point. The position vector ↑ V (i) _j of the actual contact force center point of the contact surface of each movable link 3 (jth movable link 3) of the link group is determined.

  The matrix VV (i) _j related to the matrix A (i) _j is VV (i) _j · ↑ F (i) _j = ↑ V (i) _j × ↑ F (i) _j according to the definition. It is determined.

Of the variable values necessary for the calculation of the right side of Expression (27), the movable link Jacobian matrix J (i) _j related to the jth movable link 3 of the i th contact movable link group is determined as follows. The movable link Jacobian matrix J (i) _j is the amount of springy translational / rotational displacement ↑ X (i) _j of the jth movable link contact surface (the contact surface between the jth movable link 3 and the i th contact target surface) ↑ X (i) _j (= [↑ Xorg (i) _j, ↑ Xrot (i) _j] This is a matrix that expresses the relationship between ^T and the amount of change ↑ Δq per unit time of the generalized variable vector ↑ q by the above equation (24). .

  Here, the spring-like translational displacement amount ↑ Xorg (i) _j of ↑ X (i) _j is regarded as the translational displacement amount per unit time of the j-th movable link contact surface. It may be considered that it coincides with the temporal change rate (translation speed) of the position of the current contact portion of the front end portion 4c. Further, the spring-like rotational displacement amount ↑ Xrot (i) _j of ↑ X (i) _j is the rotational displacement amount per unit time of the jth movable link contact surface. It may be considered that it coincides with the temporal change rate (angular velocity) of the current posture of the tip portion 4c.

  Therefore, in this embodiment, the change per unit time of the position of the current contact portion of the tip portion 4c of the j-th movable link 3 and the posture of the tip portion 4c (more specifically, a vertical vector having the position and posture as components). Assuming that the amount (time change rate) coincides with ↑ X (i) _j on the left side of the equation (24), the leg link Jacobian matrix J (i) for the jth movable link 3 of the i th contact movable link group. Determine _j. The position of the current contact portion of the tip portion 4c of the jth movable link 3 is, for example, the actual contact force center point of the contact surface of the jth movable link 3 (jth movable link contact surface). The position of the point of the tip 4c is used.

  In this case, the movable link Jacobian matrix J (i) _j indicates the current operating state of the moving body 1 (which is specified by the measured value of the current actual displacement amount of each joint 7) as a minute change of ↑ q (perturbation). ) Is determined by a known method. For example, using the geometric model (rigid link model) of the moving body 1 or by analytical calculation, the j-th movable with respect to a minute change (a minute change from the current state) of each component of the generalized variable vector ↑ q The movable link Jacobian matrix J (i) _j is determined by calculating the change in the position of the contact portion of the tip 4c of the link 3 and the change in the posture of the tip 4c.

  In this case, the current position of the contact portion of the tip 4c of the j-th movable link 3 of the i-th movable link group is the current actual contact of the contact surface with the i-th contact target surface of the j-th movable link 3. It is assumed to coincide with the observed value of the position of the force center point. Then, a change in the position of this point (change with respect to a minute change in each component of ↑ q) is calculated as a change in the position of the contact portion of the tip end portion 4c of the jth movable link 3.

  Further, the current posture of the distal end portion 4 c of the j-th movable link 3 is calculated based on the observed value (measured value) of the actual displacement amount of each joint indicated by the output of the rotary encoder 53. It is assumed that the measured value of the current actual posture of the tip portion 4c coincides with it, and the change in the posture of the tip portion 4c (change with respect to a minute change of each component of ↑ q) is calculated.

  In the present embodiment, the representative contact surface Jacobian matrix calculator 66 calculates the weight coefficient r () determined as described above for each of the i-th contact target surfaces (i = 1, 2,..., N) with which the moving body 1 contacts. i) _j (j = 1, 2,..., m (i)), matrix VV (i) _j, and movable link Jacobian matrix J (i) _j are used to calculate the right side of equation (27). Thus, the i-th representative contact surface Jacobian matrix Jc (i) is calculated.

  The above is the processing of the representative contact surface Jacobian matrix calculation unit 66 in the present embodiment.

  Here, the case where the moving body 1 is moved in the form shown in FIG. 6 will be described as an example. In the walking operation before and after the opening operation of the door D by the moving body 1, the floor surface FL ( A first representative contact surface Jacobian matrix Jc (1) corresponding to the first contact target surface) is calculated by the equation (27).

  In this case, in the state where only one of the movable links 3a and 3b of the moving body 1 is in contact with the floor surface FL, m (1) in the equation (27) is m (1) = 1, and the movable link Jacobian. The matrix J (1) _1 is a movable link Jacobian matrix related to the movable link 3a or 3b in contact with the floor surface FL.

  When both the movable links 3a and 3b are in contact with the floor surface FL, m (1) in Expression (27) is m (1) = 2, and the movable link Jacobin matrix J (1) One of _1 and J (1) _2 is a movable link Jacobian matrix related to the movable link 3a, and the other is a movable link Jacobian matrix related to the movable link 3b.

  Further, when the door D is pushed open, it corresponds to the first representative contact surface Jacobian matrix Jc (1) corresponding to the floor surface FL (first contact target surface) and the door surface Da (second contact target surface). The second representative contact surface Jacobian matrix Jc (2) is calculated by the equation (27).

  In this case, Jc (1) is calculated in the same manner as when both the movable links 3a and 3b are in contact with the floor surface FL during the walking motion. In calculating Jc (2), m (2) in equation (27) is m (2) = 1, and the movable link Jacobian matrix J (2) _1 is in contact with the door surface Da. It is a movable link Jacobian matrix regarding the movable link 3d.

[Processing of Joint Displacement Correction Amount Determination Unit 68]
In the compliance operation amount determination process, as described above, the controller 50 determines the amount of springy translation / rotation displacement ↑ Xc of the i-th representative contact surface for each of the i-th contact target surfaces (i = 1, 2,..., N). After calculating (i) and the representative contact surface Jacobian matrix Jc (i), the process of the joint displacement correction amount determining unit 68 is executed.

  This joint displacement correction amount determining unit 68 is a correction amount of each joint displacement amount corresponding to all of the spring-like translational / rotational displacement amounts ↑ Xc (1) to ↑ Xc (N) of the first to Nth representative contact surfaces. It is a functional unit that determines a joint displacement correction amount that is (a correction amount from a target joint displacement amount corresponding to the target motion of the reference gait).

In the process of the joint displacement correction amount determination unit 68, first, a general Jacobian matrix Jc (≡ [Jc (1), Jc (1), Jc (1), Jc (1), Jc (1), Jc (N)] is arranged. Jc (2), ......, pseudo inverse matrix Jc ^-1 of Jc (N)] ^T) is calculated. Here, since the general Jacobian matrix Jc is generally not a square matrix, there is no inverse matrix of Jc. For this reason, the joint displacement correction amount determination unit 68 calculates a pseudo inverse matrix Jc ⁻¹ of the general Jacobian matrix Jc.

Then, the joint displacement correction amount determining unit 68 converts the pseudo inverse matrix Jc ⁻¹ to the springy translational / rotational displacement amount ↑ Xc (1) of the first to Nth representative contact surfaces, as shown in the equation (30). 〜 ↑ Xc (= [↑ Xc (1), ↑ Xc (2), ..., ↑ Xc (N)] ^By multiplying ^T ), a generalized variable required correction amount vector ↑ Δq_dmd having the required correction amount (required perturbation amount) of each component of the generalized variable vector ↑ q as a component is calculated. Of the components of the generalized variable required correction amount vector ↑ Δq_dmd, a component indicating the required correction amount of the displacement amount of each joint is determined as the joint displacement correction amount.

In this case, the joint displacement correction amount determination unit 68 calculates the pseudo inverse matrix Jc ⁻¹ by the equation (31).

That is, in the present embodiment, the pseudo inverse matrix Jc ⁻¹ is calculated as a weighted pseudo inverse matrix (weighted SR-Inverse).

Specifically, the process of calculating the pseudo inverse matrix Jc ⁻¹ is executed as shown in the flowchart of FIG. 7 including the process of determining the value of the adjustment parameter k.

First, in STEP 1, a transposed matrix Jc ^T of the general Jacobian matrix Jc and an inverse matrix W ⁻¹ of the weight coefficient matrix W are calculated. In the present embodiment, the weighting coefficient matrix W is a matrix in which the value of each diagonal component (weighting coefficient) is determined in advance. However, the value of each diagonal component of the weighting coefficient matrix W may be changed as appropriate according to the operating state of the moving body 1.

  Next, in STEP 2, zero that is the standard value of k is set as the initial value of the candidate value of the adjustment parameter data k.

  Next, in STEP 3, using the current candidate value of the adjustment parameter data k, the value of the determinant DET is calculated by the equation (32).

  Next, in STEP 4, whether or not the magnitude (absolute value) of this determinant DET is smaller than a predetermined lower limit threshold DET_L (> 0) (whether the magnitude of DET is too small). Is judged.

When the determination result of STEP 4 is negative (when the size of DET is an appropriate size that is not too small), the processing of STEP 10 is executed. In this process, the current candidate value of k is determined as the value of the adjustment parameter data k, and the pseudo inverse matrix Jc ⁻¹ is calculated by the calculation of the equation (31) using the value of the adjustment parameter k.

  On the other hand, if the determination result in STEP 4 is affirmative (when the magnitude of DET is too small), the amount of increase Δk (> 0) is determined. In STEP 9, the candidate value of the adjustment parameter k is updated to a value that is increased from the current value by the increase amount Δk. Thereafter, the processing of STEP 3 to 9 is repeated until the determination result of STEP 4 becomes negative. As a result, the value of the adjustment parameter k is determined in an exploratory manner so that the determinant DET has an appropriate size equal to or greater than the predetermined lower threshold DET_L.

  Here, the determination process of the increase amount Δk will be described below. In the process of exploringly determining the value of the adjustment parameter k such that the determinant DET has an appropriate size equal to or greater than a predetermined lower threshold DET_L, for example, an increase amount Δk of the adjustment parameter k candidate value is determined in advance. It is generally considered to update the candidate value of the adjustment parameter k as a constant value.

However, in this case, if the increase amount Δk is set to a larger value, the determinant DET calculated as k = 0 is too small, and is sequentially determined at each calculation processing cycle of the control device 50. Since the k value is a discrete value with a step size that is an integral multiple of Δk, it tends to be excessively larger than necessary for the minimum k value such that DET ≧ DET_L. For this reason, it is easy to produce the dispersion | variation in the difference of the magnitude | size of the determinant DET corresponding to the value of k sequentially determined by each arithmetic processing period of the control apparatus 50, and the said lower limit threshold value DET_L. As a result, when the joint displacement correction amount is determined by the equation (29) using the pseudo inverse matrix Jc ⁻¹ determined by the equation (30), the joint displacement correction amount is likely to vary discontinuously. As a result, the smoothness of the movement of the moving body 1 may be impaired.

  On the other hand, by setting the increase amount Δk to a sufficiently small value, the above inconvenience can be solved. However, in that case, it takes a long time to determine an appropriate value of the adjustment parameter k for which the determination result in STEP 4 is negative. For this reason, there is a possibility that an appropriate value of the adjustment parameter k cannot be determined within the time of each calculation processing cycle of the control device 50.

  Therefore, in the present embodiment, in the processes of STEPs 5 to 8, the increase amount Δk of the adjustment parameter k candidate value is determined according to the magnitude of the deviation between the determinant DET calculated in STEP 3 and the lower limit threshold DET_L. It was set to be variable.

  Specifically, in STEP 5, the value calculated by the following equation (43) is set as the provisional value Δkp of the increase amount Δk of the adjustment parameter k.

Δkp = G · (DET_L− | DET |) ^{1 / n} (43)

That is, a value obtained by multiplying the nth root of the deviation (= DET_L− | DET |) by subtracting the absolute value of the determinant DET from the lower limit threshold DET_L (deviation (= DET_L− | DET |) is set as the provisional value Δkp of the increase amount Δk. Note that n is the order of the matrix (Jc · W ⁻¹ · Jc ⁻¹ ). The value of the gain G is a predetermined constant value.

The provisional value Δkp of the increase amount Δk is thus determined for the following reason. The value of the determinant det (Jc · W ⁻¹ · Jc ^T ) (the value of DET when k = 0) and the value of the determinant DET (= det (Jc · W ⁻¹ · Jc ^T + k · I)) Is an n-order function of k (a function of the form k ⁿ + a · k ^n-1 +...), And therefore, the change in the value of the determinant DET with respect to the change in the value of k is approximately k ⁿ ( It is considered that it is proportional to k raised to the nth power. Therefore, in the present embodiment, the value calculated by the above (43) is set as the provisional value Δkp of the increase amount Δk.

  In STEPs 6 to 8, limit processing for preventing the increase amount Δk from becoming excessively small is applied to the provisional value Δkp, thereby determining the increase amount Δk. Specifically, in STEP 6, the provisional value Δkp of the increase amount Δk is compared with a predetermined lower limit value Δkmin (> 0). If Δkp ≧ Δkmin, the provisional value Δkp is used as it is in the increment Δk As determined. If Δkp <Δkmin, Δkmin is determined as the increase amount Δk in STEP8.

  Thereby, the increase amount Δk is determined to be a value calculated by the equation (43) according to the magnitude of the deviation between the magnitude of the determinant DET and the lower limit threshold value DET_L, with Δkmin being the lower limit value. Become.

The joint displacement correction amount determination unit 68 calculates the generalized variable request correction amount vector ↑ Δq_dmd by performing the calculation of the right side of the equation (30) using the pseudo inverse matrix Jc ⁻¹ calculated as described above. To do. Then, the joint displacement correction amount determination unit 68 determines, as a joint displacement correction amount, a component indicating the required correction amount of the displacement amount of each joint among the components of the generalized variable request correction amount vector ↑ Δq_dmd. The joint displacement correction amount determined in this way is the i-th actual total contact force (i = 1, 2,..., N) for each of the first to N-th contact target surfaces. This is a compliance operation amount as a required correction amount of the displacement amount of each joint to approach FMt (i) _cmd1.

[Processing of Joint Displacement Control Unit 69]
The compliance operation amount determination process described above (posture stabilization compensation force determination unit 62, target total contact force determination unit 63, total contact force request correction amount determination unit 64, representative contact surface translation / rotation displacement amount calculation unit 65, representative After executing the processing of the contact surface Jacobian matrix calculation unit 66 and the joint displacement correction amount determination unit 68), the control device 50 next executes the processing of the joint displacement control unit 69.

  This joint displacement control unit 69 uses the target joint displacement amount correction unit 68 to set the target joint displacement amount (target joint displacement amount corresponding to the target motion of the reference gait) determined by the target joint displacement amount determination unit 67. By adding the determined joint displacement correction amount, the corrected target joint displacement amount as the final joint displacement command for each joint is determined.

  Then, the joint displacement control unit 69 sets the electric motor 41 (joint actuator) to a motor drive circuit such as a servo amplifier (not shown) so that the actual displacement amount of each joint matches the corrected target joint displacement amount determined as described above. Control through.

[Process of Contact Target Surface Estimator 70]
The control device 50 executes the process of the contact target surface estimation unit 70 in parallel with the drive control of each joint of the moving body 1. The contact target surface estimation unit 70 is a functional unit that calculates a contact target surface estimated value that is an estimated value of the position and orientation of each actual contact target surface (actual contact target surface) with which the mobile body 1 contacts.

  For this processing, the contact target surface estimation unit 70 receives the required value ↑ ΔFMt (i) _dmd (i = 1) of the i-th perturbation total contact force ↑ ΔFMt (i) from the total contact force request correction amount determination unit 64. , 2,..., N), the value of the integral term (second term) on the right side of the equation (41), that is, the i th target total contact force ↑ FMt_cmd1 and the i th actual total contact force. Deviation from value ↑ FMt (i) _act ↑ Dfmt (i) (= ↑ FMt (i) _cmd1− ↑ FMt (i) _act) is subjected to low-pass filtering, and value ↑ Dfmt (i) _filt An integrated value obtained by multiplying a predetermined value gain Kestm (scalar or diagonal matrix) (= 入 力 (Kestm · ↑ Dfmt (i) _filt)) is input. Hereinafter, the value of this integral term is expressed as ΔFMt (i) _int as shown in the following equation (44).

↑ ΔFMt (i) _int≡∫ （Kestm ・ ↑ Dfmt (i) _filt) …… （44）

Here, due to the drive control of each joint of the moving body 1 described above, the displacement amount of each joint is the observed value ↑ FMt (i) _act (i = 1, 2,..., N) of the i th actual total contact force. Since each i-th target total contact force ↑ FMt (i) _cmd1 is controlled so that the deviation ↑ Dfmt (i) approaches zero, the integral term value ↑ ΔFMt (i) _int is This occurs due to a steady error of the position and orientation of the assumed i-th contact target surface used for creating the reference gait with respect to the i-th actual contact target surface.

  Therefore, the springy translational / rotational displacement amount of the i-th representative contact surface corresponding to this ↑ ΔFMt (i) _int is the position and posture of the assumed i-th contact target surface used for creating the target motion of the standard gait. This is considered to correspond to a steady error with respect to the actual i th contact target surface (i th actual contact target surface).

Therefore, the contact target surface estimation unit 70 converts the integral term value ↑ ΔFMt (i) _int given with respect to the i-th contact target surface into a springy translational / rotational displacement amount by the above equation (28-1). Is obtained as an error ↑ Xc (i) _int of the position and orientation of the assumed i-th contact target surface. In this case, more specifically, the translational force component ↑ ΔFt (i) _int of the integral term value ↑ ΔFMt (i) _int is added to the inverse matrix Kc_org () of the translational spring constant matrix Kc_org (i) of the i-th representative contact surface. i) Multiplying ⁻¹ (= Kc_org (i) ⁻¹ · ↑ ΔFt (i) _int) is assumed to be an error ↑ Xc_org (i) _int of the position of the assumed i th contact target surface. The moment component ↑ ΔMt (i) _int of the integral term value ↑ ΔFMt (i) _int is multiplied by the inverse matrix Kc_rot (i) ⁻¹ of the rotation spring constant matrix Kc_rot (i) of the i-th representative contact surface. (= Kc_rot (i) ⁻¹ · ↑ ΔMt (i) _int) is assumed as an error ↑ Xc_rot (i) _int of the posture of the assumed i th contact target surface.

  Then, the contact target surface estimation unit 70 corrects the position and orientation of the assumed i-th contact target surface according to the error ↑ Xc (i) _int calculated as described above, thereby correcting the position of the i-th actual contact target surface. And an i-th contact target surface estimated value (i = 1, 2,..., N) as estimated values of the posture are determined.

  In this embodiment, the i-th contact target surface estimated values (i = 1, 2,..., N) determined by the touch target surface estimation unit 70 are respectively given to the reference gait generation processing unit 61 in this embodiment. . Then, in the reference gait generation processing unit 61, the assumed i-th contact target surface used for creating the reference gait is periodically (eg, walking of the moving body 1) according to the i-th contact target surface estimated value. Update every step or every step during operation).

  Note that the assumed i-th contact target surface used for creating the reference gait does not need to be determined only according to the i-th contact target surface estimated value. For example, when the position and orientation of the i th actual contact target surface can be estimated based on the visual sensor mounted on the moving body 1 or information on each contact target surface given from the outside, the estimated first The i-th contact target surface estimated value may be used for evaluating the reliability and complementary correction of the position and orientation of the i-contact target surface.

  The above is the details of the processing of the control device 50 in the present embodiment.

  Here, a supplementary description will be given of the correspondence between the present embodiment and the present invention. In the present embodiment, the total contact force request correction amount determination unit 64 implements the total contact force request correction amount determination means in the present invention. In this case, in this embodiment, the i-th target total contact force ↑ FMt (i) _cmd1 determined by the target total contact force determination unit 63 corresponds to the target total contact force corresponding to the i-th contact target surface in the present invention. To do.

  The representative contact surface translation / rotation displacement amount calculation unit 65, the representative contact surface Jacobian matrix calculation unit 66, the joint displacement correction amount determination unit 68, and the joint displacement control unit 69 respectively represent the representative contact surface position / posture displacement amount in the present invention. A calculation means, a representative contact surface Jacobian matrix calculation means, a joint displacement correction amount determination means, and a joint displacement control means are realized.

  Of the processes executed in the joint displacement correction amount determining unit 68, the process of STEP 2 to 9 in FIG. 5 realizes the pseudo inverse matrix operation parameter determining means in the present invention.

  Further, the contact target surface estimation unit 70 implements the contact target surface estimation means in the present invention (the fifth invention). In this case, the error target ↑ Xc (i) _int calculated by the equation (28-1) from the integral term value ↑ ΔFMt (i) _int is calculated by the contact target surface estimation unit 70 for each contact target surface. This corresponds to the representative contact surface steady displacement amount in the present invention. In the present embodiment, each of the first to Nth contact target surfaces corresponds to the hth contact target surface in the present invention.

  According to the embodiment described above, the control device 50 determines the i th actual total contact force ↑ FMt (i) _cmd1 (i = 1, 2,..., N) in each control processing cycle. The required value ↑ ΔFMt (i) _dmd of the i-th perturbation total contact force ↑ ΔFMt (i), which is the operation amount (control input) for following the total contact force, is the amount of spring translation / rotation displacement of the i-th representative contact surface. ↑ Convert to Xc (i).

Further, the controller 50 determines the total springiness translation / rotation displacement amount ↑ Xc (= [↑ Xc () as a vector (vertical vector) in which the ↑ Xc (i) (i = 1, 2,..., N) are arranged. 1), ↑ Xc (2), ..., ↑ Xc (N)] ^T ) is multiplied by the pseudo inverse matrix Jc ^-1 of the general Jacobin matrix Jc in which the i-th representative contact surface Jacobian matrix Jc (i) is arranged To realize all of ↑ Xc (i) (i = 1, 2,..., N) (and thus the i-th actual total contact force of each contact target surface is the i-th target total floor reaction force ↑ FMt ( i) Determine a joint displacement correction amount of each joint of the moving body 1 which is a compliance operation amount (to follow _cmd1). Then, the control device 50 electrically drives the displacement amount of each joint according to the corrected target joint displacement amount obtained by correcting the target joint displacement amount of each joint corresponding to the target motion of the reference gait with the joint displacement correction amount. Control is performed via a motor 8 (joint actuator).

  Thereby, according to this embodiment, correction of the position and posture of the tip part (element link) 4c of each movable link 3 that contacts each contact target surface, and the change of the actual total contact force of each contact target surface The i-th total floor reaction force is set to the i-th target without executing the process of determining the correction amount of the position and orientation of the tip of each movable link 3 in consideration of the relationship between them and their mutual relationship. The joint displacement correction amount of each joint for following the total contact force ↑ FMt (i) _cmd1 can be determined collectively. Therefore, the process of determining the joint displacement correction amount for compliance control can be efficiently performed in a short time.

  In this case, each movable link 3 (j-th j) of the i-th movable movable link group in the equation (27) for calculating the i-th representative contact surface Jacobian matrix Jc (i) (i = 1, 2,..., N). The weight coefficient r (i) _j of the movable link 3) is set to a larger value (a value closer to “1”) as the movable link 3 has a relatively large contact surface normal force component Fn (i) _j. It becomes.

  For this reason, the share of each movable link 3 in the i-th movable link group with respect to the required value ↑ ΔFMt (i) _dmd of the i-th perturbation total contact force ↑ ΔFMt (i) results in the contact surface normal force component Fn ( i) The movable link 3 having a relatively large _j becomes larger. In other words, the required value ↑ ΔFMt (i) _dmd of the i-th perturbation total contact force ↑ ΔFMt (i) is as large as possible in the tip 4c of the movable link 3 where the contact surface normal force component Fn (i) _j is relatively large. The i-th representative contact surface Jacobian matrix Jc (i) is determined so that it can be realized by correcting the position and orientation of.

  Therefore, the i-th perturbation total contact force ↑ ΔFMt (i) without unnecessarily correcting the position and posture of the tip portion 4c of the movable link 3 having a relatively small contact surface normal force component Fn (i) _j. Required displacement value ↑ ΔFMt (i) _dmd can be reliably realized, that is, the i-th actual total floor reaction force (i = 1, 2,..., N) is expressed as the i-th target total contact force, respectively. ↑ An appropriate joint displacement correction amount can be determined for following FMt (i) _cmd1.

  Further, since the weight coefficient r (i) _j of each movable link 3 of the i-th contact movable link group (i = 1, 2,..., N) changes continuously, the i-th representative contact surface Jacobian matrix Jc (i ) Does not cause discontinuous changes. For this reason, the displacement amount of each joint of the moving body 1 can be changed smoothly and continuously. As a result, the movement of the moving body 1 can be performed smoothly.

  In the embodiment, the joint displacement correction amount determining unit 68 searches for the value of the adjustment parameter k for preventing the value of the determinant DET from becoming too small (the processes in STEP 2 to 9 in FIG. 7). ), The increase amount Δk of the adjustment parameter k is set to a value proportional to the n-th root of the deviation (= DET_L− | DET |) obtained by subtracting the absolute value of the determinant DET from the lower threshold DET_L.

Thus, in each control processing cycle of the control device 50, an appropriate adjustment parameter k value for satisfying | DET | ≧ DET_L is set so that discontinuous fluctuation of the k value does not occur. It can be determined efficiently and in a short time. Therefore, it is possible to smoothly change the pseudo inverse matrix Jc ⁻¹ for determining the joint displacement correction amount from the total springiness translational / rotational displacement amount ↑ Xc. As a result, the joint displacement correction amount can be determined so that the displacement amount of each joint of the moving body 1 is smoothly changed.

  Further, as described above, the i-th perturbation total contact force is controlled while driving the joints of the mobile body 1 so that the i-th actual total contact force follows the i-th target total contact force ↑ FMt (i) _cmd1. ↑ Required value of ΔFMt (i) ↑ Translation / rotational displacement amount of i-th representative contact surface corresponding to the value of the integral term ↑ ΔFMt (i) _int which is a component of ΔFMt (i) _dmd ↑ Xc (i) _int ↑ Xc (i) _int as a steady error with respect to the actual i-th contact target surface with respect to the position and orientation of the assumed i-th contact target surface is calculated with high reliability. Can be obtained. For this reason, it is possible to accurately estimate the actual position and orientation of each contact target surface.

  Next, some modifications of the embodiment described above will be described.

  In the above-described embodiment, the moving body 1 includes the moving body including the four movable links 3. However, the moving body may include five or more or three movable links.

  Moreover, the front-end | tip part 4c of each movable link 3 of the mobile body 1 of the said embodiment changes the contact-force moment which acts on this from a contact object surface (it changes a contact-force center point with the contact surface of this front-end | tip part). However, the tip of each movable link may have a structure in which the contact force moment cannot be changed.

  For example, as shown in FIG. 8, m (i) movable links 103 (i) _j (j = 1, 2,..., M (i) of the i-th contact movable link group that contacts any i-th contact target surface. )) Having a spherical tip (more generally, the contact surface with the i-th contact target surface is substantially point-like (including the contact surface having a very small area)). There may be.

  In this case, the point-like contact surface (contact point) with the i-th contact target surface of each movable link 103 (i) _j of the i-th contact movable link group matches the contact force center point. It is substantially impossible to apply a contact force moment to the movable link 103 (i) _j (and to change it). In this case, since the perturbation contact force moment ↑ M (i) _j that can be added to the movable link 103 (i) _j is always zero, the spring property of the contact surface of the movable link 103 (i) _j The rotational displacement amount ↑ Xrot (i) _j (= Krot (i) _j · ↑ M (i) _j) is always zero. As a result, the Jacobian matrix Jrot (i) _j representing the relationship between the spring-like rotational displacement amount ↑ Xrot (i) _j and the variation amount ↑ Δq of the generalized variable vector ↑ q per unit time is always a zero matrix. Become.

Therefore, the movable link Jacobian matrix J (i) _j in this case becomes J (i) _j = [Jorg (i) _j, 0] ^T by the above equation (26c). For this reason, the process of calculating the i-th representative contact surface Jacobian matrix Jc (i) by the equation (27) is equivalent to the process of calculating Jc by the following equation (27a).

この式（２７ａ）におけるｒ(i)_j、Ｒk(i)、VV(i)_jは、それぞれ、前記式（２２）に示したものと同じある。また、Ｊorg(i)_jは、前記式（２６ａ）に示したヤコビアン行列、すなわち、第ｊ可動リンク接触面のばね性並進変位量↑Ｘorg_iと、一般化変数ベクトル↑ｑの単位時間当たりの変化量↑Δｑとの間の関係を表すヤコビアン行列である。

In this equation (27a), r (i) _j, Rk (i), and VV (i) _j are the same as those shown in the equation (22). Jorg (i) _j is the Jacobian matrix shown in the above formula (26a), that is, the change per unit time of the springy translational displacement amount ↑ Xorg_i of the jth movable link contact surface and the generalized variable vector ↑ q. It is a Jacobian matrix representing the relationship between the quantity ↑ Δq.

The values of r (i) _j, Rk (i), and VV (i) _j for performing the calculation of the equation (27a) can be determined in the same manner as in the above embodiment. Since J (i) _j = [Jorg (i) _j, 0] ^T , Jorg (i) _j is set in the same manner as in the case of calculating the movable link Jacobian matrix J (i) _j in the above embodiment. Can be calculated.

  Further, for example, as shown in FIG. 9, the moving body has m (i) movable links 103 (i) _j (j = 1, 2, j in the i-th contact movable link group that contacts an arbitrary i-th contact target surface. .., M (i)) may be a moving body having a wheel at the tip.

  In this embodiment, in order to improve the stability of the posture of the moving body 1, the target total external force ↑ FMt_cmd0 of the reference gait is corrected and the first to Nth target total contact force ↑ FMt (1) _cmd1 ↑ FMt (N) _cmd1 is determined, but the target total external force ↑ FMt_cmd0 of the standard gait is used as it is, and the first to Nth target total contact forces ↑ FMt (1) _cmd1 to ↑ FMt (N) _cmd1 May be determined.

  In the embodiment, the deviation ↑ Dfmt (i) (= ↑ FMt (i) between the i-th target contact force ↑ FMt (i) _cmd1 and the observed value of the i-th actual total contact force ↑ FMt (i) _act The required value ↑ ΔFMt (i) _dmd of the i-th perturbation total floor reaction force ↑ ΔFMt (i) is obtained by synthesizing (adding together) the proportional term and the integral term corresponding to _cmd1- ↑ FMt (i) _act). However, the integral term (= ↑ ΔFMt (i) _int) may be determined as it is as the required value ↑ ΔFMt (i) _dmd of the i-th perturbation total contact force ↑ ΔFMt (i).

  In such a case, the springiness translational / rotational displacement amount ↑ ΔXc (i) of the i-th representative contact surface calculated from ↑ ΔFMt (i) _dmd by the representative contact surface translational / rotational displacement amount calculating unit 65 is obtained. This corresponds to the steady error ↑ Xc (i) _int of the position and orientation of the assumed i-th contact target surface with respect to the actual i-th contact target surface.

  Therefore, in this case, the contact target surface estimation unit 70 does not need to calculate ↑ Xc (i) _int again by the equation (28-1), and the representative contact surface translational / rotational displacement amount calculation unit 65. By correcting the position and orientation of the assumed i th contact target surface according to the springiness translational / rotational displacement amount ↑ ΔXc (i) of the i th representative contact surface calculated by It may be determined. By doing in this way, one Embodiment of the said 2nd invention and the 4th invention will be constructed | assembled.

  Moreover, in the said embodiment, although the actual position and attitude | position were estimated about all of the 1st-Nth contact target surface, only about the specific contact target surface (hth contact target surface), the actual The position and orientation may be estimated. For example, when the operation of the moving body 1 shown in FIG. 6 is performed, the actual position and orientation of the floor surface FL (first contact target surface) may be estimated by the contact target surface estimation unit 70. .

  Moreover, in the said embodiment, although the contact target surface estimation part 70 was provided, this may be abbreviate | omitted. In that case, the deviation ↑ Dfmt (i) (= ↑ FMt (i) _cmd1− between the i-th target contact force ↑ FMt (i) _cmd1 and the observed value of the i-th actual total contact force ↑ FMt (i) _act The proportional term corresponding to ↑ FMt (i) _act) may be determined as the required value ↑ ΔFMt (i) _dmd of the i-th perturbation total floor reaction force ↑ ΔFMt (i).

  In the above embodiment, the case where the moving body 1 contacts two contact target surfaces has been described as an example. However, the present invention is also applied to the case where the moving body 1 contacts three or more contact target surfaces. Of course you can. Further, the plurality of contact target surfaces with which the mobile body 1 is brought into contact may be contact target surfaces in which two contact target surfaces are substantially parallel to each other.

  DESCRIPTION OF SYMBOLS 1,101 ... Moving body, 2,102 ... Base | substrate, 3,103 ... Movable link, 50 ... Control apparatus, 64 ... Total contact force request | requirement correction amount determination part (total contact force request | requirement correction amount determination means), 65 ... Representative contact Surface translation / rotation displacement amount calculation unit (representative contact surface position / posture displacement amount calculation unit), 66 ... representative contact surface Jacobian matrix calculation unit (representative contact surface Jacobian matrix calculation unit), 68 ... joint displacement correction amount determination unit (joint displacement) Correction amount determination means), 69 ... joint displacement control section (joint displacement control means, 70 ... contact target surface estimation section (contact target surface estimation means), STEP 2 to 9 ... pseudo inverse matrix calculation parameter determination means.

Claims (6)

A plurality of different contact target surfaces existing in an operating environment of the mobile body are provided with a base body, a plurality of movable links connected to the base body, and a joint actuator that drives a joint of each movable link. A target motion for causing the moving body to move while bringing one or more movable links into contact with each of the first to Nth contact target surfaces (N: an integer of 2 or more), and realizing the target motion A control device that controls the moving body according to a target total contact force that is a target value of a total contact force to be applied from each of the first to Nth contact target surfaces;
An observed value of the total contact force actually acting on the moving body from the i-th contact target surface (i = 1, 2,..., N), which is each of the first to N-th contact target surfaces, and the i-th contact The i-th total contact force, which is a required correction amount of the total contact force to be additionally applied to the moving body from the i-th contact target surface in order to bring the deviation from the target total contact force corresponding to the target surface close to zero. A total contact force required correction amount determining means for determining each of the required correction amounts (i = 1, 2,..., N) according to the deviation;
The i-th total contact force required correction amount is a spring property of the position and posture of the i-th representative contact surface as a single virtual contact surface that represents all the contact surfaces of the movable body and the i-th contact target surface. Assuming that this occurs due to displacement, the i-th total contact force required correction amount and the spring constant of the i-th representative contact surface determined in advance corresponding to the i-th contact target surface, Representative contact surface position / posture displacement amount calculating means for calculating a required displacement amount of each position and posture of the representative contact surface (i = 1, 2,..., N);
A temporal change rate of the position and orientation of the i-th representative contact surface, a temporal change rate of a generalized variable vector composed of the position and orientation of the base body and the displacement amount of each joint of the movable body as components; Of the i-th representative contact surface Jacobian matrix Jc (i) (i = 1, 2,..., N), which is a Jacobian matrix representing the relationship between the i-th contact surface and the movable link in contact with the i-th contact target surface. The temporal change rate of the position of the contact portion of each movable link of the i-th contact movable link group which is a group or the temporal change rate of the position and posture of the contact portion of each movable link of the i-th contact movable link group Each movable link Jacobian matrix J (i) _j, which is a Jacobian matrix representing the relationship between the temporal change rate of the activation variable vector, the spring constant of the i-th representative contact surface, and the i-th contact with the movable body Total contact force actually acting from the target surface The relative position of the actual contact force center point of the contact portion of each movable link of the i-th contact movable link group with respect to the total contact force center point as the action point, and actually acts on each movable link of the i-th contact movable link group Representative contact surface Jacobian matrix calculating means for calculating from the contact force value according to the following equation (27):
The calculated first to Nth representative contact surface Jacobian matrix Jc (i) (i =) is added to the total required displacement amount obtained by arranging the calculated first and Nth representative contact surface position and orientation required displacement amounts. 1,..., N) are multiplied by a pseudo inverse matrix Jc ⁻¹ of a general Jacobian matrix Jc, and the required displacement amounts of the respective positions and orientations of the first to Nth representative contact surfaces are realized. A joint displacement correction amount determining means for determining a joint displacement correction amount that is a correction amount of a displacement amount of each joint of the mobile body;
The target joint displacement amount, which is the displacement amount of each joint of the mobile body defined by the target motion of the mobile body, is corrected according to the corrected target joint displacement amount obtained by correcting the determined joint displacement correction amount. A moving body control device comprising: a joint displacement control means for controlling a joint actuator.

In the moving body control device according to claim 1,
The total contact force required correction amount determining means determines each of the i th total contact force required correction amount (i = 1, 2,..., N) by integrating the deviation on the i th contact target surface. A control device for a moving body. In the moving body control device according to claim 1,
The total contact force request correction amount determination means includes a proportional term in which each of the i-th total contact force request correction amount (i = 1, 2,..., N) is proportional to at least the deviation on the i-th contact target surface. A control apparatus for a moving body, characterized in that it is determined by combining an integral term obtained by integrating the deviation. In the control apparatus of the moving body of Claim 2,
An assumed hth contact object that is a contact object surface assumed in the target motion corresponding to an hth contact object surface that is a predetermined specific contact object surface among the first to Nth contact object surfaces. The actual position and orientation of the h-th contact target surface are corrected by correcting the position and orientation of the surface in accordance with the required displacement amount of the h-th representative contact surface calculated by the representative contact surface position / posture displacement amount calculation unit. A moving body control device, further comprising a contact target surface estimating means for estimating. In the control apparatus of the moving body of Claim 3,
An assumed hth contact object that is a contact object surface assumed in the target motion corresponding to an hth contact object surface that is a predetermined specific contact object surface among the first to Nth contact object surfaces. Contact surface estimation means for estimating the actual position and orientation of the h-th contact target surface by correcting the position and orientation of the surface;
The contact target surface estimation means includes the integral term that composes the h-th total contact force required correction amount corresponding to the h-th contact target surface and the spring constant of the h-th representative contact surface corresponding to the h-th contact target surface. The h-th representative contact surface steady displacement amount, which is a displacement amount of the position and orientation of the h-th representative contact surface corresponding to the integral term, is calculated, and the assumption is made according to the h-th representative contact surface steady-state displacement amount. An apparatus for controlling a moving body, wherein the position and orientation of an actual h-th contact target surface are estimated by correcting the position and orientation of the h-contact target surface. In the control apparatus of the moving body of any one of Claims 1-5,
The pseudo inverse matrix Jc ⁻¹ of the calculated total Jacobian matrix Jc is a matrix calculated by the following equation (31) from a preset weight matrix W and the calculated total Jacobian matrix Jc. Pseudo-inverse matrix operation parameter determination means for determining the value of k in the expression (31) so that the determinant DET represented by the following expression (32) is equal to or greater than a predetermined positive threshold value;

^{Jc -1 = W -1 · Jc T} · (Jc · W -1 · Jc T + k · I) -1 ...... (31)
DET = det (Jc ・ W ⁻¹・ Jc ^T + k ・ I) (32)
W: Predetermined weight matrix (diagonal matrix)

The pseudo inverse matrix calculation parameter determining means sets the provisional value of k so as to increase stepwise from a predetermined initial value, and uses the set provisional values to determine the value of the determinant DET. Repeatedly calculating and determining whether or not the absolute value of the calculated determinant DET is equal to or greater than the predetermined threshold, and the provisional value of k when the determination result is affirmative , A means for determining the value of k used for calculating the pseudo inverse matrix by the equation (31), and the increase amount of the provisional value of k when the determination result is negative, Set to a value proportional to the nth root of the absolute value of the deviation between the absolute value of the determinant DET calculated using the provisional value and the predetermined threshold (n: the order of Jc · W ⁻¹ · Jc ^T ). A control device for a moving body.

Images