WO2006067904A1 - 脚式移動ロボットの歩容生成装置 - Google Patents

脚式移動ロボットの歩容生成装置 Download PDF

Info

Publication number
WO2006067904A1
WO2006067904A1 PCT/JP2005/017806 JP2005017806W WO2006067904A1 WO 2006067904 A1 WO2006067904 A1 WO 2006067904A1 JP 2005017806 W JP2005017806 W JP 2005017806W WO 2006067904 A1 WO2006067904 A1 WO 2006067904A1
Authority
WO
WIPO (PCT)
Prior art keywords
gait
target
trajectory
robot
reaction force
Prior art date
Application number
PCT/JP2005/017806
Other languages
English (en)
French (fr)
Inventor
Tadaaki Hasegawa
Original Assignee
Honda Motor Co., Ltd.
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Honda Motor Co., Ltd. filed Critical Honda Motor Co., Ltd.
Priority to DE602005011492T priority Critical patent/DE602005011492D1/de
Priority to EP05788198A priority patent/EP1842629B1/en
Priority to US11/722,052 priority patent/US7840309B2/en
Publication of WO2006067904A1 publication Critical patent/WO2006067904A1/ja

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
    • B62D—MOTOR VEHICLES; TRAILERS
    • B62D57/00—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track
    • B62D57/02—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track with ground-engaging propulsion means, e.g. walking members
    • B62D57/032—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track with ground-engaging propulsion means, e.g. walking members with alternately or sequentially lifted supporting base and legs; with alternately or sequentially lifted feet or skid
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J5/00—Manipulators mounted on wheels or on carriages
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y10—TECHNICAL SUBJECTS COVERED BY FORMER USPC
    • Y10S—TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y10S901/00—Robots
    • Y10S901/01—Mobile robot

Definitions

  • the present invention relates to an apparatus for generating a target gait for causing a legged mobile robot to perform an operation of moving an object.
  • a legged mobile robot such as a biped mobile robot performs a task such as pushing a target object to move it
  • the robot receives a floor force or a floor reaction force that contacts the tip of the leg.
  • the reaction force that the robot receives from the object is called the object reaction force.
  • Patent Document 1 Japanese Patent Laid-Open No. 10-230485 by the present inventor is disclosed.
  • This technology removes the vertical component of the moment generated around the target ZMP (target floor reaction force center point) by the resultant force of the inertial force generated by the movement of the mouth bot and the gravity and target reaction force acting on the robot.
  • the desired gait is generated so as to satisfy the dynamic equilibrium condition that (horizontal component) becomes 0 (the floor reaction force acting on the center point of the desired floor reaction force balances the resultant force). .
  • the center of gravity of the robot can maintain the robot's dynamic balance.
  • the robot's target gait is adjusted to balance the center of gravity.
  • the trajectory (time series) of the target value of the acting force from the robot to the object or the object It is necessary to determine the trajectory (time series) of the target value of the reaction force.
  • the target value of the object reaction force will be representatively shown below. The trajectory of this shall be determined.
  • the trajectory of the target value of the object reaction force (hereinafter referred to as the target object reaction force) is, for example, a movement plan of the object (a plan such as how to move the object at any timing) To be determined.
  • the actual object reaction force acting on the robot does not always coincide with the target object reaction force.
  • the target target reaction force is 0 to a certain value ( ⁇ 0) at the scheduled time.
  • the actual reaction force of the object may not change at the scheduled time due to a shift in the positional relationship between the actual robot and the object.
  • the gait generated based on the target object reaction force trajectory is generated with the object reaction force scheduled to change at the scheduled time.
  • the stability of the robot is likely to be impaired.
  • the present invention has been made in view of a strong background, and a gait for causing a robot to perform an operation of moving an object is obtained by applying an action force between a future robot and the object.
  • Legged movement that can be generated considering the deviation from the target value, and can generate the robot's gait so that the stability of the robot's posture can be ensured even if the applied force deviates from the target value.
  • An object is to provide a robot gait generator.
  • a first invention of a gait generator for a legged mobile robot provides an object to a legged mobile robot including a plurality of legs extending from an upper body.
  • the robot in the new target gait is at least based on the movement plan of the object.
  • a first robot which is a trajectory of the target value of the acting force between the object and a first acting force trajectory determining means for temporarily determining a target acting force trajectory between the objects;
  • the robot in the new target gait First leg motion parameter determining means for tentatively determining first leg motion parameters defining the movement of the legs of the robot;
  • the first robot's target target trajectory between the actual robot and the target may be changed after the predetermined time in the future. Assuming that the trajectory has changed to a trajectory different from the applied force trajectory, set the target applied force trajectory between the second robot and the target, which is the trajectory of the target value of the applied force between the robot and the object after the predetermined time.
  • Second acting force trajectory setting means
  • the target gait of the robot up to the predetermined time point is generated using the first leg motion parameter and the first robot 'target motion force trajectory between the objects, and further the target gait up to the predetermined time point
  • the target gait of the robot after a predetermined time is defined as a variable that can be variably set as a second leg motion parameter that is a leg motion parameter that defines the motion of the leg of the robot after the predetermined time.
  • the second leg motion that allows the target gait of the robot after the predetermined time point to satisfy a predetermined dynamical constraint condition when generated using the leg motion parameters and the second robot / object interaction force
  • a judgment means for judging whether or not the force is capable of setting a parameter
  • the first leg When it is determined by the determining means that the second leg motion parameter that allows the target gait of the robot after the predetermined time to satisfy the predetermined dynamic constraint condition can be set, the first leg The new target gait is generated using the body motion parameter and the first robot 'target motion force trajectory between the objects, and the target gait of the robot after the predetermined time satisfies the predetermined dynamic constraint condition.
  • the movement plan of the object is corrected, and at least the corrected movement plan is used to change the first leg motion parameter and the first leg movement parameter.
  • Robot 'target target force trajectory between objects is re-determined, and the new target gait is generated using the re-determined first leg motion parameters and the first mouth bot / target target force trajectory. Do It is characterized by doing so.
  • the target gait of the robot after the predetermined time point that is, the trajectory of the action force between the robot and the target object after the predetermined time point is the target action force between the first robot and the target object.
  • the second robot is different from the trajectory.
  • the second leg motion parameter (hereinafter, this second leg motion parameter is referred to as the appropriate second leg motion parameter) in which the target gait of the formed robot can satisfy the dynamic constraint conditions.
  • the determination means determines whether or not the force is settable. In this determination, if an appropriate second leg motion parameter can be set, it is assumed that the actual acting force between the mouth bot and the object is the target action force between the first robot and the object after the predetermined time.
  • the new target gait is generated by using the first leg motion parameter and the first robot'target motion force trajectory between objects.
  • the actual acting force between the robot and the target object is determined after the predetermined time point.
  • the robot becomes the target action force trajectory between the first robot and the target action force between the target robots
  • the target gait that satisfies the dynamic constraint conditions cannot be generated and the robot It is difficult to ensure the stability. Therefore, according to the first aspect of the present invention, in this case, the movement plan of the object is corrected, and at least the corrected movement plan is used to determine the first leg motion parameter and the target action force between the first robot and the object. Redetermine the trajectory. Then, the new target gait is generated using the re-determined first leg motion parameter and the first robot 'target motion target trajectory.
  • the gait for causing the robot to perform the operation of moving the object is taken into consideration when the acting force between the robot and the object in the future deviates from the target value force.
  • the robot's gait can be generated so that the stability of the robot's posture can be ensured.
  • a leg motion parameter is generated.
  • ZMP ZMP (the horizontal component of the moment generated by the combined force of the inertial force generated by the robot's motion and the force acting on the robot's object force and the gravity acting on the robot becomes zero)
  • target ZMP the trajectory of the target position of zero moment point
  • robot dynamic model to satisfy the target ZMP (so that the horizontal component force SO of the moment around the target ZMP).
  • the predetermined time point is, for example, the magnitude of the acting force of the first robot's target acting force trajectory between the objects temporarily determined by the first acting force trajectory determining means, or of the acting force.
  • the time point at which the magnitude of the change speed exceeds a predetermined threshold value, and the second robot 'target target action force trajectory is the magnitude of the action force between the robot and the object after the predetermined time, or It is preferable that the trajectory has a change speed of the acting force that is smaller than the predetermined threshold (second invention).
  • the predetermined dynamic constraint condition is that at least the inertia force generated by the motion of the target gait of the robot, the gravity acting on the robot, and the robot If the ZMP determined by the resultant force acting on the object is within the predetermined allowable range, it is preferable to include the condition (third invention).
  • the actual acting force between the robot and the object is Said
  • the target gait (ZMP is predetermined) to ensure the stability of the robot when the second robot 'target target force trajectory different from the first robot' target target force applied after the predetermined time point It is possible to appropriately determine whether or not it is possible to generate a gait that falls within the allowable range.
  • a gait generator for a legged mobile robot according to an embodiment of the present invention will be described with reference to the accompanying drawings.
  • a legged mobile robot a biped mobile robot is taken as an example.
  • Fig. 1 is a schematic diagram showing the entire biped mobile robot as a legged mobile robot according to this embodiment.
  • a bipedal mobile robot (hereinafter referred to as a robot) 1 includes a pair of left and right legs (leg links) 2 and 2 extending downward from an upper body (base of the robot 1) 3. . Both legs 2, 2 have the same structure, each with six joints. The six joints are the joints 10R, 10L for the rotation (rotation) of the crotch (waist) (in the direction of the rotation relative to the upper body 3) in order of the upper body 3 side force.
  • the rotation joints 20R, 20L and force are also configured.
  • a foot (foot) 22R (L) constituting the tip of each leg 2 is attached.
  • the upper body 3 is attached to the uppermost positions of both legs 2 and 2 through three joints 10R (L), 12R (L) and 14R (L) of the crotch of each leg 2. ing.
  • a control unit 60 and the like, which will be described in detail later, are stored.
  • the control unit 60 is shown outside the upper body 3 for the convenience of illustration.
  • the hip joint (or the hip joint) has joints 10R (L) and 12R.
  • the knee joint is composed of joint 16R (L)
  • the ankle joint is composed of joints 18R (L) and 20R (L).
  • the hip joint and knee joint are connected by a thigh link 24R (L)
  • the knee joint and ankle joint are connected by a crus link 26R (L).
  • a pair of left and right arms (arm links) 5 and 5 are attached to both sides of the upper part of the upper body 3
  • the head 4 is disposed at the upper end of the upper body 3.
  • Both arms 5 and 5 have the same structure, each with 7 joints. That is, each arm body 5 has a shoulder joint composed of three joints 30R (L), 32R (L), 34R (L), an elbow joint composed of one joint 36R (L), and three
  • a wrist joint composed of joints 38R (L), 40R (L), and 42R (L) and a hand portion (hand) 44R (L) connected to the wrist joint are provided.
  • the head 4 does not have a direct relationship with the gist of the present invention, and therefore a detailed description is omitted.
  • the foot 22R (L) of each leg 2 is given six degrees of freedom with respect to the upper body 3.
  • “*" is multiplication for scalar operations and cross product for vectors.
  • the desired joints 22R and 22L can be exercised by driving the joint shown in FIG.
  • the robot 1 can arbitrarily move in the three-dimensional space.
  • the desired work such as pushing the trolley can be performed.
  • a known 6-axis force sensor 50 is interposed between the foot 22R (L) below the ankle joints 18R (L) and 20R (L) of each leg 2 as shown in FIG. ing.
  • the six-axis force sensor 50 is for detecting the presence / absence of the foot 22R (L) of each leg 2 and the floor reaction force (ground load) acting on each leg 2, etc.
  • the detection signals of the three-direction components Fx, Fy, Fz of the translational force of the floor reaction force and the three-way components Mx, My, Mz of the moment are output to the control unit 60.
  • the 6-axis force sensor 52 which is similar to the 6-axis force sensor 50, includes a wrist portion (node) 44R (L) and wrist joints 38R (L), 40R (L), 42R (L) of each arm body 5.
  • the detection signal of the three-direction component of the translational force of the external force acting on the hand portion 44R (L) and the three-way component of the moment is output from the six-axis force sensor 52 to the control unit 60.
  • the body 3 detects the inclination angle of the body 3 with respect to the Z axis (vertical direction (gravity direction)) and its angular velocity, and the rotation angle (one angle) of the body 3 around the Z axis and its angular velocity.
  • a posture sensor 54 is provided, and a detection signal thereof is output from the posture sensor 54 to the control unit 60.
  • This posture sensor 54 includes a triaxial acceleration sensor and a triaxial gyro sensor (not shown).
  • the detection signals of these sensors indicate the posture angle (tilt angle and horizontal angle) of the upper body 3 and its angular velocity. Inspection It is used for estimating the self-position and posture of the robot 1.
  • each joint of the robot 1 has an electric motor 64 (see FIG. 2) for driving the joint and a rotation amount of the electric motor 64 (a rotation angle of each joint).
  • An encoder (rotary encoder) 65 for detection is provided, and a detection signal of the encoder 65 is output from the encoder 65 to the control unit 60.
  • an elastic body such as a spring is interposed between each foot 22R (L) and the six-axis force sensor 50, and the bottom surface of the foot 22R (L)
  • An elastic body such as rubber is attached to the surface.
  • FIG. 2 is a block diagram showing the configuration of the control unit 60.
  • the control unit 60 is constituted by a microcomputer, and the first arithmetic unit 90 and the second arithmetic unit 92, which also have CPU power, an AZD variable ⁇ 80, a counter 86, a DZA variable ⁇ 96, a RAM 84, a ROM 94, a parallel And a bus line 82 for exchanging data between them.
  • the output signals from the 6-axis force sensor 50 of each leg 2, the 6-axis force sensor 52 of each arm 5, and the attitude sensor 54 (acceleration sensor and rate gyro sensor) are output from the AZD converter 80. After being converted to a digital value, it is sent to the RAM 84 via the bus line 82.
  • the output of the encoder 65 (rotary encoder) of each joint of the robot 1 is input to the RAM 84 via the counter 86.
  • the first arithmetic unit 90 generates a target gait as described later, calculates a joint angle displacement command (a command value of a displacement angle of each joint or a rotation angle of each electric motor 64), and RAM8 Send to 4. Further, the second arithmetic unit 92 reads out the joint angle displacement command from the RAM 84 and the actually measured value of the joint angle detected based on the output signal of the encoder 65, and calculates the operation amount necessary for driving each joint. It is calculated and output to the electric motor 64 that drives each joint via the DZA converter 96 and the servo amplifier 64a.
  • FIG. 3 is a block diagram generally showing a functional configuration of the control device for the legged mobile robot according to the embodiment of the present specification.
  • the portion surrounded by a broken line in FIG. 3 is constituted by processing functions (mainly functions of the first arithmetic device 90 and the second arithmetic device 92) executed by the control unit 60.
  • processing functions mainly functions of the first arithmetic device 90 and the second arithmetic device 92
  • the left and right sides of leg 2 and arm 5 are The symbols R and L are omitted when there is no need to distinguish between them.
  • the functional configuration of the control unit 60 includes a gait generator 100, an object reaction force balance controller 102, a leg main controller 104, and an arm main controller 106.
  • the applicant of the present invention has previously described the processing of the object reaction force balance control device 102, the leg main control device 104, and the arm main control device 106. This is the same as that proposed in Japanese Patent Laid-Open No. 10-230485 (Patent Document 1). Therefore, in the following description of the embodiment, the gait generator 100 will be mainly described, and the object reaction force balance controller 102, the leg main controller 104, and the arm main controller 106 will be schematically described. stop.
  • the gait generator 100 generates and outputs the target gait of the robot 1 freely and in real time.
  • the target gait output by the gait generator 100 is a target body position / posture trajectory (target position trajectory and target posture trajectory of upper body 3), target foot position / posture trajectory (target position trajectory of each foot 22) And target posture trajectory), target hand position / posture trajectory (target position trajectory and target posture trajectory of the hand 44 of each arm 5), target total floor reaction force center point trajectory (the target position of all floor reaction force center points) Trajectory))), target total floor reaction force trajectory, and target object reaction force trajectory.
  • the target position / posture trajectory of the movable part is added to the target gait.
  • target is often omitted when there is no risk of misunderstanding.
  • a “trajectory” in a gait means a temporal change pattern (time-series pattern).
  • the "position" of each part of the robot 1, such as the foot position and the body position means the position of a certain representative point that is fixedly set in that part.
  • the representative point of each foot 22 is a point where a perpendicular extending from the center of the ankle joint of the leg 2 including the foot 22 to the bottom surface of the foot 22 intersects the bottom surface.
  • the position of the representative point is the foot position.
  • “Position” means spatial orientation.
  • the body posture is the tilt angle of the upper body 3 in the roll direction (around the X axis) and the tilt direction of the upper body 3 in the pitch direction (around the Y axis) It is represented by the angle of rotation of the upper body 3 (around the Z axis). It is expressed as a biaxial spatial azimuth that is fixedly set on each foot 22.
  • the gait related to the motion of the robot 1 such as the foot position / posture, the body position / posture, etc. other than the components related to the floor reaction force and the object reaction force in the gait are generic. It is called “exercise”.
  • the motion trajectory may be constant (does not change with time) or substantially constant. For example, when the robot 1 keeps its feet 22 and 22 landed and does not move, the foot position / posture trajectory is constant.
  • the object reaction force in the gait is the reaction force of the force that acts on the object from the robot 1 when moving the object on the robot 1 (the force that acts on the robot 1 from the object).
  • the target object reaction force trajectory corresponds to the trajectory of the target acting force between the robot and the object in the present invention.
  • the hand portions 44R and 44L of the arms 5 and 5 are engaged with a predetermined portion of the object 120 (the carriage in the illustrated example) where the robot 1 is located.
  • An example will be described in which the object 120 is pushed while walking in the combined state.
  • the force received by the robot 1 from the object 120 is the object reaction force.
  • the object reaction force is generally composed of a translational force component and a moment component.
  • the object reaction force means the translational force acting on the robot 1 from the object 120, and the moment around the point acting on the robot 1 by the translational force is targeted. This is called the reaction force moment.
  • the object reaction force is obtained by reversing the sign (direction) of the force applied to the object 120 from the robot 1, so that the action force from the robot 1 to the object 120 is replaced with the object reaction force. You may use for.
  • the position of the object 120 means the position of a representative point fixedly set on the object 120, like the foot position of the robot 1.
  • the posture of the object 120 means the spatial orientation of the object 120, like the posture of the foot 22 of the robot 1.
  • the position and orientation of the object 120 are sometimes collectively referred to as object movement.
  • each foot 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) foot 22R,
  • the resultant force of “each foot floor reaction force” for 22L is called “all floor reaction force”.
  • each foot floor reaction force is hardly mentioned, and unless otherwise noted, “floor reaction force” is treated as the same as “whole floor reaction force”.
  • the target floor reaction force is generally expressed by an action point and a translational force and a moment acting on the point.
  • the action point can be used anywhere, and there are countless expressions for the same target floor reaction force, but the target floor reaction force center point (target position of the center point of all floor reaction forces) is the target point.
  • the moment component of the desired floor reaction force is zero except for the vertical component (moment about the lead straight axis (Z axis)).
  • the horizontal component of the moment of the desired floor reaction force around the desired floor reaction force center point is zero. Therefore, in the embodiment of the present specification, the gait generator 100 generates the target floor reaction force center point as the action point of the target total floor reaction force.
  • the center point of the desired floor reaction force has a meaning as the point of action of the desired total floor reaction force, so it can be said that it is essentially one component of the desired total floor reaction force.
  • the target floor reaction force center point of the target floor reaction force is closely related to the motion control of the robot 1, in FIG.
  • the center point of the desired floor reaction force, which is the point of action, is described separately from the desired total floor reaction force.
  • ZMP zero moment point
  • ZMP is the resultant force of the inertial force generated by the movement of the robot and the gravity acting on the robot and the reaction force of the object (or the floor reaction force that balances the resultant force).
  • ZMP Is used to mean the point where the moment acting around the point becomes 0 except for the vertical component.
  • the ZMP calculated from the target motion trajectory of the robot, gravity, and the target object reaction force coincides with the target floor reaction force center point.
  • the target ZMP is often referred to as the target ZMP instead of the target floor reaction force center point.
  • the target gait means a set of a target motion, a target floor reaction force (including a target ZMP) for a predetermined period, and a target object reaction force.
  • the target gait (including the case where it is simply referred to as a gait) is the target motion and the target floor reaction force (including the target ZMP) for one step of the robot 1 unless otherwise stated.
  • the target object reaction force “1 step” of the target gait is used in the meaning from the time when one leg 2 of the robot 1 is landed until the other leg 2 is landed. It is assumed that a series of gaits are connected with several gaits (gait for a predetermined period).
  • the target gait for the period of one step may be called a unit gait.
  • the period of one step when the robot 1 does not move is a predetermined period ( This is the period corresponding to the period of one step when Robot 1 moves.
  • 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 (a period in which both legs 2 and 2 become support legs). This is the period during which either one of the legs 2 supports the weight of the robot 1 (the period during which only one leg 2 is the supporting leg).
  • the target gait (unit gait) in the present embodiment is the target gait for the period until the end of the next one-leg support period.
  • the leg 2 that supports the weight of the robot 1 is called the support leg.
  • both legs 2 and 2 are support legs
  • one leg 2 is a support leg.
  • the leg 2 on the side that does not support the weight of the robot 1 during the one-leg support period (the leg 2 that is not the support leg) is called a free leg.
  • the supporting leg in the target gait is the leg 2 that lands at the start of the both-leg supporting period 2 (the leg that becomes the supporting leg in the one-leg supporting period following the both-leg supporting period) It shall mean 2).
  • the foot 22 of the leg 2 on the support leg side and the foot 22 of the leg 2 on the free leg side are referred to as the support leg foot 22 and the free leg foot 22, respectively.
  • the robot 1 is not moving (while maintaining the contact between both feet 22 and 22), for convenience, it is assumed that one leg 2 is a supporting leg and the other leg 2 is a free leg. .
  • a target gait (unit gait) that is newly generated by the gait generator 100 or a target gait that is being generated (unit gait) is referred to as a current time gait.
  • the target gait one step before is called the previous time gait
  • the target gait one step after the current gait is called the next gait
  • the next one step is called the next gait.
  • the target gait is described in a support leg coordinate system as a global coordinate system.
  • the support leg coordinate system is such that the support leg foot 22 is in a horizontal posture (more generally, a posture parallel to the floor surface) and substantially the entire bottom surface of the support leg foot 22 is on the floor surface.
  • the center force of the ankle joints 18 and 20 of the supporting leg intersects with the floor surface (this point is supported in the example of the embodiment of the present specification).
  • the global coordinate system floor that matches the representative point of the foot foot 22
  • the horizontal plane that passes through the origin is the XY plane. Coordinate system).
  • the X-axis direction and the Y-axis direction are the front-rear direction and the left-right direction of the support leg foot 22, respectively.
  • the X-axis, Y-axis, and Z-axis are the support leg seats unless otherwise specified. It shall mean the three axes of the standard system. Note that the origin of the support leg coordinate system does not necessarily match the representative point of the foot 22 in a state where almost the entire bottom surface of the support leg foot 22 is in contact with the floor surface. It may be set to a point on the surface.
  • the gait generator 100 includes first acting force trajectory determining means, first leg motion parameter determining means, second acting force trajectory determining means, and judging means according to the present invention, depending on its arithmetic processing function.
  • the gait generator 100 sequentially executes the processes shown in the flowcharts of FIGS. 5 to 8 at a predetermined calculation processing cycle.
  • a movement plan for the object 120 is determined.
  • the movement plan determined here includes a movement plan of the object 120 for at least a predetermined period in the future (for a plurality of steps of the robot 1) from the current time.
  • This movement plan is basically determined according to the movement request of the object 120 (design requirement of how and when the object 120 is moved), and the position of the object 120 is determined. It consists of a posture trajectory (time series of position and posture), or a parameter or a function expression that defines the trajectory. For example, if the movement request of the object 120 is a request to move the object 120 at a constant speed in the X-axis direction from a certain time tO, the movement plan is determined as shown by the graph gl in FIG.
  • the Graph gl shows the time course of the object position (X-axis direction position) in the movement plan.
  • the time series of the position of the object 120 at each time after the current time (at every step) may be determined as the movement plan! /
  • the time tO and the slope of the graph gl target
  • the moving speed of the object 120 may be determined as an element (parameter) that defines the movement plan, or a function expression of the graph gl may be determined as an element (parameter) that defines the movement plan.
  • the movement request is appropriately given to the gait generator 100 from the outside, or is stored and held in a storage unit (not shown) of the control unit 60 in advance.
  • the movement plan determined by the SOI is not necessarily determined according to the movement request, but is appropriately corrected as necessary. This will be described later.
  • Next step S03 Go forward, based on the movement plan determined in S01 as described above, the target object motion trajectory (target target position / posture trajectory) and target The object reaction trajectory is provisionally determined.
  • FIG. 10 is a block diagram showing a calculation process for obtaining a target object position trajectory and a target object reaction force trajectory in the target object motion trajectory in S03.
  • a portion surrounded by a broken line in this block diagram is an object dynamics model representing a relationship between a force acting on the object 120 and a motion of the object 120.
  • the object 120 is moved on a substantially horizontal floor will be described as an example.
  • the object dynamic model shown in FIG. 10 is a dynamic model (forward motion) that outputs the position of the object 120 by inputting a force acting on the object 120 (more specifically, a translational force in the horizontal direction).
  • Dynamic model More specifically, this object dynamics model is obtained by setting the reciprocal 1ZM of the mass M of the object 120 to the input value of the horizontal translation force acting on the object 120 (value obtained by the adding unit 204 described later). By multiplying by the multiplication unit 206, the motion acceleration of the object 120 is obtained, and this is sequentially integrated by the integrators 208 and 210 (double integration) to thereby determine the position of the object 120 (on the object dynamics model). Output position). That is, in the present embodiment, the object dynamic model shown in FIG.
  • the output of the integrator 208 means the moving speed of the object 120 on the object dynamic model, and this is hereinafter referred to as the object model speed.
  • the target object speed that is the instantaneous target speed (at each time) of the object 120 based on the movement plan determined in S01 is subtracted from the object model speed previously obtained by the integrator 208.
  • the target object speed is the object in the movement plan. It is a value obtained as the first derivative of 120 position trajectories, or a component of the movement plan. In the example of the movement plan shown in FIG. 9, it is sufficient to use the slope of the graph gl as it is as the target object speed. In this case, the target object speed is continuously constant.
  • a time series of target object speeds for a predetermined period in the future (a plurality of steps of robot 1) from the current time is sequentially input to the subtracting unit 200.
  • the object model speed input to the subtraction unit 200 is the value output from the integrator 208 when the target object speed one before the newly input target object speed is input to the subtraction unit 200. is there.
  • the required value of the translational force to be applied to the object 120 from the robot 1 is obtained by multiplying the deviation by a predetermined gain Kv by the multiplication unit 202.
  • the required value of the translational force is set so that the deviation between the target object speed and the target model speed converges to 0 (so that the target model speed follows the target target speed).
  • the feed knock control law In this example, a proportional control law is used as the feedback control law.
  • a time series obtained by reversing the sign of the calculated required value of the translational force is output as the target object reaction force trajectory.
  • the required value of the obtained translational force and an estimated disturbance force that is an estimated value of a disturbance force (such as a friction force acting on the floor force object 120) that acts on the actual object 120 other than the robot 1 force.
  • the adding unit 204 is added to the adding unit 204, and the sum of the required value of the translational force and the estimated disturbance force is obtained by the adding unit 204 as an input value of the translational force for the object dynamic model.
  • the estimated disturbance force is obtained by the processing of S61 described later for each arithmetic processing cycle of the gait generator 100.
  • a value (previous value) obtained in the previous calculation processing cycle of the gait generator 100 is used as the estimated disturbance force input to the adding unit 120.
  • the position of the object 120 is obtained as described above, and the time series of the obtained position is obtained from the integrator 210 as a target.
  • Target position Output as trajectory.
  • the initial value of the output of the integrator 208 of the object dynamics model (the initial value for each arithmetic processing cycle of the gait generator 100) is the same as that of S03 in the previous arithmetic processing cycle of the gait generator 100.
  • the time series of the object model speed obtained by executing the process it is set to a value at the time corresponding to the previous calculation processing cycle (current time in the previous calculation processing cycle).
  • the initial value of the output of the integrator 210 is the previous calculation processing cycle in the time series of the target object position obtained by executing the processing of S03 in the previous calculation processing cycle of the gait generator 100. It is set to a value at the time corresponding to the period (current time in the previous calculation processing cycle).
  • the target object posture trajectory of the target object motion trajectory is determined so as to substantially coincide with the direction of the target object speed, for example.
  • the processing of S03 is not necessarily performed by the gait generator 100. It is not necessary to carry out every calculation processing cycle. Therefore, for example, the process of S03 is performed for each step of the robot 1 or for each of a plurality of calculation processing cycles of the gait generator 100, or when the movement plan is changed in S01, or the estimated disturbance force is relatively low. You can do it when there is a big change!
  • the target object movement trajectory and the target object reaction force trajectory for a predetermined period in the future from the current time are provisionally determined.
  • process of S03 includes the function as the first acting force trajectory determining means in the present invention because the target object reaction force trajectory is provisionally determined as described above.
  • the inverse dynamic model is used as the target dynamic model, but the target target position trajectory and the target target reaction force trajectory are provisionally determined using the forward dynamic model. You may rub.
  • the target object position trajectory is generated by integrating the target object speed based on the movement plan, and the minute value (first-order differential value) of the target object speed is set to the target object speed trajectory.
  • the estimated disturbance force is subtracted from the product of the mass.
  • the target object reaction force trajectory may be generated by reversing the sign of the subtraction result.
  • the planned landing position / posture and scheduled landing time of the free leg foot 22 of the mouth bot 1 are provisionally determined based on the target object motion trajectory provisionally determined as described above.
  • the planned landing position / posture and scheduled landing time to be provisionally determined here are the planned landing position / posture and landing scheduled time of the free leg foot 22 in the current time's gait, which is the target gait that is currently being created. This includes the planned landing position and scheduled landing time of the free leg foot 22 for multiple steps (at least two steps) of the robot 1.
  • the planned landing position The scheduled landing time includes at least the planned landing position / posture and landing time of the free leg foot 22 of the current time gait and the planned landing position / posture and planned landing time of the free leg foot 22 of the next time's gait.
  • the planned landing position / posture and the planned landing time of the free leg foot 22 are, for example, the landing position / posture of the free leg foot 22 at the planned landing time with respect to the target object motion trajectory determined in S03. The relationship between the object position and orientation at that time and a predetermined relative position and orientation is determined so as to be different.
  • the planned landing position / posture and the planned landing time of the free leg foot 22 do not necessarily have to be determined in such a manner as to follow the target object position / posture.
  • the object 120 is pushed and moved.
  • the planned landing position / posture of the free leg foot 22 in each gait is maintained constant over time.
  • the planned landing position / posture and scheduled landing time of the free leg foot 22 are designed according to the motion of the robot 1 that moves only by the target object motion trajectory. Determined on demand
  • the planned landing position / posture of the free leg foot 22 is not slipped after the heel of the free leg foot 22 is landed and the free leg foot 22 is in contact with the floor.
  • the position / posture of the supporting leg coordinate system in each unit gait is correspondingly determined. Will be determined. Specifically, the supporting leg coordinate system in each unit gait is such that the foot 22 of the unit gait immediately preceding the unit gait matches the planned landing position / posture.
  • the center force of the ankle joint of the leg 2 having the flat 22 is a coordinate system with the origin at the point where the perpendicular extending to the floor intersects the floor.
  • the ZMP trajectory parameters tentatively determined here are the target ZMP of the current gait specified by the gait (hereinafter referred to as the temporary target ZMP and ⁇ ⁇ ), the planned landing position / posture and planned landing time (details) Is determined by the landing position / posture and scheduled landing time of the free leg foot 22 in the previous gait and the current gait), and is supported by the current time gait. It exists near the center of the so-called support polygon (including the ground plane) and is determined so as not to make a sudden change.
  • the temporary target ZMP of the gait this time is located in the position where the stability margin of the robot 1 is as high as possible within the contact surface (or support polygon) of the robot 1 and changes rapidly.
  • the ZMP trajectory parameters are determined as follows.
  • the temporary target ZMP determined by such a guide has a pattern as shown in Fig. 11 (a), for example.
  • Fig. 11 (a) shows the pattern of the position of the temporary target ZMP in the X-axis direction.
  • the position and time of the break point of the temporary target ZMP pattern (trajectory) are provisionally determined as the ZMP trajectory parameters.
  • Force moment trajectory time series of instantaneous values of the object reaction force moment in the current time's gait
  • the position / posture of the part has a predetermined position / posture relationship with respect to the position / posture of the object 120.
  • the hand position / posture trajectory of the robot 1 (which defines the trajectory of the point of action of the object reaction force on the robot 1) is obtained so as to match the motion trajectory (position / posture trajectory) of the part.
  • the object reaction force moment around the temporary target ZMP at each time is calculated from the obtained hand position / posture trajectory, the target object reaction force trajectory, and the temporary target ZMP trajectory.
  • the time series of the calculated object reaction force moment is obtained as the object reaction force moment trajectory.
  • the gait parameters are the foot trajectory parameters that define the foot position / posture trajectory in the normal turning gait, the ZMP trajectory parameters that define the target ZMP trajectory, and the object reaction force that defines the target object reaction force moment trajectory. Includes trajectory parameters.
  • the normal turning gait is the motion state (foot position / posture, body position) of the robot 1 at the gait boundary (in this embodiment, the gait boundary for each step) when the gait is repeated. It means a cyclic gait that does not cause discontinuities in the posture and other conditions.
  • the normal turning gait which is a periodic gait, follows the gait of two steps of the robot 1, that is, the first turning gait following the current time gait and the first turning gait.
  • the gait consisting of the second turning gait is the gait for one cycle of the normal turning gait and the gait is repeated for one cycle.
  • turning is used here as a force that can be included in a turn in a broad sense because straight turn means straight turn when the turn rate is zero.
  • the normal turning gait may be abbreviated as a normal gait.
  • one cycle of the normal gait is composed of at least two steps of gaits (two consecutive unit gaits). It is also possible to set a complex normal gait with a gait of three steps or more for one cycle. However, the normal gait is used only to determine the divergent component (details will be described later) at the end of the current gait as described below. For this reason, using a normal gait with a gait of three or more steps as one cycle is less effective, although the gait generation process is complicated. Therefore, the gait for one cycle of the normal gait in this embodiment is configured by gaits for two steps (first and second turning gaits).
  • the normal gait composed of two steps of gait is regarded as one step gait.
  • the normal gait is temporarily determined by the gait generator 100 to determine the divergent component at the end of the current gait. This is a hypothetical virtual gait (in the arithmetic processing of the generation device 100) and is not output from the gait generation device 100 as it is.
  • divergence means that the horizontal position of the upper body 3 of the bipedal mobile robot 1 is shifted to a position far from the positions of both feet 22 and 22.
  • the value of the divergent component means that the horizontal position of the upper body 3 of the biped robot 1 is the position of both feet 22 and 22 (more specifically, the support leg coordinates set on the ground contact surface of the support foot 22) This is a numerical value indicating how far away from the origin of the system.
  • the gait is generated using the divergent component as an index so that the desired gait is continuously generated without causing the divergence. That is, after determining the initial divergent component of the normal gait following the current time's gait, the terminal divergent component of the current time's gait matches the initial divergent component of the normal gait (more generally, the current time's gait) The current time's gait is generated (the gait parameter that defines the current time's gait is determined) so that the body position / posture of the body converge to the body position / posture of the normal gait).
  • the initial divergent component of the normal gait is based on the gait parameters determined in S11, at the initial stage of the normal gait (initial stage of the first turning gait) and at the end (end of the second turning gait). ) And the normal gait condition that the motion state of the robot 1 is the same (hereinafter referred to as the boundary condition of the normal gait) on the robot 1 dynamic model.
  • the basic guidelines for generating such a gait are the same as those of PCT International Publication No. WO / 02 / 40224A1 previously proposed by the present applicant. Therefore, in the following description of the normal gait, technical matters different from those described in PCT International Publication No. WO / 02 / 40224A1 will be mainly described, and detailed descriptions of similar technical matters may be omitted. .
  • the foot trajectory parameters of the gait parameters of the normal gait are the foot leg 22 and the supporting leg foot 22 at the initial stage and the end of the second turning gait, respectively. It consists of the position and orientation of the free leg foot 22 and the gait cycle of each turning gait, etc.
  • the foot position / posture trajectory is connected in the order of the current time's gait, the first turning gait, and the second turning gait. To be determined. A specific setting method will be described below with reference to FIG.
  • the initial swing leg position / posture of the first turning gait is the foot position / posture at the end of the current time's gait viewed from the support leg coordinate system of the next time's gait.
  • the support leg foot position / posture at the end of this time's gait is such that the support leg foot 22 in the initial gait is in contact with the floor and does not slip.
  • the position and orientation of the supporting leg foot 22 when the flat 22 is rotated in the pitch direction until almost the entire bottom surface touches the floor (this is the landing position / posture of the free leg foot 22 in the previous gait) Match).
  • the position / posture of the next time's gait support leg coordinate system is determined in correspondence with the planned landing position / posture of the free leg foot 22 of the current time's gait as shown in FIG.
  • the initial supporting leg foot position / posture of the first turning gait is the free leg foot position / posture of the current time gait as seen from the next time's gait supporting leg coordinate system.
  • the free leg foot position / posture at the end of this time's gait is determined to be the position / posture corresponding to the planned landing position / posture of the free leg foot 22 of the current time's gait (or the next time's gait support leg coordinate system determined accordingly). Is done.
  • the free leg foot position / posture at the end of the current time gait is determined so that is the planned landing position / posture of the free leg foot 22 of the current time's gait.
  • the free leg foot position / posture at the end of the first turning gait is the free leg foot position / posture at the end of the next gait viewed from the next time's gait supporting leg coordinate system.
  • This next gait end free leg foot position / posture is the same as the method for determining the current time gait end free leg foot position / posture.
  • the position and orientation corresponding to the next gait supporting leg coordinate system) determined in this way are determined.
  • the supporting leg foot position / posture at the end of the first turning gait is such that the supporting leg foot 22 whose position / posture coincides with the next time's gait supporting leg coordinate system is kept in contact with the floor and does not slip. Position and posture of the supporting foot foot 22 when the foot foot 22 is rotated in the pitch direction until almost the entire bottom surface of the foot foot touches the floor (this position and posture is the landing position of the free leg foot 22 of this time's gait) Agree with the planned position).
  • the initial swing leg position / posture of the second turning gait is the foot position / posture at the end of the first turning gait as seen from the next time's gait supporting leg coordinate system.
  • the foot position / posture at the initial stage of the second turning gait is the foot position / posture at the end of the first turning gait as seen from the next time's gait supporting leg coordinate system.
  • the free leg foot position / posture at the end of the second turning gait is the position / posture when viewed from the next time's gait supporting leg coordinate system. It is determined to match the foot position / posture.
  • the foot position / posture at the end of the second turning gait The position / posture viewed from the next time's gait supporting leg coordinate system is determined so as to match the foot position / posture of the current time's gait supporting leg viewed from the current time's gait supporting leg coordinate system.
  • the gait cycle of the first turning gait and the second turning gait is the next gait cycle (this is the scheduled landing time of the free leg foot 22 of the current time gait and the free leg foot of the next time gait 22). It is set to be the same as the estimated landing time.
  • 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 both cycles according to at least the next gait cycle.
  • the movement parameters (including time parameters such as both leg support periods) of the current time's gait, the first turning gait and the second turning gait are determined according to the parameters determined above. Determine appropriately so as to satisfy the conditions (for example, whether the speed of the actuator is within the allowable range, whether it does not exceed the movable angle, or does not interfere with the floor, etc.).
  • the ZMP trajectory parameter of the gait parameters of the normal gait is the same as when the target ZMP trajectory parameter is provisionally determined in S07, and the ZMP trajectory specified by the ZMP trajectory parameter is The foot trajectory parameters of the normal gait so as to increase the margin and prevent a sudden change (support leg foot 22 in the initial and final gait of the first turning gait and the second turning gait, respectively) The position and orientation of the free leg foot 22 and the support time of both legs are determined).
  • the object reaction force moment trajectory parameter of the normal gait is determined so as to satisfy the condition of periodicity. For example, assume that the object reaction force moment trajectory corresponding to the current time's gait, the next time's gait and the next time's gait is as shown in Fig. 13 (a). Here, the object reaction force moment trajectory of the gait this time is the trajectory obtained in S09.
  • the object reaction moment trajectory of the next gait is the ZMP trajectory defined by the target object reaction force trajectory and target object motion trajectory of the next gait obtained in S03 and the ZMP trajectory parameters of the normal turning gait ( The trajectory is calculated in the same way as S09 based on the ZMP trajectory in the first turning gait of the first turning gait.
  • the object reaction force moment trajectory of the next gait is the ZMP trajectory defined by the target object reaction force trajectory and target object motion trajectory of the next gait obtained in S03 and the ZMP trajectory parameters of the normal turning gait.
  • the trajectory is calculated in the same way as S09 based on (the ZMP trajectory in the first and second turning gait of the normal turning gait).
  • the object reaction force moment trajectory parameter of the normal turning gait is determined so that the object reaction force moment trajectory of the normal turning gait becomes, for example, the trajectory shown in Fig. 13 (b). .
  • the object reaction force moment trajectory from the beginning to the end of the first turning gait of the normal turning gait continues to the object reaction force moment trajectory of the current time gait, and the object of the next gait.
  • the object reaction force moment trajectory of the second turning gait in the normal turning gait is continuous with the object reaction force moment trajectory of the first turning gait and immediately before the end of the initial force of the second turning gait.
  • the initial value of the first turning gait (the steady turning gait) It is determined to continuously change to the same value as the initial value.
  • the time tx corresponds to the time of the break point of the ZMP trajectory of the normal turning gait, for example.
  • the initial divergent component of the normal turning gait is determined.
  • the initial state of motion of the first turning gait is determined on the dynamic model determined by the robot 1.
  • the initial divergent component of the normal turning gait is determined so that the terminal state (including the terminal divergent component) of the movement of the second turning gait matches.
  • FIG. 14 shows the structure of the robot simplified model.
  • the simplified robot model shown in the figure is a three-mass model.
  • the dynamics of the leg 2 and the upper body 3 are configured to be non-interfering with each other, and the overall dynamics of the robot 1 is , Represented by their linear combination.
  • This simplified robot model is composed of an inverted pendulum A and two leg mass points 2m and 2m (support leg mass point and free leg mass point).
  • Each leg mass point 2m is a mass point corresponding to each leg 2.
  • Each foot mass point 2m is a fixed point in the local coordinate system that is arbitrarily fixedly set on the foot 22 of the corresponding leg 2 and its position corresponds to each foot position / posture. It is supposed to be determined uniquely. For example, each foot mass point 2m is set to a point shifted from the representative point of the bottom surface of the foot 22 toward the ankle joint by a predetermined distance by force toward the center of the ankle joint.
  • Inverted pendulum A is composed of a free fulcrum a that moves horizontally, one mass point b, and a variable-length link c that connects fulcrum a and mass point b without mass. In this case, even if the link c is inclined, the link c expands and contracts, and the height h of the mass point b viewed from the fulcrum a is kept constant.
  • the mass point b of the inverted pendulum A corresponds to the mass point of the upper body 3 of the robot 3, and hereinafter, the inverted pendulum mass point b is changed to the upper mass point b. Sometimes it is.
  • the position of the inverted pendulum mass point b is hereinafter referred to as the inverted pendulum position for short.
  • the mass of the upper mass point b includes the mass of the upper body 3 and the masses of the arm bodies 5 and 5 and the head 4.
  • the horizontal position of the upper body 3 of the robot 1 is geometrically determined from the horizontal position of the inverted pendulum mass point b. Specifically, for example, the horizontal position of the representative point of the upper body 3 (the XY coordinates seen in the support leg coordinate system), that is, the horizontal component of the upper body position matches the horizontal position of the inverted pendulum mass point b. Is done. In other words, as shown in FIG. 14, the representative point of the upper body 3 and the inverted pendulum mass point b are on the same vertical line.
  • inverted pendulum ZMP Since ZMP of inverted pendulum A is at the position of free fulcrum a (no moment is generated around fulcrum a), the position of fulcrum a of inverted pendulum A will be referred to as inverted pendulum ZMP. Call it ZMPpend.
  • msup mass of support leg mass 2m
  • mswg mass of free leg mass 2m
  • mb inverted pendulum mass b
  • xsup support Position of leg mass point 2m
  • xswg Position of free leg mass point 2m
  • xb Inverted pendulum position (position of upper body point b)
  • h Inverted pendulum height (inverted pendulum A fulcrum a to inverted pendulum mass point b
  • xb, xsup, and xswg are expressed as three-dimensional vectors (XYZ coordinate vectors).
  • dX / dt represents the first derivative of X
  • d2X / dt2 represents the second derivative of X.
  • g represents a gravitational acceleration constant.
  • G is a gravitational acceleration vector whose X, Y, and Z components are 0, 0, and 1 g, respectively.
  • Equation 01 is a dynamic definition equation of the total leg inertia moment about the point P.
  • leg ZMP is described as ZMPfeet and defined by Equation 02. However, the height of the leg ZMP (Z component of ZMPfeet) is the same as the height of the point P. This leg ZMP is a value that artificially corresponds to the resultant force of the inertial force and gravity generated by the motion of both legs 2 and 2.
  • the object reaction force moment around the target ZMP divided by the total mass mtotal of the robot 1 is defined as the object reaction force ZMP, which is described as ZMPobj. That is, ZMPobj is defined by the following equation 03.
  • ZMPobj Object reaction force moment around target ZMP Zmtotal — Formula 03
  • the target ZMP, leg ZMP (ZMPfeet), inverted pendulum ZMP (ZMPpend), and object reaction force ZMP are approximately The linear relationship of Equation 04 holds.
  • Formula 04 is a relational expression on the sagittal plane (XZ plane). On the lateral plane (YZ plane), the sign of the third term on the right side of Formula 03 is " Invert from "1" to "+”.
  • the target foot position / posture, the target reaction force moment around the target ZMP, the target ZMP, and the target body posture are calculated by the following dynamic calculation.
  • the target body position can be determined from the above.
  • the dynamic calculation will be described with reference to the block diagram of FIG. Figure 15 is a block diagram showing this dynamic calculation.
  • Target both foot position / posture (supporting foot foot 22 and free leg foot)
  • the leg ZMP (ZMPfeet) is calculated by the leg ZMP calculator 220 using the equations 01 and 02 from the trajectory of the target position / posture 22) and the action point P set as described above.
  • the object reaction force ZMP (ZMPobj) is calculated by multiplying the object reaction force moment around the target ZMP by lZmtotal by the multiplication unit 222. Then, the calculated ZMPfeet multiplied by mfeetZmtotal at the multiplication unit 224 and ZMPobj are subtracted from the target ZMP at the subtraction unit 226, and the subtraction result is multiplied by mtotalZmb at the multiplication unit 228. The right-hand side of is computed. Thus, the inverted pendulum ZMP (ZMPpend) is calculated. The ZMPpend obtained by the above arithmetic processing is on the sagittal plane. When calculating the ZMPpend on the lateral plane, the subtraction unit 226 uses the result of inverting the sign of the operation result of the multiplication unit 222. Enter it.
  • the inverted pendulum horizontal position xb is calculated based on the above-described equation 05. Furthermore, based on the horizontal position xb of the inverted pendulum and the target body posture, the body position determiner 230 determines the target body position. In this case, the body position determiner 230 sets the inverted pendulum horizontal position xb as the target body horizontal position. Further, the target body vertical position is determined based on the target body posture, for example, by the body height determination method previously proposed by the present applicant in Japanese Patent Laid-Open No. 10-86080. In the present embodiment, the target body posture of the robot 1 is, for example, a vertical posture.
  • the initial divergent component of the normal gait is determined based on the robot simple model described above. If the divergence component in the gait is set as q and the horizontal velocity (or body horizontal velocity) vb of the mass point b of the inverted pendulum A is set as vb, q is defined by the following equation 06 in this embodiment.
  • ⁇ ⁇ is the natural frequency of the inverted pendulum ⁇ , that is, the square root of gZh.
  • Expression 07 is an expression describing the divergent component q on the sagittal plane, and the expression describing the divergent component q on the lateral plane is the sign of the entire fourth term on the right side of Expression 07. The expression is reversed from “one” to “+”.
  • the divergent component (hereinafter referred to as the initial divergent component) and the divergent component (hereinafter referred to as the second divergent gait) at the beginning of the normal turning gait (the first turning gait) Focusing on the terminal divergence component and! /, U), let q [0] (the divergence component at time 0) be the initial divergence component, and q [k] (the divergence component at time kAt) be the terminal divergence component.
  • the first term on the right side of Expression 07 represents the terminal divergence component generated by the initial divergence component.
  • the second term on the right side represents the terminal divergence component generated by the target ZMP pattern.
  • the third term on the right-hand side represents the terminal divergent component generated by the movement of both legs 2 and 2 (movement of both leg mass points 2m and 2m).
  • the fourth term on the right side represents the terminal divergence component generated by the object reaction force moment pattern.
  • the second term on the right side is Wzmptotal
  • the third term on the right side is Wfeet
  • the fourth term on the right side Is written as Wobj. That is, WzmptotaU Wfeet and Wobj are defined by the following equations 08a, 08b, and 08c.
  • Formula 08c defines Wobj on the sagittal plane, and Wobj on the lateral plane is obtained by inverting the sign of the right side of Formula 08c.
  • the initial divergent component that satisfies the boundary condition of the normal turning gait is the current gait.
  • the initial divergence component as seen in the support leg coordinate system of the normal turning gait following (the next time's gait support leg coordinate system ( ⁇ ' ⁇ 'coordinate system) in Fig. 12)
  • the terminal divergence component of the normal turning gait (the normal turning gait following the current gait) as seen from the supporting leg coordinate system (next gait supporting leg coordinate system ( ⁇ "' ⁇ "' coordinate system in Fig.
  • Equation 12 shows that the initial divergence component q [0] and the terminal divergence component q [k] This is a condition to be met.
  • inv () is the inverse of the matrix in parentheses. Yes, I is the identity matrix.
  • each foot at each time i At (i 0, 1, ..., k 1) of the normal turning gait.
  • ZMPfeetD] is obtained by obtaining the instantaneous value of the flat position and posture and applying it to the expression expressing the above-mentioned expressions 01 and 02 in a discrete system.
  • the foot position / posture is calculated for each foot 22 by using, for example, a time limit settling filter proposed by the present applicant in Japanese Patent No. 3233450.
  • the calculated foot position / posture trajectory is such that the free leg foot 22 in each of the first turning gait and the second turning gait of the normal turning gait rises from the start time of the one-leg support period.
  • the free leg foot 22 moves toward the planned landing position, and at the scheduled landing time of the free leg foot 22, the free leg foot 22 lands at a position corresponding to the planned landing position. Orbit.
  • Wfeet is calculated by calculating the right side of the above-mentioned expression 08b.
  • the foot position / posture trajectory is fixed. The trajectory is maintained at the landing position / posture.
  • the foot position / posture trajectory may be generated so that the free leg foot 22 is once raised and then returned to the original landing position / posture.
  • step time ⁇ t used in the calculation of Wzmptotal, Wfeet, and Wobj is preferably the same as the calculation processing cycle of the gait generator 100, but it is set to a longer time to perform the calculation. You can reduce the calculation processing time!
  • the initial divergent component q [0] is analytically obtained.
  • the initial divergence of the normal turning gait that almost matches the terminal divergence component of the normal turning gait as seen by the supporting leg coordinate system force of the gait following the normal turning gait following the current gait Try to determine the ingredients.
  • the gait parameters are the foot trajectory parameter that defines the foot position / posture trajectory in the current gait, the ZMP trajectory parameter that defines the target ZMP trajectory, and the object reaction force trajectory parameter that defines the target object reaction force moment trajectory. including.
  • the foot trajectory parameter temporarily determined here corresponds to the first leg motion parameter in the present invention. Therefore, the process of S13 has a function as the first leg motion parameter determination means in the present invention.
  • the foot trajectory parameter of the gait parameters of the current time's gait is the position and orientation of the supporting leg foot 22 and the free leg foot 22 at the beginning and end of the current time's gait, It consists of a gait cycle.
  • the free leg foot position posture of the current time's gait is the free leg foot position / posture of the current time's gait at the end of the previous time's gait, as seen from the current time's gait supporting leg coordinate system.
  • Flat position This time's initial gait support leg foot position / posture is the support leg foot position / posture of the current time's gait at the end of the previous time's gait, as seen from the current time's gait support leg coordinate system.
  • the free leg foot position / posture at the end of the current time's gait is determined according to the planned landing position / posture of the free leg foot 22 of the current time's gait. Specifically, the position when the free leg foot 22 of the current time's gait is rotated by a predetermined angle in the pitch direction so that it does not slide from the foot position / posture at the end of the current time's gait. This time the gait end free leg foot position figure so that the posture is the planned landing position posture of the gait this time The momentum is determined.
  • the foot position / posture at the end of the current time's gait is determined so that the foot 22 does not slip while the foot 22 is in contact with the floor and the position / posture is matched to the coordinate system of the current time's gait.
  • the position and posture of the supporting leg foot 22 when the foot is rotated in the pitch direction until almost the entire bottom surface touches the floor (this position and posture is the planned landing position and posture of the free leg foot 22 of the previous gait). Match).
  • the support leg foot 22 at the end of the current time's gait is in a posture substantially parallel to the floor surface, but this is not necessarily required. For example, at the end of the gait, make sure that the heel side of the foot 22 is away from the floor.
  • the ZMP trajectory parameter is tentatively determined in S07
  • the target ZMP trajectory defined by this ZMP trajectory parameter has a pattern as shown in FIG. 11 (a), for example.
  • the object reaction force moment trajectory parameter in the gait parameters of the current time's gait is the same as the object reaction force moment trajectory specified in S09. To be decided.
  • the ZMP trajectory parameters of the gait parameters provisionally determined in S15 are corrected so that the current time's gait converges to the normal turning gait. More specifically, the ZMP trajectory parameters are modified so that the final divergent component of the current time's gait approximately matches the initial divergent component of the normal turning gait.
  • the temporary target ZMP in order to make the final divergent component of the current time's gait approximately coincide with the initial divergent component of the normal turning gait, the temporary target ZMP has a ZMP as shown in FIG. 11 (b). By adjusting the correction amount, the temporary target ZMP is corrected, thereby obtaining the target ZMP as shown in FIG. 11 (c).
  • the basic guideline for correcting the temporary target ZMP is the same as that of PCT International Publication No. WO / 02 / 40224A1. is doing.
  • the process of S17 will be specifically described below with a focus on the differences.
  • variables having the same meaning as described for the process for determining the initial divergent component of the normal gait (S13) are defined above for the normal gait.
  • the correction of the XMP direction component of ZMP will be explained.
  • the ZMP correction amount is trapezoidal as shown in FIG. 11B, and its height is a.
  • the trapezoid break point time is set according to the break point time of the temporary target ZMP trajectory as shown.
  • the ZMP correction amount is generated during the one leg support period of the current time's gait.
  • Equation 15 k A t is the end time of the current gait, q [0] Is the initial divergent component of the current time's gait, Wfeet is the final divergent component generated by the movement of both legs 2 and 2 of the current time's gait, and Wobj is the final divergent component generated by the object reaction force moment trajectory of the current time's gait.
  • Equation 16 the trapezoidal height of the ZMP correction amount is obtained by Equation 16.
  • the initial divergent component q [0] is the inverted pendulum position (or body position) at the beginning of the current time's gait. From the change speed, it is calculated by the equation 06.
  • Wzmptmp is obtained in the same manner as when Wzmptotal related to the normal turning gait is obtained from the temporary target ZMP trajectory defined by the ZMP trajectory parameters provisionally determined in S15.
  • Wtrim is obtained in the same manner as when Wzmptotal for the normal turning gait is obtained from the ZMP correction amount pattern determined with trapezoidal height a being 1.
  • Wfeet is obtained based on the above equation 08b in the same manner as the Wfeet related to the normal turning gait is obtained from the foot trajectory parameter provisionally determined in S15.
  • Wobj is obtained based on the above equation 08c in the same manner as when obtaining Wob j related to the normal turning gait from the object reaction force moment trajectory parameter provisionally determined in S15.
  • the ZMP trajectory parameter is corrected by adding the ZMP correction amount determined by the trapezoidal height a thus obtained to the temporary target ZMP trajectory.
  • the ZMP correction amount is obtained analytically, but may be obtained exploratively.
  • the current time's gait is temporarily created using the target ZMP trajectory obtained by correcting the temporary target ZMP with the ZMP correction amount in which the trapezoid height a is set to an appropriate value, and the temporary gait is created.
  • the process of correcting the trapezoidal height a value so as to reduce the differential force is repeated.
  • the ZMP correction amount may be determined based on the value of the trapezoidal height a when the final divergent component of the current time gait that is provisionally created and the initial divergent component of the normal gait substantially coincide.
  • the routine proceeds to S19, where it is determined whether the target ZMP determined by the corrected ZMP trajectory parameter is appropriate force. Specifically, the target ZMP force after correction If the ground contact surface force of the support leg foot 22 of this time's gait does not deviate or is not biased near the boundary of the contact surface, that is, the stability margin of robot 1 If it can be secured, it is judged that the target ZMP is appropriate. Otherwise, it is judged that the target ZMP is inappropriate.
  • the process proceeds to S21, and the target object related to the elements related to parameters other than the ZMP trajectory parameters of the current time's gait parameters, for example, the object reaction force moment trajectory parameters. Correct the reaction force trajectory.
  • the target object reaction force trajectory is such that the ZMP correction amount becomes as small as possible when the processing of S09 to S17 is executed again after the correction (at least the ZMP obtained before the target object reaction force trajectory correction). It is corrected so that it is smaller than the correction amount.
  • the target object motion trajectory (position trajectory) based on the movement plan decelerates the object 120 in the X-axis direction from time tl and stops it at time t3.
  • the graph g3 in FIG. 16 (a) is an example of the target body position trajectory determined corresponding to the target object motion trajectory.
  • the ground contact surface of the supporting leg foot 22 may be deviated (in this case, the judgment result in S19 is NO). If the target gait continues to be generated as it is, a target gait that collides with the object 120 at a certain time t4 is generated as shown in the figure.
  • the target object reaction force is set to the current time as shown in Fig. 16 (b) with respect to the target object reaction force trajectory temporarily determined in S03 as described above.
  • the target object reaction force trajectory is corrected so as to increase in the negative direction of the X axis within a certain period ⁇ 1 after t2.
  • the target object reaction force is corrected so that the acting force applied to the object 120 from the robot 1 (the force obtained by inverting the sign of the target object reaction force) is increased in the positive direction of the X axis.
  • the period ⁇ 1 during which the target object reaction force is increased or the magnitude of the increase A F1 is determined by the ZMP correction amount determined by the processing of S09 to S17 based on the target object reaction force after the correction.
  • the determination result of S19 is YES.
  • Such ⁇ T1 or ⁇ F1 can be obtained analytically or exploratoryly.
  • the object in the current time gait parameter is set so that the robot 1 moves the body 3 such that the robot 1 positively pushes the object 120.
  • the reaction force moment trajectory parameter is determined by S15.
  • the target gait generated based on the gait parameters this time is such that the upper body 3 of the robot 1 does not collide with the object 120 while ensuring a stability margin. .
  • the target object reaction force trajectory currently set (the target object reaction force trajectory set when the judgment result at S19 is YES) It is determined whether or not there is a change point of the target object reaction force within the period.
  • the change point of the target object reaction force is a point at which the target object changes relatively largely before and after the time of the change point in the present embodiment.
  • the change point is the point where the magnitude of the first-order differential value (temporal rate of change) of the target object reaction force exceeds a predetermined threshold, or the magnitude (absolute value) of the target object reaction force.
  • a point that exceeds a predetermined threshold is extracted.
  • the time of the change point extracted in this way is referred to as the object reaction force change time.
  • the target foot position / posture calculation process uses the finite-time settling filter proposed by the applicant in Japanese Patent No. 3233450 in the same manner as described for the process of calculating ZMPfeetD] in the process of S13. Calculated for each foot. [0160] Furthermore, based on the ZMP trajectory parameter of the current time gait parameter and the object reaction force moment trajectory parameter (the trajectory parameter after correction of S17), the target ZMP momentary until the end time of the current gait, respectively.
  • the instantaneous value of the target object reaction force moment in time series is calculated in time series.
  • the target body posture is, for example, a steady vertical posture.
  • the instantaneous values of the desired foot position / posture, the desired ZMP, and the desired object reaction force moment in the desired gait from the current time to the object reaction force change time are time-series. Is calculated.
  • the gait (next gait) including the object reaction force change time is the virtual current time gait
  • the next gait is the virtual next time gait
  • the next gait is the virtual next time gait.
  • the virtual current time gait means the current time gait when the object reaction force change time is regarded as the current time.
  • a virtual target object reaction force trajectory that is a virtual target object reaction force trajectory after the object reaction force change time is determined.
  • This virtual target object reaction force trajectory is different from the trajectory after the target reaction force change time among the target object reaction force trajectories finally determined by the loop processing of S09 to S21 described above.
  • the target object reaction force does not change at the object reaction force change time, and the time immediately after the object reaction force change time (for example, the object reaction force change time
  • the trajectory is continuously maintained at the value (time before the predetermined step time).
  • the virtual target object reaction trajectory indicates that the actual position of the object 120 at the object reaction force change time is the target object motion trajectory.
  • the target object is set assuming that the actual target reaction force does not change according to the target target reaction force at the target reaction force change time due to causes such as the positional force at the It is a reaction force trajectory.
  • the movement speed (object speed) of the object 120 according to the movement plan (current movement plan) of the object 120 is not until the time tl in the next gait. It is assumed that 0 is increased from time tl to time t2 and then maintained at a constant speed from time t2.
  • the target object reaction force trajectory generally increases to a value with a target object reaction force between time tl and time t2, as shown by a solid line graph in FIG. 17 (b), for example.
  • the virtual target object reaction force trajectory is such that the object reaction force does not increase from time tl, which is the object reaction force change time as shown by the two-dot chain line graph in FIG.
  • the trajectory is determined so that the object reaction force is continuously maintained at the same value as before time tl.
  • the virtual target object reaction force trajectory determined in this way corresponds to the second robot 'target inter-object target acting force trajectory in the present invention. Therefore, the process of S27 has a function as the second acting force trajectory setting means in the present invention.
  • the planned landing position / posture and scheduled landing time of the free leg foot 22 after the object reaction force change time are provisionally determined.
  • the planned landing position / posture and scheduled landing time of the free leg foot 22 tentatively determined here are at least the planned landing position / posture and planned landing time of the free leg foot 22 in the virtual current time gait and the virtual next time gait. included.
  • the expected landing position / posture and time of the free leg foot 22 of the virtual current time gait are determined by the normal turning gait following the current time gait (the gait parameter is finally determined by the loop process of S09 to S21).
  • the planned landing position / posture and time of the free leg foot 22 of the virtual next time gait are the normal turning gait following the current gait (S09 to S21). Tentatively determined to be the same as the planned landing position / posture and time of the free leg foot 22 of the first 2nd turning gait of the first turning gait) .
  • the planned landing position / posture and time of the free leg foot 22 of the virtual next gait are the same as the planned landing position / posture and time of the free leg foot 22 of the next gait determined in S05. Alternatively, it may be determined based on the target object motion trajectory until the next gait.
  • a ZMP trajectory parameter defining the target ZMP trajectory of the virtual current time gait after the object reaction force change time is provisionally determined.
  • ZMP trajectory of the virtual current time gait after the target reaction force change time so that the target ZMP is positioned approximately near the center of the ground contact surface of the support leg foot 2 2 of the virtual current time gait (support polygon in the two-leg support period) Parameters are provisionally determined.
  • the object reaction force change Calculate the target reaction force moment trajectory (trajectory up to the end time of the virtual gait) around the target ZMP after the time (target ZMP specified by the ZMP trajectory parameter provisionally determined in S31).
  • This calculation process consists of the target object motion trajectory after the object reaction force change time (the trajectory after the object reaction force change time in the trajectory temporarily determined in S03) and the virtual target object reaction force trajectory.
  • a gait parameter (virtual normal gait parameter) of a virtual steady turning gait as a periodic gait following the virtual current time gait is determined.
  • the foot trajectory parameters, ZMP trajectory parameters, and object reaction force moment trajectory parameters of the virtual steady turning gait are determined.
  • the foot trajectory parameter of the virtual normal gait parameter (the supporting leg foot 22 and the free leg foot foot at the beginning and the end of the first turning gait and the second turning gait in the virtual steady gait, respectively)
  • the foot position / posture trajectory is in the order of the virtual current time gait, the first turning gait of the virtual steady turning gait, and the second turning gait.
  • the foot position / posture trajectory of the connected and virtual steady turning gait is periodic It is determined to satisfy the conditions.
  • S35 is executed for the first time after S29
  • the planned landing position and scheduled landing time of the free leg foot 22 of the virtual current time gait and the virtual next time gait are provisionally determined in S29.
  • the process of S35 is executed after S47 described later, the one modified as described later in S47 is used.
  • the ZMP trajectory parameter of the virtual normal gait parameter is the virtual normal gait determined by the foot trajectory parameter based on the foot trajectory parameter of the assumed normal gait parameter, as in the processing of S07. It is determined that the target ZMP is located near the center of the ground contact surface of the support leg foot 22 (support polygon in the both-leg support period).
  • the object reaction force moment trajectory parameter of the virtual normal gait parameter includes the target object motion trajectory and virtual target object reaction trajectory of the virtual next time gait, and the virtual next time gait (virtual normal gait).
  • the object reaction force moment trajectory of the virtual next time gait obtained in the same manner as S09 based on the ZMP trajectory specified by the ZMP trajectory parameter of the first first turning gait of ZMP specified by ZMP trajectory parameters of target object motion trajectory and virtual target object reaction trajectory of gait and virtual next gait (first and second turning gait of virtual steady gait) Based on the trajectory, the object reaction force moment trajectory of the virtual next time gait obtained in the same manner as in S09 is determined in the same manner as in S11.
  • the virtual next gait and the virtual target gait reaction force trajectory of the virtual next gait are constant trajectories after the object reaction force change time of the virtual current time gait.
  • the process proceeds to S37, where the initial divergent component of the virtual steady turning gait is determined. That is, the initial state of the virtual steady turning gait is determined so that the initial state of the first turning gait of the virtual steady turning gait matches the terminal state of the second turning gait. This process is performed in the same manner as the process of S13.
  • the process proceeds to S39, and the gait parameters (foot trajectory parameters, ZMP trajectory parameters, object reaction force moment trajectory parameters) of the virtual current time gait are provisionally determined.
  • the foot trajectory parameters of the virtual current time gait are determined in the same manner as the process of S15.
  • the ZMP trajectory parameters of the virtual current time gait are determined to be the same as those provisionally determined in S31.
  • the ZMP trajectory parameters of the virtual current time gait determined here are provisional values.
  • the target reaction force moment trajectory parameter of the virtual current time gait is the target target calculated in S25 until the target reaction force moment trajectory of the virtual current time gait specified by the target current reaction time moment trajectory. It is determined to match the object reaction force moment trajectory and to match the object reaction force moment trajectory determined in S33 from the object reaction force change time to the end time of the virtual current time gait.
  • the foot trajectory parameter provisionally determined in S39 corresponds to the second leg motion parameter in the present invention.
  • the target ZMP of the virtual current gait specified by the ZMP trajectory parameter corrected in S41 is appropriate. This determination is made in the same manner as in S19. In other words, the target ZMP of the virtual current time gait should be in a position that is not too close to the boundary within the contact surface of the foot 22 of the virtual current time's gait (in the support polygon during the two-leg support period).
  • ZMP constraint condition and V the dynamic constraint condition
  • S43 determines whether or not the target ZMP of the virtual current time gait can be corrected appropriately. More specifically, when the planned landing position / posture and scheduled landing time of the free leg foot 22 in the virtual current time gait are corrected in the later-described processing of S47 and the processing from S31 is executed again, the determination result of S43 Can be determined as YES (whether the target ZMP of the virtual current time gait can satisfy the ZMP constraint condition).
  • the target ZMP of the virtual current time gait deviates from the ground contact surface of the supporting leg foot 22 of the virtual current time gait, and the amount of deviation (such as the distance of the boundary force on the ground surface) is a predetermined amount. If it is greater than this, the determination result in S45 is NO. Alternatively, if the number of times the planned landing position / posture and scheduled landing time of the free leg foot 22 in the virtual current time gait are corrected in the later-described processing of S47 exceeds a predetermined number, the determination result of S45 is NO. [0179] Supplementally, the determination processing in S45 corresponds to the processing of the determination means in the present invention.
  • the process returns to S01 and re-determines the movement plan of the object 120 (corrects the movement plan).
  • the movement plan of the object 120 to be redetermined in this case Is determined so that the posture of the robot 1 can be kept stable even when the actual object reaction force does not change as the target object reaction force trajectory occurs.
  • the movement plan is redetermined so as to reduce the rapid fluctuation of the target object reaction force.
  • Fig. 17 and Fig. 17 show specific examples of the case where the planned landing position of the free leg foot 22 of the virtual current time gait is corrected in S47 and the specific case where the movement plan is determined again in S01.
  • the target object position trajectory position trajectory in the X-axis direction
  • the movement plan current movement plan
  • the supporting leg foot 22 and the free leg foot 22 are respectively represented by a white rectangle and a hatched rectangle.
  • the supporting leg feet with reference numerals 22L1, 22 R2, and 22L3 are the supporting leg feet at the end time of the current time's gait, the next time's gait, and the next time's gait, respectively.
  • the attached free leg feet are the free leg feet at the end time of the current time's gait, the next time's gait and the next time's gait, respectively.
  • the position of the free leg foot relating to each of these gaits is an example of a position determined according to the target object position trajectory of the solid line graph in FIG.
  • the position of the free leg foot 22R1 related to the current time gait is the planned landing position of the free leg foot 22 of the current time gait determined in S05, and the position of the free leg foot 22L2 related to the next time gait.
  • the position is the planned landing position of the free leg foot 22 of the virtual current time gait determined in S29, and the position of the free leg foot 22R3 related to the next time gait is the free leg foot of the virtual next time gait determined in S29.
  • the black spot in the supporting leg foot 22L1 related to the current time's gait is the target at the end time of the current time's gait specified by the ZMP trajectory parameters of the current time's gait finally determined in the processing of S01 to S21.
  • the position of the ZMP is shown as an example.
  • the black spot in the support leg foot 22R2 related to the next gait is the target ZMP at the end time of the virtual current gait defined by the ZMP trajectory parameters initially determined in S31.
  • the black spot in the support leg foot 22L3 related to the next gait is the virtual next gait (virtual next gait (virtual gait) defined by the ZMP trajectory parameter of the virtual steady turning gait first determined in S35.
  • the target ZMP (target ZMP at the end time of the virtual current time gait) specified by the corrected ZMP trajectory parameter is the virtual current time gait.
  • the ground contact surface force of the support leg foot 22R2 also deviates and may become a point indicated by a circle in FIG. This is because the target gait of the virtual current time gait is temporarily generated in S25, assuming that the target reaction force changes according to the target object reaction force until the target reaction force change time of the virtual current time gait.
  • the object reaction force moment trajectory of the gait parameters of the virtual steady turning gait follows that the object reaction force does not change at the object reaction force change time and follows the virtual target object reaction force trajectory.
  • the target target object is the target reaction force change time. Since the reaction force is expected to increase rapidly, the target gait (especially the target body position trajectory) is generated in S25 so that the upper body 3 of the robot 1 accelerates forward. However, after the object reaction force change time, it is planned that the object reaction force will be continuously maintained in the same way as immediately before the object reaction force change time according to the virtual target object reaction force trajectory. Therefore, it is necessary to decelerate the upper body 3 of the robot 1 in order to converge the virtual current time gait into the virtual steady turning gait.
  • the target ZMP (target ZMP at the end time of the virtual current time gait) defined by the corrected ZMP trajectory parameter is the virtual current time gait.
  • the planned landing position of the free leg foot 22 of the virtual current time gait is, for example, the reference numeral 22L2 in the figure from the position indicated by the reference numeral 22L2 in FIG. It is corrected to the front side of robot 1 as shown by the dashed rectangle with a '.
  • the target ZMP defined by the corrected ZMP trajectory parameters is the ZMP trajectory. It is possible to satisfy the constraint conditions, and the determination result in S43 may be YES
  • the object reaction force change time By correcting the planned landing position of the free leg foot 22 in a gait including, the stability of the posture of the robot 1 can be secured.
  • the target ZMP (target ZMP at the end time of the virtual current time gait) defined by the corrected ZMP trajectory parameter is
  • the support leg of the virtual current time gait such as the point indicated by ⁇ in FIG. 18, may deviate significantly from the contact surface of the foot 22R2.
  • the planned landing position of the free leg foot 22 of the virtual current time gait or the virtual current time gait and the virtual next time gait is within the mechanical movable range of the free leg foot 22.
  • the ZMP trajectory parameters may not be determined so that the target ZMP satisfies the ZMP constraint condition. In this case, the judgment result of S45 is NO.
  • the original movement plan of the target object 120 is inappropriate for ensuring the stability of the robot 1 continuously.
  • Redetermined corrected.
  • the movement plan is corrected as shown by the broken line graph in FIG. That is, the moving speed of the object 120 according to the corrected movement plan increases more slowly from the time tl (target reaction force change time) than the original movement plan shown by the solid line graph in Fig. 17 (a).
  • time t2 target reaction force change time
  • the travel plan is re-determined so that it is maintained at a constant V and speed smaller than the original travel plan.
  • the object reaction force corresponding to this revised movement plan is the original movement plan during the period from time tl to time t2 as shown by the broken line graph in Fig. 17 (b).
  • the reaction force is smaller than the corresponding object reaction force. Therefore, a sudden change in the target object reaction force at the object reaction force change time tl is alleviated.
  • the judgment result of S43 can be set to YES.
  • the current gait parameter is set so that the stability of the robot 1 can be ensured even if the actual object reaction force does not change according to the target object reaction force by the processes of S01 to S47 described above. It will be a decision.
  • the first turning gait of the normal turning gait, and the second turning gait based on the gait parameters of each gait.
  • the desired body position / posture trajectory as described. More specifically, the target bipedal foot position / posture trajectory is calculated based on the foot trajectory parameters of each gait, and the target ZMP trajectory, The object reaction force moment trajectory is calculated. Note that the calculation process of the desired both foot position / posture trajectory is performed in S13. As in the case of the process for calculating ZMPfeetD] in the process, it is calculated for each foot 22 using the finite time settling filter proposed by the applicant in Japanese Patent No. 3233450.
  • the target body position / posture is determined by using the target both foot position / posture trajectory, the target ZMP trajectory, and the object reaction force moment trajectory for the calculation process of the block diagram of FIG.
  • the target body posture is constantly in a vertical posture.
  • the target body position / posture trajectory of the target motion of robot 1 is calculated so as to satisfy the dynamic equilibrium condition that the horizontal component of the moment generated around the target ZMP is zero.
  • the target hand position / posture trajectory is the target object motion trajectory in the order of the current time gait, the first turning gait of the normal turning gait, and the second turning gait in the same manner as obtained in S09.
  • the target object motion trajectory corresponding to the first turning gait and the second turning gait of the normal turning gait is the next gait and next gait of the target object motion trajectories provisionally determined in S03. This is the target object motion trajectory corresponding to the condition.
  • this geometrical constraint condition is that the presence or absence of interference between the object 120 and the robot 1 (such as the upper body 3 hitting the object 120) and the rotational angle of the joint of each arm body 3 are mechanically limited. It is a condition such as the power to stay within.
  • the presence / absence of interference between the upper body 3 of the robot 1 and the object 120 can be determined from the target body position / posture and the target object motion trajectory. Further, since the rotation angle of the joint of each arm body 3 can be determined from the target body position / posture and the target hand position / posture, it is possible to determine whether the rotation angle is within the mechanical limits or not.
  • the determination processing in S51 is sequentially performed on instantaneous values such as target body postures at predetermined time intervals from the current time to the end of the second turning gait. That is, if the determination result in S51 is OK for each instantaneous value, it is determined in S53 whether or not the check for all times from the current time to the end of the second turning gait has been completed. . If the determination result of S53 is NO, the determination process of S51 is repeated.
  • the target object movement trajectory (position trajectory) in the X-axis direction based on the movement plan at the current time tl is the trajectory indicated by graph g4 in the figure (hereinafter referred to as the original trajectory and ⁇ ⁇ )
  • the original trajectory and ⁇ ⁇ the trajectory indicated by graph g4 in the figure
  • the target body position and the target object position are too close at a certain time t2 in the future, causing interference between the body 3 and the object 120, and the determination result in S51 is NG.
  • the target object position at time t2 at which the interference occurs is shifted in the positive direction of the X axis, i.e., away from the robot 1, as indicated by the dashed arrow in the figure, and a period extending before and after time t2.
  • This corrected trajectory g5 is determined so as to smoothly connect to the original trajectory g4 at both ends of the period ⁇ ⁇ 2 in order to avoid a sudden change in the behavior of the robot 1. Further, the shift amount ⁇ at time t2 is determined so that there is no interference between the upper body 3 of the robot 1 and the object 120 (so that the determination result in S51 is OK).
  • the time corresponding to the judgment result force SNG in S51 is calculated. It may be possible to correct the planned landing position / posture of the free leg foot 22 in the gait including it. For example, when interference occurs at time t2 as shown in FIG. 19, the planned landing position / posture for the gait including that time t2 is adjusted to a position / posture that is away from the object 120 in the X-axis direction. Correct it. Alternatively, both the target object motion trajectory and the planned landing position / posture may be corrected. Alternatively, the movement plan may be corrected.
  • This calculation process is performed as follows. That is, the instantaneous value of the desired foot position / posture is calculated based on the foot trajectory parameter of the current time gait parameters. This calculation process is performed by using the finite-time settling filter proposed by the present applicant in Japanese Patent No. 3233450 in the same manner as described for the process of calculating ZMPfeetD] in the process of S13. Calculated every 22nd.
  • the instantaneous value of the target ZMP is calculated based on the ZMP trajectory parameter of the current time's gait parameters, and the instantaneous target reaction moment is calculated based on the target reaction force moment trajectory parameter. A value is calculated.
  • the target object reaction force moment is calculated by the dynamic calculation shown in FIG.
  • An instantaneous value of the body position / posture is calculated.
  • the target body posture is, for example, a vertical posture.
  • the target body posture may be changed as needed.
  • the robot simplification model be a dynamic model that takes into account the change in the angular momentum of the robot 1 due to the posture change of the upper body 3.
  • each leg 2 has six degrees of freedom, so if the target body position / posture and the desired foot position / posture are determined, the rotation angle of each joint of each leg 2 is uniquely determined. Determined.
  • S The target body position / posture obtained in 57 is further corrected by the processing of S65 described later.
  • the previous value of the final corrected target hand position (the value in the previous calculation processing cycle), in the previous calculation processing cycle (the calculation processing cycle of the gait generator 100).
  • the actual object position which is the actual position (movement position) of the object 120 to be detected, is estimated.
  • the final corrected target hand position is obtained by the arm main controller 106 described later, and corresponds to the command value of the actual hand position of the mouth bot 1. Therefore, for example, the previous value of the final corrected target hand position is set as the previous value of the actual hand position of the robot 1, and a position having a predetermined relative positional relationship with this is estimated as the actual object position. It is also possible to mount a gyro sensor or an acceleration sensor on the object 120 and to estimate the actual object position based on the detected value force.
  • FIG. 20 is a block diagram showing a calculation process for obtaining the target object position, the target object reaction force, and the estimated disturbance force in the target object motion in S61, and a part denoted by reference numeral 238 in the figure.
  • the object dynamic model represents the relationship between the force acting on the object 120 and the movement of the object 120. Since this object dynamic model is partly different from that shown in Fig. 10, in the following explanation, the object dynamic model in Fig. 20 is replaced with the object dynamic model in Fig. 10. To distinguish it from the object dynamics model 2 and!
  • the basic structure of the object dynamics model 2 is the same as that of the object dynamics model of Fig. 10, and the force acting on the object 120 (more specifically, the translational force in the horizontal direction) is input as the input.
  • the reciprocal of the mass M of the object 120 1ZM Is multiplied by the multiplication unit 244 to obtain the motion acceleration of the object 120, and this is sequentially integrated (double-integrated) by the integrators 246 and 250.
  • the integrator 250 has a model speed manipulated variable in addition to the output of the integrator 246 (integral value of motion acceleration). It is designed to be additionally entered.
  • This model speed manipulated variable is a speed manipulated variable determined by the feedback control rule so that the deviation between the actual target position and the target target position approaches 0 by the model speed manipulated variable determining unit 252. Then, it is determined by the following equation 17.
  • the model speed manipulated variable determining unit 252 in FIG. 20 is an arithmetic processing unit that performs the calculation of the right side of Equation 17.
  • the model speed manipulated variable determination unit 252 determines the actual object position (current value obtained in S33 in the current computation processing cycle) and the target object position (previous value obtained in S35 in the previous computation processing cycle). Thus, the object position deviation, which is the difference between them, is obtained by the subtraction unit 254. The obtained object position deviation is multiplied by the gain Kel by the multiplier 256 (proportional term), and the object position deviation multiplied by the gain Ke2 by the multiplier 258 is further integrated by the integrator 260. The model speed manipulated variable is calculated by adding the product (integral term) by the adder 262.
  • the calculated model speed manipulated variable is added to the output of the integrator 246 by the adder 248 provided on the input side of the integrator 250 of the object dynamic model 2 and then input to the integrator 250. .
  • the model speed manipulated variable determining unit 252 may be input to the object dynamic model 2 in an additional manner.
  • the integrator 260 is omitted, and the output of the multiplier 258 is additionally input to the integrator 246. At this time, the sum of the output of the integrator 246 and the output of the multiplier 256 is input to the integrator 250. You may make it input.
  • the output of the object dynamic model 2 (the output of the integrator 250) is the same as that of FIG. [0212]
  • the output of the adder 248 integrated by the integrator 250 is obtained as the instantaneous value of the target object position. Further, the output of the adding unit 248 becomes the object model speed that is the moving speed of the object 120 on the object dynamic model 2.
  • the required value of the acting force (translation force) on the object 120 is the current target object motion trajectory (the target when the judgment result in S53 is YES)
  • Multiplier 242 adds a predetermined gain Kv to the deviation (target object speed minus object model speed) obtained by subtractor 240 from the target object speed based on the object motion trajectory) and the object model speed. It is obtained by multiplication. That is, the required value of the translational force input to the object dynamic model 2 is the same as that in FIG. 10 so that the deviation between the object target speed and the object model speed converges to 0. In this example, it is determined by the proportional control law). However, since the object model speed in this case is the output of the adder 248 as described above, it is different from that in FIG. The target object speed is obtained as the first derivative of the position trajectory of the current target object motion trajectory.
  • a value obtained by reversing the sign of the required translational force value obtained as described above is obtained as an instantaneous value of the target object reaction force.
  • the required value of the translational force is input to the multiplication unit 244 of the object dynamics model 2
  • the model speed manipulated variable is input to the addition unit 248, whereby the object dynamics model 2
  • the instantaneous value of the target object position is output from the integrator 250.
  • the object model speed as the motion state quantity of the object 120 on the object dynamic model 2 is the model speed manipulated variable for making the deviation between the target object position and the actual object position close to zero.
  • the target object position is sequentially determined by the dynamics calculation of the object dynamic model 2 while correcting the target.
  • a part denoted by reference numeral 264 in FIG. 20 is an estimated disturbance force determining unit that performs processing for obtaining an estimated disturbance force.
  • the estimated disturbance force determining unit 264 includes an object model speed,
  • the translational force requirement value is input.
  • the object model velocity is input to a conversion unit 266 represented by a transfer function of the form M ′ sZ (Tc ′ s + l), and the conversion unit 266 determines the translational force acting on the object 120.
  • An estimate is calculated.
  • M in the transfer function of the conversion unit 266 is the mass of the object 120, and Tc is a predetermined time constant.
  • the conversion unit 266 A filter with a time constant Tc is obtained by multiplying the differential value of the Dell velocity (the first-order differential value) by the mass of the object 120 (this corresponds to the instantaneous value of the total force acting on the object 120).
  • the result of the tulling process is obtained as an estimated value of the translational force acting on the object 120.
  • the estimated value calculated by the conversion unit 266 is the translational force that generates the motion acceleration (differential value of the object model speed) of the object 120 on the object dynamic model 2 with the time constant Tc. It follows with a first-order lag.
  • the instantaneous value of the estimated disturbance force is calculated by subtracting the required value of the translation force, which is the output of the multiplier 242, from the estimated value of the translation force by the subtractor 268.
  • the instantaneous value of the estimated disturbance force obtained here is used in the calculation process of S02 (see Fig. 10) as described above.
  • the instantaneous value of the target object posture in the target object motion is determined so as to substantially match the direction of the target object speed, for example.
  • the above is the calculation processing of S61.
  • the estimated disturbance force obtained by the calculation processing of S61 is an estimate of the actual disturbance force that acts on the target 120 other than the robot 1 in real time, and thus this estimated disturbance force. Is used in the calculation processing of S02 (input to the object dynamic model of FIG. 10), and the behavior (motion state) of the object 120 on the object dynamic model of FIG. It can be close to 120 behavior (operational state).
  • the process proceeds to S63, where the instantaneous value (current value) of the target hand position / posture is determined based on the instantaneous value of the target object motion obtained in S61.
  • the instantaneous value of the target hand position / posture is obtained in the same manner as when the target hand position / posture trajectory is obtained in S09.
  • the desired body position / posture is obtained by using the robot simple model to calculate the target Z MP on the robot simple model. It is calculated so that it is satisfied (so that the resultant force of inertial force, gravity, and object reaction force moment of robot 1 is zero in the horizontal component of the moment generated around the target ZMP). Therefore, the target gait obtained by the processing up to S63 is a gait in which the floor reaction force moment horizontal component around the target ZMP is zero on the robot simple model.
  • the robot simple model has a great advantage in that it can effectively reduce the computation load of the gait generator 100, but the dynamic accuracy is not necessarily high. Yes. Therefore, in this embodiment, a part of the target gait (specifically, using a robot dynamic model with high dynamic accuracy (hereinafter referred to as a full model) (specifically, Correct the target body position / posture instantaneous value and the floor reaction force moment instantaneous value around the target ZMP). This correction process is called full model correction, and this full model correction is executed in S65 after the process of S63.
  • a full model specifically, Correct the target body position / posture instantaneous value and the floor reaction force moment instantaneous value around the target ZMP.
  • This full model correction is performed in the same manner as that described in Japanese Patent Laid-Open No. 2002-326173 previously proposed by the applicant of the present application, for example. Therefore, the description in this specification is omitted.
  • the full model correction may be performed, for example, in the same manner as described in PCT International Publication No. W 0 03/057427 A1 by the present applicant.
  • the robot 1 When the robot 1 is moving while pushing the object 120, there is a step (a convex part of the floor) that is not assumed in the current movement plan (such as the movement plan as described above).
  • the target object motion trajectory based on the movement plan and the actual object motion trajectory (hereinafter referred to as the actual object motion trajectory)
  • the gait generator 100 generates a gait by determining a gait meter or the like based on the target object motion trajectory, and therefore, there is a difference between the target object motion trajectory and the actual object motion trajectory. If it becomes excessive, it will be difficult to generate a gait that can ensure the continuous stability of the robot 1. Therefore, in S01 in the present embodiment, when the deviation becomes large to some extent (or is predicted to increase), the movement plan determined in S01 is corrected in real time.
  • the trajectory of the actual object position (the time series of the actual object position estimated in S59) is the trajectory as shown by the graph g7 in FIG. 21 (a).
  • the deviation between the target object position based on the pre-correction movement plan and the actual object position increases with time.
  • the object position deviation difference between the actual object position and the target object position obtained in S61 of the previous calculation processing cycle, that is, the difference obtained by the subtraction unit 254 in FIG.
  • the magnitude (absolute value) of the figurine position deviation is compared with a predetermined value.
  • the object position deviation becomes larger than the predetermined value (time t2 in FIG. 21 (a)
  • the object position deviation as shown by the graph g8 in FIG. 21 (a) is obtained.
  • the pre-correction movement plan is corrected to suppress the increase.
  • the corrected movement plan is hereinafter referred to as a corrected movement plan.
  • a corrected movement plan In the example of FIG.
  • the corrected movement plan is a movement plan that gradually decelerates until the movement speed of the object 120 becomes almost zero after time t2.
  • the post-correction movement plan is determined so that the target object motion specified thereby is smoothly continuous without causing a sudden change with respect to the pre-correction movement plan.
  • the followability of the actual object position to the target object position trajectory corresponding to the corrected movement plan is as follows. If the difference between the target object motion trajectory and the actual object motion trajectory becomes small to some extent, the travel plan is corrected so that the corrected travel plan approaches the pre-correction travel plan. This revised movement plan is hereinafter referred to as the revised revision plan.
  • the movement speed of the object 120 based on the movement plan after re-correction is made closer to the movement speed based on the movement plan before correction, but the target object position trajectory based on the movement plan after re-correction. Can be made closer to the object position trajectory based on the pre-correction movement plan.
  • the target body position / posture trajectory, the target ZMP trajectory, and the target object reaction force trajectory are input to the object reaction force balance controller 102.
  • the object reaction force balance controller 102 eliminates the deviation between the target object reaction force and the actual object reaction force that is the reaction force that the actual robot 1 receives from the object 120 (closes to 0).
  • Target Compensation for target reaction force balance control to correct the target floor reaction force moment around ZMP Calculates the total floor reaction force and corrects the target body position / posture of the target motion The body position / posture is obtained.
  • the processing of the object reaction force balance control device 102 will be described later.
  • the desired foot position / posture trajectory, the desired ZMP trajectory, and the desired total floor reaction force trajectory in the desired gait are input to the leg main control device 104.
  • the leg main controller 104 inputs the corrected target body position / posture and the compensated total floor reaction force for the object reaction force balance control from the object reaction force balance controller 102.
  • the leg main control device 104 uses a joint control actuator (electric motor) for the legs 2 and 2 to follow the movement of the target gait (except for the movements of the arms 5 and 5) and the floor reaction force by compliance control processing. ) To control.
  • the restored total floor reaction force that should be generated in the target ZMP is calculated and applied to the target ZMP.
  • the actual total floor reaction force moment component (which is obtained from the detection values of the 6-axis force sensor 50 of the legs 2 and 2) is the restored total floor reaction force, the desired total floor reaction force, and the object reaction force balance.
  • the desired foot position / posture is corrected so that it matches the moment component of the resultant force with the control compensation total floor reaction force.
  • the corrected target foot position / posture is referred to as a corrected target foot position / posture.
  • the main leg control device 104 then moves the two leg bodies 2 so that the actual joint displacement follows the target joint displacement of both legs 2 and 2 determined from the corrected target foot position / posture and the corrected target body position / posture.
  • 2 joint actuators are controlled (motor drive commands for each leg 2 are output to each joint actuator).
  • the target hand position / posture trajectory and the target object reaction force trajectory in the target gait are input to the arm main controller 106.
  • the corrected target body position / posture is input to the arm main control device 106 from the object reaction force balance control device 102.
  • the arm main control device 106 controls the joint actuators (electric motors) of the arm bodies 5 and 5 so as to follow the target hand position / posture trajectory and the target object reaction trajectory by compliance control processing. More specifically, the target hand position / posture is corrected according to the difference between the detected value (actual object reaction force) of the 6-axis force sensor 52 and the target object reaction force.
  • the corrected target hand position / posture is referred to as the final corrected target hand position / posture.
  • the arm main controller 106 then moves both arms so that the actual joint displacement follows the target joint displacement of both arms 5, 5 determined from the final corrected target hand position and the corrected target body position / posture.
  • 5 and 5 joint actuators are controlled (motor drive commands for each arm body 5 are output to each joint actuator).
  • FIG. 22 is a block diagram functionally showing the processing of the object reaction force balance control apparatus 102.
  • the target object reaction force moment around the target ZMP by the target object reaction force and the actual object reaction force moment around the target ZMP by the actual object reaction force are calculated.
  • the object reaction force moment deviation which is a deviation, is calculated by the moment deviation calculation unit 270.
  • the target object reaction force moment includes the target object reaction force and target ZMP output from the gait generator 100, and the final corrected target hand position / posture determined by the arm main controller 106 (more details). Is calculated from the previous control processing cycle).
  • the actual object reaction force moment is the detected value of the 6-axis force sensor 52 (actual object reaction force), the target ZMP, and the final corrected target hand position / posture (more specifically, the value in the previous control processing cycle) And calculated from
  • the center-of-gravity position perturbation amount calculation unit 272 calculates a target center-of-gravity position perturbation amount that is the target perturbation amount of the entire center-of-gravity position of the robot 1.
  • the target center-of-gravity position perturbation amount has a meaning as a perturbation amount of the entire center of gravity of the robot 1 in order to resolve the object reaction force moment deviation in the long term by gravity acting on the robot 1, for example, the target object
  • the value is proportional to the reaction force moment.
  • the deviation between the total center of gravity position model perturbation amount which is the perturbation amount of the entire center of gravity of the robot 1 on the perturbation dynamic model calculated by the perturbation dynamic model described later, and the target center of gravity position perturbation amount is subtracted.
  • This deviation force is also calculated by the unit 274, and the compensation force floor for the object reaction force balance control is the moment manipulated variable around the target ZMP to converge the deviation to 0 by the feedback control law 276, for example, the PD control law.
  • the reaction force moment is calculated.
  • the sum of the total reaction force moment compensated for the object reaction force balance control and the deviation of the object reaction force moment is calculated by the adder 278.
  • the output of the adder 278 shows the relationship between the perturbation of the overall center of gravity of the robot 1 and the moment perturbation around the target ZMP, as well as the relationship between the perturbation of the overall center of gravity and the perturbation of the upper body posture.
  • the body position / posture perturbation amount is calculated using this perturbation dynamics model.
  • the perturbation dynamics model is a model that describes the relationship (dynamic relationship) between the perturbation of the total center of gravity of the robot 1 and the moment perturbation around the target ZMP by the following equation 18.
  • mtotal * hG * d2 ⁇ xG / dt2 AxG water mtotal water g + ⁇ ⁇ ?? Equation 18
  • hG is the target ZMP force height to the total center of gravity
  • AxG is the horizontal perturbation of the total center of gravity
  • ⁇ ⁇ is Moment horizontal component around the target ZMP.
  • the other variables are the same as those defined for the robot simple model.
  • Expression 18 is an expression on the sagittal plane, and the relational expression on the lateral plane may be obtained by inverting the sign of the second term on the right side of Expression 18.
  • This equation 18 has the mass of the total mass mtotal of robot 1, and the dynamics of the inverted pendulum such that the moment horizontal component generated around the target ZMP, which is the fulcrum of the mass, is ⁇ Mx. It is a formula which shows a behavior.
  • the perturbation amount AxG of the entire center of gravity position of the robot 1 and the perturbation amount of the upper body position (hereinafter referred to as this)
  • Equation 19 For perturbation motion, it can be assumed that Equation 19 holds approximately.
  • the perturbation amount AxG of the total center of gravity is calculated by using the output of the adder 278 as ⁇ ⁇ on the right side of Equation 18, and this AxG force is also calculated by Equation 19.
  • the body position perturbation amount is obtained.
  • the corrected target body posture out of the corrected target body posture is assumed to match the target body posture, and the perturbation amount of the body posture is 0.
  • the object reaction force balance controller 102 adds the body position perturbation amount output from the perturbation dynamics model 280 in this way to the target body position / posture (output of the gait generator 100). By adding in 282, the corrected target body position / posture is calculated.
  • the object reaction force trajectory is changed to the target object reaction force trajectory at the object reaction force change time.
  • the next gait virtual current time gait
  • the movement plan is corrected and the current time's gait parameters are determined again, and the robot 1 moves according to the instantaneous value of the desired gait calculated based on the current time's gait parameters. Take control. Therefore, even when the object reaction force does not change along the target object reaction force trajectory, the target gait can be generated so as to ensure the continuous stability of the robot 1.
  • FIG. 23 is a flowchart showing a process corresponding to the process of FIG. 5 according to the first embodiment in the process of the gait generator 100 according to the second embodiment. As shown in FIG. 23, the second embodiment is different from the first embodiment only in the processing in the case of the determination result power SNO of S19.
  • the estimated landing position / posture or scheduled landing time is set so that the ZMP correction amount becomes as small as possible when the processing of S07 to S17 is executed again after the correction (at least the planned landing position / posture or landing). It is corrected so that it is smaller than the ZMP correction amount obtained before the scheduled time is corrected. After the correction, the processing from S07 is executed again. Otherwise, the first embodiment Is the same as the state.
  • the target object reaction force trajectory as shown in FIG. 25 (a) is a step-like trajectory such as a rising force from the middle of the next time's gait, for example.
  • the ZMP correction amount (X-axis direction component) determined in the processing of S17 is For example, it may become relatively large as shown by the solid line in Fig. 25 (b), and the judgment result of S19 may be NO.
  • the planned landing position / posture of the free leg foot 22 of the next time gait is indicated on the supporting leg foot 22L of the current time gait as indicated by the broken line in FIG. Correct it so that it is closer with. In other words, the planned landing position / posture of the free leg foot 22L is corrected so as to make the stride of the next time gait smaller. If the processing from S05 is executed again after correcting the planned landing position / posture of the next time's gait in this way, the ZMP correction amount determined in S17 becomes smaller as shown by the broken line in FIG. 25 (b). As a result, the determination result in S 19 is YES.
  • the second turning step of the normal gait is determined by the above-described method for determining the foot trajectory parameters of the normal gait.
  • the free leg foot position at the end of the foot will also approach the initial free leg foot position of the second turning gait (the planned landing position of the free leg foot 22 R of the current time's gait).
  • the planned landing position / posture of the free leg foot 22L of the next time's gait was corrected, but there is room to correct the planned landing position / posture of the free leg foot 22R of the current time's gait. If there is, it may be corrected. Or, try to correct the planned landing positions and postures for both the current time gait and the next time gait.
  • the planned landing position / posture is corrected.
  • the scheduled landing time of the free leg foot 22 of at least one of the gait may be corrected.
  • the scheduled landing time of the next gait may be delayed.
  • the second embodiment of the present invention is powerful, the same operational effects as in the first embodiment can be obtained.
  • the planned landing position / posture or the scheduled landing time of the free leg foot 22 is corrected when the determination result of S19 is NO. Correct the target object reaction force trajectory.
  • the gait parameter of the normal gait which is a periodic gait, is determined, and then the current time gait is added to the normal gait.
  • Force to determine the current time's gait parameter so that it converges (to match the initial divergent component of the current time's gait to the initial divergent component of the normal gait) In the present invention, this is not essential. .
  • the finally determined target force between the robot and the object the target object reaction force in each of the above embodiments
  • the target movement position of the object the target object in each of the above embodiments. If the robot motion can be controlled to satisfy the (object motion trajectory), the robot motion should be controlled by a method different from the method described in the above embodiments.
  • a robot such as a bipedal robot performs an operation of moving an object
  • the robot performs the operation smoothly while ensuring the stability of the robot. It is useful as something that can be made.
  • FIG. 1 is a diagram showing a schematic configuration of a biped mobile robot as a legged mobile robot in an embodiment of the present invention.
  • FIG. 2 is a block diagram showing the configuration of a control unit provided in the robot of FIG.
  • FIG. 3 is a block diagram showing a functional configuration of a main part of the control unit of FIG.
  • FIG. 4 is a diagram showing a relationship between a robot and an object in the embodiment.
  • FIG. 5 is a flowchart showing a calculation process of the gait generator in the first embodiment.
  • FIG. 6 is a flowchart showing a calculation process of the gait generator in the first embodiment.
  • FIG. 7 is a flowchart showing a calculation process of the gait generator in the first embodiment.
  • FIG. 8 is a flowchart showing a calculation process of the gait generator in the first embodiment.
  • FIG. 9 is a diagram showing an example of a movement plan determined in S02 of FIG.
  • FIG. 10 is a block diagram showing processing of S02 in FIG.
  • FIG. 11 (a) is a diagram showing an example of a temporary target ZMP trajectory related to S07 in FIG. 5
  • FIG. 10 (b) is a diagram showing an example of the ZMP correction amount determined in S17 in FIG. 10 (c) shows an example of the target ZMP trajectory modified in S17.
  • FIG. 12 is a diagram showing an example of foot trajectory parameters determined in S11 of FIG.
  • Fig. 13 shows an example of the object floor reaction force moment trajectory of the current time gait
  • Fig. 13 (b) shows the object reaction force moment of the normal gait determined by SI 1 in Fig. 5.
  • the figure which shows the example of an orbit.
  • FIG. 14 is a diagram showing an example of a robot dynamics model used in the embodiment.
  • FIG. 15 is a block diagram showing a process for obtaining a body position using the robot dynamic model of FIG.
  • FIGS. 16 (a) and 16 (b) are diagrams for explaining the processing of S21 in FIG.
  • Figure 17 (a) is a graph and figure illustrating the temporal change in the speed of an object according to the movement plan.
  • 17 (b) is a graph illustrating the temporal change of the object reaction force corresponding to FIG. 17 (a).
  • FIG. 18 is a diagram for explaining an example of the processing of S47 in FIG. 6 and the movement plan correction processing when the determination result of S45 is NO.
  • FIG. 19 is a diagram for explaining the processing of S55 in FIG.
  • FIG. 20 is a block diagram showing processing of S61 in FIG.
  • FIGS. 21 (a) and 21 (b) are diagrams for explaining the movement plan correction process of S01 of FIG.
  • FIG. 22 is a block diagram showing processing of the object reaction force balance control device shown in FIG. 3.
  • FIG. 24 is a diagram for explaining the processing of S21 ′ in FIG.
  • FIGS. 25 (a) and 25 (b) are diagrams for explaining the processing of S21 ′ in FIG.

Landscapes

  • Engineering & Computer Science (AREA)
  • Mechanical Engineering (AREA)
  • Chemical & Material Sciences (AREA)
  • Combustion & Propulsion (AREA)
  • Transportation (AREA)
  • Robotics (AREA)
  • Automation & Control Theory (AREA)
  • Manipulator (AREA)
  • Toys (AREA)

Abstract

　ロボットの新たな目標歩容を生成するとき、将来の所定時点でロボット・対象物間の作用力の軌道が目標軌道と異なる軌道に変化したとして、該所定時点以後の目標歩容を生成した場合に所定の動力学的制約条件を満たすことができるか否かを判断する。満たすことができない場合には、対象物の移動計画を修正した上で、ロボット・対象物間の作用力の目標軌道などを再決定し、それを用いて新たな目標歩容を生成する。これにより、対象物を移動させる作業をロボットに行わせるための歩容を、将来のロボットと対象物との間の作用力が目標値からずれても、ロボットの姿勢の安定性を確保することができるようにロボットの歩容を生成する。

Description

明 細 書
脚式移動ロボットの歩容生成装置
技術分野
[0001] 本発明は、脚式移動ロボットに対象物を移動させる作業を行わせるための目標歩 容を生成する装置に関する。
背景技術
[0002] 2足移動ロボットなどの脚式移動ロボットに、ある対象物を押して移動させるなどの 作業を行わせる場合には、ロボットはその脚体の先端部が接触する床力 床反力を 受けるだけでなぐ対象物力 反力を受ける。このように対象物からロボットが受ける 反力を本明細書では対象物反力という。
[0003] このように脚式移動ロボットに対象物反力が作用する状態でロボットの歩容を生成 したり、動作制御を行なう技術としては、例えば本願発明者による特開平 10— 2304 85号公報（以下、特許文献 1という）に見られるものが知られている。この技術は、口 ボットの運動により発生する慣性力と、ロボットに作用する重力および対象物反力との 合力が目標 ZMP (目標床反力中心点)まわりに発生するモーメントの鉛直成分を除く 成分 (水平成分)が 0になる（目標床反力中心点に作用する床反力と上記合力とが釣 り合う）という動力学的平衡条件を満たすように、目標歩容を生成するものである。さら に、この技術では、ロボットに作用する外力が予期しない外力になっても（目標外力と 実外力との偏差がある程度大きくなつても）、ロボットの重心位置がロボットの動バラン スを保ち得るような重心位置に平衡するようにロボットの目標歩容を調整するようにし ている。
[0004] ところで、上記の如くロボットに対象物を移動させる作業を行なわせるロボットの歩 容を生成する場合、ロボットから対象物への作用力の目標値の軌道（時系列）、また は前記対象物反力の目標値の軌道（時系列）を決定しておく必要がある。なお、ロボ ットから対象物への作用力の符号（向き）を反転させたものが対象物反力であるから、 本欄での説明では、以降、代表的に対象物反力の目標値の軌道を決定しておくもの とする。 [0005] この場合、対象物反力の目標値 (以下、目標対象物反力という）の軌道は、例えば 対象物の移動計画（どのタイミングでどのように対象物を移動させるなどの計画)など に基づいて決定される。
[0006] し力しながら、ロボットに作用する実際の対象物反力が目標対象物反力と常に一致 するとは限らない。例えば、ある予定時刻から、対象物にロボットのハンドを係合させ て該対象物を移動させる作業を開始する場合には、目標対象物反力は、その予定 時刻で 0からある値（≠0)に急変するものとなる力 実際のロボットと対象物との位置 関係のずれなどによって、当該予定時刻では、実際の対象物反力が変化しないもの となる場合もある。そして、このような場合には、目標対象物反力軌道を基に生成され た歩容は、前記予定時刻で対象物反力が変化することを予定して生成されたもので あるから、実際の対象物反力が該予定時刻で変化しな力つたときには、ロボットの安 定性が損なわれることとなりやすい。また、該予定時刻で、前記特許文献 1のもののよ うに、実際の対象物反力と目標対象物反力との偏差に応じてロボットの姿勢の修正を しょうとしても、即座にロボットの姿勢を安定な姿勢に修正することができない場合が 多々ある。
[0007] 本発明は力かる背景に鑑みてなされたものであり、対象物を移動させる作業をロボ ットに行わせるための歩容を、将来のロボットと対象物との間の作用力が目標値から ずれる場合を考慮しつつ生成し、該作用力が目標値からずれても、ロボットの姿勢の 安定性を確保することができるようにロボットの歩容を生成することができる脚式移動 ロボットの歩容生成装置を提供することを目的とする。
発明の開示
[0008] 本発明の脚式移動ロボットの歩容生成装置の第 1発明は、かかる目的を達成する ために、上体から延設された複数の脚体を備える脚式移動ロボットに対象物を移動さ せる作業を行わせるための該ロボットの目標歩容を生成する装置において、 新たな目標歩容を生成するとき、少なくとも対象物の移動計画に基づいて、該新た な目標歩容におけるロボットと対象物との間の作用力の目標値の軌道である第 1ロボ ット ·対象物間目標作用力軌道を仮決定する第 1作用力軌道決定手段と、
少なくとも前記対象物の移動計画に基づいて、前記新たな目標歩容におけるロボ ットの脚体の運動を規定する第 1脚体運動パラメータを仮決定する第 1脚体運動パラ メータ決定手段と、
前記仮決定された第 1ロボット '対象物間目標作用力軌道に対し、実際のロボットと 対象物との間の作用力の軌道が将来の所定時点以後に該第 1ロボット '対象物間目 標作用力軌道と異なる軌道に変化したと仮定して、該所定時点以後のロボットと対象 物との間の作用力の目標値の軌道である第 2ロボット '対象物間目標作用力軌道を 設定する第 2作用力軌道設定手段と、
前記所定時点までのロボットの目標歩容を前記第 1脚体運動パラメータと前記第 1 ロボット '対象物間目標作用力軌道とを用いて生成し、さらに該所定時点までの目標 歩容に続く該所定時点以後のロボットの目標歩容を、該所定時点以後のロボットの 脚体の運動を規定する脚体運動パラメータである第 2脚体運動パラメータを可変的 に設定可能な変数として、該第 2脚体運動パラメータと前記第 2ロボット ·対象物間作 用力とを用いて生成した場合に、該所定時点以後のロボットの目標歩容が所定の動 力学的制約条件を満たし得る第 2脚体運動パラメータを設定可能である力否かを判 断する判断手段とを備え、
該判断手段によって、前記所定時点以後のロボットの目標歩容が前記所定の動力 学的制約条件を満たし得る前記第 2脚体運動パラメータを設定可能であると判断さ れたときには、前記第 1脚体運動パラメータと第 1ロボット '対象物間目標作用力軌道 とを用いて前記新たな目標歩容を生成し、前記所定時点以後のロボットの目標歩容 が前記所定の動力学的制約条件を満たし得る前記第 2脚体運動パラメータを設定可 能でないと判断されたときには、前記対象物の移動計画を修正すると共に、その修 正した移動計画を少なくとも用いて前記第 1脚体運動パラメータと第 1ロボット '対象 物間目標作用力軌道とを再決定し、その再決定した第 1脚体運動パラメータと第 1口 ボット ·対象物間目標作用力軌道とを用いて前記新たな目標歩容を生成するようにし たことを特徴とするものである。
力かる第 1発明によれば、前記所定時点以後のロボットの目標歩容、すなわち、該 所定時点以後にロボット '対象物間の作用力の軌道が前記第 1ロボット '対象物間目 標作用力軌道と異なる第 2ロボット '対象物間目標作用力軌道になったと仮定して生 成されるロボットの目標歩容が前記動力学的制約条件を満たすことができる第 2脚体 運動パラメータ（以下、本欄では、この第 2脚体運動パラメータを適正第 2脚体運動パ ラメータということがある）を設定可能である力否かが前記判断手段により判断される。 この判断において、適正第 2脚体運動パラメータを設定可能である場合には、仮に口 ボット'対象物間の実際の作用力が前記所定時点以後に前記第 1ロボット '対象物間 目標作用力と異なる第 2ロボット '対象物間目標作用力軌道になったとしても、脚体 運動パラメータ、ひいては脚体の運動を調整することによって、前記動力学的制約条 件を満たし得る目標歩容を生成することが可能であることを意味する。そこで、第 1発 明では、この場合には、前記第 1脚体運動パラメータと第 1ロボット '対象物間目標作 用力軌道とを用いて前記新たな目標歩容を生成する。
[0010] 一方、前記判断手段の判断にお!、て、適正第 2脚体運動パラメータを設定可能で 無い場合には、仮にロボット '対象物間の実際の作用力が前記所定時点以後に前記 第 1ロボット '対象物間目標作用力と異なる第 2ロボット '対象物間目標作用力軌道に なった場合に、前記動力学的制約条件を満たすような目標歩容を生成できず、ロボ ットの安定性を確保することが困難となる。そこで、第 1発明では、この場合には、対 象物の移動計画を修正すると共に、その修正した移動計画を少なくとも用いて前記 第 1脚体運動パラメータと第 1ロボット '対象物間目標作用力軌道とを再決定する。そ して、その再決定した第 1脚体運動パラメータと第 1ロボット '対象物間目標作用カ軌 道とを用いて前記新たな目標歩容を生成する。
[0011] これにより、仮にロボット '対象物間の実際の作用力が前記所定時点以後に前記第 1ロボット '対象物間目標作用力と異なる第 2ロボット '対象物間目標作用力軌道にな つた場合であっても、脚体運動パラメータ、ひいては脚体の運動を調整することによ つて、前記動力学的制約条件を満たし得る目標歩容を生成することが可能となる。
[0012] 従って、第 1発明によれば、対象物を移動させる作業をロボットに行わせるための歩 容を、将来のロボットと対象物との間の作用力が目標値力もずれる場合を考慮しつつ 生成し、該作用力が目標値からずれても、ロボットの姿勢の安定性を確保することが できるようにロボットの歩容を生成することができる。
[0013] なお、第 1発明において、新たな目標歩容を生成するときには、例えば脚体運動パ ラメータなどを基に、ロボットの運動により発生する慣性力とロボットに対象物力も作 用する作用力とロボットに作用する重力との合力によって発生するモーメントの水平 成分が 0となる点である ZMP (Zero Moment Point)の目標位置（目標 ZMP)の軌道 を決定し、その目標 ZMPを満足するように（目標 ZMPまわりのモーメントの水平成分 力 SOになるように）ロボットの動力学モデルを用いて目標歩容を生成するようにすれば よい。この際、目標 ZMP軌道を決定するにあたっては、新たな目標歩容が、仮想的 な周期的歩容に収束して 、くように目標 ZMP軌道を決定することが望ま 、。また、 脚式移動ロボットは、 2脚以上の脚を持つロボットであってもよ 、。
[0014] かかる第 1発明では、前記所定時点は、例えば前記第 1作用力軌道決定手段が仮 決定した第 1ロボット '対象物間目標作用力軌道の作用力の大きさ、または該作用力 の変化速度の大きさが所定の閾値を超える時点であり、前記第 2ロボット '対象物間 目標作用力軌道は、前記所定時点以後のロボットと対象物との間の作用力の大きさ 、または該作用力の変化速度の大きさが前記所定の閾値よりも小さい値となる軌道で あることが好ま 、 (第 2発明）。
[0015] すなわち、第 1ロボット '対象物間目標作用力軌道が比較的急激に変化する時点（ 例えばロボットにより対象物を動力 始める時点など、対象物の移動速度を比較的に 急激に変化させる時点)で、実際のロボット '対象物間の作用力が第 1ロボット '対象 物間目標作用力軌道どおりに変化しないような場合に、前記動力学的制約条件を満 たし得る目標歩容 (ロボットの安定性を確保できる歩容)を生成することが困難となり やすい。従って、第 2発明の如ぐ前記所定時点を定めて、第 2ロボット '対象物間目 標作用力軌道を決定することにより、前記第 1発明で説明した効果を好適に得ること ができる。
[0016] また、前記第 1発明あるいは第 2発明では、前記所定の動力学的制約条件は、少な くともロボットの目標歩容の運動によって発生する慣性力と該ロボットに作用する重力 と該ロボットに対象物力 作用する作用力との合力によって定まる ZMPが所定の許 容範囲内に存在すると 、う条件を含むことが好ま Uヽ (第 3発明）。
[0017] この第 3発明によれば、 ZMP (前記合力が発生するモーメントの水平成分力^にな るような点）の存在範囲を制約するので、ロボット '対象物間の実際の作用力が前記 所定時点以後に前記第 1ロボット '対象物間目標作用力と異なる第 2ロボット '対象物 間目標作用力軌道になった場合にロボットの安定性を確保できるような目標歩容 (Z MPが所定の許容範囲内に収まるような歩容）を生成できる力否かを適切に判断でき る。
発明を実施するための最良の形態
[0018] 以下、添付図面を参照して本発明の実施形態に係る脚式移動ロボットの歩容生成 装置を説明する。尚、脚式移動ロボットとしては 2足移動ロボットを例にとる。
[0019] 図 1は、この実施形態に係る脚式移動ロボットとしての 2足移動ロボットを全体的に 示す概略図である。
[0020] 図示の如ぐ 2足移動ロボット（以下、ロボットという） 1は上体 (ロボット 1の基体） 3から 下方に延設された左右一対の脚体 (脚部リンク） 2, 2を備える。両脚体 2, 2は同一構 造であり、それぞれ 6個の関節を備える。その 6個の関節は上体 3側力 順に、股 (腰 部）の回旋（回転)用（上体 3に対するョー方向の回転用）の関節 10R, 10L (符号 R, Lはそれぞれ右側脚体、左側脚体に対応するものであることを意味する符号である。 以下同じ)と、股 (腰部）のロール方向 (X軸まわり）の回転用の関節 12R, 12Lと、股（ 腰部）のピッチ方向（Y軸まわり）の回転用の関節 14R, 14Lと、膝部のピッチ方向の 回転用の関節 16R, 16Lと、足首のピッチ方向の回転用の関節 18R, 18Lと、足首 のロール方向の回転用の関節 20R, 20Lと力も構成される。
[0021] 各脚体 2の足首の 2つの関節 18R (L) , 20R(L)の下部には、各脚体 2の先端部を 構成する足平 (足部） 22R (L)が取着されると共に、両脚体 2, 2の最上位には、各脚 体 2の股の 3つの関節 10R(L) , 12R (L) , 14R(L)を介して前記上体 3が取り付けら れている。上体 3の内部には、詳細を後述する制御ユニット 60などが格納される。な お、図 1では図示の便宜上、制御ユニット 60を上体 3の外部に記載している。
[0022] 上記構成の各脚体 2においては、股関節（あるいは腰関節）は関節 10R (L) , 12R
(L) , 14R(L)から構成され、膝関節は関節 16R(L)から構成され、足首関節は関節 18R(L) , 20R(L)から構成される。また股関節と膝関節とは大腿リンク 24R(L)で連 結され、膝関節と足首関節とは下腿リンク 26R (L)で連結される。
[0023] 上体 3の上部の両側部には左右一対の腕体 (腕リンク） 5, 5が取り付けられると共に 、上体 3の上端部には頭部 4が配置される。両腕体 5, 5は同一構造であり、それぞれ 7個の関節を備える。すなわち、各腕体 5は、 3つの関節 30R(L) , 32R (L) , 34R(L )から構成された肩関節と、 1つの関節 36R (L)から構成された肘関節と、 3つの関節 38R(L) , 40R(L) , 42R (L)から構成された手首関節と、この手首関節に連結され た手先部 (ハンド) 44R (L)とを備えている。なお、頭部 4は、本発明の要旨と直接的 な関連を有しな 、ため詳細な説明を省略する。
[0024] 上記の構成により、各脚体 2の足平 22R(L)は、上体 3に対して 6つの自由度を与 えられている。そして、ロボット 1の歩行等の移動中に、両脚体 2, 2を合わせて 6 * 2 = 12個（この明細書で「 *」はスカラに対する演算としては乗算を、ベクトルに対する 演算としては外積を示す)の関節を適宜な角度で駆動することで、両足平 22R, 22L の所望の運動を行うことができる。これにより、ロボット 1は任意に 3次元空間を移動す ることができる。また、各腕体 5は、上体 3に対して 7つの自由度を与えられ、両腕体 5 , 5を合わせて 7 * 2= 14個の関節を適宜な角度で駆動することで、後述する台車を 押すなどの所望の作業を行なうことができる。
[0025] 図 1に示す如ぐ各脚体 2の足首関節 18R (L) , 20R(L)の下方には足平 22R (L) との間に公知の 6軸力センサ 50が介装されている。該 6軸力センサ 50は、各脚体 2 の足平 22R(L)の着地の有無、および各脚体 2に作用する床反力（接地荷重)等を 検出するためのものであり、該床反力の並進力の 3方向成分 Fx, Fy, Fz並びにモー メントの 3方向成分 Mx, My, Mzの検出信号を制御ユニット 60に出力する。この 6軸 力センサ 50と同様の 6軸力センサ 52が、各腕体 5の手先部（ノ、ンド) 44R(L)と手首 関節 38R(L) , 40R(L) , 42R (L)との間に介装され、手先部 44R(L)に作用する外 力の並進力の 3方向成分並びにモーメントの 3方向成分の検出信号が該 6軸力セン サ 52から制御ユニット 60に出力される。また、上体 3には、 Z軸 (鉛直方向（重力方向 ) )に対する上体 3の傾斜角およびその角速度、並びに Z軸回りの上体 3の回転角（ョ 一角）およびその角速度を検出するための姿勢センサ 54が備えられ、その検出信号 が該姿勢センサ 54から制御ユニット 60に出力される。この姿勢センサ 54は、図示を 省略する 3軸方向の加速度センサおよび 3軸方向のジャイロセンサを備え、これらの センサの検出信号が上体 3の姿勢角（傾斜角およびョ一角）およびその角速度を検 出するために用いられると共に、ロボット 1の自己位置姿勢を推定するために用いら れる。また、詳細構造の図示は省略するが、ロボット 1の各関節には、それを駆動する ための電動モータ 64 (図 2参照）と、その電動モータ 64の回転量 (各関節の回転角） を検出するためのエンコーダ（ロータリエンコーダ) 65 (図 2参照）とが設けられ、該ェ ンコーダ 65の検出信号が該エンコーダ 65から制御ユニット 60に出力される。
[0026] なお、図示は省略するが、各足平 22R(L)と 6軸力センサ 50との間には、ばね等の 弾性体が介装され、また、足平 22R(L)の底面には、ゴム等の弾性体が貼り付けられ ている。これらの弾性体は、コンプライアンス機構を構成するものであり、各脚体 2が 床反力を受けたときに弾性変形するようになって 、る。
[0027] 図 2は制御ユニット 60の構成を示すブロック図である。該制御ユニット 60はマイクロ コンピュータにより構成されており、 CPU力もなる第 1の演算装置 90及び第 2の演算 装置 92、 AZD変^^ 80、カウンタ 86、 DZA変^^ 96、 RAM84、 ROM94、並 びにこれらの間のデータ授受を行うバスライン 82を備えている。この制御ユニット 60 では、各脚体 2の 6軸力センサ 50、各腕体 5の 6軸力センサ 52、姿勢センサ 54 (加速 度センサおよびレートジャイロセンサ）等の出力信号は AZD変換器 80でデジタル値 に変換された後、バスライン 82を介して RAM84に送られる。またロボット 1の各関節 のエンコーダ 65 (ロータリーエンコーダ）の出力は、カウンタ 86を介して RAM84に入 力される。
[0028] 前記第 1の演算装置 90は後述の如く目標歩容を生成すると共に、関節角変位指 令 (各関節の変位角もしくは各電動モータ 64の回転角の指令値)を算出し、 RAM8 4に送出する。また、第 2の演算装置 92は RAM84から関節角変位指令と、前記ェン コーダ 65の出力信号に基づいて検出された関節角の実測値とを読み出し、各関節 の駆動に必要な操作量を算出して DZA変換器 96とサーボアンプ 64aとを介して各 関節を駆動する電動モータ 64に出力する。
[0029] 図 3は、本明細書の実施形態に係る脚式移動ロボットの制御装置の機能的構成を 全体的に示すブロック図である。この図 3中の破線で囲んだ部分が制御ユニット 60が 実行する処理機能 (主として第 1の演算装置 90及び第 2の演算装置 92の機能）によ つて構成されるものである。なお、以下の説明では、脚体 2および腕体 5の左右を特 に区別する必要がないときは、前記符号 R, Lを省略する。
[0030] 以下説明すると、制御ユニット 60の機能的構成は、歩容生成装置 100、対象物反 力平衡制御装置 102、脚メイン制御装置 104および腕メイン制御装置 106から構成 されている。この明細書の実施形態では、歩容生成装置 100の処理を除き、対象物 反力平衡制御装置 102、脚メイン制御装置 104および腕メイン制御装置 106の処理 は、本願出願人が先に特開平 10— 230485号公報 (前記特許文献 1)にて提案した ものと同じである。従って、以下の実施形態の説明では、歩容生成装置 100を中心 に説明し、対象物反力平衡制御装置 102、脚メイン制御装置 104および腕メイン制 御装置 106の説明は概略的な説明に留める。
[0031] 歩容生成装置 100は、ロボット 1の目標歩容を自在かつリアルタイムに生成して出 力するものである。この歩容生成装置 100が出力する目標歩容は、目標上体位置姿 勢軌道 (上体 3の目標位置軌道及び目標姿勢軌道)、目標足平位置姿勢軌道 (各足 平 22の目標位置軌道及び目標姿勢軌道)、目標手先位置姿勢軌道 (各腕体 5の手 先部 44の目標位置軌道および目標姿勢軌道)、目標全床反力中心点軌道 (全床反 力中心点の目標位置の軌道) )、目標全床反力軌道、および目標対象物反力軌道か ら構成される。なお、脚体 2や腕体 5以外に上体 3に対して可動な部位を備える場合 には、その可動部位の目標位置姿勢軌道が目標歩容に加えられる。以下の説明で は、誤解を生じるおそれがない場合には、しばしば「目標」を省略する。
[0032] ここで、歩容生成装置 100が生成する目標歩容に係る用語の意味および定義につ いて説明しておく。歩容における「軌道」は時間的変化のパターン（時系列パターン） を意味する。
[0033] 足平位置、上体位置など、ロボット 1の各部位の「位置」は、その部位に固定的に設 定されたある代表点の位置を意味する。例えば、本実施形態では、各足平 22の代表 点は、その足平 22を備える脚体 2の足首関節の中心から、該足平 22の底面に延ば した垂線が該底面と交わる点であり、その代表点の位置が足平位置である。また、「 姿勢」は空間的な向きを意味する。具体的には、例えば上体姿勢は Z軸 (鉛直軸）に 対するロール方向（X軸まわり）の上体 3の傾斜角とピッチ方向（Y軸まわり）の上体 3 の傾斜角とョー方向（Z軸まわり）の上体 3の回転角（ョ一角）とで表され、足平姿勢は 各足平 22に固定的に設定された 2軸の空間的な方位角で表される。
[0034] なお、歩容のうちの床反力および対象物反力に係わる構成要素以外の構成要素、 すなわち足平位置姿勢、上体位置姿勢等、ロボット 1の運動に係わる歩容を総称的 に「運動」という。また、運動の軌道は、一定（時間的に変化しない）あるいはほぼ一 定のものであってもよい。例えばロボット 1がその両足平 22, 22を着地させた状態に 維持して、移動しない状態では、足平位置姿勢軌道は一定になる。
[0035] 歩容のうちの対象物反力は、ロボット 1にある対象物を移動させるときに該対象物に ロボット 1から作用させる力の反力（対象物からロボット 1に作用する力）を意味する。 従って、目標対象物反力軌道は、本発明におけるロボット '対象物間目標作用力の 軌道に相当する。本明細書の実施形態では、例えば図 4に示すように、ロボット 1があ る対象物 120 (図示の例では台車)の所定の部位に両腕体 5, 5の手先部 44R, 44L を係合させた状態で歩行動作を行いながら該対象物 120を押す作業を行なう場合を 例にとって説明する。この対象物 120からロボット 1が受ける力が対象物反力である。 対象物反力は、一般的には、並進力成分とモーメント成分とから構成される。但し、 本明細書の実施形態では、対象物反力はロボット 1に対象物 120から作用する並進 力を意味するものとし、その並進力によってロボット 1に作用する、ある点回りのモーメ ントを対象物反力モーメントという。補足すると、対象物反力は、ロボット 1から対象物 120に作用させる力の符号（向き）を反転させたものであるので、ロボット 1から対象物 120への作用力を対象物反力の代わりに用いてもよい。
[0036] なお、本明細書では、対象物 120の運動に関し、対象物 120の位置はロボット 1の 足平位置などと同様に、対象物 120に固定的に設定された代表点の位置を意味す る。また、対象物 120の姿勢はロボット 1の足平 22の姿勢などと同様に、対象物 120 の空間的な向きを意味する。そして、対象物 120の位置および姿勢を総称的に対象 物運動ということがある。
[0037] また、各足平 22に作用する床反力（並進力及びモーメントからなる床反力）を「各足 平床反力」と呼び、ロボット 1の全て（2つ）の足平 22R, 22Lについての「各足平床反 力」の合力を「全床反力」という。ただし、以下の説明においては、各足平床反力はほ とんど言及しないので、特に断らない限り、「床反力」は「全床反力」と同義として扱う。 [0038] 目標床反力は、一般的には、作用点とその点に作用する並進力及びモーメントに よって表現される。作用点はどこにとっても良いので、同一の目標床反力でも無数の 表現が考えられるが、特に目標床反力中心点 (全床反力の中心点の目標位置)を作 用点にして目標床反力を表現すると、目標床反力のモーメント成分は、鉛直成分 (鉛 直軸 (Z軸)まわりのモーメント）を除いて 0になる。換言すれば、目標床反力中心点ま わりの目標床反力のモーメントの水平成分 (水平軸 (X軸及び Y軸）回りのモーメント） は 0になる。そこで、本明細書の実施形態では、目標床反力中心点を目標全床反力 の作用点として、歩容生成装置 100で生成するものとした。なお、目標床反力中心点 は、目標全床反力の作用点としての意味を持つものであるから、本質的には目標全 床反力の 1つの構成要素であると言える。但し、本明細書の実施形態では、目標床 反力のうちの目標床反力中心点がロボット 1の動作制御と密接に関連するものである ことから、図 3では、目標全床反力のうちの作用点である目標床反力中心点を目標全 床反力とは別に記載するものとした。
[0039] また、本明細書では、 ZMP(zero moment point)は、ロボットの運動によって生じる 慣性力と該ロボットに作用する重力と対象物反力との合力（あるいはその合力と釣り 合う床反力）がその点まわりに作用するモーメントが、鉛直成分を除いて 0になる点の 意味で使用する。動力学的平衡条件を満足する歩容では、ロボットの目標運動軌道 と重力と目標対象物反力とから算出される ZMPと目標床反力中心点とは一致する。 本明細書では、多くの場合、目標床反力中心点の代わりに、目標 ZMPという。
[0040] また、本明細書では、目標歩容は、所定期間分の目標運動と目標床反力（目標 Z MPを含む）と目標対象物反力との組を意味するものとする。特に、本実施形態では 、目標歩容（単に歩容と称した場合を含む）は、ことわらない限り、ロボット 1の 1歩の 期間分の目標運動と目標床反力（目標 ZMPを含む)と目標対象物反力との組を意 味するものとする。この場合、目標歩容の「1歩」は、ロボット 1の片方の脚体 2が着地 してから、もう一方の脚体 2が着地するまでの意味で使用する。なお、一連の歩容は 、いくつかの歩容 (所定期間分の歩容）がつながったものとする。また、以降の説明で は、 1歩の期間分の目標歩容を単位歩容ということがある。また、ロボット 1が移動しな い状態（両足平 22, 22の接地を維持する状態)での 1歩の期間は、ある所定の期間（ ロボット 1が移動する場合の 1歩の期間に相当する期間）を意味する。
[0041] また、歩容における両脚支持期とは、ロボット 1がその自重を両脚体 2, 2で支持す る期間（両脚体 2, 2が支持脚となる期間）、片脚支持期とはいずれか一方のみの脚 体 2でロボット 1の自重を支持する期間（一方のみの脚体 2が支持脚となる期間）を言 う。本実施形態での目標歩容 (単位歩容）は、別の言い方をすれば、両脚支持期の 開始時力も次の片脚支持期の終了時までの期間分の目標歩容である。ロボット 1の 自重を支持する脚体 2を支持脚と呼ぶ。両脚支持期では、両脚体 2, 2が支持脚とな り、片脚支持期では 1つの脚体 2が支持脚となる。また、片脚支持期においてロボット 1の自重を支持しない側の脚体 2 (支持脚でない脚体 2)を遊脚と呼ぶ。なお、以降の 説明では、特にことわならい限り、目標歩容における支持脚は、両脚支持期の開始 時に着地する脚体 2 (該両脚支持期に続く片脚支持期で支持脚となる脚体 2)を意味 するものとする。また、支持脚側の脚体 2の足平 22、遊脚側の脚体 2の足平 22をそ れぞれ支持脚足平 22、遊脚足平 22という。また、ロボット 1が移動しない状態（両足 平 22, 22の接地を維持する状態)であっても、便宜上、一方の脚体 2が支持脚、他 方の脚体 2が遊脚であるとする。
[0042] また、歩容生成装置 100により新たに生成しょうとしている目標歩容（単位歩容）、 あるいは生成しつつある目標歩容（単位歩容）を今回歩容と呼び、この今回歩容の 1 歩前の目標歩容を前回歩容、今回歩容の次の 1歩の目標歩容を次回歩容、さらにそ の次の 1歩の目標歩容を次次回歩容などと呼ぶ。
[0043] なお、目標歩容は、グローバル座標系としての支持脚座標系で記述される。本実施 形態では、支持脚座標系は、支持脚足平 22を水平姿勢 (より一般的には床面に平 行な姿勢）にして該支持脚足平 22の底面のほぼ全面を床面に接触 (密着)させた状 態において、該支持脚の足首関節 18, 20の中心力 床面に延ばした垂線が該床面 と交わる点（この点は、本明細書の実施形態の例では支持脚足平 22の底面のほぼ 全面を床面に接触させた状態では、該足平 22の代表点と合致する）を原点とし、そ の原点を通る水平面を XY平面とするグローバル座標系（床に固定された座標系）で ある。この場合、 X軸方向、 Y軸方向は、それぞれ支持脚足平 22の前後方向、左右 方向である。本明細書では、 X軸、 Y軸、 Z軸は、特にことわらない限り、この支持脚座 標系の 3軸を意味するものとする。尚、支持脚座標系の原点は、必ずしも支持脚足平 22の底面のほぼ全面を床面に接触させた状態での該足平 22の代表点に合致する 必要はなぐ該代表点と異なる床面上の点に設定されてもよい。
[0044] 以上説明した事項は、以下に説明する第 1実施形態に限らず、本明細書で説明す る各実施形態にぉ 、て共通の事項である。
[0045]
[第 1実施形態]
以下に本発明の第 1実施形態に係る歩容生成装置 100の詳細を説明する。図 5〜 図 8は歩容生成装置 100の処理を示すフローチャートである。歩容生成装置 100は 、その演算処理機能によって、本発明における第 1作用力軌道決定手段、第 1脚体 運動パラメータ決定手段、第 2作用力軌道決定手段、判断手段を備えている。
[0046] 歩容生成装置 100は、図 5〜図 8のフローチャートに示す処理を所定の演算処理 周期で逐次実行する。
[0047] まず、 SO 1において、対象物 120の移動計画が決定される。ここで決定される移動 計画は、少なくとも現在時刻から将来の所定期間分 (ロボット 1の複数歩分)の対象物 120の移動計画を含んでいる。この移動計画は、基本的には対象物 120の移動要 求 (対象物 120をどのようなタイミングでどのように動かすかという設計的な要求）に応 じて決定され、該対象物 120の位置姿勢軌道 (位置および姿勢の時系列）、あるいは 、その軌道を規定するパラメータもしくは関数式等から構成される。例えば対象物 12 0の移動要求が、ある時刻 tOから X軸方向に一定速度で対象物 120を移動させると いう要求である場合には、移動計画は、図 9のグラフ glで示す如く決定される。グラフ glは、移動計画における対象物位置 (X軸方向位置）の経時変化を示している。この 場合、例えば現在時刻以降の各時刻（ある刻み時間毎の時刻）における対象物 120 の位置の時系列を移動計画として決定してもよ!/、が、時刻 tOとグラフ glの傾き (対象 物 120の移動速度）とを移動計画を規定する要素 (パラメータ）として決定したり、ある いはグラフ glの関数式を移動計画を規定する要素 (パラメータ）として決定してもよい 。なお、移動要求は、歩容生成装置 100に外部から適宜与えられ、あるいは、あらか じめ制御ユニット 60の図示しない記憶手段に記憶保持される。 [0048] 補足すると、 SOIで決定する移動計画は、必ずしも移動要求通りに決定されるわけ ではなぐ必要に応じて適宜修正される力 これについては後述する。
[0049] 次〖こ S03〖こ進み、上記の如く S01で決定された移動計画を基に、対象物動力学モ デルを用いて目標対象物運動軌道 (目標対象物位置姿勢軌道)と、目標対象物反 力軌道とが仮決定される。
[0050] ここで、この S03の処理と、この処理で使用する対象物動力学モデルとを図 10のブ ロック図を参照して説明する。図 10は、 S03で目標対象物運動軌道のうちの目標対 象物位置軌道と、目標対象物反力軌道とを求める演算処理を示すブロック図である 。このブロック図中の破線で囲んだ部分が対象物 120に作用する力と対象物 120の 運動との関係を表す対象物動力学モデルとなっている。なお、本実施形態では、理 解の便宜上、対象物 120をほぼ水平な床上で移動させる場合を例に採って説明す る。
[0051] 図 10に示す対象物動力学モデルは、対象物 120に作用させる力（より詳しくは水 平方向の並進力）を入力として、対象物 120の位置を出力する動力学モデル (順動 力学モデル)である。より詳しくは、この対象物動力学モデルは、対象物 120に作用 させる水平方向の並進力の入力値 (後述の加算部 204で求められる値）に、対象物 1 20の質量 Mの逆数 1ZMを乗算部 206で乗算することにより対象物 120の運動加速 度を求め、これを積分器 208, 210で順次積分する（2重積分する）ことにより対象物 120の位置（対象物動力学モデル上での位置）を出力するものとなっている。つまり、 本実施形態では、図 10に示す対象物動力学モデルは、対象物 120に作用する並進 力が、対象物 120の加速度と質量との積に等しいという運動方程式を基に構成され ている。なお、積分器 208の出力は、対象物動力学モデル上での対象物 120の移動 速度を意味しており、以下、これを対象物モデル速度という。
[0052] 力かる対象物動力学モデルを用いる S03の演算処理を図 10を参照して具体的に 説明する。まず、前記 S01で決定された移動計画に基づく対象物 120の瞬時瞬時の (各時刻の）目標速度である目標対象物速度と、積分器 208で先に求められた対象 物モデル速度とが減算部 200に入力され、それらの偏差（=目標対象物速度一対象 物モデル速度）が求められる。ここで、目標対象物速度は、移動計画における対象物 120の位置軌道の 1階微分値として得られる値、あるいは移動計画の構成要素であ る。なお、前記図 9に示した移動計画の例では、グラフ glの傾きをそのまま目標対象 物速度とすればよぐこの場合には、目標対象物速度は継続的に一定値となる。そし て、現在時刻から将来の所定期間分 (ロボット 1の複数歩分)の目標対象物速度の時 系列が減算部 200に順次入力される。また、減算部 200に入力される対象物モデル 速度は、新たに入力する目標対象物速度の 1つ前の目標対象物速度を減算部 200 に入力したときに積分器 208から出力された値である。
[0053] 次いで、上記偏差に所定のゲイン Kvを乗算部 202で乗算することにより、対象物 1 20にロボット 1から作用させるべき並進力の要求値が求められる。つまり、本実施形 態では、該並進力の要求値は、対象物目標速度と対象物モデル速度との偏差が 0に 収束するように（対象物モデル速度が対象物目標速度に追従するように）、フィード ノ ック制御則により決定される。そのフィードバック制御則としては、この例では比例 制御則が使用される。そして、求められた並進力の要求値の符号を反転させたもの の時系列が目標対象物反力軌道として出力される。
[0054] また、求められた並進力の要求値と、実際の対象物 120にロボット 1以外力も作用 する外乱力 (床力 対象物 120に作用する摩擦力など)の推定値である推定外乱力 とが加算部 204に入力され、該並進力の要求値と推定外乱力との和が、前記対象物 動力学モデルに対する並進力の入力値として加算部 204により求められる。ここで、 推定外乱力は、歩容生成装置 100の演算処理周期毎に後述の S61の処理で求めら れるものである。加算部 120に入力する推定外乱力としては、歩容生成装置 100の 前回の演算処理周期で求められた値 (前回値）が用いられる。そして、このように求め た並進力の入力値を対象物動力学モデルに入力することで、前記した如く対象物 1 20の位置が求められ、この求めた位置の時系列が積分器 210から目標対象物位置 軌道として出力される。
[0055] なお、対象物動力学モデルの積分器 208の出力の初期値 (歩容生成装置 100の 演算処理周期毎の初期値）は、歩容生成装置 100の前回の演算処理周期で S03の 処理を実行して求めた対象物モデル速度の時系列のうちの、前回の演算処理周期 に対応する時刻（前回の演算処理周期での現在時刻）における値に設定される。ま た、積分器 210の出力の初期値は、歩容生成装置 100の前回の演算処理周期で S 03の処理を実行して求めた目標対象物位置の時系列のうちの、前回の演算処理周 期に対応する時刻（前回の演算処理周期での現在時刻）における値に設定される。
[0056] また、目標対象物運動軌道のうちの目標対象物姿勢軌道は、例えば目標対象物 速度の向きにほぼ一致するように決定される。
[0057] 補足すると、移動計画および推定外乱力が一定に維持されている限り、目標対象 物運動軌道や目標対象物反力軌道は一定になるので、 S03の処理は、必ずしも歩 容生成装置 100の演算処理周期毎に行なう必要はない。従って、例えば、 S03の処 理は、ロボット 1の 1歩毎あるいは歩容生成装置 100の演算処理周期の複数周期毎 に行なったり、 S01で移動計画を変更した場合や、推定外乱力が比較的大きく変化 した場合に行なうものとしてもよ!/、。
[0058] 以上説明した S03の処理により、現在時刻から将来の所定期間分の目標対象物運 動軌道と目標対象物反力軌道とが仮決定される。
[0059] なお、この S03の処理は、前記したように目標対象物反力軌道を仮決定するので、 本発明における第 1作用力軌道決定手段としての機能を含んでいる。
[0060] 補足すると、本実施形態では、対象物動力学モデルとして逆動力学モデルを用い たが、順動力学モデルを用いて目標対象物位置軌道と目標対象物反力軌道とを仮 決定するよう〖こしてもよい。この場合には、例えば移動計画に基づく目標対象物速度 を積分することで、目標対象物位置軌道を生成すると共に、該目標対象物速度の微 分値（1階微分値）に対象物 120の質量を乗算したものから推定外乱力を減算する。 そして、その減算結果の符合を反転させたものを目標対象物反力軌道として生成す るようにすればよい。
[0061] 次いで、 S05〖こ進んで、上記の如く仮決定された目標対象物運動軌道を基に、口 ボット 1の遊脚足平 22の着地予定位置姿勢および着地予定時刻が仮決定される。こ こで仮決定する着地予定位置姿勢および着地予定時刻は、現在、作成しょうとして V、る目標歩容である今回歩容での遊脚足平 22の着地予定位置姿勢および着地予 定時刻を含む、ロボット 1の複数歩分 (少なくとも 2歩分)の遊脚足平 22の着地予定位 置姿勢および着地予定時刻である。すなわち、仮決定する着地予定位置姿勢およ び着地予定時刻は、少なくとも今回歩容の遊脚足平 22の着地予定位置姿勢および 着地予定時刻と、次回歩容の遊脚足平 22の着地予定位置姿勢および着地予定時 刻とを含む。この場合、遊脚足平 22の着地予定位置姿勢および着地予定時刻は、 例えば、 S03で決定された目標対象物運動軌道に対して、着地予定時刻での遊脚 足平 22の着地位置姿勢がその時刻での対象物位置姿勢と所定の相対的位置姿勢 関係〖こなるように決定される。
[0062] 但し、遊脚足平 22の着地予定位置姿勢および着地予定時刻は、必ずしも目標対 象物位置姿勢に追従するような形態で決定する必要はない。例えば、ロボット 1の移 動を行なわない状態（両足平 22, 22の接地を維持する状態)で、対象物 120を押し て移動させ、その移動開始後は、対象物 120から腕体 5, 5の手先部 44を対象物 12 0からはずすような場合には、各歩容における遊脚足平 22の着地予定位置姿勢は 経時的に一定に維持されることとなる。つまり、一般的には、遊脚足平 22の着地予定 位置姿勢および着地予定時刻は、目標対象物運動軌道だけでなぐロボット 1のどの ような運動によって、対象物 120を動かすかという設計的な要求に応じて決定される
[0063] なお、遊脚足平 22の着地予定位置姿勢は、より詳しくは、遊脚足平 22の踵を着地 してから、該遊脚足平 22を床に接触させたまま、すべらないようにピッチ方向に回転 させて該遊脚足平 22の底面のほぼ全面を床面に接触させた状態での該遊脚足平 2 2の位置姿勢である。従って、本実施形態では、遊脚足平 22Rまたは 22Lの着地予 定位置姿勢は、その着地時力も次の遊脚足平 22Lまたは 22Rの着地までの単位歩 容における支持脚座標系の位置姿勢を規定するものとなり、遊脚足平 22の着地予 定位置姿勢を決定することで、それに対応して、前記した支持脚座標系の設定手法 に従って各単位歩容における支持脚座標系の位置姿勢が決定されることとなる。具 体的には、各単位歩容における支持脚座標系は、該単位歩容の一つ前の単位歩容 の遊脚足平 22をその着地予定位置姿勢に合致させた状態において、該足平 22を 有する脚体 2の足首関節の中心力 床面に延ばした垂線が該床面と交わる点を原点 とする座標系となる。
[0064] 次!、で、 S07〖こ進んで、今回歩容の目標 ZMP軌道を規定する ZMP軌道パラメ一 タが仮決定される。ここで仮決定される ZMP軌道パラメータは、それにより規定される 今回歩容の目標 ZMP (以下、仮目標 ZMPと ヽぅ）が、 S05で仮決定された着地予定 位置姿勢および着地予定時刻（詳しくは前回歩容および今回歩容における遊脚足 平 22の着地予定位置姿勢および着地予定時刻）により定まる、今回歩容での支持 脚足平 22の接地面（両脚支持期では、両足平 22の接地面を含む、いわゆる支持多 角形)のほぼ中央付近に存在し、且つ、急激な変化をしないように決定される。すな わち、今回歩容の仮目標 ZMPが、ロボット 1の接地面内（あるいは支持多角形内）の 、ロボット 1の安定余裕ができるだけ高くなるような位置に存在し、且つ、急激な変化 をしな ヽように ZMP軌道パラメータが決定される。このような指針で決定される仮目 標 ZMPは、例えば図 11 (a)で示すようなパターンのものとなる。なお、図 11 (a)は仮 目標 ZMPの X軸方向位置のパターンを示している。この例の場合には、仮目標 ZM Pパターン (軌道）の折れ点の位置および時刻が ZMP軌道パラメータとして仮決定さ れる。
次いで、 S09に進んで、現在仮決定されている目標対象物運動軌道と目標対象物 反力軌道とを基に、現在仮決定されている ZMP軌道パラメータにより定まる仮目標 Z MPまわりの対象物反力モーメント軌道 (今回歩容における対象物反力モーメントの 瞬時値の時系列）が算出される。より具体的には、まず、目標対象物運動軌道を基に 、ロボット 120の両腕体 5, 5の手先部 44R, 44Lを係合させるべき対象物 120の部位 の運動軌道 (位置姿勢軌道)が決定される。該部位の位置姿勢は、対象物 120の位 置姿勢に対して所定の位置姿勢関係を有するものとされる。そして、その部位の運動 軌道 (位置姿勢軌道）に合致するように、ロボット 1の手先位置姿勢軌道 (これはロボ ット 1に対する対象物反力の作用点の軌道を規定する）が求められる。次いで、この 求めた手先位置姿勢軌道と目標対象物反力軌道と仮目標 ZMP軌道とから各時刻 ( ある刻み時間毎の時刻）の仮目標 ZMPまわりの対象物反力モーメントが算出される 。そして、その算出された対象物反力モーメントの時系列が対象物反力モーメント軌 道として得られる。なお、対象物 120から離れた箇所からロボット 1を対象物 120の近 くに移動させ、その後、手先部 44R, 44Lを対象物 120に係合させて該対象物 120 を押す作業を開始する場合には、対象物 120を手先部 44R, 44Lを係合させる時刻 (この時刻は適宜決定される）までの対象物反力軌道、ひいては、対象物反力モーメ ント軌道は 0とされる。
[0066] 次いで、 S11に進んで、今回歩容に続く周期的歩容としての定常旋回歩容の歩容 ノ メータが決定される。該歩容パラメータは、定常旋回歩容における足平位置姿勢 軌道を規定する足平軌道パラメータ、目標 ZMP軌道を規定する ZMP軌道パラメ一 タ、目標対象物反力モーメント軌道を規定する対象物反力軌道パラメータを含む。
[0067] これらの歩容パラメータを具体的に説明する前に、定常旋回歩容の概要を説明し ておく。なお、以降の説明では、歩容の「初期」、「終端」はそれぞれ歩容の開始時刻 、終了時刻もしくはそれらの時刻における瞬時歩容を意味する。
[0068] 定常旋回歩容は、その歩容を繰り返したときに歩容の境界 (本実施形態では 1歩毎 の歩容の境界）においてロボット 1の運動状態 (足平位置姿勢、上体位置姿勢等の状 態）に不連続が生じないような周期的歩容を意味する。
[0069] 周期的歩容である定常旋回歩容は、本実施形態では、ロボット 1の 2歩分の歩容、 すなわち今回歩容に続く第 1旋回歩容と該第 1旋回歩容に続く第 2旋回歩容とからな る歩容を該定常旋回歩容の 1周期分の歩容として、その 1周期分の歩容を繰り返す 歩容である。尚、ここで「旋回」なる用語を用いたのは、旋回率を零とするときは直進 を意味するので、直進も広義の意味で旋回に含ませることができる力 である。以降 、定常旋回歩容を定常歩容と略す場合もある。
[0070] 定常歩容について補足すると、ロボット 1としての 2足移動ロボットでは、定常歩容の 1周期分は、少なくとも 2歩分の歩容 (連続する 2つの単位歩容)から構成される。な お、 3歩以上の歩容を 1周期分の歩容とする複雑な定常歩容を設定することも可能で ある。但し、定常歩容は、後述の如ぐ今回歩容の終端における発散成分 (詳細は後 述する）を決定するためだけに用いられる。このため、 3歩以上の歩容を 1周期とする 定常歩容を用いることは、歩容生成の処理が煩雑となるにも関わらず、効果は少ない 。そこで、本実施形態での定常歩容の 1周期分の歩容を 2歩分の歩容 (第 1及び第 2 旋回歩容）により構成するようにしている。以下の定常歩容の説明では、説明の便宜 上、 2歩分の歩容から成る定常歩容を 1歩の歩容とみなす。定常歩容は、歩容生成 装置 100で今回歩容の終端における発散成分を決定するために暫定的に (歩容生 成装置 100の演算処理上で)想定される仮想歩容であり、歩容生成装置 100からそ のまま出力されるものではない。
[0071] 尚、「発散」とは、 2足移動ロボット 1の上体 3の水平位置が両足平 22, 22の位置か らかけ離れた位置にずれてしまうことを意味する。発散成分の値とは、 2足移動ロボッ ト 1の上体 3の水平位置が両足平 22, 22の位置（より具体的には、支持脚足平 22の 接地面に設定された支持脚座標系の原点)からかけ離れていく具合を表す数値であ る。
[0072] 本実施形態では、目標歩容が、前記発散を生じることなぐ継続的に生成されるよう に、発散成分を指標にして歩容を生成する。すなわち、今回歩容の後に続く定常歩 容の初期発散成分を決定してから、今回歩容の終端発散成分を定常歩容の初期発 散成分に一致させる (より一般的には、今回歩容の上体位置姿勢を定常歩容の上体 位置姿勢に収束させる)ように、今回歩容を生成する (今回歩容を規定する歩容パラ メータを決定する)。そして、定常歩容の初期発散成分は、 S 11で決定した歩容パラ メータを基に、該定常歩容の初期（第 1旋回歩容の初期）と終端 (第 2旋回歩容の終 端)とでロボット 1の運動状態が一致するという定常歩容の条件 (以下、これを定常歩 容の境界条件という）をロボット 1の動力学モデル上で満たすように決定される。この ような歩容生成の基本的な指針は本出願人が先に提案した PCT国際公開公報 WO /02/40224A1のものと同様である。従って、以下の定常歩容に関する説明では、 PC T国際公開公報 WO/02/40224A1の記載事項と異なる技術事項を主体に説明し、同 様の技術事項については詳細な説明を省略することがある。
[0073] S11の説明に戻って、定常歩容の歩容パラメータのうちの足平軌道パラメータは、 第 1旋回歩容および第 2旋回歩容の初期及び終端のそれぞれにおける支持脚足平 22及び遊脚足平 22のそれぞれの位置姿勢、各旋回歩容の歩容周期等から構成さ れ、今回歩容、第 1旋回歩容、第 2旋回歩容の順に、足平位置姿勢軌道がつながる ように決定される。以下に具体的な設定方法を図 12を参照して説明する。
[0074] 第 1旋回歩容初期遊脚足平位置姿勢は、次回歩容の支持脚座標系から見た今回 歩容終端支持脚足平位置姿勢とする。この今回歩容終端支持脚足平位置姿勢は、 今回歩容初期の支持脚足平 22を床に接触させたまま、すべらないように該支持脚足 平 22をその底面のほぼ全面が床に接触するまでピッチ方向に回転させたときの該支 持脚足平 22の位置姿勢 (これは前回歩容の遊脚足平 22の着地予定位置姿勢に一 致する）である。なお、次回歩容支持脚座標系の位置姿勢は、図 12に示す如ぐ今 回歩容の遊脚足平 22の着地予定位置姿勢に対応して決定される。
[0075] 第 1旋回歩容初期支持脚足平位置姿勢は、次回歩容支持脚座標系から見た今回 歩容終端遊脚足平位置姿勢とする。この今回歩容終端遊脚足平位置姿勢は、今回 歩容の遊脚足平 22の着地予定位置姿勢 (あるいはこれに対応して定まる次回歩容 支持脚座標系）に対応した位置姿勢に決定される。具体的には、今回歩容の遊脚足 平 22を今回歩容終端遊脚足平位置姿勢力も床に接触させたまま、すべらないように ピッチ方向に所定角度、回転させたときの位置姿勢が今回歩容の遊脚足平 22の着 地予定位置姿勢になるように今回歩容終端遊脚足平位置姿勢が決定される。
[0076] 第 1旋回歩容終端遊脚足平位置姿勢は、次回歩容支持脚座標系から見た次回歩 容終端遊脚足平位置姿勢とする。この次回歩容終端遊脚足平位置姿勢は、今回歩 容終端遊脚足平位置姿勢の決定手法と同様に、次回歩容の遊脚足平 22の着地予 定位置姿勢 (あるいはこれに対応して定まる次次回歩容支持脚座標系）に対応した 位置姿勢に決定される。
[0077] 第 1旋回歩容終端支持脚足平位置姿勢は、次回歩容支持脚座標系に位置姿勢を 合致させた支持脚足平 22を床に接触させたまま、すべらないように該支持脚足平 22 をその底面のほぼ全面が床に接触するまでピッチ方向に回転させたときの該支持脚 足平 22の位置姿勢 (この位置姿勢は、今回歩容の遊脚足平 22の着地予定位置姿 勢に一致する）とする。
[0078] 第 2旋回歩容初期遊脚足平位置姿勢は、次次回歩容支持脚座標系から見た第 1 旋回歩容終端支持脚足平位置姿勢とする。第 2旋回歩容初期支持脚足平位置姿勢 は、次次回歩容支持脚座標系から見た第 1旋回歩容終端遊脚足平位置姿勢とする
[0079] 第 2旋回歩容終端遊脚足平位置姿勢は、それを次次回歩容支持脚座標系から見 た位置姿勢が、今回歩容支持脚座標系から見た今回歩容終端遊脚足平位置姿勢 に一致するように決定される。第 2旋回歩容終端支持脚足平位置姿勢は、それを次 次回歩容支持脚座標系から見た位置姿勢が、今回歩容支持脚座標系から見た今回 歩容終端支持脚足平位置姿勢に一致するように決定される。
[0080] 第 1旋回歩容および第 2旋回歩容の歩容周期は、次回歩容周期（これは今回歩容 の遊脚足平 22の着地予定時刻と次回歩容の遊脚足平 22の着地予定時刻との差で ある）と同一に設定される。これらの第 1旋回歩容及び第 2旋回歩容の歩容周期は、 互いに同一にすることは必ずしも必要ではないが、いずれの周期も、少なくとも次回 歩容周期に応じて決定するのが好ましい。尚、今回歩容、第 1旋回歩容および第 2旋 回歩容の上記以外の運動パラメータ（両脚支持期時間などの時間パラメータを含む） は、上記決定されたパラメータに応じて、歩容の条件 (ァクチユエータの速度が許容 範囲に入っている力、可動角を超えていないか、床などと干渉していないかなど）を 満足するように適宜決定する。
[0081] 定常歩容の歩容パラメータのうちの ZMP軌道パラメータは、前記 S07で目標 ZMP 軌道パラメータを仮決定した場合と同様に、該 ZMP軌道パラメータにより規定される ZMP軌道が、ロボット 1の安定余裕が高くなり、且つ、急激な変化をしないように定常 歩容の足平軌道パラメータ (第 1旋回歩容および第 2旋回歩容の初期及び終端のそ れぞれにおける支持脚足平 22及び遊脚足平 22のそれぞれの位置姿勢、両脚支持 期時間など)を基に決定される。
[0082] また、定常歩容の対象物反力モーメント軌道パラメータは、周期性の条件を満足す るように決定される。例えば、今回歩容、次回歩容および次次回歩容に対応する対 象物反力モーメント軌道が図 13 (a)に示すようになつているとする。ここで、今回歩容 の対象物反力モーメント軌道は S09で求められた軌道である。次回歩容の対象物反 力モーメント軌道は、 S03で求めた次回歩容の目標対象物反力軌道および目標対 象物運動軌道と、定常旋回歩容の ZMP軌道パラメータにより規定される ZMP軌道 ( 定常旋回歩容の第 1番目の第 1旋回歩容における ZMP軌道）とを基に S09と同様に 算出される軌道である。次次回歩容の対象物反力モーメント軌道は、 S03で求めた 次次回歩容の目標対象物反力軌道および目標対象物運動軌道と、定常旋回歩容 の ZMP軌道パラメータにより規定される ZMP軌道 (定常旋回歩容の第 1番目の第 2 旋回歩容における ZMP軌道）とを基に S09と同様に算出される軌道である。 [0083] このとき、定常旋回歩容の対象物反力モーメント軌道は、例えば図 13 (b)で示す軌 道になるように、定常旋回歩容の対象物反力モーメント軌道パラメータが決定される 。この例では、定常旋回歩容の第 1旋回歩容の初期から終端までの対象物反力モー メント軌道は、今回歩容の対象物反力モーメント軌道に連続して、次回歩容の対象 物反力モーメント軌道と一致するように決定されている。また、定常旋回歩容の第 2旋 回歩容の対象物反力モーメント軌道は、第 1旋回歩容の対象物反力モーメント軌道 に連続すると共に、第 2旋回歩容の初期力 終端の直前のある時刻 txまでは、次次 回歩容の対象物反力モーメント軌道と一致し、時刻 txから終端までは、時刻 txの値か ら、第 1旋回歩容の初期（定常旋回歩容の初期）の値と同じ値まで連続的に変化する ように決定されている。時刻 txは、例えば定常旋回歩容の ZMP軌道の折れ点の時 刻に対応する。
[0084] なお、次回歩容の初期の対象物反力モーメントの値と、次次回歩容の終端の対象 物反力モーメントの値との差がさほど大きくないときには、必ずしも、定常旋回歩容の 初期の対象物反力モーメントの値と終端の対象物反力モーメントの値とを同じにしな くてちょい。
[0085] 次 、で、 S 13に進み、定常旋回歩容の初期発散成分が決定される。この場合、定 常旋回歩容は周期的歩容であるから、ロボット 1のあら力じめ定めた動力学モデル上 で、第 1旋回歩容の運動の初期状態 (初期発散成分を含む)と第 2旋回歩容の運動 の終端状態 (終端発散成分を含む）とが一致するように定常旋回歩容の初期発散成 分が決定される。
[0086] この S13の処理を詳細に説明する前に、まず、この S13での処理に用いるロボット 1 の動力学モデルと、この動力学モデルを使用して目標上体位置を決定するための動 力学演算とを説明する。この動力学モデルは、 S13の処理に用いられるほか、後述 の S37までの処理で今回歩容を決定するために用いられ、ロボット 1の動力学的挙動 (ロボット 1に作用する力と運動との関係)を単純ィ匕して (近似的に)表現した動力学モ デルである。以下、この動力学モデルをロボット単純化モデルという。なお、このロボ ット単純ィ匕モデルの基本構造は、 PCT国際公開公報 WO/02/40224A1と同様である 力 対象物反力モーメントが加味されて 、る点で同公報のものと相違して 、る。 [0087] 図 14はロボット単純化モデルの構造を示している。図示の如ぐこのロボット単純化 モデルは、 3質点モデルであり、脚体 2の動力学と上体 3の動力学とが相互に非干渉 に構成されると共に、ロボット 1の全体の動力学は、それらの線形結合で表される。
[0088] このロボット単純化モデルは、倒立振子 Aと 2つの脚質点 2m, 2m (支持脚質点お よび遊脚質点）とから構成される。各脚質点 2mは、各脚体 2に対応する質点である。 この各脚質点 2mは、それぞれに対応する脚体 2の足平 22に任意に固定的に設定さ れたローカル座標系での固定点であり、その位置は、各足平位置姿勢に対応して一 義的に定まるものとされている。例えば、各足平質点 2mは、その足平 22の底面の代 表点から足首関節の中心に向力つて所定の距離だけ該足首関節側にずれた点に設 定される。
[0089] 倒立振子 Aは、水平に移動するフリーの支点 aと、一つの質点 bと、支点 aと質点 bと を結ぶ質量の無い可変長のリンク cとから構成される。この場合、リンク cが傾いてもリ ンク cが伸縮し、支点 aから見た質点 bの高さ hが一定値に保たれるものとする。
[0090] この倒立振子 Aの質点 b (以下、単に倒立振子質点 bと 、う）は、ロボット 3の上体 3の 質点に相当するものであり、以降、倒立振子質点 bを上体質点 bということもある。倒 立振子質点 bの位置を以降、略して倒立振子位置という。なお、上体質点 bの質量は 、上体 3の質量と腕体 5, 5および頭部 4の質量を含んでいる。
[0091] ロボット 1の上体 3の水平位置は、倒立振子質点 bの水平位置から幾何学的に決定 される。具体的には、例えば、上体 3の代表点の水平位置 (支持脚座標系で見た XY 座標）、すなわち上体位置の水平成分が、倒立振子質点 bの水平位置に一致するも のとされる。換言すれば、図 14に示すように、上体 3の代表点と倒立振子質点 bとは、 同一鉛直線上にあるものとされる。
[0092] なお、倒立振子 Aの ZMPは、フリーの支点 aの位置にあるので（支点 aのまわりには モーメントが発生しない）、以降、倒立振子 Aの支点 aの位置を、倒立振子 ZMPと呼 び、 ZMPpendと記述する。
[0093] 力かるロボット単純ィ匕モデルを数式表現するために、該モデルに関する変数および ノ ラメータを以下のように定義する。
[0094] msup :支持脚質点 2mの質量、 mswg:遊脚質点 2mの質量、 mb :倒立振子質点 b の質量、 1111：0 &1:ロボット1の全体質量（=1113 + 1113\\¾ + 11113)、 mfeet :両脚体 2, 2の総質量（=1^ +1!13\^)、 xsup :支持脚質点 2mの位置、 xswg:遊脚質点 2m の位置、 xb :倒立振子位置 (上体質点 bの位置)、 h:倒立振子高さ (倒立振子 Aの 支点 aから倒立振子質点 bまでの高さ） 以降、ことわらない限り、 xb, xsup, xswgは 3次元ベクトル (XYZ座標ベクトル）で表 す。また、 Xを任意の変数としたとき、 dX/dtは Xの 1階微分を表し、 d2X/dt2は Xの 2 階微分を表す。 gは重力加速度定数を示す。 Gは、その X成分、 Y成分、 Z成分がそ れぞれ 0、 0、 一 gである重力加速度ベクトルとする。
[0095] 図 14のロボット単純化モデルにおいて、両脚質点 2m, 2mの総慣性力（両脚質点 2m, 2mの運動による慣性力と重力との合力）がある作用点 Pまわりに作用するモー メントを点 Pまわりの脚総慣性力モーメントと定義し、その作用点 Pの座標 (位置)を xp とする。
[0096] 下記の式 01は、点 Pまわりの脚総慣性力モーメントの動力学的定義式である。
[0097]
点 Pまわりの脚総慣性力モーメント
=msup(xsup— xp; * G— msup、xsup— xp) * d2xsup/ dt2
+ mswg(xswg― xp) * mswg(xswg― xp) * d2xswg/ dt2 式 01 脚 ZMPを ZMPfeetと記述し、式 02で定義する。ただし、脚 ZMPの高さ（ZMPfeetの Z成分）は、前記点 Pの高さと同一とする。この脚 ZMPは、両脚体 2, 2の運動によつ て発生する慣性力と重力との合力に擬似的に対応させた値である。
[0098]
点 Pまわりの脚総慣性力モーメント = mfeet * (ZMPfeet— xp) * G ……式 02 なお、前記作用点 Pは、ロボット単純ィ匕モデルの近似精度が高くなるように設定され る。例えば、今回歩容に係る作用点 Pは、前回歩容の支持脚座標系の原点から、今 回歩容の支持脚座標系の原点に、両脚支持期の間に直線的に等速移動し、該両脚 支持期に続く片脚支持期では今回歩容支持脚座標系の原点に維持されるように設 定される。このことは、定常旋回歩容の第 1旋回歩容、第 2旋回歩容に係る作用点 P についても同様である。
[0099] また、目標 ZMPまわりの対象物反力モーメントをロボット 1の全体質量 mtotalで除算 したものを、対象物反力 ZMPと定義し、これを ZMPobjと記述する。すなわち、 ZMPobj を次式 03で定義する。
[0100]
ZMPobj =目標 ZMPまわりの対象物反力モーメント Zmtotal ……式 03 このとき、目標 ZMPと脚 ZMP (ZMPfeet)と倒立振子 ZMP(ZMPpend)と対象物反力 ZMPとの間には、近似的に次式 04の線形関係が成り立つ。
[0101]
ZMPpend = mtotal/mb *目標 ZMP - mfeet/mb * ZMPfeet
- mtotal/mb * ZMPobj ……式 04 なお、式 04は、サジタルプレーン (XZ平面）上での関係式であり、ラテラルプレーン (YZ平面）上では、式 03の右辺第 3項の符号が「一」から「 +」に反転する。
[0102] また、倒立振子 Aの挙動を表す微分方程式は、次式 05で表される。
[0103]
d2xb/dt2の水平成分=gZh * (xbの水平成分ーZMPpendの水平成分） ……式 05 以上の式 01〜05が図 14のロボット単純化モデルの動力学を記述する式である。
[0104] このようなロボット単純ィ匕モデノレを用いることで、次のような動力学的演算により、目 標足平位置姿勢と目標 ZMPまわりの対象物反力モーメントと目標 ZMPと目標上体 姿勢とから目標上体位置を決定できる。
[0105] その動力学演算を図 15のブロック図を参照して説明しておく。図 15はこの動力学 演算を示すブロック図である。目標両足平位置姿勢 (支持脚足平 22および遊脚足平 22の目標位置姿勢)の軌道と、前記の如く設定される作用点 Pとから、脚 ZMP算出 器 220により、前記式 01および 02を用いて脚 ZMP(ZMPfeet)が算出される。
[0106] また、目標 ZMPまわりの対象物反力モーメントに、乗算部 222で lZmtotalを乗算 することで、対象物反力 ZMP(ZMPobj)が算出される。そして、算出した ZMPfeetに乗 算部 224で mfeetZmtotalを乗算したものと ZMPobjとを減算部 226で目標 ZMPから 減算し、さらにその減算結果に乗算部 228で mtotalZmbを乗算することによって、前 記式 04の右辺の演算が行なわれる。これにより倒立振子 ZMP(ZMPpend)が算出さ れる。なお、上記の演算処理により求められる ZMPpendは、サジタルプレーン上のも のであり、ラテラルプレーン上の ZMPpendを算出する場合には、乗算部 222の演算 結果の符号を反転させたものを減算部 226に入力すればよい。
[0107] このようにして算出された ZMPpendを倒立振子 Aに入力することで、前記式 05に基 づいて、倒立振子水平位置 xbが算出される。さら〖こ、この倒立振子水平位置 xbと目 標上体姿勢とを基に、上体位置決定器 230により目標上体位置が決定される。この 場合、上体位置決定器 230は、倒立振子水平位置 xbを目標上体水平位置とする。 また、目標上体鉛直位置は、例えば本出願人が先に特開平 10— 86080号公報で 提案した上体高さ決定手法によって、目標上体姿勢などを基に決定される。なお、本 実施形態では、ロボット 1の目標上体姿勢は例えば鉛直姿勢とされる。
[0108] 図 5の説明に戻って、以下に、 S13の処理を詳細に説明する。
[0109] S 13の処理では、前記したロボット単純ィ匕モデルを基に、定常歩容の初期発散成 分を決定する。歩容における発散成分を qとおき、前記倒立振子 Aの質点 bの水平速 度 (あるいは上体水平速度) vbとおくと、 qは、本実施形態では次式 06により定義され る。
[0110]
q=xo+vb/ ω θ 式 06 ただし、 ω θは、倒立振子 Αの固有周波数、すなわち、 gZhの平方根である。
[0111] なお、このように定義される発散成分の技術的意味については、 PCT国際公開公 報 WO/02/40224A1に詳細に説明されているので、ここでの説明は省略する。 [0112] このように発散成分 qを定義したとき、倒立振子 Aの運動方程式 (前記式 05)を離散 化して、 qに関して解き、それに前記式 04を適用すると、次式 07が得られる。なお、 目標 ZMPを ZMPtotalと記述する。
[0113]
q[k] = exp( ω Ok Δ t) * q[0]
+ exp( ω Ok Δ t) * (exp(— o O A t)— 1) *∑ (exp(— i ω 0 Δ t) * mtotal/mb * ZMPtot al[i])
- exp( ω Ok Δ t) * (exp(— o O A t)— 1) *∑ (exp(— i ω 0 Δ t) * mfeet/mb * ZMPfeet[ il)
- exp( ω Ok Δ t) * (exp(— o O A t)— 1) *∑ (exp(— i ω 0 Δ t) * mtotal/mb * ZMPobj[ il)
…式 07 ただし、 exp( )は、自然対数の底 eの指数関数を意味する。また、 A tは離散系の刻 み時間、 kはステップ数 (k A tが時刻を意味する）である。また、式 07の∑( )は、 i=0 力も i=k— 1までの累積加算演算を意味している。
[0114] なお、式 07はサジタルプレーン上での発散成分 qを記述する式であり、ラテラルプ レーン上での発散成分 qを記述する式は、式 07の右辺の第 4項の全体の符号を「一」 から「 +」に反転させた式となる。
[0115] ここで、定常旋回歩容の初期 (第 1旋回歩容の初期）における発散成分 (以下、初 期発散成分という）と終端 (第 2旋回歩容の終端）における発散成分 (以下、終端発散 成分と!/、う）に着目し、 q[0] (時刻 0における発散成分)を初期発散成分、 q[k] (時刻 k A tにおける発散成分)を終端発散成分とする。
[0116] このとき、式 07の右辺第 1項は、初期発散成分によって発生する終端発散成分を 表す。右辺第 2項は、目標 ZMPパターンによって発生する終端発散成分を表す。右 辺第 3項は、両脚体 2, 2の運動（両脚質点 2m, 2mの運動）によって発生する終端 発散成分を表す。右辺第 4項は、対象物反力モーメントパターンによって発生する終 端発散成分を表す。以降、右辺第 2項を Wzmptotal 右辺第 3項を Wfeet、右辺第 4項 を Wobjと記述する。すなわち、 WzmptotaU Wfeet、 Wobjを次式 08a, 08b, 08cで定 義する。
[0117]
Wzmptotal
= exp( ω Ok Δ t) * (exp(— o O A t)— 1) *∑ (exp(— i ω 0 Δ t) * mtotal/mb * ZMPtot al[i])
……式 08a
Wfeet
= - exp( ω Ok Δ t) * (exp(— o O A t)— 1) *∑ (exp(— i ω 0 Δ t) * mfeet/mb * ZMPfe et[i])
……式 08b
Wobj
= - exp( ω Ok Δ t) * (exp(— o O A t)— 1) *∑ (exp(— i ω 0 Δ t) * mtotal/mb * ZMPo bj[i])
……式 08c なお、式 08cは、サジタルプレーン上での Wobjを定義する式であり、ラテラルプレー ン上での Wobjは、式 08cの右辺の符号を反転させたものとなる。
[0118] これらの Wzmptotal、 Wfeet, Wobjを用いて前記 07を書き換えると、次式 09となる。
[0119]
q[k] = exp( ω Ok Δ t) * q[0] + Wzmptotal + Wfeet + Wobj ……式 09 本実施形態では、定常旋回歩容の境界条件を満足する初期発散成分は、今回歩 容に続く定常旋回歩容の支持脚座標系（図 12の次回歩容支持脚座標系 (Χ'Υ'座標 系)）で見た初期発散成分が、該定常旋回歩容の次の歩容の支持脚座標系（図 12 の次次次回歩容支持脚座標系 (Χ"'Υ" '座標系））から見た定常旋回歩容 (今回歩容 に続く定常旋回歩容）の終端発散成分に一致するように、上記式 08a, 08b, 08c, 0 9に基づ ヽて解析的に求められる。 [0120] ここで、定常旋回歩容の次の歩容の支持脚座標系（図 12の次次次回歩容支持脚 座標系 (X"'Y" '座標系））から見た該定常旋回歩容の終端発散成分を q'[k]とおくと、 定常旋回歩容の境界条件を満足するためには、次式 10が成立しなければならない
[0121]
q[0] = q'[k] …式 10 また、定常旋回歩容の次の歩容の支持脚座標系（図 12の次次次回歩容支持脚座 標系 (X"'Y" '座標系)）で記述される値を、該定常旋回歩容の支持脚座標系（図 12 の次回歩容支持脚座標系 (Χ'Υ'座標系））で記述される値に座標変換するための回 転行列を Μ、座標原点の平行移動ベクトルを Βとおくと、次式 11が得られる。
[0122]
q[k] = Mq'[k] + B ……式 11 従って、これらの式 10、 11から次式 12が得られる。
[0123]
q[k] = Mq[0] + B ……式 12 この式 12は、定常歩容の境界条件を満足するために、初期発散成分 q[0]と終端発 散成分 q[k]とが満たすべき条件である。
[0124] そして、この式 12と前記式 08a〜08cおよび 09と力ら、次式 13が得られる。
[0125]
q[0] = inv(M - exp( ω Ok Δ t)l)(Wzmptotal + Wfeet + Wobj— B) ……式 13 なお、式 13において、 inv( )は、括弧内の行列の逆行列であり、 Iは単位行列であ る。
[0126] 本実施形態では、この式 13に基づいて、定常旋回歩容の境界条件を満足する初 期発散成分 q[0]を決定する。 [0127] 具体的には、前記 SI 1で決定した定常旋回歩容の ZMP軌道パラメータを基に、定 常旋回歩容の各時刻 i A t (i=0, 1,…… , k— 1)における目標 ZMPの瞬時値である ZMPtotal[i] (i=0, 1,…… , k— 1)を求め、これを用いて前記式 08aの右辺の演算を 行なうことで、 Wzmptotalを算出する。なお、 Wzmptotalの算出は、 PCT国際公開公 報 WO/02/40224A1に説明されている如ぐ目標 ZMP軌道が折れ線軌道であること を利用したアルゴリズムによって算出するようにしてもよ!、。
[0128] また、前記 S 11で決定した定常旋回歩容の足平軌道パラメータを基に、定常旋回 歩容の各時刻 i A t (i=0, 1,…… , k 1)における各足平位置姿勢の瞬時値を求め 、それを、前記式 01, 02を離散系で表現した式に適用することで、 ZMPfeetD]を求め る。なお、足平位置姿勢は、例えば本出願人が特許第 3233450号にて提案した有 限時間整定フィルタを用いて各足平 22毎に算出される。この場合、算出される足平 位置姿勢の軌道は、定常旋回歩容の第 1旋回歩容および第 2旋回歩容のそれぞれ における遊脚足平 22が片脚支持期の開始時刻から上昇して、該遊脚足平 22の着 地予定位置に向力つて移動し、該遊脚足平 22の着地予定時刻にて該遊脚足平 22 がその踵で着地予定位置に対応する位置に着地するような軌道である。そして、上 記の如く求めた ZMPfeetlD]を用いて前記式 08bの右辺の演算を行なうことで、 Wfeet を算出する。補足すると、遊脚足平 22の着地予定位置姿勢が、 1つ前の歩容におけ る支持脚足平 22の着地位置姿勢と同じである場合には、足平位置姿勢軌道は、一 定の着地位置姿勢に維持される軌道とされる。但し、遊脚足平 22を一旦上昇させ、 その後、元の着地位置姿勢に戻すような足平位置姿勢軌道を生成するようにしてもよ い。
[0129] さらに、前記 S 11で決定した定常旋回歩容の対象物反力モーメント軌道パラメータ を基に、定常旋回歩容の各時刻 i A t (i=0, 1,…… , k 1)における、目標 ZMPま わりの対象物反力モーメントを求めると共に、それを mtotalで除算することにより、 ZM Pobj[i]を求め、その求めた ZMPobjD]を用いて前記式 08cの右辺の演算を行なうこと で、 Wobjを算出する。
[0130] なお、 Wzmptotal、 Wfeet, Wobjの算出で用いる刻み時間 Δ tは、歩容生成装置 10 0の演算処理周期と同一であることが好ましいが、それよりも長い時間に設定して、演 算処理時間を短縮するようにしてもよ!ヽ。
[0131] そして、上記の如く求めた Wzmptotal、 Wfeet、 Wobjから、前記式 12の右辺の演算 を行なうことで、定常旋回歩容の境界条件を満足する初期発散成分 q[0]が算出され る。
[0132] 以上が、本実施形態における S13の処理の詳細である。
[0133] なお、本実施形態では、初期発散成分 q[0]を解析的に求めるようにしたが、例えば PCT国際公開公報 WO/02/40224A1の第 2実施形態で説明されている如ぐ探索的 な手法により、今回歩容に続く定常旋回歩容の次の歩容の支持脚座標系力 見た 該定常旋回歩容の終端発散成分にほぼ一致するような該定常旋回歩容の初期発散 成分を決定するようにしてもょ 、。
[0134] 図 5のフローチャートの説明に戻って、上記の如く S13の処理を実行した後、 S15 に進んで、今回歩容の歩容パラメータが仮決定される。該歩容パラメータは、今回歩 容における足平位置姿勢軌道を規定する足平軌道パラメータ、目標 ZMP軌道を規 定する ZMP軌道パラメータ、目標対象物反力モーメント軌道を規定する対象物反力 軌道パラメータを含む。なお、ここで仮決定する足平軌道パラメータは、本発明にお ける第 1脚体運動パラメータに相当する。従って、 S13の処理は、本発明における第 1脚体運動パラメータ決定手段としての機能を持つ。
[0135] 今回歩容の歩容パラメータのうちの足平軌道パラメータは、今回歩容の初期及び 終端のそれぞれにおける支持脚足平 22及び遊脚足平 22のそれぞれの位置姿勢、 今回歩容の歩容周期等から構成される。この場合、今回歩容初期遊脚足平位置姿 勢は、今回歩容支持脚座標系から見た、前回歩容終端における今回歩容の遊脚足 平位置姿勢 (前回歩容の支持脚足平位置姿勢)とする。今回歩容初期支持脚足平 位置姿勢は、今回歩容支持脚座標系から見た、前回歩容終端における今回歩容の 支持脚足平位置姿勢 (前回歩容の遊脚足平位置姿勢)とする。今回歩容終端遊脚 足平位置姿勢は、今回歩容の遊脚足平 22の着地予定位置姿勢に対応させて決定 される。具体的には、今回歩容の遊脚足平 22を今回歩容終端遊脚足平位置姿勢か ら床に接触させたまま、すべらないようにピッチ方向に所定角度、回転させたときの位 置姿勢が今回歩容の着地予定位置姿勢になるように今回歩容終端遊脚足平位置姿 勢が決定される。今回歩容終端支持脚足平位置姿勢は、今回歩容支持脚座標系に 位置姿勢を合致させた支持脚足平 22を床に接触させたまま、すべらないように該支 持脚足平 22をその底面のほぼ全面が床に接触するまでピッチ方向に回転させたとき の該支持脚足平 22の位置姿勢 (この位置姿勢は、前回歩容の遊脚足平 22の着地 予定位置姿勢に一致する）とする。
[0136] なお、本実施形態では、今回歩容終端における支持脚足平 22は、床面にほぼ平 行な姿勢になるが、必ずしもこのようにする必要はない。例えば歩容終端において、 支持脚足平 22の踵側が床面から離れるようにしてもょ ヽ。
[0137] 今回歩容の歩容パラメータのうちの ZMP軌道パラメータは、 S07で仮決定された Z
MP軌道パラメータと同一に設定される。従って、この ZMP軌道パラメータにより規定 される目標 ZMP軌道は、例えば前記図 11 (a)に示したようなパターンのものである。
[0138] また、今回歩容の歩容パラメータのうちの対象物反力モーメント軌道パラメータは、 それにより規定される対象物反力モーメント軌道が、前記 S09で算出した対象物反 力モーメント軌道に一致するように決定される。
[0139] 次いで、 S17に進んで、定常旋回歩容に今回歩容が収束するように、 S15で仮決 定した歩容パラメータのうちの ZMP軌道パラメータを修正する。より詳しくは、定常旋 回歩容の初期発散成分に今回歩容の終端発散成分がほぼ一致するように ZMP軌 道パラメータが修正される。
[0140] 本実施形態では、定常旋回歩容の初期発散成分に今回歩容の終端発散成分をほ ぼ一致させるようにするために、前記仮目標 ZMPに図 11 (b)に示すような ZMP修正 量をカ卩えることにより、該仮目標 ZMPを修正し、それにより、図 11 (c)に示すような目 標 ZMPを得る。
[0141] この場合、仮目標 ZMPの修正処理の基本的指針は、 PCT国際公開公報 WO/02/ 40224A1と同様である力 対象物反力モーメントが考慮される点で同出願のものと相 違している。以下にその相違する点を中心に S17の処理を具体的に説明する。なお 、この処理で使用する変数、記号のうち、定常歩容の初期発散成分を決定する処理 (S13)に関して説明したものと同様の意味を持つ変数については、定常歩容に関し て先に定義したものと同じ変数、記号を用いて S17の処理を説明する。また、以下の 説明では、理解の便宜上、 ZMPの X軸方向成分の修正に関して説明を行なう。
[0142] 本実施形態では、 ZMP修正量は、図 11 (b)に示す如く台形状とされ、その高さを a とおく。なお、台形の折れ点の時刻は、図示の如ぐ仮目標 ZMP軌道の折れ点の時 刻に合わせて設定される。本実施形態では、 ZMP修正量は、今回歩容の片脚支持 期で発生させる。また、前記 S13で決定した初期発散成分を今回歩容支持脚座標系 カゝら見た値に座標変換したものを q"とおく。すなわち、今回歩容支持脚座標系で見 た定常旋回歩容の初期発散成分を q"とおく。また、仮目標 ZMP軌道によって発生 する今回歩容の終端発散成分を Wzmptmp、 a= 1である場合の ZMP修正量パター ンによって発生する今回歩容の終端発散成分を Wtrimとおく。
[0143] このとき、修正後の目標 ZMP軌道によって発生する終端発散成分を Wzmptotalと おくと、近似的に次式 14が得られる。
[0144]
Wzmptotal = Wzmptmp + a * Wtrim 式 14 従って、目標 ZMPを修正した後の今回歩容の終端発散成分 q"は、定常歩容に関 して説明した前記式 09と同様の次式 15により表される。
[0145]
q " = exp( ω Ok Δ t) * q[0] + Wzmptmp + a * Wtrim + Wfeet + Wobj ……式 15 なお、式 15において、 k A tは今回歩容の終端時刻、 q[0]は今回歩容の初期発散 成分、 Wfeetは今回歩容の両脚体 2, 2の運動によって発生する終端発散成分、 Wobj は今回歩容の対象物反力モーメント軌道によって発生する終端発散成分である。
[0146] この式 15から、 ZMP修正量の台形高さ aを決定するための次式 16が得られる。
[0147]
a = (q "— exp( ω 0k Δ t) * q[0]— Wzmptmp - Wfeet - Wob j)/Wtrim ……式 16 そこで、本実施形態では、式 16により、 ZMP修正量の台形高さ aを求める。この場 合、初期発散成分 q[0]は今回歩容初期の倒立振子位置 (あるいは上体位置)とその 変化速度とから、前記式 06により算出される。また、 Wzmptmpは、 S 15で仮決定され た ZMP軌道パラメータにより規定される仮目標 ZMP軌道から、定常旋回歩容に係る Wzmptotalを求めた場合と同様に求められる。 Wtrimは、台形高さ aを 1として定まる Z MP修正量パターンから、定常旋回歩容に係る Wzmptotalを求めた場合と同様に求 められる。 Wfeetは、 S 15で仮決定された足平軌道パラメータから、定常旋回歩容に 係る Wfeetを求めた場合と同様に前記式 08bに基づいて求められる。 Wobjは、 S15 で仮決定された対象物反力モーメント軌道パラメータから、定常旋回歩容に係る Wob jを求めた場合と同様に前記式 08cに基づいて求められる。
[0148] S17では、このようにして求めた台形高さ aにより定まる ZMP修正量を仮目標 ZMP 軌道に加えることで、 ZMP軌道パラメータが修正される。
[0149] なお、本実施形態では、 ZMP修正量を解析的に求めるようにしたが、探索的に求 めるようにしてもよい。この場合には、台形高さ aの値を適当な値に設定した ZMP修 正量により仮目標 ZMPを修正してなる目標 ZMP軌道を用いて、今回歩容を仮に作 成し、その仮作成した今回歩容の終端発散成分と定常歩容の初期発散成分との差 などに応じて、その差力 、さくなるように台形高さ aの値を修正する、という処理を繰り 返す。そして、最終的に仮作成した今回歩容の終端発散成分と定常歩容の初期発 散成分とがほぼ一致するときの台形高さ aの値によって ZMP修正量を決定すればよ い。
[0150] S17の処理の後、 S19に進んで修正後の ZMP軌道パラメータにより定まる目標 Z MPが適正力否かが判断される。具体的には、修正後の目標 ZMP力 今回歩容の 支持脚足平 22の接地面力 逸脱したり、接地面の境界付近に偏ったりしていない場 合、すなわち、ロボット 1の安定余裕が確保できる場合には、目標 ZMPが適正である と判断され、そうでない場合には、目標 ZMPが不適正であると判断される。
[0151] S19の判断結果が NOである場合には、 S21に進んで、今回歩容パラメータのうち の ZMP軌道パラメータ以外のパラメータに係わる要素、例えば対象物反力モーメント 軌道パラメータに係わる目標対象物反力軌道を修正する。この場合、目標対象物反 力軌道は、その修正後に S09〜S17の処理を再度実行したときに、 ZMP修正量が できるだけ小さくなるように (少なくとも目標対象物反力軌道の修正前に求めた ZMP 修正量よりも小さくなるように)修正される。
[0152] この目標対象物反力軌道の修正例を以下に説明する。例えば図 16 (a)のグラフ g2 で示すように移動計画に基づく目標対象物運動軌道 (位置軌道)が時刻 tlから X軸 方向で対象物 120の減速を行なって、時刻 t3で停止させるような軌道であるとする。 なお、図 16 (a)のグラフ g3は、目標対象物運動軌道に対応して決定される目標上体 位置軌道の例である。このとき、時刻 tl力もの対象物 120の減速加速度が比較的大 きい場合には、その時刻 tl以後の、ある現在時刻 t2における今回歩容に係る ZMP 修正量が X軸の正方向に過大になって、支持脚足平 22の接地面を逸脱する場合が ある（この場合、 S19の判断結果が NOになる）。そして、仮に、そのまま目標歩容を 生成し続けた場合には、図示のようにロボット 1の上体 3がある時刻 t4で対象物 120 に衝突するような目標歩容が生成されてしまう。
[0153] そこで、このような場合には、目標対象物反力を、前記したように S03で仮決定され た目標対象物反力軌道に対して、図 16 (b)に示す如ぐ現在時刻 t2以降のある期間 ΔΤ1内で、 X軸の負方向に増加させるように該目標対象物反力軌道を修正する。換 言すれば、ロボット 1から対象物 120に加える作用力（目標対象物反力の符号を反転 させた力）を X軸の正方向に増カロさせるように目標対象物反力を修正する。この場合 、目標対象物反力を増加させる期間 ΔΤ1あるいはその増加分 A F1の大きさは、修 正後の目標対象物反力を基に前記 S09〜S17の処理により決定される ZMP修正量 ができるだけ小さくなつて、 S 19の判断結果が YESになるように決定される。このよう な Δ T1あるいは Δ F1は、解析的あるいは探索的に求めることが可能である。
[0154] このように目標対象物反力軌道を修正することにより、ロボット 1が積極的に対象物 120を押すような上体 3の運動が行なわれるように今回歩容パラメータのうちの対象 物反力モーメント軌道パラメータが S 15で決定されることとなる。その結果、この今回 歩容パラメータを基に生成される目標歩容が、安定余裕を確保しつつ、対象物 120 にロボット 1の上体 3が衝突することが無いようなような歩容になる。
[0155] S21の処理を行なった後、 S09〜S 17の処理が前記したように再度実行される。こ のとき、 S 17の次の S 19の半 IJ断結果は YES〖こなり、この場合には、次〖こ図 6の S23に 進む。 [0156] 補足すると、図示は省略する力 S19の判断結果が YESになったときには、 S17で 修正された ZMP軌道パラメータにより規定される今回歩容の目標 ZMP軌道を用い て、前記 S09の処理と同様に、今回歩容の目標対象物反力モーメント軌道が改めて 算出され、これに応じて今回歩容の対象物反力モーメント軌道パラメータが更新され る。
[0157] S23では、現在設定されている目標対象物反力軌道（S 19の判断結果が YESにな つた時に設定されて 、る目標対象物反力軌道）にお 、て、次回歩容の期間内に目標 対象物反力の変化点がある力否かが判断される。ここで、目標対象物反力の変化点 は、本実施形態では、その変化点の時刻前後で目標対象物が比較的大きく変化す るようなポイントである。そして、その変化点は、目標対象物反力の 1階微分値 (時間 的変化率)の大きさが所定の閾値を超えるようなポイント、あるいは目標対象物反力 の大きさ（絶対値）が所定の閾値を超えるようなポイントとして抽出される。以下、この ように抽出される変化点の時刻を対象物反力変化時刻という。
[0158] そして、 S23の判断結果が NOである場合には後述の S49に進み、 YESである場 合には S25に進む。 S25では、今回歩容およびこれに続く定常旋回歩容の現在設 定されて!/ヽる歩容パラメータを基に、現在時刻から前記対象物反力変化時刻までの ロボット 1の目標歩容 (所定のきざみ時間毎の目標歩容の瞬時値の時系列）が仮生 成される。
[0159] より具体的には、今回歩容の歩容パラメータ（以下、単に今回歩容パラメータという ことがある）のうちの足平軌道パラメータを基に、現在時刻から今回歩容の終了時刻 まで各足平 22の目標足平位置姿勢の瞬時値の時系列を算出し、さらにこれに続い て、定常旋回歩容の歩容パラメータ (以下、単に定常歩容パラメータということがある） のうちの第 1旋回歩容 (今回歩容の次の第 1旋回歩容）に係る足平軌道パラメータを 基に、今回歩容の終了時刻から対象物反力変化時刻までの各足平 22の目標足平 位置姿勢の瞬時値を時系列的に算出する。この目標足平位置姿勢の算出処理は、 前記 S13の処理の中で ZMPfeetD]を算出する処理に関して説明した場合と同様に、 本出願人が特許第 3233450号にて提案した有限時間整定フィルタを用いて各足平 22毎に算出される。 [0160] さらに、今回歩容パラメータの ZMP軌道パラメータおよび対象物反力モーメント軌 道パラメータ（S 17の修正後の軌道パラメータ）を基に、それぞれ今回歩容の終了時 刻までの目標 ZMPの瞬時値と標対象物反力モーメントの瞬時値とを時系列的に算 出し、さらにこれに続いて、定常歩容パラメータのうちの第 1旋回歩容 (今回歩容の次 の第 1旋回歩容）〖こ係る ZMP軌道パラメータおよび対象物反力モーメント軌道パラメ ータを基に、それぞれ今回歩容の終了時刻から対象物反力変化時刻までの目標 Z MPの瞬時値と目標対象物反力モーメントの瞬時値とが時系列的に算出される。
[0161] そして、上記の如ぐ目標足平位置姿勢、目標 ZMP、および目標対象物反力モー メントの瞬時値を算出しながら、これらの瞬時値を用いて、前記図 15のブロック図で 示した動力学的演算によって、対象物反力変化時刻までの目標上体位置姿勢の瞬 時値が目標 ZMPを満足するように算出される。なお、本実施形態では、目標上体姿 勢は、例えば定常的な鉛直姿勢とされる。
[0162] このように S25では、現在時刻から対象物反力変化時刻までの目標歩容のうちの 目標足平位置姿勢、目標 ZMP、および目標対象物反力モーメントの瞬時値が時系 列的に算出される。補足すると、 S25では目標手先位置姿勢の瞬時値の時系列は 算出する必要はない。これはロボット 1の腕体 5の運動は ZMPへの影響が小さいから である。
[0163] 以降、対象物反力変化時刻を含む歩容 (次回歩容)を仮想今回歩容、その次の歩 容を仮想次回歩容、さらにその次の歩容を仮想次次回歩容という。仮想今回歩容は 、対象物反力変化時刻を現在時刻とみなしたときの今回歩容を意味する。
[0164] 次いで S27に進んで、対象物反力変化時刻以後の仮想的な目標対象物反力軌道 である仮想目標対象物反力軌道を決定する。この仮想目標対象物反力軌道は、前 記した S09〜S21のループ処理で最終的に決定された目標対象物反力軌道のうち の対象物反力変化時刻以後の軌道とは異なる仮想的な軌道であり、本実施形態で は、対象物反力変化時刻で目標対象物反力が変化せず、該対象物反力変化時刻 以後、その直前の時刻（例えば対象物反力変化時刻よりも所定のきざみ時間前の時 刻）の値に継続的に維持される軌道に決定される。つまり、仮想目標対象物反カ軌 道は、対象物反力変化時刻での対象物 120の実際の位置が目標対象物運動軌道 における位置力もずれているなどの原因によって、実際の対象物反力が対象物反力 変化時刻にて目標対象物反力通りに変化しな ヽような状況を想定して設定される対 象物反力軌道である。例えば図 17 (a)の実線のグラフで示す如ぐ対象物 120の移 動計画 (現在の移動計画）による対象物 120の移動速度 (対象物速度）が、次回歩容 内の時刻 tlまでは 0で、該時刻 tlから時刻 t2まで増加した後、時刻 t2から一定速度に 維持されるようになっているとする。この場合、目標対象物反力軌道は、概略的には 、例えば図 17 (b)の実線のグラフで示すように、時刻 tlから時刻 t2の間で目標対象 物反力がある値に増加するような軌道に決定されることとなる。そして、このとき、仮想 目標対象物反力軌道は、図 17 (b)に二点鎖線のグラフで示す如ぐ対象物反力変 化時刻である時刻 tlから対象物反力が増加することなぐ対象物反力が時刻 tl以前 と同じ値に継続的に維持されるような軌道に決定される。
[0165] なお、このように決定される仮想目標対象物反力軌道は、本発明における第 2ロボ ット '対象物間目標作用力軌道に相当するものである。従って、 S27の処理は、本発 明における第 2作用力軌道設定手段としての機能を持つ。
[0166] 次いで、 S29に進んで、対象物反力変化時刻以後の遊脚足平 22の着地予定位置 姿勢および着地予定時刻が仮決定される。ここで仮決定される遊脚足平 22の着地 予定位置姿勢および着地予定時刻は、少なくとも仮想今回歩容と仮想次回歩容とに おける遊脚足平 22の着地予定位置姿勢および着地予定時刻が含まれる。この場合 、例えば、仮想今回歩容の遊脚足平 22の着地予定位置姿勢および時刻は、今回歩 容に続く前記定常旋回歩容（S09〜S21のループ処理で最終的に歩容パラメータが 決定された定常旋回歩容)の第 1番目の第 1旋回歩容の遊脚足平 22の着地予定位 置姿勢および時刻（これは本実施形態では S05で決定した次回歩容の遊脚足平 22 の着地予定位置姿勢および時刻である）と同一に仮決定され、仮想次回歩容の遊脚 足平 22の着地予定位置姿勢および時刻は、今回歩容に続く定常旋回歩容 (S09〜 S21のループ処理で最終的に歩容パラメータが決定された定常旋回歩容）の第 1番 目の第 2旋回歩容の遊脚足平 22の着地予定位置姿勢および時刻と同一に仮決定さ れる。なお、仮想次回歩容の遊脚足平 22の着地予定位置姿勢および時刻は、前記 S05で決定した次次回歩容の遊脚足平 22の着地予定位置姿勢および時刻と同じに 決定してもよぐあるいは、次次回歩容までの目標対象物運動軌道を基に決定しても よい。
[0167] 次いで、 S31に進んで、対象物反力変化時刻以後の仮想今回歩容の目標 ZMP軌 道を規定する ZMP軌道パラメータを仮決定する。この S31の処理では、前記 S25で 仮生成した仮想今回歩容の支持脚足平 22の目標位置姿勢と、仮想今回歩容の遊 脚足平 22の着地予定位置姿勢および時刻とを基に、仮想今回歩容の支持脚足平 2 2の接地面（両脚支持期では支持多角形)の概ね中央付近に目標 ZMPが位置する ように対象物反力変化時刻以後の仮想今回歩容の ZMP軌道パラメータが仮決定さ れる。なお、この場合、仮想今回歩容の遊脚足平 22の着地予定位置姿勢および時 刻としては、 S29の後に最初に S31の処理を実行するときには、 S29で仮決定された ものが用いられ、後述の S47の後に S31の処理を実行するときには、 S47で後述す る如く修正されたものが用いられる。
[0168] 次いで、 S33に進んで、対象物反力変化時刻以後の対象物反力が前記 S27で決 定された仮想目標対象物反力軌道になったと仮定して、該対象物反力変化時刻以 後の目標 ZMP (S31で仮決定した ZMP軌道パラメータにより規定される目標 ZMP) まわりの対象物反力モーメント軌道 (仮想今回歩容の終了時刻までの軌道)を算出す る。この算出処理は、対象物反力変化時刻以後の目標対象物運動軌道 (前記 S03 で仮決定された軌道のうちの対象物反力変化時刻以後の軌道)と前記仮想目標対 象物反力軌道とを基に、前記 S09の処理と同様に行なわれる。
[0169] 次いで、 S35に進んで、前記仮想今回歩容に続く周期的な歩容としての仮想定常 旋回歩容の歩容パラメータ (仮想定常歩容パラメータ）が決定される。すなわち、仮想 定常旋回歩容の足平軌道パラメータ、 ZMP軌道パラメータ、対象物反力モーメント 軌道パラメータが決定される。
[0170] この場合、仮想定常歩容パラメータの足平軌道パラメータ (仮想定常歩容における 第 1旋回歩容および第 2旋回歩容の初期および終端のそれぞれにおける支持脚足 平 22および遊脚足平 22のそれぞれの位置姿勢など）は、前記 S 11の処理と同様に 、仮想今回歩容、仮想定常旋回歩容の第 1旋回歩容、第 2旋回歩容の順に足平位 置姿勢軌道がつながり、且つ、仮想定常旋回歩容の足平位置姿勢軌道が周期性の 条件を満たすように決定される。なお、この場合、 S29の後に S35の処理を最初に実 行するときには、仮想今回歩容および仮想次回歩容の遊脚足平 22の着地予定位置 および着地予定時刻としては、前記 S29で仮決定したものが用いられ、後述の S47 の後に S35の処理を実行するときには、 S47で後述するように修正されたものが用い られる。
[0171] また、仮想定常歩容パラメータの ZMP軌道パラメータは、 S07の処理と同様に、仮 想定常歩容パラメータの足平軌道パラメータを基に、該足平軌道パラメータにより定 まる、仮想定常歩容の支持脚足平 22の接地面 (両脚支持期では支持多角形)の中 央付近に目標 ZMPが位置するように決定される。
[0172] また、仮想定常歩容パラメータの対象物反力モーメント軌道パラメータは、仮想次 回歩容の目標対象物運動軌道および仮想目標対象物反力軌道と、仮想次回歩容（ 仮想定常歩容の第 1番目の第 1旋回歩容)の ZMP軌道パラメータにより規定される Z MP軌道とを基に前記 S09と同様に求められる仮想次回歩容の対象物反力モーメン ト軌道と、仮想次次回歩容の目標対象物運動軌道および仮想目標対象物反力軌道 と、仮想次次回歩容 (仮想定常歩容の第 1番目の第 2旋回歩容)の ZMP軌道パラメ ータにより規定される ZMP軌道とを基に前記 S09と同様に求められる仮想次次回歩 容の対象物反力モーメント軌道とから、前記 S 11の処理と同様に決定される。補足す ると、仮想次回歩容および仮想次次回歩容の仮想目標対象物反力軌道は、仮想今 回歩容の対象物反力変化時刻以後の一定の軌道である。
[0173] 次いで、 S37に進み、仮想定常旋回歩容の初期発散成分が決定される。すなわち 、仮想定常旋回歩容の第 1旋回歩容の初期状態と第 2旋回歩容の終端状態とがー 致するように仮想定常旋回歩容の初期状態が決定される。この処理は、前記 S 13の 処理と同様に行なわれる。
[0174] 次いで、 S39に進み、仮想今回歩容の歩容パラメータ (足平軌道パラメータ、 ZMP 軌道パラメータ、対象物反力モーメント軌道パラメータ）が仮決定される。この処理で は、仮想今回歩容の足平軌道パラメータが前記 S15の処理と同様に決定される。ま た、仮想今回歩容の ZMP軌道パラメータは、前記 S31で仮決定したものと同一に決 定される。なお、ここで、決定される仮想今回歩容の ZMP軌道パラメータは仮値であ る。また、仮想今回歩容の対象物反力モーメント軌道パラメータは、それにより規定さ れる仮想今回歩容の目標対象物反力モーメント軌道が、対象物反力変化時刻まで は、 S25で算出した目標対象物反力モーメント軌道に一致し、且つ、対象物反力変 化時刻から仮想今回歩容の終了時刻まで、 S33で決定した対象物反力モーメント軌 道に一致するように決定される。
[0175] 補足すると、 S39で仮決定される足平軌道パラメータは、本発明における第 2脚体 運動パラメータに相当する。
[0176] 次いで S41に進んで、仮想定常旋回歩容に仮想今回歩容が収束するように (仮想 定常旋回歩容の初期発散成分に仮想今回歩容の終端発散成分がほぼ一致するよう に）、 S39で仮決定した仮想今回歩容の ZMP軌道パラメータを修正する。この処理 は、前記 S 17の処理と同様に行なわれる。
[0177] 次いで、 S43〖こ進んで、 S41で修正した ZMP軌道パラメータにより規定される仮想 今回歩容の目標 ZMPが適正である力否かが判断される。この判断は、前記 S19の 場合と同様に行なわれる。つまり、仮想今回歩容の目標 ZMPが、仮想今回歩容の支 持脚足平 22の接地面内（両脚支持期では支持多角形内）で、その境界に近づきす ぎない位置に存在すべきであるという動力学的な制約条件 (以下、 ZMP制約条件と V、う）を満たす力否かが S43で判断される。
[0178] そして、 S43の判断結果が YESである場合には後述の S49に進み、 NOである場 合には S45に進む。 S45では、仮想今回歩容の目標 ZMPの適正な修正が可能であ る力否かを判断する。より詳しくは、後述の S47の処理で仮想今回歩容における遊脚 足平 22の着地予定位置姿勢および着地予定時刻を修正して、 S31からの処理を再 び実行したときに、 S43の判断結果を YESとすることができるか否力 (仮想今回歩容 の目標 ZMPが、前記 ZMP制約条件を満たし得るカゝ否か)を判断する。この場合、例 えば、仮想今回歩容の目標 ZMPが、仮想今回歩容の支持脚足平 22の接地面から 逸脱し、且つ、その逸脱量 (接地面の境界力もの距離など）が所定量以上大きいとき には、 S45の判断結果を NOとする。あるいは、後述の S47の処理で、仮想今回歩容 における遊脚足平 22の着地予定位置姿勢および着地予定時刻を修正した回数が 所定数を超えた場合に、 S45の判断結果を NOとする。 [0179] 補足すると、 S45の判断処理は、本発明における判断手段の処理に相当する。
[0180] S45の判断結果が YESである場合には、 S47〖こ進んで、対象物反力変化時刻以 後の遊脚足平 22 (仮想今回歩容と仮想次回歩容における遊脚足平 22)の着地予定 位置及び時刻を修正する。この場合、 S41で修正する仮想今回歩容の ZMP軌道パ ラメータにより規定される目標 ZMPが仮想今回歩容の支持脚足平 22の接地面内に より近づくように仮想今回歩容の遊脚足平 22の着地予定位置及び時刻を修正する。 なお、 S47での修正では、仮想今回歩容と仮想次回歩容の遊脚足平 22の着地予定 時刻は修正せずに、着地予定位置のみを修正するようにしてもよい。また、仮想今回 歩容の遊脚足平 22の着地予定位置を併せて修正するようにしてもょ 、。
[0181] S47の修正後には、 S31からの処理が再び実行される。そして、 S43の判断結果が YESになれば、後述の S49に進む。
[0182] 一方、 S45の判断結果が NOである場合、すなわち、 ZMP制約条件を満たすような 仮想今回歩容の目標 ZMPの修正を行なうことができない場合には、仮に、実際の対 象物反力が前記 S01〜S21の処理で最終的に決定された目標対象物反力軌道に 則したものとならずに、前記対象物反力変化時刻で、該目標対象物反力軌道どおり に変化しないような状況が発生すると、該対象物反力変化時刻以後のロボット 1の姿 勢 (全体的な姿勢)を安定に保つことが困難となる。
[0183] そこで、このような場合には、前記 S01に戻って、対象物 120の移動計画を改めて 決定しなおす (移動計画を修正する） oこの場合に再決定する対象物 120の移動計 画は、実際の対象物反力が目標対象物反力軌道どおりには変化しない状況が発生 しても、ロボット 1の姿勢を安定に保つことが可能となるように決定される。この場合、 目標対象物反力の急激な変動を少なくするように移動計画が再決定される。
[0184] 以下に、前記 S47で仮想今回歩容の遊脚足平 22の着地予定位置を修正する場合 の具体例と、 S01で移動計画を決定しなおす場合の具体例とを図 17と図 18とを参 照して説明する。図 18には、図 17 (a)の実線のグラフで表した移動計画 (現在の移 動計画）による目標対象物位置軌道 (X軸方向の位置軌道)を実線のグラフで示して いる。また、今回歩容、次回歩容 (仮想今回歩容)および次次回歩容のそれぞれの 終了時刻における支持脚足平 22および遊脚足平 22をそれぞれの歩容の終了時刻 に対応する箇所で図 18中に図示している。この場合、支持脚足平 22、遊脚足平 22 はそれぞれ白抜きの長方形、斜線付きの長方形で表している。参照符号 22L1, 22 R2, 22L3を付した支持脚足平は、それぞれ今回歩容、次回歩容、次次回歩容の終 了時刻における支持脚足平であり、参照符号 22R1, 22L2, 22R3を付した遊脚足 平は、それぞれ今回歩容、次回歩容、次次回歩容の終了時刻における遊脚足平で ある。これらの各歩容に係る遊脚足平の位置は、図 18中の実線のグラフの目標対象 物位置軌道に応じて決定される位置の例である。より詳しくは、今回歩容に係る遊脚 足平 22R1の位置は、 S05で決定した今回歩容の遊脚足平 22の着地予定位置であ り、次回歩容に係る遊脚足平 22L2の位置は、 S29で決定した仮想今回歩容の遊脚 足平 22の着地予定位置、次次回歩容に係る遊脚足平 22R3の位置は、 S29で決定 した仮想次回歩容の遊脚足平 22の着地予定位置である。また、今回歩容に係る支 持脚足平 22L1内の黒点は、 S01〜S21の処理で最終的に決定された今回歩容の ZMP軌道パラメータにより規定される、今回歩容の終了時刻における目標 ZMPの 位置を例示し、次回歩容に係る支持脚足平 22R2内の黒点は、 S31で最初に仮決 定された ZMP軌道パラメータにより規定される、仮想今回歩容の終了時刻における 目標 ZMPの位置を例示し、次次回歩容に係る支持脚足平 22L3内の黒点は、 S35 で最初に決定された仮想定常旋回歩容の ZMP軌道パラメータにより規定される、仮 想次次回歩容 (仮想定常旋回歩容の第 1番目の第 1旋回歩容)の終了時刻における 目標 ZMPの位置を例示して!/、る。
前記 S41の処理で仮想今回歩容の ZMP軌道パラメータを修正したとき、その修正 後の ZMP軌道パラメータにより規定される目標 ZMP (仮想今回歩容の終了時刻に おける目標 ZMP)は、仮想今回歩容の支持脚足平 22R2の接地面力も逸脱し、図 1 8中の〇で示す点になることがある。これは、仮想今回歩容の対象物反力変化時刻 までは、対象物反力が目標対象物反力どおりに変化するとして、仮想今回歩容の目 標歩容が S25で仮生成される一方、仮想定常旋回歩容の歩容パラメータのうちの対 象物反力モーメント軌道は、対象物反力が対象物反力変化時刻で変化せずに、仮 想目標対象物反力軌道に従うものとなると仮定して決定されるからである。すなわち 、仮想今回歩容の対象物反力変化時刻までは、対象物反力変化時刻で目標対象物 反力が急増することが予定されているので、ロボット 1の上体 3が前方に加速するよう に S25で目標歩容 (特に目標上体位置軌道）が生成される。し力るに、対象物反力 変化時刻以後は、仮想目標対象物反力軌道に従って、対象物反力が対象物反力変 化時刻の直前と同じに継続的に維持されることが予定されているので、仮想今回歩 容を仮想定常旋回歩容に収束させるためには、ロボット 1の上体 3を減速する必要が ある。その結果、 S41の処理で仮想今回歩容の ZMP軌道パラメータを修正したとき、 その修正後の ZMP軌道パラメータにより規定される目標 ZMP (仮想今回歩容の終 了時刻における目標 ZMP)は、仮想今回歩容の支持脚足平 22R2の接地面力 口 ボット 1の前方側 (X軸の正方向）に逸脱することがある。
[0186] このような場合、前記 S47の処理では、仮想今回歩容の遊脚足平 22の着地予定位 置が、図 18の参照符号 22L2で示す位置から、例えば同図中に参照符号 22L2'を 付した破線の長方形で示すように、ロボット 1の前方側に修正される。このようにするこ とで、再び S31〜S41の処理を実行して、仮想今回歩容の ZMP軌道パラメータを修 正したとき、その修正後の ZMP軌道パラメータにより規定される目標 ZMPが前記 Z MP制約条件を満たすことが可能となって、 S43の判断結果が YESとなる場合がある
[0187] つまり、この場合には、実際の対象物反力が将来の対象物反力変化時刻で仮に目 標対象物反力どおりに変化しな力つたとしても、その対象物反力変化時刻を含む歩 容での遊脚足平 22の着地予定位置を修正することで、ロボット 1の姿勢の安定性を 確保できることとなる。
[0188] 一方、前記 S41の処理で仮想今回歩容の ZMP軌道パラメータを修正したとき、そ の修正後の ZMP軌道パラメータにより規定される目標 ZMP (仮想今回歩容の終了 時刻における目標 ZMP)が、図 18中の△で示す点の如ぐ仮想今回歩容の支持脚 足平 22R2の接地面から大きく逸脱する場合もある。そして、このような場合には、仮 想今回歩容、あるいは仮想今回歩容と仮想次回歩容の遊脚足平 22の着地予定位 置を遊脚足平 22の機構的な可動範囲内でどのように修正しても、目標 ZMPが ZMP 制約条件を満たすように ZMP軌道パラメータを決定できな 、場合があり、このとき S4 5の判断結果が NOになる。 [0189] このような場合は、元々の対象物 120の移動計画がロボット 1の安定性を継続的に 確保する上で不適切であるので、 S01〖こおいて、対象物 120の移動計画を再決定（ 修正する）。この場合、図 18にて説明した上記の例では、図 17 (a)の破線のグラフで 示すように、移動計画を修正する。すなわち、修正後の移動計画による対象物 120 の移動速度が図 17 (a)に実線のグラフで示した元の移動計画よりも時刻 tl (対象物 反力変化時刻）から緩やかに増加し、さらに時刻 t2以後に、元の移動計画よりも小さ V、速度で一定に維持されるように移動計画を決定しなおす。
[0190] このようにしたとき、この修正後の移動計画に対応する対象物反力は、図 17 (b)に 破線のグラフで示す如ぐ時刻 tlから時刻 t2の期間で元の移動計画に対応する対象 物反力よりも小さい反力に抑えられる。従って、対象物反力変化時刻 tlにおける目標 対象物反力の急激な変化が緩和される。その結果、再度、 S43までの処理を実行し たときに、 S43の判断結果を YESにすることが可能となる。
[0191] つまり、前述した S01〜S47の処理によって、実際の対象物反力が目標対象物反 力どおりに変化しない場合でも、ロボット 1の安定性を確保できるように、今回歩容パ ラメータを決定でさることとなる。
[0192] 前記 S23の判断結果が NOである場合、あるいは、 S43の判断結果が YESである 場合〖こ〖ま、図 7の S49〖こ進む。
[0193] この S49では、現在の今回歩容パラメータと定常歩容パラメータとを基に、現在時 刻から定常歩容の第 2旋回歩容終端までの (今回歩容の 1歩を含めて 3歩目までの） 目標上体位置姿勢軌道を算出すると共に、目標対象物運動軌道を基に目標手先位 置姿勢軌道を算出する。つまり、 S49では、今回歩容を含めて 3歩の期間における目 標歩容が仮生成される。
[0194] この場合、今回歩容、定常旋回歩容の第 1旋回歩容、第 2旋回歩容の順に、各歩 容の歩容パラメータを基に、前記図 15のブロック図を参照して説明した如ぐ目標上 体位置姿勢軌道を算出する。より具体的には、各歩容の足平軌道パラメータを基に、 目標両足平位置姿勢軌道が算出されると共に、 ZMP軌道パラメータおよび対象物 反力モーメント軌道パラメータを基に、それぞれ目標 ZMP軌道、対象物反力モーメ ント軌道が算出される。なお、目標両足平位置姿勢軌道の算出処理は、前記 S13の 処理の中で、 ZMPfeetD]を算出する処理に関して説明した場合と同様に、本出願人 が特許第 3233450号にて提案した有限時間整定フィルタを用いて各足平 22毎に 算出される。そして、これらの目標両足平位置姿勢軌道、目標 ZMP軌道、および対 象物反力モーメント軌道を図 15のブロック図の演算処理に用いることで、目標上体 位置姿勢が決定される。なお、本実施形態では、目標上体姿勢は定常的に鉛直姿 勢とされる。このように、目標上体位置姿勢を算出することで、ロボット単純ィ匕モデル 上で、ロボット 1の運動が発生する慣性力と、ロボット 1に作用する目標対象物反力と 重力との合力が目標 ZMPまわりの発生するモーメントの水平成分が 0になるという動 力学的平衡条件を満たすようにロボット 1の目標運動のうちの目標上体位置姿勢軌 道が算出される。
[0195] さらに、目標手先位置姿勢軌道は、今回歩容、定常旋回歩容の第 1旋回歩容、第 2 旋回歩容の順に、 S09で求めた場合と同様に、目標対象物運動軌道を基に算出す る。なお、定常旋回歩容の第 1旋回歩容、第 2旋回歩容に対応する目標対象物運動 軌道は、 S03で仮決定された目標対象物運動軌道のうちの、次回歩容、次次回歩容 に対応する目標対象物運動軌道である。
[0196] 次いで、 S51に進んで、ロボット 1と対象物 120との幾何学的制約条件をチェックす る。ここで、この幾何学的制約条件は、対象物 120とロボット 1との干渉 (上体 3が対象 物 120に当たるなど)の有無、並びに、各腕体 3の関節の回転角が機構的な制限内 に収まって 、る力などの条件である。目標上体位置姿勢と目標対象物運動軌道とか らロボット 1の上体 3と対象物 120との干渉の有無を判断できる。また、目標上体位置 姿勢と目標手先位置姿勢とから、各腕体 3の関節の回転角を決定できるので、その 回転角が機構的な制限内に収まって 、る力否かが判る。
[0197] S51の判断処理は、現在時刻から第 2旋回歩容の終端まで、所定の刻み時間間隔 の目標上体姿勢等の瞬時値に対して逐次行なわれる。すなわち、各瞬時値に対して 、 S51の判断結果が OKである場合には、 S53において、現在時刻から第 2旋回歩 容の終端までの全時刻分のチェックが完了したか否かを判断する。そして、この S53 の判断結果が NOである場合には、 S51の判断処理を繰り返す。
[0198] S51の判断結果が NGである場合には、 S55に進んで、 NGとなった該当時刻にお ける幾何学的制約条件を満たすように着地予定位置姿勢と目標対象物運動軌道と のうちの少なくともいずれか一方を修正する。その修正例を図 19を参照して説明する
[0199] 同図を参照して、現在時刻 tlにおける移動計画に基づく X軸方向の目標対象物運 動軌道 (位置軌道）が図中のグラフ g4で示す軌道 (以下、元軌道と ヽぅ）であるとし、 将来のある時刻 t2で目標上体位置と目標対象物位置とが近すぎて、上体 3と対象物 120との干渉が発生し、 S51の判断結果が NGになるとする。このとき、その干渉が発 生する時刻 t2の目標対象物位置を、 X軸の正方向、すなわち、ロボット 1から遠ざける 方向に図中の破線矢印で示す如くずらすと共に、時刻 t2の前後に亘る期間 Δ Τ2で の目標対象物運動軌道 (位置軌道)をグラフ g5で示す修正軌道に変更する。この修 正軌道 g5は、ロボット 1の挙動の急変を避けるために、その期間 Δ Τ2の両端で元軌 道 g4に滑らかにつながるように決定される。また、時刻 t2でのずらし量 ΔΧは、それに よって、ロボット 1の上体 3と対象物 120との干渉が無くなるように（S51の判断結果が OKとなるように）決定される。
[0200] なお、上記の例では、目標対象物運動軌道を修正する場合を例に採って説明した 力 目標対象物運動軌道を修正する代わりに、 S51の判断結果力 SNGとなる該当時 刻を含む歩容での遊脚足平 22の着地予定位置姿勢を修正するようにしてもよヽ。例 えば、図 19の場合のように時刻 t2で干渉が発生する場合には、その時刻 t2を含む歩 容での着地予定位置姿勢を対象物 120から X軸方向で遠ざけるような位置姿勢に修 正すればよい。あるいは、目標対象物運動軌道と着地予定位置姿勢の両者を修正 するようにしてもよい。あるいは、移動計画を修正するようにしてもよい。
[0201] S55の修正処理が終了すると、 S05に戻って、この S05からの処理が前述したよう に実行される。これにより、前記幾何学的制約条件が満たされない場合に、目標対 象物運動軌道あるいは着地予定位置姿勢が修正され、ひいては、今回歩容パラメ一 タのうちの足平軌道パラメータが修正されることとなる。なお、 S55で目標対象物運動 軌道を修正せずに、遊脚足平 22の着地予定位置を修正した場合には、 S05の処理 を省略して S07からの処理を実行すればよい。補足すると、本発明では、 S49〜S55 の処理は必ずしも必要ではなぐ省略してもよい。 [0202] 一方、 S53の判断結果が YESになると、図 8の S57に進む。この S57では、今回歩 容パラメータを基に、目標 ZMPを満足するように、手先位置姿勢瞬時値を除ぐ現在 時刻の今回歩容瞬時値を算出する。
[0203] この算出処理は、次のように行なわれる。すなわち、今回歩容パラメータのうちの足 平軌道パラメータを基に、目標足平位置姿勢の瞬時値が算出される。この算出処理 は、前記 S13の処理の中で、 ZMPfeetD]を算出する処理に関して説明した場合と同 様に、本出願人が特許第 3233450号にて提案した有限時間整定フィルタを用いて 各足平 22毎に算出される。なお、定常歩容に関して説明した場合と同様、今回歩容 の遊脚足平 22の着地予定位置姿勢が、前回歩容の支持脚足平 22の着地位置姿勢 と同じである場合には、今回歩容の各目標足平位置姿勢の瞬時値は、今回歩容初 期の状態に維持される。但し、今回歩容の遊脚足平 22を一旦上昇させて、元の着地 位置姿勢に戻すように今回歩容の遊脚足平 22の足平位置姿勢軌道の瞬時値を生 成するようにしてちょい。
[0204] また、今回歩容パラメータのうちの、 ZMP軌道パラメータを基に、目標 ZMPの瞬時 値が算出されると共に、対象物反力モーメント軌道パラメータを基に、目標対象物反 力モーメントの瞬時値が算出される。
[0205] さらに、上記算出された目標足平位置姿勢の瞬時値と、目標 ZMPの瞬時値と、目 標対象物反力モーメントの瞬時値とから、前記図 15の動力学的演算によって、目標 上体位置姿勢の瞬時値が算出される。なお、本実施形態では、目標上体姿勢は、例 えば鉛直姿勢とされる。但し、目標上体姿勢を鉛直姿勢に維持する必要はなぐ変 ィ匕させるようにしてもよい。目標上体姿勢を変化させる場合には、ロボット単純化モデ ルは、上体 3の姿勢変化に伴うロボット 1の角運動量変化を考慮した動力学モデルに することが望ましぐ例えば本願出願人が PCT国際公開公報 WO/03/057422A1にて 提案した、同公報の図 10に示すような動力学モデルを使用すればよ!、。
[0206] 以上の S57の処理によりロボット 1の目標歩容の運動のうちの、目標足平位置姿勢 および目標上体位置姿勢の瞬時値が決定される。本実施形態のロボット 1では、各 脚体 2は 6自由度を有するので、目標上体位置姿勢と目標足平位置姿勢とが決まれ ば、各脚体 2の各関節の回転角は一義的に定まる。補足すると、本実施形態では、 S 57で求められる目標上体位置姿勢は後述する S65の処理によってさらに補正される
[0207] 次いで、 S59に進んで、最終修正目標手先位置の前回値 (前回の演算処理周期 での値)を基に、前回の演算処理周期（歩容生成装置 100の演算処理周期）におけ る対象物 120の実際の位置 (移動位置)である実対象物位置を推定する。ここで、最 終修正目標手先位置は、後述する腕メイン制御装置 106で求められるものであり、口 ボット 1の実際の手先位置の指令値に相当するものである。そこで、例えば最終修正 目標手先位置の前回値をロボット 1の実際の手先位置の前回値とし、これと所定の相 対位置関係を有する位置を実対象物位置として推定する。なお、対象物 120にジャ イロセンサや加速度センサを搭載しておき、それらの検出値力も実対象物位置を推 定するようにすることも可能である。
[0208] 次いで、 S61に進み、上記の如く推定した実対象物位置と、目標対象物位置との 偏差 (以下、対象物位置偏差という）と、現在の目標対象物運動軌道とを基に、対象 物動力学モデルを用いて目標対象物運動の瞬時値 (今回値)と、推定外乱力の瞬時 値 (今回値)と、目標対象物反力の瞬時値 (今回値)とが算出される。なお、対象物位 置偏差を規定する実対象物位置と目標対象物位置とのうちの目標対象物位置は、 前回の演算処理周期で求めた値 (前回値)が用いられる。
[0209] ここで、この S61の処理で使用する対象物動力学モデルと S61の処理とを図 20の ブロック図を参照して説明する。図 20は、 S61で目標対象物運動のうちの目標対象 物位置と目標対象物反力と推定外乱力とを求める演算処理を示すブロック図であり、 図中の参照符号 238を付した部分が対象物 120に作用する力と対象物 120の運動 との関係を表す対象物動力学モデルとなっている。この対象物動力学モデルは、前 記図 10に示したものと一部の構造が相違するので、以下の説明では、図 20中の対 象物動力学モデルを図 10の対象物動力学モデルと区別するため、対象物動力学モ デノレ 2と! /、う。
[0210] この対象物動力学モデル 2の基本構造は、図 10の対象物動力学モデルと同様で あり、対象物 120に作用する力（より詳しくは水平方向の並進力）を入力として、その 入力値 (後述の乗算部 242で求められる値）に、対象物 120の質量 Mの逆数 1ZM を乗算部 244で乗算することにより対象物 120の運動加速度を求め、これを積分器 2 46, 250で順次積分する（2重積分する)ものとなっている。但し、対象物動力学モデ ル 2では、図 10の対象物動力学モデルと異なり、積分器 250には、積分器 246の出 力（運動加速度の積分値）に加えて、モデル速度操作量が追加的に入力されるよう になっている。このモデル速度操作量は、モデル速度操作量決定部 252によって、 実対象物位置と目標対象物位置との偏差を 0に近づけるようにフィードバック制御則 により決定される速度操作量であり、本実施形態では次式 17により決定される。 モデル速度操作量 = Kel *対象物位置偏差 + J (Ke2 *対象物位置偏差) dt … …式 17 すなわち、モデル速度操作量は、対象物位置偏差力も PI制御則 (比例積分制御則 )により決定される。なお、式 17の Kel、 Ke2は所定のゲインである。図 20のモデル速 度操作量決定部 252は、式 17の右辺の演算を行なう演算処理部である。すなわち、 モデル速度操作量決定部 252は、実対象物位置 (現在の演算処理周期での S33で 求めた今回値)と目標対象物位置 (前回の演算処理周期での S35で求めた前回値） とから、それらの差である対象物位置偏差を減算部 254で求める。そして、求めた対 象物位置偏差に乗算部 256でゲイン Kelを乗算したもの (比例項)と、該対象物位置 偏差に乗算部 258でゲイン Ke2を乗算したものをさらに積分器 260で積分したもの（ 積分項)とを、加算部 262で加算することにより、モデル速度操作量を算出する。なお 、算出されたモデル速度操作量は、対象物動力学モデル 2の積分器 250の入力側 に備えた加算部 248で、積分器 246の出力に加算された後、積分器 250に入力され る。補足すると、図 20のブロック図では、モデル速度操作量を算出した後に、これを 対象物動力学モデル 2に追カ卩的に入力するようになつてヽるが、モデル速度操作量 決定部 252の積分器 260を省略し、前記乗算部 258の出力を追加的に積分器 246 に入力しつつ、このときの積分器 246の出力と、乗算部 256の出力との和を積分器 2 50に入力するようにしてもよい。このようにしても、対象物動力学モデル 2の出力（積 分器 250の出力）は図 20のものと同じになる。 [0212] カゝかる対象物動力学モデル 2では、加算部 248の出力を積分器 250で積分したも のが目標対象物位置の瞬時値として得られる。また、加算部 248の出力が、該対象 物動力学モデル 2上での対象物 120の移動速度である対象物モデル速度となる。
[0213] 対象物動力学モデル 2の入力である、対象物 120への作用力（並進力）の要求値 は、現在の目標対象物運動軌道 (S53の判断結果が YESになったときの目標対象 物運動軌道）に基づく目標対象物速度と、対象物モデル速度とから減算部 240で求 められるそれらの偏差（目標対象物速度一対象物モデル速度）に乗算部 242で所定 のゲイン Kvを乗算することで求められる。すなわち、対象物動力学モデル 2に入力す る並進力の要求値は、図 10のものと同様、対象物目標速度と対象物モデル速度との 偏差が 0に収束するように、フィードバック制御則（この例では比例制御則）により決 定される。但し、この場合の対象物モデル速度は、前記したように加算部 248の出力 であるから、図 10のものと異なり、モデル速度操作量が加味されたものである。また、 目標対象物速度は、現在の目標対象物運動軌道のうちの位置軌道の 1階微分値と して得られる。
[0214] そして、上記の如く求められた並進力の要求値の符号を反転させたものが目標対 象物反力の瞬時値として求められる。また、該並進力の要求値が対象物動力学モデ ル 2の乗算部 244に入力されると共に、前記モデル速度操作量が加算部 248に入力 され、これにより、該対象物動力学モデル 2の積分器 250から目標対象物位置の瞬 時値が出力される。換言すれば、対象物動力学モデル 2上での対象物 120の運動 状態量としての対象物モデル速度を、目標対象物位置と実対象物位置との偏差を 0 に近づけるためのモデル速度操作量で修正しつつ、対象物動力学モデル 2の動力 学演算により目標対象物位置が逐次決定される。
[0215] また、図 20中の参照符号 264を付した部分は、推定外乱力を求める処理を行なう 推定外乱力決定部であり、この推定外乱力決定部 264には、対象物モデル速度と、 前記並進力の要求値とが入力される。対象物モデル速度は、 M' sZ(Tc' s + l)という 形の伝達関数で表される変換部 266に入力され、この変換部 266により、対象物 12 0に作用している並進力の推定値が算出される。この変換部 266の伝達関数中の M は対象物 120の質量、 Tcは所定の時定数である。従って、変換部 266は、対象物モ デル速度の微分値 ( 1階微分値）に対象物 120の質量を乗算したもの (これは対象物 120に作用する全ての力の合力の瞬時値に相当する）に、時定数 Tcを有するフィル タリング処理を施したものを対象物 120に作用している並進力の推定値として求める 。換言すれば、変換部 266により算出される推定値は、対象物動力学モデル 2上で の対象物 120の運動加速度 (対象物モデル速度の微分値)を発生させる並進力に、 時定数 Tcの 1次遅れを伴って追従するものである。さらに、この並進力の推定値から 前記乗算部 242の出力である並進力の要求値を減算部 268で差し引くことにより、推 定外乱力の瞬時値が算出される。ここで求められた推定外乱力の瞬時値は、前記し たように S02の演算処理（図 10を参照）で使用される。
[0216] なお、目標対象物運動のうちの目標対象物姿勢の瞬時値は、例えば目標対象物 速度の向きにほぼ一致するように決定される。
[0217] 以上が、 S61の演算処理である。この S61の演算処理で求められる推定外乱力は 、実際の対象物 120に作用する力のうち、ロボット 1以外力も作用する実際の該外乱 力をリアルタイムで推定したものであるから、この推定外乱力を前記 S02の演算処理 で用いる（図 10の対象物動力学モデルに入力する）ことにより、前記図 10の対象物 動力学モデル上での対象物 120の挙動 (運動状態)を実際の対象物 120の挙動 (運 動状態）に近いものとすることができる。
[0218] 次いで S63に進んで、 S61で求めた目標対象物運動の瞬時値を基に、目標手先 位置姿勢の瞬時値 (今回値)を決定する。 目標手先位置姿勢の瞬時値は、 S09で目 標手先位置姿勢軌道を求めた場合と同様に求められる。
[0219] 以上の S63までの処理によって求められる目標歩容の瞬時値のうち、目標上体位 置姿勢は、前記ロボット単純ィ匕モデルを用いて、該ロボット単純ィ匕モデル上で目標 Z MPを満足するように（ロボット 1の運動による慣性力と重力と対象物反力モーメントと の合力が目標 ZMPまわりに発生するモーメントの水平成分が 0になるように）求めら れている。従って、 S63までの処理によって求められる目標歩容は、目標 ZMPまわり の床反力モーメント水平成分がロボット単純ィ匕モデル上で 0となる歩容である。
[0220] ここで、ロボット単純ィ匕モデルは、歩容生成装置 100の演算負荷を効果的に軽減で きる点での利点は大きいものの、動力学的精度は必ずしも高精度であるとはいえな い。そこで、本実施形態では、さらに、動力学的精度の高いロボット動力学モデル (以 下、これをフルモデルという）を用いて、目標歩容のうちの一部の構成要素（具体的 には、目標上体位置姿勢瞬時値と目標 ZMPまわりの床反力モーメント瞬時値)とを 補正する。この補正処理をフルモデル補正と呼び、このフルモデル補正が S63の処 理の次に S65で実行される。
[0221] このフルモデル補正は、例えば本願出願人が先に提案した特開 2002— 326173 号公報にて説明したものと全く同様に行なわれる。従って、本明細書での説明は省 略する。なお、フルモデル補正は、例えば、本願出願人による PCT国際公開公報 W 0 03/057427 A1にて説明したものと同様に行なうようにしてもよい。
[0222] この S65のフルモデル補正により、目標上体位置姿勢と、目標 ZMPまわりの床反 力モーメントとが補正され、これにより、最終的に歩容生成装置 100が出力する目標 歩容の全ての構成要素の瞬時値が得られる。
[0223] 以上説明した S01〜S65までの処理が、歩容生成装置 100の演算処理周期毎に 実行される処理である。
[0224] 次に、説明を後回しにした、 S01での移動計画の修正処理 (これは前記 S45の判 断結果が NOとなった場合の修正処理とは異なる）につ 、て説明する。
[0225] ロボット 1が対象物 120を押しながら移動しているときに、現在の移動計画 (前記移 動要求通りの移動計画など)で想定していない段差 (床の凸部）があったり、対象物 1 20にロボット 1以外力も予期しない外力が作用したような場合には、移動計画に基づ く目標対象物運動軌道と実際の対象物運動軌道 (以下、実対象物運動軌道という）と のずれが大きくなる。一方、歩容生成装置 100は、目標対象物運動軌道を基に歩容 ノ メータ等を決定して、歩容を生成するので、目標対象物運動軌道と実対象物運 動軌道とのずれが過大になると、ロボット 1の継続的安定性を確保し得る歩容を生成 することが困難となる。そこで、本実施形態における S01では、そのずれがある程度 大きくなつた場合 (あるいは大きくなると予測される場合）には、 S01で決定する移動 計画をリアルタイムで修正する。
[0226] 以下、この処理の具体例を図 21 (a) , (b)を参照して説明する。例えば現在 (修正 前の）移動計画が、対象物 120を一定速度で X軸の正方向に移動させると ヽぅ計画 であるとする。このとき、この修正前の移動計画 (以下、修正前移動計画という）を基 に、前記した如く S61で算出される目標対象物運動軌道 (瞬時値の時系列）のうちの 目標対象物位置軌道は、ほぼ、図 21 (a)のグラフ g6で示すような軌道となる。ここで、 時刻 tlにおいて、実際の対象物 120が、修正前移動計画で予期していない床の段 差（凸部）に引つ力かって、対象物 120が止まってしまったとする。このとき、実対象物 位置の軌道（S59で推定される実対象物位置の時系列）は、図 21 (a)のグラフ g7で 示すような軌道となる。この場合、時刻 tl以後、修正前移動計画に基づく目標対象物 位置と、実対象物位置との偏差が時間の経過に伴い大きくなつていく。
[0227] そこで、 S01では、例えば前回の演算処理周期の S61で求めた対象物位置偏差（ 実対象物位置と目標対象物位置との差)、すなわち、図 20の減算部 254で求めた対 象物位置偏差の大きさ (絶対値)を所定値と比較するようにしている。そして、その対 象物位置偏差の大きさが所定値よりも大きくなつたとき（図 21 (a)の時刻 t2)には、図 21 (a)のグラフ g8で示す如ぐ対象物位置偏差の増加を抑制するように修正前移動 計画を修正する。修正した移動計画を以下、修正後移動計画という。図 21 (a)の例 では、修正後移動計画は、時刻 t2以後、対象物 120の移動速度ほぼ 0になるまで徐 々に減速するような移動計画となる。なお、修正後移動計画は、それにより規定され る目標対象物運動が修正前移動計画に対して急激な変化を生じることなく滑らかに 連続するように決定することが望ま 、。
[0228] このように移動計画を修正することによって、目標対象物反力が過剰に大きくなつ たりすることがなぐロボット 1の継続的な安定性を確保し得る歩容を生成することがで きる。
[0229] さらに、本実施形態での S01の処理では、上記のように移動計画を修正した後、実 対象物位置の、前記修正後移動計画に対応する目標対象物位置軌道への追従性 が良くなり、目標対象物運動軌道と実対象物運動軌道とのずれがある程度小さくなつ たら、修正後移動計画を修正前移動計画に近づけるように移動計画を修正する。こ の修正後の移動計画を以下、再修正後移動計画という。
[0230] 図 21 (a)に示したように修正後移動計画を決定した場合を例に採って説明すると、 対象物 120が段差を乗り越えると（図 21 (b)の時刻 t3)、実対象物位置軌道は、図 21 (b)のグラフ g7で示すように修正後移動計画に基づく目標対象物位置軌道 g8に近づ いていく。
[0231] このとき、 S01では、対象物位置偏差が所定値よりも小さくなると（図 21 (b)の時刻 t 4)、図 21 (b)のグラフ g9で示す如ぐ対象物 120の移動速度が当初の修正前移動計 画に基づく対象物 120の移動速度に近づくように、修正後移動計画を再修正して、 再修正後移動計画を決定する。図 21 (b)の例では、再修正後移動計画は、時刻 t4 以後、対象物 120の移動速度が当初の修正前移動計画に基づく移動速度にほぼ一 致するまで、該移動速度を徐々に増加させるような移動計画となる。なお、再修正後 移動計画は、それにより規定される目標対象物運動が修正後移動計画に対して急 減な変化を生じることなく滑らかに連続するように決定することが望ましい。
[0232] 上記の例では、再修正後移動計画に基づく対象物 120の移動速度を修正前移動 計画に基づく移動速度に近づけるようにしたが、再修正後移動計画に基づく目標対 象物位置軌道を修正前移動計画に基づく対象物位置軌道に近づけるようにすること も可能である。
[0233] 補足すると、上記のように S01で移動計画を修正する処理が、本発明の第 8発明で 移動計画を修正する処理に相当するものである。
[0234] 以上が、本実施形態における歩容生成装置 100の処理の詳細である。
[0235] 次に、制御ユニット 60の歩容生成装置 100以外の処理を以下に説明する。なお、 この処理は、先に述べたように、特開平 10— 230485号公報 (前記特許文献 1)の第 1実施例にて提案したものと同じであるので、概略的な説明に留める。
[0236] 歩容生成装置 100で生成された目標歩容のうち、目標上体位置姿勢軌道と、目標 ZMP軌道と、目標対象物反力軌道とが対象物反力平衡制御装置 102に入力される 。対象物反力平衡制御装置 102は、目標対象物反力と実際のロボット 1が対象物 12 0から受ける反力である実対象物反力との偏差を解消する（0に近づける）ように、目 標 ZMPまわりの目標床反力モーメントを修正するための対象物反力平衡制御用補 償全床反力を算出すると共に、目標運動のうちの目標上体位置姿勢を修正してなる 修正目標上体位置姿勢を求めるものである。この対象物反力平衡制御装置 102の 処理については後述する。 [0237] また、目標歩容のうちの目標足平位置姿勢軌道、目標 ZMP軌道および目標全床 反力軌道は、脚メイン制御装置 104に入力される。さら〖こ、脚メイン制御装置 104〖こ は、対象物反力平衡制御装置 102から修正目標上体位置姿勢と対象物反力平衡制 御用補償全床反力が入力される。脚メイン制御装置 104は、コンプライアンス制御処 理によって、目標歩容の運動 (腕体 5, 5の運動を除く）と床反力とに追従させるように 脚体 2, 2の関節ァクチユエータ (電動モータ）を制御する。より具体的には、姿勢セン サ 54の検出値 (実上体姿勢)を修正目標上体姿勢に復元させるために、目標 ZMP に発生させるべき復元全床反力を算出し、目標 ZMPに作用する実全床反力モーメ ント成分 (これは脚体 2, 2の 6軸力センサ 50の検出値から求められる）が、この復元 全床反力と目標全床反力と対象物反力平衡制御用補償全床反力との合力のモーメ ント成分に一致するように、目標足平位置姿勢を修正する。修正された目標足平位 置姿勢を修正目標足平位置姿勢という。そして、脚メイン制御装置 104は、この修正 目標足平位置姿勢と修正目標上体位置姿勢とから決定される両脚体 2, 2の目標関 節変位に実関節変位が追従するように両脚体 2, 2の関節ァクチ エータを制御する (各脚体 2のモータ駆動指令を各関節ァクチユエータに出力する)。
[0238] また、目標歩容のうちの目標手先位置姿勢軌道と目標対象物反力軌道とは、腕メ イン制御装置 106に入力される。さらに、腕メイン制御装置 106には、対象物反力平 衡制御装置 102から修正目標上体位置姿勢が入力される。腕メイン制御装置 106は 、コンプライアンス制御処理によって、目標手先位置姿勢軌道と目標対象物反カ軌 道に追従させるように腕体 5, 5の関節ァクチユエータ（電動モータ）を制御する。より 具体的には、 6軸力センサ 52の検出値 (実対象物反力）と目標対象物反力との差に 応じて目標手先位置姿勢を修正する。修正された目標手先位置姿勢を最終修正目 標手先位置姿勢という。そして腕メイン制御装置 106は、この最終修正目標手先位 置姿勢と修正目標上体位置姿勢とから決定される両腕体 5, 5の目標関節変位に実 関節変位が追従するように両腕体 5, 5の関節ァクチユエータを制御する（各腕体 5の モータ駆動指令を各関節ァクチユエータに出力する)。
[0239] 対象物反力平衡制御装置 102の処理をより具体的に以下に説明する。図 22は、 対象物反力平衡制御装置 102の処理を機能的に示すブロック図である。 [0240] 対象物反力平衡制御装置 102では、まず、目標対象物反力による目標 ZMPまわり の目標対象物反力モーメントと実対象物反力による目標 ZMPまわりの実対象物反力 モーメントとの偏差である対象物反力モーメント偏差がモーメント偏差算出部 270に より算出される。この場合、目標対象物反力モーメントは、歩容生成装置 100から出 力された目標対象物反力および目標 ZMPと、腕メイン制御装置 106で求められた最 終修正目標手先位置姿勢 (より詳しくは前回の制御処理周期での値)とから算出され る。また、実対象物反力モーメントは、 6軸力センサ 52の検出値 (実対象物反力）と、 目標 ZMPと、最終修正目標手先位置姿勢 (より詳しくは前回の制御処理周期での値 )とから算出される。
[0241] この対象物反力モーメント偏差に応じて、重心位置摂動量算出部 272により、ロボ ット 1の全体重心位置の目標摂動量である目標重心位置摂動量が算出される。目標 重心位置摂動量は、対象物反力モーメント偏差を、ロボット 1に作用する重力によつ て長期的に解消するためのロボット 1の全体重心の摂動量としての意味を持ち、例え ば対象物反力モーメントに比例した値に決定される。次いで、後述する摂動動力学 モデルで算出される、該摂動動力学モデル上でのロボット 1の全体重心の摂動量で ある全体重心位置モデル摂動量と、前記目標重心位置摂動量との偏差が減算部 27 4により算出され、この偏差力もフィードバック制御則 276、例えば PD制御則によって 、該偏差を 0に収束させるための、目標 ZMPまわりのモーメント操作量である対象物 反力平衡制御用補償全床反力モーメントが算出される。さらに、この対象物反力平 衡制御用補償全床反力モーメントと前記対象物反力モーメント偏差との和が加算部 278により算出される。そして、この加算部 278の出力がロボット 1の全体重心の摂動 と、目標 ZMPまわりのモーメントの摂動との関係、並びに、全体重心の摂動と上体位 置姿勢の摂動との関係を表す摂動動力学モデル 280に入力され、この摂動動力学 モデルで、上体位置姿勢摂動量が算出される。
[0242] この場合、摂動動力学モデルは、ロボット 1の全体重心の摂動と、目標 ZMPまわり のモーメントの摂動との関係（動力学的関係）を次式 18により記述するモデルである
[0243] mtotal * hG * d2 Δ xG/dt2 = AxG水 mtotal水 g+ Δ Μχ ……式 18 ここで、 hGは、目標 ZMP力 全体重心までの高さ、 AxGは全体重心の水平方向摂 動量、 Δ Μχは目標 ZMPまわりのモーメント水平成分である。その他の変数は、前記 ロボット単純ィ匕モデルに関して定義したものと同じである。なお、式 18は、サジタルブ レーン上での式であり、ラテラルプレーン上での関係式は、式 18の右辺の第 2項の 符号を反転させればよい。
[0244] この式 18は、ロボット 1の全体質量 mtotalの質点を持ち、また、その質点の支点であ る目標 ZMPまわりに発生するモーメント水平成分が Δ Mxとなるような倒立振子の動 力学的挙動を示す式である。
[0245] また、ロボット 1の全体重心位置の摂動量 AxGと上体位置の摂動量（以下、これを
Axbと記述する）との関係は、次式 19により表される。
[0246]
Axb=k* AxG ……式 19 ここで、 kはある比例定数である。従って、 Axbは、 AxGに比例するものとされる。摂 動運動に対しては、式 19は近似的に成立すると考えてよい。
[0247] 従って、摂動動力学モデル 280では、式 18の右辺の Δ Μχとして、加算部 278の出 力を用いることで、全体重心の摂動量 AxGが算出され、さらに、この AxG力も式 19 によって、上体位置摂動量が求められる。なお、本実施形態では、修正目標上体位 置姿勢のうちの修正目標上体姿勢は、目標上体姿勢に一致させるものとし、上体姿 勢の摂動量は 0とする。
[0248] 対象物反力平衡制御装置 102では、このようにして摂動動力学モデル 280から出 力される上体位置摂動量を目標上体位置姿勢 (歩容生成装置 100の出力）に加算 部 282で加算することにより、修正目標上体位置姿勢を算出する。
[0249] 以上が対象物反力平衡制御装置 102の具体的な演算処理である。
[0250] 以上説明した第 1実施形態の作動、特に歩容生成装置 100の処理によって、対象 物反力を考慮しつつ、今回歩容が定常歩容に収束するように今回歩容が生成される 。このため、ロボット 1の継続的な安定性を確保しながら、ロボット 1により対象物 120 を押すなどの作業を円滑に行なうことができる。
[0251] 特に、 S23〜S27の処理では、次回歩容の期間内に目標対象物反力の変化点が ある場合に、対象物反力変化時刻で対象物反力が目標対象物反力軌道どおりには 変化しない場合を想定し、そのような場合でも ZMP制約条件を満たしつつ、定常旋 回歩容に収束するような次回歩容 (仮想今回歩容)を生成可能であるか否かが確認 される。そして、生成可能でないときには、移動計画を修正した上で、今回歩容パラメ ータを改めて決定し、今回歩容パラメータを基に算出される目標歩容の瞬時値に応 じてロボット 1の動作制御を行なう。このため、対象物反力が目標対象物反力軌道ど おりに変化しない場合であっても、ロボット 1の継続的な安定性を確保できるように目 標歩容を生成することができる。
[0252]
[第 2実施形態]
次に本発明の第 2実施形態を図 23〜図 25を参照して説明する。なお、この第 2実 施形態の説明では、第 1実施形態と同一構成部分あるいは同一機能部分について は第 1実施形態と同じ参照符号を用い、説明を省略する。
[0253] 図 23は第 2実施形態での歩容生成装置 100の処理のうち、第 1実施形態に係る図 5の処理に対応する部分の処理を示すフローチャートである。同図 23に示すように、 第 2実施形態では、 S19の判断結果力 SNOである場合の処理のみが第 1実施形態と 相違している。
[0254] すなわち、第 1実施形態では、 S19の判断結果力 SNOであるときに、目標対象物反 力軌道を修正するようにした力 第 2実施形態では、これに代えて、 S21 'において、 遊脚足平 22の着地予定位置姿勢または着地予定時刻を修正する。これは今回歩容 ノ メータのうちの足平軌道パラメータを修正することを意味する。この場合、着地予 定位置姿勢または着地予定時刻は、その修正後に S07〜S 17の処理を再度実行し たときに、 ZMP修正量ができるだけ小さくなるように (少なくとも着地予定位置姿勢ま たは着地予定時刻の修正前に求めた ZMP修正量よりも小さくなるように)修正される 。そして、その修正後に、 S07からの処理を再度実行する。これ以外は、第 1実施形 態と同一である。
[0255] S21 'での具体的な修正例を以下に説明する。例えば今現在、今回歩容の遊脚足 平 22の着地予定位置姿勢と次回歩容の遊脚足平 22の着地予定位置姿勢が図 22 の実線で示す如く決定されているとする。なお、図示の例では、ロボット 1をほぼ一定 の歩幅で、今回歩容支持脚座標系の X軸方向に直進歩行させるものとなっている。
[0256] そして、今回歩容まではロボット 1が対象物 120に近づき、次回歩容の途中から、口 ボット 1が対象物 120を X軸の正方向に押す作業を開始するものとする。この場合、 図 25 (a)に示す如ぐ目標対象物反力軌道は、例えば次回歩容の途中から立ち上 力 ¾ようなステップ状の軌道となる。
[0257] このとき、図 24の実線で示すような着地予定位置姿勢を維持したまま、 S17までの 処理を実行すると、 S 17の処理で決定される ZMP修正量 (X軸方向成分）は、例えば 図 25 (b)に実線で示す如ぐ比較的大きなものとなって、 S19の判断結果が NOとな る場合がある。
[0258] この場合に、 S21 'の処理では、例えば次回歩容の遊脚足平 22の着地予定位置 姿勢を図 24の破線で示す如ぐ今回歩容の支持脚足平 22Lに X軸方向で近づける ように修正する。すなわち、次回歩容の歩幅をより小さくするように遊脚足平 22Lの着 地予定位置姿勢を修正する。このように次回歩容の着地予定位置姿勢を修正した後 に、 S05からの処理を再び実行すると、 S 17で決定される ZMP修正量が、図 25 (b) に破線で示す如く小さくなる。その結果、 S 19の判断結果が YESになる。補足すると 、次回歩容の遊脚足平 22Lの着地予定位置姿勢を図 24の如く修正したとき、前記し た定常歩容の足平軌道パラメータの決定手法によって、定常歩容の第 2旋回歩容の 終端遊脚足平位置も、第 2旋回歩容の初期遊脚足平位置 (今回歩容の遊脚足平 22 Rの着地予定位置）〖こ近づくこととなる。
[0259] なお、上記の例では、次回歩容の遊脚足平 22Lの着地予定位置姿勢を修正する ようにしたが、今回歩容の遊脚足平 22Rの着地予定位置姿勢を修正する余裕がある 場合には、それを修正するようにしてもよい。あるいは、今回歩容および次回歩容の 両者の着地予定位置姿勢を修正するようにしてもょ 、。
[0260] また、上記の例では、着地予定位置姿勢を修正するようにしたが、今回歩容と次回 歩容とのうちの少なくともいずれか一方の遊脚足平 22の着地予定時刻を修正するよ うにしてもよい。図 25に示した状況では、例えば次回歩容の着地予定時刻を遅らせ るようにすればよい。
[0261] 以上が本発明の第 2実施形態である。力かる第 2実施形態においても、第 1実施形 態と同様の作用効果を奏することができる。なお、第 2実施形態では、 S19の判断結 果が NOである場合に遊脚足平 22の着地予定位置姿勢または着地予定時刻を修正 するようにしたが、それと併せて、第 1実施形態の如ぐ目標対象物反力軌道を修正 するようにしてちょい。
[0262] なお、以上説明した第 1および第 2実施形態では、ロボット 1により対象物 120を押 して移動させる場合を例に採って説明したが、対象物 120を引いて移動させる場合 や、対象物 120を持ち上げて移動させる場合などにも本発明を適用することができる
[0263] また、前記各実施形態では、今回歩容パラメータを決定するときに、周期的な歩容 である定常歩容の歩容パラメータを決定した上で、該定常歩容に今回歩容を収束さ せるように（定常歩容の初期発散成分に今回歩容の終端発散成分を一致させるよう に)今回歩容パラメータを決定するようにした力 本発明では、それを必須とするもの ではない。本発明は、基本的には、最終的に決定されたロボット ·対象物間目標作用 力 (前記各実施形態では目標対象物反力）と対象物の目標移動位置 (前記各実施 形態では目標対象物運動軌道）とを満足し得るようにロボットの動作を制御できるもの であれば、前記各実施形態で説明した手法と異なる手法でロボットの動作を制御す るようにしてちょい。
産業上の利用可能性
[0264] 以上のように、本発明は、 2足移動ロボットなどのロボットに対象物を移動させる作 業を行わせる場合に、ロボットの安定性を確保しながら、該作業をロボットに円滑に行 なわせることができるものとして有用である。
図面の簡単な説明
[0265] [図 1]本発明の実施形態における脚式移動ロボットのとしての 2足移動ロボットの概略 構成を示す図。 [図 2]図 1のロボットに備えた制御ユニットの構成を示すブロック図。
[図 3]図 2の制御ユニットの要部の機能的構成を示すブロック図。
[図 4]実施形態におけるロボットと対象物との関係を示す図。
[図 5]第 1実施形態における歩容生成装置の演算処理を示すフローチャート。
[図 6]第 1実施形態における歩容生成装置の演算処理を示すフローチャート。
[図 7]第 1実施形態における歩容生成装置の演算処理を示すフローチャート。
[図 8]第 1実施形態における歩容生成装置の演算処理を示すフローチャート。
[図 9]図 5の S02で決定する移動計画の例を示す図。
[図 10]図 5の S02の処理を示すブロック図。
[図 11]図 11 (a)は図 5の S07に係る仮目標 ZMP軌道の例を示す図、図 10 (b)は図 5 の S17で決定される ZMP修正量の例を示す図、図 10 (c)は S17で修正された目標 ZMP軌道の例を示す図。
[図 12]図 5の S11で決定する足平軌道パラメータの例を示す図。
[図 13]図 13 (a)は今回歩容の対象物床反力モーメント軌道の例を示す図、図 13 (b) は図 5の SI 1で決定する定常歩容の対象物反力モーメント軌道の例を示す図。
[図 14]実施形態で用いるロボット動力学モデルの例を示す図。
[図 15]図 14のロボット動力学モデルを使用して上体位置を求める処理を示すブロッ ク図。
[図 16]図 16 (a) , (b)は、図 5の S21の処理を説明するための図。
圆 17]図 17 (a)は移動計画による対象物の速度の時間的変化を例示するグラフ、図
17 (b)は図 17 (a)に対応する対象物反力の時間的変化を例示するグラフ。
[図 18]図 6の S47の処理と S45の判断結果が NOである場合の移動計画の修正処理 との例を説明するための図。
[図 19]図 7の S55の処理を説明するための図。
[図 20]図 8の S61の処理を示すブロック図。
[図 21]図 21 (a) , (b)は、図 5の S01の移動計画の修正処理を説明するための図。
[図 22]図 3に示す対象物反力平衡制御装置の処理を示すブロック図。
圆 23]本発明の第 2実施形態における歩容生成装置の要部の処理を示すフローチ ヤート。
[図 24]図 23の S21 'の処理を説明するための図。
[図 25]図 25 (a) , (b)は図 23の S21 'の処理を説明するための図

Claims

請求の範囲
上体から延設された複数の脚体を備える脚式移動ロボットに対象物を移動させる作 業を行わせるための該ロボットの目標歩容を生成する装置において、
新たな目標歩容を生成するとき、少なくとも対象物の移動計画に基づいて、該新た な目標歩容におけるロボットと対象物との間の作用力の目標値の軌道である第 1ロボ ット ·対象物間目標作用力軌道を仮決定する第 1作用力軌道決定手段と、
少なくとも前記対象物の移動計画に基づいて、前記新たな目標歩容におけるロボ ットの脚体の運動を規定する第 1脚体運動パラメータを仮決定する第 1脚体運動パラ メータ決定手段と、
前記仮決定された第 1ロボット '対象物間目標作用力軌道に対し、実際のロボットと 対象物との間の作用力の軌道が将来の所定時点以後に該第 1ロボット '対象物間目 標作用力軌道と異なる軌道に変化したと仮定して、該所定時点以後のロボットと対象 物との間の作用力の目標値の軌道である第 2ロボット '対象物間目標作用力軌道を 設定する第 2作用力軌道設定手段と、
前記所定時点までのロボットの目標歩容を前記第 1脚体運動パラメータと前記第 1 ロボット '対象物間目標作用力軌道とを用いて生成し、さらに該所定時点までの目標 歩容に続く該所定時点以後のロボットの目標歩容を、該所定時点以後のロボットの 脚体の運動を規定する脚体運動パラメータである第 2脚体運動パラメータを可変的 に設定可能な変数として、該第 2脚体運動パラメータと前記第 2ロボット ·対象物間作 用力とを用いて生成した場合に、該所定時点以後のロボットの目標歩容が所定の動 力学的制約条件を満たし得る第 2脚体運動パラメータを設定可能である力否かを判 断する判断手段とを備え、
該判断手段によって、前記所定時点以後のロボットの目標歩容が前記所定の動力 学的制約条件を満たし得る前記第 2脚体運動パラメータを設定可能であると判断さ れたときには、前記第 1脚体運動パラメータと第 1ロボット '対象物間目標作用力軌道 とを用いて前記新たな目標歩容を生成し、前記所定時点以後のロボットの目標歩容 が前記所定の動力学的制約条件を満たし得る前記第 2脚体運動パラメータを設定可 能でないと判断されたときには、前記対象物の移動計画を修正すると共に、その修 正した移動計画を少なくとも用いて前記第 1脚体運動パラメータと第 1ロボット '対象 物間目標作用力軌道とを再決定し、その再決定した第 1脚体運動パラメータと第 1口 ボット ·対象物間目標作用力軌道とを用いて前記新たな目標歩容を生成するようにし たことを特徴とする脚式移動ロボットの歩容生成装置。
[2] 前記所定時点は、前記第 1作用力軌道決定手段が仮決定した第 1ロボット ·対象物 間目標作用力軌道の作用力の大きさ、または該作用力の変化速度の大きさが所定 の閾値を超える時点であり、前記第 2ロボット '対象物間目標作用力軌道は、前記所 定時点以後のロボットと対象物との間の作用力の大きさ、または該作用力の変化速 度の大きさが前記所定の閾値よりも小さい値となる軌道であることを特徴とする請求 項 1記載の脚式移動ロボットの歩容生成装置。
[3] 前記所定の動力学的制約条件は、少なくともロボットの目標歩容の運動によって発 生する慣性力と該ロボットに作用する重力と該ロボットに対象物から作用する作用力 との合力によって定まる ZMPが所定の許容範囲内に存在するという条件を含むこと を特徴とする請求項 1または 2記載の脚式移動ロボットの歩容生成装置。
PCT/JP2005/017806 2004-12-24 2005-09-28 脚式移動ロボットの歩容生成装置 WO2006067904A1 (ja)

Priority Applications (3)

Application Number Priority Date Filing Date Title
DE602005011492T DE602005011492D1 (de) 2004-12-24 2005-09-28 Gangarterzeugungsvorrichtung eines mit beinen versehenen roboters
EP05788198A EP1842629B1 (en) 2004-12-24 2005-09-28 Gait generating device of a legged mobile robot
US11/722,052 US7840309B2 (en) 2004-12-24 2005-09-28 Gait generating device of legged mobile robot

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2004-372489 2004-12-24
JP2004372489A JP4440761B2 (ja) 2004-12-24 2004-12-24 脚式移動ロボットの制御装置

Publications (1)

Publication Number Publication Date
WO2006067904A1 true WO2006067904A1 (ja) 2006-06-29

Family

ID=36601514

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2005/017806 WO2006067904A1 (ja) 2004-12-24 2005-09-28 脚式移動ロボットの歩容生成装置

Country Status (6)

Country Link
US (1) US7840309B2 (ja)
EP (1) EP1842629B1 (ja)
JP (1) JP4440761B2 (ja)
KR (1) KR101214434B1 (ja)
DE (1) DE602005011492D1 (ja)
WO (1) WO2006067904A1 (ja)

Families Citing this family (19)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP4485279B2 (ja) * 2004-08-02 2010-06-16 本田技研工業株式会社 脚式移動ロボットの歩容生成装置および制御装置
JP4531520B2 (ja) * 2004-10-15 2010-08-25 本田技研工業株式会社 脚式移動ロボットの制御装置
JP4548135B2 (ja) 2005-02-03 2010-09-22 トヨタ自動車株式会社 脚式ロボットとその制御方法
JP4641252B2 (ja) * 2005-12-12 2011-03-02 本田技研工業株式会社 脚式移動ロボットの歩容生成装置
JP4591419B2 (ja) * 2006-07-18 2010-12-01 トヨタ自動車株式会社 ロボットとその制御方法
JP4877520B2 (ja) * 2007-06-26 2012-02-15 株式会社Ｉｈｉ 搬送ロボットとその動作制御方法並びに協調搬送システム及び方法
JP4985776B2 (ja) * 2007-09-25 2012-07-25 富士通株式会社 ロボット制御装置、ロボット制御方法およびロボット制御プログラム
JP4560658B2 (ja) * 2007-12-10 2010-10-13 本田技研工業株式会社 脚式移動ロボットの制御装置
ES2424244T3 (es) * 2009-04-22 2013-09-30 Kuka Roboter Gmbh Procedimiento y dispositivo para regular un manipulador
JP5633166B2 (ja) * 2010-03-23 2014-12-03 トヨタ自動車株式会社 ロボット、及びその制御方法
JP2014073222A (ja) * 2012-10-04 2014-04-24 Sony Corp 運動補助装置及び運動補助方法
KR102146363B1 (ko) * 2013-10-31 2020-08-20 삼성전자주식회사 착용형 로봇 및 그 제어 방법
US9778132B1 (en) * 2015-12-16 2017-10-03 X Development Llc Methods and systems for force sensor calibration
US10351189B2 (en) * 2016-12-13 2019-07-16 Boston Dynamics, Inc. Whole body manipulation on a legged robot using dynamic balance
US10946518B2 (en) * 2018-07-24 2021-03-16 Invia Robotics, Inc. Spatiotemporal controller for controlling robot operation
CN114253260B (zh) * 2021-12-08 2023-08-18 深圳市优必选科技股份有限公司 机器人步态规划方法及装置、运动规划设备和存储介质
GB2620638B (en) * 2022-07-15 2024-10-16 Dyson Technology Ltd Dynamic stability of a robot manipulator
CN115157271B (zh) * 2022-09-05 2022-12-16 杭州柳叶刀机器人有限公司 机械臂控制方法、装置、控制终端及存储介质
KR20240070199A (ko) * 2022-11-14 2024-05-21 주식회사 레인보우로보틱스 복수의 다리를 갖는 로봇 및 그것의 터치다운 위치 결정 방법

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH1148170A (ja) * 1997-08-04 1999-02-23 Honda Motor Co Ltd 脚式移動ロボットの制御装置
WO2003057424A1 (fr) * 2001-12-28 2003-07-17 Honda Giken Kogyo Kabushiki Kaisha Production de demarche pour robot se deplaçant sur des jambes

Family Cites Families (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP3233450B2 (ja) 1992-05-22 2001-11-26 本田技研工業株式会社 指定時刻到達関数発生器
JP3663034B2 (ja) 1996-07-25 2005-06-22 本田技研工業株式会社 脚式移動ロボットの歩容生成装置
JP3672426B2 (ja) 1996-12-19 2005-07-20 本田技研工業株式会社 脚式移動ロボットの姿勢制御装置
DE69734835T2 (de) * 1996-12-19 2006-07-20 Honda Giken Kogyo K.K. Haltungskontrolleur einen sich auf beinen bewegenden robotern
WO1999054095A1 (fr) * 1998-04-20 1999-10-28 Honda Giken Kogyo Kabushiki Kaisha Controleur pour robot mobile muni de jambes
JP4213310B2 (ja) * 1999-08-30 2009-01-21 本田技研工業株式会社 ２足歩行脚式移動ロボット
JP3615702B2 (ja) * 1999-11-25 2005-02-02 ソニー株式会社 脚式移動ロボットの動作制御装置及び動作制御方法、並びに、脚式移動ロボット
EP1361027B1 (en) 2000-11-17 2007-01-17 Honda Giken Kogyo Kabushiki Kaisha Gait pattern generating device for legged mobile robot
JP3726032B2 (ja) 2001-04-27 2005-12-14 本田技研工業株式会社 脚式移動ロボットの目標運動生成装置

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH1148170A (ja) * 1997-08-04 1999-02-23 Honda Motor Co Ltd 脚式移動ロボットの制御装置
WO2003057424A1 (fr) * 2001-12-28 2003-07-17 Honda Giken Kogyo Kabushiki Kaisha Production de demarche pour robot se deplaçant sur des jambes

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
See also references of EP1842629A4 *

Also Published As

Publication number Publication date
EP1842629A1 (en) 2007-10-10
EP1842629B1 (en) 2008-12-03
US7840309B2 (en) 2010-11-23
JP4440761B2 (ja) 2010-03-24
KR20070090988A (ko) 2007-09-06
KR101214434B1 (ko) 2012-12-21
US20080133057A1 (en) 2008-06-05
EP1842629A4 (en) 2008-04-16
DE602005011492D1 (de) 2009-01-15
JP2006175567A (ja) 2006-07-06

Similar Documents

Publication Publication Date Title
JP4485279B2 (ja) 脚式移動ロボットの歩容生成装置および制御装置
JP4808626B2 (ja) 脚式移動ロボットの歩容生成装置
WO2006067904A1 (ja) 脚式移動ロボットの歩容生成装置
JP4641252B2 (ja) 脚式移動ロボットの歩容生成装置
JP4531520B2 (ja) 脚式移動ロボットの制御装置
WO2002040224A1 (fr) Dispositif generateur d'un modele de demarche pour robot mobile pourvu de jambes
JP2005238443A (ja) 脚式移動ロボットの姿勢制御装置

Legal Events

Date Code Title Description
AK Designated states

Kind code of ref document: A1

Designated state(s): AE AG AL AM AT AU AZ BA BB BG BR BW BY BZ CA CH CN CO CR CU CZ DE DK DM DZ EC EE EG ES FI GB GD GE GH GM HR HU ID IL IN IS KE KG KM KP KR KZ LC LK LR LS LT LU LV LY MA MD MG MK MN MW MX MZ NA NG NI NO NZ OM PG PH PL PT RO RU SC SD SE SG SK SL SM SY TJ TM TN TR TT TZ UA UG US UZ VC VN YU ZA ZM ZW

AL Designated countries for regional patents

Kind code of ref document: A1

Designated state(s): BW GH GM KE LS MW MZ NA SD SL SZ TZ UG ZM ZW AM AZ BY KG KZ MD RU TJ TM AT BE BG CH CY CZ DE DK EE ES FI FR GB GR HU IE IS IT LT LU LV MC NL PL PT RO SE SI SK TR BF BJ CF CG CI CM GA GN GQ GW ML MR NE SN TD TG

121 Ep: the epo has been informed by wipo that ep was designated in this application
WWE Wipo information: entry into national phase

Ref document number: 11722052

Country of ref document: US

NENP Non-entry into the national phase

Ref country code: DE

WWE Wipo information: entry into national phase

Ref document number: 1020077014979

Country of ref document: KR

WWE Wipo information: entry into national phase

Ref document number: 2005788198

Country of ref document: EP

WWP Wipo information: published in national office

Ref document number: 2005788198

Country of ref document: EP

WWP Wipo information: published in national office

Ref document number: 11722052

Country of ref document: US