A floor reaction force estimation method for a two-legged walking mobile body, which estimates a floor reaction force acting on each leg of the two-legged walking mobile body, comprising:
- a first step of sequentially measuring displacements of a plurality of predetermined joints including at least a hip joint and a knee joint of the each leg of the two-legged walking mobile body;
- a second step of sequentially calculating center-of-gravity locations of the two-legged walking mobile body by using at least a rigid link model in which the two-legged walking mobile body is represented as a link body made of a plurality of rigid elements and a plurality of joint elements and the displacements of the joints measured in the first step;
- a third step of sequentially calculating accelerations of the centers of gravity of the two-legged walking mobile body by using at least the center-of-gravity locations calculated in the second step and outputs of an acceleration sensor attached to the two-legged walking mobile body;
- a fourth step of sequentially calculating component vectors on a plane perpendicular to at least one predetermined axis of a total floor reaction force, which is a total sum of all floor reaction forces acting on the two-legged walking mobile body from the accelerations of the centers of gravity calculated in the third step and the weight of the two-legged walking mobile body;
- a fifth step of sequentially calculating components around the predetermined axis of a total rate of change of angular momentum, which is a time rate of change of an total angular momentum of the two-legged walking mobile body, by using at least the rigid link model, the displacements of the joints measured in the first step, the center-of-gravity locations calculated in the second step, the accelerations of the centers of gravity calculated in the third step, and the outputs of an angular velocity sensor attached to the two-legged walking mobile body;
- a sixth step of sequentially grasping floor reaction force acting points, each of which is a point of action of a floor reaction force acting on the each landing leg of the two-legged walking mobile body;
- a seventh step of determining the direction of the component vector on the plane perpendicular to the predetermined axis of the floor reaction force acting on the each landing leg in such a way that the direction of the moment generated around the center-of-gravity of the two-legged walking mobile body by the total sum of the component vectors on the plane perpendicular to the predetermined axis of the floor reaction force acting on the each landing leg is coincident with the direction of the component around the predetermined axis of the total rate of change of angular momentum by using the center-of-gravity location calculated in the second step, the component vector of the total reaction force calculated in the fourth step, the component of the total rate of change of angular momentum calculated in the fifth step, and the floor reaction force acting point grasped in the sixth step;
- and
- an eighth step of using the direction of the component vector of the floor reaction force determined in the seventh step and the component vector of the total reaction force calculated in the fourth step to calculate an estimated value of the component vector on the plane perpendicular to the predetermined axis of the floor reaction force acting on the each leg in such a way that the vector of the total sum of the component vector on the plane perpendicular to the predetermined axis of the floor reaction force acting on each of the both legs equals the component vector on the plane of the total floor reaction force, by regulating the direction of the component vector on the plane perpendicular to the predetermined axis of the floor reaction force acting on each of the landing legs to the direction determined in the seventh step if the two-legged walking mobile body is in the two-leg supporting state, and to calculate an estimated value of the component vector on the plane perpendicular to the predetermined axis of the floor reaction force acting on the landing leg in such a way that the magnitude of the component vector on the plane perpendicular to the predetermined axis of the floor reaction force acting on the leg equals the magnitude of the component vector on the plane of the total floor reaction force, by regulating the direction of the component vector on the plane perpendicular to the predetermined axis of the floor reaction force acting on the landing leg to the direction determined in the seventh step if the two-legged walking mobile body is in the one-leg supporting state.