ロボット制御システム、ロボットシステム
【課題】制御ハードウェアーへの性能要求を低減できると共に応答性の改善も図れるロボット制御システム、ロボットシステム等の提供。
【解決手段】ロボット制御システムは、ロボット１００のフィードバック制御の目標値を出力する目標値出力部６０と、力覚センサー１０からのセンサー情報に基づいて力制御を行い、目標値の補正値を出力する力制御部２０と、補正値により補正された目標値に基づいて、ロボットのフィードバック制御を行うロボット制御部８０を含む。力制御部２０は、ロボットの姿勢に応じて変化する制御パラメーターを事前計算する事前計算部２２と、センサー情報と制御パラメーターとに基づき補正値を求める演算処理を実行する実行部２６を含む。
【解決手段】ロボット制御システムは、ロボット１００のフィードバック制御の目標値を出力する目標値出力部６０と、力覚センサー１０からのセンサー情報に基づいて力制御を行い、目標値の補正値を出力する力制御部２０と、補正値により補正された目標値に基づいて、ロボットのフィードバック制御を行うロボット制御部８０を含む。力制御部２０は、ロボットの姿勢に応じて変化する制御パラメーターを事前計算する事前計算部２２と、センサー情報と制御パラメーターとに基づき補正値を求める演算処理を実行する実行部２６を含む。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボット制御システム、ロボットシステム等に関する。
【背景技術】
【０００２】
マニピュレーター等のロボットを用いて行う作業において、種々の拘束条件、例えば物体との接触を伴う作業がある。このような場合においては、位置の制御に加えて、力の制御が要求される場合が多い。例えば、物体の表面をなぞる場合、１つの物体を他の物体に嵌め合わせる場合、柔軟物を破壊しないように把持する場合などにおいては、単なる位置の制御に加えて、物体からの反力に対応した動きが必要となる。
【０００３】
ロボットにおいて力制御を行う代表的な手法としては、インピーダンス制御と呼ばれる手法がある。インピーダンス制御とは、ロボットを、その実際の質量や粘性特性や弾性特性に関わらず、あたかも作業に適したそれらの値を持つかのように動作させる制御手法である。これは、ロボットに取り付けられた力覚センサーなどから得られる力情報に基づいて運動方程式を解き、ロボットをその解に従って動作させる制御手法である。この運動方程式を適切に設定することにより、マニピュレーター等のロボットを、あたかも所定の質量、粘性、弾性を持っているかのごとく動作させることが可能になる。このようなインピーダンス制御や力制御に関する従来技術としては、特許文献１、２、３に開示される技術が知られている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平６−３２０４５１号公報
【特許文献２】特開平２−２０５４８９号公報
【特許文献３】特開平４−３６９００４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
特許文献１には、制御対象が柔軟である場合のような、非常に複雑な系においても、精度良くインピーダンス制御を行う手法が開示されている。
【０００６】
特許文献２には、力覚センサーを用いずに力を推定し、より単純な機械的な構造により、インピーダンス制御を行う手法が開示されている。
【０００７】
これらの特許文献１、２の手法は、より複雑な系での制御精度の改善、或いは、より単純な系でのインピーダンス制御を実現するものであるが、その代償として、リアルタイム性が要求される多くの演算が必要になるという問題がある。
【０００８】
例えば、特許文献１の手法では、力制御の１ループごとに、順ダイナミクスモデルと逆ダイナミクスモデルの計算を必要とする。また特許文献２の手法では、力制御の１ループごとに力を推定するために、その明細書に記載された式の演算を行う必要がある。
【０００９】
一方、特許文献３には、演算量を削減するための１つの手法が開示されている。これは、上記の特許文献１、２に記載される逆ヤコビアン行列を求める計算を行わずに、転置ヤコビアン行列を用いることにより、計算量を削減するものである。
【００１０】
しかしながら、この特許文献３の手法においても、リアルタイム性が要求される多くの演算が必要になるという問題がある。また、所望の反力を得るまでにマニピュレーターが加速してしまう事態が生じるおそれがあるため、異常時の対応にも問題がある。
【００１１】
本発明の幾つかの態様によれば、制御ハードウェアーへの性能要求を低減できると共に応答性の改善も図れるロボット制御システム、ロボットシステム等を提供できる。
【課題を解決するための手段】
【００１２】
本発明の一態様は、ロボットのフィードバック制御の目標値を出力する目標値出力部と、力覚センサーからのセンサー情報に基づいて力制御を行い、前記目標値の補正値を出力する力制御部と、前記補正値により補正された前記目標値に基づいて、前記ロボットの前記フィードバック制御を行うロボット制御部とを含み、前記力制御部は、前記ロボットの姿勢に応じて変化する制御パラメーターを事前計算する事前計算部と、前記センサー情報と前記制御パラメーターとに基づき前記補正値を求める演算処理を実行する実行部とを含むロボット制御システムに関係する。
【００１３】
本発明の一態様によれば、ロボットの姿勢に応じて変化する制御パラメーターの事前計算が行われ、力覚センサーからのセンサー情報と、事前計算された制御パラメーターとに基づき補正値を求める演算処理が実行される。そして、得られた補正値に基づいて目標値が補正され、補正された目標値に基づいて、ロボットのフィードバック制御が行われる。このようにすれば、実行部での演算処理の実行タイミング前に、ロボットの姿勢に応じて変化する制御パラメーターが、事前計算されるようになる。従って、制御ハードウェアーへの性能要求を低減できると共に応答性の改善も図れるロボット制御システムの提供が可能になる。
【００１４】
また本発明の一態様では、力制御部は、前記事前計算部で計算された前記制御パラメーターが格納されるＦＩＦＯを含み、前記実行部は、前記ＦＩＦＯから前記制御パラメーターを読み出し、読み出された前記制御パラメーターに基づき前記補正値を求める前記演算処理を実行してもよい。
【００１５】
このようにすれば、事前計算部は、事前計算した制御パラメーターをＦＩＦＯに格納した後は、他の処理を行ったり、処理を停止することなどが可能になるため、処理の効率化等を図れる。
【００１６】
また本発明の一態様では、前記ＦＩＦＯには、前記実行部で使用される前記制御パラメーターが、前記実行部の演算処理が実行されるタイミングごとに、先入れ先出し方式で記憶されてもよい。
【００１７】
このようにすれば、実行部は、各演算処理が実行されるタイミングにおいて、制御パラメーターをＦＩＦＯから読み出すだけで、制御パラメーターを用いた演算処理を実行できるようになる。
【００１８】
また本発明の一態様では、前記実行部は、前記ＦＩＦＯの残りデータ量が所与のしきい値以下になった場合に、前記事前計算部に対して、前記制御パラメーターを事前計算することを要求する事前計算要求を行ってもよい。
【００１９】
このようにすれば、事前計算部は、少なくともＦＩＦＯの残りデータ量が無くなるまでの期間に、制御パラメーターを事前計算すれば済むようになるため、事前計算部に対するリアルタイム処理要求の緩和等を図れる。
【００２０】
また本発明の一態様では、前記事前計算部は、前記制御パラメーターとして、逆ヤコビアンの行列要素を事前計算し、前記実行部は、事前計算された前記逆ヤコビアンの前記行列要素に基づいて、前記目標値の前記補正値を求めてもよい。
【００２１】
このようにすれば、逆ヤコビアンの行列要素については事前計算されるようになり、実行部は、逆ヤコビアンの行列要素を演算する必要がなくなるため、実行部に対する性能要求等を軽減できる。
【００２２】
また本発明の一態様では、前記事前計算部は、前記制御パラメーターとして、計画軌道に対するロボット軌道の許容範囲を示すリミッター値を事前計算し、前記実行部は、前記計画軌道に対して前記ロボット軌道が前記許容範囲を超えたと判断した場合に、前記許容範囲を超えたことを示す割り込みを発生させてもよい。
【００２３】
このようにすれば、ロボット軌道の許容範囲を示すリミッター値については事前計算されるようになり、実行部は、リミッター値を演算する必要がなくなるため、実行部に対する性能要求等を軽減できる。また、ロボット軌道が許容範囲を超えた場合には、割り込みが発生するようになり、適切なエラー処理等を実行できるようになる。
【００２４】
また本発明の一態様では、前記事前計算部は、前記計画軌道の周辺のヤコビアンを求めることで、前記リミッター値を事前計算してもよい。
【００２５】
このようにすれば、ロボットの姿勢に応じて許容範囲が変化する場合にも、許容範囲を示すリミッター値を適切に事前計算できるようになる。
【００２６】
また本発明の一態様では、前記事前計算部は、前記制御パラメーターとして、前記力覚センサーの前記センサー情報を補正するための補正係数を事前計算し、前記実行部は、前記補正係数に基づいて前記センサー情報の補正処理を行ってもよい。
【００２７】
このようにすれば、センサー情報を補正するための補正係数については事前計算されるようになり、実行部は、補正係数を演算する必要がなくなるため、実行部に対する性能要求等を軽減できる。
【００２８】
また本発明の一態様では、前記目標値出力部は、前記ロボットの軌道情報を出力する軌道生成部と、前記軌道生成部からの前記軌道情報に基づいてインバースキネマティクス処理を行い、前記ロボットの関節角情報を前記目標値として出力するインバースキネマティクス処理部とを含んでもよい。
【００２９】
このようにすれば、軌道生成部からの軌道情報に基づいて、インバースキネマティクス処理によりロボットの関節角情報を求め、求められた関節角情報を目標値として、ロボットのフィードバック制御を実行できるようになる。
【００３０】
また本発明の一態様では、前記力制御部は、前記力制御としてインピーダンス制御を行ってもよい。
【００３１】
また本発明の一態様では、前記目標値は、前記ロボットの関節角情報であり、前記補正値は、前記関節角情報の補正値であってもよい。
【００３２】
また本発明の一態様では、上記のいずれかに記載のロボット制御システムと、前記ロボットとを含むロボットシステムに関係する。
【図面の簡単な説明】
【００３３】
【図１】本実施形態のロボット制御システム及びロボットシステムの構成例。
【図２】本実施形態のロボットシステムの一例。
【図３】図３（Ａ）〜図３（Ｃ）は力制御についての説明図。
【図４】図４（Ａ）、図４（Ｂ）はコンプライアンス制御についての説明図。
【図５】図５（Ａ）、図５（Ｂ）はインピーダンス制御についての説明図。
【図６】図６（Ａ）、図６（Ｂ）はインバースキネマティクスについての説明図。
【図７】図７（Ａ）、図７（Ｂ）はヤコビアンについての説明図。
【図８】ヤコビアンによる制御手法についての説明図。
【図９】ヤコビアンによる制御手法についての説明図。
【図１０】力覚フィードバックを含まない場合の制御系の基本構成例。
【図１１】力覚フィードバックを含む場合の制御系の基本構成例。
【図１２】本実施形態のロボット制御システムの制御系の基本構成例。
【図１３】本実施形態のロボット制御システムの詳細な構成例。
【図１４】ＦＩＦＯを用いた本実施形態の処理の説明図。
【図１５】本実施形態のロボット制御システムの更に詳細な構成例。
【図１６】本実施形態の詳細な処理を示すフローチャート。
【図１７】本実施形態の詳細な処理を示すフローチャート。
【図１８】図１８（Ａ）〜図１８（Ｃ）は本実施形態の詳細な処理を示すフローチャート。
【図１９】本実施形態の詳細な処理を示すフローチャート。
【図２０】本実施形態の詳細な処理を示すフローチャート。
【発明を実施するための形態】
【００３４】
以下、本発明の好適な実施の形態について詳細に説明する。なお以下に説明する本実施形態は特許請求の範囲に記載された本発明の内容を不当に限定するものではなく、本実施形態で説明される構成の全てが本発明の解決手段として必須であるとは限らない。
【００３５】
１．構成
本実施形態のロボット制御システム（マニピュレーター制御システム）及びこれを含むロボットシステムの構成例を図１に示す。なお本実施形態のロボット制御システム、ロボットシステムは図１の構成には限定されず、その一部の構成要素を省略したり、他の構成要素を追加するなどの種々の変形実施が可能である。
【００３６】
本実施形態のロボット制御システムは、力制御部２０、目標値出力部６０、ロボット制御部８０を含む。また本実施形態のロボットシステムは、このロボット制御システムと、ロボット１００（力覚センサー１０）により構成される。
【００３７】
目標値出力部６０は、ロボット（狭義にはマニピュレーター）のフィードバック制御の目標値を出力する。この目標値に基づいてロボット１００のフィードバック制御が実現される。多関節ロボット等を例に取れば、この目標値は、ロボットの関節角情報などである。ロボットの関節角情報は、例えばロボットのアームのリンク機構における各関節の角度（ジョイント軸とジョイント軸のなす角度）を表す情報である。
【００３８】
目標値出力部６０は、軌道生成部６２とインバースキネマティクス処理部６４を含むことができる。軌道生成部６２は、ロボットの軌道情報を出力する。軌道情報は、ロボットのエンドエフェクター部（エンドポイント）の位置情報（ｘ，ｙ，ｚ）と、各座標軸回りでの回転角度情報（ｕ，ｖ，ｗ）を含むことができる。インバースキネマティクス処理部６４は、軌道生成部６２からの軌道情報に基づいてインバースキネマティクス処理を行い、例えばロボットの関節角情報を目標値として出力する。インバースキネマティクス処理は、関節を有するロボットの動きを計算する処理であり、ロボットのエンドエフェクター部の位置姿勢などから関節角情報等を逆運動学により計算する処理である。
【００３９】
力制御部２０（狭義にはインピーダンス制御部）は、力覚センサー１０からのセンサー情報に基づいて力制御（力覚制御）を行って、目標値の補正値を出力する。更に具体的には、力制御部２０（インピーダンス制御部）は、力覚センサー１０からのセンサー情報（力情報、モーメント情報）に基づいてインピーダンス制御（或いはコンプライアンス制御）を行う。力制御は、例えば、従来の位置制御に対して、力のフィードバックが加わった制御である。インピーダンス制御は、外力に対するエンドエフェクター部（手先）の変位の生じやすさ（機械インピーダンス）を、制御により望ましい状態にする手法である。具体的には、ロボットのエンドエフェクター部の質量に粘性係数と弾性要素が接続されるモデルにおいて、目標として設定した粘性係数と弾性係数で物体に接触するようにする制御である。また力覚センサー１０は、ロボット１００が出している力の反力として受けている力やモーメントを検出するセンサーである。この力覚センサー１０は、通常、ロボット１００のアームの手首部分に取り付けられ、検出された力やモーメントは、センサー情報として、各種の力制御（インピーダンス制御）に用いられる。
【００４０】
ロボット制御部８０は、目標値出力部６０からの目標値に基づいて、ロボットのフィードバック制御を行う。具体的には、力制御部２０からの補正値により補正された目標値に基づいて、ロボットのフィードバック制御を行う。例えば補正値により補正された目標値と、ロボット１００からのフィードバック信号に基づいて、ロボット１００のフィードバック制御を行う。例えばロボット制御部８０は、複数の駆動制御部８２-1〜８２-N（狭義にはモーター制御部）を含み、ロボット１００が有する駆動部１０２-1〜１０２-Nに対して、その制御信号を出力する。ここで駆動部１０２-1〜１０２-Nは、ロボット１００の各関節を動かすための駆動機構であり、例えばモーター等により実現される。
【００４１】
そして本実施形態では、力制御部２０が、事前計算部２２と実行部２６を含む。事前計算部２２は、ロボット１００の姿勢に応じて変化する制御パラメーター（演算パラメーター）を事前計算する。そして実行部２６（ハードリアルタイム処理部）は、センサー情報と制御パラメーターとに基づき補正値を求める演算処理を実行する。即ち力覚センサー１０からのセンサー情報と、事前計算された制御パラメーターとに基づいて、目標値の補正値を求め、例えば目標値出力部６０に出力する。これにより、力制御により補正された目標値に基づいて、ロボット１００のフィードバック制御が行われるようになる。
【００４２】
このように本実施形態では、ロボットの姿勢に応じて変化する制御パラメーターが事前計算され、この事前計算された制御パラメーターに基づいて、各制御タイミングでの補正値が求められる。そして、この補正値により補正された目標値に基づいてロボットのフィードバック制御が行われる。このように、ロボット制御に必要な制御パラメーターを、リアルタイム応答性が必要とされない事前計算により求めるようにすれば、制御ハードウェアーへの性能要求を低減できると共に、ロボット制御の応答性等も改善できるようになる。なお、事前計算は、例えば、実際に制御パラメーターが必要となるタイミングよりも前のタイミングで、ロボットの計画軌道等に基づいて制御パラメーターを予め計算しておくことである。
【００４３】
また力制御部２０は、事前計算部２２で計算された制御パラメーターが格納されるＦＩＦＯ（First-In First-Out）２４を含む。このＦＩＦＯ２４は、先入れ先出しのメモリー（レジスター）などにより実現できる。このＦＩＦＯ２４には、実行部２６で使用される制御パラメーターが、実行部２６の演算処理が実行されるタイミングごとに、先入れ先出し方式で記憶される。
【００４４】
そして実行部２６は、ＦＩＦＯ２４から制御パラメーターを読み出し、読み出された制御パラメーターに基づき補正値を求める演算処理を実行する。即ち本実施形態では、事前計算された制御パラメーターをＦＩＦＯ２４に格納しておく。そして実行部２６は、各演算処理実行タイミングにおいて、ＦＩＦＯ２４からの制御パラメーターを読み出して、補正値を求める演算処理を実行する。
【００４５】
この場合に実行部２６は、ＦＩＦＯ２４の残りデータ量が所与のしきい値以下（所与のデータ量以下）になった場合に、事前計算部２２に対して、制御パラメーターを事前計算することを要求する事前計算要求を行う。この事前計算要求により、事前計算部２２は、制御パラメーターの事前計算を行い、事前計算された制御パラメーターをＦＩＦＯ２４に格納する。
【００４６】
ここで、制御パラメーターは、補正値の演算処理に使用されるパラメーター（演算パラメーター）であり、例えばロボット１００の姿勢に応じてその値が変化するパラメーターである。この制御パラメーターとしては、逆ヤコビアンの行列要素や、ロボット軌道の許容範囲を示すリミッター値や、力覚センサー１０のセンサー情報を補正するための補正係数などの種々のパラメーターを想定できる。
【００４７】
例えば事前計算部２２は、制御パラメーターとして、逆ヤコビアンの行列要素を事前計算する。そして実行部２６は、事前計算された逆ヤコビアンの行列要素に基づいて、目標値の補正値を求める。また事前計算部２２は、制御パラメーターとして、計画軌道に対するロボット軌道の許容範囲を示すリミッター値を事前計算する。そして実行部２６は、計画軌道に対してロボット軌道が許容範囲を超えたと判断した場合に、許容範囲を超えたことを示す割り込みを発生させる。この場合に事前計算部２２は、計画軌道の周辺のヤコビアン（計画軌道からの所与の範囲のポイントでのヤコビアン）を求めることで、リミッター値を事前計算する。また事前計算部２２は、制御パラメーターとして、力覚センサー１０のセンサー情報を補正するための補正係数（力覚センサー補正値）を事前計算する。そして実行部２６は、補正係数に基づいてセンサー情報の補正処理を行う。この補正処理後のセンサー情報に基づいて、目標値の補正値が求められる。なお、逆ヤコビアン、リミッター値、補正係数の詳細については後述する。なお、ロボット軌道は、例えばエンドエフェクター部（エンドポイント）の移動軌道であり、計画軌道は、制御系が予め予定しているロボット軌道である。
【００４８】
図２に本実施形態のロボット制御システムを含むロボットシステムの例を示す。このロボットシステムは、制御装置３００（情報処理装置）とロボット３１０（図１のロボット１００）とを含む。制御装置３００はロボット３１０の制御処理を行う。具体的には、動作シーケンス情報（シナリオ情報）に基づいてロボット３１０を動作させる制御を行う。ロボット３１０は、アーム３２０及びハンド（把持部）３３０を有する。そして制御装置３００からの動作指示にしたがって動作する。例えば、図示しないパレットに載せられたワークを把持したり、移動するなどの動作を行う。また、図示しない撮像装置で取得された撮像画像情報に基づいて、ロボットの姿勢やワークの位置などの情報が検出され、検出された情報が制御装置３００に送られる。
【００４９】
本実施形態のロボット制御システムは例えば図２の制御装置３００に設けられ、例えば制御装置３００のハードウェアーやプログラムによりロボット制御システムが実現される。そして本実施形態のロボット制御システムによれば、制御装置３００などの制御ハードウェアーに対する性能要求を低減できると共に、ロボット３１０を高い応答性で動作させることが可能になる。なお図２は単腕型の例であるが、双腕型等の多腕型のロボットであってもよい。
【００５０】
２．本実施形態の手法
次に本実施形態のロボット制御手法について詳細に説明する。
【００５１】
２．１ 概要
本実施形態では、リアルタイム応答性の必要とされる部分を最小化することにより、応答性を改善し、また制御ハードウェアーの性能要求を低減することを目的としている。即ち、ロボット（マニピュレーター）に用いられる力制御、インピーダンス制御を、単純で低コストな制御系で実現する。現状の制御系が高コストである原因は、高速なハードリアルタイム性を要求される部分が大きいからであり、高コストが許容されるのは、現在のシステム全体が高価なものであるからである。従って、低コスト化のためには、高速なリアルタイム性が要求される部分を小さくし、性能要求を緩和することが必要である。
【００５２】
このような本実施形態の基本的なアイデアは次のような事実による。つまり、インピーダンス制御を用いようとも、その軌道は予測可能である場合が多いということである。
【００５３】
例えば、双腕又は単腕のロボット（マニピュレーター）が対象物体をインピーダンス制御しながら運搬するというような場合、その軌道は予測可能である。
【００５４】
例えば、ロボットを用いてある物体を別の物体に嵌め合わせるような場合、或いは、溶接等のようにロボットがある物体の表面をなぞる場合を考える。このような場合に、従来手法においても、全くの手探りということはなく、治具を用いてある程度の位置が決まっているか、或いは近年の場合には撮像装置を用いて位置を決める場合がほとんどである。この場合も、その位置、軌道はほぼ予測可能である。
【００５５】
要するに、従来の場合も近年の場合も、インピーダンス制御を用いる場合に、その軌道が全くの手探りということは非常に少なく、むしろ多くの場合は事前に予測可能である。つまり、インピーダンス制御などを用い、外部拘束による軌道変化は非常に小さい。
【００５６】
このように、主たる軌道が予測可能であり、インピーダンス制御などによる軌道変化が小さいものであるならば、それに最適な構造・制御手法を採用することにより、応答性を改善すると共に、制御ハードウェアーの性能要求を低減することが可能になる。つまり、インピーダンス制御に関わるヤコビアンや逆ヤコビアンは、ロボットの姿勢に依存するものである。しかしながら、軌道が予測可能であるならば、その軌道上のある点について、ヤコビアンや逆ヤコビアンを予め計算しておくことが可能となる。この部分はリアルタイム応答性を必要としない。
【００５７】
その結果、各点での値が予め計算されていることによる応答性の改善や、その値の事前計算にはリアルタイム応答性を必要としないことによるハードウェアへの性能要求の低減が可能となる。
【００５８】
以上の本実施形態の基本的アイデアをまとめると、以下の通りである。
【００５９】
即ち、従来の場合、逐次に変化する軌道に対し、姿勢の変化に対応して変化するパラメーターを、その都度、時間遅れなく計算するする必要があった。しかしながら、人や他の機器との時間的に変化する相互作用のある場合を除き、インピーダンス制御を行っても、軌道は予想可能である。産業用機械のタスクのうちのかなりの部分は、これに該当する。つまり、産業用機械において、予測不能な外力により軌道が変化する事態は、異常な事態である。
【００６０】
軌道が予測可能であれば、高速リアルタイム性が要求される部分のうち、事前計算が可能な部分がかなり存在する。この事前計算を予め行っておき、ＦＩＦＯ（先入れ先出しメモリ）に入れておく。ＦＩＦＯの長さを適当に取ることにより、事前計算処理のリアルタイム性は非常に緩和される。なお、リアルタイム処理とは、応答時間の最悪値（イベント発生後に実際に応答が起こるまでの時間の最悪値）が保証されている処理である。そして、その応答の価値が時間経過に対してゆっくりと下がるのがソフトリアルタイム処理であり、急激に下がるのがハードリアルタイム処理である。例えばロボット制御やエアーバック制御の処理は、ハードリアルタイム処理になる。
【００６１】
本実施形態では、事前計算を、ＦＩＦＯが空にならないような時間間隔で行う。高速なハードリアルタイム処理を行う部分は、ＦＩＦＯから制御パラメーターを読み出して、その制御パラメーターを用いる。
【００６２】
また、ハードリアルタイム処理の結果が、事前計算において想定していた範囲を超えた場合は、それを事前計算部へ通知する。また、指定のエラー処理を行う。
【００６３】
具体的には本実施形態では、従来では一体型であったインピーダンス制御部を、上位制御層と下位制御層に分割する。即ち図１に示すように、上位制御層である事前計算部２２と、下位制御層のハードリアルタイム処理部である実行部２６と、これらの間に介在するＦＩＦＯ２４に分割する。ここで実行部２６とＦＩＦＯ２４は各関節ごとに与えられる。なお、前述した特許文献３でも階層分割を行っているが、その内容は本実施形態とは全く異なる。
【００６４】
また、これらの各制御層での処理の時間間隔が異ならせる。即ち、リアルタイム応答を行う下位制御層である実行部２６は、例えば１００μｓ〜１ｍｓの時間間隔で処理を行う.一方、まとめて事前計算を行う上位制御層である事前計算部２２は、例えば数ｍｓ〜数十ｍｓの時間間隔で処理を行う。つまり、マルチレートの処理を行う。
【００６５】
上位制御層でまとめて事前計算された値は、先入れ先出しメモリであるＦＩＦＯ２４に送られ、下位制御層は、事前計算された値を逐次的にＦＩＦＯから読み出して、ハードリアルタイム処理を行う。
【００６６】
ここで、事前計算部２２で事前計算され、ＦＩＦＯ２４を通じて通信されるパラメーターは、例えば、後に詳述する逆ヤコビアン、リミッター値、センサー姿勢の補正係数（ハンド自重補正係数）である。
【００６７】
事前計算部２２は、上記のパラメーターの事前計算を行う。即ち、上位制御層である事前計算部２２は、ＦＩＦＯ２４の状態を見ながら、適切な間隔で事前計算を行う。この適切な間隔とは、ＦＩＦＯ２４が空にならないように、余裕を持って事前計算が行える程度であればよい。具体的には、事前計算部２２は、ＦＩＦＯ２４の残りデータ量が少なくなった時に起動される。これにより事前計算部２２に対するリアルタイム処理要求は緩和される。そして、ハードリアルタイム処理部である実行部２６は、センサー姿勢の補正係数、逆ヤコビアンを用いてインピーダンス制御処理を行う。そして実行部２６は、インピーダンス制御処理の結果が、リミッター値の範囲を超えた場合に、事前計算部２２に対してエラーを通知する。そして実行部２６は、エラーの発生時に、所定のエラー処理を行う。
【００６８】
なお、本実施形態では、軌道は予測可能であることを仮定しており、リミッター値は、その仮定から外れるほどの大きなインピーダンス制御による軌道変化があったことを検出するためのものである。このような状態となった場合には、例えば上位制御層への割り込み処理を要求するようにする。
【００６９】
以上の本実施形態の手法によれば、パラメーターが予め事前計算されていることによる応答性の改善と、その事前計算にはリアルタイム応答性を必要としないことによる制御ハードウェアーへの性能要求の低減を実現できるようになる。
【００７０】
２．２ 力制御、インピーダンス制御
次に、力制御、インピーダンス制御（コンプライアンス制御）の概要について説明する。
【００７１】
図３（Ａ）は、ロボットの左のアームＡＬ、右のアームＡＲで物体ＯＢを挟んで移動している様子を示している。例えば、位置制御だけでは、物体を落としたり、破壊してしまうおそれがある、力制御によれば、柔軟な物体や脆い物体を、図３（Ａ）のように両側から適切な力で挟んで移動させることが可能になる。
【００７２】
また、力制御によれば、図３（Ｂ）に示すように、不確実性のある物体の表面ＳＦをアームＡＭ等でなぞることが可能になる。このような制御は位置制御だけでは実現不能である。また、力制御によれば、図３（Ｃ）に示すように、粗い位置決めの後に、探って位置合わせをして、物体ＯＢを穴部ＨＬにはめ込むことも可能になる。
【００７３】
しかしながら、バネなどの実際の機械部品による力制御では、用途が限られるという問題がある。また、このような機械部品による力制御では、特性の動的な切り替えが困難である。
【００７４】
一方、モーターのトルクを制御するトルク制御は簡単であるが、位置精度が悪くなるという問題がある。また、異常時に衝突などの問題が生じる。例えば図３（Ａ）において、異常事態が起きて、物体ＯＢを落としてしまった場合に、トルク制御では、釣合うべき反力が無くなるため、左右のアームＡＬ、ＡＲが衝突してしまうなどの問題が生じる。
【００７５】
これに対して、インピーダンス制御（コンプライアンス制御）は、制御が複雑であるという不利点があるものの、汎用性や安全性が高いという利点がある。
【００７６】
図４（Ａ）、図４（Ｂ）は、インピーダンス制御の１つであるコンプライアンス制御を説明する図である。コンプライアンスはバネ定数の逆数を意味し、バネ定数が硬さを表すのに対して、コンプライアンスは柔らかさを意味する。ロボットと環境との間に相互作用が働くときに、機械的柔軟性であるコンプライアンスを与える制御をコンプライアンス制御と呼ぶ。
【００７７】
例えば図４（Ａ）では、ロボットのアームＡＭには力覚センサーＳＥが取り付けられている。このロボットのアームＡＭは、力覚センサーＳＥで得られたセンサー情報（力・トルク情報）に応じて姿勢が変わるようにプログラムされている。具体的には、図４（Ａ）のＡ１に示す仮想的なバネが、あたかもアームＡＭの先端に取り付けられているかのようにロボットを制御する。
【００７８】
例えばＡ１に示すバネのバネ定数が１００Ｋｇ／ｍであったとする。これを図４（Ｂ）のＡ２に示すように５Ｋｇの力で押せば、Ａ３に示すようにバネは５ｃｍだけ縮む。逆に言えば、５ｃｍだけ縮んでいれば、５Ｋｇの力で押されているといえる。つまり、力情報と位置情報とが対応づけられている。
【００７９】
コンプライアンス制御では、このＡ１に示す仮想的なバネがアームＡＭの先端に取り付けられているかのような制御が行われる。具体的には、ロボットは、力覚センサーＳＥの入力に応答して動作し、Ａ２に示す５Ｋｇの加重に対して、Ａ３に示すように５ｃｍだけ後退するように制御され、力情報に対応して位置情報が変化するように制御される。
【００８０】
このような単純なコンプライアンス制御では時間項を含まないが、時間項を含み、その２次の項までを考慮した制御が、インピーダンス制御である。具体的には、２次の項は質量項であり、１次の項は粘性項であり、インピーダンス制御のモデルは下式（１）に示すような運動方程式で表すことができる。
【００８１】
【数１】
【００８２】
上式（１）において、ｍは質量、μは粘性係数、ｋは弾性係数、ｆは力、ｘは目標位置からの変位である。またｘの１次微分、２次微分は、各々、速度、加速度に対応する。インピーダンス制御では、上式（１）の特性をアームの先端であるエンドエフェクター部に持たせるための制御系を構成する。即ち上式（１）で表される仮想質量、仮想粘性係数、仮想弾性係数を、あたかもアームの先端が持っているかのように制御を行う。
【００８３】
このように、インピーダンス制御は、アームの先端の質量に粘性要素と弾性要素が各方向に接続されるモデルにおいて、目的として設定された粘性係数と弾性係数で物体に接触するようにする制御である。
【００８４】
例えば図５（Ａ）に示すように、ロボットのアームＡＬ、ＡＲで物体ＯＢをつかんで、軌道ＴＲに沿って移動させる制御を考える。この場合に、軌道ＴＲＬは、物体ＯＢの左側の内側に設定された点ＰＬが通る軌道であり、インピーダンス制御を想定して決定された仮想的な左手の軌道である。また軌道ＴＲＲは、物体ＯＢの右側の内側に設定された点ＰＲが通る軌道であり、インピーダンス制御を想定して決定された仮想的な右手の軌道である。この場合に、アームＡＬは、アームＡＬの先端と点ＰＬの距離差に応じた力が発生するように制御される。またアームＡＲは、アームＡＲの先端と点ＰＲの距離差に応じた力が発生するように制御される。このようにすれば、物体ＯＢを柔らかくつかみながら移動させるインピーダンス制御を実現できる。そして、インピーダンス制御では、図５（Ａ）のＢ１に示すように物体ＯＢが落下する事態が生じたとしても、アームＡＬ、ＡＲは、Ｂ２、Ｂ３に示すように、その先端が点ＰＬ、ＰＲの位置で止まるように制御される。即ち、仮想的な軌道が衝突軌道でなければ、アームＡＬ、ＡＲが衝突するのを防止できる。
【００８５】
また図５（Ｂ）のように、物体の表面ＳＦをなぞるように制御する場合にも、インピーダンス制御では、アームＡＭの先端に対して、仮想的な軌道ＴＲＶＡと先端との距離差ＤＦに応じた力が働くように制御される。従って、アームＡＭを、力を加えながら表面ＳＦをなぞるような制御が可能になる。
【００８６】
２．３ ヤコビアン、逆ヤコビアン
以上のようなインピーダンス制御、コンプライアンス制御等においては、その動作は、基本的にはデカルト座標系で計算される。そして、デカルト座標系でのロボットの手先の動きを、ロボットの各関節の動きに翻訳するためにはインバースキネマティクス処理が必要となる。例えば図６（Ａ）において、Ｃ１に示す力が働いて、アームＡＭの先端ＰＴがＣ２に示すように移動した場合に、各関節Ｊ１、Ｊ２の角度を求めるためには、インバースキネマティクス処理が必要になる。
【００８７】
しかしながら、図６（Ｂ）に示すように、一般的には、インバースキネマティクス処理は非常に難しい処理である。一方、ロボットの各関節の値を設定し、それを手先の動きに変換する処理であるフォワードキネマティクス処理は簡単な処理である。このような処理において、姿勢変化が微小であれば、フォワードキネマティクスを用いたヤコビアンを用いることができる。
【００８８】
例えば下式（２）に示すように、ｍ個の変数を入力とし、ｎ個の出力を与えるある関数ｆを考える。或いは、ｍ個の変数を入力とし、１つの出力を与える関数ｆをｎ組用意する。
【００８９】
【数２】
【００９０】
この場合に、ヤコビアン（ヤコビ行列）は、下式（３）に表される行列である。
【００９１】
【数３】
【００９２】
つまり、ヤコビアンは、ｘの「ある値」の近傍において、ｘとｙの関係を線形近似したものである。従って、一般的には、ｘの「ある値」が変化すれば、ヤコビアンも変化する。
【００９３】
ここで、ｘを関節角、ｙをロボットの手先の姿勢（xyzuvw）とすれば、上式（２）の関数は、フォワードキネマティクス関数となり、これは簡単に求めることができる。
【００９４】
上式（２）の関数をフォワードキネマティクス関数と考えれば、上式（３）のヤコビアン（ヤコビ行列）は、ある姿勢において各関節が微小に変化した場合の、それに伴う手先の微小な変化を与える関係式となる。即ち図７（Ａ）において、関節Ｊ１の角度が微小な単位量だけ変位した場合に、先端ＰＴがどれだけ変位するかを表す関係式になる。或いは図７（Ｂ）において、関節Ｊ２の角度が微小な単位量だけ変位した場合に、先端ＰＴがどれだけ変位するかを表す関係式になる。
【００９５】
この場合に、各関節角の微小変化と、手先の微小変化との対応は、その姿勢に応じて変化する。つまり、ヤコビアンは、姿勢の変化に対応して、その都度、計算する必要がある。
【００９６】
さて、前述したように、手先の位置から関節角を求めるインバースキネマティクス処理は、一般的には非常に難しい処理である。
【００９７】
しかしながら、下式（４）のように、ヤコビアンの逆行列である逆ヤコビアンを求めることができれば、手先の微小な変化を各関節角の微小な変化に変換する関係式を求めることができる。
【００９８】
【数４】
【００９９】
ところが、一般的には、ヤコビアンは正方行列ではなく、且つ、ｒａｎｋ（階数）も低く、通常の意味の逆行列は存在しない。例えば７軸のロボットでは、手先の自由度は６つしかないので、７×７ではなく、７×６の行列になってしまう。但し、通常の意味の逆行列と類似した性質を持つ一般化逆行列は存在し得る。この一般化逆行列を用いることにより、インバースキネマティクス関数を必要とする処理を、フォワードキネマティクス関数を用いて実行することが可能になる。これを利用して、本実施形態では、処理の簡素化を図っている。具体的には、制御パラメーターとして、逆ヤコビアンの行列要素を事前計算し、事前計算された逆ヤコビアンの行列要素に基づいて、目標値の補正値を求めている。
【０１００】
２．４ リミッター値
さて、図８において、計画軌道ＴＲＰに沿ってアームＡＭの先端ＰＴを移動させる場合を考える。この場合に、産業用途のロボットにおいては、ロボット軌道が、軌道ＴＲＡのように大きくずれることはなく、このような軌道ＴＲＡになった場合には、それは異常状態であると判断できる。
【０１０１】
そこで本実施形態では、制御パラメーターとして、計画軌道ＴＲＰに対するロボット軌道の許容範囲ＲＴを示すリミッター値（例えばモーターのエンコーダー値のリミッター値）を事前計算する。そして計画軌道ＴＲＰに対してロボット軌道が許容範囲ＲＴを超えたか否かを判断する。そして許容範囲を超えた場合には、それを示す割り込みを発生させる。
【０１０２】
具体的には本実施形態では、計画軌道ＴＲＰの周辺のヤコビアンを求めることで、リミッター値を事前計算する。即ち、ヤコビアンの誤差が許容される範囲を、許容範囲ＲＴとして、リミッター値を事前計算する。例えばヤコビアンの行列要素の誤差が所与の値以下になる範囲を許容範囲ＲＴとして、リミッター値を事前計算する。
【０１０３】
この場合に、図８に示すように、許容範囲ＲＴは一定ではなく、許容範囲ＲＴの大きさはロボットのアームＡＭの姿勢に依存する。例えば図８のＤ１に示す姿勢の場合とＤ２に示す姿勢の場合とで、許容範囲ＲＴは同一ではない。そしてＤ１とＤ２とで、ヤコビアン（逆ヤコビアン）の値も異なる。このため本実施形態では、計画軌道ＴＲＰの各点の周辺においてヤコビアンを求めることで（各姿勢におけるヤコビアンを求めることで）、リミッター値を事前計算するようにしている。
【０１０４】
なお、この場合にヤコビアンを計算する姿勢は、図９において実線で示す姿勢であり、破線で示す姿勢ではない。即ち、図５（Ａ）で説明したように、インピーダンス制御においては、物体の内側に設定された点ＰＬ、ＰＲを通る仮想的な軌道を目標として制御が行われる。例えば図９のアームＡＭの先端ＰＴと仮想点ＶＰＴ（ＰＬ、ＰＲ）との差に応じた力が発生するように制御される。そしてヤコビアンの計算時には、このような仮想的な軌道に対応する破線で示す姿勢ではなく、実線で示す姿勢においてヤコビアンを計算することになる。
【０１０５】
２．５ 制御系の構成
図１０に、力覚フィードバックを含まない場合の制御系の基本構成例を示す。
【０１０６】
軌道生成部５６２は、軌道情報ｐ（xyzuvw）を生成してインバースキネマティクス処理部５６４に出力する。ここで軌道情報ｐは、例えばアームの先端（エンドエフェクター部）の位置情報(xyz)と各軸回りの回転情報（uvw）を含む。そしてインバースキネマティクス処理部５６４は、この軌道情報ｐに基づいてインバースキネマティクス処理を行って、目標値である各関節の関節角θを生成して出力する。そして、この関節角θに基づいて、モーター制御を行うことで、ロボットのアームの動作制御が行われる。この場合に図１０のモーター（Ｍ）の制御は、公知のＰＩＤ制御により実現されている。このＰＩＤ制御は公知の技術であるため、ここでは詳細な説明を省略する。
【０１０７】
図１０において、軌道生成部５６２とインバースキネマティクス処理部５６４とにより目標値出力部が構成される。この目標値出力部の処理は、ロボットの全体的な処理になる。一方、後段のモーター制御は、関節毎の制御になる。
【０１０８】
なお、前述した特許文献３の従来技術においても、上位制御層と下位制御層の区分分割を行っている。しかしながら、特許文献３における上位制御層は、図１０の軌道生成部５６２とインバースキネマティクス処理部５６４の部分であり、下位制御層は、モーター制御の部分であり、本実施形態とは異なる区分分割となっている。
【０１０９】
図１１に、力覚フィードバックを含む場合の制御系の基本構成例を示す。図１１では、図１０に対して、更に、力覚センサー５１０と、姿勢補正部５３２と、ハンド・ツール自重補正部５３４と、運動方程式処理部５３６と、フォワードキネマティクス処理部５４０が設けられている。
【０１１０】
図１１では、力覚センサー５１０からのセンサー情報を受けて、姿勢補正部５３２がセンサーの姿勢補正を行い、ハンド・ツール自重補正部５３４がハンド・ツール自重補正を行う。そして、運動方程式処理部５３６が、前述の式（１）に示すような運動方程式の解を求める処理を行い、補正値Δｐを出力する。この補正値Δｐにより軌道情報ｐが補正されることで、目標値である関節角θの補正処理が行われる。またフォワードキネマティクス処理部５４０は、フォワードキネマティクス処理を行って、ロボットの軌道情報ｐ’を求めて軌道生成部５６２にフィードバックする。また姿勢補正部５３２、ハンド・ツール補正部５３４に対して姿勢を特定するための情報を出力する。
【０１１１】
図１２に、本実施形態の制御系の基本構成例を示す。この制御系は、図１１と同様に力覚フィードバックを含む。図１２が図１１と異なるのは、図１２では逆ヤコビアン処理部３８が更に設けられている点である。
【０１１２】
具体的には、図１１では、運動方程式処理部５３６からのΔｐにより、軌道生成部５６２が出力するｐが補正されている。これに対して図１２では、運動方程式処理部５３６からのΔｐに基づいて、逆ヤコビアン処理部３８が、Δθを求める逆ヤコビアン処理を行う。即ち、前述の式（４）において、Δｙ（Δｙ１，Δｙ２・・・）がΔｐになり、Δｘ（Δｘ１，Δｘ２・・・）がΔθになる逆ヤコビアン処理を行って、Δθを求める。そして、求められたΔθにより、目標値である関節角θが補正される。このような構成にすることにより、運動方程式の処理結果を関節ごとに分解できるようになる。
【０１１３】
そして図１２において、姿勢補正部３２、ハンド・ツール自重補正部３４、逆ヤコビアン処理部３８、フォワードキネマティクス処理部４０の処理では、ロボット姿勢に応じて、使用されるパラメーターの値が変化する。
【０１１４】
しかしながら、ロボットの姿勢変化が予めプログラムされたものであるか、或いはそこからの差異が小さければ、これらのパラメーター値は軌道をプログラムする時点で予め計算しておくことができる。そこで本実施形態では、これらのパラメーター値を事前計算で求める手法を採用している。
【０１１５】
また、上述の「差異が小さい」とは、各補正における誤差が、運動方程式の解の誤差となった時に、その誤差が許容範囲内にあるということであり、またその結果のヤコビアン処理の結果の誤差が許容範囲内にあるということである。
【０１１６】
この「差異が小さい」ということは、上記のように、ロボットの実際の姿勢とプログラムされた姿勢との差異が小さいということに依存するものであり、その差異が想定よりも大きなものとなった場合には、エラーとして処理する必要がある。そこで本実施形態では、図８で説明したように許容範囲ＲＴに対応するリミッター値を事前計算しておき、ロボット軌道が許容範囲ＲＴを超えたと判断された場合には、割り込みを発生させるようにしている。
【０１１７】
２．６ 詳細な構成例
図１３に本実施形態のロボット制御システムの詳細な構成例を示す。図１３は、図１２で説明した制御系に対応する構成である。なお本実施形態のロボット制御システムは図１３の構成には限定されず、その一部の構成要素を省略したり、他の構成要素を追加するなどの種々の変形実施が可能である。例えば本実施形態のロボット制御システムとして、図１１等で説明した制御系の構成を採用してもよい。
【０１１８】
図１３の逆ヤコビアン事前計算部４２、リミッター値事前計算部４４、補正係数事前計算部４６が、図１の事前計算部２２に相当する。また図１３のＦＩＦＯ５０、５２、５４が、図１のＦＩＦＯ２４に相当する。また図１３のハンド・ツール自重・姿勢補正部３５、運動方程式処理部３６、逆ヤコビアン処理部３８、リミッター処理部３９、割り込み発生部４８が、図１の実行部２６に相当する。なおＦＩＦＯ５６はインバースキネマティクス処理部６４からの関節角θを格納するためのＦＩＦＯである。
【０１１９】
逆ヤコビアン事前計算部４２は、前述の式（４）の逆ヤコビアン（逆ヤコビアンの行列要素）を事前計算し、事前計算した逆ヤコビアン（逆ヤコビアンに対応する値）をＦＩＦＯ５０に格納する。そして逆ヤコビアン処理部３８は、各演算処理実行タイミングにおいて、事前計算された逆ヤコビアンをＦＩＦＯ５０から読み出して、前述の式（４）に示すような逆ヤコビアン処理を行って、関節角を補正するための補正値を出力する。
【０１２０】
リミッター値事前計算部４４は、図８で説明したロボット軌道の許容範囲に対応するリミッター値を事前計算し、事前計算したリミッター値をＦＩＦＯ５２に格納する。そしてリミッター処理部３９は、各演算処理実行タイミングにおいて、事前計算されたリミッター値をＦＩＦＯ５２から読み出して、計画軌道に対してロボット軌道が許容範囲を超えたか否かを判断する。そして、許容範囲を超えたと判断されると、割り込み発生部４８が、許容範囲を超えたことを示す割り込みを発生する。
【０１２１】
補正係数事前計算部４６は、力覚センサー１０のセンサー情報を補正するための補正係数を事前計算し、事前計算した補正係数をＦＩＦＯ５４に格納する。そしてハンド・ツール自重・姿勢補正部３５は、事前計算された補正係数に基づいてセンサー情報の補正処理を行う。そして運動方程式処理部３６は、この補正処理後のセンサー情報を用いて、前述の式（１）に対応する運動方程式の解を求める処理を行い、処理結果を逆ヤコビアン処理部３８に出力する。すると、逆ヤコビアン処理部３８は、この運動方程式処理部３８からの処理結果と、事前計算された逆ヤコビアンに基づいて、前述の式（４）に示す逆ヤコビアン処理を行って、関節角の補正値を生成する。
【０１２２】
ここで、ハンド・ツール自重・姿勢補正部３５は、図１２のハンド・ツール自重補正部３４及び姿勢補正部３２に対応するものである。ここでハンド・ツール自重補正は、ロボットのハンドの自重や、ハンドがつかむツールの自重による影響を、力覚センサー１０からのセンサー情報（力情報）から相殺するための補正処理である。また姿勢補正は、力覚センサー１０の姿勢による影響を、センサー情報（力情報）から相殺するための補正処理である。これらのハンド・ツール自重補正及び姿勢補正は、例えば下式（５）のように表すことができる。
【０１２３】
【数５】
【０１２４】
上式（５）において、Ｆｘ，Ｆｙ，Ｆｚ、Ｆｕ，Ｆｖ，Ｆｗは力覚センサー１０からのセンサー情報である力情報、トルク情報である。またＢｘ，Ｂｙ，Ｂｚ、Ｂｕ，Ｂｖ，Ｂｗはバイアス項である。そして、補正後のセンサー情報（力情報、トルク情報）であるｆｘ，ｆｙ，ｆｚ、ｆｕ，ｆｖ，ｆｗが運動方程式処理部３６に入力される。なお、データには固定値があるため、実質的な補正係数は６×７＝４２個となる。これらのハンド・ツール自重補正及び姿勢補正は公知の補正処理であるため、詳しい説明は省略する。
【０１２５】
図１４は、ＦＩＦＯを用いた本実施形態の処理を説明するための図である。図１４において、ＴＭ１、ＴＭ２、ＴＭ３・・・は、実行部２６（ハンド・ツール自重・姿勢補正部３５、運動方程式処理部３６、逆ヤコビアン処理部３８、リミッター処理部３９等）の演算処理の実行タイミングである。このように、本実施形態では、実行部２６は、ＴＭ１、ＴＭ２、ＴＭ３・・・の各演算処理実行タイミングにおいて、事前計算された制御パラメーター（逆ヤコビアン、リミッター値、補正係数）をＦＩＦＯ（ＦＩＦＯ５０、５２、５４）から読み出す。
【０１２６】
そして図１４のＥ１では、ＦＩＦＯの残りデータ量（制御パラメーターの残りデータ量）がしきい値以下になったため、下位制御層である実行部２６は、Ｅ２に示すように、上位制御層である事前計算部２２（逆ヤコビアン事前計算部４２、リミッター値事前計算部４４、補正係数事前計算部４６）に対して、制御パラメーターの事前計算要求を行う。すると、事前計算部２２が、制御パラメーターの事前計算を行い、Ｅ３に示すように、事前計算された制御パラメーターをＦＩＦＯに格納する。
【０１２７】
この場合に、ＦＩＦＯには、以前に事前計算した制御パラメーターが残存しているため、Ｅ４に示すタイミングについては、ＴＭ１、ＴＭ２、ＴＭ３・・・の演算処理実行タイミングに比べて、十分に長いインターバルを取ることができる。従って、実行部２６に対しては、高速且つハードなリアルタイム処理が要求されるものの、事前計算部２２に対しては、ある程度の処理の遅延が許容されるため、低速又はソフトなリアルタイム処理が要求されるだけになる。従って、事前計算部２２は、逆ヤコビアンやリミッター値や補正係数を求めるというような負荷の高い計算処理を、時間的に余裕を持って実行することが可能になる。一方、実行部２６では、逆ヤコビアンやリミッター値や補正係数を求めるというような負荷の高い計算処理を行わなくも済むため、高速且つハードなリアルタイム処理が要求されても、これに対応することが可能になる。
【０１２８】
なお図１５は、ロボット制御部８０と実行部２６の接続関係の例を示す図である。図１５では、図１のロボット制御部８０が制御するモーター（広義には駆動部）が２つの場合を示している。
【０１２９】
インバースキネマティクス処理部６４からの目標値（関節角）は、ＦＩＦＯ５６-1を介してモーター制御部８４-1に入力される。この場合に、この目標値は、逆ヤコビアン処理部３８からの補正値により補正されて、モーター制御部８４-1に入力される。そしてモーター制御部８４-1は、補正後の目標値に基づいて、モーターＭ１の制御を行う。
【０１３０】
同様に、インバースキネマティクス処理部６４からの目標値（関節角）は、ＦＩＦＯ５６-2を介してモーター制御部８４-2に入力される。この場合に、この目標値は、逆ヤコビアン処理部３８からの補正値により補正されて、モーター制御部８４-2に入力される。そしてモーター制御部８４-2は、補正後の目標値に基づいて、モーターＭ２の制御を行う。
【０１３１】
また、リミッター値事前計算部４４で事前計算されたリミッター値は、ＦＩＦＯ５２-1を介してリミッター処理部３９-1に入力される。リミッター処理部３９-1は、事前計算されたリミッター値と、モーター制御部８４-1からのエンコード値（回転角度検出情報）に基づいて、ロボット軌道が許容範囲を超えたか否かを判断する。そして、許容範囲を超えたと判断された場合には、割り込み発生部４８が割り込みを発生する。
【０１３２】
同様に、リミッター値事前計算部４４で事前計算されたリミッター値は、ＦＩＦＯ５２-2を介してリミッター処理部３９-2に入力される。リミッター処理部３９-2は、事前計算されたリミッター値と、モーター制御部８４-2からのエンコード値に基づいて、ロボット軌道が許容範囲を超えたか否かを判断する。そして、許容範囲を超えたと判断された場合には、割り込み発生部４８が割り込みを発生する。
【０１３３】
３．詳細な処理
次に本実施形態の詳細な処理例について図１６〜図２０のフローチャートを用いて説明する。
【０１３４】
図１６は、ハードリアルタイム処理部である実行部の全体的な処理についてのフローチャートである。
【０１３５】
まず、制御パラメーターを更新するタイミングか否かを判断する（ステップＳ１）。そして、更新タイミングである場合には、図１４で説明したように、ＦＩＦＯに残りデータ（制御パラメーター）があるか否かを判断する（ステップＳ２）。そして、残りデータが無い場合には、制御パラメーター無しの割り込みを発生させて（ステップＳ３）、割り込み処理へ移行する（ステップＳ４）。一方、ＦＩＦＯに残りデータがある場合には、ＦＩＦＯから制御パラメーターを取得する（ステップＳ５）。
【０１３６】
次に、ＦＩＦＯの残りデータ量はしきい値よりも多いか否かを判断する（ステップＳ６）。そして、しきい値以下である場合には、図１４で説明したように、事前計算部に対して制御パラメーターの作成を要求する（ステップＳ７）。一方、残りデータ量がしきい値よりも多い場合には、インピーダンス制御処理を実行する（ステップＳ８）。
【０１３７】
次に、インピーダンス制御処理が正常に終了したか否かを判断する（ステップＳ９）。そして、正常に終了しなかった場合には、事前計算部に対して制御パラメーターの更新を要求する（ステップＳ１０）。一方、正常に終了した場合にはステップＳ１に戻る。
【０１３８】
図１７は、図１６のステップＳ８のインピーダンス制御処理についてのフローチャートである。
【０１３９】
まず、力覚センサーから６次元入力値を得て（ステップＳ１１）、７×７の補正行列との積を計算する（ステップＳ１２）。即ち、前述の式（５）のＦｘ，Ｆｙ，Ｆｚ、Ｆｕ，Ｆｖ，Ｆｗを得て、ハンド・自重補正及び姿勢補正を行う。そして、得られた補正済み値に基づいてインピーダンスフィルター処理を行う（ステップＳ１３）。即ち、前述の式（１）の運動方程式の処理を行う。
【０１４０】
次に、インピーダンスフィルターの出力値と、７×６のヤコビ行列との積を求める（ステップＳ１４）。即ち、前述の式（４）の行列演算を行う。そして、各関節の指令値に対して、ステップＳ１４で得られた値を加える補正処理を行う（ステップＳ１５）。
【０１４１】
次に、モーターのエンコーダー値（回転角度）を得る（ステップＳ１６）。そして、エンコーダー値はリミッター値の範囲内か否かを判断し（ステップＳ１７）、リミッター値の範囲内でない場合には、リミッターエラー割り込みを発生する（ステップＳ１８）。一方、リミッター値の範囲内である場合には、正常状態であるとして図１６のＬｏｏｐへ戻る。
【０１４２】
図１８（Ａ）〜図１８（Ｃ）は割り込み処理についてのフローチャートである。
【０１４３】
図１８（Ａ）に示すように、図１６のステップＳ３で制御パラメーター無し割り込みが発生した場合には、予め設定された停止処理に入る（ステップＳ２１）。
【０１４４】
図１８（Ｂ）に示すように、図１７のステップＳ１８でリミッターエラー割り込みが発生した場合には、予め設定された停止処理に入る（ステップＳ２２）。即ち、上位コントローラー（上位制御層）で、許容範囲のリミッター値を設定し、それを超えるような事態では、ともかく停止させる。
【０１４５】
或いは、図１８（Ｃ）に示すように、図１７のステップＳ１８でリミッターエラー割り込みが発生した場合に、ＮＯＰ（No OPeration）とし、異常事態発生としてＬｏｏｐに戻るようにしてもよい（ステップＳ２３）。即ち、上位コントローラー（上位制御層）で、余裕を見込んだリミッター値を設定し、下位コントローラー（下位制御層）のオーバーシュートを許容する。そして、割り込みを受けた上位コントローラーは、軌道を更新し、ＦＩＦＯを新しいデータで上書きする。一方、下位コントローラーは、新しいデータで動作を継続する。そして、上位コントローラーが軌道を更新できない場合には、下位コントローラーへの割り込みを発生させて、動作を停止させる。
【０１４６】
図１９は事前計算部の処理についてのフローチャートである。
【０１４７】
まず、事前計算部は、割り込みが発生するのを待ち（ステップＳ３１）、割り込みが発生した場合には、制御パラメーター無し割り込みか否かを判断する（ステップＳ３２）。そして、制御パラメーター無し割り込みの場合には、制御パラメーターを計算して、ＦＩＦＯに設定する（ステップＳ３３）。そして、ハードリアルタイム処理部（実行部）を再起動する（ステップＳ３４）。
【０１４８】
一方、制御パラメーター無し割り込みではない場合には、制御パラメーター作成割り込みか否かを判断する（ステップＳ３５）。そして、制御パラメーター作成割り込みの場合には、ＦＩＦＯの空きデータ量を取得し（ステップＳ３６）、制御パラメーターを計算して、ＦＩＦＯに設定する（ステップＳ３７）。
【０１４９】
制御パラメーター作成割り込みではない場合には、制御パラメーター更新割り込みか否かを判断する（ステップＳ３８）。そして、制御パラメーター更新割り込みの場合には、現在のロボットの姿勢を取得し、復旧軌道を求める（ステップＳ３９）。そして、制御パラメーターを計算して、ＦＩＦＯに上書きする（ステップＳ４０）。
【０１５０】
図２０は、パラメーター計算処理についてのフローチャートである。
【０１５１】
まず、ＦＩＦＯに空きはあるか否かを判断し（ステップＳ４１）、空きがある場合には、計画軌道を求める（ステップＳ４２）。そして、計画軌道のヤコビアン、逆ヤコビアンを求め（ステップＳ４３）、ＦＩＦＯに逆ヤコビアンの値を設定する（ステップＳ４４）。
【０１５２】
次に、図８で説明したように、計画軌道の周辺のヤコビアンを求め、差異が所定の値以下である範囲を求め（ステップＳ４５）、ＦＩＦＯにリミッター値を設定する（ステップＳ４６）。また、計画軌道に対する力覚センサー補正値（補正係数）を求め（ステップＳ４７）、ＦＩＦＯに力覚センサー補正値を設定する（ステップＳ４８）。
【０１５３】
なお、上記のように本実施形態について詳細に説明したが、本発明の新規事項および効果から実体的に逸脱しない多くの変形が可能であることは当業者には容易に理解できるであろう。従って、このような変形例はすべて本発明の範囲に含まれるものとする。例えば、明細書又は図面において、少なくとも一度、より広義または同義な異なる用語と共に記載された用語は、明細書又は図面のいかなる箇所においても、その異なる用語に置き換えることができる。また本実施形態及び変形例の全ての組み合わせも、本発明の範囲に含まれる。またロボット制御システム、ロボットシステムの構成・動作や、制御パラメーターの事前計算手法、ＦＩＦＯへの制御パラメーターの格納手法、制御パラメーターに基づく演算処理の実行手法、制御系の構成等も、本実施形態で説明したものに限定されず、種々の変形実施が可能である。
【符号の説明】
【０１５４】
ＯＢ 物体、ＡＬ、ＡＲ、ＡＭ アーム、ＳＦ 表面、ＨＬ 穴部、Ｊ１〜Ｊ３ 関節、
ＰＴ 先端部、Ｍ、Ｍ１、Ｍ２ モーター、
１０ 力覚センサー、２０ 力制御部（インピーダンス制御部）、２２ 事前計算部、
２４ ＦＩＦＯ、２６ 実行部、３２ 姿勢補正部、３４ ハンド・ツール自重補正部、
３５ ハンド・ツール自重・姿勢補正部、３６ 運動方程式処理部、
３８ 逆ヤコビアン処理部、３９、３９-1、３９-2 リミッター処理部、
４０ フォワードキネマティクス処理部、４２ 逆ヤコビアン事前計算部、
４４ リミッター値事前計算部、４６ 補正係数事前計算部、
５０、５２、５２-1、５２-2、５４、５６、５６-1、５６-2 ＦＩＦＯ、
６０ 目標値出力部、６２ 軌道生成部、６４ インバースキネマティクス処理部、
８０ ロボット制御部、８２-1〜８２-N 駆動制御部、
８４-1、８４-2 モーター制御部、１００ ロボット、１０２-1〜１０２-N 駆動部、
３００ 制御装置、３１０ ロボット、３２０ アーム、３３０ ハンド
【技術分野】
【０００１】
本発明は、ロボット制御システム、ロボットシステム等に関する。
【背景技術】
【０００２】
マニピュレーター等のロボットを用いて行う作業において、種々の拘束条件、例えば物体との接触を伴う作業がある。このような場合においては、位置の制御に加えて、力の制御が要求される場合が多い。例えば、物体の表面をなぞる場合、１つの物体を他の物体に嵌め合わせる場合、柔軟物を破壊しないように把持する場合などにおいては、単なる位置の制御に加えて、物体からの反力に対応した動きが必要となる。
【０００３】
ロボットにおいて力制御を行う代表的な手法としては、インピーダンス制御と呼ばれる手法がある。インピーダンス制御とは、ロボットを、その実際の質量や粘性特性や弾性特性に関わらず、あたかも作業に適したそれらの値を持つかのように動作させる制御手法である。これは、ロボットに取り付けられた力覚センサーなどから得られる力情報に基づいて運動方程式を解き、ロボットをその解に従って動作させる制御手法である。この運動方程式を適切に設定することにより、マニピュレーター等のロボットを、あたかも所定の質量、粘性、弾性を持っているかのごとく動作させることが可能になる。このようなインピーダンス制御や力制御に関する従来技術としては、特許文献１、２、３に開示される技術が知られている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平６−３２０４５１号公報
【特許文献２】特開平２−２０５４８９号公報
【特許文献３】特開平４−３６９００４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
特許文献１には、制御対象が柔軟である場合のような、非常に複雑な系においても、精度良くインピーダンス制御を行う手法が開示されている。
【０００６】
特許文献２には、力覚センサーを用いずに力を推定し、より単純な機械的な構造により、インピーダンス制御を行う手法が開示されている。
【０００７】
これらの特許文献１、２の手法は、より複雑な系での制御精度の改善、或いは、より単純な系でのインピーダンス制御を実現するものであるが、その代償として、リアルタイム性が要求される多くの演算が必要になるという問題がある。
【０００８】
例えば、特許文献１の手法では、力制御の１ループごとに、順ダイナミクスモデルと逆ダイナミクスモデルの計算を必要とする。また特許文献２の手法では、力制御の１ループごとに力を推定するために、その明細書に記載された式の演算を行う必要がある。
【０００９】
一方、特許文献３には、演算量を削減するための１つの手法が開示されている。これは、上記の特許文献１、２に記載される逆ヤコビアン行列を求める計算を行わずに、転置ヤコビアン行列を用いることにより、計算量を削減するものである。
【００１０】
しかしながら、この特許文献３の手法においても、リアルタイム性が要求される多くの演算が必要になるという問題がある。また、所望の反力を得るまでにマニピュレーターが加速してしまう事態が生じるおそれがあるため、異常時の対応にも問題がある。
【００１１】
本発明の幾つかの態様によれば、制御ハードウェアーへの性能要求を低減できると共に応答性の改善も図れるロボット制御システム、ロボットシステム等を提供できる。
【課題を解決するための手段】
【００１２】
本発明の一態様は、ロボットのフィードバック制御の目標値を出力する目標値出力部と、力覚センサーからのセンサー情報に基づいて力制御を行い、前記目標値の補正値を出力する力制御部と、前記補正値により補正された前記目標値に基づいて、前記ロボットの前記フィードバック制御を行うロボット制御部とを含み、前記力制御部は、前記ロボットの姿勢に応じて変化する制御パラメーターを事前計算する事前計算部と、前記センサー情報と前記制御パラメーターとに基づき前記補正値を求める演算処理を実行する実行部とを含むロボット制御システムに関係する。
【００１３】
本発明の一態様によれば、ロボットの姿勢に応じて変化する制御パラメーターの事前計算が行われ、力覚センサーからのセンサー情報と、事前計算された制御パラメーターとに基づき補正値を求める演算処理が実行される。そして、得られた補正値に基づいて目標値が補正され、補正された目標値に基づいて、ロボットのフィードバック制御が行われる。このようにすれば、実行部での演算処理の実行タイミング前に、ロボットの姿勢に応じて変化する制御パラメーターが、事前計算されるようになる。従って、制御ハードウェアーへの性能要求を低減できると共に応答性の改善も図れるロボット制御システムの提供が可能になる。
【００１４】
また本発明の一態様では、力制御部は、前記事前計算部で計算された前記制御パラメーターが格納されるＦＩＦＯを含み、前記実行部は、前記ＦＩＦＯから前記制御パラメーターを読み出し、読み出された前記制御パラメーターに基づき前記補正値を求める前記演算処理を実行してもよい。
【００１５】
このようにすれば、事前計算部は、事前計算した制御パラメーターをＦＩＦＯに格納した後は、他の処理を行ったり、処理を停止することなどが可能になるため、処理の効率化等を図れる。
【００１６】
また本発明の一態様では、前記ＦＩＦＯには、前記実行部で使用される前記制御パラメーターが、前記実行部の演算処理が実行されるタイミングごとに、先入れ先出し方式で記憶されてもよい。
【００１７】
このようにすれば、実行部は、各演算処理が実行されるタイミングにおいて、制御パラメーターをＦＩＦＯから読み出すだけで、制御パラメーターを用いた演算処理を実行できるようになる。
【００１８】
また本発明の一態様では、前記実行部は、前記ＦＩＦＯの残りデータ量が所与のしきい値以下になった場合に、前記事前計算部に対して、前記制御パラメーターを事前計算することを要求する事前計算要求を行ってもよい。
【００１９】
このようにすれば、事前計算部は、少なくともＦＩＦＯの残りデータ量が無くなるまでの期間に、制御パラメーターを事前計算すれば済むようになるため、事前計算部に対するリアルタイム処理要求の緩和等を図れる。
【００２０】
また本発明の一態様では、前記事前計算部は、前記制御パラメーターとして、逆ヤコビアンの行列要素を事前計算し、前記実行部は、事前計算された前記逆ヤコビアンの前記行列要素に基づいて、前記目標値の前記補正値を求めてもよい。
【００２１】
このようにすれば、逆ヤコビアンの行列要素については事前計算されるようになり、実行部は、逆ヤコビアンの行列要素を演算する必要がなくなるため、実行部に対する性能要求等を軽減できる。
【００２２】
また本発明の一態様では、前記事前計算部は、前記制御パラメーターとして、計画軌道に対するロボット軌道の許容範囲を示すリミッター値を事前計算し、前記実行部は、前記計画軌道に対して前記ロボット軌道が前記許容範囲を超えたと判断した場合に、前記許容範囲を超えたことを示す割り込みを発生させてもよい。
【００２３】
このようにすれば、ロボット軌道の許容範囲を示すリミッター値については事前計算されるようになり、実行部は、リミッター値を演算する必要がなくなるため、実行部に対する性能要求等を軽減できる。また、ロボット軌道が許容範囲を超えた場合には、割り込みが発生するようになり、適切なエラー処理等を実行できるようになる。
【００２４】
また本発明の一態様では、前記事前計算部は、前記計画軌道の周辺のヤコビアンを求めることで、前記リミッター値を事前計算してもよい。
【００２５】
このようにすれば、ロボットの姿勢に応じて許容範囲が変化する場合にも、許容範囲を示すリミッター値を適切に事前計算できるようになる。
【００２６】
また本発明の一態様では、前記事前計算部は、前記制御パラメーターとして、前記力覚センサーの前記センサー情報を補正するための補正係数を事前計算し、前記実行部は、前記補正係数に基づいて前記センサー情報の補正処理を行ってもよい。
【００２７】
このようにすれば、センサー情報を補正するための補正係数については事前計算されるようになり、実行部は、補正係数を演算する必要がなくなるため、実行部に対する性能要求等を軽減できる。
【００２８】
また本発明の一態様では、前記目標値出力部は、前記ロボットの軌道情報を出力する軌道生成部と、前記軌道生成部からの前記軌道情報に基づいてインバースキネマティクス処理を行い、前記ロボットの関節角情報を前記目標値として出力するインバースキネマティクス処理部とを含んでもよい。
【００２９】
このようにすれば、軌道生成部からの軌道情報に基づいて、インバースキネマティクス処理によりロボットの関節角情報を求め、求められた関節角情報を目標値として、ロボットのフィードバック制御を実行できるようになる。
【００３０】
また本発明の一態様では、前記力制御部は、前記力制御としてインピーダンス制御を行ってもよい。
【００３１】
また本発明の一態様では、前記目標値は、前記ロボットの関節角情報であり、前記補正値は、前記関節角情報の補正値であってもよい。
【００３２】
また本発明の一態様では、上記のいずれかに記載のロボット制御システムと、前記ロボットとを含むロボットシステムに関係する。
【図面の簡単な説明】
【００３３】
【図１】本実施形態のロボット制御システム及びロボットシステムの構成例。
【図２】本実施形態のロボットシステムの一例。
【図３】図３（Ａ）〜図３（Ｃ）は力制御についての説明図。
【図４】図４（Ａ）、図４（Ｂ）はコンプライアンス制御についての説明図。
【図５】図５（Ａ）、図５（Ｂ）はインピーダンス制御についての説明図。
【図６】図６（Ａ）、図６（Ｂ）はインバースキネマティクスについての説明図。
【図７】図７（Ａ）、図７（Ｂ）はヤコビアンについての説明図。
【図８】ヤコビアンによる制御手法についての説明図。
【図９】ヤコビアンによる制御手法についての説明図。
【図１０】力覚フィードバックを含まない場合の制御系の基本構成例。
【図１１】力覚フィードバックを含む場合の制御系の基本構成例。
【図１２】本実施形態のロボット制御システムの制御系の基本構成例。
【図１３】本実施形態のロボット制御システムの詳細な構成例。
【図１４】ＦＩＦＯを用いた本実施形態の処理の説明図。
【図１５】本実施形態のロボット制御システムの更に詳細な構成例。
【図１６】本実施形態の詳細な処理を示すフローチャート。
【図１７】本実施形態の詳細な処理を示すフローチャート。
【図１８】図１８（Ａ）〜図１８（Ｃ）は本実施形態の詳細な処理を示すフローチャート。
【図１９】本実施形態の詳細な処理を示すフローチャート。
【図２０】本実施形態の詳細な処理を示すフローチャート。
【発明を実施するための形態】
【００３４】
以下、本発明の好適な実施の形態について詳細に説明する。なお以下に説明する本実施形態は特許請求の範囲に記載された本発明の内容を不当に限定するものではなく、本実施形態で説明される構成の全てが本発明の解決手段として必須であるとは限らない。
【００３５】
１．構成
本実施形態のロボット制御システム（マニピュレーター制御システム）及びこれを含むロボットシステムの構成例を図１に示す。なお本実施形態のロボット制御システム、ロボットシステムは図１の構成には限定されず、その一部の構成要素を省略したり、他の構成要素を追加するなどの種々の変形実施が可能である。
【００３６】
本実施形態のロボット制御システムは、力制御部２０、目標値出力部６０、ロボット制御部８０を含む。また本実施形態のロボットシステムは、このロボット制御システムと、ロボット１００（力覚センサー１０）により構成される。
【００３７】
目標値出力部６０は、ロボット（狭義にはマニピュレーター）のフィードバック制御の目標値を出力する。この目標値に基づいてロボット１００のフィードバック制御が実現される。多関節ロボット等を例に取れば、この目標値は、ロボットの関節角情報などである。ロボットの関節角情報は、例えばロボットのアームのリンク機構における各関節の角度（ジョイント軸とジョイント軸のなす角度）を表す情報である。
【００３８】
目標値出力部６０は、軌道生成部６２とインバースキネマティクス処理部６４を含むことができる。軌道生成部６２は、ロボットの軌道情報を出力する。軌道情報は、ロボットのエンドエフェクター部（エンドポイント）の位置情報（ｘ，ｙ，ｚ）と、各座標軸回りでの回転角度情報（ｕ，ｖ，ｗ）を含むことができる。インバースキネマティクス処理部６４は、軌道生成部６２からの軌道情報に基づいてインバースキネマティクス処理を行い、例えばロボットの関節角情報を目標値として出力する。インバースキネマティクス処理は、関節を有するロボットの動きを計算する処理であり、ロボットのエンドエフェクター部の位置姿勢などから関節角情報等を逆運動学により計算する処理である。
【００３９】
力制御部２０（狭義にはインピーダンス制御部）は、力覚センサー１０からのセンサー情報に基づいて力制御（力覚制御）を行って、目標値の補正値を出力する。更に具体的には、力制御部２０（インピーダンス制御部）は、力覚センサー１０からのセンサー情報（力情報、モーメント情報）に基づいてインピーダンス制御（或いはコンプライアンス制御）を行う。力制御は、例えば、従来の位置制御に対して、力のフィードバックが加わった制御である。インピーダンス制御は、外力に対するエンドエフェクター部（手先）の変位の生じやすさ（機械インピーダンス）を、制御により望ましい状態にする手法である。具体的には、ロボットのエンドエフェクター部の質量に粘性係数と弾性要素が接続されるモデルにおいて、目標として設定した粘性係数と弾性係数で物体に接触するようにする制御である。また力覚センサー１０は、ロボット１００が出している力の反力として受けている力やモーメントを検出するセンサーである。この力覚センサー１０は、通常、ロボット１００のアームの手首部分に取り付けられ、検出された力やモーメントは、センサー情報として、各種の力制御（インピーダンス制御）に用いられる。
【００４０】
ロボット制御部８０は、目標値出力部６０からの目標値に基づいて、ロボットのフィードバック制御を行う。具体的には、力制御部２０からの補正値により補正された目標値に基づいて、ロボットのフィードバック制御を行う。例えば補正値により補正された目標値と、ロボット１００からのフィードバック信号に基づいて、ロボット１００のフィードバック制御を行う。例えばロボット制御部８０は、複数の駆動制御部８２-1〜８２-N（狭義にはモーター制御部）を含み、ロボット１００が有する駆動部１０２-1〜１０２-Nに対して、その制御信号を出力する。ここで駆動部１０２-1〜１０２-Nは、ロボット１００の各関節を動かすための駆動機構であり、例えばモーター等により実現される。
【００４１】
そして本実施形態では、力制御部２０が、事前計算部２２と実行部２６を含む。事前計算部２２は、ロボット１００の姿勢に応じて変化する制御パラメーター（演算パラメーター）を事前計算する。そして実行部２６（ハードリアルタイム処理部）は、センサー情報と制御パラメーターとに基づき補正値を求める演算処理を実行する。即ち力覚センサー１０からのセンサー情報と、事前計算された制御パラメーターとに基づいて、目標値の補正値を求め、例えば目標値出力部６０に出力する。これにより、力制御により補正された目標値に基づいて、ロボット１００のフィードバック制御が行われるようになる。
【００４２】
このように本実施形態では、ロボットの姿勢に応じて変化する制御パラメーターが事前計算され、この事前計算された制御パラメーターに基づいて、各制御タイミングでの補正値が求められる。そして、この補正値により補正された目標値に基づいてロボットのフィードバック制御が行われる。このように、ロボット制御に必要な制御パラメーターを、リアルタイム応答性が必要とされない事前計算により求めるようにすれば、制御ハードウェアーへの性能要求を低減できると共に、ロボット制御の応答性等も改善できるようになる。なお、事前計算は、例えば、実際に制御パラメーターが必要となるタイミングよりも前のタイミングで、ロボットの計画軌道等に基づいて制御パラメーターを予め計算しておくことである。
【００４３】
また力制御部２０は、事前計算部２２で計算された制御パラメーターが格納されるＦＩＦＯ（First-In First-Out）２４を含む。このＦＩＦＯ２４は、先入れ先出しのメモリー（レジスター）などにより実現できる。このＦＩＦＯ２４には、実行部２６で使用される制御パラメーターが、実行部２６の演算処理が実行されるタイミングごとに、先入れ先出し方式で記憶される。
【００４４】
そして実行部２６は、ＦＩＦＯ２４から制御パラメーターを読み出し、読み出された制御パラメーターに基づき補正値を求める演算処理を実行する。即ち本実施形態では、事前計算された制御パラメーターをＦＩＦＯ２４に格納しておく。そして実行部２６は、各演算処理実行タイミングにおいて、ＦＩＦＯ２４からの制御パラメーターを読み出して、補正値を求める演算処理を実行する。
【００４５】
この場合に実行部２６は、ＦＩＦＯ２４の残りデータ量が所与のしきい値以下（所与のデータ量以下）になった場合に、事前計算部２２に対して、制御パラメーターを事前計算することを要求する事前計算要求を行う。この事前計算要求により、事前計算部２２は、制御パラメーターの事前計算を行い、事前計算された制御パラメーターをＦＩＦＯ２４に格納する。
【００４６】
ここで、制御パラメーターは、補正値の演算処理に使用されるパラメーター（演算パラメーター）であり、例えばロボット１００の姿勢に応じてその値が変化するパラメーターである。この制御パラメーターとしては、逆ヤコビアンの行列要素や、ロボット軌道の許容範囲を示すリミッター値や、力覚センサー１０のセンサー情報を補正するための補正係数などの種々のパラメーターを想定できる。
【００４７】
例えば事前計算部２２は、制御パラメーターとして、逆ヤコビアンの行列要素を事前計算する。そして実行部２６は、事前計算された逆ヤコビアンの行列要素に基づいて、目標値の補正値を求める。また事前計算部２２は、制御パラメーターとして、計画軌道に対するロボット軌道の許容範囲を示すリミッター値を事前計算する。そして実行部２６は、計画軌道に対してロボット軌道が許容範囲を超えたと判断した場合に、許容範囲を超えたことを示す割り込みを発生させる。この場合に事前計算部２２は、計画軌道の周辺のヤコビアン（計画軌道からの所与の範囲のポイントでのヤコビアン）を求めることで、リミッター値を事前計算する。また事前計算部２２は、制御パラメーターとして、力覚センサー１０のセンサー情報を補正するための補正係数（力覚センサー補正値）を事前計算する。そして実行部２６は、補正係数に基づいてセンサー情報の補正処理を行う。この補正処理後のセンサー情報に基づいて、目標値の補正値が求められる。なお、逆ヤコビアン、リミッター値、補正係数の詳細については後述する。なお、ロボット軌道は、例えばエンドエフェクター部（エンドポイント）の移動軌道であり、計画軌道は、制御系が予め予定しているロボット軌道である。
【００４８】
図２に本実施形態のロボット制御システムを含むロボットシステムの例を示す。このロボットシステムは、制御装置３００（情報処理装置）とロボット３１０（図１のロボット１００）とを含む。制御装置３００はロボット３１０の制御処理を行う。具体的には、動作シーケンス情報（シナリオ情報）に基づいてロボット３１０を動作させる制御を行う。ロボット３１０は、アーム３２０及びハンド（把持部）３３０を有する。そして制御装置３００からの動作指示にしたがって動作する。例えば、図示しないパレットに載せられたワークを把持したり、移動するなどの動作を行う。また、図示しない撮像装置で取得された撮像画像情報に基づいて、ロボットの姿勢やワークの位置などの情報が検出され、検出された情報が制御装置３００に送られる。
【００４９】
本実施形態のロボット制御システムは例えば図２の制御装置３００に設けられ、例えば制御装置３００のハードウェアーやプログラムによりロボット制御システムが実現される。そして本実施形態のロボット制御システムによれば、制御装置３００などの制御ハードウェアーに対する性能要求を低減できると共に、ロボット３１０を高い応答性で動作させることが可能になる。なお図２は単腕型の例であるが、双腕型等の多腕型のロボットであってもよい。
【００５０】
２．本実施形態の手法
次に本実施形態のロボット制御手法について詳細に説明する。
【００５１】
２．１ 概要
本実施形態では、リアルタイム応答性の必要とされる部分を最小化することにより、応答性を改善し、また制御ハードウェアーの性能要求を低減することを目的としている。即ち、ロボット（マニピュレーター）に用いられる力制御、インピーダンス制御を、単純で低コストな制御系で実現する。現状の制御系が高コストである原因は、高速なハードリアルタイム性を要求される部分が大きいからであり、高コストが許容されるのは、現在のシステム全体が高価なものであるからである。従って、低コスト化のためには、高速なリアルタイム性が要求される部分を小さくし、性能要求を緩和することが必要である。
【００５２】
このような本実施形態の基本的なアイデアは次のような事実による。つまり、インピーダンス制御を用いようとも、その軌道は予測可能である場合が多いということである。
【００５３】
例えば、双腕又は単腕のロボット（マニピュレーター）が対象物体をインピーダンス制御しながら運搬するというような場合、その軌道は予測可能である。
【００５４】
例えば、ロボットを用いてある物体を別の物体に嵌め合わせるような場合、或いは、溶接等のようにロボットがある物体の表面をなぞる場合を考える。このような場合に、従来手法においても、全くの手探りということはなく、治具を用いてある程度の位置が決まっているか、或いは近年の場合には撮像装置を用いて位置を決める場合がほとんどである。この場合も、その位置、軌道はほぼ予測可能である。
【００５５】
要するに、従来の場合も近年の場合も、インピーダンス制御を用いる場合に、その軌道が全くの手探りということは非常に少なく、むしろ多くの場合は事前に予測可能である。つまり、インピーダンス制御などを用い、外部拘束による軌道変化は非常に小さい。
【００５６】
このように、主たる軌道が予測可能であり、インピーダンス制御などによる軌道変化が小さいものであるならば、それに最適な構造・制御手法を採用することにより、応答性を改善すると共に、制御ハードウェアーの性能要求を低減することが可能になる。つまり、インピーダンス制御に関わるヤコビアンや逆ヤコビアンは、ロボットの姿勢に依存するものである。しかしながら、軌道が予測可能であるならば、その軌道上のある点について、ヤコビアンや逆ヤコビアンを予め計算しておくことが可能となる。この部分はリアルタイム応答性を必要としない。
【００５７】
その結果、各点での値が予め計算されていることによる応答性の改善や、その値の事前計算にはリアルタイム応答性を必要としないことによるハードウェアへの性能要求の低減が可能となる。
【００５８】
以上の本実施形態の基本的アイデアをまとめると、以下の通りである。
【００５９】
即ち、従来の場合、逐次に変化する軌道に対し、姿勢の変化に対応して変化するパラメーターを、その都度、時間遅れなく計算するする必要があった。しかしながら、人や他の機器との時間的に変化する相互作用のある場合を除き、インピーダンス制御を行っても、軌道は予想可能である。産業用機械のタスクのうちのかなりの部分は、これに該当する。つまり、産業用機械において、予測不能な外力により軌道が変化する事態は、異常な事態である。
【００６０】
軌道が予測可能であれば、高速リアルタイム性が要求される部分のうち、事前計算が可能な部分がかなり存在する。この事前計算を予め行っておき、ＦＩＦＯ（先入れ先出しメモリ）に入れておく。ＦＩＦＯの長さを適当に取ることにより、事前計算処理のリアルタイム性は非常に緩和される。なお、リアルタイム処理とは、応答時間の最悪値（イベント発生後に実際に応答が起こるまでの時間の最悪値）が保証されている処理である。そして、その応答の価値が時間経過に対してゆっくりと下がるのがソフトリアルタイム処理であり、急激に下がるのがハードリアルタイム処理である。例えばロボット制御やエアーバック制御の処理は、ハードリアルタイム処理になる。
【００６１】
本実施形態では、事前計算を、ＦＩＦＯが空にならないような時間間隔で行う。高速なハードリアルタイム処理を行う部分は、ＦＩＦＯから制御パラメーターを読み出して、その制御パラメーターを用いる。
【００６２】
また、ハードリアルタイム処理の結果が、事前計算において想定していた範囲を超えた場合は、それを事前計算部へ通知する。また、指定のエラー処理を行う。
【００６３】
具体的には本実施形態では、従来では一体型であったインピーダンス制御部を、上位制御層と下位制御層に分割する。即ち図１に示すように、上位制御層である事前計算部２２と、下位制御層のハードリアルタイム処理部である実行部２６と、これらの間に介在するＦＩＦＯ２４に分割する。ここで実行部２６とＦＩＦＯ２４は各関節ごとに与えられる。なお、前述した特許文献３でも階層分割を行っているが、その内容は本実施形態とは全く異なる。
【００６４】
また、これらの各制御層での処理の時間間隔が異ならせる。即ち、リアルタイム応答を行う下位制御層である実行部２６は、例えば１００μｓ〜１ｍｓの時間間隔で処理を行う.一方、まとめて事前計算を行う上位制御層である事前計算部２２は、例えば数ｍｓ〜数十ｍｓの時間間隔で処理を行う。つまり、マルチレートの処理を行う。
【００６５】
上位制御層でまとめて事前計算された値は、先入れ先出しメモリであるＦＩＦＯ２４に送られ、下位制御層は、事前計算された値を逐次的にＦＩＦＯから読み出して、ハードリアルタイム処理を行う。
【００６６】
ここで、事前計算部２２で事前計算され、ＦＩＦＯ２４を通じて通信されるパラメーターは、例えば、後に詳述する逆ヤコビアン、リミッター値、センサー姿勢の補正係数（ハンド自重補正係数）である。
【００６７】
事前計算部２２は、上記のパラメーターの事前計算を行う。即ち、上位制御層である事前計算部２２は、ＦＩＦＯ２４の状態を見ながら、適切な間隔で事前計算を行う。この適切な間隔とは、ＦＩＦＯ２４が空にならないように、余裕を持って事前計算が行える程度であればよい。具体的には、事前計算部２２は、ＦＩＦＯ２４の残りデータ量が少なくなった時に起動される。これにより事前計算部２２に対するリアルタイム処理要求は緩和される。そして、ハードリアルタイム処理部である実行部２６は、センサー姿勢の補正係数、逆ヤコビアンを用いてインピーダンス制御処理を行う。そして実行部２６は、インピーダンス制御処理の結果が、リミッター値の範囲を超えた場合に、事前計算部２２に対してエラーを通知する。そして実行部２６は、エラーの発生時に、所定のエラー処理を行う。
【００６８】
なお、本実施形態では、軌道は予測可能であることを仮定しており、リミッター値は、その仮定から外れるほどの大きなインピーダンス制御による軌道変化があったことを検出するためのものである。このような状態となった場合には、例えば上位制御層への割り込み処理を要求するようにする。
【００６９】
以上の本実施形態の手法によれば、パラメーターが予め事前計算されていることによる応答性の改善と、その事前計算にはリアルタイム応答性を必要としないことによる制御ハードウェアーへの性能要求の低減を実現できるようになる。
【００７０】
２．２ 力制御、インピーダンス制御
次に、力制御、インピーダンス制御（コンプライアンス制御）の概要について説明する。
【００７１】
図３（Ａ）は、ロボットの左のアームＡＬ、右のアームＡＲで物体ＯＢを挟んで移動している様子を示している。例えば、位置制御だけでは、物体を落としたり、破壊してしまうおそれがある、力制御によれば、柔軟な物体や脆い物体を、図３（Ａ）のように両側から適切な力で挟んで移動させることが可能になる。
【００７２】
また、力制御によれば、図３（Ｂ）に示すように、不確実性のある物体の表面ＳＦをアームＡＭ等でなぞることが可能になる。このような制御は位置制御だけでは実現不能である。また、力制御によれば、図３（Ｃ）に示すように、粗い位置決めの後に、探って位置合わせをして、物体ＯＢを穴部ＨＬにはめ込むことも可能になる。
【００７３】
しかしながら、バネなどの実際の機械部品による力制御では、用途が限られるという問題がある。また、このような機械部品による力制御では、特性の動的な切り替えが困難である。
【００７４】
一方、モーターのトルクを制御するトルク制御は簡単であるが、位置精度が悪くなるという問題がある。また、異常時に衝突などの問題が生じる。例えば図３（Ａ）において、異常事態が起きて、物体ＯＢを落としてしまった場合に、トルク制御では、釣合うべき反力が無くなるため、左右のアームＡＬ、ＡＲが衝突してしまうなどの問題が生じる。
【００７５】
これに対して、インピーダンス制御（コンプライアンス制御）は、制御が複雑であるという不利点があるものの、汎用性や安全性が高いという利点がある。
【００７６】
図４（Ａ）、図４（Ｂ）は、インピーダンス制御の１つであるコンプライアンス制御を説明する図である。コンプライアンスはバネ定数の逆数を意味し、バネ定数が硬さを表すのに対して、コンプライアンスは柔らかさを意味する。ロボットと環境との間に相互作用が働くときに、機械的柔軟性であるコンプライアンスを与える制御をコンプライアンス制御と呼ぶ。
【００７７】
例えば図４（Ａ）では、ロボットのアームＡＭには力覚センサーＳＥが取り付けられている。このロボットのアームＡＭは、力覚センサーＳＥで得られたセンサー情報（力・トルク情報）に応じて姿勢が変わるようにプログラムされている。具体的には、図４（Ａ）のＡ１に示す仮想的なバネが、あたかもアームＡＭの先端に取り付けられているかのようにロボットを制御する。
【００７８】
例えばＡ１に示すバネのバネ定数が１００Ｋｇ／ｍであったとする。これを図４（Ｂ）のＡ２に示すように５Ｋｇの力で押せば、Ａ３に示すようにバネは５ｃｍだけ縮む。逆に言えば、５ｃｍだけ縮んでいれば、５Ｋｇの力で押されているといえる。つまり、力情報と位置情報とが対応づけられている。
【００７９】
コンプライアンス制御では、このＡ１に示す仮想的なバネがアームＡＭの先端に取り付けられているかのような制御が行われる。具体的には、ロボットは、力覚センサーＳＥの入力に応答して動作し、Ａ２に示す５Ｋｇの加重に対して、Ａ３に示すように５ｃｍだけ後退するように制御され、力情報に対応して位置情報が変化するように制御される。
【００８０】
このような単純なコンプライアンス制御では時間項を含まないが、時間項を含み、その２次の項までを考慮した制御が、インピーダンス制御である。具体的には、２次の項は質量項であり、１次の項は粘性項であり、インピーダンス制御のモデルは下式（１）に示すような運動方程式で表すことができる。
【００８１】
【数１】
【００８２】
上式（１）において、ｍは質量、μは粘性係数、ｋは弾性係数、ｆは力、ｘは目標位置からの変位である。またｘの１次微分、２次微分は、各々、速度、加速度に対応する。インピーダンス制御では、上式（１）の特性をアームの先端であるエンドエフェクター部に持たせるための制御系を構成する。即ち上式（１）で表される仮想質量、仮想粘性係数、仮想弾性係数を、あたかもアームの先端が持っているかのように制御を行う。
【００８３】
このように、インピーダンス制御は、アームの先端の質量に粘性要素と弾性要素が各方向に接続されるモデルにおいて、目的として設定された粘性係数と弾性係数で物体に接触するようにする制御である。
【００８４】
例えば図５（Ａ）に示すように、ロボットのアームＡＬ、ＡＲで物体ＯＢをつかんで、軌道ＴＲに沿って移動させる制御を考える。この場合に、軌道ＴＲＬは、物体ＯＢの左側の内側に設定された点ＰＬが通る軌道であり、インピーダンス制御を想定して決定された仮想的な左手の軌道である。また軌道ＴＲＲは、物体ＯＢの右側の内側に設定された点ＰＲが通る軌道であり、インピーダンス制御を想定して決定された仮想的な右手の軌道である。この場合に、アームＡＬは、アームＡＬの先端と点ＰＬの距離差に応じた力が発生するように制御される。またアームＡＲは、アームＡＲの先端と点ＰＲの距離差に応じた力が発生するように制御される。このようにすれば、物体ＯＢを柔らかくつかみながら移動させるインピーダンス制御を実現できる。そして、インピーダンス制御では、図５（Ａ）のＢ１に示すように物体ＯＢが落下する事態が生じたとしても、アームＡＬ、ＡＲは、Ｂ２、Ｂ３に示すように、その先端が点ＰＬ、ＰＲの位置で止まるように制御される。即ち、仮想的な軌道が衝突軌道でなければ、アームＡＬ、ＡＲが衝突するのを防止できる。
【００８５】
また図５（Ｂ）のように、物体の表面ＳＦをなぞるように制御する場合にも、インピーダンス制御では、アームＡＭの先端に対して、仮想的な軌道ＴＲＶＡと先端との距離差ＤＦに応じた力が働くように制御される。従って、アームＡＭを、力を加えながら表面ＳＦをなぞるような制御が可能になる。
【００８６】
２．３ ヤコビアン、逆ヤコビアン
以上のようなインピーダンス制御、コンプライアンス制御等においては、その動作は、基本的にはデカルト座標系で計算される。そして、デカルト座標系でのロボットの手先の動きを、ロボットの各関節の動きに翻訳するためにはインバースキネマティクス処理が必要となる。例えば図６（Ａ）において、Ｃ１に示す力が働いて、アームＡＭの先端ＰＴがＣ２に示すように移動した場合に、各関節Ｊ１、Ｊ２の角度を求めるためには、インバースキネマティクス処理が必要になる。
【００８７】
しかしながら、図６（Ｂ）に示すように、一般的には、インバースキネマティクス処理は非常に難しい処理である。一方、ロボットの各関節の値を設定し、それを手先の動きに変換する処理であるフォワードキネマティクス処理は簡単な処理である。このような処理において、姿勢変化が微小であれば、フォワードキネマティクスを用いたヤコビアンを用いることができる。
【００８８】
例えば下式（２）に示すように、ｍ個の変数を入力とし、ｎ個の出力を与えるある関数ｆを考える。或いは、ｍ個の変数を入力とし、１つの出力を与える関数ｆをｎ組用意する。
【００８９】
【数２】
【００９０】
この場合に、ヤコビアン（ヤコビ行列）は、下式（３）に表される行列である。
【００９１】
【数３】
【００９２】
つまり、ヤコビアンは、ｘの「ある値」の近傍において、ｘとｙの関係を線形近似したものである。従って、一般的には、ｘの「ある値」が変化すれば、ヤコビアンも変化する。
【００９３】
ここで、ｘを関節角、ｙをロボットの手先の姿勢（xyzuvw）とすれば、上式（２）の関数は、フォワードキネマティクス関数となり、これは簡単に求めることができる。
【００９４】
上式（２）の関数をフォワードキネマティクス関数と考えれば、上式（３）のヤコビアン（ヤコビ行列）は、ある姿勢において各関節が微小に変化した場合の、それに伴う手先の微小な変化を与える関係式となる。即ち図７（Ａ）において、関節Ｊ１の角度が微小な単位量だけ変位した場合に、先端ＰＴがどれだけ変位するかを表す関係式になる。或いは図７（Ｂ）において、関節Ｊ２の角度が微小な単位量だけ変位した場合に、先端ＰＴがどれだけ変位するかを表す関係式になる。
【００９５】
この場合に、各関節角の微小変化と、手先の微小変化との対応は、その姿勢に応じて変化する。つまり、ヤコビアンは、姿勢の変化に対応して、その都度、計算する必要がある。
【００９６】
さて、前述したように、手先の位置から関節角を求めるインバースキネマティクス処理は、一般的には非常に難しい処理である。
【００９７】
しかしながら、下式（４）のように、ヤコビアンの逆行列である逆ヤコビアンを求めることができれば、手先の微小な変化を各関節角の微小な変化に変換する関係式を求めることができる。
【００９８】
【数４】
【００９９】
ところが、一般的には、ヤコビアンは正方行列ではなく、且つ、ｒａｎｋ（階数）も低く、通常の意味の逆行列は存在しない。例えば７軸のロボットでは、手先の自由度は６つしかないので、７×７ではなく、７×６の行列になってしまう。但し、通常の意味の逆行列と類似した性質を持つ一般化逆行列は存在し得る。この一般化逆行列を用いることにより、インバースキネマティクス関数を必要とする処理を、フォワードキネマティクス関数を用いて実行することが可能になる。これを利用して、本実施形態では、処理の簡素化を図っている。具体的には、制御パラメーターとして、逆ヤコビアンの行列要素を事前計算し、事前計算された逆ヤコビアンの行列要素に基づいて、目標値の補正値を求めている。
【０１００】
２．４ リミッター値
さて、図８において、計画軌道ＴＲＰに沿ってアームＡＭの先端ＰＴを移動させる場合を考える。この場合に、産業用途のロボットにおいては、ロボット軌道が、軌道ＴＲＡのように大きくずれることはなく、このような軌道ＴＲＡになった場合には、それは異常状態であると判断できる。
【０１０１】
そこで本実施形態では、制御パラメーターとして、計画軌道ＴＲＰに対するロボット軌道の許容範囲ＲＴを示すリミッター値（例えばモーターのエンコーダー値のリミッター値）を事前計算する。そして計画軌道ＴＲＰに対してロボット軌道が許容範囲ＲＴを超えたか否かを判断する。そして許容範囲を超えた場合には、それを示す割り込みを発生させる。
【０１０２】
具体的には本実施形態では、計画軌道ＴＲＰの周辺のヤコビアンを求めることで、リミッター値を事前計算する。即ち、ヤコビアンの誤差が許容される範囲を、許容範囲ＲＴとして、リミッター値を事前計算する。例えばヤコビアンの行列要素の誤差が所与の値以下になる範囲を許容範囲ＲＴとして、リミッター値を事前計算する。
【０１０３】
この場合に、図８に示すように、許容範囲ＲＴは一定ではなく、許容範囲ＲＴの大きさはロボットのアームＡＭの姿勢に依存する。例えば図８のＤ１に示す姿勢の場合とＤ２に示す姿勢の場合とで、許容範囲ＲＴは同一ではない。そしてＤ１とＤ２とで、ヤコビアン（逆ヤコビアン）の値も異なる。このため本実施形態では、計画軌道ＴＲＰの各点の周辺においてヤコビアンを求めることで（各姿勢におけるヤコビアンを求めることで）、リミッター値を事前計算するようにしている。
【０１０４】
なお、この場合にヤコビアンを計算する姿勢は、図９において実線で示す姿勢であり、破線で示す姿勢ではない。即ち、図５（Ａ）で説明したように、インピーダンス制御においては、物体の内側に設定された点ＰＬ、ＰＲを通る仮想的な軌道を目標として制御が行われる。例えば図９のアームＡＭの先端ＰＴと仮想点ＶＰＴ（ＰＬ、ＰＲ）との差に応じた力が発生するように制御される。そしてヤコビアンの計算時には、このような仮想的な軌道に対応する破線で示す姿勢ではなく、実線で示す姿勢においてヤコビアンを計算することになる。
【０１０５】
２．５ 制御系の構成
図１０に、力覚フィードバックを含まない場合の制御系の基本構成例を示す。
【０１０６】
軌道生成部５６２は、軌道情報ｐ（xyzuvw）を生成してインバースキネマティクス処理部５６４に出力する。ここで軌道情報ｐは、例えばアームの先端（エンドエフェクター部）の位置情報(xyz)と各軸回りの回転情報（uvw）を含む。そしてインバースキネマティクス処理部５６４は、この軌道情報ｐに基づいてインバースキネマティクス処理を行って、目標値である各関節の関節角θを生成して出力する。そして、この関節角θに基づいて、モーター制御を行うことで、ロボットのアームの動作制御が行われる。この場合に図１０のモーター（Ｍ）の制御は、公知のＰＩＤ制御により実現されている。このＰＩＤ制御は公知の技術であるため、ここでは詳細な説明を省略する。
【０１０７】
図１０において、軌道生成部５６２とインバースキネマティクス処理部５６４とにより目標値出力部が構成される。この目標値出力部の処理は、ロボットの全体的な処理になる。一方、後段のモーター制御は、関節毎の制御になる。
【０１０８】
なお、前述した特許文献３の従来技術においても、上位制御層と下位制御層の区分分割を行っている。しかしながら、特許文献３における上位制御層は、図１０の軌道生成部５６２とインバースキネマティクス処理部５６４の部分であり、下位制御層は、モーター制御の部分であり、本実施形態とは異なる区分分割となっている。
【０１０９】
図１１に、力覚フィードバックを含む場合の制御系の基本構成例を示す。図１１では、図１０に対して、更に、力覚センサー５１０と、姿勢補正部５３２と、ハンド・ツール自重補正部５３４と、運動方程式処理部５３６と、フォワードキネマティクス処理部５４０が設けられている。
【０１１０】
図１１では、力覚センサー５１０からのセンサー情報を受けて、姿勢補正部５３２がセンサーの姿勢補正を行い、ハンド・ツール自重補正部５３４がハンド・ツール自重補正を行う。そして、運動方程式処理部５３６が、前述の式（１）に示すような運動方程式の解を求める処理を行い、補正値Δｐを出力する。この補正値Δｐにより軌道情報ｐが補正されることで、目標値である関節角θの補正処理が行われる。またフォワードキネマティクス処理部５４０は、フォワードキネマティクス処理を行って、ロボットの軌道情報ｐ’を求めて軌道生成部５６２にフィードバックする。また姿勢補正部５３２、ハンド・ツール補正部５３４に対して姿勢を特定するための情報を出力する。
【０１１１】
図１２に、本実施形態の制御系の基本構成例を示す。この制御系は、図１１と同様に力覚フィードバックを含む。図１２が図１１と異なるのは、図１２では逆ヤコビアン処理部３８が更に設けられている点である。
【０１１２】
具体的には、図１１では、運動方程式処理部５３６からのΔｐにより、軌道生成部５６２が出力するｐが補正されている。これに対して図１２では、運動方程式処理部５３６からのΔｐに基づいて、逆ヤコビアン処理部３８が、Δθを求める逆ヤコビアン処理を行う。即ち、前述の式（４）において、Δｙ（Δｙ１，Δｙ２・・・）がΔｐになり、Δｘ（Δｘ１，Δｘ２・・・）がΔθになる逆ヤコビアン処理を行って、Δθを求める。そして、求められたΔθにより、目標値である関節角θが補正される。このような構成にすることにより、運動方程式の処理結果を関節ごとに分解できるようになる。
【０１１３】
そして図１２において、姿勢補正部３２、ハンド・ツール自重補正部３４、逆ヤコビアン処理部３８、フォワードキネマティクス処理部４０の処理では、ロボット姿勢に応じて、使用されるパラメーターの値が変化する。
【０１１４】
しかしながら、ロボットの姿勢変化が予めプログラムされたものであるか、或いはそこからの差異が小さければ、これらのパラメーター値は軌道をプログラムする時点で予め計算しておくことができる。そこで本実施形態では、これらのパラメーター値を事前計算で求める手法を採用している。
【０１１５】
また、上述の「差異が小さい」とは、各補正における誤差が、運動方程式の解の誤差となった時に、その誤差が許容範囲内にあるということであり、またその結果のヤコビアン処理の結果の誤差が許容範囲内にあるということである。
【０１１６】
この「差異が小さい」ということは、上記のように、ロボットの実際の姿勢とプログラムされた姿勢との差異が小さいということに依存するものであり、その差異が想定よりも大きなものとなった場合には、エラーとして処理する必要がある。そこで本実施形態では、図８で説明したように許容範囲ＲＴに対応するリミッター値を事前計算しておき、ロボット軌道が許容範囲ＲＴを超えたと判断された場合には、割り込みを発生させるようにしている。
【０１１７】
２．６ 詳細な構成例
図１３に本実施形態のロボット制御システムの詳細な構成例を示す。図１３は、図１２で説明した制御系に対応する構成である。なお本実施形態のロボット制御システムは図１３の構成には限定されず、その一部の構成要素を省略したり、他の構成要素を追加するなどの種々の変形実施が可能である。例えば本実施形態のロボット制御システムとして、図１１等で説明した制御系の構成を採用してもよい。
【０１１８】
図１３の逆ヤコビアン事前計算部４２、リミッター値事前計算部４４、補正係数事前計算部４６が、図１の事前計算部２２に相当する。また図１３のＦＩＦＯ５０、５２、５４が、図１のＦＩＦＯ２４に相当する。また図１３のハンド・ツール自重・姿勢補正部３５、運動方程式処理部３６、逆ヤコビアン処理部３８、リミッター処理部３９、割り込み発生部４８が、図１の実行部２６に相当する。なおＦＩＦＯ５６はインバースキネマティクス処理部６４からの関節角θを格納するためのＦＩＦＯである。
【０１１９】
逆ヤコビアン事前計算部４２は、前述の式（４）の逆ヤコビアン（逆ヤコビアンの行列要素）を事前計算し、事前計算した逆ヤコビアン（逆ヤコビアンに対応する値）をＦＩＦＯ５０に格納する。そして逆ヤコビアン処理部３８は、各演算処理実行タイミングにおいて、事前計算された逆ヤコビアンをＦＩＦＯ５０から読み出して、前述の式（４）に示すような逆ヤコビアン処理を行って、関節角を補正するための補正値を出力する。
【０１２０】
リミッター値事前計算部４４は、図８で説明したロボット軌道の許容範囲に対応するリミッター値を事前計算し、事前計算したリミッター値をＦＩＦＯ５２に格納する。そしてリミッター処理部３９は、各演算処理実行タイミングにおいて、事前計算されたリミッター値をＦＩＦＯ５２から読み出して、計画軌道に対してロボット軌道が許容範囲を超えたか否かを判断する。そして、許容範囲を超えたと判断されると、割り込み発生部４８が、許容範囲を超えたことを示す割り込みを発生する。
【０１２１】
補正係数事前計算部４６は、力覚センサー１０のセンサー情報を補正するための補正係数を事前計算し、事前計算した補正係数をＦＩＦＯ５４に格納する。そしてハンド・ツール自重・姿勢補正部３５は、事前計算された補正係数に基づいてセンサー情報の補正処理を行う。そして運動方程式処理部３６は、この補正処理後のセンサー情報を用いて、前述の式（１）に対応する運動方程式の解を求める処理を行い、処理結果を逆ヤコビアン処理部３８に出力する。すると、逆ヤコビアン処理部３８は、この運動方程式処理部３８からの処理結果と、事前計算された逆ヤコビアンに基づいて、前述の式（４）に示す逆ヤコビアン処理を行って、関節角の補正値を生成する。
【０１２２】
ここで、ハンド・ツール自重・姿勢補正部３５は、図１２のハンド・ツール自重補正部３４及び姿勢補正部３２に対応するものである。ここでハンド・ツール自重補正は、ロボットのハンドの自重や、ハンドがつかむツールの自重による影響を、力覚センサー１０からのセンサー情報（力情報）から相殺するための補正処理である。また姿勢補正は、力覚センサー１０の姿勢による影響を、センサー情報（力情報）から相殺するための補正処理である。これらのハンド・ツール自重補正及び姿勢補正は、例えば下式（５）のように表すことができる。
【０１２３】
【数５】
【０１２４】
上式（５）において、Ｆｘ，Ｆｙ，Ｆｚ、Ｆｕ，Ｆｖ，Ｆｗは力覚センサー１０からのセンサー情報である力情報、トルク情報である。またＢｘ，Ｂｙ，Ｂｚ、Ｂｕ，Ｂｖ，Ｂｗはバイアス項である。そして、補正後のセンサー情報（力情報、トルク情報）であるｆｘ，ｆｙ，ｆｚ、ｆｕ，ｆｖ，ｆｗが運動方程式処理部３６に入力される。なお、データには固定値があるため、実質的な補正係数は６×７＝４２個となる。これらのハンド・ツール自重補正及び姿勢補正は公知の補正処理であるため、詳しい説明は省略する。
【０１２５】
図１４は、ＦＩＦＯを用いた本実施形態の処理を説明するための図である。図１４において、ＴＭ１、ＴＭ２、ＴＭ３・・・は、実行部２６（ハンド・ツール自重・姿勢補正部３５、運動方程式処理部３６、逆ヤコビアン処理部３８、リミッター処理部３９等）の演算処理の実行タイミングである。このように、本実施形態では、実行部２６は、ＴＭ１、ＴＭ２、ＴＭ３・・・の各演算処理実行タイミングにおいて、事前計算された制御パラメーター（逆ヤコビアン、リミッター値、補正係数）をＦＩＦＯ（ＦＩＦＯ５０、５２、５４）から読み出す。
【０１２６】
そして図１４のＥ１では、ＦＩＦＯの残りデータ量（制御パラメーターの残りデータ量）がしきい値以下になったため、下位制御層である実行部２６は、Ｅ２に示すように、上位制御層である事前計算部２２（逆ヤコビアン事前計算部４２、リミッター値事前計算部４４、補正係数事前計算部４６）に対して、制御パラメーターの事前計算要求を行う。すると、事前計算部２２が、制御パラメーターの事前計算を行い、Ｅ３に示すように、事前計算された制御パラメーターをＦＩＦＯに格納する。
【０１２７】
この場合に、ＦＩＦＯには、以前に事前計算した制御パラメーターが残存しているため、Ｅ４に示すタイミングについては、ＴＭ１、ＴＭ２、ＴＭ３・・・の演算処理実行タイミングに比べて、十分に長いインターバルを取ることができる。従って、実行部２６に対しては、高速且つハードなリアルタイム処理が要求されるものの、事前計算部２２に対しては、ある程度の処理の遅延が許容されるため、低速又はソフトなリアルタイム処理が要求されるだけになる。従って、事前計算部２２は、逆ヤコビアンやリミッター値や補正係数を求めるというような負荷の高い計算処理を、時間的に余裕を持って実行することが可能になる。一方、実行部２６では、逆ヤコビアンやリミッター値や補正係数を求めるというような負荷の高い計算処理を行わなくも済むため、高速且つハードなリアルタイム処理が要求されても、これに対応することが可能になる。
【０１２８】
なお図１５は、ロボット制御部８０と実行部２６の接続関係の例を示す図である。図１５では、図１のロボット制御部８０が制御するモーター（広義には駆動部）が２つの場合を示している。
【０１２９】
インバースキネマティクス処理部６４からの目標値（関節角）は、ＦＩＦＯ５６-1を介してモーター制御部８４-1に入力される。この場合に、この目標値は、逆ヤコビアン処理部３８からの補正値により補正されて、モーター制御部８４-1に入力される。そしてモーター制御部８４-1は、補正後の目標値に基づいて、モーターＭ１の制御を行う。
【０１３０】
同様に、インバースキネマティクス処理部６４からの目標値（関節角）は、ＦＩＦＯ５６-2を介してモーター制御部８４-2に入力される。この場合に、この目標値は、逆ヤコビアン処理部３８からの補正値により補正されて、モーター制御部８４-2に入力される。そしてモーター制御部８４-2は、補正後の目標値に基づいて、モーターＭ２の制御を行う。
【０１３１】
また、リミッター値事前計算部４４で事前計算されたリミッター値は、ＦＩＦＯ５２-1を介してリミッター処理部３９-1に入力される。リミッター処理部３９-1は、事前計算されたリミッター値と、モーター制御部８４-1からのエンコード値（回転角度検出情報）に基づいて、ロボット軌道が許容範囲を超えたか否かを判断する。そして、許容範囲を超えたと判断された場合には、割り込み発生部４８が割り込みを発生する。
【０１３２】
同様に、リミッター値事前計算部４４で事前計算されたリミッター値は、ＦＩＦＯ５２-2を介してリミッター処理部３９-2に入力される。リミッター処理部３９-2は、事前計算されたリミッター値と、モーター制御部８４-2からのエンコード値に基づいて、ロボット軌道が許容範囲を超えたか否かを判断する。そして、許容範囲を超えたと判断された場合には、割り込み発生部４８が割り込みを発生する。
【０１３３】
３．詳細な処理
次に本実施形態の詳細な処理例について図１６〜図２０のフローチャートを用いて説明する。
【０１３４】
図１６は、ハードリアルタイム処理部である実行部の全体的な処理についてのフローチャートである。
【０１３５】
まず、制御パラメーターを更新するタイミングか否かを判断する（ステップＳ１）。そして、更新タイミングである場合には、図１４で説明したように、ＦＩＦＯに残りデータ（制御パラメーター）があるか否かを判断する（ステップＳ２）。そして、残りデータが無い場合には、制御パラメーター無しの割り込みを発生させて（ステップＳ３）、割り込み処理へ移行する（ステップＳ４）。一方、ＦＩＦＯに残りデータがある場合には、ＦＩＦＯから制御パラメーターを取得する（ステップＳ５）。
【０１３６】
次に、ＦＩＦＯの残りデータ量はしきい値よりも多いか否かを判断する（ステップＳ６）。そして、しきい値以下である場合には、図１４で説明したように、事前計算部に対して制御パラメーターの作成を要求する（ステップＳ７）。一方、残りデータ量がしきい値よりも多い場合には、インピーダンス制御処理を実行する（ステップＳ８）。
【０１３７】
次に、インピーダンス制御処理が正常に終了したか否かを判断する（ステップＳ９）。そして、正常に終了しなかった場合には、事前計算部に対して制御パラメーターの更新を要求する（ステップＳ１０）。一方、正常に終了した場合にはステップＳ１に戻る。
【０１３８】
図１７は、図１６のステップＳ８のインピーダンス制御処理についてのフローチャートである。
【０１３９】
まず、力覚センサーから６次元入力値を得て（ステップＳ１１）、７×７の補正行列との積を計算する（ステップＳ１２）。即ち、前述の式（５）のＦｘ，Ｆｙ，Ｆｚ、Ｆｕ，Ｆｖ，Ｆｗを得て、ハンド・自重補正及び姿勢補正を行う。そして、得られた補正済み値に基づいてインピーダンスフィルター処理を行う（ステップＳ１３）。即ち、前述の式（１）の運動方程式の処理を行う。
【０１４０】
次に、インピーダンスフィルターの出力値と、７×６のヤコビ行列との積を求める（ステップＳ１４）。即ち、前述の式（４）の行列演算を行う。そして、各関節の指令値に対して、ステップＳ１４で得られた値を加える補正処理を行う（ステップＳ１５）。
【０１４１】
次に、モーターのエンコーダー値（回転角度）を得る（ステップＳ１６）。そして、エンコーダー値はリミッター値の範囲内か否かを判断し（ステップＳ１７）、リミッター値の範囲内でない場合には、リミッターエラー割り込みを発生する（ステップＳ１８）。一方、リミッター値の範囲内である場合には、正常状態であるとして図１６のＬｏｏｐへ戻る。
【０１４２】
図１８（Ａ）〜図１８（Ｃ）は割り込み処理についてのフローチャートである。
【０１４３】
図１８（Ａ）に示すように、図１６のステップＳ３で制御パラメーター無し割り込みが発生した場合には、予め設定された停止処理に入る（ステップＳ２１）。
【０１４４】
図１８（Ｂ）に示すように、図１７のステップＳ１８でリミッターエラー割り込みが発生した場合には、予め設定された停止処理に入る（ステップＳ２２）。即ち、上位コントローラー（上位制御層）で、許容範囲のリミッター値を設定し、それを超えるような事態では、ともかく停止させる。
【０１４５】
或いは、図１８（Ｃ）に示すように、図１７のステップＳ１８でリミッターエラー割り込みが発生した場合に、ＮＯＰ（No OPeration）とし、異常事態発生としてＬｏｏｐに戻るようにしてもよい（ステップＳ２３）。即ち、上位コントローラー（上位制御層）で、余裕を見込んだリミッター値を設定し、下位コントローラー（下位制御層）のオーバーシュートを許容する。そして、割り込みを受けた上位コントローラーは、軌道を更新し、ＦＩＦＯを新しいデータで上書きする。一方、下位コントローラーは、新しいデータで動作を継続する。そして、上位コントローラーが軌道を更新できない場合には、下位コントローラーへの割り込みを発生させて、動作を停止させる。
【０１４６】
図１９は事前計算部の処理についてのフローチャートである。
【０１４７】
まず、事前計算部は、割り込みが発生するのを待ち（ステップＳ３１）、割り込みが発生した場合には、制御パラメーター無し割り込みか否かを判断する（ステップＳ３２）。そして、制御パラメーター無し割り込みの場合には、制御パラメーターを計算して、ＦＩＦＯに設定する（ステップＳ３３）。そして、ハードリアルタイム処理部（実行部）を再起動する（ステップＳ３４）。
【０１４８】
一方、制御パラメーター無し割り込みではない場合には、制御パラメーター作成割り込みか否かを判断する（ステップＳ３５）。そして、制御パラメーター作成割り込みの場合には、ＦＩＦＯの空きデータ量を取得し（ステップＳ３６）、制御パラメーターを計算して、ＦＩＦＯに設定する（ステップＳ３７）。
【０１４９】
制御パラメーター作成割り込みではない場合には、制御パラメーター更新割り込みか否かを判断する（ステップＳ３８）。そして、制御パラメーター更新割り込みの場合には、現在のロボットの姿勢を取得し、復旧軌道を求める（ステップＳ３９）。そして、制御パラメーターを計算して、ＦＩＦＯに上書きする（ステップＳ４０）。
【０１５０】
図２０は、パラメーター計算処理についてのフローチャートである。
【０１５１】
まず、ＦＩＦＯに空きはあるか否かを判断し（ステップＳ４１）、空きがある場合には、計画軌道を求める（ステップＳ４２）。そして、計画軌道のヤコビアン、逆ヤコビアンを求め（ステップＳ４３）、ＦＩＦＯに逆ヤコビアンの値を設定する（ステップＳ４４）。
【０１５２】
次に、図８で説明したように、計画軌道の周辺のヤコビアンを求め、差異が所定の値以下である範囲を求め（ステップＳ４５）、ＦＩＦＯにリミッター値を設定する（ステップＳ４６）。また、計画軌道に対する力覚センサー補正値（補正係数）を求め（ステップＳ４７）、ＦＩＦＯに力覚センサー補正値を設定する（ステップＳ４８）。
【０１５３】
なお、上記のように本実施形態について詳細に説明したが、本発明の新規事項および効果から実体的に逸脱しない多くの変形が可能であることは当業者には容易に理解できるであろう。従って、このような変形例はすべて本発明の範囲に含まれるものとする。例えば、明細書又は図面において、少なくとも一度、より広義または同義な異なる用語と共に記載された用語は、明細書又は図面のいかなる箇所においても、その異なる用語に置き換えることができる。また本実施形態及び変形例の全ての組み合わせも、本発明の範囲に含まれる。またロボット制御システム、ロボットシステムの構成・動作や、制御パラメーターの事前計算手法、ＦＩＦＯへの制御パラメーターの格納手法、制御パラメーターに基づく演算処理の実行手法、制御系の構成等も、本実施形態で説明したものに限定されず、種々の変形実施が可能である。
【符号の説明】
【０１５４】
ＯＢ 物体、ＡＬ、ＡＲ、ＡＭ アーム、ＳＦ 表面、ＨＬ 穴部、Ｊ１〜Ｊ３ 関節、
ＰＴ 先端部、Ｍ、Ｍ１、Ｍ２ モーター、
１０ 力覚センサー、２０ 力制御部（インピーダンス制御部）、２２ 事前計算部、
２４ ＦＩＦＯ、２６ 実行部、３２ 姿勢補正部、３４ ハンド・ツール自重補正部、
３５ ハンド・ツール自重・姿勢補正部、３６ 運動方程式処理部、
３８ 逆ヤコビアン処理部、３９、３９-1、３９-2 リミッター処理部、
４０ フォワードキネマティクス処理部、４２ 逆ヤコビアン事前計算部、
４４ リミッター値事前計算部、４６ 補正係数事前計算部、
５０、５２、５２-1、５２-2、５４、５６、５６-1、５６-2 ＦＩＦＯ、
６０ 目標値出力部、６２ 軌道生成部、６４ インバースキネマティクス処理部、
８０ ロボット制御部、８２-1〜８２-N 駆動制御部、
８４-1、８４-2 モーター制御部、１００ ロボット、１０２-1〜１０２-N 駆動部、
３００ 制御装置、３１０ ロボット、３２０ アーム、３３０ ハンド
【特許請求の範囲】
【請求項１】
ロボットのフィードバック制御の目標値を出力する目標値出力部と、
力覚センサーからのセンサー情報に基づいて力制御を行い、前記目標値の補正値を出力する力制御部と、
前記補正値により補正された前記目標値に基づいて、前記ロボットの前記フィードバック制御を行うロボット制御部と、
を含み、
前記力制御部は、
前記ロボットの姿勢に応じて変化する制御パラメーターを事前計算する事前計算部と、
前記センサー情報と前記制御パラメーターとに基づき前記補正値を求める演算処理を実行する実行部と、
を含むことを特徴とするロボット制御システム。
【請求項２】
請求項１において、
前記力制御部は、
前記事前計算部で計算された前記制御パラメーターが格納されるＦＩＦＯを含み、
前記実行部は、
前記ＦＩＦＯから前記制御パラメーターを読み出し、読み出された前記制御パラメーターに基づき前記補正値を求める前記演算処理を実行することを特徴とするロボット制御システム。
【請求項３】
請求項２において、
前記ＦＩＦＯには、
前記実行部で使用される前記制御パラメーターが、前記実行部の演算処理が実行されるタイミングごとに、先入れ先出し方式で記憶されることを特徴とするロボット制御システム。
【請求項４】
請求項２又は３において、
前記実行部は、
前記ＦＩＦＯの残りデータ量が所与のしきい値以下になった場合に、前記事前計算部に対して、前記制御パラメーターを事前計算することを要求する事前計算要求を行うことを特徴とするロボット制御システム。
【請求項５】
請求項１乃至４のいずれかにおいて、
前記事前計算部は、
前記制御パラメーターとして、逆ヤコビアンの行列要素を事前計算し、
前記実行部は、
事前計算された前記逆ヤコビアンの前記行列要素に基づいて、前記目標値の前記補正値を求めることを特徴とするロボット制御システム。
【請求項６】
請求項１乃至５のいずれかにおいて、
前記事前計算部は、
前記制御パラメーターとして、計画軌道に対するロボット軌道の許容範囲を示すリミッター値を事前計算し、
前記実行部は、
前記計画軌道に対して前記ロボット軌道が前記許容範囲を超えたと判断した場合に、前記許容範囲を超えたことを示す割り込みを発生させることを特徴とするロボット制御システム。
【請求項７】
請求項６において、
前記事前計算部は、
前記計画軌道の周辺のヤコビアンを求めることで、前記リミッター値を事前計算することを特徴とするロボット制御システム。
【請求項８】
請求項１乃至７のいずれかにおいて、
前記事前計算部は、
前記制御パラメーターとして、前記力覚センサーの前記センサー情報を補正するための補正係数を事前計算し、
前記実行部は、
前記補正係数に基づいて前記センサー情報の補正処理を行うことを特徴するロボット制御システム。
【請求項９】
請求項１乃至８のいずれかにおいて、
前記目標値出力部は、
前記ロボットの軌道情報を出力する軌道生成部と、
前記軌道生成部からの前記軌道情報に基づいてインバースキネマティクス処理を行い、前記ロボットの関節角情報を前記目標値として出力するインバースキネマティクス処理部と、
を含むことを特徴とするロボット制御システム。
【請求項１０】
請求項１乃至９のいずれかにおいて、
前記力制御部は、
前記力制御としてインピーダンス制御を行うことを特徴とするロボット制御システム。
【請求項１１】
請求項１乃至１０のいずれかにおいて、
前記目標値は、前記ロボットの関節角情報であり、前記補正値は、前記関節角情報の補正値であることを特徴とするロボット制御システム。
【請求項１２】
請求項１乃至１１のいずれかに記載のロボット制御システムと、
前記ロボットと、
を含むことを特徴とするロボットシステム。
【請求項１】
ロボットのフィードバック制御の目標値を出力する目標値出力部と、
力覚センサーからのセンサー情報に基づいて力制御を行い、前記目標値の補正値を出力する力制御部と、
前記補正値により補正された前記目標値に基づいて、前記ロボットの前記フィードバック制御を行うロボット制御部と、
を含み、
前記力制御部は、
前記ロボットの姿勢に応じて変化する制御パラメーターを事前計算する事前計算部と、
前記センサー情報と前記制御パラメーターとに基づき前記補正値を求める演算処理を実行する実行部と、
を含むことを特徴とするロボット制御システム。
【請求項２】
請求項１において、
前記力制御部は、
前記事前計算部で計算された前記制御パラメーターが格納されるＦＩＦＯを含み、
前記実行部は、
前記ＦＩＦＯから前記制御パラメーターを読み出し、読み出された前記制御パラメーターに基づき前記補正値を求める前記演算処理を実行することを特徴とするロボット制御システム。
【請求項３】
請求項２において、
前記ＦＩＦＯには、
前記実行部で使用される前記制御パラメーターが、前記実行部の演算処理が実行されるタイミングごとに、先入れ先出し方式で記憶されることを特徴とするロボット制御システム。
【請求項４】
請求項２又は３において、
前記実行部は、
前記ＦＩＦＯの残りデータ量が所与のしきい値以下になった場合に、前記事前計算部に対して、前記制御パラメーターを事前計算することを要求する事前計算要求を行うことを特徴とするロボット制御システム。
【請求項５】
請求項１乃至４のいずれかにおいて、
前記事前計算部は、
前記制御パラメーターとして、逆ヤコビアンの行列要素を事前計算し、
前記実行部は、
事前計算された前記逆ヤコビアンの前記行列要素に基づいて、前記目標値の前記補正値を求めることを特徴とするロボット制御システム。
【請求項６】
請求項１乃至５のいずれかにおいて、
前記事前計算部は、
前記制御パラメーターとして、計画軌道に対するロボット軌道の許容範囲を示すリミッター値を事前計算し、
前記実行部は、
前記計画軌道に対して前記ロボット軌道が前記許容範囲を超えたと判断した場合に、前記許容範囲を超えたことを示す割り込みを発生させることを特徴とするロボット制御システム。
【請求項７】
請求項６において、
前記事前計算部は、
前記計画軌道の周辺のヤコビアンを求めることで、前記リミッター値を事前計算することを特徴とするロボット制御システム。
【請求項８】
請求項１乃至７のいずれかにおいて、
前記事前計算部は、
前記制御パラメーターとして、前記力覚センサーの前記センサー情報を補正するための補正係数を事前計算し、
前記実行部は、
前記補正係数に基づいて前記センサー情報の補正処理を行うことを特徴するロボット制御システム。
【請求項９】
請求項１乃至８のいずれかにおいて、
前記目標値出力部は、
前記ロボットの軌道情報を出力する軌道生成部と、
前記軌道生成部からの前記軌道情報に基づいてインバースキネマティクス処理を行い、前記ロボットの関節角情報を前記目標値として出力するインバースキネマティクス処理部と、
を含むことを特徴とするロボット制御システム。
【請求項１０】
請求項１乃至９のいずれかにおいて、
前記力制御部は、
前記力制御としてインピーダンス制御を行うことを特徴とするロボット制御システム。
【請求項１１】
請求項１乃至１０のいずれかにおいて、
前記目標値は、前記ロボットの関節角情報であり、前記補正値は、前記関節角情報の補正値であることを特徴とするロボット制御システム。
【請求項１２】
請求項１乃至１１のいずれかに記載のロボット制御システムと、
前記ロボットと、
を含むことを特徴とするロボットシステム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【公開番号】特開２０１３−８５６（Ｐ２０１３−８５６Ａ）
【公開日】平成２５年１月７日（２０１３．１．７）
【国際特許分類】
【出願番号】特願２０１１−１３６４２１（Ｐ２０１１−１３６４２１）
【出願日】平成２３年６月２０日（２０１１．６．２０）
【出願人】（０００００２３６９）セイコーエプソン株式会社 (51,324)
【Ｆターム（参考）】
【公開日】平成２５年１月７日（２０１３．１．７）
【国際特許分類】
【出願日】平成２３年６月２０日（２０１１．６．２０）
【出願人】（０００００２３６９）セイコーエプソン株式会社 (51,324)
【Ｆターム（参考）】
[ Back to top ]