(57)【要約】【課題】接触反力制約条件を満たすための特定状態量（移動ロボットの全体重心の位置等）、の存在可能領域を適切に推定することができる装置を提供する。【解決手段】演算処理装置４０は、移動ロボット１の複数の接触動作部（１３，２３）のそれぞれの接触反力と特定状態量とを変数成分として構成される空間内の多面体であって、接触反力と特定状態量との間の力学的関係と接触反力制約条件を表す線形不等式とにより規定される多面体を、特定状態量を表す部分空間に射影してなる射影領域を求める多面体射影処理を実行することにより、特定状態量の存在可能領域を推定する。【選択図】図５
2018001316.tif 000002
【特許請求の範囲】
【請求項１】
複数の接触動作部のそれぞれの空中移動と該空中移動に続く外界物への接触とを繰り返す運動により移動する移動ロボットの全体重心の位置、全体重心の並進運動状態量、及び全体重心周りの回転運動状態量のうちの１つ以上の状態量から成る特定状態量の値の存在可能領域であって、前記各接触動作部に作用する接触反力に関する制約条件である接触反力制約条件を満たし得る存在可能領域を推定する装置であって、
前記複数の接触動作部のそれぞれの接触反力と前記特定状態量とを変数成分として構成される空間内の凸多面体であって、該接触反力と特定状態量との間の力学的関係と前記接触反力制約条件を表す線形不等式とにより規定される凸多面体を、前記変数成分のうちの前記特定状態量を表す部分空間に射影してなる射影領域を求める多面体射影処理として、Multi parametric linear programingの処理と、線形計画法の処理を複数回繰り返す処理とのうちのいずれかの処理を実行する多面体射影処理部を有し、該多面体射影処理により求められた射影領域を、前記存在可能領域の推定領域として出力するように構成されていることを特徴とする移動ロボットの特定状態量の存在可能領域推定装置。
【請求項２】
請求項１記載の移動ロボットの特定状態量の存在可能領域推定装置において、
前記接触反力制約条件は、前記複数の接触動作部のそれぞれの接触反力のうちの摩擦力の許容範囲を表す条件と、該接触反力のうちの抗力の向きを表す条件と、該接触反力の圧力中心点の存在許容範囲を表す条件と、前記移動ロボットの各関節で発生し得る駆動力の範囲に応じて規定される該接触反力の許容範囲を表す条件とのうちの１つ以上の条件を含むことを特徴とする移動ロボットの特定状態量の存在可能領域推定装置。
【請求項３】
請求項１又は２記載の移動ロボットの特定状態量の存在可能領域推定装置において、
前記特定状態量は、２個の状態量ｓ(1)，ｓ(2)から成り、
前記多面体射影処理部は、次の処理１〜４を前記多面体射影処理として実行するように構成されていることを特徴とする移動ロボットの特定状態量の存在可能領域推定装置。
処理１：前記力学的関係と前記接触反力制約条件とを満たし得るｓ(1)の上限値と下限値とをそれぞれ線形計画法の処理により求める。
処理２：ｓ(1)の上限値と下限値との間の範囲に、該上限値と下限値とを含めて複数の代表値を設定する。
処理３：ｓ(1)の値が前記処理２で設定した複数の代表値のうちの１つの代表値に一致することを前提条件として、前記力学的関係と前記接触反力制約条件とを満たし得るｓ(2)の上限値と下限値とをそれぞれ線形計画法の処理により求めることを、ｓ(1)の全ての代表値に対して実行する。
処理４：ｓ(1)の全ての代表値のそれぞれと、ｓ(1)のそれぞれの代表値毎のｓ(2)の上限値及び下限値のそれぞれとにより規定される複数の点を凸包してなる領域を前記射影領域として求める。
【請求項４】
請求項１又は２記載の移動ロボットの特定状態量の存在可能領域推定装置において、
前記特定状態量は３個以上の状態量ｓ(i)（ｉ＝１，２，…，Ｎ）から成り、
前記多面体射影処理部は、次の処理１１〜１８を前記多面体射影処理として実行するように構成されていることを特徴とする移動ロボットの特定状態量の存在可能領域推定装置。
処理１１：前記力学的関係と前記接触反力制約条件とを満たし得るｓ(1)の上限値と下限値とをそれぞれ線形計画法の処理により求める。
処理１２：ｓ(1)の上限値と下限値との間の範囲に、該上限値と下限値とを含めて複数の代表値を設定する。
処理１３：ｓ(1)の値が前記処理１２で設定した複数の代表値のうちの１つの代表値に一致することを前提条件として、前記力学的関係と前記接触反力制約条件とを満たし得るｓ(2)の上限値と下限値とをそれぞれ線形計画法の処理により求めることを、ｓ(1)の全ての代表値に対して実行する。
処理１４：ｓ(1)，ｓ(2)を変数成分とする２次元空間において、ｓ(1)の全ての代表値のそれぞれと、当該それぞれの代表値毎のｓ(2)の上限値及び下限値のそれぞれとにより規定される複数の点を凸包してなる２次元凸包領域内の複数の代表点であって、該２次元凸包領域の各頂点を含む複数の代表点を設定する。
処理１５：Ｎ個の状態量ｓ(i)（ｉ＝１，２，…，Ｎ）のうち、ｋ−１個（ただし、ｋ：３以上の整数）の状態量ｓ(1)，…，ｓ(k-1)のそれぞれの値が、ｋ−1次元凸包領域内の設定済の１つの代表点での値に一致することを前提条件として、前記力学的関係と前記接触反力制約条件とを満たし得るｓ(k)の上限値と下限値とをそれぞれ線形計画法の処理により求めることを、前記ｋ−１次元凸包領域内で設定された全ての代表点に対して実行する。
処理１６：ｋ＜Ｎである場合に、ｋ個の状態量ｓ(1)，…，ｓ(k)を変数成分とするｋ次元空間において、前記ｋ−１次元凸包領域内で設定された全ての代表点のそれぞれに対応するｓ(1)，…，ｓ(k-1)の値の組と、当該それぞれの代表点毎のｓ(k)の上限値及び下限値のそれぞれとにより規定される複数の点を凸包してなるｋ次元凸包領域内の複数の代表点であって、該ｋ次元凸包領域の各頂点を含む複数の代表点を設定する。
処理１７：ｋ＝Ｎであるときの前記処理１５が完了するまでは、前記処理１５及び処理１６を繰り返し、ｋ＝Ｎであるときの前記処理１５が完了した場合には、処理１８に進む。
処理１８：Ｎ個の状態量ｓ(i)（ｉ＝１，２，…，Ｎ）を変数成分とするＮ次元空間において、Ｎ−１次元凸包領域内で設定された全ての代表点のそれぞれに対応するｓ(1)，…，ｓ(N-1)の値の組と、当該それぞれの代表点毎のｓ(N)の上限値及び下限値のそれぞれとにより規定される複数の点を凸包してなるＮ次元凸包領域を、前記多面体射影領域として求める。
【請求項５】
複数の接触動作部のそれぞれの空中移動と該空中移動に続く外界物への接触とを繰り返す運動により移動する移動ロボットの全体重心の位置、全体重心の並進運動状態量、及び全体重心周りの回転運動状態量のうちの１つ以上の状態量から成る特定状態量の値の存在可能領域であって、前記各接触動作部に作用する接触反力に関する制約条件である接触反力制約条件を満たし得る存在可能領域を推定する方法であって、
前記複数の接触動作部のそれぞれの接触反力と前記特定状態量とを変数成分として構成される空間内の凸多面体であって、該接触反力と特定状態量との間の力学的関係と前記接触反力制約条件を表す線形不等式とにより規定される凸多面体を、前記変数成分のうちの前記特定状態量を表す部分空間に射影してなる射影領域を求める多面体射影処理として、Multi parametric linear programingの処理と、線形計画法の処理を複数回繰り返す処理とのうちのいずれかの処理を実行するステップを有し、該多面体射影処理により求められた射影領域を、前記存在可能領域の推定領域として出力することを特徴とする移動ロボットの特定状態量の存在可能領域推定方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動ロボットの全体重心の位置等の特定状態量の存在可能領域を推定する装置及び方法に関する。
【背景技術】
【０００２】
複数の接触動作部のそれぞれの空中移動と該空中移動に続く外界物への接触とを行う運動により移動する移動ロボットとしては、人型ロボット等の脚式移動ロボットが従来より一般に知られている。なお、本明細書では、移動ロボットの「接触動作部」は、移動ロボットの移動時に、空中移動と該空中移動に続く外界物への接触とを行う部位を意味する。例えば、脚式移動ロボットでは、その各脚の先端部（足部）が「接触動作部」に該当する。
【０００３】
この種の移動ロボットの目標運動を生成する技術では、各接触動作部の目標とする接触位置姿勢に応じて規定される支持多角形内に目標ＺＭＰ（ＺＭＰ（Zero Moment Point）の目標位置）を設定し、該目標ＺＭＰを満足するように、倒立振子モデル等の動力学モデルを用いて移動ロボットの目標運動を生成することが従来より一般に行われている（例えば特許文献１を参照）。
【０００４】
また、非特許文献１には、脚式移動ロボットの静的平衡のロバスト性に関する試験技術が提案されている。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特許第５２２１６８８号公報
【非特許文献】
【０００６】
【非特許文献１】Fast Algorithms to Test Robust Static Equilibrium for Legged Robots/ Andrea Del Prete, Steve Tonneau and Nicolas Mansard/ 2016 IEEE International Conference on Robotics and Automation (ICRA)/ Stockholm, Sweden, May 16-21, 2016
【発明の概要】
【発明が解決しようとする課題】
【０００７】
近年、人型ロボット等の移動ロボットを、概ね平坦な床面上を移動させるだけでなく、種々様々な環境下にて、種々様々な動作パターンで３次元的に移動させることの要求が高まっている。例えば、梯子の昇降、段差もしくは凹凸が多い不整地での移動、坂道もしくは段差での手摺りを使用した移動等を行わせることの要求が高まっている。
【０００８】
このように移動ロボットを種々様々な動作パターンで３次元的に移動させる場合、該移動ロボットは、平坦な床面上を移動する場合と異なる形態で、外界物から各接触動作部を介して接触反力を受ける。例えば、脚式移動ロボットが平坦な床面上を移動（歩行）する場合には、該移動ロボットは、その全体重心が概ね支持多角形の直上近辺に位置する状態で接触反力（床面からの反力）を受けるのに対して、該移動ロボットが梯子を昇降する場合には、該移動ロボットは、その全体重心が梯子に接触させた各接触動作部から水平方向に離れた状態で接触反力（梯子からの反力）を受ける。
【０００９】
従って、移動ロボットを種々様々な動作パターンで３次元的に移動させる場合、その移動を安定に行い得る目標運動を生成するためには、各接触動作部が受ける接触反力に関して、移動ロボットを平坦な床面上で移動させる場合には不要であるか、もしくは必要性が乏しい制約条件（例えば、移動ロボットのスピンの発生を防止するための制約条件等）を満たすように、移動ロボットの目標運動を生成する必要がある。
【００１０】
このため、移動ロボットを種々様々な動作パターンで３次元的に移動させる目標運動を、平坦な床面上で移動ロボットを移動させる場合と同様の手法によって適切に生成することは困難であった。
【００１１】
一方、移動ロボットの全体重心の位置、あるいは、全体重心の並進運動状態量、あるいは、全体重心周りの回転運動状態量に着目した場合、これらの３種類の状態量（以降、総称的に全体運動状態量ということがある）は、移動ロボットの動作パターンによらずに、外界物に接触させる全ての接触動作部が受ける接触反力の合力と一定の力学的関係を有する。
【００１２】
ここで、本明細書では、移動ロボットの「全体重心の並進運動状態量」は、移動ロボットの全体重心の並進運動によって発生する並進慣性力を規定する状態量（具体的には、全体重心の並進加速度、又は全体重心の並進運動量の時間的変化率）を意味する。また、移動ロボットの「全体重心周りの回転運動状態量」は、移動ロボットの全体重心周りの回転運動によって発生する回転慣性力を規定する状態量（具体的には、全体重心周りの回転角加速度、又は全体重心周りの角運動量の時間的変化率）を意味する。
【００１３】
この場合、上記「力学的関係」は、移動ロボットに各接触動作部を介して作用する接触反力の合力と、移動ロボットに作用する重力と、移動ロボットに発生する全体の慣性力との間の釣合を表す式により表される。
【００１４】
従って、目標運動での移動ロボットの動作パターンに対応して、該移動ロボットの各接触動作部が受ける接触反力の制約条件（以降、単に、接触反力制約条件ということがある）を満たすために必要な上記全体運動状態量の値の存在可能領域が、該接触反力制約条件と、上記力学的関係とに応じて規定されることとなる。
【００１５】
そして、上記全体運動状態量のそれぞれは、移動ロボットの全体の運動状態に応じたものとなる。
【００１６】
従って、移動ロボットを種々様々な動作パターンで３次元的に移動させる目標運動を生成する場合、各全体運動状態量の値の存在可能領域は、目標運動での移動ロボットの動作パターンに対応する接触反力制約条件を満たすように生成する（ひいては、移動ロボットが安定に移動し得るように生成する）ための運動学的な基本指針（運動学的に満たすべき条件）として使用することができることとなる。
【００１７】
そこで、移動ロボットを種々様々な動作パターンで３次元的に移動させる目標運動を生成する場合、接触反力制約条件を満たすように目標運動を生成するための指針として、１つ又は複数の全体運動状態量についての存在可能領域を推定（特定）し得る技術が望まれる。
【００１８】
また、この場合、移動ロボットの移動を行いながら目標運動を生成することを可能とするために、上記存在可能領域を比較的高速で推定し得ることが望まれる。
【００１９】
本発明はかかる背景に鑑みてなされたものであり、接触反力制約条件を満たすための特定状態量（移動ロボットの全体重心の位置、全体重心の並進運動状態量、及び全体重心周りの回転運動状態量のうちの１つ以上の状態量）の存在可能領域を適切に推定することができる装置及び方法を提供することを目的とする。
【課題を解決するための手段】
【００２０】
本発明の移動ロボットの特定状態量の存在可能領域推定装置は、複数の接触動作部のそれぞれの空中移動と該空中移動に続く外界物への接触とを繰り返す運動により移動する移動ロボットの全体重心の位置、全体重心の並進運動状態量、及び全体重心周りの回転運動状態量のうちの１つ以上の状態量から成る特定状態量の値の存在可能領域であって、前記各接触動作部に作用する接触反力に関する制約条件である接触反力制約条件を満たし得る存在可能領域を推定する装置であって、
前記複数の接触動作部のそれぞれの接触反力と前記特定状態量とを変数成分として構成される空間内の凸多面体であって、該接触反力と特定状態量との間の力学的関係と前記接触反力制約条件を表す線形不等式とにより規定される凸多面体を、前記変数成分のうちの前記特定状態量を表す部分空間に射影してなる射影領域を求める多面体射影処理として、Multi parametric linear programingの処理と、線形計画法の処理を複数回繰り返す処理とのうちのいずれかの処理を実行する多面体射影処理部を有し、該多面体射影処理により求められた射影領域を、前記存在可能領域の推定領域として出力するように構成されていることを特徴とする（第１発明）。
【００２１】
なお、上記「接触反力」は、並進力、あるいは、並進力及びモーメントの組によって表される接触力である。
【００２２】
ここで、移動ロボットを種々様々な動作パターンで移動させる場合、該移動ロボットの各接触動作部に作用する接触反力に関する制約条件（接触反力制約条件）は、一般に、線形不等式により表すことができる。この場合、前記複数の接触動作部のそれぞれの接触反力と前記特定状態量とを変数成分として構成される空間（以降、多変数空間ということがある）を想定した場合、該多変数空間内で、前記力学的関係と前記接触反力制約条件とを満たす点（接触反力及び特定状態量の値の組）の集合は、当該多変数空間における凸多面体により囲まれた領域として表される。
【００２３】
なお、第１発明では、上記凸多面体は、有界の領域（閉領域）を形成する凸多面体と、有界でない領域（開領域）を形成する凸多面体とのいずれであってもよい。
【００２４】
そして、上記凸多面体は、多面体射影処理により前記多変数空間の一部の変数成分を表す部分空間（上記多変数空間よりも低次元の空間）に射影することが可能である。そして、該多面体射影処理は、Multi parametric linear programingの処理と、線形計画法の処理を複数回繰り返す処理とのうちのいずれかの処理によって、比較的短時間で実行することが可能である。
【００２５】
そこで、本発明の存在可能領域推定装置は、上記凸多面体を、前記多変数空間の変数成分のうちの前記特定状態量を表す部分空間に射影してなる射影領域を求める多面体射影処理として、Multi parametric linear programingの処理と、線形計画法の処理を複数回繰り返す処理とのいずれかの処理を実行する。この多面体射影処理により求められる射影領域が、前記接触反力制約条件を満たし得る特定状態量の存在可能領域として得られることとなる。
【００２６】
この場合、前記凸多面体を規定する接触反力制約条件は、線形不等式により表される条件である。また、前記力学的関係は、後述する式（８）で示す如く、比較的簡易な関係式により表される。このため、前記存在可能領域の推定領域としての前記射影領域は、Multi parametric linear programingの処理と、線形計画法の処理を複数回繰り返す処理とのいずれかの処理により比較的高速に求めることができる。
【００２７】
よって、本発明によれば、接触反力制約条件を満たすための特定状態量（移動ロボットの全体重心の位置、全体重心の並進運動状態量、及び全体重心周りの回転運動状態量のうちの１つ以上の状態量）の存在可能領域を適切に推定することができる。
【００２８】
かかる本発明では、前記制約条件は、前記複数の接触動作部のそれぞれの接触反力のうちの摩擦力の許容範囲を表す条件と、該接触反力のうちの抗力の向きを表す条件と、該接触反力の圧力中心点の存在許容範囲を表す条件と、前記移動ロボットの各関節で発生し得る駆動力の範囲に応じて規定される該接触反力の許容範囲を表す条件とのうちの１つ以上の条件を含むことが好ましい（第２発明）。
【００２９】
これによれば、移動ロボットの目標運動により実現しようとする種々様々な動作パターンに対して、移動ロボットの移動を安定に行い得るようにするための適切な接触反力制約条件を設定することが可能となる。ひいては、種々様々な動作パターンに対して、特定状態量の好適な存在許容領域を推定することが可能となる。
【００３０】
上記第１発明又は第２発明では、前記多面体射影処理のうち、線形計画法の処理を複数回繰り返す処理は、具体的には、次のように構成され得る。
【００３１】
すなわち、前記特定状態量が、２個の状態量ｓ(1)，ｓ(2)から成る場合において、前記多面体射影処理部は、次の処理１〜４を前記多面体射影処理として実行するように構成され得る（第３発明）。
【００３２】
処理１：前記力学的関係と前記接触反力制約条件とを満たし得るｓ(1)の上限値と下限値とをそれぞれ線形計画法の処理により求める。
【００３３】
処理２：ｓ(1)の上限値と下限値との間の範囲に、該上限値と下限値とを含めて複数の代表値を設定する。
【００３４】
処理３：ｓ(1)の値が前記処理２で設定した複数の代表値のうちの１つの代表値に一致することを前提条件として、前記力学的関係と前記接触反力制約条件とを満たし得るｓ(2)の上限値と下限値とをそれぞれ線形計画法の処理により求めることを、ｓ(1)の全ての代表値に対して実行する。
【００３５】
処理４：ｓ(1)の全ての代表値のそれぞれと、ｓ(1)のそれぞれの代表値毎のｓ(2)の上限値及び下限値のそれぞれとにより規定される複数の点を凸包してなる領域を前記射影領域として求める。
【００３６】
また、前記特定状態量が、３個以上の状態量ｓ(i)（ｉ＝１，２，…，Ｎ）から成る場合において、前記多面体射影処理部は、次の処理１１〜１８を前記多面体射影処理として実行するように構成され得る（第４発明）。
【００３７】
処理１１：前記力学的関係と前記接触反力制約条件とを満たし得るｓ(1)の上限値と下限値とをそれぞれ線形計画法の処理により求める。
【００３８】
処理１２：ｓ(1)の上限値と下限値との間の範囲に、該上限値と下限値とを含めて複数の代表値を設定する。
【００３９】
処理１３：ｓ(1)の値が前記処理１２で設定した複数の代表値のうちの１つの代表値に一致することを前提条件として、前記力学的関係と前記接触反力制約条件とを満たし得るｓ(2)の上限値と下限値とをそれぞれ線形計画法の処理により求めることを、ｓ(1)の全ての代表値に対して実行する。
【００４０】
処理１４：ｓ(1)，ｓ(2)を変数成分とする２次元空間において、ｓ(1)の全ての代表値のそれぞれと、当該それぞれの代表値毎のｓ(2)の上限値及び下限値のそれぞれとにより規定される複数の点を凸包してなる２次元凸包領域内の複数の代表点であって、該２次元凸包領域の各頂点を含む複数の代表点を設定する。
【００４１】
処理１５：Ｎ個の状態量ｓ(i)（ｉ＝１，２，…，Ｎ）のうち、ｋ−１個（ただし、ｋ：３以上の整数）の状態量ｓ(1)，…，ｓ(k-1)のそれぞれの値が、ｋ−1次元凸包領域内の設定済の１つの代表点での値に一致することを前提条件として、前記力学的関係と前記接触反力制約条件とを満たし得るｓ(k)の上限値と下限値とをそれぞれ線形計画法の処理により求めることを、前記ｋ−１次元凸包領域内で設定された全ての代表点に対して実行する。
【００４２】
処理１６：ｋ＜Ｎである場合に、ｋ個の状態量ｓ(1)，…，ｓ(k)を変数成分とするｋ次元空間において、前記ｋ−１次元凸包領域内で設定された全ての代表点のそれぞれに対応するｓ(1)，…，ｓ(k-1)の値の組と、当該それぞれの代表点毎のｓ(k)の上限値及び下限値のそれぞれとにより規定される複数の点を凸包してなるｋ次元凸包領域内の複数の代表点であって、該ｋ次元凸包領域の各頂点を含む複数の代表点を設定する。
【００４３】
処理１７：ｋ＝Ｎであるときの前記処理１５が完了するまでは、前記処理１５及び処理１６を繰り返し、ｋ＝Ｎであるときの前記処理１５が完了した場合には、処理１８に進む。
【００４４】
処理１８：Ｎ個の状態量ｓ(i)（ｉ＝１，２，…，Ｎ）を変数成分とするＮ次元空間において、Ｎ−１次元凸包領域内で設定された全ての代表点のそれぞれに対応するｓ(1)，…，ｓ(N-1)の値の組と、当該それぞれの代表点毎のｓ(N)の上限値及び下限値のそれぞれとにより規定される複数の点を凸包してなるＮ次元凸包領域を、前記射影領域として求める。
【００４５】
補足すると、かかる第４発明における前記処理１５及び処理１６の繰り返しは、その繰り返し毎に、ｋの値が１ずつ、増加するように行われる。また、前記処理１５における「前記ｋ−１次元凸包領域内で設定された」代表点というのは、前記処理１４で設定された代表点、あるいは、該処理１５の直前に実行された前記処理１６で設定された代表点である。さらに、Ｎ＝３である場合には、処理１４の次の処理１５において、ｋ＝Ｎとなるので、処理１６，１７は実質的に省略されることとなる。
【００４６】
上記第３発明によれば、前記特定状態量が２個の状態量ｓ(1)，ｓ(2)から成る場合において、前記処理１〜処理３によって、前記射影領域の境界面上の複数の点が求められることとなる。従って、前記処理４において、これらの複数の点を凸包してなる領域を求めることで、前記射影領域を近似的に求めることができることとなる。
【００４７】
また、上記第４発明によれば、前記特定状態量が、３個以上の状態量ｓ(i)（ｉ＝１，２，…，Ｎ）から成る場合において、前記処理１１〜処理１７によって、前記射影領域の境界面上の複数の点が求められることとなる。従って、前記処理１８において、これらの複数の点を凸包してなる領域を求めることで、前記射影領域を近似的に求めることができることとなる。
【００４８】
この場合、第３発明における前記処理１及び処理３、あるいは、第４発明における処理１１、処理１３及び処理１５では、公知の線形計画法の処理により比較的高速に各状態量の上限値及び下限値を求めることができる。
【００４９】
また、本発明の移動ロボットの特定状態量の存在可能領域推定方法は、上記目的を達成するために、複数の接触動作部のそれぞれの空中移動と該空中移動に続く外界物への接触とを繰り返す運動により移動する移動ロボットの全体重心の位置、全体重心の並進運動状態量、及び全体重心周りの回転運動状態量のうちの１つ以上の状態量から成る特定状態量の値の存在可能領域であって、前記各接触動作部に作用する接触反力に関する制約条件である接触反力制約条件を満たし得る存在可能領域を推定する方法であって、
前記複数の接触動作部のそれぞれの接触反力と前記特定状態量とを変数成分として構成される空間内の凸多面体であって、該接触反力と特定状態量との間の力学的関係と前記接触反力制約条件を表す線形不等式とにより規定される凸多面体を、前記変数成分のうちの前記特定状態量を表す部分空間に射影してなる射影領域を求める多面体射影処理として、Multi parametric linear programingの処理と、線形計画法の処理を複数回繰り返す処理とのうちのいずれかの処理を実行するステップを有し、該多面体射影処理により求められた射影領域を、前記存在可能領域の推定領域として出力することを特徴とする（第５発明）。
【００５０】
かかる第５発明によれば、前記第１発明と同様の効果を奏することができる。なお、第５発明は、前記第２〜第４発明と同様の態様を採用することもできる。
【図面の簡単な説明】
【００５１】
【図１】本発明の一実施形態における移動ロボットの概略構成を示す図。
【図２】実施形態における存在可能領域推定装置としての演算処理装置を示すブロック図。
【図３】各接地動作部に対して設定される座標系（第ｉ接触座標系）を示す図。
【図４】多面体射影処理を概念的に示す図。
【図５】多面体射影処理の第１の例の処理を示すフローチャート。
【図６】図６Ａ及び図６Ｂは図５のフローチャートに関する説明図。
【図７】多面体射影処理の第２の例の処理を示すフローチャート。
【図８】図８Ａ及び図８Ｂは図７のフローチャートに関する説明図。
【図９】図９Ａは梯子の昇降を行わせる移動ロボットの接触動作部の梯子への接触パターンの第１例を示す図、図９Ｂは該第１例に対応して求められる重心存在許容領域を示す図。
【図１０】図１０Ａは梯子の昇降を行わせる移動ロボットの接触動作部の梯子への接触パターンの第２例を示す図、図１０Ｂは該第２例に対応して求められる重心存在許容領域を示す図。
【図１１】図１１Ａは梯子の昇降を行わせる移動ロボットの接触動作部の梯子への接触パターンの第３例を示す図、図１１Ｂは該第３例に対応して求められる重心存在許容領域を示す図。
【図１２】図１２Ａは移動ロボットを基台上から梯子に乗り移らせる場合の動作例を示す図、図１２Ｂは該動作例に対応して生成される目標運動における移動ロボットの全体重心の遷移形態と、重心存在可能領域とを例示する図。
【発明を実施するための形態】
【００５２】
本発明の一実施形態を図１〜図１０Ｂを参照して以下に説明する。図１を参照して、本実施形態における移動ロボット１は、一例として、人型のロボットである。この移動ロボット１（以降、単にロボット１ということがある）は、上体に相当する基体２と、基体２から延設された複数の可動リンクとしての左右一対の（２つの）脚リンク３Ｌ，３Ｒ及び左右一対の（２つの）腕リンク４Ｌ，４Ｒと、頭部５とを備える。
【００５３】
なお、本実施形態の説明では、参照符号に付加する「Ｒ」，「Ｌ」は、それぞれロボット１の基体２の正面に向かって右側の要素、左側の要素を意味する。ただし、左右を特に区別する必要が無いときは、符号「Ｒ」，「Ｌ」を省略する。
【００５４】
また、以降の説明では、脚リンク３Ｌ，３Ｒ及び腕リンク４Ｌ，４Ｒのそれぞれを単に可動リンクということがある。
【００５５】
各脚リンク３は、基体２の下部から延設されている。各脚リンク３は、大腿部１１、下腿部１２、足部１３にそれぞれ相当する要素リンクを、基体２側から順番に、股関節１４、膝関節１５、足首関節１６を介して連結して構成されている。各脚リンク３の先端部たる足部１３は、ロボット１の移動時等に外界物（地面、床面、壁面等）に適宜接触させる接触動作部である。
【００５６】
各脚リンク３の足部１３と基体２との間の関節１４，１５，１６は、本実施形態では、該脚リンク３の足部１３が、基体２に対して例えば６自由度の運動自由度を有するように構成されている。
【００５７】
例えば、股関節１４は、総計３軸の回転自由度を有するように３つの要素関節（図示省略）により構成される。膝関節１５は、1軸の回転自由度を有するように単一の要素関節（図示省略）により構成される。足首関節１６は、総計２軸の回転自由度を有するように２つの要素関節（図示省略）により構成される。なお、要素関節は、１軸の回転自由度を有する関節である。該要素関節は、１軸周りに相対回転自在に係合された２つの部材により構成される。
【００５８】
各腕リンク４は、基体２の上部から延設されている。各腕リンク４は、上腕部２１、前腕部２２、ハンド部２３にそれぞれ相当する要素リンクを、基体２側から順番に、肩関節２４、肘関節２５、手首関節２６を介して連結して構成されている。
【００５９】
本実施形態では、各腕リンク４は、脚リンク３，３以外で、必要に応じて外界物に接触させる脚リンクとして機能し得る可動リンクである。この場合、各腕リンク４の先端部たるハンド部２３が、ロボット１の移動時の動作形態等に応じて、外界物（地面、床面、壁面等）に適宜接触させる接触動作部である。
【００６０】
各腕リンク４のハンド部２３と基体２との間の関節２４，２５，２６は、本実施形態では、各腕リンク４のハンド部２３が、基体２に対して例えば６自由度を有するように構成されている。
【００６１】
例えば、肩関節２４は、総計３軸の回転自由度を有するように３つの要素関節（図示省略）により構成される。肘関節２５は、1軸の回転自由度を有するように単一の要素関節（図示省略）により構成される。手首関節２６は、総計２軸の回転自由度を有するように２つの要素関節（図示省略）により構成される。
【００６２】
また、各腕リンク４のハンド部２３は、本実施形態では、物体の把持を行うことができるように構成されている。例えば、各ハンド部２３は、適宜のクランプ機構、あるいは、人の手指と同様の動作を行い得る複数の指機構等により構成される。
【００６３】
頭部５は、基体２の上端部に首関節３１を介して取り付けられている。首関節３１は、１軸、２軸、又は３軸の回転自由度を有するように構成される。
【００６４】
以上がロボット１の機構的な構成の概要である。
【００６５】
上記の如く構成されたロボット１は、４つの可動リンク３，３，４，４のうちの１つ以上の可動リンク３又は４を外界物に接触させずに空中で動かす動作（空中移動動作）と該動作に続いて該可動リンク３又は４を外界物に接触させる動作とを繰り返す運動によって移動することができる。
【００６６】
例えば、人の歩行動作における脚の運動と同様の運動パターン（２脚歩容の運動パターン）で２つの脚リンク３，３を動かすことで、ロボット１の歩行動作を行うことができる。
【００６７】
この場合、腕リンク４を壁、手すり等に適宜接触させながら、脚リンク３，３を動かすことで、ロボット１の移動を行うこともできる。
【００６８】
また、例えば、４つの可動リンク３，３，４，４を脚として使用して、該４つの可動リンク３，３，４，４を、トロット、クロール、もしくはペース等の運動パターン（４脚歩容の運動パターン）で動かすことで、ロボット１の移動を行うこともできる。
【００６９】
さらに、例えば４つの可動リンク３，３，４，４を使用して、梯子等の昇降を行うこともできる。
【００７０】
補足すると、ロボット１の各脚リンク３あるいは各腕リンク４は、７自由度以上の運動自由度を有するように構成されていてもよい。また、各脚リンク３及び各腕リンク４のそれぞれは、回転型の関節に限らず、直動型の関節を含んでいてもよい。
【００７１】
また、ロボット１がハンド部２３による把持動作を必要としないロボットである場合には、各腕リンク４の先端部（接触動作部）は、把持動作のための機構を備えない構造のものであってもよい。また、ロボット１は、頭部５を備えないものであってもよい。
【００７２】
また、基体２は関節を含んでいてもよい。例えば、基体２は、脚リンク３，３が連結された下側基体と、腕リンク４，４が連結された上側基体と、これらの上側基体及び下側基体を相対変位可能に連結する関節とにより構成されたものであってもよい。
【００７３】
なお、以降の説明では、各脚リンク３の足部１３及び各腕リンク４のハンド部２３を総称的に接触動作部（１３又は２３）ということがある。
【００７４】
図２は、ロボット１の特定状態量の存在可能領域推定装置としての機能を有する演算処理装置４０を示している。この演算処理装置４０は、ＣＰＵ、ＲＡＭ、ＲＯＭ、インターフェース回路等を含む１つ以上の電子回路ユニットにより構成され、あるいは、１つ以上のコンピュータにより構成される。該演算処理装置４０は、ロボット１に搭載され、あるいは、外部に配置される。なお、演算処理装置４０は、ロボット１の動作制御を行う機能を含んでいてもよい。
【００７５】
本実施形態では、演算処理装置４０には、目標とする動作パターンでのロボット１の移動動作（目標運動）の複数のサンプリング時刻（例えば、所定の刻み時間毎の時刻）のそれぞれの時刻において、外界物に接触させる各接触動作部（１３又は２３）の目標位置姿勢を示す接触動作部位置姿勢情報と、各サンプリング時刻で外界物に接触させる各接触動作部（１３又は２３）が外界物から受ける接触反力に関する制約条件である接触反力制約条件を示す情報と、ロボット１の目標運動に関して演算処理装置４０により存在可能領域を推定すべき特定状態量の種類を指示する特定状態量指示情報とが与えられる。なお、上記接触動作部位置姿勢情報には、各サンプリング時刻で外界物に接触させる各接触動作部（１３又は２３）が、ロボット１のどの部位（足部１３Ｒ，１３Ｌ及びハンド部２３Ｒ，２３Ｌのいずれか）であるかを示す情報も含まれる。
【００７６】
ここで、上記特定状態量指示情報により示される特定状態量は、ロボット１の全体の運動に関する状態量である。より詳しくは、該特定状態量は、ロボット１の全体重心の位置と、ロボット１の全体重心の並進運動状態量（全体重心の並進加速度、又は、全体重心の（ロボット１全体の）並進運動量の時間的変化率）と、ロボット１の全体重心周りの回転運動状態量（全体重心周りにおけるロボット１全体の回転角加速度、又は、全体重心周りにおけるロボット１全体の角運動量の時間的変化率）とのうちの１つ以上の状態量である。
【００７７】
例えば、ロボット１の全体重心の空間的な位置を表す３次元ベクトルの１つもしくは複数の成分、あるいは、ロボット１の全体重心の空間的な並進運動量もしくは並進加速度を表す３次元ベクトルの１つもしくは複数の成分、あるいは、ロボット１の全体重心周りの空間的な角運動量の時間的変化率を表す３次元ベクトルの１つもしくは複数の成分、あるいは、これらの成分の組み合わせを、特定状態量として採用することが可能である。
【００７８】
また、上記接触動作部位置姿勢情報により示される各接触動作部（１３又は２３）の目標位置姿勢は、各接触動作部（１３又は２３）の位置及び姿勢のそれぞれの目標値の組を意味する。
【００７９】
なお、本実施形態の説明では、各接触動作部（１３又は２３）の「位置」は、該接触動作部（１３又は２３）の代表点の空間的な位置を意味し、各接触動作部（１３又は２３）の「姿勢」は、該接触動作部（１３又は２３）の空間的な向きを意味する。
【００８０】
そして、各接触動作部（１３又は２３）の目標位置姿勢は、ロボット１の移動環境に対して任意に設計的に設定されたグローバル座標系（例えば図１に示すＸＹＺ座標系）で見た位置及び姿勢の組である。
【００８１】
また、上記接触反力制約条件に関して、各接触動作部（１３又は２３）が外界物から受ける接触反力は、所謂、接触レンチを意味するものである。この接触反力（接触レンチ）は、並進力（３次元ベクトル）とモーメント（３次元ベクトル）との組（６成分のベクトル）である。
【００８２】
そして、上記接触反力制約条件は、本実施形態では、例えば、各接触動作部（１３又は２３）が外界物から受ける接触反力のうちの摩擦力の許容範囲を表す条件（以降、摩擦力制約条件という）と、各接触動作部（１３又は２３）が受ける接触反力の圧力中心点（所謂、ＣＯＰ（Center of Pressure））の存在許容範囲を表す条件（以降、ＣＯＰ制約条件という）と、接触反力のうちの抗力の向きを制限する条件（抗力制約条件）とを含む。
【００８３】
これらの接触反力制約条件は、以下に説明する如く設定（定義）された条件である。
【００８４】
ここで、以降の説明では、各接触動作部（１３又は２３）を区別する識別番号を表す符号として、「ｉ」（ｉ＝１，２，…，ＮＬ。ＮＬ：接触動作部（１３又は２３）の総個数）を使用する。そして、識別番号ｉの接触動作部（１３又は２３）を第ｉ接触動作部ということがある。
【００８５】
また、第ｉ接触動作部に関する接触反力制約条件に係る接触反力を表現する座標系として、便宜上、例えば図３に示す如く設定されるｘｙｚ座標系を使用する。該ｘｙｚ座標系は、詳しくは、第ｉ接触動作部と外界物との接触面に垂直な方向の軸をｚ軸、該接触面を含む平面上で互いに直交する２軸をｘ軸及びｙ軸として定義したローカル座標系（以降、第ｉ接触座標系という）である。この第ｉ接触座標系の原点は、第ｉ接触動作部と外界物との接触面を含む平面上の点である。
【００８６】
そして、第ｉ接触動作部が、接触する外界物から受ける接触反力のうちの並進力のｘ軸方向成分、ｙ軸方向成分、ｚ軸方向成分をそれぞれ、ｆx(i)，ｆy(i)，ｆz(i)、該接触反力のうちのモーメントのｘ軸周り方向成分、ｙ軸周り方向成分、ｚ軸周り方向成分をそれぞれ、ｎx(i)，ｎy(i)，ｎz(i)と表記する。
【００８７】
さらに、ｆx(i)，ｆy(i)，ｆz(i)，ｎx(i)，ｎy(i)，ｎz(i)の６つの成分からなるベクトル（縦ベクトル）、すなわち、第ｉ接触動作部が受ける接触反力を、第ｉ接触座標系で見たベクトルとして表現したものを↑ｗ(i)（≡[ｆx(i)，ｆy(i)，ｆz(i)，ｎx(i)，ｎy(i)，ｎz(i)]T）と表記する。なお、本明細書では、先頭に「↑」を付した変数は、複数成分のベクトル（縦ベクトル）を意味し、上付き添え字「Ｔ」は、ベクトル又は行列の転置を意味する。
【００８８】
また、本明細書では、乗算又は内積を表す算術記号として「・」を使用し、外積（ベクトル積）を表す算術記号として「×」と使用する。
【００８９】
以上を前提として、接触反力制約条件を以下に説明する。上記摩擦力制約条件は、より詳しくは、各接触動作部（１３又は２３）が外界物から受ける接触反力のうちの並進摩擦力の許容範囲を表す並進摩擦力制約条件と、該接触反力のうちの回転摩擦力（摩擦力モーメント）の許容範囲を表す回転摩擦力制約条件とから構成される。
【００９０】
並進摩擦力制約条件は、換言すれば、各接触動作部（１３又は２３）と外界物との間の並進滑りを発生させないようにするための制約条件である。この並進摩擦力制約条件は、例えば、次式（１ａ），（１ｂ）により表される。
【００９１】
μt(i)・ｆz(i)≧ｆx(i)≧−μt(i)・ｆz(i)    ……（１ａ）
μt(i)・ｆz(i)≧ｆy(i)≧−μt(i)・ｆz(i)    ……（１ｂ）
ここで、μt(i)は、第ｉ接触動作部が外界物から受ける接触反力↑ｗ(i)のうちの並進摩擦力に関する摩擦係数（設定値）である。そして、ｆx(i)，ｆy(i)は、該並進摩擦力のｘ軸方向成分及びｙ軸方向成分に相当し、ｆz(i)は接触反力のうちの垂直抗力に相当する。
【００９２】
補足すると、第ｉ接触動作部と外界物との間の並進滑りが発生しないための条件は、原理的には、√(ｆx(i)2＋ｆy(i)2)≦μt(i)・ｆz(i)という不等式（２次元ベクトルとしての並進摩擦力が、所謂、摩擦円と言われる許容領域内に収まるということを示す不等式）により表される。
【００９３】
ただし、この不等式は非線形な不等式であることから、本実施形態では、並進摩擦力制約条件を、近似的に、上記式（１ａ），（１ｂ）の不等式により表している。この場合、式（１ａ），（１ｂ）の不等式により規定される並進摩擦力（ベクトル）の許容領域としての四角形領域が、上記摩擦円内から逸脱しないように、式（１ａ），（１ｂ）における摩擦係数μt(i)の値は、第ｉ接触動作部と外界物との実際の摩擦係数の推定値よりも小さめの値に設定される。
【００９４】
なお、式（１ａ），（１ｂ）の不等式により規定される並進摩擦力の許容領域は、詳しくは、第ｉ接触座標系のｘ軸に平行な２つの直線とｙ軸に平行な２つの直線とにより囲まれた四角形領域である。ただし、並進摩擦力制約条件により規定される並進摩擦力の許容領域の境界線は、ｘ軸あるいはｙ軸に対して傾斜していてもよい。また、該許容領域は、四角形以外の多角形領域であってもよい。この場合、並進摩擦力制約条件は、例えばａ・ｆx(i)＋ｂ・ｆy(i)≧ｃ・ｆz(i)（ａ，ｂ，ｃは定数値）という形式の複数の不等式により表すことができる。
【００９５】
前記回転摩擦力制約条件は、換言すれば、各接触動作部（１３又は２３）と外界物との間の回転滑り（スピン）を発生させないようにするための制約条件である。この回転摩擦力制約条件は、例えば、次式（２）により表される。
【００９６】
μr(i)・ｆz(i)≧ｎz(i)≧−μr(i)・ｆz(i)    ……（２）
ここで、μr(i)は、第ｉ接触動作部が外界物から受ける接触反力↑ｗ(i)のうちの接触面に垂直な軸周り方向の摩擦力モーメント（回転摩擦力）に関する摩擦係数（設定値）である。そして、ｎz(i)は、当該摩擦力モーメントに相当する。
【００９７】
補足すると、並進摩擦力制約条件及び回転摩擦力制約条件における上記摩擦係数μt(i)，μr(i)のそれぞれの設定値は、第ｉ接触動作部と外界物との間の滑り（並進滑り及び回転滑り）が発生するのを極力防止するようにロボット１の目標運動を生成する上では、実際の摩擦係数の推定値よりも小さめの値に設定しおくことが望ましい。
【００９８】
前記ＣＯＰ制約条件は、各接触動作部（１３又は２３）が外界物から受ける接触反力のＣＯＰを、該接触動作部（１３又は２３）と外界物との接触面内に収めるための条件である。このＣＯＰ制約条件は、次式（３ａ），（３ｂ）により表される。
【００９９】
Ｌxp(i)≧−ｎy(i)／ｆz(i)≧Ｌxm(i)    ……（３ａ）
Ｌyp(i)≧ｎx(i)／ｆz(i)≧Ｌym(i)    ……（３ｂ）
ここで、式（３ａ）における−ｎy(i)／ｆz(i)は、第ｉ接触動作部のＣＯＰのｘ軸方向位置に相当し、Ｌxp(i)，Ｌxm(i)は、それぞれ当該ＣＯＰのｘ軸方向位置の存在可能範囲の上限値、下限値の設定値である。
【０１００】
また、式（３ｂ）におけるｎx(i)／ｆz(i)は、第ｉ接触動作部のＣＯＰのｙ軸方向位置に相当し、Ｌyp(i)，Ｌym(i)は、それぞれ当該ＣＯＰのｙ軸方向位置の存在可能範囲の上限値、下限値の設定値である。
【０１０１】
Ｌxp(i)，Ｌxm(i)，Ｌyp(i)，Ｌym(i)は、式（３ａ），（３ｂ）により規定されるＣＯＰの存在許容領域（四角形領域）が、第ｉ接地動作部と外界物との接触面内に収まる領域（好ましくは、接触面よりも多少小さめの領域）となるように設定される。
【０１０２】
なお、式（３ａ），（３ｂ）の不等式により規定されるＣＯＰの存在許容領域は、詳しくは、第ｉ接触座標系のｘ軸に平行な２つの直線とｙ軸に平行な２つの直線とにより囲まれた四角形領域である。ただし、ＣＯＰ制約条件により規定されるＣＯＰの存在許容領域の境界線は、ｘ軸あるいはｙ軸に対して傾斜していてもよい。また、該存在許容領域は、四角形以外の多角形領域（六角形領域等）であってもよい。この場合、ＣＯＰ制約条件は、例えばａ・ｎx(i)＋ｂ・ｎy(i)≧ｃ・ｆz(i)（ａ，ｂ，ｃは定数値）という形式の複数の不等式により表すことができる。
【０１０３】
前記抗力制約条件は、各接触動作部（１３又は２３）が受ける接触反力のうちの並進力のベクトルの向きは、第ｉ接触動作部を外界物の内部側に引き込む向きにならないという条件である。この抗力制約条件は、例えば次式（４）により表される。
【０１０４】
ｆz(i)≧０    ……（４）
なお、ここでは、ｆz(i)の正方向は、第ｉ接触座標系のｚ軸方向で、接触面から第ｉ接触動作部側に向かう方向である。
【０１０５】
上記式（４）により表される抗力制約条件は、換言すれば、第ｉ接触動作部が受ける接触反力↑ｗ(i)のうち、前記第ｉ接触座標系のｚ軸方向の並進力成分ｆz(i)が負の値にならないという条件、あるいは、ｆz(i)の下限値がゼロであるという条件である。なお、抗力制約条件にけるｆz(i)の下限値は、ゼロよりも若干大きい値に設定されていてもよい。
【０１０６】
本実施形態では、演算処理装置４０に与えられる接触反力制約条件（摩擦力制約条件、ＣＯＰ制約条件、抗力制約条件）は以上の如く設定されている。
【０１０７】
ここで、上記の如く各接触動作部（１３又は２３）に関して設定された並進摩擦力制約条件、回転摩擦力制約条件、ＣＯＰ制約条件、及び抗力制約条件は、換言すれば、各接触動作部（１３又は２３）が受ける接触反力に関する線形不等式により表し得る制約条件である。
【０１０８】
すなわち、第ｉ接触動作部に関する上記各制約条件を表す各不等式は、接触反力↑ｗ(i)の各成分ｆx(i)，ｆy(i)，ｆz(i)，ｎx(i)，ｎy(i)，ｎz(i)を線形結合してなる値が、所定値以上又は所定値以下となるという形式の線形不等式（次式（５ａ）又は（５ｂ）の線形不等式）と等価である。
【０１０９】
↑ｃ(i)T・↑ｗ(i)≧ｋ(i)    ……（５ａ）
↑ｃ(i)T・↑ｗ(i)≦ｋ(i)    ……（５ｂ）
なお、↑ｃ(i)は、第ｉ接触動作部に作用する接触反力↑ｗ(i)の各成分ｆx(i)，ｆy(i)，ｆz(i)，ｎx(i)，ｎy(i)，ｎz(i)に係る係数を並べた係数ベクトル、ｋ(i)はある値の定数値である。
【０１１０】
例えば、前記並進摩擦力条件に関する式（１ａ）の不等式は、↑ｃ(i)＝[−１，０，μt(i)，０，０，０]T、且つｋ(i)＝０とした式（５ａ）の線形不等式（あるいは、↑ｃ(i)＝[１，０，−μt(i)，０，０，０]T、且つｋ(i)＝０とした式（５ｂ）の線形不等式）と、↑ｃ(i)＝[１，０，μt(i)，０，０，０]T、且つｋ(i)＝０とした式（５ａ）の線形不等式（あるいは、↑ｃ(i)＝[−１，０，−μt(i)，０，０，０]T、且つｋ(i)＝０とした式（５ｂ）の線形不等式）との組と等価である。このことは、前記式（１ｂ），（２），（３ａ），（３ｂ），（４）の不等式に関しても同様である。
【０１１１】
さらに、第ｉ接触座標系で見た接触反力↑ｗ(i)を、前記グローバル座標系（ロボット１の動作環境に設定されたＸＹＺ座標系）で見たベクトル（６成分のベクトル）に変換したものを、↑Ｗ(i)＝[ｆX(i)，ｆY(i)，ｆZ(i)，ｎX(i)，ｎY(i)，ｎZ(i)]と表記すると、↑Ｗと↑ｗとの間には、次式（６）の関係が成立する。
【０１１２】
↑ｗ(i)＝Ｒ(i)T・↑Ｗ    ……（６）
なお、Ｒ(i)は、グローバル座標系で見た第ｉ接地動作部の姿勢に応じて規定される６行６列の行列、Ｒ(i)TはＲ(i)の転置行列である。
【０１１３】
従って、第ｉ接触動作部が受ける接触反力に関する各接触反力制約条件を表す各不等式（式（１ａ），（１ｂ），（２），（３ａ），（３ｂ），（４）の不等式）は、次式（７ａ）又は（７ｂ）で示す如く、第ｉ接触動作部のグローバル座標系で見た接触反力↑Ｗ(i)の各成分ｆX(i)，ｆY(i)，ｆZ(i)，ｎX(i)，ｎY(i)，ｎZ(i)を線形結合してなる値が、所定値以上又は所定値以下となるという形式の線形不等式と等価でもある。
【０１１４】
(↑ｃ(i)T・Ｒ(i)T)・↑Ｗ(i)≧ｋ(i)    ……（７ａ）
(↑ｃ(i)T・Ｒ(i)T)・↑Ｗ(i)≦ｋ(i)    ……（７ｂ）
この場合、(↑ｃ(i)T・Ｒ(i)T)が、グローバル座標系で見た接触反力↑Ｗ(i)の各成分ｆX(i)，ｆY(i)，ｆZ(i)，ｎX(i)，ｎY(i)，ｎZ(i)に係る係数を並べた係数ベクトルとなる。
【０１１５】
以上の如く、各接触動作部（１３又は２３）に関して設定された並進摩擦力制約条件、回転摩擦力制約条件、ＣＯＰ制約条件、及び抗力制約条件は、各接触動作部（１３又は２３）が受ける接触反力を、該接触動作部（１３又は２３）に対応する第ｉ接触座標系で見た場合と、グローバル座標系で見た場合とのいずれの場合でも、接触反力の各成分を線形結合してなる値が、所定値以上又は所定値以下となるという形式の線形不等式により表し得る制約条件である。
【０１１６】
補足すると、接触反力制約条件は、各接触動作部（１３又は２３）が外界物から受ける接触反力のうちの１つ以上の各成分（並進力に係る成分又はモーメントに係る成分）の値の範囲を、ロボット１の各関節を駆動するアクチュエータの能力、各関節の構造等の仕様に応じて各関節で実際に発生させ得る駆動力により実現可能な範囲に制限する制約条件、あるいは、上記接触反力のうちの１つ以上の各成分の値の範囲を、実験的もしくは設計的に任意に設定した実用域の範囲に制限する制約条件を含んでいてもよい。これらの制約条件は、ａ≦接触反力の成分≦ｂ（ａ，ｂは定数値）という形式の不等式により表すことができる。従って、該制約条件も、上記の他の接触反力制約条件と同様に、線形不等式により表現し得る制約条件である。
【０１１７】
演算処理装置４０には、以上の如く、各サンプリング時刻での接触動作部位置姿勢情報と、各接触動作部（１３又は２３）毎の第ｉ接触座標系で見た接触反力↑ｗに関する接触反力制約条件（前記式（１ａ）〜（４）の不等式、又はこれと等価な線形不等式（式（５ａ）又は（５ｂ）の線形不等式）により表される制約条件）と、特定状態量指示情報とが与えられる。
【０１１８】
この演算処理装置４０は、実装されるプログラム又はハードウェア構成により実現される機能として、各サンプリング時刻での各接触動作部（１３又は２３）の接触反力の各成分（６成分）と、特定状態量指示情報で指示された特定状態量（グローバル座標系で見た特定状態量）の各成分とを変数成分として構成される多変数空間内の凸多面体を規定するパラメータ（以降、多面体パラメータという）を決定する多面体パラメータ決定部４１と、該多面体パラメータにより規定される凸多面体を、特定状態量の各成分を変数成分とする部分空間に射影する多面体射影処理を実行する多面体射影処理部４２とを含む。多面体射影処理部４２の処理により、特定状態量の存在可能領域が決定される。
【０１１９】
ここで、本実施形態における上記凸多面体は、より詳しくグローバル座標系で見た接触反力↑Ｗ(i)の各成分（ｆX(i)，ｆY(i)，ｆZ(i)，ｎX(i)，ｎY(i)，ｎZ(i)の６成分）と、グローバル座標系で見た特定状態量の各成分とを変数成分として構成される多変数空間（接触反力↑Ｗ(i)の成分の個数と、特定状態量の成分の個数との総和の次元数の空間）での多面体であって、該多変数空間内で、各サンプリング時刻での全ての接触動作部（第１接触動作部〜第ＮＬ接触動作部）の接触反力と特定状態量との間の力学的関係と、全ての接触動作部（第１接触動作部〜第ＮＬ接触動作部）に関する前記接触反力制約条件とを満たし得る点の集合領域に相当するものである。
【０１２０】
この場合、上記力学的関係は、ロボット１の全体の運動に関する運動方程式としての次式（８）により表される関係である。
【０１２１】
【数１】
2018001316.tif 000003
上記式（８）は、ロボット１に作用する全ての接触反力の合力（並進力及びモーメント）と、ロボット１に作用する重力と、ロボット１の運動によって発生する全体の慣性力（並進力及びモーメント）との間の釣合を表す式である。
【０１２２】
この場合、式（８）の第１行〜第３行の式は、並進力に関する釣合式、第４行〜第６行の式は、モーメント（グローバル座標系の原点周りのモーメント）に関する釣合式である。
【０１２３】
そして、↑ｐg、↑ｐg_dot2、↑Ｌg_dotの各成分が前記特定状態量として採用し得る状態量である。
【０１２４】
なお、各サンプリング時刻におけるロボット１の全体重心周りのイナーシャが判っている場合又は該イナーシャを推定し得る場合には、式（８ｅ）の↑Ｌg_dotの代わりに、Ｉ・↑ω_dot（ただし、Ｉ：イナーシャを表す行列又はスカラー、↑ω_dot：全体重心周りの角加速度（ベクトル））を用いることもできる。この場合、↑ω_dotの各成分を特定状態量として採用し得る。
【０１２５】
また、多面体を規定する前記接触反力制約条件は、各サンプリング時刻における各接触動作部（１３又は２３）毎に、第ｉ接触座標系で見た接触反力↑ｗ(i)に関する接触反力制約条件を、グローバル座標系で見た接触反力↑Ｗ(i)に関する接触反力制約条件に変換したものである。
【０１２６】
この場合、全ての接触動作部（第１〜第ＮＬ接触動作部）に関する接触反力制約条件の全体は、前記式（７ａ）又は（７ｂ）に基づいて、次式（９ａ）又は（９ｂ）の形式の行列線形不等式により表される。
【０１２７】
Ｃall・↑Ｗall≧↑ｋ    ……（９ａ）
Ｃall・↑Ｗall≦↑ｋ    ……（９ｂ）
ここで、Ｃallは、↑Ｗallの係数行列、↑ｋはその各成分が定数値となるベクトルである。そして、式（９ａ）又は（９ｂ）の各行が、各接触反力制約条件を表す個々の線形不等式である。この場合、係数行列Ｃallの行数（＝↑ｋの成分数）は、全ての接触動作部（第１〜第ＮＬ接触動作部）に関する接触反力制約条件を表す全ての線形不等式の総数である。
【０１２８】
例えば、各接触動作部（１３又は２３）毎に、第ｉ接触座標系で見た接触反力↑ｗ(i)に関する接触反力制約条件が、前記式（１ａ）〜（４）の不等式により表される場合、各接触動作部（１３又は２３）毎の不等式の総数が１１個（ここでは、式（１ａ），（１ｂ），（２），（３ａ），（３ｂ）は、それぞれ２つの不等式から成ると見なす）であるから、係数行列Ｃallの行数は、（１１・ＮＬ）個となる。
【０１２９】
上記の如く、多面体を規定する力学的関係及び接触反力制約条件は、式（８）と、式（９ａ）又は（９ｂ）とにより表される。そして、本実施形態では、多面体パラメータ決定部４１が決定する多面体パラメータ（各サンプリング時刻での多面体パラメータ）は、式（８）により表される力学的関係における係数行列Ｇallの各成分値と、式（９ａ）又は（９ｂ）により表される行列線形不等式の左辺の係数行列Ｃallの各成分値及び右辺のベクトル↑ｋの各成分値とから構成される。
【０１３０】
演算処理装置４０は、各サンプリング時刻での特定状態量の存在可能領域を、以下に説明する如く算出する。
【０１３１】
演算処理装置４０は、まず、多面体パラメータ決定部４１により上記多面体パラメータＧall，Ｃall，↑ｋを決定する（ひいては、各サンプリング時刻での多面体を確定する）。
【０１３２】
この場合、多面体パラメータ決定部４１は、各サンプリング時刻での接触動作部位置姿勢情報により示される第１〜第ＮＬ接触動作部のそれぞれの目標位置（グローバル座標系で見た目標位置）を、前記式（８ｄ）における位置ｐ(i)（ｉ＝１，２，…，ＮＬ）として用いて、式（８ｂ）〜（８ｄ）により示される定義に従って、式（８）の係数行列Ｇallを決定する。
【０１３３】
また、多面体パラメータ決定部４１は、各接触動作部（１３又は２３）毎の第ｉ接触座標系で見た接触反力↑ｗ(i)（ｉ＝１，２，…，ＮＬ）に関する接触反力制約条件を表す不等式（前記式（１ａ）〜（４）の不等式）から、前記式（７ａ）又は（７ｂ）に基づいて、グローバル座標系で見た接触反力↑Ｗ(i)に関する接触反力制約条件を表す個々の線形不等式を決定する。この場合、式（７ａ）又は（７ｂ）における行列Ｒ(i)（ｉ＝１，２，…，ＮＬ）は、各サンプリング時刻での接触動作部位置姿勢情報により示される第ｉ接触動作部の目標姿勢（グローバル座標系で見た目標姿勢）から決定される。
【０１３４】
そして、多面体パラメータ決定部４１は、これらの線形不等式から、式（９ａ）又は（９ｂ）における係数行列Ｃallと、ベクトル↑ｋとを決定する。
【０１３５】
演算処理装置４０は、以上の如く、各サンプリング時刻での多面体パラメータを決定した後、該多面体パラメータにより規定される多面体を、特定状態量指示情報により示される特定状態量を変数成分とする部分空間に射影してなる射影領域を求める多面体射影処理を多面体射影処理部４２により実行する。該部分空間は、多面体を表す多変数空間（本実施形態では、６次元よりも高次元の空間）の一部の変数成分により構成される空間であるから、該多変数空間よりも低次元の空間である。例えば、特定状態量が、ロボット１の全体重心の位置↑ｐgの３つの成分であるとした場合、上記部分空間は、３次元の空間となる。
【０１３６】
上記多面体射影処理は、概念的には、図４に示す如き処理である。図４では、Ｍ次元の多面体が、より低次元（ｍ次元）の空間に射影される様子を概念的に表している。本実施形態での上記多面体は、６次元よりも高次元（Ｍ次元）の多変数空間の多面体であるので、該多面体を視覚的に図示することができない。このため、図４では、便宜的に、多面体を３次元の多面体、多面体を射影してなる射影領域を２次元空間（平面）の領域として図示している。
【０１３７】
かかる多面体射影処理は、例えばMulti parametric linear programingという公知の処理（Ｖ-rep法の処理を含む）により行うことができる。あるいは、前記した処理１〜処理４、もしくは、処理１１〜１８を多面体射影処理として用いることもできる。なお、この多面体射影処理では、全体重心の位置↑ｐg、全体重心の並進加速度↑ｐg_dot2、及び全体重心周りの角運動量の時間的変化率↑Ｌg_dotのそれぞれの成分のうち、特定状態量以外の成分は、各サンプリング時刻での当該成分の値（目標値）としてあらかじめ決定された所定値とされる。
【０１３８】
かかる多面体射影処理により、多面体を特定状態量を変数成分として構成される部分空間に射影してなる射影領域が決定される。この射影領域は、前記式（８）により表される力学的関係と、前記式（９ａ）又は（９ｂ）により表される接触反力制約条件とを満たし得る特定状態量の存在許容領域を示すものとなる。従って、多面体射影処理部４２の処理によって、特定状態量の存在許容領域が求められることとなる。
【０１３９】
かかる多面体射影処理のより具体的な第１の例を図５、図６Ａ及び図６Ｂを参照して以下に説明する。この例は、例えば、特定状態量が全体重心の位置↑ｐgのＸ軸方向位置ｐgx及びＹ軸方向位置ｐgyの２つの状態量である場合の多面体射影処理として、前記した処理１〜処理４を実行する場合の例である。
【０１４０】
この場合、演算処理装置４０の多面体射影処理部４２は、図５のフローチャートに示す如く多面体射影処理を実行する。
【０１４１】
ＳＴＥＰ１において、多面体射影処理部４２は、前記式（８）により表される力学的関係と、前記式（９ａ）又は（９ｂ）により表される接触反力制約条件とを満たし得る全体重心のＸ軸方向位置ｐgxの最小値を、線形計画法の処理により求める。
【０１４２】
さらに、ＳＴＥＰ２において、多面体射影処理部４２は、前記式（８）により表される力学的関係と、前記式（９ａ）又は（９ｂ）により表される接触反力制約条件とを満たし得る全体重心のＸ軸方向位置ｐgxの符号を反転させてなる値（−ｐgx）の最小値を、線形計画法の処理により求める。
【０１４３】
なお、ＳＴＥＰ１，２における線形計画法の処理は、公知の一般的な処理でよい。また、ＳＴＥＰ１，２における線形計画法の処理では、全体重心のＸ軸方向位置ｐgx及びＹ軸方向位置ｐgy以外の特定状態量の値は、あらかじめ定めた値（例えばゼロ等）とされる。
【０１４４】
そして、ＳＴＥＰ３において、多面体射影処理部４２は、ＳＴＥＰ１で求めたｐgxの最小値を、ｐgxの下限値として決定すると共に、ＳＴＥＰ２で求めた（−ｐgx）の最小値の符号を反転させてなる値を、ｐgxの上限値として決定する。
【０１４５】
上記ＳＴＥＰ１〜３の処理は、前記した処理１に相当する。かかるＳＴＥＰ１〜３の処理により、前記式（８）により表される力学的関係と、前記式（９ａ）又は（９ｂ）により表される接触反力制約条件とを満たし得るｐgxの下限値及び上限値が求められる。
【０１４６】
次いで、多面体射影処理部４２は、ＳＴＥＰ４において、ｐgxの下限値と上限値との間の範囲に、該下限値及び上限値を含めてｎ個（複数）の代表値ｐgx(1)，ｐgx(2)，……，ｐgx(n)を設定する。かかるＳＴＥＰ４の処理が、前記処理２に相当する。
【０１４７】
この場合、ｎ個の代表値は、例えば、隣り合う代表値の間の差が一定値となるように設定される。また、ｎ個の代表値のうちの１つ、例えばｐgx(1)がｐgxの下限値に一致する値に設定され、他の１つ、例えばｐgx(n)がｐgxの上限値に一致する値に設定される。
【０１４８】
例えば、図６Ａに示す如く、ｐgxの下限値及び上限値が求められた場合において、ｐgxの複数（図示例では４個）の代表値ｐgx(1)〜ｐgx(4)が設定される。なお、図６Ａに二点鎖線で示す領域（ＸＹ座標平面上の領域）は、ｐgx，ｐgyの組の真の存在可能領域を示している。
【０１４９】
補足すると、ｐgxの下限値及び上限値以外の代表値は、例えば、該下限値と上限値との間の範囲内でランダムに設定してもよい。
【０１５０】
次いで、多面体射影処理部４２は、ＳＴＥＰ５において、ｐgxの代表値の番数パラメータｈの値を「１」に設定した後、ＳＴＥＰ６において、ｐgx＝ｐgx(h)であることを前提条件として、前記式（８）により表される力学的関係と、前記式（９ａ）又は（９ｂ）により表される接触反力制約条件とを満たし得る全体重心のＹ軸方向位置ｐgyの最小値を、線形計画法の処理により求める。
【０１５１】
さらに、多面体射影処理部４２は、ＳＴＥＰ７において、ｐgx＝ｐgx(h)であることを前提条件として、前記式（８）により表される力学的関係と、前記式（９ａ）又は（９ｂ）により表される接触反力制約条件とを満たし得る全体重心のＹ軸方向位置ｐgyの符号を反転させてなる値（−ｐgy）の最小値を、線形計画法の処理により求める。
【０１５２】
ＳＴＥＰ６，７における線形計画法の処理は、前記式（８）により表される力学的関係と、前記式（９ａ）又は（９ｂ）により表される接触反力制約条件との他に、ｐgx＝ｐgx(h)という条件をさらなる条件として付加した上で、前記ＳＴＥＰ１，２と同様に行い得る処理である。
【０１５３】
そして、ＳＴＥＰ８において、多面体射影処理部４２は、ＳＴＥＰ６で求めたｐgyの最小値を、ｐgx＝ｐgx(h)である場合におけるｐgyの下限値(h)として決定すると共に、ＳＴＥＰ７で求めた（−ｐgy）の最小値の符号を反転させてなる値を、ｐgx＝ｐgx(h)である場合におけるｐgyの上限値(h)として決定する。
【０１５４】
次いで、多面体射影処理部４２は、ＳＴＥＰ９において、番数パラメータｈの値を「１」だけ増加させた後、ＳＴＥＰ１０において、ｈ＞ｎであるか否か（ｐgxの全ての代表値に対してＳＴＥＰ６〜８の処理を実行したか否か）を判断する。
【０１５５】
そして、ＳＴＥＰ１０の判断結果が否定的である場合には、多面体射影処理部４２は、ＳＴＥＰ６〜８の処理を繰り返す。
【０１５６】
上記の如くＳＴＥＰ１０の判断結果が肯定的になるまで繰り返されるＳＴＥＰ６〜８の処理は、前記した処理３に相当する。かかるＳＴＥＰ６〜８の処理の繰り返しによって、ｐgxの各代表値毎に、前記式（８）により表される力学的関係と、前記式（９ａ）又は（９ｂ）により表される接触反力制約条件とを満たし得るｐgyの下限値及び上限値が求められる。
【０１５７】
例えば、図６Ａにおいて、ｐgxの代表値ｐgx(2)に対応して、ｐgyの下限値(2)と上限値(2)とが求められる。ｐgxの他の代表値ｐgx(1)，ｐgx(3)，ｐgx(4)についても同様である。なお、図６Ａに示す例では、ｐgx(4)に対応するｐgyの下限値(4)と上限値(4)とが一致している。
【０１５８】
ＳＴＥＰ１０の判断結果が肯定的になると、多面体射影処理部４２は、ＳＴＥＰ１１において、ｐgxの全ての代表値のそれぞれと、各代表値に対応するｐgyの下限値及び上限値とにより規定される複数の点（ＸＹ座標平面上の点）を凸包してなる領域を、ｐgx，ｐgyの組の存在可能領域（射影領域）として求める。
【０１５９】
かかるＳＴＥＰ１１の処理が前記処理４に相当する処理である。この処理によって、例えば、図６Ｂに実線で示す領域がｐgx，ｐgyの組の存在可能領域（射影領域）として求められる。該存在可能領域は、真の存在可能領域を近似的に表す領域である。
【０１６０】
以上、例示した多面体射影処理（前記処理１〜４の処理に基づく多面体射影処理）によれば、特定状態量としてのｐgx，ｐgyの組の存在可能領域を短時間で高速に求めることができる。
【０１６１】
次に、多面体射影処理の第２の例を図７、図８Ａ及び図８Ｂを参照して以下に説明する。この例は、例えば、特定状態量が全体重心の位置↑ｐgの３成分ｐgx，ｐgy，ｐgzである場合の多面体射影処理として、前記した処理１１〜処理１８（ただし、処理１６，１７を除く）を実行する場合の例である。
【０１６２】
この場合、演算処理装置４０の多面体射影処理部４２は、図７のフローチャートに示す如く多面体射影処理を実行する。
【０１６３】
多面体射影処理部４２は、まず、前記した図５のＳＴＥＰ１〜１０の処理を同じ処理を実行する。この場合、ＳＴＥＰ１〜３の処理は、前記した処理１１に相当し、ＳＴＥＰ４の処理は、前記した処理１２に相当する。また、ＳＴＥＰ１０の判断結果が肯定的になるまで繰り返されるＳＴＥＰ６〜８の処理が、前記した処理１３に相当する。
【０１６４】
次いで、ＳＴＥＰ１２において、多面体射影処理部４２は、ｐgxの全ての代表値のそれぞれと、各代表値に対応するｐgyの下限値及び上限値とにより規定される複数の点（ｐgx、ｐgyを座標軸成分とする２次元平面上の点）を凸包してなる領域（２次元凸包領域）内に、複数の（ｍ個の）代表点（ｐgx_h，ｐgy_h）（ｈ＝１，２，…，ｍ）を設定する。
【０１６５】
この場合、図８Ａ又は図８Ｂに例示する如く、ｍ個の代表点は、上記２次元凸包領域の各頂点を含むように設定される。そして、上記２次元凸包領域の各頂点以外の代表点は、任意に設定し得る。
【０１６６】
例えば、図８Ａに示すように、ｐgxの各代表値（前記ＳＴＥＰ４で設定した各代表値ｐgx(1)，ｐgx(2)，…，ｐgx(n)。図示例では、ｎ＝４）に対応するｐgyの下限値及び上限値の間に、該下限値及び上限値を含めてｐgyの複数の代表値を設定する。そして、ｐgxの各代表値と、それに対応して設定したｐgyの各代表値との組により規定される点を上記代表点として設定し得る。
【０１６７】
なお、図８Ａに示す例では、ｐgx＝ｐgx(4)であるときのｐgyの上限値及び下限値が一致しているので、ｐgx＝ｐgx(4)となる代表点は１つ（２次元凸包領域の頂点）だけとなっている。
【０１６８】
補足すると、上記の如く、ｐgxの値がＳＴＥＰ４で設定した各代表値ｐgx(1)，ｐgx(2)，…，ｐgx(n)のいずれかに一致するように複数の代表点を設定した場合には、各代表点が結果的に上記２次元凸包領域に含まれることとなる。このため、該２次元凸包領域そのものを求めておく必要はない。
【０１６９】
あるいは、例えば図８Ｂに示すように、上記２次元凸包領域を求めた上で、該２次元凸包領域の内部に任意に（例えばランダムに）該２次元凸包領域の頂点以外の各代表点を設定することも可能である。この場合には、２次元凸包領域の頂点以外の各代表点でのｐgxの値は、前記ＳＴＥＰ４で設定した各代表値ｐgx(1)，ｐgx(2)，…，ｐgx(n)と異なっていてもよい。
【０１７０】
上記の如くＳＴＥＰ１２では、２次元凸包領域に複数の代表点が設定される。このＳＴＥＰ１２の処理は、前記した処理１４に相当する。
【０１７１】
多面体射影処理部４２は、次にＳＴＥＰ１３において、番数パラメータｈの値を「１」に設定した後、ＳＴＥＰ１４において、（ｐgx，ｐgy）＝（ｐgx_h，ｐgy_h)であることを前提条件として、前記式（８）により表される力学的関係と、前記式（９ａ）又は（９ｂ）により表される接触反力制約条件とを満たし得る全体重心のＺ軸方向位置ｐgzの最小値を、線形計画法の処理により求める。
【０１７２】
さらに、多面体射影処理部４２は、ＳＴＥＰ１５において、（ｐgx，ｐgy）＝（ｐgx_h，ｐgy_h)であることを前提条件として、前記式（８）により表される力学的関係と、前記式（９ａ）又は（９ｂ）により表される接触反力制約条件とを満たし得る全体重心のＺ軸方向位置ｐgzの符号を反転させてなる値（−ｐgz）の最小値を、線形計画法の処理により求める。
【０１７３】
なお、ＳＴＥＰ１４，１５における線形計画法の処理では、全体重心の各成分ｐgx，ｐgy，ｐgz以外の特定状態量の値は、あらかじめ定めた値（例えばゼロ等）とされる。
【０１７４】
そして、ＳＴＥＰ１６において、多面体射影処理部４２は、ＳＴＥＰ１４で求めたｐgzの最小値を、ｐgzの下限値として決定すると共に、ＳＴＥＰ１５で求めた（−ｐgz）の最小値の符号を反転させてなる値を、ｐgzの上限値として決定する。
【０１７５】
次いで、多面体射影処理部４２は、ＳＴＥＰ１７において、番数パラメータｈの値を「１」だけ増加させた後、ＳＴＥＰ１８において、ｈ＞ｍであるか否か（（ｐgx，ｐgy）の全ての代表点に対してＳＴＥＰ１４〜１６の処理を実行したか否か）を判断する。
【０１７６】
そして、ＳＴＥＰ１８の判断結果が否定的である場合には、多面体射影処理部４２は、ＳＴＥＰ１４〜１６の処理を繰り返す。
【０１７７】
上記の如くＳＴＥＰ１８の判断結果が肯定的になるまで繰り返されるＳＴＥＰ１４〜１６の処理は、前記した処理１５に相当する。かかるＳＴＥＰ１４〜１６の処理の繰り返しによって、（ｐgx，ｐgy）の各代表点毎に、前記式（８）により表される力学的関係と、前記式（９ａ）又は（９ｂ）により表される接触反力制約条件とを満たし得るｐgzの下限値及び上限値が求められる。
【０１７８】
次いで、ＳＴＥＰ１８の判断結果が肯定的になると、多面体射影処理部４２は、ＳＴＥＰ１９において、（ｐgx，ｐgy）の全ての代表点のそれぞれと、各代表点に対応するｐgzの下限値及び上限値とにより規定される複数の点（ｐgx，ｐgy，ｐgzを各座標軸成分とする３次元空間上の点）を凸包してなる領域（３次元凸包領域）を、ｐgx，ｐgy，ｐgzの組の存在可能領域（射影領域）として求める。
【０１７９】
かかるＳＴＥＰ１９の処理が前記処理１８に相当する処理である。これにより、前記処理１１〜１８に基づく多面体射影処理によって、ｐgx，ｐgy，ｐgzの組の存在可能領域（射影領域）を求めることができる。
【０１８０】
なお、図７では、特定状態量が、３個の状態量（ｐgx，ｐgy，ｐgz）である場合の処理を説明したが、特定状態量が４個以上である場合にも、前記処理１１〜１８に基づく多面体処理により該特定状態量の存在可能領域を求めることができる。
【０１８１】
例えば特定状態量が、ｐgx，ｐgy，ｐgzを含む４個の状態量（以降、４個目の状態量を便宜上、参照符号Ｓで表す）である場合には、ＳＴＥＰ１８の判断結果が肯定的となった後に、前記した処理１６，１７が実行される。
【０１８２】
具体的には、（ｐgx，ｐgy）の全ての代表点のそれぞれと、各代表点に対応するｐgzの下限値及び上限値とにより規定される複数の点（ｐgx，ｐgy，ｐgzを各座標軸成分とする３次元空間上の点）を凸包してなる３次元凸包領域内に複数の代表点（当該３次元凸包領域の各頂点を含む）が設定される。これらの代表点の設定は、前記ＳＴＥＰ１２と同様の手法で行い得る。
【０１８３】
そして、当該各代表点毎に、（ｐgx，ｐgy，ｐgz）が、該代表点での値に一致することを前提条件として、前記式（８）により表される力学的関係と、前記式（９ａ）又は（９ｂ）により表される接触反力制約条件とを満たし得る状態量Ｓの下限値及び上限値が求められる。この処理は、前記ＳＴＥＰ１４〜１６の処理を同様に、線形計画法の処理を用いて行うことができる。
【０１８４】
次いで、（ｐgx，ｐgy，ｐgz）の全ての代表点のそれぞれと、各代表点に対応する状態量Ｓの下限値及び上限値とにより規定される複数の点（ｐgx，ｐgy，ｐgz，Ｓを各座標軸成分とする４次元空間上の点）を凸包してなる領域（４次元凸包領域）が、ｐgx，ｐgy，ｐgz，Ｓの組の存在可能領域（射影領域）として求められる。
【０１８５】
以上の如く線形計画法の処理を複数回繰り返す多面体射影処理（前記処理１〜４の処理に基づく多面体射影処理、又は前記処理１１〜１８の処理に基づく多面体射影処理）によれば、特に、特定状態量の成分数が比較的少ない場合に、該特定状態量の存在可能領域を短時間で高速に求めることができる。
【０１８６】
以上説明した演算処理装置４０の処理により求められる特定状態量の存在可能領域の例を図９Ｂ、図１０Ｂ、図１１Ｂにそれぞれ斜線領域として示す。これらの例では、特定状態量は、ロボット１に梯子を昇降させる場合における１つのサンプリング時刻における全体重心の位置↑ＰgのうちのＸ軸方向位置及びＹ軸方向位置の２つの成分である。従って、図９Ｂ、図１０Ｂ、図１１Ｂにそれぞれ示す特定状態量の存在可能領域は、ロボット１の全体重心を水平面（ＸＹ座標平面）に投影してなる点の存在可能領域（以降、重心存在可能領域という）である。
【０１８７】
そして、図９Ｂ、図１０Ｂ、図１１Ｂにそれぞれ対応する図９Ａ、図１０Ａ、図１１Ａは、ロボット１のどの接触動作部（１３又は２３）が梯子に接触した状態であるかを示している。
【０１８８】
この場合、図９Ａは、所謂、クロールの形態で、ロボット１に梯子を昇降させる場合の１つのサンプリング時刻において、ロボット１の右側及び左側のハンド部２３Ｒ，２３Ｌと、右側の足部１３Ｒとの３つの接触動作部２３Ｒ，２３Ｌ，１３Ｒを、梯子の踏ざんに接触させた状態を示している。この状態に対応して、演算処理装置４０により求められた重心存在可能領域が図９Ｂに示す斜線領域である。
【０１８９】
また、図１０Ａは、所謂、ペースの形態で、ロボット１に梯子を昇降させる場合の１つのサンプリング時刻において、ロボット１の右側ハンド部２３Ｒと、右側の足部１３Ｒとの２つの接触動作部２３Ｒ，１３Ｒを、梯子の踏ざんに接触させた状態を示している。この状態に対応して、演算処理装置４０により求められた重心存在可能領域が図１０Ｂに示す斜線領域である。
【０１９０】
また、図１１Ａは、所謂、トロットの形態で、ロボット１に梯子を昇降させる場合の１つのサンプリング時刻において、ロボット１の左側ハンド部２３Ｌと、右側の足部１３Ｒとの２つの接触動作部２３Ｌ，１３Ｒを、梯子の踏ざんに接触させた状態を示している。この状態に対応して、演算処理装置４０により求められた重心存在可能領域が図１１Ｂに示す斜線領域である。
【０１９１】
なお、図９Ｂ、図１０Ｂ、図１１Ｂのいずれの場合においても、演算処理装置４０の多面体射影処理部４２の処理は、ロボット１の全体重心の並進加速度↑ｐg_dot2と、全体重心周りの角運動量の時間的変化率↑Ｌg_dotとをいずれもゼロとして（換言すれば、図９Ａ、図１０Ａ、図１１Ａのそれぞれのサンプリング時刻でのロボット１が静止状態であるとみなして）、実行した。
【０１９２】
上記の如く求められる重心存在可能領域を用いることで、例えば、梯子の昇降をロボット１に行わせるべく適宜作成したロボット１の目標運動が適切なものであるか否かを評価することができる。すなわち、作成したロボット１の目標運動により規定される全体重心の位置が、重心存在可能領域内に収まるか否かによって、該目標運動が適切なものであるか否かを評価することができる。
【０１９３】
補足すると、上記の如く重心存在可能領域を求める場合において、例えば、ロボット１の目標運動の各サンプリング時刻での全体重心の並進加速度↑Ｐg_dot2（又は全体重心の並進運動量の時間的変化率）と、全体重心周り角運動量の時間的変化率↑Ｌg_dot（又は全体重心周りの角加速度）とをあらかじめ決定しておき、それらの既定値を用いて、多面体射影処理を実行することで、重心存在可能領域を求めるようにしてもよい。この場合には、ロボット１が動的に発生する慣性力の影響を反映させて、重心存在可能領域を求めることができる。
【０１９４】
さらに、この場合、全体重心の位置↑ＰgのＺ軸方向位置（高さ）を含めた空間的な位置の存在可能領域を求めることもできる。
【０１９５】
なお、全体重心の並進加速度↑Ｐg_dot2（又は全体重心の並進運動量の時間的変化率）の上記既定値は、あらかじめ決定した全体重心の並進移動速度又は並進運動量の経時変化のパターンから微分演算により算出される値であってもよい。同様に、全体重心周り角運動量の時間的変化率↑Ｌg_dot又は全体重心周りの角加速度は、あらかじめ決定した全体重心周りの角運動量又は角速度の経時変化のパターンから微分演算により算出される値であってもよい。
【０１９６】
次に、前記した如く求められる特定状態量の存在可能領域、例えば前記重心存在可能領域を利用して、ロボット１の実際の運動を行いながら、ロボット１の目標運動（現在時刻以後の将来の目標運動）を生成する手法の一例を説明する。
【０１９７】
かかる手法としては、例えば、次式（１０ｃ），（１０ｄ）の条件を満たす範囲で、次式（１０ａ），（１０ｂ）により定義される評価関数ＦＥの値を最小化するように、ロボット１の各関節（要素関節）の変位量を逐次決定する手法を採用することができる。
【０１９８】
ＦＥ＝↑ｑ_dotT・Ｗt1・↑ｑ_dot
＋Σ↑ｄＶ(j)T・Ｗt2(j)・↑ｄＶ(j)    ……（１０ａ）
ただし、↑ｄＶ(j)＝Ｊ(j)・↑ｑ_dot−↑ｘ_dot(j)    ……（１０ｂ）
ξ・(↑θmax−↑θ0)≧↑θ_dot・dt≧ξ・(↑θmin−↑θ0)    ……（１０ｃ）
Ｊcol・↑ｑ_dot≧↑ｘc_dot    ……（１０ｄ）
ここで、式（１０ａ），（１０ｂ）における↑ｑ_dotは、グローバル座標系で見たロボット１の基体２の並進速度及び回転角速度（それぞれ３成分のベクトル）と、ロボット１の各要素関節の変位速度（単位時間当たりの変位量）とを並べた一般化変数ベクトル、Ｗt1,Ｗt2(j)は重み行列、Ｊ(j)はロボット１の各接触動作部（１３又は２３）と全体重心とのそれぞれに対応するヤコビアン、↑ｘ_dot(j)は各接触動作部（１３又は２３）と全体重心とのそれぞれの目標速度（並進速度の目標値と回転角速度の目標値とからなるベクトル）である。
【０１９９】
この場合、各目標速度は、例えば、各接触動作部（１３又は２３）の実際の位置及び姿勢、基体２の実際の姿勢、各要素関節の実際の変位量、及び全体重心の実際の位置のそれぞれの計測値又は推定値を、それぞれの目標値に比例制御則等のフィードバック制御則により追従させるように算出される。これらの目標値のうち、全体重心の位置の目標値は、例えば、前記重心存在可能領域の中心点（重心点）に設定される。
【０２００】
また、式（１０ｃ）における↑θmaxは、各要素関節の上限変位量を並べたベクトル、↑θminは、各要素関節の下限変位量を並べたベクトル、↑θ0は新たに生成しようとする目標運動における各関節要素の変位量の初期値、↑θ_dotは、各関節要素の変位速度を並べたベクトル、dtは目標運動の生成の刻み時間、ξは所定値の係数である。
【０２０１】
この式（１０ｃ）は、目標運動の生成処理により決定される各要素関節の目標変位量を、上限変位量と下限変位量との間の範囲内に制限するための制約条件を示す不等式である。
【０２０２】
また、式（１０ｄ）におけるＪcolは、ロボット１の自己干渉を生じ得る部位、又は外界物との干渉を生じ得る部位に対応するヤコビアン、↑ｘc_dotは当該部位の所定の速度制限値（並進速度及び回転角速度の制限値）である。この式（１０ｄ）は、当該部位の干渉を防止するための制約条件を示す不等式である。
【０２０３】
そして、上記式（１０ｃ），（１０ｄ）の条件を満たす範囲で、式（１０ａ），（１０ｂ）により定義される評価関数ＦＥの値を最小化するように、ロボット１の各関節（要素関節）の変位量を逐次決定することは、２次計画問題の公知の解法手法により行うことができる。
【０２０４】
かかる手法により目標運動を生成することで、目標運動でのロボット１の全体重心の位置が重心存在可能領域内に収まるように生成することができる。
【０２０５】
ここで、図１２Ｂは、上記手法によりロボット１の目標運動を生成した場合におけるロボット１の全体重心の目標位置（ＸＹ座標平面上での位置）がどのように遷移していくかを例示する図である。そして、図１２Ａは、図１２Ｂに対応するロボット１の動作形態を示す図である。
【０２０６】
この例でのロボット１の目標運動は、図１２Ａに示すように、ロボット１を基台上から、梯子に乗り移らせる場合の目標運動である。この目標運動では、ロボット１は、両方のハンド部２３Ｒ，２３Ｌを梯子の踏ざんに支持した状態で、まず、右側及び左側の足部１３Ｒ，１３Ｌのうちの左側の足部１３Ｌを、基台上から梯子の踏ざんに移動させる。次いで、ロボット１は、右側の足部１３Ｒを、基台上から梯子の踏ざんに移動させる。
【０２０７】
また、図１２Ｂにおいて、斜線領域ＡＲ１は、ロボット１がその両方の足部１３Ｒ，１３Ｌを基台上に接地させた状態での重心存在可能領域、斜線領域ＡＲ２は、ロボット１がその両方の足部１３Ｒ，１３Ｌを梯子の踏ざんに接地させた状態での重心存在可能領域である。そして、黒点の点列が、目標運動でのロボット１の全体重心の遷移を示している。ｋなお、白点Ｐ１は重心存在可能領域ＡＲ１の重心点、白点Ｐ２は重心存在可能領域ＡＲ２の重心点である。
【０２０８】
図１２Ｂに示されるように、ロボット１の全体重心が、重心存在可能領域ＡＲ１，ＡＲ２内に収まって遷移していくように、ロボット１の目標運動を生成し得ることが判る。
【０２０９】
なお、以上説明した実施形態では、特定状態量の存在可能領域として、重心存在可能領域を求める場合を主要例として説明した。ただし、特定状態量は、ロボット１の全体重心の位置↑Ｐgに限られない。例えば、存在可能領域を求める特定状態量は、例えば、ロボット１の全体重心の並進加速度↑Ｐg_dot2又は並進運動量の時間的変化率の１つ以上の成分、あるいは、全体重心周りの角運動量の時間的変化率↑Ｌg_dot又は全体重心周りの角加速度の１つ以上の成分であってもよい。
【０２１０】
さらには、特定状態量は、ロボット１の全体重心の位置↑Ｐgの１つ以上の成分と、全体重心の並進加速度↑Ｐg_dot2又は並進運動量の時間的変化率の１つ以上の成分と、全体重心周りの角運動量の時間的変化率↑Ｌg_dot又は全体重心周りの角加速度の１つ以上の成分とを組み合わせたものであってもよい。
【０２１１】
また、多面体を規定する接触反力制約条件は、前記並進摩擦力制約条件、回転摩擦力制約条件、ＣＯＰ制約条件、抗力制約条件、あるいは、各関節で発生可能な駆動力に応じた制約条件のうちの一部の制約条件だけであってもよく、あるいは、これらの制約条件以外の制約条件を含んでいてもよい。
【０２１２】
また、前記実施形態では、各接触動作部（１３又は２３）の接触反力（接触レンチ）を、並進力（３次元ベクトル）とモーメント（３次元ベクトル）との組（６成分のベクトル）とした。ただし、該接触反力として、並進力だけを採用してもよい。
【０２１３】
また、図５又は図７を参照して説明した多面体処理（線形計画法の処理を繰り返す多面体処理）は、前記凸多面体が有界領域（閉領域）である場合の処理であるが、本発明は、前記凸多面体が有界でない場合にも適用可能である。この場合には、多面体射影処理として、Multi parametric linear programingの処理を用いることができる。
【０２１４】
なお、個々の移動ロボットにおいて、各接触動作部に作用する接触反力の各成分の値は、無限大の値をとり得ないので、ある有限範囲内の値となる。そして、例えば、接触反力の各成分の値の範囲を当該有限範囲内に制限する制約条件を接触反力制約条件に含めておくことで、前記凸多面体を有界領域（閉領域）にすることができる。ひいては、多面体射影処理として、線形計画法の処理を繰り返す処理を用いることができる。
【０２１５】
また、本発明を適用し得る移動ロボットは、前記ロボット１に限らず、外界物に接触させる部分を有するリンクを、５つ以上備えるロボットであってもよい。また、本発明は、人型に移動ロボットに限らず、種々様々な形態の移動ロボットに適用できる。
【符号の説明】
【０２１６】
１…移動ロボット、１３，１３Ｒ，１３Ｌ…足部（接触動作部）、２３，２３Ｒ，２３Ｌ…ハンド部（接触動作部）、４０…演算処理装置（存在可能領域推定装置）、４２…多面体射影処理部。
【図１】
2018001316.tif 000004
【図２】
2018001316.tif 000005
【図３】
2018001316.tif 000006
【図４】
2018001316.tif 000007
【図５】
2018001316.tif 000008
【図６】
2018001316.tif 000009
【図７】
2018001316.tif 000010
【図８】
2018001316.tif 000011
【図９】
2018001316.tif 000012
【図１０】
2018001316.tif 000013
【図１１】
2018001316.tif 000014
【図１２】
2018001316.tif 000015