JP2003220584A - Floor reaction force action point estimation method for bipedal moving objects - Google Patents

Abstract

(57) [Summary] [PROBLEMS] To enable accurate and real-time grasp of the position of the point of application of a floor reaction force acting on the leg of a bipedal walking body such as a human. A position vector of a floor reaction force acting point with respect to an ankle changes with a remarkable correlation with a tilt angle of a thigh of a leg and a bending angle of a knee joint. Based on this correlation, the position vector of the floor reaction force acting point is obtained from at least one of the inclination angle of the thigh and the bending angle of the knee joint, for example, the inclination angle of the thigh.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention sequentially determines the position of a floor reaction force acting point on each leg of a biped walking vehicle such as a human or a biped walking robot in real time during movement of the biped walking vehicle. Regarding the method of estimating.

[0002]

2. Description of the Related Art For example, in the case of controlling the movement of a walking assist device that assists a human walking movement or the movement of a bipedal walking robot, the floor reaction force acting on the leg of the human or the bipedal walking robot ( Specifically, it is necessary to sequentially grasp the force acting on the foot of the leg from the floor) and the position of the floor reaction force acting point. By grasping the position of the floor reaction force and the floor reaction force action point, it becomes possible to grasp the moment etc. acting on the joint of the leg of the bipedal walking body, and based on the grasped moment etc. It is possible to determine the target assisting force of the walking assist device, the target driving torque of each joint of the biped walking robot, and the like.

As a method of grasping the floor reaction force, for example, one disclosed in Japanese Patent Laid-Open No. 2000-249570 is known. In this technology, the floor reaction force of each leg changes with time of 1 / (1) of the walking cycle because the time-dependent waveform of the floor reaction force of each leg changes periodically during steady walking of a bipedal walking body. n
It is to be understood as a composite value (linear combination) of a plurality of trigonometric functions having different periods (n = 1, 2, ...). However, this cannot grasp the position of the floor reaction force acting point, and is insufficient to grasp the moment acting on the joint of the leg of the bipedal walking body.

A method is also known in which a bipedal walking body is made to walk on a force plate installed on the floor, and the position of the floor reaction force and the floor reaction force acting point is grasped by the output of the force plate (special feature). Open 2001-29329 gazette). However,
This device has a problem that the position of the floor reaction force and the floor reaction force application point can be grasped only under the environment in which the force plate is installed, and it cannot be applied to walking in a normal environment.

[0005]

SUMMARY OF THE INVENTION In view of the above points, the present invention is capable of accurately grasping the position of the floor reaction force acting point in real time by a relatively simple method, and particularly as a bipedal walking vehicle. It is an object of the present invention to provide a method for estimating a floor reaction force action point suitable for grasping the position of the floor reaction force action point relating to the human.

[0006]

In order to achieve the above object, the present invention is a method for successively estimating the position of a floor reaction force acting point with respect to each leg of a bipedal walking mobile body. At least one of the thigh inclination angle and the knee joint flexion angle is set as the measurement target angle, and the measurement target angle is sequentially measured during the movement of the bipedal walking mobile body, and the floor reaction against the ankle part of each leg is performed. It is characterized in that the position vector is sequentially estimated from the measured value of the measurement target angle based on a predetermined correlation established between the position vector of the force application point and the measurement target angle.

As a result of earnest efforts, the inventor of the present application has found that, for example, the inclination angle of the thigh of the leg and the flexion angle of the knee joint, which are in contact with the ground during normal walking, have a relatively significant correlation with the floor reaction force acting point. It has been found that, for example, the correlations shown in FIGS. 5 and 6 are established between the advancing direction component and the vertical direction component of the position vector of the floor reaction force acting point and the tilt angle of the thigh. It was Therefore, the position vector of the floor reaction force acting point can be grasped in real time from the measured values of the tilt angle of the thigh and the bending angle of the knee joint, which are the measurement target angles, as in the present invention.

By the way, in order to obtain the position vector of the floor reaction force acting point from the measured value of the measurement target angle, the above correlation is stored as a data table, and the floor reaction force action corresponding to the measured value of the measurement target angle is stored. You may make it table-search the position vector of a point. However, since this requires a large storage capacity, an approximate expression that represents the above correlation and that has the measurement target angle as a parameter is created and stored, and the measured value of the measurement target angle is substituted into this approximation expression. It is desirable to calculate the position vector of the floor reaction force acting point.
Here, it may be difficult to approximate the correlation between the position vector of the floor reaction force acting point and the measurement target angle by one approximate expression. In this case, when creating the approximate expression, the transition of the correlation from the heel of the foot of each leg to the time the toe leaves the floor is divided into several phases, and each phase is the same or It may be approximated by a different function. In particular, as shown in FIG. 5 and FIG. 6, there is a correlation having a minimum value with respect to the measurement target angle (tilt angle of the thigh), and even if the measurement target angle is the same, the measurement target angle decrease process If the value of the position vector of the floor reaction force action point differs from the increasing process, the transition of the correlation from the heel of the foot of each leg to the landing of the toe to the floor is divided into several phases. In order to classify, the phase is divided at least according to the positive / negative of the change speed of the measurement target angle (change amount of the measurement target angle per unit time). Accordingly, the value of the position vector in the process of decreasing the measurement target angle and the value of the position vector in the process of increasing the measurement target angle can be calculated separately.

[0009]

BEST MODE FOR CARRYING OUT THE INVENTION Before describing the embodiments of the present invention, the basic concept of a floor reaction force estimation method for a bipedal walking vehicle will be described with reference to FIG. The movement state of the legs of the bipedal walking body, for example, the movement state of the legs during walking motion is shown in FIG.
As illustrated in (a), both legs 2, of the bipedal walking body 1.
2 is a single leg supporting state in which only one leg 2 (front leg in the traveling direction in the figure) of the two is grounded, and both leg supporting states in which both legs 2 and 2 are grounded as shown in FIG. 1B. There is.

First, in the single-leg support state, the equation of motion of the center of gravity of the bipedal walking body in the absolute coordinate system fixed to the floor on which the bipedal locomotion body moves (specifically, the center of gravity is described below). The equation of motion regarding the translational movement of the robot is calculated by multiplying the product of the acceleration of the center of gravity and the weight of the bipedal walking body by the gravity acting on the center of gravity (= weight of the bipedal walking body × gravitational acceleration). The relational expression is equal to the resultant force with the floor reaction force acting from the floor on the ground contacting part of the leg. Specifically, for example, as shown in FIG. 1A, in the absolute coordinate system Cf fixed to the floor A, the acceleration a of the center of gravity G0 of the bipedal walking mobile unit 1 in the X-axis direction (bipedal locomotion). Ax and az are components of the body 1 in the horizontal direction and the Z-axis direction (vertical direction), respectively, and X of the floor reaction force F related to the grounded leg 2 (the leg 2 on the supporting leg side). Letting Fx and Fz be the components in the axial direction and the Z-axis direction, respectively, the equation of motion of the center of gravity G0 is expressed by the following equation (1).

^T (Fx, Fz−M · g) = M · ^T (ax, az) (1) (where M is the weight of the bipedal moving body, g is the acceleration of gravity) The parentheses ^T (,) on both sides of the parentheses mean a two-component vector. In this specification, the notation in the form of ^T (,) represents a vector.

Therefore, if the acceleration a = ^T (ax, az) of the center of gravity G0 of the bipedal walking body 1 is grasped, the acceleration a and
Using the value of the weight M of the bipedal walking body 1 and the value of the gravitational acceleration g, the floor reaction force F = ^T (Fx, Fz) according to the following equation (2).
It is possible to obtain an estimated value of.

^T (Fx, Fz) = M · ^T (ax, az−g) (2)

In this case, the weight M required to obtain the estimated value of the floor reaction force F can be grasped in advance by measurement or the like. The position of the center of gravity G0 and the acceleration a will be described in detail later, but the output of a sensor that detects a bending angle (rotation angle) of each joint of the bipedal walking body 1 or an output of a sensor such as an acceleration sensor or a gyro sensor. It is possible to sequentially grasp by using a known method and the like.

Next, the equation of motion of the center of gravity of the bipedal walking vehicle in the two-legged ground contact state (more specifically, the equation of motion for translational movement of the center of gravity) is calculated by multiplying the product of the acceleration of the center of gravity and the weight of the bipedal walking vehicle. , Gravity acting on the center of gravity (= weight of bipedal moving body x gravitational acceleration), and floor reaction force acting from the floor on each ground contact portion of both legs (two floor reaction forces corresponding to both legs) It becomes the relational expression that it is equal to the resultant force with. Specifically, as shown in FIG. 1B, the floor reaction force F applied to the leg 2 on the front side in the traveling direction of the bipedal walking body 1.
When the XZ coordinate components of f are Ffx and Ffz, and the XZ coordinate components of the floor reaction force Fr related to the rear leg 2 are Frx and Frz, the center of gravity G0
The equation of motion of is expressed by the following equation (3).

^T (Ffx + Frx, Ffz + Frz−M · g) = M · ^T (ax, az) (3) Incidentally, the meanings of ax, az, M, and g in the formula (3) are as described above. .

On the other hand, according to the findings of the inventors of the present application, the floor reaction forces Ff and Fr respectively applied to the legs 2 and 2 in the two-leg supporting state are as shown in FIG. 1 (b). It can be considered that it acts from the specific portions 12f and 12r (for example, ankles) near the lower ends of the legs 2 and 2 toward the center of gravity G0 of the bipedal walking body 1. Then, at this time, the specific portions 12f, 12 of the legs 2, 2 with respect to the center of gravity G0.
A constant relational expression between the position of r and the floor reaction forces Ff and Fr acting on the legs 2 and 2, that is, the center of gravity G0 and the specific portions 3f and 3r of the legs 2 and 2 are A relational expression representing a relation that the direction of the connecting line segment (the direction of the position vector of the specific parts 3f, 3r with respect to the center of gravity G0) is equal to the direction of the floor reaction forces Ff, Fr related to the legs 2, 2 is established. .

Specifically, referring to FIG. 1 (b), the coordinates of the position of the center of gravity G0 in the absolute coordinate system Cf are (Xg,
Zg) and the coordinates of the position of the specific portion 3f of the front leg 2 (X
f, Zf) and the coordinates of the position of the specific portion 3r of the rear leg 2 are (Xr, Zr), the above relational expression becomes the following expression (4).

[0019]       (Zf-Zg) / (Xf-Xg) = Ffz / Ffx       (Zr-Zg) / (Xr-Xg) = Frz / Frx                                           …… (4)

Then, from the equation (4) and the equation (3), the following equation (5) is obtained.

[0021]       Ffx = M ・ {ΔXf ・ (ΔZr ・ ax−ΔXr ・ az                   -ΔXr ・ g)} / (ΔXf ・ ΔZr-ΔXr ・ ΔZf)       Ffz = M · {ΔZf · (ΔZr · ax−ΔXr · az                   -ΔXr ・ g)} / (ΔXf ・ ΔZr-ΔXr ・ ΔZf)       Frx = M ・ {ΔXr ・ (-ΔZf ・ ax + ΔXf ・ az                   + ΔXf ・ g)} / (ΔXf ・ ΔZr-ΔXr ・ ΔZf)       Frz = M ・ {ΔZr ・ (-ΔZf ・ ax + ΔXf ・ az                   + ΔXf ・ g)} / (ΔXf ・ ΔZr-ΔXr ・ ΔZf)                                                               …… (5)           (However, ΔZf = Xf−Xg, ΔZf = Zf−Zg,                       ΔXr = Xr−Xg, ΔZr = Zr−Zg)

Therefore, the acceleration a = ^T (ax, az) of the center of gravity G0 of the bipedal walking body 1 is grasped, and at the same time, the specific portions of the legs 2 and 2 with respect to the center of gravity G0 of the bipedal walking body 1 are determined. The positions of 3f and 3r (this is ΔXf, Δ in equation (5))
Zf, ΔXr, ΔZr), the acceleration a and the positions of the specific parts 3f and 3r, the weight M of the bipedal walking body 1 and the gravity acceleration g are used. ,
From the equation (5), the floor reaction force Ff = ^T (Ff for each leg 2
It is possible to obtain the estimated values of x, Ffz) and Fr = ^T (Frx, Frz).

In this case, the weight M required to obtain the estimated values of the floor reaction forces Ff and Fr can be grasped in advance by measurement or the like. The acceleration a of the center of gravity G0, the position of the center of gravity G0, and the positions of the specific portions 3f and 3r with respect to the center of gravity G0 will be described in detail later, but the bending angle (rotation angle) of each joint of the bipedal walking body 1 is described. By using the output of a sensor that detects, or an sensor such as an acceleration sensor or a gyro sensor,
It is possible to sequentially grasp by a known method or the like.

An embodiment in which the floor reaction force action point estimating method of the present invention is applied to a human being as a bipedal walking body will be described below.

As schematically shown in FIG. 2, the human 1 is
The structure is roughly divided into a pair of left and right legs 2, 2, a body 5 including a waist 3 and a chest 4, a head 6, and a pair of left and right arms 7, 7. The waist 5 of the body 5 is connected to each of the legs 2 and 2 via a pair of left and right hip joints 8 and is supported on the legs 2 and 2. Also, the chest 4 of the body 5 is located above the waist 3 and the human body 1 with respect to the waist 3.
It can be tilted to the front side of. And this chest 4
Arms 7, 7 are extended from the left and right sides of the upper part of the upper part, and the head part 6 is supported on the upper end part of the chest part 4.

Each leg 2, 2 has a thigh portion 9 extending from the hip joint 8 and a lower leg portion 11 extending from the tip of the thigh portion 9 via a knee joint 10, A foot portion 13 is connected to a tip portion of the thigh 11 via an ankle portion (ankle joint) 12.

In the present embodiment, in order to estimate the floor reaction force acting on each leg 2 of the human 1 having such a configuration and further the moment acting on the knee joint 10 and the hip joint 8, the following is performed. Human 1 is equipped with such a device.

That is, a gyro sensor 14 (hereinafter referred to as a chest gyro sensor 14) which produces an output according to an angular velocity associated with the inclination of the chest 4 is provided on the chest 4 of the body 5, and the chest 4 is accelerated in the longitudinal direction. Acceleration sensor 15 (hereinafter referred to as chest longitudinal acceleration sensor 15)
And an arithmetic processing unit 16 including a CPU, RAM, ROM, etc.
And a battery 17 serving as a power source for the arithmetic processing unit 16 and the like. In this case, the chest gyro sensor 14, the chest longitudinal acceleration sensor 15, the arithmetic processing unit 1
The battery 6 and the battery 17 are housed in, for example, a shoulder bag-shaped accommodating member 18 that is fixed to the chest 4 via a belt (not shown) or the like, and are integrally fixed to the chest 4 via the accommodating member 18.

The acceleration represented by the output of the chest acceleration sensor 15 is, more specifically, the horizontal cross-sectional direction of the chest 4 (the chest 4
Acceleration in the front-back direction in the direction orthogonal to the axis of
When the human 1 stands upright on a flat ground, the acceleration is in the front-rear horizontal direction (X-axis direction of the absolute coordinate system in FIG. 2), but the waist 3 or chest 4 is in the vertical direction (absolute coordinate system in FIG. 2). In the Z-axis direction), the acceleration is in a direction inclined to the horizontal direction by the inclination angle of the chest 4 with respect to the vertical direction.

The waist portion 3 of the body 5 has a gyro sensor 1 which produces an output corresponding to an angular velocity associated with the inclination of the waist portion 3.
9 (hereinafter, referred to as waist gyro sensor 19) and waist 3
An acceleration sensor 20 (hereinafter, referred to as waist longitudinal acceleration sensor 20) that generates an output according to the longitudinal acceleration of
An acceleration sensor 21 (hereinafter referred to as waist vertical acceleration sensor 21) that generates an output according to the vertical acceleration of the waist 3 is integrally mounted and fixed via a fixing means such as a belt (not shown).

Here, more specifically, the waist longitudinal acceleration sensor 20 detects the longitudinal acceleration in the horizontal sectional direction of the waist 3 (the direction orthogonal to the axial center of the waist 3), similarly to the chest longitudinal acceleration sensor 15. It is a sensor. More specifically, the waist vertical acceleration sensor 21 is more specifically a vertical acceleration in the axial direction of the waist 3 (this is the waist longitudinal acceleration sensor 2).
This is a sensor that detects (0 is orthogonal to the acceleration detected). The waist longitudinal acceleration sensor 20 and the waist vertical acceleration sensor 21 may be integrally configured by a biaxial acceleration sensor.

Further, at the hip joint 8 and the knee joint 10 of each leg 2, the hip joint angle sensor 22 and the knee joint angle sensor 2 which generate outputs according to the respective bending angles Δθc and Δθd.
3 is installed. The hip joint angle sensor 22 is shown in FIG. 2 on the near side (to the front of the human 1 on the right side).
Although only the hip joint angle sensor 22 related to the hip joint 8 of the leg body 2 of FIG. 2 is shown, the hip joint angle sensor 22 on the front side is attached to the hip joint 8 of the leg body 2 on the other side (left side when facing the front of the human 1). A hip joint angle sensor 22 is mounted concentrically with.

These angle sensors 22 and 23 are constituted by, for example, potentiometers, and are attached to each leg 2 through means such as band members (not shown). Here, the flexion angle Δθc detected by each hip joint angle sensor 22 is, more specifically, a predetermined posture relationship between the waist 3 and the thigh 9 of each leg 2 (for example, in the upright posture of the human 1). Around the hip joint 8 of the thigh portion 9 of each leg 2 with respect to the waist portion 3 (human being), with reference to the posture in which the axis of the waist 3 and the axis of the thigh 9 are substantially parallel to each other. 1 around the axis of the hip joint 8 in the left-right direction). Similarly, the bending angle Δθd detected by each knee joint angle sensor 23 has a predetermined posture relationship between the thigh 9 and the lower leg 11 of each leg 2 (for example, the axial center of the thigh 9). Around the knee joint 10 of the lower leg 11 with respect to the thigh 9 (the axis of the knee joint 10 in the left-right direction of the human 1) with reference to the time when the posture of the lower leg 11 is substantially parallel to the axial center of the lower leg 11. It is the rotation angle of the circumference.

The sensors 14, 15, 19 to 23 are used.
Are connected to the arithmetic processing unit 16 via signal lines (not shown) so as to input their outputs to the arithmetic processing unit 16.

The arithmetic processing unit 16 has functional means as shown in FIG. That is, the arithmetic processing device 1
6 uses the detection data of the waist vertical acceleration sensor 21 and the data of the estimated value of the floor reaction force of each leg 2 obtained by the floor reaction force estimation means 35, which will be described later, and the leg 2 of the human 1. Two
The leg motion determination means 24 is provided to determine whether the exercise state is the single-leg support state (the state of FIG. 1A) or the both-leg support state (the state of FIG. 1B). Further, the arithmetic processing unit 16 uses the detection data of the chest longitudinal acceleration sensor 15 and the chest gyro sensor 14 to detect the inclination angle θa of the chest 4 in the absolute coordinate system Cf (specifically, for example, the inclination angle θa with respect to the vertical direction. 2) is used to measure the waist inclination angle measuring means 25, and the detection data of the waist longitudinal acceleration sensor 20 and the waist gyro sensor 19 are used.
The waist inclination angle measuring means 26 for measuring the inclination angle θb in the absolute coordinate system Cf (specifically, for example, the inclination angle θb with respect to the vertical direction; see FIG. 2).

Further, the arithmetic processing unit 16 uses the detection data of the waist longitudinal acceleration sensor 20 and the waist vertical acceleration sensor 21 and the data of the inclination angle θb of the waist 3 measured by the waist inclination angle measuring means 26. As shown in FIG. 2, the waist 3 is used as a reference point for the human 1 in the present embodiment.
Acceleration (translational acceleration) a ₀ = in the absolute coordinate system Cf of the origin O of the body coordinate system Cp (xz coordinates in FIG. 2) set to
A reference acceleration measuring means 27 for calculating ^T (a ₀ x, a ₀ z) is provided. Here, more specifically, the body coordinate system Cp is, for example, the midpoint of the line connecting the centers of the left and right hip joints 8 of the human 1 as the origin O, the vertical direction as the z-axis direction, and the human 1
Is a coordinate system in which the horizontal direction toward the front is defined as the x-axis direction (the coordinate system in which the directions of three axes are the same as the absolute coordinate system Cf).

Further, the arithmetic processing unit 16 uses the detection data of the hip joint angle sensor 22 and the knee joint angle sensor 23 of each leg 2 and the data of the inclination angle θb of the waist 3 by the waist inclination angle measuring means 26. Then, the inclination angles θc and θd of the thigh 9 and the lower leg 11 of each leg 2 in the absolute coordinate system Cf (specifically, for example, the inclination angles θc and θd with respect to the vertical direction, see FIG. 2). Body posture calculating means 28
Is equipped with.

Further, the arithmetic processing unit 16 has the inclination angle θ of the chest 4 obtained by the chest inclination angle measuring means 25, the waist inclination angle measuring means 26 and the leg posture calculating means 28.
a, the inclination angle θb of the waist 3, the inclination angle θc of the thigh 9 of each leg 2, and the inclination angle θd of the lower leg 11, using the data of the human 1 corresponding to the rigid link model described later. By using the center-of-gravity position calculating means 29 for each part for obtaining the position of the center of gravity of the rigid body equivalent part (specifically, the position of the center of gravity of each rigid body equivalent part in the body coordinate system Cp) and the data of the position of the center of gravity of each rigid body equivalent part, A body center of gravity position calculating means 30 for obtaining the position of the center of gravity of the entire human 1 in the body coordinate system Cp,
The center of gravity G0 of the entire human 1 (see FIG. 1; hereinafter, the center of gravity of the body)
G0) position data and the leg posture calculation means 28
Using the data of the inclination angles θc and θd of the thigh 9 and the lower leg 11 of each leg 2 according to the above, the ankle portion 12 of each leg 2 as a specific portion of each leg 2 in the present embodiment. Of the body center of gravity G0 with respect to the body center of gravity G0 (specifically, ΔXf, ΔZf, ΔXr, ΔZr in the equation (5)), position data of the body center of gravity by the body center of gravity position calculating means 30, and the reference. Using the data of the acceleration a ₀ of the origin O of the body coordinate system Cp by the acceleration measuring means 27, the acceleration a of the center of gravity G 0 of the body in the absolute coordinate system Cf =
^And a body center-of-gravity acceleration calculation means 32 for obtaining ^T (ax, az) (see FIG. 1).

Further, the arithmetic processing unit 16 has
Center of gravity of each rigid body equivalent part of the human 1 by the center position calculation means 29
Position (specifically, the position of the center of gravity of the rigid body equivalent part related to leg 2)
Data and the body measured by the reference acceleration measuring means 27.
Acceleration a at the origin O of the coordinate system Cp ₀Absolute using and data of
Thigh 9 and lower leg 11 of each leg 2 in the coordinate system Cf
Legs that calculate the acceleration (translational acceleration) of each center of gravity of
Each part acceleration calculation means 33 and the leg posture calculation means 28
Of the thigh 9 and lower leg 11 of each leg 2
The absolute coordinate system Cf is calculated using the data of the inclination angles θc and θd.
Angular acceleration of thigh 9 and lower leg 11 of each leg 2, 2
For calculating the angular acceleration of each leg of the body, and the leg figure
Inclination angle θ of the thigh 9 calculated by the force calculating means 28
c and the knee joint 1 measured by the knee joint angle sensor 23
Each leg grounded using 0 bending angle Δθd data
Floor reaction force action point estimation for estimating the position of the floor reaction force action point of the body 2
And means 35.

In the arithmetic processing unit 16, the data of the acceleration a of the body center of gravity by the body center of gravity acceleration calculating means 32 and the ankle portion 1 of each leg 2 by the ankle position calculating means 31.
Floor reaction force for obtaining an estimated value of the floor reaction force acting on each leg 2 by using the data of the position of the body 2 with respect to the center of gravity of the body and the data of the determination result of the motion state of the leg 2 by the leg motion determination means 24. The estimation means 36, the data of the estimated value of the floor reaction force, and the acceleration calculation means 33 for each leg body calculate the acceleration data of the center of gravity of the thigh 9 and the crus 11 of each leg 2 and calculate the angular acceleration of each leg. Thigh 9 and lower leg 1 of each leg 2 by means 34
1 data of the angular acceleration, the data of the estimated position of the floor reaction force acting point by the floor reaction force acting point estimating means 35, and the thigh 9 and the crus 11 of each leg 2 by the leg posture calculating means 28. Each leg 2 using the data of each inclination angle θc and θd
The joint moment estimating means 37 for estimating the moment acting on each of the knee joint 10 and the hip joint 8 is provided.

Next, the operation of this embodiment will be described together with the more detailed processing contents of each means of the arithmetic processing unit 16 described above.

In the present embodiment, for example, when a person 1 exercises the leg 2 such as walking, when a power switch (not shown) of the arithmetic processing unit 16 is turned on with both legs 2 and 2 in the floor, The processing by the arithmetic processing unit 16 is sequentially executed every predetermined cycle time as described below,
The estimated value of the floor reaction force acting on each leg 2 is sequentially obtained.

That is, first, the arithmetic processing unit 16 executes the processing of the leg motion judging means 24. In the processing of the leg motion determining means 24, at each cycle time,
The detection data of the upward acceleration of the waist 3 by the waist vertical acceleration sensor 21 is compared with a predetermined threshold value. Then, when the detected value of the acceleration exceeds the threshold value, the two-leg supporting state as shown in FIG. 1B is started, and the single-leg supporting state as shown in FIG. It is determined that the condition ends. That is, human 1
At the time of walking, when the transition from the single-leg support state to the two-leg support state occurs, the leg 2 on the free leg side is landed, so that the waist portion 3 near the hip joint 8 is relatively large in the upward direction. Acceleration (acceleration that cannot occur in a normal single-leg support state) occurs. Therefore, the leg motion determining means 24 compares the detection data of the upward acceleration of the waist 3 by the waist vertical acceleration sensor 21 with a predetermined threshold value as described above, thereby starting the two-leg supporting state and the single leg. Determine the end of supportive state.

In the processing of the leg motion judging means 24,
Of the estimated values of the floor reaction forces Ff and Fr (see FIG. 1 (b)) acting on each of the two legs 2 and 2 which are obtained by the floor reaction force estimation means 35 in the two-leg supported state as described later, Floor reaction force Fr = ^T (Fr on the leg 2 on the rear side with respect to the traveling direction)
x, Frz) estimated value (specifically, the absolute value of the floor reaction force Fr obtained in the previous cycle time of the arithmetic processing unit 16 = √ (F
rx ² + Frz ² )) is a predetermined threshold value (substantially “0”)
Positive value of). Then, when the absolute value of the estimated value of the floor reaction force Fr falls below the threshold value, it is determined that the both-leg supporting state ends and the single-leg supporting state starts. In the present embodiment, the initial state of motion of the leg 2 is the both-leg support state, and the leg is not supported until the estimated value of the floor reaction force related to one of the legs 2 falls below the threshold value. The body movement determination means 24 determines that the movement state of the leg body 2 is the both-leg support state.

In parallel with the processing of the leg movement determining means 24 as described above, the arithmetic processing unit 16 executes the processing by the chest inclination angle measuring means 25 and the waist inclination angle measuring means 26. In this case, in the processing of the chest inclination angle measuring means 25, a so-called Kalman filter processing is performed based on the longitudinal acceleration of the chest 4 and the detection data of the angular velocity of the chest 4 which are respectively input from the chest longitudinal acceleration sensor 15 and the chest gyro sensor 14. By the known method used, the absolute coordinate system C
The inclination angle θa of the chest 4 at f is sequentially obtained for each cycle time. Similarly, the waist inclination angle measuring means 25
In the processing of 1, the waist 3 in the absolute coordinate system Cf is processed by the Kalman filter from the detection data of the longitudinal acceleration of the waist 3 and the angular velocity of the waist 3 which are respectively input from the waist longitudinal acceleration sensor 20 and the waist gyro sensor 19.
The inclination angle θb of is successively obtained. Here, the inclination angles θ of the chest 4 and the waist 3 in the absolute coordinate system Cf
In the present embodiment, a and θb are, for example, the vertical direction (gravitational direction)
Is the tilt angle with respect to.

It is also possible to obtain the tilt angles of the chest 4 and the waist 3 by integrating the angular velocity detection data from the gyro sensors 14 and 19, for example, but the Kalman filter processing is used as in the present embodiment. As a result, the inclination angles θa and θb of the chest 4 and the waist 3 can be accurately measured.

Next, the arithmetic processing unit 16 executes the processing of the leg posture calculating means 28 and the processing of the reference acceleration measuring means 27.

In the processing by the leg posture calculating means 28, the inclination angles θc and θd of the thigh 9 and the lower leg 11 of each leg 2 in the absolute coordinate system Cf (inclination angles with respect to the vertical direction, see FIG. 2). Is calculated for each cycle time as follows. That is, the inclination angle θ of the thigh 9 of each leg 2
c is the current value of the detection data of the flexion angle Δθc of the hip joint 8 by the hip joint angle sensor 22 attached to the leg 2 and the inclination angle θb of the waist 3 obtained by the waist inclination angle measuring means 25. It is calculated from the current value and the following equation (6).

Θc = θb + Δθc (6)

Here, the inclination angle θb of the waist 3 is negative when the waist 3 is inclined with respect to the vertical direction so that the upper end of the waist 3 projects more toward the front side of the human 1 than the lower end. And the flexion angle Δθc of the hip joint 8 is
This is a positive value when the thigh 9 is inclined with respect to the axis of the waist 3 so that the lower end of the thigh 9 projects toward the front side of the human 1.

Furthermore, the inclination angle θd of the lower leg 11 of each leg 2 is the inclination angle θc of the thigh 9 obtained as described above.
And the current value of the detection data of the flexion angle Δθd of the knee joint 10 by the knee joint angle sensor 23 attached to the leg 2 are calculated by the following equation (7).

Θd = θc−Δθd (7)

Here, the bending angle Δθd of the knee joint 10 is
This is a positive value when the lower leg 11 is inclined to the back side of the thigh 9 with respect to the axis of the thigh 9.

In the processing of the reference acceleration measuring means 27, the acceleration a ₀ = ^T (a ₀ x, a ₀ z) in the absolute coordinate system Cf of the origin O of the body coordinate system Cp is obtained as follows. . That is, when the current value of the detection data of the longitudinal acceleration of the waist 3 by the waist longitudinal acceleration sensor 20 is ap, and the current value of the detection data of the vertical acceleration of the waist 3 by the waist vertical acceleration sensor 21 is aq, Based on the detected data ap and aq and the present value of the inclination angle θb of the waist 3 obtained by the waist inclination angle measuring means 25, the acceleration a ₀ = ^T (a in the absolute coordinate system Cf by the following equation (8). ₀
x, a ₀ z) is required.

A ₀ = ^T (a ₀ x, a ₀ z) = ^T (ap · cos θb−aq · sin θb, ap · sin θb + aq · cos θb−g) (8)

Next, the arithmetic processing unit 16 executes the processing of the center-of-gravity position calculating means 29 of each part, and uses the rigid link model described below to calculate the rigid body equivalent parts of the human 1 in the body coordinate system Cp. The position of the center of gravity (position with respect to the origin of the body coordinate system Cp) is obtained.

As shown in FIG. 4, the rigid body link model R used in this embodiment corresponds to the human body 1 with the rigid bodies R1 and R1 corresponding to the thighs 9 of the legs 2 and the lower leg 11. Rigid body R2,
R2, a rigid body R3 corresponding to the waist 3, and a portion 38 in which the chest 4, the arms 7, 7 and the head 6 are combined (hereinafter, the upper body 3
It is a model that is expressed by connecting with a rigid body R4 corresponding to (8). In this case, each rigid body R1 and rigid body R3
And the connecting portion between the rigid bodies R1 and R2 correspond to the hip joint 8 and the knee joint 10, respectively. Also, rigid body R3
And the rigid body R4 is a tilt fulcrum 39 of the chest 4 with respect to the waist 3.

In the present embodiment, the rigid body equivalent portions of the human 1 (the thighs 9 and the lower legs 11, the waist 3 of each leg 2) corresponding to the respective rigid bodies R1 to R4 of the rigid body link model R as described above. ,
The positions of the respective centers of gravity G1, G2, G3, and G4 of the upper body portion 38) at respective rigid body equivalent portions are obtained in advance and stored in a memory (not shown) of the arithmetic processing unit 16.

Here, the positions of the centers of gravity G1, G2, G3, and G4 of the respective rigid body equivalent portions stored in the arithmetic processing unit 16 are positions in the coordinate system fixed with respect to the respective rigid body equivalent portions. In this case, as the data representing the positions of the centers of gravity G1, G2, G3, and G4 of each rigid body equivalent portion, for example, the distance in the axial direction of the rigid body equivalent portion from the center point of the joint at one end of each rigid body equivalent portion is used. To be Specifically, for example, as shown in FIG. 4, each thigh 9
The position of the center of gravity G1 of the lower thigh 11 is the distance t1 from the center of the hip joint 8 of the thigh 9 in the axial direction of the thigh 9,
The position of G2 is represented as a position of a distance t2 from the center of the knee joint 10 of the lower leg 11 in the axial direction of the lower leg 11, and the values of the distances t1 and t2 are obtained in advance to calculate the processing device. 16 is stored and held. Center of gravity of other rigid body equivalent part, G
The same applies to the positions of 3 and G4.

Strictly speaking, the position of the center of gravity G4 of the body 38 is affected by the movement of the arms 7, 7 included in the body 38, but each arm 7, 7 during walking is Generally has a symmetric positional relationship with respect to the axis of the chest 4, the position of the center of gravity G4 of the body 38 does not change so much, and, for example, the position of the center of gravity G4 of the body 38 in the upright posture is It will be almost the same.

Further, in the present embodiment, the portions corresponding to the respective rigid bodies (the thigh portion 9 and the lower leg portion 11 of each leg 2, the waist portion 3, the upper body portion 3).
In addition to the data indicating the positions of the centers of gravity G1, G2, G3, and G4 in 8), the data of the weight of each rigid body equivalent part and the data of the size of each rigid body equivalent part (for example, the data of the length of each rigid body equivalent part) It is obtained in advance and stored and held in the arithmetic processing unit 16.

The weight of the lower leg portion 11 is the weight including the foot portion 13. Further, as described above, the arithmetic processing unit 16
The data stored and held in advance may be obtained by actual measurement or the like, but may be estimated from the height and weight of the human 1 based on average statistical data of the human. Generally, the position of the center of gravity G1, G2, G3, G4 of each of the rigid body equivalents, weight, and size are correlated with the height and weight of the human, and based on the correlation data, the data of the height and weight of the human From the center of gravity G1, G2, G3, G4
It is possible to estimate the position, weight, and size of the with relative accuracy.

The center-of-gravity position calculating means 29 for each part stores the data stored in advance in the arithmetic processing device 16 as described above, and the chest 4 obtained by the chest inclination angle measuring means 25 and the waist inclination angle measuring means 26, respectively. The current values of the inclination angle θa (= the inclination angle of the upper body portion 38) and the inclination angle θb of the waist portion 3, and the thigh portion 9 and the lower leg portion 11 of each leg 2 obtained by the leg posture calculating means 28. Based on the current values of the respective inclination angles θc and θd of, the center of gravity G1, G2, G3, G4 of each rigid body equivalent portion in the body coordinate system Cp (xz coordinate in FIG. 4) having the origin O fixed to the waist 3 Find the position of.

In this case, since the inclination angles θa to θd of the respective rigid body equivalent portions (the thigh 9 and the lower leg 11, the waist 3, the upper body 38 of each leg 2) are obtained as described above. The position and orientation of each rigid body equivalent part in the body coordinate system Cp can be known from the data of the inclination angles θa to θd and the size data of each rigid body equivalent part. Therefore, the positions of the centers of gravity G1, G2, G3, G4 of the respective rigid body equivalent parts in the body coordinate system Cp can be obtained.

Specifically, referring to FIG. 4, for example, regarding the leg 2 located on the left side of FIG. 4, the inclination angle of the thigh 9 in the body coordinate system Cp (the inclination angle with respect to the z-axis direction).
Is θc (in this case, θc <0 in FIG. 4), the coordinates of the position of the center of gravity G1 of the thigh 9 in the body coordinate system Cp are (t1 · sin θc, −t1 · cos θc). Also,
Since the inclination angle of the lower leg 11 in the body coordinate system Cp is θd (θd <0 in FIG. 4), the coordinate of the position of the center of gravity G2 of the lower leg 11 in the body coordinate system Cp is the length of the thigh 9 Sa is L
Let c be (Lc ・ sinθc + t2 ・ sinθd, −Lc ・ cosθc
−t2 · cos θd). The center of gravity of the thigh portion 9 and the lower leg portion 11 of the other leg body 2, and the center of gravity of the waist portion 3 and the upper body portion 38 are also obtained in the same manner as above.

In this way, the center of gravity position calculating means 2 for each part
9, the center of gravity G of each rigid body equivalent part in the body coordinate system Cp
After obtaining the positions of 1, G2, G3, and G4, the arithmetic processing unit 16
Is executed by the body center-of-gravity position calculation means 30, and the data of the positions of the centers of gravity G1, G2, G3, and G4 of each rigid body equivalent portion and the data of the weight of each rigid body equivalent portion are used in the body coordinate system Cp. The position (xg, zg) of the body center of gravity G0 of the human 1 is calculated.

Here, the position and weight of the center of gravity G3 of the waist 3 in the body coordinate system Cp are (x3, z3), m3, and the position and weight of the center of gravity G4 of the body 38 are (x4, z4), respectively.
m4, the position and weight of the center of gravity G1 of the thigh 9 of the left leg 2 toward the front of the human 1 (x1L, z1L), m1L,
The position and weight of the center of gravity G2 of the lower leg 11 of the leg 2 are (x2L, z2L), m2L, and the center of gravity G of the thigh 9 of the right leg 2 respectively.
The position and weight of 1 are (x1R, z1R), m1R, and the position and weight of the center of gravity G2 of the lower leg 11 of the leg 2 (x2R)
R, z2R), m2R, weight of human 1 is M (= m1L + m2L + m1R +)
m2R + m3 + m4), human 1 in the body coordinate system Cp
The position (xg, zg) of the body center of gravity G0 of is calculated by the following equation (9).

[0068]     xg = (m1L ・ x1L ＋ m1R ・ x1R ＋ m2L ・ x2L ＋ m2R ・ x2R                   ＋ m3 ・ x3 ＋ m4 ・ x4) / M     zg = (m1L ・ z1L ＋ m1R ・ z1R ＋ m2L ・ z2L ＋ m2R ・ z2R                   ＋ m3 ・ z3 ＋ m4 ・ z4) / M …… (9)

In this way, the body center of gravity position calculating means 30
After executing the processing of 1, the arithmetic processing unit 16 further executes the processing of the body center-of-gravity acceleration calculation means 32 and the ankle position calculation means 31.

In this case, in the processing of the body center-of-gravity acceleration calculating means 32, first, the time series of the position (xg, zg) of the body center of gravity G0 in the body coordinate system Cp obtained by the body center of gravity position calculating means 30 for each cycle time. Using the data,
The second derivative of the position (xg, zg) of the body center of gravity G0 in the body coordinate system Cp, that is, the acceleration ^T (d ² xg / dt ² , d ² zg / dt of the body center of gravity G 0 with respect to the origin O of the body coordinate system Cp. ² ) is required. And this acceleration ^T (d ² xg / dt ² , d ² zg / dt ² )
And the acceleration a ₀ = in the absolute coordinate system Cf of the origin O of the body coordinate system Cp obtained by the reference acceleration measuring means 27.
By obtaining the vector sum with ^T (a ₀ x, a ₀ z), the acceleration a = ^T (ax, az) of the body center of gravity G0 in the absolute coordinate system Cf is obtained.

In the processing of the ankle position calculating means 31, first, the inclination angles θc and θd of the thigh 9 and the lower leg 11 of each leg 2 obtained by the leg posture calculating means 28 are calculated. And the current value of the data of the inclination angle θb of the waist 3 obtained by the waist inclination angle measuring means 26, and the size (length) data of the thigh 9 and the lower leg 11. From the above, the position of the ankle portion 12 of each leg 2 in the body coordinate system Cp is obtained by the same processing as the processing of the center-of-gravity position calculating means 29. Specifically, referring to FIG. 4, regarding the leg 2 located on the left side of FIG. 4, the length of the lower leg portion 11 (the length from the center of the knee joint 10 to the ankle portion 12) is defined as Ld. Then, the coordinates (x12, z12) of the position of the ankle 12 in the body coordinate system Cp are (Lc · sin
θc + Ld · sin θd, −Lc · cos θc−Ld · cos θd) (however, in FIG. 4, θc <0, θd <0). The same applies to the other leg 2.

Then, the body coordinate system Cp of the ankle portion 12 is
Position (x12, z12) and the body center of gravity G in the body coordinate system Cp obtained by the body center of gravity position calculating means 30.
From the current value of the data at position 0 (xg, zg), the position vector ^T (x12 of the ankle 12 of each leg 2 with respect to the body center of gravity G0
−xg, z12−zg), that is, Δ in the above formula (5)
Xf, ΔZf, ΔXr, and ΔZr are obtained.

Next, the arithmetic processing means 16 executes the processing of the floor reaction force estimating means 36 as follows. That is, in this processing, when the motion state of the leg 2 judged by the leg motion judging means 24 at the current cycle time is the single-leg support state, the weight M of the human 1 and the gravitational acceleration g.
Value (these are stored in advance in the arithmetic processing unit 16) and the acceleration a of the body center of gravity G0 in the absolute coordinate system Cf obtained by the body center of gravity acceleration calculating means 32.
= From the current value of ^T (ax, az), according to the equation (2),
Floor reaction force F = ^T (Fx, Fz) acting on the grounded leg 2
The estimated value of is obtained. In this case, the floor reaction force acting on the leg 2 on the non-grounded side (leg 2 on the free leg side) is ^T (0,0).
Is.

When the leg body 2 is in the two-leg supporting state as judged by the leg body movement judging means 24 in the current cycle time, the weight M and the gravitational acceleration g of the human 1 and the center of gravity of the body are measured. Acceleration of the body center of gravity G0 in the absolute coordinate system Cf obtained by the acceleration calculating means 32 a = ^T (a
x, az) and the current value data of the current value of the position of the ankle portion 12 of each leg 2 with respect to the body center of gravity G0 obtained by the ankle position calculating means 31 (ΔXf, ΔZf, ΔX in the equation (5)).
From the current values of r and ΔZr data), the floor reaction force Ff = ^T (Ffx, Ffz), Fr = for each leg 2 is calculated by the equation (5).
An estimated value of ^T (Frx, Frz) is obtained.

On the other hand, the arithmetic processing unit 16 carries out parallel processing of the body center of gravity position calculating means 30, body center of gravity acceleration acceleration calculating means 32, ankle position calculating means 31, and floor reaction force estimating means 36 as described above. Acceleration calculating means 3 for each part of the leg
3. The processes of the angular acceleration calculating means 34 for each leg and the floor reaction force acting point estimating means 35 are executed.

In this case, the acceleration calculating means 3 for each part of the leg
In the processing of No. 3, similarly to the processing of the body center-of-gravity acceleration calculation means 32, first, the large body equivalent portion of each leg 2 in the body coordinate system Cp obtained by the respective portion center-of-gravity position calculation means 29 for each cycle time is large. Thigh 9 and Lower Thigh 11
The respective second-order differential values of the positions of the centers of gravity G1 and G2 of the thigh 9 and the lower leg 11 in the body coordinate system Cp using the time series data of the positions of the centers of gravity G1 and G2 of
Center of gravity G of thigh 9 and lower leg 11 in body coordinate system Cp
The respective accelerations of 1 and G2 (acceleration with respect to the origin O of the body coordinate system Cp) are obtained. Then, by obtaining the vector sum of each acceleration and the acceleration a ₀ = ^T (a ₀ x, a ₀ z) in the absolute coordinate system Cf of the waist 3 by the reference acceleration measuring means 27, the absolute coordinate system Cf The respective accelerations (more specifically, the coordinate components in the absolute coordinate system Cf of the accelerations) of the thigh 9 and the lower leg 11 at are calculated.

Further, the angular acceleration calculating means 34 for each part of the leg body
In the processing, the time series data of the inclination angles θc and θd of the thigh 9 and the lower leg 11 of each leg 2 obtained by the leg posture calculating means 28 at each cycle time is used to Second-order differential values of the inclination angles θc and θd of the thigh 9 and the lower leg 11, that is, the angular accelerations of the thigh 9 and the lower leg 11 are obtained.

Further, in the processing of the floor reaction force action point estimating means 35, the inclination angle θc of the thigh 9 of the leg 2 which is in contact with the ground is calculated by the leg posture calculating means 28, for example.
From this value, the floor reaction force acting point (foot) from the ankle portion 12 of the leg body 2 to the foot portion 13 of the leg body 2 based on a predetermined correlation as shown in FIGS. 5 and 6. Flat part 13
To the point where all floor reaction forces acting on the ground contact point are concentrated (position vector of the floor reaction force action point with respect to the ankle portion 12. Hereinafter, referred to as floor reaction force action point vector) is the floor reaction force action point. Is obtained as data representing the position of.

That is, according to the knowledge of the inventor of the present application, the inclination angle θc of the thigh 9 of the leg 2 which is in contact with the ground and the knee joint 1
The bending angle Δθd of 0 has a relatively significant correlation with the floor reaction force acting point, and for example, with respect to the inclination angle θc of the thigh 9,
The floor reaction force action point vector, specifically, the component of the floor reaction force action point vector in the traveling direction (X axis direction) of the human 1 and the component of the floor reaction force action point vector in the vertical direction (Z axis direction). And change as shown in FIGS. 5 and 6, respectively. Here, the negative inclination angle θc of the thigh 9 is equal to the leg 2
Is an angle when the thigh portion 9 is inclined with respect to the axis of the waist portion 3 so as to extend to the rear side of the human 1 (for example, the leg 2 on the right side toward the front of the human 1 in FIG. 2). , The positive inclination angle θc is set so that the leg 2 is on the front side of the human 1
Is inclined with respect to the axis of the waist 3 (see, for example, FIG.
This is the angle of the left leg 2) facing the front of the human 1.

Therefore, in the present embodiment, an approximate expression having the inclination angle θc of the thigh 9 as a parameter, which represents the correlation between FIGS. 5 and 6, is created, and this approximate expression is calculated by the arithmetic processing unit 1.
6 is stored and held in advance. Then, in the processing of the floor reaction force action point estimating means 35, the current value of the inclination angle θc of the thigh 9 obtained by the leg posture calculating means 28 is substituted into the above-mentioned approximate expression to obtain the floor reaction force. The action point vector (specifically, the components of the floor reaction force action point vector in the X-axis direction and the Z-axis direction) is obtained.

Here, as shown in FIGS. 5 and 6, the thigh 9
In the correlation in which the inclination angle θc of has a minimum value, the thigh 9
Even if the inclination angle θc is the same, the value of the floor reaction force action point vector is different in the decreasing process and the increasing process of the inclination angle θc. Therefore, in the present embodiment, when the above approximate expression is created, the inclination angle θc of the thigh 9 is calculated as a positive value from the transition of the correlation from the heel of the foot 13 to the floor of the toe. And a first phase (a1 phase in FIG. 5, b1 phase in FIG. 6)
The second phase (the phase of a2 in FIG. 5) in which the inclination angle θc of the thigh 9 is negative and the change speed of the inclination angle θc of the thigh 9, that is, the inclination angular velocity of the thigh 9 is negative. , The phase of b2 in FIG. 6) and the third phase in which the inclination angle θc of the thigh 9 is negative and the inclination angular velocity of the thigh 9 is positive (phase of a3 in FIG. 5, FIG. 6). b3 phase), and each phase is approximated by the same or different function for each of the X-axis direction component and the Z-axis direction component of the floor reaction force action point vector. The first and second phases a1, in the correlation of FIG.
The approximate expression of the phase combining a2 is, for example, px = x ₁ · θc ⁶ + x ₂ · θc ⁵ + x ₃ · θc ⁴ + x ₄ · θc ³ + x, where the X-axis direction component of the floor reaction force action point vector is px ₅
It is represented by a sixth-order polynomial function of the form θc ² + x ₆ · θc + x ₇ (x _{1 to} x ₇ are constant values). In addition, the third phase a3 in the correlation of FIG.
The approximate expression is represented by, for example, ^{_{px = x 8 · θc 4 +}} x 9 · θc 3 + x 10 · θc 2 + x 11 · θc + x in the form of ₁₂ fourth order polynomial functions (x ₈ ~x ₁₂ is a constant value) .

Further, the approximate expression of the phase obtained by combining the first and second phases b1 and b2 in the correlation of FIG. 6 is, for example, pz = z _{1 where} pz is the component of the floor reaction force action point vector.・ Θc ⁶ + z ₂・ θc ⁵ + z ₃・ θc ⁴ + z ₄・ θc ³ + z ₅・
It is represented by a 6th-order polynomial function of the form θc ² + z ₆ · θc + z ₇ (z _{1 to} z ₇ are constant values). In addition, the third phase b3 in the correlation of FIG.
The approximate expression of is expressed by, for example, a cubic polynomial function in the form of pz = z ₈ · θc ³ + z ₉ · θc ² + z ₁₀ · θc + z ₁₁ (z _{8 to} z ₁₁ are constant values).

When obtaining the floor reaction force action point vector, the positive / negative of the inclination angle θc of the thigh 9 is identified, and
The positive or negative of the tilt angular velocity of the thigh 9 calculated by the first derivative of the time series data of the tilt angle θc of the thigh 9 is identified. Furthermore, it is determined which phase the current phase is in from the positive / negative of the identified tilt angle θc and the positive / negative of the tilt angular velocity, and the present value of the tilt angle θc of the thigh 9 is added to the approximate expression of the determined phase. The floor reaction force action point vector is calculated by substituting.
Thereby, the value of the floor reaction force action point vector in the process of decreasing the inclination angle θc of the thigh 9 and the value of the floor reaction force action point vector in the process of increasing can be calculated separately.

The position of the floor reaction force acting point has a correlation with the flexion angle of the knee joint 10 of the leg 2 that is in contact with the ground, and instead of the inclination angle θc of the thigh 9, the knee joint angle is used. Sensor 23
The position of the floor reaction force acting point may be estimated from the bending angle Δθd of the knee joint 10 measured by
The position of the floor reaction force acting point may be estimated by a map or the like using both the inclination angle θc of the thigh 9 and the bending angle Δθd of the knee joint 10.

Further, when the human 1 is sitting on the chair or standing up from the state of sitting on the chair, FIG. 7 (when sitting on the chair) is shown between the position of the floor reaction force acting point and the bending angle Δθd of the knee joint 10. , The correlation shown in FIG. 8 (when standing up) is established,
When going up and down the stairs, FIG. 9 (when climbing the stairs) and FIG. 10 (when descending the stairs) are established between the position of the floor reaction force acting point and the inclination angle θc of the thigh 9. Therefore, when sitting or standing up on a chair, the position of the floor reaction force acting point can be estimated from the bending angle Δθd of the knee joint 10 based on the correlation of FIGS. 7 and 8, and when going up and down the stairs, Thigh 9
The position of the floor reaction force acting point can be estimated based on the correlation of FIGS.

When the position of the floor reaction force acting point is estimated as described above, the arithmetic processing unit 16 then executes the processing of the joint moment estimating means 37 to make the knee joint 10 and the hip joint 8 of each leg 2. Find the moment that acts on. This processing is obtained by the floor reaction force estimation means 36, the leg portion acceleration calculation means 33, the leg portion angular acceleration calculation means 34, the floor reaction force action point estimation means 35, and the leg posture calculation means 28, respectively. This is done on the basis of the so-called inverse dynamics model, using the current values of the data. This inverse dynamic model is for human 1
Using the equation of motion regarding the translational motion of each rigid body equivalent part and the equation of motion related to the rotational motion, the moments acting on the joints are sequentially obtained from the joints closer to the floor reaction force action point. Knee joint 1 of each leg 2
0, the moment acting on the hip joint 8 is sequentially obtained.

More specifically, referring to FIG. 11, first, regarding the lower leg 11 of each leg 2, the force (joint reaction force) acting on the ankle 12 at the tip of the lower leg 11, The force (joint reaction force) acting on the knee joint 10 portion of the thigh 11 and the translational acceleration of the center of gravity G2 of the lower leg 11 are respectively expressed in the absolute coordinate system C.
According to the component notation in f, ^T (F ₁ x, F ₁ z), ^T (F ₂ x,
F ₂ z) and ^T (a ₂ x, a ₂ z), and the weight of the lower leg 11 is m ₂ . At this time, the equation of motion regarding the translational movement of the center of gravity G2 of the lower leg 11 is given by the following equation (10).

^T (m ₂ · a ₂ x, m ₂ · a ₂ z) = ^T (F ₁ x−F ₂ x, F ₁ z−F ₂ z−m ₂ · g) Therefore, ^T (F ₂ x , F ₂ z) = ^T (F ₁ x−m ₂ · a ₂ x, F ₁ z−m ₂ · a ₂ z−m ₂ · g) (10)

Here, the acceleration ^T (a ₂
x, a ₂ z) is obtained by the acceleration calculating means 33 for each leg. The joint reaction force ^T (F ₁ x, F ₁ z) acting on the ankle 12 at the tip of the lower leg 11 is approximately the same as the floor reaction of the leg 2 having the lower leg 11. It is equal to the estimated value of the floor reaction force obtained by the force estimating means 36.
More specifically, when the leg 2 is in contact with the ground in the single-leg supporting state, the joint reaction force ^T (F ₁ x, F ₁ z) is the floor reaction force ^T (Fx , Fz), and when the leg 2 is the leg on the free leg side, ^T (F ₁ x, F ₁ z) = ^T
It is (0,0). When the leg 2 is a leg on the rear side in the forward direction of the human 1 in the both-leg support state, the joint reaction force ^T (F ₁ x, F ₁ z) is expressed by the equation (5). Is the floor reaction force ^T (Frx, Frz), and when the leg 2 is the front leg, it is the floor reaction force ^T (Ffx, Ffz) in the equation (5).

Therefore, the joint reaction force ^T (F ₂ x, F ₂ z) acting on the knee joint 10 of each leg ₂ is calculated by the acceleration calculating means 3 of each leg.
Of the crus 11 determined by the three center of gravity G2 of the acceleration ^T (a
₂ x, a ₂ z) data, the floor reaction force (= ^T (F ₁ x, F ₁ z)) data obtained by the floor reaction force estimating means 36, and the lower leg 1
From the data of the weight m ₂ obtained in advance and the value of the gravitational acceleration g, the value is obtained by the above equation (10).

Referring to FIG. 11, the moment acting on the ankle 12 at the tip of the lower leg 11 is M ₁ , the moment acting on the knee joint 10 of the lower leg 11 is M ₂ ,
The moment of inertia about the center of gravity G2 of the lower leg 11 is I _G2 , and the angular acceleration about the center of gravity G2 of the lower leg 11 is α ₂ . Corresponding to FIG. 4, the distance between the center of gravity G2 of the lower leg 11 and the center of the knee joint 10 is t2, and the distance between the center of gravity G2 of the lower leg 11 and the ankle 12 is t2 ′. (= Ld-t2),
The equation of motion regarding the rotational movement of the lower leg 11 around the center of gravity G2 is given by the following equation (11).

I _G2 · α ₂ = M ₁ −M ₂ + F ₁ x · t2 ′ · cos θd−F ₁ z · t2 ′ · sin θd + F ₂ x · t2 · cos θd−F ₂ z · t2 · sin θd Therefore M ₂ = M ₁ −I _G2・ α ₂ + F ₁ x ・ t2 '・ cos θd−F ₁ z ・ t2' ・ sin θd + F ₂ x ・ t 2 ・ cos θd−F ₂ z ・ t2 ・ sin θd …… (11)

Here, M ₁ in the equation (11) is the same as the equation (1
1) The floor reaction force action point vector obtained by the floor reaction force action point estimating means 35 for the leg 2 having the lower leg 11 and the floor reaction force estimating means 36 for the leg 2
Is a moment obtained as an outer product (vector product) with the floor reaction force vector obtained by. Also, α ₂ is
It is the angular acceleration of the lower leg 11 obtained by the angular acceleration calculation means 34 for each leg. Further, θd is an inclination angle of the lower leg 11 calculated by the leg posture calculating means 28. Further, ^T (F ₁ x, F ₁ z) is an estimated value of the floor reaction force obtained by the floor reaction force estimation means 36 as described above. Furthermore, ^T (F ₂ x, F ₂ z) is obtained by the above equation (10). In addition, the moment of inertia I _G2 is the lower leg 11
It is obtained in advance and stored in the arithmetic processing unit 16 together with the data of the weight m ₂ and the size thereof.

Therefore, the moment acting on the knee joint 10
M ₂ is the data of the estimated value of the floor reaction force by the floor reaction force estimation means 36, the data of the floor reaction force action point vector by the floor reaction force action point estimation means 35, and the lower value by the leg acceleration calculation means 34. Data of the angular acceleration α ₂ of the thigh 11 and data of the inclination angle θd of the lower leg 11 calculated by the leg posture calculating means 28;
Joint reaction force ^T (F ₂ x, F ₂ z) obtained by the above equation (10)
And the data of the moment of inertia I _G2 of the lower leg 11, the size (Ld), and the position (t2) of the center of gravity G2, which are obtained in advance, by the above equation (11).

The joint moment estimating means 37 obtains the moment M ₂ acting on the knee joint 10 portion of the lower leg 11 as described above, and then performs the same process as the calculation process to calculate the thigh 9 The moment acting on the hip joint 8 is obtained. The basic idea of this processing is the same as the method of obtaining the moment M ₂ of the knee joint 10, so detailed illustration and description thereof will be omitted, but the outline thereof is as follows.

That is, first, the center of gravity G1 of the thigh 9 (see FIG.
The following equation (1
2) (equation of the same form as the equation (10)), the thigh 9
The joint reaction force ^T (F ₃ x, F ₃ z) that acts on the hip joint 8 portion is calculated.

^T (F ₃ x, F ₃ z) = ^T (F ₂ x−m ₁ · a ₁ x, F ₂ z−m ₁ · a ₁ z−m ₁ · g) (12)

Here, ^T (F ₂ x, F ₂ z) is the joint reaction force of the knee joint 10 previously obtained by the equation (10). Further, ^T (a ₁ x, a ₁ z) is the acceleration calculating means 33 for each part of the leg.
Is the acceleration (translational acceleration) of the center of gravity G1 of the thigh 9 in the absolute coordinate system Cf. Further, m ₁ is the weight of the thigh 9 which is obtained in advance, and g is the gravitational acceleration.

Next, the portion of the hip joint 8 of the thigh 9 is calculated by the following equation (13) (equal to the above equation (11)) based on the equation of motion related to the rotational movement of the thigh 9 around the center of gravity G1. The moment M ₃ acting on is calculated.

M ₃ = M ₂ −I _G1 · α ₁ + F ₂ x · t1 ′ · cos θc−F ₂ z · t1 ′ · sin θc + F ₃ x · t1 · cos θc−F ₃ z · t1 · sin θc (13 )

Here, M2 is the moment of the knee joint 10 obtained by the equation (11), and ^T (F ₂ x, F ₂ z) is the joint of the knee joint 10 obtained by the equation (10). Reaction force,
^T (F ₃ x, F ₃ z) is the joint reaction force of the hip joint 8 obtained by the equation (12), and I _G1 is the thigh portion 9 obtained in advance.
Of inertia about the center of gravity G1 of the thigh, α ₁ is the angular acceleration of the thigh 9 calculated by the angular acceleration calculating means 34 for each leg, and θc is the inclination of the thigh 9 calculated by the leg posture calculating means 28. It is an angle. Also, t1 is the distance from the center of the hip joint 8 to the center of gravity G1 of the thigh 9 (see FIG. 4), t1
1'is the distance from the center of the knee joint 10 to the center of gravity G1 of the thigh 9 (Lc-t1 in FIG. 4), and these are the position of the center of gravity G1 and the size of the thigh 9 (long Sa)).

The processing described above is the same as that of the arithmetic processing unit 1.
It is sequentially executed every 6 cycle times, and the floor reaction force acting on each leg 2 and the moment acting on the knee joint 10 and the hip joint 8 of each leg 2 are sequentially estimated in real time.

Although detailed description will be omitted in the present specification, the estimated values of the moments of the knee joint 10 and the hip joint 8 obtained are, for example, a device for assisting the walking of the human 1 (knee joint 1
0 or a device including an electric motor or the like that can apply an auxiliary torque to the hip joint 8).

12 to 14 show the changes over time in the estimated value of the floor reaction force (specifically, the absolute value of the estimated value of the floor reaction force) obtained by the processing of the arithmetic processing unit 16 described above. To illustrate. A solid line in FIG. 15 illustrates how the estimated values of the moments of the knee joint 10 and the hip joint 8 obtained by the processing of the arithmetic processing unit 16 change with time. Here, FIG.
2 and 15 are examples when the human 1 walks on a flat ground at a substantially constant speed, FIG. 13 is an example when the human 1 climbs stairs, and FIG. 14 is a state in which the human 1 stands up from a state of sitting on a chair. It is an example of the case. In this case, in FIGS.
A comparative example (equivalent to the true value of the floor reaction force) in which the floor reaction force is actually measured using a force meter or the like is also shown by a virtual line. Further, in FIG. 15, a knee joint 1 is used by using a torque meter or the like.
Comparative example in which the moments of 0 and the hip joint 8 were measured (corresponding to the true values of the moments of the knee joint 10 and the hip joint 8)
Are also shown in phantom lines.

As apparent from FIGS. 12 to 14, according to the present embodiment, an accurate estimated value of the floor reaction force can be obtained regardless of the motion form of the leg 2 and the motion environment. It is understood that there is. Further, in the present embodiment, by using the estimated value of the floor reaction force and the estimated position of the floor reaction force action point, as shown in FIG. 15, the moments of the knee joint 10 and the hip joint 8 are relatively accurately estimated. be able to.

As described above, according to the present embodiment, the hip joint can be provided without disturbing the walking of the human body 1 on the leg body 2 or mounting a sensor that imposes a burden on the motion of the leg body 2. 8 and the angle sensors 22, 23 attached to the hip joint 8,
The relatively small and lightweight sensors such as the gyro sensors 14 and 19 and the acceleration sensors 15, 20 and 21 mounted on the body 5 are used to determine the floor reaction force acting on each leg 2 and the position of the floor reaction force application point. It can be estimated, and the moment acting on the hip joint 8 and the knee joint 10 of each leg 2 can be easily estimated in real time from this estimated value. Moreover, the estimation can be performed relatively accurately regardless of the movement form or the movement environment of the leg 2 such as walking on a flat ground or walking on stairs.

In the above-described embodiments, the case where the present invention is applied to the human 1 has been described as an example, but the present invention can also be applied to a bipedal walking robot as a bipedal walking vehicle. Here, in a bipedal walking robot, the waist and the chest may have an integrated structure, but in this case, a gyro sensor and a longitudinal acceleration sensor are provided on only one of the waist and the chest. It is also possible to attach it and to estimate the floor reaction force and the moment of the joint of the leg like the present embodiment. Further, in the bipedal robot, the bending angles of the hip joint and the knee joint can be grasped by the control amount of the control device for the actuators of those joints.

Further, in the above embodiment, the detection data of the waist vertical acceleration sensor 21 is used as it is in order to judge the motion state of the leg 2, but instead of the detection data,
For example, the value of the component in the vertical direction (Z-axis direction) of the acceleration a ₀ of the waist 3 in the absolute coordinate system Cf obtained by the reference acceleration measuring means 27 may be used.

[Brief description of drawings]

FIG. 1 is a diagram for explaining the basic principle of a floor reaction force estimation method.

FIG. 2 is a diagram schematically showing a human being as a bipedal walking body and a device configuration equipped to the human being according to an embodiment of the present invention.

FIG. 3 is a block diagram for explaining a function of an arithmetic processing unit included in the device of FIG.

FIG. 4 is a diagram showing a rigid link model used in the processing of the arithmetic processing unit shown in FIG. 3;

FIG. 5 is a diagram showing a correlation between a traveling direction component of a floor reaction force action point vector and a tilt angle of a thigh during normal walking.

FIG. 6 is a diagram showing a correlation between a vertical component of a floor reaction force action point vector and a thigh inclination angle during normal walking.

FIG. 7 is a diagram showing a correlation between a traveling direction component of a floor reaction force action point vector and a flexion angle of a knee joint when sitting on a chair.

FIG. 8 is a diagram showing the correlation between the advancing direction component of the floor reaction force action point vector and the flexion angle of the knee joint when the chair stands up.

FIG. 9 is a diagram showing a correlation between a traveling direction component of a floor reaction force action point vector and a thigh inclination angle when climbing stairs.

FIG. 10 is a diagram showing a correlation between a traveling direction component of a floor reaction force action point vector and a thigh inclination angle when descending stairs.

FIG. 11 is a diagram for explaining the process in the joint moment estimating means of the arithmetic processing device in FIG.

FIG. 12 is a graph exemplifying how the estimated value of the floor reaction force during normal walking obtained by the embodiment of the present invention changes with time.

FIG. 13 is a graph exemplifying how the estimated value of the floor reaction force when climbing the stairs, which is obtained according to the embodiment of the present invention, changes with time.

FIG. 14 is a graph exemplifying how the estimated value of the floor reaction force when the chair stands up changes with time, which is obtained according to the embodiment of the present invention.

FIG. 15 is a graph exemplifying how the estimated values of the moments of the knee joint and the hip joint obtained by the embodiment of the present invention change with time.

[Explanation of symbols]

1 ... Human (bipedal walking body), 2 ... Leg, 8 ... Hip joint,
10 ... Knee joint, 12 ... Ankle part (specific site), 13 ... Foot part, 22 ... Hip joint angle sensor, 23 ... Knee joint angle sensor.

   ─────────────────────────────────────────────────── ─── Continued front page    F term (reference) 2F069 AA03 AA71 BB04 DD15 DD27                       GG41 GG59 NN02 NN06 NN15                       NN16                 3C007 AS36 CS08 KS20 KS24 KS34                       WA03 WA13

Claims (3)

[Claims] 1. A method for successively estimating the position of a floor reaction force acting point on each leg of a bipedal walking body, comprising at least the inclination angle of the thigh of each leg and the bending angle of the knee joint. One of them is set as the measurement target angle, and the measurement target angle is sequentially measured during the movement of the bipedal walking body, and is established between the position vector of the floor reaction force acting point on the ankle of each leg and the measurement target angle. A floor reaction force action point estimation method for a bipedal walking body, characterized in that the position vector is sequentially estimated from a measured value of a measurement target angle based on a predetermined correlation. 2. An approximate expression that represents the correlation and that has the measurement target angle as a parameter is created and stored, and the position vector is calculated by substituting the measured value of the measurement target angle into the approximation expression. At the same time, when creating the approximate expression, the transition of the correlation from the heel of the foot of each leg to the time when the toes leave the floor is divided into several phases, and each phase is the same or The method for estimating a floor reaction force action point of a bipedal walking body according to claim 1, wherein the method is approximated by different functions. 3. A rate of change of at least the angle to be measured in dividing the transition of the correlation into several phases from the time when the heel of the foot of each leg is landed to the time when the toe is removed from the floor. The floor reaction force action point estimating method for a bipedal walking body according to claim 2, wherein the phases are divided according to the positive or negative of.