JP4237130B2 - Control device for legged mobile robot - Google Patents

Description

  BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a legged mobile robot control device, and more particularly to a posture control device thereof, and more particularly to a floor that acts on a legged mobile robot by performing compliance control on the operation of legs of a legged mobile robot such as a biped robot. It relates to the one that appropriately controls the reaction force.

  The most basic and simple legged mobile robot, more specifically, a control device for a biped walking robot is composed of 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. Usually, the walking motion pattern is generated by dynamic calculation, that is, the ZMP trajectory obtained by solving the Euler-Newton equation becomes a desired trajectory set in advance. The joint drive control device controls each joint so as to follow the displacement command of each joint generated by the gait generator.

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

  In this device, although the gait generating device generates a gait assuming a flat floor surface, as shown in FIG. 40, at the beginning of the both-leg support period, If the foot stepped on an unexpected road surface, an excessive floor reaction force greater than expected on the foot would be generated, causing the robot to tilt. In order to solve the problem, the present applicant has proposed such a control device for a bipedal legged mobile robot in, for example, Patent Document 1 below.

  In this case, the required amount of restoring moment required to restore the body posture by detecting the body inclination is obtained, and the actual total floor reaction force moment component around the target total floor reaction force central point (target ZMP) is obtained. Detection is performed and control is performed so that each foot is raised and lowered in order to match it with the required amount of restoring moment. This 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 previously proposed control (hereinafter referred to as “both leg compliance control”) will be described taking as an example a case where there is an unexpected inclination as shown in FIG. For the sake of explanation, each foot is numbered as shown in this figure. Although the gait generator has generated a gait on the assumption of a flat floor surface, as shown in FIG. 40, the slope where the first foot was not expected at the beginning of the both-leg support period at present. It is assumed that this is the moment when a foot floor reaction force larger than a desired value is generated on the first foot. Further, it is assumed that the robot is still in a desirable posture (body tilt 0).

  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, the actual total floor reaction force moment acts in the direction of causing the robot to fall over later because the vertical component of the first foot floor reaction force is excessive.

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

  Thereby, 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 target total floor reaction force center point (target ZMP) becomes substantially zero. That is, even if there is an unexpected slope on the floor, both-leg compliance control works normally, so that the robot can continue walking without falling.

However, since this proposed technology alone cannot control the actual floor reaction force of each foot during the support period for both legs, if there is an unexpected local inclination or unevenness in the floor shape around the foot contact point, the foot The grounding ability of the robot may decrease, making it easier to spin, or causing a sudden change in posture and falling.

  For example, as shown in FIG. 42, if the toe of the first foot steps on an unexpected protrusion (step) during the support period of both legs, the toe of the first foot will drop rapidly during the support period of both legs. Since it is a certain time, the floor is strongly kicked by toes and the vertical component of the first foot floor reaction force increases rapidly. As a result, an actual total floor reaction force moment abruptly occurs around the target total floor reaction force center point (target ZMP), and in the worst case, even if an attempt is made to restore the posture by both-leg compliance, the vehicle falls over in time.

  In addition, even if the body does not fall during the period when both legs are supported, when the second foot is released from the floor immediately after that, the target total floor reaction force center point (target ZMP) is on the heel of the first foot. Because the heel is floating, the actual total floor reaction force center point is in a pinch, so an actual total floor reaction force moment is generated that tries to tilt the robot later around the target total floor reaction force center point (target ZMP). And falls.

  That is, it can be said that this both-legs compliance control can cope with the global inclination and the undulation that change slowly over a long distance, but cannot cope with the local inclination and the step of the landing point of the foot.

  In addition to the above-described both-leg compliance control, the present applicant, for example, in Patent Document 2 below, includes a landing shock absorbing mechanism having a spring characteristic such as rubber at the ankle portion of a biped walking robot, We have proposed ankle compliance control that detects the actual foot floor reaction force moment component and rotates each ankle to make it zero.

  In order to solve the above problems, the technique proposed in Patent Document 2 (hereinafter referred to as “ankle compliance control”) can be used in combination with the both-leg compliance control.

As a result, by the ankle compliance control, as shown in FIG. 43, the first ankle can be rotated in a direction to cancel the unexpected first foot floor reaction force moment, and the heel can be brought into contact with the floor. Therefore, the robot does not fall down as described above even in the subsequent one leg support period.
JP-A-5-305586 JP-A-5-305584

  However, simply using both the leg compliance control and the ankle compliance control described above causes the two types of controls to interfere with each other, causing the actual total floor reaction force and the actual each floor floor reaction force to deviate or oscillate from the desired values. was there.

  Accordingly, an object of the present invention is to eliminate the above-mentioned inconvenience, and a legged mobile robot that can easily and appropriately control the actual floor reaction force acting on the legged mobile robot without causing interference. It is to provide a control device.

  The second object of the present invention is to provide a legged mobile robot that is not greatly affected by unexpected floor shape changes including not only global undulation and inclination but also local unevenness and inclination. An object of the present invention is to provide a control device for a legged mobile robot capable of appropriately controlling an acting floor reaction force.

  A third object of the present invention is to provide a control apparatus for a legged mobile robot that can facilitate posture stabilization control of the legged mobile robot by appropriately controlling the floor reaction force acting on the legged mobile robot. Is to provide.

  A fourth object of the present invention is to provide a control apparatus for a legged mobile robot capable of reducing the landing impact received by the legged mobile robot by appropriately controlling the floor reaction force acting on the legged mobile robot. There is to do.

  In addition, biped robots walk with swinging swinging legs, which causes a moment of inertia around the vertical (gravity) axis of the robot, causing the upper body to oscillate around the vertical axis, resulting in each The vertical component of the foot floor reaction force moment vibrates. When the amplitude of the vibration becomes excessive, the peak value of the actual foot floor reaction force moment exceeds the limit of friction, and at that moment, the sole slips and the robot spins. If the spin is large, it may lose posture stability and fall. Therefore, it is desirable to reduce such vibrations in addition to the control described above.

  Accordingly, a fifth object of the present invention is to appropriately control the floor reaction force acting on the legged mobile robot and to reduce the vibration of the vertical component of the floor reaction force moment. Is to provide.

  The sixth 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 preventing slipping and the above-mentioned spin during walking. It is an object of the present invention to provide a control device for a legged mobile robot.

  A seventh object of the present invention is to provide a control device for a legged mobile robot that can reduce the load on the actuator of the legged mobile robot by appropriately controlling the floor reaction force acting on the legged mobile robot. There is to do.

  In order to achieve the above object, according to the first aspect of the present invention, at least the base is connected to the base via the first joint and is connected to the tip of the base via the second joint. In a control apparatus for a legged mobile robot comprising a plurality of legs having a foot, a movement pattern including at least a target position or posture of the foot of the robot, and a target of a total floor reaction force acting on the robot A gait generating means for generating a target gait of the robot comprising a trajectory pattern, a posture stabilization compensating total floor reaction force determining means for determining a compensation total floor reaction force for posture stabilization of the robot, An actual foot reaction force detecting means for detecting an actual foot reaction force acting on the floor, a floor reaction force distribution for distributing the total floor reaction force of the target gait and the compensating total floor reaction force to at least each of the feet. Means, the whole floor of the distributed target gait Correction means for correcting the position or posture of the foot part of the target gait based on the force, the total floor reaction force and the detected actual floor reaction force of the foot, and the foot part of the corrected target gait It is configured to include joint displacement control means for controlling displacement of the first and second joints based on position or posture.

  In this claim and the following claims, the “target pattern of the total floor reaction force” means a target pattern related to the total floor reaction force including at least the center point locus of the total floor reaction force. Further, the “total floor reaction force” specifically means the resultant force of the floor reaction force acting on the robot via the leg tips. “Foot part” specifically means a foot resembling the human foot of a biped robot, but includes the tip of the leg part of the other three or more legged mobile robots. It is also used in the sense that includes mainly nails, etc. that differ from the feet.

  In the legged mobile robot according to claim 2, the posture stabilization compensation total floor reaction force determining means obtains the posture stabilization compensation total floor reaction force based on at least the tilt deviation of the robot. Configured.

  In the control apparatus for a legged mobile robot according to claim 3, the posture stabilization compensation total floor reaction force determination means determines the posture stabilization compensation total floor reaction force based on at least the yaw rate of the robot. Configured as desired.

  In the control device for a legged mobile robot according to claim 4, the posture stabilization compensation total floor reaction force determination means generates the posture stabilization compensation total floor reaction force from at least the target path of the robot. It was configured as required based on the deviation.

  In the control device for a legged mobile robot according to claim 5, the posture stabilization compensation total floor reaction force determining means sets a predetermined component in the posture stabilization compensation total floor reaction force to zero or the predetermined component. It was configured to be set in the vicinity.

  In the control device for a legged mobile robot according to claim 6, the correction means is configured to further correct the position or posture of the foot of the target gait based on the posture deviation of the robot.

  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 the control similar to the combined use of the two-leg compliance control and the ankle compliance control previously proposed is performed, there is no control interference, and the actual total floor reaction force and each actual floor reaction force deviate from desired values or oscillate. There is nothing to do. In addition, since the correction amount of the foot can be more appropriately distributed, it is possible to obtain a greater restoring force and a higher grounding property for posture stabilization.

  In other words, the floor reaction force that acts on the legged mobile robot without being affected by an unexpected floor shape change including not only global undulation and inclination but also local unevenness and inclination is appropriate. Can be controlled.

  In addition, the legged mobile robot's posture stabilization control can be easily realized, the landing impact received by the legged mobile robot can be reduced, the grounding performance of the legged mobile robot can be improved, and slip and spin during walking can be reduced. Can be prevented. Furthermore, the load on the actuator of the legged mobile robot can be reduced.

  In the second aspect, the posture stabilization ability can be further improved.

  According to the third aspect, it is possible to obtain the same effects as described above, to prevent the robot from spinning or slipping, and to further improve the posture stabilization ability. Therefore, even when walking on a road surface with a low friction coefficient, spin or slip can be effectively prevented and walking can be continued in a stable posture.

  According to the fourth aspect, it is possible to obtain the same operational effects as described above and to continue walking in a stable posture along the target route.

  According to the fifth aspect, it is possible to obtain the same effect as described above, and to prevent the robot from spinning or slipping to a considerable extent without detecting the yaw rate. Stabilization ability can be further improved.

  According to the sixth aspect, it is possible to obtain the same effect as described above, and to control the actual floor reaction force acting on the robot with higher accuracy.

  Hereinafter, a legged mobile robot controller according to the present invention will be described with reference to the accompanying drawings. As a legged mobile robot, a biped walking robot is taken as an example.

  FIG. 1 is a schematic diagram showing the control apparatus for the legged mobile robot as a whole.

  As shown in the figure, the bipedal walking robot 1 has six joints on 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, 10L for leg rotation of the crotch (waist) (R on the right side and L on the left side, the same applies hereinafter), the roll direction of the crotch (waist) (around the Y axis) Joints 12R and 12L, joints 14R and 14L in the same pitch direction (around the X axis), joints 16R and 16L in the knee roll direction, joints 18R and 18L in the ankle roll direction, and joints 20R and 20L in the same pitch direction Composed.

  Foot feet (foot portions) 22R and 22L are attached to the lower portions of the joints 18R (L) and 20R (L), and an upper body (base body) 24 is provided at the uppermost position. The control unit 26 (described later) is stored. In the above, the hip joint (or waist joint) is composed of joints 10R (L), 12R (L), 14R (L), and the ankle joint (ankle joint) is composed of joints 18R (L), 20R (L). The hip joint and knee joint are connected by thigh links 28R and 28L, and the knee joint and ankle joint are connected by crus links 30R and 30L.

  With the above configuration, the leg link 2 is given six degrees of freedom for the left and right feet, and by driving these 6 * 2 = 12 joints at an appropriate angle during walking, it is desired for the entire foot. Can be arbitrarily walked in a three-dimensional space (in this specification, “*” indicates multiplication for a scalar operation and outer product for a vector operation).

  Note that the position of the upper body and its speed described later in this specification mean a predetermined position of the upper body 24, specifically, the position of a representative point such as the position of the center of gravity of the upper body 24 and its moving speed.

  As shown in FIG. 1, a known six-axis force sensor 44 is attached below the ankle joint to measure three-direction components Fx, Fy, Fz of force and three-direction components Mx, My, Mz of moment, The presence or absence of landing on the foot and the floor reaction force (ground load) are detected. Further, an inclination sensor 60 is installed on the body 24 to detect an inclination with respect to the Z axis (vertical direction (gravity direction)) and its angular velocity. Each electric motor of each joint is provided with a rotary encoder that detects 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. Specifically, the spring mechanism 32 is attached to the rectangular guide member attached to the foot 22R (L), the ankle joint 18R (L) and the six-axis force sensor 44, and an elastic material is applied to the guide member. And a piston-like member that is housed so as to be freely movable.

  A foot 22R (L) indicated by a solid line in the figure indicates a state when the floor reaction force is not 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 for mitigating landing impacts but also for improving controllability. The details are described in the above-mentioned Japanese Patent Laid-Open No. 5-305584, and detailed description thereof is omitted.

  Further, although not shown in FIG. 1, a joystick 62 is provided at an appropriate position of the biped walking robot 1, and a request for a gait such as turning a robot that advances straight from the outside as needed is input. Configured to be able to.

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

  The control unit is provided with first and second arithmetic devices 80 and 82 composed of a CPU. The first arithmetic device 80 is based on the gait stored in the ROM 84, as will be described later, as will be described later. A joint angle displacement command is calculated and sent to the RAM 74. The second arithmetic unit 82 reads out the command and the detected actual value from the RAM 74, calculates a control value necessary for driving each joint, and sets each joint via the D / A converter 86 and the servo amplifier. Output to the electric motor to be driven.

  Here, terms used in this specification and drawings are defined (for terms that are not defined, definitions used in an application (Japanese Patent Application No. 8-214261) proposed separately from the technique described above by the present applicant). Follow).

  Unlike the general definition in robot engineering, “gait” is used to refer to 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 “ZMP locus only”. Therefore, the term “gait generator” is not used for a device that outputs only the target motion pattern and does not output information on the floor reaction force pattern.

  Each leg is given a serial number. The floor reaction force acting on the nth leg is referred to as the nth foot floor reaction force (n: 1 or 2, hereinafter the same). The total floor reaction force is a combination of floor reaction forces acting on all legs (generally called floor reaction forces in robotics, but here we will refer to them as “total floor reaction forces” to distinguish them from foot floor reaction forces. Called).

  The foot floor reaction force is expressed by the point of action, the force applied to it, and the moment of force. For the same foot floor reaction force, there are infinite combinations of expressions. Among them, there is a notation that the moment component excluding the component around the vertical axis is zero and the action point is on the floor surface. The action point in this expression is herein referred to as the foot floor reaction force center point (referred to as “ground pressure center of gravity point” in Japanese Patent Laid-Open No. 6-79657, which will be described later separately proposed by the present applicant).

  Similarly, the total floor reaction force is expressed 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 an expression in which the moment component excluding the component around the vertical axis is 0 and the action point is on the floor surface. The action point in this expression is referred to herein as the center point of the total floor reaction force.

  The target value of the total floor reaction force is called the target total floor reaction force. The target total floor reaction force is usually a total floor reaction force that is dynamically balanced with respect to the target motion pattern. Therefore, the target total floor reaction force central point usually coincides with the target ZMP.

  As mentioned at the beginning, the target ZMP (Zero Moment Point) is defined as follows. That is, if the inertial force generated by the target motion pattern and the resultant force of gravity are determined kinetically and the moment acting on a certain point on the floor is zero except for the component around the vertical axis, that point is This is called a target ZMP (Zero Moment Point). The target ZMP is uniquely determined unless the vertical force component of the resultant force is zero. In the following description, for the sake of easy understanding, the term target ZMP may be used, but strictly speaking, there are many points that should be referred to as the target total floor reaction force central point.

  The target value of each foot floor reaction force is referred to as a target 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 movement 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 an actual robot is called an actual foot floor reaction force.

  Here, to reiterate the problem of the present invention, it is difficult to obtain good posture stability with the two-leg compliance control previously proposed for local inclination and step, and the disadvantage is that ankle compliance control is used. However, there is a disadvantage that the actual total floor reaction force and the actual foot floor reaction force deviate from desired values or oscillate.

  Explaining the problem in the situation shown in FIG. 40, since the first foot receives an unexpected excessive floor reaction force on the heel, an excessive actual foot floor reaction force is generated around the first ankle. A moment is generated. In ankle compliance control, the first ankle is rotated as shown in FIG. 43 in order to reduce this moment.

  However, since the heel position of the first foot increases due to the rotation of the ankle, the vertical component of the first foot floor reaction force decreases. As a result, the actual total floor reaction force moment around the target total floor reaction force central point (target ZMP) changes. This means that the actual total floor reaction force moment, which is the control amount of the both-leg compliance control, is interfered with the ankle compliance control.

  Accordingly, if both legs compliance control is operated in the same manner as without ankle compliance control without considering interference due to ankle compliance control, the actual total floor reaction force moment around the target total floor reaction force central point (target ZMP) is 0. Or vibration or oscillation due to interference occurs.

  One way to prevent this is to calculate the amount of interference between the two-leg compliance control and the ankle compliance control, and to avoid interference by adding an operation amount that cancels it. However, it is extremely difficult to avoid interference with this method because the interference relationship changes every moment.

  In addition, in the situation shown in FIG. 43, the floor with which the first foot is in contact is climbing more inclined than the assumed floor, so the first foot should be better than the target gait. Nevertheless, it can be said that the ankle compliance control does not work properly if the ankle compliance control lowers the tow.

  As described above, ankle compliance control is effective for local floor inclinations and steps at the landing point of the foot, but it may adversely affect inclinations and undulations that change slowly over long distances. .

  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 about the target total floor reaction force center point, and the desired foot floor reaction force. Each foot floor reaction force moment around the center point can be controlled easily and appropriately.

  In addition to this, even if there is an unexpected floor shape change including not only global waviness and inclination but also local unevenness and inclination, the robot should continue to walk in a stable posture without being affected by it. I made it.

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

  The apparatus includes a gait generator, and the gait generator generates and outputs a target gait. The target gait is the target motion pattern and target floor reaction force pattern, more specifically, the target body position / posture trajectory, target foot position / posture trajectory, target total floor reaction force center point ( Target ZMP) trajectory and target total floor reaction force trajectory. Thus, the target floor reaction force pattern includes the target total floor reaction force center point locus (if the mechanism deformation compensation described later is not performed, the target floor reaction force center point locus alone is used as the target floor reaction force center pattern. good).

  In this embodiment, the desired total floor reaction force output by the gait generator is a total floor reaction force that is dynamically balanced with respect to the desired motion pattern. Accordingly, the target total floor reaction force central point coincides with the target ZMP.

  FIG. 5 shows an example of a target motion pattern when the robot 1 walks on a flat ground. The trajectory on the floor of the target ZMP trajectory corresponding to this is shown in FIG. 6, and the time chart is shown in FIG. The foot that is in contact with the floor during this gait period is referred to as the first foot, and the other foot as the second foot. The details of the gait generator are described in detail in the previously proposed Japanese Patent Application No. 8-214261, so further explanation is omitted.

  Returning to the explanation of FIG. 4, this apparatus includes a target floor reaction force distributor, and the target floor reaction force distributor mainly inputs a target total floor reaction force center point (target ZMP) and a target foot position / posture. The target foot floor reaction force center point is determined and output. Actually, from the gait generator, the gait parameters (for example, the time of both leg support period and the target landing position of the free leg foot) and the time and time of the gait (for example, the current time of Information such as 0.1 sec from the beginning) is also imported if necessary.

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

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

  The target floor reaction force distributor further determines and outputs each foot floor reaction force, although it is incidental. Each desired foot floor reaction force is necessary to compensate for deflection of the spring mechanism 32 and the like.

  If the target floor reaction force corresponding to the target foot floor reaction force center point set as described above is determined using the following equation, the resultant force of the target foot floor reaction force matches the target total floor reaction force. Satisfy the condition of having to.

Target first foot floor reaction force = Target total floor reaction force * (Distance between target second foot floor reaction force center point and target ZMP) / (Target first foot floor reaction force center point and target 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 second foot) Flat floor reaction force center point distance)
... Formula 1

  Since each desired foot floor reaction force obtained in this way changes continuously, it is suitable for realizing walking with less impact. The details described above are described in a technique (Japanese Patent Laid-Open No. 6-79657) separately proposed by the present applicant.

  Returning to the description of FIG. 4, this apparatus includes a posture stabilization control calculation unit, which estimates the state of the robot based on the sensor information of the robot and calculates a compensated total floor reaction force. That is, when the robot is actually walking or standing upright, even if the actual joint displacement can be made to completely follow the target joint displacement by the displacement controller described later, the position / posture of the robot is not necessarily the desired position / posture. I can't take a posture.

  To stabilize the robot's posture in the long term, obtain the force and moment necessary to restore the robot to the desired position and posture, and use this as the target point of the total floor reaction force center (target ZMP). It must be generated additionally. This additional force and moment is called compensated total floor reaction force. The moment component of the compensated total floor reaction force is referred to as a compensated total floor reaction force moment.

  It is assumed that the target gait of the legged mobile robot receives a reaction force other than the floor reaction force from the environment, which is referred to as a target object reaction force, for example, and the definition of the target ZMP described above is as follows: You may extend as follows. That is, the resultant force of the inertial force, gravity and target object reaction force generated by the target motion pattern is obtained dynamically, and the moment that acts on a certain point on the floor surface is zero except for the component around the vertical axis. If so, this point may be set as the target ZMP again.

  If it is assumed that the robot 1 is a completely rigid body, and the actual joint displacement can be made to follow the target joint displacement completely by the displacement controller, the robot generated by the deflection of the foot spring mechanism 32 and the sole elastic body 34. The perturbative motion of the entire position / posture can be decomposed into the following six degrees of freedom.

Mode 1) Rotation around the longitudinal axis about the target floor reaction force center point (target ZMP) (ie, tilting left and right)
Mode 2) Rotation around the left / right axis about the target floor reaction force center point (target ZMP) (ie, forward / backward tilt)
Mode 3) Rotation around the vertical axis around the target total floor reaction force center point (target ZMP) (ie, spin)
Mode 4) Forward / backward translation shaking mode 5) Left / right translational shaking mode 6) Vertical translational shaking

  Of these modes, mode 4 and mode 5 occur when the foot spring mechanism 32 and the elastic body 34 are bent by receiving a shearing force in the front-rear and left-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, this swing is extremely small and there is almost no adverse effect on walking.

  Of the remaining four degrees of freedom, mode 3 and mode 6 are not directly related to the present invention, so only control for mode 1 and mode 2 is performed in this embodiment and the second embodiment described later. The control for mode 1 and mode 2 is very important because the robot will fall over most of the time if it is not. Note that control for mode 3 is performed in the third embodiment.

  The operation amount for controlling the mode 1 is a moment component around the longitudinal axis (X axis) of the compensating total floor reaction force. The operation amount for controlling the mode 2 is a moment component around the left / right axis (Y axis) of the compensating total floor reaction force. Therefore, only the longitudinal axial moment component and the lateral axial moment component need be obtained from the components of the compensated total floor reaction force. The other components are not used in this example (and the second example) and may be zero.

  The following definition follows. That is, the moment component of the compensated total floor reaction force is referred to as a compensated total floor reaction force moment Mdmd (specifically, the compensated total floor reaction force moment Mdmd around the target total floor reaction force central point (target ZMP)). As shown in FIG. 5, the robot's forward direction is the X-axis, the left lateral direction is the Y-axis, the upward direction is the Z-axis, and the coordinate system with the point on the floor just below the ankle of the first foot as the origin is the support leg. This is called the coordinate system, and unless otherwise specified, the position, force and moment are expressed in this coordinate system. Further, the X component of Mdmd is described as Mdmdx, the Y component as Mdmdy, and the Z component as Mdmdz. The tilt deviation of the body 24 (ie, the actual body tilt—the target body tilt) θerr is described as θerrx, the Y component as θerry, and their time differential values as (dθerrx / dt) and (dθerry / dt). To do.

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

  The composite compliance operation determination unit described later works to make the actual total floor reaction force coincide with the resultant force of the target total floor reaction force and the compensated total floor reaction force.

  Returning to the description of FIG. 4, this apparatus is provided with actual foot floor reaction force detectors, and each actual foot floor reaction force detector is detected by the 6-axis force sensor 44 with the actual foot floor reaction force (the resultant force is actually Floor reaction force) is detected. 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 upper body is calculated, and the detected value of the six-axis force sensor 44 is thereby calculated. Is converted into coordinates, and the actual foot floor reaction force expressed in the coordinate system fixed to the upper body is calculated, and then converted into the support leg coordinate system.

  This device is equipped with a robot geometric model (inverse kinematics calculation unit). When a body position / posture and a foot position / posture are input, the robot geometric model calculates joint displacements that satisfy them. . When the degree of joint freedom per leg is 6 as in the robot 1 in this embodiment, each joint displacement is uniquely determined.

  In this embodiment, the inverse kinematics solution equation is obtained directly, and each joint displacement is obtained simply by substituting the body position / posture and the foot position / posture into the equation. 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 mechanism deformation compensation) corrected by the composite compliance action determination unit. From them, joint displacement commands (values) of 12 joints (10R (L), etc.) are calculated.

  This apparatus includes a displacement controller (same as the above-described second arithmetic unit 82). The displacement controller uses a joint displacement command (value) calculated by a robot geometric model (inverse kinematics arithmetic unit) as a target value. The follow-up control of the displacement of 12 joints of 1 is performed.

  This apparatus includes the composite compliance operation determining unit, and the composite compliance operation determining unit corrects the desired foot position / posture trajectory so as to satisfy the following two requirements.

  Requirement 1) To control the position / posture of the robot, the actual total floor reaction force is caused to follow the resultant force of the compensated total floor reaction force (moment Mdmd) output from the posture stabilization control unit and the target total floor reaction force. When it is desired to control only the posture inclination of the robot, only the actual total floor reaction force horizontal moment component around the target total floor reaction force center point is caused to follow the compensated total floor reaction force moment Mdmd.

  Requirement 2) In order to ensure the ground contact of each foot, the absolute value of the actual foot floor reaction force moment around the desired foot floor reaction force center point should be made as small as possible.

  As a supplementary note, the actual foot floor reaction force moment around the center point of the desired foot floor reaction force is normally set to 0 while the actual total floor reaction force is made to coincide with the resultant force of the compensated total floor reaction force and the desired total floor reaction force. In many cases, it is physically impossible. Therefore, requirements 1) and 2) cannot be completely compatible and must be compromised in some respects.

  Based on the above, the operation of this apparatus will be described with reference to FIG. 10 flow chart (structured flow chart). In addition, the component of FIG. 4 apparatus which performs the process applicable to the left end of a figure is shown.

  First, in S10, the apparatus is initialized, and in S12, the process proceeds to S14 to wait for a timer interrupt. A timer interrupt is made every 50 ms, that is, the control period is 50 ms.

  Subsequently, the process proceeds to S16 to determine whether the gait is switched, that is, whether the support leg is switched. When the determination is negative, the process proceeds to S22, and when the determination is positive, the process proceeds to S18 to initialize the timer t. Proceeding to S20, the target gait parameter is set. As described above, the gait parameters are composed of motion parameters and floor reaction force parameters (ZMP trajectory parameters).

  Subsequently, the process proceeds to S22, and an instantaneous value of the target gait is determined. Here, “instantaneous value” means a value for each control cycle, and the desired gait instantaneous value is composed of a desired body position / posture, each desired foot position / posture, and a desired ZMP position. Here, “posture” means “orientation” in X, Y, Z space.

  Subsequently, the process proceeds to S24 to obtain a target foot floor reaction force central point. This is done as described in the description of the target floor reaction force distributor. Specifically, as shown in FIG. 8 and FIG. 9, it is performed by obtaining a value at the current time t of the set target foot floor reaction force central point locus.

  Then, it progresses to S26 and calculates | requires each desired foot floor reaction force. This is performed by calculating each desired foot floor reaction force using the equation 1 described in the explanation of the target floor reaction force distributor.

  Subsequently, the process proceeds to S28, and the state of the robot 1 such as the tilt of the upper body 24 is detected from the output of the tilt sensor 60 and the like.

  Subsequently, the process proceeds to S30, and compensated total floor reaction force moments Mdmdx and Mdmdy (around the target total floor reaction force central point (target ZMP)) for obtaining a posture from the state of the robot 1 and the like are obtained. Specifically, when the body inclination is detected, the compensated total floor reaction force moments Mdmdx and Mdmdy are calculated according to the above-described equation 2 in order to stabilize the posture.

  Subsequently, the process proceeds to S32 to detect the actual foot floor reaction force. This is detected from the output of the six-axis force sensor 44 as described above.

  Subsequently, the process proceeds to S34, in which both leg compensation angles θdbv and each foot compensation angle θnx (y) are determined. This is an operation performed by the composite compliance operation determination unit described above.

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

  Here, the vector Fnact represents the force component of the nth foot floor reaction force. Vector Mact represents the moment component of the nth foot floor reaction force. The direction of the vector Mact represents 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 target total floor reaction force moment vector Msumref at the target total floor reaction force central point (target ZMP) is vertical (by definition, the target ZMP is a point where the horizontal component of the target total floor reaction force moment is zero. From).

  When this is distributed to each desired foot floor reaction force according to Equation 1, it is as shown in FIG. In the figure, a vector Fnref represents a force component of the target nth foot floor reaction force. The vector Mnref represents the moment component of the target nth foot floor reaction force. The expression of the direction of the vector Mnref is the same as that of Mnact.

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

  In the above-described posture stabilization control calculation unit, the compensated total floor reaction force moment Mdmd is calculated based on the detected body tilt deviation values θerrx and θerror of the robot 1. In this embodiment, since the spin around the vertical axis (Z axis) is not controlled, the vertical axis component of the compensated total floor reaction force moment Mdmd is zero. Since the swing of the body position is not controlled, the force component of the compensated total floor reaction force moment is zero. FIG. 14 shows the compensated 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 target total floor reaction force central point (target ZMP) is obtained by adding the sum of the target total floor reaction force moment Msumref and the compensated total floor reaction force moment Mdmd. What is necessary is just to make it follow a horizontal component.

  On the other hand, at the target total floor reaction force central point (target ZMP), the horizontal component of the target total floor reaction force moment Msumref is zero. Accordingly, in order to restore the posture inclination of the front, rear, left and right, the horizontal component of the actual total floor reaction force moment around the target ZMP may be made to follow the horizontal component of Mdmd.

In this embodiment, the composite compliance operation determining unit corrects the position / 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 direction (X, Y axis direction) component of the actual total floor reaction force moment around the target total floor reaction force center point (target ZMP) is The horizontal component of the reaction force moment Mdmd is made to follow.
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 desired foot floor reaction force center point is made as small as possible.

  However, as described above, request 1) and request 2) cannot be made completely compatible, and must be compromised at a certain point.

In this embodiment, the foot position / posture is corrected as follows.
1) A normal vector V of a plane including the target first foot floor reaction force center point Q1 and the target second foot floor reaction force center point Q2 and perpendicular to the horizontal plane is obtained. The magnitude of V is 1. V is shown in FIG.

  2) The coordinates of the target first foot floor reaction force center point Q1 are rotated by a certain rotation angle θdbv around the normal vector V around the target total floor reaction force center point (target ZMP). Let Q1 'be the point after the movement. Similarly, the coordinates of the target second foot floor reaction force center point Q2 are rotated by the rotation angle θdbv around the normal vector V around the target total floor reaction force center point (target ZMP). Let Q2 'be the point after the movement.

  This rotation angle θdbv is referred to as a both-leg compensation angle. A vector having a start point Q1 and an end point Q1 'is defined as a vector Q1Q1'. Similarly, a vector having a start point Q2 and an end point Q2 'is a vector Q2Q2'. FIG. 16 shows Q1 'and Q2'.

  3) The target first foot is translated (substantially moved up and down) by the vector Q1Q1 'without changing the posture. Similarly, the target second foot is translated by the vector Q2Q2 'without changing the posture. Each foot of the target after movement is indicated by a thick line in FIG.

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

  If the above compensation operation amount is not excessive, even if the ground pressure distribution changes, the ground contact area (area where the pressure on the sole surface 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 compensation operation amount, and an actual foot floor reaction force corresponding to the deformation amount is generated. As a result, the relationship between the compensation operation amount and the change amount of the actual floor reaction force generated by the compensation operation has the following favorable characteristics.

  Characteristic 1) When only the leg compensation angle θdbv is operated to move the target foot position, the force component of the actual foot floor reaction force of the lowered foot increases, and the actual foot floor reaction force of the raised foot increases. The force component is reduced. At this time, the actual foot floor reaction force moments around the corrected desired foot floor reaction force center point hardly change.

  Characteristic 2) When only the nth foot X compensation angle is operated to rotate the target nth foot posture, the X component of the moment of the actual nth foot floor reaction force acting on the target nth foot floor reaction force center point Only changes, and other floor reaction force components change little. Similarly, when only the nth foot Y compensation angle is operated to rotate the target nth foot posture, only the Y component of the moment of the actual nth foot floor reaction force changes, and the other floor reaction force components are It changes only a little.

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

  Characteristic 1 and characteristic 2 indicate that these operations are independent, and it can be said that characteristic 3 indicates that these operations are linear.

  FIG. 18 is a block diagram showing the calculation processing of the composite compliance operation determining unit, and this work will be described with reference to FIG.

  Briefly, the compensation total floor reaction force moment distributor distributes the compensation total floor reaction force moment Mdmd. Next, from the actual foot floor reaction force and the distributed total floor reaction force moment, etc., the above-described compensation angles θdbv and θnx (y ).

  Next, the corrected desired foot position calculation unit obtains the compensated foot position / posture (referred to as the corrected target foot position / posture) by geometric calculation based on the determined various compensation angles. Finally, the corrected desired foot position / posture calculation unit with mechanism deformation compensation calculates the deformation amount of the spring mechanism 32 and the sole elastic body 34 that are expected to be generated by the desired foot floor reaction force, and cancels them. Correct the corrected foot position / posture.

  More specifically, the compensating total floor reaction force moment distributor distributes the compensating total floor reaction force moment Mdmd to both leg compensation moments Mdmddb and each foot compensation moment Mdmd1x, y, Mdmd2x, y. The both-leg compensation moment Mdmdb is a target value of a moment generated by the force component of each foot floor reaction force around the target total floor reaction force center point (target ZMP) by manipulating the both leg compensation angle (foot vertical amount) θdbv. .

  A component around the V direction of the both-leg compensation moment Mdmdb is described as Mdmddvv. The vector V is a vector defined in the description of the composite compliance operation determination unit. If a vector orthogonal to V and orthogonal to the vertical direction is U, the U-direction component Mdmddbu of the both-leg compensation moment Mdmddbu is set to zero. This is because the U-direction moment component of the floor reaction force cannot be generated even when the both-leg compensation angle θdbv is manipulated.

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

  The first foot compensation moment Mdmd1 is a moment that is desired to be generated around the target first foot floor reaction force center 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 a moment that is desired to be generated around the desired second foot floor reaction force center point by manipulating the second foot compensation angles θ2x and θ2y. The X component of the second foot compensation moment Mdmd2 is described as Mdmd2x, and the Y component is described as Mdmd2y.

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

  Here, Wdbx, Wdby, W1x, W1y, W2x, W2y, and Wint are distribution weight variables. 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 these, Wint is for canceling out the total floor reaction force moment generated by manipulating both leg compensation angles by manipulating each foot compensation angle.

  FIG. 19 shows a block diagram of a compensating total floor reaction force moment distributor that performs the arithmetic processing of Expression 3 and Expression 4.

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

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

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

  At this time, it is better to change the setting policy as shown below according to the situation such as standing upright or walking. In a situation where the V-direction component Mdmdbv of the both leg compensation moments and the respective foot compensation moments Mdmd1 and Mdmd2 can be faithfully generated in each actual foot floor reaction force, such as when standing upright, the following settings are made.

  In this situation, in order to make the horizontal component of the actual total floor reaction force moment around the target total floor reaction force center point (target ZMP) coincide with the horizontal component of the compensated total floor reaction force moment Mdmd (that is, as described above). In order to satisfy the requirement 1 for the composite compliance operation determining unit, the weights should be set so as to satisfy both the formulas 5 and 6 as much as possible.

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

If Expression 3 and Expression 4 are substituted for this, Expression 5 is converted to Expression 7, and Expression 6 is converted to Expression 8.
(Wdbx * Mdmdx + Wdby * Mdmdy) * Vx + W1x * (Mdmdx-Wint * Vx * (Wdbx *
Mdmdx + Wdby * Mdmdy)) + W2x * (Mdmdx-Wint * Vx * (Wdbx * Mdmdx +
Wdby * Mdmdy)) = Mdmdx
... Formula 7
(Wdbx * Mdmdx + Wdby * Mdmdy) * Vy + W1y * (Mdmdy-Wint * Vy * (Wdbx *
Mdmdx + Wdby * Mdmdy)) + W2y * (Mdmdy-Wint * Vy * (Wdbx * Mdmdx +
Wdby * Mdmdy)) = Mdmdy
... Formula 8

Even if Mdmdx and Mdmdy take arbitrary values, Equations 9, 10 and 11 need only be satisfied at the same time in order for Equation 7 and Equation 8 to be established equally.
Wint = 1 ・ ・ ・ Equation 9
W1x + W2x = 1 ・ ・ ・ Equation 10
W1y + W2y = 1 ・ ・ ・ Formula 11
That is, in the above situation, the weight may be determined so as to satisfy the expressions 9, 10, and 11 simultaneously.

  When walking, even if the position of the foot is corrected by manipulating both leg compensation angle θdbv with Mdmddbv as a target, the amount of actual floor reaction force moment generated may be insufficient compared to Mdmddv. For example, as shown in FIG. 21, in a situation where the robot tilts backward in the early stage of both-leg support and the first foot has not yet landed, 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 second foot angle is corrected by operating the second foot compensation angle θ2 with Mdmd2 as a target, the increase amount of the actual floor reaction force moment may be insufficient as compared with Mdmd2. For example, in the situation where the robot is tilted backward in the latter half of the both-leg support period as shown in FIG. 22, even if the heel of the second foot is lowered by θ2, the actual floor reaction force does not change.

  Therefore, even if the respective weights are set so as to satisfy the expressions 5 and 6, the increase amount of the actual total floor reaction force generated by the composite compliance control may not reach the compensated total floor reaction force moment Mdmd. In situations where this is likely to occur, the value on the left side of Equations 5 and 6 should be greater than 1.

  In FIG. 20, which is an example of setting the distribution weight variable at the time of walking, by setting Wint to 0, an actual total floor reaction force moment can be generated even if the both-leg compensation angle θdbv is operated as in the situation of FIG. Even if it disappeared, each foot compensation angle was manipulated to compensate for the shortage.

  Conveniently, as shown in FIG. 21, if the heel of the second foot is lowered as a result, the heel of the second foot tends to come into contact with the floor, so that the actual total floor reaction force moment can be obtained by manipulating the second foot compensation angle. Can be generated.

  In addition, an actual total floor reaction force moment is generated by manipulating the both-leg compensation angle θdbv when the vehicle is not tilted backward, but the heel of the second foot does not touch the floor, so the second foot compensation angle is manipulated. However, the actual total floor reaction force moment does not occur.

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

  Depending on the situation, the total amount of actual floor reaction force moment generated by manipulating both leg compensation angles 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 manipulated variable for posture stabilization 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, but only the open loop gain of the control system changes slightly, and the closed loop characteristics hardly change. .

  Note 3) In the one-leg support period, the absolute values of Wdbx and Wdby, which are distribution weight variables for both-leg compensation angles, are reduced. This is because in the one-leg support period, even if the compensation angle for both legs is changed, the foot that is not in contact with the foot only moves up and down unnecessarily, and the actual foot floor reaction force does not change.

  Note 4) When the force component of the desired foot floor reaction force is small in order to secure the ground contact property of the foot, the absolute value of the distribution weight variable for the foot compensation angle of the foot is made small. In particular, 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. The absolute value of the distribution weight variable for the foot compensation angle of the foot should be reduced.

  Note 5) The direction of the actual total floor reaction force moment that can be controlled by manipulating both leg compensation angles is different from the direction of the actual total floor reaction force moment that can be controlled by manipulating each foot compensation angle.

  For example, the direction of the actual total floor reaction force moment generated by manipulating the both-leg compensation angle θdbv is always 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 manipulating each foot compensation angle is limited by the ground contact state of the foot.

  For example, when only the toe edge or only the heel edge is grounded, a moment cannot be generated in the edge line direction. In the both-leg support period, in consideration of this characteristic, the both-leg compensation angle and each foot compensation angle are operated without waste as much as possible.

  For example, the distribution weights Wdbx and Wdby for operating both leg compensation angles are determined as follows.

  Assuming that a vector having an X component of Wdbx, a Y component of Wdby, and a Z component of 0 is Wdb, Equation 3 is an inner product of the vectors Wdb and Mdmd. Therefore, it can be said that Mdmdb obtained by Equation 3 is obtained by decomposing Mdmd into a vector Wdb direction component and its orthogonal component, extracting only the vector Wdb direction component, and multiplying by the magnitude of the vector Wdb.

  FIG. 23 shows Mdmddbv in this case. This means that a feedback control system is configured to control the Wdb direction component of the actual total floor reaction force moment by manipulating both leg compensation angles. If the Wdb direction is orthogonal to the vector V, no matter how much the both-leg compensation angle is manipulated, the Wdb-direction component of the actual total floor reaction force moment does not occur, so this feedback control system simply manipulates the both-leg compensation angle. Just do it.

  Therefore, when the useless movement is reduced, the Wdb direction should coincide with the vector V direction or be as close as possible. If the Wdb direction component of the compensation total floor reaction force moment Mdmd is generated only by operating both leg compensation angles without depending on each foot compensation angle, the inner product of Wdb and V is set to 1. To do. If it is desired to rely on each foot compensation angle for a part, the inner product of Wdb and V is set to be smaller than 1.

  By the way, 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 smaller. In this case, Wdbx is set larger. The Wdb direction and the vector V direction do not coincide with each other, and the variation of the both-leg compensation angle increases, but the stability increases.

  Further details of the both-leg compensation angle determination unit will be described below. FIG. 24 is a block diagram of the calculation process of the both-leg compensation angle determination unit, and the both-leg compensation angle θdbv is calculated as shown in the figure.

  Referring to FIG. 24, F1act acting on the target first foot floor reaction force center point Q1 and F2act acting on the target second foot floor reaction force center point Q2 are around the desired total floor reaction force center point P. The moment Mf1f2act to be generated is calculated by the following equation.

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

Moreover, there is practically no problem even if the following equation is used instead of equation 12.
Mf1f2act = PQ1 * F1act + PQ2 * F2act + M1act + M2act ・ ・ ・ Equation 12a
Expression 12a is an expression for calculating the actual total floor reaction force moment Mact acting around the target total floor reaction force center point. Equation 12 is obtained by subtracting the actual foot floor reaction force moment acting around the target foot floor reaction force center point from the actual floor reaction force moment acting around the target floor reaction force center point. ing.

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

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

  Next, the both-leg compensation moment V-direction component Mdmddbv is passed through the compensation filter, and is subtracted from Mf1f2actvfilt to obtain the deviation moment V-direction component Mdiffv.

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

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

  Specifically, a line connecting the target first foot floor reaction force center point Q1 and the target second foot floor reaction force center point Q2 using a mechanism compliance model representing the relationship between the both-leg compensation moment V-direction component Mdmddbv and the deformation amount. What is necessary is just to obtain | require the deformation angle of a minute, and what reverse | inverted the polarity is made into the both-legs mechanism deformation compensation angle (theta) ffdbv.

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

Finally, the both-leg compensation angle θdbv is obtained by the following equation. Here, Kdb is a control gain, which is normally 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 calculated as shown in the figure. To do. 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, only the algorithm for obtaining the first foot X compensation angle θ1x will be described.

  The first foot floor reaction force moment X component M1actx is passed through a low-pass filter to obtain M1actfiltx. The first foot compensation moment X component Mdmd1x is passed through a compensation filter and is subtracted from M1actfiltx to obtain a deviation moment Mdiff1x. Similar to the determination of the both-leg compensation angle, the compensation filter improves the frequency response characteristic of the transfer function from Mdmd1x to the actual total floor reaction force.

  Next, the first foot X mechanism deformation compensation angle θff1x for canceling the influence on the first foot compensation moment X component due to the deformation of the foot spring mechanism or the like is obtained in the same manner as the both-leg compensation angle determination. This is so-called feedforward compensation.

  Specifically, using a mechanism compliance model representing the relationship between the first foot compensation moment X-direction component Mdmd1x and the amount of deformation, the deformation angle of the first foot is obtained, and the polarity of the first foot is inverted. The flat X mechanism deformation compensation angle θff1x may be used.

The first foot X mechanism deformation compensation angle θff1x may be determined approximately by the following equation.
θff1x = −α1x * Mdmd1x Equation 16
Here, α1x is a predetermined constant.

Finally, the first foot X compensation angle θ1x is obtained by the following equation. Here, K1x is a control gain, which is normally set to a positive value.
θ1x = K1x * Mdiff1x + θff1x Equation 17
It should be noted that the illustrated block diagram may be equivalently modified such as changing the processing order.

  Returning to FIG. 18 and continuing the description, the corrected target foot position / posture calculation unit performs both leg compensation angle θdbv, first foot X compensation angle θ1x, first foot Y compensation angle θ1y, and second foot X compensation. Based on the angle θ2x and the second foot Y compensation angle θ2y, the target foot position / posture is corrected according to the above-described method for correcting the foot position / posture of the composite compliance operation to obtain the corrected target foot position / posture.

  The mechanism deformation amount calculation unit obtains deformation amounts of the spring mechanism 32 and the sole elastic body 34 that are expected to be generated by the desired foot floor reaction force.

  The corrected target foot position / posture calculation unit including mechanism deformation compensation further corrects the corrected target foot position / posture so as to cancel the calculated mechanism deformation amount, and the corrected target foot position / posture including mechanism deformation compensation is determined. obtain.

  For example, when a mechanism deformation amount as shown in FIG. 26 is predicted, the corrected target foot position / posture with mechanism deformation compensation is corrected to the position / posture indicated by the solid line in FIG. That is, the position / posture when the foot after mechanism deformation compensation shown in FIG. 27 is deformed by receiving the desired foot floor reaction force matches the foot position / posture before mechanism deformation compensation shown in FIG. Then, the corrected target foot position / posture with mechanism deformation compensation is calculated.

  The mechanism deformation compensation is a control that cancels out 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 feedforward manner. Compared to the case without this control, the target step is further increased. It is possible to realize a walking that is close to your heart.

  Returning to the description of the flowchart of FIG. 10 based on the above, the compensation angle described above is determined in S34 as described above.

  FIG. 28 is a subroutine flow chart showing the work.

  Referring to FIG. 20, first, the vector V described above is obtained in S100, the process proceeds to S102, the distribution weight variable is set as shown in FIG. 20, and these values at the current time t are obtained. Subsequently, the process proceeds to S104, and the total floor reaction force moment Mdmd is distributed to the both leg compensation moments Mdmddbv and the respective foot compensation moments Mdmdnx (y) according to Expressions 3 and 4, and the process proceeds to S106 and the both leg compensation angles θdbv as described above. The process proceeds to S108, and each foot compensation angle θnx (y) is obtained.

  Next, the process proceeds to S36 in the flowchart of FIG. 10 to calculate a mechanism deformation compensation amount based on each desired foot floor reaction force, and proceeds to S38 to determine the desired foot position / posture according to the compensation angles θdbv, θnx (y). Then, this is corrected according to the mechanism deformation compensation amount to obtain the corrected target foot position / posture with mechanism deformation compensation.

  Next, the process proceeds to S40, a joint displacement command (value) is calculated from the body position / posture and the corrected foot position / posture with mechanism deformation compensation, and the process proceeds to S42 to calculate the actual joint displacement to the calculated joint displacement command (value). Servo control is performed, the process proceeds to S44, the time is updated by Δt, and the process returns to S14 to repeat the above processing.

  Since this embodiment is configured as described above, in summary, 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 can be easily controlled. it can.

  That is, the apparatus according to this embodiment has the following improvements over the previously proposed technology. That is, in the ankle compliance control proposed in JP-A-5-305584, an actual floor reaction force moment at a point fixed to the foot such as the reference point of the ankle or the sole is detected, and the fixed force is detected based on the detected moment. In the apparatus according to this embodiment, the actual foot floor reaction force moment at the moving target foot floor reaction force center point is calculated, and the desired foot floor reaction force center is calculated based on the calculated foot floor reaction force moment. Changed to rotate the foot around the point, and controlled the moment around the point to the desired value.

  As a result, the actual total floor reaction force and the actual foot floor reaction force can be easily controlled with little interference. In order to further reduce the interference, a more appropriate point may be selected within the assumed ground contact area at each moment.

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

  Therefore, the floor reaction force acting on the legged mobile robot is not affected by unexpected changes in the floor shape, including not only global swell and inclination but also local unevenness and inclination. Can be controlled.

  In addition, the legged mobile robot's posture stabilization control can be easily realized, the landing impact received by the legged mobile robot can be reduced, the grounding performance of the legged mobile robot can be improved, and slip and spin during walking can be reduced. Can be prevented. Furthermore, the load on the actuator of the legged mobile robot can be reduced.

  Further, in the both-leg compliance control proposed in JP-A-5-305586, the moment component around the target total floor reaction force center point (target ZMP) of the actual total floor reaction force (the resultant force of each floor floor reaction force) is detected, Although the value was controlled so as to be a desirable value, in the apparatus according to this embodiment, the translational force component excluding the moment component of the foot floor reaction force acting on each desired foot floor reaction force central point. Is detected so as to control the value to be a desired value (this point is the previously proposed control method) May be).

  FIG. 29 is an explanatory view similar to FIG. 16, showing a second embodiment of the control device for the legged mobile robot according to the present invention.

The apparatus according to the second embodiment simplified the compensation operation. In this embodiment, the foot position correcting operation method for operating the force component of the floor reaction force of each foot is replaced with the method shown in FIG. 16 and moved only in the vertical direction as shown in FIG. I tried to make it. At this time, the first foot vertical movement amount Z1 and the second foot vertical movement amount Z2 are obtained by the following equations.
Z1 = -Length of line segment PQ1 * θdbv
Z2 = length of line segment PQ2 * θdbv
... Formula 18
However, here, the value obtained by Equation 15 is substituted for θdbv.

  The other configuration is not different from the first embodiment. In the second embodiment, by configuring as described above, it is possible to obtain substantially the same operations and effects as in the first embodiment.

  FIG. 30 is an explanatory view similar to FIG. 4, showing a third embodiment of the control device for the legged mobile robot according to the present invention. FIG. 31 is a flowchart similar to FIG. 10 showing the operation.

  In the third embodiment, compliance control for the Z component (the component around the vertical axis) of the actual total floor reaction force moment is added. That is, in the third embodiment, the Z component of the actual foot floor reaction force moment caused by the natural rotational vibration around the Z axis is reduced.

  In the third embodiment, for this purpose, new functions are added to the configuration of the apparatus according to the first embodiment in the state detector, the posture stabilization control calculation unit, and the composite compliance operation determination unit.

  Referring to FIG. 30, in the apparatus according to the third embodiment, first, the yaw rate sensor 100 is provided at an appropriate position of the upper body 24 of the robot 1 and its output is input to obtain the current position and posture. And a self-position / posture / direction estimator 102 for estimating the traveling direction. The yaw rate sensor 100 outputs a signal corresponding to the yaw rate (rotational angular velocity) around the Z axis of the robot 1. The rotational angular velocity about the X and Y axes is calculated based on the output of the tilt sensor 60.

  The self-position / posture / direction estimator 102 obtains the relative position and direction of the current landing position with respect to the previous landing position based on the desired foot trajectory or actual joint angle, and integrates the detection value of the yaw rate sensor 100. Thus, the traveling direction of the robot is detected.

  Further, based on these pieces of information, the positional deviation and the direction deviation of the robot 1 with respect to the target route are estimated by dead reckoning. In dead reckoning, an error in the estimated value tends to diverge, so the environment may be recognized and corrected using a camera or the like. Further, when the body 24 is tilted with respect to the vertical axis, the yaw rate detection value at the time of turning becomes a value smaller than the actual value.

  The apparatus according to the third embodiment will be described with reference to the flowchart of FIG. 31. After proceeding from S10 to S30, in S30a, based on the estimated position and / or direction deviation of the robot 1, the deviation is determined. The Z component Mdmdz of the compensated total floor reaction force moment is obtained so that.

Hereinafter, the case of walking along the route shown in FIG. 32 will be described as an example. The following equation is used as a control law for obtaining the Z component Mdmdz of the compensated total floor reaction force moment.
Mdmdz = −Kthzθerrz− Kwz (dθerrz / dt) − Khzh
... Formula 19
Here, θerrz is a deviation in direction, dθerrz / dt is a time differential value thereof, and h is a lateral deviation from the path. The figure shows θerrz, h. In the state of this figure, θerrz, h is positive. Kthz, Kwz, and Khz are route guidance control gains (constants).

  As described above, when the robot 1 is walking, the upper body 24 rotates about the vertical axis by the torsional elasticity of the foot spring mechanism 32 and the sole elastic body 34 and the inertial moment of moment about the vertical axis of the robot 1. Vibrate. This is called natural rotational vibration around the Z axis. Due to this vibration, the Z component of the actual foot floor reaction force moment vibrates. When the robot 1 is walking, the actual foot floor reaction force is approximately the sum of the desired foot floor reaction force moment and the vibration.

  When the amplitude of the vibration becomes excessive, the peak value of the actual foot floor reaction force moment exceeds the limit of friction, and at that moment, the sole slips and the robot 1 spins. If the spin is large, it may lose posture stability and fall. That is, it can be seen that simply suppressing the natural rotational vibration around the Z-axis has the effect of improving posture stability.

  If the natural rotational vibration around the Z axis is merely suppressed, the Z component Mdmdz of the compensated total floor reaction force moment may be determined from only the yaw rate deviation (time differential value of the direction deviation). That is, in Equation 19, the route guidance control gain other than Kwz may be set to 0.

  In the third embodiment, a new function is added to the composite compliance operation determination unit. Specifically, in order to manipulate the moment component around the Z-axis of the actual total floor reaction force and the actual foot floor reaction force, the position / posture correction operation of the foot 22R (L) is the operation in the first embodiment. Added to. The composite compliance operation determination unit determines this correction amount.

  Before describing the determination method, the added operation will be specifically described below.

  The correction of the position and posture of the foot for manipulating the moment component about the Z-axis of the total floor reaction force and the floor reaction force of each foot is performed by the composite compliance operation described in the first embodiment. This is done by applying the following correction to the flat position / posture (the thick foot in FIG. 16) and the corrected foot floor reaction force center points (Q1 ′, Q2 ′ in FIG. 16).

  1) The coordinates of the corrected target first foot floor reaction force center point (Q1 ′ in FIG. 16) are rotated by a rotation angle θdbz around the Z axis with the target total floor reaction force action point (target ZMP) as the rotation center. Rotate. Let Q1 "be the point after the movement.

  Similarly, the coordinates of the target second foot floor reaction force central point (Q2 ′ in FIG. 16) are rotated by a rotation angle θdbz around the Z axis with the target total floor reaction force action point (target ZMP) as the rotation center. Rotate. The point after the movement is defined as Q2 ″. This rotation angle θdbz is called a both-leg compensation angle around the Z axis.

  2) A vector having a start point Q1 'and an end point Q1 "is defined as a vector Q1'Q1". Similarly, a vector having a start point Q2 'and an end point Q2 "is set as a vector Q2'Q2". FIG. 33 shows Q1 ″ and Q2 ″ viewed from above.

  3) The correction target first foot is translated by the vector Q1′Q1 ″ without changing the posture. Similarly, the correction target second foot is parallel by the vector Q2′Q2 ″ without changing the posture. Move. Each corrected target foot after movement is indicated by a bold line in FIG.

  4) Next, the correction target first foot obtained in 3) is rotated about the vertical axis (Z axis) by Q1 ″ around the rotation angle θ1z. Similarly, the correction obtained in 3) The target second foot is rotated about the vertical axis (Z-axis) by a rotation angle θ2z around Q2 ″. The rotation angle θnz is called the nth foot Z compensation angle. Each corrected target foot after rotation is indicated by a thick line in FIG.

  If the above compensation operation amount is not excessive, the relationship between the compensation operation amount and the amount of change in the actual floor reaction force generated by the compensation operation has the following good characteristics.

  Characteristic 1) When only the leg compensation angle θdbz around the Z axis is operated to move the corrected target foot position, a force component of the actual foot floor reaction force is generated in the direction opposite to the moving direction. At this time, the actual foot floor reaction force moment around the corrected target foot actual floor reaction force center point hardly changes.

  Characteristic 2) When only the nth foot Z compensation angle is operated to rotate each desired foot posture, the Z-axis component of the moment of the actual nth foot floor reaction force acting on the target nth foot floor reaction force center point Only changes, and other floor reaction force components change little.

  Characteristic 3) When the both leg compensation angle θdbz, each foot X compensation angle, each foot Y compensation angle, both Z-leg both leg compensation angle θdbz and each foot Z compensation angle are operated simultaneously, the amount of change in the actual foot floor reaction force Is the sum of the amount of change when each is operated independently (in other words, linear combination is possible).

  Characteristic 1 and characteristic 2 indicate that these operations are independent, and it can be said that characteristic 3 indicates that all operations are linear.

  The Z-axis both-legs compensation angle θdbz and each foot Z compensation angle in the above operation are determined in the composite compliance operation determination unit as follows.

  FIG. 35 shows a schematic block diagram of the composite compliance unit of the third embodiment. As shown in the figure, a moment Z component compensation operation determining unit 104 is added. FIG. 36 shows details of the moment Z component compensation operation determination unit 104. Hereinafter, this additional portion will be mainly described.

  As shown in FIG. 36, the Z component compensation moment determination unit 104 determines the Z component Mdmdbz and the foot compensation moments Mdmd1z and Mdmd2z of both leg compensation moments Mdmdb from the output of the compensating total floor reaction force moment distributor. From the actual foot floor reaction force and the distributed compensation moment Z component (Mdmddbz, Mdmdnz), etc., both leg compensation angle θdbz around the Z axis, first foot Z compensation angle θ1z and second foot Z compensation angle θ2z are determined. (This corresponds to the process of S34a in the flowchart of FIG. 38).

  The corrected desired foot position / posture calculation unit obtains a corrected foot position / posture including geometric compensation by including both leg compensation angles around the Z axis and each foot Z compensation angle.

  Details of the additional processing will be described below.

  The distribution of the compensated total floor reaction force moment will be described with reference to FIG. 37. In the compensated total floor reaction force moment distributor, the Z component Mdmddz of the compensation total floor reaction force moment Mdmd is changed to the Z component Mdmdbz of both legs compensation moment Mdmddb, A process of distributing to the Z component Mdmd1z of the first foot compensation moment Mdmd1 and the Z component Mdmd2z of the second foot compensation moment Mdmd2 is added.

  Note that the Z component Mdmddbz of the both-leg compensation moment is the target of the Z component of the moment created by the force component Fnact of each foot floor reaction force around the desired total floor reaction force center point (target ZMP) by manipulating the both-leg compensation angle θdbz. Value.

  Further, the Z component Mdmd1z of the first foot compensation moment is the moment Z component (indicated by M1 in FIG. 34) that is desired to be generated around the desired first foot floor reaction force center point by manipulating the first foot compensation angle θ1z. ). Similarly, the Z component Mdmd2 of the second foot compensation moment is the moment Z component (M2 in FIG. 34) that is desired to be generated around the target second foot floor reaction force center point by manipulating the second foot compensation angle θ2z. Show).

The distribution is performed using, for example, the following equation.
Mdmddbz = Wdbz x Mdmdz
Mdmd1z = W1z × Mdmdz
Mdmd2z = W2z × Mdmdz
... Formula 20
Here, Wdbz, W1z, and W2z are distribution weight variables during walking. An example of setting the distribution weight variables Wdbz, W1z, and W2z is shown in FIG. The setting pattern shown in the figure is determined in consideration of the following points.

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

  Note 2) The Z component of the actual floor reaction force moment generated by manipulating both leg compensation angles around the Z axis and each foot Z compensation angle is as close as possible to the Z component Mdmdz of the compensation total floor reaction force moment. Thus, the distribution weight variables Wdbz, W1z, and W2z are determined.

  In a situation where the Z component Mdmddbz and the foot compensation moment Z components Mdmd1z and Mdmd2z of the both leg compensation moments can be faithfully generated in the actual foot floor reaction force, such as when standing upright, the following settings are made. That is, the Z component of the actual floor reaction force moment Mact around the target total floor reaction force center point (target ZMP) is made to coincide with the Z component of the compensated total floor reaction force moment Mdmd (in other words, in the first embodiment). In order to satisfy the requirement 1 for the composite compliance operation unit described above), weights are set so as to satisfy the following expression 21 as much as possible.

Wdbz + W1z + W2z = 1
... Formula 21
When walking, it is sufficient if the left side of Equation 21 is close to 1. In other words, it does not necessarily have to be 1.

  Note 3) If the leg compensation angle θdbz around the Z-axis is not 0 when the free leg foot lands, the foot landing position may deviate from the target position, which may adversely affect the trajectory guidance control. Therefore, it is desirable to set the distribution weight variable Wdbz for the both-leg compensation angle around the Z axis to 0 near the time when the foot lands.

  Note 4) If the Z-compensation angle of the free leg foot is not zero at the time of landing, the foot landing direction may deviate from the target direction, which may adversely affect the trajectory guidance control. Accordingly, the distribution weight variable W1z for the first foot Z compensation angle is set to 0 near the time when the first foot lands, and when the second foot lands, the second foot Z compensation angle It is desirable to set the distribution weight variable W2z to 0.

  In addition, a process of determining both leg compensation angles θdbz around the Z axis is added. The both-leg compensation angle θdbz around the Z-axis is obtained by the same algorithm as the both-leg compensation angle θdbv. The only difference is that the direction of moment and angle has changed from V direction to Z direction. Therefore, a block diagram of the process for determining the both-legs compensation angle θdbz around the Z-axis can be obtained by replacing V in FIG.

  Further, a process for determining the first foot compensation angle θ1z and the second foot compensation angle θ2z is added. The n-th foot Z compensation angle θnz is obtained by an algorithm similar to the algorithm for obtaining the first foot X compensation angle θ1x. The only difference is that X has changed to Z and 1 has changed to n. Therefore, the block diagram of the process for determining the n-th foot Z compensation angle θnz can be obtained by replacing X in FIG. 25 with Z and 1 with n.

  The above-described processing subroutine performed in S34a of FIG. 31 is shown in S200 to S206 of FIG.

  Based on the above, in the corrected target foot position / posture calculation (corresponding to S38 in the flow chart of FIG. 31), both legs compensation angle θdbv, both legs around Z axis compensation angle θdbz, first foot X compensation angle θ1x, first foot Based on the flat Y compensation angle θ1y, the first foot Z compensation angle θ1z, the second foot X compensation angle θ2x, the second foot Y compensation angle θ2y, and the second foot Z compensation angle θ2z, the above-mentioned Z axis compensation is performed. The corrected desired foot position / posture is obtained by correcting the desired foot position / posture in accordance with the foot position / posture correction method of the composite compliance operation to which the motion is added.

  In the third embodiment, as described above, since compliance control for the Z component (component around the vertical axis) of the actual total floor reaction force moment is added, in addition to the operations and effects described in the previous embodiment, the Z axis It is possible to suppress the vibration of the Z component of the actual foot floor reaction force moment caused by the natural rotational vibration of the surroundings, and thus it is possible to more effectively realize the posture stabilization control of the legged mobile robot.

  Furthermore, when performing the route guidance control shown in FIG. 32, it is possible to guide along the target route with high accuracy.

  In the third embodiment, the yaw rate sensor 100, the self-position / attitude / direction estimator 102, the route guidance control calculation unit, etc. are not provided, and the Z component Mdmdz of the compensated total floor reaction force moment is simply set to zero or in the vicinity thereof. Even if it is fixed, it is quite effective as compliance control for the Z component of the actual total floor reaction force moment.

  In the first to third embodiments, as described above, at least the base body (upper body 24) is connected to the base body via the first joint (10, 12, 14R (L)), and at the tip thereof. Leg type consisting of a plurality of (two) legs (leg link 2) having a foot (foot 22R (L)) connected via a second joint (18, 20R (L)). In the control device for a mobile robot, 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 the total floor reaction force acting on the robot Gait generation means (gait generator, S10 to S22) for generating a gait of the robot including at least a target pattern (target total floor reaction force, target total floor reaction force central point (= target ZMP)), the generation The total floor reaction force of the gait made on each of the feet Target foot floor reaction force center point determining means (target floor reaction force distribution point) for determining a target foot floor reaction force center point (target foot floor reaction force center point) that is the action center point on the foot when placed , S24, S26), an actual floor reaction force detecting means for detecting the actual floor reaction force (actual foot floor reaction force) acting on the foot part (six-axis force sensor 44, actual foot floor reaction force detector, S32), calculating a moment (actual nth foot floor reaction force moment Mactx, y, z) at which the detected actual floor reaction force acts around the calculated desired foot floor reaction force center point; Foot rotation amount determining means (composite compliance operation determining unit) for determining a rotation amount (both leg compensation angle θdbv, z, nth foot compensation angle θnx, y, z) based on the calculated moment. , S32 to S34, S34a, both leg compensation angle determining unit, nth foot A compensation angle determination unit, S100 to S108 and S200 to S206), a foot that corrects the target position and / or posture so that the position and / or posture of the foot rotate based on the determined amount of foot rotation. Position / posture correction means (composite compliance operation determination unit, S38, S40, corrected target foot position / posture calculation unit), and the first and second of the robot based on the corrected position / posture of the foot Joint displacement means (robot geometric model (kinematics calculation unit), displacement controller, S40, S42) for displacing the two joints (10, 12, 14, 18, 20R (L)) is provided.

  In addition, at least the base body (upper body 24) is connected to the base body via a first joint (10, 12, 14R (L)), and a second joint (18, 20R (L)) is connected to the tip thereof. In the control device for the legged mobile robot 1 composed of a plurality of (two) legs (leg links 2) having legs (foot 22R (L)) connected via A motion pattern (target body position / posture, target foot position / posture) including at least the target position and posture of the foot, and a target pattern (target total floor reaction force, target) of the total floor reaction force acting on the robot Gait generation means (gait generators S10 to S22) for generating a gait of the robot including at least the total floor reaction force central point (= target ZMP), and the total floor reaction force of the generated gait. Action on the foot when distributed to each of the foot Target foot floor reaction force center point determining means (target floor reaction force distributor, S24) for determining a target foot floor reaction force center point (target foot floor reaction force center point) as a center point, acting on the foot Actual floor reaction force detecting means (6-axis force sensor 44, actual foot floor reaction force detector, S32) for detecting actual floor reaction force (actual foot floor reaction force), at least the detected actual floor reaction force Foot rotation amount determining means (composite compliance operation determining unit, S32, S34) for determining a rotation amount (both leg compensation angle θdbv, z, nth foot compensation angle θnx, y, z) based on , S34a, both leg compensation angle determination unit, nth foot compensation angle determination unit, S100 to S108 and S200 to S206), and the position and / or posture of the foot based on the determined foot rotation amount, The determined target foot floor reaction force center point or its vicinity Instead, the foot position / posture correcting means (composite compliance operation determining unit, S38, S40, corrected target foot position / posture calculating unit) for correcting the target position and / or posture so as to rotate, and the corrected Joint displacement means for displacing the first and second joints (10, 12, 14, 18, 20R (L)) of the robot based on the position / posture of the foot (robot geometric model (kinematic computing unit)) , A displacement controller, S42).

  Further, the foot position / posture correction means may determine whether the foot position and / or posture is based on the determined foot rotation amount or around the determined target foot floor reaction force center point. The target position and / or posture is corrected so as to rotate in the forward direction.

  Furthermore, the total floor reaction force moment actually acting on the robot (more precisely, moment component PQ1 * F1act + PQ2 * F2act + M1act + M2act), or the moment of the total floor reaction force actually acting on the robot (PQ1 * F1act + PQ2 Calculate one of the moments (Mf1f2act = PQ1 * F1act + PQ2 * F2act) obtained by subtracting the floor reaction force moment (M1act + M2act) acting on the foot from * F2act + M1act + M2act), and at least the calculated moment Foot movement amount determining means for determining the amount of movement (θdbv, z) for moving the foot according to (composite compliance operation determining unit, S34, S34a, both leg compensation angle determining unit, S100 to S108, S200 to S206) The foot position / posture correcting means includes a front position based on the determined foot rotation amount and the determined movement amount. And configured to modify the position and / or posture of the foot.

  Further, a posture stabilization 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 determination means and / or the foot movement amount determination means. Determines the amount of rotation and / or movement of the foot based on at least the detected actual floor reaction force (actual foot floor reaction force) and the calculated posture stabilization compensation total floor reaction force moment ( S34, S34a, S100 to S108, S200 to S206).

  The posture stabilization compensation total floor reaction force moment is determined based on at least the tilt deviation (θerrx, y) of the robot (S28, S30a).

  The posture stabilization compensating total floor reaction force moment is determined based on at least the yaw rate (θerrz, dθerrz / dt) of the robot (S28, S30a).

  The posture stabilization compensating total floor reaction force moment is determined based on at least a deviation from the target path of the robot, that is, a lateral deviation or a direction deviation h from the target trajectory (S28, S30a).

  The predetermined component (Mdmdz) in the posture stabilization compensation total floor reaction force moment (compensation total floor reaction force Mdmd) is set to zero or in the vicinity thereof.

  Further, the foot position / posture correcting means is configured to further correct the target position and / or posture based on the posture deviation of the robot.

  Further, the foot rotation amount determination means and / or the foot movement amount determination means may be configured so that the posture stabilization compensation total floor reaction force moment is distributed to each of the plurality of legs. The amount of rotation and / or the amount of movement is determined (S34, S34a, S100 to S108, S200 to S206).

  In addition, at least the base body (upper body 24) is connected to the base body via a first joint (10, 12, 14R (L)), and a second joint (18, 20R (L)) is connected to the tip thereof. In the control device for the legged mobile robot 1 composed of a plurality of (two) legs (leg links 2) having legs (foot 22R (L)) connected via A movement pattern (target body position / posture, target foot position / posture) including at least the target position and posture of the foot, and a target trajectory pattern (target total floor reaction force, acting on the robot) Gait generation means (gait generator, S10 to S22, S24) for generating a gait of the robot composed of a target total floor reaction force central point (= target ZMP), and compensation for posture stabilization of the robot Calculate total floor reaction force (compensated total floor reaction force Mdmd) Posture stabilization compensation total floor reaction force calculating means (posture stabilization control calculation unit, S30, S30a), foot floor reaction force detection for detecting actual floor reaction force (actual foot floor reaction force) acting on the foot. Means (6-axis force sensor 44, actual foot floor reaction force detector, S32), floor reaction force distribution means for distributing the total floor reaction force of the target gait and the compensation total floor reaction force (target floor reaction force distribution) S34, S34a, S100 to S104, S200 to S202), the target gait based on the total floor reaction force, the compensation floor reaction force and the detected actual foot reaction force of the distributed target gait. Correction means for correcting the position and / or posture of the foot (composite compliance operation determination unit, S36 to S38, compensation angle determination unit, mechanism deformation amount calculation unit, corrected target foot position / posture calculation unit, mechanism deformation compensation included Corrected target foot position / posture calculation unit), and the corrected Joint displacement control means (robot geometric model (kinematics) for controlling displacement of the first and second joints (10, 12, 14, 18, 20R (L)) of the robot based on the target foot position and posture. The calculation unit), the displacement controller, and S40 and S42) are provided.

  Further, the correcting means is configured to further correct the position and / or posture of the foot part of the target gait based on the posture deviation of the robot.

  In the first to third embodiments described above, if the method of determining the foot rotation center point in the compensation operation is further extended, the foot rotation center point in the compensation operation is determined as shown in FIG. Instead of the flat floor reaction force center point, another point in the sole ground contact area assumed at that moment may be set as the rotation center point.

  Examples of setting methods are listed below. Computation is complicated, but in some cases, even if the foot is rotated, only the actual foot floor reaction force moment changes and does not interfere with the force component of the actual foot floor reaction force. Can be. However, care must be taken so that the trajectory of the foot rotation center point does not become discontinuous in any method. This is because if it is discontinuous, the compensation operation changes abruptly and the foot flutters.

  Method 1) From each compensation moment and the force component of each foot desired floor reaction force, find the position where each actual foot floor reaction force central point should be when each compensation moment is generated, and use this to find the corrected desired foot floor Called reaction force center point. However, the correction target foot floor reaction force center point is set so as not to exceed the assumed ground contact area at that moment. Revision target Each foot floor reaction force center point or its vicinity is set as the rotation center point.

  Method 2) The area center point of the assumed sole contact area is obtained, and the point or the vicinity thereof is set as the rotation center point.

  Method 3) The actual foot floor reaction force center point is obtained from the actual foot floor reaction force, and the point or the vicinity thereof is set as the rotation center point.

  Method 4) A plurality of candidates are selected from the various rotation center point candidates listed in methods 1 to 3, the desired foot floor reaction force center point, and the like, and the weighted average point is set as the rotation center point.

  In the above description, unless a negative pressure is generated in a part of the pressure distribution in the sole ground contact area, that is, unless an adhesive force is generated, the actual foot floor reaction force center point always exists in the sole ground contact area.

  In the first to third embodiments described above, in FIG. 4, the horizontal position of the target body position / posture trajectory is used instead of inputting the target body position / posture trajectory directly to the robot geometric model. The body height is corrected by recalculating the body height from the corrected target foot position / posture trajectory using the body height determination method previously proposed by the present applicant in Japanese Patent Application No. 8-214260, and this is corrected. You may enter into a geometric model.

  If the target foot position / posture trajectory is significantly corrected, there is a risk that the legs will be stretched and the posture cannot be taken with the original upper body height. In such a case, if the above recalculation is performed, there is no possibility that the leg will be extended unless there is a special case.

  Further, in the first to third embodiments described above, when the upper body tilts, the position / posture of the actual foot with respect to the floor shifts, and as a result, the actual foot floor reaction force deviates from the target square foot floor reaction force. . In order to cancel this shift, the position / posture shift of the actual foot caused by the tilting of the upper body may be canceled by correcting the both leg compensation angles θdbv and the foot compensation angles θnx and θny.

Specifically, correction is performed using the X component θerrx and Y component θerror of the body tilt deviation and the vector V as follows. That is, a value obtained by adding the following equation Δθdbv to the both-leg compensation angle θdbv obtained by the above-described both-leg compensation angle determination method is referred to as a both-leg compensation angle θdbv.
Δθdbv =-(θerrx * Vx + θerry * Vy)

  The first foot X compensation angle θ1x and the second foot X compensation angle θ1x obtained by subtracting θerrx from the first foot X compensation angle θ1x and the second foot X compensation angle θ2x obtained by the respective foot compensation angle determination methods described above. The foot X compensation angle θ2x.

  Similarly, the first foot Y compensation angle θ1y and the second foot Y compensation angle θ1y obtained by subtracting θerry from the first foot Y compensation angle θ1y and the second foot Y compensation angle θ2y obtained by the respective foot compensation angle determination methods. The foot Y compensation angle θ2y is assumed.

  In the first to third embodiments described above, when it is not necessary to increase the control accuracy, the both-leg mechanism deformation compensation angle θffdbv may be zero. That is, the calculation of the mechanism deformation compensation angle may be omitted.

  In the first to third embodiments described above, when it is not necessary to increase the control accuracy, the first foot X mechanism deformation compensation angle θff1x may be zero. That is, the calculation of the mechanism deformation compensation angle may be omitted.

  In the first to third embodiments described above, the distribution weight variable is determined in accordance with the timing of the target gait, so the processing is simple. However, when the actual floor surface condition is significantly different from the floor surface assumed by the target gait, the landing timing may be shifted, so that the increase amount of the actual floor reaction force may be largely deviated from Mdmd.

  In order to enhance the robustness against unexpected changes in the floor surface condition, the moment of landing and leaving the floor may be detected from the force component of the actual floor reaction force, and the distribution weight variable may be changed using this as a trigger.

  Estimate the ground contact state of each foot from the actual foot floor reaction force (for example, whether each foot actual floor reaction force center point is out of the desired ground contact area). The value of the distribution weight variable may be changed as appropriate in consideration of the actual foot floor reaction force, such as reducing the moment by lowering the moment.

  In the first to third embodiments described above, the inverse kinematics solution equation is obtained directly, and each joint displacement can be determined by simply substituting the body position / posture and the foot position / posture into the equation. To get. In these embodiments, there is a solution, but depending on the joint arrangement, there may be no direct solution.

  At that time, using the inverse Jacobian or pseudo-inverse Jacobian that expresses the relative position of the foot relative to the body position / posture and the ratio of the joint perturbation to the perturbation of the posture in the form of a matrix, each joint displacement is obtained approximately. May be. This technique is often used for ordinary industrial robots. Even if the above-described method cannot be used, this method can approximate the solution.

  Further, in the first to third embodiments described above, the spring mechanism 32 (and the sole elastic body 34) itself is not an essential part of the present invention. The essence of the present invention resides in the feedback control portion, and the mechanism deformation compensation is incidental.

  In the first to third embodiments described above, the block diagram may be equivalently modified such as changing the processing order.

  Further, in the first to third embodiments described above, it is assumed that the target gait receives a reaction force (target object reaction force) other than the floor reaction force from the environment, as described above, and the target ZMP. The inertial force generated by the target motion pattern, the resultant force of gravity and the target object reaction force is obtained dynamically, and the moment that acts on a certain point on the floor surface excludes the component around the vertical axis. If it is zero, that point may be set as the target ZMP again.

  In addition, in the Japanese Patent Application Laid-Open No. 5-337849, the present applicant corrects only the target motion pattern while leaving the target total floor reaction force center point as it is, We have proposed a method that restores the tilt of the posture by causing the point to shift. When this method is used in combination, the target total floor reaction force central point does not coincide with the target ZMP.

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

It is explanatory drawing which shows the control apparatus of the leg type mobile robot which concerns on this invention generally. It is explanatory side view which shows the structure of the leg | foot part of the biped walking robot shown in FIG. It is a block diagram which shows the detail of the control unit of the biped walking robot shown in FIG. 1 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. It is explanatory drawing which shows an example of the exercise | movement pattern when the leg type mobile robot shown in FIG. 1 walks on a flat ground. It is explanatory drawing which shows the locus | trajectory on a floor surface of the target all floor reaction force center point (target ZMP) locus | trajectory corresponding to the motion pattern of FIG. FIG. 6 is a time chart of a target total floor reaction force central point (target ZMP) locus corresponding to the motion pattern of FIG. 5. 6 is a time chart of a target first foot floor reaction force central point locus set to satisfy a predetermined condition corresponding to the motion pattern of FIG. 6 is a time chart of a target second foot floor reaction force central point locus set to satisfy a predetermined condition corresponding to the motion pattern of FIG. 5. FIG. 5 is a flowchart showing the operation of the control device for the legged mobile robot according to the present invention, as in FIG. 4. In order to explain the operation of the composite compliance operation determining unit shown in FIG. 4 that performs the calculation processing such as the compensation angle of both legs in the flowchart of FIG. 10, the first foot and the second foot are actually used in the both-leg support period. It is explanatory drawing which shows the condition where each foot floor reaction force is acting. It is explanatory drawing which shows the setting of the target total floor reaction force in the situation shown in FIG. It is explanatory drawing which shows distribution of each desired foot floor reaction force in the condition shown in FIG. It is explanatory drawing which shows the compensation total floor reaction force moment in the condition shown in FIG. It is explanatory drawing which shows the normal vector V of the plane perpendicular | vertical to a horizontal surface including each foot floor reaction force center point in the condition shown in FIG. FIG. 12 is an explanatory diagram showing a state when the target foot floor reaction force center point is rotated by a predetermined angle θdbv around the target total floor reaction force center point (target ZMP) in the situation shown in FIG. 11. FIG. 12 is an explanatory diagram showing a state when each foot is rotated by a predetermined angle θnx, θny around the front-rear direction axis and the left-right direction axis in the situation shown in FIG. It is a block diagram which shows the arithmetic processing of the composite compliance operation | movement determination part of FIG. It is a block diagram which shows the arithmetic processing of the compensation total floor reaction force moment distributor shown in FIG. It is a time chart which shows the example of a setting of the distribution weight variable for operating the both-legs compensation angle etc. of the compensation total floor reaction force moment distributor shown in FIG. It is explanatory drawing which shows the attitude | position of the robot for demonstrating the setting of the distribution weight variable of the compensation total floor reaction force moment distributor of FIG. FIG. 22 is an explanatory diagram showing the posture of the robot for explaining the setting of the distribution weight variable of the compensated total floor reaction force moment distributor as in FIG. 21. It is explanatory drawing which shows both legs compensation moment V direction component Mdmdbv when the distribution weight for operating both legs compensation angles is determined on the predetermined conditions. It is a block diagram which shows the arithmetic processing of the both leg compensation angle determination part shown in FIG. It is a block diagram which shows the arithmetic processing of the compensation angle determination part of each foot shown in FIG. It is explanatory drawing for demonstrating the arithmetic processing of the correction | amendment target foot position / posture calculation part with mechanism deformation compensation shown in FIG. FIG. 27 is an explanatory diagram for explaining the calculation processing of the corrected desired foot position / posture calculation unit with mechanism deformation compensation shown in FIG. 18, similarly to FIG. 26. FIG. 10 is a subroutine flow chart showing determination work such as a compensation angle for both legs in the flowchart of FIG. 10. It is explanatory drawing similar to FIG. 16 shown in the 2nd Example of this invention, and is explanatory drawing which shows another example of correction operation | movement of a foot position. It is explanatory drawing similar to FIG. 14 which shows the apparatus which concerns on a 3rd Example. FIG. 11 is a flow chart similar to FIG. 10, showing an apparatus according to a third embodiment. It is explanatory top view which shows the route guidance which the apparatus concerning a 3rd Example schedules. It is explanatory drawing similar to FIG. 16 which shows operation | movement of the apparatus which concerns on a 3rd Example. It is explanatory drawing similar to FIG. 17 which shows operation | movement of the apparatus which concerns on a 3rd Example. It is a block diagram similar to FIG. 18 which shows the arithmetic processing of the composite compliance operation | movement part of the apparatus which concerns on a 3rd Example. FIG. 36 is a block diagram illustrating a calculation process of a Z component compensation moment determination unit in FIG. 35. It is a block diagram which shows the arithmetic processing of the compensation total floor reaction force moment distributor shown in FIG. It is a time chart which shows the example of a setting of the distribution weight variable of the compensation total floor reaction force moment distributor shown in FIG. 31 is a subroutine flow chart showing determination work such as a both-legs compensation angle around the Z axis in the flowchart of FIG. 31. It is explanatory drawing when a biped walking robot walks on the inclined surface which was not anticipated. It is explanatory drawing at the time of performing the both-legs compliance control previously proposed with respect to the biped walking robot shown in FIG. It is explanatory drawing when a bipedal walking robot similar to FIG. 40 steps on the protrusion which was not anticipated. It is explanatory drawing when the ankle compliance control proposed previously is performed in the situation shown in FIG.

Explanation of symbols

1 Biped 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 Tilt Sensor

Claims (6)

Control of a legged mobile robot comprising at least a base and a plurality of legs each having a foot connected to the base via a first joint and a foot connected to the tip via a second joint In the device
a. Gait generating means for generating a target gait of the robot comprising a movement pattern including at least a target position or posture of the foot of the robot and a target locus pattern of a total floor reaction force acting on the robot;
b. Posture stabilization compensation total floor reaction force determining means for determining a compensation total floor reaction force for posture stabilization of the robot;
c. A foot actual floor reaction force detecting means for detecting a foot actual floor reaction force acting on the foot,
d. Floor reaction force distribution means for distributing the total floor reaction force of the target gait and the compensation total floor reaction force to at least each of the feet,
e. Correction means for correcting the position or posture of the foot part of the desired gait based on the total floor reaction force, the compensated total floor reaction force of the distributed target gait and the detected actual foot reaction force of the foot,
And f. Joint displacement control means for controlling the displacement of the first and second joints based on the position or posture of the foot of the corrected desired gait;
A control device for a legged mobile robot, comprising:   2. The legged mobile robot according to claim 1, wherein the posture stabilization compensation total floor reaction force determining unit obtains the posture stabilization compensation total floor reaction force based on at least a tilt deviation of the robot. 3. Control device.   3. The legged movement according to claim 1, wherein the posture stabilization compensating total floor reaction force determining unit obtains the posture stabilization compensating total floor reaction force based on at least a yaw rate of the robot. Robot control device.   4. The posture stabilization compensating total floor reaction force determining means obtains the posture stabilization compensating total floor reaction force based on at least a deviation from a target path of the robot. The control apparatus of the leg type mobile robot in any one.   5. The posture stabilization compensating total floor reaction force determining unit sets a predetermined component in the posture stabilization compensating total floor reaction force to zero or in the vicinity thereof. A control device for a legged mobile robot according to claim 1. The legged mobile robot according to any one of claims 1 to 5, wherein the correcting means further corrects the position or posture of the foot of the target gait based on the posture deviation of the robot. Control device.

Images