JP5232120B2 - Control device for moving body - Google Patents

Description

  The present invention relates to a control device for a moving body such as a legged mobile robot.

  For example, a legged mobile robot is generally known as a moving body including a base and a moving mechanism that moves the base on the floor surface. In the legged mobile robot, the leg corresponds to the moving mechanism, and the upper body corresponds to the base.

  For example, in a legged mobile robot, a technique disclosed in Patent Document 1 or the like has been previously proposed by the applicant of the present application as a technique for improving the stability of the posture of the robot. In this technology, the state quantity deviation of the actual posture of the robot's upper body with respect to the target posture is observed, and the feedback request operation amount (control input) required to converge the state quantity deviation to “0” is used as the robot. The operation moment (operation moment around the target ZMP) to be additionally acted on is calculated from the observed value of the state quantity deviation by the PD law (proportional / derivative law). Furthermore, this operation moment is distributed to the actual robot side operation moment that acts directly on the actual robot and the model side operation moment that acts on the robot in the dynamic model for generating the target gait of the robot. . In an actual robot, the position / posture of the foot of both legs of the actual robot is changed to the position / posture of the foot in the target gait by compliance operation control so as to generate an actual robot side operation moment around the target ZMP. Corrected from posture. In the dynamic model, the target gait of the robot is generated so as to generate a model side operation moment around the target ZMP.

  Thus, when the actual posture of the upper body of the robot deviates from the target posture due to disturbance or the like, a floor reaction force moment that cancels the deviation is applied to the robot.

PCT International Publication No. WO2003 / 061917

  By the way, in the technique for determining the required operation amount (operation moment) for converging the state quantity deviation to “0” by the PD rule as seen in Patent Document 1, the gain coefficient applied to the observed value of the state quantity deviation is The gain coefficient related to the temporal change rate (differential value) of the observed value of the state quantity deviation is usually set to a predetermined fixed value.

  In this case, according to the knowledge of the inventor of the present application, when the robot is moved in a gait (for example, a walking gait) that keeps the height of the upper body of the robot (position in the vertical direction) substantially constant. Using the operation moment determined by the PD rule as described above, the posture of the upper body of the robot can be smoothly restored to the target posture.

  However, in the case of a gait accompanied by a vertical motion of the upper body, such as when the robot is running, the robot's operating moment is determined by the operation moment determined by the PD rule with a fixed gain coefficient as described above. When feedback control is performed on the posture of the upper body, the actual posture of the upper body of the robot becomes the target posture due to the influence of the inertial force generated on the upper body (and thus the robot) due to the vertical motion of the upper body. On the other hand, it has been found that there are many cases where overshoot and undershoot occur.

  Therefore, the present invention provides an actual posture with respect to the target posture of the base body while suppressing the occurrence of overshoot and undershoot when moving a moving body such as a legged mobile robot while moving the base body up and down. To provide a control device for a moving body that can determine a required operation amount that can smoothly eliminate the deviation of the movement, and can smoothly eliminate the deviation of the posture of the base according to the requested operation amount. Objective.

According to a first aspect of the mobile body control device of the present invention, a mobile body including a base and a moving mechanism for moving the base on the floor is controlled so as to move while performing the vertical movement of the base. A control device for
Vertical inertial force parameter determining means for determining a vertical inertial force parameter that is a parameter that defines a time series of the target inertial force in the vertical direction of the movable body or the base;
Target motion determining means for determining the time series of the target motion of the moving body using at least the vertical inertia force parameter so as to satisfy the time series of the target inertia force defined by the determined vertical inertia force parameter. When,
Operation control means for performing operation control of the moving body according to at least the determined target motion;
State quantity deviation observing means for sequentially observing a state quantity deviation representing the degree of deviation of the actual posture of the base body from the target posture of the base body in the determined target motion;
A required operation amount determining means for sequentially determining a required operation amount that defines an additional external force to be additionally applied to the moving body in order to converge the state quantity deviation to “0”;
The operation control means operates the mobile body so that an additional external force defined by the required operation amount is additionally applied to the mobile body while causing the actual motion of the mobile body to follow the target motion. A means of controlling,
The requested operation amount determining means is configured such that, at each time when the requested operation amount is determined, the requested operation amount to be determined is a current time in a time series of target inertia force defined by the vertical inertia force parameter. An inertial force-dependent manipulated variable component that changes according to the instantaneous value of the target inertial force and the observed value of the state quantity deviation, and changes according to the observed value of the state quantity deviation without depending on the target inertial force The requested manipulated variable is determined using the vertical inertial force parameter and the observed value of the state quantity deviation so as to be a value obtained by combining an inertial force-independent manipulated variable component. (First invention).

According to a second aspect of the control device for a moving body of the present invention, a moving body including a base and a moving mechanism for moving the base on the floor is moved while moving the base up and down. A control device for controlling
Vertical inertial force parameter determining means for determining a vertical inertial force parameter that is a parameter that defines a time series of the target inertial force in the vertical direction of the movable body or the base;
A time series of at least a dynamic model preset as representing dynamics of the moving body, the determined vertical inertia force parameter, and a target external force to be applied to the moving body on the dynamic model A target motion determining means for determining a time series of the target motion of the moving body using
Operation control means for performing operation control of the moving body according to at least the determined target motion;
State quantity deviation observing means for sequentially observing a state quantity deviation representing the degree of deviation of the actual posture of the base body from the target posture of the base body in the determined target motion;
A required operation amount determining means for sequentially determining a required operation amount that defines an additional external force to be additionally applied to the moving body in order to converge the state quantity deviation to “0”;
The target motion determining means determines a model operation external force to be added to the target external force on the dynamic model according to the determined required operation amount, and adds the determined model operation external force to the target external force. The target force is applied to the moving body on the dynamic model while satisfying the target inertia force time series defined by the determined vertical inertia force parameter on the dynamic model. A means of determining exercise,
The requested operation amount determining means is configured such that, at each time when the requested operation amount is determined, the requested operation amount to be determined is a current time in a time series of target inertia force defined by the vertical inertia force parameter. An inertial force-dependent manipulated variable component that changes according to the instantaneous value of the target inertial force and the observed value of the state quantity deviation, and changes according to the observed value of the state quantity deviation without depending on the target inertial force The requested manipulated variable is determined using the vertical inertial force parameter and the observed value of the state quantity deviation so as to be a value obtained by combining an inertial force-independent manipulated variable component. (Second invention).

  In the present specification, “floor” does not mean a floor in a normal sense (such as an indoor floor) but also includes an outdoor ground or road surface.

  In the first and second aspects of the invention, the required manipulated variable determining means is a target inertia in which the determined requested manipulated variable is defined by the vertical inertia force parameter at each time when the requested manipulated variable is determined. An inertial force-dependent manipulated variable component that changes according to the instantaneous value of the target inertial force at the current time in the time series of the force and the observed value of the state quantity deviation, and without depending on the target inertial force, The vertical direction inertial force parameter and the observed value of the state quantity deviation are used to obtain a value obtained by synthesizing the inertial force-independent manipulated variable component that changes according to the observed value of the state quantity deviation. Determine the requested operation amount. For this reason, the requested operation amount determining means is configured to take into account the influence of the instantaneous value of the inertial force in the vertical direction of the moving body or the substrate accompanying the vertical movement of the substrate at the current time on the posture of the substrate. The amount can be determined. In other words, when it is assumed that the actual inertial force in the vertical direction of the moving body or the base body matches the target inertial force, the state quantity deviation is smoothly set to “0” regardless of the inertial force. The required operation amount can be determined so as to be converged (so as to suppress the occurrence of overshoot and undershoot during convergence).

  Therefore, according to the first and second inventions, it is possible to determine the required operation amount that can smoothly eliminate the deviation of the actual posture of the base body from the target posture while suppressing the occurrence of overshoot and undershoot. .

  In the first aspect of the invention, the requested operation amount determined in this way is given to the operation control means. At this time, the movement control means causes the additional external force defined by the required operation amount to act on the moving body in addition to causing the actual movement of the moving body to follow the target movement. Perform body movement control. That is, the motion control means causes the mobile body to additionally act on the mobile body with an additional external force defined by the requested operation amount while causing the actual motion of the mobile body to follow the target motion. Drive the actuator. Thereby, an external force that causes the state quantity deviation to approach “0” is additionally applied to the actual moving body, and the deviation of the actual posture (actual posture) from the target posture of the base body can be smoothly eliminated. .

  In the second invention, the requested operation amount determined as described above is given to the target motion determining means. At this time, the target motion determining means determines a model operation external force to be added to the target external force on the dynamic model according to the determined required operation amount, and uses the determined model operation external force as the target external force. The time series of the target inertia force defined by the determined vertical inertia force parameter is satisfied on the dynamic model while the external force added to is applied to the moving body on the dynamic model. The target motion is determined. The motion control means controls the motion of the moving body according to at least the determined target motion. That is, the actuator of the moving body is driven so that the actual movement of the moving body follows the determined target movement. Thereby, an external force that causes the state quantity deviation to approach “0” is additionally applied to the moving body, and the deviation of the actual posture (actual posture) from the target posture of the base can be smoothly eliminated.

  In the first and second inventions, the state quantity deviation is, for example, an angle difference between the target posture of the base body and the actual posture, a value obtained by multiplying the angle difference by a predetermined value, or a target motion. For example, the horizontal deviation between the position of the center of gravity of the base and the position of the center of gravity of the actual base may be mentioned.

  In addition, the “vertical direction” in the first and second inventions may be basically the vertical direction (gravity direction), but may not be the vertical direction in a strict sense. For example, when the floor surface is an inclined surface slightly inclined with respect to the horizontal direction, the direction perpendicular to the floor surface may be regarded as the “vertical direction”.

  In addition, as the required operation amount in the first and second inventions, for example, a target value of an external force (translational force or force moment such as floor reaction force or floor reaction force moment) applied to the moving body, the external force The target value of the action point or the correction amount of the target value can be used.

  As an example of the model operation external force in the second invention, for example, an external force obtained by inverting the sign of the additional external force defined by the required operation amount can be used.

  Further, the requested operation amount may be distributed to both the motion control means and the target motion determination means. In this case, for example, in the second aspect of the invention, the target motion determining means may use the determined requested operation amount to cause the model operating external force and the actual moving body operating external force to actually act on the moving body. The movement control means is configured to cause the actual moving body operating external force to additionally act on the moving body while causing the actual movement of the moving body to follow the target movement. Operation control of the moving body is performed (third invention).

  According to the third aspect of the invention, the target motion determination unit and the operation control unit cooperate to function so as to converge the state quantity deviation to “0”.

  In the third invention, the model operating external force and the actual moving body operating external force may be determined so that the difference between the model operating external force and the actual moving body operating external force changes monotonously with respect to the requested operation amount. desirable. In order to do this, for example, a value obtained by subtracting the model operation external force from the actual moving body operation external force matches the additional external force defined by the required operation amount.

  In the first to third aspects of the invention, more specifically, the requested operation amount determined by the requested operation amount determining means is, for example, a first value of a predetermined value set in advance to the observed value of the state quantity deviation. The inertial force-independent manipulated variable component including at least a value obtained by multiplying a coefficient and the second coefficient that is a function value of the instantaneous value of the target inertial force at the current time is multiplied by the observed value of the state quantity deviation. A value obtained by adding the inertial force-dependent manipulated variable component including at least the value can be employed (fourth invention).

  According to the fourth aspect of the invention, the required operation capable of converging the state quantity deviation to “0” while appropriately compensating for the influence of the vertical inertial force of the moving body or the base on the posture of the base. The amount can be determined by simple calculation.

  In the fourth aspect of the invention, the inertial force dependent manipulated variable component includes a value obtained by multiplying the observed value of the state quantity deviation by a first coefficient of a predetermined value set in advance, and a time of the observed value of the state quantity deviation. The inertial force-independent manipulated variable component is obtained by multiplying the observed value of the state quantity deviation by the inertial force-independent manipulated variable component. It is preferable to be composed of values (fifth invention).

  According to the fifth aspect of the invention, the inertial force-independent manipulated variable component is an manipulated variable component that functions to converge the state variable deviation to “0” by the proportional / derivative law (PD law). Responsiveness of control of state quantity deviation can be improved. Further, when the state quantity deviation converges to “0”, the inertial force dependent manipulated variable component becomes “0”, so that the inertial force dependent manipulated variable component reduces the state quantity deviation from “0”. It is possible to prevent the function from being shifted.

  In the fourth invention or the fifth invention, the required operation amount may be a value obtained by adding the inertial force-independent operation amount component and the inertial force-dependent operation amount component as a result, In the process for determining the required manipulated variable, it is not necessary to treat the inertial force-independent manipulated variable component and the inertial force dependent manipulated variable component separately. For example, the process executed by the requested operation amount determination means to determine the requested operation amount is a combination coefficient obtained by adding the first coefficient and the second coefficient to the observed value of the state quantity deviation. You may make it include the process which multiplies (6th invention).

  In this case, the value obtained by multiplying the observed value of the state quantity deviation by the composite coefficient results in a value obtained by multiplying the observed value of the state quantity deviation by the first coefficient, A value obtained by multiplying the observed value of the state quantity deviation by the second coefficient is added.

  When the sixth aspect of the invention is combined with the fifth aspect of the invention, a value obtained by multiplying the observed value of the state quantity deviation by the composite coefficient, and a time of the observed value of the state quantity deviation by the third coefficient. The required operation amount is determined by adding the value obtained by multiplying the target change rate.

  In the fourth to sixth aspects of the invention, the second coefficient that is a function value of the instantaneous value of the target inertial force at the current time is, for example, a value proportional to the instantaneous value of the target inertial force (the target inertial force The value obtained by multiplying the instantaneous value by a proportional constant of a predetermined value) can be employed.

  In the first to sixth inventions, the inertial force-independent manipulated variable component converges the state quantity deviation to “0” when it is assumed that the target inertial force is maintained at “0”. An operation amount component that defines the additional external force necessary to cause the inertial force-dependent operation amount component to cancel the additional external force generated due to the instantaneous value of the target inertia force at the current time Therefore, it is preferable that the operation amount component is necessary for this purpose (seventh invention).

  As the required operation amount that can converge the state quantity deviation to “0” while appropriately compensating for the influence of the vertical inertial force of the movable body or the substrate on the posture of the substrate, a suitable operation amount is Can be determined.

The perspective view which shows schematic structure of the bipedal mobile robot as a moving body in one Embodiment of this invention. The block diagram which shows the hardware constitutions of the control unit with which the robot of FIG. 1 was equipped. The block diagram which shows the functional structure of a control unit. The block diagram which shows the process of the compensation whole floor reaction force moment distributor shown in FIG. The figure which shows the running gait of the robot of FIG. The graph which shows the example of the desired floor reaction force vertical component track | orbit produced | generated with the gait generator shown in FIG. FIGS. 7A and 7B are graphs showing examples of target floor reaction force vertical component trajectories (X-axis direction component and Y-axis direction component) generated by the gait generator shown in FIG. 3. The figure which shows visually the dynamic model of the robot used by the process of the gait generator shown in FIG. 3, and an attitude | position stabilization control calculating part. The flowchart which shows the main routine process which the gait generator shown in FIG. 3 performs. The figure which shows visually the divergence state of the attitude | position of the robot shown in FIG. 10 is a flowchart showing subroutine processing of S022 of FIG. The figure which illustrates the form of movement of the foot of the robot shown in FIG. 1, and a support leg coordinate system. The graph which illustrates the desired floor reaction force vertical component trajectory in the normal turning gait of the robot shown in FIG. The graph which illustrates the target ZMP trajectory (X-axis direction component) in the normal turning gait of the robot shown in FIG. 10 is a flowchart showing subroutine processing of S024 in FIG. The flowchart which shows the subroutine processing of S208 of FIG. 10 is a flowchart showing subroutine processing of S026 of FIG. 10 is a flowchart showing subroutine processing of S028 of FIG. The flowchart which shows the subroutine processing of S702 of FIG. The graph for demonstrating the process of S710 of FIG. The figure for demonstrating the process of S030 of FIG. 10 is a flowchart showing a subroutine process of S032 of FIG. The graph which illustrates the time-dependent change of the value of the gain Kx (synthesis coefficient) and Kv (3rd coefficient) for determining a request | requirement operation amount by the process of the attitude | position stabilization control calculating part shown in FIG. The graph which illustrates the desired floor reaction force vertical component trajectory in the walking gait of the robot shown in FIG. FIGS. 25A and 25B are diagrams schematically showing another example of a moving object to which the present invention is applied.

  Hereinafter, an embodiment of the present invention will be described taking a bipedal mobile robot as an example of the moving body.

  As shown in FIG. 1, a bipedal mobile robot 1 (hereinafter simply referred to as a robot 1) according to the present embodiment includes an upper body 24 as a base body, and the upper body as a moving mechanism for moving the upper body 24 on the floor surface. 24 and a pair of left and right legs (leg link) 2 and 2 interposed between the floor and the floor.

  The upper body 24 is connected to the base end portions (upper end portions) of both the leg bodies 2 and 2 via a hip joint (hip joint) described later, and the floor 24 is connected to the floor by the grounded leg body of the both leg bodies 2 and 2. Supported upward.

  Both legs 2 and 2 have the same structure, and each has six joints. The six joints are, in order from the upper body 24 side, joints 10R and 10L for rotating the hip (crotch) (for rotating in the yaw direction with respect to the upper body 24), and the roll direction of the hip (crotch) (around the X axis). Joints 12R, 12L for rotation, joints 14R, 14L for rotation in the hip (crotch) pitch direction (around the Y axis), joints 16R, 16L for rotation in the pitch direction of the knee, It consists of joints 18R, 18L for rotation in the pitch direction and joints 20R, 20L for rotation in the roll direction of the ankle part.

  In the description of this embodiment, the symbols R and L mean that they correspond to the right leg and the left leg, respectively. Further, the X axis, the Y axis, and the Z axis mean three coordinate axes of a support leg coordinate system to be described later. In this support leg coordinate system, the X-axis direction and the Y-axis direction are two axis directions orthogonal to each other on the horizontal plane, the X-axis direction is the front-rear direction (roll axis direction) of the robot 1, and the Y-axis direction is the left-right direction of the robot This corresponds to the direction (pitch axis direction). The Z-axis direction is the vertical direction (gravity direction) and corresponds to the vertical direction (yaw axis direction) of the robot 1. In this case, in the present embodiment, the vertical direction that is the Z-axis direction has the meaning of the vertical direction in the present invention.

  The joints 10R (L), 12R (L), and 14R (L) of each leg 2 constitute a three-degree-of-freedom hip joint (hip joint), and the joint 16R (L) constitutes a one-degree-of-freedom knee joint, The joints 18R (L) and 20R (L) constitute an ankle joint with two degrees of freedom.

  The waist joints (hip joints) 10R (L), 12R (L), 14R (L) and the knee joint 16R (L) are connected by a thigh link 32R (L), and the knee joint 16R (L) and the ankle joint 18R. (L) and 20R (L) are connected by a crus link 34R (L). In addition, a foot 22R (L) that constitutes the tip (lower end) of each leg 2 is attached to the lower part of the ankle joints 18R (L) and 20R (L) of each leg 2. Moreover, the upper end part (base end part) of each leg 2 is connected to the upper body 24 via the hip joints (hip joints) 10R (L), 12R (L), 14R (L).

  Each of the joints described above may have a known structure proposed by the applicant of the present invention in Japanese Patent Application Laid-Open No. 3-184787. In this case, the actuator that rotationally drives each joint is configured by an electric motor 42 (see FIG. 2) that includes a speed reducer.

  With the above-described configuration of each leg 2, the foot 22 </ b> R (L) of each leg 2 has six degrees of freedom with respect to the upper body 24. When the robot 1 moves, the joints of both legs 2 and 2 are 6 * 2 = 12 joints (in this specification, “*” indicates multiplication in the operation for the scalar and outer product in the operation for the vector). By driving each at an appropriate angle, it is possible to perform a desired movement of both feet 22R and 22L. Thereby, the robot 1 can perform a movement that moves in a three-dimensional space, such as a walking action and a running action.

  In this case, both legs 2 and 2 function as a moving mechanism for moving the upper body 24 as a base on the floor surface. Further, the upper body 24 as a base is supported by the legs 2 and 2 so as to be able to perform relative movement with respect to the legs 2 and 2 via the hip joint (hip joint). Furthermore, it is possible to control the movement of the upper body 24 relative to the floor surface by controlling the joints of the legs 2 and 2. In addition to the movement of the upper body 24 in the horizontal direction (or the direction parallel to the floor surface), the movement of the upper body 24 includes the movement in the vertical direction (the vertical direction, the direction perpendicular to the floor surface, etc.) An exercise that changes the posture of the body 24 is also included.

  Although illustration is omitted, in the present embodiment, a pair of left and right arms are attached to both sides of the upper portion of the upper body 24, and a head is mounted on the upper end portion of the upper body 24. Each arm body can perform a motion such as swinging the arm body back and forth with respect to the upper body 24 by a plurality of joints (shoulder joint, elbow joint, wrist joint, etc.) provided therein. . However, these arms and heads may be omitted.

  A control unit 26 that controls the operation of the robot 1 is stored in the upper body 24. In FIG. 1, for convenience of illustration, the control unit 26 is illustrated outside the upper body 24.

  A six-axis force sensor 36 is interposed between the ankle joints 18R (L), 20R (L) and the foot 22R (L) of each leg 2. The six-axis force sensor 36 detects the translational force component in the three-axis direction and the moment component around the three axes of the floor reaction force transmitted from the floor to each leg 2 via the foot 22R (L). A detection signal is output to the control unit 26.

  The body 24 is equipped with an inclination sensor 40 for measuring the inclination angle (inclination angle in the roll direction and the pitch direction) of the body 24 with respect to the vertical direction (gravity direction) and the rate of change (angular velocity) thereof. . More specifically, the tilt sensor 40 includes an acceleration sensor and a rate sensor (angular velocity sensor) such as a gyro sensor, and outputs detection signals of these sensors to the control unit 26. Then, in the control unit 26, based on the output of the tilt sensor 40, the tilt angle and angular velocity of the body 24 with respect to the vertical direction are measured by a known method.

  In addition, an electric motor 42 (see FIG. 2) that rotationally drives each joint is provided with an encoder (rotary encoder) 44 (see FIG. 2) for detecting the rotation angle of each joint. Is output to the control unit 26.

  Referring to FIG. 2, the control unit 26 is composed of an electronic circuit unit having a microcomputer, and includes a first arithmetic unit 50 and a second arithmetic unit 52 comprising an CPU, an A / D converter 54, a counter 56, D / A converter 58, RAM 60, ROM 62, and bus line 64 for exchanging data between them.

  In this control unit 26, the outputs of the six-axis force sensor 36 and the tilt sensor 40 are converted into digital values by the A / D converter 54 and then input to the RAM 60 via the bus line 64. Further, the output of the encoder (rotary encoder) 44 of each joint of the robot 1 is input to the RAM 60 via the counter 56.

  The first arithmetic device 50 generates a target gait described later, calculates a joint displacement command (target value of the rotation angle of each joint), and sends it to the RAM 60. Further, the second arithmetic unit 52 reads out the joint displacement command from the RAM 60 and the actual joint displacement (actual measured value of the rotation angle of each joint) measured from the output of the encoder 44 via the counter 56, and the actual joint A drive command for the electric motor 42 of each joint necessary for causing the displacement to follow the joint displacement command (command value that defines the output torque of the electric motor 42) is calculated.

  Then, the second arithmetic unit 52 outputs the calculated drive command to the servo amplifier 46 for driving the electric motor 42 via the D / A converter 58. At this time, the servo amplifier 46 drives the electric motor 42 according to the input drive command (energizes the electric motor 42). Thereby, the actual joint displacement of each joint is controlled to follow the joint displacement command.

  Next, an outline of the operation of the control device for the robot 1 in the present embodiment will be described with reference to FIG. Portions other than the “real robot” in FIG. 3 are functions realized by processing executed by the control unit 26 (mainly processing of the first arithmetic device 60 and the second arithmetic device 62).

  In FIG. 3, for the sake of convenience, actual values (actual joint displacement, etc.) recognized by the control unit 26 from the outputs of the respective sensors mounted on the robot 1 are shown as being output from the actual robot 1. In the following description, the symbols R and L are omitted when it is not necessary to distinguish between the left and right of the leg 2.

  The control unit 26 includes a gait generator 100 that generates and outputs a target gait that is a target of the operation (gait) of the robot 1. In this embodiment, the target gait generated and output by the gait generator 100 is a target body position / posture trajectory that is a trajectory of the target position and target posture of the body 24 and a target position of each foot 22. And the desired foot position / posture trajectory which is the trajectory of the target posture, the target arm posture trajectory which is the trajectory of the target posture of each arm, and the target ZMP trajectory which is the trajectory of the ZMP (Zero Morment Point) of the robot 1 And a desired total floor reaction force trajectory that is a target trajectory of the total floor reaction force acting on the robot 1. In addition to the leg body 2 and the arm body, when a part movable with respect to the upper body 24 is provided, the target position / posture trajectory of the movable part is added to the target gait.

  Here, the “trajectory” in the target gait means a temporal change pattern (time series pattern), and is based on a time series of instantaneous values calculated every control cycle (arithmetic processing cycle) of the gait generator 100. Composed. In the following description, “pattern” may be used instead of “orbit”. Also, in the following description, “target” is often omitted when there is no possibility of misunderstanding.

  The position and speed of the upper body 24 mean the position of a predetermined representative point of the upper body 24 (for example, the center point between the left and right hip joints) and the moving speed thereof. Similarly, the position and speed of each foot 22 mean the position of the representative point determined in advance of each foot 22 and its moving speed. In this embodiment, the representative point of each foot 22 is a point on the bottom surface of each foot 22, for example, a perpendicular line from the center of the ankle joint of each leg 2 to the bottom surface of each foot 22 intersects the bottom surface. Set to a point.

  “Position” means a spatial orientation. For example, the body posture is represented by the inclination angle (posture angle) of the upper body 24 in the roll direction (around the X axis) and the inclination angle (posture angle) of the upper body 24 in the pitch direction (around the Y axis), The foot posture is represented by a biaxial spatial azimuth angle fixedly set on each foot 22. In this specification, the body posture is sometimes referred to as a body posture angle. The body posture may include the rotation angle of the body 24 in the yaw direction (around the Z axis).

  Further, in the gait, the elements other than the elements related to the floor reaction force (target ZMP and target total floor reaction force), that is, the steps related to the motion of each part of the robot 1, such as the foot position / posture and the body position / posture. Collectively, it is called “exercise”.

  In addition, the floor reaction force (the floor reaction force consisting of translational force and moment) acting on each foot 22 is called “each foot floor reaction force”, and all (two) feet 22R and 22L of the robot 1 The resultant force of “each foot floor reaction force” is called “whole floor reaction force”. However, in the following description, since each foot floor reaction force is hardly mentioned, “floor reaction force” is treated as the same as “total floor reaction force” unless otherwise specified.

  ZMP is a point on the floor where the horizontal component (moment component around the horizontal axis) of the moment that the resultant force of the inertial force generated by the motion of the robot 1 and the gravity acting on the robot 1 acts around that point is zero. Means. In a gait that satisfies the dynamic equilibrium condition, the ZMP and the floor reaction force central point coincide. In this case, giving the target ZMP is the same as giving the target floor reaction force central point.

  The target floor reaction force is generally expressed by an action point and a translational force and a moment acting on the point. Although the action point may be set anywhere, in the present embodiment, the target ZMP is set as the action point of the desired floor reaction force. In a gait that satisfies the dynamic equilibrium condition, the ZMP and the floor reaction force center point coincide with each other as described above. Therefore, the moment component of the target floor reaction force with the target ZMP as the action point is the vertical component (around the Z axis). Except for the moment component).

  Of the target gait generated by the gait generator 100, the target body position / posture trajectory and the target arm posture trajectory are input to the robot geometric model (kinematics calculation unit) 102.

  Also, the desired foot position / posture trajectory, the desired ZMP trajectory (target floor reaction force center point trajectory), and the desired total floor reaction force trajectory (specifically, the desired translational floor reaction force vertical component trajectory, the desired translational floor reaction force horizontal component trajectory) , The target floor reaction force moment trajectory around the target ZMP) is input to the composite compliance operation determination unit 104 and the target floor reaction force distributor 106.

  Then, the desired floor reaction force distributor 106 distributes the desired floor reaction force to each foot 22R, 22L, and the desired foot floor reaction force center point (the target of the floor reaction force center point of each foot 22R, 22L). Position) and the desired foot floor reaction force (the desired floor reaction force to be applied to the floor reaction force central point of each foot 22R, 22L). The determined target foot floor reaction force center point and the desired foot floor reaction force trajectory are input to the composite compliance operation determining unit 104. Note that the target floor reaction force output from the gait generator 100 may output only components necessary for compliance control by the composite compliance operation determination unit 104. For example, outputting the desired translational floor reaction force horizontal component from the gait generator 100 may be omitted.

  In the composite compliance operation determination unit 104, a corrected target foot position / posture with mechanism deformation compensation obtained by correcting the target foot position / posture is obtained, and the trajectory of the corrected target foot position / posture is input to the robot geometric model 102. The

  The robot geometric model 102 is a kinematics model of the robot 1 that gives joint displacement commands for the joints of both legs 2 and 2 that satisfy the input target body position / posture and the corrected target foot position / posture with mechanism deformation compensation. Calculation is performed by inverse kinematics calculation based on (rigid link model), and the calculated joint displacement command is output to the displacement controller 108. Further, the robot geometric model 102 calculates a joint displacement command for each joint of each arm body that satisfies the target arm posture, and outputs the calculated joint displacement command to the displacement controller 108.

  Then, the displacement controller 108 uses the joint displacement command calculated by the robot geometric model 102 as a target value, and calculates the rotation angle (actual joint displacement) of each joint of the legs 2 and 2 and both arms of the robot 1 as the servo. Follow-up control is performed via the amplifier 46. More specifically, the displacement controller 108 adjusts the output torque of the electric motor 42 as the actuator driving force so that the actual joint displacement (measured value) measured from the output of the encoder 44 matches the joint displacement command.

  The actual foot floor reaction force, which is the floor reaction force actually acting on each foot 22 of the robot 1 by the actual movement of the robot 1 by the follow-up control as described above, is measured from the output of the six-axis force sensor 36. The actual measured value of each floor floor reaction force is input to the composite compliance operation determining unit 104.

  The actual body posture angle, which is the actual posture angle (tilt angle with respect to the vertical direction) of the upper body 24 of the robot 1 is measured from the output of the tilt sensor 40, and the actual measured body posture angle is stabilized in posture. Input to the control calculation unit 112. Further, the posture stabilization control calculation unit 112 includes a target body posture angle (target value of the posture angle of the body 24 with respect to the vertical direction) of the target body position and posture generated by the gait generator 100 and a total target. The target floor reaction force vertical component of the floor reaction force is also input. The target body posture angle is a constant value (fixed value) in the present embodiment. For example, the posture angle (= 0) of the body 24 in a posture in which the trunk axis of the body 24 of the robot 1 faces the vertical direction. ). In this way, when the target body posture angle is a constant value (fixed value), inputting the target body posture angle to the posture stabilization control calculation unit 112 may be omitted.

  The posture stabilization control calculation unit 112 multiplies the body posture angle deviation Δθ, which is a deviation between the input actual body posture angle (actually measured value) and the target body posture angle, by a predetermined value. This is converted into a positional deviation ΔX described later. In the present embodiment, this position deviation ΔX corresponds to the state quantity deviation in the present invention.

  Further, the posture stabilization control calculation unit 112 is a required value of the floor reaction force moment to be additionally applied to the robot 1 around the target ZMP as a required operation amount for converging the position deviation ΔX to “0”. The compensated total floor reaction force moment Mdmd is calculated using the calculated position deviation ΔX, the inputted target floor reaction force vertical component, and the like. More specifically, the compensated total floor reaction force moment Mdmd is composed of a component Mdmdx in the roll direction (around the X axis) and a component Mdmdy in the pitch direction (around the Y axis). Mdmdx and Mdmdy are respectively around the target ZMP necessary to converge the component in the roll direction (around the X axis) and the component in the pitch direction (around the Y axis) of the body posture angle deviation Δθ to “0”. Required moment (specifically, a required value for the perturbation of the floor reaction force moment around the target ZMP). A more detailed calculation process of the compensated total floor reaction force moment Mdmd will be described later.

  In the present embodiment, the state quantity deviation observing means and the requested operation amount determining means in the present invention are realized by the above-described processing of the posture stabilization control calculation unit 112. In this case, the compensated total floor reaction force moment Mdmd is an additional external force to be applied to the robot 1 in order to converge the body posture angle deviation Δθ as a state quantity deviation to “0” (in this embodiment, It has a meaning as a required operation amount that defines the perturbation of the floor reaction force moment around the target ZMP.

  The compensated total floor reaction force moment Mdmd determined by the posture stabilization control calculation unit 112 is distributed to the target floor reaction force moment for compliance control and the model operation floor reaction force moment via the compensated total floor reaction force moment distributor 110. Is done.

  The target floor reaction force moment for compliance control is a perturbed floor reaction force moment around the target ZMP that is additionally applied to the actual robot 1 in order to bring the body posture angle deviation Δθ close to “0”. The model operation floor reaction force moment is a perturbed floor reaction force moment around the target ZMP that is additionally generated in a dynamic model for generating gaits described later for the same purpose as the target floor reaction force moment for compliance control. In other words, the model operation floor reaction force moment is a perturbed floor in which the motion of the target gait finally determined by the gait generator 100 (the target gait output by the gait generator 100) is generated around the target ZMP. Reaction force moment.

  These floor reaction force moments are determined as follows for each component in the roll direction (around the X axis) and the pitch direction (around the Y axis). First, the model operation floor reaction force moment is determined by the following equation 50. Note that Mdmd in Expression 50 means each component (Mdmdx or Mdmdy) in the roll direction (around the X axis) and the pitch direction (around the Y axis). Similarly, the floor reaction force moment allowable range means the allowable range of the floor reaction force moment of each component in the roll direction (around the X axis) and the pitch direction (around the Y axis). The floor reaction force moment allowable range is determined in the gait generator 100 as described later.

When Mdmd> Floor reaction force moment allowable range upper limit value Model operation floor reaction force moment =-(Mdmd-Floor reaction force moment allowable range upper limit value)
When Mdmd <floor reaction force moment allowable range lower limit value Model operation floor reaction force moment =-(Mdmd-floor reaction force moment allowable range lower limit value)
When floor reaction force moment allowable range lower limit value ≤ Mdmd ≤ floor reaction force moment allowable range upper limit value Model operation floor reaction force moment = 0
...... Formula 50
In the above formula 50, the compensation total floor reaction force moment Mdmd (more specifically, Mdmdx or Mdmdy) itself is compared with the floor reaction force moment allowable range. The object to be compared is a moment obtained by adding Mdmd to the reference instantaneous value of the floor reaction force moment around the target ZMP. The reference instantaneous value is a value around the target ZMP of the target total floor reaction force generated by the gait generator 100 when it is assumed that the body posture angle deviation Δθ is constantly maintained at “0”. It is a moment.

  In this case, in the present embodiment, the reference instantaneous value of the moment around the target ZMP is constantly “0” for both components in the roll direction (around the X axis) and the pitch direction (around the Y axis). Therefore, the value obtained by adding Mdmd to this reference instantaneous value is equal to Mdmd. Therefore, in the above formula 50, Mdmd (specifically, Mdmdx or Mdmdy) is directly compared with the floor reaction force moment allowable range.

  Next, the compliance control target floor reaction force moment is determined by the following equation 52 for each component in the roll direction (around the X axis) and the pitch direction (around the Y axis). As in the case of Formula 50, Mdmd in Formula 52 means each component (Mdmdx or Mdmdy) in the roll direction (around the X axis) and the pitch direction (around the Y axis).

Target floor reaction force moment for compliance control = Mdmd + Model operation floor reaction force moment ...... Formula 52

Accordingly, the floor reaction force moments are determined so that the difference between the target floor reaction force moment for compliance control and the model operation floor reaction force moment becomes equal to Mdmd.

  A block diagram of the compensating total floor reaction force moment distributor 110 that performs the above calculation is as shown in FIG. By this calculation processing, the compensation total floor reaction force moment Mdmd (Mdmdx or Mdmdy) is within the floor reaction force moment allowable range for each component in the roll direction (around the X axis) and pitch direction (around the Y axis). If Mdmd is present, the target floor reaction force moment for compliance control is determined as it is, and the model operation floor reaction force moment is determined to be “0”.

  Also, when the compensation total floor reaction force moment Mdmd (Mdmdx or Mdmdy) deviates from the floor reaction force moment allowable range for each component in the roll direction (around X axis) and pitch direction (around Y axis) The boundary value closer to Mdmd of the upper and lower limits of the floor reaction force moment allowable range is determined as the target floor reaction force moment for compliance control, and Mdmd from the floor reaction force moment allowable range is determined. A moment obtained by inverting the sign of the deviation (= Mdmd−boundary value of the floor reaction force moment allowable range) is determined as the model operation floor reaction force moment.

  Supplementally, the distribution means in the present invention is realized by the processing of the compensating total floor reaction force moment distributor 110 described above. In this case, the target floor reaction force moment for compliance control functions as an actual moving body operation external force as an additional external force to be additionally applied to the actual robot 1, and the model operation floor reaction force moment generates a target gait. It functions as a model operation external force to be applied to the robot 1 on the dynamic model used for this purpose.

  Returning to the description of FIG. 3, the model operation floor reaction force moment determined as described above is input to the gait generator 100. As will be described in detail later, the gait generator 100 uses the dynamic model so that the horizontal component of the floor reaction force moment around the target ZMP determined by the gait generator 100 becomes the model operation floor reaction force moment. Is used to generate the motion of the desired gait (target motion). Note that the target total floor reaction force output from the gait generator 100 to the composite compliance action determination unit 104 is targeted for the horizontal component of the floor reaction force moment around the target ZMP to be “0”. Is output as

  Further, the target floor reaction force moment for compliance control determined as described above by the compensating total floor reaction force moment distributor 110 is input to the composite compliance operation determination unit 104. Then, the composite compliance operation determination unit 104 causes the actual floor reaction force moment around the target ZMP to be the target floor reaction force for compliance control while causing the movement of the robot 1 to follow the movement of the target gait generated by the gait generator 100. The corrected desired foot position / posture (trajectory) with mechanism deformation compensation is determined by correcting the desired foot position / posture so as to approach the moment.

  In this case, since it is practically impossible to make all the foot position / posture and floor reaction force of the robot 1 coincide with the target, a trade-off relationship is given between them to make them coincide as much as possible. That is, the composite compliance action determination unit 104 gives a weight to the control deviation for each target, and the corrected target foot position with mechanism deformation compensation so that the weighted average of the control deviation (or the square of the control deviation) is minimized. Determine posture (orbit).

  In other words, mechanism deformation compensation is provided so that the actual floor reaction force moment around the target ZMP and the actual foot position / posture of the robot 1 are as close as possible to the target floor reaction force moment for compliance control and the target foot position / posture. A corrected desired foot position / posture (orbit) is determined. Then, the composite compliance operation determination unit 104 controls the operation of the robot 1 by outputting the corrected target foot position / posture as a final target value of the foot position / posture to the robot geometric model 102.

  In the present embodiment, the operation control means in the present invention is realized by the processing of the composite compliance operation determination unit 104 described above. That is, the composite compliance operation determination unit 104 causes the robot 1 to additionally act on the compliance control target floor reaction force moment as an additional external force while causing the actual motion of the robot 1 to follow the motion of the target gait. The actual operation control of the robot 1 is performed.

  The configuration and operation of the above-described composite compliance operation determination unit 104 and the like are described in detail in Japanese Patent Application Laid-Open No. 10-277969 filed earlier by the present applicant. Therefore, the description in the present specification regarding the composite compliance operation determination unit 104 is only described above.

  As described above, by controlling the operation of the robot 1, the movement of the target gait additionally generates a model operation floor reaction force moment as a model operation external force around the target ZMP on the dynamic model of the robot 1. The model operation floor reaction force moment is controlled so as not to be added to the actual floor reaction force of the robot 1. Accordingly, the movement of the desired gait and the unbalance (floor balance) of the floor reaction force are generated by the subtraction model operation floor reaction force moment. In terms of the effect of converging the body posture angle deviation Δθ to 0, this is equivalent to applying a floor reaction force moment obtained by inverting the sign of the model operation floor reaction force moment to the real robot 1.

  That is, by appropriately determining the model operation floor reaction force moment, the actual posture of the upper body 24 of the real robot 1 can be converged to the target posture, and as a result, the entire posture of the real robot 1 can be stabilized.

  In this case, the sum of the moment obtained by reversing the sign of the model operation floor reaction force moment and the target floor reaction force moment for compliance control is the total restoring force (floor reaction force moment) that converges the body posture angle deviation Δθ to zero. Become. That is, the difference between the target floor reaction force moment for compliance control and the model operation floor reaction force moment is the total restoring force.

  Note that the model operation floor reaction force moment can take any value ignoring the ZMP existence possible range, so that a very high posture restoring force can be generated.

  Next, the outline of the target gait generated by the gait generator 100 will be described using the running gait shown in FIG. 5 as an example.

  In the following explanation, “floor reaction force vertical component” and “floor reaction force horizontal component” mean “translation floor reaction force vertical component” and “translation floor reaction force horizontal component”, respectively, unless otherwise specified. It shall be.

  In addition, the “both leg support period” in the gait is a period in which the robot 1 supports its own weight with both legs 2 and 2, and the “one leg support period” is the weight of the robot 1 with only one leg 2. The “air stage” means a period during which both legs 2 and 2 are away from the floor (floating in the air). In addition, the leg 2 on the side that supports the weight of the robot 1 in the one-leg support period is referred to as a “support leg”, and the leg 2 on the side that does not support the weight is referred to as a “free leg”. In the running gait mainly described in the present embodiment, there is no both-leg support period, and the one-leg support period (landing period) and the air period are alternately repeated. In this case, both legs 2 and 2 do not support the own weight of the robot 1 in the air period, but the leg body 2 and the leg that were the support legs in the single leg support period immediately before the air period. The body 2 is also referred to as a free leg and a support leg in the aerial period.

  First, the traveling gait shown in FIG. 5 will be described. This traveling gait is the same gait as a normal human traveling gait. In this running gait, there are a single leg support period in which the foot 22 of the leg 2 (support leg) of only one of the left and right sides of the robot 1 lands (grounds), and an air period in which both legs 2 and 2 float in the air Are repeated alternately.

  The numbers with circles in FIG. 5 indicate the chronological order of the running gaits. In this case, the first state in FIG. 5 is a state at the beginning (initial stage) of the one-leg support period, the second state is an intermediate point in the one-leg support period, and the third state is following the one-leg support period The state at the start of the air phase (at the end of the one-leg support period), the fourth state at the middle of the air period, the fifth state at the end of the air period (at the start of the next one-leg support period) ) State. In addition, a white arrow in FIG. 5 indicates the traveling direction of the robot 1.

  In this running gait, as shown in the first state of FIG. 5, the robot 1 heels the foot 22 of the supporting leg (the leg 2 on the front side in the traveling direction of the robot 1) at the start of the one-leg supporting period. Land on.

  Subsequently, as shown in the second state of FIG. 5, the robot 1 lands substantially the entire bottom surface of the landing foot 22 (foot 22 of the supporting leg), and then the third state of FIG. 5. As shown in FIG. 5, the floor is kicked by the toes of the foot 22 of the supporting leg (the foot 22 of the leg 2 on the rear side in the moving direction of the robot 1 in the third state in FIG. 5) and jumps into the air. As a result, the one leg support period ends and the aerial period begins. As shown in the first state of FIG. 5, the free leg in the one-leg support period exists on the rear side of the support leg at the start of the one-leg support period. Thereafter, as shown in the second and third states in FIG. 5, the free leg is swung out to the front side of the support leg toward the next planned landing position.

  Next, after the aerial period shown in the fourth state in FIG. 5, the robot 1 moves the foot 22 of the free leg (the leg 2 that has become a free leg in the one-leg support period immediately before the start of the aerial period). Land on the heel and the next one-leg support period begins.

  A basic outline of the target gait generated by the gait generator 100 will be described while assuming the running gait of FIG.

  When the gait generator 100 generates a target gait, request parameters representing basic requests regarding the motion form of the robot 1 are generated by wireless communication or the like from a control device or server (not shown) outside the robot 1. Input to the device 100. The required parameters include, for example, the motion type (walking, running, etc.) of the robot 1, the target landing position / posture (planned landing position / posture) of the free leg side foot 22, and the target landing time (planned landing time). ) Or parameters necessary for determining them (for example, the average moving speed and moving direction of the robot 1). Such request parameters may be stored in advance in a storage device (not shown) of the robot 1 and read by the gait generator 100 according to a predetermined schedule.

  Then, the gait generator 100 generates a target gait by a predetermined algorithm using the request parameter. More specifically, in the present embodiment, the gait generating device 100 is a part of the desired gait such as the desired foot position / posture trajectory of the desired gait, the desired floor reaction force vertical component trajectory, etc., according to the required parameter. The gait parameter as a parameter that defines the constituent elements of is determined, and the instantaneous value of the desired gait is sequentially determined using the gait parameter and the dynamic model of the robot 1. As a result, the gait generator 100 generates a time-series pattern (trajectory) of the target gait.

  In this case, the desired foot position / posture trajectory is generated for each foot 22 by using, for example, a finite time settling filter proposed by the present applicant in Japanese Patent No. 3233450. This finite time settling filter is a variable time constant primary delay filter, that is, a filter whose transfer function is expressed in the form of 1 / (1 + τs) (τ is a variable time constant. This filter is hereinafter referred to as a unit filter). Are connected in series, and can generate and output a trajectory that reaches a specified value at a desired specified time. In this case, the time constant τ of each stage unit filter is variably set in accordance with the remaining time from the start of output generation of the finite time settling filter to the specified time. More specifically, as the remaining time becomes shorter, the value of τ is decreased from a predetermined initial value (> 0), and finally, at the specified time when the remaining time becomes “0”, τ Is set to be “0”. The finite time settling filter is given a step input having a height corresponding to the specified value (more specifically, the amount of change from the initial value of the output of the finite time settling filter to the specified value). Such a finite time settling filter not only generates an output that reaches a specified value at a specified time, but also changes the rate of change of the output of the finite time settling filter to “0” or almost “0” at the specified time. can do. In particular, when unit filters are connected in three or more stages (three stages are sufficient), the change acceleration (differential value of the change speed) of the output of the finite time settling filter is also set to “0” or almost “0”. Can do.

  Generation of a foot position / posture trajectory using such a finite time settling filter (a position / posture trajectory from when the foot 22 lands to the next landing) is performed, for example, as follows. For example, the desired foot position trajectory in the X-axis direction (front-rear direction) is generated as follows. That is, the X-axis direction position of the next scheduled landing position of each foot 22 defined by the required parameter (more specifically, the change amount (movement amount in the X-axis with respect to the landing position immediately before the next planned landing position) This corresponds to the specified value) and the step input height to the finite time settling filter is determined and the time constant τ is initialized to a predetermined initial value and then determined. The step input is given to the finite time settling filter, and the trajectory generation of the foot 22 in the X-axis direction is started. At the time of generating the trajectory, the time constant τ is sequentially variably set so as to decrease from the initial value to “0” by the scheduled landing time of the foot 22 (which corresponds to the specified time). The As a result, a trajectory of the position of the foot 22 in the X-axis direction that reaches the planned landing position at the planned landing time is generated.

  The desired foot position trajectory in the Z-axis direction (vertical direction) is generated as follows, for example. That is, first, the position of the foot 22 in the Z-axis direction when the height (vertical position) of the foot 22 is maximized (hereinafter, referred to as the following) The highest point position) and the arrival time at the highest point position are determined. Then, the height of the step input to the finite time settling filter is determined according to the highest point position (which corresponds to the specified value) and the time constant τ is initialized, and then the determined step An input is given to a finite time settling filter, and a foot position trajectory in the Z-axis direction up to the highest point position is sequentially generated. At this time, the time constant τ is sequentially variably set so as to decrease from the initial value to 0 by the time of arrival at the highest point position (corresponding to the specified time). When the generation of the trajectory in the Z-axis direction up to the highest point position is completed, the time constant τ is initialized and the step input having the opposite polarity to the previous step input (more specifically, the next landing from the highest point position) The amount of change in the Z-axis direction up to the planned position (this is a step input with a reverse polarity corresponding to the specified value) is input to the finite time settling filter, from the highest point position to the planned landing position The trajectory of the foot position in the Z-axis direction is sequentially generated. At this time, the time constant τ is sequentially variably set so as to decrease from the initial value to 0 by the scheduled landing time of the foot 22.

  In the generation of the foot position trajectory in the Z-axis direction, the time constant τ is variably set so as to continuously decrease from the initial value to “0” from the trajectory generation start time to the scheduled landing time of the foot 22. In addition, the foot position trajectory in the Z-axis direction may be generated by switching the polarity of the step input to the reverse polarity at the time of arrival at or near the highest point position. In this case, the foot 22 cannot reach the desired highest point position with high accuracy, but can reach the planned landing position at the planned landing time without any problem.

  The foot posture trajectory can also be generated using a finite time settling filter in the same manner as the foot position trajectory described above. In this case, among the spatial components of the foot posture, for the component in which the angle change of the posture is monotonous (monotonically increasing or monotonically decreasing), the above-described generation of the foot position trajectory in the X-axis direction is generated. A foot posture trajectory may be generated in the same manner as described above. In addition, for a component whose posture angle change has a maximum value or a minimum value, a foot posture trajectory may be generated in the same manner as the generation of the foot position trajectory in the Z-axis direction.

  The target foot position / posture trajectory generated by the finite time settling filter as described above is a target position / posture trajectory of each foot 22 in the support leg coordinate system described later.

  The desired foot position / posture trajectory generated as described above moves while gradually accelerating the position of each foot 22 from the initial ground contact state (the state of the initial time of the target gait) toward the planned landing position. Is generated to start. The target foot position / posture trajectory gradually decelerates the speed of position change to “0” or almost “0” by the planned landing time, and reaches the planned landing position at the planned landing time. Generated to stop. For this reason, the ground speed at the moment of landing of each foot 22 (change speed of the position of each foot 22 in the support leg coordinate system fixed to the floor) becomes “0” or almost “0”. Therefore, even when the landing gait lands from the state where all the legs 2 and 2 are present in the air at the same time (the state in the air), the landing impact is reduced.

  In the running gait, the vertical velocity of the upper body 24 starts downward from the latter half of the air due to the gravity acting on the robot 1, and remains downward even when landing. Therefore, as described above, the desired foot position / posture trajectory is generated so that the ground speed at the moment of landing of each foot 22 becomes “0” or almost “0”, and the dynamic equilibrium condition is satisfied as described later. Thus, when the target position / posture trajectory of the upper body 24 is generated, immediately before landing, the relative speed of the foot 22 on the free leg side with respect to the upper body 24 becomes upward. That is, at the moment of landing of the running gait, the target gait of the robot 1 is a gait that makes landing while retracting the leg 2 on the free leg side to the upper body 24 side. In other words, in the target gait in the present embodiment, the robot 1 sees from the upper body 24 so that the ground speed of the foot 22 on the free leg side becomes “0” or almost “0” at the moment of landing. Then, the foot 22 is landed so that the foot 22 is pulled up. This reduces the landing impact and prevents the landing impact from becoming excessive.

  In this embodiment, since the finite time settling filter is a unit filter having three or more stages (for example, three stages) connected in series, the speed of each foot 22 (the position of the foot position) by the scheduled landing time. (Change speed) becomes “0” or almost “0”, and each foot 22 stops at its acceleration of 0 or almost 0 at the scheduled landing time. That is, the ground acceleration at the moment of landing is also “0” or almost “0”. Therefore, the landing impact is further reduced. In particular, even if the actual landing time of the robot 1 deviates from the target landing time, the impact does not increase so much. Supplementally, in order to set the ground speed of each foot 22 to “0” or almost “0” at the scheduled landing time, the number of unit filters of the finite time settling filter may be two, but in this case, The acceleration of each foot 22 at the scheduled landing time is generally not “0”.

  Regarding the foot posture, after each foot 22 has landed with its heel at the scheduled landing time, the foot 22 continues to move until almost the entire bottom surface of the foot 22 contacts the floor. For this reason, the time at which almost the entire bottom surface of the foot 22 contacts the floor is set as the designated time, and the foot posture trajectory is generated by the finite time settling filter.

  In this embodiment, the foot position trajectory is generated using a finite time settling filter. However, the change speed of the foot position at the scheduled landing time is “0” or almost “0” (the foot position is changed). Furthermore, the foot position change acceleration (time differential value of the change speed) at the scheduled landing time is set to “0” or almost “0” so that the time differential value becomes “0”. The desired foot position trajectory may be generated using a function such as a polynomial. The same applies to the generation of the desired foot posture trajectory. However, regarding the generation of the desired foot posture trajectory, as described above, at the time when almost the entire bottom surface of each foot 22 is in contact with the floor, the changing speed of the posture of each foot 22 and further the changing acceleration thereof. A function such as a polynomial is set so that becomes “0” or almost “0”.

  The gait generator 100 explicitly sets the desired floor reaction force vertical component. This desired floor reaction force vertical component trajectory is set as shown in FIG. 6, for example. In the present embodiment, the shape of the desired floor reaction force vertical component trajectory in the running gait (specifically, the shape in the one-leg support period) is defined as a trapezoidal shape (a shape convex to the increase side of the floor reaction force vertical component). ing. Then, the height of the trapezoid and the time of the break point are determined as gait parameters (floor reaction force vertical component trajectory parameters) that define the desired floor reaction force vertical component trajectory.

  Note that the target floor reaction force vertical component is constantly set to “0” in the air phase of the running gait. As in this example, the desired floor reaction force vertical component trajectory is preferably set so as to be substantially continuous (so that the value does not become discontinuous). This is to smooth the movement of the joint of the robot 1 when controlling the floor reaction force. Here, “substantially continuous” means that an analog continuous trajectory (a true continuous trajectory) digitally expressed in a discrete-time system is a value jump that occurs inevitably. It means that the continuity of is not lost.

  Supplementally, in this embodiment, since the floor reaction force is assumed as the entire external force acting on the robot 1, the target floor reaction force vertical component is the vertical direction of the entire robot 1 (of the entire center of gravity of the robot 1). It defines the inertial force. That is, a value obtained by subtracting a component that balances the gravity acting on the entire robot 1 from the target floor reaction force vertical component is balanced with the vertical inertial force of the entire robot 1. Therefore, by determining the desired floor reaction force vertical component, the vertical inertia force of the entire robot 1 is determined as a result.

  The target ZMP trajectory is set as shown in FIGS. 7A and 7B, for example. In the running gait of FIG. 5, the robot 1 lands on the heel of the supporting leg side foot 22 as described above, then kicks with the toe of the supporting leg side foot 22 and jumps into the air, and finally plays. Land on the heel of the leg foot 22. Therefore, the X-axis direction position (front-rear direction position) of the target ZMP trajectory in the one-leg support period is set to the heel of the support leg side foot 22 as the initial position as shown in FIG. It is set so that it moves to the center in the front-rear direction of the foot 22 during a period in which almost the entire bottom surface of the side foot 22 is in contact with the ground, and then moves to the toe of the supporting leg-side foot 22 before leaving the floor. . In addition, the Y-axis direction position (left-right direction position) of the target ZMP trajectory in the one-leg support period is the same as the Y-axis direction position of the center of the ankle joint of the support leg side leg 2 as shown in FIG. Set to position.

  As shown in FIG. 7A, the X-axis direction position of the target ZMP trajectory in the aerial phase is such that the support leg side foot 22 is moved until the aerial phase is completed (until the free leg side leg 2 is landed). It is set to move continuously from the toes to the landing position of the heel of the free leg side foot 22. Further, as shown in FIG. 7 (b), the position of the target ZMP trajectory in the Y axis direction in the aerial phase is such that the supporting leg side leg until the aerial phase ends (until the free leg side leg 2 lands). It is set so as to continuously move from the Y-axis direction position of the center of the two ankle joints to the Y-axis direction position of the center of the ankle joint of the free leg side leg body 2. That is, the target ZMP trajectory is set to a trajectory that is continuous (substantially continuous) during the entire gait. Here, the meaning of “substantially continuous” in the ZMP trajectory is the same as that in the case of the floor reaction force vertical component trajectory.

  In this embodiment, the position and time of the break point of the target ZMP trajectory as shown in FIGS. 7A and 7B are set as ZMP trajectory parameters (parameters that define the target ZMP trajectory).

  The ZMP trajectory parameter is determined so that it has a high stability margin and does not change suddenly. Here, a state in which the target ZMP exists in the vicinity of the center of the smallest convex polygon (so-called support polygon) including the ground contact surface of the robot 1 is said to have a high stability margin (for details, see Japanese Patent Laid-Open No. 10-86081). ). The target ZMP trajectory shown in FIGS. 7A and 7B is set to satisfy such a condition.

  The target arm posture is expressed as a relative posture with respect to the upper body 24.

  The target body position / posture and the desired foot position / posture are described in a global coordinate system. The global coordinate system is a coordinate system fixed to the floor, and more specifically, a support leg coordinate system described below is used. The support leg coordinate system is configured such that the support leg side foot 22 is in a posture parallel to the floor surface and the bottom surface of the support leg side foot 22 is substantially in contact with (contacted with) the floor surface. A point where a perpendicular extending from the center of the ankle of the side leg 2 to the floor intersects the floor (this is the case where almost the entire bottom surface of the supporting leg side foot 22 is brought into contact with the floor in the example of this embodiment. In this state, the coordinate system is fixed to the floor, with the origin being the representative point of the foot 22 and the XY plane passing through the origin. In this case, the X-axis direction and the Y-axis direction are the front-rear direction and the left-right direction of the support leg side foot 22, respectively.

  The origin of the support leg coordinate system is not necessarily the representative point of the foot 22 in a state where almost the entire bottom surface of the foot 22 on the support leg is in contact with the floor surface (a point representing the position of the foot 22). May be set to a point on the floor different from the representative point.

  Next, a dynamic model of the robot 1 used for gait generation in this embodiment will be described with reference to FIG.

  As shown in FIG. 8, the dynamic model of the robot 1 used in this embodiment includes a variable-length rod 24a that can swing with a target ZMP as a fulcrum, and a mass point 24b supported on the upper end of the rod 24a. It is a model composed of an inverted pendulum (hereinafter sometimes referred to as an inverted pendulum model). The rod 24a has no mass. In this dynamic model, the motion of the mass point 24b of the inverted pendulum corresponds to the motion of the upper body 24 of the robot 1, and the relationship between the motion of the upper body 24 and the floor reaction force acting on the robot 1 is This is expressed as a relationship between the movement of the mass point 24b (hereinafter referred to as the upper mass point 24b) and the floor reaction force acting on the inverted pendulum. Further, in this dynamic model, the mass of the legs 2 and 2 of the robot 1 is considered to be sufficiently smaller than the upper body 24 (or the total mass of the upper body 24, the arm body, and the head), and the upper body structure It is assumed that the mass of the point 24 b matches the overall mass of the robot 1. The position of the body mass point 24b is set to a position that is uniquely determined from the body position / posture of the robot 1.

  The behavior of this dynamic model is expressed as follows: However, in order to simplify the explanation, only the equation of motion in the sagittal plane (plane including the X axis and Z axis of the supporting leg coordinate system) is described, and the lateral plane (Y axis and Z axis of the supporting leg coordinate system) is described. The equation of motion is omitted.

For convenience of explanation, variables and parameters related to the dynamic model are defined as follows.
Zb: Upper body mass point vertical position (Z-axis direction position)
Xb: Upper body point horizontal position (X-axis direction position)
mb: Upper mass point mass Xzmp: Horizontal position of target ZMP (X-axis direction position)
Zzmp: Vertical position of target ZMP (Z-axis direction position)
Fx: Floor reaction force horizontal component (specifically, X-axis direction component of translational floor reaction force)
Fz: Floor reaction force vertical component (specifically, the Z-axis direction component of the translational floor reaction force)
Mzmp_y: Floor reaction force moment around the target ZMP (specifically, the component around the Y axis of the floor reaction force moment)
Mb_y: Floor reaction force moment around the origin of the support leg coordinate system (specifically, the component around the Y axis of the floor reaction force moment)

For an arbitrary variable A, dA / dt represents the first derivative of A, and d2A / dt2 represents the second derivative of A. Therefore, if the variable A is displacement (position), dA / dt means velocity and d2A / dt2 means acceleration. g represents a gravitational acceleration constant. Here, g is a positive value.

  The equation of motion of the dynamic model is expressed by equations 01 to 04.

Fz = mb * (g + d2Zb / dt2) ...... Formula 01
Fx = mb * d2Xb / dt2 ...... Formula 02
Mb_y = -mb * Xb * (g + d2Zb / dt2)
+ Mb * Zb * (d2Xb / dt2) ...... Formula 03
Mzmp_y = -mb * (Xb-Xzmp) * (g + d2Zb / dt2)
+ Mb * (Zb-Zzmp) * (d2Xb / dt2) ...... Formula 04

In this case, the relationship between Mb_y and Mzmp_y is expressed by the following equation 05.

Mb_y = Mzmp_y−mb * Xzmp * (g + d2Zb / dt2) + mb * Zzmp * (d2Xb / dt2)
= Mzmp_y-Xzmp * Fz + Zzmp * Fx
...... Formula 05

Note that the difference in the position in the vertical direction (Z-axis direction) between the target ZMP and the origin of the support leg coordinate system is normally “0” or almost “0”, so it may be considered that Zzmp = 0.

  In addition, the relative variation of the height (vertical position) of the upper body 24 when the robot 1 moves relative to the average height (in other words, the dispersion of the vertical position of the upper body 24) is generally sufficient. Therefore, it can be considered that Zb≈h (h: a constant value as the average height of the body mass point 24b corresponding to the average height of the body 24). Therefore, Zb in the second term on the right side of Equation 03 and the second term on the right side of Equation 04 may be replaced with a constant value h.

  The gait generator 100 according to the present embodiment has a target gait for one step from the time when one leg 2 of the robot 1 is landed until the other leg 2 is landed as a unit. Generate target gaits in order. Therefore, in the running gait of FIG. 5 generated in the present embodiment, the target gait is from the start of the one-leg support period to the end of the following air period (at the start of the next one-leg support period). The desired gaits are generated in order.

  Here, in this specification, “one step” of the target gait is used in the meaning from the time when one leg 2 of the robot 1 lands until the other leg 2 lands. Also, the new target gait to be generated is called "current gait", the next target gait is called "next gait", and the next target gait is called "next gait". . A target gait generated immediately before the “current time gait” is referred to as a “previous gait”.

  When the gait generator 100 generates the current time gait, the gait generator 100 defines the planned landing position / posture and scheduled landing time of the free leg side foot 22 up to two steps ahead of the robot 1. (Or the gait generator 100 reads the request parameter from the storage device). The gait generator 100 generates a desired body position / posture trajectory, a desired foot position / posture trajectory, a target ZMP trajectory, a desired floor reaction force vertical component trajectory, a target arm posture trajectory, and the like using these required parameters. To do.

  At this time, the gait generator 100 generates a virtual periodic gait following the current gait (the motion of the robot 1 having the same pattern) to generate the current gait that can ensure the continuity of the movement of the robot 1. The normal turning gait as a gait that is continuously repeated at a constant period is determined according to the required parameter. Then, the gait generator 100 generates the current time gait by converging the current time gait into a normal turning gait in the future.

  The details of the gait generation processing of the gait generation device 100 and the details of the processing of the posture stabilization control calculation unit 112 will be described below mainly using generation of the running gait of FIG.

  First, details of the gait generation process of the gait generator 100 will be described with reference to FIGS. The gait generator 100 generates a target gait by executing a gait generation process (main routine process) shown in the flowchart (structured flowchart) of FIG. In the present embodiment, the gait generator 100 includes a function as a target motion determination unit and a function as a vertical inertial force parameter determination unit, depending on the processing it executes.

  First, various initialization operations such as initializing time t to “0” in S010 are performed. This process is performed when the gait generator 100 is activated.

  Next, the process proceeds to S014 through S012, and the gait generator 100 waits for a timer interrupt for each control cycle (the calculation processing cycle in the flowchart of FIG. 9). The control period is Δt.

  Next, in S016, the gait generator 100 determines whether or not it is a gait change. At this time, if the gait is switched, the process proceeds to S018, and if not, the process proceeds to S032. Here, the “gait change point” means a timing when generation of the previous gait is completed and generation of the current time's gait is started. For example, control next to a control cycle in which generation of the previous gait is completed is performed. The cycle is the gait change.

  When proceeding to S018, the time t is initialized to “0”. Next, proceeding to S020, the gait generator 100 determines the next time gait supporting leg coordinate system, the next time next gait supporting leg coordinate system, the current time gait cycle, and the next time gait cycle. Note that determining the support leg coordinate system means determining the position of the origin and the posture (direction of each coordinate axis) of the support leg coordinate system.

  These supporting leg coordinate systems and gait cycles are determined based on the required parameters. In other words, in the present embodiment, the requested parameter input to the gait generator 100 is the planned landing position / posture of the free leg side foot 22 up to two steps ahead (the foot 22 is grounded after the foot 22 has landed). The foot position / posture in a state of being rotated without slipping and the scheduled landing time are included. Then, the request value for the first step and the request value for the second step correspond to the current time's gait and the next time's gait, respectively, before the start of generation of the current time's gait (the gait change point in S016). To the gait generator 100. These required values can be changed during the generation of the current time's gait.

  The next time gait supporting leg coordinate system is determined in accordance with the required landing position / posture value of the free leg side foot 22 of the first step (the free leg side foot 22 in the current time's gait) in the required parameters. Is done.

  For example, referring to FIG. 12, the required landing position / posture value of the free leg side foot 22 (22L in the figure) related to the current time's gait (first step) is the support leg side foot 22 ( For the landing position / posture 22R in the figure, the X-axis direction of the current time's gait support leg coordinate system (the front and rear direction of the foot 22R on the support leg side of the current time's gait) and the Y-axis direction (the support leg side of the current time's gait) It is assumed that the positions and orientations are respectively moved by xnext and ynext in the horizontal direction of the foot 22R and rotated by θznext around the Z axis (around the vertical axis).

  At this time, when the next time's gait supporting leg coordinate system is landed on the free leg side foot 22L of the current time's gait according to the required landing position / posture value of the gait as shown in the figure (representative of the foot 22). A representative point of the foot 22L (more specifically, the representative point) when the point is matched with the required value of the planned landing position and the posture (orientation) of the foot 22 is matched with the required value of the planned landing position) And a coordinate system in which the front and rear directions and the left and right directions of the foot 22L in the horizontal plane passing through the origin are the X′-axis direction and the Y′-axis direction, respectively.

  Similarly to the above, the gait support leg coordinate system (see the X "Y" coordinate in FIG. 12) is determined one after another according to the required value of the expected landing position / posture of the free leg side foot 22 in the second step. Also, the current time's gait cycle is the planned landing time (requested value) of the free leg side foot 22 of the first step (current time's gait) from the planned landing time (requested value) of the supporting leg side foot 22 of the current time gait. ), And the next gait cycle is the estimated landing time (requested value) of the free leg side foot 22 of the second step from the estimated landing time (requested value) of the free leg side foot 22 of the first step. ).

  The required parameters are input to the gait generator 100 from a control device or a server outside the robot 1, for example. Alternatively, the request parameter may be stored in advance in the storage device in the robot 1 as a movement schedule of the robot 1. Alternatively, the next and next gait support leg coordinate systems and the current and next gait cycles may be determined based on a command (request) from the control device and the movement history of one robot up to that time. . Further, the request parameter may be a parameter that directly designates the position and posture of the next and next gait support leg coordinate system, and the current and next gait cycle.

  Next, proceeding to S022, the gait generating device 100 gaits of a normal turning gait as a virtual periodic gait following the current gait (a virtual periodic gait targeted for convergence of the current gait). Determine the parameters. In this embodiment, the gait parameters specify a foot trajectory parameter that defines a desired foot position / posture trajectory in a normal turning gait, a body posture trajectory parameter that defines a target body posture trajectory, and a target arm posture trajectory. Arm trajectory parameters, a ZMP trajectory parameter that defines a target ZMP trajectory, and a floor reaction force vertical component trajectory parameter that defines a target floor reaction force vertical component trajectory.

  Here, the “steady turning gait” is the motion state (foot position / posture, body position / posture, etc.) of the robot 1 at the gait boundary (boundary gait boundary) when the gait is repeated. It means a periodic gait that does not cause discontinuity in the movement state. Further, the “steady turning gait” is a gait in which the robot 1 can continuously perform the movement because it is a periodic gait, that is, a gait in which the same pattern of gaits are repeated at a constant period. In other words, the “steady turning gait” is a periodic gait that can repeat the motion of the same pattern without causing a discontinuity of the trajectory of the gait (in principle, it will be described later “ Gait that does not generate "divergence").

  In this embodiment, the normal turning gait that is a periodic gait is a gait for two steps of the robot 1, that is, a first turning gait following the current time gait and a second turning following the first turning gait. A gait consisting of gaits is a gait for one cycle, and the gait for one cycle is repeated at a constant cycle. The term “turning” is used here because when the turning rate is zero, it means straight travel, and straight travel can be included in the turn in a broad sense.

  When the target gait to be generated is the above-described running gait shown in FIG. 5, the first turning gait and the second turning gait of the normal gait are both supported by one leg in the same manner as the target gait. A gait having a period and an aerial period. That is, the basic gait forms of the first turning gait and the second turning gait are the same as the current time gait.

  Supplementing the normal turning gait (hereinafter, sometimes simply referred to as a normal gait), in a biped robot, the gait for one cycle of the normal gait needs to include at least two gaits. It is. In this case, it is possible to set a complicated normal gait in which a gait of three or more steps is a gait for one cycle. However, the normal gait is used only to determine a target (appropriate) divergent component value at the end (end time) of the current time gait, as will be described later. For this reason, using a normal gait with a gait of three or more steps as one cycle has little effect, although the gait generation process is complicated. Therefore, the gait for one cycle of the normal gait in this embodiment is composed of gaits for two steps (a set of a first turning gait and a second turning gait). In a legged mobile robot with three or more legs, the number of gaits sufficient to define a normal gait increases accordingly.

  Here, “divergence” means that the position of the upper body 24 of the robot 1 shifts to a position far from the positions of both feet 22, 22 as shown in FIG. 10. Further, the value of the divergent component is the position of the upper body 24 of the robot 1 at the positions of both feet 22 and 22 (more specifically, in the support leg coordinate system set on the ground contact surface of the support leg side foot 22). This is a numerical value indicating how far away from the origin.

  The normal gait is a virtual gait created by the gait generator 100 in order to determine the target motion state of the robot 1 at the end of the current time gait. Accordingly, the normal turning gait is not output from the gait generator 100 as it is.

  In this embodiment, the gait is generated using the divergent component as an index so that the desired gait is continuously generated without causing the divergence. In this case, even if the normal gait is a typical example of a continuous gait, if the gait parameter of the normal gait changes, the initial divergence component of the normal gait (the divergence at the initial time of the normal gait). Ingredients) also change. That is, an appropriate divergent component changes depending on the gait form such as how to walk, how to run, and moving speed.

  Therefore, in this embodiment, when the gait generator 100 generates a current time gait, the gait following the current time gait to be generated (a gait for which future convergence is also targeted). The normal gait as the future virtual periodic gait (the future virtual periodic gait capable of continuing the stable movement of the robot 1) suitable for the current time gait. An initial divergent component of the normal gait is obtained after determining according to the required parameters. Then, the gait generator 100 matches the terminal divergent component of the current time gait with the obtained initial divergent component of the normal gait (more generally, the current time's gait is made continuous with the normal gait or asymptotically. Gait is generated this time. The basic guideline for generating such a gait is the same as that of Japanese Patent No. 3726081 previously proposed by the present applicant.

  However, in the present embodiment, the divergent component of the gait is defined as described later based on the state equation of the dynamic model (inverted pendulum model) in FIG. Details of the definition of the divergent component will be described later.

  Returning to the main subject, in S022, the gait generator 100 executes the subroutine processing shown in the flowchart of FIG.

  First, in S100, the gait generator 100 causes the foot in the gait parameters of the normal gait so that the foot position / posture trajectory is connected in the order of the current time gait, the first turning gait and the second turning gait. A flat trajectory parameter is determined. A specific setting method will be described below with reference to FIG. In the following description, the foot 22 of the leg 2 on the support leg side is referred to as a support leg foot 22, and the foot 22 of the leg 2 on the free leg side is referred to as a free leg foot 22. The “initial” and “end” of the gait mean the gait start time, end time, or instantaneous gait at those times, respectively.

  The foot trajectory parameters are the respective positions and orientations of the supporting leg foot 22 and the free leg foot 22 at the initial stage and the end of the first turning gait and the second turning gait, and the gait cycle of each turning gait. (Time from the beginning to the end of each turning gait) and the like. Among the foot trajectory parameters, the initial free leg foot position / posture of the first turning gait is the current foot gait support leg foot position / posture as viewed from the next time's gait support leg coordinate system.

  In this case, in the running gait, the supporting leg foot 22 at the end of the current time's gait is moving in the air. Then, the current leg gait support leg foot position / posture from the current time gait initial support leg foot position / posture (= previous gait end free leg foot position / posture). According to the required value of the planned landing position / posture of the flat 22 (required value of the planned landing position / posture of the supporting leg foot 22 of the current time's gait 22) or the next gait supporting leg coordinate system corresponding to the required value. The foot position / posture trajectory (specifically, the trajectory seen from the next time's gait supporting leg coordinate system) is generated using the finite time settling filter until the end of the current time's gait. Is required.

  It should be noted that the next gait end free leg foot position / posture is the position of the foot 22 when the foot 22 is rotated in the pitch direction to lower the toe while the foot 22 is grounded from the position / posture to the horizontal posture. The posture is determined so as to match the position and posture of the next time gait supporting leg coordinate system. In other words, the free leg foot position / posture at the end of the next gait is grounded so as not to slip from the required landing position / posture value of the free leg foot 22 of the second step in the required parameter. The position and posture of the foot 22 in a state in which the toe is lifted and rotated by a predetermined angle in the pitch direction (a state where the toe is raised and the heel is landed).

  Further, the initial support leg foot position / posture of the first turning gait is the free leg foot position / posture of the current time gait viewed from the next time's gait support leg coordinate system. In this case, the current leg gait end free leg foot position / posture is the next time gait end free leg foot position / posture of the next time gait supporting leg coordinate system or one step of the required parameter corresponding thereto. This is determined according to the required value of the planned landing position / posture of the eye (current gait). That is, the free leg foot position / posture at the end of the gait this time is that the foot 22 is rotated from the position / posture while the foot 22 is grounded to lower the toe, so that the bottom surface of the foot 22 is almost the entire surface. Is determined so that the representative point of the foot 22 when it touches the floor surface matches the origin of the next time gait supporting leg coordinate system.

  The free leg foot position / posture at the end of the first turning gait is determined from the next time's gait supporting leg coordinate system in the same manner as the method for determining the free leg foot position / posture at the end of the current gait and the free leg foot position / posture at the end of the next gait. It is determined based on the position / posture of the next gait supporting leg coordinate system. More specifically, the free leg foot position / posture at the end of the first turning gait is rotated in the pitch direction from the position / posture to the horizontal posture so that the foot 22 does not slip while being grounded. The position / posture of the foot 22 at the time of being determined is determined so as to match the position / posture of the next time gait supporting leg coordinate system viewed from the next time gait supporting leg coordinate system.

  At the end of the first turning gait, the supporting leg foot 22 leaves the floor and is in the air. In order to determine a trajectory after the support leg foot 22 has left the floor, the first landing gait support leg foot planned position / posture is set. The planned landing position / posture of the first turning gait support leg foot is determined based on the position / posture of the next next time gait support leg coordinate system viewed from the next time gait support leg coordinate system. More specifically, the first landing gait support leg foot position / posture of the first turning gait is the position / posture of the next next time gait support leg coordinate system viewed from the next time gait support leg coordinate system. The next gait support leg coordinate system has a relative position / posture relationship between the next gait support leg coordinate system and the next next gait support leg coordinate system. It is determined so as to coincide with the relative position and orientation relationship with the support leg coordinate system.

  As in the case of obtaining the initial supporting leg foot position / posture of the first turning gait, the supporting leg foot position / posture at the end of the first turning gait is determined from the initial supporting leg foot position / posture of the first turning gait. The foot position / posture trajectory (more specifically, the trajectory as seen from the next time's gait support leg coordinate system) to the end of the first turning gait It is calculated | required by producing | generating.

  The initial free leg foot position / posture of the second turning gait is the supporting leg foot position / posture at the end of the first turning gait as seen from the next time's gait supporting leg coordinate system. The initial supporting leg foot position / posture of the second turning gait is the free leg foot position / posture at the end of the first turning gait as seen from the next time gait supporting leg coordinate system.

  The free leg foot position / posture at the end of the second turning gait is the free leg foot position / posture at the end of the current time gait viewed from the current time's gait supporting leg coordinate system. The second turning gait end support leg foot position / posture is the current time gait end support leg foot position / posture viewed from the current time gait support leg coordinate system.

  The gait cycles of the first turning gait and the second turning gait are set to be the same as the next time gait cycle. The gait cycles of the first turning gait and the second turning gait do not necessarily need to be the same, but it is preferable to determine either cycle according to at least the next gait cycle.

  Next, proceeding to S102, the gait generator 100 determines a body posture trajectory parameter that defines a target body posture trajectory in the normal gait.

  In this case, in this embodiment, the body posture trajectory parameters of the normal gait are determined so that the target body posture trajectory defined thereby matches the reference body posture trajectory of a predetermined pattern. The In this embodiment, the reference body posture is set to a fixed posture (fixed posture) that does not change with time. The reference body posture is, for example, a posture in which the trunk axis of the body 24 is steadily oriented in the vertical direction (a body posture in a state where the robot 1 stands upright), that is, a body posture angle with respect to the vertical direction. Is a posture that is constantly maintained at “0”. Then, parameters that define the trajectory of the reference body posture (such as a value of a constant body posture angle of the reference body posture) are determined as the body posture trajectory parameters. When the body posture is set to a constant posture in this way, the angular velocity and angular acceleration of the body posture angle are inevitably kept at “0”.

  Supplementally, the body posture of the normal gait is connected at the initial stage of the normal gait (initial stage of the first turning gait) and the end (end of the second turning gait) (body posture angle and its angular velocity). As long as they match each other at the beginning and end of the normal gait. In the present embodiment, the reference body posture is set to a constant posture as described above in order to facilitate understanding of the present embodiment.

  Next, proceeding to S104, the gait generator 100 determines the arm posture trajectory parameters, more specifically, the arm posture trajectories other than those related to the angular momentum change of both arms around the vertical axis (or trunk axis of the upper body 24). Parameters are determined. For example, parameters that define the relative height of the hand of the arm body with respect to the upper body 24 and the relative center of gravity position of the entire arm body are determined as the arm posture trajectory parameters. In this case, in the present embodiment, the relative center-of-gravity position of the entire arm body is set to be kept constant with respect to the upper body 24.

  Next, proceeding to S106, the gait generator 100 determines a floor reaction force vertical component trajectory parameter. In this case, the floor reaction force vertical component trajectory parameter is set so that the floor reaction force vertical component trajectory defined by the parameter is substantially continuous in both the first turning gait and the second turning gait. Is determined.

  Specifically, the desired floor reaction force vertical component trajectory of the normal gait is set in a pattern as shown in FIG. 13, for example. In that pattern, in both the first turning gait and the second turning gait, the floor reaction force vertical component changes to a trapezoid during the one-leg support period, and the floor reaction force vertical component is maintained at zero in the aerial period. The Then, the break point time of the pattern and the height (peak value) of the trapezoidal portion are determined as the floor reaction force vertical component trajectory parameters.

  This floor reaction force vertical component trajectory parameter is the average value of the floor reaction force vertical component during a period of one cycle of the normal gait (a period combining the period of the first turning gait and the period of the second turning gait). Is made to coincide with the own weight of the robot 1. That is, the average value of the floor reaction force vertical component in the period of one cycle of the normal gait is set to be the same magnitude as the gravity acting on the entire robot 1 and in the opposite direction.

  As described above, it is necessary to determine the floor reaction force vertical component trajectory parameter (and hence the floor reaction force vertical component trajectory) in order to satisfy the condition of the normal gait. The condition of the normal gait is the initial state (support leg of the first turning gait of the normal gait) for all the state variables of the normal gait (motion state quantities such as the position, posture, speed, etc. of each part of the robot 1). The initial state of the first turning gait viewed from the coordinate system) and the terminal state (the second turning gait viewed from the supporting leg coordinate system of the first turning gait following the second turning gait of the normal gait). Terminal condition) (hereinafter, this condition may be referred to as a boundary condition of a normal gait).

  Therefore, the difference between the total center of gravity vertical velocity of the robot 1 at the end of the normal gait and the total center of gravity vertical velocity at the beginning of the normal gait (specifically, the total center of gravity vertical velocity at the end of the second turning gait and the initial first turning gait The difference from the overall center of gravity vertical velocity) must also be zero. Since the difference is an integral value of the difference between the floor reaction force vertical component and gravity (first-order integral value in a period of one cycle from the initial stage to the end of the normal gait), the difference is made zero. As described above, it is necessary to determine the floor reaction force vertical component trajectory so that the average value of the floor reaction force vertical component in the period of one cycle of the normal gait coincides with the weight of the robot 1.

  In the present embodiment, the average value of the floor reaction force vertical component in each period of the first turning gait and the second turning gait is made to coincide with the own weight of the robot 1. More specifically, for example, after setting the time of the break point of the trapezoidal portion of the floor reaction force vertical component trajectory in each turning gait according to the gait cycle of the first turning gait and the second turning gait, The height of the trapezoidal portion (the peak value of the floor reaction force vertical component) is set so that the average value of the floor reaction force vertical component in each period of the first turning gait and the second turning gait coincides with the weight of the robot 1. Decided. In this case, the height of the trapezoidal portion can be obtained by solving the equation representing the coincidence condition of the average value and the dead weight with the height as an unknown.

  By doing this, the difference between the overall center-of-gravity vertical velocity at the end of the first turning gait and the overall center-of-gravity vertical velocity at the beginning of the first turning gait becomes “0”, and the overall center-of-gravity vertical velocity at the end of the second turning gait. And the difference between the overall center-of-gravity vertical velocity at the beginning of the second turning gait is also “0”. However, there is no necessity to do this. For example, when there is a possibility that the vertical position of the body is too high or too low near the boundary between the first turning gait and the second turning gait, the average value and the weight are calculated for each turning gait. The height of the trapezoid of the floor reaction force vertical component trajectory of each turning gait may be corrected from the matched state.

  Next, proceeding to S108, the gait generator 100 determines a ZMP trajectory parameter that defines a target ZMP trajectory of a normal gait combining the first turning gait and the second turning gait. In this case, the target ZMP trajectory is determined such that the stability margin is high and does not change suddenly as described above.

  More specifically, in the running gait of FIG. 5, after the heel of the supporting leg foot 22 (the free leg foot 22 in the air period) has landed, after a while, the bottom surface of the supporting leg foot 22 is almost the same. The entire surface is grounded, and after a while, only the toes of the supporting leg foot 22 are grounded. Next, the robot 1 kicks with the toes of the supporting leg foot 22 and jumps into the air, and finally lands on the heel of the free leg foot 22. Also, the target ZMP must be in the ground plane.

  Therefore, in the present embodiment, the X-axis direction position of each target ZMP of the first turning gait and the second turning gait of the normal gait is as shown in FIG. With the heel as the initial position, it is determined so that almost the entire bottom surface of the foot 22 stays at that position until it contacts the ground.

  Subsequently, the target ZMP moves to the center of the support leg foot 22 and moves to the toe until the foot 22 reaches the toe grounding state, and then stays on the toe of the support leg foot 22 until the time of getting off the floor. To be determined. Thereafter, as described above, the target ZMP continuously moves from the toes of the supporting leg foot 22 to the landing position of the heel of the free leg foot 22 until the landing of the next free leg foot 22 as described above. To be decided.

  Therefore, the target ZMP trajectory (the trajectory in the X-axis direction) of the normal gait composed of the first turning gait and the second turning gait is determined as shown in FIG. Then, the break time and position of the target ZMP trajectory are determined as the ZMP trajectory parameters of the normal gait. In this case, the breakpoint time is determined according to the gait cycle of the first turning gait and the second turning gait determined according to the required parameter. In addition, the position of the break point is the position of the next gait support leg coordinate system and the next time gait support leg coordinate system, or the positions of the first and second steps that define the position and orientation of these coordinate systems. It is determined according to a required value of the leg-side foot landing planned position / posture.

  Note that the position of the target ZMP trajectory in the Y-axis direction is determined in the same manner as that shown in FIG. More specifically, the trajectory of the position of the target ZMP in the Y-axis direction in the first turning gait is determined in the same pattern as that in FIG. Furthermore, the trajectory of the position of the target ZMP in the Y-axis direction in the second turning gait is determined to be the same shape as that of the first turning gait and connected to the end of the trajectory.

  The above is the details of the processing in S022 of FIG.

  Returning to FIG. 9, the gait generator 100 executes the process of S022 as described above, and then proceeds to S024 to calculate the initial state of the normal gait. Specifically, the initial state calculated here is the initial body position speed (initial body position and initial body speed) and the initial divergence component of the normal gait. The calculation of the initial state is exploratoryly performed by the process shown in the flowchart of FIG.

  To describe below, first, in S200, the gait generator 100 determines the desired foot position / posture and target arm posture of the normal gait based on the normal gait parameters (gait parameters determined in S022 of FIG. 9). , And the initial state of the desired body posture angle (the state at the initial time of the normal gait (= the end time of the current time's gait)) is determined. Here, the “state” means a set of a position or posture and a temporal change rate (change speed of the position or posture). For example, the initial state of the desired foot position / posture means a set of the foot position / posture at the initial time of the normal gait and the temporal change rate (that is, the moving speed of the foot 22 and the changing speed of the posture). To do. The same applies to the initial state of the target arm posture and the initial state of the target body posture angle.

  In this case, the initial state of the foot position / posture on the supporting leg side and the initial state of the foot position / posture on the free leg side are determined based on the foot trajectory parameters determined in S100 of FIG.

  Specifically, among the foot trajectory parameters, the initial foot position / posture of the first turning gait initial supporting gait and the foot position / posture of the initial swing gait initial free leg are the initial values of the normal gait. The foot position / posture on the supporting leg side at the time and the foot position / posture on the free leg side are determined.

  Further, the temporal change rate of the foot position / posture on the supporting leg side at the initial time of the normal gait is the first leg position / posture position / posture leg position / posture at the end of the second turning gait. Foot position / posture trajectory (trajectory viewed from the next time's gait supporting leg coordinate system) using the finite time settling filter, the foot position / posture at the initial time of the trajectory (initial time of the normal gait) Calculated as the rate of change over time. In this case, from the initial position of the normal gait to the time immediately after it in the foot position / posture trajectory from the initial supporting leg foot position / posture at the first turning gait to the free leg foot position / posture at the end of the second turning gait. The temporal change rate of the foot position / posture on the supporting leg side at the initial time of the normal gait can be calculated from the trajectory.

  In addition, the temporal change rate of the foot position / posture on the free leg side at the initial time of the normal gait is changed from the initial supporting leg foot position / posture of the current time's gait to the free leg foot position / posture at the end of the first turning gait. Foot position / posture trajectory (trajectory viewed from the next time's gait supporting leg coordinate system) using a finite time settling filter, foot position / posture time at the initial time of the trajectory (initial time of the normal gait) Calculated as the rate of change. In this case, of the foot position / posture trajectory from the current foot gait initial supporting leg foot position / posture to the first swing gait end free leg foot position / posture, the initial time of the current time gait to the initial time of the normal gait. If the trajectory up to (or the time immediately after) is generated, the temporal change in the foot position and posture on the free leg side from the trajectory (the trajectory near the initial time of the normal gait) to the initial time of the normal gait The rate can be calculated.

  Further, the initial state of the arm posture is based on the arm posture trajectory parameter determined in S104 of FIG. 11 and the arm posture at the initial time of the normal gait (the position of the center of gravity of both arms relative to the upper body 24) and the initial state. It is determined by obtaining the change amount of the arm posture in the period immediately after the time.

  Further, the initial state of the body posture angle is based on the body posture trajectory parameter determined in S102 of FIG. 11 and the body posture angle at the initial time of the normal gait and the body posture angle in the period immediately after the initial time. It is determined by calculating the amount of change. In the present embodiment, the body posture defined by the body posture trajectory parameter is a posture in which the trunk axis of the body 24 is steadily oriented in the vertical direction, and therefore, the upper body posture at the initial time of the normal gait is determined. The body posture angle and its angular velocity are both “0”.

  Next, in S202, the gait generator 100 is an initial body horizontal position speed candidate (horizontal position and horizontal speed candidate of the body 24 at the initial time of the normal gait) (Xs, Vxs) (Xs: horizontal Position, Vxs: Horizontal speed) is provisionally determined. The candidates (Xs, Vxs) to be provisionally determined here may be arbitrary. For example, they may be provisionally determined as candidates for the body horizontal position speed (Xs, Vxs) in the initial state of the normal gait obtained when generating the previous gait. That's fine.

  In order to simplify the description, a case where an initial state (initial body horizontal position speed) of a normal gait in the X-axis direction (roll axis direction) is searched on the sagittal plane will be described as an example. However, in actuality, in any of the position and speed, the initial state of the normal gait (the normal gait described above) is separately or simultaneously in the X-axis direction (roll axis direction) and the Y-axis direction (pitch axis direction). It is necessary to search for an initial state satisfying the boundary condition.

  As an exploratory determination method, a pseudo Jacobian (sensitivity matrix) is obtained, and the next candidate is determined by the steepest descent method or the simplex method. In the present embodiment, for example, the steepest descent method is used.

  Next, the process proceeds to S206 via S204, and the initial body vertical position velocity which is a set of the vertical position (Z-axis direction position) Zs and the vertical velocity (Z-axis direction velocity) Vzs of the body 24 at the initial time of the normal gait. (Zs, Vzs) is determined.

  The initial body vertical velocity Vzs is determined in the present embodiment as follows, for example.

  The following formula is established for the robot 1 as a dynamic relationship.

Terminal total center-of-gravity vertical position-initial total center-of-gravity vertical position = (floor reaction force vertical component / total mass of robot) second-order integration + second-order integration of gravity acceleration + initial total center-of-gravity vertical speed * time of one step
... Formula 13
(However, gravity acceleration is a negative value.)
Further, in the normal gait, the terminal overall center-of-gravity vertical position matches the initial overall center-of-gravity vertical position, so the right side of the above equation 13 must be zero. Accordingly, the initial overall center-of-gravity vertical velocity can be obtained from these relationships. Specifically, first, a value obtained by dividing the floor reaction force vertical component calculated by the floor reaction force vertical component trajectory parameter determined in S104 of FIG. 11 by the total mass of the robot 1 is terminated from the initial stage of the normal gait. By performing the second-order integration in the period until, the total center-of-gravity movement amount (the first term on the right side of Equation 13) by the floor reaction force vertical component is obtained.

  Furthermore, the total center-of-gravity movement amount (the second term on the right side of Equation 13) due to gravity is obtained by second-order integration of the gravitational acceleration during the period from the beginning to the end of the normal gait. Then, the result obtained by inverting the sign of the sum of the total center of gravity movement due to the floor reaction force vertical component and the total center of gravity movement due to gravity, obtained as described above, is divided by the time of one cycle Tcyc of the normal gait. The initial overall center-of-gravity vertical velocity is determined.

  In the present embodiment, for example, a multi-mass point model (geometric model) having mass points on the upper body 24 and other parts (legs 2 and 2 and arms) is used, and the center of gravity point of the multi-mass point model is used. The vertical velocity of the body 24 such that the vertical velocity coincides with the initial overall center-of-gravity vertical velocity is determined in S202 (or S218 described later) and the initial state such as the foot position / posture and arm posture determined in S200. Based on the initial body horizontal position velocity (Xs, Vxs), the obtained vertical velocity is determined as the initial body vertical velocity Vzs.

  As the multi-mass point model, for example, one upper mass point having the mass of the upper body 24 (or the total mass of the upper body 24 and the arm) and two masses having the respective masses of both legs 2 and 2 are used. There is a three-mass model composed of leg mass points. In this case, for example, the position of the body mass point is defined according to the position and orientation of the body 24, and the position of each leg mass point is defined according to the foot position and orientation of each leg 2. When such a three mass point model is used, the vertical velocity of the upper mass point can be calculated based on the initial overall center-of-gravity vertical velocity obtained as described above and the initial state of the foot position / posture. The initial body vertical velocity Vzs can be determined based on the vertical velocity of the body mass point and the initial state of the body posture angle determined in S200.

  Note that the initial body vertical velocity Vzs may be determined more accurately by using a multi-mass point model having more mass points (for example, a geometric model having a mass point at each link of the robot 1). Further, when the mass of the part other than the upper body 24 of the robot 1 is sufficiently smaller than the upper body 24, for example, the vertical velocity of the entire center of gravity of the robot 1 substantially matches the vertical velocity of the upper body 24. Therefore, the initial overall center-of-gravity vertical velocity may be determined as the initial body vertical velocity Vzs as it is.

  On the other hand, the initial body vertical position Zs of the normal gait is determined using, for example, the body height determination method previously proposed by the present applicant in Japanese Patent Laid-Open No. 10-86080. At this time, the foot position / posture at the initial time (the first foot position / posture of the first support gait initially determined in S100 of FIG. 11 and the foot position / posture of the first swing gait initial free leg) and each leg The initial body vertical position Zs at which the knees of the legs 2 and 2 at the initial time are not fully extended is determined on the basis of the predetermined geometric condition relating to the bending angle of the knee 2. For example, when the knee bending angle of the supporting leg side leg 2 is θsup and the knee bending angle of the free leg side leg 2 is θswg, there is a predetermined sum of reciprocal values of sine function values of the knee bending angles θsup and θswg. The initial body vertical position is determined so as to be a value (finite value). Here, the knee bending angles θsup and θswg are angles of the axis of the lower leg with respect to the axis of the thigh of each leg 2, and the knee is bent from the state where each leg 2 is fully extended. Accordingly, the angle increases from “0”. Note that such a method for determining the vertical position of the body 24 is described in detail in the above-mentioned Japanese Patent Application Laid-Open No. 10-86080, so only the above description is given here.

  Supplementally, since the robot 1 has 6 degrees of freedom per leg, if the initial state of the foot position / posture trajectory and the body position / posture trajectory (position / posture and its temporal change rate) is given, the robot 1 The initial state of motion of the upper body 24 and the legs 2 and 2 is uniquely determined. Therefore, for example, when the entire mass of the upper body 24 and the legs 2 and 2 substantially matches the entire mass of the robot 1, the initial state of motion of the upper body 24 and the legs 2 and 2 is given. If determined, the initial overall center-of-gravity vertical velocity is also uniquely determined. Conversely, since there are 6 degrees of freedom per leg, even if one of the speed-related states (for example, initial body vertical speed) is undecided among the initial states of the foot position / posture trajectory and the body position / posture trajectory. If the initial overall center-of-gravity vertical velocity is given, the undetermined initial state is uniquely determined.

  After the process of S206, the process then proceeds to S208, and a gait as a normal gait candidate (a gait for one normal gait (one cycle)) is temporarily generated. More specifically, based on the normal gait parameters determined in S022 of FIG. 9, the desired ZMP, the desired floor reaction force vertical component, the desired foot position / posture, the desired body at each moment from the initial time to the end time. Instantaneous values of posture and target arm posture are obtained sequentially. Then, by sequentially determining the body position using the dynamic model (inverted pendulum model) shown in FIG. 10 so as to satisfy the obtained target ZMP and the desired floor reaction force vertical component, A gait from the initial time to the end time is generated. In this case, the initial body horizontal position speed (Xs, Vxs) and the initial body vertical position speed (Zs, Vzs) are set as the initial state of the position and speed of the body 24.

  The gait generation is only performed inside the gait generation device 100 and is not output from the gait generation device 100 as a target value for driving the actual robot 1.

  Specifically, the process of S208 is executed as shown in the flowchart of FIG.

  In the following, the gait generator 100 first performs various initializations in S300. Specifically, the gait generation time Tk is initialized to the initial time Ts of the normal gait. Further, the latest candidate value of the initial body horizontal position speed (Xs, Vxs) (latest candidate value determined in S202 of FIG. 15 or S216 or S218 described later) in FIG. The latest value of the initial body vertical position velocity (Zs, Vzs) (the latest value determined in S206 of FIG. 15) is substituted into the body vertical position velocity. The initial value of the reference body posture angle is substituted for the body posture angle, and the initial value of the reference body posture angular velocity is substituted for the body posture angular velocity.

  Next, in S304 via S302, the gait generator 100 determines whether the gait generation time Tk (current value) is a time before the end time Te (= Ts + Tcyc) (Tk ≦ Te). ). If the determination result is affirmative, the gait generator 100 determines the instantaneous value of the gait at time Tk by executing the processes of S306 to S316 (details will be described later).

  Next, in S318, the gait generator 100 increases the gait generation time Tk by a predetermined step time ΔTk, and then performs the determination in S304 again. Here, the step time ΔTk may be set to coincide with the control cycle Δt, for example. However, in order to reduce the amount of calculation, ΔTk may be set to a time longer than Δt.

  If the determination result in S304 is affirmative, the processes from S306 to S318 are repeated. If the determination result in S304 is negative, the process in FIG. 16, that is, the process in S208 in FIG. Ends. Thereby, generation of a normal gait for one cycle from the initial time to the end time of the normal gait (temporary normal gait) is completed.

  The gait generator 100 executes the processing for determining the instantaneous value of the temporary normal gait in S306 to S316 as follows. In step S306, the gait generator 100 first determines the instantaneous value of the desired floor reaction force vertical component trajectory shown in FIG. 13 at time Tk based on the normal gait parameters (more specifically, floor reaction force vertical component trajectory parameters). Ask for.

  Further, in S308, the gait generator 100 obtains an instantaneous value at the time Tk of the target ZMP trajectory shown in FIG. 14 based on the normal gait parameter (more specifically, the ZMP trajectory parameter).

  Next, in S308, the gait generating device 100 determines the desired foot position / posture (supporting leg side) based on the normal gait parameters (more specifically, the foot trajectory parameters, the body posture trajectory parameters, and the arm posture trajectory parameters). And the desired foot position / posture on both the free leg side), the desired body posture and the desired arm posture at the time Tk. However, regarding the target arm posture, the position of the center of gravity of both arms is determined in more detail, but the movement of the arm (arm swing) that changes the angular momentum around the vertical axis (or the trunk axis of the upper body 24). Exercise) is not yet determined.

  Next, in S310, the gait generator 100 satisfies the desired floor reaction force vertical component obtained in S306 (the sum of the vertical inertia force and gravity of the entire center of gravity of the robot 1 is used as the desired floor reaction force vertical component). The body vertical position at time Tk is calculated as follows.

  Specifically, using the dynamic relational expression expressed by the following expressions 15 and 16 (an expression obtained by discretizing Newton's equation of motion in the vertical direction), the total center-of-gravity vertical velocity and the total center-of-gravity vertical position Is calculated.

Total center-of-gravity vertical velocity at time Tk = total center-of-gravity vertical velocity at time (Tk−ΔTk) + ((floor reaction force vertical component at time Tk / total mass of robot) + gravity acceleration) * ΔTk
(However, gravity acceleration is a negative value.)
... Formula 15

Overall center of gravity vertical position at time Tk = Overall center of gravity vertical position at time (Tk−ΔTk) + Overall center of gravity vertical speed at time Tk * ΔTk
... Formula 16

In this embodiment, the gait generator 100 is based on the overall center-of-gravity vertical position at the time Tk calculated as described above, the desired foot position / posture, and the reference body posture (target body posture). For example, the body vertical position is obtained using the above-described three-mass model (the three-mass model described with respect to the process of S206). In this case, the positions of the two leg mass points in the three mass model are determined based on the desired foot position / posture. Further, the vertical position of the body mass point is determined such that the vertical position of the overall center of gravity in the three mass point model matches the overall center of gravity vertical position at time Tk obtained as described above. Then, the body vertical position is determined from the vertical position of the body mass point and the target body posture (reference body posture).

  Note that the body vertical position may be obtained more accurately by using a multi-mass point model having more mass points (for example, a model having a mass point at each link of the robot 1). In addition, when the mass of a part other than the body 24 is sufficiently smaller than the body 24, the overall center-of-gravity vertical position simply corresponds to the vertical position of the center of gravity of the body 24. Therefore, the body vertical position may be determined from the overall center-of-gravity vertical position and the target body posture (reference body posture).

  Next, the process proceeds to S314, where the gait generator 100 satisfies the target ZMP (dynamics that the horizontal component of the moment generated by the resultant force of the inertial force and gravity of the robot 1 around the target ZMP is “0”). Body horizontal acceleration at the time Tk (horizontal acceleration of the body 24) is determined.

  At this time, the instantaneous values (values at the current time Tk) of the foot position / posture, arm posture, body posture, and body vertical position of the normal gait (provisional normal gait) have been determined. If the upper body horizontal position is determined, the overall target motion of the robot 1 is determined except for the degree of freedom of motion of the arm that changes the angular momentum about the vertical axis. Therefore, if the horizontal position of the body is determined, all floor reaction forces are uniquely determined except for the moment of the floor reaction force around the vertical axis.

  In the present embodiment, the target floor reaction force vertical component and the target ZMP of the normal gait (temporary normal gait) are the floor reaction force vertical component trajectory parameters of the normal gait parameters determined in S022 of FIG. , Defined by the target ZMP trajectory parameters. Therefore, the floor reaction force that is subordinately determined according to the determination of the horizontal body position is only the floor reaction force horizontal component.

  Supplementally, since the robot 1 of the present embodiment has 6 degrees of freedom for each leg 2, each part of each leg 2 can be determined by determining the desired foot position / posture and the desired body position / posture. The position and orientation are also uniquely determined. The degree of freedom of movement of the arm that changes the angular momentum about the vertical axis is used to cancel the spin force, as will be described later.

  In S314, for example, the body horizontal acceleration is obtained using the above-described equation 04 related to the dynamic model (inverted pendulum model) of FIG. More specifically, the body mass point 24b at the current time Tk is calculated from the body vertical position at the current time Tk, the body horizontal position at the time (Tk−ΔTk), and the target body posture at the current time Tk. A vertical position and a horizontal position are determined. The body horizontal position at time Tk is estimated by interpolation based on the time series of the body horizontal position up to time (Tk−ΔTk) or the gait state at time (Tk−ΔTk). The horizontal body position may be used instead of the horizontal body position at time (Tk−ΔTk).

  Further, the value obtained by subtracting the gravity (= mb * g) acting on the upper body point 24b from the floor reaction force vertical component at the current time Tk is divided by the mass mb of the upper body point 24b, thereby obtaining the current time Tk. The vertical acceleration of the upper body point 24b is obtained.

  Then, the vertical position, the horizontal position, and the vertical acceleration of the body mass point 24b obtained as described above are substituted into Zb, Xb, d2Zb / dt2 of the above equation 04, and the horizontal position of the target ZMP at the current time Tk and By substituting the vertical position into Xzmp and Zzmp in the equation 04 and further solving the equation in which Mzmp_y in the equation 04 is set to “0” with respect to d2Xb / dt2, the upper body point horizontal acceleration d2Xb / at the current time Tk dt2 is calculated. Then, the body mass point horizontal acceleration d2Xb / dt2 is obtained as the body horizontal acceleration at the current time k.

  It should be noted that the body horizontal acceleration may be obtained in an exploratory manner so that the horizontal component of the floor reaction force moment around the target ZMP is set to “0” using a stricter dynamic model.

  Next, proceeding to S316, the gait generator 100 calculates the horizontal body position at the current time Tk by performing second-order integration of the horizontal body acceleration obtained in S314. More specifically, the body horizontal speed at the current time Tk is obtained by adding a value obtained by multiplying the body horizontal acceleration by the increment time ΔTk to the body horizontal speed at the time Tk−ΔTk. Further, the body horizontal position at the current time Tk is obtained by adding a value obtained by multiplying the body horizontal speed by the step time ΔTk to the body horizontal position at time Tk−ΔTk.

  The above is the details of the gait generation process executed in S306 to S316.

  After completing the process of S208 in FIG. 15, the process proceeds to S210, in which the gait generator 100 corresponds the terminal body horizontal position / velocity of the generated gait (assumed normal gait) to the support leg at that moment. The value is converted into a value seen from the support leg coordinate system (support leg coordinate system in which the X ′ ″ axis and the Y ′ ″ axis in FIG. 12 are the two horizontal axes), and the value is (Xe, Vxe). (Xe: end body horizontal position, Vxe: end body horizontal speed).

  Next, in S212, the gait generator 100 calculates the difference between the initial body horizontal position speed (Xs, Vxs) and the terminal body horizontal position speed (Xe, Vxe) as shown in the figure. This difference (Xs−Xe, Vxs−Vxe) is referred to as a body horizontal position velocity boundary condition error (errx, errv). Since the normal gait is a gait that satisfies the boundary condition, (Xs, Vxs) and (Xe, Vxe) need to match. Therefore, the body horizontal position velocity boundary condition error (errx, errv) needs to be zero or almost zero. In the present embodiment, (Xs, Vxs) in which the body horizontal position / velocity boundary condition error (errx, errv) is substantially zero is obtained as described below.

  Next, in S214, the gait generator 100 determines whether or not the body horizontal position / velocity boundary condition error (errx, errv) is within a predetermined allowable range (both errx and errv are allowable). Or not).

  If the determination result in S214 is negative, the process proceeds to S216. In S216, a plurality (two in the present embodiment) of initial value candidates (Xs + ΔXs, Vxs) and (Xs, Vxs + ΔVxs) are determined in the vicinity of (Xs, Vxs). Here, ΔXs and ΔVxs mean predetermined minute change amounts with respect to Xs and Vxs, respectively. Then, with each of these initial value candidates as the initial state of the body horizontal position speed, a normal gait (assumed normal gait) is generated using the gait parameters by the same process as in S208. Furthermore, the terminal body position speed of the generated normal gait is represented by the support leg coordinate system corresponding to the instantaneous support leg (the X ′ ″ axis and the Y ′ ″ axis in FIG. 12 are two horizontal axes). (Xe + ΔXe1, Vxe + ΔVxe1) and (Xe + ΔXe2, Vxe + ΔVxe2) converted to values viewed from the support leg coordinate system (1). Here, (Xe + ΔXe1, Vxe + ΔVxe1) means the terminal body position velocity corresponding to (Xs + ΔXs, Vxs), and (Xe + ΔXe2, Vxe + ΔVxe2) means the terminal body position velocity corresponding to (Xs, Vxs + ΔVxs). . In the normal gait (assumed normal gait) generation process in this case, the initial state of the state quantity other than the body horizontal position speed is, for example, the initial value candidate of the body horizontal position speed is (Xs, Vxs). What is necessary is just to set to the same case. Further, in S216, by the same process as in S210, the difference between each initial value candidate and the corresponding terminal body position velocity, that is, each of the initial value candidates (Xs + ΔXs, Vxs) and (Xs, Vxs + ΔVxs) is determined. The corresponding body horizontal position velocity boundary condition error is determined.

  Next, the process proceeds to S218, where the gait generator 100 is based on the body horizontal position velocity boundary condition error for each of (Xs, Vxs) and the initial value candidates (Xs + ΔXs, Vxs) and (Xs, Vxs + ΔVxs) in the vicinity thereof. The next initial value candidate of (Xs, Vxs) is determined by a search method. As the search method, a pseudo Jacobian (sensitivity matrix) is obtained, and the next candidate is determined by the steepest descent method or the simplex method. For example, the body horizontal position and the body horizontal speed are respectively initialized by the body horizontal position speed boundary condition error with respect to (Xs, Vxs) and initial value candidates (Xs + ΔXs, Vxs) and (Xs, Vxs + ΔVxs) in the vicinity thereof. A sensitivity matrix indicating the degree of change in the body horizontal position / velocity boundary condition error when the value candidate (Xs, Vxs) is slightly changed is obtained. Then, based on the sensitivity matrix, initial value candidates (Xs, Vxs) that further reduce the body horizontal position / velocity boundary condition error are determined. After the new initial value candidate (Xs, Vxs) of the body horizontal position speed is thus determined, the process returns to S206.

  As long as the determination result in S214 is negative, the gait generator 100 repeats the processes in S206 to S218. If the determination result in S214 is affirmative, the process exits the repeat loop (S204) and proceeds to S220. In this case, the assumed ordinary gait generated immediately before exiting the repeat loop of S204 is obtained as a normal gait that satisfies the boundary condition.

  In S220, initial initial body horizontal position velocity initial value candidates (Xs, Vxs) and initial body vertical position velocity (Zs, Vzs) in the assumed normal gait finally generated in S208 of the iteration loop of S204 are obtained. The initial body horizontal position speed (X0, Vx0) and the initial body vertical position speed (Z0, Vz0) of the normal gait to be obtained are determined.

  Next, in S222, the gait generator 100 calculates the normal gait initial divergent component (value of the divergent component at the initial time Ts of the normal gait) q0.

  Here, in this embodiment, the value of the gait divergent component is defined as described below. In S222, the normal gait initial divergent component q0 is calculated according to this definition.

  First, the equation 03 representing the relationship between the motion of the upper mass point 24b and the floor reaction force moment (the floor reaction force moment around the origin of the support leg coordinate system) in the inverted pendulum model of FIG. When expressed, the following equation 100 is obtained. Here, it is assumed that the relative variation (height dispersion of the body 24) of the height (vertical position) of the body 24 during movement of the robot 1 with respect to the average height is sufficiently small. Zb of the second term on the right side of Expression 03 is assumed to coincide with a constant value h set in advance as the average height of the upper mass point 24b. Further, here, the floor reaction force moment Mb_y of the above equation 03 is expressed as Min as an input moment to the inverted pendulum.

  By rewriting the expression 100 into the expression in the discrete time system, the state equation of the following expression 102 is obtained.

XVb (k + 1) = A (k) * XVb (k) + B (k) * Min (k) (Formula 102)

The variables with suffixes (k) and (k + 1) mean the value at the kth time and the value at the (k + 1) time in the discrete time system, respectively. In the following description, the kth time and the (k + 1) th time may be expressed simply as “time k” and “time k + 1”, respectively.

  XVb in Equation 102 is a state variable vector (vertical vector) having the horizontal position Xb of the upper mass point 24b and the horizontal velocity Vxb, which is the temporal change rate, as state variables, as defined in the proviso to Equation 100. is there.

  In addition, A (k) and B (k) in Equation 102 are respectively a quadratic square matrix (state transition matrix) and a quadratic vertical vector, and each component value is represented by the following Equations 104a to 104c, 106a to 106c are determined. ΔT is a discrete time step, and ω 0 is an angular frequency value defined by the following formula 108. Exp () is an exponential function of the base of natural logarithm.

  In this case, as is clear from the equations 104a to 104c, 106a to 106c and the equation 108, the component values of A (k) and B (k) are expressed as the vertical acceleration d2Zb / dt2 (the vertical direction of the upper mass point 24b). It depends on the value of motion acceleration d2Zb / dt2). Therefore, each component value of A (k) and B (k) is a value depending on the vertical inertial force (= −mb * d2Zb / dt2) of the upper mass point 24b, that is, a function value of the inertial force. . Since the gravity acting on the upper body point 24a is a constant value, the component values of A (k) and B (k) are, in other words, the inertial force in the vertical direction of the upper body point 24b. It is also a function value of the resultant force with gravity acting on the upper mass point 24b or a floor reaction force vertical component balanced with the resultant force.

  Supplementally, when the mass mb of the upper mass point 24b of the inverted pendulum matches the overall mass of the robot 1 as in this embodiment, the vertical inertial force of the upper mass point 24b and the upper mass point 24b The floor reaction force vertical component that balances the resultant force with the applied gravity coincides with the floor reaction force vertical component acting on the robot 1 (the translational floor reaction force vertical component of the total floor reaction force).

  In the running gait of FIG. 5, g + d2Zb / dt2> 0 in the one-leg support period, and g + d2Zb / dt2 = 0 in the aerial period.

  In the system represented by the state equation of the above equation 102, the initial value XVb (0) of the state variable vector XVb (the value of XVb at time k = 0 (time 0)) and from time 0 to time k−1. When the input moment Min (the horizontal component Mb_y of the floor reaction force moment) is given, the value XVb (k) of the state variable vector XVb at an arbitrary time k (> 0) is given by Will be given.

  Note that φ (k, j) defined in the proviso in Expression 110 is φ (k, j) ≡unit matrix when j = k.

  Next, paying attention to the behavior of the inverted pendulum model in the normal gait described above, the initial time Ts of the normal gait is k = 0, the end time Te (= Ts + Tcyc) of the normal gait is k = kcyc (however, , Kcyc≡Tcyc / ΔT), and the state variable vector XVb defined by the motion of the normal gait at the initial time Ts is XVb (0). Then, among the state variable vector XVb at the normal gait termination time Te (time of k = kcyc), the component depending on the value XVb (0) of the state variable vector at the initial time Ts of the normal gait is XVb (kcyc )far. At this time, the relationship between XVb (kcyc) and XVb (0) is expressed by the following expression 112 based on the above expression 110.

XVb (kcyc) = φ (kcyc, 0) * XVb (0) ...... Formula 112
However, φ (kcyc, 0) = A (kcyc-1) * …… * A (1) * A (0)

Further, two eigenvalues of the matrix φ (kcyc, 0) on the right side of the expression 112 are λ1 and λ2, and eigenvectors (vertical vectors) corresponding to the respective eigenvalues are (a11, a21) ^T , (a12, a22) ^T , A quadratic square matrix having these eigenvectors as the first column and the second column, respectively, is denoted as Γcyc, and a matrix obtained by diagonalizing φ (kcyc, 0) using this matrix Γcyc is denoted as Λ. That is, Λ is defined by the following expression 114. Hereinafter, the matrix Γcyc is referred to as a diagonalization matrix.

  From this expression 114 and the above expression 112, the following expression 116 is obtained.

Γcyc ⁻¹ * XVb (kcyc) = Λ * Γcyc ⁻¹ * XVb (0) (Formula 116)

Note that the matrix φ (kcyc, 0) to be diagonalized by the diagonalization matrix Γcyc is the initial time (time 0) of one cycle of the normal gait, as shown in the proviso of the equation 112. To state transition matrices A (0), A (1) at each time within the period from the time to the end time (time kcyc) (specifically, each time every time interval ΔT from time 0 to time kcyc-1). Since it is a product of A (kcyc-1), it is determined depending on the time series of the inertial force of the upper mass point 24b within one period of the normal gait. Therefore, the diagonalization matrix Γcyc is also determined depending on the time series of the inertial force of the upper mass point 24b within one period of the normal gait.

Here, a vector obtained by linearly transforming the state variable vector XVb (k) at an arbitrary time k by the inverse matrix Γcyc ⁻¹ of the diagonalization matrix Γcyc (hereinafter referred to as a transformed state variable vector) (p ( k), q (k)) ^T. That is, the conversion state variable vector (p (k), q (k)) ^T is defined by the following equation 118.

(p (k), q (k)) ^T ≡Γcyc ⁻¹ * XVb (k) ...... Formula 118

From the equation 118 and the equation 116, the following equation 120 is obtained.

(p (kcyc), q (kcyc)) ^T = Λ * (p (0), q (0)) ^T ...... Equation 120

In Equation 120, if λ1> 1, the absolute value of p (kcyc)> the absolute value of initial value p (0). If λ1 ≦ 1, the absolute value of p (kcyc) ≦ initial value p ( The absolute value of 0). Similarly, if λ2> 1, the absolute value of q (kcyc)> the absolute value of initial value q (0). If λ2 ≦ 1, the absolute value of q (kcyc) ≦ initial value q (0). It becomes.

  On the other hand, the normal gait in the present embodiment is a gait that alternately repeats the one-leg support period and the aerial period (running gait in FIG. 5), and thus is always maintained in a state where g + d2Zb / dt2 ≦ 0. In other words, a state where g + d2Zb / dt2 ≦ 0 and a state where g + d2Zb / dt2> 0 occur alternately may be considered. In such a normal gait, one of the two eigenvalues λ1 and λ2 of φ (kcyc, 0) is generally a value greater than “1” and the other is a value smaller than “1”.

Therefore, hereinafter, it is assumed that λ1 <1 and λ2> 1. That is, in the conversion state variable vector (p (k), q (k)) ^T , the first component p (k) corresponding to the eigenvalue λ1 smaller than “1”, the second component q (k ) Is a component corresponding to an eigenvalue λ2 larger than “1”. At this time, p (k) has a meaning as a state quantity of a motion component having convergence in a normal gait repeated infinitely, and q (k) has a divergent property in a normal gait repeated infinitely. It has a meaning as a state quantity of the movement component.

Therefore, in the present embodiment, the second component q (k) of the conversion state variable vector (p (k), q (k)) ^T defined by Expression 118 is defined as the divergent component. The divergent component q (k) defined in this way is a linear combination value of the state variables Xb (k) and Vxb (k). In this case, in the linear combination, the weighting coefficients respectively applied to Xb (k) and Vxb (k) are determined depending on the time series of the inertial force of the upper mass point 24b within one period of the normal gait. It will be a thing. The first component p (k) of the conversion state variable vector (p (k), q (k)) ^T defined by Expression 118 is referred to as a convergence component.

It should be noted that the convergence component p (k) and the divergence component q (k) defined by the equation 118 are more specifically expressed as the convergence component q (k) and the divergence component q (k) on the sagittal plane (in the X-axis direction). The convergence component and the divergence component corresponding to the state variable vector XVb having the horizontal position Xb and the horizontal velocity Vxb of the upper body point 24b as components, but similarly, the convergence component and the divergence component on the lateral plane are also Defined. Specifically, a lateral plane is obtained by replacing the respective components Xb and Vxb of the state variable vector XVb (k) on the right side of Expression 118 with the horizontal position and horizontal velocity of the upper mass point 24b in the Y-axis direction. The convergence and divergence components above are defined. In this case, the matrix Γcyc ^{−1 of} Expression 118 is the same for both the X-axis direction and the Y-axis direction.

  According to the definition of the divergent component q described above, in S222, the normal gait initial divergent component q0 is calculated as follows.

  In other words, the gait generator 100 is configured so that each time Tk for each step time ΔT within the period of one cycle from the initial time Ts to the end time Ts of the normal gait (k = 0 from the time k = 0 in the discrete time system). = The instantaneous value of the inertial force of the upper mass point 24b at each time k) up to the time of kcyc-1 is calculated based on the normal gait parameters. In this case, when the mass mb of the upper mass point 24b of the inverted pendulum matches the entire mass of the robot 1, the vertical inertia force of the upper mass point 24b in the normal gait and the upper mass point The resultant force with gravity acting on 24b balances the floor reaction force vertical component in the normal gait. Therefore, in the present embodiment, the gait generator 100 is based on the floor reaction force vertical component trajectory parameter of the normal gait parameters, and the floor reaction force vertical at each time within the period of one cycle of the normal gait. Calculate the instantaneous value of the component. The instantaneous value is the same as the value calculated in S306 of FIG. Then, the gait generator 100 divides each momentary value of the calculated floor reaction force vertical component by the mass mb of the upper mass point 24b (= the total mass of the robot 1) (g + d2Zb) of the equation 100. As the value of / dt2), the state transition matrix A (k) (k = 0, 1,..., kcyc−1) is calculated by the above-described equations 104a to 104c.

  In the running gait of FIG. 5, the floor reaction force vertical component does not take a negative value. Therefore, in practice, A (k) is calculated by one of the expressions 104a and 104b.

  Supplementally, the vertical inertia force (or vertical acceleration) of the upper body 24 of the robot 1 in a normal gait is calculated using a multi-mass model (geometric model) such as the above-described three-mass model, and the calculation is performed. By using the inertial force (or vertical acceleration) as the inertial force (or vertical acceleration) of the upper mass point 24b, the state transition matrix A (k) at each time (k = 0, 1,..., Kcyc−1 ) May be calculated. For example, the instantaneous value of the inertial force of the upper mass point 24b in the normal gait may be calculated using the three mass point model as follows. That is, based on the foot trajectory parameters of the normal gait, the instantaneous value of the inertial force in the vertical direction of the two leg mass points in the three mass point model is calculated, and the robot 1 is calculated based on the floor reaction force vertical component trajectory. The instantaneous value of the inertial force in the vertical direction of the total center of gravity is calculated. Then, a value obtained by subtracting an instantaneous value of the vertical inertial force of the two leg mass points from an instantaneous value of the vertical inertial force of the entire center of gravity is calculated as an instantaneous value of the vertical inertial force of the upper mass point 24b. . In this case, the resultant force (or g + d2Zb / dt2) of the inertial force in the vertical direction of the upper body point 24b and the gravity acting on the upper body point 24b can be a negative value.

  The gait generator 100 calculates kcyc A (k) (k = 0, 1,..., Kcyc−1) in one cycle period from the initial time Ts to the end time Ts of the normal gait as described above. After that, the matrix φ (kcyc, 0) (= A (kcyc-1) *... * A (1) * A (0)) on the right side of Expression 112 is calculated by multiplying these A (k). To do.

Furthermore, the gait generator 100 calculates the eigenvalues λ1 and λ2 of the matrix φ (kcyc, 0) and the corresponding eigenvectors (a11, a21) ^T and (a12, a22) ^T, and uses these eigenvectors. The diagonalization matrix Γcyc is determined in accordance with the proviso of the expression 114. The gait generator 100 calculates an inverse matrix Γcyc ⁻¹ of the diagonalization matrix Γcyc.

  Also, the gait generator 100 determines the initial time of the normal gait from the initial body horizontal position speed (X0, Vx0) of the normal gait determined in S220 and the initial state of the body posture angle of the normal gait. An initial upper body point position velocity that is a set of the horizontal position and the horizontal velocity of the upper body point 24b of the inverted pendulum at Ts is determined.

The gait generator 100 calculates the normal gait initial divergent component q0 based on the equation 118 from the inverse matrix Γcyc ⁻¹ obtained as described above and the initial upper body mass point horizontal position velocity. More specifically, the X-axis direction component (state quantity vector in the X-axis direction) and the Y-axis direction component (state quantity vector in the Y-axis direction) of the initial body mass horizontal position velocity are respectively multiplied by Γ ^−1. Thus, an initial divergence component q0 in each of the X-axis direction and the Y-axis direction is calculated.

  After calculating the initial divergent component q0 of the normal gait as described above, the process proceeds to S224, and the gait generator 100 converts the initial divergent component q0 of the normal gait into a value viewed from the current time gait supporting leg coordinate system. The gait generator 100 converts the initial body vertical position velocity (Z0, Vz0) into a value viewed from the current time's gait support leg coordinate system, and converts this to (Z0 ", Vz0 ").

  Supplementally, (Z0 ″, Vz0 ″) is the second view from the support leg coordinate system of the second turning gait (the support leg coordinate system having the X ″ axis and the Y ″ axis in FIG. 12 as two horizontal axes). It matches the body vertical position velocity at the end of the turning gait. Further, q0 ″ also coincides with the end divergent component of the second turning gait seen from the supporting leg coordinate system of the second turning gait. Therefore, using these properties, (Z0 ″, Vz0 ″) and q0 "May be calculated.

  Thus, the process of S024 in FIG. 9, that is, the subroutine process for obtaining the initial state of the normal gait is completed.

  Next, the process proceeds to S026 in FIG. 9, and the gait generator 100 determines (partially determines) a gait parameter of the current time's gait. More specifically, in S026, the process shown in the flowchart of FIG. 17 is executed.

  First, in S600, the gait generator 100 determines the foot gait parameters of the current time gait so that the foot position / posture trajectory of the current time gait is connected to the foot position / posture trajectory of the normal gait.

  Specifically, the initial free leg foot position / posture of the current time's gait (the initial free leg foot position / posture of the current time's gait) is the last leg support position of the last gait as viewed from the current time's gait support leg coordinate system. It is set to the posture (current free leg foot position / posture).

  This time's initial gait supporting leg foot position / posture (the initial supporting leg foot position / posture of the current time's gait). Leg foot position / posture).

  Also, the free leg foot position / posture at the end of the current time's gait is the next gait supporting leg coordinate system viewed from the current time's gait supporting leg coordinate system (Required attitude value). That is, the free leg foot 22 is kept in contact with the floor from the gait end free leg foot position / posture at this time, so that the foot 22 does not slip so that almost the entire bottom surface of the foot 22 touches the pitch direction by a predetermined angle. The foot position / posture at the end of the current time's gait is determined so that the representative point of the foot 22 when rotated is coincident with the origin of the next time's gait supporting leg coordinate system viewed from the current time's gait supporting leg coordinate system. Is done.

  At this time, at the end of the gait, the supporting leg foot 22 leaves the floor and is in the air. In order to determine the trajectory after the support leg foot 22 leaves the floor, first, the position / posture of the support leg foot 22 of the current time's gait at the end of the next gait, that is, the free leg foot position at the end of the next gait. Posture is determined. The next gait end free leg foot position / posture is the next gait support leg coordinate viewed from the current time's gait support leg coordinate (the request for the expected landing position / posture of the second free leg foot 22 related to the current time gait). Value). More specifically, in the next gait end free leg foot position / posture, from the position / posture, the foot 22 is almost entirely on the floor so that the foot 22 does not slip while keeping the foot 22 in contact with the floor. The representative point of the foot 22 when it is rotated by a predetermined angle in the pitch direction until it touches the ground is determined so as to coincide with the origin of the next time gait support leg coordinate viewed from the current time gait support leg coordinate.

  And the current time gait support leg foot position / posture is the foot position / posture trajectory from the current time gait initial support leg foot position / posture determined as described above to the next time gait end free leg foot position / posture, This time is obtained by generating the gait end using the finite time settling filter.

  Next, in step S602, the gait generator 100 determines the body posture trajectory parameters of the current time's gait. This body posture trajectory parameter is such that the body posture trajectory specified thereby is continuously connected to the body posture trajectory of the normal gait (the body posture angle and angular velocity at the end of the current gait are the normal gait respectively) The initial body posture angle and the angular velocity coincide with each other). In this case, in the present embodiment, the body posture defined by the body posture trajectory parameter of the current time's gait is the above-mentioned reference body posture (which is a constant posture), like the body posture of the normal gait. Posture in which the trunk axis of the upper body 24 faces the vertical direction).

  If the body posture trajectory of the current time's gait is set so as to be continuously connected to the body posture trajectory of the normal gait, the body posture trajectory of the current time's gait is set to change over time from the initial stage of the current time's gait. May be.

  Next, in step S604, the gait generator 100 determines the arm posture trajectory parameters of the current time's gait. The arm posture trajectory parameter is determined in the same manner as the arm posture trajectory parameter of the normal gait so that the arm posture trajectory of the current time gait is continuously connected to the arm posture trajectory of the normal gait.

  The arm posture trajectory parameter of the current time gait determined here is the change in angular momentum of both arms around the vertical axis (or the trunk axis of the upper body 24), similarly to the arm posture trajectory parameter of the normal gait. (For example, parameters defining the relative height of the hand of the arm body with respect to the upper body 24, the relative center of gravity position of the entire arm body, etc.).

  Next, proceeding to S606, the gait generator 100 uses the floor reaction force vertical component trajectory parameters of the current time's gait and the floor reaction force vertical component trajectory defined thereby is substantially continuous as shown in FIG. Decide to be in orbit. However, the floor reaction force vertical component trajectory parameters of the current time's gait are such that both the overall center-of-gravity vertical position velocity and the floor reaction force vertical component trajectory of the robot 1 of the current time's gait are continuously connected to the normal gait. To be determined.

  Specifically, first, the initial body vertical position velocity of the normal gait finally obtained in the process of S024 in FIG. 9 (processing to determine the initial state of the normal gait) was viewed from the current time's gait support leg coordinate system. Based on the converted values (Z0 ″, Vz0 ″), that is, (Z0 ″, Vz0 ″) obtained in S224 of FIG. 15, etc., the entire initial gait as viewed from the current time's gait supporting leg coordinate system The center of gravity vertical position speed is obtained.

  For example, the initial overall center-of-gravity vertical position velocity of the normal gait is obtained using the above-described three-mass model (the three-mass model described with respect to the processing of S206). In this case, based on the desired foot position / posture in the initial stage of the normal gait and its temporal change rate (change speed) as seen from the current time's gait supporting leg coordinate system, the position / speed of the two leg mass points in the three-mass model is Desired. Further, from the above (Z0 ″, Vz0 ″), the body posture angle (= reference body posture angle) at the beginning of the normal gait and its temporal change rate, the position / velocity of the body mass point in the 3-mass model is Desired. Then, from the positions and velocities of these three mass points, the centroid position and velocity of the three mass points are calculated as the initial overall centroid vertical position velocity of the normal gait.

  Note that the initial overall center-of-gravity vertical position velocity of the normal gait may be obtained more accurately by using a multi-mass point model having more mass points (for example, a geometric model having a mass point at each link of the robot 1). Good. When the mass of the part other than the body 24 is sufficiently smaller than the body 24, the above (Z0 ", Vz0") and the body posture angle in the initial stage of normal gait (= reference upper The initial overall center-of-gravity vertical position velocity of the normal gait may be obtained from the body posture angle) and the temporal change rate thereof.

  The initial total center-of-gravity vertical position speed of the normal gait thus determined is substituted into the terminal total center-of-gravity vertical position speed of Equation 13 and Equation 41 below, and the instantaneous value of the previous gait (more specifically, the previous step The total center-of-gravity vertical position and speed of the terminal end state of the gait supporting leg coordinate system) are substituted into the initial total center-of-gravity vertical position and speed of Expression 13 and Expression 41 below, and Expression 13 and Expression 41 The floor reaction force vertical component trajectory parameter of the current time's gait is determined so as to satisfy the above relationship. However, the integral values in Equation 13 and Equation 41 are the integral values during the period from the beginning to the end of the current time's gait.

Final total center-of-gravity vertical velocity-initial total center-of-gravity vertical velocity = first-floor integral of (floor reaction force vertical component / robot mass) + first-floor integral of gravity acceleration
... Formula 41
(However, gravity acceleration is a negative value.)

More specifically, first, at least two parameters out of floor reaction force vertical component trajectory parameters (such as break point time) that define the floor reaction force vertical component trajectory as shown in FIG. As a variable, the value of the unknown variable is determined by solving simultaneous equations consisting of Equation 13 and Equation 41.

  The floor reaction force vertical component trajectory parameters as unknown variables include, for example, trapezoidal height (peak value of floor reaction force vertical component) and width (single leg support period time) in the floor reaction force vertical component trajectory shown in FIG. ) Can be selected. In this case, the slope of both sides of the trapezoid in FIG. 6 is a value determined according to the current time's gait cycle or the like, or the break point of the floor reaction force vertical component trajectory excluding the time of transition from the one-leg support period to the air period Is a value determined according to the current time's gait cycle or the like. Supplementally, if there is one unknown variable, there is generally no solution that satisfies the simultaneous equations of Equation 13 and Equation 41.

  Supplementally, in this embodiment, the floor reaction force vertical component trajectory parameter of the current time's gait determined as described above corresponds to the vertical inertia force parameter in the present invention. Therefore, the vertical inertia force parameter determining means in the present invention is realized by the processing of S606.

  Next, in S608, the gait generator 100 determines that the ZMP trajectory of the current time's gait has a high stability margin and does not change suddenly. Time, position, etc.). For example, the ZMP trajectory parameters are provisionally determined so that the ZMP trajectory has a pattern as shown in FIG. However, the current time gait so that the ZMP trajectory of the current time gait is continuously connected to the ZMP trajectory of the normal gait (so that the position of the ZMP at the end of the current time gait coincides with the position of the ZMP at the beginning of the normal gait). The ZMP trajectory parameter is temporarily determined. In this case, in the running gait, the method of setting the time and position of the break point of the ZMP trajectory in the single leg support period may be the same as the method of setting the ZMP trajectory parameter of the normal gait described above. Then, the ZMP trajectory parameters may be set so that the target ZMP trajectory in the air period changes linearly and continuously from the start of the air period to the position of the ZMP at the beginning of the normal gait.

  Note that the ZMP trajectory parameter of the current time gait determined in S608 is provisionally determined and is corrected as described later. Hereinafter, the target ZMP of the current time gait defined by the temporarily determined ZMP trajectory parameters until the correction is completed is hereinafter referred to as a temporary target ZMP. The gait parameters of the current time gait including the provisionally determined ZMP trajectory parameters are referred to as provisional current time gait parameters.

  Returning to the description of FIG. 9, the gait generator 100 performs the process of S026 as described above, and then executes a process of correcting the gait parameters (specifically, ZMP trajectory parameters) of the current time gait in S028. To do. In this process, the ZMP trajectory parameter of the gait parameters is corrected so that the body position / posture trajectory of the current time's gait is made continuous with or close to the normal gait.

  This process is executed as shown in the flowchart of FIG.

  First, the process proceeds to S702 via S700, and the gait generator 100 generates a provisional current time gait up to the end time of the current time gait based on the provisional current time gait parameters including the provisionally determined ZMP trajectory parameters.

  In S702, the process shown in the flowchart of FIG. 19 is executed.

  As will be described below, the gait generator 100 first performs various initializations in S800. Specifically, the temporary current time gait generation time Tk is initialized to “0”. Also, the terminal state of the previous gait (more specifically, the target body position and target body speed at the end time of the previous gait, the target body posture angle and its angular velocity, the target foot position and posture, the target arm) A posture, etc.) converted to the current time's gait supporting leg coordinate system is set as an initial state of the provisional current time's gait.

  Next, in step S804 through step S802, the gait generator 100 determines whether the provisional current time gait generation time k (current value) is a time before the current time gait end time Tcurr (Tk ≦ Tcurr). Whether or not). If this determination result is affirmative, the gait generator 100 determines the instantaneous value of the gait at time Tk by executing the processing of S806 to S816 (details will be described later).

  Next, in S818, the gait generator 100 increases the provisional current time gait generation time Tk by a predetermined step time ΔTk, and then performs the determination in S804 again. Here, the step time ΔTk may be set to coincide with the control cycle Δt, for example. However, in order to reduce the amount of calculation, ΔTk may be set to a time longer than Δt.

  If the determination result in S804 is affirmative, the processing from S806 to S818 is repeated, and if the determination result in S804 is negative, the processing in FIG. 19, that is, the processing in S702 in FIG. Ends. Thereby, generation of the provisional current time gait from the initial time to the end time of the current time gait is completed.

  The gait generator 100 executes the process of determining the instantaneous value of the provisional current time gait in S806 to S816 as follows. The method of determining the instantaneous value of the temporary current time gait is the same as the method of determining the instantaneous value of the normal gait (provisional normal gait) in S306 to S316 of FIG.

  First, in step S806, the gait generating device 100 instantiates the desired floor reaction force vertical component trajectory shown in FIG. 6 at the time Tk based on the provisional current time gait parameter (more specifically, the floor reaction force vertical component trajectory parameter). Find the value.

  Next, in S808, the gait generator 100 at the time Tk of the target ZMP trajectory (provisional target ZMP trajectory) shown in FIG. 7 based on the provisional current time gait parameter (more specifically, the provisionally determined ZMP trajectory parameter). Find the instantaneous value of.

  Next, in step S810, the gait generating device 100 determines the desired foot position / posture and the target upper gait based on the provisional current time gait parameters (more specifically, the foot trajectory parameters, the body posture trajectory parameters, and the arm posture trajectory parameters). Instantaneous values of the body posture and the target arm posture at time Tk are obtained. However, regarding the target arm posture, the position of the center of gravity of both arms is determined in more detail, but the movement of the arm (arm swing) that changes the angular momentum around the vertical axis (or the trunk axis of the upper body 24). Exercise) is not yet determined.

  Next, in S812, the gait generator 100 satisfies the target floor reaction force vertical component obtained in S806 (the sum of the vertical inertia force and gravity of the entire center of gravity of the robot 1 is used as the target floor reaction force vertical component). The body vertical position at time Tk is calculated as follows. The calculation method may be the same as S312 (processing for calculating the instantaneous value of the body vertical position of the normal gait) in FIG. However, the multi-mass point model used in S812 may be different from that used in S312.

  Next, the process proceeds to S814, where the gait generator 100 satisfies the target ZMP (the dynamics that the horizontal component of the moment generated by the resultant force of the inertial force and gravity of the robot 1 around the target ZMP is “0”). Body horizontal acceleration at time Tk is determined so as to satisfy the general equilibrium condition). The method of calculating the body horizontal acceleration is the same as S314 in FIG. 16, and the body horizontal acceleration is calculated using, for example, the dynamic model (inverted pendulum model) in FIG.

  It should be noted that the body horizontal acceleration may be obtained in an exploratory manner so that the horizontal component of the floor reaction force moment around the target ZMP is set to “0” using a stricter dynamic model.

  Next, in S816, the gait generator 100 calculates the body horizontal position at the current time Tk by performing second-order integration of the body horizontal acceleration obtained in S814. This calculation method is the same as S314 in FIG.

  The above is the details of the process (provisional current time gait generation process) executed by the gait generator 100 in S702 of FIG.

  The gait generator 100 executes the processing of S702 as described above, and then executes the processing of S704.

  In S704, the gait generator 100 determines the end divergence component qe1 (divergence at the end of the provisional current time gait) based on the body horizontal position velocity (Xe, Vxe) at the end of the provisional current time gait generated in S702. The component qe1) is calculated.

Specifically, the gait generator 100 first determines the inverted pendulum model of FIG. 8 from (Xe, Vxe) and the body posture at the end of the provisional current time gait (reference body posture in the present embodiment). The horizontal position speed (horizontal position and horizontal speed) of the upper mass point 24b is determined. Then, the terminal divergent component qe1 of the provisional current time gait is calculated from the upper body point horizontal position velocity based on the equation 118 in the same manner as S222 in FIG. In this case, Γ ⁻¹ used for calculating qe1 is the same as that calculated in the process of S222.

  Next, the processing proceeds to S706, where the gait generator 100 determines the end point which is the difference between the terminal divergent component qe1 of the provisional current time gait obtained as described above and the initial divergent component q0 "of the normal gait obtained in S224 of FIG. A divergence component error errq (= qe1−q0 ″) is calculated.

  Further, proceeding to S708, the gait generator 100 determines whether or not the terminal divergence component error errq obtained as described above is within an allowable range (within a range near “0”), that is, errq is “0”. Alternatively, it is determined whether or not it is almost “0”. In detail, the determination in S708 is performed on the terminal divergence component error errq in each of the X-axis direction and the Y-axis direction. When the terminal divergence component error errq in both the X-axis direction and the Y-axis direction is within the allowable range, the determination result in S708 becomes affirmative, and the terminal divergence component error errq in any axial direction is allowable. If it is not within the range, the determination result of S708 is negative.

  If the determination result in S708 is negative, the process proceeds to S710, where the gait generator 100 sets the current temporary target ZMP trajectory as the trapezoidal pattern correction amount with a = Δa (Δa: a predetermined minute amount). Thus, using the target ZMP trajectory corrected as shown in FIG. 20, a temporary current time gait up to the end time is generated in the same manner as in S702. In other words, the gait generating device 100 generates a temporary current time gait again using a target ZMP trajectory obtained by correcting it instead of the temporary target ZMP trajectory used for generating the temporary current time gait in S702. . In this case, the current time gait parameters other than the target ZMP are the same as those used in S702.

  Referring to FIG. 20, “a” matches the terminal divergent component of the provisional current time gait with the initial divergent component of the normal gait as much as possible (and consequently the body horizontal position / posture trajectory of the current time gait is steady). Height of the trapezoidal pattern correction amount (hereinafter also referred to as ZMP correction amount) for correcting the temporary target ZMP trajectory (adding to the temporary target ZMP trajectory) in order to converge to the body horizontal position / posture trajectory of the gait) It is.

  In this case, in the present embodiment, the temporary target ZMP trajectory is corrected during a period in which substantially the entire bottom surface of the supporting leg foot 22 is in contact with the ground (the entire surface of the sole in contact with the ground). For this reason, the ZMP correction amount is set such that the ZMP correction amount ≠ 0 within the entire sole surface contact period, and the ZMP correction amount = 0 during the periods other than the entire sole surface contact period. In addition, the time of the break point of the ZMP correction amount of the trapezoid pattern is set in accordance with the time of the break point of the temporary target ZMP trajectory during the entire sole contact surface period. Note that a = Δa (Δa: a predetermined minute amount) in S710 is that the terminal divergence component error errq when the current temporary target ZMP trajectory is corrected by a minute amount by the ZMP correction amount of the trapezoid pattern is set. This is to observe changes.

  Supplementally, in the above-described processing of S710, the description has been made by taking the example of correcting the position of the target ZMP in the X-axis direction, but actually the correction of the position of the target ZMP in the Y-axis direction is also performed. The correction of the position in the Y-axis direction is performed so that the target ZMP trajectory during the sole full surface contact period is changed in a trapezoidal pattern from the temporary target ZMP trajectory, as in the correction of the position in the X-axis direction. In this case, Δa may be the same value in each axial direction, but may be set to different values.

  If the correction of the target ZMP trajectory in one of the X-axis direction and the Y-axis direction does not affect the terminal divergence component in the other axial direction, or if the effect is sufficiently small, Processing from S710 to S716, which will be described later, may be performed separately in the X-axis direction and the Y-axis direction.

  After generating the temporary current time gait in S710 as described above, the process then proceeds to S712, where the gait generating device 100 is the horizontal body position velocity (Xe2, Vxe2) at the end of the temporary current time gait obtained in S710. ), The terminal divergence component qe2 in this provisional current time gait is calculated in the same manner as in S704.

  In this embodiment, Δa is set to a predetermined small amount in S710. However, Δa can be varied so that Δa approaches “0” as the terminal divergence component error errq decreases by the repetitive calculation described below. It may be set automatically. However, generally, even if Δa is set to a constant, the terminal divergence component error errq can be kept within an allowable range by several iterations.

  Next, proceeding to S714, the gait generator 100 calculates the parameter sensitivity r (the rate of change in the terminal divergence component error with respect to Δa) by the equation in the figure. That is, the gait generator 100 calculates the parameter sensitivity r by dividing the difference (= qe2−qe1) between the terminal divergence component qe2 calculated in S712 and the terminal divergence component qe1 calculated in S704 by Δa. In this case, more specifically, for example, the parameter sensitivity r in the X-axis direction is calculated by dividing the X-axis direction component of (qe2−qe1) by Δa related to the ZMP correction amount in the X-axis direction. Further, the parameter sensitivity r in the Y-axis direction is calculated by dividing the Y-axis direction component of (qe2−qe1) by Δa related to the ZMP correction amount in the Y-axis direction.

  Next, in S716, the gait generator 100 reverses the sign of the value obtained by dividing the terminal divergence component error errq obtained in S706 by the parameter sensitivity r obtained in S714, a = −errq / r. Is the height a of the ZMP correction amount of the trapezoidal pattern, and the target ZMP obtained by correcting the temporary target ZMP pattern as shown in FIG. 20 by this ZMP correction amount is again determined as the temporary target ZMP. In this case, the height a of the ZMP correction amount is calculated separately for the X-axis direction and the Y-axis direction.

  In addition, when the correction of the target ZMP trajectory in one of the X-axis direction and the Y-axis direction affects the terminal divergence component in the other axial direction, each axial direction is considered in consideration of the influence. It is preferable to determine the height a of the ZMP correction amount at.

  Next, the gait generator 100 executes the processing from S702 again. At this time, as long as the determination result of S708 is negative, the above-described processing of S702 to S716 is repeated, and when the determination result of S708 becomes affirmative, the process repeats the loop (S700) and returns to S718. move on.

  By the loop processing of S702 to S716 described above, the ZMP correction amount that can make the terminal divergence component qe1 coincide with or substantially coincide with the initial divergence component q0 ″ of the normal gait (and thus the target ZMP trajectory of the current time gait). Exploratory search is required.

  In S718 next to the loop processing, the gait generator 100 finally uses the current temporary target ZMP trajectory (the temporary target ZMP trajectory set immediately before exiting the repeat loop in S700) as the target ZMP trajectory of the current time gait. The corresponding ZMP trajectory parameters are determined. Thereby, the processing of S028 in FIG. 9 (current gait parameter correction processing) ends.

  In the present embodiment, the ZMP correction amount is set to a trapezoidal pattern, but may be determined to be, for example, a triangular pattern or a pattern in which the curvature changes continuously.

  Returning to FIG. 9, after correcting the current time's gait parameter in S028 as described above, the process proceeds to S030, where the gait generating device 100 has a floor reaction around the target ZMP at each time from the initial time to the end of the current time's gait. The floor reaction force moment tolerance range parameter that defines the tolerance range of the force moment (more specifically, the horizontal component of the floor reaction force moment) is determined.

  Note that the floor reaction force moment divided by the floor reaction force vertical component corresponds to the amount of deviation of the ZMP (floor reaction force center point) from the target ZMP. Accordingly, the floor reaction force moment allowable range may be converted into a ZMP allowable range (floor reaction force center point allowable range) as a ZMP position allowable range, and a parameter defining the ZMP allowable range may be determined. .

  The floor reaction force moment allowable range determined in S030 is the allowable range of the actual floor reaction force moment around the target ZMP controlled by the control processing (compliance control) of the composite compliance operation determination unit 104. The floor reaction force moment allowable range will be described below.

  The compliance control by the processing of the composite compliance operation determination unit 104 controls the position and orientation of the foot 22 so that the floor reaction force moment generated around the target ZMP becomes the target floor reaction force moment for compliance control. When the compliance control is faithfully operated according to the target, the actual floor reaction force center point is a point shifted in the horizontal direction from the target ZMP by a value obtained by dividing the target floor reaction force moment by the floor reaction force vertical component. The actual floor reaction force center point cannot deviate from the ZMP existence possible range represented by a so-called support polygon (in a strict expression, the existence range of the actual floor reaction force central point). If the actual floor reaction force center point is too close to the boundary of the ZMP existence possible range, there is a problem that the original touch feeling of the foot 22 is lowered or the bottom surface of the foot 22 is lifted.

  The floor reaction force moment allowable range limits the floor reaction force moment for compliance control in order to prevent such problems. Therefore, it is desirable that the floor reaction force moment allowable range is determined so that the floor reaction force center point allowable range obtained by dividing this by the floor reaction force vertical component is included in the ZMP possible range.

  More specifically, the floor reaction force center point allowable range (ZMP allowable range) should be determined according to the support polygon. In this case, the floor reaction force center point allowable range can generally be set to a complicated shape. However, in this embodiment, in order to simplify the calculation, the floor reaction force center point allowable range is, for example, a rectangular region (a rectangle having sides parallel to the X axis direction and the Y axis direction) on the floor surface. . In this case, the floor reaction force moment allowable range obtained by converting the floor reaction force center point allowable range into the floor reaction force moment is set independently for the X-axis direction component and the Y-axis direction component.

  For example, when the support polygon and the target ZMP are in the situation shown in FIG. 21, the floor reaction force center point allowable range (ZMP allowable range) is set to be included in the support polygon as shown in the figure. Is done. At the same time, the floor reaction force center point allowable range (ZMP allowable range) is set so as to include the target ZMP.

  In addition, the support polygon shown as a stippled region in FIG. 21 illustrates the support polygon in the both-leg support period of the walking gait (the gait generation related to the walking gait will be described later). In this example, a state where the toe of the right foot 22R is grounded and the heel of the left foot 22L is grounded is schematically shown.

  In the one-leg support period in the walking gait or the running gait in FIG. 5, the support polygon is the ground contact surface (contact surface with the floor surface) of the support leg foot 22. In this case, the ZMP allowable range may be set so as to be included in the support polygon while being included in the target ZMP or substantially coincide with the support polygon. Further, in the aerial phase of the running gait in FIG. 5, the ZMP allowable range is, for example, a region having a width of 0 in both the X-axis and Y-axis directions around the target ZMP, that is, a point of the target ZMP itself (this is the area It means “0” area).

  Returning to FIG. 9, after the floor reaction force moment allowable range is determined in S030 as described above, or when the determination result in S016 is negative, the gait generator 100 next determines that the current time gait generator 100 in S032. An instantaneous value (an instantaneous value such as a desired foot position / posture of the current time's gait in the current control cycle) is determined. In S032, the current time gait instantaneous value is determined so that the model operation floor reaction force moment determined as described above by the posture stabilization control calculation unit 112 is generated around the target ZMP.

  Specifically, the gait instantaneous value is determined as shown in the flowchart of FIG. To explain below, the gait generator 100 first executes the current gait of the current time's gait by executing the same processes as in S1000 to S1006, S806 to S812 in FIG. 19 (or S306 to S312 in FIG. 16). Instantaneous values of the target floor reaction force vertical component, target ZMP, target foot position / posture, target body posture, target arm posture, and target body vertical position at time t (current control cycle) are calculated.

  These instantaneous values are calculated from S806 to S812 in FIG. 19 (or in FIG. 16) except that the current time gait parameter finally corrected in S028 in FIG. 9 is used as the gait parameter. This is the same as the processing from S306 to S312).

  Next, proceeding to S1008, the gait generator 100 determines the instantaneous value of the floor reaction force moment allowable range at the current time t of the current time gait based on the floor reaction force moment allowable range parameter determined in S030 of FIG. Ask. The instantaneous value of the floor reaction force moment allowable range is output from the gait generator 100 to the compensating total floor reaction force moment distributor 110 (see FIG. 3). Then, the model operation floor reaction force moment (the value of the current time t) calculated by the above equation 50 by the distributor 110 is given to the gait generator 100.

  Next, in S1010, the gait generator 100 causes the model operation floor reaction force moment given from the compensated total floor reaction force moment distributor 110 to be generated around the target ZMP (the inertial force and gravity of the robot 1). The body horizontal acceleration at the current time t of the current time's gait is determined so that the horizontal component of the moment generated by the resultant force is equal to the model operation floor reaction force moment.

  In other words, the moment of the current time's gait is increased so that the moment generated by the resultant inertial force and gravity of the robot 1 around the target ZMP coincides with the moment obtained by inverting the sign of the model operation floor reaction force moment. Body horizontal acceleration is determined.

  In this case, the body horizontal acceleration of the current time's gait is calculated using, for example, the dynamic model (inverted pendulum model) of FIG.

  Specifically, the body constitution at the current time t from the body vertical position at the current time t of the current time gait, the body horizontal position at the time (t−Δt), and the target body posture at the current time t. The vertical position and horizontal position of the point 24b are determined. The body horizontal position at time t is estimated by interpolation based on the time series of the body horizontal position up to time (t−Δt) or the gait state at time (t−Δt), and the estimation is performed. The horizontal body position may be used instead of the horizontal body position at time (t−Δt).

  Further, by dividing the value obtained by subtracting the gravity (= mb * g) acting on the upper body point 24b from the floor reaction force vertical component at the current time t of the gait this time, by the mass mb of the upper body point 24b. Then, the vertical acceleration of the upper mass point 24b at the current time t is obtained.

  Then, the vertical position, horizontal position, and vertical acceleration of the body mass point 24b obtained as described above are respectively substituted into Zb, Xb, d2Zb / dt2 of the above equation 04, and the horizontal position of the target ZMP at the current time t and Substituting the vertical position into Xzmp and Zzmp in equation 04, and further solving the equation for Mzmp_y in equation 04 with the model operation floor reaction force moment for d2Xb / dt2, The body point horizontal acceleration d2Xb / dt2 is calculated. Then, the body mass point horizontal acceleration d2Xb / dt2 is obtained as the body horizontal acceleration at the current time t.

  It should be noted that a more rigorous dynamic model may be used to search for a body horizontal acceleration that matches the horizontal component of the floor reaction force moment around the target ZMP with the model operation floor reaction force moment.

  Next, in S1012, the horizontal body position at the current time t is calculated by performing second-order integration of the horizontal body acceleration obtained as described above in S1010. This calculation method is the same as S314 in FIG. 16 (or S816 in FIG. 19). Thereby, the process of S032 of FIG. 9 is completed.

  Next, the process proceeds to S034, and the arm body motion for canceling the spin force (the floor reaction force moment vertical component generated around the target ZMP by the motion other than the arm body of the robot 1 is set to “0” or almost “0”). Is determined. Specifically, the floor reaction force moment vertical component trajectory in the target ZMP when the arm is not shaken (strictly speaking, when the gait is generated without shaking the arm, the resultant force of the robot's gravity and inertial force) Is obtained by inverting the sign of each instantaneous value of the moment vertical component trajectory acting on the target ZMP). That is, the instantaneous value of the vertical component of the floor reaction force moment around the target ZMP that matches the instantaneous value of the motion of the current time's gait generated by the processing of S032 (this does not include arm swing motion) is obtained. Then, by dividing the instantaneous value of the vertical component of the floor reaction force moment by the equivalent inertia moment of the arm swing motion, the angular acceleration of the arm swing motion necessary for the spin force cancellation is obtained. If the arm swing is too large, the instantaneous value of the floor reaction force moment vertical component may be divided by a value larger than the equivalent inertia moment.

  Next, the gait generator 100 performs second-order integration of this angular acceleration, and sets the angle obtained by passing this angular acceleration through a low-cut filter for preventing the integrated value from becoming excessive as the arm swinging operation angle. However, in the arm swing operation, the left and right arm bodies are swung in the reverse direction, so that the positions of the center of gravity of both arm bodies are not changed. Note that an arm swing motion for canceling the spin force may be generated even in a normal gait, and the arm swing motion in the current time gait may be determined so as to be connected to this.

  Next, in S036, the gait generator 100 adds the control cycle Δt to time t, and executes the processing from S014 again.

  As a result, the instantaneous value of the current time's gait is generated in time series.

  In the present embodiment, the target motion determination means in the present invention is realized by the processing of the gait generator 100 described above (the processing shown in the flowchart of FIG. 9). In this case, the trajectory (time series) of the target motion (target foot position / posture, target body position / posture, and target arm posture) of the current time's gait of the robot 1 is set as the vertical inertia force parameter in the present invention. One requirement is to satisfy the target floor reaction force vertical component trajectory defined by the floor reaction force vertical component trajectory parameter (the parameter determined in S606 of FIG. 17).

  In addition, the target motion trajectory of the current time's gait is determined as another requirement to converge (asymptotically) to the motion trajectory of the normal gait in the future.

  Further, when the model operation floor reaction force moment is not “0” (in this embodiment, the compensated total floor reaction force moment is within the floor reaction force moment allowable range), the target motion of the current time gait In the dynamic model (inverted pendulum model in this embodiment) representing the relationship between the motion of the robot 1 and the floor reaction force, the model operation floor reaction force moment is additionally generated around the target ZMP. Determined as one requirement. In this case, assuming that the actual motion of the robot 1 is performed according to the target motion of the current time's gait, the actual robot 1 additionally has a moment opposite to the model operation floor reaction force moment around the target ZMP. Will act. Therefore, the target motion is determined such that the floor reaction force moment that suppresses the actual body posture of the real robot 1 from deviating from the target posture is additionally applied.

  Next, the calculation processing of the posture stabilization control calculation unit 112, which has been described later, will be described in more detail.

  The compensated total floor reaction force moment Mdmd calculated by the posture stabilization control calculation unit 112 is a deviation between the actual body posture angle (actually measured body posture angle) and the target body posture angle, as described above. This is a requested operation amount for converging the body posture angle deviation Δθ to “0”.

  In this case, in this embodiment, the body posture angle deviation Δθ is an approximation of the dynamics of the robot 1 (relation between the motion of the body 24 and the floor reaction force) of the inverted pendulum model of FIG. The horizontal position of the body mass point 24b is converted into a position deviation ΔX with respect to the reference position (the horizontal position of the body mass point 24b corresponding to the target body position / posture when the body posture angle deviation Δθ is “0”). . That is, the body posture angle deviation Δθ is converted into a position deviation ΔX as a perturbation amount of the horizontal position of the body mass point 24b.

  This conversion is performed by the following equation 150. Note that h in the expression 150 is the average height (constant value) of the upper mass point 24b as described above.

ΔX = h * Δθ ...... Formula 150

Supplementing this formula 150, the body posture angle deviation Δθ is generally an angle close to “0”. In this case, the position deviation ΔX corresponding to the body posture angle deviation Δθ is approximately given by Expression 150.

  Further, the posture stabilization control calculation unit 112 calculates a speed deviation ΔVx that is a temporal change rate (differential value) of the position deviation ΔX calculated from the body posture angle deviation Δθ by the expression 150 as described above. Note that the velocity deviation ΔVx is the actual measured value of the angular velocity of the body posture angle of the actual robot 1 (or the temporal change rate of the measured value of the body posture angle) and the angular velocity of the target body posture angle (in this embodiment, “ The deviation from 0 ″) may be obtained by multiplying the average height h of the upper body point 24b of the inverted pendulum.

  Supplementally, in this embodiment, as described above, the position deviation ΔX corresponds to the state quantity deviation in the present invention. Accordingly, the posture stabilization control calculation unit 112 calculates the body posture angle deviation Δθ, which is a deviation between the actual body posture angle (measured value based on the detection signal of the tilt sensor 40) and the target body posture angle, by Equation 150. By the process of calculating the position deviation ΔX, the state quantity deviation observing means in the present invention is realized.

  The posture stabilization control calculation unit 112 also includes a floor reaction force vertical component trajectory parameter (the floor reaction force vertical component trajectory parameter determined in S606 of FIG. 17) of the current time gait parameters determined by the gait generating device 100. ) Is entered.

  Then, the posture stabilization control calculation unit 112 calculates the instantaneous value of the vertical inertial force of the upper mass point 24b at the current time k of the current time's gait based on the floor reaction force vertical component trajectory parameter. The calculation method is the same as the calculation method of the inertial force in the vertical direction of the upper mass point 24b regarding the normal gait.

  Specifically, in this embodiment, the posture stabilization control calculation unit 112 assumes that the mass mb of the upper mass point 24b of the inverted pendulum matches the entire mass of the robot 1, and the floor of the current time gait parameters. Based on the reaction force vertical component trajectory parameter, the instantaneous value of the floor reaction force vertical component at the current time k (this is the inertial force of the upper body point 24b at the current time k and the gravity acting on the upper body point 24b). Is balanced with the vertical component of the resultant force. The instantaneous value is the same as the value calculated in S806 of FIG. Then, the posture stabilization control calculation unit 112 calculates the product of the mass mb (= total mass of the robot 1) of the upper mass point 24b and the gravitational acceleration constant g from the instantaneous value of the calculated floor reaction force vertical component, that is, A value obtained by subtracting gravity (= mb * g) acting on the upper mass point 24b is calculated as a value of the vertical inertia force (= −mb * d2Zb / dt2) of the upper mass point 24b at the current time k. .

  Supplementally, as supplemented for the calculation of the vertical inertia force of the upper mass point 24b related to the normal gait, based on the current time gait parameter, using a multi-mass model (geometric model) such as a three-mass model, The vertical inertial force of the upper mass point 24b at the current time of the current time's gait may be calculated.

  Then, the posture stabilization control calculation unit 112 uses the position deviation ΔX and the speed deviation ΔVx calculated as described above and the inertial force in the vertical direction of the upper body point 24b at the current time k, and uses the following expressions 152a and 152b. The compensation total floor reaction force moment Mdmd is determined by the following calculation.

Mdmd = (Kx * ΔX + Kv * ΔVx) + Mdmd_ff Equation 152a
However, Mdmd_ff = (mb * d2Zb / dt2) * ΔX (Formula 152b)

As described above, Mdmd is a vector composed of the component Mdmdx in the roll direction (around the X axis) and the component Mdmdy in the pitch direction (around the Y axis). Therefore, ΔX and ΔVx in the formulas 152a and 152b mean the position deviation and speed deviation in the Y-axis direction for calculating Mdmdx, respectively, and the position deviation and speed deviation in the X-axis direction for calculating Mdmdy, respectively. To do.

  Here, in the process of calculating Mdmd by the calculation of the equations 152a and 152b, the vertical inertia force (or motion acceleration) of the upper body 24 is “0” (as a result, the vertical inertia of the upper body mass point 24b). The PD law (proportional / derivative law) as a feedback control law constructed so that the positional deviation ΔX converges to “0” based on the assumption that the force is “0”, and the vertical direction of the body 24 This means that the compensation total floor reaction force moment Mdmd is determined by a control law combined with a feedforward law for compensating for the influence of the inertia force (and consequently the inertial force in the vertical direction of the upper mass point 24b). .

  In this case, the terms (first term and second term) in parentheses on the right side of the expression 152a correspond to the feedback manipulated variable component according to the PD rule. Kx and Kv are pole placement and optimum so that ΔX can smoothly converge to “0” under the assumption that the vertical inertial force of the upper body point 24b is “0”. They are a proportional gain and a differential gain of a predetermined value (fixed value) set in advance using a method such as a regulator.

  Further, the third term Mdmd_ff on the right side of the expression 152a corresponds to a feedforward manipulated variable component according to the feedforward rule for compensating for the influence of the vertical inertial force of the body 24. This feedforward manipulated variable component Mdmd_ff is a function value of the inertial force in the vertical direction of the upper mass point 24b and the position deviation ΔX, and is calculated by the expression 152b in this embodiment. That is, Mdmd_ff is calculated as the product of the value obtained by inverting the sign of the inertial force in the vertical direction of the upper mass point 24b at the current time (= mb * d2Zb / dt2) and the position deviation ΔX. Mdmd_ff calculated in this way is a moment component to be applied around the fulcrum to cancel the moment generated around the fulcrum (target ZMP) of the inverted pendulum model by the vertical inertial force of the upper body point 24b. I mean.

  Therefore, the processing for determining the compensated total floor reaction force moment Mdmd using the equations 152a and 152b is performed by the feedback manipulated variable component (= Kx * ΔX + Kv * ΔVx) and the feedforward manipulated variable component Mdmd_ff (= (mb * d2Zb / dt2). ) * ΔX) and Mdmd is determined by a combination operation.

  Supplementally, as described above, in this embodiment, the compensated total floor reaction force moment Mdmd corresponds to the required operation amount in the present invention. Therefore, the required operation amount determining means in the present invention is realized by the process of calculating Mdmd using the equations 152a and 152b. In this case, the feedback manipulated variable component (= Kx * ΔX + Kv * ΔVx) and the feedforward manipulated variable component Mdmd_ff (= (mb * d2Zb / dt2) * ΔX) are respectively the inertial force-independent manipulated variables in the present invention. It corresponds to a component and an inertial force dependent manipulated variable component. The proportional gain Kx and the differential gain Kv correspond to the first coefficient and the third coefficient in the present invention, respectively, and the sign of the inertial force in the vertical direction of the body mass point 24b is inverted (mb * d2Zb / dt2) corresponds to the second coefficient in the present invention.

  Further, the above formulas 152a and 152b can be replaced by the following formula 153 when they are arranged together. Therefore, the compensated total floor reaction force moment Mdmd may be calculated by the calculation of Expression 153.

Mdmd = (Kx + (mb * d2Zb / dt2)) * ΔX + Kv * ΔVx (Formula 153)

In this case, in the first term on the right side of the equation 153, the coefficient (Kx + (mb * d2Zb / dt2)) applied to ΔX is the proportional gain Kx (first coefficient) and the vertical inertial force of the upper body point 24b. It has a meaning as a composite coefficient with the second coefficient (= mb * d2Zb / dt2) as a function value.

  In other words, the process of calculating Mdmd by the expression 153 in this way is based on the PD law using a proportional gain (the above-mentioned synthesis coefficient) that is variably determined according to the vertical inertia force of the upper mass point 24b. , Mdmd is determined. In this case, if the synthesis coefficient is set to Kx ′, Kx ′ (= Kx + (mb * d2Zb / dt2)) in the calculation of Expression 153 is synchronized with the change in the floor reaction force vertical component, for example, FIG. It is determined so as to change as shown in the graph. FIG. 23 also shows a graph of Kv, which is a constant value.

  In the present embodiment, the posture stabilization control calculation unit 112 calculates the position deviation ΔX using the position deviation ΔX and the speed deviation ΔVx and the inertial force in the vertical direction of the upper mass point 24b at the current time k as described above. A compensation total floor reaction force moment Mdmd as a required operation amount for converging to “0” (and converging the body posture angle deviation Δθ to “0”) is determined.

  The above is the details of the calculation processing of the posture stabilization control calculation unit 112.

  By processing the posture stabilization control calculation part 112 described above, Mdmd can smoothly converge ΔX to “0” on the assumption that the inertial force in the vertical direction of the upper mass point 24b is “0”. The feedback manipulated variable component (= Kx * ΔX + Kv * ΔVx) and the feedback manipulated variable component Mdmd_ff (= (mb * d2Zb / dt2) * ΔX) corresponding to the inertial force at the current time of the upper mass point 24b were synthesized. It is determined as a value (added value). As a result, even when the robot 1 moves with a gait (in this embodiment, a running gait) in which the vertical inertia force of the upper body 24 changes, the actual body posture is affected by disturbances and the like. When a deviation occurs from the target posture, the actual state posture can be restored to the target posture smoothly (so that overshoot and undershoot do not occur) while appropriately compensating for the influence of the inertial force. it can.

  In the embodiment described above, the traveling gait in FIG. 5 is described as an example of the target gait in which the vertical inertia force of the upper body 24 changes. However, the vertical inertia force of the robot 1 changes. The robot 1 may be exercised with the walking gait as the target gait. In this case, the floor reaction force vertical component trajectory (trajectory for one step) is set, for example, in a polygonal line pattern as shown in FIG. The inertial force in the vertical direction can be changed. In the example of FIG. 24, the floor reaction force vertical component trajectory is set in a trapezoidal shape that protrudes upward (convex upward) in the both-leg support period, and in the one-leg support period. It is set to a trapezoidal shape that is convex (convex downward) to the decreasing side of the vertical component.

  Thus, even when the robot 1 is operated with a walking gait that changes the floor reaction force vertical component trajectory, the robot 1 can be operated while generating the target gait in the same manner as in the above-described embodiment. By controlling, when the actual body posture deviates from the target body posture, the actual body posture can be smoothly restored to the target posture.

  In the embodiment, the position deviation ΔX calculated by the expression 150 is used as the state quantity deviation. However, the body posture angle deviation Δθ may be used as the state quantity deviation.

  In this case, for example, the compensated total floor reaction force moment Mdmd may be calculated by the arithmetic processing of the following equations 154a and 154b or the arithmetic processing of the following equation 155.

Mdmd = (Kθ * Δθ + Kω * Δω) + Mdmd_ff Equation 154a
However, Mdmd_ff = (mb * d2Zb / dt2) * h * Δθ (Formula 154b)

Mdmd = (Kθ + (mb * d2Zb / dt2) * h) * Δθ + Kω * Δω (Formula 155)

Δω is a temporal change rate of the body posture angle deviation Δθ, and Kθ and Kω are predetermined values that are set in advance based on the assumption that the vertical inertial force of the body mass point 24b is “0”. It is the proportional gain and differential gain of the value (fixed value).

  Further, as the state quantity deviation, for example, the difference between the position of the center of gravity of the body 24 in the target gait and the position of the center of gravity of the actual body 24 may be used.

  In the above embodiment, the vertical direction (gravity direction) is set as the vertical direction, and Mdmd is determined so as to compensate for the influence of the vertical inertia force of the upper body 24 (upper body mass point 24b). When the floor surface is inclined, the target value of the inertial force of the upper body 24 (upper body mass point 24b) in the vertical direction (the target value at the current time) is defined as the vertical direction. ) May be used to determine Mdmd so as to compensate for the influence of the inertial force.

  In the above-described embodiment, the biped mobile robot 1 is described as an example of the mobile body. However, the mobile body to which the present invention can be applied is not limited to the biped mobile robot. For example, the present invention can be applied to a legged mobile robot having three or more legs.

  Alternatively, for example, the present invention can be applied to a moving body having a structure schematically shown in FIGS. A moving body 200 illustrated in FIG. 25A includes wheels 202 as a moving mechanism. The base body 206 is supported on the wheel 202 via a link mechanism 204 pivotally supported on the rotating shaft of the wheel 202. In this case, the link mechanism 204 has a linear motion type joint 208 between the rotating shaft of the wheel 202 and the base body 206, and the base body 206 can be moved up and down by the operation of the linear motion joint 208. Yes.

  Moreover, the moving body 210 illustrated in FIG. 25B is different from the moving body 200 illustrated in FIG. 25A only in the structure of the link mechanism 212. In this example, the link mechanism 212 is pivotally supported by the rotating shaft of the wheel 202 and has rotary joints 214 and 216 between the rotating shaft and the base body 206. The base body 206 can be moved up and down by the movements of the joints 214 and 216.

  Note that Δθ in FIGS. 25A and 25B indicates a deviation amount (angle difference) of the actual posture of the base body 206 from the target posture. In this example, the posture in which the base body 206 stands in the vertical direction is shown as the target posture of the base body 206.

  In the above embodiment, the compensation total floor reaction force moment Mdmd is distributed to the model operation floor reaction force moment and the compliance control floor reaction force moment. However, the model operation floor reaction force moment and the compliance control floor reaction force moment Only one of these may be used. For example, a value obtained by reversing the sign of the compensation total floor reaction force moment Mdmd (or a required operation moment calculated by a proportional / differential law calculation in which the signs of the proportional gain Kx and the differential gain Kv are reversed) is used. It may be used as a force moment, and the floor reaction force moment for compliance control may be always set to “0”.

  Also, for example, when the floor reaction force moment allowable range can be kept sufficiently wide, the compensated total floor reaction force moment Mdmd is used as it is as a compliance control moment, so that the model operation floor reaction force moment is always It may be set to “0”. By doing in this way, one Embodiment of the said 1st invention will be constructed | assembled.

  In the above embodiment, the dynamic model of the robot 1 used to generate the target gait is exemplified by a one-mass point or three-mass point model. However, for example, the posture of the body 24 is appropriately changed. When generating the desired gait, a dynamic model including a flywheel or the like expressing the relationship between the posture change of the upper body 24 and the floor reaction force moment may be used.

  In the above embodiment, the compensated total floor reaction force moment Mdmd is used as the required operation amount in the present invention. For example, the position deviation ΔX and the body posture angle deviation Δθ such as the correction amount of the target ZMP can be manipulated. The requested operation amount of the type may be used.

  DESCRIPTION OF SYMBOLS 1 ... Biped mobile robot (moving body), 2 ... Leg body (moving mechanism), 24 ... Upper body (base | substrate), 100 ... Gait generator (Upper-direction inertia force parameter determination means, target motion determination means), 104 ... composite compliance operation determining unit (operation control unit), 110 ... compensated total floor reaction force moment distributor (distribution unit), 112 ... posture stabilization control calculation unit (state quantity deviation observation unit, required operation amount determination unit), S606: Vertical inertial force parameter determining means, 200, 210: moving body, 202: wheel (moving mechanism), 206: base body.

Claims (7)

A control device that controls a moving body including a base and a moving mechanism that moves the base on the floor surface to move the base while moving the base up and down.
Vertical inertial force parameter determining means for determining a vertical inertial force parameter that is a parameter that defines a time series of the target inertial force in the vertical direction of the movable body or the base;
Target motion determining means for determining the time series of the target motion of the moving body using at least the vertical inertia force parameter so as to satisfy the time series of the target inertia force defined by the determined vertical inertia force parameter. When,
Operation control means for performing operation control of the moving body according to at least the determined target motion;
State quantity deviation observing means for sequentially observing a state quantity deviation representing the degree of deviation of the actual posture of the base body from the target posture of the base body in the determined target motion;
A required operation amount determining means for sequentially determining a required operation amount that defines an additional external force to be additionally applied to the moving body in order to converge the state quantity deviation to “0”;
The operation control means operates the mobile body so that an additional external force defined by the required operation amount is additionally applied to the mobile body while causing the actual motion of the mobile body to follow the target motion. A means of controlling,
The requested operation amount determining means is configured such that, at each time when the requested operation amount is determined, the requested operation amount to be determined is a current time in a time series of target inertia force defined by the vertical inertia force parameter. An inertial force-dependent manipulated variable component that changes according to the instantaneous value of the target inertial force and the observed value of the state quantity deviation, and changes according to the observed value of the state quantity deviation without depending on the target inertial force The requested manipulated variable is determined using the vertical inertial force parameter and the observed value of the state quantity deviation so as to be a value obtained by combining an inertial force-independent manipulated variable component. Control device for moving objects. A control device that controls a moving body including a base and a moving mechanism that moves the base on the floor surface to move the base while moving the base up and down.
Vertical inertial force parameter determining means for determining a vertical inertial force parameter that is a parameter that defines a time series of the target inertial force in the vertical direction of the movable body or the base;
A time series of at least a dynamic model preset as representing dynamics of the moving body, the determined vertical inertia force parameter, and a target external force to be applied to the moving body on the dynamic model A target motion determining means for determining a time series of the target motion of the moving body using
Operation control means for performing operation control of the moving body according to at least the determined target motion;
State quantity deviation observing means for sequentially observing a state quantity deviation representing the degree of deviation of the actual posture of the base body from the target posture of the base body in the determined target motion;
A required operation amount determining means for sequentially determining a required operation amount that defines an additional external force to be additionally applied to the moving body in order to converge the state quantity deviation to “0”;
The target motion determining means determines a model operation external force to be added to the target external force on the dynamic model according to the determined required operation amount, and adds the determined model operation external force to the target external force. The target force is applied to the moving body on the dynamic model while satisfying the target inertia force time series defined by the determined vertical inertia force parameter on the dynamic model. A means of determining exercise,
The requested operation amount determining means is configured such that, at each time when the requested operation amount is determined, the requested operation amount to be determined is a current time in a time series of target inertia force defined by the vertical inertia force parameter. An inertial force-dependent manipulated variable component that changes according to the instantaneous value of the target inertial force and the observed value of the state quantity deviation, and changes according to the observed value of the state quantity deviation without depending on the target inertial force The requested manipulated variable is determined using the vertical inertial force parameter and the observed value of the state quantity deviation so as to be a value obtained by combining an inertial force-independent manipulated variable component. Control device for moving objects. In the control apparatus of the moving body of Claim 2,
The target motion determining means includes a distributing means for distributing the determined required operation amount to the model operating external force and an actual moving body operating external force that should actually act on the moving body.
The operation control means performs the operation control of the moving body so that the actual moving body operating external force is additionally applied to the moving body while causing the actual movement of the moving body to follow the target movement. A control device for a moving object. In the control apparatus of the moving body of any one of Claims 1-3,
The requested operation amount determined by the requested operation amount determining means includes at least an inertial force-independent operation amount component including at least a value obtained by multiplying the observed value of the state quantity deviation by a predetermined first coefficient. And the inertial force-dependent manipulated variable component including at least a value obtained by multiplying the observed value of the state quantity deviation by a second coefficient that is a function value of the instantaneous value of the target inertial force at the current time. A control device for a moving body, characterized in that it is a value. In the control apparatus of the moving body of Claim 4,
The inertial force dependent manipulated variable component is set in advance to a value obtained by multiplying the observed value of the state quantity deviation by a first coefficient of a predetermined value set in advance, and a temporal change rate of the observed value of the state quantity deviation. The inertial force-independent manipulated variable component is composed of a value obtained by multiplying the observed value of the state quantity deviation by the second coefficient. A control device for a moving body. In the control apparatus of the moving body of Claim 4 or 5,
The process executed by the requested operation amount determining means to determine the requested operation quantity is a process of multiplying the observed value of the state quantity deviation by a composite coefficient obtained by adding the first coefficient and the second coefficient. The control apparatus of the moving body characterized by including. In the control apparatus of the moving body of any one of Claims 1-6,
The inertial force-independent manipulated variable component includes the additional external force necessary to converge the state quantity deviation to “0”, assuming that the target inertial force is maintained at “0”. The inertial force dependent manipulated variable component is a manipulated variable component necessary for canceling the additional external force generated due to the instantaneous value of the target inertial force at the current time. A control device for a moving body.

Images