JP6217322B2

JP6217322B2 - ロボット制御装置、ロボット及びロボット制御方法

Info

Publication number: JP6217322B2
Application number: JP2013228653A
Authority: JP
Inventors: 正樹元▲吉▼
Original assignee: Seiko Epson Corp
Current assignee: Seiko Epson Corp
Priority date: 2013-11-01
Filing date: 2013-11-01
Publication date: 2017-10-25
Anticipated expiration: 2033-11-01
Also published as: JP2015085491A

Description

本発明は、ロボット制御装置、ロボット及びロボット制御方法等に関する。

ロボットの制御において、撮像部により撮像された撮像画像等の画像情報を用いる手法が知られている。画像情報に基づくロボット制御としては、実時間で画像を取得して、その情報をもとにロボットを制御するビジュアルサーボが考えられる。ビジュアルサーボには大きく分けて位置ベースの手法と特徴量ベースの手法がある。

位置ベースの手法は、画像処理により対象物の位置姿勢を検出し、その位置姿勢に基づいてロボットを動作させる。一方、特徴量ベースの手法は、対象物を動かした際に画像の特徴量（画像中の領域や面積、線分の長さ、特徴点の位置、等の特徴を表す量）がどのように変化するかという情報を動作と直接対応付けて、ロボットを動作させる。特徴量ベースの手法ではカメラとロボットの間のキャリブレーションの精度が低い場合も動作させることが出来る等の利点がある。

特許文献１には、位置ベースのビジュアルサーボにおいて、画像から算出した空間的位置もしくは移動速度とエンコーダーから算出した空間的位置もしくは移動速度を比較して、異常を検出する手法が開示されている。

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

ロボットの制御で取得される情報（例えばエンコーダーの情報）から、ロボットの手先等の位置姿勢を求めることは容易である。特許文献１は、位置ベースのビジュアルサーボを前提としているため、ロボットの制御において画像から位置姿勢（狭義には空間的位置）を求める。つまり位置ベースの場合であれば、容易に求められるロボット等の位置姿勢と、制御に用いられる画像から求めた位置姿勢との比較処理を行えばよいため、異常を検出する手法を容易に実現できる。

しかし、特徴量ベースの手法では、画像特徴量とロボットの位置姿勢との対応を直接的に決めることが出来ないために特許文献１の手法を適用することが出来ない。

本発明の一態様は、画像情報に基づいてロボットを制御するロボット制御部と、前記画像情報に基づいて、画像特徴量変化量を求める変化量演算部と、前記ロボット又は対象物の情報であって、前記画像情報以外の情報である変化量推定用情報に基づいて、前記画像特徴量変化量の推定量である推定画像特徴量変化量を演算する変化量推定部と、前記画像特徴量変化量と前記推定画像特徴量変化量の比較処理により異常判定を行う異常判定部と、を含むロボット制御装置に関係する。

本発明の一態様では、画像特徴量変化量と、変化量推定用情報に基づいて求められた推定画像特徴量変化量に基づいて、画像情報を用いたロボットの制御における異常判定を行う。これにより、画像情報を用いたロボットの制御のうち、特に画像特徴量を用いる手法において、異常判定を適切に行うこと等が可能になる。

本発明の一態様では、前記変化量推定用情報は、前記ロボットの関節角情報であってもよい。

これにより、変化量推定用情報として、ロボットの関節角情報を用いることが可能になる。

また、本発明の一態様では、前記変化量推定部は、前記関節角情報の変化量に対して、前記関節角情報の変化量と前記画像特徴量変化量を対応付けるヤコビアンを作用させることで、前記推定画像特徴量変化量を演算してもよい。

これにより、関節角情報の変化量とヤコビアンを用いて推定画像特徴量変化量を求めること等が可能になる。

また、本発明の一態様では、前記変化量推定用情報は、前記ロボットのエンドエフェクター又は前記対象物の位置姿勢情報であってもよい。

これにより、変化量推定用情報として、ロボットのエンドエフェクター又は前記対象物の位置姿勢情報を用いることが可能になる。

また、本発明の一態様では、前記変化量推定部は、前記位置姿勢情報の変化量に対して、前記位置姿勢情報の変化量と前記画像特徴量変化量を対応付けるヤコビアンを作用させることで、前記推定画像特徴量変化量を演算してもよい。

これにより、位置姿勢情報の変化量とヤコビアンを用いて推定画像特徴量変化量を求めること等が可能になる。

また、本発明の一態様では、第１の画像情報の画像特徴量ｆ１が第ｉ（ｉは自然数）のタイミングで取得され、第２の画像情報の画像特徴量ｆ２が第ｊ（ｊはｊ≠ｉを満たす自然数）のタイミングで取得された場合に、前記変化量演算部は、前記画像特徴量ｆ１と前記画像特徴量ｆ２の差分を、前記画像特徴量変化量として求め、前記変化量推定部は、前記第１の画像情報に対応する前記変化量推定用情報ｐ１が第ｋ（ｋは自然数）のタイミングで取得され、前記第２の画像情報に対応する前記変化量推定用情報ｐ２が第ｌ（ｌは自然数）のタイミングで取得された場合に、前記変化量推定用情報ｐ１と前記変化量推定用情報ｐ２とに基づいて、前記推定画像特徴量変化量を求めてもよい。

これにより、タイミングを考慮して、対応する画像特徴量変化量と推定画像特徴量変化量を求めること等が可能になる。

また、本発明の一態様では、前記第ｋのタイミングは、前記第１の画像情報の取得タイミングであり、前記第ｌのタイミングは、前記第２の画像情報の取得タイミングであってもよい。

これにより、関節角情報の取得が高速で行えると考えられるケースにおいて、タイミングを考慮した処理を容易に行うこと等が可能になる。

また、本発明の一態様では、前記異常判定部は、前記画像特徴量変化量と前記推定画像特徴量変化量の差分情報と、閾値との比較処理を行い、前記差分情報が前記閾値よりも大きい場合に異常と判定してもよい。

これにより、閾値判定により異常判定を行うこと等が可能になる。

また、本発明の一態様では、前記異常判定部は、前記変化量演算部での、前記画像特徴量変化量の演算に用いられる２つの前記画像情報の取得タイミングの差が大きいほど、前記閾値を大きく設定してもよい。

これにより、状況に応じて閾値を変更すること等が可能になる。

また、本発明の一態様では、前記異常判定部により異常が検出された場合に、前記ロボット制御部は、前記ロボットを停止させる制御を行ってもよい。

これにより、異常検出時にロボットを停止させることで、安全なロボットの制御を実現すること等が可能になる。

また、本発明の一態様では、前記異常判定部により異常が検出された場合に、前記ロボット制御部は、前記変化量演算部での、前記画像特徴量変化量の演算に用いられる２つの前記画像情報のうち、時系列的に後のタイミングで取得された前記画像情報である異常判定画像情報に基づく制御をスキップし、前記異常判定画像情報よりも前のタイミングで取得された前記画像情報に基づいて制御を行ってもよい。

これにより、異常検出時に、異常判定画像情報を用いたロボットの制御をスキップすること等が可能になる。

また、本発明の他の態様は、画像情報に基づいてロボットを制御するロボット制御部と、前記ロボットのエンドエフェクター又は対象物の位置姿勢情報の変化量を表す位置姿勢変化量、又は、前記ロボットの関節角情報の変化量を表す関節角変化量を求める変化量演算部と、前記画像情報に基づいて、画像特徴量変化量を求め、前記画像特徴量変化量に基づいて、前記位置姿勢変化量の推定量である推定位置姿勢変化量、又は、前記関節角変化量の推定量である推定関節角変化量を求める変化量推定部と、前記位置姿勢変化量と前記推定位置姿勢変化量の比較処理、又は、前記関節角変化量と前記推定関節角変化量の比較処理により異常判定を行う異常判定部と、を含むロボット制御装置に関係する。

本発明の他の態様では、画像特徴量変化量から推定位置姿勢変化量又は推定関節角変化量を求め、位置姿勢変化量と推定位置姿勢変化量の比較、又は関節角変化量と推定関節角変化量比較により異常を判定する。これにより、位置姿勢情報又は関節角情報の比較によっても、画像情報を用いたロボット制御のうち、特に画像特徴量を用いる手法において、異常判定を適切に行うこと等が可能になる。

また、本発明の一態様では、前記変化量演算部は、複数の前記位置姿勢情報を取得して、複数の前記位置姿勢情報の差分を前記位置姿勢変化量として求める処理、複数の前記位置姿勢情報を取得して、複数の前記位置姿勢情報の差分に基づいて前記関節角変化量を求める処理、複数の前記関節角情報を取得して、複数の前記関節角情報の差分を前記関節角変化量として求める処理、及び複数の前記関節角情報を取得して、複数の前記関節角情報の差分に基づいて前記位置姿勢変化量を求める処理、のいずれかの処理を行ってもよい。

これにより、位置姿勢変化量又は関節角変化量を種々の手法により求めること等が可能になる。

また、本発明の他の態様は、画像情報に基づいてロボットを制御するロボット制御部と、前記画像情報に基づいて、画像特徴量変化量を求める変化量演算部と、前記ロボット又は対象物の情報であって、前記画像情報以外の情報である変化量推定用情報に基づいて、前記画像特徴量変化量の推定量である推定画像特徴量変化量を演算する変化量推定部と、前記画像特徴量変化量と前記推定画像特徴量変化量の比較処理により異常判定を行う異常判定部と、を含むロボットに関係する。

本発明の他の態様では、画像特徴量変化量と、変化量推定用情報に基づいて求められた推定画像特徴量変化量に基づいて、画像情報を用いたロボットの制御における異常判定を行う。これにより、画像情報を用いたロボットの制御のうち、特に画像特徴量を用いる手法において、異常判定を適切に行うこと等が可能になる。

また、本発明の他の態様は、画像情報に基づいてロボットを制御するロボット制御方法であって、前記画像情報に基づいて、画像特徴量変化量を求める変化量演算処理を行うことと、前記ロボット又は対象物の情報であって、前記画像情報以外の情報である変化量推定用情報に基づいて、前記画像特徴量変化量の推定量である推定画像特徴量変化量を演算する変化量推定処理を行うことと、前記画像特徴量変化量と前記推定画像特徴量変化量の比較処理により異常判定を行うことと、を含むロボット制御方法に関係する。

また、本発明の他の態様は、画像情報に基づいてロボットを制御するロボット制御部と、前記画像情報に基づいて、画像特徴量変化量を求める変化量演算部と、前記ロボット又は対象物の情報であって、前記画像情報以外の情報である変化量推定用情報に基づいて、前記画像特徴量変化量の推定量である推定画像特徴量変化量を演算する変化量推定部と、前記画像特徴量変化量と前記推定画像特徴量変化量の比較処理により異常判定を行う異常判定部として、コンピューターを機能させるプログラムに関係する。

本発明の他の態様では、画像特徴量変化量と、変化量推定用情報に基づいて求められた推定画像特徴量変化量に基づいて、画像情報を用いたロボットの制御における異常判定をコンピューターに実行させる。これにより、画像情報を用いたロボットの制御のうち、特に画像特徴量を用いる手法において、異常判定を適切に行うこと等が可能になる。

このように、本発明の幾つかの態様によれば、画像情報に基づくロボットの制御のうち、画像特徴量を用いた制御における異常の検出を適切に行うロボット制御装置、ロボット及びロボット制御方法等を提供することができる。

本実施形態に係るロボット制御装置の構成例。本実施形態に係るロボット制御装置の詳細な構成例。画像情報を取得する撮像部の配置例。本実施形態に係るロボットの構成例。本実施形態に係るロボットの構造の例。本実施形態に係るロボットの構造の他の例。本実施形態に係るロボット制御装置がサーバーにより実現される例。一般的なビジュアルサーボ制御系の構成例。画像特徴量変化量、位置姿勢情報の変化量及び関節角情報の変化量と、ヤコビアンの関係を説明する図。ビジュアルサーボ制御を説明する図。図１１（Ａ）、図１１（Ｂ）は本実施形態に係る異常検出手法の説明図。画像取得タイミングの差に応じて閾値を設定する手法の説明図。画像取得タイミング、関節角情報の取得タイミング及び画像特徴量取得タイミングの関係を表す図。画像取得タイミング、関節角情報の取得タイミング及び画像特徴量取得タイミングの関係を表す他の図。画像特徴量変化量、位置姿勢情報の変化量及び関節角情報の変化量の相互の関係を数式と合わせて説明する図。本実施形態の処理を説明するフローチャート。本実施形態に係るロボット制御装置の他の詳細な構成例。

以下、本実施形態について説明する。なお、以下に説明する本実施形態は、特許請求の範囲に記載された本発明の内容を不当に限定するものではない。また本実施形態で説明される構成の全てが、本発明の必須構成要件であるとは限らない。

１．本実施形態の手法
画像情報を用いたロボット制御が広く知られている。例えば、連続的に画像情報を取得し、当該画像情報から取得される情報と、目標となる情報との比較処理の結果をフィードバックするビジュアルサーボ制御が知られている。ビジュアルサーボでは、最新の画像情報から取得された情報と、目標となる情報との差が小さくなる方向にロボットを制御する。具体的には、目標に近づくような関節角の変化量等を求め、当該変化量等に従って関節を駆動する制御を行う。

目標となるロボットの手先等の位置姿勢が与えられ、当該目標の位置姿勢となるようにロボットを制御する手法では、位置決め精度を高くすること、すなわち手先（ハンド）等を正確に目標の位置姿勢に移動させることは容易ではない。理想的には、ロボットのモデルが決定されれば、当該モデルに従って手先位置姿勢を一意に求めることができる。ここでのモデルとは、例えば２つの関節の間に設けられるフレーム（リンク）の長さや、関節の構造（関節の回転方向や、オフセットがあるか否か等）等の情報である。

しかし、ロボットには種々の誤差が含まれる。例えばリンクの長さのずれや重力によるたわみ等である。これらの誤差要因により、ロボットに所与の姿勢を取らせる制御（例えば各関節の角度を決定する制御）を行った場合に、理想的な位置姿勢と、実際の位置姿勢とが異なる値となってしまう。

その点、ビジュアルサーボ制御では撮像画像に対する画像処理結果をフィードバックするため、人間が作業状況を目で見ながら腕や手の動かし方を微調整できるのと同様に、現在の位置姿勢が目標の位置姿勢からずれていたとしても、当該ずれを認識して修正することができる。

ビジュアルサーボ制御では、上記の「画像から取得された情報」及び「目標となる情報」として、ロボットの手先等の３次元的な位置姿勢情報を用いることもできるし、画像から取得される画像特徴量を位置姿勢情報に変換することなく用いることもできる。位置姿勢情報を用いるビジュアルサーボを位置ベースのビジュアルサーボと呼び、画像特徴量を用いるビジュアルサーボを特徴量ベースのビジュアルサーボと呼ぶ。

ビジュアルサーボを適切に行うためには、画像情報から位置姿勢情報或いは画像特徴量を精度よく検出する必要がある。この検出処理の精度が低いと、現在の状態を誤って認識することになってしまう。そのため、制御ループにフィードバックされる情報も、ロボットの状態を目標状態に適切に近づけるものにならず、精度の高いロボット制御ができなくなってしまう。

位置姿勢情報も画像特徴量も、何らかの検出処理（例えばマッチング処理）等により求めることが想定されるが、当該検出処理の精度は必ずしも充分ではない。なぜなら、実際にロボットが動作する環境では、撮像画像中には認識対象である物体（例えばロボットのハンド）だけでなく、ワークや治具、或いは動作環境に配置された物体等も撮像されてしまうためである。画像の背景に種々の物体が写り込むことで、所望の物体の認識精度（検出精度）は低下してしまい、求められる位置姿勢情報や画像特徴量の精度も低くなってしまう。

特許文献１では、位置ベースのビジュアルサーボにおいて、画像から算出した空間的位置もしくは移動速度とエンコーダーから算出した空間的位置もしくは移動速度を比較して、異常を検出する手法が開示されている。なお、空間的位置は位置姿勢情報に含まれるものであり、移動速度も位置姿勢情報の変化量から求められる情報であるため、以下では空間的位置もしくは移動速度を、位置姿勢情報として説明する。

特許文献１の手法を用いることで、画像情報から求めた位置姿勢情報に大きな誤差が生じている等、ビジュアルサーボに何らかの異常が発生している場合に、当該異常を検出することが可能になると考えられる。異常の検出ができれば、ロボットの制御を停止するにしろ、位置姿勢情報の検出をやり直すにしろ、少なくとも異常な情報をそのまま制御に用いることを抑止できる。

しかし、特許文献１の手法は位置ベースのビジュアルサーボを前提としている。位置ベースであれば上述したように、エンコーダー等の情報から容易に求められる位置姿勢情報と、画像情報から求めた位置姿勢情報との比較処理を行えばよいため、実現は容易である。一方、特徴量ベースのビジュアルサーボでは、ロボットの制御においては画像特徴量を用いる。そして、エンコーダー等の情報からは、ロボットの手先等の空間的位置を求めることは容易であっても画像特徴量との関係は直接的に求められるものではない。つまり、特徴量ベースのビジュアルサーボを想定した場合、特許文献１の手法を適用することは困難であった。

そこで本出願人は、画像特徴量を用いた制御において、実際に画像情報から取得された画像特徴量変化量と、ロボット制御の結果取得される情報から推定した推定画像特徴量変化量とを用いて異常を検出する手法を提案する。具体的には、本実施形態に係るロボット制御装置１００は、図１に示したように、画像情報に基づいてロボット２００を制御するロボット制御部１１０と、画像情報に基づいて、画像特徴量変化量を求める変化量演算部１２０と、ロボット２００又は対象物の情報であって、画像情報以外の情報である変化量推定用情報に基づいて、画像特徴量変化量の推定量である推定画像特徴量変化量を演算する変化量推定部１３０と、画像特徴量変化量と推定画像特徴量変化量の比較処理により異常判定を行う異常判定部１４０を含む。

ここで、画像特徴量とは、上述したように画像中の領域や面積、線分の長さ、特徴点の位置等の特徴を表す量であり、画像特徴量変化量とは、複数の（狭義には２つの）画像情報から取得された複数の画像特徴量の間の変化を表す情報である。画像特徴量として３つの特徴点の画像上での２次元位置を用いる例であれば、画像特徴量は６次元のベクトルであり、画像特徴量変化量は２つの６次元ベクトルの差、すなわちベクトルの各要素の差を要素とする６次元ベクトルとなる。

また、変化量推定用情報とは、画像特徴量変化量の推定に用いられる情報であって、画像情報以外の情報である。変化量推定用情報は、例えばロボット制御の結果により取得される（実測される）情報であってもよく、具体的にはロボット２００の関節角情報であってもよい。関節角情報は、ロボットの関節駆動用のモーター（広義にはアクチュエーター）の動作を測定、制御するエンコーダーから取得することが可能である。或いは、変化量推定用情報は、ロボット２００のエンドエフェクター２２０、又はロボット２００による作業の対象物の位置姿勢情報であってもよい。位置姿勢情報は，例えば物体の基準点の３次元位置（ｘ，ｙ，ｚ）と、基準姿勢に対する各軸周りの回転（Ｒ１，Ｒ２，Ｒ３）を含む６次元ベクトルである。物体の位置姿勢情報を求める手法は種々考えられるが、例えば超音波を用いた距離測定手法や、測量機を用いる手法、手先にＬＥＤ等を設けて当該ＬＥＤを検出して測量する手法、機械式の３次元測定器を用いる手法等を用いればよい。

このようにすれば、画像特徴量を用いたロボット制御（狭義には特徴量ベースのビジュアルサーボ）において、異常を検出することが可能になる。この際、実際に取得した画像情報から求めた画像特徴量変化量と、画像情報とは異なる観点から取得された変化量推定用情報から求めた推定画像特徴量変化量との比較処理が行われることになる。

なお、画像情報に基づくロボットの制御はビジュアルサーボに限定されるものではない。例えば、ビジュアルサーボでは、制御ループに対する画像情報に基づく情報のフィードバックを継続するが、１回の画像情報の取得を行い、当該画像情報に基づいて目標位置姿勢への移動量を求め、当該移動量に基づいて位置制御を行うビジョン方式を、画像情報に基づくロボットの制御として用いてもよい。その他、ビジュアルサーボやビジョン方式以外であっても、画像情報を用いるロボットの制御において、画像情報からの情報の検出等において異常を検出する手法として、本実施形態の手法を適用可能である。

ただし、後述するように本実施形態の手法では、推定画像特徴量変化量の演算にヤコビアンを用いることを想定している。そしてヤコビアンは、所与の値の変化量と、他の値の変化量との関係を表す情報である。例えば、第１の情報ｘと第２の情報ｙが非線形な関係（ｙ＝ｇ（ｘ）におけるｇが非線形関数）であっても、所与の値の近傍においては第１の情報の変化量Δｘと第２の情報の変化量Δｙが線形な関係（Δｙ＝ｈ（Δｘ）におけるｈが線形関数）であると考えることができ、ヤコビアンは当該線形な関係を表す。つまり、本実施形態では画像特徴量そのものではなく、画像特徴量変化量を処理に用いることが想定される。よって、本実施形態の手法をビジョン方式等、ビジュアルサーボ以外に適用する場合には、画像情報を１回だけ取得する手法ではなく、少なくとも画像特徴量変化量が求められるように２回以上画像情報を取得する手法を用いる必要がある点に留意すべきである。例えば本実施形態の手法をビジョン方式に適用するのであれば、画像情報の取得及び目標となる移動量の演算を複数回行うものとする必要がある。

以下、本実施形態に係るロボット制御装置１００やロボットのシステム構成例について説明した後、ビジュアルサーボの概要について説明する。その上で、本実施形態の異常検出手法を説明し、最後に、変形例についても説明する。なお、以下では画像情報を用いたロボットの制御としてビジュアルサーボを例に取るが、以下の説明は、その他の画像情報を用いたロボットの制御に拡張可能である。

２．システム構成例
図２に本実施形態に係るロボット制御装置１００の詳細なシステム構成例を示す。ただし、ロボット制御装置１００は図２の構成に限定されず、これらの一部の構成要素を省略したり、他の構成要素を追加するなどの種々の変形実施が可能である。

図２に示したように、ロボット制御装置１００は、目標特徴量入力部１１１と、目標軌道生成部１１２と、関節角制御部１１３と、駆動部１１４と、関節角検出部１１５と、画像情報取得部１１６と、画像特徴量演算部１１７と、変化量演算部１２０と、変化量推定部１３０と、異常判定部１４０を含む。

目標特徴量入力部１１１は、目標となる画像特徴量ｆｇを目標軌道生成部１１２に対して入力する。目標特徴量入力部１１１は、例えば、ユーザーによる目標画像特徴量ｆｇの入力を受け付けるインターフェース等として実現されてもよい。ロボット制御においては、画像情報から求められる画像特徴量ｆを、ここで入力される目標画像特徴量ｆｇに近づける（狭義には一致させる）制御が行われる。なお、目標画像特徴量ｆｇは、目標状態に対応する画像情報（参照画像、ゴール画像）を取得し、当該画像情報から求めてもよい。或いは、参照画像は保持せずに、直接目標画像特徴量ｆｇの入力を受け付けてもよい。

目標軌道生成部１１２は、目標画像特徴量ｆｇと、画像情報から求められた画像特徴量ｆとに基づいて、ロボット２００を動作させる目標軌道を生成する。具体的には、ロボット２００を目標状態（ｆｇに対応する状態）に近づけるための関節角の変化量Δθｇを求める処理を行う。このΔθｇは関節角の暫定的な目標値となる。なお、目標軌道生成部１１２では、Δθｇから、単位時間当たりの関節角の駆動量（図２におけるドット付きθｇ）を求めてもよい。

関節角制御部１１３は、関節角の目標値Δθｇと、現在の関節角の値θとに基づいて、関節角の制御を行う。例えば、Δθｇは関節角の変化量であるため、θとΔθｇを用いて、関節角をどのような値にすればよいかを求める処理を行う。駆動部１１４は、関節角制御部１１３の制御に従って、ロボット２００の関節を駆動する制御を行う。

関節角検出部１１５は、ロボット２００の関節角が、どのような値となっているかを検出する処理を行う。具体的には、駆動部１１４による駆動制御により関節角が変化した後に、当該変化後の関節角の値を検出して、現在の関節角の値θとして関節角制御部１１３に出力する。関節角検出部１１５は、具体的にはエンコーダーの情報を取得するインターフェース等として実現されてもよい。

画像情報取得部１１６は、撮像部等から画像情報の取得を行う。ここでの撮像部は、図３に示したように環境に配置されるものであってもよいし、ロボット２００のアーム２１０等に設けられる撮像部（例えばハンドアイカメラ）であってもよい。画像特徴量演算部１１７は、画像情報取得部１１６が取得した画像情報に基づいて、画像特徴量の演算処理を行う。なお、画像情報から画像特徴量を演算する手法は、エッジ検出処理やマッチング処理等、種々の手法が知られており、本実施形態ではそれらを広く適用可能であるため、詳細な説明は省略する。画像特徴量演算部１１７で求められた画像特徴量は、最新の画像特徴量ｆとして、目標軌道生成部１１２に出力される。

変化量演算部１２０は、画像特徴量演算部１１７で演算された画像特徴量を保持しておき、過去に取得された画像特徴量ｆｏｌｄと、処理対象である画像特徴量ｆ（狭義には最新の画像特徴量）との差分から、画像特徴量変化量Δｆを演算する。

変化量推定部１３０は、関節角検出部１１５で検出された関節角情報を保持しておき、過去に取得された関節角情報θｏｌｄと、処理対象である関節角情報θ（狭義には最新の関節角情報）との差分から、関節角情報の変化量Δθを演算する。さらに、Δθに基づいて、推定画像特徴量変化量Δｆｅを求める。なお、図２では変化量推定用情報が関節角情報である例について説明したが、変化量推定用情報としてロボット２００のエンドエフェクター２２０又は対象物の位置姿勢情報を用いてもよいことは上述したとおりである。

なお、図１のロボット制御部１１０は、図２の目標特徴量入力部１１１と、目標軌道生成部１１２と、関節角制御部１１３と、駆動部１１４と、関節角検出部１１５と、画像情報取得部１１６と、画像特徴量演算部１１７とに対応するものであってもよい。

また本実施形態の手法は、図４に示したように、画像情報に基づいてロボット（具体的には、アーム２１０及びエンドエフェクター２２０を含むロボット本体３００）を制御するロボット制御部１１０と、画像情報に基づいて、画像特徴量変化量を求める変化量演算部１２０と、ロボット２００又は対象物の情報であって、画像情報以外の情報である変化量推定用情報に基づいて、画像特徴量変化量の推定量である推定画像特徴量変化量を演算する変化量推定部１３０と、画像特徴量と推定画像特徴量変化量の比較処理により異常判定を行う異常判定部１４０を含むロボットに適用できる。

ここでのロボットとは、図５に示したように、制御装置６００と、ロボット本体３００と、を含むロボットであってもよい。図５の構成であれば、制御装置６００に図４のロボット制御部１１０等が含まれる。そしてロボット本体３００は、アーム２１０と、エンドエフェクター２２０を含む。このようにすれば、画像情報に基づく制御に従った動作を行いつつ、制御における異常を自動検出するロボットを実現することが可能になる。

なお、本実施形態に係るロボットの構成例は図５に限定されない。例えば、図６に示したように、ロボットは、ロボット本体３００と、ベースユニット部３５０を含んでもよい。本実施形態に係るロボットは図６に示したように双腕ロボットであってもよく、頭部や胴体に相当する部分に加え、第１のアーム２１０−１と第２のアーム２１０−２を含む。図６では第１のアーム２１０−１は、関節２１１，２１３と、関節の間に設けられるフレーム２１５，２１７から構成され、第２のアーム２１０−２についても同様のものとしたがこれに限定されない。なお、図６では２本のアームを有する双腕ロボットの例を示したが、本実施形態のロボットは３本以上のアームを有してもよい。

ベースユニット部３５０は、ロボット本体３００の下部に設けられ、ロボット本体３００を支持する。図６の例では、ベースユニット部３５０には車輪等が設けられ、ロボット全体が移動可能な構成となっている。ただし、ベースユニット部３５０が車輪等を持たず、床面等に固定される構成であってもよい。図６では図５の制御装置６００に対応する装置が不図示であるが、図６のロボットシステムでは、ベースユニット部３５０に制御装置６００が格納されることで、ロボット本体３００と制御装置６００とが一体として構成される。

或いは、制御装置６００のように、特定の制御用の機器を設けることなく、ロボットに内蔵される基板（更に具体的には基板上に設けられるＩＣ等）により、上記のロボット制御部１１０等を実現してもよい。

また、図７に示すように、ロボット制御装置１００の機能は、有線及び無線の少なくとも一方を含むネットワーク４００を介して、ロボットと通信接続されたサーバー５００により実現されてもよい。

或いは本実施形態では、本発明のロボット制御装置の処理の一部を、ロボット制御装置であるサーバー５００が行ってもよい。この場合には、ロボット側に設けられたロボット制御装置との分散処理により、当該処理を実現する。

そして、この場合に、ロボット制御装置であるサーバー５００は、本発明のロボット制御装置における各処理のうち、サーバー５００に割り当てられた処理を行う。一方、ロボットに設けられたロボット制御装置は、本発明のロボット制御装置の各処理のうち、ロボットのロボット制御装置に割り当てられた処理を行う。

例えば、本発明のロボット制御装置が第１〜第Ｍ（Ｍは整数）の処理を行うものであり、第１の処理がサブ処理１ａ及びサブ処理１ｂにより実現され、第２の処理がサブ処理２ａ及びサブ処理２ｂにより実現されるように、第１〜第Ｍの各処理が複数のサブ処理に分割できる場合を考える。この場合、ロボット制御装置であるサーバー５００がサブ処理１ａ、サブ処理２ａ、・・・サブ処理Ｍａを行い、ロボット側に設けられたロボット制御装置がサブ処理１ｂ、サブ処理２ｂ、・・・サブ処理Ｍｂを行うといった分散処理が考えられる。この際、本実施形態に係るロボット制御装置、すなわち、第１〜第Ｍの処理を実行するロボット制御装置とは、サブ処理１ａ〜サブ処理Ｍａを実行するロボット制御装置であってもよいし、サブ処理１ｂ〜サブ処理Ｍｂを実行するロボット制御装置であってもよいし、サブ処理１ａ〜サブ処理Ｍａ及びサブ処理１ｂ〜サブ処理Ｍｂの全てを実行するロボット制御装置であってもよい。更にいえば、本実施形態に係るロボット制御装置は、第１〜第Ｍの処理の各処理について、少なくとも１つのサブ処理を実行するロボット制御装置である。

これにより、例えばロボット側の端末装置（例えば図５の制御装置６００）よりも処理能力の高いサーバー５００が、処理負荷の高い処理を行うこと等が可能になる。さらに、サーバー５００が各ロボットの動作を一括して制御することができ、例えば複数のロボットに協調動作をさせること等が容易になる。

また近年は、多品種少数の部品を製造することが増えてきている。そして、製造する部品の種類を変更する場合には、ロボットが行う動作を変更する必要がある。図７に示すような構成であれば、複数のロボットの各ロボットへ教示作業をし直さなくても、サーバー５００が一括して、ロボットが行う動作を変更すること等が可能になる。さらに、各ロボットに対して一つのロボット制御装置１００を設ける場合に比べて、ロボット制御装置１００のソフトウェアアップデートを行う際の手間を大幅に減らすこと等が可能になる。

３．ビジュアルサーボ制御
本実施形態に係る異常検出手法を説明する前に、一般的なビジュアルサーボ制御について説明する。一般的なビジュアルサーボ制御系の構成例を図８に示しす。図８からわかるように、図２に示した本実施形態に係るロボット制御装置１００と比較した場合に、変化量演算部１２０と、変化量推定部１３０と、異常判定部１４０が除かれた構成となっている。

ビジュアルサーボに用いる画像特徴量の次元数をｎ（ｎは整数）とした場合に、画像特徴量ｆはｆ＝［ｆ１，ｆ２，・・・，ｆｎ］^Ｔとなる画像特徴量ベクトルで表現される。ｆの各要素は、例えば特徴点（制御点）の画像における座標値等を用いればよい。この場合、目標特徴量入力部１１１から入力される目標画像特徴量ｆｇも同様に、ｆｇ＝［ｆｇ１，ｆｇ２，・・・，ｆｇｎ］^Ｔと表現される。

また、関節角についても、ロボット２００（狭義にはアーム２１０）に含まれる関節数に対応する次元数の関節角ベクトルとして表現される。例えば、アーム２１０が６つの関節を有する６自由度のアームであれば、関節角ベクトルθは、θ＝［θ１，θ２，・・・，θ６］^Ｔと表現される。

ビジュアルサーボでは、現在の画像特徴量ｆが取得された場合に、当該画像特徴量ｆと目標画像特徴量ｆｇとの差分をロボットの動きへフィードバックする。具体的には、画像特徴量ｆと目標画像特徴量ｆｇの差分を小さくする方向にロボットを動作させる。そのためには、関節角θをどのように動かせば、画像特徴量ｆがどのように変化する、という関係性を知らなくてはならない。一般的にこの関係性は非線形となり、例えばｆ１＝ｇ（θ１，θ２，θ３，θ４，θ５，θ６）となる場合に、関数ｇは非線形関数となる。

そこで、ビジュアルサーボにおいてヤコビアンＪを用いる手法が広く知られている。２つの空間が非線形な関係にあっても、それぞれの空間での微少な変化量の間は線形な関係で表現することができる。ヤコビアンＪはその微少変化量同士を関連づけるものである。

具体的には、ロボット２００の手先の位置姿勢ＸをＸ＝［ｘ，ｙ，ｚ，Ｒ１，Ｒ２，Ｒ３］^Ｔとした場合に、関節角の変化量と位置姿勢の変化量の間のヤコビアンＪａは、下式（１）で表現され、位置姿勢の変化量と画像特徴量変化量の間のヤコビアンＪｉは、下式（２）で表現される。

そして、Ｊａ，Ｊｉを用いることで、Δθ、ΔＸ、Δｆの関係を下式（３）、（４）のように表記できる。Ｊａは一般的にロボットヤコビアンと呼ばれるものであり、ロボット２００のリンク長や回転軸等の機構情報があれば解析的に算出することが出来る。一方、Ｊｉは事前にロボット２００の手先の位置姿勢を微少量変化させた際の画像特徴量の変化などから推測することが出来るし、Ｊｉを動作中に随時推定する手法も提案されている。
ΔＸ＝ＪａΔθ ・・・・・（３）
Δｆ＝ＪｉΔＸ・・・・・（４）

さらに、上式（３）、（４）を用いることで、画像特徴量変化量Δｆと、関節角の変化量Δθの関係を下式（５）のように表現できる。
Δｆ＝ＪｖΔθ ・・・・・（５）

ここで、Ｊｖ＝ＪｉＪａであり、関節角の変化量と画像特徴量変化量の間のヤコビアンを表す。また、Ｊｖを画像ヤコビアンと表記することもある。上式（３）〜（５）の関係性を図示したものが図９である。

以上をふまえると、目標軌道生成部１１２は、ｆとｆｇの差分をΔｆとして、関節角の駆動量（関節角の変化量）Δθを求めればよい。このようにすれば、画像特徴量ｆをｆｇに近づけるための関節角の変化量を求めることができる。具体的には、ΔｆからΔθを求めるため、上式（５）の両辺に左からＪｖの逆行列Ｊｖ^−１をかければよいが、さらに制御ゲインをλを考慮して、下式（６）により目標となる関節角の変化量Δθｇを求める。
Δθｇ＝−λＪｖ^−１（ｆ−ｆｇ）・・・・・（６）

なお、上式（６）ではＪｖの逆行列Ｊｖ^−１を求められるものとしているが、Ｊｖ^−１を求められない場合には、Ｊｖの一般化逆行列（疑似逆行列）Ｊｖ^＃を用いてもよい。

上式（６）を用いることで、新たな画像が取得されるごとに、新たなΔθｇが求められる。よって、取得した画像を用いて、目標となる関節角を更新しながら、ゴール状態（画像特徴量がｆｇとなる状態）に近づける制御を行うことが可能になる。この流れを図示したものが図１０である。第ｍ−１の画像（ｍは整数）から画像特徴量ｆｍ−１が求められれば、上式（６）のｆ＝ｆｍ−１とすることでΔθｇｍ−１を求めることができる。そして、第ｍ−１の画像と、次の画像である第ｍの画像の間で、求めたΔθｇｍ−１を目標としてロボット２００の制御を行えばよい。そして、第ｍの画像が取得されたら、当該第ｍの画像から画像特徴量ｆｇを求め、上式（６）により新たな目標であるΔθｇｍを算出する。算出したΔθｇｍは、第ｍの画像と第ｍ＋１の画像の間で制御に用いられる。以下、この処理を収束するまで（画像特徴量がｆｇに充分近づくまで）継続すればよい。

なお、目標となる関節角の変化量を求めたからといって、必ずしも関節角を目標分だけ変化させる必要はない。例えば、第ｍの画像と第ｍ＋１の画像の間では、Δθｇｍを目標値として制御を行うが、実際の変化量がΔθｇｍとならないうちに、次の画像である第ｍ＋１の画像が取得され、それにより新たな目標値Δθｇｍ＋１が算出されるという場合も大いに考えられる。

４．異常検出手法
本実施形態における異常検出手法について説明する。図１１（Ａ）に示したように、ロボット２００の関節角がθｐであるときに、第ｐの画像情報が取得され、当該第ｐの画像情報から画像特徴量ｆｐが算出されたとする。そして、第ｐの画像情報の取得タイミングよりも後のタイミングにおいて、ロボット２００の関節角がθｑであるときに、第ｑの画像情報が取得され、当該第ｑの画像情報から画像特徴量ｆｑが算出されたとする。ここで、第ｐの画像情報と第ｑの画像情報は、時系列的に隣接する画像情報であってもよいし、隣接しない（第ｐの画像情報の取得後、第ｑの画像情報の取得前に他の画像情報を取得する）画像情報であってもよい。

ビジュアルサーボにおいては、ｆｐやｆｑは、上述したようにｆｇとの差分をΔｆとしてΔθｇの算出に用いられていたが、ｆｐとｆｑの差分ｆｑ−ｆｐも画像特徴量変化量に他ならない。また、関節角θｐや関節角θｑは関節角検出部１１５がエンコーダー等から取得するため、実測値として求めることが可能であり、θｐとθｑの差分θｑ−θｐは関節角の変化量Δθである。つまり、２つの画像情報について、それぞれ対応する画像特徴量ｆと関節角θが求められるため、画像特徴量変化量はΔｆ＝ｆｑ−ｆｐとして求められ、対応する関節角の変化量もΔθ＝θｑ−θｐとして求められる。

そして、上式（５）に示したように、Δｆ＝ＪｖΔθという関係がある。つまり実測したΔθ＝θｑ−θｐと、ヤコビアンＪｖを用いてΔｆｅ＝ＪｖΔθを求めれば、求めたΔｆｅは誤差が全く生じない理想的な環境においては実測されたΔｆ＝ｆｑ−ｆｐに一致するはずである。

よって、変化量推定部１３０は、関節角情報の変化量に対して、関節角情報と画像特徴量を対応付ける（具体的には関節角情報の変化量と画像特徴量変化量を対応付ける）ヤコビアンＪｖを作用させることで、推定画像特徴量変化量Δｆｅを演算する。上述したように、理想的な環境であれば求められた推定画像特徴量変化量Δｆｅは、変化量演算部１２０においてΔｆ＝ｆｑ−ｆｐとして求められた画像特徴量変化量Δｆに一致するはずであり、逆に言えばΔｆとΔｆｅが大きく異なる場合には、何らかの異常が発生していると判定することができる。

ここで、ΔｆとΔｆｅに誤差が生じる要因としては、画像情報から画像特徴量を演算する際の誤差、エンコーダーが関節角の値を読み取る際の誤差、ヤコビアンＪｖに含まれる誤差等が考えられる。ただし、エンコーダーが関節角の値を読み取る際に誤差が生じる可能性は他の２つと比較した場合低い。また、ヤコビアンＪｖに含まれる誤差についてもそれほど大きな誤差とはならない。それに対して、画像中に認識対象ではない多数の物体が撮像されたりすることで、画像情報から画像特徴量を演算する際の誤差の発生頻度は比較的高いものとなる。また、画像特徴量演算において異常が発生した場合には、誤差が非常に大きくなる可能性がある。例えば、画像中から所望の物体を認識する認識処理に失敗すれば、本来の物体位置とは大きく異なる画像上の位置に、物体があると誤認識する可能性がある。よって本実施形態では、主として画像特徴量の演算における異常を検出するものとする。ただし、他の要因による誤差も異常として検出してもよい。

異常判定においては例えば閾値を用いた判定処理を行えばよい。具体的には、異常判定部１４０は、画像特徴量変化量Δｆと推定画像特徴量変化量Δｆｅの差分情報と、閾値との比較処理を行い、差分情報が閾値よりも大きい場合に異常と判定する。例えば所与の閾値Ｔｈを設定し、下式（７）が満たされる場合に、異常が発生していると判定すればよい。このようにすれば、下式（７）等の容易な演算により異常を検出することが可能になる。
｜Δｆ−Δｆｅ｜＞Ｔｈ・・・・・（７）

また、閾値Ｔｈは固定値である必要はなく、状況に応じて値を変化させてもよい。例えば、異常判定部１４０は、変化量演算部１２０での、画像特徴量変化量の演算に用いられる２つの画像情報の取得タイミングの差が大きいほど、閾値を大きく設定してもよい。

ヤコビアンＪｖは図９等に示したように、ΔθとΔｆを関連づけるものである。そして図１２に示したように、同じヤコビアンＪｖを作用させた場合でも、Δθに作用させて得られるΔｆｅに比べて、Δθよりも変化量の大きいΔθ’に作用させて得られるΔｆｅ’の方が変化量が大きくなる。この際、ヤコビアンＪｖに全く誤差が生じないということは考えにくく、ΔθやΔθ’だけ関節角が変化した場合の画像特徴量の理想的な変化量ΔｆｉやΔｆｉ’に比べた場合、図１２に示したようにΔｆｅやΔｆｅ’はずれが生じる。そして図１２のＡ１とＡ２の比較から明らかなように、当該ずれは変化量が大きくなるほど大きくなってしまう。

仮に、画像特徴量演算において誤差が全くないと仮定すれば、画像情報から求められる画像特徴量変化量ΔｆはΔｆｉやΔｆｉ’に等しくなる。その場合、上式（７）の左辺はヤコビアンによる誤差を表すことになり、ΔθやΔｆｅ等のように変化量が小さい場合にはＡ１に相当する値となり、Δθ’やΔｆｅ’等のように変化量が大きい場合にはＡ２に相当する値となる。しかし上述したように、Δｆｅ、Δｆｅ’の両方で用いているヤコビアンＪｖは同一であり、上式（７）の左辺の値が大きくなったからといって、Ａ２の方がＡ１に比べてより異常度が高い状態であると判定することは不適切である。つまり、Ａ１に対応する状況では上式（７）を満たさず（異常と判定されず）、Ａ２に対応する状況では上式（７）を満たす（異常と判定される）ことは適切とは言えない。そこで異常判定部１４０では、よりΔθやΔｆｅ等の変化量が大きいほど、閾値Ｔｈも大きく設定する。このようにすれば、Ａ１に対応する状況に比べ、Ａ２に対応する状況での閾値Ｔｈが大きくなるため、適切な異常判定が可能である。２つの画像情報（図１１（Ａ）であれば第ｐの画像情報と第ｑの画像情報）の取得タイミングの差が大きいほど、ΔθやΔｆｅ等も大きくなると考えられるため、処理上は例えば、画像取得タイミングの差に応じて閾値Ｔｈを設定すればよい。

また、異常判定部１４０において異常が検出された場合の制御は種々考えられる。例えば、異常判定部１４０により異常が検出された場合に、ロボット制御部１１０は、ロボット２００を停止させる制御を行ってもよい。上述したように、異常が検出された場合とは、例えば画像情報からの画像特徴量の演算に大きな誤差が生じている場合等である。つまり、当該画像特徴量（図１１（Ａ）の例であればｆｑ）を用いてロボット２００の制御を行ってしまうと、画像特徴量が目標画像特徴量ｆｇに近づく方向とはかけ離れた方向へロボット２００を移動させてしまう可能性がある。場合によっては、アーム２１０等が他の物体に衝突したり、無理な姿勢を取ることでハンド等に把持していた対象物を落下させてしまうおそれもある。よって異常時の制御の一例としては、ロボット２００の動作自体を停止させてしまい、そのようなリスクの大きい動作を行わないというものが考えられる。

また、画像特徴量ｆｑに誤差が大きいと推定され、ｆｑを用いた制御が望ましくないのであれば、即座にロボット動作を停止してしまうのではなく、ｆｑを制御に用いないものとしてもよい。よって例えば、異常判定部１４０により異常が検出された場合に、ロボット制御部１１０は、変化量演算部１２０での、画像特徴量変化量の演算に用いられる２つの画像情報のうち、時系列的に後のタイミングで取得された画像情報である異常判定画像情報に基づく制御をスキップし、異常判定画像情報よりも前のタイミングで取得された画像情報に基づいて制御を行ってもよい。

図１１（Ａ）の例であれば、異常判定画像情報とは第ｑの画像である。また、図１０の例で、隣接する２つの画像情報を用いて異常判定を行い、第ｍ−２の画像情報と第ｍ−１の画像情報では異常なし、第ｍ−１の画像情報と第ｍの画像情報では異常なし、第ｍの画像情報と第ｍ＋１の画像情報では異常ありと判定されたとする。その場合、ｆｍ−１及びｆｍには異常がないことがわかり、ｆｍ＋１に異常があると考えられ、Δθｇｍ−１、Δθｇｍは制御に用いることができるが、Δθｇｍ＋１は制御に用いることが適切でない。本来、第ｍ＋１の画像情報とその次の第ｍ＋２の画像情報の間ではΔθｇｍ＋１を制御に用いるが、ここでは当該制御は適切でないため行わない。その場合、第ｍ＋１の画像情報と第ｍ＋２の画像情報の間でも、その前に求められたΔθｇｍを用いてロボット２００を動作させればよい。Δθｇｍは、少なくともｆｍの算出タイミングにおいては目標方向へロボット２００を移動させる情報であるから、ｆｍ＋１の算出後に継続利用したとしても大きな誤差を生じさせるとは考えにくい。このように、異常が検出された場合にも、それ以前の情報、特に異常検出タイミングよりも前に取得され、且つ異常が検出されていない情報で一応の制御を行うことでロボット２００の動作を継続できる。その後、新たな画像情報（図１０の例であれば第ｍ＋２の画像情報）が取得されたら、当該新たな画像情報から求められる新たな画像特徴量により制御を行えばよい。

異常検出時まで考慮した本実施形態の処理の流れを、図１６のフローチャートに示す。この処理が開始されると、まず画像取得部１１６による画像の取得と、画像特徴量演算部１１７による画像特徴量の演算を行い、変化量演算部１２０において画像特徴量変化量を演算する（Ｓ１０１）。また、関節角検出部１１５による関節角の検出を行い、変化量推定部１３０において推定画像特徴量変化量を推定する（Ｓ１０２）。そして画像特徴量変化量と推定画像特徴量変化量の差分が閾値以下であるか否かにより異常判定を行う（Ｓ１０３）。

差分が閾値以下である（Ｓ１０３でＹｅｓ）の場合には、異常は発生していないとして、Ｓ１０１で求められた画像特徴量を用いて制御を行う（Ｓ１０４）。そして、目標となる画像特徴量と、現在の画像特徴量が充分近づいたか（狭義には一致したか）の判定を行い、Ｙｅｓの場合には正常に目標に到達したとして処理を終了する。一方、Ｓ１０５でＮｏの場合には、動作自体に異常は発生していないが、目標には到達していないとして、Ｓ１０１に戻り制御を継続する。

また、画像特徴量変化量と推定画像特徴量変化量の差分が閾値より大きい（Ｓ１０３でＮｏ）の場合には、異常が発生していると判定する。そして、異常の発生がＮ回連続しているかの判定を行い（Ｓ１０６）、連続発生している場合には、動作の継続が好ましくない程度の異常であるとして、動作を停止する。一方、異常の発生がＮ回連続でない場合には、過去の画像特徴量であり、且つ異常が発生していないと判定された画像特徴量を用いて制御を行い（Ｓ１０７）、Ｓ１０１に戻って次のタイミングの画像について処理を継続する。図１６のフローチャートでは、上述したようにある程度の異常まで（ここでは連続Ｎ−１回以下の異常発生）は、即座に動作を停止するのではなく、動作を継続する方向での制御を行っている。

なお、以上の説明においては、画像情報の取得タイミング、関節角情報の取得タイミング、画像特徴量の取得タイミング（演算完了タイミング）の間の時間差を特に考慮していなかった。しかし実際には、図１３に示したように、所与のタイミングで画像情報が取得されたとしても、当該画像情報の取得時の関節角情報をエンコーダーが読み取り、且つ読み取った情報が関節角検出部１１５に送信されるまでにタイムラグが生じる。また、画像特徴量の演算が画像取得後に行われるため、そこでもタイムラグが生じるし、画像情報によって画像特徴量の演算負荷が異なるためタイムラグの長さも異なる。例えば、認識対象物以外の物体が全く撮影されず、且つ背景が単色無地等の場合は画像特徴量の演算は高速でできるが、種々の物体が撮影された場合等には画像特徴量の演算に時間を要する。

つまり、図１１（Ａ）では第ｐの画像情報と第ｑの画像情報を用いた異常判定を行うとシンプルに説明したが、実際には図１３に示したように、第ｐの画像情報の取得から対応する関節角情報の取得までのタイムラグｔ_θｐと、第ｐの画像情報の取得から画像特徴量の演算完了までのタイムラグｔ_ｆｐを考慮する必要があり、第ｑの画像情報についても同様にｔ_θｑと、ｔ_ｆｑを考慮する必要がある。

異常判定処理は、例えば第ｑの画像情報の画像特徴量ｆｑが取得されたタイミングで開始されるが、差分を取る対象であるｆｐはどれだけ前に取得された画像特徴量であるか、また、θｑとθｐの取得タイミングはいつであるかということを適切に判定しなくてはならない。

具体的には、第１の画像情報の画像特徴量ｆ１が第ｉ（ｉは自然数）のタイミングで取得され、第２の画像情報の前記画像特徴量ｆ２が第ｊ（ｊはｊ≠ｉを満たす自然数）のタイミングで取得された場合に、変化量演算部１２０は、画像特徴量ｆ１と画像特徴量ｆ２の差分を、画像特徴量変化量として求め、変化量推定部１３０は、第１の画像情報に対応する変化量推定用情報ｐ１が第ｋ（ｋは自然数）のタイミングで取得され、第２の画像情報に対応する変化量推定用情報ｐ２が第ｌ（ｌは自然数）のタイミングで取得された場合に、変化量推定用情報ｐ１と変化量推定用情報ｐ２とに基づいて、推定画像特徴量変化量を求める。

図１３の例であれば、画像特徴量や関節角情報は種々のタイミングで取得されているものであるが、ｆｑの取得タイミング（例えば第ｊのタイミング）を基準とした場合に、第ｐの画像情報に対応する画像特徴量ｆｐは、（ｔ_ｆｑ＋ｔｉ−ｔ_ｆｐ）だけ前に取得された画像特徴量である、すなわち、第ｉのタイミングは第ｊのタイミングより（ｔ_ｆｑ＋ｔｉ−ｔ_ｆｐ）だけ前であるということを特定する。ここで、ｔｉは図１３に示したように画像取得タイミングの差を表す。

同様に、θｑの取得タイミングである第ｌのタイミングは、第ｊのタイミングより（ｔ_ｆｑ−ｔ_θｑ）だけ前のタイミングであり、θｐの取得タイミングである第ｋのタイミングは、第ｊのタイミングより（ｔ_ｆｑ＋ｔｉ−ｔ_θｐ）だけ前のタイミングであると特定する。本実施形態の手法では、ΔｆとΔθの対応がとれている必要があり、具体的にはΔｆが第ｐの画像情報と第ｑの画像情報から求められているのであれば、Δθも第ｐの画像情報と第ｑの画像情報に対応する必要がある。そうでなければ、Δθから求められる推定画像特徴量変化量ΔｆｅはそもそもΔｆと対応関係を持たなくなり、比較処理を行う意義がないためである。よって上述したように、タイミングの対応関係を特定することは重要と言える。なお、図１３においては、関節角の駆動自体は非常に高速且つ高頻度で行われるため、連続的なものとして扱っている。

なお、現状のロボット２００及びロボット制御装置１００では、画像情報の取得タイミングと、対応する関節角情報の取得タイミングの差は充分小さいと考えることが可能である。よって、第ｋのタイミングは、第１の画像情報の取得タイミングであり、第ｌのタイミングは、第２の画像情報の取得タイミングであると考えてよい。この場合、図１３におけるｔ_θｐやｔ_θｑを０とすることができるため処理が容易になる。

また、さらに具体的な例としては、１つ前の画像情報から画像特徴量が算出されたタイミングに、次の画像情報の取得を行う手法が考えられる。この場合の例を図１４に示す。図１４の縦軸は画像特徴量の値であり、「実際の特徴量」とは、当該タイミングでの関節角情報に対応する画像特徴量が仮に取得された場合の値であり、処理上確認できるものではない。実際の特徴量が滑らかに推移することからわかるように、関節角の駆動は連続的であると考えてよい。

この場合、Ｂ１のタイミングで取得された画像情報に対応する画像特徴量は、ｔ２だけ経過後のＢ２のタイミングに取得されるため、Ｂ１での実際の特徴量と、Ｂ２での画像特徴量が対応する（誤差がなければ一致する）ことになる。そしてＢ２のタイミングで次の画像情報が取得される。

同様に、Ｂ２で取得された画像情報の画像特徴量はＢ３で演算が完了し、Ｂ３で次の画像情報が取得される。以下同様であり、Ｂ４で取得された画像情報の画像特徴量はｔ１だけ経過後のＢ５で演算が完了し、Ｂ５で次の画像情報が取得される。

図１４の例であれば、Ｂ５のタイミングで算出された画像特徴量と、Ｂ２のタイミングで算出された画像特徴量とを異常判定処理に用いる場合、画像情報の取得タイミングはそれぞれＢ４とＢ１になる。上述したように、画像情報の取得タイミングと対応する関節角情報の取得タイミングの差は充分小さい場合には、関節角情報はＢ４のタイミングのものと、Ｂ１のタイミングのものを用いればよい。つまり図１４に示したようにＢ２とＢ５の差をＴｓとし、タイミングの基準をＢ５とした場合には、比較対象である画像特徴量はＴｓだけ前のものを用いる。また、関節角情報の差分を求める際に用いる２つの関節角情報は、ｔ１だけ前のものと、Ｔｓ＋ｔ２だけ前のものを用いればよい。また、２つの画像情報の取得タイミングの差は（Ｔｓ＋ｔ２−ｔ１）となる。よって、画像情報の取得タイミングの差に基づいて閾値Ｔｈを決定する場合には、（Ｔｓ＋ｔ２−ｔ１）の値を用いればよいことになる。

その他、各種情報の取得タイミングは種々考えられるが、上述したようにΔｆとΔθの間に対応関係があるようにタイミングの特定を行えばよい点は同様である。

５．変形例
以上の説明では、ΔｆとΔθが取得され、Δθから推定画像特徴量変化量Δｆｅを求め、ΔｆとΔｆｅを比較していた。しかし本実施形態の手法はこれに限定されない。例えば上述した測定手法のように、何らかの手法でロボット２００の手先の位置姿勢情報、又は手先によって把持等が行われる対象物の位置姿勢情報を取得してもよい。

この場合、変化量推定用情報としては位置姿勢情報Ｘが取得されるため、その変化量ΔＸを求めることができる。そして上式（４）に示したように、ΔＸに対してヤコビアンＪｉを作用させることで、Δθの場合と同様に推定画像特徴量変化量Δｆｅを求めることができる。Δｆｅが求められれば、その後の処理は上述の例と同様である。つまり、変化量推定部１３０は、位置姿勢情報の変化量に対して、位置姿勢情報と画像特徴量を対応付ける（具体的には位置姿勢情報の変化量と画像特徴量変化量を対応付ける）ヤコビアンを作用させることで、推定画像特徴量変化量を演算する。この処理の流れを、図１１（Ａ）と対応させて示したものが図１１（Ｂ）である。

ここで、位置姿勢情報としてロボット２００の手先（ハンド或いはエンドエフェクター２２０）の位置姿勢を用いる場合には、Ｊｉは手先の位置姿勢情報の変化量と画像特徴量変化量を対応付ける情報である。また、位置姿勢情報として対象物の位置姿勢を用いる場合には、Ｊｉは対象物の位置姿勢情報の変化量と画像特徴量変化量を対応付ける情報である。或いは、エンドエフェクターにより対象物をどのような相対的な位置姿勢で把持するか、という情報が既知であれば、エンドエフェクター２２０の位置姿勢情報と、対象物の位置姿勢情報は１対１に対応するため、一方の情報を他方の情報に変換することも可能である。つまり、エンドエフェクター２２０の位置姿勢情報を取得してから、対象物の位置姿勢情報に変換し、その後、対象物の位置姿勢情報の変化量と画像特徴量変化量を対応付けるヤコビアンＪｉを用いてΔｆｅを求める等、種々の実施形態が考えられる。

また、本実施形態の異常判定における比較処理は、画像特徴量変化量Δｆと推定画像特徴量変化量Δｆｅを用いて行うものに限定されない。画像特徴量変化量Δｆと、位置姿勢情報の変化量ΔＸと、関節角情報の変化量Δθとは、ヤコビアンや、ヤコビアンの逆行列（広義には一般化逆行列）である逆ヤコビアンを用いることで相互に変換が可能である。

つまり本実施形態の手法は、図１７に示すように、画像情報に基づいてロボット２００を制御するロボット制御部１１０と、ロボット２００のエンドエフェクター２２０又は対象物の位置姿勢情報の変化量を表す位置姿勢変化量、又は、ロボット２００の関節角情報の変化量を表す関節角変化量を求める変化量演算部１２０と、画像情報に基づいて、画像特徴量変化量を求め、画像特徴量変化量に基づいて、位置姿勢変化量の推定量である推定位置姿勢変化量、又は、関節角変化量の推定量である推定関節角変化量を求める変化量推定部１３０と、位置姿勢変化量と推定位置姿勢変化量の比較処理、又は、関節角変化量と推定関節角変化量の比較処理により異常判定を行う異常判定部１４０とを含むロボット制御装置に適用できる。

図１７では、図２と比較した場合に、変化量演算部１２０と変化量推定部１３０が入れ替わった構成となっている。つまり変化量演算部１２０は、関節角情報に基づいて変化量（ここでは関節角変化量又は位置姿勢変化量）を求め、変化量推定部１３０は、画像特徴量の差分に基づいて変化量を推定する（推定関節角変化量又は推定位置姿勢変化量を求める）。また図１７では変化量演算部１２０は関節角情報を取得するものとしたが、上述したように変化量演算部１２０において、測定結果等を用いて位置姿勢情報を取得してもよい。

具体的には、ΔｆとΔθが取得された場合に、上式（５）から求められる下式（８）により推定関節角変化量Δθｅを求め、ΔθとΔθｅの比較処理を行ってもよい。具体的には、所与の閾値Ｔｈ２を用いて、下式（９）が成り立つ場合に異常であると判定すればよい。
Δθｅ＝Ｊｖ^−１Δｆ・・・・・（８）
｜Δθ−Δθｅ｜＞Ｔｈ２・・・・・（９）

或いは、上述したような測定手法を用いて、ΔｆとΔＸが取得された場合に、上式（４）から求められる下式（１０）により推定位置姿勢変化量ΔＸｅを求め、ΔＸとΔＸｅの比較処理を行ってもよい。具体的には、所与の閾値Ｔｈ３を用いて、下式（１１）が成り立つ場合に異常であると判定すればよい。
ΔＸｅ＝Ｊｉ^−１Δｆ・・・・・（１０）
｜ΔＸ−ΔＸｅ｜＞Ｔｈ３・・・・・（１１）

また、直接的に求められる情報により比較を行うものにも限定されない。例えば、ΔｆとΔθが取得された場合に、Δｆから上式（１０）により推定位置姿勢変化量ΔＸｅを求め、Δθから上式（３）により位置姿勢変化量ΔＸ（厳密にはこのΔＸも実測値ではなく推定値）を求め、上式（１１）を用いた判定を行ってもよい。

或いは、ΔｆとΔＸが取得された場合に、Δｆから上式（８）により推定関節角変化量Δθｅを求め、ΔＸから上式（３）から求められる下式（１２）により関節角変化量Δθ（厳密にはこのΔθも実測値ではなく推定値）を求め、上式（９）を用いた判定を行ってもよい。
Δθ＝Ｊａ^−１ΔＸ・・・・・（１２）

つまり、変化量演算部１２０は、複数の位置姿勢情報を取得して、複数の位置姿勢情報の差分を位置姿勢変化量として求める処理、複数の位置姿勢情報を取得して、複数の位置姿勢情報の差分に基づいて関節変化量を求める処理、複数の関節角情報を取得して、複数の関節角情報の差分を前記関節角変化量として求める処理、及び複数の関節角情報を取得して、複数の関節角情報の差分に基づいて位置姿勢変化量を求める処理、のいずれかの処理を行う。

以上に示したΔｆ、ΔＸ、Δθの関係を、本明細書における数式の番号を併記してまとめたものが図１５である。つまり本実施形態の手法は、Δｆと、ΔＸ及びΔθのいずれかの２つの情報が取得されれば、それらをΔｆ、ΔＸ、Δθのいずれかの情報に変換して比較することで実現することができ、取得する情報や比較に用いる情報については種々の変形実施が可能である。

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

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

１００ロボット制御装置、１１０ロボット制御部、１１１目標特徴量入力部、
１１２目標軌道生成部、１１３関節角制御部、１１４駆動部、
１１５関節角検出部、１１６画像情報取得部、１１７画像特徴量演算部、
１２０変化量演算部、１３０変化量推定部、１４０異常判定部、
２００ロボット、２１０アーム、２１１，２１３関節、
２１５，２１７フレーム、２２０エンドエフェクター、３００ロボット本体、
３５０ベースユニット部、４００ネットワーク、５００サーバー、
６００制御装置

Claims

画像情報に基づいてロボットを制御するロボット制御部と、
前記画像情報に基づいて、画像特徴量変化量を求める変化量演算部と、
前記ロボット又は対象物の情報であって、前記画像情報以外の情報である変化量推定用情報に基づいて、前記画像特徴量変化量の推定量である推定画像特徴量変化量を演算する変化量推定部と、
前記画像特徴量変化量と前記推定画像特徴量変化量の比較処理により異常判定を行う異常判定部と、
を含むことを特徴とするロボット制御装置。
請求項１において、
前記変化量推定用情報は、前記ロボットの関節角情報であることを特徴とするロボット制御装置。
請求項２において、
前記変化量推定部は、
前記関節角情報の変化量に対して、前記関節角情報の変化量と前記画像特徴量変化量を対応付けるヤコビアンを作用させることで、前記推定画像特徴量変化量を演算することを特徴とするロボット制御装置。
請求項１において、
前記変化量推定用情報は、前記ロボットのエンドエフェクター又は前記対象物の位置姿勢情報であることを特徴とするロボット制御装置。
請求項４において、
前記変化量推定部は、
前記位置姿勢情報の変化量に対して、前記位置姿勢情報の変化量と前記画像特徴量変化量を対応付けるヤコビアンを作用させることで、前記推定画像特徴量変化量を演算することを特徴とするロボット制御装置。
請求項１乃至５のいずれかにおいて、
第１の画像情報の画像特徴量ｆ１が第ｉ（ｉは自然数）のタイミングで取得され、第２の画像情報の画像特徴量ｆ２が第ｊ（ｊはｊ≠ｉを満たす自然数）のタイミングで取得された場合に、
前記変化量演算部は、
前記画像特徴量ｆ１と前記画像特徴量ｆ２の差分を、前記画像特徴量変化量として求め、
前記変化量推定部は、
前記第１の画像情報に対応する前記変化量推定用情報ｐ１が第ｋ（ｋは自然数）のタイミングで取得され、前記第２の画像情報に対応する前記変化量推定用情報ｐ２が第ｌ（ｌは自然数）のタイミングで取得された場合に、前記変化量推定用情報ｐ１と前記変化量推定用情報ｐ２とに基づいて、前記推定画像特徴量変化量を求めることを特徴とするロボット制御装置。
請求項６において、
前記第ｋのタイミングは、前記第１の画像情報の取得タイミングであり、
前記第ｌのタイミングは、前記第２の画像情報の取得タイミングであることを特徴とするロボット制御装置。
請求項１乃至７のいずれかにおいて、
前記異常判定部は、
前記画像特徴量変化量と前記推定画像特徴量変化量の差分情報と、閾値との比較処理を行い、前記差分情報が前記閾値よりも大きい場合に異常と判定することを特徴とするロボット制御装置。
請求項８において、
前記異常判定部は、
前記変化量演算部での、前記画像特徴量変化量の演算に用いられる２つの前記画像情報の取得タイミングの差が大きいほど、前記閾値を大きく設定することを特徴とするロボット制御装置。
請求項１乃至９のいずれかにおいて、
前記異常判定部により異常が検出された場合に、
前記ロボット制御部は、
前記ロボットを停止させる制御を行うことを特徴とするロボット制御装置。
請求項１乃至９のいずれかにおいて、
前記異常判定部により異常が検出された場合に、
前記ロボット制御部は、
前記変化量演算部での、前記画像特徴量変化量の演算に用いられる２つの前記画像情報のうち、時系列的に後のタイミングで取得された前記画像情報である異常判定画像情報に基づく制御をスキップし、前記異常判定画像情報よりも前のタイミングで取得された前記画像情報に基づいて制御を行うことを特徴とするロボット制御装置。
画像情報に基づいてロボットを制御するロボット制御部と、
前記ロボットのエンドエフェクター又は対象物の位置姿勢情報の変化量を表す位置姿勢変化量、又は、前記ロボットの関節角情報の変化量を表す関節角変化量を求める変化量演算部と、
前記画像情報に基づいて、画像特徴量変化量を求め、前記画像特徴量変化量に基づいて、前記位置姿勢変化量の推定量である推定位置姿勢変化量、又は、前記関節角変化量の推定量である推定関節角変化量を求める変化量推定部と、
前記位置姿勢変化量と前記推定位置姿勢変化量の比較処理、又は、前記関節角変化量と前記推定関節角変化量の比較処理により異常判定を行う異常判定部と、
を含むことを特徴とするロボット制御装置。
請求項１２において、
前記変化量演算部は、
複数の前記位置姿勢情報を取得して、複数の前記位置姿勢情報の差分を前記位置姿勢変化量として求める処理、
複数の前記位置姿勢情報を取得して、複数の前記位置姿勢情報の差分に基づいて前記関節角変化量を求める処理、
複数の前記関節角情報を取得して、複数の前記関節角情報の差分を前記関節角変化量として求める処理、及び
複数の前記関節角情報を取得して、複数の前記関節角情報の差分に基づいて前記位置姿勢変化量を求める処理、のいずれかの処理を行うことを特徴とするロボット制御装置。
画像情報に基づいてロボットを制御するロボット制御部と、
前記画像情報に基づいて、画像特徴量変化量を求める変化量演算部と、
前記ロボット又は対象物の情報であって、前記画像情報以外の情報である変化量推定用情報に基づいて、前記画像特徴量変化量の推定量である推定画像特徴量変化量を演算する変化量推定部と、
前記画像特徴量変化量と前記推定画像特徴量変化量の比較処理により異常判定を行う異常判定部と、
を含むことを特徴とするロボット。
画像情報に基づいてロボットを制御するロボット制御方法であって、
前記画像情報に基づいて、画像特徴量変化量を求める変化量演算処理を行うことと、
前記ロボット又は対象物の情報であって、前記画像情報以外の情報である変化量推定用情報に基づいて、前記画像特徴量変化量の推定量である推定画像特徴量変化量を演算する変化量推定処理を行うことと、
前記画像特徴量変化量と前記推定画像特徴量変化量の比較処理により異常判定を行うことと、
を含むことを特徴とするロボット制御方法。