JP2017144512A - Zmp calculation method and device - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a ZMP calculation method and a device not depending on a 6-axial force sensor.SOLUTION: A device comprises a single one- or more-axis force sensor provided in each of links that make contact with a floor surface, joint torque acquisition means for acquiring joint torque of each link that makes contact with the floor surface, and ZMP calculation means. In torque Tabout an x-axis acting on the links that make contact with the floor surface, torque Tabout a y-axis, x-direction force F, y-direction force Fand z-direction force F, the joint torque acquired by the joint torque acquisition means is used as the torque Tabout the x-axis and the torque Tabout the y-axis, and the force Fin the z-direction is acquired by the one- or more-axis force sensor as floor reaction. The ZMP calculation means calculates a ZMP position by using the joint torque, the z-direction force Fand a position of the force sensor while ignoring the x-direction force Fand the y-direction force F.SELECTED DRAWING: Figure 9

Description

The present invention relates to a method and apparatus for calculating a ZMP (zero moment point).

ZMP (zero moment point) is defined as a point at which the horizontal component of the moment due to floor reaction force becomes zero, and is widely used for balance control of humanoids and the like (Non-Patent Document 1).

In a general humanoid, ZMP is often calculated using output information (force of xyz, torque around xyz) of a six-axis force sensor mounted on a foot link. Specifically, the torque T _x around the x-axis acting on the link in contact with the floor, the torque T _y around the y-axis, the force F _{x in} the x direction, the force F _{y in the y} direction, and the force F _{z in the} z direction , T _x , T _y , F _x , F _y , and F _z are all acquired by a six-axis force sensor. The calculation of ZMP using a six-axis force sensor has an advantage that the configuration is simple because one sensor is provided for each humanoid foot. However, the measurement method depending on the 6-axis force sensor has the following drawbacks.

● Small 6-axis force sensors suitable for mounting on humanoids have a low rating for humanoid loads (see Fig. 7). On the other hand, in order to measure a moment with a large rating, it is necessary to use a large sensor, and the configuration tends to increase in size.
● In general, 6-axis force sensors are not designed with a large impact acting on the humanoid foot, and the foot, which is the main attachment of the 6-axis force sensor, is subject to impact load. A failure of the axial force sensor can occur.
● If the ZMP calculation for balance control depends on a six-axis force sensor (used as a non-redundant centralized sensor), sensor failure is fatal in control.
● The 6-axis force sensor is relatively expensive, and if it is designed to measure a moment with a large rating and withstand a shock load, the cost increases further.

As a ZMP calculation method that does not depend on a six-axis force sensor, a method using a plurality of one-axis force sensors or three-axis force sensors (for example, using four three-axis force sensors) is also known. The mounting of the sensor has a problem that the structure becomes complicated.
Humanoid Robot Edited by Shuji Hamada Ohm Co., Ltd.

An object of the present invention is to provide a ZMP calculation method and apparatus that does not depend on a six-axis force sensor, while having a simple configuration.

The technical means adopted by the present invention are:
In the torque T _x around the x axis acting on the link in contact with the floor surface, the torque T _y around the y axis, the force F _{x in} the x direction, the force F _{y in the y} direction, and the force F _z in the z direction,
Using a joint torque obtained by the joint torque obtaining means for obtaining the joint torque of the link torque T _x around x-axis, as the torque T _y around the y-axis,
The floor reaction force acquisition means acquires the force F _{z in the} z direction,
The joint torque and the a z-direction of the force F _z essential element, the x-direction of the force F _x and y direction force F _y as optional elements, ZMP calculation method of and calculates the position of the ZMP,
and,
Floor reaction force acquisition means for acquiring a floor reaction force acting on a link in contact with the floor surface;
A joint torque acquisition means for acquiring a joint torque of a link in contact with the floor surface;
ZMP calculation means,
With
In the torque T _x around the x axis acting on the link in contact with the floor surface, the torque T _y around the y axis, the force F _{x in} the x direction, the force F _{y in the y} direction, and the force F _z in the z direction,
Using a joint torque obtained by the joint torque obtaining section torque T _x around x-axis, as the torque T _y around the y-axis,
The floor reaction force acquisition means acquires the force F _{z in the} z direction,
The ZMP calculating means, and wherein the joint torque and the z-direction of the force F _z essential element, the x-direction of the force F _x and y direction force F _y as optional elements, to calculate the position of the ZMP ZMP calculation device.

In one aspect, the link that contacts the floor is a humanoid link (foot link), but the link that contacts the floor is a link (foot link) of a multi-legged robot other than two legs. Also good.
In the embodiment described later, a wire-driven musculoskeletal humanoid is illustrated, but the type of robot to which the present invention can be applied is not limited.
Further, although the contents of the present invention are not limited, in this specification, the front-rear direction of the robot is the x-axis (the front is +), the left-right direction is the y-axis (left is +), and the up-down direction is the z-axis ( The right-handed coordinate system has an upward direction of +).
Here, the technical scope covered by the concept of ZMP is not limited to stabilization control of robots such as humanoid robots, but is a concept that can be widely used to prevent falls of objects (including human bodies and personal mobility). is there.
In one aspect, the target for calculating the position of the ZMP may be a human body wearing a body-mounted device, and the calculated ZMP may be used to maintain a human balance with the body-mounted device. In addition, control for preventing a fall may be performed. For example, a body-worn device is an assist suit that controls balance of the human body and prevents falls.

In one aspect, the joint torque acquisition means acquires joint torque using muscle tension and a coefficient for converting muscle tension into joint torque.
In one exemplary aspect, the coefficient is a Jacobian (Jacobi transpose matrix). It is well known to those skilled in the art that torque can be calculated by a (moment arm) * F (muscle tension), and when a changes according to posture (joint angle) and the number of muscles is large, Calculate torque using Jacobian. Note that the coefficient is not limited to Jacobian. If a does not change according to the posture (such as using a pulley with a constant diameter), or if the number of muscles is one, simply use a moment arm. The torque may be obtained.

In an aspect in which joint torque is calculated using muscle tension, it is assumed that the target robot includes an element (typically a wire) that substitutes muscle. Examples of such robots include musculoskeletal robots, wire drive robots, and pneumatic drive robots. In a musculoskeletal robot, a muscle actuator (motor + wire, air pressure, or linear cylinder) is an element corresponding to a muscle. In the wire driven robot, the wire is an element corresponding to the muscle. In a pneumatic drive robot, a pneumatic actuator (or a wire attached to the pneumatic actuator) is an element corresponding to a muscle.
Moreover, in the aspect of the human body wearing the body-worn device, for example, the wire driving mechanism constituting the body-worn device has a wire stretched so that joint torque (for example, ankle joint torque) can be calculated. The joint torque is calculated by measuring the tension of the wire.

In one aspect, the floor reaction force acquisition means is a uniaxial or more force sensor.
For each link, one force sensor with one or more axes may be provided.
In one aspect, the floor reaction force acquisition means is a uniaxial force sensor or a triaxial force sensor.
When a force sensor with three or more axial forces is used as the floor reaction force acquisition means, the force sensor can acquire the force F _{x in} the x direction and the force F _{y in the} y direction. In this case, the ZMP is calculated. in the force F _x in the x-direction, (in this case, the formula to be described later (2), (3) can be used) y direction of the force F _y may be used, or, as described below, x The direction force F _x and the y direction force F _y (which can be acquired) may not be used.
Further, in the aspect of the human body wearing the body-mounted device, for example, the vertical reaction force may be measured from a uniaxial force sensor embedded in a foot (shoe) of the human body.

In one embodiment, the position of the ZMP is calculated by the following formula, ignoring the force F _{x in} the x direction and the force F _{y in the} y direction.
here,
p _{m, x} : ZMP x coordinate,
p _{m, y} : y coordinate of ZMP,
p _jx : x coordinate of each force sensor,
p _jy : y coordinate of each force sensor,
f _jz : z direction force at each force sensor,
τ _{m, jx} : Torque around the x axis based on the tension at each joint,
τ _{m, jy} : Torque around y-axis based on tension at each joint,
j (1,…, N): Link number that contacts the floor,
It is.

That is, each joint torque τ _{m, jx} , τ _{m, jy of} each link acquired by the joint torque acquisition means, f _jz acquired by a force sensor of one or more axes, position p _{jx of} each one or more force sensor, p _jy , ZMP positions p _{m, x} and p _{m, y} are calculated.
As the positions p _jx and p _jy of one or more force sensors, for example, in the humanoid (N = 2), the center position of both feet in an upright posture may be used as the origin, and the distance from there may be measured. For example, the value is stored in the storage unit as an initial value, and when the foot is moved in an upright state, the initial value is used as it is. May be used by updating the initial value (which is understood by those skilled in the art that position calculation is possible using each joint angle and link length).
The height position p _jz of one or more force sensors is preferably as close as possible to the floor surface. It is considered that the closer the force sensor of one or more axes is to the floor surface, the less influence of ignoring the force F _{x in} the x direction and the force F _{y in the} y direction.

In one aspect, the position information of the force sensor is used as arbitrary information. In the above formula, p _jx = 0, p _jy = 0, and one or both of the calculated values p _{m, x} , p _{m, y} are used. It may be used to control.
For example, in the embodiments described later, with respect to _{pm , x in} the front-rear direction, τ _{m, jy} is dominant, and it is considered that a relatively effective value can be obtained. Considering the application to be used, such as an assist suit and simple front-rear stabilization control, there is a possibility of control using only _{pm, x in the} front-rear direction.

In one aspect, the joint torque acquisition means is a joint torque sensor.
As the joint torque sensor, for example, a (strain gauge type) torque sensor, a capacitance type torque sensor, and a magnetostrictive torque sensor are known, and a commercially available sensor or a unique one based on these principles is used. It is possible to use the newly developed sensor.
In a robot that uses a motor to directly drive a joint instead of a wire drive system, a joint torque is acquired using a joint torque sensor.

The floor reaction force acquisition means is not limited to a force sensor having one or more axes, for example,
The first node (foot) that contacts the floor, the second node (shin) connected to the first node via the first joint, and the third node (second portion) connected to the second node via the second joint ( The floor reaction force may be acquired using the joint torque and joint angle of the first joint (ankle joint), the second joint (knee joint), and the third joint (hip joint). Alternatively, the floor reaction force may be acquired from the force plate in an environment equipped with the force plate.

According to the present invention, a value necessary for ZMP calculation, which has been obtained using T _x , T _y , F _x , F _y , F _z of the six-axis force sensor so far, is obtained by another means (joint torque acquisition means, Floor reaction force acquisition means).
T _x and T _y are moment loads acting on the link contacting the floor surface, and can be obtained by obtaining the joint torque of the link. The joint torque acquisition means includes a technique using muscle tension (wire tension) and a joint torque sensor.
F _z is a vertical reaction force, and an F _z component of a force sensor having one or more axes can be used.

Therefore, according to the present invention, ZMP can be calculated independent of the 6-axis force sensor.
A relatively inexpensive and compact uniaxial force sensor (which has a higher rating than the six-axis force sensor) can be used as a force sensor that requires impact resistance and a large rating.
In the case of using the one-axis force sensors, x-direction of the force F _x, ignoring the y direction of the force F _y, performs the calculation of the position of the ZMP, can be stabilized control by using the position of the ZMP It is.
In a mode in which muscle tension (wire tension) is used for moment load measurement, since measurement of wire tension is relatively easy, measurement of a large moment load is possible with a compact configuration.
In addition, the redundancy of the sensor becomes possible, and the failure reliability is improved.

It is a figure which shows the upper half body of the musculoskeletal humanoid which concerns on this embodiment. It is a figure which shows the backbone structure of the musculoskeletal humanoid which concerns on this embodiment. It is a figure which shows the muscle arrangement | positioning of the leg part of the musculoskeletal humanoid which concerns on this embodiment. It is a figure which shows joint arrangement | positioning of the leg part of the musculoskeletal humanoid which concerns on this embodiment. It is a figure which shows the muscle arrangement | positioning of the ankle of the musculoskeletal humanoid which concerns on this embodiment. It is a figure which shows the geometric structure of ZMP calculation. It is a figure which compares and shows the torque acquired from the muscle tension which concerns on this embodiment, and the torque acquired from the 6-axis force sensor. An ankle geometric condition for deriving a ZMP according to the present embodiment is shown. It is a block diagram of ZMP calculation concerning this embodiment. It is a figure which shows the comparison of ZMP computed using the 6-axis force sensor and ZMP computed according to this embodiment. It is a figure which shows the view of the control using a spine stabilizer. It is a flowchart which shows the sequence of a spine stabilizer. It is a figure which shows the forward bending operation | movement of the musculoskeletal humanoid which concerns on this embodiment. It is a figure which shows the experimental data of forward bending operation control. It is a figure which shows the back curvature operation | movement of the musculoskeletal humanoid which concerns on this embodiment. It is a figure which shows the experimental data of back curvature operation | movement control. It is a figure which shows the weight load experiment with respect to the musculoskeletal humanoid which concerns on this embodiment. It is a figure which shows the data of the experiment of control at the time of weight load.

[A] Musculoskeletal humanoid according to the present embodiment The muscle (muscle wire) arrangement of the musculoskeletal humanoid according to the present embodiment (referred to as “tenro shiro”) selects a muscle important for the basic motion of the human body, It is a layout that imitates the human body based on science. FIG. 1 and Table 1 show muscle arrangements and muscle names (with numbers and associated joints) of the upper body of the humanoid according to the present embodiment, respectively.
The spine structure in the upper half of the humanoid according to the present embodiment is shown in FIG. The spine structure is composed of five vertebrae.

FIG. 3 and Table 2 show the muscle arrangement and name (with numbers and associated joints) of the leg of the humanoid according to the present embodiment, respectively.
FIG. 4 shows the joint arrangement of the leg portions of the humanoid according to the present embodiment. FIG. 5 shows an ankle muscle arrangement of the humanoid ankle according to the present embodiment, and the ankle includes six muscles (numbers: 7, 8, 9, 23, 24, 25) and a six-axis force sensor.

The humanoid according to the present embodiment has 64 degrees of freedom (excluding the face and hands). Each joint is driven by a muscle actuator (wire and motor). The total number of actuators is 105 (55 for the upper body and 50 for the legs). Table 3 shows the movable range of the joint.
Note that the musculoskeletal humanoid according to the present embodiment is merely an example of a wire-driven musculoskeletal humanoid, and does not limit the target to which the present invention can be applied.

[B] Calculation of ZMP
The derivation of ZMP is described in, for example, Non-Patent Document 1, and the position of ZMP can be calculated using the output information of a force sensor attached to the foot when the humanoid foot is pressed against the floor surface. it can. According to the geometric condition shown in FIG. 6, the moment acting on the point p = [p _y p _y p _z ] ^T can be described as follows.
Here, p is the position of ZMP. p _j (j = 1,..., N) is the position of the force sensor, and it is assumed that force f _j and moment τ _j are measured at p _j . The position of ZMP is calculated by solving the above equation for p _x and p _y with the x and y components on the right side set to 0.

That is, the positions p _x and p _y of ZMP can be described as follows.
here,
p _x : X coordinate of ZMP,
p _y : y coordinate of ZMP,
p _z : z coordinate of ZMP,
p _jx : x coordinate of each force sensor,
p _jy : y coordinate of each force sensor,
p _jz : z coordinate of each force sensor,
f _jx : Force in x direction at each force sensor,
f _jy : force in y direction at each force sensor,
f _jz : z direction force at each force sensor,
τ _jx : Torque around x axis at each force sensor,
τ _jy : Torque around y-axis at each force sensor,
j (1, ..., N): Link number that contacts the floor,
It is.

In a general humanoid, ZMP is calculated using output information of a six-axis force sensor mounted on a foot link. However, as mentioned in the section of the prior art, there is a drawback in the measurement method depending on the 6-axis force sensor. Hereinafter, ZMP calculation that does not depend on the six-axis force sensor, specifically, ZMP calculated using muscle tension and floor reaction force (referred to as “muscle ZMP”) will be described.

[C] Muscle ZMP in musculoskeletal humanoids
[C-1] Joint Torque in Musculoskeletal Humanoid Acquisition of joint torque based on muscle tension (wire tension) in a musculoskeletal humanoid will be described. In a musculoskeletal humanoid with n muscles at the ankle joint, the joint torque vector T (∈R ² ) around the x, y, z axes is the muscle tension vector F (∈R ⁿ ) at the ankle, muscle Jacobian G ( ΕR ^{n × 2} ) and is obtained from the following conversion formula.
here,
T: joint torque vector,
F: wire tension vector,
G: Jacobian (wire length-joint angle),
τ _{m, x} : Joint torque around x axis,
τ _{m, y} : Joint torque around y-axis,
f _n : Each wire tension,
It is.

The muscle Jacobian is a Jacobian matrix that correlates the muscle length vector L and the joint angle vector θ, and the muscle Jacobian is
It is requested from.

As shown in FIG. 5, the humanoid according to the present embodiment has six muscles (numbers: 7, 8, 9, 23, 24, 25) for the ankle, and the torque of the ankle joint is the six muscles. It can be obtained from the muscle tension (wire tension). As a means for acquiring wire tension, a tension measuring mechanism using a uniaxial force sensor such as a load cell is known to those skilled in the art. A specific example of such a tension measuring mechanism is
Yuki Asano, Toyotaka Kozuki, Soichi Ookubo, Koji Kawasaki, Takuma Shirai, Kohei Kimura, Kei Okada and Masayuki Inaba: A sensor-driver integrated muscle module with high-tension measurability and flexibility for tendon-driven robots, Intelligent Robots and Systems (IROS ), 2015 IEEE / RSJ International Conference, pp. 5960-5965
Can be referred to as appropriate.

FIG. 7 shows a comparison between the joint torque obtained from the muscle tension (wire tension) and the torque acquired by the six-axis force sensor. The small 6-axis force sensor has a low rating with respect to the load of the humanoid, and it has been shown that a large torque could not be measured. Incidentally, humanoid according to this embodiment is provided with the six-axis sensor to the foot link, in the calculation of the muscular ZMP according to the present embodiment, the six-axis force sensor, in order to obtain only the floor reaction force f _z Used for.

[C-2] Calculation of muscle ZMP Normally, in order to calculate the position of ZMP, T _x , T _y , F _x , F _y , and F _z at the link that contacts the floor surface are required. In this embodiment, it is assumed that the horizontal forces F _x and F _y have little influence on the calculation of the ZMP position (if the force sensor provided on the link is close to the floor, these influences are It is considered that the horizontal forces F _x and F _y which have been obtained by the conventional 6-axis force sensor are omitted. Therefore, the values used for calculating the position of ZMP are T _x , T _y , and F _z .

As T _x and T _y , joint torques τ _{m, x} , τ _{m, y} acquired from the muscle tension f _muscle are used instead of the torque acquired by the six-axis force sensor. As F _z , a floor reaction force f _z measured by a force sensor (FT sensor) having one or more axes is used. In the normal ZMP calculation formulas (2) and (3), it is assumed that the values of f _x and f _y are very small and are ignored. By rewriting the equations (2) and (3) _, the calculation equations (5) and (6) for the positions pm _{, x} and pm _{, y} of the muscle ZMP are obtained. The ankle geometric conditions are shown in FIG.

[C-3] Calculation formula of muscle ZMP (general case)
here,
p _{m, x} : x coordinate of muscle ZMP,
p _{m, y} : y coordinate of muscle ZMP,
p _jx : x coordinate of each force sensor,
p _jy : y coordinate of each force sensor,
f _jz : z direction force at each force sensor,
τ _{m, jx} : Torque around the x axis based on the tension at each joint,
τ _{m, jy} : Torque around y-axis based on tension at each joint,
j (1, ..., N): Link number that contacts the floor,
It is.

[D-4] Calculation formula of muscle ZMP (specific case of bipod)
In the case of biped (N = 2), the muscle ZMP is calculated by the following formula.

here,
p _{m, x} : x coordinate of muscle ZMP,
p _{m, y} : y coordinate of muscle ZMP,
p _Lx : x coordinate of left foot force sensor,
p _Ly : y coordinate of left foot sensor
p _Rx : x coordinate of right foot force sensor,
p _Ry : y coordinate of the right foot force sensor,
f _Lz : Force in the z direction at the left foot force sensor,
f _Rz : Force in z direction with right foot force sensor,
τ _{m, Lx} : Torque around the x-axis based on the tension at the left ankle joint,
τ _{m, Ly} : Torque around the y-axis based on the tension at the left ankle joint,
τ _{m, Rx} : Torque around the x axis based on the tension at the right ankle joint,
τ _{m, Ry} : Torque around the y-axis based on the tension at the right ankle joint,
It is.

FIG. 9 shows a block diagram for calculating the position of the ZMP according to the present embodiment. The ZMP calculation device includes a floor reaction force acquisition means provided at each link that contacts the floor surface, a joint torque of each link that contacts the floor surface (a first link that contacts the floor surface, and an adjacent first link). A joint torque acquiring means for acquiring a torque of a joint connecting the second link, and a ZMP calculating means.

In the present embodiment, the floor reaction force acquisition means is a single one-axis or more force sensor (in the figure, “one-axis force sensor” is illustrated) for each link, and the floor reaction force f _iz is acquired by the sensor. The The positions p _jx and p _jy of each one or more force sensors are known or can be calculated. The joint torque acquisition means acquires joint torques τ _{m, jx} and τ _{m, jy} using muscle tension and a Jacobian that converts muscle tension into joint torque. The ZMP calculating means calculates the position P _{m, x} , p _{m, y} of the muscle ZMP using f _iz , p _jx , p _jy , τ _{m, jx} , τ _{m, jy} .

Such a ZMP calculating apparatus includes a one-axis or more force sensor as a floor reaction force acquisition means, a uniaxial force sensor (load cell or the like) as a muscle tension acquisition means, a computer constituting a joint torque calculation or ZMP calculation means. (Including at least a storage unit and a calculation unit).

FIG. 10 shows ZMP acquired using a six-axis force sensor when the musculoskeletal humanoid according to the present embodiment is moved so that the center of gravity locus of the upright posture is along a circle, joint torque and floor based on muscle tension. This is shown in comparison with the reaction muscle ZMP. Both values were confirmed to be almost the same. Note that when the influence of the horizontal forces f _x and f _y was verified, in the present embodiment, the value of f _x is smaller than the value of T _y , while the value of f _y is smaller than the value of T _x. It turned out to be relatively large. Therefore, in terms of accuracy, it can be said that the muscle ZMP _x is high and the muscle ZMP _y is lower than the muscle ZMP _x .

Position p _m of muscle ZMP can be used as an indicator of stability control. In one aspect, control is performed to cause the obtained muscle ZMP to follow the target ZMP (a stable position that does not fall). Specifically, a predetermined joint so as to reduce the deviation between the muscle ZMP and the target ZMP (in the embodiment described later, it is a spinal joint, but is not limited thereto, and may be a hip joint, an ankle joint, or the like). The balance of humanoid is controlled by driving.

[E] Spine stabilizer for balance control [E-1] The spine stabilizer humanoid does not fall when the projection point of the center of gravity on the floor surface is within a so-called support polygon. As shown in the left diagram of FIG. 11, when the humanoid is in an upright posture, the projection point of the center of gravity of the whole body on the floor surface is located within the support polygon. From the state shown in the left figure, when only the ankle and hip joint are tilted forward by movement and the posture changes as shown in the central figure, the projection point of the center of gravity of the whole body on the floor surface is located outside the support polygon. The spine stabilizer controls the whole body so that the projection point of the center of gravity on the floor surface is located within the support polygon. As shown in the right diagram of FIG. 11, the spine stabilizer according to the present embodiment compensates for the movement of the center of gravity of the whole body caused by the movement of the legs by controlling the spine so as to move the center of gravity of the upper body. Is located within the support polygon. That is, the spine stabilizer compensates for the movement of the center of gravity of the leg by moving the center of gravity of the upper body using the movable range of the spine.

FIG. 12 shows a flowchart of control using the spine stabilizer. At the start of the stabilizer sequence, the humanoid is set upright on the floor, and the muscle ZMP is calculated to be the target muscle ZMP _ref . Next, each joint torque is estimated using the muscle tension (wire tension) of each muscle (wire) to obtain the joint torque. In the control by the spine stabilizer, the target spine angle determined by the muscle ZMP and the PI control based on the error between the target muscle ZMP _ref and the muscle ZMP in this cycle is calculated. The humanoid operates based on the determined spine angle, and the muscle length, muscle tension, and floor reaction force are updated. The control cycle was 8 [ms]. In addition, the spine stabilizer compensates for humanoid motion generated using each joint angle.

[E-2] Spine Stabilizer Based on Muscle ZMP The spine stabilizer is implemented to compensate for the balance of humanoid by spine roll and pitch motion. This motion is generated by PI control of the spine angle based on the error between the target ZMP and the current muscle ZMP. That is, the spine is controlled as follows.
Here, e (t) is a ZMP error in each control cycle, and p _x ^ref and p _y ^ref are x and y elements of the target ZMP. The initial value of the spine stabilizer sequence was used as the target ZMP.

[E-3] Forward bending control, rear warpage control,
A forward bending experiment and a back warping experiment were performed along the sagittal plane. In the experiment, the ankle and hip angles determined by the operator are input, and the stabilizer compensates for the balance of the humanoid. In the experiment, the angles of the head and pelvis were acquired by an inertial measurement device (IMU) mounted on each link. FIG. 13 shows the change in posture of the forward bending experiment, and FIG. 14 shows a plot of the experimental data. FIG. 15 shows changes in the posture of the back warping experiment, and FIG. 16 shows a plot of the experimental data. By using the muscle ZMP according to the present embodiment, the humanoid balance operation as shown in FIGS. 13 and 15 is possible. The current ZMP was calculated from the muscle ZMP, and control was performed to follow the stable target ZMP with PI control. As shown in FIGS. 14 and 16, the current muscle ZMP follows the ZMP _ref , which is the target ZMP, and it can be seen that the muscle ZMP is effective for balance control.

[E-4] A balance control humanoid with a weight was subjected to a verification experiment by applying a load using the weight. An experiment was conducted by adding a weight to a bag attached to a humanoid end effector. The humanoid was able to continue standing with a weight of 15 [kgf]. FIG. 17 shows the posture of the experiment (the right figure shows a state where a weight of 15 [kgf] is loaded), and FIG. 18 shows a plot of the experimental data. It was confirmed that the spine stabilizer works properly. Regarding floor reaction force, the floor reaction force was 477 [N] at 0 [s] and 622 [N] at 110 [s], so 15 [kgf] (147 [N]) was loaded on the humanoid. It was confirmed.

Claims (12)

In the torque T _x around the x axis acting on the link in contact with the floor surface, the torque T _y around the y axis, the force F _{x in} the x direction, the force F _{y in the y} direction, and the force F _z in the z direction,
Using a joint torque obtained by the joint torque obtaining means for obtaining the joint torque of the link torque T _x around x-axis, as the torque T _y around the y-axis,
The floor reaction force acquisition means acquires the force F _{z in the} z direction,
The joint torque and the a z-direction of the force F _z essential element, the x-direction of the force F _x and y direction force F _y as optional elements, ZMP calculation method of and calculates the position of the ZMP.   The ZMP calculation method according to claim 1, wherein the joint torque acquisition unit acquires joint torque using muscle tension and a coefficient for converting muscle tension into joint torque.   The ZMP calculation method according to claim 1, wherein the floor reaction force acquisition unit is a force sensor having one or more axes.   The ZMP calculation method according to claim 3, wherein the one-axis or more force sensor is a one-axis force sensor or a three-axis force sensor. 5. The ZMP calculation method according to claim 3, wherein the position of the ZMP is calculated by the following formula ignoring the force F _{x in} the x direction and the force F _{y in the} y direction.
here,
p _{m, x} : ZMP x coordinate,
p _{m, y} : y coordinate of ZMP,
p _jx : x coordinate of each force sensor,
p _jy : y coordinate of each force sensor,
f _jz : z direction force at each force sensor,
τ _{m, jx} : Torque around the x axis based on the tension at each joint,
τ _{m, jy} : Torque around y-axis based on tension at each joint,
j (1,…, N): Link number that contacts the floor,
It is.   The ZMP calculation method according to claim 1, wherein the joint torque acquisition unit is a joint torque sensor. Floor reaction force acquisition means for acquiring a floor reaction force acting on a link in contact with the floor surface;
A joint torque acquisition means for acquiring a joint torque of a link in contact with the floor surface;
ZMP calculation means,
With
In the torque T _x around the x axis acting on the link in contact with the floor surface, the torque T _y around the y axis, the force F _{x in} the x direction, the force F _{y in the y} direction, and the force F _z in the z direction,
Using a joint torque obtained by the joint torque obtaining section torque T _x around x-axis, as the torque T _y around the y-axis,
The floor reaction force acquisition means acquires the force F _{z in the} z direction,
The ZMP calculating means, and wherein the joint torque and the z-direction of the force F _z essential element, the x-direction of the force F _x and y direction force F _y as optional elements, to calculate the position of the ZMP ZMP calculator.   The ZMP calculation apparatus according to claim 7, wherein the joint torque acquisition unit acquires joint torque using muscle tension and a coefficient for converting muscle tension into joint torque.   The ZMP calculation apparatus according to claim 7, wherein the floor reaction force acquisition unit is a force sensor having one or more axes.   The ZMP calculation apparatus according to claim 9, wherein the one or more axis force sensor is a one-axis force sensor or a three-axis force sensor. 11. The ZMP calculation according to claim 9, wherein the ZMP calculation unit calculates the position of the ZMP according to the following formula, ignoring the force F _{x in} the x direction and the force F _{y in the} y direction. apparatus.
here,
p _{m, x} : ZMP x coordinate,
p _{m, y} : y coordinate of ZMP,
p _jx : x coordinate of each force sensor,
p _jy : y coordinate of each force sensor,
f _jz : z direction force at each force sensor,
τ _{m, jx} : Torque around the x axis based on the tension at each joint,
τ _{m, jy} : Torque around y-axis based on tension at each joint,
j (1,…, N): Link number that contacts the floor,
It is. The ZMP calculation apparatus according to claim 7, wherein the joint torque acquisition unit is a joint torque sensor.