(57)【特許請求の範囲】
【請求項１】
少なくとも上体と、前記上体に連結された複数本の脚部からなる脚式移動ロボットの制御装置において、
前記ロボットが動作する際に、慣性力及び重力によりロボットが受ける３次元の力（ｆＧ）及び３次元のモーメント（τＧ）を求め、
前記ロボットが環境に接触している各接触点（１・・・・Ｋ）において、該ロボットに対し発生可能な３次元の力の集合である摩擦コーンをＬ角錐として近似するため、当該Ｌ角錐における各側辺（１・・・Ｌ）毎に、前記Ｌ角錐の各側辺となる単位ベクトルと、当該側辺方向の力の大きさを定める非負のスカラーとの積により、各側辺方向に発生可能な力を規定し、これらの和を各接触点においてロボットに対し発生可能な３次元の力の集合とするとともに、
各接触点において、前記Ｌ角錐の各側辺方向に発生可能な力と各接触点の位置ベクトルとの外積を規定し、これらの和を各接触点においてロボットに対し発生可能な３次元のモーメントの集合とし、
各接触点それぞれにおける前記３次元の力の集合及び前記３次元のモーメントの集合の全接触点（１・・・・Ｋ）についての和を、前記ロボットに対し発生可能な反力及びモーメントの集合である接触力凸多面錐として規定し、
３次元の力及びモーメントの空間において、前記ロボットが動作する際に慣性力及び重力によりロボットが受ける力（ｆＧ）及びモーメント（τＧ）を打ち消す逆方向の力及びモーメント（−ｆＧ、―τＧ）が、前記接触力凸多面錐に含まれない場合、少なくとも一部の接触点において接触が離れる接触状態の遷移を判定する手段を備えたことを特徴とする脚式移動ロボットの制御装置。
【請求項２】
生成したロボットの動作に対応して、慣性力及び重力によりロボットが受ける３次元の力（ｆＧ）及び３次元のモーメント（τＧ）を求め、該３次元の力（ｆＧ）及び３次元のモーメント（τＧ）を打ち消す逆方向の力及びモーメント（−ｆＧ、―τＧ）が、前記接触力凸多面錐に含まれるように、ロボットの動作を生成する手段を備えたことを特徴とする請求項１記載の脚式移動ロボットの制御装置。
【請求項３】
ロボットが動作する際に、慣性力及び重力によりロボットが受ける３次元の力（ｆＧ）及び３次元のモーメント（τＧ）を現在のロボットの関節角度情報及び位置姿勢の情報に基づいて算出し、平面上の歩行において、実時間で算出した３次元の力（ｆＧ）及び３次元のモーメント（τＧ）を打ち消す逆方向の力及びモーメント（−ｆＧ、―τＧ）が前記接触力凸多面錐に含まれるように、前記ロボットの姿勢をフィードバック制御する手段を備えたことを特徴とする請求項２記載の脚式移動ロボットの制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明はホームロボットや各種サービスロボット等に用いる脚式移動型の２足歩行ロボットの動作を生成したり、動作をフィードバック制御する装置に関するものである。
【背景技術】
【０００２】
従来、脚式移動ロボットの動作は、ロボットが床から受ける反力の作用中心点であるZMP(Zero-Moment  Point)を規範として生成されてきた（高西、石田、山崎、加藤：日本ロボット学会誌、vol.3、no.4、pp.325-336、1985、特開2002-326173）。ここで、ZMPは床反力の作用中心点であるため、床反力中心点と呼ばれる場合もある。
【０００３】
脚式移動ロボットが水平な床面上を歩く場合、ZMPが足裏と床面の接触点が形成する支持多角形の内部に含まれる限り、ロボットは転倒せずに歩行を継続することが保証される。一方、ZMPが支持多角形の端に来ると、ロボットの足裏と床面との接触が離れることで、ロボットが転倒する可能性が生じる。
【０００４】
しかしながら、先に述べたようにZMPは床反力の作用中心点であるため、ZMPを用いても足裏と床面が滑るかどうかを判定することはできない。また、脚式移動ロボットの手先が環境と接触する場合には、接触点の間で内力が発生するが、内力の影響はZMPの位置には現れない。つまり、この場合もZMPを用いることでは、ロボットと環境の接触点で、滑りが生じたり接触が離れるといった接触状態の遷移を判定することは不可能である。
【０００５】
特許第3132156号明細書において、ZMPを規範にして脚式移動ロボットが階段を上る動作が生成されている。しかし、先に述べたように、ZMPを用いたのでは接触点が単一の面上にない場合や、接触点で滑りが生じる場合には、接触状態の遷移を判定することは出来ない。これは、ロボットは床から３次元の力と３次元のモーメントを受けるが、ZMPではモーメントのうち床面と平行な２成分のみを用いているからである。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開2002-326173号公報
【特許文献２】特許第3132156号明細書
【発明の概要】
【発明が解決しようとする課題】
【０００７】
本発明の第１の目的は、脚式移動ロボットにおいて、ロボットに加わる３次元の力と３次元のモーメントを考慮し、そして、これら３次元の力と３次元のモーメントが力−モーメント空間において形成する接触力凸多面錐を用いて、接触状態の遷移を判定する機能を有する脚式移動ロボットの制御装置を提供することにある。
【０００８】
さらに、本発明の第２の目的は、脚式移動ロボットにおいて、ロボットに加わる３次元の力と３次元のモーメントを考慮し、そして、これら３次元の力と３次元のモーメントが力−モーメント空間において形成する接触力凸多面錐を用いて、接触状態が遷移しない脚式移動ロボットの動作を生成する装置を提供することにある。
【０００９】
さらに、本発明の第３の目的は、脚式移動ロボットにおいて、ロボットに加わる３次元の力と３次元のモーメントを考慮し、ロボットの姿勢を実時間でフィードバック制御する脚式移動ロボットの動作制御装置を提供することにある。
【課題を解決するための手段】
【００１０】
上記した第１の目的を達成するために、少なくとも上体と、前記上体に連結された複数本の脚部からなる脚式移動ロボットの制御装置において、前記ロボットが動作する際に、慣性力及び重力によりロボットが受ける３次元の力（ｆＧ）及び３次元のモーメント（τＧ）を求め、前記ロボットが環境に接触している各接触点（１・・・・Ｋ）において、該ロボットに対し発生可能な３次元の力の集合である摩擦コーンをＬ角錐として近似するため、当該Ｌ角錐における各側辺（１・・・Ｌ）毎に、前記Ｌ角錐の各側辺となる単位ベクトルと、当該側辺方向の力の大きさを定める非負のスカラーとの積により、各側辺方向に発生可能な力を規定し、これらの和を各接触点においてロボットに対し発生可能な３次元の力の集合とするとともに、各接触点において、前記Ｌ角錐の各側辺方向に発生可能な力と各接触点の位置ベクトルとの外積を規定し、これらの和を各接触点においてロボットに対し発生可能な３次元のモーメントの集合とし、各接触点それぞれにおける前記３次元の力の集合及び前記３次元のモーメントの集合の全接触点（１・・・・Ｋ）についての和を、前記ロボットに対し発生可能な反力及びモーメントの集合である接触力凸多面錐として規定し、３次元の力及びモーメントの空間において、前記ロボットが動作する際に慣性力及び重力によりロボットが受ける力（ｆＧ）及びモーメント（τＧ）を打ち消す逆方向の力及びモーメント（−ｆＧ、―τＧ）が、前記接触力凸多面錐に含まれない場合、少なくとも一部の接触点において接触が離れる接触状態の遷移を判定する手段を具備するようにした。
【００１２】
さらに、上記した第２の目的を達成するために、生成したロボットの動作に対応して、慣性力及び重力によりロボットが受ける３次元の力（ｆＧ）及び３次元のモーメント（τＧ）を求め、該３次元の力（ｆＧ）及び３次元のモーメント（τＧ）を打ち消す逆方向の力及びモーメント（−ｆＧ、―τＧ）が、前記接触力凸多面錐に含まれるように、ロボットの動作を生成する手段を具備するようにした。
【００１３】
目標となる３次元の力と３次元のモーメントを計算し、与えられたロボットの姿勢に対応して力−モーメント空間において形成される接触力凸多面錐を算出し、さらに、前記計算された３次元の力と３次元のモーメントの目標値が接触力凸多面錐に含まれない場合は３次元の力と３次元のモーメントの目標値を修正する手段を有するため、ロボットと環境との接触が離れるような、接触状態の遷移がない安定な脚式移動ロボットの動作を生成することができる。
【００１４】
さらに、上記した第３の目的を達成するために、ロボットが動作する際に、慣性力及び重力によりロボットが受ける３次元の力（ｆＧ）及び３次元のモーメント（τＧ）を現在のロボットの関節角度情報及び位置姿勢の情報に基づいて算出し、平面上の歩行において、実時間で算出した３次元の力（ｆＧ）及び３次元のモーメント（τＧ）を打ち消す逆方向の力及びモーメント（−ｆＧ、―τＧ）が前記接触力凸多面錐に含まれるように、前記ロボットの姿勢をフィードバック制御する手段を具備した。
【００１５】
３次元の力と３次元のモーメントを考慮して、実時間で姿勢をフィードバック制御する手段を有するため、接触状態が遷移しないような、安定な脚式移動ロボットの動作制御が実時間で実現できる。
【発明の効果】
【００１６】
本発明は上記のように構成したので、脚式移動ロボットにおいて、ロボットと環境との接触状態の遷移を判定でき、かつ接触状態を遷移させないロボットの動作を容易に得ることができ、ロボットの姿勢を実時間でフィードバック制御することが可能になる。
【図面の簡単な説明】
【００１７】
【図１】脚式移動ロボットが床面上に立ち、手先で環境と接している図である。
【図２】脚式移動ロボットの足裏と床面との接触を表す図である。
【図３】摩擦が十分ある平らな床面上での歩行における、足裏支持領域と、力・モーメントの領域を示す図である。
【図４】滑りやすい床面上の歩行における、力の領域を示す図である。
【図５】滑りやすい床面上の歩行における、鉛直軸まわりのモーメントの範囲を示す図である。
【図６】２次元平面内で脚式移動ロボットが対象物を押しながら歩く図である。
【図７】接触力凸多面体に含まれない力・モーメントの凸多面体への正射影を示す図である。
【図８】接触力凸多面錐を用いた脚式移動ロボットの動作生成を示す図である。
【図９】力・モーメントを考慮した実時間制御系の図である。
【発明を実施するための形態】
【００１８】
本発明は脚式移動ロボットにおいて、ロボットと環境との接触状態の遷移を判定でき、かつ接触状態を遷移させないロボットの動作を容易に得ることができ、ロボットの姿勢を実時間でフィードバック制御することを可能にするため、少なくとも上体と、前記上体に連結された複数本の脚部からなる脚式移動ロボットの制御装置において、ロボットに加わる３次元の力と３次元のモーメントを算出することにより、平面上の歩行における足裏と床面との接触において、少なくとも一部の接触点において接触が離れる接触状態の遷移を判定する手段を備えることによって実現した。
【実施例１】
【００１９】
以下、添付図面を参照してこの発明の一つの実施の形態に係る脚式移動ロボットの制御装置を説明する。脚式移動ロボットとしては２足歩行ロボットを例にとる。
【００２０】
まず、本発明の第一の実施例として、脚式移動ロボットが歩行などの動作を行うとき、環境からロボットが受ける３次元の力と３次元のモーメントを算出する手段、与えられたロボットの姿勢に対応して力−モーメント空間において形成される接触力凸多面錐を算出する手段を示す。
【００２１】
図１は、この実施の形態に係る制御装置が適用される脚式移動ロボット、より具体的には２足歩行ロボットを全体的に示す概略図である。ΣＲは基準座標系であり、ΣＢはロボット腰部に固定された座標系、ΣＬｉは第iリンク（i＝1,・・・,n)の重心に固定された座標系を表す。ΣＲに関する位置ベクトルについて、ＰＨｊ（＝［ｘＨｊｙＨｊｚＨｊ］Ｔ（ｊ＝１，２）は各手先、ｐＦｊ（＝［ｘＦｊｙＦｊｚＦｊ］Ｔ）（ｊ＝１，２）は各足裏のリンクに固定された点、ｐＬｉ（＝［ｘＬｉｙＬｉｚＬｉ］Ｔ）はΣＬｉの原点、ｐＢ（＝［ｘＢｙＢｚＢ］Ｔ）はΣＢの原点を表す。また、各手先の接触領域と各足裏の支持領域を構成する多角形の頂点を手先か足先かを区別せずｐｋ（ｋ＝1,・・・，ｋ）とし、ｐＧ（＝［ｘＧｙＧｚＧ］Ｔ）はロボットの重心を表す位置ベクトルであり、
【数１】
0005099733.tif 000002
のように定義されるとする。さらに、ｚ０はロボットが水平面上を歩行する場合の床面の高さを表すものとする。ｆＨｉ（＝［（ｆＨｉ）ｘ（ｆＨｉ）ｙ（ｆＨｉ）ｚ］Ｔ）、τＨｊ（ｊ＝１，２）は各手先が受ける力・モーメント、ｆＦｊ，τＦｊ（ｊ＝１，２）は各足裏が受ける力・モーメントである。同様に、ｆｋは点ｐｋで受ける力とする。なお、図ではモーメントを太矢印で表している。ｎｋは、点ｐｋにおける単位拘束法線ベクトルを表す。Ｉｉ、ωｉはそれぞれ第iリンクの基準座標系に関する慣性テンソル、角速度ベクトルを表す。以上述べた位置ベクトル、力、モーメントは、全て基準座標系ΣＲで表すものとする。
【００２２】
ロボットが動作する際に、慣性力および重力によりロボットが受ける力を基準座標系ΣＲで表したものをｆＧ、ロボットが受ける基準座標系まわりのモーメントをτＧとする。ロボットに加わる力・モーメントは数式（１）、数式（２）により与えられる。
【数２】
0005099733.tif 000003
【数３】
0005099733.tif 000004
ここで、
【数４】
0005099733.tif 000005
はロボットの質量、ｇ＝［０ ０ −ｇ］Ｔは重力ベクトルを表す。また、
【数５】
0005099733.tif 000006
は重心まわりの角運動量を表す。
【００２３】
ロボットに力ｆＧおよびモーメントτＧが加わったとき、ロボットは接触している環境から反力・モーメントを受ける。ロボットが接触している環境から受ける反力を基準座標系で表したものをｆＣ、ロボットが受ける基準座標系まわりのモーメントをτＣとすると、これらは手先・足裏と環境との接触面の幾何学的な形状から、数式（３）、数式（４）が得られる。
【数６】
0005099733.tif 000007
【数７】
0005099733.tif 000008
さらに、各接触点がロボットに対して発生可能な力の集合は、摩擦コーンをL角錐で近似した場合、数式（５）により与えられる。
【数８】
0005099733.tif 000009
ここに、μｋは各点の摩擦係数、
【数９】
0005099733.tif 000010
が各点の摩擦コーンを近似するL角錐の側辺となる様な単位ベクトルで、
【数１０】
0005099733.tif 000011
は各点に加わる接線方向の力の大きさにより定まる非負のスカラである。数式（５）を数式（３）、数式（４）に代入すると、（ｆＣ，τＣ）がとり得る値の集合は、数式（６）、数式（７）により得られる。
【数１１】
0005099733.tif 000012
【数１２】
0005099733.tif 000013
（ｆＣ，τＣ）はベクトルの非負１次結合で表されていることから、これらが力・モーメントの空間でとり得る値の集合は接触力凸多面錐になる。
【００２４】
ロボットに力ｆＧおよびモーメントτＧが加わった場合、（−ｆＧ，−τＧ）が数式（６）、数式（７）によって表される接触力凸多面錐に含まれない場合は、釣り合いの関係は成立せず、接触状態は必ず遷移する。
【実施例２】
【００２５】
次に、この発明の第２番目の実施例として、十分な摩擦のある平面上を脚式移動ロボットが歩行する場合に、接触状態の遷移を判定する手段を示す。十分に摩擦がある床面上で接触状態が遷移するのは、ある接触点において接触が離れる場合である。図２に示す様に、ロボットの足裏のみが水平面と接触している場合を考える。ロボットの足先と水平面の間には十分に大きな摩擦が働くと仮定すると、水平面方向の力と鉛直軸まわりのモーメントについては任意の反力が発生し、数式（８）、数式（９）、数式（１０）の釣り合いの関係は必ず成立する。
【数１３】
0005099733.tif 000014
【数１４】
0005099733.tif 000015
【数１５】
0005099733.tif 000016
ここで、
【数１６】
0005099733.tif 000017
は任意の非負数である。一方、鉛直方向の力と水平方向のモーメントは、数式（１１）、数式（１２）、数式（１３）で表されるが、これらは常に釣り合いが成立するとは限らない。 
【数１７】
0005099733.tif 000018
【数１８】
0005099733.tif 000019
【数１９】
0005099733.tif 000020
なお、本実施例における数式（８）〜（１３）は、実施例１において行った（−ｆＧ，−τＧ）が数式（６）と数式（７）によって表される接触力凸多面錐に含まれるか否かの評価を、水平面方向の力と鉛直軸周りのモーメントについては任意の反力が発生することを前提にして、力、モーメントの関係式で表したものである。
ここでは、より具体的には、数式（１１）、数式（１２）、数式（１３）が成立するためには、右辺に含まれる非負のスカラー
【数２０】
0005099733.tif 000021
が存在しなくてはならない。逆に言うと、与えられたロボットの動作に対して、非負のスカラー
【数２１】
0005099733.tif 000022
が存在するかどうかによって、接触状態が遷移するかどうかが判定される。
【００２６】
図３に、平らな床面上におけるロボットと床との接触領域と、これに対応する数式（１１）、数式（１２）、数式（１３）で表された接触力凸多面錐の適当な（ｆＣ)ｚの値における断面を示す。
【実施例３】
【００２７】
本発明の第３番目の実施例として、腕を有する脚式移動ロボットを仮定し、腕が環境と接触する場合を想定する。前項までとは異なり、この場合はロボットと環境との接触が単一の面内に含まれるとは限らない。また、この場合、手先と足先の接触点の間で働く内力の影響が顕著になる。図６に示す２次元の場合を例にとり、実施例を説明する。２次元平面内の運動を仮定するので、２次元の力と１次元のモーメントを考慮すればよい。力やモーメントの釣り合いの関係式をまとめると、数式（１４）のようになる。
【数２２】
0005099733.tif 000023
次に、この接触力凸多面錐に加えて、内力を考える。ロボットの手が環境と接する場合、ロボットが持つ運動量や角運動量には影響を及ぼさない接触力の成分である内力が存在する。この場合、内力の関係式は数式（１５）のように表すことができる。
【数２３】
0005099733.tif 000024
ここで、φは内力に関するパラメータである。数式（１４）は式が三つに対して、変数が
【数２４】
0005099733.tif 000025
の四つである。しかしながら、数式（１４）に加えて数式（１５）を考えることで、式の数と変数の数が合い、変数を一意に求めることができる。このとき、これらの変数に負のものが含まれるならば、接触状態が遷移することを判定できる。つまり、手先が環境と接触する場合は、接触力凸多面錐に加えて内力の関係式を用いることで、接触状態の遷移を判定する。
【実施例４】
【００２８】
本発明の第４番目の実施例として、接触状態を遷移させないような脚式移動ロボットの動作を生成する手法を示す。本発明の第１番目の実施例により与えられたロボットの動作に対して、ロボットが受ける力・モーメントを算出する手法、ならびに接触力凸多面錐を計算する手法を示した。ここでは、算出された力・モーメントが接触力凸多面錐に含まれない場合に、動作を修正する手法を示す。 
【００２９】
接触力凸多面錐に含まれない（−ｆＧ，−τＧ）が与えられたとき、次のアルゴリムにより、接触力凸多面錐に含まれ、かつ、ユークリッドノルムにより定義された距離が最も近いものに変換することができる。変換後の値を
【数２５】
0005099733.tif 000026
と表記する。（ｆＧ，τＧ）を数式（１５）の連立不等式で表される凸多面錐に正射影する。 
【数２６】
0005099733.tif 000027
この射影を
【数２７】
0005099733.tif 000028
とすると、所望の射影は、
【数２８】
0005099733.tif 000029
【数２９】
0005099733.tif 000030
と求めることができる。このアルゴリズムの説明図を図７に示す。
【００３０】
数式（１５）を利用した脚式移動ロボットの動作生成手法を図８に示す。つまり、最初にロボットの動作を生成しておき、数式（１）、数式（２）にもとづいてロボットに加わる３次元の力、モーメントを計算する。これが接触力凸多面錐に含まれない場合は、数式（１７）、数式（１８）に基づいて（−ｆＧ，−τＧ）を射影し、動作を修正する。
【００３１】
本発明の第２から第４の実施例において、摩擦が十分ある床面上の歩行や手先が環境と接触する場合について、接触状態の遷移を判定した。これら全ての場合において共通して言えることは、凸多面錐を用いていることである。つまり、それぞれの場合において、本発明の第５の実施例を用いることで、接触状態が遷移しない脚式移動ロボットの動作を生成することができる。
【実施例５】
【００３２】
本発明の第５番目の実施例として、ロボットの姿勢を実時間でフィードバック制御する手法を示す。この手法においては、実時間で現在のロボットの関節角度情報や機体の位置姿勢の情報より、ロボットが受ける力・モーメントを算出する。そして、ロボットが受ける力・モーメントの目標値と比較をし、その目標値に収束するようにロボットの動作を制御する。この制御系のブロック線図を図９に示す。
【００３３】
本発明の第２、第３の実施例において、摩擦が十分ある床面上の歩行や手先が環境と接触する場合について、接触状態の遷移を判定した。これらすべての場合において共通して数式（１）、数式（２）を用いている。つまり、それぞれの場合において、本発明の第５の実施例を用いることで、実時間でロボットの動作を制御することができる。
【図１】
0005099733.tif 000031
【図２】
0005099733.tif 000032
【図３】
0005099733.tif 000033
【図４】
0005099733.tif 000034
【図５】
0005099733.tif 000035
【図６】
0005099733.tif 000036
【図７】
0005099733.tif 000037
【図８】
0005099733.tif 000038
【図９】
0005099733.tif 000039