脚式移動ロボットの歩容生成装置
【課題】複数の接触対象面からロボットに外力が作用する状況でのロボットの目標歩容を生成する場合に、ロボットに作用させるべき外力(並進力)の目標値を適切に決定する。
【解決手段】ロボット1の動作環境の複数の接触対象面FL,WL1,WL2を相互の姿勢関係が予め定められた仮想面S3a,S2a,S2bで近似し、これらの仮想面S3a,S2a,S2bからロボット1に作用させるべき並進力(仮想面必要並進力)を、その垂直抗力成分及び摩擦力成分に関する第1A必要条件、第1B必要条件、第2必要条件を少なくとも満足するように決定する。決定した仮想面必要並進力に基づいて、各仮想面に対応する接触対象面からロボット1に作用させるべき目標並進力を決定する。
【解決手段】ロボット1の動作環境の複数の接触対象面FL,WL1,WL2を相互の姿勢関係が予め定められた仮想面S3a,S2a,S2bで近似し、これらの仮想面S3a,S2a,S2bからロボット1に作用させるべき並進力(仮想面必要並進力)を、その垂直抗力成分及び摩擦力成分に関する第1A必要条件、第1B必要条件、第2必要条件を少なくとも満足するように決定する。決定した仮想面必要並進力に基づいて、各仮想面に対応する接触対象面からロボット1に作用させるべき目標並進力を決定する。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、脚式移動ロボットの目標歩容を生成する装置に関する。
【背景技術】
【0002】
脚式移動ロボット(以下、単にロボットということがある)の目標歩容を生成する技術としては、例えば、本願出願人が提案した特許文献1、2に見られる技術等が従来より知られている。これらの技術は、2足歩行ロボット等の脚式移動ロボットを床上で歩行動作や走行動作によって移動させるような目標歩容を生成する技術である。その目標歩容は、ロボットの基体、脚リンク等の各部の運動(位置及び姿勢の時系列パターン)を規定する目標運動と、ロボットに床から作用させる床反力やその作用点(全床反力中心点)を規定する目標床反力とから構成される。
【0003】
なお、ロボットの実際の動作制御を行う場合には、一般に、ロボットの実際の運動を目標運動に制御するだけでは、ロボットの実際の動作環境(床形状等)と目標歩容で想定されていた動作環境との誤差等に起因して、目標運動を実現するために必要な外力をロボットに作用させることができない状況がしばしば発生する。そして、このような状況では、ロボットの運動の制御だけでなく、ロボットに外界(ロボットの動作環境)から作用させる外力の制御も必要となる。このため、ロボットの目標歩容には、一般に、ロボットの目標運動に加えて、上記目標床反力の如き、外力に関する目標も必要となる。
【先行技術文献】
【特許文献】
【0004】
【特許文献1】特許第3674788号公報
【特許文献2】特許第3679105号公報
【発明の概要】
【発明が解決しようとする課題】
【0005】
ロボットに種々様々な作業を行なわせるためには、ロボットを単に床上で移動させるだけでなく、ロボットの外界に存する複数の接触対象面にロボットの複数の部位を接触させながら(例えば、ロボットの脚の先端部と腕の先端部とをそれぞれ床面と壁面とに接触させながら)、ロボットの所要の運動を行なわせることが要求される場合もある。
【0006】
このような場合には、ロボットを単に床上で移動させる場合と異なり、ロボットの外界の複数の接触対象面からロボットの複数の部位に外力が作用することとなる。
【0007】
従って、このような場合には、ロボットの適切な動作制御を行うために、ロボットの目標運動に加えて、複数の接触対象面のそれぞれから作用させるべき外力に関する目標(目標外力や、その外力の作用点の目標位置)を設定しておく必要がある。
【0008】
ところが、上記のように複数の接触対象面からロボットに外力が作用する状況でのロボットの目標歩容を生成する場合には、ロボットの目標運動を、要求される運動形態に即して生成することは可能であるものの、その目標運動を実現するために、複数の接触対象面のそれぞれから動力学的にロボットに作用させるべき外力の時系列や組み合わせのパターンは、一般には1つのパターン又は限定的な範囲内のパターンに定まるものではなく、幅広い範囲で種々様々なパターンが存在し得る。
【0009】
従って、複数の接触対象面からロボットに外力が作用する状況でのロボットの目標歩容を生成する場合に、複数の接触対象面のそれぞれからロボットに作用させるべき外力に関する適切な目標をどのような指針で決定すべきかが問題となる。
【0010】
しかるに、従来は、上記の如く複数の接触対象面からロボットに外力が作用する状況での目標歩容を生成する技術については、十分な検討がなされていないのが実状である。従って、このような目標歩容を適切に生成し得る技術の構築が望まれていた。
【0011】
本発明はかかる背景に鑑みてなされたものであり、複数の接触対象面からロボットに外力が作用する状況でのロボットの目標歩容を生成する場合に、ロボットの目標運動を実現するためにロボットに作用させるべき外力に関する目標(特に並進力の目標値)を適切に決定することができる歩容生成装置を提供することを目的とする。
【課題を解決するための手段】
【0012】
本発明の脚式移動ロボットの歩容生成装置は、上記の目的を達成するために、脚式移動ロボットの動作環境に存する互いに異なる複数の接触対象面に該ロボットの複数の部位を接触させながら、該ロボットの運動を行なわせるための該ロボットの目標運動と、該ロボットに作用させるべき外力に関する目標とを含む目標歩容を生成する歩容生成装置であって、
前記ロボットの目標運動を生成する目標運動生成手段と、
相互の姿勢関係があらかじめ定められた複数の仮想面から成る仮想面群を用い、該仮想面群の複数の仮想面が前記複数の接触対象面に一致又は近似する面になるように、該仮想面群を前記ロボットの動作環境の空間に仮想的に設定する仮想面群設定手段と、
前記生成された目標運動に基づいて、該目標運動を実現するために前記ロボットに作用させるべきトータルの外力のうちの並進力成分である必要総並進外力を決定する必要総並進外力決定手段と、
前記決定された必要総並進外力を実現するために前記仮想面群の複数の仮想面のそれぞれから前記ロボットに作用させるべき並進力である仮想面必要並進力を決定する手段であり、前記複数の仮想面のそれぞれにおける仮想面必要並進力のうちの該仮想面に垂直な成分である仮想面垂直抗力成分の合力が、前記必要総並進外力のうちの、前記仮想面群の全ての仮想面に対して平行な成分を除く成分に一致するという第1A必要条件と、前記複数の仮想面のそれぞれにおける仮想面必要並進力のうちの該仮想面に平行な成分である仮想面摩擦力成分の合力が、前記必要総並進外力のうちの、前記仮想面群の全ての仮想面に対して平行な成分に一致するという第1B必要条件と、各仮想面における仮想面摩擦力成分の大きさが、少なくとも該仮想面における仮想面垂直抗力成分に応じて設定された第1所定値以下になるという第2必要条件とを少なくとも満足するように、各仮想面における仮想面必要並進力を決定する仮想面必要並進力決定手段と、
少なくとも前記各仮想面に対応して決定された前記仮想面必要並進力に基づいて、各仮想面に対応する接触対象面から前記ロボットに作用させるべき目標並進力を決定し、その決定した目標並進力を前記目標歩容の構成要素として出力する接触対象面目標並進力決定手段とを備えることを特徴とする(第1発明)。
【0013】
なお、本発明(後述の第2発明、第3発明を含む)において、前記ロボットの動作環境に存する互いに異なる複数の接触対象面というのは、その複数の接触対象面に含まれる任意の2つの接触対象面(又はその2つの接触対象面をそれぞれ包含する平面)が互いに交差するか、又は、互いに間隔を存して平行もしくはほぼ平行に対向するような複数の接触対象面を意味する。この場合、各接触対象面は、それに接触させるロボットの部位に比して広い面積を有する面(例えば、通常の床面の如き面)である必要はなく、例えば、該部位と同程度又はそれよりも小さい面積を有する局所的な面であってもよい。
【0014】
また、前記目標歩容の目標運動において、各接触対象面に接触させるロボットの部位は、1つの部位である必要はなく、2つ以上の部位であってもよい。この場合において、ある接触対象面に接触させるロボットの部位が2つ以上である場合には、該接触対象面からロボットに作用させるべき前記目標並進力は、該接触対象面からロボットの当該2つ以上の部位のそれぞれに作用させる並進力の合力の目標値を意味する。
【0015】
前記第1発明によれば、前記複数の接触対象面のそれぞれからロボットに作用させるべき外力に関する目標(目標歩容の構成要素)としての前記目標並進力を決定するために、相互の姿勢関係があらかじめ定められた複数の仮想面から成る仮想面群が、前記複数の接触対象面の代用的な面として使用される。そして、この仮想面群は、前記仮想面群決定手段によって、該仮想面群の複数の仮想面が前記複数の接触対象面に一致又は近似する面になるように、前記ロボットの動作環境の空間に仮想的に設定される。
【0016】
なお、前記仮想面群設定手段の処理において、前記仮想面群をロボットの動作環境の空間に仮想的に設定するということは、該仮想面群を構成する各仮想面の、当該空間での配置を決定すること、より詳しくは、当該空間での各仮想面の位置と姿勢(空間的な向き)とを決定することを意味する。
【0017】
また、前記目標運動生成手段によって生成された目標運動を実現するために前記ロボットに作用させるべきトータルの外力のうちの並進力成分である必要総並進外力が前記必要総並進外力決定手段によって決定される。
【0018】
ここで、前記必要総並進外力は、換言すれば、前記目標運動によって発生するロボット全体の並進慣性力(慣性力の並進力成分)と、該ロボット全体に作用する重力との合力に釣り合う並進力である。この場合、ロボットの目標運動によって、ロボットの各部(各リンク)の運動とロボットの全体重心点の運動とが規定されることとなるので、ロボットの適当な幾何学モデル(剛体リンクモデル)を用いることで、ロボット全体の上記並進慣性力を前記目標運動に基づいて決定できる。また、ロボット全体に作用する重力は、ロボットの全体質量と重力加速度との積である。従って、前記目標運動に基づいて、前記必要総並進外力を決定できる。
【0019】
なお、本発明では、前記目標運動は、例えば、ロボットに要求される運動形態(ロボットの脚リンクの先端部等の動かし方や、接触対象面への接触のさせ方等)に即して生成すればよい。
【0020】
そして、第1発明では、前記必要総並進外力決定手段によって決定された必要総並進外力を実現するために前記仮想面群の複数の仮想面のそれぞれから前記ロボットに作用させるべき並進力である仮想面必要並進力が前記仮想面必要並進力決定手段によって決定される。なお、各仮想面は、各接触対象面に対応しているので、各仮想面における仮想面必要並進力は、該仮想面に対応する接触対象面からロボットに作用させるべき並進力に対応するものである。
【0021】
ここで、該仮想面必要並進力決定手段が決定する、各仮想面における仮想面必要並進力は、換言すれば、それらの合力が、前記必要総並進外力に一致するような並進力を意味する。そして、各仮想面における仮想面必要並進力は、該仮想面に垂直な成分である前記仮想面垂直抗力成分と該仮想面に平行な成分である前記仮想面摩擦力成分との一方又は両方から構成される。
【0022】
この場合、本発明では、前記複数の接触対象面からロボットに外力を作用させることに対応して、前記仮想面群の複数の仮想面からロボットに外力を作用させることとなるので、前記必要総並進外力を実現するための各仮想面における仮想面必要並進力の組合せは、一般には、無数の態様を採り得る。
【0023】
但し、各仮想面に対応する接触対象面からロボットに作用させ得る並進力のうちの摩擦力成分(接触対象面に平行な成分)の大きさは、その上限値が該接触対象面における垂直抗力成分(接触対象面に垂直な成分)の影響を受け、該垂直抗力成分の大きさが小さいほど、摩擦力成分の大きさの上限値も小さくなる。また、該摩擦力成分が大きいと、接触対象面とロボットとの間の滑りも発生し易くなる。これに対して、接触対象面における垂直抗力成分は、摩擦力成分に比して幅広い範囲で、実現し得る。
【0024】
従って、各接触対象面に対応する仮想面における仮想面必要並進力は、できるだけ、仮想面摩擦力成分を含まないか、もしくは、該仮想面摩擦力成分の大きさが小さくなるように、決定することが望ましいと考えられる。換言すれば、前記必要総並進外力が、できるだけ、各仮想面における仮想面垂直抗力成分の合力によって実現され得るように、仮想面必要並進力を決定することが望ましいと考えられる。そして、前記必要総並進外力を実現するために、各仮想面における仮想面摩擦力成分が必要となる場合には、その仮想面摩擦力成分の大きさは、該仮想面における仮想面垂直抗力成分の大きさに応じて設定される、ある所定値以下になるように決定することが望ましい。
【0025】
そこで、第1発明では、前記仮想面必要並進力決定手段は、前記複数の仮想面のそれぞれにおける仮想面必要並進力のうちの仮想面垂直抗力成分の合力が、前記必要総並進外力のうちの、前記仮想面群の全ての仮想面に対して平行な成分を除く成分に一致するという第1A必要条件と、前記複数の仮想面のそれぞれにおける仮想面必要並進力のうちの仮想面摩擦力成分の合力が、前記必要総並進外力のうちの、前記仮想面群の全ての仮想面に対して平行な成分に一致するという第1B必要条件と、各仮想面における仮想面摩擦力成分の大きさが、少なくとも該仮想面における仮想面垂直抗力成分に応じて設定された第1所定値以下になるという第2必要条件とを少なくとも満足するように、各仮想面における仮想面必要並進力を決定する。
【0026】
なお、前記第2必要条件における第1所定値は、仮想面で発生可能な摩擦力の大きさの上限値に相当するものである。この第1所定値は、例えば、各仮想面に対応する接触対象面における垂直抗力成分を該仮想面における仮想面垂直抗力成分に一致させた場合に該接触対象面で発生可能な摩擦力の大きさの上限値に一致するか、またはそれよりも小さめの値に設定することが好ましい。
【0027】
これにより、前記仮想面必要並進力決定手段は、可能な限り、各仮想面における仮想面垂直抗力成分の合力によって前記必要総並進外力を実現すると共に、仮想面摩擦力成分を、前記第2必要条件を満足する範囲で必要最小限に留めるように、各仮想面における仮想面必要並進力を決定できることとなる。
【0028】
そして、第1発明では、前記接触対象面目標並進力決定手段が、少なくとも前記各仮想面に対応して決定された前記仮想面必要並進力に基づいて、各仮想面に対応する接触対象面から前記ロボットに作用させるべき目標並進力を決定し、その決定した目標並進力を前記目標歩容の構成要素として出力する。
【0029】
この場合、各仮想面に対応する接触対象面からロボットに作用させるべき目標並進力を、例えば、該目標並進力のうちの該接触対象面に垂直な方向の成分(垂直抗力成分)と該接触対象面に平行な成分(摩擦力成分)とがそれぞれ、該接触対象面に対応する仮想面における仮想面必要並進力の仮想面垂直抗力成分、仮想面摩擦力成分に一致するように決定すればよい。
【0030】
以上説明した第1発明によれば、前記したように、前記仮想面必要並進力決定手段は、可能な限り、各仮想面における仮想面垂直抗力成分の合力によって前記必要総並進外力を実現すると共に、仮想面摩擦力成分を、前記第2必要条件を満足する範囲で必要最小限に留めるように、各仮想面における仮想面必要並進力を決定できる。このため、各接触対象面からロボットへの目標並進力についても、可能な限り、各接触対象面における目標並進力のうちの垂直抗力成分の合力によって前記必要総並進外力を実現すると共に、該目標並進力のうちの摩擦力成分を、該接触対象面で発生可能な摩擦力の大きさの限界内で必要最小限に留めるように、各接触対象面からロボットへの目標並進力を決定できることとなる。
【0031】
そして、この場合、前記仮想面必要並進力決定手段の処理では、前記第1A必要条件、第1B必要条件、及び第2必要条件を用いることで、各接触対象面に対応する仮想面における仮想面必要並進力を決定するための指針(前記必要総並進外力を、各仮想面における仮想面必要並進力に分配する指針)が限定され、少なくとも、前記仮想面群の全ての仮想面に対して平行な成分を除く成分を、いずれかの仮想面における仮想面摩擦力成分に一致させ、又は分配することとなるような形態が排除される。また、各仮想面は、その相互の姿勢関係があらかじめ定まっているので、前記第1A必要条件、第1B必要条件、及び第2必要条件を満足し得る仮想面必要並進力を決定する処理のアルゴリズムの構築が容易になる。
【0032】
従って、第1発明によれば、複数の接触対象面からロボットに外力が作用する状況でのロボットの目標歩容を生成する場合に、ロボットの目標運動を実現するためにロボットに作用させるべき外力のうちの並進力の目標値を、適切な指針の基で決定することができる。
【0033】
かかる第1発明においては、前記仮想面必要並進力決定手段は、前記必要総並進外力のうちの前記仮想面群の全ての仮想面に対して平行な成分が“0”でない場合において、前記第1A必要条件、第1B必要条件及び第2必要条件を満足しつつ、前記仮想面群の複数の仮想面のうちの前記仮想面垂直抗力成分が“0”でない各仮想面において、該仮想面に対応して設定された前記第1所定値が大きいほど、該仮想面の仮想面摩擦力成分の大きさが大きくなるように、各仮想面における仮想面必要並進力を決定することが好ましい(第2発明)。
【0034】
この第2発明によれば、前記必要総並進外力のうちの前記仮想面群の全ての仮想面に対して平行な成分を、各仮想面に分配する指針がさらに限定され、各仮想面における仮想面並進力成分を決定するための処理のアルゴリズムの構築がより一層容易になる。
【0035】
また、各仮想面における仮想面摩擦力成分の大きさを、各仮想面で発生可能な摩擦力の上限値に相当する前記第1所定値の大きさに整合した大きさに設定することができる。ひいては、各接触対象面における目標並進力の摩擦力成分を、該接触対象面で発生可能な摩擦力の大きさの限界内にバランスよく収まるように(いずれかの接触対象面での摩擦力成分が、他の接触対象面での摩擦力成分に比して、摩擦力の大きさの限界に近づき過ぎたりしないように)、決定できる。
【0036】
前記第1発明又は第2発明において、前記複数の接触対象面が、互いに間隔を存して対向する2つの接触対象面を含むと共に、前記仮想面群が、当該2つの接触対象面に対応する仮想面として、互いに平行に対向する2つの仮想面である対向仮想面を含んでいる場合には、その2つの対向仮想面における仮想面垂直抗力成分は、それらの大きさが“0”で無い場合に、互いに逆向きの並進力となる。
【0037】
従って、2つの対向仮想面における仮想面垂直抗力成分のうち、大きさが小さい方の仮想面垂直抗力成分を第1仮想面垂直抗力成分、大きい方の仮想面垂直抗力成分を第2仮想面垂直抗力成分としたとき、第1仮想面垂直抗力成分と、第2仮想面垂直抗力成分のうちの第1仮想面垂直抗力成分と大きさの等しい成分とは、ロボットの運動に寄与しない内力として機能するものとなる。そして、この内力の大きさが過大になると、ロボットのアクチュエータが発生可能な動力のうち、上記内力の発生のために必要な負担分が大きくなり、ロボットの運動を変化させるために利用し得るアクチュエータの動力が不足し易いものとなる。
【0038】
そこで、前記第1発明又は第2発明では、前記複数の接触対象面が、互いに間隔を存して対向する2つの接触対象面を含むと共に、前記仮想面群が、当該2つの接触対象面に対応する2つの前記対向仮想面を含んでいる場合には、前記仮想面必要並進力決定手段は、次のように各仮想面における仮想面必要並進力を決定することが好ましい。
【0039】
すなわち、前記仮想面必要並進力決定手段は、前記決定された必要総並進外力が前記2つの対向仮想面と直交する方向の成分である対向仮想面直交成分を有する場合に、前記第1A必要条件、第1B必要条件及び第2必要条件を満足することに加えて、前記2つの対向仮想面のそれぞれにおける仮想面垂直抗力成分のうち、小さい方の仮想面垂直抗力成分の大きさがあらかじめ設定された第2所定値以下になるという第3必要条件を満足するように、各仮想面における仮想面必要並進力を決定することが好ましい(第3発明)。
【0040】
この第3発明では、前記第3必要条件における前記第2所定値は、前記2つの対向仮想面の間での前記内力の大きさの上限値に相当するものとなる。従って、第3発明によれば、前記決定された必要総並進外力が前記2つの対向仮想面と直交する方向の成分である対向仮想面直交成分を有する場合に、前記第1A必要条件、第1B必要条件及び第2必要条件を満足することに加えて、前記2つの対向仮想面の間での前記内力の大きさが過大になるのを防止しつつ、各仮想面における仮想面必要並進力を決定できる。
【0041】
ひいては、前記2つの対向仮想面に対応する2つ接触対象面間でロボットに発生する内力が過大になるのを防止するように、各接触対象面における目標並進力を決定できる。換言すれば、ロボットのアクチュエータが発生可能な動力のうち、目標運動の実現に利用し得る動力が前記内力によって不足するのを防止しつつ、各接触対象面における目標並進力を決定できる。
【図面の簡単な説明】
【0042】
【図1】本発明の実施形態における脚式移動ロボットの機構的な外力構成を示す図。
【図2】図1のロボットに備えた制御ユニットの機能とセンサ群とを示すブロック図。
【図3】図3(a)〜(c)は、図1のロボットの目標歩容の一例としての壁登り歩容におけるロボットの瞬時的な動作状態を時系列的に示す図。
【図4】図4(a),(b)は、図3(a)〜(c)の動作状態の後におけるロボットの瞬時的な動作状態を時系列的に示す図。
【図5】図5(a)〜(c)は壁登り歩容における外力作用点の設定位置の経時的変化の例を示すグラフ。
【図6】壁登り歩容における外力作用点の存在許容領域の面積の経時的変化の例を示すグラフ。
【図7】図2に示す外力目標生成部の、第1実施形態における処理を示すフローチャート。
【図8】図2に示す外力目標生成部の、第1実施形態における処理を示すフローチャート。
【図9】図2に示す外力目標生成部の第1実施形態における処理を示すフローチャート。
【図10】図7のS01で設定する仮想面を説明するための図。
【図11】図11(a)は図7のS05の処理により決定される仮想面必要並進力(仮想面垂直抗力成分及び仮想面摩擦力成分)の例を示す図、図11(b)は図7のS13までの処理により決定される仮想面必要並進力(仮想面垂直抗力成分及び仮想面摩擦力成分)の例を示す図。
【図12】図8のS19の処理を説明するための図。
【図13】図8のS25の処理を説明するための図。
【図14】図2に示す外力目標生成部の、第2実施形態における処理を示すフローチャート。
【図15】図2に示す外力目標生成部の、第2実施形態における処理を示すフローチャート。
【図16】図2に示す外力目標生成部の、第3実施形態における処理を示すフローチャート。
【図17】図2に示す外力目標生成部の、第4実施形態における処理を示すフローチャート。
【図18】図2に示す外力目標生成部の、第4実施形態における処理を示すフローチャート。
【図19】図19(a)〜(e)は図1のロボットの目標歩容の他の例におけるロボットの瞬時的な動作状態を時系列的に示す図。
【図20】図20(a)〜(d)は図1のロボットの目標歩容の他の例におけるロボットの瞬時的な動作状態を時系列的に示す図。
【発明を実施するための形態】
【0043】
[第1実施形態]
本発明の第1実施形態を図1〜図13を参照して以下に説明する。
【0044】
まず、図1を参照して、本実施形態における脚式移動ロボット1(以降、単にロボット1という)の機構的な概略構成を説明する。
【0045】
このロボット1は、基体3と、この基体3から延設された複数のリンク機構5とを有する。各リンク機構5は、ロボット1の脚として機能し得る機構である。そして、リンク機構5の個数は、本実施形態の例では4つである。
【0046】
なお、本実施形態の説明において、リンク機構5がロボット1の脚として機能し得るということは、該リンク機構5の先端部を、ロボット1の外界(動作環境)の床あるいは壁等の接触対象面に接触させた場合に、該リンク機構5が、ロボット1の自重(より正確には、ロボット1に作用する重力とロボット1の運動によって発生する慣性力との合力)の全部又は一部を支える外力を接触対象面から受けることができることを意味する。
【0047】
各リンク機構5は、複数の要素リンク7と、これらの要素リンク7を基体3側から順番に連接する複数の関節9とから構成されている。この場合、各リンク機構5の先端部を構成する要素リンク7(以降、これを先端部リンク7a、又は単にリンク機構5の先端部という)が、該リンク機構5を脚として機能させる場合に、ロボット1の外界の床あるいは壁等の接触対象面に接触させる部分である。そして、各先端部リンク7aは、板状に形成されており、その表面(外界との接触面)には図示を省略するゴム材等の弾性樹脂材が装着されている。
【0048】
各リンク機構5の各関節9は、例えば公知の構造の回転型の関節であり、それぞれ、1つ又は複数の自由度を有する。この場合、各リンク機構5に含まれる全ての関節9のトータルの自由度数が“6”以上の自由度数となるように、各リンク機構5毎の各関節9の自由度数が設定されている。このため、基体3に対する各先端部リンク7aの空間的な位置と姿勢(空間的な向き)とは、それぞれ3自由度を有する。なお、各リンク機構5には、回転型の関節の他に、直動型の関節が含まれていてもよい。
【0049】
また、図1では図示を省略するが、ロボット1には、各関節9をそれぞれ駆動する電動モータ等の複数の関節アクチュエータ11(図2に示す)が搭載されている。そして、各リンク機構5の各関節9は、それに対応する関節アクチュエータ11から、図示しない減速機等を含む動力伝達機構を介して駆動力が伝達されるようになっている。
【0050】
以上が本実施形態におけるロボット1の機構的な概略構成である。
【0051】
補足すると、図1に例示するロボット1では、各リンク機構5の要素リンク7のサイズや個数、関節9の個数、トータルの自由度数等を含めた各リンク機構5の構造が、全てのリンク機構5で同じであるように記載されているが、それらの構造は、全てのリンク機構5で同じである必要はない。
【0052】
また、図1では、ロボット1が、2つのリンク機構5a,5bを脚として機能させて平地を移動させる2脚移動ロボット(人型ロボットに類似するロボット)であるように記載されているが、例えば全てのリンク機構5を脚として機能させて平地を移動させるようなロボット(例えば4脚ロボット)であってもよい。
【0053】
また、リンク機構5の個数は、4個である必要ななく、例えば、3個もしくは5個以上であってもよい。
【0054】
次に、図2を参照して、以上の如く構成されたロボット1には、ロボット1の動作制御に利用されるセンサ群13と、CPU、RAM、ROM、関節アクチュエータ11の駆動回路等を含む電子回路ユニットにより構成された制御ユニット21とが搭載されている。
【0055】
センサ群13は、本実施形態の例では、ロボット1の代表リンク(代表部位)としての基体3の空間的な姿勢を計測するための姿勢センサ15と、各リンク機構5の先端部に作用する外力を計測するための力センサ17と、各関節9の変位量(本実施形態では回転角)を計測するための関節変位センサ19とを備える。
【0056】
この場合、姿勢センサ15は、例えばジャイロセンサ等の角速度センサ(レートセンサ)と加速度センサとから構成され、図1に示す如く基体3に搭載されている。また、力センサ17は、例えば3軸方向の並進力及び3軸周りのモーメントを検出可能な6軸力センサにより構成され、図1に示す如く、各リンク機構5の最も先端側の関節9と先端部リンク7aとの間に介装されている。また、関節変位センサ19は、例えば各関節9に装着されたエンコーダやポテンショメータ等により構成される。
【0057】
制御ユニット21は、ロボット1の適所、例えば基体3に搭載されている。この制御ユニット21は、実装されるプログラム等により実現される主要な機能として、ロボット1の目標とする動作を表す目標歩容を生成(作成)して出力する歩容生成部23と、この目標歩容に応じてロボット1の動作制御を行う動作制御部29とを備える。
【0058】
歩容生成部23の処理の詳細な説明は後述するが、該歩容生成部23が生成して出力する目標歩容は、ロボット1の目標運動と、ロボット1に外界から作用させる外力に関する目標とから構成される。
【0059】
この場合、目標歩容の構成要素のうちの目標運動は、各関節9の目標変位量の時系列と、ロボット1の代表リンク(本実施形態では基体3)のグローバル座標系での目標位置及び目標姿勢の時系列とから構成される。なお、代表リンク(基体3)の目標位置は、より詳しくは、該代表リンクに設定された代表点の目標位置である。また、グローバル座標系は、ロボット1の外界に対して固定される座標系である。
【0060】
また、目標歩容の構成要素のうちの外力に関する目標は、1つ以上のリンク機構5の先端部リンク7aを外界の接触対象面に接触させる場合に該接触対象面からロボット1に作用させるべき外力の目標値(目標外力)の時系列と、その外力の作用点の目標位置としての目標外力作用点の時系列とから構成される。この場合、目標外力は、本実施形態では、該外力のうちの並進力成分である並進外力の目標値(目標並進力)と、該外力のうち、外界の接触対象面に垂直な方向の軸周りでのねじり力(モーメント外力)の目標値(目標ねじり力)とから構成される。
【0061】
なお、接触対象面からロボット1に作用させるべき外力の作用点というのは、その接触対象面からロボットに作用する外力の全体によって、該作用点の周りに発生するモーメントのうち、該接触対象面に平行な軸周りの成分が“0”となるような点を意味する。その作用点は、ロボット1を単に床上で移動させる場合には、所謂、床反力中心点に相当する点である。このことは、後述する仮想面上の外力作用点についても同様である。
【0062】
補足すると、目標歩容に関する上記「時系列」は、ある期間内の瞬時値の順序列そのものでよいことはもちろんであるが、該時系列のパターンを規定する関数式等のパラメータであってもよい。
【0063】
また、ロボット1のあるリンク機構5のトータルの自由度数が6自由度である場合には、基体3に対する該リンク機構5の先端部リンク7aの位置及び姿勢を指定すれば、該リンク機構5の各関節9の変位量が一義的に決定されることとなる。従って、6自由度のリンク機構5については、該リンク機構5の各関節9の目標変位量の代わりに、該リンク機構5の先端部リンク7aの目標位置及び目標姿勢を、前記目標運動の構成要素として用いてもよい。
【0064】
さらに、必要に応じて、ロボット1の所定の運動状態量の目標値、例えば、ロボット1の全体重心点の位置、速度、加速度の目標値、あるいは、ロボット1の基体3以外の特定の部位の位置、姿勢の目標値を、歩容生成部23で生成する目標運動の構成要素に含めるようにしてもよい。
【0065】
動作制御部25は、歩容生成部23が生成した目標歩容にロボット1の実際の動作を追従させるようにロボット1の各関節9をそれに対応する関節アクチュエータ11を介して制御する。この場合、本実施形態では、姿勢センサ15の出力と関節変位センサ19の出力とから認識されるロボット1全体の重心点の実際の位置と、目標運動により規定されるロボット1全体の重心点の目標位置との偏差に応じて、該偏差を“0”に近づけるために、ロボット1に付加的に作用させるべき外力が決定される。さらに、この付加的外力を目標歩容における外力の目標値に付加してなる外力と目標運動とに、それぞれ、実際の外力(前記力センサ17の出力から認識される外力)とロボット1の実際の運動とを追従させるように、各関節9に付与すべき駆動力が決定される。そして、この決定された駆動力に応じて各関節アクチュエータ11の駆動力が制御される。
【0066】
次に、前記歩容生成部23による目標歩容の生成処理を詳細に説明する。なお、以降の説明では、4つのリンク機構5を区別する場合には、図1に示す如く参照符号5a,5b,5c,5dを付する。
【0067】
歩容生成部23は、本願発明を適用した処理によって目標歩容を生成する。この生成処理のアルゴリズムは、種々様々な動作環境でロボット1を運動させる歩容を生成することが可能である。
【0068】
ただし、本実施形態における目標歩容の生成処理は、特に、ロボット1の外界(動作環境)に存する互いに異なる複数の接触対象面にロボット1の複数の部位を接触させながら、ロボット1の運動を行なわせるための目標歩容を生成する場合に特徴を有する。
【0069】
そこで、以降の説明では、本実施形態の理解の便宜上、複数の接触対象面にロボット1の複数のリンク機構5の先端部リンク7aを接触させながら、ロボット1の運動を行なわせるための一例の目標歩容を参考的に提示する。そして、その一例の目標歩容の生成処理に関する説明を適宜行いつつ、歩容生成部23の処理を説明する。
【0070】
以降の本実施形態の説明で参考的に例示する目標歩容に係わるロボット1の動作環境は、ロボット1の外界に存する互いに異なる接触対象面として、例えば図1に示すように、床面FLと、この床面FLからほぼ垂直に起立する2つの壁面WL1,WL2とを備える動作環境である。この場合、2つの壁面WL1,WL2は、互いに間隔を存して平行もしくはほぼ平行に対向する接触対象面である。
【0071】
そして、本実施形態の説明で参考的に例示する目標歩容は、2つの壁面WL1,WL2の間で、床面FL上から壁面WL1,WL2をよじ登らせるようにロボット1を動作させる目標歩容である。以降、この目標歩容を壁登り歩容ということがある。
【0072】
この壁登り歩容におけるロボット1の動作形態の概要を図3(a),(b),(c)及び図4(a),(b)を参照して以下に説明しておく。図3(a),(b),(c)及び図4(a),(b)は、壁登り歩容におけるロボット1の瞬時的な動作状態(時刻t1〜t5での動作状態)を時系列順に示している。
【0073】
なお、これらの図では、便宜上、ロボット1の各リンク機構5のリンク要素7及び関節9の図示を省略し、各リンク機構5をその基端部(基体3側の端部)と先端部とを結ぶ線分として記載している。この場合、図面上は、各リンク機構5の先端部は点となるが、実際には、前記した板状の先端部リンク7aである。
【0074】
また、これらの図における矢印F1〜F10は、各図のロボット1の動作状態で、接触対象面FL又はWL1又はWL2からロボット1に作用する外力(並進外力)の例を視覚的に示している。この場合、各並進外力F1〜F10の始点が接触対象面FL又はWL1又はWL2からロボット1への該並進外力の作用点(外力作用点)に相当する。
【0075】
また、これらの図において、接触対象面FL又はWL1又はWL2上に記載している丸枠領域AR1〜AR12は、各図のロボット1の動作状態で、接触対象面FL又はWL1又はWL2からロボット1への外力作用点の概略的な存在可能領域を視覚的に示している。その存在可能領域は、単一のリンク機構5の先端部だけが接触している接触対象面では、その単一のリンク機構5の先端部リンク7aと接触対象面との接触面内の領域である。また、複数のリンク機構5の先端部が接触している接触対象面での外力作用点の存在可能領域は、それらの複数のリンク機構5の先端部リンク7aのそれぞれと接触対象面との各接触面内の領域をひとまとめに連接してなる領域(所謂、支持多角形に相当する領域)である。
【0076】
以下、これらの図を参照して壁登り歩容を説明すると、図3(a)は壁登り歩容の初期状態を示している。この初期状態では、ロボット1の2つのリンク機構5a,5bの先端部を床面FLに接触(接地)させた状態で、ロボット1が壁面WL1,WL2の間で床面FL上に起立する。また、ロボット1の壁登り歩容の動作を開始するために、ロボット1は、残りのリンク機構5c,5dのうちの壁面WL1側のリンク機構5cの先端部を該壁面WL1に接触させると共に、壁面WL2側のリンク機構5dの先端部を該壁面WL2に接触させる。
【0077】
この場合、図3(a)の初期状態では、ロボット1の自重(より正確には、ロボット1に作用する重力とロボット1の運動によって発生する慣性力との合力)を支えるための外力として、例えば、矢印F1で示す外力が、床面FLの領域AR1からロボット1に作用する。なお、図示例では、壁面WL1,WL2からロボット1に作用する外力は“0”である。
【0078】
次いで、ロボット1の動作状態は、図3(b)に示す状態に移行する。この図3(b)の動作状態は、図3(a)の初期状態で壁面WL1,WL2に接触させたリンク機構5c,5dの先端部をそれぞれ壁面WL1,WL2に押し付けつつ、リンク機構5a,5bのうちの一方(図示例では、リンク機構5a)の先端部を床面FL上から浮上させることによって、図3(a)の初期状態から移行する動作状態である。
【0079】
この場合、図3(b)の動作状態では、ロボット1の自重を支えるための外力として、例えば、床面FLの領域AR4から矢印F2で示す外力がロボット1に作用すると共に、壁面WL1の領域AR5と壁面WL2の領域AR6とからそれぞれ矢印F3,F4で示す外力(上向きの摩擦力成分を有する外力)がロボット1に作用する。
【0080】
次いで、ロボット1の動作状態は、図3(c)に示す状態に移行する。この図3(c)の動作状態は、図3(b)の動作状態で壁面WL1,WL2にそれぞれ押し付けたリンク機構5c,5dの先端部の押し付け力を増加させ、さらに、図3(b)の動作状態で床面FLに接触させていたリンク機構5bを床面FL上から浮上させる(ひいては、リンク機構5a,5bの両方を床面FL上から浮上させる)ことによって、図3(b)の動作状態から移行する動作状態である。
【0081】
この場合、図3(c)の動作状態では、ロボット1の自重を支えるための外力として、例えば、壁面WL1の領域AR7と、壁面WL2の領域AR8とから、それぞれ矢印F5,F6で示す外力(上向きの摩擦力成分を有する外力)がロボット1に作用する。
【0082】
次いで、ロボット1の動作状態は、図4(a)に示す状態に移行する。この図4(a)の動作状態は、図3(c)の動作状態までに床面FLから浮上させたリンク機構5a,5bのうちのリンク機構5aの先端部を、リンク機構5cの先端部の下側で壁面WL1に接触させると共に、リンク機構5bの先端部を、リンク機構5dの先端部の下側で壁面WL2に接触させることによって、図3(c)の動作状態から移行する動作状態である。
【0083】
この場合、図4(a)に示した動作状態は、リンク機構5a,5bをそれぞれ壁面WL1,WL2に接触させた直後の状態である。この状態では、例えば、壁面WL1の領域AR9のうち、リンク機構5cの先端部寄りの点を外力作用点として、矢印F7で示す外力(上向きの摩擦力成分を有する外力)がロボット1に作用すると共に、壁面WL2の領域AR10のうち、リンク機構5dの先端部寄りの点を外力作用点として、矢印F8で示す外力(上向きの摩擦力成分を有する外力)がロボット1に作用する。
【0084】
次いで、ロボット1の動作状態を、図4(a)に示した如くリンク機構5a,5cを壁面WL1に接触させ、且つ、リンク機構5b,5dを壁面WL2に接触させた状態に維持したまま、下側のリンク機構5a,5bのそれぞれの壁面WL1,WL2への押し付け力を増加させていくと共に、上側のリンク機構5c,5dのそれぞれの壁面WL1,WL2への押し付け力を減少させていく。
【0085】
これにより、壁面WL1からロボット1への外力作用点を、領域AR9のうちのリンク機構5aの先端部寄りの箇所に近づけていくと共に、壁面WL2からロボット1への外力作用点を、領域AR10のうちのリンク機構5bの先端部寄りの箇所に近づけていく。そして、最終的には、壁面WL1からロボット1への外力作用点を、リンク機構5aの先端部と壁面WL1との接触面内に移動させると共に、壁面WL2からロボット1への外力作用点を、リンク機構5bの先端部と壁面WL2との接触面内に移動させる。
【0086】
次いで、ロボット1の動作状態は、図4(b)に示す状態に移行する。この図4(b)の動作状態は、図4(a)の動作状態で、上記の如く壁面WL1,WL2からロボット1への外力作用点を移動させた後に、リンク機構5a,5bをそれぞれ壁面WL1,WL2に押し付けた状態を維持したまま、リンク機構5c,5dの先端部のうちの一方(図示例では、リンク機構5cの先端部)を壁面WL1から離反させることによって、図4(a)の動作状態から移行する動作状態である。
【0087】
この場合、図4(b)に示した動作状態では、ロボット1の自重を支えるための外力として、壁面WL1の領域AR11から矢印F9で示す外力(上向きの摩擦力成分を有する外力)がロボット1に作用すると共に、壁面WL2の領域AR12から矢印F10で示す外力(上向きの摩擦力成分を有する外力)がロボット1に作用する。
【0088】
その後は、リンク機構5dの先端部も壁面WL2から離反される。そして、壁面WL1,WL2から離反されたリンク機構5c,5dの先端部が、それぞれ、図4(a)の動作状態よりも高所で壁面WL1,WL2に接触される。そして、図3(b)以降の動作と同様の動作の繰り返しによって、ロボット1が壁面WL1,WL2をよじ登っていく。
【0089】
以上が、壁登り歩容の概略的な形態である。
【0090】
このような壁登り歩容等の目標歩容を生成する歩容生成部23が実行する処理は、図2のブロック図に示す如く、基本歩容要素生成部25の処理と、外力目標生成部27の処理とに大別される。
【0091】
上記基本歩容要素生成部25は、目標歩容のうちの基本的な構成要素として、ロボット1の目標運動と、暫定的な目標外力作用点である暫定外力作用点とを生成する処理を実行する機能部である。なお、基本歩容要素生成部25は、暫定外力作用点を決定するために、目標外力作用点の存在許容領域を設定する処理も実行する。
【0092】
また、上記外力目標生成部27は、基本歩容要素生成部25が生成した目標運動と暫定外力作用点とを用いて、該目標運動を実現する上で適切な外力の目標値(目標外力)と目標外力作用点とを決定する処理を実行する機能部である。
【0093】
この場合、本実施形態では、外力目標生成部27は、基本歩容要素生成部25が生成した目標運動を評価する機能を有する。そして、外力目標生成部27は、生成された目標運動が適切である場合に、目標歩容の全期間における目標外力と目標外力作用点とを決定する処理を実行し、該目標運動が不適切であると評価されるときには、目標外力と目標外力作用点とを決定する処理を中止する。
【0094】
歩容生成部23が目標歩容を生成するとき、歩容生成部23には、制御ユニット21にあらかじめ記憶保持された移動指令データ、あるいは、外部から無線通信などにより制御ユニット21に入力される移動指令データが与えられると共に、ロボット1の外界に存在する接触対象面等に関する環境情報が与えられる。
【0095】
上記移動指令データは、例えば、ロボット1の移動開始時刻や、移動経路、移動の仕方等を指定するデータである。また、上記環境情報は、例えば、ロボット1の外界の接触対象面(床面FL、壁面WL1,WL2等)のロボット1に対する相対位置及び相対姿勢を認識するために必要な情報や、各接触対象面の性状(摩擦係数等)を認識するために必要な情報等から構成される。
【0096】
なお、上記環境情報は、制御ユニット21に外部から無線通信等により入力される情報、又は制御ユニット21にあらかじめ記憶保持された情報でもよいが、ロボット1に搭載した図示しない撮像カメラ等の環境認識用のセンサの出力を基に、制御ユニット21で認識された情報であってもよい。
【0097】
そして、歩容生成部23は、これらの移動指令データと環境情報とを用いて、まず、基本歩容要素生成部25の処理を実行する。この基本歩容要素生成部25は、入力された移動指令データと環境情報とに基づいて、ロボット1の運動を制約する条件(ロボット1の目標運動が満たすべき条件)を設定し、その条件を満足させるように、逆運動学の演算処理によってロボット1の目標運動を生成する。
【0098】
この場合、ロボット1の運動を制約する条件は、基本的には、該ロボット1の要求される運動形態に応じて適宜設定される。その制約条件としては、例えば、各リンク機構5の先端部の運動を制約する条件(各リンク機構5の先端部リンク7aを外界の接触対象面に接触させる時刻の目標、その接触時における接触対象面上の接触位置の目標、その接触時における先端部リンク7aの姿勢の目標、外界の接触対象面に接触させていない先端部リンク7aの、ある時刻又は期間における位置及び姿勢の目標等)と、基体2の運動を制約する条件(ある時刻又は期間における基体2の位置及び姿勢の目標等)とを使用することができる。
【0099】
なお、基体2の運動を制約する条件に代えて、もしくは、該条件に加えて、ロボット1の運動に関する所定の状態量を制約する条件(例えばロボット1の全体の並進運動量又は角運動量の目標等)を使用するようにしてもよい。また、該制約条件には、一般には、ロボット1の関節9の可動範囲や動作速度(関節9の変位量の変化速度)の許容範囲等、機構的な制約条件も含まれる。
【0100】
前記壁登り歩容の生成を行なう場合には、基本歩容要素生成部25は、図3及び図4を参照して説明した時系列的な形態で、ロボット1の各リンク機構5の先端部が接触対象面FL,WL1,WL2のいずれかに接触する動作が行なわれるように、ロボット1の運動を制約する条件(例えば、各リンク機構5の先端部の運動を制約する条件と、基体3の運動を制約する条件とを含む制約条件)を決定する。そして、基本歩容要素生成部25は、この制約条件を満足させるように、逆運動学の演算処理によってロボット1の目標運動の時系列を生成する。なお、目標運動は、不連続に(ステップ状に)変化することが無いように生成される。
【0101】
また、基本歩容要素生成部25は、上記の如く決定した目標運動に基づいて、各接触対象面(1つ又は複数のリンク機構5の先端部リンク7aを接触させている面)における暫定外力作用点の時系列を決定する。
【0102】
この場合、基本歩容要素生成部25は、まず、上記の如く生成した目標運動に基づいて、各接触対象面における外力作用点の存在許容領域を設定する。より詳しくは、基本歩容要素生成部25は、上記の如く生成した目標運動において、1つのリンク機構5の先端部リンク7aだけを接触させる接触対象面においては、該接触対象面と先端部リンク7aとの接触面内に外力作用点の存在許容領域を設定する。その存在許容領域は、接触対象面と先端部リンク7aとの接触面内で、その接触面の境界に近づき過ぎないような領域に設定される。
【0103】
また、2つ以上のリンク機構5の先端部リンク7aを接触させる接触対象面においては、基本歩容要素生成部25は、該接触対象面と各先端部リンク7aとの接触面を連接してなる領域(所謂、支持多角形の領域)内に外力作用点の存在許容領域を設定する。その存在許容領域は、支持多角形の領域内で、該支持多角形の領域の境界に近づき過ぎないような領域に設定される。
【0104】
そして、基本歩容要素生成部25は、上記の如く設定した外力作用点の存在許容領域内の点(例えば、該存在許容範囲内の中央点)を暫定外力作用点として決定する。なお、各接触対象面上の暫定外力作用点は、該接触対象面にいずれかのリンク機構5が接触する状態が連続的に継続する期間内において、該作用点の位置が不連続に(ステップ状に)変化することが無いように決定される。
【0105】
歩容生成部23が図3及び図4に例示した壁登り歩容の生成を行なう場合には、暫定外力作用点は、例えば、図5(a),(b),(c)に例示する時系列パターンで生成される。なお、図5(a)〜(c)における横軸の時刻t1〜t5は、それぞれ順番に、図3(a)、図3(b)、図3(c)、図4(a)、図4(b)の動作状態に対応する時刻を示している。
【0106】
図5(a)〜(c)に示す例では、床面FL上の暫定外力作用点の位置(壁面WL1,WL2に垂直な方向での位置)は、時刻t1から時刻t2の直前までに壁面WL1側から壁面WL2側に連続的に移動していくように決定される。
【0107】
また、壁面WL1上の暫定外力作用点の位置(高さ方向での位置)は、時刻t3と時刻t4との間の途中時刻まで一定の位置(壁面WL1とリンク機構5cの先端部との接触面内の位置)に維持された後、該途中時刻から時刻t4の直前まで、下方に連続的に移動していくように決定される。次いで、壁面WL1上の暫定外力作用点の位置は、時刻t4と時刻t5との間の途中時刻まで一定の位置(リンク機構5cの先端部の壁面WL1への接触面とリンク機構5aの先端部の壁面WL1への接触面との間の中間位置)に維持された後、時刻t5の直前の時刻まで、下方に連続的に移動していき、さらに、時刻t5の前後の期間で一定の位置(壁面WL1とリンク機構5aの先端部との接触面内の位置)に維持されるように決定される。
【0108】
また、壁面WL2上の暫定外力作用点の位置(高さ方向での位置)は、時刻t3と時刻t4との間の途中時刻まで一定の位置(壁面WL2とリンク機構5dの先端部との接触面内の位置)に維持された後、該途中時刻から時刻t4の直前の時刻まで、下方に連続的に移動していき、その後、時刻t5の直後の時刻までの期間で一定の位置(リンク機構5dの先端部の壁面WL2への接触面とリンク機構5bの先端部の壁面WL2への接触面との間の中間位置)に維持されるように決定される。
【0109】
なお、この場合、床面FLと壁面WL1,WL2のそれぞれにおける外力作用点の存在許容領域は、その面積が、例えば図6に示すようなパターンで連続的に変化するように設定される。
【0110】
補足すると、基本歩容要素生成部25が生成する目標運動は、基本的には、ロボット1に行なわせようとする運動に関する要求を形式的に(少なくとも運動学的に)実現し得るように生成されたものであればよい。換言すれば、該目標運動は、動力学的な実現性が厳密に保障されたものでなくてもよい。従って、該目標運動を実現するためにロボット1に作用させるべき外力が、実際にはロボット1に作用させることが困難となる場合や、該外力が好適な範囲を逸脱するようなものとなる場合(該外力が過大になり過ぎる場合等)もあり得る。
【0111】
歩容生成部23は、次に、外力目標生成部27の処理を実行する。この外力目標生成部27には、基本歩容要素生成部25で生成された目標運動、暫定外力作用点及び外力作用点の存在許容領域が入力される。さらに、外力目標生成部27には、前記環境情報も入力される。
【0112】
そして、外力目標生成部27は、これらの入力データを用いて、図7〜図9のフローチャートに示す処理を実行することによって、目標歩容の目標外力と目標外力作用点とを決定する。図7〜図9のフローチャートに示す処理は、目標歩容の各時刻(所定の刻み時間毎の各時刻)における目標運動の評価を行いつつ、目標外力及び目標外力作用点の瞬時値を決定するための処理を示しており、この処理を目標歩容の各時刻毎に順番に行なうことによって、目標運動が適切である場合に、目標外力と目標外力作用点とが時系列的に決定される。
【0113】
なお、外力目標生成部27の処理に関する以降の説明においては、図7〜図9のフローチャートの処理により今現在、決定しようとする目標外力及び目標外力作用点の瞬時値に対応する時刻を今回時刻という。また、今回時刻の1つ前の時刻を前回時刻、次の時刻を次回時刻という。
【0114】
以下説明すると、外力目標生成部27は、まず、S01の処理を実行する。このS01では、外力目標生成部27は、目標運動と環境情報とを基に、今回時刻での目標外力及び目標外力作用点を決定するための演算処理に用いる作業用の仮想面を、ロボット1の外界の各接触対象面(目標運動における今回時刻でいずれかのリンク機構5の先端部を接触させる接触対象面)に一致又は近似させるように設定する。
【0115】
ここで、本実施形態では、外力目標生成部27の処理のアルゴリズムに汎用性を持たせると共に該アルゴリズムの構築を行い易くするために図10に示す如く、相互の姿勢関係があらかじめ定められた6つの平面S1a,S2a,S3a,S1b,S2b,S3bが、作業用の仮想面として使用し得る面として用意されている。そして、外力目標生成部27の処理では、ロボット1の目標運動を実現するために該ロボット1に各接触対象面から作用すべき外力は、各接触対象面に対応する仮想面からロボット1に作用する外力であると見なされる。
【0116】
この場合、本実施形態では、6つの仮想面のうちの仮想面S1a,S2a,S3aは相互に直交する平面とされ、仮想面S1b,S2b,S3bは、それぞれ仮想面S1a,S2a,S3aに平行な平面とされている。
【0117】
また、互いに平行に対向する2つの仮想面は、それぞれが発生し得る垂直抗力が互いに逆向き(各仮想面からこれに対向する仮想面に向かう向き)であるとされる。例えば、図10に示す如く、3軸直交座標系(XYZ座標系)を想定した場合において、X軸に直交する仮想面S1a,S1bがそれぞれ発生可能な垂直抗力N_1a,N_1bの向きは、それぞれX軸方向の正の向き(S1aからS1bに向かう向き)、X軸方向の負の向き(S1bからS1aの向かう向き)である。このことは、Y軸に直交する仮想面S2a,S2bの組、Z軸に直交する仮想面のS3a,S3bの組についても同様である。
【0118】
そして、S01の処理では、外力目標生成部27は、今回時刻での外界の各接触対象面(詳しくは、各接触対象面のうちのロボット1のリンク機構5の先端部との接触面)に、それぞれ、仮想面S1a,S2a,S3a,S1b,S2b,S3bのいずれか1つを対応付ける。そして、外力目標生成部27は、今回時刻での各接触対象面に対応付けた仮想面が、それに対応する接触対象面に一致又は近似するように、今回時刻での接触対象面と同数の仮想面を、ロボット1の外界(前記環境情報により認識される外界)に対して仮想的に設定する。なお、外界に仮想面を設定するということは、外界における仮想面の配置位置及び姿勢を決定することを意味する。
【0119】
この場合、今回時刻での外界の接触対象面が複数となる場合には、各接触対象面(各接触対象面のうちのロボット1のリンク機構5の先端部との接触面)とこれに対応する仮想面との間の姿勢差(空間的な角度差)が、いずれの接触対象面についてもできるだけ“0”又はそれに近いものとなるように、外界における仮想面の配置(接触対象面と同数の仮想面から成る仮想面群の配置)が決定される。例えば、各接触対象面毎の上記姿勢差の空間的な各成分(3軸周りの角度差成分)の2乗値又は絶対値を、今回時刻での全ての接触対象面に関して加え合わせてなる値(又は該2乗値又は絶対値の平均値)が最小となるように、今回時刻での全ての接触対象面と同数の仮想面から成る仮想面群の配置が決定される。
【0120】
従って、本実施形態では、今回時刻での外界の接触対象面が複数となる場合に、S01で設定される複数の仮想面(仮想面群)は、接触対象面と同数の仮想面により構成され、その相互の姿勢関係は、任意の2つの仮想面が互いに直交するか、又は互いに平行に対向するものとなる。
【0121】
なお、今回時刻での接触対象面が前回時刻での接触対象面と同一の接触対象面を有する場合には、今回時刻において、その同一の接触対象面に対応付けられる仮想面は、前回時刻と同じ仮想面に維持される。
【0122】
以上のS01の処理によって、歩容生成部23が、前記壁登り歩容を生成する場合には、例えば、次のように仮想面が設定される。すなわち、図3(a)又は図3(b)に示した動作状態の時刻では、6つの仮想面S1a,S2a,S3a,S1b,S2b,S3bのうちの互いに平行な2つの仮想面を含む3つの仮想面、例えば、仮想面S3a,S2a,S2bがそれぞれ接触対象面としての床面FL、壁面WL1、壁面WL2に対応付けられる(図中の括弧付きの参照符号を参照)。そして、これらの仮想面S3a,S2a,S2bがそれぞれ床面FL、壁面WL1、壁面WL2に一致又はほぼ一致する面になるように、ロボット1の外界における仮想面S3a,S2a,S2bの配置が決定される。
【0123】
また、図3(c)、図4(a)、図4(b)の動作状態の時刻では、図3(a),(b)の動作状態での壁面WL1,WL2にそれぞれ対応付けられた仮想面S2a,S2bが、そのまま、壁面WL1,WL2にそれぞれ対応付けられる(図中の括弧付きの参照符号を参照)。そして、これらの仮想面S2a,S2bがそれぞれ壁面WL1,WL2に一致又はほぼ一致するように、ロボット1の外界における仮想面S2a,S2bの配置が決定される。
【0124】
なお、以降の本実施形態の説明では、S01で設定される仮想面のうち、互いに平行に対向する2つの仮想面を対向仮想面ということがある。この対向仮想面の組は、本実施形態では、仮想面S1a,S1bの組、仮想面S2a,S2bの組、仮想面S3a,S3bの組のいずれかである。そして、これらの各組の対向仮想面を総称的に表記する場合に、対向仮想面Sna,Snb(n=1又は2又は3)、あるいは、単に対向仮想面Sna,Snbと表記することがある。
【0125】
図5の説明に戻って、外力目標生成部27は、次に、S03の処理を実行する。このS03では、外力目標生成部27は、基本歩容要素生成部25が作成した目標運動を基に、この目標運動を実現するために、今回時刻でロボット1に作用させるべきトータルの外力である必要総外力のうちの並進力成分である必要総並進外力Fallと、該必要総外力のうちの所定の基準点周りのモーメント成分である必要総モーメント外力Mallとを決定する。
【0126】
ここで、上記所定の基準点(以降、モーメント基準点という)は、ロボット1の外界に対して固定された点(例えば、外界の接触対象面又はこれに対応する仮想面上に設定された点)である。このモーメント基準点は、目標歩容の全期間で一定の点に維持してもよいが、目標歩容の所定の期間毎に適宜更新するようにしてもよい。
【0127】
なお、本実施形態における外力目標生成部27の処理では、必要総並進外力Fall、必要総モーメント外力Mall等の並進力もしくはモーメント、あるいは、空間的な位置、姿勢等の変数(ベクトル量)は、S01で設定される仮想面に対して固定されたグローバル座標系(例えば、各仮想面に対して図10に示すような位置及び姿勢関係で設定される3軸直交座標系(XYZ座標系))で記述される。
【0128】
上記必要総並進外力Fall及び必要総モーメント外力Mallは、より具体的には次のように決定される。すなわち、必要総並進外力Fallを決定する場合には、外力目標生成部27は、ロボット1の目標運動を基に、ロボット1の幾何学モデル(剛体リンクモデル)を用いて、ロボット1の全体重心点の目標位置の2階微分値である目標重心加速度(今回時刻での目標重心加速度)を算出する。なお、ロボット1の全体重心点の目標位置、あるいは、目標重心加速度は、前記基本歩容要素生成部25で目標運動の要素として算出しておくようにしてもよい。
【0129】
そして、外力目標生成部27は、この目標重心加速度とロボット1の全体質量とから算出される並進慣性力(ロボット1の目標運動によって発生する慣性力の並進力成分)とロボット1に作用する重力との合力に釣り合う並進力を必要総並進外力Fallとして算出する。
【0130】
なお、目標運動によって発生するロボット1の全体の並進慣性力は、例えば、ロボット1の各リンク3,7毎の重心点の並進慣性力を加え合わせることによって算出するようにしてもよい。
【0131】
また、必要総モーメント外力Mallを決定する場合には、外力目標生成部27は、ロボット1の目標運動を基に、ロボット1の幾何学モデル(剛体リンクモデル)を用いて、ロボット1の目標運動によって該ロボット1の全体重心点の周りに発生する今回時刻での慣性力モーメント(慣性力のモーメント成分)と、該目標運動におけるロボット1の全体重心点の運動(並進運動)が前記モーメント基準点周りに発生する今回時刻での慣性力モーメントとの総和の慣性力モーメントを算出する。そして、外力目標生成部27は、この総和の慣性力モーメントに釣り合うモーメントを今回時刻での必要総モーメント外力Mallとして算出する。
【0132】
なお、ロボット1の目標運動によって発生する並進慣性力の時系列と、前記モーメント基準点周りの慣性力モーメントの時系列とを前記基本歩容要素生成部25で算出しておいてもよい。そして、今回時刻での並進慣性力の値と慣性力モーメントの値とを基本歩容要素生成部25から外力目標生成部27に入力するようにしてもよい。
【0133】
上記の如く必要総並進外力Fall及び必要総モーメント外力Mallを算出した後、外力目標生成部27は、次に、S05〜S15の処理を実行する。このS05〜S15の処理は、ロボット1の目標運動(より詳しくは、該目標運動により規定されるロボット1の全体重心点の並進運動)を実現するために、S01で設定した各仮想面からロボット1に作用させるべき並進力である仮想面必要並進力を決定する処理である。この各仮想面における仮想面必要並進力は、該仮想面に垂直な成分である仮想面垂直抗力成分と、該仮想面に平行な成分である仮想面摩擦力成分とから構成される。なお、S05〜S15の処理によって決定される仮想面必要並進力は暫定値であり、後述のS19以降の処理によって修正される場合がある。また、S05〜S15の処理には、決定する仮想面必要並進力に基づいて、目標運動の評価を行なう処理も含まれる。
【0134】
以下、S05〜S15の処理を具体的に説明すると、外力目標生成部27は、まず、S05の処理を実行する。このS05では、外力目標生成部27は、S03で決定した必要総並進外力Fallに応じて、各仮想面における仮想面必要総並進力のうちの、該仮想面に垂直な成分(垂直抗力成分)である仮想面垂直抗力成分N_iと、該仮想面に平行な成分(摩擦力成分)である仮想面摩擦力成分F_iとを決定する。
【0135】
なお、本実施形態の説明では、参照符号N_i,F_i等における添え字“i”は、今回時刻におけるS01で設定された仮想面のうちの任意の仮想面の識別符号を意味する。そして、この添え字“i”を付した参照符号は、識別符号iの仮想面に関連するものの参照符号であることを意味する。この場合、本実施形態の説明では、仮想面S1a,S2a,S3a,S1b,S2b,S3bのそれぞれの識別符号は、仮想面を表す参照符号中の“S”の添え字、すなわち、1a,2a,3a,1b,2b,3bであるとする。そして、ある特定の仮想面に関連する参照符号には、その特定の仮想面の添え字と同じ添え字を付する。例えば、仮想面S2aにおける仮想面垂直抗力成分の参照符号を“N_2a”と表記する。
【0136】
この場合、外力目標生成部27は、今回時刻のS01で設定した全ての仮想面における仮想面必要並進力の合力がS03で決定した必要総並進外力Fallに一致するという第1必要条件を満足するように、各仮想面における仮想面必要並進力を構成する仮想面垂直抗力成分N_iと仮想面摩擦力成分F_iとを決定する。
【0137】
より詳しくは、本実施形態におけるS05の処理では、上記第1必要条件を満足するために、外力目標生成部27は、今回時刻で設定した全ての仮想面における仮想面垂直抗力成分N_iの合力が、必要総並進外力Fallのうち、当該全ての仮想面に対して平行な成分を除く成分に一致するという第1A必要条件と、当該全ての仮想面における仮想面摩擦力成分F_iの合力が、必要総並進外力Fallのうち、当該全ての仮想面に対して平行な成分に一致するという第1B必要条件とを満足するように、各仮想面における仮想面垂直抗力成分N_iと仮想面摩擦力成分F_iとを決定する。
【0138】
なお、S05の処理では、今回時刻で設定した仮想面が、互いに平行に対向する2つの対向仮想面Sna,Snb(n=1又は2又は3)を含んでおり、且つ、必要総並進外力Fallが、これらの対向仮想面Sna,Snbに垂直な成分Fall_nを有するような場合には、外力目標生成部27は、これらの対向仮想面Sna,Snbのうち、Fall_nと同じ向きの垂直抗力を発生可能な対向仮想面における仮想面垂直抗力成分Sna又はSnbを、Fall_nに一致させると共に、他方の対向仮想面における仮想面垂直抗力成分Snb又はSnaを“0”に設定する。
【0139】
また、S05の処理では、今回時刻で設定した仮想面が複数であり、且つ、必要総並進外力Fallが全ての仮想面に平行な成分を有する場合には、外力目標生成部27は、上記第1A必要条件を満たすように、各仮想面における仮想面垂直抗力成分N_iを決定した上で、前記第1B必要条件を満たしつつ、各仮想面における仮想面摩擦力成分F_iの大きさが、該仮想面で発生可能な摩擦力の上限値に応じた大きさとなるように(詳しくは該仮想面で発生可能な摩擦力の上限値が大きいほど、仮想面摩擦力成分F_iの大きさが大きくなるように)、各仮想面の仮想面摩擦力成分F_iを決定する。
【0140】
具体的には、外力目標生成部27は、今回時刻で設定した各仮想面の摩擦係数μ_iを、前記環境情報(詳しくは、該仮想面に対応する接触対象面の摩擦係数に関する環境情報)に基づいて設定する。この場合、各仮想面の摩擦係数μ_iは、該仮想面に対応する接触対象面の実際の摩擦係数と同一もしくはほぼ同一の値に設定してもよいが、余裕を見込んで、実際の摩擦係数よりも小さめに設定しておくようにしてもよい。
【0141】
なお、各仮想面の摩擦係数μ_iの設定は、目標歩容の各時刻で行なう必要はない。例えば、外力目標生成部27の処理を開始する前に、各接触対象面に対応付ける仮想面の摩擦係数を前記環境情報に基づいて事前に決定して記憶保持しておき、その記憶保持した摩擦係数から、S01で設定した各仮想面に対応させる摩擦係数を選定してもよい。
【0142】
さらに、外力目標生成部27は、各仮想面において、その仮想面に対応する摩擦係数μ_iを仮想面垂直抗力成分N_iの大きさ|N_i|に乗じてなる値(=μ_i*|N_i|)を、その仮想面で発生可能な摩擦力の上限値(以降、単に摩擦力上限値という)として設定する。
【0143】
なお、本明細書では、「*」は乗算記号として使用する。この場合、ベクトル同士の乗算演算における「*」は、外積(ベクトル積)を意味するものとする。
【0144】
そして、外力目標生成部27は、上記の如く設定した摩擦力上限値が大きい仮想面ほど、仮想面摩擦力成分F_iの大きさがより大きくなるように、各仮想面における仮想面摩擦力成分F_iを決定する。
【0145】
例えば、上記第1B必要条件を満たしつつ、各仮想面の仮想面摩擦力成分F_iの大きさの相互の比率が、各仮想面の摩擦力上限値の相互の比率に一致するように、各仮想面の仮想面摩擦力成分F_iが決定される。なお、この場合、例えば、Fallのうちの全ての仮想面に平行な成分を、最も大きな摩擦力上限値を有する仮想面の仮想面摩擦力成分に一致させ、他の仮想面の仮想面摩擦力成分を“0”に設定するようにしてもよい。
【0146】
歩容生成部23が前記壁登り歩容を生成する場合には、以上説明したS05の処理によって、各仮想面における仮想面垂直抗力成分N_i及び仮想面摩擦力成分F_iは例えば、次のように決定されることとなる。
【0147】
例えば、図3(b)に示した動作状態の時刻t2において、必要総並進外力Fallが、図11(a)に示す如く床面FLに対応する仮想面S3aに垂直な成分Fall_zと、壁面WL1,WL2にそれぞれ対応する仮想面(対向仮想面)S2a,S2bに垂直な成分Fall_yと、これらの仮想面S3a,S2a,S2bの全てに平行な成分Fall_xを有する状況を想定する。
【0148】
この状況においては、S05の処理では、仮想面S3aの仮想面垂直抗力成分N_3aと仮想面S2aの仮想面垂直抗力成分N_2aとがそれぞれ、Fall_z、Fall_yに一致する値に決定されると共に、仮想面S2bの仮想面垂直抗力成分N_2bが“0”に決定される。これにより前記第1A必要条件を満足する各仮想面垂直抗力成分N_3a,N_2a,N_2bが決定される。なお、ここでは、μ_3a*|N_3a|>μ_2a*|N_2a|であるとする。
【0149】
そして、仮想面S3aの仮想面摩擦力成分F_3aと仮想面S2aの仮想面摩擦力成分F_2aとの合力(=F_3a+F_2a)がFall_xに一致し、且つ、|F_3a|>|F_2a|となるように(例えば|F_2a|/|F_3a|=μ_2a*|N_2a|/(μ_3a*|N_3a|)となるように)、仮想面S3aの仮想面摩擦力成分F_3aと仮想面S2aの仮想面摩擦力成分F_2aとが決定されると共に、仮想面S2bの仮想面摩擦力成分F_2bが“0”に決定される。これにより前記第1B必要条件を満足する各仮想面摩擦力成分F_3a,F_2a,F_2bが決定される。
【0150】
なお、壁登り歩容における図3(c)の動作状態以降の時刻では、必要総並進外力Fallが例えば図11(a)に示した場合と同様であるとした場合、S05の処理では、仮想面S2aの仮想面垂直抗力成分N_2aが、FallのうちのFall_yに一致し、且つ、仮想面摩擦力成分F_2aが、FallのうちのFall_zとFall_xとの合力に一致するように仮想面垂直抗力成分N_2a及び仮想面摩擦力成分F_2aが決定されると共に、仮想面S2bの仮想面垂直抗力成分N_2b及び仮想面摩擦力成分F_2bはいずれも“0”に決定される。
【0151】
図5のフローチャートの説明に戻って、以上のようにS05の処理を実行した後、外力目標生成部27は、S07の処理を実行する。このS07では、外力目標生成部27は、今回時刻のS01で設定した全ての仮想面のそれぞれにおいて、仮想面摩擦力成分F_iの大きさが仮想面垂直抗力成分N_iの大きさに応じて設定される所定の上限値としての前記摩擦力上限値以下になるという第2必要条件を満足するか否かを判断する。すなわち、外力目標生成部27は、S01で設定した全ての仮想面のそれぞれにおいて、前記の如く決定した摩擦力上限値(=μ_i*|N_i|)を用い、|F_i|≦μ_i*|N_i|であるか否かを判断する。
【0152】
外力目標生成部27は、S07の判断結果が肯定的である場合には、各仮想面における仮想面必要並進力(暫定値)を決定する処理を終了し、図8に示すS19以降の処理を実行する。この場合、S05で決定された、各仮想面における仮想面垂直抗力成分N_iと仮想面摩擦力成分F_iとの組が、そのまま仮想面必要並進力(暫定値)として決定されることとなる。
【0153】
一方、S07の判断結果が否定的である場合には、外力目標生成部27は、次にS09の処理を実行する。このS09では、外力目標生成部27は、S07の判断結果が否定的となる仮想面が、前記対向仮想面Sna又はSnb(n=1又は2又は3)であるか否かを判断する。
【0154】
このS09の判断結果が否定的となる状況では、S07の判断結果が否定的となる仮想面における仮想面垂直抗力成分の大きさを、前記第1A必要条件を満たしつつ増加させることができず、ひいては該仮想面における仮想面摩擦力成分の大きさを、前記摩擦力上限値以下で増加させることができない。そこで、この場合には、外力目標生成部27は、S17において、目標運動が不適切である(目標運動を修正する必要がある)とし、図7〜図9のフローチャートの処理(目標外力及び目標外力作用点を決定する処理)を中止する。
【0155】
従って、本実施形態では、S05において前記第1A必要条件及び第1B必要条件を満足するように仮想面必要並進力(仮想面垂直抗力成分N_iと仮想面摩擦力成分F_iとの組)を決定した上で、S07、S09の判断処理によって目標運動が評価される。そして、S07及びS09の判断結果がいずれも否定的となる場合に、目標運動が不適切であると評価されることとなる。
【0156】
なお、S17で目標運動が不適切であると評価される場合、外力目標生成部27は、基本歩容要素生成部25にエラー情報を出力する。そして、基本歩容要素生成部25は、そのエラー情報に応じて、目標運動を作成し直す。
【0157】
一方、S09の判断結果が肯定的となる状況では、互いに平行な対向仮想面Sna,Snb(n=1又は2又は3)の仮想面垂直抗力成分N_na,N_nbの大きさを、前記第1A必要条件を満足しつつ(N_na+N_nbを一定に保ちつつ)、増加させることができる。すなわち、N_naとN_nbとは、互いに逆向きの垂直抗力であるので、それらの大きさを同じ量だけ増加させても、N_naとN_nbとの総和は変化しない。
【0158】
そこで、S09の判断結果が肯定的となる場合には、外力目標生成部27は、次に、S11の処理を実行する。このS11では、外力目標生成部27は、対向仮想面Sna,Snbの仮想面垂直抗力成分N_na,N_nbの大きさの増加量IN_nを、前記第2必要条件を満足させ得るように決定する。
【0159】
具体的には、外力目標生成部27は、対向仮想面Sna,Snbのうちの、S07の判断結果が否定的となった対向仮想面において、その仮想面摩擦力成分F_i(=F_na又はF_nb)の大きさから、S07の判断処理で用いた摩擦力上限値(=μ_i*|N_i|)を差し引いたものを、摩擦係数μ_iで除算してなる値を、上記増加量IN_nとして算出する。
【0160】
すなわち、外力目標生成部27は、次式01により、増加量IN_nを算出する。
【0161】
IN_n=(|F_i|−μ_i*|N_i|)/μ_i ……式01
次いで、外力目標生成部27は、S13の処理を実行する。このS13では、外力目標生成部27は、対向仮想面Sna,Snbの仮想面垂直抗力成分N_na,N_nbをそれぞれ、上記の如く決定した増加量IN_nだけ、S05の処理で決定した値の大きさよりも増加させ、その増加後の値を改めて、対向仮想面Sna,Snbのそれぞれの仮想面垂直抗力成分N_na,N_nbとして決定する。
【0162】
このS13までの処理によって、第1A必要条件、第1B必要条件及び第2必要条件を満足する仮想面必要並進力が決定されることとなる。
【0163】
次いで、外力目標生成部27は、S15の処理を実行する。このS15では、外力目標生成部27は、S13で大きさを増加させた仮想面垂直抗力成分N_na,N_nbの大きさのうちの小さい方の大きさ(=min(|N_na|,|N_nb|))があらかじめ設定された所定値IFC_n以下になるという第3必要条件を満足するか否かを判断する。
【0164】
ここで、min(|N_na|,|N_nb|)は、ロボット1の全体の並進運動に寄与しない内力に相当するものである。このような内力は、その大きさが大きくなると、ロボット1の関節アクチュエータ11が発生可能な動力(駆動トルク)のうち、該内力を発生させるために必要な負担分が増大し、ロボット1の所望の運動を行なうための利用し得る関節アクチュエータ11の動力(ロボット1の全体の並進運動を変化させるために利用し得る動力)が不足し易くなる。そこで、本実施形態では、この内力を所定値IFC_n以下に制限するために、S15の判断処理を実行する。
【0165】
なお、この所定値IFC_n(以降、内力上限値IFC_nという)は、例えば対向仮想面Sna,Snbにより発生する内力の向き(対向仮想面Sna,Snbの法線方向)に応じて設定される。ただし、内力上限値IFC_nを、内力の向きによらずに固定的な値に設定するようにしてもよい。
【0166】
このS15の判断結果が肯定的である場合には、外力目標生成部27は、S07の判断結果が肯定的である場合と同様に、各仮想面における仮想面必要並進力(暫定値)を決定する処理を終了し、図8に示すS19以降の処理を実行する。
【0167】
この場合、S07の判断結果が否定的となった仮想面を含む対向仮想面Sna,Snaを除く各仮想面については、S05で決定された仮想面垂直抗力成分N_iと仮想面摩擦力成分F_iとの組が、そのまま仮想面必要並進力(暫定値)として決定されることとなる。
【0168】
そして、上記対向仮想面Sna,Snaのそれぞれにおいては、S13で増加させた仮想面垂直抗力成分N_i(N_na,N_nb)と、S05で決定された仮想面摩擦力成分F_i(F_na,F_nb)との組が、仮想面必要並進力(暫定値)として決定されることとなる。
【0169】
また、S15の判断結果が否定的となる状況では、内力に関する第3必要条件を満足しつつ、ロボット1の目標運動を実現するために動力学的に必要な外力をロボット1に作用させることができないか、もしくは困難となる。このため、S15の判断結果が否定的となる場合には、外力目標生成部27は、S09の判断結果が否定的となる場合と同様に、S17において、目標運動が不適切である(目標運動を修正する必要がある)とし、図7〜図9のフローチャートの処理を中止する。
【0170】
補足すると、本実施形態では、S01で設定される仮想面が互いに平行な対向仮想面Sna,Snb(n=1又は2又は3)を含む場合に、S05の処理で決定される対向仮想面Sna,Snbの仮想面垂直抗力成分N_na,N_nbのうちの一方は“0”であるので、これらのN_na,N_nbの組は常に前記第3必要条件を満足する。従って、S07の判断結果が肯定的である場合に、S15の判断処理を行う必要は無い。
【0171】
以上のS05〜S15の処理によって、S09又はS15の判断結果が否定的となる場合(目標運動が不適切である場合)を除いて、前記第1A必要条件、第1B必要条件、第2必要条件及び第3必要条件を満足するように、各仮想面の仮想面必要並進力(暫定値)を構成する仮想面垂直抗力成分N_iと仮想面摩擦力成分F_iとの組が決定されることとなる。これにより、第1A必要条件、第1B必要条件、第2必要条件及び第3必要条件を満足しつつ、ロボット1の目標運動におけるロボット1の全体の並進運動(全体重心点の並進運動)を実現し得る仮想面必要並進力(暫定値)が決定されることとなる。
【0172】
従って、本実施形態では、S13までの処理によって、第1A必要条件、第1B必要条件及び第2必要条件を満足する仮想面必要並進力を決定した上で、S15の判断処理によって、再度、目標運動が評価される。そして、このS15の判断結果が否定的となる場合に、目標運動が不適切であると評価されることとなる。
【0173】
また、S07、S09、S15の判断結果がいずれも肯定的となる場合には、第1A必要条件、第1B必要条件、第2必要条件及び第3必要条件を満足し得る仮想面必要並進力を決定できることとなるので、基本歩容要素生成部25で生成された目標運動は、少なくとも該目標運動のうちのロボット1全体の並進運動(ロボット1の全体重心点の並進運動)を支障なく実現し得るように、ロボット1に外力を作用させることができる適切な目標運動であると言えることとなる。
【0174】
歩容生成部23が前記壁登り歩容を生成する場合に、以上説明したS05〜S15の処理によって仮想面垂直抗力成分N_i及び仮想面摩擦力成分F_iは例えば、次のように決定されることとなる。
【0175】
例えば、前記壁登り歩容のうちの図3(b)に示す動作状態の時刻t2において、S05の処理によって、前記した図11(a)に示した如く、各仮想面における仮想面垂直抗力成分N_i及び仮想面摩擦力成分F_i(i=3a,2a,2b)が決定された場合を想定する。この場合、仮想面S3aにおける仮想面摩擦力成分F_3aと、仮想面S2aにおける仮想面摩擦力成分F_2aとが、前記第2必要条件を満たしている場合(S07の判断結果が肯定的になる場合)には、S15までの処理によって最終的に決定される、各仮想面における仮想面必要並進力(N_i,F_i)は、S05で決定されたものに一致する。
【0176】
一方、例えば、仮想面S2aにおける仮想面摩擦力成分F_2aが前記第2必要条件を満たしていない場合には、図11(b)に示す如く、該仮想面S2aを含む対向仮想面S2a,S2bのそれぞれの仮想面垂直抗力成分N_2a,N_2bが、S05で決定された値よりも、S11で決定される増加量IN_2だけ増加してなる値に決定される。また、仮想面S3aの仮想面垂直抗力成分N_3a及び仮想面摩擦力成分F_3aと、仮想面S2a,S2bのそれぞれの仮想面摩擦力成分F2a,F2bの大きさは、S05で決定された値と同じ値に決定される。
【0177】
補足すると、本実施形態では、S01で設定される仮想面が互いに平行な2つの対向仮想面Sna,Snb(n=1又は2又は3)を含む場合に、S05の処理によって、一方の対向仮想面Sna又はSnbの仮想面垂直抗力成分及び仮想面摩擦力成分はいずれも常に“0”に決定される。但し、S05の処理において、必要総並進外力Fallが対向仮想面Sna,Snbに垂直な成分を有する場合に、前記第1A必要条件及び第3必要条件を満足する範囲で、両方の対向仮想面Sna,Snbの仮想面垂直抗力成分を“0”でない値に決定してもよい。そして、この場合において、必要総並進外力Fallが全ての仮想面に平行な成分を有する場合に、前記第1B必要条件を満足する範囲内で両方の対向仮想面Sna,Snbの仮想面摩擦力成分を“0”でない値に設定するようにしてもよい。
【0178】
なお、S05の処理において、上記の如く、両方の対向仮想面Sna,Snbの仮想面摩擦力成分を“0”でない値に設定するようにした場合には、その両方の対向仮想面Sna,Snbにおいて、S07の判断結果が否定的となる場合もあり得る。このような場合には、対向仮想面Sna,Snbの仮想面垂直抗力成分の増加量IN_nは、例えば次のように決定することができる。すなわち、各対向仮想面Sna,Snb毎に、前記したS11の処理によって暫定的に増加量IN_nを算出し、それらの暫定的な増加量IN_nのうちの大きい方を、対向仮想面Sna,Snbの仮想面垂直抗力成分の増加量IN_nとして決定する。
【0179】
以上説明したS15までの処理において、S07又はS15の判断結果が肯定的である場合(各仮想面における適切な仮想面必要並進力(暫定値)が決定された場合)には、外力目標生成部27は、次に、図8のフローチャートに示すS19の処理を実行する。このS19では、外力目標生成部27は、S15までに決定された仮想面必要並進力(暫定値)を各仮想面における仮想面上暫定外力作用点に作用させた場合(各仮想面における外力作用点を仮想面上暫定外力作用点に一致させた場合)に、該仮想面必要並進力によって前記モーメント基準点周りに発生するモーメントである仮想面必要並進力依存モーメントMsを算出する。
【0180】
ここで、上記仮想面上暫定外力作用点は、前記基本歩容要素生成部25で生成された今回時刻での各接触対象面上の暫定外力作用点を、あらかじめ定められた所定の規則に従って、該接触対象面に対応する仮想面上に投影してなる該仮想面上の外力作用点である。
【0181】
この場合、本実施形態では、上記投影のための規則は次のように設定されている。すなわち、図12に示す如く、ある接触対象面上の外力作用点を点Pr_i、該接触対象面に対応する仮想面上の外力作用点を点Ps_iとしたとき、仮想面上の外力作用点Ps_iは、例えば、接触対象面上の外力作用点Pr_iを通って、仮想面に垂直な直線(外力作用点Pr_iから仮想面への垂線)が該仮想面と交わる点に一致するものとされる。
【0182】
上記仮想面上暫定外力作用点は、このような規則に従って、今回時刻での各接触対象面上の暫定外力作用点を該接触対象面に対応する仮想面上に投影してなる該仮想面上の外力作用点である。
【0183】
S19の処理では、外力目標生成部27は、上記の如く決定される仮想面上暫定外力作用点の前記モーメント基準点に対する位置ベクトルを用いて、次式03によって、各仮想面の仮想面上暫定外力作用点に仮想面必要並進力(N_i,F_i)が作用した場合に、前記モーメント基準点の周りに発生する仮想面必要並進力依存モーメントMsを算出する。
【0184】
Ms=Σ(Irp_i*N_i)+Σ((Irp_i+Poff_i)*F_i) ……式03
この式03におけるIrp_iは、各仮想面(今回時刻のS01で設定された各仮想面)の仮想面上暫定外力作用点の前記モーメント基準点に対する位置ベクトル、Poff_iは仮想面上暫定外力作用点に対する接触対象面上の暫定外力作用点の位置ベクトル(又は該位置ベクトルのうちの仮想面に垂直な方向の成分ベクトル)である。
【0185】
さらに、(Irp_i*N_i)は、各仮想面における仮想面上暫定外力作用点の位置ベクトルIrp_iと仮想面垂直抗力成分N_iとのベクトル積、すなわち、N_iが仮想面暫定外力作用点に作用した場合にモーメント基準点周りに発生するモーメントである。
【0186】
そして、Σ(Irp_i*N_i)は、各仮想面毎のモーメント(Irp_i*N_i)の、全ての仮想面(今回時刻のS01で設定される全ての仮想面)についての総和、すなわち、各仮想面における仮想面垂直抗力成分N_iに起因してモーメント基準点周りに発生するトータルのモーメントを意味する。
【0187】
また、((Irp_i+Poff_i)*F_i)は、各仮想面における仮想面上暫定外力作用点の位置ベクトルIrp_iと仮想面摩擦力成分F_iとのベクトル積(Irp_i*F_i)、すなわち、F_iが仮想面暫定外力作用点に作用した場合にモーメント基準点周りに発生するモーメントと、仮想面上暫定外力作用点に対する接触対象面上の暫定外力作用点の位置ベクトルPoff_iとF_iとのベクトル積(Poff_i*F_i)、すなわち、F_iが接触対象面上の暫定外力作用点の周りに発生するモーメントとの総和のモーメントである。
【0188】
そして、Σ((Irp_i+Poff_i)*F_i)は、各仮想面毎のモーメント((Irp_i+Poff_i)*F_i)の、全ての仮想面(今回時刻のS01で設定される全ての仮想面)についての総和、すなわち、各仮想面における仮想面摩擦力成分F_iに起因してモーメント基準点周りに発生するトータルのモーメントを意味する。
【0189】
ここで、上記モーメント(Poff_i*F_i)に関して補足すると、接触対象面とこれに対応する仮想面とは、一般には同一面とはならないので、多くの場合、図12に示したように、接触対象面上の外力作用点Pr_iとこれに対応する仮想面上の外力作用点Ps_iとの位置ずれが生じる。そして、このような状況で、各仮想面における仮想面必要並進力が仮想面上暫定外力作用点に作用した場合にモーメント基準点周りに発生するモーメントと、該仮想面必要並進力と同じ並進力が該仮想面に対応する接触対象面上の暫定外力作用点に作用した場合にモーメント基準点周りに発生するモーメントとを比較すると、それらのモーメントには、上記位置ずれに起因する差異が生じる。この場合、接触対象面とこれに対応する仮想面とは、それらの法線方向が概ね同方向となるので、各仮想面毎の上記差異は、主に、仮想面摩擦力成分F_iに依存するものとなり、上記モーメント(Poff_i*F_i)にほぼ一致する。
【0190】
そこで、本実施形態では、上記位置ずれの影響を補償して、Msの信頼性を高めるために、上記モーメント(Poff_i*F_i)を付加した式03によって、仮想面必要並進力依存モーメントMsを算出するようにしている。
【0191】
なお、接触対象面上の暫定外力作用点と、これに対応する仮想面上暫定外力作用点との位置ずれが十分に微小なものとなる場合には、式03から、上記モーメント(Poff_i*F_i)の項を省略した式によって、仮想面必要並進力依存モーメントMsを算出するようにしてもよい。
【0192】
以上のS19の処理を、歩容生成部23が前記壁登り歩容を生成する場合に関して例示的に説明すると、例えば図11(b)に示した如く、仮想面必要並進力が決定された状況においては、図中に示す仮想面垂直抗力成分N_2a,N_2b,N_3aと仮想面摩擦力成分F_2a,F_3aとによって、モーメント基準点(例えば仮想面S2aとS3aとの交線上の点)の周りに発生するモーメントMsが上記式03によって算出されることとなる。この場合、図11(b)に示す例では、仮想面S2a,S2b,S3aは、それぞれ接触対象面WL1,WL2,FLに一致(又はほぼ一致)するので、式03における各仮想面S2a,S2b,S3a毎のPoff_i(Poff_2a,Poff_2b,Poff_3a)はいずれも“0”でよい。
【0193】
次いで、外力目標生成部27は、モーメント補償量を決定するための処理であるS21〜S37の処理を実行する。ここで、この処理の概要を説明しておく。
【0194】
上記の如くS19で算出された仮想面必要並進力依存モーメントMsが、仮に、S03で算出した必要総モーメント外力Mallに一致もしくはほぼ一致するような場合には、S15までの処理で決定された仮想面必要並進力(暫定値)と、基本歩容要素生成部25で生成された暫定外力作用点に対応する仮想面上暫定外力作用点とは、ロボット1の目標運動のうちの並進運動(全体重心点の並進運動)を適切に実現できるだけでなく、回転運動(モーメント基準点周りの角運動量を変化させる運動)をも適切に実現し得る外力として機能し得るものとなる。
【0195】
しかるに、S15までの処理では、ロボット1の目標運動のうちの回転運動に関する動力学については考慮されていないので、一般には、必要総モーメント外力Mallと仮想面必要並進力依存モーメントMsとの偏差(以下、モーメント偏差errMという)は“0”もしくはそれに近い値とはならない。
【0196】
S21〜S37の処理によって決定するモーメント補償量は、このモーメント偏差errMを解消する(“0”に近づける)ための操作量(外力に関する目標の操作量)である。
【0197】
本実施形態では、S21〜S37の処理によって決定するモーメント補償量には、3種類の補償量(第1〜第3補償量)があり、その第1補償量は、1つ以上の仮想面における仮想面上外力作用点の位置の修正量(仮想面上暫定外力作用点からの修正量。以降、仮想面上外力作用点修正量ということがある)により構成されるモーメント補償量である。この場合、第1補償量は、この第1補償量による修正後の仮想面上外力作用点に対応する接触対象面上の外力作用点が、前記存在許容領域内に存在するように決定される補償量である。
【0198】
また、第2補償量は、2つ以上の仮想面における仮想面必要並進力(暫定値)の仮想面摩擦力成分の修正量(以降、仮想面摩擦力成分修正量ということがある)から構成されるモーメント補償量である。この場合、第2補償量は、それを構成する仮想面摩擦力成分修正量の総和(合力)が“0”となるように決定される補償量である。
【0199】
また、第3補償量は、1つ以上の仮想面に垂直な軸周りのモーメント外力として該仮想面からロボット1に付加的に作用させるねじり力(以降、仮想面ねじり力ということがある)から構成されるモーメント補償量である。
【0200】
なお、上記第2補償量が、2つ以上の仮想面における仮想面摩擦力成分修正量から構成されるのは、該第2補償量による修正後における仮想面必要並進力の合力が前記第1必要条件を満足する必要があるからである。
【0201】
ここで、上記第1〜第3補償量のうち、第1補償量は、S15までの処理で決定した仮想面必要並進力(暫定値)の修正を必要とせずに、前記モーメント偏差を解消し、又は低減する機能を有する。従って、第1補償量によって、前記モーメント偏差を解消できるときには、目標運動を実現するための今回時刻での仮想面必要並進力として、S15までの処理で決定した仮想面必要並進力(暫定値)をそのまま使用することができる。
【0202】
一方、前記第2補償量又は第3補償量は、仮想面からロボット1に作用させる外力に、仮想面摩擦力成分修正量、又は仮想面ねじり力を追加的に付加するものであるから、該仮想面で発生可能な摩擦力の限界の制約を受ける。そして、その制約を満たすために、対向仮想面Sna,Snb(n=1又は2又は3)における仮想面垂直抗力成分N_na,N_nbの修正(ひいては、前記内力の修正)が必要となる場合もある。さらに、これらの仮想面垂直抗力成分N_na,N_nbの修正に起因して、一般には、前記モーメント基準点周りに発生するモーメントMsも変化する。
【0203】
そこで、本実施形態では、外力目標生成部27は、前記モーメント偏差errMを解消するためのモーメント補償量として、第1〜第3補償量のうちの第1補償量を優先的に決定する。そして、外力目標生成部27は、この第1補償量によってモーメント偏差を解消できない場合(より詳しくは、モーメント偏差errMの全体を解消し得るように第1補償量を決定した場合に、その第1補償量による修正後の仮想面上外力作用点に対応する接触対象面上の外力作用点が前記存在許容領域を逸脱する場合)に、第1補償量を制限する。さらに、モーメント偏差errMのうち、制限した第1補償量により解消できない部分を第2補償量及び第3補償量によって解消するように、第2補償量及び第3補償量の組を決定する。
【0204】
以上が、S21〜S37の処理(モーメント補償量の決定処理)の概要である。
【0205】
以下、このモーメント補償量の決定処理を具体的に説明すると、外力目標生成部27は、まず、S21の処理を実行する。このS21では、外力目標生成部27は、S03で算出した必要総モーメント外力Mallと、S19で算出した仮想面必要並進力依存モーメントMsとの偏差であるモーメント偏差errM(=Mall−Ms)を算出する。
【0206】
次いで、外力目標生成部27は、S23の処理を実行する。このS23では、外力目標生成部27は、仮想面上外力作用点の位置の変化(仮想面上暫定外力作用点からの変化)に対する仮想面必要並進力依存モーメントMsの変化の感度を表現するヤコビアンJを算出する。
【0207】
ここで、各仮想面上の外力作用点の位置の変化量(ベクトル)をΔrp_i、各仮想面上の外力作用点を仮想面上暫定外力作用点からΔrp_iだけ変化させたときの、接触対象面上の外力作用点(仮想面上の外力作用点を通って該仮想面に垂直な直線が該接触対象面と交わる点)の位置の変化量(ベクトル)をΔPoff_i、各仮想面上の外力作用点を仮想面上暫定外力作用点からΔrp_iだけ変化させたときの仮想面必要並進力依存モーメントMsの変化量(ベクトル)をΔMsとおく。このとき、前記式03に基づいて、次式05の関係式が得られる。
【0208】
ΔMs=Σ(Δrp_i*(N_i+Fi_)+ΔPoff_i*F_i) ……式05
この場合、ΔPoff_iは、Δrp_iの関数値(詳しくは、仮想面とこれに対応する接触対象面との間の姿勢差(空間的な角度差)と、Δrp_iとから一義的に規定される値)となるので、式05は、次式07に書き換えることができる。
【0209】
ΔMs=J*(Δrp_i) ……式07
この式07における(Δrp_i)は、全ての仮想面(今回時刻のS01で設定した全ての仮想面)に係わるΔrp_iを成分とする縦ベクトルである。そして、式07におけるJが、仮想面上外力作用点の位置の変化(仮想面上暫定外力作用点の位置からの変化)に対する仮想面必要並進力依存モーメントMsの変化の感度を表現するヤコビアン(行列)である。
【0210】
そこで、S23の処理では、外力目標生成部27は、各仮想面の仮想面必要並進力N_i,F_iと、式05とに基づいてヤコビアンJを算出する。なお、この場合、ΔPoff_iと、Δrp_iとの間の関係は、各仮想面とこれに対応する接触対象面との姿勢差に基づいて決定される。
【0211】
次いで、外力目標生成部27は、S25の処理を実行する。このS25処理は、式07のΔMsをS21で算出したモーメント偏差errMに一致させた場合に、式07の関係を満たす(Δrp_i)を、errMを解消するための各仮想面における仮想面上外力作用点修正量Δrpc_iの組として算出する処理である。
【0212】
ここで、式07におけるヤコビアンJは、一般には正則行列ではない。そこで、外力目標生成部27は、S23で求めたヤコビアンJの擬似逆行列Jinvを算出し、この擬似逆行列Jinvを、モーメント偏差errMに乗じることによって、各仮想面の仮想面上外力作用点修正量Δrpc_iを算出する。この場合、擬似逆行列Jinvは、例えば、各仮想面に対応するΔrpc_iの二乗値又は絶対値の平均値が最小になるように決定される。
【0213】
なお、例えば、各仮想面に対応する接触対象面における外力作用点の存在許容領域の面積等に応じて、各仮想面に対応するΔrpc_iの重み付けをしておき、各仮想面に対応するΔrpc_iの二乗値又は絶対値の重み付き平均値が最小になるように擬似逆行列Jinvを決定するようにしてもよい。
【0214】
次いで、外力目標生成部27は、S27の処理を実行する。このS27では、外力目標生成部27は、各仮想面上の外力作用点の位置を、仮想面上暫定外力作用点の位置Irp_iからS25で求めたΔrpc_iによって修正した場合に、全ての仮想面において、その修正後の仮想面上外力作用点の位置(Irp_i+Δrpc_i)が、外力作用点存在領域条件を満足するか否かを判断する。
【0215】
ここで、上記外力作用点存在領域条件は、Δrpc_iによる修正後の各仮想面の仮想面上外力作用点の位置が、基本歩容要素生成部25で生成された今回時刻での各接触対象面の外力作用点の存在許容領域に対応する仮想面上の存在許容領域(該接触対象面上の外力作用点の存在許容領域を仮想面上に投影してなる領域)内に存在するという条件である。この場合、接触対象面上の外力作用点の存在許容領域の、仮想面上への投影は、図12に示す如く、接触対象面上の外力作用点の仮想面上への投影の場合と同様に行なわれる。
【0216】
なお、外力作用点存在領域条件は、Δrpc_iによる修正後の各仮想面の仮想面上外力作用点を、該仮想面に対応する接触対象面に逆投影してなる点が、該接触対象面上の外力作用点の存在許容領域内に存在するという条件と等価である。
【0217】
S27の判断結果が肯定的である場合には、S15までの処理で決定された各仮想面の仮想面必要並進力を修正せずとも、各仮想面の外力作用点の位置を仮想面上暫定外力作用点から修正する(ひいては各接触対象面の外力作用点の位置を暫定外力作用点から修正する)だけで、前記モーメント偏差errMを解消できることとなる。
【0218】
そこで、この場合には、外力目標生成部27は、S29の処理を実行し、今回時刻での図7〜図9のフローチャートの処理を終了する。このS29の処理では、外力目標生成部27は、今回時刻での各接触対象面からロボット1への目標外力と目標外力作用点とを確定する。
【0219】
具体的には、外力目標生成部27は、S15までの処理で決定した各仮想面における仮想面必要並進力(N_i,F_i)を、該仮想面とこれに対応する接触対象面との姿勢差に応じて、該接触対象面からロボット1に作用させる並進力に変換し、この並進力を目標外力の構成要素である並進外力の目標値として決定する。
【0220】
この場合、各仮想面毎の上記変換は、例えば次のように行なわれる。すなわち、外力目標生成部27は、各仮想面の姿勢(法線ベクトルの向き)を該仮想面に対応する接触対象面の姿勢(法線ベクトルの向き)に変換する回転変換行列Rθを、これらの仮想面と接触対象面との姿勢差に基づき算出する。そして、外力目標生成部27は、この回転変換行列Rθを仮想面必要並進力(ベクトル)に乗じることによって、該仮想面必要並進力を、接触対象面からロボット1への並進外力の目標値に変換する。
【0221】
従って、本実施形態では、各接触対象面からロボット1に作用させるべき並進外力の目標値(目標並進力)は、該接触対象面に対応する仮想面における仮想面垂直抗力成分N_iと仮想面摩擦力成分F_iとそれぞれ同じ大きさの垂直抗力成分(接触対象面に垂直な成分)と摩擦力成分(接触対象面に平行な成分)とから構成される並進外力として決定される。
【0222】
また、外力目標生成部27は、S25で決定したΔrpc_iをモーメント補償量として確定する。そして、外力目標生成部27は、このΔrpc_iによる修正後の各仮想面の仮想面上外力作用点を、該仮想面に対応する接触対象面上に逆投影してなる点(本実施形態では、修正後の仮想面上外力作用点を通って該仮想面に垂直な直線が、該仮想面に対応する接触対象面と交わる点)を、該接触対象面上の目標外力作用点として決定する。
【0223】
なお、S29の処理では、目標外力の構成要素のうちの、各接触対象面のねじり力の目標値(目標ねじり力)は、いずれの接触対象面についても“0”とされる。
【0224】
一方、前記S27の判断結果が否定的である場合には、モーメント偏差errMを解消するための各仮想面における仮想面上外力作用点修正量Δrpc_iを、前記外力作用点存在領域条件を満足するように制限する必要がある。
【0225】
そこで、外力目標生成部27は、次に、S31の処理を実行する。このS31では、外力目標生成部27は、外力作用点存在領域条件を満足するように、各仮想面における仮想面上外力作用点修正量Δrpc_iを制限してなる仮想面上外力作用点制限修正量Δrpc_lim_iを第1補償量として決定する。
【0226】
この場合、外力作用点存在領域条件を満足する仮想面については、S25で決定された修正量Δrpc_iがそのまま、仮想面上外力作用点制限修正量Δrpc_lim_iとして決定される。
【0227】
一方、外力作用点存在領域条件を満足しない仮想面については、外力目標生成部27は、次のように仮想面上外力作用点制限修正量Δrpc_lim_iを決定する。
【0228】
図13を参照して、修正前の仮想面上外力作用点(接触対象面上の暫定外力作用点を仮想面上に投影してなる点)をPs1_i、この修正前の仮想面上外力作用点Ps1_iをS25で決定した仮想面上外力作用点修正量Δrpc_iにより修正してなる点をPs2_iとおく。この場合、外力作用点存在領域条件を満足しない仮想面については、修正後の仮想面上外力作用点Ps2_iが、図示の如く、仮想面上の外力作用点許容領域から逸脱することとなる。
【0229】
なお、図13では、図示の便宜上、仮想面上の外力作用許容領域を単純な楕円状の領域として記載しているが、その領域の形状は他の形状であってもよいことはもちろんである。
【0230】
このとき、外力目標生成部27は、上記仮想面上外力作用点制限修正量Δrpc_lim_iにより点Ps1_iを修正してなる点が、例えば、修正前の点Ps1_iとΔrpc_iによる修正後の点Ps2_iとを結ぶ線分上で、仮想面上の外力作用点許容領域の境界に存する点Ps_lim_iに一致するように、仮想面上外力作用点制限修正量Δrpc_lim_iを決定する。
【0231】
なお、上記のようにΔrpc_lim_iを決定することに代えて、例えば、仮想面上外力作用点制限修正量Δrpc_lim_iにより点Ps1_iを修正してなる点が、仮想面上の外力作用点許容領域の境界にする点のうち、Δrpc_iによる修正後の点Ps2_iに最も近い点(点Ps2_iとの距離が最小となる点)に一致するように、仮想面上外力作用点制限修正量Δrpc_lim_iを決定してもよい。
【0232】
以降の説明では、上記の如くS31で決定される仮想面上外力作用点制限修正量Δrpc_lim_iによる修正後の各仮想面上の外力作用点を、仮想面上制限修正後外力作用点という。
【0233】
以上の如くS31の処理を実行した後、外力目標生成部27は、S33の処理を実行する。このS33では、外力目標生成部27は、前記モーメント偏差errMのうち、S29で決定した各仮想面における仮想面上外力作用点制限修正量Δrpc_lim_iによって解消できない偏差成分としての残偏差成分errM'を算出する。
【0234】
具体的には、外力目標生成部27は、制限前の仮想面上外力作用点修正量Δrpc_iと仮想面上外力作用点制限修正量Δrpc_lim_iとの差(=Δrpc_i−Δrpc_lim_i)である仮想面上外力作用点修正誤差Δrpc_err_iを算出する。そして、外力目標生成部27は、前記式05におけるΔrp_iを、Δrpc_err_iに置き換えてなる次式09によって、残偏差成分errM’を算出する。
【0235】
errM'=Σ(Δrpc_err_i*(N_i+Fi_)+ΔPoff_i*F_i) ……式09
この場合、式09におけるΔPoff_iは、Δrpc_err_iに応じて決定される値であり、前記式05におけるΔPoff_iとΔrp_iとの間の関係に従って、Δrp_i=Δrpc_err_iとしたときの、ΔPoff_iの値として決定される。
【0236】
なお、例えば、前記式05のΔrp_iを、Δrpc_limt_iに置き換えた式によって算出されるモーメント変化量ΔMs、すなわち、各仮想面上の外力作用点の位置を仮想面上暫定外力作用点からΔrpc_limt_iだけ修正した場合に生じるモーメント基準点周りのモーメント変化量を算出し、このモーメント変化量をモーメント偏差errMから差し引くことによって、残偏差成分errM'を算出するようにしてもよい。この場合、ΔPoff_iは、前記式05におけるΔPoff_iとΔrp_iとの間の関係に従って、Δrp_i=Δrpc_limt_iとしたときの、ΔPoff_iの値として決定すればよい。
【0237】
以上の如くerrMのうちの残偏差成分errM'を算出した後、次に、外力目標生成部27は、図9に示すS35、S37の処理を順次実行することによって、第2補償量としての前記仮想面摩擦力成分修正量f_iと第3補償量としての仮想面ねじり力R_iとを決定する。
【0238】
S35では、各仮想面(S01で設定した各仮想面)に任意の値の前記仮想面摩擦力成分修正量f_iと前記仮想面ねじり力R_iとを付加した場合(Δrpc_lim_iによる修正後の仮想面上制限修正後外力作用点にf_iとR_iを追加的に作用させた場合)にそれらのf_i,R_iによって前記モーメント基準点周りに発生するモーメントMfrと、f_i,R_iとの間の関係を次式11により表現するヤコビアン(行列)Jfrを算出する。
【0239】
Mfr=Jfr*(f_i R_i)T ……式11
この式11における(f_i R_i)Tは、f_i及びR_iを成分とする縦ベクトルである。ここで、前記仮想面摩擦力成分修正量f_iは、それらの総和(合力)が“0”になるように決定されるべきモーメント補償量である。
【0240】
従って、仮想面摩擦力成分修正量f_iのうち、任意の1つの仮想面に対応する仮想面摩擦力成分修正量は、他の残りの仮想面摩擦力成分修正量の総和の符号を反転させてなる並進力として一義的に定まる。このため、式11における縦ベクトル(f_i R_i)Tの成分としてのf_iは、任意の1つの仮想面に対応するf_i(以降、これをf_i0とおく)を除いたものである。そして、ヤコビアンJfrは、f_i0が他のf_iの総和の符号を反転させてなる並進力であるとして、この並進力によるモーメント基準点周りのモーメント成分が式11の右辺の演算により算出されるMfrに含まれるように決定される。
【0241】
次いで、S37では、外力目標生成部27は、式11のMfrを残偏差成分errM'に一致させた場合に、式11の関係を満たすf_i及びR_iの組(f_i R_i)Tを算出する。
【0242】
ここで、式11におけるヤコビアンJfrは、一般には正則行列ではないので、外力目標生成部27は、S37で求めたヤコビアンJfrの擬似逆行列Jfr_invを算出し、この擬似逆行列Jfr_invを、残偏差成分errM'に乗じることによって、f_i及びR_iの組を算出する。この場合、擬似逆行列Jfr_invは、例えば、式11の縦ベクトル(f_i R_i)Tを組成するf_i及びR_iのそれぞれの2乗値又は絶対値の平均値が最小になるように決定される。なお、上記f_i0は、上記の如く算出したf_iの総和の符号を反転させてなる値として決定される。
【0243】
補足すると、上記擬似逆行列Jfr_invは、例えば、式11の縦ベクトル(f_i R_i)Tを組成するf_i及びR_iのそれぞれの2乗値又は絶対値の重み付き平均値が最小になるように決定してもよい。例えば、前記摩擦力上限値が大きい仮想面ほど、その仮想面におけるf_iの大きさが相対的に大きくなるように、各f_iに対応する重みを設定し、その重みを用いた上記重み付き平均値が最小になるように、Jfr_invを決定するようにしてもよい。
【0244】
また、例えば、S01で設定された仮想面が対向仮想面Sna,Snbを含む場合において、仮想面垂直抗力成分N_iの大きさが小さい方の対抗仮想面Sna又はSnbにおける仮想面摩擦力成分修正量f_na又はfnbを“0”に設定するようにしてもよい。
【0245】
歩容生成部23が、前記壁登り歩容を生成する場合においては、上記S35、S37の処理は、例えば次にように行なわれる。
【0246】
例えば、図11(b)に示した動作状態において、モーメント偏差errMのうちの残偏差成分errM'が存在する場合を想定する。そして、床面FL及び壁面WL1,WL2にそれぞれ対応する各仮想面S3a,S2a,S2bにおける仮想面上制限修正後外力作用点の位置ベクトル(モーメント基準点に対する位置ベクトル)をそれぞれrp_3a,rp_2a,rp_2bとおく。このとき、各仮想面S3a,S2a,S2bにおける仮想面上摩擦力成分修正量f_3a,f_2a,f_2b及び仮想面ねじり力R_3a,R_2a,R_2bと、これらのf_3a,f_2a,f_2b,R_3a,R_2a,R_2bによってモーメント基準点周りに発生するモーメントMfrとの間の関係は、次式11aにより表される。
【0247】
Mfr=rp_3a*f_3a+rp_2a*f_2a+rp_2b*f_2b+R_3a+R_2a+R_2b
……式11a
なお、式11aにおける(rp_3a*f_3a),(rp_2a*f_2a),(rp_2b*f_2b)はそれぞれベクトル積である。
【0248】
この場合、仮想面摩擦力成分修正量に関しては、f_3a+f_2a+f_2b=0であるから、f_2b=−(f_3a+f_2a)となる。従って、式11aは、例えばf_2bを含まない次式11bに書き換えられる。
【0249】
Mfr=(rp_3a−rp_2b)*f_3a+(rp_2a−rp_2b)*f_2a+R_3a+R_2a+R_2b
……式11b
従って、S35では、この式11bの右辺を、座標成分で表して整理することで、前記式11におけるヤコビアンJfrが求められることとなる。
【0250】
そして、S37では、このヤコビアンJfrの擬似逆行列Jfr_invが前記した如く算出され、このJfr_invを、残偏差成分errM’に乗じることによって、f_3a,f_2a,R_3a,R_2a,R_2bが算出される。さらに、f_3a,f_2aから、f_2b=−(f_3a+f_2a)という関係に従って、f_3bが算出される。
【0251】
補足すると、例えば、前記壁登り歩容における図3(c)の動作状態以降の時刻、すなわち、仮想面が対向仮想面S2a,S2bの2つだけになる状況では、式11aの右辺から(rp_3a*f_3a)と、R_3aとを除去してなる式と、f_2a+f_2b=0という関係とに基づいて、上記と同様に、ヤコビアンJfrを決定することができる。
【0252】
以上のS37までの処理によって、前記S27の判断結果が否定的となる場合には、前記第1A必要条件及び第1B必要条件を満足し、且つ、ロボット1の目標運動のうちのロボット1全体の回転運動(モーメント基準点周りの角運動量を変化させる回転運動)を実現し得る仮想面必要並進力(f_iによる修正後の仮想面必要並進力)と、仮想面上外力作用点(Δrpc_lim_iによる修正後の仮想面上外力作用点)と、仮想面上ねじり力R_iとの組が実質的に決定されることとなる。ただし、この場合、f_iによる仮想面必要並進力の修正が行なわれるので、その修正後の仮想面必要並進力の仮想面摩擦力成分は、前記第2必要条件を満足するとは限らない。また、仮想面上ねじり力R_iは、それを付加する仮想面とロボット1との間の摩擦力、ひいては、該仮想面に対応する接触対象面とロボット1との間の摩擦力を必要とする。
【0253】
そこで、以上の如く仮想面摩擦力成分修正量f_iと仮想面ねじり力R_iとを決定した後、次に、外力目標生成部27は、S39の判断処理を実行する。このS39では、外力目標生成部27は、各仮想面において、f_iによる修正後の仮想面摩擦力成分F_i(S15までの処理で決定されたF_iとf_iとの総和の摩擦力成分)が、前記第2必要条件を満足するか否かを判断すると共に、R_iの大きさが、仮想面垂直抗力成分N_iの大きさに応じて設定される所定の上限値以下になるという第4必要条件を満足するか否かを判断する。
【0254】
ここで、上記第4必要条件における所定の上限値は、R_iを付加する仮想面の摩擦力によって発生可能なねじり力の大きさの上限値として、各仮想面毎に設定される値である。この上限値(以降、ねじり力上限値という)は、本実施形態では、各仮想面に対応する接触対象面の性状に関する環境情報に基づいて各仮想面毎に設定した係数μr_i(以降、ねじり力係数という)を、仮想面垂直抗力成分N_iの大きさ|N_i|に乗じてなる値(=μr_i*|N_i|)に設定される。
【0255】
なお、本実施形態では、外力目標生成部27の処理を開始する前に、各接触対象面に対応付ける仮想面のねじり力係数が前記環境情報に基づいて事前に決定されると共に記憶保持される。そして、S39では、その記憶保持したねじり力係数から、R_iを付加する仮想面におけるねじり力係数が選定される。
【0256】
補足すると、各仮想面におけるねじり力係数μr_iは、該仮想面に対応する接触対象面の性状に関する環境情報だけでなく、該仮想面における仮想面摩擦力成分のF_i大きさや、ロボット1と接触対象面との接触面の大きさ等を考慮して、目標歩容の各時刻で決定するように設定してもよい。
【0257】
S39の判断結果が肯定的である場合には、外力目標生成部27は、S41の処理を実行し、今回時刻での図7〜図9のフローチャートの処理を終了する。このS41の処理では、外力目標生成部27は、今回時刻での各接触対象面からロボット1の目標外力と目標外力作用点とを確定する。
【0258】
具体的には、外力目標生成部27は、S15までの処理で決定した各仮想面における仮想面必要並進力(N_i,F_i)と、S37の処理で決定した仮想面摩擦力成分修正量f_iとから、今回時刻での各仮想面における仮想面必要並進力を確定する。この場合、S15までの処理で決定した仮想面垂直抗力成分N_iがそのまま該仮想面における仮想面垂直抗力成分として確定されると共に、S15までの処理で決定した仮想面摩擦力成分F_iに仮想面摩擦力成分修正量f_iを加え合わせてなる摩擦力成分が、該仮想面における仮想面摩擦力成分として確定される。
【0259】
なお、f_i=0となる仮想面については、結果的には、S15までの処理で決定した仮想面必要並進力(N_i,F_i)がそのまま該仮想面における仮想面必要並進力として確定されることとなる。
【0260】
また、外力目標生成部27は、S37の処理で決定した仮想面ねじり力R_iを、このR_iに対応する仮想面におけるねじり力として確定する。
【0261】
そして、外力目標生成部27は、以上の如く確定した各仮想面における仮想面必要並進力を、該仮想面とこれに対応する接触対象面との姿勢差に応じて、該接触対象面からロボット1に作用させる並進力に変換し、この並進力を目標外力の構成要素である並進外力の目標値(目標並進力)として決定する。同様に、外力目標生成部27は、各仮想面における仮想面ねじり力を、該仮想面に対応する接触対象面からロボット1に作用させるねじり力に変換し、この変換したねじり力を、目標外力の構成要素であるねじり力の目標値(目標ねじり力)として決定する。
【0262】
なお、上記変換は、図8のS29の処理に関して説明した変換と同じであり、上記姿勢差に応じて決定される前記回転変換行列Rθを用いて行なわれる。
【0263】
また、外力目標生成部27は、S29の処理の場合と同様に、S31で決定した仮想面上外力作用点制限修正量Δrpc_lim_iによる修正後の各仮想面上の外力作用点を該仮想面に対応する接触対象面上に逆投影してなる点を、該接触対象面上の目標外力作用点として決定する。
【0264】
一方、S39の判断結果が否定的である場合、すなわち、第2必要条件又は第4必要条件を満足しない仮想面が存在する場合には、ロボット1の目標運動のうちの回転運動を実現する上で、その仮想面における摩擦力が不足する。この場合には、外力目標生成部27は、次にS43の処理を実行する。このS43では、外力目標生成部27は、S39の判断処理で、第2必要条件又は第4必要条件を満たさない仮想面が、対向仮想面Sna又はSnb(n=1又は2又は3)であるか否かを判断する。
【0265】
このS43の判断結果が否定的となる状況では、第2必要条件又は第4必要条件を満足しない仮想面における仮想面垂直抗力成分の大きさを、前記第1A必要条件を満たしつつ増加させることができず、ひいては該仮想面における仮想面摩擦力成分の大きさを、前記摩擦力上限値以下で増加させることができない。そこで、この場合には、外力目標生成部27は、S47において、目標運動が不適切である(目標運動を修正する必要がある)とし、図7〜図9のフローチャートの処理を中止する。なお、この場合、外力目標生成部27は、基本歩容要素生成部25にエラー情報を出力する。そして、基本歩容要素生成部25は、そのエラー情報に応じて、目標運動を作成し直す。
【0266】
従って、本実施形態では、S37までの処理によって、前記S27の判断結果が否定的となる場合には、前記第1A必要条件及び第1B必要条件を満足し、且つ、ロボット1の目標運動のうちのロボット1全体の回転運動を実現し得る仮想面必要並進力(f_iによる修正後の仮想面必要並進力)と、仮想面上外力作用点(Δrpc_lim_iによる修正後の仮想面上外力作用点)と、仮想面上ねじり力R_iとの組を決定した上で、S39,S43の判断処理によって、目標運動が評価される。そして、このS39及びS43の判断結果がいずれも否定的となる場合に、目標運動が不適切であると評価されることとなる。
【0267】
一方、S43の判断結果が肯定的となる状況では、第2必要条件又は第4必要条件を満たさない仮想面を含む、互いに平行な対向仮想面Sna,Snb(n=1又は2又は3)の仮想面垂直抗力成分N_na,N_nbの大きさを、前記第1A必要条件を満足しつつ(N_na+N_nbを一定に保ちつつ)、増加させることができる。
【0268】
そこで、S43の判断結果が肯定的となる場合には、外力目標生成部27は、次に、S45の処理を実行する。このS45では、外力目標生成部27は、対向仮想面Sna,Snbの仮想面垂直抗力成分N_na,N_nbの大きさの増加量IN_nを、前記第2必要条件及び第4必要条件を満足させ得るように決定する。
【0269】
具体的には、外力目標生成部27は、第2必要条件を満足しない仮想面を含む対向仮想面Sna,Snbのそれぞれにおいて、前記仮想面摩擦力成分修正量f_iによる修正後の仮想面摩擦力成分F_iの大きさから、S39の判断処理で用いた摩擦力上限値(=μ_i*|N_i|)を差し引いたものを、摩擦係数μ_iで除算してなる値(=(|F_i|−μ_i*|N_i|)/μ_i)を、上記増加量IN_nの候補値として算出する(以下、このように算出されたIN_nの候補値を摩擦力成分補償用候補値という)。この場合、この摩擦力成分補償用候補値IN_nは、第2必要条件を満足しない仮想面を含む対向仮想面Sna,Snbのそれぞれ毎に算出される。
【0270】
例えば、歩容生成部23が前記壁登り歩容を生成する場合において、壁面WL1,WL2にそれぞれ対応する対向仮想面S2a,S2bのうちの一方又は両方がS39の判断処理で第2必要条件を満足しないものとなった場合には、これらの対向仮想面S2a,S2bのぞれぞれにおいて、IN_2の摩擦力成分補償用候補値((|F_2a|−μ_2a*|N_2a|)/μ_2a、(|F_2b|−μ_2b*|N_2b|)/μ_2b)が算出される。
【0271】
また、外力目標生成部27は、第4必要条件を満足しない仮想面を含む対向仮想面Sna,Snbのそれぞれにおいて、前記仮想面ねじり力R_iの大きさから、S39の判断処理で用いたねじり力上限値(=μr_i*|N_i|)を差し引いたものを、ねじり力係数μr_iで除算してなる値(=(|R_i|−μr_i*|N_i|)/μr_i)を、上記増加量IN_nの候補値として算出する(以降、このように算出されたIN_nの候補値をねじり力補償用候補値という)。この場合、摩擦力成分補償用候補値と同様に、ねじり力補償用候補値IN_nは、第4必要条件を満足しない仮想面を含む対向仮想面Sna,Snbのそれぞれ毎に算出される。
【0272】
そして、外力目標生成部27は、摩擦力成分補償用候補値及びねじり力補償用候補値のうちの摩擦力成分補償用候補値だけを算出した対向仮想面Sna,Snbについては、各対向仮想面Sna,Snbのそれぞれの摩擦力成分補償用候補値のうちのいずれか大きい方を、該対向仮想面Sna,Snbにおける仮想面垂直抗力成分N_na,N_nbの大きさの増加量IN_nとして決定する。
【0273】
また、外力目標生成部27は、摩擦力成分補償用候補値及びねじり力補償用候補値のうちのねじり力補償用候補値だけを算出した対向仮想面Sna,Snbについては、各対向仮想面Sna,Snbのそれぞれのねじり力補償用候補値のうちのいずれか大きい方を、該対向仮想面Sna,Snbにおける仮想面垂直抗力成分N_na,N_nbの大きさの増加量IN_nとして決定する。
【0274】
また、外力目標生成部27は、摩擦力成分補償用候補値及びねじり力補償用候補値の両方を算出した対向仮想面Sna,Snbについては、各対向仮想面Sna,Snbのそれぞれの摩擦力成分補償用候補値及びねじり力補償用候補値のうちの最も大きい候補値を、該対向仮想面Sna,Snbにおける仮想面垂直抗力成分の増加量IN_nとして決定する。
【0275】
ここで、S45で上記の如く決定した増加量IN_nだけ対向仮想面Sna,Snbにおける仮想面垂直抗力成分N_na,N_nbを増加させた場合、対向仮想面Sna,Snbに直交する方向の内力が増加することとなる。また、N_na,N_nbの増加に伴い、モーメント基準点周りに発生するモーメントが一般には変化してしまう。そこで、本実施形態では、外力目標生成部27は、S45の処理を実行した後、図7のS13からの処理を改めて実行する。
【0276】
以上が、本実施形態における外力目標生成部27の処理の詳細である。
【0277】
以上説明した外力目標生成部27の処理によって、まず、S05〜S15の処理によって、ロボット1の目標運動のうちの、ロボット1全体の並進運動を実現するために必要な仮想面必要並進力(N_i,F_i)が暫定的に決定される。この場合、前記モーメント基準点周りに発生するモーメントを考慮することなく、第1必要条件(第1A必要条件、第1B必要条件)、第2必要条件、及び第3必要条件を満足することを指針として、仮想面必要並進力(N_i,F_i)が暫定的に決定される。この場合、各仮想面は、その相互の姿勢関係があらかじめ定まっているので、ロボット1全体の並進運動を実現し得る仮想面必要並進力(N_i,F_i)の暫定値を簡素なアルゴリズムによって効率よく決定することができる。
【0278】
また、S07、S09の判断結果がいずれも否定的となる場合、あるいは、S07、S09、S15の判断結果がそれぞれ、否定的、肯定的、否定的となる場合に、基本歩容要素生成部25で生成された目標運動が不適切であると評価され、外力目標生成部27の処理が中止される。このため、目標運動のうちのロボット1全体の並進運動を実現するために、接触対象面とロボット1との間の摩擦力が過大になりやすい目標運動、あるいは、ロボット1の内力が過大になりやすい目標運動を、簡素なアルゴリズムで効率よく不適切であると評価することができる。ひいては、目標歩容を生成するために、無駄な演算処理を実行することとなるのを抑制することができる。
【0279】
そして、上記のようにロボット1全体の並進運動を実現し得る仮想面必要並進力(N_i,F_i)の暫定値を決定した上で、S19〜S37の処理(S29の処理を除く)によって、ロボット1全体の回転運動(モーメント基準点周りの角運動量の変化を生じる回転運動)を実現するための付加的な操作量(モーメント補償量)として、前記仮想面上外力作用点修正量Δrpc_iが決定され、あるいは、前記仮想面上外力作用点制限修正量Δrpc_lim_iと仮想面摩擦力成分修正量Δf_iと仮想面ねじり力R_iとの組が決定される。これらの、モーメント補償量は、それによる修正対象の修正の前後で、仮想面上必要並進力の合力を変化させることのない操作量である。
【0280】
このため、S05〜S15の処理によって決定した仮想面必要並進力(N_i,F_i)の暫定値と、S19〜S37の処理(S29の処理を除く)によって決定したモーメント補償量とを単純に組合わせるだけで、ロボット1全体の並進運動と回転運動との両方を適切に実現できる仮想面上必要並進力と仮想面上ねじり力と仮想面上外力作用点との組(仮想面上ねじり力が“0”になる場合を含む)を決定することができることとなる。そして、この仮想面上必要並進力と仮想面上ねじり力と仮想面上外力作用点との組から、各接触対象面からロボット1に作用させるべき目標外力(目標並進力及び目標ねじり力)と目標外力作用点とを効率よく適切に決定できることとなる。
【0281】
なお、この場合、S39、S43の判断結果がいずれも否定的となる場合に、基本歩容要素生成部25で生成された目標運動が不適切であると評価され、外力目標生成部27の処理が中止される。このため、目標運動のうちのロボット1全体の並進運動と回転運動とを実現するために、接触対象面とロボット1との間の摩擦力が過大になりやすい目標運動を、効率よく不適切であると評価することができる。ひいては、目標歩容を生成するために、無駄な演算処理を実行することとなるのを抑制することができる。
【0282】
また、本実施形態では、外力目標生成部27の処理によって、S09、S15、S43のいずれかの判断結果が否定的となる場合(目標運動が不適切である場合)を除いて、今回時刻での各接触対象面に対応する各仮想面における最終的な仮想面必要並進力(仮想面垂直抗力成分N_iと仮想面摩擦力成分F_iとの組)は、結果的に、前記第1A必要条件、第1B必要条件、第2必要条件及び第3必要条件を満足するように決定される。
【0283】
そして、各仮想面に対応する接触対象面における並進外力の目標値(目標並進力)は、その垂直抗力成分(接触対象面に垂直な成分)と摩擦力成分(接触対象面に平行な成分)とが、それぞれ、該仮想面における仮想面垂直抗力成分N_iと仮想面摩擦力成分F_iとに一致するように決定される。
【0284】
このため、各接触対象面からロボット1への目標並進力は、可能な限り、各接触対象面における目標並進力のうちの垂直抗力成分の合力によって前記必要総並進外力を実現すると共に、該目標並進力のうちの摩擦力成分を、該接触対象面で発生可能な摩擦力の大きさの限界内で可能な限り小さくなるように、各接触対象面からロボット1への目標並進力を決定できる。
【0285】
特に、本実施形態では、モーメント補償量として、仮想面上外力作用点修正量Δrpc_i(第1補償量)が優先的に決定される。すなわち、S27の判断結果が肯定的となる場合には、モーメント補償量には、仮想面摩擦力成分修正量f_i及び仮想面ねじり力R_iのいずれも含まれない。このため、各仮想面における仮想面摩擦力成分F_iの大きさを、前記第1A必要条件、第1B必要条件、第2必要条件及び第3必要条件を満足し得る範囲内で、必要最低限の大きさに留めることができる。ひいては、各接触対象面における目標並進力の摩擦力成分の大きさも必要最低限もしくはこれと同程度に留めることができる。
【0286】
また、S01で設定される仮想面が対向仮想面Sna,Snbを含む場合において、S27の判断結果が肯定的となる場合はもちろん、S39の判断結果が肯定的となる場合においても、これらの対向仮想面Sna,Snbの間でロボット1に発生する内力の大きさ(N_na,N_nbの大きさのうちの小さい方の大きさ)を、前記第1A必要条件、第1B必要条件、第2必要条件及び第3必要条件を満足し得る範囲内で、必要最低限に留めることができる。ひいては、これらの対向仮想面Sna,Snbに対応する2つの接触対象面(例えば、前記壁登り歩容における壁面WL1,WL2)の間でロボット1に発生する内力の大きさを必要最低限もしくはこれと同程度に留めることができる。このため、ロボット1の関節アクチュエータ11が発生可能な動力(本実施形態では駆動トルク)のうち、該内力を発生するために必要な負担分を軽減することができるように対向仮想面Sna,Snbに対応する2つの接触対象面を含む各接触対象面における目標並進力を決定できる。
【0287】
ここで、以上説明した本実施形態と本願発明との対応関係を補足しておく。本実施形態ででは、制御ユニット21の歩容生成部23が本発明における歩容生成装置として機能する。
【0288】
そして、この歩容生成部23のうちの基本歩容要素生成部25によって、本発明における目標運動生成手段が実現される。
【0289】
また、外力目標生成部27が実行する処理のうち、S01の処理によって、本発明における仮想面群設定手段が実現される。
【0290】
また、S03の処理によって、本発明における必要総並進外力決定手段が実現される。
【0291】
また、S05〜S15、S19〜S45の処理によって、本発明における仮想面必要並進力決定手段が実現される。
【0292】
また、S29及びS41の処理によって、本発明における接触対象面目標並進力決定手段が実現される。
【0293】
[第2実施形態]
次に本発明の第2実施形態を図14及び図15を参照して説明する。なお、本実施形態は、外力目標生成部27の処理の一部だけが第1実施形態と相違するものであるので、第1実施形態と同一事項については説明を省略する。
【0294】
前記第1実施形態では、前記モーメント偏差errMを解消するためのモーメント補償量を決定する処理(S21〜S37の処理)において、第1補償量としての仮想面上外力作用点修正量を優先的に決定するようにした。
【0295】
ただし、例えば、第2補償量としての仮想面摩擦力成分修正量f_i又は第3補償量としての仮想面ねじり力R_iを、仮想面上外力作用点修正量よりも優先的に決定するようにすることも可能である。本実施形態は、このようにモーメント補償量として、第2補償量又は第2補償量を優先的に決定する場合の一例を示す実施形態である。
【0296】
本実施形態では、外力目標生成部27の処理においては、S01〜S21までの処理は第1実施形態と同じである。そして、本実施形態では、外力目標生成部27は、S21の処理(モーメント偏差errMを算出する処理)に続いて、図14及び図15フローチャートに示す処理を実行する。
【0297】
以下説明すると、外力目標生成部27は、S21の処理の次にS61の処理を実行する。このS61では、外力目標生成部27は、第1実施形態で説明したS35の処理と同様の処理によって、仮想面摩擦力成分修正量f_i(第2補償量)及び仮想面ねじり力R_i(第3補償量)と、これらのf_i,R_iによってモーメント基準点周りに発生するモーメントMfrとの関係を前記式11により表現するヤコビアンJfrを決定する。ただし、本実施形態では、S61において、f_i,R_iを作用させる各仮想面上の外力作用点としては、仮想面上暫定外力作用点(基本歩容要素生成部25が決定した接触対象面上の暫定外力作用点を該接触対象面に対応する仮想面に投影してなる点)が使用される。
【0298】
次いで、外力目標生成部27は、S63の処理を実行する。このS63では、外力目標生成部27は、前記式11のMfrをS21で求めたモーメント偏差errMに一致させた場合に、f_i及びR_iの組(f_i R_i)Tを算出する。
【0299】
なお、必ずしもf_i及びR_iの両方を算出する必要はなく、いずれか一方を“0”に設定するようにしてもよい。
【0300】
この場合、外力目標生成部27は、S61で求めたヤコビアンJfrの擬似逆行列Jfr_invを、第1実施形態におけるS37の処理と同様の処理によって算出し、この擬似逆行列Jfr_invを、モーメント偏差errMに乗じることによって、f_i及びR_iの組(f_i R_i)Tを算出する。これにより、errMを解消するために必要なf_i及びR_iの組が算出される。
【0301】
以上のS63までの処理によって、前記第1A必要条件及び第1B必要条件を満足し、且つ、ロボット1の目標運動のうちのロボット1全体の回転運動(モーメント基準点周りの角運動量を変化させる回転運動)を実現し得る仮想面必要並進力(f_iによる修正後の仮想面必要並進力)と、仮想面上外力作用点(=仮想面上暫定外力作用点)と、仮想面上ねじり力R_iとの組が暫定的に決定されることとなる。ただし、この場合、f_iによる仮想面必要並進力の修正が行なわれるので、その修正後の仮想面必要並進力の仮想面摩擦力成分は、前記第2必要条件を満足するとは限らない。また、仮想面上ねじり力R_iも第1実施形態で説明した第4必要条件を満足するとは限らない。
【0302】
そこで、外力目標生成部27は、次に、S65の処理を実行する。このS65では、外力目標生成部27は、第1実施形態におけるS39と同じ判断処理を実行する。すなわち、外力目標生成部27は、各仮想面において、f_iによる修正後の仮想面摩擦力成分F_i(S15までの処理で決定されたF_iとf_iとの総和の摩擦力成分)が、前記第2必要条件を満足するか否かを判断すると共に、R_iの大きさが、前記第4必要条件を満足するか否かを判断する。
【0303】
そして、この判断結果が肯定的である場合には、外力目標生成部27は、S67の処理を実行し、今回時刻での処理を終了する。このS67の処理では、外力目標生成部27は、第1実施形態におけるS41と同じ処理によって、今回時刻での各接触対象面からロボット1に作用させる目標外力を確定する。
【0304】
一方、本実施形態におけるS67の処理では、外力目標生成部27は、各接触対象面の目標外力作用点に関しては、基本歩容要素生成部25で生成された暫定外力作用点をそのまま目標外力作用点として確定する。
【0305】
前記S65の判断結果が否定的である場合には、外力目標生成部27は、次にS69の処理を実行する。このS69では、外力目標生成部27は、第2必要条件を満足するように、仮想面摩擦力成分修正量f_iを制限してなる仮想面摩擦力成分制限修正量f_lim_iを決定すると共に、第4必要条件を満足するように、仮想面ねじり力R_iを制限してなる仮想面制限ねじり力R_lim_iを決定する。
【0306】
すなわち、外力目標生成部27は、f_lim_iによる修正後の仮想面摩擦力成分F_iが、S15までの処理で決定した仮想面垂直抗力成分Niに応じて設定した摩擦力上限値(=μ_i*|N_i|)以下になるようにf_lim_iを決定すると共に、R_lim_iがS15までの処理で決定した仮想面垂直抗力成分Niに応じて設定したねじり力上限値(=μr_i*|N_i|)以下になるようにR_lim_iを決定する。
【0307】
この場合、f_lim_iに関しては、第2必要条件を満足することに加えて、それらの総和(合力)が“0”に保たれるようにf_lim_iが決定される。
【0308】
より具体的には、例えば、S63で決定した各仮想面における仮想面摩擦力成分修正量f_iのそれぞれに同じ値の、ある係数kを乗じてなる値を仮想面摩擦力成分制限修正量f_lim_iに一致させるように、f_lim_iを決定する。このようにすることでf_lim_iの総和(合力)は、f_iの総和(=0)のk倍、すなわち、“0”になる。この場合、係数kの値を未知数とし、各仮想面において、S15までの処理で決定した修正前の仮想面摩擦力成分F_iにk*f_iを加え合わせてなる摩擦力の大きさ(=|F_i+k*f_i|)が、第2必要条件を満たすように(|F_i+k*f_i|≦μ_i*|N_i|となるように)、上記係数kの値を決定する。そして、この決定した係数kを、S63で算出した各仮想面におけるf_iに乗じることによって、各仮想面におけるf_lim_iを決定する。
【0309】
なお、S01で設定された全ての仮想面において、f_iによる修正後の仮想面摩擦力成分F_iが前記第2必要条件を満足している場合には、上記係数kの値は“1”とされる。従って、この場合には、S63で算出した各仮想面におけるf_iがそのまま、flim_iとして決定される。
【0310】
また、各仮想面における仮想面制限ねじり力R_lim_iは例えば次のように決定される。すなわち、S65で前記第4必要条件を満たさない各仮想面において、仮想面制限ねじり力R_lim_iは、その大きさがねじり力上限値(=μr_i*|N_i|)に一致するように決定される。また、第4必要条件を満たす各仮想面においては、S63で算出された仮想面ねじり力R_iがそのまま仮想面制限ねじり力R_lim_iとして決定される。
【0311】
なお、第4必要条件を満たさない各仮想面における仮想面制限ねじり力R_lim_iを、ねじり力上限値よりも若干小さい大きさのねじり力に決定するようにしてもよい。
【0312】
上記の如く仮想面摩擦力成分制限修正量f_lim_iと仮想面制限ねじり力R_lim_iとを決定した後、外力目標生成部27は、次に、S71の処理を実行する。このS71では、外力目標生成部27は、前記モーメント偏差errMのうち、S69で決定した各仮想面におけるf_lim_iとR_lim_iとによって解消できない偏差成分としての残偏差成分errM'を算出する。
【0313】
具体的には、外力目標生成部27は、各仮想面において、制限前の仮想面摩擦力成分修正量f_iと仮想面摩擦力成分制限修正量f_lim_iとの差Δf_i(=f_i−f_lim_i)を算出すると共に、制限前の仮想面ねじり力R_iと仮想面制限ねじり力R_lim_iとの差ΔR_i(=R_i−R_lim_i)とを算出する。そして、外力目標生成部27は、前記式11の右辺における縦ベクトル(f_i R_i)Tの成分f_i,R_iをそれぞれに対応するΔf_i,ΔR_iに置き換え、且つ、前記式11の左辺をerrM'に置き換えた式によって、残偏差成分errM’を算出する。
【0314】
なお、f_lim_i及びR_lim_iを、仮想面上暫定外力作用点に作用させた場合にモーメント基準点周りに発生するモーメントを算出し、このモーメントを、モーメント偏差errMから差し引くことによって、残偏差成分errM'を算出するようにしてもよい。
【0315】
次いで、外力目標生成部27は、図15のS73の処理を実行する。このS73では、外力目標生成部27は、仮想面上外力作用点の位置の変化(仮想面上暫定外力作用点からの変化)に対する仮想面必要並進力依存モーメントMsの変化の感度を表現するヤコビアンJ(前記式07の関係式におけるヤコビアンJ)を、第1実施形態におけるS23と同じ処理によって算出する。
【0316】
次いで、外力目標生成部27は、S75の処理を実行する。このS75では、外力目標生成部27は、第1実施形態におけるS25と同様の処理によって、前記式07におけるΔMs(仮想面必要並進力依存モーメントMsの変化量)をS71で算出した残偏差成分errM'に一致させた場合に、式07の関係を満たす縦ベクトル(Δrp_i)を、errMを解消するための各仮想面における仮想面上外力作用点修正量Δrpc_iの組として算出する。すなわち、外力目標生成部27は、S73で決定したヤコビアンJの擬似逆行列JinvをS25の処理と同様に算出し、このJinvをerrM'に乗じることによって、各仮想面における仮想面上外力作用点修正量Δrpc_iの組を算出する。
【0317】
次いで、外力目標生成部27は、S77の判断処理を実行する。このS77の判断処理は、前記第1実施形態におけるS27の判断処理と同じである。すなわち、外力目標生成部27は、全ての仮想面において、S75で決定したΔrpc_iによる修正後の仮想面上外力作用点の位置が外力作用点存在領域条件を満足するか否かを判断する。
【0318】
このS77の判断結果が肯定的である場合には、外力目標生成部27は、S79の処理を実行し、今回時刻での処理を終了する。このS79の処理では、外力目標生成部27は、今回時刻での各接触対象面からロボット1の目標外力と目標外力作用点とを確定する。
【0319】
具体的には、外力目標生成部27は、S15までの処理で決定した各仮想面における仮想面必要並進力(N_i,F_i)と、S69の処理で決定した仮想面摩擦力成分制限修正量f_lim_iとから、今回時刻での各仮想面における仮想面必要並進力を確定する。この場合、S15までの処理で決定した仮想面垂直抗力成分N_iがそのまま該仮想面における仮想面垂直抗力成分として確定されると共に、S15までの処理で決定した仮想面摩擦力成分F_iに仮想面摩擦力成分制限修正量f_lim_iを加え合わせてなる摩擦力成分が、該仮想面における仮想面摩擦力成分として確定される。
【0320】
また、外力目標生成部27は、S69の処理で決定した仮想面制限ねじり力R_lim_iを、このR_lim_iに対応する仮想面における仮想面ねじり力として確定する。
【0321】
そして、外力目標生成部27は、以上の如く確定した各仮想面における仮想面必要並進力と仮想面ねじり力とを、第1実施形態におけるS29又はS41と同じ処理によって、該接触対象面からロボット1に作用させる並進力及びねじり力にそれぞれ変換し、この変換後の並進力とねじり力とをそれぞれ目標外力の構成要素である並進外力の目標値(目標並進力)、ねじり力の目標値(目標ねじり力)として決定する。
【0322】
また、外力目標生成部27は、第1実施形態におけるS29の処理の場合と同様に、S75で決定した仮想面上外力作用点修正量Δrpc_iによる修正後の各仮想面上の外力作用点を該仮想面に対応する接触対象面上に逆投影してなる点を、該接触対象面上の目標外力作用点として決定する。
【0323】
前記S77の判断結果が否定的となる状況では、外力作用点存在領域条件を満たし得る範囲内での仮想面上外力作用点の修正では、前記残偏差成分errM'を解消できないこととなる。このため、この場合には、各仮想面に付加する摩擦力又はねじり力をさらに増加させ得るように(ひいてはS65での判断結果が肯定的になり得るように)、仮想面垂直抗力成分N_iの大きさを大きくする必要がある。
【0324】
そこで、S77の判断結果が否定的となる場合には、外力目標生成部27は、次に、S81の処理を実行する。このS77では、外力目標生成部27は、S65の判断処理で、第2必要条件又は第4必要条件を満たさない仮想面が、対向仮想面Sna又はSnb(n=1又は2又は3)であるか否かを判断する。
【0325】
このS65の判断結果が否定的となる状況では、S65の判断処理で第2必要条件又は第4必要条件を満足しない仮想面における仮想面垂直抗力成分の大きさを、前記第1A必要条件を満たしつつ増加させることができない。そこで、この場合には、外力目標生成部27は、S85において、目標運動が不適切である(目標運動を修正する必要がある)とし、今回時刻での処理を中止する。なお、この場合、外力目標生成部27は、基本歩容要素生成部25にエラー情報を出力する。そして、基本歩容要素生成部25は、そのエラー情報に応じて、目標運動を作成し直す。
【0326】
従って、本実施形態では、S77の判断結果が否定的となる場合に、S81の判断処理によって、目標運動が評価される。そして、このS81の判断結果が否定的となる場合に、目標運動が不適切であると評価されることとなる。
【0327】
一方、S81の判断結果が肯定的となる状況では、第2必要条件又は第4必要条件を満たさない仮想面を含む、互いに平行な対向仮想面Sna,Snb(n=1又は2又は3)の仮想面垂直抗力成分N_na,N_nbの大きさを、前記第1A必要条件を満足しつつ(N_na+N_nbを一定に保ちつつ)、増加させることができる。
【0328】
そこで、S81の判断結果が肯定的となる場合には、外力目標生成部27は、次に、S83の処理を実行する。このS83では、外力目標生成部27は、対向仮想面Sna,Snbの仮想面垂直抗力成分N_na,N_nbの大きさの増加量IN_nを決定する。この場合、本実施形態では、S83で決定する増加量IN_nは、あらかじめ定められた所定値に設定される。
【0329】
ただし、この増加量IN_nを、例えば、第1実施形態におけるS45と同様の処理によって決定するようにしてもよい。
【0330】
次いで、外力目標生成部27は、図7のS13からの処理を改めて実行する。
【0331】
以上が、本実施形態における外力目標生成部27の処理の詳細である。
【0332】
以上説明した外力目標生成部27の処理によって、まず、S05〜S15の処理によって、第1実施形態と同様に、ロボット1の目標運動のうちの、ロボット1全体の並進運動を実現するために必要な仮想面必要並進力(N_i,F_i)が暫定的に決定される。このため、ロボット1全体の並進運動を実現し得る仮想面必要並進力(N_i,F_i)の暫定値を簡素なアルゴリズムによって効率よく決定することができる。
【0333】
また、第1実施形態と同様に、S07、S09の判断結果がいずれも否定的となる場合、あるいは、S07、S09、S15の判断結果がそれぞれ、否定的、肯定的、否定的となる場合に、基本歩容要素生成部25で生成された目標運動が不適切であると評価され、外力目標生成部27の処理が中止される。このため、第1実施形態と同様に、目標運動のうちのロボット1全体の並進運動を実現するために、接触対象面とロボット1との間の摩擦力が過大になりやすい目標運動、あるいは、ロボット1の内力が過大になりやすい目標運動を、簡素なアルゴリズムで効率よく不適切であると評価することができる。ひいては、目標歩容を生成するために、無駄な演算処理を実行することとなるのを抑制することができる。
【0334】
そして、本実施形態では、このようにロボット1全体の並進運動を実現し得る仮想面必要並進力(N_i,F_i)の暫定値を決定した上で、S19、S21、S61〜S75の処理(S67の処理を除く)によって、ロボット1全体の回転運動(モーメント基準点周りの角運動量の変化を生じる回転運動)を実現するための付加的な操作量(モーメント補償量)として、前記仮想面摩擦力成分修正量Δf_iと仮想面ねじり力R_iとの組が決定され、あるいは、前記仮想面摩擦力成分修正量Δf_iと仮想面ねじり力R_iと仮想面上外力作用点修正量Δrpc_iとの組が決定される。
【0335】
このため、第1実施形態の場合と同様に、S05〜S15の処理によって決定した仮想面必要並進力(N_i,F_i)の暫定値と、S19、S21、S61〜S75の処理(S67の処理を除く)によって決定したモーメント補償量とを単純に組合わせるだけで、ロボット1全体の並進運動と回転運動との両方を適切に実現できる仮想面上必要並進力と仮想面上ねじり力と仮想面上外力作用点との組(仮想面上ねじり力が“0”になる場合を含む)を決定することができることとなる。そして、この仮想面上必要並進力と仮想面上ねじり力と仮想面上外力作用点との組から、各接触対象面からロボット1に作用させるべき目標外力(目標並進力及び目標ねじり力)と目標外力作用点とを効率よく適切に決定できることとなる。
【0336】
なお、この場合、S77、S81の判断結果がいずれも否定的となる場合に、基本歩容要素生成部25で生成された目標運動が不適切であると評価され、外力目標生成部27の処理が中止される。このため、前記第1A必要条件及び第1B必要条件とを少なくとも満足しつつ、ロボット1全体の回転運動を実現することが困難となる目標運動を不適切であると評価することができる。ひいては、目標歩容を生成するために、無駄な演算処理を実行することとなるのを抑制することができる。
【0337】
また、図7に示したS09及びS15、並びに図15のS81のいずれかの判断結果が否定的となる場合(目標運動が不適切である場合)を除いて、第1実施形態と同様に、今回時刻での各接触対象面に対応する各仮想面における最終的な仮想面必要並進力(仮想面垂直抗力成分N_iと仮想面摩擦力成分F_iとの組)は、結果的に、前記第1A必要条件、第1B必要条件、第2必要条件及び第3必要条件を満足するように決定される。
【0338】
そして、各仮想面に対応する接触対象面における並進外力の目標値(目標並進力)は、その垂直抗力成分(接触対象面に垂直な成分)と摩擦力成分(接触対象面に平行な成分)とが、それぞれ、該仮想面における仮想面垂直抗力成分N_iと仮想面摩擦力成分F_iとに一致するように決定される。
【0339】
このため、第1実施形態と同様に、各接触対象面からロボット1への目標並進力は、可能な限り、各接触対象面における目標並進力のうちの垂直抗力成分の合力によって前記必要総並進外力を実現すると共に、該目標並進力のうちの摩擦力成分を、該接触対象面で発生可能な摩擦力の大きさの限界内で可能な限り小さくなるように、各接触対象面からロボットへの目標並進力を決定できる。
【0340】
また、S01で設定される仮想面が対向仮想面Sna,Snbを含む場合において、S65又はS77の判断結果が肯定的となる場合に、第1実施形態と同様に、これらの対向仮想面Sna,Snbの間でロボット1に発生する内力の大きさ(N_na,N_nbの大きさのうちの小さい方の大きさ)を、前記第1A必要条件、第1B必要条件、第2必要条件及び第3必要条件を満足し得る範囲内で、必要最低限に留めることができる。従って、第1実施形態と同様に、ロボット1の関節アクチュエータ11が発生可能な動力(本実施形態では駆動トルク)のうち、該内力を発生するために必要な負担分を軽減することができるように対向仮想面Sna,Snbに対応する2つの接触対象面を含む各接触対象面における目標並進力を決定できる。
【0341】
ここで、以上説明した本実施形態と本願発明との対応関係を補足しておく。本実施形態ででは、S05〜S15、S19、S21、S61〜S83の処理によって、本発明における仮想面必要並進力決定手段が実現される。また、S67及びS79の処理によって、本発明における接触対象面目標並進力決定手段が実現される。これら以外の対応関係は、第1実施形態と同じである。
【0342】
[第3実施形態]
次に本発明の第3実施形態を図16を参照して説明する。なお、本実施形態は、外力目標生成部27の処理の一部だけが第1実施形態と相違するものであるので、第1実施形態と同一事項については説明を省略する。
【0343】
ロボット1の外界の接触対象面が、前記壁面WL1,WL2の如く、互いに間隔を存して対向する2つの接触対象面を有する場合、ひいては、S01で設定される仮想面群が対向仮想面Sna,Snbを含む場合には、これらの対向仮想面Sna,Snbにおける仮想面垂直抗力成分N_na,N_nbの合力を一定に維持したまま、該対向仮想面Sna,Snbの間の内力(=min(|N_na|,|N_nb|)を操作する(増減させる)ことによって、モーメント基準点周りに発生するモーメントを変化させることができる。従って、この内力は、モーメント偏差errM(又は残偏差成分errM')を解消し、もしくは低減するためモーメント補償量として利用することもできる。
【0344】
そこで、本実施形態では、S01で設定される仮想面群が対向仮想面Sna,Snbを含む場合に、S33で算出される前記残偏差成分errM'を解消するためのモーメント補償量として、第2補償量(仮想面摩擦力成分修正量f_i)及び第3補償量(仮想面ねじり力R_i)と、第4補償量としての内力修正量ΔINとの組を決定する。
【0345】
具体的には、本実施形態における外力目標生成部27の処理のうち、前記S33までの処理は、第1実施形態と同じである。そして、S33の後の一部の処理が第1実施形態と相違する。
【0346】
以下、図16を参照して説明すると、外力目標生成部27は、S33の次に、S101の処理を実行する。このS101では、外力目標生成部27は、S01で設定した仮想面群に、対向仮想面Sna,Snbが有るか否かを判断する。そして、この判断結果が否定的である場合には、外力目標生成部27は、図9に示したS35からの処理を第1実施形態と同様に実行する。
【0347】
なお、本実施形態では、S101の判断結果が否定的となった場合において、その後、図9に示したS35、S37、S39、S43の処理を順次実行したとき、S43の判断結果は常に否定的となる。従って、S101の判断結果が否定的となった場合の後の処理では、S43及びS45の処理は実質的に不要であり、S39の判断結果が、否定的である場合に、直ちにS47の処理を実行するようにしてもよい。
【0348】
一方、S101の判断結果が肯定的である場合には、外力目標生成部27は、次に、S103の処理を実行する。このS103では、各仮想面(S01で設定した各仮想面)の仮想面上制限修正後外力作用点に任意の値の前記仮想面摩擦力成分修正量f_iと前記仮想面ねじり力R_iとを付加し、且つ、各組の対向仮想面Sna,Snbの仮想面垂直抗力成分N_na,N_nbの大きさを任意の値の内力修正量ΔIN_nだけ変化させた場合に、それらのf_i,R_i,ΔIN_nによって前記モーメント基準点周りに発生するモーメントMfrinと、f_i,R_i,ΔIN_nとの間の関係を次式13により表現するヤコビアン(行列)Jfrinを算出する。
【0349】
Mfrin=Jfrin*(f_i R_i ΔIN_n)T ……式13
この式13における(f_i R_i ΔIN_n)Tは、f_i,R_i及びΔIN_nを成分とする縦ベクトルである。なお、仮想面摩擦力成分修正量f_iは、それらの総和(合力)が“0”になるように決定されるべきモーメント補償量であるので、式13における縦ベクトル(f_i R_i ΔIN_n)Tの成分としてのf_iは、第1実施形態で説明した前記式11におけるf_iと同様に、任意の1つの仮想面に対応するf_i(≡f_i0)を除いたものである。また、S01で設定した仮想面群に、対向仮想面Sna,Snbの組が複数組含まれる場合には、その全ての組の対向仮想面Sna,Snbに対応する内力修正量ΔIN_nが式13における縦ベクトル(f_i R_i ΔIN_n)Tの成分として含まれる。
【0350】
次いで、外力目標生成部27は、S105の処理を実行する。このS105では、外力目標生成部27は、式13のMfrinを、S33で算出した残偏差成分errM'に一致させた場合に、式13の関係を満たすf_i,R_i及びΔIN_nの組(f_i R_i ΔIN_n)Tを算出する。
【0351】
すなわち、外力目標生成部27は、S103で求めたヤコビアンJfrinの擬似逆行列Jfrin_invを算出し、この擬似逆行列Jfrin_invを、残偏差成分errM'に乗じることによって、f_i,R_i及びΔIN_nの組を算出する。この場合、擬似逆行列Jfrin_invは、第1実施形態で説明した図9のS37の処理で擬似逆行列Jfr_invを算出する手法と同様に、例えば式13の縦ベクトル(f_i R_i ΔIN_n)Tを組成するf_i,R_i,ΔIN_nのそれぞれの2乗値又は絶対値の平均値(又は重み付き平均値)が最小になるように決定される。また、式13の縦ベクトル(f_i R_i ΔIN_n)Tから除いた上記f_i0は、上記の如く算出したf_iの総和の符号を反転させてなる値として決定される。
【0352】
なお、必ずしもf_i,R_i,ΔIN_nの全てを算出する必要はない。例えば、摩擦係数μ_iが比較的小さい仮想面におけるf_iを“0”に設定したり、ねじり力係数μr_iが比較的小さい仮想面におけるR_iを“0”に設定するようにしてもよい。
【0353】
歩容生成部23が、前記壁登り歩容を生成する場合においては、上記S103、S105の処理は、例えば次にように行なわれる。
【0354】
例えば、第1実施形態で説明した場合と同様に、図11(b)に示した動作状態において、床面FL及び壁面WL1,WL2にそれぞれ対応する各仮想面S3a,S2a,S2bにおける仮想面上制限修正後外力作用点の位置ベクトル(モーメント基準点に対する位置ベクトル)をそれぞれrp_3a,rp_2a,rp_2bとおく。このとき、各仮想面S3a,S2a,S2bにおける仮想面上摩擦力成分修正量f_3a,f_2a,f_2b、仮想面ねじり力R_3a,R_2a,R_2b、並びに、仮想面S2a,S2b間の内力修正量ΔIN_2と、これらのf_3a,f_2a,f_2b,R_3a,R_2a,R_2b,ΔIN_2によってモーメント基準点周りに発生するモーメントMfrinとの間の関係は、次式13aにより表される。
【0355】
Mfrin=rp_3a*f_3a+rp_2a*f_2a+rp_2b*f_2b+R_3a+R_2a+R_2b
+(rp_2a−rp_2b)*ΔIN_2
……式13a
なお、式13aにおける(rp_3a*f_3a),(rp_2a*f_2a),(rp_2b*f_2b),(rp_2a−rp_2b)*ΔIN_2はそれぞれベクトル積である。この場合、式13aでは、ΔIN_2は、仮想面S2aから仮想面S2bに向かうベクトル(大きさがΔIN_2であるベクトル)として表現されている。
【0356】
この場合、仮想面摩擦力成分修正量に関しては、f_3a+f_2a+f_2b=0であるから、f_2b=−(f_3a+f_2a)となる。従って、式13aは、例えばf_2bを含まない次式13bに書き換えられる。
【0357】
Mfrin=(rp_3a−rp_2b)*f_3a+(rp_2a−rp_2b)*f_2a+R_3a+R_2a+R_2b
+(rp_2a−rp_2b)*ΔIN_2
……式13b
従って、S103では、この式13bの右辺を、座標成分で表して整理することで、前記式13におけるヤコビアンJfrinが求められることとなる。
【0358】
そして、S105では、このヤコビアンJfrinの擬似逆行列Jfrin_invが前記した如く算出され、このJfr_invを、残偏差成分errM’に乗じることによって、f_3a,f_2a,R_3a,R_2a,R_2b,ΔIN_2が算出される。さらに、f_3a,f_2aから、f_2b=−(f_3a+f_2a)という関係に従って、f_3bが算出される。
【0359】
以上のS105までの処理によって、前記S27の判断結果が否定的となり、且つ、S101の判断結果が肯定的となる場合には、前記第1A必要条件及び第1B必要条件を満足しつつ、ロボット1の目標運動のうちのロボット1全体の回転運動(モーメント基準点周りの角運動量を変化させる回転運動)を実現するためのモーメント補償量として、仮想面上外力作用点制限修正量Δrpc_lim_iに加えて、仮想面摩擦力成分修正量f_i、仮想面ねじり力R_i及び内力修正量ΔIN_nの組が算出されることとなる。
【0360】
次いで、外力目標生成部27は、S107の判断処理を実行する。このS107では、外力目標生成部27は、内力修正量ΔIN_nによる内力の修正を行なう対向仮想面Sna,Snbにおいて、修正後の内力が前記第3必要条件を満足するか否かを判断する。より詳しくは、外力目標生成部27は、対向仮想面Sna,Snbにおいて、内力修正量ΔIN_nだけ大きさを変化させた仮想面垂直抗力成分N_na,N_nbの大きさのうちの小さい方の大きさ(すなわちΔIN_nによる修正後の内力の大きさ)が、前記内力上限値IFC_n以下であるか否かを判断する。
【0361】
このS107の判断結果が否定的である場合には、外力目標生成部27は、S117において、目標運動が不適切である(目標運動を修正する必要がある)とし、外力に関する目標を生成する処理を中止する。なお、この場合、外力目標生成部27は、基本歩容要素生成部25にエラー情報を出力する。そして、基本歩容要素生成部25は、そのエラー情報に応じて、目標運動を作成し直す。
【0362】
また、S107の判断結果が肯定的である場合には、外力目標生成部27は、図9のS39と同じ判断処理(第2必要条件及び第4必要条件に関する判断処理)をS109で実行する。なお、この場合、対向仮想面Sna,Snbにおける摩擦力上限値に関する仮想面垂直抗力成分N_na,N_nbの大きさは、内力修正量ΔIN_nによる修正後の値(詳しくは図7のS15までの処理で決定された仮想面垂直抗力成分N_na,N_nbの大きさを、内力修正量ΔIN_nだけ変化させた後の値)である。
【0363】
そして、S109の判断結果が肯定的である場合には、外力目標生成部27は、図9のS41と同じ処理をS111で実行し、今回時刻での各接触対象面からロボット1への目標外力と目標外力作用点とを確定する。
【0364】
また、S109の判断結果が否定的である場合には、外力目標生成部27は、図9のS43と同じ判断処理をS113で実行する。そして、この判断結果が、否定的である場合には、外力目標生成部27は、S117において、目標運動が不適切であるとして、外力目標生成部27の処理(外力に関する目標を生成する処理)を中止する。
【0365】
従って、本実施形態では、S107の判断結果が否定的となる場合、あるいは、109及びS113の判断結果がいずれも否定的となる場合に、目標運動が不適切であると評価されることとなる。
【0366】
また、S113の判断結果が肯定的である場合には、図9のS45と同じ処理をS115で実行し、対向仮想面Sna,Snbにおける仮想面垂直抗力成分N_na,N_nbの大きさの増加量IN_nを決定する。なお、S115で決定する増加量IN_nは、詳しくは、S109の判断処理で使用した仮想面垂直抗力成分N_na,N_nbの大きさ(すなわち、内力修正量ΔIN_nによる修正後の値)からの増加量である。
【0367】
次いで、外力目標生成部27は、図7のS13からの処理を改めて実行する。
【0368】
なお、このようにS13からの処理を改めて実行する場合には、S15の判断結果が肯定的となる場合に、S101の判断処理を行うことなく(あるいは、S101の判断結果に依存することなく)、S35からの処理を実行するようにしてもよい。
【0369】
以上が、本実施形態における外力目標生成部27の処理の詳細である。
【0370】
以上説明した外力目標生成部27の処理によって、第1実施形態と同様に、まず、S05〜S15の処理によって、ロボット1の目標運動のうちの、ロボット1全体の並進運動を実現するために必要な仮想面必要並進力(N_i,F_i)が暫定的に決定される。このため、ロボット1全体の並進運動を実現し得る仮想面必要並進力(N_i,F_i)の暫定値を簡素なアルゴリズムによって効率よく決定することができる。
【0371】
また、第1実施形態と同様に、S07、S09の判断結果がいずれも否定的となる場合、あるいは、S07、S09、S15の判断結果がそれぞれ、否定的、肯定的、否定的となる場合に、基本歩容要素生成部25で生成された目標運動が不適切であると評価され、外力目標生成部27の処理が中止される。このため、第1実施形態と同様に、目標運動のうちのロボット1全体の並進運動を実現するために、接触対象面とロボット1との間の摩擦力が過大になりやすい目標運動、あるいは、ロボット1の内力が過大になりやすい目標運動を、簡素なアルゴリズムで効率よく不適切であると評価することができる。ひいては、目標歩容を生成するために、無駄な演算処理を実行することとなるのを抑制することができる。
【0372】
そして、本実施形態では、このようにロボット1全体の並進運動を実現し得る仮想面必要並進力(N_i,F_i)の暫定値を決定した上で、S19〜S37の処理(S29の処理を除く)、又は、S19〜S33(S29の処理を除く)、S103〜105の処理によって、ロボット1全体の回転運動(モーメント基準点周りの角運動量の変化を生じる回転運動)を実現するための付加的な操作量(モーメント補償量)として、前記仮想面上外力作用点修正量Δrpc_iが決定され、あるいは、前記仮想面上外力作用点制限修正量Δrpc_lim_iと仮想面摩擦力成分修正量Δf_iと仮想面ねじり力R_iとの組が決定され、あるいは、Δrpc_lim_i、Δf_i及びR_iと内力修正量ΔIN_nとの組が決定される。
【0373】
このため、第1実施形態と同様に、S05〜S15の処理によって決定した仮想面必要並進力(N_i,F_i)の暫定値と、S19〜S37の処理(S29の処理を除く)、又は、S19〜S33(S29の処理を除く)とS103〜105の処理とによって決定したモーメント補償量とを単純に組合わせるだけで、ロボット1全体の並進運動と回転運動との両方を適切に実現できる仮想面上必要並進力と仮想面上ねじり力と仮想面上外力作用点との組(仮想面上ねじり力が“0”になる場合を含む)を決定することができることとなる。そして、この仮想面上必要並進力と仮想面上ねじり力と仮想面上外力作用点との組から、各接触対象面からロボット1に作用させるべき目標外力(目標並進力及び目標ねじり力)と目標外力作用点とを効率よく適切に決定できることとなる。
【0374】
なお、この場合、S109、S113の判断結果がいずれも否定的となる場合に、基本歩容要素生成部25で生成された目標運動が不適切であると評価され、外力目標生成部27の処理が中止される。このため、前記第1A必要条件及び第1B必要条件とを少なくとも満足しつつ、ロボット1全体の回転運動を実現することが困難となる目標運動を不適切であると評価することができる。ひいては、目標歩容を生成するために、無駄な演算処理を実行することとなるのを抑制することができる。
【0375】
また、図7に示したS09及びS15、並びに図16のS107、S113のいずれかの判断結果が否定的となる場合(目標運動が不適切である場合)を除いて、第1実施形態と同様に、今回時刻での各接触対象面に対応する各仮想面における最終的な仮想面必要並進力(仮想面垂直抗力成分N_iと仮想面摩擦力成分F_iとの組)は、結果的に、前記第1A必要条件、第1B必要条件、第2必要条件及び第3必要条件を満足するように決定される。
【0376】
そして、各仮想面に対応する接触対象面における並進外力の目標値(目標並進力)は、その垂直抗力成分(接触対象面に垂直な成分)と摩擦力成分(接触対象面に平行な成分)とが、それぞれ、該仮想面における仮想面垂直抗力成分N_iと仮想面摩擦力成分F_iとに一致するように決定される。
【0377】
このため、第1実施形態と同様に、各接触対象面からロボット1への目標並進力は、可能な限り、各接触対象面における目標並進力のうちの垂直抗力成分の合力によって前記必要総並進外力を実現すると共に、該目標並進力のうちの摩擦力成分を、該接触対象面で発生可能な摩擦力の大きさの限界内で可能な限り小さくなるように、各接触対象面からロボットへの目標並進力を決定できる。
【0378】
特に、本実施形態では、第1実施形態と同様に、モーメント補償量として、仮想面上外力作用点修正量Δrpc_i(第1補償量)が優先的に決定されるため、各仮想面における仮想面摩擦力成分F_iの大きさを、前記第1A必要条件、第1B必要条件、第2必要条件及び第3必要条件を満足し得る範囲内で、必要最低限の大きさに留めることができる。ひいては、各接触対象面における目標並進力の摩擦力成分の大きさも必要最低限もしくはこれと同程度に留めることができる。
【0379】
また、S01で設定される仮想面が対向仮想面Sna,Snbを含む場合において、目標運動が不適切であると判断される場合を除いて、対向仮想面Sna,Snbの間でロボット1に発生する内力の大きさ(N_na,N_nbの大きさのうちの小さい方の大きさ)が前記内力上限値IFC_n以下に収まるように(前記第3必要条件を満足するように)、仮想面垂直抗力成分N_na,N_nbが決定される。このため、ロボット1の関節アクチュエータ11が発生可能な動力(本実施形態では駆動トルク)のうち、内力を発生するために必要な負担分が過大にならないように、対向仮想面Sna,Snbに対応する2つの接触対象面を含む各接触対象面における目標並進力を決定できる。
【0380】
ここで、以上説明した本実施形態と本願発明との対応関係を補足しておく。本実施形態ででは、S05〜S15、S19〜S45、S103〜S115の処理によって、本発明における仮想面必要並進力決定手段が実現される。また、S29,S41,S111の処理によって、本発明における接触対象面目標並進力決定手段が実現される。これら以外の対応関係は、第1実施形態と同じである。
【0381】
[第4実施形態]
次に本発明の第4実施形態を図17及び図18を参照して説明する。なお、本実施形態は、外力目標生成部27の処理の一部だけが第2実施形態又は第3実施形態と相違するものであるので、第2実施形態又は第3実施形態と同一事項については説明を省略する。
【0382】
本実施形態は、モーメント偏差errMを解消もしくは低減するためのモーメント補償量として、第3実施形態と同様に内力修正量ΔIN_n(第4補償量)を適宜利用するものである。この場合、本実施形態では、S01で設定した仮想面群に対向仮想面が含まれる場合に、モーメント補償量として、仮想面摩擦力成分修正量f_i(第2補償量)と仮想面ねじり力R_i(第3補償量)と内力修正量ΔIN_n(第4補償量)との組を決定した上で、これらの補償量によって解消できない残偏差成分errM’を解消するように、仮想面上外力作用点修正量Δrpc_i(第1補償量)を決定する。
【0383】
具体的には、本実施形態における外力目標生成部27の処理のうち、前記S21までの処理は、第2実施形態と同じである。そして、S21の後の一部の処理が第2実施形態と相違する。
【0384】
以下図17及び図18を参照して説明すると、外力目標生成部27は、S21の次に、S121の判断処理を実行する。このS121では、外力目標生成部27は、S01で設定した仮想面群に、対向仮想面Sna,Snbが有るか否かを判断する。そして、この判断結果が否定的である場合には、外力目標生成部27は、図14及び図15に示したS61からの処理を第2実施形態と同様に実行する。
【0385】
なお、本実施形態では、S121の判断結果が否定的となった場合において、その後、図15に示したS81の判断処理を実行したとき、その判断結果は常に否定的となる。従って、S121の判断結果が否定的となった場合の後の処理では、S81及びS83の処理は実質的に不要であり、S77の判断結果が否定的である場合に、直ちにS85の処理を実行するようにしてもよい。
【0386】
一方、S121の判断結果が肯定的である場合には、外力目標生成部27は、次に、S123の処理を実行する。このS123では、外力目標生成部27は、第3実施形態で説明したS103と同様の処理によって、仮想面摩擦力成分修正量f_i(第2補償量)、仮想面ねじり力R_i(第3補償量)及び内力修正量ΔIN_n(第4補償量)と、これらのf_i,R_i,ΔIN_nによってモーメント基準点周りに発生するモーメントMfrinとの関係を前記式13により表現するヤコビアンJfrinを決定する。ただし、本実施形態では、S123の処理における各仮想面上の外力作用点としては、仮想面上暫定外力作用点(基本歩容要素生成部25が決定した接触対象面上の暫定外力作用点を該接触対象面に対応する仮想面に投影してなる点)が使用される。
【0387】
次いで、外力目標生成部27は、S125の処理を実行する。このS125では、外力目標生成部27は、前記式13のMfrinをS21で求めたモーメント偏差errMに一致させた場合に式13の関係式を満足させ得るf_i、R_i及びΔIN_nの組(f_i R_i ΔIN_n)Tを算出する。
【0388】
この場合、外力目標生成部27は、S123で求めたヤコビアンJfrinの擬似逆行列Jfrin_invを、第3実施形態におけるS105の処理と同様の処理によって算出し、この擬似逆行列Jfrin_invを、モーメント偏差errMに乗じることによって、f_i,R_i,ΔIN_nの組(f_i R_i ΔIN_n)Tを算出する。これにより、errMを解消するために必要なf_i,R_i,ΔIN_nの組が算出される。
【0389】
なお、必ずしもf_i,R_i,ΔIN_nの全てを算出する必要はない。例えば、摩擦係数μ_iが比較的小さい仮想面におけるf_iを“0”に設定したり、ねじり力係数μr_iが比較的小さい仮想面におけるR_iを“0”に設定するようにしてもよい。
【0390】
以上のS125までの処理によって、S121の判断結果が否定的となる場合には、前記第1A必要条件及び第1B必要条件を満足しつつ、ロボット1の目標運動のうちのロボット1全体の回転運動(モーメント基準点周りの角運動量を変化させる回転運動)を実現するためのモーメント補償量として、仮想面摩擦力成分修正量f_i、仮想面ねじり力R_i及び内力修正量ΔIN_nの組が算出されることとなる。
【0391】
次いで、外力目標生成部27は、第3実施形態におけるS107と同じ判断処理をS127で実行する。すなわち、外力目標生成部27は、ΔIN_nによる修正後の内力(=min(|N_na|,|N_nb|))が前記第3必要条件を満足するか否かを判断する。
【0392】
このS127の判断結果が肯定的である場合には、外力目標生成部27は、次に、S131の処理を実行する。このS131では、外力目標生成部27は、第2実施形態におけるS65と同じ判断処理を実行する。すなわち、外力目標生成部27は、各仮想面において、f_iによる修正後の仮想面摩擦力成分F_i(S15までの処理で決定されたF_iとf_iとの総和の摩擦力成分)が、前記第2必要条件を満足するか否かを判断すると共に、R_iの大きさが、前記第4必要条件を満足するか否かを判断する。
【0393】
そして、S131の判断結果が肯定的である場合には、外力目標生成部27は、S133の処理を実行し、今回時刻での処理を終了する。このS133の処理では、外力目標生成部27は、第2実施形態におけるS67と同様の処理によって、今回時刻での各接触対象面からロボット1の目標外力と目標外力作用点とを確定する。
【0394】
具体的には、外力目標生成部27は、S15までの処理で決定した各仮想面における仮想面必要並進力(N_i,F_i)と、S125の処理で決定した仮想面摩擦力成分修正量f_i及び内力修正量ΔIN_nとから、今回時刻での各仮想面における仮想面必要並進力を確定する。この場合、S15までの処理で決定した仮想面垂直抗力成分N_iの大きさをΔIN_nだけ変化させてなる垂直抗力成分が、該仮想面における仮想面垂直抗力成分として確定されると共に、S15までの処理で決定した仮想面摩擦力成分F_iに仮想面摩擦力成分修正量f_iを加え合わせてなる摩擦力成分が、該仮想面における仮想面摩擦力成分として確定される。
【0395】
また、外力目標生成部27は、S125の処理で決定した仮想面ねじり力R_iを、このR_iに対応する仮想面におけるねじり力として確定する。
【0396】
そして、外力目標生成部27は、以上の如く確定した各仮想面における仮想面必要並進力と仮想面ねじり力R_iとを、S67の処理と同様に、該仮想面に対応する接触対象面からロボット1に作用させる並進力とねじり力とにそれぞれ変換することで、目標外力の構成要素である目標並進力と目標ねじり力とを決定する。
【0397】
また、外力目標生成部27は、基本歩容要素生成部25で生成された暫定外力作用点をそのまま目標外力作用点として確定する。
【0398】
前記S131の判断結果が否定的である場合には、外力目標生成部27は、次にS135の処理を実行する。このS135では、外力目標生成部27は、第2実施形態におけるS69と同じ処理によって、第2必要条件を満足するように、仮想面摩擦力成分修正量f_iを制限してなる仮想面摩擦力成分制限修正量f_lim_iを決定すると共に、第4必要条件を満足するように、仮想面ねじり力R_iを制限してなる仮想面制限ねじり力R_lim_iを決定する。
【0399】
一方、前記S127の判断結果が否定的である場合には、外力目標生成部27は、S129の処理を実行した後、上記S135の処理を実行する。
【0400】
この場合、S129では、外力目標生成部27は、前記第3必要条件を満足するように、内力修正量ΔIN_nを制限してなる内力制限修正量ΔIN_lim_nを決定する。具体的には、S127の判断結果が否定的となった対向仮想面Sna,Snbに関して、S15までの処理で決定された仮想面垂直抗力成分N_na,Nnbの大きさをそれぞれ、ΔIN_lim_nだけ変化させた後の仮想面垂直抗力成分N_na,Nnbの大きさのうちの小さい方の大きさ(ΔIN_lim_nによる修正後の内力の大きさ)が前記内力上限値IFC_nに一致するように(あるいは、IFC_nよりも若干小さい値となるように)、ΔIN_lim_nを決定する。
【0401】
なお、S01で設定される仮想面群が2組以上の対向仮想面Sna,Snbを有する場合において、S127の判断結果が否定的とならない対向仮想面Sna,Snbに関しては、S125で算出された内力修正量ΔIN_nがそのまま、内力制限修正量ΔIN_lim_nとして決定される。
【0402】
そして、S129の処理に続くS135の処理では、対向仮想面Sna,Snbのそれぞれの摩擦力上限値及びねじり力上限値を決定するための仮想面垂直抗力成分N_na,N_nbの大きさとしては、S15までの処理で決定された仮想面垂直抗力成分の大きさを、S129で決定されたΔIN_lim_nにより修正してなる値が用いられる。
【0403】
なお、S129の処理に続くS135の処理においては、S01で設定された全ての仮想面について、S125で算出されたf_iによる修正後の仮想面摩擦力成分F_iが第2必要条件を満足し、且つ、S125で算出されたR_iが第4必要条件を満足するものとなっている場合もある。この場合には、S125で算出されたf_i,R_iがそのまま各仮想面における仮想面摩擦力成分制限修正量f_lim_i、仮想面制限ねじり力R_lim_iとして決定されることとなる。
【0404】
また、図示は省略したが、本実施形態では、S127の判断結果と、S131の判断結果とがそれぞれ肯定的、否定的となった場合におけるS135の処理では、後の処理の便宜上、S125で算出された内力修正量ΔIN_nがそのまま内力制限修正量ΔIN_lim_nとして決定される。
【0405】
上記の如くΔIN_lim_n、f_lim_i、R_lim_iを決定した後、外力目標生成部27は、次に、S137の処理を実行する。このS137では、外力目標生成部27は、前記モーメント偏差errMのうち、ΔIN_lim_nとf_lim_iとR_lim_iとの組によって解消できない偏差成分としての残偏差成分errM'を算出する。
【0406】
具体的には、外力目標生成部27は、各仮想面において、制限前の仮想面摩擦力成分修正量f_iと仮想面摩擦力成分制限修正量f_lim_iとの差Δf_i(=f_i−f_lim_i)を算出すると共に、制限前の仮想面ねじり力R_iと仮想面制限ねじり力R_lim_iとの差ΔR_i(=R_i−R_lim_i)とを算出する。さらに、外力目標生成部27は、制限前の内力修正量ΔIN_nと内力制限修正量ΔIN_lim_iとの差ΔΔIN_n(=ΔIN_n−ΔIN_lim_n)を算出する。
【0407】
そして、外力目標生成部27は、前記式13の右辺における縦ベクトル(f_i R_i ΔIN_n)Tの成分f_i,R_i,ΔIN_nをそれぞれに対応するΔf_i,ΔR_i,ΔΔIN_nに置き換え、且つ、前記式13の左辺をerrM'に置き換えた式によって、残偏差成分errM’を算出する。
【0408】
なお、式13の右辺における縦ベクトル(f_i R_i ΔIN_n)Tの成分f_i,R_i,ΔIN_nを、それぞれf_lim_i,R_lim_i,ΔIN_lim_nに置き換えた式によって、これらのf_lim_i,R_lim_i,ΔIN_lim_nに起因してモーメント基準点周りに発生するモーメントを算出し、このモーメントを、モーメント偏差errMから差し引くことによって、残偏差成分errM'を算出するようにしてもよい。
【0409】
次いで、外力目標生成部27は、図18のS139からの処理を実行する。この場合、S139からS143までの処理は、第2実施形態におけるS73からS77までの処理と同じである。
【0410】
そして、S143の判断結果が肯定的である場合には、外力目標生成部27は、外力目標S145の処理を実行し、今回時刻での処理を終了する。このS145の処理では、外力目標生成部27は、第2実施形態におけるS79と同様の処理によって、今回時刻での各接触対象面からロボット1の目標外力と目標外力作用点とを確定する。
【0411】
具体的には、外力目標生成部27は、S15までの処理で決定した各仮想面における仮想面必要並進力(N_i,F_i)と、S135までの処理で決定した内力制限修正量ΔIN_lim_nと仮想面摩擦力成分制限修正量f_lim_iとから、今回時刻での各仮想面における仮想面必要並進力を確定する。この場合、対向仮想面Sna,Snbについては、S15までの処理で決定した仮想面垂直抗力成分N_na,N_nbの大きさをΔIN_lim_nにより変化させてなる垂直抗力成分が、該対向仮想面Sna,Snbにおける仮想面垂直抗力成分として確定されると共に、その他の各仮想面については、S15までの処理で決定した仮想面垂直抗力成分N_iが、そのまま該仮想面における仮想面垂直抗力成分として確定される。また、いずれの仮想面についても、S15までの処理で決定した仮想面摩擦力成分F_iに仮想面摩擦力成分制限修正量f_lim_iを加え合わせてなる摩擦力成分が、該仮想面における仮想面摩擦力成分として確定される。
【0412】
また、外力目標生成部27は、S135の処理で決定した仮想面制限ねじり力R_lim_iを、このR_lim_iに対応する仮想面における仮想面ねじり力として確定する。
【0413】
そして、外力目標生成部27は、以上の如く確定した各仮想面における仮想面必要並進力と仮想面ねじり力とを、S79と同様の処理によって、該仮想面に対応する接触対象面からロボット1に作用させる並進力及びねじり力にそれぞれ変換することによって、目標外力の構成要素である目標並進力と目標ねじり力とを決定する。
【0414】
また、外力目標生成部27は、S141で決定した仮想面上外力作用点修正量Δrpc_iによる修正後の各仮想面上の外力作用点を該仮想面に対応する接触対象面上に逆投影してなる点を、該接触対象面上の目標外力作用点として決定する。
【0415】
また、S143の判断結果が否定的となる場合には、外力目標生成部27は、S147の処理を実行した後、図7のS13からの処理を改めて実行する。この場合、S147では、外力目標生成部27は、対向仮想面Sna,Snbにおける仮想面垂直抗力成分N_na,N_nbの大きさの増加量IN_n(詳しくは、S15までの処理で決定された仮想面垂直抗力成分N_na,N_nbからの増加量)を決定する。この場合、本実施形態では、S83で決定する増加量IN_nは、あらかじめ定められた所定値に設定される。
【0416】
なお、このようにS13からの処理を改めて実行する場合には、S21の処理の次に、S121の判断処理を行うことなく(あるいは、S121の判断結果に依存することなく)、S61からの処理を実行するようにしてもよい。
【0417】
以上が、本実施形態における外力目標生成部27の処理の詳細である。
【0418】
以上説明した外力目標生成部27の処理によって、まず、S05〜S15の処理によって、第1実施形態と同様に、ロボット1の目標運動のうちの、ロボット1全体の並進運動を実現するために必要な仮想面必要並進力(N_i,F_i)が暫定的に決定される。このため、ロボット1全体の並進運動を実現し得る仮想面必要並進力(N_i,F_i)の暫定値を簡素なアルゴリズムによって効率よく決定することができる。
【0419】
また、第1実施形態と同様に、S07、S09の判断結果がいずれも否定的となる場合、あるいは、S07、S09、S15の判断結果がそれぞれ、否定的、肯定的、否定的となる場合に、基本歩容要素生成部25で生成された目標運動が不適切であると評価され、外力目標生成部27の処理が中止される。このため、第1実施形態と同様に、目標運動のうちのロボット1全体の並進運動を実現するために、接触対象面とロボット1との間の摩擦力が過大になりやすい目標運動、あるいは、ロボット1の内力が過大になりやすい目標運動を、簡素なアルゴリズムで効率よく不適切であると評価することができる。ひいては、目標歩容を生成するために、無駄な演算処理を実行することとなるのを抑制することができる。
【0420】
そして、本実施形態では、このようにロボット1全体の並進運動を実現し得る仮想面必要並進力(N_i,F_i)の暫定値を決定した上で、S19、S21、S61〜S75の処理(S67の処理を除く)、又はS19、S21、S123〜S141(S133の処理を除く)によって、ロボット1全体の回転運動(モーメント基準点周りの角運動量の変化を生じる回転運動)を実現するための付加的な操作量(モーメント補償量)として、前記仮想面摩擦力成分修正量f_iと仮想面ねじり力R_iとの組、あるいは、f_iとR_limiと内力修正量ΔIN_nとの組、あるいは、f_lim_iとR_lim_iとΔrpc_iと内力制限修正量ΔIN_lim_nとの組が決定される。
【0421】
このため、第1実施形態の場合と同様に、S05〜S15の処理によって決定した仮想面必要並進力(N_i,F_i)の暫定値と、S19、S21、S61〜S75の処理(S67の処理を除く)、又はS19、S21、S123〜S141(S133の処理を除く)によって決定したモーメント補償量とを単純に組合わせるだけで、ロボット1全体の並進運動と回転運動との両方を適切に実現できる仮想面上必要並進力と仮想面上ねじり力と仮想面上外力作用点との組(仮想面上ねじり力が“0”になる場合を含む)を決定することができることとなる。そして、この仮想面上必要並進力と仮想面上ねじり力と仮想面上外力作用点との組から、各接触対象面からロボット1に作用させるべき目標外力(目標並進力及び目標ねじり力)と目標外力作用点とを効率よく適切に決定できることとなる。
【0422】
また、図7に示したS09及びS15、並びにS81のいずれかの判断結果が否定的となる場合(目標運動が不適切である場合)を除いて、第1実施形態と同様に、今回時刻での各接触対象面に対応する各仮想面における最終的な仮想面必要並進力(仮想面垂直抗力成分N_iと仮想面摩擦力成分F_iとの組)は、結果的に、前記第1A必要条件、第1B必要条件、第2必要条件及び第3必要条件を満足するように決定される。
【0423】
そして、各仮想面に対応する接触対象面における並進外力の目標値(目標並進力)は、その垂直抗力成分(接触対象面に垂直な成分)と摩擦力成分(接触対象面に平行な成分)とが、それぞれ、該仮想面における仮想面垂直抗力成分N_iと仮想面摩擦力成分F_iとに一致するように決定される。
【0424】
このため、第1実施形態と同様に、各接触対象面からロボット1への目標並進力は、可能な限り、各接触対象面における目標並進力のうちの垂直抗力成分の合力によって前記必要総並進外力を実現すると共に、該目標並進力のうちの摩擦力成分を、該接触対象面で発生可能な摩擦力の大きさの限界内で可能な限り小さくなるように、各接触対象面からロボットへの目標並進力を決定できる。
【0425】
また、S01で設定される仮想面が対向仮想面Sna,Snbを含む場合において、目標運動が不適切であると判断される場合と除いて、対向仮想面Sna,Snbの間でロボット1に発生する内力の大きさ(N_na,N_nbの大きさのうちの小さい方の大きさ)が前記内力上限値IFC_n以下に収まるように(前記第3必要条件を満足するように)、仮想面垂直抗力成分が決定される。このため、ロボット1の関節アクチュエータ11が発生可能な動力(本実施形態では駆動トルク)のうち、内力を発生するために必要な負担分が過大にならないように、対向仮想面Sna,Snbに対応する2つの接触対象面を含む各接触対象面における目標並進力を決定できる。
【0426】
ここで、以上説明した本実施形態と本願発明との対応関係を補足しておく。本実施形態ででは、S05〜S15、S19、S21、S61〜S83、S123〜S147の処理によって、本発明における仮想面必要並進力決定手段が実現される。また、S67、S79、S133及びS145の処理によって、本発明における接触対象面目標並進力決定手段が実現される。これら以外の対応関係は、第1実施形態と同じである。
【0427】
なお、以上説明した各実施形態では、前記壁登り歩容のように、ロボット1の外界(動作環境)に互いに間隔を存して対向する接触対象面WL1,WL2が存在する場合を想定して説明したが、本発明は、互いに間隔を存して対向する接触対象面が存在しない動作環境でロボットを運動を行なわせるための目標歩容を生成する場合にも適用できる。例えば、互いに交差する3つの接触対象面、あるいは2つの接触対象面にロボットの複数の部位を接触させながら、ロボットの運動を行なわせるための目標歩容を生成する場合にも本発明を適用できる。
【0428】
前記壁登り歩容以外の目標歩容の例を図19及び図20に示す。
【0429】
図19(a)〜(e)は、例えば互いに交差する2つの接触対象面として、床面FLと壁面WL1とが存在する動作環境で、ロボット1の運動を行なわせるための一例の目標歩容における該ロボット1の瞬時的な動作状態を時系列順に示している。
【0430】
この目標歩容におけるロボット1の運動は、次のような形態の運動である。すなわち、図19(a)に示すようにロボット1が2つのリンク機構5a,5bを床面FLに接地させて起立した状態から、図19(b)に示すように、リンク機構5c,5dを壁面WL1に接触させる。そして、この状態で、基体3を壁面WL1に近づけて、図19(c)に示す如く、該基体3を壁面WL1に寄り掛からせるように接触させる。その後、図19(d)に示すように、リンク機構5c,5dにより壁面WL1を押すことによって、基体3を壁面WL1から離反させ、最終的に、図19(e)に示す如く、リンク機構5c,5dを壁面WL1から離反させる。
【0431】
また、図20(a)〜(d)は、例えば互いに交差する3つの接触対象面として、床面FLと2つの壁面WL1,WL3とが存在する動作環境(例えば屋内の部屋の隅部)で、ロボット1の運動を行なわせるための一例の目標歩容における該ロボット1の瞬時的な動作状態を時系列順に示している。なお、図20(a)〜(d)は、ロボット1と動作環境とを上方から見た平面視で示している。そして、図20(a)〜(d)の各図では、ロボット1はリンク機構5a,5d(図示省略)を床面FLに接地させて起立している。
【0432】
この目標歩容におけるロボット1の運動は、次のような形態の運動である。すなわち、図20(a)に示すようにロボット1が床面FL上に接地させて起立した状態から、図20(b)に示すように、リンク機構5c,5dをそれぞれ壁面WL1,WL3に接触させる。そして、この状態で、基体3を壁面WL3に近づけて、図20(c)に示す如く、該基体3を壁面WL3に寄り掛からせるように接触させる。その後、リンク機構5c,5dによりそれぞれ壁面WL1,WL2を押すことによって、図20(d)に示すように、基体3とリンク機構5cとを壁面WL1から離反させると共に、リンク機構5dを壁面WL3から離反させる。
【0433】
図19又は図20に示したような目標歩容を生成する場合においても、前記した各実施形態における外力目標生成部27の処理によって、ロボット1に各接触対象面FL,WL1から作用させるべき目標外力と目標外力作用点とを適切に決定できる。
【0434】
なお、この場合、図19に示した目標歩容の生成処理において、前記S01の処理で設定される仮想面は、図19(a),(e)に示す動作状態では、1つの仮想面(図示例では仮想面S3a)であり、図19(b)〜(d)に示す動作状態では、2つの仮想面(図示例では仮想面S2a,S3a)である。
【0435】
また、図20に示した目標歩容の生成処理において、前記S01の処理で設定される仮想面は、図20(a),(d)に示す動作状態では、1つの仮想面(図示例では仮想面S3a)であり、図20(b),(c)に示す動作状態では、3つの仮想面(図示例では仮想面S1a,S2a,S3a)である。
【0436】
なお、図19又は図20に例示した場合のように、ロボット1の動作環境に対向仮想面Sna,Snbに対応する接触対象面が存在しない場合には、図7のS09、図9のS43、図15のS81、図16のS101、図17のS121の判断処理の判断結果が常に否定的となる。従って、これらの判断結果が肯定的となる場合に専用的な処理は不要である。
【0437】
また、前記各実施形態では、ロボット1に搭載した制御ユニット21で目標歩容を生成する場合を例に採って説明したが、本実施形態における目標歩容の生成処理(歩容生成部23の処理)は、ロボット1の実際の動作時に行なう必要はない。従って、歩容生成部23の処理は、ロボット1の外部の適宜のコンピュータによって随時行なうようにしてもよい。
【符号の説明】
【0438】
1…脚式移動ロボット、23…歩容生成部(歩容生成装置)、25…基本歩容要素生成部(目標運動生成手段)、S01…仮想面群設定手段、S03…必要総並進外力決定手段、S05〜S15,S19〜S45,S61〜S83,S103〜S115,S123〜S147…仮想面必要並進力決定手段、S29,S41,S67,S79,S111,S133,S145…接触対象面目標並進力決定手段、FL,WL1,WL2…接触対象面、S1a,S2a,S3a,S1b,S2b,S3b…仮想面。
【技術分野】
【0001】
本発明は、脚式移動ロボットの目標歩容を生成する装置に関する。
【背景技術】
【0002】
脚式移動ロボット(以下、単にロボットということがある)の目標歩容を生成する技術としては、例えば、本願出願人が提案した特許文献1、2に見られる技術等が従来より知られている。これらの技術は、2足歩行ロボット等の脚式移動ロボットを床上で歩行動作や走行動作によって移動させるような目標歩容を生成する技術である。その目標歩容は、ロボットの基体、脚リンク等の各部の運動(位置及び姿勢の時系列パターン)を規定する目標運動と、ロボットに床から作用させる床反力やその作用点(全床反力中心点)を規定する目標床反力とから構成される。
【0003】
なお、ロボットの実際の動作制御を行う場合には、一般に、ロボットの実際の運動を目標運動に制御するだけでは、ロボットの実際の動作環境(床形状等)と目標歩容で想定されていた動作環境との誤差等に起因して、目標運動を実現するために必要な外力をロボットに作用させることができない状況がしばしば発生する。そして、このような状況では、ロボットの運動の制御だけでなく、ロボットに外界(ロボットの動作環境)から作用させる外力の制御も必要となる。このため、ロボットの目標歩容には、一般に、ロボットの目標運動に加えて、上記目標床反力の如き、外力に関する目標も必要となる。
【先行技術文献】
【特許文献】
【0004】
【特許文献1】特許第3674788号公報
【特許文献2】特許第3679105号公報
【発明の概要】
【発明が解決しようとする課題】
【0005】
ロボットに種々様々な作業を行なわせるためには、ロボットを単に床上で移動させるだけでなく、ロボットの外界に存する複数の接触対象面にロボットの複数の部位を接触させながら(例えば、ロボットの脚の先端部と腕の先端部とをそれぞれ床面と壁面とに接触させながら)、ロボットの所要の運動を行なわせることが要求される場合もある。
【0006】
このような場合には、ロボットを単に床上で移動させる場合と異なり、ロボットの外界の複数の接触対象面からロボットの複数の部位に外力が作用することとなる。
【0007】
従って、このような場合には、ロボットの適切な動作制御を行うために、ロボットの目標運動に加えて、複数の接触対象面のそれぞれから作用させるべき外力に関する目標(目標外力や、その外力の作用点の目標位置)を設定しておく必要がある。
【0008】
ところが、上記のように複数の接触対象面からロボットに外力が作用する状況でのロボットの目標歩容を生成する場合には、ロボットの目標運動を、要求される運動形態に即して生成することは可能であるものの、その目標運動を実現するために、複数の接触対象面のそれぞれから動力学的にロボットに作用させるべき外力の時系列や組み合わせのパターンは、一般には1つのパターン又は限定的な範囲内のパターンに定まるものではなく、幅広い範囲で種々様々なパターンが存在し得る。
【0009】
従って、複数の接触対象面からロボットに外力が作用する状況でのロボットの目標歩容を生成する場合に、複数の接触対象面のそれぞれからロボットに作用させるべき外力に関する適切な目標をどのような指針で決定すべきかが問題となる。
【0010】
しかるに、従来は、上記の如く複数の接触対象面からロボットに外力が作用する状況での目標歩容を生成する技術については、十分な検討がなされていないのが実状である。従って、このような目標歩容を適切に生成し得る技術の構築が望まれていた。
【0011】
本発明はかかる背景に鑑みてなされたものであり、複数の接触対象面からロボットに外力が作用する状況でのロボットの目標歩容を生成する場合に、ロボットの目標運動を実現するためにロボットに作用させるべき外力に関する目標(特に並進力の目標値)を適切に決定することができる歩容生成装置を提供することを目的とする。
【課題を解決するための手段】
【0012】
本発明の脚式移動ロボットの歩容生成装置は、上記の目的を達成するために、脚式移動ロボットの動作環境に存する互いに異なる複数の接触対象面に該ロボットの複数の部位を接触させながら、該ロボットの運動を行なわせるための該ロボットの目標運動と、該ロボットに作用させるべき外力に関する目標とを含む目標歩容を生成する歩容生成装置であって、
前記ロボットの目標運動を生成する目標運動生成手段と、
相互の姿勢関係があらかじめ定められた複数の仮想面から成る仮想面群を用い、該仮想面群の複数の仮想面が前記複数の接触対象面に一致又は近似する面になるように、該仮想面群を前記ロボットの動作環境の空間に仮想的に設定する仮想面群設定手段と、
前記生成された目標運動に基づいて、該目標運動を実現するために前記ロボットに作用させるべきトータルの外力のうちの並進力成分である必要総並進外力を決定する必要総並進外力決定手段と、
前記決定された必要総並進外力を実現するために前記仮想面群の複数の仮想面のそれぞれから前記ロボットに作用させるべき並進力である仮想面必要並進力を決定する手段であり、前記複数の仮想面のそれぞれにおける仮想面必要並進力のうちの該仮想面に垂直な成分である仮想面垂直抗力成分の合力が、前記必要総並進外力のうちの、前記仮想面群の全ての仮想面に対して平行な成分を除く成分に一致するという第1A必要条件と、前記複数の仮想面のそれぞれにおける仮想面必要並進力のうちの該仮想面に平行な成分である仮想面摩擦力成分の合力が、前記必要総並進外力のうちの、前記仮想面群の全ての仮想面に対して平行な成分に一致するという第1B必要条件と、各仮想面における仮想面摩擦力成分の大きさが、少なくとも該仮想面における仮想面垂直抗力成分に応じて設定された第1所定値以下になるという第2必要条件とを少なくとも満足するように、各仮想面における仮想面必要並進力を決定する仮想面必要並進力決定手段と、
少なくとも前記各仮想面に対応して決定された前記仮想面必要並進力に基づいて、各仮想面に対応する接触対象面から前記ロボットに作用させるべき目標並進力を決定し、その決定した目標並進力を前記目標歩容の構成要素として出力する接触対象面目標並進力決定手段とを備えることを特徴とする(第1発明)。
【0013】
なお、本発明(後述の第2発明、第3発明を含む)において、前記ロボットの動作環境に存する互いに異なる複数の接触対象面というのは、その複数の接触対象面に含まれる任意の2つの接触対象面(又はその2つの接触対象面をそれぞれ包含する平面)が互いに交差するか、又は、互いに間隔を存して平行もしくはほぼ平行に対向するような複数の接触対象面を意味する。この場合、各接触対象面は、それに接触させるロボットの部位に比して広い面積を有する面(例えば、通常の床面の如き面)である必要はなく、例えば、該部位と同程度又はそれよりも小さい面積を有する局所的な面であってもよい。
【0014】
また、前記目標歩容の目標運動において、各接触対象面に接触させるロボットの部位は、1つの部位である必要はなく、2つ以上の部位であってもよい。この場合において、ある接触対象面に接触させるロボットの部位が2つ以上である場合には、該接触対象面からロボットに作用させるべき前記目標並進力は、該接触対象面からロボットの当該2つ以上の部位のそれぞれに作用させる並進力の合力の目標値を意味する。
【0015】
前記第1発明によれば、前記複数の接触対象面のそれぞれからロボットに作用させるべき外力に関する目標(目標歩容の構成要素)としての前記目標並進力を決定するために、相互の姿勢関係があらかじめ定められた複数の仮想面から成る仮想面群が、前記複数の接触対象面の代用的な面として使用される。そして、この仮想面群は、前記仮想面群決定手段によって、該仮想面群の複数の仮想面が前記複数の接触対象面に一致又は近似する面になるように、前記ロボットの動作環境の空間に仮想的に設定される。
【0016】
なお、前記仮想面群設定手段の処理において、前記仮想面群をロボットの動作環境の空間に仮想的に設定するということは、該仮想面群を構成する各仮想面の、当該空間での配置を決定すること、より詳しくは、当該空間での各仮想面の位置と姿勢(空間的な向き)とを決定することを意味する。
【0017】
また、前記目標運動生成手段によって生成された目標運動を実現するために前記ロボットに作用させるべきトータルの外力のうちの並進力成分である必要総並進外力が前記必要総並進外力決定手段によって決定される。
【0018】
ここで、前記必要総並進外力は、換言すれば、前記目標運動によって発生するロボット全体の並進慣性力(慣性力の並進力成分)と、該ロボット全体に作用する重力との合力に釣り合う並進力である。この場合、ロボットの目標運動によって、ロボットの各部(各リンク)の運動とロボットの全体重心点の運動とが規定されることとなるので、ロボットの適当な幾何学モデル(剛体リンクモデル)を用いることで、ロボット全体の上記並進慣性力を前記目標運動に基づいて決定できる。また、ロボット全体に作用する重力は、ロボットの全体質量と重力加速度との積である。従って、前記目標運動に基づいて、前記必要総並進外力を決定できる。
【0019】
なお、本発明では、前記目標運動は、例えば、ロボットに要求される運動形態(ロボットの脚リンクの先端部等の動かし方や、接触対象面への接触のさせ方等)に即して生成すればよい。
【0020】
そして、第1発明では、前記必要総並進外力決定手段によって決定された必要総並進外力を実現するために前記仮想面群の複数の仮想面のそれぞれから前記ロボットに作用させるべき並進力である仮想面必要並進力が前記仮想面必要並進力決定手段によって決定される。なお、各仮想面は、各接触対象面に対応しているので、各仮想面における仮想面必要並進力は、該仮想面に対応する接触対象面からロボットに作用させるべき並進力に対応するものである。
【0021】
ここで、該仮想面必要並進力決定手段が決定する、各仮想面における仮想面必要並進力は、換言すれば、それらの合力が、前記必要総並進外力に一致するような並進力を意味する。そして、各仮想面における仮想面必要並進力は、該仮想面に垂直な成分である前記仮想面垂直抗力成分と該仮想面に平行な成分である前記仮想面摩擦力成分との一方又は両方から構成される。
【0022】
この場合、本発明では、前記複数の接触対象面からロボットに外力を作用させることに対応して、前記仮想面群の複数の仮想面からロボットに外力を作用させることとなるので、前記必要総並進外力を実現するための各仮想面における仮想面必要並進力の組合せは、一般には、無数の態様を採り得る。
【0023】
但し、各仮想面に対応する接触対象面からロボットに作用させ得る並進力のうちの摩擦力成分(接触対象面に平行な成分)の大きさは、その上限値が該接触対象面における垂直抗力成分(接触対象面に垂直な成分)の影響を受け、該垂直抗力成分の大きさが小さいほど、摩擦力成分の大きさの上限値も小さくなる。また、該摩擦力成分が大きいと、接触対象面とロボットとの間の滑りも発生し易くなる。これに対して、接触対象面における垂直抗力成分は、摩擦力成分に比して幅広い範囲で、実現し得る。
【0024】
従って、各接触対象面に対応する仮想面における仮想面必要並進力は、できるだけ、仮想面摩擦力成分を含まないか、もしくは、該仮想面摩擦力成分の大きさが小さくなるように、決定することが望ましいと考えられる。換言すれば、前記必要総並進外力が、できるだけ、各仮想面における仮想面垂直抗力成分の合力によって実現され得るように、仮想面必要並進力を決定することが望ましいと考えられる。そして、前記必要総並進外力を実現するために、各仮想面における仮想面摩擦力成分が必要となる場合には、その仮想面摩擦力成分の大きさは、該仮想面における仮想面垂直抗力成分の大きさに応じて設定される、ある所定値以下になるように決定することが望ましい。
【0025】
そこで、第1発明では、前記仮想面必要並進力決定手段は、前記複数の仮想面のそれぞれにおける仮想面必要並進力のうちの仮想面垂直抗力成分の合力が、前記必要総並進外力のうちの、前記仮想面群の全ての仮想面に対して平行な成分を除く成分に一致するという第1A必要条件と、前記複数の仮想面のそれぞれにおける仮想面必要並進力のうちの仮想面摩擦力成分の合力が、前記必要総並進外力のうちの、前記仮想面群の全ての仮想面に対して平行な成分に一致するという第1B必要条件と、各仮想面における仮想面摩擦力成分の大きさが、少なくとも該仮想面における仮想面垂直抗力成分に応じて設定された第1所定値以下になるという第2必要条件とを少なくとも満足するように、各仮想面における仮想面必要並進力を決定する。
【0026】
なお、前記第2必要条件における第1所定値は、仮想面で発生可能な摩擦力の大きさの上限値に相当するものである。この第1所定値は、例えば、各仮想面に対応する接触対象面における垂直抗力成分を該仮想面における仮想面垂直抗力成分に一致させた場合に該接触対象面で発生可能な摩擦力の大きさの上限値に一致するか、またはそれよりも小さめの値に設定することが好ましい。
【0027】
これにより、前記仮想面必要並進力決定手段は、可能な限り、各仮想面における仮想面垂直抗力成分の合力によって前記必要総並進外力を実現すると共に、仮想面摩擦力成分を、前記第2必要条件を満足する範囲で必要最小限に留めるように、各仮想面における仮想面必要並進力を決定できることとなる。
【0028】
そして、第1発明では、前記接触対象面目標並進力決定手段が、少なくとも前記各仮想面に対応して決定された前記仮想面必要並進力に基づいて、各仮想面に対応する接触対象面から前記ロボットに作用させるべき目標並進力を決定し、その決定した目標並進力を前記目標歩容の構成要素として出力する。
【0029】
この場合、各仮想面に対応する接触対象面からロボットに作用させるべき目標並進力を、例えば、該目標並進力のうちの該接触対象面に垂直な方向の成分(垂直抗力成分)と該接触対象面に平行な成分(摩擦力成分)とがそれぞれ、該接触対象面に対応する仮想面における仮想面必要並進力の仮想面垂直抗力成分、仮想面摩擦力成分に一致するように決定すればよい。
【0030】
以上説明した第1発明によれば、前記したように、前記仮想面必要並進力決定手段は、可能な限り、各仮想面における仮想面垂直抗力成分の合力によって前記必要総並進外力を実現すると共に、仮想面摩擦力成分を、前記第2必要条件を満足する範囲で必要最小限に留めるように、各仮想面における仮想面必要並進力を決定できる。このため、各接触対象面からロボットへの目標並進力についても、可能な限り、各接触対象面における目標並進力のうちの垂直抗力成分の合力によって前記必要総並進外力を実現すると共に、該目標並進力のうちの摩擦力成分を、該接触対象面で発生可能な摩擦力の大きさの限界内で必要最小限に留めるように、各接触対象面からロボットへの目標並進力を決定できることとなる。
【0031】
そして、この場合、前記仮想面必要並進力決定手段の処理では、前記第1A必要条件、第1B必要条件、及び第2必要条件を用いることで、各接触対象面に対応する仮想面における仮想面必要並進力を決定するための指針(前記必要総並進外力を、各仮想面における仮想面必要並進力に分配する指針)が限定され、少なくとも、前記仮想面群の全ての仮想面に対して平行な成分を除く成分を、いずれかの仮想面における仮想面摩擦力成分に一致させ、又は分配することとなるような形態が排除される。また、各仮想面は、その相互の姿勢関係があらかじめ定まっているので、前記第1A必要条件、第1B必要条件、及び第2必要条件を満足し得る仮想面必要並進力を決定する処理のアルゴリズムの構築が容易になる。
【0032】
従って、第1発明によれば、複数の接触対象面からロボットに外力が作用する状況でのロボットの目標歩容を生成する場合に、ロボットの目標運動を実現するためにロボットに作用させるべき外力のうちの並進力の目標値を、適切な指針の基で決定することができる。
【0033】
かかる第1発明においては、前記仮想面必要並進力決定手段は、前記必要総並進外力のうちの前記仮想面群の全ての仮想面に対して平行な成分が“0”でない場合において、前記第1A必要条件、第1B必要条件及び第2必要条件を満足しつつ、前記仮想面群の複数の仮想面のうちの前記仮想面垂直抗力成分が“0”でない各仮想面において、該仮想面に対応して設定された前記第1所定値が大きいほど、該仮想面の仮想面摩擦力成分の大きさが大きくなるように、各仮想面における仮想面必要並進力を決定することが好ましい(第2発明)。
【0034】
この第2発明によれば、前記必要総並進外力のうちの前記仮想面群の全ての仮想面に対して平行な成分を、各仮想面に分配する指針がさらに限定され、各仮想面における仮想面並進力成分を決定するための処理のアルゴリズムの構築がより一層容易になる。
【0035】
また、各仮想面における仮想面摩擦力成分の大きさを、各仮想面で発生可能な摩擦力の上限値に相当する前記第1所定値の大きさに整合した大きさに設定することができる。ひいては、各接触対象面における目標並進力の摩擦力成分を、該接触対象面で発生可能な摩擦力の大きさの限界内にバランスよく収まるように(いずれかの接触対象面での摩擦力成分が、他の接触対象面での摩擦力成分に比して、摩擦力の大きさの限界に近づき過ぎたりしないように)、決定できる。
【0036】
前記第1発明又は第2発明において、前記複数の接触対象面が、互いに間隔を存して対向する2つの接触対象面を含むと共に、前記仮想面群が、当該2つの接触対象面に対応する仮想面として、互いに平行に対向する2つの仮想面である対向仮想面を含んでいる場合には、その2つの対向仮想面における仮想面垂直抗力成分は、それらの大きさが“0”で無い場合に、互いに逆向きの並進力となる。
【0037】
従って、2つの対向仮想面における仮想面垂直抗力成分のうち、大きさが小さい方の仮想面垂直抗力成分を第1仮想面垂直抗力成分、大きい方の仮想面垂直抗力成分を第2仮想面垂直抗力成分としたとき、第1仮想面垂直抗力成分と、第2仮想面垂直抗力成分のうちの第1仮想面垂直抗力成分と大きさの等しい成分とは、ロボットの運動に寄与しない内力として機能するものとなる。そして、この内力の大きさが過大になると、ロボットのアクチュエータが発生可能な動力のうち、上記内力の発生のために必要な負担分が大きくなり、ロボットの運動を変化させるために利用し得るアクチュエータの動力が不足し易いものとなる。
【0038】
そこで、前記第1発明又は第2発明では、前記複数の接触対象面が、互いに間隔を存して対向する2つの接触対象面を含むと共に、前記仮想面群が、当該2つの接触対象面に対応する2つの前記対向仮想面を含んでいる場合には、前記仮想面必要並進力決定手段は、次のように各仮想面における仮想面必要並進力を決定することが好ましい。
【0039】
すなわち、前記仮想面必要並進力決定手段は、前記決定された必要総並進外力が前記2つの対向仮想面と直交する方向の成分である対向仮想面直交成分を有する場合に、前記第1A必要条件、第1B必要条件及び第2必要条件を満足することに加えて、前記2つの対向仮想面のそれぞれにおける仮想面垂直抗力成分のうち、小さい方の仮想面垂直抗力成分の大きさがあらかじめ設定された第2所定値以下になるという第3必要条件を満足するように、各仮想面における仮想面必要並進力を決定することが好ましい(第3発明)。
【0040】
この第3発明では、前記第3必要条件における前記第2所定値は、前記2つの対向仮想面の間での前記内力の大きさの上限値に相当するものとなる。従って、第3発明によれば、前記決定された必要総並進外力が前記2つの対向仮想面と直交する方向の成分である対向仮想面直交成分を有する場合に、前記第1A必要条件、第1B必要条件及び第2必要条件を満足することに加えて、前記2つの対向仮想面の間での前記内力の大きさが過大になるのを防止しつつ、各仮想面における仮想面必要並進力を決定できる。
【0041】
ひいては、前記2つの対向仮想面に対応する2つ接触対象面間でロボットに発生する内力が過大になるのを防止するように、各接触対象面における目標並進力を決定できる。換言すれば、ロボットのアクチュエータが発生可能な動力のうち、目標運動の実現に利用し得る動力が前記内力によって不足するのを防止しつつ、各接触対象面における目標並進力を決定できる。
【図面の簡単な説明】
【0042】
【図1】本発明の実施形態における脚式移動ロボットの機構的な外力構成を示す図。
【図2】図1のロボットに備えた制御ユニットの機能とセンサ群とを示すブロック図。
【図3】図3(a)〜(c)は、図1のロボットの目標歩容の一例としての壁登り歩容におけるロボットの瞬時的な動作状態を時系列的に示す図。
【図4】図4(a),(b)は、図3(a)〜(c)の動作状態の後におけるロボットの瞬時的な動作状態を時系列的に示す図。
【図5】図5(a)〜(c)は壁登り歩容における外力作用点の設定位置の経時的変化の例を示すグラフ。
【図6】壁登り歩容における外力作用点の存在許容領域の面積の経時的変化の例を示すグラフ。
【図7】図2に示す外力目標生成部の、第1実施形態における処理を示すフローチャート。
【図8】図2に示す外力目標生成部の、第1実施形態における処理を示すフローチャート。
【図9】図2に示す外力目標生成部の第1実施形態における処理を示すフローチャート。
【図10】図7のS01で設定する仮想面を説明するための図。
【図11】図11(a)は図7のS05の処理により決定される仮想面必要並進力(仮想面垂直抗力成分及び仮想面摩擦力成分)の例を示す図、図11(b)は図7のS13までの処理により決定される仮想面必要並進力(仮想面垂直抗力成分及び仮想面摩擦力成分)の例を示す図。
【図12】図8のS19の処理を説明するための図。
【図13】図8のS25の処理を説明するための図。
【図14】図2に示す外力目標生成部の、第2実施形態における処理を示すフローチャート。
【図15】図2に示す外力目標生成部の、第2実施形態における処理を示すフローチャート。
【図16】図2に示す外力目標生成部の、第3実施形態における処理を示すフローチャート。
【図17】図2に示す外力目標生成部の、第4実施形態における処理を示すフローチャート。
【図18】図2に示す外力目標生成部の、第4実施形態における処理を示すフローチャート。
【図19】図19(a)〜(e)は図1のロボットの目標歩容の他の例におけるロボットの瞬時的な動作状態を時系列的に示す図。
【図20】図20(a)〜(d)は図1のロボットの目標歩容の他の例におけるロボットの瞬時的な動作状態を時系列的に示す図。
【発明を実施するための形態】
【0043】
[第1実施形態]
本発明の第1実施形態を図1〜図13を参照して以下に説明する。
【0044】
まず、図1を参照して、本実施形態における脚式移動ロボット1(以降、単にロボット1という)の機構的な概略構成を説明する。
【0045】
このロボット1は、基体3と、この基体3から延設された複数のリンク機構5とを有する。各リンク機構5は、ロボット1の脚として機能し得る機構である。そして、リンク機構5の個数は、本実施形態の例では4つである。
【0046】
なお、本実施形態の説明において、リンク機構5がロボット1の脚として機能し得るということは、該リンク機構5の先端部を、ロボット1の外界(動作環境)の床あるいは壁等の接触対象面に接触させた場合に、該リンク機構5が、ロボット1の自重(より正確には、ロボット1に作用する重力とロボット1の運動によって発生する慣性力との合力)の全部又は一部を支える外力を接触対象面から受けることができることを意味する。
【0047】
各リンク機構5は、複数の要素リンク7と、これらの要素リンク7を基体3側から順番に連接する複数の関節9とから構成されている。この場合、各リンク機構5の先端部を構成する要素リンク7(以降、これを先端部リンク7a、又は単にリンク機構5の先端部という)が、該リンク機構5を脚として機能させる場合に、ロボット1の外界の床あるいは壁等の接触対象面に接触させる部分である。そして、各先端部リンク7aは、板状に形成されており、その表面(外界との接触面)には図示を省略するゴム材等の弾性樹脂材が装着されている。
【0048】
各リンク機構5の各関節9は、例えば公知の構造の回転型の関節であり、それぞれ、1つ又は複数の自由度を有する。この場合、各リンク機構5に含まれる全ての関節9のトータルの自由度数が“6”以上の自由度数となるように、各リンク機構5毎の各関節9の自由度数が設定されている。このため、基体3に対する各先端部リンク7aの空間的な位置と姿勢(空間的な向き)とは、それぞれ3自由度を有する。なお、各リンク機構5には、回転型の関節の他に、直動型の関節が含まれていてもよい。
【0049】
また、図1では図示を省略するが、ロボット1には、各関節9をそれぞれ駆動する電動モータ等の複数の関節アクチュエータ11(図2に示す)が搭載されている。そして、各リンク機構5の各関節9は、それに対応する関節アクチュエータ11から、図示しない減速機等を含む動力伝達機構を介して駆動力が伝達されるようになっている。
【0050】
以上が本実施形態におけるロボット1の機構的な概略構成である。
【0051】
補足すると、図1に例示するロボット1では、各リンク機構5の要素リンク7のサイズや個数、関節9の個数、トータルの自由度数等を含めた各リンク機構5の構造が、全てのリンク機構5で同じであるように記載されているが、それらの構造は、全てのリンク機構5で同じである必要はない。
【0052】
また、図1では、ロボット1が、2つのリンク機構5a,5bを脚として機能させて平地を移動させる2脚移動ロボット(人型ロボットに類似するロボット)であるように記載されているが、例えば全てのリンク機構5を脚として機能させて平地を移動させるようなロボット(例えば4脚ロボット)であってもよい。
【0053】
また、リンク機構5の個数は、4個である必要ななく、例えば、3個もしくは5個以上であってもよい。
【0054】
次に、図2を参照して、以上の如く構成されたロボット1には、ロボット1の動作制御に利用されるセンサ群13と、CPU、RAM、ROM、関節アクチュエータ11の駆動回路等を含む電子回路ユニットにより構成された制御ユニット21とが搭載されている。
【0055】
センサ群13は、本実施形態の例では、ロボット1の代表リンク(代表部位)としての基体3の空間的な姿勢を計測するための姿勢センサ15と、各リンク機構5の先端部に作用する外力を計測するための力センサ17と、各関節9の変位量(本実施形態では回転角)を計測するための関節変位センサ19とを備える。
【0056】
この場合、姿勢センサ15は、例えばジャイロセンサ等の角速度センサ(レートセンサ)と加速度センサとから構成され、図1に示す如く基体3に搭載されている。また、力センサ17は、例えば3軸方向の並進力及び3軸周りのモーメントを検出可能な6軸力センサにより構成され、図1に示す如く、各リンク機構5の最も先端側の関節9と先端部リンク7aとの間に介装されている。また、関節変位センサ19は、例えば各関節9に装着されたエンコーダやポテンショメータ等により構成される。
【0057】
制御ユニット21は、ロボット1の適所、例えば基体3に搭載されている。この制御ユニット21は、実装されるプログラム等により実現される主要な機能として、ロボット1の目標とする動作を表す目標歩容を生成(作成)して出力する歩容生成部23と、この目標歩容に応じてロボット1の動作制御を行う動作制御部29とを備える。
【0058】
歩容生成部23の処理の詳細な説明は後述するが、該歩容生成部23が生成して出力する目標歩容は、ロボット1の目標運動と、ロボット1に外界から作用させる外力に関する目標とから構成される。
【0059】
この場合、目標歩容の構成要素のうちの目標運動は、各関節9の目標変位量の時系列と、ロボット1の代表リンク(本実施形態では基体3)のグローバル座標系での目標位置及び目標姿勢の時系列とから構成される。なお、代表リンク(基体3)の目標位置は、より詳しくは、該代表リンクに設定された代表点の目標位置である。また、グローバル座標系は、ロボット1の外界に対して固定される座標系である。
【0060】
また、目標歩容の構成要素のうちの外力に関する目標は、1つ以上のリンク機構5の先端部リンク7aを外界の接触対象面に接触させる場合に該接触対象面からロボット1に作用させるべき外力の目標値(目標外力)の時系列と、その外力の作用点の目標位置としての目標外力作用点の時系列とから構成される。この場合、目標外力は、本実施形態では、該外力のうちの並進力成分である並進外力の目標値(目標並進力)と、該外力のうち、外界の接触対象面に垂直な方向の軸周りでのねじり力(モーメント外力)の目標値(目標ねじり力)とから構成される。
【0061】
なお、接触対象面からロボット1に作用させるべき外力の作用点というのは、その接触対象面からロボットに作用する外力の全体によって、該作用点の周りに発生するモーメントのうち、該接触対象面に平行な軸周りの成分が“0”となるような点を意味する。その作用点は、ロボット1を単に床上で移動させる場合には、所謂、床反力中心点に相当する点である。このことは、後述する仮想面上の外力作用点についても同様である。
【0062】
補足すると、目標歩容に関する上記「時系列」は、ある期間内の瞬時値の順序列そのものでよいことはもちろんであるが、該時系列のパターンを規定する関数式等のパラメータであってもよい。
【0063】
また、ロボット1のあるリンク機構5のトータルの自由度数が6自由度である場合には、基体3に対する該リンク機構5の先端部リンク7aの位置及び姿勢を指定すれば、該リンク機構5の各関節9の変位量が一義的に決定されることとなる。従って、6自由度のリンク機構5については、該リンク機構5の各関節9の目標変位量の代わりに、該リンク機構5の先端部リンク7aの目標位置及び目標姿勢を、前記目標運動の構成要素として用いてもよい。
【0064】
さらに、必要に応じて、ロボット1の所定の運動状態量の目標値、例えば、ロボット1の全体重心点の位置、速度、加速度の目標値、あるいは、ロボット1の基体3以外の特定の部位の位置、姿勢の目標値を、歩容生成部23で生成する目標運動の構成要素に含めるようにしてもよい。
【0065】
動作制御部25は、歩容生成部23が生成した目標歩容にロボット1の実際の動作を追従させるようにロボット1の各関節9をそれに対応する関節アクチュエータ11を介して制御する。この場合、本実施形態では、姿勢センサ15の出力と関節変位センサ19の出力とから認識されるロボット1全体の重心点の実際の位置と、目標運動により規定されるロボット1全体の重心点の目標位置との偏差に応じて、該偏差を“0”に近づけるために、ロボット1に付加的に作用させるべき外力が決定される。さらに、この付加的外力を目標歩容における外力の目標値に付加してなる外力と目標運動とに、それぞれ、実際の外力(前記力センサ17の出力から認識される外力)とロボット1の実際の運動とを追従させるように、各関節9に付与すべき駆動力が決定される。そして、この決定された駆動力に応じて各関節アクチュエータ11の駆動力が制御される。
【0066】
次に、前記歩容生成部23による目標歩容の生成処理を詳細に説明する。なお、以降の説明では、4つのリンク機構5を区別する場合には、図1に示す如く参照符号5a,5b,5c,5dを付する。
【0067】
歩容生成部23は、本願発明を適用した処理によって目標歩容を生成する。この生成処理のアルゴリズムは、種々様々な動作環境でロボット1を運動させる歩容を生成することが可能である。
【0068】
ただし、本実施形態における目標歩容の生成処理は、特に、ロボット1の外界(動作環境)に存する互いに異なる複数の接触対象面にロボット1の複数の部位を接触させながら、ロボット1の運動を行なわせるための目標歩容を生成する場合に特徴を有する。
【0069】
そこで、以降の説明では、本実施形態の理解の便宜上、複数の接触対象面にロボット1の複数のリンク機構5の先端部リンク7aを接触させながら、ロボット1の運動を行なわせるための一例の目標歩容を参考的に提示する。そして、その一例の目標歩容の生成処理に関する説明を適宜行いつつ、歩容生成部23の処理を説明する。
【0070】
以降の本実施形態の説明で参考的に例示する目標歩容に係わるロボット1の動作環境は、ロボット1の外界に存する互いに異なる接触対象面として、例えば図1に示すように、床面FLと、この床面FLからほぼ垂直に起立する2つの壁面WL1,WL2とを備える動作環境である。この場合、2つの壁面WL1,WL2は、互いに間隔を存して平行もしくはほぼ平行に対向する接触対象面である。
【0071】
そして、本実施形態の説明で参考的に例示する目標歩容は、2つの壁面WL1,WL2の間で、床面FL上から壁面WL1,WL2をよじ登らせるようにロボット1を動作させる目標歩容である。以降、この目標歩容を壁登り歩容ということがある。
【0072】
この壁登り歩容におけるロボット1の動作形態の概要を図3(a),(b),(c)及び図4(a),(b)を参照して以下に説明しておく。図3(a),(b),(c)及び図4(a),(b)は、壁登り歩容におけるロボット1の瞬時的な動作状態(時刻t1〜t5での動作状態)を時系列順に示している。
【0073】
なお、これらの図では、便宜上、ロボット1の各リンク機構5のリンク要素7及び関節9の図示を省略し、各リンク機構5をその基端部(基体3側の端部)と先端部とを結ぶ線分として記載している。この場合、図面上は、各リンク機構5の先端部は点となるが、実際には、前記した板状の先端部リンク7aである。
【0074】
また、これらの図における矢印F1〜F10は、各図のロボット1の動作状態で、接触対象面FL又はWL1又はWL2からロボット1に作用する外力(並進外力)の例を視覚的に示している。この場合、各並進外力F1〜F10の始点が接触対象面FL又はWL1又はWL2からロボット1への該並進外力の作用点(外力作用点)に相当する。
【0075】
また、これらの図において、接触対象面FL又はWL1又はWL2上に記載している丸枠領域AR1〜AR12は、各図のロボット1の動作状態で、接触対象面FL又はWL1又はWL2からロボット1への外力作用点の概略的な存在可能領域を視覚的に示している。その存在可能領域は、単一のリンク機構5の先端部だけが接触している接触対象面では、その単一のリンク機構5の先端部リンク7aと接触対象面との接触面内の領域である。また、複数のリンク機構5の先端部が接触している接触対象面での外力作用点の存在可能領域は、それらの複数のリンク機構5の先端部リンク7aのそれぞれと接触対象面との各接触面内の領域をひとまとめに連接してなる領域(所謂、支持多角形に相当する領域)である。
【0076】
以下、これらの図を参照して壁登り歩容を説明すると、図3(a)は壁登り歩容の初期状態を示している。この初期状態では、ロボット1の2つのリンク機構5a,5bの先端部を床面FLに接触(接地)させた状態で、ロボット1が壁面WL1,WL2の間で床面FL上に起立する。また、ロボット1の壁登り歩容の動作を開始するために、ロボット1は、残りのリンク機構5c,5dのうちの壁面WL1側のリンク機構5cの先端部を該壁面WL1に接触させると共に、壁面WL2側のリンク機構5dの先端部を該壁面WL2に接触させる。
【0077】
この場合、図3(a)の初期状態では、ロボット1の自重(より正確には、ロボット1に作用する重力とロボット1の運動によって発生する慣性力との合力)を支えるための外力として、例えば、矢印F1で示す外力が、床面FLの領域AR1からロボット1に作用する。なお、図示例では、壁面WL1,WL2からロボット1に作用する外力は“0”である。
【0078】
次いで、ロボット1の動作状態は、図3(b)に示す状態に移行する。この図3(b)の動作状態は、図3(a)の初期状態で壁面WL1,WL2に接触させたリンク機構5c,5dの先端部をそれぞれ壁面WL1,WL2に押し付けつつ、リンク機構5a,5bのうちの一方(図示例では、リンク機構5a)の先端部を床面FL上から浮上させることによって、図3(a)の初期状態から移行する動作状態である。
【0079】
この場合、図3(b)の動作状態では、ロボット1の自重を支えるための外力として、例えば、床面FLの領域AR4から矢印F2で示す外力がロボット1に作用すると共に、壁面WL1の領域AR5と壁面WL2の領域AR6とからそれぞれ矢印F3,F4で示す外力(上向きの摩擦力成分を有する外力)がロボット1に作用する。
【0080】
次いで、ロボット1の動作状態は、図3(c)に示す状態に移行する。この図3(c)の動作状態は、図3(b)の動作状態で壁面WL1,WL2にそれぞれ押し付けたリンク機構5c,5dの先端部の押し付け力を増加させ、さらに、図3(b)の動作状態で床面FLに接触させていたリンク機構5bを床面FL上から浮上させる(ひいては、リンク機構5a,5bの両方を床面FL上から浮上させる)ことによって、図3(b)の動作状態から移行する動作状態である。
【0081】
この場合、図3(c)の動作状態では、ロボット1の自重を支えるための外力として、例えば、壁面WL1の領域AR7と、壁面WL2の領域AR8とから、それぞれ矢印F5,F6で示す外力(上向きの摩擦力成分を有する外力)がロボット1に作用する。
【0082】
次いで、ロボット1の動作状態は、図4(a)に示す状態に移行する。この図4(a)の動作状態は、図3(c)の動作状態までに床面FLから浮上させたリンク機構5a,5bのうちのリンク機構5aの先端部を、リンク機構5cの先端部の下側で壁面WL1に接触させると共に、リンク機構5bの先端部を、リンク機構5dの先端部の下側で壁面WL2に接触させることによって、図3(c)の動作状態から移行する動作状態である。
【0083】
この場合、図4(a)に示した動作状態は、リンク機構5a,5bをそれぞれ壁面WL1,WL2に接触させた直後の状態である。この状態では、例えば、壁面WL1の領域AR9のうち、リンク機構5cの先端部寄りの点を外力作用点として、矢印F7で示す外力(上向きの摩擦力成分を有する外力)がロボット1に作用すると共に、壁面WL2の領域AR10のうち、リンク機構5dの先端部寄りの点を外力作用点として、矢印F8で示す外力(上向きの摩擦力成分を有する外力)がロボット1に作用する。
【0084】
次いで、ロボット1の動作状態を、図4(a)に示した如くリンク機構5a,5cを壁面WL1に接触させ、且つ、リンク機構5b,5dを壁面WL2に接触させた状態に維持したまま、下側のリンク機構5a,5bのそれぞれの壁面WL1,WL2への押し付け力を増加させていくと共に、上側のリンク機構5c,5dのそれぞれの壁面WL1,WL2への押し付け力を減少させていく。
【0085】
これにより、壁面WL1からロボット1への外力作用点を、領域AR9のうちのリンク機構5aの先端部寄りの箇所に近づけていくと共に、壁面WL2からロボット1への外力作用点を、領域AR10のうちのリンク機構5bの先端部寄りの箇所に近づけていく。そして、最終的には、壁面WL1からロボット1への外力作用点を、リンク機構5aの先端部と壁面WL1との接触面内に移動させると共に、壁面WL2からロボット1への外力作用点を、リンク機構5bの先端部と壁面WL2との接触面内に移動させる。
【0086】
次いで、ロボット1の動作状態は、図4(b)に示す状態に移行する。この図4(b)の動作状態は、図4(a)の動作状態で、上記の如く壁面WL1,WL2からロボット1への外力作用点を移動させた後に、リンク機構5a,5bをそれぞれ壁面WL1,WL2に押し付けた状態を維持したまま、リンク機構5c,5dの先端部のうちの一方(図示例では、リンク機構5cの先端部)を壁面WL1から離反させることによって、図4(a)の動作状態から移行する動作状態である。
【0087】
この場合、図4(b)に示した動作状態では、ロボット1の自重を支えるための外力として、壁面WL1の領域AR11から矢印F9で示す外力(上向きの摩擦力成分を有する外力)がロボット1に作用すると共に、壁面WL2の領域AR12から矢印F10で示す外力(上向きの摩擦力成分を有する外力)がロボット1に作用する。
【0088】
その後は、リンク機構5dの先端部も壁面WL2から離反される。そして、壁面WL1,WL2から離反されたリンク機構5c,5dの先端部が、それぞれ、図4(a)の動作状態よりも高所で壁面WL1,WL2に接触される。そして、図3(b)以降の動作と同様の動作の繰り返しによって、ロボット1が壁面WL1,WL2をよじ登っていく。
【0089】
以上が、壁登り歩容の概略的な形態である。
【0090】
このような壁登り歩容等の目標歩容を生成する歩容生成部23が実行する処理は、図2のブロック図に示す如く、基本歩容要素生成部25の処理と、外力目標生成部27の処理とに大別される。
【0091】
上記基本歩容要素生成部25は、目標歩容のうちの基本的な構成要素として、ロボット1の目標運動と、暫定的な目標外力作用点である暫定外力作用点とを生成する処理を実行する機能部である。なお、基本歩容要素生成部25は、暫定外力作用点を決定するために、目標外力作用点の存在許容領域を設定する処理も実行する。
【0092】
また、上記外力目標生成部27は、基本歩容要素生成部25が生成した目標運動と暫定外力作用点とを用いて、該目標運動を実現する上で適切な外力の目標値(目標外力)と目標外力作用点とを決定する処理を実行する機能部である。
【0093】
この場合、本実施形態では、外力目標生成部27は、基本歩容要素生成部25が生成した目標運動を評価する機能を有する。そして、外力目標生成部27は、生成された目標運動が適切である場合に、目標歩容の全期間における目標外力と目標外力作用点とを決定する処理を実行し、該目標運動が不適切であると評価されるときには、目標外力と目標外力作用点とを決定する処理を中止する。
【0094】
歩容生成部23が目標歩容を生成するとき、歩容生成部23には、制御ユニット21にあらかじめ記憶保持された移動指令データ、あるいは、外部から無線通信などにより制御ユニット21に入力される移動指令データが与えられると共に、ロボット1の外界に存在する接触対象面等に関する環境情報が与えられる。
【0095】
上記移動指令データは、例えば、ロボット1の移動開始時刻や、移動経路、移動の仕方等を指定するデータである。また、上記環境情報は、例えば、ロボット1の外界の接触対象面(床面FL、壁面WL1,WL2等)のロボット1に対する相対位置及び相対姿勢を認識するために必要な情報や、各接触対象面の性状(摩擦係数等)を認識するために必要な情報等から構成される。
【0096】
なお、上記環境情報は、制御ユニット21に外部から無線通信等により入力される情報、又は制御ユニット21にあらかじめ記憶保持された情報でもよいが、ロボット1に搭載した図示しない撮像カメラ等の環境認識用のセンサの出力を基に、制御ユニット21で認識された情報であってもよい。
【0097】
そして、歩容生成部23は、これらの移動指令データと環境情報とを用いて、まず、基本歩容要素生成部25の処理を実行する。この基本歩容要素生成部25は、入力された移動指令データと環境情報とに基づいて、ロボット1の運動を制約する条件(ロボット1の目標運動が満たすべき条件)を設定し、その条件を満足させるように、逆運動学の演算処理によってロボット1の目標運動を生成する。
【0098】
この場合、ロボット1の運動を制約する条件は、基本的には、該ロボット1の要求される運動形態に応じて適宜設定される。その制約条件としては、例えば、各リンク機構5の先端部の運動を制約する条件(各リンク機構5の先端部リンク7aを外界の接触対象面に接触させる時刻の目標、その接触時における接触対象面上の接触位置の目標、その接触時における先端部リンク7aの姿勢の目標、外界の接触対象面に接触させていない先端部リンク7aの、ある時刻又は期間における位置及び姿勢の目標等)と、基体2の運動を制約する条件(ある時刻又は期間における基体2の位置及び姿勢の目標等)とを使用することができる。
【0099】
なお、基体2の運動を制約する条件に代えて、もしくは、該条件に加えて、ロボット1の運動に関する所定の状態量を制約する条件(例えばロボット1の全体の並進運動量又は角運動量の目標等)を使用するようにしてもよい。また、該制約条件には、一般には、ロボット1の関節9の可動範囲や動作速度(関節9の変位量の変化速度)の許容範囲等、機構的な制約条件も含まれる。
【0100】
前記壁登り歩容の生成を行なう場合には、基本歩容要素生成部25は、図3及び図4を参照して説明した時系列的な形態で、ロボット1の各リンク機構5の先端部が接触対象面FL,WL1,WL2のいずれかに接触する動作が行なわれるように、ロボット1の運動を制約する条件(例えば、各リンク機構5の先端部の運動を制約する条件と、基体3の運動を制約する条件とを含む制約条件)を決定する。そして、基本歩容要素生成部25は、この制約条件を満足させるように、逆運動学の演算処理によってロボット1の目標運動の時系列を生成する。なお、目標運動は、不連続に(ステップ状に)変化することが無いように生成される。
【0101】
また、基本歩容要素生成部25は、上記の如く決定した目標運動に基づいて、各接触対象面(1つ又は複数のリンク機構5の先端部リンク7aを接触させている面)における暫定外力作用点の時系列を決定する。
【0102】
この場合、基本歩容要素生成部25は、まず、上記の如く生成した目標運動に基づいて、各接触対象面における外力作用点の存在許容領域を設定する。より詳しくは、基本歩容要素生成部25は、上記の如く生成した目標運動において、1つのリンク機構5の先端部リンク7aだけを接触させる接触対象面においては、該接触対象面と先端部リンク7aとの接触面内に外力作用点の存在許容領域を設定する。その存在許容領域は、接触対象面と先端部リンク7aとの接触面内で、その接触面の境界に近づき過ぎないような領域に設定される。
【0103】
また、2つ以上のリンク機構5の先端部リンク7aを接触させる接触対象面においては、基本歩容要素生成部25は、該接触対象面と各先端部リンク7aとの接触面を連接してなる領域(所謂、支持多角形の領域)内に外力作用点の存在許容領域を設定する。その存在許容領域は、支持多角形の領域内で、該支持多角形の領域の境界に近づき過ぎないような領域に設定される。
【0104】
そして、基本歩容要素生成部25は、上記の如く設定した外力作用点の存在許容領域内の点(例えば、該存在許容範囲内の中央点)を暫定外力作用点として決定する。なお、各接触対象面上の暫定外力作用点は、該接触対象面にいずれかのリンク機構5が接触する状態が連続的に継続する期間内において、該作用点の位置が不連続に(ステップ状に)変化することが無いように決定される。
【0105】
歩容生成部23が図3及び図4に例示した壁登り歩容の生成を行なう場合には、暫定外力作用点は、例えば、図5(a),(b),(c)に例示する時系列パターンで生成される。なお、図5(a)〜(c)における横軸の時刻t1〜t5は、それぞれ順番に、図3(a)、図3(b)、図3(c)、図4(a)、図4(b)の動作状態に対応する時刻を示している。
【0106】
図5(a)〜(c)に示す例では、床面FL上の暫定外力作用点の位置(壁面WL1,WL2に垂直な方向での位置)は、時刻t1から時刻t2の直前までに壁面WL1側から壁面WL2側に連続的に移動していくように決定される。
【0107】
また、壁面WL1上の暫定外力作用点の位置(高さ方向での位置)は、時刻t3と時刻t4との間の途中時刻まで一定の位置(壁面WL1とリンク機構5cの先端部との接触面内の位置)に維持された後、該途中時刻から時刻t4の直前まで、下方に連続的に移動していくように決定される。次いで、壁面WL1上の暫定外力作用点の位置は、時刻t4と時刻t5との間の途中時刻まで一定の位置(リンク機構5cの先端部の壁面WL1への接触面とリンク機構5aの先端部の壁面WL1への接触面との間の中間位置)に維持された後、時刻t5の直前の時刻まで、下方に連続的に移動していき、さらに、時刻t5の前後の期間で一定の位置(壁面WL1とリンク機構5aの先端部との接触面内の位置)に維持されるように決定される。
【0108】
また、壁面WL2上の暫定外力作用点の位置(高さ方向での位置)は、時刻t3と時刻t4との間の途中時刻まで一定の位置(壁面WL2とリンク機構5dの先端部との接触面内の位置)に維持された後、該途中時刻から時刻t4の直前の時刻まで、下方に連続的に移動していき、その後、時刻t5の直後の時刻までの期間で一定の位置(リンク機構5dの先端部の壁面WL2への接触面とリンク機構5bの先端部の壁面WL2への接触面との間の中間位置)に維持されるように決定される。
【0109】
なお、この場合、床面FLと壁面WL1,WL2のそれぞれにおける外力作用点の存在許容領域は、その面積が、例えば図6に示すようなパターンで連続的に変化するように設定される。
【0110】
補足すると、基本歩容要素生成部25が生成する目標運動は、基本的には、ロボット1に行なわせようとする運動に関する要求を形式的に(少なくとも運動学的に)実現し得るように生成されたものであればよい。換言すれば、該目標運動は、動力学的な実現性が厳密に保障されたものでなくてもよい。従って、該目標運動を実現するためにロボット1に作用させるべき外力が、実際にはロボット1に作用させることが困難となる場合や、該外力が好適な範囲を逸脱するようなものとなる場合(該外力が過大になり過ぎる場合等)もあり得る。
【0111】
歩容生成部23は、次に、外力目標生成部27の処理を実行する。この外力目標生成部27には、基本歩容要素生成部25で生成された目標運動、暫定外力作用点及び外力作用点の存在許容領域が入力される。さらに、外力目標生成部27には、前記環境情報も入力される。
【0112】
そして、外力目標生成部27は、これらの入力データを用いて、図7〜図9のフローチャートに示す処理を実行することによって、目標歩容の目標外力と目標外力作用点とを決定する。図7〜図9のフローチャートに示す処理は、目標歩容の各時刻(所定の刻み時間毎の各時刻)における目標運動の評価を行いつつ、目標外力及び目標外力作用点の瞬時値を決定するための処理を示しており、この処理を目標歩容の各時刻毎に順番に行なうことによって、目標運動が適切である場合に、目標外力と目標外力作用点とが時系列的に決定される。
【0113】
なお、外力目標生成部27の処理に関する以降の説明においては、図7〜図9のフローチャートの処理により今現在、決定しようとする目標外力及び目標外力作用点の瞬時値に対応する時刻を今回時刻という。また、今回時刻の1つ前の時刻を前回時刻、次の時刻を次回時刻という。
【0114】
以下説明すると、外力目標生成部27は、まず、S01の処理を実行する。このS01では、外力目標生成部27は、目標運動と環境情報とを基に、今回時刻での目標外力及び目標外力作用点を決定するための演算処理に用いる作業用の仮想面を、ロボット1の外界の各接触対象面(目標運動における今回時刻でいずれかのリンク機構5の先端部を接触させる接触対象面)に一致又は近似させるように設定する。
【0115】
ここで、本実施形態では、外力目標生成部27の処理のアルゴリズムに汎用性を持たせると共に該アルゴリズムの構築を行い易くするために図10に示す如く、相互の姿勢関係があらかじめ定められた6つの平面S1a,S2a,S3a,S1b,S2b,S3bが、作業用の仮想面として使用し得る面として用意されている。そして、外力目標生成部27の処理では、ロボット1の目標運動を実現するために該ロボット1に各接触対象面から作用すべき外力は、各接触対象面に対応する仮想面からロボット1に作用する外力であると見なされる。
【0116】
この場合、本実施形態では、6つの仮想面のうちの仮想面S1a,S2a,S3aは相互に直交する平面とされ、仮想面S1b,S2b,S3bは、それぞれ仮想面S1a,S2a,S3aに平行な平面とされている。
【0117】
また、互いに平行に対向する2つの仮想面は、それぞれが発生し得る垂直抗力が互いに逆向き(各仮想面からこれに対向する仮想面に向かう向き)であるとされる。例えば、図10に示す如く、3軸直交座標系(XYZ座標系)を想定した場合において、X軸に直交する仮想面S1a,S1bがそれぞれ発生可能な垂直抗力N_1a,N_1bの向きは、それぞれX軸方向の正の向き(S1aからS1bに向かう向き)、X軸方向の負の向き(S1bからS1aの向かう向き)である。このことは、Y軸に直交する仮想面S2a,S2bの組、Z軸に直交する仮想面のS3a,S3bの組についても同様である。
【0118】
そして、S01の処理では、外力目標生成部27は、今回時刻での外界の各接触対象面(詳しくは、各接触対象面のうちのロボット1のリンク機構5の先端部との接触面)に、それぞれ、仮想面S1a,S2a,S3a,S1b,S2b,S3bのいずれか1つを対応付ける。そして、外力目標生成部27は、今回時刻での各接触対象面に対応付けた仮想面が、それに対応する接触対象面に一致又は近似するように、今回時刻での接触対象面と同数の仮想面を、ロボット1の外界(前記環境情報により認識される外界)に対して仮想的に設定する。なお、外界に仮想面を設定するということは、外界における仮想面の配置位置及び姿勢を決定することを意味する。
【0119】
この場合、今回時刻での外界の接触対象面が複数となる場合には、各接触対象面(各接触対象面のうちのロボット1のリンク機構5の先端部との接触面)とこれに対応する仮想面との間の姿勢差(空間的な角度差)が、いずれの接触対象面についてもできるだけ“0”又はそれに近いものとなるように、外界における仮想面の配置(接触対象面と同数の仮想面から成る仮想面群の配置)が決定される。例えば、各接触対象面毎の上記姿勢差の空間的な各成分(3軸周りの角度差成分)の2乗値又は絶対値を、今回時刻での全ての接触対象面に関して加え合わせてなる値(又は該2乗値又は絶対値の平均値)が最小となるように、今回時刻での全ての接触対象面と同数の仮想面から成る仮想面群の配置が決定される。
【0120】
従って、本実施形態では、今回時刻での外界の接触対象面が複数となる場合に、S01で設定される複数の仮想面(仮想面群)は、接触対象面と同数の仮想面により構成され、その相互の姿勢関係は、任意の2つの仮想面が互いに直交するか、又は互いに平行に対向するものとなる。
【0121】
なお、今回時刻での接触対象面が前回時刻での接触対象面と同一の接触対象面を有する場合には、今回時刻において、その同一の接触対象面に対応付けられる仮想面は、前回時刻と同じ仮想面に維持される。
【0122】
以上のS01の処理によって、歩容生成部23が、前記壁登り歩容を生成する場合には、例えば、次のように仮想面が設定される。すなわち、図3(a)又は図3(b)に示した動作状態の時刻では、6つの仮想面S1a,S2a,S3a,S1b,S2b,S3bのうちの互いに平行な2つの仮想面を含む3つの仮想面、例えば、仮想面S3a,S2a,S2bがそれぞれ接触対象面としての床面FL、壁面WL1、壁面WL2に対応付けられる(図中の括弧付きの参照符号を参照)。そして、これらの仮想面S3a,S2a,S2bがそれぞれ床面FL、壁面WL1、壁面WL2に一致又はほぼ一致する面になるように、ロボット1の外界における仮想面S3a,S2a,S2bの配置が決定される。
【0123】
また、図3(c)、図4(a)、図4(b)の動作状態の時刻では、図3(a),(b)の動作状態での壁面WL1,WL2にそれぞれ対応付けられた仮想面S2a,S2bが、そのまま、壁面WL1,WL2にそれぞれ対応付けられる(図中の括弧付きの参照符号を参照)。そして、これらの仮想面S2a,S2bがそれぞれ壁面WL1,WL2に一致又はほぼ一致するように、ロボット1の外界における仮想面S2a,S2bの配置が決定される。
【0124】
なお、以降の本実施形態の説明では、S01で設定される仮想面のうち、互いに平行に対向する2つの仮想面を対向仮想面ということがある。この対向仮想面の組は、本実施形態では、仮想面S1a,S1bの組、仮想面S2a,S2bの組、仮想面S3a,S3bの組のいずれかである。そして、これらの各組の対向仮想面を総称的に表記する場合に、対向仮想面Sna,Snb(n=1又は2又は3)、あるいは、単に対向仮想面Sna,Snbと表記することがある。
【0125】
図5の説明に戻って、外力目標生成部27は、次に、S03の処理を実行する。このS03では、外力目標生成部27は、基本歩容要素生成部25が作成した目標運動を基に、この目標運動を実現するために、今回時刻でロボット1に作用させるべきトータルの外力である必要総外力のうちの並進力成分である必要総並進外力Fallと、該必要総外力のうちの所定の基準点周りのモーメント成分である必要総モーメント外力Mallとを決定する。
【0126】
ここで、上記所定の基準点(以降、モーメント基準点という)は、ロボット1の外界に対して固定された点(例えば、外界の接触対象面又はこれに対応する仮想面上に設定された点)である。このモーメント基準点は、目標歩容の全期間で一定の点に維持してもよいが、目標歩容の所定の期間毎に適宜更新するようにしてもよい。
【0127】
なお、本実施形態における外力目標生成部27の処理では、必要総並進外力Fall、必要総モーメント外力Mall等の並進力もしくはモーメント、あるいは、空間的な位置、姿勢等の変数(ベクトル量)は、S01で設定される仮想面に対して固定されたグローバル座標系(例えば、各仮想面に対して図10に示すような位置及び姿勢関係で設定される3軸直交座標系(XYZ座標系))で記述される。
【0128】
上記必要総並進外力Fall及び必要総モーメント外力Mallは、より具体的には次のように決定される。すなわち、必要総並進外力Fallを決定する場合には、外力目標生成部27は、ロボット1の目標運動を基に、ロボット1の幾何学モデル(剛体リンクモデル)を用いて、ロボット1の全体重心点の目標位置の2階微分値である目標重心加速度(今回時刻での目標重心加速度)を算出する。なお、ロボット1の全体重心点の目標位置、あるいは、目標重心加速度は、前記基本歩容要素生成部25で目標運動の要素として算出しておくようにしてもよい。
【0129】
そして、外力目標生成部27は、この目標重心加速度とロボット1の全体質量とから算出される並進慣性力(ロボット1の目標運動によって発生する慣性力の並進力成分)とロボット1に作用する重力との合力に釣り合う並進力を必要総並進外力Fallとして算出する。
【0130】
なお、目標運動によって発生するロボット1の全体の並進慣性力は、例えば、ロボット1の各リンク3,7毎の重心点の並進慣性力を加え合わせることによって算出するようにしてもよい。
【0131】
また、必要総モーメント外力Mallを決定する場合には、外力目標生成部27は、ロボット1の目標運動を基に、ロボット1の幾何学モデル(剛体リンクモデル)を用いて、ロボット1の目標運動によって該ロボット1の全体重心点の周りに発生する今回時刻での慣性力モーメント(慣性力のモーメント成分)と、該目標運動におけるロボット1の全体重心点の運動(並進運動)が前記モーメント基準点周りに発生する今回時刻での慣性力モーメントとの総和の慣性力モーメントを算出する。そして、外力目標生成部27は、この総和の慣性力モーメントに釣り合うモーメントを今回時刻での必要総モーメント外力Mallとして算出する。
【0132】
なお、ロボット1の目標運動によって発生する並進慣性力の時系列と、前記モーメント基準点周りの慣性力モーメントの時系列とを前記基本歩容要素生成部25で算出しておいてもよい。そして、今回時刻での並進慣性力の値と慣性力モーメントの値とを基本歩容要素生成部25から外力目標生成部27に入力するようにしてもよい。
【0133】
上記の如く必要総並進外力Fall及び必要総モーメント外力Mallを算出した後、外力目標生成部27は、次に、S05〜S15の処理を実行する。このS05〜S15の処理は、ロボット1の目標運動(より詳しくは、該目標運動により規定されるロボット1の全体重心点の並進運動)を実現するために、S01で設定した各仮想面からロボット1に作用させるべき並進力である仮想面必要並進力を決定する処理である。この各仮想面における仮想面必要並進力は、該仮想面に垂直な成分である仮想面垂直抗力成分と、該仮想面に平行な成分である仮想面摩擦力成分とから構成される。なお、S05〜S15の処理によって決定される仮想面必要並進力は暫定値であり、後述のS19以降の処理によって修正される場合がある。また、S05〜S15の処理には、決定する仮想面必要並進力に基づいて、目標運動の評価を行なう処理も含まれる。
【0134】
以下、S05〜S15の処理を具体的に説明すると、外力目標生成部27は、まず、S05の処理を実行する。このS05では、外力目標生成部27は、S03で決定した必要総並進外力Fallに応じて、各仮想面における仮想面必要総並進力のうちの、該仮想面に垂直な成分(垂直抗力成分)である仮想面垂直抗力成分N_iと、該仮想面に平行な成分(摩擦力成分)である仮想面摩擦力成分F_iとを決定する。
【0135】
なお、本実施形態の説明では、参照符号N_i,F_i等における添え字“i”は、今回時刻におけるS01で設定された仮想面のうちの任意の仮想面の識別符号を意味する。そして、この添え字“i”を付した参照符号は、識別符号iの仮想面に関連するものの参照符号であることを意味する。この場合、本実施形態の説明では、仮想面S1a,S2a,S3a,S1b,S2b,S3bのそれぞれの識別符号は、仮想面を表す参照符号中の“S”の添え字、すなわち、1a,2a,3a,1b,2b,3bであるとする。そして、ある特定の仮想面に関連する参照符号には、その特定の仮想面の添え字と同じ添え字を付する。例えば、仮想面S2aにおける仮想面垂直抗力成分の参照符号を“N_2a”と表記する。
【0136】
この場合、外力目標生成部27は、今回時刻のS01で設定した全ての仮想面における仮想面必要並進力の合力がS03で決定した必要総並進外力Fallに一致するという第1必要条件を満足するように、各仮想面における仮想面必要並進力を構成する仮想面垂直抗力成分N_iと仮想面摩擦力成分F_iとを決定する。
【0137】
より詳しくは、本実施形態におけるS05の処理では、上記第1必要条件を満足するために、外力目標生成部27は、今回時刻で設定した全ての仮想面における仮想面垂直抗力成分N_iの合力が、必要総並進外力Fallのうち、当該全ての仮想面に対して平行な成分を除く成分に一致するという第1A必要条件と、当該全ての仮想面における仮想面摩擦力成分F_iの合力が、必要総並進外力Fallのうち、当該全ての仮想面に対して平行な成分に一致するという第1B必要条件とを満足するように、各仮想面における仮想面垂直抗力成分N_iと仮想面摩擦力成分F_iとを決定する。
【0138】
なお、S05の処理では、今回時刻で設定した仮想面が、互いに平行に対向する2つの対向仮想面Sna,Snb(n=1又は2又は3)を含んでおり、且つ、必要総並進外力Fallが、これらの対向仮想面Sna,Snbに垂直な成分Fall_nを有するような場合には、外力目標生成部27は、これらの対向仮想面Sna,Snbのうち、Fall_nと同じ向きの垂直抗力を発生可能な対向仮想面における仮想面垂直抗力成分Sna又はSnbを、Fall_nに一致させると共に、他方の対向仮想面における仮想面垂直抗力成分Snb又はSnaを“0”に設定する。
【0139】
また、S05の処理では、今回時刻で設定した仮想面が複数であり、且つ、必要総並進外力Fallが全ての仮想面に平行な成分を有する場合には、外力目標生成部27は、上記第1A必要条件を満たすように、各仮想面における仮想面垂直抗力成分N_iを決定した上で、前記第1B必要条件を満たしつつ、各仮想面における仮想面摩擦力成分F_iの大きさが、該仮想面で発生可能な摩擦力の上限値に応じた大きさとなるように(詳しくは該仮想面で発生可能な摩擦力の上限値が大きいほど、仮想面摩擦力成分F_iの大きさが大きくなるように)、各仮想面の仮想面摩擦力成分F_iを決定する。
【0140】
具体的には、外力目標生成部27は、今回時刻で設定した各仮想面の摩擦係数μ_iを、前記環境情報(詳しくは、該仮想面に対応する接触対象面の摩擦係数に関する環境情報)に基づいて設定する。この場合、各仮想面の摩擦係数μ_iは、該仮想面に対応する接触対象面の実際の摩擦係数と同一もしくはほぼ同一の値に設定してもよいが、余裕を見込んで、実際の摩擦係数よりも小さめに設定しておくようにしてもよい。
【0141】
なお、各仮想面の摩擦係数μ_iの設定は、目標歩容の各時刻で行なう必要はない。例えば、外力目標生成部27の処理を開始する前に、各接触対象面に対応付ける仮想面の摩擦係数を前記環境情報に基づいて事前に決定して記憶保持しておき、その記憶保持した摩擦係数から、S01で設定した各仮想面に対応させる摩擦係数を選定してもよい。
【0142】
さらに、外力目標生成部27は、各仮想面において、その仮想面に対応する摩擦係数μ_iを仮想面垂直抗力成分N_iの大きさ|N_i|に乗じてなる値(=μ_i*|N_i|)を、その仮想面で発生可能な摩擦力の上限値(以降、単に摩擦力上限値という)として設定する。
【0143】
なお、本明細書では、「*」は乗算記号として使用する。この場合、ベクトル同士の乗算演算における「*」は、外積(ベクトル積)を意味するものとする。
【0144】
そして、外力目標生成部27は、上記の如く設定した摩擦力上限値が大きい仮想面ほど、仮想面摩擦力成分F_iの大きさがより大きくなるように、各仮想面における仮想面摩擦力成分F_iを決定する。
【0145】
例えば、上記第1B必要条件を満たしつつ、各仮想面の仮想面摩擦力成分F_iの大きさの相互の比率が、各仮想面の摩擦力上限値の相互の比率に一致するように、各仮想面の仮想面摩擦力成分F_iが決定される。なお、この場合、例えば、Fallのうちの全ての仮想面に平行な成分を、最も大きな摩擦力上限値を有する仮想面の仮想面摩擦力成分に一致させ、他の仮想面の仮想面摩擦力成分を“0”に設定するようにしてもよい。
【0146】
歩容生成部23が前記壁登り歩容を生成する場合には、以上説明したS05の処理によって、各仮想面における仮想面垂直抗力成分N_i及び仮想面摩擦力成分F_iは例えば、次のように決定されることとなる。
【0147】
例えば、図3(b)に示した動作状態の時刻t2において、必要総並進外力Fallが、図11(a)に示す如く床面FLに対応する仮想面S3aに垂直な成分Fall_zと、壁面WL1,WL2にそれぞれ対応する仮想面(対向仮想面)S2a,S2bに垂直な成分Fall_yと、これらの仮想面S3a,S2a,S2bの全てに平行な成分Fall_xを有する状況を想定する。
【0148】
この状況においては、S05の処理では、仮想面S3aの仮想面垂直抗力成分N_3aと仮想面S2aの仮想面垂直抗力成分N_2aとがそれぞれ、Fall_z、Fall_yに一致する値に決定されると共に、仮想面S2bの仮想面垂直抗力成分N_2bが“0”に決定される。これにより前記第1A必要条件を満足する各仮想面垂直抗力成分N_3a,N_2a,N_2bが決定される。なお、ここでは、μ_3a*|N_3a|>μ_2a*|N_2a|であるとする。
【0149】
そして、仮想面S3aの仮想面摩擦力成分F_3aと仮想面S2aの仮想面摩擦力成分F_2aとの合力(=F_3a+F_2a)がFall_xに一致し、且つ、|F_3a|>|F_2a|となるように(例えば|F_2a|/|F_3a|=μ_2a*|N_2a|/(μ_3a*|N_3a|)となるように)、仮想面S3aの仮想面摩擦力成分F_3aと仮想面S2aの仮想面摩擦力成分F_2aとが決定されると共に、仮想面S2bの仮想面摩擦力成分F_2bが“0”に決定される。これにより前記第1B必要条件を満足する各仮想面摩擦力成分F_3a,F_2a,F_2bが決定される。
【0150】
なお、壁登り歩容における図3(c)の動作状態以降の時刻では、必要総並進外力Fallが例えば図11(a)に示した場合と同様であるとした場合、S05の処理では、仮想面S2aの仮想面垂直抗力成分N_2aが、FallのうちのFall_yに一致し、且つ、仮想面摩擦力成分F_2aが、FallのうちのFall_zとFall_xとの合力に一致するように仮想面垂直抗力成分N_2a及び仮想面摩擦力成分F_2aが決定されると共に、仮想面S2bの仮想面垂直抗力成分N_2b及び仮想面摩擦力成分F_2bはいずれも“0”に決定される。
【0151】
図5のフローチャートの説明に戻って、以上のようにS05の処理を実行した後、外力目標生成部27は、S07の処理を実行する。このS07では、外力目標生成部27は、今回時刻のS01で設定した全ての仮想面のそれぞれにおいて、仮想面摩擦力成分F_iの大きさが仮想面垂直抗力成分N_iの大きさに応じて設定される所定の上限値としての前記摩擦力上限値以下になるという第2必要条件を満足するか否かを判断する。すなわち、外力目標生成部27は、S01で設定した全ての仮想面のそれぞれにおいて、前記の如く決定した摩擦力上限値(=μ_i*|N_i|)を用い、|F_i|≦μ_i*|N_i|であるか否かを判断する。
【0152】
外力目標生成部27は、S07の判断結果が肯定的である場合には、各仮想面における仮想面必要並進力(暫定値)を決定する処理を終了し、図8に示すS19以降の処理を実行する。この場合、S05で決定された、各仮想面における仮想面垂直抗力成分N_iと仮想面摩擦力成分F_iとの組が、そのまま仮想面必要並進力(暫定値)として決定されることとなる。
【0153】
一方、S07の判断結果が否定的である場合には、外力目標生成部27は、次にS09の処理を実行する。このS09では、外力目標生成部27は、S07の判断結果が否定的となる仮想面が、前記対向仮想面Sna又はSnb(n=1又は2又は3)であるか否かを判断する。
【0154】
このS09の判断結果が否定的となる状況では、S07の判断結果が否定的となる仮想面における仮想面垂直抗力成分の大きさを、前記第1A必要条件を満たしつつ増加させることができず、ひいては該仮想面における仮想面摩擦力成分の大きさを、前記摩擦力上限値以下で増加させることができない。そこで、この場合には、外力目標生成部27は、S17において、目標運動が不適切である(目標運動を修正する必要がある)とし、図7〜図9のフローチャートの処理(目標外力及び目標外力作用点を決定する処理)を中止する。
【0155】
従って、本実施形態では、S05において前記第1A必要条件及び第1B必要条件を満足するように仮想面必要並進力(仮想面垂直抗力成分N_iと仮想面摩擦力成分F_iとの組)を決定した上で、S07、S09の判断処理によって目標運動が評価される。そして、S07及びS09の判断結果がいずれも否定的となる場合に、目標運動が不適切であると評価されることとなる。
【0156】
なお、S17で目標運動が不適切であると評価される場合、外力目標生成部27は、基本歩容要素生成部25にエラー情報を出力する。そして、基本歩容要素生成部25は、そのエラー情報に応じて、目標運動を作成し直す。
【0157】
一方、S09の判断結果が肯定的となる状況では、互いに平行な対向仮想面Sna,Snb(n=1又は2又は3)の仮想面垂直抗力成分N_na,N_nbの大きさを、前記第1A必要条件を満足しつつ(N_na+N_nbを一定に保ちつつ)、増加させることができる。すなわち、N_naとN_nbとは、互いに逆向きの垂直抗力であるので、それらの大きさを同じ量だけ増加させても、N_naとN_nbとの総和は変化しない。
【0158】
そこで、S09の判断結果が肯定的となる場合には、外力目標生成部27は、次に、S11の処理を実行する。このS11では、外力目標生成部27は、対向仮想面Sna,Snbの仮想面垂直抗力成分N_na,N_nbの大きさの増加量IN_nを、前記第2必要条件を満足させ得るように決定する。
【0159】
具体的には、外力目標生成部27は、対向仮想面Sna,Snbのうちの、S07の判断結果が否定的となった対向仮想面において、その仮想面摩擦力成分F_i(=F_na又はF_nb)の大きさから、S07の判断処理で用いた摩擦力上限値(=μ_i*|N_i|)を差し引いたものを、摩擦係数μ_iで除算してなる値を、上記増加量IN_nとして算出する。
【0160】
すなわち、外力目標生成部27は、次式01により、増加量IN_nを算出する。
【0161】
IN_n=(|F_i|−μ_i*|N_i|)/μ_i ……式01
次いで、外力目標生成部27は、S13の処理を実行する。このS13では、外力目標生成部27は、対向仮想面Sna,Snbの仮想面垂直抗力成分N_na,N_nbをそれぞれ、上記の如く決定した増加量IN_nだけ、S05の処理で決定した値の大きさよりも増加させ、その増加後の値を改めて、対向仮想面Sna,Snbのそれぞれの仮想面垂直抗力成分N_na,N_nbとして決定する。
【0162】
このS13までの処理によって、第1A必要条件、第1B必要条件及び第2必要条件を満足する仮想面必要並進力が決定されることとなる。
【0163】
次いで、外力目標生成部27は、S15の処理を実行する。このS15では、外力目標生成部27は、S13で大きさを増加させた仮想面垂直抗力成分N_na,N_nbの大きさのうちの小さい方の大きさ(=min(|N_na|,|N_nb|))があらかじめ設定された所定値IFC_n以下になるという第3必要条件を満足するか否かを判断する。
【0164】
ここで、min(|N_na|,|N_nb|)は、ロボット1の全体の並進運動に寄与しない内力に相当するものである。このような内力は、その大きさが大きくなると、ロボット1の関節アクチュエータ11が発生可能な動力(駆動トルク)のうち、該内力を発生させるために必要な負担分が増大し、ロボット1の所望の運動を行なうための利用し得る関節アクチュエータ11の動力(ロボット1の全体の並進運動を変化させるために利用し得る動力)が不足し易くなる。そこで、本実施形態では、この内力を所定値IFC_n以下に制限するために、S15の判断処理を実行する。
【0165】
なお、この所定値IFC_n(以降、内力上限値IFC_nという)は、例えば対向仮想面Sna,Snbにより発生する内力の向き(対向仮想面Sna,Snbの法線方向)に応じて設定される。ただし、内力上限値IFC_nを、内力の向きによらずに固定的な値に設定するようにしてもよい。
【0166】
このS15の判断結果が肯定的である場合には、外力目標生成部27は、S07の判断結果が肯定的である場合と同様に、各仮想面における仮想面必要並進力(暫定値)を決定する処理を終了し、図8に示すS19以降の処理を実行する。
【0167】
この場合、S07の判断結果が否定的となった仮想面を含む対向仮想面Sna,Snaを除く各仮想面については、S05で決定された仮想面垂直抗力成分N_iと仮想面摩擦力成分F_iとの組が、そのまま仮想面必要並進力(暫定値)として決定されることとなる。
【0168】
そして、上記対向仮想面Sna,Snaのそれぞれにおいては、S13で増加させた仮想面垂直抗力成分N_i(N_na,N_nb)と、S05で決定された仮想面摩擦力成分F_i(F_na,F_nb)との組が、仮想面必要並進力(暫定値)として決定されることとなる。
【0169】
また、S15の判断結果が否定的となる状況では、内力に関する第3必要条件を満足しつつ、ロボット1の目標運動を実現するために動力学的に必要な外力をロボット1に作用させることができないか、もしくは困難となる。このため、S15の判断結果が否定的となる場合には、外力目標生成部27は、S09の判断結果が否定的となる場合と同様に、S17において、目標運動が不適切である(目標運動を修正する必要がある)とし、図7〜図9のフローチャートの処理を中止する。
【0170】
補足すると、本実施形態では、S01で設定される仮想面が互いに平行な対向仮想面Sna,Snb(n=1又は2又は3)を含む場合に、S05の処理で決定される対向仮想面Sna,Snbの仮想面垂直抗力成分N_na,N_nbのうちの一方は“0”であるので、これらのN_na,N_nbの組は常に前記第3必要条件を満足する。従って、S07の判断結果が肯定的である場合に、S15の判断処理を行う必要は無い。
【0171】
以上のS05〜S15の処理によって、S09又はS15の判断結果が否定的となる場合(目標運動が不適切である場合)を除いて、前記第1A必要条件、第1B必要条件、第2必要条件及び第3必要条件を満足するように、各仮想面の仮想面必要並進力(暫定値)を構成する仮想面垂直抗力成分N_iと仮想面摩擦力成分F_iとの組が決定されることとなる。これにより、第1A必要条件、第1B必要条件、第2必要条件及び第3必要条件を満足しつつ、ロボット1の目標運動におけるロボット1の全体の並進運動(全体重心点の並進運動)を実現し得る仮想面必要並進力(暫定値)が決定されることとなる。
【0172】
従って、本実施形態では、S13までの処理によって、第1A必要条件、第1B必要条件及び第2必要条件を満足する仮想面必要並進力を決定した上で、S15の判断処理によって、再度、目標運動が評価される。そして、このS15の判断結果が否定的となる場合に、目標運動が不適切であると評価されることとなる。
【0173】
また、S07、S09、S15の判断結果がいずれも肯定的となる場合には、第1A必要条件、第1B必要条件、第2必要条件及び第3必要条件を満足し得る仮想面必要並進力を決定できることとなるので、基本歩容要素生成部25で生成された目標運動は、少なくとも該目標運動のうちのロボット1全体の並進運動(ロボット1の全体重心点の並進運動)を支障なく実現し得るように、ロボット1に外力を作用させることができる適切な目標運動であると言えることとなる。
【0174】
歩容生成部23が前記壁登り歩容を生成する場合に、以上説明したS05〜S15の処理によって仮想面垂直抗力成分N_i及び仮想面摩擦力成分F_iは例えば、次のように決定されることとなる。
【0175】
例えば、前記壁登り歩容のうちの図3(b)に示す動作状態の時刻t2において、S05の処理によって、前記した図11(a)に示した如く、各仮想面における仮想面垂直抗力成分N_i及び仮想面摩擦力成分F_i(i=3a,2a,2b)が決定された場合を想定する。この場合、仮想面S3aにおける仮想面摩擦力成分F_3aと、仮想面S2aにおける仮想面摩擦力成分F_2aとが、前記第2必要条件を満たしている場合(S07の判断結果が肯定的になる場合)には、S15までの処理によって最終的に決定される、各仮想面における仮想面必要並進力(N_i,F_i)は、S05で決定されたものに一致する。
【0176】
一方、例えば、仮想面S2aにおける仮想面摩擦力成分F_2aが前記第2必要条件を満たしていない場合には、図11(b)に示す如く、該仮想面S2aを含む対向仮想面S2a,S2bのそれぞれの仮想面垂直抗力成分N_2a,N_2bが、S05で決定された値よりも、S11で決定される増加量IN_2だけ増加してなる値に決定される。また、仮想面S3aの仮想面垂直抗力成分N_3a及び仮想面摩擦力成分F_3aと、仮想面S2a,S2bのそれぞれの仮想面摩擦力成分F2a,F2bの大きさは、S05で決定された値と同じ値に決定される。
【0177】
補足すると、本実施形態では、S01で設定される仮想面が互いに平行な2つの対向仮想面Sna,Snb(n=1又は2又は3)を含む場合に、S05の処理によって、一方の対向仮想面Sna又はSnbの仮想面垂直抗力成分及び仮想面摩擦力成分はいずれも常に“0”に決定される。但し、S05の処理において、必要総並進外力Fallが対向仮想面Sna,Snbに垂直な成分を有する場合に、前記第1A必要条件及び第3必要条件を満足する範囲で、両方の対向仮想面Sna,Snbの仮想面垂直抗力成分を“0”でない値に決定してもよい。そして、この場合において、必要総並進外力Fallが全ての仮想面に平行な成分を有する場合に、前記第1B必要条件を満足する範囲内で両方の対向仮想面Sna,Snbの仮想面摩擦力成分を“0”でない値に設定するようにしてもよい。
【0178】
なお、S05の処理において、上記の如く、両方の対向仮想面Sna,Snbの仮想面摩擦力成分を“0”でない値に設定するようにした場合には、その両方の対向仮想面Sna,Snbにおいて、S07の判断結果が否定的となる場合もあり得る。このような場合には、対向仮想面Sna,Snbの仮想面垂直抗力成分の増加量IN_nは、例えば次のように決定することができる。すなわち、各対向仮想面Sna,Snb毎に、前記したS11の処理によって暫定的に増加量IN_nを算出し、それらの暫定的な増加量IN_nのうちの大きい方を、対向仮想面Sna,Snbの仮想面垂直抗力成分の増加量IN_nとして決定する。
【0179】
以上説明したS15までの処理において、S07又はS15の判断結果が肯定的である場合(各仮想面における適切な仮想面必要並進力(暫定値)が決定された場合)には、外力目標生成部27は、次に、図8のフローチャートに示すS19の処理を実行する。このS19では、外力目標生成部27は、S15までに決定された仮想面必要並進力(暫定値)を各仮想面における仮想面上暫定外力作用点に作用させた場合(各仮想面における外力作用点を仮想面上暫定外力作用点に一致させた場合)に、該仮想面必要並進力によって前記モーメント基準点周りに発生するモーメントである仮想面必要並進力依存モーメントMsを算出する。
【0180】
ここで、上記仮想面上暫定外力作用点は、前記基本歩容要素生成部25で生成された今回時刻での各接触対象面上の暫定外力作用点を、あらかじめ定められた所定の規則に従って、該接触対象面に対応する仮想面上に投影してなる該仮想面上の外力作用点である。
【0181】
この場合、本実施形態では、上記投影のための規則は次のように設定されている。すなわち、図12に示す如く、ある接触対象面上の外力作用点を点Pr_i、該接触対象面に対応する仮想面上の外力作用点を点Ps_iとしたとき、仮想面上の外力作用点Ps_iは、例えば、接触対象面上の外力作用点Pr_iを通って、仮想面に垂直な直線(外力作用点Pr_iから仮想面への垂線)が該仮想面と交わる点に一致するものとされる。
【0182】
上記仮想面上暫定外力作用点は、このような規則に従って、今回時刻での各接触対象面上の暫定外力作用点を該接触対象面に対応する仮想面上に投影してなる該仮想面上の外力作用点である。
【0183】
S19の処理では、外力目標生成部27は、上記の如く決定される仮想面上暫定外力作用点の前記モーメント基準点に対する位置ベクトルを用いて、次式03によって、各仮想面の仮想面上暫定外力作用点に仮想面必要並進力(N_i,F_i)が作用した場合に、前記モーメント基準点の周りに発生する仮想面必要並進力依存モーメントMsを算出する。
【0184】
Ms=Σ(Irp_i*N_i)+Σ((Irp_i+Poff_i)*F_i) ……式03
この式03におけるIrp_iは、各仮想面(今回時刻のS01で設定された各仮想面)の仮想面上暫定外力作用点の前記モーメント基準点に対する位置ベクトル、Poff_iは仮想面上暫定外力作用点に対する接触対象面上の暫定外力作用点の位置ベクトル(又は該位置ベクトルのうちの仮想面に垂直な方向の成分ベクトル)である。
【0185】
さらに、(Irp_i*N_i)は、各仮想面における仮想面上暫定外力作用点の位置ベクトルIrp_iと仮想面垂直抗力成分N_iとのベクトル積、すなわち、N_iが仮想面暫定外力作用点に作用した場合にモーメント基準点周りに発生するモーメントである。
【0186】
そして、Σ(Irp_i*N_i)は、各仮想面毎のモーメント(Irp_i*N_i)の、全ての仮想面(今回時刻のS01で設定される全ての仮想面)についての総和、すなわち、各仮想面における仮想面垂直抗力成分N_iに起因してモーメント基準点周りに発生するトータルのモーメントを意味する。
【0187】
また、((Irp_i+Poff_i)*F_i)は、各仮想面における仮想面上暫定外力作用点の位置ベクトルIrp_iと仮想面摩擦力成分F_iとのベクトル積(Irp_i*F_i)、すなわち、F_iが仮想面暫定外力作用点に作用した場合にモーメント基準点周りに発生するモーメントと、仮想面上暫定外力作用点に対する接触対象面上の暫定外力作用点の位置ベクトルPoff_iとF_iとのベクトル積(Poff_i*F_i)、すなわち、F_iが接触対象面上の暫定外力作用点の周りに発生するモーメントとの総和のモーメントである。
【0188】
そして、Σ((Irp_i+Poff_i)*F_i)は、各仮想面毎のモーメント((Irp_i+Poff_i)*F_i)の、全ての仮想面(今回時刻のS01で設定される全ての仮想面)についての総和、すなわち、各仮想面における仮想面摩擦力成分F_iに起因してモーメント基準点周りに発生するトータルのモーメントを意味する。
【0189】
ここで、上記モーメント(Poff_i*F_i)に関して補足すると、接触対象面とこれに対応する仮想面とは、一般には同一面とはならないので、多くの場合、図12に示したように、接触対象面上の外力作用点Pr_iとこれに対応する仮想面上の外力作用点Ps_iとの位置ずれが生じる。そして、このような状況で、各仮想面における仮想面必要並進力が仮想面上暫定外力作用点に作用した場合にモーメント基準点周りに発生するモーメントと、該仮想面必要並進力と同じ並進力が該仮想面に対応する接触対象面上の暫定外力作用点に作用した場合にモーメント基準点周りに発生するモーメントとを比較すると、それらのモーメントには、上記位置ずれに起因する差異が生じる。この場合、接触対象面とこれに対応する仮想面とは、それらの法線方向が概ね同方向となるので、各仮想面毎の上記差異は、主に、仮想面摩擦力成分F_iに依存するものとなり、上記モーメント(Poff_i*F_i)にほぼ一致する。
【0190】
そこで、本実施形態では、上記位置ずれの影響を補償して、Msの信頼性を高めるために、上記モーメント(Poff_i*F_i)を付加した式03によって、仮想面必要並進力依存モーメントMsを算出するようにしている。
【0191】
なお、接触対象面上の暫定外力作用点と、これに対応する仮想面上暫定外力作用点との位置ずれが十分に微小なものとなる場合には、式03から、上記モーメント(Poff_i*F_i)の項を省略した式によって、仮想面必要並進力依存モーメントMsを算出するようにしてもよい。
【0192】
以上のS19の処理を、歩容生成部23が前記壁登り歩容を生成する場合に関して例示的に説明すると、例えば図11(b)に示した如く、仮想面必要並進力が決定された状況においては、図中に示す仮想面垂直抗力成分N_2a,N_2b,N_3aと仮想面摩擦力成分F_2a,F_3aとによって、モーメント基準点(例えば仮想面S2aとS3aとの交線上の点)の周りに発生するモーメントMsが上記式03によって算出されることとなる。この場合、図11(b)に示す例では、仮想面S2a,S2b,S3aは、それぞれ接触対象面WL1,WL2,FLに一致(又はほぼ一致)するので、式03における各仮想面S2a,S2b,S3a毎のPoff_i(Poff_2a,Poff_2b,Poff_3a)はいずれも“0”でよい。
【0193】
次いで、外力目標生成部27は、モーメント補償量を決定するための処理であるS21〜S37の処理を実行する。ここで、この処理の概要を説明しておく。
【0194】
上記の如くS19で算出された仮想面必要並進力依存モーメントMsが、仮に、S03で算出した必要総モーメント外力Mallに一致もしくはほぼ一致するような場合には、S15までの処理で決定された仮想面必要並進力(暫定値)と、基本歩容要素生成部25で生成された暫定外力作用点に対応する仮想面上暫定外力作用点とは、ロボット1の目標運動のうちの並進運動(全体重心点の並進運動)を適切に実現できるだけでなく、回転運動(モーメント基準点周りの角運動量を変化させる運動)をも適切に実現し得る外力として機能し得るものとなる。
【0195】
しかるに、S15までの処理では、ロボット1の目標運動のうちの回転運動に関する動力学については考慮されていないので、一般には、必要総モーメント外力Mallと仮想面必要並進力依存モーメントMsとの偏差(以下、モーメント偏差errMという)は“0”もしくはそれに近い値とはならない。
【0196】
S21〜S37の処理によって決定するモーメント補償量は、このモーメント偏差errMを解消する(“0”に近づける)ための操作量(外力に関する目標の操作量)である。
【0197】
本実施形態では、S21〜S37の処理によって決定するモーメント補償量には、3種類の補償量(第1〜第3補償量)があり、その第1補償量は、1つ以上の仮想面における仮想面上外力作用点の位置の修正量(仮想面上暫定外力作用点からの修正量。以降、仮想面上外力作用点修正量ということがある)により構成されるモーメント補償量である。この場合、第1補償量は、この第1補償量による修正後の仮想面上外力作用点に対応する接触対象面上の外力作用点が、前記存在許容領域内に存在するように決定される補償量である。
【0198】
また、第2補償量は、2つ以上の仮想面における仮想面必要並進力(暫定値)の仮想面摩擦力成分の修正量(以降、仮想面摩擦力成分修正量ということがある)から構成されるモーメント補償量である。この場合、第2補償量は、それを構成する仮想面摩擦力成分修正量の総和(合力)が“0”となるように決定される補償量である。
【0199】
また、第3補償量は、1つ以上の仮想面に垂直な軸周りのモーメント外力として該仮想面からロボット1に付加的に作用させるねじり力(以降、仮想面ねじり力ということがある)から構成されるモーメント補償量である。
【0200】
なお、上記第2補償量が、2つ以上の仮想面における仮想面摩擦力成分修正量から構成されるのは、該第2補償量による修正後における仮想面必要並進力の合力が前記第1必要条件を満足する必要があるからである。
【0201】
ここで、上記第1〜第3補償量のうち、第1補償量は、S15までの処理で決定した仮想面必要並進力(暫定値)の修正を必要とせずに、前記モーメント偏差を解消し、又は低減する機能を有する。従って、第1補償量によって、前記モーメント偏差を解消できるときには、目標運動を実現するための今回時刻での仮想面必要並進力として、S15までの処理で決定した仮想面必要並進力(暫定値)をそのまま使用することができる。
【0202】
一方、前記第2補償量又は第3補償量は、仮想面からロボット1に作用させる外力に、仮想面摩擦力成分修正量、又は仮想面ねじり力を追加的に付加するものであるから、該仮想面で発生可能な摩擦力の限界の制約を受ける。そして、その制約を満たすために、対向仮想面Sna,Snb(n=1又は2又は3)における仮想面垂直抗力成分N_na,N_nbの修正(ひいては、前記内力の修正)が必要となる場合もある。さらに、これらの仮想面垂直抗力成分N_na,N_nbの修正に起因して、一般には、前記モーメント基準点周りに発生するモーメントMsも変化する。
【0203】
そこで、本実施形態では、外力目標生成部27は、前記モーメント偏差errMを解消するためのモーメント補償量として、第1〜第3補償量のうちの第1補償量を優先的に決定する。そして、外力目標生成部27は、この第1補償量によってモーメント偏差を解消できない場合(より詳しくは、モーメント偏差errMの全体を解消し得るように第1補償量を決定した場合に、その第1補償量による修正後の仮想面上外力作用点に対応する接触対象面上の外力作用点が前記存在許容領域を逸脱する場合)に、第1補償量を制限する。さらに、モーメント偏差errMのうち、制限した第1補償量により解消できない部分を第2補償量及び第3補償量によって解消するように、第2補償量及び第3補償量の組を決定する。
【0204】
以上が、S21〜S37の処理(モーメント補償量の決定処理)の概要である。
【0205】
以下、このモーメント補償量の決定処理を具体的に説明すると、外力目標生成部27は、まず、S21の処理を実行する。このS21では、外力目標生成部27は、S03で算出した必要総モーメント外力Mallと、S19で算出した仮想面必要並進力依存モーメントMsとの偏差であるモーメント偏差errM(=Mall−Ms)を算出する。
【0206】
次いで、外力目標生成部27は、S23の処理を実行する。このS23では、外力目標生成部27は、仮想面上外力作用点の位置の変化(仮想面上暫定外力作用点からの変化)に対する仮想面必要並進力依存モーメントMsの変化の感度を表現するヤコビアンJを算出する。
【0207】
ここで、各仮想面上の外力作用点の位置の変化量(ベクトル)をΔrp_i、各仮想面上の外力作用点を仮想面上暫定外力作用点からΔrp_iだけ変化させたときの、接触対象面上の外力作用点(仮想面上の外力作用点を通って該仮想面に垂直な直線が該接触対象面と交わる点)の位置の変化量(ベクトル)をΔPoff_i、各仮想面上の外力作用点を仮想面上暫定外力作用点からΔrp_iだけ変化させたときの仮想面必要並進力依存モーメントMsの変化量(ベクトル)をΔMsとおく。このとき、前記式03に基づいて、次式05の関係式が得られる。
【0208】
ΔMs=Σ(Δrp_i*(N_i+Fi_)+ΔPoff_i*F_i) ……式05
この場合、ΔPoff_iは、Δrp_iの関数値(詳しくは、仮想面とこれに対応する接触対象面との間の姿勢差(空間的な角度差)と、Δrp_iとから一義的に規定される値)となるので、式05は、次式07に書き換えることができる。
【0209】
ΔMs=J*(Δrp_i) ……式07
この式07における(Δrp_i)は、全ての仮想面(今回時刻のS01で設定した全ての仮想面)に係わるΔrp_iを成分とする縦ベクトルである。そして、式07におけるJが、仮想面上外力作用点の位置の変化(仮想面上暫定外力作用点の位置からの変化)に対する仮想面必要並進力依存モーメントMsの変化の感度を表現するヤコビアン(行列)である。
【0210】
そこで、S23の処理では、外力目標生成部27は、各仮想面の仮想面必要並進力N_i,F_iと、式05とに基づいてヤコビアンJを算出する。なお、この場合、ΔPoff_iと、Δrp_iとの間の関係は、各仮想面とこれに対応する接触対象面との姿勢差に基づいて決定される。
【0211】
次いで、外力目標生成部27は、S25の処理を実行する。このS25処理は、式07のΔMsをS21で算出したモーメント偏差errMに一致させた場合に、式07の関係を満たす(Δrp_i)を、errMを解消するための各仮想面における仮想面上外力作用点修正量Δrpc_iの組として算出する処理である。
【0212】
ここで、式07におけるヤコビアンJは、一般には正則行列ではない。そこで、外力目標生成部27は、S23で求めたヤコビアンJの擬似逆行列Jinvを算出し、この擬似逆行列Jinvを、モーメント偏差errMに乗じることによって、各仮想面の仮想面上外力作用点修正量Δrpc_iを算出する。この場合、擬似逆行列Jinvは、例えば、各仮想面に対応するΔrpc_iの二乗値又は絶対値の平均値が最小になるように決定される。
【0213】
なお、例えば、各仮想面に対応する接触対象面における外力作用点の存在許容領域の面積等に応じて、各仮想面に対応するΔrpc_iの重み付けをしておき、各仮想面に対応するΔrpc_iの二乗値又は絶対値の重み付き平均値が最小になるように擬似逆行列Jinvを決定するようにしてもよい。
【0214】
次いで、外力目標生成部27は、S27の処理を実行する。このS27では、外力目標生成部27は、各仮想面上の外力作用点の位置を、仮想面上暫定外力作用点の位置Irp_iからS25で求めたΔrpc_iによって修正した場合に、全ての仮想面において、その修正後の仮想面上外力作用点の位置(Irp_i+Δrpc_i)が、外力作用点存在領域条件を満足するか否かを判断する。
【0215】
ここで、上記外力作用点存在領域条件は、Δrpc_iによる修正後の各仮想面の仮想面上外力作用点の位置が、基本歩容要素生成部25で生成された今回時刻での各接触対象面の外力作用点の存在許容領域に対応する仮想面上の存在許容領域(該接触対象面上の外力作用点の存在許容領域を仮想面上に投影してなる領域)内に存在するという条件である。この場合、接触対象面上の外力作用点の存在許容領域の、仮想面上への投影は、図12に示す如く、接触対象面上の外力作用点の仮想面上への投影の場合と同様に行なわれる。
【0216】
なお、外力作用点存在領域条件は、Δrpc_iによる修正後の各仮想面の仮想面上外力作用点を、該仮想面に対応する接触対象面に逆投影してなる点が、該接触対象面上の外力作用点の存在許容領域内に存在するという条件と等価である。
【0217】
S27の判断結果が肯定的である場合には、S15までの処理で決定された各仮想面の仮想面必要並進力を修正せずとも、各仮想面の外力作用点の位置を仮想面上暫定外力作用点から修正する(ひいては各接触対象面の外力作用点の位置を暫定外力作用点から修正する)だけで、前記モーメント偏差errMを解消できることとなる。
【0218】
そこで、この場合には、外力目標生成部27は、S29の処理を実行し、今回時刻での図7〜図9のフローチャートの処理を終了する。このS29の処理では、外力目標生成部27は、今回時刻での各接触対象面からロボット1への目標外力と目標外力作用点とを確定する。
【0219】
具体的には、外力目標生成部27は、S15までの処理で決定した各仮想面における仮想面必要並進力(N_i,F_i)を、該仮想面とこれに対応する接触対象面との姿勢差に応じて、該接触対象面からロボット1に作用させる並進力に変換し、この並進力を目標外力の構成要素である並進外力の目標値として決定する。
【0220】
この場合、各仮想面毎の上記変換は、例えば次のように行なわれる。すなわち、外力目標生成部27は、各仮想面の姿勢(法線ベクトルの向き)を該仮想面に対応する接触対象面の姿勢(法線ベクトルの向き)に変換する回転変換行列Rθを、これらの仮想面と接触対象面との姿勢差に基づき算出する。そして、外力目標生成部27は、この回転変換行列Rθを仮想面必要並進力(ベクトル)に乗じることによって、該仮想面必要並進力を、接触対象面からロボット1への並進外力の目標値に変換する。
【0221】
従って、本実施形態では、各接触対象面からロボット1に作用させるべき並進外力の目標値(目標並進力)は、該接触対象面に対応する仮想面における仮想面垂直抗力成分N_iと仮想面摩擦力成分F_iとそれぞれ同じ大きさの垂直抗力成分(接触対象面に垂直な成分)と摩擦力成分(接触対象面に平行な成分)とから構成される並進外力として決定される。
【0222】
また、外力目標生成部27は、S25で決定したΔrpc_iをモーメント補償量として確定する。そして、外力目標生成部27は、このΔrpc_iによる修正後の各仮想面の仮想面上外力作用点を、該仮想面に対応する接触対象面上に逆投影してなる点(本実施形態では、修正後の仮想面上外力作用点を通って該仮想面に垂直な直線が、該仮想面に対応する接触対象面と交わる点)を、該接触対象面上の目標外力作用点として決定する。
【0223】
なお、S29の処理では、目標外力の構成要素のうちの、各接触対象面のねじり力の目標値(目標ねじり力)は、いずれの接触対象面についても“0”とされる。
【0224】
一方、前記S27の判断結果が否定的である場合には、モーメント偏差errMを解消するための各仮想面における仮想面上外力作用点修正量Δrpc_iを、前記外力作用点存在領域条件を満足するように制限する必要がある。
【0225】
そこで、外力目標生成部27は、次に、S31の処理を実行する。このS31では、外力目標生成部27は、外力作用点存在領域条件を満足するように、各仮想面における仮想面上外力作用点修正量Δrpc_iを制限してなる仮想面上外力作用点制限修正量Δrpc_lim_iを第1補償量として決定する。
【0226】
この場合、外力作用点存在領域条件を満足する仮想面については、S25で決定された修正量Δrpc_iがそのまま、仮想面上外力作用点制限修正量Δrpc_lim_iとして決定される。
【0227】
一方、外力作用点存在領域条件を満足しない仮想面については、外力目標生成部27は、次のように仮想面上外力作用点制限修正量Δrpc_lim_iを決定する。
【0228】
図13を参照して、修正前の仮想面上外力作用点(接触対象面上の暫定外力作用点を仮想面上に投影してなる点)をPs1_i、この修正前の仮想面上外力作用点Ps1_iをS25で決定した仮想面上外力作用点修正量Δrpc_iにより修正してなる点をPs2_iとおく。この場合、外力作用点存在領域条件を満足しない仮想面については、修正後の仮想面上外力作用点Ps2_iが、図示の如く、仮想面上の外力作用点許容領域から逸脱することとなる。
【0229】
なお、図13では、図示の便宜上、仮想面上の外力作用許容領域を単純な楕円状の領域として記載しているが、その領域の形状は他の形状であってもよいことはもちろんである。
【0230】
このとき、外力目標生成部27は、上記仮想面上外力作用点制限修正量Δrpc_lim_iにより点Ps1_iを修正してなる点が、例えば、修正前の点Ps1_iとΔrpc_iによる修正後の点Ps2_iとを結ぶ線分上で、仮想面上の外力作用点許容領域の境界に存する点Ps_lim_iに一致するように、仮想面上外力作用点制限修正量Δrpc_lim_iを決定する。
【0231】
なお、上記のようにΔrpc_lim_iを決定することに代えて、例えば、仮想面上外力作用点制限修正量Δrpc_lim_iにより点Ps1_iを修正してなる点が、仮想面上の外力作用点許容領域の境界にする点のうち、Δrpc_iによる修正後の点Ps2_iに最も近い点(点Ps2_iとの距離が最小となる点)に一致するように、仮想面上外力作用点制限修正量Δrpc_lim_iを決定してもよい。
【0232】
以降の説明では、上記の如くS31で決定される仮想面上外力作用点制限修正量Δrpc_lim_iによる修正後の各仮想面上の外力作用点を、仮想面上制限修正後外力作用点という。
【0233】
以上の如くS31の処理を実行した後、外力目標生成部27は、S33の処理を実行する。このS33では、外力目標生成部27は、前記モーメント偏差errMのうち、S29で決定した各仮想面における仮想面上外力作用点制限修正量Δrpc_lim_iによって解消できない偏差成分としての残偏差成分errM'を算出する。
【0234】
具体的には、外力目標生成部27は、制限前の仮想面上外力作用点修正量Δrpc_iと仮想面上外力作用点制限修正量Δrpc_lim_iとの差(=Δrpc_i−Δrpc_lim_i)である仮想面上外力作用点修正誤差Δrpc_err_iを算出する。そして、外力目標生成部27は、前記式05におけるΔrp_iを、Δrpc_err_iに置き換えてなる次式09によって、残偏差成分errM’を算出する。
【0235】
errM'=Σ(Δrpc_err_i*(N_i+Fi_)+ΔPoff_i*F_i) ……式09
この場合、式09におけるΔPoff_iは、Δrpc_err_iに応じて決定される値であり、前記式05におけるΔPoff_iとΔrp_iとの間の関係に従って、Δrp_i=Δrpc_err_iとしたときの、ΔPoff_iの値として決定される。
【0236】
なお、例えば、前記式05のΔrp_iを、Δrpc_limt_iに置き換えた式によって算出されるモーメント変化量ΔMs、すなわち、各仮想面上の外力作用点の位置を仮想面上暫定外力作用点からΔrpc_limt_iだけ修正した場合に生じるモーメント基準点周りのモーメント変化量を算出し、このモーメント変化量をモーメント偏差errMから差し引くことによって、残偏差成分errM'を算出するようにしてもよい。この場合、ΔPoff_iは、前記式05におけるΔPoff_iとΔrp_iとの間の関係に従って、Δrp_i=Δrpc_limt_iとしたときの、ΔPoff_iの値として決定すればよい。
【0237】
以上の如くerrMのうちの残偏差成分errM'を算出した後、次に、外力目標生成部27は、図9に示すS35、S37の処理を順次実行することによって、第2補償量としての前記仮想面摩擦力成分修正量f_iと第3補償量としての仮想面ねじり力R_iとを決定する。
【0238】
S35では、各仮想面(S01で設定した各仮想面)に任意の値の前記仮想面摩擦力成分修正量f_iと前記仮想面ねじり力R_iとを付加した場合(Δrpc_lim_iによる修正後の仮想面上制限修正後外力作用点にf_iとR_iを追加的に作用させた場合)にそれらのf_i,R_iによって前記モーメント基準点周りに発生するモーメントMfrと、f_i,R_iとの間の関係を次式11により表現するヤコビアン(行列)Jfrを算出する。
【0239】
Mfr=Jfr*(f_i R_i)T ……式11
この式11における(f_i R_i)Tは、f_i及びR_iを成分とする縦ベクトルである。ここで、前記仮想面摩擦力成分修正量f_iは、それらの総和(合力)が“0”になるように決定されるべきモーメント補償量である。
【0240】
従って、仮想面摩擦力成分修正量f_iのうち、任意の1つの仮想面に対応する仮想面摩擦力成分修正量は、他の残りの仮想面摩擦力成分修正量の総和の符号を反転させてなる並進力として一義的に定まる。このため、式11における縦ベクトル(f_i R_i)Tの成分としてのf_iは、任意の1つの仮想面に対応するf_i(以降、これをf_i0とおく)を除いたものである。そして、ヤコビアンJfrは、f_i0が他のf_iの総和の符号を反転させてなる並進力であるとして、この並進力によるモーメント基準点周りのモーメント成分が式11の右辺の演算により算出されるMfrに含まれるように決定される。
【0241】
次いで、S37では、外力目標生成部27は、式11のMfrを残偏差成分errM'に一致させた場合に、式11の関係を満たすf_i及びR_iの組(f_i R_i)Tを算出する。
【0242】
ここで、式11におけるヤコビアンJfrは、一般には正則行列ではないので、外力目標生成部27は、S37で求めたヤコビアンJfrの擬似逆行列Jfr_invを算出し、この擬似逆行列Jfr_invを、残偏差成分errM'に乗じることによって、f_i及びR_iの組を算出する。この場合、擬似逆行列Jfr_invは、例えば、式11の縦ベクトル(f_i R_i)Tを組成するf_i及びR_iのそれぞれの2乗値又は絶対値の平均値が最小になるように決定される。なお、上記f_i0は、上記の如く算出したf_iの総和の符号を反転させてなる値として決定される。
【0243】
補足すると、上記擬似逆行列Jfr_invは、例えば、式11の縦ベクトル(f_i R_i)Tを組成するf_i及びR_iのそれぞれの2乗値又は絶対値の重み付き平均値が最小になるように決定してもよい。例えば、前記摩擦力上限値が大きい仮想面ほど、その仮想面におけるf_iの大きさが相対的に大きくなるように、各f_iに対応する重みを設定し、その重みを用いた上記重み付き平均値が最小になるように、Jfr_invを決定するようにしてもよい。
【0244】
また、例えば、S01で設定された仮想面が対向仮想面Sna,Snbを含む場合において、仮想面垂直抗力成分N_iの大きさが小さい方の対抗仮想面Sna又はSnbにおける仮想面摩擦力成分修正量f_na又はfnbを“0”に設定するようにしてもよい。
【0245】
歩容生成部23が、前記壁登り歩容を生成する場合においては、上記S35、S37の処理は、例えば次にように行なわれる。
【0246】
例えば、図11(b)に示した動作状態において、モーメント偏差errMのうちの残偏差成分errM'が存在する場合を想定する。そして、床面FL及び壁面WL1,WL2にそれぞれ対応する各仮想面S3a,S2a,S2bにおける仮想面上制限修正後外力作用点の位置ベクトル(モーメント基準点に対する位置ベクトル)をそれぞれrp_3a,rp_2a,rp_2bとおく。このとき、各仮想面S3a,S2a,S2bにおける仮想面上摩擦力成分修正量f_3a,f_2a,f_2b及び仮想面ねじり力R_3a,R_2a,R_2bと、これらのf_3a,f_2a,f_2b,R_3a,R_2a,R_2bによってモーメント基準点周りに発生するモーメントMfrとの間の関係は、次式11aにより表される。
【0247】
Mfr=rp_3a*f_3a+rp_2a*f_2a+rp_2b*f_2b+R_3a+R_2a+R_2b
……式11a
なお、式11aにおける(rp_3a*f_3a),(rp_2a*f_2a),(rp_2b*f_2b)はそれぞれベクトル積である。
【0248】
この場合、仮想面摩擦力成分修正量に関しては、f_3a+f_2a+f_2b=0であるから、f_2b=−(f_3a+f_2a)となる。従って、式11aは、例えばf_2bを含まない次式11bに書き換えられる。
【0249】
Mfr=(rp_3a−rp_2b)*f_3a+(rp_2a−rp_2b)*f_2a+R_3a+R_2a+R_2b
……式11b
従って、S35では、この式11bの右辺を、座標成分で表して整理することで、前記式11におけるヤコビアンJfrが求められることとなる。
【0250】
そして、S37では、このヤコビアンJfrの擬似逆行列Jfr_invが前記した如く算出され、このJfr_invを、残偏差成分errM’に乗じることによって、f_3a,f_2a,R_3a,R_2a,R_2bが算出される。さらに、f_3a,f_2aから、f_2b=−(f_3a+f_2a)という関係に従って、f_3bが算出される。
【0251】
補足すると、例えば、前記壁登り歩容における図3(c)の動作状態以降の時刻、すなわち、仮想面が対向仮想面S2a,S2bの2つだけになる状況では、式11aの右辺から(rp_3a*f_3a)と、R_3aとを除去してなる式と、f_2a+f_2b=0という関係とに基づいて、上記と同様に、ヤコビアンJfrを決定することができる。
【0252】
以上のS37までの処理によって、前記S27の判断結果が否定的となる場合には、前記第1A必要条件及び第1B必要条件を満足し、且つ、ロボット1の目標運動のうちのロボット1全体の回転運動(モーメント基準点周りの角運動量を変化させる回転運動)を実現し得る仮想面必要並進力(f_iによる修正後の仮想面必要並進力)と、仮想面上外力作用点(Δrpc_lim_iによる修正後の仮想面上外力作用点)と、仮想面上ねじり力R_iとの組が実質的に決定されることとなる。ただし、この場合、f_iによる仮想面必要並進力の修正が行なわれるので、その修正後の仮想面必要並進力の仮想面摩擦力成分は、前記第2必要条件を満足するとは限らない。また、仮想面上ねじり力R_iは、それを付加する仮想面とロボット1との間の摩擦力、ひいては、該仮想面に対応する接触対象面とロボット1との間の摩擦力を必要とする。
【0253】
そこで、以上の如く仮想面摩擦力成分修正量f_iと仮想面ねじり力R_iとを決定した後、次に、外力目標生成部27は、S39の判断処理を実行する。このS39では、外力目標生成部27は、各仮想面において、f_iによる修正後の仮想面摩擦力成分F_i(S15までの処理で決定されたF_iとf_iとの総和の摩擦力成分)が、前記第2必要条件を満足するか否かを判断すると共に、R_iの大きさが、仮想面垂直抗力成分N_iの大きさに応じて設定される所定の上限値以下になるという第4必要条件を満足するか否かを判断する。
【0254】
ここで、上記第4必要条件における所定の上限値は、R_iを付加する仮想面の摩擦力によって発生可能なねじり力の大きさの上限値として、各仮想面毎に設定される値である。この上限値(以降、ねじり力上限値という)は、本実施形態では、各仮想面に対応する接触対象面の性状に関する環境情報に基づいて各仮想面毎に設定した係数μr_i(以降、ねじり力係数という)を、仮想面垂直抗力成分N_iの大きさ|N_i|に乗じてなる値(=μr_i*|N_i|)に設定される。
【0255】
なお、本実施形態では、外力目標生成部27の処理を開始する前に、各接触対象面に対応付ける仮想面のねじり力係数が前記環境情報に基づいて事前に決定されると共に記憶保持される。そして、S39では、その記憶保持したねじり力係数から、R_iを付加する仮想面におけるねじり力係数が選定される。
【0256】
補足すると、各仮想面におけるねじり力係数μr_iは、該仮想面に対応する接触対象面の性状に関する環境情報だけでなく、該仮想面における仮想面摩擦力成分のF_i大きさや、ロボット1と接触対象面との接触面の大きさ等を考慮して、目標歩容の各時刻で決定するように設定してもよい。
【0257】
S39の判断結果が肯定的である場合には、外力目標生成部27は、S41の処理を実行し、今回時刻での図7〜図9のフローチャートの処理を終了する。このS41の処理では、外力目標生成部27は、今回時刻での各接触対象面からロボット1の目標外力と目標外力作用点とを確定する。
【0258】
具体的には、外力目標生成部27は、S15までの処理で決定した各仮想面における仮想面必要並進力(N_i,F_i)と、S37の処理で決定した仮想面摩擦力成分修正量f_iとから、今回時刻での各仮想面における仮想面必要並進力を確定する。この場合、S15までの処理で決定した仮想面垂直抗力成分N_iがそのまま該仮想面における仮想面垂直抗力成分として確定されると共に、S15までの処理で決定した仮想面摩擦力成分F_iに仮想面摩擦力成分修正量f_iを加え合わせてなる摩擦力成分が、該仮想面における仮想面摩擦力成分として確定される。
【0259】
なお、f_i=0となる仮想面については、結果的には、S15までの処理で決定した仮想面必要並進力(N_i,F_i)がそのまま該仮想面における仮想面必要並進力として確定されることとなる。
【0260】
また、外力目標生成部27は、S37の処理で決定した仮想面ねじり力R_iを、このR_iに対応する仮想面におけるねじり力として確定する。
【0261】
そして、外力目標生成部27は、以上の如く確定した各仮想面における仮想面必要並進力を、該仮想面とこれに対応する接触対象面との姿勢差に応じて、該接触対象面からロボット1に作用させる並進力に変換し、この並進力を目標外力の構成要素である並進外力の目標値(目標並進力)として決定する。同様に、外力目標生成部27は、各仮想面における仮想面ねじり力を、該仮想面に対応する接触対象面からロボット1に作用させるねじり力に変換し、この変換したねじり力を、目標外力の構成要素であるねじり力の目標値(目標ねじり力)として決定する。
【0262】
なお、上記変換は、図8のS29の処理に関して説明した変換と同じであり、上記姿勢差に応じて決定される前記回転変換行列Rθを用いて行なわれる。
【0263】
また、外力目標生成部27は、S29の処理の場合と同様に、S31で決定した仮想面上外力作用点制限修正量Δrpc_lim_iによる修正後の各仮想面上の外力作用点を該仮想面に対応する接触対象面上に逆投影してなる点を、該接触対象面上の目標外力作用点として決定する。
【0264】
一方、S39の判断結果が否定的である場合、すなわち、第2必要条件又は第4必要条件を満足しない仮想面が存在する場合には、ロボット1の目標運動のうちの回転運動を実現する上で、その仮想面における摩擦力が不足する。この場合には、外力目標生成部27は、次にS43の処理を実行する。このS43では、外力目標生成部27は、S39の判断処理で、第2必要条件又は第4必要条件を満たさない仮想面が、対向仮想面Sna又はSnb(n=1又は2又は3)であるか否かを判断する。
【0265】
このS43の判断結果が否定的となる状況では、第2必要条件又は第4必要条件を満足しない仮想面における仮想面垂直抗力成分の大きさを、前記第1A必要条件を満たしつつ増加させることができず、ひいては該仮想面における仮想面摩擦力成分の大きさを、前記摩擦力上限値以下で増加させることができない。そこで、この場合には、外力目標生成部27は、S47において、目標運動が不適切である(目標運動を修正する必要がある)とし、図7〜図9のフローチャートの処理を中止する。なお、この場合、外力目標生成部27は、基本歩容要素生成部25にエラー情報を出力する。そして、基本歩容要素生成部25は、そのエラー情報に応じて、目標運動を作成し直す。
【0266】
従って、本実施形態では、S37までの処理によって、前記S27の判断結果が否定的となる場合には、前記第1A必要条件及び第1B必要条件を満足し、且つ、ロボット1の目標運動のうちのロボット1全体の回転運動を実現し得る仮想面必要並進力(f_iによる修正後の仮想面必要並進力)と、仮想面上外力作用点(Δrpc_lim_iによる修正後の仮想面上外力作用点)と、仮想面上ねじり力R_iとの組を決定した上で、S39,S43の判断処理によって、目標運動が評価される。そして、このS39及びS43の判断結果がいずれも否定的となる場合に、目標運動が不適切であると評価されることとなる。
【0267】
一方、S43の判断結果が肯定的となる状況では、第2必要条件又は第4必要条件を満たさない仮想面を含む、互いに平行な対向仮想面Sna,Snb(n=1又は2又は3)の仮想面垂直抗力成分N_na,N_nbの大きさを、前記第1A必要条件を満足しつつ(N_na+N_nbを一定に保ちつつ)、増加させることができる。
【0268】
そこで、S43の判断結果が肯定的となる場合には、外力目標生成部27は、次に、S45の処理を実行する。このS45では、外力目標生成部27は、対向仮想面Sna,Snbの仮想面垂直抗力成分N_na,N_nbの大きさの増加量IN_nを、前記第2必要条件及び第4必要条件を満足させ得るように決定する。
【0269】
具体的には、外力目標生成部27は、第2必要条件を満足しない仮想面を含む対向仮想面Sna,Snbのそれぞれにおいて、前記仮想面摩擦力成分修正量f_iによる修正後の仮想面摩擦力成分F_iの大きさから、S39の判断処理で用いた摩擦力上限値(=μ_i*|N_i|)を差し引いたものを、摩擦係数μ_iで除算してなる値(=(|F_i|−μ_i*|N_i|)/μ_i)を、上記増加量IN_nの候補値として算出する(以下、このように算出されたIN_nの候補値を摩擦力成分補償用候補値という)。この場合、この摩擦力成分補償用候補値IN_nは、第2必要条件を満足しない仮想面を含む対向仮想面Sna,Snbのそれぞれ毎に算出される。
【0270】
例えば、歩容生成部23が前記壁登り歩容を生成する場合において、壁面WL1,WL2にそれぞれ対応する対向仮想面S2a,S2bのうちの一方又は両方がS39の判断処理で第2必要条件を満足しないものとなった場合には、これらの対向仮想面S2a,S2bのぞれぞれにおいて、IN_2の摩擦力成分補償用候補値((|F_2a|−μ_2a*|N_2a|)/μ_2a、(|F_2b|−μ_2b*|N_2b|)/μ_2b)が算出される。
【0271】
また、外力目標生成部27は、第4必要条件を満足しない仮想面を含む対向仮想面Sna,Snbのそれぞれにおいて、前記仮想面ねじり力R_iの大きさから、S39の判断処理で用いたねじり力上限値(=μr_i*|N_i|)を差し引いたものを、ねじり力係数μr_iで除算してなる値(=(|R_i|−μr_i*|N_i|)/μr_i)を、上記増加量IN_nの候補値として算出する(以降、このように算出されたIN_nの候補値をねじり力補償用候補値という)。この場合、摩擦力成分補償用候補値と同様に、ねじり力補償用候補値IN_nは、第4必要条件を満足しない仮想面を含む対向仮想面Sna,Snbのそれぞれ毎に算出される。
【0272】
そして、外力目標生成部27は、摩擦力成分補償用候補値及びねじり力補償用候補値のうちの摩擦力成分補償用候補値だけを算出した対向仮想面Sna,Snbについては、各対向仮想面Sna,Snbのそれぞれの摩擦力成分補償用候補値のうちのいずれか大きい方を、該対向仮想面Sna,Snbにおける仮想面垂直抗力成分N_na,N_nbの大きさの増加量IN_nとして決定する。
【0273】
また、外力目標生成部27は、摩擦力成分補償用候補値及びねじり力補償用候補値のうちのねじり力補償用候補値だけを算出した対向仮想面Sna,Snbについては、各対向仮想面Sna,Snbのそれぞれのねじり力補償用候補値のうちのいずれか大きい方を、該対向仮想面Sna,Snbにおける仮想面垂直抗力成分N_na,N_nbの大きさの増加量IN_nとして決定する。
【0274】
また、外力目標生成部27は、摩擦力成分補償用候補値及びねじり力補償用候補値の両方を算出した対向仮想面Sna,Snbについては、各対向仮想面Sna,Snbのそれぞれの摩擦力成分補償用候補値及びねじり力補償用候補値のうちの最も大きい候補値を、該対向仮想面Sna,Snbにおける仮想面垂直抗力成分の増加量IN_nとして決定する。
【0275】
ここで、S45で上記の如く決定した増加量IN_nだけ対向仮想面Sna,Snbにおける仮想面垂直抗力成分N_na,N_nbを増加させた場合、対向仮想面Sna,Snbに直交する方向の内力が増加することとなる。また、N_na,N_nbの増加に伴い、モーメント基準点周りに発生するモーメントが一般には変化してしまう。そこで、本実施形態では、外力目標生成部27は、S45の処理を実行した後、図7のS13からの処理を改めて実行する。
【0276】
以上が、本実施形態における外力目標生成部27の処理の詳細である。
【0277】
以上説明した外力目標生成部27の処理によって、まず、S05〜S15の処理によって、ロボット1の目標運動のうちの、ロボット1全体の並進運動を実現するために必要な仮想面必要並進力(N_i,F_i)が暫定的に決定される。この場合、前記モーメント基準点周りに発生するモーメントを考慮することなく、第1必要条件(第1A必要条件、第1B必要条件)、第2必要条件、及び第3必要条件を満足することを指針として、仮想面必要並進力(N_i,F_i)が暫定的に決定される。この場合、各仮想面は、その相互の姿勢関係があらかじめ定まっているので、ロボット1全体の並進運動を実現し得る仮想面必要並進力(N_i,F_i)の暫定値を簡素なアルゴリズムによって効率よく決定することができる。
【0278】
また、S07、S09の判断結果がいずれも否定的となる場合、あるいは、S07、S09、S15の判断結果がそれぞれ、否定的、肯定的、否定的となる場合に、基本歩容要素生成部25で生成された目標運動が不適切であると評価され、外力目標生成部27の処理が中止される。このため、目標運動のうちのロボット1全体の並進運動を実現するために、接触対象面とロボット1との間の摩擦力が過大になりやすい目標運動、あるいは、ロボット1の内力が過大になりやすい目標運動を、簡素なアルゴリズムで効率よく不適切であると評価することができる。ひいては、目標歩容を生成するために、無駄な演算処理を実行することとなるのを抑制することができる。
【0279】
そして、上記のようにロボット1全体の並進運動を実現し得る仮想面必要並進力(N_i,F_i)の暫定値を決定した上で、S19〜S37の処理(S29の処理を除く)によって、ロボット1全体の回転運動(モーメント基準点周りの角運動量の変化を生じる回転運動)を実現するための付加的な操作量(モーメント補償量)として、前記仮想面上外力作用点修正量Δrpc_iが決定され、あるいは、前記仮想面上外力作用点制限修正量Δrpc_lim_iと仮想面摩擦力成分修正量Δf_iと仮想面ねじり力R_iとの組が決定される。これらの、モーメント補償量は、それによる修正対象の修正の前後で、仮想面上必要並進力の合力を変化させることのない操作量である。
【0280】
このため、S05〜S15の処理によって決定した仮想面必要並進力(N_i,F_i)の暫定値と、S19〜S37の処理(S29の処理を除く)によって決定したモーメント補償量とを単純に組合わせるだけで、ロボット1全体の並進運動と回転運動との両方を適切に実現できる仮想面上必要並進力と仮想面上ねじり力と仮想面上外力作用点との組(仮想面上ねじり力が“0”になる場合を含む)を決定することができることとなる。そして、この仮想面上必要並進力と仮想面上ねじり力と仮想面上外力作用点との組から、各接触対象面からロボット1に作用させるべき目標外力(目標並進力及び目標ねじり力)と目標外力作用点とを効率よく適切に決定できることとなる。
【0281】
なお、この場合、S39、S43の判断結果がいずれも否定的となる場合に、基本歩容要素生成部25で生成された目標運動が不適切であると評価され、外力目標生成部27の処理が中止される。このため、目標運動のうちのロボット1全体の並進運動と回転運動とを実現するために、接触対象面とロボット1との間の摩擦力が過大になりやすい目標運動を、効率よく不適切であると評価することができる。ひいては、目標歩容を生成するために、無駄な演算処理を実行することとなるのを抑制することができる。
【0282】
また、本実施形態では、外力目標生成部27の処理によって、S09、S15、S43のいずれかの判断結果が否定的となる場合(目標運動が不適切である場合)を除いて、今回時刻での各接触対象面に対応する各仮想面における最終的な仮想面必要並進力(仮想面垂直抗力成分N_iと仮想面摩擦力成分F_iとの組)は、結果的に、前記第1A必要条件、第1B必要条件、第2必要条件及び第3必要条件を満足するように決定される。
【0283】
そして、各仮想面に対応する接触対象面における並進外力の目標値(目標並進力)は、その垂直抗力成分(接触対象面に垂直な成分)と摩擦力成分(接触対象面に平行な成分)とが、それぞれ、該仮想面における仮想面垂直抗力成分N_iと仮想面摩擦力成分F_iとに一致するように決定される。
【0284】
このため、各接触対象面からロボット1への目標並進力は、可能な限り、各接触対象面における目標並進力のうちの垂直抗力成分の合力によって前記必要総並進外力を実現すると共に、該目標並進力のうちの摩擦力成分を、該接触対象面で発生可能な摩擦力の大きさの限界内で可能な限り小さくなるように、各接触対象面からロボット1への目標並進力を決定できる。
【0285】
特に、本実施形態では、モーメント補償量として、仮想面上外力作用点修正量Δrpc_i(第1補償量)が優先的に決定される。すなわち、S27の判断結果が肯定的となる場合には、モーメント補償量には、仮想面摩擦力成分修正量f_i及び仮想面ねじり力R_iのいずれも含まれない。このため、各仮想面における仮想面摩擦力成分F_iの大きさを、前記第1A必要条件、第1B必要条件、第2必要条件及び第3必要条件を満足し得る範囲内で、必要最低限の大きさに留めることができる。ひいては、各接触対象面における目標並進力の摩擦力成分の大きさも必要最低限もしくはこれと同程度に留めることができる。
【0286】
また、S01で設定される仮想面が対向仮想面Sna,Snbを含む場合において、S27の判断結果が肯定的となる場合はもちろん、S39の判断結果が肯定的となる場合においても、これらの対向仮想面Sna,Snbの間でロボット1に発生する内力の大きさ(N_na,N_nbの大きさのうちの小さい方の大きさ)を、前記第1A必要条件、第1B必要条件、第2必要条件及び第3必要条件を満足し得る範囲内で、必要最低限に留めることができる。ひいては、これらの対向仮想面Sna,Snbに対応する2つの接触対象面(例えば、前記壁登り歩容における壁面WL1,WL2)の間でロボット1に発生する内力の大きさを必要最低限もしくはこれと同程度に留めることができる。このため、ロボット1の関節アクチュエータ11が発生可能な動力(本実施形態では駆動トルク)のうち、該内力を発生するために必要な負担分を軽減することができるように対向仮想面Sna,Snbに対応する2つの接触対象面を含む各接触対象面における目標並進力を決定できる。
【0287】
ここで、以上説明した本実施形態と本願発明との対応関係を補足しておく。本実施形態ででは、制御ユニット21の歩容生成部23が本発明における歩容生成装置として機能する。
【0288】
そして、この歩容生成部23のうちの基本歩容要素生成部25によって、本発明における目標運動生成手段が実現される。
【0289】
また、外力目標生成部27が実行する処理のうち、S01の処理によって、本発明における仮想面群設定手段が実現される。
【0290】
また、S03の処理によって、本発明における必要総並進外力決定手段が実現される。
【0291】
また、S05〜S15、S19〜S45の処理によって、本発明における仮想面必要並進力決定手段が実現される。
【0292】
また、S29及びS41の処理によって、本発明における接触対象面目標並進力決定手段が実現される。
【0293】
[第2実施形態]
次に本発明の第2実施形態を図14及び図15を参照して説明する。なお、本実施形態は、外力目標生成部27の処理の一部だけが第1実施形態と相違するものであるので、第1実施形態と同一事項については説明を省略する。
【0294】
前記第1実施形態では、前記モーメント偏差errMを解消するためのモーメント補償量を決定する処理(S21〜S37の処理)において、第1補償量としての仮想面上外力作用点修正量を優先的に決定するようにした。
【0295】
ただし、例えば、第2補償量としての仮想面摩擦力成分修正量f_i又は第3補償量としての仮想面ねじり力R_iを、仮想面上外力作用点修正量よりも優先的に決定するようにすることも可能である。本実施形態は、このようにモーメント補償量として、第2補償量又は第2補償量を優先的に決定する場合の一例を示す実施形態である。
【0296】
本実施形態では、外力目標生成部27の処理においては、S01〜S21までの処理は第1実施形態と同じである。そして、本実施形態では、外力目標生成部27は、S21の処理(モーメント偏差errMを算出する処理)に続いて、図14及び図15フローチャートに示す処理を実行する。
【0297】
以下説明すると、外力目標生成部27は、S21の処理の次にS61の処理を実行する。このS61では、外力目標生成部27は、第1実施形態で説明したS35の処理と同様の処理によって、仮想面摩擦力成分修正量f_i(第2補償量)及び仮想面ねじり力R_i(第3補償量)と、これらのf_i,R_iによってモーメント基準点周りに発生するモーメントMfrとの関係を前記式11により表現するヤコビアンJfrを決定する。ただし、本実施形態では、S61において、f_i,R_iを作用させる各仮想面上の外力作用点としては、仮想面上暫定外力作用点(基本歩容要素生成部25が決定した接触対象面上の暫定外力作用点を該接触対象面に対応する仮想面に投影してなる点)が使用される。
【0298】
次いで、外力目標生成部27は、S63の処理を実行する。このS63では、外力目標生成部27は、前記式11のMfrをS21で求めたモーメント偏差errMに一致させた場合に、f_i及びR_iの組(f_i R_i)Tを算出する。
【0299】
なお、必ずしもf_i及びR_iの両方を算出する必要はなく、いずれか一方を“0”に設定するようにしてもよい。
【0300】
この場合、外力目標生成部27は、S61で求めたヤコビアンJfrの擬似逆行列Jfr_invを、第1実施形態におけるS37の処理と同様の処理によって算出し、この擬似逆行列Jfr_invを、モーメント偏差errMに乗じることによって、f_i及びR_iの組(f_i R_i)Tを算出する。これにより、errMを解消するために必要なf_i及びR_iの組が算出される。
【0301】
以上のS63までの処理によって、前記第1A必要条件及び第1B必要条件を満足し、且つ、ロボット1の目標運動のうちのロボット1全体の回転運動(モーメント基準点周りの角運動量を変化させる回転運動)を実現し得る仮想面必要並進力(f_iによる修正後の仮想面必要並進力)と、仮想面上外力作用点(=仮想面上暫定外力作用点)と、仮想面上ねじり力R_iとの組が暫定的に決定されることとなる。ただし、この場合、f_iによる仮想面必要並進力の修正が行なわれるので、その修正後の仮想面必要並進力の仮想面摩擦力成分は、前記第2必要条件を満足するとは限らない。また、仮想面上ねじり力R_iも第1実施形態で説明した第4必要条件を満足するとは限らない。
【0302】
そこで、外力目標生成部27は、次に、S65の処理を実行する。このS65では、外力目標生成部27は、第1実施形態におけるS39と同じ判断処理を実行する。すなわち、外力目標生成部27は、各仮想面において、f_iによる修正後の仮想面摩擦力成分F_i(S15までの処理で決定されたF_iとf_iとの総和の摩擦力成分)が、前記第2必要条件を満足するか否かを判断すると共に、R_iの大きさが、前記第4必要条件を満足するか否かを判断する。
【0303】
そして、この判断結果が肯定的である場合には、外力目標生成部27は、S67の処理を実行し、今回時刻での処理を終了する。このS67の処理では、外力目標生成部27は、第1実施形態におけるS41と同じ処理によって、今回時刻での各接触対象面からロボット1に作用させる目標外力を確定する。
【0304】
一方、本実施形態におけるS67の処理では、外力目標生成部27は、各接触対象面の目標外力作用点に関しては、基本歩容要素生成部25で生成された暫定外力作用点をそのまま目標外力作用点として確定する。
【0305】
前記S65の判断結果が否定的である場合には、外力目標生成部27は、次にS69の処理を実行する。このS69では、外力目標生成部27は、第2必要条件を満足するように、仮想面摩擦力成分修正量f_iを制限してなる仮想面摩擦力成分制限修正量f_lim_iを決定すると共に、第4必要条件を満足するように、仮想面ねじり力R_iを制限してなる仮想面制限ねじり力R_lim_iを決定する。
【0306】
すなわち、外力目標生成部27は、f_lim_iによる修正後の仮想面摩擦力成分F_iが、S15までの処理で決定した仮想面垂直抗力成分Niに応じて設定した摩擦力上限値(=μ_i*|N_i|)以下になるようにf_lim_iを決定すると共に、R_lim_iがS15までの処理で決定した仮想面垂直抗力成分Niに応じて設定したねじり力上限値(=μr_i*|N_i|)以下になるようにR_lim_iを決定する。
【0307】
この場合、f_lim_iに関しては、第2必要条件を満足することに加えて、それらの総和(合力)が“0”に保たれるようにf_lim_iが決定される。
【0308】
より具体的には、例えば、S63で決定した各仮想面における仮想面摩擦力成分修正量f_iのそれぞれに同じ値の、ある係数kを乗じてなる値を仮想面摩擦力成分制限修正量f_lim_iに一致させるように、f_lim_iを決定する。このようにすることでf_lim_iの総和(合力)は、f_iの総和(=0)のk倍、すなわち、“0”になる。この場合、係数kの値を未知数とし、各仮想面において、S15までの処理で決定した修正前の仮想面摩擦力成分F_iにk*f_iを加え合わせてなる摩擦力の大きさ(=|F_i+k*f_i|)が、第2必要条件を満たすように(|F_i+k*f_i|≦μ_i*|N_i|となるように)、上記係数kの値を決定する。そして、この決定した係数kを、S63で算出した各仮想面におけるf_iに乗じることによって、各仮想面におけるf_lim_iを決定する。
【0309】
なお、S01で設定された全ての仮想面において、f_iによる修正後の仮想面摩擦力成分F_iが前記第2必要条件を満足している場合には、上記係数kの値は“1”とされる。従って、この場合には、S63で算出した各仮想面におけるf_iがそのまま、flim_iとして決定される。
【0310】
また、各仮想面における仮想面制限ねじり力R_lim_iは例えば次のように決定される。すなわち、S65で前記第4必要条件を満たさない各仮想面において、仮想面制限ねじり力R_lim_iは、その大きさがねじり力上限値(=μr_i*|N_i|)に一致するように決定される。また、第4必要条件を満たす各仮想面においては、S63で算出された仮想面ねじり力R_iがそのまま仮想面制限ねじり力R_lim_iとして決定される。
【0311】
なお、第4必要条件を満たさない各仮想面における仮想面制限ねじり力R_lim_iを、ねじり力上限値よりも若干小さい大きさのねじり力に決定するようにしてもよい。
【0312】
上記の如く仮想面摩擦力成分制限修正量f_lim_iと仮想面制限ねじり力R_lim_iとを決定した後、外力目標生成部27は、次に、S71の処理を実行する。このS71では、外力目標生成部27は、前記モーメント偏差errMのうち、S69で決定した各仮想面におけるf_lim_iとR_lim_iとによって解消できない偏差成分としての残偏差成分errM'を算出する。
【0313】
具体的には、外力目標生成部27は、各仮想面において、制限前の仮想面摩擦力成分修正量f_iと仮想面摩擦力成分制限修正量f_lim_iとの差Δf_i(=f_i−f_lim_i)を算出すると共に、制限前の仮想面ねじり力R_iと仮想面制限ねじり力R_lim_iとの差ΔR_i(=R_i−R_lim_i)とを算出する。そして、外力目標生成部27は、前記式11の右辺における縦ベクトル(f_i R_i)Tの成分f_i,R_iをそれぞれに対応するΔf_i,ΔR_iに置き換え、且つ、前記式11の左辺をerrM'に置き換えた式によって、残偏差成分errM’を算出する。
【0314】
なお、f_lim_i及びR_lim_iを、仮想面上暫定外力作用点に作用させた場合にモーメント基準点周りに発生するモーメントを算出し、このモーメントを、モーメント偏差errMから差し引くことによって、残偏差成分errM'を算出するようにしてもよい。
【0315】
次いで、外力目標生成部27は、図15のS73の処理を実行する。このS73では、外力目標生成部27は、仮想面上外力作用点の位置の変化(仮想面上暫定外力作用点からの変化)に対する仮想面必要並進力依存モーメントMsの変化の感度を表現するヤコビアンJ(前記式07の関係式におけるヤコビアンJ)を、第1実施形態におけるS23と同じ処理によって算出する。
【0316】
次いで、外力目標生成部27は、S75の処理を実行する。このS75では、外力目標生成部27は、第1実施形態におけるS25と同様の処理によって、前記式07におけるΔMs(仮想面必要並進力依存モーメントMsの変化量)をS71で算出した残偏差成分errM'に一致させた場合に、式07の関係を満たす縦ベクトル(Δrp_i)を、errMを解消するための各仮想面における仮想面上外力作用点修正量Δrpc_iの組として算出する。すなわち、外力目標生成部27は、S73で決定したヤコビアンJの擬似逆行列JinvをS25の処理と同様に算出し、このJinvをerrM'に乗じることによって、各仮想面における仮想面上外力作用点修正量Δrpc_iの組を算出する。
【0317】
次いで、外力目標生成部27は、S77の判断処理を実行する。このS77の判断処理は、前記第1実施形態におけるS27の判断処理と同じである。すなわち、外力目標生成部27は、全ての仮想面において、S75で決定したΔrpc_iによる修正後の仮想面上外力作用点の位置が外力作用点存在領域条件を満足するか否かを判断する。
【0318】
このS77の判断結果が肯定的である場合には、外力目標生成部27は、S79の処理を実行し、今回時刻での処理を終了する。このS79の処理では、外力目標生成部27は、今回時刻での各接触対象面からロボット1の目標外力と目標外力作用点とを確定する。
【0319】
具体的には、外力目標生成部27は、S15までの処理で決定した各仮想面における仮想面必要並進力(N_i,F_i)と、S69の処理で決定した仮想面摩擦力成分制限修正量f_lim_iとから、今回時刻での各仮想面における仮想面必要並進力を確定する。この場合、S15までの処理で決定した仮想面垂直抗力成分N_iがそのまま該仮想面における仮想面垂直抗力成分として確定されると共に、S15までの処理で決定した仮想面摩擦力成分F_iに仮想面摩擦力成分制限修正量f_lim_iを加え合わせてなる摩擦力成分が、該仮想面における仮想面摩擦力成分として確定される。
【0320】
また、外力目標生成部27は、S69の処理で決定した仮想面制限ねじり力R_lim_iを、このR_lim_iに対応する仮想面における仮想面ねじり力として確定する。
【0321】
そして、外力目標生成部27は、以上の如く確定した各仮想面における仮想面必要並進力と仮想面ねじり力とを、第1実施形態におけるS29又はS41と同じ処理によって、該接触対象面からロボット1に作用させる並進力及びねじり力にそれぞれ変換し、この変換後の並進力とねじり力とをそれぞれ目標外力の構成要素である並進外力の目標値(目標並進力)、ねじり力の目標値(目標ねじり力)として決定する。
【0322】
また、外力目標生成部27は、第1実施形態におけるS29の処理の場合と同様に、S75で決定した仮想面上外力作用点修正量Δrpc_iによる修正後の各仮想面上の外力作用点を該仮想面に対応する接触対象面上に逆投影してなる点を、該接触対象面上の目標外力作用点として決定する。
【0323】
前記S77の判断結果が否定的となる状況では、外力作用点存在領域条件を満たし得る範囲内での仮想面上外力作用点の修正では、前記残偏差成分errM'を解消できないこととなる。このため、この場合には、各仮想面に付加する摩擦力又はねじり力をさらに増加させ得るように(ひいてはS65での判断結果が肯定的になり得るように)、仮想面垂直抗力成分N_iの大きさを大きくする必要がある。
【0324】
そこで、S77の判断結果が否定的となる場合には、外力目標生成部27は、次に、S81の処理を実行する。このS77では、外力目標生成部27は、S65の判断処理で、第2必要条件又は第4必要条件を満たさない仮想面が、対向仮想面Sna又はSnb(n=1又は2又は3)であるか否かを判断する。
【0325】
このS65の判断結果が否定的となる状況では、S65の判断処理で第2必要条件又は第4必要条件を満足しない仮想面における仮想面垂直抗力成分の大きさを、前記第1A必要条件を満たしつつ増加させることができない。そこで、この場合には、外力目標生成部27は、S85において、目標運動が不適切である(目標運動を修正する必要がある)とし、今回時刻での処理を中止する。なお、この場合、外力目標生成部27は、基本歩容要素生成部25にエラー情報を出力する。そして、基本歩容要素生成部25は、そのエラー情報に応じて、目標運動を作成し直す。
【0326】
従って、本実施形態では、S77の判断結果が否定的となる場合に、S81の判断処理によって、目標運動が評価される。そして、このS81の判断結果が否定的となる場合に、目標運動が不適切であると評価されることとなる。
【0327】
一方、S81の判断結果が肯定的となる状況では、第2必要条件又は第4必要条件を満たさない仮想面を含む、互いに平行な対向仮想面Sna,Snb(n=1又は2又は3)の仮想面垂直抗力成分N_na,N_nbの大きさを、前記第1A必要条件を満足しつつ(N_na+N_nbを一定に保ちつつ)、増加させることができる。
【0328】
そこで、S81の判断結果が肯定的となる場合には、外力目標生成部27は、次に、S83の処理を実行する。このS83では、外力目標生成部27は、対向仮想面Sna,Snbの仮想面垂直抗力成分N_na,N_nbの大きさの増加量IN_nを決定する。この場合、本実施形態では、S83で決定する増加量IN_nは、あらかじめ定められた所定値に設定される。
【0329】
ただし、この増加量IN_nを、例えば、第1実施形態におけるS45と同様の処理によって決定するようにしてもよい。
【0330】
次いで、外力目標生成部27は、図7のS13からの処理を改めて実行する。
【0331】
以上が、本実施形態における外力目標生成部27の処理の詳細である。
【0332】
以上説明した外力目標生成部27の処理によって、まず、S05〜S15の処理によって、第1実施形態と同様に、ロボット1の目標運動のうちの、ロボット1全体の並進運動を実現するために必要な仮想面必要並進力(N_i,F_i)が暫定的に決定される。このため、ロボット1全体の並進運動を実現し得る仮想面必要並進力(N_i,F_i)の暫定値を簡素なアルゴリズムによって効率よく決定することができる。
【0333】
また、第1実施形態と同様に、S07、S09の判断結果がいずれも否定的となる場合、あるいは、S07、S09、S15の判断結果がそれぞれ、否定的、肯定的、否定的となる場合に、基本歩容要素生成部25で生成された目標運動が不適切であると評価され、外力目標生成部27の処理が中止される。このため、第1実施形態と同様に、目標運動のうちのロボット1全体の並進運動を実現するために、接触対象面とロボット1との間の摩擦力が過大になりやすい目標運動、あるいは、ロボット1の内力が過大になりやすい目標運動を、簡素なアルゴリズムで効率よく不適切であると評価することができる。ひいては、目標歩容を生成するために、無駄な演算処理を実行することとなるのを抑制することができる。
【0334】
そして、本実施形態では、このようにロボット1全体の並進運動を実現し得る仮想面必要並進力(N_i,F_i)の暫定値を決定した上で、S19、S21、S61〜S75の処理(S67の処理を除く)によって、ロボット1全体の回転運動(モーメント基準点周りの角運動量の変化を生じる回転運動)を実現するための付加的な操作量(モーメント補償量)として、前記仮想面摩擦力成分修正量Δf_iと仮想面ねじり力R_iとの組が決定され、あるいは、前記仮想面摩擦力成分修正量Δf_iと仮想面ねじり力R_iと仮想面上外力作用点修正量Δrpc_iとの組が決定される。
【0335】
このため、第1実施形態の場合と同様に、S05〜S15の処理によって決定した仮想面必要並進力(N_i,F_i)の暫定値と、S19、S21、S61〜S75の処理(S67の処理を除く)によって決定したモーメント補償量とを単純に組合わせるだけで、ロボット1全体の並進運動と回転運動との両方を適切に実現できる仮想面上必要並進力と仮想面上ねじり力と仮想面上外力作用点との組(仮想面上ねじり力が“0”になる場合を含む)を決定することができることとなる。そして、この仮想面上必要並進力と仮想面上ねじり力と仮想面上外力作用点との組から、各接触対象面からロボット1に作用させるべき目標外力(目標並進力及び目標ねじり力)と目標外力作用点とを効率よく適切に決定できることとなる。
【0336】
なお、この場合、S77、S81の判断結果がいずれも否定的となる場合に、基本歩容要素生成部25で生成された目標運動が不適切であると評価され、外力目標生成部27の処理が中止される。このため、前記第1A必要条件及び第1B必要条件とを少なくとも満足しつつ、ロボット1全体の回転運動を実現することが困難となる目標運動を不適切であると評価することができる。ひいては、目標歩容を生成するために、無駄な演算処理を実行することとなるのを抑制することができる。
【0337】
また、図7に示したS09及びS15、並びに図15のS81のいずれかの判断結果が否定的となる場合(目標運動が不適切である場合)を除いて、第1実施形態と同様に、今回時刻での各接触対象面に対応する各仮想面における最終的な仮想面必要並進力(仮想面垂直抗力成分N_iと仮想面摩擦力成分F_iとの組)は、結果的に、前記第1A必要条件、第1B必要条件、第2必要条件及び第3必要条件を満足するように決定される。
【0338】
そして、各仮想面に対応する接触対象面における並進外力の目標値(目標並進力)は、その垂直抗力成分(接触対象面に垂直な成分)と摩擦力成分(接触対象面に平行な成分)とが、それぞれ、該仮想面における仮想面垂直抗力成分N_iと仮想面摩擦力成分F_iとに一致するように決定される。
【0339】
このため、第1実施形態と同様に、各接触対象面からロボット1への目標並進力は、可能な限り、各接触対象面における目標並進力のうちの垂直抗力成分の合力によって前記必要総並進外力を実現すると共に、該目標並進力のうちの摩擦力成分を、該接触対象面で発生可能な摩擦力の大きさの限界内で可能な限り小さくなるように、各接触対象面からロボットへの目標並進力を決定できる。
【0340】
また、S01で設定される仮想面が対向仮想面Sna,Snbを含む場合において、S65又はS77の判断結果が肯定的となる場合に、第1実施形態と同様に、これらの対向仮想面Sna,Snbの間でロボット1に発生する内力の大きさ(N_na,N_nbの大きさのうちの小さい方の大きさ)を、前記第1A必要条件、第1B必要条件、第2必要条件及び第3必要条件を満足し得る範囲内で、必要最低限に留めることができる。従って、第1実施形態と同様に、ロボット1の関節アクチュエータ11が発生可能な動力(本実施形態では駆動トルク)のうち、該内力を発生するために必要な負担分を軽減することができるように対向仮想面Sna,Snbに対応する2つの接触対象面を含む各接触対象面における目標並進力を決定できる。
【0341】
ここで、以上説明した本実施形態と本願発明との対応関係を補足しておく。本実施形態ででは、S05〜S15、S19、S21、S61〜S83の処理によって、本発明における仮想面必要並進力決定手段が実現される。また、S67及びS79の処理によって、本発明における接触対象面目標並進力決定手段が実現される。これら以外の対応関係は、第1実施形態と同じである。
【0342】
[第3実施形態]
次に本発明の第3実施形態を図16を参照して説明する。なお、本実施形態は、外力目標生成部27の処理の一部だけが第1実施形態と相違するものであるので、第1実施形態と同一事項については説明を省略する。
【0343】
ロボット1の外界の接触対象面が、前記壁面WL1,WL2の如く、互いに間隔を存して対向する2つの接触対象面を有する場合、ひいては、S01で設定される仮想面群が対向仮想面Sna,Snbを含む場合には、これらの対向仮想面Sna,Snbにおける仮想面垂直抗力成分N_na,N_nbの合力を一定に維持したまま、該対向仮想面Sna,Snbの間の内力(=min(|N_na|,|N_nb|)を操作する(増減させる)ことによって、モーメント基準点周りに発生するモーメントを変化させることができる。従って、この内力は、モーメント偏差errM(又は残偏差成分errM')を解消し、もしくは低減するためモーメント補償量として利用することもできる。
【0344】
そこで、本実施形態では、S01で設定される仮想面群が対向仮想面Sna,Snbを含む場合に、S33で算出される前記残偏差成分errM'を解消するためのモーメント補償量として、第2補償量(仮想面摩擦力成分修正量f_i)及び第3補償量(仮想面ねじり力R_i)と、第4補償量としての内力修正量ΔINとの組を決定する。
【0345】
具体的には、本実施形態における外力目標生成部27の処理のうち、前記S33までの処理は、第1実施形態と同じである。そして、S33の後の一部の処理が第1実施形態と相違する。
【0346】
以下、図16を参照して説明すると、外力目標生成部27は、S33の次に、S101の処理を実行する。このS101では、外力目標生成部27は、S01で設定した仮想面群に、対向仮想面Sna,Snbが有るか否かを判断する。そして、この判断結果が否定的である場合には、外力目標生成部27は、図9に示したS35からの処理を第1実施形態と同様に実行する。
【0347】
なお、本実施形態では、S101の判断結果が否定的となった場合において、その後、図9に示したS35、S37、S39、S43の処理を順次実行したとき、S43の判断結果は常に否定的となる。従って、S101の判断結果が否定的となった場合の後の処理では、S43及びS45の処理は実質的に不要であり、S39の判断結果が、否定的である場合に、直ちにS47の処理を実行するようにしてもよい。
【0348】
一方、S101の判断結果が肯定的である場合には、外力目標生成部27は、次に、S103の処理を実行する。このS103では、各仮想面(S01で設定した各仮想面)の仮想面上制限修正後外力作用点に任意の値の前記仮想面摩擦力成分修正量f_iと前記仮想面ねじり力R_iとを付加し、且つ、各組の対向仮想面Sna,Snbの仮想面垂直抗力成分N_na,N_nbの大きさを任意の値の内力修正量ΔIN_nだけ変化させた場合に、それらのf_i,R_i,ΔIN_nによって前記モーメント基準点周りに発生するモーメントMfrinと、f_i,R_i,ΔIN_nとの間の関係を次式13により表現するヤコビアン(行列)Jfrinを算出する。
【0349】
Mfrin=Jfrin*(f_i R_i ΔIN_n)T ……式13
この式13における(f_i R_i ΔIN_n)Tは、f_i,R_i及びΔIN_nを成分とする縦ベクトルである。なお、仮想面摩擦力成分修正量f_iは、それらの総和(合力)が“0”になるように決定されるべきモーメント補償量であるので、式13における縦ベクトル(f_i R_i ΔIN_n)Tの成分としてのf_iは、第1実施形態で説明した前記式11におけるf_iと同様に、任意の1つの仮想面に対応するf_i(≡f_i0)を除いたものである。また、S01で設定した仮想面群に、対向仮想面Sna,Snbの組が複数組含まれる場合には、その全ての組の対向仮想面Sna,Snbに対応する内力修正量ΔIN_nが式13における縦ベクトル(f_i R_i ΔIN_n)Tの成分として含まれる。
【0350】
次いで、外力目標生成部27は、S105の処理を実行する。このS105では、外力目標生成部27は、式13のMfrinを、S33で算出した残偏差成分errM'に一致させた場合に、式13の関係を満たすf_i,R_i及びΔIN_nの組(f_i R_i ΔIN_n)Tを算出する。
【0351】
すなわち、外力目標生成部27は、S103で求めたヤコビアンJfrinの擬似逆行列Jfrin_invを算出し、この擬似逆行列Jfrin_invを、残偏差成分errM'に乗じることによって、f_i,R_i及びΔIN_nの組を算出する。この場合、擬似逆行列Jfrin_invは、第1実施形態で説明した図9のS37の処理で擬似逆行列Jfr_invを算出する手法と同様に、例えば式13の縦ベクトル(f_i R_i ΔIN_n)Tを組成するf_i,R_i,ΔIN_nのそれぞれの2乗値又は絶対値の平均値(又は重み付き平均値)が最小になるように決定される。また、式13の縦ベクトル(f_i R_i ΔIN_n)Tから除いた上記f_i0は、上記の如く算出したf_iの総和の符号を反転させてなる値として決定される。
【0352】
なお、必ずしもf_i,R_i,ΔIN_nの全てを算出する必要はない。例えば、摩擦係数μ_iが比較的小さい仮想面におけるf_iを“0”に設定したり、ねじり力係数μr_iが比較的小さい仮想面におけるR_iを“0”に設定するようにしてもよい。
【0353】
歩容生成部23が、前記壁登り歩容を生成する場合においては、上記S103、S105の処理は、例えば次にように行なわれる。
【0354】
例えば、第1実施形態で説明した場合と同様に、図11(b)に示した動作状態において、床面FL及び壁面WL1,WL2にそれぞれ対応する各仮想面S3a,S2a,S2bにおける仮想面上制限修正後外力作用点の位置ベクトル(モーメント基準点に対する位置ベクトル)をそれぞれrp_3a,rp_2a,rp_2bとおく。このとき、各仮想面S3a,S2a,S2bにおける仮想面上摩擦力成分修正量f_3a,f_2a,f_2b、仮想面ねじり力R_3a,R_2a,R_2b、並びに、仮想面S2a,S2b間の内力修正量ΔIN_2と、これらのf_3a,f_2a,f_2b,R_3a,R_2a,R_2b,ΔIN_2によってモーメント基準点周りに発生するモーメントMfrinとの間の関係は、次式13aにより表される。
【0355】
Mfrin=rp_3a*f_3a+rp_2a*f_2a+rp_2b*f_2b+R_3a+R_2a+R_2b
+(rp_2a−rp_2b)*ΔIN_2
……式13a
なお、式13aにおける(rp_3a*f_3a),(rp_2a*f_2a),(rp_2b*f_2b),(rp_2a−rp_2b)*ΔIN_2はそれぞれベクトル積である。この場合、式13aでは、ΔIN_2は、仮想面S2aから仮想面S2bに向かうベクトル(大きさがΔIN_2であるベクトル)として表現されている。
【0356】
この場合、仮想面摩擦力成分修正量に関しては、f_3a+f_2a+f_2b=0であるから、f_2b=−(f_3a+f_2a)となる。従って、式13aは、例えばf_2bを含まない次式13bに書き換えられる。
【0357】
Mfrin=(rp_3a−rp_2b)*f_3a+(rp_2a−rp_2b)*f_2a+R_3a+R_2a+R_2b
+(rp_2a−rp_2b)*ΔIN_2
……式13b
従って、S103では、この式13bの右辺を、座標成分で表して整理することで、前記式13におけるヤコビアンJfrinが求められることとなる。
【0358】
そして、S105では、このヤコビアンJfrinの擬似逆行列Jfrin_invが前記した如く算出され、このJfr_invを、残偏差成分errM’に乗じることによって、f_3a,f_2a,R_3a,R_2a,R_2b,ΔIN_2が算出される。さらに、f_3a,f_2aから、f_2b=−(f_3a+f_2a)という関係に従って、f_3bが算出される。
【0359】
以上のS105までの処理によって、前記S27の判断結果が否定的となり、且つ、S101の判断結果が肯定的となる場合には、前記第1A必要条件及び第1B必要条件を満足しつつ、ロボット1の目標運動のうちのロボット1全体の回転運動(モーメント基準点周りの角運動量を変化させる回転運動)を実現するためのモーメント補償量として、仮想面上外力作用点制限修正量Δrpc_lim_iに加えて、仮想面摩擦力成分修正量f_i、仮想面ねじり力R_i及び内力修正量ΔIN_nの組が算出されることとなる。
【0360】
次いで、外力目標生成部27は、S107の判断処理を実行する。このS107では、外力目標生成部27は、内力修正量ΔIN_nによる内力の修正を行なう対向仮想面Sna,Snbにおいて、修正後の内力が前記第3必要条件を満足するか否かを判断する。より詳しくは、外力目標生成部27は、対向仮想面Sna,Snbにおいて、内力修正量ΔIN_nだけ大きさを変化させた仮想面垂直抗力成分N_na,N_nbの大きさのうちの小さい方の大きさ(すなわちΔIN_nによる修正後の内力の大きさ)が、前記内力上限値IFC_n以下であるか否かを判断する。
【0361】
このS107の判断結果が否定的である場合には、外力目標生成部27は、S117において、目標運動が不適切である(目標運動を修正する必要がある)とし、外力に関する目標を生成する処理を中止する。なお、この場合、外力目標生成部27は、基本歩容要素生成部25にエラー情報を出力する。そして、基本歩容要素生成部25は、そのエラー情報に応じて、目標運動を作成し直す。
【0362】
また、S107の判断結果が肯定的である場合には、外力目標生成部27は、図9のS39と同じ判断処理(第2必要条件及び第4必要条件に関する判断処理)をS109で実行する。なお、この場合、対向仮想面Sna,Snbにおける摩擦力上限値に関する仮想面垂直抗力成分N_na,N_nbの大きさは、内力修正量ΔIN_nによる修正後の値(詳しくは図7のS15までの処理で決定された仮想面垂直抗力成分N_na,N_nbの大きさを、内力修正量ΔIN_nだけ変化させた後の値)である。
【0363】
そして、S109の判断結果が肯定的である場合には、外力目標生成部27は、図9のS41と同じ処理をS111で実行し、今回時刻での各接触対象面からロボット1への目標外力と目標外力作用点とを確定する。
【0364】
また、S109の判断結果が否定的である場合には、外力目標生成部27は、図9のS43と同じ判断処理をS113で実行する。そして、この判断結果が、否定的である場合には、外力目標生成部27は、S117において、目標運動が不適切であるとして、外力目標生成部27の処理(外力に関する目標を生成する処理)を中止する。
【0365】
従って、本実施形態では、S107の判断結果が否定的となる場合、あるいは、109及びS113の判断結果がいずれも否定的となる場合に、目標運動が不適切であると評価されることとなる。
【0366】
また、S113の判断結果が肯定的である場合には、図9のS45と同じ処理をS115で実行し、対向仮想面Sna,Snbにおける仮想面垂直抗力成分N_na,N_nbの大きさの増加量IN_nを決定する。なお、S115で決定する増加量IN_nは、詳しくは、S109の判断処理で使用した仮想面垂直抗力成分N_na,N_nbの大きさ(すなわち、内力修正量ΔIN_nによる修正後の値)からの増加量である。
【0367】
次いで、外力目標生成部27は、図7のS13からの処理を改めて実行する。
【0368】
なお、このようにS13からの処理を改めて実行する場合には、S15の判断結果が肯定的となる場合に、S101の判断処理を行うことなく(あるいは、S101の判断結果に依存することなく)、S35からの処理を実行するようにしてもよい。
【0369】
以上が、本実施形態における外力目標生成部27の処理の詳細である。
【0370】
以上説明した外力目標生成部27の処理によって、第1実施形態と同様に、まず、S05〜S15の処理によって、ロボット1の目標運動のうちの、ロボット1全体の並進運動を実現するために必要な仮想面必要並進力(N_i,F_i)が暫定的に決定される。このため、ロボット1全体の並進運動を実現し得る仮想面必要並進力(N_i,F_i)の暫定値を簡素なアルゴリズムによって効率よく決定することができる。
【0371】
また、第1実施形態と同様に、S07、S09の判断結果がいずれも否定的となる場合、あるいは、S07、S09、S15の判断結果がそれぞれ、否定的、肯定的、否定的となる場合に、基本歩容要素生成部25で生成された目標運動が不適切であると評価され、外力目標生成部27の処理が中止される。このため、第1実施形態と同様に、目標運動のうちのロボット1全体の並進運動を実現するために、接触対象面とロボット1との間の摩擦力が過大になりやすい目標運動、あるいは、ロボット1の内力が過大になりやすい目標運動を、簡素なアルゴリズムで効率よく不適切であると評価することができる。ひいては、目標歩容を生成するために、無駄な演算処理を実行することとなるのを抑制することができる。
【0372】
そして、本実施形態では、このようにロボット1全体の並進運動を実現し得る仮想面必要並進力(N_i,F_i)の暫定値を決定した上で、S19〜S37の処理(S29の処理を除く)、又は、S19〜S33(S29の処理を除く)、S103〜105の処理によって、ロボット1全体の回転運動(モーメント基準点周りの角運動量の変化を生じる回転運動)を実現するための付加的な操作量(モーメント補償量)として、前記仮想面上外力作用点修正量Δrpc_iが決定され、あるいは、前記仮想面上外力作用点制限修正量Δrpc_lim_iと仮想面摩擦力成分修正量Δf_iと仮想面ねじり力R_iとの組が決定され、あるいは、Δrpc_lim_i、Δf_i及びR_iと内力修正量ΔIN_nとの組が決定される。
【0373】
このため、第1実施形態と同様に、S05〜S15の処理によって決定した仮想面必要並進力(N_i,F_i)の暫定値と、S19〜S37の処理(S29の処理を除く)、又は、S19〜S33(S29の処理を除く)とS103〜105の処理とによって決定したモーメント補償量とを単純に組合わせるだけで、ロボット1全体の並進運動と回転運動との両方を適切に実現できる仮想面上必要並進力と仮想面上ねじり力と仮想面上外力作用点との組(仮想面上ねじり力が“0”になる場合を含む)を決定することができることとなる。そして、この仮想面上必要並進力と仮想面上ねじり力と仮想面上外力作用点との組から、各接触対象面からロボット1に作用させるべき目標外力(目標並進力及び目標ねじり力)と目標外力作用点とを効率よく適切に決定できることとなる。
【0374】
なお、この場合、S109、S113の判断結果がいずれも否定的となる場合に、基本歩容要素生成部25で生成された目標運動が不適切であると評価され、外力目標生成部27の処理が中止される。このため、前記第1A必要条件及び第1B必要条件とを少なくとも満足しつつ、ロボット1全体の回転運動を実現することが困難となる目標運動を不適切であると評価することができる。ひいては、目標歩容を生成するために、無駄な演算処理を実行することとなるのを抑制することができる。
【0375】
また、図7に示したS09及びS15、並びに図16のS107、S113のいずれかの判断結果が否定的となる場合(目標運動が不適切である場合)を除いて、第1実施形態と同様に、今回時刻での各接触対象面に対応する各仮想面における最終的な仮想面必要並進力(仮想面垂直抗力成分N_iと仮想面摩擦力成分F_iとの組)は、結果的に、前記第1A必要条件、第1B必要条件、第2必要条件及び第3必要条件を満足するように決定される。
【0376】
そして、各仮想面に対応する接触対象面における並進外力の目標値(目標並進力)は、その垂直抗力成分(接触対象面に垂直な成分)と摩擦力成分(接触対象面に平行な成分)とが、それぞれ、該仮想面における仮想面垂直抗力成分N_iと仮想面摩擦力成分F_iとに一致するように決定される。
【0377】
このため、第1実施形態と同様に、各接触対象面からロボット1への目標並進力は、可能な限り、各接触対象面における目標並進力のうちの垂直抗力成分の合力によって前記必要総並進外力を実現すると共に、該目標並進力のうちの摩擦力成分を、該接触対象面で発生可能な摩擦力の大きさの限界内で可能な限り小さくなるように、各接触対象面からロボットへの目標並進力を決定できる。
【0378】
特に、本実施形態では、第1実施形態と同様に、モーメント補償量として、仮想面上外力作用点修正量Δrpc_i(第1補償量)が優先的に決定されるため、各仮想面における仮想面摩擦力成分F_iの大きさを、前記第1A必要条件、第1B必要条件、第2必要条件及び第3必要条件を満足し得る範囲内で、必要最低限の大きさに留めることができる。ひいては、各接触対象面における目標並進力の摩擦力成分の大きさも必要最低限もしくはこれと同程度に留めることができる。
【0379】
また、S01で設定される仮想面が対向仮想面Sna,Snbを含む場合において、目標運動が不適切であると判断される場合を除いて、対向仮想面Sna,Snbの間でロボット1に発生する内力の大きさ(N_na,N_nbの大きさのうちの小さい方の大きさ)が前記内力上限値IFC_n以下に収まるように(前記第3必要条件を満足するように)、仮想面垂直抗力成分N_na,N_nbが決定される。このため、ロボット1の関節アクチュエータ11が発生可能な動力(本実施形態では駆動トルク)のうち、内力を発生するために必要な負担分が過大にならないように、対向仮想面Sna,Snbに対応する2つの接触対象面を含む各接触対象面における目標並進力を決定できる。
【0380】
ここで、以上説明した本実施形態と本願発明との対応関係を補足しておく。本実施形態ででは、S05〜S15、S19〜S45、S103〜S115の処理によって、本発明における仮想面必要並進力決定手段が実現される。また、S29,S41,S111の処理によって、本発明における接触対象面目標並進力決定手段が実現される。これら以外の対応関係は、第1実施形態と同じである。
【0381】
[第4実施形態]
次に本発明の第4実施形態を図17及び図18を参照して説明する。なお、本実施形態は、外力目標生成部27の処理の一部だけが第2実施形態又は第3実施形態と相違するものであるので、第2実施形態又は第3実施形態と同一事項については説明を省略する。
【0382】
本実施形態は、モーメント偏差errMを解消もしくは低減するためのモーメント補償量として、第3実施形態と同様に内力修正量ΔIN_n(第4補償量)を適宜利用するものである。この場合、本実施形態では、S01で設定した仮想面群に対向仮想面が含まれる場合に、モーメント補償量として、仮想面摩擦力成分修正量f_i(第2補償量)と仮想面ねじり力R_i(第3補償量)と内力修正量ΔIN_n(第4補償量)との組を決定した上で、これらの補償量によって解消できない残偏差成分errM’を解消するように、仮想面上外力作用点修正量Δrpc_i(第1補償量)を決定する。
【0383】
具体的には、本実施形態における外力目標生成部27の処理のうち、前記S21までの処理は、第2実施形態と同じである。そして、S21の後の一部の処理が第2実施形態と相違する。
【0384】
以下図17及び図18を参照して説明すると、外力目標生成部27は、S21の次に、S121の判断処理を実行する。このS121では、外力目標生成部27は、S01で設定した仮想面群に、対向仮想面Sna,Snbが有るか否かを判断する。そして、この判断結果が否定的である場合には、外力目標生成部27は、図14及び図15に示したS61からの処理を第2実施形態と同様に実行する。
【0385】
なお、本実施形態では、S121の判断結果が否定的となった場合において、その後、図15に示したS81の判断処理を実行したとき、その判断結果は常に否定的となる。従って、S121の判断結果が否定的となった場合の後の処理では、S81及びS83の処理は実質的に不要であり、S77の判断結果が否定的である場合に、直ちにS85の処理を実行するようにしてもよい。
【0386】
一方、S121の判断結果が肯定的である場合には、外力目標生成部27は、次に、S123の処理を実行する。このS123では、外力目標生成部27は、第3実施形態で説明したS103と同様の処理によって、仮想面摩擦力成分修正量f_i(第2補償量)、仮想面ねじり力R_i(第3補償量)及び内力修正量ΔIN_n(第4補償量)と、これらのf_i,R_i,ΔIN_nによってモーメント基準点周りに発生するモーメントMfrinとの関係を前記式13により表現するヤコビアンJfrinを決定する。ただし、本実施形態では、S123の処理における各仮想面上の外力作用点としては、仮想面上暫定外力作用点(基本歩容要素生成部25が決定した接触対象面上の暫定外力作用点を該接触対象面に対応する仮想面に投影してなる点)が使用される。
【0387】
次いで、外力目標生成部27は、S125の処理を実行する。このS125では、外力目標生成部27は、前記式13のMfrinをS21で求めたモーメント偏差errMに一致させた場合に式13の関係式を満足させ得るf_i、R_i及びΔIN_nの組(f_i R_i ΔIN_n)Tを算出する。
【0388】
この場合、外力目標生成部27は、S123で求めたヤコビアンJfrinの擬似逆行列Jfrin_invを、第3実施形態におけるS105の処理と同様の処理によって算出し、この擬似逆行列Jfrin_invを、モーメント偏差errMに乗じることによって、f_i,R_i,ΔIN_nの組(f_i R_i ΔIN_n)Tを算出する。これにより、errMを解消するために必要なf_i,R_i,ΔIN_nの組が算出される。
【0389】
なお、必ずしもf_i,R_i,ΔIN_nの全てを算出する必要はない。例えば、摩擦係数μ_iが比較的小さい仮想面におけるf_iを“0”に設定したり、ねじり力係数μr_iが比較的小さい仮想面におけるR_iを“0”に設定するようにしてもよい。
【0390】
以上のS125までの処理によって、S121の判断結果が否定的となる場合には、前記第1A必要条件及び第1B必要条件を満足しつつ、ロボット1の目標運動のうちのロボット1全体の回転運動(モーメント基準点周りの角運動量を変化させる回転運動)を実現するためのモーメント補償量として、仮想面摩擦力成分修正量f_i、仮想面ねじり力R_i及び内力修正量ΔIN_nの組が算出されることとなる。
【0391】
次いで、外力目標生成部27は、第3実施形態におけるS107と同じ判断処理をS127で実行する。すなわち、外力目標生成部27は、ΔIN_nによる修正後の内力(=min(|N_na|,|N_nb|))が前記第3必要条件を満足するか否かを判断する。
【0392】
このS127の判断結果が肯定的である場合には、外力目標生成部27は、次に、S131の処理を実行する。このS131では、外力目標生成部27は、第2実施形態におけるS65と同じ判断処理を実行する。すなわち、外力目標生成部27は、各仮想面において、f_iによる修正後の仮想面摩擦力成分F_i(S15までの処理で決定されたF_iとf_iとの総和の摩擦力成分)が、前記第2必要条件を満足するか否かを判断すると共に、R_iの大きさが、前記第4必要条件を満足するか否かを判断する。
【0393】
そして、S131の判断結果が肯定的である場合には、外力目標生成部27は、S133の処理を実行し、今回時刻での処理を終了する。このS133の処理では、外力目標生成部27は、第2実施形態におけるS67と同様の処理によって、今回時刻での各接触対象面からロボット1の目標外力と目標外力作用点とを確定する。
【0394】
具体的には、外力目標生成部27は、S15までの処理で決定した各仮想面における仮想面必要並進力(N_i,F_i)と、S125の処理で決定した仮想面摩擦力成分修正量f_i及び内力修正量ΔIN_nとから、今回時刻での各仮想面における仮想面必要並進力を確定する。この場合、S15までの処理で決定した仮想面垂直抗力成分N_iの大きさをΔIN_nだけ変化させてなる垂直抗力成分が、該仮想面における仮想面垂直抗力成分として確定されると共に、S15までの処理で決定した仮想面摩擦力成分F_iに仮想面摩擦力成分修正量f_iを加え合わせてなる摩擦力成分が、該仮想面における仮想面摩擦力成分として確定される。
【0395】
また、外力目標生成部27は、S125の処理で決定した仮想面ねじり力R_iを、このR_iに対応する仮想面におけるねじり力として確定する。
【0396】
そして、外力目標生成部27は、以上の如く確定した各仮想面における仮想面必要並進力と仮想面ねじり力R_iとを、S67の処理と同様に、該仮想面に対応する接触対象面からロボット1に作用させる並進力とねじり力とにそれぞれ変換することで、目標外力の構成要素である目標並進力と目標ねじり力とを決定する。
【0397】
また、外力目標生成部27は、基本歩容要素生成部25で生成された暫定外力作用点をそのまま目標外力作用点として確定する。
【0398】
前記S131の判断結果が否定的である場合には、外力目標生成部27は、次にS135の処理を実行する。このS135では、外力目標生成部27は、第2実施形態におけるS69と同じ処理によって、第2必要条件を満足するように、仮想面摩擦力成分修正量f_iを制限してなる仮想面摩擦力成分制限修正量f_lim_iを決定すると共に、第4必要条件を満足するように、仮想面ねじり力R_iを制限してなる仮想面制限ねじり力R_lim_iを決定する。
【0399】
一方、前記S127の判断結果が否定的である場合には、外力目標生成部27は、S129の処理を実行した後、上記S135の処理を実行する。
【0400】
この場合、S129では、外力目標生成部27は、前記第3必要条件を満足するように、内力修正量ΔIN_nを制限してなる内力制限修正量ΔIN_lim_nを決定する。具体的には、S127の判断結果が否定的となった対向仮想面Sna,Snbに関して、S15までの処理で決定された仮想面垂直抗力成分N_na,Nnbの大きさをそれぞれ、ΔIN_lim_nだけ変化させた後の仮想面垂直抗力成分N_na,Nnbの大きさのうちの小さい方の大きさ(ΔIN_lim_nによる修正後の内力の大きさ)が前記内力上限値IFC_nに一致するように(あるいは、IFC_nよりも若干小さい値となるように)、ΔIN_lim_nを決定する。
【0401】
なお、S01で設定される仮想面群が2組以上の対向仮想面Sna,Snbを有する場合において、S127の判断結果が否定的とならない対向仮想面Sna,Snbに関しては、S125で算出された内力修正量ΔIN_nがそのまま、内力制限修正量ΔIN_lim_nとして決定される。
【0402】
そして、S129の処理に続くS135の処理では、対向仮想面Sna,Snbのそれぞれの摩擦力上限値及びねじり力上限値を決定するための仮想面垂直抗力成分N_na,N_nbの大きさとしては、S15までの処理で決定された仮想面垂直抗力成分の大きさを、S129で決定されたΔIN_lim_nにより修正してなる値が用いられる。
【0403】
なお、S129の処理に続くS135の処理においては、S01で設定された全ての仮想面について、S125で算出されたf_iによる修正後の仮想面摩擦力成分F_iが第2必要条件を満足し、且つ、S125で算出されたR_iが第4必要条件を満足するものとなっている場合もある。この場合には、S125で算出されたf_i,R_iがそのまま各仮想面における仮想面摩擦力成分制限修正量f_lim_i、仮想面制限ねじり力R_lim_iとして決定されることとなる。
【0404】
また、図示は省略したが、本実施形態では、S127の判断結果と、S131の判断結果とがそれぞれ肯定的、否定的となった場合におけるS135の処理では、後の処理の便宜上、S125で算出された内力修正量ΔIN_nがそのまま内力制限修正量ΔIN_lim_nとして決定される。
【0405】
上記の如くΔIN_lim_n、f_lim_i、R_lim_iを決定した後、外力目標生成部27は、次に、S137の処理を実行する。このS137では、外力目標生成部27は、前記モーメント偏差errMのうち、ΔIN_lim_nとf_lim_iとR_lim_iとの組によって解消できない偏差成分としての残偏差成分errM'を算出する。
【0406】
具体的には、外力目標生成部27は、各仮想面において、制限前の仮想面摩擦力成分修正量f_iと仮想面摩擦力成分制限修正量f_lim_iとの差Δf_i(=f_i−f_lim_i)を算出すると共に、制限前の仮想面ねじり力R_iと仮想面制限ねじり力R_lim_iとの差ΔR_i(=R_i−R_lim_i)とを算出する。さらに、外力目標生成部27は、制限前の内力修正量ΔIN_nと内力制限修正量ΔIN_lim_iとの差ΔΔIN_n(=ΔIN_n−ΔIN_lim_n)を算出する。
【0407】
そして、外力目標生成部27は、前記式13の右辺における縦ベクトル(f_i R_i ΔIN_n)Tの成分f_i,R_i,ΔIN_nをそれぞれに対応するΔf_i,ΔR_i,ΔΔIN_nに置き換え、且つ、前記式13の左辺をerrM'に置き換えた式によって、残偏差成分errM’を算出する。
【0408】
なお、式13の右辺における縦ベクトル(f_i R_i ΔIN_n)Tの成分f_i,R_i,ΔIN_nを、それぞれf_lim_i,R_lim_i,ΔIN_lim_nに置き換えた式によって、これらのf_lim_i,R_lim_i,ΔIN_lim_nに起因してモーメント基準点周りに発生するモーメントを算出し、このモーメントを、モーメント偏差errMから差し引くことによって、残偏差成分errM'を算出するようにしてもよい。
【0409】
次いで、外力目標生成部27は、図18のS139からの処理を実行する。この場合、S139からS143までの処理は、第2実施形態におけるS73からS77までの処理と同じである。
【0410】
そして、S143の判断結果が肯定的である場合には、外力目標生成部27は、外力目標S145の処理を実行し、今回時刻での処理を終了する。このS145の処理では、外力目標生成部27は、第2実施形態におけるS79と同様の処理によって、今回時刻での各接触対象面からロボット1の目標外力と目標外力作用点とを確定する。
【0411】
具体的には、外力目標生成部27は、S15までの処理で決定した各仮想面における仮想面必要並進力(N_i,F_i)と、S135までの処理で決定した内力制限修正量ΔIN_lim_nと仮想面摩擦力成分制限修正量f_lim_iとから、今回時刻での各仮想面における仮想面必要並進力を確定する。この場合、対向仮想面Sna,Snbについては、S15までの処理で決定した仮想面垂直抗力成分N_na,N_nbの大きさをΔIN_lim_nにより変化させてなる垂直抗力成分が、該対向仮想面Sna,Snbにおける仮想面垂直抗力成分として確定されると共に、その他の各仮想面については、S15までの処理で決定した仮想面垂直抗力成分N_iが、そのまま該仮想面における仮想面垂直抗力成分として確定される。また、いずれの仮想面についても、S15までの処理で決定した仮想面摩擦力成分F_iに仮想面摩擦力成分制限修正量f_lim_iを加え合わせてなる摩擦力成分が、該仮想面における仮想面摩擦力成分として確定される。
【0412】
また、外力目標生成部27は、S135の処理で決定した仮想面制限ねじり力R_lim_iを、このR_lim_iに対応する仮想面における仮想面ねじり力として確定する。
【0413】
そして、外力目標生成部27は、以上の如く確定した各仮想面における仮想面必要並進力と仮想面ねじり力とを、S79と同様の処理によって、該仮想面に対応する接触対象面からロボット1に作用させる並進力及びねじり力にそれぞれ変換することによって、目標外力の構成要素である目標並進力と目標ねじり力とを決定する。
【0414】
また、外力目標生成部27は、S141で決定した仮想面上外力作用点修正量Δrpc_iによる修正後の各仮想面上の外力作用点を該仮想面に対応する接触対象面上に逆投影してなる点を、該接触対象面上の目標外力作用点として決定する。
【0415】
また、S143の判断結果が否定的となる場合には、外力目標生成部27は、S147の処理を実行した後、図7のS13からの処理を改めて実行する。この場合、S147では、外力目標生成部27は、対向仮想面Sna,Snbにおける仮想面垂直抗力成分N_na,N_nbの大きさの増加量IN_n(詳しくは、S15までの処理で決定された仮想面垂直抗力成分N_na,N_nbからの増加量)を決定する。この場合、本実施形態では、S83で決定する増加量IN_nは、あらかじめ定められた所定値に設定される。
【0416】
なお、このようにS13からの処理を改めて実行する場合には、S21の処理の次に、S121の判断処理を行うことなく(あるいは、S121の判断結果に依存することなく)、S61からの処理を実行するようにしてもよい。
【0417】
以上が、本実施形態における外力目標生成部27の処理の詳細である。
【0418】
以上説明した外力目標生成部27の処理によって、まず、S05〜S15の処理によって、第1実施形態と同様に、ロボット1の目標運動のうちの、ロボット1全体の並進運動を実現するために必要な仮想面必要並進力(N_i,F_i)が暫定的に決定される。このため、ロボット1全体の並進運動を実現し得る仮想面必要並進力(N_i,F_i)の暫定値を簡素なアルゴリズムによって効率よく決定することができる。
【0419】
また、第1実施形態と同様に、S07、S09の判断結果がいずれも否定的となる場合、あるいは、S07、S09、S15の判断結果がそれぞれ、否定的、肯定的、否定的となる場合に、基本歩容要素生成部25で生成された目標運動が不適切であると評価され、外力目標生成部27の処理が中止される。このため、第1実施形態と同様に、目標運動のうちのロボット1全体の並進運動を実現するために、接触対象面とロボット1との間の摩擦力が過大になりやすい目標運動、あるいは、ロボット1の内力が過大になりやすい目標運動を、簡素なアルゴリズムで効率よく不適切であると評価することができる。ひいては、目標歩容を生成するために、無駄な演算処理を実行することとなるのを抑制することができる。
【0420】
そして、本実施形態では、このようにロボット1全体の並進運動を実現し得る仮想面必要並進力(N_i,F_i)の暫定値を決定した上で、S19、S21、S61〜S75の処理(S67の処理を除く)、又はS19、S21、S123〜S141(S133の処理を除く)によって、ロボット1全体の回転運動(モーメント基準点周りの角運動量の変化を生じる回転運動)を実現するための付加的な操作量(モーメント補償量)として、前記仮想面摩擦力成分修正量f_iと仮想面ねじり力R_iとの組、あるいは、f_iとR_limiと内力修正量ΔIN_nとの組、あるいは、f_lim_iとR_lim_iとΔrpc_iと内力制限修正量ΔIN_lim_nとの組が決定される。
【0421】
このため、第1実施形態の場合と同様に、S05〜S15の処理によって決定した仮想面必要並進力(N_i,F_i)の暫定値と、S19、S21、S61〜S75の処理(S67の処理を除く)、又はS19、S21、S123〜S141(S133の処理を除く)によって決定したモーメント補償量とを単純に組合わせるだけで、ロボット1全体の並進運動と回転運動との両方を適切に実現できる仮想面上必要並進力と仮想面上ねじり力と仮想面上外力作用点との組(仮想面上ねじり力が“0”になる場合を含む)を決定することができることとなる。そして、この仮想面上必要並進力と仮想面上ねじり力と仮想面上外力作用点との組から、各接触対象面からロボット1に作用させるべき目標外力(目標並進力及び目標ねじり力)と目標外力作用点とを効率よく適切に決定できることとなる。
【0422】
また、図7に示したS09及びS15、並びにS81のいずれかの判断結果が否定的となる場合(目標運動が不適切である場合)を除いて、第1実施形態と同様に、今回時刻での各接触対象面に対応する各仮想面における最終的な仮想面必要並進力(仮想面垂直抗力成分N_iと仮想面摩擦力成分F_iとの組)は、結果的に、前記第1A必要条件、第1B必要条件、第2必要条件及び第3必要条件を満足するように決定される。
【0423】
そして、各仮想面に対応する接触対象面における並進外力の目標値(目標並進力)は、その垂直抗力成分(接触対象面に垂直な成分)と摩擦力成分(接触対象面に平行な成分)とが、それぞれ、該仮想面における仮想面垂直抗力成分N_iと仮想面摩擦力成分F_iとに一致するように決定される。
【0424】
このため、第1実施形態と同様に、各接触対象面からロボット1への目標並進力は、可能な限り、各接触対象面における目標並進力のうちの垂直抗力成分の合力によって前記必要総並進外力を実現すると共に、該目標並進力のうちの摩擦力成分を、該接触対象面で発生可能な摩擦力の大きさの限界内で可能な限り小さくなるように、各接触対象面からロボットへの目標並進力を決定できる。
【0425】
また、S01で設定される仮想面が対向仮想面Sna,Snbを含む場合において、目標運動が不適切であると判断される場合と除いて、対向仮想面Sna,Snbの間でロボット1に発生する内力の大きさ(N_na,N_nbの大きさのうちの小さい方の大きさ)が前記内力上限値IFC_n以下に収まるように(前記第3必要条件を満足するように)、仮想面垂直抗力成分が決定される。このため、ロボット1の関節アクチュエータ11が発生可能な動力(本実施形態では駆動トルク)のうち、内力を発生するために必要な負担分が過大にならないように、対向仮想面Sna,Snbに対応する2つの接触対象面を含む各接触対象面における目標並進力を決定できる。
【0426】
ここで、以上説明した本実施形態と本願発明との対応関係を補足しておく。本実施形態ででは、S05〜S15、S19、S21、S61〜S83、S123〜S147の処理によって、本発明における仮想面必要並進力決定手段が実現される。また、S67、S79、S133及びS145の処理によって、本発明における接触対象面目標並進力決定手段が実現される。これら以外の対応関係は、第1実施形態と同じである。
【0427】
なお、以上説明した各実施形態では、前記壁登り歩容のように、ロボット1の外界(動作環境)に互いに間隔を存して対向する接触対象面WL1,WL2が存在する場合を想定して説明したが、本発明は、互いに間隔を存して対向する接触対象面が存在しない動作環境でロボットを運動を行なわせるための目標歩容を生成する場合にも適用できる。例えば、互いに交差する3つの接触対象面、あるいは2つの接触対象面にロボットの複数の部位を接触させながら、ロボットの運動を行なわせるための目標歩容を生成する場合にも本発明を適用できる。
【0428】
前記壁登り歩容以外の目標歩容の例を図19及び図20に示す。
【0429】
図19(a)〜(e)は、例えば互いに交差する2つの接触対象面として、床面FLと壁面WL1とが存在する動作環境で、ロボット1の運動を行なわせるための一例の目標歩容における該ロボット1の瞬時的な動作状態を時系列順に示している。
【0430】
この目標歩容におけるロボット1の運動は、次のような形態の運動である。すなわち、図19(a)に示すようにロボット1が2つのリンク機構5a,5bを床面FLに接地させて起立した状態から、図19(b)に示すように、リンク機構5c,5dを壁面WL1に接触させる。そして、この状態で、基体3を壁面WL1に近づけて、図19(c)に示す如く、該基体3を壁面WL1に寄り掛からせるように接触させる。その後、図19(d)に示すように、リンク機構5c,5dにより壁面WL1を押すことによって、基体3を壁面WL1から離反させ、最終的に、図19(e)に示す如く、リンク機構5c,5dを壁面WL1から離反させる。
【0431】
また、図20(a)〜(d)は、例えば互いに交差する3つの接触対象面として、床面FLと2つの壁面WL1,WL3とが存在する動作環境(例えば屋内の部屋の隅部)で、ロボット1の運動を行なわせるための一例の目標歩容における該ロボット1の瞬時的な動作状態を時系列順に示している。なお、図20(a)〜(d)は、ロボット1と動作環境とを上方から見た平面視で示している。そして、図20(a)〜(d)の各図では、ロボット1はリンク機構5a,5d(図示省略)を床面FLに接地させて起立している。
【0432】
この目標歩容におけるロボット1の運動は、次のような形態の運動である。すなわち、図20(a)に示すようにロボット1が床面FL上に接地させて起立した状態から、図20(b)に示すように、リンク機構5c,5dをそれぞれ壁面WL1,WL3に接触させる。そして、この状態で、基体3を壁面WL3に近づけて、図20(c)に示す如く、該基体3を壁面WL3に寄り掛からせるように接触させる。その後、リンク機構5c,5dによりそれぞれ壁面WL1,WL2を押すことによって、図20(d)に示すように、基体3とリンク機構5cとを壁面WL1から離反させると共に、リンク機構5dを壁面WL3から離反させる。
【0433】
図19又は図20に示したような目標歩容を生成する場合においても、前記した各実施形態における外力目標生成部27の処理によって、ロボット1に各接触対象面FL,WL1から作用させるべき目標外力と目標外力作用点とを適切に決定できる。
【0434】
なお、この場合、図19に示した目標歩容の生成処理において、前記S01の処理で設定される仮想面は、図19(a),(e)に示す動作状態では、1つの仮想面(図示例では仮想面S3a)であり、図19(b)〜(d)に示す動作状態では、2つの仮想面(図示例では仮想面S2a,S3a)である。
【0435】
また、図20に示した目標歩容の生成処理において、前記S01の処理で設定される仮想面は、図20(a),(d)に示す動作状態では、1つの仮想面(図示例では仮想面S3a)であり、図20(b),(c)に示す動作状態では、3つの仮想面(図示例では仮想面S1a,S2a,S3a)である。
【0436】
なお、図19又は図20に例示した場合のように、ロボット1の動作環境に対向仮想面Sna,Snbに対応する接触対象面が存在しない場合には、図7のS09、図9のS43、図15のS81、図16のS101、図17のS121の判断処理の判断結果が常に否定的となる。従って、これらの判断結果が肯定的となる場合に専用的な処理は不要である。
【0437】
また、前記各実施形態では、ロボット1に搭載した制御ユニット21で目標歩容を生成する場合を例に採って説明したが、本実施形態における目標歩容の生成処理(歩容生成部23の処理)は、ロボット1の実際の動作時に行なう必要はない。従って、歩容生成部23の処理は、ロボット1の外部の適宜のコンピュータによって随時行なうようにしてもよい。
【符号の説明】
【0438】
1…脚式移動ロボット、23…歩容生成部(歩容生成装置)、25…基本歩容要素生成部(目標運動生成手段)、S01…仮想面群設定手段、S03…必要総並進外力決定手段、S05〜S15,S19〜S45,S61〜S83,S103〜S115,S123〜S147…仮想面必要並進力決定手段、S29,S41,S67,S79,S111,S133,S145…接触対象面目標並進力決定手段、FL,WL1,WL2…接触対象面、S1a,S2a,S3a,S1b,S2b,S3b…仮想面。
【特許請求の範囲】
【請求項1】
脚式移動ロボットの動作環境に存する互いに異なる複数の接触対象面に該ロボットの複数の部位を接触させながら、該ロボットの運動を行なわせるための該ロボットの目標運動と、該ロボットに作用させるべき外力に関する目標とを含む目標歩容を生成する歩容生成装置であって、
前記ロボットの目標運動を生成する目標運動生成手段と、
相互の姿勢関係があらかじめ定められた複数の仮想面から成る仮想面群を用い、該仮想面群の複数の仮想面が前記複数の接触対象面に一致又は近似する面になるように、該仮想面群を前記ロボットの動作環境の空間に仮想的に設定する仮想面群設定手段と、
前記生成された目標運動に基づいて、該目標運動を実現するために前記ロボットに作用させるべきトータルの外力のうちの並進力成分である必要総並進外力を決定する必要総並進外力決定手段と、
前記決定された必要総並進外力を実現するために前記仮想面群の複数の仮想面のそれぞれから前記ロボットに作用させるべき並進力である仮想面必要並進力を決定する手段であり、前記複数の仮想面のそれぞれにおける仮想面必要並進力のうちの該仮想面に垂直な成分である仮想面垂直抗力成分の合力が、前記必要総並進外力のうちの、前記仮想面群の全ての仮想面に対して平行な成分を除く成分に一致するという第1A必要条件と、前記複数の仮想面のそれぞれにおける仮想面必要並進力のうちの該仮想面に平行な成分である仮想面摩擦力成分の合力が、前記必要総並進外力のうちの、前記仮想面群の全ての仮想面に対して平行な成分に一致するという第1B必要条件と、各仮想面における仮想面摩擦力成分の大きさが、少なくとも該仮想面における仮想面垂直抗力成分に応じて設定された第1所定値以下になるという第2必要条件とを少なくとも満足するように、各仮想面における仮想面必要並進力を決定する仮想面必要並進力決定手段と、
少なくとも前記各仮想面に対応して決定された前記仮想面必要並進力に基づいて、各仮想面に対応する接触対象面から前記ロボットに作用させるべき目標並進力を決定し、その決定した目標並進力を前記目標歩容の構成要素として出力する接触対象面目標並進力決定手段とを備えることを特徴とする脚式移動ロボットの歩容生成装置。
【請求項2】
請求項1記載の脚式移動ロボットの歩容生成装置において
前記仮想面必要並進力決定手段は、前記必要総並進外力のうちの前記仮想面群の全ての仮想面に対して平行な成分が“0”でない場合において、前記第1A必要条件、第1B必要条件及び第2必要条件を満足しつつ、前記仮想面群の複数の仮想面のうちの前記仮想面垂直抗力成分が“0”でない各仮想面において、該仮想面に対応して設定された前記第1所定値が大きいほど、該仮想面の仮想面摩擦力成分の大きさが大きくなるように、各仮想面における仮想面必要並進力を決定することを特徴とする脚式移動ロボットの歩容生成装置。
【請求項3】
請求項1又は2記載の脚式移動ロボットの歩容生成装置において、
前記複数の接触対象面は、互いに間隔を存して対向する2つの接触対象面を含むと共に、前記仮想面群は、当該2つの接触対象面に対応する仮想面として、互いに平行に対向する2つの仮想面である対向仮想面を含んでおり、
前記仮想面必要並進力決定手段は、前記決定された必要総並進外力が前記2つの対向仮想面と直交する方向の成分である対向仮想面直交成分を有する場合に、前記第1A必要条件、第1B必要条件及び第2必要条件を満足することに加えて、前記2つの対向仮想面のそれぞれにおける仮想面垂直抗力成分のうち、小さい方の仮想面垂直抗力成分の大きさがあらかじめ設定された第2所定値以下になるという第3必要条件を満足するように、各仮想面における仮想面必要並進力を決定することを特徴とする脚式移動ロボットの歩容生成装置。
【請求項1】
脚式移動ロボットの動作環境に存する互いに異なる複数の接触対象面に該ロボットの複数の部位を接触させながら、該ロボットの運動を行なわせるための該ロボットの目標運動と、該ロボットに作用させるべき外力に関する目標とを含む目標歩容を生成する歩容生成装置であって、
前記ロボットの目標運動を生成する目標運動生成手段と、
相互の姿勢関係があらかじめ定められた複数の仮想面から成る仮想面群を用い、該仮想面群の複数の仮想面が前記複数の接触対象面に一致又は近似する面になるように、該仮想面群を前記ロボットの動作環境の空間に仮想的に設定する仮想面群設定手段と、
前記生成された目標運動に基づいて、該目標運動を実現するために前記ロボットに作用させるべきトータルの外力のうちの並進力成分である必要総並進外力を決定する必要総並進外力決定手段と、
前記決定された必要総並進外力を実現するために前記仮想面群の複数の仮想面のそれぞれから前記ロボットに作用させるべき並進力である仮想面必要並進力を決定する手段であり、前記複数の仮想面のそれぞれにおける仮想面必要並進力のうちの該仮想面に垂直な成分である仮想面垂直抗力成分の合力が、前記必要総並進外力のうちの、前記仮想面群の全ての仮想面に対して平行な成分を除く成分に一致するという第1A必要条件と、前記複数の仮想面のそれぞれにおける仮想面必要並進力のうちの該仮想面に平行な成分である仮想面摩擦力成分の合力が、前記必要総並進外力のうちの、前記仮想面群の全ての仮想面に対して平行な成分に一致するという第1B必要条件と、各仮想面における仮想面摩擦力成分の大きさが、少なくとも該仮想面における仮想面垂直抗力成分に応じて設定された第1所定値以下になるという第2必要条件とを少なくとも満足するように、各仮想面における仮想面必要並進力を決定する仮想面必要並進力決定手段と、
少なくとも前記各仮想面に対応して決定された前記仮想面必要並進力に基づいて、各仮想面に対応する接触対象面から前記ロボットに作用させるべき目標並進力を決定し、その決定した目標並進力を前記目標歩容の構成要素として出力する接触対象面目標並進力決定手段とを備えることを特徴とする脚式移動ロボットの歩容生成装置。
【請求項2】
請求項1記載の脚式移動ロボットの歩容生成装置において
前記仮想面必要並進力決定手段は、前記必要総並進外力のうちの前記仮想面群の全ての仮想面に対して平行な成分が“0”でない場合において、前記第1A必要条件、第1B必要条件及び第2必要条件を満足しつつ、前記仮想面群の複数の仮想面のうちの前記仮想面垂直抗力成分が“0”でない各仮想面において、該仮想面に対応して設定された前記第1所定値が大きいほど、該仮想面の仮想面摩擦力成分の大きさが大きくなるように、各仮想面における仮想面必要並進力を決定することを特徴とする脚式移動ロボットの歩容生成装置。
【請求項3】
請求項1又は2記載の脚式移動ロボットの歩容生成装置において、
前記複数の接触対象面は、互いに間隔を存して対向する2つの接触対象面を含むと共に、前記仮想面群は、当該2つの接触対象面に対応する仮想面として、互いに平行に対向する2つの仮想面である対向仮想面を含んでおり、
前記仮想面必要並進力決定手段は、前記決定された必要総並進外力が前記2つの対向仮想面と直交する方向の成分である対向仮想面直交成分を有する場合に、前記第1A必要条件、第1B必要条件及び第2必要条件を満足することに加えて、前記2つの対向仮想面のそれぞれにおける仮想面垂直抗力成分のうち、小さい方の仮想面垂直抗力成分の大きさがあらかじめ設定された第2所定値以下になるという第3必要条件を満足するように、各仮想面における仮想面必要並進力を決定することを特徴とする脚式移動ロボットの歩容生成装置。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】
【図17】
【図18】
【図19】
【図20】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】
【図17】
【図18】
【図19】
【図20】
【公開番号】特開2011−177834(P2011−177834A)
【公開日】平成23年9月15日(2011.9.15)
【国際特許分類】
【出願番号】特願2010−44699(P2010−44699)
【出願日】平成22年3月1日(2010.3.1)
【出願人】(000005326)本田技研工業株式会社 (23,863)
【Fターム(参考)】
【公開日】平成23年9月15日(2011.9.15)
【国際特許分類】
【出願日】平成22年3月1日(2010.3.1)
【出願人】(000005326)本田技研工業株式会社 (23,863)
【Fターム(参考)】
[ Back to top ]