WO2005000536A1

WO2005000536A1 - 脚式移動ロボットの制御装置

Info

Publication number: WO2005000536A1
Application number: PCT/JP2004/009477
Authority: WO
Inventors: Toru Takenaka; Takashi Matsumoto; Takahide Yoshiike
Original assignee: Honda Motor Co., Ltd.
Priority date: 2003-06-27
Filing date: 2004-06-28
Publication date: 2005-01-06
Also published as: US20100042256A1; EP2206583A1; EP2353794B1; JP4181175B2; WO2005000535A1; KR20060024363A; JPWO2005000534A1; EP2210713B1; JP4155993B2; EP2210713A1; EP1642687A1; DE602004031526D1; EP2206583B1; EP2353795B1; US20060184276A1; EP2208582B1; KR101083417B1; EP1642688A4; EP2206584B1; EP1642687B1

Abstract

目標歩容に追従して動作しているロボットの滑りの発生を判断し、ロボットに作用させる床反力水平成分や床反力モーメント鉛直成分等の制限対象量の許容範囲を滑りの判断結果に応じて可変的に設定する。目標歩容の仮運動を動力学モデルを用いて決定し、その仮運動により定まる制限対象量が許容範囲を逸脱したとき、動力学的平衡条件を満たしつつ、制限対象量を許容範囲内に制限するようにロボットの角運動量変化率を仮運動から変化させることにより該仮運動を修正して目標歩容の運動を決定する。

Description

明細書脚式移動ロポッ卜の制御装置技術分野

本発明は、脚式移動口ポットの歩行のみならず、走行にも適した歩容生成装置および制御装置に関する。背景技術

脚式移動ロポット、例えば 2足移動ロポットの移動動作を行わせるための歩容（目標歩容）の生成は、従来、主に口ポットにスムーズな歩行動作を行なわせる歩容（歩行歩容）を生成することを主目的として行なわれてきた。しかし、近年では、脚式移動口.ポットの開発が進むにつれて、該ロボットに歩行だけでなく、走行も行なわせることができる歩容を生'成することが望まれている。さらには、十分な摩擦力を発生できない滑りやすい床 (いわゆる低ミュー路）上でもロボットを支障なく移動させることができる歩容を生成することが望まれている。

尚、「歩容」という漢字に「歩」が入っているので歩行に限定されると誤解されやすいが、本来「歩容」とは、トロットなど、馬の走行形態を指す言葉として用いられているように、走行も含んだ概念である。

ここで、歩行と走行の特徴の違いを説明しておく。

同時に全脚が空中に存在する瞬間がある移動形態を走行と定義することが一般的ではある。しかし、この定義によって歩行と走行が明確に区別できるとは限らない。たとえば、速いジョギングでは、ほとんどの人間において、同時に全脚が空中に存在する瞬間があるが、遅いジョギングでは、かなりの人間において、常にいずれかの脚が接地している。速いジョギングは走行であるのに遅いジョギングは歩行である、と定義するのは、感覚的に少し無理がある。

図 5 1に、典型的な走行における上体鉛直位置と床反力鉛直成分（左右の脚に作用する床反力鉛直成分の和）のパターンを示し、図 5 2には. 典型的な歩行における上体鉛直位置と床反力鉛直成分のパターンを示す, 尚、上体鉛直位置速度は、上体代表点の鉛直位置とその速度を意味する。上体水平位置速度は、上体代表点の水平位置とその速度を意味する, 上体鉛直位置速度と上体水平位置速度を合わせて、上体位置速度と呼ぶ, また、「床反力鉛直成分」は、厳密には、床反力の鉛直軸まわりのモ —メント成分と区別するために、「並進床反力鉛直成分」と記述すべきであるが、言葉が長くなるので、ここでは「並進」を省略する。以降、「並進床反力水平成分」も「並進」を省略して「床反力水平成分」と記述する。

まず上体の動きを見てみると、歩行においては、支持脚の上を上体が通過する瞬間に上体は最も高く、走行においては、この瞬間に最も低い, すなわち、歩行と走行では、上体の上下動パターンの位相が反転している。

一方、床反力に関しては、歩行では比較的一定であるのに対し、走行においては、大きく変動し、支持脚の上を上体が通過する瞬間に床反力は最も大きい。また、当然のことながら、同時に全脚が空中に存在する瞬間において床反力は 0になっている。より詳しく観察すると、走行では、支持脚を縮めた量にほぼ比例した大きさの床反力が発生する。言いかえると、走行では、脚をばねのように使って跳ねながら移動していると Θ る

遅いジョギングでは、典型的な走行と同じ上体上下動の位相を持つ。また、遅いジョギングでは、同時に全脚が空中に存在する瞬間がない場合が多いが、その場合でも、支持脚と遊脚の切り換えが行われる瞬間において、床反力は、完全には 0にならないがほぼ 0になっている。

したがって、以上のように、上体の上下動や床反力のパターンの上記特徴から歩行と走行を区別した方が、遅いジョギングも走行とみなされ. 感覚とも一致して、より適切と言えよう。

特に、最も特徴的な点で両者を区別するならば、走行とは、支持脚の切り換えが行われる瞬間において、床反力が 0またはほぼ 0になる移動形態であり、歩行は、それ以外の移動形態（床反力鉛直成分が比較的一定）であると言えよう。

ところで、本出願人は先に P C T公開公報 WOZ 02/ 40224において. 脚式移動口ポットの床反力を含む歩容を、動力学的平衡条件（目標歩容の重力と慣性力と床反力との釣り合い条件のことである。狭義では、目標歩容の運動による重力と慣性力との合力が目標 Z M Pまわりに作用するモーメントの水平成分が 0であること。詳細は後述する。）をほぼ満 '足しながら、自在かつリアルタイムに生成する技術を先に提案した。この技術や、特開平 1 0— 8 6 0 8 1号公報、特開平 1 0— 2 7 7 9 6 9 号公報などにて本出願人が提案してきた一連の脚式移動ロポットの制御装置は、歩行のみならず走行にも適用できるものである。

しかしながら、この技術においては、目標歩容の並進床反力水平成分や目標歩容の Z M Pまわりの床反カモ一メント鉛直成分の大きさを考慮していなかった。このため、摩擦の限界を超えて、口ポットの支持脚足平の滑り（床面に並行な方向のスリップやスピン）が発生するおそれがあった。なお、スピンとは、実際の口ポットのョ一角（鉛直軸まわり回転角）速度が目標ョー角速度からずれることを言う。 - 摩擦係数の高い床面を歩行する塲合（この場合、少なくとも 1つの脚体は常時接地している）には、床反力鉛直成分が常にほぼ口ポットの自重相当であるので、摩擦力の限界が高い。このため、口ポットの滑りは発生しにくい。

しかるに、走行においては、床反力鉛直成分が 0になる、または 0に近くなる時があるので、その時には、たとえ摩擦係数が高くとも、床面の摩擦力の限界が 0になるまたは 0に近くなる。したがって、目標歩容の並進床反力水平成分や床反力モーメント鉛直成分が限界を超え、ロボットが滑って転倒する恐れがあった。

また、歩行の場合でも、床の摩擦係数が低い場合には、口ポットの滑りが発生して転倒する恐れがあった。

一方、本出願人は先に、例えば P C T出願 P C T Z J P 0 2 / 1 3 5 9 6等において、目標歩容の並進床反力水平成分が許容範囲を超えないように、目標歩容を生成したり、目標歩容において腕以外が発生するモ —メント鉛直成分を打ち消すように、腕を振るようにする技術を提案した。この技術によれば、ロボットの滑りの発生を抑制できる。

- しかるに、床面の状態などによっては、並進床反力水平成分の許容範囲が実際の床面の摩擦力の限界と整合しない場合もあり、その場合には, 口ポットの滑りが発生する恐れがあった。そして、これを回避するために、並進床反力水平成分の許容範囲を狭めに設定すると、上体の姿勢 (傾斜角）の大きく変動しやすい。また、脚を激しく振って移動する場合には、モーメント鉛直成分を打ち消すために、腕の振りも激しくなるしたがって、本発明の目的は、上記の問題点を解消し、歩行、走行等の歩容形態や床面の摩擦状態によらずに、口ポットの滑りの発生をより確実に防止し、より一層好適な歩容を生成することができる制御装置を提供することにある。発明の開示本発明の脚式移動ロポットの制御装置の第 1発明は、

上体から延設された複数の脚体を運動させて移動する脚式移動ロポットの目標歩容を生成し、その目標歩容に追従するようにロポットの動作を制御する制御装置において、

前記目標歩容に追従して動作している前記ロボッ卜の滑りの発生を判断する滑り判断手段と、

前記ロボットに作用させる並進床反力の水平成分又は該並進床反力の床面平行成分又は該ロポットの全体重.心加速度の水平成分又は該全体重心加速度の床面平行成分を制限対象量として、該制限対象量の許容範囲を前記滑り判断手段の判断結果に応じて可変的に設定する許容範囲設定手段と、

所定の動力学モデル上で前記ロボットに作用する重力と慣性力との合力が所定の動力学的平衡条件を満たすように前記目標歩容の仮運動を決定する仮運動決定手段と、

前記目標歩容の仮運動により定まる前記制限対象量が前記許容範囲を逸脱したとき、前記動力学的平衡条件を満たしつつ、該制限対象量を前記許容範囲内に制限するようにロボットの重心回りの角運動量変化率を前記仮運動から変化させることにより該仮運動を修正して目標歩容の運動を決定する仮運動修正手段とを備えたことを特徴とする。

この第 1発明によれば、目標歩容の仮運動により定まる前記制限対象量（並進床反力の水平成分又は該並進床反力の床面平行成分又は該ロボットの全体重心加速度の水平成分又は該全体重心加速度の床面平行成分）が前記許容範囲を逸脱したとき、該仮運動を修正して目標歩容の運動を決定する。この場合、口ポットの重心回りの角運動量変化率を前記仮運動から変化させるので、動力学的平衡条件（例えば目標 Z M Pまわりの床反力モーメント水平成分が 0になるなどの条件）を満たしつつ、制限対象量を許容範囲内に制限することができる。なお、変化させる角運動量変化率は、基本的にはロール方向、あるいはピッチ方向の角運動両変化率でよい。そして、この場合、前記許容範囲は、前記滑り判断手段の判断結果に応じて可変的に設定されるので、実際のロボットの滑りの発生の有無に応じて即応的に目標運動を修正できる。この結果、実際の口ポットの滑りが発生しても、直ちにその滑りを抑制するようにロボットの運動が修正される。従って、第 1発明によれば、実際の口ポットの滑りの発生を抑制でき、口ポットの姿勢の安定性を保つことができる, なお、滑りの発生が有ると判断されたときには、前記許容範囲は、それを狭めるように設定されるべきであり、このことは、以下に説明する第 2〜第 4発明においても同様である。

次に本発明の脚式移動ロボッ卜の第 2発明は、

前記目標歩容に追従して動作している前記ロポットの滑りの発生を判断する滑り判断手段と、

前記ロポットに作用させる床反カモ一メントの鉛直成分又は該床反力モーメントの床面法線方向成分又は該ロポットの角運動量変化率の鉛直成分又は該角運動量変化率の床面法線方向成分を制限対象量として、該制限対象量の許容範囲を前記滑り判断手段の判断結果に応じて可変的に設定する許容範囲設定手段と、

前記目標歩容の仮運動により定まる前記制限対象量が前記許容範囲を逸脱したとき、前記動力学的平衡条件を満たしつつ、該制限対象量を前記許容範囲内に制限するようにロポットの角運動量変化率を前記仮運動から変化させることにより該仮運動を修正して目標歩容の運動を決定する仮運動修正手段とを備えたことを特徴とする。

この第 2発明によれば、目標歩容の仮運動により定まる前記制限対象量 (床反力モーメントの鉛直成分又は該床反力モーメントの床面法線方向成分又は該ロポットの角運動量変化率の鉛直成分又は該角運動量変化率の床面法線方向成分）が前記許容範囲を逸脱したとき、該仮運動を修正して目標歩容の運動を決定する。この場合、口ポットの角運動量変化率を前記仮運動から変化させるので、動力学的平衡条件（例えば目標 Z M Pまわりの床反力モーメント水平成分が 0になるなどの条件) を満たしつつ、制限対象量を許容範囲内に制限することができる。なお、変化させる角運動量変化率は、基本的にはョ一方向の角運動量変化率でよい, そして、この場合、前記許容範囲は、前記滑り判断手段の判断結果に応じて可変的に設定されるので、実際の口ポットの滑りの発生の有無に応じて即応的に目標運動を修正できる。この結果、実際の口ポットの滑りが発生しても、直ちにその滑り（特にスピン）を抑制するようにロボットの運動が修正される。従って、第 1発明によれば、実際の口ポットの滑りの発生を抑制でき、ロボットの姿勢の安定性を保つことができる。なお、第 1発明と第 2発明とは、併用してもよい。この場合には、並進床反力の水平成分又は該並進床反力の床面平行成分又は該ロポッ卜の全体重心加速度の水平成分又は該全体重心加速度の床面平行成分を 1つの制限対象量とすると共に、床反力モーメントの鉛直成分又は該床反力モーメントの床面法線方向成分又は該ロポットの角運動量変化率の鉛直成分又は該角運動量変化率の床面法線方向成分を他の 1つの制限対象量とし、これらの許容範囲を滑り判断手段の判断結果に応じて可変的に設定すればよい。この場合には、いずれか一方の制限対象量が許容範囲を逸脱したときに、目標運動の仮運動を修正することが望ましい。

次に、本発明に脚式移動ロポッ卜の制御装置の第 3発明は、

上体から延設された脚体を運動させて移動する脚式移動ロボッ卜の目標運動の瞬時値を、少なくとも該ロポットの運動と床反力との関係を表す動力学モデルを用いて逐次決定しつつ、その決定した目標運動の瞬時値に追従させるように前記ロボッ卜の動作を制御する脚式移動ロボットの制御装置において、 .

前記目標運動に追従して動作しているロボットの滑りの発生を判断する滑り判断手段と、

少なくとも前記ロポットに作用させる並進床反力の水平成分又は該並進床反力の床面平行成分又は該ロポッ卜の全体重心加速度の水平成分又は該全体重心加速度の床面平行成分を制限対象量として、該制限対象量の許容範囲を前記滑り判断手段の判断結果に応じて可変的に設定する許容範囲設定手段と、

少なくとも前記決定された目標運動の瞬時値に対応するロボッ卜の姿勢の目標状態量と該ロポットの姿勢の実状態量との偏差に基づいて、前記目標運動の新たな瞬時値を、該新たな瞬時値に対応して前記動力学モデルに基づき定まる前記制限対象量が前記許容範囲内に収まると共に前記偏差が 0に近づくように決定する目標瞬時値決定手段とを備えたことを特徴とする。

この第 3発明によれば、少なくともロポッ卜の姿勢の状態量に関する前記偏差に基づいて、前記目標運動の新たな瞬時値を、該新たな瞬時値に対応して前記動力学モデルに基づき定まる前記制限対象量（並進床反力の水平成分又は該並進床反力の床面平行成分又は該ロポットの全体重心加速度の水平成分又は該全体重心加速度の床面平行成分）が前記許容範囲内に収まると共に前記偏差が 0に近づくように決定する。つまり、ロボットの姿勢を所望の姿勢に保つようにしつつ、制限対象量が許容範囲を逸脱しないように（以下、この条件を、ここでの説明では姿勢 Z制限対象量条件ということがある）目標運動の瞬時値が決定される。そして、この場合、前記許容範囲は、前記滑り判断手段の判断結果に応じて可変的に設定されるので、実際の口ポットの滑りの発生の有無に応じて即応的に、前記姿勢 Z制限対象量条件を満たすように目標運動の瞬時値を決定できる。この結果、実際の口ポットの滑りが発生しても、該ロボットの姿勢を適切な姿勢に安定に保ちつつ、直ちにその滑りを抑制することができる。

また、本発明の脚式移動ロボットの制御装置の第 4発明は、

上体から延設された脚体を運動させて移動する脚式移動ロポッ卜の目標運動及び目標床反力の瞬時値を、少なくとも該ロポットの運動と床反力との関係を表す動力学モデルを用いて逐次決定しつつ、その決定した目標運動及び目標床反力の瞬時値に追従させるように前記ロボッ卜の動作を制御する脚式移動ロボッ卜の制御装置において、

前記目標運動及び目標床反力に追従して動作しているロボットの滑りの発生を判断する滑り判断手段と、

少なくとも前記決定された目標運動及び目標床反力の瞬時値に対応するロポットの姿勢の目標状態量と該ロポットの姿勢の実状態量との偏差に基づいて、前記目標運動及び目標床反力の新たな瞬時値を、該新たな目標運動の瞬時値に対応して前記動力学モデルに基づき定まる前記制限対象量が前記許容範囲内に収まると共に前記偏差が 0に近づくように決定する目標瞬時値決定手段とを備えたことを特徴とする。

この第 4発明によれば、少なくともロポッ卜の姿勢の状態量に関する前記偏差に基づいて、前記目標運動及び目標床反力の新たな瞬時値を、該新たな目標運動の瞬時値に対応して前記動力学モデルに基づき定まる前記制限対象量（並進床反力の水平成分又は該並進床反力の床面平行成分又は該ロボットの全体重心加速度の水平成分又は該全体重心加速度の床面平行成分）が前記許容範囲内に収まると共に前記偏差が 0に近づくように決定する。つまり、口ポットの姿勢を所望の姿勢に保つようにしつつ、制限対象量が許容範囲を逸脱しないように' （以下、この条件を、第 3発明と同様に、姿勢 Z制限対象量条件ということがある）目標運動および目標床反力の瞬時値が決定される。そして、この場合、前記許容範囲は、前記滑り判断手段の判断結果に応じて可変的に設定されるので. 前記第 3発明と同様、実際のロボットの滑りの発生の有無に応じて即応的に、前記姿勢制限対象量条件を満たすように目標運動及び目標床反力の瞬時値を決定できる。この結果、実際の口ポットの滑りが発生しても、該ロポットの姿勢を適切な姿勢に安定に保ちつつ、直ちにその滑りを抑制することができる。また、この場合、目標運動と目標床反力との両者が前記姿勢 Z制限対象量条件によって規制されるので、効果的に滑りを抑制できる。

前記第 1〜第 4発明では、例えば、例えば、次のようにして、滑りの発生を判断することができる。

すなわち、前記滑り判断手段は、少なくとも接地している脚体の先端部の対地速度に基づき滑りの発生を判断する（第 5発明）。この場合、例えば該対地速度の絶対値が所定値よりも大きいときには、滑りの発生が有ると判断することができる。

あるいは、前記滑り判断手段は、少なくとも接地している脚体に作用する実床反力の時間的変化率と該脚体の先端部の対地速度とに基づき、該脚体の見かけばね定数を求める手段を備え、少なくとも該見かけばね定数に基づき滑りの発生を判断する（第 6発明）。この場合、例えば該見かけばね定数が所定値よりも小さいときには、滑りの発生が有ると判断することができる。

あるいは、前記滑り判断手段は、少.なくとも接地している脚体に作用する実床反力を、所定周波数の近傍範囲に周波数通過特性を有するパンドバスフィル夕に通したものに基づき滑りの発生を判断する（第 7発明）。この場合、前記実床反力をバンドパスフィルタに通したものは、いわゆる滑り振動が発生しているときの、実床反力の振動成分に相当するものとなる。そして、例えばこの振動成分の大きさ（絶対値）が所定値よりも大きいときには、滑り振動の発生が有ると判断することができる。

なお、滑りの発生の判断は、第 5〜第 7発明のいずれでも行なうことができるが、これらの第 5〜第 7発明の 2つ以上を組み合わせて滑りの発生の判断を行なうようにしてもよい。図面の簡単な説明

図 1は本発明の実施形態およびこれに関連する参考例における脚式移動ロポットとしての 2足移動ロポットの全体構成を概略的に示す図、図 2は図 1のロボットの脚体の先端部の構造を示す図、図 3は図 1のロボッ卜に備えた制御ュニッ卜の構成を示すブロック図、図 4は第 1参考例における制御ユニットの機能的構成を示すブロック図である。図 5は実施形態および参考例で生成する走行歩容を説明するための図、図 6は目標床反力鉛直成分軌道の例を示すグラフ、図 7は目標 Z M P軌道の X成分および Y成分の例を示すグラフ、図 8はロポットの上体並進モ一ドを説明するための図、図 9はロポットの上体傾斜モードを説明するための図、図 1 0は口ポットの上体ョ一回転モードを説明するための図、図 1 1 ( a ) は口ポットの逆位相腕振りモードを平面視で説明するための図. 図 1 1 ( b ) は口ポットの逆位相腕振りモ一ドを側面視で説明するための図、図 1 2は実施形態で用いる動力学モデルを説明するための図である。図 1 3は第 1参考例における歩容生成装置のメインルーチン処理を示すフローチヤ一ト、図 1 4はロポットの発散状態を説明するための図, 図 1 5は図 1 3の S 0 2 2のサブルーチン処理を示すフローチャート、図 1 6は定常歩容と支持脚座標系とを説明するための図、図 1 7は定常歩容の上体軌道と支持脚座標系とを例示する図、図 1 8は基準逆位相腕振り角の例を示すグラフ、図 1 9は定常歩容における目標床反力鉛直成分軌道の設定例を示すグラフ、図 2 0は定常歩容における床反力水平成 '分許容範囲の設定例を示すグラフ、図 2 1は定常歩容における床反カモーメント鉛直成分許容範囲の設定例を示すグラフ、 0 2 2は定常歩容における目標 Z M P軌道の設定例を示すグラフである。図 2 3は図 1 3の S 0 2 4のサブルーチン処理を示すフローチヤ一ト、図 2 4は図 2 3の S 2 0 8のサブルーチン処理を示すフローチヤ一ト、図 2 5は図 2 4の S 3 0 6のサブルーチン処理を示すフローチャート、図 2 6は図 2 5の S 4 1 2のサブルーチン処理を示すフローチヤ一トである。図 2 7は許容範囲を考慮しない床反力水平成分の例を示すグラフ、図 2 8は許容範囲を考慮した床反力水平成分の例を示すグラフ、図 2 9は上体傾斜角加速度の例を示すグラフ、図 3 0はロボッ卜の上体傾斜角を復元するための上体傾斜復元モーメント Z M P換算値の例を示すグラフ、図 3 1は上体傾斜復元モーメント Z M P換算値を加味した上体傾斜角加速度の例を示すグラフ、図 3 2は許容範囲を考慮しない床反カモ一メント鉛直成分の例を示すグラフ、図 3 3は許容範囲を考慮した床反力モーメント鉛直成分の例を示すグラフ、図 3 4は逆位相腕振りモーメントの例を示すグラフ、図 3 5は図 3 4の逆位相腕振りモーメントに対応する逆位相腕振り角加速度を示すグラフ、図 3 6は逆位相腕振り角を復元するための逆位相腕振り復元角加速度の例を示すグラフ、図 3 7は図 3 6の逆位相腕振り復元角加速度に対応する逆位相腕振り復元モーメントを示すグラフ. 図 3 8は図 3 3の床反力モーメント鉛直成分と図 3 7の逆位相腕振り復元モーメントとを合成してなる床反力モーメント鉛直成分を示すグラフである。図 3 9は図 1 3の S 0 2 6のサブルーチン処理を示すフローチヤート、図 4 0は今回歩容の床反力水平成分許容範囲の設定例を示すグラフ、図 4 1は今回歩容の床反力モーメント鉛直成分許容範囲の設定例を示すグラフ、図 4 2は図 1 3の S 0 2 8のサブルーチン処理を示すフローチャート、図 4 3は図 4 2の S 7 0 2のサブルーチン処理を示すフローチヤ一卜、図 4 4は今回歩容の仮目標 Z M Pと Z M P修正量と修正後の目標 Z M Pとの例を示すグラフ、図 4 5は図 1 3の S 0 3 0のサブルーチン処理を示すフローチャート、図 4 6は図 4 5の S 1 4 1 2のサブルーチン処理を示すフロ一チヤ一トである。図 4 7は定常歩容と目標歩容の上体位置軌道との関係を示すグラフ、図 4 8は上体傾斜モード (腰中心の上体傾斜）の他の例を示す図、図 4 9は動力学モデルの他の例を説明するための図、図 5 0は歩行歩容における目標床反力鉛直成分の設定例を示す図、図 5 1はロポットの走行歩容における上体鉛直方向位置と床反力鉛直成分との関係を示す図、図 5 2はロボットの歩行歩容における上体鉛直方向位置と床反力鉛直成分との関係を示す図である。図 5 3は第 2参考例における制御ュニットの機能的構成を示すブロック図、図 5 4は図 5 3に示す補償全床反力モーメント水平成分分配器の処理を示すブロック図、図 5 5は図 5 3.に示すモデル操作床反力モーメント鉛直成分決定器の処理を示すブロック図、図 5 6は第 2参考例における歩容生成装置のメインルーチン処理を示すフローチャート、図 5 7は図 5 6の S 3 0 3 2のサブルーチン処理を示すフローチャート、図 5 8 は図 5 7の S 3 4 1 4のサブルーチン処理を示すフロ一チヤ一トである, 図 5 9は第 3参考例における制御ュニットの機能的構成を示すブロック図、図 6 0は第 2実施形態における歩容生成装置のメインルーチン処理を示すフ口一チャート、図 6 1は図 6 .0の S 2 0 3 4のサブル一チン処理を示すフローチヤ一ト、図 6 2は図 6 1の S 2 1 1 4のサブルーチン処理を示すフローチャートである。図 6 3は第 4参考例における歩容生成装置の機能的構成を示すブロック図、図 6 4は第 4参考例における Z M P許容範囲の設定例を示す図、図 6 5は第 4参考例における歩容生成装置のメインルーチン処理を示すフローチャート、図 6 6は図 6 5の S 3 5 3 6の処理を示すブロック図、図 6 7は図 6 6に示す上体水平位置摂動用モデルを説明するための図、図 6 8は図 6 6に示す上体姿勢角補正用摂動モデルの説明するための図、図 6 9は図 6 6に示す逆位相腕振り角補正用摂動モデルを説明するための図、図 7 0は図 6 6に示す逆位相腕振り角補正用摂動モデルモーメント決定部の処理を示すプロック図である。図 7 1は第 5参考例における図 6 5の S 3 5 3 6の処理を示すブロック図、図 7 2は図 7 1に示す逆位相腕振り角補正用摂動モデルモーメント決定部の処理を示すブロック図である。図 7 3は第 6参考例における歩容生成装置の機能的構成を示すブロック図、図 7 4は図 7 3に示す擬似順フルモデルの処理を示すブロック図である。図 7 5は第 7参考例における図 6 5の S 3 5 3 6の処理を示すブロック図である。図 7 6は本発明の第 1実施形態における制御ュニットの機能的構成を示すブロック図、図 7 7は第 1実施形態における歩容生成装置のメインルーチン処理を示すフローチヤ一ト、図 7 8は図 7 7の S 2 3 3 4の処理を示すフローチャート、図 7 9は図 7 6に示す滑り判定部の処理を示すフロ —チヤ一ト、図 8 0〜図 8 2はそれぞれ図 7 9の S 5 2 1 0、 S 5 2 1 2、 S 5 2 1 4のサブルーチン処理を示すフローチャート、図 8 3は滑り判定部の判定結果と、許容範囲の低減率と、床反力許容範囲との例を示すグラフ、図 8 4は第 5〜第 7参考例に関する変形態様の例を示すブロック図、図 8 5は第 4参考例に関する逆位相腕振り角補正用摂動モデルモーメント決定部の処理の変形態様の例を示すブロック図である。発明を実施するための最良の形態

以下、添付図面を参照してこの発明の実施形態に係る脚式移動ロポットの制御装置を説明する。尚、脚式移動口ポットとしては 2足移動ロボッ卜を例にとる。

まず、図 1〜図 4 7を参照して、本発明の脚式移動口ポットの制御装置に関連する第 1参考例を説明しておく。さらに、図 5 3〜図 5 8を参照して第 2参考例を、図 5 9〜図 6 2を参照して第 3参考例を、図 6 3 〜図 7 0を参照して第 4参考例を、図 7 1及び図 7 2を参照して第 5参考例を、図 7 3及び図 7 4を参照して第 6参考例を、図 7 5を参照して第 7参考例を説明しておく。後述する本発明の実施形態は、その機構的構成は第 1参考例と同一であり、また、口ポットの歩容生成処理および制御処理の一部（具体的には許容範囲設定処理）のみが第 1〜第 7参考例のいずれかと相違するものである。従って、後述する実施形態の説明では、この第 1〜第 7参考例の説明をしばしば援用する。補足すると、図 1 と後述の図 2、図 3並びに図 5〜図 1 2を参照して説明する事項は、後述の実施形態においても同一である。

まず、第 1参考例を説明する。図 1は、この第 1参考例に係る脚式移動口ポットとしての 2足移動ロボットを全体的に示す概略図である。図示の如く、 2足移動口ポット（以下、口ポットという） 1 は上体 (口ポット 1の基体） 3から下方に延設された左右一対の脚体（脚部リンク） 2， 2を備える。両脚体 2 , 2は同一構造であり、それぞれ 6個の関節を備える。その 6個の関節は上体 3側から順に、股（腰部）の回旋（回転）用（上体 3に対するョ一方向の回転用）の関節 1 0 R, 1 0 L (符号 R， Lはそれぞれ右側脚体、左側脚体に対応するものであることを意味する符号である。以下同じ） .と、股（腰部）のロール方向（X 軸まわり）の回転用の関節 1 2 R， 1 2 Lと、股（腰部）のピッチ方向 (Y軸まわり）の回転用の関節 1 4 R, 1 4 Lと、膝部のピッチ方向の回転用の関節 1 6 R, 1 6 Lと、足首のピッチ方向の回転用の関節 1 8 R， 1 8 Lと、足首のロール方向の回転用の関節 2 0 R, 2 0 Lとから構成される。

各脚体 2の足首の 2つの関節 1 8 R (L ), 2 O R (L) の下部には、各脚体 2の先端部を構成する足平（足部） 2.2 R (L) が取着されると共に、両脚体 2 , 2の最上位には、各脚体 2の股の 3つの関節 1 O R (L), 1 2 R (L), 1 4 R (L) を介して前記上体 3が取り付けられている。上体 3の内部には、詳細を後述する制御ユニット 6 0などが格納される。なお、図 1では図示の便宜上、制御ユニット 6 0を上体 3の外部に記載している。

上記構成の各脚体 2においては、股関節（あるいは腰関節）は関節 1 O R (L)， 1 2 R (L), 1 4 R (L) から構成され、膝関節は関節 1 6 R (L) から構成され、足首関節は関節 1 8 R (L), 2 0 R (L) から構成される。また股関節と膝関節とは大腿リンク 2 4 R (L) で連結され、膝関節と足首関節とは下腿リンク 2 6 R (L) で連結される。上体 3の上部の両側部には左右一対の腕体 5 , 5が取り付けられると共に、上体 3の上端部には頭部 4が配置される。各腕体 5は、 3つの関節 3 0 R (L), 3 2 R (L), 3 4 R (L) から構成された肩関節と、関節 3 6 R (L) から構成された肘関節と、関節 3 8 R (L) から構成された手首関節と、この手首関節に連結された手先部 4 0 R (L) とを備えている。なお、頭部 4は、本発明の要旨と直接的な関連を有しないため詳細な説明を省略する。

上記の構成により、各脚体 2の足平 2 2 R (L) は、上体 3に対して 6つの自由度を与えられている。そして、口ポット 1の歩行等の移動中に、両脚体 2 , 2を合わせて 6 * 2 = 1 2個（この明細書で「*」はスカラに対する演算としては乗算を、ベクトルに対する演算としては外積を示す）の関節を適宜な角度で駆動することで、両足平 2 2 R， 2 2 L の所望の運動を行うことができる。これにより、口ポット 1は任意に 3 次元空間を移動することができる。また、各腕体 5は、その肩関節、肘関節、手首関節の回転によって、腕振り等の運動を行うことができる。 · 図 1 に示す如く、各脚体 2の足首関節 1 8 R (L), 2 0 R (L) の下方には足平 2 2 R (L) との間に公知の 6軸力センサ 5 0が介装されている。該 6軸力センサ 5 0は、各脚体 2の足平 2 2 R (L) の着地の有無、および各脚体 2に作用する床反力（接地荷重）等を検出するためのものであり、該床反力の並進力の 3方向成分 F x， F y , F z並びにモーメントの 3方向成分 M X , My, M zの検出信号を制御ユニット 6 0に出力する。また、上体 3には、 Z軸（鉛直方向（重力方向））に対する上体 3の傾斜角およびその角速度、並びに Z軸回りの上体 3の回転角（ョ一角）およびその角速度を検出するための姿勢センサ 5 4が備えられ、その検出信号が該姿勢センサ 5 4から制御ュニット 6 0に出力される。この姿勢センサ 5 4は、図示を省略する 3軸方向の加速度センサおよび 3軸方向のジャィ口センサを備え、これらのセンサの検出信号が上体 3の姿勢角（傾斜角およびョ一角）およびその角速度を検出するために用いられると共に、口ポット 1の自己位置姿勢を推定するために用いられる。また、詳細構造の図示は省略するが、口ポット 1の各関節には、それを駆動するための電動モータ 6 4 (図 3参照）と、その電動モ一夕 6 4の回転量（各関節の回転角）を検出するためのエンコーダ（口一夕リエンコーダ） 6 5 (図 3参照）とが設けられ、該エンコーダ 6 5 の検出信号が該エンコーダ 6 5から制御ュニット 6 0に出力される。さらに、図 1では図示を省略するが、ロボット 1の適宜な位置にはジョィスティック（操作器） 7 3 (図 3参照）が設けられ、そのジョイスティック 7 3を操作することで、直進移動しているロポット 1を旋回させるなど、口ポット 1の歩容に対する要求を必要に応じて制御ュニット 6 0に入力できるように構成されている。

図 2 は本第 1参考例における各脚体 2 の先端部分 (各足平 2 2 R ( L ) を含む）の基本構成を概略的に示す図である。同図に示すように. 各足平 2 2 R ( L ) の上方には、前記 6軸力センサ 5 0との間にばね機構 7 0が装備されると共に、足底（各足平 2 2 R ( L ) の底面）にはゴムなどからなる足底弾性体 7 1が貼られている。これらのばね機構 7 0 及び足底弾性体 7 1によりコンプライアンス機構 7 2が構成されている < ばね機構 7 0は詳細は後述するが、足平 2 2 R ( L ) の上面部に取り付けられた方形状のガイド部材（図示省略）と、足首関節 1 8 R ( L ) (図 2では足首関節 2 O R ( L ) を省略している）および 6軸力センサ 5 0側に取り付けられ、前記ガイド部材に弹性材（ゴムやばね）を介して微動自在に収納されるピストン状部材（図示省略）とから構成されている。

図 2に実線で表示された足平 2 2 R ( L ) は、床反力を受けていないときの状態を示している。各脚体 2が床反力を受けると、コンプライアンス機構 7 2のばね機構 7 0と足底弾性体 7 1 とがたわみ、足平 2 2 R (L) は図中に点線で例示したような位置姿勢に移る。このコンプラインァス機構 7 2の構造は、例えば本出願人が先に提案した特開平 5— 3 0 5 5 8 4号公報に詳細に説明されている如く、着地衝撃を緩和するためだけでなく、制御性を高めるためにも重要なものである。

図 3は制御ュニット 6 0の構成を示すブロック図である。該制御ュニット 6 0はマイクロコンピュータにより構成されており、 C PUからなる第 1の演算装置 9 0及び第 2の演算装置 9 2、 AZD変換器 8 0、力ゥン夕 8 6、 D/A変換器 9 6、 RAM 8 4 , ROM 9 4、並びにこれらの間のデータ授受を行うバスライン 8 2を備えている。この制御ュニット 6 0では、各脚体 2の 6軸力センサ 5 0、姿勢センサ 5 4 (加速度センサおよびレートジャィ口センサ）、ジョイスティック 7 3等の出力信号は AZD変換器 8 0でデジタル値に変換された後、バスライン 8 2 を介して RAM 8 4に送られる。またロポット 1の各関節のエンコーダ 6 5 (口一夕リーエンコーダ）の出力は、カウン夕 8 6を介して RAM 84に入力される。

前記第 1の演算装置 9 0は後述の如く目標歩容を生成すると共に、関節角変位指令（各関節の変位角もしくは各電動モータ 6 4の回転角の指令値）を算出し、 RAM 8 4に送出する。また第 2の演算装置 9 2は R AM 84から関節角変位指令と、前記エンコーダ 6 5の出力信号に基づいて検出された関節角の実測値とを読み出し、各関節の駆動に必要な操作量を算出して DZ A変換器 9 6とサーポアンプ 6 4 aとを介して各関節を駆動する電動モータ 6 4に出力する。

図 4は、この第 1参考例に係る脚式移動ロポッ卜の制御装置の機能的構成を全体的に示すブロック図である。この図 4中の「実口ポット」の部分以外の部分が制御ュニット 6 0が実行する処理機能（主として第 1 の演算装置 9 0及び第 2の演算装置 9 2の機能）によって構成されるものである。なお、以下の説明では、脚体 2および腕体 5の左右を特に区別する必要がないときは、前記符号 R , Lを省略する。

以下説明すると、制御ユニット 6 0は、口ポット 1の目標歩容を自在かつリアルタイムに生成して出力する歩容生成装置 1 0 0を備えている, 該歩容生成装置 1 0 0は、その機能によって、本願発明の各手段を構成するものである。この歩容生成装置 1 0 0が出力する目標歩容は、目標上体位置姿勢軌道（上体 3の目標位置及び目標姿勢の軌道）、目標足平位置姿勢軌道（各足平 2 2の目標位置及び目標姿勢の軌道）、目標腕姿勢軌道（各腕体 5の目標姿勢の軌道）、目標全床反力中心点（目標 Z M P ) 軌道、目標全床反力軌道から構成される。なお、脚体 2や腕体 5以外に上体 3に対して可動な部位を備える場合には、その可動部位の目標位置姿勢軌道が目標歩容に加えられる。

ここで、上記歩容における「軌道」は時間的変化のパターン（時系列パターン）を意味し、以下の説明では、「軌道」の代わりに「パターン」と称することもある。また、「姿勢」は空間的な向きを意味する。具体的には、例えば上体姿勢は Z軸（鉛直軸）に対する口一ル方向（X 軸まわり）の上体 3の傾斜角とピッチ方向（Y軸まわり）の上体 3の傾斜角とョー方向（Z軸まわり）の上体 3の回転角（ョ一角）とで表され, 足平姿勢は各足平 2 2に固定的に設定された 2軸の空間的な方位角で表される。本明細書では、上体姿勢は上体姿勢角ということもある。また、上体姿勢のうち、鉛直方向に対する姿勢を上体姿勢傾斜もしくは上体姿勢傾斜角ということもある。

尚、以下の説明では、誤解を生じるおそれがない場合には、しばしば「目標」を省略する。また、歩容のうちの、床反力に係わる構成要素以外の構成要素、すなわち足平位置姿勢、上体位置姿勢等、口ポット 1の運動に係わる歩容を総称的に「運動」という。また、各足平 2 2に作用する床反力（並進力及びモーメントからなる床反力）を「各足平床反力」と呼び、口ポット 1の全て（ 2つ）の足平 2 2 R， 2 2 Lについての「各足平床反力」の合力を「全床反力」という。ただし、以下の説明においては、各足平床反力はほとんど言及しないので、特に断らない限り、「床反力」は「全床反力」と同義として扱う。

目標床反力は、一般的には、作用点とその点に作用する並進力及びモ —メントによって表現される。作用点はどこにとっても良いので、同一の目標床反力でも無数の表現が考えられるが、特に目標床反力中心点 (全床反力の中心点の目標位置）を作用点にして目標床反力を表現すると、目標床反力のモーメント成分は、鉛直成分（鉛直軸（Z軸）まわりのモーメント）を除いて零になる。換言すれば、目標床反力中心点まわりの目標床反力のモーメントの水平成分（水平軸（X軸及び Y軸）回りのモーメント）は零になる。

· 尚、動力学的平衡条件を満足する歩容では、ロボット 1の目標運動軌道から算出される Z M P (目標運動軌道から算出される慣性力と重力との合力がその点まわりに作用するモーメントが、鉛直成分を除いて零になる点）と目標床反力中心点は一致することから、目標床反力中心点軌道の代わりに目標 Z M P軌道を与えると言っても同じ'ことである（詳細は、本願出願人による P C T公開公報 WOZ02Z40224等を参照）。

このような背景から、上記 P C T公開公報 WOZ02Z40224の明細書では目標歩容を、次のように定義していた。

a ) 広義の目標歩容とは、 1歩ないしは複数歩の期間の目標運動軌道とその目標床反力軌道との組である。

b ) 狭義の目標歩容とは、 1歩の期間の目標運動軌道とその Z M P軌道との組である。 c )一連の歩容は、いくつかの歩容がつながったものとする。

歩行においては、本出願人が先に特開平 1 0— 8 6 0 8 0号公報で提案した上体高さ決定手法によってロポット 1の上体 3の鉛直位置（上体高さ）が決定されると、並進床反力鉛直成分は従属的に決定される。さらに、目標歩容の運動による慣性力と重力との合力が目標 Z M Pまわりに発生するモーメン卜の水平成分が零になるように口ポット 1の上体水平位置軌道を決定することで、並進床反力水平成分も決定される。このため、 P C T公開公報 WOZ 02Z40224の明細書では、目標歩容の床反力に関して明示的に設定すべき物理量としては、目標 Z M Pだけで十分であった。したがって、狭義の目標歩容の定義としては、上記の b ) で十分であった。それに対し、本第 1参考例で説明する口ポット 1の走行歩容（詳細は後述する）においては、床反力鉛直成分（並進床反力鉛直成分）も制御上重要である。このため本発明では、この床反力鉛直成分の目標軌道を明示的に設定した上で、ロボット 1の目標上体鉛直位置等の軌道を決定する。そこで、本明細書では、狭義の目標歩容の定義として、次の b ' ) を用いる。

b ' ) 狭義の目標歩容とは、 1歩の期間の目標運動軌道と、その目標 Z M P軌道と目標並進床反力鉛直成分軌道とを少なくとも含む目標床反力軌道との組である。

この明細書では以降、理解を容易にするために、特にことわらない限り、目標歩容は上記 b ' ) の狭義の目標歩容の意味で使用する。この場合、目標歩容の「 1歩」は、口ポット 1の片方の脚体 2が着地してからもう一方の脚体 2が着地するまでの意味で使用する。補足すると、本明細書の参考例および実施形態では、目標歩容は 1歩の期間分の歩容の意味で使用するが、それは必ずしも 1歩の期間分のものである必要はなく、複数歩分、あるいは 1歩よりも短い（例えば半歩など）の期間分の歩容であってもよい。尚、以下の説明では、「床反力鉛直成分」は「並進床反力鉛直成分」を意味するものとし、床反力のうちのモーメントの鉛直成分（鉛直軸回り成分）は、「モーメント」という用語を用いて「床反力鉛直成分」と区別をする。同様に、「床反力水平成分」は「並進床反力水平成分」を意味するものとする。

また、歩容における両脚支持期とは言うまでもなく、ロボット 1がその自重を両脚体 2， 2で支持する期間、片脚支持期とはいずれか一方のみの脚体 2でロボット 1の自重を支持する期間、空中期とは両脚体 2 , 2が床から離れている（空中に浮いている）期間を言う。片脚支持期においてロボット 1の自重を支持しない側の脚体 2を「遊脚」と呼ぶ。尚. 本第 1参考例で説明する走行歩容では、両脚支持期は無く、片脚支持期

(着地期）と空中期とが交互に繰り返される。この場合、空中期では両脚体 2 , 2 とも、口ポット 1の自重を支持しないこととなるが、該空中期の直前の片脚支持期において遊脚であった脚体 2、支持脚であった脚体 2をそれぞれ該空中期においても遊脚、支持脚と呼ぶ。

図 5に示す走行歩容を例にして、歩容生成装置 1 0 0が生成する目標歩容の概要を説明する。なお、歩容に関するその他の定義および詳細は，先に本願出願人が提案した特開平 1 0 — 8 6 0 8 1号公報にも記載されているので、以下では、特開平 1 0— 8 6 0 8 1号公報に記載されていない内容を主に説明する。

まず、図 5に示す走行歩容を説明する。この走行歩容は、人間の通常的な走行歩容と同様の歩容である。この走行歩容では、口ポット 1の左右いずれか一方のみの脚体 2 (支持脚）の足平 2 2が着地（接地）する片脚支持期と、両脚体 2， 2が空中に浮く空中期とが交互に繰り返される。図 5の 1番目の状態は片脚支持期の開始時（初期）の状態、 2番目の状態は片脚支持期の中間時点の状態、 3番目の状態は片脚支持期に続く空中期の開始時（片脚支持期の終了時）の状態、 4番目の状態は空中期の中間時点の状態、 5番目の状態は空中期の終了時（次の片脚支持期の開始時）の状態を示している。

この走行歩容では、ロボット 1は、図 5の 1番目の状態で示すように. 片脚支持期の開始時において支持脚（ロボット 1の進行方向前側の脚体 2 ) 側の足平 2 2のかかとで着地する。続いて、図 5の 2番目の状態で示すように、口ポット 1は、着地した足平 2 2 (支持脚側の足平 2 2 ) の底面のほぼ全面を着地させた後、図. 5の 3番目の状態で示すように、その支持脚側の足平 2 2 (図 5の 3番目の状態におけるロポット 1の進行方向後側の脚体 2の足平 2 2 ) のつま先で床を蹴って空中に飛び上がる。これより片脚支持期が終了すると共に空中期が開始する。尚、片脚支持期における遊脚は、図 5の 1番目の状態で示すように、片脚支持期の開始時では、支持脚の後側に存在するが、図 5の 2番目及び 3番目の状態で示すように次の着地予定位置に向かって支持脚の前側に振り出される。次いで、図 5の 4番目の状態で示す空中期を経た後、口ポット 1 は、遊脚（空中期の開始直前の片脚支持期で遊脚となっていた脚体 2 ) の足平 2 2のかかとで着地し、次の片脚支持期が開始される。

図 5の走行歩容を考慮しつつ、前記歩容生成装置 1 0 0が生成する目標歩容の基本的な概要を説明する。詳細は後述するが、歩容生成装置 1 0 0が目標歩容を生成するとき、遊脚側の足平 2 2の着地位置姿勢（着地予定位置姿勢）や着地時刻（着地予定時刻）等の目標歩容生成用の基本的な要求値（要求パラメ一夕）が、前記ジョイスティック 7 3の所要の操作等に応じて歩容生成装置 1 0 0に与えられる。そして、歩容生成装置 1 0 0は、その要求パラメ一夕を用いて目標歩容を生成する。より詳しく言えば、歩容生成装置 1 0 0は、上記要求パラメ一夕に応じて、目標歩容の目標足平位置姿勢軌道、目標床反力鉛直成分軌道等、目標歩容の一部の構成要素を規定するパラメ一夕（歩容パラメ一夕という）を決定した上で、その歩容パラメ一夕を用いて目標歩容の瞬時値を逐次決定し、該目標歩容の時系列パターンを生成する。

この場合、目標足平位置姿勢軌道（より詳しくは、足平の位置及び姿勢の空間的な各成分（X軸成分等）の目標軌道）は、例えば本出願人が特許第 3 2 3 3 4 5 0号にて提案した有限時間整定フィル夕を用いて各足平 2 2毎に生成される。この有限時間整定フィルタは、可変時定数の 1次遅れフィル夕、すなわち、伝達関数が 1 / ( 1 + て s ) の形で表されるフィル夕（ては可変の時定数。以下、この'フィル夕を単位フィル夕という）を複数段（本第 1参考例では 3段以上）、直列に接続したものであり、所望の指定時刻に指定値に到達するような軌道を生成 · 出力することができるものである。この場合、各段の単位フィル夕の時定数ては、いずれも、有限時間整定フィル夕の出力生成を開始してから、上記指定時刻までの残時間に応じて逐次可変的に設定される。より詳しくは, 該残時間が短くなるに伴いての値が所定の初期値 ( > 0 ) から減少されていき、最終的には、該残時間が 0になる指定時刻にて、ての値が 0になるように設定される。そして、有限時間整定フィルタには、前記指定値（より詳しくは、有限時間整定フィル夕の出力の初期値から前記指定値への変化量）に応じた高さのステップ入力が与えられる。このような有限時間整定フィルタは、指定時刻にて指定値に達するような出力が生成されるだけでなく、指定時刻における有限時間整定フィルタの出力の変化速度を 0もしくはほぼ 0にすることができる。特に、単位フィルタを 3段以上（ 3段でよい）、接続した場合には、有限時間整定フィル夕の出力の変化加速度（変化速度の微分値）をも 0もしくはほぼ 0にすることができる。

尚、上記のように有限時間整定フィル夕により生成される目標足平位置姿勢軌道は、床面に固定された後述の支持脚座標系での各足平 2 2の目標位置姿勢軌道である。

上述のように生成される目標足平位置姿勢軌道は、各足平 2 2の位置が、その初期接地状態（目標歩容の初期時刻の状態）から着地予定位置に向かって徐々に加速しながら移動を開始するように生成される。そして、該目標足平位置姿勢軌道は、最終的に着地予定時刻までに徐々に位置の変化速度を 0またはほぼ 0にまで減速し、着地予定時刻にて着地予定位置に到達して停止するように生成される。このため、各足平 2 2の着地瞬間における対地速度（床に固定された支持脚座標系での各足平 2 2の位置の変化速度）が 0またはほぼ 0になる。したがって、走行歩容において同時に全脚体 2 , 2が空中に存在する状態（空中期での状態）から着地しても、着地衝撃が小さくなる。

前記走行歩容においては、口ポット 1に作用する重力によって空中期後半から上体 3の鉛直速度は下向きになり、着地時でも下向きのままである。したがって、上記のように各足平 2 2.の着地瞬間における対地速度が 0またはほぼ 0になるように目標足平位置姿勢軌道を生成すると共に、後述する如く動力学的平衡条件を満たすように上体 3の目標位置姿勢軌道を生成したとき、着地直前において、上体 3に対する遊脚側の足平 2 2の相対速度は、上向きになる。すなわち、走行歩容の着地瞬間では、ロボット 1の目標歩容は遊脚側の脚体 2 2を上体 3側に引っ込めながら着地するような歩容となる。言い換えれば、本第 1参考例での目標歩容では、ロボット 1は、着地瞬間において、遊脚側の足平 2 2の対地速度が 0またはほぼ 0になるように、上体 3から見て該足平 2 2を引き上げるようにして着地する。これによつて、着地衝撃は小さくなり、着地衝撃が過大になるのを防止するようにしている _ώ

また、本第 1参考例では、有限時間整定フィルタは、単位フィル夕を 3段以上（例えば 3段）、直列に接続したものであるため、着地予定時刻までに各足平 2 2の速度（足平位置の変化速度）が 0またはほぼ 0になるだけでなく、各足平 2 2は、その加速度も着地予定時刻にて 0またはほぼ 0になって停止する。つまり、着地瞬間における対地加速度も 0 またはほぼ 0になる。したがって、着地衝撃がより一層小さくなる。特に、実際のロボット 1の着地時刻が目標の着地時刻からずれても、衝撃があまり増大しなくなる。補足すると、着地予定時刻にて各足平 2 2の対地速度を 0またはほぼ 0にする上では、有限時間整定フィルタの単位フィルタの段数は 2段でもよいが、この場合には、着地予定時刻での各足平 2 2の加速度は一般には 0にならない。

尚、足平姿勢に関しては、各足平 2 2が着地予定時刻にてその踵で着地した後、該足平 2 2の底面のほぼ全面が床に接地するまで動き続ける < このため、該足平 2 2の底面のほぼ全面が床に接地する時刻を前記指定時刻に設定して、前記有限時間整定フィル夕により足平姿勢軌道が生成される。

また、本第 1参考例では、有限時間整定フィルタを用いて足平位置軌道を生成したが、着地予定時刻での足平位置の変化速度（足平位置の時間微分値）が 0またはほぼ 0になるように、さらには、該着地予定時刻での足平位置の変化加速度（変化速度の時間微分値）が 0またはほぼ 0 になるように設定された多項式などの関数を用いて目標足平位置軌道を生成するようにしても良い。このことは、目標足平姿勢軌道の生成に関しても同様である。但し、該目標足平姿勢軌道の生成に関しては、上述の如く、各足平 2 2の底面のほぼ全面が床に設置する時刻にて、各足平 2 2の姿勢の変化速度、さらにはその変化加速度が 0またはほぼ 0になるように多項式などの関数が設定される。 .

目標床反力鉛直成分軌道は、例えば図 6のように設定される。本第 1 参考例では、走行歩容における目標床反力鉛直成分軌道の形状（詳しくは片脚支持期での形状）は、台形状（床反力鉛直成分の増加側に凸の形状）に定められており、その台形の高さ、折れ点の時刻を目標床反力鉛直成分軌道を規定する歩容パラメ一夕として、それらの歩容パラメータ (床反力鉛直成分軌道パラメータ）が決定される。尚、走行歩容の空中期では、目標床反力鉛直成分は定常的に 0に設定される。この例のように、目標床反力鉛直成分軌道は、実質的に連続になるように（値が不達続にならないように）設定するのが良い。これは床反力を制御する際のロボット 1の関節の動作を滑らかにするためである。尚、「実質的に連続」というのは、アナログ的に連続な軌道（真の意味での連続な軌道）を離散時間系でデジタル表現したときに必然的に生じる値の飛びは、該軌道の連続性を失わせるものではないということを意味するものである, 目標 Z M P軌道は次のように設定される。図 5の走行歩容においては. 前記したように支持脚側足平 2 2のかかとで着地し、次にその支持脚側足平 2 2のつま先で蹴って空中に飛び上がり、最後に遊脚側足平 2 2のかかとで着地する。したがって、片脚支持期での目標 Z M P軌道は、図 7の上段図に示すように、支持脚側足平 2 2のかかとを初期位置として，次に支持脚側足平 2 2の底面のほぼ全面が接地する期間において該足平 2 2の前後方向の中央に移動し、その後、離床時までに支持脚側足平 2 2のつまさきに移動するように設定される。ここで、図 7の上段図は、 X軸方向（前後方向）の目標 Z M P軌道を示すものであり、図 7の下段図は Y軸方向 (左右方向) の目標 Z M P軌道を示すものである。尚、片脚支持期における Y軸方向の目標 Z M P軌道は、図 7の下段図に示すように、 Y軸方向での支持脚側脚体 2の足首関節の中心位置と同じ位置に設定される。

走行歩容では、さらに片脚支持期の終了後、両脚体 2， 2が床から離れ、床反力鉛直成分が 0になる。床反力鉛直成分が 0の時、すなわち空中期には、ロボット 1の全体重心は自由落下運動をし、全体重心まわりの角運動量変化は零である。この時、床の任意の点において、口ポット 1に作用する重力と慣性力との合力のモーメントは 0であるので、目標 ZMPは不定である。すなわち、床のいずれの点も、「重力と慣性力との合力が作用するモーメン卜の水平成分が 0である作用点」という ZM Pの条件を満たす。言いかえれば、任意の点に目標 Z MPを設定しても上記合力が目標 Z MP回りに作用する.モ一メントの水平成分が 0であるという動力学的平衡条件を満足する。したがって、目標 ZMPを不連続に設定しても構わない。例えば、空中期では、目標 Z MPを、離床時 (片脚支持期の終了時）の目標 Z MP位置から移動しないように設定し. 空中期終端において、着地時の目標 ZMP位置に不連続（ステップ状）に移動するように該目標 Z MP軌道を設定しても構わない。しかし本第 1参考例では、図 7の上段図に示すように、空中期における目標 Z MP 軌道の X軸方向位置は、次の遊脚側脚体 2の着地までに支持脚側足平 2 2のつまさきから遊脚側足平 2 2のかかとの着地位置まで連続.的に移動するようにした。また、図 7の下段図に示すように、空中期における目標 ZMP軌道の Y軸方向位置は、次の遊脚側脚体 2の着地までに支持脚側脚体 2の足首関節の中心の Y軸方向位置から遊脚側脚体 2の足首関節の中心の Y軸方向位置まで連続的に移動するようにした。すなわち、歩容の全期間において目標 ZMP軌道を連続（実質的に連続）にした。そして、後述するように、目標 Z MPまわりの重力と慣性力との合力のモ一メント（鉛直成分を除く）が零になるように目標歩容を生成する（より具体的には目標上体位置姿勢軌道を調整する）ようにした。モデルの近似誤差を考慮すると、生成された歩容が滑らかになるようにするために、空中期においても目標 ZMP軌道を連続（実質的に連続）にしておくのが望ましい。但し、本第 1参考例で用いる後述の動力学モデルでは. 目標 Z M Pの位置によらずに、目標 Z M P回りのモ一メン卜の水平成分をある値（その値は本第 1参考例では 0であるが、後述の実施形態では必ずしも 0ではない）にするような目標歩容を一義的に生成できるので. 必ずしも目標 Z M Pを連続にしておく必要はない。

尚、本第 1参考例では、図 7に示したような目標 Z M P軌道の折れ点の位置や時刻が、 Z M P軌道パラメ一夕（目標 Z M P軌道を規定するパラメ一夕）として設定される。また、 .上記した Z M P軌道の「実質的に連続」の意味は、前記床反力鉛直成分軌道の場合と同様である。

Z M P軌道パラメ一夕は、安定余裕が高く、かつ急激な変化をしないように決定される。ここで、ロボット 1の接地面を含む最小の凸多角形 (いわゆる支持多角形）の中央付近に目標 Z M Pが存在する状態を安定余裕が高いと言う（詳細は特開平 1 0— 8 6 0 8 1号公報を参照）。図 7の目標 Z M P軌道はこのような条件を満たすように設定したものである。

また、目標上体位置姿勢、目標足平位置姿勢および後述の基準上体姿勢はグローバル座標系で記述される。グローバル座標系は床に固定された座標系である。グローバル座標系としては、より具体的には、後述する支持脚座標系が用いられる。

本第 1参考例では、歩容生成装置 1 0 0は、目標上体姿勢だけでなく , 基準上体姿勢も生成する。この基準上体姿勢は、歩容に対する要求（歩容生成装置 1 0 0に対する行動計画部などの装置または外部（前記ジョィスティック 7 3等）からの要求）にそのまま従って生成される上体姿勢である。

目標上体姿勢（以降、「基準」が付いていない場合には、目標上体姿勢を表す）は、長期的には基準上体姿勢に追従するかまたは一致するように生成される。 . 目標上体姿勢は、歩行においては、本願出願人による P C T公開公報 WO/02/40224 の明細書に記載した実施形態のごとく通常、常に基準上体姿勢に一致させておけば良い。 P C T公開公報 WOZ 02Z40224では、基準上体姿勢という概念は記載されていないが、目標上体姿勢パ夕ーンを、明示的かつ優先的に与えていたので、基準上体姿勢に目標上体姿勢が常に一致していることと同じことである。

しかし、走行など空中期がある歩容ゃ低摩擦床面での歩行においては, 単に上体水平加速度等を調整するだけでは、目標歩容の床反力水平成分と床反力モーメント鉛直成分とが許容範囲以内（あるいは摩擦限界以内）に存在するようにしつつ動力学的平衡条件を満足することができない。

そこで、本第 1参考例においては、目標上体姿勢を必要に応じて基準上体姿勢から故意にずらすこととした。より具体的には、以下に説明する運動モードを複合的に発生させることにより、目標歩容の床反力水平成分や床反力モーメント鉛直成分が許容範囲以内（あるいは摩擦限界以内）に存在しつつ動力学的平衡条件を満足させるようにした。

図 8のように、口ポット 1のある運動状態から、上体水平加速度だけを摂動（微小変化）させると、口ポット 1の全体重心水平加速度と全体重心まわりの角運動量が摂動する。すなわち、上体水平加速度の摂動は、それによつて発生するロポット 1の慣性力と重力との合力に対して動力学的に釣り合う床反力鉛直成分を摂動させずに（口ポット 1の全体重心鉛直加速度を摂動させずに）、目標 Z M Pまわりの床反力モーメント水平成分（水平軸まわり成分）と床反力水平成分とを摂動させる。このように口ポット 1の上体水平加速度を摂動させる運動モードを上体並進モ一ドと呼ぶ。言いかえると、床反力鉛直成分を変化させずに、目標 Z M Pまわりの床反カモ一メント水平成分と床反力水平成分とを変化させる運動を上体並進モードと呼ぶ。尚、上体並進モードでは、床反力モーメント鉛直成分（鉛直軸まわり成分）も摂動するが、ここではこの点に注目しない。

この時の単位加速度当たりの床反力モ一メント水平成分の変化を Δ Μ P、単位加速度当たりの床反力水平成分の変化を A Fp とする。図 8に示す状況で上体 3を前方に水平加速すると、 Δ Μ ρと A Fp は図 8に示す矢印の向きに作用する。

感覚的に判り易くするために、運動によって発生する慣性力と重力との合力に釣り合う床反力を用いて表現したが、慣性力と重力との合力を用いて表現した方が、理論的には的確である。なお、上記合力と床反力とは、互いに大きさが同じで向きが逆になつている。

それに対し、図 9のように、口ポット 1のある運動状態から、ある点 P rまわりに上体傾斜角加速度（上体 3の傾斜角の角加速度）を摂動させると、口ポット 1の全体重心は摂動せずに、全体重心まわりの角運動量（鉛直軸まわり成分を除く）が摂動する。すなわち、点 P rまわりの上体傾斜角加速度の摂動は、床反力鉛直成分と床反力水平成分とを摂動させずに、目標 Z M Pまわりの床反力モーメント水平成分を摂動させる。このように口ポット 1の上体傾斜角加速度を摂動させる運動モードを上体傾斜モードと呼ぶ。

言いかえると、床反力鉛直成分と床反力水平成分を変化させずに、目標 Z M Pまわりの床反力モーメント水平成分を変化させる運動を上体傾斜モードと呼ぶ。

この時の単位角加速度当たりの床反カモ一メント水平成分の変化を△ M r、単位角加速度当たりの床反力水平成分の変化を r とする。 Δ F rは零である。図 9に示す状況で上体 3が前傾するように上体傾斜角の角加速度を発生させると、 Δ Μ Γは図 9に示す矢印の向きに作用する _c また、図 1 0のように、口ポット 1のある運動状態から、ある点 Pq まわりに上体ョ一角加速度（上体 3の鉛直軸まわりの回転角加速度）を摂動させると、口ポット 1の全体重心は摂動せずに、全体重心まわりの角運動量鉛直成分が摂動する。ところで、ロボット 1の全体重心が摂動しなければ、角運動量鉛直成分の摂動は、作用点に依存しない。したがつて、点 Pq まわりの上体ョ一角加速度の摂動は、床反力鉛直成分、床反力水平成分および床反カモ一メン卜水平成分を摂動させずに、目標 Z M Pまわりの床反力モーメント鉛直成分を摂動させる。このようにロボット 1の上体ョ一角加速度を摂動させる運動モ一ドを上体ョ一回転モードと呼ぶ。

言いかえると、床反力鉛直成分、床反力水平成分および床反力モーメント水平成分を変化させずに、目標 Z M Pまわりの床反力モーメント鉛直成分を変化させる上体運動を上体ョ一回転モードと呼ぶ。

この時の単位角加速度当たりの床反カモ一メント鉛直成分の変化を Δ Mbz、単位角加速度当たりの ^反力水平成分の変化を A Fb とする。 Δ Fb は零である。図 1 0に示す状況で上体 3を矢印の向きに単位角加速度で回転させる（角加速度 /3 b=l で回転させる）と、 A Mbz は図 1 0 に示す矢印の向きに作用する。

尚、図 1 0に示す運動では、両腕体 5， 5の手先位置が支持脚座標系 (床に固定された座標系）から見て変わらないように、上体 3を回転させたが、上体 3に対する腕体 5の相対位置姿勢を変えずに、上体 3と共に腕体 5を回転させる運動を上体ョ一回転モードと定義しても良い。ただし、この場合には、後述する運動方程式に若干変更が必要である。

また、図 1 1 ( a )，（ b ) のように、口ポット .1のある運動状態から、両腕体 5， 5の先端を互いに前後に逆方向に摂動させると、口ポット 1 の全体重心は摂動せずに、全体重心まわりの角運動量鉛直成分が摂動する。以降、この運動モードを逆位相腕振りモードと呼ぶ。言い換えると床反力鉛直成分、床反力水平成分および床反カモ一メント水平成分を摂動させずに、目標 Z MPまわりの床反力モーメント鉛直成分を摂動させる腕振り運動モードを逆位相腕振りモ一ドと呼ぶ。

右腕体 5 Rを前側に単位量移動させ、左腕体 5 Lを後ろに単位量移動させる動作を、単位角度の逆位相腕振りと呼ぶ。図 1 1 ( a)，（b) は. 逆位相腕振り角度が Θ azの状態を表す。

逆位相腕振りモ一ドの単位角加速度当たりの床反力モーメント鉛直成分の変化を AMaz、単位角加速度当たりの床反力水平成分の変化を厶 Fa とする。 AFaは零である。図 1 1 ( a), (b) に示す状況で右腕体 5 Rを前側に加速し、左腕体 5 Lを後ろ側に加速する（角加速度 /3 a> 0 で振る）と、図 1 1 ( a) に示す矢印の向き（鉛直軸の正の向き）に床反力モ一メント鉛直成分 Mazが作用する。

· 次に、本第 1参考例に用いられる口ポット 1の動力学モデルについて説明する。本第 1参考例では単純化された（近似された）以下に示す動力学モデルを用いる。ただし、以下に示す動力学モデルに関しては、キネマテイクスモデル（関節、リンクの構造や寸法を表すモデルであり、言い換えると、関節変位とリンクの位置姿勢との関係を表すモデルである。）も必要である。

図 1 2は、本第 1参考例に用いられるロポット 1の動力学モデルである。図示の如く、この動力学モデルは、口ポット 1の各脚体 2にそれぞれ対応する 2つの質点 2 m， 2m、及び上体 3に対応する質点 3 mからなる合計 3質点と、イナーシャがあって質量のない 4つのフライホイ一ル F Hx、 FHy、 F Hbz、 F Haz とから構成されるモデルである。フライホイール F Hx、 F Hy、 F Hbz、 F Haz は、それぞれ X軸（前後方向軸）、 Y軸（左右方向軸）、 Ζ軸（鉛直軸）、 Ζ軸（鉛直軸）の回りに回転可能なものである。この動力学モデルでは、デカップルド、即ち. 脚体 2 , 2の動力学（各質点 2 m, 2 mの動力学）と上体 3の動力学 (質点 3 m及びフライホイール F Hx、 FHy および FHbz の動力学）と腕体 5， 5の動力学（フライホイ一ル F Haz の動力学）とが相互に非干渉に構成されると共に、口ポット 1全体の動力学は、それらの線形結合で表される。また、さらに、上体 3の運動と床反力との関係も、上体 3の並進運動（上体並進モード）と床反力との関係、上体 3の傾斜運動（上体傾斜モード）と床反力との関係、上体 3のョ一回転運動（上体ョ一回転モード）と床反力との関係、並びに両腕体 5， 5の逆位相腕振り運動（逆位相腕振りモード）と床反力との関係に分離される。具体的には、上体質点 3 mの水平運動によって発生する床反力は、上体 3の水平方向並進運動（上体並進モード）によって発生する床反力に対応し、フライホイール FHx および FHy の回転運動によって発生する床反力は、上体 3の傾斜角の回転運動（上体傾斜モード）によって発生する床反力に対応する。なお、フライホイール F Hx の回転運動は、上体 3の傾斜角のロール方向（X軸回り）の回転運動に対応し、フライホイール FHy の回転運動は、上体 3の傾斜角のピッチ方向（Y軸回り）の回転運動に対応している。また、フライホイール F Hbz の回転運動によつて発生する床反力は、上体 3のョ一回転運動（上体ョ一回転モード）によって発生する床反力に対応する。また、フライホイール F Haz の回転運動によって発生する床反力は、逆位相腕振り（逆位相腕振りモード）によって発生する床反力に対応する。

尚、口ポット 1の腕体の質量は上体 3に含まれるものとし、上体質点 3 mは腕体 5， 5の質量を含む質量をもつ。

説明の便宜上、動力学モデルに関する変数およびパラメ一夕を以下のように定義する。なお、各質点 2 m, 2 m， 3 mはそれが対応する部位の代表点であるか、あるいはその部位の位置姿勢から幾何学的に一義的に決定される点に相当する。例えば、支持脚側脚体 2の質点 2 mの位置は、該脚体 2の足平 2 2の底面の前記代表点から所定の距離だけ上方の点とする。

Z sup：支持脚質点鉛直位置

Z swg：遊脚質点鉛直位置

Z b：上体質点鉛直位置（上体鉛直位置とは一般的には異なる。）

Z Gtotal：全体重心鉛直位置

Xsup：支持脚質点 X位置

Ysup：支持脚質点 Y位置

Xswg : 遊脚質点 X位置

Yswg：遊脚質点 Y位置

Xb : 上体質点 X位置（上体質点位置は、前記点 P rから上体の前後方向にある所定の距離だけオフセットした点である。オフセッ卜は、直立時などにおいて、厳密モデルの重心位置と、本動力学モデルの重心位置がなるベく一致するように、決定される。上体水平位置とは一般的には異なる。）

Yb：上体質点 Y位置

XGtotal：全体重心水平 X位置

Y Gtotal：全体重心水平 Y位置

9 bx：鉛直方向に対する X軸回りの上体傾斜角

6 hy：鉛直方向に対する Y軸回りの上体傾斜角

Θ bz：上体ョ一回転角

Θ az：逆位相腕振り角 .

mb：上体質点質量 msup：支持脚質点質量

mswg：遊脚質点質量

mtotal：口ポット総質量（= mtotal + msup + mswg)

J ：上体慣性モーメント（上体傾斜モードにおける等価慣性モーメントである。すなわち、 FHxおよび； FHy の慣性モーメントである。実ロボット 1の上体 3部位の慣性モ一メントとは一般的には一致しない。） J bz：鉛直軸まわり上体慣性モーメント（上体ョ一回転モードにおける等価慣性モ一メントである。実口ポット 1の上体 3部位の慣性モーメントとは一般的には一致しない。）

J az：腕振り鉛直軸まわり慣性モーメント（スピンキャンセルのための逆位相腕振りにおける等価慣性モーメントである。すなわち、 FHz の慣性モーメントである。）

Fx：床反力 X成分 (詳しくは並進床反力の前後方向 ( X軸）成分) Fy : 床反力 Y成分（詳しくは並進床反力の左右方向（Y軸）成分） Fz : 床反力鉛直成分（詳しくは並進床反力の鉛直方向（ Z軸）成分。本第 1参考例では、これは目標並進床反力鉛直成分に等しい）

Mx：目標 Z M Pまわりの床反力モーメント X成分 (詳しくは床反カモ一メントの前後軸（X軸）まわり成分)

My : 目標 Z M Pまわりの床反力モーメント Y成分（詳しくは床反カモ一メントの左右軸（Y軸）まわり成分）

Mz：目標 Z M Pまわりの床反力モーメント Z成分（詳しくは床反カモーメン卜の鉛直軸（Z軸）まわり成分）

なお、各質点 2 m， 3 mの X位置、 Y位置はそれぞれ前後方向（X軸方向）の位置、左右方向（Y軸方向）の位置を意味している。また、本第 1参考例では、各脚体 2の質点 2 mの位置と、その脚体 2の足平 2 2 の位置（足平 2 2のあらかじめ定めた代表点の位置）との位置関係はあらかじめ定められており、一方の位置が決まれば、他方の位置は一義的に定まるものとされている。また、上体質点 3 mと、上体 3の位置（上体 3のあらかじめ定められた代表点の位置）との位置関係は、上体 3の姿勢角（以降、上体に関しては、姿勢角とは、傾斜角とョ一角を意味するものとする。）に応じてあらかじめ定められており、一方の位置と姿勢角とが決まれば、他方の位置は一義的に定まるものとされている。

また、任意の変数 Xに対して、 dX/dt は Xの 1階微分を表わし、 d2X/dt2 は Xの 2階微分を表わす。し.たがって、変数 Xが変位ならば、 dX/dt は速度、 d2X/dt2 は加速度を意味する。 gは重力加速度定数を示す。ここでは、 gは正の値とする。

上記動力学モデルの運動方程式（動力学的平衡条件を表す式）は、式 0 1、式 0 2 x、式 0 2 y、式 0 3 x、式 0 3 y、式 0 3 zで表される。 F z = mb * (g+d2Zb/dt2) + msup * (g+d2Zsup/dt2)

+ mswg * (g+d2Zswg/dt2) 式 0 1

F x= mb * d2Xb/dt2 + msup * d2Xsup/dt2 + mswg * d2Xswg/dt2

式 0 2 x

F y= mb * d2Yb/dt2 + msup * d2Ysup/dt2 + mswg * d2Yswg/dt2

……式 0 2 y

M x= mb * (Yb - Yzmp) * (g+d2Zb/dt2)

- mb * (Zb一 Zzmp) * (d2Yb/dt2)

+ msup * (Ysup一 Yzmp) * (g+d2Zsup/dt2)

- msup * (Zsup― Zzmp) * (d2Ysup/dt2)

+ mswg * (Yswg— Yzmp) * (g+d2Zswg/dt2) 一 mswg * (Zswg - Zzmp) * (d2Yswg/dt2) + J * d2 Θ bx/dt2

……式 0 3 x

M y= - mb * (Xb― Xzmp) * (g+d2Zb/dt2)

+ mb * (Zb一 Zzmp) ホ (d2Xb/dt2)

― msup * (Xsup一 Xzmp) * (g+d2Zsup/dt2)

+ msup * (Zsup - Zzmp) * (d2Xsup/dt2)

― mswg * (Xswg— Xzmp) * (g+d2Zswg/dt2)

+ mswg * (Zswg - Zzmp) * (d2Xswg/dt2) + J * d2 0 by/dt2

……式 0 3 y

M z = mb * (Xb - Xzmp) * (d2Yb/dt2) - mb * (Yb - Yzmp) (d2Xb/dt2)

+ msup * (Xsup― Xzmp) * (d2Ysup/dt2)

一 msup * (Ysup一 Yzmp) * (d2Xsup/dt2)

+ mswg * (Xswg - Xzmp) * (d2Yswg/dt2)

― mswg * (Yswg- Yzmp) * (d2Xswg/dt2)

+ Jbz * d2 0 bz/dt2 + Jaz*d2 Θ az/dt2

式 0 3 z また、ロポット全体重心位置には次の関係式が成立する。

Z Gtotal= (mb * Zb + msup * Zsup + mswg * Zswg) / mtotal

式 0 4

X Gtotal= (mb * Xb + msup * Xsup + mswg * Xswg)/ mtotal

式 0 5 x

Y Gtotal= (mb * Yb + msup * Ysup + mswg * Yswg)/ mtotal

式 0 5 y 以下に上記動力学モデルと前記 AFp、 ΔΜρ、 Α¥τ , ΔΜ Γの関係を示す。

前記 ΔΡρ は、式 0 2 Xまたは式 0 2 yにおいて、 d2Xb/dt2 または d2Yb/dt2を単位量だけ摂動させたときの Fxまたは Fyの摂動量であるから、次式によって求められる。

AFp = mb …式 0 6

すなわち、上体並進モードの各水平軸（X軸、 Y軸）方向の単位加速度当たりの床反力水平成分の変化 AFp は、上記動力学モデルの上体質点 3mの質量に相当する。

前記 ΔΜρ は、式 0 3 yまたは式 0 3 Xにおいて、 d2Xb/dt2 または d2Yb/dt2 を単位量だけ摂動させたときの My または Mxの摂動量であるから、次式によって求められる。

AMp = mb * (Zb一 Zzmp) …式 0 7

すなわち、上体並進モードの各水平軸（X軸、 Y軸）方向の単位加速度当たりの床反力モ一メント水平成分の変化 Δ Mpは、上記動力学モデルの上体質点質量に、上体質点 3 mの目標 Z MPからの高さ（鉛直位置）を乗じたものになる。このときの上体質点 3 m及び目標 Z M Pの位置と，上体質点 3 mの運動との関係は、上体質点 3 mを倒立振子質点、目標 Z MPを倒立振子支点に対応づけたときの倒立振子の挙動に相当する。なお、 Y軸方向の ΔΜρ は、より正確には、式 0 7の右辺の符号を反転させたものである。

前記 AFrは、式 0 2 Xまたは式 0 2 yにおいて、 d20by/dt2 を単位量だけ摂動させたときの Fx または Fy の摂動量であるから、次式によつて求められる。

AFr =0 …式 0 8 すなわち、上体傾斜モードの各水平軸（X軸、 Y軸）方向の単位加速度当たりの床反力水平成分の変化 A Frは、 0である。

前記 Δ M rは、式 0 3 Xまたは式 0 3 yにおいて、 d2 0 bx/dt2または d2 Θ by/dt2を単位量だけ摂動させたときの Mxまたは Myの摂動量であるから、次式によって求められる。

Δ Μ r = J …式 0 9

すなわち、上体傾斜モードの各水平軸（X軸、 Y軸）方向の単位加速度当たりの床反力モーメント水平成分の.変化 Δ Mr は、水平軸フライホイール（FHxおよび Fhy) の慣性モーメントに相当する。

前記 A Mbz は、式 0 3 z において、 d2 0 bz/dt2 を単位量だけ摂動させたときの Mzの摂動量であるから、次式によって求められる。

A Mbz= J bz …式 0 9 b

すなわち、上体ョ一回転モ一ドの単位加速度当たりの床反力モ一メント成分の変化 A Mbz は、上体ョ一回転に対応するフライホイール F Hbz の慣性モーメントに相当する。

前記 A Maz は、式 0 3 z において、 d2 Θ az/dt2 を単位量だけ摂動させたときの Mzの摂動量であるから、次式によって求められる。

A Maz= J az …式 0 9 a

すなわち、逆位相腕振りの単位角加速度当たりの床反力モーメント成分の変化 A Maz は、腕振りに対応するフライホイール FHaz の慣性モーメントに相当する。

本第 1参考例における歩容生成装置 1 0 0は、口ポット 1の片方の脚体 2が着地してから他方の脚体 2が着地するまでの 1歩分の目標歩容 (前記狭義の意味での目標歩容）を単位として、その 1歩分の目標歩容を順番に生成する。従って、本第 1参考例で生成する図 5の走行歩容では、該目標歩容は片脚支持期の開始時から、これに続く空中期の終了時 (次の片脚支持期の開始時）までの目標歩容が順番に生成される。ここで、新たに生成しょうとしている目標歩容を「今回歩容」、その次の目標歩容を「次回歩容」、さらにその次の目標歩容を「次次回歩容」、というように呼ぶ。また、「今回歩容」の一つ前に生成した目標歩容を「前回歩容」と呼ぶ。

また、歩容生成装置 1 0 0が今回歩容を新たに生成するとき、該歩容生成装置 1 0 0には、ロボット 1の 2歩先までの遊脚側足平 2 2の着地予定位置姿勢、着地予定時刻の要求値（要求）が歩容に対する要求パラメータとして入力される（あるいは歩容生成装置 1 0 0が記憶装置から要求パラメ一夕を読み込む）。そして、歩容生成装置 1 0 0は、これらの要求パラメ一夕を用いて、目標上体位置姿勢軌道、目標足平位置姿勢軌道、目標 Z M P軌道、目標床反力鉛直成分軌道、目標腕姿勢軌道等を生成する。このとき、これらの軌道を規定する歩容パラメ一夕の一部は. 歩行の継続性を確保するように適宜修正される。

以下に図 5の走行歩容を生成することを例にして、本第 1参考例における歩容生成装置 1 0 0の歩容生成処理の詳細を図 1 3〜図 4 6を参照しつつ説明する。図 1 3は、その歩容生成装置 1 0 0が実行する歩容生成処理のメインルーチンを示すフローチャート（構造化フ口一チヤ一ト）である。

まず S 0 1 0において時刻 tを 0に初期化するなど種々の初期化作業が行なわれる。この処理は、歩容生成装置 1 0 0の起動時等に行なわれる。次いで、 S O 1 2を経て S O 1 4に進み、歩容生成装置 1 0 0は、制御周期（図 1 3のフローチヤ一卜の演算処理周期）毎の夕イマ割り込みを待つ。制御周期は A tである。

次いで、 S 0 1 6に進み、歩容の切り替わり目であるか否かが判断され、歩容の切り替わり目であるときは S 0 1 8に進むと共に、切り替わり目でないときは S O 3 0に進む。ここで、上記「歩容の切り替わり目」は、前回歩容の生成が完了し、今回歩容の生成を開始するタイミングを意味し、例えば前回歩容の生成を完了した制御周期の次の制御周期が歩容の切り替わり目になる。

S 0 1 8に進むときは時刻 tが 0に初期化され、次いで S 0 2 0に進み、次回歩容支持脚座標系、次次回歩容支持脚座標系、今回歩容周期および次回歩容周期が読み込まれる。これらの支持脚座標系及び歩容周期は、前記要求パラメ一夕により定まる.ものである。すなわち、本第 1参考例では、歩容生成装置 1 0 0にジョイスティック 7 3等から与えらる要求パラメ一夕は、 2歩先までの遊脚側足平 2 2の着地予定位置姿勢 (足平 2 2が着地してから足底を床面にほぼ全面的に接触させるように. 滑らさずに回転させた状態での足平位置姿勢）、着地予定時刻の要求値を含んでおり、その 1歩目の要求値、 2歩目の要求値がそれぞれ、今回歩容、次回歩容に対応するものとして、今回歩容の生成開始時 (前記 S 0 1 6の歩容の切り替わり目）以前に歩容生成装置 1 0 0に与えられたものである。なお、これらの要求値は今回歩容の生成途中でも変更することは可能である。

そして、上記要求パラメ一タにおける 1歩目の遊脚側足平 2 2 (今回歩容での遊脚側足平 2 2 ) の着地予定位置姿勢の要求値に対応して次回歩容支持脚座標系が定まる。

例えば図 1 6を参照して、今回歩容（ 1歩目）に係わる遊脚側足平 2 2 (図では 2 2 L ) の着地予定位置姿勢の要求値が、今回歩容の支持脚側足平 2 2 (図では 2 2 R ) の着地位置姿勢に対して、今回歩容支持脚座標系の X軸方向（今回歩容の支持脚側足平 2 2 Rの前後方向）及び Y 軸方向（今回歩容の支持脚側足平 2 2 Rの左右方向）に、それぞれ xnext、 ynext だけ移動し、且つ Z軸回り（鉛直軸回り）に Θ znext だけ回転させた位置姿勢であるとする。ここで、支持脚座標系は、支持脚側足平 2 2を水平姿勢（より一般的には床面に平行な姿勢）にして該支持脚側足平 2 2の底面のほぼ全面を床面に接触（密着）させた状態において、該支持脚側脚体 2の足首中心から床面に延ばした垂線が該床面と交わる点（この点は、本第 1参考例の例では支持脚側足平 2 2の底面のほぼ全面を床面に接触させた状態では、該足平 2 2の代表点と合致する）を原点とし、その原点を通る水平面を X Y平面とするグローバル座標系（床に固定された座標系）である。この場合、 X軸方向、 Y軸方向は、それぞれ支持脚側足平 2 2の前後方向、左右方向である。尚、支持脚座標系の原点は、必ずしも支持脚側足平 2 2の底面のほぼ全面を床面に接触させた状態での該足平 2 2の代表点（足平 2 2の位置を代表する点）に合致する必要はなく、該代表点と異なる床面上の点に設定されてもよい。

このとさ、次回歩容支持脚座標系は、図示のように今回歩容の遊脚側足平 2 2 Lの着地予定位置姿勢の要求値に従って該足平 2 2を着地させた場合（足平 2 2の代表点を着地予定位置の要求値に一致させ、且つ、足平 2 2の姿勢（向き）を着地予定姿勢の要求値に一致させた場合）における該足平 2 2 Lの代表点（より詳しくは該代表点に合致する床上の点）を原点とし、該原点を通る水平面内における該足平 2 2 Lの前後方向、左右方向をそれぞれ X ' 軸方向、 Y ' 軸方向とする座標系である。上記と同様に、 2歩目の遊脚側足平 2 2の着地予定位置姿勢の要求値に応じて次々回歩容支持脚座標系（図 1 6の X " Y " 座標を参照）が定まる。また、今回歩容周期は、今回歩容の支持脚側足平 2 2の着地予定時刻（要求値）から、 1歩目（今回歩容）の遊脚側足平 2 2の着地予定時刻（要求値）までの時間として定まり、次回歩容周期は、 1歩目の遊脚側足平 2 2の着地予定時刻（要求値）から 2歩目の遊脚側足平 2 2の着地予定時刻（要求値）までの時間として定まる。

尚、前記要求パラメータは、本第 1参考例ではジョイスティック 7 3 の所要の操作によって歩容生成装置 1 0 0に入力されるものであるが、あらかじめ該要求パラメータあるいはこれに対応する上記の支持脚座標系の位置姿勢及び歩容周期をロポット 1の移動スケジュールとして記憶しておいても良い。あるいは、ジョイスティック 7 3などの操縦装置からの指令（要求）とそのときまでの口ポットの 1の移動履歴とを基に前記次回及び次次回歩容支持脚座標系、 .並びに今回及び次回歩容周期を決定しても良い。

次いで S 0 2 2に進み、歩容生成装置 1 0 0は、今回歩容に続く仮想的な周期的歩容としての定常旋回歩容の歩容パラメータを決定する。該歩容パラメ一夕は、定常旋回歩容における目標足平位置姿勢軌道を規定する足平軌道パラメータ、基準とする上体姿勢軌道を規定する基準上体姿勢軌道パラメータ、基準とする腕姿勢軌道を規定する基準腕姿勢軌道パラメ一夕、目標 Z M P軌道を規定する Z M P軌道パラメ一夕、目標床反力鉛直成分軌道を規定する床反力鉛直成分軌道パラメ一夕を含む。さらに、床反力水平成分許容範囲および床反力モーメント鉛直成分許容範囲を規定するパラメータも歩容パラメ一夕に含まれる。

尚、この明細書で「定常旋回歩容」は、その歩容を繰り返したときに歩容の境界（本第 1参考例では 1歩毎の歩容の境界）においてロボット 1の運動状態（足平位置姿勢、上体位置姿勢等の状態）に不連続が生じないような周期的歩容を意味するものとして使用する。以降、「定常旋回歩容」を「定常歩容」と略す場合もある。

周期的歩容である定常旋回歩容は、本第 1参考例では、口ポット 1の 2歩分の歩容、すなわち今回歩容に続く第 1旋回歩容と該第 1旋回歩容に続く第 2旋回歩容とからなる歩容を該定常旋回歩容の 1周期分の歩容として、その 1周期分の歩容を繰り返す歩容である。尚、ここで「旋回」なる用語を用いたのは、旋回率を零とするときは直進を意味するので、直進も広義の意味で旋回に含ませることができるからである。また生成する目標歩容が前記図 5の走行歩容であると、その目標歩容の今回歩容は、片脚支持期及び空中期を有する走行歩容であるので、定常旋回歩容の第 1旋回歩容及び第 2旋回歩容は、いずれも今回歩容と同様に片脚支持期と空中期とを有する歩容である。つまり、第 1旋回歩容及び第 2旋回歩容の基本的な歩容形態は今回歩容と同一である。

定常旋回歩容について補足すると、 2足移動口ポットでは、定常旋回歩容の 1周期分は、前記した狭義の意味での歩容が、少なくとも 2歩分. 必要である。そして、 3歩以上の歩容を 1周期分の歩容とする複雑な定常旋回歩容を設定することも可能である。但し、定常旋回歩容は、後述の如く、今回歩容の終端 (終了時刻) における発散成分 (詳細は後述する）を決定するためだけに用いられる。このため、 3歩以上の歩容を 1 周期とする定常旋回歩容を用いることは、歩容生成の処理が煩雑となるにも関わらず、効果は少ない。そこで、本第 1参考例での定常旋回歩容の 1周期分の歩容を 2歩分の歩容（第 1及び第 2旋回歩容）により構成するようにしている。尚、 3足以上の脚式移動口ポットにあっては、それに応じて定常旋回歩容を定義するに足る歩容数が増加する。以下では, 説明の便宜上、複数の狭義の歩容（本第 1参考例では 2歩分の歩容）から成る定常旋回歩容を 1歩の歩容とみなす。

定常旋回歩容は、歩容生成装置 1 0 0で今回歩容の終端における発散成分や上体鉛直位置速度、上体姿勢角及びその角速度等のロポット 1の運動状態を決定するために暫定的に作成されるものであり、歩容生成装置 1 0 0からそのまま出力されるものではない。 .

尚、「発散」とは、図 1 4に示すように、 2足移動口ポット 1 の上体 3の位置が両足平 2 2， 2 2の位置からかけ離れた位置にずれてしまうことを意味する。発散成分の値とは、 2足移動ロボット 1の上体 3の位置が両足平 2 2， 2 2の位置（より具体的には、支持脚側足平 2 2の接地面に設定されたグローバル座標系（支持脚座標系）の原点）からかけ離れていく具合を表す数値である。

本第 1参考例では、目標歩容が、前記発散を生じることなく、継続的に生成されるように、発散成分を指標にして歩容を生成するようにした, ただし、継続的な歩容の代表例である定常歩容（歩容の軌道の不連続を発生することなく、同じパターンの歩容を繰り返すことができる周期的歩容であり、原理上、無限回繰り返しても発散しない歩容）の初期発散成分（定常歩容の初期時刻での発散成分）であっても、単純に 0ではなく、定常歩容のパラメ一夕が変われば、その初期発散成分も変わる。すなわち、歩き方あるいは走り方等の歩容形態によって、適切な発散成分が変わる。そこで、本第 1参考例では、これから生成しょうとする今回歩容の後に続く定常歩容が前記今回歩容に係わる要求パラメータに応じて設定され、定常歩容の初期発散成分を求めてから、今回歩容の終端発散成分を定常歩容の初期発散成分に一致させる（より一般的には、今回歩容を定常歩容に連続させもしくは近づける）ように、今回歩容を生成する。これのような歩容生成の基本的な指針は本出願人が先に提案した P C T公開公報 WOZ 02/40224のものと同様である。

本発明の第 1参考例では、 P C T公開公報 WOZ02 40224の第 1実施形態で用いたような線形な 3質点動力学モデルを用いていないが、次式で定義される発散成分および収束成分の概念は、図 1 2に示したモデルのような非線形な動力学モデルの挙動の摂動分に対しても、近似的に十分な精度で適用することができる。発散成分-上体質点水平位置 +上体質点水平速度/ ω θ

…式 1 0 収束成分 =上体質点水平位置一上体質点水平速度/ ω θ '

…式 1 1 ただし、ここでの上体質点水平位置は、図 1 2に示す前記動力学モデルにおいては、上体質点水平位置 Xbを表す。

また、 ω 0、 ω θ ' はある所定の値である。これらの ω 0、 ω 0 ' 値はほぼ同じ値であるが、厳密には一致レない。そして、走行時では、 P C T公開公報 WOZ02Z40224の歩行歩容生成時の値に対して若干変更する必要がある。

発散成分および収束成分に関するその他の詳細は、 P C T公開公報 WO/02/ 40224 に記述されているので、ここではこれ以上言及しない, 本第 1参考例においては、 P C T公開公報 WOZ02Z40224に示した方式に加え、さらに、後述するように、目標床反力鉛直成分軌道を規定 · する歩容パラメ一夕を設定し、この目標床反力鉛直成分を動力学的に満足するように口ポット 1の全体重心鉛直位置を決定する。この場合、床反力鉛直成分の 2階積分値がロボット 1の全体重心鉛直位置を規定するものになるので、目標床反力鉛直成分の設定が悪いと、口ポット 1の全体重心鉛直位置もしくは上体鉛直位置が高くなり過ぎたり低くなり過ぎたりする。したがって、目標床反力鉛直成分の設定方法も重要な課題である。しかし、床反力鉛直成分と上体鉛直位置との関係は、 Z M Pと上体水平位置との関係に似ているので、上体水平位置速度を適切にするための目標 Z M P決定手法の一部を、以下の本第 1参考例に示すように、少し変更するだけで、上体鉛直位置速度を適切にするための目標床反力鉛直成分の決定手法に適用することができる。 .

本題に戻り、 S 0 2 2では、図 1 5に示すフローチャートに従って、以下の処理が行われる。

まず、 S 1 0 0において、今回歩容、第 1旋回歩容、第 2旋回歩容の順に足平位置姿勢軌道がつながるように、定常歩容の歩容パラメ一夕のうちの足平軌道パラメータが決定される。以下に具体的な設定方法を図 1 6を参照して説明する。尚、以降の説明では、支持脚側の脚体 2の足平 2 2を支持脚足平と称し、遊脚側の脚体 2の足平 2 2を遊脚足平を称する。また、歩容の「初期」、「終端」はそれぞれ歩容の開始時刻、終了時刻もしくはそれらの時刻における瞬時歩容を意味する。

足平軌道パラメータは、第 1旋回歩容および第 2旋回歩容の初期及び終端のそれぞれにおける支持脚足平及び遊脚足平のそれぞれの位置姿勢. 各旋回歩容の歩容周期等から構成される。この足平軌道パラメ一夕のうち、第 1旋回歩容初期遊脚足平位置姿勢は、.次回歩容支持脚座標系から見た今回歩容終端支持脚足平位置姿勢とされる。この場合、走行歩容では、今回歩容終端における支持脚足平 2 2は、空中に移動している。そして、今回歩容終端支持脚足平位置姿勢は、 .今回歩容初期支持脚足平位置姿勢（=前回歩容終端遊脚足平位置姿勢）から、前記要求パラメータにおける 2歩目の遊脚側足平 2 2の着地予定位置姿勢の要求値（今回歩容の支持脚足平 2 2の次回歩容における着地予定位置姿勢の要求値）もしくは該要求値に対応する次次回歩容支持脚座標系に応じて定まる次回歩容終端遊脚足平位置姿勢に至る足平位置姿勢軌道（詳しくは次回歩容支持脚座標系から見た軌道）を、今回歩容終端まで前記有限時間整定フィルタを用いて生成することにより求められる。

尚、次回歩容終端遊脚足平位置姿勢は、その位置姿勢から足平 2 2を接地させたまま、つま先を下げるように該足平 2 2を水平姿勢までピッチ方向に所定角度回転させたときの該足平の位置姿勢が次次回歩容支持脚座標系の位置姿勢に合致するように決定される。換言すれば、次回歩容終端遊脚足平位置姿勢は、前記要求パラメ一夕における 2歩目の遊脚側足平 2 2の着地位置姿勢の要求値から、該足平 2 2をすベらないように接地させたまま、つま先を持ち上げるように該足平 2 2をピッチ方向に所定角度回転させた状態（つま先を上げてかかとを着地させた状態）での該足平 2 2の位置姿勢である。

また、第 1旋回歩容初期支持脚足平位置姿勢は、次回歩容支持脚座標系から見た今回歩容終端遊脚足平位置姿勢とされる。この場合、今回歩容終端遊脚足平位置姿勢は、前記次回歩容終端遊脚足平位置姿勢の場合と同様、前記次回歩容支持脚座標系もしくはこれに対応する前記要求パラメータの 1歩目（今回歩容）の遊脚着地予定位置姿勢の要求値に応じて定まるものである。すなわち、該今回歩容終端遊脚足平位置姿勢は、その位置姿勢から、足平 2 2を接地させたまま、つま先を下げるように該足平 2 2を回転させて該足平 2 2の底面のほぼ全面を床面に接地させたときの該足平の代表点が次回歩容支持脚座標系の原点に合致するように決定される。

第 1旋回歩容終端遊脚足平位置姿勢は、今回歩容終端遊脚足平位置姿勢や次回歩容終端遊脚足平位置姿勢の決定手法と同様に、次回歩容支持脚座標系から見た次次回歩容支持脚座標系の位置姿勢に基づいて決定される。より具体的には、第 1旋回歩容終端遊脚足平位置姿勢は、その位置姿勢から足平 2 2を接地させたまま、すべらないように該足平 2 2を水平姿勢まで所定角度回転させたときの足平位置姿勢が、次回歩容支持脚座標系から見た次次回歩容支持脚座標系の位置姿勢に合致するように設定される。

第 1旋回歩容終端では、支持脚足平 2 2は離床して空中にある。支持脚足平 2 2が離床してから後の軌道を決定するために、第 1旋回歩容支持脚足平着地予定位置姿勢が設定される。第 1旋回歩容支持脚足平着地予定位置姿勢は、次回歩容支持脚座標系から見た次次次回歩容支持脚座標系の位置姿勢に基づいて設定される。より具体的には、第 1旋回歩容支持脚足平着地予定位置姿勢は、次回歩容支持脚座標系から見た次次次回歩容支持脚座標系の位置姿勢である。なお、次次次回歩容支持脚座標系は、次次回歩容支持脚座標系と次次次回歩容支持脚座標系との相対的位置姿勢関係が、今回歩容支持脚座標系と次回歩容支持脚座標系との相対的位置姿勢関係と一致するように設定される。

第 1旋回歩容終端支持脚足平位置姿勢は、第 1旋回歩容初期支持脚足平位置姿勢を求めた場合と同様に、第 1旋回歩容初期支持脚足平位置姿勢から、前記第 1旋回歩容支持脚足平着地予定位置姿勢に至る足平位置姿勢軌道（より詳しくは次回歩容支持脚座標系から見た軌道）を、第 1 旋回歩容終端まで前記有限時間整定フィルタを用いて生成することにより求められる。

第 2旋回歩容初期遊脚足平位置姿勢は、次次回歩容支持脚座標系から見た第 1旋回歩容終端支持脚足平位置姿勢と.される。第 2旋回歩容初期支持脚足平位置姿勢は、次次回歩容支持脚座標系から見た第 1旋回歩容終端遊脚足平位置姿勢とされる。

第 2旋回歩容終端遊脚足平位置姿勢は、今回歩容支持脚座標系から見た今回歩容終端遊脚足平位置姿勢とされる。第 2旋回歩容終端支持脚足平位置姿勢は、今回歩容支持脚座標系から見た今回歩容終端支持脚足平位置姿勢とされる。

第 1旋回歩容および第 2旋回歩容の歩容周期は、次回歩容周期と同一に設定される。これらの第 1旋回歩容及び第 2旋回歩容の歩容周期は、互いに同一にすることは必ずしも必要ではないが、いずれの周期も、少なくとも次回歩容周期に応じて決定するのが好ましい。尚、今回歩容、第 1旋回歩容および第 2旋回歩容の上記以外の運動パラメ一夕（両脚支持期時間などの時間パラメ一夕を含む）は、上記決定されたパラメータに応じて、歩容の条件（ァクチユエ一夕の速度が許容範囲に入っているか、可動角を超えていないか、床などと干渉していないかなど）を満足するように適宜決定する。

次に、 S 1 0 2に進み、目標上体姿勢が追従すべき基準上体姿勢軌道を規定する基準上体姿勢軌道パラメータが決定される。基準上体姿勢は定常歩容の初期（第 1旋回歩容の初期）と終端（第 2旋回歩容の終端）とでつながるように（定常歩容の初期、終端での基準上体姿勢の姿勢角及びその角速度が一致するように）に設定されている限り、一定姿勢である必要はないが、本第 1参考例では理解を容易にするため、基準上体姿勢のうちの傾斜角（鉛直方向に対する傾斜角）に係わる姿勢は、直立姿勢（鉛直姿勢）に設定される。つまり、本第 1参考例では、上体 3の傾斜角に関する基準上体姿勢は、定常歩容の全期間において直立姿勢に設定される。従って、本第 1参考例では、基準上体姿勢のうちの傾斜角の角速度及び角加速度は 0である。基準上体姿勢のうちのョ一角軌道 (以降、基準ョ一角軌道とも呼ぶ） 0 bz は、例えば一定角速度（定常歩容の平均旋回速度）の運動にすれば良い。あるいは、後述の基準逆位相腕振り軌道の例（図 1 8 ) のようにサイン波状にしても良い。ただし. 定常歩容を繰り返した時に基準ョ一角およびその角速度が連続となるように設定する。

尚、本第 1参考例では、目標上体姿勢のうちのョ一角軌道（以降、目標ョ一角軌道とも呼ぶ）は、基準ョ一角軌道に一致させるものとする。次に、 S 1 0 4に進み、基準腕姿勢軌道パラメ一夕が決定される。具体的には、両腕体 5， 5の全体の重心位置（上体 3に対する相対的な重心位置）、左右の手先（両腕体 5， 5の先端）の左右方向の間隔、および逆位相腕振り角に関するパラメータが決定される。基準逆位相腕振り角は、たとえば図 1 7のように左旋回する場合には、図 1 8のように設定すれば良い。図 1 8のように、基準逆位相腕振り角 0 azi'ef は、定常歩容を繰り返した時に歩容の境界（第 2旋回歩容の終端と次の第 1旋回歩容の境界）において逆位相腕振り角および角速度が共に連続になり、かつ第 1旋回歩容初期における支持脚と逆位相腕振り角の相対関係が、次の第 1旋回歩容初期における支持脚と逆位相腕振り角の相対関係に一致するように設定される。すなわち、第 1旋回歩容の初期逆位相腕振り角速度と第 2旋回歩容の終端逆位相腕振り角速度が一致し、第 2旋回歩容の終端逆位相腕振り角は、定常歩容の旋回角（第 1旋回歩容と第 2旋回歩容の旋回角の和）に第 1旋回歩容初期逆位相腕振り角を加えた値に設定される。図 1 8では基準逆位相腕振り角 Q azref をサイン波状の波形としたが、一定角速度に設定しても良い。 .あるいは、支持脚のョ一角と遊脚のョ一角の平均の値としても良い。

尚、本第 1参考例では、目標腕姿勢の両腕体 5， 5の全体の重心位置 (上体 3に対する相対位置) は、上体 3に対,して一定に維持されるように設定される。

次に、 S 1 0 6に進み、床反力鉛直成分軌道パラメ一夕が設定される ( この場合、該パラメ一夕により規定される床反力鉛直成分軌道が、第 1 旋回歩容および第 2旋回歩容のいずれにおいても図 6のように実質的に連続な（値がステップ状に飛ばない）ものとなるように床反力鉛直成分軌道パラメータが設定される。すなわち、定常旋回歩容の目標床反力鉛直成分軌道は図 1 9のようなパターンで設定される。そのパターンでは、第 1旋回歩容及び第 2旋回歩容のいずれにおいても、片脚支持期では床反力鉛直成分が台形状に変化し、空中期では床反力鉛直成分が 0に維持される。そして、このパターンの折れ点の時刻や、. 台形部分の高さ（ピーク値）が床反力鉛直成分軌道パラメ一夕として設定される。この床反力鉛直成分軌道パラメータの設定に際しては、床反力鉛直成分の歩容全期間（第 1旋回歩容と第 2旋回歩容両方の期間を合わせた期間で定常歩容の 1周期の期間）における平均値をロポット 1の自重と一致させる。すなわち、床反力鉛直成分の平均値が口ポット 1に作用する重力と同じ大きさで反対向きになるようにする。

上記のごとく床反力鉛直成分軌道を設定することは、定常歩容の条件を満たすために必要である。定常歩容の条件とは、支持脚座標系（支持脚側足平 2 2の接地面に設定された座標系）から見た歩容のあらゆる状態変数（口ポット 1の各部の位置、姿勢、速度等）の初期状態（第 1旋回歩容の初期状態）と次の支持脚座標系（次の第 1旋回歩容の支持脚座標系）から見た歩'容の終端状態（第 2旋回歩容の終端状態）がー致していることである（以下、この条件を定常歩容の境界条件ということがある）。したがって、定常歩容終端における口ポット 1の全体重心鉛直速度と定常歩容初期における全体重心鉛直速度との差（詳細には、第 2旋回歩容終端の全体重心鉛直速度と第 1旋回歩容初期の全体重心鉛直速度との差) も零でなければならない。上記差は床反力鉛直成分と重力との差の積分値（ 1階積分値）であるから、上記差を零にするためには、上記のごとく床反力鉛直成分軌道を設定する必要がある。

尚、本第 1参考例では、床反力鉛直成分の第 1旋回歩容、第 2旋回歩容それぞれの期間における平均値をロボット 1の自重と一致させるようにした。より具体的には、例えば第 1旋回歩容及び第 2旋回歩容の歩容周期に応じて各旋回歩容における床反力鉛直成分軌道の台形部分の折れ点の時刻を設定した上で、その台形部分の高さを、床反力鉛直成分の第 1旋回歩容、第 2旋回歩容それぞれの期間における平均値がロポット 1 の自重と一致するように決定した（台形の高さを未知数として、上記平均値及ぴ自重の一致条件を表す方程式を解くことで台形の高さを求める）。

こうすることにより、第 1旋回歩容終端の全体重心鉛直速度と第 1旋回歩容初期の全体重心鉛直速度との差も 0、第 2旋回歩容終端の全体重心鉛直速度と第 2旋回歩容初期の全体重心鉛直速度との差も 0になる。ただし、このようにする必然性はない。例えば、第 1旋回歩容と第 2旋回歩容との境界あたりで上体鉛直位置が高くなり過ぎたり低くなり過ぎたりして無理な姿勢になりそうな場合には、各旋回歩容で上記平均値及び自重を一致させた状態から各旋回歩容の床反力鉛直成分軌道の台形の高さ等を修正するようにしてもよい。

次に、 S 1 0 8に進み、上記のごとく図 1 9のように設定された床反力鉛直成分軌道に応じて、床反力水平成分の許容範囲 [Fxmin, Fxmax] (より詳しくはこれを規定するパラメ一夕）.が図 2 0のように設定される。図 2 0の負側の折れ線が床反力水平成分許容下限値 Fxmin、正側の折れ線が床反力水平成分許容上限値 Fxmax を表す。これらの設定方法に関して以下に補足する。以下では、床面が水平である場合について説明する。

床反力水平成分は、床と足平 2 2との間の摩擦によって発生するが、摩擦はいくらでも発生できるわけではなく、限界がある。したがって、生成された目標歩容に従って実際のロポット 1が移動した時にスリップしないようにするためには、目標歩容の床反力水平成分が、常に摩擦限界以内になければならない。そこで、この条件を満足させるために、床反力水平成分許容範囲を設定し、後述するように、目標歩容の床反力水平成分がこの許容範囲内になるように、目標歩容を生成することとした _c 床と足平 2 2 との間の摩擦係数をとすると、 Fxmin は、常に i *床反力鉛直成分以上に、 Fxmax は; *床反力鉛直成分以下に設定されなければならない。最も単純な設定方法は、次式による設定である。ただし、 kaは 1より小さい正の定数である。

Fxmin=— ka * H *床反力鉛直成分

Fxmax= ka * *床反力鉛直成分

…式 1 2

図 2 0の床反力水平成分許容範囲は、式 1 2にしたがって設定した例である。床反力水平成分許容範囲を規定するパラメータとして、図 2 0 の台形波形などの折れ点での値と時刻を設定してもよいが、式 1 2により床反力水平成分許容範囲を決定するときには、単に式 1 2における (ka * ）の値をパラメ一夕として設定するだけでも良い。

尚、上記条件（目標歩容の床反力水平成分を常に摩擦限界以内にするという条件）を満足しているならば、他の設定方法で床反力水平成分許容範囲を設定するようにしても構わない。 .

次に、 S 1 0 9に進み、上記のごとく図 1 9のように設定された床反力鉛直成分軌道などに応じて、床反力モーメント鉛直成分の許容範囲 [Mzmin , Mzmax] (より詳しくはこれを規定するパラメ一夕）が図 2 1のように設定される。図 2 1の負側の折れ線が床反力モーメント鉛直成分許容下限値 Mzmin、正側の折れ線が床反力モーメント鉛直成分許容上限値 Mzmax を表す。これらの設定方法に関して以下に補足する。以下では、床面が水平である場合について説明する。

床反力モーメント鉛直成分は、床と足平 2 2との間の摩擦によって発生するが、摩擦はいくらでも発生できるわけではなく、限界がある。したがって、生成された目標歩容に従って実際のロポット 1が移動した時にスピンしないようにするためには、目標歩容の床反力モーメント鉛直成分が、常に摩擦限界以内になければならない。そこで、この条件を満足させるために、床反力モーメント鉛直成分許容範囲を設定し、後述するように、目標歩容の床反カモ一メント鉛直成分がこの許容範囲内になるように、目標歩容を生成することとした。

床と足平 2 2 との間の摩擦係数を、モーメント鉛直成分を発生するための床と足平 2 2との間の接触面の有効半径（あるいは床と足平 2 2 との間の接触面の目標 Z M Pまわりの断面 2次モ一メントの平方根）を r とすると、 Mzmin は、常に— * r *床反力鉛直成分以上に、 Mzmaxは * r *床反力鉛直成分以下に設定されなければならない。最も単純な設定方法は、次式による設定である。ただし、 ka は 1より小さい正の定数である。

Mzxmin=—ka * * r *床反力鉛直成分

Mzmax= ka * * r *床反力鉛直成分

…式 1 0 1 2 図 2 1の床反力モーメント鉛直成分許容範囲は、式 1 0 1 2にしたがつて設定した例である。床反力モーメント鉛直成分許容範囲を規定するパラメータとして、図 2 1の台形波形などの折れ点での値と時刻を設定してもよいが、式 1 0 1 2により床反力モーメント鉛直成分許容範囲を決定するときには、単に式 1 0 1 2における（ka * )の値をパラメータとして設定するだけでも良い。 rは、各瞬間において目標 Z M Pと接地面から算出するのが良いが、定数としても構わない。

尚、上記条件（目標歩容の床反力モーメント鉛直成分を常に摩擦限界以内にするという条件）を満足しているならば、他の設定方法で床反力モーメント鉛直成分許容範囲を設定するようにしても構わない。

さらには、床反力水平成分許容範囲と床反カモ一メント鉛直成分許容範囲を独立に設定するのではなく、床反力水平成分と床反力鉛直成分モーメン卜の組み合わせの許容範囲を設定するようにしても良い。なぜなら、床反力水平成分が大きくなると、床反力モーメント鉛直成分の許容範囲が狭くなり、また、床反力モーメント鉛直成分が大きくなると、床反力水平成分の許容範囲が狭くなるからである。

次に、 S 1 1 0に進み、第 1旋回歩容および第 2旋回歩容を合わせた定常歩容の Z M P軌道を規定する Z M P軌道パラメ一夕が設定される。この場合、目標 Z M P軌道は、前述したように安定余裕が高くかつ急激な変化をしないように設定される。

さらに詳細には、図 5の走行歩容においては、支持脚足平 2 2のかかとで着地した後、しばらくして該支持脚足平 2 2の底面のほぼ全面が接地し、さらにしばらくして、支持脚足平 2 2のつま先だけが接地する。そして、次に支持脚足平 2 2のつま先で蹴って空中に飛び上がり、最後に遊脚足平 2 2のかかとで着地する。また、目標 Z M Pは接地面内に存在しなければならない。そこで、本第 1参考例では、定常歩容の第 1旋回歩容及び第 2旋回歩容のそれぞれの目標 Z M Pの X軸方向の位置は、前記図 7の上段図に示したように、支持脚足平 2 2のかかとを初期位置として、該足平 2 2の底面のほぼ全体が接地するまでその位置に留まるように設定される。次に、目標 Z M Pは支持脚足平 2 2の中央に移動し. 該足平 2 2がつま先接地状態になるまでにつま先に移動し、その後、離床時まで支持脚足平 2 2のつまさきに留まるように設定される。さらにその後は、目標 Z M Pは、前述のごとく、次の遊脚足平 2 2の着地までに目標 Z M Pが、支持脚足平 2 2のつまさきから遊脚足平 2 2のかかとの着地位置まで連続的に移動するように設定される。したがって、第 1 旋回歩容と第 2旋回歩容とから成る定常歩容の目標 Z M P軌道（X軸方向の軌道）は、図 2 2のようになる。そして、この目標 Z M P軌道の折れ点の時刻及び位置が Z M P軌道パラメ一夕として設定される。この場合、折れ点の時刻は、前記要求パラメ一夕に応じて定めた第 1旋回歩容及び第 2旋回歩容の歩容周期に応じて設定され、該折れ点の位置は、次回歩容支持脚座標系及び次次回歩容支持脚座標系の位置姿勢あるいはこれらの座標系を規定する要求パラメータの 1歩目及び 2歩目の遊脚側足平着地予定位置姿勢の要求値に応じて設定される。尚、 Z M P軌道の Y 軸方向の位置は、前記図 7の下段図に示したものと同様に設定される。より詳しくは、第 1旋回歩容における目標 Z M Pの Y軸方向位置の軌道は、図 7の下段図のものと同一パターンで設定され、第 2旋回歩容における目標 Z M Pの Y軸方向位置の軌道は、第 1旋回歩容のものと同じ形の軌道で、該軌道の終端につながるものに設定される。

次に、 S 1 1 2に進み、定常歩容 1.歩（ 1周期）の初期時刻、終端時刻、期間が以下のように再定義される。

定常歩容は、その初期と終端での状態変数が連続的につながる歩容でなければならない。このような歩容の決定を容易にするために、本第 1 参考例では、定常歩容を決定するまでは、前述の狭義の歩容の定義とは異なり、定常歩容の 1歩の初期、終端、期間を便宜上、図 1 9のように定める。すなわち、第 1旋回歩容の片脚支持期の後半において床反力鉛直成分がある程度減少した時刻を定常歩容の初期時刻 Ts として設定する。なお、初期時刻 Ts は、図 7に示すように、支持脚足平 2 2の底面のほぼ全面が接地した状態からつま先接地に変わる瞬間あるいはその直前の時刻（図 7の足底全面接地期間の終了時刻もしくはその直前の時刻）に設定するのが望ましい。また、 S 1 1 0において設定した図 2 2 (または図 7 ) に示す目標 Z M Pと時刻 Ts との関係を説明すると、第 1旋回歩容において支持脚足平 2 2の底面のほぼ全面が接地した後に、目標 Z M Pが支持脚足平 2 2の中央に移動し、つま先接地状態になるまでにつま先に移動を完了した瞬間が、時刻 Ts となっているのが望ましい。このような初期時刻 Ts は例えば先に設定した目標 Z M P軌道パラメータに応じて設定される。上記のごとく初期時刻 Ts を設定する理由は後述する。また、図 1 9に示すように、定常歩容の周期 Tcyc は第 1旋回歩容と第 2旋回歩容の歩容周期の和である。定常歩容の終端時刻を Te とする, Teは、 Tsに Tcycを加えた時刻に設定される。

定常歩容が決定された時点（後述の図 2 3の S 2 0 4のループを抜けた時点）から、改めて、歩容の初期、終端等の定義を、前述の狭義の歩容の定義に戻すこととする。以下の説明では、前述の狭義の歩容の定義による初期時刻（支持脚側足平 2 2の着地当初の時刻）を 0 とし、定常歩容を決定するまで用いる上記初期時刻 Ts を参照符号 Ts を用いて (単に「Ts」と省略することもある）本来の初期時刻 0と区別する。最後に S 1 1 4に進み、定常歩容の上体姿勢角 ·逆位相腕振り角復元期間 [Tm， Ts2]および [Tm2， Te]を設定する。補足すると、定常歩容を繰り返した時に歩容の境界において上体姿勢角および逆位相腕振り角は連続であるべきである。そのためには、定常歩容の初期上体姿勢角速度と終端上体姿勢角速度が一致し、定常歩容の初期逆位相腕振り角速度と終端逆位相腕振り角速度が一致しなければならない。前記期間は、そのようにするための、上体姿勢角軌道と逆位相腕振り角軌道を調整する期間である。

具体的には、初期時刻 Ts から第 1旋回歩容の空中期を経て第 2旋回歩容になり、床反力鉛直成分がある所定の大きさまで増加した時刻 Tm が設定される。さらに、第 2旋回歩容の片脚支持期の後半において床反力鉛直成分がある程度減少した時刻 Ts2 が設定される。さらに、第 2 旋回歩容の空中期を経て第 1旋回歩容になり、床反力鉛直成分がある所定の大きさまで増加した時刻 Tm2が設定される。

図 1 9にこれらの時刻を示す。時刻 Tmは、支持脚足平 2 2の底面のほぼ全面が接地した瞬間あるいはその直後に設定するのが望ましい。時刻 Tm2 も同様である。また、時刻 Ts2 は、前記初期時刻 Ts 同様、足平 2 2の底面のほぼ全面が接地した状態からつま先接地に変わる瞬間あるいはその直前に設定するのが望ましい。

また、前記図 1 5の S 1 1 0において設定した図 2 2の目標 Z M Pとこれらの時刻 Tm, Ts2， Tm2 との関係を説明すると、第 2旋回歩容において、目標 Z M Pが、支持脚足平 2 2のかかとを初期位置として、該足平 2 2の底面のほぼ全体が接地するまでその位置に留まり、次に支持脚足平 2 2の中央に移動し始める瞬間を時刻 Tm とすることが望ましい, その後、支持脚足平 2 2のつま先だけ接地した状態になるまでに、目標 Z M Pがつま先に移動を完了した瞬間を、時刻 Ts2 とすることが望ましい。さらに次の第 1旋回歩容において、目標 Z M Pが、支持脚足平 2 2のかかとを初期位置として該足平 2 2の底面のほぼ全体が接地するまでその位置に留まり、次に支持脚足平 2 2の中央に移動し始める瞬間を時刻 Tm2とすることが望ましい。

上記のごとく設定する理由は後述する。尚、上体姿勢角を復元（調整）する期間と逆位相腕振り角を復元（調整）する期間を別々に設定しても良い。

図 1 3の S 0 1 0から S 0 2 2までに示す処理が行われた後、 S 0 2 4に進み、定常歩容の初期状態が算出される。ここで算出される初期状態は、定常歩容の初期上体水平位置速度（水平方向での初期上体位置及び初期上体速度）、初期上体鉛直位置速度（鉛直方向での初期上体位置及び初期上体速度）、初期発散成分、初期上体姿勢角、角速度、および初期逆位相腕振り角、角速度である。この初期状態の算出は、図 2 3のフローチャートに従って、探索的に行なわれる。

図 2 3のフローチャートにおいては、先ず、 S 2 0 0において、定常歩容の歩容パラメータ（前記図 1 3の S 0 2 2で設定したパラメータ）に基づいて、目標足平位置姿勢、目標腕姿勢、および目標上体姿勢角 (傾斜角とョ一角）の初期状態（初期時刻 Ts での状態）が決定される, ここで状態とは、位置や姿勢角とその変化率（時間微分）とを表す。この場合、支持脚側の目標足平位置姿勢の初期状態は、前記図 1 5の S 1 0 0で決定した足平軌道パラメ一夕の第 1旋回歩容初期支持脚足平位置姿勢から、第 2旋回歩容終端遊脚足平位置姿勢に至る足平位置姿勢軌道（次回歩容支持脚座標系から見た軌道）を時刻 Ts まで有限時間整定フィルタにより生成することにより決定される。遊脚側の目標足平位置姿勢の初期状態は、次回歩容支持脚座標系から見た今回歩容初期支持脚足平位置姿勢から、第 1旋回歩容終端遊脚足平位置姿勢に至る足平位置姿勢軌道を時刻 Ts まで有限時間整定フィル夕により生成することにより決定される。また、目標腕姿勢の初期状態は、図 1 5の S 1 0 4で決定した基準腕姿勢軌道パラメ一夕に基づいて求められる時刻 Ts における基準腕姿勢に決定される。より具体的には、目標腕姿勢の両腕体 5， 5の全体重心位置 (上体 3に対する相対位置）、左右の手先 (両腕体 5 , · 5の先端部）の左右方向の間隔および逆位相.腕振り角 · 角速度が決定される。但し、逆位相腕振り角 · 角速度は、後述するように、定常歩容を繰り返した時に歩容の境界で連続するように修正されるので、ここではまだ仮に決定されただけである。

また、目標上体姿勢角の初期状態は、図 1 5の S 1 0 2で決定した基準上体姿勢軌道パラメータにより定まる時刻 Ts における基準上体姿勢 (傾斜角およびョ一角）及びその角速度が該目標上体姿勢角の初期状態として決定される。なお、本第 1参考例では、上体 3の傾斜角に係る基準上体姿勢は鉛直姿勢であるので、目標上体姿勢のうちの傾斜角の初期状態（傾斜角及びその角速度）は 0である。

また、本第 1参考例では、定常歩容の目標足平位置姿勢軌道、床反力鉛直成分軌道、及び目標 Z M P軌道は、それぞれ、図 1 5のフローチヤ ―トで決定した足平軌道パラメ一夕、床反力鉛直成分軌道パラメータ、 Z MP軌道パラメ一夕により互いに独立的に決定される。例えば定常歩容の瞬時瞬時の目標足平位置姿勢は、床反力鉛直成分の瞬時値に依存することなく足平軌道パラメ一夕に応じて決定される。

次いで S 2 0 2において初期上体水平位置速度の候補（すなわち初期時刻 Tsでの上体水平位置速度候補）である（Xs， Vxs) (Xs : 水平位置、 Vxs : 水平速度）が仮決めされる。ここで仮決めする候補（Xs， Vxs) は、任意でよいが、例えば前回歩容の生成時に求めた定常歩容の初期状態における上体水平位置速度を仮決め候補（Xs, Vxs) とすればよい。

尚、説明を簡略にするため、サジタルプレーン上で X方向（前後方向）での定常歩容の初期状態を探索する場合を例にとるが、実際には位置 · 速度とも X方向 (前後方向）， Y方向 (左右方向) を別々にあるいは同時に定常歩容の初期状態（定常歩容の前記境界条件を満たす初期状態) を探索する必要がある。 ,

補足すると、サジタルプレーン上では、ョ一回転やモーメント鉛直成分など鉛直軸まわりに関する概念は存在しないので、少なくともョ一回転やモーメント鉛直成分に関しては、 3次元空間で演算される。

探索的な決定手法としては、擬似ヤコビアン（感度マトリックス）を求め、最急降下法などにより次候捕を決定する方法や、シンプレックス法などを用いれば良い。本実施例では、最急降下法を用いる。

次いで、 S 2 0 4を経て S 2 0 6において、定常歩容を繰り返した時に上体鉛直位置速度が連続になり、かつ、膝等の関節角が伸び過ぎたり縮み過ぎたりしないように、初期（時刻 Ts) 上体鉛直位置速度（Zs， Vzs) (Zs : 鉛直位置、 Vzs : 鉛直速度）が、決定される。これについての詳細は、例えば本願出願人が先に出願した P CTZ J P 0 2 Z 1 3 5 9 2に記載されているので、ここでは省略する。

S 2 0 6の処理の後、次いで S 2 0 8に進み、定常旋回歩容が仮に生成される（この仮生成される定常旋回歩容を以降、仮歩容ということがある）。より具体的には、前記図 1 3の S 0 2 2で決定した定常歩容の歩容パラメ一夕を基に、初期時刻 Ts から終端時刻 Te までの各瞬間における目標 Z M P、目標床反力鉛直成分、目標足平位置姿勢、基準上体姿勢、目標腕姿勢、床反力水平成分許容範囲および床反力モーメント鉛直成分許容範囲が逐次求められる。そして、求めた目標 Z M Pと目標床反力鉛直成分とに関する動力学的平衡条件、並びに床反力水平成分許容範囲の条件を満足するように、前記動力学モデル（図 1 2のモデル）を用いて、前記上体水平位置速度（X s， V x s)、上体鉛直位置速度（Zs, V z s) を上体 3の初期（時刻 Ts) 状態として、上体位置姿勢を逐次決定することによって、時刻 Ts から終端時刻 Te までの歩容を生成する ₍ また、この時、上体姿勢は、基準上体姿勢になるべく一致するように生成される。 ,

また、床反力モーメント鉛直成分に関する条件、すなわち、床反カモーメント鉛直成分許容範囲を満足するように、逆位相腕振り動作が決定される。

尚、この定常歩容の歩容生成は歩容生成装置 1 0 0の内部で行われるだけであって、実際のロボット 1を駆動するための目標値として、後述する複合コンプライアンス動作決定部 1 0 4に出力されることはない。以下に、 S 2 0 8の処理である逐次演算による定常歩容生成処理の詳細を説明する。

図 2 4はその処理を示すサブルーチンフローチヤ一トである。

以下説明すると、 S 3 0 0において、諸々の初期化が行われる。具体的には、仮歩容生成用時刻 kに初期時刻 Ts を代入する。さらに、上体水平位置速度に、現在仮決定されている（Xs, V xs) (図 2 3の S 2 0 2もしくは後述の S 2 1 6もしくは S 2 1 8で決定されたもの）が代入され、上体鉛直位置速度に、前記 S 2 0 6で求めた最新の（Zs, V z s) が代入される。また、上体姿勢角には、基準上体姿勢角初期値 (初期時刻 Ts での角度）が代入され、上体姿勢角速度には、基準上体上体姿勢角速度初期値（初期時刻 Tsでの角速度）が代入される。

逆位相腕振り角には、基準初期逆位相腕振り角（初期時刻 Ts での角度）逆位相腕振り角速度には、基準初期逆位相腕振り角速度（初期時刻 Tsでの角速度）が代入される。

次いで、 S 3 0 2を経て S 3 0 4において、仮歩容生成用時刻 k が歩容終端時刻以前であるか否か（k ≤ Ts + Tcyc であるか否か）が判断され、その判断結果が Y E Sである場合には、 S 3 0 6の歩容瞬時値決定サブルーチンに進み、歩容瞬時値が決定される。次いで歩容生成装置 1 0 0の処理は、 S 3 0 8に進み、仮歩容生成用時刻 kをだけ増やした後、 S 3 0 4に戻る。

ここで、 Ak は、仮歩容生成のインターバルであり、通常、制御周期 Δ t と一致させておけば良い。なお、は、仮歩容の動力学的精度を落としても良いならば、演算量を低減するために、 Ak を Δ tよりも長く設定しても良い。

S 3 0 4の判断結果が NOである場合には、 S 3 1 0に進む。以上の処理により、 S 3 1 0に進むまでに定常歩容（仮歩容）が初期から終端まで生成される。

以下に、 S 3 0 6の歩容瞬時値決定サブルーチンの詳細を図 2 5を参照して説明する。

まず図 2 5の S 4 0 0において、定常歩容パラメ一夕（床反力鉛直成分軌道パラメータ）を基に、前記図 1 9に示した目標床反力鉛直成分の時刻 k における値（今回値）が求められる。さらに、 S 4 0 2において、定常歩容パラメ一夕（Z M P軌道パラメ一夕）を基に、前記図 2 2 に示した目標 Z M P軌道の時刻 k における値（今回値）が求められる, 次いで S 4 0 4に進み、定常歩容パラメータ（足平軌道パラメータ、基準上体姿勢軌道パラメ一夕、及び腕姿勢軌道パラメータ）を基に、時刻 k における目標両足平位置姿勢（支持脚側及び遊脚側の両者の目標足平位置姿勢）、基準上体姿勢および基準腕姿勢の値（今回値）が求められる。基準腕姿勢に関しては、具体的には、両腕体 5， 5の全体重心位置（上体 3 に対する相対位置）、左右の手先（両腕体 5， 5の先端部）の左右方向の間隔および逆位相腕振り角の値（今回値）が求められる。

尚、目標足平位置姿勢の今回値（時刻 kでの値）は、前記図 2 3の S 2 0 0で初期時刻 Ts での足平位置姿勢を求めた場合と同様に求められる。

次いで S 4 0 6に進み、目標床反力鉛直成分を満足する (ロボット 1 の鉛直方向の慣性力と重力との和を目標床反力鉛直成分に釣り合わせる）ような時刻 k における全体重心鉛直位置速度の値（今回値）が算出される。具体的には、例えば前記図 1 2の動力学モデルに係わる前記式 0 1 と式 0 4とに基づいて、全体重心鉛直位置速度が算出される。すなわち、式 0 1 と式 0 4とによれば、口ポット 1の運動による全体重心鉛直加速度と重力加速度との和にロポッ卜 1の全体質量を乗算したものが床反力鉛直成分に等しいという関係式（口ポット 1の全体重心の鉛直方向に関する運動方程式）が得られる。従って、この関係式と目標床反力鉛直成分とから、全体重心鉛直加速度が求められる。

尚、この関係式自体は、ロボット 1のモデルに依存することなく一般的に成立するものである。そして、この求めた全体重心鉛直加速度を積分することにより、全体重心鉛直速度が算出され、さらにその全体重心鉛直速度を積分することにより、全体重心鉛直位置が算出される。これらの算出は、より一般的には、次式 1 5および式 1 6で表される動力学関係式（ニュートンの運動方程式を離散化した式）を用いて行なわれる, 時刻 kにおける全体重心鉛直速度

=時刻（k一 A k)における全体重心鉛直速度

+ ((床反力鉛直成分 Zロポットの全体質量） +重力加速度） *厶 k

(ただし、重力加速度は負の値とする。）

…式 1 5 時刻 kにおける全体重心鉛直位置

=時刻（k一 A k)における全体重心鉛直位置

+時刻 kにおける全体重心鉛直速度 * A k

式 1 6 次いで S 4 0 8に進み、全体重心鉛直位置を満足する上体鉛直位置が算出される。具体的には、例えば前記図 1 2のモデルに係わる式 0 4を用いて上体鉛直位置が算出される。すなわち、支持脚側及び遊脚側の目標足平位置姿勢の今回値から、図 1 2のモデルの支持脚質点 2 m及び遊脚質点 2 mの鉛直位置が求められる。そして、これらの求めた支持脚質点 2 m及び遊脚質点 2 mの鉛直位置と、 S 4 0 7で求めた全体重心鉛直位置の今回値とを式 0 4に適用することで、上体質点 3 mの鉛直位置が求められる。さらに、この求めた上体質点 3 mの鉛直位置と目標上体姿勢角の現在値（S 4 0 4で設定された基準上体姿勢角または後述の S 4 1 4で決定された前回（時刻 k— A k ) の目標上体姿勢角）とから上体鉛直位置が求められる。

次いで S 4 1 0に進み、前記図 1 5の S 1 0 8で決定した歩容パラメ —夕（床反力水平成分許容範囲を規定するパラメータ）に基づいて、図 2 0に示した床反力水平成分許容範囲 [Fxmin， Fxmax]の時刻 k における値（今回値）が求められる。

次いで S 4 1 1に進み、前記図 1 5の S 1 0 9で決定した歩容パラメ一夕（床反力モーメント鉛直成分許容範囲を規定するパラメ一夕）に基づいて、図 2 1 に示した床反力モーメント鉛直成分許容範囲 [Mzmin, Mzmax]の時刻 kにおける値（今回値）が求められる。

次いで S 4 1 2に進み、目標 Z M Pに関する動力学的平衡条件（ロボット 1の慣性力と重力との合力が目標 Z M Pまわりに発生するモーメントの水平成分が 0であるという条件）を満足するように、目標上体水平加速度と目標上体姿勢加速度との今回値が決定される。ただし、床反力水平成分 Fx が [Fxmin, Fxmax]を越えないように、上体水平加速度と上体姿勢角加速度（より詳しくは、上体傾斜角加速度）とが決定される, また、床反力モーメント鉛直成分 Mzが [Mzmin, Mzmax]を越えないように、目標逆位相腕振り角加速度の今回値が決定される。

尚、上体姿勢角のうち、ョ一角は基準上体姿勢角のうちのョ一角に一致するように決定される。また、目標腕姿勢に関しては、逆位相腕振り角以外の成分は、基準腕姿勢に一致するように決定される。また、この際、上記条件を満足しつつも、目標上体傾斜角と目標逆位相腕振り角とは、それぞれ基準上体傾斜角と基準逆位相腕振り角とになるべく追従するように決定される。以下にこの詳細を説明する。

この時点において、前記の如く足平位置姿勢および上体鉛直位置の瞬時値（今回値）が決定されており、また、腕姿勢に関して、逆位相腕振り角以外の成分は、基準腕姿勢のそれに一致するように決定されている _t このため、残りの上体水平位置と上体姿勢角と逆位相腕振り角とを決定すれば、ロボット 1の目標運動は一義的に決定できる。したがって、すベての床反力も一義的に決定される。なお、本第 1参考例では、定常歩容の目標床反力鉛直成分及び目標 Z M Pは、それぞれ前記図 1 3の S 0 2 2で決定した床反力鉛直成分軌道パラメ一夕及び目標 Z M P軌道パラメ一夕によって規定される。

ところで、歩容生成において、前記上体並進モードをあまり用いずに主に上体傾斜モードを用いることにより、目標 Z M Pを満足させる（目標 Z M Pまわりの床反力モーメントの水平成分を 0にする）と、上体姿勢角が大きく振れ過ぎてしまう恐れがある。したがって、これを防ぐためには、なるべく上体並進モードを用いるべきである。しかし、上体並進モードは、床反力水平成分の変化を伴うので、床反力水平成分許容範囲が狭い場合には、上体並進モードを強く働かせるとスリップする恐れがある。この場合には、上体傾斜モードに頼るしかない。特に、前記走行歩容のように床反力水平成分許容範囲が 0.となる期間では、床反力水平成分を発生させるような歩容を生成することはできないので、上体傾斜モードに頼らざるを得ない。

一方、逆位相腕振り運動は、目標 Z M Pまわりの床反力モーメントの水平成分および床反力水平成分のいずれも変化させることなく、床反力モーメント鉛直成分のみを変化させることができるので、床反力モーメント鉛直成分が前記床反力モーメント鉛直成分許容範囲を越えないようにするために用いることができる。以上のことを考慮し、本第 1参考例では、図 2 6に示すフローチャートに従って、上体水平加速度と上体姿勢角加速度と逆位相腕振り加速度とが決定される。尚、理解の便宜上、上体水平加速度と上体姿勢角加速度（上体 3の傾斜角の角加速度）の決定に関しては、サジタルプレーン上で X方向（前後方向）での上体水平加速度および上体姿勢角加速度を決定する場合を例にとって説明するが. 実際には Y方向（左右方向）の上体水平加速度および上体姿勢角加速度も X方向と同様に決定される。

まず S 5 0 0において、目標上体ョ一角に時刻 k での基準上体ョ一角の値を代入する。また、腕姿勢の逆位相腕振り角 · 角速度成分を除き. 目標腕姿勢に時刻 kでの基準腕姿勢の値を代入する。

次に S 5 0 2において現在時刻（定常歩容作成用夕イマ一の値） k が. 上体姿勢角 · 逆位相腕振り角復元期間（定常歩容の場合、上体姿勢角 · 逆位相腕振り角復元期間は、時刻 Tm から時刻 Ts2 の期間および時刻 Tm2 から Te までの期間である。）の間にあるか否かが判定される。 S 5 0 2の判定結果が N Oであるならば、 S 5 0 4に進み、 Y E Sであるならば S 5 3 0に進む。

S 5 0 4では、ロボット 1の前回瞬時歩容状態（時刻 k 一 1の歩容状態）から、仮に、上体傾斜モードの角加速度を 0にして、上体並進モ一ドの運動をロボット 1 にさせた場合に今回 (時刻 k ) の目標 Z M Pを満足するために必要な上体水平加速度 a tm が求められる。乙の a tmp は、例えば前記図 1 2の動力学モデルに係わる前記式 0 3 yを用いて求められる。より具体的には、例えば、現在時刻 kまでに求められた目標足平位置姿勢の時系列値を用いて、現在時刻 kにおける支持脚質点 2 m 及び遊脚質点 2 mの鉛直加速度が求められると共に、現在時刻 k (今回）の目標足平位置姿勢を用いて支持脚質点 2 m及び遊脚質点 2 mの鉛直位置が求められる。また、現在時刻 k (今回）の床反力鉛直位置を用いて上体質点 3 mの鉛直位置が求められると共に、現在時刻 kまでに求めた目標上体鉛直位置の時系列値を用いて現在時刻 kにおける上体質点 3 mの鉛直加速度が求められる。そして、これらの求めた値を前記式 0 3 yに代入すると共に、同式 0 3 yの My、 d2 0 by/dt2を 0にしてなる式を、 d2Xb/dt2 について解くことにより、上体質点水平加速度が上体水平加速度 a tmp として求められる。尚、より厳密な動力学モデルを用いて、目標 Z M P回りの床反力モーメントの水平成分を零にするような上体水平加速度 a tmp を探索的に求めるようにしてもよい。また、本第 1参考例では、上体 3の傾斜角に関する基準上体姿勢が鉛直姿勢で. 該基準上体姿勢による上体姿勢角加速度（上体 3の傾斜角の角加速度）が 0であるため、上体傾斜モードの角加速度を 0にして上体水平加速度 a tmp を求めるようにした。但し、基準上体姿勢のうちの傾斜角が変化するように該基準上体姿勢軌道パラメ一夕を設定した場合で、それにより定まる現在時刻 kにおける基準上体姿勢角加速度（上体 3の傾斜角の基準角加速度）が 0でない場合には、上体傾斜モードの角加速度をその 0でない基準上体姿勢角加速度の値にして、上体水平加速度 a tmp を動力学モデルを用いて求める (例えば式 0 3 yの d2 Θ by/dt2 を 0でない基準上体姿勢角加速度に設定して上記と同様に上体水平加速度 0!

•tmpを求める）ようにすればよい。

次に、 S 5 0 6に進み、上体水平加速度が a tmp の場合の時刻 kにおける床反力水平成分 Fxtmp が動力学モデルを用いて求められる。本第 1参考例においては、前記動力学モデルの式 0 2 Xを用いて Fxtmp が求められる。すなわち、次式 1 7によって Fxtmp が求められる。伹し、 d2XsupZ dt2および d2Xswg/ dt2 はそれぞれ時刻 kにおける支持脚足平質点水平加速度、遊脚足平質点水平加速度を表す。

Fxtmp = mb * a tmp + msup * d2Xsup/ dt2

+ msw * d2Xswg/ dt2 …式 1 7 このようにして求められる Fxtmp の例を図 2 7に示す。図 2 7に、 Fxtmpが床反力水平成分許容範囲 [Fxmin, Fxmax]を越えている部分を斜線で示す。

次に、 S 5 0 8に進み、上体並進モードの上体水平加速度ひ、これによって発生する床反力水平成分 Fx、上体傾斜モードの上体角加速度 i8 が次のように決定される（S 5 0 8〜 S 5 1 6 )。

すなわち、

Fxtmp >Fxmax ならば、 S 5 1 0に進み、次式により Fx が決定される。 . Fx=Fxmax …式 1 8

Fxtmp < Fxminならば、 S 5 1 2に進み、次式により Fxが決定される ₍ Fx= Fxmin …式 1 9 それ以外、すなわち、 Fxtmp が、床反力水平成分許容範囲 [Fxmin, Fxmax]内に存するならば、 S 5 1 4に進み、次式により Fx が決定される。 Fx = Fxtmp …式 2 0 いずれの場合においても、次いで S 5 1 6に進み、次式により上体水平加速度ひ、上体姿勢角加速度（上体傾斜角加速度））3が決定される。 = tmp + (Fx- Fxtmp)/ Δ Fp …式 2 1 '

β =( tmp - «)* ΔΜΡ ΔΜ Γ …式 2 2 ただし、 A F p、 Δ Μ ρおよび Δ Μ rはそれぞれ前記式 0 6、 0 7、および式 0 9により決定される。

補足すると、より動力学演算の精度を高めようとするならば、上記のごとく上体角加速度 ]3を求めた後、上体並進モードと上記求めた上体角加速度 ]3の上体傾斜モードとを合成した運動が目標 Z M Pを満足するように、上体並進モードの上体水平加速度 c¾を、より厳密な動力学モデルを用いて解析的あるいは探索的に決定した方が良い。なお、探索的な決定法としては、擬似ヤコビアン (感度マトリックス) を求め、擬似ニュ一トン法などにより次候補を決定する方法や、シンプレックス法などを用いれば良い。

さらに床反力水平成分 Fx が床反力水.平成分許容範囲 [Fxmin , Fxmax]を厳密に越えないようにするために、 S 5 1 0においては、 Fx = Fxmax かつ目標 Z M Pまわりの床反力モーメン卜の水平成分が 0になるように、 S 5 1 2においては、 Fx= Fxmin かつ目標 Z M Pまわりの床反カモ一メン卜の水平成分が 0になるように、上体水平加速度と上体角加速度 |Sとの組を探索的に求めても良い。

上記のように求められる Fx を図 2 8に示す。 Fx は、 Fxtmp の値を床反力水平成分許容範囲 [Fxmin, Fxmax]から越えないように制限（飽和）させたものになっている。より詳しくは、上体並進モードのみによる上体水平加速度 cUmp に応じた Fxtmp が許容範囲 [Fxmin, Fxmax] 内にある場合には、 Fxtmpがそのまま Fxとなる。また、上体並進モードのみによる上体水平加速度 tmp に応じた Fxtmpが許容範囲 [Fxmin, Fxmax]の上限を超え、もしくは下限を下回った場合には、それぞれ； Fx は、それぞれ強制的に Fxmax、 Fxmin に制限される。特に走行歩容の空中期では、常に、 Fxmax= Fxmin= 0であるので、 Fx= 0となる。また、上記のように求められる上体姿勢角加速度 ]3を図 2 9に示す。上体並進モードによって発生する Fx が許容範囲 [Fxmiii, Fxmax]を越えないように、上体並進モードの加速度を制限したことによって不足した床反力モーメント（より詳しくは目標 Z M P回りの床反力モーメントの水平成分を 0にする上で必要な慣性カモ一メン卜から、制限した上体水平運動と両脚体 2， 2の運動とによるモーメント成分を差し引いたモーメント）を、上体傾斜モードで補ったことになる。なお、走行歩容の空中期では、上体並進モードによる上体水平加速度 αは常に 0に制限されることとなるので、上体傾斜モードによる上体姿勢角加速度 iSのみによって、上記不足分の床反力モーメン卜が補われることとなる。

次に、 S 5 1 8に進み、仮に上体並進モードの上体水平加速度が α、上体傾斜モードの上体角加速度（上体傾斜角加速度）が /3、上体ョ一回転モードの上体加速度（上体ョ一角加速度）が基準ョ一角加速度 d20 bzref/dt2, 逆位相腕振り角加速度 )3 a が基準逆位相腕振り角加速度 d2 Θ azref/dt2 である運動をさせた場合の床.反カモ一メント鉛直成分 Mztmp を求める。以降、 d20 bzref/dt2 を /3 bref、 d2 Θ azref/dt を β aref とする。

具体的には、式 0 3 z に、式 1 0 0 1から式 1 0 0 4を代入することによって得られる Mzが Mztmpである。 d2Xb/dt2= x …式 1 0 0 1 d2Yb/dt2= o; y …式 1 0 0 2 d20 bz/dt2=i3 bref …式 1 0 0 3 d2 Θ

aref …式 1 0 0 4 ただし、ひ x は、上体水平加速度 αの X成分、 a y は、上体水平加速度 o;の Y成分を表す。また、 Xb、 Yb には時刻 k一 1における上体水平位置を代入し、 Xzmp、 Yzmp、 Xsup、 d2Ysup/dt2、 Xswg 、 d2Yswg/dt2には、時刻 kの値を代入するものとする。

このようにして求められる Mztmp の例を図 3 2に示す。図 3 2に、 Mztmp が床反力モーメント鉛直成分許容範囲 [Mzmin, Mzmax]を越えている部分を斜線で示す。

次に、 S 5 2 0に進み、逆位相腕振り角加速度 /3 a が次のように決定される（S 5 2 0〜 S 5 2 8 )。

すなわち、

Mztmp >Mzmax ならば、 S 5 2 2に進み、次式により Mz が決定される。

Mz = Mzmax …式 1 0 1 8 Mztinp< Mzmin ならば、 S 5 2 4に進み、次式により Mz が決定される。

Mz= Mzmin …式 1 0 1 9 それ以外、すなわち、 Mztmp が、床反力水平成分許容範囲 [Mzmin, Mzmax]内に存するならば、 S 5 2 6に進み、次式により Mz が決定される。

Mz = Mztmp …式 1 0 2 0 いずれの場合においても、次いで S 5 2 8に進み、次式により逆位相腕振り角加速度 jS aが決定される。 β 8L = β aref + (Mztmp - Mz) Z Δ Maz 式 1 0 2 1 ただし、 A Mazは式 0 9 aにより決定される。

以下に S 5 1 8から S 5 2 8までの処理を解説する。

上記のように求められる Mzは、逆位相腕振りを含めたロポット全体の運動による床反力モーメント鉛直成分を示している。

上記処理では、この Mz が床反力モーメント鉛直成分許容範囲 [Mzmin, Mzmax]を越えないように逆位相腕振り角加速度 /3 aが決定されている。具体的には、 Mz は、図 3 3に示すように、 Mztmp の値を床反力水平成分許容範囲 [Mzmin , Mzmax]から越えないように制限 (飽和）させたものに決定されている。より詳しくは、 Mztmp が許容範囲 [Mzmin, Mzmax]内にある場合には、 Mztmp がそのまま Mz となり、 Mztmp が許容範囲 [Mzmin, Mzmax]の.上限を越え、もしくは下限を下回った場合には、それぞれ Mz は、それぞれ強制的に Mzmax、 Mzmin に制限される。特に走行歩容の空中期では、常に、 Mzmax = Mzmin= 0であるので、 Mz = 0となる。

Mz が床反力モーメント鉛直成分許容範囲 [Mzmin, Mzmax]を越えないようにするために、逆位相腕振りによって発生すべきモーメント鉛直成分 Mazは、（Mz— Mztmp)である。 Maz(= Mz— Mztmp)を図 3 4に示す。

逆位相腕振り角加速度 β aは、 Mazを逆位相腕振りの等価慣性モ一メント A Maz で割ったものを基準逆位相腕振り角加速度 ) 3 aref (基準逆位相腕振り角を 2階微分した値）に加えることによって得られる。すなわち、 /3 aは前記式 1 0 2 1で求められる。逆位相腕振り角加速度 β a を図 3 5に示す。

以上のごとく S 5 0 4から S 5 2 8までの処理では、逆位相腕振りを含めたロポット全体の運動によって発生する床反力モーメント鉛直成分 Mz が許容範囲 [Mzmin, Mzmax]を越えないように（逆位相腕振り角加速度を基準逆位相腕振り角加速度 /3 aref に一致させた場合に発生する床反力モーメント鉛直成分 M_Ztmp が許容範囲を越えた分を打ち消す (キャンセルする）ように）、逆位相腕振り角加速度 i3 a が決定される, 補足すると、床反力モーメント鉛直成分 Mzが床反力モーメント鉛直成分許容範囲 [Mzmin, Mzmax]を厳密に越えないようにするために、 S 5 0 4から S 5 2 8までの処理の代わりに、逆位相腕振り角加速度) 3 a を、より厳密な動力学モデルを用いて解析的あるいは探索的に決定した方が良い。なお、探索的な決定法としては、擬似ヤコビアン (感度マ卜リックス) を求め、擬似ニュートン法などにより次候補を決定する方法や、シンプレツクス法などを用いれば良い。

· 以上が、時刻 kが上体姿勢角 ·逆位相腕振り角復元期間の間にない場合の処理である。

S 5 0 2の判定結果が Y E Sである場合には以下の処理が行われる。まず、 S 5 3 0に進み、口ポット 1の前回瞬時歩容状態（時刻 k一 1の歩容状態）から、上体傾斜モードの角加速度を 0にして、上体並進モードの運動を口ポット 1 にさせた場合に今回（時刻 k ) の目標 Z M Pを満足するために必要な上体水平加速度 αが求め、これを最終的な上体水平加速度と決定する。

次に、 S 5 3 2に進み、上記場合に床反力水平成分 Fxを求める。

次に、 S 5 3 4に進み、上体姿勢角加速度（上体傾斜角加速度） βを 0に決定する。なお、上体ョ一角加速度は、基準上体ョ一角加速度 3 bref (基準上体ョ一角を 2階微分した値）に決定される。次いで最後に、 S 5 3 6に進み、逆位相腕振り角加速度 ]3 a に、基準逆位相腕振り角加速度 β aref (基準逆位相腕振り角を 2階微分した値）を代入する。

以上が、 S 5 0 2の判定結果が Y E Sである場合の処理である。すなわち、この場合には、上体姿勢角加速度（上体傾斜角加速度および上体ョ一角加速度）を基準上体姿勢角加速度に、逆位相腕振り角加速度を基準逆位相腕振り角加速度に一致させる。このようにしても運動によって発生する床反力が、床反力水平成分許容範囲および床反カモ一メント鉛直成分許容範囲のいずれも越えないと予想されているので、このように決定しても問題ない。

S 5 2 8または S 5 3 6の処理の後、図 2 5の S 4 1 4に進み、 S 4 1 2で求めた上体水平加速度を逐次積分する (時刻 Ts から現在時刻 k まで累積加算する）ことにより上体水平速度が求められ、さらにその上体水平速度を逐次積分する (時刻 Ts から現在時刻 kまで累積加算する）ことにより、上体水平位置 (今回値) が求められる。また、 S 4 1 2で求めた上体姿勢角加速度を逐次積分する (時刻 Ts から現在時刻 k まで累積加算する）ことにより上体姿勢角速度が求められ、さらにその上体姿勢角速度を逐次積分する（時刻 Ts から現在時刻 kまで累積加算する）ことにより、上体姿勢角（今回値）が求められる。

次に S 4 1 6に進み、 S 4 1 2で求めた逆位相腕振り加速度 j8 a を逐次積分する（時刻 Ts から現在時刻 kまで累積加算する）ことにより逆位相腕振り速度が求められ、さらにこれを逐次積分する（時刻 Ts から現在時刻 kまで累積加算する）ことにより、逆位相腕振り角 0 az (今回値）が求められる。

以上のごとく図 2 4の S 3 0 6の定常歩容瞬時値決定サブルーチンが実行された後、 S 3 0 8に進み、歩容生成用時刻 k の値が歩容生成ィン夕一バル△ kだけ増加される。次いで S 3 0 4に戻り、 S 3 0 4に示す条件が成立している限り、 S 3 0 6 と S 3 0 8の処理が繰り返され、 S 3 0 4に示す条件が成立しなくなったら、すなわち終端（時刻 Te = Ts + Tcyc) まで仮歩容の生成が完了したら、 S 3 1 0に進む。

定常歩容では、それを繰り返したときに境界でロボット 1の運動状態が不連続にならないように、初期上体姿勢角及びその角速度を決定する必要がある。

そのために、 S 3 1 0では、上体姿勢角速度を、時刻 Te までに、初期値（時刻 Ts での値）に戻すための上体姿勢角加速度を発生させる床反力モーメントの ZMP換算値（以降、上体姿勢復元モーメント Z MP 換算値と呼び、 Z MPrecと略す）のパターンを設定する。

これについて以下に詳細に説明する。

上体姿勢角 ·逆位相腕振り角復元期間（時刻 Tm から時刻 Ts2 の期間および時刻 Tm2 から Te までの期間）に、上体傾斜モードを用いて上体姿勢角加速度を発生させることにより、上体姿勢角速度を初期値 (時刻 Ts での値）に戻すことを考える。このための上体姿勢角加速度パターンを、 3 (k)とする。なお、上記期間以外では、 /3(k)=0 とする, 上体傾斜モードでは、上体姿勢角加速度 iS (k)を発生させようとすると、床反力モーメント 3 (k)* ΔΜ Γが発生する。この結果、その瞬間における床反力鉛直成分が Fz(k)であるとすると、（目標 Z M Pでなく）運動から算出される Z MP (k)は、次式で求められる Δ Z M Pだけずれる。

Δ ZMP(k)= - /3 (k)* ΔΜ r /Fz(k) …式 2 3 したがって、 △ M rのパターンと Fz( k )のパ夕一ンが決まっていれば (既知であれば）、 △ Z MP(k)のパターンを適当に設定することによつて、式 2 3を満足する上体姿勢角加速度パターンを発生させて、上体姿勢角速度を初期値（時刻 Ts での値）、すなわち基準上体姿勢軌道の初期（時刻 Ts)状態における上体姿勢角速度に戻すことができる。

前記上体姿勢復元モーメント Z M P換算値（Z MPrec) は、そのように適切に設定された Δ ZMP(k)を意味する。前記式 2 3を用いて上体姿勢復元モーメント Z M P換算値を設定する際に、 ΔΜ Γは厳密には変動するが、近似的に一定値としても良い。定常歩容は仮に生成されるだけであり、実際の口ポットをこの歩容に追従させるわけではないので. 定常歩容の動力学的精度はあまり高くなくても良いからである。

図 3 0に Z M P rec の一例を示す。図 3 0では、 Z M P rec のパタ一ンとして、時刻 Tmから時刻 Ts2 の期間および時刻 Tm2から Te までの期間に台形パターンが形成されている。その台形部の折れ点の時刻は. 時刻 Tm と時刻 Ts2 との間の期間および時刻 Tm2から Te までの期間における目標 Z M Pパターンの折れ点の時刻 (図 2 2参照) に一致させてある。これは、後述するが、今回歩容の目標 Z M Pパターンの修正が簡単になるからである。

式 2 3の Δ Z MP(k)に ZMPrec(k)を代入することにょり次式が得られる。 i3 (k) = - Z M Prec(k)* Fz(k)/ ΔΜ r …式 2 4 したがって、この式 2 4で求められる 3 (k)は、図 3 1の実線で示すようになる。なお、図 3 1の点線で示すものは、時刻 Ts から時刻 Tm までの期間および時刻 Tm2 から Te までの期間の上体姿勢角加速度 (図 2 9の実線示のもの）である。（以降、時刻 kでの値であることが明らかな場合には、（k )を省略する場合がある。）

初期（時刻 Ts) 上体姿勢角は、初期（時刻 Ts) 基準上体姿勢角に一致させる。

また、初期上体姿勢角速度は、式 3 7 a、 3 7 を満たすように決定する。終端上体姿勢角一初期上体姿勢角

=床反力水平成分許容範囲を満たすように決定された上体姿勢角加速度の 2階積分

+ Z M P recによって発生する上体姿勢角加速度の 2階積分

+初期上体姿勢角速度 *定常歩容の周期

……式 3 7 a 終端上体姿勢角速度一初期上体姿勢角速度

· =床反力水平成分許容範囲を満たすように決定された上体姿勢角加速度の 1階積分

+ Z M P recによって発生する上体姿勢角加速度の 1階積分

……式 3 7 b なお、式 3 7 a、 3 7 bのそれぞれの右辺第 1項の積分期間は、時刻 Tsから Tm までの期間と、 Ts2から Tm2 までの期間とを合わせた期間であり、右辺第 2項の積分期間は、時刻 Tm から Ts2 までの期間と Tm2から Teまでの期間とを合わせた期間である。

より具体的に説明すると定常歩容では、第 1旋回歩容の支持脚座標系 (次回歩容支持客座標系）から見た初期状態姿勢角及び角速度が、それぞれ、次の第 1旋回歩容の支持脚座標系（次次次回歩容支持脚座標系）から見た終端上体姿勢角および角速度に一致していなければならない。そこで、本第 1参考例では、初期（時刻 Ts) 上体姿勢角は、初期（時刻 Ts) 基準上体姿勢角の値に決定し、これと、これを定常歩容における口ポット 1のトータルの旋回角（鉛直軸回りの旋回角）に応じたマトリクス（回転座標変換のマトリクス）により次回歩容支持脚座標系から見た値に座標変換したものとをそれぞれ式 3 7 aの左辺の初期上体姿勢角、終端上体姿勢角に代入する。また、式 3 7 a， 3 7 bのそれぞれの右辺第 1項の積分に係る上体姿勢角加速度は、前記図 2 6の S 5 1 8で求められたものが用いられる。

そして、式 3 7 a , 3 7 bの初期上体姿勢角速度と、式 3 7 a， 3 7 bの右辺第 2項の積分に係る Z M P rec (図 3 0の台形状のパターン）の台形高さをと未知数として (但し、 Z M P rec の台形パターンの折れ点の時刻は前記のごとくあらかじめ定める。また、第 1旋回歩容の Z M P rec の台形高さ acycl と第 2旋回歩容の Z M P rec の台形高さ acyc2 を同一値とする。）、それらの未知数を含む式 3 7 a , 3 7 bの連立方程式を解くことにより求められる初期上体姿勢角速度を新たな初期上体姿勢角速度として決定する。この場合、式 3 7 bの終端上体姿勢角速度は，未知数である初期上体姿勢角速度を、定常歩容の上記トータルの旋回角に応じたマトリクスにより次回支持脚座標系から見た値に座標変換したものである。

次いで S 3 1 2に進み、上体傾斜復元モ一メント Z M P換算値（Z M P rec) パターンを基に、これによる上体水平位置 · 速度への影響量を求め、終端上体水平位置 ·速度に加算する。

この処理について解説する。なお、その詳細は、本願出願人の P C T Z J P 0 2 1 3 5 9 2にて説明されているので、' ここでは簡略な説明に留める。時刻 Tsから Tm の期間および時刻 Ts2から Te の期間において、上記のごとく上体傾斜復元モーメント Z MP換算値（ Z MPrec) パターンを発生するように上体姿勢角加速度 /3を変化させると、上体姿勢角加速度 βは、次式のようになる。 β =— ZMPrecネ FZ/ ΔΜ Γ …式 1 0 2 5 上体傾斜復元モーメントを発生させない場合に目標 Z MPを満足する上体水平加速度は、 S 5 3 2において求めたごとく atmp であるが、上記のごとく上体姿勢角加速度 iSを変化させると、目標 Z MPを満足するために必要な上体水平加速度は、次式のようになる。 = a t v- Δ M r / Δ Mp ) * β 式 1 0 2 6 · 式 1 0 2 5および 1 0 2 6より、 , a = a tmp + Z M P rec * Fz/ Δ M p …式 1 0 2 7 すなわち、上体傾斜復元モーメント Z M P換算値（ Z M Prec) によつて、加速度が式 1 0 2 7の右辺第 2項分だけ増加する。

式が線形であることを利用すると、上記のごとく上体傾斜復元モーメント Z M P換算値（Z M Prec) パターンを発生するように上体姿勢角加速度 )3を変化させた場合の終端上体水平速度は、上体傾斜復元モーメント Z MP換算値（ Z M Prec) パターンを発生させなかった場合の終端上体水平速度、すなわち S 4 1 4で求めた上体水平速度の終端値に、時刻 Tsから Te までの（Z MPrec* FzZAMp) の 1階積分を加算したものとして求められる。また、上記のごとく上体傾斜復元モーメント Z M P換算値（ Z M P rec) パターンを発生するように上体姿勢角加速度 /3を変化させた場合の終端上体水平位置は、上体傾斜復元モーメント Z M P換算値（ Z M P rec) パターンを発生させなかった場合の終端上体水平位置、すなわち S 4 1 4で求めた上体水平位置の終端値に、時刻 Ts から Te までの（ Z M P rec * FzZ Δ Μ ρ ) の 2階積分を加算したものとして求められる。

S 3 1 2の処理を完了した後、 S 3. 1 4に進み、逆位相腕振り角速度が初期と終端で一致するように逆位相腕振り復元角加速度（ arec) パターンを決定する。

具体的には、逆位相腕振り復元角加速度パターンを図 3 6のように台形状に設定し、時刻 Tmから Ts2 までの期間の台形高さ azcyc2 と時刻 Tm2 から Te までの期間の台形高さ azcycl を同一とし、時刻 Ts から Te までの i3 arec の積分値と床反力モーメント鉛直成分 Mz が許容範囲を越えないようにするための前記求めた逆位相腕振り加速度 /3 a の積分値との和が 0となるように台形高さ azcycl、 azcyc2 を決定する。なお. 両区間の台形高さは、同一でなくても構わない。

補足すると、このように決定した逆位相腕振り復元角加速度パターンによって発生する床反力モーメント鉛直成分（Mazrec) は、図 3 7のようになる。したがって、逆位相腕振りを含む口ポットの運動によって発生する床反力モーメント鉛直成分 Mzは、最終的には、図 3 8に示すごとく、図 3 2の Mztmp と図 3 4の Mazと図 3 7の Mazrecとの和、すなわち、図 3 3の Mz と図 3 7の Mazrec との和になる。時刻 Tmから Ts2 までの期間および時刻 Tm2 から Te までの期間においては、台形状の復元モーメントが加算されるが、これらの期間は、許容範囲が十分に広い期間に設定されているので、逆位相腕振りを含むロポッ卜の運動によって発生する床反力モーメント鉛直成分は、許容範囲を越えることはない。

次に S 3 1 6に進み、定常歩容の初期（時刻 Ts) 逆位相腕振り角および角速度を決定する。

具体的には、初期逆位相腕振り角速度は、以下の式によって決定する初期逆位相腕振り角速度

=基準初期逆位相腕振り角速度

- ( /3 arecが 0の場合の逆位相腕振り角

+ )3 arecパターンの 2階積分）ノ Tcyc

…式 1 0 3 0 ただし、上式において、 iS arec が 0の場合の逆位相腕振り角とは、 S 4 1 6において求めた逆位相腕振り角（時刻 Te における逆位相腕振り角）のことである。また、 β arec の 2階積分とは、図 3 6のように設定した逆位相腕振り復元角加速度の時刻 Ts から Te までの 2階積分値である。また、基準初期逆位相腕振り角速度は前記基準逆位相腕振り角速度（基準逆位相腕振り角 0 aref の 1階微分値）の時刻 Ts での値である

初期逆位相腕振り角は、基準初期逆位相腕振り角に一致させれば良い, または、最終的に決定された逆位相腕振り角加速度（すなわち、床反力モーメント鉛直成分 Mzが許容範囲を越えないようにするための前記求めた逆位相腕振り加速度 j8 aと復元角加速度) 3 arecの和）と上記求めた初期逆位相腕振り角速度を基に、仮に初期逆位相腕振り角を基準初期逆位相腕振り角に一致させた場合に算出される腕振り角と基準逆位相腕振り角との差の平均値または前記差の最大値と最小値の平均値を求め、求めた平均値の 2分の 1を基準初期逆位相腕振り角から減じた値を最終的な初期逆位相腕振り角として決定しても良い。このようにすることで、算出される腕振り角と基準逆位相腕振り角との差の絶対値が大きくなり過ぎることを防止することができる。

時刻 Ts、 Tm、 Ts2 および Tm2 を前記のごとく設定した理由のひとつは、時刻 Tmから Ts2 までの期間と時刻 Tm2から Te までの期間とにおいて、上体姿勢角速度を基準上体姿勢軌道の初期角速度に戻すように上体姿勢角加速度を発生させても、床反力水平成分 Fx が許容範囲 [Fxmin, Fxmax]を越えないようにするためである。換言すれば、時刻 Tmから Ts2までの期間と時刻 Tm2から Teまでの期間とにおいては、床反力水平成分許容範囲が十分に大きいため、目標 Z M Pを満足しつつ. 上体姿勢角速度を'戻すように上体姿勢角加速度 3を発生させても、床反力水平成分 F xは、許容範囲を越えることがない。

また時刻 Ts、 Tm、 Ts2 および Tm2 を前記のごとく設定したもうひとつの理由は、時刻 Tmから Ts2 までの期間と時刻 Tm2から Te までの期間とにおいて、逆位相腕振り角速度を基準逆位相腕振り角軌道の初期角速度に戻すように逆位相腕振り角加速度) 3 a を発生させても、床反力モーメント鉛直成分 Mzが許容範囲 [Mzmin, Mzmax]を越えないようにするためである。換言すれば、時刻 Tm から Ts2 までの期間と時刻 Tm2から Te までの期間とにおいては、床反カモ一メント鉛直成分許容範囲が十分に大きいため、逆位相腕振り角速度を戻すように逆位相腕振り角加速度 j3 a を発生させても、床反力モーメント鉛直成分 Mz は、許容範囲を越えることがない。

以上のごとく図 2 4の S 3 1 6の処理が完了した後、図 2 3の S 2 1 0に進み、生成した歩容（仮定常歩容）の終端上体水平位置 ·速度を、その瞬間の支持脚に対応する支持脚座標系（図 1. 7の Χ "'， Υ '" ， Ζ'" 座標系）から見た値に変換し、その値を（X e， V e ) とする（X e ：終端上体水平位置、 V x e ：終端上体水平速度）。

次いで S 2 1 2に進み、図示の如く、初期上体水平位置速度（X s , V x s ) と終端上体水平位置速度（X e , V e ) との差を算出する。この差（X s — X e， V x s - Vx e ) を上体水平位置速度境界条件誤差（errx, errvx) と呼ぶ。定常歩容では、前記境界条件を満たす必要があるので、（X s， V X s ) と（X e， V e ) とが一致していなければならない。従って、上体水平位置速度境界条件誤差（errx, errvx) が零またはほぼ零でなければならない。本第 1参考例では、以下のように、上体水平位置速度境界条件誤差（errx, errvx) がほぼ零となる（X s， V x s ) が探索的に求められる。

次いで S 2 1 4に進み、算出した上体水平位置速度境界条件誤差 (errx, errvx) が、あらかじめ適宜設定された許容範囲内にあるか否かが判断される。尚、このように上体水平位置速度境界条件誤差の許容範囲を設定する代わりに、初期発散成分（X s + V x s /ωΟ) と終端発散成分 (X e + V x e /ωΟ) との差、および初期収束成分 (X s — V X s /ωθ') と終端収束成分 (X e - V x e /ωθ') との差がそれぞれある許容範囲内にあるか否か判断するようにしても良い。ただし ω 0， ωθ' は、前述したように、ある所定の値である。

S 2 1 4の判断結果が Ν Οであるときには S 2 1 6に進む。この S 2 1 6では、（X s， V X s ) の付近に複数（本第 1参考例では 2つ）の初期値候補（X s + A X s , V X s ), (X s , V x s + A V x s ) が決定される。ここで、 A X s , A V x sは、それぞれ X s， V x sに対する所定の微小変化量を意味している。そして、これらの初期値候補のそれぞれを上体水平位置速度の初期状態として、前記 S 2 0 8と同様の処理によって定常歩容が歩容パラメータを用いて生成される。さらに、その生成された定常歩容の終端上体位置速度を、その瞬間の支持脚に対応する支持脚座標系（図 1 7の X"', Υ'" ， Ζ'"座標系）から見た値に変換した値（X e + A X e l， V x e +厶 V x e l ), ( X e + Δ X e 2 , V x e + A V x e 2 ) が求められる。ここで、（X e + A X e l， V x e + Δ V x e 1 ) は（X s + A X s， V x s ) に対応する終端上体位置速度を意味し、（X e + A X e 2， V x e + A V x e 2 ) は（X s， V x s + Δ V x s ) に対応する終端上体位置速度を意味している。なお、この場合の定常歩容（仮歩容）の生成処理では、上体水平位置速度以外の変数の初期状態（時刻 T sでの状態）は、例えば上体水平位置速度の初期値候補を（X s , V X s ) とした場合と同一に設定すればよい。 S 2 1 6ではさらに、前記 S 2 1 0と同様の処理によって、各初期値候補とこれに対応する終端上体位置速度との差、すなわち、各初期値候補 (X s + Δ X s , V s ), (X s , V X s + Δ V X s ) のそれぞれに対応する上体水平位置速度境界条件誤差が求められる。

次いで S 2 1 8に進み、（X s , V X s ) およびその付近の初期値候補（X s + A X s， V X s ), (X s , V x s.+ A V x s ) のそれぞれに対する上体水平位置速度境界条件誤差を基に、（X s， V X s ) の次の初期値候補が探索法（擬似ヤコビアン（感度マトリックス）を求め、最急降下法などにより次候補を決定する方法や、シンプレックス法など）により決定される。すなわち、（X s， V s ) およびその付近の初期値候補（X s + A X s , V X s ), (X s， V x s + Δ V x s ) のそれぞれに対する上体水平位置速度境界条件誤差によって、上体水平位置及び上体水平速度をそれぞれ初期値候補（X s , V X s ) から微小変化させたときの、上体水平位置速度境界条件誤差の変化度合いを示す感度マトリックスが求められ、その感度マトリックスに基づいて、上体水平位置速度境界条件誤差をより小さくするような初期値候補（X s， V x s ) が新たに決定される。そして、このように上体水平位置速度の新たな初期値候補（X s， V X s ) が決定された後、 S 2 0 6に戻る。

S 2 1 4の判断結果が NOとなる限りは上記した処理（S 2 0 6〜S 2 1 8の処理）が繰り返される。なお、この場合、上体水平位置速度の新たな初期値候補（X s， V X s ) に対応する定常歩容の生成処理（S 2 0 8 ) における S 3 0 0 (図 2 4参照）では、上体姿勢角速度の初期値は、基準上体姿勢角速度の初期値ではなく、上体水平位置速度の前回の初期値候補（X s， V s ) に対応する S 2 0 8の処理における S 3 1 0 (図 2 4参照）で決定したものに設定される。そして、 S 2 1 4の判断結果が YE Sになると、繰り返しループ（S 2 0 4) を抜けて S 2 2 0に進む。なお、 S 2 0 4の繰り返しループを抜けたときの直前に生成された仮定常歩容が、境界条件を満たす定常歩容として得られることとなる。

S 2 2 0では、本来の初期時刻 0 (今回歩容の終端時刻) における初期上体水平位置速度（X0， V0) と初期時刻 0における初期上体鉛直位置速度（ Z0、 V z 0) と初期時刻 0における初期上体姿勢角および角速度が求められる。

具体的には、（X0， V0) と（Z0， V z 0) は、 S 3 1 0において決定された上体傾斜復元モーメント Z MP換算値パターン、時刻 Ts における定常歩容の初期上体姿勢角および角速度と、 S 2 0 4のループを抜けた後の時刻 Ts における上体水平位置速度（Xs,Vxs)を基に、歩容の条件を満足するように歩容を生成した場合における、第 2旋回歩容から第 1旋回歩容に切り換わった瞬間の時刻、すなわち、時刻 k二 Tcyc (時刻 Te— Ts) であるときに決定される上体水平位置速度と上体鉛直位置速度とを、それぞれ時刻 Tcyc から始まる一歩（すなわち 2度目の第 1 旋回歩容）の支持脚に対応する支持脚座標系（図.1 7の Χ"'， Υ"'， Ζ'" 座標系）から見た値に変換した値に決定される。同様に、初期状態姿勢角および角速度は、時刻 k =Tcyc (時刻 Te— Ts) であるときに決定される上体姿勢角及び角加速度を、時刻 Tcyc から始まる一歩（すなわち 2度目の第 1旋回歩容）の支持脚に対応する支持脚座標系（図 1 7の X "', Υ'"， Ζ'"座標系）から見た値に変換した値に決定される。

次いで S 2 2 2に進み、定常歩容初期発散成分 q [ 0 ] が次式から求められる。 q [0]=X0+ VO/ωΟ …式 4 0

ただし ωθ は、発散についての解説で述べたように、ある所定の値である。

次に S 2 2 4に進み、定常歩容初期発散成分 d [ 0 ] が今回歩容支持脚座標系から見た値に変換され、これが q " [ 0 ] として求められる。また、初期上体鉛直位置速度（Z 0 , V z 0 ) が今回歩容支持脚座標系から見た値に変換され、これが（Z0 "， V z 0 ") として求められる。 · 補足すると、（ Z 0 "， V z 0 ") は、第 2旋回歩容の支持脚座標系 (図 1 7の X"， Y"， Ζ" 座標系）から見た第 2旋回歩容終端上体鉛直位置速度に一致している。また、 Q " [ 03 も第 2旋回歩容の支持脚座標系（図 1 7の Χ"， Υ", Ζ" 座標系）から見た第 2旋回歩容終端発散成分に一致している。したがって、これらの性質を用いて、（Ζ 0 "， V ζ 0 ") および Q" [ 0 ] を算出するようにしても良い。

さらに、 S 2 2 6に進み、本来の初期時刻 0 (今回歩容の終端時刻）における初期逆位相腕振り角および角速度（0 azO， coazO)を求め、さらに、これを今回歩容の支持脚座標系から見た値である（0 az0"， coazO") を求める。具体的には、（0 az0， ω azO)は、 S 3 1 4および S 3 1 6において決定された逆位相腕振り復元角加速度パタン、定常歩容の初期 (時刻 Ts) 逆位相腕振り角および角速度とを基に、歩容の条件を満足するように歩容を生成した場合における（より具体的には、上体姿勢角 ·逆位相腕振り角復元期間以外では、床反カモ一メント鉛直成分が許容範囲を越えないように、逆位相腕振り角軌道を決定し、上体姿勢角 · 逆位相腕振り角復元期間では、前記基準逆位相腕振り角加速度 )3 aref と逆位相腕振り復元角加速度 3 arec の和が発生するように逆位相腕振り角軌道を決定した場合における）第 2旋回歩容から第 1旋回歩容に切り換わった瞬間の時刻、すなわち、時刻 k = Tcyc (時刻 Te— Ts) であるときに決定される逆位相腕振り角および角速度を、時刻 Tcyc から始まる一歩（すなわち 2度目の第 1旋回歩容）の支持脚に対応する支持脚座標系（図 1 7の Χ '", Υ '", Z"'座標系）から見た値に変換した値に決定される。

以上により、図 1 3の S 0 2 4の処理、すなわち定常歩容の初期状態を求めるサブルーチン処理が終了する。

次いで、図 1 3の S O 2 6に進み、今回歩容の歩容パラメ一夕を決定 (一部は仮決定) する。 S O 2 6では、より具体的には、図 3 9に示すフローチャートに従って、以下の処理が行われる。

まず、 S 6 0 0において、今回歩容の足平位置姿勢軌道が定常歩容の足平位置姿勢軌道につながるように、今回歩容の足平軌道パラメータが設定される。

具体的には、今回歩容初期遊脚足平位置姿勢（今回歩容遊脚足平位置姿勢の初期値）は、今回歩容支持脚座標系から見た現在遊脚位置姿勢 (前回歩容終端遊脚位置姿勢）に設定される。今回歩容初期支持脚足平位置姿勢（今回歩容支持脚足平位置姿勢の初期値）は、今回歩容支持脚座標系から見た現在支持脚足平位置姿勢（前回歩容終端支持脚足平位置姿勢）に設定される。また、今回歩容終端遊脚足平位置姿勢は、今回歩容支持脚座標系から見た次回歩容支持脚座標系（今回歩容に係わる 1歩目の遊脚着床位置姿勢の要求値）に対応して決定される。すなわち、今回歩容終端遊脚足平位置姿勢から遊脚足平 2 2を床に接触させたまま、すべらないように該足平 2 2をその底面のほぼ全面が接地するまで回転させたときの、該足平 2 2の代表点が今回歩容支持脚座標系から見た次回歩容支持脚座標系の原点に合致するように今回歩容終端遊脚足平位置姿勢が決定される。

今回歩容終端では、支持脚足平 2 2は離床して空中にある。支持脚足平 2 2が離床してから後の軌道を決定するために、支持脚足平着地予定位置姿勢を設定する。支持脚足平着地予定位置姿勢は、今回歩容支持脚座標から見た次次回歩容支持脚座標（今回歩容に係わる 2歩目の遊脚足平位置姿勢の要求値）に対応して設定される。より具体的には、支持脚足平着地予定位置姿勢は、その位置姿勢から、足平 2 2を床に接触させたまま、すべらないように該足平 2 2をその底面のほぼ全面が床に接地するまで回転させたときの該足平 2 2の代表点が今回歩容支持脚座標から見た次次回歩容支持脚座標の原点に合致す.るように設定される。

今回歩容終端支持脚足平位置姿勢は、現在支持脚位置姿勢（今回歩容初期支持脚足平位置姿勢）から、次回歩容支持脚座標系に対応する足平着地予定位置姿勢（前記要求パラメ一夕における 2歩目の遊脚側足平着地位置姿勢の要求値）に至る足平位置姿勢軌道を、今回歩容終端まで前記有限時間整定フィル夕により生成することにより求められる。

次いで、 S 6 0 2に進み、今回歩容の基準上体姿勢軌道パラメ一夕が、定常歩容の第 1旋回歩容ゃ第 2旋回歩容と同様に決定される。ただし、今回歩容の基準上体姿勢軌道が前記定常歩容の基準上体姿勢軌道に連続してつながる（今回歩容終端での基準上体姿勢角及び角速度がそれぞれ定常歩容初期の基準上体姿勢角及び角速度に一致する）ように上記パラメータが設定される。なお、本第 1参考例では、傾斜角に関する基準上体姿勢は、今回歩容及び定常歩容のいずれでも定常的な鉛直姿勢である, 次いで、 S 6 0 4に進み、今回歩容の基準腕姿勢軌道パラメ一夕が、定常歩容の第 1旋回歩容ゃ第 2旋回歩容と同様に決定される。ただし、今回歩容の初期基準腕姿勢およびその変化率が基準腕姿勢および変化率の現在瞬間値に一致し、かつ、今回歩容の腕姿勢軌道が前記定常歩容の腕姿勢軌道に連続してつながるように上記パラメ一夕を設定する。なおここで決定される腕姿勢軌道パラメ一夕は、定常歩容パラメ一夕の決定 (図 1 5の S 1 0 4 ) の場合と同様、 .たとえば、両腕体 5， 5の全体の重心位置（上体 3に対する相対位置）、左右の手先（両腕体 5， 5の先端）の左右方向の間隔および逆位相腕振り角に関するパラメ一夕が決定される。尚、本第 1参考例では、両腕体 5 , 5の全体の重心位置は、上体 3に対して一定に維持されるように設定される。

次いで、 S 6 0 6に進み、今回歩容の床反力鉛直成分軌道パラメ一夕が、定常歩容の第 1旋回歩容ゃ第 2旋回歩容と同様、該パラメータにより規定される床反力鉛直成分軌道が前記図.6のように実質的に連続な (値がステップ状に飛ばない）軌道になるように設定される。

ただし、床反力鉛直成分軌道パラメ一夕は、今回歩容の全体重心鉛直位置速度と床反力鉛直成分軌道とのいずれもが、前記定常歩容と連続してつながるように決定される。

具体的には、まず、前記図 1 3の S 0 2 4の処理（定常歩容初期状態の決定処理）で最終的に求めた定常歩容の初期上体鉛直位置速度を今回歩容支持脚座標系から見た値に変換した値（Z 0 "， V z 0 ")、すなわち、図 2 3の S 2 2 4で求めた（Z 0 "， V z 0 ") 等を基に、例えば前記式 0 4 (あるいは口ポット 1のキネマテイクスモデル）を用いて、今回歩容支持脚座標系から見た定常歩容の初期全体重心鉛直位置速度が求められる。具体的には、今回歩容支持脚座標系から見た定常歩容の初期全体重心鉛直位置は、 S 2 2 4で求めた定常歩容の上体鉛直位置 Z0" に対応する図 1 2のモデルの上体質点鉛直位置と、定常歩容初期の各足平位置を今回歩容支持脚座標系から見た値に変換したものに対応する支持脚側及び遊脚側の脚体質点鉛直位置とを式 0 4に代入することで求められる。また、今回歩容支持脚座標系から見た定常歩容の初期全体重心鉛直速度は、 S 2 2 4で求めた定常歩容の上体鉛直速度 VzO" に対応する図 1 2のモデルの上体質点鉛直速度と、定常歩容初期の各足平鉛直速度を今回歩容支持脚座標系から見た値に変換したものに対応する支持脚側及び遊脚側の脚体質点鉛直速度とを式 0 4の両辺を微分してなる式に代入することにより求められる。なお、この初期全体重心鉛直位置速度の算出は、より厳密なモデルを用いて行なうようにしてもよい。

そして、このようにして求めた定常歩容の初期全体重心鉛直位置速度を、次式 4 1 a、 4 1 bの終端全体重心鉛直位置速度に代入すると共に, 前回の目標歩容瞬時値（より厳密には、前回目標歩容の終端状態を今回支持脚座標系に変換したもの）の全体重心鉛直位置および速度を、式 4 1 a、 4 1 bの初期全体重心鉛直位置および速度に代入し、式 4 1 a、 4 1 bの関係を満足するように、今回歩容の床反力鉛直成分パターン (より具体的にはパラメ一夕値）が決定される。ただし、式 4 1 a、 4 . 1 bにおける積分値は今回歩容の初期から終端までの期間の積分値とする。終端全体重心鉛直位置一初期全体重心鉛直位置

= (床反力鉛直成分 Z口ポットの全体質量）の 2階積分

+重力加速度の 2階積分

+初期全体重心鉛直速度 * 1歩の時間

…式 4 1 a 終端全体重心鉛直速度 -初期全体重心鉛直速度

= (床反力鉛直成分/口ポットの全体質量）の 1階積分

+重力加速度の 1階積分

…式 4 1 b ただし、重力加速度は負の値とする。

より具体的には、まず、図 6のような床反力鉛直成分パターンを規定する床反力鉛直成分パラメ一夕（折れ点の時刻等）のうちの、少なくとも 2つのパラメ一夕を独立な未知変数として、その未知変数の値を、式 4 1 a、 4 1 bからなる連立方程式を解いて決定する。

未知変数とする床反力鉛直成分パラメ一夕には、例えば、図 6の台形の高さ（床反力鉛直成分のピーク値）と幅（片脚支持期時間）とを選択すれば良い。この場合、図 6の台形の両側部分の傾きは今回歩容周期等に応じてあらかじめ定めた値とし、あるいは、片脚支持期から空中期に移行する時刻を除く床反力鉛直成分パターンの折れ点の時刻を今回歩容周期等に応じてあらかじめ定めた値とする。補足すると、未知変数を 1 つとすると、一般的には式 4 1 a、 4 1 bの連立方程式を満足する解が存在しない。

次いで、 S 6 0 8に進み、床反力水平成分許容範囲 [Fxniin, Fxmax] (具体的には該床反力水平成分許容範囲のパターンを規定するパラメ一夕）が、定常歩容の第 1旋回歩容ゃ第 2旋回歩容と同様に設定される。たとえば図 4 0に示すようなパターンで床反力水平成分許容範囲が設定される。本第 1参考例では、先に S 6 0 6で決定した床反力鉛直成分パターンに応じて前記式 1 2に基づいて床反力水平成分許容範囲が設定される。

次いで、 S 6 1 0 に進み、床反力モーメント鉛直成分許容範囲 [Mzmin, Mzmax] (具体的には該床反力モーメント鉛直成分許容範囲のパターンを規定するパラメ一夕）が、定常歩容の第 1旋回歩容ゃ第 2 旋回歩容と同様に設定される。たとえば図 4 1に示すようなパターンで床反力モーメント鉛直成分許容範囲が設定される。本第 1参考例では、先に S 6 0 6で決定した床反力鉛直成分パターンに応じて前記式 1 0 1 2に基づいて床反力モーメント鉛直成分許容範囲が設定される

次いで、 S 6 1 2に進み、今回歩容の Z M P軌道（具体的には Z M P 軌道を規定するパラメータで、軌道の折れ点の時刻や位置）が、定常歩容の第 1旋回歩容ゃ第 2旋回歩容と同様、安定余裕が高くかつ急激な変化をしないように、前記図 7に示すごとく設定される。ただし、今回歩容の Z MP軌道が前記定常歩容の Z MP軌道に連続してつながるように上記パラメ一夕を設定される。つまり、今回歩容終端における Z MPの位置が定常歩容初期の Z MP位置に一致するように、 Z MP軌道パラメ一夕が決定される。この場合、走行歩容では、片脚支持期における ZM P軌道の折れ点の時刻や位置の設定の仕方は、前述した定常歩容の Z M P軌道パラメ一夕の設定の仕方と同様でよい.。そして、空中期における目標 Z MP軌道が、空中期開始時から、定常歩容初期の Z MP位置まで. 直線的に連続して変化するように Z MP軌道パラメータを設定すればよい。

なお、 S 6 1 2で決定される今回歩容の ZMP軌道パラメ一夕は、仮決定されただけであり、後述するように修正される。そこで、上記のごとく設定した今回歩容の ZMP軌道を、以降、今回歩容の仮目標 Z MP 軌道と呼ぶこととする。

最後に、 S 6 1 4に進み、上体姿勢角，逆位相腕振り角復元期間 [Ta， Tb]が設定される。上体姿勢角，逆位相腕振り角復元開始時刻 Ta は、定常歩容の第 2旋回歩容における Tmに相当し、上体姿勢角 · 逆位相腕振り角復元修了時刻 Tbは、定常歩容の第 2旋回歩容における Ts2 に相当するものである。これらの時刻 Ta, Tb の設定の仕方は、それぞれ Tmおよび Ts2の設定の仕方と同様である。

図 1 3の説明に戻って、上記のごとく S 0 2 6に示す処理（今回歩容の歩容パラメ一夕決定処理）を行って後、次いで S O 2 8に進み、今回歩容の歩容パラメ一夕（Z M P軌道パラメータ）が修正され、また、逆位相腕振り角に関するパラメータが決定される。この処理では、上体位置姿勢軌道を定常歩容に連続させ、もしくは近づけるべく Z M P軌道パラメ一夕が修正され、かつ、逆位相腕振り角が定常歩容の逆位相腕振り角軌道に収束するように今回歩容の逆位相腕振り角に関するパラメ一夕が決定される。

図 4 2はその処理を示すサブルーチン · フロー ' チャートである。まず S 7 0 0を経て S 7 0 2に進み、仮目標 Z M Pパターンとその他の今回歩容パラメータを基に、今回歩容終端時刻までの仮今回歩容が仮生成される。

S 7 0 2では、図 4 3に示すフローチヤ一.卜に従って、以下の処理が行われる。

以下説明すると、 S 8 0 0において、諸々の初期化が行われる。具体的には、仮歩容生成用時刻 k に 0が代入される。また、前回目標歩容の終端状態（より具体的には、上体水平位置速度、上体鉛直位置速度、上体姿勢角及びその角速度、目標足平位置姿勢、目標腕姿勢などの歩容状態の終端値）を今回支持脚座標系に変換したものを、今回歩容初期状態とする。

補足すると、目標腕姿勢は、目標逆位相腕振り角 · 角速度を含む。次いで、 S 8 0 2を経て S 8 0 4において、仮歩容生成用時刻 k が今回歩容終端時刻 Tcui'r 以前であるか否か（k^ Tcurr であるか否か）が判断され、その判断結果が Y E Sある場合には、 S 8 0 6の今回歩容瞬時値決定サブルーチンに進み、今回歩容の時刻 k の瞬時値が決定される。 S 8 0 6の歩容瞬時値決定サブルーチンでは、前述の S 3 0 6と同様、図 2 5に示すごとく仮歩容を生成する。ただし、歩容パラメ一夕として、定常歩容パラメ一夕の代わりに今回歩容パラメ一夕が用いられる。

次いで S 8 0 8に進み、仮歩容生成用時刻 kがだけ増加され、 S 8 0 4に戻る。

S 8 0 4の判断結果が N Oである場合には、図 4 3に示すフローチヤ一トの処理が完了する。

以上の処理により、仮の今回歩容が初期から終端まで生成される。次いで S 7 0 4に進み、上記のごとく S 7 0 2で求めた今回歩容終端での上体水平位置速度（X e， V X e ) から、終端発散成分 q 0[k] (k=Tcurr) が図示の式（前記式 1 0 ) を用いて求められる。

次いで S 7 0 6に進み、今回歩容終端発散成分 Q 0[k]と定常歩容初期発散成分 q " (図 2 3の S 2 2 4で求めたもの) との差である終端発散成分誤差 errq が図示の式を用いて求められる。さらに、 S 7 0 8に進んで、その求めた終端発散成分誤差 errq が許容範囲内（ 0近傍の範囲）にあるか否か判断する。

S 7 0 8の判断結果が N Oであるときは S 7 1 0に進み、 a = A a ( Δ aは所定の微小量）とし、図 4 4の関係に従って現在の仮目標 Z M Pパターンに台形状の修正を加えた目標 Z M Pを基に、前記 S 7 0 2と同様に、終端までの仮今回歩容を計算する。ここで、図 4 4を参照して、 aは、今回歩容終端発散成分を定常歩容初期発散成分にできるだけ一致させる（今回歩容の上体水平位置姿勢軌道を定常歩容の上体水平位置姿勢軌道に近づける）ために、仮目標 Z M Pを修正するための台形パ夕一ンの高さである。この場合、本第 1参考例では、仮目標 Z M Pの修正は、支持脚足平 2 2の底面のほぼ全面が接地する期間（足底全面接地期間）すなわち、床反力水平成分許容範囲が十分に広いものとなる期間において行なうようにしており、上記台形パターンの折れ点の時刻は、足底全面接地期間における仮目標 ZM Pの折れ点の時刻に合わせて設定されている。なお、 S 7 1 0で a = z\ aとするのは、現在の仮目標 Z MP軌道を上記台形パターンによって微小量、修正したときの前記終端発散成分誤差 errqの変化を観測するためである。

上記のように S 7 1 0にて、 a = A aとして仮目標 ZMP軌道を修正したときの仮今回歩容を終端まで生成した後、さらに S 7 1 2に進み、 S 7 1 0で求めた仮今回歩容終端での上体水平位置速度（X e 1， V x e l) を基に、この仮今回歩容における終端発散成分 Q l[k]が図示の式 (前記式 1 0 ) を用いて求められる。

尚、 S 7 1 0において Δ aは、本第 1参考例では、適宜に設定される微小量の定数としたが、以下に説明する繰り返し演算によって終端発散成分誤差 errq が小さくなるにつれて Δ aを減少させるように該 Δ aを設定するようにしても良い。ただし、定数にしておいても、数回の繰り返し演算で終端発散成分誤差 _errq を許容範囲内に収めることができる < 次いで S 7 1 4に進み、パラメ一夕感度 r (Δ aに対する終端発散成分誤差の変化の割合）が図示の式から求められる。さらに、 S 7 1 6に進み、 a =— errqZr、すなわち S 7 0 6で求めた終端発散成分誤差 errq を、 S 7 1 4で求めたパラメータ感度 rで割った値を高さ aとする台形パターンの修正量を図 44の関係に従って仮目標 Z MPパターンに加えることにより、仮目標 ZMPパターンが修正される（新たな仮目標 Z MPパターンが決定される）。

次いで S 7 0 2に戻る。そして、 S 7 0 8の判断結果が N Oである限りは、上記した S 7 0 2〜S 7 1 6の処理が繰り返され、 S 7 0 8の判断結果が YE Sになったときには、その繰り返しループ（S 7 0 0 ) を抜けて S 7 1 8に進む。

S 7 1 8では、仮今回歩容の終端上体姿勢角と定常歩容の初期上体姿勢角との差、および仮今回歩容の終端上体姿勢角速度と定常歩容の初期上体姿勢角速度との差などを基に、今回歩容の上体姿勢角軌道が定常歩容の上体姿勢角軌道に近づくように、今回歩容の上体姿勢復元モーメント Z MP換算値（Z MPrec) のパターンが決定される。ここで決定する ZMPrec は、後述する今回歩容瞬時値の生成処理で、床反力水平成分許容範囲が十分に大きいものとなる期間（片脚支持期内の期間）において、上体姿勢角軌道を定常歩容につながらせる（近づける）ように上体姿勢角加速度を発生させても、今回歩容終端発散成分と定常歩容初期発散成分との一致性（S 7 0 8の条件）を維持できるように、仮目標 Z

MPを修正するためのものである。

この ZMPrec は、前記定常歩容生成処理で説明したものと同様に台形状のパターンであり、具体的には次のように決定される。すなわち、図 3 0における第 2旋回歩容の期間の Z M Prec の台形パターンと同様に、今回歩容の Z MPrec の台形パターンを設定し、台形の頂点（折れ点）の時刻は既知とする（より具体的には台形の折れ点の時刻を目標 Z

MPの折れ点時刻に合わせる）と共に、台形高さを未知数とし、以下のように ZMPrec の台形高さ（パラメ一夕）が求められる。ただし、 Z

MPrec の台形パターンが立ち上がり始める時刻を Ta とし、台形パ夕ーンから 0に戻る時刻を Tb とする。

上記のように上体姿勢復元モーメント Z M P換算値パターンの未知パラメ一夕がひとつの場合には、今回歩容の終端において上体姿勢角と上体姿勢角速度の両方を定常歩容に連続につなげることは一般的にはで.きない。そこで、本第 1参考例では、複数歩に渡って徐々に、生成される歩容の状態が定常歩容の状態に近づけるように、未知パラメ一夕を決定することとする。

補足すると、 1歩容の中での Z M Prec パターンを複雑にし、未知パラメ一夕を 2つ以上にして、今回歩容の終端において上体姿勢角と上体姿勢角速度との両者を定常歩容に連続につなげても良いが、 Z M P rec パターンがジグザグに大きく変動し過ぎる恐れがある。、

以下では、算出原理を説明した後、算出手順を説明する。

前述のごとく S 7 0 2で Z M Precノ、。夕一ンの台形高さを 0 として求めた仮今回歩容の終端上体姿勢角と定常歩容の初期上体姿勢角との差を求め、これを 0 err とする。また、仮今回歩容の終端上体姿勢角速度と定常歩容の初期上体姿勢角速度との差を求め、ごれを _V0 _errとする。

ここで、 Z M Precパ夕一ンの台形高さをある値 bcurr として今回歩容が生成され、引き続いて、今回歩容と同様のアルゴリズムによって第 1旋回歩容が生成されると想定する。ただし、第 1旋回歩容の上体姿勢復元モ一メン卜 Z M P換算値 Z M Prec パダーンは、図 2 4の S 3 1 0 において求められた第 1旋回歩容の Z M P rec パターン (高さが前記 acyclである図 3 0の台形パターン) とある値 blの和であるとする。

このようにして生成された歩容を Z M Prec 修正歩容と呼び、その終端（第 1旋回歩容の終端）上体姿勢角と角速度をそれぞれ Θ 1、 νθ 1 とする。

S 0 2 4における定常歩容の初期状態を求めるサブルーチンの処理が完了した時点で求められた本来の定常歩容（最終的に S 3 1 0において決定された定常歩容初期上体姿勢角および角速度を初期値とし、 Z M P recパターンを S 3 1 0において求められたパターン（高さが acycl である図 3 0の台形パターン）とした場合の定常歩容）の第 1旋回歩容終端上体姿勢角と角速度をそれぞれ Θ lorg、 νθ lorgとする。ここで、 Δ 0 1、 Δνθ 1を次のように定義する。

Α θ 1 =θ 1— Θ lorg …式 5 0

Ανθ 1 =νθ 1-νθ lorg …式 5 1 厶 0 1および Δν01 は、 Z M Prec修正歩容と本来の定常歩容を第 1 旋回歩容の終端まで生成した時点において、両者間における上体姿勢角の差及び角速度の差を意味する。もし、 Δ 0 1 および Δ v0 1 が 0になれば、 Z M Prec 修正歩容に引き続いて、今回歩容と同様のァルゴリズムによって、 Z M Precパターンの台形高さを前記 acyc2 として第 2旋回歩容が生成されると、この歩容は、本来の定常歩容に一致する。

したがって、 Δ θ 1および Δνθ 1が 0となる今回歩容台形高さ bcurr、第 1旋回歩容台形高さ bl を求め、求めた bcurr を今回歩容の台形高さとして最終的に決定すれば良い。

- ところで、口ポット 1の上体姿勢角に関する動力学モデルが図 1 2に示すフライホイール FHx、 FHy のような線形特性を持つことから、 Δ 0 1および Δν0 1は、今回歩容台形高さ bcurr、第 1旋回歩容台形高さ bl、仮今回歩容の終端上体姿勢角と定常歩容の初期上体姿勢角の差 Θ err, 仮今回歩容の終端上体姿勢角速度と定常歩容の初期上体姿勢角速度の差 v0 errと以下の関係を持つ。

Δ Θ l = cll * bcurr+cl2* bl+ Θ err+el * _ν0 err …式 5 2

Ανθ l = c21* bcurr +c22* bl + e2* v0 err …式 5 3 だだし、 cll、 cl2、 c21、 c22、 el、 e2 は、今回歩容、第 1旋回歩容の歩容周期、上体姿勢復元モーメント Z M P換算値 Z M Prec パターンのパラメ一夕（特に時間に関するパラメ一夕）などによって一義的に決定される係数である。

以上の原理に基づき、算出手順として、まず、仮今回歩容と定常歩容との境界での上体姿勢角の差 err と角速度の差 V 0err が求められる, 次いで、式 5 2， 5 3の係数でぁる 011、 cl2、 c21、 c22、 el、 e2 が. 今回歩容、第 1旋回歩容の歩容周期、上体姿勢復元モーメント Z MP換算値 Z MPrec パターンのパラメ一夕（特に時間に関するパラメータ）などを基に求められる。

次いで式 5 2、式 5 3の右辺が 0 となるように今回歩容台形高さ bcurr, 第 1旋回歩容台形高さ bl が決定される。すなわち、式 5 2、式 5 3の左辺を 0とした連立方程式を解くことによって、 bcurr、 bl が求められる。

最後に、今回歩容の上体姿勢復元モーメント Z M P換算値（ Z MP rec) の台形パターンの台形高さが、上記求めた今回歩容台形高さ bcurrに設定される。

次いで S 7 2 0に進み、現在の仮目標 Z MPパターン（S 7 0 0の繰り返しループを抜けたときの仮目標 ZMPパターン）に、 S 7 1 8で上記のように求めた上体姿勢復元モーメント Z M P換算値パターンを加算してなるパターンが今回歩容の目標 Z MPパターンとして決定される。なお、この処理は、 S 7 1 0において Aa の高さの台形パターンを仮目標 Z MPパターンに加える処理と同様である。

以下に仮目標 Z MPパターンに上体姿勢復元モーメント Z MP換算値パターンを加算した理由を述べる。

S 7 0 0のループにおいて生成される仮今回歩容は、上体姿勢復元モ一メント Z MP換算値 ZMPrec を 0 ( Z M P rec の台形パターンの高さパラメ一夕を 0 ) にして生成されたものである。このようにして S 7 0 0のループで最終的に生成された仮今回歩容では、その上体位置速度は、定常歩容に連続し、もしくは近づくが、上体姿勢角は、定常歩容の上体姿勢角からずれ、場合によっては発散してしまう。

S 7 1 8で求められた上体姿勢復元モーメント Z MP換算値パターンは、上体姿勢角の定常歩容に対するずれを 0に近づけるための上体姿勢角加速度を発生させるものである。

しかし、 S 7 0 0のループで最終的に得られた仮目標 Z MPパターンを修正することなく、 S 7 1 8で求められた上体姿勢復元モ一メント Z MP換算値パターンに応じた上体姿勢角加速度を発生させると、動力学的平衡条件（口ポットの重力と慣性力との合力が目標 ZMPに作用するモーメントが鉛直成分を除いて 0になること）を満足させるために、上体水平位置軌道を前記仮今回歩容の上体水平位置軌道からずらさざるを得なくなる。そこで、本実施例では、上体水平位置軌道を S 7 0 0のループで最終的に得られるものからずらさなくて済むように、 Z M P rec によって仮目標 Z MPパターンを修正することとした。

前記仮今回歩容の運動に加えて、 S 7 1 8で求めた上体姿勢復元モーメント Z MP換算値パターンに対応する上体姿勢角加速度を発生させると、 Z MP (運動によって発生する重力と慣性力の合力の鉛直成分を除くモーメントが 0になる点）は上体姿勢復元モ一メント Z MP換算値だけずれる。したがって、逆に、仮目標 ZMPパターンに上体姿勢復元モ一メント Z MP換算値パターンを加算したパターンを目標 Z MPパターンとして、 S 7 1 8で求めた上体姿勢復元モーメント Z M P換算値パ夕 —ンに対応する上体傾斜モードの上体姿勢角加速度を発生させながら、目標 Z MPパターンを満足する今回歩容を生成すれば、前記仮今回歩容と同じ上体並進運動となる。 '

以上が、仮目標 Z MPパターンに上体姿勢復元モーメント Z MP換算値パターンを加算したパターンを目標 Z M Pパターンとした理由である, 次いで S 7 2 2に進み、仮今回歩容の終端逆位相腕振り角と定常歩容の初期逆位相腕振り角との差および仮今回歩容の終端逆位相腕振り角速度と定常歩容の初期逆位相腕振り角速度との差を基に、今回歩容の逆位相腕振り角軌道が定常歩容の逆位相腕振り角軌道に近づくように、逆位相腕振り復元角加速度パターンが決定される。このパターンの決定方法は、 S 7 1 8で上体姿勢復元モーメント Z M P換算値パターンを決定する方法と、変数名が次のように異なること以外は、ほぼ同じである。上体姿勢復元モーメント Z M P換算値パターン—逆位相腕振り復元角加速度パターン

水平成分—モーメント鉛直成分以下に詳細を説明する。ここで決定する逆位相腕振り復元角加速度パターンは、後述する今回歩容瞬時値の生成処理で、床反力モーメント鉛直成分許容範囲が十分に大きいものとなる期間（片脚支持期内の期間）において、逆位相腕振り角軌道を定常歩容につながらせる（近づける）ように修正するためのものである。

この逆位相腕振り復元角加速度パターンは、前記定常歩容生成処理で説明したものと同様に台形状のパターンであり、具体的には次のように決定される。すなわち、図 3 6における第 2旋回歩容の期間の逆位相腕振り復元角加速度の台形パターンと同様に、今回歩容の逆位相腕振り復元角加速度の台形パターンを設定し、台形の頂点（折れ点）の時刻は既知とする（より具体的には台形の折れ点の時刻を目標 Z M Pの折れ点時刻に合わせる）と共に、台形高さを未知数とし、以下のように逆位相腕振り復元角加速度の台形高さ（パラメータ）が求められる。ただし、逆位相腕振り復元角加速度の台形パターンが立ち上がり始める時刻を Ta とし、台形パターンから 0に戻る時刻を Tbとする。

上記のように逆位相腕振り復元角加速度パターンの未知パラメ一夕がひとつの場合には、今回歩容の終端において逆位相腕振り角と逆位相腕振り角速度の両方を定常歩容に連続につなげることは一般的にはできなレ。そこで、本第 1参考例では、複数歩に渡って徐々に、生成される歩容の状態が定常歩容の状態に近づけるように、未知パラメータを決定することとする。

補足すると、 1歩容の中での逆位相腕振り復元角加速度パターンを複雑にし、未知パラメ一夕を 2つ以上にして、今回歩容の終端において逆位相腕振り角と逆位相腕振り角速度との両者を定常歩容に連続につなげても良いが、逆位相腕振り復元角加速度パターンがジグザグに大きく変動し過ぎる恐れがある。

前述のごとく S 7 0 2で逆位相腕振り復元角加速度パターンの台形高さを 0として求めた仮今回歩容の終端逆位相腕振り角と定常歩容の初期逆位相腕振り角との差を求め、これを 0 azerr とする。また、仮今回歩容の終端逆位相腕振り角速度と定常歩容の初期逆位相腕振り角速度との差を求め、これを v 0 zerrとする。

ここで、逆位相腕振り復元角加速度パターンの台形高さをある値 bzcurr として今回歩容が生成され、引き続いて、今回歩容と同様のァルゴリズムによって第 1旋回歩容が生成されると想定する。ただし、第

1旋回歩容の逆位相腕振り復元角加速度パターンは、図 2 4の S 3 1 4 において求められた第 1旋回歩容の逆位相腕振り復元角加速度パターン (高さが前記 azcyclである図 3 6の台形パターン）とある値 bzl の和であるとする。

このようにして生成された歩容を逆位相腕振り復元角加速度修正歩容と呼び、その終端（第 1旋回歩容の終端）逆位相腕振り角と角速度をそれぞれ θ zl、 νθ zlとする。

S 0 2 4における定常歩容の初期状態を求めるサブルーチンの処理が完了した時点で求められた本来の定常歩容（最終的に S 3 1 4において決定された定常歩容初期逆位相腕振り角および角速度を初期値とし、逆位相腕振り復元角加速度パターンを S 3 1 4において求められたパターン（高さが azcycl である図 3 6の台形パターン）とした場合の定常歩. 容）の第 1旋回歩容終端逆位相腕振り角と角速度をそれぞれ 0 zlorg, V Θ zlorgとする。

ここで、 Δ 0 ζ1、 Δ v0 zlを次のように定義する。

A θ ζ1 =θ zl- Θ zlorg …式 1 0 5 0

ν Θ zl =ν θ zl~v θ zlorg …式 1 0 5 1 Α θ ζΐ および Δνθ ζΐ は、逆位相腕振り復元角加速度修正歩容と本来の定常歩容を第 1旋回歩容の終端まで生成した時点において、両者間における逆位相腕振り角の差及び角速度の差を意味する。もし、 zl および Δν ζΐ が 0になれば、逆位相腕振り復元角加速度修正歩容に引き続いて、今回歩容と同様のアルゴリズムによって、逆位相腕振り復元角加速度パターンの台形高さを前記 azcyc2 として第 2旋回歩容が生成されると、この歩容は、本来の定常歩容に一致する。

したがって、 Δ 0 ζ1 および Δν θ ζΐ が 0 となる今回歩容台形高さ bzcurr, 第 1旋回歩容台形高さ bzl を求め、求めた bzcurr を今回歩容の台形高さとして最終的に決定すれば良い。

ところで、口ポット 1の逆位相腕振り角に関する動力学モデルが図 1 2に示すフライホイール FHazのような線形特性を持つことから、 Δ 0 zl および Δ ν 0 ζ 1 は、今回歩容台形高さ bzcurr、第 1旋回歩容台形高さ bzl、仮今回歩容の終端逆位相腕振り角と定常歩容の初期逆位相腕振り角の差 Θ zerr, 仮今回歩容の終端逆位相腕振り角速度と定常歩容の初期逆位相腕振り角速度の差 v 0 zerrと以下の関係を持つ。

Δ Θ zl = czll * bzcurr + czl2 * bzl + Gzerr + ezl * νθζβιτ

…式 1 0 5 2

Δ v0zl = cz21 * bzcurr + cz22 * bzl + ez2 * vGzerr

…式 1 0 5 3 だだし、 czll、 czl2、 cz21、 cz22、 ezl、 ez2 は、今回歩容、第 1旋回歩容の歩容周期、逆位相腕振り復元角加速度パダーンのパラメ一夕（特に時間に関するパラメータ）などによって一義的に決定される係数である。

以上の原理に基づき、算出手順として、まず、仮今回歩容と定常歩容との境界での逆位相腕振り角の差 0 zerr と角速度の差 v 0 zerr が求められる。

次いで、式 1 0 5 2 ， 1 0 5 3の係数である czll、 czl2、 cz21、 cz22、 ezl、 ez2 が、今回歩容、第 1旋回歩容の歩容周期、逆位相腕振り復元角加速度パターンのパラメータ（特に時間に関するパラメータ）などを基に求められる。

次いで式 1 0 5 2、式 1 0 5 3の右辺が 0となるように今回歩容台形高さ bzcurr、第 1旋回歩容台形高さ bzl が決定される。すなわち、式 1 0 5 2、式 1 0 5 3の左辺を 0とした連立方程式を解くことによって、 bzcurr, bzlが求められる。

最後に、今回歩容の逆位相腕振り復元角加速度の台形パターンの台形高さが、上記求めた今回歩容台形高さ bzcurrに設定される。図 1 3に戻って、上記のごとく S 0 2 8において今回歩容パラメ一夕を修正した後、あるいは S 0 1 6の判断結果が NOである場合には、 S 0 3 0に進み、今回歩容瞬時値が決定される。

S O 3 0では、そのサブル一チンである図 4 5の処理が実行される。図 4 5の S 1 4 0 0から S 1 4 1 1までは、図 2 5の S 4 0 0から S 4 1 1までと同一の処理が行なわれ、次に、 S 1 4 1 2のサブルーチンである図 4 6の S 1 0 0 0〜S 1 0 1 8の処理が行なわれる。

具体的には、まず S 1 0 0 0におい.て、目標上体ョ一角に現在時刻での基準上体ョ一角の値を代入する。また、腕姿勢逆位相腕振り角 · 角速度を除き、目標腕姿勢に現在時刻での基準腕姿勢の値を代入する。

次いで S 1 0 0 2に進み、現在時刻が、上体姿勢角 · 逆位相腕振り角復元期間（時刻 Ta から時刻 Tb の期間）の間にあるか否かが判定される。 S 1 0 0 2の判定結果が NOであるならば、 S 1 0 0 4に進み、 Y E Sであるならば S 1 0 0 6に進む。

· S 1 0 0 4では、上体傾斜角 ·逆位相腕振り角復元期間でない場合の上体水平加速度ひ、上体角加速度 /3、逆位相腕振り角加速度) 3 a 算出処理（図 2 6の S 5 0 4から S 5 2 8 ) と同様の処理を行う。

S 1 0 0 6に進む場合では、まず、 S 1 0 0 6において、仮に上体並進モードの運動をさせた場合に今回（時刻 k) 目標 Z MPを満足するために必要な上体水平加速度 atmpを求める。

次いで S 1 0 0 8に進み、現在時刻における上体傾斜復元モーメント ZMP換算値パターンの瞬間値 ZMPrec を、 S 7 1 8において決定した上体傾斜復元モ一メント Z MP換算値パターンに関するパラメ一夕を基に算出する。

次いで S 1 0 1 0に進み、現在時刻における逆位相腕振り復元角加速度パターンの瞬間値 j8 arec を、 S 7 2 2において決定した逆位相腕振り復元角加速度パターンに関するパラメ一夕を基に算出する。

次いで S 1 0 1 2に進み、図示する式によって、上体傾斜モードの上体角加速度（上体傾斜角加速度） /3が決定される。

次いで S 1 0 1 4に進み、図示する式によって、上体水平加速度 αが決定される。

次いで S 1 0 1 6に進み、 S 1 0 1 0で算出した逆位相腕振り復元角加速度パターンの瞬間値 3 arec と基準逆位相腕振り角加速度 |3 aref (基準逆位相腕振り角を 2階微分した値）の和を目標逆位相腕振り角加速度 i8 aに代入する。

次いで S 1 0 1 8に進み、上体水平加速度が αの場合の床反力水平成分 Fxを求める。

次いで S 1 4 1 4に進み、上体水平加速度と上体姿勢角加速度を積分して上体水平速度と上体姿勢角速度（上体傾斜角速度）を算出する。これをさらに積分して、上体水平位置と上体姿勢 (上体傾斜角) を決定する。なお、上体姿勢のうちの上体ョ一角は、.本第 1参考例では基準上体ョ一角に決定される。

次いで S 1 4 1 6に進み、逆位相腕振り加速度を積分して逆位相腕振り角速度を算出する。これをさらに積分して、逆位相腕振り角を決定する。

以上のごとく、図 1 3の S 0 3 0の処理が完了する。

次いで S 0 3 2に進み、歩容生成用時刻 tを Δ tだけ増やし、 S 0 1 4に戻り、以上のごとく歩容生成を続ける。

以上が、歩容生成装置 1 0 0における目標歩容生成処理である。

図 4を参照して本第 1参考例に係る装置の動作をさらに説明すると、歩容生成装置 1 0 0において、上記したように目標歩容が生成される。生成された目標歩容のうち、目標上体位置姿勢（軌道）、目標腕姿勢 (軌道）が、口ポット幾何学モデル（逆キネマテイクス演算部） 1 0 2 に送出される。

また、目標足平位置姿勢（軌道）、目標 Z M P軌道（目標全床反力中心点軌道）、および目標全床反力（軌道）（目標床反力水平成分と目標床反力鉛直成分）は、複合コンプライアンス動作決定部 1 0 4に送られると共に、目標床反力分配器 1 0 6にも送られる。そして、目標床反力分配器 1 0 6で、床反力は各足平 2 2 R， 2 2 Lに分配され、目標各足平床反力中心点および目標各足平床反力.が決定される。この決定された目標各足平床反力中心点および目標各足平床反力は複合コンプライアンス動作決定部 1 0 4に送られる。

複合コンプライアンス動作決定部 1 0 4から、機構変形補償付き修正目標足平位置姿勢（軌道）がロポット幾何学モデル 1 0 2に送られる。口ポット幾何学モデル 1 0 2は、目標上体位置姿勢 (軌道) と機構変形補償付き修正目標足平位置姿勢（軌道）を入力されると、それらを満足する脚体 2 , 2の 1 2個の関節 ( 1 0 R ( L ) など）の関節変位指令 (値) を算出して変位コントローラ 1 0 8に送る。変位コントローラ 1 0 8は、ロポット幾何学モデル 1 0 2で算出された関節変位指令（値）を目標値としてロボット 1の 1 2個の関節の変位を追従制御する。また、ロボット幾何学モデル 1 0 2は、目標腕姿勢を満足する腕関節の変位指定（値）を算出して変位コントローラ 1 0 8に送る。変位コントローラ 1 0 8 は、ロボット幾何学モデル 1 0 2で算出された関節変位指令 (値）を目標値として口ポット 1の腕体の 1 2個の関節の変位を追従制御する。

口ポット 1に生じた床反力（詳しくは実各足床反力）は 6軸力センサ 5 0によって検出される。その検出値は前記複合コンプライアンス動作決定部 1 0 4に送られる。また、口ポット 1に生じた実上体姿勢角偏差（目標上体姿勢と実上体姿勢（上体 3の実際の姿勢）との偏差）のうちの姿勢傾斜角偏差 S errx, Θ erry (詳しくは目標上体姿勢のうちの鉛直方向に対する傾斜角に対する実上体姿勢のうちの鉛直方向に対する傾斜角の偏差で、ロール方向 ( X軸回り）の姿勢傾斜角偏差が 0 errx であり、ピッチ方向（Y軸回り）の姿勢傾斜角偏差が 0 erry である）が姿勢センサ 5 4を介して検出され、その検出値は姿勢傾斜安定化制御演算部 1 1 2に送られる。この姿勢傾斜安定化制御演算部 1 1 2で、ロボット 1の実上体姿勢角を目標上体姿勢角に復元するための目標全床反力中心点（目標 Z M P ) まわり補償全床反カモ一メント水平成分が算出されて複合コンプライアンス動作決定部 1 0 4に送られる。

より具体的には、本第 1参考例では、補償全床反力モーメント水平成分 Mdmdxyは、例えば P D制御則を用いて次式により決定される。辅償全床反力モーメント水平成分 Mdmdxy .

= K 0 b *上体姿勢傾斜角偏差

+ K c b *上体姿勢傾斜角速度偏差

d 2 5 ここで、 K 0 b、 Κ ω bは所定のゲインである。また、上体姿勢傾斜角速度偏差は、上体姿勢傾斜角偏差の時間微分値であり、目標上体姿勢傾斜角速度に対する実上体姿勢傾斜角速度の偏差を意味する。また、上体姿勢傾斜角偏差は、より詳しくは、口ポット 1の上体 3のロール方向 ( X軸回り）の姿勢傾斜角偏差と、ピッチ方向（Y軸回り）の姿勢傾斜角偏差とからなるべクトルである。

また、口ポット 1に生じた前記.実上体姿勢角偏差のうちのョ一角偏差 θ βννζ (詳しくは実上体姿勢角偏差のうちのョー方向（Ζ軸回り）の姿勢角偏差が Θ errz である）が姿勢センサ 5 4を介して検出され、その検出値はョー安定化制御演算部 1 1 3に送られる。このョー安定化制御演算部 1 1 3で、ロポット 1の実上体ョ一角および/"または角速度を目標上体ョ一角および Zまたは角速度に収束させるための目標全床反力中心点（目標 Z MP) まわり補償全床反力モーメント鉛直成分が算出されて複合コンプライアンス動作決定部 1 0 4に送られる。複合コンプライアンス動作決定部 1 0 4は、入力値に基づいて目標床反力を修正する。具体的には、目標全床反力中心点（目.標 Z MP) 回りに補償全床反カモ —メントが作用するように目標床反力を修正する。

より具体的には、本第 1参考例では、補償全床反力モーメント鉛直成分 Mdmdzは、例えば P D制御則を用いて次式により決定される。補償全床反力モーメント鉛直成分 Mdmdz = K0 bz*上体ョ一角偏差

+ K obz*上体ョー角速度偏差 …… d 2 6 ここで、 Κ Θ bz、 Kc bz は所定のゲインである。また、上体ョ一角速度偏差は、上体ョ一角偏差の時間微分値であり、目標上体ョ一角速度に対する実上体ョー角速度の偏差を意味する。

補足すると、補償全床反力モーメント鉛直成分 Mdmdz を前記式 d 2 6により求めるとき、足平 2 2と床の間の鉛直軸まわり回転滑りまたは鉛直軸まわり回転振動を防止したいだけなら、 K 0 bz を 0にすれば良い。上体ョ一角偏差までも 0に近づけようとすると、実床反力モーメント鉛直成分が大きくなる傾向があるからである。

複合コンプライアンス動作決定部 1 0 4は、入力値に基づいて目標床反力を修正する。具体的には、目標全床反力中心点（目標 Z MP) 回りに補償全床反力モーメント水平成分が作用するように目標床反力モーメント水平成分を修正し、さらに、目標歩容に動力学的に釣り合う目標全床反力中心点（目標 Z M P ) 回りの目標床反力鉛直成分に補償全床反力モ一メント鉛直成分を付加的に追加することによって目標床反力モーメント鉛直成分を修正する。

複合コンプライアンス動作決定部 1 0 4は、修正された目標床反力にセンサ検出値などから算出される実ロポット 1の状態および床反力を一致させようと上記機構変形補償付き修正目標足平位置姿勢（軌道）を決定する。ただしすベての状態を目標に一致させることは事実上不可能であるので、これらの間にトレードオフ関係を与えて妥協的になるベく一致させる。すなわち、各目標に対する制御偏差に重みを与えて、制御偏差（あるいは制御偏差の 2乗）の重み付き平均が最小になるように制御する。これにより、実際の足平位置姿勢と全床反力とが目標足平位置姿勢と目標全床反力とに概ね従うように制御される。

尚、この発明の要旨は歩容生成装置 1 0 0.における口ポット 1の歩容生成にあり、上記した複合コンプライアンス動作決定部 1 0 4などの構成および動作は、本出願人が先に出願した特開平 1 1— 3 0 0 6 6 1号公報などに詳細に記載されているので、説明を以上に留める。

S O 2 8において、今回歩容パラメ一夕は、前述のごとく、今回歩容の終端発散成分が、定常旋回歩容の初期発散成分 d [ 0 ] を今回歩容の支持脚座標系から見た値である Q " に一致するように修正される。

実は、発散成分とは、動力学モデルを用いて今回歩容パラメータに従つて歩容を生成し、そのまま連続して定常旋回歩容パラメ一夕に従って歩容を繰り返し生成したとき、生成した歩容の上体水平位置が定常旋回歩容に収束するか否かを評価する指標である。収束する場合にちょうど、今回歩容の終端発散成分が、定常旋回初期発散成分 Q [ 0 ] を今回歩容の支持脚座標系から見た値である Q " に一致するように、本来、発散成分は定義されなければならない。

式 1 0で定義した発散成分は、実は、上記の性質を近似的に満足する発散成分である。

したがって、本第 1参考例においては、動力学モデルを用いて今回歩容パラメ一夕に従って歩容を生成し、そのまま連続して定常旋回歩容パラメ一夕に従って歩容を繰り返し生成したとき、生成した歩容の上体水平位置が定常旋回歩容の上体水平位置に収束する（近づく）ように、今回歩容パラメ一夕を修正した、と言える。但し、厳密には、今回歩容直後の第 1旋回歩容だけは、前記の如く求めた第 1旋回歩容台形高さ bl, bzl によって修正された歩容にする必要がある。別の言い方をすれば、今回歩容と第 1旋回歩容とを合わせた歩容を今回歩容と見なせば、本参考例においては、上記の如く歩容を繰り返し生成したとき、生成した歩容の上体姿勢角が第 2旋回歩容と第 1旋回歩容とからなる定常歩容の上体姿勢角に収束し (近づき）、もしくは一致するように、今回歩容パラメータを修正した、と言える。

これは、 P C T公開公報 WO/ 02/ 40224のものと同様である。

特に、本第 1参考例においては、その条件（今回歩容が定常歩容に近づく）を満足するように、今回歩容の歩容パラメ一夕のうちの目標 Z M Pパターンを修正するようにした。図 4 7を参照して説明すると、同図に符合 Bで示す軌道は、上記したように歩容の境界において発散成分が一致するように生成した上体水平位置軌道を示す。

同図に符合 Aで示す軌道は、定常旋回歩容との境界における上体水平位置速度が一致するように今回歩容を生成し、続いて定常歩容を繰り返し生成した場合の上体水平位置軌道を示す。

図示の如く、符合 Bで示す軌道は、今回歩容と最初の定常旋回歩容との境界においては、一般的に符合 Aで示す軌道からずれているが、その後、徐々に符合 Aで示す軌道に収束し（近づき）、次の定常旋回歩容時期において符合 Aで示す軌道にほぼ一致する。このように、歩容境界で発散成分のみを一致させる歩容生成手法でも、歩容境界で位置速度両方を一致させる歩容生成手法と同様、歩容の発散を防止することができる, 尚、同図に符合 Cで示す例は、それらを考慮せずに軌道を生成した場合の例を示す。このような場合は、生成した軌道は経時的に発散してしまう。当然、目標 Z M Pパターンを複雑なものにし、複数のパラメ一夕を調整して、位置速度両方を一致させても良いが、目標 Z M Pパターンがジグザグになる恐れがある。なお、位置速度両方を一致させれば、発散成分も一致するから、位置速度両方を一致させる方法も発散成分を一致させる方法の特殊例であると言える。

またさらに、本第 1参考例においては、動力学モデルを用いて今回歩容パラメ一夕に従って歩容を生成し、そのまま連続して定常旋回歩容パラメ一夕に従って歩容を繰り返し生成したとき、生成した歩容の上体姿勢角が定常旋回歩容の上体姿勢角に収束し（近づき）、もしくは一致するように、今回歩容パラメ一夕を修正した、と言える。但し、厳密には, 今回歩容直後の第 1旋回歩容だけは、前記の如く求めた第 1旋回歩容台形高さ bl， bzlによって修正された歩容にする必要がある。

次に、本第 1参考例の変形態様を以下にいくつか説明する。なお、以下に説明する変形態様は、後述の各実施形態においても同様である。

本第 1参考例では、理解を容易にするために、床反力水平成分許容範囲は、前後方向（X軸方向）成分と左右方向（Y軸方向）成分それぞれ独立に設定できることとしたが、前後方向と左右方向の関係式で表した方が、よりスリップし難い歩容が生成される。，

例えば、次式のようにいわゆる摩擦円を許容範囲としても良い。 (床反力水平成分の X成分) * (床反力水平成分の X成分) + (床反力水平成分の Y成分) * (床反力水平成分の Y成分)

≤ (ka * * Fz) * (ka * * Fz)

…式 5 9 ただし、 Fz は、目標床反力鉛直成分を ^は摩擦係数、 ka は 1以下の正の定数を表す。

ただし、以上のように、床反力水平成分許容範囲を前後方向と左右方向の関係式で表す場合、許容範囲を同時にあるいは交互に満足するように、サジタルプレーン上での運動とラテラルプレーン上での運動を同時あるいは交互に求める必要がある。

また、床反力水平成分許容範囲と床反力モーメント鉛直成分許容範囲とを上記のごとく別々に設定する代わりに、床反力水平成分と床反カモーメン卜鉛直成分との組の許容範囲を設定するようにしても良い。床反力水平成分が大きくなるにつれ、床反力モー.メント鉛直成分の摩擦限界は、小さくなる。また、床反カモ一メント鉛直成分が大きくなるにつれ. 床反力水平成分の摩擦限界は、小さくなる。したがって、このことを鑑みて床反力水平成分と床反カモ一メント鉛直成分との組の許容範囲を設定するようにした方が、より現実の摩擦限界特性に近い許容範囲を設定することができる。具体的には、床反力水平成分の絶対値と床反力モーメント鉛直成分の絶対値との重み付き平均に対して許容範囲を設定しても良い。

前記第 1参考例では、床反力水平成分と目標 Z M Pまわりの床反カモーメント水平成分を適切な値にするために、上体傾斜モードと上体並進モードとの 2つの運動モードを用いたが、これら以外の運動モードを用いても良い。例えば、図 4 8のように、股関節を中心に上体姿勢を回転させると、全体重心まわりの角運動量が変化すると共に全体重心も変化する。この運動と前記上体並進モードをある所定の割合で重ね合わせると（合成すると）、前記上体傾斜モードとほとんど同じ運動になり、床反力水平成分が発生しなくなる。したがって、これを改めて前記上体傾斜モードとみなせば、本第 1参考例のアルゴリズムにしたがって、同様の歩容を生成することができる。

したがって、運動モードのひとつが床反力水平成分を発生しない運動モードである必要もない。いかなるモードの組み合わせであっても、床反力水平成分と目標 Z M Pまわりの床反力モーメントとの発生の割合が異なるふたつの運動モードを用いさえすれば、上記例のように、任意の床反力水平成分と目標 Z M Pまわりの床反力モーメントとを発生させることができるからである。

また、上体姿勢を変化させる運動モード以外の運動モードを用いても良い。ただし、なるべく小さい変位で大きな床反力水平成分または目標 Z M Pまわりの床反力モ一メントが発生できる運動モードを選ぶことが好ましい。

例えば、左右の腕体の先端位置を同一方向に振りまわす運動モード、接地していない (空中に存在する) 足平の位置を摂動させる運動モードでも良い。ただし、遊脚軌道を摂動させる場合には、着地位置が変わらないように着地直前までには、摂動量を実質的に 0に戻すべきである。なお、左右の腕体の先端位置を同一方向に振りまわす運動モードと逆位相腕振りモードとを併用してもよいことはもちろんである。

また、 3つ以上の運動モードを用いても良い。

また、選択した運動モードのうちの少なくとも 2つは、その運動モ一ドによる床反力水平成分と目標 Z M Pまわりの床反力モーメントとの発生比率が、互いに異なるものである必要がある。そうでないと、一般的に連立方程式の解がなくなる（各運動モードの挙動を一義的に決定できない）からである。

さらにできる限り、床反力水平成分をあまり変化させないで目標 Z M Pまわりの床反力モーメントを十分に大きく変化させることができる運動モードと、目標 Z M Pまわりの床反力モーメントをあまり変化させないで床反力水平成分を十分に大きく変化させることができる運動モードとを組み合わせることが望ましい。 .

言いかえると、全体重心をあまり変化させないで角運動量を十分に大きく変化させることができる運動モードと、角運動量をあまり変化させないで全体重心を十分に大きく変化させることができる運動モードとを組み合わせることが望ましい。このようにすると、運動モードの変位が小さくなるからである。

また、床反力モーメント鉛直成分が許容範囲を越えないようにするために（スピン力をキャンセルするために）、 .逆位相腕振りモードの代わりに、上体ョ一回転モードを用いても良い。上体ョ一回転モードを用いる場合には、ロボット 1の上体 3を腰寄りの部分（例えば図 1の 5 4の部材の下側の部分）とその上側の部分（例えば図 1の 5 4の部材の上側の部分）とに分割し、上側の部分が腰寄りの部分に対してョー方向に (例えば上体 3の体幹軸回り）に回転し得るように設けておくことが好ましい。このようにすると、脚体 2， 2の姿勢に影響を及ぼすことなく、上体 3の上側の部分を回転させて、床反カモ一メント鉛直成分を調整することができる。このようにした場合の参考例は、例えば前記第 1参考例で逆位相腕振り角、その角速度およびその角加速度を決定する代わりに、上体 3の上側の部分の回転角、その角速度およびその角加速度を決定するようにすればよい。この場合、両腕体 5， 5は、例えば上体 3の上側の部分に対する相対位置を一定とすればよい。但し、上体 3の上側の部分のョ一回転に加えて、両腕体 5， 5の逆位相腕振り動作を加えてもよいことはもちろんである。

また、床反力モーメント鉛直成分が発生するものであれば、腕、上体以外の部位を変位させる運動モードを用いても良い。

例えば、空中期において、両脚先端を前後逆方向に動かすモードを用いても良い。

あるいは、床反カモ一メント鉛直成分が発生するいくつかの運動モードを併用しても良い。例えば、逆位相腕振りモードと上体ョ一回転モードを併用しても良い。

また、上体ョ一回転モードおよび逆位相腕振りモードは、全体重心位置が変わらないように (言い換えると床反力水平成分を発生させずに) 床反力モーメント鉛直成分を発生させるモードであるが、全体重心位置が変わってしまう (言い換えると床反力水平成分を発生してしまう）運動でも良い。上体並進モードと組み合わせることにより、床反力水平成分を調整することができるからである。

前記第 1参考例に用いた動力学モデル（図 1 2の動力学モデル）以外にも以下のモデルを用いても良い。

1 ) 図 4 9に示したように複数のリンクに質点を設定した非線形なモデル（多質点モデル）。このモデルの各リンクには、イナーシャ（慣性モ —メント）が設定されていてもよい。

2 ) 本願出願人による P C T公開公報 WOZ 02/ 40224に示された 3質点モデル

3 ) 上体ョ一回転モードあるいは逆位相腕振りモードによって生じる慣性力のモーメントを無視したモデル。 '

4 ) 重力と慣性力との合力（またはこれと釣り合う床反力）と上体並進運動との関係を表す部分モデルと、上記合力と上体姿勢回転運動（上体傾斜運動と上体ョー回転運動）の関係を表す部分モデルとを分離して持つ分離型モデル。例えば、図 1 2に示す質点は、上記合力と上体並進運動との関係を表す部分モデルであり、図 1 2に示すフライホイールは、上記合力と上体姿勢回転運動との関係を表す部分モデルである。

なお、上記 1 ) 〜 4 ) のいずれの場合でも、慣性力のモーメント鉛直成分を発生させる運動モードは必要である。

また、各処理で用いられるモデルは同一でも良いし、処理に応じて適宜、モデルを使い分けても良い。例えば、前記定常歩容は今回歩容の終端状態を決定するためだけに生成されるものであるので、その定常歩容の動力学的な精度に対する要求は、今回歩容に比べて小さい。そこで、例えば、今回歩容の生成処理では、図 1 2の動力学モデル（ 3質点 +フライホイールのモデル) を用いる一方、定常歩容の生成処理 (特に図 2 1の S 4 0 8、 S 4 1 2 ) では、各脚体 2の質量を無視し、上体 3に対応する上体質点 3 mと、フライホール FHx、 F Hy F Haz、 F Hbz とで構成される動力学モデル (図 1 2のモデルで脚質点 2 m , 2 mを除去した 1質点 +フライホイールのモデル) を用いて定常歩容を生成するようにしてもよい。この場合の定常歩容の生成処理では、前記第 1参考例において、脚質点 2 mの質量を 0として前記図 2 5の S 4 0 8、 S 4 1 2の処理を行なうようにすればよい。このようにすると、定常歩容の生成処理における演算量を大幅に削減できる。

尚、前記第 1参考例において、ブロック線図、フローチャート、およびアルゴリズムなどは、演算処理順序を変えるなどの等価変形をしても良い。また、適宜ローパスフィルタを挿入しても良い。

また、前記第 1参考例を 2足移動口ポットに関して説明してきたが、 3足以上の多脚ロポットにも応用することができる。前記第 1参考例のごとく、探索手法を用いたり、部分的に解析手法を用いたりして、定常歩容の初期状態（主に初期上体水平位置速度、初期上体鉛直位置速度および逆位相腕振り角 · 角速度のこと）を決定する手法の代わりに、あらかじめ、上記手法を用いて様々な定常歩容パラメ一夕に対して計算しておき、定常歩容パラメ一夕と定常歩容の初期状態との関係をマツプ化あるいは近似式化して記憶して置き、実移動時には、マツプ化あるいは近似式化された上記関係を基に、定常歩容の初期値を決定しても良い。

さらには、マツプ化あるいは近似式化された上記関係と上記関数 f を合成した関数をマップ化あるいは近似式化して記憶しても良い。すなわち、前記した足平軌道パラメータ、床反力鉛直軌道パラメ一夕等からなる定常歩容パラメ一夕から、定常歩容の発散成分を直接求める関数をマップ化あるいは近似式化して記憶しても良い。例えば、複数種類の代表的な定常歩容パラメ一夕の組のそれぞれに対して、事前に定常歩容を生成しておき、各組の定常歩容パラメ一夕に対する定常歩容の初期状態 (図 1 3の S 0 2 4で求めるもの) を求めておき、さらにその各組の定常歩容パラメ一夕と、定常歩容初期状態との関係を表すマツプをあらかじめ作成しておく。そして、目標歩容を生成する際に、決定された定常歩容パラメ一夕の組から、上記マツプに基づいて選択あるいは補間によつて定常歩容初期状態を求めるようにすればよい。このようにすれば、今回歩容の生成毎に、定常歩容を生成する必要がなくなり、目標歩容生成処理の演算量を大幅に削減できる。

定常歩容につなげる（近づける）ための今回歩容の修正方法として、本第 1参考例では、今回歩容の目標 Z M Pパラメータを修正したが、それ以外のパラメタを修正しても良い。

たとえば、今回歩容の遊脚の空中での軌道を変えても良い。また、たとえば、上体水平位置が定常歩容ょりも後方にずれてしまいそうな場合には、遊脚を離床後すばやく前方に移動させて遊脚重心位置を前方にずらす。こうすることにより、目標 Z M Pを満足するための上体水平位置は、前方により一層加速せざるを得なくなる。この結果、上体水平位置が今回歩容終端において、より一層前方に移動し、定常歩容に一致させることができる。

また、目標 Z M Pパラメ一夕を修正する代わりに、今回歩容の周期を修正しても良い。たとえば、上体水平位置が定常歩容よりも後方にずれてしまいそうな場合には、今回歩容の周期を長くすれば良い。今回歩容の周期を長くことで、上体水平位置が移動する時間が延び、それだけ前方に移動できるからである。

ただし、探索手法によって、上体水平位 g等の適正値を決定するときに、目標 Z M Pパラメ一夕を修正する場合では、目標 Z M Pの修正量にほぼ比例して、今回歩容終端上体水平位置が変化するので、上記適正値の探索回数が少なくて済む。これに対して、 .遊脚重心軌道や歩容の周期を修正する場合には、その修正に対して、今回歩容終端上体水平位置が. かなり非線形に変化するので、上記適正値の探索により多くの探索回数を要する。

定常歩容につなげる（近づける）ための今回歩容の修正方法として、本第 1参考例では、今回歩容の目標 Z M Pパラメ一夕を修正した。この方法では、場合によっては、目標 Z M Pパラメータの修正量（前記図 3 4の修正量 _a) が過大になることがある。たとえば、その場でホッピングしている歩容からいきなり高速で移動する要求（走行要求）が与えられると、高速の定常歩容（走行の定常歩容）につなげる（近づける）ために、目標 Z M Pパラメータは、進行方向に対して後ろに極端に大きくずらす必要が生じる。この場合には、上記したように目標 Z M Pパラメ一夕以外の歩容パラメ一夕も修正することが望ましい。但し、この場合では、実は、急激な加速を要求すること自体に無理があったわけであるから、要求値自身を修正しても良い。

要求値の修正を行なう場合には、例えば、とりあえず本第 1参考例に示した手順に従って、要求（要求パラメ一夕）を満足する定常歩容を求め、この定常歩容につながるように今回歩容パラメータを決定した時点で、今回歩容の目標 Z MP軌道の安定余裕が少なくなり過ぎたか否かを判定する。そして、安定余裕が少なぐなり過ぎた場合（いわゆる支持多角形から目標 Z MPが外れたり、支持多角形の端部付近に目標 Z MPが位置するような場合）には、要求を修正するようにすれば良い。

または、あらかじめ歩容の加減速度（次回歩容初期速度一今回歩容初期速度） Z今回歩容の周期）の許容範囲を設定して置き、要求（歩容周期に関する要求パラメ一夕）を受けた時点で、該要求に対応する加減速度を求め、その求めた加減速度が許容範囲を越えていたら、許容範囲になるように、要求を修正しても良い。

補足すると、前記 ΔΜρ、 Δ Fp, ΔΜ r , Δ F r , Δ Maz および Δ

Mbz は、上記のごとく単純な動力学モデルを用いる場合には、動力学演算により解析的に求めても良いが、一般的なより複雑な動力学モデルを用いる場合には、上体 3を上体並進モードで微小量だけ加速または上体傾斜モードで微小量だけ加速させた場合の床反力を求め、加速させなかった場合の床反力との差を求め、この差を上記微小量で割ることによつて求めれば良い。

また、あらかじめ標準的な歩容における ΔΜΡ、 AFp、 ΔΜ Γ 、 Δ F r、 AMaz、 Δ Mbz および Δ M p Z Δ M rなどのそれぞれの平均値を求めておいて、それを用いても良い。 ΔΜρ、 AFp AMr、 AFr、 Δ

Maz、 AMbz および ΔΜρ ΖΔΜΓは、状態（姿勢とその変化率）によつて変わるので、各瞬間の状態ごとに求める方法に較べて、精度は若干低下するが、上記モデルよりも複雑なモデルを用いた場合には、演算量を大幅に低減できる。

今回歩容の逆位相腕振り復元角加速度パターンの台形高さ b zcurr を決定する方法として、以下の方法を用いても良い。

前記逆位相腕振り復元角加速度修正歩容（図 4 2の S 7 2 2の説明を参照）の今回歩容終端時点での逆位相腕振り角と角速度をそれぞれ Θ zcurr、 ν θ zcurr とし、これらと、定常歩容の逆位相腕振り角、角速度との △ Θ zcerr、 Α ν Θ zcerrとする。

歩容周期をインターバルとし、仮今回歩容の終端逆位相腕振り角、角速度と定常歩容の初期逆位相腕振り角、角速度との差 0 zerr、 ν Θ zerr を前回状態、 b zcurr を入力、 Δ Θ zcerr、 Α ν θ zcerr を今回状態とする離散系の状態方程式を立てて、 Δ Θ zcerr、 Α ν θ zcerr を 0に収束させるように、現代制御理論等を用いてフィ一ドバック則を決定し、これに基づいて bzcurrを求めても良い。

今回歩容およびまたは定常歩容の逆位相腕振り復元角加速度 β arec は、台形パターンとせず、各瞬間において、目標逆位相腕振り角 · 角速度と基準逆位相腕振り角 · 角速度との差などを基に、その差が 0に収束するように、状態フィードバック則などを用いて、各瞬間における逆位相腕振り復元角加速度 β arecの値を決定しても良い。

今回歩容の逆位相腕振り復元角加速度 3 arec は、台形パターンとせず、各瞬間において今回歩容の目標逆位相腕振り角 · 角速度を基に、これらが第 1旋回歩容の初期逆位相腕振り角 · 角速度に近づくように、状態フィードパック則などを用いて、各瞬間における逆位相腕振り復元角加速度 /3 arecを決定しても良い。

斜面移動時（傾斜した床面で口ポット 1を移動させるとき）の歩容生成においては、床反力水平成分許容範囲や全体重心加速度水平成分許容範囲の代わりに、並進床反力の床面平行成分（床面に平行な成分）、すなわち摩擦力の許容範囲、あるいは全体重心加速度の床面平行成分（これは摩擦力に比例する）の許容範囲を設定するようにしてもよい。例えば、並進床反力の床面平行成分（摩擦力）の許容範囲を設定する場合に関して説明（この説明は全体重心加速度の床面平行成分の許容範囲を設定する場合でも同様である）すると、該摩擦力は、床面の水平面に対する傾斜角度 0 f (ロボット 1の進行方向に向かって前下がり斜面の場合を正とする) とすると、次式 7 2の関係が成立する。従って、前記第 1 参考例と同様のアルゴリズムに歩容を生成する場合、この式 7 2の関係を用いて、摩擦力許容範囲を床反力水平成分許容範囲に変換することで，該床反力水平成分許容範囲を設定するようにすればよい。なお、この場合、式 7 2の床反力鉛直成分には、目標床反力鉛直成分を用いればよい ₍ 摩擦力 =床反力水平成分 * cos( 0 f)—床反力鉛直成分 * sin( 0 f)

…式 7 2 また、斜面移動時（傾斜した床面でロボット 1 を移動させるとき）の歩容生成においては、床反力モーメント鉛直成分は、式 1 0 7 2によつて床面摩擦力法線方向モーメントに変換することができるので、床反力モーメント鉛直成分許容範囲の代わりに、床反力モーメントの床面法線方向成分、すなわち床面摩擦力法線方向モーメントの許容範囲の許容範囲を設定するようにしてもよい。床面摩擦力法線方向モーメント=床反力モ一メント鉛直成分 * cos( 0 f)

…式 1 0 7 2 今回歩容のパラメ一夕決定は、前記第 1参考例のように前回歩容が完了した時点で行う以外に、本願出願人による P C T公開公報 WOZ02Z 40224に記述されているように、今回歩容の生成途中で、再決定しても良い。こうすることにより、演算量は増えるが、歩容に対する要求に変更があった時に即応できるからである。

また、歩容の修正（今回歩容パラメータを再決定すること）が、今回制御周期で間に合わない場合には、とりあえず今回は、修正しない歩容あるいは暫定的に修正した歩容（探索途中の歩容であって、完全には、探索完了条件（歩容境界条件のずれが許容値以内に入っていること）を満足していない歩容）を出力し、次回制御周期までに、あるいは複数制御周期後までに（暫定的でない）適切な修正歩容にしても良い。修正された目標 Z M P軌道と目標床反力鉛直成分軌道は連続であり、短時間後では、これらが急激に変化することはないので、今回歩容の目標 Z M P 軌道と目標床反力鉛直成分軌道が若干ぎざぎざになるだけで、ほとんど問題はない。

また、前記第 1参考例では、図 5の走行歩容を生成する場合を例に採つて説明したが、ロボット 1の歩行歩容を生成する場合でも、前記第 1 参考例と同様に目標歩容を生成できる。この場合には、例えば前記第 1 参考例において、目標床反力鉛直成分を前記図 6に示したものに代えて，例えば図 5 0に示すようなパターンで設定すればよい。この例では、床反力鉛直成分軌道は、両脚支持期では床反力鉛直成分の増加側に凸の台形状に設定され、片脚支持期では床反力鉛直成分の減少側に凸の台形状に設定される。なお、台形部分の高さ C l、 C 2 の設定方法などの詳細は、例えば本願出願人による P C T公開公報 WOZ03Z 057425ZA1 に詳細に説明されているのでここでの説明は省略する。次に、以上説明した第 1参考例およびその変形態様を基礎として、第 2参考例を図 5 3〜図 5 8を参照して説明する。なお、本第 2参考例の説明において、前記第 1参考例と同一構成部分もしくは同一機能部分については前記第 1参考例と同一の参照符号を用いて説明を省略する。特に、本第 2参考例は、先にも述べたように、前記図 1〜図 3および図 5 〜図 1 2を参照して説明した事項は、.前記第 1参考例と同一である。本第 2参考例が前記第 1参考例と相違する点の概要を説明すると、本第 2参考例では、目標上体姿勢角と実上体姿勢角の差である実上体姿勢角偏差（上体 3の鉛直方向に対する傾き角の偏差およびョ一角の偏差）. 及び Z又はその変化率を 0に近づけるために、コンプライアンス制御への目標床反力を操作することに加え、目標歩容も修正するようにした。特に、実上体姿勢偏差のうちのョ一角成分及び/又はその角速度に応じて、目標歩容に動力学的に釣り合う（目標桊容の運動の慣性力と重力との合力が目標 Z M Pまわりに発生するモーメントの鉛直成分に釣り合う）目標 Z M Pまわりの床反力モーメントの鉛直成分も修正するようにした。

本第 2参考例における制御ュニット 6 0の機能的構成を表すブロック図を図 5 3に示す。以下、本第 2参考例における制御ュニット 6 0の機能的構成が前記第 1参考例のもの（図 4のもの）と相違する点を説明する。

本第 2参考例においては、姿勢傾斜安定化制御演算部 1 1 2において算出される補償全床反力モーメント水平成分 Mdmdxy は、補償全床反力モーメント水平成分分配器 1 1 0に入力される捕償全床反力モーメント水平成分分配器 1 1 0 は、補償全床反力モーメント水平成分 Mdmdxy を、コンプライアンス制御用目標床反力モーメント水平成分とモデル操作床反力モーメント水平成分とに分配する。換言すれば、実上体姿勢傾斜角偏差を基に、姿勢傾斜安定化制御演算部 1 1 2及び補償全床反力モーメン卜水平成分分配器 1 1 0により、コンプライアンス制御用目標床反力モーメント水平成分とモデル操作床反力モーメント水平成分とが決定される。

具体的には、補償全床反力モーメント水平成分分配器 1 1 0においては、まず、モデル操作床反力モーメント水平成分が次式で決定される。なお、床反力モーメント水平成分許容範囲は、後述するごとく歩容生成装置 1 0 0において決定される。

Mdmdxy>床反力モ一メント水平成分許容範囲上限値である場合

モデル操作床反力モーメント水平成分 =—Mdmdxy

+床反カモ一メント水平成分許容範囲上限値 Mdmdxy<床反力モーメント水平成分許容範囲下限値である場合

モデル操作床反力モーメント水平成分 =—Mdmdxy

+床反カモ一メント水平成分許容範囲下限値床反力モ一メント水平成分許容範囲下限値≤ Mdmdxy、且つ、 Mdmdxy≤床反力モーメント水平成分許容範囲上限値である場合

モデル操作床反カモ一メン卜水平成分 = 0

……式 d 2 7 a 次に、コンプライアンス制御用目標床反力モーメント水平成分が次式で決定される。

コンプライアンス制御用目標床反力モーメント水平成分

= Mdmdxy +モデル操作床反力モーメント水平成分

式 d 2 7 b 従って、コンプライアンス制御用目標床反力モーメント水平成分とモデル操作床反力モーメン卜水平成分との差が Mdmdxy に等しくなるようにそれらの床反カモ一メン卜水平成分が決定される。

上記のような演算を行なう補償全床反力モ一メント水平成分分配器 1 1 0をブロック図で表すと、図 5 4に示すようになる。

また、ョー安定化制御演算部 1 1 3において前記第 1参考例と同様に決定された補償全床反カモ一メント鉛直成分 Mdmdz (前記式 d 2 6を参照）は、モデル操作床反力モーメント鉛直成分決定器 1 1 1に入力される。モデル操作床反力モーメント鉛直成分決定器 1 1 1は、補償全床反力モーメント鉛直成分 Mdmdz を基に、モデル操作床反力モーメント鉛直成分を決定する。換言すれば、実上体姿勢角偏差のうちの上体ョー角偏差を基に、ョ一安定化制御演算部 1 1 3及びモデル操作床反カモ一メント鉛直成分決定器 1 1 1により、補償垒床反力モーメント鉛直成分 Mdmdzとモデル操作床反カモ一メント鉛直成分とが決定される。

具体的には、モデル操作床反力モーメント鉛直成分決定器 1 1 1 において、モデル操作床反力モーメント鉛直成分が次式で決定される。なお. 床反力モーメント鉛直成分補償量許容範囲は、後述するごとく歩容生成装置 1 0 0において決定される。

Mdmdz>床反力モ一メント鉛直成分補償量許容範囲上限値である場モデル操作床反力モーメント鉛直成分 =—Mdmdz

+床反力モーメント鉛直成分補償量許容範囲上限値 Mdmdz<床反力モーメント鉛直成分補償量許容範囲下限値である場合

モデル操作床反力モーメント鉛直成分 = — Mdmdz

+床反力モーメント鉛直成分補償量許容範囲下限値床反力モーメント鉛直成分補償量許容範囲下限値≤Mdmd_Z、且つ、 Mdmdz≤床反力モーメント鉛直成分補償量許容範囲上限値である場合モデル操作床反力モーメント鉛直成分 = 0

…式 d 2 6 b 上記のような演算を行なうモデル操作床反カモ一メント鉛直成分決定器 1 1 1をブロック図で表すと、図 5 5に示すようになる。従って、モデル操作床反カモ一メント鉛直成分は、補償全床反力モーメント鉛直成分 M dmdz のうち、床反力モーメント鉛直成分補償量許容範囲を逸脱した分の符号を反転させたものに設定される。

コンプライアンス制御用目標床反力モーメント水平成分および補償全床反力モーメント鉛直成分 Mdmdzは、複合コンプライアンス動作決定部 1 0 4に送られる。

モデル操作床反カモ一メント水平成分および鉛直成分は、歩容生成装置 1 0 0に送られる。

尚、補償全床反力モーメント鉛直成分 Mdmdzの代わりに、補償全床反カモ一メント鉛直成分 Mdmdz とモデル操作床反力モーメン卜鉛直成分との和を、コンプライアンス制御用目標値として、複合コンプライアンス動作決定部 1 0 4に送っても良い。

複合コンプライアンス動作決定部 1 0 4は、歩容生成装置 1 0 0が生成する目標歩容の運動にロポット 1の運動を追従させつつ、歩容生成装置 1 0 0が生成する目標全床反力にコンプライアンス制御用目標床反力モーメント水平成分と補償全床反力モーメント鉛直成分 Mdmdz を加えることによって修正される目標全床反力に実床反力が近づくように、目標足平位置姿勢を修正することにより、機構変形補償付き修正目標足平位置姿勢（軌道）を決定する。

この場合、口ポット 1の足平位置姿勢や床反力の全ての状態を目標に一致させることは事実上不可能であるので、前記第 1参考例と同様、これらの間にトレードオフ関係を与えて妥協的になるベく一致させる。詳細は後述するが、歩容生成装置 1 0 0は、該歩容生成装置 1 0 0において決定される目標 Z M Pまわりの床反カモ一メン卜水平成分がモデル操作床反力モーメント水平成分となるように動力学モデルを用いて目標歩容の運動（特に上体位置姿勢軌道）を生成する。さらに、歩容生成装置 1 0 0は、モデル操作床反カモ一メントが 0であるとして生成された目標歩容（仮目標歩容）に動力学的に釣り合う目標全床反力中心点 (目標 Z M P ) 回りの目標床反力モー.メント鉛直成分にモデル操作床反力モーメント鉛直成分が付加的に発生するように目標歩容の運動（特に腕振り軌道）を修正する。

これら以外の、制御ユニット 6 0の機能的構成は、前記第 1参考例と同一である。補足すると、前記第 1参考例で生成した目標歩容は、本第 2参考例で、前記モデル操作床反力モーメント水平成分と、モデル操作床反力モーメン卜鉛直成分とを定常的に 0に設定した場合に生成される目標歩容と同じである。

第 2参考例における歩容生成装置 1 0 0の動作（歩容生成処理）をそのメインフローチャートである図 5 6を用いて以下に詳説する。 S 3 0 1 0から S 3 0 2 8までは、前記第 1参考例の図 1 3の S 0 1 0から S 0 2 8 と同様の処理を行う。

次いで、 S 3 0 3 0に進み、コンプライアンス制御用の目標 Z M Pまわりの床反カモ一メント水平成分と床反カモ一メント鉛直成分補償量の許容範囲を規定するパラメ一夕が決定される。

なお、床反力モーメント水平成分を、床反力鉛直成分で割ったものが. Z M P (床反力中心点）の目標 Z M Pからのずれ量を表すものとなる。従って、床反力モーメント水平成分許容範囲を、床反力鉛直成分で割ることによって、床反力中心点に変換した Z M P許容範囲（床反力中心点許容範囲）のパラメータを設定するようにしてもよい。

補足すると、 S 3 0 3 0で決定するコンプライアンス制御用の床反力モ一メント水平成分許容範囲と床反力モーメント鉛直成分補償量許容範囲のパラメ一夕とを基に、後述する今回歩容瞬時値決定サブルーチン ( S 3 0 3 2のサブルーチン) において、それらの許容範囲の瞬時値が決定され、それが、前記補償全床反力モーメント水平成分分配器 1 1 0 およびモデル操作床反力モーメント鉛直成分決定器 1 1 1における前記の処理に用いられる。

この床反力モーメント水平成分許容範囲については、本願出願人による P C T出願 P C T Z J P 0 3 Z 0 0 4 3 5に床反力モ一メント許容範囲として設定方法が詳細に記載されているので、本明細書では、これ以上の説明を省略する。

前記床反力モーメント鉛直成分補償量は、歩容生成装置 1 0 0において、仮に、床反カモ一メント鉛直成分が歩容生成用の床反カモ一メン卜鉛直成分許容範囲（これは S 3 0 2 6において設定される）内に制限された目標歩容を生成した場合に、目標歩容の運動が発生する床反力モーメントに付加することができる床反力モーメント鉛直成分の補償量を意味する。したがって、歩容生成用の床反力モーメント鉛直成分許容範囲が実際の摩擦限界よりも十分に狭く設定されていなければ、床反力モーメント鉛直成分補償量許容範囲を広く設定することはできない。

床反力モーメント鉛直成分補償量許容範囲は、歩容生成用の床反カモ一メント鉛直成分許容範囲（前記図 4 1を参照）と相似形に設定すれば良い。また、前記図 5の走行歩容の空中期では、コンプライアンス制御用の床反力モーメント鉛直成分補償量許容範囲は、上限値が 0、下限値が 0の領域に設定される。

図 5 6に戻って、上記のごとく S 3 0 3 0においてコンプライアンス制御用の目標 Z MPまわりの床反カモ一メント水平成分と床反力モーメント鉛直成分補償量の許容範囲を規定するパラメ一夕を決定した後、あるいは S 3 0 1 6の判断結果が N〇である場合には、 S 3 0 3 2に進み. 今回歩容瞬時値が決定される。 S 3 0 3 2では、目標 ZMPまわりに前記式 d 2 7 aに従って決定されるモデル操作床反力モーメント水平成分が発生するように今回歩容瞬時値が決定される。ただし、今回歩容に釣り合う（今回歩容の運動の慣性力と重力との合力が目標 Z MPまわりに発生する慣性力モーメントの鉛直成分に釣り合う）床反力モーメント鉛直成分が床反カモ一メント鉛直成分許容範囲を越えないように今回歩容瞬時値が決定される。

具体的には、図 5 7、図 5 8のフローチヤ一トにしたがって歩容瞬時値が決定される。すなわち、 S 3 0 3 0では、まず、図 5 7の S 3 4 0 0から S 3 4 1 1までの処理が実行される。これらの S 3 4 0 0から S 34 1 1までの処理は、前述した図 4 5の S 1 4 0 0から S 1 4 1 1までの処理と全く同一である。

次いで、 S 3 4 1 2に進み、前記図 5 6の S 3 0 3 0で決定したコンプライアンス制御用の床反カモ一メント水平成分許容範囲および床反力モ一メント鉛直成分補償量容範囲のパラメ一夕を基に、現在時刻での床反カモ一メント水平成分許容範囲 [Mxymin, Mxymax]と床反力モーメント鉛直成分補償量許容範囲 [Mzcmin, Mzcmax]の瞬時値（現在時刻 tの今回値）が求められる。

求められた床反力モーメント水平成分許容範囲は、前記補償全床反力モーメント水平成分分配器 1 1 0 (図 5 3参照）に送られる。そして、この分配器 1 1 0で前記式 d 2 7 aにより算出されたモデル操作床反力モーメントの今回値（現在時刻 t.の値）が歩容生成装置 1 0 0に与えられる。また、求められた床反力モーメント鉛直成分補償量容範囲は、前記モデル操作床反力モーメント鉛直成分決定器 1 1 1 (図 5 3参照）に送られる。そして、このモデル操作床反力モーメント鉛直成分決定器 1 1 1 で前記式 d 2 6b により算出されたモデル操作床反力モーメント鉛直成分の今回値（現在時刻 tの値）が歩容生成装置 1 0 0に与えられる。次いで、歩容生成装置 1 0 0の処理は、 S 3 4 1 4に進み、補償全床反力モーメント分配器 1 1 0から与えられたモデル操作床反カモ一メント水平成分が目標 Z MPまわりに発生するように、今回歩容の上体水平加速度と上体姿勢傾斜角加速とが決定される。ただし、床反力水平成分 Fx が、 S 3 4 1 0で決定された床反力水平成分許容範囲 [Fxmin, Fxmax]を越えないように上体水平加速度および上体姿勢角加速度（上体傾斜角加速度）が決定される。

言い換えると、口ポット 1の運動の慣性力と重力との合力が目標 ZM Pまわりに作用するモーメン卜水平成分がモデル操作床反力モーメント水平成分の符号を反転したモーメントになるように、今回歩容の上体水平加速度と上体姿勢角加速（上体傾斜角加速度）とが決定される。ただし、慣性力水平成分の符号を反転した力が床反力水平成分許容範囲 [Fxmin, Fxmax]を越えないように上体水平加速度および上体姿勢傾斜角加速度が決定される。

S3 4 1 4では、具体的には、図 5 8に示すフローチャートに従って. 上体水平加速度と上体姿勢角加速度とが決定される。本フローチャートは、 S 3 1 0 4および S 3 1 3 0を除いて、前記図 2 6と同様の処理が行われる。 S3 1 0 4および S3 1 3 0では、図 2 6の S5 0 4および S 5 3 0と異なり、口ポット 1の前回瞬時歩容状態（時刻 k一 1の歩容状態）から、上体傾斜モードの角加速度を 0にして（より正確には上体傾斜モードの角加速度を基準上体姿勢角加速度に一致させて）、上体並進モードの運動をロボット 1にさせた場合に今回（時刻 k ) の目標 Z M Pまわりに前記モデル操作床反カモ一メント水平成分が発生するために必要な上体水平加速度（ S 3 1 0 4では a tmp、 S 3 1 3 0では ο; ) が求められる。

これ以外の処理は図 2 6の処理と同じである。

以上のごとく S 3 4 1 4の処理を行った後、 S 3 4 1 6に進み、図 4 5の S 1 4 1 4と同一の処理が実行され、今回歩容の上体水平位置および上体姿勢傾斜角（詳しくはそれらの現在時刻 t における今回値）が決定される。

次いで S 3 4 1 8に進み、図 4 5の S 1 4 1 6と同一の処理が実行され、今回歩容の逆'位相腕振り角および角速度（詳しくはそれらの現在時刻 t における今回値）が決定される。

これにより、図 5 6の S 3 0 3 2の処理が完了する。

次いで図 5 6の S 3 0 3 4に進み、目標 Z M Pまわりにモデル操作床反力モーメント鉛直成分が付加的に発生するように、 S 3 0 3 2で生成した今回歩容瞬時値を修正する。

具体的には、モデル操作床反力モーメント鉛直成分に対応する逆位相腕振り角加速度の修正量) 3 aaddを次式によって求める。逆位相腕振り角加速度の修正量 β aadd

=モデル操作床反力モーメント鉛直成分

Z逆位相腕振り運動の等価慣性モーメント A Maz さらに、 jS aadd を制御周期の期間積分して、逆位相腕振り角速度の修正量を求め、さらにこれを積分して逆位相腕振り角の修正量を求める ₍ 最後に、 S 3 0 3 2で生成した今回歩容の逆位相腕振り角および角速度にそれぞれ、逆位相腕振り角速度の修正量および逆位相腕振り角の修正量を加えることにより、今回歩容の逆位相腕振り角および角速度を修正する。

次いで S 3 0 3 6に進み、時刻 t に制御周期△ tを加え、再び、 S 3 0 1 4に戻り、制御周期毎のタイマー割り込みを待つ。

第 2参考例では、以上のごとく、歩容生成装置 1 0 0における目標歩容生成処理が行なわれ、目標上体位置姿勢、目標足平位置姿勢、目標腕姿勢（逆位相腕振り角を含む）、目標. Z M Pおよび目標全床反力の瞬時値が逐次決定されて出力される。

この場合、目標全床反力は、コンプライアンス制御に必要な成分だけを出力しても良い。なお、目標 Z M Pは、目標全床反力に含まれるが特に重要であるので、あえて出力として挙げた。また、複合コンプライアンス制御装置（図 5 3の点線で囲った部分）に対しては、モデル操作床反力モーメント水平成分は、目標床反力として出力されない。すなわちコンプライアンス制御に対しては、目標 Z M Pまわりの床反力モ一メント水平成分が 0になることを目標とする目標床反力（本来の意味での目標 Z M Pを満足する目標床反力）が、歩容生成装置 1 0 0から出力される。

また、複合コンプライアンス制御装置に対しては、 S 3 0 3 4において修正された今回歩容の床反力モーメント鉛直成分が目標値として歩容生成装置 1 0 0から出力される。

以上説明した第 2参考例の作用 1 として、目標歩容の運動は、モデル操作床反力モーメント水平成分が目標 Z M Pまわりに発生するように生成されつつ、実際のロポッ 1の床反力は、モデル操作床反力モーメント水平成分が付加されないように制御される。したがって、差し引きモデル操作床反力モーメント水平成分の分だけ、目標歩容の運動と床反力の不平衡（アンバランス）を生じる。このことは、実ロボット 1の上体姿勢傾斜角と目標歩容の上体姿勢傾斜角との差を 0に収束させる効果に関して言えば、モデル操作床反力モーメント水平成分の符号を反転した床反カモ一メント水平成分を実ロボット 1に作用させることと等価である, つまり、モデル操作床反力モーメント水平成分を適宜決定することにより、実口ポット 1を、修正目標歩容（実口ポット 1の上体姿勢傾斜角と目標歩容の上体姿勢傾斜角との差を 0に収束させる歩容）に収束させることができる。つまり、実口ポット 1の姿勢傾斜を安定化させることができる。

作用 2として、モデル操作床反力モーメント水平成分の符号を反転したモ一メントとコンプライアンス制御用目標床反力モーメント水平成分との和がトータルの傾斜復元力（口ポット iの実上体姿勢傾斜角を目標上体姿勢傾斜角に復元させる力) になる。すなわち、コンプライアンス制御用目標床反力モーメント水平成分とモデル操作床反力モーメント水平成分との差がトータルの姿勢傾斜復元力になる。

また、作用 3として、モデル操作床反力モーメント水平成分は、 Z M P存在可能範囲を無視していかなる値でも取ることが可能であるので、非常に高い姿勢傾斜復元力を発生することができる。

また、作用 4として、床反力水平成分が、床反力水平成分許容範囲を越えないように上体並進モードの上体並進加速度と上体傾斜モードの上体姿勢傾斜角加速度とが決定されるので、走行歩容における支持脚側脚体 2の離床直前や着床直後など、大きな床反力水平成分を発生できない時期や、摩擦係数の小さい床をロボット 1が移動する場合においても口ポット 1のスリップを防止することができる。

また、作用 5として、床反力の並進力鉛直成分が 0の時期、すなわち，両脚がいずれも接地していない時期では、床反力水平成分の許容範囲が 0に設定されることにより、前記した第 2参考例のアルゴリズムにより自動的に、上体並進モードに依存せずに上体傾斜モードに依存した姿勢傾斜復元が行われ、床と足底の間の摩擦力に依存せずに姿勢復元が行われる。したがって、この時期（空中期）においても、単に、上体並進モ —ドを修正するだけの方式と異なり、有効に姿勢傾斜復元作用が働く。なお、この時には、床反力水平成分が 0になるように生成されるから、歩容の全体重心水平加速度は 0となる。

また、作用 6として、上記したごと.く、コンプライアンス制御に対しては、モデル操作床反力モーメント水平成分は、目標床反力として出力されない。すなわち、目標 Z M Pまわりにモデル操作床反力モーメント水平成分を発生させるように歩容生成が行なわれても、コンプライアンス制御に対しては、目標 Z M Pまわりの床反.カモ一メント水平成分が 0 になることを目標とする目標床反力が、歩容生成装置 1 0 0から与えられる。したがって、コンプライアンス制御による床反力制御を妨げず、コンプライアンス制御による床反力制御を適切に行うことができる。より具体的には、足平 2 2の本来の接地性が低下したり、足平 2 2の底面が浮いてしまうような不具合が発生することを防止または抑制することができる。

なお、後述するように、第 3参考例以降でも、目標 Z M Pまわりのコンプライアンス制御用目標床反力モーメント水平成分は、床反力モーメント水平成分許容範囲を越えないように決定される。

なお、作用 1から 6は、本願出願人が先に提案した P C T Z J P 0 3 / 0 0 4 3 5の技術と同様である。

作用 7として、目標歩容の運動は、モデル操作床反力モーメント鉛直成分が目標 Z M Pまわりに付加的に発生するように生成されつつ、実際のロボット 1の床反力は、歩容生成装置 1 0 0によりモデル操作床反力モ一メント鉛直成分が付加された目標歩容に釣り合う目標床反力モーメント鉛直成分に補償全床反力モーメント鉛直成分 Mdmdz を加えたものを目標値として、複合コンプライアンス制御により前記目標値に近づくように実床反力が制御される。 Mdmdzが大きくなつてくると、 Mdmdz と逆方向のモデル操作床反力モーメント鉛直成分が目標歩容に付加されることから、複合コンプライアンス制御により前記目標値に近づくように実床反力を制御するようにしても、実床反力のモーメントの鉛直成分が過大になることがなく、その結果、 .実ロボット 1のスピンを生じることなく、実ロボット 1の上体姿勢ョ一角及び又は上体姿勢ョ一角速度と目標歩容の上体姿勢ョ一角及び Z又はョー角速度との差を 0に収束させる効果を奏することができる。

つまり、モデル操作床反力モーメント鉛直成分を適宜決定することにより、実口ポット 1を、そのスピンが生じないようにしつつ、修正目標歩容（実ロポット 1の上体姿勢ョ一角及び又は上体姿勢ョー角速度と目標歩容の上体姿勢ョ一角及び/又は上体姿勢ョ一角速度との差を 0に収束させる歩容) に収束させることができる。つまり、実ロボット 1のョ一回転を安定化させることができる。

作用 8として、補償全床反力モーメント鉛直成分 Mdmdzがトータルのョ一回転復元力になる。補償全床反力モーメント鉛直成分 Mdmdzはョ一角偏差およびまたはョ一角速度偏差を 0に近づけるようにフィードバック制御則に基づき決定されるので、ョ一角偏差の制御的な安定性が保証されつつ、ョ一角偏差および zまたはョ一角速度偏差を 0に近づけることができる。

また、作用 9として、モデル操作床反力モーメント鉛直成分は、床反力モーメント鉛直成分の許容範囲（あるいは摩擦限界）を無視していかなる値でも取ることが可能であるので、非常に高い姿勢ョ一回転復元力を発生することができる。

また、作用 1 0として、コンプライアンス制御に対しては、最終的に目標床反力モーメント鉛直成分が、床反カモ一メント鉛直成分許容範囲と床反力モーメント鉛直成分補償量許容範囲の和の範囲を越えないように決定されるので、走行歩容における支持脚側脚体 2の離床直前や着床直後など、大きな床反力モーメント鉛直成分を発生できない時期や、摩擦係数の小さい床を口ポット 1が移動する場合においてもコンプライアンス制御による床反力制御を適切に行うことができ、口ポット 1のスピンを防止することができる。より具体的には、足平 2 2の本来の接地性が低下したり、足平 2 2の底面が浮いてしまうような不具合が発生することを防止または抑制することができる。

また、作用 1 1 として、床反力の並進力鉛直成分が 0の時期、すなわち、両脚がいずれも接地していない時期では、床反力モーメント鉛直成分許容範囲と床反力モーメント鉛直成分補償量許容範囲が 0に設定されることにより、前記した本第 2参考例のアルゴリズムにより自動的に、実床反力モーメント鉛直成分に依存せずに逆位相腕振りモードに依存したョ一回転復元が行われ、床と足底の間の摩擦力に依存せずにョ一回転復元が行われる。したがって、この時期（空中期）においても、単に、コンプライアンス制御の目標床反力モーメント鉛直成分を修正するだけの方式と異なり、有効にョ一回転復元作用が働く。

作用 1 2 として、目標 Z M Pまわりに発生するモーメント水平成分が 0になるように生成したとした場合の歩容を元歩容、前記第 2参考例の如く目標 Z M Pまわりに発生するモ一メント水平成分がモデル操作床反力モーメント水平成分になり、且つ、目標 Z M Pまわりに発生するモーメント鉛直成分に付加的にモデル操作床反力モー'メント鉛直成分が発生するように生成される歩容を修正歩容と呼ぶと、通常、元歩容と修正歩容とは異なる歩容となる。元歩容は定常歩容に漸近するように設定されているので、修正歩容は、通常、定常歩容に漸近しない歩容となる。

しかし、今回歩容（修正歩容）の生成が完了した直後に、再び S 3 0 2 0から S 3 0 2 8が実行され、修正歩容の終端状態を新たな初期状態とした新たな今回歩容が、新たに設定される定常歩容に漸近するように，新たな今回歩容パラメ一夕が決定されるので、継続的に（長期的に）安定性が保証された歩容を生成し続けることができる。

上記した作用 1 2は、本願出願人が先に提案した P C T Z J P 0 3 / 0 0 4 3 5の技術とほぼ同様である。但し、これに加えて本第 2参考例では、次のような作用も発生する。すなわち、ョ一角回転復元のために修正された逆位相腕振り角および角速度の終端状態を新たな初期状態とした新たな今回歩容の逆位相腕振り角軌道が新たに設定される定常歩容の逆位相腕振り角軌道に漸近するように、新たな今回歩容の逆位相腕振り角軌道に関するパラメータが決定されるので、継続的に（長期的に）逆位相腕振り角の安定性が保証された歩容を生成し続けることができる c なお、本第 2参考例では、補償全床反力モーメント水平成分 Mdmdxy が床反力モーメント水平成分許容範囲内の値であるときには, モデル操作床反カモ一メント水平成分は 0になるが、このときのモデル操作床反力モーメント水平成分を図 1 2の動力学モデルの状態量（例えば動力学モデル上での口ポットの重心位置や、上体質点 3 mの位置等）に応じて設定するようにしてもよい。

また、本第 2参考例では、補償全床反力モーメント鉛直成分 Mdmdz が床反力モーメント鉛直成分補償量許容範囲内の値であるときには、モデル操作床反力モーメント鉛直成分補償量は 0になるが、このときのモデル操作床反カモ一メント鉛直成分補償量を図 1 2の動力学モデルの状態量（例えば動力学モデル上でのロボット 1の逆位相腕振り角および角速度、上体ョ一角および角速度等）に応じて設定するようにしてもよい

次に本発明の第 3参考例を図 5 9〜図 6 2を参照して説明する。なお. 第 3参考例の説明において、前記第 1参考例もしくは前記第 2参考例と同一構成部分もしくは同一機能部分については前記第 1参考例もしくは第 2参考例と同一の参照符号を用いて説明を省略する。第 3参考例は、前記第 1参考例で前記図 1〜図 3および図 5〜図 1 2を参照して説明した事項は、前記第 1参考例と同一である。

第 3参考例が前記第 1参考例および第 2参考例と相違する点の概要を説明すると、元歩容と修正歩容が同時に生成される。修正歩容は、実口ポット 1の上体姿勢（傾斜角およびョ一角）.の安定化のために元歩容から修正される。さらに、該修正歩容は、コンプライアンス制御により姿勢復元に必要な床反カモ一メントを発生してもまだ余裕がある (目標 Z M Pまわりに発生可能な床反カモ一メントに余裕がある）場合には、この余裕を用いて、可能な範囲で元歩容に収束するように決定される。第 3参考例における制御ュニット 6 0の機能的構成を表すプロック図を図 5 9に示す。第 3参考例では、姿勢傾斜安定化制御演算部 1 1 2が求める補償全床反力モーメント水平成分 Mdmdxy は、歩容生成装置 1 0 0に入力される。

また、ョー安定化制御演算部 1 1 3が求める補償全床反力モーメント鉛直成分 Mdmdzも、歩容生成装置 1 0 0に入力される。

そして、この Mdmdxy および Mdmdz を基にモデル操作床反力モーメント（水平成分および鉛直成分）とコンプライアンス制御用目標床反力モーメント（水平成分および鉛直成分）とを決定する補償全床反カ乇 —メント分配器 1 2 0が、歩容生成装置 1 0 0に組み込まれ、該歩容生成装置 1 0 0からコンプライアンス制御用目標床反力モーメントが複合コンプライアンス動作決定部 1 0 4に出力される。そして、歩容生成装置 1 0 0内の補償全床反力モーメント分配器 1 2 0は後述するように、前記第 2参考例の補償全床反力モーメント水平成分分配器 1 1 0およびモデル操作床反力モーメント鉛直成分決定器 1 1 1よりも複雑な処理を行う。これ以外の、制御ユニット 6 0の機能的構成は、前記第 2参考例と同一である。

図 6 0に第 3参考例における歩容生成装置 1 0 0のメインル一チン処理のフ口一チャートを示す。

同図 6 0おいて、 S 2 0 1 0から S 2 0 2 8までは、前記第 1参考例のメインフローチャート（図 1 3) の S 0 1 0から S 0 2 8までの処理と同一の処理が行なわれる。なお、 S 0 2 3 (本実施形態では S 2 0 2 8 ) のサブルーチンである図 4 3のフローチャートの S 8 0 0における初期化においては、今回歩容初期状態は、前回修正歩容（歩容生成装置 1 0 0が最終的に出力する歩容) の終端状態を今回支持脚座標系に変換したものであり、後述の S 2 0 3 2で決定される元歩容の終端状態は、 S 2 0 2 8のサブルーチンの S 8 0 0では用いられない。

次いで S 2 0 3 0に進み、コンプライアンス制御用の床反力モ一メント水平成分許容範囲を決定する。この床反カモ一メント水平成分許容範囲の決定方法は、第 2参考例の S 3 0 3 2 (図 5 6 ) と同様である。

S 2 0 3 0の処理を完了した後、あるいは S 2 0 1 6の判断結果が N Oである場合には、 S 2 0 3 2に進み、元歩容の瞬時値（時刻 tの今回値）が決定される。元歩容は、目標 Z MPまわりの床反力モーメント水平成分が 0になるように生成される歩容である。

この元歩容は、前記第 2参考例の図 5 6の S 3 0 3 2のサブルーチン処理の一部を変更したアルゴリズムによって生成される。すなわち、 S 3 0 3 2内のサブルーチン処理（詳しくは S 3 0 3 2のサブルーチン処理である図 5 7の S 3 4 1 4のサブルーチン処理）である図 5 8の S 3 1 0 4および S 3 1 3 0において、モデル操作床反力モ一メント水平成分を 0 として（目標 Z MPまわりの目標床反力モーメント水平成分を 0 として）、上体水平加速度 atmp が求められる。これ以外の処理は、図 5 6の S 3 0 3 2の処理と同一でよい。

次いで、 S 2 0 3 4に進み、修正歩容の瞬時値を決定する。なお、修正歩容は、歩容生成装置 1 0 0から最終的に出力される目標歩容である, S 2 0 34の処理は、図 6 1のフローチヤ一トで示すサブルーチン処理により行なわれ、以下にその詳細を説明する。

まず S 2 1 0 0から S 2 1 1 1までは、第 2参考例で説明した図 5 7 の S 3 4 0 0から S 3 4 1 1 と同一の処理が実行される。

次いで、 S 2 1 1 2に進み、歩容パラメ一夕を基に現在時刻での床反力モーメント水平成分許容範囲 [Mxymin, Mxymax]を求める。これは, 図 5 7の S 3 4 1 2での床反カモ一メント水平成分許容範囲 [Mxymin， Mxymax]の決定と同様に行われる。

次いで、 S 2 1 1 4に進み、床反カモ一メント水平成分許容範囲と床反力モーメント鉛直成分許容範囲と床反力水平成分許容範囲との条件を満足するように、モデル操作床反力モーメント（水平成分および鉛直成分）、コンプライアンス制御用目標床反力モーメント、上体水平加速度，上体姿勢傾斜角加速および逆位相腕振り角加速度が決定される。

S 2 1 1 4の詳細を、その処理を示す図 6 2のフローチャートを用いて、以下に説明する。

まず、 S 2 2 0 0において、修正歩容の上体水平位置と元歩容の上体水平位置との差であるモデル間上体水平位置差が求められる。この時点では、修正歩容の上体水平位置の今回値（時刻 tでの値）は未だ求められていない。そこで、 S 2 2 0 0では、修正歩容の上体水平位置の前回値（時刻 t 一 Δ tの制御周期で最終的に求められた値）と、元歩容の上体水平位置の前回値（時刻 t 一△ tの制御周期で S 2 0 3 2において求められた値）あるいは今回値（時刻 tの制御周期で S 2 0 3 2において求められた値）とを用いて、モデル間上体水平位置差が算出される。次いで、 S 2 2 0 2に進み、修正歩容の上体姿勢傾斜角と元歩容の上体姿勢傾斜角との差であるモデル間上体姿勢傾斜角差が求められる。この S 2 2 0 2では、 S 2 2 0 0のモデル間上体水平位置差の算出処理の場合と同様、修正歩容の上体姿勢傾斜角の前回値と、元歩容の上体姿勢傾斜角の前回値又は今回値とを用いてモデル間上体姿勢傾斜角差が求められる。

次いで、 S 2 2 0 4に進み、修正歩容の逆位相腕振り角と元歩容の逆位相腕振り角との差であるモデル間逆位相腕振り角差が求められる。この S 2 2 0 4では、 S 2 2 0 0のモデル間上体水平位置差の算出処理の場合と同様、修正歩容の逆位相腕振り角の前回値と、元歩容の逆位相腕振り角の前回値又は今回値とを用いてモデル間逆位相腕振り角差が求められる。

次いで、 S 2 2 0 6に進み、モデル間上体水平位置差を基に、その差を 0に収束させるために必要なモデル上体水平位置安定化床反力モーメント要求値 Mpfdmd が決定される。単に修正歩容の上体並進モードの上体水平加速度を発生させる床反力モーメントを元歩容の上体並進モードの上体水平加速度を発生させる床反カモ一メントに一致させると、モデル間上体水平位置差は発散する。モデル上体水平位置安定化床反カモ —メント要求値 Mpfdmd は、前記上体並進モードによって修正歩容の上体水平位置を元歩容の上体水平位置に戻すよう動作を行った時に、これに伴って発生する床反力モーメントから元歩容の上体並進モ一ドの上体水平加速度を発生させる床反カモ一メントを減じたモーメントとしての意味を持つ。

具体的には、モデル上体水平位置安定化床反力モーメント要求値 Mpfdmd は、例えば次式のフィードバック制御則によって決定される, この例では、フィードバック制御則として、 P D制御則を用いているが. P I D等、他のフィードバック制御則を用いてもよい。

Mpfdmd = Kmp *モデル間上体水平位置差

+ Kmpv *モデル間上体水平位置差の時間微分値

式 d 2 8 但し、 Kmp、 Kmpv はフィードバックゲイン（比例ゲイン、微分ゲイン）である。次いで、 S 2 2 0 8に進み、モデル間上体姿勢傾斜角差を基に、その差を 0に収束させるために必要なモデル上体姿勢傾斜角安定化床反カモ一メント要求値 Mrfdmd が決定される。単に修正歩容の上体傾斜モードの上体姿勢傾斜角加速度を発生させる床反カモ一メントを元歩容の上体傾斜モードの上体姿勢傾斜角加速度を発生させる床反力モーメントに一致させると、モデル間上体姿勢傾斜角差は 0に収束しない。モデル上体姿勢傾斜角安定化床反力モーメント要求値 Mrfdmd は、上体傾斜モ一ドによって修正歩容の上体姿勢傾斜角を元歩容の上体姿勢傾斜角に戻すよう動作を行った時に、これに伴って発生する床反力モーメントから元歩容の上体傾斜モードの上体姿勢傾斜角加速度を発生させる床反カモ一メントを減じたモーメントとしての意味を持つ。

具体的には、モデル上体姿勢傾斜角安定化床反力モーメント要求値 Mrfdmdは、例えば次式のフィードバック制御則によって決定される。この例では、フィードバック制御則として、 P D制御則を用いているが. P I D等、他のフィードバック制御則を用いてもよい。

Mrfdmd= Kmr *モデル間上体姿勢傾斜角差

+ Kmrv *モデル間上体姿勢傾斜角差の時間微分値

……式 d 2 9 但し、 Kmr、 Kmrv はフィードバックゲイン (比例ゲイン、微分ゲイン）である。 . 次いで、 S 2 2 1 0に進み、モデル間逆位相腕振り角差を基に、その差を 0に収束させるために必要なモデル逆位相腕振り角安定化床反カモ —メント要求値 Mafdmd が決定される。単に修正歩容の逆位相腕振りモードの逆位相腕振り角加速度を発生させる床反力モーメントを元歩容の逆位相腕振りモードの逆位相腕振り角加速度を発生させる床反力モーメントに一致させると、モデル間逆位相腕振り角は 0に収束しない。モデル逆位相腕振り角安定化床反力モーメント要求値 Mafdmd は、逆位相腕振りモードによつて修正歩容の逆位相腕振り角を元歩容の逆位相腕振り角に戻すよう動作を行った時に、これに伴って発生する床反力モーメン卜から元歩容の逆位相腕振りモードの逆位相腕振り角加速度を発生させる床反カモ一メントを減じたモーメントとしての意味を持つ。

具体的には、モデル逆位相腕振り角安定化床反力モーメント要求値 Mafdmdは、例えば次式のフィードバック制御則によって決定される。この例では、フィードバック制御則として、 P D制御則を用いているが、 P I D等、他のフィードバック制御則を用いてもよい。

Mafdmd= Ka *モデル間逆位相腕振り角差 + Kav *モデル間逆位相腕振り角差の時間微分値

……式 d 2 9 b 但し、 Kar、 Kav はフィードバックゲイン (比例ゲイン、微分ゲイン）である。ところで、最終的に決定された修正歩容の上体並進モードに伴って発生する床反カモ一メント水平成分から元歩容の上体並進モードの上体水平加速度を発生させる床反力モーメント水平成分を減じたモ一メントをモデル上体水平位置安定化床反力モーメントと呼ぶ。また、最終的に決定された修正歩容の上体傾斜運動モ一ドに伴って発生する床反力モーメント水平成分から元歩容の上体傾斜運動モードの上体姿勢傾斜角加速度を発生させる床反力モーメント水平成分を減じたモーメントを、モデル上体姿勢傾斜角安定化床反力モーメン卜と呼ぶ。また、最終的に決定された修正歩容の逆位相腕振りモードに伴って発生する床反力モーメント鉛直成分から元歩容の逆位相腕振りモ一ドの.逆位相腕振り角加速度を発生させる床反力モーメン卜鉛直成分を減じたモーメントを、モデル逆位相腕振り角安定化床反力モーメントと呼ぶ。

一方、摂動運動と摂動床反力とには線形性が近似的に成立する、すなわち、異なる摂動運動を元歩容の運動に加えた運動の床反力は、元歩容の床反力にそれぞれの摂動運動が発生する摂動床反力を加えたものにほぼ一致する。また、逆位相腕振りモードでは、床反力モーメント水平成分は変化しない。したがって、次式が近似的に成立する。モデル操作床反力モーメント水平成分

=モデル上体水平位置安定化床反力モーメント .

+モデル上体姿勢傾斜角安定化床反カモ一メント式 d 3 0 この式 d 3 0が近似的に成立することと、逆位相腕振り角加速度に比例して床反カモ一メン卜鉛直成分が変化することとを考慮しつつ、モデル上体水平位置安定化床反力モーメントをモデル上体水平位置安定化床反力モーメント要求値 Mpfdmd に一致またはなるべく近くなるように決定し、かつ、モデル上体姿勢傾斜角安定化床反力モーメントをモデル上体姿勢傾斜角安定化床反力モーメント要求値 Mrfdmd に一致またはなるべく近くなるように決定しつつ、かつ、モデル逆位相腕振り角安定化床反力モーメントをモデル逆位相腕振り角安定化床反力モーメント要求値 Mafdmd に一致またはなるべく近くなるように決定すれば、修正歩容に適切なモデル操作床反カモ一メントを発生させて、以下に示す復元条件を満足しつつ、修正歩容の上体水平加速度と上体姿勢傾斜角加速度とを、それぞれ元歩容の上体水平加速度と上体姿勢傾斜角加速度とに可能な範囲で収束させることができる。

そこで、 S 2 2 1 0の後、 S 2 2 1 2に進み、以下の条件 (これらを復元条件と呼ぶ) をできる限り満足するようにモデル上体水平位置安定化床反力モーメント（上体並進モードの床反力モーメント）とモデル上体姿勢傾斜角安定化床反力モーメント（上体傾斜モードの床反力モ一メント）とモデル逆位相腕振り角安定化床反力モーメントとを決定する。さらに、上記のモデル上体水平位置安定化床反力モーメントとモデル上体姿勢傾斜角安定化床反カモ一メントとモデル逆位相腕振り角安定化床反力モーメントとの定義を満足するように、修正歩容の上体水平加速度と上体姿勢傾斜角加速度と逆位相腕振り角加速度とを決定する。なお、以下に示す復元条件は、番号が小さい条件ほど優先度が高いものとする, つまり相反して両立できない条件がある場合には、番号が小さい条件を優先して満足（成立）させる。ただし、復元条件 1、 2および 3は必ず満足（成立）させる。

復元条件 1 ) 補償全床反力モーメント水平成分 Mdmdxy とモデル操作床反力モーメントの和（これは、前記式 d 2 7 bが成立する場合にはコンプライアンス制御用目標床反力モーメント水平成分に相当する）が床反力モーメント水平成分許容範囲を越えないこと。

復元条件 2 ) 修正歩容の床反力水平成分が床反力水平成分許容範囲を越えないこと。

復元条件 3 ) 修正歩容の床反カモ一メント鉛直成分と補償全床反カモ一メント鉛直成分 Mdmdzの和（これは、コンプライアンス制御用目標床反力モーメント鉛直成分に相当する）が、床反力モーメント鉛直成分許容範囲を越えないこと。

復元条件 4 ) モデル上体姿勢傾斜角安定化床反力モーメントが、モデル上体姿勢傾斜角安定化床反力モーメント要求値 Mrfdmd に一致またはできる限り近いこと。この条件は、修正歩.容の上体姿勢傾斜角が元歩容 (当初予定した歩容) の上体姿勢傾斜角に収束するための条件である, 復元条件 5 ) モデル上体水平位置安定化床反力モーメントが、モデル上体水平位置安定化床反力モーメント要求値 Mpfdmd に一致またはできる限り近いこと。この条件は、修正歩容の上体水平位置が元歩容（当初予定した歩容）の上体水平位置に収束するための条件である。

復元条件 6 ) モデル逆位相腕振り角安定化床反力モーメントが、モデル逆位相腕振り角安定化床反力モーメント要求値 Mafdmd に一致またはできる限り近いこと。この条件は、修正歩容の逆位相腕振り角が元歩容（当初予定した歩容）の逆位相腕振り角に収束するための条件である, 復元条件 7 ) モデル上体姿勢傾斜角安定化床反力モーメント、モデル上体水平位置安定化床反力モーメントおよびモデル逆位相腕振り角安定化床反力モーメントがそれぞれ連続であること。

上記のような復元条件 1 〜 6を満足するような上体水平加速度、上体姿勢傾斜角加速度、逆位相腕振り角加速度等を決定する S 2 2 1 2の処理は具体的には例えば次のように実行される。

まず、上記復元条件 1 、 2、 4および 5を満足するように、モデル上体水平位置安定化床反力モーメントおよびモデル上体姿勢傾斜角安定化床反カモ一メントが決定され、さらに上体水平加速度および上体姿勢傾斜角加速度が決定される。この処理の詳細は、本願出願人が先に提案した P C T / J P 0 3 / 0 0 4 3 5の技術に記載されているので、ここでは説明を省略する。

次いで、上記復元条件 3および 6を満足するようにモデル逆位相腕振り安定化床反力モーメントが決定され、さらに逆位相腕振り角加速度が決定される。

具体的には、上記のごとく決定された上体水平加速度および上体姿勢傾斜角加速度と元歩容の逆位相腕振り角加速度 β aorg で仮に運動した場合に発生する（運動に動力学的に釣り合う）目標 Z M Pまわりの床反力モーメント鉛直成分を求める。以降、これを無補正時床反力モーメント鉛直成分と呼ぶ。

次いで、次式のように、無補正時床反力モーメント鉛直成分とモデル逆位相腕振り角安定化床反力モーメント要求値 Mafdmd と補償全床反力モーメント鉛直成分 Mdmdzとの和 Msumzを求める。

Msumz=無補正時床反力モーメント鉛直成分

+ Mafdmd + Mdmdz 次いで、次式により、モデル逆位相腕振り安定化床反力モーメントが決定される。

Msumz>床反力モーメント鉛直成分許容範囲上限値である場合

モデル逆位相腕振り安定化床反力モーメント =— Mdmdz

一無補正時床反力モーメント鉛直成分

+床反力モーメント鉛直成分許容範囲上限値

Msumz<床反力モーメント鉛直成分許容範囲下限値である場合

モデル逆位相腕振り安定化床反力モ一メント=一 Mdmdz

一無補正時床反力モーメント鉛直成分

+床反力モーメント鉛直成分許容範囲下限値床反力モーメント鉛直成分許容範囲下限値≤Msmnz、且つ、 Msumz≤ 床反力モーメン卜鉛直成分許容範囲上限値である場合

モデル逆位相腕振り安定化床反力モーメント

· =モデル逆位相腕振り角安定化床反力.モーメント要求値 Mafdmd

……式 d 2 6 c 次いで、修正歩容の逆位相腕振り角加速度が次式によって求められる修正歩容の逆位相腕振り角加速度

=元歩容の逆位相腕振り角加速度 β aorg

+モデル逆位相腕振り安定化床反力モーメント

宛振り運動の等価慣性モーメント A Maz 修正歩容の床反力モーメント鉛直成分は、元歩容の逆位相腕振り角加速度 0 aorg とモデル逆位相腕振り安定化床反力モ一メントの和であることから、上式のごとくモデル逆位相腕振り安定化床反力モ一メントを決定することにより、上記復元条件 3が満足される。

上述したように S 2 2 1 2の処理を実行した後、 S 2 2 1 4に進み、前記式 d 3 0により、モデル操作床反力モーメント水平成分が決定される。すなねち、 S 2 2 0 8で求めたモデル上体水平位置安定化床反カモーメントとモデル上体姿勢傾斜角安定化床反カモ一メントとの和がモデル操作床反力モーメント水平成分として決定される。なお、最終的に決定される修正歩容の運動の今回瞬時値に基づいて目標 Z M Pまわりの床反カモ一メントを直接算出して、これをもってモデル操作床反力モ一メン卜としても良い。

次いで、 S 2 2 1 6に進み、前記式 d 2 7 bにより、コンプライアンス制御用目標床反力モーメント水平成分が決定される。すなわち、補償全床反力モーメント水平成分 Mdmdxy と S 2 2 1 4で求めたモデル操作床反力モーメント水平成分との和がコンプライアンス制御用目標床反力モーメント水平成分として決定される。 .

次いで、 S 2 2 1 8に進み、図示する式によって、コンプライアンス制御用目標床反力モーメント鉛直成分を決定する。なお、図示する式における修正歩容に釣り合う（修正歩容の運動に動力学的に釣り合う）床反力モーメント鉛直成分は、無補正時床反力モーメント鉛直とモデル逆位相腕振り安定化床反力モーメントの和であるが、最終的に決定される修正歩容の運動の今回瞬時値に基づいて目標 Z M Pまわりの床反カモ一メント鉛直成分を直接算出しても良い。

以上により、図 6 1の S 2 1 1 4の処理が終了し、次に、 S 2 1 1 6 に進む。この S 2 1 1 6の処理は、前記第 2参考例における図 5 7の S 3 4 1 6と同一であり、上体水平加速度の 2階積分により上体水平位置の今回値が決定されると共に、上体姿勢傾斜角加速度の 2階積分により上体姿勢傾斜角の今回値が決定される。

次に、 S 2 1 1 8に進む。この S 2 1 1 8の処理は、前記第 1実施形態における図 5 7の S 3 4 1 8 と同一であり、逆位相腕振り角加速度の 2階積分により逆位相腕振り角の今回値が決定される。

次いで、図 6 0の S 2 0 3 6に進んで、時刻 t に制御周期 Δ t を加え再び、 S 2 0 1 4に戻り、制御周期毎のタイマー割り込みを待つ。

補足すると、第 3参考例における動力学モデルによる歩容瞬時値決定においては、動力学モデルの状態量（あるいは前回、前前回の歩容瞬時値）も必要であるので、修正歩容生成用と元歩容生成用との 2つの動力学モデルが必要である。第 3参考例では、それらの動力学モデルは図 1 2に示した動力学モデルである。

本実施形態においては、上記のごとく、元歩容と修正歩容とが並列的に生成され、修正歩容は、実ロボット 1の姿勢 (傾斜角およびョ一角) 安定化のために修正されると共に、コンプライアンス制御により姿勢復元に必要な床反力モーメント（水平成分および鉛直成分）を発生してもまだ余裕がある場合には、この余裕を用いて、可能な範囲で元歩容に収束するようにしている。このため、前記第 2参考例の作用効果に加え、当初設定した元歩容に近い、すなわち、当初の要求通りの歩容に近い歩容を生成することができる。したがって、あらかじめ設定された移動経路がある場合には、移動経路から大きくずれることを防止することが可能となる。また、修正歩容の上体姿勢傾斜角が元歩容（当初決定した歩容）の上体姿勢傾斜角に収束することを、修正歩容の上体水平位置が元歩容（当初決定した歩容）の上体水平位置に収束することよりも優先した（床反力水平成分許容範囲を満たす範囲で可能な限り上体並進モードの運動を調整した）ので、上体姿勢傾斜角が大きく変動することを抑制することができる。次に第 4参考例を図 6 3〜図 7 0を参照して説明する。なお、本第 4 参考例の説明で、前記第 1〜第 3参考例と同一構成部分もしくは同一機能部分については該第 1〜第 3参考例と同一の参照符号を用いて詳細な説明を省略する。

本第 4参考例では、制御ユニット 6 0の機能的構成は、前記第 3参考例のもの、すなわち前記図 5 9に示したものと同一である。但し、第 4 参考例では、歩容生成装置 1 0 0が実行する歩容生成のアルゴリズムは. 前記第 3参考例のものと相違している。そして、歩容生成装置 1 0 0以外の各部の処理は、前記第 3参考例と同一である。

図 6 3は、本実施形態における歩容生成装置 1 0 0の処理の概要を示すブロック図である。この図 6 3を参照して、歩容生成装置 1 0 0の処理の概要を以下に説明する。なお、図 6 3を用いて以下に説明する処理の概要は、後述する第 5〜第 7参考例についても同様である。また、第

4参考例および後述の第 5〜第 7参考例では、前記図 1 2の動力学モデルを単純化モデルと称する。

図示の如く、歩容生成装置 1 0 0は歩容パラメータ決定部 1 0 0 aを備える。歩容パラメータ決定部 1 0 0 aは、目標歩容のパラメータ（目標歩容を規定するパラメ一夕）の値あるいは時系列テーブルを決定する < これは、後述の図 6 5のフローチャートの S 3 5 2 0から S 3 5 3 0までの処理に相当する。

詳細は後述するが、歩容パラメータ決定部 1 0 0 aが決定するパラメ一夕には、目標足平位置姿勢軌道、目標腕姿勢軌道、基準上体姿勢軌道，目標 Z M P軌道、目標床反力鉛直成分軌道等を規定するパラメ一夕が含まれる他、床反力水平成分許容範囲を規定するパラメ一夕と、 Z M P許容範囲（もしくは床反力モーメント水平成分許容範囲）を規定するパラメータと、床反力モーメント鉛直成分許容範囲を規定するパラメ一夕とが含まれる。この場合、第 4参考例で設定する床反力水平成分許容範囲および床反カモ一メント鉛直成分許容範囲は、後述の S 3 5 2 6の処理で設定される単純化モデル歩容用のものと、 S 3 5 3 0で設定される歩容補正用のものとの 2種類がある。一方、 ZMP許容範囲（もしくは床反力モーメント水平成分許容範囲）は、 S 3 5 3 0の処理で設定されるフルモデル補正用（歩容補正用）のものだけである。なお、第 4参考例では、 S 3 5 3 0では ZMP許容範囲を規定するパラメ一夕を設定するが、これは、床反力モーメント水平成分許容範囲を規定するパラメ一夕と設定することと同等である。これは、前記第 2参考例の図 5 6の S 3 0 3 0に関して説明した如く、床反力モーメント水平成分を、目標床反力鉛直成分で割ったものが、 Z MP (床反力中心点) の目標 Z M Pからのずれ量を表すものとなるからである。

· ここで、第 4参考例で設定する Z M P許容範囲は、図 6 4の如く設定される。詳細は、 P C T/ J P 0 3 / 0 0 4 3 5に詳細に記載されているのでここでは、これ以上の説明を省略する。

歩容パラメ一夕決定部 1 0 0 aで決定された歩容パラメータは目標瞬時値発生部 1 0 0 bに入力される。目標瞬時値発生部 1 0 0 bは入力された歩容パラメータに基づき、目標足平位置姿勢、目標 ZMP、目標床反力鉛直成分、目標腕姿勢、目標全体重心鉛直位置、目標上体鉛直位置. 床反力水平成分許容範囲、 ZMP許容範囲、基準上体姿勢角および基準逆位相腕振り角の、現在時刻 t における瞬時値を逐次算出（発生）する (図 6 3では一部の目標瞬時値のみを記載している）。この目標瞬時値発生部 1 0 O bの処理は、後述の図 6 5のフローチャート S 3 5 3 2の処理のなかで実行される図 5 7の S 3 4 0 0〜S 3 4 1 2の処理、並びに、図 6 5の S 3 5 3 4の処理に相当する。なお、第 4参考例では、目標瞬時値発生部 1 0 0 bが算出する目標瞬時値のうち、一部の瞬時値 (具体的には目標上体鉛直位置の瞬時値）は仮値であり、後に修正される。また、目標瞬時値発生部 1 0 0 bが算出する床反力水平成分許容範囲および床反力モーメント鉛直成分許容範囲の瞬時値は、単純化モデル歩容用の瞬時値と歩容補正用の瞬時値とがある。

目標瞬時値発生部 1 0 0 bで算出（発生）された目標瞬時値（一部は仮瞬時値）は、フルモデル補正部 1 0. 0 cに入力される。また、フルモデル補正部 1 0 0 cには、前記姿勢傾斜安定化制御演算部 1 1 2 (図 5 9を参照）で求められる補償全床反力モーメント水平成分 Mdmdxy および前記ョー安定化制御演算部 1 1 3 (図 5 9を参照）で求められる補償全床反力モ一メント鉛直成分 Mdmdz も入力される。フルモデル補正部 1 0 0 cは、動力学モデルとして、単純化モデル 1 0 0 c 1 とフルモデル 1 0 0 c 2とを備える。該フルモデル補正部 1 0 0 cは、単純化モデル 1 0 0 c 1に基づいて、入力値から目標上体位置姿勢および逆位相腕振り角の仮瞬時値等を決定すると共に、さらに決定された上体位置姿勢および逆位相腕振り角の仮瞬時値等をフルモデル 1 0 0 c 2を用いて修正する。

尚、単純化モデル 1 0 0 c 1をフル.モデル補正部 1 0 0 c 2に含ませない構成も可能である。また、フルモデル 1 0 0 c 2は、後述するように、逆フルモデル（逆動力学フルモデル）と順フルモデル（順動力学フルモデル）とのいずれかを含む。

フルモデル補正部 1 0 0 cは、基本的には、次の A1〜A4の条件を満足するように、 B の処理を実行する。すなわち、フルモデル補正部 1 0 0 cは、 '

A1) フルモデル補正部 1 0 0 cにより生成される補正歩容の運動に釣り合う床反力モーメントに補償全床反力モーメント水平成分 Mdmdxy と補償全床反カモ一メント鉛直成分 Mdmdz とを加えたものがフルモデル補正部 1 0 0 cから出力するコンプライアンス制御用床反力モ一メントに一致する。

A2) 真の Z M P (目標 Z MPのまわりにコンプライアンス制御用目標床反力モーメントを発生させることによって修正された本来の定義を満足する ZMP) は、 Z MP許容範囲（安定余裕が十分維持できる許容範囲）に存在する。 .

A3) 床反力水平成分は歩容補正用の床反力水平成分許容範囲内にある c A4) 目標 Z M Pのまわりに発生すべきコンプライアンス制御用目標床反力モーメント鉛直成分が床反力モーメント鉛直成分許容範囲内にある _c という条件を満足するように、

B) 単純化モデルを用いて決定された単純化モデル歩容の上体位置姿勢を補正すると共に、目標 Z MPまわりのコンプライアンス制御用目標床反力モーメン卜を出力する。

なお、上記条件 A2 は、目標 Z MPまわりに発生させる床反力モ一メントを ZMP許容範囲に対応する床反カモ一メント水平成分許容範囲内に収めることと同等である。

ここで、上記単純化モデル 1 0 0 c 1 とフルモデル 1 0 0 c 2とについて説明しておく。単純化モデル 1 0 0 c 1は、動力学的精度よりも、演算量低減や挙動解析のし易さに重点を置いた動力学モデルであり、動力学的に一部無視があったり（例えば、重心まわりの角運動量変化を無視したり）、矛盾があるもの（厳密さを欠くもの）でも良い。第 4参考例では前記第 1参考例で説明した図 1 2の動力学モデル（前記式 0 1〜 0 5で記述される動力学モデル）が単純化モデル' 1 0 0 c 1 として用いられる。また、フルモデル 1 0 0 c 2は、単純化モデル 1 0 0 c 1 とは異なるロボット動力学モデルを意味する。これは、単純化モデル 1 0 0 c 1よりも近似精度の高いロポット動力学モデルであることが望ましい。図示例で説明すれば、第 4参考例では先に述べたように前記図 1 2に示した動力学モデルを単純化モデル 1 0 0 c 1 として用いるので、それよりも- 近似精度の高い動力学モデル、例えば前記図 4 9に示した多質点モデル (口ポット 1の各リンクにそれぞれ質点を有するモデル）のようなロボット動力学モデルをフルモデル 1 0 Q c 2として用いることが望ましい < この場合、該フルモデル 1 0 0 c 2は、質点のまわりに慣性モーメントを設定するものであっても良い。

しかし、単純化モデル 1 0 0 c 1 とフルモデル 1 0 0 c 2とは、必ずしもモデルの近似精度が異なる必要はない。単純化モデル 1 0 0 c 1 とフルモデル 1 0 0 c 2 とは、動力学方程式が同じで、床反力水平成分許容範囲およびまたは床反力モーメント鉛直成分許容範囲が異なっている. すなわち、単純化モデル歩容用の許容範囲と、歩容補正用 (フルモデル補正用）の許容範囲とが異なっているだけでも良い。たとえば、単純化モデル 1 0 0 c 1を用いた歩容を生成する際の床反力水平成分許容範囲および床反カモ一メント鉛直成分許容範囲を大きくとり（摩擦限界を越えていても良い）、フルモデル 1 0 0 c 2を用いて歩容を修正する際の床反力水平成分許容範囲および床反力モーメント鉛直成分許容範囲を口ポット 1のスリップもしくはスピンが発生しにくいように狭く設定するだけでも良い。

尚、この明細書において、目標足平位置姿勢、目標床反力（特に、目標 Z M Pと目標床反力鉛直成分）に基づいて（入力して）上体位置姿勢および逆位相腕振り角を算出する（出力させる）のに使用するモデルを「順動力学モデル」と呼び、目標足平位置姿勢、目標上体姿勢、目標上体位置および逆位相腕振り角に基づいて（入力して）床反力（特に、目標 Z M Pもしくは目標 Z M Pまわりの床反力モーメント（水平成分および鉛直成分）と床反力水平成分）を算出する（出力させる）のに使用するモデルを「逆動力学モデル」と呼ぶ。順動力学モデルの入力には、少なくとも目標床反力が含まれ、逆動力学モデルの入力には、少なくとも目標運動が含まれる。

フルモデル補正部 1 0 0 cが備えるフルモデル 1 0 0 c 2は、逆動力学フルモデル（しばしば「逆フルモデル」と略称）または順動力学フルモデル（しばしば「順フルモデル」と略称）を備える。一般的には、逆動力学モデルの演算に比べ、順動力学モデルの演算は、演算量が多くなりがちである。

以上が第 4参考例での歩容生成装置 1 0 0における処理の概要である, 次に、第 4参考例における歩容生成装置 1 0 0の処理を詳説する。第 4参考例における歩容生成装置 1 0 0は、図 6 5のフローチャートに示す処理を実行して、歩容を生成する。

まず、 S 3 5 1 0から S 3 5 2 8までにて、前記第 2参考例で説明した図 5 6の S 3 0 1 0から S 3 0 2 8までの処理と同一の処理が実行される。なお、 S 3 0 2 6のサブルーチンである図 3 9の S 6 0 8で決定する今回歩容の床反力水平成分許容範囲および S 6 1 0で決定する床反力モーメント鉛直成分許容範囲は、前記第 1参考例〜第 3参考例の場合よりも厳密に摩擦力限界を考慮せずともよく、摩擦力限界を超えるような範囲に設定してもよい。これは、後述するフルモデル補正によって、床反力水平成分および床反力モーメント鉛直成分は最終的にフルモデル補正用の床反力水平成分許容範囲および床反力モーメント鉛直成分許容範囲によって制限されることとなるからである。 .

次いで、 S 3 5 3 0に進み、フルモデル補正用（歩容補正用）の床反力水平成分許容範囲と床反力モーメント鉛直成分許容範囲と Z M P許容範囲（床反力中心点許容範囲）とをそれぞれ規定するパラメ一夕を決定する。この場合、フルモデル補正用の床反力水平成分許容範囲は、例えば前記単純化モデル歩容用の床反力水平成分許容範囲と同様、 X軸方向 (前後方向）、 Y軸方向（左右方向）のそれぞれについて、床反力鉛直成分軌道と、前記式 1 2とに基づいて、前記図 3 0に示したようなパ夕ーンに設定される。そして、例えば、前記式 1 2の ka * tの値がフルモデル補正用の床反力水平成分許容範囲を規定するパラメータとして設定される。但し、この床反力水平成分許容範囲は、式 1 2の係数 k a の値を、単純化モデル歩容用の床反力水平成分許容範囲よりも小さめに設定する等して、確実に摩擦限界内の範囲に設定することが望ましい。床反力モーメント鉛直成分許容範囲に関しても床反力水平成分許容範囲と同様に設定される。すなわち、前記式 1 0 1 2に基づいて、前記図 4 1に示したようなパターンに設定される。

また、 Z M P許容範囲は、前記第 2参考例における図 5 6の S 3 0 3 0で床反力モーメント水平成分許容範囲の設定に関して説明した場合と同様に設定される。なお、 Z M P許容範囲に目標床反力鉛直成分を乗じることで、 Z M P許容範囲をこれと等価な床反力モーメント水平成分許容範囲に変換してもよいことはもちろんである。

次いで、 S 3 5 3 2に進み今回歩容瞬時値（現在時刻 tの値）が前記単純化モデル（図 1 2の動力学モデル）を用いて決定される。この S 3 5 3 2の処理は、前記第 1参考例における図 1 3の S O 3 0の処理と同一である。

以上説明した S 3 5 3 2までの処理によって生成された現在時刻 tにおける歩容の瞬時値を、以降、単純化モデル歩容瞬時値という。第 4参考例では、この単純化モデル歩容瞬時値は、単純化モデル（図 1 2の動力学モデル）を用いて、口ポット 1にその運動によって発生する慣性力と重力との合力が目標 Z M Pまわりに発生する床反力モーメント水平成分が 0になるように（目標 Z M Pに関する動力学的平衡条件を満たすように）決定される。

この場合、単純化モデル歩容瞬時値のうちの、上体水平位置および上体姿勢傾斜角の瞬時値と、上体鉛直位置の瞬時値と、逆位相腕振り角の瞬時値とは仮瞬時値であり、後述するフルモデル補正によって補正されるものである。また、第 4参考例における単純化モデル歩容瞬時値のうち、目標 Z M Pまわりの目標床反カモ一メント水平成分の瞬時値は定常的に 0であるが、後述するフルモデル補正によって目標 Z M Pまわりに発生させる床反力モーメント水平成分の目標値としてのコンプライアンス制御用目標床反力モーメント水平成分が生成される。

次に、 S 3 5 3 4に進み、歩容補正用（フルモデル補正用）の床反力水平成分許容範囲と床反カモ一メント鉛直成分許容範囲と Z M P許容範囲を規定するパラメータ（S 3 5 3 0で設定したもの）を基に、歩容補正用の床反力水平成分許容範囲と床反力モーメント鉛直成分許容範囲と Z M P許容範囲との瞬時値（現在時刻 tの値）が求められる。

次いで、 S 3 5 3 6に進み、フルモデルを用いて補正歩容の発生（歩容の補正）を行い、最終的な目標歩容の瞬時値が決定される。すなわち- 前記図 6 3を参照して説明したように、補正目標上体位置姿勢、捕正目標逆位相腕振り角や、目標 Z M Pまわりの目標床反力モーメント（水平成分および鉛直成分）としてのコンプライアンス制御用目標床反カモ一メント（水平成分および鉛直成分）の算出（決定）などが実行される。次いで、 S 3 5 3 8に進み、時刻 tを Δ tだけ増やし、再び S 3 5 1 4に戻り、 S 3 5 1 4から S 3 5 3 8までの処理が繰り返される。

上記 S 3 5 3 6の処理は、第 4参考例の特徴をなす部分であり、以下にその処理を詳説する。第 4参考例に係る装置の歩容補正手法は、フルモデルフィードフォワード補正型である。また、逆動力学フルモデル (逆フルモデル）を用いる手法で、かつ単純化モデル歩容の入力を補正しない手法であり、かつ摂動モデルを用いる手法である。

図 6 6は、第 4参考例に係る歩容生成装置 1 0 0の動作、具体的には. 図 6 5のフロー · チャートの S 3 5 3 6の歩容の修正手法を説明する機能ブロック図である。ただし、図 6 6の単純化モデル 2 0 0は、単に動力学モデルだけでなく、前述の S 3 5. 1 0から S 3 5 3 2の処理、すなわち単純化モデル歩容瞬時値の算出（決定）処理を表す。したがって、図 6 6において、単純化モデル 2 0 0から先の部分が S 3 5 3 6の処理に相当する。

尚、歩容補正用（フルモデル補正用）の床反力水平成分許容範囲と Z M P許容範囲の瞬時値を決定する処理は、図 6 5のフローチャートの参照符号 S 3 5 3 4を用いて示している。

実際の処理はひとつのコンピュータで実行されるので、ブロック図を離散化した後、ブロック図の上流側から下流側（歩容出力側）に向かつて順に実行される。ただし、上流側に戻るフィードバック量は、前回制御周期（現在時刻 t に対して時刻 t— Δ t ) に算出した値（状態量）を用いることとする。尚、以降、前回制御周期（時刻 t — A t ) に算出した値を前回値と略す。

S 3 5 3 6の処理が実行される毎に、ブロック図の 1制御周期分の演算が実行される。

S 3 5 3 6では、まず、上記のごとく得られた単純化モデル歩容の目標上体姿勢角（以降、単純化モデル上体姿勢角と呼ぶ。また、このうちの傾斜角成分を単純化モデル上体姿勢傾斜角と呼ぶ。）、目標上体水平位置（以降、単純化モデル上体水平位置と呼ぶ）、目標重心位置、目標足平位置姿勢、目標腕姿勢（目標逆位相腕振り角を含む）などの運動を表す変数（これを運動変数と呼ぶ）の瞬時値と、目標 Z M Pの瞬時値とが前記逆動力学フルモデル（逆フルモデル） 2 0 1に入力される。入力された運動変数で表される運動に釣り合う（すなわち運動によってフルモデルが発生する）床反力水平成分と目標 Z M Pまわりの床反力モーメント（水平成分および鉛直成分）が、逆フルモデル 2 0 1の演算処理によつて算出される。なお、第 4参考例では、単純化モデル歩容における目標 Z M Pまわりの床反力モ一メント水平成分は 0であるので、逆フルモデル 2 0 1によって算出される目標 Z M Pまわりの床反力モーメント水平成分は、単純化モデル歩容の誤差としての意味を持つものである。なお、逆フルモデル 2 0 1により求められた床反力水平成分と床反カモーメント水平成分と床反力モーメント鉛直成分とをそれぞれ「フルモデル床反力水平成分」、「フルモデル床反力モーメン卜水平成分」、「フルモデル床反力モーメント鉛直成分」と呼ぶ。また、以降、しばしば、フルモデル床反力水平成分を FfulL フルモデル床反力モ一メント水平成分を Mfullxy、フルモデル床反力モーメント鉛直成分を Mfullz、と略す < また、前記逆フルモデル 2 0 1は、目標重心位置を満足する上体鉛直位置を算出する。さらに、図示は省略しているが、逆フルモデル 2 0 1 は、重心水平位置も算出する。

補足すると、フルモデルには目標全体重心鉛直位置を入力しており、また目標床反力鉛直成分は目標全体重心鉛直位置の 2階微分値から求められるので、フルモデルには、特に目標床反力鉛直成分を入力する必要はない。冗長であっても演算を減らすためなどの理由で、フルモデルに目標床反力鉛直成分を入力しても良い。

ここで、歩容補正に用いられる摂動モデルについて説明する。

摂動モデルは、上体水平位置補正用摂動モデル 2 0 2、上体姿勢傾斜角補正用摂動モデル 2 0 3および逆位相腕振り角補正用摂動モデル 2 3 1から成る。尚、摂動モデルは、このように 3つのモデルに分離しないで、図 1 2のようなひとつのモデルにしても良い。また、上体姿勢傾斜角補正用摂動モデル 2 0 3は図では上体姿勢角補正用摂動モデル 2 0 3 と略す。

上体水平位置補正用摂動モデル 2 0 2は、前記上体並進モードにおける床反力の摂動と上体水平位置の摂動との関係を表す。

上体水平位置補正用摂動モデル 2 0. 2は、目標床反力モーメン卜の補正量を入力し、これに動力学的に釣り合う目標上体水平位置の補正量を算出する。この入力（目標床反力モーメントの補正量）を、上体水平位置補正用摂動モデルモーメント Mp と呼ぶ。また、上体水平位置補正用摂動モデル 2 0 2の出力（目標上体水平位置の補正量）を、補正用摂動モデル上体水平位置 Xc と呼ぶ。また、上体水平位置補正用摂動モデル 2 0 2が発生する床反力水平成分を上体水平位置補正用摂動モデル床反力水平成分 Fpと呼ぶ。

上体水平位置補正用摂動モデル 2 0 2は、図 6 7のように、支点と倒立振子質点（上体質点）とこれらを結合する伸縮自在な支持棒から成る倒立振子で表される。支点の水平位置は前記した今回歩容の支持脚座標系の原点の水平位置に一致し、支点の鉛直位置は目標 Z M Pの鉛直位置に一致するように支点の位置が設定されている。倒立振子質点の質量 mb は、図 1 2に示す前記単純化モデル（ 3質点 +フライホイールのモデル）の上体質点質量と同じである。倒立振子質点の鉛直位置 Zc は、単純化歩容に対応する図 1 2に示す単純化モデルの上体質点位置の鉛直位置 Zbと同一とする。 '

この上体水平位置補正用摂動モデル 2 0 2は、前記単純化モデルにおいて、床反力モーメントの摂動 A My と上体質点水平位置の摂動 A Xb との関係を表す。

そこで、 My、 Xb、 Zb 以外は定数とみなして、床反力モーメントの摂動 ΔΜγと上体質点水平位置の摂動 AXb との関係を前記式 0 3yから求めると、次式が得られる。

△ My =— mb * Δ Xb * (g+d2Zb/dt2) + mb * (Zb - Zzmp) * d2△ Xb/dt2

…式 a 1 2 同様に、 Fx、 Xb 以外は定数とみなして、床反力水平成分の摂動 AFx と上体質点水平位置の摂動 AXb の関係を前記式 0 2 Xから求めると、次式が得られる。

AFx=mb* ά2Δ Xb/dt2 …式 a 1 3 · 上体水平加速度によって発生する ΔΜρと AFp の比である上体並進モード床反力比率 hは、式 al 2の右辺のうちの上体水平加速度によつて発生する項（すなわち第 2項）と式 al 3との比であるから、次式が得られる。 h=(Zb-Zzmp) …式 al 4 すなわち、上体並進モード床反力比率 hは、単純化モデルの上体質点 (倒立振子質点）の支点からの高さに相当する。

よって、式 al 2と式 al 4とより次式が得られる。

△ My =— mb * Δ Xb * (g+d2Zb/dt2) + mb* h* d2A Xb/dt2 …式 a 1 5 一方、上体質点（倒立振子質点）に作用する重力と慣性力との合力の並進力鉛直成分に釣り合う床反力鉛直成分を上体床反力鉛直成分 Fbz と呼ぶ。すなわち、上体床反力鉛直成分 Fbzは、次式で定義される。

Fbz=mb* (g+d2Zb/dt2) …式 al 6 そして、この式 a l 6と前記式 0 1.とから、上体床反力鉛直成分 Fbz は次式となる。

Fbz=Fz一 msup * (g+d2Zsup/dt2)― mswg * (g+d2Zswg/dt2)

…式 al 7 言い換えると、上体床反力鉛直成分は、床反力鉛直成分 Fz と図 1 2 に示す前記単純化モデル（ 3質点 +フライホイールのモデル）の両脚質点に作用する重力と慣性力との合力の並進力.鉛直成分との和に等しい。式 al 6を式 al 5に代入することにより、次式が得られる。

ΔΜν= - Fbz* AXb+mb* h* d2AXb/dt2 …式 a 1 8 式 al 8の AMy を上体水平位置補正用摂動モデルモーメント Mp、

△ Xb を補正用摂動モデル上体水平位置 Xc に対応させることにより (式 al 8の AMy に上体水平位置補正用摂動モデルモーメント Mp、

△ Xb に補正用摂動モデル上体水平位置 Xc を代入することにより）、次式が得られる。

Μυ = - Fbz * Xc + mb * h * d2Xc/dt2 …式 al 9 すなわち、上体水平位置補正用摂動モデル 2 0 2は、式 _a 1 4によつて求められる上体並進モード床反力比率 hと式 a l 7によって求められる上体床反力鉛直成分 Fbzを用いて、式 a l 9で表される。

また、式 a l 3の A Fx を上体水平位置補正用摂動モデル床反力水平成分 Fpに対応させることにより、次式が得られる。

Fp = mb * d2Xc/dt2 …式 a 2 0 すなわち、式 a l 4、式 a l 7、式 a l 9、式 a 2 0によって上体水平位置補正用摂動モデル 2 0 2が記述される。

補足すると、ここでは、上体質点位置の摂動と上体位置（上体代表点位置）の摂動は一致するものとみなしているが、厳密には、一致するとは限らない。従って、 Mp、 Fp と Xc の関係を求めるためには、さらに上体質点水平位置と上体位置の幾何学的関係を表すモデルが必要である, · 一方、上体姿勢傾斜角補正用摂動モデル 2. 0 3は、前記上体傾斜モードにおける床反力の摂動と上体姿勢傾斜角の摂動の関係を表す。

上体姿勢傾斜角補正用摂動モデル 2 0 3は、床反力モーメント水平成分の補正量が入力され、これに動力学的に釣り合う目標上体姿勢傾斜角の補正量を算出する。この入力（床反力モーメントの補正量）を、上体姿勢傾斜角補正用摂動モデルモーメント Mr (上体姿勢角補正用摂動モデル M r と略すこともある）と呼ぶ。また、上体姿勢傾斜角補正用摂動モデル 2 0 3の出力（目標上体姿勢傾斜角の補正量）を補正用摂動モデル上体姿勢傾斜角 0 c と呼ぶ。また、上体姿勢傾斜角補正用摂動モデル 2 0 3が発生する床反力水平成分を上体水平位置補正用摂動モデル床反力水平成分 Frと呼ぶ。 Frは前述したごとく 0である。すなわち、次式が常に成立する。 Fr= 0 …式 a 2 1 上体姿勢傾斜角補正用摂動モデル 2 0 3は、図 6 8のように、フライホイールで表される。補足すると、図 6 8では、 Y軸まわり回転のフラィホイールのみを示したが、上体姿勢傾斜角補正用摂動モデル 2 0 3は. 実際には、 Y軸まわり回転のフライホールだけでなく、 X軸まわり回転のフラ一ホイールも必要とする。それ.らのフライホイールのイナーシャは、図 1 2に示した前記単純化モデル（ 3質点モデル +フライホイ一ル）のフライホイール FHx、 FHy と同じである。この上体姿勢傾斜角補正用摂動モデル 2 0 3のフライホイールの回転角度が補正用摂動モデル上体姿勢傾斜角 0 c、そのフライホイールによって発生する床反カモーメント水平成分が上体姿勢傾斜角補正用摂動モデルモ一メント Mr に相当する。なお、以下の説明では理解の便宜上、 Y軸まわり回転のフラィホイールを中心に説明するが、 X軸まわり回転のフライホイ一ルについても同様である。

この上体姿勢傾斜角補正用摂動モデル 2 0 3 (より詳しくはサジタルプレーンに関するモデル）は、前記単純化モデル（ 3質点 +フライホイールのモデル）の動力学方程式の前記式 0 3 yにおいて、床反力モーメントの摂動 Δ Myと上体姿勢傾斜角の摂動 Δ Θ byの関係を表している。そこで、 My、 6 by 以外は定数とみなして、床反力モーメント水平成分の摂動 Δ My と上体姿勢傾斜角の摂動 Δ 0 byの関係を式 0 3 yから求めると、次式が得られる。 A My= J * ά2 Δ Θ by/dt2 …式 a 2 2 ' 式 a 2 2の A My を上体姿勢傾斜角補正用摂動モデルモーメント Mr Δ Θ by を補正用摂動モデル上体姿勢傾斜角 0 c に対応させることにより、次式が得られる。 Mr = J * d2 A Θ c/dt2 …式 a 2 3 すなわち、上体姿勢傾斜角補正用摂動モデル 2 0 3は、式 a 2 3で表される。また、上体水平位置補正用摂動モデル床反力水平成分 Fr は上記したごとく式 a 2 1のようになる（F r= 0 )。なお、上記では、上体姿勢傾斜角補正用摂動モデル 2 0 3の動力学をサジタルプレーン上で説明したが、ラテラルプレーン上での動力学も、式 a 2 3と同じ形の式で表される。

また、逆位相腕振り角補正用摂動モデル 2 3 1は、前記逆位相腕振りモードにおける床反力の摂動と逆位相腕振り角の摂動との関係を表す。 · 逆位相腕振り角補正用摂動モデル 2 3 1は.、床反力モーメント鉛直成分の補正量が入力され、これに動力学的に釣り合う目標逆位相腕振り角の補正量を算出する。この入力（床反力モーメント鉛直成分の補正量）を、逆位相腕振り角補正用摂動モデルモーメント Maと呼ぶ。また、逆位相腕振り角補正用摂動モデル 2 0 3の出力（目標逆位相腕振り角の補正量）を補正用摂動モデル逆位相腕振り角 0 ca と呼ぶ。また、逆位相腕振り角補正用摂動モデルが発生する床反力水平成分を逆位相腕振り角補正用摂動モデル床反力水平成分 Fa と呼ぶ。 Fa は前述したごとく 0 である。すなわち、次式が常に成立する。 Fa = 0 …式 a 2 1 c 逆位相腕振り角補正用摂動モデル 2 3 1は、図 6 9のように、フライホイールで表される。フライホイールのイナーシャは、図 1 2に示した前記単純化モデル（ 3質点モデル +フライホイール）のフライホイール FHaz と同じである。この逆位相腕振り角補正用摂動モデル 2 3 1のフライホイールの回転角度が補正用摂動モデル逆位相腕振り角 S ca、そのフライホイールによって発生する床反カモ一メント鉛直成分が逆位相腕振り角補正用摂動モデルモ一メント Maに相当する。

この逆位相腕振り角補正用摂動モデル 2 3 1は、前記単純化モデル ( 3質点 +フライホイールのモデル）の動力学方程式の前記式 0.3 _Z において、床反力モーメントの摂動 ΔΜζ と逆位相腕振り角の摂動△ 0 az との関係を表している。

そこで、 Μζ、 Θ az 以外は定数とみなして、床反力モーメントの摂動 ΔΜζ と逆位相腕振り角の摂動 A S azの関係を式 0 3 z ら求めると、次式が得られる。

ΔΜζ = Jaz* ά2Δ Θ az/dt2 …式 a2 2 c 式 a2 2 cの ΔΜζ を逆位相腕振り角補正用摂動モデルモーメント Ma、 Δ Θ azを補正用摂動モデル逆位相腕振り角 0 caに対応させることにより、次式が得られる。

Ma= Jaz* ά2Α Θ ca/dt2 …式 a2 3 c すなわち、逆位相腕振り角補正用摂動モデル 2 3 1は、式 a2 3 c で表される。また、逆位相腕振り角補正用摂動モデル床反力水平成分 Fr は上記したごとく式 a 2 1 cのようになる（Fa= 0 )。後述するように、 S 3 5 3 6では、最終的に、補正歩容（より詳しくは、単純化モデル歩容の一部の瞬時値を補正した目標瞬時値）が生成 (出力）される。補正歩容の目標上体姿勢傾斜角（これを以降、補正目標上体姿勢傾斜角と呼ぶ）は、前記求めた単純化モデル上体姿勢傾斜角の瞬時値（ S 3 5 3 2で求めた今回歩容の現在時刻 tでの目標上体姿勢傾斜角の瞬時値）に前記補正用摂動モデル上体姿勢傾斜角 Θ c (現在時刻 tの制御周期で求められる値）を演算部 2 0 4で加えることにより得られる。補正歩容の目標上体水平位置（これを以降、補正目標上体水平位置と呼ぶ）は、前記求めた単純化モデル上体水平位置の瞬時値（S 3 5 3 2で求めた今回歩容の現在時刻 tでの目標上体水平位置の瞬時値）に補正用摂動モデル上体水平位置 Xc (現在時刻 tの制御周期で求められる値）を演算部 2 0 5で加えることにより得られる。また、補正歩容の目標逆位相腕振り角（これを以降、補正目標逆位相腕振り角と呼ぶ）は、前記求めた単純化モデル逆位相腕振り角の瞬時値（S 3 5 3 2で求めた今回歩容の現在時刻 tでの目標逆位相腕振り角の瞬時値）に補正用摂動モデル逆位相腕振り角 0 ca (現在時刻 tの制御周期で求められる値）を演算部 2 3 2で加えることにより得られる。

補正歩容の目標床反力も修正される。具体的には、目標 Z M Pまわりの床反力モーメント水平成分が 0でなくなり、コンプライアンス制御用目標床反力モーメント水平成分が目標値として出力される。また、目標床反力水平成分も、補正目標床反力水平成分に修正され、出力される。また、目標床反力モーメント鉛直成分も、補正目標床反力モーメント鉛直成分に修正され、出力される。

ところで、上記のごとく、補正歩容の運動は、単純化モデル歩容の運動に摂動モデルの運動（詳しくは上体水平位置補正用摂動モデル 2 0 2 と上体姿勢傾斜角補正用摂動モデル 2 0 3の運動と逆位相腕振り角補正用摂動モデル 2 3 1の運動）を加えた（合成した）運動である。

一般に、ある基準運動に、ある摂動運動を加えた運動が発生する床反力は、基準運動が発生する床反力（運動によって発生する重力と慣性力に釣り合う床反力）と摂動蓮動によって発生する床反力の摂動分の和で近似される。

また、上体傾斜モードが発生する床反力水平成分、上体傾斜モードが発生する床反カモ一メント鉛直成分、 .逆位相腕振りモードが発生する床反力水平成分および逆位相腕振りモ一ドが発生する床反カモ一メント水平成分がいずれも 0であることを考慮すると、補正歩容の運動に逆フルモデル 2 0 1上で釣り合う床反力モーメントに補償全床反力モーメント水平成分 Mdmdxy と補償全床反力モーメント鉛直成分 Mdmdz とを加えたものが補正目標床反カモ一メントに一致する (前記 A1 の条件を満たす）ためには、以下の 3つの式を満足しなければならない。フルモデル床反力モーメント水平成分 Mfullxy

+上体水平位置補正用摂動モデルモーメント Mp

+上体姿勢傾斜角補正用摂動モデルモーメント Mr

+補償全床反力モーメント水平成分 Mdmdxy

= 補正目標床反力モーメント水平成分

…式 h 5 フルモデル床反力水平成分 Ffull

+上体水平位置補正用摂動モデル床反力水平成分 Fp

= 補正目標床反力水平成分 '

…式 h 6 フルモデル床反力モ一メント鉛直成分 Mfullz

+上体水平位置補正用摂動モデルモーメント鉛直成分 Mpz

+逆位相腕振り角補正用摂動モデルモーメント鉛直成分 Maz

+補償全床反力モーメント鉛直成分 Mdmdz

= 補正目標床反カモ一メント鉛直成分

…式 hi 0 0 6 ただし、上体水平位置補正用摂動モデルモーメント鉛直成分 Mpz は. 上体水平位置補正用摂動モデルの運動によって逆フルモデル 2 0 1上で発生する床反力モーメント鉛直成分である。また、式 h5、式 hi 0 0 6のモーメントは、元の（単純化モデルの）目標 Z MPまわりのモ一メントである。

また、補正歩容の真の ZMPは、補正目標床反力モーメントを目標床反力鉛直成分で割った値だけ、単純化モデル歩容の目標 ZMP (理想目標 Z M P ) からずれた点に変更される。補正歩容の真の Z MP =目標 ZMP

+補正目標床反力モーメント Z目標床反力鉛直成分

…式 h7 尚、補正歩容の真の Z MPの X 方向（前後方向）成分を算出する場合には、補正目標床反力モーメントの Y 軸（左右方向軸）まわり成分を用いる。また、補正歩容の真の Z MPの Y 方向成分を算出する場合には、補正目標床反力モーメントの X 軸（前後軸）まわり成分を用いる。ただし、補正歩容の真の ZMPの Y 方向成分を算出する場合には. 式 h7の右辺の「十」は「一」にしなければならない。

補足すると、補正歩容の真の ZMPまわりの補正目標床反カモ一メント鉛直成分を算出するようにしても良い。

補正歩容の真の Z M Pまわりの補正目標床反力モーメント鉛直成分は. 元の目標 ZMPまわりのモーメントに補正歩容の真の Z MPと目標 Z M Pの差と式 h 6で求めた補正目標床反力水平成分との外積を加えたものとなる。

式 h7から求められる補正歩容の真の Z M Pは、 ZMP許容範囲内になければならない。これを Z MP制約条件と呼ぶ。

また、補正目標床反力水平成分は、 .歩容補正用（フルモデル補正用）の床反力水平成分許容範囲内になければならない。これを床反力水平成分制約条件と呼ぶ。

また、補正目標床反力モーメント鉛直成分は、歩容補正用（フルモデル補正用）の床反力モーメント鉛直成分許容範囲内になければならない, これを床反カモ一メント鉛直成分制約条件と呼ぶ。

以上のように、補正歩容は、式 h5、式 h6、式 hl 0 0 6、 ZMP 制約条件（式 h7から得られる補正歩容の真の Z MPの存在範囲条件) 床反力水平成分制約条件および床反力モーメント鉛直成分制約条件を満足しなければならない。

しかし、単に、これらの式や条件を満足するだけでは、前記補正用摂動モデル上体位置と前記補正用摂動モデル上体姿勢傾斜角と前記補正用摂動モデル逆位相腕振り角が発散してしまう。

そこで、前記上体水平位置補正用摂動モデル 2 0 2と前記上体姿勢傾斜角補正用摂動モデル 2 0 3と前記逆位相腕振り角補正用摂動モデル 2 3 1の状態量（より具体的には、補正用摂動モデル上体水平位置速度、補正用摂動モデル上体姿勢傾斜角 · 角速度、補正用摂動モデル逆位相腕振り角 ·角速度など）を基に、これらの状態量がある所定の状態に収束する（安定する）ように、補正用摂動モデル 2 0 2， 2 0 3 , 2 3 1の安定化制御を行う。

まず、上体水平位置補正用摂動モデル 2 0 2の安定化制御について詳細を説明する。

上体水平位置補正用摂動モデル 2 0 2を目標整定位置に収束（安定）させるための制御則を上体水平位置補正用摂動モデル制御則 2 0 6と呼び、この制御則が決定するフィードバック量（操作量）を上体水平位置補正用摂動モデル安定化モーメント要求値 Mpfdmd と呼ぶ。ここで「要求値」を付けたのは、後述するように、真の Z M Pが前記 Z M P許容範囲内に存在し、かつ、床反力水平成分が床反力水平成分許容範囲内になるように、上記制御則で決定された値に制限が加えられて修正されるからである。制限が加えられて修正されたモーメントを、上体水平位置補正用摂動モデル安定化モ一メント Mpf と呼ぶ。

具体的に上体水平位置補正用摂動モデル制御則 2 0 6としては、式 h 1 0を用いる。ただし、目標整定位置は、式 h 8で与えられる。また、 nitotal は前記ロボット総重量、 mb は前記上体質点質量（倒立振子質量）であり、 XGf は、単純化モデル歩容の瞬間姿勢を基にフルモデルを用いて計算される重心水平位置、すなわち、前記逆フルモデルが算出する重心水平位置である。また、 Kpp、 Κρνはフィードバック制御のゲィンである。目標整定位置 =—mtotal/mb * (XGf— XGs) …式 h 8 上体水平位置補正用摂動モデル安定化モ一メント要求値 Mpfdmd = Kpp * (捕正用摂動モデル上体水平位置 Xc—目標整定位置）

+ Kpv *補正用摂動モデル上体水平速度 dXc/dt ■

一補正用摂動モデル上体水平位置 Xc *上体床反力鉛直成分 Fbz …式 h 1 0 上体水平位置補正用摂動モデル安定化モーメント要求値 Mpfdmd の X軸（前後方向軸）まわり成分を求める場合には、補正用摂動モデル上体水平位置速度および目標整定位置は Y 軸方向（左右方向）成分を用いる。

上体水平位置補正用摂動モデル安定化モーメント要求値 Mpfdmd の Y軸（左右方向軸）まわり成分を求める場合には、補正用摂動モデル上体水平位置速度および目標整定位置は X 軸方向（前後方向）成分を用い、右辺第 3項の「一」を「+」に代える。

詳細は、先に本出願人が提案した P C T / J P 0 3 / 0 0 4 3 5に記載されているので、ここではこれ以上の説明を省略する。

次に、上体姿勢傾斜角補正用摂動モデル 2 0 3の安定化制御について詳細を説明する。

· 目標瞬時値発生部 1 0 0 bが出力する基準上体姿勢傾斜角（図 5 7の S 3 4 0 4で求められたもの）あるいは単純化モデルによる目標上体姿勢傾斜角（図 5 7の S 3 4 1 4で求められたもの）に、補正目標上体姿勢傾斜角、すなわち、単純化モデルによる目標上体姿勢傾斜角に補正用摂動モデル上体姿勢傾斜角を加えたものが整定あるいは追従するように. 上体姿勢傾斜角補正用摂動モデル 2 0 3の状態に応じて、 P I制御などのフィードバック制御則によってフィードバック量（操作量）を決定し. これを上体姿勢傾斜角補正用摂動モデル 2 0 3に付加的に入力する。この制御則を上体姿勢傾斜角補正用摂動モデル制御則 2 0 7 (上体姿勢角補正用摂動モデル制御則 2 0 7 と略すこともある）と呼び、このフイードバック量（操作量）を上体姿勢傾斜角補正用摂動モデル安定化モ —メント要求値 Mrfdmd と呼ぶ。ここで「要求値」を付けたのは、上体水平位置補正用摂動モデル安定化モーメント要求値 Mpfdmd と同様の理由からである。制限が加えられて修正されたモーメントを、上体姿勢傾斜角補正用摂動モデル安定化モ一メント Mrf と呼ぶ。

上体姿勢傾斜角補正用摂動モデル安定化モ一メント要求値 Mrfdmd を決定する上体姿勢傾斜角補正用摂動モデル制御則 2 0 7は、具体的には、次式のようにすれば良い。上体姿勢傾斜角補正用摂動モデル安定化モーメント要求値 Mi'fdmd = Krp * (補正用摂動モデル上体姿勢傾斜角 0 c

― (基準上体姿勢傾斜角—単純化モデルによる目標上体姿勢傾斜角））

+ Krv *補正用摂動モデル上体姿勢傾斜角速度 d 0 c/dt

…式 h i 1 ただし、 Ki'p、 Krvはフィードバック制御のゲインである。なお、式 h i 1 において、（基準上体姿勢傾斜角一単純化モデルによる目標上体姿勢傾斜角）の代わりに 0としてもよい。

再び、図 6 6の機能ブロック図を用いて S 3 5 3 6の処理を説明すると、上記のごとく、前記上体水平位置補正用摂動モデル制御則 2 0 6 (式 h l O ) によって上体水平位置補正用摂動モデル安定化モーメント要求値 Mpfdmd が求められる。また、前記上体姿勢傾斜角補正用摂動モデル制御則 2 0 7 (式 h l l ) によって上体姿勢傾斜角補正用摂動モデル安定化モーメント要求値 Mrfdmdが求められる。

次いで、上体水平位置補正用摂動モデル安定化モーメント Mpf が 0 であったと仮定した場合の上体位置補正用摂動モデルの床反力推定（算出）値 F0 が F0 演算部 2 0 8により求められる。後述するように、上体水平位置補正用摂動モデル 2 0 2には、フルモデル床反力モーメント水平成分 Mfullxy に、上体水平位置補正用摂動モデル安定化モ一メント Mpf を加えられたものが入力される。したがって、 F0 は、上体水平位置補正用摂動モデル 2 0 2にフルモデル床反力モーメント水平成分 Mfullxy の符号を反転させたものだけが入力された場合に上体水平位置補正用摂動モデル 2 0 2が発生する床反力である。

具体的には、次式によって F0が求められる。

F0= mb * d2Xc/dt2 - l/h * Mpf …式 h i 2 右辺第 1項は、前回（時刻 t 一 A t ) の上体水平位置補正用摂動モデル 2 0 2の床反力水平成分を表す。

右辺第 2項は、前回の上体水平位置補正用摂動モデル安定化モーメント Mpf によって、上体水平位置補正用摂動モデル 2 0 2に直接的に発 '生した（すなわち直達項の）床反力水平成分を表す。

すなわち、上体水平位置補正用摂動モデル 2 0 2の前回の上体質点水平加速度に上体質点の質量 mb を乗じた値から、前回の上体水平位置補正用摂動モデル安定化モーメント Mpf を上体並進モード床反力比率 h で割った値を減じることによって、 Mpf が 0であつたと仮定した場合の上体位置補正用摂動モデルの床反力 F0の推定値が求められる。

次に、前記制約 (床反力水平成分制約条件および Z M P制約条件) を無視して、上体水平位置補正用摂動モデル安定化モーメント Mpf を上体水平位置補正用摂動モデル安定化モーメント要求値 Mpfdmd に一致させ、かつ、上体姿勢傾斜角補正用摂動モデル安定化モーメント Mrf を、上体姿勢傾斜角補正用摂動モデル安定化モーメント要求値 Mrfdmd に一致させると共に、目標 Z M Pまわりの目標床反カモ一メント水平成分としてのコンプライアンス制御用目標床反力モーメントを補償全床反力モーメン卜水平成分 Mdmdxy と Mpf と Mrf との総和に一致させたと仮定した場合に、目標 Z M Pまわりに発生する床反カモ一メント水平成分 Minが Min演算部 2 0 9により求められる。この床反力モ一メント水平成分を無制限時補正目標床反力モーメント水平成分 Min と呼ぶ。無制限時補正目標床反力モーメント水平成分 Min は、次式によって求められる。

Min= Mpfdmd + Mrfdmd + Mdmdxy …式 h i 3 すなわち、無制限時補正目標床反カモ一メンド水平成分 Min は、上体水平位置補正用摂動モデル安定化モーメント要求値 Mpfdmd と上体姿勢傾斜角補正用摂動モデル安定化モーメント要求値 Mrfdmd と補償全床反力モーメント水平成分 Mdmdxy とを加算することにより得られる。

次いで、前記制約 (床反力水平成分制約条件および Z M P制約条件) を無視して、上体水平位置補正用摂動モデル安定化モーメント Mpf を上体水平位置補正用摂動モデル安定化モーメント要求値 Mpfdmd に一致させ、かつ、上体姿勢傾斜角補正用摂動モデル安定化モーメント Mrf を、上体姿勢傾斜角補正用摂動モデル安定化モーメント要求値 Mrfdmd に一致させると共に、コンプライアンス制御用目標床反カモ一メント水平成分を補償全床反力モーメント水平成分 Mdmdxyと Mpf と Mrf との総和に一致させたと仮定した場合に、発生する床反力水平成分 Fin が Fin演算部 2 1 0により求められる。この床反力水平成分を無制限時補正目標床反力水平成分 Finと呼ぶ。 .

補正目標床反力水平成分は、前記式 h 6によって得られるが、前述のごとく、上体姿勢傾斜角補正用摂動モデル 2 0 3の挙動によって上体姿勢傾斜角補正用摂動モデル 2 0 3には床反力水平成分は発生しない、すなわち Fr は 0である。したがって、無制限時補正目標床反力水平成分 Fin は、上体水平位置補正用摂動モデル安定化モーメント Mpf が 0であつたと仮定した場合の補正目標床反力水平成分に、上体水平位置補正用摂動モデル安定化モ一メント Mpf を 0から上体水平位置補正用摂動モデル安定化モーメント要求値 Mpfdmd に変更したことによって増加する床反力水平成分を加えたものになる。

ところで、上体水平位置補正用摂動モデル安定化モーメント Mpf を 0 から上体水平位置補正用摂動モデル安定化モーメント要求値 Mpfdmd に変更したことによって増加する床反力水平成分は、上体水平位置補正用摂動モデル安定化モーメント要求値 Mpfdmd を上体並進モード床反力比率 hで割った値になる。

したがって、無制限時補正目標床反力水平成分 Finは、式 h i 5に示すように、上体水平位置補正用摂動モデル安定化モーメント要求値 Mpfdmd を上体並進モード床反力比率 hで割った値に、前記求めたフルモデル床反力水平成分 Ffull を加算し、さらに、上体水平'位置補正用摂動モデル安定化モ一メント Mpf が 0であったと仮定した場合の上体位置補正用摂動モデルの床反力の算出値 F0 を加算することによって得られる。

Fin= 1/h * Mpfdmd + Ffull + F0 …式 h i 5 次いで、制限手段（制限処理部） 2 1 1によって、無制限時補正目標床反力モーメント水平成分 Min と無制限時補正目標床反力水平成分 Fin とから、これらに制限を加えた値である（目標 Z M Pまわりの）制限補正目標床反力モーメント水平成分 Mltd と制限補正目標床反力水平成分 Fltd が決定される。本実施形態では、コンプライアンス制御用目標床反カモ一メント水平成分は、制限補正目標床反力モーメント水平成分 Mltd に一致し、補正歩容の床反力水平成分は制限補正目標床反力水平成分 Fltdに一致する。

制限補正目標床反力モーメント水平成分 Mltd と制限補正目標床反力水平成分 Fltd とは、補正歩容（コンプライアンス制御用目標床反カモ —メント水平成分を含む）の真の Z M Pが前記 Z M P許容範囲内に存在し、かつ、補正歩容の床反力水平成分が床反力水平成分許容範囲内になるように決定される。すなわち、 Mltdおよび Fltdは Z M P制約条件および床反力水平成分制約条件を満足するように決定される。

また、上記制約条件下で、上体水平位置補正用摂動モデル安定化モーメント Mpは、可能な限り、上体水平位置補正用摂動モデル安定化モーメント要求値 Mpfdmd に一致または近い値になるように決定される。同様に、上体姿勢傾斜角補正用摂動モデル安定化モーメント Mrは、可能な限り上体姿勢傾斜角補正用摂動モデル安定化モーメント要求値 Mrfdmd に一致または近い値になるように決定される。これにより、前記補正用摂動モデル上体位置 Xc と前記補正用摂動モデル上体姿勢傾斜角とを安定化し、発散することを防止する。

制限手段（制限処理部） 2 1 1の詳細は、先に本出願人が提案した P

C T J P 0 3 / 0 0 4 3 5に記載されているので、ここではこれ以上の説明を省略する。

次いで、以下の式により、上体水平位置補正用摂動モデル安定化モーメント Mpf、および上体姿勢傾斜角捕正用摂動モデル安定化モーメント Mrf がそれぞれ Mpf 演算部 2 1 2 、 Mrf 演算部 2 1 3により求められる。 8 4

Mpf= (Fltd— Ffull - FO) * h 式 h 2 0

Mrf= Mltd— Mpf- Mdmdxy 式 h 2 1 すなわち、 Mpf 演算部 2 1 2では、制限補正目標床反力水平成分 Fltdから、フルモデル床反力水平成分 Ffull を減じ、 Mp が 0であったと仮定した場合の上体位置補正用摂動モデル 2 0 2の床反力の算出値 F0 を減じた値に、上体並進モード床反力比率 hを乗じることにより、前記上体水平位置補正用摂動モデル安定化モーメント Mpf が得られる, また、 Mrf 演算部 2 1 3では、目標 Z M Pまわりの制限補正目標床反力モーメント水平成分 Mltdから前記上体水平位置補正用摂動モデル安定化モ一メント Mpf と補償全床反力モ一メント水平成分 Mdmdxy とを減じることにより、上体姿勢傾斜角補正用摂動モデル安定化モーメント Mrfが得られる。

次いで、以下の式により、上体水平位置補正用摂動モデル床反力モーメント Mp、および上体姿勢傾斜角補正用摂動モデル床反力モーメント Mrが求められる。 Mp = Mpf- Mfull 式 1ι 2 2

Mr= Mrf '式 h 2 3 すなわち、 Mp 演算部 2 1 4にて、上体水平位置補正用摂動モデル安定化モーメント Mpf からフルモデル床反力モーメント水平成分 Mfullxy を減じることにより、上体水平位置補正用摂動モデル床反カモ一メント Mpが得られる。また、上体姿勢傾斜角補正用摂動モデル床反力モーメント Mrは、上体姿勢傾斜角補正用摂動モデル安定化モ一メント Mrf と同一の値とされる。

次いで、上体水平位置補正用摂動モデル床反力モーメント Mpが上体位置補正用摂動モデル 2 0 2に入力され、その入力された床反力モ一メントに釣り合う補正用摂動モデル上体位置 Xcが算出される。

また、上体姿勢傾斜角補正用摂動モデル床反力モーメント Mrが上体姿勢傾斜角補正用摂動モデル 2 0 3に入力され、その入力された床反力モーメント水平成分に釣り合う補正用摂動モデル上体姿勢傾斜角 0 c が算出される。

次いで、演算部 2 0 5， 2 0 4にて、それぞれ次式 h 2 4、式 h 2 5 に従つて、補正目標上体水平位置と補正目標上体姿勢傾斜角とが決定され、これらがそれぞれ上体水平位置姿勢の最終的な目標瞬時値として出力される。補正目標上体水平位置 .

=単純化モデル上体水平位置 +補正用摂動モデル上体位置 Xc

…式 h 2 4 補正目標上体姿勢傾斜角

=単純化モデル上体姿勢傾斜角 +補正用摂動モデル上体姿勢傾斜角 Θ c

…式 h 2 5 すなわち、単純化モデル上体水平位置に補正用摂動モデル上体位置 Xc を加えることにより補正目標上体水平位置が得られ、これが最終的な目標上体水平位置として出力される。また、単純化モデル上体姿勢傾斜角に補正用摂動モデル上体姿勢傾斜角 0 c を加えることにより補正目標上体姿勢傾斜角が得られ、これが最終的な目標上体姿勢傾斜角として出力される。

また、目標 ZMPまわりのコンプライアンス制御用目標床反力モーメント水平成分として制限補正目標床反力モーメント水平成分 Mltd が出力されると共に、補正目標床反力水平成分として制限補正目標床反力水平成分 Fltdが出力される。

すなわち、次式 h2 6、式 h2 7に従って、補正目標床反力水平成分と目標 ZMPまわりの補正目標床反カモ一メント水平成分とがそれぞれ床反力水平成分および床反力モーメント水平成分（目標 ZMPまわりのモーメント水平成分）の最終的な目標瞬時値として決定され、これらが出力される。コンプライアンス制御用目標床反カモ一メント水平成分

=制限補正目標床反力モーメント水平成分 Mltd

…式 h2 6 補正目標床反力水平成分

=制限補正目標床反力水平成分 Fltd

…式 h2 7

S 3 5 3 6における以上までの処理は、本願出願人による P C T/ J P 0 3 / 0 0 4 3 5の通りであるので、ここでの説明は以上に留める。第 4参考例ではさらに、逆位相腕振り角補正用摂動モデルモーメント決定部 2 3 0において、逆位相腕振り角補正用摂動モデル 2 3 1への入力である逆位相腕振り角補正用摂動モデルモーメント Maと目標床反力モーメント鉛直成分が決定される。次いで、逆位相腕振り角補正用摂動モデルモーメント Maは、逆位相腕振り角補正用摂動モデル 2 3 1 に入力され、補正用摂動モデル逆位相腕振り角 0 caが決定される。

補足すると、逆位相腕振り角補正用摂動モデルモーメント決定部 2 3 0は、目標瞬時値発生部 1 0 0 bが出力する基準逆位相腕振り角（図 5 7の S 3 4 0 4で求められたもの）あるいは単純化モデルによる目標逆位相腕振り角（図 5 7の S 3 4 1 4で求められたもの）に、補正目標逆位相腕振り角、すなわち、単純化モデルによる目標逆位相腕振り角に補正用摂動モデル逆位相腕振り角を加え.たものが整定あるいは追従するように、逆位相腕振り角補正用摂動モデル 2 3 1の状態に応じて、 P I制御などのフィードパック制御則によってフィードバック量（操作量）を決定し、これを逆位相腕振り角補正用摂動モデル 2 3 1に付加的に入力する。

この制御則を逆位相腕振り角補正用摂動モデル制御則と呼び、このフイードバック量（操作量）を逆位相腕振り角補正用摂動モデル安定化モ一メント要求値 Mafdmd と呼ぶ。ここで「要求値」を付けたのは、上体水平位置補正用摂動モデル安定化モーメント要求値 Mpfdmd と同様の理由からである。制限が加えられて修正されたモーメントを、逆位相腕振り角補正用摂動モデル安定化モーメント Maf と呼ぶ。なお、本実施形態では、この M af が、逆位相腕振り角補正用摂動モデルモ一メント Maとして、逆位相腕振り角補正用摂動モデル 2 3 1に入力される。逆位相腕振り角補正用摂動モデル安定化モーメント要求値 Mafdmd を決定する逆位相腕振り角補正用摂動モデル制御則は、具体的には、次式のようにすれば良い。逆位相腕振り角補正用摂動モデル安定化モーメント要求値 Mafdmd = Kap * (補正用摂動モデル逆位相腕振り角 0 ca 一（基準逆位相腕振り角一単純化モデルによる目標逆位相腕振り角））

+ Kav*補正用摂動モデル逆位相腕振り角速度 d0 ca/dt

…式 h3 0 ただし、 Kap、 Kavはフィードバック制御則のゲインである。なお、式 h3 0において、（基準逆位相腕振り角一単純化モデルによる目標逆位相腕振り角）の代わりに 0.としてもよい。

以下に、逆位相腕振り角補正用摂動モデルモーメント決定部 2 3 0の動作についてその機能ブロック図である図 7 0を用いて詳細を説明する, まず、上記のごとく、前記逆位相腕振り角補正'用摂動モデル制御則 2 3 0 a (式 h3 0 ) によって逆位相腕振り角補正用摂動モデル安定化モーメント要求値 Mafdmdが求められる。

次いで、上体水平位置補正用摂動モデル 2 0 2を用いた上体水平位置補正に起因する床反力モーメント鉛直成分摂動量 Mpz が、上体水平位置補正に起因する床反力モーメント鉛直成分摂動量算出部 2 3 0 bによつて求められる。

具体的には、次式によって求められる。 Mpz= (単純モデル上体質点位置

+補正用摂動モデル上体位置 Xc—目標 ZMP) * (単純モデル上体質点水平加速度

+補正用摂動モデル上体加速度 d2Xc/dt2)

一（単純モデル上体質点位置一目標 Z MP)

* (単純モデル上体質点水平加速度） .

…式 h3 1 あるいは、上式を近似した以下の式を用いても良い。補正用摂動モデル上体位置 Xc

* (単純モデル上体質点水平加速度

+補正用摂動モデル上体加速度 d2Xc/dt2)

+ (単純モデル上体質点位置一目標 Z M P )

* 補正用摂動モデル上体加速度 d2Xc/dt2)

…式 h 3 2 次に、前記床反力モーメント鉛直成分制約条件を無視して、逆位相腕振り角補正用摂動モデル安定化モーメント Maf を、逆位相腕振り角補正用摂動モデル安定化モーメント要求値 Mafdmd に一致させると共に目標 Z M Pまわりの目標床反力モ一メント鉛直成分としてのコンプライアンス制御用目標床反力モーメント鉛直成分を補償全床反力モーメント鉛直成分 Mdmdz と Maf と Mfullz と Mpz との総和に一致させたと仮定した場合に、目標 Z M Pまわりに発生する床反カモ一メント鉛直成分 Minz が演算部 2 3 0 cにより求められる。この床反力モーメント鉛直成分を無制限時補正目標床反カモ一メント鉛直成分 Minz と呼ぶ。無制限時補正目標床反力モーメント鉛直成分 Minz は、次式によって求められる。

Minz = Mafdmd + Mdmdz + Mfullz + Mpz …式 h 3 3 すなわち、無制限時補正目標床反力モーメント鉛直成分 Minz は、逆位相腕振り角補正用摂動モデル安定化モーメント'要求値 Mafdmd と補償全床反力モーメント鉛直成分 Mdmdz とフルモデル床反力モーメント鉛直成分を Mfullz と上体水平位置補正用摂動モデルモーメント鉛直成分 Mpzとを加算することにより得られる。

次いで、制限手段（制限処理部） 2 3 0 dによって、無制限時補正目標床反力モ一メント鉛直成分 Minzから、これに制限を加えた値である (目標 Z M Pまわりの）制限補正目標床反力モーメント鉛直成分 Mltdz が決定される。第 4参考例では、コンプライアンス制御用目標床反カモ一メント鉛直成分は、制限補正目標床反力モーメント鉛直成分 Mltdz に一致する。

制限補正目標床反力モーメント鉛直成分 Mltdz は、床反力モ一メント鉛直成分許容範囲内になるように決定される。すなわち、 Mltdz は床反力モーメント鉛直成分制約条件を満足するように決定される。

また、上記床反力モーメント鉛直成分制約条件下で、逆位相腕振り角補正用摂動モデル安定化モーメント Maf は、可能な限り、逆位相腕振り角補正用摂動モデル安定化モーメント要求値 Mafdmd に一致または近い値になるように決定される。これにより.、前記補正用摂動モデル逆位相腕振り角 0 caを安定化し、発散することを防止する。

制限手段（制限処理部） 2 3 0 dは、次式に示す飽和特性を持った関数である。 Minz>床反カモ一メント鉛直成分許容範囲上限値である場合

Mltdz=床反力モ一メント鉛直成分許容範囲上限値

Minz<床反力モーメント鉛直成分許容範囲下限値である場合

Mltdz=床反力モーメント鉛直成分許容範囲下限値

床反力モーメント鉛直成分許容範囲下限値≤Minz、且つ、 Minz≤床反力モーメント鉛直成分許容範囲上限値である場合 '

Mltdz = Minz 式 h 3 4 次いで、以下の式により、逆位相腕振り角補正用摂動モデルモ一メント Ma ( =逆位相腕振り角補正用摂動モデル安定化モーメント Maf) が Maf演算部 2 3 0 eにより求められる。

Ma= Mltdz - (Mdmdz + Mfullz + Mpz) …式 h 3 5 すなわち、 Maf 演算部 2 3 0 eでは、制限補正目標床反力モ一メント鉛直成分 Mltdzから、補償全床反力モーメント鉛直成分 Mdmdzとフルモデル床反力モーメント鉛直成分 Mfullz と上体水平位置補正用摂動モデルモーメント鉛直成分 Mpz とを減じることによって、逆位相腕振り角補正用摂動モデル安定化モーメント Maf としての逆位相腕振り角補正用摂動モデルモ一メント Maが得られる。

一方、目標 Z M Pまわりのコンプライアンス制御用目標床反力モーメント鉛直成分として制限補正目標床反力モーメント鉛直成分 Mltd が出力される。

すなわち、次式 h 3 5に従って、目標 Z M Pまわりの補正目標床反力モーメント鉛直成分が床反力モーメント鉛直成分（目標 Z M Pまわりのモーメント鉛直成分）の最終的な目標瞬時値として決定され、これらが出力される。コンプライアンス制御用目標床反力モーメント鉛直成分

=制限補正目標床反力モーメント鉛直成分 Mltdz

…式 h 3 6 以上のごとく、逆位相腕振り角補正用摂動モデルモーメント 2 3 0の処理が行われた後、逆位相腕振り角補正用摂動モデルモーメント Maは図 6 6の逆位相腕振り角補正用モデル 2 3 1に入力され、その入力された逆位相腕振り角補正用摂動モデルモーメント Maに釣り合う補正用摂動モデル逆位相腕振り角 0 caが、式 a 2 3 c を用いて（積分することによって）算出される。

次いで、演算部 2 3 2にて、次式 h 3 7に従って、補正目標逆位相腕振り角が決定され、これが逆位相腕振.り角の最終的な目標瞬時値として出力される。補正目標逆位相腕振り角

=単純化モデル逆位相腕振り角

+補正用摂動モデル逆位相腕振り角 0 ca

…式 h 3 7 すなわち、単純化モデル逆位相腕振り角に補正用摂動モデル逆位相腕振り角 0 ca を加えることにより補正目標逆位相腕振り角が得られる、これが最終的な目標逆位相腕振り角として出力される。

以上のごとく、 S 3 5 3 6の歩容補正が行われる。

尚、補足すると、第 4参考例は、フィードフォワード型の補正であり . また、摂動動力学モデルは厳密モデルではない。このため、上記のごとく式 h 5、式 h 6および式 h i 0 0 6を満足するように歩容を補正しても、厳密には、動力学平衡条件を満足するするわけではなく、近似的に動力学平衡条件を満足する。

また、第 4参考例では、例えば 1歩の歩容の終端（今回歩容終端）において、上体水平位置補正用摂動モデル 2 0 2と上体姿勢傾斜角補正用摂動モデル 2 0 3 と逆位相腕振り角補正用摂動モデル 2 3 1 との状態量. 例えば上体水平位置補正用摂動モデル 2 0 2の上体質点（倒立振子質点）の水平位置や、上体姿勢傾斜角補正用摂動モデル 2 0 3のフライホールの回転角や、逆位相腕振り角補正用摂動モデル 2 3 1のフライホールの回転角は、単純化モデル 2 0 0の状態量として追加される。つまり . 今回歩容終端における単純化モデル 2 0 0の状態量が、上体水平位置補正用摂動モデル 2 0 2の上体運動と上体姿勢傾斜角補正用摂動モデル 2 0 3と逆位相腕振り角補正用摂動モデル 2 3 1 との状態量を加えたものに修正される。さらに、各摂動モデル 2 0 2 , 2 0 3 , 2 3 1の状態量は初期化される（例えば上体水平位置補正用摂動モデル 2 0 2の上体質点（倒立振子質点）の水平位置や、上体姿勢傾斜角補正用摂動モデル 2 0 3のフライホールの回転角や、逆位相腕振り角補正用摂動モデル 2 3 1のフライホールの回転角が 0にリセットされる）。そして、上記の如く修正した単純化モデル 2 0 0の状態量を次回歩容の初期値として、次回歩容の生成が行なわれると共に、各摂動モデル 2 0 2， 2 0 3 , 2 3 1 による補正用摂動モデル上体水平位置 Xc、補正用摂動モデル上体姿勢傾斜角 Θ cおよび補正用摂動モデル逆位相腕振り角 0 caの算出が行なわれる。これにより各摂動モデル 2 0 2， 2 0 3 , 2 3 1の挙動の安定性をより一層高めることができる。なお、上記のような単純化モデル 2 0 0の状態量の修正は、歩容の生成途中で適宜行なってもよい。逆に、上記のような単純化モデル 2 0 0の状態量の修正や摂動モデルのリセットを、歩容の生成途中に行なわなくてもよい。

また、上記のような単純化モデル 2 0 0の状態量の修正は、後述する第 5、第 6参考例においても同様に行なわれる。

第 4参考例では、前記復元条件すベてを満足する。したがって、前記第 3参考例と同様の作用効果を奏することができる。さらに、第 3参考例の作用効果に加え、前記復元条件を満足するように上体水平位置、上体姿勢傾斜角および逆位相腕振り角を決定する際に演算量を比較的少ないものとすることができる。

次に第 5参考例を以下に図 7 1及び図 7 2を参照して説明する。第 5 参考例に係る装置の補正手法は、前記第 4参考例のものと歩容生成装置 1 0 0の処理（図 6 5の S 3 5 3 6の処理）のみが相違するものであり . フルモデルフィードバック補正型である。また、逆動力学フルモデル (逆フルモデル）を用いる手法で、かつ単純化モデル歩容の入力を補正しない手法であり、かつ摂動モデルを用いる手法である。

図 7 1は、この第 5参考例に係る装置の動作、具体的には、図 6 5のフローチャートの S 3 5 3 6の歩容の修正手法を説明する機能プロック図である。ただし、図 7 1の単純化モデル 2 0 0は、前記第 4参考例と同様、単に動力学モデルだけでなく、図 6 5.の S 3 5 1 0から S 3 5 3 2の処理、すなわち単純化モデル歩容瞬時値の算出 (決定) 処理を表す, したがって、図 7 1において、単純化モデル 2 0 0から先の部分が S 3 5 3 6の処理に相当する。なお、図 7 1の機能部分のうち、第 4参考例に係る図 6 6 と同一の機能分については、図 6 6と同一の参照符号を用い、詳細な説明を省略する。

S 3 5 3 6の処理以外の構成は第 4参考例と同じであるので説明を省略し、以下に図 7 1を用いて S 3 5 3 6の処理を詳細に説明する。

S 3 5 3 6では、まず、前述のごとく図 6 5の S 3 5 3 2で求めた単純化モデル歩容の目標上体水平位置の瞬時値（現在時刻 tの制御周期で求めた仮瞬時値）に、前回制御周期（時刻 t 一 A t ) で後述のごとく算出した補正用摂動モデル上体水平位置 Xc を演算部 2 0 5により加えることにより、最終的な目標上体水平位置（補正目標上体水平位置）が求められる。また、図 6 5の S 3 5 3 2で求めた単純化モデル歩容の目標上体姿勢傾斜角の瞬時値（現在時刻 tの制御周期で求めた仮瞬時値）に、前回制御周期（時刻 t 一 A t ) で算出した後述する補正用摂動モデル上体姿勢傾斜角 Θ c を演算部 2 0 4により加えることにより、最終的な目標上体姿勢傾斜角（補正目標上体姿勢傾斜角）が求められる。また、図 6 5の S 3 5 3 2で求めた単純化モデル歩容の目標逆位相腕振り角の瞬時値（現在時刻 tの制御周期で求めた仮瞬時値）に、前回制御周期（時刻 t 一 Δ t ) で算出した後述する補正用摂動モデル逆位相腕振り角 0 ca を演算部 2 3 2により加えることにより、最終的な目標逆位相腕振り角 (補正目標逆位相腕振り角）が求められる。

そして、これらの補正目標上体水平位置、補正目標上体姿勢傾斜角および補正目標逆位相腕振り角が、それぞれ上体水平位置、上体姿勢傾斜角および逆位相腕振り角の最終的な目標瞬時値として出力される。

- すなわち、前記式 h 2 4、式 h 2 5、式 h 3 7により、補正目標上体水平位置と補正目標上体姿勢傾斜角と補正目標逆位相腕振り角とが求められる。

次いで、上記の如く単純化モデル歩容を補正して得られた目標上体水平位置（補正目標上体水平位置）、目標上体姿勢傾斜角（補正目標上体姿勢傾斜角）、目標逆位相腕振り角（補正目標逆位相腕振り角）、および前述のごとく得られた単純化モデル歩容の目標重心位置、目標足平位置姿勢、目標腕姿勢などの運動変数の瞬時値と目標 Z M Pの瞬時値とを、前記逆動力学フルモデル 2 0 1に入力し、その入力された運動変数で表される運動に釣り合う（すなわち運動によって逆フルモデル 2 0 1が発生する）床反力水平成分と目標 Z M Pまわりの床反力モーメント（水平成分および鉛直成分）とが算出される。従って、本第 5参考例では、逆フルモデル 2 0 1には、単純化モデル上体水平位置、上体姿勢傾斜角、逆位相腕振り角に加えて、補正用摂動モデル上体水平位置 Xc と補正用摂動モデル上体姿勢傾斜角 Θ c と補正用摂動モデル逆位相腕振り角 S ea とが追加的に入力される。以降、第 4参考例と同様、逆フルモデル 2 0 1により算出された床反力水平成分、床反力モーメント水平成分、床反力モーメント鉛直成分、をそれぞれフルモデル床反力水平成分 Ffullxy , フルモデル床反力モーメント水平成分 Mfullxy、フルモデル床反力モーメント鉛直成分 Mfullzと呼ぶ。

フルモデル床反力水平成分 Ffull は、補正目標床反力水平成分（現在時刻 t における床反力水平成分の最終的な目標瞬時値）として出力される。

すなわち、以下の式により、補正目標床反力水平成分が決定されて出力される。補正目標床反力水平成分 =フルモデル床反ガ水平成分 Ffull

…式 h 4 8 以上の処理からわかるように、本第 5参考例では、フルモデル補正歩容は、単純化モデル歩容に上体水平位置補正用摂動モデル 2 0 2の挙動と上体姿勢傾斜角補正用摂動モデル 2 0 3の挙動と逆位相腕振り角補正用摂動モデル 2 3 1の挙動とを加えたものになる。さらに、上体傾斜モ一ドが発生する床反力水平成分および逆位相腕振りモードが発生する床反力水平成分がいずれも 0であることを考慮すると、次式が成立する。ただし、単純化モデル床反力水平成分は、単純化モデル歩容の運動によつて発生する床反力を逆フルモデル 2 0 1を用いて算出したものの並進力水平成分である。フルモデル床反力水平成分 Ffull

=単純化モデル床反力水平成分

+上体水平位置補正用摂動モデル床反力水平成分 Fp

…式 h 5 1 次いで、上体水平位置補正用摂動モデル制御則 2 0 6により、上体水平位置補正用摂動モデル安定化モーメント要求値 Mpfdmd が決定される。本実施形態における上体水平位置補正用摂動モデル制御則 2 0 6は. 特願 2 0 0 1 - 1 3 3 6 2 1号で本願出願人が提案した如く、設定される。例えば、次式のように該制御則 2 0 6が決定される。上体水平位置補正用摂動モデル安定化モーメント要求値 Mpfdmd

= Kpg *重心差 + Kvg *補正用摂動モデル上体水平速度 dXc/dt

…式 h 5 2 ただし、重心差は、次式で求められる。重心差 =フルモデル重心水平位置 -単純化モデル重心水平位置

…式 h 5 ύ 式 h 5 2における Kpg、 Kv はフードバック制御則のゲインであり，フルモデル重心水平位置、単純化モデル重心水平位置および補正用摂動モデル上体水平速度 dXc/dt は、それぞれ、前回（時刻 t— Δ t ) に後述のごとく算出したフルモデル歩容瞬時値の重心水平位置、単純化モデル歩容瞬時値の重心水平位置（単純化モデル歩容の瞬間姿勢を基に単純化モデルを用いて計算される重心水平位置 XGs) および補正用摂動モデル上体水平速度 dXc/dtである。即ち、フルモデル重心水平位置から単純化モデル重心水平位置を減算して得た重心差と摂動モデルの状態量のひとつである摂動モデル上体速度とに基づいて摂動モデル制御用フィードバック量（操作量）を演算するようにした。かかる摂動モデル制御則により、前記重心差の時間的平均値を、ほぼ 0に制御することができる。

次いで、上体姿勢傾斜角補正用摂動モデル制御則 2 0 7により、上体姿勢補正用摂動モデル安定化モーメント要求値 Mrfdmd が決定される, これは、第 4参考例と同様の制御則を用いれば良い。従って、該制御則 2 0 7 としては、例えば前記式 h i 1が用いられる。

次いで、無制限時補正目標床反力モーメント Minが Min演算部 2 0 9により求められる（推定される）。無制限時捕正目標床反力モーメント Min は、第 4参考例と同様、前記制約（Z M P制約条件および床反力水平成分制約条件) を無視して、上体水平位置補正用摂動モデル安定化モーメント Mpf を上体水平位置補正用摂動モデル安定化モーメント要求値 Mpfdmd に一致させ、かつ、上体姿勢傾斜角補正用摂動モデル安定化モーメント Mrf を、上体姿勢傾斜角補正用摂動モデル安定化モ —メント要求値 Mrfdmd に一致させると共に、目標 Z M Pまわりの目標床反力モーメントとしてのコンプライアンス制御用目標床反力モーメント水平成分を補償全床反力モーメント水平成分 Mdmdxy と Mpf と Mrf との総和に一致させた場合に、目標 Z M Pまわりに発生する床反力モーメント水平成分である。

無制限時補正目標床反力モーメント水平成分 Min は、第 4参考例と同様、前記式 h i 3の演算によって求められる。すなわち、無制限時補正目標床反力モーメント水平成分 Min は、上体水平位置補正用摂動モデル安定化モーメント要求値 Mpfdmd と上体姿勢傾斜角補正用摂動モデル安定化モーメント要求値 Mrfdmd と補償全床反力モーメント水平成分 Mdmdxyとを加算することにより得られる。

さらに、 Fin 演算部 2 1 0により、無制限時補正目標床反力水平成分 Fin も求められる（推定される）。無制限時補正目標床反力水平成分 Fin は、第 4参考例と同様、前記制約（Z M P制約条件および床反力水平成分制約条件）を無視して、上体水平位置補正用摂動モデル安定化モ —メント Mpf を上体水平位置補正用摂動モデル安定化モーメント要求値 Mpfdmd に一致させ、かつ、上体姿勢傾斜角補正用摂動モデル安定化モーメント Mrf を、上体姿勢傾斜角補正用摂動モデル安定化モーメント要求値 Mrfdmd に一致させると共に、コンプライアンス制御用目標床反力モーメント水平成分を補償全床反力モーメント水平成分 Mdmdxy と Mpf と Mrf との総和に一致させたとした場合に、逆フルモデル 2 0 1が発生する床反力水平成分 ( Ffullに相当）である。

無制限時補正目標床反力水平成分 Fin は、第 4参考例とは異なり、次式により求められる。無制限時補正目標床反力水平成分 Fin

=フルモデル床反力水平成分 Ffull

+ 1/h

* (上体水平位置補正用摂動モデル安定化モーメント要求値 Mpfdmd 一上体水平位置補正用摂動モデル安定化モーメント Mpf)

…式 h 5 4 ただし、上体水平位置補正用摂動モデル安定化モーメント Mpf は前回値（時刻 t — A tでの値）が用いられる。すなわち、上体水平位置補正用摂動モデル安定化モーメント要求値 Mpfdmd と上体水平位置補正用摂動モデル安定化モーメント Mpf の差を求め、. 上体水平位置補正用摂動モデルの入力を前記差だけ増加させることに起因するフルモデル床反力水平成分： Ffull の増加量を、前記差を上体並進モ一ド床反力比率 h で割ることにより推定する。さらに、これにフルモデル床反力水平成分 Ffull を加えることにより、無制限時補正目標床反力水平成分 Fin が推定される。

次いで、第 4参考例と同様の制限手段（制限処理部 2 1 1 ) によって、無制限時補正目標床反力モーメント水平成分 Min と無制限時補正目標床反力水平成分 Fin とから、前記制約（Z M P制約条件および床反力水平成分制約条件）を満足するように、これらに制限を加えた値である (目標 Z M Pまわりの）制限補正目標床反力モーメント水平成分 Mltd と制限補正目標床反力水平成分 Fltd とが決定される。この処理手法は、第 4参考例と同一である。

第 5参考例においても、コンプライアンス制御用目標床反力モーメント水平成分は制限補正目標床反力モーメント水平成分 Mltd に一致され、また、補正目標床反力水平成分は制限補正目標床反力水平成分 Fltd にほぼ一致するので、上記のごとく制限補正目標床反力モーメント水平成分 Mltd と制限補正目標床反力水平成分： Fltd とを決定することによつて、コンプライアンス制御用目標床反力モーメント水平成分および補正目標床反力水平成分は、それぞれ Z M P制約条件および床反力水平成分制約条件をほぼ満足する。

次いで、 Mpf 演算部 2 1 5によって、上体水平位置補正用摂動モデル安定化モーメント Mpf が求められる。さらに詳細には、制限補正目標床反力水平成分 Fltd からフルモデル床反力水平成分 Ffull を減じた値にゲイン Kc を乗じたものを積分器 2 1 5 aにて積分し、さらに得られた積分値に上体並進モード床反力比率 hを乗じることにより、上体水平位置補正用摂動モデル安定化モーメント Mpf が求められる。すなわち、次式により、上体水平位置補正用摂動モデル安定化モーメント Mpfが得られる。

Mpf= h * S Kc(Fltd- Ffull)dt …式 h 5 5 次いで、 Mrf 演算部 2 1 4によって、制限補正目標床反カモ一メント水平成分 Mltd から上体水平位置補正用摂動モデル安定化モーメント Mpf と補償全床反カモ一メント水平成分 Mdmdxy とを減じることにより、上体姿勢傾斜角補正用摂動モデル安定化モーメント Mrf が求められる。すなわち、前記式 h 2 1により、上体姿勢傾斜角補正用摂動モデル安定化モーメント Mrfが得られる。

さらに、前記式 h 2 3により、上体姿勢傾斜角補正用摂動モデル床反力モーメント Mrが決定される。すなわち、. Mrf 演算部 2 1 4の出力である上体姿勢傾斜角補正用摂動モデル安定化モーメント Mrf がそのまま上体姿勢傾斜角補正用摂動モデル床反カモ一メント Mr として決定される。

次いで、次式で定義されるフルモデル床反力モーメント誤差 Merrが Merr演算部 2 1 6により算出される。フルモデル床反力モーメント誤差 Merr

=フルモデル床反力モーメント水平成分 Mfullxy

一制限補正目標床反力モーメント水平成分 Mltd

…式 h 5 6 次いで、 Mp 演算部 2 1 7により、次式に従って、上体水平位置補正用摂動モデル床反カモ一メント Mpが求められる _ά Mp = Mpf— S Km*Merr dt …式 h 5 7 すなわち、フルモデル床反力モーメント誤差 Merr に積分ゲイン Km を乗じた値を積分器 2 1 7 aにて積分すると共に、その積分値の符号を反転する。さらに、該積分器 2 1 7 aの出力を上体水平位置補正用摂動モデル安定化モーメント Mpf に加えることにより、上体水平位置補正用摂動モデル床反力モーメント Mpが求められる。

次いで、上体水平位置補正用摂動モ.デル床反力モーメント Mp を上体位置補正用摂動モデル 2 0 2に入力して、入力された床反力モーメント水平成分に釣り合う補正用摂動モデル上体位置 Xcが算出される。

また、上体姿勢傾斜角補正用摂動モデル床反力モーメント Mr を上体姿勢傾斜角補正用摂動モデル 2 0 3に入カレて、入力された床反力モーメント水平成分に釣り合う補正用摂動モデル上体姿勢傾斜角 0 c が算出される。

S 3 5 3 6における以上までの処理は、 P C T / J P 0 3 / 0 0 4 3 5の通りであるので、ここでの説明は以上に留める。

第 5参考例ではさらに、逆位相腕振り角補正用摂動モデルモーメント決定部 2 3 0において、逆位相腕振り角モーメント補正用摂動モデル Maが決定される。

さらに逆位相腕振り角補正用摂動モデルモーメント Maが逆位相腕振り角補正用摂動モデルに入力されて、捕正用摂動モデル逆位相腕振り角 Θ caが決定される。

以下に、第 5参考例での逆位相腕振り角補正用摂動モデルモ一メント決定部 2 3 0の動作についてその機能ブロック図である図 7 2を用いて詳細を説明する。 .

まず、逆位相腕振り角補正用摂動モデル制御則により、逆位相腕振り角補正用摂動モデル安定化モーメント要求値 Mafdmd が決定される。これは、第 4参考例と同様の制御則を用いれば良い。従って、該制御則としては、例えば前記式 h 3 0が用いられる。

次いで、演算部 2 3 0 gにおいて、フルモデル床反力モーメント鉛直成分 Mfullz と補償全床反力モーメント鉛直成分 Mdmdz と逆位相腕振り角補正用摂動モデル安定化モーメント要求値 Mafdmd との和から逆位相腕振り補正用摂動モデルモ一メント Maの前回値（後述する積分器の出力）を減じることによって無制限時補正目標床反力モーメント鉛直成分 Minz を求める。次いで、制限手段（制限処理部） 2 3 O hにおいて、無制限時補正目標床反力モーメント鉛直成分 Minz を床反力鉛直成分モ一メント許容範囲を越えないように制限を加えることにより（すなわち図示する飽和特性関数に通して）、制限時補正目標床反力モーメント鉛直成分 Mltdz を求める。次いで、フルモデル床反力モ一メント鉛直成分 Mfullz と補償全床反力モーメント鉛直成分 Mdmdz と逆位相腕振り角補正用摂動モデル安定化モーメント要求値 Mafdmd との和を制限時補正目標床反力モーメント鉛直成分 Mltdz から減じた値を積分器

2 3 0 i により、積分ゲイン Kaで積分することにより逆位相腕振り補正用摂動モデルモーメント Maを求めて、これを出力する。また、制限時補正目標床反力モーメント鉛直成分 Mltdz をコンプライアンス制御用目標床反力モーメント鉛直成分として出力する。

以上のごとく、逆位相腕振り角補正用摂動モデルモーメント決定部 2

3 0において、逆位相腕振り補正用摂動モデルモーメント Ma とコンプライアンス制御用目標床反力モーメント鉛直成分が決定される。

求めた補正用摂動モデル上体位置 Xc、補正用摂動モデル上体姿勢傾斜角 0 c、補正用摂動モデル逆位相腕振り角 0 ca、' 上体水平位置補正用摂動モデル安定化モーメント Mpf および逆位相腕振り補正用摂動モデルモ一メン卜 Maは、前述のごとく次回制御周期（時刻 t + Δ t ) において、前回値として用いられる。

残余の構成および処理は、第 4参考例と同一である。この第 5参考例によれば、第 4参考例と同様の作用効果を奏することができる。

次に、第 6参考例を図 7 3および図 7 4を参照して説明する。第 6参考例は、前記第 4および第 5参考例で用いた逆動力学フルモデル（逆フルモデル） 2 0 1に代えて、順動力学モデル（より正確には擬似順動力学モデル）を用いて歩容を修正しつつ生成する手法である。

図 7 3は、この第 6参考例に係る装置の動作を説明する機能ブロック図である。同図 7 3に示すように、本第 6参考例では、擬似順フルモデル（擬似順動力学フルモデル） 2 2 2を備えている。

この擬似順フルモデル 2 2 2は、上体水平位置安定化モーメン卜要求値 Mpfdmd、上体姿勢傾斜角安定化モーメント要求値 Mrfdmd、逆位相腕振り角安定化モーメント要求値 Mafdmd、目標 Z M P、目標床反力鉛直成分、補償全床反力モーメント水平成分 Mdmdxy および補償全床反力モーメント鉛直成分 Mdmdz と、目標足平位置姿勢および目標腕姿勢など上体 3を除く部位の運動状態とを入力として、目標上体鉛直位置、目標上体水平位置、目標上体姿勢傾斜角、目標逆位相腕振り角、目標 Z M Pまわりの目標床反力モーメントとしてのコンプライアンス制御用目標床反力モーメント（水平成分および鉛直成分）、および目標床反力水平成分を出力するモデルである。なお、擬似順フルモデル 2 2 2の目標値入力は、前記図 6 3を参照して説明した如く、歩容パラメータ決定部 1 0 0 aで決定される歩容パラメータを基に、目標瞬時値発生部 1 0 0 により生成される。上記擬似順フルモデル 2 2 2は、具体的には図 7 4の機能ブロック図で表され、図 7 4の破線で囲った部分が擬似順フルモデル 2 2 2に相当する。なお、この機能ブロック図で前記第 5参考例の図 7 1 と同一機能部分については、図 7 1 と同一の参照符号を用いている。

ここで、同図 7 4の単純化モデル 2 0 0は、単に動力学モデルだけでなく、前述の図 6 5の S 3 5 1 0から S 3 5 3 2の処理、すなわち単純化モデル歩容瞬時値の算出（決定）処理を表す。また、 S 3 5 3 2の今回歩容瞬時値（単純化モデル歩容瞬時値）の算出（決定）処理においては、前記第 4参考例で説明したように目標 Z M Pまわりのモデル操作床反力モーメント水平成分を 0として歩容の瞬時値を生成した上で、さらに第 3実施形態において述べた上体姿勢傾斜角補正用摂動モデルモ一メント Mr に相当する単純化歩容上体姿勢傾斜角補正用モーメント Mr (前回値）を発生する上体傾斜モードの摂動運動と第 3実施形態において述べた上体水平位置補正用摂動モデルモ一メント Mp に相当する単純化モデル上体水平位置補正用モーメント Mp (前回値）を発生する上体並進モ一ドの摂動運動と第 4参考例において述べた逆位相腕振り角補正用摂動モデルモ一メント Maに相当する単純化モデル逆位相腕振り角補正用モーメント Ma (前回値）を発生する逆位相腕振りモードの摂動運動を付加する。これにより、単純化モデル 2 0 0が出力する歩容の瞬時値を補正する。

より具体的に説明すると、本第 6参考例における図 6 5の S 3 5 3 2 の処理においては、そのサブルーチン処理である図 5 7の S 3 4 1 4と S 3 4 1 6において、単純化モデル 2 0 0で求めた上体水平加速度に、前記式 0 3 y の右辺第 2項を Mp (前回値）に等しいとした式、すなわち Mp = mb * (Zb— Zzmp) * (d2Xb/dt2)という式に基づいて求められる上体水平加速度（d2Xb/dt2)を摂動分として加えてなる上体水平加速度を今回歩容の初期から現在時刻 t まで 2階積分することにより、現在時刻 tにおける上体水平位置の瞬時値が求められる。また、図 5 7の S 3 4 1 4において、単純化モデル 2 0 0で求めた上体姿勢傾斜角加速度に、前記式 0 3 y の右辺第 7項を Mi' (前回値）に等しいとした式、すなわち、 Mr = J * d2 0 by/dt2 という式に基づいて求められる上体姿勢傾斜角加速度（d2 0 by/dt2) を摂動分として加えてなる上体姿勢傾斜角加速度を今回歩容の初期から現在時刻 tまで 2階積分することにより、現在時刻 tにおける上体姿勢傾斜角の瞬時値が求められる。また、図 5 7 の S 3 4 1 4において、単純化モデル 2 0 0で求めた逆位相腕振り角加速度に、前記式 0 3 _Z の右辺第 8項を Ma (前回値）に等しいとした式. すなわち、 Ma = Jaz * d2 0 az/dt2 という式に基づいて求められる逆位相腕振り角加速度（d2 0 az/dt2)を摂動分として加えてなる逆位相腕振り角加速度を今回歩容の初期から現在時刻 t まで 2階積分することにより、現在時刻 tにおける逆位相腕振り角の瞬時値が求められる。

補足すると、上体水平位置と上体姿勢傾斜角については、サジタルブレーン上の瞬時値について述べたが、ラテラルプレーン上の瞬時値も同様に求める。

図 7 4において、単純化モデル 2 0 0から先の部分が S 3 5 3 6の処理に相当する処理を実行する部分である。以下に図 7 4を用いて S 3 5 3 6の処理を詳細に説明する。

S 3 5 3 6では、まず、 S 3 0 3 2において上記の如く単純化モデル上体水平位置補正用モ一メント Mp (詳しくは時刻（ t _ Δ t ) の制御周期での前回値）を基に補正された単純化モデル水平上体位置が目標上体水平位置（時刻 tでの上体水平位置の最終的な目標瞬時値）として出力される。また、単純化モデル上体姿勢傾斜角補正用モーメント Mr (詳しくは時刻（ t — A t ) の制御周期での前回値）を基に補正された単純化モデル上体姿勢傾斜角が目標上体姿勢傾斜角（時刻 tでの上体姿勢傾斜角の最終的な目標瞬時値）として出力される。また、単純化モデル逆位相腕振り角補正用摂動モデルモーメント Ma (詳しくは時刻（ t - A t ) の制御周期での前回値）を基に補正された単純化モデル逆位相腕振り角が目標逆位相腕振り角（時刻 tでの逆位相腕振り角の最終的な目標瞬時値）として出力される。

すなわち、式 hl 0 0、式 hl 0 1、式 hl 0 2により、最終的な目標上体水平位置と目標上体姿勢傾斜角.とが決定される。目標上体水平位置 =単純化モデル上体水平位置 …式 hi 0 0 目標上体姿勢傾斜角 =単純化モデル上体姿勢傾斜角 …式 hi 0 1 目標逆位相腕振り角 =単純化モデル逆位相腕振り角 …式 hi 0 2 次いで、目標上体水平位置（すなわち単純化モデル上体水平位置）、 '目標上体姿勢傾斜角（すなわち単純化モデル上体姿勢傾斜角）、目標逆位相腕振り角（すなわち単純化モデル逆位相腕振り角）および前述のごとく得られた単純化モデル歩容の目標全体重心位置、目標足平位置姿勢, 目標腕姿勢などの運動変数の瞬時値と目標 Z MPの瞬時値とを、前記逆動力学フルモデル（逆フルモデル） 2 0 1に入力し、入力された運動変数で表される運動に釣り合う（すなわち運動によって逆フルモデル 2 0 1が発生する）床反力水平成分と目標 Z MPまわりの床反力モーメント (水平成分および鉛直成分）とが算出される。以降、第 5参考例と同様，これらの算出された床反力水平成分、床反カモ一メン卜水平成分および床反力モーメント鉛直成分をそれぞれフルモデル床反力水平成分 FfulL フルモデル床反力モーメント水平成分 Mfullxy、床反力モーメント鉛直成分 Mfullzと呼ぶ。第 5参考例と同様、フルモデル床反力水平成分 Ffullは、目標床反力水平成分（時刻 tでの床反力水平成分の最終的な目標瞬時値）として出力される。

本第 6参考例では、上体姿勢傾斜角補正用摂動モデルと上体水平位置補正用摂動モデルと逆位相腕振り角補正用摂動モデルとを備えないので，上体水平位置補正用摂動モデル制御則と上体姿勢傾斜角補正用摂動モデル制御則と逆位相腕振り角補正用摂動モデル制御則とに相当する処理は，後述するように、第 5参考例とは異なる。

このことを除けば、上記処理以降、単純化モデル上体姿勢傾斜角補正用モ一メント Mr と単純化モデル上体水平位置補正用モ一メント Mp と単純化モデル逆位相腕振り角補正用モーメント Ma とを決定するまでは, 第 5参考例において、上体姿勢傾斜角補正用摂動モデルモーメント Mr と上体水平位置補正用摂動モデルモーメント Mp と逆位相腕振り角補正用摂動モデルモーメント Maとを決定する処理と同一の処理が実行される。すなわち、 Min演算部 2 0 9 、 Fin演算部 2 1 0、制限処理部 2 1 1 (制限手段）、 Mpf 演算部 2 1 5 、 Merr 演算部 2 1 6 、 Mrf 演算部 2 1 7 ( = Mr演算部）、 Mp演算部 2 1 4の処理は、第 5参考例と同一である。さらに、逆位相腕振り角補正用摂動モデル制御則とに相当する処理を除けば、単純化モデル逆位相腕振り角補正用モ一メント決定部 2 3 0の処理は、前記第 5参考例における逆位相腕振り角補正用摂動モデルモーメント決定部 2 3 0の処理と同一である。なお、本第 6参考例では、 Mr、 Mpおよび Maは、それぞれ第 5参考例における上体姿勢傾斜角補正用摂動モデルモーメント Mr と上体水平位置補正用摂動モデルモ —メント Mp と逆位相腕振り角補正用摂動モデルモーメント Maとにそれぞれ対応するものであるが、第 5参考例のように摂動モデルに入力するものではなく、単純化モデル 2 0 0に入力するものである。このため、 Mr, Mpおよび Maの名称を本第 6参考例では、単純化モデル上体姿勢傾斜角補正用モ一メント、単純化モデル上体水平位置補正用モ一メント，単純化モデル逆位相腕振り角補正用モ一メントと称している。

上記のごとく求められた単純化モデル上体姿勢傾斜角補正用モーメント Mr と単純化モデル上体水平位置補正用モーメント Mp と単純化モデル逆位相腕振り角補正用モーメント Maとは、前述のごとく次回制御周期（時刻 t + A t ) に、単純化モデル歩容瞬時値の決定（生成）時に前回値として用いられる。

残余の構成及び処理は、第 5参考例と同一である。

以下に、上体姿勢傾斜角安定化モーメント要求値 Mrfdmd と上体水平位置安定化モーメント要求値 Mpfdmd と逆位相腕振り角安定化モーメント要求値 Mafdmd との決定処理について図 7 3を参照して説明する。

図 7 3に示すように、第 6参考例では、前述したように擬似順フルモデル 2 2 2に備える単純化モデル 2 0 0とは別の単純化モデル 2 2 3を備えている。この単純化モデル 2 2 3は、本第 6参考例でのその機能は, 前述した単純化モデル 2 0 0 と同一であり、単に動力学モデルだけでなく、前述の図 6 5の S 3 5 1 0から S 3 5 3 2の処理、すなわち単純化モデル歩容瞬時値の算出（決定）処理を表すものである。なお、単純化モデル 2 2 3は、実際上は、上体姿勢傾斜角の瞬時値と、上体水平位置の瞬時値とを決定できればよく、必ずしも図 6 5の S 3 5 1 0から S 3 5 3 2の処理の全てを行なう必要はない。

以下に図 7 3を用いて第 6参考例における Mpfdmd および Mrfdmd の決定処理を詳細に説明する。

本第 6参考例においては、単純化モデル 2 2 3を用いて生成される歩容と前記擬似順動力学フルモデル 2 2 2を用いて前述の通り算出される歩容との間の上体水平位置、上体姿勢傾斜角および逆位相腕振り角のそれぞれの差が演算部 2 2 4， 2 2 5、 2 2 8により求められる。そして. これらの差に応じて、これらの差が 0に収束するように P I Dなどのフイードバック制御則によつて上体水平位置安定化モーメント要求値 Mpfdmd, 上体姿勢傾斜角安定化モーメント要求値 Mrfdmdおよび逆位相腕振り角安定化モーメント要求値 Mafdmd が決定される。すなわち. 単純化モデル 2 2 3による上体水平位置と擬似順フルモデル 2 2 2による上体水平位置との差に応じて、フィードバック制御則からなる上体水平位置安定化制御則 2 2 6によって、 Mpfdmd が求められる。また、単純化モデル 2 2 3による上体姿勢傾斜角と擬似順フルモデル 2 2 2による上体姿勢傾斜角との差に応じて、フィードバック制御則からなる上体姿勢傾斜角安定化制御則 2 2 7によって、 Mrfdmdが求められる。また、単純化モデル 2 2 3による逆位相腕振り角と擬似順フルモデル 2 2 2による逆位相腕振り角との差に応じて、フィ一ドバック制御則からな 'る逆位相腕振り角安定化制御則によって、 Mafdmdが求められる。そして、この決定された Mpfdmd、 Mrfdmd 及び Mafdmd が、前記擬似順動力学フルモデルにフィ一ドバックされて入力される。

なお、第 6参考例では、歩容生成装置 1 0 0は、前記擬似順動力学フルモデル 2 2 2への入力の一部である目標 Z M P、目標床反力鉛直成分, 目標足平位置姿勢および目標腕姿勢などと、前記擬似順動力学フルモデル 2 2 2からの出力である目標上体鉛直位置、目標上体水平位置、目標上体姿勢傾斜角、目標逆位相腕振り角、目標床反力水平成分およびコンプライアンス制御用目標床反力モーメント（水平成分および鉛直成分）を最終的な今回歩容の目標瞬時値として出力する。

以上説明した第 6参考例によれば、前記第 5参考例と同様の作用効果を奏することができる。なお、前記第 6参考例では、上体姿勢傾斜角安定モーメント要求値 Mrfdmd、上体水平位置安定化モーメント要求値 Mpfdmdおよび逆位相腕振り角安定化モーメント要求値 Mafdmd を擬似順フルモデル 2 2 2 のみに入力するようにしたが、 Mrfdmd、 Mpfdmdおよび Mafdmdを単純化モデル 2 2 3に入力するようにしてもよく、あるいは、単純化モデル 2 2 3と擬似順フルモデル 2 2 2とに分配 · 供給するようにしてもよい。また、補償全床反力モーメント鉛直成分 Mdmdz を単純化モデル 2 2 3に入力し、この Mdmdzが付加的に発生するように単純化モデル逆位相腕振り角を決定するようにしてもよい。

次に、本発明の第 7参考例を図 7 5を参照して説明する。図 7 5は、この第 7参考例に係る装置の動作、具体的には、図 6 5のフローチヤ一トの S 3 5 3 6の歩容の修正手法を説明する機能プロック図である。な 'お、図 7 5では、第 4参考例もしくは第 5参考例と同一機能部分については、図 6 6又は図 7 1 と同一の参照符号を用いる。

第 7参考例においては、上体水平位置補正用摂動モデル 2 0 2 と上体姿勢傾斜角補正用摂動モデル 2 0 3と逆位相腕振り角補正用摂動モデル 2 3 1 とを備える。また、 3つの分配器 2 2 0 , 2 2 1 , 2 4 1を備える。

分配器 2 2 0 , 2 2 1 , 2 4 1を定義すると、いずれも、 1入力 2出力の伝達ブロックであり、入力に応じて、一方の出力を決定し（例えば，入力に周波数特性、不感帯特性、飽和特性などの信号処理を行って一方の出力を決定し）、 2つの出力の和が入力に一致またはほぼ一致するように、他方の出力を決定する伝達ブロックである ό

分配器 2 2 0には、制限補正目標床反力モーメント水平成分 Mltdxy から、上体水平位置補正用摂動モデル安定化モーメント Mpf と補償全床反力モーメント水平成分 Mdmdxyとを減じたもの（Mr演算部 2 1 4 の出力）である上体姿勢傾斜角補正用モーメント Mrが入力され、上体姿勢傾斜角補正用摂動モデル 2 0 3に入力する上体姿勢傾斜角補正用摂動モデル入力 Mri と、単純化モデル 2 0 0に入力する単純化モデル上体姿勢傾斜角補正用モーメント Mrs とに分配される。この時、上体姿勢傾斜角補正用摂動モデル入力 Mri と単純化モデル上体姿勢傾斜角補正用モーメント Mrs との和が上体姿勢傾斜角補正用モーメント Mr に一致するように、上体姿勢傾斜角補正用摂動モデル入力 Mri と単純化モデル上体姿勢傾斜角補正用モーメント Mrs とが決定される（出力される）。

より具体的には、上体姿勢傾斜角補正用モーメント Mr に応じて、上体姿勢傾斜角補正用摂動モデル入力 Mri が決定される。例えば、上体姿勢傾斜角補正用モーメント Mr を不感帯特性、飽和特性、あるいは周波数特性を持つ信号処理を通して、上体姿勢傾斜角補正用摂動モデル入力 Mri が決定される。また、上体姿勢傾斜角補正用モーメント Mr から上体姿勢傾斜角補正用摂動モデル入力 Mri を差し引いたものが、単純化モデル上体姿勢傾斜角補正用モーメント Mrs として決定される。さらに具体的に説明すると、第 7参考例では、分配器 2 2 0は、例えば. 入力（上体姿勢傾斜角補正用モーメント Mr = Mltd—Mpf—Mdmdxy) をローパスフィルタに通してなる低周波成分（直流分）を単純化モデル上体姿勢傾斜角補正用モーメント Mrs として出力すると共に、その入力（上体姿勢傾斜角補正用モーメント Mr) から Mrsを差し引いた成分が上体姿勢傾斜角補正用摂動モデル入力 Mri として出力される。この場合、低周波成分（直流分）である単純化モデル上体姿勢傾斜角補正用モーメント Mrs に関しては、不感帯特性を持たせておき、上記ローパスフィル夕の出力が、ある所定値を中心とする所定範囲内に存在する状態では、 Mi'sが該所定値（例えば 0 ) に維持されるようにしておく。分配器 2 2 0の出力である上体姿勢傾斜角補正用摂動モデル入力 Mri は、前記上体姿勢傾斜角補正用摂動モデル 2 0 3に入力され、上体姿勢傾斜角補正用摂動モデル 2 0 3により補正用摂動モデル上体姿勢傾斜角 Θ cが決定される。

分配器 2 2 0のもうひとつの出力である単純化モデル上体姿勢傾斜角補正用モーメント Mrs は単純化モデル 2 0 0に入力される。これは、前記第 6参考例における図 7 4において、単純化モデル上体姿勢傾斜角補正用モ一メント Mr を単純化モデル 2 0 0に入力することに相当するものである。

分配器 2 2 1においては、フルモデル床反力モーメント水平成分誤差 Merr にゲイン Km を乗じた値を積分器 2 1 7 aによつて積分し、さらに符号を反転した値を入力とする。

分配器 2 2 1の入力は、分配器 2 2 0 と同様、単純化モデル 2 0 0に入力する単純化モデル上体水平位置補正用モーメント Mps と、上体水平位置補正用摂動モデル 2 0 2に入力する誤差補正モーメント Me とに分配される。より具体的には、積分器 2 1 7 aの出力に応じて、誤差補正モーメント Meが決定される。例えば、積分器 2 1 7 aの出力（分配器 2 2 0の入力）を不感帯特性、飽和特性、あるいは周波数特性を持つ信号処理を通して、誤差補正モーメント Meが決定される。また、積分器 2 1 7 aの出力から、誤差補正モーメント Me を差し引いたものが、単純化モデル上体姿勢傾斜角補正用モーメント Mrs として決定される。さらに具体的に説明すると、本第 7参考例では、分配器 2 2 1は、例えば、入力（積分器 2 1 7 aの出力）をローパスフィル夕に通してなる低周波成分（直流分）を単純化モデル上体水平位置補正用モーメント Mps として出力すると共に、その入力（積分器 2 1 7 aの出力）から Mps を差し引いた成分を誤差補正モーメント Me として出力する。この場合、低周波成分（直流分）である単純化モデル上体水平位置補正用モーメント Mps に関しては、不感帯特性を持たせておき、上記ローパスフィルタの出力が、ある所定値を中心とする所定範囲内に存在する状態では、 Mpsが該所定値（例えば 0 ) に維持されるようにしておく。分配器 2 2 1の出力である誤差補正モーメント Me に上体水平位置補正用モデル安定化モメント Mpf を .Mp 演算部 2 1 7 bで加算することにより上体水平位置補正用摂動モデルモーメント Mpが求められる。そして、上体水平位置補正用摂動モデルモーメント Mpは、上体水平位置補正用摂動モデル 2 0 2に入力され、上体水平位置補正用摂動モデル 2 0 2により補正用摂動モデル上体水平位置 Xcが決定される。

分配器 2 2 1のもうひとつの出力である単純化モデル上体水平位置補正用モーメン卜 Mps は単純化モデル 2 0 0に入力される。これは、前記第 6参考例の図 7 4において、単純化モデル上体水平位置補正用モーメント Mpを単純化モデル 2 0 0に入力することに相当するものである, 逆位相腕振り角補正用摂動モデルモーメント決定部 2 3 0は、第 5参考例において用いた図 7 2に示す逆位相腕振り角補正用摂動モデルモーメント決定部と同一である。ただし、図 7 2の出力である逆位相腕振り角補正用摂動モデルモーメント Maは、逆位相腕振り角補正用摂動モデル 2 3 1に直接入力されず、分配器 2 4 1に入力される。そこで、本第 7参考例では逆位相腕振り角補正用摂動モデルモーメント Maは、名称を改めて逆位相腕振り角補正用モデルモーメント Maと呼ぶ。

分配器 2 4 1においては、逆位相腕振り角補正用摂動モデルモ一メント決定部 2 3 0が決定した逆位相腕振り角補正用モデルモーメント Ma を入力とする。分配器 2 4 1の入力は、分配器 2 2 0 と同様、単純化モデル 2 0 0に入力する単純化モデル逆位相腕振り角補正用モーメント Mas と、逆位相腕振り角補正用摂動モデル 2 3 1に入力する逆位相腕振り角補正用摂動モデル入力 Maiとに分配される。

この時、逆位相腕振り角補正用摂動モデル入力 Mai と単純化モデル逆位相腕振り角補正用モーメント Mas との和が逆位相腕振り角補正用モデルモーメント Maに一致するように、逆位相腕振り角補正用摂動モデル入力 Mai と単純化モデル逆位相腕振り角補正用モ一メント Mas とが決定される（出力される）。

より具体的には、逆位相腕振り角補正用モデルモーメント Maに応じて、逆位相腕振り角補正用摂動モデル入力 Mai が決定される。例えば. 逆位相腕振り角補正用モデルモーメント Ma.を不感帯特性、飽和特性、あるいは周波数特性を持つ信号処理を通して、逆位相腕振り角補正用摂動モデル入力 Mai が決定される。また、逆位相腕振り角補正用モデルモーメント Ma から逆位相腕振り角補正用摂動モデル入力 Mai を差し引いたものが、単純化モデル逆位相腕振り角補正用モーメント Mas として決定される。さらに具体的に説明すると、第 7参考例では、分配器 2 4 1 は、例えば、入力（逆位相腕振り角補正用モデルモーメント Ma) を口一パスフィルタに通してなる低周波成分（直流分）を単純化モデル逆位相腕振り角補正用モーメント Mas として出力すると共に、その入力（逆位相腕振り角補正用モデルモーメント Ma) から Mas を差し引いた成分が逆位相腕振り角補正用摂動モデル入力 Mai として出力される。この場合、低周波成分（直流分）である単純化モデル逆位相腕振り角補正用モーメント Mas に関しては、不感帯特性を持たせておき、上記ローパスフィル夕の出力が、ある所定値を中心とする所定範囲内に存在する状態では、 Mas が該所定値（例えば 0 ) に維持されるようにしておく。

分配器 2 4 1 の出力である逆位相腕振り角補正用摂動モデル入力 Mai は、前記逆位相腕振り角補正用摂動モデル 2 3 1 に入力され、逆位相腕振り角補正用摂動モデル 2 3 1により補正用摂動モデル逆位相腕振り角 0 caが決定される。

分配器 2 4 1のもうひとつの出力である単純化モデル逆位相腕振り角補正用モーメント Mas は単純化モデル 2 0 0に入力される。これは、前記第 6参考例における図 7 4において、単純化モデル逆位相腕振り角補正用モーメント Maを単純化モデル 2 0 0に入力することに相当するものである。

単純化モデル 2 0 0は、前記第 6参考例と同様、単純化モデル歩容瞬時値の算出（決定）処理において、目標 Z M Pまわりに床反力モーメント水平成分が発生しないように (モデル操作床反カモ一メン卜を 0として）歩容の瞬時値を生成した上で、単純化モデル上体姿勢傾斜角補正用モーメント Mrs (前回値）を発生する上体.傾斜モードの摂動運動を付加し、単純化モデル上体水平位置補正用モーメント Mps (前回値）を発生する上体並進モードの摂動運動を付加し、さらに単純化モデル逆位相腕振り角補正用モーメント Mas を発生する逆位相腕振りモードの摂動運動を付加して、歩容の瞬時値を補正する。

なお、本第 7参考例では、図 6 5の S 3 5 2 8の処理の一部である図 4 3の S 8 0 0において、前回歩容の終端状態として、前回歩容終端における単純化モデルの状態量を用いる。従って、分配器 2 2 0， 2 2 1， 2 4 1から単純化モデルに出力される Mrs、 Mps及び Masが 0以外の値となる時があれば、単純化モデルは当初の挙動からずれるので、これに応じて S 3 5 2 8において、歩容パラメ一夕が適宜修正される。また，前記不感帯の領域を大きく設定すると、 Mrs、 Mps 及び Mas の絶対値が小さくなるので、歩容パラメ一夕の修正量の絶対値も小さくなる。その他の構成及び処理は、第 5参考例と同一である。より詳しくは、演算部 2 0 4， 2 0 5 , 目標上体水平位置を演算する演算部、 Merr 演算部 2 1 6、上体姿勢傾斜角補正用摂動モデル制御則 2 0 7、上体水平位置補正用摂動モデル制御則 2 0 6、 Min演算部 2 0 9、 Fin演算部 2 1 0、制限処理部 2 1 1 、 Mpf 演算部 2 1 5、逆位相腕振り角補正用摂動モデルモーメント決定部 2 4 0の処理は、前記第 5参考例と同一である。 .

なお、本第 7参考例では、前記第 4参考例で説明したように今回歩容の終端等で各摂動モデル 2 0 2、 2 0 3、 2 3 1の状態量により単純化モデル 2 0 0の状態量を修正するような処理は行なう必要はない。これは、分配器 2 2 0， 2 2 1， 2 4 1から単純化モデル上体姿勢傾斜角補正用モーメント Mrs、単純化モデル上体水平位置補正用モーメント Mps 及び単純化モデル逆位相腕振り角補正用モーメント Mas が追加入力されるからである。

かかる本実施形態では、前記第 5参考例あるいは第 6参考例と同様の作用効果を奏することができる。

なお、本第 7参考例では、各分配器 2 2 0， 2 2 1、 2 4 1の 2つの出力のうち一方を 0とし、他方を入力に一致させるようにしても良い。

この場合、例えば分配器 2 2 0の出力である単純化モデル上体姿勢傾斜角補正用モーメント Mrs を 0にし、分配器 2 2 1の出力である単純化モデル上体水平位置補正用モーメント Mps を 0にし、分配器 2 4 1 の出力である単純化モデル逆位相腕振り角補正用モーメント Mas を 0 にすると、第 5参考例と同一の作用効果を奏することとなる（事実上、第 5参考例のものと同一構成となる）。 '

また、誤差補正モーメント Me に上体水平位置補正用モデル安定化モ一メント Mpf を加算した値を図示しない第 3の分配器に入力し、その出力の一方を上体水平位置補正用摂動モデルに入力し、もう一方の出力を単純化モデル上体水平位置補正用モーメント Mps に加算しても良い, この場合、分配器 2 2 1の出力である単純化モデル上体水平位置補正用モーメント Mps を 0にしても良い。すなわち、分配器 2 2 1を省略しフルモデル床反力モーメント誤差 Merrにゲイン Kmを乗じた値を積分して符号を反転した値を上体水平位置補正用モデル安定化モーメント Mpf に加算した値を第 3の分配器に入力しても良い。

なお、以上説明した第 4〜第 7参考例では、図 6 5の S 3 5 3 2において、目標 Z M Pを満足するように、すなわち、目標 Z M Pまわりの床反力モーメント水平成分が 0になるように、今回歩容瞬時値（目標運動の仮瞬時値）を決定するようにしたが、これに代えて、第 2参考例における図 5 6の S 3 0 3 2のように、目標 Z M Pまわりにモデル操作床反カモ一メント水平成分が発生するように今回歩容瞬時値を決定するようにしてもよい。この場合には、図 5 3に示す如く、歩容生成装置 1 0 0 に補償全床反力モーメント水平成分分配器 1 1 0からモデル操作床反力モーメント水平成分を与えるようにすればよい。

また、以上説明した第 1〜第 7参考例においては、補償全床反力モーメント水平成分 Mdmdxy は、上体姿勢角 · 角速度の代わりに、全体重心水平位置 · 速度など、口ポット 1の他の姿勢に関する状態量に基づいて決定しても良い。

次に、本発明の第 1実施形態を図 7 6から図 8 3を参照して説明する本実施形態における制御ュニット 6 0の機能ブロック図を図 7 6に示す。なお、本実施形態では、前記第 3参考例と同一構成部分もしくは同一機能部分については第 3参考例と同一の参照符号を用いる。

本実施形態では、第 3参考例（図 5 9参照）に比較して、実ロボット 1の滑りの有無を判定する滑り判定部 1 1 4が追加されている。また、滑り判定部 1 1 4から出力される滑り判定結果が歩容生成装置 1 0 0に入力される。歩容生成装置 1 0 0においては、滑り判定結果が滑りありと判定されると、歩容パラメータを基に決定される床反力水平成分許容範囲および床反力モーメント鉛直成分許容範囲を狭める。また、歩容生成装置 1 0 0は、滑り判定結果が滑りなしと判定されると、歩容パラメ一夕を基に決定される床反力水平成分許容範囲および床反力モーメント鉛直成分許容範囲に戻す。他の構成および処理に関しては、前記第 3参考例と同一である。

滑り判定部 1 1 4を、その処理を表すフローチヤ一トである図 7 9を用いて詳細に説明すると、まず、 S 5 2 1 0において、スピン判定サブル一チンが実行される。スピン判定サブルーチンでは、そのフローチヤートである図 8 0の S 5 3 1 0に進み、実上体姿勢角速度および関節角指令（検出値）を基に支持脚足平 2 2の対地角速度鉛直成分 co supz を求める。次いで S 5 3 1 2に進み、足平 2 2の 6軸力センサ 5 0の検出値 (実床反力) を基に、支持脚に作用している実床反力モーメント鉛直成分の時間的な変化率である支持脚床反力モーメント鉛直成分の変化率 dMsupactz/dt を求める。次いで S 5 3 1 4に進み、前記支持脚足平 2 2の対地角速度鉛直成分 supz の絶対値がある所定の値 o> e を越えているか否かを判定する。越えている場合には、 S 5 3 1 6に進み、前記支持脚床反力モーメント鉛直成分の変化率 dMsupactz/dt を前記支持脚足平の対地角速度鉛直成分 co supz で割って符号を反転した値である、支持脚の見かけねじりばね定数 Ksupt を求める。次いで S 5 3 1 8に進み、 Ksupt がある所定の値 Ksuptmin よりも小さいか否かを判定し. 小さい場合には、 S 5 3 2 0に進み、スピン判定結果を、スピンありと判定し、そうでない場合には、 S 5 3 2 2に進み、スピン判定結果を、スピンなしと判定する。また、 S 5 3 1 4において、前記支持脚足平の対地角速度鉛直成分 supz の絶対値が前記所定の値 c e を越えていないと判定された場合には、 S 5 3 2 4に進み、スピン判定結果を、スピンなしと判定する。このようにスピンの有無を判定することで、足平 2 2などの撓み（弾性変形）によって、 o supz の絶対値がある所定の値 ωβ を超えたようなときに、スピン有りと誤判定されるような事態が回避される。

以上のごとく S 5 2 1 0の処理を完了した後、 S 5 2 1 2に進み、並進滑り判定サブルーチンが実行される。

並進滑り判定サブルーチンでは、そのフローチャートである図 8 1の S 5 4 1 0に進み、実上体姿勢角速度、加速度検出値および関節角指令 (検出値）を基に支持脚足平 2 2の対地並進速度水平成分 Vsupxyを求める。次いで S 5 4 1 2に進み、足平 2 2の 6軸力センサ 5 0の検出値 (実床反力）を基に、支持脚に作用している実床反力水平成分の時間的な変化率である支持脚床反力水平成分の変化率 dFsupactxy/dt を求める。次いで S 5 4 1 4に進み、前記支持脚足平の対地並進速度水平成分 Vsupxy の絶対値がある所定の値 Ve を越えているか否かを判定する。越えている場合には、 S 5 4 1 6に進み、前記支持脚床反力水平成分の変化率 dFsupactxy/dt を前記支持脚足平 2 2の対地並進速度水平成分 Vsupxy で割って符号を反転した値である、支持脚の見かけせん断ばね定数 Ksups(=(— dFsupactxy/dt)/Vsupxy)を求める。次いで S 5 4 1 8 に進み、 Ksups がある所定の値 Ksupsmin よりも小さいか否かを判定し、小さい場合には、 S 5 4 2 0に進み、並進滑り判定結果を、並進滑りありと判定し、そうでない場合には、 S 5 4 2 2に進み、並進滑り判定結果を、並進滑りなしと判定する。また、 S 5 4 1 4において、前記支持脚足平の対地並進速度水平成分 Vsupxy の絶対値が前記所定の値 Ve を越えていないと判定された場合には、 S 5 4 2 4に進み、並進滑り判定結果を、並進滑りなしと判定する。このように並進滑りの有無を判定することで、足平 2 2などの撓み（弹性変形）によって、 K sups の絶対値がある所定の値 Ksupsmin .を超えたようなときに、並進滑り有りと誤判定されるような事態が回避される。

以上のごとく S 5 2 1 2の処理を完了した後、 S 5 2 1 4に進み、滑り振動判定サブルーチンが実行される。

滑り振動判定サブルーチンでは、そのフローチャートである図 8 2の S 5 5 1 0に進み、足平 2 2の 6軸力センサ 5 0の検出値（並進力水平成分およびまたはモーメン卜鉛直成分) をバンドパスフィルタに通すことにより、滑り振動成分を抽出する。尚、 .足平 2 2の 6軸力センサ 5 0の検出値の代わりに、足平 2 2にョーレートセンサ、加速度センサ等を備えて、その検出値を用いても良い。また、バンドパスフィル夕の通過周波数帯は、足平 2 2の足底や床面の材質などに応じてあらかじめ設定される。次いで S 5 5 1 2に進み、検出された滑り振動成分の大きさの絶対値がある所定の値 Ampe を越えているか否かを判定する。越えている場合には、 S 5 5 1 4に進み、滑り振動判定結果を、滑り振動ありと判定し、そうでない場合には、 S 5 5 1 6に進み、滑り振動判定結果を、滑り振動なしと判定する。

以上のごとく S 5 2 1 4の処理を完了した後、 S 5 2 1 6に進み、スピン判定結果がスピンあり、または並進滑り判定結果が並進滑りあり、または滑り振動判定結果が滑り振動ありであるならば、 S 5 2 1 8に進み、総合判定結果である滑り判定結果を滑りありと決定し、そうでないならば、 S 5 2 2 0に進み、総合判定結果である滑り判定結果を滑りなしと決定する。以上が、滑り判定部 1 1 4の処理である。

図 7 7に本実施形態における歩容生成装置 1 0 0のメインルーチン処理のフローチャートを示す。

同図 7 7おいて、 S 2 3 1 0から S 2 3 3 2までは、第 3参考例のフ口—チャート（図 6 0 ) の S 2 0 1 0から S 2 0 3 2までの処理と同一の処理が行なわれる。

次いで S 2 3 3 4に進み、修正歩容瞬時値決定サブルーチンが実行される。

本実施形態では、修正歩容瞬時値決定サブルーチンでは、元歩容に近づくように目標 ZMPおよび逆位相腕振り復元角加速度パターンを修正しつつ、修正された目標 Z MPまわりにモデル操作床反力モーメント (水平成分および鉛直成分）が付加的に発生するように修正歩容瞬時値が決定される。ただし床反力許容範囲を滑り判定結果に応じて変更されこの点が第 3参考例と相違する。

図 7 8に修正歩容瞬時値決定サブルーチンのフローチヤ一トを示す。修正歩容瞬時値決定サブルーチンでは、まず S 5 1 0 0から S 5 1 1 4 までの処理が、第 3参考例の修正歩容瞬時値決定サブルーチンである図 6 1の S 2 1 0 0から S 2 1 1 2と同様に実行される。次いで、 S 5 1 1 6に進み、前記滑り判定部 1 1 4による滑り判定結果が滑りありであるか否かを判定し、前記滑り判定結果が滑りありの場合には、 S 5 1 1 8に進み、許容範囲低減率 att を徐々に 0に近づける（実質的に連続的に減少させる）。尚、許容範囲低減率 att は 0から 1までの値をとるものとする。 S 5 1 1 6において前記滑り判定結果が滑りなしと判定された場合には、 S 5 1 2 0に進み、許容範囲低減率 att を徐々に 1に近づける（実質的に連続的に増加させる）。

次いで S 5 1 2 2に進み、 S 5 1 1 0および S 5 1 1 2で決定した床反力水平成分と床反力モーメント鉛直成分の許容範囲の上限値および下限値である Fxmin, Fxmax, Mzmin および Mzmax にそれぞれ許容範囲低減率 att を乗じることにより、床反力水平成分許容範囲と床反カモーメント鉛直成分許容範囲を狭める。以降、この S 5 1 2 2で決定される床反力水平成分許容範囲および床反力モーメント鉛直成分許容範囲を総称的に最終的床反力許容範囲という。尚、当然のことながら低減率が 1の場合には、最終的床反力許容範囲は、 S 5 1 1 0および S 5 1 1 2 で決定した床反力水平成分と床反力モーメント鉛直成分の許容範囲（以下、これらを元の床反力許容範囲ということがある）に一致し、該元の床反力許容範囲から狭まらない。

図 8 3に、滑り判定結果に応じて、許容範囲低減率 att、最終的床反力許容範囲（具体的には、床反力水平成分許容範囲および床反力モ一メント鉛直成分許容範囲）が変化する様子の例を示す。

滑りが発生した場合には、グリップ状態に早急に回復しないとロポット 1が転倒する恐れが生じるので、滑りが発生したと判定されたならば. 最終的床反力許容範囲をすばやく狭めるようにした方が良い。ただし、急激過ぎると、歩容の加速度変化が大きくなり過ぎ、衝撃などが生じるので、急激過ぎない適切な値にすべきである。

滑りがなくなったと判定されたならば、最終的床反力許容範囲を元の床反力許容範囲にすばやく戻すようにした方が良い。最終的床反力許容範囲が狭いままであると、修正歩容が元歩容から大きくずれていってしまうからである。ただし、急激過ぎると、歩容の運動の加速度変化が大きくなり過ぎ、衝撃などが生じて再び滑ってしまうので、急激過ぎない適切な値にすべきである。次いで S 5 1 2 4に進み、図 6 1の S 2 1 1 4と同様、床反力モ一メント水平成分許容範囲と床反力モーメント鉛直成分許容範囲と床反力水平成分許容範囲との条件を満足するように、モデル操作床反力モーメント水平成分、コンプライアンス制御用目標床反力モーメント（水平成分および鉛直成分）、上体水平加速度および上体姿勢傾斜角加速度および逆位相腕振り角加速度を決定する。

次いで S 5 1 2 6から S 5 1 2 8まで進み、図 6 1の S 2 1 1 6から S 2 1 1 8までと同様の処理が行われる。

上記のごとく修正歩容瞬時値決定サブルーチンが実行され、次いで図 7 7の S 2 3 3 6に進み、現在時刻 tを Δ tだけ増加して、再び S 2 3 1 4に戻り、歩容生成を続ける。

以上が、本実施形態における歩容生成装置 1 0 0の処理である。

上記のごとく、本実施形態においては、床反力水平成分許容範囲および床反力モーメント鉛直成分許容範囲を滑り判定結果に応じて変更するようにしたが、床反力水平成分許容範囲および床反力モーメント鉛直成分許容範囲のいずれか一方だけを変更するようにしても良い。

一般的に、足平 2 2が床面上で平行に滑っている場合（せん断方向に滑っている場合）には、スピン方向（ョ一方向）のグリップ力も低下する。また逆に、スピン方向（ョ一方向）に滑っている場合には、足平 2 2が床面上で平行に滑り易くなる。したがって、いずれかの滑りが検知されたならば、床反力水平成分許容範囲および床反力モーメント鉛直成分許容範囲の両方を狭める（ 0にする）方が滑りから回復する効果は高い。

以上説明した第 1実施形態は、本発明の第 3〜第 7発明の実施形態になっている。この場合、第 1実施形態における床反力水平成分が制限対象量、上体姿勢傾斜角偏差およびノまたは上体姿勢傾斜角速度偏差ががロボット 1の状態量の偏差に相当する。また、第 1実施形態における歩容生成用の床反力水平成分許容範囲 [Fxmin,Fxmax]が制限対象量の許容範囲に相当する。また、発明における動力学モデルは、図 1 2の動力学モデルが相当する。また、図 7 7の S 2 3 3 4で決定される修正歩容瞬時値の運動成分と床反力成分とがそれぞれ目標運動の瞬時値および目標床反力の瞬時値に相当する。

前記第 1実施形態では、歩容生成装置 1 0 0として前記第 3参考例のものを用いたが、第 3参考例の代わりに第 1、第 2、第 4〜第 7参考例のいずれのものを用いても良い。

第 1参考例を用いる場合の実施形態では、 .例えば図 1 3の S 0 3 0で今回歩容瞬時値を決定するときに、そのサブルーチンである図 4 5の S 1 4 1 1 と S 1 4 1 2との間に図 7 8の S 5 1 1 6〜S 5 1 2 2の処理を挿入すればよい。このようにした実施形態（第 2実施形態）は、本発明のうちの第 1発明、第 2発明並びに第 5〜第 7発明の実施形態となる, この場合、第 1実施形態における床反力水平成分が第 1発明における制限対象量に相当し、床反力鉛直成分が第 2発明における制限対象量に相当する。また、第 1実施形態における歩容生成用の床反力水平成分許容範囲 [Fxmin，Fxma_X]が第 1発明における制限対象量の許容範囲に相当し、歩容生成用の床反力モーメント鉛直成分許容範囲 [Mzmi_n，Mzmax] が第 2発明における制限対象量の許容範囲に相当する。また、発明における動力学モデルは、図 1 2の動力学モデルが相当する。また、図 1 3 の S 0 3 0で決定される今回歩容瞬時値の運動成分が目標運動に相当する。また、図 1 3の S 0 3 0の中で実行される図 4 6の S 1 0 0 4において、 S 5 0 4， S 5 1 8 (図 2 6を参照）で仮に決定される運動が前記第 1発明あるいは第 2発明における仮運動に相当する。

また、第 2参考例を用いる場合の実施形態では、例えば図 5 6の S O 3 0 3 2で今回歩容瞬時値を決定するときに、そのサブルーチンである図 5 7の S 3 4 1 1 と S 3 4 1 2との間に図 7 8の S 5 1 1 6〜S 5 1 2 2の処理を挿入すればよい。このようにした実施形態（第 3実施形態）は、本発明のうちの第 3〜第 7発明の実施形態となる。この場合、第 3実施形態における床反力水平成分が制限対象量に相当し、上体姿勢傾斜角偏差および Zまたは上体姿勢傾斜角速度偏差が第 3及び第 4発明におけるロボット 1の状態量の偏差に相当する。また、第 3実施形態における歩容生成用の床反力水平成分許容範囲 [Fxmin,F_Xmax]が制限対象量の許容範囲に相当する。また、発明における動力学モデルは、図 1 2の動力学モデルが相当する。また、図 5 6.の S 3 0 3 4で決定される今回歩容瞬時値の運動成分と床反力成分とが第目標運動の瞬時値、目標床反力の瞬時値に相当する。なお、第 3実施形態では、床反力モーメント鉛直成分補償量許容範囲を滑りの発生の判断結果に応じて可変的に設定するようにしてもよい。

また、第 4参考例を用いる場合の実施形態では、例えば図 6 5の S 3 5 3 6の処理（フルモデルを用いた歩容の修正処理）を実行するときに. 図 7 8の S 5 1 1 6〜 S 5 1 2 2の処理を実行して、許容範囲を可変的に設定するようにすればよい。このようにした実施形態（第 4実施形態）は、本発明のうちの第 3〜第 7発明の実施形態となる。この場合、第 4実施形態における床反力水平成分が制限対象量に相当し、上体姿勢傾斜角偏差およびまたは上体姿勢傾斜角速度偏差がロポット 1の状態量の偏差に相当する。また、第 4実施形態における歩容生成用の床反力水平成分許容範囲 [Fxmin，Fxmax]が制限対象量の許容範囲に相当する' また、図 6 5の S 3 5 3 6で修正された歩容の運動成分および床反力成分がそれぞれ第 3発明及び第 4発明における目標運動の瞬時値、目標床反力の瞬時値に相当する。また、発明における動力学モデルは、逆フルモデル（各摂動モデル 2 0 2， 2 0 3 , 2 3 1を含む）が相当する。また、第 5参考例を用いる場合の実施形態では、例えば図 6 5の S 3 5 3 6の処理（フルモデルを用いた歩容の修正処理）を実行するときに図 7 8の S 5 1 1 6〜S 5 1 2 2の処理を実行して、許容範囲を可変的に設定するようにすればよい。このようにした実施形態（第 5実施形態）は、本発明のうちの第 3〜第 7発明の実施形態となる。この場合、第 5実施形態における床反力水平成分が制限対象量に相当し、上体姿勢傾斜角偏差およびノまたは上体姿勢傾斜角速度偏差がロポット 1の状態量の偏差に相当する。また、第 5実施形態における歩容生成用の床反力水平成分許容範囲 [Fxmin,Fxinax]が制限対象量の許容範囲に相当する, また、図 6 5の S 3 5 3 6で修正された歩容の運動成分および床反力成分がそれぞれ第 3発明及び第 4発明における目標運動の瞬時値、目標床 '反力の瞬時値に相当する。また、発明における動力学モデルは、フルモデル（各摂動モデル 2 0 2 , 2 0 3， 2 3 1を含む）が相当する。

なお、第 6および第 7参考例を用いる場合の実施形態は、上記第 5実施形態と同様であり、第 3〜第 7発明の実施形態となる。また、滑りの判定方法としては、前記第 1実施形態において挙げた方式以外にも、以下のようないずれを用いても良い。あるいはこれらの結果を総合的に（論理和、積などの論理演算を用いてい）判断しても良い, 1 ) 角速度検出器などを基に床に対する足平 2 2の角速度を求め、この値の絶対値がある所定の値を越えたら、滑りが発生と判定する。

2 ) いずれかの脚体 2の床反力モーメントの絶対値がある所定の値を越えたら、滑りが発生と判定する。 3 ) 全床反力モーメント（両脚体 2 , 2の床反力の合力のモーメント）の絶対値がある所定の値を越えたら、滑りが発生と判定する。

4 ) 両脚支持期において、全床反力モーメントの変化率と上体姿勢角速度偏差との比に（一 1 ) を乗じた値がある所定の値（両脚接地時ねじり剛性）よりも小さいならば、滑りが発生と判定する。

5 ) 視覚センサや空間フィルタ等を用いた非接触型の対地速度、角速度検出器、あるいは接触型の対地速度、角速度検出器の検出値（推定値）の絶対値がある所定の値を越えたならば、滑りが発生と判定する。

次に、前記第 1〜第 7参考例およびこれを用いる実施形態（前記第 1 〜第 5実施形態を含む）に係る変形態様を以下に説明する。

前記第 2〜第 7参考例及びこれを用いる実施形態では、補償全床反力モーメン卜水平成分およびを補償全床反力モーメント鉛直成分を決定するようにしたが、補償全床反カモ一メント水平成分および補償全床反力モ一メント鉛直成分のいずれか、もしくは両者を 0に設定しても良い。ただしこの場合には、フルモデル補正は、単に単純化モデルによる目標歩容の精度を高めるために用いられ、フルモデルを用いて歩容を修正することによる上体姿勢傾斜復元効果および Zまたは上体ョ一角（角速度）復元効果はなくなる。

前記第 4〜第 7参考例およびこれを用いる実施形態においては、単純化モデル 2 0 0による歩容生成（単純化モデル歩容生成）には、本願出願人が先に提案した特開平 5 - 3 3 7 8 4 9号公報の第 1実施例、第 2 実施例の歩容生成装置を用いても良い。フルモデルによる補正量を小さくすることができるので、補正ざれた歩容が単純化モデル歩容から大きくずれることを防止できる。前記第 2参考例およびこれを用いる実施形態においては、補償全床反力モーメント水平成分 Mdmdxy が床反力モーメント許容範囲水平成分を越えると（第 4参考例以降およびそれを用いる実施形態においては、補償全床反力モーメント水平成分 Mdmdxy とモデル操作床反力モ一メント水平成分との和が床反力モーメント許容範囲を越えると）、コンプライアンス制御用目標床反力モーメント水平成分は、床反力モーメント水平成分許容範囲の上限値あるいは下限値になる。但し、補償全床反力モーメント水平成分 Mdmdxy (第 4参考例以降およびそれを用いる実施形態においては、補償全床反力モーメント水平成分 Mdmdxy とモデル操作床反力モーメント水平成分との和）が床反力モーメント水平成分許容範囲を越えていても、補償全床反力モーメント水平成分 Mdmdxy (第 4参考例以降およびそれを用いる実施形態においては、補償全床反力モーメント水平成分 Mdmdxy とモデル操作床反力モーメン卜水平成分との和) の増加もしくは減少に応じてコンプライアンス制御用目標床反力モーメント水平成分も増加もしくは減少するようにしても良い。コンプライアンス制御用目標床反力モーメン卜水平成分が床反力モーメント水平成分許容範囲に接近すると、コンプライアンス制御によって制御される実床反力モ一メント水平成分が目標よりも少なめになる傾向があり、このためコンプライアンス制御用目標床反カモ一メント水平成分が少々許容範囲を越えても、すぐには、足平 2 2の本来の接地感を低下させたり、足平 2 2の底面が浮いてしまうような不具合が発生しない場合が多いからである。

同様の理由により、床反力モーメント水平成分許容範囲をいわゆる支持多角形で表される Z M P存在可能範囲（厳密な表現では、実床反力作用点の存在許容範囲）をモーメン小水平成分に換算して得られる許容範囲を越えて設定しても良い。コンプライアンス制御によつて発生する床反力モ一メント水平成分に過度に期待すると、上記のごとく足平 2 2の本来の接地感を低下させたり、足平 2 2の底面が浮いてしまうような不具合が発生する。よって、床反力モーメント水平成分許容範囲は、姿勢制御がコンプライアンス制御に期待する実床反力モーメント水平成分の許容範囲であると言える。床反力モーメント水平成分許容範囲は、歩容パラメータからのみならず、検出された実床反力にも基づいて決定しても良い。また、階段でのエッジ位置検出値など、足平 2 2の接地領域の検出値にも基づいて決定してもよい。

一方、コンプライアンス制御によって発生する床反力モーメント鉛直成分に過度に期待すると、スピンしてしまうような不具合が発生する。よって、床反力モーメント鉛直成分許容範囲は、姿勢制御がコンプライアンス制御に期待する実床反力モーメント鉛直成分の許容範囲であると言える。

前記第 2参考例およびこれを用いる実施形態の図 5 6の S 3 0 2 8における今回歩容パラメ一夕の修正（目標 Z M Pの修正）を制御周期毎に実行するように、メインフローチャートの処理を変更しても良い。

修正歩容（目標歩容）が元歩容より大きくずれてしまう場合（発散してしまう場合）には、次回歩容の歩容パラメ一夕修正をする頃に既に、大きくずれて過ぎてしまい（発散し過ぎてしまい）、次回歩容の歩容パラメ一夕修正だけでは、継続的長期的に安定な目標歩容を生成することが困難になる。今回歩容パラメ一夕の修正（目標 Z M Pの修正）を制御周期毎に実行することによって、この問題をかなり解決することができる。

さらには、元歩容からのずれ具合によって、今回歩容の足平着地位置や着地時刻などを制御周期毎に変更しても良い。具体的には、 S 3 0 2 0から S 3 0 3 0までの処理を制御周期毎に実行するように処理フローを変更しておき、 S 3 0 2 0において、次回歩容支持脚座標系（次回歩容支持脚座標系は、次の足平着地位置姿勢に対応）、次次回歩容支持脚座標系（次次回歩容支持脚座標系は、次の次の足平着地位置姿勢に対応）、今回歩容周期（今回歩容周期は、次の足平着地時刻に対応）および次回歩容周期（次回歩容周期は、定常歩容の周期に相当）の少なくともいずれかを、 S 3 0 2 8における今回歩容パラメータの修正（特に目標 Z M Pの修正）が小さくなるように（すなわち、今回歩容の安定余裕が高く維持さ'れるように）、適宜変更すれば良い。

さらには、上記以外の歩容パラメ一夕を変更しても良い。

また、修正歩容の元歩容からのずれは、単純化モデル上体姿勢傾斜角補正用モーメント Mr、単純化モデル上体水平位置補正用モーメント Mp および単純化モデル逆位相腕振り角補正用モーメント Ma などから動力学モデルを用いて推定することができるので、 Mr, Mp および Ma 'に応じてモデルの挙動ずれを推定し、推定された挙動ずれを基に、歩容パラメータを修正してもよい。あるいは、 Mr， Mpおよび Maと歩容パラメ一夕の修正量の適正値との関係をあらかじめ求めてマツプ化しておいて、 Mr， Mp および Ma に応じてマップを基に歩容パラメータの修正量を決定してもよい。

また、他の実施形態（第 4〜第 7参考例を用いる実施形態）に対しても上記と同様に、処理フロ一を変更して良い。

復元条件には、前記した条件以外にも、関節角が許容範囲を越えていないか、脚などの干渉がないか、関節角速度、トルクが過大でないかなどのキネマティクス条件、ダイナミクス条件を加えても良い。

また、これに伴って、前述したごとく、今回歩容パラメ一夕の修正 (目標 Z M Pあるいは着地位置、時刻などの修正）を制御周期毎に実行するようにメインフローチヤ一卜の処理を変更した場合には、今回歩容の安定余裕が高く維持されるように適宜変更される歩容パラメ一夕の値が、適切な値になるように（ある制約条件を満足するように）することも、復元条件のひとつに加えても良い。

歩容の切り変わり目で決定された（読み込まれた）着地位置、着地時刻は、上位の制御装置からの指示（歩行計画決定部やオペレータからの指示など。これを当初の要求と呼ぶ。）によって決定されるので、修正歩容の着地位置、着地時刻は、できる限り歩容の切り変わり目で決定された（読み込まれた）着地位置、着地時刻に戻るべきである。そこで、歩容の切り変わり目で決定された（読み込まれた）着地位置、着地時刻を記憶し、修正歩容の着地位置、着地時刻が、記憶された着地位置、着地時刻にできる限り一致または近づくことも復元条件に加えても良い。ただし、実は、前記復元条件 4、 5および 6によって、修正歩容の着地位置、着地時刻は、できる限り歩容の切り変わり目で決定された（読み込まれた）着地位置、着地時刻に徐々に戻る作用が働くので、あえて追加する必要はない。

また、状況の変化に対応して、当初の要求を変更し、変更した要求を満足する歩容パラメ一夕に、上記のごとく歩容の安定余裕を高く維持するために適宜変更される歩容パラメ一夕が、できる限り一致または近づくことを復元条件に加えても良い。この場合、前記前記復元条件 4、 5 および 6は削除するべきである。

前述した各種の復元条件を満足するモデル上体水平位置安定化床反力モーメントとモデル上体姿勢傾斜角安定化床反力モーメントの決定法には、制約条件下で最適値を求めるための線形計画法（シンプレックス法など）や探索法を用いれば良い。または、フアジ一推論を用いても良い, また、着地位置を変更する場合には、歩行環境の障害物なども考慮しなければならない状況も考えられる。このような状況にも対処できるようにするためには、環境認識や行動決定などの人工知能の分野に属する処理も加えて、修正歩容を決定すべきである。

モデル逆位相腕振り角安定化床反力モーメントの決定法も同様である, 前記第 4〜第 6参考例およびこれを用いる実施形態における制限手段 (制限処理部）の前後のブロック図は、例えば本願出願人による P C T / J P 0 2 / 1 3 5 9 3で示したように、等価変換あるいは近似変換しても良い。

この場合、上体水平位置補正用モデル安定化モーメント Mpf を hで割った値をローパスフィル夕に通した値を補正目標床反力水平成分（フルモデル床反力水平成分 Ffull) から減じた値が上記 P C T出願におけるバイアス推定値 destm に相当する。した.がって、例えば、前記第 5 〜第 7参考例およびこれを用いる実施形態におけるゲイン Kc の積分を時定数 1 /Kc の 1次遅れをフィ―ドバック要素としたポジティブフィ一卞バック系に置き換えたりして、図 8 4のように、近似変換をしても良レ。この例では、第 5〜第 7参考例およびこれを用いる実施形態における Mpf演算部 2 1 5が図示の如く近似変換されている。

逆位相腕振り角補正用摂動モデルモーメントも、同様に等価変換あるいは近似変換して求めるようにしてもよい。

前記第 4参考例およびこれを用いる実施形態においては、前記逆位相腕振り角補正用摂動モデルモーメントは、図 7 0に示す機能ブロック図の代わりに、図 8 5に示す機能ブロック図の処理を行うようにしても良い。以下に説明すると、図 7 0では、逆位相腕振り角補正用モデルが復元する動作によって発生するモ一メント鉛直成分までも含めて、床反力モーメント鉛直成分許容範囲を越えるか否かが判定されて、逆位相腕振り角補正用モデルの挙動が決定されたが、図 8 5に示す機能ブロック図の処理では、逆位相腕振り角補正用モデルが復元する動作によって発生するモーメント鉛直成分は、床反力モーメント許容範囲を越えるか否かを判定する上で無視される。

図 8 5に示す機能ブロック図の処理を詳細に説明すると、フルモデル床反力モーメント鉛直成分 Mfullz と上体水平位置補正に起因する床反力モーメント鉛直成分摂動量 Mpz と補償全床反力モーメント鉛直成分 Mdmdz との和を無制限時補正目標床反力モーメント鉛直成分 Minz とし、これに図 7 0と同様に制限を加え.ることにより、制限時補正目標床反力モーメント鉛直成分 Mltdz を求める。制限時補正目標床反力モーメント鉛直成分 Mltdz は、コンプライアンス制御用目標床反力モーメント鉛直成分として出力される。さらに、制限時補正目標床反力モーメント鉛直成分 Mltdz から無制限時補正目標.床反力モーメント鉛直成分 Minz を減じた値、すなわち、無制限時補正目標床反力モーメント鉛直成分 Minz のうちの床反力モーメント鉛直成分許容範囲を越えた分である Maa を求める。次いで、補正用摂動モデル逆位相腕振り角 Θ ca の前回値を基に、 P D制御等を用いた逆位相腕振り角補正用摂動モデル制御則によって逆位相腕振り角補正用モデル安定化モーメント要求値 Mafdmd を求め、これを Maa から減じた値（モーメント）を逆位相腕振り角補正用摂動モデルに入力し、補正用摂動モデル逆位相腕振り角 0 caを得る。

Maa から補正用摂動モデル逆位相腕振り角 0 ca までの伝達関数は、ローパスフィル夕の伝達関数になる。

すなわち、言い換えると、逆位相腕振り角補正用モデルによる床反力モーメント鉛直成分のキャンセル（超過防止動作）を行わない場合に発生する床反力モーメント鉛直成分（無制限時床反力モーメント鉛直成分）を、床反力モーメント鉛直成分許容範囲を越えないように制限する制限手段（飽和手段）に通すことにより、コンプライアンス制御用目標床反カモ一メント鉛直成分（制限時補正目標床反カモ一メント鉛直成分 Mltdz) を得ると共に、無制限時床反力モーメント鉛直成分を、床反力モーメント鉛直成分許容範囲を越えた分を求める不感帯手段に通した値を口一パスフィル夕（すなわちハイカットフィルタ）に通すことにより補正用摂動モデル逆位相腕振り角 0 caを得る。

前記第 1および第 2参考例並びにこれを用いる実施形態においては、本願出願人が提案した特開平 5— 3 3. 7 8 4 9号公報に記載の構成に加え、床反力の並進力水平成分が床反力水平成分の許容範囲を越えないように、床反力水平成分と目標 Z M Pまわりの床反力モーメント水平成分との発生の割合が異なる 2つの運動モードの挙動、例えば、上体並進運動モ一ドの上体並進加速度と上体傾斜運動モードの上体姿勢傾斜角加速度が決定されるので、実ロボット 1を、修正目標歩容（最終的に歩容生成装置 1 0 0が出力する歩容）に収束させることができる。つまり、実口ポット 1の姿勢を安定化させることがでぎる。

また、コンプライアンス制御用目標床反力モーメント水平成分とモデル操作床反力モーメント水平成分の差がトータルの復元力になる。

また、モデル操作床反力モーメント水平成分は、 Z M P存在可能範囲を無視していかなる値でも取ることが可能であるので、非常に高い姿勢復元力を発生することができる。

また、床反力の並進力水平成分が、床反力水平成分の許容範囲を越えないので、口ポット 1のスリップを防止できる。

また、床反力モーメント鉛直成分（コンプライアンス制御用目標床反力モーメント鉛直成分）しいては実床反力モーメント鉛直成分が、床反力モーメント鉛直成分の許容範囲を越えないので、ロボット 1のスリツプをより一層防止できる。また、床反力鉛直成分が 0の時期、すなわち、両脚体 2， 2がいずれも接地していない時期では、上体並進運動モードに依存せずに上体回転運動モードに依存した姿勢復元が行われ、床と足平 2 2の間の摩擦力に依存せずに効果的に姿勢復元が行われる。

また、実床反力モーメントが過大になることを防止するので、足平 2 2の本来の接地性が低下したり、足平 2 2の底面が浮いてしまうような不具合が発生することを防止または抑制することができる。

また、 1歩分の修正歩容の終端状態を新たな初期状態とした新たな今回歩容が、定常歩容に漸近するように、今回歩容パラメ一夕が決定または変更されるので、継続的に（長期的に）安定性が保証された歩容を生成し続けることができる。

前記第 3参考例およびこれを用いる前記第 1実施形態においては、上記のごとく、元歩容と修正歩容が同時に生成され、修正歩容は、実ロボット 1の姿勢安定化のために修正されると共に、コンプライアンス制御により姿勢復元に必要な床反力モーメント（水平成分及び鉛直成分）を発生してもまだ余裕がある場合には、この余裕を用いて、可能な範囲で元歩容に収束するようにしている。このため、第 2参考例の効果に加え. 当初設定した元歩容に近い、すなわち、当初の要求通りの歩容に近い歩容を生成することができる。したがって、あらかじめ設定された移動経路がある場合には、移動経路から大きくずれることを防止できる。また，修正歩容の上体姿勢角が元歩容（当初決定した歩容）の上体姿勢角に収束することを、修正歩容の上体水平位置が元歩容（当初決定した歩容）の上体水平位置に収束することよりも優先したので、上体姿勢角が大きく変動することを抑制することができる。

前記第 1〜第 7参考例およびこれを用いる実施形態では、床反力水平成分許容範囲を設定するようにしたが、床反力水平成分とロボット全体重心水平加速度は、比例関係にあるので、前記各実施形態における床反力水平成分およびその許容範囲の代わりに、ロポット全体重心水平加速度およびその許容範囲を用いても構わない。また、ロポット全体重心水平軌道に近い挙動をする部位の水平加速度軌道に関するパラメ一夕を明示的に設定しても構わない。例えば、脚体 2， 2の質量が上体 3の質量に比べ十分に小さい場合には、上体水平加速度軌道とロボット 1の全体重心水平加速度軌道は、ほぼ同じか比例関係にあるので、床反力水平成分およびその許容範囲の代わりに上体水平加速度およびその許容範囲を用いても良い。

さらに、斜面移動時（傾斜した床面で口ポット 1を移動させるとき）の歩容生成においては、床反力水平成分許容範囲'や全体重心加速度水平成分の許容範囲の代わりに、並進床反力の床面平行成分（床面に平行な成分）、すなわち摩擦力の許容範囲、あるいは全体重心加速度の床面平行成分 (これは重力成分を除けば摩擦力に比例する) の許容範囲を設定するようにしてもよい。例えば、並進床反力の床面平行成分 (摩擦力) の許容範囲を設定する場合に関して説明（この説明は全体重心加速度の床面平行成分の許容範囲を設定する場合でも同様である）すると、該摩擦力は、床面の水平面に対する傾斜角度を 0 f (ロボット 1の進行方向に向かって前下がり斜面の場合を正とする）とすると、次式 c 7 2の関係が成立する。従って、前記実施形態と同様のアルゴリズムに歩容を生成する場合、この式 c 7 2の関係を用いて、摩擦力許容範囲を床反力水平成分許容範囲に変換することで、該床反力水平成分許容範囲を設定するようにすればよい。なお、この場合、式 c 7 2の床反力鉛直成分には, 目標床反力鉛直成分を用いればよい。摩擦力 =床反力水平成分 * cos( 0 f)—床反力鉛直成分 * sin( 0 f) …式 c 7 2 また、斜面移動時（傾斜した床面で口ポット 1を移動させるとき）の歩容生成においては、床反カモ一メント鉛直成分許容範囲の代わりに、床反力モーメントの床面法線方向成分、すなわち摩擦力モーメントの許容範囲を設定するようにしてもよい。例えば、該摩擦力モーメントは、床面の水平面に対する傾斜角度を 0 f (口ポット 1の進行方向に向かつて前下がり斜面の場合を正とする）とすると、次式 c 7 3の関係が成立する。従って、前記実施形態と同様のアルゴリズムに歩容を生成する場合、この式 c 7 3の関係を用いて、床反カモ一メント鉛直成分と—床反力モーメント水平成分から摩擦カモ一メントを算出し、この値が摩擦力モーメントの許容範囲を越えないように処理を変更すれば良い。摩擦力モーメント =床反力モーメント鉛直成分 * cos( 0 f)

+床反力モーメント水平成分 * sin( Θ f)

…式 c 7 3 また、前記第 1参考例に関する変形態様で説明した如く、床反力水平成分と目標 Z M Pまわりの床反力モーメント水平成分とを適切な値にするために、前記の実施形態では、上体傾斜モードと上体並進モードとの 2つの運動モードを用いたが、これら以外の運動モードを用いても良い, この場合、運動モードのひとつが床反力水平成分を発生しない運動モードである必要もない。いかなるモードの組み合わせであっても、床反力水平成分と目標 Z M Pまわりの床反カモ一メントの発生の割合が異なる 2つの運動モードを用いさえすれば、上記例のように、任意の床反力水平成分と目標 Z M Pまわりの床反力モーメントとを発生させることができるからである。

また、上体姿勢以外の運動モードを用いても良い。ただし、なるべく小さい変位で大きな床反力水平成分または目標 Z M Pまわりの床反カモーメントが発生できる運動モ一ドを選ぶべきである。

例えば、左右の腕を同一回転方向に振りまわす運動モード、接地していない（空中に存在する）足平の位置を摂動させる運動モードでも良いただし、遊脚軌道を摂動させる場合には、着地位置が変わらないように着地直前までには、摂動量を実質的に. 0に戻すべきである。

また、 3つ以上の運動モードを用いても良い。

また、選んだモードのうちの少なくとも 2つは、床反力水平成分と目標 Z M Pまわりの床反力モーメントとの発生比率が、互いに異なるものである必要がある。そうでないと、一般的に連立方程式の解がなくなるからである。

さらにできる限り、床反力水平成分をあまり変化させないで目標 Z M Pまわりの床反力モ一メントを十分に大きぐ変化させることができる運動モードと、目標 Z M Pまわりの床反カモ一メントをあまり変化させないで床反力水平成分を十分に大きく変化させることができる運動モードとを組み合わせることが望ましい。

言いかえると、全体重心をあまり変化させないで角運動量を十分に大きく変化させることができる運動モード、角運動量をあまり変化させないで全体重心を十分に大きく変化させることができる運動モードとを組み合わせることが望ましい。運動モードの変位が小さくなるからである。

床反力モーメント鉛直成分を適切な値にするために、前記の実施形態では、逆位相腕振りモードを用いたが、これら以外の運動モードを用いても良い。例えば、前記第 1参考例の変形態様について説明した如く、上体ョ一回転モードを用いたり、上体ョ一回転モ一ドと逆位相腕振りモ一ドとを併用しても良い。

また、床反力モーメント鉛直成分を適切な値にするために用いられる運動モードが床反力水平成分および床反カモ一メント水平成分を発生しない運動モードである必要もない。例えば、遊脚を前後に振る運動モードを用いても良い。その運動モーによって発生した床反力水平成分および床反力モーメント水平成分は、上体傾斜モードと上体並進モードとの 2つの運動モードを調整することによって、打ち消すことができるからである。

フルモデルを用いる参考例および実施形態では、前記単純化モデルとして、前記実施形態に用いた動力学モデル（図 1 2のモデル）以外にも以下のモデルを用いても良い。

1 ) 図 4 9に示したように複数のリンクに質点を設定した非線形なモデル (多質点モデル)

2 ) 本願出願人による P C T公開公報 WO/.02 40224に示された 3質点モデル

3 ) 上体にのみ質量がある 1質点モデル

4 ) 全体重心まわりの角運動量変化によって生じる慣性力のモ一メントを無視したモデル

5 ) 重力と慣性力との合力（または床反力）と上体並進運動との関係を表す部分モデルと、上記合力と上体回転運動の関係を表す部分モデルを分離して持つ分離型モデル。例えば、図 1 2に示す質点は、上記合力と上体並進運動との関係を表す部分モデルであり、図 1 2に示すフライホィールは、上記合力と上体回転運動との関係を表す部分モデルである。ただし、単純化モデルに単純化モデル上体姿勢傾斜角補正用モーメントが加えられる参考例および実施形態においては、上記 2 )、 3 )、および 4 ) のモデルを用いることはできない。

尚、フルモデルは、基本的には、単純化モデルよりもより近似精度の高い動力学モデルを用いることが好ましいが、単純化モデルと同等の近似精度の動力学モデルを用いてもよい。

また、前述した各実施形態において、ブロック線図、フローチャートおよびアルゴリズムなどは、演算処理順序を変えるなどの等価変形をしても良い。また、適宜ローパスフィルタを揷入しても良い。 ' また、前記各実施形態を 2足移動口.ポットに関して説明してきたが、 1足もしくは 3足以上の多脚ロポットにも本発明を適応することができる。産業上の利用可能性

以上のように本発明は、 2足移動ロボット等の脚式移動ロポッ卜と床面との摩擦力が小さくなるような状況でも、口ポットの滑りの発生を抑制しつつ、該ロポットの姿勢を安定に保ち、該ロポットを円滑に移動させることができるものとして有用である。

Claims

請求の範囲

1 . 上体から延設された複数の脚体を運動させて移動する脚式移動ロボッ卜の目標歩容を生成し、その目標歩容に追従するようにロポッ卜の動作を制御する制御装置において、

前記目標歩容に追従して動作している前記ロボットの滑りの発生を判断する滑り判断手段と、

前記ロボットに作用させる並進床反力の水平成分又は該並進床反力の床面平行成分又は該ロボットの全体重心加速度の水平成分又は該全体重心加速度の床面平行成分を制限対象量として、該制限対象量の許容範囲を前記滑り判断手段の判断結果に応じて可変的に設定する許容範囲設定手段と、

' 前記目標歩容の仮運動により定まる前記制限対象量が前記許容範囲を逸脱したとき、前記動力学的平衡条件を満たしつつ、該制限対象量を前記許容範囲内に制限するようにロポットの重心回りの角運動量変化率を前記仮運動から変化させることにより該仮運動を修正して目標歩容の運動を決定する仮運動修正手段とを備えたことを特徴とする脚式移動ロボッ卜の制御装置。

2 . 上体から延設された複数の脚体を運動させて移動する脚式移動ロボットの目標歩容を生成し、その目標歩容に追従するようにロポットの動作を制御する制御装置において、

前記目標歩容に追従して動作している前記ロポットの滑りの発生を判断する滑り判断手段と、 '

前記ロポットに作用させる床反力モーメントの鉛直成分又は該床反力モーメントの床面法線方向成分又は該ロボットの角運動量変化率の鉛直成分又は該角運動量変化率の床面法線方向成分を制限対象量として、該制限対象量の許容範囲を前記滑り判断手段の判断結果に応じて可変的に設定する許容範囲設定手段と、

所定の動力学モデル上で前記ロポットに作用する重力と慣性力との合力が所定の動力学的平衡条件を満たすように前記目標歩容の仮運動を決定する仮運動決定手段と、

前記目標歩容の仮運動により定まる.前記制限対象量が前記許容範囲を逸脱したとき、前記動力学的平衡条件を満たしつつ、該制限対象量を前記許容範囲内に制限するようにロボットの角運動量変化率を前記仮運動から変化させることにより該仮運動を修正して目標歩容の運動を決定する仮運動修正手段とを備えたことを特徴とする脚式移動ロボットの制御

3 . 上体から延設された脚体を運動させて移動する脚式移動ロポットの '目標運動の瞬時値を、少なくとも該ロボット.の運動と床反力との関係を表す動力学モデルを用いて逐次決定しつつ、その決定した目標運動の瞬時値に追従させるように前記ロポットの動作を制御する脚式移動ロボッ卜の制御装置において、

前記目標運動に追従して動作しているロボッ卜の滑りの発生を判断する滑り判断手段と、

少なくとも前記ロボッ卜に作用させる並進床反力の水平成分又は該並進床反力の床面平行成分又は該ロポットの全体重心加速度の水平成分又は該全体重心加速度の床面平行成分を制限対象量として、該制限対象量の許容範囲を前記滑り判断手段の判断結果に応じて可変的に設定する許容範囲設定手段と、 '

少なくとも前記決定された目標運動の瞬時値に対応するロポッ卜の姿勢の目標状態量と該ロポットの姿勢の実状態量との偏差に基づいて、前記目標運動の新たな瞬時値を、該新たな瞬時値に対応して前記動力学モデルに基づき定まる前記制限対象量が前記許容範囲内に収まると共に前記偏差が 0に近づくように決定する目標瞬時値決定手段とを備えたことを特徴とする脚式移動ロポットの制御装置。

4 . 上体から延設された脚体を運動させて移動する脚式移動ロボットの目標運動及び目標床反力の瞬時値を、少なくとも該ロボットの運動と床反力との関係を表す動力学モデルを用.いて逐次決定しつつ、その決定した目標運動及び目標床反力の瞬時値に追従させるように前記ロポットの動作を制御する脚式移動ロボットの制御装置において、

前記目標運動及目標床反力に追従して動作しているロポットの滑りの発生を判断する滑り判断手段と、

少なくとも前記ロボッ卜に作用させる並進床反力の水平成分又は該並進床反力の床面平行成分又は該ロボットの全体重心加速度の水平成分又は該全体重心加速度の床面平行成分を制限対象量として、該制限対象量の許容範囲を前記滑り判断手段の判断結果に応じて可変的に設定する許容範囲設定手段と、

少なくとも前記決定された目標運動及び目標床反力の瞬時値に対応するロポットの姿勢の目標状態量と該ロポットの姿勢の実状態量との偏差に基づいて、前記目標運動及び目標床反力の新たな瞬時値を、該新たな目標運動の瞬時値に対応して前記動力学モデルに基づき定まる前記制限対象量が前記許容範囲内に収まると共に前記偏差が 0に近づくように決定する目標瞬時値決定手段とを備えたことを特徴とする脚式移動ロポッ卜の制御装置。

5 . 前記滑り判断手段は、少なくとも接地している脚体の先端部の対地速度に基づき滑りの発生を判断することを特徴とする請求の範囲第 1項〜第 4項のいずれか 1項に記載の脚式移動ロポッ卜の制御装置。

6 . 前記滑り判断手段は、少なくとも接地している脚体に作用する実床反力の時間的変化率と該脚体の先端部の対地速度とに基づき、該脚体の見かけばね定数を求める手段を備え、少なくとも該見かけばね定数に基づき滑りの発生を判断することを特徴とする請求の範囲第 1項〜第 4項のいずれか 1項に記載の脚式移動ロポッ卜の制御装置。

7 . 前記滑り判断手段は、少なくとも接地している脚体に作用する実床反力を、所定周波数の近傍範囲に周波数通過特性を有するバンドパスフィル夕に通したものに基づき滑りの発生を判断することを特徴とする請求の範囲第 1項〜第 4項のいずれか 1項に記載の脚式移動ロボットの制御装置。