ロボット装置
【課題】 ロボットが外界と多様な物理インタラクションを行なった際にも、体の任意箇所における接触状態と作用力をともに計測する。
【解決手段】 各関節アクチュエータにおける現在の位置、速度、発生力を計測するとともに、機体上の１箇所で加速度及び角速度を計測する。機体上の外界との接触状態を検出して、外力の作用点を特定して、操作空間上でダイナミクス演算により操作空間慣性逆行列及びバイアス加速度を求めることにより、運動方程式上の既知項を算出し、操作空間慣性逆行列及びバイアス加速度の間に前記運動方程式に合致する関係が成立するように、外力の作用状態を推定する。
【解決手段】 各関節アクチュエータにおける現在の位置、速度、発生力を計測するとともに、機体上の１箇所で加速度及び角速度を計測する。機体上の外界との接触状態を検出して、外力の作用点を特定して、操作空間上でダイナミクス演算により操作空間慣性逆行列及びバイアス加速度を求めることにより、運動方程式上の既知項を算出し、操作空間慣性逆行列及びバイアス加速度の間に前記運動方程式に合致する関係が成立するように、外力の作用状態を推定する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、複数の関節自由度を持つロボット装置に係り、特に、可動脚などの移動手段を備えたロボット装置に関する。
【０００２】
さらに詳しくは、本発明は、地上を自律的に移動して人や物と積極的且つ多様な物理的インタラクションを行なうロボット装置に係り、特に、人や物と物理的インタラクションを行なう際における外界との接触状態や、力の作用状況を把握するロボット装置に関する。
【背景技術】
【０００３】
最近、脚式移動ロボットの構造やその安定歩行制御に関する研究開発が進展し、実用化への期待も高まってきている。これら脚式移動ロボットは、クローラ式ロボットに比し不安定で姿勢制御や歩行制御が難しくなるが、階段の昇降や障害物の乗り越えなど、柔軟な歩行・走行動作を実現できるという点で優れている。
【０００４】
従来のロボットは、６軸のマニピュレータに代表されるように、主な用途は、産業活動・生産活動などにおける各種の人的作業の代行や支援であった。この場合のロボットは、工場などの構造化された環境内で動作することを前提としており、人や環境との複雑な物理的インタラクションは基本的に行なわない。
【０００５】
これに対し、最近では、パートナー型、すなわち人間との「共生」あるいは「エンターティンメント」という用途が注目されてきている。この種のロボットは、環境やものだけでなく、人と接するなど、高度な物理インタラクション能力を備えていることが望まれる。
【０００６】
パートナー型ロボットは、６軸マニピュレータなどのように所定の場所に固定されるのではなく、自律的に移動してさまざまなサービスを提供する、いわゆる移動ロボットが一般的である。パートナー型ロボットの代表例はヒューマノイドである。例えば、移動手段を備え、接地・無接地間を遷移する安定な運動パターンを実時間で生成することができるロボット装置について提案がなされている（例えば、特許文献１を参照のこと）。
【０００７】
この種のパートナー型ロボットの多くは、姿勢が常に一定となるような使われ方をするとは限らない。場合によっては、ロボットは床上を転げまわったり、転倒したりというように、大きな姿勢変化が発生しうる。また、ロボットが外界と行なうインタラクションの形態は、歩行時における足先と路面の接触や、手先で道具を扱うなどさまざまである。すなわち、外界とインタラクションを行なう部位は特に限定されず、体の任意の箇所で外界と接触しうる。
【０００８】
このように人や物と積極的且つ多様な物理的インタラクションを行なうロボットにとっては、外界との接触状態や力の作用状況、すなわち任意の部位に印加される外力を把握することは極めて重要である。
【０００９】
ロボットが受ける外力を検出する方法として、該当する部位に６軸力センサなどに代表される力センサを配備し、外力を直接検出することが一般的である。
【００１０】
例えば、足首に６軸力センサを取り付け、足底に作用する力の作用点と作用力を検出する脚式移動ロボットの足底接地位置検出装置について提案がなされている（例えば、特許文献２を参照のこと）。このロボットは、足平部の上方に力センサを設け、階段を昇降する際に足底に垂直なＺ方向の力成分Ｆzと、前後のＸ軸方向の力成分Ｆxと、左右のＹ軸方向の軸回りのモーメントＭyを検出し、これらの検出力足底に作用する抗力の床反力中心の位置ｘを逐次求める。そして、力成分Ｆzの検出値から足底の着床開始時点や離床終了時点を検知し、その検知時点近傍の時間範囲で求めた位置ｘから足底の着床開始接地位置や離床終了設置位置を求めるようになっている。
【００１１】
しかしながら、この種のロボットでは、力センサにより足平の先端に印加された力しか計測することができない。また、力センサよりも先端側に力の作用点が複数ある場合であっても、それらの合力しか検出することができない。このため、それぞれの作用点の場所を正確に特定することができず、各作用点に作用する力を独立に求めることができない。
【００１２】
また、圧力に応じて抵抗値や静電容量が変化する材料からなる接触センサや、オン／オフ・スイッチをロボットの表面全体に取り付け、これらの電気的信号の変化を検出することで、外界と接触しているからだの部位を特定する方法について提案がなされている（例えば、非特許文献１〜３を参照のこと）。
【００１３】
しかしながら、この種のセンサは、６軸力センサのような一般の力センサに比較して非常に精度が粗く、基本的にオン／オフすなわち外界に接触の有無に関する情報しか取得できない。すなわち、接触部位を特定することはできても、その部位に作用する力を検出することはできない。また、そのセンサ値を基本的にそのまま用いるだけでは、接触部位に作用する力に関する情報を得ることはできず、ロボット全体のダイナミクスのコントロールに活用することはできない。
【００１４】
また、分布型触覚センサで取得できる力情報の精度を高めることで、より豊富な外界との接触状態を取得することができる（例えば、非特許文献４を参照のこと）。しかしながら、本出願時の技術水準では、摩擦力と垂直反力を精度良く取得でき、ロボットの機体全体に分布配置できるような小型のセンサは実用レベルにはない。また、今後このようなセンサを実現することができたとしても、入力されるセンサ値の情報量が膨大となるため、そのデータ処理やデータ伝送方法の実現が困難となったり、システムの価格を引き上げたりする結果を招来する可能性がある。
【００１５】
【特許文献１】特開２００４−１６７６７６号公報
【特許文献２】特開平８−３３８３４１号公報
【非特許文献１】「ロボット表面多値接触センサの開発と応用」（日本機械学会ロボティクスメカトロニクス講演会９８ 講演予稿集，１ＣＩ１−２，１９９８）
【非特許文献２】「導電性ゲルを用いたロボット用柔軟触覚センサの開発と応用」（第１６ 回日本ロボット学会学術講演会予稿集，８７３−８７４，１９９８）
【非特許文献３】「導電性ファブリックを用いた全身被覆型触覚センサスーツ」（日本ロボット学会誌Ｖｏｌ．１６，Ｎｏ．１，ｐｐ．８０−８６， １９９８）
【非特許文献４】「器用な手の皮膚と触覚」（日本ロボット学会誌Ｖｏｌ．１８，Ｎｏ．６，ｐｐ．７６７−７７１， ２０００」
【発明の開示】
【発明が解決しようとする課題】
【００１６】
本発明の目的は、可動脚などの移動手段を備え、地上を自律的に移動して人や物と積極的且つ多様な物理的インタラクションを行なうことができる、優れたロボット装置を提供することにある。
【００１７】
本発明のさらなる目的は、人や物と物理的インタラクションを行なう際における外界との接触状態や、力の作用状況を好適に把握することができる、優れたロボット装置を提供することにある。
【００１８】
本発明のさらなる目的は、人や物と物理的インタラクションを行なう際に、外界と接触する部位を特定し、該接触部位に作用する力を推定して、ロボット全体のダイナミクスを好適にコントロールすることができる、優れたロボット装置を提供することにある。
【００１９】
本発明のさらなる目的は、全身（若しくは必要な場所）に分布型触覚センサを配備し、これらのセンサ出力と身体ダイナミクスの組み合わせから、外界と接している部位に印加する外力を好適に推定することができる、優れたロボット装置を提供することにある。
【課題を解決するための手段】
【００２０】
本発明は、上記課題を参酌してなされたものであり、その第１の側面は、複数の可動関節を備えた機体からなるロボット装置に作用する外力を推定する外力推定システムであって、
前記機体の任意の箇所における外界との接触状態を検出する分布型接触状態検出手段と、
各関節アクチュエータにおける現在の位置、速度、発生力を計測するアクチュエータ現在状態計測手段と、
前記機体上の少なくとも１箇所において加速度及び角速度を計測する運動状態計測手段と、
前記ロボット装置の運動方程式を設定する運動方程式設定手段と、
前記アクチュエータ現在状態計測手段により得られる各関節アクチュエータの現在状態、及び前記運動状態計測手段により得られる前記ロボット装置の現在状態から、前記運動方程式上の既知項を算出する既知項算出手段と、
前記運動方程式上の未知項としての外力の作用状態を推定する外力推定手段と、
を具備することを特徴とする外力推定システムである。
【００２１】
但し、ここで言う「システム」とは、複数の装置（又は特定の機能を実現する機能モジュール）が論理的に集合した物のことを言い、各装置や機能モジュールが単一の筐体内にあるか否かは特に問わない（以下、同様）。
【００２２】
人や物と積極的且つ多様な物理的インタラクションを行なうロボットにとっては、外界との接触状態や力の作用状況、すなわち任意の部位に印加される外力を把握することは極めて重要である。
【００２３】
ロボットが受ける外力を検出する方法として、該当する部位に６軸力センサなどに代表される力センサを配備し、外力を直接検出することが一般的である。しかしながら、力センサを設置した部位に印加される力を直接計測することしかできない。また、力センサよりも先端側に力の作用点が複数ある場合であっても、それらの合力しか検出することができない。このため、それぞれの作用点の場所を正確に特定することができず、各作用点に作用する力を独立に求めることができない。また、高価な力センサを、ロボットの機体全体にわたって高密度で配置することは現実的でない。
【００２４】
また、圧力に応じて抵抗値や静電容量が変化する材料からなる接触センサや、オン／オフ・スイッチをロボットの表面全体に取り付け、これらの電気的信号の変化を検出することで、外界と接触しているからだの部位を特定することはできるが、基本的にオン／オフすなわち外界に接触の有無に関する情報しか取得できず、接触部位を特定することはできても、その部位に作用する力を検出することはできない。
【００２５】
これに対し、本発明に係る外力推定システムは、オン／オフ情報が取得できる接触センサが機体上の任意の場所に配置されたロボットにおいて、各部位から得られるオン／オフ情報を、ロボットの機体全体のダイナミクス、関節力状態、ロボット全体の運動状態といった他の物理量と組み合わせることで、接触センサ部に作用する力を推定することができる。
【００２６】
本発明に係る外力推定システムによれば、機体上の所定の箇所においてロボット装置の運動方程式を設定し、前記既知項算出手段は、操作空間上でダイナミクス演算により操作空間慣性逆行列及びバイアス加速度を求めることにより、運動方程式上の既知項を算出する。そして、前記外力推定手段は、操作空間慣性逆行列及びバイアス加速度の間に前記運動方程式に合致する関係が成立するように、外力の作用状態を推定することができる。
【００２７】
ここで、前記既知項算出手段において実行する前記の操作空間ダイナミクス演算は、例えば順動力学演算を用いて構成することができる。
【００２８】
また、前記外力推定手段は、加速度と外力の間で成立する線形相補性問題を形成してこの問題を解くことにより、外力の作用状態を推定することができる。
【００２９】
また、前記運動状態計測手段は、所定の部位における姿勢及び位置を求める。
【００３０】
前記外力推定手段は、前記分布型接触状態検出手段により外界との接触が検出された部位に作用する外力を推定する。このとき、前記分布型接触状態検出手段により外界との接触が検出された部位において、当該部位の外形から得られる作用方向についての外力を推定することができる。具体的には、前記分布型接触状態検出手段により外界との接触が検出された部位における垂直反力、並びに摩擦力を推定することができる。
【００３１】
また、本発明の第２の側面は、複数の可動関節を備えた機体からなるロボット装置に作用する外力を推定する外力推定システムであって、
前記機体の任意の領域における外界との接触状態を検出する分布型面接触状態検出手段と、
各関節アクチュエータにおける現在の位置、速度、発生力を計測するアクチュエータ現在状態計測手段と、
前記機体上の少なくとも１箇所において加速度及び角速度を計測する運動状態計測手段と、
前記ロボット装置の運動方程式を設定する運動方程式設定手段と、
前記アクチュエータ現在状態計測手段により得られる各関節アクチュエータの現在状態、及び前記運動状態計測手段により得られる前記ロボット装置の現在状態から、前記運動方程式上の既知項を算出する既知項算出手段と、
前記運動方程式上の未知項としての外力を推定する外力推定手段と、
を具備することを特徴とする外力推定システムである。
【００３２】
上述した第１の側面に係る外力推定システムでは、分布型接触状態検出手段により機体の任意の箇所における外界との接触状態を検出するが、第２の側面に係る外力推定システムでは、分布型面接触状態検出手段は、外界との接触状態を領域単位で検出する。このような場合、前記外力推定手段は、前記分布型面接触状態検出手段により外界と接触していることが検出された領域の外形上の点に作用する外力を推定する。さらに、前記分布型面接触状態検出手段により外界と接触していることが検出された領域の外形を構成する各頂点に作用する外力を求め、その重心位置に外力が印加されたものと推定する。
【００３３】
また、本発明の第３の側面は、複数の可動関節を備えた機体と、外力が作用する作用点を有するロボット装置に作用する外力を推定する外力推定システムであって、
各関節アクチュエータにおける現在の位置、速度、発生力を計測するアクチュエータ現在状態計測手段と、
前記機体上の少なくとも１箇所において加速度及び角速度を計測する運動状態計測手段と、
前記作用点における前記ロボット装置の運動方程式を設定する運動方程式設定手段と、
前記アクチュエータ現在状態計測手段により得られる各関節アクチュエータの現在状態、及び前記運動状態計測手段により得られる前記ロボット装置の現在状態から、前記運動方程式上の既知項を算出する既知項算出手段と、
前記運動方程式上の未知項としての作用点における外力を推定する外力推定手段と、
を具備することを特徴とする外力推定システムである。
【００３４】
本発明の第３の側面に係る外力推定システムでは、外力推定の対象となるロボット装置において、外力が作用する作用点があらかじめ設定されているので、分布型の接触状態を検出する手段は必要ない。このような場合、前記既知項算出手段は、外力作用点における操作空間慣性逆行列及びバイアス加速度をダイナミクス演算により求めることにより、運動方程式上の既知項を算出する。そして、前記外力推定手段は、操作空間慣性逆行列及びバイアス加速度の間に前記運動方程式に合致する関係が成立するように、所定の外力作用点に印加される外力を推定することができる。
【００３５】
また、本発明の第４の側面は、記述されたコンピュータ・プログラムであって、前記コンピュータ・システムに対し、
前記機体の任意の箇所における外界との接触状態を検出する分布型接触状態検出手順と、
各関節アクチュエータにおける現在の位置、速度、発生力を計測するアクチュエータ現在状態計測手順と、
前記機体上の少なくとも１箇所において加速度及び角速度を計測する運動状態計測手順と、
前記ロボット装置の運動方程式を設定する運動方程式設定手順と、
前記アクチュエータ現在状態計測手順により得られる各関節アクチュエータの現在状態、及び前記運動状態計測手順により得られる前記ロボット装置の現在状態から、前記運動方程式上の既知項を算出する既知項算出手順と、
前記運動方程式上の未知項としての外力の作用状態を推定する外力推定手順と、
を実行させることを特徴とするコンピュータ・プログラムである。
【００３６】
また、本発明の第５の側面は、複数の可動関節を備えた機体からなるロボット装置に作用する外力を推定するための処理をコンピュータ・システム上で実行するようにコンピュータ可読形式で記述されたコンピュータ・プログラムであって、前記コンピュータ・システムに対し、
前記機体の任意の領域における外界との接触状態を検出する分布型面接触状態検出手順と、
各関節アクチュエータにおける現在の位置、速度、発生力を計測するアクチュエータ現在状態計測手順と、
前記機体上の少なくとも１箇所において加速度及び角速度を計測する運動状態計測手順と、
前記ロボット装置の運動方程式を設定する運動方程式設定手順と、
前記アクチュエータ現在状態計測手順により得られる各関節アクチュエータの現在状態、及び前記運動状態計測手順により得られる前記ロボット装置の現在状態から、前記運動方程式上の既知項を算出する既知項算出手順と、
前記運動方程式上の未知項としての外力を推定する外力推定手順と、
を実行させることを特徴とするコンピュータ・プログラムである。
【００３７】
また、本発明の第６の側面は、複数の可動関節を備えた機体と、外力が作用する作用点を有するロボット装置に作用する外力を推定するための処理をコンピュータ・システム上で実行するようにコンピュータ可読形式で述されたコンピュータ・プログラムであって、前記コンピュータ・システムに対し、
各関節アクチュエータにおける現在の位置、速度、発生力を計測するアクチュエータ現在状態計測ステップと、
前記機体上の少なくとも１箇所において加速度及び角速度を計測する運動状態計測ステップと、
前記作用点における前記ロボット装置の運動方程式を設定する運動方程式設定ステップと、
前記アクチュエータ現在状態計測ステップにより得られる各関節アクチュエータの現在状態、及び前記運動状態計測ステップにより得られる前記ロボット装置の現在状態から、前記運動方程式上の既知項を算出する既知項算出ステップと、
前記運動方程式上の未知項としての作用点における外力を推定する外力推定ステップと、
を実行させることを特徴とするコンピュータ・プログラムである。
【００３８】
本発明の第４乃至第６の各側面に係るコンピュータ・プログラムは、コンピュータ・システム上で所定の処理を実現するようにコンピュータ可読形式で記述されたコンピュータ・プログラムを定義したものである。換言すれば、本発明の第４乃至第６の各側面に係るコンピュータ・プログラムをコンピュータ・システムにインストールすることによって、コンピュータ・システム上では協働的作用が発揮され、本発明の第１乃至第３の各側面に係る外力推定システムと同様の作用効果を得ることができる。
【発明の効果】
【００３９】
本発明によれば、人や物と物理的インタラクションを行なう際の外界と接触する部位を特定し、該接触部位に作用する力を推定して、ロボット全体のダイナミクスを好適にコントロールすることができる、優れたロボット装置を提供することができる。
【００４０】
本発明によれば、本出願時において入手可能なオン／オフ型の分布型触覚センサと、ロボット全体のダイナミクス演算とを組み合わせて、各触覚センサに作用する力を推定することにより、ロボットが外界と多様な物理インタラクションを行なった際にも、体の任意箇所における接触状態と作用力をともに計測することが可能な外力推定方法を提供することができる。
【００４１】
また、本発明に係る外力推定方法を、加速度センサやジャイロといった慣性センサと組み合わせることにより、ロボット全体が移動し、その姿勢が一定に保たれないような状況に対しても。適用可能とすることができる。
【００４２】
従来、オン／オフ情報を検出する接触センサしか設置されない系では、そのセンサ出力値を何ら加工せず、オン／オフ情報としてのみ利用していた。これに対し、本発明によれば、各部位から得られるオン／オフ情報を、ロボットの機体全体のダイナミクス、関節力状態、ロボット全体の運動状態といった他の物理量と組み合わせることで、接触センサ部に作用する力を推定することができる。
【００４３】
本発明によれば、オン／オフ情報しか取得できない接触センサが配置された機械装置に適用することで、その他の運動状態で、オン状態を検出したセンサの部位に作用する力を推定することができる。すなわち、簡便なシステム構成により多くの力情報を推定することができ、その結果として、推定できる力の情報量に対し安価なシステムを実現することができる。
【００４４】
多くの力センサは、希歪体などの脆弱な構造を有する。これに対し、本発明に係る外力推定方法は、オン／オフ情報のみを検出する接触センサと、姿勢センサと、アクチュエータ内部のエンコーダ並びに電流センサなどのセンサ出力に基づいて力を推定するので、全体として脆弱な構造が少なく、ロバストである。したがって、再キャリブレーションなどの手間も発生し難い。
【００４５】
本発明のさらに他の目的、特徴や利点は、後述する本発明の実施形態や添付する図面に基づくより詳細な説明によって明らかになるであろう。
【発明を実施するための最良の形態】
【００４６】
以下、図面を参照しながら本発明の実施形態について詳解する。
【００４７】
Ａ．ロボットの構成
図１には、本発明の一実施形態に係るロボット装置の外観構成を示している。図示のロボット装置は、いわゆる人間型ロボットであり、骨盤部には、移動手段としての２肢の脚体と、腰関節を介して上体が接続されている。上体には、２肢の腕部と、首関節を介して頭部が接続されている。
【００４８】
左右の脚体は、それぞれ股関節３自由度と、膝関節１自由度と、足首関節２自由度の、計６自由度を備えている。また、左右の腕部は、それぞれ肩関節３自由度と、肘関節１自由度と、手首関節２自由度の、計６自由度を備えている。首関節及び腰関節は、ともにＸ、Ｙ、Ｚ 軸回りに３自由度を有するとする。
【００４９】
各関節軸を駆動するアクチュエータは、例えばＤＣブラシレス・モータと減速機、並びに減速機の出力軸の回転位置を検出する位置センサで構成される。これら関節駆動アクチュエータは、ロボット装置全体の動作を統括的にコントロールするホスト・コンピュータと接続され、その位置制御目標値がホスト・コンピュータから与えられるとともに、現在の出力トルクや、関節角度、関節角速度をホスト・コンピュータに送信することができるものとする。
【００５０】
図１に示すロボット装置の表面のうち、外界との接触が想定されている各部位には、触覚センサ群ｔ１、ｔ２、…、ｔ１７が取り付けられている。図２には、１つの触覚センサ群の構成を示している。同図に示すように、１つの触覚センサ群ｔは、接触状態をそれぞれ独立して検出することができる複数の触覚センサＣＳをアレイ状に配置して構成され、触覚センサ群ｔとしてはいずれの触覚センサＣＳが接触状態にあるかによって、詳細な接触位置を特定することができる。
【００５１】
図３には、触覚センサＣＳの構成を模式的に示している。触覚センサＣＳは、２枚の極板Ｐ１及びＰ２で空隙Ｓを挟んだ構造をしており、一方の極板Ｐ１には電位Ｖccが印加され、他方の極板Ｐ２は接地されている。そして、一方の極板Ｐ１をパラレル・インターフェース（ＰＩＯ）経由でマイクロ・コンピュータに入力し、極板間の接触状態すなわち触覚センサＣＳに外力が作用しているか否かを判定することができる。
【００５２】
触覚センサ群ｔ毎に、その近隣に１つのマイクロ・コンピュータを配置して、触覚センサ群を構成するすべての触覚センサＣＳの検出信号を入力し、これらのオン／オフ状態を収集して、当該部位における外界との接触の有無並びに接触位置をホスト・コンピュータに送信するものとする。
【００５３】
また、図１に示すように、ロボット装置の骨盤部には、３軸の加速度センサａ１と３軸の角速度センサ（ジャイロ）ｇ１が搭載されている。その近隣には、これらのセンサ値を計測するマイクロ・コンピュータが配備され、その計測結果をホスト・コンピュータに送信するものとする。
【００５４】
図４には、図１に示したロボット装置における結線トポロジの構成例を示している。
【００５５】
ロボット装置は、胴体部に、３軸の腰関節アクチュエータａ１、ａ２、ａ３、及び３軸の首関節アクチュエータａ１６、ａ１７、ａ１８を持ち、これらはホスト・コンピュータに対しシリアル接続されている。各関節アクチュエータは、シリアル・ケーブルを通じて、その位置制御目標値を受け取るとともに、現在の出力トルクや、関節角度、関節角速度をホスト・コンピュータに送信する。
【００５６】
また、ロボット装置は、左腕部に、３軸の肩関節アクチュエータａ４、ａ５、ａ６、１軸の肘関節アクチュエータａ７、及び２軸の手首関節アクチュエータａ８、ａ９を持ち、これらはホスト・コンピュータに対しシリアル接続されている。同様に、右腕部には、３軸の肩関節アクチュエータａ１０、ａ１１、ａ１２、１軸の肘関節アクチュエータａ１３、及び２軸の手首関節アクチュエータａ１４、ａ１５を持ち、これらはホスト・コンピュータに対しシリアル接続されている。
【００５７】
また、ロボット装置は、左脚部に、３軸の股関節アクチュエータａ１９、ａ２０、ａ２１、１軸の膝関節アクチュエータａ２２、及び２軸の足首関節アクチュエータａ２３、ａ２４を持ち、これらはホスト・コンピュータに対しシリアル接続されている。同様に、右脚部には、３軸の肩関節アクチュエータａ２５、ａ２６、ａ２７、１軸の肘関節アクチュエータａ２８、及び２軸の足首関節アクチュエータａ２９、ａ３０を持ち、これらはホスト・コンピュータに対しシリアル接続されている。
【００５８】
各関節で使用されるアクチュエータａ１〜ａ３０は、例えばＤＣブラシレス・モータと減速機、並びに減速機の出力軸の回転位置を検出する位置センサで構成され、外部から与えられた位置制御目標値に従って回転駆動するとともに、現在の出力トルクや、関節角度、関節角速度を出力する。この種の関節アクチュエータについては、例えば本出願人に既に譲渡されている特開２００４−１８１６１３号公報に記載されている。
【００５９】
ロボット装置の右脚部には、右足部触覚センサ群ｔ１、右脛部触覚センサ群ｔ２、右大腿部触覚センサ群ｔ３が配設され、これらはホスト・コンピュータに対しシリアル接続されている。各々の触覚センサ群ｔ１〜ｔ３にはマイクロ・コンピュータが装備されており（前述）、それぞれ触覚センサ群における触覚センサＣＳのオン／オフ状態を収集して、シリアル・ケーブルを通じてホスト・コンピュータに送信する。同様に、左脚部には、左足部触覚センサ群ｔ９、左脛部触覚センサ群ｔ１０、左大腿部触覚センサ群ｔ１１が配設され、それぞれ触覚センサ群における触覚センサＣＳのオン／オフ状態を内部のマイクロ・コンピュータで収集して、シリアル・ケーブルを通じてホスト・コンピュータに送信する。
【００６０】
また、ロボット装置の右腕部には、右手首触覚センサ群ｔ４、右前腕部触覚センサ群ｔ５、右上腕部触覚センサ群ｔ６が配設され、それぞれ触覚センサ群における触覚センサＣＳのオン／オフ状態を内部のマイクロ・コンピュータで収集して、シリアル・ケーブルを通じてホスト・コンピュータに送信する。同様に、左腕部には、左手首触覚センサ群ｔ１２、左前腕部触覚センサ群ｔ１３、左上腕部触覚センサ群ｔ１４が配設され、それぞれ触覚センサ群における触覚センサＣＳのオン／オフ状態を内部のマイクロ・コンピュータで収集して、シリアル・ケーブルを通じてホスト・コンピュータに送信する。
【００６１】
また、ロボット装置の胴体部の左右にはそれぞれ胴部触覚センサ群ｔ７及びｔ１５が取り付けられており、それぞれ触覚センサ群における触覚センサＣＳのオン／オフ状態を内部のマイクロ・コンピュータで収集して、シリアル・ケーブルを通じてホスト・コンピュータに送信する。
【００６２】
また、ロボット装置の頭部の左右にはそれぞれ頭部触覚センサ群ｔ８及びｔ１６が取り付けられており、それぞれ触覚センサ群における触覚センサＣＳのオン／オフ状態を内部のマイクロ・コンピュータで収集して、シリアル・ケーブルを通じてホスト・コンピュータに送信する。
【００６３】
Ｂ．外力の推定方法
本発明に係るロボット装置では、オン／オフ型の分布型触覚センサとロボット全体のダイナミクス演算とを組み合わせて外力を推定することができる。すなわち、各部位から得られるオン／オフ情報を、ロボットの機体全体のダイナミクス、関節力状態、ロボット全体の運動状態といった他の物理量と組み合わせることで、接触センサ部に作用する力を推定する。したがって、ロボット装置は、外界と多様な物理インタラクションを行なった際にも、体の任意箇所における接触状態と作用力をともに計測することが可能である。
【００６４】
以下では、外界からの力の作用点を検出することができても、その力の大きさを方向が検出することができないシステムにおいて、すべての力の作用点における外力の大きさとその方向を推定する方法について考察する。
【００６５】
図５には、図１に示したロボット装置の関節自由度モデルを示している。同図から分かるように、２足歩行の移動ロボットを、骨盤Ｂを基底とする開リンク木構造として表現することができる。
【００６６】
移動ロボットの場合、ロボットがワールド空間中を自由に移動し、姿勢を変化させることができる。したがって、ロボット全体の状態を表すための状態変数として、骨盤Ｂのオイラー角α＝（α，β，γ）Tとその位置ｐ0（ｐ0x，ｐ0y，ｐ0z）Tを導入する。すると、ロボットはリンク構造物として扱うことができるので、ロボットの運動方程式は、下式（１）のような形で表現することができる。
【００６７】
【数１】
【００６８】
ここで、ｑはロボット全体の一般化変数であり、ロボットの運動状態としてのベースの姿勢α及び位置ｐ0を並べたベクトルと、アクチュエータの現在状態としての全関節値を並べてできるベクトルθで構成される。すなわち、ｑは下式（２）のような形で表現される。
【００６９】
【数２】
【００７０】
また、上式（１）中で、τは、一般化変数ｑに対応した一般化力であり、αとｐ0に対応する成分は（力を発生するアクチュエータが存在しないため）０となり、θに対応する成分は各関節部を駆動するアクチュエータの発生力に等しい。Ｈは、ロボット全体の慣性行列、ｂは重力やコリオリ力などを表す。ｆは外力である。Ｊは、外力作用方向に発生する速度／ｄｏｔ ｘ（Ｌａｔｅｘの書式に基づく）と、一般化速度／ｄｏｔ ｑ（Ｌａｔｅｘの書式に基づく）とを対応付けるヤコビアンであり、下式（３）の通りとなる。
【００７１】
【数３】
【００７２】
上式（１）を変形すると、下式（４）が得られる。
【００７３】
【数４】
【００７４】
ここで、Λ-1は、外力作用方向に関する操作空間慣性逆行列であり、下式（５）で表される。
【００７５】
【数５】
【００７６】
上式（４）において、右辺第１項、同第２項、及び操作空間慣性逆行列Λ-1は、後述する方法によって、現在の運動状態から見積もることができる。しかし、外力作用方向に発生する加速度と外力ｆは自由な組み合わせが許される訳ではない。垂直反力は多くの場合、ロボットと環境を乖離させる方向にのみ作用し、逆向きには作用しない。摩擦力はこの垂直反力によって決定される摩擦円錐以上の力を作用することはできない。すなわち、以下の式（６）〜（８）に示すような線形相補性問題を形成する。
【００７７】
【数６】
【００７８】
【数７】
【００７９】
【数８】
【００８０】
ここで、上式（６）中のｃはバイアス加速度、すなわち外力が一切作用しない場合に作用点に発生する加速度であり、下式で表される。
【００８１】
【数９】
【００８２】
また、上式（７）及び（８）中でそれぞれ存在するｆNi及びｆFiは、作用点ｉにおける法線方向の外力及び摩擦力を表す。μiは当該作用点における摩擦係数である。
【００８３】
本発明によれば、このような線形相補性問題を解くことによってすべての外力ｆを推定することができる、という点を十分理解されたい。なお、線形相補性問題の数学的な解法自体については、例えば“Ｆａｓｔ Ｃｏｎｔａｃｔ Ｆｏｒｃｅ Ｃｏｍｐｕｔａｔｉｏｎ ｆｏｒ Ｎｏｎｐｅｎｅｔｒａｔｉｎｇ Ｒｉｇｉｄ Ｂｏｄｉｅｓ”（ＳＩＧＧＲＡＰＨ９４，ｐｐ．２３−３４，１９９４）に記載されているので、ここでは説明を省略する。以下では、ロボットの運動状態から操作空間慣性逆行列Λ-1並びにバイアス加速度ｃを推定する方法について述べる。
【００８４】
操作空間慣性逆行列Λ-1の算出方法：
上式（６）において、右辺から左辺を得ることを考えると、外力やアクチュエータ発生力、重力、速度積に関連する力（コリオリ力など）が作用したときの、外力作用点における外力作用方向の加速度を求める問題として扱うことができる。力から加速度を得る問題は、一般に、順動力学演算として知られており、下式のように表現される。
【００８５】
【数１０】
【００８６】
最も基本的な順動力学演算については、例えば、“Ｅｆｆｉｃｉｅｎｔ Ｄｙｎａｍｉｃ Ｃｏｍｐｕｔｅｒ Ｓｉｍｕｌａｔｉｏｎ ｏｆ Ｒｏｂｏｔｉｃ Ｍｅｃｈａｎｉｓｍｓ”（Ｊｏｕｒｎａｌ ｏｆ Ｄｙｎａｍｉｃ Ｓｙｓｔｅｍｓ，Ｍｅａｓｕｒｅｍｅｎｔ ａｎｄ Ｃｏｎｔｒｏｌ，ｐｐ．２０５−２１１，１９８２）を参照されたい。
【００８７】
ここで、下式のようにおくと、重力、関節力、速度積に関連する力（コリオリ力など）が発生しない状況において発生する加速度を求めることができる。すなわち、ｃ＝０とすることができる。
【００８８】
【数１１】
【００８９】
ｆ＝ｅi、すなわち、ｉ番目の単位外力のみ作用させる条件下で計算を実行すると、Λ-1の第ｉ列を求めることができることが分かる。よって、以下の演算をすべてのｉについて実行することよってΛ-1を得ることができる。
【００９０】
【数１２】
【００９１】
バイアス加速度ｃの算出方法：
逆に、順動力学演算を外力ｆ＝０として実行することで、バイアス加速度ｃを抽出することができる。すなわち、
【００９２】
【数１３】
【００９３】
上記の各式（９）及び（１０）から分かるように、操作空間慣性逆行列Λ-1及びバイアス加速度ｃを算出する際に必要となる情報として、一般化変数ｑ及びその一般化速度／ｄｏｔ ｑ（Ｌａｔｅｘの書式に基づく）、一般化力τ、重力加速度ｇがある。以下では、さらにこれらの算出方法について説明する。
【００９４】
一般化変数ｑの算出方法：
一般化変数ｑの要素のうち、ロボットの実関節に対応する要素については、ロボットの関節角そのものであり、エンコーダによって計測した結果をそのまま用いることができる。
【００９５】
ロボット全体の姿勢及び位置に対応した成分、すなわち、骨盤Ｂのオイラー角αとその位置ｐ0については、基本的に慣性センサ（図１中のａ１、ｇ１）によって見積もることができる。しかし、そのすべての成分を決定する必要はない。順動力学の演算結果を積分して位置の履歴を形成する力学シミュレーションへの応用と異なり、本実施形態では、重力方向に対する骨盤Ｂの姿勢が分かれば十分である。但し、この点についての詳細は順動力学の詳細な演算内容に関わり煩雑になるので、本明細書ではこれ以上説明しない。
【００９６】
このような重力ベクトルに対する機体姿勢を、そこに搭載された加速度センサとジャイロの出力から求める方法は、ストラップダウン方式と呼ばれる周知の手法であり、この結果から骨盤のオイラー角αを得ることができる。ストラップダウン方式による姿勢検出方法については、例えば特開平６−２０１８６３号公報に記載されている。一方、骨盤の位置ｐ0は特に重要でないことから、ここではｐ0＝０としておく。
【００９７】
一般化速度の算出方法：
一般化速度／ｄｏｔ ｑ（Ｌａｔｅｘの書式に基づく）の要素のうち、ロボットの実関節に対応する要素については、ロボットの関節角速度に等しいので、エンコーダの数値微分やタコメータによって計測した結果をそのまま用いることができる。
【００９８】
ロボット全体の姿勢及び位置に対応した成分、すなわち、骨盤Ｂのオイラー角αの角速度／ｄｏｔ α（Ｌａｔｅｘの書式に基づく）及びその位置ｐ0の速度／ｄｏｔ ｐ0（Ｌａｔｅｘの書式に基づく）については、基本的には慣性センサによって見積もることができる。順動力学演算は、各リンク座標系におけるローカル表現に変換することが可能であり、その場合、これらの物理量は、骨盤Ｂに固定された座標系から見た骨盤Ｂに配設したセンサ（図５を参照のこと）から得られる角速度及び並進速度によって代替することができる。但し、この点についての詳細は順動力学の詳細な演算内容に関わり煩雑になるので、本明細書では説明を省略する。
【００９９】
つまり、骨盤Ｂのオイラー角αの角速度値／ｄｏｔ α（Ｌａｔｅｘの書式に基づく）としてジャイロの出力を用いることができ、その位置ｐ0の速度値／ｄｏｔ ｐ0（Ｌａｔｅｘの書式に基づく）として加速度センサから重力加速度を除去した成分の積分値を用いることができる。加速度センサから重力加速度を除去するには、以下の演算を行なえばよい。
【０１００】
【数１４】
【０１０１】
ここで、ａmは加速度センサの出力であり、ＲBは前述のストラップダウン方式で得られた骨盤Ｂの姿勢行列、ａは骨盤Ｂに固定された座標系から見た並進加速度である。原理的には、ａを数値積分すれば骨盤Ｂの位置ｐ0における速度／ｄｏｔ ｐ0（Ｌａｔｅｘの書式に基づく）を得ることができるが、実際にはドリフトの問題があるため、適切なハイパス・フィルタに積分値を透過させる結果を用いる必要がある。
【０１０２】
一般化力τの算出方法：
一般化力τの成分のうち、ロボットの実関節に対応する要素については、ロボットのアクチュエータが発するトルクそのものであるから、アクチュエータ内のトルクセンサや電流センサによって計測した結果を用いることができる。
【０１０３】
ロボット全体の姿勢及び位置に対応した成分については、アクチュエータが存在しないことから、０となる。なお、アクチュエータ内の慣性や摩擦は順動力学演算に含まれ、考慮されるが、高い精度でその値を同定することは難しく、定式化されない要素も少なくない。その値はアクチュエータの減速比とともに大きくなることから、アクチュエータの減速比は極力低くすることが望ましい。総じて、本発明を好適に実現するには、バックドライバビリティの高いアクチュエータを用いることが望ましい。
【０１０４】
重力加速度ｇの算出について：
重力加速度に特別な計算は必要ない。順動力学演算では、演算の基底が上向きに重力加速度ｇで加速しているとモデル化することで重力を考慮する。この値を外部から変更可能とすれば、順動力学演算において重力加速度を変数として扱うことができる。
【０１０５】
外力の作用方向の定義：
以上の説明で、一般化変数ｑ及びその一般化速度／ｄｏｔ ｑ（Ｌａｔｅｘの書式に基づく）、一般化力τ、重力加速度ｇの算出方法が明らかとなった。さらに、上式（６）を解くには、外力の作用点とその方向を定める必要がある。以下では、外力の作用方向の設定方法について説明する。
【０１０６】
図６には、接触センサＣＳが設置されている場所に設定された座標系Ｃcsを示している。座標系は、この点におけるロボット形状の法線方向にｚ軸を、これと垂直な方向にｘ軸及びｙ軸を持つものとする。ロボットの形状は個々のリンクレベルでは変形しないものとすると、各接触センサの設置点におけるｘ軸、ｙ軸及びｘ軸の方向は不変であり、あらかじめ算出して固定テーブルとして保持しておくことができる。
【０１０７】
外力の作用点は、出力がオンとなった接触センサＣＳの設置位置、すなわち座標系Ｃcsの位置となる。接触センサＣＳにおける力の作用方向は、これらの座標軸と反対方向に設定する。すなわち、垂直反力ｆNの方向は−ｚ軸方向、摩擦力ｆFx及びｆFyの方向は−ｘ軸及び−ｙ軸方向にそれぞれ設定される。順動力学演算がローカル座標系表現で構成されているとすると、外力の作用点及び作用方向についても、外力が作用するリンクに固定された座標系から見た、ローカル座標系表現で用意すればよい。リンクが変形しないことを前提とすれば、いずれも固定値となる。また、この点における摩擦係数についても、このリンクの素材に基づいて予め決定しておくことができ、定数となる。
【０１０８】
垂直反力と摩擦力の作用方向は、実際には外界の形状にも依存する。したがって、外力推定の精度をより高めたい場合には、各リンクの外形を微分可能な曲線で構成するなどの対策をとってもよい。摩擦係数も厳密には接触対象の素材にも依存するが、ここでは近似的にリンクの素材のみで決定するように構成した。相手の素材も既知であれば、両者の平均を取るなどの処理を行なってもよい。
【０１０９】
以上の結果、すべての接触センサにおける垂直反力ｆNと摩擦力ｆFx及びｆFyを連結して並べたベクトルとして、下式（１２）に示す外力ベクトル全体が定義される。
【０１１０】
【数１５】
【０１１１】
上式（１２）中で、Ｍは接触センサの総数である。上述した方法により、上式（６）で示した線形相補性問題式を解くことにより、これらの外力の値がすべて決定することができる。
【０１１２】
図７には、これまで説明してきた外力推定方法を実現するためのロボット装置の機能構成をブロック図の形式で示している。同図に示すように、本発明に係る外力推定方法を搭載したロボット装置は、接触検出部１、アクチュエータ部２、姿勢検出部３、外力推定部４という４つの機能部ロックに大別される。
【０１１３】
接触検出部１は、上述した触覚センサ群（図２を参照のこと）と、各触覚センサが取り付けられている位置データ、接触点における外形の法線ベクトル、接線ベクトル、摩擦係数に関するデータを有する。位置データ、法線ベクトル、接線ベクトルに関しては、触覚センサが取り付けられているリンクに固定されたローカル座標系から見た表現で保持する。接触検出部１は、接触センサ群１−１を構成するすべての接触センサのオン／オフ状態を計測し、要求に応じて接触部位の位置データ、法線ベクトル、接線ベクトル、摩擦係数のリストを返答する。
【０１１４】
アクチュエータ部２は、すべての位置制御型アクチュエータの制御と、その状態検出を行なう。すなわち、角度目標値を実現するようにアクチュエータのトルク出力を調整する位置制御部２−１と、アクチュエータの現在角度を計測する位置検出部２−２、アクチュエータの現在角速度を計測する速度検出部２−３、アクチュエータの出力トルクを計測するトルク検出部２−４を備えている。アクチュエータ部２は、要求に応じてアクチュエータの目標角度を受理したり、現在の角度、角速度、トルク状態を返答したりする。
【０１１５】
姿勢検出部３は、骨盤部に取り付けられたジャイロ３−１と加速度センサ３−２（図４を参照のこと）の計測を行ない、要求に応じてそれらの値を返答する。
【０１１６】
外力推定部４は、接触検出部１、アクチュエータ部２、及び姿勢検出部３からの返答結果を基に、各接触センサ部に発生する外力を推定する。以下、その内部構成について説明する。
【０１１７】
接触部位決定部４−１は、接触検出部１に問い合わせ、現在接触状態すなわちオン状態にある接触センサＣＳのリストとその位置リストを作成する。
【０１１８】
外力方向設定部４−２は、接触部位決定部４−１でオン状態として検出された接触センサにおける、法線情報と接線情報を接触検出部１に問い合わせ、垂直反力と摩擦力（ｘ及びｙの２方向）の方向を決定する。方向はそれぞれ法線ベクトルと接線ベクトルの逆向きとなる。
【０１１９】
摩擦係数設定部４−３では、外力の作用点における摩擦係数を設定する。摩擦係数は、リンクの素材に基づいて予め決定しておくことができ、定数となる。摩擦係数は、厳密には接触対象の素材にも依存するが、ここでは近似的にリンクの素材のみで決定するように構成した。相手の素材も既知であれば、両者の平均を取るなどの処理を行なってもよい。
【０１２０】
骨盤姿勢算出部４−４は、姿勢検出部で計測された骨盤の角速度と加速度情報を基に、ストラップダウン方式に基づく姿勢演算を実行して、骨盤部の姿勢情報を算出する。
【０１２１】
一般化変数決定部４−５は、骨盤姿勢算出部４−４が作成した骨盤部の姿勢情報と、アクチュエータ部から得られるすべてのアクチュエータの角度情報を基に、一般化変数ベクトルｑを作成する。
【０１２２】
一般化速度決定部４−６は、姿勢検出部３で計測された骨盤の角速度（ジャイロ計測値）と加速度の積分値（実際には既に説明したようにハイパス・フィルタに透過させるのが望ましい）、及びアクチュエータ部２から得られるすべてのアクチュエータの角速度を基に、一般化変数ｑの速度ベクトルを作成する。
【０１２３】
一般化力決定部４−７は、アクチュエータ部２から得られるすべてのアクチュエータの出力トルクを基に、一般化力ベクトルτを作成する。
【０１２４】
順動力学演算部４−８は、下式に示す順動力学演算を実行する演算ユニットである。
【０１２５】
【数１６】
【０１２６】
引数の値を可変に与えられるように構成されている。返り値として、一般化加速度／ｄｄｏｔ ｑ（Ｌａｔｅｘの書式に基づく）ではなく、外力の作用方向に発生する加速度のベクトルｘを返すところが、一般的な順動力学演算と異なるが、内部的には一旦一般化加速度を得て、これを基に各外力方向の加速度を算出するように構成すればよい。
【０１２７】
操作空間慣性逆行列演算部４−９は、順動力学演算部４−８を用い、上式（９） の演算をすべての外力に関して実行し、操作空間慣性逆行列Λ-1を算出する。すなわち、すべての外力に関して単位外力（ノルムが１の外力）をテスト・フォースとして作用させたときの操作空間加速度（外力作用点に発生する加速度）／ｄｄｏｔ ｘ（Ｌａｔｅｘの書式に基づく）を算出するという処理を繰り返してΛ-1を得る。外力の作用位置や作用方向については、接触部位決定部４−１と外力方向決定部４−２の結果を用いる。
【０１２８】
バイアス加速度演算部４−１０は、順動力学演算部４−８を用い、上式（１０）に示した演算を実行し、バイアス加速度ｃを算出する。この際、一般化変数決定部４−５が作成した一般化変数ｑ、一般化速度決定部４−６が作成した一般化速度／ｄｏｔ ｑ（Ｌａｔｅｘの書式に基づく）、一般化力決定部４−７が作成した一般化力τを用いる。外力の作用位置や作用方向については、操作空間慣性逆行列演算部４−９と同様に、接触部位決定部４−１、外力方向決定部４−２の結果を用いる。
【０１２９】
線形相補性問題解決部４−１１は、操作空間慣性逆行列算出部４−９によって算出された操作空間慣性逆行列Λ-1、バイアス加速度算出部４−１０によって算出されたバイアス加速度ｃを線形相補性問題式（６）、式（７）、式（８）に設定し、これを解く。式（８）の摩擦係数には、摩擦係数設定部４−３で得られた結果を用いる。この線形相補性問題を解いた結果、すべての外力情報ｆが決定される。
【０１３０】
図８には、本発明に係る外力推定を行なうための処理手順をフローチャートの形式で示している。
【０１３１】
まず、すべての接触センサの状態計測を行なう（ステップＳ１）。次いで、すべてのアクチュエータの関節角度を計測する（ステップＳ２）。次いで、すべてのアクチュエータの関節角速度を計測する（ステップＳ３）。次いで、すべてのアクチュエータの関節トルクを計測する（ステップＳ４）。
【０１３２】
次いで、骨盤部Ｂに設置された加速度センサ及びジャイロを計測する（ステップＳ５）。そして、ステップＳ５で得られた結果に対してストラップダウン方式に基づく姿勢演算を行ない、骨盤部の姿勢情報を算出する（ステップＳ６）。
【０１３３】
次いで、ステップＳ１で得られた結果に基づいて、外界と接触を起こしている部位の位置（リンクに固定された座標系から見たローカル表現）をリストアップする（ステップＳ７）。そして、外界と接触している各点において作用する垂直反力と摩擦力の方向を決定する（ステップＳ８）。
【０１３４】
次いで、外界と接触している各点における摩擦係数をデータベースから取り出す（ステップＳ９）。
【０１３５】
そして、ステップＳ２及びステップＳ６の結果を用いて、一般化変数ｑの値を決定する（ステップＳ１０）。また、ステップＳ３及びＳ５の結果を用いて一般化速度／ｄｏｔ ｑ（Ｌａｔｅｘの書式に基づく）の値を決定する（ステップＳ１１）。そして、ステップＳ４の結果を用いて、一般化力τの値を決定する（ステップＳ１２）。
【０１３６】
次いで、順動力学演算を用いて、すべての外力に関して式（９）を実行して、操作空間慣性逆行列Λ-1を算出する（ステップＳ１３）。また、順動力学演算を用いて式（１０）を実行して、バイアス加速度ｃを算出する（ステップＳ１４）。
【０１３７】
次いで、ステップＳ１３で得られた操作空間慣性逆行列Λ-1、並びにステップＳ１４で得られたバイアス加速度ｃ、並びにステップＳ９で得られた摩擦係数を線形相補性問題（式（６）から式（８））に設定する（ステップＳ１５）。
【０１３８】
そして、ステップＳ１５で設定された線形相補性問題を解いて、すべての外力（垂直反力と摩擦力）を求める。
【０１３９】
以上の処理を一定周期（例えば制御周期）で実行する（ステップＳ１６）。操作空間慣性逆行列Λ-1の算出など、計算時間を要する部分に関しては、選択的に低い更新レートで実行するように構成してもよい（操作空間慣性逆行列は時間的に急激に変化しない特性を有しているため、特に有効である）。
【０１４０】
Ｃ．変形例
上述した説明では、接触検出部１は、図２に示したように複数の接触センサをアレイ状に配設して構成される触覚センサ群をロボットの全身に配設して構成されており、外界と接触している部位を特定することができることを前提としている。その変形例として、外界と接触している部位ではなく、外界と接触している領域を検出する接触検出部を用いることによっても、同様に本発明に係る外力推定を実現することができる。
【０１４１】
ここでは、凸領域のいずれかの場所が外界と接触していることを検出できる面接触センサＲＣＳ（図９を参照のこと）がロボット表面に分散して配置されている場合について考察する。
【０１４２】
このような場合、面接触センサＲＣＳの外形上に外力作用点を設定して、これらの点に上述した点接触センサがあるとみなして、同様の方法で外力を推定することができる。図９に示した例では、５つの点の点接触センサに分割して扱う様子を示している。
【０１４３】
線形相補性問題を解くと、これらすべての点において印加される外力の値が決定される。ここから面接触センサ領域内の１点の作用点ｘaを得るには、面接触センサが平面とみなすことができる場合に、下式に示すように垂直反力の重心位置を求めればよい。
【０１４４】
【数１７】
【０１４５】
上式において、ｆNiは第ｉ頂点の垂直反力、ｘiは第ｉ頂点の位置を表す。ｘaへの作用力はすべての頂点ｘiにおける垂直反力と摩擦力の合力となる。
【０１４６】
Ｄ．変形例２
上述した説明では、接触検出部１により外界と接触している部位又は領域を特定するようになっている。これに対し、接触部位が決まっている、あるいはリンク原点など決められた位置に作用する外力を推定したい場合は、必ずしも接触センサを配置する必要はない。本発明によれば、いずれの方法であれ外力の作用位置を指定しさえすれば、そこに印加される外力を推定することができる。すなわち、接触センサを配置して外力の作用点を検出するのではなく、あらかじめ外力作用位置を指定し、そこに印加される外力を推定することができる。
【０１４７】
また、本発明に係る外力推定方法は、外力として、「力」だけでなく「モーメント」を求めるように構成することも可能である。具体的には、以下に示す順動力学演算において、力の代わりにモーメントも作用させられるように構成する。また、これに対応する加速度の成分については、並進加速度の代わりに角加速度を算出するように構成すればよい。
【０１４８】
【数１８】
【０１４９】
このようにすれば、上式（９）において、モーメントに該当する外力ｆの成分に関しては、単位外力の代わりに単位外モーメントを作用させるなど、上述した外力を推定するのと基本的に同一の手順に従ってモーメントを推定することができる。
【０１５０】
また、線形相補性問題の拘束条件は、例に挙げた摩擦力や垂直反力に関する拘束条件限らず、線形相補性問題ソルバの構成によってより多様な拘束を扱うこともできる。
【産業上の利用可能性】
【０１５１】
以上、特定の実施形態を参照しながら、本発明について詳解してきた。しかしながら、本発明の要旨を逸脱しない範囲で当業者が該実施形態の修正や代用を成し得ることは自明である。
【０１５２】
本明細書では、本発明を２足歩行の脚式移動ロボットに適用した実施形態を中心に説明してきたが、本発明の要旨はこれに限定されるものではない。本発明に係る外力推定方法は、基本的にはロボット装置の構造に依存せず、任意の構造に対して適用することができる。具体的には、順動力学演算を構造非依存な高汎用性を有した形態で実装することができる。
【０１５３】
本発明に係る外力推定方法は、６軸力センサを用いて局所的な箇所への作用力を検出する従来の方法とは異なり、接触センサを配置した任意の箇所に作用する力を推定することができる。したがって、移動ロボットに限らず、機体の至る所で外界と接触を生じるさまざまな機械装置に適用して、その状況を把握するのに有効である。
【０１５４】
要するに、例示という形態で本発明を開示してきたのであり、本明細書の記載内容を限定的に解釈するべきではない。本発明の要旨を判断するためには、特許請求の範囲を参酌すべきである。
【図面の簡単な説明】
【０１５５】
【図１】図１は、本発明の一実施形態に係るロボット装置の外観構成を示した図である。
【図２】図２は、触覚センサ群の構成を示した図である。
【図３】図３は、触覚センサＣＳの構成を模式的に示した図である。
【図４】図４は、図１に示したロボット装置における結線トポロジの構成例を示した図である。
【図５】図５は、図１に示したロボット装置の関節自由度モデルを示した図である。
【図６】図６は、接触センサＣＳ が設置されている場所に設定された座標系Ｃcsを示した図である。
【図７】図７は、本発明に係る外力推定方法を実現するための機能構成を示したブロック図である。
【図８】図８は、ロボット装置に置ける外力推定を行なうための処理手順を示したフローチャートである。
【図９】図９は、凸領域のいずれかの場所が外界と接触していることを検出できる面接触センサＲＣＳの構成例を示した図である。
【符号の説明】
【０１５６】
１…接触検出部
２…アクチュエータ部
３…姿勢検出部
４…外力推定部
【技術分野】
【０００１】
本発明は、複数の関節自由度を持つロボット装置に係り、特に、可動脚などの移動手段を備えたロボット装置に関する。
【０００２】
さらに詳しくは、本発明は、地上を自律的に移動して人や物と積極的且つ多様な物理的インタラクションを行なうロボット装置に係り、特に、人や物と物理的インタラクションを行なう際における外界との接触状態や、力の作用状況を把握するロボット装置に関する。
【背景技術】
【０００３】
最近、脚式移動ロボットの構造やその安定歩行制御に関する研究開発が進展し、実用化への期待も高まってきている。これら脚式移動ロボットは、クローラ式ロボットに比し不安定で姿勢制御や歩行制御が難しくなるが、階段の昇降や障害物の乗り越えなど、柔軟な歩行・走行動作を実現できるという点で優れている。
【０００４】
従来のロボットは、６軸のマニピュレータに代表されるように、主な用途は、産業活動・生産活動などにおける各種の人的作業の代行や支援であった。この場合のロボットは、工場などの構造化された環境内で動作することを前提としており、人や環境との複雑な物理的インタラクションは基本的に行なわない。
【０００５】
これに対し、最近では、パートナー型、すなわち人間との「共生」あるいは「エンターティンメント」という用途が注目されてきている。この種のロボットは、環境やものだけでなく、人と接するなど、高度な物理インタラクション能力を備えていることが望まれる。
【０００６】
パートナー型ロボットは、６軸マニピュレータなどのように所定の場所に固定されるのではなく、自律的に移動してさまざまなサービスを提供する、いわゆる移動ロボットが一般的である。パートナー型ロボットの代表例はヒューマノイドである。例えば、移動手段を備え、接地・無接地間を遷移する安定な運動パターンを実時間で生成することができるロボット装置について提案がなされている（例えば、特許文献１を参照のこと）。
【０００７】
この種のパートナー型ロボットの多くは、姿勢が常に一定となるような使われ方をするとは限らない。場合によっては、ロボットは床上を転げまわったり、転倒したりというように、大きな姿勢変化が発生しうる。また、ロボットが外界と行なうインタラクションの形態は、歩行時における足先と路面の接触や、手先で道具を扱うなどさまざまである。すなわち、外界とインタラクションを行なう部位は特に限定されず、体の任意の箇所で外界と接触しうる。
【０００８】
このように人や物と積極的且つ多様な物理的インタラクションを行なうロボットにとっては、外界との接触状態や力の作用状況、すなわち任意の部位に印加される外力を把握することは極めて重要である。
【０００９】
ロボットが受ける外力を検出する方法として、該当する部位に６軸力センサなどに代表される力センサを配備し、外力を直接検出することが一般的である。
【００１０】
例えば、足首に６軸力センサを取り付け、足底に作用する力の作用点と作用力を検出する脚式移動ロボットの足底接地位置検出装置について提案がなされている（例えば、特許文献２を参照のこと）。このロボットは、足平部の上方に力センサを設け、階段を昇降する際に足底に垂直なＺ方向の力成分Ｆzと、前後のＸ軸方向の力成分Ｆxと、左右のＹ軸方向の軸回りのモーメントＭyを検出し、これらの検出力足底に作用する抗力の床反力中心の位置ｘを逐次求める。そして、力成分Ｆzの検出値から足底の着床開始時点や離床終了時点を検知し、その検知時点近傍の時間範囲で求めた位置ｘから足底の着床開始接地位置や離床終了設置位置を求めるようになっている。
【００１１】
しかしながら、この種のロボットでは、力センサにより足平の先端に印加された力しか計測することができない。また、力センサよりも先端側に力の作用点が複数ある場合であっても、それらの合力しか検出することができない。このため、それぞれの作用点の場所を正確に特定することができず、各作用点に作用する力を独立に求めることができない。
【００１２】
また、圧力に応じて抵抗値や静電容量が変化する材料からなる接触センサや、オン／オフ・スイッチをロボットの表面全体に取り付け、これらの電気的信号の変化を検出することで、外界と接触しているからだの部位を特定する方法について提案がなされている（例えば、非特許文献１〜３を参照のこと）。
【００１３】
しかしながら、この種のセンサは、６軸力センサのような一般の力センサに比較して非常に精度が粗く、基本的にオン／オフすなわち外界に接触の有無に関する情報しか取得できない。すなわち、接触部位を特定することはできても、その部位に作用する力を検出することはできない。また、そのセンサ値を基本的にそのまま用いるだけでは、接触部位に作用する力に関する情報を得ることはできず、ロボット全体のダイナミクスのコントロールに活用することはできない。
【００１４】
また、分布型触覚センサで取得できる力情報の精度を高めることで、より豊富な外界との接触状態を取得することができる（例えば、非特許文献４を参照のこと）。しかしながら、本出願時の技術水準では、摩擦力と垂直反力を精度良く取得でき、ロボットの機体全体に分布配置できるような小型のセンサは実用レベルにはない。また、今後このようなセンサを実現することができたとしても、入力されるセンサ値の情報量が膨大となるため、そのデータ処理やデータ伝送方法の実現が困難となったり、システムの価格を引き上げたりする結果を招来する可能性がある。
【００１５】
【特許文献１】特開２００４−１６７６７６号公報
【特許文献２】特開平８−３３８３４１号公報
【非特許文献１】「ロボット表面多値接触センサの開発と応用」（日本機械学会ロボティクスメカトロニクス講演会９８ 講演予稿集，１ＣＩ１−２，１９９８）
【非特許文献２】「導電性ゲルを用いたロボット用柔軟触覚センサの開発と応用」（第１６ 回日本ロボット学会学術講演会予稿集，８７３−８７４，１９９８）
【非特許文献３】「導電性ファブリックを用いた全身被覆型触覚センサスーツ」（日本ロボット学会誌Ｖｏｌ．１６，Ｎｏ．１，ｐｐ．８０−８６， １９９８）
【非特許文献４】「器用な手の皮膚と触覚」（日本ロボット学会誌Ｖｏｌ．１８，Ｎｏ．６，ｐｐ．７６７−７７１， ２０００」
【発明の開示】
【発明が解決しようとする課題】
【００１６】
本発明の目的は、可動脚などの移動手段を備え、地上を自律的に移動して人や物と積極的且つ多様な物理的インタラクションを行なうことができる、優れたロボット装置を提供することにある。
【００１７】
本発明のさらなる目的は、人や物と物理的インタラクションを行なう際における外界との接触状態や、力の作用状況を好適に把握することができる、優れたロボット装置を提供することにある。
【００１８】
本発明のさらなる目的は、人や物と物理的インタラクションを行なう際に、外界と接触する部位を特定し、該接触部位に作用する力を推定して、ロボット全体のダイナミクスを好適にコントロールすることができる、優れたロボット装置を提供することにある。
【００１９】
本発明のさらなる目的は、全身（若しくは必要な場所）に分布型触覚センサを配備し、これらのセンサ出力と身体ダイナミクスの組み合わせから、外界と接している部位に印加する外力を好適に推定することができる、優れたロボット装置を提供することにある。
【課題を解決するための手段】
【００２０】
本発明は、上記課題を参酌してなされたものであり、その第１の側面は、複数の可動関節を備えた機体からなるロボット装置に作用する外力を推定する外力推定システムであって、
前記機体の任意の箇所における外界との接触状態を検出する分布型接触状態検出手段と、
各関節アクチュエータにおける現在の位置、速度、発生力を計測するアクチュエータ現在状態計測手段と、
前記機体上の少なくとも１箇所において加速度及び角速度を計測する運動状態計測手段と、
前記ロボット装置の運動方程式を設定する運動方程式設定手段と、
前記アクチュエータ現在状態計測手段により得られる各関節アクチュエータの現在状態、及び前記運動状態計測手段により得られる前記ロボット装置の現在状態から、前記運動方程式上の既知項を算出する既知項算出手段と、
前記運動方程式上の未知項としての外力の作用状態を推定する外力推定手段と、
を具備することを特徴とする外力推定システムである。
【００２１】
但し、ここで言う「システム」とは、複数の装置（又は特定の機能を実現する機能モジュール）が論理的に集合した物のことを言い、各装置や機能モジュールが単一の筐体内にあるか否かは特に問わない（以下、同様）。
【００２２】
人や物と積極的且つ多様な物理的インタラクションを行なうロボットにとっては、外界との接触状態や力の作用状況、すなわち任意の部位に印加される外力を把握することは極めて重要である。
【００２３】
ロボットが受ける外力を検出する方法として、該当する部位に６軸力センサなどに代表される力センサを配備し、外力を直接検出することが一般的である。しかしながら、力センサを設置した部位に印加される力を直接計測することしかできない。また、力センサよりも先端側に力の作用点が複数ある場合であっても、それらの合力しか検出することができない。このため、それぞれの作用点の場所を正確に特定することができず、各作用点に作用する力を独立に求めることができない。また、高価な力センサを、ロボットの機体全体にわたって高密度で配置することは現実的でない。
【００２４】
また、圧力に応じて抵抗値や静電容量が変化する材料からなる接触センサや、オン／オフ・スイッチをロボットの表面全体に取り付け、これらの電気的信号の変化を検出することで、外界と接触しているからだの部位を特定することはできるが、基本的にオン／オフすなわち外界に接触の有無に関する情報しか取得できず、接触部位を特定することはできても、その部位に作用する力を検出することはできない。
【００２５】
これに対し、本発明に係る外力推定システムは、オン／オフ情報が取得できる接触センサが機体上の任意の場所に配置されたロボットにおいて、各部位から得られるオン／オフ情報を、ロボットの機体全体のダイナミクス、関節力状態、ロボット全体の運動状態といった他の物理量と組み合わせることで、接触センサ部に作用する力を推定することができる。
【００２６】
本発明に係る外力推定システムによれば、機体上の所定の箇所においてロボット装置の運動方程式を設定し、前記既知項算出手段は、操作空間上でダイナミクス演算により操作空間慣性逆行列及びバイアス加速度を求めることにより、運動方程式上の既知項を算出する。そして、前記外力推定手段は、操作空間慣性逆行列及びバイアス加速度の間に前記運動方程式に合致する関係が成立するように、外力の作用状態を推定することができる。
【００２７】
ここで、前記既知項算出手段において実行する前記の操作空間ダイナミクス演算は、例えば順動力学演算を用いて構成することができる。
【００２８】
また、前記外力推定手段は、加速度と外力の間で成立する線形相補性問題を形成してこの問題を解くことにより、外力の作用状態を推定することができる。
【００２９】
また、前記運動状態計測手段は、所定の部位における姿勢及び位置を求める。
【００３０】
前記外力推定手段は、前記分布型接触状態検出手段により外界との接触が検出された部位に作用する外力を推定する。このとき、前記分布型接触状態検出手段により外界との接触が検出された部位において、当該部位の外形から得られる作用方向についての外力を推定することができる。具体的には、前記分布型接触状態検出手段により外界との接触が検出された部位における垂直反力、並びに摩擦力を推定することができる。
【００３１】
また、本発明の第２の側面は、複数の可動関節を備えた機体からなるロボット装置に作用する外力を推定する外力推定システムであって、
前記機体の任意の領域における外界との接触状態を検出する分布型面接触状態検出手段と、
各関節アクチュエータにおける現在の位置、速度、発生力を計測するアクチュエータ現在状態計測手段と、
前記機体上の少なくとも１箇所において加速度及び角速度を計測する運動状態計測手段と、
前記ロボット装置の運動方程式を設定する運動方程式設定手段と、
前記アクチュエータ現在状態計測手段により得られる各関節アクチュエータの現在状態、及び前記運動状態計測手段により得られる前記ロボット装置の現在状態から、前記運動方程式上の既知項を算出する既知項算出手段と、
前記運動方程式上の未知項としての外力を推定する外力推定手段と、
を具備することを特徴とする外力推定システムである。
【００３２】
上述した第１の側面に係る外力推定システムでは、分布型接触状態検出手段により機体の任意の箇所における外界との接触状態を検出するが、第２の側面に係る外力推定システムでは、分布型面接触状態検出手段は、外界との接触状態を領域単位で検出する。このような場合、前記外力推定手段は、前記分布型面接触状態検出手段により外界と接触していることが検出された領域の外形上の点に作用する外力を推定する。さらに、前記分布型面接触状態検出手段により外界と接触していることが検出された領域の外形を構成する各頂点に作用する外力を求め、その重心位置に外力が印加されたものと推定する。
【００３３】
また、本発明の第３の側面は、複数の可動関節を備えた機体と、外力が作用する作用点を有するロボット装置に作用する外力を推定する外力推定システムであって、
各関節アクチュエータにおける現在の位置、速度、発生力を計測するアクチュエータ現在状態計測手段と、
前記機体上の少なくとも１箇所において加速度及び角速度を計測する運動状態計測手段と、
前記作用点における前記ロボット装置の運動方程式を設定する運動方程式設定手段と、
前記アクチュエータ現在状態計測手段により得られる各関節アクチュエータの現在状態、及び前記運動状態計測手段により得られる前記ロボット装置の現在状態から、前記運動方程式上の既知項を算出する既知項算出手段と、
前記運動方程式上の未知項としての作用点における外力を推定する外力推定手段と、
を具備することを特徴とする外力推定システムである。
【００３４】
本発明の第３の側面に係る外力推定システムでは、外力推定の対象となるロボット装置において、外力が作用する作用点があらかじめ設定されているので、分布型の接触状態を検出する手段は必要ない。このような場合、前記既知項算出手段は、外力作用点における操作空間慣性逆行列及びバイアス加速度をダイナミクス演算により求めることにより、運動方程式上の既知項を算出する。そして、前記外力推定手段は、操作空間慣性逆行列及びバイアス加速度の間に前記運動方程式に合致する関係が成立するように、所定の外力作用点に印加される外力を推定することができる。
【００３５】
また、本発明の第４の側面は、記述されたコンピュータ・プログラムであって、前記コンピュータ・システムに対し、
前記機体の任意の箇所における外界との接触状態を検出する分布型接触状態検出手順と、
各関節アクチュエータにおける現在の位置、速度、発生力を計測するアクチュエータ現在状態計測手順と、
前記機体上の少なくとも１箇所において加速度及び角速度を計測する運動状態計測手順と、
前記ロボット装置の運動方程式を設定する運動方程式設定手順と、
前記アクチュエータ現在状態計測手順により得られる各関節アクチュエータの現在状態、及び前記運動状態計測手順により得られる前記ロボット装置の現在状態から、前記運動方程式上の既知項を算出する既知項算出手順と、
前記運動方程式上の未知項としての外力の作用状態を推定する外力推定手順と、
を実行させることを特徴とするコンピュータ・プログラムである。
【００３６】
また、本発明の第５の側面は、複数の可動関節を備えた機体からなるロボット装置に作用する外力を推定するための処理をコンピュータ・システム上で実行するようにコンピュータ可読形式で記述されたコンピュータ・プログラムであって、前記コンピュータ・システムに対し、
前記機体の任意の領域における外界との接触状態を検出する分布型面接触状態検出手順と、
各関節アクチュエータにおける現在の位置、速度、発生力を計測するアクチュエータ現在状態計測手順と、
前記機体上の少なくとも１箇所において加速度及び角速度を計測する運動状態計測手順と、
前記ロボット装置の運動方程式を設定する運動方程式設定手順と、
前記アクチュエータ現在状態計測手順により得られる各関節アクチュエータの現在状態、及び前記運動状態計測手順により得られる前記ロボット装置の現在状態から、前記運動方程式上の既知項を算出する既知項算出手順と、
前記運動方程式上の未知項としての外力を推定する外力推定手順と、
を実行させることを特徴とするコンピュータ・プログラムである。
【００３７】
また、本発明の第６の側面は、複数の可動関節を備えた機体と、外力が作用する作用点を有するロボット装置に作用する外力を推定するための処理をコンピュータ・システム上で実行するようにコンピュータ可読形式で述されたコンピュータ・プログラムであって、前記コンピュータ・システムに対し、
各関節アクチュエータにおける現在の位置、速度、発生力を計測するアクチュエータ現在状態計測ステップと、
前記機体上の少なくとも１箇所において加速度及び角速度を計測する運動状態計測ステップと、
前記作用点における前記ロボット装置の運動方程式を設定する運動方程式設定ステップと、
前記アクチュエータ現在状態計測ステップにより得られる各関節アクチュエータの現在状態、及び前記運動状態計測ステップにより得られる前記ロボット装置の現在状態から、前記運動方程式上の既知項を算出する既知項算出ステップと、
前記運動方程式上の未知項としての作用点における外力を推定する外力推定ステップと、
を実行させることを特徴とするコンピュータ・プログラムである。
【００３８】
本発明の第４乃至第６の各側面に係るコンピュータ・プログラムは、コンピュータ・システム上で所定の処理を実現するようにコンピュータ可読形式で記述されたコンピュータ・プログラムを定義したものである。換言すれば、本発明の第４乃至第６の各側面に係るコンピュータ・プログラムをコンピュータ・システムにインストールすることによって、コンピュータ・システム上では協働的作用が発揮され、本発明の第１乃至第３の各側面に係る外力推定システムと同様の作用効果を得ることができる。
【発明の効果】
【００３９】
本発明によれば、人や物と物理的インタラクションを行なう際の外界と接触する部位を特定し、該接触部位に作用する力を推定して、ロボット全体のダイナミクスを好適にコントロールすることができる、優れたロボット装置を提供することができる。
【００４０】
本発明によれば、本出願時において入手可能なオン／オフ型の分布型触覚センサと、ロボット全体のダイナミクス演算とを組み合わせて、各触覚センサに作用する力を推定することにより、ロボットが外界と多様な物理インタラクションを行なった際にも、体の任意箇所における接触状態と作用力をともに計測することが可能な外力推定方法を提供することができる。
【００４１】
また、本発明に係る外力推定方法を、加速度センサやジャイロといった慣性センサと組み合わせることにより、ロボット全体が移動し、その姿勢が一定に保たれないような状況に対しても。適用可能とすることができる。
【００４２】
従来、オン／オフ情報を検出する接触センサしか設置されない系では、そのセンサ出力値を何ら加工せず、オン／オフ情報としてのみ利用していた。これに対し、本発明によれば、各部位から得られるオン／オフ情報を、ロボットの機体全体のダイナミクス、関節力状態、ロボット全体の運動状態といった他の物理量と組み合わせることで、接触センサ部に作用する力を推定することができる。
【００４３】
本発明によれば、オン／オフ情報しか取得できない接触センサが配置された機械装置に適用することで、その他の運動状態で、オン状態を検出したセンサの部位に作用する力を推定することができる。すなわち、簡便なシステム構成により多くの力情報を推定することができ、その結果として、推定できる力の情報量に対し安価なシステムを実現することができる。
【００４４】
多くの力センサは、希歪体などの脆弱な構造を有する。これに対し、本発明に係る外力推定方法は、オン／オフ情報のみを検出する接触センサと、姿勢センサと、アクチュエータ内部のエンコーダ並びに電流センサなどのセンサ出力に基づいて力を推定するので、全体として脆弱な構造が少なく、ロバストである。したがって、再キャリブレーションなどの手間も発生し難い。
【００４５】
本発明のさらに他の目的、特徴や利点は、後述する本発明の実施形態や添付する図面に基づくより詳細な説明によって明らかになるであろう。
【発明を実施するための最良の形態】
【００４６】
以下、図面を参照しながら本発明の実施形態について詳解する。
【００４７】
Ａ．ロボットの構成
図１には、本発明の一実施形態に係るロボット装置の外観構成を示している。図示のロボット装置は、いわゆる人間型ロボットであり、骨盤部には、移動手段としての２肢の脚体と、腰関節を介して上体が接続されている。上体には、２肢の腕部と、首関節を介して頭部が接続されている。
【００４８】
左右の脚体は、それぞれ股関節３自由度と、膝関節１自由度と、足首関節２自由度の、計６自由度を備えている。また、左右の腕部は、それぞれ肩関節３自由度と、肘関節１自由度と、手首関節２自由度の、計６自由度を備えている。首関節及び腰関節は、ともにＸ、Ｙ、Ｚ 軸回りに３自由度を有するとする。
【００４９】
各関節軸を駆動するアクチュエータは、例えばＤＣブラシレス・モータと減速機、並びに減速機の出力軸の回転位置を検出する位置センサで構成される。これら関節駆動アクチュエータは、ロボット装置全体の動作を統括的にコントロールするホスト・コンピュータと接続され、その位置制御目標値がホスト・コンピュータから与えられるとともに、現在の出力トルクや、関節角度、関節角速度をホスト・コンピュータに送信することができるものとする。
【００５０】
図１に示すロボット装置の表面のうち、外界との接触が想定されている各部位には、触覚センサ群ｔ１、ｔ２、…、ｔ１７が取り付けられている。図２には、１つの触覚センサ群の構成を示している。同図に示すように、１つの触覚センサ群ｔは、接触状態をそれぞれ独立して検出することができる複数の触覚センサＣＳをアレイ状に配置して構成され、触覚センサ群ｔとしてはいずれの触覚センサＣＳが接触状態にあるかによって、詳細な接触位置を特定することができる。
【００５１】
図３には、触覚センサＣＳの構成を模式的に示している。触覚センサＣＳは、２枚の極板Ｐ１及びＰ２で空隙Ｓを挟んだ構造をしており、一方の極板Ｐ１には電位Ｖccが印加され、他方の極板Ｐ２は接地されている。そして、一方の極板Ｐ１をパラレル・インターフェース（ＰＩＯ）経由でマイクロ・コンピュータに入力し、極板間の接触状態すなわち触覚センサＣＳに外力が作用しているか否かを判定することができる。
【００５２】
触覚センサ群ｔ毎に、その近隣に１つのマイクロ・コンピュータを配置して、触覚センサ群を構成するすべての触覚センサＣＳの検出信号を入力し、これらのオン／オフ状態を収集して、当該部位における外界との接触の有無並びに接触位置をホスト・コンピュータに送信するものとする。
【００５３】
また、図１に示すように、ロボット装置の骨盤部には、３軸の加速度センサａ１と３軸の角速度センサ（ジャイロ）ｇ１が搭載されている。その近隣には、これらのセンサ値を計測するマイクロ・コンピュータが配備され、その計測結果をホスト・コンピュータに送信するものとする。
【００５４】
図４には、図１に示したロボット装置における結線トポロジの構成例を示している。
【００５５】
ロボット装置は、胴体部に、３軸の腰関節アクチュエータａ１、ａ２、ａ３、及び３軸の首関節アクチュエータａ１６、ａ１７、ａ１８を持ち、これらはホスト・コンピュータに対しシリアル接続されている。各関節アクチュエータは、シリアル・ケーブルを通じて、その位置制御目標値を受け取るとともに、現在の出力トルクや、関節角度、関節角速度をホスト・コンピュータに送信する。
【００５６】
また、ロボット装置は、左腕部に、３軸の肩関節アクチュエータａ４、ａ５、ａ６、１軸の肘関節アクチュエータａ７、及び２軸の手首関節アクチュエータａ８、ａ９を持ち、これらはホスト・コンピュータに対しシリアル接続されている。同様に、右腕部には、３軸の肩関節アクチュエータａ１０、ａ１１、ａ１２、１軸の肘関節アクチュエータａ１３、及び２軸の手首関節アクチュエータａ１４、ａ１５を持ち、これらはホスト・コンピュータに対しシリアル接続されている。
【００５７】
また、ロボット装置は、左脚部に、３軸の股関節アクチュエータａ１９、ａ２０、ａ２１、１軸の膝関節アクチュエータａ２２、及び２軸の足首関節アクチュエータａ２３、ａ２４を持ち、これらはホスト・コンピュータに対しシリアル接続されている。同様に、右脚部には、３軸の肩関節アクチュエータａ２５、ａ２６、ａ２７、１軸の肘関節アクチュエータａ２８、及び２軸の足首関節アクチュエータａ２９、ａ３０を持ち、これらはホスト・コンピュータに対しシリアル接続されている。
【００５８】
各関節で使用されるアクチュエータａ１〜ａ３０は、例えばＤＣブラシレス・モータと減速機、並びに減速機の出力軸の回転位置を検出する位置センサで構成され、外部から与えられた位置制御目標値に従って回転駆動するとともに、現在の出力トルクや、関節角度、関節角速度を出力する。この種の関節アクチュエータについては、例えば本出願人に既に譲渡されている特開２００４−１８１６１３号公報に記載されている。
【００５９】
ロボット装置の右脚部には、右足部触覚センサ群ｔ１、右脛部触覚センサ群ｔ２、右大腿部触覚センサ群ｔ３が配設され、これらはホスト・コンピュータに対しシリアル接続されている。各々の触覚センサ群ｔ１〜ｔ３にはマイクロ・コンピュータが装備されており（前述）、それぞれ触覚センサ群における触覚センサＣＳのオン／オフ状態を収集して、シリアル・ケーブルを通じてホスト・コンピュータに送信する。同様に、左脚部には、左足部触覚センサ群ｔ９、左脛部触覚センサ群ｔ１０、左大腿部触覚センサ群ｔ１１が配設され、それぞれ触覚センサ群における触覚センサＣＳのオン／オフ状態を内部のマイクロ・コンピュータで収集して、シリアル・ケーブルを通じてホスト・コンピュータに送信する。
【００６０】
また、ロボット装置の右腕部には、右手首触覚センサ群ｔ４、右前腕部触覚センサ群ｔ５、右上腕部触覚センサ群ｔ６が配設され、それぞれ触覚センサ群における触覚センサＣＳのオン／オフ状態を内部のマイクロ・コンピュータで収集して、シリアル・ケーブルを通じてホスト・コンピュータに送信する。同様に、左腕部には、左手首触覚センサ群ｔ１２、左前腕部触覚センサ群ｔ１３、左上腕部触覚センサ群ｔ１４が配設され、それぞれ触覚センサ群における触覚センサＣＳのオン／オフ状態を内部のマイクロ・コンピュータで収集して、シリアル・ケーブルを通じてホスト・コンピュータに送信する。
【００６１】
また、ロボット装置の胴体部の左右にはそれぞれ胴部触覚センサ群ｔ７及びｔ１５が取り付けられており、それぞれ触覚センサ群における触覚センサＣＳのオン／オフ状態を内部のマイクロ・コンピュータで収集して、シリアル・ケーブルを通じてホスト・コンピュータに送信する。
【００６２】
また、ロボット装置の頭部の左右にはそれぞれ頭部触覚センサ群ｔ８及びｔ１６が取り付けられており、それぞれ触覚センサ群における触覚センサＣＳのオン／オフ状態を内部のマイクロ・コンピュータで収集して、シリアル・ケーブルを通じてホスト・コンピュータに送信する。
【００６３】
Ｂ．外力の推定方法
本発明に係るロボット装置では、オン／オフ型の分布型触覚センサとロボット全体のダイナミクス演算とを組み合わせて外力を推定することができる。すなわち、各部位から得られるオン／オフ情報を、ロボットの機体全体のダイナミクス、関節力状態、ロボット全体の運動状態といった他の物理量と組み合わせることで、接触センサ部に作用する力を推定する。したがって、ロボット装置は、外界と多様な物理インタラクションを行なった際にも、体の任意箇所における接触状態と作用力をともに計測することが可能である。
【００６４】
以下では、外界からの力の作用点を検出することができても、その力の大きさを方向が検出することができないシステムにおいて、すべての力の作用点における外力の大きさとその方向を推定する方法について考察する。
【００６５】
図５には、図１に示したロボット装置の関節自由度モデルを示している。同図から分かるように、２足歩行の移動ロボットを、骨盤Ｂを基底とする開リンク木構造として表現することができる。
【００６６】
移動ロボットの場合、ロボットがワールド空間中を自由に移動し、姿勢を変化させることができる。したがって、ロボット全体の状態を表すための状態変数として、骨盤Ｂのオイラー角α＝（α，β，γ）Tとその位置ｐ0（ｐ0x，ｐ0y，ｐ0z）Tを導入する。すると、ロボットはリンク構造物として扱うことができるので、ロボットの運動方程式は、下式（１）のような形で表現することができる。
【００６７】
【数１】
【００６８】
ここで、ｑはロボット全体の一般化変数であり、ロボットの運動状態としてのベースの姿勢α及び位置ｐ0を並べたベクトルと、アクチュエータの現在状態としての全関節値を並べてできるベクトルθで構成される。すなわち、ｑは下式（２）のような形で表現される。
【００６９】
【数２】
【００７０】
また、上式（１）中で、τは、一般化変数ｑに対応した一般化力であり、αとｐ0に対応する成分は（力を発生するアクチュエータが存在しないため）０となり、θに対応する成分は各関節部を駆動するアクチュエータの発生力に等しい。Ｈは、ロボット全体の慣性行列、ｂは重力やコリオリ力などを表す。ｆは外力である。Ｊは、外力作用方向に発生する速度／ｄｏｔ ｘ（Ｌａｔｅｘの書式に基づく）と、一般化速度／ｄｏｔ ｑ（Ｌａｔｅｘの書式に基づく）とを対応付けるヤコビアンであり、下式（３）の通りとなる。
【００７１】
【数３】
【００７２】
上式（１）を変形すると、下式（４）が得られる。
【００７３】
【数４】
【００７４】
ここで、Λ-1は、外力作用方向に関する操作空間慣性逆行列であり、下式（５）で表される。
【００７５】
【数５】
【００７６】
上式（４）において、右辺第１項、同第２項、及び操作空間慣性逆行列Λ-1は、後述する方法によって、現在の運動状態から見積もることができる。しかし、外力作用方向に発生する加速度と外力ｆは自由な組み合わせが許される訳ではない。垂直反力は多くの場合、ロボットと環境を乖離させる方向にのみ作用し、逆向きには作用しない。摩擦力はこの垂直反力によって決定される摩擦円錐以上の力を作用することはできない。すなわち、以下の式（６）〜（８）に示すような線形相補性問題を形成する。
【００７７】
【数６】
【００７８】
【数７】
【００７９】
【数８】
【００８０】
ここで、上式（６）中のｃはバイアス加速度、すなわち外力が一切作用しない場合に作用点に発生する加速度であり、下式で表される。
【００８１】
【数９】
【００８２】
また、上式（７）及び（８）中でそれぞれ存在するｆNi及びｆFiは、作用点ｉにおける法線方向の外力及び摩擦力を表す。μiは当該作用点における摩擦係数である。
【００８３】
本発明によれば、このような線形相補性問題を解くことによってすべての外力ｆを推定することができる、という点を十分理解されたい。なお、線形相補性問題の数学的な解法自体については、例えば“Ｆａｓｔ Ｃｏｎｔａｃｔ Ｆｏｒｃｅ Ｃｏｍｐｕｔａｔｉｏｎ ｆｏｒ Ｎｏｎｐｅｎｅｔｒａｔｉｎｇ Ｒｉｇｉｄ Ｂｏｄｉｅｓ”（ＳＩＧＧＲＡＰＨ９４，ｐｐ．２３−３４，１９９４）に記載されているので、ここでは説明を省略する。以下では、ロボットの運動状態から操作空間慣性逆行列Λ-1並びにバイアス加速度ｃを推定する方法について述べる。
【００８４】
操作空間慣性逆行列Λ-1の算出方法：
上式（６）において、右辺から左辺を得ることを考えると、外力やアクチュエータ発生力、重力、速度積に関連する力（コリオリ力など）が作用したときの、外力作用点における外力作用方向の加速度を求める問題として扱うことができる。力から加速度を得る問題は、一般に、順動力学演算として知られており、下式のように表現される。
【００８５】
【数１０】
【００８６】
最も基本的な順動力学演算については、例えば、“Ｅｆｆｉｃｉｅｎｔ Ｄｙｎａｍｉｃ Ｃｏｍｐｕｔｅｒ Ｓｉｍｕｌａｔｉｏｎ ｏｆ Ｒｏｂｏｔｉｃ Ｍｅｃｈａｎｉｓｍｓ”（Ｊｏｕｒｎａｌ ｏｆ Ｄｙｎａｍｉｃ Ｓｙｓｔｅｍｓ，Ｍｅａｓｕｒｅｍｅｎｔ ａｎｄ Ｃｏｎｔｒｏｌ，ｐｐ．２０５−２１１，１９８２）を参照されたい。
【００８７】
ここで、下式のようにおくと、重力、関節力、速度積に関連する力（コリオリ力など）が発生しない状況において発生する加速度を求めることができる。すなわち、ｃ＝０とすることができる。
【００８８】
【数１１】
【００８９】
ｆ＝ｅi、すなわち、ｉ番目の単位外力のみ作用させる条件下で計算を実行すると、Λ-1の第ｉ列を求めることができることが分かる。よって、以下の演算をすべてのｉについて実行することよってΛ-1を得ることができる。
【００９０】
【数１２】
【００９１】
バイアス加速度ｃの算出方法：
逆に、順動力学演算を外力ｆ＝０として実行することで、バイアス加速度ｃを抽出することができる。すなわち、
【００９２】
【数１３】
【００９３】
上記の各式（９）及び（１０）から分かるように、操作空間慣性逆行列Λ-1及びバイアス加速度ｃを算出する際に必要となる情報として、一般化変数ｑ及びその一般化速度／ｄｏｔ ｑ（Ｌａｔｅｘの書式に基づく）、一般化力τ、重力加速度ｇがある。以下では、さらにこれらの算出方法について説明する。
【００９４】
一般化変数ｑの算出方法：
一般化変数ｑの要素のうち、ロボットの実関節に対応する要素については、ロボットの関節角そのものであり、エンコーダによって計測した結果をそのまま用いることができる。
【００９５】
ロボット全体の姿勢及び位置に対応した成分、すなわち、骨盤Ｂのオイラー角αとその位置ｐ0については、基本的に慣性センサ（図１中のａ１、ｇ１）によって見積もることができる。しかし、そのすべての成分を決定する必要はない。順動力学の演算結果を積分して位置の履歴を形成する力学シミュレーションへの応用と異なり、本実施形態では、重力方向に対する骨盤Ｂの姿勢が分かれば十分である。但し、この点についての詳細は順動力学の詳細な演算内容に関わり煩雑になるので、本明細書ではこれ以上説明しない。
【００９６】
このような重力ベクトルに対する機体姿勢を、そこに搭載された加速度センサとジャイロの出力から求める方法は、ストラップダウン方式と呼ばれる周知の手法であり、この結果から骨盤のオイラー角αを得ることができる。ストラップダウン方式による姿勢検出方法については、例えば特開平６−２０１８６３号公報に記載されている。一方、骨盤の位置ｐ0は特に重要でないことから、ここではｐ0＝０としておく。
【００９７】
一般化速度の算出方法：
一般化速度／ｄｏｔ ｑ（Ｌａｔｅｘの書式に基づく）の要素のうち、ロボットの実関節に対応する要素については、ロボットの関節角速度に等しいので、エンコーダの数値微分やタコメータによって計測した結果をそのまま用いることができる。
【００９８】
ロボット全体の姿勢及び位置に対応した成分、すなわち、骨盤Ｂのオイラー角αの角速度／ｄｏｔ α（Ｌａｔｅｘの書式に基づく）及びその位置ｐ0の速度／ｄｏｔ ｐ0（Ｌａｔｅｘの書式に基づく）については、基本的には慣性センサによって見積もることができる。順動力学演算は、各リンク座標系におけるローカル表現に変換することが可能であり、その場合、これらの物理量は、骨盤Ｂに固定された座標系から見た骨盤Ｂに配設したセンサ（図５を参照のこと）から得られる角速度及び並進速度によって代替することができる。但し、この点についての詳細は順動力学の詳細な演算内容に関わり煩雑になるので、本明細書では説明を省略する。
【００９９】
つまり、骨盤Ｂのオイラー角αの角速度値／ｄｏｔ α（Ｌａｔｅｘの書式に基づく）としてジャイロの出力を用いることができ、その位置ｐ0の速度値／ｄｏｔ ｐ0（Ｌａｔｅｘの書式に基づく）として加速度センサから重力加速度を除去した成分の積分値を用いることができる。加速度センサから重力加速度を除去するには、以下の演算を行なえばよい。
【０１００】
【数１４】
【０１０１】
ここで、ａmは加速度センサの出力であり、ＲBは前述のストラップダウン方式で得られた骨盤Ｂの姿勢行列、ａは骨盤Ｂに固定された座標系から見た並進加速度である。原理的には、ａを数値積分すれば骨盤Ｂの位置ｐ0における速度／ｄｏｔ ｐ0（Ｌａｔｅｘの書式に基づく）を得ることができるが、実際にはドリフトの問題があるため、適切なハイパス・フィルタに積分値を透過させる結果を用いる必要がある。
【０１０２】
一般化力τの算出方法：
一般化力τの成分のうち、ロボットの実関節に対応する要素については、ロボットのアクチュエータが発するトルクそのものであるから、アクチュエータ内のトルクセンサや電流センサによって計測した結果を用いることができる。
【０１０３】
ロボット全体の姿勢及び位置に対応した成分については、アクチュエータが存在しないことから、０となる。なお、アクチュエータ内の慣性や摩擦は順動力学演算に含まれ、考慮されるが、高い精度でその値を同定することは難しく、定式化されない要素も少なくない。その値はアクチュエータの減速比とともに大きくなることから、アクチュエータの減速比は極力低くすることが望ましい。総じて、本発明を好適に実現するには、バックドライバビリティの高いアクチュエータを用いることが望ましい。
【０１０４】
重力加速度ｇの算出について：
重力加速度に特別な計算は必要ない。順動力学演算では、演算の基底が上向きに重力加速度ｇで加速しているとモデル化することで重力を考慮する。この値を外部から変更可能とすれば、順動力学演算において重力加速度を変数として扱うことができる。
【０１０５】
外力の作用方向の定義：
以上の説明で、一般化変数ｑ及びその一般化速度／ｄｏｔ ｑ（Ｌａｔｅｘの書式に基づく）、一般化力τ、重力加速度ｇの算出方法が明らかとなった。さらに、上式（６）を解くには、外力の作用点とその方向を定める必要がある。以下では、外力の作用方向の設定方法について説明する。
【０１０６】
図６には、接触センサＣＳが設置されている場所に設定された座標系Ｃcsを示している。座標系は、この点におけるロボット形状の法線方向にｚ軸を、これと垂直な方向にｘ軸及びｙ軸を持つものとする。ロボットの形状は個々のリンクレベルでは変形しないものとすると、各接触センサの設置点におけるｘ軸、ｙ軸及びｘ軸の方向は不変であり、あらかじめ算出して固定テーブルとして保持しておくことができる。
【０１０７】
外力の作用点は、出力がオンとなった接触センサＣＳの設置位置、すなわち座標系Ｃcsの位置となる。接触センサＣＳにおける力の作用方向は、これらの座標軸と反対方向に設定する。すなわち、垂直反力ｆNの方向は−ｚ軸方向、摩擦力ｆFx及びｆFyの方向は−ｘ軸及び−ｙ軸方向にそれぞれ設定される。順動力学演算がローカル座標系表現で構成されているとすると、外力の作用点及び作用方向についても、外力が作用するリンクに固定された座標系から見た、ローカル座標系表現で用意すればよい。リンクが変形しないことを前提とすれば、いずれも固定値となる。また、この点における摩擦係数についても、このリンクの素材に基づいて予め決定しておくことができ、定数となる。
【０１０８】
垂直反力と摩擦力の作用方向は、実際には外界の形状にも依存する。したがって、外力推定の精度をより高めたい場合には、各リンクの外形を微分可能な曲線で構成するなどの対策をとってもよい。摩擦係数も厳密には接触対象の素材にも依存するが、ここでは近似的にリンクの素材のみで決定するように構成した。相手の素材も既知であれば、両者の平均を取るなどの処理を行なってもよい。
【０１０９】
以上の結果、すべての接触センサにおける垂直反力ｆNと摩擦力ｆFx及びｆFyを連結して並べたベクトルとして、下式（１２）に示す外力ベクトル全体が定義される。
【０１１０】
【数１５】
【０１１１】
上式（１２）中で、Ｍは接触センサの総数である。上述した方法により、上式（６）で示した線形相補性問題式を解くことにより、これらの外力の値がすべて決定することができる。
【０１１２】
図７には、これまで説明してきた外力推定方法を実現するためのロボット装置の機能構成をブロック図の形式で示している。同図に示すように、本発明に係る外力推定方法を搭載したロボット装置は、接触検出部１、アクチュエータ部２、姿勢検出部３、外力推定部４という４つの機能部ロックに大別される。
【０１１３】
接触検出部１は、上述した触覚センサ群（図２を参照のこと）と、各触覚センサが取り付けられている位置データ、接触点における外形の法線ベクトル、接線ベクトル、摩擦係数に関するデータを有する。位置データ、法線ベクトル、接線ベクトルに関しては、触覚センサが取り付けられているリンクに固定されたローカル座標系から見た表現で保持する。接触検出部１は、接触センサ群１−１を構成するすべての接触センサのオン／オフ状態を計測し、要求に応じて接触部位の位置データ、法線ベクトル、接線ベクトル、摩擦係数のリストを返答する。
【０１１４】
アクチュエータ部２は、すべての位置制御型アクチュエータの制御と、その状態検出を行なう。すなわち、角度目標値を実現するようにアクチュエータのトルク出力を調整する位置制御部２−１と、アクチュエータの現在角度を計測する位置検出部２−２、アクチュエータの現在角速度を計測する速度検出部２−３、アクチュエータの出力トルクを計測するトルク検出部２−４を備えている。アクチュエータ部２は、要求に応じてアクチュエータの目標角度を受理したり、現在の角度、角速度、トルク状態を返答したりする。
【０１１５】
姿勢検出部３は、骨盤部に取り付けられたジャイロ３−１と加速度センサ３−２（図４を参照のこと）の計測を行ない、要求に応じてそれらの値を返答する。
【０１１６】
外力推定部４は、接触検出部１、アクチュエータ部２、及び姿勢検出部３からの返答結果を基に、各接触センサ部に発生する外力を推定する。以下、その内部構成について説明する。
【０１１７】
接触部位決定部４−１は、接触検出部１に問い合わせ、現在接触状態すなわちオン状態にある接触センサＣＳのリストとその位置リストを作成する。
【０１１８】
外力方向設定部４−２は、接触部位決定部４−１でオン状態として検出された接触センサにおける、法線情報と接線情報を接触検出部１に問い合わせ、垂直反力と摩擦力（ｘ及びｙの２方向）の方向を決定する。方向はそれぞれ法線ベクトルと接線ベクトルの逆向きとなる。
【０１１９】
摩擦係数設定部４−３では、外力の作用点における摩擦係数を設定する。摩擦係数は、リンクの素材に基づいて予め決定しておくことができ、定数となる。摩擦係数は、厳密には接触対象の素材にも依存するが、ここでは近似的にリンクの素材のみで決定するように構成した。相手の素材も既知であれば、両者の平均を取るなどの処理を行なってもよい。
【０１２０】
骨盤姿勢算出部４−４は、姿勢検出部で計測された骨盤の角速度と加速度情報を基に、ストラップダウン方式に基づく姿勢演算を実行して、骨盤部の姿勢情報を算出する。
【０１２１】
一般化変数決定部４−５は、骨盤姿勢算出部４−４が作成した骨盤部の姿勢情報と、アクチュエータ部から得られるすべてのアクチュエータの角度情報を基に、一般化変数ベクトルｑを作成する。
【０１２２】
一般化速度決定部４−６は、姿勢検出部３で計測された骨盤の角速度（ジャイロ計測値）と加速度の積分値（実際には既に説明したようにハイパス・フィルタに透過させるのが望ましい）、及びアクチュエータ部２から得られるすべてのアクチュエータの角速度を基に、一般化変数ｑの速度ベクトルを作成する。
【０１２３】
一般化力決定部４−７は、アクチュエータ部２から得られるすべてのアクチュエータの出力トルクを基に、一般化力ベクトルτを作成する。
【０１２４】
順動力学演算部４−８は、下式に示す順動力学演算を実行する演算ユニットである。
【０１２５】
【数１６】
【０１２６】
引数の値を可変に与えられるように構成されている。返り値として、一般化加速度／ｄｄｏｔ ｑ（Ｌａｔｅｘの書式に基づく）ではなく、外力の作用方向に発生する加速度のベクトルｘを返すところが、一般的な順動力学演算と異なるが、内部的には一旦一般化加速度を得て、これを基に各外力方向の加速度を算出するように構成すればよい。
【０１２７】
操作空間慣性逆行列演算部４−９は、順動力学演算部４−８を用い、上式（９） の演算をすべての外力に関して実行し、操作空間慣性逆行列Λ-1を算出する。すなわち、すべての外力に関して単位外力（ノルムが１の外力）をテスト・フォースとして作用させたときの操作空間加速度（外力作用点に発生する加速度）／ｄｄｏｔ ｘ（Ｌａｔｅｘの書式に基づく）を算出するという処理を繰り返してΛ-1を得る。外力の作用位置や作用方向については、接触部位決定部４−１と外力方向決定部４−２の結果を用いる。
【０１２８】
バイアス加速度演算部４−１０は、順動力学演算部４−８を用い、上式（１０）に示した演算を実行し、バイアス加速度ｃを算出する。この際、一般化変数決定部４−５が作成した一般化変数ｑ、一般化速度決定部４−６が作成した一般化速度／ｄｏｔ ｑ（Ｌａｔｅｘの書式に基づく）、一般化力決定部４−７が作成した一般化力τを用いる。外力の作用位置や作用方向については、操作空間慣性逆行列演算部４−９と同様に、接触部位決定部４−１、外力方向決定部４−２の結果を用いる。
【０１２９】
線形相補性問題解決部４−１１は、操作空間慣性逆行列算出部４−９によって算出された操作空間慣性逆行列Λ-1、バイアス加速度算出部４−１０によって算出されたバイアス加速度ｃを線形相補性問題式（６）、式（７）、式（８）に設定し、これを解く。式（８）の摩擦係数には、摩擦係数設定部４−３で得られた結果を用いる。この線形相補性問題を解いた結果、すべての外力情報ｆが決定される。
【０１３０】
図８には、本発明に係る外力推定を行なうための処理手順をフローチャートの形式で示している。
【０１３１】
まず、すべての接触センサの状態計測を行なう（ステップＳ１）。次いで、すべてのアクチュエータの関節角度を計測する（ステップＳ２）。次いで、すべてのアクチュエータの関節角速度を計測する（ステップＳ３）。次いで、すべてのアクチュエータの関節トルクを計測する（ステップＳ４）。
【０１３２】
次いで、骨盤部Ｂに設置された加速度センサ及びジャイロを計測する（ステップＳ５）。そして、ステップＳ５で得られた結果に対してストラップダウン方式に基づく姿勢演算を行ない、骨盤部の姿勢情報を算出する（ステップＳ６）。
【０１３３】
次いで、ステップＳ１で得られた結果に基づいて、外界と接触を起こしている部位の位置（リンクに固定された座標系から見たローカル表現）をリストアップする（ステップＳ７）。そして、外界と接触している各点において作用する垂直反力と摩擦力の方向を決定する（ステップＳ８）。
【０１３４】
次いで、外界と接触している各点における摩擦係数をデータベースから取り出す（ステップＳ９）。
【０１３５】
そして、ステップＳ２及びステップＳ６の結果を用いて、一般化変数ｑの値を決定する（ステップＳ１０）。また、ステップＳ３及びＳ５の結果を用いて一般化速度／ｄｏｔ ｑ（Ｌａｔｅｘの書式に基づく）の値を決定する（ステップＳ１１）。そして、ステップＳ４の結果を用いて、一般化力τの値を決定する（ステップＳ１２）。
【０１３６】
次いで、順動力学演算を用いて、すべての外力に関して式（９）を実行して、操作空間慣性逆行列Λ-1を算出する（ステップＳ１３）。また、順動力学演算を用いて式（１０）を実行して、バイアス加速度ｃを算出する（ステップＳ１４）。
【０１３７】
次いで、ステップＳ１３で得られた操作空間慣性逆行列Λ-1、並びにステップＳ１４で得られたバイアス加速度ｃ、並びにステップＳ９で得られた摩擦係数を線形相補性問題（式（６）から式（８））に設定する（ステップＳ１５）。
【０１３８】
そして、ステップＳ１５で設定された線形相補性問題を解いて、すべての外力（垂直反力と摩擦力）を求める。
【０１３９】
以上の処理を一定周期（例えば制御周期）で実行する（ステップＳ１６）。操作空間慣性逆行列Λ-1の算出など、計算時間を要する部分に関しては、選択的に低い更新レートで実行するように構成してもよい（操作空間慣性逆行列は時間的に急激に変化しない特性を有しているため、特に有効である）。
【０１４０】
Ｃ．変形例
上述した説明では、接触検出部１は、図２に示したように複数の接触センサをアレイ状に配設して構成される触覚センサ群をロボットの全身に配設して構成されており、外界と接触している部位を特定することができることを前提としている。その変形例として、外界と接触している部位ではなく、外界と接触している領域を検出する接触検出部を用いることによっても、同様に本発明に係る外力推定を実現することができる。
【０１４１】
ここでは、凸領域のいずれかの場所が外界と接触していることを検出できる面接触センサＲＣＳ（図９を参照のこと）がロボット表面に分散して配置されている場合について考察する。
【０１４２】
このような場合、面接触センサＲＣＳの外形上に外力作用点を設定して、これらの点に上述した点接触センサがあるとみなして、同様の方法で外力を推定することができる。図９に示した例では、５つの点の点接触センサに分割して扱う様子を示している。
【０１４３】
線形相補性問題を解くと、これらすべての点において印加される外力の値が決定される。ここから面接触センサ領域内の１点の作用点ｘaを得るには、面接触センサが平面とみなすことができる場合に、下式に示すように垂直反力の重心位置を求めればよい。
【０１４４】
【数１７】
【０１４５】
上式において、ｆNiは第ｉ頂点の垂直反力、ｘiは第ｉ頂点の位置を表す。ｘaへの作用力はすべての頂点ｘiにおける垂直反力と摩擦力の合力となる。
【０１４６】
Ｄ．変形例２
上述した説明では、接触検出部１により外界と接触している部位又は領域を特定するようになっている。これに対し、接触部位が決まっている、あるいはリンク原点など決められた位置に作用する外力を推定したい場合は、必ずしも接触センサを配置する必要はない。本発明によれば、いずれの方法であれ外力の作用位置を指定しさえすれば、そこに印加される外力を推定することができる。すなわち、接触センサを配置して外力の作用点を検出するのではなく、あらかじめ外力作用位置を指定し、そこに印加される外力を推定することができる。
【０１４７】
また、本発明に係る外力推定方法は、外力として、「力」だけでなく「モーメント」を求めるように構成することも可能である。具体的には、以下に示す順動力学演算において、力の代わりにモーメントも作用させられるように構成する。また、これに対応する加速度の成分については、並進加速度の代わりに角加速度を算出するように構成すればよい。
【０１４８】
【数１８】
【０１４９】
このようにすれば、上式（９）において、モーメントに該当する外力ｆの成分に関しては、単位外力の代わりに単位外モーメントを作用させるなど、上述した外力を推定するのと基本的に同一の手順に従ってモーメントを推定することができる。
【０１５０】
また、線形相補性問題の拘束条件は、例に挙げた摩擦力や垂直反力に関する拘束条件限らず、線形相補性問題ソルバの構成によってより多様な拘束を扱うこともできる。
【産業上の利用可能性】
【０１５１】
以上、特定の実施形態を参照しながら、本発明について詳解してきた。しかしながら、本発明の要旨を逸脱しない範囲で当業者が該実施形態の修正や代用を成し得ることは自明である。
【０１５２】
本明細書では、本発明を２足歩行の脚式移動ロボットに適用した実施形態を中心に説明してきたが、本発明の要旨はこれに限定されるものではない。本発明に係る外力推定方法は、基本的にはロボット装置の構造に依存せず、任意の構造に対して適用することができる。具体的には、順動力学演算を構造非依存な高汎用性を有した形態で実装することができる。
【０１５３】
本発明に係る外力推定方法は、６軸力センサを用いて局所的な箇所への作用力を検出する従来の方法とは異なり、接触センサを配置した任意の箇所に作用する力を推定することができる。したがって、移動ロボットに限らず、機体の至る所で外界と接触を生じるさまざまな機械装置に適用して、その状況を把握するのに有効である。
【０１５４】
要するに、例示という形態で本発明を開示してきたのであり、本明細書の記載内容を限定的に解釈するべきではない。本発明の要旨を判断するためには、特許請求の範囲を参酌すべきである。
【図面の簡単な説明】
【０１５５】
【図１】図１は、本発明の一実施形態に係るロボット装置の外観構成を示した図である。
【図２】図２は、触覚センサ群の構成を示した図である。
【図３】図３は、触覚センサＣＳの構成を模式的に示した図である。
【図４】図４は、図１に示したロボット装置における結線トポロジの構成例を示した図である。
【図５】図５は、図１に示したロボット装置の関節自由度モデルを示した図である。
【図６】図６は、接触センサＣＳ が設置されている場所に設定された座標系Ｃcsを示した図である。
【図７】図７は、本発明に係る外力推定方法を実現するための機能構成を示したブロック図である。
【図８】図８は、ロボット装置に置ける外力推定を行なうための処理手順を示したフローチャートである。
【図９】図９は、凸領域のいずれかの場所が外界と接触していることを検出できる面接触センサＲＣＳの構成例を示した図である。
【符号の説明】
【０１５６】
１…接触検出部
２…アクチュエータ部
３…姿勢検出部
４…外力推定部
【特許請求の範囲】
【請求項１】
複数の可動関節を備えた機体からなるロボット装置に作用する外力を推定する外力推定システムであって、
前記機体の任意の箇所における外界との接触状態を検出する分布型接触状態検出手段と、
各関節アクチュエータにおける現在の位置、速度、発生力を計測するアクチュエータ現在状態計測手段と、
前記機体上の少なくとも１箇所において加速度及び角速度を計測する運動状態計測手段と、
前記ロボット装置の運動方程式を設定する運動方程式設定手段と、
前記アクチュエータ現在状態計測手段により得られる各関節アクチュエータの現在状態、及び前記運動状態計測手段により得られる前記ロボット装置の現在状態から、前記運動方程式上の既知項を算出する既知項算出手段と、
前記運動方程式上の未知項としての外力の作用状態を推定する外力推定手段と、
を具備することを特徴とする外力推定システム。
【請求項２】
前記既知項算出手段は、操作空間上でダイナミクス演算により操作空間慣性逆行列及びバイアス加速度を求め、
前記外力推定手段は、操作空間慣性逆行列及びバイアス加速度の間に前記運動方程式に合致する関係が成立するように、外力の作用状態を推定する、
ことを特徴とする請求項１に記載の外力推定システム。
【請求項３】
前記既知項算出手段は、前記の操作空間ダイナミクス演算を、順動力学演算を用いて行なう、
ことを特徴とする請求項２に記載の外力推定システム。
【請求項４】
前記外力推定手段は、加速度と外力の間で成立する線形相補性問題を形成し、該問題を解くことにより、外力の作用状態を推定する、
ことを特徴とする請求項２に記載の外力推定システム。
【請求項５】
前記運動状態計測手段は、所定の部位における姿勢及び位置を求める、
ことを特徴とする請求項２に記載の外力推定システム。
【請求項６】
前記外力推定手段は、前記分布型接触状態検出手段により外界との接触が検出された部位に作用する外力を推定する、
ことを特徴とする請求項２に記載の外力推定システム。
【請求項７】
前記外力推定手段は、前記分布型接触状態検出手段により外界との接触が検出された部位において、当該部位の外形から得られる作用方向についての外力を推定する、
ことを特徴とする請求項２に記載の外力推定システム。
【請求項８】
前記外力推定手段は、前記分布型接触状態検出手段により外界との接触が検出された部位における垂直反力を推定する、
ことを特徴とする請求項７に記載の外力推定システム。
【請求項９】
前記外力推定手段は、前記分布型接触状態検出手段により外界との接触が検出された部位における摩擦力を推定する、
ことを特徴とする請求項７に記載の外力推定システム。
【請求項１０】
複数の可動関節を備えた機体からなるロボット装置に作用する外力を推定する外力推定システムであって、
前記機体の任意の領域における外界との接触状態を検出する分布型面接触状態検出手段と、
各関節アクチュエータにおける現在の位置、速度、発生力を計測するアクチュエータ現在状態計測手段と、
前記機体上の少なくとも１箇所において加速度及び角速度を計測する運動状態計測手段と、
前記ロボット装置の運動方程式を設定する運動方程式設定手段と、
前記アクチュエータ現在状態計測手段により得られる各関節アクチュエータの現在状態、及び前記運動状態計測手段により得られる前記ロボット装置の現在状態から、前記運動方程式上の既知項を算出する既知項算出手段と、
前記運動方程式上の未知項としての外力を推定する外力推定手段と、
を具備することを特徴とする外力推定システム。
【請求項１１】
前記既知項算出手段は、操作空間上でダイナミクス演算により操作空間慣性逆行列及びバイアス加速度を求め、
前記外力推定手段は、操作空間慣性逆行列及びバイアス加速度の間に前記運動方程式に合致する関係が成立するように、外力を推定する、
ことを特徴とする請求項１０に記載の外力推定システム。
【請求項１２】
前記既知項算出手段は、前記の操作空間ダイナミクス演算を、順動力学演算を用いて行なう、
ことを特徴とする請求項１１に記載の外力推定システム。
【請求項１３】
前記外力推定手段は、加速度と外力の間で成立する線形相補性問題を形成し、該問題を解いて外力の作用状態を推定する、
ことを特徴とする請求項１１に記載の外力推定システム。
【請求項１４】
前記外力推定手段は、前記分布型面接触状態検出手段により外界と接触していることが検出された領域の外形上の点に作用する外力を推定する、
ことを特徴とする請求項１０に記載の外力推定システム。
【請求項１５】
前記外力推定手段は、前記分布型面接触状態検出手段により外界と接触していることが検出された領域の外形を構成する各頂点に作用する外力を求め、その重心位置に外力が印加されたものと推定する、
ことを特徴とする請求項１０に記載の外力推定システム。
【請求項１６】
複数の可動関節を備えた機体と、外力が作用する作用点を有するロボット装置に作用する外力を推定する外力推定システムであって、
各関節アクチュエータにおける現在の位置、速度、発生力を計測するアクチュエータ現在状態計測手段と、
前記機体上の少なくとも１箇所において加速度及び角速度を計測する運動状態計測手段と、
前記作用点における前記ロボット装置の運動方程式を設定する運動方程式設定手段と、
前記アクチュエータ現在状態計測手段により得られる各関節アクチュエータの現在状態、及び前記運動状態計測手段により得られる前記ロボット装置の現在状態から、前記運動方程式上の既知項を算出する既知項算出手段と、
前記運動方程式上の未知項としての作用点における外力を推定する外力推定手段と、
を具備することを特徴とする外力推定システム。
【請求項１７】
複数の可動関節を備えた機体からなるロボット装置に作用する外力を推定する外力推定方法であって、
前記機体の任意の箇所における外界との接触状態を検出する分布型接触状態検出ステップと、
各関節アクチュエータにおける現在の位置、速度、発生力を計測するアクチュエータ現在状態計測ステップと、
前記機体上の少なくとも１箇所において加速度及び角速度を計測する運動状態計測ステップと、
前記ロボット装置の運動方程式を設定する運動方程式設定ステップと、
前記アクチュエータ現在状態計測ステップにより得られる各関節アクチュエータの現在状態、及び前記運動状態計測ステップにより得られる前記ロボット装置の現在状態から、前記運動方程式上の既知項を算出する既知項算出ステップと、
前記運動方程式上の未知項としての外力の作用状態を推定する外力推定ステップと、
を具備することを特徴とする外力推定方法。
【請求項１８】
前記既知項算出ステップでは、操作空間上でダイナミクス演算により操作空間慣性逆行列及びバイアス加速度を求め、
前記外力推定ステップでは、操作空間慣性逆行列及びバイアス加速度の間に前記運動方程式に合致する関係が成立するように、外力の作用状態を推定する、
ことを特徴とする請求項１７に記載の外力推定方法。
【請求項１９】
前記既知項算出ステップでは、前記の操作空間ダイナミクス演算を、順動力学演算を用いて行なう、
ことを特徴とする請求項１８に記載の外力推定方法。
【請求項２０】
前記外力推定ステップでは、加速度と外力の間で成立する線形相補性問題を形成し、該問題を解くことにより、外力の作用状態を推定する、
ことを特徴とする請求項１８に記載の外力推定方法。
【請求項２１】
前記運動状態計測ステップでは、所定の部位における姿勢及び位置を求める、
ことを特徴とする請求項１８に記載の外力推定方法。
【請求項２２】
前記外力推定ステップでは、前記分布型接触状態検出ステップにより外界との接触が検出された部位に作用する外力を推定する、
ことを特徴とする請求項１８に記載の外力推定方法。
【請求項２３】
前記外力推定ステップでは、前記分布型接触状態検出ステップにより外界との接触が検出された部位において、当該部位の外形から得られる作用方向についての外力を推定する、
ことを特徴とする請求項１８に記載の外力推定方法。
【請求項２４】
前記外力推定ステップでは、前記分布型接触状態検出ステップにより外界との接触が検出された部位における垂直反力を推定する、
ことを特徴とする請求項２３に記載の外力推定方法。
【請求項２５】
前記外力推定ステップでは、前記分布型接触状態検出ステップにより外界との接触が検出された部位における摩擦力を推定する、
ことを特徴とする請求項２３に記載の外力推定方法。
【請求項２６】
複数の可動関節を備えた機体からなるロボット装置に作用する外力を推定する外力推定方法であって、
前記機体の任意の領域における外界との接触状態を検出する分布型面接触状態検出ステップと、
各関節アクチュエータにおける現在の位置、速度、発生力を計測するアクチュエータ現在状態計測ステップと、
前記機体上の少なくとも１箇所において加速度及び角速度を計測する運動状態計測ステップと、
前記ロボット装置の運動方程式を設定する運動方程式設定ステップと、
前記アクチュエータ現在状態計測ステップにより得られる各関節アクチュエータの現在状態、及び前記運動状態計測ステップにより得られる前記ロボット装置の現在状態から、前記運動方程式上の既知項を算出する既知項算出ステップと、
前記運動方程式上の未知項としての外力を推定する外力推定ステップと、
を具備することを特徴とする外力推定方法。
【請求項２７】
前記外力推定ステップでは、前記分布型面接触状態検出ステップにより外界と接触していることが検出された領域の外形上の点に作用する外力を推定する、
ことを特徴とする請求項２６に記載の外力推定方法。
【請求項２８】
前記外力推定ステップでは、前記分布型面接触状態検出ステップにより外界と接触していることが検出された領域の外形を構成する各頂点に作用する外力を求め、その重心位置に外力が印加されたものと推定する、
ことを特徴とする請求項２６に記載の外力推定方法。
【請求項２９】
複数の可動関節を備えた機体と、外力が作用する作用点を有するロボット装置に作用する外力を推定する外力推定方法であって、
各関節アクチュエータにおける現在の位置、速度、発生力を計測するアクチュエータ現在状態計測ステップと、
前記機体上の少なくとも１箇所において加速度及び角速度を計測する運動状態計測ステップと、
前記作用点における前記ロボット装置の運動方程式を設定する運動方程式設定ステップと、
前記アクチュエータ現在状態計測ステップにより得られる各関節アクチュエータの現在状態、及び前記運動状態計測ステップにより得られる前記ロボット装置の現在状態から、前記運動方程式上の既知項を算出する既知項算出ステップと、
前記運動方程式上の未知項としての作用点における外力を推定する外力推定ステップと、
を具備することを特徴とする外力推定方法。
【請求項３０】
複数の可動関節を備えた機体からなるロボット装置に作用する外力を推定するための処理をコンピュータ・システム上で実行するようにコンピュータ可読形式で記述されたコンピュータ・プログラムであって、前記コンピュータ・システムに対し、
前記機体の任意の箇所における外界との接触状態を検出する分布型接触状態検出手順と、
各関節アクチュエータにおける現在の位置、速度、発生力を計測するアクチュエータ現在状態計測手順と、
前記機体上の少なくとも１箇所において加速度及び角速度を計測する運動状態計測手順と、
前記ロボット装置の運動方程式を設定する運動方程式設定手順と、
前記アクチュエータ現在状態計測手順により得られる各関節アクチュエータの現在状態、及び前記運動状態計測手順により得られる前記ロボット装置の現在状態から、前記運動方程式上の既知項を算出する既知項算出手順と、
前記運動方程式上の未知項としての外力の作用状態を推定する外力推定手順と、
を実行させることを特徴とするコンピュータ・プログラム。
【請求項３１】
複数の可動関節を備えた機体からなるロボット装置に作用する外力を推定するための処理をコンピュータ・システム上で実行するようにコンピュータ可読形式で記述されたコンピュータ・プログラムであって、前記コンピュータ・システムに対し、
前記機体の任意の領域における外界との接触状態を検出する分布型面接触状態検出手順と、
各関節アクチュエータにおける現在の位置、速度、発生力を計測するアクチュエータ現在状態計測手順と、
前記機体上の少なくとも１箇所において加速度及び角速度を計測する運動状態計測手順と、
前記ロボット装置の運動方程式を設定する運動方程式設定手順と、
前記アクチュエータ現在状態計測手順により得られる各関節アクチュエータの現在状態、及び前記運動状態計測手順により得られる前記ロボット装置の現在状態から、前記運動方程式上の既知項を算出する既知項算出手順と、
前記運動方程式上の未知項としての外力を推定する外力推定手順と、
を実行させることを特徴とするコンピュータ・プログラム。
【請求項３２】
複数の可動関節を備えた機体と、外力が作用する作用点を有するロボット装置に作用する外力を推定するための処理をコンピュータ・システム上で実行するようにコンピュータ可読形式で述されたコンピュータ・プログラムであって、前記コンピュータ・システムに対し、
各関節アクチュエータにおける現在の位置、速度、発生力を計測するアクチュエータ現在状態計測ステップと、
前記機体上の少なくとも１箇所において加速度及び角速度を計測する運動状態計測ステップと、
前記作用点における前記ロボット装置の運動方程式を設定する運動方程式設定ステップと、
前記アクチュエータ現在状態計測ステップにより得られる各関節アクチュエータの現在状態、及び前記運動状態計測ステップにより得られる前記ロボット装置の現在状態から、前記運動方程式上の既知項を算出する既知項算出ステップと、
前記運動方程式上の未知項としての作用点における外力を推定する外力推定ステップと、
を実行させることを特徴とするコンピュータ・プログラム。
【請求項１】
複数の可動関節を備えた機体からなるロボット装置に作用する外力を推定する外力推定システムであって、
前記機体の任意の箇所における外界との接触状態を検出する分布型接触状態検出手段と、
各関節アクチュエータにおける現在の位置、速度、発生力を計測するアクチュエータ現在状態計測手段と、
前記機体上の少なくとも１箇所において加速度及び角速度を計測する運動状態計測手段と、
前記ロボット装置の運動方程式を設定する運動方程式設定手段と、
前記アクチュエータ現在状態計測手段により得られる各関節アクチュエータの現在状態、及び前記運動状態計測手段により得られる前記ロボット装置の現在状態から、前記運動方程式上の既知項を算出する既知項算出手段と、
前記運動方程式上の未知項としての外力の作用状態を推定する外力推定手段と、
を具備することを特徴とする外力推定システム。
【請求項２】
前記既知項算出手段は、操作空間上でダイナミクス演算により操作空間慣性逆行列及びバイアス加速度を求め、
前記外力推定手段は、操作空間慣性逆行列及びバイアス加速度の間に前記運動方程式に合致する関係が成立するように、外力の作用状態を推定する、
ことを特徴とする請求項１に記載の外力推定システム。
【請求項３】
前記既知項算出手段は、前記の操作空間ダイナミクス演算を、順動力学演算を用いて行なう、
ことを特徴とする請求項２に記載の外力推定システム。
【請求項４】
前記外力推定手段は、加速度と外力の間で成立する線形相補性問題を形成し、該問題を解くことにより、外力の作用状態を推定する、
ことを特徴とする請求項２に記載の外力推定システム。
【請求項５】
前記運動状態計測手段は、所定の部位における姿勢及び位置を求める、
ことを特徴とする請求項２に記載の外力推定システム。
【請求項６】
前記外力推定手段は、前記分布型接触状態検出手段により外界との接触が検出された部位に作用する外力を推定する、
ことを特徴とする請求項２に記載の外力推定システム。
【請求項７】
前記外力推定手段は、前記分布型接触状態検出手段により外界との接触が検出された部位において、当該部位の外形から得られる作用方向についての外力を推定する、
ことを特徴とする請求項２に記載の外力推定システム。
【請求項８】
前記外力推定手段は、前記分布型接触状態検出手段により外界との接触が検出された部位における垂直反力を推定する、
ことを特徴とする請求項７に記載の外力推定システム。
【請求項９】
前記外力推定手段は、前記分布型接触状態検出手段により外界との接触が検出された部位における摩擦力を推定する、
ことを特徴とする請求項７に記載の外力推定システム。
【請求項１０】
複数の可動関節を備えた機体からなるロボット装置に作用する外力を推定する外力推定システムであって、
前記機体の任意の領域における外界との接触状態を検出する分布型面接触状態検出手段と、
各関節アクチュエータにおける現在の位置、速度、発生力を計測するアクチュエータ現在状態計測手段と、
前記機体上の少なくとも１箇所において加速度及び角速度を計測する運動状態計測手段と、
前記ロボット装置の運動方程式を設定する運動方程式設定手段と、
前記アクチュエータ現在状態計測手段により得られる各関節アクチュエータの現在状態、及び前記運動状態計測手段により得られる前記ロボット装置の現在状態から、前記運動方程式上の既知項を算出する既知項算出手段と、
前記運動方程式上の未知項としての外力を推定する外力推定手段と、
を具備することを特徴とする外力推定システム。
【請求項１１】
前記既知項算出手段は、操作空間上でダイナミクス演算により操作空間慣性逆行列及びバイアス加速度を求め、
前記外力推定手段は、操作空間慣性逆行列及びバイアス加速度の間に前記運動方程式に合致する関係が成立するように、外力を推定する、
ことを特徴とする請求項１０に記載の外力推定システム。
【請求項１２】
前記既知項算出手段は、前記の操作空間ダイナミクス演算を、順動力学演算を用いて行なう、
ことを特徴とする請求項１１に記載の外力推定システム。
【請求項１３】
前記外力推定手段は、加速度と外力の間で成立する線形相補性問題を形成し、該問題を解いて外力の作用状態を推定する、
ことを特徴とする請求項１１に記載の外力推定システム。
【請求項１４】
前記外力推定手段は、前記分布型面接触状態検出手段により外界と接触していることが検出された領域の外形上の点に作用する外力を推定する、
ことを特徴とする請求項１０に記載の外力推定システム。
【請求項１５】
前記外力推定手段は、前記分布型面接触状態検出手段により外界と接触していることが検出された領域の外形を構成する各頂点に作用する外力を求め、その重心位置に外力が印加されたものと推定する、
ことを特徴とする請求項１０に記載の外力推定システム。
【請求項１６】
複数の可動関節を備えた機体と、外力が作用する作用点を有するロボット装置に作用する外力を推定する外力推定システムであって、
各関節アクチュエータにおける現在の位置、速度、発生力を計測するアクチュエータ現在状態計測手段と、
前記機体上の少なくとも１箇所において加速度及び角速度を計測する運動状態計測手段と、
前記作用点における前記ロボット装置の運動方程式を設定する運動方程式設定手段と、
前記アクチュエータ現在状態計測手段により得られる各関節アクチュエータの現在状態、及び前記運動状態計測手段により得られる前記ロボット装置の現在状態から、前記運動方程式上の既知項を算出する既知項算出手段と、
前記運動方程式上の未知項としての作用点における外力を推定する外力推定手段と、
を具備することを特徴とする外力推定システム。
【請求項１７】
複数の可動関節を備えた機体からなるロボット装置に作用する外力を推定する外力推定方法であって、
前記機体の任意の箇所における外界との接触状態を検出する分布型接触状態検出ステップと、
各関節アクチュエータにおける現在の位置、速度、発生力を計測するアクチュエータ現在状態計測ステップと、
前記機体上の少なくとも１箇所において加速度及び角速度を計測する運動状態計測ステップと、
前記ロボット装置の運動方程式を設定する運動方程式設定ステップと、
前記アクチュエータ現在状態計測ステップにより得られる各関節アクチュエータの現在状態、及び前記運動状態計測ステップにより得られる前記ロボット装置の現在状態から、前記運動方程式上の既知項を算出する既知項算出ステップと、
前記運動方程式上の未知項としての外力の作用状態を推定する外力推定ステップと、
を具備することを特徴とする外力推定方法。
【請求項１８】
前記既知項算出ステップでは、操作空間上でダイナミクス演算により操作空間慣性逆行列及びバイアス加速度を求め、
前記外力推定ステップでは、操作空間慣性逆行列及びバイアス加速度の間に前記運動方程式に合致する関係が成立するように、外力の作用状態を推定する、
ことを特徴とする請求項１７に記載の外力推定方法。
【請求項１９】
前記既知項算出ステップでは、前記の操作空間ダイナミクス演算を、順動力学演算を用いて行なう、
ことを特徴とする請求項１８に記載の外力推定方法。
【請求項２０】
前記外力推定ステップでは、加速度と外力の間で成立する線形相補性問題を形成し、該問題を解くことにより、外力の作用状態を推定する、
ことを特徴とする請求項１８に記載の外力推定方法。
【請求項２１】
前記運動状態計測ステップでは、所定の部位における姿勢及び位置を求める、
ことを特徴とする請求項１８に記載の外力推定方法。
【請求項２２】
前記外力推定ステップでは、前記分布型接触状態検出ステップにより外界との接触が検出された部位に作用する外力を推定する、
ことを特徴とする請求項１８に記載の外力推定方法。
【請求項２３】
前記外力推定ステップでは、前記分布型接触状態検出ステップにより外界との接触が検出された部位において、当該部位の外形から得られる作用方向についての外力を推定する、
ことを特徴とする請求項１８に記載の外力推定方法。
【請求項２４】
前記外力推定ステップでは、前記分布型接触状態検出ステップにより外界との接触が検出された部位における垂直反力を推定する、
ことを特徴とする請求項２３に記載の外力推定方法。
【請求項２５】
前記外力推定ステップでは、前記分布型接触状態検出ステップにより外界との接触が検出された部位における摩擦力を推定する、
ことを特徴とする請求項２３に記載の外力推定方法。
【請求項２６】
複数の可動関節を備えた機体からなるロボット装置に作用する外力を推定する外力推定方法であって、
前記機体の任意の領域における外界との接触状態を検出する分布型面接触状態検出ステップと、
各関節アクチュエータにおける現在の位置、速度、発生力を計測するアクチュエータ現在状態計測ステップと、
前記機体上の少なくとも１箇所において加速度及び角速度を計測する運動状態計測ステップと、
前記ロボット装置の運動方程式を設定する運動方程式設定ステップと、
前記アクチュエータ現在状態計測ステップにより得られる各関節アクチュエータの現在状態、及び前記運動状態計測ステップにより得られる前記ロボット装置の現在状態から、前記運動方程式上の既知項を算出する既知項算出ステップと、
前記運動方程式上の未知項としての外力を推定する外力推定ステップと、
を具備することを特徴とする外力推定方法。
【請求項２７】
前記外力推定ステップでは、前記分布型面接触状態検出ステップにより外界と接触していることが検出された領域の外形上の点に作用する外力を推定する、
ことを特徴とする請求項２６に記載の外力推定方法。
【請求項２８】
前記外力推定ステップでは、前記分布型面接触状態検出ステップにより外界と接触していることが検出された領域の外形を構成する各頂点に作用する外力を求め、その重心位置に外力が印加されたものと推定する、
ことを特徴とする請求項２６に記載の外力推定方法。
【請求項２９】
複数の可動関節を備えた機体と、外力が作用する作用点を有するロボット装置に作用する外力を推定する外力推定方法であって、
各関節アクチュエータにおける現在の位置、速度、発生力を計測するアクチュエータ現在状態計測ステップと、
前記機体上の少なくとも１箇所において加速度及び角速度を計測する運動状態計測ステップと、
前記作用点における前記ロボット装置の運動方程式を設定する運動方程式設定ステップと、
前記アクチュエータ現在状態計測ステップにより得られる各関節アクチュエータの現在状態、及び前記運動状態計測ステップにより得られる前記ロボット装置の現在状態から、前記運動方程式上の既知項を算出する既知項算出ステップと、
前記運動方程式上の未知項としての作用点における外力を推定する外力推定ステップと、
を具備することを特徴とする外力推定方法。
【請求項３０】
複数の可動関節を備えた機体からなるロボット装置に作用する外力を推定するための処理をコンピュータ・システム上で実行するようにコンピュータ可読形式で記述されたコンピュータ・プログラムであって、前記コンピュータ・システムに対し、
前記機体の任意の箇所における外界との接触状態を検出する分布型接触状態検出手順と、
各関節アクチュエータにおける現在の位置、速度、発生力を計測するアクチュエータ現在状態計測手順と、
前記機体上の少なくとも１箇所において加速度及び角速度を計測する運動状態計測手順と、
前記ロボット装置の運動方程式を設定する運動方程式設定手順と、
前記アクチュエータ現在状態計測手順により得られる各関節アクチュエータの現在状態、及び前記運動状態計測手順により得られる前記ロボット装置の現在状態から、前記運動方程式上の既知項を算出する既知項算出手順と、
前記運動方程式上の未知項としての外力の作用状態を推定する外力推定手順と、
を実行させることを特徴とするコンピュータ・プログラム。
【請求項３１】
複数の可動関節を備えた機体からなるロボット装置に作用する外力を推定するための処理をコンピュータ・システム上で実行するようにコンピュータ可読形式で記述されたコンピュータ・プログラムであって、前記コンピュータ・システムに対し、
前記機体の任意の領域における外界との接触状態を検出する分布型面接触状態検出手順と、
各関節アクチュエータにおける現在の位置、速度、発生力を計測するアクチュエータ現在状態計測手順と、
前記機体上の少なくとも１箇所において加速度及び角速度を計測する運動状態計測手順と、
前記ロボット装置の運動方程式を設定する運動方程式設定手順と、
前記アクチュエータ現在状態計測手順により得られる各関節アクチュエータの現在状態、及び前記運動状態計測手順により得られる前記ロボット装置の現在状態から、前記運動方程式上の既知項を算出する既知項算出手順と、
前記運動方程式上の未知項としての外力を推定する外力推定手順と、
を実行させることを特徴とするコンピュータ・プログラム。
【請求項３２】
複数の可動関節を備えた機体と、外力が作用する作用点を有するロボット装置に作用する外力を推定するための処理をコンピュータ・システム上で実行するようにコンピュータ可読形式で述されたコンピュータ・プログラムであって、前記コンピュータ・システムに対し、
各関節アクチュエータにおける現在の位置、速度、発生力を計測するアクチュエータ現在状態計測ステップと、
前記機体上の少なくとも１箇所において加速度及び角速度を計測する運動状態計測ステップと、
前記作用点における前記ロボット装置の運動方程式を設定する運動方程式設定ステップと、
前記アクチュエータ現在状態計測ステップにより得られる各関節アクチュエータの現在状態、及び前記運動状態計測ステップにより得られる前記ロボット装置の現在状態から、前記運動方程式上の既知項を算出する既知項算出ステップと、
前記運動方程式上の未知項としての作用点における外力を推定する外力推定ステップと、
を実行させることを特徴とするコンピュータ・プログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【公開番号】特開２００７−３００５４（Ｐ２００７−３００５４Ａ）
【公開日】平成１９年２月８日（２００７．２．８）
【国際特許分類】
【出願番号】特願２００５−２１２６２９（Ｐ２００５−２１２６２９）
【出願日】平成１７年７月２２日（２００５．７．２２）
【出願人】（０００００２１８５）ソニー株式会社 (34,172)
【Ｆターム（参考）】
【公開日】平成１９年２月８日（２００７．２．８）
【国際特許分類】
【出願日】平成１７年７月２２日（２００５．７．２２）
【出願人】（０００００２１８５）ソニー株式会社 (34,172)
【Ｆターム（参考）】
[ Back to top ]