JP5109573B2 - 制御システム及び制御方法、並びにロボット装置 - Google Patents

Description

本発明は、複数の剛体が連なって構成されるリンク構造物の制御システム及び制御方法、並びにロボット装置に係り、特に、未知の環境をセンシングし、時々刻々と変化する周囲環境から適切な外力を得て、目的の作業（タスク）が達成されるように、アクチュエータの発生力を適切に調整する制御システム及び制御方法、並びにロボット装置に関する。

さらに詳しくは、本発明は、位置や姿勢の精度だけでなく器用な力の操作を行ない、対人・対物インタラクションを伴うサービスを実現する制御システム及び制御方法、並びにロボット装置に係り、特に、接触部位を作用点とする外力を適切に用いながら力学モデルを厳密に解いて各関節アクチュエータの発生力目標値を決定するとともに、各関節内のモデル化困難な力を補償して、作用点の限定のない、良好な力触覚インタラクションを実現する制御システム及び制御方法、並びにロボット装置に関する。

急激な高齢化社会の到来により、高齢者とそれを支える生産年齢人口の比率は現在（２００５年）の１人：３．３人から、２０１５年には１人：２．４人に、さらに２０２５年には１人：２．１人になると見込まれている。このように、人口構成に占める比重が急速に高まっていく高齢者が、できる限り要介護状態とならず健康で活き活きと暮らせること、また、要介護状態となってもできる限り悪化を防ぎ、自立した生活を送ることができる社会とすることが急務となっている。

このような高齢化社会の到来に伴い、高齢者介護施設や高齢者を抱える家庭では、高齢者の心身の補助を行なうことを目的としたメカトロ機器への要求が高まっている。また、歩行補助器や上肢のパワーアシストといった物理的な補助に留まらず、ロボットを作業療法の中に効果的に取り入れた、メンタルアシストへの要求も存在する。

上記の分野では、メカトロ機器は、人や複雑な実環境と柔軟且つ安全に物理接触しながら作業を遂行しなければならない。つまり、従来の産業ロボットが既知環境下で固定的動作を行なっていたのとは異なり、上記メカトロ機器は、未知の環境をセンシングし、時々刻々と変化する周囲環境から適切な外力を得て、目的の作業（タスク）が達成されるように、アクチュエータの発生力を適切に調整しなければならない。

このような環境適応型のロボット制御方法として、例えば、複数台カメラを用いたステレオ視やレーザレンジファインダを用いて周辺環境形状を計測し、それに適合するようにロボットの位置・姿勢を修正する制御方法が従来から試みられている（例えば、非特許文献１〜２を参照のこと）。しかし、この制御方法では、カメラの精度が求められるだけでなく、計測結果に基づいて周辺環境の形状を再現する際の演算量が多くなることから、更新レートが低いという問題がある。また、位置制御をベースとすることから、リアクティブに環境に適応することは難しいと思料される。

位置制御は、基本的に位置を保持することが目的であることから、俗に「硬い制御」と呼ばれ、外力に柔軟に応じたり、速度や加速度の制御を精密に行なったりするのには適さない。例えば、多様な外界との物理インタラクションを行ないながらタスクを遂行するロボット装置は、本来は位置制御との親和性は低い。

これに対し、力制御は、制御則、システム構成は複雑化するが、理想的には力制御系で駆動することが望ましいと思料される。例えば、力センサをエンドエフェクタや脚部に配備し、環境からの作用力を計測しながら周囲の物体をマニピュレートしたり、不整地に適応したりする方法も提案されている（例えば、非特許文献３、特許文献１を参照のこと）。しかし、機体に作用する力を漏れなく検出できるように力センサを配備することは技術的に難しく、結果として、環境との接触が許容されない部位が発生する問題があると考えられる。言い換えれば、力学モデルを厳密に解いて力触覚インタラクションを実現するには、外力の作用点が限定される。

環境との接触状況を把握するために全身に接触センサを分布状に配備した研究も挙げられるが（例えば、非特許文献４〜６を参照のこと）、タスクを達成するために、接触点に如何ほどの力を掛ければ良いかなど、力学・制御の観点から十分な考察は行なわれていない。このため、環境との接触状況は検知できても、かかる接触状況下で目的のタスクを達成するための能力が達成されたとは言えない。

力学モデルを厳密に解いて所定部位に指定された力を発生するための関節トルクを決定する計算トルク法も提案されているが（例えば、非特許文献７を参照のこと）、関節内部にはギヤ摩擦などに起因するモデル化困難な属性（外乱）が含まれており、この主の属性を適切に扱われないと誤差が生じ、振動を励起する問題がある。

特開平５−３０５５８３号公報 "Ａ Ｍｏｄｕｌａｒ Ａｒｃｈｉｔｅｃｔｕｒｅ ｆｏｒ Ｈｕｍａｎｏｉｄ Ｒｏｂｏｔ Ｎａｖｉｇａｔｉｏｎ"（Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ２００５ ５ｔｈ ＩＥＥＥＲＡＳ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｈｕｍａｎｏｉｄ Ｒｏｂｏｔｓ，ｐｐ．２６−３１） "Ｖｉｓｉｏｎ−ｂａｓｅｄ ２．５Ｄ ｔｅｒｒａｉｎ ｍｏｄｅｌｉｎｇ ｆｏｒ ｈｕｍａｎｏｉｄ ｌｏｃｏｍｏｔｉｏｎ"（Ｉｎ Ｉｎｔ．Ｃｏｎｆ．ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ（ＩＣＲＡ），Ｔａｉｐｅｉ，Ｔａｉｗａｎ，２００３） 「力覚センサ」（日本ロボット学会誌Ｖｏｌ６，Ｎｏ．９，ｐｐ７５９−７６５， １９９１） 「ロボット表面多値接触センサの開発と応用」（日本機械学会ロボティクスメカトロニクス講演会９８ 講演予稿集，１ＣＩ１−２，１９９８） 「導電性ゲルを用いたロボット用柔軟触覚センサの開発と応用」（第１６ 回日本ロボット学会学術講演会予稿集，８７３−８７４，１９９８） 「導電性ファブリックを用いた全身被覆型触覚センサスーツ」（日本ロボット学会誌Ｖｏｌ．１６，Ｎｏ．１，ｐｐ．８０−８６，１９９８） "Ａ ｐｒｉｏｒｉｔｉｚｅｄ ｍｕｌｔｉ−ｏｂｊｅｃｔｉｖｅ ｄｙｎａｍｉｃ ｃｏｎｔｒｏｌｌｅｒ ｆｏｒ ｒｏｂｏｔｓ ｉｎ ｈｕｍａｎ ｅｎｖｉｒｏｎｍｅｎｔｓ"（Ｉｎ Ｐｒｏｃｅｅｄｉｎｇ ｏｆ ｔｈｅ ＩＥＥＥ／ＲＳＪ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｈｕｍａｎｏｉｄ Ｒｏｂｏｔｓ，２００４）

本発明の目的は、複数の剛体が連なって構成されるリンク構造物のための優れた制御システム及び制御方法、並びにロボット装置を提供することにある。

本発明のさらなる目的は、未知の環境をセンシングし、時々刻々と変化する周囲環境から適切な外力を得て、目的の作業（タスク）が達成されるように、アクチュエータの発生力を適切に調整することができる、優れた制御システム及び制御方法、並びにロボット装置を提供することにある。

本発明のさらなる目的は、本発明は、位置や姿勢の精度だけでなく器用な力の操作を行ない、対人・対物インタラクションを伴うサービスを実現することができる、優れた制御システム及び制御方法、並びにロボット装置を提供することにある。

本発明のさらなる目的は、接触部位を作用点とする外力を適切に用いながら力学モデルを厳密に解いて各関節アクチュエータの発生力目標値を決定するとともに、各関節内のモデル化困難な力を補償して、作用点の限定のない、良好な力触覚インタラクションを実現することができる、優れた制御システム及び制御方法、並びにロボット装置を提供することにある。

本発明は、上記課題を参酌してなされたものであり、複数の剛体リンクが関節を介して連接して構成され、関節アクチュエータにアクチュエータ力を発生させることによって駆動するリンク構造物を制御する制御システムであって、
前記リンク構造物の幾何学的パラメータ並びに動力学的パラメータからなる力学モデルと、
前記リンク構造物の前記力学モデルに作用する仮想的な力を算出する仮想的外力算出手段と、
前記リンク構造物と外界との接触部位を検出する接触部位検出手段と、
前記接触部位検出手段によって検出された接触情報を用いて、前記仮想的外力算出手段が算出した仮想的な力を実在可能な外力と前記関節アクチュエータのアクチュエータ力に変換する実在力変換手段とを備え、
前記実在力変換が出力するアクチュエータ力を前記関節アクチュエータに発生させることを特徴とする制御システムである。

但し、ここで言う「システム」とは、複数の装置（又は特定の機能を実現する機能モジュール）が論理的に集合した物のことを言い、各装置や機能モジュールが単一の筐体内にあるか否かは特に問わない。

ロボットなどのリンク構造物からなるメカトロ機器は、人や複雑な実環境と柔軟且つ安全に物理接触しながら作業を遂行することが求められており、既知環境下で固定的動作を行なう産業ロボットとは相違し、未知の環境をセンシングし、時々刻々と変化する周囲環境から適切な外力を得て、目的の作業（タスク）が達成されるように、アクチュエータの発生力を適切に調整しなければならない。また、位置制御をベースとする制御方法ではリアクティブに環境に適応することは難しく、理想的には力制御系で駆動することが望ましいと思料される。

しかし、機体に作用する力を漏れなく検出できるように力センサを配備することは技術的に難しく、環境との接触が許容されない部位があるため、力学モデルを厳密に解いて力触覚インタラクションを実現するには、外力の作用点が限定される。

また、関節内部にはギヤ摩擦などに起因するモデル化困難な属性（外乱）が含まれているため、この主の属性を適切に扱われないと誤差が生じ、振動を励起する問題がある。

これに対し、本発明に係る制御システムは、リンク構造物の力学モデルに作用する仮想的な力を算出する一方、リンク構造物と外界との接触部位を漏れなく検出するために接触センサを機体表面全体にわたって分布状に配備しており、このような接触部位検出手段によって検出された接触情報を用いて、上記の仮想的な力を実在可能な外力とアクチュエータ力に変換することによって、力学モデルを厳密に解いて全アクチュエータの発生力目標値を決定することができる。

さらに、本発明に係る制御システムは、関節部にトルク・センサを配設しており、上記の算出したアクチュエータ力と、トルク・センサの検出したトルクの偏差が最小となるようにトルク・フィードバック制御を行なって、モデル化困難な力を補償するようになっている

したがって、本発明によれば、作用点の限定されない、良好な力触覚インタラクションを実現することができる。

本発明によれば、未知の環境をセンシングし、時々刻々と変化する周囲環境から適切な外力を得て、目的の作業（タスク）が達成されるように、アクチュエータの発生力を適切に調整することができる、優れた制御システム及び制御方法、並びにロボット装置を提供することができる。

また、本発明によれば、本発明は、位置や姿勢の精度だけでなく器用な力の操作を行ない、対人・対物インタラクションを伴うサービスを実現することができる、優れた制御システム及び制御方法、並びにロボット装置を提供することができる。

本発明によれば、ロボットなどのリンク構造物の機体表面全体にわたって分布状に配備した接触センサを用いて外界との接触部位を漏れなく検出し、検出された接触部位を作用点とする外力を適切に用いながら、目的の運動が達成されるように力学モデルを厳密に解いて、全アクチュエータの発生力目標値を決定することができる。さらに、リンク間を接続する各関節内のモデル化困難な力を関節部に設けられたトルク・センサで補償することで、作用点の限定されない、良好な力触覚インタラクションを実現することができる。

本発明のさらに他の目的、特徴や利点は、後述する本発明の実施形態や添付する図面に基づくより詳細な説明によって明らかになるであろう。

以下、図面を参照しながら本発明の実施形態について詳解する。

本発明は、複数の剛体が連なって構成されるリンク構造物の制御システムに関する。ここで言うリンク構造物の一例は、人間型ロボットである。

図１には、本発明を適用することができる人間型ロボットの外観構成を示している。図示の人間型ロボットは、骨盤部には、移動手段としての２肢の脚体と、腰関節を介して上体が接続されている。上体には、２肢の腕部と、首関節を介して頭部が接続されている。

左右の脚体は、それぞれ股関節３自由度と、膝関節１自由度と、足首関節２自由度の、計６自由度を備えている。また、左右の腕部は、それぞれ肩関節３自由度と、肘関節１自由度と、手首関節２自由度の、計６自由度を備えている。首関節及び腰関節は、ともにＸ、Ｙ、Ｚ軸回りに３自由度を有している。

各関節軸を駆動するアクチュエータは、例えばＤＣブラシレス・モータと減速機、並びに減速機の出力軸の回転位置を検出する位置センサで構成される。これら関節駆動アクチュエータは、人間型ロボット全体の動作を統括的にコントロールするホスト・コンピュータと接続され、その位置制御目標値がホスト・コンピュータから与えられるとともに、現在の関節角度や関節角速度をホスト・コンピュータに送信することができるものとする。

図１に示す人間型ロボットの表面のうち、外界との接触が想定されている各部位には、触覚センサ群ｔ１、ｔ２、…、ｔ１７が取り付けられている。図２には、１つの触覚センサ群の構成を示している。同図に示すように、１つの触覚センサ群ｔは、接触状態をそれぞれ独立して検出することができる複数の触覚センサＣＳをアレイ状に配置して構成され、触覚センサ群ｔとしてはいずれの触覚センサＣＳが接触状態にあるかによって、詳細な接触位置を特定することができる。

図３には、触覚センサＣＳの構成を模式的に示している。触覚センサＣＳは、２枚の極板Ｐ１及びＰ２で空隙Ｓを挟んだ構造をしており、一方の極板Ｐ１には電位Ｖ_ccが印加され、他方の極板Ｐ２は接地されている。そして、一方の極板Ｐ１をパラレル・インターフェース（ＰＩＯ）経由でマイクロ・コンピュータに入力し、極板間の接触状態すなわち触覚センサＣＳに外力が作用しているか否かを判定することができる。

触覚センサ群ｔ毎に、その近隣に１つのマイクロ・コンピュータを配置して、触覚センサ群を構成するすべての触覚センサＣＳの検出信号を入力し、これらのオン／オフ状態を収集して、当該部位における外界との接触の有無並びに接触位置をホスト・コンピュータに送信するものとする。

また、図１に示すように、人間型ロボットの骨盤部には、３軸の加速度センサａ１と３軸の角速度センサ（ジャイロ）ｇ１が搭載されている。その近隣には、これらのセンサ値を計測するマイクロ・コンピュータが配備され、その計測結果をホスト・コンピュータに送信するものとする。

さらに、この人間型ロボットの関節部には、アクチュエータ・モータの出力軸とフレームとの間には、図４に示すようなトルク・センサが配置されており、これによって関節軸回りに発生するトルクを計測することができる。図示のトルク・センサは、稀歪構造を有し、その微小変形量を歪みゲージで計測する方法で構成することができる。また、トルク計測結果は、アクチュエータ内に装備されたマイコンで収集された後、ホスト・コンピュータに送信される。

図５には、図１に示した人間型ロボットにおける結線トポロジの構成例を示している。

人間型ロボットは、胴体部に、３軸の腰関節アクチュエータａ１、ａ２、ａ３、及び３軸の首関節アクチュエータａ１６、ａ１７、ａ１８を持ち、これらはホスト・コンピュータに対しシリアル接続されている。また、各アクチュエータ・モータａ１６、ａ１７、ａ１８の出力軸とフレームとの間には、図４に示したトルク・センサｔｑ１６、ｔｑ１７、ｔｑ１８が配置されている。各関節アクチュエータは、シリアル・ケーブルを通じて、その位置制御目標値を受け取るとともに、現在の出力トルクや関節角度、関節角速度をホスト・コンピュータに送信する。

また、人間型ロボットは、左腕部に、３軸の肩関節アクチュエータａ４、ａ５、ａ６、１軸の肘関節アクチュエータａ７、及び２軸の手首関節アクチュエータａ８、ａ９を持ち、各アクチュエータ・モータａ４、ａ５、ａ６、ａ７、ａ８、ａ９の出力軸とフレームとの間には、図４に示したトルク・センサｔｑ４、ｔｑ５、ｔｑ６、ｔｑ７、ｔｑ８、ｔｑ９が配置され、これらはホスト・コンピュータに対しシリアル接続されている。同様に、人間型ロボットの右腕部には、３軸の肩関節アクチュエータａ１０、ａ１１、ａ１２、１軸の肘関節アクチュエータａ１３、及び２軸の手首関節アクチュエータａ１４、ａ１５を持つとともに、各アクチュエータ・モータａ１０、ａ１１、ａ１２、ａ１３、ａ１４、ａ１５の出力軸とフレームとの間には図４に示したトルク・センサｔｑ１０、ｔｑ１１、ｔｑ１２、ｔｑ１３、ｔｑ１４、ｔｑ１５が配置され、これらはホスト・コンピュータに対しシリアル接続されている。

また、人間型ロボットは、左脚部に、３軸の股関節アクチュエータａ１９、ａ２０、ａ２１、１軸の膝関節アクチュエータａ２２、及び２軸の足首関節アクチュエータａ２３、ａ２４を持つとともに、各アクチュエータ・モータａ１９、ａ２０、ａ２１、ａ２２、ａ２３、ａ２４の出力軸とフレームとの間には図４に示したトルク・センサｔｑ１９、ｔｑ２０、ｔｑ２１ｔｑ２２、ｔｑ２３、ｔｑ２４が配置され、これらはホスト・コンピュータに対しシリアル接続されている。同様に、右脚部には、３軸の股関節アクチュエータａ２５、ａ２６、ａ２７、１軸の肘関節アクチュエータａ２８、及び２軸の足首関節アクチュエータａ２９、ａ３０を持つとともに、各アクチュエータ・モータａ２５、ａ２６、ａ２７、ａ２８、ａ２９、ａ３０の出力軸とフレームとの間には図４に示したトルク・センサｔｑ２５、ｔｑ２６、ｔｑ２７、ｔｑ２８、ｔｑ２９、ｔｑ３０が配置され、これらはホスト・コンピュータに対しシリアル接続されている。

各関節で使用されるアクチュエータａ１〜ａ３０は、例えばＤＣブラシレス・モータと減速機、並びに減速機の出力軸の回転位置を検出する位置センサで構成され、外部から与えられた位置制御目標値に従って回転駆動するとともに、現在の出力トルクや、関節角度、関節角速度を出力する。この種の関節アクチュエータについては、例えば本出願人に既に譲渡されている特開２００４−１８１６１３号公報に記載されている。

人間型ロボットの右脚部には、右足部触覚センサ群ｔ１、右脛部触覚センサ群ｔ２、右大腿部触覚センサ群ｔ３が配設され、これらはホスト・コンピュータに対しシリアル接続されている。各々の触覚センサ群ｔ１〜ｔ３にはマイクロ・コンピュータが装備されており（前述）、それぞれ触覚センサ群における触覚センサＣＳのオン／オフ状態を収集して、シリアル・ケーブルを通じてホスト・コンピュータに送信する。同様に、左脚部には、左足部触覚センサ群ｔ９、左脛部触覚センサ群ｔ１０、左大腿部触覚センサ群ｔ１１が配設され、それぞれ触覚センサ群における触覚センサＣＳのオン／オフ状態を内部のマイクロ・コンピュータで収集して、シリアル・ケーブルを通じてホスト・コンピュータに送信する。

また、人間型ロボットの右腕部には、右手首触覚センサ群ｔ４、右前腕部触覚センサ群ｔ５、右上腕部触覚センサ群ｔ６が配設され、それぞれ触覚センサ群における触覚センサＣＳのオン／オフ状態を内部のマイクロ・コンピュータで収集して、シリアル・ケーブルを通じてホスト・コンピュータに送信する。同様に、左腕部には、左手首触覚センサ群ｔ１２、左前腕部触覚センサ群ｔ１３、左上腕部触覚センサ群ｔ１４が配設され、それぞれ触覚センサ群における触覚センサＣＳのオン／オフ状態を内部のマイクロ・コンピュータで収集して、シリアル・ケーブルを通じてホスト・コンピュータに送信する。

また、人間型ロボットの胴体部の左右にはそれぞれ胴部触覚センサ群ｔ７及びｔ１５が取り付けられており、それぞれ触覚センサ群における触覚センサＣＳのオン／オフ状態を内部のマイクロ・コンピュータで収集して、シリアル・ケーブルを通じてホスト・コンピュータに送信する。

また、人間型ロボットの頭部の左右にはそれぞれ頭部触覚センサ群ｔ８及びｔ１６が取り付けられており、それぞれ触覚センサ群における触覚センサＣＳのオン／オフ状態を内部のマイクロ・コンピュータで収集して、シリアル・ケーブルを通じてホスト・コンピュータに送信する。

以下では、上記の接触センサｔ１〜ｔ１７によって、外界からの力の作用点を検出することができたとき、そこを作用点として目的の動作を達成するためのロボット制御系を構成することを考える。

図６には、図１に示した人間型ロボットの関節自由度モデルを示している。同図から分かるように、２足歩行の移動ロボットを、骨盤Ｂを基底とする開リンク木構造として表現することができる。

移動ロボットの場合、ロボットがワールド空間中を自由に移動し、姿勢を変化させることができる。したがって、ロボット全体の状態を表すための状態変数として、骨盤Ｂのオイラー角α＝（α，β，γ）^Tとその位置ｐ₀（ｐ_0x，ｐ_0y，ｐ_0z）^Tを導入する。

すると、ロボット全体の姿勢を表す一般化変数ｑは、アクチュエータの現在状態としての全関節値を並べてできるベクトルθと、ロボットの運動状態としてのベースの姿勢α及び位置ｐ₀を並べたベクトルとして、下式（１）のような形で表現することができる。

力制御系のロボット制御における重要な概念として、「操作空間（Ｏｐｅｒａｔｉｏｎａｌ Ｓｐａｃｅ）」と呼ばれる概念がある。操作空間は、ロボットに作用する力と発生する加速度の関係を記述するための空間である。ロボットの関節角を位置制御ではなく力制御する際にロボットと環境の接し方を拘束条件として用いるときに、操作空間が必須となる。ここで言う拘束条件とは、例えば、ロボットの自己干渉に対する拘束条件、関節可動角の拘束条件、「手が物にフィットする」や「左右の眼が水平になる」といった事象が挙げられる。

剛体リンクが関節を介して連なったリンク構造物において、一般化変数ｑの時間微分値との関係がヤコビアンＪを用いて、下式（２）に示す形に関連付けるとき、物理量ｘに対して操作空間を定義することができる。

マニピュレータ先端の手先位置姿勢など、タスク遂行のためのデカルト座標系が操作空間の一例である。操作空間の基本的な考え方については、例えば、“Ａ ｕｎｉｆｉｅｄ ａｐｐｒｏａｃｈ ｔｏ ｍｏｔｉｏｎ ａｎｄ ｆｏｒｃｅ ｃｏｎｔｒｏｌ ｏｆ ｒｏｂｏｔ ｍａｎｉｐｕｌａｔｏｒｓ”（Ｔｈｅ ｏｐｅｒａｔｉｏｎａｌ ｓｐａｃｅ ｆｏｒｍｕｌａｔｉｏｎ，ＩＥＥＥ Ｊｏｕｒｎａｌ ｏｆ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ，ＲＡ−３（１），ｐｐ．４３−５３，１９８７）を参照されたい。

一般に、リンク構造物全体の運動方程式は、下式（３）に示すような形で表現できることが知られている。

ここで、τは一般化変数ｑに対応した一般化力、ｂは重力・コリオリ力、ｆは操作空間に作用する外力である。上式（３）を下式（４）のように変形する。

ここで、Λ^-1は操作空間慣性逆行列と呼ばれ、下式（５）のように表される。但し、Ｈは構造全体の関節空間に対する慣性行列である。

また、上式（４）の右辺第３項のｃは操作空間バイアス加速度（すなわち、外力が作用しない場合に操作空間に作用する加速度）に相当し、下式（６）のように表される。

操作空間、すなわち加速度と力の関係は操作空間慣性逆行列によって与えられる。操作空間慣性逆行列は、力制御、力学シミュレーション、コンピュータ・アニメーション、姿勢編集、外力推定など、多くの分野への応用を可能にする重要な物理量である。例えば、動力学を考慮した拘束条件を追加して関節加速度を計算し、又はリンクに働く力とそれによる加速度との関係を表す操作空間慣性逆行列を用いて定式化された動力学的な拘束条件と、運動学的な拘束条件とを連立させて解くことにより、人型リンク形の関節の運動を生成する運動生成方法について提案がなされている（例えば、特開２００３−２３１０７７号公報を参照のこと）。

なお、上式（５）に示した定義通りの計算では、構造全体の関節空間に対する慣性行列Ｈが介在して計算の無駄が発生するために、操作空間慣性逆行列の算出には非常に多くの計算量となるため、実時間処理に向かないという問題がある。これに対し、リンク構造物の一般化力（関節力）から一般化加速度（関節加速度）を得る順動力学演算を応用することで、操作空間慣性逆行列を高速に算出するとともに計算負荷の軽減を図ることができる。操作空間慣性逆行列及びバイアス加速度の高速な計算方法に関しては、例えば本出願人に既に譲渡されている特開２００７−１０８９５５号公報を参照されたい。

位置・速度・加速度に関する運動要求は、式（４）の左辺の加速度として与えられる。それを実現するために操作空間に印加すべき力ｆは、下式（７）及び（８）に示すような線形相補性問題を解くことで得ることができる。

また、力に関する運動要求として、既知の力ｆ_kを操作空間Ｊ_kに発生する要求があるものとすると、上記の位置・速度・加速度に関する運動要求と併せて、全体で要求される一般化力τ_vは下式（９）のように表される。

但し、ｆ_vはｆとｆ_kの連結ベクトルであり、Ｊ_vはＪとＪ_kを縦に並べてできるｆ_vの操作空間を表すヤコビアンである。

ｆ_vは実在しない力も含む仮想力である。実際には、下式（１０）に示すように、環境から得られる外力ｆ_eと、関節部のアクチュエータのトルクτ_aを協調的に用いて仮想力ｆ_vは実現される。

ここで、Ｊ_e、Ｊ_aは、ｆ_e、τ_aの作用する操作空間に対応するヤコビアンである。

式（１０）を満足するｆ_e及びτ_aが常に存在するとは限らない。そこで、上式仮想力ｆ_vの修正分Δｆ_vを考慮する。

上式（１１）は、下式（１２）及び（１３）のような問題を解くことで、解を得ることができる。

ここで、ｅは、上式（１１）の左辺から右辺を引いた値であり、上式（１１）の誤差を与える。また、ｙは、τ_a、ｆ_e、Δｆ_vの連結ベクトルである。よって、上式（１２）の 第１項は、上式（１１）の等式成立の誤差最小化のための条件を、上式（１２）の第２項は、仮想力修正量Δｆ_v、実在力ｆ_e及びτ_aの最小化のための条件を表している。Ｑ₁並びにＱ₂はそれらの間の最小化の重みを表す正定値対称行列である。不等式拘束式（１３）は、垂直反力、摩擦条件、関節発生力の上限・下限などを与える。

上式（１２）及び（１３）を整理すると、下式（１４）及び（１５）に示すような２次計画問題として定式化される。

２次計画問題を用いることで、上式（１４）及び（１５）をｙ、ひいてはτ_a、ｆ_e、Δｆ_vについて解くことができる。これを解いて得られる関節部のアクチュエータのトルクτaをロボットに作用すれば良い。

環境から得られる外力ｆ_eに対応する操作空間Ｊ_eを適切に選ばないと、仮想力から実在力を得る変換式（１４）及び式（１５）の解は妥当なものにならない。すなわち、操作空間Ｊ_eは、実際に外力が得られる部位・方向に対して設定しなければならない。

一般に、実際に外界に接触している部位は、環境から外力が供給されうる点とみなすことができる。このため、接触センサから得られる外界との接触部位にＪeを設定する。操作空間の方向は、垂直反力方向（１次元）と摩擦力方向（２次元）の計３方向に設定すればよい。すなわち、図７に示すように、接触センサＣＳがオンとなったとき、ＣＳの配置部位における垂直反力方向（ｆ_N）、摩擦力方向（ｆ_Fx, ｆ_Fy）を決定し、それぞれに操作空間を定義すればよい。垂直反力方向は、接触センサＣＳの配置部位におけるロボット形状の法線ベクトルで良好に近似される。摩擦力方向はそれに直交する任意の２方向を算出すればよい。すなわち、接触部位におけるロボット形状の法線ベクトル方向並びに接線ベクトル方向に、実在的な外力を設定する。

なお、ヤコビアンの算出には、一般に、周知の逆運動力学演算の一部を用いればよい。上式（３）の右辺から左辺を得る演算は、一般に逆動力学演算として知られ、下式（１６）のように表記できる。

ここでｇは重力加速度を表す。人間型ロボットのように外力の作用する宙に浮いた枝分かれのあるリンク鎖に適用できる形に拡張した逆動力学演算の構成法については、例えば長阪憲一郎著「動力学フィルタによる人間型ロボットの全身運動生成」（東京大学工学部情報工学専攻博士論文、１９９９年１２月、第４．２．２節）を参照されたい。

上式（３）の右辺をよく眺めると、下式（１７）のようにヤコビアンを算出することができる。

ここで、ｅ_iは第ｉ要素が１の単位ベクトルを表す。上記Ｊ_eの場合、すべての接触センサＣＳの配置部位における垂直反力方向（ｆ_N）及び摩擦力方向（ｆ_Fx，ｆ_Fy）に対して、それぞれ単位力を作用させた状態で上式（１７）を実行すれば、操作空間Ｊ_eの各行を求めることができる。

上式（１４）及び（１５）を解いて得られる関節部のアクチュエータのトルクτ_aは、精度よく実現する必要がある。アクチュエータのトルクはおよそ電流値に比例し、電流値を制御すればトルクの操作が可能である。しかしながら、実際のモータ駆動系には、モデル化困難な摩擦などが存在し、開ループでの電流指令を送っただけでは所望のトルクを達成することができない。

そこで、本実施形態では、関節部に作用するトルク情報を、関節部に配置されたトルク・センサ（図４を参照のこと）により検出し、電流指令にフィードバックする。これにより、上記のようなモデル化困難な摩擦がモータ駆動系に含まれていても、精度良く、所望のトルクに追従することが可能となる。例えば，周知の外乱オブザーバなどを用いて、良好なトルク追従が可能となる。

以上説明してきたように、本実施形態では、人間型ロボットの機体表面全体にわたって分布状に配備した接触センサを用いて外界との接触部位を漏れなく検出し、検出された接触部位を作用点とする外力を適切に用いながら、目的の運動が達成されるように力学モデルを厳密に解いて、全アクチュエータの発生力目標値を決定する。さらに、リンク間を接続する各関節内のモデル化困難な力を関節部に設けられたトルク・センサで補償することで、作用点の限定されない、良好な力触覚インタラクションを実現することができる。

図８には、このようなロボット制御方法を実現する制御システム１０の機能ブロック図を示している。

力学モデル１１は、制御対象となるロボットの剛体リンクの幾何学的パラメータ、動力学的パラメータを保持する。関節角度など、現在のロボットの状態に応じて時々刻々と変化するものも、力学モデル１１が保持するデータに含まれる。

目標値設定部１２は、ロボットの各部位、関節、運動量に課される、位置・速度・加速度・姿勢・角速度・角加速度・力・モーメントなどに関する目標値を設定する。例えば、位置・速度・加速度・姿勢・角速度・角加速度については、上式（４）の左辺の値として目標値が設定される。既知の力ｆ_kについては別途記憶しておく。

仮想的外力算出部１３は、目標値設定部１２で設定された目標値を実現するために必要な仮想的外力を求める。具体的には、未知の仮想的外力については、上式（７）及び（８）を満足する力ｆを線形相補性問題解決器１３−２によって求める。上式（７）の係数行列Λ^-1やバイアス・ベクトルｃは、操作空間物理量算出部１３−１において、既に報告済みの操作空間物理量の高速演算（例えば、特開２００７−１０８９５５号公報を参照のこと）を用いて求める。操作空間物理量の算出に、力学モデルの情報が利用される。既知の仮想的外力ｆ_kが更に付加される場合は、全体で要求される仮想的外力は式（９）で得る。

接触部位検出手段１４は、ロボットの全身にわたって表面に取り付けられた触覚センサ群からなり（図１を参照のこと）、ロボットが外界と接触している部位を検出する。接触部位情報として、外界と接触している剛体リンクＩＤと、該剛体リンクから見た接触位置座標を出力する。

実在力変換部１５は、仮想的外力算出部１３で得られた仮想的外力τ_v＝Ｊ^T _vｆ_v を、上式（１１）を満足するように、実在力、すなわち、環境から得られる外力ｆ_eと、関節部のアクチュエータのトルクτ_aに変換する。環境から得られる外力の作用点及び方向には、接触部位検出手段１４の出力結果を用いる。接触部位検出手段１４によって検出された接触部位における、ロボット表面形状の法線ベクトル方向並びに接線ベクトル方向に、実在的な外力である垂直反力並びに摩擦力をそれぞれ設定する（前述）。仮想的外力から実在力への変換処理は、２次計画問題解決器１５−１で上式（１４）及び（１５）を解くことで達成される。実在力変換部１５は、そのうち、アクチュエータのトルクτ_aを出力する。

トルク検出手段１６は、関節部に取り付けられたトルク・センサ（図４を参照のこと）であり、各関節部に作用する実トルクを計測し、出力する。

トルク・フィードバック制御部１７は、トルク検出部１６で検出されたトルクと、指令トルクの偏差を検出し、電流目標値にフィードバックする。これにより、モータ駆動系に含まれる摩擦や慣性などの外乱を抑圧し、精度良くアクチュエータ目標トルクが各関節において実現される。

図９には、仮想的外力算出部１３及び実在力変換部１５によってアクチュエータの目標トルクを算出するための処理手順をフローチャートの形式で示している。

目標値設定部１２で目標値を設定すると（ステップＳ１）、関節角度・角速度、骨盤部の姿勢・角速度等を検出し、力学モデル１１の状態を現在のロボットの運動状態と合致するように更新する（ステップＳ２）。

次いで、仮想的外力算出部１３にて、操作空間物理量を算出し（ステップＳ３）、さらに仮想的外力算出部１３にて、線形相補性問題を解いて、未知の仮想的外力を算出する（ステップＳ４）。

次いで、仮想的外力算出部１３にて、上式（９）により仮想的外力の総和を求める（ステップＳ５）。

次いで、接触部位検出手段１４において接触部位を検出すると（ステップＳ６）、実在力変換部１５において２次計画問題を解いて、仮想的外力を実在力に変換する（ステップＳ７）。

そして、実在力中、アクチュエータ発生分を抽出して、アクチュエータ目標トルクとする（ステップＳ８）。

以上の処理を所定の周期（例えば１ミリ秒）で実行する。

図１０には、トルク・フィードバック制御部１７で実行される、実在力変換部１５が出力するアクチュエータ力とトルク検出手段１６が検出したトルクの偏差が縮小するための処理手順をフローチャートの形式で示している。

トルク検出手段１６が関節部に印加されるトルクを計測すると（ステップＳ１１）、トルク・フィードバック制御部１７は、実在力変換部１５が出力するアクチュエータ目標トルクとの偏差を算出する（ステップＳ１２）。

そして、トルク・フィードバック制御部１７は、算出したトルク偏差を基に、アクチュエータ・モータへの電流目標値を算出し（ステップＳ１３）、電流制御を実施する（ステップＳ１４）。

このように、リンク間を接続する各関節内のモデル化困難な力を関節部に配設されたトルク・センサで補償することで、作用点の限定されない、良好な力触覚インタラクションを実現することができる。

トルク・フィードバック制御部１７は、図１０に示した処理手順を、図９に示した仮想的外力算出部１３及び実在力変換部１５による処理とは独立して、より速い周期（例えば、１００マイクロ秒）で繰り返し実行する。

図１１には、これまで説明してきた制御システム１０を用いて人間型ロボットの制御を行なった様子を示している。図示の例では、並進運動量をゼロに保つよう運動目的が課されている。

ロボットを逆立ち状態で地面に置くと、接触部位検出手段１４が頭部、手部の接触点を検出し、実在力変換部１５は、そこから得られる床反力と、全身の関節力を駆使して、並進運動量＝０（重心不動）の目的を自律的に達成する。この結果、倒れず逆立ちを維持することができる。

本実施形態に係る制御システムでは、力制御を行なっているので、冗長自由度分の受動性を有し、外界と柔軟に接することもできる。図１１に示した例では、床面に接地した後、右足先端を前方へ押している。ロボット右足部はこれに応じて受動的に前方に移動するが、力学計算の結果、左足を逆に後方に移動させるなど全身を協調的に動作させることで、全体として転倒することなく、バランスを維持している。すなわち、重心不動の目的を侵犯しない範囲で、受動性を有することが分かる。

また、図１２には、その他の接地状態において、図１１と同様に並進運動量＝０（重心不動）の運動目的を与えた例を示している。同図左は、四つ這い状態でロボットを床面に置き、両足，両手先端に接触点が発生した場合を示している。また、同図中央は、座位でロボットを床面に置き、臀部、脚部、足部に接触点が発生した場合を示している。また、同図右は、ブリッジ姿勢でロボットを床面に置き、片足、両手先端に接触点が発生した場合を示している。

床面に置くまではロボットは脱力状態にあるが、接地と同時に重心不動の目的を達成するよう、床反力と全身の関節力を駆使して、図示の姿勢を維持する。また、ロボットが床面に接触した後も、冗長自由度に関して、受動性を有している点は、図１１に示した例と同様である。

上記の数例では、制御システムは同一であり、作用点に特化した実装は一切含まれない。本制御システムにより、機体の任意箇所を作用点とし、外界と多様で柔軟な力触覚インタラクションが達成可能であることが分かる。

以上、特定の実施形態を参照しながら、本発明について詳解してきた。しかしながら、本発明の要旨を逸脱しない範囲で当業者が該実施形態の修正や代用を成し得ることは自明である。

本明細書では、本発明を２足歩行の脚式移動ロボットに適用した実施形態を中心に説明してきたが、本発明の要旨はこれに限定されるものではない。

要するに、例示という形態で本発明を開示してきたのであり、本明細書の記載内容を限定的に解釈するべきではない。本発明の要旨を判断するためには、特許請求の範囲を参酌すべきである。

図１は、本発明を適用することができる人間型ロボットの外観構成を示した図である。 図２は、触覚センサ群の構成を示した図である。 図３は、触覚センサＣＳの構成を模式的に示した図である。 図４は、稀歪構造を有するトルク・センサの構成例を示した図である。 図５は、図１に示したロボット装置における結線トポロジの構成例を示した図である。 図６は、図１に示した人間型ロボットの関節自由度モデルを示した図である。 図７は、接触センサＣＳがオンとなったとき、ＣＳの配置部位における垂直反力方向（ｆ_N）、摩擦力方向（ｆ_Fx, ｆ_Fy）を決定し、それぞれに操作空間を定義する様子を示した図である。 図８は、本発明に係る制御システムの機能ブロック図である。 図９は、仮想的外力算出部１３及び実在力変換部１５によってアクチュエータの目標トルクを算出するための処理手順を示したフローチャートである。 図１０は、実在力変換部１５が出力するアクチュエータ力とトルク検出手段１６が検出したトルクの偏差が縮小するための処理手順を示したフローチャートである。 図１１は、本発明に係る制御システム１０を用いて人間型ロボットの制御を行なった様子を示した図である。 図１２は、本発明に係る制御システム１０を用いて人間型ロボットの制御を行なった様子を示した図である。

符号の説明

１０…制御システム
１１…力学モデル
１２…目標設定部
１３…仮想的外力算出部
１３−１…操作空間物理量算出部
１３−２…線形相補性問題解決器
１４…接触部位検出手段
１５…実在力変換部
１５−１…２次計画問題解決器
１６…トルク検出手段
１７…トルク・フィードバック制御部

Claims (12)

1. 複数の剛体リンクが関節を介して連接して構成され、関節アクチュエータにアクチュエータ力を発生させることによって駆動するリンク構造物を制御する制御システムであって、
   前記リンク構造物の幾何学的パラメータ並びに動力学的パラメータからなる力学モデルと、
   前記リンク構造物に課される目標値を設定する目標値設定部と、
   前記リンク構造物が前記目標値設定部によって設定された目標値を実現する際に前記力学モデルに作用する仮想的な力を算出する仮想的外力算出手段と、
   前記リンク構造物と外界との接触部位情報を検出する接触部位情報検出手段と、
   前記接触部位情報検出手段によって検出された接触部位情報を拘束条件として用いて、前記仮想的外力算出手段が算出した仮想的な力を実在可能な外力と前記関節アクチュエータのアクチュエータ力に変換する実在力変換手段とを備え、
   前記実在力変換手段が出力するアクチュエータ力を前記関節アクチュエータに発生させることを特徴とする制御システム。
2. 前記接触部位情報検出手段は、前記リンク構造物と外界との接触部位を漏れなく検出するために前記リンク構造物の機体表面全体にわたって分布状に配備された接触センサからなる、
   ことを特徴とする請求項１に記載の制御システム。
3. 前記関節に発生するトルクを検出するトルク検出手段をさらに備え、
   前記実在力変換手段が出力するアクチュエータ力と前記トルク検出手段が検出したトルクの偏差が縮小するように制御する、
   ことを特徴とする請求項１に記載の制御システム。
4. 前記実在力変換手段は、前記接触部位情報検出手段によって検出された接触部位における、前記リンク構造物形状の法線ベクトル方向に実在可能な外力を設定する、
   ことを特徴とする請求項１に記載の制御システム。
5. 前記実在力変換手段は、前記接触部位情報検出手段によって検出された接触部位における、前記リンク構造物形状の接線ベクトル方向に実在可能な外力を設定する、
   ことを特徴とする請求項１に記載の制御システム。
6. 前記接触部位情報は、外力の作用点及び方向である、
   ことを特徴とする請求項１に記載の制御システム。
7. 前記実在力変換手段は、前記拘束条件の下に前記仮想的な力と前記実在可能な外力との最適化問題を解くことで、前記アクチュエータ力を算出する、
   ことを特徴とする請求項１に記載の制御システム。
8. 複数の剛体リンクが関節を介して連接して構成され、関節アクチュエータにアクチュエータ力を発生させることによって駆動するリンク構造物を制御する制御方法であって、
   前記リンク構造物の幾何学的パラメータ並びに動力学的パラメータからなる力学モデルを保持するステップと、
   前記リンク構造物に課される目標値を設定する目標値設定ステップと、
   前記リンク構造物が前記目標値設定部によって設定された目標値を実現する際に前記力学モデルに作用する仮想的な力を算出する仮想的外力算出ステップと、
   前記リンク構造物と外界との接触部位情報を検出する接触部位情報検出ステップと、
   前記接触部位情報検出ステップによって検出された接触部位情報を拘束条件として用いて、前記仮想的外力算出ステップにより算出された仮想的な力を実在可能な外力と前記関節アクチュエータのアクチュエータ力に変換する実在力変換ステップとを備え、
   前記実在力変換ステップで得られたアクチュエータ力を前記関節アクチュエータに発生させることを特徴とする制御方法。
9. 前記関節に発生するトルクを検出するトルク検出ステップをさらに備え、
   前記実在力変換ステップが出力するアクチュエータ力と前記トルク検出ステップが検出したトルクの偏差が縮小するように制御する、
   ことを特徴とする請求項８に記載の制御方法。
10. 前記実在力変換ステップでは、前記接触部位情報検出ステップによって検出された接触部位における、前記リンク構造物形状の法線ベクトル方向に実在可能な外力を設定する、
    ことを特徴とする請求項８に記載の制御方法。
11. 前記実在力変換ステップでは、前記接触部位情報検出ステップによって検出された接触部位における、前記リンク構造物形状の接線ベクトル方向に実在可能な外力を設定する、
    ことを特徴とする請求項８に記載の制御方法。
12. 複数の剛体リンクが連なって構成されたリンク構造物からなるロボット装置であって、
    リンク間を連接する関節を駆動する関節アクチュエータと、
    前記関節に発生するトルクを検出するトルク検出手段と、
    前記リンク構造物の幾何学的パラメータ並びに動力学的パラメータからなる力学モデルと、
    前記リンク構造物に課される目標値を設定する目標値設定部と、
    前記リンク構造物が前記目標値設定部によって設定された目標値を実現する際に前記力学モデルに作用する仮想的な力を算出する仮想的外力算出手段と、
    前記リンク構造物と外界との接触部位情報を検出する接触部位情報検出手段と、
    前記接触部位情報検出手段によって検出された接触部位情報を拘束条件として用いて、前記仮想的外力算出手段が算出した仮想的な力を実在可能な外力と前記関節アクチュエータのアクチュエータ力に変換する実在力変換手段と、
    前記実在力変換手段が出力するアクチュエータ力を前記関節アクチュエータに発生させるアクチュエータ駆動手段と、
    前記実在力変換手段が出力するアクチュエータ力と前記トルク検出手段が検出したトルクの偏差が縮小するように制御するトルク補償制御手段と、
    を具備することを特徴とするロボット装置。