WO2007032120A1 - 歩行パターン作成装置、２足歩行ロボット装置、歩行パターン作成方法、２足歩行ロボット装置の制御方法、プログラムおよび記録媒体 - Google Patents

歩行パターン作成装置、２足歩行ロボット装置、歩行パターン作成方法、２足歩行ロボット装置の制御方法、プログラムおよび記録媒体 Download PDF

Info

Publication number
WO2007032120A1
WO2007032120A1 PCT/JP2006/308590 JP2006308590W WO2007032120A1 WO 2007032120 A1 WO2007032120 A1 WO 2007032120A1 JP 2006308590 W JP2006308590 W JP 2006308590W WO 2007032120 A1 WO2007032120 A1 WO 2007032120A1
Authority
WO
WIPO (PCT)
Prior art keywords
foot
coordinate system
posture
moment
calculating
Prior art date
Application number
PCT/JP2006/308590
Other languages
English (en)
French (fr)
Inventor
Atsuo Takanishi
Kenji Hashimoto
Yusuke Sugahara
Hiroyuki Sunazuka
Masamiki Kawase
Akihiro Ohta
Chiaki Tanaka
Nobutsuna Endo
Terumasa Sawato
Akihiro Hayashi
Yoichi Takamoto
Katsuyuki Baba
Shigeaki Ino
Original Assignee
Tmsuk 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 Tmsuk Co., Ltd. filed Critical Tmsuk Co., Ltd.
Priority to JP2007535380A priority Critical patent/JPWO2007032120A1/ja
Publication of WO2007032120A1 publication Critical patent/WO2007032120A1/ja

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
    • B62D—MOTOR VEHICLES; TRAILERS
    • B62D57/00—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track
    • B62D57/02—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track with ground-engaging propulsion means, e.g. walking members
    • B62D57/032—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track with ground-engaging propulsion means, e.g. walking members with alternately or sequentially lifted supporting base and legs; with alternately or sequentially lifted feet or skid
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J17/00—Joints
    • B25J17/02—Wrist joints
    • B25J17/0208—Compliance devices
    • B25J17/0216—Compliance devices comprising a stewart mechanism

Definitions

  • Walking pattern creation device biped walking robot device, walking pattern creation method, biped walking robot device control method, program and recording medium
  • the present invention relates to an apparatus for creating a walking pattern that causes a biped walking robot apparatus having a parallel link mechanism to perform biped walking, a biped walking robot apparatus that performs biped walking based on the walking pattern, and biped
  • Patent Document 1 a parallel link driving mechanism using three unit links is described. It also describes that no ringing can be realized.
  • walking there is no description about a biped robot device having a normal link mechanism, and also about stable walking of this robot device. It has been,
  • the biped walking stability method on irregular road surface uses ZMP, trunk posture, body acceleration, etc. as the observation amount, and when the robot steps on the road surface unevenness, ZMP and trunk posture
  • ZMP trunk posture
  • a common method is to observe rapid changes in acceleration deviation and perform high-precision acceleration control based on the dynamic model of the robot. These are very effective when the model of the robot itself can be obtained relatively accurately. It was difficult to apply to robots that are difficult to model accurately, such as walking with humans.
  • Patent Document 1 Japanese Patent Laid-Open No. 2001-121460 Disclosure of the invention
  • the present invention satisfies the above-described conventional requirements, and is a walking pattern creation device that creates a walking pattern that can cause a biped walking robot device having a parallel link mechanism to perform stable biped walking.
  • a biped walking robot device with a parallel link mechanism capable of biped walking, and a walking pattern creation method for creating a biped walking pattern that can cause a biped walking robot device with a parallel link mechanism to perform stable biped walking A biped walking robot device that can perform stable biped walking on a biped robot device with a parallel link mechanism, a method for creating the walking pattern, and a control method for the biped robot device.
  • An object is to provide a program and a recording medium for executing the program.
  • a walking pattern creation device of the present invention is a walking pattern creation device that creates a walking pattern of a biped robot device including a leg portion and a waist portion including a foot portion.
  • Set the target zero moment point at the foot calculate the moment compensation trajectory of the waist according to the set target mouth moment point, and the foot coordinate system in the absolute coordinate system and the origin of the waist coordinate system in the absolute coordinate system
  • From the rotation matrix calculate the rotation matrix that represents the posture of the foot coordinate system as seen from the hip coordinate system, calculate the posture of the foot using the rotation matrix that represents the posture of the foot coordinate system, Late or free leg
  • a configuration for determining and storing the gait flag is a flag indicating whether the period or after stage.
  • a walking pattern creation device for creating a walking pattern that allows a biped walking robot device with a normal link mechanism to perform stable biped walking is obtained.
  • a biped robot device of the present invention includes a leg portion including a foot portion, A biped walking mouth bot device comprising a waist part, a leg part having a cylinder that expands and contracts the leg part, and a waist part that controls the whole and also has a control computer that performs wireless communication with the walking pattern creation device, The control computer is configured to control biped walking based on the walking pattern received from the walking pattern creation device.
  • a walking pattern creation method of the present invention is a walking pattern creation method for creating a walking pattern of a biped robot device including a leg portion and a waist portion including a foot portion.
  • Set the target zero moment point at the foot calculate the moment compensation trajectory of the waist according to the set target mouth moment point, and the foot coordinate system in the absolute coordinate system and the origin of the waist coordinate system in the absolute coordinate system
  • From the rotation matrix calculate the rotation matrix that represents the posture of the foot coordinate system as seen from the hip coordinate system, calculate the posture of the foot using the rotation matrix that represents the posture of the foot coordinate system, Late or free leg
  • a configuration for determining and storing the gait flag is a flag indicating whether the period or after stage.
  • a walking pattern creation method for creating a walking pattern that causes a biped walking robot device with a normal link mechanism to perform stable biped walking is obtained.
  • a control method for a biped robot device includes a leg portion and a waist portion including a foot portion, and the leg portion includes a cylinder for extending and contracting the leg portion.
  • a control method of a biped walking robot apparatus having a control computer that controls the whole and wirelessly communicates with the above-described walking pattern creation device, and the control computer uses the walking pattern creation device force 2 based on the received walking pattern. It has a structure that controls foot walking.
  • a program of the present invention causes a computer to execute the steps of the walking pattern creation method and the control method of the biped robot device. It has a configuration that is a program.
  • a recording medium of the present invention has a configuration that is a computer-readable recording medium that records the program.
  • a walking pattern creation device is a walking pattern creation device that creates a walking pattern of a biped walking robot device including a leg portion and a waist portion including a foot portion.
  • the target zero moment point is set in the head
  • the moment compensation trajectory of the waist is calculated according to the set target zero moment point
  • the waist is calculated from the origin of the waist coordinate system in the absolute coordinate system and the origin of the foot coordinate system in the absolute coordinate system.
  • the position of the foot viewed from the coordinate system is calculated
  • the waist coordinate system is calculated from the rotation matrix that represents the posture of the foot coordinate system viewed from the absolute coordinate system and the rotation matrix that represents the posture of the waist coordinate system viewed from the absolute coordinate system.
  • the biped robot It is possible to stabilize the walking to prevent falling, and to generate the position and posture data of the foot viewed from the waist coordinate system, so real-time calculation is difficult in the normal link mechanism
  • the length of the cylinder that expands and contracts the leg can be controlled by inverse kinematic calculations that are easy to perform real-time calculations.
  • the gait flag which is a flag indicating whether it is the first period or the second period and the first period or the second period of the swing leg.
  • Execute complex control charts depending on the state of each leg As a result, the free leg generates a large floor reaction force, and the copying operation to the road surface shape can be completed before the ZMP, posture angle deviation or acceleration occurs. It is possible to accurately perform control according to the unevenness, and to realize walking on rough terrain even when accurate modeling is difficult, such as when a human is mounted. There is an advantageous effect that it can be performed using only a force sensor (6-axis force sensor) for ZMP measurement without requiring a special sensor or mechanism including a gyro.
  • the walking pattern generation device is a trajectory setting means for setting a foot trajectory and a waist initial trajectory, and a target zero moment based on the set foot trajectory and the waist initial trajectory.
  • the target ZMP trajectory setting means for setting the trajectory of the point
  • the target ZMP moment calculating means for calculating the moment around the target zero moment point due to the movement of the leg and the waist, and the moment around the calculated target zero moment point
  • the foot position seen from the waist coordinate system is calculated from the origin of the waist coordinate system in the absolute coordinate system and the origin of the foot coordinate system in the absolute coordinate system, and the foot seen from the absolute coordinate system.
  • the rotation matrix that represents the posture of the coordinate system and the rotation matrix that represents the posture of the hip coordinate system viewed from the absolute coordinate system is calculated.
  • foot position / posture calculation means for calculating the posture of the foot using a rotation matrix representing the posture
  • gait flag determination means for determining and storing a gait flag
  • the movement of the leg and the movement of the waist Whether or not the moment around the target zero moment point is near zero is determined by the error moment, and there is no limit to the moment around the target zero moment point when setting the walking pattern. It is possible to approach the advantageous effect that it is possible to perform the bipedal walking robot high biped Te pole Me stability is obtained.
  • the walking pattern creation device includes a ZMP rotation moment calculation means.
  • Fourier transform means for calculating the Fourier coefficient based on the sum of the calculated moment around the target zero moment point and the calculated error moment, and Fourier for obtaining an approximate solution of the moment compensation trajectory based on the calculated Fourier coefficient Fourier coefficient determination means for determining a coefficient
  • the moment compensation trajectory calculation means accurately calculates the moment compensation trajectory of the waist by calculating the moment compensation trajectory of the waist by inverse Fourier transform based on the determined Fourier coefficient. Therefore, there is an advantageous effect that the moment around the target zero moment point can be brought close to zero quickly without the trajectory diverging.
  • the target ZMP rotation moment calculation means includes a target ZMP rotation moment calculation means by lower limb movement that calculates a target ZMP rotation moment generated by the movement of the leg,
  • the foot position / posture calculation means is viewed from the waist coordinate system from the origin of the waist coordinate system in the absolute coordinate system and the origin of the foot coordinate system in the absolute coordinate system. Coordinates based on the origin position calculation means for calculating the position of the foot, the rotation matrix that represents the posture of the foot coordinate system as seen from the absolute coordinate system force, and the rotation matrix that represents the posture of the waist coordinate system as seen from the absolute coordinate system A rotation matrix calculating means for calculating a rotation matrix representing the posture of the foot coordinate system viewed from the system, and a posture calculating means for calculating the posture of the foot using the rotation matrix calculated by the rotation matrix calculating means, Since the position and posture data of the foot as seen from the waist coordinate system can be generated accurately, the length of the cylinder that expands and contracts the leg can be accurately controlled using inverse kinematics, which is easy to calculate in real time. Can be used for inverse kinematics If accurate control can be performed when various control is performed using various correction data
  • the biped walking robot device includes a leg portion and a waist portion including a foot portion, the leg portion includes a cylinder that expands and contracts the leg portion, and the waist portion controls the whole and is claimed.
  • Item 1 to 5 A biped walking robot device having a control computer that performs wireless communication with the walking pattern creation device according to V, Deviation 1.
  • the control computer is biped walking based on the walking pattern received from the walking pattern creation device. By controlling this, the moment at the target zero moment point of the foot can be brought close to zero by the moment compensation trajectory of the waist, and walking can be stabilized and the walking pattern can be received even remotely via radio.
  • a biped robot that can be used for various purposes (for example, for rescue, nursing, and dangerous work) can be quickly constructed. It is suitable for various purposes (for example, for welfare, for transportation) if a riding section such as a chair is attached to the lumbar area to enable human boarding.
  • the biped walking robot can be constructed easily and quickly, and the length of the cylinder that expands and contracts the leg by performing inverse kinematics operation on the foot position and posture data viewed from the waist coordinate system.
  • the control computer receives the walking pattern setting computer means for receiving the walking pattern wirelessly and storing the walking pattern in the memory.
  • a walking start command means for selecting a pattern to be used for walking and instructing the start of walking, and a motor control means for controlling the length of the cylinder by controlling the motor based on the command of the walking start command means! Therefore, the biped robot device can perform walking with high stability without being physically constrained by the computer power for creating the walking pattern in the walking state. If the user can walk with an appropriate walking pattern according to the light weight state), a favorable effect can be obtained.
  • the motor control means includes basic control means for performing basic control, and the free leg is allowed to land on the road surface while reducing the impact, and the foot portion is provided on the uneven surface of the road surface.
  • The By having the landing control means to follow, the basic operation based on the basic control can be performed and the landing control can be performed according to the situation.
  • the link length (that is, the cylinder length) can be controlled in real time by inverse kinematics, which is easy to calculate.
  • the road surface shape can be controlled.
  • a robot that is able to perform stable walking without falling over an irregular road surface, such as a human-climbing biped walking robot, where accurate modeling is difficult
  • the advantage is that walking on rough terrain can be realized.
  • the basic control means is based on the command of the walking start command means based on the target value of the foot position / posture calculated by the foot position / posture calculation means!
  • Gait pattern output means for outputting and gait pattern output means
  • a reverse kinematics calculation means for calculating the length of the cylinder by performing reverse kinematics calculation of the target position of the foot position where the force is also output. Therefore, it is not necessary to perform forward kinematics, which is difficult for the leg mechanism of the parallel mechanism due to the large amount of calculation, and the link length can be controlled in real time by inverse kinematics, which is easy to calculate. Therefore, it is possible to obtain an advantageous effect that various controls can be performed on the position and posture of the foot.
  • the landing control means includes mode selection means for selecting a control mode such as control, maintenance, return, compliance, etc. based on a gait flag;
  • the component dividing means for dividing the control target component that is the target of landing control and the non-control target component other than the control target component, and the non-linear compliance movement amount that reduces the landing impact of the free leg.
  • Non-linear compliance movement amount calculation means to calculate based on the floor reaction force at the foot, z-direction trajectory correction amount calculation means to calculate the vertical trajectory correction amount based on the nonlinear compliance movement amount, and Roll 'pitch trajectory correction amount calculation means for calculating the roll and pitch trajectory correction amount based on the roll and pitch direction torque and the walking pattern output means force
  • the output walking pattern, movement allowance for landing road surface detection, and nonlinear compliance movement By having a walking pattern correction means that corrects the walking pattern based on the vertical trajectory correction amount and the roll 'pitch trajectory correction amount, the nonlinear compliance movement amount is reduced for mitigating landing impact and suppressing vibration.
  • the biped robot device wherein the z-direction trajectory correction amount calculating means is a theoretical compliance movement that should occur when landing ideally on an ideal horizontal flat road surface without impact or vibration.
  • a theoretical compliance movement amount calculating means for calculating the amount based on the control target component, a compliance movement amount deviation calculating means for calculating a landing road surface height error which is a difference between the theoretical compliance movement amount and the nonlinear compliance movement amount, Track correction amount calculating means for calculating the amount of correction of the vertical trajectory based on the control mode selected by the road selection means and the landing road surface height error.
  • the free leg generates a large floor reaction force against the irregular road surface, and the deviation of ZMP and posture angle Mouth bots that follow the shape of the road surface before acceleration occurs and can perform stable walking without falling, making accurate modeling difficult like a human-carrying biped robot
  • the advantage is that walking on rough terrain can be realized even more reliably.
  • the biped walking robot device wherein the roll 'pitch trajectory correction amount calculating means calculates the compliance moving amount based on the roll and pitch direction torque in the foot!
  • the amount calculating means and the trajectory correction amount calculating means for calculating the trajectory correction amount in the roll and pitch directions based on the amount of compliance movement, it is possible to accurately cope with the unevenness of the road surface around the roll pitch axis. be able to Therefore, if the vertical direction is dealt with, the free leg generates a large floor reaction force on the irregular road surface, and before the ZMP, attitude angle deviation or acceleration occurs, the copying operation to the road surface shape is completed.
  • the motor control means performs estimated posture compensation control that corrects based on the posture angle compensation amount of the waist that estimates the walking pattern corrected by the landing control means.
  • Estimated attitude compensation control means is provided.
  • ZMP actual value calculation means to calculate the actual MP value from the force component and moment component at the foot
  • ZMP deviation value calculation means to calculate the ZMP deviation value which is the deviation between the calculated ZMP actual value and the ZMP target value
  • Posture angle compensation amount calculation means for calculating the posture angle compensation amount of the waist based on the integrated value of the ZMP deviation value, and the target value of the foot position or posture in the corrected walking pattern is the posture angle compensation amount. Therefore, it is possible to estimate the accumulated error of the posture angle of the lower back and compensate for it based on the estimated value, and to approximate the lower foot and the free foot side foot.
  • attitude angle compensation amount is calculated based on the integral value of the ZMP deviation value, an advantageous effect is obtained in that the compensation operation with a sudden change can be prevented and the stability is excellent.
  • posture compensation control can be performed only with a force sensor (6-axis force sensor) for ZMP measurement that does not require the mounting of a posture angle sensor, and an advantageous effect of preventing an increase in the number of parts and manufacturing costs can be obtained. .
  • the posture angle compensation amount calculating means includes a zero setting means for setting the integral value of the ZMP deviation value to zero in a predetermined period, and an integral of the ZMP deviation value.
  • An integral value calculation means for calculating an integral value from the time when the value is set to zero, an operation period setting means for setting an operation period for correcting the position or posture of the foot, and a start point of the operation period Compensation amount calculation means for calculating the posture angle compensation amount based on the integral value of the ZMP deviation value at Setting) can prevent an increase in integration error with the actual posture angle deviation, and can obtain an advantageous effect that the posture angle compensation amount can be estimated.
  • a predetermined operation period is set, excessive accumulation of posture angle errors can be prevented, and if stable walking can be performed, an advantageous effect can be obtained.
  • the walking pattern creation method is a walking pattern creation method for creating a walking pattern of a biped robot device including a leg portion and a waist portion including a foot portion.
  • Set the zero moment point calculate the moment compensation trajectory of the waist according to the set target zero moment point, and calculate the waist coordinate system force from the origin of the waist coordinate system in the absolute coordinate system and the origin of the foot coordinate system in the absolute coordinate system
  • Indication By determining and storing the gait flag, which is a flag, the moment at the target zero moment point can be brought close to zero by the moment compensation trajectory of the lumbar part of the biped robot device.
  • the length of the cylinder that expands and contracts the leg can be controlled by inverse kinematic computation that is easy to perform in real time, and various controls can be performed using various correction data in inverse kinematic computation.
  • the biped robot can walk more stably, and it uses a gait flag that indicates whether it is the first or second stance phase and the first or second stance phase. Therefore, a complicated control chart can be executed depending on the state of each leg, such as a copying operation, a return operation, and a trajectory correction amount maintenance. It is possible to perform control according to unevenness and unevenness, and it is possible to obtain an advantageous effect that walking on rough terrain can be realized even when accurate modeling is difficult such as when a human is mounted.
  • the gait pattern creation method is based on a trajectory setting step for setting a foot trajectory and a waist initial trajectory, and a set foot trajectory and a waist initial trajectory.
  • Target ZMP trajectory setting step for setting the trajectory of the target zero moment point
  • target ZMP moment calculating step for calculating the moment around the target zero moment point due to leg movement and waist motion
  • the calculated target zero moment Based on moment compensation trajectory calculation step to calculate the moment compensation trajectory of the waist based on moments around the point, and acceleration of each mass calculated by the set foot trajectory and waist moment compensation trajectory Error moment calculation step to calculate the error moment at the target zero moment point, and when the calculated error moment becomes smaller than the predetermined moment, the origin of the waist coordinate system in the absolute coordinate system and the origin of the footrest system in the absolute coordinate system To calculate the position of the foot, which also saw the waist coordinate system force
  • the posture of the foot coordinate system as seen from the waist coordinate system from the rotation matrix that represents the posture of the foot coordinate system viewed from the absolute coordinate system and the rotation matrix that
  • the Fourier coefficient is calculated based on the sum of the moment around the target zero moment point calculated in the ZMP moment calculation step and the calculated error moment.
  • the moment compensation trajectory calculation step the determined Fourier By calculating the lumbar moment compensation trajectory by the inverse Fourier transform based on the coefficient, the lumbar moment compensation trajectory can be accurately calculated, so that the moment around the target zero moment point can be quickly generated without causing the trajectory to diverge.
  • the beneficial effect is that it can approach zero. It is.
  • the gait pattern creation method includes a target ZMP moment calculation step.
  • the target ZMP moment calculation step by lower limb movement to calculate the target ZMP moment generated by the leg movement and the target ZMP known moment calculation by the waist movement to calculate the target ZMP moment by the waist movement is calculated. Since the moment about the target zero moment point due to the movement of the leg and the movement of the waist can be accurately calculated, the moment compensation trajectory of the waist can be calculated accurately. Effects can be obtained.
  • the foot position / posture calculation step may be performed from the waist coordinate system based on the origin of the waist coordinate system in the absolute coordinate system and the origin of the foot coordinate system in the absolute coordinate system. From the origin position calculation step to calculate the position of the foot, the rotation matrix representing the posture of the foot coordinate system viewed from the absolute coordinate system, and the rotation matrix representing the posture of the waist coordinate system viewed from the absolute coordinate system force.
  • a rotation matrix calculation step that calculates a posture of the foot coordinate system viewed from the coordinate system, and a posture calculation step that calculates the posture of the foot using the rotation matrix calculated in the rotation matrix calculation step.
  • the leg position and posture data can be accurately generated by the control method of the leg walking robot device, the leg position can be expanded and contracted using inverse kinematics, which is easy to calculate in real time. It is possible to accurately control the length of the inverter, and to perform accurate control when performing various controls using various correction data in inverse kinematics calculation! can get.
  • the control method of the biped robot device according to claim 20 includes a leg portion and a waist portion including a foot portion, the leg portion includes a cylinder for extending and contracting the leg portion, and the waist portion controls the whole. And a control method for a biped robot device having a control computer that performs wireless communication with the walking pattern creation device according to any one of claims 1 to 5, wherein the control computer also has the power of the walking pattern creation device.
  • the pattern can be received remotely via radio, and various upper bodies (trunks) can be attached to the waist, it can be used for various purposes (for example, for rescue and nursing). It is possible to build a two-legged walking robot conforms to the dangerous work, etc.) quickly, also, with take the boarding unit such as a chair in the lumbar region If human beings can be boarded, they can be used for various purposes (for example, welfare and transportation).
  • a biped robot can be constructed easily and quickly, and the length of the cylinder that expands and contracts the leg by performing inverse kinematics operation on the foot position and posture data as seen from the waist coordinate system. Because it is possible to control the robot, the parallel link mechanism does not need to perform forward kinematic calculations, which are difficult to perform in real time, and the parallel link mechanism controls robot walking with inverse kinematic calculations that are easy to perform in real time. Easy implementation of performing force inverse kinematic calculation by adding various correction data (for example, compliance movement amount and waist position / posture operation amount) in the foot position and posture dimensions. It is possible to obtain an advantageous effect that the walking of the biped robot can be further stabilized.
  • various correction data for example, compliance movement amount and waist position / posture operation amount
  • the control computer receives a walking pattern creation computer power walking pattern by radio and stores the walking pattern in a memory.
  • Walking pattern force A walking start command step for selecting a pattern to be used for walking and instructing the start of walking, and a motor control step for controlling the motor based on the command in the walking start command step to control the length of the cylinder
  • the biped robot device can walk with high stability without being physically restricted by the computer force for creating the walking pattern in the walking state. The advantage of being able to walk with an appropriate walking pattern according to the light weight state) is obtained.
  • the motor control step includes a basic control step for performing basic control, a swinging landing of the free leg on the road surface while reducing the impact, and By having a landing control step that imitates the foot in unevenness, it is possible to perform basic operations based on basic control and to perform landing control according to the situation.
  • the leg mechanism of the mechanism it is not necessary to perform forward kinematics, which is difficult to implement due to the enormous amount of calculation, and the link length can be controlled in real time by inverse kinematics that is easy to calculate, It is possible to perform various controls on the foot position 'posture, and in landing control, the free leg generates a large floor reaction force before the ZMP, posture angle deviation and acceleration are generated. form Human body-type 2 which can complete stable copying without falling over an irregular road surface. An advantageous effect is obtained in that walking on rough terrain of a robot that is difficult to accurately model, such as a legged robot, can be realized.
  • the basic control step is a command in the walking start command step for the target value of the foot position and posture calculated by the foot position and posture calculating means.
  • Gait pattern output step output based on the gait pattern, and gait pattern output step force output foot position 'Inverse kinematics calculation step to calculate the length of the cylinder by inverse kinematics calculation of the target value of posture
  • the landing control step selects a control mode such as control, maintenance, return, compliance, etc. based on the gait flag, and a walking
  • the component division step that divides the control target component that is the target of landing control and the non-control target component other than the control target component, and the non-linear compliance movement that reduces the landing impact of the free leg
  • a non-linear compliance movement amount calculation step that calculates the amount based on the floor reaction force at the foot, a z-direction trajectory correction amount calculation step that calculates the vertical orbit correction amount based on the non-linear compliance movement amount, Roll and pitch trajectory correction amount calculation to calculate roll and pitch trajectory correction amount based on roll and pitch torque at foot
  • walking pattern output in the walking pattern output step landing road surface detection allowance, nonlinear compliance movement amount, vertical trajectory correction amount, and roll 'pitch trajectory correction amount!
  • the z-direction trajectory correction amount calculating step should occur when landing ideally on an ideal horizontal flat road surface without impact or vibration.
  • the motor control step includes: The estimated posture compensation control step performs the estimated posture compensation control to correct based on the posture angle compensation amount of the lumbar region estimated from the corrected walking pattern in the ground control step.
  • ZMP actual value calculation step that calculates the force component and moment component at the foot
  • ZMP deviation value calculation step that calculates the ZMP deviation value that is the difference between the calculated ZMP actual value and the ZMP target value
  • the posture angle compensation amount calculation step calculates the posture angle compensation amount of the waist based on the integrated deviation value, and the target position or posture value of the foot in the corrected walking pattern is based on the posture angle compensation amount.
  • attitude angle compensation amount is calculated based on the integral value of the ZMP deviation value, it is possible to prevent a compensation operation accompanied by a sudden change, and an excellent effect can be obtained if the stability is excellent.
  • posture compensation control can be performed only with a force sensor (6-axis force sensor) for ZMP measurement that does not require the mounting of a posture angle sensor, and an increase in the number of parts and manufacturing costs can be prevented. Is obtained.
  • the posture angle compensation amount calculating step includes: a zero setting step of setting an integral value of the ZMP deviation value to zero at a predetermined period; An integration value calculating step for calculating an integral value from the time when the integral value of the deviation value is set to zero, an operation period setting step for setting an operation period for correcting the position or posture of the foot, and an operation period A compensation amount calculation step for calculating a posture angle compensation amount based on the integral value of the ZMP deviation value at the start of the operation, so that the integral value of the ZMP deviation value is set to zero (reset) at a predetermined cycle.
  • the program according to claim 29 is stored in any one of claims 15 to 19 on a computer.
  • the program for executing the steps of the described walking pattern creation method executes the walking pattern creation method according to any one of claims 15 to 19 on a general-purpose computer at an arbitrary location and at an arbitrary time. An advantageous effect that it can be achieved is obtained.
  • the recording medium according to claim 30 is a computer-readable recording medium in which the program according to claim 29 is recorded, the recording medium can be read only by reading the recording medium with a general-purpose computer.
  • the general-purpose computer can execute the walking pattern creation method according to any one of 15 to 19 at an arbitrary place and at an arbitrary time.
  • the program according to claim 31 is a program for causing a computer to execute each step of the control method of the biped robot device according to any one of claims 20 to 28.
  • the recording medium according to claim 32 is a computer-readable recording medium in which the program according to claim 31 is recorded, and as long as the recording medium is read by a general-purpose computer, An advantageous effect is obtained that the control method for the biped robot device according to any one of 20 to 28 can be executed by a general-purpose computer at an arbitrary place and at an arbitrary time.
  • FIG. 2 (a) Schematic diagram showing the hip coordinate system and foot coordinate system, (b) Schematic diagram showing the hip coordinate system and foot coordinate system
  • FIG. 4 is a block diagram showing the overall configuration of a walking pattern creation device and a biped walking robot device according to Embodiment 1 of the present invention.
  • FIG.5 Block diagram mainly showing the waist
  • FIG.10 Block diagram mainly showing control computer
  • FIG.12 Block diagram mainly showing switch circuit
  • FIG. 13 Block diagram mainly showing the brake control unit
  • FIG. 14 is a block diagram mainly showing the motor control unit.
  • FIG.15 Block diagram mainly showing cylinders
  • FIG.17 Block diagram mainly showing the linear motion part
  • FIG. 18 Block diagram showing the means for realizing functions in the CPU of the walking pattern creation computer
  • FIG. 19 is a flowchart showing the CPU operation of the walking pattern creation computer.
  • FIG. 20 (a) Flow chart showing CPU operation of walking pattern creation computer, (b) Flow chart showing CPU operation of walking pattern creation computer, (c) Flow chart showing CPU operation of walking pattern creation computer.
  • FIG. 21 is a flowchart showing the CPU operation of the walking pattern creation computer.
  • FIG. 22 is a flowchart showing the CPU operation of the walking pattern creation computer.
  • FIG. 23 is a flowchart showing the CPU operation of the walking pattern creation computer.
  • FIG. 24 is a flowchart showing the CPU operation of the walking pattern creation computer.
  • FIG. 25 is a flowchart showing the CPU operation of the walking pattern creation computer.
  • FIG. 26 is a flowchart showing the CPU operation of the walking pattern creation computer.
  • FIG. 27 is a flowchart showing the CPU operation of the walking pattern creation computer.
  • FIG. 28 is a flowchart showing the CPU operation of the walking pattern creation computer.
  • FIG. 29 is a flowchart showing the CPU operation of the walking pattern creation computer.
  • FIG. 30 is a flowchart showing the CPU operation of the walking pattern creation computer.
  • FIG.31 Flow chart showing the CPU operation of the walking pattern creation computer ⁇ 32] Explanatory diagram explaining the calculation of the origin position, rotation matrix, and posture angle of the foot coordinate system
  • FIG. 33 is a block diagram showing the means for realizing functions in the CPU of the control computer.
  • FIG. 34 is a flowchart showing the CPU operation of the control computer.
  • FIG. 35 is a flowchart showing the CPU operation of the control computer.
  • FIG. 36 is a flowchart showing the CPU operation of the control computer.
  • FIG. 37 Functional block diagram showing motor control means
  • FIG. 38 is a block diagram showing motor control means and a motor drive device.
  • FIG. 41 (a) Block explanatory diagram showing each block, (b) Block explanatory diagram showing each block, (c) Block explanatory diagram showing each block
  • FIG. 42 (a) Block explanatory diagram showing each block, (b) Block explanatory diagram showing each block. [FIG. 43] Flow chart showing control in which landing control is added to basic control.
  • FIG. 44 is a flowchart showing landing control.
  • FIG. 45 (a) Functional block diagram showing motor control means of the biped robot device in Embodiment 2 (b) Functional block diagram showing posture angle compensation amount calculation means
  • FIG. 50 (a) Schematic perspective view of the lower body module of the biped robot in the first embodiment, (b) Schematic plan view of the lower body module of the biped robot in the first embodiment.
  • FIG.52 Perspective view of main part of linear motion link
  • FIG.53 (a) Side view of main part of linear link, (b) Cross section of main part taken along line A—A in (a)
  • FIG.54 Perspective view of main part of base side passive joint
  • the walking pattern creation device is a walking pattern creation device for creating a walking pattern of a biped walking robot device including a leg portion and a waist portion provided with a foot portion, and a target at the foot portion.
  • Set the zero moment point calculate the moment compensation trajectory of the waist according to the set target zero moment point, and from the waist coordinate system from the origin of the waist coordinate system in the absolute coordinate system and the origin of the foot coordinate system in the absolute coordinate system Viewed foot position Of the foot coordinate system viewed from the waist coordinate system from the rotation matrix representing the posture of the foot coordinate system viewed from the absolute coordinate system and the rotation matrix representing the posture of the waist coordinate system viewed from the absolute coordinate system.
  • a gait flag which is a flag indicating the early or late stance and the early or late stance of the walking pattern in the walking pattern, by calculating a rotation matrix that represents the posture and calculating the foot posture using the rotation matrix that represents the posture of the foot coordinate system. Is determined and memorized.
  • This configuration has the following effects.
  • the gait flag which is the flag indicating whether the first stance or late force and the first or second swing leg, can be controlled according to the state of the leg
  • the following movement, return movement, orbit correction Complex control charts can be executed depending on the state of each leg, such as maintaining the amount, and this causes the free leg to generate a large floor reaction force and to follow the road surface shape before ZMP, attitude angle deviation or acceleration occurs
  • road surface conditions such as inclined surfaces and unevenness
  • walking on rough terrain is realized even when accurate modeling is difficult, such as when a human is mounted can do.
  • the walking pattern creation device is the walking pattern creation device according to claim 1, wherein the trajectory setting means for setting the trajectory of the foot and the initial trajectory of the waist, the trajectory of the set foot trajectory and the waist Target ZMP trajectory setting means to set the trajectory of the target zero moment point based on the initial trajectory, target ZMP moment calculating means to calculate the moment around the target zero moment point due to leg motion and waist motion, and calculation Based on the moment around the target zero moment point
  • the calculated error moment becomes smaller than the predetermined moment, calculate the position of the foot as seen from the waist coordinate system force from the origin of the waist coordinate system in the absolute coordinate system and the origin of the foot coordinate system in the absolute coordinate system.
  • a foot position / orientation calculating means for calculating a posture of the foot by a rotation matrix representing the posture of the foot coordinate system by calculating a rotation matrix representing the foot coordinate system, and determining and storing a gait flag In which it was decided to have the contents flag decision means.
  • the error moment can be used to determine whether the moment around the target zero moment point due to leg movement and waist movement is zero, and the moment around the target zero moment point can be determined when setting the walking pattern. Since it can approach zero as much as possible, the biped walking robot device can perform biped walking with extremely high stability.
  • the walking pattern creation device is the walking pattern creation device according to claim 2, wherein the moment around the target zero moment point calculated by the ZMP rotation moment calculation means and the calculated error moment are calculated.
  • a Fourier transform means for calculating a Fourier coefficient based on the sum, and a Fourier coefficient determination means for determining a Fourier coefficient for obtaining an approximate solution of the moment compensation trajectory based on the calculated Fourier coefficient, and calculating a moment compensation trajectory
  • the means is to calculate the moment compensation trajectory of the waist by inverse Fourier transform based on the determined Fourier coefficient.
  • the walking pattern creation device is the walking pattern creation device according to claim 2 or 3, wherein the target ZMP rotation moment calculating means is generated by the movement of the leg.
  • Target ZMP around moment calculation means by lower limb movement to calculate the target ZMP moment, and target ZMP around moment calculation means to calculate the target ZMP around moment generated by the waist movement It is.
  • the walking pattern creation device is the walking pattern creation device according to any one of claims 2 to 4, wherein the foot position / posture calculation means is a waist coordinate system in an absolute coordinate system. Origin position calculating means for calculating the position of the foot as seen from the waist coordinate system from the origin of the foot coordinate system in the absolute coordinate system and a rotation matrix representing the posture of the foot coordinate system as seen from the absolute coordinate system And a rotation matrix calculation means for calculating a rotation matrix representing the posture of the foot coordinate system as seen from the waist coordinate system, and a rotation matrix calculation means. It has posture calculation means for calculating the posture of the foot from the rotation matrix.
  • the bipedal walking robot device includes a leg portion and a waist portion including a foot portion, the leg portion includes a cylinder for extending and contracting the leg portion, and the waist portion controls the whole and is charged.
  • a biped robot apparatus having a control computer that wirelessly communicates with the walking pattern creation device according to any one of items 1 to 5, V, and deviation 1. The control computer uses the walking pattern received from the walking pattern creation device. Based on this, biped walking is controlled.
  • This configuration has the following effects.
  • the lumbar moment compensation trajectory can stabilize the walking by bringing the moment at the target zero moment point of the foot closer to zero.
  • Walking patterns can be received at a remote location via radio.
  • the biped robot device is the biped robot device according to claim 6, wherein the control computer wirelessly receives the walking pattern creation computer force walking pattern and stores it in the memory.
  • Walking pattern setting means stored walking pattern force
  • walking start command means for selecting a pattern to be used for walking and commanding the start of walking
  • controlling the motor based on the command of the walking start command means to control the length of the cylinder
  • Motor control means for controlling the motor.
  • the biped robot device can perform highly stable walking without being physically restricted by the computer for creating the walking pattern, and it can also be in the surrounding or self-state (for example, light weight state of the mounted weight). It is possible to walk with an appropriate walking pattern according to).
  • the biped walking robot device is the biped walking robot device according to claim 7, wherein the motor control means includes: basic control means for performing basic control; and impact on the road surface with the free leg. It has a landing control means for mitigating and landing and imitating the foot to the unevenness of the road surface That's what it meant.
  • the biped walking robot apparatus is the biped walking robot apparatus according to claim 8, wherein the basic control means is configured to calculate the position of the foot portion 'posture' calculated by the foot position / posture calculating means.
  • a walking pattern output means that outputs a target value based on a command from the walking start command means, and a walking pattern output means force. Calculates the length of the cylinder by performing inverse kinematics calculation of the target position and posture values of the output foot. And having an inverse kinematics computing means.
  • the biped walking robot device is the biped walking mouth bot device according to claim 8 or 9, wherein the landing control means controls, maintains, restores, and coordinates based on the gait flag.
  • Component selection that divides the control target component that is the target of landing control and the non-control target component other than the control target component in the walking pattern component
  • non-linear compliance that calculates the amount of non-linear compliance movement that reduces the landing impact of the free leg based on the floor reaction force at the foot
  • the z-direction trajectory correction amount calculation means for calculating the trajectory correction amount in the vertical direction based on the nonlinear compliance movement amount, and the roll and pitch based on the torque in the roll and pitch directions at the foot!
  • Roll / pitch trajectory correction amount calculation means walking pattern output means, walking pattern and landing road surface detection allowance, nonlinear compliance movement amount, vertical trajectory correction amount and roll 'The walking pattern correction means for correcting the walking pattern based on the pitch trajectory correction amount is provided.
  • Mitigation of landing impact and suppression of vibration can be handled by using non-linear compliance movement, and for road surface irregularities and inclined road surfaces, trajectory correction amount in the vertical direction and roll and pitch trajectories. This can be dealt with using the correction amount.
  • the biped walking robot apparatus is the biped walking robot apparatus according to claim 10, wherein the z-direction trajectory correction amount calculating means is ideally ideal on an ideal horizontal flat road surface without impact or vibration.
  • Theoretical compliance movement amount calculation means that calculates the theoretical compliance movement amount that should occur when landing, based on the control target component, and the landing road surface height error that is the difference between the theoretical compliance movement amount and the nonlinear compliance movement amount Compliance movement amount deviation calculating means for calculating the above and a trajectory correction amount calculating means for calculating the trajectory correction amount in the vertical direction based on the control mode selected by the mode selection means and the landing road surface height error. It is what.
  • the biped walking robot device is the biped walking robot device according to claim 10, wherein the roll 'pitch trajectory correction amount calculating means includes torque in a roll and a pitch direction at the foot. Based on the compliance movement amount calculation means for calculating the compliance movement amount and the trajectory correction amount calculation means for calculating the trajectory correction amount in the roll and pitch directions based on the compliance movement amount. It is.
  • the biped robot device is the biped robot device according to any one of claims 8 to 12, wherein the motor control means estimates the walking pattern corrected by the landing control means.
  • Estimated posture compensation control means that performs estimated posture compensation control that corrects based on the posture angle compensation amount of the waist, and the estimated posture compensation control means calculates the measured ZMP value from the force component and moment component at the foot.
  • ZMP actual value calculation means ZMP deviation value calculation means for calculating the ZMP deviation value that is the deviation between the calculated ZMP actual value and the ZMP target value, and posture angle compensation of the waist based on the integrated value of the ZMP deviation value
  • Posture angle compensation amount calculating means for calculating the amount
  • foot position / posture correction means for correcting the target position or posture value of the foot in the corrected walking pattern based on the posture angle compensation amount! With what That.
  • attitude angle compensation amount is calculated based on the integral value of the ZMP deviation value, it is possible to prevent a compensation operation with a sudden change and to have excellent stability.
  • a sudden change in posture angle can be prevented by landing control, walking control can be performed with higher reliability by combining this with estimated posture compensation control.
  • the posture angle compensation amount calculating unit sets the integral value of the ZMP deviation value to zero in a predetermined cycle.
  • the operation period setting means and the compensation amount calculation means for calculating the attitude angle compensation amount based on the integrated value of the ZMP deviation value at the start time of the operation period are provided.
  • the walking pattern creation method according to claim 15 includes a leg portion having a foot portion and a waist portion.
  • the waist as seen from the absolute coordinate system Calculate the rotation matrix that represents the posture of the foot coordinate system as seen from the waist coordinate system from the rotation matrix that represents the posture of the coordinate system, and calculate the posture of the foot using the rotation matrix that represents the posture of the foot coordinate system.
  • the gait flag which is a flag indicating whether the first stage of stance or late stage force and the first stage or late stage of swing leg, is determined and stored.
  • This configuration has the following effects.
  • the gait flag which is the flag indicating whether the first stance or late force and the first or second swing leg, can be controlled according to the state of the leg
  • Complex control charts can be executed according to the state of each leg, such as maintaining the amount, and it is possible to perform control according to road surface conditions such as inclined surfaces and unevenness, making accurate modeling difficult when mounting humans In any case, walking on rough terrain can be realized.
  • the gait pattern creation method is the gait pattern creation method according to claim 15, wherein the trajectory setting step for setting the trajectory of the foot and the initial trajectory of the waist, and the trajectory of the set foot A target ZMP trajectory setting step that sets the trajectory of the target zero moment point based on the initial trajectory of the lumbar region, and a target ZMP moment calculation step that calculates the moment around the target zero moment point due to the leg motion and the lumbar motion And a moment compensation trajectory calculation step for calculating a lumbar moment compensation trajectory based on the calculated moment around the target zero moment point, and each mass point calculated by the set foot trajectory and the lumbar moment compensation trajectory.
  • Error mode that calculates the error moment at the target zero moment point based on the acceleration of Calculation step, and when the calculated error moment becomes smaller than the predetermined moment, Calculate the position of the foot as seen from the waist coordinate system from the origin of the waist coordinate system and the origin of the foot coordinate system in the absolute coordinate system, as well as the rotation matrix and absolute representing the posture of the foot coordinate system as seen from the absolute coordinate system
  • the rotation matrix representing the posture of the foot coordinate system as seen from the waist coordinate system is calculated from the rotation matrix representing the posture of the waist coordinate system as seen from the coordinate system and the rotation matrix representing the posture of the foot coordinate system is calculated.
  • the foot position / posture calculation step for calculating the posture and the gait flag determination step for determining and storing the gait flag are provided.
  • the error moment can be used to determine whether the moment around the target zero moment point due to leg movement and waist movement is zero, and the moment around the target zero moment point can be determined when setting the walking pattern. Since it can approach zero as much as possible, the biped walking robot device can perform biped walking with extremely high stability.
  • the walking pattern creation method according to claim 17 is the walking pattern creation method according to claim 16, wherein the moment around the target zero moment point calculated in the ZMP rotation moment calculation step and the calculated error moment are calculated.
  • the moment compensation trajectory of the lumbar region is calculated by inverse Fourier transform based on the determined Fourier coefficient.
  • the gait pattern creation method according to claim 18 is the gait pattern creation method according to claim 16 or 17, wherein the target ZMP moment calculation step calculates a target ZMP moment generated by the movement of the leg. Step for calculating the moment around the target ZMP by the lower limb movement, and the waist movement to calculate the moment around the target ZMP caused by the movement of the lower back And a step of calculating a known moment around the target ZMP by the movement.
  • the target ZMP moment calculation step calculates a target ZMP moment generated by the movement of the leg.
  • a step of calculating a known moment around the target ZMP by the movement is obtained.
  • the gait pattern creation method is the gait pattern creation method according to any one of claims 16 to 18, wherein the foot position / posture calculation step is based on the waist coordinate system in the absolute coordinate system.
  • An origin position calculation step that calculates the position of the foot viewed from the waist coordinate system from the origin and the origin of the foot coordinate system in the absolute coordinate system, and a rotation matrix that represents the posture of the foot coordinate system viewed from the absolute coordinate system and the absolute
  • a rotation matrix that calculates a rotation matrix that represents the posture of the foot coordinate system viewed from the waist coordinate system from a rotation matrix that represents the posture of the waist coordinate system viewed from the coordinate system, and a rotation matrix calculated in the rotation matrix calculation step
  • a posture calculation step for calculating the posture of the foot.
  • the control method of the biped robot device according to claim 20 includes a leg portion and a waist portion including a foot portion, the leg portion includes a cylinder for extending and contracting the leg portion, and the waist portion controls the whole. And a control method for a biped robot device having a control computer that performs wireless communication with the walking pattern creation device according to any one of claims 1 to 5, wherein the control computer also has the power of the walking pattern creation device. Biped walking is controlled based on the received walking pattern.
  • This configuration has the following effects.
  • Walking patterns can be received at a remote location via radio.
  • the control method of the biped robot device according to claim 21 is the control method of the biped robot device according to claim 20, wherein the control computer is configured to generate a walking pattern by the computer for creating the walking pattern.
  • the walking pattern setting step that is received wirelessly and stored in the memory, the stored walking pattern force
  • the walking start command step that selects the pattern to be used for walking and commands the start of walking, and the commands in the walking start command step Therefore, the motor control step for controlling the motor to control the length of the cylinder is provided.
  • the biped robot device can perform highly stable walking without being physically restricted by the computer for creating the walking pattern, and it can also be in the surrounding or self-state (for example, light weight state of the mounted weight). It is possible to walk with an appropriate walking pattern according to).
  • a control method for a biped robot device is a bipedal walking according to claim 21.
  • the motor control step includes a basic control step for performing basic control, and a landing control step for causing the free leg to land on the road surface while mitigating the impact and imitating the foot on the road surface unevenness. It is supposed to have.
  • the control method of the biped robot device according to claim 23 is the control method of the biped robot device according to claim 22, wherein the basic control step is calculated by the foot position / posture calculation means.
  • the walking pattern output step that outputs the target value of the foot position / posture based on the command in the walking start command step, and the target value of the foot position 'posture output from the walking pattern output step is calculated by inverse kinematics calculation.
  • the control method for the biped robot device according to claim 24 is the control method for the biped robot device according to claim 22 or 23, wherein the landing control step is controlled based on a gait flag.
  • Mode selection to select control mode such as maintenance, return, compliance
  • the component division step that divides the control target component, which is the component subject to landing control, and the non-control target component other than the control target component into the step and walking pattern components, and mitigates landing leg impact
  • Roll and pitch trajectory correction amount calculation step for calculating the trajectory correction amount of roll and pitch based on the calculation step and the roll and pitch torque at the foot, and the walking pattern output in the walking pattern output step
  • movement tolerance for detecting the road surface, nonlinear compliance movement, vertical trajectory correction, and roll pitch It and was it even to a and a walking pattern correction step for correcting the gait pattern based
  • Mitigation of landing impact and suppression of vibration can be handled by using non-linear compliance movement, and for road surface irregularities and inclined road surfaces, trajectory correction amount in the vertical direction and roll and pitch trajectories. This can be dealt with using the correction amount.
  • Landing control can be performed using only a force sensor for ZMP measurement without the need for special sensors and mechanisms including gyroscopes, and an increase in the number of parts and manufacturing costs can be prevented.
  • the biped walking robot apparatus control method is the biped walking robot apparatus control method according to claim 24, wherein the z-direction trajectory correction amount calculating step applies an impact to an ideal horizontal flat road surface.
  • the theoretical compliance movement amount calculation step that calculates the theoretical compliance movement amount that should occur when landing ideally without vibration or vibration based on the control target component, and the difference between the theoretical compliance movement amount and the nonlinear compliance movement amount Based on the compliance movement amount deviation calculation step for calculating a certain landing road surface height error, the control mode selected in the mode selection step, and the landing road surface height error And a trajectory correction amount calculating step for calculating the trajectory correction amount.
  • the control method for the biped robot device according to claim 26 is the control method for the biped robot device according to claim 24, wherein the roll 'pitch trajectory correction amount calculating means is provided at the foot.
  • a compliance movement amount calculating means for calculating a compliance movement amount based on the torque in the roll and pitch directions; and a trajectory correction amount calculating means for calculating a trajectory correction amount in the roll and pitch directions based on the compliance movement amount. It is a matter of course.
  • the control method of the biped robot device according to claim 27 is the same as the control method of the biped robot device according to any one of claims 22 to 26, and the motor control step is The estimated posture compensation control step performs estimated posture compensation control based on the posture angle compensation amount of the waist that has been estimated based on the corrected walking pattern in the landing control step.
  • ZMP actual value calculation step that calculates the value from the force component and moment component at the foot
  • ZMP deviation value calculation step that calculates the ZMP deviation value that is the deviation between the calculated ZMP actual value and the ZMP target value
  • Integrate deviation value A posture angle compensation amount calculating step for calculating a posture angle compensation amount of the waist based on the calculated value, and a foot portion for correcting the target position or posture value of the foot in the corrected walking pattern based on the posture angle compensation amount
  • a posture operation amount adding step for calculating a posture angle compensation amount of the waist based on the calculated value
  • a foot portion for correcting the target position or posture value of the foot in the corrected walking pattern based on the posture angle compensation amount
  • attitude angle compensation amount is calculated based on the integral value of the ZMP deviation value, it is possible to prevent a compensation operation with a sudden change and to have excellent stability.
  • a sudden change in posture angle can be prevented by landing control, walking control can be performed with higher reliability by combining this with estimated posture compensation control.
  • the control method of the biped robot device according to claim 28 is the control method of the biped robot device according to claim 27, wherein the posture angle compensation amount calculating step includes a ZMP deviation at a predetermined cycle.
  • Zero setting step to set the integral value of the value to zero
  • integration value calculation step to calculate the integral value from the time when the integral value of the ZMP deviation value was set to zero
  • the correction operation of the foot position or posture An operation period setting step for setting an operation period for performing an operation period, and a compensation amount calculation step for calculating an attitude angle compensation amount based on an integrated value of the ZMP deviation value at the start time of the operation period.
  • a program according to claim 29 is stored in any one of claims 15 to 19 on a computer.
  • the program is to execute each step of the described walking pattern creation method.
  • This configuration has the following effects.
  • the general-purpose computer can execute the walking pattern creation method according to any one of claims 15 to 19 at an arbitrary place and an arbitrary time.
  • the recording medium according to claim 30 is a computer-readable recording medium in which the program according to claim 29 is recorded.
  • This configuration has the following effects.
  • the general-purpose computer can execute the walking pattern creation method according to any one of claims 15 to 19 at any place and at any time.
  • the program according to claim 31 is a program for causing a computer to execute each step of the control method of the biped robot device according to any one of claims 20 to 28. .
  • This configuration has the following effects.
  • the method for controlling the biped robot device according to any one of claims 20 to 28 can be executed by a general-purpose computer at an arbitrary place and at an arbitrary time.
  • the recording medium according to claim 32 is a computer-readable recording medium in which the program according to claim 31 is recorded.
  • This configuration has the following effects.
  • FIG. 50 (a) is a schematic perspective view of the lower body module of the biped robot device according to the present embodiment
  • FIG. 50 (b) is a schematic plan view of the lower body module of the biped robot device according to the present embodiment.
  • 201 is the lower body module of the biped robot device in this embodiment
  • 201a is the parallel link mechanism of the right leg
  • 201b is the parallel link mechanism of the left leg
  • 202 is the base (lumbar).
  • 203 is the right foot
  • 204 is the left foot.
  • the base portion side passive joints 206a and 206a ′ are disposed on the right rear portion side of the lower surface of the base portion 202.
  • the foot-side passive joint 207a is disposed on the right rear side of the right foot 203, and a rotating passive joint 208a is disposed below the foot-side passive joint 207a.
  • the linear motion link 205a has an upper end portion connected to the base portion side passive joint 206a and a lower end portion connected to the foot portion side passive joint 206a.
  • the linear motion link 205a ′ has an upper end connected to the base-side passive joint 206a ′ and a lower end connected to the foot-side passive joint 207a.
  • the foot-side passive joint 207b is disposed on the right front side of the right foot 203, and a rotating passive joint 208b is disposed below the foot-side passive joint 207b.
  • the linear motion link 205b has an upper end connected to the base side passive joint 206b and a lower end connected to the foot side passive joint 207b. It is connected.
  • the linear motion link 205b ′ has an upper end connected to the base-side passive joint 2061 / and a lower end connected to the foot-side passive joint 207b.
  • the foot side passive joint 207c is disposed at the right center portion of the right foot portion 203, and a rotating passive joint 208c is disposed at the lower portion thereof.
  • the linear motion link 205c has an upper end connected to the base-side passive joint 206c and a lower end connected to the foot-side passive joint 207c.
  • the linear motion link 205c ′ has an upper end connected to the base-side passive joint 206 and a lower end connected to the foot-side passive joint 207c.
  • the parallel link mechanism 201a on the right leg and the parallel link mechanism 201b on the left leg are arranged symmetrically on both sides with respect to the center of the base 202.
  • linear motion links 205, 205a, 205a ', 205b, 205b', 205c, 205c ' (hereinafter referred to as linear motion links 205, 205a to 205c') include a feed screw mechanism using a motor, It is formed to be extendable and retractable using a direct acting type actuator such as a cylinder using hydraulic pressure, water pressure, air pressure, etc., and is formed with one degree of freedom to expand and contract in the longitudinal direction.
  • Base part side passive join ⁇ 206, 206a, 206a ', 206b, 206b', 206c, 206c ' are linear motion links 205, 205a- It is formed with two degrees of freedom to rotate in the axial direction of two axes orthogonal to the longitudinal direction of 205c ′ and orthogonal to each other.
  • the foot side passive join rods 207, 207a, 207b, 207c are orthogonal to the longitudinal direction of the linear motion links 205, 205a-205c ', and rotate in the axial direction of two orthogonal axes, with two degrees of freedom. It is formed.
  • the rotary passive join rods 208, 208a, 208b, 208c are formed with one degree of freedom to rotate in the axial direction of the linear motion links 205, 205a to 205c ′.
  • the right leg portion and the left leg parallel link mechanism portions 201a and 201b are each formed with six degrees of freedom, the right foot portion 203 and the left foot portion 204 are front and rear, left and right, up and down, and Operation in the axial direction around the front-rear direction, left-right direction, and up-down direction is possible, so that various operations can be performed, and walking can be performed smoothly.
  • the inverse kinematics of the right foot 203 is calculated based on a preset walking pattern, and the linear motion links 205a to 205c are not shown based on the calculated values.
  • the actuator is driven to expand and contract the linear motion links 205a to 205c ′. It is arranged at the connecting part between the linear link 205a ⁇ 205c 'and the base part 202 or the right foot part 203!
  • FIG. 51 is a perspective view of the lower body module of the biped robot device according to the present embodiment.
  • 201c is a lower body module of the biped walking robot apparatus in the present embodiment
  • 202 is a base part
  • 203 is a right foot part
  • 203a is a flat plate fixed to the upper part of the right foot part
  • 204 is 204a is a flat plate fixed to the upper part of the left foot 204
  • 209 is a control unit disposed on the upper surface of the base 202
  • 231 is a battery
  • 232 is a motor drive circuit part
  • 235 is a linear motion link erected between the base part 202 and the right foot part 203 and the left foot part 204
  • 236 is a lower side of the base part 202.
  • a base-side passive joint fixed at a predetermined position 237 is a foot-side passive joint fixed at a predetermined position on the upper surface side of the fixing plate 203a, 204a of the right foot 203 and the left foot 204, and 248 is This is a DC servo motor for extending and retracting the linear link 235.
  • the foot-side passive joint 237 is a ball joint that has the same functions as the foot-side passive join rods 207, 207a, 207b, 207c and the rotating passive join rods 208, 208a, 208b, 208c described in Fig. 50. is there. This ball joint will be described later.
  • the linear link 235 is formed to be extendable and contractable in the longitudinal direction by a feed screw mechanism described later.
  • a mechanism using a cylinder using a hydraulic pressure, water pressure, air pressure or the like or a direct acting type actuator may be used.
  • the linear motion link 235 is arranged in a V shape in which the two upper ends are connected to the base side passive joint 236 and the lower end force is connected to the two foot side passive joints 237. ing .
  • One set of linear motion links 235 is arranged in a triangular shape in plan view, with three sets on each of the right and left legs, and six on one leg, for a total of twelve.
  • the left and right legs of the lower body module 201c of the biped walking robot device in this embodiment are each a steer platform (a kind of parallel mechanism, and both ends of six linear cylinders are connected to a ball joint. It has a structure that is connected to the base and end-effector by a universal joint. As a result, the stability and strength of operation, rigidity, and output are excellent.
  • the upper end of one linear link 235 is connected to one base side passive joint 236, and the lower ends of two linear links 215 are connected to one foot side passive joint 237. .
  • the notch 231 and the motor drive circuit unit 232 are disposed in the control unit 209.
  • a nickel metal hydride battery is used as the battery 211.
  • six-axis force sensors 203b and 204b (142 in FIG. 40) for detecting a floor reaction force are provided on the bottom surfaces of the right foot 203 and the left foot 204.
  • the 6-axis force sensors 203b and 204b can detect the three force components in each axis direction and the three moment components around each axis simultaneously and successively with high accuracy.
  • virtual compliance control, landing trajectory correction control (described later), and estimated attitude compensation control (Embodiment 2) are performed based on the values detected by the six-axis force sensors 203b and 204b.
  • FIG. 52 is a perspective view of the main part of the linear link
  • FIG. 53 (a) is a side view of the main part of the linear link
  • FIG. 53 (b) is a partial internal structure diagram of FIG. 53 (a).
  • reference numeral 235 denotes a linear motion link
  • 241 denotes a holding casing that holds an inner rod portion 243 and the like to be described later
  • 243 denotes an inner member that is provided on the holding casing 241 and slides in the longitudinal direction of the linear motion link 235. It is a rod part.
  • the expansion / contraction of the linear link 235 is the inner rod part 243 This is done by sliding.
  • 244 is a joint coupling portion formed in the longitudinal direction of the inner rod portion 243
  • 245 is a rod rail portion arranged on the upper side and the lower side of one side portion of the inner rod portion 243 and laid in the longitudinal direction. It is.
  • 246 is a rail guide
  • 247 is an initial position sensor for detecting an initial position of the inner rod portion 243
  • 248 is a motor arranged in parallel with the holding casing 241
  • 248a is a motor 248.
  • a holding brake for holding the part 243 a rotary encoder 251 for detecting the turning of the turning shaft of the motor 248, and a reference numeral 252 for transmitting the rotational power of the motor leading end shaft part 249 to a male screw leading end shaft part 254a described later.
  • a grooved belt, 253 is a bearing that supports the male screw shaft portion 254, 254 is a male screw shaft portion that is inserted through a female screw nut portion 255 (described later) inside the holding casing 241 and is threaded on the outer periphery, 254a is male screw The child tip shaft portion 255 is fixed to the inner rod portion 243 and is a female screw nut portion screwed into the male screw shaft portion 254, and 256a and 256b are stoppers. Since the motor 248 is arranged in parallel to the holding casing 241 in this way, the stroke can be increased without increasing the shortest length (the shortest, the length of time) of the linear motion link 235, and the robot The movable range of the device can be widened.
  • lightweight and relatively high strength aluminum is used as the material of the holding casing 241.
  • a photomicrosensor was used as the initial position sensor 247 for confirming the initial position.
  • a ball screw is used to screw the male screw shaft portion 254 and the female screw nut portion 255 together.
  • the inner rod part 243 can be slid at high speed, and the walking speed can be improved.
  • the frictional resistance of the screwed portion can be reduced. Further, the ball screw can be surely operated with little play.
  • FIG. 54 is a perspective view of the main part of the base side passive joint.
  • 202 is a base part
  • 235 is a linear motion link
  • 236 is a passive joint on the base part side
  • 241 is a holding casing for the linear motion link 235
  • 261 is a U-shape fixed to the lower part of the base part 202.
  • Upper joint on the base side, 262 is the upper joint shaft installed on a pair of standing parts of the base side upper joint 261
  • 263 is a U-shape fixed to the upper end of the holding casing 241 side of the linear motion link 235
  • 264 is a lower joint shaft installed on a pair of upright portions of the base side lower joint 263
  • 265 is a connecting joint that connects the upper joint shaft 262 and the lower joint shaft 264 orthogonally It is a moving part.
  • the base portion side lower joint 263 rotates in the axial direction of the upper joint shaft 262 and the lower joint shaft 264 with respect to the base portion side upper joint 261.
  • the base-side passive joint 236 has two degrees of freedom in the circumferential direction of the upper joint shaft 262 and the lower joint shaft 264 perpendicular to the longitudinal direction of the linear motion link 235.
  • FIG. 55 the structure of the foot side passive joint will be described in detail with reference to FIGS. 55 and 56.
  • FIG. 55 is a perspective view of the main part of the foot-side passive joint
  • FIG. 56 is a vertical sectional view of the main part of the foot-side passive joint.
  • Fig. 55 and Fig. 56 [This is a fixed plate for 203ai right foot rod 203, 235, 235 ' ⁇ linear motion link, 237 is a foot side passive joint, 244 is a joint joint of 235 linear motion link , 244 'is the joint joint of the linear link 23, 271 is the first foot upper joint connected to the lower end of the joint joint 244, 271a, 27 lb are both sides of the first foot upper joint 271 , 273 is a second foot side upper joint connected to the lower end of the joint joint 244 ′.
  • each side shaft support plate 271a, 271b, 273a, 273b is supported by a joint shaft described later via a ball bearing 272a (see FIG. 56).
  • 275 is a ball holding portion having a ball housing portion 275a having a spherical inner wall surface
  • 276a and 276b are joint shafts protruding on both sides of the ball holding portion 275 (see FIG. 56)
  • 277 is a joint shaft.
  • 276a, 276b insects are attached to each heel J ⁇ axle support plate 271a, 271b, 273a, 273b retaining nut, 278 is the foot base fixed on fixing plate 203a, 279 is foot side This is a ball joint shaft erected on the base 278 and having a ball portion 279a accommodated in the ball accommodating portion 275a at the upper end.
  • the foot side passive joint 237 has two degrees of freedom in the axial direction of the joint shafts 276a and 276b and the axial direction of the ball joint shaft 279, and at least the joint shafts 276a and 276b and the ball joint. Since there is one degree of freedom in the circumferential direction of the shaft orthogonal to the shaft 279, it follows the expansion and contraction of the linear motion links 23 5 and 235 ′ and smoothly follows without obstructing this.
  • the foot-side passive joint 237 has a structure using a ball joint, so the force that has been generated in the past due to the large number of fastening points in the structure has been reduced, but the operation has been reduced. This improves the accuracy of the robot, increases the stability and rigidity of the control, and enables stable walking at high speed.
  • Fig. 1 (a) is an explanatory diagram showing the support polygon
  • Fig. 1 (b) is an explanatory diagram showing the definition of the coordinate system and vectors.
  • the biped robot cannot maintain a stable support state, that is, it falls, it means that a support polygon formed by the contact point (three or more points) between the sole and the road surface (the contact point between the road surface and the foot is formed). This is considered to be rotating around the straight line on the road surface passing through the side or point with the largest convex polygon). That is, it is thought that an outward moment is acting from the support polygon. In other words, if no outward moment is generated around all sides and points on the support polygon, but only an inward moment is generated, the biped robot will have a stable support state. Can be maintained.
  • This algorithm has the following four points.
  • the ground contact state between the sole and the road surface of the biped robot is a set of point ground contacts.
  • the propulsive force when a biped robot walks should be within the range where no translational (X and Y axis) slip occurs at the contact point.
  • the mass point of the trunk will not move.
  • the trajectory of the waist mass point and the trajectory of the trunk mass point are different in the Z-axis direction and follow the same trajectory in the XY plane.
  • Equation 1 Equation 1 that derives the moment balance equation for an arbitrary point ⁇ in the absolute coordinate system ⁇ — ⁇ . It is shown in equation 26)).
  • an XYZ bar is set for the motion coordinate system O-bar that is fixed near the waist of the biped robot shown in Fig. 1 (b).
  • the waist trajectory is calculated to satisfy this ZMP equation.
  • the exact solution of the moment compensation trajectory is obtained by calculating the approximate solution of the moment compensation trajectory and substituting it into a rigorous model, feedback of the moment error, and accumulating iterative calculations.
  • the approximate modeling process is as follows.
  • Equation 5 (Equation 5) interferes with each other in terms of the apparent force generated by the rotation of the motion coordinate system. Therefore, in order to make these differential equations incoherent, first assume that no moment is generated by the apparent force, that is, if the motion coordinate system does not rotate, 5) becomes (Equation 6) and (Equation 7). [Equation 6] m T (Z T -Z ZMP ) ( ⁇ + Xq-g,) ⁇ ⁇ ⁇ (X T -X ZMP ) (Z T + Z q —)
  • Equation 6 and (Equation 7) are non-linear and interfering systems because they share the movement in the Z-axis direction and the lower limbs and trunk are connected via a rotating joint. Therefore, biped robots are assumed to have a constant waist height during movement, and are linear and non-interacting. In other words, (Equation 8), (Equation 9), and (Equation 10) hold.
  • (Equation 9) relates to the pitch axis
  • (Equation 10) relates to the roll axis.
  • Equation 20 is a known function because it can calculate the lower limb trajectory and the ZMP trajectory force. Further, if it is a steady walking, each part point and ZMP of the biped robot will be in the motion coordinate system. Since the O-bar XYZ bar has a periodic relative motion, (Equation 20) becomes a periodic function.
  • Equation 18 is expressed as a Fourier series with unknown coefficients as (Equation 23).
  • G Lg x , g y , gj
  • the parallel link mechanism of a biped robot consists of a base plate (the hip of the biped robot) and an end effector (the sole of the biped robot).
  • Figs. 2 (a) and 2 (b) are schematic diagrams showing the waist coordinate system (Equation 27) and the foot coordinate system (Equation 28).
  • Equation 29) is the position vector of the end effector side joint that also saw the waist coordinate system force.
  • Equation 27 The position of the waist joint in the waist coordinate system (Equation 27) is (Equation 30), and the position of the foot joint in the foot coordinate system (Equation 28) is (Equation 31).
  • Equation 31 Here, we define a vector (Equation 32) that represents the position / posture of the foot coordinate system (endefector position / posture) as seen from the waist coordinate system.
  • E xi (cos6 z cos0 y ) e xi + os0 z si n0 y si n0 x -si nd z cos6 x ) e yi
  • E zi (-sin0 y ) e xi + (cos0 y sin0 x ) e yi + (cos0 y cos0 x ) e zi + z
  • the length of the link connecting the i-th waist joint and the i-th foot joint is (Equation 40).
  • FIGS 3 (a) and 3 (b) are explanatory diagrams of the theoretical compliance shift.
  • (Equation 42) indicates the movement allowance for landing road surface detection described in the control software
  • (Equation 43) is the coordinates of the walking pattern (coordinates of the free leg) in the waist coordinate system
  • (Equation 44) is the absolute value. It is the waist coordinate (ie, waist height) in the coordinate system.
  • the dotted line indicates the position of the foot of the free leg in the walking pattern.
  • the walking pattern is output as if the leg was extended by (Equation 42).
  • (Equation 43) is a waist coordinate system, it has a negative value as shown in Fig. 3.
  • Fig. 3 (c) is an explanatory diagram of nonlinearity
  • Fig. 3 (d) and (e) are explanatory diagrams of nonlinear compliance movement amount. The amount of nonlinear compliance movement is shown by (Equation 49).
  • Figure 3 (c) shows that the compliance coefficient is different from the value of (Equation 42).
  • the nonlinear compliance movement amount satisfies the condition of (Equation 50
  • the nonlinear compliance movement amount is expressed by (Equation 51)
  • the nonlinear compliance movement amount is When the compliance movement amount satisfies the condition of (Equation 52), the nonlinear compliance movement amount is expressed by (Equation 53).
  • Equation 54 shows the compliance coefficient for detecting the landing road surface
  • Equation 55 shows the vibration suppression. Indicates the in-service compliance coefficient.
  • the compliance coefficient for detecting the landing road surface is set to be as small as possible without causing oscillation of the control system. This makes it possible to detect the shape of the road surface before a large floor reaction force occurs (and if the theoretical compliance movement amount is within the range of (Equation 42), this constant can be expressed as (Equation 48). Is taken sufficiently small).
  • Equation 56 shows the floor reaction force in the z direction
  • Equation 57 shows the control cycle (for example, lms).
  • Figure 3 (f) is an explanatory diagram showing compliance.
  • ZCOM indicates z-direction compliance
  • RCOM indicates roll direction compliance
  • PCOM indicates pitch direction compliance.
  • the torque in the roll direction detected by the 6-axis force sensor described later is expressed by (Equation 61)
  • the torque in the pitch direction detected by the 6-axis force sensor is also expressed by (Equation 62).
  • the amount of compliance movement in the roll direction of 63) and the amount of compliance movement in the pitch direction of (Equation 64) are expressed by (Equation 65) and (Equation 66).
  • the landing control described above can complete the following operation on the road surface before the free leg generates a large floor reaction force and ZMP, attitude angle deviation and acceleration occur.
  • Equation 69 is used to calculate the attitude angle compensation amount.
  • the posture angle compensation amount, gain matrix, and ZMP deviation value of (Equation 69) are expressed by (Equation 70), (Equation 71), and (Equation 72), respectively.
  • Equation 74 is the ZMP actual measurement value calculated from the detection value of the 6-axis force sensor using (Equation 76).
  • M R [M XR , M YR , M zR ( ⁇ , moment determined by S R )
  • Equation 75 is the ZMP target value, which is the same as the target ZMP trajectory set when creating the walking pattern. Value.
  • an operation period that is an intermittent period of a predetermined cycle is set in advance, and the operation is performed during the operation period.
  • the actual hip posture angle manipulated variable was calculated using (Equation 78) as a function of the time t of the force at the start of the manipulation period in order to prevent sudden posture angle fluctuations. calculate.
  • the attitude angle operation amount and operation period of (Equation 78) are expressed by (Equation 79) and (Equation 80), respectively.
  • the attitude angle operation amount is 0 at the start of the operation period, and becomes the attitude angle compensation amount (Equation 70) at the end of the operation period.
  • Equation 79 r ⁇ (Roll direction angle ⁇ ⁇ *)
  • the posture of the lumbar region is manipulated by manipulating the position or posture of the foot relative to the lumbar region.
  • the leg to be operated is the leg on the stance side when it is a single leg, and its posture (roll and pitch direction) is manipulated with reference to the center of the leg.
  • both feet are operated, and their positions and postures are manipulated with reference to the midpoint of the line connecting the centers of the feet.
  • the integration error with the actual posture angle error will increase, so the integration will start and the integration value of the ZMP deviation value will be set to zero after the specified time has elapsed. (Reset) to avoid this.
  • FIG. 4 is a block diagram showing the overall configuration of the walking pattern creation device (walking pattern creation converter) and the biped walking robot device according to the present embodiment.
  • 1 is a robot ⁇ ⁇ ⁇ as a biped walking robot device (corresponding to 201 and 201c in Figs. 50 and 51), 2 is for creating a walking pattern as a walking pattern creation device that creates a walking pattern Computer, 3 is an external power supply unit that is supplied with commercial power (100 VAC), 4 is a robot unit (biped robot device) 1 is an external switch for emergency stop, etc. 5 is a waist part of the robot unit 1 (Equivalent to 202 and 209 in FIGS. 50 and 51), 6 is a leg (lower limb) constituting the robot unit 1 (corresponding to 201a and 201b in FIG. 50). As shown in FIG.
  • the waist part 5 of the robot part 1 and the walking pattern creation computer 2 are connected via wireless (for example, wireless LAN).
  • the waist 5 is supplied with power from the external power supply 3.
  • the battery built into the waist 5 External power supply unit 3 is not required when using a remote control (231 in Fig. 51).
  • FIG. 5 is a block diagram mainly showing the waist 5.
  • control computer 7 (corresponding to 2 in FIG. 51 (corresponding to W), motor control unit 9, brake control unit 10, power supply unit 11, and switch circuit 12.
  • Control computer 7 includes: It is connected to an external walking pattern creation computer 2 via a wireless LAN, and acquires and processes sensor information from the leg 6. The control computer 7 also sends a motor control unit 9 to the motor control unit 9. Outputs the speed command value (voltage) and turns on / off the DC servo motor (described later) in the cylinder Cl to C12 (described later) on the leg 6 and shorts the DC servo motor terminal. Outputs the function control signal to the switch circuit 12.
  • the power supply unit 11 is supplied with power from the external power supply unit 3, but is not required when using a battery.
  • the switch circuit 12 receives signals from the external switch 4 and the control computer 7, and based on the logic of the exclusive OR (described later), the DC servo motor Controls on / off, brake on / off, and DC servo motor terminal short-circuit function, and the motor controller 9 controls the speed of the DC servo motor of the leg 6 according to the speed command value from the control computer 7. Also, DC servo motor is turned on / off by servo on / off signal from switch circuit 12, and DC servo motor terminal is shorted by motor terminal short circuit signal from switch circuit 12. Brake control unit 10 The brake is opened and closed according to the signal from the switch circuit 12.
  • FIG. 6 is a block diagram mainly showing the leg portion 6.
  • the leg 6 is composed of two legs, a right leg 13a and a left leg 13b.
  • Each leg 13a, 13b is connected to 6 actuator cylinders C1-C6, C7-C12 (linear motion link 235 in FIGS. 50 and 51) and right foot bottom 14a (right foot 203 in FIGS. 50 and 51). Equivalent) and left sole 14b (corresponding to left foot 204 in FIGS. 50 and 51) and force.
  • the DC servo motor of each cylinder is controlled by the current from the motor controller 9. The terminal voltage and current of the DC servo motor are fed back to the motor controller 9.
  • a rotary encoder (described later) of each cylinder C1 to C12 detects the rotation angle of the DC servo motor and feeds it back to the control converter 7.
  • Photomicroscope for offset detection of each cylinder C1 to C12 The sensor detects the initial position and outputs a signal to the control computer 7.
  • 6-axis force sensors (described later) on the soles 14a and 14b acquire floor reaction force information. This is recorded in the control computer 7. Power is supplied from the power supply unit 11 to the sensors of the cylinders C1 to C12.
  • FIG. 7 is a block diagram showing the external power supply unit 3.
  • the external power supply unit 3 includes an ACZDC converter 31.
  • the ACZDC converter 31 converts commercial power (AC 100V) into a DC voltage of 48V and supplies it to the power unit 11 of the waist 5 of the robot unit 1.
  • FIG. 8 is a block diagram showing the walking pattern creation computer 2.
  • the walking pattern creation computer 2 includes a CPU 21, a memory 22, a hard disk 23, an input device 24, a display device 25, an interface unit 26, a LAN card 27, a wireless LAN 28, and a robot using a walking pattern creation software. Create a walking pattern for part 1.
  • the wireless LAN 28 can communicate with the control computer 7, and robot control software including a walking pattern is loaded onto the control computer 7.
  • FIG. 9 is a block diagram showing the external switch 4.
  • the external switch 4 includes a brake control switch 41, a servo ON switch 42, and a motor terminal short-circuit switch 43.
  • the state of each of the switches 41 to 43 is sent as a signal to the switch circuit 12 of the lumbar part 5 of the robot unit 1 to control the corresponding function.
  • Brake control switch 41 turns on and off the holding brakes of each actuator cylinder C1 to C12.
  • Servo ON switch 42 controls the servo on / off of the DC servo driver (described later).
  • Motor terminal short-circuit switch 43 controls the motor terminal short-circuit switch of the DC servo driver. When the motor terminal is short-circuited, the motor becomes difficult to rotate due to the back electromotive force of the motor.
  • FIG. 10 is a block diagram mainly showing the control computer 7.
  • the control computer 7 includes basic components such as a CPU 71, a memory 72, and a hard disk 73, an interface unit 74 for a pulse counter 79, a LAN power mode 75, a wireless LAN 76, a D / A converter 77, It consists of a 6-axis force sensor receiver board 78.
  • the control computer 7 is connected to a walking pattern creation computer via a wireless LAN 76.
  • the robot control software that can communicate with the computer 2 and is created by the walking pattern creation computer 2 and stored in the node disk 73 is loaded into the memory 72 and executed.
  • control computer 7 converts the speed command value calculated by the robot control software into an analog voltage using the DZA converter 77 and outputs it to the motor control unit 9 and also outputs the cylinders C1 to C12 of the leg 6 to each other.
  • the motor angle information output from the rotary encoder is acquired via the pulse counter 79, and this is fed back to the CPU 71 for motor position control.
  • control computer 7 can control each function by outputting a brake control signal, a servo ON signal, and a motor terminal short circuit signal to the switch circuit 12 as necessary. All power in the control computer 7 is supplied from the power source 11 of the waist 5.
  • FIG. 11 is a block diagram mainly showing the power supply unit 11.
  • the power source unit 11 is provided with a battery Z power source switching unit 112 for selecting a power source supplied from the external power source unit 3 and a power source supplied from the 48V nickel metal hydride battery 111.
  • the DC-DC converters 114 and 115 are used to supply power to the control computer 7, and the voltages are 5V and ⁇ 12V.
  • the DC-DC converter 116 is for supplying power to the brake control unit 10 and has a voltage of 24V.
  • the DC-DC converter 117 is used to supply power to digital devices such as the sensors of the leg 6 and the digital IC of the switch circuit 12, and the voltage is 5V.
  • the motor control unit 9 is supplied with the main power supply voltage, that is, the voltage of the battery 111 or the external power supply unit 3 as it is. When the power switch 113 of the power supply unit 11 is turned on, power is supplied to the robot unit 1.
  • FIG. 12 is a block diagram mainly showing the switch circuit 12.
  • the switch circuit 12 controls each function of turning the brake on / off via the brake control unit 10, turning the servo on / off via the motor control unit 9, and shorting the motor terminal via the motor control unit 9.
  • the logic circuit exclusive OR
  • the switch circuit 12 inputs two signals, the signal from the control computer 7 and the signal from the external switch 4, and outputs an exclusive OR. In other words, if either input changes Since the output state always changes, the robot unit 1 can be urgently stopped for any combination of inputs.
  • the digital IC constituting the switch circuit 12 is supplied with power from the power supply unit 11 of the waist 5.
  • FIG. 13 is a block diagram mainly showing the brake control unit 10.
  • the brake control unit 10 includes a semiconductor relay 101, receives a brake on / off signal from the switch circuit 12, and turns on and off the current supplied from the power supply unit 11. Turn the holding brake of each cylinder C1 to C12 on and off.
  • the holding brake controlled by the brake control unit 10 is in a released state when a current is applied, and is in a state where the brake is active when the current is off (non-energized state).
  • FIG. 14 is a block diagram mainly showing the motor control unit 9.
  • the motor control unit 9 is composed of 12 DC servo drivers D1 to D12, and each driver D1 to D12 corresponds to each cylinder C1 to C12 of the leg 6.
  • the power necessary for the DC servo drivers D1 to D12 is supplied from the power supply 11 of the waist 5.
  • DC servo drivers D1 to D12 turn the servo on and off according to the signal from the switch circuit 12 (servo on and off signal). When the servo is off, the motor terminals are opened and become free.
  • DC servo drivers D1 to D12 short the motor terminals in response to a signal from the switch circuit 12 (motor terminal short-circuit signal).
  • the DC servo drivers D1 to D12 use the speed command value (voltage) from the control computer 7 as a target value and control the speed of the motors in the cylinders C1 to C12 of the leg 6. Furthermore, the DC servo drivers D1 to D12 supply current to the motor, and acquire and feed back the voltage and current between the terminals of the motor to enable speed control.
  • FIG. 15 is a block diagram mainly showing cylinders C1 to C12.
  • each cylinder C1 to C12 (linear motion link 235) has a rotary encoder 151 (251 in Fig. 52, 53) and a DC servo motor 152 (248 in Fig. 52, 53). ), Holding brake 153 (corresponding to 250 in FIGS. 52 and 53), linear motion part 154, and photo-mic mouth sensor 155 for offset detection (corresponding to initial position sensor 247 in FIGS. 52 and 53).
  • the rotary encoder 151 and photomicrosensor 155 are powered from the power supply 11 of the waist 5 Supplied.
  • the speed of the DC servo motor 152 is controlled by the motor control unit 9.
  • the holding brake 153 is controlled by the brake control unit 10, and when the holding brake 153 is turned on, the DC servo motor 152 is fixed by a frictional force.
  • the rotary encoder 151 detects the relative rotation angle of the DC servo motor 152 and sends a number of pulses proportional to the relative rotation angle to the control computer 7. Based on the feedback angle data, the control computer 7 can control the position of the DC servo motor 152.
  • the rotational motion of the DC servo motor 152 is converted into a linear motion (linear motion) by the linear motion part 154, and the linear motion of each of the six cylinders on the left and right is transmitted to the sole 14.
  • the movement of the sole 14 at this time follows the forward kinematics of the Stewart platform.
  • inverse kinematics is used when creating walking patterns.
  • the rotary encoder 151 is a sensor that detects only a relative angular displacement
  • the initial position is detected using the photomicrosensor 155.
  • the photomicrosensor 155 reacts when the cylinder is most contracted, and informs the control computer 7 that the cylinder is in the initial position.
  • FIG. 16 is a block diagram mainly showing the sole (foot) 14.
  • the sole 14 is composed of a right sole 14a (203) and a left sole 14b (204) (see FIGS. 6, 45 and 46).
  • the sole 14 is composed of a sole plate 141 (corresponding to 203a and 204a in FIG. 51) and a six-axis force sensor 142 (corresponding to 203b and 204b in FIG. 51).
  • the motion of the sole plate 141 is determined.
  • the floor reaction force information is obtained by the 6-axis force sensor 142 and processed by the control computer 7 so that the actual ZMP position can be checked.
  • FIG. 17 is a block diagram mainly showing the linear motion portion 154 (the linear motion portion of the linear motion link 235 in FIGS. 50, 51, 52, and 53).
  • the linear motion part 154 includes a power transmission part 171 (which also includes a toothed pulley 249 and a grooved belt 252 in FIG. 52 and a belt 252 equal force), a slide screw 172 (corresponding to the male screw shaft part 254 in FIG. It consists of nut 173 (corresponding to 255 in FIG. 53) and linear guide 174 (corresponding to rod rail 245 and rail guide 246 in FIGS. 52 and 53).
  • the power transmission unit 171 transmits the rotation of the DC servo motor 152 to the shaft of the slide screw 172 and absorbs the deviation of the axis of the rotation shaft.
  • the nut 173 moves linearly (linear motion, rectilinear motion). Since the rotation of the nut 173 is restrained by the linear guide 174, only the rectilinear movement can be taken out.
  • the movement on the driven side of the linear motion part 154 is transmitted to the sole plate 141 through the joint.
  • FIG. 18 is a block diagram showing function realizing means (software configuration) in the CPU 21 of the walking pattern creation computer 2.
  • 211 is a setting data input means for inputting setting data such as the total number of phases of the robot part 1, and 212 is a path for the foot part (the sole part) 14 and an initial trajectory for the waist part 5.
  • Trajectory setting means, 213 is a target ZMP trajectory setting means for setting the trajectory of the target zero moment point (target ZMP) based on the set trajectory of the foot 14 and the initial trajectory of the waist 5, and 14 is the motion of the leg
  • target ZMP moment calculation means to calculate the moment around the target zero moment point due to the movement of the waist and 215, Fourier transform means to calculate the Fourier coefficient by performing Fourier transformation on the moment around the target zero moment point, etc.
  • the moment compensation trajectory calculation means for calculating the moment compensation trajectory of the waist 5 (the trajectory compensated for the moment relative to the initial trajectory), 218 is the trajectory of the foot 16 and the moment compensation trajectory of the waist 5 according to the moment compensation trajectory of each mass point.
  • Mass point acceleration calculating means for calculating acceleration 219 is an error moment calculating means for calculating an error moment that is a moment around the target zero moment point based on the calculated acceleration of each mass point, 220 is a calculated moment of error is a predetermined moment
  • the error moment determination means for determining whether or not it is smaller than 221 is the position and posture (roll angle, horizontal angle, Foot position / posture calculation means for calculating the pitch angle)
  • 222 is a gait flag determination means for determining and storing the gait flag
  • 223 is the position of the foot as seen from the waist force.
  • a file output unit that outputs the file I le of the hard disk 23 as the ZMP trajectory and the gait flag walking pattern seen from the time-series data and the waist portion.
  • the target ZMP rotation moment calculation means 21 4 is a known ZMP rotation moment calculation means 2141 based on the lower limb movement that calculates the target ZMP rotation moment generated by the movement of the leg 6 and the known ZMP rotation moment generated by the movement of the waist 5 Mean moment calculation means around the target ZMP by waist motion to calculate moment 2
  • the Fourier transform means 215 includes a moment sum calculating means 2151 for calculating the sum of the moment around the target zero moment point calculated by the target ZMP moment calculating means 214 and the calculated error moment, and And a Fourier calculation means 2152 for calculating a Fourier coefficient based on the calculated sum
  • the foot position / posture calculation means 222 is an origin position calculation means for calculating the origin position of the foot coordinate system (Equation 28) 2211
  • rotation matrix calculation means 2212 for obtaining a rotation matrix for calculating the posture of the foot
  • posture calculation means 2213 for calculating the posture angle of roll angle, single angle, and pitch angle.
  • FIG. 33 is a block diagram showing function realizing means (software configuration) in the CPU 71 of the control computer 7.
  • reference numeral 711 denotes a power-on means for turning on the power via the power supply unit 11
  • 712 denotes a walking pattern setting means for receiving the walking pattern wirelessly from the walking pattern creating computer 2 and storing it in the hard disk 73.
  • 713 is a cylinder initial position setting means for setting the initial positions of the cylinders C1 to C12 that extend and contract the legs of the leg portion 6, and 714 is a walking pattern force stored in the hard disk 73.
  • a pattern used for walking is selected to indicate the start of walking.
  • 715 is a motor control means for controlling the motor (DC servo motor) based on the walking pattern to be used
  • 716 is an end judging means for judging whether or not the end force of the walking motion is sufficient.
  • FIGS. 19 to 31 are flowcharts showing the operation of the CPU 21 of the walking pattern creation computer 2 (walking pattern creation operation), and Fig. 32 is the calculation of the origin position of the foot coordinate system, the calculation of the rotation matrix, and the calculation of the posture angle. It is explanatory drawing explaining about.
  • FIG. 19 shows the overall operation of creating a walking pattern.
  • the setting data input means 211 inputs the total number of steps, the position and posture of the sole 14 and the time of one phase (walking speed) via the input device 24 as setting data (S1).
  • the time of one phase is the time length of one piece when a certain period is divided.
  • a continuous walking motion can be performed by giving a walking pattern to the legged robot device.
  • the trajectory setting means 212 sets the trajectory of the foot (plantar part) 14 and the initial trajectory of the waist 5 (S2, trajectory setting step), and the target ZMP trajectory setting means 213 reads the set foot 14
  • the trajectory of the target zero moment point (target ZMP) is set based on the trajectory and the initial trajectory of the waist 5 (S3, target ZMP trajectory setting step).
  • Target ZMP moment calculation means 214 Target ZMP moment calculation means 2141 based on lower limb movement calculates target ZMP moment generated by movement of leg 6 (S4, target ZMP moment calculation step based on lower limb movement) Target ZMP moment calculation means 214 Target ZMP known moment calculation means 2142 due to waist movement 2142 calculates the target ZMP around moment generated by the waist 5 movement (S5, target ZMP known moment calculation due to waist movement) Step).
  • the moment total calculation means 2151 of the Fourier transform means 215 is the moment around the target zero moment point calculated by the movement of the foot 14 and waist 5 calculated in the target ZMP moment calculation means 214 and the calculated error moment (initial value is zero) (Moment sum calculation step), Fourier coefficient calculation means 2152 of Fourier transform means 215 calculates Fourier coefficients based on the calculated sum (S6, Fourier coefficient calculation step), and Fourier Coefficient determination means 216 determines a Fourier coefficient based on the Fourier coefficient calculated in step S6 (S7, Fourier coefficient determination step).
  • the moment compensation trajectory calculating means 217 calculates the moment compensation trajectory by performing inverse Fourier transform on the Fourier coefficient determined in step S7, and based on the calculated moment compensation trajectory, the trajectory of the waist 5 (the waist 5 Set moment compensation trajectory (S8, moment compensation trajectory calculation step).
  • the mass point acceleration calculation means 218 calculates the acceleration of each mass point from the set foot 14 trajectory and the waist 5 moment compensation trajectory calculated in step S8, and acquires acceleration time-series data ( S9, mass point acceleration calculating step)
  • error moment calculating means 219 calculates the error moment at the target zero moment point based on the calculated time series data of the acceleration of each mass point (S10, error moment calculating step).
  • the error moment determination means 220 determines whether or not the calculated error moment (eM) is smaller than a predetermined moment ( ⁇ ⁇ ) (Sl l, error moment determination step), and is larger than the predetermined moment. If it is determined, the error moment determination means 220 provides feedback. Calculate the moment of inertia, and then return to step S4 (S12). If it is determined in step S11 that the error moment is smaller than the predetermined moment, the origin position calculation means 2211 calculates the origin position of the foot coordinate system, and the rotation matrix calculation means 2212 calculates the posture of the foot. After calculating the rotation matrix, the posture calculating means 2213 calculates the posture angles of roll angle, single angle, and pitch angle (S13, foot position / posture calculation step).
  • the gait flag determining means 222 determines the gait flag based on the first half of the stance, the second half, and the first and second half of the swing leg (S14, gait flag determining step), and the file output means 223
  • the time-series data of the foot position as seen from the position and the ZMP trajectory as seen from the waist are output as a walking pattern to a file on the hard disk 23 (S15).
  • Fig. 20 (a) shows the setting operation of the track of the sole 14 and the initial track of the waist 5
  • Fig. 20 (b) shows the setting of the track 14 of the sole 14 for one step
  • Fig. 20 (c) shows the setting of one step.
  • the initial trajectory setting operation of waist 5 is shown.
  • i is set to zero (S21), and it is determined whether i is smaller than the total number of phases (S22).
  • the process proceeds to step S23, and the trajectory of the sole portion 14 for one step is set (step sole trajectory setting process for one step).
  • the initial trajectory of the lumbar part 5 for one step is set (S24, lumbar initial trajectory setting process for one step).
  • the phase number is 1 ⁇ stance phase, so move to step S235, and based on the above midpoint and end point, the position of the sole of the free leg (leg away from road surface force) It is approximated by a polynomial (the fifth-order polynomial is a force that needs to be sufficiently smooth even after being differentiated twice).
  • the position of the sole of the standing leg that is stationary is set (S236).
  • step S3 the operation of the target ZMP trajectory setting means 213 shown in step S3 will be described with reference to FIG.
  • step S36 the process returns to step S36 (S38).
  • steps S36 to S38 until i> the phase number of one step. If it is determined in step S36 that i> the number of phases in one step, then it is determined whether or not j ⁇ total number of phases (S39). Return to S32 (S40). Next, i is set to 1 again, and steps S32 to S38 are repeated until j ⁇ the total number of phases. If it is determined that j ⁇ the total number of phases, this process is terminated.
  • step S4 the operation of the target ZMP rotation moment calculating means 2141 by the lower limb movement shown in step S4 will be described with reference to FIG.
  • n is set to 1 and the number of mass points of the lower limb is set to N (S41), and it is determined whether n ⁇ total phase power (S42). Since n ⁇ total phase number at first, the moment (vector M) generated around the target ZMP by the movement of the lower limbs is calculated (S43). The vector M shown in step S43 is a vector when the external force is zero in (Equation 2). Next, n is incremented by 1, and the process returns to step S42 (S44). Steps S42 to S44 are repeated until it is determined in step S42 that n> total number of phases. If it is determined that n> total number of phases, this process is terminated. The total number of phases is, for example, 1024 (2 to the 10th power).
  • n is set to 1 (S51), and it is determined whether n ⁇ total phase power (S52). Since n ⁇ total phase at first, the known moment (Mky, Mkx) generated around the target ZMP by hip motion is calculated (S53). The known moment shown in step S53 is the moment when the coordinates related to the waist 5 are unknown in (Equation 6) and (Equation 7) (only the known terms on the left side in Equations 6 and 7 are extracted and transferred to the right side). ) Next, n is incremented by 1, and the process returns to step S52 (S54). Steps S52 to S54 are repeated until it is determined in step S52 that n> the total number of phases. If it is determined that n> total number of phases, this process is terminated.
  • the moment sum calculating means 2151 obtains the sum of the vector M obtained in step S4 and the known moment (vector Mk) obtained in step S5 and the error moment (vector E) obtained in step SI 2 described later. Set the total number of phases to N (S61, moment sum calculation step). Note that the initial value of the error moment is zero.
  • the Fourier coefficient calculating means 2152 performs a Fourier transform on the sum (S62) and calculates a Fourier coefficient (S63). Fourier transform and Fourier coefficient calculation are common methods. [0136] Next, the operation of the Fourier coefficient determination means 216 shown in step S7 will be described with reference to FIG.
  • the Fourier coefficient determination means 216 represents the moment compensation trajectory of the waist 5 by a Fourier series (S71).
  • the Fourier series is substituted into (Equation 15) and (Equation 16) to compare the Fourier coefficients (S72), and the Fourier coefficient is determined based on the comparison result (S73).
  • the moment compensation trajectory calculating means 217 sets the number of all phases to N (S81), and sets the data for inverse Fourier transform (that is, data for inverse Fourier transform set based on the Fourier coefficient determined in step S73). Set (see (Equation 25)) (S82), and calculate the moment compensation trajectory of the waist 5 by inverse Fourier transforming the data for inverse Fourier transform (S83).
  • i is set to 1 (S84), and it is determined whether i ⁇ N (S85). Since i ⁇ N at first, the trajectory of waist 5 is set based on the moment compensation trajectory calculated in step S83 (S86).
  • i is increased by 1 (S87). Steps S85 to S87 are repeated until i> N. When it is determined that i> N, this process is terminated.
  • the mass point acceleration calculation means 218 sets the time of one phase to T (for example, 30 ms), n to 1, and the total number of phases to N (S91, S92), and determines whether n ⁇ N force is satisfied (S93 ). Since n ⁇ N at first, i is set to 1 and the number of phases of the mass points is set to J (S94), and it is determined whether i ⁇ J (S95). Since i ⁇ J at first, the acceleration of each mass point in discrete time (the second-order fraction of ri (n)) is calculated (S96). Next, i is increased by 1 (S97). Steps S95 to S97 are repeated until i> J. If it is determined that i> J, n is incremented by 1 (S98), and steps S94 to S98 are repeated until n> N. If it is determined as N, this process is terminated.
  • the error moment calculation means 219 sets n to 1 (S101), and determines whether n ⁇ total phase power (S102). Since n ⁇ total number of phases at first, the error moment (eM (n)) is calculated in the same manner as in step S43 (S103), and n is increased by 1 (S104). Steps S102 to 3104 are repeated until 11> total number of phases.
  • the error moment determination means 220 sets the number of repetitions to n (S121), if n is 1, sets the error moment to 0 (S122, S123), and if n is 2, sets the error moment to the value calculated at the first time. (S124, S125), When n is 3 or more, the error moment used last time plus half of the error moment calculated last time is used as the current error moment (S126).
  • step S13 the operation of the foot position / posture calculation means 221 shown in step S13 will be described with reference to FIG.
  • n is set to 1 (S131), and it is determined whether n ⁇ total number of phases (S132). Since n ⁇ the total number of phases at first, the origin position calculation means 2211 calculates the origin position of the foot coordinate system (see S1 33, Fig. 32 and (Equation 82) described later), and the rotation matrix calculation means 2212 The rotation matrix for calculating the posture of the part is calculated (S134, see (Equation 87) described later), and the posture calculation means 2213 calculates the posture angle of the roll angle, the first angle, and the pitch angle (S135, described later). (See Equation 91). Next, n is incremented by 1, and the process returns to step S132. Repeat this until n> total number of phases. When n> the total number of phases is reached, this process is terminated.
  • each cylinder length (that is, the distance between the waist joint and the foot joint) is calculated from the foot position / posture and waist position / posture in the absolute coordinate system by inverse kinematics, and this is used as the walking pattern.
  • the position of the foot coordinate system as seen from the waist coordinate system is calculated from the foot position / posture and waist position / posture in the absolute coordinate system in the following procedure. This is output as a walking pattern.
  • Lumbar coordinate system force The position vector of the origin of the foot coordinate system (Equation 81) is (Equation 82) from the absolute coordinates of the foot and waist (see Fig. 32).
  • the relationship of the rotation matrix (Expression 85) representing the posture of the lumbar coordinate system is as shown in (Expression 86) and (Expression 87).
  • Equation 83 representing the posture of the foot coordinate system, which also shows the waist coordinate system force, calculated in this way, the roll angle, the horizontal angle, and the pitch angle are converted by the following procedure.
  • Lumbar coordinate system force The component of the rotation matrix (Formula 83) representing the posture of the foot coordinate system as seen is set as (Formula 90).
  • the foot coordinate system position and posture (Equation 92) and (Equation 93) as seen from this waist coordinate system are calculated for both feet and output as a walking pattern.
  • Atan2 (y, x) is calculated as (Equation 94)
  • the phase number n is set to “1” (SS51), and it is determined whether n is equal to or less than the total number of phases (SS52). Since n is initially equal to or less than the total number of phases, the process proceeds to step SS53 to determine whether or not it is a stance. Step If SS53 is determined to be a stance stand, it is next determined whether or not the stance of the stance is early (SS54). The determination of whether or not the body is standing or whether or not the stance of the stance is in the first stage is performed in association with, for example, the elapsed walking time or the distance and speed between the foot and the road surface.
  • the gait flag F [n] is set to "0" when it is determined to be in the early stance phase (SS55), and the gait flag F [n] is determined to be not in the early stance phase (that is, the late stance phase). Set to 1 "(SS56). If it is determined in step SS53 that the leg is not standing (that is, it is a free leg), it is next determined whether or not the first leg of the free leg has been moved (SS57). Whether or not it is the first stage of the free leg is determined, for example, based on whether or not the free leg is moving up. If it is determined at step SS57 that it is in the first half of the free leg, the gait flag F [n] is set to "2" (SS58). Set to “SS59”. After setting the gait flag in steps SS55, 56, 58 and 59, the phase number n is increased by 1 (SS60). If it is determined in step SS52 that the number of phases n has exceeded the total number of phases, this processing is terminated
  • FIG. 33 is a block diagram showing the means for realizing functions in the CPU 71 of the control computer 7
  • FIGS. 34 to 36 are flowcharts showing the CPU operation of the control computer 7
  • FIG. 37 shows the motor control means 715 of the CPU 71. It is a functional block diagram.
  • the power-on means 711 supplies the power of the biped walking robot apparatus via the power supply unit 11. (SS1).
  • the walking pattern setting means 712 wirelessly receives the walking pattern from the walking pattern creation computer (walking pattern creation device) 2 and stores it in the hard disk 73 (SS2, walking pattern setting step), and sets the initial cylinder position.
  • Means 713 sets the initial positions of the cylinders C1 to C12 for extending and contracting the legs 6 (SS3, cylinder initial position setting step).
  • the walking start command means 714 is stored in the hard disk 73.
  • the selected walking pattern force is also used to select a pattern to be used for walking (SS4, walking start command step), and the motor control means 715 controls the motor (DC servo motor) 152 based on the walking pattern to be used. (SS5, motor control step).
  • the end determination means 716 determines whether or not the walking motion is finished (whether or not the end command is output from the control computer 7) (SS6).
  • step SS2 the operation of the walking pattern setting means 712 shown in step SS2 will be described with reference to FIG.
  • the number of patterns to be set is set to N and j is set to zero (SS21).
  • j is set to zero (SS21).
  • j is determined whether or not; j ⁇ N force (SS22).
  • the data created by the computer 2 for creating walking patterns (foot position / posture in the waist coordinate system, waist trajectory in the absolute coordinate system, ZMP trajectory in the absolute coordinate system) ) Is stored in the memory 72 of the control computer 7 (SS23).
  • j is increased by 1 (SS24). Repeat steps SS22 to SS24 until j ⁇ N. If it is determined that j ⁇ N, this process is terminated.
  • step SS3 the operation of cylinder initial position setting means 713 shown in step SS3 will be described using FIG.
  • a constant voltage is output from the D / A converter 77 to each motor dryer (DC servo dryer) D1 to D12, and the cylinders C1 to C12 are moved in a contraction direction at a low speed and at a constant speed (SS31). ).
  • SS32 the sensor 155 for detecting the initial position has reacted
  • the process returns to step SS31, and if it has reacted, the position control is performed on the cylinder with the reacted sensor.
  • FIG. 37 is a functional block diagram showing the motor control means 715.
  • reference numeral 810 denotes basic control means for performing basic control
  • 820 denotes landing control means for performing landing control.
  • the basic control means 810 adds the walking pattern output means 811 for outputting the walking pattern stored in the memory 72 and the z-direction landing trajectory correction amount described later.
  • Correction amount adding means 812 including a direction adding means 8121, compliance calorie calculating means 8122 for adding a non-linear compliance movement amount, and roll 'roll for adding pitch direction trajectory correction amount' pitch adding means 81 23, and foot position ' Inverse kinematics calculation unit 813 that generates cylinder length data (that is, link length data) by performing inverse kinematics operation on the posture target value, and cylinder length data as motor angular displacement data
  • Data conversion means 814 for converting to, initial position fine adjustment means 815 for finely adjusting the initial position of the cylinder, and cylinder drive device drive means 816 for driving a cylinder drive device 400 (see FIG. 37) described later.
  • the landing control means 820 includes a mode selection means 821 for selecting a control mode based on the gait flag F [n], and each component of the walking pattern as a control target for landing control (control).
  • Ground trajectory correction amount calculation means 823 nonlinear compliance movement amount calculation means 824 that calculates nonlinear compliance movement amount from z-direction pressure (floor reaction force), and roll.pitch direction trajectory correction amount from roll 'pitch direction torque And a roll pitch direction trajectory correction amount calculation means 825 and a walking pattern correction means 826 that creates a new walking pattern based on the correction amount and movement amount to be added.
  • the z-direction landing trajectory correction amount calculation means 823 is a theoretical compliance movement amount calculation means 823 for calculating theoretical compliance.
  • Compliance movement amount deviation calculation means 8232 that calculates the deviation between theoretical compliance movement amount and nonlinear compliance movement amount as compliance movement amount deviation (landing road surface height error), control mode and compliance based on gait flag
  • Trajectory correction amount calculating means 8233 for calculating the z-direction landing trajectory correction amount based on the movement amount deviation.
  • the roll 'pitch direction trajectory correction amount calculation means 825 includes a compliance movement amount calculation means 8251 for calculating a roll' pitch direction compliance movement amount and a roll 'pitch direction torque force, and a roll' pitch direction trajectory correction amount. Trajectory correction amount calculation means 8252 to be calculated.
  • FIG. 38 shows the motor It is a block diagram which shows a control means and a motor drive device.
  • FIG. 39 is a flowchart showing the operation of the basic control means 810.
  • walking pattern output means 811 inverse kinematics computing means 813, data conversion means 814, initial position fine adjustment means 815, and cylinder drive drive means 816 are the same as those in FIG. The description is omitted.
  • the operation of the basic control means 810 configured as described above will be described with reference to FIG. 39, when a walking pattern is first selected and a walking start command is issued (the walking pattern selection and the walking start command are transmitted via an external computer power radio.
  • the walking pattern output means 811 is the foot position / posture data in the hip coordinate system and the hip coordinate / heel coordinate data in the absolute coordinate system (hereinafter referred to as “foot etc. data”). Are interpolated at each control cycle (approximately lms) and output (SS41).
  • the correction amount adding means 812 adds the correction amount to the foot part data, but the basic control does not add the correction amount (S S42).
  • the inverse kinematic calculation means 813 The cylinder length data is calculated by performing the above-described inverse kinematics operation on the foot data (SS43), and the data conversion means 814 converts the calculated cylinder length data into the motor angular displacement data. Convert (SS44).
  • the cylinder drive driving means 816 outputs the addition data a obtained by adding the initial position fine adjustment data (SS45) from the initial position fine adjustment means 815 to the motor angular displacement data to the comparator COM (SS46).
  • the added data “a” is compared with the actual angular displacement data “b” from the pulse counter 79 by the comparator COM and becomes difference data (a ⁇ b).
  • the integral data and proportional data of the difference data (a ⁇ b) are added by the proportional integration unit 300 to become the motor rotation speed data c.
  • This data c is converted into a digital voltage by the conversion unit 301 and is converted into a DZA converter. Is output to 77 and converted to an analog voltage by the DZ A converter 77.
  • This analog voltage is input to the servo drivers D1 to D12, and the servo drivers D1 to D12 rotate and drive the DC servo motor 152 by the analog voltage.
  • the rotational displacement (angular displacement) of the DC servo motor 152 is detected as the number of pulses by the rotary encoder 151, and this number of pulses is counted by the pulse counter 79 and fed back to the comparator COM. Comparing unit COM, proportional integration unit 300, conversion unit 301, DZA converter 77, servo driver D1-D12, DC servo motor 15 2.
  • the encoder 151 and the counter 79 constitute a cylinder driving device 400
  • FIG. Fig. 40 is a functional block diagram showing landing control.
  • Figs. 41 (a) to (c) are block explanation diagrams showing each block, and
  • Figs. 42 (a) and (b) are block explanations showing each block.
  • FIG. 43 is a flowchart showing overall control including the landing control (operation of the motor control means 715).
  • FIG. 44 is a flowchart showing the operation of the landing control means 820.
  • walking pattern output means 811 In FIG. 40, walking pattern output means 811, inverse kinematics calculation means 813, data conversion means 814, initial position fine adjustment means 815, cylinder drive device drive means 816, motor drive device 400, mode selection means 821, component Dividing means 822, nonlinear compliance movement amount calculation means 824, walking pattern correction means 826, z direction addition means 8121, compliance addition means 8122, roll 'pitch direction addition means 8123, theoretical compliance movement amount calculation means 8231, compliance movement amount
  • the deviation calculating means 8232, the trajectory correction amount calculating means 8233, the roll 'pitch direction movement amount calculating means 8251 and the trajectory correction amount calculating means 8252 are the same as in FIG.
  • 142 is a 6-axis force sensor that detects 6-axis force components (force components related to position and posture angle), 142a is a 6-axis force sensor 142 outputs the force and moment of each axis based on the data from 142 Receiver board.
  • Equation 96 K z i ⁇ ⁇ Z f cc acR (t) + C z i ⁇ ⁇ Z f cc aC R ( ⁇ )
  • the mode selection means 821 selects a control mode using the mode selection table based on the gait flag F [n] output from the walking pattern output means 811.
  • the mode selection table is shown in (Table 1).
  • the maintenance period (Keep Period), return period (Return Period), and control period (Control Period) are selected, and the roll pitch For the direction, a maintenance period, a return period, and a compliance period are selected.
  • the component dividing unit 822 determines whether each component of the walking pattern output from the walking pattern output unit 811 is a control target component or a non-control target component, and performs landing control only for the control target component, and the non-control target.
  • the component is output to the walking pattern correction means 826.
  • the control target components are z, roll ( ⁇ X), and pitch ( ⁇ y) components.
  • the theoretical compliance movement amount calculation means 8231 calculates the theoretical compliance movement amount from the control target component and the movement allowance for landing road surface detection of (Equation 42) (see (Equation 46) and (Equation 48)). .
  • the non-linear compliance movement amount calculating means 824 calculates the non-linear compliance movement amount by inputting the floor reaction force in the z direction output from the 6-axis force sensor 142.
  • the deviation between the theoretical compliance movement amount from the compliance movement amount calculation means 8231 and the non-linear compliance movement amount calculation means 824 is calculated as the compliance movement amount deviation (see (Formula 58)) in the compliance movement amount deviation calculation means 8232
  • the trajectory correction amount calculation means 8233 is calculated.
  • the trajectory correction amount calculation means 8233 calculates the z-direction landing trajectory correction amount based on the compliance movement amount deviation and the control mode from the mode selection means 821.
  • the control mode period is the control period
  • the z-direction landing trajectory correction amount is (Equation 104).
  • (Equation 105) is gain.
  • the z-direction adding means 8121 stores the z-component of the walking pattern from the component dividing means 822, the z-direction landing trajectory correction amount from the trajectory correction amount calculating means 8233, and the storage unit 827 (Equation 42). Add the movement allowance for landing road surface detection and output as a z-direction corrected trajectory.
  • the compliance adding means 8122 adds the nonlinear compliance movement amount from the non-linear compliance movement amount calculating means 824 and the z-direction corrected trajectory from the z-direction adding means 8121, and outputs the result as a final z-direction trajectory.
  • the compliance movement amount calculation means 8251 uses (Equation 65) and (Equation 66) based on the torque in the roll direction and the pitch direction input from the 6-axis force sensor 142 and calculates the mouth of (Equation 67). Calculate the compliance movement amount in the pitch direction and the movement amount in the pitch direction of (Equation 68).
  • the trajectory correction amount calculation means 8252 is based on the roll direction compliance movement amount and the pitch direction compliance movement amount from the compliance movement amount calculation means 8251.
  • the roll 'pitch direction trajectory correction amount is calculated.
  • the roll direction trajectory correction amount is (Equation 111)
  • the pitch direction trajectory correction amount is (Equation 112).
  • the roll direction trajectory correction amount is (Equation 113), and the pitch direction trajectory correction amount is (Equation 114).
  • the roll direction trajectory correction amount is (Equation 115), and the pitch direction trajectory correction amount is (Equation 116).
  • (Equation 117) and (Equation 118) indicate the last value of the maintenance period, and these equations set the trajectory correction amount that is the last value of the maintenance period at the beginning of the return period to zero at the end of the return period. It is an expression like this.
  • the roll “pitch direction adding means 8123 adds the roll“ pitch component from the component dividing means 822 and the roll “pitch direction trajectory correction amount” from the trajectory correction amount calculating means 8252 to obtain the roll “pitch direction pitch”.
  • the result is output to the walking pattern correction means 826 as the final direction trajectory.
  • the walking pattern correction means 826 inputs the final z-direction trajectory, the final roll / pitch direction trajectory, and the non-control target component to correct the walking pattern.
  • the inverse kinematics computing means 813 performs inverse kinematics computation on the corrected walking pattern, and the inverse kinematics computing means 813 and later have already been described.
  • FIG. 43 is a flowchart showing overall control in landing control (that is, control obtained by adding landing control to basic control), and FIG. 44 is a flowchart showing details of landing control.
  • FIG. 43 shows a landing control step SS42a instead of the correction amount force calculation step SS42 of FIG.
  • the landing control operation is performed as shown in FIG. This includes an operation of adding a positive amount and a movement amount) and an operation of correcting a walking pattern for correcting a walking pattern with respect to the addition result. That is, when the landing control is included in the basic control, the correction amount power calculation step SS42 in FIG. 39 is merely replaced with the step SS42a in FIG.
  • step SS42a in FIG. 43 The landing control of step SS42a in FIG. 43 will be described with reference to FIG.
  • mode selection means 821 selects a control mode using the mode selection table based on gait flag F [n] output from walking pattern output means 811 (SS61). Mode selection is performed as shown in (Table 1).
  • the component dividing unit 822 determines whether each component of the walking pattern output from the walking pattern output unit 811 is a control target component or a non-control target component, performs landing control only for the control target component, The control target component is output to the walking pattern correction means 826 (SS62).
  • the components to be controlled are the components of z, roll ( ⁇ X), and pitch ( ⁇ y). In other words, the following processing is performed only for the components of z, touch, and pitch, and the following processing is not performed for x, y, and one. However, the walking pattern correction process in the final step SS72 is performed using all components.
  • the theoretical compliance movement amount calculation means 8231 calculates the theoretical compliance movement amount from the control target component and the movement allowance for landing road surface detection of (Equation 42) (see (Equation 46) and (Equation 48)). (SS63).
  • the non-linear compliance movement amount calculation means 824 calculates the non-linear compliance movement amount by inputting the floor reaction force in the z direction output from the six-axis force sensor 142 (Equation 5 1), (See Equation 53)) (SS64).
  • Theoretical compliance movement amount calculation means 8231 Theoretical compliance movement amount and nonlinear compliance movement amount calculation means 824 The deviation between the non-linear compliance movement amount from 824 is the compliance movement amount deviation calculation means 8232. Reference), and the trajectory correction amount calculation means 8233 is input (SS65).
  • the trajectory correction amount calculation means 8233 calculates the z-direction landing trajectory correction amount based on the compliance movement amount deviation and the control mode from the mode selection means 821 ((Equation 104), (Equation 106), (Equation 107). )) (SS66).
  • the z-direction adding means 8121 includes the z component of the walking pattern from the component dividing means 822 and Add the z-direction landing trajectory correction amount from the trajectory correction amount calculation means 8233 and the movement allowance for landing road surface detection stored in the memory (memory) 827 as the z-direction corrected trajectory.
  • the compliance adding means 8122 adds the nonlinear compliance movement amount from the non-linear compliance movement amount calculating means 824 and the z-direction corrected trajectory from the z-direction adding means 8 121 and outputs the result as the final z-direction trajectory (SS68). ).
  • the compliance movement amount calculation means 8251 uses (Equation 65) and (Equation 66) based on the torque in the roll direction and the pitch direction input from the 6-axis force sensor 142 to obtain (Equation 67). Calculate the amount of compliance movement in the direction of the pitch and the amount of movement in the pitch direction of (Equation 68) (SS69).
  • the trajectory correction amount calculation means 8252 uses the control mode from the mode selection means 821 based on the roll direction compliance movement amount and the pitch direction compliance movement amount from the compliance movement amount calculation means 8251 to correct the roll 'pitch direction trajectory. Calculate the amount (see (Equation 111) to (Equation 116)) (SS70).
  • the roll 'pitch direction adding means 8123 adds the roll' pitch component from the component dividing means 822 and the roll 'pitch direction trajectory correction amount from the trajectory correction amount calculating means 8252 to obtain the roll' pitch direction final trajectory.
  • gait pattern correction means 826 SS71
  • the walking pattern correction means 826 creates a new walking pattern by inputting the final trajectory in the z direction, the final trajectory in the roll / pitch direction, and the non-control target component (SS72).
  • the new gait pattern is processed after the inverse kinematics computation (steps SS43 to SS46 in Fig. 43).
  • a walking pattern creation device for creating a walking pattern of a biped robot including a leg portion and a waist portion including a foot portion, V, set a target zero moment point, calculate the moment compensation trajectory of the waist according to the set target zero moment point, and calculate from the origin of the waist coordinate system in the absolute coordinate system and the origin of the foot coordinate system in the absolute coordinate system
  • the position of the foot viewed from the waist coordinate system is calculated, and the waist coordinate system is calculated from the rotation matrix that represents the posture of the foot coordinate system viewed from the absolute coordinate system and the rotation matrix that represents the posture of the waist coordinate system viewed from the absolute coordinate system.
  • the rotation matrix that represents the posture of the foot coordinate system viewed from above is calculated, the posture of the foot is calculated from the rotation matrix that represents the posture of the foot coordinate system, and the walking path is calculated.
  • the gait flag which is the flag indicating the early or late stage of stance and the early or late stage of the swing leg, the moment at the target zero moment point is determined by the moment compensation trajectory of the waist of the biped robot device.
  • the parallel link mechanism Since it can approach zero, it is possible to stabilize the walking by preventing the biped robot from falling over, and it is possible to generate foot position and posture data as seen from the waist coordinate system Therefore, in the parallel link mechanism, it is not necessary to perform forward kinematic calculations that are difficult to perform in real time, and in the normal link mechanism, the length of the cylinder that expands and contracts the legs is controlled by reverse kinematic calculations that are easy in real time. It is possible to perform various controls using various correction data in inverse kinematics computation.
  • the line can be further stabilized, and further, control according to the state of the leg can be performed using the gait flag which is a flag indicating whether the first leg or the second leg is the first leg or the second leg.
  • the free leg Since a complicated control chart can be executed depending on the state of each leg, such as copying, returning, and maintaining the trajectory correction amount, the free leg generates a large floor reaction force, which causes ZMP, posture angle deviation and acceleration. Can be completed before the occurrence of a road surface, and can be controlled accurately according to the road surface conditions, such as slopes and unevenness, and can be accurately modeled as if a human is mounted. It is possible to walk on rough terrain even when it is difficult, and this control does not require a special sensor or mechanism including a gyro etc. ) Only It can be carried out.
  • trajectory setting means 212 for setting the trajectory of the foot and the initial trajectory of the waist
  • a target ZMP trajectory setting means for setting the trajectory of the target zero moment point based on the set trajectory of the foot and the initial trajectory of the waist 213
  • target ZMP moment calculation means 214 that calculates the moment around the target zero moment point due to leg movement and waist movement, and the lumbar moment compensation trajectory based on the calculated moment around the target zero moment point
  • Moment compensation trajectory calculation means 217 for calculating the error moment calculation means 217 for calculating the error moment at the target zero moment point based on the acceleration of each mass point calculated by the set foot trajectory and waist moment compensation trajectory
  • the calculated error moment becomes smaller than the predetermined moment Foot as seen from the waist coordinate system from the origin of the leg coordinate system in the absolute coordinate system and the origin of the waist coordinate system in the coordinate system Foot coordinates as seen from the waist coordinate system from the rotation matrix representing the posture of the foot coordinate system viewed from the absolute coordinate system and the rotation matrix representing the posture of the waist coordinate system viewed from the absolute coordinate system Foot position
  • Fourier transform means 215 that calculates a Fourier coefficient based on the sum of the moment around the target zero moment point calculated by the ZMP rotation moment calculation means and the calculated error moment, and a moment based on the calculated Fourier coefficient
  • the moment compensation trajectory calculation means 217 calculates the moment compensation trajectory of the waist by inverse Fourier transform based on the determined Fourier coefficient, and a Fourier coefficient determination means 216 for determining a Fourier coefficient for obtaining an approximate solution of the compensation trajectory.
  • the target ZMP moment calculation means 2141 is a target ZMP moment calculation means 2141 based on the lower limb movement that calculates the target ZMP rotation moment generated by the leg movement, and the target ZMP rotation moment generated by the waist movement.
  • the target Z MP known moment calculation means 2142 for calculating the waist the moment around the target zero moment point due to the leg motion and the waist motion can be accurately calculated.
  • the moment compensation trajectory can be accurately calculated.
  • the foot position / orientation calculation means 221 calculates the position of the foot viewed from the waist coordinate system from the origin of the waist coordinate system in the absolute coordinate system and the origin of the foot coordinate system in the absolute coordinate system.
  • Rotation matrix calculation means 2212 for calculating a rotation matrix representing the posture of the system, and rotation Since it has posture calculation means 2213 that calculates the posture of the foot based on the rotation matrix calculated by the matrix calculation means, it is possible to accurately generate the position and posture data of the foot as seen from the waist coordinate system force.
  • the length of the cylinder that expands and contracts the leg can be accurately controlled by using reverse kinematics, which is easy to calculate in real time, and various correction data can be used in reverse kinematics. Accurate control can be performed when this control is performed.
  • the leg part includes a leg part and a waist part.
  • the leg part has a cylinder that expands and contracts the leg part.
  • the waist part controls the whole and performs wireless communication with the walking pattern creation device described above.
  • a biped robot device having a control computer 7, and the control computer controls biped walking based on the walking pattern that also received the walking turn creation device force, and thereby the foot is compensated by the moment compensation trajectory of the waist.
  • the moment at the target zero moment point can be brought close to zero to stabilize walking, and the walking pattern can be received remotely via radio.
  • a biped robot that can be used for various purposes (for example, for rescue, nursing, dangerous work, etc.) If a riding part such as a chair is attached to the lumbar region to enable human boarding, a biped robot that can be used for various purposes (for example, for welfare and for movement) can be constructed easily and quickly. Furthermore, the length of the cylinder that expands and contracts the leg can be controlled by performing inverse kinematics computation on the foot position and posture data as seen from the waist coordinate system. Does not require forward kinematics calculations, which are difficult to calculate in real time, and the parallel link mechanism can control robot walking by inverse kinematics calculations that are easy to calculate in real time. And the amount of manipulating the position and posture of the lower back) and the foot position and posture dimensions can be added together to perform force inverse kinematics calculation, which further improves the walking of biped robots. It is possible to stably I spoon.
  • control computer 7 selects the walking pattern setting means 712 for receiving the walking pattern wirelessly and storing the walking pattern in the memory and the pattern used by the stored walking pattern controller for walking.
  • the walking start command means 714 for instructing the start of walking and the length of the cylinder is controlled by controlling the motor based on the command of the walking start command means.
  • the biped robot device can walk with high stability without being physically constrained by the computer force for creating the walking pattern in the walking state. Or you can walk with an appropriate walking pattern according to your condition (for example, light weight).
  • the motor control means 715 includes basic control means 810 that performs basic control, and landing control means 820 that causes the free leg to land on the road surface while mitigating the impact and imitates the foot on the road surface unevenness.
  • landing control can be performed according to the situation.
  • basic control the amount of calculations for the leg mechanism of the parallel mechanism is enormous. It is no longer necessary to perform forward kinematics, which is difficult to implement, and the link length (ie cylinder length) can be controlled in real time by inverse kinematics, which is easy to calculate.
  • the basic control means 810 includes a walking pattern output means 811 for outputting a target value of the foot position-posture calculated by the foot position / posture calculating means based on a command from the walking start command means
  • the leg mechanism of the normal mechanism includes the reverse kinematic calculation means 813 for calculating the length of the cylinder by reverse kinematic calculation of the target value of the foot position output from the walking pattern output means.
  • the landing control means 820 includes a mode selection means 821 for selecting a control mode such as control, maintenance, return, and compliance based on the gait flag, and a landing control target for the walking pattern component.
  • a control mode such as control, maintenance, return, and compliance
  • a landing control target for the walking pattern component.
  • the component dividing means 822 that divides the control target component and the non-control target component other than the control target component, and the nonlinear compliance movement amount that reduces the landing impact of the free leg.
  • Nonlinear complier calculated by! / Movement amount calculation means 824, z-direction trajectory correction amount calculation means 823 for calculating the vertical trajectory correction amount based on the non-linear compliance movement amount, and roll and pitch directions based on torque in the foot and pitch direction.
  • the amount of trajectory correction in the vertical direction and the trajectory correction amount for rolls and pitches are applied to uneven road surfaces and inclined road surfaces.
  • the free leg Because there is no need for a special sensor or mechanism including a gyro, only the force sensor for ZMP measurement is used, the free leg generates a large floor reaction force, and the deviation of ZMP and posture angle Before the acceleration occurs, the copying operation to the road surface shape can be completed, so that stable walking can be performed without falling over an irregular road surface, and an accurate model like a human-carrying biped robot It is possible to reliably realize walking on rough terrain where it is difficult to perform robots.
  • the z-direction trajectory correction amount calculating means 823 includes a theoretical compliance movement amount calculating means 8231 for calculating a theoretical compliance movement amount that alleviates the landing impact of the free leg on the ideal road surface based on the control target component.
  • the compliance movement amount deviation calculation means 8232 for calculating the landing road surface height error which is the difference between the theoretical compliance movement amount and the nonlinear compliance movement amount, and the control mode selected by the mode selection means and the landing road surface height error
  • the free leg Based on the trajectory correction amount calculation means 8233 that calculates the trajectory correction amount in the vertical direction based on it, the free leg generates a large floor reaction force against the unevenness of the road surface, generating ZMP, attitude angle deviation and acceleration
  • the copying operation to the road surface shape is completed and the vertical direction can be accurately handled. It can perform stable walking without tipping against, walking accurate Moderui spoon like a human riding type biped robot in rough terrain difficult robot further can be more reliably achieved.
  • the roll 'pitch trajectory correction amount calculating means 825 is adapted to perform roll and pitch at the foot.
  • the program is for executing each step of the walking pattern creation method shown in FIGS. 19 to 31, the above-mentioned walking pattern creation method is applied to a general-purpose computer at an arbitrary place and at an arbitrary time. Can be executed.
  • the walking pattern creation method can be executed on a general-purpose computer at an arbitrary place and at any time as long as the general-purpose computer reads the recording medium. Can be made.
  • the program for executing each step of the control method of the biped robot device shown in Figs. 34 to 36 and 38 to 44 provides the control of the biped robot device.
  • the general-purpose computer can execute the control method at an arbitrary place at an arbitrary time.
  • the biped walking robot apparatus can be controlled at any place for any time as long as the general-purpose computer reads the recording medium. Can be executed by a general-purpose computer.
  • FIG. 45 (a) is a functional block diagram showing motor control means 715 ′ of the biped robot device according to the second embodiment
  • FIG. 45 (b) is a functional block diagram showing posture angle compensation amount calculating means 833. It is.
  • reference numeral 810 denotes basic control means
  • 820 denotes landing control means, which are the same as those described in the first embodiment, and are denoted by the same reference numerals and description thereof is omitted.
  • Reference numeral 830 denotes estimated posture compensation control means for performing estimated posture compensation control. Estimated posture compensation The control means 830 includes a ZMP actual value calculation means 831 for calculating the ZMP actual value as well as the detected value force of the 6-axis force sensor 142 (see FIG. 40), and the calculated ZMP actual value and the ZMP set as the walking pattern.
  • ZMP deviation value calculating means 832 for calculating a deviation from the target value
  • posture angle compensation amount calculating means 833 for calculating a waist posture angle compensation amount estimated based on a value obtained by integrating the ZMP deviation value
  • landing control means Foot position / posture correction means for correcting the walking pattern corrected by 820 based on the calculated posture angle compensation amount.
  • the attitude angle compensation amount calculation means 833 includes zero setting means 8331 for setting (resetting) the integrated value of the ZMP deviation value to zero at a predetermined period, and a force at the time when the integrated value of the ZMP deviation value is set to zero.
  • Compensation amount calculation means 8334 for calculating a compensation amount and posture angle operation amount calculation means 8335 for calculating an actual posture angle operation amount of the waist based on the calculated posture angle compensation amount.
  • the operation of the motor control means 715 configured in this manner is the same as that described in the first embodiment except that the estimated posture compensation control is performed together with the landing control.
  • the operation of the estimated posture compensation control will be described below with reference to FIGS. 46 to 49.
  • FIG. 46 is a functional block diagram showing the estimated posture compensation control
  • FIG. 47 is an explanatory diagram for explaining the setting period by the zero setting means 8 331 and the operation period by the operation period setting means 8333
  • FIG. 48 is the estimated posture compensation.
  • 49 is a flowchart showing the operation of the control means 830
  • FIG. 49 is a flowchart showing the operation of the attitude angle compensation amount calculation means 833.
  • 826 is a walking pattern correction means
  • 813 is an inverse kinematics calculation means
  • 811 is a walking pattern output means
  • 142 is a six-axis motion sensor
  • 142a is a receiver board, which are described in the first embodiment. The same reference numerals are given, and the description is omitted.
  • 841a and 841b are preset periods set in predetermined lengths, and when transitioning from one setup period 841a to another setup period 841b, that is, at the start of the setup periods 841a and 84 lb,
  • the zero setting means 8331 sets the integral value of the ZMP deviation value described later to zero. That is, the integration is started and the force is set to a predetermined length 841a, 84 After lb has elapsed, the integral value is set to zero (reset).
  • 842a to 842g are operation periods set in advance by the operation period setting means 8333 as intermittent periods of a predetermined period in each of the setting periods 841a and 841b.
  • the second embodiment is different from the first embodiment in that an estimated posture compensation control step SS42b is added after the landing control step SS42a.
  • the estimated attitude compensation control operation is as follows. First, the ZMP actual measurement value calculation means 831 performs the coordinate of the origin of the 6-axis force sensor 142 output from the receiver board 142a and the measured force and moment ((number 77))), the actual measured value of ZMP is calculated by (Equation 76) (SS81). Next, the ZMP deviation value calculation means 832 is set as the ZMP actual value calculated by the ZMP actual value calculation means 831 and the walking pattern output from the walking pattern output means 811 (see FIG. 37). Calculate the deviation from the value (ZMP trajectory with lumbar force taken into account) (SS82).
  • the integral value calculating means 8332 of the posture angle compensation amount calculating means 833 calculates the integral value of the ZMP deviation value
  • the compensation amount calculating means 8334 calculates the posture angle compensation amount of the waist by (Equation 69) ( SS83).
  • the integral value calculation means 8332 has a ZMP deviation value at the start time of the operation periods 842a to 842g at the start time of the operation periods 842a to 842g. The integral value of is calculated.
  • the operation amount calculation means 8335 calculates the posture angle operation amount of the waist using (Equation 78) from the posture angle compensation amount calculated by the compensation amount calculation means 8334 (SS84).
  • the foot position / posture correction means 834 controls the position of the foot relative to the waist or the operation amount of the posture (foot position) so that the waist is operated by the calculated posture angle operation amount during the operation period 842a to 842g.
  • (Posture operation amount) is calculated (SS85), and the foot position / posture operation amount is corrected as a correction amount using the homogeneous transformation matrix for the walking pattern corrected by the landing control means 820,
  • the target value for the new foot position / posture is calculated (SS86).
  • the correction of the foot is performed on the position of the leg on the stance side 'posture data in the single leg support period, and on the position / posture data of both feet in the both leg support period. Will be done.
  • the determination of the single leg support period or the both leg support period is made based on the gait flag.
  • the walking pattern corrected by the foot position / posture operation amount is output to the inverse kinematics calculation means 813, and as in Embodiment 1, the inverse kinematics calculation, cylinder length data calculation, and motor angular displacement data are performed. Processing such as conversion to motor and motor drive is performed.
  • the estimated posture compensation control means 830 is calculated by the ZMP actual value calculation means 831 that calculates the ZMP actual value from the force component and the moment component at the foot.
  • ZMP deviation value calculation means 832 that calculates the ZMP deviation value, which is the deviation between the measured ZMP value and the ZMP target value, and the posture angle that calculates the posture angle compensation amount for the waist based on the integrated value of the ZMP deviation value Compensation amount calculation means 833 and foot position / posture correction means 834 for correcting the target position or position of the foot of the walking pattern by the foot position / posture operation amount based on the posture angle compensation amount.
  • Compensation amount calculation means 833 includes zero setting means 8331 for setting the integral value of the ZMP deviation value to zero at a predetermined cycle, and an integral for calculating the integral value of the momentary force when the integral value of the ZMP deviation value is set to zero. Value calculation means 8332 and the operation period during which the foot position or posture is corrected. By having the operation period setting means 8333 to be set and the compensation amount calculation means 8334 for calculating the attitude angle compensation amount based on the integrated value of the ZMP deviation value at the start of the operation period, the machine rigidity and the motor response deviation are obtained.
  • the present invention provides a biped walking robot apparatus having a parallel link mechanism for bipedal walking.
  • a device for creating a row pattern a biped walking mouth bot device that performs biped walking based on the walking pattern, a method for creating a walking pattern that performs biped walking, and a biped walking robot device that performs biped walking
  • the present invention relates to a control method, and a program and a recording medium for executing the walking pattern creating method and the biped walking robot apparatus control method, and causing the biped walking robot apparatus having a parallel link mechanism to perform stable biped walking. You can.

Landscapes

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

Abstract

　パラレルリンク機構の２足歩行ロボット装置に安定した２足歩行を行わせることができる歩行パターン作成装置を提供することを目的とする。 　足部において目標ゼロモーメントポイントを設定し、設定した目標ゼロモーメントポイントに応じて腰部のモーメント補償軌道を算出し、絶対座標系における腰座標系の原点と絶対座標系における足座標系の原点とから腰座標系から見た足部の位置を算出し、絶対座標系から見た足座標系の姿勢を表わす回転行列と絶対座標系から見た腰座標系の姿勢を表わす回転行列とから腰座標系から見た足座標系原点の姿勢を表わす回転行列を算出して足座標系原点の姿勢を表わす回転行列により足部の姿勢を算出し、歩行パターンにおける立脚前期か後期か及び遊脚前期か後期かを示すフラグである歩容フラグを決定し記憶する。

Description

明 細 書
歩行パターン作成装置、 2足歩行ロボット装置、歩行パターン作成方法、 2足歩行ロボット装置の制御方法、プログラムおよび記録媒体
技術分野
[0001] 本発明は、パラレルリンク機構を有する 2足歩行ロボット装置に 2足歩行を行わせる 歩行パターンを作成する装置、その歩行パターンに基づいて 2足歩行を行う 2足歩行 ロボット装置、 2足歩行を行わせる歩行パターンを作成する方法、および 2足歩行を 行う 2足歩行ロボット装置の制御方法、ならびに、その歩行パターン作成方法とその 2 足歩行ロボット装置の制御方法を実行させるためのプログラムおよび記録媒体に関 するものである。
背景技術
[0002] 従来、パラレルリンク機構を有するロボット装置としては、例えば (特許文献 1)に記 載されたものがあり、この文献においては、ユニットリンクを 3本用いたパラレルリンク 駆動機構について記載され、またノ、ンドリングを実現できることが記載されている。し かし、上記文献においては歩行については何ら記載されておらず、従って、ノラレル リンク機構を有する 2足歩行ロボット装置についても、また、このロボット装置の安定し た歩行にっ 、ても何ら記載されて 、な 、。
また、不整路面における 2足歩行の安定ィ匕の方法は、その観測量に ZMPや体幹 姿勢、上体加速度などを用いており、路面の凹凸をロボットが踏み込むことにより ZM Pや体幹姿勢、加速度偏差の急激な変化を観測し、ロボットの力学モデルに基づく 高精度な加速度制御を行う方法が一般的である。これらはロボット自体のモデルが比 較的正確に取得できる場合に非常に有効である力 人間を乗せて歩行する場合のよ うに正確なモデル化が困難なロボットでは適用が困難であった。ほかにもロボットの運 動が路面形状の影響を受ける以前に路面に対する適応動作を開始する手法の例も あるが、路面を検知するセンサや機構の搭載が必要であり、コストの面から不利であ つた o
特許文献 1 :特開 2001—121460号公報 発明の開示
発明が解決しょうとする課題
[0003] このように、人間が搭乗した場合等の正確なモデルィ匕が困難な場合であっても、安 定歩行が可能で歩行時の衝撃が少なぐ緩い傾斜面や路面凹凸に対して十分に対 応することができ、且つ、このような 2足歩行の安定ィ匕を主にソフトウェアによる処理で 実現可能なパラレルリンク機構の 2足歩行ロボット装置の出現が要求されていた。
[0004] 本発明は、上記従来の要求を充たすもので、パラレルリンク機構の 2足歩行ロボット 装置に安定した 2足歩行を行わせることができる歩行パターンを作成する歩行パター ン作成装置、安定した 2足歩行を行うことができるパラレルリンク機構の 2足歩行ロボ ット装置、パラレルリンク機構の 2足歩行ロボット装置に安定した 2足歩行を行わせる ことができる歩行パターンを作成する歩行パターン作成方法、パラレルリンク機構の 2 足歩行ロボット装置に安定した 2足歩行を行うことができる 2足歩行ロボット装置の制 御方法、ならびに、その歩行パターン作成方法とその 2足歩行ロボット装置の制御方 法を実行させるためのプログラムおよび記録媒体を提供することを目的とする。
課題を解決するための手段
[0005] 上記課題を解決するために本発明の歩行パターン作成装置は、足部を備えた脚 部と腰部とから成る 2足歩行ロボット装置の歩行パターンを作成する歩行パターン作 成装置であって、足部において目標ゼロモーメントポイントを設定し、設定した目標ゼ 口モーメントポイントに応じて腰部のモーメント補償軌道を算出し、絶対座標系におけ る腰座標系の原点と絶対座標系における足座標系の原点とから腰座標系カゝら見た 足部の位置を算出し、絶対座標系力 見た足座標系の姿勢を表わす回転行列と絶 対座標系から見た腰座標系の姿勢を表わす回転行列とから腰座標系カゝら見た足座 標系の姿勢を表わす回転行列を算出して足座標系の姿勢を表わす回転行列により 足部の姿勢を算出し、歩行パターンにおける立脚前期か後期か及び遊脚前期か後 期かを示すフラグである歩容フラグを決定し記憶する構成を備えている。
これにより、 ノラレルリンク機構の 2足歩行ロボット装置に安定した 2足歩行を行わせ る歩行パターンを作成する歩行パターン作成装置が得られる。
[0006] 上記課題を解決するために本発明の 2足歩行ロボット装置は、足部を備えた脚部と 腰部とから成り、脚部は脚部を伸縮するシリンダを有し、腰部は全体を制御すると共 に上記歩行パターン作成装置と無線通信を行う制御コンピュータを有する 2足歩行口 ボット装置であって、制御コンピュータは、歩行パターン作成装置から受信した歩行 パターンに基づ 、て 2足歩行の制御を行う構成を備えて 、る。
これにより、安定した 2足歩行を行うパラレルリンク機構の 2足歩行ロボット装置が得 られる。
[0007] 上記課題を解決するために本発明の歩行パターン作成方法は、足部を備えた脚 部と腰部とから成る 2足歩行ロボット装置の歩行パターンを作成する歩行パターン作 成方法であって、足部において目標ゼロモーメントポイントを設定し、設定した目標ゼ 口モーメントポイントに応じて腰部のモーメント補償軌道を算出し、絶対座標系におけ る腰座標系の原点と絶対座標系における足座標系の原点とから腰座標系カゝら見た 足部の位置を算出し、絶対座標系力 見た足座標系の姿勢を表わす回転行列と絶 対座標系から見た腰座標系の姿勢を表わす回転行列とから腰座標系カゝら見た足座 標系の姿勢を表わす回転行列を算出して足座標系の姿勢を表わす回転行列により 足部の姿勢を算出し、歩行パターンにおける立脚前期か後期か及び遊脚前期か後 期かを示すフラグである歩容フラグを決定し記憶する構成を備えている。
これにより、 ノラレルリンク機構の 2足歩行ロボット装置に安定した 2足歩行を行わせ る歩行パターンを作成する歩行パターン作成方法が得られる。
[0008] 上記課題を解決するために本発明の 2足歩行ロボット装置の制御方法は、足部を 備えた脚部と腰部とから成り、脚部は脚部を伸縮するシリンダを有し、腰部は全体を 制御すると共に上記歩行パターン作成装置と無線通信を行う制御コンピュータを有 する 2足歩行ロボット装置の制御方法であって、制御コンピュータは、歩行パターン 作成装置力 受信した歩行パターンに基づいて 2足歩行の制御を行う構成を備えて いる。
これにより、 ノラレルリンク機構の 2足歩行ロボット装置に安定した 2足歩行を行わせ る 2足歩行ロボット装置の制御方法が得られる。
[0009] 上記課題を解決するために本発明のプログラムは、コンピュータに上記歩行パター ン作成方法と上記 2足歩行ロボット装置の制御方法の各ステップを実行させるための プログラムである構成を備えて 、る。
これにより、上記歩行パターン作成方法と上記 2足歩行ロボット装置の制御方法を 実行させるためのプログラムが得られる。
[0010] 上記課題を解決するために本発明の記録媒体は、上記プログラムを記録したコン ピュータ読み取り可能な記録媒体である構成を備えている。
これにより、上記歩行パターン作成方法と上記 2足歩行ロボット装置の制御方法を 実行させるための記録媒体が得られる。
発明の効果
[0011] 本発明の請求項 1に記載の歩行パターン作成装置は、足部を備えた脚部と腰部と から成る 2足歩行ロボット装置の歩行パターンを作成する歩行パターン作成装置であ つて、足部において目標ゼロモーメントポイントを設定し、設定した目標ゼロモーメント ポイントに応じて腰部のモーメント補償軌道を算出し、絶対座標系における腰座標系 の原点と絶対座標系における足座標系の原点とから腰座標系から見た足部の位置 を算出し、絶対座標系から見た足座標系の姿勢を表わす回転行列と絶対座標系か ら見た腰座標系の姿勢を表わす回転行列とから腰座標系カゝら見た足座標系の姿勢 を表わす回転行列を算出して足座標系の姿勢を表わす回転行列により足部の姿勢 を算出し、歩行パターンにおける立脚前期か後期か及び遊脚前期か後期かを示す フラグである歩容フラグを決定し記憶することにより、 2足歩行ロボット装置の腰部の モーメント補償軌道により目標ゼロモーメントポイントにおけるモーメントをゼロに近づ けることができるので、 2足歩行ロボットの転倒を防止して歩行を安定ィ匕することがで き、また、腰座標系からみた足部の位置と姿勢のデータを生成することができるので 、 ノラレルリンク機構においてはリアルタイム演算が困難な順運動学演算を行う必要 がなぐパラレルリンク機構においてはリアルタイム演算が容易な逆運動学演算により 脚部を伸縮するシリンダの長さを制御することができ、逆運動学演算において種々の 補正データを用いて種々の制御を行うことができ、 2足歩行ロボットの歩行を更に安 定ィ匕することができ、さらに、立脚前期か後期か及び遊脚前期か後期かを示すフラグ である歩容フラグを用いて脚部の状態に応じた制御を行うことができるので、倣 ヽ動 作や復帰動作、軌道修正量維持など各脚の状態によって複雑な制御チャートを実行 させることができ、これにより遊脚が大きな床反力を発生し ZMPや姿勢角の偏差や 加速度が発生する以前に路面形状への倣い動作を完了することができ、路面状態た とえば傾斜面や凹凸に応じた制御を的確に行うことができ、人間を搭載する場合のよ うに正確なモデルィ匕が困難な場合にも不整地における歩行を実現することができ、さ らに、本制御はジャイロ等を含む特別なセンサや機構を必要とすることなく ZMP計測 用の力センサ（6軸カ覚センサ）のみを用いて行うことができるという有利な効果が得 られる。
[0012] 請求項 2に記載の歩行パターン作成装置は、足部の軌道と腰部の初期軌道を設定 する軌道設定手段と、設定した足部の軌道と腰部の初期軌道とに基づいて目標ゼロ モーメントポイントの軌道を設定する目標 ZMP軌道設定手段と、脚部の運動および 腰部の運動による目標ゼロモーメントポイント回りのモーメントを算出する目標 ZMP 回りモーメント算出手段と、算出した目標ゼロモーメントポイント回りのモーメントに基 づいて腰部のモーメント補償軌道を算出するモーメント補償軌道算出手段と、設定し た足部の軌道と腰部のモーメント補償軌道とにより算出された各質点の加速度に基 づいて目標ゼロモーメントポイントにおけるエラーモーメントを算出するエラーモーメ ント算出手段と、算出したエラーモーメントが所定モーメントよりも小さくなつたとき絶 対座標系における腰座標系の原点と絶対座標系における足座標系の原点とから腰 座標系から見た足部の位置を算出すると共に、絶対座標系から見た足座標系の姿 勢を表わす回転行列と絶対座標系から見た腰座標系の姿勢を表わす回転行列とか ら腰座標系から見た足座標系の姿勢を表わす回転行列を算出して足座標系の姿勢 を表わす回転行列により足部の姿勢を算出する足部位置姿勢算出手段と、歩容フラ グを決定し記憶する歩容フラグ決定手段とを有することにより、脚部の運動および腰 部の運動による目標ゼロモーメントポイント回りのモーメントがゼロの近傍か否かをェ ラーモーメントにより判定することができ、歩行パターンの設定において目標ゼロモー メントポイント回りのモーメントを限り無くゼロに近づけることができるので、安定性の極 めて高い 2足歩行を 2足歩行ロボット装置に行わせることができるという有利な効果が 得られる。
[0013] 請求項 3に記載の歩行パターン作成装置は、 ZMP回りモーメント算出手段におい て算出した目標ゼロモーメントポイント回りのモーメントと算出したエラーモーメントと の総和に基づいてフーリエ係数を算出するフーリエ変換手段と、算出したフーリエ係 数に基づいてモーメント補償軌道の近似解を求めるためのフーリエ係数を決定する フーリエ係数決定手段とを備え、モーメント補償軌道算出手段は、決定したフーリエ 係数に基づく逆フーリエ変換により腰部のモーメント補償軌道を算出することにより、 腰部のモーメント補償軌道を正確に算出することができるので、目標ゼロモーメントポ イント回りのモーメントを軌道が発散することなく迅速にゼロに近づけることができると いう有利な効果が得られる。
[0014] 請求項 4に記載の歩行パターン作成装置は、目標 ZMP回りモーメント算出手段は 、脚部の運動により生じる目標 ZMP回りモーメントを算出する下肢運動による目標 Z MP回りモーメント算出手段と、腰部の運動により生じる目標 ZMP回りモーメントを算 出する腰運動による目標 ZMP回り既知モーメント算出手段とを有することにより、脚 部の運動および腰部の運動による目標ゼロモーメントポイント回りのモーメントを正確 に算出することができるので、腰部のモーメント補償軌道を正確に算出することがで きると ヽぅ有利な効果が得られる。
[0015] 請求項 5に記載の歩行パターン作成装置は、足部位置姿勢算出手段は、絶対座 標系における腰座標系の原点と絶対座標系における足座標系の原点とから腰座標 系から見た足部の位置を算出する原点位置算出手段と、絶対座標系力 見た足座 標系の姿勢を表わす回転行列と絶対座標系から見た腰座標系の姿勢を表わす回転 行列とから腰座標系から見た足座標系の姿勢を表わす回転行列を算出する回転行 列算出手段と、回転行列算出手段で算出した回転行列により足部の姿勢を算出す る姿勢算出手段とを有することにより、腰座標系からみた足部の位置と姿勢のデータ を正確に生成することができるので、リアルタイム演算が容易な逆運動学演算を用い て脚部を伸縮するシリンダの長さを正確に制御することができ、逆運動学演算におい て種々の補正データを用いて種々の制御を行う場合に正確な制御を行うことができ ると ヽぅ有利な効果が得られる。
[0016] 請求項 6に記載の 2足歩行ロボット装置は、足部を備えた脚部と腰部とから成り、脚 部は脚部を伸縮するシリンダを有し、腰部は全体を制御すると共に請求項 1乃至 5の V、ずれか 1に記載の歩行パターン作成装置と無線通信を行う制御コンピュータを有 する 2足歩行ロボット装置であって、制御コンピュータは、歩行パターン作成装置から 受信した歩行パターンに基づいて 2足歩行の制御を行うことにより、腰部のモーメント 補償軌道により足部の目標ゼロモーメントポイントにおけるモーメントをゼロに近づけ て歩行を安定ィ匕することができると共に歩行パターンを無線を介して遠隔地でも受信 することができ、また、腰部に種々の上体 (体幹)を取り付け可能とすれば、種々の用 途 (たとえば救助用、介護用、危険作業用等）に適合する 2足歩行ロボットを迅速に 構築することができ、また、腰部にいす等の搭乗部を取り付けて人間の搭乗を可能と すれば、種々の用途 (たとえば福祉用、移動用）に適合する 2足歩行ロボットを容易 且つ迅速に構築することができ、さらに、腰座標系からみた足部の位置と姿勢のデー タに対して逆運動学演算を行って脚部を伸縮するシリンダの長さを制御することがで きるので、パラレルリンク機構においてはリアルタイム演算が困難な順運動学演算を 行う必要がなく、パラレルリンク機構においてはリアルタイム演算が容易な逆運動学 演算によりロボットの歩行を制御することができ、種々の補正データ（たとえばコンブラ ィアンス移動量や腰部の位置 ·姿勢の操作量)を足部の位置や姿勢の次元で足し合 わせて力 逆運動学演算を行うという容易な実装ができ、 2足歩行ロボットの歩行を 更に安定ィ匕することができるという有利な効果が得られる。
[0017] 請求項 7に記載の 2足歩行ロボット装置は、制御コンピュータは、歩行パターン作成 用コンピュータ力 歩行パターンを無線で受信してメモリに格納する歩行パターン設 定手段と、格納した歩行パターン力も歩行に使用するパターンを選択して歩行開始 を指令する歩行開始命令手段と、歩行開始命令手段の指令に基づ!、てモータを制 御してシリンダの長さを制御するモータ制御手段とを有することにより、歩行状態にお いて 2足歩行ロボット装置は歩行パターン作成用コンピュータ力 物理的な拘束を受 けることなく高安定な歩行を行うことができ、また、周囲または自己の状態 (たとえば搭 載重量の軽重状態）に応じた適切な歩行パターンで歩行することができると 、う有利 な効果が得られる。
[0018] 請求項 8に記載の 2足歩行ロボット装置は、モータ制御手段は、基本制御を行う基 本制御手段と、遊脚を路面に衝撃を緩和して着地させると共に路面の凹凸に足部を 倣わせる着地制御手段とを有することにより、基本制御に基づく基本動作を行うこと ができると共に、状況に応じて着地制御を行うことができ、基本制御においては、パラ レルメカニズムの脚機構にとっては計算量が膨大なために実施が困難な順運動学を 行う必要がなくなり、計算が容易な逆運動学によりリアルタイムにリンク長さ（つまりシリ ンダ長さ）を制御することができるので、足部の位置 '姿勢に関して種々の制御を行う ことができるようになり、また着地制御においては、遊脚が大きな床反力を発生し ZM Pや姿勢角の偏差や加速度が発生する以前に、路面形状への倣い動作を完了し、 不整路面に対して転倒することなく安定した歩行を行うことができ、人間搭乗型 2足 歩行ロボットのように正確なモデルィ匕が困難なロボットの不整地における歩行を実現 することができるという有利な効果が得られる。
[0019] 請求項 9に記載の 2足歩行ロボット装置は、基本制御手段は、足部位置姿勢算出 手段で算出した足部の位置 ·姿勢の目標値を歩行開始命令手段の指令に基づ!、て 出力する歩行パターン出力手段と、歩行パターン出力手段力も出力される足部の位 置'姿勢の目標値を逆運動学演算してシリンダの長さを算出する逆運動学演算手段 とを有することにより、パラレルメカニズムの脚機構にとっては計算量が膨大なために 実施が困難な順運動学を行う必要がなくなり、計算が容易な逆運動学によりリアルタ ィムにリンク長さを制御することができるので、足部の位置 '姿勢に関して種々の制御 を行うことができるという有利な効果が得られる。
[0020] 請求項 10に記載の 2足歩行ロボット装置は、着地制御手段は、歩容フラグに基づ いて制御、維持、復帰、コンプライアンス等の制御モードを選択するモード選択手段 と、歩行パターンの成分にぉ ヽて着地制御の対象となる成分である制御対象成分と 制御対象成分以外の非制御対象成分とを分割する成分分割手段と、遊脚の着地衝 撃を緩和する非線形コンプライアンス移動量を足部における床反力に基づ 、て算出 する非線形コンプライアンス移動量算出手段と、非線形コンプライアンス移動量に基 づいて上下方向の軌道修正量を算出する z方向軌道修正量算出手段と、足部にお けるロールおよびピッチ方向のトルクに基づいてロールおよびピッチの軌道修正量を 算出するロール'ピッチ軌道修正量算出手段と、歩行パターン出力手段力 出力さ れる歩行パターンと着地路面検知用移動許容量と非線形コンプライアンス移動量と 上下方向の軌道修正量とロール'ピッチ軌道修正量とに基づいて歩行パターンを修 正する歩行パターン修正手段とを有することにより、着地衝撃の緩和と振動の抑制に 対しては非線形コンプライアンス移動量を用いて対応することができ、路面の凹凸や 傾斜路面に対しては上下方向の軌道修正量およびロールおよびピッチの軌道修正 量を用いて対応することができるので、ジャイロを含め特別なセンサや機構を必要と せず、 ZMP計測用の力センサのみを用いて、遊脚が大きな床反力を発生し ZMPや 姿勢角の偏差や加速度が発生する以前に、路面形状への倣い動作を完了できるた め、不整路面に対して転倒することなく安定した歩行を行うことができ、人間搭乗型 2 足歩行ロボットのように正確なモデル化が困難なロボットの不整地における歩行を確 実に実現することができるという有利な効果が得られる。
[0021] 請求項 11に記載の 2足歩行ロボット装置は、 z方向軌道修正量算出手段は、理想 的水平平坦路面に衝撃や振動なく理想的に着地したときに発生するはずの理論コン プライアンス移動量を制御対象成分に基づいて算出する理論コンプライアンス移動 量算出手段と、理論コンプライアンス移動量と非線形コンプライアンス移動量との差 分である着地路面高さ誤差を算出するコンプライアンス移動量偏差算出手段と、モ ード選択手段で選択した制御モードと着地路面高さ誤差とに基づいて上下方向の軌 道修正量を算出する軌道修正量算出手段とを有することにより、路面の凹凸に対し て上下方向に正確に対応することができるので、ロール ·ピッチについて対応すれば 、不整路面に対して、遊脚が大きな床反力を発生し ZMPや姿勢角の偏差や加速度 が発生する以前に、路面形状への倣い動作を完了し、転倒することなく安定した歩 行を行うことができ、人間搭乗型 2足歩行ロボットのように正確なモデルィ匕が困難な口 ボットの不整地における歩行を更に一層確実に実現することができるという有利な効 果が得られる。
[0022] 請求項 12に記載の 2足歩行ロボット装置は、ロール'ピッチ軌道修正量算出手段は 、足部におけるロールおよびピッチ方向のトルクに基づ!/ヽてコンプライアンス移動量 を算出するコンプライアンス移動量算出手段と、コンプライアンス移動量に基づいて ロールおよびピッチ方向の軌道修正量を算出する軌道修正量算出手段とを有するこ とにより、路面の凹凸に対してロール'ピッチ軸まわりに正確に対応することができる ので、上下方向について対応すれば、不整路面に対して、遊脚が大きな床反力を発 生し ZMPや姿勢角の偏差や加速度が発生する以前に、路面形状への倣い動作を 完了し、転倒することなく安定した歩行を行うことができ、人間搭乗型 2足歩行ロボット のように正確なモデルィ匕が困難なロボットの不整地における歩行を更に一層確実に 実現することができるという有利な効果が得られる。
[0023] 請求項 13に記載の 2足歩行ロボット装置は、モータ制御手段は、着地制御手段に より修正した歩行パターンを推定した腰部の姿勢角補償量に基づいて補正する推定 姿勢補償制御を行う推定姿勢補償制御手段を備え、推定姿勢補償制御手段は、 Z
MP実測値を足部における力成分とモーメント成分とから算出する ZMP実測値算出 手段と、算出された ZMP実測値と ZMP目標値との偏差である ZMP偏差値を算出す る ZMP偏差値算出手段と、 ZMP偏差値を積分した値に基づいて腰部の姿勢角補 償量を算出する姿勢角補償量算出手段と、修正した歩行パターンにおける足部の位 置又は姿勢の目標値を姿勢角補償量に基づ 、て補正する足部位置姿勢補正手段 とを有することにより、腰部の姿勢角の累積誤差を推定し、推定した値に基づきそれ を補償して腰部と遊足側の足部を略水平に維持することができるので、正確な着地 制御を行うことができ、安定した歩行を行うことができ、人間搭乗型 2足歩行ロボットの ように正確なモデルィ匕が困難なロボットの不整地における歩行を更に一層確実に実 現することができるという有利な効果が得られる。また、 ZMP偏差値の積分値に基づ いて姿勢角補償量を算出するため、急激な変化を伴う補償動作を防止でき安定性に 優れるという有利な効果が得られる。また、姿勢角センサを搭載する必要がなぐ ZM P測定用の力センサ (6軸カ覚センサ)だけで姿勢補償制御ができ、部品点数や製造 コストの増加を防止できるという有利な効果が得られる。
[0024] 請求項 14に記載の 2足歩行ロボット装置は、姿勢角補償量算出手段は、所定の周 期で ZMP偏差値の積分値をゼロに設定するゼロ設定手段と、 ZMP偏差値の積分値 がゼロに設定された時点からの積分値を算出する積分値算出手段と、足部の位置又 は姿勢の補正操作を行う操作期間を設定する操作期間設定手段と、操作期間の開 始時点における ZMP偏差値の積分値に基づいて姿勢角補償量を算出する補償量 算出手段とを有することにより、所定の周期で ZMP偏差値の積分値をゼロに設定 (リ セット)することで実際の姿勢角偏差との積分誤差の増加を防止でき、姿勢角補償量 を推定することができるという有利な効果が得られる。また、所定の操作期間を設定 するため姿勢角誤差が過度に累積するのを防止でき、安定した歩行を行うことができ ると ヽぅ有利な効果が得られる。
[0025] 請求項 15に記載の歩行パターン作成方法は、足部を備えた脚部と腰部とから成る 2足歩行ロボット装置の歩行パターンを作成する歩行パターン作成方法であって、足 部において目標ゼロモーメントポイントを設定し、設定した目標ゼロモーメントポイント に応じて腰部のモーメント補償軌道を算出し、絶対座標系における腰座標系の原点 と絶対座標系における足座標系の原点とから腰座標系力 見た足部の位置を算出し 、絶対座標系カゝら見た足座標系の姿勢を表わす回転行列と絶対座標系カゝら見た腰 座標系の姿勢を表わす回転行列とから腰座標系から見た足座標系の姿勢を表わす 回転行列を算出して足座標系の姿勢を表わす回転行列により足部の姿勢を算出し、 歩行パターンにおける立脚前期か後期力及び遊脚前期か後期かを示すフラグであ る歩容フラグを決定し記憶することにより、 2足歩行ロボット装置の腰部のモーメント補 償軌道により目標ゼロモーメントポイントにおけるモーメントをゼロに近づけることがで きるので、 2足歩行ロボットの転倒を防止して歩行を安定ィ匕することができ、また、腰 座標系からみた足部の位置と姿勢のデータを生成することができるので、リアルタイ ム演算が困難な順運動学演算を行う必要がなぐリアルタイム演算が容易な逆運動 学演算により脚部を伸縮するシリンダの長さを制御することができ、逆運動学演算に おいて種々の補正データを用いて種々の制御を行うことができ、 2足歩行ロボットの 歩行を更に安定ィ匕することができ、さらに、立脚前期か後期か及び遊脚前期か後期 かを示すフラグである歩容フラグを用いて脚部の状態に応じた制御を行うことができ るため、倣い動作や復帰動作、軌道修正量維持など各脚の状態によって複雑な制 御チャートを実行させることができ、路面状態たとえば傾斜面や凹凸に応じた制御を 行うことができ、人間を搭載する場合のように正確なモデルィ匕が困難な場合にも不整 地における歩行を実現することができるという有利な効果が得られる。
[0026] 請求項 16に記載の歩行パターン作成方法は、足部の軌道と腰部の初期軌道を設 定する軌道設定ステップと、設定した足部の軌道と腰部の初期軌道とに基づいて目 標ゼロモーメントポイントの軌道を設定する目標 ZMP軌道設定ステップと、脚部の運 動および腰部の運動による目標ゼロモーメントポイント回りのモーメントを算出する目 標 ZMP回りモーメント算出ステップと、算出した目標ゼロモーメントポイント回りのモ 一メントに基づいて腰部のモーメント補償軌道を算出するモーメント補償軌道算出ス テツプと、設定した足部の軌道と腰部のモーメント補償軌道とにより算出された各質 点の加速度に基づいて目標ゼロモーメントポイントにおけるエラーモーメントを算出 するエラーモーメント算出ステップと、算出したエラーモーメントが所定モーメントより も小さくなつたとき絶対座標系における腰座標系の原点と絶対座標系における足座 標系の原点とから腰座標系力も見た足部の位置を算出すると共に、絶対座標系から 見た足座標系の姿勢を表わす回転行列と絶対座標系力 見た腰座標系の姿勢を表 わす回転行列とから腰座標系カゝら見た足座標系の姿勢を表わす回転行列を算出し て足座標系の姿勢を表わす回転行列により足部の姿勢を算出する足部位置姿勢算 出ステップと、歩容フラグを決定し記憶する歩容フラグ決定ステップとを有することに より、脚部の運動および腰部の運動による目標ゼロモーメントポイント回りのモーメント がゼロの近傍力否かをエラーモーメントにより判定することができ、歩行パターンの設 定において目標ゼロモーメントポイント回りのモーメントを限り無くゼロに近づけること ができるので、安定性の極めて高い 2足歩行を 2足歩行ロボット装置に行わせること ができると!、う有利な効果が得られる。
[0027] 請求項 17に記載の歩行パターン作成方法は、 ZMP回りモーメント算出ステップに おいて算出した目標ゼロモーメントポイント回りのモーメントと算出したエラーモーメン トとの総和に基づいてフーリエ係数を算出するフーリエ変換ステップと、算出したフー リエ係数に基づいてモーメント補償軌道の近似解を求めるためのフーリエ係数を決 定するフーリエ係数決定ステップとを備え、モーメント補償軌道算出ステップにお ヽ ては、決定したフーリエ係数に基づく逆フーリエ変換により腰部のモーメント補償軌 道を算出することにより、腰部のモーメント補償軌道を正確に算出することができるの で、 目標ゼロモーメントポイント回りのモーメントを軌道が発散することなく迅速にゼロ に近づけることができるという有利な効果が得られる。
[0028] 請求項 18に記載の歩行パターン作成方法は、 目標 ZMP回りモーメント算出ステツ プは、脚部の運動により生じる目標 ZMP回りモーメントを算出する下肢運動による目 標 ZMP回りモーメント算出ステップと、腰部の運動により生じる目標 ZMP回りモーメ ントを算出する腰運動による目標 ZMP回り既知モーメント算出ステップとを有すること により、脚部の運動および腰部の運動による目標ゼロモーメントポイント回りのモーメ ントを正確に算出することができるので、腰部のモーメント補償軌道を正確に算出す ることができるという有利な効果が得られる。
[0029] 請求項 19に記載の歩行パターン作成方法は、足部位置姿勢算出ステップは、絶 対座標系における腰座標系の原点と絶対座標系における足座標系の原点とから腰 座標系から見た足部の位置を算出する原点位置算出ステップと、絶対座標系から見 た足座標系の姿勢を表わす回転行列と絶対座標系力 見た腰座標系の姿勢を表わ す回転行列とから腰座標系から見た足座標系の姿勢を表わす回転行列を算出する 回転行列算出ステップと、回転行列算出ステップで算出した回転行列により足部の 姿勢を算出する姿勢算出ステップとを有することにより、 2足歩行ロボット装置の制御 方法において腰座標系力 みた足部の位置と姿勢のデータを正確に生成することが できるので、リアルタイム演算が容易な逆運動学演算を用いて脚部を伸縮するシリン ダの長さを正確に制御することができ、逆運動学演算において種々の補正データを 用いて種々の制御を行う場合に正確な制御を行うことができると!/、う有利な効果が得 られる。
[0030] 請求項 20に記載の 2足歩行ロボット装置の制御方法は、足部を備えた脚部と腰部 とから成り、脚部は脚部を伸縮するシリンダを有し、腰部は全体を制御すると共に請 求項 1乃至 5のいずれか 1に記載の歩行パターン作成装置と無線通信を行う制御コ ンピュータを有する 2足歩行ロボット装置の制御方法であって、制御コンピュータは、 歩行パターン作成装置力も受信した歩行パターンに基づいて 2足歩行の制御を行う ことにより、腰部のモーメント補償軌道により足部の目標ゼロモーメントポイントにおけ るモーメントをゼロに近づけて歩行を安定ィ匕することができると共に歩行パターンを無 線を介して遠隔地でも受信することができ、また、腰部に種々の上体 (体幹)を取り付 け可能とすれば、種々の用途 (たとえば救助用、介護用、危険作業用等）に適合する 2足歩行ロボットを迅速に構築することができ、また、腰部にいす等の搭乗部を取り付 けて人間の搭乗を可能とすれば、種々の用途 (たとえば福祉用、移動用）に適合する
2足歩行ロボットを容易且つ迅速に構築することができ、さらに、腰座標系からみた足 部の位置と姿勢のデータに対して逆運動学演算を行って脚部を伸縮するシリンダの 長さを制御することができるので、パラレルリンク機構にぉ 、てはリアルタイム演算が 困難な順運動学演算を行う必要がなく、パラレルリンク機構においてはリアルタイム 演算が容易な逆運動学演算によりロボットの歩行を制御することができ、種々の補正 データ（たとえばコンプライアンス移動量や腰部の位置 ·姿勢の操作量)を足部の位 置や姿勢の次元で足し合わせて力 逆運動学演算を行うという容易な実装ができ、 2 足歩行ロボットの歩行を更に安定ィ匕することができるという有利な効果が得られる。
[0031] 請求項 21に記載の 2足歩行ロボット装置の制御方法は、制御コンピュータは、歩行 パターン作成用コンピュータ力 歩行パターンを無線で受信してメモリに格納する歩 行パターン設定ステップと、格納した歩行パターン力 歩行に使用するパターンを選 択して歩行開始を指令する歩行開始命令ステップと、歩行開始命令ステップにおけ る指令に基づいてモータを制御してシリンダの長さを制御するモータ制御ステップと を有することにより、歩行状態において 2足歩行ロボット装置は歩行パターン作成用 コンピュータ力も物理的な拘束を受けることなく高安定な歩行を行うことができ、また、 周囲または自己の状態 (たとえば搭載重量の軽重状態）に応じた適切な歩行パター ンで歩行することができるという有利な効果が得られる。
[0032] 請求項 22に記載の 2足歩行ロボット装置の制御方法は、モータ制御ステップは、基 本制御を行う基本制御ステップと、遊脚を路面に衝撃を緩和して着地させると共に路 面の凹凸に足部を倣わせる着地制御ステップとを有することにより、基本制御に基づ く基本動作を行うことができると共に、状況に応じて着地制御を行うことができ、基本 制御においては、パラレルメカニズムの脚機構にとっては計算量が膨大なために実 施が困難な順運動学を行う必要がなくなり、計算が容易な逆運動学によりリアルタイ ムにリンク長さを制御することができるので、足部の位置 '姿勢に関して種々の制御を 行うことができるようになり、また着地制御においては、遊脚が大きな床反力を発生し ZMPや姿勢角の偏差や加速度が発生する以前に、路面形状への倣い動作を完了 し、不整路面に対して転倒することなく安定した歩行を行うことができ、人間搭乗型 2 足歩行ロボットのように正確なモデルィ匕が困難なロボットの不整地における歩行を実 現することができるという有利な効果が得られる。
[0033] 請求項 23に記載の 2足歩行ロボット装置の制御方法は、基本制御ステップは、足 部位置姿勢算出手段で算出した足部の位置，姿勢の目標値を歩行開始命令ステツ プにおける指令に基づいて出力する歩行パターン出力ステップと、歩行パターン出 力ステップ力 出力される足部の位置 '姿勢の目標値を逆運動学演算してシリンダの 長さを算出する逆運動学演算ステップとを有することにより、パラレルメカニズムの脚 機構にとっては計算量が膨大なために実施が困難な順運動学を行う必要がなくなり 、計算が容易な逆運動学によりリアルタイムにリンク長さを制御することができるので、 足部の位置 *姿勢に関して種々の制御を行うことができるという有利な効果が得られ る。
[0034] 請求項 24に記載の 2足歩行ロボット装置の制御方法は、着地制御ステップは、歩 容フラグに基づいて制御、維持、復帰、コンプライアンス等の制御モードを選択する モード選択ステップと、歩行パターンの成分にぉ 、て着地制御の対象となる成分で ある制御対象成分と制御対象成分以外の非制御対象成分とを分割する成分分割ス テツプと、遊脚の着地衝撃を緩和する非線形コンプライアンス移動量を足部における 床反力に基づ 、て算出する非線形コンプライアンス移動量算出ステップと、非線形コ ンプライアンス移動量に基づいて上下方向の軌道修正量を算出する z方向軌道修正 量算出ステップと、足部におけるロールおよびピッチ方向のトルクに基づいてロール およびピッチの軌道修正量を算出するロール ·ピッチ軌道修正量算出ステップと、歩 行パターン出力ステップにおいて出力される歩行パターンと着地路面検知用移動許 容量と非線形コンプライアンス移動量と上下方向の軌道修正量とロール'ピッチ軌道 修正量とに基づ!/、て歩行パターンを修正する歩行パターン修正ステップとを有するこ とにより、着地衝撃の緩和と振動の抑制に対しては非線形コンプライアンス移動量を 用いて対応することができ、路面の凹凸や傾斜路面に対しては上下方向の軌道修正 量およびロールおよびピッチの軌道修正量を用いて対応することができるので、ジャ イロを含む特別なセンサや機構を必要とせず、 ZMP計測用の力センサのみを用い て、遊脚が大きな床反力を発生し ZMPや姿勢角の偏差や加速度が発生する以前に 、路面形状へのならい動作を完了できるため、不整路面に対して転倒することなく安 定した歩行を行うことができ、人間搭乗型 2足歩行ロボットのように正確なモデルィ匕が 困難なロボットの不整地における歩行を確実に実現することができるという有利な効 果が得られる。
[0035] 請求項 25に記載の 2足歩行ロボット装置の制御方法は、 z方向軌道修正量算出ス テツプは、理想的水平平坦路面に衝撃や振動なく理想的に着地したときに発生する はずの理論コンプライアンス移動量を制御対象成分に基づいて算出する理論コンプ ライアンス移動量算出ステップと、理論コンプライアンス移動量と非線形コンプライア ンス移動量との差分である着地路面高さ誤差を算出するコンプライアンス移動量偏 差算出ステップと、モード選択ステップで選択した制御モードと着地路面高さ誤差と に基づいて上下方向の軌道修正量を算出する軌道修正量算出ステップとを有するこ とにより、路面の凹凸に対して上下方向に正確に対応することができるので、ロール · ピッチについて対応すれば、不整路面に対して、遊脚が大きな床反力を発生し ZMP や姿勢角の偏差や加速度が発生する以前に、路面形状へのならい動作を完了し、 転倒することなく安定した歩行を行うことができ、人間搭乗型 2足歩行ロボットのように 正確なモデルィヒが困難なロボットの不整地における歩行を更に一層確実に実現する ことができるという有利な効果が得られる。
[0036] 請求項 26に記載の 2足歩行ロボット装置の制御方法は、ロール'ピッチ軌道修正量 算出手段は、足部におけるロールおよびピッチ方向のトルクに基づいてコンプライア ンス移動量を算出するコンプライアンス移動量算出手段と、コンプライアンス移動量 に基づいてロールおよびピッチ方向の軌道修正量を算出する軌道修正量算出手段 とを有することにより、路面の凹凸に対してロール'ピッチ軸まわりに正確に対応する ことができるので、上下方向について対応すれば、不整路面に対して、遊脚が大きな 床反力を発生し ZMPや姿勢角の偏差や加速度が発生する以前に、路面形状への ならい動作を完了し、転倒することなく安定した歩行を行うことができ、人間搭乗型 2 足歩行ロボットのように正確なモデル化が困難なロボットの不整地における歩行を更 に一層確実に実現することができるという有利な効果が得られる。
[0037] 請求項 27に記載の 2足歩行ロボット装置の制御方法は、モータ制御ステップは、着 地制御ステップにおいて修正した歩行パターンを推定した腰部の姿勢角補償量に基 づ!、て補正する推定姿勢補償制御を行う推定姿勢補償制御ステップを備え、推定姿 勢補償制御ステップは、 ZMP実測値を足部における力成分とモーメント成分とから 算出する ZMP実測値算出ステップと、算出された ZMP実測値と ZMP目標値との偏 差である ZMP偏差値を算出する ZMP偏差値算出ステップと、 ZMP偏差値を積分し た値に基づいて腰部の姿勢角補償量を算出する姿勢角補償量算出ステップと、修 正した歩行パターンにおける足部の位置又は姿勢の目標値を姿勢角補償量に基づ いて補正する足部姿勢操作量加算ステップとを有することにより、腰部の姿勢角の累 積誤差を推定し、推定した値に基づきそれを補償して腰部と遊脚側の足部を略水平 に維持することができるので、正確な着地制御を行うことができ、安定した歩行を行う ことができ、人間搭乗型 2足歩行ロボットのように正確なモデルィ匕が困難なロボットの 不整地における歩行を更に一層確実に実現することができるという有利な効果が得 られる。また、 ZMP偏差値の積分値に基づいて姿勢角補償量を算出するため、急激 な変化を伴う補償動作を防止でき安定性に優れると ヽぅ有利な効果が得られる。また 、姿勢角センサを搭載する必要がなぐ ZMP測定用の力センサ（6軸カ覚センサ)だ けで姿勢補償制御ができ、部品点数や製造コストの増加を防止できると 、う有利な効 果が得られる。
[0038] 請求項 28に記載の 2足歩行ロボット装置の制御方法は、姿勢角補償量算出ステツ プは、所定の周期で ZMP偏差値の積分値をゼロに設定するゼロ設定ステップと、 Z MP偏差値の積分値がゼロに設定された時点からの積分値を算出する積分値算出 ステップと、足部の位置又は姿勢の補正操作を行う操作期間を設定する操作期間設 定ステップと、操作期間の開始時点における ZMP偏差値の積分値に基づいて姿勢 角補償量を算出する補償量算出ステップとを有することにより、所定の周期で ZMP 偏差値の積分値をゼロに設定 (リセット)することで実際の姿勢角偏差との積分誤差 の増加を防止でき、姿勢角補償量を推定することができるという有利な効果が得られ る。また、所定の操作期間を設定するため姿勢角誤差が過度に累積するのを防止で き、安定した歩行を行うことができると 、う有利な効果が得られる。
[0039] 請求項 29に記載のプログラムは、コンピュータに請求項 15乃至 19のいずれか 1に 記載の歩行パターン作成方法の各ステップを実行させるためのプログラムであること により、請求項 15乃至 19のいずれか 1に記載の歩行パターン作成方法を任意の場 所で任意の時間に汎用コンピュータに実行させることができるという有利な効果が得 られる。
[0040] 請求項 30に記載の記録媒体は、請求項 29に記載のプログラムを記録したコンビュ ータ読み取り可能な記録媒体であることにより、汎用コンピュータで記録媒体を読み 取りさえすれば、請求項 15乃至 19のいずれか 1に記載の歩行パターン作成方法を 任意の場所で任意の時間に汎用コンピュータに実行させることができるという有利な 効果が得られる。
[0041] 請求項 31に記載のプログラムは、コンピュータに請求項 20乃至 28のいずれか 1に 記載の 2足歩行ロボット装置の制御方法の各ステップを実行させるためのプログラム であることにより、請求項 20乃至 28のいずれ力 1に記載の 2足歩行ロボット装置の制 御方法を任意の場所で任意の時間に汎用コンピュータに実行させることができるとい う有利な効果が得られる。
[0042] 請求項 32に記載の記録媒体は、請求項 31に記載のプログラムを記録したコンビュ ータ読み取り可能な記録媒体であることにより、汎用コンピュータで記録媒体を読み 取りさえすれば、請求項 20乃至 28のいずれか 1に記載の 2足歩行ロボット装置の制 御方法を任意の場所で任意の時間に汎用コンピュータに実行させることができるとい う有利な効果が得られる。
図面の簡単な説明
[0043] [図 l] (a)支持多角形を示す説明図、座標系とベクトルの定義を示す説明図
[図 2] (a)腰座標系および足座標系を示す模式図、（b)腰座標系および足座標系を 示す模式図
[図 3] (a)理論コンプライアンス移動量の説明図、（b)理論コンプライアンス移動量の 説明図、（c)非線形の説明図、（d)非線形コンプライアンス移動量の説明図、（e)非 線形コンプライアンス移動量の説明図、（f)コンプライアンスを示す説明図
[図 4]本発明の実施の形態 1による歩行パターン作成装置と 2足歩行ロボット装置の 全体構成を示すブロック図 [図 5]主に腰部を示すブロック図
圆 6]主に脚部を示すブロック図
圆 7]外部電源部を示すブロック図
[図 8]歩行パターン作成用コンピュータを示すブロック図
[図 9]外部スィッチを示すブロック図
[図 10]主に制御用コンピュータを示すブロック図
[図 11]主に電源部を示すブロック図
[図 12]主にスィッチ回路を示すブロック図
[図 13]主にブレーキ制御部を示すブロック図
[図 14]主にモータ制御部を示すブロック図
[図 15]主にシリンダを示すブロック図
圆 16]主に足底部 (脚部）を示すブロック図
[図 17]主に直動部を示すブロック図
[図 18]歩行パターン作成用コンピュータの CPUにおける機能実現手段を示すブロッ ク図
[図 19]歩行パターン作成用コンピュータの CPU動作を示すフローチャート
[図 20] (a)歩行パターン作成用コンピュータの CPU動作を示すフローチャート、 (b) 歩行パターン作成用コンピュータの CPU動作を示すフローチャート、（c)歩行パター ン作成用コンピュータの CPU動作を示すフローチャート
[図 21]歩行パターン作成用コンピュータの CPU動作を示すフローチャート
[図 22]歩行パターン作成用コンピュータの CPU動作を示すフローチャート
[図 23]歩行パターン作成用コンピュータの CPU動作を示すフローチャート
[図 24]歩行パターン作成用コンピュータの CPU動作を示すフローチャート
[図 25]歩行パターン作成用コンピュータの CPU動作を示すフローチャート
[図 26]歩行パターン作成用コンピュータの CPU動作を示すフローチャート
[図 27]歩行パターン作成用コンピュータの CPU動作を示すフローチャート
[図 28]歩行パターン作成用コンピュータの CPU動作を示すフローチャート
[図 29]歩行パターン作成用コンピュータの CPU動作を示すフローチャート [図 30]歩行パターン作成用コンピュータの CPU動作を示すフローチャート
[図 31]歩行パターン作成用コンピュータの CPU動作を示すフローチャート 圆 32]足座標系の原点位置の算出、回転行列の算出および姿勢角算出について説 明する説明図
[図 33]制御用コンピュータの CPUにおける機能実現手段を示すブロック図
[図 34]制御用コンピュータの CPU動作を示すフローチャート
[図 35]制御用コンピュータの CPU動作を示すフローチャート
[図 36]制御用コンピュータの CPU動作を示すフローチャート
[図 37]モータ制御手段を示す機能ブロック図
[図 38]モータ制御手段とモータ駆動装置を示すブロック図
圆 39]基本制御手段の動作を示すフローチャート
圆 40]着地制御を示す機能的ブロック図
[図 41] (a)各ブロックを示すブロック説明図、（b)各ブロックを示すブロック説明図、（c )各ブロックを示すブロック説明図
[図 42] (a)各ブロックを示すブロック説明図、（b)各ブロックを示すブロック説明図 [図 43]基本制御に着地制御を加えた制御を示すフローチャート
[図 44]着地制御を示すフローチャート
[図 45] (a)実施の形態 2における 2足歩行ロボット装置のモータ制御手段を示す機能 ブロック図 (b)姿勢角補償量算出手段を示す機能ブロック図
圆 46]推定姿勢補償制御を示す機能的ブロック図
圆 47]ゼロ設定手段による設定期間と操作期間設定手段による操作期間を説明する 説明図
圆 48]推定姿勢補償制御手段の動作を示すフローチャート
圆 49]姿勢角補償量算出手段の動作を示すフローチャート
[図 50] (a)実施の形態 1における 2足歩行ロボットの下半身モジュールの模式斜視図 、 (b)実施の形態 1における 2足歩行ロボットの下半身モジュールの模式平面図
[図 51]2足歩行ロボットの下半身モジュールの斜視図
[図 52]直動リンクの要部斜視図 [図 53] (a)直動リンクの要部側面図、（b) (a)の A— A線の要部矢視断面図 [図 54]ベース部側受動ジョイントの要部斜視図
[図 55]足部側受動ジョイントの要部斜視図
[図 56]足部側受動ジョイントの要部縦断面図
符号の説明
1 ロボット部（2足歩行ロボット装置）
2 歩行パターン作成用コンピュータ (歩行パターン作成装置） 3 外部電源部
4 外部スィッチ
5 腰部
6 脚部
7 制御用コンピュータ
9 モータ制御部
10 ブレーキ制御部
11 電源部
12 スィッチ回路
13a 右脚部
13b 左脚部
14 足底部 (足部）
14a 右足底部 (右足部）
14b 左足底部 (左足部）
21、 71 CPU
22、 72 メモリ
23、 73 ハードディスク
24 入力装置
25 表示装置
26、 74 インタフェース §
27、 75 LANカード 28、 76 無線 LAN
31 ACZDCコンバータ
41 ブレーキ制御スィッチ
42 サーボ ONスィッチ
43 モータ端子短絡スィッチ
77 DZAコンバータ
78 6軸カ覚センサレシーバボード
79 ノ レスカウンタ
101 半導体リレー
111 48Vニッケル水素バッテリ
112 ノ ッテリ Z電源切替部
113 電源スィッチ
114、 115、 116、 117 DC— DCコンノ一 121、 122、 123 排他的論理和
141 足底板
142、 203b, 204b 6軸カ覚センサ
151 ロータリエンコーダ
152 DCサーボモータ
153 保持ブレーキ
154 直動部
155 フォトマイクロセンサ
171 動力伝達部
172 すべりねじ
173 ナット
174 リニアガイド
C1〜C12 シリンダ
D1〜D12 DCサーボドライバ
201、 201c 2足歩行ロボットの下半身モジ 202 ベース部
203 右足部
203a 固定板
204 左足部
204a 固定板
205、 205' 、 205a, 205a' 、 205b, 205b' 、 205c, 205c' 直動リンク
206、 206a, 206a' 、 206b, 206b' 、 206c, 206c' ベース部側受動ジョイ ント
207、 207a, 207b, 207c 足部側受動ジョイン卜
208、 208a, 208b, 208c 回動受動ジョイン卜
209 制御装置部
209' 制御用コンピュータ
211 設定データ入力手段
212 軌道設定手段
213 目標 ZMP軌道設定手段
214 目標 ZMP回りモーメント算出手段
215 フーリエ変換手段
216 フーリエ係数決定手段
217 モーメント補償軌道算出手段
218 質点加速度算出手段
219 エラーモーメント算出手段
220 エラーモーメント判定手段
221 足部位置姿勢算出手段
222 歩容フラグ決定手段
223 ファイル出力手段
231 ノ ッテリ
232 モータ駆動用回路部
235 直動リンク 236 ベース部側受動ジョイント
237 足部側受動ジョイント
241 保持ケーシング
242 アウターチューブ部
243 インナーロッド部
244、 244, ジョイント結合部
245 ロッドレール部
246 レールガイド
247 初期位置センサ
248 モータ
248a モータホルダ
249 モータ先端軸部
250 保持ブレーキ
251 ロータリエンコーダ
252 溝付きベルト
254 雄螺子軸部
254a 雄螺子先端軸部
255 雌螺子ナット部
256a, 256b ストツバ
261 ベース部側上部継手
262 上部継手軸
263 ベース部側下部継手
264 下部継手軸
265 連結回動部
271 第 1の足側上部継手
271a, 271b, 273a, 273b 側部軸支板
273 第 2の足側上部継手
272a ボーノレベアリング 275 ボール保持部
275a ボーノレ収容咅
276a, 276b 継手軸
277 止めナット
278 足側基部
279 ボール継手軸
279a ボーノレ咅
COM 比較部
300 比例積分部
301 換算部
400 モータ駆動装置
711 電源投入手段
712 歩行パターン設定手段
713 シリンダ初期位置設定手段
714 歩行開始命令手段
715, 715' モータ制御手段
716 終了判定手段
810 基本制御手段
811 歩行パターン出力手段
812 補正量加算手段
813 逆運動学演算手段
814 データ変換手段
815 初期位置微調整手段
816 シリンダ駆動装置駆動手段
820 着地制御手段
821 モード選択手段
822 成分分割手段
823 z方向着地軌道修正量算出 824 非線形コンプライアンス移動量算出手段
825 ロール'ピッチ方向軌道修正量算出手段
826 歩行パターン修正手段
8121 z方向加算手段
8122 コンプライアンス加算手段
8123 ロール'ピッチ方向加算手段
8231 理論コンプライアンス移動量算出手段
8232 コンプライアンス移動量偏差算出手段
8233、 8252 軌道修正量算出手段
8251 コンプライアンス移動量算出手段
830 推定姿勢補償制御手段
831 ZMP実測値算出手段
832 ZMP偏差値算出手段
833 姿勢角補償量算出手段
834 足部位置姿勢補正手段
8331 ゼロ設定手段
8332 積分値算出手段
8333 操作期間設定手段
8334 補償量算出手段
8335 操作量算出手段
841a, 841b 設定期間
842a〜842g 操作期間
発明を実施するための最良の形態
本発明の請求項 1に記載の歩行パターン作成装置は、足部を備えた脚部と腰部と から成る 2足歩行ロボット装置の歩行パターンを作成する歩行パターン作成装置であ つて、足部において目標ゼロモーメントポイントを設定し、設定した目標ゼロモーメント ポイントに応じて腰部のモーメント補償軌道を算出し、絶対座標系における腰座標系 の原点と絶対座標系における足座標系の原点とから腰座標系から見た足部の位置 を算出し、絶対座標系から見た足座標系の姿勢を表わす回転行列と絶対座標系か ら見た腰座標系の姿勢を表わす回転行列とから腰座標系カゝら見た足座標系の姿勢 を表わす回転行列を算出して足座標系の姿勢を表わす回転行列により足部の姿勢 を算出し、歩行パターンにおける立脚前期か後期か及び遊脚前期か後期かを示す フラグである歩容フラグを決定し記憶することとしたものである。
この構成により、以下の作用を有する。
(1) 2足歩行ロボット装置の腰部のモーメント補償軌道により目標ゼロモーメントボイ ントにおけるモーメントをゼロに近づけることができるので、 2足歩行ロボットの転倒を 防止して歩行を安定ィ匕することができる。
(2)腰座標系からみた足部の位置と姿勢のデータを生成することができるので、パラ レルリンク機構においてはリアルタイム演算が困難な順運動学演算を行う必要がなく 、 ノ ラレルリンク機構においてはリアルタイム演算が容易な逆運動学演算により脚部 を伸縮するシリンダの長さを制御することができる。
(3)逆運動学演算にお!、て種々の補正データを用いて種々の制御を行うことができ 、 2足歩行ロボットの歩行を更に安定ィ匕することができる。
(4)立脚前期か後期力及び遊脚前期か後期かを示すフラグである歩容フラグを用い て脚部の状態に応じた制御を行うことができるので、倣い動作や復帰動作、軌道修 正量維持など各脚の状態によって複雑な制御チャートを実行させることができ、これ により遊脚が大きな床反力を発生し ZMPや姿勢角の偏差や加速度が発生する以前 に路面形状への倣い動作を完了することができ、路面状態たとえば傾斜面や凹凸に 応じた制御を的確に行うことができ、人間を搭載する場合のように正確なモデルィ匕が 困難な場合にも不整地における歩行を実現することができる。
請求項 2に記載の歩行パターン作成装置は、請求項 1に記載の歩行パターン作成 装置において、足部の軌道と腰部の初期軌道を設定する軌道設定手段と、設定した 足部の軌道と腰部の初期軌道とに基づいて目標ゼロモーメントポイントの軌道を設定 する目標 ZMP軌道設定手段と、脚部の運動および腰部の運動による目標ゼロモー メントポイント回りのモーメントを算出する目標 ZMP回りモーメント算出手段と、算出し た目標ゼロモーメントポイント回りのモーメントに基づいて腰部のモーメント補償軌道 を算出するモーメント補償軌道算出手段と、設定した足部の軌道と腰部のモーメント 補償軌道とにより算出された各質点の加速度に基づいて目標ゼロモーメントポイント におけるエラーモーメントを算出するエラーモーメント算出手段と、算出したエラーモ 一メントが所定モーメントよりも小さくなつたとき絶対座標系における腰座標系の原点 と絶対座標系における足座標系の原点とから腰座標系力 見た足部の位置を算出 すると共に、絶対座標系カゝら見た足座標系の姿勢を表わす回転行列と絶対座標系 力も見た腰座標系の姿勢を表わす回転行列とから腰座標系カゝら見た足座標系の姿 勢を表わす回転行列を算出して足座標系の姿勢を表わす回転行列により足部の姿 勢を算出する足部位置姿勢算出手段と、歩容フラグを決定し記憶する歩容フラグ決 定手段とを有することとしたものである。
この構成により、請求項 1の作用に加え、以下の作用を有する。
(1)脚部の運動および腰部の運動による目標ゼロモーメントポイント回りのモーメント がゼロの近傍力否かをエラーモーメントにより判定することができ、歩行パターンの設 定において目標ゼロモーメントポイント回りのモーメントを限り無くゼロに近づけること ができるので、安定性の極めて高い 2足歩行を 2足歩行ロボット装置に行わせること ができる。
[0047] 請求項 3に記載の歩行パターン作成装置は、請求項 2に記載の歩行パターン作成 装置において、 ZMP回りモーメント算出手段において算出した目標ゼロモーメントポ イント回りのモーメントと算出したエラーモーメントとの総和に基づいてフーリエ係数を 算出するフーリエ変換手段と、算出したフーリエ係数に基づいてモーメント補償軌道 の近似解を求めるためのフーリエ係数を決定するフーリエ係数決定手段とを備え、モ 一メント補償軌道算出手段は、決定したフーリエ係数に基づく逆フーリエ変換により 腰部のモーメント補償軌道を算出することとしたものである。
この構成により、請求項 2の作用に加え、以下の作用を有する。
(1)腰部のモーメント補償軌道を正確に算出することができるので、目標ゼロモーメン トポイント回りのモーメントを迅速にゼロに近づけることができる。
[0048] 請求項 4に記載の歩行パターン作成装置は、請求項 2または 3に記載の歩行バタ ーン作成装置において、目標 ZMP回りモーメント算出手段は、脚部の運動により生 じる目標 ZMP回りモーメントを算出する下肢運動による目標 ZMP回りモーメント算出 手段と、腰部の運動により生じる目標 ZMP回りモーメントを算出する腰運動による目 標 ZMP回り既知モーメント算出手段とを有することとしたものである。
この構成により、請求項 2または 3の作用に加え、以下の作用を有する。 (1)脚部の運動および腰部の運動による目標ゼロモーメントポイント回りのモーメント を正確に算出することができるので、腰部のモーメント補償軌道を正確に算出するこ とがでさる。
[0049] 請求項 5に記載の歩行パターン作成装置は、請求項 2乃至 4のいずれか 1に記載 の歩行パターン作成装置において、足部位置姿勢算出手段は、絶対座標系におけ る腰座標系の原点と絶対座標系における足座標系の原点とから腰座標系カゝら見た 足部の位置を算出する原点位置算出手段と、絶対座標系から見た足座標系の姿勢 を表わす回転行列と絶対座標系から見た腰座標系の姿勢を表わす回転行列とから 腰座標系から見た足座標系の姿勢を表わす回転行列を算出する回転行列算出手 段と、回転行列算出手段で算出した回転行列により足部の姿勢を算出する姿勢算 出手段とを有することとしたものである。
この構成により、請求項 2乃至 4のいずれか 1の作用にカ卩え、以下の作用を有する。 (1)腰座標系からみた足部の位置と姿勢のデータを正確に生成することができるの で、リアルタイム演算が容易な逆運動学演算を用いて脚部を伸縮するシリンダの長さ を正確に制御することができ、逆運動学演算において種々の補正データを用いて種 々の制御を行う場合に正確な制御を行うことができる。
[0050] 請求項 6に記載の 2足歩行ロボット装置は、足部を備えた脚部と腰部とから成り、脚 部は脚部を伸縮するシリンダを有し、腰部は全体を制御すると共に請求項 1乃至 5の V、ずれか 1に記載の歩行パターン作成装置と無線通信を行う制御コンピュータを有 する 2足歩行ロボット装置であって、制御コンピュータは、歩行パターン作成装置から 受信した歩行パターンに基づいて 2足歩行の制御を行うこととしたものである。
この構成により、以下の作用を有する。
(1)腰部のモーメント補償軌道により足部の目標ゼロモーメントポイントにおけるモー メントをゼロに近づけて歩行を安定ィ匕することができる。 (2)歩行パターンを無線を介して遠隔地でも受信することができる。
(3)腰部に種々の上体 (体幹)を取り付け可能とすれば、種々の用途 (たとえば救助 用、介護用、危険作業用等）に適合する 2足歩行ロボットを迅速に構築することができ 、また、腰部にいす等の搭乗部を取り付けて人間の搭乗を可能とすれば、種々の用 途 (たとえば福祉用、移動用）に適合する 2足歩行ロボットを容易且つ迅速に構築す ることがでさる。
(4)腰座標系からみた足部の位置と姿勢のデータに対して逆運動学演算を行って脚 部を伸縮するシリンダの長さを制御することができるので、ノ ラレルリンク機構にぉ ヽ てはリアルタイム演算が困難な順運動学演算を行う必要がなぐパラレルリンク機構 においてはリアルタイム演算が容易な逆運動学演算によりロボットの歩行を制御する ことができる。
(5)種々の補正データ（たとえばコンプライアンス移動量や腰部の位置 ·姿勢の操作 量)を足部の位置や姿勢の次元で足し合わせて力 逆運動学演算を行うという容易 な実装ができ、 2足歩行ロボットの歩行を更に安定ィ匕することができる。
[0051] 請求項 7に記載の 2足歩行ロボット装置は、請求項 6に記載の 2足歩行ロボット装置 において、制御コンピュータは、歩行パターン作成用コンピュータ力 歩行パターン を無線で受信してメモリに格納する歩行パターン設定手段と、格納した歩行パターン 力も歩行に使用するパターンを選択して歩行開始を指令する歩行開始命令手段と、 歩行開始命令手段の指令に基づいてモータを制御してシリンダの長さを制御するモ ータ制御手段とを有することとしたものである。
この構成により、請求項 6の作用に加え、以下の作用を有する。
(1)歩行状態において 2足歩行ロボット装置は歩行パターン作成用コンピュータから 物理的な拘束を受けることなく高安定な歩行を行うことができ、また、周囲または自己 の状態 (たとえば搭載重量の軽重状態）に応じた適切な歩行パターンで歩行すること ができる。
[0052] 請求項 8に記載の 2足歩行ロボット装置は、請求項 7に記載の 2足歩行ロボット装置 において、モータ制御手段は、基本制御を行う基本制御手段と、遊脚を路面に衝撃 を緩和して着地させると共に路面の凹凸に足部を倣わせる着地制御手段とを有する こととしたものである。
この構成により、請求項 7の作用に加え、以下の作用を有する。
(1)基本制御に基づく基本動作を行うことができると共に、状況に応じて着地制御を 行うことができる。
(2)基本制御においては、パラレルメカニズムの脚機構にとっては計算量が膨大なた めに実施が困難な順運動学を行う必要がなくなり、計算が容易な逆運動学によりリア ルタイムにリンク長さ（つまりシリンダ長さ）を制御することができるので、足部の位置' 姿勢に関して種々の制御を行うことができるようになる。
(3)着地制御においては、遊脚が大きな床反力を発生し ZMPや姿勢角の偏差や加 速度が発生する以前に、路面形状への倣い動作を完了し、不整路面に対して転倒 することなく安定した歩行を行うことができ、人間搭乗型 2足歩行ロボットのように正確 なモデルィ匕が困難なロボットの不整地における歩行を実現することができる。
[0053] 請求項 9に記載の 2足歩行ロボット装置は、請求項 8に記載の 2足歩行ロボット装置 において、基本制御手段は、足部位置姿勢算出手段で算出した足部の位置 '姿勢 の目標値を歩行開始命令手段の指令に基づいて出力する歩行パターン出力手段と 、歩行パターン出力手段力 出力される足部の位置，姿勢の目標値を逆運動学演算 してシリンダの長さを算出する逆運動学演算手段とを有することとしたものである。 この構成により、請求項 8の作用に加え、以下の作用を有する。
(1)パラレルメカニズムの脚機構にとっては計算量が膨大なために実施が困難な順 運動学を行う必要がなくなり、計算が容易な逆運動学によりリアルタイムにリンク長さ を制御することができるので、足部の位置 '姿勢に関して種々の制御を行うことができ る。
[0054] 請求項 10に記載の 2足歩行ロボット装置は、請求項 8または 9に記載の 2足歩行口 ボット装置において、着地制御手段は、歩容フラグに基づいて制御、維持、復帰、コ ンプライアンス等の制御モードを選択するモード選択手段と、歩行パターンの成分に おいて着地制御の対象となる成分である制御対象成分と制御対象成分以外の非制 御対象成分とを分割する成分分割手段と、遊脚の着地衝撃を緩和する非線形コンプ ライアンス移動量を足部における床反力に基づいて算出する非線形コンプライアンス 移動量算出手段と、非線形コンプライアンス移動量に基づいて上下方向の軌道修正 量を算出する z方向軌道修正量算出手段と、足部におけるロールおよびピッチ方向 のトルクに基づ!/、てロールおよびピッチの軌道修正量を算出するロール ·ピッチ軌道 修正量算出手段と、歩行パターン出力手段力も出力される歩行パターンと着地路面 検知用移動許容量と非線形コンプライアンス移動量と上下方向の軌道修正量とロー ル 'ピッチ軌道修正量とに基づいて歩行パターンを修正する歩行パターン修正手段 とを有することとしたものである。
この構成により、請求項 8または 9の作用に加え、以下の作用を有する。
(1)着地衝撃の緩和と振動の抑制に対しては非線形コンプライアンス移動量を用い て対応することができ、路面の凹凸や傾斜路面に対しては上下方向の軌道修正量 およびロールおよびピッチの軌道修正量を用いて対応することができる。
(2)ジャイロを含め特別なセンサや機構を必要とせず、 ZMPや計測用の力センサの みを用いて着地制御を行うことができ、部品点数や製造コストの増加を防止できる。
(3)遊脚が大きな床反力を発生し ZMPや姿勢角の偏差や加速度が発生する以前に 、路面形状への倣い動作を完了できるため、不整路面に対して転倒することなく安定 した歩行を行うことができ、人間搭乗型 2足歩行ロボットのように正確なモデルィ匕が困 難なロボットの不整地における歩行を確実に実現することができる。
請求項 11に記載の 2足歩行ロボット装置は、請求項 10に記載の 2足歩行ロボット装 置において、 z方向軌道修正量算出手段は、理想的水平平坦路面に衝撃や振動な く理想的に着地したときに発生するはずの理論コンプライアンス移動量を制御対象 成分に基づいて算出する理論コンプライアンス移動量算出手段と、理論コンプライア ンス移動量と非線形コンプライアンス移動量との差分である着地路面高さ誤差を算 出するコンプライアンス移動量偏差算出手段と、モード選択手段で選択した制御モ ードと着地路面高さ誤差とに基づいて上下方向の軌道修正量を算出する軌道修正 量算出手段とを有することとしたものである。
この構成により、請求項 10の作用に加え、以下の作用を有する。
(1)路面の凹凸に対して上下方向に正確に対応することができるので、ロール'ピッ チについて対応すれば、不整路面に対して、遊脚が大きな床反力を発生し ZMPや 姿勢角の偏差や加速度が発生する以前に、路面形状への倣い動作を完了し、転倒 することなく安定した歩行を行うことができ、人間搭乗型 2足歩行ロボットのように正確 なモデルィ匕が困難なロボットの不整地における歩行を更に一層確実に実現すること ができる。
[0056] 請求項 12に記載の 2足歩行ロボット装置は、請求項 10に記載の 2足歩行ロボット装 置において、ロール'ピッチ軌道修正量算出手段は、足部におけるロールおよびピッ チ方向のトルクに基づ!/ヽてコンプライアンス移動量を算出するコンプライアンス移動 量算出手段と、コンプライアンス移動量に基づいてロールおよびピッチ方向の軌道修 正量を算出する軌道修正量算出手段とを有することとしたものである。
この構成により、請求項 10の作用に加え、以下の作用を有する。
(1)路面の凹凸に対してロール'ピッチ軸まわりに正確に対応することができるので、 上下方向について対応すれば、不整路面に対して、遊脚が大きな床反力を発生し Z MPや姿勢角の偏差や加速度が発生する以前に、路面形状への倣い動作を完了し 、転倒することなく安定した歩行を行うことができ、人間搭乗型 2足歩行ロボットのよう に正確なモデルィ匕が困難なロボットの不整地における歩行を更に一層確実に実現 することができるという作用 ·効果を有する。
[0057] 請求項 13に記載の 2足歩行ロボット装置は、請求項 8乃至 12のいずれか 1に記載 の 2足歩行ロボット装置において、モータ制御手段は、着地制御手段により修正した 歩行パターンを推定した腰部の姿勢角補償量に基づいて補正する推定姿勢補償制 御を行う推定姿勢補償制御手段を備え、推定姿勢補償制御手段は、 ZMP実測値を 足部における力成分とモーメント成分とから算出する ZMP実測値算出手段と、算出 された ZMP実測値と ZMP目標値との偏差である ZMP偏差値を算出する ZMP偏差 値算出手段と、 ZMP偏差値を積分した値に基づいて腰部の姿勢角補償量を算出す る姿勢角補償量算出手段と、修正した歩行パターンにおける足部の位置又は姿勢 の目標値を姿勢角補償量に基づ!、て補正する足部位置姿勢補正手段とを有するこ ととしたものである。
この構成により、請求項 8乃至 12のいずれか 1の作用にカ卩え、以下の作用を有する (1)機械剛性やモータの応答偏差等に起因し着地制御における軌道修正により生じ る腰部の姿勢角の累積誤差を ZMP実測値より推定し、推定した値に基づきそれを補 償して腰部と遊脚側の足部を略水平に維持することができるので、着地制御時の足 部による不整地面のセンシング精度の低下等を補い、正確な着地制御を行うことが でき、安定した歩行を行うことができる。
(2) ZMP偏差値の積分値に基づいて姿勢角補償量を算出するため、急激な変化を 伴う補償動作を防止でき安定性に優れる。特に、着地制御により急激な姿勢角の変 化を防止できるので、これと推定姿勢補償制御を併せることでより信頼性の高 、歩行 制御を行うことができる。
(3)腰部の姿勢角を測定するための姿勢角センサを搭載する必要がなぐ部品点数 や製造コストの増加を防止できる。
[0058] 請求項 14に記載の 2足歩行ロボット装置は、請求項 13に記載の 2足歩行ロボット装 置において、姿勢角補償量算出手段は、所定の周期で ZMP偏差値の積分値をゼロ に設定するゼロ設定手段と、 ZMP偏差値の積分値がゼロに設定された時点力 の 積分値を算出する積分値算出手段と、足部の位置又は姿勢の補正操作を行う操作 期間を設定する操作期間設定手段と、操作期間の開始時点における ZMP偏差値の 積分値に基づいて姿勢角補償量を算出する補償量算出手段とを有することとしたも のである。
この構成により、請求項 13の作用に加え、以下の作用を有する。
(1)所定の周期で ZMP偏差値の積分値をゼロに設定 (リセット)することにより、実際 の姿勢角偏差との積分誤差の増加を防止できる。
(2)所定の操作期間を設定するため姿勢角誤差が過度に累積するのを防止できる。
[0059] 請求項 15に記載の歩行パターン作成方法は、足部を備えた脚部と腰部とから成る
2足歩行ロボット装置の歩行パターンを作成する歩行パターン作成方法であって、足 部において目標ゼロモーメントポイントを設定し、設定した目標ゼロモーメントポイント に応じて腰部のモーメント補償軌道を算出し、絶対座標系における腰座標系の原点 と絶対座標系における足座標系の原点とから腰座標系力 見た足部の位置を算出し 、絶対座標系カゝら見た足座標系の姿勢を表わす回転行列と絶対座標系カゝら見た腰 座標系の姿勢を表わす回転行列とから腰座標系から見た足座標系の姿勢を表わす 回転行列を算出して足座標系の姿勢を表わす回転行列により足部の姿勢を算出し、 歩行パターンにおける立脚前期か後期力及び遊脚前期か後期かを示すフラグであ る歩容フラグを決定し記憶することとしたものである。
この構成により、以下の作用を有する。
(1) 2足歩行ロボット装置の腰部のモーメント補償軌道により目標ゼロモーメントボイ ントにおけるモーメントをゼロに近づけることができるので、 2足歩行ロボットの転倒を 防止して歩行を安定ィ匕することができる。
(2)腰座標系からみた足部の位置と姿勢のデータを生成することができるので、リア ルタイム演算が困難な順運動学演算を行う必要がなぐリアルタイム演算が容易な逆 運動学演算により脚部を伸縮するシリンダの長さを制御することができる。
(3)逆運動学演算にお!、て種々の補正データを用いて種々の制御を行うことができ 、 2足歩行ロボットの歩行を更に安定ィ匕することができる。
(4)立脚前期か後期力及び遊脚前期か後期かを示すフラグである歩容フラグを用い て脚部の状態に応じた制御を行うことができるため、倣い動作や復帰動作、軌道修 正量維持など各脚の状態によって複雑な制御チャートを実行させることができ、路面 状態たとえば傾斜面や凹凸に応じた制御を行うことができ、人間を搭載する場合のよ うに正確なモデルィ匕が困難な場合にも不整地における歩行を実現することができる。 請求項 16に記載の歩行パターン作成方法は、請求項 15に記載の歩行パターン作 成方法において、足部の軌道と腰部の初期軌道を設定する軌道設定ステップと、設 定した足部の軌道と腰部の初期軌道とに基づいて目標ゼロモーメントポイントの軌道 を設定する目標 ZMP軌道設定ステップと、脚部の運動および腰部の運動による目標 ゼロモーメントポイント回りのモーメントを算出する目標 ZMP回りモーメント算出ステツ プと、算出した目標ゼロモーメントポイント回りのモーメントに基づいて腰部のモーメン ト補償軌道を算出するモーメント補償軌道算出ステップと、設定した足部の軌道と腰 部のモーメント補償軌道とにより算出された各質点の加速度に基づいて目標ゼロモ 一メントポイントにおけるエラーモーメントを算出するエラーモーメント算出ステップと、 算出したエラーモーメントが所定モーメントよりも小さくなつたとき絶対座標系における 腰座標系の原点と絶対座標系における足座標系の原点とから腰座標系から見た足 部の位置を算出すると共に、絶対座標系から見た足座標系の姿勢を表わす回転行 列と絶対座標系カゝら見た腰座標系の姿勢を表わす回転行列とから腰座標系から見 た足座標系の姿勢を表わす回転行列を算出して足座標系の姿勢を表わす回転行列 により足部の姿勢を算出する足部位置姿勢算出ステップと、歩容フラグを決定し記憶 する歩容フラグ決定ステップとを有することとしたものである。
この構成により、請求項 15の作用に加え、以下の作用を有する。
(1)脚部の運動および腰部の運動による目標ゼロモーメントポイント回りのモーメント がゼロの近傍力否かをエラーモーメントにより判定することができ、歩行パターンの設 定において目標ゼロモーメントポイント回りのモーメントを限り無くゼロに近づけること ができるので、安定性の極めて高い 2足歩行を 2足歩行ロボット装置に行わせること ができる。
[0061] 請求項 17に記載の歩行パターン作成方法は、請求項 16に記載の歩行パターン作 成方法において、 ZMP回りモーメント算出ステップにおいて算出した目標ゼロモーメ ントポイント回りのモーメントと算出したエラーモーメントとの総和に基づいてフーリエ 係数を算出するフーリエ変換ステップと、算出したフーリエ係数に基づいてモーメント 補償軌道の近似解を求めるためのフーリエ係数を決定するフーリエ係数決定ステツ プとを備え、モーメント補償軌道算出ステップにおいては、決定したフーリエ係数に 基づく逆フーリエ変換により腰部のモーメント補償軌道を算出することとしたものであ る。
この構成により、請求項 16の作用に加え、以下の作用を有する。
(1)腰部のモーメント補償軌道を正確に算出することができるので、 目標ゼロモーメン トポイント回りのモーメントを軌道が発散することなく迅速にゼロに近づけることができ る。
[0062] 請求項 18に記載の歩行パターン作成方法は、請求項 16または 17に記載の歩行 ノ ターン作成方法において、 目標 ZMP回りモーメント算出ステップは、脚部の運動 により生じる目標 ZMP回りモーメントを算出する下肢運動による目標 ZMP回りモーメ ント算出ステップと、腰部の運動により生じる目標 ZMP回りモーメントを算出する腰運 動による目標 ZMP回り既知モーメント算出ステップとを有することとしたものである。 この構成により、請求項 16または 17の作用に加え、以下の作用を有する。 (1)脚部の運動および腰部の運動による目標ゼロモーメントポイント回りのモーメント を正確に算出することができるので、腰部のモーメント補償軌道を正確に算出するこ とがでさる。
[0063] 請求項 19に記載の歩行パターン作成方法は、請求項 16乃至 18のいずれか 1に記 載の歩行パターン作成方法において、足部位置姿勢算出ステップは、絶対座標系 における腰座標系の原点と絶対座標系における足座標系の原点とから腰座標系か ら見た足部の位置を算出する原点位置算出ステップと、絶対座標系から見た足座標 系の姿勢を表わす回転行列と絶対座標系から見た腰座標系の姿勢を表わす回転行 列とから腰座標系から見た足座標系の姿勢を表わす回転行列を算出する回転行列 算出ステップと、回転行列算出ステップで算出した回転行列により足部の姿勢を算 出する姿勢算出ステップとを有することとしたものである。
この構成により、請求項 16乃至 18のいずれか 1の作用に加え、以下の作用を有す る。
(1) 2足歩行ロボット装置の制御方法において腰座標系力 みた足部の位置と姿勢 のデータを正確に生成することができるので、リアルタイム演算が容易な逆運動学演 算を用いて脚部を伸縮するシリンダの長さを正確に制御することができ、逆運動学演 算において種々の補正データを用いて種々の制御を行う場合に正確な制御を行うこ とがでさる。
[0064] 請求項 20に記載の 2足歩行ロボット装置の制御方法は、足部を備えた脚部と腰部 とから成り、脚部は脚部を伸縮するシリンダを有し、腰部は全体を制御すると共に請 求項 1乃至 5のいずれか 1に記載の歩行パターン作成装置と無線通信を行う制御コ ンピュータを有する 2足歩行ロボット装置の制御方法であって、制御コンピュータは、 歩行パターン作成装置力も受信した歩行パターンに基づいて 2足歩行の制御を行う こととしたものである。
この構成により、以下の作用を有する。
(1)腰部のモーメント補償軌道により足部の目標ゼロモーメントポイントにおけるモー メントをゼロに近づけて歩行を安定ィ匕することができる .
(2)歩行パターンを無線を介して遠隔地でも受信することができる。
(3)腰部に種々の上体 (体幹)を取り付け可能とすれば、種々の用途 (たとえば救助 用、介護用、危険作業用等）に適合する 2足歩行ロボットを迅速に構築することができ 、また、腰部にいす等の搭乗部を取り付けて人間の搭乗を可能とすれば、種々の用 途 (たとえば福祉用、移動用）に適合する 2足歩行ロボットを容易且つ迅速に構築す ることがでさる。
(4)腰座標系からみた足部の位置と姿勢のデータに対して逆運動学演算を行って脚 部を伸縮するシリンダの長さを制御することができるので、ノ ラレルリンク機構にぉ ヽ てはリアルタイム演算が困難な順運動学演算を行う必要がなぐパラレルリンク機構 においてはリアルタイム演算が容易な逆運動学演算によりロボットの歩行を制御する ことができる。
(5)種々の補正データ（たとえばコンプライアンス移動量や腰部の位置 ·姿勢の操作 量)を足部の位置や姿勢の次元で足し合わせて力 逆運動学演算を行うという容易 な実装ができ、 2足歩行ロボットの歩行を更に安定ィ匕することができる。
[0065] 請求項 21に記載の 2足歩行ロボット装置の制御方法は、請求項 20に記載の 2足歩 行ロボット装置の制御方法において、制御コンピュータは、歩行パターン作成用コン ピュータカも歩行パターンを無線で受信してメモリに格納する歩行パターン設定ステ ップと、格納した歩行パターン力 歩行に使用するパターンを選択して歩行開始を指 令する歩行開始命令ステップと、歩行開始命令ステップにおける指令に基づ 、てモ ータを制御してシリンダの長さを制御するモータ制御ステップとを有することとしたも のである。
この構成により、請求項 20の作用に加え、以下の作用を有する。
(1)歩行状態において 2足歩行ロボット装置は歩行パターン作成用コンピュータから 物理的な拘束を受けることなく高安定な歩行を行うことができ、また、周囲または自己 の状態 (たとえば搭載重量の軽重状態）に応じた適切な歩行パターンで歩行すること ができる。
[0066] 請求項 22に記載の 2足歩行ロボット装置の制御方法は、請求項 21に記載の 2足歩 行ロボット装置の制御方法において、モータ制御ステップは、基本制御を行う基本制 御ステップと、遊脚を路面に衝撃を緩和して着地させると共に路面の凹凸に足部を 倣わせる着地制御ステップとを有することとしたものである。
この構成により、請求項 21の作用に加え、以下の作用を有する。
(1)基本制御に基づく基本動作を行うことができると共に、状況に応じて着地制御を 行うことができる。
(2)基本制御においては、パラレルメカニズムの脚機構にとっては計算量が膨大なた めに実施が困難な順運動学を行う必要がなくなり、計算が容易な逆運動学によりリア ルタイムにリンク長さを制御することができるので、足部の位置.姿勢に関して種々の 制御を行うことができるようになる。
(3)着地制御においては、遊脚が大きな床反力を発生し ZMPや姿勢角の偏差や加 速度が発生する以前に、路面形状への倣い動作を完了し、不整路面に対して転倒 することなく安定した歩行を行うことができ、人間搭乗型 2足歩行ロボットのように正確 なモデルィ匕が困難なロボットの不整地における歩行を実現することができる。
[0067] 請求項 23に記載の 2足歩行ロボット装置の制御方法は、請求項 22に記載の 2足歩 行ロボット装置の制御方法において、基本制御ステップは、足部位置姿勢算出手段 で算出した足部の位置 ·姿勢の目標値を歩行開始命令ステップにおける指令に基づ いて出力する歩行パターン出力ステップと、歩行パターン出力ステップから出力され る足部の位置'姿勢の目標値を逆運動学演算してシリンダの長さを算出する逆運動 学演算ステップとを有することとしたものである。
この構成により、請求項 22の作用に加え、以下の作用を有する。
(1)パラレルメカニズムの脚機構にとっては計算量が膨大なために実施が困難な順 運動学を行う必要がなくなり、計算が容易な逆運動学によりリアルタイムにリンク長さ を制御することができるので、足部の位置 '姿勢に関して種々の制御を行うことができ る。
[0068] 請求項 24に記載の 2足歩行ロボット装置の制御方法は、請求項 22または 23に記 載の 2足歩行ロボット装置の制御方法において、着地制御ステップは、歩容フラグに 基づいて制御、維持、復帰、コンプライアンス等の制御モードを選択するモード選択 ステップと、歩行パターンの成分にぉ 、て着地制御の対象となる成分である制御対 象成分と制御対象成分以外の非制御対象成分とを分割する成分分割ステップと、遊 脚の着地衝撃を緩和する非線形コンプライアンス移動量を足部における床反力に基 づ 、て算出する非線形コンプライアンス移動量算出ステップと、非線形コンプライア ンス移動量に基づいて上下方向の軌道修正量を算出する z方向軌道修正量算出ス テツプと、足部におけるロールおよびピッチ方向のトルクに基づ 、てロールおよびピッ チの軌道修正量を算出するロール ·ピッチ軌道修正量算出ステップと、歩行パターン 出力ステップにおいて出力される歩行パターンと着地路面検知用移動許容量と非線 形コンプライアンス移動量と上下方向の軌道修正量とロール'ピッチ軌道修正量と〖こ 基づいて歩行パターンを修正する歩行パターン修正ステップとを有することとしたも のである。
この構成により、請求項 22または 23の作用に加え、以下の作用を有する。
(1)着地衝撃の緩和と振動の抑制に対しては非線形コンプライアンス移動量を用い て対応することができ、路面の凹凸や傾斜路面に対しては上下方向の軌道修正量 およびロールおよびピッチの軌道修正量を用いて対応することができる。
(2)ジャイロを含め特別なセンサや機構を必要とせず、 ZMP計測用の力センサのみ を用いて着地制御を行うことができ、部品点数や製造コストの増加を防止できる。
(3)遊脚が大きな床反力を発生し ZMPや姿勢角の偏差や加速度が発生する以前に 、路面形状への倣い動作を完了できるため、不整路面に対して転倒することなく安定 した歩行を行うことができ、人間搭乗型 2足歩行ロボットのように正確なモデルィ匕が困 難なロボットの不整地における歩行を確実に実現することができる。
請求項 25に記載の 2足歩行ロボット装置の制御方法は、請求項 24に記載の 2足歩 行ロボット装置の制御方法において、 z方向軌道修正量算出ステップは、理想的水 平平坦路面に衝撃や振動なく理想的に着地したときに発生するはずの理論コンブラ ィアンス移動量を制御対象成分に基づいて算出する理論コンプライアンス移動量算 出ステップと、理論コンプライアンス移動量と非線形コンプライアンス移動量との差分 である着地路面高さ誤差を算出するコンプライアンス移動量偏差算出ステップと、モ ード選択ステップで選択した制御モードと着地路面高さ誤差とに基づいて上下方向 の軌道修正量を算出する軌道修正量算出ステップとを有することとしたものである。 この構成により、請求項 24の作用に加え、以下の作用を有する。
(1)路面の凹凸に対して上下方向に正確に対応することができるので、ロール'ピッ チについて対応すれば、不整路面に対して、遊脚が大きな床反力を発生し ZMPや 姿勢角の偏差や加速度が発生する以前に、路面形状への倣い動作を完了し、転倒 することなく安定した歩行を行うことができ、人間搭乗型 2足歩行ロボットのように正確 なモデルィ匕が困難なロボットの不整地における歩行を更に一層確実に実現すること ができる。
[0070] 請求項 26に記載の 2足歩行ロボット装置の制御方法は、請求項 24に記載の 2足歩 行ロボット装置の制御方法において、ロール'ピッチ軌道修正量算出手段は、足部に おけるロールおよびピッチ方向のトルクに基づいてコンプライアンス移動量を算出す るコンプライアンス移動量算出手段と、コンプライアンス移動量に基づいてロールおよ びピッチ方向の軌道修正量を算出する軌道修正量算出手段とを有することとしたも のである。
この構成により、請求項 24の作用に加え、以下の作用を有する。
(1)路面の凹凸に対してロール'ピッチ軸まわりに正確に対応することができるので、 上下方向について対応すれば、不整路面に対して、遊脚が大きな床反力を発生し Z MPや姿勢角の偏差や加速度が発生する以前に、路面形状への倣い動作を完了し 、転倒することなく安定した歩行を行うことができ、人間搭乗型 2足歩行ロボットのよう に正確なモデルィ匕が困難なロボットの不整地における歩行を更に一層確実に実現 することができる。
[0071] 請求項 27に記載の 2足歩行ロボット装置の制御方法は、請求項 22乃至 26のいず れカ 1に記載の 2足歩行ロボット装置の制御方法にぉ 、て、モータ制御ステップは、 着地制御ステップにおいて修正した歩行パターンを推定した腰部の姿勢角補償量に 基づ!/、て補正する推定姿勢補償制御を行う推定姿勢補償制御ステップを備え、推定 姿勢補償制御ステップは、 ZMP実測値を足部における力成分とモーメント成分とか ら算出する ZMP実測値算出ステップと、算出された ZMP実測値と ZMP目標値との 偏差である ZMP偏差値を算出する ZMP偏差値算出ステップと、 ZMP偏差値を積分 した値に基づいて腰部の姿勢角補償量を算出する姿勢角補償量算出ステップと、修 正した歩行パターンにおける足部の位置又は姿勢の目標値を姿勢角補償量に基づ いて補正する足部姿勢操作量加算ステップとを有することとしたものである。
この構成により、請求項 22乃至 26のいずれか 1の作用に加え、以下の作用を有す る。
(1)機械剛性やモータの応答偏差等に起因し着地制御における軌道修正により生じ る腰部の姿勢角の累積誤差を ZMP実測値より推定し、推定した値に基づきそれを補 償して腰部と遊脚側の足部を略水平に維持することができるので、着地制御時の足 部による不整地面のセンシング精度の低下等を補い、正確な着地制御を行うことが でき、安定した歩行を行うことができる。
(2) ZMP偏差値の積分値に基づいて姿勢角補償量を算出するため、急激な変化を 伴う補償動作を防止でき安定性に優れる。特に、着地制御により急激な姿勢角の変 化を防止できるので、これと推定姿勢補償制御を併せることでより信頼性の高 、歩行 制御を行うことができる。
(3)腰部の姿勢角を測定するための姿勢角センサを搭載する必要がなぐ部品点数 や製造コストの増加を防止できる。
[0072] 請求項 28に記載の 2足歩行ロボット装置の制御方法は、請求項 27に記載の 2足歩 行ロボット装置の制御方法において、姿勢角補償量算出ステップは、所定の周期で ZMP偏差値の積分値をゼロに設定するゼロ設定ステップと、 ZMP偏差値の積分値 がゼロに設定された時点からの積分値を算出する積分値算出ステップと、足部の位 置又は姿勢の補正操作を行う操作期間を設定する操作期間設定ステップと、操作期 間の開始時点における ZMP偏差値の積分値に基づいて姿勢角補償量を算出する 補償量算出ステップとを有することとしたものである。
この構成により、請求項 27の作用に加え、以下の作用を有する。
(1)所定の周期で ZMP偏差値の積分値をゼロに設定 (リセット)することにより、実際 の姿勢角偏差との積分誤差の増加を防止できる。
(2)所定の操作期間を設定するため姿勢角誤差が過度に累積するのを防止できる。
[0073] 請求項 29に記載のプログラムは、コンピュータに請求項 15乃至 19のいずれか 1に 記載の歩行パターン作成方法の各ステップを実行させるためのプログラムであること としたものである。
この構成により、以下の作用を有する。
(1)請求項 15乃至 19のいずれか 1に記載の歩行パターン作成方法を任意の場所で 任意の時間に汎用コンピュータに実行させることができる。
[0074] 請求項 30に記載の記録媒体は、請求項 29に記載のプログラムを記録したコンビュ ータ読み取り可能な記録媒体であることとしたものである。
この構成により、以下の作用を有する。
(1)汎用コンピュータで記録媒体を読み取りさえすれば、請求項 15乃至 19のいずれ 力 1に記載の歩行パターン作成方法を任意の場所で任意の時間に汎用コンピュータ に実行させることができる。
[0075] 請求項 31に記載のプログラムは、コンピュータに請求項 20乃至 28のいずれか 1に 記載の 2足歩行ロボット装置の制御方法の各ステップを実行させるためのプログラム であることとしたちのである。
この構成により、以下の作用を有する。
(1)請求項 20乃至 28のいずれか 1に記載の 2足歩行ロボット装置の制御方法を任意 の場所で任意の時間に汎用コンピュータに実行させることができる。
[0076] 請求項 32に記載の記録媒体は、請求項 31に記載のプログラムを記録したコンビュ ータ読み取り可能な記録媒体であることとしたものである。
この構成により、以下の作用を有する。
(1)汎用コンピュータで記録媒体を読み取りさえすれば、請求項 20乃至 28のいずれ 力 1に記載の 2足歩行ロボット装置の制御方法を任意の場所で任意の時間に汎用コ ンピュータ〖こ実行させることができる。
[0077] 以下、本発明の実施の形態について、図 1〜図 56を用いて説明する。
(実施の形態 1)
本発明の実施の形態 1による歩行パターン作成装置と 2足歩行ロボット装置の説明 においては、まず、（1) 2足歩行ロボット装置の機構について説明し、次に（2) 2足歩 行の制御理論、 (3) 2足歩行ロボット装置の構成 (ノヽ一ドウエア構成およびソフトゥェ ァ構成 (機能実現手段の構成)）について説明し、最後に上記制御理論に基づく (4) 歩行パターン作成装置と 2足歩行ロボット装置の動作 (歩行パターン作成動作と 2足 歩行ロボット装置の制御動作)をフローチャートに基づいて説明する。
[0078] (1)ロボット装置の機構について
最初に、本発明の実施の形態 1における 2足歩行ロボット装置の機構について、図 50〜図 56を用いて説明する。
図 50 (a)は本実施の形態における 2足歩行ロボット装置の下半身モジュールの模 式斜視図であり、図 50 (b)は本実施の形態による 2足歩行ロボット装置の下半身モジ ユールの模式平面図である。
図 50において、 201は本実施の形態における 2足歩行ロボット装置の下半身モジ ユール、 201aは右脚部のパラレルリンク機構部、 201bは左脚部のパラレルリンク機 構部、 202はベース部 (腰部）、 203は右足部、 204は左足部である。右脚部のパラ レノレリンク機構言 201aにお!/ヽて、 205a、 205az , 205b, 205b' , 205c, 205c' は直動リンク、 206a, 206a' 、 206b, 206b' 、 206c, 206c' はベース部側受動 ジョイン卜、 207a, 207b, 207cは足部側受動ジョイン卜、 208a, 208b, 208cは回動 受動ジョイントである。なお、左脚部のパラレルリンク機構部 201bの各部は右脚部の ノラレルリンク機構部 201aと対称で同一構成なので同様の符号を付し説明を省略 する。
[0079] ここで、ベース部側受動ジョイント 206a、 206a' はベース部 202の下面の右後部 側に配設されて 、る。足部側受動ジョイント 207aは右足部 203の右後部側に配設さ れ、その下部には回動受動ジョイント 208aが配設されている。直動リンク 205aは上 端部がベース部側受動ジョイント 206aに連結され、下端部が足部側受動ジョイント 2 07aに連結されている。直動リンク 205a' は上端部がベース部側受動ジョイント 206 a' に連結され、下端部が足部側受動ジョイント 207aに連結されている。
ベース部側受動ジョイント 206b、 206b' はベース部 202の下面の右前部側に配 設されている。足部側受動ジョイント 207bは右足部 203の右前部側に配設され、そ の下部には回動受動ジョイント 208bが配設されている。直動リンク 205bは上端部が ベース部側受動ジョイント 206bに連結され、下端部が足部側受動ジョイント 207bに 連結されている。直動リンク 205b' は上端部がベース部側受動ジョイント 2061/ に 連結され、下端部が足部側受動ジョイント 207bに連結されて 、る。
[0080] ベース部側受動ジョイント 206c、 206c' はベース部 202の下面の右中央部に配 設されている。足部側受動ジョイント 207cは右足部 203の右中央部に配設され、そ の下部には回動受動ジョイント 208cが配設されている。直動リンク 205cは上端部が ベース部側受動ジョイント 206cに連結され、下端部が足部側受動ジョイント 207cに 連結されている。直動リンク 205c' は上端部がベース部側受動ジョイント 206 に 連結され、下端部が足部側受動ジョイント 207cに連結されて 、る。
このように、右脚部のパラレルリンク機構部 201aと左脚部のパラレルリンク機構部 2 01bはベース部 202の中央に対してその両側に対称に配設されている。
[0081] また、直動リンク 205、 205a, 205a' 、 205b, 205b' 、 205c, 205c' (以下、直 動リンク 205、 205a〜205c' とする）は、モータを用いた送り螺子機構や、油圧、水 圧、空気圧等を用いたシリンダ等の直動型ァクチユエータを用いて伸縮自在に形成 され、その長手方向に伸縮する 1自由度に形成されている。ベース部側受動ジョイン 卜 206、 206a, 206a' 、 206b, 206b' 、 206c, 206c' (以下、ベース部側受動ジ ョイン卜 206、 206a〜206c' とする）は、直動リンク 205、 205a〜205c' の長手方 向に直交し、且つ各々直交する 2軸の軸周方向に回動する 2自由度に形成されてい る。足部側受動ジョイン卜 207、 207a, 207b, 207cは、直動リンク 205、 205a~205 c' の長手方向に直交し、且つ各々直交する 2軸の軸周方向に回動する 2自由度に 形成されて ヽる。回動受動ジョイン卜 208、 208a, 208b, 208cは、直動リンク 205、 2 05a〜205c' の軸周方向に回動する 1自由度に形成されている。
[0082] このように、右脚部、左脚部のパラレルリンク機構部 201a、 201bは各々 6自由度に 形成されているため、右足部 203及び左足部 204は、前後、左右、上下、及び前後 方向、左右方向、上下方向を軸とした軸周方向の動作が可能であり、多様な動作を 行うことができ、歩行動作を円滑に行うことができる。
[0083] 以上のように構成された本実施の形態における 2足歩行ロボットの下半身モジユー ルについて、以下その動作を図を用いて説明する。なお、本実施の形態においては 、下半身モジュールの右脚部の動作について説明する。左脚部の動作については 右脚部と同様であるので説明を省略する。
[0084] 右脚部を動作させる場合、予め設定された歩行パターンに基づいて右足部 203の 逆運動学を計算し、算出された値に基づいて直動リンク 205a〜205c の各々の図 示しないァクチユエータを駆動させ直動リンク 205a〜205c' を伸縮させる。直動リン ク 205a〜205c' とベース部 202又は右足部 203との連結部分に配設されて!/、るべ 一ス咅刚受動ジョイント 206a〜206c' ゝ足咅側受動ジョイント 207a〜207cゝ及び 回動受動ジョイン卜 208a〜208cは、直動リンク 205a〜205c の f申縮に追従してこ れを妨げることなく円滑に従動する。直動リンク 205a〜205c' のァクチユエータの 駆動は各々に配設されたロータリエンコーダ等の図示しない検出器により検出され、 取得された検出値は角度データ等としてフィードバックされ直動リンク 205a〜205c ' はフィードバック制御される。これにより、右足部 203は、一歩踏み出す動作やそ の場で足踏みする動作等を行うことができる。更に、このような動作を右足部 203と左 足部 204で交互に連続して行うことにより、歩行動作を行うことができる。
[0085] 図 51は本実施の形態における 2足歩行ロボット装置の下半身モジュールの斜視図 である。
図 51において、 201cは本実施の形態における 2足歩行ロボット装置の下半身モジ ユール、 202はベース部、 203は右足部、 203aは右足部 203の上部に固定された 平板状の固定板、 204は左足部、 204aは左足部 204の上部に固定された平板状の 固定板、 209はベース部 202の上面に配設された制御装置部、 209' は制御装置 部 209の後部に配設された制御用コンピュータ、 231はバッテリ、 232はモータ駆動 用回路部、 235はベース部 202と右足部 203及び左足部 204の間に各々立設され た直動リンク、 236はベース部 202の下面側の所定位置に固定されたベース部側受 動ジョイント、 237は右足部 203及び左足部 204の固定板 203a、 204aの上面側の 所定位置に回動自在に固定された足部側受動ジョイント、 248は直動リンク 235を伸 縮するための直流サーボモータである。足部側受動ジョイント 237は図 50で説明した 足部側受動ジョイン卜 207、 207a, 207b, 207c及び回動受動ジョイン卜 208、 208a 、 208b, 208cと同様の機能をボールジョイントで実現したものである。このボールジ ョイントについては後述する。 [0086] ここで、本実施の形態においては、直動リンク 235は後述する送り螺子機構により 長手方向に伸縮自在に形成されている。送り螺子機構の代わりに油圧、水圧、空気 圧等を用いたシリンダや直動型ァクチユエータを用いた機構としてもよい。また、直動 リンク 235は、 2本を 1組として上端部が各々ベース部側受動ジョイント 236に連結さ れ下端部力^つの足部側受動ジョイント 237に連結された V字形状に配設されている 。 1組の直動リンク 235は、右脚部及び左脚部に各々 3組ずつ、平面視三角形状に 配設され、片脚に 6本、合計 12本が配設されている。すなわち、本実施の形態にお ける 2足歩行ロボット装置の下半身モジュール 201cの左右の脚部は、各々スチュヮ ートプラットホーム（パラレルメカニズムの一種であり、 6本の直動シリンダの両端をボ ールジョイントとユニバーサルジョイントでベースとエンドェフエクタに結合する構造を 有している）により構成されている。これにより、動作の安定性及び強度、剛性、出力 に優れる。また、 1つのベース部側受動ジョイント 236には 1つの直動リンク 235の上 端部が連結され、 1つの足部側受動ジョイント 237には 2つの直動リンク 215の下端 部が連結されている。
ノ ッテリ 231、モータ駆動用回路部 232は制御装置部 209に配設されている。本実 施の形態においては、バッテリ 211としてニッケル水素ノ ッテリを使用した。
また、右足部 203及び左足部 204の底面側に、床反力を検出する 6軸カ覚センサ 203b, 204b (図 40の 142)を設ける。 6軸カ覚センサ 203b、 204bは、各軸方向の 力 3成分と各軸周りのモーメント 3成分を同時に且つ逐次連続的に高精度で検出す ることができる。また、 6軸カ覚センサ 203b、 204bで検出された値を基に仮想コンプ ライアンス制御や着地軌道修正制御 (後述)、推定姿勢補償制御 (実施の形態 2)を 行うことちでさる。
[0087] 次に、直動リンクの構造について図 52及び図 53を用いて詳細に説明する。
図 52は直動リンクの要部斜視図であり、図 53 (a)は直動リンクの要部側面図であり 、図 53 (b)は図 53 (a)の部分内部構造図である。
[0088] 図 52において、 235は直動リンク、 241は後述のインナーロッド部 243等を保持す る保持ケーシング、 243は保持ケーシング 241に揷設され直動リンク 235の長手方向 に摺動するインナーロッド部である。直動リンク 235の伸縮はインナーロッド部 243が 摺動することにより行われる。 244はインナーロッド部 243にその長手方向に形成さ れたジョイント結合部、 245はインナーロッド部 243の一方の側部の上部側および下 部側に配置されその長手方向に敷設されたロッドレール部である。
[0089] 図 53において、 246はレールガイド、 247はインナーロッド部 243の初期位置を検 出する初期位置センサ、 248は保持ケーシング 241に平行に配設されたモータ、 24 8aはモータ 248を保持ケーシング 241の端部の側部に平行に保持するモータホル ダ、 249はモータ 248のモータ先端軸部に取り付けられた歯付プーリ、 250は非通電 時においてモータ 248の回動軸を固定しインナーロッド部 243を保持するための保 持ブレーキ、 251はモータ 248の回動軸の回動を検出するロータリエンコーダ、 252 はモータ先端軸部 249の回転動力を後述の雄螺子先端軸部 254aへ伝達する溝付 きベルト、 253は雄螺子軸部 254を支持するベアリング、 254は保持ケーシング 241 の内部に後述の雌螺子ナット部 255に挿通されて配設され外周が螺子切りされた雄 螺子軸部、 254aは雄螺子先端軸部、 255はインナーロッド部 243に固定され雄螺子 軸部 254に螺合した雌螺子ナット部、 256a、 256bはストッパである。このようにモー タ 248を保持ケーシング 241に平行に配設したので、直動リンク 235の最短長さ（最 も短 、時の長さ）を長くすることなくストロークを長くすることができ、ロボット装置の可 動範囲を広くすることができる。
[0090] ここで、本実施の形態においては、保持ケーシング 241の材質として、軽量で且つ 比較的強度の高いアルミニウムを用いた。また、初期位置の確認のための初期位置 センサ 247として、フォトマイクロセンサを用いた。また、本実施の形態においては、 雄螺子軸部 254と雌螺子ナット部 255を螺合させるためにボール螺子を用いて 、る。 これにより、インナーロッド部 243を高速で摺動させることができ、歩行速度を向上さ せることができる。またボール螺子を用いた場合は螺合部分の摩擦抵抗を低減する ことができる。また、ボール螺子はがたが少なぐ動作を確実にすることができる。
[0091] 図 52や図 53〖こ示すよう〖こ、モータ 248が駆動されると、その回動軸の回動力がベ ルト 252を介して雄螺子先端軸部 254aに伝えられ、雄螺子軸部 254が回動する。雄 螺子軸部 254にはインナーロッド部 243に固定された雌螺子ナット部 255が螺合して いる。ここで、インナーロッド部 243は外周面に設けられたロッドレール部 245がレー ルガイド 246に嵌合して 、るため、長手方向には摺動するが軸周方向には回動しな い。これにより、雄螺子軸部 254が回動すると、送り螺子機構により雌螺子ナット部 25 5を介してインナーロッド部 243がその長手方向に摺動する。このようにして、直動リン ク 235の伸縮が行われる。
[0092] 続いて、ベース部側受動ジョイントの構造について図 54を用いて詳細に説明する。
図 54はベース部側受動ジョイントの要部斜視図である。
図 54において、 202はベース部、 235は直動リンク、 236はベース部側受動ジョイ ント、 241は直動リンク 235の保持ケーシング、 261はベース部 202の下部に固定さ れたコ字形状のベース部側上部継手、 262はベース部側上部継手 261の一対の立 設部に架設された上部継手軸、 263は直動リンク 235の保持ケーシング 241側の上 端部に固定されたコ字形状のベース部側下部継手、 264はベース部側下部継手 26 3の一対の立設部に架設された下部継手軸、 265は上部継手軸 262と下部継手軸 2 64を直交状に連結する連結回動部である。
[0093] 図 54に示すように、ベース部側下部継手 263は、ベース部側上部継手 261に対し て、上部継手軸 262と下部継手軸 264の軸周方向に回動する。これにより、ベース部 側受動ジョイント 236は、直動リンク 235の長手方向に直交する上部継手軸 262と下 部継手軸 264の軸周方向に 2自由度を有するので、直動リンク 235の伸縮に追従し てこれを妨げることなく円滑に従動する。
[0094] 続いて、足部側受動ジョイントの構造について図 55及び図 56を用いて詳細に説明 する。
図 55は足部側受動ジョイントの要部斜視図であり、図 56は足部側受動ジョイントの 要部縦断面図である。
図 55および図 56【こお!ヽて、 203aiま右足咅 203の固定板、 235、 235' ίま直動リ ンク、 237は足部側受動ジョイント、 244は直動リンク 235のジョイント結合部、 244' は直動リンク 23 のジョイント結合部、 271はジョイント結合部 244の下端部に連結 された第 1の足側上部継手、 271a、 27 lbは第 1の足側上部継手 271の両側部に互 いに対向して配設され下端部が後述の継手軸に軸支された側部軸支板、 273はジョ イント結合部 244' の下端部に連結された第 2の足側上部継手、 273a、 273bは第 2 の足側上部継手 273の両側部に互いに対向して配設され下端部が後述の継手軸に 軸支された側部軸支板である。ここで、各側部軸支板 271a, 271b, 273a, 273bは ボールベアリング 272a (図 56参照）を介して後述の継手軸に軸支されている。 275 は内部に内壁面が球面状のボール収容部 275aを有するボール保持部、 276a、 27 6bはボール保持部 275の両側部に突設された継手軸（図 56参照）、 277は各継手 軸 276a、 276bに虫累着され各佃 J咅軸支板 271a、 271b, 273a, 273bを抜け止めす るための止めナット、 278は固定板 203a上に固定された足側基部、 279は足側基部 278上に立設され上端部にボール収容部 275aに収容されるボール部 279aが形成 されたボール継手軸である。
[0095] 図 55および図 56に示すように、第 1の足側上部継手 271及び第 2の足側上部継手 273ίま、ボーノレ保持咅 275【こ対して、 ϋ手軸 276a、 276bの軸周方向【こ回動する。 また、ボール保持部 275は、足側基部 278に対して任意の方向に回動する。これに より、足部側受動ジョイント 237は、継手軸 276a、 276bの軸周方向およびボール継 手軸 279の軸周方向に 2自由度を有すると共に、少なくとも継手軸 276a、 276bとボ 一ル継手軸 279とに直交する軸の軸周方向に 1自由度を有するので、直動リンク 23 5、 235' の伸縮に追従してこれを妨げることなく円滑に従動する。
このように足部側受動ジョイント 237をボールジョイントを用いた構造にしたので、従 来は構造的に締結箇所が多くがたが生じていた力 締結箇所が少なくなりがたが低 減し、動作の精度が向上し、制御の安定性および剛性を高めることができ、高速で安 定した歩行動作を行うことができる。
以上のように構成された本実施の形態における 2足歩行ロボット装置の下半身モジ ユールの歩行動作については、図 50で説明したものと同様であるので説明を省略す る。
[0096] (2) 2足歩行ロボットの制御理論につ!、て
次に、 2足歩行ロボットの制御理論について、図 1〜図 3を用いて説明する。
図 1 (a)は支持多角形を示す説明図であり、図 1 (b)は座標系とベクトルの定義を示 す説明図である。
2足歩行のパターンを生成するにあたり、安定性を判別する必要がある。ここでは、 2足歩行ロボット装置に対して力学的視点力 安定性を判別するために、「ZMP安定 判別規範」を用いる。以下に、 ZMPの説明および安定判別法を述べる。
2足歩行ロボットが歩行中に安定な支持状態を維持するために必要な条件を考え る。 2足歩行ロボットが安定な支持状態を保てなくなる、つまり転倒するということは、 足底部と路面との接点（3点以上)が形成する支持多角形 (路面と足底の接地点が形 成する面積最大の凸多角形)のある辺または点を通る路面上の直線を軸に回転運 動をしていると考えられる。すなわち、支持多角形から見て外向きのモーメントが作用 していると考えられる。逆に言えば、支持多角形上の全ての辺および点のまわりに外 向きのモーメントが発生せず、内向きのモーメントのみ発生していれば、 2足歩行ロボ ットは安定な支持状態を維持できる。このときの 2足歩行ロボットの支持状態を考える と、足底の接地点が浮かないため、全ての接地点において 2足歩行ロボットから路面 に作用する重力および慣性力による力は路面を押す向きであり、これらの合力が作 用する点 P (図 1 (a)参照）は足底の支持多角形内になければならない。この点 P回り の合力によるモーメントは明らかにゼロであり、この点を ZMP (Zero Moment Poi nt、ゼロモーメントポイント）と呼ぶ。
[0097] 2足歩行の制御に用いるモーメント補償軌道算出アルゴリズムについて説明する。
本アルゴリズムは次の 4点力 なる。
I. 2足歩行ロボットのモデル化
II. Iのモデルにおける ZMP方程式の導出
III. 2足歩行ロボット近似モデルィ匕
IV. IIIの近似モデルを用いた繰り返し計算による IIを満足するモーメント補償軌道 の厳密解の算出
[0098] まず 2足歩行ロボットのモデルィ匕について説明する。最初に、 2足歩行ロボット、座 標系および路面に次の条件を設定する。
1) 2足歩行ロボットは質点の集合力 成る。
2)路面は十分に硬ぐどんな力やモーメントが作用しても変形したり動いたりするこ とはない。
3) X軸（2足歩行ロボットの正面方向に一致)および Y軸を含む平面が路面に一致 し、路面に垂直な軸を Z軸とする直交座標系 O (右手系）を設定する（図 1 (b)参照）
4) 2足歩行ロボットの足底と路面との接地状況は点接地の集合とする。
5)歩行系と路面との接地点において、路面での回転 (X、 Yおよび Z軸回り）に対す る摩擦係数は無視できるほど小さい。
6) 2足歩行ロボットが歩行する際の推進力は、接地点における並進 (Xおよび Y軸） 方向の滑りが生じない範囲とする。
7)体幹の質点は動かないものとする。つまり、腰の質点の軌道と体幹の質点の軌 道は Z軸方向に関しては異なる力 XY平面に関しては同じ軌道を通るものとする。
8)路面に対してロボットは滑らないものとして、ョー軸に関しての補償 (Z軸回りの回 転に関してのネ ΐ償）は行わない。
次に、 ΖΜΡ方程式の導出について説明する。
以上の仮定と設定のもとに絶対座標系 Ο—ΧΥΖにおいて任意の点 Ρについてのモ 一メントの釣り合い式を導出する次式 (数 1)を得る（なお、各記号の説明は後述の（ 数 26)に示されている)。
[数 1]
Al l Particles Al l Points
ΣΓΤ^ ( — rP) X ( -G)—∑ { (rFk - rP) XFk } +T=0 i k
(数 1)において点 Pを ZMPとすることで、 T=0となり、次式 (数 2)の ΖΜΡ方程式を 得る。
[数 2]
Al l Particles Al I Raints
∑mi (r - rZMP) X (r -G) —∑ { (rFk— rZMP) XFk ^l k} =0 i k
さらに、各部の相対運動を考えるために、図 1 (b)に示す 2足歩行ロボットの腰部付 近に固定された運動座標系 Oバ一一 XYZバーを設定する。この座標系を用いて ZM P方程式を表すと、次式 (数 3)を得る。
[数 3] ) }
Figure imgf000055_0001
Al l faints ― ―
-∑ { (？ F k - ZMp ) XFk-^k} =0
k
この ZMP方程式を満たすように腰部の軌道を算出する。
[0100] 次に、 2足歩行ロボットの近似モデルィ匕について説明する。
本アルゴリズムでは、モーメント補償軌道の近似解の算出とその厳密モデルへの代 入、モーメント誤差のフィードバック、蓄積する繰り返し計算により、モーメント補償軌 道の厳密解を得て!、る。その近似モデル化のプロセスは次の通りである。
i)外力は近似モデルでは考慮しな 、。
ii)腰、体幹などの下肢以外の部位は 2質点近似モデルとする（図 1 (b)参照)。 iii)線形、非干渉ィ匕のために運動座標系の回転は考慮しない (ただし体幹ョー軸に 関しては線形、非干渉ィ匕できる範囲でその一部を考慮して 、る）。
iv) Z軸の運動を共有しな 、ものとして線形、非干渉化する。
[0101] まず、外力を近似モデルで考慮しないとすると、（数 3)は (数 4)のようになる。
画
Al l
) } =0
Figure imgf000055_0002
ここで、被補償モーメントとなる下肢の運動により発生するモーメントをベクトル Mと おき、 2質点近似モデル化を行うと、（数 4)は (数 5)のようになる。
[数 5] mT (rT-rZMP) X { ΓΤ+0-0-ώΧΓτ+2ωΧΓτ+ωΧ fc)XrT) } +mw (rw-rZMp ) x { rw+Q-G+ Χ rw+ 2ωΧ rw-koX i¾)Xrw) } =-
(数 5)は、運動座標系の回転により発生する見掛けの力の項において互いに干渉 している。よって、これらの微分方程式を非干渉なものにするためには、まずその見 掛けの力によるモーメントが発生しな 、ものと仮定、つまり運動座標系が回転して ヽ ないものとすると、（数 5)は (数 6)、（数 7)のようになる。 [数 6] mT (ZT-ZZMP) (Χχ+Xq-g,) ^ ΊΤ (XT-XZMP) (ZT+Zq— )
-nr^ \tw— zZHP) ( +xq - gx) 、xw - xZMP) (zw+zq— gz) =-My
[数 7] mT (yT-yZMp) (zT+zq-gz) -^nT (zT-zZMP) (yT+yq-gy)
+ηλν(ννν-νζΜρ) (zw+zq-g2)
Figure imgf000056_0001
(yw+yq-gy) =-Mx
(数 6)、（数 7)は Z軸方向の運動を共有し、また下肢と体幹が回転型の間節を介し て連結していることから、非線形かつ干渉な系である。そこで、 2足歩行ロボットは運 動中腰高さ一定と仮定し、線形、非干渉化する。すなわち、（数 8)、（数 9)、（数 10) が成立する。ここで、（数 9)はピッチ軸に関し、（数 10)はロール軸に関する。
[数 8] T=o ， !w=o ， 'iQ=o
Figure imgf000056_0002
[数 9] mT (zT-zZMP) 、xT+Xp) -mT ( 丁—又 ZMP) (—
"l"fTlV (Zw——ZZMp) (Xyy+Xg )
Figure imgf000056_0003
Figure imgf000056_0004
[数 10] mT (yT-yZMP) (-g2) -mT(zT-zZHP) (yT+yq
(yw+yq)
=- Mx また、（数 9)、（数 10)において左辺の既知項を右辺に移動し、（数 9)、（数 10)に おいて右辺を改めて (数 11)と置き直すと、次の（数 12)、（数 13)を得る。ここで、（数 12)はピッチ軸に関し、（数 13)はロール軸に関する。
[数 11]
-M*=- , Mj, M*]T
[数 12] mT (zT-zZMP) xT~mT、― gz) xT
- Y ^ (zw-zZMP) Xw-rriw (-¾) xw=-M*
[数 13]
(zT-zZMP ) yT mT (-gz) yT W—ZZMP)
Figure imgf000057_0001
ここで、 7)の仮定より（数 14)であるから、（数 12)、（数 13)は (数 15)、（数 16)、（数 17)のように書き換えられる。
[数 14] ズ τ一ズ w， 乂丁一
[数 15] 1 ψ― ^2Λνν―一 M y
[数 16]
- , yw+k2yw=-M x
[数 17] 1
Figure imgf000057_0002
、 T一 ZMP/ "("•"AV ( W一 ZMP) } '
Figure imgf000057_0003
次に、近似解の算出について説明する。
モーメント補償軌道 (数 18)、（数 19)の近似解を求めるために、（数 15)、（数 16)を 用いて解析的に近似解を得る。
[数 18]
Xw
[数 19]
Vw
ここで、（数 15)において、（数 20)は下肢軌道および ZMP軌道力 算出できるので 既知関数となり、さらに定常歩行であるとすれば、 2足歩行ロボットの各部質点ならび に ZMPは運動座標系 Oバ一一 XYZバーに関して周期的な相対運動をするので、（ 数 20)は周期関数となる。
[数 20]
M*y よって、（数 15)の左辺も同じく周期関数になる。そこで (数 15)の右辺を (数 21)に示 すようにフーリエ級数展開する。
[数 21]
Figure imgf000058_0001
Ν：新パターン 職
このときのフーリエ係数は（数 22)のようになる。
[数 22]
2Υ [1]
an =
0 an
Figure imgf000058_0002
続いて、（数 18)を係数未知のフーリエ級数で表すと、（数 23)のようになる。
[数 23]
Figure imgf000059_0001
2K
N：新パターン (^激
(数 23)を (数 15)の左辺に代入し、各両辺のフーリエ係数を比較することで、（数 1 8)のフーリエ係数を求めると、（数 24)のようになる。
[数 24]
A = -^ ,
0 k2 ，
△ an bn
An一 ^2n2+ 2 ， Dn~ k^2n2+k2 そして、これを逆フーリエ級数展開することで、ピッチ軸回りのモーメント補償軌道（ 数 18)の近似解を (数 25)に示すように得ることができる。
[数 25]
Figure imgf000059_0002
— r.， 1 ^ rn - (j-D (k-D
xw [J] =— k∑ xw [k] ωΝ
(-2ττί)/Ή
ωΝ= θ
N：節パターン ( 嫌
なお、（数 26)に各記号の説明を示す。
[数 26] べクトル
T
= [ | , Yi , ζ.] i番目の！^ (^立置べクトル
FK [XFK，
'τ = [x丁， w ― [xw，
Figure imgf000060_0001
' w] 卜ル
トル
Figure imgf000060_0002
ク卜ル
Q = [χ, q, ' y "nQ, z 1
τ ― ベク卜ル ω = [ων, ω„， ω 1 O-XYZの原 の
角 iiJべクトル
G = Lgx, gy， gj 動力 Ρϋϋべクトル
T = [TX， TY， TZ]T ΖΜΡにおける床反力モーメン卜 M = DVIX, MZ]T 下鼠力 する ΖΜΡにおける モーメン卜 M*= [ *, M^, M*]T モーメン卜補 iic^:される モーメン卜 Μ = 0^， MYK, MZK] k番目 乍用点に作用する
外力モーメン卜
'k一 [Fxk， yk ] k番目 (^； ^乍用点に作用する外力 eM_ [θΜχ， θΜγ' eMz] 設定 Z M Pにおけるモーメン卜誤差
Figure imgf000060_0003
m. i番目の鬣^) IS
Figure imgf000060_0004
m τ 次に、スチュワートプラットホームの逆運動学について説明する。
ノラレルリンク機構 (特にスチュワートプラットホーム型)では、シリアルリンク機構と 比較して、非常に逆運動学は容易になる。これは、シリアルリンク機構においては、 特別な場合を除いて解析解が存在しないため、解を繰り返し計算による数値解として 求める必要があり、計算の量が非常に多くなるためである。また、解析解が存在する 場合でも、多くの座標変換があるために計算が複雑になる。以下、逆運動学の解法 について述べる。
2足歩行ロボットのパラレルリンク機構は、ベースプレート（2足歩行ロボットの腰部） とエンドェフエクタ（2足歩行ロボットの足底部）からなる。まず、右足のリンク配置、座 標系を図 2 (a)、（b)に示す。図 2 (a)、（b)は腰座標系 (数 27)および足座標系 (数 2 8)を示す模式図である。また、（数 29)は腰座標系力も見たエンドェフエクタ側ジョイ ントの位置ベクトルである。
[数 27]
W-XWYW
[数 28]
:— X f Yf z f
[数 29]
腰座標系（数 27)においての腰側ジョイントの位置を (数 30)とし、足座標系（数 28) にお 、ての足側ジョイントの位置を (数 31)とする。
[数 30]
ヽ
B，
Figure imgf000061_0001
ノ
[数 31]
Figure imgf000062_0001
ここで、腰座標系から見た足座標系の位置 ·姿勢 (エンドェフエクタの位置 ·姿勢)を 表わすベクトル (数 32)を定義する。
[数 32]
Xi = (x， y， ζ， θχ, θ θζ)
また、腰座標系から見た足座標系の原点の位置ベクトル (数 33)と、腰座標系におい て 0x、 0y、 0zの順に回転させる回転変換行列 (数 34)とを、（数 35)、（数 36)のよ うに定義する。
[数 33]
[数 34]
T
[数 35]
Figure imgf000062_0002
[数 36]
=
Figure imgf000062_0003
(数 31)、（数 35)、（数 36)より（数 37)となり、（数 29)の各成分は、（数 38)となる。 [数 37]
E T + Xc
[数 38]
Exi = (cos6zcos0y) exi + os0z s i n0y s i n0x-s i ndzcos6x) eyi
+ (cos0zsin0ycos0x+sin0zsin0x) ezi +x
Eyi = (sin0zcos0y) exi + (sin0zsin0ysin0x+cos0zcos^x) eyi
+ (sinfizsin0ycos0x-cosezsin0x) ezi + y
Ezi = (-sin0y) exi+ ( cos0ysin0x ) eyi + (cos0ycos0x) ezi + z ここで、（数 39)となる変数を導入すると、単に幾何学的関係より、 i番目の腰側ジョ イントと i番目の足側ジョイントを結ぶリンクの長さは、（数 40)となる。
[数 39]
I — p 一 p
I —— 一 D
[数 40]
I 2 2 2
一 Ι χί + l yi + I zi
詳細に書けば、（数 41)となる。
[数 41] l i≡ f (X)
= [{ os0zcosfiv) exi + (cos0z s i n0y s i n5x-s i n0zcos0x) e j
+ (cos0z s i n0ycos0x+s i ηθζ s i ηθχ) ezi +x— Bxi}2
+ {(sin0zcos0y) exi+ (s i ηθζ s i n0y s i n0x+cos0zcos0x) eyi
+ (sin6zsin0ycos0x-cos0zsin0x) ezi + y— Byi
+ {(-sin^) exi + (cos sin0x) eyi+(cos0ycosx) ezi+z— Bzi } 2] 1/2 着地制御においては、理論コンプライアンス移動量、非線形コンプライアンス移動 量、 Z方向着地軌道修正量、ロール'ピッチ方向コンプライアンス移動量、ロール'ピッ チ方向軌道修正量を算出する必要がある。これらの移動量、修正量の算出について 説明する。なお、ここでは右足を遊脚として説明されており、各式に示す Rの文字は 右足であることを意味する。
まず、理論コンプライアンス移動量の算出について説明する。
図 3(a)、 （b)は理論コンプライアンス移動量の説明図である。まず、（数 42)は制御 ソフトウェア内部に記述される着地路面検知用移動許容量を示し、（数 43)は腰座標 系における歩行パターンの座標（遊脚の座標）、（数 44)は絶対座標系における腰座 標（つまり腰の高さ）である。図 3 (a)において点線で示されているのが歩行パターン における遊脚の足部位置であり、本手法では歩行パターンより（数 42)だけ足を伸ば したように歩行パターンが出力される。（数 43)は腰座標系であるので、図 3に示すよ うに負の値となる。
[数 42]
Δ I
[数 43]
f patR (t)
[数 44]
Z wa i s t (
(数 45)の条件にぉ ヽては遊脚は完全に路面から離れて ヽるので、理論コンプライア ンス移動量は (数 46)となり、（数 47)の条件においては遊脚は路面に対して着地寸 前であるので、理論コンプライアンス移動量は (数 48)となる。
[数 45]
A I - z f patR (tノ "\ Z wa j s t ( t )
[数 46]
AZ fccthR (t) =0
[数 47]
△ I一 Z fpatR (t)≥ Zwai st (t) [数 48]
△ z fccthR(t) =Δ I― Zwa i st ( t ) — Z f patR ( t) 次に、非線形コンプライアンス移動量の算出について説明する。
図 3(c)は非線形の説明図であり、図 3(d)、 （e)は非線形コンプライアンス移動量の 説明図である。非線形コンプライアンス移動量を (数 49)で示す。
[数 49]
△ Z fccacR ( t ) 図 3 (c)は、コンプライアンス係数が (数 42)の値を境に異なることを示す。図 3 (d)に 示すように、非線形コンプライアンス移動量が (数 50)の条件を満たすときに、非線形 コンプライアンス移動量は (数 51)で表され、図 3(e)に示すように、非線形コンプライ アンス移動量が (数 52)の条件を満たすときに、非線形コンプライアンス移動量は (数 53)で表される。
[数 50]
AzfccacR(t)≤Δ I
[数 51]
A 7 f ,Γ Δ Z fccacR (t - A t) L fccacR 、r zR卞し zl 厶
Figure imgf000065_0001
[数 52] ΔΖ fccacR (t) >Δ I
[数 53]
△ Z fccacR ( t )
、 ノ △
Figure imgf000065_0002
ここで、（数 54)は着地路面検知用コンプライアンス係数を示し、（数 55)は振動抑 制用コンプライアンス係数を示す。この際、着地路面検知用コンプライアンス係数は 制御系の発振が起こらない範囲でなるべく小さく取る。これにより大きな床反力が発 生する以前に路面の形状を検知することができる（また理論コンプライアンス移動量 が（数 42)までの範囲でなら (数 48)のようになるのも、この定数を十分小さく取るから である)。また、（数 56)は z方向の床反力を示し、（数 57)は制御周期（例えば lms) を示す。
[数 54]
κζ1 ， cz1
[数 55]
Figure imgf000066_0001
[数 56]
1 zR
[数 57]
△ t 次に、 z方向着地軌道修正量の算出について説明する。
まず、コンプライアンス移動量偏差としての着地路面高さ誤差を示す (数 58)を (数
59)を用いて算出する。
[数 58]
eR (t)
[数 59]
eR (t) =AzfccthR(t) -AzfccacR(t) 前述の歩容フラグ力 求めた制御モードと上記着地路面高さ誤差との基づ 、て (数
60)の Z方向着地軌道修正量を算出する。この z方向着地軌道修正量の算出につい ては後述する。
[数 60] HR(t) 次に、ロール'ピッチ方向コンプライアンス移動量について説明する。
図 3(f)は、コンプライアンスを示す説明図であり、 ZCOMは z方向コンプライアンス を示し、 RCOMはロール方向コンプライアンス、 PCOMはピッチ方向コンプライアン スを示す。まず、後述の 6軸カ覚センサで検出されるロール方向のトルクを (数 61)で 示し、同じく 6軸カ覚センサで検出されるピッチ方向のトルクを (数 62)で示すと、（数 63)のロール方向のコンプライアンス移動量および（数 64)のピッチ方向のコンプライ アンス移動量は、（数 65)および (数 66)で表される。
[数 61]
Figure imgf000067_0001
[数 62]
[数 63]
△6xfccR (セ)
[数 64]
△0yfccR(t)
[数 65]
^ 上 Cex A0xfccR(t— At) 厶 xfccR ( >·ノ = •^θχ " 、—，
r" Α ,ノ " 「 卞し θχ
At ν - At
[数 66] ceyy A vfccR(t-At)
Δ △ t
Figure imgf000067_0002
ノ . 、卜 十 ey At
(数 63)のロール方向のコンプライアンス移動量および（数 64)のピッチ方向のコン プライアンス移動量と、前述の歩容フラグから求めた制御モードとに基づいて、（数 6 7)のロール方向軌道修正量および (数 68)のピッチ方向軌道修正量が算出される。 このロール'ピッチ方向軌道修正量の算出については後述する。
[数 67]
A 0x f , cR ( t )
[数 68]
A 0y f l cR ( t )
[0108] 次に、推定姿勢補償制御について説明する。
上述した着地制御は、遊脚が大きな床反力を発生し ZMPや姿勢角の偏差や加速 度が発生する以前に、路面へのならい動作を完了できるため、例えば人間搭乗型の 歩行ロボットに対して非常に有力な制御手法であるが、腰部と遊脚側の足部がそれ ぞれ常に水平であるということが着地制御における前提条件であり、機械剛性やモー タの応答偏差等によりこの前提条件が崩れると、腰部の姿勢角の誤差が累積されて しまう。なお、姿勢角センサを搭載してその検出値をフィードバックすれば、容易に姿 勢角の累積誤差を修正できるが、部品点数が増加すると共に、高精度の姿勢角セン サは非常に高価であり製造コストが増加するという問題点がある。
そこで、この推定姿勢補償制御においては、足部に搭載されている 6軸カ覚センサ の検出値力 ZMP実測値を算出し、これと予め設定されている ZMP目標値との偏 差 (ZMP偏差値)に基づ 、て腰部の姿勢角の累積誤差（ =姿勢角補償量)を推定し 、その値に応じて補償制御を行う。
[0109] まず、推定姿勢補償制御において用いる姿勢角補償量の算出法について説明す る。
姿勢角補償量の算出には (数 69)を用いる。
[数 69]
Δ Θ = K · ∑ Δ χ ^ Δ t
(数 69)の姿勢角補償量、ゲイン行列、 ZMP偏差値はそれぞれ (数 70)、（数 71)、 (数 72)で表される。
[数 70] ΑΘΧ (ロール方向 幾角補 fit)
Δθν (ピッチ^] (^角補 ill)ノ
[数 71]
K = (ゲイン行列)
Figure imgf000069_0001
[数 72]
△ X zmp
Δ ζττρ ― (ZMPil^iiD
Ay zmp なお、（数 72)の ZMP偏差値は (数 73)を用いて算出する。
[数 73]
Δ X zmp = λ zmp_res一 λ zmp— ref
(数 73)の ZMP実測値、 ZMP目標値はそれぞれ (数 74)、（数 75)で表される c [数 74]
X zrrp res (ZMP« )
^ zrrp
[数 75]
X zmp— ref
X zrrp ref (ZMP目繊
Figure imgf000069_0002
(数 74)は 6軸カ覚センサの検出値から (数 76)を用いて算出された ZMP実測値で ある。
[数 76]
Figure imgf000070_0001
(数 77)に (数 76)中の各記号の説明を示す。
[数 77]
[xSR， ysR， zsR]T (右足の 6軸カ覚セ ytii^sRの j tD
FR = [ FxR ) FYR , FZR]T ^SRで則定される力）
MR = [MXR， MYR， MzR (^、SRで則定されるモーメント）
[xsL， ysL •sL ] ( の 6軸カ覚セ sLの麵 xL ' ' yL FZL]t ¾sLで則定される力）
ML = [ MXL， MYし ， MZL ] 0 SLで則定されるモーメン卜） また、（数 75)は ZMP目標値であり、歩行パターン作成時に設定された目標 ZMP 軌道と同様の値である。
腰部の姿勢角を補償する操作を行う操作タイミングとしては、所定周期の断続的な 期間である操作期間を予め設定しておき、該操作期間中に操作を行う。なお、実際 の腰部の姿勢角の操作量 (姿勢角操作量）は、急激な姿勢角の変動を防止するため に、操作期間の開始時点力 の時間 tの関数として (数 78)を用いて算出する。
[数 78]
Figure imgf000070_0002
(数 78)の姿勢角操作量、操作期間はそれぞれ (数 79)、（数 80)で表される。姿勢 角操作量は操作期間の開始時点で 0であり、終了時点で姿勢角補償量 (数 70)とな る。 [数 79] r△ （ロール方向 角揉ィ乍 *)
L6 ' =
厶 0 （ピッチ方向^ ^角^乍 1)
[数 80]
i f 謹月間）
なお、推定姿勢補償制御においては、腰部に対する足部の位置又は姿勢を操作 することで腰部の姿勢を操作して 、る。操作される足部は単立脚時であれば立脚側 の足部であり、該足部の中心を基準としてその姿勢 (ロール及びピッチ方向）を操作 する。両脚支持時であれば両方の足部であり、各足部の中心を結ぶ線分の中点を基 準としてその位置及び姿勢を操作する。
また、 ZMP偏差値を長時間積分し続けると、実際の姿勢角の誤差との積分誤差が 大きくなつてしまうため、積分を開始して力 所定時間経過後に ZMP偏差値の積分 値をゼロに設定 (リセット)することで、これを回避する。
(3)歩行パターン作成装置と 2足歩行ロボット装置の構成について
次に、本実施の形態による歩行パターン作成装置と 2足歩行ロボット装置の構成に ついて図 4〜図 17を用いてハードウェア構成を説明し、図 18、図 33を用いてソフトゥ エア構成を説明する。
図 4は、本実施の形態による歩行パターン作成装置 (歩行パターン作成用コンビュ ータ）と 2足歩行ロボット装置の全体構成を示すブロック図である。
図 4にお!/、て、 1は 2足歩行ロボット装置としてのロボット咅 (図 50、 51の 201、 201c に相当）、 2は歩行パターンを作成する歩行パターン作成装置としての歩行パターン 作成用コンピュータ、 3は商用電源 (AC100V)が供給される外部電源部、 4はロボッ ト部（2足歩行ロボット装置） 1の緊急停止等を行うための外部スィッチ、 5はロボット部 1を構成する腰部（図 50、 51の 202と 209に相当）、 6はロボット部 1を構成する脚部（ 下肢）（図 50の 201a、 201bに相当）である。図 4に示すように、ロボット部 1の腰部 5と 歩行パターン作成コンピュータ 2とは無線 (たとえば無線 LAN)を介して接続されて!ヽ る。腰部 5には外部電源部 3から電源が供給される。ただし、腰部 5に内蔵するバッテ リ（図 51の 231)を使用しているときは外部電源部 3は必要ない。
[0112] 図 5は、主に腰部 5を示すブロック図である。
図 5において、腰部 5は、制御用コンピュータ 7 (図 51の 2(W に相当）、モータ制 御部 9、ブレーキ制御部 10、電源部 11、スィッチ回路 12を有する。制御用コンビユー タ 7は、外部の歩行パターン作成用コンピュータ 2と無線 LANを介して接続され、ま た脚部 6からのセンサ情報を取得して処理する。また、制御用コンピュータ 7は、モー タ制御部 9にモータの速度指令値 (電圧）を出力すると共に、脚部 6のシリンダ Cl〜 C12 (後述する）に内蔵の DCサーボモータ（後述）のオン'オフ、ブレーキのオン'ォ フ、 DCサーボモータ端子の短絡機能を制御する信号をスィッチ回路 12に出力する 。電源部 11は、外部電源部 3から電源を供給されるが、バッテリ使用時は必要ない。 電源部 11は、脚部 6のセンサ、制御用コンピュータ 7、モータ制御部 9、ブレーキ制御 部 10、スィッチ回路 12に電源を供給する。スィッチ回路 12は、外部スィッチ 4および 制御用コンピュータ 7からの信号を受け取り、排他的論理和 (後述する）の論理に基 づき、 DCサーボモータのオン'オフ、ブレーキのオン'オフ、 DCサーボモータ端子の 短絡機能を制御する。モータ制御部 9は、制御用コンピュータ 7からの速度指令値に 従い、脚部 6の DCサーボモータの速度制御を行う。また、スィッチ回路 12からのサ ーボオン'オフ信号により DCサーボモータのオン'オフを行い、スィッチ回路 12から のモータ端子短絡信号により DCサーボモータ端子の短絡を行う。ブレーキ制御部 1 0は、スィッチ回路 12からの信号に従い、ブレーキの開閉を行う。
[0113] 図 6は、主に脚部 6を示すブロック図である。
図 6において、脚部 6は、 2本の脚部、右脚部 13aおよび左脚部 13bから成る。各脚 部 13a、 13bは、各々 6本のァクチユエータシリンダ C1〜C6、 C7〜C12 (図 50、 51 の直動リンク 235)と、右足底部 14a (図 50、 51の右足部 203に相当）および左足底 部 14b (図 50、 51の左足部 204に相当）と力ら成る。各シリンダの DCサーボモータ はモータ制御部 9からの電流により制御される。また、 DCサーボモータの端子間電 圧および電流はモータ制御部 9にフィードバックされる。各シリンダ C1〜C12のロー タリエンコーダ (後述する）は DCサーボモータの回転角度を検出し、制御用コンビュ ータ 7にフィードバックする。各シリンダ C1〜C12のオフセット検出用フォトマイクロセ ンサは、初期位置を検出し、制御用コンピュータ 7に信号を出力する。足底部 14a、 1 4bの 6軸カ覚センサ（後述する）は床反力情報を取得する。これは制御用コンビユー タ 7に記録される。各シリンダ C1〜C12のセンサ類には、電源部 11より電源が供給さ れる。
[0114] 図 7は、外部電源部 3を示すブロック図である。
図 7において、外部電源部 3は ACZDCコンバータ 31で構成される。 ACZDCコ ンバータ 31は、商用電源（交流 100V)を 48Vの直流電圧に変換し、ロボット部 1の腰 部 5の電源部 11に供給する。
図 8は、歩行パターン作成用コンピュータ 2を示すブロック図である。
図 8において、歩行パターン作成用コンピュータ 2は、 CPU21、メモリ 22、ハードデ イスク 23、入力装置 24、表示装置 25、インタフェース部 26、 LANカード 27、無線 L AN28力ら成り、歩行パターン作成ソフトウェアによりロボット部 1の歩行パターンを作 成する。また、無線 LAN28により制御用コンピュータ 7と通信することができ、歩行パ ターンを含むロボット制御ソフトウェアを制御用コンピュータ 7にロードする。
[0115] 図 9は、外部スィッチ 4を示すブロック図である。
図 9において、外部スィッチ 4は、ブレーキ制御スィッチ 41、サーボ ONスィッチ 42、 モータ端子短絡スィッチ 43から成る。各スィッチ 41〜43の状態はロボット部 1の腰部 5のスィッチ回路 12に信号として送られ、該当する機能が制御される。ブレーキ制御 スィッチ 41は各ァクチユエ一タシリンダ C 1〜C 12の保持ブレーキをオン 'オフする。 サーボ ONスィッチ 42は DCサーボドライバ（後述する）のサーボのオン.オフを制御 する。モータ端子短絡スィッチ 43は DCサーボドライバのモータ端子短絡スィッチを 制御する。モータ端子が短絡されると、モータの逆起電力により、モータが回転し難く なる。
[0116] 図 10は、主に制御用コンピュータ 7を示すブロック図である。
図 10において、制御用コンピュータ 7は、 CPU71、メモリ 72、ハードディスク 73など の基本的な構成要素と、パルスカウンタ 79等に対するインタフェース部 74、 LAN力 ード 75、無線 LAN76、 D/Aコンバータ 77、 6軸カ覚センサレシーバボード 78から 成る。制御用コンピュータ 7は、無線 LAN76により、歩行パターン作成用コンビユー タ 2と通信可能であり、歩行パターン作成用コンピュータ 2で作成されノヽードディスク 7 3に格納されているロボット制御ソフトウェアをメモリ 72にロードし、実行する。また、制 御用コンピュータ 7は、ロボット制御ソフトウェアで計算された速度指令値を DZAコン バータ 77を用いてアナログ電圧に変換してモータ制御部 9に出力すると共に、脚部 6 の各シリンダ C1〜C12のロータリエンコーダから出力されるモータ角度情報をパルス カウンタ 79を介して取得し、これを CPU71にフィードバックしてモータの位置制御を 行う。さらに、制御用コンピュータ 7は、必要に応じてスィッチ回路 12にブレーキ制御 信号、サーボ ON信号、モータ端子短絡信号を出力することにより、各機能を制御す ることができる。制御コンピュータ 7内の全ての電源は腰部 5の電源部 11から供給さ れる。
[0117] 図 11は、主に電源部 11を示すブロック図である。
図 1 Uこお!/ヽて、電源咅 lliま、 48Vニッケノレ水素ノ ッテリ 111 (図 51の 231【こネ目当） 、バッテリ/電源切替部 112、電源スィッチ 113、 4つの DC— DCコンバータ 114〜1 17から成る。電源部 11には、外部電源部 3より供給される電源と、 48Vニッケル水素 バッテリ 111により供給される電源とを選択するためのバッテリ Z電源切替部 112が 設けられている。 DC— DCコンバータ 114、 115は、制御用コンピュータ 7に電源を 供給するために用いられ、電圧は 5V、 ± 12Vである。 DC— DCコンバータ 116は、 ブレーキ制御部 10に電源を供給するためのもので、電圧は 24Vである。 DC— DCコ ンバータ 117は、脚部 6のセンサ類、スィッチ回路 12のデジタル ICなどのデジタル機 器に電源を供給するために用いられ、電圧は 5Vである。モータ制御部 9には、主電 源電圧つまりバッテリ 111または外部電源部 3の電圧がそのまま供給される。電源部 11の電源スィッチ 113をオンにすることによってロボット部 1に電源が供給される。
[0118] 図 12は、主にスィッチ回路 12を示すブロック図である。
図 12において、スィッチ回路 12は、ブレーキ制御部 10を介するブレーキのオン 'ォ フ、モータ制御部 9を介するサーボのオン'オフ、モータ制御部 9を介するモータ端子 短絡の各機能を制御するための論理回路 (排他的論理和） 123、 121、 122である。 スィッチ回路 12は、制御用コンピュータ 7からの信号と外部スィッチ 4からの信号との 2つを入力し、排他的論理和をとつて出力する。つまり、どちらかの入力が変化すると 、出力の状態は必ず変化するので、任意の入力の組み合わせにおいてロボット部 1 を緊急停止させることが可能である。スィッチ回路 12を構成するデジタル ICには、腰 部 5の電源部 11から電源が供給される。
[0119] 図 13は、主にブレーキ制御部 10を示すブロック図である。
図 13において、ブレーキ制御部 10は、半導体リレー 101で構成され、スィッチ回路 12からのブレーキオン ·オフ信号を入力とし、電源部 11より供給される電流をオン ·ォ フし、脚部 6の各シリンダ C1〜C12の保持ブレーキをオン'オフする。ブレーキ制御 部 10の制御する上記保持ブレーキは、電流を流した状態で解放状態になり、電流ォ フ（非通電状態)でブレーキが力かった状態になる。
[0120] 図 14は、主にモータ制御部 9を示すブロック図である。
図 14において、モータ制御部 9は 12個の DCサーボドライバ D1〜D12で構成され 、各ドライバ D1〜D12が脚部 6の各シリンダ C1〜C12に対応する。 DCサーボドライ バ D1〜D12に必要な電源は、腰部 5の電源部 11から供給される。 DCサーボドライ バ D1〜D12は、スィッチ回路 12からの信号（サーボオン'オフ信号）により、サーボ をオン'オフする。サーボオフ時はモータの端子は開放され、フリーな状態になる。ま た、 DCサーボドライバ D1〜D12は、スィッチ回路 12からの信号 (モータ端子短絡信 号）により、モータの端子を短絡する。モータ端子短絡時は、モータ自身の逆起電力 により、モータが回転し難くなる。さらに、 DCサーボドライバ D1〜D12は、制御用コ ンピュータ 7からの速度指令値 (電圧）を目標値とし、脚部 6のシリンダ C1〜C12のモ ータを速度制御する。さらに、 DCサーボドライバ D1〜D12は、モータに電流を供給 し、またモータの端子間電圧および電流を取得してフィードバックすることにより、速 度制御を可能にしている。
[0121] 図 15は、主にシリンダ C1〜C12を示すブロック図である。
図 15において、各シリンダ C1〜C12 (直動リンク 235)は、ロータリエンコーダ 151 ( 図 52、 53の 251【こネ目当）、 DCサーボモータ 152 (図 52、 53の 248【こネ目当）、保持ブ レーキ 153 (図 52、 53の 250に相当）、直動部 154およびオフセット検出用のフォトマ イク口センサ 155 (図 52、 53の初期位置センサ 247に相当）から構成される。ロータリ エンコーダ 151およびフォトマイクロセンサ 155には、腰部 5の電源部 11から電源が 供給される。 DCサーボモータ 152は、モータ制御部 9により速度制御される。保持ブ レーキ 153は、ブレーキ制御部 10により制御され、保持ブレーキ 153がオンになると 、 DCサーボモータ 152は摩擦力により固定される。ロータリエンコーダ 151は、 DCサ ーボモータ 152の相対回転角度を検出し、それに比例した数のパルスを制御用コン ピュータ 7に送る。フィードバックされた角度データをもとに、制御用コンピュータ 7は、 DCサーボモータ 152を位置制御することができる。 DCサーボモータ 152の回転運 動は、直動部 154で直動運動（直線運動）に変換され、左右それぞれ 6つのシリンダ の直動運動が足底部 14に伝えられる。このときの足底部 14の運動はスチュアートプ ラットホームの順運動学に従う。なお、歩行パターン作成時には逆運動学を用いる。 また、ロータリエンコーダ 151は、相対的な角度変位のみを検出するセンサであるの で、フォトマイクロセンサ 155を用いて初期位置を検出する。フォトマイクロセンサ 155 は、最もシリンダが縮んだ状態で反応し、シリンダが初期位置にあることを制御用コン ピュータ 7に知らせる。
[0122] 図 16は、主に足底部（足部） 14を示すブロック図である。なお、足底部 14は右足底 部 14a (203)と左足底部 14b (204)から成る（図 6、 45、 46参照）。
図 16において、足底部 14は、足底板 141 (図 51の 203a、 204aに相当）と 6軸力 覚センサ 142 (図 51の 203b、 204bに相当）力ら成る。 6本のシリンダ C1〜C6 (直動 リンク 235)の直動運動の結果として、足底板 141の運動が決定される。立脚時には 6軸カ覚センサ 142によって床反力情報を取得し、これを制御用コンピュータ 7で処 理することにより、実際の ZMPの位置を調べることができる。
[0123] 図 17は、主に直動部 154 (図 50、 51、 52、 53の直動リンク 235の直動部分）を示 すブロック図である。
図 17において、直動部 154は、動力伝達部 171 (図 52、 53の歯付プーリ 249、溝 付きベルト 252等力も構成）、すべりねじ 172 (図 53の雄螺子軸部 254に相当）、ナツ ト 173 (図 53の 255に相当）、リニアガイド 174 (図 52、 53のロッドレール部 245、レー ルガイド 246に相当）から構成される。動力伝達部 171は、 DCサーボモータ 152の 回転をすべりねじ 172の軸に伝え、また回転軸の軸心のずれを吸収する。すべりねじ 172の軸が回転すると、ナット 173が直動運動（直線運動、直進後退運動）をする。 ナット 173の回転はリニアガイド 174により抑制されるので、直進後退運動だけを取り 出すことができる。直動部 154の従動側の運動はジョイントを介して足底板 141に伝 えられる。
図 18は、歩行パターン作成用コンピュータ 2の CPU21における機能実現手段（ソ フトウェア構成）を示すブロック図である。
図 18において、 211はロボット部 1の全相数等の設定データを入力するための設 定データ入力手段、 212は足部 (足底部） 14の軌道と腰部 5の初期軌道とを設定す る軌道設定手段、 213は設定した足部 14の軌道と腰部 5の初期軌道とに基づいて目 標ゼロモーメントポイント（目標 ZMP)の軌道を設定する目標 ZMP軌道設定手段、 2 14は脚部の運動および腰部の運動による目標ゼロモーメントポイント回りのモーメン トを算出する目標 ZMP回りモーメント算出手段、 215は目標ゼロモーメントポイント回 りのモーメント等に対してフーリエ変換を行ってフーリエ係数を算出するフーリエ変換 手段、 216は算出したフーリエ係数に基づいてフーリエ係数を決定するフーリエ係数 決定手段、 217は算出した目標ゼロモーメントポイント回りのモーメント等に基づいて 腰部 5のモーメント補償軌道 (初期軌道に対してモーメントを補償した軌道)を算出す るモーメント補償軌道算出手段、 218は設定した足部 16の軌道と腰部 5のモーメント 補償軌道とにより各質点の加速度を算出する質点加速度算出手段、 219は算出され た各質点の加速度に基づいて目標ゼロモーメントポイント回りのモーメントであるエラ 一モーメントを算出するエラーモーメント算出手段、 220は算出したエラーモーメント が所定モーメントよりも小さいか否かを判定するエラーモーメント判定手段、 221は腰 座標系（数 27)から見た足部（「足底部」とも!/、う）の位置と姿勢 (ロール角、ョ一角、ピ ツチ角）を算出する足部位置姿勢算出手段、 222は歩容フラグを決定し記憶する歩 容フラグ決定手段、 223は腰部力 見た足部の位置 '姿勢の時系列データおよび腰 部から見た ZMP軌道および歩容フラグを歩行パターンとしてハードディスク 23のファ ィルに出力するファイル出力手段である。また、 目標 ZMP回りモーメント算出手段 21 4は、脚部 6の運動により生じる目標 ZMP回りモーメントを算出する下肢運動による 目標 ZMP回りモーメント算出手段 2141と、腰部 5の運動により生じる目標 ZMP回り の既知のモーメントを算出する腰運動による目標 ZMP回り既知モーメント算出手段 2 142とを有し、フーリエ変換手段 215は、目標 ZMP回りモーメント算出手段 214にお いて算出した目標ゼロモーメントポイント回りのモーメントと算出したエラーモーメント との総和を算出するモーメント総和算出手段 2151と、その算出した総和に基づ 、て フーリエ係数を算出するフーリエ算出手段 2152とを有し、足部位置姿勢算出手段 2 21は、足座標系 (数 28)の原点位置を算出する原点位置算出手段 2211と、足部の 姿勢を算出するための回転行列を求める回転行列算出手段 2212と、ロール角、ョ 一角、ピッチ角の姿勢角を算出する姿勢算出手段 2213とを有する。
[0125] 図 33は、制御用コンピュータ 7の CPU71における機能実現手段 (ソフトウェア構成 )を示すブロック図である。
図 33において、 711は電源部 11を介して電源を投入するための電源投入手段、 7 12は歩行パターン作成用コンピュータ 2から歩行パターンを無線で受信してハード ディスク 73に格納する歩行パターン設定手段、 713は脚部 6の脚を伸縮するシリンダ C1〜C12の初期位置を設定するシリンダ初期位置設定手段、 714はハードディスク 73に格納した歩行パターン力 歩行に使用するパターンを選択して歩行開始を指 令する歩行開始命令手段、 715は使用する歩行パターンに基づいてモータ (DCサ ーボモータ）を制御するモータ制御手段、 716は歩行動作の終了力否かを判定する 終了判定手段である。
[0126] (4)歩行パターン作成動作と 2足歩行ロボット装置の制御動作
このように構成された 2足歩行ロボット装置について、その動作を図 19〜図 36を用 いて説明する。まず、図 4〜図 18の 2足歩行ロボット装置における歩行パターン作成 用コンピュータ 2による歩行パターン作成動作について、図 19〜図 32を用いて説明 する。図 19〜図 31は歩行パターン作成用コンピュータ 2の CPU21の動作（歩行パタ ーン作成動作)を示すフローチャートであり、図 32は足座標系の原点位置の算出、 回転行列の算出および姿勢角算出について説明する説明図である。
[0127] 図 19は歩行パターン作成の全体動作を示す。
図 19において、まず、設定データ入力手段 211は、総歩数、足底部 14の位置と姿 勢、 1相の時間 (歩行速度)を設定データとして入力装置 24を介して入力する (S1)。 ここで、 1相の時間とは或る期間を分割したときの 1個分の時間長さであり、 1相毎に 2 足歩行ロボット装置に歩行パターンを与えることで連続した歩行動作を行うことができ る。次に、軌道設定手段 212は、足部 (足底部） 14の軌道と腰部 5の初期軌道とを設 定し (S2、軌道設定ステップ）、 目標 ZMP軌道設定手段 213は、設定した足部 14の 軌道と腰部 5の初期軌道とに基づいて目標ゼロモーメントポイント（目標 ZMP)の軌 道を設定する（S3、 目標 ZMP軌道設定ステップ)。 目標 ZMP回りモーメント算出手 段 214の下肢運動による目標 ZMP回りモーメント算出手段 2141は、脚部 6の運動 により生じる目標 ZMP回りモーメントを算出し (S4、下肢運動による目標 ZMP回りモ 一メント算出ステップ)、 目標 ZMP回りモーメント算出手段 214の腰運動による目標 Z MP回り既知モーメント算出手段 2142は、腰部 5の運動により生じる目標 ZMP回りモ 一メントを算出する（S5、腰運動による目標 ZMP回り既知モーメント算出ステップ)。 フーリエ変換手段 215のモーメント総和算出手段 2151は、 目標 ZMP回りモーメント 算出手段 214において算出した足部 14と腰部 5の運動による目標ゼロモーメントボイ ント回りのモーメントと算出したエラーモーメント (初期値はゼロ）との総和を算出し (モ 一メント総和算出ステップ）、フーリエ変換手段 215のフーリエ係数算出手段 2152は 、その算出した総和に基づいてフーリエ係数を算出し (S6、フーリエ係数算出ステツ プ）、フーリエ係数決定手段 216は、ステップ S6で算出したフーリエ係数に基づいて フーリエ係数を決定する（S7、フーリエ係数決定ステップ)。次に、モーメント補償軌 道算出手段 217は、ステップ S7で決定したフーリエ係数を逆フーリエ変換してモーメ ント補償軌道を算出し、この算出したモーメント補償軌道に基づいて腰部 5の軌道（ 腰部 5のモーメント補償軌道)を設定する (S8、モーメント補償軌道算出ステップ)。次 に、質点加速度算出手段 218は、設定した足部 14の軌道とステップ S8で算出した 腰部 5のモーメント補償軌道とにより各質点の加速度を算出して加速度の時系列デ ータを取得し (S9、質点加速度算出ステップ)、エラーモーメント算出手段 219は、算 出された各質点の加速度の時系列データに基づいて目標ゼロモーメントポイントに おけるエラーモーメントを算出する（S10、エラーモーメント算出ステップ)。次に、エラ 一モーメント判定手段 220は、算出したエラーモーメント（eM)が所定モーメント（ ε Μ)よりも小さいか否かを判定し (Sl l、エラーモーメント判定ステップ）、所定モーメン トよりも大きいと判定したときはエラーモーメント判定手段 220はフィードバックするェ ラーモーメントを算出し、その後ステップ S4へ戻る（S 12)。ステップ S 11でエラーモ 一メントが所定モーメントよりも小さいと判定した場合は原点位置算出手段 2211は足 座標系の原点位置を算出し、回転行列算出手段 2212は足部の姿勢を算出するた めの回転行列を算出し、姿勢算出手段 2213はロール角、ョ一角、ピッチ角の姿勢角 を算出する（S13、足部位置姿勢算出ステップ)。次に、歩容フラグ決定手段 222は、 立脚の前期、後期および遊脚の前期、後期に基づいて歩容フラグを決定し (S 14、 歩容フラグ決定ステップ）、ファイル出力手段 223は、腰部から見た足部の位置'姿 勢の時系列データおよび腰部から見た ZMP軌道を歩行パターンとしてハードデイス ク 23のファイルに出力する（S15)。
[0128] 次に、ステップ S2に示す軌道設定手段 212の動作を図 20 (a)〜（c)を用いて詳細 に説明する。図 20 (a)は足底部 14の軌道と腰部 5の初期軌道の設定動作を示し、図 20 (b)は一歩分の足底部 14の軌道設定動作を、図 20 (c)は一歩分の腰部 5の初期 軌道設定動作を示す。
[0129] まず図 20 (a)において、 iをゼロに設定し（S21)、 iが全相数より小さいか否かを判 定する（S22)。最初は i<全相数であるので、ステップ S23へ移行し、一歩分の足底 部 14の軌道を設定する（一歩分の足底部軌道設定処理)。次に、一歩分の腰部 5の 初期軌道を設定する（S24、一歩分の腰部初期軌道設定処理)。次に、 iを 1だけ増 加して（i=i+ lの演算を行って）ステップ S22へ戻る（S25)。ステップ S22〜S25を i ≥全相数と成るまで繰り返す。ステップ S22で i≥全相数と判定すると、この処理を終 了する。
[0130] 次に、ステップ S23の一歩分の足部軌道設定処理について図 20 (b)を用いて説明 する。
まず一歩の中間点と終点を設定し (S231)、一歩の片立脚期の相数 (或る期間を 所定時間たとえば 30msで分割したときの分割数）と一歩の相数とを設定する（S232 )。次に、 iを 1に設定し (S233)、 iが片立脚期の相数以下か否かを判定する（S234) 。最初は 1≤片立脚期の相数であるので、ステップ S235へ移行し、上記中間点と終 点に基づ 、て遊脚 (路面力 離れて 、る脚)の足底部の位置を 5次多項式 (5次多項 式とするのは、 2回微分しても充分になめらかである必要がある力もである）で近似し て求め、静止している立脚の足底部の位置を設定する（S236)。次に、 iを 1だけ増加 して（i=i+ lの演算を行って）ステップ S234へ戻る（S237)。ステップ S234〜S237 を 片立脚期の相数となるまで繰り返す。ステップ S234で 片立脚期の相数と判 定すると、ステップ S238へ移行し、 i≤一歩の相数か否かを判定し、最初は i≤一歩 の相数であるので、次に、静止している両脚の足底部 14の位置を設定する（S239) 。次に、 iを 1だけ増加して（i=i+ lの演算を行って)ステップ S238へ戻る（S240)。 ステップ S238〜S240を i>一歩の相数と成るまで繰り返す。ステップ S238で i>一 歩の相数と判定すると、この処理を終了する。
[0131] 次に、ステップ S 24の一歩分の腰部初期軌道設定処理について図 20 (c)を用いて 説明する。
まず iを 1に設定し (S241)、 iがー歩の相数以下力否かを判定する（S242)。最初 は i≤一歩の相数であるので、次に、腰部 5は一定の高さかつ一定速度で直線的に 移動するとして腰の位置を求める（S243)。次に、 iを 1だけ増加して (i=i+ lの演算 を行って）ステップ S242へ戻る（S244)。ステップ S242〜S244を i>一歩の相数と 成るまで繰り返す。ステップ S242で i>一歩の相数と判定すると、この処理を終了す る。
[0132] 次に、ステップ S3に示す目標 ZMP軌道設定手段 213の動作について図 21を用い て説明する。
まず、 jと iを 1に設定する（S31、 S32) 0次に、 ^片立脚期の相数力否かを判定し（ S33)、最初は 片立脚期の相数であるので、次に、 ZMPの位置を立脚側の足底 部 14の中心に設定する（S34)。次に、 iを 1だけ増加して (i=i+ lの演算を行って） ステップ S33へ戻る（S35)。ステップ S33〜S35を 1>片立脚期の相数と成るまで繰 り返す。ステップ S33で 片立脚期の相数と判定すると、次に、 i≤一歩の相数か否 かを判定し（S36)、最初は i≤一歩の相数であるので、次に、 ZMPの位置を 2つの足 底部 14 (右足底部 14aと左足底部 14b)の中間点に設定する（S37)。次に、 iを 1だ け増加してステップ S36へ戻る（S38)。ステップ S36〜S38を i>一歩の相数と成る まで繰り返す。ステップ S36で i>一歩の相数と判定すると、次に、 j<全相数か否か を判定し (S39)、最初は jく全相数であるので、 jを 1だけ増加してステップ S32へ戻 る（S40)。次に、再び iを 1に設定して、 j≥全相数となるまでステップ S32〜S38を繰 り返す。 j≥全相数と判定すると、この処理を終了する。
[0133] 次に、ステップ S4に示す下肢運動による目標 ZMP回りモーメント算出手段 2141の 動作について図 22を用いて説明する。
まず nを 1に、下肢の質点数を Nに設定し (S41)、 n≤全相数力否かを判定する（S 42)。最初は n≤全相数であるので、下肢の運動により目標 ZMPの回りに生ずるモ 一メント（ベクトル M)を算出する（S43)。ステップ S43に示すベクトル Mは、（数 2)に おいて外力をゼロとした場合のベクトルである。次に、 nを 1だけ増加してステップ S42 へ戻る（S44)。ステップ S42で n>全相数と判定されるまで、ステップ S42〜S44を 繰り返す。 n>全相数と判定すると、この処理を終了する。全相数とは例えば 1024 ( 2の 10乗）である。
[0134] 次に、ステップ S5に示す腰運動による目標 ZMP回り既知モーメント算出手段 214 2の動作について図 23を用いて説明する。
まず nを 1に設定し (S51)、 n≤全相数力否かを判定する（S52)。最初は n≤全相 数であるので、腰の運動により目標 ZMPの回りに生ずる既知モーメント（Mky, Mkx )を算出する（S53)。ステップ S53に示す既知モーメントは、（数 6) , (数 7)において 腰部 5に関する座標を未知とした場合のモーメントである（数 6、 7における左辺の既 知の項だけを抜き出し，右辺に移項したもの）。次に、 nを 1だけ増加してステップ S5 2へ戻る（S54)。ステップ S52で n>全相数と判定されるまで、ステップ S52〜S54を 繰り返す。 n>全相数と判定すると、この処理を終了する。
[0135] 次に、ステップ S6に示すフーリエ変換手段 215の動作について図 24を用いて説明 する。
まずモーメント総和算出手段 2151は、ステップ S4で求めたベクトル Mとステップ S5 で求めた既知モーメント（ベクトル Mk)と後述のステップ SI 2で求めたエラーモーメン ト（ベクトル E)との総和を求めると共に、全相数を Nに設定する（S61、モーメント総和 算出ステップ)。なお、エラーモーメントの初期値はゼロである。次に、フーリエ係数算 出手段 2152は、上記総和をフーリエ変換し (S62)、フーリエ係数を算出する（S63) 。フーリエ変換やフーリエ係数の算出は一般的手法である。 [0136] 次に、ステップ S7に示すフーリエ係数決定手段 216の動作について図 25を用いて 説明する。
まずフーリエ係数決定手段 216は、腰部 5のモーメント補償軌道をフーリエ級数で 表す（S71)。ステップ S71の An, Bn, Cn, Dn (n=0〜N— 1)はフーリエ係数であ る。次に、このフーリエ級数を (数 15)、（数 16)に代入してフーリエ係数を比較し (S7 2)、比較結果に基づいてフーリエ係数を決定する（S73)。
[0137] 次に、ステップ S8に示すモーメント補償軌道算出手段 217の動作について図 26を 用いて説明する。
まずモーメント補償軌道算出手段 217は、全相数を Nに設定し (S81)、逆フーリエ 変換用のデータ（つまりステップ S73で決定したフーリエ係数を元に設定した逆フー リエ変換用のデータ)を設定し（(数 25)参照）（S82)、腰部 5のモーメント補償軌道を 上記逆フーリエ変換用のデータを逆フーリエ変換することにより算出する（S83)。次 に、 iを 1に設定し（S84)、 i≤Nか否かを判定する（S85)。最初は i≤Nであるので、 次に、ステップ S83で算出したモーメント補償軌道に基づいて腰部 5の軌道を設定す る（S86)。次に、 iを 1だけ増加する（S87)。ステップ S85〜S87を i>Nとなるまで繰 り返し、 i>Nと判定すると、この処理を終了する。
[0138] 次に、ステップ S9に示す質点加速度算出手段 218の動作について図 27を用いて 説明する。
まず質点加速度算出手段 218は、 1相の時間を T (たとえば 30ms)に、 nを 1に、全 相数を Nに設定し（S91、 S92)、 n≤N力否かを判定する（S93)。最初は n≤Nであ るので、次に、 iを 1に、質点の相数を Jに設定し (S94)、 i≤Jか否かを判定する（S95) 。最初は i≤Jであるので、次に、離散時間における各質点の加速度 (ri (n)の 2階微 分)を計算する（S96)。次に、 iを 1だけ増加する（S97)。ステップ S95〜S97を i>Jと なるまで繰り返し、 i>Jであると判定すると、次に nを 1だけ増加し (S98)、ステップ S9 4〜S98を n>Nとなるまで繰り返し、 n>Nであると判定すると、この処理を終了する
[0139] 次に、ステップ S10に示すエラーモーメント算出手段 219の動作について図 28を 用いて説明する。 まずエラーモーメント算出手段 219は、 nを 1に設定し (S101)、 n≤全相数力否か を判定する（S102)。最初は n≤全相数であるので、次に、ステップ S43と同様にして エラーモーメント（eM (n) )を算出し（S 103)、 nを 1だけ増加する（S 104)。ステップ S 102〜3104を11 >全相数となるまで繰り返す。
[0140] 次に、ステップ S12に示すエラーモーメント判定手段 220の動作について図 29を 用いて説明する。図 29はエラーモーメントの算出方法について説明するものである。 まずエラーモーメント判定手段 220は、繰り返し回数を nとし (S121)、 nが 1の場合 はエラーモーメントを 0とし（S122、 S123)、 nが 2の場合はエラーモーメントを 1回目 で算出した値とし（S 124、 S 125)、 nが 3以上の場合は前回で使用したエラーモーメ ントに前回で算出したエラーモーメントの半分をカ卩えたものを今回のエラーモーメント とする（S126)。
n= lのときにエラーモーメントを 0とする理由について説明する。 n= lは、 1回目の 計算の後にフィードバックする値を表わしているのでは無くて、 1回目の計算で使うェ ラーモーメントを示している。すなわち、 n= lの場合のエラーモーメント E1を計算に 用いる時点では eMlは計算されておらず、フィードバックすべき量がない。そのため に E1 = 0というステップを便宜上入れてある。初めて計算される算出エラーモーメント eM 1が E2に代入されるのも同様の理由で、 eM 1が計算された直後は繰り返し回数 n = 2となるために、 E2に代入されて 2回目の計算に使用される。
[0141] 次に、ステップ S13に示す足部位置姿勢算出手段 221の動作について図 30を用 いて説明する。
まず、 nを 1に設定し (S 131)、 n≤全相数か否かを判定する（S 132)。最初は n≤ 全相数であるので、原点位置算出手段 2211は、足座標系の原点位置を算出し (S1 33、後述の図 32および (数 82)参照）、回転行列算出手段 2212は足部の姿勢を算 出するための回転行列を算出し (S134、後述の (数 87)参照）、姿勢算出手段 2213 はロール角、ョ一角、ピッチ角の姿勢角を算出する（S135、後述の（数 91)参照)。 次に、 nを 1増加し、ステップ S132へ戻る。これを n>全相数となるまで繰り返す。 n> 全相数となると、この処理を終了する。
[0142] ここで、足座標系の原点位置の算出、回転行列の算出および姿勢角算出について 図 32を用いて説明する。
従来は、絶対座標系における足部位置 ·姿勢および腰部位置 ·姿勢から逆運動学 により各シリンダ長さ（つまり腰部ジョイントと足部ジョイントとの間の距離)を算出し、こ れを歩行パターンとして出力していた力 本実施の形態では、次のような手順で、絶 対座標系における足部位置 ·姿勢および腰部位置 ·姿勢から、腰座標系から見た足 座標系の位置'姿勢を算出し、これを歩行パターンとして出力する。
腰座標系力 見た足座標系原点の位置ベクトル (数 81)は、足部と腰部の絶対座 標から、（数 82)となる（図 32参照)。
[数 81]
X f = ( x f y f z f )
[数 82]
また、腰座標系から見た足座標系の姿勢を表わす回転行列 (数 83)と、絶対座標 系から見た足座標系の姿勢を表わす回転行列 (数 84)と、絶対座標系から見た腰座 標系の姿勢を表わす回転行列 (数 85)の関係は (数 86)、（数 87)のようになる。
[数 83]
Tw→f
[数 84]
R foot
[数 85]
^ ai st
[数 86]
1 w-→f °wai st , foot
[数 87]
一 1
τ 1 w→f — R foot ς wai st ここで、（数 84)、（数 85)は（数 88)、 (数 89)のように表わされる c [数 88]
Θ foot = ( ， 8y θζ)
Figure imgf000086_0001
R foot Rx R y R■
[数 89]
(Φχ， Φ Φζ)
Figure imgf000086_0002
°waist一 x y S； このようにして求めた、腰座標系力も見た足座標系の姿勢を表わす回転行列 (数 8 3)より、次の手順でロール角、ョ一角、ピッチ角に換算する。
腰座標系力 見た足座標系の姿勢を表わす回転行列 (数 83)の成分を (数 90)の ようにおく。
[数 90]
Figure imgf000087_0001
これらより、 2変数逆正接関数を用い、腰座標系から見た足座標系の姿勢を表わす ロール角、ョ一角、ピッチ角は、（数 91)のようになる。
[数 91]
Figure imgf000087_0002
この腰座標系から見た足座標系の位置 ·姿勢 (数 92)、（数 93)を両足分算出し、歩 行パターンとして出力する。
[数 92]
Xf = ( x f ， y f ， z f )
[数 93] = ( ， Ψζ )
ここで、（数 91)について説明する。 Atan2 (y, x)は (数 94)として計算される力 結 果の角度が存在する象限は、 Xと yの両方の符号によって決定される。例えば、 Atan 2 (- 2. 0, - 2. 0) =— 135° である。 [数 94] t an 1 (y/x)
[0144] 次に、ステップ S14の歩容フラグの算出について図 31を用いて説明する。まず、相 数 nを" 1"に設定し (SS51)、 nが全相数以下か否かを判定する（SS52)。最初は n は全相数以下であるので、ステップ SS53へ移行して立脚か否かを判定する。ステツ プ SS53で立脚と判定したときは次に立脚の前期力否かを判定する（SS54)。立脚 か否かの判定や立脚の前期力否かの判定は例えば歩行経過時間や、または足部と 路面の距離や速度と対応付けて行う。立脚前期と判定されたときは歩容フラグ F[n] を" 0"に設定し (SS55)、立脚前期でない (つまり立脚後期である）と判定したときは 歩容フラグ F[n]を" 1"に設定する（SS56)。ステップ SS53で立脚でな ヽ（つまり遊 脚である）と判定されたときは次に遊脚の前期カゝ否かを判定する（SS57)。遊脚の前 期か否かは例えば遊脚が上昇中である力否かによって判定する。ステップ SS57で 遊脚前期と判定されたときは歩容フラグ F[n]を" 2"に設定し (SS58)、遊脚後期と判 定されたときは歩容フラグ F[n]を" 3"に設定する（SS59)。ステップ SS55、 56、 58、 59で歩容フラグを設定した後は、相数 nを 1つ増加する（SS60)。ステップ SS52で 相数 nが全相数を越えたと判定したときはこの処理を終了する。
[0145] 次に、 2足歩行ロボット装置の制御動作について、図 33〜図 37を用いて説明する 。図 33は制御用コンピュータ 7の CPU71における機能実現手段を示すブロック図で あり、図 34〜図 36は制御用コンピュータ 7の CPU動作を示すフローチャートであり、 図 37は CPU71のモータ制御手段 715を示す機能ブロック図である。
[0146] まず 2足歩行ロボット装置の制御動作の全体動作について図 34を用いて説明する 図 34にお 、て、電源投入手段 711は電源部 11を介して 2足歩行ロボット装置の電 源を投入する（SS1)。次に歩行パターン設定手段 712は、歩行パターン作成用コン ピュータ（歩行パターン作成装置） 2から歩行パターンを無線で受信してハードデイス ク 73に格納し (SS2、歩行パターン設定ステップ）、シリンダ初期位置設定手段 713 は、脚部 6の脚を伸縮するシリンダ C1〜C12の初期位置を設定する（SS3、シリンダ 初期位置設定ステップ)。次に、歩行開始命令手段 714は、ハードディスク 73に格納 した歩行パターン力も歩行に使用するパターンを選択して歩行開始を指令し (SS4、 歩行開始命令ステップ）、モータ制御手段 715は、使用する歩行パターンに基づい てモータ（DCサーボモータ） 152を制御する（SS5、モータ制御ステップ）。次に、終 了判定手段 716は、歩行動作の終了力否かを (終了命令が制御用コンピュータ 7か ら出て 、る力否かを)判定する（SS6)。
[0147] 次に、ステップ SS2に示す歩行パターン設定手段 712の動作を図 35を用いて説明 する。
図 35において、まず、設定したいパターンの数を Nに、 jをゼロに設定する（SS21) 。次に、; j <N力否かを判定する（SS22)。最初は; j <Nであるので、次に、歩行パター ン作成用コンピュータ 2で作成したデータ (腰座標系における足部の位置 ·姿勢、絶 対座標系における腰軌道、絶対座標系における ZMP軌道）を制御用コンピュータ 7 のメモリ 72に記憶する（SS23)。次に、 jを 1だけ増加する（SS24)。ステップ SS22〜 SS24を j≥Nとなるまで繰り返す。 j≥Nであると判定すると、この処理を終了する。
[0148] 次に、ステップ SS3に示すシリンダ初期位置設定手段 713の動作を図 36を用いて 説明する。
図 36において、まず、 D/ Aコンバータ 77から一定電圧を各モータドライノく（DCサ ーボドライノく） D1〜D12に出力し、低速かつ一定速度でシリンダ C1〜C 12を縮める 方向に動かす (SS31)。次に、初期位置検出用のセンサ 155が反応したか否かを判 定し (SS32)、反応しない場合にはステップ SS31へ戻り、反応した場合には反応し たセンサがついているシリンダに位置制御（PI制御）をかけて固定する（SS33)。ステ ップ SS31〜SS33の動作を全てのセンサが反応するまで行う（SS34)。全てのセン サが反応したと判定した場合は、全てのシリンダを動かして、 2足歩行ロボット装置を 歩行パターンに基づく初期位置にする（SS35)。
[0149] 次に、モータ制御手段 715について図 37を用いて説明する。図 37はモータ制御 手段 715を示す機能ブロック図である。
図 37において、 810は基本制御を行う基本制御手段、 820は着地制御を行う着地 制御手段である。基本制御手段 810は、メモリ 72に記憶されている歩行パターンを 出力する歩行パターン出力手段 811と、後述の z方向着地軌道修正量を加算する z 方向加算手段 8121と非線形コンプライアンス移動量を加算するコンプライアンスカロ 算手段 8122とロール'ピッチ方向軌道修正量を加算するロール'ピッチ加算手段 81 23とを有する補正量加算手段 812と、足部の位置'姿勢の目標値に対して逆運動学 演算を行ってシリンダ長さのデータ (つまりリンク長さのデータ)を生成する逆運動学 演算手段 813と、シリンダ長さのデータをモータの角度変位のデータに変換するデ ータ変換手段 814と、シリンダの初期位置の微調整を行う初期位置微調整手段 815 と、後述のシリンダ駆動装置 400 (図 37参照）を駆動するシリンダ駆動装置駆動手段 816とを有する。
[0150] また、着地制御手段 820は、歩容フラグ F[n]に基づいて制御モードを選択するモ ード選択手段 821と、歩行パターンの各成分を着地制御の制御対象となる成分 (制 御対象成分)と制御対象とならな!/ヽ成分 (非制御対象成分)とに分割する成分分割手 段 822と、上記制御モード等に基づいて z方向着地軌道修正量を算出する z方向着 地軌道修正量算出手段 823と、非線形コンプライアンス移動量を z方向圧力（床反力 )から算出する非線形コンプライアンス移動量算出手段 824と、ロール'ピッチ方向の トルクからロール.ピッチ方向軌道修正量を算出するロール.ピッチ方向軌道修正量 算出手段 825と、加算する修正量や移動量に基づいて新たな歩行パターンを作成 する歩行パターン修正手段 826とを有する。さらに、 z方向着地軌道修正量算出手段 823は、理論コンプライアンスを算出する理論コンプライアンス移動量算出手段 823
1と、理論コンプライアンス移動量と非線形コンプライアンス移動量との偏差をコンプ ライアンス移動量偏差 (着地路面高さ誤差)として算出するコンプライアンス移動量偏 差算出手段 8232と、歩容フラグに基づく制御モードとコンプライアンス移動量偏差と に基づいて z方向着地軌道修正量を算出する軌道修正量算出手段 8233とを有する 。さらに、ロール'ピッチ方向軌道修正量算出手段 825は、ロール'ピッチ方向のコン プライアンス移動量をロール'ピッチ方向のトルク力 算出するコンプライアンス移動 量算出手段 8251と、ロール'ピッチ方向の軌道修正量を算出する軌道修正量算出 手段 8252とを有する。
[0151] このように構成されたモータ制御手段 715の動作について説明する。
まず、基本制御手段 810について、図 38、図 39を用いて説明する。図 38はモータ 制御手段とモータ駆動装置を示すブロック図である。図 39は基本制御手段 810の動 作を示すフローチャートである。
図 38において、歩行パターン出力手段 811、逆運動学演算手段 813、データ変換 手段 814、初期位置微調整手段 815、シリンダ駆動装置駆動手段 816は図 37と同 様のものであり、同一符号を付し、説明は省略する。
このように構成された基本制御手段 810の動作について図 39を用いて説明する。 図 39において、まず、歩行パターンが選択され、歩行開始命令が出されると（歩行 パターン選択と歩行開始命令とは外部のコンピュータ力 無線を介して伝えられる。 通常は歩行パターン作成用コンピュータ 2がこの役割を果たしている）、歩行パターン 出力手段 811は、腰座標系における足部の位置 ·姿勢のデータや絶対座標系にお ける腰座標 ·ΖΜΡ座標のデータ（以下、「足部等データ」という）を制御周期（約 lms) 毎に補間して出力する（SS41)。この足部等データに対して補正量加算手段 812が 補正量を加算する場合もあるが、基本制御においては補正量の加算は行わない（S S42) 0次に、逆運動学演算手段 813は、足部等データに対して前述した逆運動学 演算を行ってシリンダ長さのデータを算出し (SS43)、データ変換手段 814は、算出 したシリンダ長さのデータをモータの角度変位のデータに変換する（SS44)。このモ ータ角度変位データに初期位置微調整手段 815からの初期位置の微調整データ（ SS45)を加えた加算データ aをシリンダ駆動装置駆動手段 816が比較部 COMに出 力する（SS46)。加算データ aはパルスカウンタ 79からの実角度変位データ bと比較 部 COMで比較され、差データ（a— b)となる。この差データ（a— b)の積分データと比 例データとは比例積分部 300において加算されてモータ回転速度データ cとなり、こ のデータ cは換算部 301でデジタル電圧に換算されて、 DZAコンバータ 77に出力さ れ、 DZ Aコンバータ 77でアナログ電圧となり、このアナログ電圧はサーボドライバ D 1〜D12に入力され、サーボドライバ D1〜D12は上記アナログ電圧により DCサーボ モータ 152を回転駆動する。 DCサーボモータ 152の回転変位 (角度変位）はロータ リエンコーダ 151でパルス数として検出され、このパルス数はパルスカウンタ 79で計 数されて比較部 COMへフィードバックされる。ここで比較部 COM、比例積分部 300 、換算部 301、 DZAコンバータ 77、サーボドライバ D1〜D12、 DCサーボモータ 15 2、エンコーダ 151、カウンタ 79はシリンダ駆動装置 400を構成する。
[0153] このように、本実施の形態においては、ノ ラレルメカニズムの脚機構にとっては計算 量が膨大なために実施が困難な順運動学を行う必要がなくなり、計算が容易な逆運 動学によりリアルタイムにリンク長さ（つまりシリンダ長さ）を制御することができるので、 足部の位置 '姿勢に関して種々の制御を行うことができるようになり、例えば着地衝撃 を緩和するための仮想コンプライアンス制御や凹凸や傾斜のある不整路面を支障な く歩行するための着地制御を行うことができるようになる。
[0154] 次に、着地制御手段 820の動作について、図 37、図 40〜図 44を用いて説明する 。図 40は着地制御を示す機能的ブロック図であり、図 41 (a)〜（c)は各ブロックを示 すブロック説明図、図 42 (a)、（b)は同じく各ブロックを示すブロック説明図、図 43は 着地制御を含む全体制御（モータ制御手段 715の動作)を示すフローチャート、図 4 4は着地制御手段 820の動作を示すフローチャートである。
[0155] 図 40において、歩行パターン出力手段 811、逆運動学演算手段 813、データ変換 手段 814、初期位置微調整手段 815、シリンダ駆動装置駆動手段 816、モータ駆動 装置 400、モード選択手段 821、成分分割手段 822、非線形コンプライアンス移動量 算出手段 824、歩行パターン修正手段 826、 z方向加算手段 8121、コンプライアン ス加算手段 8122、ロール'ピッチ方向加算手段 8123、理論コンプライアンス移動量 算出手段 8231、コンプライアンス移動量偏差算出手段 8232、軌道修正量算出手 段 8233、ロール'ピッチ方向移動量算出手段 8251、軌道修正量算出手段 8252は 図 37と同様のものであり、同一符号を付し、説明は省略する。 142は足部の 6軸の力 成分 (位置および姿勢角に関する力成分)を検出する 6軸カ覚センサ、 142aは 6軸 カ覚センサ 142からのデータに基づいて各軸の力やモーメントを出力するレシーバ ボードである。
[0156] ここで、非線形コンプライアンス移動量算出手段 824における非線形コンプライアン ス移動量の算出について説明する。
(数 50)の条件を示す図 3 (d)から、（数 95)の関係式が求まる。
[数 95] zR = Kz i · Δ Z f cc acR ( t ) +Cz i · Δ Z f c c aCR ( ^ ) 非線形コンプライアンス移動量の微分値を離散化して示すと、（数 96)となり、これ を用いて (数 95)は (数 97)に展開される。この (数 97)を変形すると (数 98)となり、こ れから (数 99)のようにして非線形コンプライアンス移動量が算出される。
[数 96]
Λ ' "、 一 △ Z fccacR ( t /— Δ Z fccacR "一△ t )
ccacR、て —— ·
[数 97]
c — Λ 、丄广 AzfccacR(t)-AzfccacR(t-At) 卜 zR一1^ zl · Δ Z fccacR てゾ し zl
[数 98]
丄
zl丁
Figure imgf000093_0001
[数 99]
Λ 7 (f) - ( , (F , r AzfccacR(t-At)
Δ Z fccacR — KZ1十 · 、ト ZR十し z1 ^
(数 52)の条件を示す図 3 (e)から、（数 100)の関係式が求まる。
[数 100]
FZR = Kzi · A Z fccacR \ c) + v^zi * △ Z fccacR (
+ ( z2-Kz1) (Δζ fccacR (t) -Δ Ι)
+ (Cz2— Czl) (Δζ fccacR (t) -Δ Ι)
= Kzl ·△ zfccacR (t) + Czl · Δ zfccacR (t) + (Kz2-Kz1 ) Δ Z fccacR (t) -(Κζ2-Κζ1)Δ l+(Cz2-Cz1)AzfccacR(t)
(数 100)を離散化して示すと、（数 101)となり、（数 101)は (数 102)に展開される 。この (数 102)を変形すると (数 103)となり、非線形コンプライアンス移動量が算出さ れる。
[数 101] - „ * ヽ ， ハ Δ Z fccacR (ΐ)-Δ Ζ f ccacR (t-Δΐ)
"zR— · Δ Z fccacR、て 十し zl ^
+ (Kz2— Kz1) AzfccacR(t)一（Kz2— Κζ1)Δ Ι 一 、 Δ Z fCCacR ( t )— A Ζ fccacR ( t ~ A t )
十、し z2一し z1 ) » - fccacR (ΐ)-Δζ fccacR (t-Δΐ)
= Κζ2·厶 zfccacR(t) + Cz2 Δ ^ 一（Kz2 - Κζ1)Δ I
[数 102] AzfccacR(t-At)
Figure imgf000094_0001
+ ( ζ2-Κζ1)Δ I
[数 103]
/
△ Z fccacR (t) · 11 zR+△セ △ Z fccacR
Figure imgf000094_0002
+ (Κζ2-Κζ1)Δ 次に、着地制御について図 40の機能的ブロック図を用いて説明する。
図 40において、モード選択手段 821は、歩行パターン出力手段 811から出力され た歩容フラグ F[n]に基づき、モード選択テーブルを用いて、制御モードを選択する。 モード選択テーブルを (表 1)に示す。
[表 1] 右足 右足
Roll, Pitch Roll, Pitch フラグ m フラグ •m Z OKIODEzR) (MODE^R) Z (MODEzL) (M0DE9L)
0 ίζί繊 0 KeepPeriod KeepPeriod KeepPeriod KeepPeriod
0 1 mm KeepPeriod KeepPeriod KeepPeriod KeepPeriod
0 inmrn 2 mmm ReturnPeriod KeepPeriod KeepPeriod ReturnPeriod
0 3遡翻 ReturnPeriod KeepPeriod ContolPeriod Compl iancePeriod
1 m 0 KeepPeriod KeepPeriod KeepPeriod KeepPeriod
1 tmm 1 mm KeepPeriod KeepPeriod KeepPeriod KeepPeriod
1 mm 2 mmm ReturnPeriod KeepPeriod KeepPeriod ReturnPeriod
1 tmm 3遡誦 ReturnPeriod KeepPeriod ContolPeriod Compl iancePeriod
2 mmm 0 m KeepPeriod ReturnPeriod ReturnPeriod KeepPeriod
2遡繊 1 mm KeepPeriod ReturnPeriod ReturnPeriod KeepPeriod
2遡繊 2 mmm
2遡議 3遡囊
3遡删 0 mm ContolPeriod Compl iancePeriod ReturnPeriod KeepPeriod
3遡議 1 麵鰂 Conto I Period Com l iancePeriod ReturnPeriod KeepPeriod
3蘭鲷 2遡讀
3遡翻 3 mm
(表 1)に示すように、歩容フラグに基づいて、 z方向については、維持期間 (Keep Period)、復帰期間（Return Period)、制御期間（Control Period)が選択され、 また、ロール'ピッチ方向については、維持期間、復帰期間、コンプライアンス期間（C ompliance Period)が選択される。
成分分割手段 822は、歩行パターン出力手段 811から出力された歩行パターンの 各成分について、制御対象成分か非制御対象成分かを判定し、制御対象成分のみ について着地制御を行レ、、非制御対象成分は歩行パターン修正手段 826へ出力す る。制御対象成分はここでは、 z、ロール（ θ X)、ピッチ（ Θ y)の成分である。
次に、理論コンプライアンス移動量算出手段 8231は、制御対象成分および (数 42 )の着地路面検知用移動許容量から、理論コンプライアンス移動量を算出する（ (数 4 6)、（数 48)参照)。
一方、非線形コンプライアンス移動量算出手段 824は、 6軸カ覚センサ 142から出 力された z方向の床反力を入力して非線形コンプライアンス移動量を算出する。理論 コンプライアンス移動量算出手段 8231からの理論コンプライアンス移動量と非線形 コンプライアンス移動量算出手段 824からの非線形コンプライアンス移動量との偏差 はコンプライアンス移動量偏差算出手段 8232においてコンプライアンス移動量偏差 ((数 58)参照）として算出され、軌道修正量算出手段 8233に入力される。軌道修正 量算出手段 8233は、このコンプライアンス移動量偏差とモード選択手段 821からの 制御モードとに基づいて z方向着地軌道修正量を算出する。制御モード期間が制御 期間であるときには、 z方向着地軌道修正量は (数 104)となる。ここで、（数 105)はゲ インである。
[数 104]
HR (t) = HR(t-At)-KE- eR(t-At)
[数 105]
KE
制御モード期間が維持期間であるときには z方向着地軌道修正量は (数 106)となり 、復帰期間であるときには (数 107)となる。（数 107)において、（数 108)は維持期間 の最後の値を示し、（数 109)は復帰期間が始まってからの時間、（数 110)は復帰期 間の長さを示す。したがって、（数 107)は、復帰期間の最初においては維持期間の 最後の値である軌道修正量を復帰期間の最後においてゼロとするような式である。
[数 106]
HR (t) = HR(t-△り
[数 107]
Figure imgf000096_0001
[数 108]
[数 109]
'zR [数 110]
ZR
次に、 z方向加算手段 8121は、成分分割手段 822からの歩行パターンの z成分と 軌道修正量算出手段 8233からの z方向着地軌道修正量と記憶部 827に格納されて いる (数 42)の着地路面検知用移動許容量とを加算して z方向修正済軌道として出 力する。また、コンプライアンス加算手段 8122は、非線形コンプライアンス移動量算 出手段 824からの非線形コンプライアンス移動量と z方向加算手段 8121からの z方 向修正済軌道とを加算して z方向最終軌道として出力する。
次に、コンプライアンス移動量算出手段 8251は、 6軸カ覚センサ 142から入力した ロール方向、ピッチ方向のトルクに基づき、（数 65)、（数 66)を用いて、（数 67)の口 ール方向コンプライアンス移動量、（数 68)のピッチ方向コンプライアンス移動量を算 出する。軌道修正量算出手段 8252は、コンプライアンス移動量算出手段 8251から のロール方向コンプライアンス移動量とピッチ方向コンプライアンス移動量とに基づき
、モード選択手段 821からの制御モードを用いて、ロール'ピッチ方向軌道修正量を 算出する。制御モード期間がコンプライアンス期間であるときには、ロール方向軌道 修正量は (数 111)となり、ピッチ方向軌道修正量は (数 112)となる。
[数 111]
△ 0xf l cR ( t ) = A 0xfccR ( t)
[数 112]
A 0yf l cR ( t ) = A 0yf ccR ( t)
制御モード期間が維持期間であるときにはロール方向軌道修正量は (数 113)とな り、ピッチ方向軌道修正量は (数 114)となる。復帰期間であるときにはロール方向軌 道修正量は (数 115)となり、ピッチ方向軌道修正量は (数 116)となる。（数 117)、 ( 数 118)は維持期間の最後の値を示し、これらの式は、復帰期間の最初においては 維持期間の最後の値である軌道修正量を復帰期間の最後においてゼロとするような 式である。
[数 113] △0Xf icR(t) = A5xf lcR (tー厶 t)
[数 114]
△0yf lcR(t) = A6yf lcR (t-Δΐ)
[数 115]
Adx cR(t)
Figure imgf000098_0001
[数 116]
/ 5 , 4 2 /
6t SR—15T0R t eR +1 TeR t 0R一 T 0R
Figure imgf000098_0002
T
[数 117]
△ ^xO
[数 118]
Δ 6yOR
[0160] 次に、ロール'ピッチ方向加算手段 8123は、成分分割手段 822からのロール'ピッ チ成分と軌道修正量算出手段 8252からのロール'ピッチ方向軌道修正量とを加算し 、ロール'ピッチ方向最終軌道として歩行パターン修正手段 826に出力する。歩行パ ターン修正手段 826は、 z方向最終軌道とロール ·ピッチ方向最終軌道と非制御対象 成分とを入力して歩行パターンを修正する。この修正した歩行パターンに対して逆運 動学演算手段 813で逆運動学演算を行うが、逆運動学演算手段 813以降について 既に説明されている。
[0161] 次に、図 37の着地制御手段 820の動作について、図 43、図 44を用いて説明する 。図 43は、着地制御における全体制御（すなわち基本制御に着地制御を加えた制 御）を示すフローチャートであり、図 44は着地制御を詳細に示すフローチャートであ る。図 43は、図 39の補正量力卩算のステップ SS42に代えて着地制御のステップ SS4 2aとしたものである。着地制御の動作は、図 44において後述するように、補正量 (修 正量と移動量)を加算する動作と、加算結果に対して歩行パターンを修正する歩行 ノ ターン修正の動作とを含む。すなわち、基本制御において着地制御を含む場合に は図 39の補正量力卩算のステップ SS42が図 43のステップ SS42aに代わるだけであ る。
図 43のステップ SS42aの着地制御を図 44を用いて説明する。
[0162] 図 44において、モード選択手段 821は、歩行パターン出力手段 811から出力され た歩容フラグ F[n]に基づき、モード選択テーブルを用いて、制御モードを選択する（ SS61)。モード選択は前述したように (表 1)のように行われる。次に、成分分割手段 822は、歩行パターン出力手段 811から出力された歩行パターンの各成分について 、制御対象成分か非制御対象成分かを判定し、制御対象成分のみについて着地制 御を行い、非制御対象成分は歩行パターン修正手段 826へ出力する（SS62)。制 御対象成分はここでは、 z、ロール（ θ X)、ピッチ（ Θ y)の成分である。すなわち、 z、口 ール、ピッチの成分についてのみ以下の処理が行われ、 x、 y、ョ一については以下 の処理が行われない。ただし、最終ステップ SS72の歩行パターン修正処理はすべ ての成分を用いて行われる。
次に、理論コンプライアンス移動量算出手段 8231は、制御対象成分および (数 42 )の着地路面検知用移動許容量から、理論コンプライアンス移動量を算出する（ (数 4 6)、（数 48)参照）（SS63)。
[0163] 次に、非線形コンプライアンス移動量算出手段 824は、 6軸カ覚センサ 142から出 力された z方向の床反力を入力して非線形コンプライアンス移動量を算出する（ (数 5 1)、（数 53)参照）（SS64)。理論コンプライアンス移動量算出手段 8231からの理論 コンプライアンス移動量と非線形コンプライアンス移動量算出手段 824からの非線形 コンプライアンス移動量との偏差はコンプライアンス移動量偏差算出手段 8232にお いてコンプライアンス移動量偏差（(数 58)参照）として算出され、軌道修正量算出手 段 8233〖こ入力される（SS65)。軌道修正量算出手段 8233は、このコンプライアンス 移動量偏差とモード選択手段 821からの制御モードとに基づいて z方向着地軌道修 正量を算出する（(数 104)、（数 106)、（数 107)参照）（SS66)。
[0164] 次に、 z方向加算手段 8121は、成分分割手段 822からの歩行パターンの z成分と 軌道修正量算出手段 8233からの z方向着地軌道修正量と記憶部 (メモリ） 827に格 納されている (数 42)の着地路面検知用移動許容量とを加算して z方向修正済軌道 として出力する（SS67)。また、コンプライアンス加算手段 8122は、非線形コンプライ アンス移動量算出手段 824からの非線形コンプライアンス移動量と z方向加算手段 8 121からの z方向修正済軌道とを加算して z方向最終軌道として出力する（SS68)。
[0165] 次に、コンプライアンス移動量算出手段 8251は、 6軸カ覚センサ 142から入力した ロール方向、ピッチ方向のトルクに基づき、（数 65)、（数 66)を用いて、（数 67)の口 ール方向コンプライアンス移動量、（数 68)のピッチ方向コンプライアンス移動量を算 出する（SS69)。軌道修正量算出手段 8252は、コンプライアンス移動量算出手段 8 251からのロール方向コンプライアンス移動量とピッチ方向コンプライアンス移動量と に基づき、モード選択手段 821からの制御モードを用いて、ロール'ピッチ方向軌道 修正量を算出する（(数 111)〜(数 116)参照） (SS70)。
次に、ロール'ピッチ方向加算手段 8123は、成分分割手段 822からのロール'ピッ チ成分と軌道修正量算出手段 8252からのロール'ピッチ方向軌道修正量とを加算し 、ロール'ピッチ方向最終軌道として歩行パターン修正手段 826に出力する（SS71)
[0166] 歩行パターン修正手段 826は、 z方向最終軌道とロール ·ピッチ方向最終軌道と非 制御対象成分とを入力して新たな歩行パターンを作成する（SS72)。この新たな歩 行パターンに対しては逆運動学演算以降の処理が行われる（図 43のステップ SS43 〜SS46)。
[0167] 以上のように本実施の形態によれば、足部を備えた脚部と腰部とから成る 2足歩行 ロボット装置の歩行パターンを作成する歩行パターン作成装置であって、足部にお V、て目標ゼロモーメントポイントを設定し、設定した目標ゼロモーメントポイントに応じ て腰部のモーメント補償軌道を算出し、絶対座標系における腰座標系の原点と絶対 座標系における足座標系の原点とから腰座標系から見た足部の位置を算出し、絶対 座標系から見た足座標系の姿勢を表わす回転行列と絶対座標系から見た腰座標系 の姿勢を表わす回転行列とから腰座標系から見た足座標系の姿勢を表わす回転行 列を算出して足座標系の姿勢を表わす回転行列により足部の姿勢を算出し、歩行パ ターンにおける立脚前期か後期力及び遊脚前期か後期かを示すフラグである歩容フ ラグを決定し記憶することにより、 2足歩行ロボット装置の腰部のモーメント補償軌道 により目標ゼロモーメントポイントにおけるモーメントをゼロに近づけることができるの で、 2足歩行ロボットの転倒を防止して歩行を安定ィ匕することができ、また、腰座標系 からみた足部の位置と姿勢のデータを生成することができるので、パラレルリンク機構 においてはリアルタイム演算が困難な順運動学演算を行う必要がなぐノラレルリンク 機構においてはリアルタイム演算が容易な逆運動学演算により脚部を伸縮するシリン ダの長さを制御することができ、逆運動学演算において種々の補正データを用いて 種々の制御を行うことができ、 2足歩行ロボットの歩行を更に安定ィ匕することができ、 さらに、立脚前期か後期か及び遊脚前期か後期かを示すフラグである歩容フラグを 用いて脚部の状態に応じた制御を行うことができるので、倣い動作や復帰動作、軌 道修正量維持など各脚の状態によって複雑な制御チャートを実行させることができる ので、これにより遊脚が大きな床反力を発生し ZMPや姿勢角の偏差や加速度が発 生する以前に路面形状への倣い動作を完了することができ、路面状態たとえば傾斜 面や凹凸に応じた制御を的確に行うことができ、人間を搭載する場合のように正確な モデルィ匕が困難な場合にも不整地における歩行を実現することができ、さらに、本制 御はジャイロ等を含む特別なセンサや機構を必要とすることなく ZMP計測用のカセ ンサ（6軸カ覚センサ）のみを用いて行うことができる。
また、足部の軌道と腰部の初期軌道を設定する軌道設定手段 212と、設定した足 部の軌道と腰部の初期軌道とに基づいて目標ゼロモーメントポイントの軌道を設定す る目標 ZMP軌道設定手段 213と、脚部の運動および腰部の運動による目標ゼロモ 一メントポイント回りのモーメントを算出する目標 ZMP回りモーメント算出手段 214と、 算出した目標ゼロモーメントポイント回りのモーメントに基づいて腰部のモーメント補 償軌道を算出するモーメント補償軌道算出手段 217と、設定した足部の軌道と腰部 のモーメント補償軌道とにより算出された各質点の加速度に基づいて目標ゼロモーメ ントポイントにおけるエラーモーメントを算出するエラーモーメント算出手段 219と、算 出したエラーモーメントが所定モーメントよりも小さくなつたとき絶対座標系における腰 座標系の原点と絶対座標系における足座標系の原点とから腰座標系から見た足部 の位置を算出すると共に、絶対座標系から見た足座標系の姿勢を表わす回転行列 と絶対座標系カゝら見た腰座標系の姿勢を表わす回転行列とから腰座標系から見た 足座標系の姿勢を表わす回転行列を算出して足座標系の姿勢を表わす回転行列 により足部の姿勢を算出する足部位置姿勢算出手段 221と、歩容フラグを決定し記 憶する歩容フラグ決定手段 222とを有することにより、脚部の運動および腰部の運動 による目標ゼロモーメントポイント回りのモーメントがゼロの近傍か否かをエラーモーメ ントにより判定することができ、歩行パターンの設定において目標ゼロモーメントボイ ント回りのモーメントを限り無くゼロに近づけることができるので、安定性の極めて高い 2足歩行を 2足歩行ロボット装置に行わせることができる。
[0169] さらに、 ZMP回りモーメント算出手段において算出した目標ゼロモーメントポイント 回りのモーメントと算出したエラーモーメントとの総和に基づいてフーリエ係数を算出 するフーリエ変換手段 215と、算出したフーリエ係数に基づいてモーメント補償軌道 の近似解を求めるためのフーリエ係数を決定するフーリエ係数決定手段 216とを備 え、モーメント補償軌道算出手段 217は、決定したフーリエ係数に基づく逆フーリエ 変換により腰部のモーメント補償軌道を算出することにより、腰部のモーメント補償軌 道を正確に算出することができるので、目標ゼロモーメントポイント回りのモーメントを 迅速にゼロに近づけることができる。
[0170] さらに、目標 ZMP回りモーメント算出手段 2141は、脚部の運動により生じる目標 Z MP回りモーメントを算出する下肢運動による目標 ZMP回りモーメント算出手段 214 1と、腰部の運動により生じる目標 ZMP回りモーメントを算出する腰運動による目標 Z MP回り既知モーメント算出手段 2142とを有することにより、脚部の運動および腰部 の運動による目標ゼロモーメントポイント回りのモーメントを正確に算出することができ るので、腰部のモーメント補償軌道を正確に算出することができる。
[0171] さらに、足部位置姿勢算出手段 221は、絶対座標系における腰座標系の原点と絶 対座標系における足座標系の原点とから腰座標系から見た足部の位置を算出する 原点位置算出手段 2211と、絶対座標系から見た足座標系の姿勢を表わす回転行 列と絶対座標系カゝら見た腰座標系の姿勢を表わす回転行列とから腰座標系から見 た足座標系の姿勢を表わす回転行列を算出する回転行列算出手段 2212と、回転 行列算出手段で算出した回転行列により足部の姿勢を算出する姿勢算出手段 221 3とを有することにより、腰座標系力 みた足部の位置と姿勢のデータを正確に生成 することができるので、パラレルリンク機構においてはリアルタイム演算が容易な逆運 動学演算を用いて脚部を伸縮するシリンダの長さを正確に制御することができ、逆運 動学演算において種々の補正データを用いて種々の制御を行う場合に正確な制御 を行うことができる。
[0172] さらに、足部を備えた脚部と腰部とから成り、脚部は脚部を伸縮するシリンダを有し 、腰部は全体を制御すると共に上記記載の歩行パターン作成装置と無線通信を行う 制御コンピュータ 7を有する 2足歩行ロボット装置であって、制御コンピュータは、歩行 ノターン作成装置力も受信した歩行パターンに基づいて 2足歩行の制御を行うことに より、腰部のモーメント補償軌道により足部の目標ゼロモーメントポイントにおけるモ 一メントをゼロに近づけて歩行を安定ィ匕することができると共に歩行パターンを無線 を介して遠隔地でも受信することができ、また、腰部に種々の上体 (体幹)を取り付け 可能とすれば、種々の用途 (たとえば救助用、介護用、危険作業用等）に適合する 2 足歩行ロボットを迅速に構築することができ、また、腰部にいす等の搭乗部を取り付 けて人間の搭乗を可能とすれば、種々の用途 (たとえば福祉用、移動用）に適合する 2足歩行ロボットを容易且つ迅速に構築することができ、さらに、腰座標系からみた足 部の位置と姿勢のデータに対して逆運動学演算を行って脚部を伸縮するシリンダの 長さを制御することができるので、パラレルリンク機構にぉ 、てはリアルタイム演算が 困難な順運動学演算を行う必要がなく、パラレルリンク機構においてはリアルタイム 演算が容易な逆運動学演算によりロボットの歩行を制御することができ、種々の補正 データ（たとえばコンプライアンス移動量や腰部の位置 ·姿勢の操作量)を足部の位 置や姿勢の次元で足し合わせて力 逆運動学演算を行うという容易な実装ができ、 2 足歩行ロボットの歩行を更に安定ィ匕することができる。
[0173] さらに、制御コンピュータ 7は、歩行パターン作成用コンピュータ力も歩行パターン を無線で受信してメモリに格納する歩行パターン設定手段 712と、格納した歩行バタ 一ンカも歩行に使用するパターンを選択して歩行開始を指令する歩行開始命令手 段 714と、歩行開始命令手段の指令に基づいてモータを制御してシリンダの長さを 制御するモータ制御手段 715とを有することにより、歩行状態において 2足歩行ロボ ット装置は歩行パターン作成用コンピュータ力も物理的な拘束を受けることなく高安 定な歩行を行うことができ、また、周囲または自己の状態 (たとえば搭載重量の軽重 状態）に応じた適切な歩行パターンで歩行することができる。
[0174] さらに、モータ制御手段 715は、基本制御を行う基本制御手段 810と、遊脚を路面 に衝撃を緩和して着地させると共に路面の凹凸に足部を倣わせる着地制御手段 82 0とを有することにより、基本制御に基づく基本動作を行うことができると共に、状況に 応じて着地制御を行うことができ、基本制御においては、パラレルメカニズムの脚機 構にとっては計算量が膨大なために実施が困難な順運動学を行う必要がなくなり、 計算が容易な逆運動学によりリアルタイムにリンク長さ（つまりシリンダ長さ）を制御す ることができるので、足部の位置 '姿勢に関して種々の制御を行うことができるようにな り、また着地制御においては、遊脚が大きな床反力を発生し ZMPや姿勢角の偏差 や加速度が発生する以前に、路面形状への倣い動作を完了し、不整路面に対して 転倒することなく安定した歩行を行うことができ、人間搭乗型 2足歩行ロボットのように 正確なモデルィ匕が困難なロボットの不整地における歩行を実現することができる。
[0175] さらに、基本制御手段 810は、足部位置姿勢算出手段で算出した足部の位置-姿 勢の目標値を歩行開始命令手段の指令に基づいて出力する歩行パターン出力手 段 811と、歩行パターン出力手段から出力される足部の位置'姿勢の目標値を逆運 動学演算してシリンダの長さを算出する逆運動学演算手段 813とを有することにより 、ノ レルメカニズムの脚機構にとっては計算量が膨大なために実施が困難な順運 動学を行う必要がなくなり、計算が容易な逆運動学によりリアルタイムにリンク長さを 制御することができるので、足部の位置 '姿勢に関して種々の制御を行うことができる
[0176] さらに、着地制御手段 820は、歩容フラグに基づいて制御、維持、復帰、コンプライ アンス等の制御モードを選択するモード選択手段 821と、歩行パターンの成分にお いて着地制御の対象となる成分である制御対象成分と制御対象成分以外の非制御 対象成分とを分割する成分分割手段 822と、遊脚の着地衝撃を緩和する非線形コン プライアンス移動量を足部における床反力に基づ!/、て算出する非線形コンプライア ンス移動量算出手段 824と、非線形コンプライアンス移動量に基づいて上下方向の 軌道修正量を算出する z方向軌道修正量算出手段 823と、足部におけるロールおよ びピッチ方向のトルクに基づいてロールおよびピッチの軌道修正量を算出するロー ル 'ピッチ軌道修正量算出手段 825と、歩行パターン出力手段から出力される歩行 パターンと着地路面検知用移動許容量と非線形コンプライアンス移動量と上下方向 の軌道修正量とロール ·ピッチ軌道修正量とに基づ!/、て歩行パターンを修正する歩 行パターン修正手段 826とを有することにより、着地衝撃の緩和と振動の抑制に対し ては非線形コンプライアンス移動量を用いて対応することができ、路面の凹凸や傾斜 路面に対しては上下方向の軌道修正量およびロールおよびピッチの軌道修正量を 用いて対応することができるので、ジャイロを含め特別なセンサや機構を必要とせず 、 ZMP計測用の力センサのみを用いて、遊脚が大きな床反力を発生し ZMPや姿勢 角の偏差や加速度が発生する以前に、路面形状への倣い動作を完了できるため、 不整路面に対して転倒することなく安定した歩行を行うことができ、人間搭乗型 2足 歩行ロボットのように正確なモデルィ匕が困難なロボットの不整地における歩行を確実 に実現することができる。
[0177] さらに、 z方向軌道修正量算出手段 823は、理想的路面において遊脚の着地衝撃 を緩和する理論コンプライアンス移動量を制御対象成分に基づいて算出する理論コ ンプライアンス移動量算出手段 8231と、理論コンプライアンス移動量と非線形コンプ ライアンス移動量との差分である着地路面高さ誤差を算出するコンプライアンス移動 量偏差算出手段 8232と、モード選択手段で選択した制御モードと着地路面高さ誤 差とに基づいて上下方向の軌道修正量を算出する軌道修正量算出手段 8233とを 有することにより、路面の凹凸に対して、遊脚が大きな床反力を発生し ZMPや姿勢 角の偏差や加速度が発生する以前に、路面形状への倣い動作を完了し、上下方向 に正確に対応することができるので、ロール'ピッチについて対応すれば、不整路面 に対して転倒することなく安定した歩行を行うことができ、人間搭乗型 2足歩行ロボッ トのように正確なモデルィ匕が困難なロボットの不整地における歩行を更に一層確実に 実現することができる。
[0178] さらに、ロール'ピッチ軌道修正量算出手段 825は、足部におけるロールおよびピッ チ方向のトルクに基づ!/ヽてコンプライアンス移動量を算出するコンプライアンス移動 量算出手段 8251と、コンプライアンス移動量に基づいてロールおよびピッチ方向の 軌道修正量を算出する軌道修正量算出手段 8252とを有することにより、路面の凹凸 に対してロール'ピッチ軸まわりに正確に対応することができるので、上下方向につ!ヽ て対応すれば、不整路面に対して、遊脚が大きな床反力を発生し ZMPや姿勢角の 偏差や加速度が発生する以前に、路面形状への倣い動作を完了し、転倒することな く安定した歩行を行うことができ、人間搭乗型 2足歩行ロボットのように正確なモデル 化が困難なロボットの不整地における歩行を更に一層確実に実現することができる。
[0179] また、図 19〜図 31に記載の歩行パターン作成方法の各ステップを実行させるため のプログラムであること〖こより、上記歩行パターン作成方法を任意の場所で任意の時 間に汎用コンピュータに実行させることができる。
[0180] さらに、上記プログラムを記録したコンピュータ読み取り可能な記録媒体であること により、汎用コンピュータで記録媒体を読み取りさえすれば、上記歩行パターン作成 方法を任意の場所で任意の時間に汎用コンピュータに実行させることができる。
[0181] また、図 34〜図 36、図 38〜図 44に記載の 2足歩行ロボット装置の制御方法の各ス テツプを実行させるためのプログラムであることにより、上記 2足歩行ロボット装置の制 御方法を任意の場所で任意の時間に汎用コンピュータに実行させることができる。
[0182] さらに、上記プログラムを記録したコンピュータ読み取り可能な記録媒体であること により、汎用コンピュータで記録媒体を読み取りさえすれば、上記 2足歩行ロボット装 置の制御方法を任意の場所で任意の時間に汎用コンピュータに実行させることがで きる。
[0183] (実施の形態 2)
図 45 (a)は本実施の形態 2における 2足歩行ロボット装置のモータ制御手段 715' を示す機能ブロック図であり、図 45 (b)は姿勢角補償量算出手段 833を示す機能ブ ロック図である。
図 45において、 810は基本制御手段、 820は着地制御手段であり、これらは実施 の形態 1において説明したものと同様のものであり、同一符号を付し、説明は省略す る。 830は推定姿勢補償制御を行う推定姿勢補償制御手段である。推定姿勢補償 制御手段 830は、 6軸カ覚センサ 142 (図 40参照）の検出値力も ZMP実測値を算出 する ZMP実測値算出手段 831と、算出された ZMP実測値と歩行パターンとして設 定されている ZMP目標値との偏差を算出する ZMP偏差値算出手段 832と、 ZMP偏 差値を積分した値に基づいて推定した腰部の姿勢角補償量を算出する姿勢角補償 量算出手段 833と、着地制御手段 820により修正した歩行パターンを算出した姿勢 角補償量に基づいて補正する足部位置姿勢補正手段とを有する。
また、姿勢角補償量算出手段 833は、所定の周期で ZMP偏差値の積分値をゼロ に設定 (リセット)するゼロ設定手段 8331と、 ZMP偏差値の積分値がゼロに設定され た時点力もの積分値を算出する積分値算出手段 8332と、歩行パターンの補正操作 を行う所定周期の断続的な操作期間を設定する操作期間設定手段 8333と、 ZMP 偏差値の積分値に基づいて腰部の姿勢角補償量を算出する補償量算出手段 8334 と、算出された姿勢角補償量に基づいて実際の腰部の姿勢角操作量を算出する姿 勢角操作量算出手段 8335とを有する。
このように構成されたモータ制御手段 715の動作は、着地制御と共に推定姿勢補 償制御を行う点以外は実施の形態 1で説明したものと同様である。以下、推定姿勢 補償制御の動作にっ 、て図 46乃至図 49を用いて説明する。
図 46は推定姿勢補償制御を示す機能的ブロック図であり、図 47はゼロ設定手段 8 331による設定期間と操作期間設定手段 8333による操作期間を説明する説明図で あり、図 48は推定姿勢補償制御手段 830の動作を示すフローチャートであり、図 49 は姿勢角補償量算出手段 833の動作を示すフローチャートである。
図 46において、 826は歩行パターン修正手段、 813は逆運動学演算手段、 811は 歩行パターン出力手段、 142は 6軸カ覚センサ、 142aはレシーバボードであり、これ らは実施の形態 1において説明したものと同様のものであり、同一符号を付し、説明 は省略する。
図 47において、 841a, 841bは所定の長さで予め設定された設定期間であり、一 の設定期間 841aから他の設定期間 841bに移行する際、すなわち設定期間 841a, 84 lbの開始時点において、ゼロ設定手段 8331により後述の ZMP偏差値の積分値 がゼロに設定される。すなわち、積分を開始して力も所定長さの設定期間 841a, 84 lbが経過した後に、積分値はゼロに設定 (リセット）される。 842a〜842gは操作期間 設定手段 8333により各設定期間 841a, 841b内における所定周期の断続的な期間 として予め設定された操作期間である。
[0185] 図 48に示すように、本実施の形態 2においては着地制御のステップ SS42aの後に 推定姿勢補償制御のステップ SS42bが追加されている点が実施の形態 1と異なって いる。
推定姿勢補償制御の動作は、図 49において、まず、 ZMP実測値算出手段 831は 、レシーバボード 142aから出力される 6軸カ覚センサ 142の原点の座標及び測定さ れたカ並びにモーメント（ (数 77)参照）のデータから (数 76)により ZMP実測値を算 出する（SS81)。次に、 ZMP偏差値算出手段 832は、 ZMP実測値算出手段 831に より算出された ZMP実測値と歩行パターン出力手段 811 (図 37参照)から出力され る歩行パターンとして設定されて 、る ZMP目標値 (腰部力もみた ZMP軌道）との偏 差値を算出する（SS82)。
次に、姿勢角補償量算出手段 833の積分値算出手段 8332は、 ZMP偏差値の積 分値を算出し、補償量算出手段 8334は (数 69)により腰部の姿勢角補償量を算出 する（SS83)。ここで、積分値算出手段 8332は、図 49に示すように、操作期間 842a 〜842gの開始時点において、設定期間 841a, 841bの開始時点力も操作期間 842 a〜842gの開始時点までの ZMP偏差値の積分値を算出する。
[0186] 次に、操作量算出手段 8335は、補償量算出手段 8334により算出された姿勢角補 償量から (数 78)を用いて腰部の姿勢角操作量を算出する（SS84)。
次に、足部位置姿勢補正手段 834は、操作期間 842a〜842gにおいて、腰部が 算出された姿勢角操作量だけ操作されるように、腰部に対する足部の位置又は姿勢 の操作量 (足部位置姿勢操作量)を算出し (SS85)、着地制御手段 820により修正さ れた歩行パターンに対して同次変換行列を用いて足部位置姿勢操作量を補正量と してその分だけ補正し、新たな足部の位置 ·姿勢の目標値を算出する（SS86)。ここ で、足部の補正は、単脚支持期であれば立脚側の足部の位置'姿勢データに対して 行われ、両脚支持期であれば両方の足部の位置 ·姿勢データに対して行われること となる。単脚支持期か両脚支持期かの判定は、歩容フラグに基づいて行う。 足部位置姿勢操作量の分だけ補正された歩行パターンは逆運動学演算手段 813 に出力され、実施の形態 1と同様に逆運動学演算、シリンダ長さデータの算出、モー タの角度変位データへの変換、モータ駆動等の処理が行われる。
[0187] 以上のように本実施の形態 2によれば、推定姿勢補償制御手段 830は、 ZMP実測 値を足部における力成分とモーメント成分とから算出する ZMP実測値算出手段 831 と、算出された ZMP実測値と ZMP目標値との偏差である ZMP偏差値を算出する Z MP偏差値算出手段 832と、 ZMP偏差値を積分した値に基づいて腰部の姿勢角補 償量を算出する姿勢角補償量算出手段 833と、歩行パターンの足部の位置又は姿 勢の目標値を姿勢角補償量に基づく足部位置姿勢操作量だけ補正する足部位置 姿勢補正手段 834とを有し、姿勢角補償量算出手段 833は、所定の周期で ZMP偏 差値の積分値をゼロに設定するゼロ設定手段 8331と、 ZMP偏差値の積分値がゼロ に設定された時点力もの積分値を算出する積分値算出手段 8332と、足部の位置又 は姿勢の補正操作を行う操作期間を設定する操作期間設定手段 8333と、操作期間 の開始時点における ZMP偏差値の積分値に基づいて姿勢角補償量を算出する補 償量算出手段 8334とを有することにより、機械剛性やモータの応答偏差等に起因し 着地制御における軌道修正より生じる腰部の姿勢角の累積誤差を推定し、推定した 値に基づきそれを補償して腰部と遊脚側の足部を略水平に維持することができるの で、着地制御時の足部による不整地面のセンシング精度の低下等を補い、正確な着 地制御を行うことができ、安定した歩行を行うことができる。また、 ZMP偏差値の積分 値に基づいて姿勢角補償量を算出するため、急激な変化を伴う補償動作を防止でき 安定性に優れる。特に、着地制御により急激な姿勢角の変化を防止できるので、これ と推定姿勢補償制御を併せることでより信頼性の高い歩行制御を行うことができる。 また、腰部の姿勢角を測定するための姿勢角センサを搭載する必要がなぐ部品点 数や製造コストの増加を防止できる。また、所定の周期で ZMP偏差値の積分値をゼ 口に設定することにより、実際の姿勢角偏差との積分誤差の増加を防止できる。また 、所定の操作期間を設定するため姿勢角誤差が過度に累積するのを防止できる。 産業上の利用可能性
[0188] 本発明はパラレルリンク機構を有する 2足歩行ロボット装置に 2足歩行を行わせる歩 行パターンを作成する装置、その歩行パターンに基づいて 2足歩行を行う 2足歩行口 ボット装置、 2足歩行を行わせる歩行パターンを作成する方法、および 2足歩行を行う 2足歩行ロボット装置の制御方法、ならびに、その歩行パターン作成方法とその 2足 歩行ロボット装置の制御方法を実行させるためのプログラムおよび記録媒体に関し、 パラレルリンク機構の 2足歩行ロボット装置に安定した 2足歩行を行わせることができ る。

Claims

請求の範囲
[1] 足部を備えた脚部と腰部とから成る 2足歩行ロボット装置の歩行パターンを作成す る歩行パターン作成装置であって、前記足部において目標ゼロモーメントポイントを 設定し、前記設定した目標ゼロモーメントポイントに応じて前記腰部のモーメント補償 軌道を算出し、絶対座標系における腰座標系の原点と絶対座標系における足座標 系の原点とから腰座標系力も見た足部の位置を算出し、絶対座標系から見た足座標 系の姿勢を表わす回転行列と絶対座標系から見た腰座標系の姿勢を表わす回転行 列とから腰座標系から見た足座標系の姿勢を表わす回転行列を算出して前記足座 標系の姿勢を表わす回転行列により足部の姿勢を算出し、歩行パターンにおける立 脚前期か後期力及び遊脚前期か後期かを示すフラグである歩容フラグを決定し記憶 することを特徴とする歩行パターン作成装置。
[2] 前記足部の軌道と前記腰部の初期軌道を設定する軌道設定手段と、前記設定し た足部の軌道と前記腰部の初期軌道とに基づ 、て前記目標ゼロモーメントポイントの 軌道を設定する目標 ZMP軌道設定手段と、前記脚部の運動および前記腰部の運 動による前記目標ゼロモーメントポイント回りのモーメントを算出する目標 ZMP回りモ 一メント算出手段と、前記算出した目標ゼロモーメントポイント回りのモーメントに基づ いて前記腰部のモーメント補償軌道を算出するモーメント補償軌道算出手段と、前記 設定した足部の軌道と前記腰部のモーメント補償軌道とにより算出された各質点の 加速度に基づいて前記目標ゼロモーメントポイントにおけるエラーモーメントを算出 するエラーモーメント算出手段と、前記算出したエラーモーメントが所定モーメントより も小さくなつたとき絶対座標系における腰座標系の原点と絶対座標系における足座 標系の原点とから腰座標系力も見た足部の位置を算出すると共に、絶対座標系から 見た足座標系の姿勢を表わす回転行列と絶対座標系力 見た腰座標系の姿勢を表 わす回転行列とから腰座標系カゝら見た足座標系の姿勢を表わす回転行列を算出し て前記足座標系の姿勢を表わす回転行列により足部の姿勢を算出する足部位置姿 勢算出手段と、前記歩容フラグを決定し記憶する歩容フラグ決定手段とを有すること を特徴とする請求項 1に記載の歩行パターン作成装置。
[3] 前記 ZMP回りモーメント算出手段において算出した目標ゼロモーメントポイント回り のモーメントと前記算出したエラーモーメントとの総和に基づいてフーリエ係数を算出 するフーリエ変換手段と、前記算出したフーリエ係数に基づいて前記モーメント補償 軌道の近似解を求めるためのフーリエ係数を決定するフーリエ係数決定手段とを備 え、前記モーメント補償軌道算出手段は、前記決定したフーリエ係数に基づく逆フー リエ変換により前記腰部のモーメント補償軌道を算出することを特徴とする請求項 2 に記載の歩行パターン作成装置。
[4] 前記目標 ZMP回りモーメント算出手段は、前記脚部の運動により生じる目標 ZMP 回りモーメントを算出する下肢運動による目標 ZMP回りモーメント算出手段と、前記 腰部の運動により生じる目標 ZMP回りモーメントを算出する腰運動による目標 ZMP 回り既知モーメント算出手段とを有することを特徴とする請求項 2または 3に記載の歩 行パターン作成装置。
[5] 前記足部位置姿勢算出手段は、絶対座標系における腰座標系の原点と絶対座標 系における足座標系の原点とから腰座標系から見た足部の位置を算出する原点位 置算出手段と、絶対座標系力 見た足座標系の姿勢を表わす回転行列と絶対座標 系から見た腰座標系の姿勢を表わす回転行列とから腰座標系カゝら見た足座標系の 姿勢を表わす回転行列を算出する回転行列算出手段と、前記回転行列算出手段で 算出した回転行列により足部の姿勢を算出する姿勢算出手段とを有することを特徴 とする請求項 2乃至 4の 、ずれか 1に記載の歩行パターン作成装置。
[6] 足部を備えた脚部と腰部とから成り、前記脚部は前記脚部を伸縮するシリンダを有 し、前記腰部は全体を制御すると共に請求項 1乃至 5のいずれか 1に記載の歩行パ ターン作成装置と無線通信を行う制御コンピュータを有する 2足歩行ロボット装置で あって、
前記制御コンピュータは、前記歩行パターン作成装置から受信した歩行パターン に基づいて 2足歩行の制御を行うことを特徴とする 2足歩行ロボット装置。
[7] 前記制御コンピュータは、前記歩行パターン作成用コンピュータから歩行パターン を無線で受信してメモリに格納する歩行パターン設定手段と、前記格納した歩行バタ 一ンカも歩行に使用するパターンを選択して歩行開始を指令する歩行開始命令手 段と、前記歩行開始命令手段の指令に基づ!、てモータを制御して前記シリンダの長 さを制御するモータ制御手段とを有することを特徴とする請求項 6に記載の 2足歩行 ロボット装置。
[8] 前記モータ制御手段は、基本制御を行う基本制御手段と、遊脚を路面に衝撃を緩 和して着地させると共に路面の凹凸に足部を倣わせる着地制御手段とを有すること を特徴とする請求項 7に記載の 2足歩行ロボット装置。
[9] 前記基本制御手段は、前記足部位置姿勢算出手段で算出した足部の位置，姿勢 の目標値を前記歩行開始命令手段の指令に基づいて出力する歩行パターン出力 手段と、前記歩行パターン出力手段から出力される前記足部の位置'姿勢の目標値 を逆運動学演算して前記シリンダの長さを算出する逆運動学演算手段とを有するこ とを特徴とする請求項 8に記載の 2足歩行ロボット装置。
[10] 前記着地制御手段は、前記歩容フラグに基づ!、て制御、維持、復帰、コンプライア ンス等の制御モードを選択するモード選択手段と、歩行パターンの成分にぉ 、て着 地制御の対象となる成分である制御対象成分と前記制御対象成分以外の非制御対 象成分とを分割する成分分割手段と、遊脚の着地衝撃を緩和する非線形コンプライ アンス移動量を足部における床反力に基づいて算出する非線形コンプライアンス移 動量算出手段と、前記非線形コンプライアンス移動量に基づいて上下方向の軌道修 正量を算出する z方向着地軌道修正量算出手段と、足部におけるロールおよびピッ チ方向のトルクに基づいてロールおよびピッチの軌道修正量を算出するロール'ピッ チ方向軌道修正量算出手段と、前記歩行パターン出力手段から出力される歩行バタ ーンと着地路面検知用移動許容量と前記非線形コンプライアンス移動量と前記上下 方向の軌道修正量と前記ロール'ピッチ方向軌道修正量とに基づいて歩行パターン を修正する歩行パターン修正手段とを有することを特徴とする請求項 8または 9に記 載の 2足歩行ロボット装置。
[11] 前記 z方向着地軌道修正量算出手段は、理想的水平平坦路面に衝撃や振動なく 理想的に着地したときに発生するはずの理論コンプライアンス移動量を前記制御対 象成分に基づいて算出する理論コンプライアンス移動量算出手段と、前記理論コン プライアンス移動量と前記非線形コンプライアンス移動量との差分である着地路面高 さ誤差を算出するコンプライアンス移動量偏差算出手段と、前記モード選択手段で 選択した制御モードと前記着地路面高さ誤差とに基づいて上下方向の軌道修正量 を算出する軌道修正量算出手段とを有することを特徴とする請求項 10に記載の 2足 歩行ロボット装置。
[12] 前記ロール'ピッチ方向軌道修正量算出手段は、足部におけるロールおよびピッチ 方向のトルクに基づ ヽてコンプライアンス移動量を算出するコンプライアンス移動量 算出手段と、前記コンプライアンス移動量に基づいてロールおよびピッチ方向の軌道 修正量を算出する軌道修正量算出手段とを有することを特徴とする請求項 10に記 載の 2足歩行ロボット装置。
[13] 前記モータ制御手段は、前記着地制御手段により修正した歩行パターンを推定し た腰部の姿勢角補償量に基づいて補正する推定姿勢補償制御を行う推定姿勢補償 制御手段を備え、
前記推定姿勢補償制御手段は、 ZMP実測値を足部における力成分とモーメント成 分とから算出する ZMP実測値算出手段と、算出された前記 ZMP実測値と ZMP目標 値との偏差である ZMP偏差値を算出する ZMP偏差値算出手段と、前記 ZMP偏差 値を積分した値に基づいて腰部の姿勢角補償量を算出する姿勢角補償量算出手段 と、前記修正した歩行パターンにおける足部の位置又は姿勢の目標値を前記姿勢 角補償量に基づいて補正する足部位置姿勢補正手段とを有することを特徴とする請 求項 8乃至 12の!、ずれか 1に記載の 2足歩行ロボット装置。
[14] 前記姿勢角補償量算出手段は、所定の周期で前記 ZMP偏差値の積分値をゼロ に設定するゼロ設定手段と、前記 ZMP偏差値の積分値がゼロに設定された時点か らの積分値を算出する積分値算出手段と、前記足部の位置又は姿勢の補正操作を 行う操作期間を設定する操作期間設定手段と、前記操作期間の開始時点における 前記 ZMP偏差値の積分値に基づいて姿勢角補償量を算出する補償量算出手段と を有することを特徴とする請求項 13に記載の 2足歩行ロボット装置。
[15] 足部を備えた脚部と腰部とから成る 2足歩行ロボット装置の歩行パターンを作成す る歩行パターン作成方法であって、前記足部において目標ゼロモーメントポイントを 設定し、前記設定した目標ゼロモーメントポイントに応じて前記腰部のモーメント補償 軌道を算出し、絶対座標系における腰座標系の原点と絶対座標系における足座標 系の原点とから腰座標系力も見た足部の位置を算出し、絶対座標系から見た足座標 系の姿勢を表わす回転行列と絶対座標系から見た腰座標系の姿勢を表わす回転行 列とから腰座標系から見た足座標系の姿勢を表わす回転行列を算出して前記足座 標系の姿勢を表わす回転行列により足部の姿勢を算出し、歩行パターンにおける立 脚前期か後期力及び遊脚前期か後期かを示すフラグである歩容フラグを決定し記憶 することを特徴とする歩行パターン作成方法。
[16] 前記足部の軌道と前記腰部の初期軌道を設定する軌道設定ステップと、前記設定 した足部の軌道と前記腰部の初期軌道とに基づ 、て前記目標ゼロモーメントポイント の軌道を設定する目標 ZMP軌道設定ステップと、前記脚部の運動および前記腰部 の運動による前記目標ゼロモーメントポイント回りのモーメントを算出する目標 ZMP 回りモーメント算出ステップと、前記算出した目標ゼロモーメントポイント回りのモーメ ントに基づいて前記腰部のモーメント補償軌道を算出するモーメント補償軌道算出ス テツプと、前記設定した足部の軌道と前記腰部のモーメント補償軌道とにより算出さ れた各質点の加速度に基づいて前記目標ゼロモーメントポイントにおけるエラーモー メントを算出するエラーモーメント算出ステップと、前記算出したエラーモーメントが所 定モーメントよりも小さくなつたとき絶対座標系における腰座標系の原点と絶対座標 系における足座標系の原点とから腰座標系から見た足部の位置を算出すると共に、 絶対座標系から見た足座標系の姿勢を表わす回転行列と絶対座標系から見た腰座 標系の姿勢を表わす回転行列とから腰座標系から見た足座標系の姿勢を表わす回 転行列を算出して前記足座標系の姿勢を表わす回転行列により足部の姿勢を算出 する足部位置姿勢算出ステップし、前記歩容フラグを決定し記憶する歩容フラグ決 定ステップとを有することを特徴とする請求項 15に記載の歩行パターン作成方法。
[17] 前記 ZMP回りモーメント算出ステップにおいて算出した目標ゼロモーメントポイント 回りのモーメントと前記算出したエラーモーメントとの総和に基づいてフーリエ係数を 算出するフーリエ変換ステップと、前記算出したフーリエ係数に基づいて前記モーメ ント補償軌道の近似解を求めるためのフーリエ係数を決定するフーリエ係数決定ス テツプとを備え、前記モーメント補償軌道算出ステップにおいては、前記決定したフ 一リエ係数に基づく逆フーリエ変換により前記腰部のモーメント補償軌道を算出する ことを特徴とする請求項 16に記載の歩行パターン作成方法。
[18] 前記目標 ZMP回りモーメント算出ステップは、前記脚部の運動により生じる目標 Z MP回りモーメントを算出する下肢運動による目標 ZMP回りモーメント算出ステップと 、前記腰部の運動により生じる目標 ZMP回りモーメントを算出する腰運動による目標 ZMP回り既知モーメント算出ステップとを有することを特徴とする請求項 16または 17 に記載の歩行パターン作成方法。
[19] 前記足部位置姿勢算出ステップは、絶対座標系における腰座標系の原点と絶対 座標系における足座標系の原点とから腰座標系から見た足部の位置を算出する原 点位置算出ステップと、絶対座標系力 見た足座標系の姿勢を表わす回転行列と絶 対座標系から見た腰座標系の姿勢を表わす回転行列とから腰座標系カゝら見た足座 標系の姿勢を表わす回転行列を算出する回転行列算出ステップと、前記回転行列 算出ステップで算出した回転行列により足部の姿勢を算出する姿勢算出ステップとを 有することを特徴とする請求項 16乃至 18のいずれか 1に記載の歩行パターン作成 方法。
[20] 足部を備えた脚部と腰部とから成り、前記脚部は前記脚部を伸縮するシリンダを有 し、前記腰部は全体を制御すると共に請求項 1乃至 5のいずれか 1に記載の歩行パ ターン作成装置と無線通信を行う制御用コンピュータを有する 2足歩行ロボット装置 の制御方法であって、前記制御コンピュータは、前記歩行パターン作成装置から受 信した歩行パターンに基づいて 2足歩行の制御を行うことを特徴とする 2足歩行ロボ ット装置の制御方法。
[21] 前記制御コンピュータは、前記歩行パターン作成用コンピュータから歩行パターン を無線で受信してメモリに格納する歩行パターン設定ステップと、前記格納した歩行 パターン力も歩行に使用するパターンを選択して歩行開始を指令する歩行開始命令 ステップと、前記歩行開始命令ステップにおける指令に基づ 、てモータを制御して前 記シリンダの長さを制御するモータ制御ステップとを有することを特徴とする請求項 2 0に記載の 2足歩行ロボット装置の制御方法。
[22] 前記モータ制御ステップは、基本制御を行う基本制御ステップと、遊脚を路面に衝 撃を緩和して着地させると共に路面の凹凸に足部を倣わせる着地制御ステップとを 有することを特徴とする請求項 21に記載の 2足歩行ロボット装置の制御方法。
[23] 前記基本制御ステップは、前記足部位置姿勢算出手段で算出した足部の位置-姿 勢の目標値を前記歩行開始命令ステップにおける指令に基づ 、て出力する歩行パ ターン出力ステップと、前記歩行パターン出力ステップから出力される前記足部の位 置'姿勢の目標値を逆運動学演算して前記シリンダの長さを算出する逆運動学演算 ステップとを有することを特徴とする請求項 22に記載の 2足歩行ロボット装置の制御 方法。
[24] 前記着地制御ステップは、前記歩容フラグに基づ!、て制御、維持、復帰、コンブラ ィアンス等の制御モードを選択するモード選択ステップと、歩行パターンの成分にお いて着地制御の対象となる成分である制御対象成分と前記制御対象成分以外の非 制御対象成分とを分割する成分分割ステップと、遊脚の着地衝撃を緩和する非線形 コンプライアンス移動量を足部における床反力に基づいて算出する非線形コンブラ ィアンス移動量算出ステップと、前記非線形コンプライアンス移動量に基づ 、て上下 方向の軌道修正量を算出する z方向着地軌道修正量算出ステップと、足部における ロールおよびピッチ方向のトルクに基づいてロールおよびピッチの軌道修正量を算 出するロール ·ピッチ方向軌道修正量算出ステップと、前記歩行パターン出カステツ プから出力される歩行パターンと着地路面検知用移動許容量と前記非線形コンブラ ィアンス移動量と前記上下方向の軌道修正量と前記ロール ·ピッチ方向軌道修正量 とに基づ 、て前記歩行パターンを修正する歩行パターン修正ステップとを有すること を特徴とする請求項 22または 23に記載の 2足歩行ロボット装置の制御方法。
[25] 前記 z方向着地軌道修正量算出ステップは、理想的水平平坦路面に衝撃や振動 なく理想的に着地したときに発生するはずの理論コンプライアンス移動量を前記制御 対象成分に基づいて算出する理論コンプライアンス移動量算出ステップと、前記理 論コンプライアンス移動量と前記非線形コンプライアンス移動量との差分である着地 路面高さ誤差を算出するコンプライアンス移動量偏差算出ステップと、前記モード選 択ステップで選択した制御モードと前記着地路面高さ誤差とに基づいて上下方向の 軌道修正量を算出する軌道修正量算出ステップとを有することを特徴とする請求項 2 4に記載の 2足歩行ロボット装置の制御方法。
[26] 前記ロール'ピッチ方向軌道修正量算出ステップは、足部におけるロールおよびピ ツチ方向のトルクに基づ ヽてコンプライアンス移動量を算出するコンプライアンス移動 量算出ステップと、前記コンプライアンス移動量に基づ!/、てロールおよびピッチ方向 の軌道修正量を算出する軌道修正量算出ステップとを有することを特徴とする請求 項 24に記載の 2足歩行ロボット装置の制御方法。
[27] 前記モータ制御ステップは、前記着地制御ステップにおいて修正した歩行パターン を推定した腰部の姿勢角補償量に基づいて補正する推定姿勢補償制御を行う推定 姿勢補償制御ステップを備え、
前記推定姿勢補償制御ステップは、 ZMP実測値を足部における力成分とモーメン ト成分とから算出する ZMP実測値算出ステップと、算出された前記 ZMP実測値と Z MP目標値との偏差である ZMP偏差値を算出する ZMP偏差値算出ステップと、前 記 ZMP偏差値を積分した値に基づいて腰部の姿勢角補償量を算出する姿勢角補 償量算出ステップと、前記修正した歩行パターンにおける足部の位置又は姿勢の目 標値を前記姿勢角補償量に基づいて補正する足部姿勢操作量加算ステップとを有 することを特徴とする請求項 22乃至 26の ヽずれか 1に記載の 2足歩行ロボット装置 の制御方法。
[28] 前記姿勢角補償量算出ステップは、所定の周期で前記 ZMP偏差値の積分値をゼ 口に設定するゼロ設定ステップと、前記 ZMP偏差値の積分値がゼロに設定された時 点力 の積分値を算出する積分値算出ステップと、前記足部の位置又は姿勢の補正 操作を行う操作期間を設定する操作期間設定ステップと、前記操作期間の開始時点 における前記 ZMP偏差値の積分値に基づいて姿勢角補償量を算出する補償量算 出ステップとを有することを特徴とする請求項 27に記載の 2足歩行ロボット装置の制 御方法。
[29] コンピュータに請求項 15乃至 19のいずれ力 1に記載の歩行パターン作成方法の 各ステップを実行させるためのプログラム。
[30] 請求項 29に記載のプログラムを記録したコンピュータ読み取り可能な記録媒体。
[31] コンピュータに請求項 20乃至 28のいずれ力 1に記載の 2足歩行ロボット装置の制 御方法の各ステップを実行させるためのプログラム。 [32] 請求項 31に記載のプログラムを記録したコンピュータ読み取り可能な記録媒体。
PCT/JP2006/308590 2005-09-14 2006-04-24 歩行パターン作成装置、２足歩行ロボット装置、歩行パターン作成方法、２足歩行ロボット装置の制御方法、プログラムおよび記録媒体 WO2007032120A1 (ja)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2007535380A JPWO2007032120A1 (ja) 2005-09-14 2006-04-24 歩行パターン作成装置、２足歩行ロボット装置、歩行パターン作成方法、２足歩行ロボット装置の制御方法、プログラムおよび記録媒体

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2005267720 2005-09-14
JP2005-267720 2005-09-14

Publications (1)

Publication Number Publication Date
WO2007032120A1 true WO2007032120A1 (ja) 2007-03-22

Family

ID=37864723

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2006/308590 WO2007032120A1 (ja) 2005-09-14 2006-04-24 歩行パターン作成装置、２足歩行ロボット装置、歩行パターン作成方法、２足歩行ロボット装置の制御方法、プログラムおよび記録媒体

Country Status (2)

Country Link
JP (1) JPWO2007032120A1 (ja)
WO (1) WO2007032120A1 (ja)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2011255439A (ja) * 2010-06-07 2011-12-22 Honda Motor Co Ltd 脚式移動ロボットの制御装置
CN113126638A (zh) * 2019-12-30 2021-07-16 深圳市优必选科技股份有限公司 姿态控制方法、装置、双足机器人及可读存储介质
CN114228860A (zh) * 2021-12-22 2022-03-25 燕山大学 新型并联腿双足机器人
WO2022247115A1 (zh) * 2021-05-26 2022-12-01 深圳市优必选科技股份有限公司 质心轨迹生成方法、装置、计算机可读存储介质及机器人

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2003145477A (ja) * 2001-11-09 2003-05-20 Honda Motor Co Ltd 脚式移動ロボットの脚体関節アシスト装置
JP2004082223A (ja) * 2002-08-22 2004-03-18 Atsuo Takanishi 歩行パターン作成装置、２足歩行ロボット装置、歩行パターン作成方法、２足歩行ロボット装置の制御方法、プログラムおよび記録媒体
JP2004167666A (ja) * 2002-08-30 2004-06-17 Sony Corp ロボット装置及びその動作制御方法
JP2006082155A (ja) * 2004-09-14 2006-03-30 Atsuo Takanishi 歩行パターン作成装置、２足歩行ロボット装置、歩行パターン作成方法、２足歩行ロボット装置の制御方法、プログラムおよび記録媒体

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP3167406B2 (ja) * 1992-03-12 2001-05-21 本田技研工業株式会社 脚式移動ロボットの歩行制御装置

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2003145477A (ja) * 2001-11-09 2003-05-20 Honda Motor Co Ltd 脚式移動ロボットの脚体関節アシスト装置
JP2004082223A (ja) * 2002-08-22 2004-03-18 Atsuo Takanishi 歩行パターン作成装置、２足歩行ロボット装置、歩行パターン作成方法、２足歩行ロボット装置の制御方法、プログラムおよび記録媒体
JP2004167666A (ja) * 2002-08-30 2004-06-17 Sony Corp ロボット装置及びその動作制御方法
JP2006082155A (ja) * 2004-09-14 2006-03-30 Atsuo Takanishi 歩行パターン作成装置、２足歩行ロボット装置、歩行パターン作成方法、２足歩行ロボット装置の制御方法、プログラムおよび記録媒体

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2011255439A (ja) * 2010-06-07 2011-12-22 Honda Motor Co Ltd 脚式移動ロボットの制御装置
CN113126638A (zh) * 2019-12-30 2021-07-16 深圳市优必选科技股份有限公司 姿态控制方法、装置、双足机器人及可读存储介质
CN113126638B (zh) * 2019-12-30 2023-07-21 深圳市优必选科技股份有限公司 姿态控制方法、装置、双足机器人及可读存储介质
WO2022247115A1 (zh) * 2021-05-26 2022-12-01 深圳市优必选科技股份有限公司 质心轨迹生成方法、装置、计算机可读存储介质及机器人
CN114228860A (zh) * 2021-12-22 2022-03-25 燕山大学 新型并联腿双足机器人

Also Published As

Publication number Publication date
JPWO2007032120A1 (ja) 2009-03-19

Similar Documents

Publication Publication Date Title
US8005573B2 (en) Control device for legged mobile robot
US9434430B2 (en) Biped walking robot control method and biped walking robot control system
EP1361027B1 (en) Gait pattern generating device for legged mobile robot
US9555543B2 (en) Robot with joints of variable rigidity and method for calculating said optimized rigidity
JP4181114B2 (ja) 脚式移動ロボットの自己姿勢推定装置
JP4800036B2 (ja) 移動ロボットの歩容生成装置
US7765030B2 (en) Gait generator for mobile robot
JP4126063B2 (ja) 移動体の制御装置
WO2007069401A1 (ja) 脚式移動ロボットの歩容生成装置
US7715945B2 (en) Gait producing device for moving robot
US8428780B2 (en) External force target generating device of legged mobile robot
WO2003061917A1 (fr) Dispositif de commande pour robot bipede
WO2005000533A1 (ja) 脚式移動ロボットの歩容生成装置
WO2006040868A1 (ja) 脚式移動ロボットの歩容生成装置
WO1998004388A1 (fr) Dispositif de reproduction de la demarche pour robot articule muni de jambes
US8396593B2 (en) Gait generating device of legged mobile robot
JP2008068339A (ja) 歩行パターン作成装置、２足歩行ロボット装置、歩行パターン作成方法、２足歩行ロボット装置の制御方法、プログラムおよび記録媒体
WO2007032120A1 (ja) 歩行パターン作成装置、２足歩行ロボット装置、歩行パターン作成方法、２足歩行ロボット装置の制御方法、プログラムおよび記録媒体
JP2008049458A (ja) 脚式ロボット
JP5198035B2 (ja) 脚式ロボット及びその制御方法
JP2006082155A (ja) 歩行パターン作成装置、２足歩行ロボット装置、歩行パターン作成方法、２足歩行ロボット装置の制御方法、プログラムおよび記録媒体
JP5468974B2 (ja) ２足移動ロボットの制御装置及び歩容生成装置
JP2004082223A (ja) 歩行パターン作成装置、２足歩行ロボット装置、歩行パターン作成方法、２足歩行ロボット装置の制御方法、プログラムおよび記録媒体
JP3674787B2 (ja) ロボット装置の動作制御装置及び動作制御方法、並びにコンピュータ・プログラム
WO2006118095A1 (ja) 歩行パターン作成装置、２足歩行ロボット装置、歩行パターン作成方法、２足歩行ロボット装置の制御方法、プログラムおよび記録媒体

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application
DPE1 Request for preliminary examination filed after expiration of 19th month from priority date (pct application filed from 20040101)
WWE Wipo information: entry into national phase

Ref document number: 2007535380

Country of ref document: JP

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 06732300

Country of ref document: EP

Kind code of ref document: A1