JPH11300660A - Controller for leg-type mobile robot - Google Patents

Abstract

PROBLEM TO BE SOLVED: To easily and properly control an actual floor reactive force applied on a robot without any interference by providing a compensating displacement calculating means or the like. SOLUTION: An upper body Z direction acceleration control and arithmetic operation unit calculates the component (target value) Fdmd of a compensating full-floor reactive force so as to prevent actual upper body up-and-down acceleration Gbodyz detected by an inclination sensor 60 from being largely deviated from target upper body acceleration Gbodyref. For this purpose, first, with the Z component of Gbodyref set as Gbodyrefz, deviation Gdmdtmp between the target value and the detected value is calculated based on an expression. In the expression, Kgx, Kgy and Kgz are control gains (constants). Fdmdtmpx, Fdmdtmpy and Fdmdtmpz are X, Y and Z components of deviation Fdmdtmp and, by totaling these, deviation Fdmttmp is obtained. Subsequently, the calculated Fdmdtmp is passed through a filter properly designed for control characteristic improvement, and its output is set as Fdmd. In this case, even if Fdmd as a floor reactive force is additionally generated around a target full-floor reactive force, Fdmd is applied toward the position of the center gravity of the robot.

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a control device for a legged mobile robot, and more particularly, to a posture control device for the legged mobile robot. The present invention relates to an apparatus for appropriately controlling a floor reaction force acting on a mobile robot.

[0002]

2. Description of the Related Art The most basic and simple legged mobile robot,
More specifically, the control device of the bipedal walking robot includes a target motion pattern generation device and a joint drive control device. The target motion pattern generation device generates at least a target motion pattern. Typically, the gait movement pattern is then calculated by kinetic calculations, ie, Z, which is determined by solving the Euler-Newton equation.
The MP trajectory is generated so as to be a desired trajectory set in advance. The joint drive control device controls each joint so as to follow a displacement command of each joint generated by the gait generating device.

Here, ZMP (Zero Moment Point) is
The moment of the resultant force of the inertial force and gravity generated by the motion pattern becomes zero, except for the component around the vertical axis, which means an action point on the floor surface.

[0004] In this device, as shown in FIG. 34, the gait generator generates the gait on the assumption that the gait is on a flat floor, but the gait generator is currently in the early stage of the two-leg supporting period. ,
If the foot on the front side steps on an unexpected road surface, an excessive floor reaction force greater than expected on the foot is generated, and the robot tilts. In order to solve the problem, the present applicant has proposed such a control device of a bipedal legged mobile robot in, for example, Japanese Patent Application Laid-Open No. 5-305586.

In this method, the required amount of restoring moment required for restoring the body posture by detecting the body inclination is determined, and the actual total floor reaction force around the desired total floor reaction force center point (target ZMP) is determined. A moment component is detected, and each foot is controlled so as to move up and down and rotate so as to match the required moment with the restoration moment. The actual total floor reaction force moment is a moment generated by the resultant force of each actual foot floor reaction force around the target total floor reaction force center point (target ZMP).

The control proposed above (hereinafter referred to as "double-leg compliance control") will be described with reference to an example in which there is an unexpected inclination as shown in FIG. For explanation, each foot is numbered as shown in FIG. Although the gait generator generates the gait on the assumption of a flat floor, as shown in FIG. 34, the slope on which the first foot is not expected in the early stage of the both-legs support period at present. Step 1
Assume that this is the moment when a foot floor reaction force greater than the desired value for the foot occurs. It is also assumed that the robot is still in a desirable posture (body tilt 0) at this moment.

In the proposed control device, the actual total floor reaction force moment around the target total floor reaction force center point (target ZMP) is detected. At this moment, this actual total floor reaction force moment is
Since the vertical component of the first foot floor reaction force is excessive, it acts in a direction to cause the robot to fall later.

In order to reduce this moment to zero, as shown in FIG. 35, a virtual floor surface AA 'is assumed, and the virtual floor surface is set to a desired total floor height while each foot is placed on the virtual floor surface. The position of each foot is moved to a position rotated by an appropriate angle Δθ around the force center point (target ZMP).

As a result, the vertical component of the first foot floor reaction force decreases, and the vertical component of the second foot floor reaction force increases. As a result, the actual total floor reaction force moment around the desired total floor reaction force center point (target ZMP) becomes substantially zero. That is, even if there is an unexpected slope on the floor, the two-leg compliance control works normally, so that the robot can continue walking without falling down.

[0010] However, since the actual floor reaction force of each foot cannot be controlled during the period of supporting both legs by using the proposed technique alone, unexpected local inclination and unevenness in the floor shape around the foot contact point of the foot is not possible. In some cases, the ground contact of the foot is reduced and the spin is easily performed, or a sudden change in posture causes a fall.

For example, as shown in FIG. 36, if the toes of the first foot step on unexpected projections (steps) during the period of supporting both legs, the toes of the first foot suddenly increase during the period of supporting both legs. Since the floor is gradually falling, the floor is strongly kicked with a toe and the vertical component of the first foot floor reaction force increases rapidly.
As a result, an actual total floor reaction force moment is suddenly generated around the target total floor reaction force center point (target ZMP), and in the worst case, the vehicle falls down in time to restore the posture by the two-leg compliance.

Even if the player does not fall down during the two-leg support period, when the second foot is separated from the floor immediately thereafter, the desired total floor reaction force center point (target ZMP) is at the heel of the first foot. Nevertheless, because the heel is floating, the actual total floor reaction force center point is at the toe, so the actual total floor reaction force that tries to defeat the robot around the target total floor reaction force central point (target ZMP) later A moment is generated and it falls.

That is, this two-leg compliance control is
It can respond to global inclinations and undulations that change slowly over long distances, but cannot respond to local inclinations and steps at the landing point of the foot.

In addition to the above-described two-leg compliance control, the present applicant has disclosed in Japanese Patent Application Laid-Open No. 5-305584, for example, that a biped walking robot is provided with a landing impact absorbing mechanism having spring characteristics such as rubber at the ankle. Proposes an ankle compliance control for detecting an actual foot floor reaction force moment component around each ankle and rotating each ankle to make it zero.

In order to solve the above problems, in addition to the two-leg compliance control, Japanese Patent Application Laid-Open No. HEI 5-30558.
The technique proposed in Japanese Patent Publication No. 4 (hereinafter referred to as "ankle compliance control") can be used in combination.

As a result, as shown in FIG. 37, the first ankle is rotated in the direction to cancel the unexpected first foot floor reaction force moment by the ankle compliance control, as shown in FIG.
The heel can be grounded to the floor. Therefore, the robot does not fall down as described above even in the subsequent one-leg supporting period.

[0017]

However, simply using the two-leg compliance control and the ankle compliance control simply interferes with each other, and the actual total floor reaction force and the actual foot floor reaction force are desirable. There was a problem of deviation from the value or oscillation.

SUMMARY OF THE INVENTION Accordingly, an object of the present invention is to solve the above-described disadvantages, and a leg type capable of easily and appropriately controlling the actual floor reaction force acting on a leg type mobile robot without causing interference. A control device for a mobile robot is provided.

Further, when the robot encounters an unexpected step while walking, for example, when the actual floor surface is lower than the expected floor surface, the acceleration of the upper body (substrate) becomes excessive and the landing impact is caused. May increase. In addition, the robot has a natural vibration that is vertically displaced and determined by its mechanism and mass. Although the vibration is slight as the amount of displacement, it sometimes lowers the contact property. Therefore, it is desirable to perform compliance control on the force component of the actual floor reaction force received by the robot.

Accordingly, a second object of the present invention is to provide a leg-type moving apparatus which performs compliance control on a force component of an actual floor reaction force received by a robot to absorb a landing impact and to improve a contact property. An object of the present invention is to provide a control device for a robot.

A third object of the present invention is to provide a leg-type system that is not significantly affected by unexpected floor shape changes including local irregularities and slopes as well as global undulations and slopes. An object of the present invention is to provide a control device for a legged mobile robot that can appropriately control a floor reaction force acting on the mobile robot.

A fourth object of the present invention is to provide a legged mobile robot capable of facilitating posture stabilization control of a legged mobile robot by further controlling a floor reaction force acting on the legged mobile robot. It is to provide a control device.

A fifth object of the present invention is to further improve the ground contact of the legged mobile robot by appropriately controlling the floor reaction force acting on the legged mobile robot, thereby making it possible to prevent slippage during walking and the aforementioned spin. An object of the present invention is to provide a control device for a legged mobile robot, which can prevent the occurrence of the problem.

A sixth object of the present invention is to control a legged mobile robot capable of reducing the load on the actuator of the legged mobile robot by appropriately controlling the floor reaction force acting on the legged mobile robot. It is to provide a device.

[0025]

In order to achieve the above object, according to the present invention, at least a base is connected to the base via a first joint, and a first end is connected to the base. In a control device for a legged mobile robot including a plurality of legs having legs connected via two joints, a motion pattern including at least a target position and a posture of the legs of the robot; Gait generating means for generating a gait of the robot including at least a target pattern of a total floor reaction force acting on the foot, wherein the total floor reaction force of the generated gait is distributed to each of the feet Target foot floor reaction force center point determining means for determining a desired foot floor reaction force center point as an action center point on the part, actual floor reaction force detecting means for detecting an actual floor reaction force acting on the foot, The detected actual floor reaction force is calculated as described above. Foot rotation determining means for calculating a moment acting around the foot reaction force center point, and determining a rotation amount for rotating the foot based on at least the calculated moment; and the determined foot. A first foot position and / or posture correcting means for correcting the target position and / or posture such that the position and / or posture of the foot is rotated based on the rotation amount; Compensation displacement calculation for calculating a deviation of the generated gait from the force component of the total floor reaction force by obtaining a force component of the total floor reaction force, and calculating a compensation displacement of the foot based on the calculated deviation. Means, a target foot position based on the calculated compensation displacement
Second foot position / posture correcting means for correcting the posture, and a first position of the robot based on the foot position / posture corrected by the first and second foot position / posture means.
And a joint displacing means for displacing the second joint.

According to a second aspect of the present invention, at least the base is connected to the base via a first joint.
In a control device for a legged mobile robot comprising a plurality of legs each having a leg connected to a distal end thereof via a second joint, a motion pattern including at least a target position and a posture of the leg of the robot. Gait generating means for generating a gait of the robot including at least a target pattern of a total floor reaction force acting on the robot; and distributing the generated total floor reaction force of the generated gait to each of the feet. A desired foot floor reaction force center point determining means for determining a desired foot floor reaction force center point as an action center point on the foot, and an actual floor reaction force for detecting an actual floor reaction force acting on the foot. Detecting means, foot rotation amount determining means for determining a rotation amount for rotating the foot based on at least the detected actual floor reaction force, the position of the foot based on the determined foot rotation amount and And / or the posture is determined as described above. First foot position / posture correction means for correcting the target position and / or posture so as to rotate around or near the desired foot floor reaction force center point; Compensation displacement calculating means for obtaining a force component of the floor reaction force, calculating a deviation from the force component of the total gait reaction force of the generated gait, and calculating a compensation displacement of the foot based on the calculated deviation. Second foot position / posture correcting means for correcting the target foot position / posture based on the calculated compensation displacement, and the foot corrected by the first and second foot position / posture means And a joint displacing means for displacing the first and second joints of the robot based on the position and posture of the robot.

According to a third aspect of the present invention, the first foot position / posture correcting means determines the position and / or posture of the foot based on the determined foot rotation amount. The target position and / or posture is modified so as to rotate around or near the desired foot floor reaction force center point.

The floor reaction force acting on the foot from the total floor reaction force moment actually acting on the robot or the total floor reaction force moment actually acting on the robot may be further provided. Calculating a moment obtained by subtracting the moment, and determining a moving amount of moving the foot in accordance with at least the calculated moment; The position / posture correction means is configured to correct the position and / or posture of the foot based on the determined rotation amount of the foot and the determined movement amount.

According to a fifth aspect of the present invention, a posture stabilizing compensation total floor reaction force moment to be added to the total floor reaction force target pattern is obtained, and the foot rotation amount determining means and / or the foot movement amount are determined. The determining means is configured to determine a rotation amount and / or a movement amount of the foot based on at least the detected actual floor reaction force and the determined posture stabilization compensation total floor reaction force moment.

According to a sixth aspect of the present invention, the posture stabilization compensation total floor reaction force moment is obtained based on at least a tilt deviation of the robot.

According to a seventh aspect of the present invention, the compensation displacement calculating means stabilizes the posture of the robot based on the base acceleration detecting means for detecting the base acceleration of the robot and the detected base acceleration. And a compensating total floor reaction force component calculating means for calculating a force component of the compensating floor reaction force for calculating the force component of the compensating floor reaction force based on a difference obtained by subtracting the deviation from the calculated compensating total floor reaction force component. The compensating displacement of the section is configured to be calculated.

In a preferred embodiment, the compensation displacement calculating means distributes the compensation displacement of the foot to each of the feet by multiplying the difference by a predetermined weight.

According to a ninth aspect of the present invention, the compensation displacement calculating means is configured to set the weight to zero when the foot is off the floor.

According to a tenth aspect of the present invention, the compensation displacement calculating means is configured to vary the weight according to the frequency of the force component of the actual total floor reaction force.

In another preferred construction, the compensation displacement calculating means calculates the compensation displacement of the foot in the direction of gravity.

According to a twelfth aspect of the present invention, the compensating displacement calculating means calculates the compensating displacement of the foot in the direction of a line connecting the center of gravity and the desired total floor reaction force center point.

According to a thirteenth aspect of the present invention, the force component of the compensating total floor reaction force is set at or near zero.

[0038]

According to the present invention, the floor reaction force acting on the legged mobile robot can be easily and appropriately controlled without causing interference. In other words, even if control similar to the combination of the previously proposed two-leg compliance control and ankle compliance control is performed, there is no control interference, and the actual total floor reaction force and the actual foot floor reaction force deviate from desired values or oscillate. Never do.

In addition, the posture stabilization control of the legged mobile robot can be easily realized, the landing impact received by the legged mobile robot can be reduced, the grounding property of the legged mobile robot can be improved, and slipping during walking can be improved. And spin can be prevented.

Further, even if there is an unexpected floor shape change including not only global undulations and inclinations but also local irregularities and inclinations, the floor acting on the legged mobile robot is not affected so much. The reaction force can be appropriately controlled.
Further, the load on the actuator of the legged mobile robot can be reduced.

According to the second aspect, the same function and effect as the first aspect can be obtained.

According to the third aspect, the same function and effect as those of the first aspect can be obtained, and the floor reaction force can be more appropriately controlled.

According to the fourth aspect, the same function and effect as those of the first aspect can be obtained, and the total floor reaction force, which is particularly important for attitude control, can be more appropriately controlled.

According to the fifth aspect, the same function and effect as those of the first aspect can be obtained, and the posture stabilizing ability can be improved.

According to the sixth aspect, the same operation and effect as described above can be obtained, and the posture stabilizing ability can be further improved.

According to the seventh aspect, the same function and effect as described above can be obtained, the landing impact can be more effectively absorbed, and the contact property can be further improved.

According to the eighth aspect, the same function and effect as described above can be obtained.

According to the ninth aspect, the same operation and effect as described above can be obtained, and the load on the actuator can be further reduced by reducing the control to a necessary limit.

According to the tenth aspect, the same operation and effect as described above can be obtained, and a more stable posture can be obtained.

According to the eleventh aspect, the same operation and effect as described above can be obtained, and the amount of calculation can be reduced.

According to the twelfth aspect, the same operation and effect as described above can be obtained, and the amount of calculation can be similarly reduced.

According to the thirteenth aspect, the same function and effect as described above can be obtained, the landing impact can be absorbed to a considerable extent, and the contact property can be improved to a considerable extent. Can be

[0053]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS A control device for a legged mobile robot according to the present invention will be described below with reference to the accompanying drawings. still,
A bipedal walking robot is taken as an example of a legged mobile robot.

FIG. 1 is a schematic diagram showing the entire control device of the legged mobile robot.

As shown, the bipedal walking robot 1 has six joints on each of the left and right leg links 2 (for convenience of understanding, each joint is shown by an electric motor that drives it). The six joints are, in order from the top, joints 10R and 10L for turning the crotch (waist) (R is the right side and L is the left side).
The same applies to the following), joints 14R and 14L in the roll direction (around the X axis) of the crotch (lumbar), joints 12R and 12L in the pitch direction (around the Y axis) of the crotch (lumbar), and joints 16R and 16L in the pitch direction of the knee. , Ankle pitch joint 18R, 18
L, and joints 20R and 20L in the same roll direction.

At the lower portions of the joints 18R (L), 20R (L), feet (foot portions) 22R, 22L are attached, and at the top, an upper body (base) 24 is provided. A control unit 26 (described later) including a microcomputer is stored. In the above, the hip joints (or hip joints) are joints 10R (L), 12R (L), and 14R (L).
From, the ankle joint (ankle joint) is joint 18R (L), 20R
(L). The hip joint and the knee joint are thigh links 28R and 28L, and the knee joint and the ankle joint are the lower leg link 3
Connected at 0R, 30L.

With the above structure, the leg link 2 is provided with six degrees of freedom for each of the left and right feet, and by driving these 6 * 2 = 12 joints at an appropriate angle during walking, A desired motion can be given to the whole, and the user can walk in a three-dimensional space arbitrarily (in this specification, “*” indicates multiplication as an operation on a scalar, and an outer product as an operation on a vector).

The position of the body and its speed, which will be described later in this specification, are determined at predetermined positions of the body 24, specifically, the body 2
4 means the position of the representative point such as the position of the center of gravity and the moving speed thereof.

As shown in FIG. 1, a known six-axis force sensor 44 is attached below the ankle joint, and three-directional components Fx and F of the force are applied.
y, Fz and the three-way components Mx, My, Mz of the moment are measured, and the presence or absence of landing on the foot and the floor reaction force (ground contact load)
And so on.

A tilt sensor 60 is provided on the body 24 to detect a tilt with respect to the Z axis (vertical direction (gravity direction)) and its angular velocity. The tilt sensor 60 includes a G sensor and an angular velocity sensor, and the G sensor detects the acceleration of the body 24 in the X, Y, and Z axis directions. The electric motor of each joint is provided with a rotary encoder for detecting the amount of rotation.

As shown in FIG. 2, a spring mechanism 32 is provided above the foot 22R (L), and a sole elastic body 34 made of rubber or the like is attached to the sole. Spring mechanism 3
Specifically, 2 is a rectangular guide member attached to the foot 22R (L) and attached to the ankle joint 18R (L) and the 6-axis force sensor 44 side, and the guide member is connected to the guide member via an elastic material. And a piston-like member that is stored so as to be finely movable.

The foot 22R (L) indicated by a solid line in the figure
Indicates a state when no floor reaction force is received. When the floor reaction force is received, the spring mechanism 32 and the sole elastic body 34 bend, and the foot moves to the position / posture indicated by the dotted line in the figure. This structure is important not only to alleviate landing impact but also to enhance controllability. The details are described in the above-mentioned Japanese Patent Application Laid-Open No. 5-305584, and the detailed description is omitted.

Further, although not shown in FIG. 1, a joystick 62 is provided at an appropriate position of the bipedal walking robot 1, and the gait can be controlled by externally turning the robot which is moving forward as needed. It is configured to allow you to enter requests.

FIG. 3 is a block diagram showing details of the control unit 26, which is constituted by a microcomputer. The output of the tilt sensor 60 and the like is converted into a digital value by the A / D converter 70, and the output
Via the RAM 74. The output of an encoder disposed adjacent to each electric motor is input to the RAM 74 via the counter 76.

In the control unit, a first unit comprising a CPU,
Second computing devices 80 and 82 are provided. The first computing device 80 calculates a joint angle displacement command based on the gait stored in the ROM 84 as described later,
Send it to AM74. The second arithmetic unit 82 is a RAM
The command and the detected actual value are read out from 74 and the control value necessary for driving each joint is calculated, and the D / A converter 86
And output to an electric motor that drives each joint via a servo amplifier.

Here, the terms used in this specification and the drawings are defined. (Note that the terms not defined are defined in an application (Japanese Patent Application No. 8-214261) proposed by the present applicant separately from the above-mentioned technology. According to the definition used).

The “gait” is different from the general definition in robotics, and is used to mean a combination of a target motion pattern and a floor reaction force pattern. However, the floor reaction force pattern may be partial information such as “only ZMP trajectory”. Therefore, the term “gait generation device” is not used for a device that outputs only a target motion pattern and does not output information about a floor reaction force pattern.

Each leg is given a serial number. The floor reaction force acting on the n-th leg is referred to as the n-th foot floor reaction force (n: 1 or 2; the same applies hereinafter). The sum of the floor reaction forces acting on all legs is called the total floor reaction force (generally called floor reaction force in robotics, but here the "total floor reaction force" ).

The foot floor reaction force is expressed by the point of action, the force applied thereto, and the moment of the force. For the same foot floor reaction force, there are infinite combinations of expressions. Among them,
There is a notation in which the moment component excluding the component around the vertical axis is 0 and the action point is on the floor surface. The point of action in this expression is herein referred to as the foot floor reaction force center point (referred to as the "ground contact pressure center point" in Japanese Patent Application Laid-Open No. Hei 6-79657, which is separately proposed by the present applicant).

Similarly, the total floor reaction force is represented by the point of action, the force applied thereto and the moment of the force, and there are infinite combinations of expressions for the same total floor reaction force. Among them, there is a representation in which the moment component excluding the component around the vertical axis is 0 and the action point is on the floor surface. The point of action in this expression is herein referred to as the total floor reaction force center point.

The target value of the total floor reaction force is called a target total floor reaction force.
The desired total floor reaction force is generally a total floor reaction force that dynamically balances with the desired movement pattern. Therefore, normally, the desired total floor reaction force center point coincides with the desired ZMP.

As mentioned earlier, the target ZMP (Ze
ro Moment Point) is defined as follows. That is, if the resultant force of the inertial force and gravity generated by the target motion pattern is dynamically obtained, and the moment acting on a certain point on the floor surface is 0 except for the component around the vertical axis,
This point is called a target ZMP (Zero Moment Point). The target ZMP is uniquely obtained as long as the vertical force component of the resultant force is not zero. In the following description, the term “target ZMP” may be used for easy understanding, but strictly speaking, there are many places that should be called the target floor reaction force center point.

The target value of each foot floor reaction force is referred to as a desired foot floor reaction force. However, unlike the desired total floor reaction force, the desired foot floor reaction force is not uniquely determined even if the desired exercise pattern is determined. The total floor reaction force acting on the actual robot is called the actual total floor reaction force. Each foot floor reaction force acting on the actual robot is called an actual foot floor reaction force.

Here, the subject of the present invention will be re-explained. In the present invention, it is difficult to obtain good posture stability by the previously proposed two-leg compliance control with respect to a local inclination and a step. The inconvenience can be solved by using the ankle compliance control, but simply using both of them will cause interference, and the actual total floor reaction force and the actual foot floor reaction force will deviate from the desired values, and the inconvenience of oscillation will occur. there were.

The problem will be described with reference to the situation shown in FIG. 34. The first foot is subjected to an unexpectedly large floor reaction force on the heel, so that an excessively large actual foot around the first ankle. A flat floor reaction force moment occurs. In the ankle compliance control, the first ankle is moved as shown in FIG.
Rotate as shown.

However, the rotation of the ankle causes the first
Since the heel position of the foot is raised, the vertical component of the first foot floor reaction force is reduced. As a result, the actual total floor reaction force moment around the target total floor reaction force center point (target ZMP) changes.
This means that the actual total floor reaction force moment, which is a control amount of the two-leg compliance control, is interfered with by the ankle compliance control.

Therefore, when the two-leg compliance control is operated in the same manner as without the ankle compliance control without considering the interference by the ankle compliance control, the actual total floor reaction force around the target total floor reaction force center point (target ZMP) is obtained. The moment deviates from zero, and vibration or oscillation occurs due to interference.

One method of preventing this is to determine the amount of interference between the two-leg compliance control and the ankle compliance control, and add an operation amount that cancels this to prevent interference. In the middle, the attitude changes every moment, and the interference relationship also changes every moment. Therefore, it is extremely difficult to avoid the interference by this method.

Further, in the situation shown in FIG. 37, the floor in contact with the first foot is more uphill than the expected floor, so the first foot should be raised more than the target gait. is there. Nevertheless, the fact that the toes are lowered by the ankle compliance control means that the ankle compliance control is not working properly.

As described above, the ankle compliance control is effective for the local floor inclination or step at the landing point of the foot, but has an adverse effect on the inclination or swell that changes slowly over a long distance. There are cases.

Therefore, in the apparatus according to the embodiment, the floor reaction force acting on the legged mobile robot, more specifically, the actual total floor reaction force moment around the target total floor reaction force center point and the target The actual foot floor reaction force moment around the foot floor reaction force center point can be controlled easily and appropriately.

In addition, even if there is an unexpected floor shape change including local irregularities and inclinations as well as global undulations and inclinations, the robot can walk in a stable posture without being significantly affected by the change. It was made to continue.

In order to solve the above-mentioned problems, the present applicant has previously proposed a control device for a legged mobile robot, and an object of the present invention is to further improve the proposed technology and to realize an actual total floor reaction force. It is an object of the present invention to provide a control device for a legged mobile robot, which performs compliance control on the force component of the legged mobile robot, in particular, the force component Fz in the direction of gravity, thereby absorbing a landing impact and improving the contact property.

FIG. 4 shows a control device of the legged mobile robot according to this embodiment (mainly the first arithmetic unit 80 in FIG. 3).
FIG. 3 is a block diagram functionally showing the configuration and operation of (equivalent to). Hereinafter, the overall configuration of this device will be outlined with reference to FIG.

This device includes a gait generator, and the gait generator generates and outputs a desired gait. The desired gait is, as defined above, a desired exercise pattern and a desired floor reaction force pattern, more specifically, a desired body position / posture trajectory, a desired foot position /
It consists of an attitude trajectory, a desired total floor reaction force center point (desired ZMP) trajectory, and a desired total floor reaction force trajectory. The desired floor reaction force pattern is
Thus, the target total floor reaction force center point locus is included (if the mechanism deformation compensation described later is not performed, the target total floor reaction force center point locus alone may be used as the target floor reaction force pattern).

In this embodiment, the desired total floor reaction force output by the gait generator is a total floor reaction force that dynamically balances with the desired movement pattern. Therefore, the desired total floor reaction force center point coincides with the desired ZMP.

FIG. 5 shows an example of a target movement pattern when the robot 1 walks on flat ground. Target Z corresponding to this
FIG. 6 shows the trajectory of the MP orbit on the floor, and FIG. 7 shows a time chart. The foot remaining in contact with the floor during this gait is referred to as a first foot, and the other foot is referred to as a second foot.
The details of the gait generator are described in the previously proposed Japanese Patent Application No. 8-214.
No. 261, and further description is omitted.

Returning to the description of FIG. 4, this device is provided with a desired floor reaction force distributor, and the desired floor reaction force distributor determines a desired total floor reaction force center point (a desired ZMP) and a desired foot position / posture. The main input is to determine and output the desired foot floor reaction force center point. Actually, the gait generator uses the gait parameters (for example, the time of the two-leg support period, the target landing position of the swing leg foot, etc.) and the time and time of the gait (for example, the current time is Information such as 0.1 seconds from the beginning) is also imported as needed.

For a gait as shown in FIG. 5, the desired floor reaction force distributor sets such that each desired foot floor reaction force center point satisfies the following conditions. Condition 1) The desired foot floor reaction force center point trajectory is continuous. Condition 2) In the two-leg support period, the target first foot floor reaction force center point is on the heel, and the target second foot floor reaction force center point is on the toes. Condition 3) At this time, the desired first foot floor reaction force center point and the desired second floor
The target total floor reaction force center point exists on a line connecting the foot floor reaction force center points. Condition 4) In the one-leg support period, the target first foot floor reaction force center point coincides with the target total floor reaction force center point. Condition 5) During the one-leg supporting period, the desired second foot floor reaction force central point moves from the toe to the heel.

FIG. 8 is a time chart of the desired first foot floor reaction force center point trajectory satisfying these conditions, and FIG. 9 is a time chart of the desired second foot floor reaction force center point trajectory.
In this figure, the vertical projection point from the ankle (joint 18, 20R (L)) to the foot 22R (L) is set as the origin, and as shown in FIG. The left direction is the positive direction of the Y axis.

The desired floor reaction force distributor further determines, and outputs the desired foot floor reaction force, though it is incidental. The desired foot floor reaction force is necessary for compensating deflection of the spring mechanism 32 and the like.

If the desired floor reaction force corresponding to the desired foot floor reaction force center point set as described above is determined using the following equation, the resultant force of the desired foot floor reaction force will be the desired total floor reaction force. Satisfies the condition that they must match.

Desired first foot floor reaction force = Target total floor reaction force * (Distance between desired second foot floor reaction force center point and target ZMP) / (Target first foot floor reaction force center point and desired second foot floor) (Reaction force center point distance) Target second foot floor reaction force = Target total floor reaction force * (Distance between target first foot floor reaction force center point and target ZMP) / (Target first foot floor reaction force center point and target Distance of the second foot floor reaction force center point) ・ ・ ・ Equation 1

Since the desired foot floor reaction force thus obtained changes continuously, it is suitable for realizing walking with less impact. The above details are described in a technique separately proposed by the present applicant (JP-A-6-79657).

Returning to the description of FIG. 4, this apparatus includes a posture stabilization control calculation unit. The posture stabilization control calculation unit estimates the state of the robot based on the sensor information of the robot, and calculates the compensation total floor reaction force. calculate. In other words, when the robot is actually walking or standing upright, even if the actual joint displacement can completely follow the target joint displacement by the displacement controller described later, the robot position / posture is not necessarily the desired position / posture. It does not become a posture.

In order to stabilize the posture of the robot for a long period of time, a force and a moment necessary for restoring the robot to a desired position and posture are obtained, and these are calculated as a desired center point of the total floor reaction force (a desired ZMP). It must be additionally generated as an action point. This additional force and moment is called the compensating total floor reaction force. The moment component of the compensating total floor reaction force is referred to as a compensating total floor reaction force moment.

It is assumed that the desired gait of the legged mobile robot receives a reaction force other than the floor reaction force from the environment, and this is called, for example, a target object reaction force. May be extended as follows. That is, the resultant of the inertial force, gravity, and the reaction force of the target object generated by the target motion pattern is dynamically obtained, and the moment acting on a certain point on the floor surface becomes zero except for the component around the vertical axis. If so, that point may be set as the target ZMP again.

If it is assumed that the robot 1 is a completely rigid body and the displacement controller allows the actual joint displacement to completely follow the target joint displacement, the deflection of the foot spring mechanism 32 and the sole elastic body 34 is assumed. Perturbation of the position and orientation of the entire robot caused by
It can be decomposed freely.

Mode 1) The desired total floor reaction force center point (the desired ZM
Mode 2) Rotation around left and right axis around target center of desired total floor reaction force (target ZMP) (ie, tilt forward and backward) Mode 3) Target whole floor Rotation (ie, spin) around the vertical axis about the reaction force center point (target ZMP) Mode 4) Forward / backward translational movement Mode 5) Right / left parallel translational oscillation Mode 6) Vertical translational oscillation

Modes 4 and 5 are generated when the spring mechanism 32 of the foot and the elastic body 34 bend by receiving a shearing force in the front, rear, left and right directions. Since the spring mechanism 32 and the sole elastic body 34 are manufactured so as to have high rigidity in the shearing direction, the swing is extremely small, and there is almost no adverse effect on walking.

Although the technique proposed above mainly deals with mode 1 and mode 2, the embodiment according to the present invention also deals with mode 6 in addition to the mode 1. Incidentally, for convenience of understanding, the above-mentioned proposed technology will be described again.

The manipulated variable for controlling the mode 1 is a moment component about the front-rear axis (X-axis) of the compensating total floor reaction force. The operation amount for controlling the mode 2 is a moment component about the left-right axis (Y-axis) of the compensating total floor reaction force. Accordingly, only the moment component in the front-rear direction and the moment component in the left-right direction need to be obtained from the components of the total floor reaction force. Other components are not used in this embodiment, and may be 0.

Note that the following definitions follow. That is, the moment component of the compensating total floor reaction force is calculated by compensating the total floor reaction force moment Md.
md (specifically, a compensated total floor reaction force moment Mdmd around the target total floor reaction force center point (target ZMP)). As shown in FIG. 5, the forward direction of the robot is the X axis, the left lateral direction is the Y axis,
A coordinate system in which the upward direction is the Z axis and the origin is a point on the floor immediately below the ankle of the first foot is referred to as a supporting leg coordinate system. Unless otherwise specified, the position, force and moment are expressed in this coordinate system. Shall be. Further, the X component of Mdmd is represented by Mdmdx,
The Y component is described as Mdmdy, and the Z component is described as Mdmdz.
The X component of the inclination deviation of the body 24 (that is, the actual body inclination-the target body inclination) θerr is described as θerrx, the Y component is θerry, and the time differential values thereof are described as (dθerrx / dt) and (dθerry / dt). I do.

Mdmdx and Mdmdy are determined by, for example, the following control law. Mdmdx =-Kthxθerrx-Kwx (dθerrx / dt) Mdmdy =-Kthyθerry-Kwy (dθerry / dt) Equation 2 where Kthx, Kthy, Kwx and Kwy are
This is the body tilt stabilization control gain.

The composite compliance operation determining section described later works to make the actual total floor reaction force equal to the resultant force of the target total floor reaction force and the compensation total floor reaction force.

Returning to the description of FIG. 4, this device is provided with actual foot floor reaction force detectors, and the actual foot floor reaction force detectors are detected by the six-axis force sensor 44 to determine the actual foot floor reaction force (the resultant force). Detects the actual total floor reaction force). Further, based on the actual displacement (or displacement command) detected by the joint encoder, the relative position / posture of each foot with respect to the coordinate system fixed to the body is calculated, whereby the detection value of the six-axis force sensor 44 is calculated. Are converted into coordinates, and actual foot floor reaction forces expressed in a coordinate system fixed to the body are calculated, and further converted to a support leg coordinate system.

This device has a robot geometric model (inverse kinematics operation unit).
When the body position / posture and the foot position / posture are input, each joint displacement satisfying them is calculated. The degree of freedom of the joint per leg as in the robot 1 in this embodiment is 6
In the case of, each joint displacement is uniquely obtained.

In this embodiment, the equations for the solution of the inverse kinematics are obtained directly, and the displacement of each joint is obtained simply by substituting the body position / posture and the foot position / posture into the equations. That is, the robot geometric model inputs the target body position / posture and the corrected target foot position / posture trajectory (corrected target foot position / posture trajectory with mechanical deformation compensation) corrected by the composite compliance operation determination unit, From these, joint displacement commands (values) of twelve joints (10R (L) and the like) are calculated. Note that the joint angle displacement may be obtained by Jacobian.

This device has a displacement controller (same as the above-mentioned second arithmetic unit 82), and the displacement controller is a robot geometric model (inverse kinematics arithmetic unit).
The displacement control of the twelve joints of the robot 1 is controlled to follow the joint displacement commands (values) calculated in (1) as target values.

The above-mentioned composite compliance operation determination unit corrects the desired foot position / posture trajectory so as to satisfy the following two requirements.

Requirement 1) For controlling the position and orientation of the robot, the actual total floor reaction force is made to follow the resultant force of the compensating total floor reaction force (moment Mdmd) output by the posture stabilization control section and the desired total floor reaction force. . When only the posture inclination of the robot is to be controlled, only the actual total floor reaction force horizontal moment component around the target total floor reaction force center point is made to follow the compensated total floor reaction force moment Mdmd.

Requirement 2) In order to secure the contact property of each foot, the absolute value of the actual foot floor reaction force moment around the target foot floor reaction force center point is reduced as much as possible.

It should be noted that normally, the actual total floor reaction force moment around the desired foot floor reaction force center point is made while making the actual total floor reaction force coincide with the resultant force of the compensation total floor reaction force and the target total floor reaction force. It is often physically impossible to make 0 Therefore, requirements 1) and 2) cannot be completely compatible and must be compromised in some respects.

The operation of this apparatus will be described with reference to the flow chart (structured flow chart) of FIG. 10 on the premise of the above. 4 shows the components of the apparatus shown in FIG. 4 that perform the processing corresponding to the left end of the figure.

First, in S10, the apparatus is initialized, and in S1
The process proceeds to S14 via 2 and waits for a timer interrupt. The timer interrupt is made every 50 ms, that is, the control cycle is 50 ms.

Subsequently, the process proceeds to S16, where it is determined whether or not the gait is switched, that is, the support leg is switched. When the result is negative, the process proceeds to S22. When the result is affirmative, the process proceeds to S1.
The program proceeds to S8, where the timer t is initialized, and proceeds to S20 to set a desired gait parameter. As described above, the gait parameters are the motion parameters and the floor reaction force parameters (ZM
P trajectory parameters).

Then, the program proceeds to S22, in which an instantaneous value of the desired gait is determined. Here, the “instantaneous value” means a value for each control cycle, and the desired gait instantaneous value includes a desired body position / posture, a desired foot position / posture, and a desired ZMP position.
Here, the “posture” is the “direction” in the X, Y, Z space.
Means

Then, the program proceeds to S24, in which a desired foot floor reaction force center point is determined. This is performed as described in the description of the desired floor reaction force distributor. Specifically, this is performed by obtaining the value at the current time t of the desired foot floor reaction force center point locus set as shown in FIGS. 8 and 9.

Then, the program proceeds to S26, in which the desired foot floor reaction force is determined. This is performed by calculating each desired foot floor reaction force using Expression 1 described in the description of the desired floor reaction force distributor.

Subsequently, the flow proceeds to S28, where the state of the robot 1 such as the inclination of the body 24 is detected from the output of the inclination sensor 60 and the like. At this time, actual accelerations acting on the body 24 in the X, Y, and Z directions are detected. In order to solve the above-mentioned problem, in this embodiment, the body acceleration (referred to as “Gbodyz”) in the Z direction will be particularly described as an example. However, the following description is equally applicable to the X and Y directions.

Subsequently, the flow advances to S30, where a compensated total floor reaction force moment Md (around the desired total floor reaction force center point (target ZMP)) for stabilizing the posture from the state of the robot 1 or the like.
mdx and Mdmdy are obtained. More specifically, the compensation total floor reaction force moments Mdmdx and Mdmdy are calculated according to the above-described equation 2 in order to stabilize the posture when the body inclination is detected.
Is calculated.

Subsequently, the program proceeds to S31, in which a compensating total floor reaction force component is calculated based on the body Z-direction acceleration control law, which will be described later.

Then, the program proceeds to S32, in which actual foot floor reaction forces are detected. This is detected from the output of the six-axis force sensor 44 as described above.

Then, the process proceeds to S34, in which the leg compensation angle θdbv is set.
And each foot compensation angle θnx (y) is determined. this is,
This is an operation performed by the composite compliance operation determination unit described above.

The operation of the composite compliance operation determining section will be described. For convenience of explanation, the first foot 22R (L) and the second foot 22R (L) are shown in FIG.
It is assumed that actual foot floor reaction force is acting on the foot 22L (R).

Here, the vector Fnact represents a force component of the n-th foot floor reaction force. The vector Mnact represents the moment component of the n-th foot floor reaction force. The direction of the vector Mact indicates that a clockwise moment acts on the foot from the floor with respect to the direction.

It is assumed that the desired total floor reaction force at this moment is as shown in FIG. Incidentally, the desired total floor reaction force moment vector Msumref at the desired total floor reaction force center point (target ZMP) is vertical (by definition, the target ZMP has a horizontal component of the desired total floor reaction force moment of 0).
Is the point).

When this is distributed to the desired foot floor reaction forces in accordance with Equation 1, the result is as shown in FIG. In the figure, a vector Fnref represents a force component of a desired n-th foot floor reaction force. The vector Mnref represents the moment component of the desired n-th foot floor reaction force. The expression of the direction of the vector Mnref is
Same as Mnact.

For the sake of explanation, it is assumed that the body posture is likely to fall to the rear left.

The above-described posture stabilization control calculation unit calculates a compensating total floor reaction force moment Mdmd based on the detected body inclination deviation values θerrx and θerry of the robot 1. In this embodiment, since the spin about the vertical axis (Z axis) is not controlled, the vertical axis component of the compensation total floor reaction force moment Mdmd is zero. FIG. 14 shows the compensation total floor reaction force moment Mdmd corresponding to this state.

In order to restore the posture, the horizontal component of the actual total floor reaction force moment around the desired total floor reaction force center point (target ZMP) is calculated by calculating the desired total floor reaction force moment Msumref and the compensation total floor reaction force moment Mdmd. May be made to follow the horizontal component of the sum of.

On the other hand, the desired total floor reaction force center point (the desired ZMP)
Then, the horizontal component of the desired total floor reaction force moment Msumref is zero. Therefore, in order to restore the posture inclination in the front, rear, left, and right directions, the horizontal component of the actual total floor reaction force moment around the target ZMP should follow the horizontal component of Mdmd.

In this embodiment, the composite compliance operation determining unit corrects the position and posture of the foot so as to satisfy the following requirements as much as possible. Requirement 1) In order to stabilize and control the posture inclination of the robot, the horizontal (X, Y axis) component of the actual total floor reaction force moment around the target total floor reaction force center point (target ZMP) is compensated for by the total floor. The horizontal direction component of the reaction force moment Mdmd is made to follow. Requirement 2) In order to secure the contact of each foot, the absolute value of the actual moment of each foot floor reaction force around the target center point of each foot floor reaction force is reduced as much as possible.

However, as described above, the request 1) and the request 2)
Cannot be completely compatible, and must compromise in some respects.

The correction of the position / posture of the foot is performed as follows in this embodiment. 1) A normal vector V including a desired first foot floor reaction force central point Q1 and a desired second foot floor reaction force central point Q2 and perpendicular to the horizontal plane is obtained. The magnitude of V is 1. V to FIG.
Shown in

2) Rotate the coordinates of the desired first foot floor reaction force central point Q1 around the normal vector V about the desired total floor reaction force central point (target ZMP) by a certain rotation angle θdbv. The point after the movement is defined as Q1 '. Similarly, the coordinates of the desired second floor floor reaction force central point Q2 are rotated around the normal vector V by the rotation angle θdbv around the desired total floor reaction force central point (target ZMP). Q after moving
2 '.

The rotation angle θdbv is referred to as a two-leg compensation angle.
The vector whose start point is Q1 and whose end point is Q1 'is a vector Q1Q
1 '. Similarly, a vector whose start point is Q2 and whose end point is Q2 'is referred to as a vector Q2Q2'. FIG. 16 shows Q1 ′ and Q
Indicates 2 '.

3) The target first foot is translated (substantially up and down) by the vector Q1Q1 'without changing the posture.
Similarly, the target second foot is moved to the vector Q2 without changing the posture.
Translate by Q2 '. Figure 1 shows the target footprints after moving
6 is indicated by a thick line.

4) Next, the target first foot is rotated about the axis Q1 'by the rotation angle θ1x about the longitudinal axis (X axis) and the rotation angle θ1y about the horizontal axis (Y axis). Similarly, the target second foot is rotated by a rotation angle θ2x about the longitudinal axis (X-axis) and a rotation angle θ2y about the horizontal axis (Y-axis) about Q2 ′. The rotation angles θnx and θny are referred to as an nth foot X compensation angle and an nth foot Y compensation angle, respectively. Each target foot after rotation is indicated by a thick line in FIG.

If the amount of compensation operation is not excessive, even if the contact pressure distribution changes, the contact area (the area where the pressure on the sole is positive) does not change. In such a case, the spring mechanism 32 and the sole elastic body 34 attached to each foot are deformed in proportion to the amount of compensation movement, and actual foot floor reaction forces corresponding to the amount of deformation are generated. As a result, the relationship between the amount of compensation operation and the amount of change in the actual floor reaction force generated by the compensation operation has the following favorable characteristics.

Characteristic 1) When the target foot position is moved by operating only the two leg compensation angle θdbv, the force component of the actual foot floor reaction force of the lowered foot increases, and the actual foot floor of the raised foot is increased. The force component of the reaction force decreases. At this time, the actual foot floor reaction force moment around the corrected target foot floor reaction force center point hardly changes.

Characteristic 2) When the desired nth foot posture is rotated by operating only the nth foot X compensation angle, the moment of the actual nth foot floor reaction force acting on the desired nth foot floor reaction force center point Only the X component changes, and the other floor reaction force components change only slightly. Similarly, when the target n-th foot posture is rotated by operating only the n-th foot Y compensation angle, only the Y component of the moment of the actual n-th foot floor reaction force changes, and the other floor reaction force components are Change only slightly.

Characteristic 3) If both feet compensation angle θdbv, each foot X compensation angle and each foot Y compensation angle are simultaneously operated, the change amount of each actual foot floor reaction force is the change amount when each is operated independently. The sum of

The characteristics 1 and 2 indicate that these operations are independent, and the characteristic 3 indicates that these operations have linearity.

FIG. 18 is a block diagram showing the arithmetic processing of the composite compliance operation determining unit. This operation will be described with reference to FIG.

In brief, the compensating total floor reaction force moment distributor distributes the compensating total floor reaction force moment Mdmd. Next, the two-leg compensation angle determination unit and the n-th foot X
(Y) The compensation angle determination unit determines the compensation angles θdbv and θnx (y).

Next, based on the determined various compensation angles, the corrected target foot position / posture calculation unit calculates the compensated foot position / posture.
A posture (this is referred to as a corrected target foot position / posture) is obtained by a geometric calculation. Lastly, the corrected target foot position / posture calculation unit including the mechanism deformation compensation calculates the deformation amounts of the spring mechanism 32 and the sole elastic body 34 expected to be generated by the desired foot floor reaction forces, and cancels them. Corrected target foot position
Modify the posture further.

More specifically, the compensating total floor reaction force moment distributor converts the compensating total floor reaction force moment Mdmd into the two-leg compensation moment Mdmdb and the foot compensation moment Md.
md1x, y, Mdmd2x, y. The two-leg compensation moment Mdmddb is the two-leg compensation angle (foot vertical amount).
By operating θdbv, it is a target value of the moment generated by the force component of each foot floor reaction force around the target total floor reaction force center point (target ZMP).

The component of the double leg compensation moment Mdmdb around the V direction is described as Mdmdbdbv. Note that the vector V is a vector defined in the description of the composite compliance operation determination unit. Assuming that a vector perpendicular to V and also perpendicular to the vertical direction is U, the two-leg compensation moment Mdmdd
The U-direction component Mdmddbu of b is set to 0. This is because the U-direction moment component of the floor reaction force cannot be generated even if the two-leg compensation angle θdbv is operated.

In this embodiment, since the vertical component of the compensation total floor reaction force moment Mdmd is zero, the vertical component Mdmdbbz of Mdmdb is also set to zero.

The first foot compensation moment Mdmd1 is a moment to be generated around the desired first foot floor reaction force central point by manipulating the first foot compensation angles θ1x and θ1y. The X component of the first foot compensation moment Mdmd1 is described as Mdmd1x, and the Y component is described as Mdmdly. The second foot compensation moment Mdmd2 is the second foot compensation angle θ
This is a moment to be generated around the target second foot floor reaction force central point by operating 2x and θ2y. Second
The X component of the foot compensation moment Mdmd2 is represented by Mdmd2
The x and Y components are described as Mdmd2y.

The distribution is performed, for example, as follows. Mdmddbv = Wdbx * Mdmdx + Wdby * Mdmdy ・ ・ ・ Equation 3 Mdmd1x = W1x * (Mdmdx-Wint * Vx * Mdmddbv) Mdmd1y = W1y * (Mdmdy-Wint * Vy * Mdmddbv) Mdmd2x = W2x * * dm * Mdmddbv) Mdmd2y = W2y * (Mdmdy-Wint * Vy * Mdmddbv) ・ ・ ・ Equation 4

Here, Wdbx, Wdby, W1x, W
1y, W2x, W2y and Wint are weighting variables for distribution. Vx is the value of the X component of the vector V, and Vy is the value of the Y component of the vector V. Among them, Wint is for canceling the total floor reaction force moment generated by operating the both leg compensation angle by operating each foot compensation angle.

FIG. 19 is a block diagram of a compensating total floor reaction force moment distributor for performing the arithmetic processing of the equations (3) and (4).

Weight variables Wdbx, Wdb for distribution during walking
FIG. 20 shows a setting example of y, W1x, W1y, W2x, W2y and Wint. The pattern of FIG. 20 is desirably determined in consideration of the following points.

Attention 1) If the both leg compensation angle and each foot compensation angle change discontinuously, an excessive torque is generated at the joint.
Therefore, in order to continuously change both leg compensation angles and each foot compensation angle, the distribution weight variable is continuously changed.

Attention 2) The distribution weight variable is set so that the actual floor reaction force moment generated by manipulating the both leg compensation angle and each foot compensation angle is as close as possible to the compensation total floor reaction force moment Mdmd. To determine.

At this time, it is better to change the setting policy as described below according to the situation such as standing up or walking. As in the case of standing upright, the V-direction component Mdmd of the two-leg compensation moment
dbv, each foot compensation moment Mdmd1, Mdmd2
Is set as follows in the situation where the actual floor reaction force can be generated faithfully.

In this situation, the desired total floor reaction force center point (the desired Z
MP) in order to make the horizontal component of the actual total floor reaction force moment around the same as the horizontal component of the compensating total floor reaction force moment Mdmd (ie, in order to satisfy the above-described requirement 1 to the composite compliance operation determination unit). Equation 5
And Equation 6 should be set so as to satisfy both as much as possible.

Mdmddbv * Vx + Mdmd1x + Mdmd2x = Mdmdx Equation 5 Mdmddbv * Vy + Mdmd1y + Mdmd2y = Mdmdy Equation 6

By substituting Equations 3 and 4, Equation 5 is converted to Equation 7, and Equation 6 is converted to Equation 8. (Wdbx * Mdmdx + Wdby * Mdmdy) * Vx + W1x * (Mdmdx-
Wint * Vx * (Wdbx * Mdmdx + Wdby * Mdmdy)) + W2x *
(Mdmdx-Wint * Vx * (Wdbx * Mdmdx + Wdby * Mdmd
y)) = Mdmdx ・ ・ ・ Equation 7 (Wdbx * Mdmdx + Wdby * Mdmdy) * Vy + W1y * (Mdmdy-
Wint * Vy * (Wdbx * Mdmdx + Wdby * Mdmdy)) + W2y *
(Mdmdy-Wint * Vy * (Wdbx * Mdmdx + Wdby * Mdmd
y)) = Mdmdy ・ ・ ・ Equation 8

Even if Mdmdx and Mdmdy take arbitrary values, in order for Equations 7 and 8 to be established equally, Equation 9
It suffices to satisfy Equations 10 and 11 simultaneously. Wint = 1 ... Equation 9 W1x + W2x = 1 ... Equation 10 W1y + W2y = 1 ... Equation 11 That is, in the above situation, weights are set so as to satisfy Equation 9, Equation 10 and Equation 11 simultaneously. Should be determined.

During walking, even if the foot position is corrected by manipulating the two-leg compensation angle θdbv with Mdmddbv as the target,
In some cases, the amount of the actual total floor reaction force moment is insufficient compared to Mdmddbv. For example, as shown in FIG. 21, in a situation where the first foot has not yet landed due to the robot tilting back in the early stage of the two-leg support period, the actual floor reaction force changes even if the position of the first foot is lowered by θdbv do not do.

Similarly, even if the angle of the second foot is corrected by manipulating the second foot compensation angle θ2 with Mdmd2 as a target, the amount of increase in the actual floor reaction force moment is insufficient as compared with Mdmd2. There is. For example, in the situation where the robot is tilted backward in the latter half of the two-leg supporting period as shown in FIG. 22, the actual floor reaction force does not change even if the heel of the second foot is lowered by θ2.

Therefore, even if each weight is set so as to satisfy Equations 5 and 6, the amount of increase in the actual total floor reaction force generated by the composite compliance control is equal to the compensation total floor reaction force moment M.
may not reach dmd. In a situation where such a possibility is high, the value obtained by dividing the value on the left side of Expressions 5 and 6 by the right side should be larger than 1.

FIG. 2 shows an example of setting distribution weight variables during walking.
At 0, by setting Wint to 0, FIG.
As in the situation described above, even if the actual total floor reaction force moment cannot be generated even if the two-leg compensation angle θdbv is operated, each foot compensation angle is operated to compensate for the shortage.

Conveniently, if the heel of the second foot is lowered as shown in FIG. 21 and the heel of the second foot is eventually lowered and it is easy to touch the floor, the actual whole floor can be operated by operating the second foot compensation angle. A reaction force moment can be generated.

When the rear leg is not tilted, the actual total floor reaction force moment is generated by operating the both leg compensation angle θdbv. However, since the heel of the second foot does not touch the floor,
Even if the second foot compensation angle is operated, no actual total floor reaction force moment is generated.

That is, when the two leg compensation angle θdbv works effectively, each foot compensation angle does not work effectively, and when each foot compensation angle works effectively, the two leg compensation angle θdbv does not work effectively. The total amount of the actual floor reaction force moment generated by manipulating the compensation angle and each foot compensation angle is substantially equal to the compensation total floor reaction force moment Mdmd.

In some situations, the total amount of the actual floor reaction force moment generated by manipulating the two leg compensation angle and each foot compensation angle may be larger than the compensation total floor reaction force moment Mdmd.

However, even in this case, if Mdmd is a feedback operation amount for stabilizing the posture as in this embodiment, there is not much problem. This is because, even if the magnitude of Mdmd is slightly different, it can generally be said to the control system. However, the closed-loop characteristic hardly changes even if the open-loop gain of the control system slightly changes. .

Attention 3) In the one-leg supporting period, the absolute values of the distribution weight variables Wdbx and Wdby for the two-leg compensation angle are reduced. This is because, in the one-leg supporting period, even if the two-leg compensation angle is changed, the foot that is not touching the ground simply goes up and down uselessly, and the actual floor floor reaction force does not change.

Attention 4) When the force component of the desired foot floor reaction force is small in order to secure the foot contact, the absolute value of the distribution weight variable for the foot compensation angle of the foot is reduced. I do. Especially when the foot is far from the floor,
Even if the foot compensation angle of the foot is moved, the actual foot floor reaction force of the foot does not change, and therefore, in order to prevent unnecessary movement, the weight for distribution for the foot compensation angle of the foot is used. The absolute value of the variable should be small.

Note 5) The direction of the actual total floor reaction force moment which can be controlled by operating the both leg compensation angles and the direction of the actual total floor reaction force moment which can be controlled by operating each foot compensation angle are usually different. .

For example, the direction of the actual total floor reaction force moment generated by operating the two-leg compensation angle θdbv is always in the V direction, and a component orthogonal to the V direction cannot be generated. On the other hand, the direction of the actual total floor reaction force moment that can be generated by operating each foot compensation angle is restricted by the foot contact state.

For example, when only the edge of the toe or the edge of the heel is in contact with the ground, no moment can be generated in the edge line direction. In the two-leg supporting period, the two-leg compensation angle and each foot compensation angle are manipulated as much as possible in consideration of this characteristic.

For example, the distribution weights Wdbx and Wdby for operating the two-leg compensation angle are determined as follows.

X component is Wdbx, Y component is Wdby, Z
Assuming that a vector having zero component is Wdb, Equation 3 is an inner product of the vector Wdb and Mdmd. Therefore, Mdm
Decompose d into a vector Wdb direction component and its orthogonal component,
By extracting only the component in the vector Wdb direction, the vector Wd
Multiplied by the magnitude of b is M
dmdbbv.

FIG. 23 shows Mdmddbv in this case. This means that a feedback control system that controls the Wdb direction component of the actual total floor reaction force moment by manipulating the two-leg compensation angle is meant. If the Wdb direction is orthogonal to the vector V, no matter how much the two-leg compensation angle is manipulated, the Wdb-direction component of the actual total floor reaction force moment will not be generated. Just do it.

Therefore, when unnecessary movements are reduced,
The Wdb direction should match or be as close as possible to the vector V direction. Also, if it is desired to generate the Wdb direction component of the compensation total floor reaction force moment Mdmd simply by operating the two-leg compensation angle without depending on each foot compensation angle,
The inner product of Wdb and V is set to 1. If you want a part to depend on each foot compensation angle, the inner product of Wdb and V is 1
Set to be smaller.

When the lateral width of the foot is narrow, the X component of the actual foot floor reaction force moment that can be generated by operating each foot compensation angle becomes small. In this case, Wdbx is set to be relatively large. The Wdb direction and the vector V direction do not match, and the fluctuation of the leg compensation angle increases, but the stability increases.

The two-leg compensation angle determination unit will be described in further detail. FIG. 24 is a block diagram of the calculation processing of the two-leg compensation angle determination unit, and the two-leg compensation angle θdbv is calculated as shown.

Referring to FIG. 24, F1act acting on desired first foot floor reaction force central point Q1 and F2act acting on desired second foot floor reaction force central point Q2 are determined by the desired total floor reaction force central point. Moment Mf1f generated around P
2act is obtained by the following equation.

Mf1f2act = PQ1 * F1act + PQ2 * F2act Expression 12 where PQ1 is a vector whose start point is P and whose end point is Q1,
Q2 is a vector whose start point is P and whose end point is Q2.

Also, there is practically no problem if the following equation is used instead of equation 12. Mf1f2act = PQ1 * F1act + PQ2 * F2act + M1act + M2act Equation 12a Equation 12a is an equation for calculating an actual total floor reaction force moment Act acting around the target total floor reaction force center point.
Expression 12 is obtained by subtracting the actual foot floor reaction force moment acting around the desired foot floor reaction force central point from the actual total floor reaction force moment acting around the target total floor reaction force center point. ing. The statement in claim 4 is based on this.

Next, a vector V-direction component Mf1f2actv of Mf1f2act is extracted. This is obtained by the following equation using a vector inner product operation. Note that the vector V is V shown in FIG. 15 in the above description of the operation. Mf1f2actv = Mf1f2act · V ・ ・ ・ Equation 13

Next, Mf1f2actvfilt is obtained by passing Mf1f2actv through a low-pass filter.

Next, the two-leg compensation moment V-direction component Md
The mddbv is passed through a compensating filter, and is passed through Mf1f
2actvfilt to obtain a deviation moment V-direction component Mdiffv.

The compensating filter improves the frequency response characteristics of the transfer function from Mdmddbv to the actual total floor reaction force moment.

Next, a two-leg mechanism deformation compensation angle θffdbv for canceling the influence of the deformation of the foot spring mechanism on the V-direction component of the two-leg compensation moment is obtained. This is so-called feed-forward compensation.

More specifically, a desired first foot floor reaction force center point Q1 is used by using a mechanism compliance model representing the relationship between the two leg compensation moment V-direction component Mdmdbbv and the amount of deformation.
And the desired second foot floor reaction force center point Q2, the deformation angle of the line segment is obtained, and the inverted polarity of the angle is used as the two-leg mechanism deformation compensation angle θffdbv.

The two-leg mechanism deformation compensation angle θffdbv can be approximately obtained by the following equation. θffdbv = −α * Mdmddbv Expression 14 where α is a predetermined constant.

Finally, a two-leg compensation angle θdbv is obtained by the following equation. Here, Kdb is a control gain, which is usually set to a positive value. θdbv = Kdb * Mdiffv + θffdbv ・ ・ ・ Equation 15

The n-th foot compensation angle determination unit will be described. FIG. 25 is a block diagram showing the calculation processing of the first foot X compensation angle determination unit therein, and the first foot X compensation angle θ1x is shown. Is calculated as follows. Although the description is omitted, the first foot Y compensation angle θ1y, the second foot X compensation angle θ2x, and the second foot Y compensation angle θ2y are similarly obtained. Here, the first foot X compensation angle θ
Only the algorithm for obtaining 1x will be described.

First foot floor reaction force moment X component M1ac
tx is passed through a low-pass filter and M1actfiltx
Get. The first foot compensation moment X component Mdmd1x is passed through a compensation filter, and is passed through M1actfiltx.
To obtain the deviation moment Mdiff1x. As with the determination of the two-leg compensation angle, the compensation filter improves the frequency response characteristics of the transfer function from Mdmd1x to the actual total floor reaction force.

Next, similarly to the determination of the two-leg compensation angle, the first foot X mechanism deformation compensation angle θff for canceling the influence of the deformation of the foot spring mechanism on the first foot compensation moment X component.
Find 1x. This is so-called feed-forward compensation.

Specifically, the deformation angle of the first foot is obtained by using a mechanism compliance model representing the relationship between the first foot compensation moment V-direction component Mdmd1x and the amount of deformation.
What is obtained by reversing the polarity may be set as the first foot X mechanism deformation compensation angle θff1x.

The first foot X mechanism deformation compensation angle θff1x is
Approximately, it can be obtained by the following equation. θff1x = −α1x * Mdmddbv Expression 16 where α1x is a predetermined constant.

Finally, the first foot X compensation angle θ1 is obtained by the following equation.
Get x. Here, K1x is a control gain, which is usually set to a positive value. θ1x = K1x * Mdiff1x + θff1x Expression 17 Note that the block diagram shown in the drawing may be subjected to an equivalent modification such as changing the operation processing order.

Referring back to FIG. 18, the corrected target foot position / posture calculation unit calculates the two feet compensation angle θdbv, the first foot X compensation angle θ1x, the first foot Y compensation angle θ1y, and the second foot. Based on the flat X compensation angle θ2x, the second foot Y compensation angle θ2y, and the in-phase compensation displacement (described later), the target foot position / posture is corrected in accordance with the foot position / posture correction method that is an extension of the above-described composite compliance operation. Then, the corrected target foot position / posture is obtained.

The mechanism deformation calculator calculates the spring mechanism 32 and the sole elastic body 34 which are expected to be generated by the desired foot floor reaction force.
Is obtained.

The corrected desired foot position / posture with mechanism deformation compensation is further corrected so as to cancel out the calculated amount of mechanism deformation, and the corrected desired foot position with mechanism deformation compensation is corrected.・ Get the posture.

For example, when the amount of mechanical deformation shown in FIG. 26 is predicted, the corrected target foot position / posture with mechanical deformation compensation is corrected to the position / posture shown by the solid line in FIG. That is, the position / posture when the foot after the mechanism deformation compensation shown in FIG. 27 is deformed by receiving the desired foot floor reaction force is as shown in FIG.
The corrected target foot position / posture with mechanical deformation compensation is calculated so as to match the foot position / posture before mechanical deformation compensation when no floor reaction force is applied as shown in (1).

The mechanism deformation compensation is a control for canceling the deviation of the actual foot position / posture caused by the deformation of the spring mechanism 32 and the sole elastic body 34 in a feed-forward manner. Thus, it is possible to realize a walking close to the desired gait.

Returning to the description of the flow chart of FIG. 10 on the premise of the above, the above-mentioned compensation angle is determined in S34 as described above.

FIG. 28 is a subroutine flowchart showing the operation.

The description will be made with reference to FIG.
, The vector V described above is obtained, and the process proceeds to S102, where the distribution weight variables are set as shown in FIG. 20, and these values at the current time t are obtained. Then, the process proceeds to S104, and the total floor reaction force moment Mdmd is compensated by Expressions 3 and 4.
Is distributed to both feet compensation moment Mdmddbv and each foot compensation moment Mdmdnx (y), the flow proceeds to S106 to obtain the two-leg compensation angle θdbv as described above, and proceeds to S108 to obtain each foot compensation angle θnx (y).

Returning to the description of the flow chart of FIG.
Subsequently, the process proceeds to S35 to determine the isotope compensation displacement, which will be described later.

Subsequently, the flow advances to S36, where a mechanical deformation compensation amount is calculated based on the desired foot floor reaction force, and the flow advances to S38 to correct the target foot position / posture to the compensation angles θdbv, θnx (y), and the in-phase compensation. Correction is made according to the displacement (described later), and further corrected according to the mechanism deformation compensation amount to obtain a corrected target foot position / posture with mechanism deformation compensation.

Then, the process proceeds to S40, where a joint displacement command (value) is calculated from the body position / posture and the corrected foot position / posture including the mechanism deformation compensation, and the process proceeds to S42, where the actual joint displacement is calculated. Value), the process proceeds to S44 to update the time by Δt, and returns to S14 to repeat the above processing.

Here, S31 in the flow chart of FIG.
And the processing of S35 will be described.

In these, the body acceleration is detected, and the force component (target value) Fdmd of the total floor reaction force to be compensated is determined by a control law described later so that the body acceleration is not excessively large or small.
This is a control in which all the feet are simultaneously translated in the same direction so that the high-frequency component of the force component of the actual total floor reaction force approaches the high-frequency component of Fdmd.

When this control is performed, for example, when the robot 1 falls due to an unexpected step during walking and the foot 22R or L lands strongly on the floor, the two feet 22R
By slightly raising (L) in the direction of gravity, landing impact can be absorbed.

Also, during walking, the robot can move the elasticity of the up-and-down expansion and contraction of the spring mechanism 32 of the foot and the sole elastic body 34 and the robot 1
, The upper body 24 vibrates up and down. Although this vibration is slight in the amount of displacement, the Z-direction force component Fz of the actual foot floor reaction force greatly varies.

If the amplitude of this vibration becomes excessive, the foot 22
The contact between R (L) and the floor surface is reduced, and the robot 1 spins in some cases. This control suppresses such vibrations,
Prevent spin and improve posture stabilization. With that in mind, in this embodiment, the previously proposed device
As shown in the lower part of FIG. 4, a body Z-direction acceleration control calculation unit is added.

The body Z-direction acceleration control calculation unit calculates the actual body acceleration Gbody detected by the tilt sensor 60 (more specifically, for example, the detected actual body vertical acceleration Gb
odyz) does not greatly deviate from the desired body gait body acceleration (ie, the desired body acceleration) Gbodyref. This corresponds to the process of S31.

In the following, Fdmd is calculated, for example, as follows. That is, the X component of Gbodyref is Gbodyrefx, the X component is Gbodyrefy, Z
Assuming that the component is Gbodyrefz, a deviation Fdmdttmp between the target value and the detected value is calculated according to the following equation.

Fdmdttmpx = −Kgx * (Gbodyz−Gbodyrefz) Fdmdttmpy = −Kgy * (Gbodyz−Gbodyrefz) Fdmdttmpz = −Kgz * (Gbodyz−Gbodyref. . . Equation 18

Here, Kgx, Kgy, and Kgz are control gains (constants). Fdmdttmpx, Fdmdt
mpy and Fdmdttmpz are the aforementioned deviations Fdm, respectively.
These are the X, Y, and Z components of dtmp, and the sum thereof is used as a deviation Fdmdtmp. Subsequently, the calculated Fdmdt
mp through a filter designed as appropriate to improve control characteristics,
The output is Fdmd.

[0220] Here, a vector having the desired total floor reaction force center point as the start point and the robot center of gravity position as the end point is represented by (Jx, Jy, J
z), it is preferable that the above ratio of Kgx, Kgy, Kgz is made equal to Jx, Jy, Jz. Then, Fdmd also becomes the direction of the vector (Jx, Jy, Jz).

Therefore, even if Fdmd is additionally generated as the floor reaction force at the target total floor reaction force center point, Fdmd acts toward the position of the center of gravity of the robot, so that no moment for rotating the robot 1 is generated. It does not affect the posture inclination of the robot 1. When the parallel movement amount of all the feet determined by Fdmd is small, Kgx
And Kgy may be set to zero. Even if doing so, similarly, the posture inclination of the robot 1 is not affected.

Next, the processing in S35 will be described.

Referring to FIG. 29, the modified target first foot obtained by the processing up to S38 ignoring the in-phase compensation displacement in the figure to manipulate the force component of the actual total floor reaction force is described. The position and orientation (indicated by a thin line in FIG. 29) are translated by a certain movement amount vector L1 without changing the orientation.

At the same time, the corrected target second foot position / posture is also translated by a certain movement vector L2 without changing the posture. FIG. 2 shows the corrected target first and second foot positions and postures after the movement.
9 is indicated by a thick line.

This is the compliance control for the force component of the actual total floor reaction force. In this specification, this is called “in-phase parallel movement operation”, and the movement vector Ln is called the n-th foot in-phase compensation displacement (n = 1). , 2).

That is, as shown in the lower part of FIG. 18, an in-phase compensation displacement calculating unit is provided, and the force component (target value) of the compensation total floor reaction force is provided.
And the corrected target foot position / posture calculation unit calculates the corrected target foot position / posture including each foot in-phase compensation displacement from the force component of the actual total floor reaction force. I did it.

FIG. 30 is a block diagram showing the operation in more detail, and FIG. 31 is a flow chart for explaining the operation in FIG.

Description will be made below with reference to FIG.
At 0, a force component Ftotalact of the actual total floor reaction force is obtained. That is, the force component F1act of the actual first foot floor reaction force and the force component F2act of the actual second foot floor reaction force are expressed by the following equation (Equation 1).
According to 9), the force component Ftotalact of the actual total floor reaction force is calculated. Ftotalact = F1act + F2act. . . Equation 19

Subsequently, the flow advances to S202 to execute Ftotalac.
From t, the desired total floor reaction force (ie, force component Fre of the desired gait)
f) is subtracted to obtain a deviation Ftotalerr, and S20
4. Go to Ftotalerr through appropriate filter
Get a fit.

Then, the program proceeds to S206, in which the force component (target value) Fdmd of the compensating total floor reaction force is passed through a compensating filter, and Ftotalerrfilt is subtracted from the filter passing value to obtain a difference Ftotaldiff. This compensating filter improves the frequency response from Fdmd to the force component of the actual total floor reaction force.

Then, the flow advances to S208, where the in-phase compensation displacement vector L is calculated by the following equation (Equation 20). L = K1 * Ftotaldiff. . . . Equation 20 Here, K1 is a control gain (constant. Scalar amount).

Next, the calculated vector L is distributed to each foot by the following equation. L1 = W11 * L L2 = W12 * L. . . . Equation 21 Here, W11 and W12 are attenuators (weights), and are set, for example, as shown in FIG.

Although W11 and W12 may always be 1,
After the foot 22R (L) leaves the floor, it may be set to 0 when in the air (in the illustrated example, the second foot side during the one-leg supporting period) in order to avoid wastefully moving up and down.

Including the in-phase compensation displacement Ln thus obtained,
In S38 of the flow chart of FIG. 10, a corrected target foot position / posture with mechanical deformation compensation is calculated.

More specifically, the two leg compensation angle θdbv, the first foot X compensation angle θ1x, the first foot Y compensation angle θ1y, the second foot X compensation angle θ2x, the second foot Y compensation angle θ2y, One foot in-phase compensation displacement L1 and a second foot in-phase compensation displacement L2
, The corrected target foot position / posture is calculated according to the composite compliance operation including the in-phase parallel movement operation described above.

In this embodiment, as described above, at least the base (upper body 24) and the first joint (10,
12, 14R (L)) and a plurality of legs (foot 22R (L)) connected to the distal ends thereof via second joints (18, 20R (L)). Books (2
In the control device of the legged mobile robot including the legs (leg links 2) of the book, a movement pattern (target body position / posture, target foot position) including at least the target position and posture of the foot of the robot. Gait for generating a gait of the robot including at least a target pattern of total floor reaction force acting on the robot (target total floor reaction force, target total floor reaction force center point (= target ZMP)) Generating means (gait generator, S10 to S22), a desired foot floor reaction force as an action center point on the foot when the generated total floor reaction force of the gait is distributed to each of the feet. A desired foot floor reaction force center point determining means (a desired floor reaction force distributor, S24, S26) for determining a center point (a desired foot floor reaction force center point), and an actual floor reaction force acting on the foot (actual floor reaction force distributor). Actual floor reaction force detection means (6-axis force sensor) for detecting each floor floor reaction force 44, actual foot floor reaction force detector,
S32), calculating a moment (actual nth foot floor reaction force moment Maxx, y, z) at which the detected actual floor reaction force acts around the calculated target foot floor reaction force center point, and The amount of rotation for rotating the foot based on the calculated moment (both leg compensation angles θdbv, z,
Foot rotation amount determining means for determining the n-th foot compensation angle θnx, y, z (combined compliance operation determining section, S32 to S34, both leg compensation angle determining section, n-th foot compensation angle determining section,
S100 to S108), first correcting the target position and / or posture such that the position and / or posture of the foot is rotated based on the determined amount of rotation of the foot.
(Combined compliance operation determination unit, S38, S40, correction target foot position / posture calculation unit), and obtains the force component Ftotalact of the actual total floor reaction force from the detected actual floor reaction force. Calculating the deviation Ftotalerr of the generated gait from the force component Fref of the total floor reaction force, and calculating the compensation displacement Ln of the foot based on the calculated deviation (the body Z direction). Acceleration control calculation unit, in-phase compensation displacement calculation unit, S31, S35, S
200 to S208), second foot position / posture correction means (body Z-direction acceleration control calculation unit, in-phase compensation displacement calculation unit) for correcting the target foot position / posture based on the calculated compensation displacement S38) and the first and second positions of the robot based on the position and posture of the foot corrected by the first and first and second foot position and posture means.
Displacement means (robot geometric model (kinematics calculation unit), displacement controller, S40, S4) for displacing joints (10, 12, 14, 18, 20R (L))
2) was provided.

Also, at least the base (upper body 24) is connected to the base via a first joint (10, 12, 14R (L)), and a second joint (18, 18
20R (L)) (foot 22R)
(L)), the control device of the legged mobile robot 1 including a plurality of (two) legs (leg links 2);
A movement pattern (target body position / posture, target foot position / posture) including at least the target position and posture of the foot of the robot, and a target pattern of total floor reaction force acting on the robot (target total floor reaction) Gait generating means (gait generator, S10 to S2) for generating a gait of the robot including at least a force and a desired total floor reaction force central point (= target ZMP)
2) a desired foot floor reaction force center point (a desired foot floor floor reaction force center) which is an action center point on the foot when the generated total floor reaction force of the gait is distributed to each of the feet; Means for determining the desired foot floor reaction force center point (target floor reaction force distributor,
S24), actual floor reaction force detecting means (6-axis force sensor 44, which detects actual floor reaction force (actual foot floor reaction force) acting on the foot portion;
Actual foot floor reaction force detector, S32), the amount of rotation for rotating the foot based on at least the detected actual floor reaction force (both leg compensation angles θdbv, z, n-th foot compensation angle θnx,
(y, z) foot rotation amount determining means (composite compliance operation determining unit, S32, S34, both leg compensation angle determining unit, n-th foot compensation angle determining unit, S100 to S108),
The target position and / or posture of the foot based on the determined rotation amount of the foot is rotated such that the position and / or posture of the foot rotates around the determined target foot floor reaction force center point or the vicinity thereof. First foot position to correct posture
Posture correction means (Composite compliance operation determination unit, S3
8, S40, corrected target foot position / posture calculation unit), a force component Ftotal of an actual total floor reaction force from the detected actual floor reaction force.
and calculating a deviation Ftotalerr of the generated gait from the force component Fref of the total floor reaction force, and calculating a compensation displacement Ln of the foot based on the calculated deviation. Body Z-direction acceleration control calculation unit, in-phase compensation displacement calculation unit, S31, S35, S200 to S
208), second foot position / posture correction means for correcting the target foot position / posture based on the calculated compensation displacement (body Z-direction acceleration control calculation unit, in-phase compensation displacement calculation unit, S38) And the first and the first and the second
Of the foot corrected by the foot position / posture means
Joint displacement means (robot geometric model (kinematics calculation unit), displacement controller, S42) for displacing the first and second joints (10, 12, 14, 18, 20R (L)) of the robot based on the posture ).

Further, the first foot position / posture correcting means may determine whether the position and / or posture of the foot is based on the determined foot rotation amount and the determined target foot floor reaction force. The target position and / or posture is modified so as to rotate around the center point or its vicinity.

Further, the total floor reaction force moment actually acting on the robot (more precisely, the moment component PQ1 *
F1act + PQ2 * F2act + M1act + M2act) or the total floor reaction force actually acting on the robot (PQ1 *
Moment (Mf1f2act = PQ1 * F1act +) obtained by subtracting the floor reaction force moment (M1act + M2act) acting on the foot from F1act + PQ2 * F2act + M1act + M2act
PQ2 * F2act) and a foot movement determining means (combined compliance operation determining unit, S34) for determining a moving amount (θdbv, z) for moving the foot at least according to the calculated moment. And a two-leg compensation angle determination unit, S100 to S108), wherein the first foot position / posture correcting means is configured to determine the position of the foot based on the determined foot rotation amount and the determined movement amount. And / or the posture is modified.

Further, a posture stabilizing compensation total floor reaction force moment (compensation total floor reaction force Mdmd) to be added to the target pattern of the total floor reaction force is obtained, and the foot rotation amount determining means and / or the foot movement is calculated. The amount determining means determines a rotation amount and / or a movement amount of the foot based on at least the detected actual floor reaction force (actual foot floor reaction force) and the determined posture stabilization compensation total floor reaction force moment. It was configured to be determined (S34, S100 to S108).

The posture stabilization compensation total floor reaction force moment is calculated as at least the inclination deviation (θerr) of the robot.
x, y) (S28, S30).

The compensation displacement calculating means includes a body acceleration detecting means (S31) for detecting a body acceleration (actual body vertical acceleration) Gbody of the robot, and a posture of the robot based on the detected body acceleration. (Target value Fdmd) of the force component of the compensating floor reaction force for stabilizing
Is calculated on the basis of the difference (Ftotaldiff or L) obtained by subtracting the deviation from the calculated total floor reaction force component. Is calculated (S200).
To S208).

The compensating displacement calculating means is configured to multiply the difference (Ftotaldiff or L) by a predetermined weight (attenuator W11, W12) to distribute the compensating displacement of the foot to each of the feet.

The compensation displacement calculating means is configured to set the weight to zero when the foot is off the floor.

The compensation displacement calculating means is configured to vary the weight according to the frequency of the force component of the actual total floor reaction force.

The compensation displacement calculation means is configured to calculate the compensation displacement of the foot in the direction of gravity.

Further, the compensation displacement calculating means is configured to calculate the compensation displacement of the foot in the direction of a line connecting the center of gravity and the desired total floor reaction force center point.

In addition, the force component of the compensating total floor reaction force is set to zero or near zero.

[0249] Since the configuration is made as described above,
In general, the control of the actual total floor reaction force and the control of the actual foot floor reaction force hardly interfere with each other, and they can be easily controlled.

That is, the apparatus according to this embodiment has the following improvements over the previously proposed technique. That is, in the ankle compliance control proposed in Japanese Patent Application Laid-Open No. 5-305584, an actual floor reaction force moment at a point fixed to the foot such as a reference point of an ankle or a sole is detected, and the fixed moment is detected based on the detected moment. Although the foot was rotated about the point, the apparatus according to the present embodiment calculates the actual foot floor reaction force moment at the moving target foot floor reaction force center point, and based on that, calculates the desired foot floor reaction force. The foot was rotated around the force center point, and the moment around that point was controlled to a desirable value.

As a result, it is possible to easily control the actual total floor reaction force and the actual foot floor reaction force with little interference. To reduce the interference, a more appropriate point may be selected in the sole contact area assumed at each moment.

Therefore, even if there is an unexpected floor shape change including not only global undulations and slopes but also local irregularities and slopes, the floor counteracting on the legged mobile robot is not greatly affected by the unexpected changes. The force can be controlled appropriately.

Further, the floor reaction force acting on the robot, more specifically, the actual total floor reaction force moment around the desired total floor reaction force center point (desired ZMP) and the actual each foot around the desired foot flat center point. The flat floor reaction force moment can be easily and appropriately controlled. In other words, compared to the previously proposed combined use of the two-leg compliance control and the ankle compliance control, there is no control interference, and the actual total floor reaction force and the actual floor floor reaction force can deviate or oscillate from desired values. Absent.

In addition, since the compliance control is performed on the force component of the actual floor reaction force received by the robot,
Even if an unexpected step is encountered and an excessive landing impact is generated, the landing impact can be absorbed and the grounding performance can be improved, thereby preventing the occurrence of spin and the like.

Also, the posture stabilization control of the legged mobile robot can be easily realized, the landing impact received by the legged mobile robot can be reduced, the grounding property of the legged mobile robot can be improved, and the slip during walking can be improved. And spin can be prevented. Further, the load on the actuator of the legged mobile robot can be reduced.

Here, supplementing the distribution of the in-phase compensation displacement L to each foot, in Equation 21, for example, the weights Wl1 and Wl2 are fixed to 1.0, or 0 in the case of a free leg.
In other words, the frequency was fixed with respect to the frequency of the force component of the actual total floor reaction force.

With such a setting, the low frequency component of the force component of the actual total floor reaction force cannot be arbitrarily controlled. Because, for example, when the feet are moved in phase and parallel in the direction of gravity when standing upright (ie, when the legs are contracted), the force component of the actual total floor reaction force decreases at that moment, but immediately thereafter Upper body 2
The reason is that the number 4 returns to the original actual total floor reaction force again when it moves downward.

If the integral term is added to Equation 20 to solve the problem, it is possible to control even the low frequency component of the actual total floor reaction force. However, during high-speed walking, if the body height greatly changes from the target gait, the dynamic balance will be lost, so it is not preferable to add an integral term.

Therefore, the distribution amount of the in-phase compensation displacement L may be different between the two feet, or may be variable with respect to the frequency of the force component of the actual total floor reaction force. That is, weights having frequency components may be used.

This will be described below. As shown in FIG. 33, the weights W11 and W12 are variably set with respect to the frequency of the force component of the actual total floor reaction force. Specifically, in a frequency region sufficiently lower than a predetermined value, for example, 5 Hz, the ratio of the weights Wl1 and Wl2 is set to around 1, and the ratio is changed so as to change in a frequency region higher than that.

For example, in a frequency region sufficiently lower than 5 Hz, it is determined that L1: L2 = 1: 1. That is, with respect to the low-frequency component of the force of the actual total floor reaction force, the upper body 24 largely moves in the direction of gravity as the foot 22R (L) expands and contracts. Make the value of both feet the same.

On the other hand, in the high frequency region of 5 Hz or more, in FIG. 29, the target total floor reaction force central point P and the corrected target It is set so as to be in inverse proportion to the distance from the n-foot floor reaction force center point Qn '. More specifically, in FIG. 29, it is determined that L1: L2 = PQ2 ′: PQ1 ′.

The above equation (18) may be generalized as follows. Fdmdtmpx = −Kgxx * (Gbodyx−Gbodyrefx) −Kgxy * (Gbodyy−Gbodyrefy) −Kgxz * (Gbodyz−Gbodyyrefz) Gbodyrefz) Fdmdttmpz = -Kgzx * (Gbodyx-Gbodyrefx) -Kgzy * (Gbodyy-Gbodyrefy) -Kgzz * (Gbodyyz-Gbodyrefz). . . . Expression 22 where Kgxx, Kgxy, Kgxz, Kgyx, K
gyy, Kgyz, Kgzx, Kgzy, Kgzz are
Control gain.

In the above embodiment, the detection of the body acceleration and the force component (target value) Fdm of the compensation floor reaction force are performed.
Omitting the control law for determining d and simply fixing Fdmd at or near zero is quite effective as compliance control for the force component of the actual total floor reaction force. still,
Claim 12 is based on this description.

In the above embodiment, the spring mechanism 32 (and the sole elastic body 34) is not an essential part of the present invention. The essence of the present invention lies in the feedback control part, and the mechanism deformation compensation is incidental.

In the above-described embodiment, the block diagram may be subjected to equivalent deformation such as changing the order of the arithmetic processing.

Although the above embodiment has been described with reference to the first embodiment of the previously proposed application, it may be added with respect to the second or subsequent embodiments.

Although the present invention has been described with reference to a bipedal walking robot, the present invention can be applied not only to a bipedal walking robot but also to a multi-legged robot.

[0269]

According to the first aspect, the floor reaction force acting on the legged mobile robot can be easily and appropriately controlled without causing interference. In other words, even if control similar to the combination of the previously proposed two-leg compliance control and ankle compliance control is performed, there is no control interference, and the actual total floor reaction force and the actual foot floor reaction force deviate from desired values or oscillate. Never do.

Further, the posture stabilization control of the legged mobile robot can be easily realized, the landing impact received by the legged mobile robot can be reduced, the grounding property of the legged mobile robot can be improved, and the slip during walking can be improved. And spin can be prevented.

Furthermore, even if there is an unexpected floor shape change including not only global undulations and inclinations but also local irregularities and inclinations, the floor acting on the legged mobile robot is not affected so much. The reaction force can be appropriately controlled.
Further, the load on the actuator of the legged mobile robot can be reduced.

According to the second aspect, the same function and effect as those of the first aspect can be obtained.

According to the third aspect, the same function and effect as those of the first aspect can be obtained, and the floor reaction force can be more appropriately controlled.

According to the fourth aspect, the same function and effect as those of the first aspect can be obtained, and the total floor reaction force, which is particularly important for attitude control, can be controlled more appropriately.

According to the fifth aspect, the same function and effect as those of the first aspect can be obtained and the posture stabilizing ability can be improved.

According to the sixth aspect, the same function and effect as described above can be obtained, and the posture stabilizing ability can be further improved.

According to the seventh aspect, the same function and effect as described above can be obtained, the landing impact can be more effectively absorbed, and the contact property can be further improved.

According to the eighth aspect, the same operation and effect as described above can be obtained.

According to the ninth aspect, the same operation and effect as described above can be obtained, and the load on the actuator can be further reduced by reducing the control to a necessary limit.

According to the tenth aspect, the same operation and effect as described above can be obtained, and a more stable posture can be obtained.

According to the eleventh aspect, the same operation and effect as described above can be obtained, and the amount of calculation can be reduced.

According to the twelfth aspect, the same operation and effect as described above can be obtained, and the amount of calculation can be reduced.

According to the thirteenth aspect, the same function and effect as described above can be obtained, the landing impact can be absorbed to a considerable extent, and the contact property can be improved to a considerable extent. Can be

[Brief description of the drawings]

FIG. 1 is an explanatory view showing an overall control device of a legged mobile robot according to the present invention.

FIG. 2 is an explanatory side view showing a structure of a foot of the bipedal walking robot shown in FIG. 1;

FIG. 3 is a block diagram showing details of a control unit of the bipedal walking robot shown in FIG. 1;

FIG. 4 is a block diagram functionally showing the configuration and operation of a control device for a legged mobile robot according to the present invention.

FIG. 5 is an explanatory diagram showing an example of a movement pattern when the legged mobile robot shown in FIG. 1 walks on flat ground.

6 is an explanatory diagram illustrating a locus on the floor surface of a locus of a desired total floor reaction force center point (a desired ZMP) corresponding to the motion pattern of FIG. 5;

FIG. 7 is a time chart of a target total floor reaction force center point (target ZMP) trajectory corresponding to the motion pattern of FIG. 5;

8 is a time chart of a target first floor floor reaction force center point trajectory set so as to satisfy a predetermined condition corresponding to the exercise pattern of FIG. 5;

9 is a time chart of a desired second foot floor reaction force center point trajectory set so as to satisfy a predetermined condition corresponding to the exercise pattern of FIG. 5;

FIG. 10 is a flowchart showing the operation of the control device for a legged mobile robot according to the present invention, similarly to FIG. 4;

FIG. 11 is a flowchart for explaining the operation of the composite compliance operation determining unit shown in FIG. 4 for performing arithmetic processing such as a double leg compensation angle in the flow chart of FIG. 10;
It is explanatory drawing which shows the situation in which each foot floor reaction force acts on a foot and a 2nd foot.

12 is an explanatory diagram showing setting of a desired total floor reaction force in the situation shown in FIG. 11;

13 is an explanatory diagram showing distribution of target foot floor reaction forces in the situation shown in FIG. 11;

FIG. 14 is an explanatory diagram showing a compensating total floor reaction force moment in the situation shown in FIG. 11;

15 is an explanatory diagram showing a normal vector V on a plane that includes each foot floor reaction force center point and that is perpendicular to the horizontal plane in the situation shown in FIG. 11;

FIG. 16 is an explanatory diagram showing a state in which each desired foot floor reaction force central point is rotated by a predetermined angle θdbv around a desired total floor reaction force central point (target ZMP) in the situation shown in FIG. 11; .

FIG. 17 shows a state in which each foot is set at a predetermined angle θnx, θny around a longitudinal axis and a lateral axis in the situation shown in FIG.
FIG. 4 is an explanatory diagram showing a state in which only rotation is performed.

FIG. 18 is a block diagram illustrating an operation process of a composite compliance operation determination unit in FIG. 4;

FIG. 19 is a block diagram showing a calculation process of the compensating total floor reaction force moment distributor shown in FIG. 18;

20 is a time chart showing an example of setting distribution weight variables for operating the two-leg compensation angle and the like of the compensating total floor reaction force moment distributor shown in FIG. 18;

21 is an explanatory diagram showing a posture of the robot for explaining setting of distribution weight variables of the compensating total floor reaction force moment distributor of FIG. 20;

FIG. 22 is an explanatory diagram showing a posture of a robot for explaining setting of a distribution weight variable of the compensating total floor reaction force moment distributor, similarly to FIG. 21;

FIG. 23 is an explanatory diagram showing a two-leg compensation moment V-direction component Mdmdbbv when distribution weights for operating the two-leg compensation angle are determined under predetermined conditions.

FIG. 24 is a block diagram illustrating a calculation process of a two-leg compensation angle determination unit illustrated in FIG. 18;

25 is a block diagram illustrating a calculation process of a compensation angle determination unit for each foot shown in FIG. 18;

26 is an explanatory diagram for describing the arithmetic processing of the corrected target foot position / posture calculation unit with mechanical deformation compensation shown in FIG. 18;

27 is an explanatory diagram similar to FIG. 26, illustrating the calculation processing of the corrected target foot position / posture calculation unit with mechanism deformation compensation shown in FIG. 18;

FIG. 28 is a subroutine flowchart showing a work of determining a leg compensation angle and the like in the flowchart of FIG. 10;

FIG. 29 is an explanatory diagram similar to FIG. 16, but showing the processing of S31 and S35 in the flowchart of FIG. 10 in the operation of the body acceleration control calculating section of FIG. .

FIG. 30 is a block diagram illustrating a process of the in-phase compensation displacement calculating unit in FIG. 18;

FIG. 31 is a subroutine showing a process of S35 in FIG. 10;
19 is a flowchart similarly illustrating the processing of the in-phase compensation displacement calculating unit in FIG. 18.

FIG. 32 is an explanatory diagram showing an example of setting weights used in FIG. 30;

FIG. 33 is an explanatory diagram showing another example of setting weights, similar to FIG. 32;

FIG. 34 is an explanatory diagram when the bipedal walking robot walks on an unexpected slope.

FIG. 35 is an explanatory diagram in the case where the previously proposed double-legged compliance control is performed on the bipedal walking robot shown in FIG. 34;

36 is an explanatory view similar to FIG. 34, when the biped walking robot steps on an unexpected projection. FIG.

FIG. 37 is an explanatory diagram when the previously proposed ankle compliance control is performed in the situation shown in FIG. 36;

[Explanation of symbols]

Reference Signs List 1 bipedal walking robot (legged mobile robot) 2 leg link 10, 12, 14R, L hip joint 16R, L knee joint 18, 20R, L ankle joint 22R, L foot (foot) 24 upper body 26 control unit 32 spring mechanism 34 sole elastic body 44 6-axis force sensor 60 inclination sensor

Claims (13)

[Claims] 1. A leg comprising at least a base and a plurality of legs having a foot connected to the base via a first joint and connected to a distal end of the base via a second joint. A control device for a mobile robot, comprising: a. Gait generating means for generating a gait of the robot including at least a movement pattern including a target position and a posture of the foot of the robot and a target pattern of a total floor reaction force acting on the robot; b. A desired foot floor reaction force center point that determines a desired foot floor reaction force center point as an action center point on the foot when the generated total floor reaction force of the gait is distributed to each of the feet. Determining means, c. Actual floor reaction force detecting means for detecting an actual floor reaction force acting on the foot; d. The detected actual floor reaction force calculates a moment acting around the calculated target foot floor reaction force central point, and determines a rotation amount for rotating the foot based on at least the calculated moment. Foot rotation amount determining means; e. First foot position / posture correcting means for correcting the target position and / or posture such that the position and / or posture of the foot is rotated based on the determined foot rotation amount; f. A force component of the actual total floor reaction force is obtained from the detected actual floor reaction force to calculate a deviation from the total floor reaction force force component of the generated gait, and the foot is calculated based on the calculated deviation. Compensation displacement calculating means for calculating a compensation displacement of the section; g. Target foot position based on the calculated compensation displacement
Second foot position / posture correcting means for correcting the posture, and h. Joint displacement means for displacing the first and second joints of the robot based on the position and posture of the foot corrected by the first and second foot position / posture means. Control device for mobile legged robots. 2. A leg comprising at least a base and a plurality of legs having a foot connected to the base via a first joint and having a tip connected to the distal end via a second joint. A control device for a mobile robot, comprising: a. Gait generating means for generating a gait of the robot including at least a movement pattern including a target position and a posture of the foot of the robot and a target pattern of a total floor reaction force acting on the robot; b. A desired foot floor reaction force center point that determines a desired foot floor reaction force center point as an action center point on the foot when the generated total floor reaction force of the gait is distributed to each of the feet. Determining means, c. Actual floor reaction force detecting means for detecting an actual floor reaction force acting on the foot; d. Foot rotation amount determining means for determining a rotation amount for rotating the foot based on at least the detected actual floor reaction force; e. The target position and / or posture is such that the position and / or posture of the foot is rotated around the determined target foot floor reaction force center point or the vicinity thereof based on the determined foot rotation amount. First foot position / posture correcting means for correcting the posture; f. A force component of the actual total floor reaction force is obtained from the detected actual floor reaction force to calculate a deviation from the total floor reaction force force component of the generated gait, and the foot is calculated based on the calculated deviation. Compensation displacement calculating means for calculating a compensation displacement of the section; g. Target foot position based on the calculated compensation displacement
Second foot position / posture correcting means for correcting the posture, and h. Joint displacement means for displacing the first and second joints of the robot based on the position and posture of the foot corrected by the first and second foot position / posture means. Control device for mobile legged robots. 3. The first foot position / posture correcting means,
The target position and / or posture is such that the position and / or posture of the foot is rotated around the determined target foot floor reaction force center point or the vicinity thereof based on the determined foot rotation amount. The control device for a legged mobile robot according to claim 1, wherein the posture is corrected. 4. The method of claim 1, further comprising: i. Either the total floor reaction force moment actually acting on the robot or the moment obtained by subtracting the floor reaction force moment acting on the foot from the total floor reaction force moment actually acting on the robot is calculated. A foot moving amount determining means for determining a moving amount for moving the foot at least in accordance with the calculated moment, wherein the first foot position / posture correcting means comprises: 4. The control device for a legged mobile robot according to claim 1, wherein the position and / or posture of the foot is corrected based on a rotation amount and the determined movement amount. 5. 5. A posture stabilizing compensation total floor reaction force moment to be added to the total floor reaction force target pattern, and the foot rotation amount determining means and / or the foot movement amount determining means determine at least the detection amount. The rotation amount and / or the movement amount of the foot are determined based on the actual floor reaction force obtained and the determined posture stabilization compensation total floor reaction force moment. A control device for a legged mobile robot according to any one of the first to third aspects. 6. The legged mobile robot according to claim 1, wherein the posture stabilization compensation total floor reaction force moment is obtained based on at least a tilt deviation of the robot. Control device. 7. The compensation displacement calculation means, j. Body acceleration detecting means for detecting the body acceleration of the robot; k. A compensating total floor reaction force component calculating means for calculating a compensating floor reaction force component for stabilizing the posture of the robot based on the detected base acceleration. The control device for a legged mobile robot according to any one of claims 1 to 6, wherein a compensation displacement of the foot is calculated based on a difference obtained by subtracting the deviation from the force component. . 8. The legged mobile robot according to claim 7, wherein the compensation displacement calculating means distributes the compensation displacement of the foot to each of the feet by multiplying the difference by a predetermined weight. Control device. 9. The control device for a legged mobile robot according to claim 8, wherein the compensation displacement calculation means sets the weight to zero when the foot is off the floor. 10. The control device for a legged mobile robot according to claim 8, wherein said compensation displacement calculating means changes said weight in accordance with a frequency of a force component of said actual total floor reaction force. . 11. The control device for a legged mobile robot according to claim 1, wherein the compensation displacement calculating means calculates the compensation displacement of the foot in the direction of gravity. 12. The apparatus according to claim 1, wherein said compensating displacement calculating means calculates the compensating displacement of the foot in a direction of a line connecting a center of gravity and a desired total floor reaction force center point.
The control device for a legged mobile robot according to any one of items 0. 13. The control device for a legged mobile robot according to claim 7, wherein a force component of the compensation total floor reaction force is set to zero or near zero.