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

Abstract

【課題】ロボットが環境から受ける６次元の力・モーメントを考慮して脚式移動ロボットの環境との接触状態の遷移を判定し、動作の生成を行う。
【解決手段】ロボットに搭載されたセンサからの信号により環境を計測し、計測された情報を元に環境形状を数値化する手段、ロボットの動作を生成する目標となる６次元の力・モーメントを計算する手段、ロボットの重心の加速度と重心まわりの角運動量の時間微分に基づいて、ロボットに作用する６次元の力・モーメントを算出する手段を設ける。そして、与えられたロボットの動作に対応して、ロボットが環境の接触により環境から受ける６次元の力・モーメントを、複数の接触点におけるそれぞれの法線ベクトルを用いて、力・モーメント空間における接触力凸多面錐として算出し、ロボットに作用する６次元の力・モーメントが、接触力凸多面錐の内部に含まれるかどうかにより、接触状態の遷移を判定する。
【選択図】図２

Description

本発明は、サービスロボットとして家庭やオフィスで活動したり、作業現場で人の代わりに働く脚式移動型ロボットの動作を生成すると共に、動作を制御する脚式移動ロボットの制御装置に関するものである。

脚式移動ロボットは、転倒せずに様々な環境で多種多様な作業を行うことが要求される。従来、脚式移動ロボットの床との接触状態の遷移を判定したり、ロボットが転倒しないような動作を生成したり、あるいはセンサ情報に基づき動作を制御する場合には、ロボットが床から受ける反力の作用中心点であるＺＭＰ（Ｚｅｒｏ−Ｍｏｍｅｎｔ Ｐｏｉｎｔ）が用いられてきた。ここで、ＺＭＰは床反力の作用中心点であるため、床反力中心点と呼ばれる場合もある。

脚式移動ロボットが、水平で摩擦係数が十分に大きな床面上で動作する場合、ＺＭＰが支持多角形の内部に含まれる限り、ロボットは転倒せずに動作を継続できる。その反面、ＺＭＰが支持多角形の端に存在すると、ロボットの足裏と床面の接触が離れて接触状態が遷移し、ロボットが転倒する。例えば、非特許文献１または特許文献１においては、ＺＭＰを規範として脚式移動ロボットの歩行動作を生成する手法が提案されている。また、特許文献４においては、脚部に装着された力センサにより実際のＺＭＰを計測し、ＺＭＰをその目標値に制御することにより、ロボットの姿勢を制御する手法が提案されている。

特許文献２においては、近似的にＺＭＰを用いて脚式移動ロボットが階段を上る動作が生成されているが、しかし、前記の理由により階段を上る場合は全ての接触点が単一の平面には含まれないため、厳密な意味においてＺＭＰを用いても接触状態の遷移は判定できない。

また、特許文献３に示されるように、先に本出願人が提案した脚式移動ロボットの制御装置の技術では、床からロボットにはたらく６次元の力・モーメントを考慮して、接触点が単一の平面に含まれない場合の接触状態の遷移の判定、動作生成の手法、ならびに動作制御の手法について提案している。

特開２００２−３２６１７３号公報 特許第３１３２１５６号 特開２００６−２４７８００公報 特開平５−３０５５８５号公報

高西、石田、山崎、加藤、「２足歩行ロボットＷＬ−１０ＲＤによる動歩行の実現」，日本ロボット学会誌、ｖｏｌ．３，ｎｏ．４，ｐｐ．３２５−３３６，１９８５

しかしながら、先に述べたように、ＺＭＰは床反力の作用中心点であるため、ＺＭＰを用いても床面上の２次元の情報しか得られない。それに対して、床からロボットには３次元の力と３次元のモーメントが働いている。ＺＭＰでは、合計６次元の力・モーメントのうち２次元の情報しか表現できないため、限定された接触状態の遷移しか扱うことができない。

したがって、従来の技術では、例えば、ロボットの足裏が床面上を滑るかどうかを判定することは不可能であり、ロボットと床面の間に十分な摩擦が存在する場合でも、ロボットと環境との全ての接触点が単一の平面に含まれない場合は接触状態の遷移を判定できない。ＺＭＰによってロボットの接触状態の遷移が判定できるのは、水平で摩擦が無視できるような床面や、水平で滑りが生じない程度に摩擦係数が十分大きい床面上に限られる。

本発明の第１の目的は、ロボットが環境から受ける６次元の力・モーメントを考慮し、脚式移動ロボットの環境との接触状態の遷移を判定して、動作の生成を行う脚式移動ロボットの制御装置を提供することにある。

ロボットが環境から受ける６次元の力・モーメントを考慮することで、各接触点が単一の平面に含まれない場合を扱うことが可能であるが、本発明においては、特にロボットの接触点における法線方向が接触点によって異なる環境を想定する。ここでの法線方向が接触点によって異なる環境とは、水平な面のみから構成される階段は含まれず、傾きの異なる複数の斜面から構成されるような環境の事を指す。

本発明の第２の目的は、ロボットに搭載された環境の形状を計測するセンサの情報を元に、６次元の力・モーメントを考慮し、脚式移動ロボットの環境との各接触点における法線方向が異なる環境において、脚式移動ロボットの動作を生成する脚式移動ロボットの制御装置を提供することにある。ここで、環境の形状を計測するセンサとは、視覚センサであるステレオカメラやレーザセンサ等を指す。

本発明の第３の目的は、ロボットに搭載された環境の形状を計測するセンサの情報を元に、６次元の力・モーメントを考慮し、脚式移動ロボットの環境との各接触点における法線方向が異なる環境において、ロボットが動作する際に、センサによって計測された環境の形状の誤差を考慮してロボットを制御する脚式移動ロボットの制御装置を提供することである。

第１の目的を達成するために、本発明による脚式移動ロボットの制御装置は、少なくとも上体と前記上体に連結される複数本の脚部からなる脚式移動ロボットの制御装置において、前記ロボットに搭載されたセンサからの信号により環境を計測し、計測された情報を元に環境形状を数値化する手段と、前記ロボットの動作を生成する目標となる６次元の力・モーメントを計算する手段と、前記ロボットの重心の加速度と重心まわりの角運動量の時間微分に基づいて、ロボットが環境から受ける６次元の力・モーメントを算出する手段と、与えられたロボットの動作に対応して力・モーメント空間での接触力凸多面錐を、複数の接触点におけるそれぞれの法線ベクトルを用いて、ロボットと環境の接触においてロボットが環境から受ける６次元の力・モーメントを算出する手段と、与えられたロボットの動作に対応して力・モーメント空間での接触力凸多面錐を算出する手段と、算出された６次元の力・モーメントが前記接触力凸多面錐の内部に含まれるかどうかを判定して、接触状態の遷移を判定する手段を有する如く構成した。

また、６次元の力・モーメントを考慮してロボットの動作を生成するために、目標となる６次元の力・モーメントを計算する手段、さらに目標となる６次元の力・モーメントを実現するようなロボットの動作を生成する手段を有する如く構成した。

上記した第２の目的を達成するために、本発明による脚式移動ロボットの制御装置は、少なくとも上体と前記上体に連結される複数本の脚部からなる脚式移動ロボットが、第１の目的を達成するための手段に加えて、ロボットに搭載されたセンサにより、環境を計測する手段、ならびに計測された情報を元に環境の形状を数値化する手段を有する如く、脚式移動ロボットの制御装置を構成した。

これにより、第１の目的を達成するための手段に加えて、ロボットに搭載されたセンサにより環境を計測する手段、ならびに計測された情報を元に環境の形状を数値化する手段を有するために、ロボットが未知の環境においても、接触状態の遷移を考慮して脚式移動ロボットの動作を生成できる。

また、上記した第３の目的を達成するために、本発明による脚式移動ロボットの制御装置は、少なくとも上体と前記上体に連結される複数本の脚部からなる脚式移動ロボットが、第１、第２の目的を達成するための手段に加えて、環境の計測誤差を検知した時点で動作を一旦停める手段、そして手先や足先を動作させて計測された環境の誤差量を見積もる手段、ならびに前記誤差量を修正してロボットを動作させる手段を有する如く、脚式移動ロボットの制御装置を構成した。

これにより、第１の目的および第２の目的を達成するための手段に加えて、環境の計測誤差を検知された時点で動作を一旦停める機能、手先や足先を動作させて計測された環境の誤差量を見積もる手段、ならびに前記誤差量を修正してロボットを動作させる手段を有するため、環境の計測情報の誤差を補償して、接触状態の遷移考慮した脚式移動ロボットの動作が実現できる。

具体的に、本発明による脚式移動ロボットの制御装置は、第１の態様として、少なくとも上体と前記上体に連結された複数本の脚部からなる脚式移動ロボットの制御装置において、ロボットに加わる６次元の力・モーメントを算出することにより、複数の接触点における法線ベクトルが同一でないロボットと環境の接触において、少なくとも一部の接触点において接触が離れるときの接触状態の遷移を判定する手段を備えることを特徴とするものである。

第２の態様として、本発明による脚式移動ロボットの制御装置は、少なくとも上体と前記上体に連結された複数本の脚部からなる脚式移動ロボットの制御装置において、ロボットと環境の複数の接触点における法線ベクトルが同一でない場合に、ロボットに加わる６次元の力・モーメントを考慮することにより、前記接触状態の遷移を判定することを特徴とするものである。

第３の態様として、本発明による脚式移動ロボットの制御装置は、少なくとも上体と前記上体に連結された複数本の脚部からなる脚式移動ロボットの制御装置において、ロボットと環境の複数の接触点における法線ベクトルが同一でない場合に、ロボットに加わる６次元の力・モーメントを考慮することにより、前記接触状態が遷移しないロボットの動作を生成する手段を備えたことを特徴とするものである。

第４の態様として、本発明による脚式移動ロボットの制御装置は、少なくとも上体と前記上体に連結された複数本の脚部からなる脚式移動ロボットの制御装置において、ロボットと環境の複数の接触点における法線ベクトルが同一でない場合に、ロボットに加わる６次元の力・モーメントを考慮することにより、センサを用いた環境の計測に基づいて、前記接触状態が遷移しないロボットの動作を生成する手段を備えたことを特徴とするものである。

第５の態様として、本発明による脚式移動ロボットの制御装置は、少なくとも上体と前記上体に連結された複数本の脚部からなる脚式移動ロボットの制御装置において、ロボットと環境の複数の接触点における法線ベクトルが同一でない場合に、ロボットに加わる６次元の力・モーメントを考慮することにより、センサを用いた環境の計測結果に誤差がある場合に、前記接触状態が遷移しないロボットの動作を生成する手段を備えたことを特徴とするものである。

このような特徴を有する本発明による脚式移動ロボットの制御装置においては、環境からロボットが受ける６次元の力・モーメントを計測し、力・モーメント空間において凸錐を算出し、計測された６次元の力・モーメントが凸錐の内部に含まれるかどうかを判定する手段を有するため、ロボットと環境とのあらゆる接触状態の遷移を判定でき、脚式移動ロボットが動作する際の安定性を高めることができる。

また、さらに目標となる６次元の力・モーメントを計算する手段、また目標となる６次元の力・モーメントを実現するようなロボットの動作を生成する手段を有するため、接触状態の遷移を考慮して脚式移動ロボットの動作を生成でき、動作の安定性を高めることができる。

この発明の実施の形態に係る制御装置が適用される脚式移動ロボット、より具体的には２足歩行ロボットを全体的に示す概略図である。 各接触点における法線方向が異なる環境において接触状態の遷移を判定するフローチャートである。 各接触点での法線方向と接触力凸多面錐の関係を説明する図である。 ６次元の力・モーメントを考慮してロボットの動作を生成する処理のフローチャートである。 鉛直方向の力を垂直抗力と摩擦力に分配したものを目標値とする説明図である。 ６次元の力・モーメントを考慮してロボットの動作を生成する処理をより詳細に説明する第１のブロック図である。 ６次元の力・モーメントを考慮してロボットの動作を生成する処理をより詳細に説明する第２のブロック図である。 センサの計測誤差を考慮して修正したアルゴリズムを説明する図である。

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

図１は、この発明の実施の形態に係る制御装置が適用される脚式移動ロボット、より具体的には２足歩行ロボットを全体的に示す概略図である。ΣＲは基準座標系であり、ΣＢはロボット腰部に固定された座標系であり、Σｉは第ｉリンク（ｉ＝１，…，ｎ）の重心に固定された座標系を表す。

ΣＲに関する位置ベクトルについて、Ｐ_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）はロボットの重心を表す位置ベクトルである。さらに、ｚ₀はロボットが水平面上を歩行する場合の床面の高さを表すものとする。ｆ_Hi（＝［（ｆ_Hi）_x （ｆ_Hi）_y （ｆ_Hi）_z］^T），τ_Hj（ｊ＝１，２）は各手先が受ける力・モーメントとし、ｆ_Fj，τ_Fj（ｊ＝１，２）は各足裏が受ける力・モーメントである。

同様に、ｆ_kは点Ｐ_kで受ける力とする。なお、図１ではモーメントを２重矢印で表している。ｎ_kは、点Ｐ_kにおける単位拘束法線ベクトルを表す。Ｉ_i，ω_iはそれぞれ第ｉリンクの基準座標系Σｉに関する慣性テンソル、角速度ベクトルを表す。以上に述べた位置ベクトル、力・モーメントは、全て基準座標系ΣＲで表すものとする。

本発明による脚式移動ロボットの制御装置は、第一の実施形態として、脚式移動ロボットが歩行などの動作を行うとき、環境からロボットが受ける３次元の力と３次元のモーメントを算出する手段、つまり、ロボットが環境から受ける６次元の力・モーメントを算出する手段と、与えられたロボットの姿勢に対応して力・モーメント空間において形成される凸錐を算出する手段を備えている。これらについて説明する。

また、これらの算出する手段を利用することで、各接触点における法線方向が異なる環境において、接触状態の遷移を判定する。この手法を、図２のフローチャートに基づいて説明する。

処理を開始すると、まず、環境の計測処理を行い（ステップ２０１）、入力として、ロボット重心の加速度Ｐ_Gと重心まわりの角運動量の時間微分Lならびに環境の形状に関する情報を与える。ロボットと環境が望みの接触状態を維持するという仮定の下で、慣性力および重力によりロボットが受ける力・モーメントを基準座標系ΣＲで表したものは、次の（１）式および（２）式により与えられるので、これらの演算処理を行う（ステップ２０２）。

これらの式において、Ｍはロボットの質量、ｇは重力ベクトルを表す。

ロボットに力ｆ_Gおよびモーメントτ_Gが加わったとき、ロボットは接触している環境から反力・モーメントを受ける。ロボットが接触している環境から受ける反力を基準座標系ΣＲで表したものｆ_Cを、ロボットが受ける基準座標系ΣＲのまわりのモーメントτ_Cをとすると、これらは手先・足裏と環境との接触面の幾何学的な形状から、（３）式、（４）式が得られる。

さらに、各接触点がロボットに対して発生可能な力の集合は、摩擦コーンをＬ角錐で近似した場合、（５）式により与えられる。

ここにおいて、μ_kは各点の摩擦係数、ｔ^l _kは（ｎ_k＋μ_kｔ^l _k）が各点の摩擦コーンを近似するＬ角錐の側辺となるような単位ベクトルであり、ｎ_kが各接触点における法線方向を表す単位ベクトルである。この単位ベクトルの向きが各接触点で異なる場合を本発明の脚式移動ロボットの制御装置では取り扱う。

また、ε^l _kは各点に加わる接線方向の力の大きさにより定まる非負のスカラーである。（５）式を（３）式および（４）式に代入すると、（ｆ_C，τ_C）がとり得る値の集合は、次の（６）式および（７）式のとおりに得られる。

ここでの（ｆ_C，τ_C）はベクトルの非負１次結合で表されていることから、これらが力・モーメントの空間でとり得る値の集合は凸多面錐になる。この力・モーメントの集合を、接触力凸多面錐と呼ぶ。

上記の（６）式および（７）式における各接触点での法線方向と接触力凸多面錐の関係を、図３を用いて説明する。図３に示すように、直方体の足が不整地に４点で接触する場合には、各接触点において法線方向が異なる。そして、各接触点における接触力を全ての接触点について合わせたものが、（ｆ_C，τ_C）である。

脚式ロボットに力ｆ_Gおよびモーメントτ_Gが加わった場合、（−ｆ_G，−τ_G）が、（６）式および（７）式によって表される接触力凸多面錐に含まれる場合には、接触状態は弱安定となり、釣り合いの関係が成立する可能性が生じる。このとき、釣り合いが必ずしも成立するとは言えないが、（−ｆ_G，−τ_G）が凸多面錐に含まれない場合には、釣り合いの関係は成立せず、接触状態は必ず遷移する。

判定処理（ステップ２０３）は接触力凸多面錐に含まれるか否かの判定を行う。図２に示すように、（−ｆ_G，−τ_G）が（６）式、（７）式によって表される接触力凸多面錐に含まれるかの判定では、ｆ_G＝−ｆ_C，τ_G＝−τ_Cとおき、ε^l _k（ｋ＝１，…，Ｋ， ｌ＝１，…，Ｌ）が非負であり、なおかつ、その三つ以上が“０”では無いことを数値的に確認することでなされる。この判定の結果に基づいて、「接触状態は遷移しない」判定結果または「遷移状態が遷移する」判定結果を出力する（ステップ２０４，ステップ２０５）。

次に、６次元の力・モーメントを考慮してロボットの動作を生成する手法の概要を説明する。図４は、６次元の力・モーメントを考慮してロボットの動作を生成する処理のフローチャートである。この処理では、各接触点における法線方向が既知であると仮定する。つまり、各接触点における法線方向を示すデータについては、前処理により予め取得されているものとする。処理を開始すると、まず、ステップＳ１において、目標となる手先、足先、ならびに腰の速度を与える。次に、ステップＳ２において、重心まわりの角運動量を計算する。さらに、ステップＳ３において、目標となる６次元の力・モーメントの軌道を計算する。この情報を基にして、次のステップＳ４において、接触力方程式を解いて、目標重心軌道を求める。最後に、ステップＳ５において、分解運動量制御で目標腰速度を修正する。そして、ステップＳ６において、動作の生成処理を行う。もし、ステップＳ１で定めた腰速度と、ステップＳ５で定めた腰速度が大きく異なる場合は、後述するように、繰り返し計算を行う。

つぎに、これらのそれぞれの処理について、詳細に説明する。まず、ステップＳ１においては、目標となる手先、足先、ならびに腰の目標速度を与える。これは、ロボットに搭載されたセンサの情報を基に、環境の形状を計測する。ここでは、計測は十分正確であると仮定し、計測誤差を考慮する手法については、後述する。環境のモデルが与えられると、環境と望みの接触状態を維持し、別の接触状態へと遷移するような、望みの手先や足先の軌道が計算できる。

また、手先・足先の軌道を時間微分することで、速度・角速度が得られる。腰の速度・角速度は、最初はヒューリスティックに与えることにし、より正確な腰の速度は、ステップＳ５において、あらためて計算する。

ステップＳ１において、手先、足先、ならびに腰の速度が与えられたときに、ステップＳ２において重心まわりの角運動量を計算する手法を説明する。基準座標系ΣＲで表現された腰に固定された座標系ΣＢの並進速度、角速度をυ_B、ω_Bとし、ロボットの全関節速度を縦に並べたｎ×１ベクトルをベクトルθの時間微分とする。このとき、ロボット全体の並進運動量ｐおよび重心まわりの角運動量Ｌは次式で与えられる。

ここで、Ｍはロボットの全質量、Ｅは３×３の単位行列、γ_B→Gは、腰リンクから全重心Ｐ_Gへの（３×１）ベクトルの位置ベクトル、Ｉは全重心まわりの（３×３）の慣性行列である。また、Ｍ_θ、Ｈ_θは、関節速度がそれぞれ並進運動量と角運動量に及ぼす効果を表わす慣性行列である。

また、記号＾は（３×１）のベクトルを外積と等価な（３×３）の歪対称行列に変換する操作を表わす。手先や足先の速度・角速度と関節速度はヤコビ行列により関連付けられている。（８）式のベクトルθの時間微分の部分に、この関係式を代入することで、手先、足先、ならびに腰の速度・角速度が与えられた時のロボット全体の運動量・角運動量は、次の形で求められる。

ここで、速度・角速度ベクトルを、（１０）式のようにまとめた。

ステップＳ３において、目標となる６次元の力・モーメントを定義する手法について説明する。より一般的に説明すると、ロボットと環境との接触状態によって一意に定まる（６）式および（７）式で定義される凸多面錐の内部に、（−ｆ_G，−τ_G）が含まれるように、その軌道を定めると、ロボットと環境との接触状態は変化しない。ここでは、十分に摩擦があり、法線方向が各接触点で異なる不整地を例にとって、目標となる力・モーメントを定義する方法について説明する。

十分に摩擦があり、接触点によって法線方向が異なる不整地では、（６）式および（７）式は任意のスカラーδ^l _kを用いて、以下のような６つの式で表すことができる。

ここでの（１１）式、（１２）式および（１３）式が力に関する関係式であり、（１４）式、（１５）式および（１６）式がモーメントに関する関係式である。また、それぞれｘ，ｙ，ｚ方向成分の式を表している。

十分に摩擦がある場合には、摩擦により発生する接触力の空間は線形部分空間になるので、接触力全体の集合は、この線形部分空間とその直交補空間の中の凸多面錐との直積になる。したがって、接触力を各線形部分空間内で独立に計画することができる。さらに、摩擦により発生する接触力の空間は、各軸方向毎に線形部分空間になるので、やはり独立に計画することができる。

（１１）式〜（１６）式は非線形微分方程式であり、これを満たすために操作可能な変数は加速度ｘ，ｙ，ｚ、速度Ｌ_x，Ｌ_y，Ｌ_zの６つある。しかしながら、一般に速度Ｌ_x，Ｌ_y，Ｌ_zを操作すると、全身を大きく振る等の不自然な動作が生成されることが多いため、速度Ｌ_x，Ｌ_y，Ｌ_zは操作せず、加速度ｘ，ｙ，ｚのみを操作して、ｚ軸方向の並進力の方程式、ｘ，ｙ軸まわりのモーメントの方程式の３つのみを満たす動作の生成を考える。残り３つの方程式については、摩擦が十分に働くという仮定をし、ロボットが２点以上で接地しているときには常に満足される。

各接触点における法線方向の単位ベクトル用いて、加速度ｚ_Gを決める。これは重心の鉛直方向の軌道を定めることに相当する。ｚ軸方向の力の釣り合いについては、（１３）式が成立する必要がある。ここで、（１３）式を

とｚ軸方向の力を垂直抗力と摩擦力に分配し、これを満足するように、加速度ｚ_Gを決める。（１７）式と（１８）式に分解するのは、図５に示すように、鉛直方向の力を垂直抗力と摩擦力に分配したものを目標値とすることに相当する。

αの決め方としては、例えば、各接触点での面の傾きの平均を取ることが考えられる。また、決めたαを（１７）式に代入した結果、ε⁰ _kは少なくとも３つのｋについて、０＜ε⁰ _kとなることが必要である。なお、本手法では、十分に摩擦力が働くという仮定をおいているが、ここでの分配方法の如何によって、生成された動作がクーロン摩擦の制約を満たすかどうかに影響する。

次に、水平軸まわりのモーメントの釣り合い式である（１４）式および（１５）式を次の式に変形する。

この（１９）式および（２０）式を満たすように加速度ｘ_G，ｙ_Gを決める。

最後に、水平方向の鉛直力と鉛直軸まわりのモーメントの釣り合いについては、（１１）式，（１２）式，（１６）式が成立することが必要である。

得られた加速度ｘ_G，ｙ_G，ｚ_Gを（１１）式〜（１６）式に代入した式において、少なくても３つのｋについて、０＜ε⁰ _kである必要がある。また、δ^l _kが一意解を持つかどうかは、（１１）式〜（１６）式の左辺をδ^l _kについて整理することで得られる

の係数行列が正則であることで確認される。

次に、ステップＳ４の目標重心軌道を求めて設定する手段を説明する。ここでも、十分に摩擦のある環境を想定して説明する。水平軸周りのモーメントの釣り合い式において、重心の加速度を中心差分近似することにすると、離散時間表現で、

とかける。ここに、ｉ＝１，…，Ｎで境界条件は、

で与えられる。これらの差分方程式を離散時刻ｉ＝１，…，Ｎについて連立し、これらの連立１次方程式を解くことにより、ｘ_G，ｙ_Gの目標軌道を求めることができる。

更に、ステップＳ５において、分解運動量制御により修正腰位置を計算するについて説明する。（９）式を以下のように書き直す。

ここで、

である。このとき、腰の速度・各速度Ａはの擬似逆行列であるＡ†を用いて次式で与えられる。

ここで、図４で示したアルゴリズムをより詳細に説明するために、図６のブロック図を用いる。図６のブロック図によるデータ処理の流れにしたがって、処理を順に説明すると、この動作パターン生成の処理は、次のような手順により行われる。

すなわち、
ステップ１０１；ロボットに搭載されたセンサで環境を計測し、環境の形状ｆ（ｘ）を得る（処理ブロック６０１）。
ステップ１０２；環境の情報を用いて、足先目標速度・角速度（ｖ_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）を求め、これを足先の目標速度・角速度とする。
ステップ３０５；ＺＭＰが接触線分から大きく移動したら面接触状態に遷移したと判定し、目標足先速度・角速度を零にする。（図８（ｄ））

Claims (1)

1. 少なくとも上体と前記上体に連結される複数本の脚部からなる脚式移動ロボットの制御装置において、
   前記ロボットに搭載されたセンサからの信号により環境を計測し、計測された情報を元に環境形状を数値化する手段と、
   前記ロボットの動作を生成する目標となる６次元の力・モーメントを計算する手段と、
   前記ロボットの重心の加速度と重心まわりの角運動量の時間微分に基づいて、前記ロボットに作用する６次元の力・モーメントを算出する手段と、
   与えられたロボットの動作に対応して、ロボットが環境の接触により環境から受ける６次元の力・モーメントを、複数の接触点におけるそれぞれの法線ベクトルを用いて、力・モーメント空間における接触力凸多面錐として算出し、前記ロボットに作用する６次元の力・モーメントが、前記接触力凸多面錐の内部に含まれるかどうかを判定して、接触状態の遷移を判定する手段とを備えることを特徴とする脚式移動ロボットの制御装置。

Images