[go: up one dir, main page]

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

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

Info

Publication number
WO2007069401A1
WO2007069401A1 PCT/JP2006/321693 JP2006321693W WO2007069401A1 WO 2007069401 A1 WO2007069401 A1 WO 2007069401A1 JP 2006321693 W JP2006321693 W JP 2006321693W WO 2007069401 A1 WO2007069401 A1 WO 2007069401A1
Authority
WO
WIPO (PCT)
Prior art keywords
posture
gait
target
robot
landing
Prior art date
Legal status (The legal status 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 status listed.)
Ceased
Application number
PCT/JP2006/321693
Other languages
English (en)
French (fr)
Inventor
Tadaaki Hasegawa
Nobuyuki Ohno
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Honda Motor Co Ltd
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 EP06822622A priority Critical patent/EP1974871B1/en
Priority to US12/096,987 priority patent/US8204625B2/en
Priority to DE602006018998T priority patent/DE602006018998D1/de
Publication of WO2007069401A1 publication Critical patent/WO2007069401A1/ja
Anticipated expiration legal-status Critical
Ceased legal-status Critical Current

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • 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

Definitions

  • the present invention relates to an apparatus for generating a desired gait for causing a legged mobile robot to perform an operation of moving an object.
  • a legged mobile robot such as a bipedal mobile robot performs a task such as pushing a target object to move it
  • it is normal to plan the movement of the target object (how and when The target gait of the robot is generated according to the target motion of the target object based on the plan to move to the target.
  • the robot operation control is performed so that the robot operation follows the generated target gait (see, for example, Japanese Patent Laid-Open No. 10-230485).
  • the robot receives only the floor force reaction force (external force) of the moving environment. Receives reaction force (external force).
  • ZMP Zero Moment Point
  • the target gait is generated so that it exists within the square.
  • the position-and-orientation relationship between the robot and the object is usually in the period of each step of the robot.
  • the target gait of the robot is generated so that is in an appropriate position and orientation relationship.
  • the robot can move the robot so that the average moving speed of the target object during each step is equal to the average moving speed of the target object during the period based on the target moving plan.
  • the target landing position / posture of the free leg side leg is determined.
  • the right leg of the mouth bot is usually relatively large in the direction in which the object should be moved. Although it can be stepped on, the left leg cannot actually step in the direction in which the object should be moved due to the influence of constraints to prevent interference with the right leg, etc. .
  • the target gait of the robot is generated so that the position and orientation relationship between the robot and the object in the one-step period becomes an appropriate position and orientation relationship.
  • the free leg side leg is easily affected by the constraint conditions for preventing interference with the support leg side leg.
  • the movement plan must be corrected, or the movement plan must be set to match the feasible form of movement of the robot's legs for each movement direction with respect to the robot. There are inconveniences such as farewell!
  • the present invention has been made in view of the strong background, and it is possible to minimize the situation that requires correction of the movement plan regardless of the moving direction of the object with respect to the robot, and to move the movement plan as much as possible. It is an object of the present invention to provide a gait generator for a legged mobile robot capable of generating a target gait of a robot that can move an object in accordance with the above.
  • an instantaneous movement of the object is performed.
  • the moving speed and moving position of the target are the target values based on the moving plan for each period of multiple steps of the robot that does not require the moving speed and moving position to follow the instantaneous target value based on the moving plan. It is often sufficient to follow the above.
  • a desired gait can be generated for each step of the robot so that the robot conforms to the target motion of the object based on the movement plan while satisfying the constraints for preventing interference between legs.
  • the target gait is satisfied so that the robot is adapted to the movement of the object based on the movement plan by the end time of the period while satisfying the constraint condition.
  • the gait generator for a legged mobile robot of the present invention described below is configured based on such knowledge.
  • the gait generating device for a legged mobile robot moves an object to a legged mobile robot having a plurality of legs extending from the upper body in order to achieve the above object.
  • a gait generator for generating a target gait for performing a work to be performed In a gait generator for generating a target gait for performing a work to be performed,
  • the first temporary target motion trajectory of the object in a predetermined future period including a period of a plurality of steps of the robot is determined based on at least the movement plan of the object.
  • the predetermined request regarding the position and orientation relationship between the object and the robot during the movement of the object is satisfied at least in a predetermined number of steps in the future two or more of the robot, and the movement of the leg of the robot
  • the required requirement for the object moving according to the determined first provisional target motion trajectory is defined as the requirement that the predetermined constraint condition is satisfied for each step up to the predetermined number of steps. It is possible to move the leg of a satisfied robot Judgment means for judging whether or not a certain force is based on at least the first temporary target motion trajectory and the necessary requirements,
  • a target gait of the robot is generated so as to satisfy the necessary requirements for an object moving according to the first temporary target motion trajectory, and the determination
  • the determination result of the means is negative, the movement plan is corrected, and a second temporary target motion trajectory of the object for the predetermined period is newly determined based on at least the corrected movement plan, The target gait of the robot is generated so as to satisfy the necessary requirements for an object moving according to a second temporary target motion trajectory.
  • the determination result of the determination means is positive when the object moves according to the determined first temporary target motion trajectory.
  • the predetermined requirements regarding the position and orientation relationship between the robot and the robot are satisfied at least in the future two or more predetermined number of steps, and the predetermined constraint on the motion of the robot's legs is the predetermined.
  • the object moving according to the first temporary target motion trajectory is satisfied with the requirement (more specifically, it is assumed that the target moves according to the first temporary target motion trajectory.
  • the target gait of the robot is generated so as to satisfy the necessary requirements for the target object.
  • the position and orientation of the future robot at least in the predetermined number of steps are matched with the position and orientation of the target object moving according to the first temporary target motion trajectory based on the movement plan (satisfying the predetermined requirement) It is possible to generate a robot's desired gait that has a position and attitude.
  • the motion trajectory of the object determined by the target gait (the motion trajectory of the object when the robot is operated according to the target gait) is the intermediate instantaneous time until the predetermined number of steps. Then, it does not necessarily coincide with the first temporary target motion trajectory.
  • the negative result of the determination means means that when the object moves according to the determined first temporary target motion trajectory, the object and the robot
  • the predetermined requirements regarding the position and orientation relationship between When a predetermined number of steps are satisfied and a predetermined constraint on the motion of the leg of the robot is satisfied for each number of steps up to the predetermined number of steps, a robot that satisfies the above requirements It means that leg movement is impossible. That is, in this case, it is impossible to generate a target gait of the robot that can move the object according to the movement plan by the predetermined number of steps. Therefore, in this case, the movement plan is corrected, and the second temporary target motion trajectory of the object in the predetermined period is newly determined based on at least the corrected movement plan, and the new determination is made.
  • the target gait of the robot is generated so as to satisfy the necessary requirements for the object moving according to the second temporary target motion trajectory.
  • the position and orientation of the future robot at least in the predetermined number of steps are matched with the position and orientation of the target object that moves according to the second temporary target motion trajectory based on the corrected movement plan (the predetermined position).
  • the target gait of the robot can be generated so that the position and orientation satisfy the above requirements.
  • the motion trajectory of the object determined by the target gait is the second temporary target motion trajectory at the intermediate instantaneous time up to the predetermined number of steps. There is no need to match.
  • the present invention defines the necessary requirements as described above, and corrects the movement plan of the target object only when the determination result is negative. Therefore, the present invention does not depend on the moving direction of the target object with respect to the robot. In addition, it is possible to generate the target gait of the robot that can move the object in accordance with the movement plan as much as possible, while keeping the situation that the movement plan needs to be corrected to the minimum necessary.
  • the predetermined request for example, a request for defining a position / posture relationship between the upper body of the robot and the object, or a position / posture relationship between the planned landing position / posture of the free leg side leg and the object.
  • the predetermined constraint conditions include the condition of the movable range of the free leg side leg of the robot with respect to the supporting leg side leg, the condition range of the operation speed of the joint of the free leg side leg, and the presence of ZMP. Examples include area conditions.
  • the determination unit may make a determination.
  • the judgment result of the judgment means Revise the move plan until the result is positive, and based on the final revised move plan
  • the temporary target motion trajectory of the object determined in V can be used as the second temporary target motion trajectory.
  • the predetermined request includes a request for defining a relationship between a planned landing position / attitude of a free leg side leg of a robot and a position / posture of the target object, and the predetermined restriction.
  • the condition includes a constraint condition that defines the relative landing position / posture of the free leg side leg of the robot with respect to the support leg leg
  • the determination means may be a robot that satisfies the necessary requirements as follows, for example. It is preferable to determine whether or not the leg can be exercised
  • the determination means satisfies the request at each step up to the predetermined number of steps based on the determined first temporary target motion trajectory of the object and the predetermined request.
  • Means for determining the first temporary landing expected position / posture of the free leg side leg of the robot up to the predetermined number of steps so as to add, and the first temporary landing at each step up to the predetermined number of steps Means for determining a second temporary landing planned position / posture by limiting the planned position / posture by the predetermined constraint, and a first temporary landing planned position / posture and a second temporary landing planned position / posture at the predetermined number of steps.
  • the difference between the first temporary landing planned position and posture at the predetermined number of steps is calculated as the difference between the first temporary landing planned position and posture. And at least 1 before the predetermined number of steps.
  • the planned position / posture can be corrected to a position / posture that matches or substantially matches the first temporary landing planned position / posture at the predetermined number of steps within a range where the predetermined constraint is satisfied, It is determined whether or not the robot leg that satisfies the necessary requirements is capable of moving.
  • each of the first temporary landing planned position and posture (first temporary landing planned position and posture at each step up to the predetermined number of steps) is limited by the predetermined restriction condition.
  • the first temporary landing planned position / posture may be determined as the second temporary landing planned position / posture.
  • the first temporary landing planned position / posture does not satisfy the predetermined restriction condition! /
  • Each of the steps up to the predetermined number of steps is within the range satisfying the predetermined restriction condition.
  • the second planned temporary landing position / posture may be determined so that the second temporary landing planned position / attitude for each step matches or is closest to the first temporary landing planned position / posture.
  • the determination unit When the determination unit is configured as described above, the determination result of the determination unit is affirmative, and the first temporary landing scheduled position / posture and the second temporary landing planned at the predetermined number of steps When the positions and orientations match or substantially coincide with each other, the landing position / posture of the free leg side leg at the at least one step before the predetermined number of steps is the first temporary landing scheduled position
  • a means for newly determining the planned landing position / posture of the free leg side leg at each step up to the predetermined number of steps so as to satisfy the necessary requirements is provided.
  • the target gait of the robot is generated according to the planned landing position / posture of the free leg side leg up to the predetermined number of steps.
  • the determination result of the determination means is affirmative, and the first temporary landing scheduled position / posture and the second temporary landing planned position / posture at the predetermined number of steps coincide with each other or substantially coincide with each other. If not, the required landing position / posture of the free leg side leg at least one step before the predetermined number of steps is different from the first temporary landing planned position / posture.
  • the planned landing position / posture of the free leg-side leg up to a predetermined number of steps can be newly determined so as to satisfy the requirements. Therefore, by generating a target gait according to the newly determined planned landing position / posture, the position / posture of the future robot at least in the predetermined number of steps can be calculated based on the first temporary target motion trajectory based on the movement plan.
  • the target gait of the robot can be generated so that the position and orientation (position and orientation satisfying the predetermined requirement) consistent with the position and orientation of the moving object is satisfied.
  • the determination means when the determination means is configured as described above, when the determination result of the determination means is negative, the predetermined number of steps within the range satisfying the predetermined constraint condition.
  • the free leg of the robot up to the predetermined number of steps so that the planned landing position / posture of the free leg side leg of the eye robot approaches the first temporary landing planned position / posture of the predetermined number of steps Means for determining a third temporary landing position / posture of the side leg;
  • the movement distance of the object during the first predetermined period, the time during the first predetermined period, and the object along the second predicted movement path Means for correcting the speed command value of the object in the movement plan based on the movement distance of the object in the second predetermined period when the object is moved and the time of the second predetermined period; I prefer to have
  • the speed command value of the object in the movement plan is calculated.
  • the movement plan of the object is appropriately corrected.
  • the movement plan is corrected so that a target gait that satisfies the necessary requirements can be generated based on the corrected movement plan. Can be done.
  • the first expected movement route is determined based on the movement distance of the object in the first predetermined period and the time of the first predetermined period (the time from the start to the end of the first predetermined period).
  • the average moving speed of the object when the object is moved in step (2) can be determined, and the moving distance of the object in the second predetermined period and the time of the second predetermined period (start of the second predetermined period) From the time until the end time), it is possible to determine the average moving speed of the object when the object is moved along the second predicted movement route.
  • the former average moving speed means an average moving speed of an object suitable for satisfying the above-mentioned requirements
  • the latter average moving speed is an average of objects following the first temporary target motion trajectory. It means moving speed.
  • the movement plan can be appropriately corrected. More specifically, for example, the ratio of the former average moving speed to the latter average moving speed is set as follows.
  • the movement plan may be corrected by multiplying the speed command value of the object in the movement plan.
  • the first predetermined period and the second predetermined period may coincide with each other, but may not coincide with each other.
  • the end times of the first predetermined period and the second predetermined period are, for example, periods from the landing time of the free leg side leg of the predetermined number of steps to the landing time of the next free leg side leg. It is desirable that the time is within. Further, it is desirable that the first predetermined period is, for example, a period corresponding to the predetermined number of steps (a period that is a predetermined number of times longer than the period corresponding to one step).
  • the second predetermined period is, for example, the current time when a new target gait is to be generated and the landing time of the next free leg side leg from the landing time of the free leg side leg of the predetermined number of steps. It may not be a period that is a predetermined multiple of the period of one step, such as the period between the time until the time.
  • the means for correcting the speed command value of the object in the movement plan is the first predetermined period when the direction of the first expected movement path does not coincide with the direction of the second expected movement path.
  • the object is moved in the first predetermined period when the object is moved along a path formed by projecting the first expected movement path in the direction of the second expected movement path. It is preferable to use a moving distance.
  • the first temporary target motion trajectory of the object is a trajectory that changes the direction of the velocity vector of the target over time, such as a turning motion trajectory (first temporary target motion trajectory).
  • first temporary target motion trajectory a trajectory that changes the direction of the velocity vector of the target over time
  • the mouth bot's leg corresponding to the third temporary landing planned position / posture is operated.
  • the direction of the actual object movement path for each step of the robot tends to be biased toward the direction of the second expected movement path. Accordingly, the movement distance of the object during the first predetermined period when the object is moved along the path formed by projecting the first expected movement path in the direction of the second expected movement path is the third temporary movement distance. It is highly reliable as an estimate of the actual movement distance of the target object when the robot's leg motion corresponding to the planned landing position and posture is performed. Therefore, when correcting the speed command value of the object in the movement plan, instead of the movement distance of the object in the first predetermined period,
  • the plan can be modified more appropriately.
  • a gait generating device 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 is provided with a pair of left and right legs (leg links) 2, 2 extending downward from an upper body (base body of 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 and knee joints are connected by a thigh link 24R (L).
  • the knee joint and the ankle joint are connected by a crus link 26R (L).
  • a pair of left and right arm bodies (arm links) 5 and 5 are attached to both side portions of the upper portion of the upper body 3, and a head 4 is disposed at the upper end portion 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) 40R (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 below the ankle joints 18R (L) and 20R (L) of each leg 2 between the foot 22R (L). 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, has a wrist portion (node) 40R (L) and wrist joints 38R (L), 40R (L), 42R (L) of each arm body 5. Detection signals for the three-direction component of the translational force of the external force acting on the hand 40R (L) and the three-way component of the moment are 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 attitude sensor 54 A 3-axis acceleration sensor and 3-axis gyro sensor are provided, and the detection signals from these sensors are used to detect the posture angle (tilt angle and angle) of the upper body 3 and its angular velocity. It is used to estimate the self-position and posture of 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 6-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 showing the overall functional configuration of the legged mobile robot control device according to the embodiment of the present specification.
  • the part surrounded by the broken line in FIG. It is constituted by processing functions to be executed (mainly functions of the first arithmetic device 90 and the second arithmetic device 92).
  • processing functions to be executed mainly functions of the first arithmetic device 90 and the second arithmetic device 92.
  • the symbols R and L are omitted when it is not necessary to distinguish between the left and right of the leg body 2 and the arm body 5.
  • 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 JP 10-230485. Therefore, in the following description of the embodiment, the gait generating device 100 will be mainly described, and the description of the object reaction force balance control device 102, the leg main control device 104, and the arm main control device 106 will be only a schematic description. .
  • the gait generator 100 generates and outputs a desired gait of the robot 1 freely and in real time.
  • the target gait output by the gait generator 100 includes a target body position / posture trajectory (target position and target posture trajectory of the upper body 3), a desired foot position / posture trajectory (the target position of each foot 22 and Target posture trajectory), target hand position / posture trajectory (target position and target posture trajectory of the hand portion 44 of each arm 5), target total floor reaction force central point trajectory (target position trajectory of all floor reaction force central points), It consists of a target total floor reaction force trajectory and a target object reaction force trajectory.
  • the target position / posture trajectory of the movable part is added to the target gait.
  • the “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 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 on the Z axis (vertical axis)
  • the foot posture is represented by a two-axis 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 the object on the robot 1 is moved (the force that acts on the robot 1 from the object). means.
  • the hand portions 44R and 44L of the arms 5 and 5 are engaged with a predetermined portion of the object 120 where the robot 1 is located (a cart in the example shown).
  • 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, but in the embodiment of the present specification, the object reaction force means a translational force acting on the robot 1 from the object 120.
  • a moment around a point acting on the robot 1 by the translational force is called an object anti-camouflage.
  • the object reaction force is the reverse of the sign (direction) of the force applied to the object 120 from the robot 1, so the action force from the robot 1 to the object 120 is replaced with the object reaction force. Use it 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 target object 120 means the spatial orientation of the target object 120 as in the foot posture of the robot 1.
  • the position and orientation of the object 120 may be collectively referred to as object movement.
  • each foot 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,
  • All floor reaction force The resultant force of “each foot floor reaction force” for 22L is called “all floor reaction force”.
  • each floor reaction force is Unless mentioned otherwise, “floor reaction force” is treated as synonymous with “whole floor reaction force” unless otherwise specified.
  • 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.
  • 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 motion of the robot, 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), and a target object reaction force for a predetermined period.
  • 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 two-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), and a one-leg support period. 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, and in the one-leg support period, 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 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.
  • 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 mean the three axes of this support leg coordinate system unless otherwise specified. Note that the origin of the support leg coordinate system is not necessarily the same as 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 floor. [0052] Details of the gait generator 100 according to the first embodiment of the present invention will be described below. 5 to 7 are flowcharts showing the processing of the gait generator 100. 1
  • the gait generator 100 sequentially executes the processes shown in the flowcharts of FIGS. 5 to 7 at a predetermined calculation processing cycle.
  • a movement plan of the object 120 is determined after 1 SO.
  • 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, when 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 in S01 is not necessarily determined according to the movement request, but is appropriately corrected as necessary. This will be described later.
  • step S03 Go forward, based on the movement plan determined in S01 as described above, the target object motion trajectory (target object position / posture trajectory) and target The object reaction trajectory is provisionally determined.
  • FIG. 9 is a block diagram showing a calculation process for obtaining the target object position trajectory and the target object reaction force trajectory in the target object motion trajectory in S03.
  • the portion surrounded by the broken line is the force acting on the object 120 and the luck of the object 120.
  • It is an object dynamics model that expresses the relationship with motion.
  • the case where the object 120 is moved on a substantially horizontal floor will be described as an example for convenience of understanding.
  • the object dynamic model shown in Fig. 9 is a dynamic model (forward dynamics) that outputs the position of the object 120 with the force acting on the object 120 (more specifically, the translational force in the horizontal direction) as an input. Model). More specifically, this object dynamics model is obtained by adding the reciprocal number 1ZM of the mass 120 of the object 120 to the input value of the horizontal translation force acting on the object 120 (the value obtained by the adding unit 204 described later). Is multiplied by a multiplication unit 206 to determine the motion acceleration of the object 120, and this is sequentially integrated (double integration) by the integrators 208 and 210 to obtain the position of the object 120 (on the object dynamics model). 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 which 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 a value obtained as a first-order differential value of the position trajectory of the object 120 in the movement plan, or a component of the movement plan. In the example of the movement plan shown in FIG. 8, it is sufficient to use the slope of the graph gl as it is as the target object speed.
  • the target object speed becomes a constant value continuously. Then, a time series of target object speeds for a predetermined period in the future (for a plurality of steps of the robot 1) from the current time is sequentially input to the subtraction 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 immediately before the newly input target object speed is input to the subtraction unit 200. It is.
  • the required value of the translational force is the target object speed and the object model in the movement plan. It is determined by the feedback control law so that the deviation from the speed converges to 0 (so that the target model speed follows the target target speed). In this example, a proportional control law is used as the feedback control law. Then, 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 that acts on the actual object 120 other than the robot 1 (such as a friction force that acts on the floor force object 120).
  • 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 S35 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 dynamic model is a time series of the object model speed obtained by executing the process of S03 in the previous arithmetic processing cycle of the gait generator 100. Is set to a value at a time corresponding to the previous calculation processing cycle.
  • the initial value of the output of the integrator 210 is set to the previous calculation processing cycle in the time sequence of the target object position obtained by executing the processing of S03 in the previous calculation processing cycle of the gait generator 100. Set to the value at the corresponding time.
  • the target object posture trajectory of the target object motion trajectory is determined so as to substantially match 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! [0064] By the process of S03 described above, a target object movement trajectory and a target object reaction force trajectory for a predetermined period in the future from the current time are provisionally determined.
  • the process of S03 corresponds to the future object motion temporary determination means in the first invention of the present invention, and the determined target object motion trajectory corresponds to the first temporary target motion trajectory.
  • 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 estimated landing position / posture and scheduled landing time to be provisionally determined are at least the planned landing position / posture and landing time of the free leg foot 22 of the current time's gait and the planned landing position of the free leg foot 22 of the next time's gait. Includes position and orientation and estimated landing time.
  • the scheduled landing time of the free leg foot 22 is determined based on the gait cycle (the set value of the time of the unit gait period). For example, the scheduled landing time of the free leg foot 22 of the current time's gait is determined by adding the gait cycle to the start time of the current time's gait.
  • the planned landing position / posture of the free leg foot 22 is within the period from the scheduled landing time to the scheduled landing time of the next free leg foot 22 (that is, 1 after landing the free leg foot 22).
  • the position and orientation relationship between the robot 1 and the object 120 during the walking period) is temporarily determined so as to satisfy a predetermined requirement. This request corresponds to a predetermined request in the present invention.
  • the position and orientation relationship between the robot 1 and the object 120 is, for example, an appropriate distance between the object 120 and the upper body 3 of the robot 1, It is required that the arm bodies 5 and 5 that engage the hand portion 44 with the object 120 are not stretched and the upper body 3 of the robot 1 is not too close to the object 120.
  • the upper body 3 is positioned almost immediately above the supporting leg foot 22 when about half of the gait cycle has elapsed since the start of the unit gait (ie, almost half a step). To do. Further, the position of the supporting leg foot 22 in the unit gait period is determined corresponding to the landing position of the free leg foot 22 in the previous gait.
  • the above request is made based on the planned landing position / posture of the free leg foot 22 and half the time of the gait period (half step time) from the planned landing time.
  • S05 defines the relative position and orientation relationship between the position and orientation of the object 120 at the time.
  • the play is performed so as to satisfy the request (hereinafter referred to as “object-robot position and orientation request”).
  • object-robot position and orientation request The landing position / posture of the leg foot 22 is determined.
  • the free leg foot with respect to the position / posture of the target object 120 when a half step period has elapsed from the scheduled landing time of the free leg foot 22 has elapsed.
  • the relative landing position / posture allowable range of flat 22 is included, and within this allowable range, the planned landing position / posture of the free leg foot 22 for multiple steps including the current time's gait and the next time's gait are tentatively determined. .
  • the planned landing position of the free leg foot 22 is determined from the predetermined part of the object 120 (for example, the portion engaged with the hand part 40 of the robot 1) when a half step has elapsed from the planned landing time.
  • the front / rear direction of the object 120 is a predetermined distance away from the front side of the object 120
  • the front / rear direction of the free leg foot 22 is a half step from the scheduled landing time
  • the landing landing posture of the free leg foot 22 is determined so as to coincide with the front-rear direction of the object 120 or the direction of the velocity vector of the object 120.
  • the planned landing position of the free leg foot 22 in the width direction of the object 120 is the planned landing position when the free leg foot 22 is the right foot 22R and the left foot 22L.
  • the planned landing position in the width direction of the object 120 is the same (for example, the planned landing position is the target). It may be made to coincide with the center position of the object 120 in the width direction).
  • the planned landing position of the free leg foot 22 is determined in accordance with the object's inter-robot position / posture request in the process of S05 by engaging the hand portion 44 of the robot 1 with the object 120, This is a period in which the object 120 should be moved by the robot 1.
  • the planned landing position / posture of the free leg foot 22 in the other period is determined according to a request different from the object / robot position / posture request. For example, when the robot 1 moves the position force away from the object 120 toward the object 120 and then starts to move the object 120. In this case, the robot 1 is moved to a position in front of the object 120 (position for engaging the hand 44 of the robot 1 with the object 120) by the time when the object 120 should start moving.
  • the planned landing position / posture and scheduled landing time of the free leg foot 22 of the robot 1 are determined.
  • the planned landing position / posture of the free leg foot 22 slips while the heel of the free leg foot 22 is landed and the free leg foot 22 is kept in contact with the floor.
  • the position / posture of the supporting leg coordinate system in each unit gait is determined according to the setting method of the supporting leg coordinate system described above. It will be done. Specifically, the supporting leg coordinate system in each unit gait is the foot in the state where the free leg foot 22 of the unit gait immediately preceding the unit gait is matched with the planned landing position / posture.
  • the center force of the ankle joint of the leg 2 having 22 is a coordinate system with the origin at the point where the perpendicular extending to the floor intersects the floor.
  • the planned landing position / posture of the free leg foot 22 temporarily determined in the process of S05 corresponds to the first temporary landing planned position / posture of the present invention.
  • the ZMP trajectory parameters provisionally determined here are determined by the planned landing position / posture and the scheduled landing time of the gait target ZMP (hereinafter referred to as temporary target ZMP), which is stipulated in this step.
  • temporary target ZMP the planned landing position / posture and the scheduled landing time of the gait target ZMP (hereinafter referred to as temporary target ZMP), which is stipulated in this step.
  • the ground contact surface of the supporting leg foot 22 in this time's gait is present in the vicinity of the center of the ground contact surface (so-called support polygon including the ground contact surface of both foot feet 22 in the both-leg support period) and does not change rapidly. To be determined.
  • the tentative target ZMP force of the current time's gait 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 does not change suddenly.
  • the ZMP trajectory parameters are determined.
  • the temporary target ZMP determined by such a guide has a pattern as shown in FIG. 10 (a), for example.
  • Figure 10 (a) shows the X-axis direction of the temporary target ZMP. The position pattern is shown.
  • the position and time of the breakpoint of the temporary target ZMP pattern (trajectory) are provisionally determined as the ZMP trajectory parameters.
  • the object reaction force moment trajectory (time series of instantaneous values of the object reaction force moment) is calculated. More specifically, first, based on the target object motion trajectory, the motion trajectory (position and orientation trajectory) of the part of the object 120 to which the hand portions 44R and 44L of the arms 120 and 5 of the robot 120 should be engaged Is determined.
  • 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 calculated 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 robot 1 is moved close to the object 120 from a location away from the object 120, and then the operation of pushing the object 120 by engaging the hand portions 44R and 44L with the object 120 is started.
  • the object reaction force trajectory up to the time when the object 120 is engaged with the hand portions 44R and 44L (this time is appropriately determined), and hence the object reaction force moment trajectory is set to zero.
  • 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.
  • Posture etc. It means a periodic gait that does not cause discontinuity in the state.
  • the normal turning gait that is a periodic gait is a gait for 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 gait (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 a virtual gait that is assumed temporarily (on the arithmetic processing of the gait generator 100) in order for the gait generator 100 to determine the divergent component at the end of the current time gait. It is not output from the content 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) Upper body position and posture of normal gait The current time's gait is generated so that it converges to the position and orientation (the gait parameter that defines the current time's gait is determined).
  • 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 parameter among the gait parameters of the normal gait is 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 free leg foot 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 foot / posture position / posture at the end of the gait this time is such that the support foot 22 in the initial stage of the gait is in contact with the floor and that the foot 22 does not slip on the floor.
  • This is the position / posture of the supporting leg foot 22 when it is rotated in the pitch direction until it comes into contact (this matches the planned landing position / posture of the free leg foot 22 of the previous gait).
  • 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 at the end of the current time gait viewed 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 support leg foot position / posture at the end of the first turning gait is such that the support leg foot 22 whose position / posture coincides with the next time's gait support 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 free leg foot position / posture of the second turning gait is the supporting leg foot position / posture at the end of the first turning gait as seen from the next time's gait supporting leg coordinate system.
  • the 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 of the supporting leg at the end of the second turning gait is the position / posture viewed from the next time's gait supporting leg coordinate system. It is determined to match the posture.
  • 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.
  • the ZMP trajectory parameter 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 It is decided not to make a drastic change with a high margin.
  • the object reaction force moment trajectory parameter of the normal gait is determined so as to satisfy the periodicity condition based on the object reaction force moment trajectory calculated in S09. For example, suppose 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. 12 (a). Here, the object reaction force moment trajectory of the gait this time is the trajectory obtained in S09. The object reaction force moment trajectory of the next gait is based on the object reaction force trajectory and target object motion trajectory of the next gait obtained in S03 and the ZMP trajectory defined by the ZMP trajectory parameters of the normal turning gait. The trajectory is calculated in the same way as S09.
  • the object reaction force moment trajectory of the next gait is determined by the ZMP trajectory parameters of the next reaction gait and the target object motion trajectory obtained in S03 and the ZMP trajectory parameters of the normal turning gait. This is a trajectory calculated in the same way as S09 based on the trajectory.
  • 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 a trajectory shown in FIG. .
  • 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. It is determined to coincide with the reaction force moment trajectory.
  • 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 object reaction force moment trajectory of the next gait coincides with the object trajectory moment trajectory, and from the time tx to the end, 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. 13 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 robot simplified 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.
  • the inverted pendulum A is composed of a free supporting point a that moves horizontally, one mass point b, and a variable-length link c that connects the supporting point a and the 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 (hereinafter simply referred to as the inverted pendulum mass point b) is the upper body 3 of the robot 3.
  • the inverted pendulum mass point b is sometimes referred to as the upper mass point b.
  • 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 is
  • the mass of upper body 3 and the masses of arm bodies 5 and 5 and head 4 are included.
  • 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. 13, 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.
  • xb, xsup, and xswg are expressed as three-dimensional vectors (XYZ coordinate vectors).
  • X is an arbitrary variable
  • 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 below is the dynamic definition of the total leg inertia moment around point P.
  • Upper 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 action point P is set so that the approximation accuracy of the robot simple model increases.
  • the action point P related to the current time's gait moves from the origin of the support leg coordinate system of the previous time's gait to the origin of the support leg coordinate system of the current time's gait in a straight line at a constant speed during both leg support periods.
  • the current gait support leg coordinate system is set to be maintained at the origin. The same applies to the action point P related to the first turning gait and the second turning gait of the normal turning gait.
  • 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 Equation 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 Expression 03 is inverted 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 leg ZMP calculator 220 calculates the above equations 01 and 02. Is used to calculate the leg ZMP (ZMPfeet).
  • the object reaction force moment around the target ZMP is multiplied by lZmtotal by the multiplication unit 222, whereby the object reaction force ZMP (ZMPobj) is calculated.
  • ZMPobj object reaction force
  • the right-hand side of is computed.
  • the inverted pendulum ZMP (ZMPpend) is calculated.
  • the ZMPpend obtained by the above arithmetic processing is on the sagittal 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 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 that describes the divergent component q on the sagittal plane, and the expression that describes 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 k At) 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 written as Wzmptotal
  • the third term on the right side is written as Wfeet
  • the fourth term on the right side is written as Wobj. That is, Wzmptotal, Wfeet, Wobj are defined by the following equations 08a, 08b, 08c.
  • Formula 08c is the formula that defines Wobj on the sagittal plane. Wobj on the screen is the reverse of the sign of the right side of expression 08c.
  • the initial divergent component that satisfies the boundary condition of the normal turning gait is the current gait.
  • the initial divergence component seen in the support leg coordinate system of the normal turning gait following is the gait next to the normal turning gait.
  • Terminal divergence component of the normal turning gait (normal gait following the current gait) as seen from the support leg coordinate system (next gait support leg coordinate system ( ⁇ "' ⁇ "' coordinate system in Fig. 11)) It is calculated analytically based on the above formulas 08a, 08b, 08c, 09.
  • the support leg coordinate system of the gait next to the normal turning gait (the next gait support leg coordinate system in Fig. 11 (X "' Y" ' The coordinate system)) is converted to a value described in the support leg coordinate system of the normal turning gait (next gait support leg coordinate system ( ⁇ ' ⁇ 'coordinate system) in Fig. 11). If the rotation matrix for ⁇ is ⁇ and the translation vector of the coordinate origin is ⁇ , the following equation 11 is obtained.
  • Equation 12 is a condition that the initial divergence component q [0] and the terminal divergence component q [k] must satisfy in order to satisfy the boundary condition of the normal gait.
  • inv () is the inverse of the matrix in parentheses. Yes, I is the identity matrix.
  • the initial divergent component q [0] that satisfies the boundary condition of the normal turning gait is determined based on Equation 13.
  • Wzmptotal may be calculated by an algorithm using the fact that the target ZMP trajectory is a polygonal trajectory as described in PCT International Publication WO / 02 / 40224A1!
  • 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 planned landing position / posture of the free leg foot 22 is in the previous gait.
  • the foot position / posture trajectory is a trajectory that is maintained at a fixed landing position / posture. However, 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 the gait generator 10
  • 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 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 the end of the current time's gait, It consists of the gait cycle of the gait this time.
  • 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.
  • the free leg foot position posture of the current time gait is determined so that the posture becomes the planned landing position / posture of the current time gait.
  • 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 supporting 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 parameters are set to be the same as the ZMP trajectory parameters provisionally determined in S07. Therefore, the target ZMP trajectory defined by this ZMP trajectory parameter has a pattern as shown in FIG. 10 (a), for example.
  • the object reaction force moment trajectory parameter of the gait parameters of the current time's gait is the object reaction force moment trajectory defined by the object reaction force moment trajectory calculated in S09. It is determined to match the trajectory corresponding to this time's gait.
  • 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 terminal divergent component of the current time gait is added to the initial divergent component of the normal turning gait. Therefore, the temporary target ZMP is corrected by adding a ZMP correction amount as shown in FIG. 10 (b) to the temporary target ZMP. Get the target ZMP as shown.
  • the basic guideline for the correction of 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 is explained for convenience of understanding.
  • the ZMP correction amount is trapezoidal as shown in FIG. 10 (b), 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 At is the end time of the current time's gait, q [0] is the initial divergent component of the current time's gait, Wfeet is the end divergent component generated by the movement of both legs 2 and 2 of the current time's gait, Wobj is the terminal divergent component generated by the object reaction force moment trajectory of the gait this time.
  • Equation 16 the trapezoidal height of the ZMP correction amount
  • the initial divergence component q [0] is calculated from the inverted pendulum position (or body position) at the beginning of the current time's gait and its rate of change, using Equation 06. It is obtained in the same way 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 S 15.
  • Wtrim is determined by setting trapezoidal height a to 1.
  • 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 may be obtained by force search, which is obtained analytically.
  • 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 process proceeds to S19 and the target Z determined by the corrected ZMP trajectory parameters It is determined whether or not the MP is appropriate. 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. 15 (a) is an example of the target body position trajectory determined in accordance with 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. 15 (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. like this ⁇ ⁇ 1 or ⁇ F1 can be obtained analytically or exploratively.
  • the object in the current time gait parameter is set so that the robot 1 moves the upper 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 landing of the free leg foot 22 for multiple steps including the current time gait and the next time gait is added.
  • the planned position / posture or the scheduled landing time may be corrected within a range that satisfies the object / robot position / posture request. In this case, after the processing of S21, the processing of S07 force etc. should be executed.
  • a determination process is performed to determine whether or not the legs 2 and 2 satisfying the necessary requirements can move. This determination process corresponds to the determination means in the present invention.
  • the necessary requirement is, for example, a planned landing position / posture of the free leg foot 22 in the future second step (a planned landing position / posture of the free leg foot 22 in the next gait).
  • a planned landing position / posture of the free leg foot 22 in the next gait is composed of the condition that the planned landing position of the free leg foot 22 of the robot 1 is within a predetermined allowable area with respect to the supporting leg foot 22.
  • the latter condition corresponds to the constraint condition in the present invention.
  • the movement of the free leg foot 22 of the robot 1 is that the free leg side leg 2 does not interfere with the supporting leg side leg 2 and that each leg 2 has a joint.
  • the displacement amount and displacement speed do not exceed the predetermined allowable range, and the ZMP of robot 1 during the movement of the free leg foot 22 increases the stability margin of robot 1 within the contact surface of robot 1 (within the support polygon).
  • Area power that can be secured Deviation Restrictions such as not to do.
  • a permissible area for the planned landing position / posture of the free leg foot 22 in each unit gait is set in advance.
  • This permissible area is an area that is generally elliptical (ellipse that is long in a direction parallel to the X axis of the support leg coordinate system).
  • this permissible area is referred to as the free leg landing position / posture permissible area.
  • the planned landing position / posture of the free leg foot 22 of the current time's gait and the next time's gait are determined as described with respect to the processing of S05, and therefore, the required position / posture of the object 120 is 2
  • the target object's inter-robot position / posture requirement is satisfied with the planned landing position / posture of the free leg foot 22 in each unit gait up to the step.
  • FIG. 16 exemplifies the position and orientation of the object 120 at the current time and the position and orientation of the object 120 obtained by the process (1).
  • the path of the object motion trajectory is a path that turns clockwise as indicated by the dashed curve in the figure.
  • points CO, Cl, and C2 are the positions of the object 120 at the current time (the position of the representative point of the object 120) and the free leg foot 22 of the current time's gait (first step), respectively.
  • the position of the object 120 at the time when the half-step time has elapsed from the scheduled landing time, the time when the half-step time has elapsed from the scheduled landing time of the free leg foot 22 of the next gait (second step) Represents the position of the object 120 at.
  • the supporting leg foot 22 of the gait this time is the foot 22L of the left leg 2L
  • the free leg foot 22 of the first step is the foot of the right leg 2R. 22R
  • the free leg foot 22 of the second step is the foot 22L of the left leg 2L.
  • the free leg foot 22R at the current time indicated by a broken-line rectangle in FIG. 16 is in the air.
  • the travel distance L01 is the position of the object 120 at the time when half the time has elapsed from the scheduled landing time of the free leg foot 22 of the current time's gait from the actual object position at the current time (the position of the point CO).
  • the movement distance L12 is the object at the time when a half step has elapsed from the scheduled landing time of the free leg foot 22 of the current time's gait. From the position of (point C1) to the position of the object 120 (position of point C2) at the time when half a step has elapsed from the scheduled landing time of the free leg foot 22 of the next gait It is the length of V12.
  • the actual object position is the position of the hand part 44 determined by geometric calculation from the detected displacement value (or target value) of each joint of the robot 1 at the current time (the relationship between the robot 1 and the object 120). Calculated based on the position of the engaging portion.
  • the movement distances L01 and L12 are used in the processing of S29 described later, and may be obtained in the processing of S29!
  • the movement path of the object 120 represented by the movement vectors V01 and V12 corresponds to the first expected movement path in the present invention.
  • the planned landing position / posture of the free leg foot 22 of the current time gait (hereinafter referred to as the current free leg landing position / posture! / ⁇ ⁇ ) and the planned landing position / posture of the free leg foot 22 of the next gait (hereinafter referred to as the next free leg landing planned position / attitude) are determined, and the first restricted free leg landing planned position / posture is determined.
  • the first restricted free leg landing position / posture is determined as follows.
  • the current provisional decision value (the value at the start of the processing of S23) of the current free landing landing position / posture is within the free landing landing position / posture allowable region in the current time's gait supporting leg coordinate system.
  • the current temporarily determined value of the planned landing position / posture of the free leg this time is used as it is, and the first restricted free leg landing position / posture of the current time's gait (hereinafter referred to as the first restricted free leg landing planned position / posture). decide.
  • the current provisional decision value for the current free landing landing position / posture deviates from the free landing landing position / posture allowable area, the current temporary landing landing position / posture determination for the current free landing landing position / posture is within the permissible area.
  • the free leg foot that is closest to the value
  • the landing position / posture of 22 is determined as the first restricted free leg landing position / posture this time.
  • the current tentative decision value (the value at the start of the processing of S23) of the next scheduled landing leg position / posture of the next swing will be determined in accordance with the current limited first predicted landing leg position / posture of the next time.
  • the current provisional decision value of the next free leg landing planned position / posture remains as it is, and the first restricted free leg landing planned position of the next gait remains unchanged. It is determined as the posture (hereinafter referred to as the next limited first planned landing position / posture).
  • the landing position posture of the free leg foot 22 that is closest to the current provisional decision value of the next free leg landing position / posture within the permissible area will be the first restriction for the next time. It is determined as the planned landing position / posture of the free swing leg.
  • the current first limited free leg landing planned position / posture and the next planned free leg landing planned position / posture correspond to the second temporary landing planned position / posture of the present invention.
  • next 1st restricted free leg landing planned position / posture matches the current provisional determined value of the next free leg flat position / posture, or the next first restricted free leg landing Whether the planned position / posture of the ground is the power to satisfy the target / robot position / posture with respect to the position / posture of the target 120 at the scheduled landing time of the free leg foot 22 of the next time's gait (in other words, the next time (1) Whether or not the planned free leg landing position / posture is almost the same as the current temporary determined value of the next free leg foot position / posture is determined.
  • the first restricted free leg landing position posture is determined as it is as the second restricted free leg landing position posture.
  • the corrected first landing position / posture of the first restricted leg after the correction deviates from the allowable landing position / posture allowable area in the current time's gait supporting leg coordinate system, The free leg foot 2 that is closest to the first restricted free leg landing position and posture after the correction 2
  • the landing position / posture No. 2 is determined as the second restricted free leg landing planned position / posture this time.
  • the current second restricted free leg landing position / posture as shown by the slanted free leg foot 22R in FIG. 17 is determined.
  • the current second restricted free leg landing planned position / posture is a position / posture different from the current provisional determined value of the current free landing landing position / posture.
  • the free leg landing position / posture allowable area in the next gait supporting leg coordinate system determined in accordance with the second restricted planned landing position and attitude of the next time
  • the current value of the next planned landing position / posture of the next free leg is present, or the landing position of the free leg landing in the next gait supporting leg coordinate system determined according to the second restricted landing planned position / posture this time
  • the next gait play that satisfies the posture tolerance region and satisfies the object's inter-robot position / posture request for the position / posture of the object 120 at the scheduled landing time of the free leg foot 22 of the next gait. It is determined whether or not it is possible to set the planned landing position / posture of the leg foot 22 (this is the same or almost the same as the current value of the planned position / posture of the next free leg landing).
  • the current provisional decision value for the current free landing landing position / posture and the current provisional decision value for the next free landing landing position / posture may satisfy the above-mentioned requirements. It has become. Therefore, in this case, the process proceeds to S31 described later.
  • the process proceeds to S27, and the planned landing position / posture of the free leg foot 22 of the current time gait and the next time gait are temporarily determined again.
  • the current second gait determined in the process of (4a) or (4b) in the determination process of S23 is used as a new temporarily determined value of the planned landing position / posture of the free leg foot 22 of the current time gait. Restricted swing leg landing position / posture is set.
  • the current provisional decision value of the next scheduled landing leg position / posture of the next time is set again as a new provisional decision value of the planned landing position / posture of the free leg foot 22 of the next time's gait.
  • the free leg landing position / posture allowable area in the next time's gait support leg coordinate system determined in accordance with the second restricted free leg landing planned position / posture is satisfied and the free leg foot 22 of the next gait is attached.
  • the planned landing position of the free leg foot 22 of the next gait that can satisfy the object's position-and-robot requirement for the object 120 with respect to the position and orientation of the object 120 at the time when a half step time has elapsed from the scheduled time
  • the posture may be re-determined, and the obtained planned landing position / posture may be set as a new provisional decision value for the planned landing position / posture of the free leg foot 22 of the next gait.
  • the next free landing gear landing position / posture is the free leg foot 22 of the next gait.
  • the object / robot position / posture request is satisfied with respect to the position / posture (position / posture based on the current movement plan) with the target 120 at the time when a half step time has elapsed from the scheduled landing time.
  • the planned landing position / posture of the current free leg tentatively determined in S27 is generally the object at the time when half a step has elapsed from the planned landing time of the free leg foot 22 of the current time's gait.
  • the target object / robot position / posture requirement is not satisfied for 120 positions / postures (position / posture based on the current movement plan).
  • This is the first and second steps in S05 Unlike the case of determining the expected landing position / posture of the eyes, in S27, by moving the planned landing position / posture of the free leg foot 22 of the first step larger, the movement amount of the object 120 in the current gait is moved.
  • This is to temporarily determine the planned landing position / posture of the free leg and the planned landing position / posture of the next free leg.
  • next step determined in accordance with the current second limited free leg landing position / posture (refer to FIG. 17) determined in the process of (4a) or (4b) in the determination process of S23.
  • the next restricted landing leg position of the next landing point that satisfies the free leg landing position / posture allowable area in the support leg coordinate system and that is closest to the current provisional landing position / posture of the next free leg landing position / posture Determine the planned position and orientation.
  • next second restricted free leg landing position / posture as shown by the slanted free leg foot 22L in FIG.
  • the next second restricted free leg landing planned position / posture is a position / posture different from the current provisional determined value of the next free leg landing planned position / posture.
  • next second restricted free leg landing planned position / posture determined here and the current second restricted free leg landing planned position / posture this time are the third temporary planned landing position / posture of the present invention. It is equivalent to.
  • the optimal position and orientation of the object 120 is determined with respect to the supporting leg foot 22 (22L in the example of FIG. 17) of the current time's gait.
  • the optimal position / posture means the optimal position / posture of the object 120 at the time when a half step has elapsed since the start of the current time's gait.
  • the position / posture (that is, the landing position / posture of the free leg foot 22 of the previous gait) is determined so as to satisfy the object / robot position / posture requirement.
  • the position / posture force of the object 120 indicated by the broken line at the position of the point COa The gait start force is determined as the optimum position / posture at the time when a half step time has elapsed (hereinafter referred to as the optimum position / posture of the object 120 in the current gait).
  • the position / posture of the target 120 satisfying the inter-robot position / posture requirement with respect to the current second restricted free leg landing position / posture of this time is the gait (one step). It is determined as the optimal position / posture after the current landing, which is the optimal position / posture of the object 120 at the time when a half step has elapsed from the scheduled landing time of the free leg foot 22.
  • the position / posture of the target 120 that satisfies the target / robot position / posture requirement for the second second restricted free leg landing planned position / posture is the second gait (second step). It is determined as the optimum position / posture after the next landing, which is the optimum position / posture of the object 120 at the time when a half step time has elapsed from the scheduled landing time of the free leg foot 22.
  • the position / posture force of the object 120 indicated by the broken line at the position of the point Cla is determined as the optimum position / posture after landing this time, and the position / posture of the target object 120 indicated by the broken line at the position of the point C2a. Is determined as the optimum position / posture after landing next time.
  • the movement distance W12 is the movement vector from the position (position of point Cla) of the object 120 in the optimal position / posture after landing this time to the position (position of point C2a) in the optimal position / posture after the next landing. It is the length of V12a.
  • the movement path of the object 120 represented by the movement vectors VOla and V12a corresponds to the first expected movement path in the present invention. Further, the movement path of the object 120 represented by the movement vectors V01 and V12 obtained in the process (2) corresponds to the second predicted movement path in the present invention.
  • the vector length WOls (projection distance V01 in the direction of the movement vector V01) is projected from the movement vector VOla in the direction of the movement vector V01. (Projected component of) (see Fig. 18). That is, WOls is obtained by multiplying W01 by the cosine value of the angle formed by the movement vector VOla and the movement vector V01. Similarly, the movement vector V12a is projected in the direction of the movement vector VI 2 based on the movement distance W12, the movement vector V12a, and the movement vector V12 obtained in the process (2) in the determination process of S23.
  • the vector length W12s (projection component in the direction of the movement vector V12 of the movement distance W12) is obtained (see Fig. 18). That is, W12s is obtained by multiplying W12 by the cosine value of the angle formed by movement vector V12a and movement vector V12.
  • the current second restricted free leg landing planned position / posture and the next second restricted free leg landing planned position / posture determined next time are at least in the next gait (second step). Since the object 120 is determined to be close to the path of the object motion trajectory based on the current movement plan (the path indicated by the broken line in FIG. 18), the path that turns the object 120 ( More generally, in the case of a route in which the direction of the velocity vector of the object 120 changes over time, the direction of the movement vector VOla, V 01 in the first step (current gait) is generally The directions of the movement vectors V12a and V12 in the second step (next gait) are different from each other.
  • the planned landing position / posture of the free leg foot 22 in the current gait and the next gait of Robot 1 are changed to the current second restricted free leg landing position / posture and the next second restricted free leg landing planned position / posture, respectively.
  • the actual movement route of the object 120 is the position of the object 120 suitable for the planned landing position / posture (point Cla in FIG. 18).
  • the current landing gait of the robot 1 and the planned landing position / posture of the free leg foot 22 in the next gait are respectively set to the second time.
  • the time corresponding to the optimal position / posture within the current gait when determined to be the restricted free leg landing planned position / posture and the next second restricted free leg landing planned position / posture (half step from the start of the current gait) After the current landing Corresponds to the effective movement distance of the object 120 up to the time corresponding to the optimal position / posture (the time when the next gait's starting force is half a step) and the optimal position / posture after the current landing
  • the effective movement distance of the object 120 from the time until the time corresponding to the optimal position and orientation after the next landing (the time when the next time's gait starts and half step time has elapsed) is estimated. . That is, the movement distances WOls and W12s obtained as described above mean the estimated values of the effective movement distances.
  • the period from the time corresponding to the optimal position / posture in the current time gait to the time corresponding to the optimal position / posture after landing next time corresponds to the first predetermined period in the present invention.
  • the correction coefficient Kt is set to 1 if the upper limit value is 1, and the calculation result on the right side of Equation 100 exceeds 1. Therefore, Vel is less than 1 It is a non-negative value.
  • the denominator of Equation 100 is the period of time from the time corresponding to the optimal position / posture in the current time gait to the time corresponding to the optimal position / posture after the next landing (this is the gait cycle Tm).
  • the numerator of Formula 100 is the time required to move the object 120 from the current position (point CO position) shown in FIGS. 16 to 18 to the position of point C2, in other words, (L01 + L12 ) Means the time required to move the object 120 along the future movement path of the object 120 based on the current movement plan.
  • the time required to move the object 120 from the point CO to the point C1 in FIG. 17 is Trest + O.5 * Tm, and it is necessary to move the object 120 from the point C1 to the point C2.
  • Time is Tm.
  • the period from the current time to the time corresponding to the point C2 corresponds to the second predetermined period in the present invention.
  • the speed command correction parameter Vel determined by Equation 101 is the average moving speed of the object 120 when the object 120 is moved by the moving distance of (W01s + W12s), and (L01 + L12) It means the ratio to the average moving speed of the object 120 when the object 120 is moved by the moving distance. In other words, the average moving speed of the object 120 and the object 120 based on the current movement plan, which are suitable for satisfying the object position / posture request between robots and the free leg landing position / posture allowable region, are satisfied. Is the speed command correction parameter V el.
  • the target moving speed of the object 120 based on the current moving plan (the time series of the target moving speed in the period of a plurality of steps of the robot 1 from the current time) is described above.
  • the movement plan is corrected by multiplying by the speed command correction parameter Vel obtained in the process of (10) as shown in FIG.
  • the target movement speed of the target 120 based on the corrected movement plan (the time series of the target movement speed in the period of multiple steps of the robot 1 from the current time) is the target movement based on the movement plan before the correction. Modify the movement plan so that the speed is multiplied by Vel.
  • the current time force the target body up to the end of the second turning gait.
  • a position and orientation trajectory is created on a trial basis, and the target hand position and orientation trajectory is determined on a trial basis based on the current target object motion trajectory. It may be confirmed whether or not interference between the object 120 and the object 120 does not occur. If the arm 4 is fully stretched or the upper body 3 interferes with the object 120 due to this confirmation, the current time's gait is the expected landing position / posture of the next gait, or You may want to further modify the target object trajectory or the movement plan of object 120.
  • the hand position / posture so as to satisfy the target ZMP based on the current time gait parameters (current time gait parameters finally determined in the processing from S01 to S29).
  • the current time gait instantaneous value at the current time excluding the instantaneous value is calculated.
  • 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. Performed every 22 days.
  • the instantaneous value of the target ZMP is calculated based on the ZMP trajectory parameter of the current time gait parameters, and the target object reaction is calculated based on the object reaction force moment trajectory parameter. The instantaneous value of the force moment is calculated.
  • the dynamic calculation of 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.
  • the target body position / posture obtained in S31 is further corrected by the process of S39 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. 19 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 S03, 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.
  • This object dynamic model is partially different in structure from that shown in Fig. 9, so in the following explanation, the object dynamic model in Fig. 19 is different from the object dynamic model in Fig. 9. To distinguish, object dynamics model 2 and!
  • the basic structure of this object dynamic model 2 is the same as that of the object dynamic model shown in Fig. 9, and the force acting on the object 120 (more specifically, the translational force in the horizontal direction) is input.
  • the input value the value obtained by the multiplier 242 described later
  • the inverse 1ZM of the mass M of the object 120 the multiplier 244
  • the motion acceleration of the object 120 is obtained.
  • 250 for sequential integration (double integration).
  • the integrator 250 adds the model speed manipulated variable in addition to the output of the integrator 246 (integral value of motion acceleration). It is designed to be input automatically.
  • This model speed manipulated variable is a speed manipulated variable determined by the feedback control law so that the deviation between the actual object position and the target object position approaches 0 by the model speed manipulated variable determining unit 252.
  • the form is determined by the following equation 17.
  • Model speed manipulated variable Kel * object position deviation + J (Ke2 * object position deviation) dt
  • the model speed manipulated variable is determined by the PI position law (proportional integral control law) as well as the object position deviation force.
  • Kel and Ke2 are predetermined gains.
  • the model speed manipulated variable determining unit 252 in FIG. 19 is an arithmetic processing unit that performs the calculation of the right side of Equation 17. That is, the model speed manipulated variable determiner 252 determines the actual object position (S33 in the current calculation processing cycle).
  • the subtraction unit 254 obtains the object position deviation which is the difference between the obtained current value) and the target object position (the previous value obtained in S35 in the previous calculation processing cycle).
  • 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. Even in this case, the output of the object dynamic model 2 (the output of the integrator 250) is the same as that of FIG.
  • 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 S27 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.
  • the required value of the translational force input to the object dynamic model 2 is the same as that in FIG. 9, 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).
  • 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.
  • the sign obtained by inverting the sign of the required value of the translational force obtained as described above is the target pair. It is obtained as an instantaneous value of the 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. 19 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 multiplies the differential value (first-order differential value) of the object model speed by the mass of the object 120 (this corresponds to the instantaneous value of the total force acting on the object 120).
  • the estimated value of the translational force acting on the object 120 is obtained by applying the filtering process having the time constant Tc.
  • 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 multiplication unit 242, by the subtraction unit 268.
  • the instantaneous value of the estimated disturbance force obtained here is used in the calculation process of S02 (see Fig. 9) 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 S35.
  • the estimated disturbance force obtained by the calculation processing of S35 is an estimate of the actual disturbance force that acts on the target object 120 other than the robot 1 in real time. Is used in the calculation process of S02 (input to the object dynamic model of FIG. 9), and the behavior (motion state) of the object 120 on the object dynamic model of FIG. It can be close to 120 behavior (motion state).
  • the process proceeds to S37, and 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 S35.
  • 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 the 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 S37 is a gait in which the horizontal component of the floor reaction force moment around the target ZMP is zero on the robot simple model.
  • the robot simple model has a great advantage in that the computation load of the gait generator 100 can be effectively reduced, the dynamic accuracy is not necessarily high. Absent. 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 S39 after the process of S37.
  • 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, for example, in exactly the same way as described in Japanese Patent Laid-Open No. 2002-326173 previously proposed by the applicant of the present application. 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).
  • Object 1 When an unexpected external force other than robot 1 is applied to 20, there is a difference between 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). growing.
  • 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 S33) is the trajectory as shown by the graph g7 in FIG. 20 (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 S35 of the previous calculation processing cycle, that is, the pair obtained by the subtraction unit 254 in FIG.
  • the figurine position deviation is compared with a predetermined value.
  • the object position deviation becomes larger than the predetermined value (time t2 in FIG. 20 (a)), as shown by the graph g8 in FIG. 20 (a), the increase in the object position deviation is suppressed.
  • the movement plan before correction is corrected as follows.
  • the revised movement plan is hereinafter referred to as the revised movement plan.
  • the corrected movement plan is a movement plan that gradually decelerates until the moving speed of the object 120 becomes substantially 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 is used. Can be made closer to the object position trajectory based on the pre-correction movement plan.
  • the situation in which the deviation between the actual object motion trajectory and the target object motion trajectory based on the movement plan becomes large is the effect of the robot 1 acting on the actual object 120. Occurs when the disturbance force other than the force that changes is relatively large. Therefore, instead of correcting the movement plan according to the object position deviation as described above, for example, the estimated disturbance You may want to modify the movement plan according to changes in force.
  • 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.
  • 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. 21 is a block diagram functionally showing processing of the object reaction force balance control apparatus 102.
  • the target object reaction force moment around the target ZMP due to the target object reaction force and the actual object reaction force moment around the target ZMP due to 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 force is calculated.
  • 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 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 is used.
  • a deviation between a certain global centroid position model perturbation amount and the target centroid position perturbation amount is calculated by the subtraction unit 274, and this deviation force is also used to converge the deviation to 0 by a feedback control law 276, for example, a PD control law. Then, the compensation total floor reaction force moment for the target reaction force balance control, which is the moment manipulated variable around the target ZMP, is calculated. Further, 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).
  • Equation 18 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 around the target ZMP that is the fulcrum of the mass is ⁇ Mx. It is a formula which shows a behavior.
  • 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 body position perturbation amount output from the perturbation dynamics model 280 in this way is added to the target body position / posture (output of the gait generator 100).
  • the corrected target body position / posture is calculated.
  • the above is the specific calculation processing of the object reaction force balance control apparatus 102.
  • the free leg feet up to the second step in the future (until the next gait) within the range that can satisfy the necessary requirements.
  • the current time's gait is generated so that the position / orientation of the object 120 can reach the position / orientation target value based on the movement plan within the period of the gait).
  • the position / posture of the object 120 cannot be made to reach the target position / posture based on the movement plan for each step due to the restriction by the free leg landing position / posture allowable region.
  • the frequency of correction of the movement plan is reduced and the object 120 is moved according to the movement plan.
  • the target gait that can be made (current gait) can be generated.
  • the moving path of the object 120 may be a path that linearly moves the object 120 in the front-rear direction of the robot 1, or a path that rotates the object 120, or Even if the robot 1 is walking sideways, the object 120 may be moved in the horizontal direction of the robot 1 to follow the path.
  • the force using the forward dynamic model shown in Fig. 9 is used as the object dynamic model used in the calculation process of S03 (see Fig. 5).
  • the calculation process of S03 may be performed using a dynamic model.
  • an example of this case will be described as a second embodiment.
  • FIG. 22 is a block diagram showing the calculation process of S03 in the present embodiment, and the portion surrounded by a broken line in this block diagram corresponds to the object dynamic model.
  • This object dynamic model uses the target object movement speed as the target motion state quantity of the object 120 based on the movement plan of the object 120 as an input, and the force (translation force) acting on the object 120 and the object It is an inverse dynamics model that outputs 120 positions. More specifically, this object dynamic model obtains the motion acceleration of the object 120 by differentiating the target object moving speed (input value) with the differentiator 300 (first-order differentiation), and further, this motion acceleration The required value of the force (translation force) acting on the object 120 is output by multiplying the mass M of the object 120 by the multiplication unit 302.
  • the required value of the translational force output from the object dynamic model is a required value of the resultant force of all the translational forces (the translational force in the moving direction of the object 120) that the object 120 acts on.
  • the target object dynamic model outputs a target object position which is a target value of the position of the target object 120 by integrating the target target moving speed with the integrator 304.
  • the target object speed is calculated as a first-order differential value of the position trajectory of the object 120 in the movement plan in the gait generator 100 as in the first embodiment, or the target speed of the movement plan
  • the moving speed as a component is directly determined as the target object speed.
  • the initial value of the output of the integrator 304 of the object dynamics model is the time series of the target object position obtained by executing the process of S03 in the previous arithmetic processing cycle of the gait generator 100.
  • the value at the time corresponding to the previous arithmetic processing cycle is set.
  • the target object posture trajectory of the target object motion trajectory is determined so as to substantially match the direction of the target object speed, for example.
  • 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. Except for the process of S03, the process is the same as the first embodiment.
  • a bipedal mobile robot has been described as an example.
  • the present invention can also be applied to a legged mobile robot having more legs.
  • the target gait of the robot 1 is generated in real time while moving so as to follow the target gait.
  • the present invention can also be applied to the case where the contents are generated.
  • the present invention provides a gait generator capable of generating an appropriate target gait for causing a legged mobile robot such as a bipedal mobile robot to move an object. It is useful in.
  • 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 diagram showing an example of a movement plan determined in S02 of FIG.
  • FIG. 9 is a block diagram showing processing of S02 in FIG.
  • FIG. 10 (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. 11 is a diagram showing an example of foot trajectory parameters determined in S 11 of FIG.
  • Fig. 12 (a) shows an example of the object floor reaction force moment trajectory of the current time's gait, and Fig. 12 (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. 13 is a diagram showing an example of a robot dynamics model used in the embodiment.
  • FIG. 14 is a block diagram showing a process for obtaining a body position using the robot dynamic model of FIG.
  • FIGS. 15A and 15B are diagrams for explaining the processing of S21 in FIG.
  • FIG. 16 is a diagram for explaining the processing of S23 and S29 in FIG.
  • FIG. 17 is a diagram for explaining the processing of S23 and S29 in FIG.
  • FIG. 18 is a diagram for explaining the processing of S23 and S29 in FIG.
  • FIG. 19 is a block diagram showing the process of S35 of FIG.
  • FIGS. 20 (a) and 20 (b) are diagrams for explaining the movement plan correction process in S01 of FIG.
  • FIG. 21 is a block diagram showing processing of the object reaction force balance control apparatus shown in FIG.
  • FIG. 22 is a block diagram showing processing of S03 (FIG. 5) in the second embodiment of the present invention.

Landscapes

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

Description

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

Claims

請求の範囲
[1] 上体から延設された複数の脚体を備える脚式移動ロボットに対象物を移動させる作 業を行わせるための目標歩容を生成する歩容生成装置において、
前記ロボットを追従させる目標歩容を新たに生成するとき、少なくとも対象物の移動 計画に基づいて、該ロボットの複数歩分の期間を含む将来の所定期間における対象 物の第 1仮目標運動軌道を決定する将来対象物運動仮決定手段と、
前記対象物の移動時の該対象物とロボットとの間の位置姿勢関係に関する所定の 要求が少なくともロボットの将来の 2以上の所定数の歩数目において満たされ、且つ 、該ロボットの脚体の運動に関する所定の制約条件が当該所定数の歩数目までの各 歩数毎に満たされることを必要要件として、前記決定された第 1仮目標運動軌道に従 つて移動する対象物に対して該必要要件を満足するロボットの脚体の運動が可能で ある力否かを少なくとも該第 1仮目標運動軌道と該必要要件とに基づいて判断する 判断手段とを備え、
前記判断手段の判断結果が肯定的であるときには、前記第 1仮目標運動軌道に従 つて移動する対象物に対して前記必要要件を満足するように前記ロボットの目標歩 容を生成し、前記判断手段の判断結果が否定的であるときには、前記移動計画を修 正すると共に、少なくともその修正後の移動計画に基づいて前記所定期間における 対象物の第 2仮目標運動軌道を新たに決定し、該第 2仮目標運動軌道に従って移 動する対象物に対して前記必要要件を満足するように前記ロボットの目標歩容を生 成するようにしたことを特徴とする脚式移動ロボットの歩容生成装置。
[2] 前記所定の要求は、前記ロボットの遊脚側脚体の着地位置姿勢と前記対象物の位 置姿勢との関係を規定する要求を含むと共に、前記所定の制約条件は、前記ロボッ トの遊脚側脚体の支持脚脚体に対する相対的な着地位置姿勢を規定する制約条件 を含み、
前記判断手段は、前記決定された対象物の第 1仮目標運動軌道と前記所定の要 求とに基づいて、前記所定数の歩数目までの各歩数目において該要求を満足する ように前記ロボットの遊脚側脚体の第 1仮着地予定位置姿勢を該所定数の歩数目ま で決定する手段と、該所定数の歩数目までの各歩数目における該第 1仮着地予定 位置姿勢を、前記所定の制約条件により制限してなる第 2仮着地予定位置姿勢を決 定する手段と、前記所定数の歩数目における第 1仮着地予定位置姿勢および第 2仮 着地予定位置姿勢が互いに一致もしくはほぼ一致している力否かを判断し、一致し ていない場合には、該所定数の歩数目における第 1仮着地予定位置姿勢と第 2仮着 地予定位置姿勢との差分に応じて、該所定数の歩数目よりも前の少なくとも 1つの歩 数目における第 2仮着地予定位置姿勢を前記所定の制約条件を満たす範囲内で修 正する手段とを備え、当該少なくとも 1つの歩数目の修正によって、前記所定数の歩 数目における第 2仮着地予定位置姿勢を、前記所定の制約条件が満足される範囲 内で、該所定数の歩数目における第 1仮着地予定位置姿勢に一致もしくはほぼ一致 する位置姿勢に修正可能であるか否かによって、前記必要要件を満足するロボット の脚体の運動が可能である力否かを判断することを特徴とする請求項 1記載の脚式 移動ロボットの歩容生成装置。
[3] 前記判断手段の判断結果が肯定的であって、且つ、前記所定数の歩数目におけ る第 1仮着地予定位置姿勢および第 2仮着地予定位置姿勢が互いに一致もしくはほ ぼ一致していないとき、前記所定数の歩数目よりも前の少なくとも 1つの歩数目にお ける遊脚側脚体の着地位置姿勢が前記第 1仮着地予定位置姿勢と異なり、且つ、前 記必要要件を満足するように該所定数の歩数目までの各歩数目における遊脚側脚 体の着地予定位置姿勢を新たに決定する手段を備え、その新たに決定した前記所 定数の歩数目までの遊脚側脚体の着地予定位置姿勢に応じてロボットの目標歩容 を生成するようにしたことを特徴とする請求項 2記載の脚式移動ロボットの歩容生成 装置。
[4] 前記判断手段の判断結果が否定的であるとき、前記所定の制約条件を満たす範 囲内で、前記所定数の歩数目のロボットの遊脚側脚体の着地予定位置姿勢を該所 定数の歩数目における前記第 1仮着地予定位置姿勢に近づけるように該所定数の 歩数目までのロボットの遊脚側脚体の第 3仮着地予定位置姿勢を決定する手段と、 その決定した第 3仮着地予定位置姿勢に対して前記所定の要求を満たす前記対 象物の第 1予想移動経路を推定する手段と、
前記第 1仮着地予定位置姿勢に対して前記所定の要求を満たす前記対象物の第 2予想移動経路を推定する手段と、
前記第 1予想移動経路で前記対象物を移動させたとした場合における第 1所定期 間での該対象物の移動距離と、該第 1所定期間の時間と、前記第 2予想移動経路で 前記対象物を移動させたとした場合における第 2所定期間での該対象物の移動距 離と、該第 2所定期間の時間とに基づいて前記移動計画における対象物の速度指 令値を修正する手段とを備えたことを特徴とする請求項 2または 3記載の脚式移動口 ボットの歩容生成装置。
前記移動計画における対象物の速度指令値を修正する手段は、前記第 1予想移 動経路の方向が前記第 2予想移動経路の方向と一致しないとき、前記第 1所定期間 での対象物の移動距離の代わりに、該第 1予想移動経路を第 2予想移動経路の方 向に投影してなる経路で対象物を移動させたとした場合における前記第 1所定期間 での対象物の移動距離を用いることを特徴とする請求項 4記載の脚式移動ロボットの 歩容生成装置。
PCT/JP2006/321693 2005-12-12 2006-10-31 脚式移動ロボットの歩容生成装置 Ceased WO2007069401A1 (ja)

Priority Applications (3)

Application Number Priority Date Filing Date Title
EP06822622A EP1974871B1 (en) 2005-12-12 2006-10-31 Gait creation device of leg-type mobile robot
US12/096,987 US8204625B2 (en) 2005-12-12 2006-10-31 Gait creation device of leg-type mobile robot
DE602006018998T DE602006018998D1 (de) 2005-12-12 2006-10-31 Gangerzeugungsvorrichtung für einen beweglichen roboter mit beinen

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2005-357490 2005-12-12
JP2005357490A JP4641252B2 (ja) 2005-12-12 2005-12-12 脚式移動ロボットの歩容生成装置

Publications (1)

Publication Number Publication Date
WO2007069401A1 true WO2007069401A1 (ja) 2007-06-21

Family

ID=38162717

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2006/321693 Ceased WO2007069401A1 (ja) 2005-12-12 2006-10-31 脚式移動ロボットの歩容生成装置

Country Status (6)

Country Link
US (1) US8204625B2 (ja)
EP (1) EP1974871B1 (ja)
JP (1) JP4641252B2 (ja)
KR (1) KR101011660B1 (ja)
DE (1) DE602006018998D1 (ja)
WO (1) WO2007069401A1 (ja)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20090148035A1 (en) * 2007-12-10 2009-06-11 Honda Motor Co., Ltd. Controller of mobile robot
JP2011255439A (ja) * 2010-06-07 2011-12-22 Honda Motor Co Ltd 脚式移動ロボットの制御装置

Families Citing this family (39)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2093025B1 (en) * 2007-10-23 2012-05-16 Honda Motor Co., Ltd. Two-legged walking robot
JP4899165B2 (ja) 2007-12-10 2012-03-21 本田技研工業株式会社 脚式移動ロボットの制御装置
JP4565229B2 (ja) 2007-12-10 2010-10-20 本田技研工業株式会社 ロボット
US8024069B2 (en) * 2009-01-28 2011-09-20 Ge Intelligent Platforms, Inc. System and method for path planning
KR20110017500A (ko) * 2009-08-14 2011-02-22 삼성전자주식회사 다리식 이동 로봇의 제어장치 및 그 제어방법
JP5483997B2 (ja) * 2009-10-28 2014-05-07 本田技研工業株式会社 脚式移動ロボットの制御装置
JP5232124B2 (ja) * 2009-10-28 2013-07-10 本田技研工業株式会社 脚式移動ロボットの制御装置
JP5506618B2 (ja) * 2009-12-28 2014-05-28 本田技研工業株式会社 ロボットの制御装置
JP5506617B2 (ja) * 2009-12-28 2014-05-28 本田技研工業株式会社 ロボットの制御装置
JP5282852B2 (ja) * 2010-07-22 2013-09-04 トヨタ自動車株式会社 二足歩行ロボット及び二足歩行ロボットの着地タイミング決定方法
JP5465142B2 (ja) 2010-09-27 2014-04-09 本田技研工業株式会社 ロボットおよびその行動制御システム
CN102183956B (zh) * 2011-02-24 2012-11-21 浙江大学宁波理工学院 欠驱动两足步行机器人水平行走时的侧向控制方法
JP2014073222A (ja) * 2012-10-04 2014-04-24 Sony Corp 運動補助装置及び運動補助方法
JP6228884B2 (ja) * 2014-04-01 2017-11-08 富士ソフト株式会社 ロボット用動作計画作成システムおよび作成方法
US10081098B1 (en) 2014-08-25 2018-09-25 Boston Dynamics, Inc. Generalized coordinate surrogates for integrated estimation and control
US9517561B2 (en) * 2014-08-25 2016-12-13 Google Inc. Natural pitch and roll
US9387588B1 (en) 2014-08-25 2016-07-12 Google Inc. Handling gait disturbances with asynchronous timing
US9618937B1 (en) 2014-08-25 2017-04-11 Google Inc. Slip detection using robotic limbs
US9446518B1 (en) 2014-11-11 2016-09-20 Google Inc. Leg collision avoidance in a robotic device
US9804593B1 (en) * 2014-12-12 2017-10-31 X Development Llc Methods and systems for teaching positions to components of devices
JP5927284B1 (ja) * 2014-12-22 2016-06-01 ファナック株式会社 人との接触力を検出してロボットを停止させるロボット制御装置
US9499218B1 (en) 2014-12-30 2016-11-22 Google Inc. Mechanically-timed footsteps for a robotic device
US11000944B2 (en) * 2015-04-22 2021-05-11 Massachusetts Institute Of Technology Foot touch position following apparatus, method of controlling movement thereof, and non-transitory computer-readable information recording medium storing the same
US9594377B1 (en) * 2015-05-12 2017-03-14 Google Inc. Auto-height swing adjustment
JP6450273B2 (ja) * 2015-07-08 2019-01-09 本田技研工業株式会社 移動ロボットの動作環境情報生成装置
US9586316B1 (en) 2015-09-15 2017-03-07 Google Inc. Determination of robotic step path
US9778132B1 (en) * 2015-12-16 2017-10-03 X Development Llc Methods and systems for force sensor calibration
JP6483014B2 (ja) * 2015-12-25 2019-03-13 本田技研工業株式会社 移動ロボットの制御装置
US9925667B1 (en) 2016-01-25 2018-03-27 Boston Dynamics, Inc. Continuous slip recovery
US9789919B1 (en) 2016-03-22 2017-10-17 Google Inc. Mitigating sensor noise in legged robots
US10351189B2 (en) * 2016-12-13 2019-07-16 Boston Dynamics, Inc. Whole body manipulation on a legged robot using dynamic balance
CN109997014B (zh) 2017-11-03 2023-08-18 北京嘀嘀无限科技发展有限公司 用于确定轨迹的系统和方法
US11000950B2 (en) 2018-06-01 2021-05-11 X Development Llc Robotic motion planning
WO2020133288A1 (zh) * 2018-12-28 2020-07-02 深圳市优必选科技有限公司 一种双足机器人步态控制方法以及双足机器人
US11518024B2 (en) 2021-02-10 2022-12-06 Intrinsic Innovation Llc Extensible underconstrained robotic motion planning
CN113253724B (zh) * 2021-04-30 2024-05-21 深圳市优必选科技股份有限公司 一种步态规划方法、装置、计算机可读存储介质及机器人
CN113246124B (zh) * 2021-04-30 2023-06-02 深圳市优必选科技股份有限公司 机器人控制方法、装置、计算机可读存储介质及机器人
US12115670B2 (en) 2021-12-15 2024-10-15 Intrinsic Innovation Llc Equipment specific motion plan generation for robotic skill adaptation
KR102642245B1 (ko) * 2023-11-24 2024-02-29 주식회사 나우로보틱스 작업 공간 힘/가속도 외란 관측기 및 이를 포함하는 로봇

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH10217161A (ja) * 1997-01-31 1998-08-18 Honda Motor Co Ltd 脚式移動ロボットの歩容生成装置
JP2006175567A (ja) * 2004-12-24 2006-07-06 Honda Motor Co Ltd 脚式移動ロボットの制御装置

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP3672426B2 (ja) * 1996-12-19 2005-07-20 本田技研工業株式会社 脚式移動ロボットの姿勢制御装置
EP0965416B1 (en) * 1996-12-19 2005-12-07 Honda Giken Kogyo Kabushiki Kaisha Attitude controller of legged moving robot
WO2003090982A1 (en) * 2002-04-26 2003-11-06 Honda Giken Kogyo Kabushiki Kaisha Control device and footstep determination device for legged mobile robot
JP3833567B2 (ja) * 2002-05-01 2006-10-11 本田技研工業株式会社 移動ロボットの姿勢制御装置
JP4029152B2 (ja) * 2003-01-08 2008-01-09 独立行政法人産業技術総合研究所 脚式移動ロボット及び制御方法
JP4485279B2 (ja) * 2004-08-02 2010-06-16 本田技研工業株式会社 脚式移動ロボットの歩容生成装置および制御装置
JP4808626B2 (ja) * 2004-10-15 2011-11-02 本田技研工業株式会社 脚式移動ロボットの歩容生成装置

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH10217161A (ja) * 1997-01-31 1998-08-18 Honda Motor Co Ltd 脚式移動ロボットの歩容生成装置
JP2006175567A (ja) * 2004-12-24 2006-07-06 Honda Motor Co Ltd 脚式移動ロボットの制御装置

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20090148035A1 (en) * 2007-12-10 2009-06-11 Honda Motor Co., Ltd. Controller of mobile robot
US8340823B2 (en) * 2007-12-10 2012-12-25 Honda Motor Co., Ltd. Controller of mobile robot
JP2011255439A (ja) * 2010-06-07 2011-12-22 Honda Motor Co Ltd 脚式移動ロボットの制御装置

Also Published As

Publication number Publication date
EP1974871B1 (en) 2010-12-15
DE602006018998D1 (de) 2011-01-27
JP4641252B2 (ja) 2011-03-02
US8204625B2 (en) 2012-06-19
US20090312867A1 (en) 2009-12-17
EP1974871A1 (en) 2008-10-01
KR101011660B1 (ko) 2011-01-28
JP2007160428A (ja) 2007-06-28
KR20080048075A (ko) 2008-05-30
EP1974871A4 (en) 2009-09-02

Similar Documents

Publication Publication Date Title
JP4641252B2 (ja) 脚式移動ロボットの歩容生成装置
JP4485279B2 (ja) 脚式移動ロボットの歩容生成装置および制御装置
JP4531520B2 (ja) 脚式移動ロボットの制御装置
JP4808626B2 (ja) 脚式移動ロボットの歩容生成装置
JP4440761B2 (ja) 脚式移動ロボットの制御装置
WO2002040224A1 (en) Gait pattern generating device for legged mobile robot
JP2002326173A (ja) 脚式移動ロボットの動作生成装置
WO2005082583A1 (ja) 移動ロボットの歩容生成装置
JP3726097B2 (ja) 脚式移動ロボットの姿勢制御装置

Legal Events

Date Code Title Description
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: 1020087009232

Country of ref document: KR

WWE Wipo information: entry into national phase

Ref document number: 12096987

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: 2006822622

Country of ref document: EP