JP4291092B2 - Method for estimating joint moments of biped walking objects - Google Patents

Description

  The present invention relates to a method for estimating a moment (joint moment) acting on a joint of each leg of a biped walking moving body such as a human or a biped walking robot.

  For example, when performing motion control of a walking assist device that assists a human walking motion, it is necessary to grasp a joint moment that actually acts on a joint of a human leg. By grasping the joint moment, it is possible to appropriately determine the target assisting force of the walking assist device. Further, even in a biped walking robot, it may be necessary to appropriately grasp the joint moment actually acting on each joint of the leg in order to control its operation.

  Therefore, the applicant of the present application has previously proposed a method for estimating the joint moment of the leg of a biped walking moving body such as a human being, for example, in Japanese Patent Laid-Open No. 2003-89083 (Patent Document 1). In this method, the displacement amount (rotation angle) of each joint of the leg of the biped walking moving body, the acceleration and angular velocity of a predetermined part are measured using a required sensor, and the measurement data and the biped walking moving body are measured. The floor reaction force vector acting on each leg and the position of the point of action are estimated using a rigid link model or the like. Here, the rigid link model is a model that expresses the structure of the biped walking moving body as a connected body formed by connecting a plurality of rigid elements by a plurality of joint elements, and the position of the overall center of gravity of the biped walking moving body. Also, estimate the position / posture of rigid body equivalent parts (thighs, lower legs, hips, etc.) and joints (knee joints, hip joints, etc.) of the biped walking moving body corresponding to each rigid body element and each joint element. It is used as a basis for a model that describes the dynamic behavior of a bipedal mobile body. Each rigid element of the rigid link model is preliminarily preset with its weight and length, and the position of the center of gravity (position on each rigid element).

  And in the thing of the said patent document 1, using the estimated floor reaction force vector, the position of its action point, and a rigid body link model, the knee joint of each leg and the joint of a hip joint by the arithmetic processing based on an inverse dynamics model The moment is estimated. In general, an inverse dynamics model estimates the reaction force and moment, which are internal forces of the object, with the external force acting on the object and position information as known (with the external force and position information as input parameters). This is a dynamic model for expressing the relationship between the motion of the object (position time-series pattern) and the force and moment acting on the object. In the method of Patent Document 1, the inverse dynamic model is constructed based on a motion equation relating to the motion (translational motion and rotational motion) of each rigid element of the rigid link model, and the joint moment of each leg is determined as the floor reaction force. It is estimated in order from the joint side closer to the vector action point.

  By the way, when estimating a joint moment like the thing of the patent document 1, it is necessary to describe a floor reaction force vector, its action point, and an inverse dynamics model with a certain one coordinate system. And in the thing of patent document 1, the absolute coordinate system fixed to the floor was used as the coordinate system.

  When the absolute coordinate system is used in this way, an inclination angle in an absolute coordinate system of a predetermined portion of the biped walking moving body (specifically, a rigid body corresponding portion corresponding to a rigid body element having a rigid link model), for example, a vertical Each part of a biped walking moving body that is grasped as a relative position / posture of a biped walking moving body with respect to a reference part while grasping an inclination angle with respect to a direction (equivalent to a rigid body corresponding to each rigid element of the rigid link model) Part)) must be converted to values in the absolute coordinate system. Also, when calculating the floor reaction force vector and the joint moment, it is necessary to estimate the position of the center of gravity of each fuselage element of the rigid link model in the absolute coordinate system. Arithmetic processing (coordinate conversion processing) is also required. Accordingly, a large number of arithmetic processes such as coordinate conversion using the tilt angle are required.

In this case, as a method for measuring an inclination angle with respect to a vertical direction of a predetermined part of a biped walking moving body, a method of integrating a detection value of a gyro sensor for detecting an angular velocity, a so-called combination of the gyro sensor and an acceleration sensor. A method of estimating the tilt angle by a Kalman filter method or a method of directly detecting the tilt angle by a pendulum type inclinometer is generally known. However, in any of these methods, accuracy that can sufficiently satisfy the tilt angle with respect to the vertical direction is generally achieved due to the accumulation of errors due to the integration of the detected value of the gyro sensor and the inertial acceleration during the movement of the biped walking moving body. It is often impossible to measure with. Therefore, it is generally difficult to grasp the inclination angle of a predetermined part of the biped walking moving body in the absolute coordinate system with high accuracy. For this reason, in the method of Patent Document 1, which requires a large number of calculation processes such as coordinate conversion using the inclination angle as described above, calculation errors are likely to accumulate, which hinders improvement in joint moment estimation accuracy. It was.
JP 2003-89083 A

  The present invention has been made in view of such a background, and it is possible to improve the estimation accuracy of the joint moment of the leg by minimizing the arithmetic processing using the tilt information with respect to the direction of gravity of the biped walking moving body as much as possible. It is an object of the present invention to provide a joint moment estimation method for a biped walking mobile body that can be used.

The joint moment estimating method for a biped walking mobile body of the present invention includes a biped walking mobile body, and a plurality of rigid elements include joint elements corresponding to at least the hip joint and knee joint of each leg of the biped walking mobile body. A method of estimating a joint moment acting on at least one joint of each leg of the biped walking moving body using a rigid body link model expressed as a connected body connected by a plurality of joint elements, the rigid body link A first step of sequentially grasping the displacement amount of each joint of the biped walking moving body corresponding to each joint element of the model, and a coordinate system fixed in advance to a predetermined one rigid body element of the rigid body link model A second step of sequentially grasping at least the value of the acceleration vector at the origin of the body coordinate system using the output of an acceleration sensor attached to the biped walking moving body, A third step of sequentially grasping a value in the body coordinate system of a floor reaction force vector acting on each leg of the foot-walking moving body; and a position vector of an action point of the floor reaction force vector in the body coordinate system The fourth step of sequentially grasping the value, the amount of displacement of each joint of the biped walking mobile body grasped in the first to fourth steps, the value of the acceleration vector at the origin of the body coordinate system, and the floor reaction force vector Inverse power that expresses the relationship between the motion of each rigid element of the rigid link model and the translational force and moment acting on the rigid element using the body coordinate system using the value and the value of the position vector of the action point Rutotomoni comprising a fifth step of sequentially estimating joint moments acting on at least one of the joints of each leg of the biped walking mobile body on the basis of the academic models,
The position vector of the total center of gravity of the biped walking moving body is sequentially grasped using the displacement amount of each joint of the biped walking moving body obtained in the first step and the rigid link model. And the time series data of the position vector value of the entire center of gravity and the acceleration vector value of the origin of the body coordinate system grasped in the second step, the value of the acceleration vector of the entire center of gravity in the body coordinate system And the biped walking mobile body is in a single-leg support state in which only one of the pair of legs is grounded, or both legs are grounded. An eighth step of sequentially determining whether or not both legs are supported; and a ninth step of sequentially grasping an inclination angle with respect to a vertical direction of a rigid body equivalent part of a biped walking moving body corresponding to a rigid element to which the body coordinate system is fixed. Step and 2 A tenth step of sequentially determining whether or not each leg of the walking moving body is in contact with the ground; and a value in the body coordinate system of the position vector of the ankle joint of each of the grounded legs The displacement amount of each joint of the biped walking moving body obtained by the first step and the value of the position vector of the middle foot joint joint of the foot of the leg in the body coordinate system, the rigid link model, 11 and the position of the position of the overall center of gravity grasped in the sixth step and the ankle joint of each grounded leg grasped in the eleventh step and the leg joint Based on the value of each position vector of the metatarsal joint joint of the foot and the inclination angle grasped in the ninth step, at least the overall center of gravity, the ankle joint of each leg that is in contact with the ground, and the leg Positional relationship of the midfoot phalanx joint of the foot and the ankle of the leg Based on the twelfth step of sequentially grasping the vertical position of the knot, and the grasped total center of gravity, the ankle joint of each leg that is in contact with the ground, and the positional relationship of the midfoot toe joint of the foot of the leg Estimating the position in the horizontal plane of the point of action of the floor reaction force vector acting on the leg, and determining the vertical position of the point of action of the floor reaction force vector acting on the leg based on the vertical position of the ankle joint of the leg A thirteenth step of sequentially estimating,
In the third step, when the motion state of the biped walking mobile body is the single leg support state, the acceleration vector value of the total center of gravity grasped in the seventh step, the total weight of the biped walking mobile body, and the ground contact The value of the floor reaction force vector in the body coordinate system is grasped based on the motion equation of the overall center of gravity of the biped walking moving body represented by the floor reaction force vector acting on the leg that is When the motion state of the walking moving body is the both-leg supporting state, the value of the acceleration vector of the entire center of gravity grasped in the seventh step, the total weight of the biped walking moving body, and the floor reaction force acting on each of the both legs The biped walking moving body from the specific part in which the equation of motion of the entire center of gravity of the biped walking moving body represented by the vector and the floor reaction force vector acting on each leg is predetermined near the lower end of the leg. Towards the overall center of gravity Based on the relational expression between the relative position of the specific part of the leg with respect to the total center of gravity of the biped walking moving body and the floor reaction force vector acting on the leg Grasping the value in the body coordinate system of the floor reaction force vector acting on each leg,
In the fourth step, the action in the body coordinate system is performed based on the position in the horizontal plane and the vertical position of the action point of the floor reaction force vector estimated in the thirteenth step and the inclination angle grasped in the ninth step. It is characterized by grasping the value of the position vector of the point .

According to the present invention, the displacement amount (joint rotation angle, etc.) of each joint of the biped walking moving body corresponding to each joint element of the rigid link model is sequentially grasped, and the predetermined rigid body element of the rigid link model is obtained. By sequentially grasping the value in the body coordinate system of the acceleration vector of the origin of the body coordinate system fixed to the body, and the value in the body coordinate system of the floor reaction force vector and the position vector of its action point, The joint moment can be estimated by an algorithm described in the coordinate system. That is, the displacement amount of each joint grasped in the first step (this corresponds to the displacement amount of each joint element of the rigid link model) of the biped walking moving body corresponding to each rigid element of the rigid link model. Since the relative positional relationship and posture relationship of each rigid body equivalent part can be grasped sequentially, the position and orientation (direction) of each rigid body equivalent part of the biped walking moving body viewed from the origin of the body coordinate system (rigid body link) The position and orientation of each rigid element of the model can be grasped sequentially. Therefore, it is possible to sequentially grasp the motion state (position, speed, acceleration, etc.) of each rigid body element or each rigid body equivalent part of the biped walking mobile body corresponding to the rigid body element as viewed from the origin of the body coordinate system. Further, the value in the body coordinate system of the acceleration vector at the origin of the body coordinate system obtained using the output of the acceleration sensor in the second step (specifically, the set of coordinate component values in the body coordinate system of the acceleration vector) By using the biped walking moving body corresponding to the predetermined rigid body element with a fixed body coordinate system, the movement of the biped walking moving body (movement with respect to the ground) and the relative position of each part of the biped walking moving body with respect to the rigid body corresponding portion Thus, the motion state such as the acceleration of each rigid body element of the rigid body link model accompanying the overall motion combined with the proper motion can be sequentially grasped by the value in the body coordinate system. The acceleration vector at the origin of the body coordinate system includes an inertial acceleration component due to gravity. Since the motion state such as the acceleration of each rigid body element of the rigid body link model can be successively grasped by the values in the body coordinate system in this way, the floor reaction force vector and the position of the action point thereof in the third step and the fourth step. By grasping the value of the vector in the body coordinate system (specifically, the set of coordinate component values in the body coordinate system of the position vector of the floor reaction force vector and its action point), the inverse dynamics The model can be expressed in the body coordinate system, and the value of the floor reaction force vector in the body coordinate system, the value of the position vector of the action point, the position of each rigid body element, the value of motion state such as acceleration, etc. It becomes possible to estimate the joint moment acting on the joint of the leg by the calculation process of the inverse dynamic model .
More specifically, in the third step of grasping the value of the floor reaction force vector in the body coordinate system, the equation of motion of the entire center of gravity of the biped walking moving body in the single leg support state and the both leg support state is used. Since the ground reaction force vector acting on the grounded leg is estimated based on (the equation of motion related to the translational motion of the entire center of gravity), a load sensor that hinders or burdens the walking of a bipedal moving body is used. The floor reaction force vector can be estimated without using it. In addition, in the state where both legs are supported, the floor reaction force vector acting on each leg cannot be specified only by the equation of motion of the entire center of gravity, but the floor reaction force vector acting on each leg is near the lower end of the leg. The biped walking moving body determined by assuming that the vector acts from the specific part (for example, ankle joint of each leg, the floor reaction force acting point, etc.) toward the entire center of gravity of the biped walking moving body. The floor reaction force vector for each leg can be estimated by further using a relational expression between the relative position of the specific part of the leg with respect to the total center of gravity and the floor reaction force vector acting on the leg. In this case, since the acceleration vector of the entire center of gravity of the biped walking moving body required in the equation of motion is obtained sequentially in the seventh step, the equation of motion of the entire center of gravity is obtained as the body coordinate. It can be described only by the coordinate component values of the system. Further, the relational expression between the relative position of the specific part of the leg with respect to the total center of gravity of the biped walking moving body and the floor reaction force vector acting on the leg can also be described only by the coordinate component value of the body coordinate system. Accordingly, the value of the floor reaction force vector in the body coordinate system can be obtained without grasping the inclination information (inclination state with respect to the vertical direction or the horizontal direction) of the biped walking moving body.
In addition, the position in the horizontal plane of the point of action of the floor reaction force vector acting on the grounded leg of the biped walking moving body is the ankle joint, the middle foot joint joint of the leg, and the entire biped walking moving body. It is closely related to the relative positional relationship with the center of gravity. Further, the vertical position of the action point of the floor reaction force vector has a substantially constant correlation with the vertical position of the ankle joint of the leg. Therefore, in the fourth step of grasping the value of the position vector of the action point of the floor reaction force vector in the body coordinate system, the overall center of gravity of the biped walking mobile body, the ankle joint of each leg that is grounded, and the leg It is possible to grasp the position in the horizontal plane of the point of action of the floor reaction force vector acting on the leg based on the positional relationship of the middle foot joint joint of the foot part of the body, and the vertical position of the ankle joint of the leg The vertical position of the point of action of the floor reaction force vector acting on the leg can be grasped based on this. Then, by using the grasped position in the horizontal plane and the vertical position and the inclination angle with respect to the vertical direction of the rigid body equivalent part of the biped walking moving body corresponding to the rigid body element to which the body coordinate system is fixed, The position vector of the action point of the floor reaction force vector can be obtained.
Thus, when grasping the value in the body coordinate system of the position vector of the floor reaction force acting point, the vertical direction of the rigid body equivalent part of the biped walking moving body corresponding to the rigid body element to which the body coordinate system is fixed is determined. It is necessary to grasp the inclination angle using a sensor (angular velocity sensor or inclinometer) for detecting the inclination angle. However, in the present invention, the inclination angle only needs to be used for estimating the action point of the floor reaction force vector, so that the calculation using the inclination angle can be minimized. Therefore, even when the tilt angle has an error, the accumulation of calculation errors can be minimized, and the joint moment estimation accuracy can be ensured with sufficient accuracy. In addition, since it is not necessary to provide a pressure distribution sensor at the bottom of the foot where a relatively large load is likely to be applied instantaneously, there is also an advantage in durability of the device configuration for estimating the joint moment.

  As described above, according to the present invention, the joint moment of the leg can be estimated using the value of the motion state of each rigid body element in the body coordinate system or the corresponding rigid body of the biped walking mobile body corresponding thereto. It is possible to reduce the calculation processing using the inclination information of the biped walking moving body (information such as how much the part where the biped walking moving body is inclined with respect to the vertical direction or the horizontal direction). As a result, it is possible to improve the estimation accuracy of the joint moment of the leg.

  In the present invention, the acceleration sensor may be basically attached to any rigid body equivalent part of the biped walking moving body, but the biped walking movement corresponding to the rigid body element to which the body coordinate system is fixed. It is preferable that it is mounted on a rigid body equivalent part (second invention). That is, when an acceleration sensor is attached to a part other than the rigid body equivalent part of the biped walking moving body corresponding to the rigid body element having a fixed body coordinate system, the biped walking movement is performed from the acceleration vector of the wearing part. It is necessary to calculate an acceleration vector of a rigid body equivalent portion corresponding to a rigid body element having a fixed body coordinate system using a displacement amount of a body joint or the like. On the other hand, as in the second invention, when an acceleration sensor is attached to a rigid body corresponding part corresponding to a rigid element having a fixed body coordinate system, the position of the origin of the body coordinate system and the acceleration sensor Since the relationship is fixed, the value of the acceleration vector at the origin of the body coordinate system in the body coordinate system can be grasped from the output of the acceleration sensor without using the amount of displacement of the joint of the biped walking moving body. As a result, the accuracy of the value in the body coordinate system of the acceleration vector at the origin of the body coordinate system to be grasped can be improved.

  Furthermore, in the second invention, the rigid body element to which the body coordinate system is fixed is preferably a rigid body element that connects a pair of joint elements corresponding to a pair of hip joints of the biped walking mobile body (third invention). That is, the rigid body element that connects the pair of joint elements corresponding to the pair of hip joints of the biped walking mobile body corresponds to the waist of the biped walking mobile body, and the waist is generally moved by the biped walking mobile body. The movement of time is relatively small. For this reason, sudden changes in the output of the acceleration sensor can be reduced and the output of the acceleration sensor can be made relatively stable. As a result, the accuracy of the value of the acceleration vector at the origin of the body coordinate system to be grasped can be improved. Can be increased.

In the first to third aspects of the invention, when the value of the floor reaction force vector and the value of the position vector of the action point grasped in the third step and the fourth step are three-dimensional values, the floor reaction force vector The position of the action point in the horizontal plane (the position of the biped walking moving body in the front-rear direction and the left-right direction) can be grasped as follows. That is, in the twelfth step, when the entire center of gravity exists on the rear side in the front-rear direction of the biped walking moving body with respect to the ankle joint of the leg with which the entire center of gravity is grounded, the horizontal plane of the ankle joint of the leg The inner position (the position in the front-rear direction and the left-right direction) is estimated as the position in the horizontal plane (the position in the front-rear direction and the left-right direction) of the point of action of the floor reaction force vector acting on the leg, and the overall center of gravity is grounded. In the horizontal plane of the metatarsal joint of the leg of the leg, when it is present in the front and rear direction of the biped walking moving body with respect to the metatarsal joint of the leg of the leg The position (the position in the front-rear direction and the left-right direction) is estimated as the position in the horizontal plane (the position in the front-rear direction and the left-right direction) of the point of action of the floor reaction force vector acting on the leg, and the overall center of gravity is grounded Existing in the front-rear direction of the biped walking moving body with respect to the ankle joint of the leg And the entire center of gravity on a line segment connecting the ankle joint of the leg and the metatarsal phalanx joint, The position in the horizontal plane (the position in the front-rear direction and the left-right direction) of the point where the position in the front-rear direction is the same as the position in the horizontal plane of the action point of the floor reaction force vector acting on the leg (in the front-rear direction and the left-right direction) ( The fourth invention ).

  That is, when the entire center of gravity is located behind the ankle joint of the leg that is in contact with the ground in the front-rear direction of the biped walking moving body, the leg usually has a foot of the foot. It is grounded, and the grounding point exists almost directly below the ankle joint of the leg. Accordingly, in this case, the position in the horizontal plane (position in the front-rear direction and the left-right direction) of the ankle joint of the leg is determined as the position in the horizontal plane (front-rear direction and left-right direction) of the point of action of the floor reaction force vector acting on the leg Position in the direction). In addition, when the entire center of gravity is present on the anterior side in the front-rear direction of the biped walking moving body with respect to the midfoot phalanx joint of the foot part of the leg, the leg is usually The toe of the foot part is grounded, and the grounding point exists almost immediately below the metatarsal joint joint of the foot part of the leg. Therefore, in this case, the horizontal plane position of the floor reaction force vector acting on the leg is set in the horizontal plane position (position in the front-rear direction and the left-right direction) of the middle foot phalanx joint of the foot part of the leg. It can be estimated as an internal position (position in the front-rear direction and the left-right direction). In addition, it exists on the front side in the front-rear direction of the biped walking moving body with respect to the ankle joint of the leg whose overall center of gravity is grounded, and on the posterior side of the midfoot phalanx joint of the foot part of the leg , The position of the action point of the floor reaction force vector in the front-rear direction is substantially the same as the position of the overall center of gravity in the front-rear direction. In addition, since the foot part can be regarded as a rigid body extending from the ankle joint to the middle foot joint joint, the action point of the floor reaction force vector is the line segment connecting the ankle joint and the middle foot joint joint. It can be considered that it exists on the line segment projected onto. Therefore, in this case, the position in the horizontal plane (in the front-rear direction and the left-right direction) where the overall center of gravity is the same as the front-rear direction position on the line segment connecting the ankle joint of the leg and the middle foot phalanx joint. Position) can be estimated as a horizontal plane position (position in the front-rear direction and the left-right direction) of the point of action of the floor reaction force vector acting on the leg.

In the first to fourth aspects of the invention , regarding the estimation of the vertical position of the floor reaction force action point vector, for example, the thirteenth step is the vertical direction of the floor reaction force vector acting on the grounded leg. The position is estimated as a position that is separated vertically downward by a predetermined value from the vertical position of the ankle joint of the leg grasped in the twelfth step ( fifth invention ). In other words, the ankle joint of the leg that is in contact with the ground when the bipedal walking body is walking or the like is generally located at a certain height from the floor. Accordingly, the position in the vertical direction of the point of action of the floor reaction force vector is a position that is vertically downward from the vertical position of the ankle joint of the leg by a predetermined value (predetermined value corresponding to the certain height). Can be estimated.

The height of the grounded leg from the floor of the ankle joint is substantially constant as described above, but the toe side portion of the bottom of the leg of the leg when moving the biped walking mobile In the state where only the ground is in contact, the height of the leg from the floor of the ankle joint is slightly higher than that in the case where almost the entire bottom surface or the heel side of the foot is grounded. . Therefore, in order to further improve the estimation accuracy of the position in the vertical direction of the action point of the floor reaction force vector, it is preferable to perform the following in the fifth aspect .

That is, in the tenth step, for the leg determined to be grounded, the toe side portion and the heel side portion of the foot portion of the leg are further determined for grounding, and in the twelfth step, In addition to the vertical position of the ankle joint of the leg that is grounded, the vertical position of the metatarsal joint of the foot of the leg is grasped. In the thirteenth step, when it is determined in the tenth step that only the toe side portion of the toe side portion and the heel side portion of the foot is grounded, instead of the predetermined value, The floor reaction force vector is calculated using the vertical distance between the ankle joint and the middle foot phalanx joint obtained from the vertical position of the ankle joint and the middle foot phalanx joint obtained in the twelfth step. estimating the lead straight direction position of the point of action (the sixth invention).

  According to this, in the state where it is determined that only the toe side portion of the foot portion of the leg that is grounded is grounded, the ankle joint and the middle ankle joint are determined from the vertical position of the ankle joint of the leg. A position that is vertically downward from the joint by a vertical distance is estimated as the vertical position of the point of action of the floor reaction force vector. As a result, it is possible to improve the estimation accuracy of the vertical position of the action point of the floor reaction force vector.

In each of the inventions described above, it is preferable that the value of the floor reaction force vector and the value of the position vector of the action point respectively grasped in the third step and the fourth step are three-dimensional values ( seventh invention ). . According to this, since with reference to the spatial behavior of the two-legged walking mobile body to grasp the values of and the position vector of the point of action of the floor reaction force vector estimated in the joint moment they Ru is grasped based on Accuracy can be increased.

  An embodiment of the present invention will be described with reference to FIGS. FIG. 1 is a diagram schematically showing an overall apparatus configuration in an embodiment in which the present invention is applied to a human being as a biped walking mobile body. As shown in the figure, the human 1 has a pair of left and right legs 2, 2, a body 3, a pair of left and right arms 4, 4, and a head 5, when the configuration is roughly divided. The torso 3 is composed of a waist part 6, an abdomen part 7, and a chest part 8, and the waist part 6 is connected to the legs 2, 2 via a pair of left and right hip joints 9, 9 and supported on both legs 2, 2. Has been. In addition, arms 4 and 4 are extended from the left and right sides of the chest 8 of the body 3, and the head 5 is supported on the upper end of the chest 8. Each leg 2 includes a thigh 10 extending from the hip joint 9, a crus 12 extending from the tip of the thigh 10 via the knee joint 11, and an ankle from the tip of the crus 12. And a foot portion 14 extending through the joint 13.

  In the present embodiment, in order to estimate the joint moment acting on each joint 9, 11, 13 of each leg 2 of the human 1 having such a configuration, the following device is installed in the human 1. Yes. That is, the sensor box 15 is mounted on the back surface of the waist 6 via a member such as a belt (not shown). As shown in the block diagram of FIG. 2, the sensor box 15 includes an acceleration sensor 16 that detects acceleration in three axes (translational acceleration) and a gyro sensor that detects angular velocity in three axes (around three axes). 17, an arithmetic processing unit 18 configured using a microcomputer, a light emitting / receiving device 19 that emits light to be introduced into optical fibers 26 and 27, which will be described later, and receives return light, an arithmetic processing unit 18, etc. And a battery 20 as a power source of each electrical component. The acceleration sensor 16 and the gyro sensor 17 are fixed to the waist part 6 via the sensor box 15 and move integrally with the waist part 6.

  Joint displacement sensors 21, 22, and 23 for detecting the displacement amount of each joint are attached to the hip joint 9, knee joint 11, and ankle joint 13 of each leg 2 via members such as a belt (not shown). Yes. The displacement amounts detected by the joint displacement sensors 21, 22, and 23 are the rotation angles around the three axes of the joints 9, 11, and 13, and the detection is performed using a potentiometer or the like.

In addition, two ground sensors 24 and 25 are provided on the bottom surface of the foot portion 14 of each leg 2 (specifically, the bottom surface of the shoe attached to the foot portion 14). Of the ground sensors 24, 25, the ground sensor 24 is provided at a position (heel) immediately below the ankle joint 13, and the ground sensor 25 is the middle foot phalanx joint 14 a (the base of the thumb of the foot section 14). (Toe) directly below (joint). These ground sensors 24 and 25 are sensors that output an ON / OFF signal indicating whether or not the place where the sensors are provided is grounded. The detection outputs of the joint displacement sensors 21, 22, 23 and the ground sensors 24, 25 are input to the arithmetic processing unit 18 of the sensor box 15 via signal lines (not shown).

  Further, as shown in FIG. 1, two optical fibers 26 and 27 are extended upward from the sensor box 15 along the back surface of the body 3, and the tip portions thereof are on the back surface of the abdomen 7 and the back surface of the chest 8, respectively. It is fixed via a member such as a belt (not shown). The optical fibers 26 and 27 are components of detection means for detecting the inclination angles (inclination angles in the sagittal plane) of the abdomen 7 and the chest part 8 with respect to the waist part 6, respectively. The inclination angles of the abdomen 7 and the chest 8 using these optical fibers 26 and 27 are measured by the following method. A typical method for measuring the inclination angle of the abdomen 7 using the optical fiber 26 will be described. The optical fiber 26 is connected to a predetermined light emitting / receiving device 19 (shown in FIG. 2) provided in the sensor box 15. Intensity light is introduced, and the introduced light is reflected at the tip of the optical fiber 26 and returns to the sensor box 15 side. The light return amount (returned light intensity) is detected by the light emitting / receiving device 19. In addition, the optical fiber 26 is provided with a plurality of notches (not shown) that allow minute light leakage at intervals in the longitudinal direction. Of the light introduced into the optical fiber 26, the waist 6 An amount of light corresponding to the inclination angle of the abdomen 7 with respect to the light leaks from the optical fiber 26 through the notches. For this reason, the return amount of light toward the sensor box 15 corresponds to the inclination angle of the abdomen 7, and the inclination angle of the abdomen 7 relative to the waist 6 is measured by detecting the return amount. That is, the detection output of the light emitting / receiving device 19 corresponding to the return amount of light of the optical fiber 25 corresponds to the inclination angle of the abdomen 7 with respect to the waist 6, and this is a signal indicating the inclination angle as the arithmetic processing unit 18. Is input. The measuring method of the tilt angle of the chest 8 using the optical fiber 27 is the same.

  The rotation angles of the hip joint 9, the knee joint 11 and the ankle joint 13 detected by the joint displacement sensors 21, 22, and 23, respectively, are such that the human 1 stands in an upright posture with both foot portions 14 and 14 facing forward in parallel. The rotation angle is based on the state (hereinafter referred to as the reference posture state of the human 1) as a reference (zero point). The same applies to the inclination angles of the abdomen 7 and the chest 8 detected using the optical fibers 26 and 27.

  Here, the rigid link model and coordinate system of the human 1 used in this embodiment will be described. FIG. 3 shows the rigid link model S1 and the coordinate system. Note that this rigid link model S1 is also shown in phantom in FIG. In the present embodiment, the rigid link model S1 represents the human 1 as a connected body composed of nine rigid elements and eight joint elements. More specifically, the rigid link model S1 can be roughly divided into a pair of leg portions S2 and S2 corresponding to each leg 2 of the human 1 and the upper body of the human 1 (upper portion from the waist 6). It is comprised from the upper-body part SU to do. The upper body SU connects a rigid element S6 corresponding to the waist 6 of the human 1 and a rigid element S7 corresponding to the abdomen 7 with a joint element JU1, and further, a rigid element S8 corresponding to the rigid element S7 and the chest 8 Are connected by a joint element JU2. Hereinafter, the rigid elements S6, S7, and S8 may be referred to as a waist element S6, an abdominal element S7, and a chest element S8, respectively, and the joint elements JU1 and JU2 may be referred to as an upper body lower joint JU1 and an upper body upper joint JU2, respectively.

  In this case, the waist element S6 has an inverted T shape, and the upper body lower joint JU1 is provided at the upper end thereof, and a pair of joint elements corresponding to the pair of hip joints 9, 9 of the human 1 at the left and right ends. J9 and J9 (hereinafter simply referred to as hip joint J9) are provided. That is, the lumbar element S6 has a portion S6a extending in the line segment direction (left-right direction) connecting the centers between the hip joints J9 and J9, and substantially upward from the center of the portion S6a toward the upper lower body joint JU1. It is comprised from the part S6b extended. Here, the lower body joint JU1 corresponds to the joint assumed on the spine of the human 1 near the boundary between the waist 6 and the abdomen 7 of the human 1, and the upper body joint JU2 includes the abdomen 7 and the chest. 8 corresponds to the joint assumed on the spine of the human 1 in the vicinity of the boundary with 8. The actual spine that controls the bending motion of the torso 3 of the human 1 is composed of a number of joints, but in the rigid link model S1, the bending motion of the upper body SU is the upper body lower joint JU1 and the upper body upper joint JU2. This is done with two joint elements. The abdomen element S7 extends in the direction of a line segment connecting the centers between the lower body joint JU1 and the upper body joint JU2. As shown in FIG. 1, the chest element S8 extends from the upper body joint JU2 to the base of the neck of the human 1 (more specifically, the site on the spine near the boundary between the body 3 and the neck). Has been.

  Each leg S2 of the rigid link model S1 connects the rigid element S10 corresponding to the thigh 10 to the waist element S6 via the hip joint J9, and connects the rigid element S12 corresponding to the crus 12 to the knee joint 11. And a rigid body element S14 corresponding to the foot 14 is connected via a joint element J13 corresponding to the ankle joint 13. Hereinafter, the rigid elements S10, S12, and S14 are referred to as a thigh element S10, a crus element S12, and a foot element S14, respectively, and the joint elements J11 and J13 are simply referred to as a knee joint J11 and an ankle joint J13, respectively. is there.

In this case, the thigh element S10 and the crus element S12 extend in the direction of a line segment connecting the centers between joint elements at both ends. Further , the tip of the foot element S14 corresponds to a midfoot phalanx joint 14a (hereinafter referred to as an MP joint 14a) of the foot 14 of the human 1, and as shown in FIG. 1, the ankle joint 13 (J13). To the midfoot phalanx joint 14a (hereinafter referred to as the MP joint 14a). In the rigid link model S1, the tip of the foot element S14 does not have a function as a joint. However, for the sake of convenience, the tip may be referred to as an MP joint J14a.

  Each rigid body element and each joint element of the rigid body link model S1 configured as described above has a mutual positional relation and posture relation (orientation relation) to each rigid body element and each joint element by the rotational motion of each joint element. It is possible to exercise so that the corresponding positional relationship and posture relationship of each part of the corresponding human 1 are substantially the same. In this case, each of the lower body joint JU1 and the upper body joint JU2 can be rotated about three axes, and one of them can be rotated around the measurement axis as a measurement axis (in FIG. 3). An arrow (an arrow indicating a rotation direction) described corresponding to each joint element JU1, JU2 is measured. In the present embodiment, the measurement axis is an axis parallel to a line segment connecting the centers of the pair of hip joints J9 and J9 (extending direction of the portion S6a of the waist element S6). Further, the hip joint J9, the knee joint J11, and the ankle joint J13 of each leg S2 are representative of the arrows (rotations) described in FIG. 3 with respect to the joint elements J9, J11, J13 of the left leg S2. As shown by the arrow indicating the direction, rotation about three axes is possible.

  In the rigid link model S1, the weight and length of each rigid element (length in the axial direction) and the position of the center of gravity of each rigid element (position at each rigid element) are determined in advance, and the arithmetic processing is performed. It is stored and held in a memory (not shown) of the device 18. Black points G8, G7, G6, G10, G12, and G14 in FIG. 3 illustrate the center of gravity of the chest element S8, the abdominal element S7, the waist element S6, the thigh element S10, the crus element S12, and the foot element S14, respectively. Is shown. Since the waist element S6 has an inverted T shape as described above, the length includes the length of the portion S6a and the length of the portion S6b.

  The weight, length, and the position of the center of gravity of each rigid element are basically set to be substantially the same as the weight, length, and the position of the center of gravity of the human body 1 corresponding to each rigid element. For example, the weight, length, and position of the center of gravity of the thigh element S10 are substantially the same as the actual weight, length, and position of the center of gravity of the thigh 10 of the human 1, respectively. However, the positions of the weight and the center of gravity are the positions of the weight and the center of gravity when the person 1 is equipped with the apparatus of the present embodiment. Further, the weight and the position of the center of gravity of the chest element S8 are the weight and the position of the center of gravity of the combination of the chest 8, the arms 4 and 4 and the head 5 of the human 1. Supplementally, the change in the position of the center of gravity of the chest element S8 due to the movement of the arms 4 and 4 (the movement of swinging the arms back and forth) during the movement of the human 1 is relatively small, and the chest element S8 is at a substantially constant position. Maintained. In addition, the position of the center of gravity of each rigid body element is set with each coordinate component value of the element coordinate system as a position vector in an element coordinate system, which will be described later, fixed and set to each rigid body element in advance.

  The weight, length, and center of gravity of each rigid element may be basically determined based on the measured values of the dimensions and weight of each part of the human 1, but from the height and weight of the human 1, the human average It may be estimated based on statistical data. In general, the position, weight, and length of the center of gravity of the portion corresponding to the rigid body of the human 1 corresponding to each rigid element have a correlation with the height and weight (total weight) of the human, and the height of the human 1 based on the correlation. In addition, it is possible to estimate the position, weight, and length of the center of gravity of the portion corresponding to the rigid body of the human 1 corresponding to each rigid body element with relatively high accuracy from the actually measured data of body weight.

  In FIG. 3, for the sake of convenience, each center of gravity G8, G7, G6, G10, G12, and G14 is described so as to be located on the axis of the corresponding rigid element, but is not necessarily on that axis. It may not be located but may exist at a position shifted from the axis.

  In the present embodiment, the following coordinate system is preset for the rigid link model S1. That is, as shown in FIG. 3, the body coordinate system BC is set fixed to the waist element S6. This body coordinate system BC uses the midpoint of the line segment connecting the centers of the pair of hip joints J11 and J11 (the center point of the portion S6a of the waist element S6) as the origin, the direction of the line segment as the Y axis, and the upper body from the origin A three-dimensional coordinate system (XYZ coordinate system) is set in which the direction toward the center of the lower joint JU1 is the Z axis, and the Y axis and the direction orthogonal to the Z axis are the X axes. In the reference posture state of the human 1, the X-axis, Y-axis, and Z-axis of the body coordinate system BC are respectively directed in the front-rear direction, the left-right direction, and the vertical direction (vertical direction) of the human 1, and the XY plane is a horizontal plane.

  For each rigid element, an element coordinate system is fixedly set, for example, as indicated by reference numerals C8, C7, C6, C10, C12, and C14. In the present embodiment, the element coordinate system C6 of the waist element S6 is the same as the body coordinate system BC. The element coordinate systems C8, C7, C10, C12, and C14 of the chest element S8, the abdominal element S7, the thigh elements S10, the crus elements S12, and the foot elements S14 are A three-dimensional coordinate system (XYZ coordinate system) having the origin at the center point of the upper body joint JU2, the upper body lower joint JU1, the knee joint J11, the ankle joint J13, and the MP joint J14a is used. In other words, for each leg S2, the element coordinate systems C10, C12, C14 of the rigid elements S10, S12, S14 are the joint elements at both ends of the rigid elements S10, S12, S14. The center point of the joint element farther from the waist element S6 is the origin. The element coordinate systems C7 and C8 of the abdominal element S7 and the chest element S8 of the upper body SU are joints closer to the waist element S6 among the joint elements at both ends of the abdominal element S7 and the chest element S8. The center point of the element is the origin. In FIG. 3, the element coordinate systems C10, C12, and C14 are shown only for the right leg part S2 for convenience of illustration, but the left leg part S2 is also shown in the element coordinate system in the same manner as the right leg part M2. Is set.

  In the element coordinate systems C8 and C7, the Z axis is set in the extending direction (axial direction) of the chest element S8 and the abdominal element S7, respectively, and the Y axis is in the same direction as the Y axis of the body coordinate system BC. Is set. In the element coordinate systems C10, C12, and C14, the Z axis is set in the extending direction (axial direction) of the thigh element S10, the crus element S12, and the foot element S14, and the Y axis is It is set in the normal direction of a plane including the center points of the hip joint J9, knee joint J11, and ankle joint J13 (hereinafter also referred to as leg plane). In any of the element coordinate systems C8, C7, C10, C12, and C14, the X axis is set in a direction orthogonal to the Y axis and the Z axis. In the following description, the element coordinate systems C8, C7, C6, C10, C12, and C14 are respectively represented as a chest coordinate system C8, an abdominal coordinate system C7, a waist coordinate system C6, a thigh coordinate system C10, and a crus coordinate system C12. , May be referred to as a foot part coordinate system C14.

  The element coordinate systems C8, C7, C10, C12, and C14 are not necessarily set as described above. Basically, the origin and the direction of each axis may be set arbitrarily.

  FIG. 4 is a block diagram showing the arithmetic processing function of the arithmetic processing unit 18. As shown in the figure, the arithmetic processing unit 18 generates a conversion tensor for generating a conversion tensor for coordinate conversion described later based on the detection outputs of the joint displacement sensors 21, 22, 23 and the light emitting / receiving device 19. The joint / element barycentric position calculating means for obtaining the position vector value (coordinate component value) in the body coordinate system BC of each joint element of the rigid body link model S1 and the center of gravity of each rigid body element using the means 28 and its conversion tensor 29 and body coordinate system acceleration calculation for obtaining the value of the acceleration vector (translational acceleration) of the origin of the body coordinate system BC (coordinate component value in the body coordinate system BC) based on the detection outputs of the acceleration sensor 16 and the gyro sensor 17 The body coordinate system tilt angle for calculating the tilt angle of the body coordinate system BC with respect to the vertical direction based on the detection output of the means 30 and the acceleration sensor 16 and the gyro sensor 17 Out and a means 31.

Furthermore, the arithmetic processing unit 18 uses the value of the position vector of the center of gravity of each rigid element obtained by the joint / element center of gravity position calculating means 29 to use the overall center of gravity of the rigid link model S1 in the body coordinate system BC (the total center of gravity of the human 1). ) And the time series data of the position vector value and the acceleration vector value of the origin of the body coordinate system BC obtained by the body coordinate system acceleration calculating means 30 are used to determine the acceleration of the entire center of gravity. The whole / element centroid motion calculating means 32 for obtaining a vector (translation acceleration) value (coordinate component value in the body coordinate system BC) is provided. In the whole / element centroid motion calculation calculation means 32 , the position vector of the centroid of each thigh element S10, crus element S12 and foot element S12 obtained by the joint / element centroid position calculation means 29 is obtained. Using the time-series data of the values and the value of the acceleration vector at the origin of the body coordinate system BC obtained by the body coordinate system acceleration calculation means 30, the thigh element S10, the crus element S12 and the foot element S14 The value of the acceleration vector (translation acceleration) of the center of gravity (the coordinate component value in the body coordinate system BC) is also obtained.

  The arithmetic processing unit 18 also determines the position vector value of each ankle joint J13 obtained by the joint / element centroid position calculating means 29, the position vector value of the entire centroid obtained by the whole / element centroid movement calculating means 32, and its acceleration. A value (coordinate component) in the body coordinate system BC of a floor reaction force vector (translational floor reaction force) acting on each leg 2, 2 of the human 1 using the vector value and the detection outputs of the ground sensors 24, 25. Value) and the position vector values of the ankle joints J13 and MP joints J14a obtained by the joint / element center-of-gravity position calculating means 29 and the body coordinate system inclination angle calculating means 31 The floor reaction force vector acting on each leg 2 using the inclination angle of the body coordinate system BC, the value of the position vector of the total center of gravity obtained by the total / element center of gravity motion calculating means 32 and the detection outputs of the ground sensors 24 and 25. Action of (Hereinafter, simply floor of reaction force acting point) and a floor reaction force acting point estimating means 34 for determining the value of the body coordinate system BC of the position vector of.

  Then, the arithmetic processing unit 18 includes each of the tensor element S10, the crus element S12, and the foot part element S14 obtained by the conversion tensor created by the conversion tensor creation means 28 and the whole / element centroid motion calculation means 30. Each leg using the acceleration vector of the center of gravity of the vehicle, the value of the floor reaction force vector obtained by the floor reaction force estimation means 33 and the value of the position vector of the floor reaction force action point obtained by the floor reaction force action point estimation means 34. Joint moment estimating means 35 for estimating joint moments acting on the two ankle joints 13, the knee joint 11 and the hip joint 9 is provided.

  Although details will be described later, the arithmetic processing unit 18 sequentially executes the arithmetic processing of each of the means 28 to 35 in a predetermined arithmetic processing cycle, and finally estimates the joint moment by the joint moment estimating means 35 in each arithmetic processing cycle. The value is calculated sequentially.

  Next, the operation of the apparatus of the present embodiment will be described together with detailed arithmetic processing of each means of the arithmetic processing unit 18. In the following description, generally, a conversion tensor for converting a vector quantity from one coordinate system Ca to another coordinate system Cb, that is, a vector quantity represented by a component value of the coordinate system Ca is used as a component of the coordinate system Cb. A tensor that converts a vector quantity represented by a value is expressed as “R (Ca → Cb)”. Further, a position vector of a certain point P or part P viewed in a certain coordinate system Ca is expressed as U (P / Ca). In addition, a vector A of physical quantities such as the acting force and acceleration of the object Q or the part Q represented by the coordinate component value of a certain coordinate system Ca is expressed as A (Q / Ca). In this case, when the coordinate component value in the coordinate system Ca of the position vector U (P / Ca) or the physical quantity vector A (Q / Ca) is represented, x, y, and z which are the names of the coordinate axes are further added. Is written. For example, the X coordinate component of the position vector U (P / Ca) is expressed as U (P / Ca) x.

  In addition, the element coordinate systems C8, C7, C6, C10, C12, and C14 are respectively used for the corresponding part names, and C_chest, C_abdomen, C_waist, C_thigh, C_crus, C_foot Sometimes referred to as a part. The same applies to the rigid elements S8, S7, S6, S10, S12, and S14, and the centers of gravity G8, G7, G6, G10, G12, and G14 of the rigid elements S. For example, the waist rigid element S8 and its center of gravity G8 may be referred to as S_waist and G_waist, respectively. In addition, about the thigh part 10, the leg part 12, and the foot part 14, when it is necessary to distinguish the right and left, "right" and "left" are further added and described. For example, the right thigh element S10 may be referred to as S_right thigh. Further, the hip joint J9, the knee joint J11, the ankle joint J13, and the MP joint J14a may be referred to as J_hip, J_knee, J_ankle, and J_MP, respectively. Also in this case, when it is necessary to distinguish between left and right, “right” and “left” are further added in the same manner as described above.

  The arithmetic processing unit 18 sends detection outputs of the joint displacement sensors 21, 22, 23, the light emitting / receiving device 19, the acceleration sensor 16, and the gyro sensor 17 through an A / D converter (not shown) at a predetermined arithmetic processing cycle. At the same time, the detection outputs (ON / OFF signals) of the respective ground sensors 24 and 25 are captured. First, the conversion tensor creation means 28 and the joint / element centroid position calculation means 29 are sequentially executed.

  In the calculation process of the conversion tensor creation means 28, a conversion tensor for performing coordinate conversion of the vector quantity between each element coordinate system and the body coordinate system BC is created. This conversion tensor is created by the following procedure. First, based on the detection outputs of the joint displacement sensors 21, 22, and 23, the rotation angles (rotation angles about three axes) of the hip joint 9, the knee joint 11 and the ankle joint 13 of each leg 2 are rigid links. Based on the rotation angle of each hip joint J9, knee joint J11 and ankle joint J13 of the model S1, and based on the detection output of the light emitting / receiving device 19, the abdominal element S7 and the chest element for the waist element M6 of the rigid link model S1 The inclination angle of S8 (specifically, the inclination angle on the sagittal plane (XZ plane) with respect to the Z-axis direction of the waist element coordinate system C6) is grasped. Then, a conversion tensor for performing coordinate conversion of the vector quantity between the element coordinate systems of the rigid elements is created using the grasped rotation angle and inclination angle. In this embodiment, this element coordinate system conversion tensor includes a conversion tensor R (C_foot part → C_crus part) from the foot part coordinate system C14 to the lower leg coordinate system C12, and the lower leg coordinate system. Conversion tensor R from C12 to thigh coordinate system C10 (C_lower leg → C_thigh) and conversion tensor R from thigh coordinate system C10 to waist coordinate system C6 (C_thigh → C_waist) ), And a conversion tensor R (C_abdomen → C_waist) from the abdominal coordinate system C7 to the lumbar coordinate system C6 and a conversion tensor R (C_chest → C_lumbar) from the chest coordinate system C8 to the lumbar coordinate system C6. Is done. Note that R (C_foot → C_crus), R (C_crus → C_thigh), and R (C_thighs → C_waist) are different for each of the left and right leg parts S2. Created.

  An example of the element coordinate system conversion tensor created here is shown in FIG. 5, for example, as shown in FIG. 5, the rotation angle of the knee joint 11 grasped from the detection output of the joint displacement sensor 22 of the knee joint 11 (the knee joint J11). (Rotation angle) is θy around the Y axis (axis perpendicular to the paper surface of FIG. 5) of the thigh coordinate system C10. However, in this example, it is assumed that the rotation angle of the knee joint 11 around the X axis and the Z axis of the element coordinate system C10 is zero. The rotation angle about the Y-axis of the knee joint 11 is positive in the direction in which the leg 2 extends, and θy <0 in the illustrated example. At this time, the conversion tensor R (C_crus → C_thigh) is represented by a cubic matrix as in the following equation (1).

  In the example of FIG. 5, since it is assumed that the knee joint 11 rotates only about one axis (around the Y axis), the conversion tensor R (C_crus → C_thigh) is as simple as equation (1). It is generally expressed in a more complex form.

  Further, for example, the conversion tensor R (C_abdomen → C_lumbar region) is set such that the inclination angle of the abdomen 7 with respect to the waist part 6 is θy (where the inclination angle θy is the forward direction of the human 1 as the positive direction). It is expressed by a matrix having the same form as the example of the right side of the equation (1). The same applies to the conversion tensor R (C_chest → C_waist). Supplementally, in the present embodiment, the upper body lower joint JU1 and the upper body upper joint JU2 of the rigid body link model S1 are rotated about one axis (C_abdomen and C_around the Y axis of the chest) of the abdominal element S7 and the chest element S8. Since the inclination angle with respect to the lumbar element S6 is measured, the transformation tensors R (C_abdomen → C_waist) and R (C_chest → C_waist) are always matrices having the same form as the row example on the right side of the equation (1). expressed. However, it is assumed that the lower body joint JU1 and the upper body joint JU2 can rotate about, for example, two axes (for example, about two axes of the Y axis and the X axis of the C_abdomen and C_chest), The tilt angle around the two axes of the chest element M8 may be measured.

  Next, from the element coordinate system conversion tensor obtained as described above, the vector quantity (described by the coordinate component value of each element coordinate system) described in each element coordinate system is converted into a body coordinate system BC (vector) A conversion tensor for converting a quantity into a description with coordinate component values in the body coordinate system BC is obtained.

  In this case, since the waist coordinate system C6 (C_waist) is equal to the body coordinate system BC in this embodiment, the conversion tensor R (C_waist → BC) from the C_waist to the body coordinate system BC is a cubic unit. Represented by a matrix. Also, the conversion tensors R (C_abdomen → BC) and R (C_chest → BC) from the abdominal coordinate system C7 (C_abdomen) and the chest coordinate system C8 (C_chest) to the body coordinate system BC are respectively set in advance. It is the same as the obtained element coordinate system conversion tensor R (C_abdomen → C_waist), R (C_chest → C_waist). The conversion tensors from the other element coordinate systems C10 (C_thigh), C12 (C_crus), C14 (C_foot) to the body coordinate system BC are expressed by the following equations (2a) to (2c). In this way, it is determined in order from the one closer to the waist element S6.

R (C_thigh → BC) = R (C_waist → BC) × R (C_thigh → C_waist) (2a)
R (C_crus → BC) = R (C_thighs → BC) × R (C_crus → C_thighs)
(2b)
R (C_foot → BC) = R (C_crus → BC) × R (C_foot → C_crus)
(2c)

Note that R (C_thigh → BC), R (C_crus → BC), and R (C_foot → BC) are obtained separately for each leg S2. Supplementally, a transposition of the conversion tensor from each element coordinate system to the body coordinate system BC becomes a conversion tensor for performing inverse conversion (coordinate conversion from the body coordinate system BC to the element coordinate system). For example, R (BC → C_thigh) = R (C_thigh → BC) ^T (T means transposition).

  In the calculation processing of the joint / centroid position calculation means 29 following the calculation processing of the conversion tensor creation means 28, the conversion tensor obtained as described above, the length and the gravity center position of each rigid body element previously stored in the memory (each element Position vector in the body coordinate system BC of the center of gravity of each joint element and each rigid element. Here, the joint element for obtaining the position vector includes the MP joint J14a.

Calculation of the position vector of each joint element is performed as follows. For example, calculation of the position vector of each joint element J9, J11, J13 of the left leg part S2 will be described. If the length S6a between the hip joints J9 and J9 of the waist element S6 is L6a, the position vector U (J_left hip / C_waist) of the left hip joint J6 at C_waist is (0, L6a / 2,0). ) ^T. Therefore, the position vector U (J_left hip / BC) of the left hip joint J9 in the body coordinate system BC is obtained by the following equation (3).

U (J_left crotch / BC) = R (C_waist → BC) x U (J_left crotch / C_waist)
= R (C_lumbar → BC) × (0, L6a / 2, 0) ^T (3)

Further, if the length of the left thigh element S10 is L10, the position vector U of the left hip joint J9 in the left thigh coordinate system C10 (C_left thigh) (J_left thigh / C_left thigh) Is (0, 0, L10) ^T , the position vector U (J_left knee / BC) of the left knee joint J11 in the body coordinate system BC is obtained by the following equation (4).

U (J_Left Knee / BC) = U (J_Left Knee / BC)
+ R (C_left thigh → BC) × (−U (J_left crotch / C_left thigh))
= U (J_Left / BC)
+ R (C_left thigh → BC) × (0, 0, −L10) ^T
(4)

  Similarly, the position vectors U (J_left ankle / BC) and U (J_left MP / BC) in the body coordinate system BC of the left ankle joint J13 and the left MP joint J14a are respectively expressed by the following equations (5), (6 ) In order.

U (J_Left ankle / BC) = U (J_Left knee / BC)
+ R (C_lower leg → BC) × (−U (J_left knee / C_left lower leg))
= U (J_Left Knee / BC)
+ R (C_lower leg → BC) × (0, 0, −L12) ^T (5)
U (J_Left MP / BC) = U (J_Left ankle / BC)
+ R (C_left foot → BC) × (−U (J_left ankle / C_left foot))
= U (J_Left ankle / BC)
+ R (C_left foot → BC) × (0,0, −L14) ^T (6)

  In addition, L12 of Formula (5) and L14 of Formula (6) are the lengths of the left crus element S12 and the left foot part element S14, respectively. The position vector of each joint element of the right leg part S2 is also obtained in the same manner as described above.

  Further, the position vector of each joint element of the upper body SU is obtained in the same manner as described above. That is, the position vectors in the body coordinate system BC of the lower body joint JU1 and the upper body joint JU2 are obtained in order by the following equations (7) and (8), respectively.

U (JU1 / BC) = R (C_lumbar → BC) ・ U (JU1 / C_lumbar)
= R (C_ waist → BC) · (0, 0, L6b) ^T (7)
U (JU2 / BC) = U (JU1 / BC) + R (C_abdomen → BC) ・ U (JU2 / C_abdomen)
= U (JU1 / BC) + R (C_abdomen → BC) · (0,0, L7) ^T ...... (8)

  In addition, L6b of Formula (7) is the length of the part S6b of the waist element S6. Moreover, L7 of Formula (8) is the length of abdominal element S7.

  Further, the calculation of the position vector of the center of gravity of each rigid element in the body coordinate system BC is performed as follows. That is, the position vectors U (G_waist / BC), U (G_thigh) of the center of gravity of the lumbar element S6, the thigh element S10, the crus element S12, and the foot element S14 in the body coordinate system BC. / BC), U (G_crus / BC), U (G_foot / BC) are U (J_left crotch / C_waist), U (on the right side of the equations (3) to (6), respectively. J_Left / C_Left), U (J_Left Knee / C_Left Lower Leg), U (J_Left Ankle / C_Left Foot) are set in advance in each element coordinate system. Position vector U (G_waist / C_waist), U (G_thigh / C_thigh), U (G_crus / C_crus), U (G_foot / C_foot) Part) is calculated by calculating the expression replaced. The calculation of the position vectors of G_thigh, G_crus, and G_foot is performed separately for each leg S2.

  The position vectors U (G_abdomen / BC) and U (G_chest / BC) in the body coordinate system BC of the centroids G7 and G8 of the abdominal element S7 and the chest element S8 are expressed by the following equations (9) and (9), respectively. 10).

U (G_abdomen / BC) = U (JU1 / BC) + R (C_abdomen → BC) · U (G_abdomen / C_abdomen)
...... (9)
U (G_chest / BC) = U (JU2 / BC) + R (C_chest → BC) ・ U (G_chest / C_chest)
(10)

  The calculation processing of the conversion tensor creation means 28 and the joint / element gravity center position calculation means 29 has been described above. As described above, the position vector of each joint element calculated by the joint / element center-of-gravity position calculation means 29 and the center of gravity of each rigid body element is the body coordinate system BC of the actual part of the corresponding human 1. It has a meaning as a viewed position vector.

  The arithmetic processing unit 18 performs the arithmetic processing of the body coordinate system acceleration calculating unit 30 and the body coordinate system inclination angle calculating unit 31 in parallel with the arithmetic processing of the conversion tensor creating unit 28 and the joint / element gravity center position calculating unit 29 described above. Execute.

  In the calculation processing of the body coordinate system acceleration calculation means 30, the following is based on the acceleration in three axes (translational acceleration) grasped from the detection output of the acceleration sensor 16 and the angular velocity around the three axes grasped from the detection output of the gyro sensor 17. Thus, the value (coordinate component value) in the body coordinate system BC of the acceleration vector at the origin of the body coordinate system BC is obtained. First, the accelerations and angular velocities detected by the sensors 16 and 17 are vector quantities expressed in a three-axis coordinate system fixed to the sensors 16 and 17 (hereinafter referred to as a sensor coordinate system SC or C_sensor). Since there is, it is converted into a value in the body coordinate system BC. The conversion depends on the relative attachment position relationship between the acceleration sensor 16 and the gyro sensor (angular velocity sensor) 17 with respect to the waist 6 (relative posture relationship of the sensor coordinate system SC with respect to the waist coordinate system C6 (= body coordinate system BC)). The conversion tensor set in advance is multiplied by the acceleration vector and the angular velocity vector respectively detected by the sensor coordinate system SC. That is, the detected value of the acceleration vector in the sensor coordinate system SC is ACC (sensor / SC), the acceleration vector obtained by converting the detected value into the body coordinate system BC is ACC (sensor / BC), and the angular velocity vector is detected in the sensor coordinate system SC. If the value is ω (sensor / SC) and the angular velocity vector obtained by converting it to the body coordinate system BC is ω (sensor / BC), the acceleration vector ACC (sensor / BC) and the angular velocity vector ω (sensor / BC) are They are obtained by the following equations (11) and (12), respectively. Here, ACC (sensor / BC) and ω (sensor / BC) are more specifically the acceleration vector and the angular velocity vector of the acceleration sensor 16 and the gyro sensor 17, respectively. In this sense, the notation of ACC and ω "Sensor" is added to In this example, the positions of the acceleration sensor 16 and the gyro sensor 17 are substantially the same, and the sensor coordinate system SC is the same coordinate system for both the sensors 16 and 17.

ACC (sensor / BC) = R (SC → BC) ・ ACC (sensor / SC) ...... (11)
ω (sensor / BC) = R (SC → BC) ・ ω (sensor / SC) (12)

  Here, the conversion tensor R (SC → BC) is a relative posture relationship between the sensor coordinate system SC and the body coordinate system BC (more specifically, the inclination angle of each axis of the sensor coordinate system SC with respect to each axis of the body coordinate system BC). ) From the element coordinate system conversion tensor (R (C_thigh → C_waist), etc.). For example, when the three axes (XYZ axes) of the sensor coordinate system SC are inclined by the angle θy around the Y axis (axis perpendicular to the paper surface of FIG. 6) of the body coordinate system BC as shown in FIG. R (SC → BC) is represented by a matrix having the same form as the right side of the equation (1). In this case, since the acceleration sensor 16 and the gyro sensor 17 are fixed to the waist 8 provided with the body coordinate system BC, the inclination angle of each axis of the sensor coordinate system SC with respect to each axis of the body coordinate system BC is determined by the acceleration sensor 16. The gyro sensor 17 is actually measured when the gyro sensor 17 is attached to the waist 8 and the conversion tensor R (SC → BC) is obtained in advance based on the inclination angle and stored in a memory (not shown) of the arithmetic processing unit 18. Has been. Supplementally, the acceleration sensor 16 and the gyro sensor 17 may be attached to a part other than the waist part 6 (a rigid body corresponding part corresponding to any rigid body element of the rigid body link model S1). In this case, the acceleration vector ACC (sensor / BC) and the angular velocity vector ω (sensor / BC) are detected in the sensor coordinate system SC in the element coordinate system of the rigid element on which the acceleration sensor 16 and the gyro sensor 17 are mounted. After conversion to the above value, it may be further converted to a value in the body coordinate system BC by the conversion tensor obtained by the conversion tensor creation means 28.

  In the calculation processing of the body coordinate system acceleration calculating means 30, after obtaining the acceleration vector ACC (sensor / BC) and the angular velocity vector ω (sensor / BC) as described above, the origin of the body coordinate system BC is obtained by the following equation (13). Acceleration vector ACC (BCO / BC) is obtained. “BCO” is a code representing the origin of the body coordinate system BC.

  In this equation (13), U (sensor / BC) is a position vector of the acceleration sensor 16 and the gyro sensor 17 in the body coordinate system BC, and U (sensor / BC) x, U (sensor / BC) y , U (sensor / BC) z is each coordinate component value in the body coordinate system BC of U (sensor / BC) in accordance with the definition of the vector coordinate component value notation method in the present specification described above. . U (sensor / BC) is actually measured when the acceleration sensor 16 and the gyro sensor 17 are attached to the waist 8, and is stored in the memory of the arithmetic processing unit 18. Further, ω (sensor / BC) x, ω (sensor / BC) y, and ω (sensor / BC) z are coordinate component values of the angular velocity vector ω (sensor / BC) obtained previously. Further, ω (sensor / BC) ′ represents a first-order differential value of ω (sensor / BC), and the value is obtained by the above formula (12) for each arithmetic processing period of the arithmetic processing unit 18. Calculated from time series data of sensor / BC). Supplementally, the angular velocity is the same in any part in the lumbar element S6. For example, the angular velocity ω (BCO / BC) of the origin BCO of the body coordinate system BC fixed to the lumbar element S6 is ω (sensor / BC). equal.

  Since the acceleration sensor 16 also detects acceleration due to gravity, the acceleration vector ACC (BCO / BC) obtained as described above includes an inertial acceleration component due to gravity. In the present embodiment, the acceleration vector ACC (BCO / BC) of the origin BCO of the body coordinate system BC is obtained in consideration of the angular velocity of the lumbar element S6. However, the angular velocity of the lumbar element S6 and the rate of change thereof are as follows. Since it is comparatively small, the acceleration vector ACC (BCO / BC) of the origin BCO of the body coordinate system BC may be directly used as ACC (sensor / BC) obtained by the equation (11).

  Further, in the calculation process of the body coordinate system tilt angle calculation means 31, the tilt angle of the waist element S6 with respect to the vertical direction (gravity direction) from the detection outputs of the acceleration sensor 16 and the gyro sensor 17 (the Z direction of the body coordinate system BC). A tilt angle of the shaft is calculated. Since this calculation method is well-known, description here is abbreviate | omitted. The inclination angle calculated here is an inclination angle around two axes, a horizontal axis in the front-rear direction and a horizontal axis in the left-right direction.

  Next, the arithmetic processing unit 18 executes the arithmetic processing of the whole / element gravity center motion calculating means 30. In the calculation process of the whole / element centroid motion calculating means 30, first, the centroid position (position vector in the body coordinate system BC) of each rigid element obtained by the joint / element centroid position calculating means 29 and the previously described above are used. The body coordinate system BC of the overall center of gravity of the rigid body link model S1 (the overall center of gravity of the human 1; hereinafter sometimes referred to as G_overall) by the following equation (14): The position vector U (G_whole / BC) at is obtained.

U (G_whole / BC) = {U (G_chest / BC) × m_chest + U (G_abdomen / BC) × m_abdomen + U (G_waist / BC) × m_waist + U (G_right thigh / BC) × m_right thigh + U (G_left thigh / BC) × m_left thigh + U (G_right lower leg / BC) × m_right lower leg + U (G_left lower leg / BC) × m_left lower leg Part + U (G_right foot / BC) × m_right foot + U (G_left foot / BC) × m_left foot} / whole weight …… (14)

  Note that “m_OO”, such as m_chest, is the weight of the rigid element corresponding to the name of OO. As shown in this equation (14), the position vector U (G_total / BC) of the total center of gravity is the product of the position vector in the body coordinate system BC of the center of gravity of each rigid element of the rigid link model S1 and the weight of the rigid element. Is divided by the total weight of human 1 (= sum of the weights of all rigid elements).

  In the calculation process of the total / element centroid motion calculating means 30, the time series data of the position vector U (G_total / BC) of the total centroid determined as described above (U (for each calculation processing cycle of the calculation processing device 18) The second-order differential value U (G_total / BC) '' of U (G_total / BC) is calculated from the time series value of G_total / BC). This second-order differential value U (G_total / BC) '' means the relative acceleration of the entire center of gravity G_ relative to the origin BCO of the body coordinate system BC. The second-order differential value U (G_whole / BC) '', that is, the body coordinate system obtained by the body coordinate system acceleration calculation means 30 in advance of the relative acceleration of G_w with respect to the origin BCO of the body coordinate system BC. By adding to the acceleration vector ACC (BCO / BC) of the origin BCO of BC, the actual acceleration vector ACC (G_total / BC) of G_total is calculated. That is, ACC (G_total / BC) is calculated by the following equation (15).

    ACC (G_Overall / BC) = ACC (BCO / BC) + U (G_Overall / BC) '' (15)

  Further, in the calculation process of the whole / element centroid motion calculating means 30, the thigh element S10, the crus element S12, and the foot part of each leg part S2 previously obtained by the joint / element centroid position calculating means 29 are obtained. From the time series data (time series value for each calculation processing period of the calculation processing device 18) of the position vector of each centroid of the element S14 (position vector in the body coordinate system BC), the second-order differentiation of the position vector of each centroid. The value, that is, the relative acceleration of the center of gravity of each rigid element with respect to the origin BCO of the body coordinate system BC is calculated. Then, by adding the relative acceleration of the center of gravity of each rigid body element to the acceleration vector ACC (BCO / BC) of the origin BCO of the body coordinate system BC previously obtained by the body coordinate system acceleration calculating means 30, each rigid body is obtained. The actual acceleration vector of the centroid of the element is calculated. For example, the acceleration vector ACC (G_thigh / BC) of the center of gravity G_thigh (G10) of each thigh element S10 is G_thigh position vector U (G_large) as in the equation (15). It is calculated by adding ACC (BCO / BC) to the second-order differential value U (G_thigh / BC) '' of thigh / BC. The same applies to the crus element S12 and the foot part element S14.

  Next, the arithmetic processing unit 18 executes calculation processing of the floor reaction force estimation means 33 and the floor reaction force action point estimation means 34. In the calculation process of the floor reaction force estimation means 33, first, based on the detection outputs of the ground sensors 24 and 25, the motion state of the human 1 is a both-leg support state in which both legs 2 and 2 are grounded, or one leg 2 It is determined whether only a single leg is in contact with the ground. That is, one of the ground sensors 24 and 25 of one leg 2 outputs an ON signal indicating that there is ground, and one of the ground sensors 24 and 25 of the other leg 2 indicates that there is ground. Is output, it is determined that both legs are in contact with the ground. In addition, one of the ground sensors 24 and 25 of one leg 2 of the legs 2 and 2 outputs an ON signal indicating that the ground is present, and the ground sensors 24 and 25 of the other leg 2 are also provided. If neither of them outputs an ON signal indicating that there is grounding, it is determined that the single leg is supported. Then, in the processing of the floor reaction force estimation means 31, the floor reaction force vector acting on each leg 2 is estimated by different calculation processing depending on whether the two-leg support state or the single-leg support state.

  The basic concept of the estimation process of the floor reaction force vector is the same as that of JP 2003-89083 A and the like previously proposed by the applicant of the present application. However, in the present embodiment, the estimation process is mainly performed. The coordinate system and the like used in is different from the method described in the publication. This will be described below with reference to FIGS. 7 and 8 (a) and 8 (b). FIG. 7 illustrates the single-leg support state of the human 1 viewed from the sagittal plane, and FIGS. 8A and 8B illustrate the dual-leg support state of the human 1 viewed from the sagittal plane and the frontal plane, respectively. is doing. In FIGS. 7 and 8, the human 1 is schematically shown in a rigid link model. As shown in FIG. 7, when the motion state of the human 1 is a single leg support state, the floor reaction force vector Frf (acting on the leg 2 that is in contact with the ground (in this case, for example, the right leg 2). Right leg / BC), that is, the floor reaction force vector acting on the grounded right leg 2 in terms of the coordinate component values of the body coordinate system BC is the body coordinate system BC of the entire center of gravity G_ The following equation (16) representing the equation of motion related to the translational motion of

    Frf (right leg / BC) = total weight × ACC (G_total / BC) (16)

  That is, from the G_total acceleration vector ACC (G_total / BC) calculated by the total / element centroid motion calculating means 30 and the total weight of the human 1 (the total weight of the rigid link model S1), A reaction force vector Frf (right leg / BC) is calculated. Even when the left leg 2 is in contact with the ground, the floor reaction force vector Frf (left leg / BC) is similarly calculated by the calculation of the right side of the equation (16) in the single leg support state. In this case, as described above, the ACC (G_total / BC) includes an inertial acceleration component due to gravity, and the floor reaction force vector Frf is represented by the body coordinate system BC. There is no need. The floor reaction force vector Frf acting on the leg 2 on the non-grounded side is zero. In FIG. 7, for convenience of illustration, the Z axis of the body coordinate system BC is shown in the vertical direction, but Equation (16) does not depend on the inclination of the body coordinate system BC.

  On the other hand, as shown in FIGS. 8A and 8B, when both legs are supported, the floor reaction force vector Frf (right leg / BC) acting on the right leg 2 and the left leg 2 are acted on. The floor reaction force vector Frf (left leg / BC) is calculated based on the following five relational expressions (17) to (21).

Frf (right leg / BC) + Frf (left leg / BC) = total weight × ACC (G_total / BC)
(17)
Frf (right leg / BC) x: Frf (right leg / BC) z
= U (G_whole / BC) x-U (J_right ankle / BC) x
: U (G_whole / BC) z-U (J_right ankle / BC) z …… (18)
Frf (left leg / BC) x: Frf (left leg / BC) z
= U (G_whole / BC) x-U (J_left ankle / BC) x
: U (G_whole / BC) z-U (J_left ankle / BC) z …… (19)
Frf (right leg / BC) y: Frf (right leg / BC) z
= U (G_whole / BC) y-U (J_right ankle / BC) y
: U (G_whole / BC) z-U (J_right ankle / BC) z …… (20)
Frf (left leg / BC) y: Frf (left leg / BC) z
= ACC (G_whole / BC) y-U (J_left ankle / BC) y
: U (G_whole / BC) z-U (J_left ankle / BC) z …… (21)

  Here, the meaning of these equations (17) to (21) will be described. Equation (17) represents an equation of motion related to translational motion of the entire center of gravity G_ in the body coordinate system BC. In addition, as shown in FIGS. 8A and 8B, the equations (18) to (21) are expressed by the floor reaction force vector Frf (right leg / BC) and the floor reaction force vector Frf (left leg / BC). ) Are vectors directed from the ankle joint 13 of the right leg 2 and the ankle joint 13 of the left leg 2 to the entire center of gravity G_, in other words, the floor reaction force vector Frf and the left ankle joint 13. Is a geometrical relational expression obtained on the assumption that the direction of the entire position vector G_ is the same. In this case, Expressions (18) and (19) are relational expressions viewed in the sagittal plane (XZ plane of the body coordinate system BC), and Expressions (20) and (21) are the frontal plane (YZ of the body coordinate system BC). ) Relational expression seen on a plane. In FIG. 8, for convenience of illustration, the Z axis of the body coordinate system BC is shown in the vertical direction, but the equations (17) to (21) do not depend on the inclination of the body coordinate system BC. In the present embodiment, the ankle joint 13 of each leg 2 has a meaning as a specific part near the lower end of the leg 2.

  When calculating the floor reaction force vectors Frf (right leg / BC) and Frf (left leg / BC) in the state where both legs are supported, the coordinate component values of these vectors are set as unknowns and the above equations (17) to ( Frf (right leg / BC) and Frf (left leg / BC) are calculated by solving the simultaneous equations constituted by 21). That is, Frf (right leg / BC) and Frf (left leg / BC) are the ACC (G_total / BC) and U (G_total) obtained by the total weight of the human 1 and the total / element centroid motion calculation means 30. / BC) and U (J_right ankle / BC) and U (J_left ankle / BC) obtained by the joint / element gravity center calculating means 29. Thus, in the present embodiment, the floor reaction force vectors Frf (right leg / BC) and Frf (left leg / BC) in the state where both legs are supported are expressed by the relational expression (17) described in the body coordinate system BC. ) To (21).

  Note that the Z-axis components of Frf (right leg / BC) and Frf (left leg / BC) are the expressions (18) and (19) relating to the sagittal plane or the expressions (20) and (21) relating to the frontal plane. Any of these can be used.

In the calculation process of the floor reaction force action point estimation means 34, first, based on the inclination angle of the waist element S6 calculated by the body coordinate system inclination angle calculation means 31 with respect to the vertical direction, the body coordinate system BC is changed to the absolute coordinate system IC. A conversion tensor R (BC → IC) is created. Here, the absolute coordinate system IC is an orthogonal coordinate system having the vertical direction as the Z axis, and is a coordinate system in which the orientation of each coordinate axis is the same as that of the body coordinate system BC in the reference posture state. The conversion tensor R (IC → BC) from the absolute coordinate system IC to the body coordinate system BC is a transposition R (BC → IC) ^T of the conversion tensor R (BC → IC).

  Next, using the conversion tensor R (BC → IC), the total center G_total position vector U (G_total / BC) previously determined by the total / element center of gravity motion calculating means 32 and the joint / element center of gravity position The conversion tensors R (BC →) are respectively added to the position vectors U (J_ankle / BC) and U (J_MP / BC) of the ankle joint J13 and the MP joint J14a of each leg body S2 previously obtained by the calculation means 29. IC), the position vector U (G_total / IC), U (J_ankle / IC), U (J_MP) as seen in the absolute coordinate system IC of the total center of gravity G_, each ankle joint J13 and MP joint J14a. / IC) is calculated. These position vectors U (G_total / IC), U (J_ankle / IC), and U (J_MP / IC) are position vectors in the absolute coordinate system IC having the same origin as the body coordinate system BC. At this time, it is not necessary to calculate the position vectors U (J_ankle / IC) and U (J_MP / IC) for the leg 2 which is determined not to be grounded based on the detection outputs of the ground sensors 24 and 25.

  Next, for each leg 2 determined to be grounded based on the detection outputs of the ground sensors 24 and 25, the X of the position vectors U (G_whole / IC), U (J_ankle / IC), U (J_MP / IC) In other words, depending on the magnitude relationship of the axial components U (G_total / IC) x, U (J_ankle / IC) x, U (J_MP / IC) x, in other words, the total center of gravity G_, the ankle joint 13 and the MP joint The X-axis component and Y-axis component of the position vector (position vector in the absolute coordinate system IC) U (COP / IC) of the floor reaction force acting point are determined according to the relative horizontal positional relationship in the front-rear direction of 14a. Is done. This determination method will be described in further detail with reference to FIGS. 9A to 9C and FIG. In the following description, it is assumed that the left leg 2 is grounded. FIGS. 9A to 9C illustrate a state in which the left leg 2 of the human 1 viewed from the sagittal plane is in contact with the ground (in these drawings, a single leg support state), and FIG. The figure which looked at the foot part 14 by the side of the grounding in the state of (b) by planar view is shown. In FIGS. 9 and 10, the human 1 is schematically shown in a rigid link model.

  As shown in FIG. 9A, when the entire center of gravity G_ is present ahead of the MP joint 14a of the left leg 2 that is in contact with the ground, U (G_total / IC) x> U (J_left MP / IC) x, the foot 14 of the left leg 2 is grounded mainly on its toes. In this case, the floor reaction force action point COP exists at a position almost immediately below the MP joint 14a of the foot portion 14. Therefore, in this case, the X and Y axis components of the position vector U (left COP / IC) of the floor reaction force action point COP are the X and Y axes of the position vector U (J_left MP / IC) of the MP joint 14a, respectively. Suppose it is equal to the component. That is, U (left COP / IC) x = U (J_left MP / IC) x, U (left COP / IC) y = U (J_left MP / IC) y.

  Further, as shown in FIG. 9C, when the entire center of gravity G_ is present behind the ankle joint 13 of the left leg 2, which is in contact with the ground, that is, U (G_total / IC) x <U (J_ In the case of left ankle / IC) x, the foot 14 of the left leg 2 is grounded mainly on the heel side thereof. In this case, the floor reaction force action point COP exists at a position almost immediately below the ankle joint 13 of the left leg 2. Therefore, in this case, the X and Y axis components of the position vector U (left COP / IC) of the floor reaction force acting point COP are the X and Y axes of the position vector U (J_right ankle / IC) of the ankle joint 13, respectively. Suppose it is equal to the component. That is, U (left COP / IC) x = U (J_left ankle / IC) x, U (left COP / IC) y = U (J_left ankle / IC) y.

  Further, as shown in FIG. 9B, when the entire center of gravity G_ is present between the ankle joint 13 and the MP joint 14a of the left leg 2 in the front-rear direction, that is, U (J_left MP / IC). When x ≦ U (G_total / IC) x ≦ U (J_left ankle / IC) x, the floor reaction force action point COP is almost directly below the entire center of gravity G_ on the sagittal plane shown in the drawing. Exists. Therefore, in this case, it is assumed that the X-axis component of the position vector U (left COP / IC) of the floor reaction force action point COP is equal to the entire X-axis component of the entire center of gravity G_. That is, U (left / right COP / IC) x = U (G_total / IC) x. The floor reaction force action point COP is present on the contact surface between the foot 14 of the left leg 2 that is in contact with the floor (in this case, almost the entire bottom of the foot 14), The position is considered to exist on a line segment obtained by projecting a line segment connecting the center point of the ankle joint 13 and the center point of the MP joint 14a onto the floor surface. Therefore, the Y-axis component of the position vector U (right COP / IC) of the floor reaction force action point COP is on the axial center of the foot portion element S14 with respect to the left leg 2 (the center of the ankle joint 13) as shown in FIG. Y-axis component of the point P such that the value of the entire center of gravity G_ and the X-axis component (X-axis component in the absolute coordinate system IC) are the same on the line segment connecting the point and the center point of the MP joint 14a) Is equal to The value of the Y-axis component of such a position vector U (right COP / IC) is obtained based on the proportional relational expression in the following expression (22).

U (left COP / IC) x-U (J_left ankle / IC) x: U (J_left MP / IC) x-U (J_left ankle / IC) x
= U (Left COP / IC) y-U (J_Left ankle / IC) y
: U (J_Left MP / IC) y-U (J_Left ankle / IC) y (22)

  In addition, the Z-axis component of the position vector U (left COP / IC) of the floor reaction force acting point is in the vertical direction by a predetermined value H0 (> 0) determined in advance from the ankle joint 13 (ankle element J13) of the left leg 2. It is assumed that it is equal to the Z-axis component at a point separated downward. That is, U (right COP / IC) z = U (J_left ankle / IC) z−H0. Here, the predetermined value H0 is a vertical distance from the floor surface to the center of the ankle joint 13 in the reference posture state (more precisely, a state in which almost the entire bottom surface of the foot 14 is in contact with the horizontal floor surface). The direction distance is measured in advance and stored in the memory of the arithmetic processing unit 18. The predetermined value H0 may be actually measured for each of the left and right legs 2, but the value actually measured for any one leg 2 may be used in common for both the left and right legs 2.

  In the present embodiment, as described above, the position vector U (left COP / IC) of the floor reaction force acting point of the floor reaction force vector Frf acting on the left leg when the left leg 2 is grounded is obtained. It is done. The same applies to the case where the right leg 2 is grounded. In this case, in the both-leg grounding state, the position vector of the floor reaction force acting point is obtained for each leg 2 as described above.

  In the present embodiment, the predetermined value H0 used for obtaining the Z-axis component of the position vector U (COP / IC) of the floor reaction force acting point is a constant value. When only the toe side of the unit 14 is grounded, that is, when only the ground sensor 25 outputs an ON signal indicating grounding, the grounded leg instead of the predetermined value H0. 2, the difference between the Z-axis components of the position vectors U (J_ankle / IC) and U (J_MP / IC) of the ankle joint 13 and the MP joint 14a (U (J_ankle / IC) z−U (J_MP / IC) z), that is, the vertical distance between the ankle joint 13 and the MP joint 14a may be used. In this way, the accuracy of U (COP / IC) can be increased.

  In the calculation process of the floor reaction force action point estimation means 34, the position vector U (COP / IC) of the floor reaction force action point obtained for each leg 2 that is grounded as described above is finally obtained. By multiplying the inverse transformation tensor R (IC → BC), which is the transpose of the transformation tensor R (BC → IC), the value U (COP / BC) in the body coordinate system BC of the position vector of the floor reaction force acting point is obtained. Desired.

  Next, the arithmetic processing unit 18 executes arithmetic processing by the joint moment estimating means 35. An outline of the calculation process of the joint moment estimating means 35 will be described. Equations of motion relating to the translational motion and rotational motion of the foot element S14, the lower leg element S12, and the thigh element S10 of each leg part S2 are given. The joint moments of the joint elements J_ankle, J_knee, J_crotch at the end points on the side of the waist element S6 of the foot element S14, the crus element S12, and the thigh element S10 by the calculation of the inverse dynamic model based on Are calculated in order.

  More specifically, the equations of translation of the foot element S14, the crus element S12, and the thigh element S10 are given by the following equations (23) to (25). In the following description, generally, one end portion on the side closer to the waist element S6 among the ends of the rigid elements of the foot element S14, the crus element S12, and the thigh element S10 is referred to as “ P_OO "and the other end" D_OO "on the far side (XX is a name indicating a rigid element) may be used. For example, the end on the knee joint J_knee (J11) side of the crus element S12 is expressed as “P_lower leg”, and the end on the ankle joint J_ankle (J13) side as “D_lower leg”.

F (P_foot / BC) = m_foot × ACC (G_foot / BC) −F (D_foot / BC)
...... (23)
F (P_crus / BC) = m_crus × ACC (G_crus / BC) −F (D_crus / BC)
...... (24)
F (P_thigh / BC) = m_thigh × ACC (G_thigh / BC) −F (D_thigh / BC)
...... (25)

  Here, two F (P_OO / BC) and F (D_OO / BC) appearing in each of the above formulas (23) to (25) are both ends of the rigid element having the name represented by OO. Means a reaction force (translational force vector expressed in the body coordinate system BC) received from an object in contact with each other. In this case, F (D_foot / BC) is equal to the floor reaction force vector Frf (leg / BC) obtained by the floor reaction force estimating means 31. Further, F (D_thigh / BC) = − F (P_foot / BC) and F (D_thigh / BC) = − F (P_crut / BC).

  Therefore, the floor reaction force vector Frf (leg / BC) obtained by the floor reaction force estimating means 31 and the acceleration vector ACC (G_foot) of the center of gravity of the foot element S14 obtained by the whole / element heavy motion calculating means 30 are obtained. Flat part / BC) and the weight m_foot part of the foot part element S14, F (P_foot part / BC), that is, translation acting on the ankle joint J_ankle by the calculation of the right side of the equation (23) Power is required. Further, the acceleration vector ACC of the center of gravity of the crus element S12 obtained by the obtained F (P_foot part / BC) (= −F (D_crus part / BC)) and the whole / element heavy motion calculation means 30. Acts on F (P_crus / BC), that is, knee joint J_knee, by calculating the right side of the equation (24) from (G_crus / BC) and the weight m_crus of the crus element S12 Translation force is required. Similarly, F (P_thigh / BC) is calculated by the calculation of the right side of Expression (25) using the obtained F (P_lower leg / BC) (= F (D_thigh / BC)). That is, a translational force acting on the hip joint J_crotch is required. Thus, reaction force vectors (translation force vectors) acting on the joint elements J_ankle, J_knee, and J_crotch are sequentially calculated based on the equations of motion (23) to (25).

  Next, the equations of motion of the respective foot movements S14, crus element S12, and thigh element S10 (rotational movements around the center of gravity) are given by the following equations (26) to (28). It is done.

M (P_foot part / C_foot part) = I_foot part × ω (foot part / C_foot part) '
+ Ω (foot part / C_foot part) × (I_foot part × ω (foot part / C_foot part))
-(U (COP / C_foot part) -U (G_foot part / C_foot part))
× (R (BC → C_foot part) × F (D_foot part / BC))
-(U (P_foot / C_foot) -U (G_foot / C_foot)))
× (R (BC → C_foot) × F (P_foot / BC))
-R (BC → C_foot part) × M (D_foot part / BC)
...... (26)
M (P_crus / C_crus) = I_crus × ω (crus / C_crus) '
+ Ω (crus / C_crus) × (I_crus × ω (crus / C_crus))
-(-U (G_crus / C_crus))
× (R (BC → C_crus) × F (D_crus / BC))
-(U (P_crus / C_crus) -U (G_crus / C_crus))
× (R (BC → C_crus) × F (P_crus / BC))
-R (BC → C_crus) × M (D_crus / BC)
...... (27)
M (P_thigh / C_thigh) = I_thigh × ω (thigh / C_thigh) '
+ Ω (thigh / C_thigh) × (I_thigh × ω (thigh / C_thigh))
-(-U (G_thigh / C_thigh))
× (R (BC → C_thigh) × F (D_thigh / BC))
-(U (P_thigh / C_thigh) -U (G_thigh / C_thigh))
× (R (BC → C_thigh) × F (P_thigh / BC))
-R (BC → C_thigh) × M (D_thigh / BC)
...... (28)

  Here, two F (P_OO / BC) and F (D_OO / BC) appearing in each of the formulas (26) to (28) are the rigid body elements having the names represented by OO. It means a reaction force moment (a moment vector expressed in the body coordinate system BCd) received from an object in contact with both ends. In this case, M (D_foot / BC) in Expression (26) is zero. M (D_foot / BC) = − R (C_foot → BC) × M (P_foot / C_foot), M (D_thigh / BC) = − R (C_crus) Part → BC) × M (P_crus / C_crus). Further, I_foot, I_crus, and I_thigh are moments of inertia around the center of gravity of the foot element S14, the crus element S12, and the thigh element S10, respectively, and ω (foot Flat part / C_foot part), ω (crus part / C_crus part), and ω (thigh part / C_thigh part) are the foot part element S14, the leg part element S12, and the thigh, respectively. It means the angular velocity around the center of gravity of each subelement S10. In this case, generally, the moments of inertia I_foot, I_crus, and I_thigh are sufficiently small values (values close to 0). Therefore, in this embodiment, I_foot, I_crus And I_thigh are both close to zero.

  Accordingly, the equations (26) to (28) are approximately rewritten into the following equations (29) to (31). In the equations (29) to (31), M (D_foot / BC) = 0, M (D_crus / BC) = − R (C_foot → BC) × M (P_foot Part / C_foot), M (D_thigh / BC) = − R (C_crus → BC) × M (P_crus / C_crus).

M (P_foot / C_foot) =-(U (COP / C_foot) -U (G_foot / C_foot)))
× (R (BC → C_foot part) × F (D_foot part / BC))
-(U (P_foot / C_foot) -U (G_foot / C_foot)))
× (R (BC → C_foot) × F (P_foot / BC))
...... (29)
M (P_crus / C_crus) =-(-U (G_crus / C_crus))
× (R (BC → C_crus) × F (D_crus / BC))
-(U (P_crus / C_crus) -U (G_crus / C_crus))
× (R (BC → C_crus) × F (P_crus / BC))
-R (BC → C_ lower leg)
× (−R (C_foot part → BC) × M (P_foot part / C_foot part))
...... (30)
M (P_thigh / C_thigh) = − (− U (G_thigh / C_thigh))
× (R (BC → C_thigh) × F (D_thigh / BC))
-(U (P_thigh / C_thigh) -U (G_thigh / C_thigh))
× (R (BC → C_thigh) × F (P_thigh / BC))
-R (BC → C_ thigh)
× (−R (C_crus → BC) × M (P_crus / C_crus))
...... (31)

In the present embodiment, M (P_foot / C_foot), that is, the joint moment M (P_foot / C_foot) acting on the ankle joint 13 is calculated by the calculation of the right side of Expression (29). Part) (the foot part coordinate system C_moment vector expressed in the foot part). In this case, U (COP / C_foot portion) on the right side of the equation (29) is the position vector U (COP / BC) of the floor reaction force action point previously obtained by the floor reaction force action point estimation means 32. By multiplying the inverse transformation tensor R (BC → C_foot part) = R (C_foot part → BC) ^T of the transformation tensor R (C_foot part → BC) obtained by the transformation tensor creation means 28 Desired. U (G_foot part / C_foot part) is set in advance, and U (P_foot part / C_foot part) is a predetermined length of the foot element S14. The position vector (0, 0, L14) ^T of the ankle joint J13 in the foot part coordinate system C_foot part determined by L14. Further, F (D_foot / BC) is a value of the floor reaction force vector Frf (leg / BC) previously obtained by the floor reaction force estimating means 31. Further, F (P_foot / BC) is obtained as described above by the equation (23). Accordingly, the conversion tensor R (C_foot → BC) created by the conversion tensor creation means 28 and the position vector U (COP / BC) of the floor reaction force action point obtained by the floor reaction force action point estimation means 32 The floor reaction force vector Frf (leg / BC) obtained by the floor reaction force estimation means 31 and the reaction force vector F (P_foot / BC) obtained by the above equation (23) ) To obtain M (P_foot part / C_foot part), that is, a joint moment acting on the ankle joint 13 (moment vector represented by the foot part coordinate system C_foot part). For the leg 2 that is not in contact with the ground, the position vector U (COP / C_foot part) of the floor reaction force acting point is indefinite, but F (D_foot part / BC) = 0. The value of the first term in equation (29) is 0 regardless of the value of U (COP / C_foot).

Further, M (P_crus / C_crus), that is, a joint moment acting on the knee joint 11 (a crus coordinate system C_moment vector expressed by the crus) by the calculation of the right side of Expression (30). ) Is required. In this case, U (G_crus / C_crus)) on the right side of equation (30) is preset, and U (P_crus / C_crus) is the crus This is a position vector (0, 0, L12) ^T of the knee joint J11 in the lower leg coordinate system C_lower leg (C12) determined by a predetermined length L12 of the element S12. R (BC → C_crus) is an inverse transformation tensor R (C_crus → BC) ^T of the transformation tensor R (C_crus → BC) created by the transformation tensor creation means 28. F (D_crus / BC) is obtained by inverting the sign of F (P_foot / BC) obtained by the above equation (23), and F (P_crus / BC) is , Which is obtained by the equation (24). Further, M (P_foot part / C_foot part) is obtained by the above equation (29). Accordingly, the conversion tensors R (C_crus → BC) and R (C_foot → BC) created by the conversion tensor creation means 28 and the equations previously obtained from the above equations (23) and (24), respectively. Force vectors F (P_foot / BC), F (P_crus / BC), and a preset position vector U (G_crus / C_crus) of the center of gravity of the crus element S12 By calculating the right side of the equation (30) using the length L12 of the crus element S12 and the moment M (P_foot / C_foot) previously obtained by the equation (29), M (P_crus / C_crus), that is, the joint moment acting on the knee joint 11 (crut coordinate system C_moment vector represented by the crus) is obtained.

Similarly, M (P_thigh / C_thigh), that is, a joint moment acting on the hip joint 9 (moment vector expressed in the thigh coordinate system C_thigh) is calculated by calculating the right side of the equation (31). ) Is required. In this case, U (G_thigh / C_thigh)) on the right side of Expression (31) is set in advance, and U (P_thigh / C_thigh) is thigh The position vector (0, 0, L10) ^T of the hip joint J9 in the thigh coordinate system C_thigh, which is determined by the predetermined length L10 of the element S10. R (BC → C_thigh) is an inverse conversion tensor R (C_thigh → BC) ^T of the conversion tensor R (C_thigh → BC) created by the conversion tensor creation means 28. F (D_thigh / BC) is obtained by inverting the sign of F (P_thigh / BC) obtained by the above equation (24), and F (P_thigh / BC) is , Which is obtained by the formula (25). Further, M (P_crus / C_crus) is obtained by the above equation (30). Accordingly, the conversion tensors R (C_thigh → BC) and R (C_crus → BC) created by the conversion tensor creation means 28 and the equations previously obtained from the above equations (24) and (25), respectively. Force vectors F (P_thigh / BC), F (P_thigh / BC), and a preset position vector U (G_thigh / C_thigh) of the center of gravity of the thigh element S10 By calculating the right side of the equation (31) using the length L10 of the thigh element S10 and the moment M (P_crus / C_crus) previously obtained by the equation (30), M (P_thigh / C_thigh), that is, a joint moment acting on the hip joint 9 (thigh coordinate system C_moment vector expressed in the thigh) is obtained.

  As described above, in the calculation process of the joint moment estimating means 35, the joint moments M (P_foot / C_foot) of the ankle joint 13, the knee joint 11 and the hip joint 9 of each leg 2 and M (P_ (Crus / C_crus) and M (P_thigh / C_thigh) are calculated in order from the ankle joint 13 side. The joint moment obtained in this way is used for controlling, for example, a device that assists walking of the human 1 (a device including an electric motor capable of applying an assist torque to the ankle joint 13, the knee joint 11, and the hip joint 9). It is done.

  In the embodiment described above, various arithmetic processes are executed using the body coordinate system BC as a basic coordinate system. And it is only the calculation process of the floor reaction force action point estimation means 32 that performs the calculation process in consideration of the inclination angle of the body coordinate system BC or the waist 6 with respect to the vertical direction. For this reason, it is possible to significantly reduce the arithmetic processing using the inclination angle of the waist 6 or the like with respect to the vertical direction as compared with the conventional case. As a result, even when it is difficult to grasp the tilt angle with high accuracy, accumulation of errors can be minimized and the accuracy of joint moment estimation can be increased. Furthermore, if floor reaction force action point estimation means that does not use an inclination angle is used, a three-dimensional posture sensor or the like is not required in the joint moment estimation system, and the system can be reduced in size and simplified.

  Moreover, in this embodiment, since the process of each means of the arithmetic processing unit 18 is performed in three dimensions, the accuracy of the estimated joint moment can be improved.

The figure which shows typically the whole apparatus structure in embodiment which applied this invention to the human as a biped walking mobile body. The block diagram which shows the structure of the sensor box with which the apparatus of FIG. 1 was equipped. A perspective view of a rigid link model used in an embodiment. The block diagram which shows the functional means of the arithmetic processing unit of FIG. The figure for demonstrating the example of the conversion tensor of the coordinate transformation used by embodiment. The figure for demonstrating the example of the conversion tensor of the coordinate transformation used by embodiment. The figure for demonstrating the estimation method of the floor reaction force vector in the single leg support state of a biped walking mobile body. The figure for demonstrating the estimation method of the floor reaction force vector in the both leg support state of a biped walking mobile body. (A)-(c) is a figure for demonstrating the estimation method of the action point of a floor reaction force vector. The figure for demonstrating the estimation method of the Y-axis direction component of the action point of the floor reaction force vector in the state of FIG.9 (b).

Explanation of symbols

  DESCRIPTION OF SYMBOLS 1 ... Human (biped walking mobile body), 2 ... Leg, 6 ... Lumbar part, 9 ... Hip joint, 10 ... Thigh part, 11 ... Knee joint, 12 ... Lower leg part, 13 ... Ankle joint, 14a ... Middle leg Knee joint, 21-23 ... joint displacement sensor, 16 ... acceleration sensor, S1 ... rigid link model, JU1, JU2, J9, J11, J13 ... joint element, S8, S7, S6, S10, S12, S14 ... rigid body element , BC: body coordinate system, G8, G7, G6, G10, G12, G14: center of gravity of rigid elements, G_total: total center of gravity, Frf: floor reaction force vector, COP: floor reaction force action point (of floor reaction force vector Point of action).

Claims (7)

A rigid link that represents a biped walking mobile body as a connected body in which a plurality of rigid elements are connected by a plurality of joint elements including joint elements corresponding to at least the hip joint and knee joint of each leg of the biped walking mobile body A method for estimating a joint moment acting on at least one joint of each leg of the biped walking moving body using a model,
As a coordinate system fixed to a predetermined one rigid body element of the rigid link model, a first step of sequentially grasping a displacement amount of each joint of the biped walking moving body corresponding to each joint element of the rigid body link model A second step of sequentially grasping at least the value of the acceleration vector at the origin of a preset body coordinate system in the body coordinate system using an output of an acceleration sensor attached to the biped walking moving body; and the biped walking A third step of sequentially grasping a value in the body coordinate system of a floor reaction force vector acting on each leg of the moving body; and a value in the body coordinate system of a position vector of an action point of the floor reaction force vector. The fourth step of sequentially grasping, the displacement amount of each joint of the biped walking moving body grasped in the first to fourth steps, the value of the acceleration vector at the origin of the body coordinate system, and the value of the floor reaction force vector So Using the position vector value of the action point, an inverse dynamics model expressing the relationship between the motion of each rigid element of the rigid link model and the translational force and moment acting on the rigid element using the body coordinate system based comprising a fifth step of sequentially estimating joint moments acting on at least one of the joints of each leg of the biped walking mobile body Rutotomoni,
The position vector of the total center of gravity of the biped walking moving body is sequentially grasped using the displacement amount of each joint of the biped walking moving body obtained in the first step and the rigid link model. And the time series data of the position vector value of the entire center of gravity and the acceleration vector value of the origin of the body coordinate system grasped in the second step, the value of the acceleration vector of the entire center of gravity in the body coordinate system And the biped walking mobile body is in a single-leg support state in which only one of the pair of legs is grounded, or both legs are grounded. An eighth step of sequentially determining whether or not both legs are supported; and a ninth step of sequentially grasping an inclination angle with respect to a vertical direction of a rigid body equivalent part of a biped walking moving body corresponding to a rigid element to which the body coordinate system is fixed. Step and 2 A tenth step of sequentially determining whether or not each leg of the walking moving body is in contact with the ground; and a value in the body coordinate system of the position vector of the ankle joint of each of the grounded legs The displacement amount of each joint of the biped walking moving body obtained by the first step and the value of the position vector of the middle foot joint joint of the foot of the leg in the body coordinate system, the rigid link model, 11 and the position of the position of the overall center of gravity grasped in the sixth step and the ankle joint of each grounded leg grasped in the eleventh step and the leg joint Based on the value of each position vector of the metatarsal joint joint of the foot and the inclination angle grasped in the ninth step, at least the overall center of gravity, the ankle joint of each leg that is in contact with the ground, and the leg Positional relationship of the midfoot phalanx joint of the foot and the ankle of the leg Based on the twelfth step of sequentially grasping the vertical position of the knot, and the grasped total center of gravity, the ankle joint of each leg that is in contact with the ground, and the positional relationship of the midfoot toe joint of the foot of the leg Estimating the position in the horizontal plane of the point of action of the floor reaction force vector acting on the leg, and determining the vertical position of the point of action of the floor reaction force vector acting on the leg based on the vertical position of the ankle joint of the leg A thirteenth step of sequentially estimating,
In the third step, when the motion state of the biped walking mobile body is the single leg support state, the acceleration vector value of the total center of gravity grasped in the seventh step, the total weight of the biped walking mobile body, and the ground contact The value of the floor reaction force vector in the body coordinate system is grasped based on the motion equation of the overall center of gravity of the biped walking moving body represented by the floor reaction force vector acting on the leg that is When the motion state of the walking moving body is the both-leg supporting state, the value of the acceleration vector of the entire center of gravity grasped in the seventh step, the total weight of the biped walking moving body, and the floor reaction force acting on each of the both legs The biped walking moving body from the specific part in which the equation of motion of the entire center of gravity of the biped walking moving body represented by the vector and the floor reaction force vector acting on each leg is predetermined near the lower end of the leg. Towards the overall center of gravity Based on the relational expression between the relative position of the specific part of the leg with respect to the total center of gravity of the biped walking moving body and the floor reaction force vector acting on the leg Grasping the value in the body coordinate system of the floor reaction force vector acting on each leg,
In the fourth step, the action in the body coordinate system is performed based on the position in the horizontal plane and the vertical position of the action point of the floor reaction force vector estimated in the thirteenth step and the inclination angle grasped in the ninth step. A method for estimating a joint moment of a biped walking moving body characterized by grasping a value of a position vector of a point .   2. The joint moment of a biped walking mobile body according to claim 1, wherein the acceleration sensor is attached to a rigid body equivalent part of the biped walking mobile body corresponding to a rigid body element to which the body coordinate system is fixed. Estimation method.   The biped walking movement according to claim 2, wherein the rigid body element having a fixed body coordinate system is a rigid body element that connects a pair of joint elements corresponding to a pair of hip joints of the biped walking moving body. Body joint moment estimation method. The value of the floor reaction force vector and the value of the position vector of the action point that are grasped in the third step and the fourth step, respectively, are three-dimensional values. In the thirteenth step, the leg whose entire center of gravity is grounded When the biped walking moving body is present in the front-rear direction with respect to the ankle joint of the body, the position in the horizontal plane of the ankle joint of the leg is the point of action of the floor reaction force vector acting on the leg. When it is estimated as a position in a horizontal plane and the entire center of gravity is present in the front and rear direction of the biped walking moving body with respect to the midfoot phalanx joint of the foot of the leg that is grounded, the leg The position of the middle foot joint of the foot of the foot of the foot is estimated as the position in the horizontal plane of the point of action of the floor reaction force vector acting on the leg, and the ankle joint of the leg is grounded at the center of gravity. On the other hand, it exists on the front side in the front-rear direction of the biped walking mobile body, and the foot of the leg The center of gravity and the position in the front-rear direction are the same on a line segment connecting the ankle joint of the leg and the middle foot phalanx joint when present 4. The biped walking mobile body according to claim 1 , wherein the position in the horizontal plane is estimated by being positioned in the horizontal plane of the point of action of the floor reaction force vector acting on the leg. Joint moment estimation method. The thirteenth step, the vertical position of the floor reaction force vector acting on the leg in contact with the ground, by a predetermined value determined in advance from the vertical position of the ankle joint of the leg that is grasped by said 12 step vertical The method for estimating a joint moment of a biped walking moving body according to any one of claims 1 to 4, wherein the position is estimated as a position separated downward in the direction. In the tenth step, for the leg that is determined to be grounded, the presence or absence of grounding of the toe side portion and the heel side portion of the foot portion of the leg is further determined. In the twelfth step, In addition to the vertical position of the ankle joint of the leg that is in contact with the ground, the vertical position of the metatarsal joint of the foot of the leg is grasped. In the thirteenth step, the foot is determined in the tenth step. When it is determined that only the toe side portion of the toe side portion and the heel side portion of the part is grounded, the vertical position and the center of the ankle joint grasped in the twelfth step are used instead of the predetermined value. and characterized by estimating the lead straight direction position of the point of the floor reaction force vector by using a vertical distance between the ankle joint and the metatarsophalangeal joint determined from the vertical position of the metatarsophalangeal joints bipedal walking mobile according to claim 5 wherein Joint moment estimation method. The value of the floor reaction force vector grasped in each of the third step and the fourth step and the value of the position vector of the action point are three-dimensional values, according to any one of claims 1 to 6. The joint moment estimation method of the biped walking mobile body of description.

Images