JP5404543B2 - 脚式移動ロボットの制御装置 - Google Patents

Description

本発明は、２足移動ロボット等の脚式移動ロボットの制御装置に関する。

２足移動ロボット等、基体から延設された複数の脚体の運動によって移動する脚式移動ロボット（以下、単にロボットということがある）においては、その姿勢が崩れたりしないようにするために、該ロボットの実際の姿勢と目標とする姿勢とのずれの程度を表す姿勢状態量偏差を逐次観測し、その姿勢状態量偏差を解消するように、ロボットに実際に作用させる外力のうちの所定種類の要素、例えば床反力モーメントを操作する技術が本願出願人により提案されている。

例えば本願出願人が特許文献１にて記載した技術では、ロボットの基体（上体）の実際の姿勢と目標とする姿勢との偏差が上記姿勢状態量偏差として逐次観測される。さらに、この姿勢状態量偏差を“０”に収束させるためのフィードバック操作量（制御入力）として、全床反力中心点の目標位置（目標ＺＭＰ）で実際のロボットに付加的に作用させるべき床反力モーメント（詳しくは、該床反力モーメントの水平成分）が上記姿勢状態量偏差の観測値からフィードバック制御則（ＰＤ則）により決定される。

そして、特許文献１に見られる技術では、基本的には、このように決定した床反力モーメントを、全床反力中心点の目標位置（目標ＺＭＰ）で実際のロボットに付加的に作用させるべき床反力モーメントの目標とし、実際のロボットの運動を目標運動に追従させつつ、該ロボットに目標の床反力モーメントが付加的に作用するように、コンプライアンス制御によって該ロボットの動作が制御される。

ここで、前記全床反力中心点は、より詳しく言えば、ロボットに作用する全床反力（トータルの床反力）によって、その点の周りに発生するモーメントの水平成分（水平軸周り成分）が“０”となるような該全床反力の作用点である。そして、このような全床反力中心点は、実際のロボットにおいては、所謂、支持多角形（ロボットと床面との接地面を含む最小の凸多角形）内に存在し得る点である。

そして、前記特許文献１に見られる技術では、上記全床反力中心点の目標位置（目標ＺＭＰ）、すなわち、前記姿勢状態量偏差が発生しない状態で全床反力中心点となるべき点の目標位置は、脚体の先端部の目標運動軌道により規定される支持多角形内の位置に設定されるものの、発生した姿勢状態量偏差を解消するために前記床反力モーメントをロボットに付加的に作用させることとなる状況では、実際のロボットにおける全床反力中心点は、上記目標位置からずれることとなる。そして、この場合、実際の全床反力中心点は、支持多角形内から逸脱することはできない。

従って、実際のロボットにおいて、全床反力中心点の目標位置の周りに発生可能な床反力モーメントは、実際の全床反力中心点の位置が支持多角形内に収まり得る範囲内に制限されることとなる。

このため、前記特許文献１に開示されている技術では、全床反力中心点の目標位置で発生させようとする床反力モーメントは、それによって該目標位置からずれることとなる実際の全床反力中心点の位置が、前記支持多角形内に設定した許容領域内に収まるように制限される。

この場合、該許容領域は、支持多角形内の方形状の領域とされ、この許容領域に対応して、互いに直交する２つの軸方向での床反力モーメントの成分（Ｘ軸方向成分及びＹ軸方向成分）の許容範囲が設定される。そして、前記姿勢状態量偏差の観測値からフィードバック制御則により算出した床反力モーメントの各軸方向成分毎に、該軸方向成分が該軸方向の許容範囲を逸脱している場合には、該軸方向成分が許容範囲内の値に制限される。

特許４２４６６３８号

ところで、前記支持多角形は、ロボットの複数の脚体の接地状況に応じて種々様々な形状となる。そして、特に、２つ以上の脚体が同時に接地する期間（例えば２足移動ロボットにおける両脚支持期）において、一般には、方形状、もしくはそれに近似する形状から乖離した形状となる場合が多い。

このような場合には、前記支持多角形内の全床反力中心点の許容領域を、前記特許文献１に見られる如く方形状に設定すると、該支持多角形内に収まり得る方形状の許容領域は、その面積が支持多角形に対して過小なものとなりやすい。

そして、このように全床反力中心点の許容領域の面積が支持多角形に対して過小なものとなる場合には、実際のロボットに付加的に作用させる床反力モーメントの目標が、実際に発生可能な床反力モーメントに対して過剰に制限を受けることとなる。すなわち、前記姿勢状態量偏差の観測値からフィードバック制御則により算出した床反力モーメントを、そのまま実際のロボットに付加的に作用させることができる状況であっても、目標とする床反力モーメントが、前記姿勢状態量偏差の観測値からフィードバック制御則により算出した床反力モーメントに対して制限されてしまうこととなる状況の発生頻度が高まる。

このため、特に、２つ以上の脚体が同時に接地する期間では、前記姿勢状態量偏差を解消するために実際のロボットに作用させる床反力モーメントが、前記姿勢状態量偏差を速やかに解消する上で適切な床反力モーメントに対して不足気味なものとなる状況が発生しやすい。

この場合、特許文献１に見られる如く、前記姿勢状態量偏差の観測値からフィードバック制御則により算出した床反力モーメントから、前記許容領域に基づく制限後の床反力モーメントを差し引いた余剰分の床反力モーメントの符号を反転させた床反力モーメントを、ロボットの目標運動を生成するための動力学モデルにフィードバックすることで、実際のロボットに付加的に作用させる床反力モーメントと協働して、前記姿勢状態量偏差を解消し得るように、ロボットの目標運動を調整することは可能である。ただし、このようにした場合には、実際のロボットに付加的に作用させる床反力モーメントが過剰に制限を受ける期間での動力学モデルへのフィードバック分が多くなることから、ロボットの目標運動が、前記姿勢状態量偏差が発生しない状況での適切な運動からずれてしまいやすくなる。

なお、全床反力中心点の許容領域の面積が支持多角形に対して過小なものとならないようにするために、該許容領域を支持多角形と同じ形状かもしくはそれに近い形状に設定することが考えられる。しかるに、このようにした場合には、該許容領域が、方形状と異なる形状となるので、実際のロボットにおける床反力の操作目標をどのように決定するかが問題となる。

本発明はかかる背景に鑑みてなされたものであり、脚式移動ロボットの実際の姿勢と目標とする姿勢とのずれが生じた場合に、該姿勢のずれを低減するように実際の該ロボットに作用させる床反力を操作することが過剰に制限されるのを防止し、該床反力の操作によって該姿勢のずれを低減することの効果を高めることを可能とする制御装置を提供することを目的とする。

本発明の脚式移動ロボットの制御装置は、かかる目的を達成するために、基体から延設された複数の脚体を運動させて移動する脚式移動ロボットの実際の運動を目標運動に追従させつつ、該脚式移動ロボットに実際に作用する外力の構成要素のうち、少なくとも全床反力の作用点としての全床反力中心点の位置を規定する所定種類の要素である制御対象外力要素を該制御対象外力要素に対する目標値に追従させるように該脚式移動ロボットの動作制御を行う制御装置であって、
前記脚式移動ロボットの実際の姿勢と前記目標運動により規定される目標姿勢とのずれの程度を表す姿勢状態量偏差を逐次観測し、該姿勢状態量偏差を“０”に近づける機能を有する第１のフィードバック操作量として、前記制御対象外力要素の目標値を操作するための実ロボット外力操作量を、少なくとも該姿勢状態量偏差の観測値に応じて逐次決定する実ロボット外力操作量決定手段と、
前記決定された実ロボット外力操作量をそのまま使用して前記制御対象外力要素の目標値を操作すると仮定した場合に該実ロボット外力操作量に応じて決定される前記制御対象外力要素の目標値により規定される前記全床反力中心点の位置である全床反力中心点要求位置を算出する全床反力中心点要求位置算出手段と、
前記目標運動のうちの前記脚式移動ロボットの各脚体の運動に基づいて規定される支持多角形内に、前記全床反力中心点の存在許容領域である全床反力中心点許容領域を設定する全床反力中心点許容領域設定手段と、
前記算出された全床反力中心点要求位置が、前記設定された全床反力中心点許容領域内に存在するか否かを判断し、該判断結果に応じて前記制御対象外力要素の目標値を逐次決定する制御対象外力要素目標値決定手段とを備え、
前記全床反力中心点許容領域設定手段は、少なくとも前記脚式移動ロボットの２つ以上の脚体を着床状態とする期間内において、前記支持多角形内に、方形状と異なる形状の前記全床反力中心点許容領域を設定する手段を有しており、
前記制御対象外力要素目標値決定手段は、前記判断結果が肯定的である場合には、前記制御対象外力要素の目標値により規定される前記全床反力中心点の位置が前記算出された全床反力中心点要求位置に一致するように該制御対象外力要素の目標値を決定し、前記判断結果が否定的である場合には、前記制御対象外力要素の目標値により規定される前記全床反力中心点の位置が、前記設定された全床反力中心点許容領域のうち、前記算出された全床反力中心点要求位置に最も近い位置である制限後全床反力中心点位置に一致するように該制御対象外力要素の目標値を決定することを特徴とする（第１発明）。

なお、本発明において、「全床反力中心点」は、脚式移動ロボットに作用する全床反力（トータルの床反力）の作用点のうち、該全床反力によってその点周りに発生するモーメントの水平成分（水平軸周り成分）が“０”となるような点を意味する。また、全床反力中心点の位置を規定する「制御対象外力要素」は、全床反力中心点そのものでもよいが、例えば、全反力のうちの床面上の所定の点の周りのモーメントの水平成分等、該全床反力中心点の位置と一定の関係を有する要素であってもよい。

また、該制御対象外力要素の目標値を操作するための「実ロボット外力操作量」としては、例えば、該制御対象外力要素の目標値の補正量（詳しくは、該実ロボット外力操作量が“０”である場合に前記制御対象外力要素目標値決定手段が決定する制御対象外力要素の目標値としての基準目標値からの修正量）を用いることができる。

また、前記姿勢状態量偏差としては、例えば、前記基体の実際の姿勢と目標姿勢との角度差・角速度差（詳しくは、角度差及び角速度差の両方もしくは一方）、あるいは、この角度差・角速度差に所定値を乗じた値、あるいは、前記基体の実際の重心点の位置・速度（詳しくは、位置及び速度の両方もしくは一方）と目標運動における基体の重心点の位置・速度との水平方向の偏差などを用いることができる。

また、本明細書では、「床」は、通常的な意味での床（屋内の床など）だけを意味するものではなく、屋外の地面もしくは路面をも含むものとして使用する。

上記第１発明によれば、前記全床反力中心点要求位置算出手段により算出される全床反力中心点要求位置は、前記姿勢状態量偏差を“０”に近づける機能を有する第１のフィードバック操作量として、前記実ロボット外力操作量決定手段により逐次決定される実ロボット外力操作量をそのまま使用して前記制御対象外力要素の目標値を操作すると仮定した場合に該実ロボット外力操作量に応じて決定される前記制御対象外力要素の目標値により規定される前記全床反力中心点の位置である。従って、該全床反力中心点要求位置は、全床反力中心点が床面上の任意の位置に存在し得ると仮定した場合に、前記姿勢状態量偏差を“０”に近づける上で要求される全床反力中心点の要求位置を意味する。

そして、この全床反力中心点要求位置が、前記全床反力中心点許容領域設定手段により設定された全床反反力中心点許容領域内に存在するか否かが判断され、該判断結果に応じて前記制御対象外力要素の目標値が逐次決定される。

ここで、全床反力中心点は、前記支持多角形内に存在し得る点であるので、前記全床反反力中心点許容領域は、前記支持多角形内に設定される。この場合、該支持多角形は、脚式移動ロボットの接地面（床との接触面）を含む最小の凸多角形であるので、前記脚式移動ロボットの２つ以上の脚体が着床状態となる期間においては、該支持多角形の形状は、一般には、方形状（正方形状又は長方形状）と比較的顕著に相違した形状となる。このため、このような支持多角形内に仮に、方形状の全床反力中心点許容領域を設定すると、該全床反力中心点許容領域の面積が支持多角形に比して過剰に小さくなり易い。

そこで、第１発明においては、前記全床反力中心点許容領域設定手段は、少なくとも前記脚式移動ロボットの２つ以上の脚体を着床状態とする期間内において、前記支持多角形内に、方形状と異なる形状の前記全床反力中心点許容領域を設定する。これにより、全床反力中心点許容領域を、その面積が支持多角形の面積に対して小さくなり過ぎないように設定することが可能となる。該全床反力中心点許容領域は、例えば、その面積が支持多角形の面積に一致し、又は近似し得る形状の領域に設定すればよい。

なお、全床反力中心点許容領域の形状は、脚式移動ロボットの２つ以上の脚体が着床状態となる期間の全期間において方形状と異なる形状である必要はなく、その一部の期間（支持多角形の形状が方形状に近い形状になる期間、あるいは、該支持多角形の面積を該支持多角形内に設定可能な方形状の領域で近似し得るような期間）では、該全床反力中心点許容領域を方形状の領域に設定してもよい。

そして、前記制御対象外力要素目標値決定手段は、前記判断結果が肯定的である場合（前記全床反力中心点要求位置が全床反力中心点許容領域内に存在する場合）には、前記制御対象外力要素の目標値により規定される前記全床反力中心点の位置が前記算出された全床反力中心点要求位置に一致するように該制御対象外力要素の目標値を決定する。従って、この場合には、前記全床反力中心点許容領域による制限を受けることなく、前記決定された実ロボット外力操作量による要求どおりに制御対象外力要素の目標値が決定されることとなる。

この場合、全床反力中心点許容領域を、その面積が支持多角形に対して小さくなり過ぎないような領域に設定できるので、前記姿勢状態量偏差が発生する多くの状況において、前記決定された実ロボット外力操作量による要求どおりに制御対象外力要素の目標値を決定できることとなる。換言すれば、前記姿勢状態量偏差が発生する多くの状況において、制御対象外力要素の目標値を、該姿勢状態量偏差を効果的に“０”に近づけ得る適切な目標値に決定できることとなる。

また、前記制御対象外力要素目標値決定手段は、前記判断結果が肯定的である場合（前記全床反力中心点要求位置が全床反力中心点許容領域から逸脱している場合）には、前記制御対象外力要素の目標値により規定される前記全床反力中心点の位置が、前記設定された全床反力中心点許容領域のうち、前記算出された全床反力中心点要求位置に最も近い位置である制限後全床反力中心点位置に一致するように該制御対象外力要素の目標値を決定する。従って、この場合には、決定される制御対象外力要素の目標値は、前記決定された実ロボット外力操作量による要求どおりの制御対象外力要素の目標値（全床反力中心点要求位置に対応する制御対象外力要素の目標値）に対して制限されることとなるが、その制限量ができるだけ小さくなるように、制御対象外力要素の目標値が決定されることとなる。このため、前記姿勢状態量偏差を“０”に近づける効果が極力損なわれないように、前記制御対象外力要素の目標値が決定されることとなる。

よって、第１発明によれば、脚式移動ロボットの実際の姿勢と目標とする姿勢とのずれが生じた場合に、該姿勢のずれを低減するように実際の該ロボットに作用させる床反力を操作することが過剰に制限されるのを防止し、該床反力の操作によって該姿勢のずれを低減することの効果を高めることができる。

かかる第１発明においては、前記脚式移動ロボットの動力学を表現するものとしてあらかじめ設定された動力学モデルを用い、該動力学モデルの動力学演算を含む演算処理を実行することにより前記目標運動を逐次決定する目標運動決定手段と、前記姿勢状態量偏差を“０”に近づける機能を有する第２のフィードバック操作量として、前記動力学モデル上でのロボットに作用させるべき外力であるモデル外力を操作するためのモデル外力操作量を、少なくとも前記算出された全床反力中心点要求位置と前記決定された制御対象外力要素の目標値により規定される前記全床反力中心点の位置との差に応じて変化させるように逐次決定するモデル外力操作量決定手段とをさらに備え、前記目標運動決定手段は、前記決定されたモデル外力操作量に応じて前記動力学モデル上でのロボットに作用させるべき外力であるモデル外力を決定しつつ、該モデル外力を用いて前記動力学モデルの動力学演算を含む演算処理を行なうことにより前記目標運動を決定することが好ましい（第２発明）。

かかる第２発明によれば、前記姿勢状態量偏差を“０”に近づける機能を有する第２のフィードバック操作量として前記モデル外力操作量決定手段が決定するモデル外力操作量（脚式移動ロボットの目標運動を決定するために用いる動力学モデル上でのロボットに作用させるべき外力であるモデル外力を操作するためのモデル外力操作量）は、少なくとも前記算出された全床反力中心点要求位置と前記決定された制御対象外力要素の目標値により規定される前記全床反力中心点の位置との差に応じて変化するように逐次決定される。

このため、前記決定された制御対象外力要素の目標値が、前記全床反力中心点許容領域による制限を受けた場合に、その制限による制御対象外力要素の目標値の不足分を補うように、上記モデル外力操作量を決定できることとなる。換言すれば、前記実ロボット外力操作量のうち、制御対象外力要素の目標値に反映させることができない余剰部分を前記動力学モデルに反映させるようにして、上記モデル外力操作量を決定できることとなる。

そして、第２発明によれば、このモデル外力操作量に応じて前記モデル外力を決定しつつ、該モデル外力を用いて前記動力学モデルの動力学演算を含む演算処理を行なうことにより前記目標運動が決定される。これにより、前記姿勢状態量偏差を“０”に近づけるように、前記目標運動が調整されることとなる。

よって、第２発明によれば、前記決定された制御対象外力要素の目標値が、前記全床反力中心点許容領域による制限を受けた場合であっても、その制限を補うように前記目標運動決定手段が逐次決定する目標運動が調整される。ひいては、姿勢状態量偏差を円滑に“０”に近づけることができる。

なお、かかる第２発明における上記モデル外力、モデル外力操作量としては、それぞれ、例えば、前記制御対象外力要素、実ロボット外力操作量と同じ種類のものを使用することができる。

また、前記第１発明では、次のような構成を採用してもよい。すなわち、前記脚式移動ロボットの動力学を表現するものとしてあらかじめ設定された第１動力学モデルを用い、該第１動力学モデルの動力学演算を含む演算処理を実行することにより前記目標運動を組成する第１運動を逐次決定する第１運動決定手段と、
前記脚式移動ロボットの動力学を表現するものとしてあらかじめ設定された第２動力学モデルを用い、該第２動力学モデルの動力学演算を含む演算処理を実行することにより前記目標運動を組成する第２運動を逐次決定する第２運動決定手段と、
前記決定された第１運動と第２運動とを合成することにより、前記目標運動を逐次決定する目標運動決定手段と、
前記姿勢状態量偏差を“０”に近づける機能を有する第２のフィードバック操作量として、前記第１動力学モデル及び第２動力学モデルの上でのロボットに作用させる外力を操作するためのモデル外力基本操作量を、少なくとも前記算出された全床反力中心点要求位置と前記決定された制御対象外力要素の目標値により規定される前記全床反力中心点の位置との差に応じて変化させるように逐次決定するモデル外力基本操作量決定手段と、
前記決定されたモデル外力基本操作量から前記第１動力学モデル及び第２動力学モデルのそれぞれに対して分配する操作量として、前記第１動力学モデル上でのロボットに作用させる外力を操作するための第１モデル外力操作量と、前記第２動力学モデル上でのロボットに作用させる外力を操作するための第２モデル外力操作量とを前記決定されたモデル外力基本操作量に応じて逐次決定するモデル外力操作量分配手段とをさらに備え、
少なくとも前記実ロボット外力操作量決定手段、前記全床反力中心点要求位置算出手段、前記制御対象外力要素目標値決定手段、前記第２運動決定手段、前記目標運動決定手段、及びモデル外力基本操作量決定手段の処理の実行周期と、前記モデル外力操作量分配手段の処理のうちの前記第２モデル外力操作量を決定する処理の実行周期とは、前記第１運動決定手段の処理の実行周期よりも短い周期に設定されており、
前記第１運動決定手段は、前記決定された第１モデル外力操作量に応じて前記第１動力学モデル上でのロボットに作用させるべき外力であるモデル外力を決定しつつ、該モデル外力を用いて前記第１動力学モデルの動力学演算を含む演算処理を行なうことにより前記第１運動を決定し、
前記第２運動決定手段は、前記決定された第２モデル外力操作量に応じて前記第２動力学モデル上でのロボットに作用させるべき外力であるモデル外力を決定しつつ、該モデル外力を用いて前記第２動力学モデルの動力学演算を含む演算処理を行なうことにより前記第２運動を決定し、
前記モデル外力操作量分配手段は、前記決定されたモデル外力基本操作量を組成する低周波成分と高周波成分とのうちの高周波成分よりも低周波成分に対する前記第１モデル外力操作量の変化の感度が相対的に高感度になり、且つ、前記低周波成分よりも高周波成分に対する前記第２モデル外力操作量の変化の感度が相対的に高感度になるように該第１モデル外力操作量と第２モデル外力操作量とを決定する（第３発明）。

この第３発明によれば、脚式移動ロボットの目標運動を決定するための動力学モデルとして、前記第１動力学モデルと第２動力学モデルとが使用され、前記第１動力学モデルを用いて前記第１運動決定手段が決定した第１運動と、前記第２動力学モデルを用いて前記第２運動決定手段が決定した第２運動とを合成することによって前記目標運動が決定される。

また、第３発明においては、前記姿勢状態量偏差を“０”に近づける機能を有する第２のフィードバック操作量として前記モデル外力基本操作量決定手段が決定するモデル外力基本操作量（前記第１動力学モデル及び第２動力学モデルの上でのロボットに作用させる外力を操作するためのモデル外力基本操作量）は、少なくとも前記算出された全床反力中心点要求位置と前記決定された制御対象外力要素の目標値により規定される前記全床反力中心点の位置との差に応じて変化するように逐次決定される。

このため、前記決定された制御対象外力要素の目標値が、前記全床反力中心点許容領域による制限を受けた場合に、その制限による制御対象外力要素の目標値の不足分を補うように、上記モデル外力基本操作量を決定できることとなる。換言すれば、前記実ロボット外力操作量のうち、制御対象外力要素の目標値に反映させることができない余剰部分を前記第１動力学モデル及び第２動力学モデルに反映させるようにして、上記モデル外力基本操作量を決定できることとなる。

そして、第３発明においては、このモデル外力基本操作量は、前記モデル外力操作量分配手段によって、第１運動決定手段の第１動力学モデルにフィードバックする第１モデル外力操作量と、第２運動決定手段の第２動力学モデルにフィードバックする第２モデル外力操作量とに分配される。

このとき、前記モデル外力操作量分配手段は、前記モデル基本操作量を組成する低周波成分と高周波成分とのうちの高周波成分よりも低周波成分に対する前記第１モデル外力操作量の変化の感度が相対的に高感度になり、且つ、前記低周波成分よりも高周波成分に対する前記第２モデル外力操作量の変化の感度が相対的に高感度になるように該第１モデル外力操作量と第２モデル外力操作量とを決定する。

換言すれば、第１モデル外力操作量の主成分がモデル基本操作量の低周波成分、第２モデル外力操作量の主成分がモデル基本操作量の高周波成分となるように、第１モデル外力操作量及び第２モデル外力操作量が決定される。なお、モデル基本操作量の低周波成分と高周波成分とをそれぞれ、そのまま、第１モデル外力操作量、第２モデル外力操作量として決定してもよい。

そして、前記第１運動決定手段は、前記決定された第１モデル外力操作量に応じて前記第１動力学モデル上でのロボットに作用させるべき外力であるモデル外力を決定しつつ、該モデル外力を用いて前記第１動力学モデルの動力学演算を含む演算処理を行なうことにより前記第１運動を決定する。従って、前記第１運動は、比較的緩やかな変化を呈する前記姿勢状態量偏差に対して、該偏差を“０”に近づけ得る運動となるように第１運動決定手段により逐次決定される。

この場合、第１運動決定手段に入力される第１モデル外力操作量は、モデル基本操作量の低周波成分を主成分とするものであるので、第１運動決定手段の処理の実行周期が比較的長い周期であっても、比較的緩やかな変化を呈する姿勢状態量偏差を適切に“０”に近づけ得るように、前記第１運動を逐次決定できる。

また、前記第２運動決定手段は、前記決定された第２モデル外力操作量に応じて前記第２動力学モデル上でのロボットに作用させるべき外力であるモデル外力を決定しつつ、該モデル外力を用いて前記第２動力学モデルの動力学演算を含む演算処理を行なうことにより前記第２運動を決定する。従って、前記第２運動は、比較的素早い変化を呈する前記姿勢状態量偏差に対して、該偏差を“０”に近づけ得る運動となるように第２運動決定手段により逐次決定される。

この場合、第２運動決定手段に入力される第２モデル外力操作量は、モデル外力基本操作量の高周波成分を主成分とするものであるものの、該第２運動決定手段の処理の実行周期は、第１運動決定手段の処理の実行周期に比して短い周期である。このため、比較的素早い変化を呈する姿勢状態量偏差を適切に“０”に近づけ得るように、前記第２運動の瞬時値を逐次決定できる。

そして、このように決定される第１運動と第２運動とを合成することにより、前記目標運動が逐次決定される。これにより、前記モデル外力基本操作量の低周波成分と高周波成分との両者を反映させた前記目標運動が逐次決定される。

第３発明では、このように決定される目標運動と、前記制御対象外力要素の目標値とを、追従目標として脚式移動ロボットの動作制御を行われることとなる。

かかる第３発明によれば、前記決定された制御対象外力要素の目標値が、前記全床反力中心点許容領域による制限を受けた場合に、前記第２発明と同様に、その制限を補うように前記目標運動決定手段が逐次決定する目標運動が調整される。ひいては、姿勢状態量偏差を円滑に“０”に近づけることができる。

加えて第３発明によれば、前記決定された制御対象外力要素の目標値が、前記全床反力中心点許容領域による制限を受けた場合に、姿勢状態量偏差が比較的緩やかな変化を呈するものであるか、素早い変化を呈するものであるかに依存することなく、該姿勢状態量偏差を適切に“０”に近づけ得るように、脚式移動ロボットの目標運動を逐次決定しつつ、該ロボットの動作を制御できる。

また、第１運動決定手段の第１動力学モデルにフィードバックされる第１モデル外力操作量は、モデル外力基本操作量の低周波成分を主成分とするものであるため、該第１運動決定手段の処理が比較的長い周期であっても、該第１モデル外力操作量を適切に反映させて、第１運動を逐次決定できる。このため、第１運動決定手段では、第１動力学モデルとして、動力学的な精度が高い動力学モデルを使用して、信頼性の高い第１運動を決定できる。

一方、第２運動決定手段の第２動力学モデルにフィードバックされる第２モデル外力操作量は、モデル外力基本操作量の高周波成分を主成分とするものである。この場合、上記の如く信頼性の高い第１運動を決定できることから、第２運動決定手段では、必要演算時間が比較的短い時間で済むような簡易な動力学モデルを使用できる。このため、モデル外力基本操作量の高周波成分を主成分とする前記第２モデル外力操作量を適切に反映させつつ、高速の周期で、第２運動の瞬時値を逐次決定できる。

なお、第３発明における上記モデル外力としては、例えば、前記制御対象外力要素と同じ種類のものを使用することができる。また、モデル外力基本操作量、第１モデル外力操作量、第２モデル外力操作量としては、例えば、前記実ロボット外力操作量と同じ種類のものを使用することができる。

また、前記第２発明においては、前記動力学モデルの所定種類の状態量の値と、該状態量の所定の目標値との偏差であるモデル状態量偏差を逐次算出するモデル状態量偏差算出手段をさらに備え、
前記実ロボット外力操作量決定手段は、前記姿勢状態量偏差を“０”に近づける機能と前記モデル状態量偏差を“０”に近づける機能とを有する前記第１のフィードバック操作量としての前記実ロボット外力操作量を、少なくとも前記姿勢状態量偏差の観測値に応じて決定したフィードバック操作量成分と前記モデル状態量偏差の算出値に応じて決定したフィードバック操作量成分とを合成することにより逐次決定し、
前記モデル外力操作量決定手段は、前記姿勢状態量偏差を“０”に近づける機能と前記モデル状態量偏差を“０”に近づける機能とを有する前記第２のフィードバック操作量としての前記モデル外力操作量を、少なくとも前記算出された全床反力中心点要求位置と前記決定された制御対象外力要素の目標値により規定される前記全床反力中心点の位置との差に応じて変化させるように決定したフィードバック操作量成分と、前記モデル状態量偏差の算出値に応じて決定したフィードバック操作量成分とを合成することにより逐次決定することが好ましい（第４発明）。

この第４発明によれば、例えば、前記算出された全床反力中心点要求位置と前記決定された制御対象外力要素の目標値により規定される前記全床反力中心点の位置との差が“０”となる状況、すなわち、前記全床反力中心点要求位置が前記全床反力中心点許容領域内に収まり、前記実ロボット外力操作量による要求通りに前記制御対象外力要素の目標値を決定できる状況では、前記モデル状態量偏差の算出値に応じて決定したフィードバック操作量成分が、実ロボット外力操作量と前記モデル外力操作量とに反映されることとなる。このため、前記姿勢状態量偏差に影響を及ぼさないようにしつつ、前記動力学モデルの所定種類の状態量が、好ましい目標値から乖離しないようにすることができる。この結果、第２発明における前記動力学モデルの状態が、不適切な状態に発散してしまうようなことを防止することができる。

なお、前記動力学モデルの所定種類の状態量としては、例えば、該動力学モデル上でのロボットの所定部位（例えば基体）の姿勢や重心点の位置等を用いることができる。

上記第４発明と同様の考え方によって、前記第３発明においては、前記第２動力学モデルの所定種類の状態量の値と、該状態量の所定の目標値との偏差であるモデル状態量偏差を逐次算出するモデル状態量偏差算出手段をさらに備え、前記実ロボット外力操作量決定手段は、前記姿勢状態量偏差を“０”に近づける機能と前記モデル状態量偏差を“０”に近づける機能とを有する前記第１のフィードバック操作量としての前記実ロボット外力操作量を、少なくとも前記姿勢状態量偏差の観測値に応じて決定したフィードバック操作量成分と前記モデル状態量偏差の算出値に応じて決定したフィードバック操作量成分とを合成することにより逐次決定し、前記モデル外力基本操作量決定手段は、前記姿勢状態量偏差を“０”に近づける機能と前記モデル状態量偏差を“０”に近づける機能とを有する前記第２のフィードバック操作量としての前記モデル外力基本操作量を、少なくとも前記算出された全床反力中心点要求位置と前記決定された制御対象外力要素の目標値により規定される前記全床反力中心点の位置との差に応じて変化させるように決定したフィードバック操作量成分と、前記モデル状態量偏差の算出値に応じて決定したフィードバック操作量成分とを合成することにより逐次決定することが好ましい（第５発明）。

この第５発明によれば、例えば、前記算出された全床反力中心点要求位置と前記決定された制御対象外力要素の目標値により規定される前記全床反力中心点の位置との差が“０”となる状況、すなわち、前記全床反力中心点要求位置が前記全床反力中心点許容領域内に収まり、前記実ロボット外力操作量による要求通りに前記制御対象外力要素の目標値を決定できる状況では、前記モデル状態量偏差の算出値に応じて決定したフィードバック操作量成分が、実ロボット外力操作量と、前記モデル外力基本操作量とに反映されることとなる。このため、前記姿勢状態量偏差に影響を及ぼさないようにしつつ、前記第２動力学モデルの所定種類の状態量が、好ましい目標値から乖離しないようにすることができる。この結果、第３発明における前記第２動力学モデルの状態が、不適切な状態に発散してしまうようなことを防止することができる。

なお、前記第２動力学モデルの所定種類の状態量としては、例えば、該第２動力学モデル上でのロボットの所定部位（例えば基体）の姿勢や重心点の位置等を用いることができる。

補足すると、前記第３発明又は第５発明では、前記モデル外力操作量分配手段は、前記第２モデル外力操作量を新たに決定するとき、現在時刻での前記低周波成分の値と前記第１運動決定手段が既に決定した前記第１運動に反映された前記低周波成分のうちの最新値との差分と、現在時刻での前記高周波成分の値との合成値に応じて前記第２モデル外力操作量を変化させるように該第２モデル外力操作量を決定するようにしてもよい（第６発明）。

ここで、前記第１運動決定手段は、一般には、その処理の各実行周期の所定のタイミングでサンプリングした前記第１モデル外力操作量を用いて前記第１演算処理を実行することとなる。このため、第１運動決定手段が、ある回の実行周期で前記第１モデル外力操作量をサンプリングした時点から、その次の実行周期で前記第１モデル外力操作量をサンプリングするまでの期間（すなわち、連続する２つの実行周期の間の期間）で第１モデル外力操作量が変化しても、その変化分は、第１運動決定手段が決定する第１運動に反映されないこととなる。

しかるに、上記第６発明によれば、前記モデル外力基本操作量の高周波成分に加えて、上記差分が、第２モデル外力操作量に反映されることとなる。これにより、第１運動決定手段の連続する２つの実行周期の間の期間での第１モデル外力操作量の変化分を、第２モデル外力操作量に反映させ、ひいては、前記第２運動に反映させることができる。

この結果、第６発明によれば、前記決定された制御対象外力要素の目標値が、前記全床反力中心点許容領域による制限を受けた場合に、前記姿勢状態量偏差をより一層適切に“０”に近づけ得る目標運動を決定できる。

なお、第６発明では、結果的に、現在時刻での前記低周波成分の値と前記第１運動決定手段が既に決定した前記第１運動の瞬時値に反映された前記低周波成分のうちの最新値との差分と、現在時刻での前記高周波成分の値との合成値に応じて前記第２モデル外力操作量を変化させるように、該第２モデル外力操作量を決定すればよく、必ずしも、上記差分そのものを算出する必要はない。例えば、現在時刻での前記モデル基本操作量の値から、前記第１運動決定手段が既に決定した前記第１運動の瞬時値に反映された前記低周波成分のうちの最新値を差し引くことによって、結果的に上記合成値が得られる。そして、このように得られる当該合成値をそのまま第２モデル外力操作量として決定するようにしてもよい。

本発明の実施形態における移動体としての２足移動ロボットの概略構成を示す斜視図。 図１のロボットに備えた制御ユニットのハード構成を示すブロック図。 図２に示す制御ユニットの機能的構成を示すブロック図。 図３に示す歩容生成装置で生成される目標床反力鉛直成分軌道の例を示すグラフ。 図３に示す歩容生成装置で生成される目標ＺＭＰ軌道（Ｘ軸方向成分）の例を示すグラフ。 図３に示す歩容生成装置と姿勢安定化制御演算部との処理で使用されるロボットの動力学モデルを視覚的に示す図。 図３に示す歩容生成装置が実行するメインルーチン処理を示すフローチャート。 図１に示すロボットの姿勢の発散状態を視覚的に示す図。 図７のＳ０２２のサブルーチン処理を示すフローチャート。 図１に示すロボットの足平の動きの形態と支持脚座標系とを例示する図。 図１に示すロボットの定常旋回歩容での目標床反力鉛直成分軌道を例示するグラフ。 図１に示すロボットの定常旋回歩容での目標ＺＭＰ軌道（Ｘ軸方向成分）を例示するグラフ。 図７のＳ０２４のサブルーチン処理を示すフローチャート。 図１３のＳ２０８のサブルーチン処理を示すフローチャート。 図７のＳ０２６のサブルーチン処理を示すフローチャート。 図７のＳ０２８のサブルーチン処理を示すフローチャート。 図１６のＳ７０２のサブルーチン処理を示すフローチャート。 図１６のＳ７１０の処理を説明するためのグラフ。 図７のＳ０３０の処理を説明するための図。 図７のＳ０３２のサブルーチン処理を示すフローチャート。 図３に示す摂動モデル演算部で使用する摂動動力学モデルを視覚的に示す図。 図３に示す補償全床反力モーメント分配器の処理を示すブロック図。 図２２に示す処理部１１６ｄでの処理を説明するための図。 図３に示す全モデル操作床反力モーメント分配器の処理を示すブロック図。 第２実施形態における全モデル操作床反力モーメント分配器の処理を示すブロック図。

［第１実施形態］
以下、脚式移動ロボットとして２足移動ロボットを例にとって、本発明の第１実施形態を説明する。

図１に示す如く、本実施形態の２足移動ロボット１（以下、単にロボット１という）は、基体としての上体２４と、この上体２４を床面上方で移動させる移動機構として該上体２４から延設された左右一対の脚体２，２とを備える。

上体２４は、両脚体２，２の基端部（上端部）に後述する腰関節（股関節）を介して連結されており、両脚体２，２のうちの接地した脚体によって床面の上方に支持される。

両脚体２，２は同一構造であり、それぞれ６個の関節を備える。その６個の関節は上体２４側から順に、腰（股）の回旋用（上体２４に対するヨー方向の回転用）の関節１０Ｒ，１０Ｌと、腰（股）のロール方向（Ｘ軸まわり）の回転用の関節１２Ｒ，１２Ｌと、腰（股）のピッチ方向（Ｙ軸まわり）の回転用の関節１４Ｒ，１４Ｌと、膝部のピッチ方向の回転用の関節１６Ｒ，１６Ｌと、足首部のピッチ方向の回転用の関節１８Ｒ，１８Ｌと、足首部のロール方向の回転用の関節２０Ｒ，２０Ｌとから構成される。

なお、本実施形態の説明では、符号Ｒ，Ｌはそれぞれ右側脚体、左側脚体に対応するものであることを意味する。また、Ｘ軸、Ｙ軸、Ｚ軸は、後述する支持脚座標系の３つの座標軸を意味する。この支持脚座標系のＸ軸方向、Ｙ軸方向は、水平面上で互いに直交する２軸方向であり、Ｘ軸方向はロボット１の前後方向（ロール軸方向）、Ｙ軸方向はロボット１の左右方向（ピッチ軸方向）に相当する。また、Ｚ軸方向は鉛直方向（重力方向）であり、ロボット１の上下方向（ヨー軸方向）に相当する。

各脚体２の関節１０Ｒ（Ｌ），１２Ｒ（Ｌ），１４Ｒ（Ｌ）によって３自由度の腰関節（股関節）が構成され、関節１６Ｒ（Ｌ）によって１自由度の膝関節が構成され、関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）によって２自由度の足首関節が構成されている。

そして、腰関節（股関節）１０Ｒ（Ｌ），１２Ｒ（Ｌ），１４Ｒ（Ｌ）と膝関節１６Ｒ（Ｌ）とは大腿リンク３２Ｒ（Ｌ）で連結され、膝関節１６Ｒ（Ｌ）と足首関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）とは下腿リンク３４Ｒ（Ｌ）で連結されている。また、各脚体２の足首関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）の下部に、各脚体２の先端部（下端部）を構成する足平２２Ｒ（Ｌ）が取着されている。また、各脚体２の上端部（基端部）が、腰関節（股関節）１０Ｒ（Ｌ），１２Ｒ（Ｌ），１４Ｒ（Ｌ）を介して上体２４に連結されている。

上記した各関節は、例えば本願出願人が特開平３−１８４７８２号などにて提案した公知の構造のものでよい。この場合、各関節を回転駆動するアクチュエータは、減速機を備える電動モータ４２（図２参照）により構成される。

各脚体２の上記構成により、各脚体２の足平２２Ｒ（Ｌ）は、上体２４に対して６自由度を有する。そして、ロボット１の移動に際して両脚体２，２を合わせて６＊２＝１２個（この明細書で「＊」はスカラに対する演算においては乗算を示し、ベクトルに対する演算においては外積を示す）の関節をそれぞれ適宜な角度に駆動することで、両足平２２Ｒ，２２Ｌの所望の運動を行なうことができる。これにより、ロボット１は歩行動作や走行動作等、３次元空間を移動する運動を行うことが可能となっている。

なお、図示は省略するが、本実施形態では、上体２４の上部の両側部には左右一対の腕体が取り付けられると共に、上体２４の上端部には頭部が搭載される。そして、各腕体は、それに備える複数の関節（肩関節、肘関節、手首関節など）によって、該腕体を上体２４に対して前後に振る等の運動を行なうことが可能となっている。ただし、これらの腕体および頭部は無くてもよい。

上体２４の内部にはロボット１の動作制御を行う制御ユニット２６が格納されている。なお、図１では図示の便宜上、制御ユニット２６を上体２４の外部に記載している。

各脚体２の足首関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）と足平２２Ｒ（Ｌ）との間には６軸力センサ３６が介装されている。この６軸力センサ３６は、床から足平２２Ｒ（Ｌ）を介して各脚体２に伝達される床反力の３軸方向の並進力成分及び３軸まわりのモーメント成分を検出し、その検出信号を制御ユニット２６に出力する。

上体２４には、鉛直方向（重力方向）に対する上体２４の傾斜角（ロール方向及びピッチ方向の傾斜角）とその変化速度（角速度）とを計測するための傾斜センサ４０が搭載されている。この傾斜センサ４０は、より詳しくは、加速度センサとジャイロセンサ等のレートセンサ（角速度センサ）とから構成され、これらのセンサの検出信号を制御ユニット２６に出力する。そして、制御ユニット２６において、傾斜センサ４０の出力を基に、公知の手法によって鉛直方向に対する上体２４の傾斜角と角速度とが計測される。

また、各関節を回転駆動する電動モータ４２（図２参照）には、各関節の回転角を検出するためのエンコーダ（ロータリエンコーダ）４４（図２参照）が付設され、該エンコーダ４４の検出信号が制御ユニット２６に出力される。

図２を参照して、制御ユニット２６はマイクロコンピュータを有する電子回路ユニットにより構成されており、ＣＰＵからなる第１演算装置５０及び第２演算装置５２、Ａ／Ｄ変換器５４、カウンタ５６、Ｄ／Ａ変換器５８、ＲＡＭ６０、ＲＯＭ６２、並びにこれらの間のデータ授受を行なうバスライン６４を備えている。

この制御ユニット２６では、前記６軸力センサ３６及び傾斜センサ４０の出力はＡ／Ｄ変換器５４でデジタル値に変換された後、バスライン６４を介してＲＡＭ６０に入力される。また、ロボット１の各関節のエンコーダ（ロータリエンコーダ）４４の出力はカウンタ５６を介してＲＡＭ６０に入力される。

前記第１の演算装置５０は、後述する目標歩容を生成すると共に、関節変位指令（各関節の回転角の目標値）を算出し、ＲＡＭ６０に送出する。また、第２の演算装置５２はＲＡＭ６０から関節変位指令と、前記エンコーダ４４の出力からカウンタ５６を介して計測された実関節変位（各関節の回転角の実測値）とを読み出し、該実関節変位を関節変位指令に追従させるために必要な各関節の電動モータ４２の駆動指令（電動モータ４２の出力トルクを規定する指令値）を算出する。

そして、第２の演算装置５２は、その算出した駆動指令をＤ／Ａ変換器５８を介して電動モータ４２の駆動用のサーボアンプ４６に出力する。このとき、サーボアンプ４６は、入力された駆動指令に応じて電動モータ４２を駆動する（電動モータ４２に通電する）。これにより、各関節の実関節変位が関節変位指令に追従するように制御される。

次に、図３を参照して、本実施形態におけるロボット１の制御装置の動作の概要を説明する。図３中の「実ロボット」以外の部分が制御ユニット２６が実行する処理（主として第１の演算装置６０及び第２の演算装置６２の処理）によって実現される機能である。

なお、図３では、便宜上、ロボット１に搭載された前記の各センサの出力から制御ユニット２６が認識する実測値（実関節変位など）が実ロボット１から出力されるものとして示している。また、以下の説明では、脚体２の左右を特に区別する必要がないときは、前記符号Ｒ，Ｌを省略する。

制御ユニット２６は、ロボット１の動作（歩容）の目標である目標歩容を、該ロボット１の動力学（ロボット１の運動とロボット１に作用する外力との関係）を表現する動力学モデルを用いて生成して出力する歩容生成装置１００を備える。この歩容生成装置１００が生成して出力する目標歩容は、ロボット１の運動に関する目標と、ロボット１に作用する外力に関する目標とから構成される。

本実施形態では、該目標歩容は、上体２４の目標位置及び目標姿勢の軌道である目標上***置姿勢軌道と、各足平２２の目標位置及び目標姿勢の軌道である目標足平位置姿勢軌道と、各腕体の目標姿勢の軌道である目標腕姿勢軌道と、ロボット１のＺＭＰ（Zero Morment Point）の目標位置の軌道である目標ＺＭＰ軌道と、ロボット１に作用する外力としての全床反力の目標の軌道である目標全床反力軌道とから構成される。これらの構成要素のうち、目標上***置姿勢軌道、目標足平位置姿勢軌道及び目標腕姿勢軌道がロボット１の運動に関する目標に相当し、目標ＺＭＰ軌道及び目標全床反力軌道が、外力に関する目標に相当する。なお、脚体２や腕体以外に、上体２４に対して可動な部位を備える場合には、その可動部位の目標位置姿勢軌道がロボット１の運動に関する目標の一部として目標歩容に加えられる。

補足すると、本実施形態では、歩容生成装置１００が生成して出力する目標歩容のうちの目標上***置姿勢軌道は、後述する如く適宜修正され、その修正後の目標上***置姿勢軌道が上体２４の実際の位置及び姿勢の追従目標として使用される。

ここで、上記目標歩容における「軌道」は時間的変化のパターン（時系列パターン）を意味し、歩容生成装置１００の制御周期（演算処理周期）毎に算出される瞬時値の時系列により構成される。以下の説明では、「軌道」の代わりに「パターン」と称することもある。また、以下の説明では、誤解を生じるおそれがない場合には、しばしば「目標」を省略する。

上体２４の位置及び速度は、上体２４のあらかじめ定めた代表点（例えば左右の股関節の間の中央点等）の位置及びその移動速度を意味する。同様に、各足平２２の位置及び速度は、各足平２２のあらかじめ定めた代表点の位置及びその移動速度を意味する。各足平２２の代表点は、本実施形態では、各足平２２の底面上の点、例えば、各脚体２の足首関節の中心から各足平２２の底面への垂線が該底面と交わる点に設定される。

また、「姿勢」は空間的な向きを意味する。例えば上体姿勢は鉛直方向に対するロール方向（Ｘ軸まわり）の上体２４の傾斜角（姿勢角）とピッチ方向（Ｙ軸まわり）の上体２４の傾斜角（姿勢角）とで表され、足平姿勢は各足平２２に固定的に設定された２軸の空間的な方位角で表される。本明細書では、上体姿勢は上体姿勢角ということもある。なお、上体姿勢にヨー方向（Ｚ軸まわり）の上体２４の回転角を含めてもよい。

また、歩容のうちの、ロボット１に作用する外力に係わる要素（目標ＺＭＰおよび目標全床反力）以外の構成要素、すなわち足平位置姿勢、上***置姿勢等、ロボット１の各部の運動に係わる歩容を総称的に「運動」という。

また、各足平２２に作用する床反力（並進力及びモーメントからなる床反力）を「各足平床反力」と呼び、ロボット１の全て（２つ）の足平２２Ｒ，２２Ｌについての「各足平床反力」の合力を「全床反力」という。ただし、以下の説明においては、各足平床反力はほとんど言及しないので、特にことわらない限り、「床反力」は「全床反力」と同義として扱う。

ＺＭＰは、ロボット１の運動によって発生する慣性力とロボット１に作用する重力との合力がその点まわりに作用するモーメントの水平成分（水平軸まわりのモーメント成分）が零になる床面上の点を意味する。動力学的平衡条件を満足する歩容では、ＺＭＰと床反力中心点（全床反力中心点）とは一致する。この場合、目標ＺＭＰを与えるということは、全床反力中心点の目標位置を与えるということと同じである。

目標床反力は、一般的には、作用点とその点に作用する並進力及びモーメントとにより表現される。作用点はどこに設定してもよいが、本実施形態では、目標ＺＭＰを目標床反力の作用点とする。動力学的平衡条件を満足する歩容では、上記の如くＺＭＰと床反力中心点とは一致するので、目標ＺＭＰを作用点とする目標床反力のモーメント成分は、鉛直成分（Ｚ軸まわりのモーメント成分）を除いて零になる。

歩容生成装置１００により生成される目標歩容の運動のうち、目標腕姿勢軌道は、ロボット幾何学モデル（キネマティクス演算部）１０２に入力される。

また、目標上***置姿勢軌道は、上***置姿勢修正部１２２に入力される。そして、この上***置姿勢修正部１２２は、入力された目標上***置姿勢を、摂動モデル演算部１２０から与えられる値によって修正し、その修正後の目標上***置姿勢（以下、修正目標上***置姿勢という）をロボット幾何学モデル１０２に入力する。

ここで、本実施形態では、上***置姿勢修正部１２２での修正対象は、より詳しくは、目標上***置姿勢のうちの上体水平位置（水平方向での上***置）であり、摂動モデル演算部１２０から上***置姿勢修正部１２２に与えられる値は、目標上体水平位置の修正量としての上体水平位置摂動量である。そして、上***置姿勢修正部１２２は、歩容生成装置１００から入力される目標上***置姿勢のうちの上体水平位置に、上記上体水平位置摂動量を合成する（加え合わせる）ことによって、修正目標上***置姿勢の上体水平位置を決定する。また、上***置姿勢修正部１２２は、歩容生成装置１００から入力される目標上***置姿勢のうちの上体水平位置以外の要素（上体鉛直位置及び上体姿勢）をそのまま、修正目標上***置姿勢の当該要素として決定する。従って、本実施形態では、修正目標上***置姿勢は、その上体水平位置だけを、歩容生成装置１００が出力する目標上***置姿勢から上体水平位置摂動量だけ修正してなるものとして決定される。そして、上***置姿勢修正部１２２は、このように決定した修正目標上***置姿勢をロボット幾何学モデル１０２に入力する。

また、目標足平位置姿勢軌道、目標ＺＭＰ軌道（目標床反力中心点軌道）、および目標全床反力軌道（詳細には目標並進床反力鉛直成分軌道、目標並進床反力水平成分軌道、目標ＺＭＰまわりの目標床反力モーメント軌道）は、複合コンプライアンス動作決定部１０４と目標床反力分配器１０６とに入力される。

そして、目標床反力分配器１０６にて、目標床反力は各足平２２Ｒ，２２Ｌに分配され、目標各足平床反力中心点（各足平２２Ｒ，２２Ｌの床反力中心点の目標位置）と、目標各足平床反力（各足平２２Ｒ，２２Ｌの床反力中心点に作用させる目標床反力）とが決定される。この決定された目標各足平床反力中心点及び目標各足平床反力の軌道は複合コンプライアンス動作決定部１０４に入力される。なお、歩容生成装置１００から出力する目標床反力は、複合コンプライアンス動作決定部１０４によるコンプライアンス制御に必要な成分だけを出力しても良い。例えば、目標並進床反力水平成分を歩容生成装置１００から出力することを省略してもよい。

複合コンプライアンス動作決定部１０４では、目標足平位置姿勢を修正してなる機構変形補償付き修正目標足平位置姿勢が求められ、この修正目標足平位置姿勢の軌道がロボット幾何学モデル１０２に入力される。

ロボット幾何学モデル１０２は、入力された目標上***置姿勢と機構変形補償付き修正目標足平位置姿勢とを満足する両脚体２，２の各関節の関節変位指令を、ロボット１のキネマティクスモデル（剛体リンクモデル）に基づく逆キネマティクス演算によって算出し、その算出した関節変位指令を関節変位コントローラ１０８に出力する。さらに、ロボット幾何学モデル１０２は、目標腕姿勢を満足する各腕体の各関節の関節変位指令を算出し、その算出した関節変位指令を関節変位コントローラ１０８に出力する。

そして、関節変位コントローラ１０８は、ロボット幾何学モデル１０２で算出された関節変位指令を目標値として、ロボット１の両脚体２，２及び両腕体の各関節の回転角（実関節変位）を前記サーボアンプ４６を介して追従制御する。より詳しくは、関節変位コントローラ１０８は、エンコーダ４４の出力から計測される実関節変位（実測値）を関節変位指令に一致させるように、アクチュエータ駆動力としての電動モータ４２の出力トルクを調整する。

上記のような追従制御によるロボット１の実際の運動によって該ロボット１の各足平２２に実際に作用する床反力である実各足平床反力が前記６軸力センサ３６の出力から計測され、その実各足平床反力の実測値が前記複合コンプライアンス動作決定部１０４に入力される。

また、ロボット１の上体２４の実際の姿勢角（鉛直方向に対する傾斜角）である実上体姿勢角が前記傾斜センサ４０の出力から計測され、その実上体姿勢角の実測値がフィードバック操作量演算部１１０に入力される。さらに、フィードバック操作量演算部１１０には、前記修正目標上***置姿勢のうちの修正目標上体姿勢角と、摂動モデル演算部１２０における後述する摂動動力学モデルの状態量（以下、摂動モデル状態量ということがある）とが入力される。

詳細は後述するが、フィードバック操作量演算部１１０は、入力された実上体姿勢角（実測値）と修正目標上体姿勢角との偏差である上体姿勢角偏差と、入力された摂動モデル状態量と該状態量に対する所定の目標値との偏差であるモデル状態量偏差とを“０”に近づける機能を有するフィードバック操作量として、全モデル操作床反力モーメントとコンプライアンス制御用目標床反力モーメントとを決定して出力する。

ここで、上記コンプライアンス制御用目標床反力モーメントは、上体姿勢角偏差とモデル状態量偏差とを“０”に近づける（収束させる）ために、実ロボット１に付加的に作用させる目標ＺＭＰまわりの摂動床反力モーメントである。また、モデル操作用床反力モーメントは、コンプライアンス制御用目標床反力モーメントと同じ目的のために、歩容生成装置１００で目標歩容を生成するために用いる後述の動力学モデルと、前記摂動モデル演算部１２０で上体水平位置摂動量を算出するために用いる後述の摂動動力学モデルとにおけるモデル上のロボットにそれぞれ付加的に作用させる目標ＺＭＰまわりの摂動床反力モーメントの総和である。

言い換えれば、上体姿勢角偏差とモデル状態量偏差とを“０”に近づけるためのトータルのフィードバック操作量のうち、実ロボット１にフィードバックする操作量成分（実ロボット１の負担分）がコンプライアンス制御用目標床反力モーメント、歩容生成装置１００で用いる動力学モデルと前記摂動モデル演算部１２０で用いる摂動動力学モデルとの全体に対してフィードバックするトータルの操作量成分（当該動力学モデルの全体の負担分）が、全モデル操作床反力モーメントである。

フィードバック操作量演算部１１０で決定されたフィードバック操作量のうち、全モデル操作床反力モーメントは、全モデル操作床反力モーメント分配器１１８に入力される。

詳細は後述するが、全モデル操作床反力モーメント分配器１１８は、入力された全モデル操作用床反力モーメントから、歩容生成装置１００にフィードバックする成分としての第１モデル操作床反力モーメントと、摂動モデル演算部１２０にフィードバックする成分としての第２モデル操作床反力モーメントとを決定し、それぞれの成分を歩容生成装置１００、摂動モデル演算部１２０に入力する。

上記第１モデル操作床反力モーメントは、全モデル操作床反力モーメントのうちの、歩容生成装置１００の動力学モデルに対する分配分である。また、上記第２モデル操作床反力モーメントは、全モデル操作床反力モーメントのうちの、摂動モデル演算部１２０の摂動動力学モデルに対する分配分である。この場合、基本的には、全モデル操作床反力モーメントは低周波成分と高周波成分とに分離され、これらの成分のうちの低周波成分が第１モデル操作床反力モーメント、高周波成分が第２モデル操作床反力モーメントとして決定される。従って、第１モデル操作床反力モーメントは、低周波域での上体姿勢角偏差とモデル状態量偏差とを“０”に近づけるためのフィードバック操作量としての機能を持ち、第２モデル操作床反力モーメントは、高周波域での上体姿勢角偏差とモデル状態量偏差とを“０”に近づけるためのフィードバック操作量としての機能を持つ。

そして、歩容生成装置１００は、該歩容生成装置１００において決定される目標ＺＭＰまわりの床反力モーメントの水平成分（詳しくは、歩容生成装置１００で用いる動力学モデル上でのロボットにおいて、目標ＺＭＰまわりに発生する床反力モーメントの水平成分）が、入力された第１モデル操作床反力モーメントに一致するように動力学モデルを用いて目標歩容の運動（目標運動）を逐次生成する。なお、歩容生成装置１００から複合コンプライアンス動作決定部１０４に対して出力される目標ＺＭＰは、全床反力中心点の目標位置としての意味を持つものであるので、該目標ＺＭＰまわりの床反力モーメントの水平成分が“０”になることを目標とするものとして出力される。

また、摂動モデル演算部１２０は、摂動動力学モデル上でのロボットに付加的に作用する床反力モーメント（目標歩容の目標ＺＭＰまわりに付加する摂動床反力モーメント）の水平成分が入力された第２モデル操作床反力モーメントに一致するように前記上体水平位置摂動量を算出し、この上体水平位置摂動量を前記上***置姿勢修正部１２２に出力する。

また、フィードバック操作量演算部１１０で決定されたフィードバック操作量のうち、コンプライアンス制御用目標床反力モーメントは、複合コンプライアンス動作決定部１０４に入力される。そして、複合コンプライアンス動作決定部１０４は、歩容生成装置１００が生成する目標歩容の運動（上***置姿勢については、修正目標上***置姿勢）にロボット１の実際の運動を追従させつつ、目標ＺＭＰまわりの実床反力モーメントがコンプライアンス制御用目標床反力モーメントに近づくように、目標足平位置姿勢を修正することにより、機構変形補償付き修正目標足平位置姿勢（軌道）を決定する。

この場合、ロボット１の足平位置姿勢及び床反力の全ての状態を目標に一致させることは事実上不可能であるので、これらの間にトレードオフ関係を与えて妥協的になるべく一致させる。すなわち、複合コンプライアンス動作決定部１０４は、各目標に対する制御偏差に重みを与えて、制御偏差（あるいは制御偏差の２乗）の重み付き平均が最小になるように機構変形補償付き修正目標足平位置姿勢（軌道）を決定する。

換言すれば、目標ＺＭＰまわりの実床反力モーメントと、ロボット１の実際の足平位置姿勢とがそれぞれコンプライアンス制御用目標床反力モーメント、目標足平位置姿勢にできるだけ近づくように機構変形補償付き修正目標足平位置姿勢（軌道）が決定される。そして、複合コンプライアンス動作決定部１０４は、この修正目標足平位置姿勢をロボット幾何学モデル１０２に足平位置姿勢の最終的な目標値として出力することで、ロボット１の動作を制御する。

なお、上記した複合コンプライアンス動作決定部１０４などの構成および動作は、本出願人が先に出願した特開平１０−２７７９６９号公報などに詳細に記載されている。従って、複合コンプライアンス動作決定部１０４に関する本明細書での説明は以上に留める。

以上が、本実施形態における制御ユニット２６の処理の概略である。なお、本実施形態では、図３に示した制御ユニット２６の処理機能のうち、歩容生成装置１００を除く機能部分の演算処理は、比較的短めの制御周期（例えば１ｍｓ）で高速に実行される。これに対して、歩容生成装置１００の演算処理は、他の機能部分の演算処理の制御周期よりも長めの制御周期（例えば１０ｍｓ）で実行される。

次に、前記歩容生成装置１００、フィードバック操作量演算部１１０、全モデル操作床反力モーメント分配器１１８、摂動モデル演算部１２０の処理の詳細を説明する。

まず、歩容生成装置１００が生成する目標歩容の基本的な概要を、歩行歩容（ロボット１の歩行動作を行なわせる歩容）を主要例として説明する。

以降の説明では、「床反力鉛直成分」、「床反力水平成分」は、それぞれ、特に断らない限り、「並進床反力鉛直成分」、「並進床反力水平成分」を意味するものとする。

また、歩容における「両脚支持期」とは、ロボット１がその自重を両脚体２，２で支持する期間、「片脚支持期」とはいずれか一方のみの脚体２でロボット１の自重を支持する期間を言う。本実施形態で目標歩容の主要例とする歩行歩容では、両脚支持期と片脚支持期とが交互に繰り返される。

また、片脚支持期においてロボット１の自重を支持する側の脚体２を「支持脚」、自重を支持しない側の脚体２を「遊脚」と呼ぶ。さらに、支持脚となる脚体２の足平２２を支持脚足平２２、遊脚となる脚体２の足平２２を遊脚足平２２という。なお、本実施形態の説明では、便宜上、片脚支持期における「支持脚」となる脚体２、「遊脚」となる脚体２をそれぞれ、該片脚支持期の直前の両脚支持期においても、支持脚、遊脚と呼ぶ。

歩容生成装置１００が目標歩容を生成するとき、ロボット１の運動形態に関する基本的な要求を表す要求パラメータが、ロボット１の外部の図示しない操縦装置やサーバ等から無線通信等により歩容生成装置１００に入力される。該要求パラメータは、例えば、ロボット１の運動種別（歩行、走行等）や、遊脚足平２２の目標着地位置姿勢（該遊脚足平２２の着地時の目標位置姿勢）、目標着地時刻を決定するための基本的な指針となるパラメータ（例えばロボット１の平均移動速度や、移動方向もしくは移動経路等）により構成される。なお、このような要求パラメータは、ロボット１の図示しない記憶装置に事前に記憶保持しておき、それを歩容生成装置１００が所定のスケジュールで読み込むようにしてもよい。

また、歩容生成装置１００には床形状情報も入力される。この床形状情報は、例えば、ロボット１の頭部等に搭載される撮像カメラ（図示省略）の撮像画像に基づいて制御ユニット２６で生成される。なお、上記床形状情報は、例えば、ロボット１の外部のサーバー等から、随時、ロボット１との無線通信によって制御ユニット２６に入力するようにしてもよい。あるいは、ロボット１に自己位置認識機能（グローバル座標系でのロボット１の自己位置及び向きを認識する機能）を備えておくと共に、ロボット１の移動環境の床形状情報を含む地図情報をロボット１の記憶装置に事前に記憶しておくようにしてもよい。そして、ロボット１の自己位置及び向きとから認識されるロボット１の周辺の床形状情報を地図情報から取得し、それを歩容生成装置１００に入力するようにしてもよい。また、床形状情報には、床の摩擦係数や硬さ等の性状情報が含まれていてもよい。

歩容生成装置１００は、上記要求パラメータや床形状情報を用いて所定のアルゴリズムにより目標歩容を生成する。より詳しく言えば、本実施形態では、歩容生成装置１００は、上記要求パラメータや床形状情報に応じて、目標歩容の目標足平位置姿勢軌道、目標床反力鉛直成分軌道、目標ＺＭＰ軌道等、目標歩容の一部の構成要素を規定するパラメータとしての歩容パラメータを決定した上で、その歩容パラメータとロボット１の動力学モデルとを用いて目標歩容の瞬時値を逐次決定する。これにより歩容生成装置１００は、目標歩容の時系列パターン（軌道）を生成する。

この場合、目標足平位置姿勢軌道は、例えば本願出願人が特許第３２３３４５０号にて提案した有限時間整定フィルタを用いて各足平２２毎に生成される。この有限時間整定フィルタは、可変時定数の１次遅れフィルタ、すなわち、伝達関数が１／（１＋τｓ）の形で表されるフィルタ（τは可変の時定数。以下、このフィルタを単位フィルタという）を複数段（本実施形態では３段以上）、直列に接続したものであり、所望の指定時刻に指定値に到達するような軌道を生成・出力することができるものである。

この場合、各段の単位フィルタの時定数τは、いずれも、有限時間整定フィルタの出力生成を開始してから、上記指定時刻までの残時間に応じて逐次可変的に設定される。より詳しくは、該残時間が短くなるに伴いτの値が所定の初期値（＞０）から減少されていき、最終的には、該残時間が“０”になる指定時刻にて、τの値が“０”になるように設定される。そして、有限時間整定フィルタには、前記指定値（より詳しくは、有限時間整定フィルタの出力の初期値から前記指定値への変化量）に応じた高さのステップ入力が与えられる。

このような有限時間整定フィルタは、指定時刻にて指定値に達するような出力が生成されるだけでなく、指定時刻における有限時間整定フィルタの出力の変化速度を“０”もしくはほぼ“０”にすることができる。特に、単位フィルタを３段以上（３段でよい）、接続した場合には、有限時間整定フィルタの出力の変化加速度（変化速度の微分値）をも“０”もしくはほぼ“０”にすることができる。

このような有限時間整定フィルタを用いる足平位置姿勢軌道（足平２２が着地してから次に着地するまでの位置姿勢軌道）の生成は、例えば次のように行なわれる。例えばＸ軸方向（前後方向）の目標足平位置軌道は次のように生成される。

すなわち、各足平２２の次の目標着地位置のＸ軸方向位置（より詳しくは、次の目標着地位置のひとつ前の着地位置に対するＸ軸方向の変化量（移動量）。これは前記指定値に相当する）に応じて有限時間整定フィルタへのステップ入力の高さが決定されると共に前記時定数τが所定の初期値に初期化された後、その決定されたステップ入力が有限時間整定フィルタに与えられ、足平２２のＸ軸方向位置の軌道生成が開始される。そして、この軌道生成時には、前記時定数τは、足平２２の目標着地時刻（これは前記指定時刻に相当する）までに初期値から“０”まで減少していくように、逐次可変設定される。これにより、目標着地時刻で目標着地位置に達するような、足平２２のＸ軸方向の位置の軌道が生成される。

また、Ｚ軸方向（鉛直方向）の目標足平位置軌道は、例えば次のように生成される。すなわち、まず、足平２２の次の目標着地位置及び目標着地時刻、並びに床形状情報（詳しくは足平２２の現在の着地位置と次の目標着地位置とを通る縦断面上での床形状情報）に応じて、該足平２２の高さ（鉛直位置）が最大になるときの該足平２２のＺ軸方向位置（以下、最高点位置という）とその最高点位置への到達時刻とが決定される。

そして、その最高点位置（これは前記指定値に相当する）に応じて有限時間整定フィルタへのステップ入力の高さが決定されると共に時定数τが初期化された後、その決定されたステップ入力が有限時間整定フィルタに与えられ、前記最高点位置までのＺ軸方向の足平位置軌道が逐次生成される。この際、時定数τは、最高点位置への到達時刻（前記指定時刻に相当）までに初期値から“０”まで減少するように逐次可変設定される。

さらに、最高点位置までのＺ軸方向位置の軌道の生成が終了したら、時定数τを初期化すると共にいままでのステップ入力と逆極性のステップ入力（より詳しくは、最高点位置から次の着地予定位置までのＺ軸方向の変化量（これは前記指定値に相当する）に応じた高さの逆極性のステップ入力）が有限時間整定フィルタに入力され、該最高点位置から目標着地位置までのＺ軸方向の足平位置の軌道が逐次生成される。この際、時定数τは足平２２の目標着地時刻までに初期値から“０”まで減少するように逐次可変設定される。

尚、Ｚ軸方向の足平位置軌道の生成においては、時定数τを軌道生成開始時刻から足平２２の目標着地時刻まで、初期値から“０”まで継続的に減少するように可変設定すると共に、最高点位置への到達時刻もしくはその近傍時刻で、ステップ入力の極性を逆極性に切り替えることで、Ｚ軸方向の足平位置軌道を生成するようにしてもよい。この場合には、足平２２を所望の最高点位置に精度よく到達させることはできないが、目標着地時刻での目標着地位置への到達は問題なく行なうことができる。

足平姿勢軌道についても、上述した足平位置軌道と同様に有限時間整定フィルタを用いて生成することができる。この場合、足平姿勢の空間的な各成分のうち、その姿勢の角度変化が単調的（単調増加もしくは単調減少）なものとなる成分については、前記したＸ軸方向の足平位置軌道の生成と同様に足平姿勢軌道を生成すればよい。また、姿勢の角度変化が極大値もしくは極小値をもつような成分については、前記したＺ軸方向の足平位置軌道の生成と同様に足平姿勢軌道を生成すればよい。

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

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

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

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

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

補足すると、本実施形態で主に説明する歩容である歩行歩容で生成される目標足平位置姿勢軌道においては、片脚支持期から両脚支持期への移行時に、遊脚側の足平２２がその踵で床面に着地する。次いで、その着地した足平２２は、そのつま先が床面に近づくように回転され、該足平２２の底面のほぼ全面が床面に接触する。次いで、該足平２２は、その踵が浮き上がると共につま先のみが接地するように回転された後、離床する。

また、歩容生成装置１００は目標床反力鉛直成分軌道を設定する。ロボット１の歩行歩容における目標床反力鉛直成分軌道は、例えば図４に示す如く設定される。この例では、目標床反力鉛直成分軌道は、両脚支持期では、床反力鉛直成分の増加側に凸（上に凸）の台形状に設定され、片脚支持期では床反力鉛直成分の減少側に凸（下に凸）の台形状に設定される。そして、各台形部分での床反力鉛直成分の値、折れ点の時刻等が目標床反力鉛直成分軌道を規定する歩容パラメータ（床反力鉛直成分軌道パラメータ）として決定される。

なお、ロボット１の歩行歩容における床反力鉛直成分軌道は、床反力鉛直成分の値がほぼ一定に保たれるような軌道であってもよい。

目標ＺＭＰ軌道は、所謂、支持多角形（ロボット１の接地面（床との接触面）を含む最小の凸多角形）内に目標ＺＭＰが存在すると共に、安定余裕が高く、かつ急激な変化をしないように決定される。なお、支持多角形の中央付近に目標ＺＭＰが存在する状態を安定余裕が高いと言う（詳細は特開平１０−８６０８１号公報を参照）。

例えば、歩行歩容における目標ＺＭＰ軌道は、図５のグラフで例示する如く設定される。なお、図５に例示する目標ＺＭＰ軌道は、詳しくは、Ｘ軸方向における目標ＺＭＰ軌道（目標ＺＭＰのＸ軸方向の位置の軌道）の例である。

この例では、目標ＺＭＰのＸ軸方向の位置は、両脚支持期においては、直前の片脚支持期で着地していた脚体２の足平２２のつま先寄りの位置から、新たに着地させた脚体２の足平２２の踵寄りの位置に移動するように設定される。そして、当該新たに着地させた脚体２が支持脚となる片脚支持期において、目標ＺＭＰのＸ軸方向の位置は、該脚体２の足平２２の踵寄りの位置からつま先寄りの位置まで移動するように設定される。この場合、図示の例では、片脚支持期における目標ＺＭＰのＸ軸方向の位置は、該脚体２の足平２２の底面のほぼ全面が接地する期間内において一定に維持される。

なお、図示は省略するが、歩行歩容における目標ＺＭＰのＹ軸方向の位置は、片脚支持期においては、例えば支持脚である脚体２の足首関節のＹ軸方向の位置と同じ位置（足首関節の直下の位置）に保たれるように設定される。そして、両脚支持期においては、目標ＺＭＰのＹ軸方向の位置は、直前の片脚支持期において支持脚となっていた脚体２の足首関節のＹ軸方向の位置と同じ位置から、新たに着地させた脚体２の足首関節のＹ軸方向の位置と同じ位置まで移動していくように設定される。

本実施形態では、図５に示す如く設定した目標ＺＭＰ軌道の折れ点の位置や時刻が、ＺＭＰ軌道パラメータ（目標ＺＭＰ軌道を規定するパラメータ）として設定される。

また、目標腕姿勢は、上体２４に対する相対姿勢で表される。

また、目標歩容はグローバル座標系で記述される。グローバル座標系は床に固定された座標系であり、より具体的には、以下に説明する支持脚座標系が用いられる。該支持脚座標系は、支持脚足平２２をその着地後に滑らせないように回転させて、該支持脚足平２２の底面のほぼ全面を床面に接触（密着）させた状態において、支持脚側脚体２の足首中心から床面に延ばした垂線が該床面と交わる点（この点は、本実施形態の例では支持脚足平２２の底面のほぼ全面を床面に接触させた状態では、該足平２２の代表点と合致する）を原点とし、その原点を通る水平面をＸＹ平面とする、床に固定された座標系である。この場合、Ｘ軸方向、Ｙ軸方向は、それぞれ支持脚足平２２の前後方向、左右方向である。また、Ｚ軸方向は、鉛直方向である。

尚、支持脚座標系の原点は、必ずしも支持脚足平２２の底面のほぼ全面を床面に接触させた状態での該足平２２の代表点（足平２２の位置を代表する点）に合致する必要はなく、該代表点と異なる床面上の点に設定されてもよい。

また、本実施形態では、歩行歩容の両脚支持期とこれに続く片脚支持期における支持脚座標系としては、該両脚支持期に続く片脚支持期での支持脚足平２２に対応して、上記した関係に従って規定される座標系が使用される。

次に、歩容生成装置１００での歩容生成に用いられるロボット１の動力学モデルについて図６を参照して説明する。

本実施形態に用いられるロボット１の動力学モデルは、図６に示す如く、３つの質点２４ａ，２ａ，２ｂを有する３質点モデルである。この場合、ロボット１の上体２４の運動が、質点２４ｂ（以降、上体質点２４という）の運動に対応し、脚体２，２のうちの支持脚の運動が、質点２ａ（以降、支持脚質点２ａ又は脚質点２ａという）の運動に対応し、遊脚の運動が質点２ｂ（以降、遊脚質点２ｂ又は脚質点２ｂという）に対応するものとされる。

そして、この動力学モデルでは、ロボット１の全体の動力学は、上体質点２４ｂ、支持脚質点２ａ、及び遊脚質点２ｂのそれぞれの動力学の線形結合によって表現される。また、上体質点２４ｂ、支持脚質点２ａ、及び遊脚質点２ｂのそれぞれの質量の総和は、ロボット１の全体の質量に一致するものとされる。また、上体質点２４ｂ、支持脚質点２ａ、及び遊脚質点２ｂの位置は、それぞれ、ロボット１の上***置姿勢、支持脚足平２２の位置姿勢、遊脚足平２２の位置姿勢に応じて定まる位置に設定される。

この動力学モデルの挙動（該動力学モデル上でのロボット１の動力学）は、以下に示すように数式化される。ただし、説明を簡単にするために、サジタルプレーン（支持脚座標系のＸ軸とＺ軸とを含む平面）での運動方程式のみを記述し、ラテラルプレーン（支持脚座標系のＹ軸とＺ軸とを含む平面）での運動方程式を省略した。

説明の便宜上、動力学モデルに関する変数およびパラメータを以下のように定義する。
Ｚsup：支持脚質点鉛直位置（Ｚ軸方向位置）
Ｘsup：支持脚質点水平位置（Ｘ軸方向位置）
Ｚswg：遊脚質点鉛直位置（Ｚ軸方向位置）
Ｘswg：遊脚質点水平位置（Ｘ軸方向位置）
Ｚb：上体質点鉛直位置（Ｚ軸方向位置）
Ｘb：上体質点水平位置（Ｘ軸方向位置）
ZGtotal：ロボットの全体重心の鉛直位置（Ｚ軸方向位置）
XGtotal：ロボットの全体重心の水平位置（Ｘ軸方向位置）
msup：支持脚質点質量
mswg：遊脚質点質量
mb：上体質点質量
mtotal：ロボットの総質量（＝mb＋msup＋mswg）
Ｘzmp：目標ＺＭＰの水平位置（Ｘ軸方向位置）
Ｚzmp：目標ＺＭＰの鉛直位置（Ｚ軸方向位置）
Ｆx：床反力水平成分（詳しくは並進床反力のＸ軸方向成分）
Ｆz：床反力鉛直成分（詳しくは並進床反力のＺ軸方向成分）
Ｍzmp_y：目標ＺＭＰまわりの床反力モーメント（詳しくは該床反力モーメントのＹ軸まわり成分）

また、任意の変数Ａに対して、dA/dtはＡの１階微分値を表わし、d2A/dt2はＡの２階微分値を表わす。したがって、変数Ａが変位（位置）ならば、dA/dtは速度、d2A/dt2は加速度を意味する。ｇは重力加速度定数を示す。ここでは、ｇは正の値とする。

上記動力学モデルの運動方程式は、式０１〜０３で表される。

Fz＝mb＊(g＋d2Zb/dt2)＋msup＊(g＋d2Zsup/dt2)
＋mswg＊(g＋d2Zswg/dt2) ……式０１
Fx＝mb＊d2Xb/dt2＋msup＊(g＋d2Xsup/dt2)
＋mswg＊(g＋d2Xswg/dt2) ……式０２
Mzmp_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) ……式０３

また、ロボットの全体重心の位置は、次式により表される。

ZGtotal＝(mb＊Zb＋msup＊Zsup＋mswg＊Zswg)／mtotal ……式０４
XGtotal＝(mb＊Xb＋msup＊Xsup＋mswg＊Xswg)／mtotal ……式０５

本実施形態における歩容生成装置１００は、ロボット１の片方の脚体２が着地してから他方の脚体２が着地するまでの１歩分の目標歩容を単位として、その１歩分の目標歩容を順番に生成する。従って、本実施形態の主要例とする歩行歩容では、該目標歩容は両脚支持期の開始時から、これに続く片脚支持期の終了時（次の両脚支持期の開始時）までの目標歩容が順番に生成される。

ここで、本明細書では、目標歩容の「１歩」は、ロボット１の片方の脚体２が着地してからもう一方の脚体２が着地するまでの意味で使用する。また、新たに生成しようとしている目標歩容を「今回歩容」、その次の目標歩容を「次回歩容」、さらにその次の目標歩容を「次次回歩容」、というように呼ぶ。また、「今回歩容」の一つ前に生成した目標歩容を「前回歩容」と呼ぶ。

また、歩容生成装置１００は、ロボット１の運動の継続性を確保し得る今回歩容を生成するために、今回歩容に続く仮想的な周期的歩容（同じパターンのロボット１の運動が一定周期で継続的に繰り返される歩容）としての定常旋回歩容を決定する。そして、歩容生成装置１００は、今回歩容の上体２４の運動軌道を、将来において定常旋回歩容での上体２４の運動軌道に収束させるようにして、該今回歩容を生成する。換言すれば、歩容生成装置１００は、定常旋回歩容を将来の収束目標として、今回歩容を生成する。この場合、本実施形態では、今回歩容の遊脚側脚体２の足平２２の着地時の位置姿勢（着地位置姿勢）又は着地時刻は、今回歩容の生成途中で、適宜更新される。

以下に歩行歩容を生成することを主要例にして、歩容生成装置１００の歩容生成処理の詳細の詳細を図７〜図２０を参照しつつ説明する。

歩容生成装置１００は、図７のフローチャート（構造化フローチャート）に示す歩容生成処理（メインルーチン処理）を実行することによって目標歩容を生成する。

まずＳ０１０において時刻ｔを“０”に初期化するなど種々の初期化作業が行なわれる。この処理は、歩容生成装置１００の起動時等に行なわれる。

次いで、Ｓ０１２を経てＳ０１４に進み、歩容生成装置１００は、制御周期（図７のフローチャートの演算処理周期）毎のタイマ割り込みを待つ。制御周期はΔｔである。

次いで、Ｓ０１６に進み、歩容生成装置１００は、歩容の切り替わり目であるか否かを判断する。このとき、歩容の切り替わり目である場合には、Ｓ０１８の処理を経て、Ｓ０２０に進む。この場合、Ｓ０１８では、歩容生成装置１００は、時刻ｔを“０”に初期化する。また、Ｓ０１６で歩容の切り替わり目でない場合には、Ｓ０２０に進む。

ここで、上記「歩容の切り替わり目」は、前回歩容の生成が完了し、今回歩容の生成を開始するタイミングを意味し、例えば前回歩容の生成を完了した制御周期の次の制御周期が歩容の切り替わり目になる。

次いで、Ｓ０２０では、歩容生成装置１００は、次回歩容支持脚座標系、次次回歩容支持脚座標系、今回歩容周期および次回歩容周期を決定する。

なお、支持脚座標系を決定するということは、詳しくは、その原点の位置と該支持脚座標系の姿勢（各座標軸の向き）とを決定することを意味する。この場合、支持脚座標系は、その位置姿勢（詳しくは原点の位置とＸ軸及びＹ軸の向き）が、前記したように、支持脚足平２２の底面のほぼ全面を床面に接触させた状態での該足平２２の位置姿勢に対して所定の関係に従って規定される座標系であるから、次回歩容支持脚座標系及び次次回歩容支持脚座標系を決定するということは、それぞれ、今回歩容の遊脚足平２２の目標着地位置姿勢、次回歩容の遊脚足平２２の目標着地位置姿勢を決定することと同等である。

これらの支持脚座標系及び歩容周期は、基本的には、前記要求パラメータに基づき決定されるものである。すなわち、本実施形態では、歩容生成装置１００に入力される要求パラメータは、２歩先までの遊脚足平２２の着地予定位置姿勢（足平２２が着地してから足底を床面にほぼ全面的に接触させるように、滑らさずに回転させた状態での足平位置姿勢）と着地予定時刻とを規定する要求値を含んでいる。そして、その１歩目の要求値、２歩目の要求値がそれぞれ、今回歩容、次回歩容に対応するものとして、今回歩容の生成開始時（前記Ｓ０１６の歩容の切り替わり目）以前に歩容生成装置１００に与えられる。

そして、歩容の切り替わり目の制御周期（ｔ＝０となる制御周期）において、上記要求パラメータにおける１歩目の遊脚足平２２（今回歩容での遊脚足平２２）の着地予定位置姿勢の要求値に対応して次回歩容支持脚座標系が決定される。

例えば図１０を参照して、今回歩容（１歩目）に係わる遊脚足平２２（図では２２Ｌ）の着地予定位置姿勢の要求値が、今回歩容の支持脚足平２２（図では２２Ｒ）の着地位置姿勢に対して、今回歩容支持脚座標系のＸ軸方向（今回歩容の支持脚足平２２Ｒの前後方向）及びＹ軸方向（今回歩容の支持脚足平２２Ｒの左右方向）に、それぞれxnext、ynextだけ移動し、且つＺ軸まわり（鉛直軸まわり）にθznextだけ回転させた位置姿勢であるとする。

このとき、次回歩容支持脚座標系は、図示のように今回歩容の遊脚足平２２Ｌの着地予定位置姿勢の要求値に従って該足平２２Ｌを着地させた場合（足平２２の代表点を着地予定位置の要求値に一致させ、且つ、足平２２の姿勢（向き）を着地予定姿勢の要求値に一致させた場合）における該足平２２Ｌの代表点（より詳しくは該代表点に合致する床上の点）を原点とし、該原点を通る水平面内における該足平２２Ｌの前後方向、左右方向をそれぞれＸ’軸方向、Ｙ’軸方向とする座標系に決定される。

上記と同様に、２歩目の遊脚側足平２２の着地予定位置姿勢の要求値に応じて次々回歩容支持脚座標系（図１０のＸ”Ｙ”座標を参照）が決定される。また、今回歩容周期は、今回歩容の支持脚足平２２の着地予定時刻（要求値）から、１歩目（今回歩容）の遊脚足平２２の着地予定時刻（要求値）までの時間として決定され、次回歩容周期は、１歩目の遊脚足平２２の着地予定時刻（要求値）から２歩目の遊脚足平２２の着地予定時刻（要求値）までの時間として決定される。

また、歩容の切り替わり目以後の制御周期（ｔ＞０となる制御周期）においては、次回歩容支持脚座標系、次次回歩容支持脚座標系、今回歩容周期および次回歩容周期は、基本的には、前回の制御周期において決定されたものと同じに設定される。

なお、２歩目までの遊脚足平２２の着地位置姿勢及び着地時刻の要求値は、外部の操縦装置等からの指令（要求）とそのときまでのロボットの１の移動履歴とを基に決定してもよい。また、要求パラメータは、前記次回及び次次回歩容支持脚座標系の位置及び姿勢の要求値、並びに今回及び次回歩容周期の要求値を直接的に指定するパラメータであってもよい。

また、遊脚足平２２の着地位置姿勢は、着地瞬間の位置姿勢でもよいが、本実施形態の説明では、理解の便宜上、該遊脚足平２２を着地させてから、該足平２２を滑らせないように回転させて、該足平２２の底面のほぼ全面を床面に接触させた状態での該足平２２の位置姿勢を意味するものとする。そして、その位置姿勢を、以降、着地時足平基準位置姿勢ということがある。また、遊脚足平２２の着地時刻は、本実施形態では、便宜上、該足平２２の着地瞬間の時刻を意味するものとする。

補足すると、本実施形態では、今回歩容及び次回歩容の一方又は両方におけるロボット１の遊脚足平２２の着地予定位置姿勢（ひいては、次回歩容支持脚座標系及び次次回歩容支持脚座標系のうちの少なくともいずれか一方の位置姿勢）、あるいは、今回歩容もしくは次回歩容の歩容周期は、各制御周期において、Ｓ０２０で最初に決定されたものから、修正される場合もある。すなわち、各制御周期において、Ｓ０２０の処理が、複数回実行される場合もある。

次いでＳ０２２に進み、歩容生成装置１００は、今回歩容に続く仮想的な周期的歩容（今回歩容の収束目標とする仮想的な周期的歩容）としての定常旋回歩容の歩容パラメータを決定する。該歩容パラメータは、本実施形態では、定常旋回歩容における目標足平位置姿勢軌道を規定する足平軌道パラメータ、目標上体姿勢軌道を規定する上体姿勢軌道パラメータ、目標腕姿勢軌道を規定する腕姿勢軌道パラメータ、目標ＺＭＰ軌道を規定するＺＭＰ軌道パラメータ、目標床反力鉛直成分軌道を規定する床反力鉛直成分軌道パラメータを含む。

ここで、「定常旋回歩容」は、その歩容を繰り返したときに歩容の境界（１周期毎の歩容の境界）においてロボット１の運動状態（足平位置姿勢、上***置姿勢等の運動状態）に不連続が生じないような周期的歩容を意味する。また、「定常旋回歩容」は、周期的歩容、すなわち同じパターンの歩容を一定周期で繰り返す歩容であるから、ロボット１の運動を継続的に行い得る歩容である。換言すれば、「定常旋回歩容」は、歩容の軌道の不連続を発生することなく、同じパターンの運動を繰り返すことができる周期的歩容（原理上、無限回繰り返しても後述の「発散」が発生しない歩容）である。

周期的歩容である定常旋回歩容は、本実施形態では、ロボット１の２歩分の歩容、すなわち今回歩容に続く第１旋回歩容と該第１旋回歩容に続く第２旋回歩容とからなる歩容を１周期分の歩容として、その１周期分の歩容を一定周期で繰り返す歩容である。ここで「旋回」なる用語を用いたのは、旋回率を零とするときは直進を意味するので、直進も広義の意味で旋回に含ませることができるからである。

定常旋回歩容（以降、単に定常歩容ということがある）について補足すると、２足移動ロボットでは、定常歩容の１周期分の歩容には、少なくとも２歩分の歩容が含まれる必要である。この場合、３歩以上の歩容を１周期分の歩容とする複雑な定常歩容を設定することも可能である。但し、定常歩容は、後述の如く、今回歩容の終端（終了時刻）における目標とする（適切な）発散成分の値を決定するためだけに用いられる。このため、３歩以上の歩容を１周期とする定常歩容を用いることは、歩容生成の処理が煩雑となるにもかかわらず、効果は少ない。そこで、本実施形態での定常歩容の１周期分の歩容を２歩分の歩容（第１旋回歩容及び第２旋回歩容の組）により構成するようにしている。尚、３足以上の脚式移動ロボットにあっては、それに応じて定常歩容を定義するに足る歩容数が増加する。

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

定常歩容は、歩容生成装置１００で今回歩容の終端におけるロボット１の目標とする運動状態を決定するために作成される仮想的な歩容である。従って、定常旋回歩容は、歩容生成装置１００からそのまま出力されるものではない。

本実施形態では、目標歩容が、上記発散を生じることなく、継続的に生成されるように、発散成分を指標にして歩容を生成するようにした。この場合、継続的な歩容の代表例である定常歩容であっても、定常歩容の歩容パラメータが変われば、該定常歩容の初期発散成分（定常歩容の初期時刻での発散成分）も変わる。すなわち、歩き方、走り方、移動速度等の歩容形態によって、適切な発散成分が変わる。

そこで、本実施形態では、歩容生成装置１００は、今回歩容を生成しようとするときに、まず、その生成しようとする今回歩容の後に続く歩容（将来的な収束目標とする歩容）として好適な将来の仮想的な周期的歩容（ロボット１の安定な移動を継続することが可能な将来の仮想的な周期的歩容）としての定常歩容を、今回歩容に係わる要求パラメータ（又はＳ０２０で決定した支持脚座標系及び歩容周期）に応じて決定した上で、該定常歩容の初期発散成分を求める。そして、歩容生成装置１００は、今回歩容の終端発散成分を、その求めた定常歩容の初期発散成分に一致させる（より一般的には、今回歩容を定常歩容に連続させもしくは漸近させる）ように、今回歩容を生成する。このような歩容生成の基本的な指針は本出願人が先に提案した特許第３７２６０８１号のものと同様である。

本題に戻り、Ｓ０２２では、歩容生成装置１００は、図９のフローチャートに示すサブルーチン処理を実行する。

まず、Ｓ１００において、歩容生成装置１００は、今回歩容、第１旋回歩容、第２旋回歩容の順に足平位置姿勢軌道がつながるように、定常歩容の歩容パラメータのうちの足平軌道パラメータが決定される。以下に、定常歩容が歩行歩容である場合を例にとって、具体的な設定方法を図１０を参照して説明する。尚、以降の説明では、歩容の「初期」、「終端」はそれぞれ歩容の開始時刻、終了時刻、あるいは、それらの時刻における瞬時歩容を意味する。

足平軌道パラメータは、第１旋回歩容および第２旋回歩容のそれぞれの初期と終端とにおける支持脚足平２２及び遊脚足平２２のそれぞれの位置姿勢、各旋回歩容の歩容周期（各旋回歩容の初期から終端までの時間）等から構成される。この足平軌道パラメータのうち、第１旋回歩容初期遊脚足平位置姿勢は、次回歩容支持脚座標系から見た今回歩容終端支持脚足平位置姿勢とされる。この場合、今回歩容終端支持脚足平位置姿勢は、今回歩容の支持脚足平２２を、今回歩容支持脚座標系に対応する前記着地時足平基準位置姿勢から、滑らせないようにピッチ方向に（該足平２２の踵が持ち上がる向きに）所定角度だけ回転させた状態での位置姿勢である。

また、第１旋回歩容初期支持脚足平位置姿勢は、次回歩容支持脚座標系から見た今回歩容終端遊脚足平位置姿勢とされる。この場合、今回歩容終端遊脚足平位置姿勢は、今回歩容の遊脚足平２２を、該今回歩容終端遊脚足平位置姿勢から、滑らせないようにピッチ方向に（該足平２２のつま先が下がる向きに）所定角度だけ回転させた状態での該足平２２の位置姿勢が、次回歩容支持脚座標系に対応する前記着地時足平基準位置姿勢に合致するように決定される。

第１旋回歩容終端遊脚足平位置姿勢は、次回歩容支持脚座標系から見た次回歩容終端遊脚足平位置姿勢とされる。この場合、次回歩容終端遊脚足平位置姿勢は、次回歩容の遊脚足平２２を、該次回歩容終端遊脚足平位置姿勢から、滑らせないようにピッチ方向に（該足平２２のつま先が下がる向きに）所定角度だけ回転させた状態での該足平２２の位置姿勢が、次次回歩容支持脚座標系に対応する前記着地時足平基準位置姿勢に合致するように決定される。

第１旋回歩容終端支持脚足平位置姿勢は、次回歩容支持脚座標系から見た次回歩容終端支持脚足平位置姿勢とされる。この場合、次回歩容終端支持脚足平位置姿勢は、次回歩容の支持脚足平２２を、次回歩容支持脚座標系に対応する前記着地時足平基準位置姿勢から、滑らせないようにピッチ方向に（該足平２２の踵が持ち上がる向きに）所定角度だけ回転させた状態での位置姿勢である。

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

第２旋回歩容終端遊脚足平位置姿勢は、次次回歩容支持脚座標系から見た位置姿勢であり、その位置姿勢は、今回歩容支持脚座標系から見た今回歩容終端遊脚足平位置姿勢に一致するものとされる。該第２旋回歩容終端遊脚足平位置姿勢は、換言すれば、第２旋回歩容での遊脚足平２２を、該第２戦回歩容終端遊脚足平位置姿勢から、滑らせないようにピッチ方向に（つま先が下がる向きに）所定角度だけ回転させた状態での該足平２２の位置姿勢が、定常歩容における次次次回歩容支持脚座標系（図１０に示す）に対応する着地時足平基準位置姿勢に合致するように決定された位置姿勢である。ここで、次次次回歩容支持脚座標系は、次次回歩容支持脚座標系で見た該次次次回歩容支持脚座標系の位置姿勢（図１０中のxnext，ynext，θznextにより表される位置姿勢）が、今回歩容支持脚座標系で見た次回歩容支持脚座標系の位置姿勢に合致する支持脚座標系である。

また、第２旋回歩容終端支持脚足平位置姿勢は、次次回歩容支持脚座標系から見た位置姿勢であり、その位置姿勢は、今回歩容支持脚座標系から見た今回歩容終端支持脚足平位置姿勢に一致するものとされる。すなわち、第２旋回歩容終端支持脚足平位置姿勢は、次次回歩容における支持脚足平２２を、次次回歩容支持脚座標系に対応する着地時足平基準位置姿勢から、滑らせないようにピッチ方向に（該足平２２の踵が持ち上がる向きに）所定角度だけ回転させた状態での位置姿勢である。

第１旋回歩容および第２旋回歩容の歩容周期は、次回歩容周期と同一に設定される。これらの第１旋回歩容及び第２旋回歩容の歩容周期は、互いに同一にすることは必ずしも必要ではないが、いずれの周期も、少なくとも次回歩容周期に応じて決定するのが好ましい。

次に、Ｓ１０２に進み、歩容生成装置１００は、定常歩容における目標上体姿勢軌道を規定する上体姿勢軌道パラメータを決定する。

この場合、本実施形態では、定常歩容の上体姿勢軌道パラメータは、それにより規定される目標上体姿勢軌道があらかじめ定められた所定パターンの基準上体姿勢の軌道に一致するように決定される。この基準上体姿勢は、本実施形態では、経時的な変化の無い一定姿勢（固定的な姿勢）に設定されている。その基準上体姿勢は、例えば上体２４の体幹軸が定常的に鉛直方向を向く姿勢（ロボット１の直立姿勢で起立した状態での上体姿勢）、すなわち、鉛直方向に対する上体姿勢角が定常的に“０”に保たれる姿勢である。そして、このような基準上体姿勢の軌道を規定するパラメータ（基準上体姿勢の一定の上体姿勢角の値等）が上体姿勢軌道パラメータとして決定される。なお、このように上体姿勢を一定姿勢とした場合には、必然的に上体姿勢角の角速度及び角加速度も定常的に“０”に保たれる。

補足すると、定常歩容の上体姿勢は、定常歩容の初期（第１旋回歩容の初期）と終端（第２旋回歩容の終端）とでつながるように（上体姿勢角及びその角速度がそれぞれ、定常歩容の初期と終端とで一致するように）に設定されている限り、一定姿勢である必要はない。本実施形態では、本実施形態の理解を容易にするために基準上体姿勢を上記の如く一定姿勢とした。

次に、Ｓ１０４に進み、歩容生成装置１００は、腕姿勢軌道パラメータ、より詳しくは鉛直軸（あるいは上体２４の体幹軸）まわりの両腕体の角運動量変化に関すること以外の腕姿勢軌道パラメータが決定される。たとえば、上体２４に対する腕体の手先の相対高さや腕体全体の相対重心位置などを規定するパラメータが腕姿勢軌道パラメータとして決定される。この場合、本実施形態では、腕体全体の相対重心位置は、上体２４に対して一定に維持されるように設定される。

次に、Ｓ１０６に進み、歩容生成装置１００は、床反力鉛直成分軌道パラメータを決定する。この場合、該パラメータにより規定される床反力鉛直成分軌道が、第１旋回歩容および第２旋回歩容のいずれにおいても、実質的に連続なものとなるように床反力鉛直成分軌道パラメータが決定される。

具体的には、定常歩容が歩行歩容である場合の目標床反力鉛直成分軌道は、例えば図１１に示すようなパターンで設定される。そのパターンは、第１旋回歩容及び第２旋回歩容のいずれにおいても、先に図４に示したパターンと同様のパターンである。そして、このパターンの折れ点の時刻や床反力鉛直成分の値が床反力鉛直成分軌道パラメータとして決定される。

この床反力鉛直成分軌道パラメータは、定常歩容の１周期分の期間（第１旋回歩容の期間と第２旋回歩容の期間とを合わせた期間）における床反力鉛直成分の平均値をロボット１の自重と一致させるように決定される。すなわち、定常歩容の１周期分の期間における床反力鉛直成分の平均値がロボット１の全体に作用する重力と同じ大きさで反対向きになるようにする。

上記のごとく床反力鉛直成分軌道パラメータ（ひいては床反力鉛直成分軌道）を決定することは、定常歩容のあらゆる状態量（ロボット１の各部の位置、姿勢、速度等の運動状態量）についての初期状態（定常歩容の第１旋回歩容の支持脚座標系から見た該第１旋回歩容の初期状態）と終端状態（定常歩容の第２旋回歩容に続く第１旋回歩容の支持脚座標系から見た該第２旋回歩容の終端状態）とが一致しているという条件（以下、この条件を定常歩容の境界条件ということがある）が成立する必要があるからである。

次いで、Ｓ１０８に進み、歩容生成装置１００は、第１旋回歩容および第２旋回歩容を合わせた定常歩容の目標ＺＭＰ軌道（目標床反力中心点軌道）を規定するＺＭＰ軌道パラメータを決定する。この場合、目標ＺＭＰ軌道は、前述したように安定余裕が高くかつ急激な変化をしないように決定される。

定常歩容が歩行歩容である場合の目標ＺＭＰ軌道（詳しくは目標ＺＭＰのＸ軸方向位置）は、例えば図１２に示すパターンで設定される。このパターンは、第１旋回歩容及び第２旋回歩容のいずれにおいても、先に図５に示したパターンと同様のパターンである。そして、このパターンの折れ点の時刻や目標ＺＭＰの位置がＺＭＰ軌道パラメータとして決定される。

なお、図示は省略するが、定常歩容が歩行歩容である場合の目標ＺＭＰのＹ軸方向の位置は、第１旋回歩容及び第２旋回歩容のいずれにおいても、片脚支持期においては、支持脚である脚体２の足首関節のＹ軸方向の位置と同じ位置（足首関節の直下の位置）に保たれるように設定される。そして、両脚支持期においては、目標ＺＭＰのＹ軸方向の位置は、直前の片脚支持期において支持脚となっていた脚体２の足首関節のＹ軸方向の位置と同じ位置から、新たに着地させた脚体２の足首関節のＹ軸方向の位置と同じ位置まで移動していくように設定される。

以上が、図７のＳ０２２の処理の詳細である。

図７に戻って、歩容生成装置１００は、上記の如くＳ０２２の処理を実行した後、Ｓ０２４に進み、定常歩容の初期状態を算出する。ここで算出する初期状態は、具体的には、定常歩容の初期上***置速度（初期上***置及び初期上体速度）及び初期発散成分である。この初期状態の算出は、図１３のフローチャートに示す処理によって、探索的に行なわれる。

以下説明すると、まず、Ｓ２００において、歩容生成装置１００は、定常歩容パラメータ（図７のＳ０２２で決定された歩容パラメータ）に基づいて、定常歩容の目標足平位置姿勢、目標腕姿勢、および目標上体姿勢角の初期状態（定常歩容の初期時刻（＝今回歩容の終端時刻）での状態）を決定する。ここで「状態」とは、位置又は姿勢とその時間的変化率（位置又は姿勢の変化速度）との組を意味する。例えば目標足平位置姿勢の初期状態は、定常歩容の初期時刻での足平位置姿勢と、その時間的変化率（すなわち、足平２２の移動速度及び姿勢の変化速度）との組を意味する。目標腕姿勢の初期状態、目標上体姿勢角の初期状態についても同様である。

この場合、支持脚側の足平位置姿勢の初期状態と、遊脚側の足平位置姿勢の初期状態とは、図９のＳ１００で決定した足平軌道パラメータに基づいて決定される。

具体的には、足平軌道パラメータにうちの、前記第１旋回歩容初期支持脚足平位置姿勢と、第１旋回歩容初期遊脚足平位置姿勢とが、それぞれ、定常歩容の初期時刻での支持脚側の足平位置姿勢、遊脚側の足平位置姿勢として決定される。

また、定常歩容の初期時刻での支持脚側の足平位置姿勢の時間的変化率は、第１旋回歩容初期支持脚足平位置姿勢から第２旋回歩容終端遊脚足平位置姿勢に至る足平位置姿勢軌道（次回歩容支持脚座標系から見た軌道）を有限時間整定フィルタにより生成した場合における該軌道の初期時刻（定常歩容の初期時刻）での足平位置姿勢の時間的変化率として算出される。この場合、第１旋回歩容初期支持脚足平位置姿勢から第２旋回歩容終端遊脚足平位置姿勢に至る足平位置姿勢軌道のうち、定常歩容の初期時刻からその直後の時刻までの軌道から定常歩容の初期時刻での支持脚側の足平位置姿勢の時間的変化率を算出することができる。

また、定常歩容の初期時刻での遊脚側の足平位置姿勢の時間的変化率は、今回歩容初期支持脚足平位置姿勢から第１旋回歩容終端終端遊脚足平位置姿勢に至る足平位置姿勢軌道（次回歩容支持脚座標系から見た軌道）を有限時間整定フィルタにより生成した場合における該軌道の初期時刻（定常歩容の初期時刻）での足平位置姿勢の時間的変化率として算出される。この場合、今回歩容初期支持脚足平位置姿勢から第１旋回歩容終端終端遊脚足平位置姿勢に至る足平位置姿勢軌道のうち、今回歩容の初期時刻から定常歩容の初期時刻（又はその直後の時刻）までの軌道を生成すれば、該軌道（定常歩容の初期時刻の近辺の軌道）から定常歩容の初期時刻での遊脚側の足平位置姿勢の時間的変化率を算出することができる。

また、腕姿勢の初期状態は、図９のＳ１０４で決定した腕姿勢軌道パラメータに基づいて、定常歩容の初期時刻における腕姿勢（上体２４に対する両腕体の全体重心位置等）と該初期時刻の直後の期間の腕姿勢の変化量とを求めることで決定される。

また、上体姿勢角の初期状態は、図９のＳ１０２で決定した上体姿勢軌道パラメータ基づいて、定常歩容の初期時刻における上体姿勢角と該初期時刻の直後の期間の上体姿勢角の変化量とを求めることで決定される。なお、本実施形態では、上体姿勢軌道パラメータにより規定される上体姿勢は、定常的に上体２４の体幹軸が鉛直方向に向く姿勢であるので、定常歩容の初期時刻での上体姿勢角およびその角速度は、いずれも“０”である。

次いで歩容生成装置１００は、Ｓ２０２において初期上体水平位置速度の候補（定常歩容の初期時刻での上体２４の水平位置及び水平速度の候補）である（Ｘs，Ｖxs）（Ｘs：水平位置、Ｖxs：水平速度）を仮決めする。ここで仮決めする候補（Ｘs，Ｖxs）は、任意でよいが、例えば前回歩容の生成時に求めた定常歩容の初期状態における上体水平位置速度の候補（Ｘs，Ｖxs）として仮決めすればよい。

尚、説明を簡略にするため、サジタルプレーン上でＸ軸方向（ロール軸方向）での定常歩容の初期状態（初期上体水平位置速度）を探索する場合を例にとって説明する。ただし、実際には位置・速度のいずれについても、Ｘ軸方向（ロール軸方向）と、Ｙ軸方向（ピッチ軸方向）とで別々に、あるいは同時に定常歩容の初期状態（定常歩容の前記境界条件を満たす初期状態）を探索する必要がある。

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

次いで、Ｓ２０４を経てＳ２０６に進み、定常歩容の初期時刻での上体２４の鉛直位置（Ｚ軸方向位置）Ｚs及び鉛直速度（Ｚ軸方向速度）Ｖzsの組である初期上体鉛直位置速度（Ｚs，Ｖzs）が決定される。

初期上体鉛直速度Ｖzsは、本実施形態では例えば以下のように決定される。

ロボット１には、動力学的関係として、次式が成立する。

終端全体重心鉛直位置−初期全体重心鉛直位置
＝（床反力鉛直成分／ロボットの全体質量）の２階積分
＋重力加速度の２階積分
＋初期全体重心鉛直速度＊１歩の時間
…式１３
（ただし、重力加速度は負の値とする。）
また、定常歩容では、終端全体重心鉛直位置と初期全体重心鉛直位置は一致するので、上式１３の右辺は零にならなければならない。したがって、これらの関係から初期全体重心鉛直速度を求めることができる。具体的には、まず、前記図９のＳ１０６で決定した床反力鉛直成分軌道パラメータによって算出される床反力鉛直成分をロボット１の全体質量で割った値を、定常歩容の初期から終端までの期間において２階積分することにより、床反力鉛直成分による全体重心移動量（式１３の右辺第１項）が求められる。

さらに、重力加速度を定常歩容の初期から終端までの期間において２階積分することにより、重力による全体重心移動量（式１３の右辺第２項）が求められる。そして、上記の如く求めた、床反力鉛直成分による全体重心移動量と重力による全体重心移動量との和の符号を反転させたものを、定常歩容の１周期Tcycの時間で割ることにより、初期全体重心鉛直速度が求められる。

そして、本実施形態では、例えば、図６に示した３質点モデルを用い、該３質点モデルの重心点の鉛直速度が上記初期全体重心鉛直速度に一致するような上体２４の鉛直速度を、Ｓ２００で決定した足平位置姿勢、腕姿勢等の初期状態と、Ｓ２０２（又は後述のＳ２１８）で決定された初期上体水平位置速度（Ｘs，Ｖxs）とを基に求め、その求めた鉛直速度を初期上体鉛直速度Ｖzsとして決定する。

この場合、上記の如く求めた初期全体重心鉛直速度と、足平位置姿勢の初期状態とを基に、前記式０４の両辺を1階微分した式によって、上体質点２４ｂの鉛直速度を算出できる。そして、その上体質点２４ｂの鉛直速度と、Ｓ２００で決定された上体姿勢角の初期状態とを基に、初期上体鉛直速度Ｖzsを決定することが可能である。

なお、より多くの質点を有する多質点モデル（例えばロボット１の各リンクに質点を有する幾何学モデル）を用いることによってより正確に、初期上体鉛直速度Ｖzsを決定するようにしてもよい。また、ロボット１の上体２４以外の部位の質量が上体２４に比して十分に小さいような場合には、例えばロボット１の全体重心の鉛直速度が上体２４の鉛直速度にほぼ一致すると見なし、上記初期全体重心鉛直速度を、そのまま初期上体鉛直速度Ｖzsとして決定するようにしてもよい。

一方、定常歩容の初期上体鉛直位置Ｚsは、例えば本出願人が先に特開平１０−８６０８０号公報で提案した上体高さ決定手法を用いて決定される。このとき、初期時刻における足平位置姿勢（前記図９のＳ１００で決定した第１旋回歩容初期支持脚足平位置姿勢及び第１旋回歩容初期遊脚足平位置姿勢）と、各脚体２の膝部の曲げ角に関する所定の幾何学的条件とに基づいて、初期時刻での各脚体２，２の膝部が伸び切らないような初期上体鉛直位置Ｚsが決定される。例えば、支持脚側脚体２の膝曲げ角をθsup、遊脚側脚体２の膝曲げ角をθswgとしたとき、それらの膝曲げ角θsup，θswgの正弦関数値の逆数の和がある所定値（有限値）になるように、初期上体鉛直位置が決定される。ここで、膝曲げ角θsup，θswgは、各脚体２の大腿部の軸心に対する下腿部の軸心の角度であり、各脚体２が伸びきった状態から膝部を曲げていくに伴い、“０”から増加していく角度である。なお、このような上体２４の鉛直位置の決定手法は、上記特開平１０−８６０８０号公報に詳細に説明されているので、ここでは以上の説明に留める。

補足すると、ロボット１は、片脚当たり６自由度であることから、足平位置姿勢軌道および上***置姿勢軌道の初期状態（位置姿勢とその時間的変化率）が与えられれば、ロボット１の上体２４及び脚体２，２の運動の初期状態は一義的にすべて決定されることになる。したがって、例えば、上体２４及び脚体２，２の全体の質量がロボット１の全体の質量にほぼ一致するような場合には、上体２４及び脚体２，２の運動の初期状態が与えられれば、初期全体重心鉛直速度も一義的に決定される。逆に、片脚当たり６自由度であることから、足平位置姿勢軌道および上***置姿勢軌道の初期状態の内で速度に関する状態のひとつ（例えば初期上体鉛直速度）が未定であっても、初期全体重心鉛直速度が与えられれば、未定の初期状態は一義的に決定される。

Ｓ２０６の処理の後、次いでＳ２０８に進み、定常歩容の候補としての歩容（定常歩容の１歩分（１周期分）の歩容）が仮に生成される。より具体的には、図７のＳ０２２で決定した定常歩容パラメータを基に、初期時刻から終端時刻までの各瞬間における目標ＺＭＰ、目標床反力鉛直成分、目標足平位置姿勢、目標上体姿勢および目標腕姿勢の瞬時値が逐次求められる。そして、求めた目標ＺＭＰと目標床反力鉛直成分とを満足するように、図６に示した動力学モデル（３質点モデル）を用いて上***置を逐次決定することによって、定常歩容の初期時刻から終端時刻までの歩容を生成する。この場合、前記初期上体水平位置速度（Ｘs，Ｖxs）、初期上体鉛直位置速度（Ｚs，Ｖzs）が上体２４の位置及び速度の初期状態として設定される。

尚、この歩容生成は歩容生成装置１００の内部で行われるだけであって、実際のロボット１を駆動するための目標値として、歩容生成装置１００から出力されることはない。

Ｓ２０８の処理は、具体的には、図１４のフローチャートに示す如く実行される。

以下説明すると、歩容生成装置１００は、まず、Ｓ３００において、諸々の初期化を行なう。具体的には、歩容生成用時刻Ｔkが定常歩容の初期時刻Ｔsに初期化される。さらに、上体水平位置速度に、初期上体水平位置速度（Ｘs，Ｖxs）の最新の候補値（図１３のＳ２０２もしくは後述のＳ２１６もしくはＳ２１８で決定された最新の候補値）が代入され、上体鉛直位置速度に、初期上体鉛直位置速度（Ｚs，Ｖzs）の最新値（図１３のＳ２０６で決定された最新値）が代入される。また、上体姿勢角には、基準上体姿勢角の初期値、上体姿勢角速度には、基準上体姿勢角速度の初期値が代入される。

次いで、Ｓ３０２を経てＳ３０４において、歩容生成装置１００は、歩容生成用時刻Ｔk（現在値）が終端時刻Ｔe（＝Ｔs＋Ｔcyc）以前の時刻であるか否か（Ｔk≦Ｔeであるか否か）を判断する。この判断結果が肯定的である場合には、歩容生成装置１００は、Ｓ３０６〜Ｓ３１６の処理（詳細は後述する）を実行することによって、時刻Ｔkでの歩容の瞬時値を決定する。

次いでＳ３１８に進み、歩容生成装置１００は、歩容生成用時刻Ｔkを所定の刻み時間ΔＴkだけ増加させた後、Ｓ３０４の判断を再び行なう。ここで、刻み時間ΔTkは、例えば制御周期Δｔと一致させておけばよい。ただし、演算量を低減するために、ΔTkをΔｔよりも長い時間に設定してもよい。

Ｓ３０４の判断結果が肯定的である場合には、Ｓ３０６からＳ３１８までの処理が繰り返され、Ｓ３０４の判断結果が否定的になった場合には、図１４の処理、すなわち、図１３のＳ２０８の処理は終了する。これにより、定常歩容の初期時刻から終端時刻までの１周期分の定常歩容（仮の定常歩容）の生成が完了する。

歩容生成装置１００は、上記Ｓ３０６〜Ｓ３１６で仮の定常歩容の瞬時値を決定する処理を次のように実行する。まずＳ３０６において、歩容生成装置１００は、定常歩容パラメータ（より詳しくは床反力鉛直成分軌道パラメータ）を基に、図１１に示した目標床反力鉛直成分軌道の時刻Ｔkでの瞬時値を求める。

さらに、Ｓ３０８において、歩容生成装置１００は、定常歩容パラメータ（より詳しくはＺＭＰ軌道パラメータ）を基に、図１２に示した目標ＺＭＰ軌道の時刻Ｔkでの瞬時値を求める。

次いでＳ３０８に進み、歩容生成装置１００は、定常歩容パラメータ（より詳しくは足平軌道パラメータ、上体姿勢軌道パラメータ、及び腕姿勢軌道パラメータ）を基に、目標足平位置姿勢（支持脚側及び遊脚側の両方の目標足平位置姿勢）、目標上体姿勢および目標腕姿勢の時刻Ｔkでの瞬時値をそれぞれ求める。ただし、目標腕姿勢に関しては、より詳しくは両腕体の全体重心位置が決定されるが、鉛直軸（または上体２４の体幹軸）まわりの角運動量を変化させる腕体の運動（腕振り運動）は、未だ決定されない。

次いでＳ３１０に進み、歩容生成装置１００は、Ｓ３０６で求めた目標床反力鉛直成分を満足する（ロボット１の全体重心の鉛直方向の慣性力と重力との和を目標床反力鉛直成分に釣り合わせる）ように、時刻Ｔkでの上体鉛直位置を算出する。

具体的には、次式１５および式１６で表される動力学関係式（鉛直方向でのニュートンの運動方程式を離散化した式）を用いてロボット１の全体重心鉛直速度と全体重心鉛直位置とが算出される。

時刻Ｔkにおける全体重心鉛直速度
＝時刻(Ｔk−ΔTk)における全体重心鉛直速度
＋((時刻Ｔkにおける床反力鉛直成分／ロボットの全体質量)＋重力加速度)＊ΔTk
（ただし、重力加速度は負の値とする。）
…式１５

時刻Ｔkにおける全体重心鉛直位置
＝時刻(Ｔk−ΔTk)における全体重心鉛直位置
＋時刻Ｔkにおける全体重心鉛直速度＊ΔTk
…式１６

そして、本実施形態では、歩容生成装置１００は、上記の如く算出した時刻Ｔkでの全体重心鉛直位置と、目標足平位置姿勢と、基準上体姿勢（目標上体姿勢）とを基に、例えば、図６に示した３質点モデルを用いて上体鉛直位置を求める。この場合、目標足平位置姿勢に基づき、３質点モデルにおける２つの脚質点２ａ，２ｂの位置が決定される。さらに、この３質点モデルにおける全体重心の鉛直位置が、上記の如く求めた時刻Ｔkでの全体重心鉛直位置に一致するような上体質点２４ｂの鉛直位置が決定される。そして、この上体質点２４ｂの鉛直位置と目標上体姿勢（基準上体姿勢）とから上体鉛直位置が決定される。

なお、より多くの質点を有する多質点モデル（例えばロボット１の各リンクに質点を有するモデル）を用いることによって、より正確に上体鉛直位置を求めるようにしてもよい。また、上体２４以外の部位の質量が上体２４に比して十分に小さいような場合には、簡略的に、全体重心鉛直位置が、上体２４の重心の鉛直位置に一致するものとみなし、該全体重心鉛直位置と、目標上体姿勢（基準上体姿勢）とから上体鉛直位置を決定するようにしてもよい。

次いでＳ３１４に進み、歩容生成装置１００は、目標ＺＭＰを満足するように（ロボット１の慣性力と重力との合力が目標ＺＭＰまわりに発生するモーメントの水平成分が“０”であるという動力学的平衡条件を満足するように）、時刻Ｔkでの上体水平加速度（上体２４の水平方向の加速度）を決定する。

この時点において、定常歩容（仮の定常歩容）の足平位置姿勢、腕姿勢、上体姿勢および上体鉛直位置の瞬時値（現在時刻Ｔkでの値）が決定されているので、残りの上体水平位置を決定すれば、鉛直軸まわりの角運動量を変化させる腕体の運動自由度を除いて、ロボット１の全体の目標運動が決定されることとなる。したがって、上体水平位置を決定すれば、床反力の鉛直軸まわりのモーメントを除き、すべての床反力も一義的に決定される。

なお、本実施形態では、定常歩容（仮の定常歩容）の目標床反力鉛直成分及び目標ＺＭＰは、それぞれ前記図７のＳ０２２で決定した定常歩容パラメータの床反力鉛直成分軌道パラメータ、目標ＺＭＰ軌道パラメータによって規定される。したがって、上体水平位置の決定に応じて従属的に決定される床反力は、床反力水平成分のみである。

補足すると、本実施形態のロボット１は、各脚体２の自由度が６自由度であることから、目標足平位置姿勢と目標上***置姿勢とを決定すれば、各脚体２の各部の位置姿勢も一義的に決定されることとなる。なお、鉛直軸まわりの角運動量を変化させる腕の運動自由度は、後述するように、スピン力を打ち消すために用いられる。

Ｓ３１４では、例えば図６の動力学モデル（３質点モデル）に係わる前記式０３を用いて上体水平加速度が求められる。より具体的には、現在時刻Ｔkまでに求められた目標足平位置姿勢の時系列を用いて、現在時刻Ｔkでの各脚質点２ａ，２ｂの鉛直加速度及び水平加速度が求められると共に、現在時刻Ｔkでの目標足平位置姿勢を用いて現在時刻Ｔkでの各脚質点２ａ，２ｂの鉛直位置及び水平位置が求められる。

また、現在時刻Ｔkの上体鉛直位置と、時刻（Ｔk−ΔTk)の上体水平位置と、現在時刻Ｔkの目標上体姿勢とから、現在時刻Ｔkでの上体質点２４ｂの鉛直位置及び水平位置が決定される。なお、時刻(Ｔk−ΔTk)までの上体水平位置の時系列又は時刻(Ｔk−ΔTk)での歩容状態を基に時刻Ｔkでの上体水平位置を補間的に推定し、その推定した上体水平位置を時刻(Ｔk−ΔTk)の上体水平位置の代わりに用いてもよい。

また、現在時刻Ｔkでの床反力鉛直成分と、各脚質点２ａ，２ｂの鉛直加速度とをそれぞれ、前記式０１のＦz、d2Zsup/dt2、d2Zswg/dt2に代入してなる式をd2Zb/dt2について解くことによって、現在時刻Ｔkでの上体質点２４ｂの鉛直加速度d2Zb/dt2が求められる。

そして、上記の如く求めた上体質点２４ｂの鉛直位置、水平位置、鉛直加速度と、各脚質点２ａ，２ｂの鉛直位置、水平位置、鉛直加速度、水平加速度をそれぞれ、前記式０３のＺb、Ｘb、d2Zb/dt2、Zsup、Xsup、d2Xsup/dt2、d2Zsup/dt2、Zswg、Xswg、d2Xswg/dt2、d2Zswg/dt2に代入すると共に、現在時刻Ｔkでの目標ＺＭＰの水平位置および鉛直位置を式０３のＸzmp，Ｚzmpに代入し、さらに、同式０３のＭzmp_yを“０”にしてなる式を、d2Xb/dt2について解くことにより、現在時刻Ｔkでの上体質点水平加速度d2Xb/dt2が算出される。そして、この上体質点水平加速度d2Xb/dt2が現在時刻Ｔkでの上体水平加速度として求められる。

尚、より厳密な動力学モデルを用いて、目標ＺＭＰまわりの床反力モーメントの水平成分を“０”にするような上体水平加速度を探索的に求めるようにしてもよい。

次いでＳ３１６に進み、歩容生成装置１００は、Ｓ３１４で求めた上体水平加速度を２階積分することにより現在時刻Ｔkでの上体水平位置を算出する。より詳しくは、上体水平加速度に刻み時間ΔTkを乗じてなる値を時刻Ｔk−ΔTkでの上体水平速度に加算することによって、現在時刻Ｔkでの上体水平速度が求められる。さらにその上体水平速度に刻み時間ΔTkを乗じてなる値を時刻Ｔk−ΔTkでの上体水平位置に加算することによって現在時刻Ｔkでの上体水平位置が求められる。

以上がＳ３０６〜Ｓ３１６で実行される歩容の生成処理の詳細である。

図１３のＳ２０８の処理を完了した後、Ｓ２１０に進み、歩容生成装置１００は、生成した歩容（仮定常歩容）の終端上体水平位置・速度を、その瞬間の支持脚に対応する支持脚座標系（図１０のＸ'''軸及びＹ'''軸を水平方向の２軸とする支持脚座標系）から見た値に変換し、その値を（Ｘe，Ｖxe）とする（Ｘe：終端上体水平位置、Ｖxe：終端上体水平速度）。

次いでＳ２１２に進み、図示の如く、歩容生成装置１００は、初期上体水平位置速度（Ｘs，Ｖxs）と終端上体水平位置速度（Ｘe，Ｖxe）との差を算出する。この差（Ｘs−Ｘe，Ｖxs−Ｖxe）を上体水平位置速度境界条件誤差（errx，errv）と呼ぶ。定常歩容は、前記境界条件を満たす歩容であるので、（Ｘs，Ｖxs）と（Ｘe，Ｖxe）とが一致している必要がある。従って、上体水平位置速度境界条件誤差（errx，errv）が零またはほぼ零である必要がある。本実施形態では、以下のように、上体水平位置速度境界条件誤差（errx，errv）がほぼ零となる（Ｘs，Ｖxs）が探索的に求められる。

次いでＳ２１４に進み、歩容生成装置１００は、上体水平位置速度境界条件誤差（errx，errv）が、あらかじめ適宜設定された許容範囲内にあるか否か（errx，errvのいずれもが、許容範囲内にあるか否か）を判断する。

Ｓ２１４の判断結果が否定的であるときにはＳ２１６に進む。このＳ２１６では、（Ｘs，Ｖxs）の近辺に複数（本実施形態では２つ）の初期値候補（Ｘs＋ΔＸs，Ｖxs），（Ｘs，Ｖxs＋ΔＶxs）が決定される。ここで、ΔＸs，ΔＶxsは、それぞれＸs，Ｖxsに対する所定の微小変化量を意味している。そして、これらの初期値候補のそれぞれを上体水平位置速度の初期状態として、前記Ｓ２０８と同様の処理によって定常歩容（仮定常歩容）が歩容パラメータを用いて生成される。

さらに、その生成された定常歩容の終端上***置速度を、その瞬間の支持脚に対応する支持脚座標系（図１０のＸ'''軸及びＹ'''軸を水平方向の２軸とする支持脚座標系）から見た値に変換した値（Ｘe＋ΔＸe1，Ｖxe＋ΔＶxe1），（Ｘe＋ΔＸe2，Ｖxe＋ΔＶxe2）が求められる。ここで、（Ｘe＋ΔＸe1，Ｖxe＋ΔＶxe1）は（Ｘs＋ΔＸs，Ｖxs）に対応する終端上***置速度を意味し、（Ｘe＋ΔＸe2，Ｖxe＋ΔＶxe2）は（Ｘs，Ｖxs＋ΔＶxs）に対応する終端上***置速度を意味している。

なお、この場合の定常歩容（仮定常歩容）の生成処理では、上体水平位置速度以外の状態量の初期状態は、例えば上体水平位置速度の初期値候補を（Ｘs，Ｖxs）とした場合と同一に設定すればよい。

Ｓ２１６ではさらに、前記Ｓ２１０と同様の処理によって、各初期値候補とこれに対応する終端上***置速度との差、すなわち、各初期値候補（Ｘs＋ΔＸs，Ｖxs），（Ｘs，Ｖxs＋ΔＶxs）のそれぞれに対応する上体水平位置速度境界条件誤差が求められる。

次いでＳ２１８に進み、歩容生成装置１００は、（Ｘs，Ｖxs）およびその付近の初期値候補（Ｘs＋ΔＸs，Ｖxs），（Ｘs，Ｖxs＋ΔＶxs）のそれぞれに対する上体水平位置速度境界条件誤差を基に、（Ｘs，Ｖxs）の次の初期値候補を探索法により決定する。該探索法としては、擬似ヤコビアン（感度マトリックス）を求め、最急降下法などにより次候補を決定する方法や、シンプレックス法などを用いることができる。

例えば、（Ｘs，Ｖxs）およびその付近の初期値候補（Ｘs＋ΔＸs，Ｖxs），（Ｘs，Ｖxs＋ΔＶxs）のそれぞれに対する上体水平位置速度境界条件誤差によって、上体水平位置及び上体水平速度をそれぞれ初期値候補（Ｘs，Ｖxs）から微小変化させたときの、上体水平位置速度境界条件誤差の変化度合いを示す感度マトリックスが求められる。そして、その感度マトリックスに基づいて、上体水平位置速度境界条件誤差をより小さくするような初期値候補（Ｘs，Ｖxs）が新たに決定される。このように上体水平位置速度の新たな初期値候補（Ｘs，Ｖxs）が決定された後、Ｓ２０６に戻る。

Ｓ２１４の判断結果が否定的となる限りは歩容生成装置１００は、Ｓ２０６〜Ｓ２１８の処理を繰り返す。そして、Ｓ２１４の判断結果が肯定的になると、繰り返しループ（Ｓ２０４）を抜けてＳ２２０に進む。この場合、Ｓ２０４の繰り返しループを抜けたときの直前に生成された仮定常歩容が、境界条件を満たす定常歩容として得られることとなる。

Ｓ２２０では、Ｓ２０４の繰り返しループのＳ２０８で最終的に生成した仮定常歩容における初期上体水平位置速度の初期値候補（Ｘs，Ｖxs）と、初期上体鉛直位置速度（Ｚs，Ｖzs）とを、それぞれ求めるべき定常歩容の初期上体水平位置速度（Ｘ0，Ｖx0）、初期上体鉛直位置速度（Ｚ0，Ｖz0）として決定する。

次いでＳ２２２に進み、歩容生成装置１００は、定常歩容初期発散成分（定常歩容の初期時刻Ｔsでの発散成分の値）ｑ0を算出する。

ここで、本実施形態では、発散成分ｑは、例えば前記特許文献１に記載されたものと同様に、次式２０により定義される。

ｑ＝上体水平位置＋上体水平速度／ω0 ……式２０

なお、ω0はあらかじめ設定された所定値である。

そして、Ｓ２２２では、歩容生成装置１００は、Ｓ２２０で決定した初期上体水平位置速度（Ｘ0，Ｖx0）を用いて式２０の右辺の演算を行ない、その演算結果の値を定常歩容初期発散成分ｑ0として決定する。

次いで、Ｓ２２４に進み、歩容生成装置１００は、定常歩容の初期発散成分ｑ0を今回歩容支持脚座標系から見た値に変換し、これをｑ0”として求める。また、歩容生成装置１００は、初期上体鉛直位置速度（Ｚ0，Ｖz0）を今回歩容支持脚座標系から見た値に変換し、これを（Ｚ0”，Ｖz0”）として求める。

補足すると、（Ｚ0”，Ｖz0”）は、第２旋回歩容の支持脚座標系（図１０のＸ”軸、Ｙ”軸を水平面の２軸とする支持脚座標系）から見た第２旋回歩容終端上体鉛直位置速度に一致する。また、ｑ0”も第２旋回歩容の支持脚座標系から見た第２旋回歩容終端発散成分に一致している。したがって、これらの性質を用いて、（Ｚ0”，Ｖz0”）およびｑ0”を算出するようにしても良い。

以上により、図７のＳ０２４の処理、すなわち定常歩容の初期状態を求めるサブルーチン処理が終了する。

次いで、図７のＳ０２６に進み、歩容生成装置１００は、今回歩容の歩容パラメータを決定（一部は仮決定）する。Ｓ０２６では、より具体的には、図１５のフローチャートに示す処理が実行される。

まず、Ｓ６００において、歩容生成装置１００は、今回歩容の足平位置姿勢軌道が定常歩容の足平位置姿勢軌道につながるように、今回歩容の足平軌道パラメータを決定する。

具体的には、今回歩容初期遊脚足平位置姿勢（今回歩容の初期の遊脚足平位置姿勢）は、今回歩容支持脚座標系から見た前回歩容終端支持脚足平位置姿勢（現在の遊脚足平位置姿勢）に設定される。

今回歩容初期支持脚足平位置姿勢（今回歩容の初期の支持脚足平位置姿勢）は、今回歩容支持脚座標系から見た前回歩容終端遊脚足平位置姿勢（現在の支持脚足平位置姿勢）に設定される。

また、今回歩容終端遊脚足平位置姿勢は、今回歩容の遊脚足平２２を、該今回歩容終端遊脚足平位置姿勢から、滑らせないようにピッチ方向に（該足平２２のつま先を下げる向きに）所定角度だけ回転させた状態での該足平２２の位置姿勢が、次回歩容支持脚座標系に対応する前記着地時足平基準位置姿勢に合致するように決定される。

今回歩容終端支持脚足平位置姿勢は、今回歩容の支持脚足平２２を、今回歩容支持脚座標系に対応する着地時足平基準位置姿勢から、滑らせないようにピッチ方向に（該足平２２の踵を持上げる向きに）所定角度だけ回転させた状態での該足平２２の位置姿勢とされる。

次いで、Ｓ６０２に進み、歩容生成装置１００は、今回歩容の上体姿勢軌道パラメータを決定する。この上体姿勢軌道パラメータは、それにより規定される上体姿勢軌道が、定常歩容の上体姿勢軌道に連続してつながる（今回歩容終端での上体姿勢角及び角速度がそれぞれ定常歩容初期の上体姿勢角及び角速度に一致する）ように決定される。この場合、本実施形態では、今回歩容の上体姿勢軌道パラメータにより規定される上体姿勢は、定常歩容の上体姿勢と同様に、定常的に一定姿勢である前記基準上体姿勢（上体２４の体幹軸が鉛直方向を向く姿勢）に設定される。

なお、今回歩容の上体姿勢軌道は、定常歩容の上体姿勢軌道に連続的につながるように設定されておれば、今回歩容の初期から終端まで経時的に変化するように設定してもよい。

次いで、Ｓ６０４に進み、歩容生成装置１００は、今回歩容の腕姿勢軌道パラメータを決定する。この腕姿勢軌道パラメータは、今回歩容の腕姿勢軌道が前記定常歩容の腕姿勢軌道に連続してつながるようにして、定常歩容の腕姿勢軌道パラメータと同様に決定される。

なお、ここで決定される今回歩容の腕姿勢軌道パラメータは、定常歩容の腕姿勢軌道パラメータと同様に、鉛直軸（あるいは上体２４の体幹軸）まわりの両腕体の角運動量変化に関すること以外のパラメータ（例えば上体２４に対する腕体の手先の相対高さや腕体全体の相対重心位置などを規定するパラメータ）である。

次いで、Ｓ６０６に進み、歩容生成装置１００は、今回歩容の床反力鉛直成分軌道パラメータを、それにより規定される床反力鉛直成分軌道が前記図４に示した如く実質的に連続な軌道になるように決定する。ただし、今回歩容の床反力鉛直成分軌道パラメータは、今回歩容のロボット１の全体重心鉛直位置速度と床反力鉛直成分軌道とのいずれもが、前記定常歩容に連続してつながるように決定される。

具体的には、まず、図７のＳ０２４の処理（定常歩容初期状態の決定処理）で最終的に求めた定常歩容の初期上体鉛直位置速度を今回歩容支持脚座標系から見た値に変換した値（Ｚ0”，Ｖz0”）、すなわち、図１３のＳ２２４で求めた（Ｚ0”，Ｖz0”）等を基に、今回歩容支持脚座標系から見た定常歩容の初期全体重心鉛直位置速度が求められる。

例えば、前記した図６の３質点モデルを用いて定常歩容の初期全体重心鉛直位置速度が求められる。この場合、今回歩容支持脚座標系から見た定常歩容初期の目標足平位置姿勢とその時間的変化率（変化速度）とに基づき、３質点モデルにおける２つの脚質点２ａ，２ｂの位置・速度が求められる。さらに、上記（Ｚ0”，Ｖz0”）と、定常歩容初期の上体姿勢角（＝基準上体姿勢角）及びその時間的変化率とから、３質点モデルにおける上体質点２４ｂの位置・速度が求められる。そして、これらの３つの質点２４ｂ，２ａ，２ｂの位置・速度から、その３質点２４ｂ，２ａ，２ｂの重心位置及び速度が、定常歩容の初期全体重心鉛直位置速度として算出される。

なお、より多くの質点を有する多質点モデル（例えばロボット１の各リンクに質点を有する幾何学モデル）を用いることによってより正確に、定常歩容の初期全体重心鉛直位置速度を求めるようにしてもよい。また、上体２４以外の部位の質量が上体２４に比して十分に小さいような場合には、上記（Ｚ0”，Ｖz0”）と、定常歩容初期の上体姿勢角（＝基準上体姿勢角）及びその時間的変化率とから、定常歩容の初期全体重心鉛直位置速度を求めるようにしてもよい。

このようにして求めた定常歩容の初期全体重心鉛直位置速度を、前記式１３と下記式４１の終端全体重心鉛直位置速度に代入すると共に、前回歩容の瞬時値（より詳しくは、前回歩容の終端状態を今回歩容支持脚座標系から見た値）の全体重心鉛直位置および速度を、前記式１３と下記式４１の初期全体重心鉛直位置および速度に代入し、式１３と式４１の関係を満足するように、今回歩容の床反力鉛直成分軌道パラメータが決定される。ただし、式１３と式４１における積分値は今回歩容の初期から終端までの期間の積分値とする。

終端全体重心鉛直速度−初期全体重心鉛直速度
＝（床反力鉛直成分/ロボットの質量）の１階積分
＋重力加速度の１階積分
…式４１
（ただし、重力加速度は負の値とする。）

より具体的には、まず、図４に示したような床反力鉛直成分軌道を規定する床反力鉛直成分軌道パラメータ（折れ点の時刻等）のうちの、少なくとも２つのパラメータ（例えば、床反力鉛直成分の最大値と最小値）を独立な未知変数として、その未知変数の値を、式１３と式４１とからなる連立方程式を解くことによって決定する。

次いで、Ｓ６０８に進み、歩容生成装置１００は、今回歩容のＺＭＰ軌道が、安定余裕が高くかつ急激な変化をしないように、該ＺＭＰ軌道を規定するＺＭＰ軌道パラメータ（ＺＭＰ軌道の折れ点の時刻や位置等）を決定する。例えば、ＺＭＰ軌道が、図５に示したようなパターンになるようにＺＭＰ軌道パラメータが仮決定される。ただし、今回歩容のＺＭＰ軌道が前記定常歩容のＺＭＰ軌道に連続してつながるように（今回歩容終端におけるＺＭＰの位置が定常歩容初期のＺＭＰの位置に一致するように）、今回歩容のＺＭＰ軌道パラメータが仮決定（暫定的に決定）される。

なお、Ｓ６０８で決定される今回歩容のＺＭＰ軌道パラメータは、仮決定されたものであり、後述するように修正される。以降、その修正が終了するまでの仮決定されたＺＭＰ軌道パラメータにより規定される今回歩容の目標ＺＭＰを、以降、仮目標ＺＭＰと呼ぶ。その仮決定されたＺＭＰ軌道パラーメータを含む今回歩容の歩容パラメータを仮今回歩容パラメータという。

図７の説明に戻って、歩容生成装置１００は、上記のごとくＳ０２６の処理を行った後、次いでＳ０２８において、今回歩容の歩容パラメータ（詳しくはＺＭＰ軌道パラメータ）を修正する処理を実行する。この処理では、今回歩容の上***置姿勢軌道を定常歩容に連続させ、もしくは近づけるべく歩容パラメータのうちのＺＭＰ軌道パラメータが修正される。

この処理は、図１６のフローチャートに示す如く実行される。

まずＳ７００を経てＳ７０２に進み、歩容生成装置１００は、仮決定されたＺＭＰ軌道パラメータを含む仮今回歩容パラメータ（暫定的な今回歩容パラメータ）を基に、今回歩容の終端時刻までの仮今回歩容を生成する。

Ｓ７０２では、図１７のフローチャートに示す処理が実行される。

以下説明すると、歩容生成装置１００は、まず、Ｓ８００において、仮今回歩容の生成用時刻Ｔk等の初期化を行なう。具体的には、仮今回歩容の生成用時刻Ｔkが現在時刻ｔに初期化される。また、時刻ｔ−Δｔで歩容生成装置１００が既に決定した目標上***置及び目標上体速度、目標上体姿勢角及びその角速度、目標足平位置姿勢、目標腕姿勢など、時刻ｔ−Δｔでの目標歩容の運動状態（今回歩容支持脚座標系で見た運動状態）が、仮今回歩容の現在時刻ｔの直前の初期運動状態として設定される。

次いで、Ｓ８０２を経てＳ８０４において、歩容生成装置１００は、仮今回歩容の生成用時刻k（現在値）が今回歩容の終端時刻Tcurr以前の時刻であるか否か（Ｔk≦Tcurrであるか否か）を判断する。この判断結果が肯定的である場合には、歩容生成装置１００は、Ｓ８０６〜Ｓ８１６の処理（詳細は後述する）を実行することによって、時刻Ｔkでの歩容の瞬時値を決定する。

次いでＳ８１８に進み、歩容生成装置１００は、仮今回歩容の生成用時刻Ｔkを、所定の刻み時間ΔＴkだけ増加させた後、Ｓ８０４の判断を再び行なう。ここで、刻み時間ΔTkは、例えば制御周期Δｔと一致させておけばよい。ただし、演算量を低減するために、ΔTkをΔｔよりも長い時間に設定してもよい。

Ｓ８０４の判断結果が肯定的である場合には、Ｓ８０６からＳ８１８までの処理が繰り返され、Ｓ８０４の判断結果が否定的になった場合には、図１７の処理、すなわち、図１６のＳ７０２の処理は終了する。これにより、今回歩容の初期時刻から終端時刻までの仮今回歩容の生成が完了する。

歩容生成装置１００は、上記Ｓ８０６〜Ｓ８１６で仮今回歩容の瞬時値を決定する処理を次のように実行する。この仮今回歩容の瞬時値の決定の仕方は、図１４のＳ３０６〜Ｓ３１６で定常歩容（仮の定常歩容）の瞬時値を決定する仕方と同様である。

まずＳ８０６において、歩容生成装置１００は、仮今回歩容パラメータ（より詳しくは床反力鉛直成分軌道パラメータ）を基に、図４に示した目標床反力鉛直成分軌道の時刻Ｔkでの瞬時値を求める。

次いで、Ｓ８０８において、歩容生成装置１００は、仮今回歩容パラメータ（より詳しくは仮決定のＺＭＰ軌道パラメータ）を基に、図５に示した目標ＺＭＰ軌道（仮目標ＺＭＰ軌道）の時刻Ｔkでの瞬時値を求める。

次いでＳ８１０に進み、歩容生成装置１００は、仮今回歩容パラメータ（より詳しくは足平軌道パラメータ、上体姿勢軌道パラメータ、及び腕姿勢軌道パラメータ）を基に、目標足平位置姿勢、目標上体姿勢および目標腕姿勢の時刻Ｔkでの瞬時値をそれぞれ求める。ただし、目標腕姿勢に関しては、より詳しくは両腕体の全体重心位置が決定されるが、鉛直軸（または上体２４の体幹軸）まわりの角運動量を変化させる腕体の運動（腕振り運動）は、未だ決定されない。

次いでＳ８１２に進み、歩容生成装置１００は、Ｓ８０６で求めた目標床反力鉛直成分を満足する（ロボット１の全体重心の鉛直方向の慣性力と重力との和を目標床反力鉛直成分に釣り合わせる）ように、時刻Ｔkでの上体鉛直位置を算出する。その算出の仕方は、図１４のＳ３１２（定常歩容の上体鉛直位置の瞬時値を算出する処理）と同じでよい。ただし、Ｓ８１２で使用する多質点モデルは、Ｓ３１２で使用するものと異ならせてもよい。

次いでＳ８１４に進み、歩容生成装置１００は、仮今回歩容の時刻Ｔkでの上体水平加速度を決定する処理を実行する。

具体的には、仮今回歩容の生成用の時刻Ｔkが現在時刻ｔ（歩容生成装置１００の現在の制御周期の時刻）に一致する場合には、歩容生成装置１００は、全モデル操作床反力モーメント分配器１１８から与えられた第１モデル操作床反力モーメント（現在の制御周期での値）が目標ＺＭＰまわりに発生するように（ロボット１の慣性力と重力との合力が目標ＺＭＰまわりに発生するモーメントの水平成分が第１モデル操作床反力モーメントに釣り合うように）、仮今回歩容の時刻Ｔkでの上体水平加速度を決定する。

言い換えると、ロボット１の慣性力と重力との合力が目標ＺＭＰまわりに発生するモーメントが第１モデル操作床反力モーメントの符号を反転したモーメントに一致するように、時刻Ｔkでの上体水平加速度が決定される。

また、仮今回歩容の生成用の時刻Ｔkが、現在時刻ｔよりも将来の時刻である場合（Ｔk＞現在時刻ｔである場合）には、歩容生成装置１００は、目標ＺＭＰを満足するように（ロボット１の慣性力と重力との合力が目標ＺＭＰまわりに発生するモーメントの水平成分が“０”であるという動力学的平衡条件を満足するように）、時刻Ｔkでの上体水平加速度を決定する。

これらの場合において、仮今回歩容の時刻Ｔkでの上体水平加速度を算出する処理は、例えば図６の動力学モデル（３質点モデル）を用いて行なわれる。

具体的には、仮今回歩容の時刻Ｔkまでの目標足平位置姿勢の時系列を用いて、時刻Ｔkでの各脚質点２ａ，２ｂの鉛直加速度及び水平加速度が求められると共に、時刻Ｔkでの目標足平位置姿勢を用いて時刻Ｔkでの各脚質点２ａ，２ｂの鉛直位置及び水平位置が求められる。

また、仮今回歩容の時刻Ｔkの上体鉛直位置と、時刻（Ｔk−ΔＴk)の上体水平位置と、時刻Ｔkでの目標上体姿勢とから、時刻Ｔkでの上体質点２４ｂの鉛直位置及び水平位置が決定される。なお、時刻Ｔkが現在時刻ｔである場合には、時刻（Ｔk−ΔＴk)の上体水平位置としては、Ｓ８００で設定した初期運動状態での上体水平位置が用いられる。

補足すると、時刻(Ｔk−ΔＴk)までの上体水平位置の時系列又は時刻(Ｔk−ΔＴk)での歩容状態を基に時刻Ｔkでの上体水平位置を補間的に推定し、その推定した上体水平位置を時刻(Ｔk−ΔＴk)の上体水平位置の代わりに用いてもよい。

また、時刻Ｔkでの床反力鉛直成分と、各脚質点２ａ，２ｂの鉛直加速度とをそれぞれ、前記式０１のＦz、d2Zsup/dt2、d2Zswg/dt2に代入してなる式をd2Zb/dt2について解くことによって、時刻Ｔkでの上体質点２４ｂの鉛直加速度d2Zb/dt2が求められる。

そして、Ｔk＝現在時刻ｔである場合には、上記の如く求めた上体質点２４ｂの鉛直位置、水平位置、鉛直加速度と、各脚質点２ａ，２ｂの鉛直位置、水平位置、鉛直加速度、水平加速度とをそれぞれ、前記式０３のＺb、Ｘb、d2Zb/dt2、Zsup、Xsup、d2Xsup/dt2、d2Zsup/dt2、Zswg、Xswg、d2Xswg/dt2、d2Zswg/dt2に代入すると共に、時刻Ｔkでの目標ＺＭＰの水平位置および鉛直位置を式０３のＸzmp，Ｚzmpに代入し、さらに、同式０３のＭzmp_yをモデル操作床反力モーメント（現在値）に一致させてなる式を、d2Xb/dt2について解くことにより、時刻Ｔkでの上体質点水平加速度d2Xb/dt2が算出される。そして、この上体質点水平加速度d2Xb/dt2が時刻Ｔkでの上体水平加速度として求められる。

また、Ｔk＞現在時刻ｔである場合には、式０３のＭzmp_yを、“０”としてなる式をd2Xb/dt2について解くことにより、時刻Ｔkでの上体水平加速度が算出される。この場合、式０３のＭzmp_yを、“０”とすること以外の処理は、Ｔk＝現在時刻ｔである場合と同様である。

尚、より厳密な動力学モデルを用いて、目標ＺＭＰまわりの床反力モーメントの水平成分をモデル操作床反力モーメントに一致させ、あるいは、“０”とするような上体水平加速度を探索的に求めるようにしてもよい。

次いでＳ８１６に進み、歩容生成装置１００は、Ｓ８１４で求めた上体水平加速度を２階積分することにより現在時刻Ｔkでの上体水平位置を算出する。この算出の仕方は、図１４のＳ３１６と同じである。

以上が図１６のＳ７０２で歩容生成装置１００が実行する処理（仮今回歩容の生成処理）の詳細である。

歩容生成装置１００は、以上のごとくＳ７０２の処理を実行した後、次にＳ７０４の処理を実行する。

Ｓ７０４では、歩容生成装置１００は、Ｓ７０２で生成した仮今回歩容の終端での上体水平位置速度（Ｘe，Ｖxe）を基に、終端発散成分ｑe1（仮今回歩容の終端での発散成分ｑe1）を算出する。

具体的には、歩容生成装置１００は、まず、（Ｘe，Ｖxe）と仮今回歩容の終端での上体姿勢（本実施形態では基準上体姿勢）とから、図６の倒立振子モデルの上体質点２４ｂの水平位置速度（水平位置及び水平速度）を決定する。そして、この上体質点水平位置速度から、前記した図１３のＳ２２２と同様に、前記式１１８に基づいて、仮今回歩容の終端発散成分ｑe1を算出する。この場合に、ｑe1を算出するために用いるΓ^-1は、Ｓ２２２の処理で算出したものと同じである。

次いでＳ７０６に進み、歩容生成装置１００は、上記の如く求めた仮今回歩容の終端発散成分ｑe1と、図１４のＳ２２４で求めた定常歩容の初期発散成分ｑ0”との差である終端発散成分誤差errq（＝ｑe1−ｑ0''）を算出する。

さらに、Ｓ７０８に進んで、歩容生成装置１００は、上記の如く求めた終端発散成分誤差errqが許容範囲内（“０”近傍の範囲内）にあるか否か、すなわち、errqが“０”もしくはほぼ“０”であるか否かを判断する。なお、Ｓ７０８の判断は、詳しくは、Ｘ軸方向及びＹ軸方向の各軸方向の終端発散成分誤差errqに対して行なわれる。そして、Ｘ軸方向及びＹ軸方向の両軸方向の終端発散成分誤差errqが許容範囲内にある場合にＳ７０８の判断結果が肯定的になり、いずれかの軸方向の終端発散成分誤差errqが許容範囲内に無い場合には、がＳ７０８の判断結果が否定的になる。

このＳ７０８の判断結果が否定的である場合には、Ｓ７１０に進み、歩容生成装置１００は、ａ＝Δａ（Δａ：所定の微小量）として、現在の仮目標ＺＭＰ軌道を台形パターンの修正量によって図１８に示す如く修正してなる目標ＺＭＰ軌道を用いて、前記Ｓ７０２と同様に、終端時刻までの仮今回歩容を生成する。すなわち、歩容生成装置１００は、Ｓ７０２での仮今回歩容の生成に用いた仮目標ＺＭＰ軌道の代わりに、それを修正してなる目標ＺＭＰ軌道を用いて、改めて仮今回歩容を生成する。この場合、目標ＺＭＰ以外の今回歩容パラメータは、Ｓ７０２で使用したものと同じである。

ここで、図１８を参照して、上記「ａ」は、仮今回歩容の終端発散成分を定常歩容の初期発散成分にできるだけ一致させる（ひいては今回歩容の上体水平位置姿勢軌道を定常歩容の上体水平位置姿勢軌道に収束させる）ために、仮目標ＺＭＰ軌道を修正する（仮目標ＺＭＰ軌道に加える）台形パターンの修正量（以下、ＺＭＰ修正量ということがある）の高さである。

この場合、本実施形態では、仮目標ＺＭＰ軌道の修正は、支持脚足平２２の底面のほぼ全面が接地する期間（足底全面接地期間）において行なうようにしている。このため、ＺＭＰ修正量は、足底全面接地期間内において、ＺＭＰ修正量≠０となり、足底全面接地期間以外の期間では、ＺＭＰ修正量＝０となるように設定されている。また、上記台形パターンのＺＭＰ修正量の折れ点の時刻は、足底全面接地期間における仮目標ＺＭＰ軌道の折れ点の時刻に合わせて設定されている。なお、Ｓ７１０でａ＝Δａ（Δａ：所定の微小量）とするのは、現在の仮目標ＺＭＰ軌道を上記台形パターンのＺＭＰ修正量によって微小量だけ、修正したときの前記終端発散成分誤差errqの変化を観測するためである。

補足すると、Ｓ７１０の上記の処理では、目標ＺＭＰのＸ軸方向位置を修正することを例に採って説明したが、実際には目標ＺＭＰのＹ軸方向位置の修正も併せて行なわれる。そのＹ軸方向位置の修正は、Ｘ軸方向位置の修正と同様に、足底全面接地期間における目標ＺＭＰ軌道を仮目標ＺＭＰ軌道から台形パターンで変化させるように行なわれる。この場合、上記Δａは各軸方向で同じ値でよいが、互いに異なる値に設定してもよい。

なお、Ｘ軸方向及びＹ軸方向の一方の軸方向での目標ＺＭＰ軌道の修正が、他方の軸方向での終端発散成分に影響を及ぼさないか、もしくはその影響が十分に小さい場合には、Ｓ７１０から、後述のＳ７１６までの処理をＸ軸方向とＹ軸方向とで各別に行なうようにしてもよい。

上記のようにＳ７１０にて仮今回歩容を生成した後、次にＳ７１２に進み、歩容生成装置１００は、Ｓ７１０で求めた仮今回歩容の終端での上体水平位置速度（Ｘe2，Ｖxe2）を基に、この仮今回歩容における終端発散成分ｑe2をＳ７０４と同様に算出する。

尚、本実施形態では、Ｓ７１０においてΔａを所定値の微小量としたが、以下に説明する繰り返し演算によって終端発散成分誤差errqが小さくなるにつれてΔａを“０”に近づけるように、該Δａを可変的に設定するようにしてもよい。ただし、一般には、Δａを定数にしておいても、数回の繰り返し演算で終端発散成分誤差errqを許容範囲内に収めることができる。

次いでＳ７１４に進み、歩容生成装置１００は、パラメータ感度ｒ（Δａに対する終端発散成分誤差の変化の割合）を図中の式によって算出する。すなわち、歩容生成装置１００は、Ｓ７１２で算出した終端発散成分ｑe2とＳ７０４で算出した終端発散成分ｑe1との差（＝ｑe2−ｑe1）をΔａにより除算することによって、パラメータ感度ｒを算出する。この場合、より詳しくは、例えば（ｑe2−ｑe1）のＸ軸方向成分を、Ｘ軸方向のＺＭＰ修正量に係わるΔａにより除算することによって、Ｘ軸方向でのパラメータ感度ｒが算出される。また、（ｑe2−ｑe1）のＹ軸方向成分を、Ｙ軸方向のＺＭＰ修正量に係わるΔａにより除算することによって、Ｙ軸方向でのパラメータ感度ｒが算出される。

次いでＳ７１６に進み、歩容生成装置１００は、ａ＝−errq／r、すなわちＳ７０６で求めた終端発散成分誤差errqを、Ｓ７１４で求めたパラメータ感度ｒで割った値の符号を反転させてなる値を台形パターンのＺＭＰ修正量の高さａとして、このＺＭＰ修正量によって図１８に示す如く仮目標ＺＭＰパターンを修正してなる目標ＺＭＰを、改めて仮目標ＺＭＰとして決定する。この場合、ＺＭＰ修正量の高さａは、Ｘ軸方向とＹ軸方向とで各別に算出される。

なお、Ｘ軸方向及びＹ軸方向の一方の軸方向での目標ＺＭＰ軌道の修正が、他方の軸方向での終端発散成分に影響を及ぼす場合には、その影響分を考慮して各軸方向でのＺＭＰ修正量の高さａを決定することが好ましい。

次いで歩容生成装置１００は、Ｓ７０２からの処理を再び実行する。このとき、Ｓ７０８の判断結果が否定的である限りは、上記したＳ７０２〜Ｓ７１６の処理が繰り返され、Ｓ７０８の判断結果が肯定的となった場合に、その繰り返しループ（Ｓ７００）を抜けてＳ７１８に進む。

以上のＳ７０２〜Ｓ７１６のループ処理によって、終端発散成分ｑe1を、定常歩容の初期発散成分ｑ0''に一致もしくはほぼ一致させることができるＺＭＰ修正量が（ひいては今回歩容の目標ＺＭＰ軌道が）探索的に求められる。

かかるループ処理の次のＳ７１８では、歩容生成装置１００は、現在の仮目標ＺＭＰ軌道（Ｓ７００の繰り返しループを抜ける直前で設定された仮目標ＺＭＰ軌道）を最終的に今回歩容の目標ＺＭＰ軌道として、これに対応するＺＭＰ軌道パラメータを決定する。これにより、図７のＳ０２８の処理（今回歩容パラメータの修正処理）が終了する。

なお、本実施形態では、ＺＭＰ修正量を台形パターンに設定したが、例えば、三角形状のパターンや、曲率が連続的に変化するようなパターン等に決定してもよい。

図７に戻って、上記のごとくＳ０２８において今回歩容パラメータを修正した後、Ｓ０３０に進み、歩容生成装置１００は、今回歩容の初期から終端までの各時刻での目標ＺＭＰの位置（全床反力中心点の目標位置）を制限するＺＭＰ存在許容領域（全床反力中心点の存在許容領域）を決定する。ＺＭＰ存在許容領域は、前記フィードバック操作量演算部１１０において、後述する如く使用されるものであると共に、後述するＳ０３１の判断処理で使用されるものである。

この場合、ＺＭＰ存在許容領域は、所謂、支持多角形内に設定される。より具体的には、歩容生成装置１００は、Ｓ０２６で決定した今回歩容パラメータのうちの足平軌道パラメータを基に、今回歩容の各足平２２の足平位置姿勢軌道を算出し、この足平位置姿勢軌道に基づいて、現在時刻（現在の制御周期の時刻）以後の各時刻（所定の刻み時間毎の各時刻）での各足平２２の接地面（床面との接触面）を求める。そして、歩容生成装置１００は、この接地面を基に、現在時刻以後の各時刻での支持多角形（上記接地面を含む最小の凸多角形）を決定する。該支持多角形は、片脚支持期では、支持脚足平２２の接地面に一致し、両脚支持期では、両足平２２，２２のそれぞれの接地面を連接した領域となる。

そして、歩容生成装置１００は、このように決定した各時刻の支持多角形内に、その時刻でのＺＭＰ存在許容領域を、所定の規則に従って設定する。

該ＺＭＰ存在許容領域は、支持多角形と同じ領域（外形状及び面積が支持多角形に一致する領域）でもよいが、本実施形態では、例えば、支持多角形よりも面積が若干小さい領域で、且つ、その外周の境界線が、支持多角形の外周の境界線と若干の間隔を有するような領域がＺＭＰ存在許容領域として設定される。換言すれば、本実施形態では、ＺＭＰ存在許容領域は、支持多角形内に包含され、且つ、その面積が支持多角形に比して小さくなり過ぎないような領域に設定される。

この場合、本実施形態では、片脚支持期においては、例えば方形状のＺＭＰ存在許容領域が支持多角形（支持脚足平２２の接地面）内に設定される。

一方、両脚支持期においては、ＺＭＰ存在許容領域は、例えば、支持多角形と概ね相似するような形状に設定される。ここで、両脚支持期における支持多角形は、例えば図１９に例示する如く、一般には、方形状と異なる形状となる。なお、図示例では、右足平２２Ｒのつま先が接地し、左足平２２Ｌのかかとが接地している状態（両脚支持期の開始直後の状態）を模式的に示している。

従って、本実施形態では、両脚支持期におけるＺＭＰ存在許容領域は、支持多角形が方形状と異なる形状となる状況では、図１９に例示する如く、方形状と異なる外形状の領域に設定される。

補足すると、ＺＭＰ存在許容領域は、支持多角形と相似する形状である必要はない。例えば、図１９に例示する支持多角形は、四角形であるが、その内部に設定するＺＭＰ存在許容領域の外形状は、５つ以上の頂点を有する多角形であってもよい。

また、両脚支持期の全期間において、ＺＭＰ存在許容領域が方形状と異なる形状である必要なない。さらに、片脚支持期においては、ＺＭＰ存在許容領域を方形状と異なる形状の領域に設定してもしてもよい。

また、ＺＭＰ存在許容領域を、あらかじめ定めた所定数の頂点を有する多角形（例えば四角形）に限定し、その多角形の各頂点の位置を支持多角形内に設定することで、ＺＭＰ存在許容領域を設定するようにしてもよい。

また、ＺＭＰ存在許容領域を、円のような曲線で設定するようにしてもよい。

上記のごとくＳ０３０においてＺＭＰ存在許容領域を決定した後、Ｓ０３１に進み、歩容生成装置１００は、Ｓ０２８で決定したＺＭＰ軌道パラメータにより規定される目標ＺＭＰ軌道が適切な軌道であるか否かを判断する。この場合、本実施形態では、歩容生成装置１００は、今回歩容のＺＭＰ軌道パラメータにより規定される目標ＺＭＰ軌道の各瞬時値（所定の刻み時間毎の瞬時値）が、前記Ｓ０３０で決定したＺＭＰ存在許容領域内に存在するか否かによって、該目標ＺＭＰ軌道が適切な軌道であるか否かを判断する。

より具体的には、歩容生成装置１００は、現在時刻以後の各時刻での今回歩容の目標ＺＭＰが、その時刻でのＺＭＰ存在許容領域内に存在する場合には、今回歩容のＺＭＰ軌道パラメータにより規定される目標ＺＭＰ軌道が適切な軌道であると判断する。また、今回歩容のいずれかの時刻での目標ＺＭＰが、その時刻でのＺＭＰ存在許容領域を逸脱する場合には、今回歩容のＺＭＰ軌道パラメータにより規定される目標ＺＭＰ軌道が適切な軌道でないと判断する。

Ｓ０３１の判断結果が否定的である場合（目標ＺＭＰ軌道が適切な軌道でない場合）には、歩容生成装置１００は、Ｓ０２０からの処理を改めて実行する。この場合、Ｓ０２０においては、歩容生成装置１００は、例えば、今回歩容の遊脚足平２２の着地予定位置姿勢（ひいては、次回歩容支持脚座標系の位置姿勢）を、現在値（今回の制御周期における１回前のＳ０２０の処理で決定した着地予定位置姿勢）からずらした位置姿勢に変更する。なお、ここで変更する着地予定位置姿勢は、より詳しくは、着地予定位置及び着地予定姿勢のうちのすくなくともいずれか一方である。この場合、該着地予定位置姿勢の変更量は、前記Ｓ０３１の判断処理において、ＺＭＰ存在許容領域から逸脱した目標ＺＭＰの、当該逸脱方向やその逸脱量に応じて決定される。該変更量は、基本的には、変更後の着地予定位置姿勢に応じて規定される支持多角形内に目標ＺＭＰが含まれるか、もしくは、含まれやすくなるように決定される。

なお、今回歩容の遊脚足平２２の着地予定位置姿勢だけでなく、次回歩容の遊脚足平２２の着地予定位置姿勢（ひいては次次回歩容支持脚座標系の位置姿勢）をも変更するようにしてもよい。さらに、今回歩容又は次回歩容の遊脚足平２２の着地予定位置姿勢の変更に合わせて、該遊脚足平２２の着地予定時刻を変更するようにしてもよい。

このようにして、歩容生成装置１００は、Ｓ０２０の処理を改めて実行した後、Ｓ０２２以降の処理を前記した通りの仕方で、再び実行する。すなわち、Ｓ０３１の判断結果が肯定的になるまで、Ｓ０２０〜Ｓ０３０の処理が繰り返される。

図７に戻って、Ｓ０３１の判断結果が肯定的である場合、あるいは、Ｓ０１６の判断結果が否定的である場合には、次にＳ０３２において、歩容生成装置１００は、今回歩容瞬時値（現在の制御周期での今回歩容の目標足平位置姿勢等の瞬時値）を決定する。このＳ０３２では、全モデル操作床反力モーメント分配器１１８で後述する如く決定される第１モデル操作床反力モーメントが目標ＺＭＰまわりに発生するように今回歩容瞬時値が決定される。

具体的には、図２０のフローチャートに示す如く、歩容瞬時値が決定される。以下説明すると、歩容生成装置１００は、まず、Ｓ１０００からＳ１００６まで、図１７のＳ８０６からＳ８１２まで（または図１４のＳ３０６からＳ３１２まで）と同様に処理を実行することによって、今回歩容の現在時刻ｔ（現在の制御周期の時刻）での目標床反力鉛直成分、目標ＺＭＰ、目標足平位置姿勢、目標上体姿勢、目標腕姿勢、目標上体鉛直位置の瞬時値を算出する。

これらの各瞬時値の算出の仕方は、歩容パラメータとして、図７のＳ０２８で最終的に修正した今回歩容パラメータを使用する点を除いて、図１７のＳ８０６からＳ８１２まで（または図１４のＳ３０６からＳ３１２まで）の処理と同じである。

次いで、Ｓ１００８に進み、歩容生成装置１００は、Ｓ１００４で算出した現在時刻ｔの目標足平位置姿勢を基に、前記Ｓ０３０と同様の処理によって、今回歩容の現在時刻ｔでのＺＭＰ存在許容領域を求める。このＺＭＰ存在許容領域は、歩容生成装置１００から、図３に示した前記フィードバック操作量演算部１１０（詳細は後述する）に出力される。

なお、詳細は後述するが、フィードバック操作量演算部１１０では、入力されたＺＭＰ存在許容領域等を用いて、前記全モデル操作床反力モーメント及びコンプライアンス制御用目標床反力モーメントが決定される。さらに、決定された全モデル操作床反力モーメントは、前記全モデル操作床反力モーメント分配器１１８に入力され、該分配器１１８により第１モデル操作床反力モーメント及び第２モデル操作床反力モーメントが決定される。そして、決定された第１モデル操作床反力モーメントが、歩容生成装置１００の制御周期で、歩容生成装置１００によりサンプリングされる（該歩容生成装置１００に取り込まれる）。

補足すると、本実施形態では、Ｓ１００８で決定する現在時刻ｔでのＺＭＰ存在許容領域は、Ｓ０３１の判断結果が肯定的となったときに、Ｓ０３０で決定されたＺＭＰ存在許容領域のうちの現在時刻ｔに対応する時刻でのＺＭＰ存在許容領域と同じである。従って、例えば現在時刻ｔが両脚支持期内の時刻である場合に、Ｓ１００８で決定されるＺＭＰ存在許容領域は、Ｓ０３０の処理に関して説明した如く、方形状と異なる外形状の領域に決定されることとなる。但し、Ｓ１００８で決定するＺＭＰ存在許容領域と、Ｓ０３１の判断結果が肯定的となったときに、Ｓ０３０で決定された現在時刻ｔでのＺＭＰ存在許容領域とは完全に一致する必要はない。例えば、それらのＺＭＰ存在許容領域の面積が若干、異なっていてもよい。

次いで、Ｓ１０１０に進み、歩容生成装置１００は、全モデル操作床反力モーメント分配器１１８から与えられた第１モデル操作床反力モーメント（最新値）が目標ＺＭＰまわりに発生するように（ロボット１の慣性力と重力との合力が目標ＺＭＰまわりに発生するモーメントの水平成分が第１モデル操作床反力モーメントに釣り合うように）、今回歩容の現在時刻ｔでの上体水平加速度を決定する。

言い換えると、ロボット１の慣性力と重力との合力が目標ＺＭＰまわりに発生するモーメントが第１モデル操作床反力モーメントの符号を反転したモーメントに一致するように、今回歩容の現在時刻ｔでの上体水平加速度が決定される。

この場合、図１７のＳ８１４の処理と同様の仕方で、例えば図６の動力学モデル（３質点モデル）を用いて今回歩容の上体水平加速度が算出される。

具体的には、今回歩容の現在時刻ｔまでの目標足平位置姿勢の時系列を用いて、現在時刻ｔでの各脚質点２ａ，２ｂの鉛直加速度及び水平加速度が求められると共に、現在時刻ｔでの目標足平位置姿勢を用いて現在時刻ｔでの各脚質点２ａ，２ｂの鉛直位置及び水平位置が求められる。

また、今回歩容の現在時刻ｔで上体鉛直位置と、時刻（ｔ−Δｔ)の上体水平位置と、現在時刻ｔの目標上体姿勢とから、現在時刻ｔでの上体質点２４ｂの鉛直位置及び水平位置が決定される。なお、時刻(ｔ−Δｔ)までの上体水平位置の時系列又は時刻(ｔ−Δｔ)での歩容状態を基に時刻ｔでの上体水平位置を補間的に推定し、その推定した上体水平位置を時刻(ｔ−Δｔ)の上体水平位置の代わりに用いてもよい。

また、今回歩容の現在時刻ｔでの床反力鉛直成分と、各脚質点２ａ，２ｂの鉛直加速度とをそれぞれ、前記式０１のＦz、d2Zsup/dt2、d2Zswg/dt2に代入してなる式をd2Zb/dt2について解くことによって、現在時刻ｔでの上体質点２４ｂの鉛直加速度d2Zb/dt2が求められる。

そして、上記の如く求めた上体質点２４ｂの鉛直位置、水平位置、鉛直加速度と、各脚質点２ａ，２ｂの鉛直位置、水平位置、鉛直加速度、水平加速度とをそれぞれ、前記式０３のＺb、Ｘb、d2Zb/dt2、Zsup、Xsup、d2Xsup/dt2、d2Zsup/dt2、Zswg、Xswg、d2Xswg/dt2、d2Zswg/dt2に代入すると共に、現在時刻ｔでの目標ＺＭＰの水平位置および鉛直位置を式０３のＸzmp，Ｚzmpに代入し、さらに、同式０３のＭzmp_yをモデル操作床反力モーメントに一致させてなる式を、d2Xb/dt2について解くことにより、現在時刻ｔでの上体質点水平加速度d2Xb/dt2が算出される。そして、この上体質点水平加速度d2Xb/dt2が現在時刻ｔでの上体水平加速度として求められる。

尚、より厳密な動力学モデルを用いて、目標ＺＭＰまわりの床反力モーメントの水平成分を第１モデル操作床反力モーメントに一致させるような上体水平加速度を探索的に求めるようにしてもよい。

次いで、Ｓ１０１２に進み、Ｓ１０１０で上記の如く求めた上体水平加速度を２階積分することにより現在時刻ｔでの上体水平位置を算出する。この算出の仕方は、図１４のＳ３１６（又は図１７のＳ８１６）と同じである。これにより、図７のＳ０３２の処理が完了する。

次いでＳ０３４に進み、スピン力をキャンセルする（ロボット１の腕体以外の運動によって目標ＺＭＰ回りに発生する床反力モーメント鉛直成分を“０”もしくはほぼ“０”にする）ための腕体の動作が決定される。具体的には、腕体を振らなかった場合の目標ＺＭＰにおける床反力モーメント鉛直成分軌道（厳密には、腕体を振らずに歩容を生成した場合において、ロボットの重力と慣性力の合力が目標ＺＭＰに作用するモーメント鉛直成分軌道の各瞬時値の符号を反転したもの）が求められる。すなわち、Ｓ０３２の処理によって生成された今回歩容の運動（これには腕振りの運動は含まれていない）の瞬時値に釣り合う目標ＺＭＰ回りの床反力モーメント鉛直成分の瞬時値が求められる。そして、この床反力モーメント鉛直成分の瞬時値を、腕振り運動の等価慣性モーメントで割ることにより、スピン力キャンセルに必要な腕振り動作の角加速度が求められる。なお、腕の振りが大き過ぎる場合には、等価慣性モーメントよりも大きな値で床反力モーメント鉛直成分の瞬時値を除算するようにしてもよい。

次に、歩容生成装置１００は、この角加速度を２階積分し、これを積分値が過大になるのを防ぐためのローカットフィルタに通して得た角度を腕振り動作角とする。ただし、腕振り動作では、左右の腕体を前後逆方向に振り、両腕体の重心位置を変化させないようにする。なお、スピン力をキャンセルするための腕振り運動を定常歩容でも生成しておき、これにつながるように、今回歩容における腕振り運動を決定するようにしてもよい。

次いでＳ０３６に進み、歩容生成装置１００は、時刻ｔに制御周期Δｔを加え、Ｓ０１４からの処理を再び実行する。

以上により、今回歩容の瞬時値が時系列的に生成されることとなる。

この場合、本実施形態では、今回歩容の運動の軌道（時系列）は、目標床反力鉛直成分軌道パラメータ（図１５のＳ６０６で決定されたパラメータ）により規定される目標床反力鉛直成分軌道を満足する（ロボット１の全体重心の鉛直方向の慣性力と該全体重心に作用する重力との総和が目標床反力鉛直成分に釣り合う）と共に、図６に示した歩容生成用の動力学モデル（本実施形態では倒立振子モデル）において、目標ＺＭＰまわりに前記第１モデル操作床反力モーメントが付加的に発生するように決定される。

加えて、今回歩容の運動の軌道は、将来において、周期的な歩容である定常歩容の運動軌道に収束するように（漸近するように）決定される。より具体的には、今回歩容の運動の軌道は、今回歩容の終端における発散成分が、定常歩容の初期の発散成分に一致するように決定される。

次に、説明を後回しにしたフィードバック操作量演算部１１０、全モデル操作床反力モーメント分配器１１８、及び摂動モデル演算部１２０の処理をさらに詳細に説明する。

まず、摂動モデル演算部１２０の処理を説明する。摂動モデル演算部１２０は、本実施形態では、ロボット１の上体２４の水平位置の摂動運動と目標ＺＭＰまわりの床反力モーメントの摂動との関係を表現する動力学モデル（摂動動力学モデル）を用いて、前記上体水平位置摂動量を算出する処理を実行する。その摂動動力学モデルは、本実施形態では、図２１に示す倒立振子モデルにより表現される。

すなわち、図２１を参照して、摂動モデル演算部１２０が用いる摂動動力学モデルでは、目標ＺＭＰを支点として揺動する倒立振子の質点としての上体質点２４ｂの、目標ＺＭＰの直上位置（鉛直方向上方位置）からの水平方向のずれ量ΔＸpが、上体２４の水平位置の摂動量に相当するものとされる。そして、ΔＸpと、倒立振子の支点（目標ＺＭＰ）のまわりの床反力モーメントの水平成分の摂動量としての摂動床反力モーメントΔＭpとの間の関係が例えば次式６０により表現される。

ΔＭp＝−mb＊ΔＸp＊(g＋d2Ｚb/dt2)
＋mb＊ｈ＊(d2ΔＸp/dt2) ……式６０

この式６０におけるｈは、上体質点２４ｂの平均的な高さとしての定数値である。なお、式６０のΔＭpは、詳しくは、Ｘ軸まわり成分とＹ軸まわり成分とからなるベクトル量である。同様に、ΔＸpは、Ｘ軸方向成分とＹ軸方向成分とからなるベクトル量である。

そして、摂動モデル演算部１２０は、式６０のΔＸpが、出力すべき前記上体水平位置摂動量に相当し、且つ、前記全モデル操作床反力モーメント分配器１１８から入力される第２モデル床反力操作モーメントが式６０のΔＭpに相当するものとして、式６０を用いて上体水平位置摂動量を決定する。

具体的には、摂動モデル演算部１２０は、逐次入力される第２モデル床反力操作モーメントの時系列をΔＭpの時系列として用い、式６０の動力学的関係を満足するΔＸpを逐次算出する。

この場合、ΔＸpの算出演算に必要な、式６０の右辺の(g＋d2Ｚb/dt2)の瞬時値（又はｍb＊(g＋d2Ｚb/dt2)の瞬時値）は、前記式０１に基づいて算出される。より詳しくは、今回歩容の床反力鉛直成分軌道パラメータ（最新の床反力鉛直成分軌道パラメータ）を基に、床反力鉛直成分の瞬時値（前記式０１の左辺のＦz）が算出されると共に、今回歩容における現在時刻ｔまでの両足平２２，２２の目標足平位置姿勢軌道を基に、前記式０１の右辺のmsup＊(g＋d2Zsup/dt2)の瞬時値とmswg＊(g＋d2Zswg/dt2)の瞬時値とが算出される。そして、これらの求めた値から、前記式０１に基づいて、(g＋d2Ｚb/dt2)又はｍb＊(g＋d2Ｚb/dt2)の瞬時値が算出される。

そして、摂動モデル演算部１２０は、算出したΔＸpを上体水平位置摂動量として前記上***置姿勢修正部１２２に出力する。なお、ΔＸpは、図７のＳ０１６の判断結果が肯定的となるタイミング（前記した歩容の切替わり目）において、換言すれば、歩容生成装置１００による新たな今回歩容の生成開始タイミングで“０”に初期化される。

以上が、摂動モデル演算部１２０の処理の詳細である。

次に、フィードバック操作量演算部１１０の処理を説明する。図３を参照して、フィードバック操作量演算部１１０には、前記したように、実上体姿勢角の実測値と、修正目標上体姿勢角と、摂動モデル演算部１２０で用いる摂動動力学モデルの状態量（摂動モデル状態量）とが入力される。なお、本実施形態では、フィードバック操作量演算部１１０に入力される摂動モデル状態量は、摂動モデル演算部１２０が逐次算出する上体水平位置摂動量ΔＸpそのものである。なお、図示は省略しているが、フィードバック操作量演算部１１０には、歩容生成装置１００から、前記Ｓ１００８の処理によって決定されたＺＭＰ存在許容領域も逐次入力される。

そして、フィードバック操作量演算部１１０は、実上体姿勢角の実測値と修正目標上体姿勢角との偏差である前記上体姿勢角偏差を“０”に収束させる機能を有するフィードバック操作量としての実姿勢補償全床反力モーメントを所定のフィードバック制御則により算出する姿勢安定化制御演算部１１２と、摂動モデル状態量ΔＸpとそれに対する所定の目標値との偏差であるモデル状態量偏差を“０”に収束させる機能を有するフィードバック操作量としてのモデル状態補償全床反力モーメントを所定のフィードバック制御則により算出するモデル安定化制御演算部１１４と、これらの実姿勢補償全床反力モーメント及びモデル状態補償全床反力モーメントから、前記コンプライアンス制御用目標床反力モーメントと全モデル操作床反力モーメントとを決定する補償全床反力モーメント分配器１１６とを備える。

上記実姿勢補償全床反力モーメントは、上体姿勢角偏差を“０”に収束させるために、実際のロボット１に対して目標ＺＭＰまわりに付加的に作用させる床反力モーメントの要求値である。また、モデル状態補償全床反力モーメントは、モデル状態量偏差を“０”に収束させるために、前記摂動動力学モデル上でのロボットに対して目標ＺＭＰまわりに付加的に作用させる床反力モーメントの要求値である。

なお、本実施形態では、摂動モデル状態量ΔＸpの目標値は“０”である。従って、モデル状態量偏差は、摂動モデル状態量ΔＸpに一致する。

補足すると、実姿勢補償全床反力モーメント及びモデル状態補償全床反力モーメントのそれぞれは、より詳しくは、ロール方向（Ｘ軸まわり）の成分と、ピッチ方向（Ｙ軸まわり）の成分とからなる。以降の説明では、実姿勢補償全床反力モーメント及びモデル状態補償全床反力モーメントをそれぞれ、参照符号Ｍdmd、Ｍmdlにより表す。そして、Ｍdmdのロール方向（Ｘ軸まわり）の成分と、ピッチ方向（Ｙ軸まわり）の成分とをそれぞれ、Ｍdmdx，Ｍdmdyと表記し、Ｍmdlのロール方向（Ｘ軸まわり）の成分と、ピッチ方向（Ｙ軸まわり）の成分とをそれぞれ、Ｍmdlx，Ｍmdlyと表記する。

上記姿勢安定化制御演算部１１２の処理は、次のように実行される。すなわち、姿勢安定化制御演算部１１２は、まず、入力された実上体姿勢角の実測値と修正目標上体姿勢角とから、前記上体姿勢角偏差Δθを算出する。そして、姿勢安定化制御演算部１１２は、上体姿勢角偏差Δθを、図６に示した倒立振子モデルの上体質点２４ｂの水平位置の、基準位置（上体姿勢角偏差Δθが“０”である場合の目標上***置姿勢に対応する上体質点２４ｂの水平位置）に対する位置偏差ΔＸに変換される。具体的には、上体姿勢角偏差Δθは、次式６２によって、上体質点２４ｂの水平位置の摂動量としての位置偏差ΔＸに変換される。なお、式６２におけるｈは、上体質点２４ｂの平均的な高さとしての定数値である。

ΔＸ＝ｈ＊Δθ ……式６２

なお、ΔＸは、Ｘ軸方向成分とＹ軸方向成分とからなり、Ｘ軸方向成分は、ΔθのうちのＹ軸まわり方向の成分、Ｙ軸方向成分は、ΔθのうちのＸ軸まわり方向の成分に比例する値となる。

さらに、姿勢安定化制御演算部１１２は、上記の如く算出した位置偏差ΔＸの時間的変化率（微分値）である速度偏差ΔＶxを算出する。なお、速度偏差ΔＶxは、実際のロボット１の上体姿勢角の角速度の実測値（又は上体姿勢角の実測値の時間的変化率）と修正目標上体姿勢角の角速度との偏差に、上記定数値ｈを乗じることによって求めてもよい。

次いで、姿勢安定化制御演算部１１２は、例えばフィードバック制御則としてのＰＤ則（比例・微分則）の演算処理によって、ΔＸを“０”に収束させるように（ひいては、Δθを“０”に収束させるように）、実姿勢補償全床反力モーメントＭdmdを算出する。

すなわち、姿勢安定化制御演算部１１２は、上記の如く算出した位置偏差ΔＸと速度偏差ΔＶxとから、次式６４によりＭdmdを算出する。

Ｍdmd＝Ｋx＊ΔＸ＋Ｋv＊ΔＶx ……式６４

この式１５２におけるＫx、Ｋvは、それぞれ比例ゲイン、微分ゲインである。これらのＫx、Ｋvの値は、本実施形態では、あらかじめ設定された所定値である。

この場合、より詳しくは、ＭdmdのＹ軸まわりの成分Ｍdmdyは、ΔＸ、ΔＶxのＸ軸方向成分に応じて式６４により算出される。また、ＭdmdのＸ軸まわりの成分Ｍdmdxは、ΔＸ、ΔＶxのＹ軸方向成分に応じて式６４により算出される。

なお、式６４のゲインＫx、Ｋvの値は、ロボット１の運動状態に応じて適宜可変的に設定するようにしてもよい。例えば、歩容生成装置１００が図１６のＳ６０６で決定した今回歩容の床反力鉛直成分軌道パラメータにより規定される現在時刻での上体質点２４ｂの鉛直方向の慣性力に応じて、あるいは、上体質点２４ｂの鉛直方向の慣性力の時系列に応じて、Ｋx、Ｋvの一方又は両方の値を可変的に決定するようにしてもよい。このようにすることにより、ロボット１の運動に伴う鉛直方向の慣性力の影響を補償して、上体姿勢角偏差Δθを円滑に“０”に収束させ得るＭdmdの値を算出することができる。

以上が、姿勢安定化制御演算部１１２の処理の詳細である。

なお、Ｍdmdは、例えば、上体姿勢角偏差Δθと、その時間的変化率である上体姿勢角の角速度偏差とから、ＰＤ則によって算出するようにしてもよい。

また、ロボット１の実際の姿勢と目標とする姿勢とのずれの程度を表す指標として、上体姿勢角偏差Δθの代わりに、例えば、ロボット１の全体重心の実際の位置と、目標歩容（詳しくは目標運動）により規定される該全体重心の目標位置との水平方向の位置偏差を使用し、この位置偏差から、ＰＤ則等のフィードバック制御則により実姿勢補償全床反力モーメントMdmdを算出するようにしてもよい。

また、ロボット１に作用する床反力以外の外力及びその作用点を、上体２４に搭載した適宜の力センサ等により計測し、その外力及び作用点の計測値から、該外力によって目標ＺＭＰまわりで実際のロボット１に作用するモーメントを算出するようにしてもよい。そして、その算出したモーメントに釣り合う（逆向きの）床反力モーメントに“１”以下の正のゲインを乗じてなる床反力モーメントを式６４の右辺に加算することよって、補償全床反力モーメントＭdmdを算出するようにしてもよい。

また、モデル安定化制御演算部１１４の処理は、次のように実行される。すなわち、モデル安定化制御演算部１１４は、まず、入力されたモデル状態量偏差ΔＸpの時間的変化率ΔＶp（＝dΔXp/dt）を算出する。そして、モデル安定化制御演算部１１４は、姿勢安定化制御演算部１１２と同様に、例えば、フィードバック制御則としてのＰＤ則の演算処理によって、ΔＸpを“０”に収束させるように、モデル状態補償全床反力モーメントＭmdlを算出する。

すなわち、モデル安定化制御演算部１１４は、入力されたモデル状態量偏差ΔＸpとその時間的変化率ΔＶpとから、次式６６によりＭmdlを算出する。

Ｍmdl＝Ｋa＊ΔＸp＋Ｋb＊ΔＶp ……式６６

この式６６におけるＫa、Ｋbは、それぞれ比例ゲイン、微分ゲインである。これらのＫa、Ｋbの値は、本実施形態では、あらかじめ設定された所定値である。

この場合、より詳しくは、ＭmdlのＹ軸まわりの成分Ｍmdlyは、ΔＸp、ΔＶpのＸ軸方向を成分に応じて式６６により算出される。また、ＭmdlのＸ軸まわりの成分Ｍdmdyは、ΔＸp、ΔＶpのＹ軸方向成分に応じて式６６により算出される。

なお、前記式６４におけるＫx，Ｋvに関して補足的に説明した場合と同様に、式６６におけるＫa，Ｋbの値をロボット１の運動状態（例えば上体２４の鉛直方向の運動状態）に応じて可変的に設定するようにしてもよい。

以上が、モデル安定化制御演算部１１４の処理の詳細である。

次に、フィードバック操作量演算部１１０の補償全床反力モーメント分配器１１６は、上記の如く算出された実姿勢補償全床反力モーメントＭdmdとモデル状態補償全床反力モーメントＭmdlとから、以下に説明する処理によって、コンプライアンス制御用目標床反力モーメントと全モデル操作床反力モーメントとを決定する。

すなわち、補償全床反力モーメント分配器１１６は、コンプライアンス制御用目標床反力モーメントと全モデル操作床反力モーメントとを図２２のブロック図で示す処理によって決定する。

以下説明すると、補償全床反力モーメント分配器１１６は、まず、処理部１１６ａにおいて、実姿勢補償全床反力モーメントＭdmdとモデル状態補償全床反力モーメントＭmdlとを加え合わせることによってそれらの合成モーメント（Ｍdmd＋Ｍmdl）を算出する。この合成モーメント（Ｍdmd＋Ｍmdl）は、上体姿勢角偏差Δθとモデル状態量偏差ΔＸpとを“０”に近づけるように、目標ＺＭＰ周りの床反力モーメントの水平成分の目標値を操作するフィードバック操作量としての意味を持つ。以降、この合成モーメント（Ｍdmd＋Ｍmdl）を、補償合成モーメントという。

次いで、補償全床反力モーメント分配器１１６は、この補償合成モーメントを、処理部１１６ｂにおいてＺＭＰ修正量に変換する。ここで、ある時刻において、目標ＺＭＰ周りに、“０”でない補償合成モーメント（Ｍdmd＋Ｍmdl）の床反力モーメントを発生させるということは、該補償合成モーメントとその時刻における全床反力鉛直成分の瞬時値とに応じて規定される変位量（変位ベクトル）だけ、実際のＺＭＰの位置、すなわち、その点周りの床反力モーメントの水平成分が“０”となるような全床反力の実際の作用点としての全床反力中心点の位置（水平面上の位置）を目標ＺＭＰの位置からずらすことと等価である。

上記変位量（変位ベクトル）は、この変位ベクトルと、全床反力鉛直成分の瞬時値の大きさを有する鉛直方向のベクトルとの外積が補償合成モーメント（ベクトル）に一致するような変位ベクトルである。換言すれば、上記変位量（変位ベクトル）は、その大きさが、補償合成モーメントの大きさを全床反力鉛直成分の瞬時値の大きさで除算してなる大きさとなり、且つ、補償合成モーメント（ベクトル）の方向と鉛直方向とに直交する向きを有する変位ベクトルである。

このような上記変位量が、処理部１１６ｂで求めるＺＭＰ修正量である。このＺＭＰ修正量を求めるために、処理部１１６ｂには、歩容生成装置１００が図２０のＳ１０００の処理で算出した目標床反力鉛直成分の瞬時値（最新値）が入力される。そして、処理部１１６ｂでは、この目標床反力鉛直成分の瞬時値を用いて、上記の関係に従って、補償合成モーメントをＺＭＰ修正量に変換する。すなわち、該ＺＭＰ修正量は、それより表される変位ベクトルと、目標床反力鉛直成分の瞬時値の大きさを有する鉛直方向ベクトルとの外積が、補償合成モーメント（ベクトル）に一致するような変位ベクトルとして算出される。

次いで、補償全床反力モーメント分配器１１６は、上記の如く算出したＺＭＰ修正量を、歩容生成装置１００において図２０のＳ１００２の処理で算出された目標ＺＭＰの瞬時値（最新値）の位置ベクトルに加え合わせる処理を処理部１１６ｃで実行することによって、目標ＺＭＰ周りに補償合成モーメントの床反力モーメントをそのまま発生させたと仮定した場合の実際の全床反力中心点の推定位置としての補償ＺＭＰを算出する。

ここで、実際のロボット１における実際のロボット１に作用させる全床反力の作用点としてのＺＭＰ（全床反力中心点）は、支持多角形内の領域から逸脱することはできないので、目標ＺＭＰ周りに発生可能な床反力モーメントは、それに応じて目標ＺＭＰからずれることとなる実際のＺＭＰが支持多角形内に存在し得る範囲内に制限される。

そこで、補償全床反力モーメント分配器１１６は、次に、上記補償ＺＭＰを、歩容生成装置１００が図２０のＳ１００８の処理で決定した最新のＺＭＰ存在許容領域内の点に制限する処理を処理部１１６ｄで実行する。

具体的には、処理部１１６ｄは、処理部１１６ｃで求められた補償ＺＭＰ（以降、制限前補償ＺＭＰという）が、上記ＺＭＰ存在許容領域内に存在するか否かを判断し、この判断結果が肯定的である場合には、該制限前補償ＺＭＰをそのまま制限後補償ＺＭＰとして決定する。例えば、図１９に示した如くＺＭＰ存在許容領域が設定されている状況において、図２３（ａ）に示す如く、制限前補償ＺＭＰが、ＺＭＰ存在許容領域内の点Ｐ１である場合には、その点Ｐ１がそのまま、制限後補償ＺＭＰとして決定される。

また、処理部１１６ｄは、処理部１１６ｃで求められた制限前補償ＺＭＰが、上記ＺＭＰ存在許容領域から逸脱している場合には、制限後補償ＺＭＰを、強制的に上記ＺＭＰ存在許容領域内の点に制限する。この場合、該制限後補償ＺＭＰは、ＺＭＰ存在許容領域の外形の境界線上で、制限前補償ＺＭＰに最も近い点に決定される。例えば、図１９に示した如くＺＭＰ存在許容領域が設定されている状況において、図２３（ｂ）に示す如く、制限前補償ＺＭＰが、ＺＭＰ存在許容領域の外側の点Ｐ２である場合には、ＺＭＰ存在許容領域の外形の境界線上で、点Ｐ２に最も近い点Ｐ３が制限後補償ＺＭＰとして決定される。これにより、制限後補償ＺＭＰは、ＺＭＰ存在許容領域から逸脱しない点に制限される。

次いで、補償全床反力モーメント分配器１１６は、上記の如く決定した制限後補償ＺＭＰと、目標ＺＭＰとの位置の差分のベクトル（制限後補償ＺＭＰの位置ベクトルから目標ＺＭＰの位置ベクトルを差し引いたベクトル）を処理部１１６ｅで算出する。この処理部１１６ｅで算出されるベクトルは、実際のロボット１で実現しようとするＺＭＰ修正量の目標を意味するものであり、以降、制限後ＺＭＰ修正目標という。

次いで、補償全床反力モーメント分配器１１６は、この制限後ＺＭＰ修正目標を、目標ＺＭＰ周りに付加すべき床反力モーメントに変換する処理を処理部１１６ｆで実行し、この変換によって算出された床反力モーメントをコンプライアンス制御用目標床反力モーメントとして決定する。この場合、処理部１１６ｆの処理は、処理部１１６ｂにおける変換処理の逆変換の処理であり、制限後ＺＭＰ修正目標と、目標床反力鉛直成分の瞬時値（歩容生成装置１００が図２０のＳ１０００の処理で算出した目標床反力鉛直成分の瞬時値（最新値）とからコンプライアンス制御用目標床反力モーメントが決定される。より詳しくは、制限後ＺＭＰ修正目標により表される変位ベクトルと、目標床反力鉛直成分の瞬時値の大きさの鉛直方向ベクトルとの外積を算出することによって、コンプライアンス制御用目標床反力モーメント（ベクトル）が決定される。

次いで、補償全床反力モーメント分配器１１６は、処理部１１６ａで算出された補償合成モーメントからコンプアライアンス制御用目標床反力モーメントを差し引く処理を処理部１１６ｇで実行することによって、補償合成モーメントのうちの、コンプアライアンス制御用目標床反力モーメントが負担しきれない余剰分のモーメントを算出する。

なお、この場合、例えば、前記制限前補償ＺＭＰと制限後補償ＺＭＰとの差を、処理部１１６ｈと同様の処理によりモーメントに変換することによって、上記余剰分のモーメントを算出するようにしてもよい。

さらに、補償全床反力モーメント分配器１１６は、この余剰分のモーメントの符号を反転させたものを、前記モデル状態補償全床反力モーメントMmdlに加え合わせる（合成する）処理を処理部１１６ｈで実行することによって、全モデル操作床反力モーメントを算出する。

以上が補償全床反力モーメント分配器１１６の処理の詳細である。

補足すると、補償全床反力モーメント分配器１１６の処理で使用する目標ＺＭＰ、床反力鉛直成分、ＺＭＰ存在許容領域は、歩容生成装置１００が各制御処理周期において最終的に決定した今回歩容パラメータ（図７のＳ０３２の処理で使用する今回歩容パラメータ）を基に、補償全床反力モーメント分配器１１６において、逐次（該分配器１１６の制御周期で）決定するようにしてもよい。

以上の補償全床反力モーメント分配器１１６の演算処理により、前記補償ＺＭＰ（制限前補償ＺＭＰ）が、ＺＭＰ存在許容領域内に収まっている場合には、前記補償合成モーメント（Mdmd＋Mmdl）がそのままコンプライアンス制御用目標床反力モーメントとして決定されると共に、Ｍmdlが全モデル操作床反力モーメントとして決定される。

また、前記補償ＺＭＰ（制限前補償ＺＭＰ）が、ＺＭＰ存在許容領域から逸脱している場合には、コンプライアンス制御用目標床反力モーメントは、このモーメントを目標ＺＭＰ周りに発生させることによって目標ＺＭＰから変位する実際のＺＭＰ（実際の全床反力中心点）が、ＺＭＰ存在許容領域の境界線上の点のうち、前記補償合成モーメント（Mdmd＋Mmdl）によって要求されるＺＭＰの位置としての補償ＺＭＰ（制限前補償ＺＭＰ）に最も近い点になるように、コンプライアンス制御用目標床反力モーメントが決定される。

そして、補償合成モーメント（Mdmd＋Mmdl）のうちの、コンプアライアンス制御用目標床反力モーメントが負担しきれない余剰分のモーメントの符号を反転させたモーメントをＭmdlに付加してなるモーメント（換言すれば、モデル状態補償全床反力モーメントから上記余剰分のモーメントを差し引いたモーメント）が、全モデル操作床反力モーメントとして決定される。例えば、上記余剰分のモーメントがMmdlと、Mdmdのうちの一部分のモーメントｋ＊Ｍdmd（０＜ｋ＜１）を合成したモーメント（＝Mmdl＋ｋ＊Ｍdmd）となる状況では、（−ｋ＊Ｍdmd）が全モデル操作床反力モーメントとして決定される。

従って、いずれの場合でも、次式７０のように、コンプライアンス制御用床反力モーメントと全モデル操作床反力モーメントとの差が、モデル状態補償全床反力モーメントMmdlに依存することなく、実姿勢補償全床反力モーメントMdmdに一致するように、該コンプライアンス制御用床反力モーメントと全モデル操作床反力モーメントとが決定される。

コンプライアンス制御用目標床反力モーメント
−モデル操作用床反力モーメント＝Mdmd ……式７０

換言すれば、コンプライアンス制御用床反力モーメントと全モデル操作床反力モーメントの差が、上体姿勢角偏差Δθを“０”に収束させる機能を有するように、コンプライアンス制御用床反力モーメントと全モデル操作床反力モーメントとが決定される。

そして、この場合、ＺＭＰ存在許容領域に対応して実際のロボット１で目標ＺＭＰ周りに発生可能な床反力モーメントが、Mdmdに対して余裕を有する場合に、コンプライアンス制御用目標床反力モーメントに起因して目標ＺＭＰから変位するＺＭＰが、ＺＭＰ存在許容領域内に収まる範囲内において、Mmdlの全部又は一部がコンプライアンス制御用床反力モーメントと全モデル操作用床反力モーメントとに、それぞれの組成成分として、付加的に包含されることとなる。

次に、前記全モデル操作床反力モーメント分配器１１８の処理を説明する。

まず、全モデル操作床反力モーメント分配器１１８の処理の技術的な意義を説明しておく。歩容生成装置１００における目標歩容の生成処理においては、今回歩容の目標運動の軌道は、将来において、定常歩容の運動軌道に収束するように（漸近するように）決定される。そして、このような目標運動を生成するために、今回歩容のＺＭＰ軌道パラメータが適宜修正される。さらに、修正後のＺＭＰ軌道パラメータにより規定される目標ＺＭＰの瞬時値が、ＺＭＰ存在許容領域から逸脱するような場合には、今回歩容の遊脚足平２２の着地予定位置姿勢や次回歩容の遊脚足平２２の着地予定位置姿勢が適宜修正される。

歩容生成装置１００では、このような処理を、各制御周期で実行する必要があるため、該歩容生成装置１００の制御周期（演算処理周期）Δtは、ある程度余裕を持たせた長めの時間に設定せざるを得ない。

一方、例えばロボット１を比較的高速の移動速度で移動させるような場合には、それに伴い、ロボット１の上体姿勢角偏差Δθの高速な変動を生じやすくなる。ひいては、上体姿勢角偏差Δθには、歩容生成装置１００の制御周期Δｔよりも短い周期の高周波成分が含まれ易くなる。そして、上体姿勢角偏差Δθにこのような高周波成分が含まれるような場合には、歩容生成装置１００による歩容生成処理（本実施形態では図８のＳ０３２の処理）に当該高周波成分を反映させることは困難である。ひいては、該高周波成分を含む上体姿勢角偏差Δθを円滑に“０”に収束させ得るような目標歩容の瞬時値を歩容生成装置１００で逐次生成することが困難となる。

そこで、本実施形態では、図３のブロック図に示した制御ユニット２６の機能のうち、摂動モデル演算部１２０等、歩容生成装置１００以外の機能部分の処理を歩容生成装置１００よりも短い制御周期（演算処理周期）で実行する。そして、上体姿勢角偏差Δθの、比較的緩やかな変動分（低周波成分）を“０”に収束させ得る目標歩容の運動の瞬時値の時系列を歩容生成装置１００の歩容生成処理により決定すると共に、Δθの比較的素早い変動分（高周波成分）を“０”に収束させるための運動修正分（歩容生成装置１００が生成する目標運動の修正量）として、前記上体水平位置摂動量ΔＸpを摂動動力学モデルを用いて決定する。

上記のように歩容生成装置１００の歩容生成処理と、上体水平位置摂動量ΔＸpを決定する処理とを行なうようにするために、本実施形態では、全モデル操作床反力モーメント分配器１１８は、フィードバック操作量演算部１１０の補償全床反力モーメント分配器１１６から逐次入力される全モデル操作床反力モーメントを低周波成分と高周波成分とに分離する。そして、フィードバック操作量演算部１１０は、これらの低周波成分と高周波成分とのうち、低周波成分を歩容生成装置１００の動力学モデル（図６に示した動力学モデル）にフィードバックする第１モデル操作床反力モーメントとして出力し、高周波成分を前記摂動モデル演算部１２０の摂動動力学モデル（図２１に示した摂動動力学モデルにフィードバックする第２モデル操作床反力モーメントとして出力する。

全モデル操作床反力モーメント分配器１１８の上記の処理は、より具体的には、図２４のブロック図に示す処理によって実現される。

すなわち、全モデル操作床反力モーメント分配器１１８は、全モデル操作床反力モーメントをローパスフィルタ（ハイカットフィルタ）１１８ａに入力し、このローパスフィルタ１１８ａの出力（すなわち、全モデル操作床反力モーメントの低周波成分）を、第１モデル操作床反力モーメントして決定する。なお、ローパスフィルタ１１８ａの周波数特性は、歩容生成装置１００の制御周期Δｔよりも長い周期の低周波成分を通過させるように設定されている。

さらに、全モデル操作床反力モーメント分配器１１８は、演算部１１８ｂにおいて、全モデル操作床反力モーメントからローパスフィルタ１１８ａの出力を差し引き、この演算部１１９ｂの演算結果の値（すなわち、全モデル操作床反力モーメントから低周波成分を除去した高周波成分）を第２モデル操作床反力モーメントとして出力する。

以上が、全モデル操作床反力モーメント分配器１１８の処理の詳細である。

以上のように、全モデル操作床反力モーメント分配器１１８の処理を実行することにより、第１モデル操作床反力モーメントは、全モデル操作床反力モーメントの低周波成分及び高周波成分のうちの高周波成分よりも低周波成分の変化に対してより高感度に変化するフィードバック操作量として決定される。また、第２モデル操作床反力モーメントは、全モデル操作床反力モーメントの低周波成分及び高周波成分のうちの低周波成分よりも高周波成分の変化に対してより高感度に変化するフィードバック操作量として決定される。

補足すると、全モデル操作床反力モーメントを、ハイパスフィルタ（ローカットフィルタ）に入力し、このハイパスフィルタの出力を第２モデル操作床反力モーメントして得るようにしてもよい。そして、この場合、ローパスフィルタ１１８ａを省略し、該ハイパスフィルタの出力を全モデル操作床反力モーメントから差し引いたもの（すなわち、全モデル操作床反力モーメントから高周波成分を除去した低周波成分）を第１モデル操作床反力モーメントとして得るようにしてもよい。

また、第１モデル操作床反力モーメントと第２モデル操作床反力モーメントとは、それぞれ、全モデル操作床反力モーメントの高周波成分の一部、低周波成分の一部を含んでいてもよい。すなわち、第１モデル操作床反力モーメントは、第２モデル操作床反力モーメントよりも少ない割合いで、全モデル操作床反力モーメントの高周波成分の一部を含んでいてもよい。同様に、第２モデル操作床反力モーメントは、第１モデル操作床反力モーメントよりも少ない割合いで、全モデル操作床反力モーメントの低周波成分の一部を含んでいてもよい。例えば、全モデル操作床反力モーメントを低周波成分と高周波成分とに分離した後、それらの低周波成分と高周波成分とを、それぞれに掛かる重み係数を互いに異ならせて線形結合する（換言すれば、低周波成分と高周波成分との重み付き平均を算出する）ことによって、第１モデル操作床反力モーメント又は第２モデル操作床反力モーメントを決定するようにしてもよい。

以上説明した実施形態によれば、前記上体姿勢角偏差Δθを“０”に近づけるように機能するフィードバック操作量成分として姿勢安定化制御演算部１１２により算出された実姿勢補償全床反力モーメントＭdmdと、前記モデル状態量偏差ΔＸpを“０”に近づけるように機能するフィードバック操作量成分として前記モデル安定化制御部１１４により算出されたモデル状態補償全床反力モーメントＭmdlとが、補償全床反力モーメント分配器１１６の処理部１１６ａにより合成される。これにより、上記ΔθとΔＸpとを“０”に近づける機能を有するフィードバック操作量として、前記補償合成モーメント（Ｍdmd＋Ｍmdl）が算出される。

そして、補償全床反力モーメント分配器１１６において、この補償合成モーメントをそのまま実際のロボット１において目標ＺＭＰ周りに発生させると仮定した場合に要求される実際のＺＭＰ（全床反力中心点）の位置としての前記制限前補償ＺＭＰが、支持多角形内のＺＭＰ存在許容領域内に収まるか否かが判断され、この判断結果が肯定的となる場合には、補償合成モーメントがそのままコンプライアンス制御用目標床反力モーメントとして決定される。

また、この場合には、モデル状態量補償全床反力モーメントＭmdlがそのまま全モデル操作床反力モーメトとして決定される。

これにより、実際のロボット１における目標ＺＭＰ周りの床反力モーメントが、上体姿勢角偏差Δθを“０”に近づけることができるように操作されると共に、実際のロボット１における目標ＺＭＰ周りの床反力モーメントと、ロボット１の目標運動を生成するための動力学モデル（本実施形態では図６及び図２１に示した動力学モデル）上でのロボット１に作用させる目標ＺＭＰ周りの床反力モーメントとが、協働してモデル状態量偏差ΔＸpを“０”に近づけることができるように操作されることとなる。

また、補償合成モーメントをそのまま実際のロボット１において目標ＺＭＰ周りに発生させると仮定した場合に要求される実際のＺＭＰ（全床反力中心点）の位置としての前記制限前補償ＺＭＰが、支持多角形内のＺＭＰ存在許容領域内から逸脱する場合には、実際のロボット１において、補償合成モーメント（Mdmd＋Ｍmdl）をそのまま目標ＺＭＰ周りに発生させることはできない（もしくは好ましくない）ので、実際のロボット１におけるＺＭＰ（全床反力中心点）をＺＭＰ存在許容領域内に収めうるように、コンプライアンス制御用目標床反力モーメントが補償合成モーメントに対して制限されることとなる。すなわち、コンプライアンス制御用目標床反力モーメントは、補償合成モーメントの一部分を負担することとなる。

そして、補償合成モーメントのうちの、コンプアライアンス制御用目標床反力モーメントが負担しきれない余剰分のモーメント（＝補償合成モーメント−コンプアライアンス制御用目標床反力モーメント）の符号を反転させたモーメントと、モデル状態補償全床反力モーメントとを合成してなるモーメント（換言すれば、モデル状態補償全床反力モーメントから上記余剰分のモーメントを差し引いたモーメント）が、動力学モデル上でのロボット１に作用させる目標ＺＭＰ周りの床反力モーメントを操作する全モデル操作床反力モーメントとして決定される。

さらに、この全モデル操作床反力モーメントの低周波成分と高周波成分とがそれぞれ歩容生成装置１００における動力学モデル、摂動モデル演算部１２０における動力学モデルのフィードバックされ、それぞれの動力学モデル上でのロボット１に作用させる床反力モーメントが操作される。

これにより、実際のロボット１の床反力モーメントを操作するためのコンプアライアンス制御用目標床反力モーメントと、動力学モデル上でのロボット１の床反力モーメントを操作するための全モデル操作床反力モーメントとが協働して、上体姿勢角偏差Δθを“０”に近づけるように機能することとなる。

ここで、支持多角形内に設定するＺＭＰ存在許容領域の面積が、該支持多角形に対して小さ過ぎると、コンプライアンス制御用目標床反力モーメントが、実際のロボット１で目標ＺＭＰ周りに発生可能な床反力モーメントに対して過剰な制限を受けることとなってしまう。

一方、ロボット１の歩行動作における支持多角形は、特に両脚支持期においては、図２３（ａ），（ｂ）に例示した如く、一般には、該支持多角形の形状が方形状と顕著に異なる形状となる。そして、このような支持多角形に対してＺＭＰ存在許容領域を単純な方形状に設定すると、該ＺＭＰ存在許容領域の面積が支持多角形に対して小さくなり過ぎるという状況が発生し易い。

このため、本実施形態では、両脚支持期におけるＺＭＰ存在許容領域を、支持多角形と概ね相似する外形状として、その面積が支持多角形に対して小さくなり過ぎないように該ＺＭＰ存在許容領域を設定するようにしている。換言すれば、両脚支持期におけるＺＭＰ存在許容領域は、その面積が支持多角形の面積を概ね近似し得るような領域に設定される。

これにより、コンプライアンス制御用目標床反力モーメントが補償合成モーメント（Ｍmdl＋Ｍdmd）に対して制限される状況の発生頻度が極力少なくなるようにすることができる。ひいては、上体姿勢角偏差Δθを“０”に近づけることを、可能な限り、実際のロボット１の目標ＺＭＰ周りに付加的に発生させる床反力モーメントによって実現し得るように、該付加的な床反力モーメントの目標としてのコンプライアンス制御用目標床反力モーメントを決定できることとなる。

また、補償合成モーメントをそのまま実際のロボット１において目標ＺＭＰ周りに発生させると仮定した場合に必要となるＺＭＰ（全床反力中心点）の位置としての前記制限前補償ＺＭＰが、支持多角形内のＺＭＰ存在許容領域内から逸脱するような状況では、制限後補償ＺＭＰがＺＭＰ存在許容領域の境界線上で、制限前補償ＺＭＰに最も近い位置に決定され、この制限後補償ＺＭＰに対応する目標ＺＭＰ周りの床反力モーメントがコンプライアンス制御用目標床反力モーメントとして決定される。

このため、コンプライアンス制御用目標床反力モーメントの大きさが、目標ＺＭＰ周りに付加すべき床反力モーメントの本来の目標としてのMdmdに対して小さくなるのを最小限に留めることができる。ひいては、上体姿勢角偏差Δθを“０”に近づける機能を、可能な限り、コンプライアンス制御用目標床反力モーメントに持たせることができる。

同時に、全モデル操作床反力モーメントの大きさを極力小さくできるため、前記修正目標上***置姿勢が、両脚体２，２の目標足平位置姿勢軌道に対して好適な姿勢（動力学モデル上での目標ＺＭＰを満足する歩容における姿勢）からずれるのを極力抑制することができる。

また、本実施形態では、全モデル操作床反力モーメントは、低周波成分と高周波成分とに分離される。そして、低周波成分が第１モデル操作床反力モーメントとして、歩容生成装置１００の歩容生成用の動力学モデルにフィードバックされる一方、高周波成分が第２モデル操作床反力モーメントとして、摂動モデル演算部１２０が上***置姿勢の修正量（上体水平位置摂動量）を生成するために用いる摂動動力学モデルにフィードバックされる。

この場合、該歩容生成装置１００にフィードバックされる第１モデル操作床反力モーメントが、低周波域のものであるため、歩容生成装置１００の制御周期を比較的長めの周期に設定しても、該第１モデル操作床反力モーメントを適切に歩容生成に反映させて目標歩容を生成できる。すなわち、比較的緩やかな変化を呈する上体姿勢角偏差Δθに対して、該上体姿勢角偏差Δθを円滑に“０”に近づけ得る目標歩容の運動を歩容生成装置１００により生成できる。

また、歩容生成装置１００の制御周期を長めの周期に設定できることから、定常歩容パラメータを探索的に決定する等、比較的演算負荷の大きい処理を１制御周期内に実行できる。このため、ロボット１の運動の長期的な安定性（継続性）を高い信頼性で確保し得る目標運動を歩容生成装置１００により決定できる。

なお、本実施形態では、歩容生成装置１００での歩容生成用の動力学モデルは、比較的単純な動力学モデル（倒立振子モデル）を使用したが、歩容生成装置１００の制御周期を長めの周期に設定できることから、より高い動力学的精度を確保し得る、より複雑な動力学モデル（非線形性の高い動力学モデル）を使用することもできる。

一方、摂動モデル演算部１２０にフィードバックされる第２モデル操作床反力モーメントは、高周波域のものであるものの、該摂動モデル演算部１２０は、歩容生成装置１００が出力する目標歩容の運動（基本骨格となる目標運動）に付加する部分的な修正分（本実施形態では、上体水平位置摂動量）だけを算出し得るものであればよい。このため、摂動モデル演算部１２０の動力学演算では、線形性の高い単純な摂動動力学モデルを使用することができる。ひいては、摂動モデル演算部１２０や、これに対する入力を生成する機能部分（全モデル操作床反力モーメント分配器１１８等）の制御周期（演算処理周期）を比較的短い周期に設定しても、その制御周期内で支障なく、摂動モデル演算部１２０の処理を実行できる。

従って、摂動モデル演算部１２０にフィードバックされる第２モデル操作床反力モーメントは、高周波域のものであっても、該第２モデル操作床反力モーメントを適切に摂動モデル演算部１２０の摂動動力学モデルに反映させて、目標歩容の運動の修正分たる上体水平位置摂動量を生成できる。すなわち、比較的素早い変化を呈する上体姿勢角偏差Δθに対して、該上体姿勢角偏差Δθを円滑に“０”に近づけ得る目標歩容の運動の修正量（上体水平位置摂動量を）を摂動モデル演算部１２０により生成できる。

そして、本実施形態では、上記の如く歩容生成装置１００が生成する目標歩容の運動と、摂動モデル演算部１２０が生成する上体水平位置摂動量により修正することによって、前記修正目標運動が決定され、基本的には、この修正目標運動に実ロボット１の運動が追従するように実ロボット１の動作が制御される。

この結果、上体姿勢角偏差Δθを“０”に近づける機能を有する全モデル操作床反力モーメントの周波数成分に依存することなく、該上体姿勢角偏差Δθを円滑に“０”に近づけることができるように修正目標運動を逐次決定しつつ、実ロボット１の動作を制御できる。

ここで、以上説明した本実施形態と、本発明との対応関係について補足しておく。

本実施形態では、前記フィードバック操作量演算部１１０によって、本発明における実ロボット外力操作量決定手段、全床反力中心点要求位置算出手段、制御対象外力要素目標値決定手段、及びモデル外力基本操作量決定手段が実現される。

より詳しく言えば、フィードバック操作量演算部１１０のうちの、姿勢安定化制御演算部１１２及びモデル安定化制御演算部１１４の処理と、補償全床反力モーメント分配器１１６の処理部１１６ａの処理とによって、本発明における実ロボット外力操作量決定手段が実現される。この場合、本実施形態では、実際のロボット１における目標ＺＭＰ周りの床反力モーメントが本発明における制御対象外力要素に相当するものとして用いられ、前記補償合成モーメント（Ｍdmd＋Ｍmdl）が、本発明における実ロボット外力操作量に相当する。また、前記上体姿勢角偏差Δθ（又はこれを前記式６２により変換してなる位置偏差ΔＸ）が本発明における姿勢状態量偏差に相当し、前記実姿勢補償全床反力モーメントＭdmdが、このΔθに応じて決定されるフィードバック操作量成分に相当する。また、前記モデル安定化制御演算部１１４に入力されるモデル状態量偏差ΔＸpが本発明におけるモデル状態量偏差に相当し、前記モデル状態補償全床反力モーメントＭmdlが、このΔＸpに応じて決定されるフィードバック操作量成分に相当する。そして、実ロボット外力操作量としての前記補償合成モーメントは、ＭdmdとＭmdlとを合成することによって、上記Δθ、ΔＸｐを“０”に近づけるように決定される。

また、補償全床反力モーメント分配器１１６の処理部１１６ｂ，１１６ｃの処理によって、本発明における全床反力中心点要求位置算出手段が実現される。この場合、制限前補償ＺＭＰが本発明における全床反力中心点要求位置に相当する。

また、補償全床反力モーメント分配器１１６の処理部１１６ｄ，１１６ｅ，１１６ｆの処理によって、本発明における制御対象外力要素目標値決定手段が実現される。この場合、処理部１１６ｄに与えられるＺＭＰ存在許容領域（前記歩容生成装置１００が図７のＳ０３０で決定したＺＭＰ存在許容領域）が、本発明における全床反力中心点許容領域に相当する。また、処理部１１６ｆにより求められる前記コンプライアンス制御用目標床反力モーメントが、本発明における制御対象外力要素の目標値に相当する。

なお、より正確に言えば、前記処理部１１６ｆにより決定されたコンプライアンス制御用目標床反力モーメントの値を、該コンプライアンス制御用目標床反力モーメントが“０”であると仮定した場合（補償合成モーメントが“０”であると仮定した場合）に目標ＺＭＰ周りに発生すべき床反力モーメントの水平成分としての基準目標値に加え合わせてなる床反力モーメントが、本発明における制御対象外力要素の目標値に相当するものである。但し、本実施形態では、上記基準目標値は“０”であるので、コンプライアンス制御用目標床反力モーメントがそのまま制御対象外力要素の目標値に相当するものとなる。

また、補償全床反力モーメント分配器１１６の処理部１１６ｇ，１１６ｈの処理によって、本発明におけるモデル外力基本操作量決定手段が実現される。この場合、処理部１１６ｈによって算出される全モデル操作床反力モーメントが本発明におけるモデル外力基本操作量に相当する。また、処理部１１６ｇの算出値が、全床反力中心点要求位置としての制限前補償ＺＭＰと、ＺＭＰ存在許容領域による制限後補償ＺＭＰとの差に応じて変化させるように決定されるフィードバック操作量成分に相当する。

また、歩容生成装置１００の処理（図７のフローチャートに示す処理）によって、本発明における第１運動決定手段が実現される。この場合、歩容生成装置１００が逐次出力する目標歩容の運動（目標足平位置姿勢、目標上***置姿勢、及び目標腕姿勢）、すなわち、図７のＳ０３２で決定する目標歩容の運動が、本発明における第１運動に相当する。また、図６に示した動力学モデル（本実施形態では倒立振子モデル）が、本発明における第１動力学モデルに相当する。また、図２０のＳ１０１０の処理で使用する第１モデル操作床反力モーメントが本発明における第１モデル外力操作量に相当する。そして、この第１モデル外力操作量に応じて、図６に示した動力学モデル上でのロボットに対して目標ＺＭＰまわりに発生させる床反力モーメントが該動力学モデル上でのロボットに作用させるべきモデル外力に相当する。このモデル外力は、本実施形態では、第１モデル操作床反力モーメントに一致するものとされる。

また、摂動モデル演算部１２０の処理によって、本発明における第２運動決定手段が実現される。この場合、図２１に示した摂動動力学モデル（本実施形態では倒立振子モデル）が本発明における第２動力学モデルに相当する。また、摂動モデル演算部１２０が逐次出力する上体水平位置摂動量が、本発明における第２運動に相当する。また、前記第２モデル操作床反力モーメントが、本発明における第２モデル外力操作量に相当する。そして、この第２モデル外力操作量に応じて、図２１に示した動力学モデル上でのロボットに対して目標ＺＭＰまわりに発生させる床反力モーメントが該動力学モデル上でのロボットに作用させるべきモデル外力に相当する。このモデル外力は、本実施形態では、第２モデル操作床反力モーメントに一致するものとされる。

また、前記目標上***置姿勢修正部１２２が、本発明における目標運動決定手段に相当し、歩容生成装置１００が出力する目標歩容の運動のうち、目標上***置姿勢を修正目標上***置姿勢に置き換えた運動である前記修正目標運動（すなわち、目標足平位置姿勢、修正目標上***置姿勢、目標腕姿勢の組）が本発明における目標運動に相当する。

また、歩容生成装置１００の処理のうち、図７のＳ０３０の処理によって、本発明における全床反力中心点許容領域決定手段が実現される。

また、前記全モデル操作床反力モーメント分配器１１８の処理によって、本発明におけるモデル外力操作量分配手段が実現される。

［第２実施形態］
次に、本発明の第２実施形態を図２５を参照して説明する。なお、本実施形態は、制御ユニット２６が実行する処理の一部のみが、第１実施形態と相違するものである。このため、本実施形態の説明では、第１実施形態と同等の構成部分又は機能部分については、第１実施形態と同一の参照符号を用い、詳細な説明を省略する。

前記第１実施形態で説明した歩容生成装置１００は、前記全モデル操作床反力モーメント分配器１１８から出力される第１モデル操作床反力モーメントの時系列のうち、歩容生成装置１００の制御周期（演算処理周期）Δｔでサンプリングした瞬時値を使用して、今回歩容の瞬時値を決定する処理（図８のＳ０３２の処理）を実行することとなる。このため、歩容生成装置１００の連続する２つの制御周期の間の期間内での第１モデル操作床反力モーメントの変化分は、歩容生成装置１００が今回歩容の瞬時値を決定する処理には反映されないこととなる。

そこで、本実施形態では、歩容生成装置１００の連続する２つの制御周期の間の期間内での第１モデル操作床反力モーメントの変化分を、摂動モデル演算部１２０の摂動動力学モデルに反映させる。

具体的には、本実施形態では、制御ユニット２６は、第１実施形態で説明した全モデル操作床反力モーメント分配器１１８の代わりに、図２５のブロックで示す処理を実行する全モデル操作床反力モーメント分配器１１９を備える。そして、この全モデル操作床反力モーメント分配器１１９には、前記フィードバック操作量演算部１１０から全モデル操作床反力モーメントが入力されることに加えて、歩容生成装置１００が図７のＳ０３２の処理で今回歩容の瞬時値を決定するために既に使用した第１モデル操作床反力モーメントの時系列のうちの最新値（以下、反映済第１モデル操作床反力モーメント最新値という）が歩容生成装置１００から入力される。

より詳しくは、歩容生成装置１００は、各制御周期において、出力する今回歩容の瞬時値を更新するまでは、現時点で出力している今回歩容の瞬時値を決定するために使用した第１モデル操作床反力モーメントの時系列のうちの最新値を上記反映済第１モデル操作床反力モーメント最新値として記憶保持している。そして、歩容生成装置１００は、その記憶保持している反映済第１モデル操作床反力モーメント最新値を全モデル操作床反力モーメント分配器１１８に出力する。従って、反映済第１モデル操作床反力モーメント最新値は、歩容生成装置１００が出力する今回歩容の瞬時値を更新する都度、それに合わせて、更新されることとなる。

なお、歩容生成装置１００とは別の記憶手段で、歩容生成装置１００の制御周期と同期した周期で反映済第１モデル操作床反力モーメント最新値を記憶保持しておき、該記憶手段から全床反力モーメント分配器１１９に、反映済第１モデル操作床反力モーメント最新値を入力するようにしてもよい。

そして、本実施形態では、全モデル操作床反力モーメント分配器１１９は、歩容生成装置１００にフィードバックする第１モデル操作床反力モーメントを、第１実施形態と同じ処理によって決定する。すなわち、全モデル操作床反力モーメント分配器１１９は、全モデル操作床反力モーメントをローパスフィルタ１１９ａ（ハイカットフィルタ）に入力し、このローパスフィルタ１１９ａの出力を第１モデル操作床反力モーメントとして決定する。なお、ローパスフィルタ１１９ａの周波数通過特性は、第１実施形態におけるローパスフィルタ１１８ａと同じでよい。

一方、全モデル操作床反力モーメント分配器１１９は、摂動モデル演算部１２０にフィードバックする第２モデル操作床反力モーメントを、第１実施形態と異なる態様で決定する。すなわち、全モデル操作床反力モーメント分配器１１８は、入力された全モデル操作床反力モーメントから、入力された反映済第１モデル操作床反力モーメント最新値を差し引く処理を演算部１１９ｂで実行し、この演算結果の値を、第２モデル操作床反力モーメントとして決定する。

以上が、本実施形態における全モデル操作床反力モーメント分配器１１９の処理である。

補足すると、以上説明した全モデル操作床反力モーメント分配器１１９によって第２モデル操作床反力モーメントを決定する処理は、次式７２の如く、全モデル操作床反力モーメントからローパスフィルタ１１９ａの出力を差し引いたもの（すなわち全モデル操作床反力モーメントの高周波成分）に、該ローパスフィルタ１１９ａの出力と反映済第１モデル操作床反力モーメント最新値との差を加え合わせることによって、第２モデル操作床反力モーメントを決定する処理と同等である。

第２モデル操作床反力モーメント
＝（全モデル操作床反力モーメント−ローパスフィルタの出力）
＋（ローパスフィルタの出力−反映済第１モデル操作床反力モーメント最新値）
……式７２

従って、本実施形態では、全モデル操作床反力モーメントの高周波成分（これは第１実施形態における全モデル操作床反力分配器１１８が決定する第２モデル操作床反力モーメントに相当する）に、歩容生成装置１００の連続する２つの制御周期の間の期間内での第１モデル操作床反力モーメントの変化分（詳しくは、現在時刻での全モデル操作床反力モーメントの低周波成分の値と、前記反映済第１モデル操作床反力モーメント最新値との差分）を重畳したものが、第２モデル操作床反力モーメントして決定されることとなる。

本実施形態は、以上説明した事項以外は、第１実施形態と同じである。

補足すると、全モデル操作床反力モーメントをハイパスフィルタ（ローカットフィルタ）に入力し、このハイパスフィルタの出力（高周波成分）から、反映済第１モデル操作床反力モーメント最新値を差し引くことによって、第２モデル操作床反力モーメントを決定するようにしてもよい。そして、この場合、ローパスフィルタ１１９ａを省略し、全モデル操作床反力モーメントから、ハイパスフィルタの出力を差し引くことで、第１モデル操作床反力モーメントを決定するようにしてもよい。

以上説明した本実施形態によれば、前記第１実施形態と同様の効果を奏することに加えて、次のような効果を奏することができる。すなわち、歩容生成装置１００の連続する２つの制御周期の間の期間内での第１モデル操作床反力モーメントの変化分を、第２モデル操作床反力モーメントに重畳させて摂動モデル演算部１２０にフィードバックすることができる。このため、第１モデル操作床反力モーメントのうち、歩容生成装置１００の歩容生成処理に反映されない部分となる上記変化分を、摂動モデル演算部１２０による上体水平位置摂動量（歩容生成装置１００が出力する目標歩容の運動の修正分）に反映させることができる。

この結果、全モデル操作床反力モーメントの周波数成分に依存することなく、該全モデル操作床反力モーメント成分により上体姿勢角偏差Δθを円滑に“０”に近づける効果をより一層高めることができる。

なお、本実施形態と、本発明との対応関係は、第１実施形態の場合と同様である。

［変形態様について］
次に、以上説明した第１実施形態又は第２実施形態の変形態様をいくつか説明する。

前記各実施形態では、制御対象外力要素として、目標ＺＭＰ周りの床反力モーメントの水平成分を用いたが、例えば、目標ＺＭＰ以外の点（例えば支持脚座標系の原点）の周りの床反力モーメントの水平成分や、全床反力中心点の位置を制御対象外力要素として使用してもよい。

また、前記各実施形態では、実ロボット外力操作量としての合成補償モーメントを、上体姿勢角偏差Δθ（姿勢状態量偏差）を“０”に近づけるための実姿勢補償全床反力モーメントＭdmdと、モデル状態量偏差ΔＸpを“０”に近づけるためのモデル状態補償全床反力モーメントＭmdlとを合成してなるモーメントして求めたが、例えば、Ｍdmdをそのまま、実ロボット外力操作量として用いてもよい。この場合には、モデル安定化制御演算部１１４を省略し、前記各実施形態における前述した制御処理を、Ｍmdlの値を“０”として実行するようにすればよい。

また、前記各実施形態では、合成補償モーメントとコンプライアンス制御用目標床反力モーメントとの差分に応じて、全モデル操作床反力モーメントを決定し、これを、ロボット１の目標運動を生成するための動力学モデルにフィードバックするようにしたが、このフィードバックを省略するようにしてもよい。この場合には、前記全モデル操作床反力モーメント分配器１１８、摂動モデル演算部１２０及び上***置姿勢修正部１２２を省略し、前記第１モデル操作床反力モーメントを“０”として、前記各実施形態における前述の制御処理を実行するようにすればよい。

なお、この場合において、歩容生成装置１００で使用する動力学モデルの所定の状態量と目標値との偏差に応じて前記モデル状態補償全床反力モーメントを算出するようにしてもよい。

また、歩容生成装置１００で使用する動力学モデルは、先にも述べた如く、より多くの質点を有するような動力学モデルを使用してもよい。さらに、例えば上体２４の姿勢を適宜変化させるような目標歩容を生成するような場合には、歩容生成装置１００、あるいは、摂動モデル演算部１２０で使用する動力学モデルに、上体２４の姿勢の変化に応じて発生する慣性力モーメントを表現する要素（例えばフラホイール）を含めるようにしてもよい。

また、前記各実施形態では、全モデル操作床反力モーメントを高周波成分と低周波成分とに分離し、低周波成分を歩容生成装置１００で使用する動力学モデルにフィードバックすると共に、高周波成分を摂動モデル演算部１２０で使用する動力学モデルにフィードバックするようにした。但し、例えば、モデル安定化制御演算部１１４、全モデル操作床反力モーメント分配器１１８、摂動モデル演算部１２０及び上***置姿勢修正部１２２を省略し、前記第１モデル操作床反力モーメントの代わりに、全モデル操作床反力モーメントをそのまま、歩容生成装置１００の動力学モデルにフィードバックするようにしてもよい。このようにすることにより、前記第２発明の一実施形態を構築できることとなる。

あるいは、例えば、全モデル操作床反力モーメント分配器１１８を省略し、前記第２モデル操作床反力モーメントの代わりに、全モデル操作床反力モーメントをそのまま、摂動モデル演算部１２０の動力学モデルにフィードバックすると共に、歩容生成装置１００の動力学モデルへのフィードバックを省略してもよい。このようにすることにより、前記第４発明の一実施形態を構築できることとなる。なお、この場合は、歩容生成装置１００と、摂動モデル演算部１２０と、上***置姿勢修正部１２２とにより、第４発明における目標運動決定手段が実現されることとなる。また、第４発明における動力学モデルは、歩容生成装置１００で使用する動力学モデルと、摂動モデル演算部１２０で使用する動力学モデルとにより実現されることとなる。

また、前記各実施形態では、脚式移動ロボットとして２足移動ロボット１を例として説明したが、本発明における脚式移動ロボットは、２足移動ロボットに限らず、３つ以上の脚体を有する脚式移動ロボットであってもよい。

１…２足移動ロボット（脚式移動ロボット）、２…脚体、２４…上体（基体）、２６…制御ユニット（制御装置）、１００…歩容生成装置（第１運動決定手段）、１１０…フィードバック操作量演算部（実ロボット外力操作量決定手段、全床反力中心点要求位置算出手段、制御対象外力要素目標値決定手段、モデル外力基本操作量決定手段）、１１８…全モデル操作床反力モーメント分配器（モデル外力操作量分配手段）、１２０…摂動モデル演算部（第２運動決定手段）、１２２…上***置姿勢修正部（目標運動決定手段）、Ｓ０３０…全床反力中心点許容領域決定手段。

Claims (5)

1. 基体から延設された複数の脚体を運動させて移動する脚式移動ロボットの実際の運動を目標運動に追従させつつ、該脚式移動ロボットに実際に作用する外力の構成要素のうち、少なくとも全床反力の作用点としての全床反力中心点の位置を規定する所定種類の要素である制御対象外力要素を該制御対象外力要素に対する目標値に追従させるように該脚式移動ロボットの動作制御を行う制御装置であって、
   前記脚式移動ロボットの実際の姿勢と前記目標運動により規定される目標姿勢とのずれの程度を表す姿勢状態量偏差を逐次観測し、該姿勢状態量偏差を“０”に近づける機能を有する第１のフィードバック操作量として、前記制御対象外力要素の目標値を操作するための実ロボット外力操作量を、少なくとも該姿勢状態量偏差の観測値に応じて逐次決定する実ロボット外力操作量決定手段と、
   前記決定された実ロボット外力操作量をそのまま使用して前記制御対象外力要素の目標値を操作すると仮定した場合に該実ロボット外力操作量に応じて決定される前記制御対象外力要素の目標値により規定される前記全床反力中心点の位置である全床反力中心点要求位置を算出する全床反力中心点要求位置算出手段と、
   前記目標運動のうちの前記脚式移動ロボットの各脚体の運動に基づいて規定される支持多角形内に、前記全床反力中心点の存在許容領域である全床反力中心点許容領域を設定する全床反力中心点許容領域設定手段と、
   前記算出された全床反力中心点要求位置が、前記設定された全床反力中心点許容領域内に存在するか否かを判断し、該判断結果に応じて前記制御対象外力要素の目標値を逐次決定する制御対象外力要素目標値決定手段とを備え、
   前記全床反力中心点許容領域設定手段は、少なくとも前記脚式移動ロボットの２つ以上の脚体を着床状態とする期間内において、前記支持多角形内に、方形状と異なる形状の前記全床反力中心点許容領域を設定する手段を有しており、
   前記制御対象外力要素目標値決定手段は、前記判断結果が肯定的である場合には、前記制御対象外力要素の目標値により規定される前記全床反力中心点の位置が前記算出された全床反力中心点要求位置に一致するように該制御対象外力要素の目標値を決定し、前記判断結果が否定的である場合には、前記制御対象外力要素の目標値により規定される前記全床反力中心点の位置が、前記設定された全床反力中心点許容領域のうち、前記算出された全床反力中心点要求位置に最も近い位置である制限後全床反力中心点位置に一致するように該制御対象外力要素の目標値を決定することを特徴とする脚式移動ロボットの制御装置。
2. 請求項１記載の脚式移動ロボットの制御装置において、
   前記脚式移動ロボットの動力学を表現するものとしてあらかじめ設定された動力学モデルを用い、該動力学モデルの動力学演算を含む演算処理を実行することにより前記目標運動を逐次決定する目標運動決定手段と、
   前記姿勢状態量偏差を“０”に近づける機能を有する第２のフィードバック操作量として、前記動力学モデル上でのロボットに作用させるべき外力を操作するためのモデル外力操作量を、少なくとも前記算出された全床反力中心点要求位置と前記決定された制御対象外力要素の目標値により規定される前記全床反力中心点の位置との差に応じて変化させるように逐次決定するモデル外力操作量決定手段とをさらに備え、
   前記目標運動決定手段は、前記決定されたモデル外力操作量に応じて前記動力学モデル上でのロボットに作用させるべき外力であるモデル外力を決定しつつ、該モデル外力を用いて前記動力学モデルの動力学演算を含む演算処理を行なうことにより前記目標運動を決定することを特徴とする脚式移動ロボットの制御装置。
3. 請求項１記載の脚式移動ロボットの制御装置において、
   前記脚式移動ロボットの動力学を表現するものとしてあらかじめ設定された第１動力学モデルを用い、該第１動力学モデルの動力学演算を含む演算処理を実行することにより前記目標運動を組成する第１運動を逐次決定する第１運動決定手段と、
   前記脚式移動ロボットの動力学を表現するものとしてあらかじめ設定された第２動力学モデルを用い、該第２動力学モデルの動力学演算を含む演算処理を実行することにより前記目標運動を組成する第２運動を逐次決定する第２運動決定手段と、
   前記決定された第１運動と第２運動とを合成することにより、前記目標運動を逐次決定する目標運動決定手段と、
   前記姿勢状態量偏差を“０”に近づける機能を有する第２のフィードバック操作量として、前記第１動力学モデル及び第２動力学モデルの上でのロボットに作用させる外力を操作するためのモデル外力基本操作量を、少なくとも前記算出された全床反力中心点要求位置と前記決定された制御対象外力要素の目標値により規定される前記全床反力中心点の位置との差に応じて変化させるように逐次決定するモデル外力基本操作量決定手段と、
   前記決定されたモデル外力基本操作量から前記第１動力学モデル及び第２動力学モデルのそれぞれに対して分配する操作量として、前記第１動力学モデル上でのロボットに作用させる外力を操作するための第１モデル外力操作量と、前記第２動力学モデル上でのロボットに作用させる外力を操作するための第２モデル外力操作量とを前記決定されたモデル外力基本操作量に応じて逐次決定するモデル外力操作量分配手段とをさらに備え、
   少なくとも前記実ロボット外力操作量決定手段、前記全床反力中心点要求位置算出手段、前記制御対象外力要素目標値決定手段、前記第２運動決定手段、前記目標運動決定手段、及びモデル外力基本操作量決定手段の処理の実行周期と、前記モデル外力操作量分配手段の処理のうちの前記第２モデル外力操作量を決定する処理の実行周期とは、前記第１運動決定手段の処理の実行周期よりも短い周期に設定されており、
   前記第１運動決定手段は、前記決定された第１モデル外力操作量に応じて前記第１動力学モデル上でのロボットに作用させるべき外力であるモデル外力を決定しつつ、該モデル外力を用いて前記第１動力学モデルの動力学演算を含む演算処理を行なうことにより前記第１運動を決定し、
   前記第２運動決定手段は、前記決定された第２モデル外力操作量に応じて前記第２動力学モデル上でのロボットに作用させるべき外力であるモデル外力を決定しつつ、該モデル外力を用いて前記第２動力学モデルの動力学演算を含む演算処理を行なうことにより前記第２運動を決定し、
   前記モデル外力操作量分配手段は、前記決定されたモデル外力基本操作量を組成する低周波成分と高周波成分とのうちの高周波成分よりも低周波成分に対する前記第１モデル外力操作量の変化の感度が相対的に高感度になり、且つ、前記低周波成分よりも高周波成分に対する前記第２モデル外力操作量の変化の感度が相対的に高感度になるように該第１モデル外力操作量と第２モデル外力操作量とを決定することを特徴とする脚式移動ロボットの制御装置。
4. 請求項２記載の脚式移動ロボットの制御装置において、
   前記動力学モデルの所定種類の状態量の値と、該状態量の所定の目標値との偏差であるモデル状態量偏差を逐次算出するモデル状態量偏差算出手段をさらに備え、
   前記実ロボット外力操作量決定手段は、前記姿勢状態量偏差を“０”に近づける機能と前記モデル状態量偏差を“０”に近づける機能とを有する前記第１のフィードバック操作量としての前記実ロボット外力操作量を、少なくとも前記姿勢状態量偏差の観測値に応じて決定したフィードバック操作量成分と前記モデル状態量偏差の算出値に応じて決定したフィードバック操作量成分とを合成することにより逐次決定し、
   前記モデル外力操作量決定手段は、前記姿勢状態量偏差を“０”に近づける機能と前記モデル状態量偏差を“０”に近づける機能とを有する前記第２のフィードバック操作量としての前記モデル外力操作量を、少なくとも前記算出された全床反力中心点要求位置と前記決定された制御対象外力要素の目標値により規定される前記全床反力中心点の位置との差に応じて変化させるように決定したフィードバック操作量成分と、前記モデル状態量偏差の算出値に応じて決定したフィードバック操作量成分とを合成することにより逐次決定することを特徴とする脚式移動ロボットの制御装置。
5. 請求項３記載の脚式移動ロボットの制御装置において、
   前記第２動力学モデルの所定種類の状態量の値と、該状態量の所定の目標値との偏差であるモデル状態量偏差を逐次算出するモデル状態量偏差算出手段をさらに備え、
   前記実ロボット外力操作量決定手段は、前記姿勢状態量偏差を“０”に近づける機能と前記モデル状態量偏差を“０”に近づける機能とを有する前記第１のフィードバック操作量としての前記実ロボット外力操作量を、少なくとも前記姿勢状態量偏差の観測値に応じて決定したフィードバック操作量成分と前記モデル状態量偏差の算出値に応じて決定したフィードバック操作量成分とを合成することにより逐次決定し、
   前記モデル外力基本操作量決定手段は、前記姿勢状態量偏差を“０”に近づける機能と前記モデル状態量偏差を“０”に近づける機能とを有する前記第２のフィードバック操作量としての前記モデル外力基本操作量を、少なくとも前記算出された全床反力中心点要求位置と前記決定された制御対象外力要素の目標値により規定される前記全床反力中心点の位置との差に応じて変化させるように決定したフィードバック操作量成分と、前記モデル状態量偏差の算出値に応じて決定したフィードバック操作量成分とを合成することにより逐次決定することを特徴とする脚式移動ロボットの制御装置。