(57)【要約】      （修正有）【課題】ロボットが環境から受ける６次元の力・モーメントを考慮して脚式移動ロボットの環境との接触状態の遷移を判定し、動作の生成を行う。【解決手段】ロボットに搭載されたセンサからの信号により環境を計測し、計測された情報を元に環境形状を数値化する手段、ロボットの動作を生成する目標となる６次元の力・モーメントを計算する手段、ロボットの重心の加速度と重心まわりの角運動量の時間微分に基づいて、ロボットに作用する６次元の力・モーメントを算出する手段を設ける。そして、与えられたロボットの動作に対応して、ロボットが環境の接触により環境から受ける６次元の力・モーメントを、複数の接触点におけるそれぞれの法線ベクトルを用いて、力・モーメント空間における接触力凸多面錐として算出し、ロボットに作用する６次元の力・モーメントが、接触力凸多面錐の内部に含まれるかどうかにより、接触状態の遷移を判定する。【選択図】図２
2011255500.tif 000002
【特許請求の範囲】
【請求項１】
少なくとも上体と前記上体に連結される複数本の脚部からなる脚式移動ロボットの制御装置において、
前記ロボットに搭載されたセンサからの信号により環境を計測し、計測された情報を元に環境形状を数値化する手段と、
前記ロボットの動作を生成する目標となる６次元の力・モーメントを計算する手段と、
前記ロボットの重心の加速度と重心まわりの角運動量の時間微分に基づいて、前記ロボットに作用する６次元の力・モーメントを算出する手段と、
与えられたロボットの動作に対応して、ロボットが環境の接触により環境から受ける６次元の力・モーメントを、複数の接触点におけるそれぞれの法線ベクトルを用いて、力・モーメント空間における接触力凸多面錐として算出し、前記ロボットに作用する６次元の力・モーメントが、前記接触力凸多面錐の内部に含まれるかどうかを判定して、接触状態の遷移を判定する手段とを備えることを特徴とする脚式移動ロボットの制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、サービスロボットとして家庭やオフィスで活動したり、作業現場で人の代わりに働く脚式移動型ロボットの動作を生成すると共に、動作を制御する脚式移動ロボットの制御装置に関するものである。
【背景技術】
【０００２】
脚式移動ロボットは、転倒せずに様々な環境で多種多様な作業を行うことが要求される。従来、脚式移動ロボットの床との接触状態の遷移を判定したり、ロボットが転倒しないような動作を生成したり、あるいはセンサ情報に基づき動作を制御する場合には、ロボットが床から受ける反力の作用中心点であるＺＭＰ（Ｚｅｒｏ−Ｍｏｍｅｎｔ  Ｐｏｉｎｔ）が用いられてきた。ここで、ＺＭＰは床反力の作用中心点であるため、床反力中心点と呼ばれる場合もある。
【０００３】
脚式移動ロボットが、水平で摩擦係数が十分に大きな床面上で動作する場合、ＺＭＰが支持多角形の内部に含まれる限り、ロボットは転倒せずに動作を継続できる。その反面、ＺＭＰが支持多角形の端に存在すると、ロボットの足裏と床面の接触が離れて接触状態が遷移し、ロボットが転倒する。例えば、非特許文献１または特許文献１においては、ＺＭＰを規範として脚式移動ロボットの歩行動作を生成する手法が提案されている。また、特許文献４においては、脚部に装着された力センサにより実際のＺＭＰを計測し、ＺＭＰをその目標値に制御することにより、ロボットの姿勢を制御する手法が提案されている。
【０００４】
特許文献２においては、近似的にＺＭＰを用いて脚式移動ロボットが階段を上る動作が生成されているが、しかし、前記の理由により階段を上る場合は全ての接触点が単一の平面には含まれないため、厳密な意味においてＺＭＰを用いても接触状態の遷移は判定できない。
【０００５】
また、特許文献３に示されるように、先に本出願人が提案した脚式移動ロボットの制御装置の技術では、床からロボットにはたらく６次元の力・モーメントを考慮して、接触点が単一の平面に含まれない場合の接触状態の遷移の判定、動作生成の手法、ならびに動作制御の手法について提案している。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開２００２−３２６１７３号公報
【特許文献２】特許第３１３２１５６号
【特許文献３】特開２００６−２４７８００公報
【特許文献４】特開平５−３０５５８５号公報
【非特許文献】
【０００７】
【非特許文献１】高西、石田、山崎、加藤、「２足歩行ロボットＷＬ−１０ＲＤによる動歩行の実現」，日本ロボット学会誌、ｖｏｌ．３，ｎｏ．４，ｐｐ．３２５−３３６，１９８５
【発明の概要】
【発明が解決しようとする課題】
【０００８】
しかしながら、先に述べたように、ＺＭＰは床反力の作用中心点であるため、ＺＭＰを用いても床面上の２次元の情報しか得られない。それに対して、床からロボットには３次元の力と３次元のモーメントが働いている。ＺＭＰでは、合計６次元の力・モーメントのうち２次元の情報しか表現できないため、限定された接触状態の遷移しか扱うことができない。
【０００９】
したがって、従来の技術では、例えば、ロボットの足裏が床面上を滑るかどうかを判定することは不可能であり、ロボットと床面の間に十分な摩擦が存在する場合でも、ロボットと環境との全ての接触点が単一の平面に含まれない場合は接触状態の遷移を判定できない。ＺＭＰによってロボットの接触状態の遷移が判定できるのは、水平で摩擦が無視できるような床面や、水平で滑りが生じない程度に摩擦係数が十分大きい床面上に限られる。
【００１０】
本発明の第１の目的は、ロボットが環境から受ける６次元の力・モーメントを考慮し、脚式移動ロボットの環境との接触状態の遷移を判定して、動作の生成を行う脚式移動ロボットの制御装置を提供することにある。
【００１１】
ロボットが環境から受ける６次元の力・モーメントを考慮することで、各接触点が単一の平面に含まれない場合を扱うことが可能であるが、本発明においては、特にロボットの接触点における法線方向が接触点によって異なる環境を想定する。ここでの法線方向が接触点によって異なる環境とは、水平な面のみから構成される階段は含まれず、傾きの異なる複数の斜面から構成されるような環境の事を指す。
【００１２】
本発明の第２の目的は、ロボットに搭載された環境の形状を計測するセンサの情報を元に、６次元の力・モーメントを考慮し、脚式移動ロボットの環境との各接触点における法線方向が異なる環境において、脚式移動ロボットの動作を生成する脚式移動ロボットの制御装置を提供することにある。ここで、環境の形状を計測するセンサとは、視覚センサであるステレオカメラやレーザセンサ等を指す。
【００１３】
本発明の第３の目的は、ロボットに搭載された環境の形状を計測するセンサの情報を元に、６次元の力・モーメントを考慮し、脚式移動ロボットの環境との各接触点における法線方向が異なる環境において、ロボットが動作する際に、センサによって計測された環境の形状の誤差を考慮してロボットを制御する脚式移動ロボットの制御装置を提供することである。
【課題を解決するための手段】
【００１４】
第１の目的を達成するために、本発明による脚式移動ロボットの制御装置は、少なくとも上体と前記上体に連結される複数本の脚部からなる脚式移動ロボットの制御装置において、前記ロボットに搭載されたセンサからの信号により環境を計測し、計測された情報を元に環境形状を数値化する手段と、前記ロボットの動作を生成する目標となる６次元の力・モーメントを計算する手段と、前記ロボットの重心の加速度と重心まわりの角運動量の時間微分に基づいて、ロボットが環境から受ける６次元の力・モーメントを算出する手段と、与えられたロボットの動作に対応して力・モーメント空間での接触力凸多面錐を、複数の接触点におけるそれぞれの法線ベクトルを用いて、ロボットと環境の接触においてロボットが環境から受ける６次元の力・モーメントを算出する手段と、与えられたロボットの動作に対応して力・モーメント空間での接触力凸多面錐を算出する手段と、算出された６次元の力・モーメントが前記接触力凸多面錐の内部に含まれるかどうかを判定して、接触状態の遷移を判定する手段を有する如く構成した。
【００１５】
また、６次元の力・モーメントを考慮してロボットの動作を生成するために、目標となる６次元の力・モーメントを計算する手段、さらに目標となる６次元の力・モーメントを実現するようなロボットの動作を生成する手段を有する如く構成した。
【００１６】
上記した第２の目的を達成するために、本発明による脚式移動ロボットの制御装置は、少なくとも上体と前記上体に連結される複数本の脚部からなる脚式移動ロボットが、第１の目的を達成するための手段に加えて、ロボットに搭載されたセンサにより、環境を計測する手段、ならびに計測された情報を元に環境の形状を数値化する手段を有する如く、脚式移動ロボットの制御装置を構成した。
【００１７】
これにより、第１の目的を達成するための手段に加えて、ロボットに搭載されたセンサにより環境を計測する手段、ならびに計測された情報を元に環境の形状を数値化する手段を有するために、ロボットが未知の環境においても、接触状態の遷移を考慮して脚式移動ロボットの動作を生成できる。
【００１８】
また、上記した第３の目的を達成するために、本発明による脚式移動ロボットの制御装置は、少なくとも上体と前記上体に連結される複数本の脚部からなる脚式移動ロボットが、第１、第２の目的を達成するための手段に加えて、環境の計測誤差を検知した時点で動作を一旦停める手段、そして手先や足先を動作させて計測された環境の誤差量を見積もる手段、ならびに前記誤差量を修正してロボットを動作させる手段を有する如く、脚式移動ロボットの制御装置を構成した。
【００１９】
これにより、第１の目的および第２の目的を達成するための手段に加えて、環境の計測誤差を検知された時点で動作を一旦停める機能、手先や足先を動作させて計測された環境の誤差量を見積もる手段、ならびに前記誤差量を修正してロボットを動作させる手段を有するため、環境の計測情報の誤差を補償して、接触状態の遷移考慮した脚式移動ロボットの動作が実現できる。
【００２０】
具体的に、本発明による脚式移動ロボットの制御装置は、第１の態様として、少なくとも上体と前記上体に連結された複数本の脚部からなる脚式移動ロボットの制御装置において、ロボットに加わる６次元の力・モーメントを算出することにより、複数の接触点における法線ベクトルが同一でないロボットと環境の接触において、少なくとも一部の接触点において接触が離れるときの接触状態の遷移を判定する手段を備えることを特徴とするものである。
【００２１】
第２の態様として、本発明による脚式移動ロボットの制御装置は、少なくとも上体と前記上体に連結された複数本の脚部からなる脚式移動ロボットの制御装置において、ロボットと環境の複数の接触点における法線ベクトルが同一でない場合に、ロボットに加わる６次元の力・モーメントを考慮することにより、前記接触状態の遷移を判定することを特徴とするものである。
【００２２】
第３の態様として、本発明による脚式移動ロボットの制御装置は、少なくとも上体と前記上体に連結された複数本の脚部からなる脚式移動ロボットの制御装置において、ロボットと環境の複数の接触点における法線ベクトルが同一でない場合に、ロボットに加わる６次元の力・モーメントを考慮することにより、前記接触状態が遷移しないロボットの動作を生成する手段を備えたことを特徴とするものである。
【００２３】
第４の態様として、本発明による脚式移動ロボットの制御装置は、少なくとも上体と前記上体に連結された複数本の脚部からなる脚式移動ロボットの制御装置において、ロボットと環境の複数の接触点における法線ベクトルが同一でない場合に、ロボットに加わる６次元の力・モーメントを考慮することにより、センサを用いた環境の計測に基づいて、前記接触状態が遷移しないロボットの動作を生成する手段を備えたことを特徴とするものである。
【００２４】
第５の態様として、本発明による脚式移動ロボットの制御装置は、少なくとも上体と前記上体に連結された複数本の脚部からなる脚式移動ロボットの制御装置において、ロボットと環境の複数の接触点における法線ベクトルが同一でない場合に、ロボットに加わる６次元の力・モーメントを考慮することにより、センサを用いた環境の計測結果に誤差がある場合に、前記接触状態が遷移しないロボットの動作を生成する手段を備えたことを特徴とするものである。
【発明の効果】
【００２５】
このような特徴を有する本発明による脚式移動ロボットの制御装置においては、環境からロボットが受ける６次元の力・モーメントを計測し、力・モーメント空間において凸錐を算出し、計測された６次元の力・モーメントが凸錐の内部に含まれるかどうかを判定する手段を有するため、ロボットと環境とのあらゆる接触状態の遷移を判定でき、脚式移動ロボットが動作する際の安定性を高めることができる。
【００２６】
また、さらに目標となる６次元の力・モーメントを計算する手段、また目標となる６次元の力・モーメントを実現するようなロボットの動作を生成する手段を有するため、接触状態の遷移を考慮して脚式移動ロボットの動作を生成でき、動作の安定性を高めることができる。
【図面の簡単な説明】
【００２７】
【図１】この発明の実施の形態に係る制御装置が適用される脚式移動ロボット、より具体的には２足歩行ロボットを全体的に示す概略図である。
【図２】各接触点における法線方向が異なる環境において接触状態の遷移を判定するフローチャートである。
【図３】各接触点での法線方向と接触力凸多面錐の関係を説明する図である。
【図４】６次元の力・モーメントを考慮してロボットの動作を生成する処理のフローチャートである。
【図５】鉛直方向の力を垂直抗力と摩擦力に分配したものを目標値とする説明図である。
【図６】６次元の力・モーメントを考慮してロボットの動作を生成する処理をより詳細に説明する第１のブロック図である。
【図７】６次元の力・モーメントを考慮してロボットの動作を生成する処理をより詳細に説明する第２のブロック図である。
【図８】センサの計測誤差を考慮して修正したアルゴリズムを説明する図である。
【発明を実施するための形態】
【００２８】
以下、添付図面を参照して、この発明の一つの実施の形態に係る脚式移動ロボットの制御装置を説明する。脚式移動ロボットとしては２足歩行ロボットを例にとる。
【００２９】
図１は、この発明の実施の形態に係る制御装置が適用される脚式移動ロボット、より具体的には２足歩行ロボットを全体的に示す概略図である。ΣＲは基準座標系であり、ΣＢはロボット腰部に固定された座標系であり、Σｉは第ｉリンク（ｉ＝１，…，ｎ）の重心に固定された座標系を表す。
【００３０】
ΣＲに関する位置ベクトルについて、ＰHj（＝［ｘHj  ｙHj  ｚHj］T）（ｊ＝１，２）は各手先を表し、ＰFj（＝［ｘFj  ｙFj  ｚFj］T）（ｊ＝１，２）は各足裏のリンクに固定された点を表し、ＰLi（＝［ｘLi  ｙLi  ｚLi］T）はΣｉの原点を表し、ＰB（＝［ｘB  ｙB  ｚB］T）はΣＢの原点を表す。
【００３１】
また、各手先の接触領域と各足裏の支持領域を構成する多角形の頂点を手先か足先かを区別せず、Ｐk（ｋ＝１，…，Ｋ）とし、ＰG（＝［ｘG  ｙG  ｚG］T）はロボットの重心を表す位置ベクトルである。さらに、ｚ0はロボットが水平面上を歩行する場合の床面の高さを表すものとする。ｆHi（＝［（ｆHi）x  （ｆHi）y  （ｆHi）z］T），τHj（ｊ＝１，２）は各手先が受ける力・モーメントとし、ｆFj，τFj（ｊ＝１，２）は各足裏が受ける力・モーメントである。
【００３２】
同様に、ｆkは点Ｐkで受ける力とする。なお、図１ではモーメントを２重矢印で表している。ｎkは、点Ｐkにおける単位拘束法線ベクトルを表す。Ｉi，ωiはそれぞれ第ｉリンクの基準座標系Σｉに関する慣性テンソル、角速度ベクトルを表す。以上に述べた位置ベクトル、力・モーメントは、全て基準座標系ΣＲで表すものとする。
【００３３】
本発明による脚式移動ロボットの制御装置は、第一の実施形態として、脚式移動ロボットが歩行などの動作を行うとき、環境からロボットが受ける３次元の力と３次元のモーメントを算出する手段、つまり、ロボットが環境から受ける６次元の力・モーメントを算出する手段と、与えられたロボットの姿勢に対応して力・モーメント空間において形成される凸錐を算出する手段を備えている。これらについて説明する。
【００３４】
また、これらの算出する手段を利用することで、各接触点における法線方向が異なる環境において、接触状態の遷移を判定する。この手法を、図２のフローチャートに基づいて説明する。
【００３５】
処理を開始すると、まず、環境の計測処理を行い（ステップ２０１）、入力として、ロボット重心の加速度ＰGと重心まわりの角運動量の時間微分Lならびに環境の形状に関する情報を与える。ロボットと環境が望みの接触状態を維持するという仮定の下で、慣性力および重力によりロボットが受ける力・モーメントを基準座標系ΣＲで表したものは、次の（１）式および（２）式により与えられるので、これらの演算処理を行う（ステップ２０２）。
【数１】
2011255500.tif 000003
【数２】
2011255500.tif 000004
これらの式において、Ｍはロボットの質量、ｇは重力ベクトルを表す。
【００３６】
ロボットに力ｆGおよびモーメントτGが加わったとき、ロボットは接触している環境から反力・モーメントを受ける。ロボットが接触している環境から受ける反力を基準座標系ΣＲで表したものｆCを、ロボットが受ける基準座標系ΣＲのまわりのモーメントτCをとすると、これらは手先・足裏と環境との接触面の幾何学的な形状から、（３）式、（４）式が得られる。
【数３】
2011255500.tif 000005
【数４】
2011255500.tif 000006
【００３７】
さらに、各接触点がロボットに対して発生可能な力の集合は、摩擦コーンをＬ角錐で近似した場合、（５）式により与えられる。
【数５】
2011255500.tif 000007
ここにおいて、μkは各点の摩擦係数、ｔlkは（ｎk＋μkｔlk）が各点の摩擦コーンを近似するＬ角錐の側辺となるような単位ベクトルであり、ｎkが各接触点における法線方向を表す単位ベクトルである。この単位ベクトルの向きが各接触点で異なる場合を本発明の脚式移動ロボットの制御装置では取り扱う。
【００３８】
また、εlkは各点に加わる接線方向の力の大きさにより定まる非負のスカラーである。（５）式を（３）式および（４）式に代入すると、（ｆC，τC）がとり得る値の集合は、次の（６）式および（７）式のとおりに得られる。
【数６】
2011255500.tif 000008
【数７】
2011255500.tif 000009
ここでの（ｆC，τC）はベクトルの非負１次結合で表されていることから、これらが力・モーメントの空間でとり得る値の集合は凸多面錐になる。この力・モーメントの集合を、接触力凸多面錐と呼ぶ。
【００３９】
上記の（６）式および（７）式における各接触点での法線方向と接触力凸多面錐の関係を、図３を用いて説明する。図３に示すように、直方体の足が不整地に４点で接触する場合には、各接触点において法線方向が異なる。そして、各接触点における接触力を全ての接触点について合わせたものが、（ｆC，τC）である。
【００４０】
脚式ロボットに力ｆGおよびモーメントτGが加わった場合、（−ｆG，−τG）が、（６）式および（７）式によって表される接触力凸多面錐に含まれる場合には、接触状態は弱安定となり、釣り合いの関係が成立する可能性が生じる。このとき、釣り合いが必ずしも成立するとは言えないが、（−ｆG，−τG）が凸多面錐に含まれない場合には、釣り合いの関係は成立せず、接触状態は必ず遷移する。
【００４１】
判定処理（ステップ２０３）は接触力凸多面錐に含まれるか否かの判定を行う。図２に示すように、（−ｆG，−τG）が（６）式、（７）式によって表される接触力凸多面錐に含まれるかの判定では、ｆG＝−ｆC，τG＝−τCとおき、εlk（ｋ＝１，…，Ｋ，  ｌ＝１，…，Ｌ）が非負であり、なおかつ、その三つ以上が“０”では無いことを数値的に確認することでなされる。この判定の結果に基づいて、「接触状態は遷移しない」判定結果または「遷移状態が遷移する」判定結果を出力する（ステップ２０４，ステップ２０５）。
【００４２】
次に、６次元の力・モーメントを考慮してロボットの動作を生成する手法の概要を説明する。図４は、６次元の力・モーメントを考慮してロボットの動作を生成する処理のフローチャートである。この処理では、各接触点における法線方向が既知であると仮定する。つまり、各接触点における法線方向を示すデータについては、前処理により予め取得されているものとする。処理を開始すると、まず、ステップＳ１において、目標となる手先、足先、ならびに腰の速度を与える。次に、ステップＳ２において、重心まわりの角運動量を計算する。さらに、ステップＳ３において、目標となる６次元の力・モーメントの軌道を計算する。この情報を基にして、次のステップＳ４において、接触力方程式を解いて、目標重心軌道を求める。最後に、ステップＳ５において、分解運動量制御で目標腰速度を修正する。そして、ステップＳ６において、動作の生成処理を行う。もし、ステップＳ１で定めた腰速度と、ステップＳ５で定めた腰速度が大きく異なる場合は、後述するように、繰り返し計算を行う。
【００４３】
つぎに、これらのそれぞれの処理について、詳細に説明する。まず、ステップＳ１においては、目標となる手先、足先、ならびに腰の目標速度を与える。これは、ロボットに搭載されたセンサの情報を基に、環境の形状を計測する。ここでは、計測は十分正確であると仮定し、計測誤差を考慮する手法については、後述する。環境のモデルが与えられると、環境と望みの接触状態を維持し、別の接触状態へと遷移するような、望みの手先や足先の軌道が計算できる。
【００４４】
また、手先・足先の軌道を時間微分することで、速度・角速度が得られる。腰の速度・角速度は、最初はヒューリスティックに与えることにし、より正確な腰の速度は、ステップＳ５において、あらためて計算する。
【００４５】
ステップＳ１において、手先、足先、ならびに腰の速度が与えられたときに、ステップＳ２において重心まわりの角運動量を計算する手法を説明する。基準座標系ΣＲで表現された腰に固定された座標系ΣＢの並進速度、角速度をυB、ωBとし、ロボットの全関節速度を縦に並べたｎ×１ベクトルをベクトルθの時間微分とする。このとき、ロボット全体の並進運動量ｐおよび重心まわりの角運動量Ｌは次式で与えられる。
【数８】
2011255500.tif 000010
ここで、Ｍはロボットの全質量、Ｅは３×３の単位行列、γB→Gは、腰リンクから全重心ＰGへの（３×１）ベクトルの位置ベクトル、Ｉは全重心まわりの（３×３）の慣性行列である。また、Ｍθ、Ｈθは、関節速度がそれぞれ並進運動量と角運動量に及ぼす効果を表わす慣性行列である。
【００４６】
また、記号＾は（３×１）のベクトルを外積と等価な（３×３）の歪対称行列に変換する操作を表わす。手先や足先の速度・角速度と関節速度はヤコビ行列により関連付けられている。（８）式のベクトルθの時間微分の部分に、この関係式を代入することで、手先、足先、ならびに腰の速度・角速度が与えられた時のロボット全体の運動量・角運動量は、次の形で求められる。
【数９】
2011255500.tif 000011
ここで、速度・角速度ベクトルを、（１０）式のようにまとめた。
【数１０】
2011255500.tif 000012
【００４７】
ステップＳ３において、目標となる６次元の力・モーメントを定義する手法について説明する。より一般的に説明すると、ロボットと環境との接触状態によって一意に定まる（６）式および（７）式で定義される凸多面錐の内部に、（−ｆG，−τG）が含まれるように、その軌道を定めると、ロボットと環境との接触状態は変化しない。ここでは、十分に摩擦があり、法線方向が各接触点で異なる不整地を例にとって、目標となる力・モーメントを定義する方法について説明する。
【００４８】
十分に摩擦があり、接触点によって法線方向が異なる不整地では、（６）式および（７）式は任意のスカラーδlkを用いて、以下のような６つの式で表すことができる。
【数１１】
2011255500.tif 000013
【数１２】
2011255500.tif 000014
【数１３】
2011255500.tif 000015
【数１４】
2011255500.tif 000016
【数１５】
2011255500.tif 000017
【数１６】
2011255500.tif 000018
ここでの（１１）式、（１２）式および（１３）式が力に関する関係式であり、（１４）式、（１５）式および（１６）式がモーメントに関する関係式である。また、それぞれｘ，ｙ，ｚ方向成分の式を表している。
【００４９】
十分に摩擦がある場合には、摩擦により発生する接触力の空間は線形部分空間になるので、接触力全体の集合は、この線形部分空間とその直交補空間の中の凸多面錐との直積になる。したがって、接触力を各線形部分空間内で独立に計画することができる。さらに、摩擦により発生する接触力の空間は、各軸方向毎に線形部分空間になるので、やはり独立に計画することができる。
【００５０】
（１１）式〜（１６）式は非線形微分方程式であり、これを満たすために操作可能な変数は加速度ｘ，ｙ，ｚ、速度Ｌx，Ｌy，Ｌzの６つある。しかしながら、一般に速度Ｌx，Ｌy，Ｌzを操作すると、全身を大きく振る等の不自然な動作が生成されることが多いため、速度Ｌx，Ｌy，Ｌzは操作せず、加速度ｘ，ｙ，ｚのみを操作して、ｚ軸方向の並進力の方程式、ｘ，ｙ軸まわりのモーメントの方程式の３つのみを満たす動作の生成を考える。残り３つの方程式については、摩擦が十分に働くという仮定をし、ロボットが２点以上で接地しているときには常に満足される。
【００５１】
各接触点における法線方向の単位ベクトル用いて、加速度ｚGを決める。これは重心の鉛直方向の軌道を定めることに相当する。ｚ軸方向の力の釣り合いについては、（１３）式が成立する必要がある。ここで、（１３）式を
【数１７】
2011255500.tif 000019
【数１８】
2011255500.tif 000020
とｚ軸方向の力を垂直抗力と摩擦力に分配し、これを満足するように、加速度ｚGを決める。（１７）式と（１８）式に分解するのは、図５に示すように、鉛直方向の力を垂直抗力と摩擦力に分配したものを目標値とすることに相当する。
【００５２】
αの決め方としては、例えば、各接触点での面の傾きの平均を取ることが考えられる。また、決めたαを（１７）式に代入した結果、ε0kは少なくとも３つのｋについて、０＜ε0kとなることが必要である。なお、本手法では、十分に摩擦力が働くという仮定をおいているが、ここでの分配方法の如何によって、生成された動作がクーロン摩擦の制約を満たすかどうかに影響する。
【００５３】
次に、水平軸まわりのモーメントの釣り合い式である（１４）式および（１５）式を次の式に変形する。
【数１９】
2011255500.tif 000021
【数２０】
2011255500.tif 000022
この（１９）式および（２０）式を満たすように加速度ｘG，ｙGを決める。
【００５４】
最後に、水平方向の鉛直力と鉛直軸まわりのモーメントの釣り合いについては、（１１）式，（１２）式，（１６）式が成立することが必要である。
【００５５】
得られた加速度ｘG，ｙG，ｚGを（１１）式〜（１６）式に代入した式において、少なくても３つのｋについて、０＜ε0kである必要がある。また、δlkが一意解を持つかどうかは、（１１）式〜（１６）式の左辺をδlkについて整理することで得られる
【数２１】
2011255500.tif 000023
の係数行列が正則であることで確認される。 
【００５６】
次に、ステップＳ４の目標重心軌道を求めて設定する手段を説明する。ここでも、十分に摩擦のある環境を想定して説明する。水平軸周りのモーメントの釣り合い式において、重心の加速度を中心差分近似することにすると、離散時間表現で、
【数２２】
2011255500.tif 000024
【数２３】
2011255500.tif 000025
とかける。ここに、ｉ＝１，…，Ｎで境界条件は、
【数２４】
2011255500.tif 000026
で与えられる。これらの差分方程式を離散時刻ｉ＝１，…，Ｎについて連立し、これらの連立１次方程式を解くことにより、ｘG，ｙGの目標軌道を求めることができる。
【００５７】
更に、ステップＳ５において、分解運動量制御により修正腰位置を計算するについて説明する。（９）式を以下のように書き直す。
【数２５】
2011255500.tif 000027
ここで、
【数２６】
2011255500.tif 000028
である。このとき、腰の速度・各速度Ａはの擬似逆行列であるＡ†を用いて次式で与えられる。
【数２７】
2011255500.tif 000029
【００５８】
ここで、図４で示したアルゴリズムをより詳細に説明するために、図６のブロック図を用いる。図６のブロック図によるデータ処理の流れにしたがって、処理を順に説明すると、この動作パターン生成の処理は、次のような手順により行われる。
【００５９】
すなわち、
ステップ１０１；ロボットに搭載されたセンサで環境を計測し、環境の形状ｆ（ｘ）を得る（処理ブロック６０１）。
ステップ１０２；環境の情報を用いて、足先目標速度・角速度（ｖFi，ωFi）、手先目標速度・角速度（ｖHi，ωHi），腰座標の目標速度・角速度（ｖB，ωB）を指定する（処理ブロック６０２）。
ステップ１０３；（９）式により目標角運動量Ｌrefを求める。さらにこれを微分して、Ｌrefの速度を求める（処理ブロック６０３）。
ステップ１０４；（１１）式〜（１６）式により、重心の位置，Ｌrefの速度，目標反力・トルクから成る微分方程式を立てる。
ステップ１０５；前項の微分方程式を数値解法により解き、重心の目標軌道を計算する（処理ブロック６０４）。
ステップ１０６；計算された重心の目標軌道から、目標運動量ｐrefを求める（処理ブロック６０５）。
ステップ１０７；求まった目標運動量ｐrefと目標角運動量Ｌrefから、（２７）式により目標速度・角速度（ｖB，ωB）を求める（処理ブロック６０６）。
ステップ１０８；処理ステップ１０２で指定した目標速度・角速度（ｖB，ωB）と、処理ステップ１０７により求まった目標速度・角速度（ｖB，ωB）を比べ、十分に近い場合は、処理ステップ１０９へ進む。そうでない場合は、処理ステップ１０７で求まった目標速度ｖBを新たな腰座標目標速度として、処理ステップ１０２に戻る。
ステップ１０９；逆運動学計算により、腕と脚の関節軌道を導出する。
【００６０】
ステップ１０８を設けているのは、ステップ１０７において目標速度ｖBの値は新しくなるが、Ｌrefの速度は古い目標速度ｖBから計算されているため、不自然な動作が生成される可能性があるためである。ステップ１０８からステップ１０２へ戻る際には、目標速度ｖBのみ更新し、角速度ωBは更新しない。
【００６１】
今まで述べた手法では、正確な環境のモデルが与えられていることが前提であった。しかし、センサを用いて環境を計測する場合、計測誤差が生じる。第３の実施例では、センサの計測誤差を考慮して、先に説明したアルゴリズムを次のように修正する。このアルゴリズムを説明するために、図７を用いる。
【００６２】
ステップ２０１；センサにより環境を計測し、環境の形状ｆ（ｘ）、ならびに法線方向の最大誤差距離Δｆを得る（処理ブロック７０１）。
ステップ２０２；足先目標速度・角速度（ｖFi，ωFi）、手先目標速度・角速度（ｖHi，ωHi），腰座標の目標速度・角速度（ｖB，ωB）を指定する（処理ブロック７０２）。ただし、足先または手先が環境と新たに接触する際には、目標位置を接触面の法線方向にセンサの最大誤差距離分干渉する位置とするとともに、接地速度は可能な限り小さな値とする。
ステップ２０３；（９）式により目標角運動量Ｌrefを求める。さらにこれを微分して、Ｌrefの速度を求める（処理ブロック７０３）。
ステップ２０４；（１１）式〜（１６）式により、重心の位置、Ｌrefの速度、目標反力・トルクから成る微分方程式を立てる。ただし、足先または手先が環境と新たに接触する際には、新たな接触が発生する前の支持足・手に、時間Δｔだけ接触発生後も支持が残るように、目標反力・トルクを設定する。
ステップ２０５；前項の微分方程式を数値解法により解き、重心の目標軌道を計算する（処理ブロック７０４）。
ステップ２０６；計算された重心の目標軌道から、目標運動量ｐrefを求める（処理ブロック７０５）。
ステップ２０７；求まった目標運動量ｐrefと目標角運動量Ｌrefから、（２７）式により目標速度・角速度（ｖB，ωB）を求める（処理ブロック７０６）。
ステップ２０８；ステップ２０２で指定した目標速度・角速度（ｖB，ωB）と、ステップ２０７で求まった目標速度・角速度（ｖB，ωB）を比べ、十分に近い場合はステップ２０９へ進む。そうでない場合は、ステップ２０７で求まった目標速度ｖBを新たな腰座標目標速度として、ステップ２０２に戻る。
ステップ２０９；逆運動学計算により、腕と脚の関節軌道を導出する。
【００６３】
以上の方法により動作パターンを生成し、実行時には、新たな処理を追加する。ここで、床面の高さおよび傾斜の計測値には誤差があり、足裏面は１点接触、線接触状態を経由して、面接触状態に至ると仮定する。この処理を図８に示し、この図を順に説明する。
【００６４】
ステップ３０１；足裏面が１点接触したことを力センサの値により検出する。接触位置はＺＭＰの位置と一致する（図８（ａ））。
ステップ３０２；接触点を中心として、足裏位置ｐFを鉛直面内で回転して降下させる。足先の速度・角速度（ｖF，ωF）を求め、これを足先の目標速度・角速度とする（図８（ｂ））。
ステップ３０３；ＺＭＰが大きく移動したら、線接触状態が遷移したと判定し、ＺＭＰの移動直線経路を計測値から最小自乗推定し、接触線分を求める（図８（ｃ））。
ステップ３０４；接触線分を軸として、足裏位置ｐFを鉛直面内で回転して降下させる。足先の速度・角速度（ｖF，ωF）を求め、これを足先の目標速度・角速度とする。
ステップ３０５；ＺＭＰが接触線分から大きく移動したら面接触状態に遷移したと判定し、目標足先速度・角速度を零にする。（図８（ｄ））
【図１】
2011255500.tif 000030
【図２】
2011255500.tif 000031
【図３】
2011255500.tif 000032
【図４】
2011255500.tif 000033
【図５】
2011255500.tif 000034
【図６】
2011255500.tif 000035
【図７】
2011255500.tif 000036
【図８】
2011255500.tif 000037