JP2012245573A - Floor surface estimating device - Google Patents

Abstract

PROBLEM TO BE SOLVED: To properly estimate the position and posture of an actual floor surface in an operating environment of a moving body while properly controlling the motion of the moving body in such a manner that the total floor reaction force actually acting on the moving body follows a target total floor reaction force.SOLUTION: The required correction amount of the total floor reaction force designed for bringing deviation between an observation value of the total floor reaction force acting on the moving body 101 and the target total floor reaction force close to zero is converted into the springy displacement of a position and a posture of a representative contact surface representing a ground contact surface of the moving body 101, the correction amount of the displacement of each joint of the moving body 101 is determined by multiplying this springy displacement by a pseudo inverse matrix Jcof a Jacobian matrix Jc representing a relationship between variations of the position and posture of the representative contact surface per unit time and variations of a generalization variable vector of the moving body 101 per unit time, thereby correcting the target displacement of each joint. The displacement of each joint is controlled according to the corrected target joint displacement. The position and posture of the actual floor surface are estimated by correcting a supposed floor surface according to a value obtained by integrating the deviation.

Description

  The present invention relates to an apparatus for estimating the position and posture of a floor surface that moves a moving body such as a legged mobile robot.

  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.

  Further, as a technique for estimating the actual inclination angle of the floor surface of the moving environment of the legged mobile robot, for example, a technique found in Patent Document 2 has been proposed by the present applicant.

Patent No. 3629133 Japanese Patent No. 3024028

  By the way, during the operation of a moving body that performs compliance control as described above, the steady deviation between the total floor reaction force actually acting on the moving body and the target total floor reaction force creates the target motion of the moving body. This is considered to be caused by an error in the position and posture of the assumed floor surface used for the actual floor surface. Therefore, while performing compliance control as seen in Patent Document 1, based on the deviation between the actual observed value of the total floor reaction force and the target total floor reaction force, It is conceivable to estimate the position and posture of the floor.

  However, in this case, the following inconvenience may occur.

  That is, in the conventional compliance control technique as seen in Patent Document 1, the actual floor reaction force moment generated around the target total floor reaction force center point is the tip of each leg link (following the target moment) ( The target position and posture of the foot part) are corrected by changing the vertical positions of the tip ends of both leg links in opposite directions (hereinafter referred to as the first correction in this section), and the tip of each leg link. It is assumed that a correction for changing the posture of the part (hereinafter referred to as a second correction in this column) is synthesized.

  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.

  However, 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 are likely 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 easily occurs.

  As a result, when the actual position and posture of the floor surface are estimated by a technique as shown in Patent Document 2 based on the deviation between the actual measured value of the total floor reaction force and the target total floor reaction force. Therefore, it may be difficult to sufficiently increase the estimation accuracy.

  In addition, 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 target total floor reaction force central point by the first correction) A gain that defines the degree of the moment to be generated around the target moment) and a gain for determining the amount of correction of the posture of the tip of each leg link by the second correction It is necessary to appropriately adjust the gain that defines the degree of the moment to be generated around the center point of the floor reaction force with respect to the target moment. The adjustment work generally has the disadvantage of requiring a large number of man-hours.

  In addition, when applying the technique shown in Patent Document 2 to the technique shown in Patent Document 1, a compliance model for floor surface estimation is set apart from the compliance control technique shown in Patent Document 1. Necessary. For this reason, there is also an inconvenience that the control process of the moving body becomes complicated.

  Each leg link is made in view of the background of the present invention, and controls the movement of the moving body so that the total floor reaction force actually acting on the moving body follows the target total floor reaction force. Appropriately estimate the actual floor surface position and posture of the moving environment while not performing the process of determining the amount of correction of the position and posture of the tip from the target motion. An object of the present invention is to provide a floor surface estimation device capable of performing the above.

  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 N (N ≧ 2) leg links 103_1 to 103_N are connected to a base body 102 is assumed. Each of the leg links 103_1 to 103_N 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, the moving body 101 moves by grounding (contacting the floor) the tip of the supporting leg with any one or more of the N leg links 103_1 to 103_N as the supporting leg. The weight of the body 101 can be supported on the floor surface through the support legs, and the mobile body 101 can move on the floor surface by performing spatial movement of the leg links 103_1 to 103_N. Hereinafter, each of the leg links 103_1 to 103_N is generically referred to as an i-th leg link 103_i (i = 1, 2,..., N) or a leg link 103_i.

  In an arbitrary motion state of the moving body 101, the total floor reaction force (vector), which is the total floor reaction force acting on the moving body 101 from the floor surface, is ↑ FMt, and all of the translational force vectors of ↑ FMt are The translational floor reaction force is represented by ↑ Ft, and the total floor reaction force moment, which is the moment vector of ↑ FMt, is represented by ↑ Mt.

These total translational floor reaction force ↑ Ft and total floor reaction force moment ↑ Mt are each expressed as a three-component vertical vector viewed in an inertial coordinate system (a coordinate system fixed to the floor). The total floor reaction force ↑ FMt is expressed as a six-component vertical vector (= [↑ Ft, ↑ Mt] ^T ) in which the components ↑ Ft and ↑ Mt are arranged. The action point Pt of the total floor reaction force ↑ FMt is a point on the floor 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, the floor reaction force (hereinafter referred to as the leg floor reaction force) acting on the i-th leg link 103_i (i = 1, 2,..., N) from the floor is represented by ↑ FM_i, and the translational force vector of the ↑ FM_i. A translational floor reaction force is expressed as ↑ F_i, and a floor reaction force moment which is a moment vector of ↑ FM_i is expressed as ↑ M_i.

These translational floor reaction force ↑ F_i and floor reaction force moment ↑ M_i are respectively expressed as three-component vertical vectors viewed in the inertial coordinate system, as in the case of the total floor reaction force ↑ FMt. The i-th floor reaction force ↑ FM_i is expressed as a six-component vertical vector (= [↑ F_i, ↑ M_i] ^T ) in which the components ↑ F_i and ↑ M_i are arranged. In addition, the action point P_i of the i-th leg floor reaction force ↑ FM_i is a point on the floor surface in the ground contact surface of the i-th leg link 103_i (in the contact surface between the tip of the i-th leg link 103_i and the floor surface). To do.

  Supplementally, when the i-th leg link 103_i is in a non-grounding state (↑ FM_i = zero vector), the point of action P_i of the i-th floor reaction force ↑ FM_i does not exist. The action point P_i is an arbitrarily set point (for example, a representative point at the tip of the i-th leg link 103_i).

  At this time, the relationship between the total floor reaction force ↑ FMt and the leg floor reaction force ↑ FM__i (i = 1, 2,..., N) is generally expressed by the following equation (1).

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

Here, the floor reaction force ↑ ΔFM_i (= [↑ ΔF_i, ↑ ΔM_i] ^T ) for the perturbation is added to the action point P_i of the i-th floor reaction force ↑ FM_i, whereby the action of the total floor reaction force ↑ FMt. It is assumed that the floor reaction force ↑ ΔFMt (= [↑ ΔFt, ↑ ΔMt] ^T ) for the perturbation is added to the point Pt. The addition of the perturbation floor reaction force ↑ ΔFM_i to the action point P_i means that the floor reaction force acting on the action point P_i is changed from ↑ FM_i to ↑ FM_i + ↑ ΔFM_i. Similarly, adding the floor reaction force ↑ ΔFMt for the perturbation to the action point Pt means changing the floor reaction force acting on the action point Pt from ↑ FMt to ↑ FMt + ↑ ΔFMt.

  Hereinafter, ↑ ΔFM_i is referred to as the i-th leg perturbed floor reaction force, and the translation force vector ↑ ΔF_i and the moment vector ↑ ΔM_i of the ↑ ΔFM_i are referred to as the i-th leg perturbation translational floor reaction force and the i-th leg perturbation floor reaction force moment, respectively. Further, ↑ ΔFMt is called a perturbation total floor reaction force, and a translation force vector ↑ ΔFt and moment vector ↑ ΔMt of ↑ ΔFMt are called a perturbation total translation floor reaction force and a perturbation total floor reaction force moment, respectively.

  The relationship between the perturbation total floor reaction force ↑ ΔFMt and the i-th leg perturbation floor reaction force ↑ ΔFM_i (i = 1, 2,..., N) is expressed by the following equation (2 ).

AA_i (i = 1, 2,..., N) in the equation (2) is the same as that defined in the proviso of the equation (1).

  Further, it is assumed that the i-th leg perturbed floor reaction force ↑ ΔFM_i is generated by the spring displacement of the position and posture of the ground contact surface (hereinafter referred to as the i-th contact surface) of the i-th leg link 103_i. More specifically, the i-th leg perturbation translational floor reaction force ↑ ΔF_i is generated by the spring-like displacement (translational displacement) of the position of the i-th ground contact surface, and the spring-like displacement (rotational displacement) of the posture of the i-th ground contact surface. Suppose that the i-th perturbed floor reaction force moment ↑ M_i is generated. Note that the spring displacement of the position and posture of the i-th ground contact surface corresponds to translational displacement and rotational displacement due to elastic deformation of the floor at the i-th ground contact surface or elastic deformation of the tip of the i-th leg link 103_i, respectively. .

  The displacement amount of the position of the i-th leg ground contact surface due to this spring displacement (a vector of translational displacement amounts in the three-axis directions of the i-th contact surface). Vectors of rotational displacement amounts around the three axes of the contact surface (hereinafter referred to as spring rotational displacement amounts) are denoted as ↑ Xorg_i and ↑ Xrot_i, respectively. At this time, the relationship between ↑ Xorg_i and ↑ ΔF_i and the relationship between ↑ Xrot_i and ↑ ΔM_i are expressed by the following equations (3) and (4), respectively.

↑ ΔF_i = Korg_i ・ ↑ Xorg_i (3)
↑ ΔM_i = Krot_i ・ ↑ Xrot_i (4)

Korg_i in the equation (3) is a cubic diagonal matrix (hereinafter referred to as a translational spring constant matrix Korg_i) configured with a spring constant for each component of the springy translational displacement amount ↑ Xorg_i of the i-th contact surface as a diagonal component. Krot_i in equation (4) is a cubic diagonal matrix (hereinafter referred to as a rotary spring) having a spring constant for each component of the spring-like rotational displacement amount ↑ Xrot_i of the i-th leg ground contact surface as a diagonal component. Constant matrix Krot_i).

  Further, a representative contact surface is assumed as a single virtual contact surface that represents all contact surfaces between the moving body 101 and the floor surface (the contact surfaces of all the leg links 103 in a grounded state). It is assumed that the total floor reaction force ↑ FMt acts on the moving body 101 on the surface. The representative contact surface corresponds to a so-called support polygon.

  Then, it is assumed that the perturbation total floor reaction force ↑ ΔFMt is generated by the spring displacement of the position and posture of the representative contact surface in the same way as in the case of the i-th leg ground contact surface. More specifically, a perturbation total translation floor reaction force ↑ ΔFt is generated by the spring displacement (translation displacement) of the representative contact surface, and the perturbation total floor reaction is caused by the spring displacement (rotational displacement) of the posture of the representative contact surface. Assume that a force moment ↑ ΔMt is generated.

  Therefore, the amount of spring translational displacement at the position of the representative contact surface (vector of translational displacement in three axes due to springiness displacement) and the amount of posture springiness rotational displacement (vector of rotational displacement about three axes due to springiness displacement) ) Is represented by ↑ Xc_org and ↑ Xc_rot, respectively, and the relationship between ↑ Xc_org and ↑ ΔFt and the relationship between ↑ Xc_rot and ↑ ΔMt are expressed by the following equations (5) and (6), respectively. The Rukoto.

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

Kc_org in the equation (5) is a cubic diagonal matrix (hereinafter referred to as a translational spring constant matrix Kc_org) composed of the spring constant of each component of the springy translational displacement amount ↑ Xc_org of the representative contact surface as a diagonal component. Kc_rot in equation (6) is a third-order diagonal matrix (hereinafter referred to as a rotational spring constant matrix Kc_rot) composed of the spring constant of each component of the spring-like rotational displacement amount ↑ Xc_rot of the representative contact surface as a diagonal component. ).

  Supplementally, in the state where the single leg link 103_i of the moving body 101 is grounded, the representative contact surface is the ground contact surface (i-th leg ground contact surface) of the single leg link 103_i. It shall be matched. In this state, the spring displacement of the position and posture of the representative contact surface is the same as the spring displacement of the position and posture of the i-th contact surface, and the elastic deformation of the floor on the i-th contact surface or the second displacement. This corresponds to translational displacement and rotational displacement due to elastic deformation of the tip of the i-leg link 103_i.

  In the state where two or more leg links 103 of the moving body 101 are grounded, the representative contact surface is a single virtual grounding that combines the grounded portions of all the leg links 103 that are grounded. It has a meaning as a ground contact surface. The spring displacement of the position and posture of the representative contact surface in this state is the translational displacement and the rotational displacement due to the elastic deformation of the floor at the ground contact surface of the virtual ground contact portion or the elastic deformation of the virtual ground contact portion. Respectively.

  In the equation (2), the total floor reaction force ↑ by the i-th leg perturbation translational floor reaction force ↑ ΔF_i added to the action point P_i of the i-th floor reaction force ↑ FM_i (i = 1, 2,..., N), respectively. When attention is paid to the perturbation total translational floor reaction force ↑ ΔFt added to the action point Pt of FMt, the following equation (7) is obtained.

↑ ΔFt = ↑ ΔF_1 + ↑ ΔF_2 + …… + ↑ ΔF_N (7)

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

Kc_org ・ ↑ Xc_org = Korg_1 ・ ↑ Xorg_1 + Korg_2 ・ ↑ Xorg_2
＋ …… ＋ Korg_N ・ ↑ Xorg_N …… （8）

On the other hand, the action point Pt of the total floor reaction force ↑ FMt before the addition of the perturbation total floor reaction force ↑ ΔFMt is defined as the total floor reaction force central point (COP), and the i th leg perturbation floor reaction force ↑ ΔFM_i before the addition. When the action point P_i of the i-leg floor reaction force ↑ FM_i is the floor reaction force center point of the i-th leg ground contact surface, the position vector of the action point Pt with respect to an arbitrary reference point (this is expressed as ↑ Pt), the action The relationship between each position vector of points P_i (i = 1, 2,..., N) (this is expressed as ↑ P_i) is given by the following equation (9).

  The center point of the total floor reaction force is the point of action of the total floor reaction force ↑ FMt, and the horizontal component (the component around the horizontal axis) of the total floor reaction force moment ↑ Mt around that point is zero. It is. Similarly, the floor reaction force central point of the i-th leg ground contact surface is the point of action of the i-th floor reaction force ↑ FM_i, and the horizontal component (horizontal axis component) of the floor reaction force moment ↑ M_i around that point. Is such that is zero.

↑ Pt = r_1 ・ ↑ P_1 + r_2 ・ ↑ P_2 + …… + r_N ・ ↑ P_N …… （9）

In this equation (9), r_i (i = 1, 2,..., N) is a weighting coefficient defined by r_i≡Fn_i / Fnt. Here, Fn_i (i = 1, 2,..., N) is an absolute value of the vertical drag component perpendicular to the floor surface of the i-th translational floor reaction force ↑ F_i (hereinafter referred to as floor vertical drag component). , Fnt is the absolute value of the vertical drag component perpendicular to the floor surface of the total translational floor reaction force ↑ Ft (floor surface vertical drag component), and Fnt = Fn_1 + Fn_2 +... + Fn_N. Accordingly, the weighting factor r_i is the ratio of the floor surface vertical force component Fn_i of the i-th floor reaction force ↑ F_i to the floor surface vertical force component Fnt of the total translational floor reaction force ↑ Ft, and its value is 0 ≦ r_i. It is a value within the range of ≦ 1.

  In the present specification, Fn_i and Fnt are assumed to coincide with or substantially coincide with the vertical component (Z-axis component) of ↑ F_i and the vertical component (Z-axis component) of ↑ Ft, respectively.

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

↑ Xc_org = r_1 ・ ↑ Xorg_1 + r_2 ・ ↑ Xorg_2 + …… + r_N ・ ↑ Xorg_N (10)

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

Korg_i = r_i ・ Kc_org (11)

In the present invention, the above expression (10) is calculated between the springy translational displacement amount ↑ Xc_org of the representative contact surface and the springy translational displacement amount ↑ Xorg_i of the i-th leg ground contact surface (i = 1, 2,..., N). (11), which represents the relationship between the representative contact surface and the translation spring constant matrix Korg_i of the i-th contact surface (i = 1, 2,..., N). The basic formula.

  Therefore, each diagonal component of the translation spring constant matrix Korg_i of the i-th leg contact surface (spring constant related to translational displacement in each of the three axes) is proportional to the weighting factor r_i, and the larger the value of the weighting factor r_i (“ 1)), that is, the larger the ratio of the floor vertical drag component Fn_i of the i-th translational floor reaction force ↑ F_i to the floor vertical drag component Fnt of the total translational floor reaction force ↑ Ft, the larger it becomes. Is done. In other words, the greater the ratio of Fn_i to Fnt, the higher the sensitivity of the change in the springy translational displacement amount ↑ Xorg_i of the i-th contact surface with respect to the required i-th leg perturbation translational floor reaction force.

  Further, when only the single i-th leg link 103_i is grounded (when r_i = 1), the i-th leg link corresponds to the fact that the i-th leg ground contact surface matches the representative contact surface. The translation spring constant matrix Korg_i of the ground contact surface and the translation spring constant matrix Kc_org of the representative contact surface coincide with each other. Further, in this case, since ↑ ΔF_i = ↑ ΔFt, the springy translational displacement amount ↑ Xorg_i of the i-th leg ground contact surface and the springy translational displacement amount ↑ Xc_org of the representative contact surface coincide with each other.

  When two or more leg links 103 are in contact with the ground, the spring-like translational displacement amount ↑ Xc_org of the representative contact surface is the i-th leg contact corresponding to each leg link 103_i excluding the non-grounded leg link. The weighted average value of the spring-like translational displacement amount ↑ Xorg_i of the ground (weighted average value using r_i as a weighting factor).

  Next, in the above formula (2), the i-th floor reaction force ↑ F_1 to ↑ F_N of each of the first to N-th floor reaction forces ↑ FM_1 to ↑ FM_N is held constant. ↑ Perturbation whole bed added to action point Pt of total floor reaction force ↑ FMt by adding i-th leg perturbation floor reaction force moment ↑ ΔM_i to action point P_i of FM_i (i = 1, 2,..., N) Focusing on the reaction force moment ↑ ΔMt, the following equation (12) is obtained.

↑ ΔMt = ↑ ΔM_1 + ↑ ΔM_2 + …… + ↑ ΔM_N (12)

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

Kc_rot ・ ↑ Xc_rot = Krot_1 ・ ↑ Xrot_1 + Krot_2 ・ ↑ Xrot_2
+ …… + Krot_N ・ ↑ Xrot_N (13)

On the other hand, when the action point P_i of the i-th floor reaction force ↑ FM_i is the center point of the floor reaction force of the i-th ground contact surface, each of the first to Nth floor reaction forces ↑ FM_1 to ↑ FM_N Adding the i-th perturbed floor reaction force moment ↑ ΔM_i to the action point P_i (floor reaction force central point) of the i-th floor reaction force ↑ FM_i with the forces ↑ F_1 to ↑ F_N held constant This is equivalent to shifting the horizontal position of the floor reaction force center point on the i-th ground contact surface from the point P_i. Similarly, when the action point Pt of the total floor reaction force ↑ FMt is defined as the total floor reaction force central point (COP), the perturbation total floor reaction force moment ↑ ΔMt is added to the action point Pt of the total floor reaction force ↑ FMt. This is equivalent to the fact that the horizontal position of the center point of the total floor reaction force deviates from the point Pt.

  In this case, the horizontal position displacement amount (two component displacement vector) of the floor reaction force center point on the i-th leg contact surface is ↑ ΔRpt_i, and the horizontal position displacement amount of all floor reaction force center points (two components). ↑ ΔCOP is the horizontal axis component of ↑ ΔCOP · Fnt≈ ↑ ΔMt (X-axis and Y-axis component), and ↑ ΔRpt_i · Fn_i≈ ↑ ΔM_i is the horizontal axis component (X-axis and Y-axis). Component around the axis). Therefore, the following equation (14) is obtained from the above equation (12).

↑ ΔCOP = r_1 ・ ↑ ΔRpt_1 + r_2 ・ ↑ ΔRpt_2
+ …… + r_N ・ ↑ ΔRpt_N ...... (14)

In the equation (14), r_i (i = 1, 2,..., N) is the above-described weight coefficient r_i (≡Fn_i / Fnt).

  In addition, since the first to Nth leg ground contact surfaces are portions on the common floor surface, when the normal drag Fn_i acting on each of the first to Nth leg ground contact surfaces is the same, the i th leg contact is made. Perturbation floor reaction force moment (= ↑ ΔRpt_i · Fn_i) corresponding to the displacement amount ↑ ΔRpt_i of the horizontal position of the floor reaction force center point of the ground and the horizontal displacement amount ↑ Xrot_i due to the spring displacement of the i-th leg contact surface It can be considered that the relationship between the components around the axis is the same for any i-th leg contact surface.

  Therefore, when the floor surface normal force component Fn_i acting on each of the first to Nth leg ground contact surfaces has the same value (this is referred to as Fna), it is considered that the following equation (15) is established.

↑ ΔRpt_i ・ Fna = Krot ・ ↑ Xrot_i_xy ...... (15)

↑ Xrot_i_xy is a component around the horizontal axis (X-axis and Y-axis component) of the spring-like rotational displacement amount ↑ Xrot_i of the i th leg contact surface, and Krot is a quadratic diagonal matrix (X-axis and (Spring constant matrix related to rotation around the Y axis).

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

↑ ΔCOP ・ Fna = Krot ・ ↑ Xc_rot_xy …… (16)

Note that ↑ Xc_rot_xy is a component around the horizontal axis (component around the X axis and Y axis) of the spring-like rotational displacement amount ↑ Xc_rot of the representative contact surface.

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

Krot ・ ↑ Xc_rot_xy = r_1 ・ Krot ・ ↑ Xrot_1_xy + r_2 ・ Krot ・ ↑ Xrot_2_xy
＋ …… ＋ r_N ・ Krot ・ ↑ Xrot_N_xy …… （17）

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

↑ Xc_rot_xy = r_1 ・ ↑ Xrot_1_xy + r_2 ・ ↑ Xrot_2_xy
＋ …… ＋ r_N ・ ↑ Xrot_N_xy …… （18）

Therefore, in this embodiment, between the spring-like rotational displacement amount ↑ Xc_org of the representative contact surface and the spring-like rotational displacement amount ↑ Xorg_i of the i-th ground contact surface, all of those components (three components) are described above. Assume that the same relationship as in equation (18) holds. That is, it is assumed that the following formula (19) is established.

↑ Xc_rot = r_1 ・ ↑ Xrot_1 + r_2 ・ ↑ Xrot_2
+ …… + r_N ・ ↑ Xrot_N (19)

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

Krot_i = r_i ・ Kc_rot (20)

In the present embodiment, the above equation (19) is obtained from the spring-like rotational displacement amount ↑ Xc_rot of the representative contact surface and the spring-like rotational displacement amount ↑ Xrot_i of the i-th leg ground contact surface (i = 1, 2,..., N). Equation (20) that expresses the relationship between the rotation spring constant matrix Kc_rot of the representative contact surface and the rotation spring constant matrix Krot_i of the i-th contact surface (i = 1, 2,..., N). It is a basic formula to represent.

  Accordingly, each diagonal component of the rotation spring constant matrix Krot_i of the i-th leg ground contact surface (spring constant relating to the respective rotational displacements around the three axes) is proportional to the weight coefficient r_i, and the larger the value of the weight coefficient r_i (“ 1)), that is, the larger the ratio of the floor vertical drag component Fn_i of the i-th translational floor reaction force ↑ F_i to the floor vertical drag component Fnt of the total translational floor reaction force ↑ Ft, the larger it becomes. Is done. In other words, the greater the ratio of Fn_i to Fnt, the higher the sensitivity of the change in the springy rotational displacement amount ↑ Xrot_i of the i-th contact surface with respect to the required i-th leg perturbed floor reaction force moment.

  Further, when only the single i-th leg link 100_i is grounded (when r_i = 1), the i-th leg link corresponds to the coincidence of the i-th leg ground contact surface and the representative contact surface. The rotation spring constant matrix Krot_i of the ground contact surface and the rotation spring constant matrix Kc_rot of the representative contact surface coincide with each other. Further, in this case, since ↑ ΔM_i = ↑ ΔMt, the spring-like rotational displacement amount ↑ Xrot_i of the i-th leg ground contact surface and the spring-like rotational displacement amount ↑ Xc_rot of the representative contact surface coincide with each other.

  When two or more leg links 103 are grounded, the spring-like rotational displacement amount ↑ Xc_rot of the representative contact surface is the i-th leg contact corresponding to each leg link 103_i excluding the non-grounded leg link. This is a weighted average value of the spring-like rotational displacement amount ↑ Xrot_i of the ground (weighted average value with r_i as a weighting factor).

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

  In addition, in the above equations (11) and (20), the action point Pt of the total floor reaction force ↑ FMt is the center point of the total floor reaction force, and the action point P_i of the i-th floor reaction force ↑ FM_i is the i-th leg ground contact surface. Since it is assumed that it is a floor reaction force center point, VV_i (i = 1, 2,..., N) in Equation (21) is the i-th leg ground contact surface with respect to the action point Pt as the total floor reaction force center point. When the position vector of the center point of the floor reaction force is again ↑ V_i, the matrix is defined as follows.

VV_i: VV_i · ↑ F_i = V_i x ↑ F_i matrix where ↑ V_i: Position vector of the floor reaction force center point of the i-th ground contact surface with respect to the center point of all floor reaction force

In this case, more specifically, the total floor reaction force central point is the total floor reaction force central point corresponding to the total floor reaction force ↑ FMt before the perturbation total floor reaction force ↑ ΔFMt is added. The floor reaction force central point of the i-leg ground contact surface is the floor reaction force central point corresponding to the i-th leg floor reaction force ↑ FM_i before the i-th perturbation floor reaction force ↑ ΔFM_i is added.

  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, the displacement vector (= [↑ Xc_org, ↑ Xc_rot] ^T ) consisting of the spring-like translational displacement amount ↑ Xc_org at the position of the representative contact surface and the spring-like rotational displacement amount ↑ Xc_rot of the posture ↑ Xc, i-th leg contact A displacement vector (= [↑ Xorg_i, ↑ Xrot_i] ^T ) composed of a springy translational displacement amount ↑ Xorg_i of the ground position and a springy rotational displacement amount ↑ Xrot_i of the posture is expressed as ↑ X_i. Hereinafter, ↑ Xc is referred to as springiness translation / rotation displacement amount of the representative contact surface, and ↑ X_i is referred to as springiness translation / rotation displacement amount of the i-th leg ground 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 of the representative contact surface is regarded as the displacement (time change rate) per unit time of the position and posture of the representative contact surface, the unit of the position and posture of the representative contact surface The Jacobian matrix that expresses the relationship between the amount of displacement per time and the amount of change per unit time (temporal rate of change) ↑ Δq of the generalized variable vector ↑ q is the relationship between ↑ Xc and ↑ Δq Is expressed as a matrix Jc 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 = Jc ・ ↑ Δq (23)

Similarly, when the amount of springy translational / rotational displacement ↑ X_i of the i-th leg ground contact surface is regarded as the displacement (time change rate) per unit time of the position and posture of the i-th contact surface, The Jacobian matrix that expresses the relationship between the amount of displacement per unit time of the position and posture of the general variable vector ↑ q per unit time (time change rate) ↑ Δq is ↑ X_i and ↑ The relationship between Δq is expressed as a matrix J_i expressed by the following equation (24).

↑ X_i = J_i ・ ↑ Δq (24)

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

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

Similarly, a Jacobian matrix that expresses the relationship between the displacement amount per unit time of the position of the i-th ground contact surface and ↑ Δq (the matrix that expresses the relationship between ↑ Xorg_i and ↑ Δq by the following equation (26a): ) Is Jorg_i, the Jacobian matrix (↑ Xrot_i and ↑ Δq is represented by the following equation (26b)) that expresses the relationship between the displacement amount per unit time of the posture of the i-th leg contact surface and ↑ Δq. When the matrix is expressed as Jrot_i, J_i = [Jorg_i, Jrot_i] ^T as shown in the equation (26c).

↑ Xorg_i = Jorg_i ・ ↑ Δq (26a)
↑ Xrot_i = Jrot_i ・ ↑ Δq (26b)
J_i = [Jorg_i, Jrot_i] ^T (26c)

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

Therefore, the Jacobian matrix Jc (hereinafter referred to as the representative contact surface Jacobian matrix Jc) relating to the displacement of the position and orientation of the representative contact surface is the Jacobian matrix J_i (i = 1, 2,... , N) can be determined based on the above equation (27).

  In this case, since the position and posture of the i-th leg contact surface are the position and posture of the tip of the i-th leg link 103_i, the Jacobian matrix J_i (hereinafter referred to as the leg link Jacobian matrix J_i) is, in other words, It is a Jacobian matrix expressing the relationship between the amount of change per unit time of the position and orientation of the tip of the i-leg link 103_i and ↑ Δq. Such a Jacobian matrix J_i 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.

On the other hand, when a certain value of perturbation total floor reaction force ↑ ΔFMt is given as a required operation amount for realizing the required state of the moving body 101, this ↑ ΔFMt is based on the above equations (5) and (6). Thus, it can be converted into the required value of the springy translational / rotational displacement amount ↑ Xc (= [↑ Xc_org, ↑ Xc_rot] ^T ) of the representative contact surface. This conversion is given by the following equation (28).

If the pseudo-inverse matrix of the representative contact surface Jacobian matrix Jc is set to Jc ⁻¹ , the perturbation whole floor can be calculated from the required value of the spring translational / rotational displacement amount of displacement ↑ Xc of the representative contact surface by the following equation (29). The correction amount of the displacement amount of each joint of the moving body 101 for realizing the required value of the reaction force ↑ ΔFMt can be determined.

↑ Δq = Jc ⁻¹・ ↑ Xc ...... Formula (29)

Therefore, when the required value of the perturbation total floor reaction force ↑ ΔFMt is given as the required operation amount for controlling the total floor reaction force acting on the moving body 101 in an arbitrary motion state of the moving body 101, the above formula Based on (27), a representative contact surface Jacobian matrix Jc is obtained, and a pseudo inverse matrix Jc ^-1 of this Jc is determined. Further, the displacement vector ↑ Xc of the representative contact surface corresponding to the required value of ↑ ΔFMt is obtained based on the equation (28). Then, from this ↑ Xc and Jc− ¹ , the correction amount of the displacement amount of each joint for realizing the required value of the perturbation total floor reaction force ↑ ΔFMt can be determined by the equation (29).

  As a result, the displacement amount of each joint can be determined collectively without determining the correction amount itself of the position and posture of the tip portion of each leg link 103_i.

  Next, the perturbation total floor reaction force ↑ ΔFMt so that the total floor reaction force actually acting on the moving body 101 follows the target total floor reaction force for realizing the target motion of the moving body 101. It is assumed that the required value is determined. In this case, the required value of ↑ ΔFMt is determined as an integral of the deviation between the actual observed value of the total floor reaction force and the target total floor reaction force, or at least an integration obtained by integrating the deviation. It is determined by combining a term and a proportional term proportional to the deviation.

  In this case, an integral of the deviation between the actual observed value of the total floor reaction force and the desired total floor reaction force (hereinafter referred to as ↑ ΔFMt_int) is assumed as the target motion of the moving body 101. This corresponds to a steady difference between the position and posture of the assumed floor surface and the actual position and posture of the floor surface. Therefore, ↑ Xc calculated by converting the integral value ↑ ΔFMt_int of the deviation by the above equation (28), that is, ↑ Xc_int calculated by the following equation (28-1) from ↑ ΔFMt_int is assumed This is considered to correspond to a steady error between the position and posture of the floor surface and the actual position and posture of the floor surface.

In Equation (28-1), the translational displacement component and the rotational displacement component of ↑ Xc_int are represented as ↑ Xc_org_int and ↑ Xc_rot_int, respectively, and the translational force component and moment component of ↑ ΔFMt_int are represented by They are written as ↑ ΔFt_int and ↑ ΔMt_int, respectively.

  Therefore, the actual position and orientation of the floor surface can be estimated by correcting the position and orientation of the assumed floor surface with the above-mentioned ↑ Xc_int.

  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.

A first aspect of the present invention includes a base body, a plurality of leg links connected to the base body, and a joint actuator that drives a joint of each leg link, and moves on the floor surface by the movement of the plurality of leg links. The movement of the moving body is controlled according to the target motion of the moving body and the target total floor reaction force that is a target value of the total floor reaction force to be applied to the moving body in order to realize the target motion. A floor surface estimation device for estimating a position and posture of an actual floor surface to which the mobile body moves in a control device for a mobile body,
The value obtained by integrating the deviation between the observed value of the total floor reaction force actually acting on the moving body and the target total floor reaction force is additionally applied to the moving body in order to bring the deviation close to zero. A total floor reaction force request correction amount determining means for determining a total floor reaction force request correction amount that is a correction amount of the total floor reaction force to be caused;
The total floor reaction force required correction amount is generated by a spring displacement of the position and posture of the representative contact surface as a single virtual contact surface representing all the contact surfaces of the movable body and the floor surface. Assuming that the position and orientation of the representative contact surface corresponding to the total floor reaction force required correction amount are determined from the determined total floor reaction force required correction amount and a predetermined spring constant of the representative contact surface. Representative contact surface position / posture displacement amount calculating means for calculating the displacement amount;
Between the temporal change rate of the position and posture of the representative contact surface, and the temporal change rate of the generalized variable vector composed of the position and posture of the base body and the displacement amount of each joint of the moving body as components. The representative contact surface Jacobian matrix Jc, which is a Jacobian matrix representing the relationship of the above, is obtained by changing the temporal change rate of the position of the tip portion of each leg link or the temporal change rate of the position and posture of the tip portion of each leg link and the generalized variable. Each floor link Jacobian matrix J_i, which is a Jacobian matrix representing the relationship between the temporal change rate of the vector, the spring constant, and the total floor reaction as the action point of the total floor reaction force that actually acts on the moving body The representative contact surface calculated by the above equation (27) from the relative position of the actual floor reaction force center point of the tip of each leg link with respect to the force center point and the value of the floor reaction force actually acting on each leg link A Jacobian matrix calculating means;
The calculated required displacement amount of the position and orientation of the representative contact surface is multiplied by the pseudo inverse matrix Jc ⁻¹ of the calculated representative contact surface Jacobian matrix Jc to obtain the required displacement amount of the position and orientation of the representative contact surface. 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 realizing
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 joint displacement control means for controlling the joint actuator,
The actual floor surface is corrected by correcting the position and posture of the assumed floor surface, which is the floor surface assumed in the target motion, according to the required displacement amount calculated by the representative contact surface position / posture displacement amount calculating means. The position and orientation of the head are estimated (first invention).

Alternatively, a second aspect of the present invention includes a base, a plurality of leg links coupled to the base, and a joint actuator that drives a joint of each leg link, and the floor link is moved by the movement of the plurality of leg links. In accordance with a target motion of the mobile body and a target total floor reaction force that is a target value of the total floor reaction force to be applied to the mobile body in order to realize the target motion. A floor surface estimation device for estimating a position and posture of an actual floor surface on which the mobile body moves in a control device for the mobile body to be controlled,
A correction amount of the total floor reaction force to be additionally applied to the moving body in order to bring the deviation between the observed value of the total floor reaction force actually acting on the moving body and the target total floor reaction force close to zero. A total floor reaction force request correction amount determining means for determining a total floor reaction force request correction amount by combining at least a proportional term proportional to the deviation and an integral term obtained by integrating the deviation;
The total floor reaction force required correction amount is generated by a spring displacement of the position and posture of the representative contact surface as a single virtual contact surface representing all the contact surfaces of the movable body and the floor surface. Assuming that the position and orientation of the representative contact surface corresponding to the total floor reaction force required correction amount are determined from the determined total floor reaction force required correction amount and a predetermined spring constant of the representative contact surface. Representative contact surface position / posture displacement amount calculating means for calculating the displacement amount;
Between the temporal change rate of the position and posture of the representative contact surface, and the temporal change rate of the generalized variable vector composed of the position and posture of the base body and the displacement amount of each joint of the moving body as components. The representative contact surface Jacobian matrix Jc, which is a Jacobian matrix representing the relationship of the above, is obtained by changing the temporal change rate of the position of the tip portion of each leg link or the temporal change rate of the position and posture of the tip portion of each leg link and the generalized variable. Each floor link Jacobian matrix J_i, which is a Jacobian matrix representing the relationship between the temporal change rate of the vector, the spring constant, and the total floor reaction as the action point of the total floor reaction force that actually acts on the moving body The representative contact surface calculated by the above equation (27) from the relative position of the actual floor reaction force center point of the tip of each leg link with respect to the force center point and the value of the floor reaction force actually acting on each leg link A Jacobian matrix calculating means;
The calculated required displacement amount of the position and orientation of the representative contact surface is multiplied by the pseudo inverse matrix Jc ⁻¹ of the calculated representative contact surface Jacobian matrix Jc to obtain the required displacement amount of the position and orientation of the representative contact surface. 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 realizing
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. Joint displacement control means for controlling the joint actuator;
A representative contact surface steady displacement amount that is a displacement amount of the position and orientation of the representative contact surface corresponding to the integral term from the integral term of the total floor reaction force required correction amount and the spring constant of the representative contact surface. A representative contact surface steady-state displacement amount calculating means for calculating
By correcting the position and orientation of the assumed floor surface, which is the floor surface assumed in the target motion, according to the representative contact surface steady displacement amount calculated by the representative contact surface steady displacement amount calculating means, The position and orientation of the floor surface are estimated (second 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 “floor surface” is not limited to a normal indoor floor surface, but may be an outdoor ground surface or a road surface. Further, the assumed floor surface or the actual position and posture of the floor surface means the position and posture of the ground contact surface at the front end of each leg link of the movable body or the floor surface in the vicinity thereof.

  According to the first and second aspects of the present invention, the observed value of the total floor reaction force actually acting on the moving body (hereinafter, sometimes referred to as the actual total floor reaction force) and the target total floor reaction force The total floor reaction force request correction amount as a feedback operation amount (control input) for bringing the deviation close to zero is determined according to the deviation.

  In this case, in the first invention, a value (integral term) obtained by integrating the deviation is determined as the total floor reaction force required correction amount as it is. In the second invention, the total floor reaction force is synthesized by synthesizing at least a proportional term proportional to the deviation (a value obtained by multiplying the deviation by a gain of a predetermined value) and an integral obtained by integrating the deviation. The required correction amount is determined.

  The total floor reaction force requirement correction amount determined in this way corresponds to the required value of the perturbation total floor wave force ↑ ΔFMt in both the first and second inventions.

  In the second invention, the total floor reaction force requirement correction amount may be a combination of only the proportional term and the integral term, but by further synthesizing a term other than the proportional term and the integral term, for example, a differential term, The total floor reaction force request correction amount may be determined.

  In addition, as the target total floor reaction force in the first invention and the second invention, for example, on an assumed floor surface as an actual floor surface model, an appropriate dynamic model for a target motion of the moving body is used. Those created so as to establish a dynamic relationship can be adopted. Alternatively, a reference total floor reaction force created so that the above dynamic relationship is established with respect to the target motion of the moving body is used as a predetermined state quantity relating to the motion of the moving body (a specific part of the moving body (base, etc.)) Alternatively, the deviation may be corrected so as to be close to zero according to the deviation between the target value and the actual value of the state value such as the position, posture, or change rate of the center of gravity point).

  In the first and second inventions, the representative contact surface position and posture required displacement amount corresponding to the total floor reaction force required correction amount determined as described above is calculated by the representative contact surface position and posture displacement amount calculating means. .

  As a result, the total floor reaction force required correction amount as a feedback operation amount (control input) for causing the actual total floor reaction force to follow the target total floor reaction force is the position and orientation of the representative contact surface as a single virtual surface. Is converted to the required displacement amount.

  Here, the displacement amount of the position and orientation of the representative contact surface corresponds to the spring translational / rotational displacement amount ↑ Xc, and the amount of change ↑ Δq per unit time of the generalized variable vector ↑ q. Thus, the relationship of the above formula (23) expressed by the representative contact surface Jacobian matrix Jc is satisfied.

Therefore, by multiplying the required displacement amount of the representative contact surface corresponding to the total floor reaction force required correction amount by the pseudo inverse matrix Jc ⁻¹ of the representative contact surface Jacobian matrix Jc (ie, the equation (29)). Therefore, the joint displacement correction amount of the moving body for realizing the required displacement amount (and thus for realizing the total floor reaction force required correction amount) can be determined.

  Therefore, in the present invention, the representative contact surface Jacobian matrix Jc is calculated by the representative contact surface Jacobian matrix calculating means. In this case, the representative contact surface Jacobian matrix Jc represents the temporal change rate of the position of the tip portion of each leg link or the temporal change rate of the position and posture of the tip portion of each leg link and the temporal change of the generalized variable vector. Each link Jacobian matrix J_i (i = 1, 2,..., N), which is a Jacobian matrix representing the relationship between the ratio, the spring constant, and the action of the total floor reaction force that actually acts on the moving body From the relative position of the actual floor reaction force center point at the tip of each leg link with respect to the total floor reaction force center point as a point, and the value of the floor reaction force actually acting on each leg link, the above equation (27) Is calculated by

  It should be noted that the value of the floor reaction force actually acting on each leg link used for the calculation of Expression (27), the position of the floor reaction force action point, and the position of the total floor reaction force action point are Obviously, the observed value measured by the installed force sensor may be sufficient, but if the actual value can be approximated accurately, it can be approximated based on the target floor reaction force or an appropriate model. It may be an estimated or predicted value.

  Further, supplementing the above-mentioned leg link Jacobian matrix, in the present invention, each leg link has a structure that can change the floor reaction force moment acting on the tip portion with the tip portion grounded. (For example, a leg link in which the tip portion is configured by a foot portion whose posture can be changed by an actuator, and the contact between the tip portion and the floor surface is in a surface contact state) and the floor reaction acting on the tip portion. The leg link may have any structure such as a leg link having a structure in which the force moment cannot be changed (for example, a leg link in which a contact state between the tip portion and the floor surface is dotted).

  In this case, when each leg link has a structure that can change the floor reaction force moment at the tip, the leg link Jacobian matrix is the time of the position and posture of the tip of the leg link. A Jacobian matrix representing the relationship between the rate of change and the rate of change of the generalized variable vector over time. In addition, when each leg link has a structure that cannot change the floor reaction force moment at its tip, the leg link Jacobian matrix is the rate of temporal change in the position of the tip of the leg link. And a Jacobian matrix representing the relationship between the generalized variable vector and the temporal change rate.

  These leg link Jacobian matrices can be obtained by a known method based on the observed values of the actual displacement amount of each joint of the moving body.

In the present invention, the joint displacement correction amount determining means multiplies the required displacement amount of the position and orientation of the representative contact surface by the pseudo inverse matrix Jc ⁻¹ of the representative contact surface Jacobian matrix Jc calculated as described above. Thus, the joint displacement correction amount is determined. As a result, the joint displacement correction amount for realizing the total floor reaction force required correction amount 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.

  Thereby, the displacement amount of each joint of the moving body is controlled so that the actual total floor reaction force follows the target total floor reaction force, and so-called compliance control is realized.

  According to the first invention and the second invention, the total floor reaction force required correction amount is converted into the required displacement amount of the position and orientation of the representative contact surface, and the joint displacement of each joint of the moving body according to the required displacement amount. By calculating the amount of correction, the position of the tip of each leg link can be determined in consideration of the relationship between the correction of the position and posture of each leg link and the change in the actual total floor reaction force and their mutual relationship. The amount of correction of each joint displacement (joint displacement correction amount) to make the actual total floor reaction force follow the target total floor reaction force without executing a process that determines the posture correction amount. Can be determined collectively. 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 of each leg link in the equation (27) is set to a larger value (a value closer to “1”) as the leg link has a relatively large vertical reaction force component. Therefore, as a result, the leg link with respect to the total floor reaction force required correction amount becomes larger as the leg link having a relatively large vertical reaction force component of the floor reaction force. Therefore, a joint that can surely achieve the required amount of floor reaction force correction without unnecessarily correcting the position and posture of the tip of the leg link where the vertical reaction force component of the floor reaction force is relatively small. The amount of displacement correction can be determined.

  Further, since the weight coefficient r_i of each leg link changes continuously, the representative contact surface Jacobian matrix Jc does not change discontinuously. 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 invention and the second invention, the movement of the moving body is controlled so that the total floor reaction force actually acting on the moving body follows the target total floor reaction force. It is possible to appropriately perform the processing without determining the amount of correction of the position and posture of the tip from the target motion.

  On the other hand, in the first invention, in the state where the compliance control of the moving body is performed as described above, the total floor reaction force required correction amount determined by the total floor reaction force required correction amount determining means is the actual total floor reaction force. Since it is a value (integral term) obtained by integrating the deviation between the observed force value and the desired total floor reaction force, the representative contact surface position / posture displacement is calculated from the total floor reaction force required correction amount and the spring constant. The required displacement amount of the position and orientation of the representative contact surface calculated by the amount calculation means is calculated based on the assumed floor surface position and orientation that are assumed in the target motion, and the actual floor surface position and orientation. It corresponds to a steady difference between the two. That is, the required displacement amount corresponds to ↑ Xc_int in the equation (28-1).

  In the second invention, in the state where the compliance control of the moving body is performed as described above, the integral term of the total floor reaction force request correction amount determined by the total floor reaction force request correction amount determination means. And the spring constant, the displacement of the position and orientation of the representative contact surface calculated by the same calculation as the representative contact surface position / posture displacement amount calculating means (instead of the total floor reaction force request correction amount, the integral term is The amount of displacement of the representative contact surface position and posture calculated using the above-mentioned is equivalent to a steady difference between the assumed floor surface position and posture and the actual floor surface position and posture, that is, This corresponds to ↑ Xc_int in the equation (28-1).

  Therefore, in the first invention, the position and posture obtained by correcting the required displacement amount calculated by the representative contact surface position and posture displacement amount calculating means from the position and posture of the assumed floor surface are changed to those of the actual floor surface. Estimated as position and orientation.

  In the second aspect of the invention, the representative contact surface steady displacement calculating means corresponds to the integral term from the integral term of the total floor reaction force required correction amount and the spring constant of the representative contact surface. A representative contact surface steady displacement amount that is a displacement amount of the position and orientation of the representative contact surface is calculated. Then, the position and posture obtained by correcting the representative contact surface steady displacement amount calculated by the representative contact surface steady displacement amount calculation means from the assumed floor surface position and posture are set as the actual floor surface position and posture. presume.

  Thereby, according to 1st invention and 2nd invention, the position and attitude | position of an actual floor surface can be estimated. In this case, it is possible to determine the joint displacement correction amount that can surely realize the total floor reaction force required correction amount by the compliance control, and to control the operation of the moving body. The reaction force requirement correction amount, or the representative contact surface steady displacement amount in the second invention is a reliability that corresponds to a steady difference between the assumed floor surface position and posture and the actual floor surface position and posture. It becomes a thing with high property. Therefore, the actual position and orientation of the floor surface can be estimated with high accuracy.

  Therefore, according to the first invention and the second invention, the movement of the moving body is controlled so that the total floor reaction force actually acting on the moving body follows the target total floor reaction force. It is possible to properly estimate the actual floor surface position and posture of the moving body's operating environment while properly performing the process without determining the amount of correction from the target movement of the position and posture of each part. it can.

  In the second invention, since the total floor reaction force requirement correction amount is a combination of at least the proportional term and the integral term, the position and posture of the assumed floor surface are steady with respect to the actual floor surface. Temporary observation value of actual total floor reaction force and target total floor reaction force caused by local unevenness of actual floor surface that is not assumed on the assumed floor surface as well as the effect of errors By compensating for the influence of the deviation, the joint displacement correction amount for causing the actual total floor reaction force to follow the target total floor reaction force can be determined.

In the first invention or the second invention, the pseudo inverse matrix Jc ⁻¹ of the representative contact surface Jacobian matrix Jc can be calculated by a known appropriate method.

However, the pseudo inverse matrix Jc ⁻¹ of the calculated representative contact surface Jacobian matrix is a matrix calculated by the following equation (30) from the preset weight matrix W and the calculated representative contact surface Jacobian matrix Jc. And a pseudo inverse matrix operation for determining the value (real value) of k in the equation (28) so that the determinant DET represented by the following equation (31) is equal to or greater than a predetermined positive threshold value. Parameter setting means,

^{Jc -1 = W -1 · Jc T} · (Jc · W -1 · Jc T + k · I) -1 ...... (30)
DET = det (Jc ・ W ⁻¹・ Jc ^T + k ・ I) (31)
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 to be used for calculating the pseudo inverse matrix by the equation (30), 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 (third invention).

  In Equation (30), 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 (30), that is, the determinant DET shown in Expression (31) from becoming too small. A real value greater than or equal to zero. Supplementing the adjustment parameter k, the pseudo inverse matrix Jc ⁻¹ can basically be calculated by the above equation (30) 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 parentheses on the right side of Expression (30) diverges, making it impossible to determine an appropriate pseudo inverse matrix Jc- ¹ . In order to prevent this, in Equation (30), 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 a value equal to or greater than the predetermined threshold, and the provisional value of k when the determination result is affirmative is expressed as the equation (30). 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.

Therefore, in the third invention, in the processing of the pseudo inverse matrix calculation parameter determining means, a matrix calculated by using the provisional value before the increase amount of the provisional value of k when the determination result is negative. 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 third 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 third aspect of the present 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 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 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.

  With reference to FIG. 2, the mobile body 1 illustrated in this embodiment is, for example, a legged mobile robot including two leg links. The moving body 1 includes an upper body 2 as a base, a pair of left and right leg links 3R and 3L, a pair of left and right arm links 4R and 4L, and a head 5.

  In the description of the present embodiment, a symbol “R” is added to the variable indicating the member on the right side or the amount related to the member toward the front of the moving body 1, and the member on the left side toward the front of the robot 1 or A symbol “L” is added to a variable indicating an amount related to the member. However, when there is no need to distinguish between the right side and the left side, the symbols “R” and “L” may be omitted.

  The pair of left and right leg links 3R, 3L have the same structure. Specifically, each leg link 3 includes a thigh 14 connected to the upper body 2 via a hip joint 13, and a crus 16 connected to the thigh 14 via a knee joint 15. And a foot 18 connected to the lower leg 16 via an ankle joint 17 as a plurality of element links constituting the leg link 3.

  An elastic member 18 a is attached to the bottom surface of the foot 18 that is the tip of each leg link 3. When the movable body 1 moves on the floor surface, each foot 18 is grounded (landed) on the floor surface via its elastic member 18a.

  The hip joint portion 13 of each leg link 3 has a degree of freedom of rotation around each of the yaw axis (Z axis in FIG. 2), pitch axis (Y axis in FIG. 2), and roll axis (X axis in FIG. 1). It is composed of three joints 19, 20, and 21. The knee joint portion 15 is constituted by a joint 22 having a degree of freedom of rotation around the pitch axis. The ankle joint portion 17 includes two joints 23 and 24 each having a degree of freedom of rotation about the pitch axis and the roll axis.

  Therefore, each leg link 3 has 6 degrees of freedom of movement with respect to the upper body 2 (base body) in this embodiment. Note that the rotation axes (roll axis, pitch axis, yaw axis) of the joints 19 to 24 of each leg link 3 in the above description indicate the rotation axes in a state where the leg link 3 extends in the vertical direction. .

  The pair of left and right arm links 4R and 4L have the same structure. Specifically, each arm link 4 includes an upper arm portion 26 connected to the upper body 2 via a shoulder joint portion 25, a forearm portion 28 connected to the upper arm portion 26 via an elbow joint portion 27, A hand 30 connected to the forearm portion 28 via a wrist joint portion 29 is provided as a plurality of element links constituting the arm link 4.

  In this case, the shoulder joint portion 25 includes three joints 31, 32, and 33 each having a degree of freedom of rotation around each of the pitch axis, the roll axis, and the yaw axis. Further, the elbow joint 27 is constituted by a joint 34 having a degree of freedom of rotation around the pitch axis. The wrist joint portion 29 includes three joints 35, 36, and 37 each having a degree of freedom of rotation around each of the yaw axis, pitch axis, and roll axis.

  Accordingly, each arm link 4 has seven degrees of freedom of movement with respect to the upper body 2 in this embodiment. In the above description, the rotation axes (roll axis, pitch axis, yaw axis) of the joints 31 to 37 of each arm link 4 indicate the rotation axis in a state where the arm link 4 extends in the vertical direction. .

  The head 5 is disposed above the upper body 2 and is connected to the upper body 2 via the neck joint 38. In this case, the neck joint portion 38 includes two joints 39 and 40 each having a degree of freedom of rotation around the pitch axis and the yaw axis. Therefore, the head 5 has two degrees of freedom of movement with respect to the upper body 6. Note that the rotation axes (pitch axis, yaw axis) of the joints 31 and 37 are rotation axes in a state where the neck joint portion 38 is extended in the vertical direction.

  Although not shown in FIG. 2, a plurality of electric motors 41 (shown in FIG. 3) are mounted on the moving body 1 as joint actuators that drive the joints described above. Each electric motor 41 is connected to a corresponding joint 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-described configuration, the six joints 19 to 24 of each leg link 3 are driven by the electric motor 41, so that the spatial movement of each leg link 3 is performed. This movement enables the moving body 1 to move on the floor. For example, the walking motion of the mobile body 1 can be performed by exercising the leg links 3R and 3L in the same form (gait) as a human walking motion.

  Supplementally, the moving body 1 of the present embodiment is a humanoid robot having the arm links 4R, 4L and the head 5, but even if the arm link 4R, 4L or the head 5 is not provided. Good. Further, for example, the upper body 2 may be composed of a lower upper body and an upper upper body, and these 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 a sensor, as shown in FIG. 2, the posture angle (inclination angle with respect to the vertical direction and the azimuth angle around the yaw axis) of the upper body 2 that is the base of the moving body 1 and its temporal change rate (angular velocity). And the ankle joint 24 of each leg link 3 to measure the external force (floor reaction force) that each foot 18 receives when it touches the ground. The force sensor 52 (52R, 52L) interposed between the foot 18 and the wrist joint 29 and the hand 30 of each arm link 4 in order to measure the external force that each hand 30 receives from the contact target object. And a force sensor 53 (53R, 53L) interposed therebetween.

  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 of the force sensors 52 and 53 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.

  Note that in the walking motion of the moving body 1 mainly described in the present embodiment, the output of the force sensor 53 (53R, 53L) of each arm link 4 is not necessary. Therefore, these force sensors 53 may be omitted.

  Although not shown in FIG. 2, the moving body 1 includes, for example, a rotary encoder 54 (shown in FIG. 2) as a sensor for detecting the displacement amount (rotation angle) of each joint. In addition, as a sensor which detects the displacement amount (rotation angle) of each joint, you may use other sensors, such as a potentiometer.

  The output of each sensor is input to the control device 50. Then, the control device 50 recognizes the observation value of the movement state of the moving body 1 recognized from these input values (measured value of the posture angle of the body 2 and its temporal change rate (angular velocity), the moving speed of the body 2). , Measured values of floor reaction force acting on each foot 18, measured values of displacement of each joint and its rate of change over time, etc.) A target value (hereinafter referred to as a joint displacement command) for determining the displacement amount of each joint is determined, and the actual displacement amount (actual displacement amount) of each joint is determined via the electric motor 41 in accordance with the joint displacement command. Control. Further, in parallel with the operation control of the moving body 1, the control device 50 uses the parameter values calculated for the operation control to determine the actual floor surface position and posture of the moving body 1. The estimation process is also executed.

  As shown in FIG. 4, the control device 50 that executes such control processing includes, as shown in FIG. 4, a reference gait generation unit 61, a posture stabilization compensation force determination unit 62, and a desired total floor reaction. Force correction unit 63, total floor reaction 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, joint displacement control unit 69, and floor 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 floor reaction force correction unit 63, the total floor reaction force request correction amount determination unit 64, and the representative contact surface translation / rotation. By sequentially executing the processing of the displacement amount calculating unit 65, the representative contact surface Jacobian matrix calculating unit 66, the target joint displacement amount determining unit 67, the joint displacement correction amount determining unit 68, and the joint displacement control unit 69 at predetermined calculation processing cycles. The joint displacement command for each joint is sequentially determined, and the electric motor 41 is controlled via a motor drive circuit (not shown) in accordance with the joint displacement command.

  Further, the control device 50 estimates the actual position and posture of the floor surface on which the moving body 1 moves by executing the processing of the floor surface estimation unit 70 in parallel with the control processing described above.

  Below, the process of the control apparatus 50 including the detailed process of each said function part is demonstrated taking the case where the walking motion of the mobile body 1 is performed as an example.

[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 desired total floor reaction force which defines the trajectory of the total floor reaction force (total floor reaction force) to be applied to the movable body 1 from the floor surface. “Orbit” means a time series of instantaneous values.

  The target motion in the reference gait is assumed to be a floor model whose shape is determined in advance as matching the actual floor surface shape (position and posture of each part of the floor surface) of the moving environment of the moving body 1 It is generated so that the moving body 1 can be moved on the floor surface. In the present embodiment, the assumed floor surface used for creating the target motion is periodically determined according to the floor surface (estimated floor surface) estimated by the floor surface estimation unit 70 as representing the actual floor surface. (For example, every time when the mobile body 1 is moved, every step, or every predetermined time).

  In the moving body 1 of the present embodiment, this target motion is performed by using the desired foot position / posture trajectory that is the trajectory of the foot position 18 and the desired posture of each leg link 3 and the target position of the upper body 2 (base body). A target body position / posture trajectory that is a trajectory of the target posture, a target arm posture trajectory that is a trajectory of the overall relative target posture of each arm link 4 with respect to the upper body 2, and a relative relationship of the head 5 with respect to the upper body 2. And a target head posture trajectory that is a trajectory of a typical target posture.

  It should be noted that the “position” of the foot 18 is a position of a representative point of the foot 18 that is arbitrarily defined as representatively indicating the spatial position of the foot 18 (for example, the foot 18 It means the position of a specific point on the bottom surface). The same applies to the “position” of the upper body 2. Further, the “posture” of the foot 18 means the spatial orientation of the foot 18. The same applies to the “posture” of the upper body 2.

  The target foot position / posture and the target body position / posture are expressed as positions and postures viewed in a global coordinate system as an inertial coordinate system fixed to the floor surface. As this global coordinate system, for example, a point on the ground contact surface (contact surface with the floor) of one leg 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 set as an origin, A supporting leg coordinate system is used in which the horizontal axis in the front-rear direction of the foot 18 of the leg link 3 is the X axis, the vertical axis is the Z axis, and the horizontal axis perpendicular to these is the Y axis.

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

  In this embodiment, the reference gait generator 61 uses a known gait generation method, for example, a method proposed by the applicant of the present application in Japanese Patent No. 3726081 as a component of a target motion such as a desired foot position / posture trajectory. It is generated by the same method.

  Specifically, in the target motion for performing the walking motion of the moving body 1, the target arm posture trajectory and the target head posture trajectory are, for example, a target arm posture and a target head posture relative to the upper body 2. Is constantly maintained constant. Then, the desired foot position / posture trajectory and the desired body position / posture trajectory are generated, for example, by the method proposed by the present applicant in Japanese Patent No. 3726081 and the like.

  The generation processing of the desired foot position / posture trajectory and the desired body position / posture trajectory will be described in brief. The desired foot position / posture such as the expected landing position and the expected landing time of each foot portion 18 with respect to the assumed floor surface. A parameter that defines the trajectory is determined according to a request for a moving direction and a moving speed of the moving body 1 given to the control device 50 from the outside of the moving body 1 or a 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.

  Then, according to the desired foot position / posture trajectory defined by this parameter, the trajectory of the target ZMP which is the target position of the ZMP (Zero Moment Point) is determined. 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 desired foot position / posture trajectory, and is smooth (produces a step-like change). (Without any change).

  Furthermore, using an appropriate dynamic model that expresses 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 body so as to satisfy the target ZMP trajectory. A position and orientation trajectory is determined. Satisfying the target ZMP trajectory means that the horizontal component of the moment generated around the target ZMP (the X axis and Y axis) is the resultant force of the inertial force generated by the movement of the moving body 1 and the gravity acting on the moving body 1. This means that the component around the axis becomes zero.

  Supplementally, as long as the method for generating the target motion can generate the target motion that can be realized by the moving body 1 on the assumed floor surface, the method other than the method proposed by the applicant of the present application in Japanese Patent No. 3726081 or the like. It may be generated by a known method.

  The target arm posture and the target head posture relative to the upper body 2 may be determined so as to change in a desired form as necessary. For example, both arm links 4R and 4L may be swung back and forth in accordance with the walking motion of the moving body 1. In that case, the target body position is determined using a dynamic model that takes into account changes in the position of the overall center of gravity of the moving body 1 and changes in the angular momentum around the overall center of gravity associated with changes in the target arm posture and the target head posture. A posture trajectory may be generated.

  Further, the constituent elements of the target exercise are not limited to the above. For example, when the moving body 1 does not include the arm links 4R and 4L or the head 5, the target arm posture trajectory and the target head posture trajectory are unnecessary. In addition to the arm links 4R and 4L and the head 5, when the movable body 1 is provided with a portion that is movable with respect to the upper body 2, the trajectory of the target position or target posture of the portion is also the target motion. It is added as a component. For example, when each leg link 3 has a degree of freedom of 7 degrees or more, in addition to the desired foot position / posture trajectory, for example, the target position / posture trajectory of the intermediate portion of each leg link 3 is used as 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 floor reaction force in the reference gait needs to be applied to the moving body 1 dynamically from the assumed floor surface in order to move the moving body 1 on the assumed floor surface according to the target motion. It defines the trajectory of the total floor reaction force.

  The target total floor reaction force is calculated by calculating the trajectory of the target total floor reaction force central point, which is the target position of the total floor reaction force central point (COP) as the action point of the total floor reaction force, and the target total floor reaction force central point. Acting on the moving body 1 from the assumed floor surface around the center point of the target total floor reaction force And a trajectory of the desired floor reaction force moment, which is the target of the moment (vector) to be generated.

  In the following description, for the sake of convenience, “target total floor reaction force” means a set of target translational floor reaction force and target floor reaction force moment unless otherwise specified. Distinguish from points.

  The reference gait generator 61 determines the target total floor reaction force center point trajectory to coincide with the target ZMP trajectory. Further, the reference gait generator 61 generates a resultant force of the inertial force generated by the translational motion of the entire center of gravity of the moving body 1 in the target motion of the moving body 1 and the gravity acting on the moving body 1. The target translational floor reaction force trajectory is determined so as to balance with.

  In addition, the reference gait generator 61 generates an inertial force generated by the target motion of the moving body 1 (an inertial force due to a translational motion of the overall center of gravity and around the overall center of gravity). The target floor reaction force moment trajectory is determined so as to be balanced with the moment generated around the center point of the target total floor reaction force by the resultant force of the inertial force due to the rotational motion of the moving body 1 and the gravity acting on the moving body 1. In this case, the components around the horizontal axis (components around the X axis and the Y axis) of the desired floor reaction force moment are zero.

In the following description, the target translational floor reaction force (vector) of the target total floor reaction force of the standard gait is expressed as ↑ Ft_cmd0, and the target floor reaction force moment (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 sum of ↑ Ft_cmd0 and ↑ Mt_cmd0 is expressed as target total floor reaction force ↑ 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.

  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 inputs the target motion of the reference gait generated as described above to the target joint displacement amount determination unit 67, and executes the processing of the target joint displacement amount determination unit 67. 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 performs the posture stabilization compensation force determination unit 62, the target total floor reaction force correction unit 63, and the total floor reaction force. The processing of the required correction amount determination unit 64, the representative contact surface position / posture displacement amount calculation unit 65, the representative contact surface Jacobian matrix calculation unit 66, and the joint displacement correction amount determination unit 68 is 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 power determination unit 62 is designed to bring the deviation between the target body posture of the reference gait and the observed value of the actual posture (actual body posture) of the upper body 2 of the moving body 1 close to zero. This is a functional unit that determines the posture stabilization compensation force ↑ Mdmd, which is a perturbed floor reaction force to be additionally applied to the moving body 1, as a correction amount for correcting the target total floor reaction force of the reference gait.

  In the present embodiment, the posture stabilization compensation force ↑ Mdmd is a perturbation moment around the horizontal axis (around the X axis and around the Y axis) added around the center point of the desired total floor reaction force of the standard gait. Accordingly, the posture stabilization compensation force ↑ Mdmd is an operation amount for correcting the component around the horizontal axis of the desired floor reaction force moment among the desired total floor reaction force of the reference gait. In other words, the posture stabilization compensation force ↑ Mdmd is an operation amount having a function of shifting the target of the total floor reaction force central point from the target total floor reaction force central point of the standard gait.

  Then, the posture stabilization compensation force determination unit 62 determines the posture stabilization compensation force ↑ Mdmd between the observed value of the actual body posture indicated by the output of the posture sensor 51 and the target body posture of the reference gait. From a predetermined feedback control law. As the feedback control law, for example, the PD law is used.

  That is, in this embodiment, a proportional term obtained by multiplying the deviation between the observed value of the actual body posture and the target body posture by a gain of a predetermined value, and a derivative obtained by multiplying the temporal change rate of the deviation by the gain of the predetermined value. The posture stabilization compensation force (moment) ↑ Mdmd is determined by adding the term. In this case, the component around the X axis of ↑ Mdmd is determined according to the component around the X axis of the deviation, and the component around the Y axis of ↑ Mdmd is determined according to the component around the Y axis of the deviation.

  The deviation between the observed value of the actual body posture and the target body posture is substantially proportional to the horizontal position deviation between the actual overall center of gravity of the moving body 1 and the overall center of gravity in the target motion of the reference gait. . Accordingly, the deviation between the observed value of the actual horizontal center of gravity of the moving body 1 and the horizontal position of the entire center of gravity of the moving body 1 in the target motion of the reference gait is calculated from the target body posture of the actual body posture. It may be used as an index representing the degree of deviation, and the posture stabilization compensation force (moment) ↑ Mdmd may be determined by a feedback control law such as the PD law according to this deviation. In this case, the actual position of the overall center of gravity of the moving body 1 is estimated using, for example, the observed value (measured value) of the actual displacement amount of each joint of the moving body 1 and the rigid link model of the moving body 1. Is possible.

  Supplementally, in the present embodiment, there are two components of the ↑ Mdmd component around the X axis and the component around the Y axis, but a component around the Z axis may be further included. In this case, for example, according to a feedback control law such as a PD law according to the deviation between the Z-axis component of the target body posture of the reference gait and the Z-axis component of the observed value of the actual body posture. , ↑ Mdmd around the Z-axis may be determined.

[Processing of the target total floor reaction force correcting unit 63]
Next, the control device 50 executes processing of the target total floor reaction force correction unit 63. The target total floor reaction force correcting unit 63 is a functional unit that corrects the target total floor reaction force ↑ FMt_cmd0 of the reference gait by the posture stabilization compensation force ↑ Mdmd determined as described above. The target total floor reaction force correcting unit 63 determines the corrected target floor reaction force by adding the posture stabilization compensation force ↑ Mdmd determined as described above to the target floor reaction force of the reference gait. In this case, the corrected target floor reaction force is obtained by using only the component around the X axis and the component around the Y axis of the target floor reaction force moment around the center point of the target total floor reaction force of the reference gait. It is a correction from the power.

Hereinafter, the translational floor reaction force (three-component vertical vector) and floor reaction force moment (three-component vertical vector) of the corrected target floor reaction force are expressed as ↑ Ft_cmd1 and ↑ Mt_cmd1, respectively, and these are combined. The component vertical vector (= [↑ Ft_cmd1, ↑ Mt_cmd1] ^T ) is expressed as a corrected target floor reaction force ↑ FMt_cmd1. In this case, ↑ Ft_cmd1 = ↑ Ft_cmd0. Further, ↑ Mt_cmd1 = ↑ Mt_cmd0 + ↑ Mdmd (however, in this embodiment, the component around the Z-axis of ↑ Mdmd is zero).

[Processing of total floor reaction force request correction amount determination unit 64]
Next, the control device 50 executes processing of the total floor reaction force request correction amount determination unit 64. The total floor reaction force required correction amount determination unit 64 uses the corrected target floor reaction force ↑ FMt_cmd1 determined as described above to observe the actual total floor reaction force (actual total floor reaction force) acting on the moving body 1. Is a functional unit that determines the required correction amount of the total floor reaction force so as to follow.

The required correction amount of the total floor reaction force corresponds to the required value of the perturbation total floor reaction force ↑ ΔFMt in the above formula (2), and the required correction amount of the translational floor reaction force out of the total floor reaction force ( Perturbation total translational floor reaction force as a three-component longitudinal vector) ↑ ΔFt required value and total correction of the total floor reaction force moment around the desired center point of the total floor reaction force (three-component vertical vector) It consists of the required value of the floor reaction force moment ↑ ΔMt. In the following description, the required value of the perturbation total translational floor reaction force ↑ ΔFt is expressed as ↑ ΔFt_dmd, and the required value of the perturbation total floor reaction force moment ↑ ΔMt is expressed as ↑ ΔMt_dmd. The required value of the total floor reaction force ↑ ΔFMt is expressed as ↑ ΔFMt_dmd (= [↑ ΔFt_dmd, ↑ ΔMt_dmd] ^T ).

Also, the observed value of the translational floor reaction force (three-component longitudinal vector) of the actual total floor reaction force acting on the moving body 1 is ↑ Ft_act, and the observed value of the floor reaction force moment of the actual total floor reaction force ( The three-component vertical vector) is expressed as ↑ Mt_act, and the observed value of the actual total floor reaction force as a six-component vertical vector that combines these ↑ Ft_act and ↑ Mt_act is ↑ FMt_act (= [↑ Ft_act, ↑ Mt_act] ^T ).

  The total floor reaction force request correction amount determination unit 64 determines the required value ↑ ΔFMt_dmd of the perturbation total floor reaction force ↑ ΔFMt by the process described below.

  That is, the total floor reaction force required correction amount determination unit 64 first calculates a deviation ↑ Dfmt (= ↑ FMt_cmd1− ↑ FMt_act) between the corrected target floor reaction force ↑ FMt_cmd1 and the observed value ↑ FMt_act of the actual total floor reaction force. calculate. In this case, the observed value ↑ FMt_act of the actual total floor reaction force is the observed value of the floor reaction force (six-axis force) for each leg link 3 indicated by the outputs of the force sensors 52R and 52L. It is calculated by combining the center point as the action point.

  However, in this embodiment, in order to prevent the deviation ↑ Dfmt from fluctuating excessively, the total floor reaction force request correction amount determination unit 64 is provided for each leg link 3 indicated by the outputs of the force sensors 52R and 52L. The actual total floor reaction force acting on the center point of the target total floor reaction force by applying low-pass filtering to the actual total floor reaction force value obtained by combining the observed values of the floor reaction force (six-axis force) Force observation value ↑ FMt_act is calculated. In this case, the observation value ↑ FMt_act of the actual total floor reaction force is obtained by combining the observation value of the floor reaction force (six-axis force) for each leg link 3 with the low-pass characteristic filtering process. You may make it calculate.

  Next, the total floor reaction force required correction amount determination unit 64, as shown in the following equation (41), the proportional term (first term on the right side of the equation (41)) determined in accordance with the deviation ↑ Dfmt and the integral term ( The required value ↑ ΔFMt_dmd of the perturbation total floor reaction force ↑ ΔFMt is determined by adding to the second term on the right side of the equation (41).

  The proportional term is obtained by multiplying the deviation ↑ Dfmt by a predetermined gain Kcmp. The integral term is obtained by integrating a value obtained by multiplying a deviation ↑ Dfmt by a low-pass characteristic filtering process ↑ Dfmt_filt and a predetermined value gain Kestm. Here, the filtering process of the low-pass characteristic to obtain ↑ Dfmt_filt is such that the cut-off frequency on the high-frequency side is higher than the cut-off frequency on the high-frequency side of the filtering process of the low-pass characteristic used when calculating the deviation ↑ Dfmt. This is a filtering process with a characteristic of low frequency.

↑ ΔFMt_dmd = Kcmp ・ ↑ Dfmt + ∫ （Kestm ・ ↑ Dfmt_filt) …… （41）

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

  Here, the integral term of the equation (41) corresponds to a steady deviation between the corrected target floor reaction force ↑ FMt_cmd1 and the observed value of the actual total floor reaction force ↑ FMt_act, which is the assumed floor surface with respect to the actual floor surface. This corresponds to an error in position and orientation.

  In the present embodiment, the required value ↑ ΔFMt_dmd of the perturbation total floor reaction force ↑ ΔFMt is calculated by the above equation (41). However, the required value ↑ ΔFMt_dmd may be calculated by omitting the proportional term. Further, in the calculation of the integral term, instead of ↑ ΔDfmt_filt, ↑ Dfmt may be used as it is.

[Processing of representative contact surface translation / rotation displacement calculation unit 65]
After executing the processing of the total floor reaction 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. This representative contact surface translation / rotation displacement amount calculation unit 65 calculates the required value ↑ ΔFMt_dmd of the perturbation total floor reaction force ↑ ΔFMt determined as described above from the above equation (28), and the spring property of the virtual representative contact surface. This is a functional unit for converting into translation / rotation displacement amount ↑ Xc (= [↑ Xc_org, ↑ Xc_rot] ^T ).

  In this case, in this embodiment, each diagonal component of the translation spring constant matrix Kc_org of the representative contact surface representing the ground contact surface of the leg link 3R, 3L of the moving body 1 (each of the translational displacements of the representative contact surface in three axial directions). The spring constant of the component) and each diagonal component of the rotational spring constant matrix Kc_rot of the representative contact surface (the spring constant of each component relating to the rotational displacement around the three axes of the representative contact surface) are predetermined constant values. ing.

  Then, the representative contact surface translation / rotation displacement amount calculation unit 65 uses these spring constant matrices Kc_org and Kc_rot to calculate the springiness translation / rotation displacement amount ↑ Xc of the representative contact surface based on the equation (28). calculate. Specifically, the springiness translational / rotational displacement amount ↑ Xc of the representative contact surface is calculated by the following equation (42) obtained by substituting ↑ ΔFMt_dmd for ↑ ΔFMt on the right side of the equation (28).

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

[Processing of Representative Contact Surface Jacobian Matrix Calculation Unit 66]
In the compliance operation amount determination process, the control device 50 performs the representative operation in parallel with the calculation of the springy translational / rotational displacement amount ↑ Xc of the representative contact surface as described above (or before or after the calculation process of ↑ Xc). The process of the contact surface Jacobian matrix calculation unit 66 is executed.

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

  In the present embodiment, the representative contact surface Jacobian matrix calculation unit 66 calculates the representative contact surface Jacobian matrix Jc by the following equation (43) where N = 2 in the equation (27). In the present embodiment, either one of the leg links 3R, 3L and the other correspond to the first leg link and the second leg link in the equation (27), respectively. In the following description, for convenience, for example, the leg link 3R is referred to as a first leg link, and the leg link 3L is referred to as a second leg link.

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

  Each leg link to the floor surface based on the observed value (measured value) of the actual displacement amount (actual joint displacement amount) of each joint indicated by the output of the rotary encoder 54 with respect to the weight coefficient r_i (i = 1, 2). The observed value of the current actual posture of the three foot portions 18 is determined. Further, based on the observed value of the actual posture of the foot 18 of each leg link 3 and the outputs of the force sensors 52R and 52L, the current actual floor vertical drag component Fn_1 and Fn_2 for each leg link 3 is obtained. Observed values of (actual floor normal force components Fn_1, Fn_2) are obtained. Then, the weighting factor r_i (i = 1, 2) is determined from the observed values of the actual floor surface vertical force components Fn_1 and Fn_2 for each leg link 3 according to the definition formula shown by the formula (43-1). In this case, in order to prevent the value of the weight coefficient r_i from fluctuating frequently, a filtering process such as a low-pass filter may be applied to the observed value of Fn_i.

  Supplementally, the weighting factor r_i is preferably determined based on the observed value of the actual floor surface normal force component Fn_i for each leg link 3, but instead of the observed value, an approximation of the actual floor surface normal force component Fn_i is used. The weight coefficient r_i may be determined using a typical estimated value or predicted value. For example, when the target floor reaction force for each leg link 3 is included as a component of the standard gait and the actual floor reaction force for each leg link 3 is considered to approximately match the target floor reaction force The weight coefficient r_i may be determined using the target value instead of the observed value of the actual floor surface normal force component Fn_i.

The coefficient matrix Rk related to the matrix A_i (i = 1, 2) is calculated from the above equation based on the translational spring constant matrix Kc_org determined in advance with respect to the representative contact surface and the rotational spring constant matrix Kc_rot (or its inverse matrix Kc_rot ⁻¹ ). Determined according to (43-3). The coefficient matrix Rk may be stored in advance in the storage device of the control device 50.

  In addition, the position vector ↑ V_i necessary for determining the matrix VV_i related to the matrix A_i (i = 1, 2) is determined as follows, for example.

  Based on the observed value (measured value) of the current actual displacement amount of each joint indicated by the output of the rotary encoder 54 and the output of the force sensors 52R and 52L, the actual actual current on the ground contact surface of each leg link 3 is determined. A position vector (hereinafter referred to as ↑ Va_i (i = 1, 2)) of the floor reaction force center point (actual floor reaction force center point) is specified. The reference point of this position vector may be an arbitrary point.

  Next, the position vector ↑ Vc (= r_1) of the internal dividing point obtained by internally dividing the line segment connecting the actual floor reaction force central points corresponding to each leg link 3 by the ratio of the weighting coefficient r_i (i = 1, 2).・ ↑ Va_1 + r_2 ・ ↑ Va_2) is calculated as the position vector of the actual center point of the total floor reaction force.

  Then, by ↑ V_i = ↑ Va_i− ↑ Vc (i = 1, 2), the position vectors ↑ V_1 and ↑ V_2 of the actual floor reaction force center points of the ground contact surface of each leg link 3 with respect to the actual total floor reaction force center point are obtained. Each is decided.

  Since ↑ V_1 = ↑ Va_1− ↑ Vc = r_2 · (↑ Va_1− ↑ Va_2) and ↑ V_2 = ↑ Va_2− ↑ Vc = r_1 · (↑ Va_2− ↑ Va_1), based on these equations, The position vectors ↑ V_1 and ↑ V_2 may be determined. In this case, it is not necessary to calculate ↑ Vc.

  The matrix VV_i relating to the matrix A_i (i = 1, 2) is determined so as to satisfy VV_i · ↑ F_i = ↑ V_i × ↑ F_i according to the definition.

  For the leg link 3 in the ungrounded state (the leg link 3 with Fn_i = 0), since there is no actual floor reaction force center point, ↑ V_i corresponding to the leg link 3 is indefinite. In this case, since the weight coefficient r_i corresponding to the leg link 3 with Fn_i = 0 is zero, r_i · A_i · J_i = 0 regardless of the component values of A_i and J_i. Therefore, it is not necessary to determine the matrix VV_i (and thus the matrix A_i) corresponding to the leg link 3 in the non-grounded state. The same applies to the leg link Jacobian matrix J_i described below.

Of the variable values necessary for the calculation of the right side of Equation (43), the leg link Jacobian matrix J_i for the i-th leg link 3 is determined as follows. The leg link Jacobian matrix J_i is the spring translation / rotation displacement vector (= [↑ Xorg_i, ↑ Xrot_i] ^T ) of the i-th ground contact surface and the variation per unit time of the generalized variable vector ↑ q ↑ Δq It is a matrix expressing the relationship between them by the above equation (24).

  Here, if the spring-like translational displacement amount ↑ Xorg_i of ↑ X_i is regarded as the translational displacement amount per unit time of the i-th leg ground contact surface, the current grounding of the foot 18 of the i-th leg link 3 is assumed. It may be considered that it coincides with the temporal change rate (translation speed) of the position of the portion. Further, if the spring-like rotational displacement amount ↑ Xrot_i of ↑ X_i is regarded as the rotational displacement amount per unit time of the i-th leg ground contact surface, the current posture of the foot 18 of the i-th leg link 3 is determined. It can be considered that it coincides with the temporal change rate (angular velocity).

  Therefore, in the present embodiment, the position of the current contact portion of the foot 18 of the i-th leg link 3 and the posture of the foot 18 (more specifically, a vertical vector having the position and posture as components) per unit time. The leg link Jacobian matrix J_i related to the i-th leg link 3 is determined on the assumption that the change amount (temporal change rate) of A coincides with ↑ X_i on the left side of the equation (24). The current position of the ground contact portion of the foot 18 of the i-th leg link 3 corresponds to, for example, the center point of the actual floor reaction force of the ground contact surface (i-th leg contact surface) of the i-th leg link 3. The position of the point of the foot 18 is used.

  In this case, the leg link Jacobian matrix J_i uses the current movement state of the moving body 1 (which is specified by the measurement value of the current actual displacement amount of each joint) as the starting point of the minute change (perturbation) of ↑ q. It is determined by a known method. For example, using the geometric model (rigid body link model) of the moving body 1 or by analytical calculation, the i-th leg for a minute change (a minute change from the current state) of each component of the generalized variable vector ↑ q The leg link Jacobian matrix J_i is determined by calculating the change in the position of the ground contact portion of the foot 18 of the link 3 and the change in the posture of the foot 18.

  In this case, the current position of the ground contact portion of the foot 18 of the i-th leg link 3 coincides with the observed value of the current center point of the actual floor reaction force on the ground contact surface of the i-th leg link 3. It is said. 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 ground contact portion of the foot 18.

  Further, the current posture of the foot 18 of the i-th leg 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 54. A change in the posture of the foot 18 (a change with respect to a minute change of each component of ↑ q) is calculated, which coincides with the observed value of the current actual posture.

  In the present embodiment, the representative contact surface Jacobian matrix calculation unit 66 uses the weight coefficients r_1, r_2, matrices VV_1, VV_2 and leg link Jacobian matrices J_1, J_2 determined as described above, and the right side of the equation (43). Thus, the representative contact surface Jacobian matrix Jc is calculated.

[Processing of Joint Displacement Correction Amount Determination Unit 68]
In the compliance operation amount determination process, the control device 50 calculates the spring translational / rotational displacement amount ↑ Xc of the representative contact surface and the representative contact surface Jacobian matrix Jc as described above, and then the joint displacement correction amount determination unit 68. Execute the process.

  This joint displacement correction amount determination unit 68 is a correction amount of the displacement amount of each joint corresponding to the spring translational / rotational displacement amount ↑ Xc of the representative contact surface (from the target joint displacement amount corresponding to the target motion of the reference gait). It is a functional unit that determines a joint displacement correction amount that is a correction amount).

In the process of the joint displacement correction amount determination unit 68, first, a pseudo inverse matrix Jc ^{−1 of} the representative contact surface Jacobian matrix Jc is calculated. Here, since the number of components of the generalized variable vector ↑ q, and hence the number of components of ↑ Δq, is larger than the number of components of the spring translational / rotational displacement amount ↑ Xc, the representative contact surface Jacobian matrix Jc is a square matrix. Absent. Therefore, 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 representative contact surface Jacobian matrix Jc.

Then, the joint displacement correction amount determination unit 68 multiplies the pseudo-inverse matrix Jc ⁻¹ by the springy translational / rotational displacement amount ↑ Xc of the representative contact surface as shown in the equation (29), thereby obtaining the generalized variable. A generalized variable required correction amount vector ↑ Δq_dmd having the required correction amount (required perturbation amount) of each component of the 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 (30).

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. 5 including the process of determining the value of the adjustment parameter k.

First, in STEP 1, a transposed matrix Jc ^T of the representative contact surface 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 (31).

  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 (30) 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 (32) is set as the provisional value Δkp of the increase amount Δk of the adjustment parameter k.

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

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 (32) 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 directly added to the increase amount Δk in STEP 7. 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 (32) 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 (29) 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 compliance operation amount as the required correction amount of the displacement amount of each joint to bring the actual total floor reaction force closer to the corrected target total floor reaction force ↑ FMt_cmd1.

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

  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.

[Processing of floor estimation unit 70]
The control device 50 executes the processing of the floor surface estimation unit 70 in parallel with the drive control of each joint of the moving body 1. The floor surface estimation unit 70 is a functional unit that calculates a floor surface estimated value that is an estimated value of the position and orientation of the actual floor surface (actual floor surface) to which each leg link 3 of the moving body 1 comes into contact.

  For this process, the floor surface estimation unit 70 receives from the total floor reaction force required correction amount determination unit 64 the right side of the equation (41), which is the composition component of the perturbed total floor reaction force ↑ ΔFMt required value ↑ ΔFMt_dmd. The difference between the value of the integral term (second term), ie, the corrected target floor reaction force ↑ FMt_cmd1 and the observed value of the actual total floor reaction force ↑ FMt_act ↑ Dfmt (= ↑ FMt_cmd1− ↑ FMt_act) The value obtained by multiplying the value ↑ Dfmt_filt subjected to the filtering process by a value obtained by multiplying a predetermined value gain Kestm (scalar or diagonal matrix) (= ∫ (Kestm · ↑ Dfmt_filt)) is input. Hereinafter, the value of this integral term is expressed as ΔFMt_int as shown by the following equation (33).

↑ ΔFMt_int≡∫ (Kestm ・ ↑ Dfmt_filt) …… (33)

Here, by the drive control of each joint of the movable body 1 described above, the displacement amount of each joint is set so that the observed value ↑ FMt_act of the actual total floor reaction force follows the corrected target floor reaction force ↑ FMt_cmd1 (the above deviation) ↑ DFMt_int is a constant value for the position and posture of the assumed floor surface used to create the reference gait relative to the actual floor surface. It is caused by an error.

  Therefore, the actual floor surface (actual floor) of the assumed floor surface position and posture used for creating the target motion of the standard gait is the amount of spring translational / rotational displacement of the representative contact surface corresponding to ↑ ΔFMt_int. It is considered that it corresponds to a steady error with respect to the surface.

Therefore, the floor surface estimation unit 70 converts the given integral term value ↑ ΔFMt_int into the springy translational / rotational displacement amount according to the equation (28-1), and calculates the position and orientation of the assumed floor surface. Obtain as error ↑ Xc_int. In this case, more specifically, the translational force component ↑ ΔFt_int of the integral term value ↑ ΔFMt_int multiplied by the inverse matrix Kc_org ⁻¹ of the translational spring constant matrix Kc_org of the representative contact surface (= Kc_org ⁻¹ · ↑ ΔFt_int ) Is assumed to be the error ↑ Xc_org_int of the assumed floor surface position. Also, the moment component ↑ ΔMt_int of the integral term value ↑ ΔFMt_int multiplied by the inverse matrix Kc_rot ⁻¹ of the rotation spring constant matrix Kc_rot of the representative contact surface (= Kc_rot ⁻¹ · ↑ ΔMt_int) The posture error is set to ↑ Xc_rot_int.

  Then, the floor surface estimation unit 70 corrects the position and posture of the assumed floor surface according to the error ↑ Xc_int calculated as described above, thereby estimating the floor surface estimated value as the estimated value of the actual floor surface position and posture. To decide.

  Thus, the floor surface estimated value determined by the floor surface estimating unit 70 is given to the reference gait generating unit 61 in the present embodiment. Then, in the standard gait generator 61, the assumed floor used to create the standard gait is periodically (for example, one step at the time of the walking motion of the mobile body 1, according to the estimated floor value). (Or every multiple steps).

  Note that the assumed floor surface used to create the reference gait need not be determined only according to the estimated floor surface value. For example, when the position and orientation of the actual floor surface can be estimated based on a visual sensor mounted on the mobile body 1 or floor information given from outside, the reliability of the estimated floor surface position and orientation is reliable. The floor surface estimated value may be used for sex evaluation and complementary correction.

  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 floor reaction force request correction amount determination unit 64 implements the total floor reaction force request correction amount determination means in the present invention. In this case, in the present embodiment, the corrected target total floor reaction force ↑ FMt_cmd1 corresponds to the target total floor reaction force in the present invention.

  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.

  Moreover, the floor surface estimation part 70 has a function as a representative contact surface steady displacement amount calculation means in this invention (2nd aspect). In this case, the floor surface estimation unit 70 calculates the error ↑ Xc_int of the assumed floor surface position and orientation from the integral term value ↑ ΔFMt_int according to the equation (28-1). A steady displacement calculation means is realized. The error ↑ Xc_int corresponds to the representative contact surface steady displacement amount in the present invention.

According to the embodiment described above, the control device 50 perturbs the entire floor as an operation amount (control input) for causing the actual total floor reaction force to follow the corrected target total floor reaction force ↑ FMt_cmd1 in each control processing cycle. The required value ↑ ΔFMt_dmd of the reaction force ↑ ΔFMt is converted into a springy translational / rotational displacement amount ↑ Xc of the representative contact surface. Further, the control device 50 multiplies the ↑ Xc by the pseudo inverse matrix Jc ⁻¹ of the representative contact surface Jacobian matrix Jc to realize the ↑ Xc (and thus the actual total floor reaction force is corrected to the target total floor reaction) A joint displacement correction amount of each joint of the moving body 1 which is a compliance operation amount (to follow the force ↑ FMt_cmd1) is determined. 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 41 (joint actuator).

  Thus, according to the present embodiment, the relationship between the correction of the position and posture of the distal end portion (foot portion 18) of each leg link 3 and the change in the actual total floor reaction force and the mutual relationship thereof are taken into consideration. Thus, the actual total floor reaction force can be adjusted to follow the target total floor reaction force ↑ FMt_cmd1 after the actual total floor reaction force without executing a process for determining the correction amount of the position and posture of each leg link 3. The joint displacement correction amount 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, the weight coefficient r_i of each leg link 3_i in the equation (43) for calculating the representative contact surface Jacobian matrix Jc is larger as the leg link 3_i having a relatively large floor surface normal force component Fn_i ( (A value close to “1”).

  For this reason, the load of each leg link 3_i with respect to the required value ↑ ΔFMt_dmd of the perturbed total floor reaction force ↑ ΔFMt increases as the leg link 3_i having a relatively large floor surface normal force component Fn_i. In other words, the required value ↑ ΔFMt_dmd of the perturbed total floor reaction force ↑ ΔFMt can be realized by correcting the position and posture of the tip of the leg link where the floor surface normal force component Fn_i is relatively large as much as possible. A representative contact surface Jacobian matrix Jc is determined.

  Therefore, the required value ↑ ΔFMt_dmd of the perturbation total floor reaction force ↑ ΔFMt can be reliably realized without unnecessarily correcting the position and posture of the tip of the leg link 3_i having a relatively small floor normal force component Fn_i. It is possible to determine an appropriate joint displacement correction amount in order to make the actual total floor reaction force follow the corrected target total floor reaction force ↑ FMt_cmd1.

  Further, since the weight coefficient r_i of each leg link 3_i changes continuously, the representative contact surface Jacobian matrix Jc does not change discontinuously. 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.

  Further, in the embodiment, the joint displacement correction amount determination unit 68 searches for the value of the adjustment parameter k for preventing the value of the determinant DET from becoming too small (the processing of STEPs 2 to 9 in FIG. 5). ), 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 representative contact surface translation / rotation 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 required value of the perturbation total floor reaction force ↑ ΔFMt is controlled while driving the joints of the moving body 1 so that the actual total floor reaction force follows the corrected target total floor reaction force ↑ FMt_cmd1. By calculating the translation / rotation displacement amount ↑ Xc_int of the representative contact surface corresponding to the integral term value ↑ ΔFMt_int, which is a constituent element of ↑ ΔFMt_dmd, by the above equation (28-1), the position and posture of the assumed floor surface are calculated. It is possible to obtain ↑ Xc_int as a steady error with respect to the actual floor surface with high reliability. For this reason, it is possible to accurately estimate the position and orientation of the actual floor surface.

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

  In the embodiment, the moving body 1 is a legged mobile robot including two leg links 3R and 3L, but may be a robot including three or more leg links. In that case, processes other than the representative contact surface Jacobian matrix calculation unit 66 among the processes of the control device 50 may be the same as those in the above embodiment. In the processing of the representative contact surface Jacobian matrix calculation unit 66, the representative contact surface Jacobian matrix Jc may be calculated based on an equation in which the value of N in the equation (27) is the number of leg links. Note that the calculation method of the weighting coefficient r_i, the matrix A_i, and the leg link Jacobian matrix J_i necessary for the calculation in this case is the same as that in the above embodiment.

  In the moving body 1 of the embodiment, the tip of each leg link 3 can change the floor reaction force moment acting on the leg link 3 (the floor reaction force center point can be changed on the ground contact surface of the tip). Although it is configured by the flat portion 18, the end portion of each leg link may have a structure in which the floor reaction force moment cannot be changed.

  For example, as shown in FIG. 6, the tip of each leg link 103_i (i = 1, 2,..., N) has a spherical shape (more generally, the contact surface with the floor surface is substantially dotted. (Including those in which the contact surface has a small area).

  In this case, since the point-like contact surface (contact point) with the floor surface of each leg link 103_i matches the floor reaction force center point, a floor reaction force moment is applied to the leg link 103_i. (As a result, it can change) is virtually impossible. In this case, since the perturbed floor reaction force moment ↑ M_i that can be added to the leg link 103_i is always zero, the amount of spring rotation displacement of the ground contact surface of the leg link 103_i ↑ Xrot_i (= Krot_i · ↑ M_i ) Is always zero. As a result, the Jacobian matrix Jrot_i representing the relationship between the spring displacement ↑ Xrot_i and the variation ↑ Δq per unit time of the generalized variable vector ↑ q is always a zero matrix.

Therefore, the leg link Jacobian matrix J_i in this case becomes J_i = [Jorg_i, 0] ^{T according} to the equation (26c). For this reason, the process of calculating the representative contact surface Jacobian matrix Jc by the equation (27) is equivalent to the process of calculating Jc by the following equation (27a).

_ R_i, Rk, VV_i in the equation (27a) are the same as those shown in the equation (22). Jorg_i is the Jacobian matrix shown in the equation (26a), that is, the amount of springy translational displacement ↑ Xorg_i of the i-th contact surface and the amount of change per unit time ↑ Δq of the generalized variable vector ↑ q. It is a Jacobian matrix that represents the relationship between the two.

The values of r_i, Rk, and VV_i for performing the calculation of the equation (27a) can be determined in the same manner as in the above embodiment. Since J_i = [Jorg_i, 0] ^T , Jorg_i can be calculated in the same manner as the leg link Jacobian matrix J_i in the above embodiment.

  Further, the moving body may be a moving body having a structure in which a wheel is provided at the tip of each leg link 103 — i (i = 1, 2,..., N) as shown in FIG.

  In the above embodiment, the target total floor reaction force of the standard gait is corrected by the posture stabilization compensation force (floor reaction force moment) ↑ Mdmd determined according to the deviation between the target body posture and the actual body posture. Thus, the corrected target total floor reaction force is set as the tracking target of the actual total floor reaction force, but the target total floor reaction force of the standard gait may be set as the tracking target of the actual floor reaction force. In that case, the posture stabilization compensation force determination unit 62 and the target total floor reaction force correction unit 63 are not necessary. In this case, the total floor reaction force required correction amount determination unit 64 uses the target total floor reaction force ↑ FMt_cmd0 of the reference gait instead of the corrected target total floor reaction force ↑ FMt_cmd1 to perturb the total floor. The required value ↑ ΔFMt_dmd of the reaction force ↑ ΔFMt may be determined.

  In the embodiment, the proportional and integral terms corresponding to the deviation ↑ Dfmt (= ↑ FMt_cmd1− ↑ FMt_act) between the corrected target floor reaction force ↑ FMt_cmd1 and the observed value ↑ FMt_act of the actual total floor reaction force The required value ↑ ΔFMt_dmd of the perturbation total floor reaction force ↑ ΔFMt is determined by combining (adding), but the perturbation total floor reaction force ↑ ΔFMt is determined without changing the integral term (= ↑ ΔFMt_int). The required value ↑ ΔFMt_dmd may be determined.

  In this case, the representative contact surface translational / rotational displacement amount calculation unit 65 calculates the springiness translational / rotational displacement amount ↑ ΔXc of the representative contact surface calculated from ↑ ΔFMt_dmd by the position and orientation of the assumed floor surface. This corresponds to the steady error ↑ Xc_int for the actual floor surface.

  Therefore, in this case, the floor surface estimation unit 70 does not need to calculate ↑ Xc_int again by the equation (28-1), and the representative contact surface translation / rotation displacement amount calculation unit 65 calculates the representative surface. The estimated floor surface value may be determined by correcting the position and orientation of the assumed floor surface in accordance with the springy translational / rotational displacement amount ↑ ΔXc of the contact surface. By doing in this way, one Embodiment of the 1st aspect of this invention will be constructed | assembled.

  DESCRIPTION OF SYMBOLS 1,101 ... Mobile body, 2,102 ... Base | substrate, 3_i, 103_i ... Leg link, 50 ... Control apparatus, 64 ... Total floor reaction force request | requirement correction amount determination part (all floor reaction 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 determining means), 69... Joint displacement control section (joint displacement control means, 70... Floor surface estimation section (representative contact surface steady displacement amount calculating means), STEP 2 to 9.

Claims (3)

A base body, a plurality of leg links connected to the base body, and a joint actuator that drives a joint of each leg link, and the movement of the moving body that moves on the floor surface by the movement of the plurality of leg links In a control device for a moving body, the moving body is controlled according to a target motion of the body and a target total floor reaction force that is a target value of a total floor reaction force to be applied to the moving body to realize the target motion. Is a floor surface estimation device that estimates the position and posture of the actual floor surface on which the
The value obtained by integrating the deviation between the observed value of the total floor reaction force actually acting on the moving body and the target total floor reaction force is additionally applied to the moving body in order to bring the deviation close to zero. A total floor reaction force request correction amount determining means for determining a total floor reaction force request correction amount that is a correction amount of the total floor reaction force to be caused;
The total floor reaction force required correction amount is generated by a spring displacement of the position and posture of the representative contact surface as a single virtual contact surface representing all the contact surfaces of the movable body and the floor surface. Assuming that the position and orientation of the representative contact surface corresponding to the total floor reaction force required correction amount are determined from the determined total floor reaction force required correction amount and a predetermined spring constant of the representative contact surface. Representative contact surface position / posture displacement amount calculating means for calculating the displacement amount;
Between the temporal change rate of the position and posture of the representative contact surface, and the temporal change rate of the generalized variable vector composed of the position and posture of the base body and the displacement amount of each joint of the moving body as components. The representative contact surface Jacobian matrix Jc, which is a Jacobian matrix representing the relationship of the above, is obtained by changing the temporal change rate of the position of the tip portion of each leg link or the temporal change rate of the position and posture of the tip portion of each leg link and the generalized variable. Each floor link Jacobian matrix J_i, which is a Jacobian matrix representing the relationship between the temporal change rate of the vector, the spring constant, and the total floor reaction as the action point of the total floor reaction force that actually acts on the moving body The representative contact surface calculated by the following equation (27) from the relative position of the actual floor reaction force center point at the tip of each leg link with respect to the force center point and the value of the floor reaction force actually acting on each leg link A Jacobian matrix calculating means;
The calculated required displacement amount of the position and orientation of the representative contact surface is multiplied by the pseudo inverse matrix Jc ⁻¹ of the calculated representative contact surface Jacobian matrix Jc to obtain the required displacement amount of the position and orientation of the representative contact surface. 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 realizing
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 joint displacement control means for controlling the joint actuator,
The actual floor surface is corrected by correcting the position and posture of the assumed floor surface, which is the floor surface assumed in the target motion, according to the required displacement amount calculated by the representative contact surface position / posture displacement amount calculating means. A floor surface estimation device characterized by estimating the position and orientation of the floor.
A base body, a plurality of leg links connected to the base body, and a joint actuator that drives a joint of each leg link, and the movement of the moving body that moves on the floor surface by the movement of the plurality of leg links In a control device for a moving body, the moving body is controlled according to a target motion of the body and a target total floor reaction force that is a target value of a total floor reaction force to be applied to the moving body to realize the target motion. Is a floor surface estimation device that estimates the position and posture of the actual floor surface on which the
A correction amount of the total floor reaction force to be additionally applied to the moving body in order to bring the deviation between the observed value of the total floor reaction force actually acting on the moving body and the target total floor reaction force close to zero. A total floor reaction force request correction amount determining means for determining a total floor reaction force request correction amount by combining at least a proportional term proportional to the deviation and an integral term obtained by integrating the deviation;
The total floor reaction force required correction amount is generated by a spring displacement of the position and posture of the representative contact surface as a single virtual contact surface representing all the contact surfaces of the movable body and the floor surface. Assuming that the position and orientation of the representative contact surface corresponding to the total floor reaction force required correction amount are determined from the determined total floor reaction force required correction amount and a predetermined spring constant of the representative contact surface. Representative contact surface position / posture displacement amount calculating means for calculating the displacement amount;
Between the temporal change rate of the position and posture of the representative contact surface, and the temporal change rate of the generalized variable vector composed of the position and posture of the base body and the displacement amount of each joint of the moving body as components. The representative contact surface Jacobian matrix Jc, which is a Jacobian matrix representing the relationship of the above, is obtained by changing the temporal change rate of the position of the tip portion of each leg link or the temporal change rate of the position and posture of the tip portion of each leg link and the generalized variable. Each floor link Jacobian matrix J_i, which is a Jacobian matrix representing the relationship between the temporal change rate of the vector, the spring constant, and the total floor reaction as the action point of the total floor reaction force that actually acts on the moving body The representative contact surface calculated by the following equation (27) from the relative position of the actual floor reaction force center point at the tip of each leg link with respect to the force center point and the value of the floor reaction force actually acting on each leg link A Jacobian matrix calculating means;
The calculated required displacement amount of the position and orientation of the representative contact surface is multiplied by the pseudo inverse matrix Jc ⁻¹ of the calculated representative contact surface Jacobian matrix Jc to obtain the required displacement amount of the position and orientation of the representative contact surface. 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 realizing
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. Joint displacement control means for controlling the joint actuator;
A representative contact surface steady displacement amount that is a displacement amount of the position and orientation of the representative contact surface corresponding to the integral term from the integral term of the total floor reaction force required correction amount and the spring constant of the representative contact surface. A representative contact surface steady-state displacement amount calculating means for calculating
By correcting the position and orientation of the assumed floor surface, which is the floor surface assumed in the target motion, according to the representative contact surface steady displacement amount calculated by the representative contact surface steady displacement amount calculating means, A floor surface estimation apparatus characterized by estimating a position and a posture of a floor surface.
In the floor surface estimation apparatus according to claim 1 or 2,
The calculated pseudo inverse matrix Jc ⁻¹ of the representative contact surface Jacobian matrix Jc is a matrix calculated by the following equation (30) from the preset weight matrix W and the calculated representative contact surface Jacobian Jc. In addition, there is provided pseudo-inverse matrix operation parameter determination means for determining the value of k in the equation (30) so that the determinant DET represented by the following equation (31) 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 ...... (30)
DET = det (Jc ・ W ⁻¹・ Jc ^T + k ・ I) (31)
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 to be used for calculating the pseudo inverse matrix by the equation (30), 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 floor surface estimation device characterized by the above.