JP2013052451A

JP2013052451A - ロボット制御システム、ロボットシステム及びプログラム

Info

Publication number: JP2013052451A
Application number: JP2011190405A
Authority: JP
Inventors: Nobuhiro Karido; 信宏狩戸
Original assignee: Seiko Epson Corp
Current assignee: Seiko Epson Corp
Priority date: 2011-09-01
Filing date: 2011-09-01
Publication date: 2013-03-21

Abstract

【課題】力覚センサーの検出値に基づいて、ロボットの軌道が計画軌道から外れたか否かを判定し、計画軌道とロボットの軌道とのずれの拡大を防止することができるロボット制御システム等の提供。
【解決手段】ロボット制御システム３００は、計画軌道に基づいて、第１の周期で、ロボット４００の制御指令を出力する第１の制御部１１０と、第１の制御部１１０から得られる制御指令と、力覚センサー４４０から得られる検出値とに基づいて、第１の周期よりも短い第２の周期で、ロボット４００を制御する第２の制御部２１０とを含む。そして、第２の制御部２１０は、力覚センサー４４０の検出値の許容範囲内に、取得された力覚センサー４４０の検出値が含まれるか否かを判定し、許容範囲内に力覚センサー４４０の検出値が含まれないと判定した場合には、ロボット４００の軌道が計画軌道から外れたと判定する。
【選択図】図３

Description

本発明は、ロボット制御システム、ロボットシステム及びプログラム等に関係する。

近年、生産現場において、人が行ってきた作業を機械化・自動化するために、産業用ロボットを導入することが多くなってきた。産業用ロボットは、アームとハンドを備えているものが多く、実際の生産現場では、産業用ロボットのハンドにより、ワークを掴んで、所定の位置に移動させる作業などが行われている。

このような産業用ロボットの動作を制御する際には、ロボットのハンドを目標位置に移動させるために、アームやハンドの計画軌道を設定し、計画軌道を通るようにアームのジョイント等を動かす制御信号を生成する。そして、制御信号に従って、ロボットを動作させる。

しかし、ロボットを動作させる際に、外力等の要因によりロボットの軌道が、計画軌道から外れてしまうことがある。

この問題を解決する手法としては、特許文献１〜特許文献３に記載される従来技術がある。

特開平１１−０２４７１８号公報特開２００３−０７１７６０号公報特開２００８−１４２８１０号公報

本発明では、例えば、産業用ロボットと離れた位置にある第１の情報処理装置を用いて、産業用ロボットの近くに設置された第２の情報処理装置に、計画軌道に対応する制御指令を送信して、第２の情報処理装置がロボットを制御するような場合を想定している。

この際に、外力等の要因により、ロボットの軌道が計画軌道から外れた場合には、ロボットの軌道修正等を行う必要がある。しかし、第１の情報処理装置によって、ロボットの軌道修正等を行う場合には、第１の情報処理装置から新たな制御指令を、第２の情報処理装置が受信するまでに、計画軌道とロボットの軌道とのずれが拡大してしまうことがある。

ところが、特許文献１〜特許文献３には、前述したような２つの情報処理装置における制御指令の待ち時間を原因とするロボットの軌道のずれの拡大を防止する手法については、記載されていない。

本発明の幾つかの態様によれば、力覚センサーの検出値に基づいて、ロボットの軌道が計画軌道から外れたか否かを判定し、計画軌道とロボットの軌道とのずれの拡大を防止することができるロボット制御システム、ロボットシステム及びプログラム等を提供することができる。

本発明の一態様は、計画軌道に基づいて、第１の周期で、ロボットの制御指令を出力する第１の制御部と、前記第１の制御部から得られる前記制御指令と、力覚センサーから得られる検出値とに基づいて、前記第１の周期よりも短い第２の周期で、前記ロボットを制御する第２の制御部と、を含み、前記第２の制御部は、前記力覚センサーの検出値の許容範囲内に、取得された前記力覚センサーの検出値が含まれるか否かを判定し、前記許容範囲内に前記力覚センサーの検出値が含まれないと判定した場合には、前記ロボットの軌道が前記計画軌道から外れたと判定することを特徴とするロボット制御システムに関係する。

本発明の一態様では、力覚センサーの検出値に基づいて、ロボットの実際の軌道を求めなくてもよく、力覚センサーの検出値が許容範囲内に含まれるか否かを判定する処理を行う。

これにより、第１の制御部へ力覚センサーの検出値を出力する処理と、力覚センサーの検出値に基づいて、ロボットの実際の軌道を求める処理を排除することが可能になり、処理量及び応答待ち時間を削減することが可能になる。

また、本発明の一態様では、前記第１の制御部は、前記計画軌道に対応する前記許容範囲を設定し、前記第２の制御部は、前記第１の制御部から得られる前記許容範囲に基づいて、前記許容範囲内に前記力覚センサーの検出値が含まれるか否かを判定してもよい。

これにより、例えば、計画軌道をユーザーが設定する場合に、第１の制御部が計画軌道に合わせて、力覚センサーの検出値の許容範囲を設定すること等が可能になる。

また、本発明の一態様では、前記第２の制御部は、前記許容範囲内に前記力覚センサーの検出値が含まれないと判定した場合には、前記許容範囲内に前記力覚センサーの検出値が含まれないことを、前記第１の制御部に通知して、前記ロボットの制御の停止処理を行い、前記許容範囲内に前記力覚センサーの検出値が含まれると判定した場合には、前記第２の周期で、前記ロボットの制御を行ってもよい。

これにより、力覚センサーの検出値が許容範囲内に含まれるか否かによって、第２の制御部の動作を変更すること等が可能になる。

また、本発明の一態様では、前記第２の制御部は、ローカル座標系において、互いに直交する第１軸、第２軸、第３軸の３軸についての力成分を有する検出値を、前記力覚センサーから取得し、前記ロボットの軌道が前記計画軌道と一致している時に前記力覚センサーの検出値の第３軸方向が鉛直方向と一致する場合に、前記力覚センサーの検出値の各力成分のうち、少なくとも一つの力成分が、前記許容範囲内に含まれるか否かを判定して、前記ロボットの軌道が前記計画軌道から外れたか否かを判定してもよい。

これにより、例えば、力覚センサーの第３軸方向を鉛直方向に一致させたまま、ロボットを動作させる場合に、第２の制御部が、力覚センサーの検出値に含まれる力成分に基づいて、ロボットの制御を継続するか停止するか判断すること等が可能になる。

また、本発明の一態様では、前記第２の制御部は、ローカル座標系において、互いに直交する第１軸、第２軸、第３軸の３軸のそれぞれを中心とするモーメント成分を有する検出値を、前記力覚センサーから取得し、前記ロボットの軌道が前記計画軌道と一致している時に前記力覚センサーの検出値の第３軸方向が鉛直方向と一致する場合に、前記力覚センサーの検出値の各モーメント成分のうち、少なくとも一つのモーメント成分が、前記許容範囲内に含まれるか否かを判定して、前記ロボットの軌道が前記計画軌道から外れたか否かを判定してもよい。

これにより、例えば、力覚センサーの第３軸を鉛直方向に一致させたまま、ロボットを動作させる場合に、第２の制御部が、力覚センサーの検出値に含まれるモーメント成分に基づいて、ロボットの制御を継続するか停止するか判断すること等が可能になる。

また、本発明の一態様では、前記第２の制御部は、前記許容範囲内に前記力覚センサーの検出値が含まれないと判定した場合には、前記力覚センサーの検出値に基づいて、前記ロボットの前記軌道の補正処理を行ってもよい。

これにより、第１の制御部から制御指令が出力される前に、第２の制御部が力覚センサーの検出値に基づいて、ロボットの軌道の補正処理を行うこと等が可能となり、ロボットの軌道が補正されるまでの応答待ち時間を短縮すること等が可能になる。

また、本発明の他の態様は、計画軌道に基づいて、第１の周期で、ロボットの制御指令を出力する第１の制御部と、前記第１の制御部から得られる前記制御指令と、前記力覚センサーから得られる検出値とに基づいて、前記第１の周期よりも短い第２の周期で、前記ロボットを制御する第２の制御部と、を含み、前記第２の制御部は、前記ロボットの軌道の補正処理として、前記力覚センサーの検出値と検出目標値との差異に基づいて、前記ロボットを制御するロボット制御システムに関係する。

本発明の他の態様によれば、力覚センサーの検出値と検出目標値との差異に基づいて、ロボットの軌道の補正処理を行うことで、例えばロボットの軌道を算出する処理等を行うことを回避し、処理量を低減し、アルゴリズムの簡易化を図ること等が可能となる。

また、本発明の一態様及び他の態様では、前記第１の制御部は、前記ロボットの軌道が前記計画軌道と一致している場合の前記力覚センサーの検出値を、検出目標値として設定して、前記第２の制御部は、前記ロボットの前記軌道の前記補正処理として、前記力覚センサーから得られる検出値と、前記第１の制御部から得られる前記力覚センサーの前記検出目標値との差異に基づいて、前記ロボットを制御してもよい。

これにより、第１の制御部が計画軌道に基づいて、力覚センサーの検出目標値を設定すること等が可能になり、例えば、ユーザーが計画軌道を設定する場合にも、第１の制御部が力覚センサーの検出目標値を求めて、ロボットの軌道の補正処理を行うこと等が可能になる。

また、本発明の一態様及び他の態様では、前記第１の制御部は、前記力覚センサーの前記検出目標値を、前記第１の周期毎に、前記第２の制御部に出力し、前記第２の制御部は、前記第１の周期よりも短い第３の周期で、検出値を前記力覚センサーから取得する場合には、取得した前記力覚センサーの検出値と、前記検出目標値との前記差異を、前記第２の周期で求めてもよい。

これにより、第３の周期が第１の周期と異なる場合においても、最新の力覚センサーの検出値を用いて、ロボットの軌道の補正処理を行うこと等が可能になる。

また、本発明の一態様及び他の態様では、前記第１の制御部は、前記第１の周期毎に、前記力覚センサーの前記検出目標値と逆ヤコビ行列とを出力し、前記第２の制御部は、前記第１の周期よりも短い第３の周期で、前記力覚センサーから検出値を取得する場合に、取得した前記力覚センサーの検出値と前記検出目標値との前記差異を、前記第２の周期で求め、前記差異に基づいて、前記第２の周期で、前記逆ヤコビ行列の補正処理を行ってもよい。

これにより、逆ヤコビ行列を補正して、現在のロボットの姿勢に基づく逆ヤコビ行列を求めること等が可能になる。

また、本発明の一態様及び他の態様では、前記第１の制御部は、前記第１の周期毎に、逆ヤコビ行列を出力し、前記第２の制御部は、前記許容範囲内に前記力覚センサーの検出値が含まれないと判断した場合には、前記逆ヤコビ行列の更新処理要求を行ってもよい。

これにより、第２の制御部で逆ヤコビ行列を補正する処理を回避して、処理量を抑制すること等が可能になる。

また、本発明の他の態様は、前記ロボット制御システムを含むことを特徴とするロボットシステムに関係する。

また、本発明の他の態様は、上記各部としてコンピューターを機能させるプログラムに関係する。

本実施形態及び比較例のシステム構成例。図２（Ａ）、図２（Ｂ）は、本実施形態と比較例との軌道修正タイミングの説明図。本実施形態の詳細なシステム構成例。図４（Ａ）、図４（Ｂ）は、力覚センサーの説明図。図５（Ａ）、図５（Ｂ）は、ロボットがワークを垂直に把持しながら移動させる場合の軌道判定処理の説明図。図６（Ａ）、図６（Ｂ）は、ロボットがワークを水平に把持しながら移動させる場合の軌道判定処理の説明図。軌道補正処理の説明図。図８（Ａ）、図８（Ｂ）は、力覚センサーの検出目標値を用いる場合の軌道補正処理の説明図。力覚センサーの検出目標値と現在の検出値との差異の更新タイミングの説明図。本実施形態の第１の制御部が行う処理の一例を説明するフローチャート。本実施形態の第２の制御部が行う処理の一例を説明するフローチャート。本実施形態の第１の制御部が行う処理の他の例を説明するフローチャート。本実施形態の第２の制御部が行う処理の他の例を説明するフローチャート。

以下、本実施形態について説明する。まず、本実施形態の概要とシステム構成例を説明し、次に本実施形態の特徴について説明する。そして最後に、フローチャートを用いて本実施形態の処理の流れについて説明する。なお、以下に説明する本実施形態は、特許請求の範囲に記載された本発明の内容を不当に限定するものではない。また本実施形態で説明される構成の全てが、本発明の必須構成要件であるとは限らない。

１．概要
本実施形態は、例えば、産業用ロボットと離れた位置にある第１の情報処理装置を用いて、産業用ロボットの近くに設置された第２の情報処理装置に、計画軌道に対応する制御指令を送信して、第２の情報処理装置がロボットを制御するような場合を想定している。

ここでは、本実施形態を説明する前に、本実施形態の比較例について、図１を用いて説明する。図１では、第１の情報処理装置１００と第２の情報処理装置２００とが無線で接続されており、第２の情報処理装置とロボット４００が有線で接続されている。

まず、第１の情報処理装置１００が、第２の情報処理装置２００へ、ロボット４００の計画軌道に基づいた制御指令を出力する。制御指令は、例えば１０ｍｓ（ミリ秒）に一度出力されるものであり、この場合には、１０ｍｓ後のロボット４００の位置等を表す制御指令が出力される。

次に、第２の情報処理装置２００は、取得した制御指令に基づいて、ロボット４００を制御する。第２の情報処理装置２００がロボット４００を制御する周期は、例えば、１ｍｓに設定され、第１の情報処理装置１００が、制御指令を出力する周期よりも短く設定される。そのため、第２の情報処理装置２００は、例えば、１０ｍｓ後のロボット４００の位置等を示す制御指令を補間して、１ｍｓ単位のロボット４００の目標位置等を求めて、ロボット４００を制御する。

最後に、ロボット４００は、第２の情報処理装置２００から取得される命令に基づいて、アーム４２０やハンド４３０を動作させて、ワーク６００を把持して、移動させる等の動作を行う。

この際に、ロボット４００に外力が加わる等の要因により、ロボット４００の軌道が計画軌道から外れた場合には、ロボット４００の軌道修正等を行う必要がある。本実施形態の比較例では、ロボット４００のハンド４３０に、力覚センサー４４０が設けられており、第２の情報処理装置２００が、力覚センサー４４０から検出値を取得して、第１の情報処理装置１００に出力する。そして、第１の情報処理装置１００が、取得した力覚センサーの検出値に基づいて、ロボット４００の各部が実際に移動した軌道を算出する。その後、第１の情報処理装置１００は、求めたロボット４００の実際の軌道と計画軌道とを比較して、実際の軌道が計画軌道から外れていると判断する場合には、ロボット４００の軌道のずれを修正するような制御指令を生成し、出力する。その後は、前述した場合と同様に、第２の情報処理装置２００が、制御指令に基づいて、ロボット４００を制御し、ロボット４００の軌道のずれを修正する。

しかし、このような手法で、ロボット４００の軌道修正等を行う場合には、第２の情報処理装置２００は、第１の情報処理装置１００から新たな制御指令を受信するまでの間も、ロボット４００を制御し続けることとなる。しかもこの際に、第２の情報処理装置２００は、軌道修正前の制御指令に基づいて、ロボット４００を制御し続けてしまう。そのため、応答待ち時間中に、計画軌道とロボット４００の軌道とのずれがさらに拡大してしまうことがある。

その具体例を図２（Ａ）に示す。図２（Ａ）では、前述した例と同様に、第１の情報処理装置が１０ｍｓに一度、制御指令を出力し、第２の情報処理装置は１ｍｓに一度ロボットを制御する（ＳＧ１）。そして、第２の情報処理装置は力覚センサーの検出値を取得し、第１の情報処理装置に出力する（ＳＧ２）。なお、ここでは、第２の情報処理装置は、５ｍｓに一度、力覚センサーの検出値を取得できるものとし、説明の簡略化のため、力覚センサーの検出値を取得してから、第１の情報処理装置に出力するまでの時間間隔は、１ｍｓとしている。そして、第１の情報処理装置は、力覚センサーの検出値に基づいて、ロボットの軌道を修正する制御指令を生成し、出力する（ＳＧ３）。

ここで、図２（Ａ）において、例えば、ロボット制御の開始から２ｍｓ時点（Ｐ１）で、第２の情報処理装置が力覚センサーの検出値を取得した後に、第１の情報処理装置からロボットの制御指令が出力されるのは、制御開始から１０ｍｓ時点（Ｐ２）となる。さらに、その制御指令に基づき、ロボットが制御されるのは、その１ｍｓ後（Ｐ３）である。すなわち、制御開始から２ｍｓ時点（Ｐ１）から１１ｍｓ時点（Ｐ３）までの９ｍｓ間（応答待ち時間ＷＴ１間）は、例え、ロボットの軌道がずれていたとしても、ロボットの軌道を修正する制御指令等がロボットの動作に反映されない。そのため、ロボットの軌道と、計画軌道とのずれが、ＷＴ１の間に拡大してしまうことがある。

前述した特許文献１〜特許文献３には、前述したような２つの情報処理装置における制御指令の応答待ち時間を原因とするロボットの軌道のずれの拡大を防止する手法については、記載されていない。

そこで、本実施形態では、第１の情報処理装置に設けられた第１の制御部が、第２の情報処理装置に設けられた第２の制御部へ、ロボットの制御指令を通知する場合に、第２の制御部が、力覚センサーの検出値に基づいて、ロボットの軌道が計画軌道から外れたか否かを判定する。

これにより、例えば、図２（Ｂ）に示すように、第１の情報処理装置と第２の情報処理装置との間で、力覚センサーの検出値等の送受信処理を省略することが可能となり、前述した応答待ち時間ＷＴ１を、大幅に短縮して、ＷＴ２とすることが可能となる。なお、ここでは、説明の簡略化のため、第２の情報処理装置が力覚センサーの検出値を取得してから、力覚センサーの検出値を反映してロボットを制御する（ＳＧ３）までの時間（応答待ち時間ＷＴ２）を１ｍｓとしているが、応答待ち時間ＷＴ２は、これに限定されるものではない。

このようにして、本実施形態では、応答待ち時間を削減することにより、計画軌道とロボットの軌道とのずれの拡大を防止すること等が可能となる。

２．システム構成例
本実施形態のシステム構成例は、前述した図１と同様である。なお、第１の情報処理装置１００と第２の情報処理装置２００は、一つの情報処理装置であってもよい。第２の情報処理装置とロボット４００も同様である。

次に、図３に本実施形態のロボット制御システム３００及びこれを含むロボットシステム５００の構成例を示す。

本実施形態では、ロボットシステム５００は、ロボット制御システム３００と、ロボット４００とを含む。そして、ロボット制御システム３００は、第１の情報処理装置１００と、第２の情報処理装置２００とを含む。さらに、第１の情報処理装置１００は、第１の制御部１１０と、記憶部１２０と、Ｉ／Ｆ部１３０とを含む。一方、第２の情報処理装置２００は、第２の制御部２１０と、記憶部２２０と、Ｉ／Ｆ部２３０とを含む。ロボット４００は、制御部４１０と、アーム４２０と、ハンド４３０と、力覚センサー４４０と、Ｉ／Ｆ部４５０とを含む。

なお、第１の情報処理装置１００と、第２の情報処理装置２００と、ロボット制御システム３００と、ロボット４００と、ロボットシステム５００は、図３の構成に限定されず、これらの一部の構成要素を省略したり、他の構成要素を追加したりするなどの種々の変形実施が可能である。他にも、例えば、第２の情報処理装置２００がロボット４００内に組み込まれていても良い。

次に各部で行われる処理について説明する。

まず、第１の制御部１１０は、記憶部１２０からのデータ等に基づいて種々の処理を行う。例えば、第１の制御部１１０は、計画軌道に基づいて、ロボットの制御指令を生成する。

一方、第２の制御部２１０は、記憶部２２０からのデータや、Ｉ／Ｆ部２３０において受信した力覚センサーの検出値等に基づいて種々の処理を行う。例えば、第２の制御部２１０は、第１の制御部１１０から取得したロボット４００の制御指令を補間しつつ、ロボット４００を制御する。

次に、記憶部１２０及び記憶部２２０は、データベースを記憶したり、第１の制御部１１０や第２の制御部２１０等のワーク領域となるもので、その機能はＲＡＭ等のメモリーやＨＤＤ（ハードディスクドライブ）などにより実現できる。

そして、Ｉ／Ｆ部１３０は、第１の情報処理装置１００と第２の情報処理装置２００間で情報をやり取りするためのインターフェースである。さらに、Ｉ／Ｆ部１３０は、ユーザーからの入力を取得する機能を有するインターフェースであってもよい。

一方、Ｉ／Ｆ部２３０は、第１の情報処理装置１００と第２の情報処理装置２００間、及び第２の情報処理装置とロボット４００間で、情報をやり取りするためのインターフェースである。

そして、ロボット４００の制御部４１０（命令実行部）は、第２の制御部２１０から取得した制御信号等に基づいて、ロボット４００を制御し、アーム４２０やハンド４３０を動作させる。

また、アーム４２０とは、ロボット４００のパーツであって、一つ以上の関節を含む可動パーツのことをいう。一方、ハンド４３０とは、ワーク６００を把持したり、ワーク６００に加工を施したりするためにアーム４２０のエンドポイントに取り付ける部品のことをいう。また、アーム４２０のエンドポイントとは、アーム４２０の先端部分のポイントであって、ロボット４００のハンド４３０以外の他の部分と接続されていない部分のことをいう。

また、力覚センサー４４０は、図４（Ａ）に示すように、互いに直交するＸ、Ｙ、Ｚ軸方向へハンド４３０が受ける力（Ｆ_ｘ、Ｆ_ｙ、Ｆ_ｚ）と、Ｘ、Ｙ、Ｚ軸を中心とするハンド４３０のモーメント（Ｍ_ｘ、Ｍ_ｙ、Ｍ_ｚ）を検出することができる６軸の力覚センサーＰＳであることが望ましい。さらに、力覚センサー４４０は、アーム４２０とハンド４３０との間に設けられることが望ましく、図４（Ｂ）に示すように、力覚センサー破壊防止機構（メカニカルヒューズ）ＤＰＳと弾性結合機構ＥＳに接続されて、ハンド４３０に取り付けられる。ただし、力覚センサー４４０は、これに限定されず、少なくとも２軸以上の検出値を検出することができればよく、他の位置及び方法で設けられるものであってもよい。

最後に、Ｉ／Ｆ部４５０は、第２の情報処理装置２００とロボット４００間で情報をやり取りするためのインターフェースである。

なお、第１の制御部１１０及び第２の制御部２１０、制御部４１０の機能は、各種プロセッサー（ＣＰＵ等）、ＡＳＩＣ（ゲートアレイ等）などのハードウェアーや、プログラムなどにより実現できる。また、Ｉ／Ｆ部１３０及びＩ／Ｆ部２３０、Ｉ／Ｆ部４５０は、有線通信または無線通信を行う通信部であってもよい。

３．本実施形態の手法
以上の本実施形態では、計画軌道に基づいて、第１の周期で、ロボット４００の制御指令を出力する第１の制御部１１０と、第１の制御部１１０から得られる制御指令と、力覚センサー４４０から得られる検出値とに基づいて、第１の周期よりも短い第２の周期で、ロボット４００を制御する第２の制御部２１０と、を含む。そして、第２の制御部２１０は、力覚センサー４４０の検出値の許容範囲内に、取得された力覚センサー４４０の検出値が含まれるか否かを判定し、許容範囲内に力覚センサー４４０の検出値が含まれないと判定した場合には、ロボット４００の軌道が計画軌道から外れたと判定する。

ここで、第１の周期とは、第１の制御部１１０が制御指令を出力する周期であり、第２の周期よりも長い周期、若しくは第２の周期と等しい周期である。一方、第２の周期とは、第２の制御部２１０がロボット４００を制御する周期であり、第１の周期よりも短い周期、若しくは第１の周期と等しい周期である。

また、制御指令とは、ロボット４００の各部の目標位置や、目標位置への到達目標時間、ロボット４００の各部の作動速度等を示す情報である。例えば、第１の周期が１０ｍｓである場合には、１０ｍｓ後のロボット４００の各部の到達位置などを制御指令として出力する。

前述した比較例では、第２の制御部は、力覚センサーの検出値を第１の制御部に出力するが、本実施形態では、力覚センサーの検出値を第１の制御部へ出力しなくてもよい。

これにより、第２の制御部から第１の制御部へ、力覚センサーの検出値を出力する処理を排除することが可能となり、応答待ち時間を削減することが可能となる。

なお、ここでは、力覚センサーから検出値が取得されてから、取得された力覚センサーの検出値に基づいて、ロボットが制御されるまでの時間のことを、応答待ち時間という。例えば、前述したように、図２（Ａ）及び図２（Ｂ）の例では、応答待ち時間をＷＴ１からＷＴ２へ短縮している。

さらに、比較例では、第１の制御部が力覚センサーの検出値に基づいて、ロボットの実際の軌道を求めて、実際の軌道と計画軌道とを比較するが、本実施形態では、力覚センサーの検出値に基づいて、ロボットの実際の軌道を求める必要はない。その代わりに、本実施形態では、力覚センサーの検出値が許容範囲内に含まれるか否かを判定する処理を行う。この処理によって、力覚センサーの検出値が許容範囲内に含まれると判定された場合には、ロボットが計画軌道から外れずに動作しているとみなすことを意味し、一方、力覚センサーの検出値が許容範囲内に含まれないと判定された場合には、ロボットが計画軌道から外れて動作しているとみなすことを意味する。

具体的に、ロボットの軌道が計画軌道から外れた場合の一例を、図５（Ａ）及び図５（Ｂ）を用いて簡単に説明する。図５（Ａ）では、計画軌道ＯＢ通りにロボットのハンドＨＤが移動している。これに対して、図５（Ｂ）では、ロボットのハンドＨＤが、計画軌道ＰＯＢ通り、本来の目標位置ＰＰに移動せずに、ＲＯＢのような軌道で移動している。図５（Ｂ）からも明確である通り、一度ＲＯＢのような軌道を通って移動してしまうと、その後制御指令通りにロボットのハンドＨＤが移動したとしても、本来の目標位置ＰＰには到達できないことがある。その場合の対処については、後述する。

これにより、力覚センサーの検出値に基づいて、ロボットの実際の軌道を求める処理を排除することが可能になり、処理量及び前述した応答待ち時間を削減すること等が可能になる。

また、第１の制御部１１０は、計画軌道に対応する許容範囲を設定してもよい。そして、第２の制御部２１０は、第１の制御部１１０から得られる許容範囲に基づいて、許容範囲内に力覚センサー４４０の検出値が含まれるか否かを判定してもよい。

また、第２の制御部２１０は、許容範囲内に力覚センサー４４０の検出値が含まれないと判定した場合には、許容範囲内に力覚センサー４４０の検出値が含まれないことを、第１の制御部１１０に通知して、ロボット４００の制御の停止処理を行い、許容範囲内に力覚センサー４４０の検出値が含まれると判定した場合には、第２の周期で、ロボット４００の制御を行ってもよい。

さらに、例えば、図５（Ｂ）のように、ロボットが計画軌道から外れて動作している場合には、力覚センサーの検出値が許容範囲内に含まれない状態となっており（この理由については、具体例を挙げて後述する）、これ以上ロボットの制御を継続しても、計画軌道とのずれが拡大する一方である。そのため、この場合には第２の制御部によるロボット制御を停止する。これにより、軌道のずれの拡大を防ぎ、その後、ロボットの軌道を修正するような制御を行う等が可能となる。一方で、力覚センサーの検出値が許容範囲内であると判断された場合には、例えば図５（Ａ）のような状態であるため、第２の制御部によるロボットの制御を継続する。

また、第２の制御部２１０は、ローカル座標系において、互いに直交する第１軸、第２軸、第３軸の３軸についての力成分を有する検出値を、力覚センサー４４０から取得してもよい。そして、ロボット４００の軌道が計画軌道と一致している時に力覚センサー４４０の検出値の第３軸方向が鉛直方向と一致する場合に、第２の制御部２１０は、力覚センサー４４０の検出値の各力成分とのうち、少なくとも一つの力成分が、許容範囲内に含まれるか否かを判定して、ロボット４００の軌道が計画軌道から外れたか否かを判定してもよい。

すなわち、ロボット４００の計画軌道が、力覚センサーの検出値の第３軸方向が鉛直方向と一致する計画軌道である場合に、第２の制御部２１０は、力覚センサー４４０の検出値の各力成分と許容範囲に基づいて、ロボット４００の軌道が計画軌道から外れたか否かを判定する。

ここで、２つの具体例を説明する。まず、第１の具体例について、図５（Ａ）及び図５（Ｂ）を用いて説明する。図５（Ａ）及び図５（Ｂ）は、どちらもワークＷＫをロボットのハンドＨＤが垂直に把持して、水平に移動させる様子を示している。この際に、ハンドＨＤには力覚センサーＰＳが設けられており、ローカル座標系において、互いに直交するＸ軸、Ｙ軸、Ｚ軸の３軸についての力成分を有する検出値を、力覚センサーＰＳから取得することができる。本例では、Ｘ軸が第１軸、Ｙ軸が第２軸、Ｚ軸が第３軸に相当している。なお、ここでは、説明の簡略化のため、慣性力は無視できるものとする。

今、このような力覚センサーＰＳのＺ軸方向が図５（Ａ）のように、鉛直方向と一致しているとする。この場合には、力覚センサーＰＳにより、ワークＷＫにかかる重力を示すＺ軸方向（第３軸方向）の力成分ＰＤＲのみが検出され、Ｘ軸及びＹ軸方向（第１軸及び第２軸方向）の力成分は検出されない。本例のように、ロボットが計画軌道ＯＢを通って移動する場合には、ハンドＨＤ及びワークＷＫの姿勢が保たれるため、移動後もＺ軸方向の力成分ＰＤＲのみが検出される。

一方、図５（Ｂ）のように、計画軌道ＰＯＢから外れてＲＯＢのような軌道を通って、ロボットが動作した場合には、移動後には力覚センサーＰＳのＺ軸方向が鉛直方向と一致しなくなる。そのため、本例で移動後には、図に示すように、Ｚ軸方向の力成分ＰＤＲＺだけでなく、Ｙ軸方向の力成分ＰＤＲＹも検出されることになる。Ｘ軸方向やＹ軸方向の力成分の絶対値が大きければ大きい程、ハンドＨＤが傾いていることを示しており、ここでは、ハンドＨＤが大きく傾きながら動作した場合には計画軌道からずれて動作していると判断する。逆に、Ｘ軸方向やＹ軸方向の力成分の絶対値が小さければ小さい程、ハンドＨＤの傾きは少なく、軌道も計画軌道から外れていないと判断する。

そこで、本例では、Ｘ軸及びＹ軸方向の力成分に対して、所定の許容範囲を設けて、Ｘ軸及びＹ軸方向の力成分が、それぞれ許容範囲内に含まれるか否かを判断する。許容範囲内に含まれない場合には、ハンドＨＤの傾きが大きく、計画軌道から軌道が外れていると判断し、第２の制御部によるロボットの制御を停止する。一方、Ｘ軸及びＹ軸方向の力成分が、許容範囲内に含まれる場合には、ハンドＨＤの傾きが小さく、計画軌道通りにロボットが動作していると判断し、第２の制御部によるロボットの制御を継続する。また、前述した判定は、Ｘ軸方向又はＹ軸方向の力成分どちらか一方のみに対して行っても良い。

次に、第２の具体例について、図６（Ａ）及び図６（Ｂ）を用いて説明する。図６（Ａ）及び図６（Ｂ）は、どちらもワークＷＫをロボットのハンドＨＤが水平に把持して、そのまま水平に移動させる様子を示している。本例では、Ｘ軸が第１軸、Ｚ軸が第２軸、Ｙ軸が第３軸に相当している。その他の条件は、第１の具体例と同様である。

今、このような力覚センサーＰＳのＹ軸方向が図６（Ａ）のように、鉛直方向と一致しているとする。この場合には、力覚センサーＰＳにより、ワークＷＫにかかる重力を示すＹ軸方向（第３軸方向）の力成分ＰＤＲのみが検出され、Ｘ軸及びＺ軸方向（第１軸及び第２軸方向）の力成分は検出されない。本例のように、ロボットが計画軌道ＯＢを通って移動する場合には、ハンドＨＤ及びワークＷＫの姿勢が保たれるため、移動後もＹ軸方向の力成分ＰＤＲのみが検出される。例えば、ワークＷＫの質量が１．０ｋｇ、力覚センサー端面と、ワークＷＫの重心との距離が０．１ｍとすると、Ｘ軸方向の力成分Ｆ_ｘ＝０（Ｎ）、Ｙ軸方向の力成分Ｆ_ｙ＝−９．８（Ｎ）、Ｚ軸方向の力成分Ｆ_ｚ＝０（Ｎ）となる。

一方、図６（Ｂ）のように、計画軌道ＰＯＢから外れてＲＯＢのような軌道を通って、ロボットが動作した場合には、移動後には力覚センサーＰＳのＹ軸方向が鉛直方向と一致しなくなる。そのため、本例で移動後には、図に示すように、Ｙ軸方向の力成分ＰＤＲＹだけでなく、Ｚ軸方向の力成分ＰＤＲＺも検出されることになる。Ｘ軸方向やＺ軸方向の力成分の絶対値が大きければ大きい程、ハンドＨＤが傾いていることを示しており、ここでは、ハンドＨＤが大きく傾きながら動作した場合には計画軌道からずれて動作していると判断する。逆に、Ｘ軸方向やＺ軸方向の力成分の絶対値が小さければ小さい程、ハンドＨＤの傾きは少なく、軌道も計画軌道から外れていないと判断する。例えば、前述した例において、ハンドＨＤの向きが仰角方向上向きに５度だけ変化した場合には、Ｘ軸方向の力成分Ｆ_ｘ＝０（Ｎ）、Ｙ軸方向の力成分Ｆ_ｙ＝−９．７６（Ｎ）、Ｚ軸方向の力成分Ｆ_ｚ＝０．８５４（Ｎ）となる。

そこで、本例では、Ｘ軸及びＺ軸方向の力成分に対して、所定の許容範囲を設けて、Ｘ軸及びＺ軸方向の力成分が、それぞれ許容範囲内に含まれるか否かを判断する。許容範囲内に含まれない場合には、ハンドＨＤの傾きが大きく、計画軌道から軌道が外れていると判断し、第２の制御部によるロボットの制御を停止する。一方、Ｘ軸及びＺ軸方向の力成分が、許容範囲内に含まれる場合には、ハンドＨＤの傾きが小さく、計画軌道通りにロボットが動作していると判断し、第２の制御部によるロボットの制御を継続する。また、前述した判定は、Ｘ軸方向又はＺ軸方向の力成分どちらか一方のみに対して行っても良い。例えば、Ｚ軸方向の力成分に対して、−０．５（Ｎ）〜＋０．５（Ｎ）という許容範囲を設定する。前述した例では、Ｆ_ｚ＝０．８５４（Ｎ）であるため、計画軌道から外れていると判断される。

また、Ｙ軸方向の力成分は、Ｙ軸方向が鉛直方向と一致している場合に最大となり、ハンドが傾くにつれてＹ軸方向への力成分が小さくなる。そのため、Ｙ軸方向の力成分に対しても許容範囲を設けて、計画軌道から軌道が外れているか否かの判断にこれを用いても良い。

また、第２の制御部２１０は、ローカル座標系において、互いに直交する第１軸、第２軸、第３軸の３軸のそれぞれを中心とするモーメント成分を有する検出値を、力覚センサー４４０から取得してもよい。そして、ロボット４００の軌道が計画軌道と一致している時に力覚センサー４４０の検出値の第３軸方向が鉛直方向と一致する場合に、第２の制御部２１０は、力覚センサー４４０の検出値の各モーメント成分のうち、少なくとも一つのモーメント成分が、許容範囲内に含まれるか否かを判定して、ロボット４００の軌道が計画軌道から外れたか否かを判定してもよい。

すなわち、ロボット４００の計画軌道が、力覚センサー４４０の検出値の第３軸方向が鉛直方向と一致する計画軌道である場合に、第２の制御部２１０は、力覚センサー４４０の検出値の各モーメント成分と許容範囲に基づいて、ロボット４００の軌道が計画軌道から外れたか否かを判定する。

ここで、２つの具体例を説明する。前述した第１の具体例と同様にロボットを制御する場合については、図５（Ａ）及び図５（Ｂ）を用いて説明し、前述した第２の具体例と同様にロボットを制御する場合については、図６（Ａ）及び図６（Ｂ）を用いて説明する。また、どちらの場合にも、力覚センサーの検出値として、Ｘ軸、Ｙ軸、Ｚ軸の３軸のそれぞれを中心とするモーメント成分を有する検出値を得ることが可能であるとする。その他の条件は、前述した第１の具体例又は第２の具体例と同様である。

まず、図５（Ａ）の場合には、移動前後において、Ｘ軸、Ｙ軸、Ｚ軸のそれぞれを中心とするモーメント成分は、どれも検出されない。一方、図６（Ａ）の場合には移動前後において、Ｘ軸を中心とし、同一の値を有するモーメント成分のみが検出され、Ｙ軸、Ｚ軸のそれぞれを中心とするモーメント成分は検出されない。

次に、図５（Ｂ）の場合には、ロボットが移動する前の時点では、図５（Ａ）と同様に、Ｘ軸、Ｙ軸、Ｚ軸のそれぞれを中心とするモーメント成分は、どれも検出されない。しかし、移動後に、ロボットのハンドＨＤが傾いた場合には、Ｘ軸、Ｙ軸、Ｚ軸のいずれかを中心とするモーメント成分が検出される。本例では、ハンドＨＤがＸ軸を中心に傾いているため、ワークＷＫにかかる重力により、Ｘ軸を中心に回転する力が発生し、モーメント成分ＭＸが検出される。

一方、図６（Ｂ）の場合には、図６（Ａ）と同様に、移動前後において、Ｘ軸を中心とするモーメント成分のみが検出され、Ｙ軸、Ｚ軸のそれぞれを中心とするモーメント成分は検出されない。しかし、移動前後において、検出されるＸ軸を中心とするモーメント成分の値が異なる。例えば、図６（Ａ）及び図６（Ｂ）の例において、前述した第２の具体例と同様に、ワークＷＫの質量が１．０ｋｇ、力覚センサー端面と、ワークＷＫの重心との距離が０．１ｍとし、ハンドＨＤの向きが仰角方向上向きに５度だけ変化した場合を考える。この場合、Ｘ軸を中心とするモーメント成分は、移動前はＭ_ｘ＝０．９８（Ｎ・ｍ）であるのに対し、移動後はＭ_ｘ＝０．９７６（Ｎ・ｍ）となる。

そこで、本例では、Ｘ軸、Ｙ軸、Ｚ軸のそれぞれを中心とするモーメント成分に対して、所定の許容範囲を設けて、Ｘ軸、Ｙ軸、Ｚ軸のそれぞれを中心とするモーメント成分が、それぞれ許容範囲内に含まれるか否かを判断する。その後の処理については、前述した第１の具体例又は第２の具体例と同様である。また、この判定は、Ｘ軸、Ｙ軸、Ｚ軸のいずれかを中心とするモーメント成分に対してのみ行っても良い。

また、例えば、図７のように、ロボットが計画軌道ＰＯＢ１から外れて動作しており、誤った軌道ＲＯＢを通って動作していると判断された時に、目標位置ＤＳＴへロボットの各部を移動させる場合には、第２の制御部によるロボットの制御を停止するだけではなく、計画軌道を通るように現在のロボットの軌道を修正するような処理が必要となる。前述した通り、軌道ＲＯＢを通って移動した後のハンドＡＭＨＤは、この後、制御指令通りに移動されたとしても、ＰＯＢ３のような軌道を辿り、誤った位置ＥＲＰに移動されてしまう可能性が高いためである。従って、計画軌道上の位置ＰＰに移動させた後、再度計画軌道に従って、ロボットの各部を動作させるために、例えばＡＭＨＤの位置をＣＯＢのように変更する必要がある。

そこで、第２の制御部２１０は、許容範囲内に力覚センサー４４０の検出値が含まれないと判定した場合には、力覚センサー４４０の検出値に基づいて、ロボット４００の軌道の補正処理を行ってもよい。

これにより、第１の制御部から制御指令が出力される前に、第２の制御部が力覚センサーの検出値に基づいて、ロボットの軌道の補正処理を行うこと等が可能となり、ロボットの軌道が補正されるまでの応答待ち時間を短縮すること等が可能になる。さらに、計画軌道とロボットの軌道とのずれを抑制することが可能となり、最短ルートを通って効率良くロボットを動作させること等が可能となる。

前述したロボットの軌道の補正処理には様々な手法が考えられる。

第２の制御部２１０は、ロボット４００の軌道の補正処理として、力覚センサー４４０の検出値と検出目標値との差異に基づいて、ロボット４００を制御してもよい。

例えば、図８（Ａ）及び図８（Ｂ）を用いて具体例を説明する。図８（Ａ）は図６（Ｂ）のロボットの移動前、図８（Ｂ）は図６（Ｂ）のロボットの移動後において、力覚センサーＰＳにより検出されるＸ軸、Ｙ軸、Ｚ軸方向への力を図示したものである。

また、本例では、前述した例と同様に、ワークＷＫの質量が１．０ｋｇ、力覚センサーＰＳ端面と、ワークＷＫの重心との距離が０．１ｍであるものとする。そして、図８（Ａ）に示す移動前の状態では、Ｘ軸方向の力成分Ｆｘ＝０（Ｎ）、Ｙ軸方向の力成分Ｆｙ＝−９．８（Ｎ）、Ｚ軸方向の力成分Ｆｚ＝０（Ｎ）が検出されているとする。さらに、図８（Ｂ）に示す移動後の状態では、ハンドＨＤの向きが仰角方向上向きに５度だけ変化しており、Ｘ軸方向の力成分Ｆｘ＝０（Ｎ）、Ｙ軸方向の力成分Ｆｙ＝−９．７６（Ｎ）、Ｚ軸方向の力成分Ｆｚ＝０．８５４（Ｎ）が検出されているとする。

本来ならば、移動後の状態でも、力覚センサーＰＳの検出値は、図８（Ａ）のように、Ｚ軸方向の力成分Ｆｚ＝０（Ｎ）であることが望ましい。

そこで、本例では、Ｚ軸方向の力成分Ｆｚ＝０（Ｎ）を力覚センサーの検出目標値として用いて、Ｚ軸方向の力成分ＰＤＲＺが検出目標値に近づくように（図８（Ｂ）のＣＯＢ）、ロボットの各所を制御する。

これにより、力覚センサーの検出値と検出目標値との差異に基づいて、ロボットの軌道の補正処理を行うことで、例えばロボットの軌道を算出する処理等を行うことを回避し、処理量を低減し、アルゴリズムの簡易化を図ること等が可能となる。

また、第１の制御部１１０は、ロボット４００の軌道が計画軌道と一致している場合の力覚センサー４４０の検出値を、検出目標値として設定してもよい。そして、第２の制御部２１０は、ロボット４００の軌道の補正処理として、力覚センサー４４０から得られる検出値と、第１の制御部１１０から得られる力覚センサー４４０の検出目標値との差異に基づいて、ロボット４００を制御してもよい。

また、第２の制御部が力覚センサー４４０から検出値を取得する周期である第３の周期は、第１の周期や第２の周期と同一であるとは限らない。例えば、第３の周期が第１の周期よりも短いにも関わらず、第１の周期で、力覚センサー４４０の検出目標値と検出値との差を求める場合には、２値の差に、力覚センサー４４０の最新の検出値が反映されていない期間がある。

そこで、第１の制御部１１０は、力覚センサーの検出目標値を、第１の周期毎に、第２の制御部２１０に出力してもよい。そして、第２の制御部２１０は、第１の周期よりも短い第３の周期で、検出値を力覚センサー４４０から取得する場合には、取得した力覚センサーの検出値と、検出目標値との差異を、第２の周期で求めてもよい。

具体例を図９に示す。図９の例は、図２（Ｂ）の例と同様の状態である。本例では、第３の周期（５ｍｓ）が第１の周期（１０ｍｓ）よりも短いため、第１の周期毎に、力覚センサーの検出目標値との差異を更新すると、例えば、制御開始から２ｍｓ時点（Ｑ１）で更新し、その後、制御開始から１０ｍｓ時点（Ｑ３）で更新することになる。しかし、制御開始から７ｍｓ時点（Ｑ２）で力覚センサーの新たな検出値を取得しているにも関わらず、８ｍｓ時点と９ｍｓ時点では未更新の値を用いて、ロボットの軌道の補正処理を行っている。本来ならば、制御開始から７ｍｓ時点（Ｑ２）で、力覚センサーの検出目標値と新たな検出値との差異を更新すべきである。したがって、本例では、図に示すＵＰＴの部分を、力覚センサーの検出目標値と検出値との差の更新タイミングとする。

これにより、第３の周期が第１の周期と異なる場合においても、力覚センサーの最新の検出値を用いて、ロボットの軌道の補正処理を行うこと等が可能になる。

また、第２の制御部は、ロボットのアームの各関節速度に基づいて、ロボットの制御を行っても良い。この際に、アームの間接速度を求める手法の一つとして、逆ヤコビ行列を利用する手法がある。

ここで、逆ヤコビ行列とは、所与のハンドの速度と逆ヤコビ行列自身の積が、所与のハンドの速度を実現するアームの関節速度として求められる行列のことをいう。

そのため、例えば、第１の制御部１１０は、計画軌道に基づいて、ロボット４００のハンド４３０の目標速度を制御指令として出力するとともに、現在のロボットの姿勢に基づく逆ヤコビ行列を第２の制御部２１０に出力してもよい。この場合には、第２の制御部２１０は、取得されたハンド４３０の目標速度と逆ヤコビ行列の積を、ハンド４３０を目標速度で移動させることができるアーム４２０の関節速度として求めて、求めたアーム４２０の関節速度に基づいて、ロボット４００を制御する。

しかし、ロボットに加わる外力等の要因により、逆ヤコビ行列を求めた時点のロボットの姿勢から、現在のロボットの姿勢が変わってしまった場合には、姿勢が変わる前の逆ヤコビ行列を用いても、正しいアームの関節速度を算出できない場合が多い。本来ならば、この場合には、現在のロボットの姿勢に対応する逆ヤコビ行列を用いる必要がある。

そこで、第１の制御部１１０は、第１の周期毎に、力覚センサー４４０の検出目標値と逆ヤコビ行列とを出力してもよい。そして、第２の制御部２１０は、第１の周期よりも短い第３の周期で、力覚センサー４４０から検出値を取得する場合に、取得した力覚センサー４４０の検出値と検出目標値との差異を、第２の周期で求め、その差異に基づいて、第２の周期で、逆ヤコビ行列の補正処理を行ってもよい。

これにより、逆ヤコビ行列を補正して、現在のロボットの姿勢に基づく逆ヤコビ行列を求めること等が可能になり、補正を行わない場合よりも、ロボットのハンドをより目標速度に近い速度で動作させることが可能となる。

しかし、一般的に、一度算出した逆ヤコビ行列を補正する場合には、処理量が膨大になり、逆ヤコビ行列を再計算した方が、処理量が小さくなる場合が多い。

そこで、第１の制御部１１０は、第１の周期毎に、逆ヤコビ行列を出力してもよい。そして、第２の制御部２１０は、許容範囲内に力覚センサー４４０の検出値が含まれないと判断した場合には、逆ヤコビ行列の更新処理要求を行ってもよい。

なお本実施形態のロボット制御システム等は、プログラムにより実現してもよい。この場合には、ＣＰＵ等のプロセッサーがプログラムを実行することで、本実施形態のロボット制御システム等が実現される。具体的には、情報記憶媒体に記憶されたプログラムが読み出され、読み出されたプログラムをＣＰＵ等のプロセッサーが実行する。ここで、情報記憶媒体（コンピューターにより読み取り可能な媒体）は、プログラムやデータなどを格納するものであり、その機能は、光ディスク（ＤＶＤ、ＣＤ等）、ＨＤＤ（ハードディスクドライブ）、或いはメモリー（カード型メモリー、ＲＯＭ等）などにより実現できる。そしてＣＰＵ等のプロセッサーは、情報記憶媒体に格納されるプログラム（データー）に基づいて本実施形態の種々の処理を行う。即ち情報記憶媒体には、本実施形態の各部としてコンピューター（操作部、処理部、記憶部、出力部を備える装置）を機能させるためのプログラム（各部の処理をコンピューターに実行させるためのプログラム）が記憶される。

４．処理の流れ
以下では、図１０〜図１３のフローチャートを用いて、本実施形態の処理の流れについて説明する。

４．１計画軌道から外れた場合にロボットの制御を停止する処理の流れ
まず、図１０を用いて第１の制御部が行う処理の流れについて説明する。まず、ロボットのアーム及びハンドの計画軌道を設定する（Ｓ１０１）。この際には、ユーザーからの入力に基づいて、計画軌道を生成してもよいし、あらかじめ記憶部に記憶されている計画軌道を読み込んでもよい。

次に、計画軌道の最終目標位置が、ロボットのアーム及びハンドの可動範囲内であるか否かを判断する（Ｓ１０２）。計画軌道の最終目標位置が、ロボットのアーム及びハンドの可動範囲内にないと判断した場合には、エラー処理を行い（Ｓ１１１）、第２の制御部に終了指令を送信して（Ｓ１０９）、処理を終了する。また、エラー処理としては、例えばエラーログを記録したり、ロボットの各部を初期位置に戻したりする等の処理がある。

一方、計画軌道の最終目標位置が、ロボットのアーム及びハンドの可動範囲内であると判断した場合には、ロボットの各部を動かした時に得られる力覚センサーの検出値の許容範囲を求める（Ｓ１０３）。後述するが、この許容範囲を超えるような力覚センサーの検出値が検出された場合には、第２の制御部がロボットの制御を停止することとなる。

次に、第２の制御部に、力覚センサーの検出値の許容範囲と、制御指令として目標速度を送信する（Ｓ１０４）。その後、所定時間が経過するまで待機し（Ｓ１０５、Ｓ１１０）、所定時間が経過した時点で、第２の制御部からロボットの状態情報を受信する（Ｓ１０６）。受信したロボットの状態情報に基づいて、第２の制御部がエラー状態ではないか否かを判断し（Ｓ１０７）、エラー状態であると判断した場合には、上記と同様にエラー処理を行い（Ｓ１１１）、第２の制御部に終了指令を送信して（Ｓ１０９）、処理を終了する。

一方、エラー状態ではないと判断した場合には、続けて、受信したロボットの状態情報に基づいてロボットの各部の現在位置を求め、ロボットの各部が最終目標位置に到達したか否かを判断する（Ｓ１０８）。最終目標位置に到達したと判断する場合には、第２の制御部に終了指令を送信して（Ｓ１０９）、処理を終了する。

一方、最終目標位置に到達していないと判断した場合には、ステップＳ１０３から同様の処理を繰り返す。

次に、図１１を用いて第２の制御部が行う処理の流れについて説明する。

まず、第１の制御部から指令が出力されるのを待ち（Ｓ２０１）、指令が受信された場合に、受信した指令が終了指令か否かを判断する（Ｓ２０２）。終了指令を受信した場合には、処理を終了する。

一方、受信した指令が終了指令ではなく、制御指令であると判断した場合には、ステップＳ２０４からＳ２１０までの処理の反復用のカウンターｉを０に初期化する（Ｓ２０３）。そして、制御指令に基づいて、第２の周期単位で、ロボットのアームとハンドの目標位置を求め（Ｓ２０４）、アームとハンドを制御する（Ｓ２０５）。

そして、力覚センサーから検出値を取得して（Ｓ２０６）、力覚センサーの検出値が第１の制御部から通知された許容範囲外であるか否かを判断する（Ｓ２０７）。力覚センサーの検出値が許容範囲内であると判断した場合には、カウンターｉを更新し（Ｓ２０８）、所定時間待機した後（Ｓ２０９）、ロボットの状態情報を第１の制御部に送信する（Ｓ２１０）。そして、カウンターｉが所定のループ数に達したか否かを判断する（Ｓ２１１）。

ここで、所定のループ数は、今回の第１の周期中に、第２の周期の満了タイミングが含まれる回数である。所定のループ数は、あらかじめ定められている値でも、第２の制御部が第１の周期と第２の周期とに基づいて求めるものであってもよい。例えば、第１の周期が１０ｍｓ、第２の周期が２ｍｓである場合には、所定のループ数は５となる。

ステップＳ２１１において、カウンターｉが所定のループ数に達していると判断する場合には、ステップＳ２０１に戻り、待機状態となる。一方、カウンターｉが所定のループ数に達していないと判断する場合には、ステップＳ２０４から処理を繰り返す。

また、ステップＳ２０７において、力覚センサーの検出値が許容範囲外であると判断した場合には、第２の制御部は現在行っているロボットの制御を非常停止し（Ｓ２１２）、エラー処理（Ｓ２１３）を行って、エラー状態情報を第１の制御部に送信して（Ｓ２１４）、処理を終了する。なお、第２の制御部は、エラー処理として、エラーログを残す処理や、ロボットの状態情報の生成処理等を行う。

４．２計画軌道から外れた場合にロボットの軌道を修正する処理の流れ
まず、図１２を用いて第１の制御部が行う処理の流れについて説明する。計画軌道から外れた場合にロボットの軌道を修正する処理を行う場合には、ステップＳ３０３とＳ３０４の処理のみが、図１０の場合と異なる。その他のステップＳ３０１、Ｓ３０２、Ｓ３０５〜Ｓ３１１の処理は、それぞれステップＳ１０１、Ｓ１０２、Ｓ１０５〜Ｓ１１１の処理と同様である。

ステップＳ３０３では、力覚センサーの検出値の許容範囲ではなく、力覚センサーの検出目標値と、補正パラメーターを求める（Ｓ３０３）。具体的には、ロボットの各部が計画軌道を通って動作した場合に、力覚センサーの検出値を予測し、求めた予測値を力覚センサーの検出目標値とする。なお、この予測を行う場合には、ハンドにかかる重力項、および慣性項、またハンドがワークを把持している場合は、ワークに加わる重力、および慣性項が考慮される。さらにここでは、ロボットの制御を行った時に、求めた検出目標値とは異なる検出値が得られた場合に、ロボットの各部の軌道（位置、姿勢等）をどの方向に補正すれば、計画軌道に近づくかを示す補正パラメーターも求める。

そして、第２の制御部に、目標速度、力覚センサーの検出目標値、補正パラメーターを送信する（Ｓ３０４）。

次に、図１３を用いて、第２の制御部が行う処理の流れについて説明する。計画軌道から外れた場合にロボットの軌道を修正する処理を行う場合には、ステップＳ４０７の処理と、ステップＳ２１２〜Ｓ２１４に対応する処理を行わない点が、図１１の場合と異なる。

ステップＳ４０７では、力覚センサーの検出目標値と検出値との差（ずれ）を求め、力覚センサーの検出目標値と検出値との差、及び第１の制御部から取得された補正パラメーターに基づき、ロボットの軌道補正処理を行う。この場合には、力覚センサーの検出目標値と検出値との差が検出された場合でも、図１１のように、第２の制御部の非常停止処理等は行わない。

その他のステップＳ４０１〜Ｓ４０６、Ｓ４０８〜Ｓ４１１の処理は、それぞれステップＳ２０１〜Ｓ２０６、Ｓ２０８〜Ｓ２１１の処理と同様である。

以上のように本実施形態について詳細に説明したが、本発明の新規事項および効果から実体的に逸脱しない多くの変形が可能であることは当業者には容易に理解できるであろう。従って、このような変形例はすべて本発明の範囲に含まれるものとする。例えば、明細書又は図面において、少なくとも一度、より広義または同義な異なる用語と共に記載された用語は、明細書又は図面のいかなる箇所においても、その異なる用語に置き換えることができる。また、ロボット制御システム、ロボットシステム及びプログラムの構成、動作も本実施形態で説明したものに限定されず、種々の変形実施が可能である。

１００第１の情報処理装置、１１０第１の制御部、１２０記憶部、
１３０Ｉ／Ｆ部、２００第２の情報処理装置、２１０第２の制御部、
２２０記憶部、２３０Ｉ／Ｆ部、３００ロボット制御システム、
４００ロボット、４１０制御部、４２０アーム、４３０ハンド、
４４０力覚センサー、４５０Ｉ／Ｆ部、５００ロボットシステム、
６００ワーク

Claims

計画軌道に基づいて、第１の周期で、ロボットの制御指令を出力する第１の制御部と、
前記第１の制御部から得られる前記制御指令と、力覚センサーから得られる検出値とに基づいて、前記第１の周期よりも短い第２の周期で、前記ロボットを制御する第２の制御部と、
を含み、
前記第２の制御部は、
前記力覚センサーの検出値の許容範囲内に、取得された前記力覚センサーの検出値が含まれるか否かを判定し、
前記許容範囲内に前記力覚センサーの検出値が含まれないと判定した場合には、前記ロボットの軌道が前記計画軌道から外れたと判定することを特徴とするロボット制御システム。
請求項１において、
前記第１の制御部は、
前記計画軌道に対応する前記許容範囲を設定し、
前記第２の制御部は、
前記第１の制御部から得られる前記許容範囲に基づいて、前記許容範囲内に前記力覚センサーの検出値が含まれるか否かを判定することを特徴とするロボット制御システム。
請求項１又は２において、
前記第２の制御部は、
前記許容範囲内に前記力覚センサーの検出値が含まれないと判定した場合には、前記許容範囲内に前記力覚センサーの検出値が含まれないことを、前記第１の制御部に通知して、前記ロボットの制御の停止処理を行い、
前記許容範囲内に前記力覚センサーの検出値が含まれると判定した場合には、前記第２の周期で、前記ロボットの制御を行うことを特徴とするロボット制御システム。
請求項１乃至３のいずれかにおいて、
前記第２の制御部は、
ローカル座標系において、互いに直交する第１軸、第２軸、第３軸の３軸についての力成分を有する検出値を、前記力覚センサーから取得し、
前記ロボットの軌道が前記計画軌道と一致している時に前記力覚センサーの検出値の第３軸方向が鉛直方向と一致する場合に、前記力覚センサーの検出値の各力成分のうち、少なくとも一つの力成分が、前記許容範囲内に含まれるか否かを判定して、前記ロボットの軌道が前記計画軌道から外れたか否かを判定することを特徴とするロボット制御システム。
請求項１乃至４のいずれかにおいて、
前記第２の制御部は、
ローカル座標系において、互いに直交する第１軸、第２軸、第３軸の３軸のそれぞれを中心とするモーメント成分を有する検出値を、前記力覚センサーから取得し、
前記ロボットの軌道が前記計画軌道と一致している時に前記力覚センサーの検出値の第３軸方向が鉛直方向と一致する場合に、前記力覚センサーの検出値の各モーメント成分のうち、少なくとも一つのモーメント成分が、前記許容範囲内に含まれるか否かを判定して、前記ロボットの軌道が前記計画軌道から外れたか否かを判定することを特徴とするロボット制御システム。
請求項１乃至５のいずれかにおいて、
前記第２の制御部は、
前記許容範囲内に前記力覚センサーの検出値が含まれないと判定した場合には、前記力覚センサーの検出値に基づいて、前記ロボットの前記軌道の補正処理を行うことを特徴とするロボット制御システム。
計画軌道に基づいて、第１の周期で、ロボットの制御指令を出力する第１の制御部と、
前記第１の制御部から得られる前記制御指令と、前記力覚センサーから得られる検出値とに基づいて、前記第１の周期よりも短い第２の周期で、前記ロボットを制御する第２の制御部と、
を含み、
前記第２の制御部は、
前記ロボットの軌道の補正処理として、前記力覚センサーの検出値と検出目標値との差異に基づいて、前記ロボットを制御することを特徴とするロボット制御システム。
請求項６又は７において、
前記第１の制御部は、
前記ロボットの軌道が前記計画軌道と一致している場合の前記力覚センサーの検出値を、検出目標値として設定して、
前記第２の制御部は、
前記ロボットの前記軌道の前記補正処理として、前記力覚センサーから得られる前記力覚センサーの検出値と、前記第１の制御部から得られる前記力覚センサーの前記検出目標値との差異に基づいて、前記ロボットを制御することを特徴とするロボット制御システム。
請求項８において、
前記第１の制御部は、
前記力覚センサーの前記検出目標値を、前記第１の周期毎に、前記第２の制御部に出力し、
前記第２の制御部は、
前記第１の周期よりも短い第３の周期で、検出値を前記力覚センサーから取得する場合には、取得した前記力覚センサーの検出値と、前記検出目標値との前記差異を、前記第２の周期で求めることを特徴とするロボット制御システム。
請求項９において、
前記第１の制御部は、
前記第１の周期毎に、前記力覚センサーの前記検出目標値と逆ヤコビ行列とを出力し、
前記第２の制御部は、
前記第１の周期よりも短い第３の周期で、前記力覚センサーから検出値を取得する場合に、取得した前記力覚センサーの検出値と前記検出目標値との前記差異を、前記第２の周期で求め、前記差異に基づいて、前記第２の周期で、前記逆ヤコビ行列の補正処理を行うことを特徴とするロボット制御システム。
請求項１乃至９のいずれかにおいて、
前記第１の制御部は、
前記第１の周期毎に、逆ヤコビ行列を出力し、
前記第２の制御部は、
前記許容範囲内に前記力覚センサーの検出値が含まれないと判断した場合には、前記逆ヤコビ行列の更新処理要求を行うことを特徴とするロボット制御システム。
請求項１乃至１１のいずれかに記載のロボット制御システムを含むことを特徴とするロボットシステム。
計画軌道に基づいて、第１の周期で、ロボットの制御指令を出力する第１の制御部と、
前記第１の制御部から得られる前記制御指令と、力覚センサーから得られる検出値とに基づいて、前記第１の周期よりも短い第２の周期で、前記ロボットを制御する第２の制御部として、
コンピューターを機能させ、
前記第２の制御部は、
力覚センサーの検出値の許容範囲内に、取得された前記力覚センサーの検出値が含まれるか否かを判定し、
前記許容範囲内に前記力覚センサーの検出値が含まれないと判定した場合には、前記ロボットの軌道が前記計画軌道から外れたと判定することを特徴とするプログラム。