JP6307431B2 - ロボット制御装置、ロボット制御方法、プログラム、記録媒体、ロボットシステム - Google Patents

Description

この発明は、変位自在な関節を有して当該関節の変位に応じてエンドエフェクタを移動させるロボットに関し、特にエンドエフェクタを撮像した結果に基づき関節の変位を制御して、エンドエフェクタを目標位置に移動させる技術に関する。

特許文献１には、エンドエフェクタをカメラで撮像した結果に基づきロボットの関節の回転を制御して、エンドエフェクタを目標位置に移動させるといったビジュアルフィードバックを用いた制御が提案されている。このようなビジュアルフィードバックによる制御では、エンドエフェクタの位置情報が、カメラの撮像結果から求められる。こうして取得されたエンドエフェクタの位置情報は、関節の回転を制御する回転制御量に変換される。そして、この回転制御量に基づき関節の回転を制御することで、エンドエフェクタを目標位置に移動させることができる。

さらに、非特許文献１では、エンドエフェクタの位置情報を回転制御量としての関節のトルクに変換する項を具備した制御則が提案されている。この制御則では、エンドエフェクタの実際の位置Ｘと目標位置Ｘdとの位置偏差ｅ（位置情報）に転置ヤコビ行列を作用させて、エンドエフェクタの位置情報を関節のトルクに変換する、運動学的な演算項が設けられている。この演算項により、エンドエフェクタの位置偏差ｅ（＝Ｘd−Ｘ）に基づき比例制御を行なうトルク（位置偏差に基づく比例トルク）が関節に働いて、位置偏差ｅを減少させる方向に関節を回転させ、エンドエフェクタを目標位置Ｘdまで移動させる。また、ビジュアルフィードバックによるロボット制御へこの制御則を適用するには、カメラの撮像結果から求めた位置偏差ｅを、制御則の演算項に当てはめれば良い。

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

Cheah, C. C., M. Hirano, Kawamura, S. and Arimoto, S. (2003)."Approximate Jacobian Control for Robots with Uncertain Kinematics and Dynamics." IEEE Transactions on Robotics and Automation, 19(4), 692-702.

ただし、この制御則では、例えばエンドエフェクタがカメラとの間の障害物に隠れる等の理由によって、エンドエフェクタの位置偏差が正確に得られなくなると、エンドエフェクタを目標位置に近づけられない場合があった。なぜなら、エンドエフェクタの正確な位置偏差が求まらないと、上記の演算項により発生するトルクが、エンドエフェクタの位置偏差を減少させるように作用するとは限らないからである。

この問題に対応するために、関節の目標角度と実際の回転角度との角度偏差に基づき比例制御を行なうトルク（角度偏差に基づく比例トルク）を発生させる項を制御則に加えることが考えられる。ここで、関節の目標角度は、目標位置にエンドエフェクタがあるときのロボットの関節の回転角度に相当する。つまり、かかる項を加えることで、角度偏差を減少させる方向に回転させる比例トルクを関節に与えて、エンドエフェクタを目標位置に近づけられる。しかも、回転角度は、関節に取り付けられた内界センサにより検出できる。したがって、エンドエフェクタの位置偏差が得られなくても、内界センサの検出結果から関節の角度偏差は取得できるため、角度偏差に基づく比例トルクを関節に与えて、エンドエフェクタを目標位置に近づけられる。そして、エンドエフェクタの位置偏差が再び得られるようになれば、この位置偏差に基づく比例トルクによって、エンドエフェクタを目標位置にまで適切に移動できると期待できる。

しかしながら、エンドエフェクタの位置偏差に基づく比例トルクと、関節の角度偏差に基づく比例トルクとが、重畳して関節に働くことで、次のような問題が発生するおそれがあった。つまり、関節の目標角度は、カメラから取得した情報に運動学的演算を施すことで求められるため、関節の目標角度には誤差が生じうる。そのため、エンドエフェクタを目標位置へ近づける途中において、位置偏差に基づく比例トルクとして求められたトルクに対して、角度偏差に基づく誤差を含んだ比例トルクが逆向きに作用して、関節の回転を停止させてしまうおそれがあった。換言すれば、エンドエフェクタが目標位置に到達する前に、これらのトルクが互いに釣り合って、関節の回転が停止してしまい、エンドエフェクタが目標位置から外れた位置で落ち着いてしまうおそれがあった。

このような問題に対応するにあたっては、一つは、運動学的な演算を正確に実行できるように構成することが考えられる。そのためには、カメラのパラメータや、カメラとロボットの位置関係等を高精度にキャリブレーションする必要がある。しかしながら、このようなキャリブレーションを高精度に行うことは、時間的あるいは費用的に多大な負担をユーザに強いることとなる。

この発明は上記課題に鑑みなされたものであり、運動学的な演算に誤差があっても、エンドエフェクタを目標位置にまで移動させることを可能とし、キャリブレーションの負担を軽減できる技術の提供を目的とする。

この発明にかかるロボット制御装置は、上記目的を達成するために、変位自在な関節を有して関節の変位に応じてエンドエフェクタを移動させるロボットを制御するロボット制御装置において、ロボットに設けられた基準箇所と基準箇所の移動先を視野に捉えつつ基準箇所を撮像する撮像部と、関節の変位量を検出する変位量検出部と、撮像部の撮像結果に基づいて移動先との間の位置偏差を基準箇所について求める位置偏差取得部と、関節を変位させるために関節に印加する力あるいはトルクである印加量として、位置偏差に応じた値に積分演算を行った積分値を基準箇所について求めた結果から第１印加量を算出するとともに、基準箇所が対応する移動先に一致するときの関節の変位量である目標変位量と変位量検出部の検出変位量との変位量偏差に応じた値に比例演算を行って第２印加量を算出する印加量算出部と、第１印加量および第２印加量を関節に加えて、基準箇所をその移動先へ向けて移動させる駆動制御部とを備えている。

この発明にかかるロボット制御方法は、上記目的を達成するために、変位自在な関節を有して関節の変位に応じてエンドエフェクタを移動させるロボットを制御するロボット制御方法において、ロボットに設けられた基準箇所と基準箇所の移動先を視野に捉えつつ基準箇所を撮像する工程と、基準箇所を撮像した結果に基づいて移動先との間の位置偏差を基準箇所について求める工程と、関節の変位量を検出して検出変位量を求める工程と、関節を変位させるために関節に印加する力あるいはトルクである印加量として、位置偏差に応じた値に積分演算を行った積分値を基準箇所について求めた結果から第１印加量を算出するとともに、基準箇所が対応する移動先に一致するときの関節の変位量である目標変位量と検出変位量との変位量偏差に応じた値に比例演算を行って第２印加量を算出する工程と、第１印加量および第２印加量を関節に加えて、基準箇所をその移動先へ向けて移動させる工程とを備えている。

この発明にかかるプログラムは、上記目的を達成するために、変位自在な関節を有して関節の変位に応じてエンドエフェクタを移動させるロボットをコンピュータに制御させるロボット制御プログラムにおいて、ロボットに設けられた基準箇所と基準箇所の移動先を視野に捉えつつ基準箇所を撮像する工程と、基準箇所を撮像した結果に基づいて移動先との間の位置偏差を基準箇所について求める工程と、関節の変位量を検出して検出変位量を求める工程と、関節を変位させるために関節に印加する力あるいはトルクである印加量として、位置偏差に応じた値に積分演算を行った積分値を基準箇所について求めた結果から第１印加量を算出するとともに、基準箇所が対応する移動先に一致するときの関節の変位量である目標変位量と検出変位量との変位量偏差に応じた値に比例演算を行って第２印加量を算出する工程と、第１印加量および第２印加量を関節に加えて、基準箇所をその移動先へ向けて移動させる工程とを、コンピュータに実行させる。

この発明にかかる記録媒体は、上記目的を達成するために、上記プログラムをコンピュータにより読み出し可能に記録している。

この発明にかかるロボットシステムは、上記目的を達成するために、変位自在な関節を有して関節の変位に応じてエンドエフェクタを移動させるロボットと、ロボットに設けられた基準箇所と基準箇所の移動先を視野に捉えつつ基準箇所を撮像する撮像部と、関節の変位量を検出する変位量検出部と、撮像部の撮像結果に基づいて移動先との間の位置偏差を基準箇所について求める位置偏差取得部と、関節を変位させるために関節に印加する力あるいはトルクである印加量として、位置偏差に応じた値に積分演算を行った積分値を基準箇所について求めた結果から第１印加量を算出するとともに、基準箇所が対応する移動先に一致するときの関節の変位量である目標変位量と変位量検出部の検出変位量との変位量偏差に応じた値に比例演算を行って第２印加量を算出する印加量算出部と、第１印加量および第２印加量を関節に加えて、基準箇所をその移動先へ向けて移動させる駆動制御部とを備えている。

このように構成された発明（ロボット制御装置、ロボット制御方法、プログラム、記録媒体、ロボットシステム）は、ロボットに設けられた基準箇所をその移動先へ向けて移動させて、エンドエフェクタの位置を制御する。具体的には、基準箇所の移動先が視野に捉えられた状態で基準箇所が撮像されるとともに、関節の変位量が検出される。そして、これらの結果に基づいて、関節を変位させるために関節に印加する力あるいはトルクである印加量が算出される。つまり、基準箇所について求められる移動先との位置偏差に基づく第１印加量が撮像結果から算出されるとともに、目標変位量と関節の検出変位量との変位量偏差に基づく第２印加量が算出される。そして、これら第１および第２印加量が関節に重畳して加えられる。かかる構成では、撮像結果から基準箇所の位置偏差が得られなくなっても、関節の変位量は検出できるため、変位量偏差に基づく第２印加量により関節を変位させて基準箇所を移動先に近づけ、その結果、エンドエフェクタを目標位置へ向けて移動できる。

ただし、上述したのと同様に、位置偏差に基づく印加量と関節の変位量偏差に基づく印加量とが重畳して関節に働く構成では、運動学的な演算が確かでないと、位置偏差に基づく印加量が変位量偏差に基づく印加量に対して逆向きに作用して、関節の変位を停止させてしまうおそれがあった。この場合、位置偏差が残存した状態で、エンドエフェクタが停止してしまうこととなる。これと同様に、基準箇所と移動先との位置偏差に基づく第１印加量と変位量偏差に基づく第２印加量とを重畳して関節に加える構成では、運動学的な演算が確かでないために、位置偏差が残存した状態で関節が停止して、エンドエフェクタが目標位置から外れた位置で落ち着いてしまうおそれがあった。

これに対して、本発明では、第２印加量と重畳して関節に与えられる第１印加量は、基準箇所の位置偏差に応じた値に積分演算を行って求められたものである。したがって、基準箇所が移動先に到達する前に関節が停止あるいは停止しようとした場合には、位置偏差が残存するので、第１印加量は時間とともに増大して、停止あるいは停止しようとする関節を動かす。そして、この第１印加量は、最終的に位置偏差が無くなるまで関節を動かし続けて、基準箇所を移動先まで移動させる。その結果、エンドエフェクタは目標位置にまで移動することができる。したがって、本発明では、運動学的な演算に誤差があっても、第１印加量の機能によって、エンドエフェクタを目標位置に確実に移動させることができる。その結果、高精度のキャリブレーションを必要とせず、キャリブレーションの負担を軽減することが可能となっている。

この際、関節の回転に応じてエンドエフェクタを移動させるロボットを制御するロボット制御装置において、撮像部は、エンドエフェクタに設けられた基準箇所と基準箇所の移動先を捉えつつ基準箇所を撮像し、変位量検出部は、関節の回転角度を検出する角度検出部であり、印加量算出部は、位置偏差に応じた値に積分演算を行って第１トルクを第１印加量として算出するとともに、基準箇所が移動先にあるときの関節の回転角度である目標変位量としての目標角度と角度検出部の検出角度との角度偏差に応じた値に比例演算を行って第２トルクを第２印加量として算出するトルク算出部であり、駆動制御部は、第１トルクおよび第２トルクが関節に加わるように、関節を駆動する関節駆動機構を制御するように、ロボット制御装置を構成しても良い。

このように構成された発明では、エンドエフェクタに設けられた基準箇所の移動先（目標位置）を視野に含みつつ基準箇所が撮像されるとともに、関節の回転角度が検出される。そして、移動先とエンドエフェクタの基準箇所（換言すればエンドエフェクタの位置）との位置偏差に基づく第１トルクが撮像結果に基づき算出されるとともに、目標角度と関節の検出角度との角度偏差に基づく第２トルクが算出される。そして、これら第１および第２トルクが関節に重畳して加えられる。かかる構成では、エンドエフェクタの基準箇所の位置偏差が撮像結果から得られなくなっても、関節の回転角度は検出できるため、角度偏差に基づく第２トルクにより関節を回転させて、エンドエフェクタの基準箇所を移動先に近づけられる。ただし、上述と同様の理由により、これら第１および第２トルクが重畳して関節に与えられる構成では、運動学的な演算に誤差があると、エンドエフェクタの基準箇所が移動先に到達する前に関節の回転が停止して、エンドエフェクタが目標位置から外れた位置で落ち着いてしまうおそれがあった。

これに対して、本発明では、第２トルクと重畳して関節に与えられる第１トルクは、エンドエフェクタの基準箇所の位置偏差に応じた値に積分演算を行って求められたものである。したがって、運動学的な演算に誤差があるために、エンドエフェクタが目標位置に到達する前に関節の回転が停止あるいは停止しようとした場合には、第１トルクは時間とともに増大して、停止あるいは停止しようとする関節を回転させる。そして、この第１トルクは、最終的に位置偏差が無くなるまで、関節を回転させ続けて、エンドエフェクタを目標位置にまで移動させる。したがって、本発明では、運動学的な演算に誤差があっても、第１トルクの機能によって、エンドエフェクタを目標位置に確実に移動させることができる。その結果、高精度のキャリブレーションを必要とせず、キャリブレーションの負担を軽減することが可能となっている。

この際、トルク算出部が位置偏差に応じた値に比例演算を行って算出したトルクが関節にさらに加わるように、駆動制御部は関節駆動機構を制御するようにロボット制御装置を構成しても良い。

また、トルク算出部は、転置ヤコビ行列が乗ぜられた位置偏差に対して積分演算を行って第１トルクを算出するようにロボット制御装置を構成しても良い。つまり、転置ヤコビ行列を位置偏差に乗じる運動学的な演算から求められた値に積分演算を行って、第１トルクを算出することができる。なお、上述のとおり本発明によれば、運動学的な演算に誤差があっても、エンドエフェクタを目標位置に確実に移動させることができる。したがって、転置ヤコビ行列が不正確で、第１トルクを求める運動学的な演算に誤差があっても、エンドエフェクタを目標位置に確実に移動させることができる。そのため、転置ヤコビ行列を正確に求めるために、キャリブレーションを高精度に実行する必要は特に無く、キャリブレーションの負担が軽減できる。

あるいは、関節を連結して６以上の自由度を持ったアームの一端にエンドエフェクタを取り付けたロボットを制御するロボット制御装置において、撮像部は、関節のうちの最も一端側の先端関節よりもエンドエフェクタ側に対して設定された少なくとも２箇所を含む、ロボットに対して設定された３箇所以上の基準箇所それぞれの移動先を視野に捉えつつ、基準箇所を撮像し、位置偏差取得部は、位置偏差を各基準箇所について求め、印加量算出部は、各関節について第１印加量および第２印加量を算出し、駆動制御部は、第１印加量および第２印加量を関節に加えて、３箇所以上の基準箇所をそれぞれの移動先へ向けて移動させることで、三次元でのエンドエフェクタの位置および姿勢を制御するように、ロボット制御装置を構成しても良い。

このように構成された発明は、先端関節よりもエンドエフェクタ側に対して設定された少なくとも２箇所を含む、ロボットに対して設定された３箇所以上の基準箇所を、それぞれの移動先へ向けて移動させて、エンドエフェクタの位置を制御する。具体的には、各基準箇所の移動先が視野に捉えられた状態で基準箇所が撮像されるとともに、関節の変位量が検出される。そして、これらの結果に基づいて、関節を変位させるために関節に印加する力あるいはトルクである印加量が算出される。つまり、各基準箇所について求められる移動先との位置偏差に基づく第１印加量が撮像結果から算出されるとともに、目標変位量と関節の検出変位量との変位量偏差に基づく第２印加量が算出される。そして、これら第１および第２印加量が関節に重畳して加えられる。かかる構成では、撮像結果から基準箇所の位置偏差が得られなくなっても、関節の変位量は検出できるため、変位量偏差に基づく第２印加量により関節を変位させて基準箇所を移動先に近づけ、その結果、エンドエフェクタを目標位置へ向けて移動できる。

ただし、上述したのと同様に、位置偏差に基づく印加量と関節の変位量偏差に基づく印加量とが重畳して関節に働く構成では、運動学的な演算が確かでないと、位置偏差に基づく印加量が変位量偏差に基づく印加量に対して逆向きに作用して、関節の変位を停止させてしまうおそれがあった。この場合、位置偏差が残存した状態で、エンドエフェクタが停止してしまうこととなる。これと同様に、基準箇所と移動先との位置偏差に基づく第１印加量と変位量偏差に基づく第２印加量とを重畳して関節に加える構成では、運動学的な演算が確かでないために、位置偏差が残存した状態で関節が停止して、エンドエフェクタが目標位置から外れた位置で落ち着いてしまうおそれがあった。

これに対して、本発明では、第２印加量と重畳して関節に与えられる第１印加量は、基準箇所の位置偏差に応じた値に積分演算を行って求められたものである。したがって、基準箇所が移動先に到達する前に関節が停止あるいは停止しようとした場合には、位置偏差が残存するので、第１印加量は時間とともに増大して、停止あるいは停止しようとする関節を動かす。そして、この第１印加量は、最終的に位置偏差が無くなるまで関節を動かし続けて、基準箇所を移動先まで移動させる。その結果、エンドエフェクタは目標位置にまで移動することができる。したがって、本発明では、運動学的な演算に誤差があっても、第１印加量の機能によって、エンドエフェクタを目標位置に確実に移動させることができる。その結果、高精度のキャリブレーションを必要とせず、キャリブレーションの負担を軽減することが可能となっている。

特にこの発明は、エンドエフェクタに対して設定された少なくとも２箇所を含む、ロボットに対して設定された３箇所以上の基準箇所を、それぞれの移動先へ向けて移動させて、エンドエフェクタの位置を制御する。その結果、三次元においてエンドエフェクタを目標の位置および姿勢に適切に制御することが可能となっている。

ところで、３箇所以上の基準箇所をそれぞれの移動先へ移動させる構成では、基準箇所の設定位置と移動先との位置関係によっては、全ての基準箇所をそれぞれの移動先へ収束させることが困難となるといった問題が生じるおそれがある。特に全ての基準箇所を先端関節よりもエンドエフェクタ側に対して設定した際に、かかる問題が発生しやすい。

そこで、駆動制御部は、先端関節あるいは先端関節よりも一端側の逆側である他端側に対して特定基準箇所として設定された基準箇所を含む３箇所以上の基準箇所のそれぞれを、対応する移動先へ向けて移動させるように、ロボット制御装置を構成しても良い。このように一部の基準箇所（特定基準箇所）を先端関節あるいは先端関節よりも他端側に設けることで、全ての基準箇所をそれぞれの移動先へ収束させることが容易となる。

さらには、複数の関節のうち一端側から順番に関節の自由度を数えた総計が３以上となる特定関節あるいは当該特定関節より他端側に特定基準箇所が設定されているように、ロボット制御装置を構成しても良い。このような構成では、特定基準箇所については特定関節より他端側の自由度によって適切に移動先へ収束させることができる。また、特定基準箇所よりエンドエフェクタ側に対して設定された基準箇所については、特定関節を含めて特定関節より一端側の３以上の自由度によって適切に移動先へ収束させることができる。つまり、特定基準箇所とそれ以外の基準箇所とで自由度を使い分ける制御が実行可能となる。その結果、各基準箇所を移動先へ適切に収束させることが容易となる。

この際、複数の関節のうち他端側から順番に関節の自由度を数えた総計が３以上となる関節よりも一端側に特定基準箇所が設定されているように、ロボット制御装置を構成してもよい。これによって、特定基準箇所を３以上の自由度でもって移動先へより確実に収束させることができる。

また、印加量算出部は、特定基準箇所についての積分値の重み係数が先端関節よりもエンドエフェクタ側に対して設定された基準箇所についての積分値の重み係数よりも大きい加重加算を各基準箇所の積分値に行って、第１印加量を算出するように、ロボット制御装置を構成しても良い。このような構成では、より大きな重み係数に対応する特定基準箇所をより速く移動先へ収束させた後に、先端関節よりもエンドエフェクタ側に対して設定された基準箇所を移動先へ収束させるといった動作が実行できる。したがって、特定関節を含めて特定関節より一端側の自由度、換言すれば特定基準箇所よりもエンドエフェクタ側の自由度を、実質的にエンドエフェクタに対して設定された基準箇所の移動にのみ用いることができる。よって、エンドエフェクタに対して設定された基準箇所を、十分な自由度でもって移動先へ確実に収束させることができる。

この際、印加量算出部は、位置偏差に応じた値に比例演算を行った比例値を各基準箇所について求めた結果から第３印加量を算出し、駆動制御部は、第３印加量を関節にさらに加えるように、ロボット制御装置を構成しても良い。

また、印加量算出部は、転置ヤコビ行列が乗ぜられた位置偏差に対して積分演算を行って積分値を求めて、第１印加量を算出するように、ロボット制御装置を構成しても良い。つまり、転置ヤコビ行列を位置偏差に乗じる運動学的な演算から求められた値に積分演算を行って、第１印加量を算出することができる。なお、上述のとおり本発明によれば、運動学的な演算に誤差があっても、エンドエフェクタを目標位置に確実に移動させることができる。したがって、転置ヤコビ行列が不正確で、第１印加量を求める運動学的な演算に誤差があっても、エンドエフェクタを目標位置に確実に移動させることができる。そのため、転置ヤコビ行列を正確に求めるために、キャリブレーションを高精度に実行する必要は特に無く、キャリブレーションの負担が軽減できる。

運動学的な演算に誤差があっても、エンドエフェクタを目標位置にまで移動させることが可能となり、キャリブレーションの負担を軽減できる。

本発明の第１実施形態にかかるロボットシステムの一例を示す模式図である。 図１のロボットシステムでエンドエフェクタの位置制御を担う電気的構成を示すブロック線図である。 ロボットシステムで実行されるエンドエフェクタの位置制御の一例を示すブロック線図である。 本発明の第２実施形態にかかるロボットシステムの一例を示す模式図である。 基準点をそれぞれの移動先へ動かす過程においてエンドエフェクタへ働くトルクの状態の一例を模式的に示す図である。 実施例１での実験結果を示す図である。 実施例１での実験結果を示す図である。 実施例２での実験結果を示す図である。 実施例２での実験結果を示す図である。 実施例２での実験結果を示す図である。 実施例２での実験結果を示す図である。 実施例３での実験結果を示す図である。 実施例３での実験結果を示す図である。 実施例４でのゲインの設定値を表として示す図である。 実施例４でのシミュレーション結果を示す図である。 実施例５でのゲインの設定値を表として示す図である。 実施例５でのシミュレーション結果を示す図である。 実施例６でのゲインの設定値を表として示す図である。 実施例６でのシミュレーション結果を示す図である。 実施例７でのゲインの設定値を表として示す図である。 実施例７でのシミュレーション結果を示す図である。 実施例８でのゲインの設定値を表として示す図である。 実施例８でのシミュレーション結果を示す図である。

第１実施形態
図１は、本発明の第１実施形態にかかるロボットシステムの一例を示す模式図である。同図に示すように、ロボットシステム１は、ロボット２と２台のカメラＣ1、Ｃ2とで構成される。なお、図１では、ロボット２は関節Ｑ1〜Ｑ3、リンクＬ1〜Ｌ3およびエンドエフェクタ４の記号表記によって模式的に示されており、カメラＣ1、Ｃ2はそれぞれの像面ＩＭ1、ＩＭ2で代表して模式的に示されている。

ロボット２は、回転自在な関節Ｑ1〜Ｑ3を有して当該関節Ｑ1〜Ｑ3の回転に応じて動くアーム３の先端に、エンドエフェクタ４（手先）を取り付けた概略構成を具備する。具体的には、関節Ｑ1、Ｑ2の間にリンクＬ1が取り付けられ、関節Ｑ2、Ｑ3の間にリンクＬ2が取り付けられ、関節Ｑ3にリンクＬ3の一端が取り付けられて、アーム３が構成される。そして、エンドエフェクタ４はリンクＬ3の他端（アーム３の先端）に取り付けられる。関節Ｑ1は、鉛直軸を中心として回転して、それよりエンドエフェクタ４側の部材を鉛直軸中心に回転させる。関節Ｑ2は、回転することで、それよりエンドエフェクタ４側の部材とリンクＬ1との角度を変化させる。関節Ｑ3は、回転することで、それよりエンドエフェクタ４側の部材とリンクＬ2との角度を変化させる。

このような構成を具備するロボット２では、関節Ｑ1〜Ｑ3の回転角度ｑ1〜ｑ3を変化させることで、エンドエフェクタ４を移動させることができる。特に、このロボットシステム１は、エンドエフェクタ４（具体的には、例えばＴＣＰ(Tool Center Point)等の代表点）の位置ｐをカメラＣ1、Ｃ2で検出した結果に基づき、関節Ｑ1〜Ｑ3の回転角度ｑ1〜ｑ3を調整して、エンドエフェクタ４の位置ｐを制御する。この際、代表点に例えばＬＥＤ(Light Emitting Diode)等のマークを取り付けて、カメラＣ1、Ｃ2での視認性を向上させても良い。

つまり、カメラＣ1、Ｃ2は、エンドエフェクタ４の移動先である目標位置ｐdをそれぞれの視野に含むように位置決めされており、エンドエフェクタ４の位置ｐと目標位置ｐdとの位置偏差Δｐを互いに異なる平面で捉える。具体的には例えば、像面ＩＭ1、ＩＭ2が互いに直交するようにカメラＣ1、Ｃ2を配置して、作業座標系のＹＺ平面をカメラＣ1で撮像するとともに、作業座標系のＺＸ平面をカメラＣ2で撮像するように構成することができる。そして、カメラＣ1、Ｃ2の撮像結果から検出したエンドエフェクタ４の位置ｐと目標位置ｐdとの位置偏差Δｐが縮まるように、関節Ｑ1〜Ｑ3の回転角度ｑ1〜ｑ3が調整されて、エンドエフェクタ４の位置制御が実行される（ビジュアルフィードバック）。

この際、特異点にあるロボット２の撮像結果は、エンドエフェクタ４の位置制御にフィードバックしないように構成することができる。具体例を挙げれば、特異点にあるロボット２がカメラＣ1、Ｃ2の視野から外れるように、カメラＣ1、Ｃ2を配置しても良い。これによって、特異点にあるロボット２の撮像結果に基づいてビジュアルフィードバックが行われて、ロボット２の制御が不安定になることを抑制できる。

ここで、エンドエフェクタ４の位置制御に用いられる座標系や各制御量の表記について説明しておく。図１に示すように、エンドエフェクタ４が作業を行う作業空間に対しては、互いに直交するＸ軸、Ｙ軸、Ｚ軸で構成された、Ｚ軸を鉛直軸とする三次元の作業座標系が設けられる。よって、エンドエフェクタ４の位置ｐは、三次元ベクトル（ｐx、ｐy、ｐz）で与えられる。同様に、エンドエフェクタ４の目標位置ｐdも三次元ベクトル（ｐdx、ｐdy、ｐdz）で与えられる。

各カメラＣiに対しては、互いに直交するＵi軸、Ｖi軸、Ｗi軸で構成された三次元のカメラ座標系が設けられている。ここで、「i」はカメラを区別するための番号であり（i＝１、２）、Ｕi軸はi番目のカメラＣiの水平方向軸であり、Ｖi軸はi番目のカメラＣiの垂直方向軸（鉛直軸）であり、Ｗi軸はi番目のカメラＣiの奥行方向軸である。また、図１では、ＵiＶi平面内にあるカメラＣiの像面ＩＭiが示されるとともに、位置ｐ（基準点）を像面ＩＭiに投影した座標αi（換言すれば、カメラＣiの座標系における位置ｐの座標αi）と、目標位置ｐd（移動先）を像面ＩＭiに投影した座標βi（換言すれば、カメラＣiの座標系における目標位置ｐdの座標βi）とが示されている。これら座標αi、βiは、具体的には次のとおりである。

ロボット２の関節Ｑの回転角度ｑは、関節Ｑnの回転角度ｑnを各成分とするベクトル（ｑ1、ｑ2、ｑ3）として表される。ここで、関節Ｑとの表記は、関節Ｑ1〜Ｑ3を総称した表記であり、「ｎ」は関節を区別するための番号である（ｎ＝１、２、３）。また、目標位置ｐdにエンドエフェクタ４があるときの関節Ｑの回転角度ｑを、目標角度ｑd（＝ｑd1、ｑd2、ｑd3）とする。さらに、ロボット２の関節Ｑに与えられるトルクτは、関節Ｑnに働くトルクτnを各成分とするベクトル（τ1、τ2、τ3）として表される。

以上が座標系および制御量の表記に関する説明である。続いて、エンドエフェクタ４の位置制御の詳細について説明する。図２は、図１のロボットシステムでエンドエフェクタの位置制御を担う電気的構成を示すブロック線図である。図２に示すように、ロボットシステム１では、ＣＰＵ(Central Processing Unit)やメモリー等で構成された演算機能を司るコントローラ５が設けられている。そして、このコントローラ５が、記録媒体６に記録されたプログラム７に従って、エンドエフェクタ４の位置制御を実行する。なお、記録媒体６としては、ＣＤ(Compact Disc)、ＤＶＤ(Digital Versatile Disc)、ＵＳＢ(Universal Serial Bus)メモリー等の種々のメディアを用いることができる。さらに、ロボットシステム１では、関節Ｑnを駆動するモータＭnが、関節Ｑ1〜Ｑ3のそれぞれについて設けられるとともに、モータＭnの回転位置を検出するエンコーダＥnが、モータＭ1〜Ｍ3のそれぞれについて設けられている。

そして、コントローラ５は、モータＭ1〜Ｍ3のそれぞれを制御することで、ロボット２の関節Ｑの回転角度ｑを調整する。この際、上述のビジュアルフィードバックを実行するために、コントローラ５は、カメラＣi（外界センサ）によるエンドエフェクタ４の撮像結果から、エンドエフェクタ４の位置偏差Δｐ（＝ｐd−ｐ）を検出する。この際、コントローラ５は、目標位置ｐdがカメラＣiの座標系の原点（像面ＩＭiの中心）に一致あるいは近接するように、カメラＣiのパン・チルトを制御しつつ、エンドエフェクタ４の位置偏差Δｐの検出を行う。

また、エンドエフェクタ４の位置偏差Δｐの検出と並行して、コントローラ５は、エンコーダＥ1〜Ｅ3（内界センサ）の出力から、関節Ｑの角度偏差Δｑ（＝ｑd−ｑ）を検出する。そして、コントローラ５は、位置偏差Δｐ（＝ｐd−ｐ）および角度偏差Δｑ（＝ｑd−ｑ）に基づいて、トルクτを算出する。そして、モータＭ1〜Ｍ3がトルクτを関節Ｑに与えることで、関節Ｑの回転角度ｑが調整される。このように、この実施形態では、カメラＣiおよびエンコーダＥnの検出結果がトルクτにフィードバックされて、エンドエフェクタ４の位置が制御される。

図３は、ロボットシステムで実行されるエンドエフェクタの位置制御の一例を示すブロック線図である。図３および以下の説明において、記号「〜」は、同記号が付された量が誤差を含みうることを示し、記号「Ｔ」を右肩に有する行列は、記号「Ｔ」の付された行列の転置行列を表し、記号「−１」を右肩に有する行列は、記号「−１」の付された行列の逆行列を表し、記号「ｓ」はラプラス変換の変数（ラプラス変数）であり、記号「・」は微分を意味する。また、図３および以下の数式では、目標位置ｐdおよび目標角度ｑdに付された記号「d」等を下付きで適宜表記する。

トルクτがロボット２の関節Ｑに与えられると、ロボット２はロボットダイナミクス２０１に従って動き、ロボット２の関節Ｑは回転角度ｑを取る。ここで、ロボットダイナミクス２０１は、ロボット２の機構に働くトルクと当該トルクが生み出す加速との関係を規定するものである。その結果、エンドエフェクタ４は、ロボット運動学２０２に従って回転角度ｑに応じた位置ｐに移動する。また、上述のフィードバック制御を実行するために、コントローラ５は、位置偏差Δｐをトルクτにフィードバックする外界ループＬxと、角度偏差Δｑをトルクτにフィードバックする内界ループＬqとを有する。

外界ループＬxでは、エンドエフェクタ４の位置ｐが２台のカメラＣ1、Ｃ2によって検出される。換言すれば、座標変換２０３によって、作業座標系におけるエンドエフェクタ４の位置ｐがカメラＣ1、Ｃ2それぞれの座標系の座標α1、α2へ変換される。そして、カメラＣ1、Ｃ2は、それぞれの座標系で位置偏差Δｐを表した値（β1−α1）、（β2−α2）を出力する。具体的には、カメラＣiの座標系で位置偏差Δｐを表した値（βi−αi）は次のとおりである。

そして、カメラＣ1、Ｃ2それぞれの座標系で表された位置偏差（βi−αi）から、作業座標系での位置偏差Δｐに基づくトルクτが算出される。このトルクτの算出では、カメラ座標系および位置座標系それぞれの位置偏差の間に成立する関係を、用いることができる。

つまり、カメラＣiの座標系と作業座標系の間には、次の関係が成立する。

なお、カメラの内部パラメータＡを表現する行列は、具体的に次式で与えられる。

数３の関係が成立することから、カメラ座標系での位置偏差（βi−αi）と作業座標系での位置偏差（ｐd−ｐ）との間には、次の関係が成立する。

さらに、次の関係も成立する。

そして、数５および数６から、次式が得られる。

ここで、カメラＣiの座標系で表された位置偏差（βi−αi）と、作業座標系で表された位置偏差（ｐd−ｐ）とを関係付ける係数行列Γiを、次式で定義する。

なお、カメラのレンズ歪みＥを表現する行列は、具体的には次式を満たす。

そして、数７および数８より、２台のカメラＣ1、Ｃ2それぞれの座標系で表された位置偏差（βi−αi）と作業座標系で表された位置偏差（ｐd−ｐ）とを関係付ける次式が得られる。

なお、数１０では、目標位置ｐdまでの奥行距離の推定値が用いられる。この推定値は、適当な定数とすることができる。具体的には、作業座標系とカメラＣiとの座標系とが十分に離れている場合には、これら座標系の原点間の距離を当該推定値としても良い。あるいは、カメラＣ1と目標位置ｐdとの両方をカメラＣ2の視野内に捉えて、カメラＣ1の当該推定値をカメラＣ2の撮像結果から求めるとともに、カメラＣ2の当該推定値についても同様にしてカメラＣ1の撮像結果から求めるようにしても良い。

さらに、補正行列を次式で定義する。

この補正行列の逆行列は、数１０の右辺で位置偏差（ｐd−ｐ）にかかる係数を正規化する機能を持つ。したがって、補正行列の逆行列と当該係数との積は、単位行列に等しくなる。ただし、各パラメータに誤差があることから、この積は、厳密には位置ｐに応じた成分を有し、行列Φ(p)を用いて次のように表される。ここで、Φ(p)は、位置ｐに応じた成分を有する行列であり、単位行列に概ね等しい。

その結果、数１０および数１２の関係から、次の関係が得られる。

このように、２台のカメラＣ1、Ｃ2それぞれの座標系で表された位置偏差（β1−α1）（β2−α2）から、作業座標系で表された位置偏差（ｐd−ｐ）に応じた位置偏差検出量（＝Φ(p)（ｐd−ｐ））を取得することができる。そこで、外界ループＬxでは、図３に示すように、カメラＣ1、Ｃ2の座標系での位置偏差（β1−α1）（β2−α2）それぞれに演算２０４、２０５を施したものが足し合わされて、作業座標系における位置偏差検出量（＝Φ(p)（ｐd−ｐ））が算出される。

なお、後述するように、この実施形態では、高精度のキャリブレーションを要さずとも、エンドエフェクタ４を目標位置ｐdに正確に移動できるといった利点がある。そのため、演算２０４、２０５において、補正行列および係数行列は誤差を含んでいても構わない。これに対応して、図３のブロック線図では、これらに記号「〜」が付されている。

そして、コントローラ５では、転置ヤコビ行列（ヤコビ行列の転置行列）を位置偏差検出量に乗じる演算２０６が実行される。さらに、コントローラ５では、転置ヤコビ行列が乗ぜられた位置偏差検出量に対して、比例演算および積分演算が実行される。具体的には、転置ヤコビ行列が乗ぜられた位置偏差検出に、比例ゲインを乗じて比例演算が実行される（演算２０７）。また、転置ヤコビ行列が乗ぜられた位置偏差検出に、時間による積分演算が実行される（演算２０８）。さらに、積分演算値に対しては、積分ゲインが乗じられる（演算２０９）。そして、これら演算２０７、２０９の結果は、ロボットダイナミクス２０１の入力側（つまり、トルクτ）に帰還される。以上が、外界ループＬxの動作である。

内界ループＬqでは、関節Ｑの回転角度ｑを検出する角度検出２１０が実行される。なお、この角度検出により検出される回転角度ｑは誤差を含みうる。これに対応して、図３の角度検出２１０では、回転角度ｑの検出値が誤差関数ε(ｑ)の出力値として表されている。この誤差関数は例えば、ε（ｑ）＝ｃ1×ｑ＋ｃ2というように、回転角度ｑ（の真値）の１次関数で表すことができる。さらに、内界ループＬqでは、回転角度ｑの検出値ε(ｑ)に対して、微分演算および比例演算が実行される。具体的には、検出値ε(ｑ)に、時間による微分演算が実行される（演算２１１）。さらに、微分演算値に対しては、微分ゲインが乗じられる（演算２１２）。また、目標角度ｑdとの偏差Δｑに比例ゲインを乗じる比例演算２１３が実行される。これら演算２１２、２１３の結果は、ロボットダイナミクス２０１の入力側（つまり、トルクτ）に帰還される。以上が、内界ループＬqの動作である。

コントローラ５は、これら外界ループＬxおよび内界ループＬqの演算結果に重力補償項ｇ(ｑ)を加算して、トルクτを決定する。ここで、重力補償項ｇ(q)は、重力に抗してロボット２を静止させるために必要となるトルクに相当する。その結果、関節Ｑに加えられるトルクτは次式で与えられる。

数１４において、第１項は重力補償項であり、第２項は角度偏差Δｑに対して比例制御を行なう項であり、第３項は回転角度ｑに対して微分制御を行なう項であり、第４項は位置偏差Δｐに対して比例制御を行なう項であり、第５項は位置偏差Δｐに対して積分制御を行なう項である。なお、数１４では、積分区間が制御開始時刻ｔoからの時間となっているが、積分区間はこれに限られず、適宜変更することもできる。具体例を挙げると、エンドエフェクタ４がカメラＣ1、Ｃ2両方の視野の外から移動を開始するような場合、エンドエフェクタ４がカメラＣ1、Ｃ2両方の視野に入って、ビジュアルフィードバックが適切に機能し始める時刻を積分区間の始点としても構わない。

以上に説明したように、この実施形態では、エンドエフェクタ４の目標位置ｐdを視野に含みつつエンドエフェクタ４が撮像されるとともに、関節Ｑの回転角度ｑが検出される（第１工程）。そして、エンドエフェクタ４の位置偏差Δｐに基づくトルク（数１４の第５項のトルク）が撮像結果に基づき算出されるとともに、関節Ｑの角度偏差Δｑに基づくトルク（数１４の第２項のトルク）が算出される（第２工程）。そして、これらのトルクが関節Ｑに加わるように、モータＭ1〜Ｍ3が制御される（第３工程）これによって、これらのトルクが関節Ｑに重畳して加えられる。かかる構成では、撮像結果からエンドエフェクタ４の位置偏差Δｐが得られなくなっても、関節Ｑの回転角度ｑは検出できるため、角度偏差Δｑに基づくトルクにより関節Ｑを回転させて、エンドエフェクタ４を目標位置ｐdに近づけられる。ただし、角度偏差Δｑに基づくトルクと位置偏差Δｐに基づくトルクが重畳して関節Ｑに与えられる構成では、運動学的な演算に誤差があると、エンドエフェクタ４が目標位置ｐdに到達する前に関節Ｑの回転が停止して、エンドエフェクタ４が目標位置ｐdから外れた位置で落ち着いてしまうおそれがあった。

これに対して、この実施形態では、角度偏差Δｑに基づくトルクと重畳して関節Ｑに与えられる位置偏差Δｐに基づくトルクは、位置偏差Δｐに応じた値に積分演算を行って求められた位置偏差積分トルクである。したがって、運動学的な演算に誤差があるために、エンドエフェクタ４が目標位置ｐdに到達する前に関節Ｑの回転が停止あるいは停止しようとした場合には、位置偏差積分トルクは時間とともに増大して、停止あるいは停止しようとする関節Ｑを回転させる。そして、この位置偏差積分トルクは、最終的に位置偏差Δｐが無くなるまで、関節Ｑを回転させ続けて、エンドエフェクタ４を目標位置ｐdにまで移動させる。したがって、この実施形態では、運動学的な演算に誤差があっても、位置偏差積分トルクの機能によって、エンドエフェクタ４を目標位置ｐdに確実に移動させることができる。その結果、高精度のキャリブレーションを必要とせず、キャリブレーションの負担を軽減することが可能となっている。

特に、この実施形態では、転置ヤコビ行列が乗ぜられた位置偏差Δｐに対して積分演算を行うといった運動学的な演算から、位置偏差積分トルクが算出されている。これに対して、この実施形態によれば、運動学的な演算に誤差があっても、エンドエフェクタ４を目標位置ｐdに確実に移動させることができる。したがって、転置ヤコビ行列が不正確で、位置偏差積分トルクを求める運動学的な演算に誤差があっても、エンドエフェクタ４を目標位置ｐdに確実に移動させることができる。そのため、転置ヤコビ行列を正確に求めるために、キャリブレーションを高精度に実行する必要は特に無く、キャリブレーションの負担が軽減できる。

上述の技術内容は、次のように捉えることもできる。つまり、角度偏差Δｑに基づくトルクと位置偏差Δｐに基づくトルクとが重畳して関節Ｑに加えられる構成では、運動学的な演算に誤差があると、両トルクが拮抗して、関節Ｑの回転が停止するおそれがあった。これは、運動学的な演算に誤差があると、作業空間でのポテンシャル分布が目標位置ｐdとは別の位置に極小値を持ち、エンドエフェクタ４がこの極小値に陥るためと考えられる。これに対して、上記の位置偏差積分トルクを関節Ｑに加えた場合、時間とともに増大する位置偏差積分トルクの作用によって、エンドエフェクタ４を目標位置ｐdにまで移動させることができる。したがって、運動学的な演算の誤差に起因して目標位置ｐdとは別の位置に極小値が生じるような場合であっても、エンドエフェクタ４を目標位置ｐdに確実に移動させることができる。その結果、高精度のキャリブレーションを必要とせず、キャリブレーションの負担を軽減することが可能となっている。

ところで、この実施形態によれば、エンドエフェクタ４に位置偏差Δｐがあれば、位置偏差積分トルクが関節Ｑに働いて、エンドエフェクタ４を目標位置ｐdにまで移動させることができる。上記説明では、この位置偏差Δｐの原因として、運動学的な演算の誤差を挙げた。しかしながら、この実施形態は、エンドエフェクタ４の位置偏差Δｐの発生原因を問わず、エンドエフェクタ４に位置偏差Δｐがあればこれを解消するように、エンドエフェクタ４を目標位置ｐdにまで移動させる。つまり、重力補償のための項、目標角度ｑdあるいは検出した回転角度ｑ等の値が不正確であることを原因とする位置偏差Δｐも、位置偏差積分トルクの作用によって解消することができる。したがって、エンドエフェクタ４の位置偏差（Δｐ＝ｐd−ｐ）さえ求まれば、エンドエフェクタ４を目標位置ｐdに移動させることができる。その結果、精度の求められるパラメータが少ないため、キャリブレーションの負担は極めて軽く、場合によってはキャリブレーションを省略することも可能となる。

このように、第１実施形態では、ロボットシステム１が本発明の「ロボットシステム」の一例に相当し、ロボット２が本発明の「ロボット」の一例に相当し、コントローラ５、カメラＣiおよびエンコーダＥnが協働して本発明の「ロボット制御装置」の一例として機能し、コントローラ５が本発明の「コンピュータ」の一例に相当し、記録媒体６が本発明の「記録媒体」の一例に相当し、プログラム７が本発明の「プログラム」の一例に相当し、エンコーダＥnが本発明の「角度検出部」の一例に相当し、コントローラ５が本発明の「トルク算出部」および「駆動制御部」の一例として機能し、モータＭnが本発明の「関節駆動機構」の一例に相当する。また、数１４において、第５項で与えられるトルクが本発明の「第１印加量」あるいは「第１トルク」の一例に相当し、第２項で与えられるトルクが本発明の「第２印加量」あるいは「第２トルク」の一例に相当する。

なお、本発明は上記した第１実施形態に限定されるものではなく、その趣旨を逸脱しない限りにおいて上述したもの以外に種々の変更を行うことが可能である。例えば、上記第１実施形態では、エンドエフェクタ４に付されたマークをエンドエフェクタ４の位置として認識していた。しかしながら、エンドエフェクタ４の特徴的な箇所（例えば先端や穴等）や、エンドエフェクタ４が把持する対象物（例えばボルト等）の特徴的な箇所を、エンドエフェクタ４の位置として認識しても良い。

また、上記第１実施形態では、エンドエフェクタ４の１点を目標位置ｐdに移動させる場合について説明した。しかしながら、姿勢も考慮してエンドエフェクタ４を移動させる場合には、エンドエフェクタ４に３点の代表点を設けておき、３点をそれぞれの目標位置に移動させるように構成すれば良い。具体的には、エンドエフェクタ４に設けられた３点の位置をｘ、ｙ、ｚで表し、それぞれの目標位置をｘd、ｙd、ｚdで表したとき、３点の位置偏差は、Δｘ（＝ｘd−ｘ）、Δｙ（＝ｙd−ｙ）、Δｚ（＝ｚd−ｚ）で与えられる。そして、位置偏差Δｘ、Δｙ、Δｚのそれぞれについて、上述と同様の位置偏差積分トルクを算出して、関節Ｑに加えれば良い。

また、数１４で表された制御則に対しても適宜変更が可能である。具体例を挙げれば、重力補償のための第１項、回転角度ｑに対して微分制御を行なう第３項、あるいは位置偏差Δｐに対して比例制御を行なう第４項を省略するといった変更が可能である。

また、目標位置ｐdがカメラＣiの座標系の原点（像面ＩＭiの中心）に一致あるいは近接するように、カメラＣiのパン・チルトが制御されていた。しかしながら、目標位置ｐdとカメラＣiの座標系の原点を一致させる必要は必ずしもない。

また、上記の外界ループＬxと内界ループＬqを重畳してトルクτにフィードバックする制御を常に行なう必要は無い。例えば、エンドエフェクタ４がカメラＣiの視野から外れている場合は、外界ループＬxからのフィードバック量をゼロに設定して、外界ループＬxを作動させないように構成しても良い。そして、エンドエフェクタ４がカメラＣiの視野に入ってから、外界ループＬxを作動させても良い。

この際、エンドエフェクタ４がカメラＣiの視野に入るまでの動作は、ロボット２に予めティーチングしておいても良い。かかるティーチングは、エンドエフェクタ４をカメラＣiの視野にまで移動させるだけで良いので、粗いもので足りる。ただし、カメラＣiの視野に入るまでのエンドエフェクタ４の軌跡をトラッキングして、エンドエフェクタ４をカメラＣiの視野に収めるように制御しても、もちろん構わない。

あるいは、エンドエフェクタ４がカメラＣiの視野に入っても、目標位置ｐdから所定範囲の外にある間は、外界ループＬxを作動させずに、内界ループＬqのみを用いるように構成することもできる。この場合には、エンドエフェクタ４が目標位置ｐdから所定範囲に入ってから外界ループＬxを作動させれば良い。かかる構成では、エンドエフェクタ４を目標位置ｐdに移動させる最終段階での位置決めを、外界ループＬxを用いて高精度に実行することができる。

第２実施形態
図４は、本発明の第２実施形態にかかるロボットシステムの一例を示す模式図である。同図に示すように、ロボットシステム１は、ロボット２と２台のカメラＣ1、Ｃ2とで構成される。なお、図１では、ロボット２は関節Ｑ1〜Ｑ6、リンク３０およびエンドエフェクタ４の記号表記によって模式的に示されており、カメラＣ1、Ｃ2はそれぞれの像面ＩＭ1、ＩＭ2で代表して模式的に示されている。

ロボット２は、回転自在な関節Ｑ1〜Ｑ6を有して当該関節Ｑ1〜Ｑ6の回転に応じて動くアーム３の先端に、エンドエフェクタ４（手先）を取り付けた概略構成を具備する。具体的には、ロボット２の土台側（他端側）から先端側（一端側）へ向けて関節Ｑ1〜Ｑ6がこの順番でリンク３０を介して連結され、さらに関節Ｑ6から先端側へ向けて突き出すリンク３０が設けられている。こうして関節Ｑ1〜Ｑ6およびリンク３０からアーム３が構成される。そして、アーム３の先端（関節Ｑ6から一端側へ突き出すリンク３０の先端）にエンドエフェクタ４が取り付けられている。なお、リンク３０を介して互いに連結される６個の関節Ｑ1〜Ｑ6のうち最も先端側にある関節Ｑ6を、先端関節と適宜称することとする。

６個の関節Ｑ1〜Ｑ6はそれぞれ１の自由度を持って回転することができる。したがって、アーム３は関節Ｑ1〜Ｑ6の回転角度ｑ1〜ｑ6を変化させることで、先端に取り付けられたエンドエフェクタ４を６の自由度で移動させることができる。特に、このロボットシステム１では、基準点ｐ1が先端関節Ｑ6あるいは先端関節Ｑ6よりも土台側の範囲に設定される一方、基準点ｐ2、ｐ3が先端関節Ｑ6よりもエンドエフェクタ４側の範囲に設定される。より具体的には、図４に示す例では、基準点ｐ1は関節Ｑ3、Ｑ4間のリンク３０上に設定される一方、基準点ｐ2、ｐ3はエンドエフェクタ４上に設定されている。そして、基準点ｐ1〜ｐ3をカメラＣ1、Ｃ2で検出した結果に基づき、関節Ｑ1〜Ｑ6の回転角度ｑ1〜ｑ6が調整されて、三次元におけるエンドエフェクタ４の位置および姿勢が制御される。この際、基準点ｐ1〜ｐ3のそれぞれに例えばＬＥＤ(Light Emitting Diode)等のマークを取り付けて、カメラＣ1、Ｃ2での視認性を向上させても良い。

つまり、カメラＣ1、Ｃ2は、基準点ｐ1〜ｐ3の移動先ｐd1〜ｐd3をそれぞれの視野に捉えるように位置決めされており、基準点ｐ1〜ｐ3と移動先ｐd1〜ｐd3との位置偏差Δｐ1〜Δｐ3を互いに異なる平面で捉える（Δｐ1＝ｐd1−ｐ1、Δｐ2＝ｐd2−ｐ2、Δｐ3＝ｐd3−ｐ3）。具体的には例えば、像面ＩＭ1、ＩＭ2が互いに直交するようにカメラＣ1、Ｃ2を配置して、作業座標系ＸＹＺのＹＺ平面をカメラＣ1で撮像するとともに、作業座標系ＸＹＺのＺＸ平面をカメラＣ2で撮像するように構成することができる。そして、カメラＣ1、Ｃ2の撮像結果から検出した基準点ｐ1〜ｐ3と移動先ｐd1〜ｐd3との位置偏差Δｐ1〜Δｐ3が縮まるように、関節Ｑ1〜Ｑ6の回転角度ｑ1〜ｑ6が調整されて、三次元におけるエンドエフェクタ４の位置および姿勢が制御される（ビジュアルフィードバック）。

このビジュアルフィードバックでは、関節Ｑ3、Ｑ4の間のリンク３０上に設定された基準点ｐ1と、エンドエフェクタ４上に設定された基準点ｐ2、ｐ3とで自由度を使い分けて位置制御を行うことができる。上述のとおり、基準点ｐ1は関節Ｑ3、Ｑ4の間のリンク３０上に設定されている。換言すれば、基準点ｐ1（特定基準箇所）の設定位置は、先端側から順番に関節の自由度を数えた総計が３となる関節Ｑ4（特定関節）より土台側であって、土台側から順番に関節の自由度を数えた総計が３となる関節Ｑ3よりも先端側の範囲にある。したがって、関節Ｑ1〜Ｑ3で実現される３個の自由度によって基準点ｐ1の位置制御を行う一方、関節Ｑ4〜Ｑ6で実現される３個の自由度によって基準点ｐ2、ｐ3の位置制御を行うことができる。こうして、基準点ｐ1の位置制御に供する自由度（関節Ｑ1〜Ｑ3による３自由度）と、基準点ｐ2、ｐ3の位置制御に供する自由度（関節Ｑ4〜Ｑ6による３自由度）とを使い分けた制御が可能となっている。

さらに、このビジュアルフィードバックでは、特異点にあるロボット２の撮像結果は、エンドエフェクタ４の位置制御にフィードバックしないように構成することができる。具体例を挙げれば、特異点にあるロボット２がカメラＣ1、Ｃ2の視野から外れるように、カメラＣ1、Ｃ2を配置しても良い。これによって、特異点にあるロボット２の撮像結果に基づいてビジュアルフィードバックが行われて、ロボット２の制御が不安定になることを抑制できる。

ここで、エンドエフェクタ４の位置制御に用いられる座標系や各制御量の表記について説明しておく。図４に示すように、エンドエフェクタ４が作業を行う作業空間に対しては、互いに直交するＸ軸、Ｙ軸、Ｚ軸で構成された、Ｚ軸を鉛直軸とする三次元の作業座標系が設けられる。よって、基準点ｐ1〜ｐ3の位置は、それぞれ三次元ベクトル（ｐ1x、ｐ1y、ｐ1z）、（ｐ2x、ｐ2y、ｐ2z）、（ｐ3x、ｐ3y、ｐ3z）で与えられる。同様に、移動先ｐd1〜ｐd3は、それぞれ三次元ベクトル（ｐd1x、ｐd1y、ｐd1z）、（ｐd2x、ｐd2y、ｐd2z）、（ｐd3x、ｐd3y、ｐd3z）で与えられる。さらに、位置偏差Δｐ1〜Δｐ3は、それぞれ三次元ベクトル（Δｐ1x、Δｐ1y、Δｐ1z）、（Δｐ2x、Δｐ2y、Δｐ2z）、（Δｐ3x、Δｐ3y、Δｐ3z）で与えられる。

なお、以下では、３個の基準点ｐ1〜ｐ3を区別せずに基準点ｐで代表する表記、３個の移動先ｐd1〜ｐd3を区別せずに移動先ｐdで代表する表記、３個の位置偏差Δｐ1〜Δ3を区別せずに位置偏差Δｐで代表する表記、を適宜用いることとする。この際、位置偏差Δｐは基準点ｐ、移動先ｐdを用いて、式Δｐ＝ｐd−ｐで与えられることとなる。また、これらに対応して、基準点ｐの位置を三次元ベクトル（ｐx、ｐy、ｐz）で表し、移動先ｐdの位置を三次元ベクトル（ｐdx、ｐdy、ｐdz）で表し、偏差Δｐを三次元ベクトル（Δｐx、Δｐy、Δｐz）で表す。

ロボット２の関節Ｑの回転角度ｑは、関節Ｑnの回転角度ｑnを各成分とするベクトル（ｑ1、ｑ2、ｑ3、ｑ4、ｑ5、ｑ6）として表される。ここで、関節Ｑとの表記は、関節Ｑ1〜Ｑ6を総称した表記であり、「ｎ」は関節を区別するための番号である（ｎ＝１、２、３、４、５、６）。また、基準点ｐ1〜ｐ3の全てがそれぞれの対応する移動先ｐd1〜ｐd3に一致するときの関節Ｑの回転角度ｑを、目標角度ｑd（ｑd1、ｑd2、ｑd3、ｑd4、ｑd5、ｑd6）とする。さらに、ロボット２の関節Ｑに与えられるトルクτは、関節Ｑnに働くトルクτnを各成分とするベクトル（τ1、τ2、τ3、τ4、τ5、τ6）として表される。

以上が座標系および制御量の表記に関する説明である。続いて、エンドエフェクタ４の位置制御の詳細について説明する。第２実施形態にかかるロボットシステム１においても、図２に示した電気的構成が具備されている。そして、第１実施形態でエンドエフェクタ４の位置ｐを目標位置ｐdに収束させたのと同様の位置制御が、基準点ｐ1〜ｐ3（ｐ）のそれぞれに対して実行されて、基準点ｐ1〜ｐ3（ｐ）をそれぞれの移動先ｐd1〜ｐd3（ｐd）に収束させる。以下では、第１実施形態と共通する構成については説明を適宜省略し、第１実施形態と異なる構成を中心に説明を行う。

ロボットシステム１では、関節Ｑnを駆動するモータＭnが、関節Ｑ1〜Ｑ6のそれぞれについて設けられるとともに、モータＭnの回転位置を検出するエンコーダＥnが、モータＭ1〜Ｍ6のそれぞれについて設けられている。そして、コントローラ５は、モータＭ1〜Ｍ6のそれぞれを制御することで、ロボット２の関節Ｑの回転角度ｑを調整する。特に、上述のビジュアルフィードバックを実行するために、コントローラ５は、カメラＣi（外界センサ）による基準点ｐの撮像結果から、基準点ｐの位置偏差Δｐ（＝ｐd−ｐ）を検出する。この際、コントローラ５は、移動先ｐd1〜ｐd3の例えば幾何重心がカメラＣiの座標系の原点（像面ＩＭiの中心）に一致あるいは近接しつつ、移動先ｐd1〜ｐd3が視野に捉えられるように、カメラＣiのパン・チルトを制御しつつ、位置偏差Δｐの検出を行う。なお、ロボット２における基準点ｐの設定場所は、コントローラ５（記憶部）のメモリーに予め格納されている。

また、基準点ｐの位置偏差Δｐの検出と並行して、コントローラ５は、エンコーダＥ1〜Ｅ6（内界センサ）の出力から、関節Ｑの角度偏差Δｑ（＝ｑd−ｑ）を検出する。そして、コントローラ５は、位置偏差Δｐ（＝ｐd−ｐ）および角度偏差Δｑ（＝ｑd−ｑ）に基づいて、トルクτを算出する。そして、モータＭ1〜Ｍ6がトルクτを関節Ｑに与えることで、関節Ｑの回転角度ｑが調整される。このように、この実施形態では、カメラＣiおよびエンコーダＥnの検出結果がトルクτにフィードバックされて、エンドエフェクタ４の位置が制御される。

第２実施形態においても、エンドエフェクタ４の位置制御は、図３のブロック線図に従って実行される。したがって、数１〜数１３より導出される数１４の制御側に基づいて、関節Ｑに加えられるトルクτが決定される。なお、上述のとおり、ロボット２に対しては３つの基準点ｐ1〜ｐ3が設けられている。したがって、位置偏差Δｐに対して比例制御を行う項（数１４の第４項）は、位置偏差Δｐ1〜Δｐ3それぞれに対して比例制御を行う項の線形結合で与えられ、位置偏差Δｐに対して積分制御を行う項（数１４の第５項）は、位置偏差Δｐ1〜Δｐ3それぞれに対して積分制御を行う項の線形結合で与えられる。その結果、関節Ｑに加えられるトルクτを与える式は、上記数１４から次式に書き換えることができる。

なお、数１５の第４項および第５項はいずれも、位置偏差Δｐ1〜Δｐ3それぞれに対する演算（比例・積分）の結果の加重平均で与えられている。この加重平均の重み係数δ1〜δ3のそれぞれは、対応する基準点ｐ1〜ｐ3がカメラＣ1、Ｃ2両方の視野にあるときはゼロより大きい正の値を有し、対応する基準点ｐ1〜ｐ3がカメラＣ1、Ｃ2のいずれかの視野から外れているときはゼロとなり、当該視野から外れた基準点ｐ1〜ｐ3に対するビジュアルフィードバックは働かない。

以上に説明したように、この実施形態では、先端関節Ｑ6よりもエンドエフェクタ４側に対して設定された少なくとも２点ｐ2、ｐ3を含む、ロボット２に対して設定された３点以上の基準点ｐ1〜ｐ3を、それぞれの移動先ｐd1〜ｐd3へ向けて移動させて、エンドエフェクタ４の位置を制御する。具体的には、各基準点ｐ1〜ｐ3の移動先ｐd1〜ｐd3が視野に捉えられた状態で基準点ｐ1〜ｐ3が撮像されて、基準点ｐ1〜ｐ3と移動先ｐd1〜ｐd3との間の位置偏差Δｐ1〜Δｐ3が求められる。これと並行して、関節Ｑ1〜Ｑ6の回転角度ｑ1〜ｑ6が検出されて、検出角度ｑと目標角度ｑdとの角度偏差Δｑが求められる。そして、位置偏差Δｐ1〜Δｐ3に基づくトルク（数１４あるいは数１５の第５項で与えられるトルク）が算出されるとともに、角度偏差Δｑに基づくトルク（数１４あるいは数１５の第２項で与えられるトルク）が算出される。そして、これらのトルクが関節Ｑに重畳して加えられる。かかる構成では、撮像結果から基準点ｐ1〜ｐ3の位置偏差Δｐ1〜ｐ3が得られなくなっても、関節Ｑの回転角度ｑは検出できるため、角度偏差Δｑに基づくトルクにより関節Ｑを回転させて基準点ｐ1〜ｐ3を移動先ｐd1〜ｐd3に近づけ、その結果、エンドエフェクタ４を目標位置へ向けて移動できる。

ただし、上述したとおり、位置偏差Δｐに基づくトルクと関節Ｑの角度偏差Δｑに基づくトルクとが重畳して関節Ｑに働く構成では、運動学的な演算が確かでないと、位置偏差Δｐに基づくトルクが角度偏差Δｑに基づくトルクに対して逆向きに作用して、関節Ｑの回転を停止させてしまうおそれがあった。この場合、位置偏差Δｐが残存した状態で、エンドエフェクタ４が停止してしまうこととなる。

これに対して、この実施形態では、角度偏差Δｑに基づくトルクに重畳されるトルクは、基準点ｐの位置偏差Δｐに応じた値に積分演算を行って求められた位置偏差積分トルクである。したがって、運動学的な演算に誤差があるために、基準点ｐが移動先ｐdに到達する前に関節Ｑの回転が停止あるいは停止しようとした場合には、位置偏差Δｐが残存するので、位置偏差積分トルクは時間とともに増大して、停止あるいは停止しようとする関節Ｑを回転させる。そして、この位置偏差積分トルクは、最終的に位置偏差Δｐが無くなるまで関節Ｑを回転させ続けて、基準点ｐを移動先ｐdまで移動させる。その結果、エンドエフェクタ４は目標位置にまで移動することができる。したがって、この実施形態では、運動学的な演算に誤差があっても、位置偏差Δｐに基づくトルクの機能によって、エンドエフェクタ４を目標位置に確実に移動させることができる。その結果、高精度のキャリブレーションを必要とせず、キャリブレーションの負担を軽減することが可能となっている。

特に、この実施形態では、転置ヤコビ行列が乗ぜられた位置偏差Δｐに対して積分演算を行うといった運動学的な演算から、位置偏差積分トルクが算出されている。これに対して、この実施形態によれば、運動学的な演算に誤差があっても、エンドエフェクタ４を目標位置に確実に移動させることができる。したがって、転置ヤコビ行列が不正確で、位置偏差積分トルクを求める運動学的な演算に誤差があっても、エンドエフェクタ４を目標位置に確実に移動させることができる。そのため、転置ヤコビ行列を正確に求めるために、キャリブレーションを高精度に実行する必要は特に無く、キャリブレーションの負担が軽減できる。

上述の技術内容は、次のように捉えることもできる。つまり、角度偏差Δｑに基づくトルクと位置偏差Δｐに基づくトルクとが重畳して関節Ｑに加えられる構成では、運動学的な演算に誤差があると、両トルクが拮抗して、関節Ｑの回転が停止するおそれがあった。これは、運動学的な演算に誤差があると、作業空間でのポテンシャル分布が目標位置とは別の位置に極小値を持ち、エンドエフェクタ４がこの極小値に陥るためと考えられる。これに対して、上記の位置偏差積分トルクを関節Ｑに加えた場合、時間とともに増大する位置偏差積分トルクの作用によって、エンドエフェクタ４を目標位置にまで移動させることができる。したがって、運動学的な演算の誤差に起因して目標位置とは別の位置に極小値が生じるような場合であっても、エンドエフェクタ４を目標位置に確実に移動させることができる。その結果、高精度のキャリブレーションを必要とせず、キャリブレーションの負担を軽減することが可能となっている。

ところで、この実施形態によれば、基準点ｐと移動先ｐdとの間に位置偏差Δｐがあれば、位置偏差積分トルクが関節Ｑに働いて、エンドエフェクタ４を目標位置にまで移動させることができる。上記説明では、この位置偏差Δｐの原因として、運動学的な演算の誤差を挙げた。しかしながら、この実施形態は、位置偏差Δｐの発生原因を問わず、位置偏差Δｐがあればこれを解消するように、基準点ｐを移動先ｐdにまで移動させる。つまり、重力補償のための項、目標角度ｑdあるいは検出角度ｑ等の値が不正確であることを原因とする位置偏差Δｐも、位置偏差積分トルクの作用によって解消することができる。したがって、基準点ｐの位置偏差（Δｐ＝ｐd−ｐ）さえ求まれば、基準点ｐを移動先ｐdへ収束させて、エンドエフェクタ４を目標位置に移動させることができる。その結果、精度の求められるパラメータが少ないため、キャリブレーションの負担は極めて軽く、場合によってはキャリブレーションを省略することも可能となる。

また、この実施形態では、エンドエフェクタ４に対して設定された少なくとも２点ｐ2、ｐ3を含む、ロボット２に対して設定された３点以上の基準点ｐ1〜ｐ3を、それぞれの移動先ｐd1〜ｐd3へ向けて移動させて、エンドエフェクタ４の位置を制御する。その結果、三次元においてエンドエフェクタ４を目標の位置および姿勢に適切に制御することが可能となっている。

ところで、３点以上の基準点ｐ1〜ｐ3をそれぞれの移動先ｐd1〜ｐd3へ移動させる構成では、基準点ｐ1〜ｐ3の設定位置と移動先ｐd1〜ｐd3との位置関係によっては、全ての基準点ｐ1〜ｐ3をそれぞれの移動先ｐd1〜ｐd3へ収束させることが困難となるといった問題が生じるおそれがある。特に基準点ｐ1〜ｐ3の全てを先端関節Ｑ6よりもエンドエフェクタ４側に対して設定した際に、かかる問題が発生しやすい。この点について具体例を挙げて説明する。

図５は、基準点をそれぞれの移動先へ動かす過程においてエンドエフェクタへ働くトルクの状態の一例を模式的に示す図である。つまり、図５では、基準点ｐ1〜ｐ3を移動先ｐd1〜ｐd3へ移動させる過程において発生しうる、基準点ｐ1〜ｐ3と移動先ｐd1〜ｐd3との位置関係が例示されている。同図の状態となった場合、位置偏差Δｐ1に基づき発生する基準点ｐ1から移動先ｐd1へ向かうトルクＴ1と、位置偏差Δｐ2に基づき発生する基準点ｐ2から移動先ｐd2へ向かうトルクＴ2と、位置偏差Δｐ3に基づき発生する基準点ｐ3から移動先ｐd1へ向かうトルクＴ3とが互いに釣り合うため、エンドエフェクタ４は停止してしまう。換言すれば、位置偏差Δｐ1〜Δｐ3が残存した状態で、作業空間でのポテンシャル分布におけるローカルミニマムにエンドエフェクタ４が陥ってしまう。これに対して、全ての基準点ｐ1〜ｐ3を先端関節Ｑ6よりエンドエフェクタ４側に設定した構成では、全基準点ｐ1〜ｐ3を移動させるための自由度が共通するために、エンドエフェクタ４をローカルミニマムから脱出させることが困難となる場合があった。

これに対して、この実施形態では、先端関節Ｑ6あるいは先端関節Ｑ6よりも他端側に対して特定基準点ｐ1が設定されている。このように一部の基準点（特定基準点ｐ1）を先端関節Ｑ6あるいは先端関節Ｑ6よりも他端側に設けた場合、上述のようなトルクＴ1〜Ｔ3の均衡を崩して、ローカルミニマムからエンドエフェクタ４を抜け出させて、全ての基準点ｐ1〜ｐ3をそれぞれの移動先ｐd1〜ｐd3へ収束させることが容易となる。

特に、この実施形態では、一端側（先端側）から順番に関節の自由度を数えた総計が３以上となる特定関節Ｑ4あるいは当該特定関節Ｑ4より他端側（土台側）に特定基準点ｐ1が設定されている。このような構成では、特定基準点ｐ1については特定関節Ｑ4より他端側の自由度（関節Ｑ1〜Ｑ3による自由度）によって適切に移動先ｐd1へ収束させることができる。また、特定基準点ｐ1よりエンドエフェクタ４側に対して設定された基準点ｐ2、ｐ3については、特定関節Ｑ4を含めて特定関節Ｑ4より一端側の３以上の自由度（関節Ｑ4〜Ｑ6による自由度）によって適切に移動先ｐd2、ｐd3へ収束させることができる。つまり、特定基準点ｐ1とそれ以外の基準点ｐ2、ｐ3とで自由度を使い分ける制御が実行可能となる。その結果、上述のようなローカルミニマムにエンドエフェクタ４が陥ったとしても、基準点ｐ1と基準点ｐ2、ｐ3とをそれぞれ別個の自由度でもって変位させて、上述のようなトルクＴ1〜Ｔ3の均衡を容易に崩すことができる。その結果、ローカルミニマムからエンドエフェクタ４を抜け出させて、各基準点ｐ1〜ｐ3を移動先へ適切に収束させることが容易となる。

さらに、この実施形態では、他端側（土台側）から順番に関節の自由度を数えた総計が３以上となる関節Ｑ3よりも一端側（先端側）に特定基準点ｐ1が設定されている。これによって、特定基準点ｐ1を３以上の自由度でもって移動先ｐd1へより確実に収束させることができる。特に、このような構成では、基準点ｐ1と基準点ｐ2、ｐ3とをそれぞれ別個の３以上の自由度でもって比較的自在に変位させることができる。その結果、上述のようなトルクＴ1〜Ｔ3の均衡をより容易に崩すことができ、ローカルミニマムからエンドエフェクタ４を抜け出させて、各基準点ｐ1〜ｐ3を移動先へ適切に収束させることがより容易となる。

なお、この明細書では、順番に関節の自由度を数えた総計が「Ｎ以上」となる関節といった表現を適宜用いる。この表現は、順番に関節の自由度を数えた場合の総計がはじめて「Ｎ以上」になる関節を示す。したがって、上記のように、一端側から順番に関節の自由度を数えた総計が「３以上」となる関節は関節Ｑ4のみであり、他の関節Ｑ1〜Ｑ3は該当しない。同様に、他端側から順番に関節の自由度を数えた総計が「３以上」となる関節は関節Ｑ3のみであり、他の関節Ｑ4〜Ｑ6は該当しない。

ところで、数１５に示すように、位置偏差積分トルクは、基準点ｐ1〜ｐ3それぞれの位置偏差Δｐ1〜Δｐ3についての積分値の線形結合で与えられ、特に重み係数δj（δ1、δ2、δ3）が乗ぜられた加重平均で与えられる。この際、特定基準点ｐ1についての積分値（位置偏差Δｐ1に対する積分値）の重み係数δ1を、他の基準点ｐ2、ｐ3についての積分値（位置偏差Δｐ2、Δｐ3に対する積分値）の重み係数δ2、δ3よりも大きく設定しても良い（δ1＞δ2かつδ1＞δ3）。このような構成では、より大きな重み係数δ1に対応する特定基準点ｐ1をより速く移動先ｐd1へ収束させた後に、先端関節Ｑ6よりもエンドエフェクタ４側に対して設定された基準点ｐ2、ｐ3を移動先ｐd2、ｐd3へ収束させるといった動作が実行できる。したがって、特定関節Ｑ4を含めて特定関節Ｑ4より一端側の自由度、換言すれば特定基準点ｐ1よりもエンドエフェクタ４側の自由度を、実質的に基準点ｐ2、ｐ3の移動にのみ用いることができる。よって、これら基準点ｐ2、ｐ3を、十分な自由度でもって移動先ｐd2、ｐd3へ確実に収束させることができる。

以上に説明したように、第２実施形態では、ロボットシステム１が本発明の「ロボットシステム」の一例に相当し、ロボット２が本発明の「ロボット」の一例に相当し、コントローラ５、カメラＣiおよびエンコーダＥnが協働して本発明の「ロボット制御装置」の一例として機能し、コントローラ５が本発明の「コンピュータ」の一例に相当し、記録媒体６が本発明の「記録媒体」の一例に相当し、プログラム７が本発明の「プログラム」の一例に相当する。また、カメラＣiが本発明の「撮像部」の一例に相当し、エンコーダＥnが本発明の「変位量検出部」の一例に相当し、コントローラ５が本発明の「位置偏差取得部」、「印加量算出部」および「駆動制御部」の一例として機能する。また、数１４、数１５において、第５項で与えられるトルクが本発明の「第１印加量」の一例に相当し、第２項で与えられるトルクが本発明の「第２印加量」の一例に相当し、第３項で与えられるトルクが本発明の「第３印加量」の一例に相当する。また、基準点ｐ1〜ｐ3が本発明の「基準箇所」の一例に相当し、特定基準点ｐ1が本発明の「特定基準箇所」に相当し、先端関節Ｑ6が本発明の「先端関節」の一例に相当し、特定関節Ｑ4が本発明の「特定関節」の一例に相当する。

上述してきたように、第２実施形態においても、位置偏差Δｐに対して積分演算を行って求めたトルクと、角度偏差Δｑに対して比例演算を行って求めたトルクとを関節Ｑに重畳することで、各基準点ｐを移動先ｐdに確実に収束させていた。なお、本発明は上記した第２実施形態に限定されるものではなく、その趣旨を逸脱しない限りにおいて上述したもの以外に種々の変更を行うことが可能である。例えば、上記第２実施形態では、ロボット２に付されたＬＥＤ等のマークを基準点ｐ1〜ｐ3に設定していた。しかしながら、ロボット２の特徴的な箇所（例えば先端や穴等）を基準点ｐ1〜ｐ3に設定しても良い。

また、数１５で表された制御則に対しても適宜変更が可能である。具体例を挙げれば、重力補償のための第１項、回転角度ｑに対して微分制御を行なう第３項、あるいは位置偏差Δｐに対して比例制御を行なう第４項を省略するといった変更が可能である。

また、数１５における加重加算の重み係数δ1〜δ3の具体的な設定値についても適宜変更可能である。したがって、特定基準点ｐ1に対する重み係数δ1よりもその他の基準点ｐ2、ｐ3の重み係数を大きくしても良いし、あるいは全ての重み係数δ1〜δ3を同じ値に設定しても良い。

また、ロボット２における特定基準点ｐ1の設定場所についても適宜変更可能である。したがって、ロボット２上において、先端関節Ｑ6を含んで先端関節Ｑ6よりも土台側（他端側）の適当な場所に特定基準点ｐ1を設定することができ、例えば、先端関節Ｑ6上あるいは特定関節Ｑ4上に特定基準点ｐ1を設定しても良い。

また、ロボット２における基準点ｐ2、ｐ3の設定場所についても適宜変更可能である。したがって、基準点ｐ2あるいは基準点ｐ3を先端関節Ｑ6から先端側に突出したリンク３０上に設けても構わない。

さらに、上述のような特定基準点ｐ1を設ける必要は必ずしもない。したがって、例えば全ての基準点ｐ1〜ｐ3をエンドエフェクタ４上に設けても構わない。

また、基準点ｐの個数も上述の３個に限られない。したがって、４個以上の基準点ｐをロボット２に対して設定しても良い。

また、ロボット２の自由度も上述の６に限られない。したがって、７以上の自由度を持つロボット２に対しても本発明を適用することができる。

また、上記第２実施形態では、関節Ｑ1〜Ｑ6はいずれも１の自由度を持つものであった。しかしながら、複数の自由度を持つ関節Ｑを用いて構成されたロボット２に対しても本発明を適用することができる。

また、上記第２実施形態では、移動先ｐd1〜ｐd3の例えば幾何重心がカメラＣiの座標系の原点（像面ＩＭiの中心）に一致あるいは近接するように、カメラＣiのパン・チルトが制御されていた。しかしながら、移動先ｐd1〜ｐd3とカメラＣiの座標系との位置関係の制御態様はこれに限られない。

また、上記実施形態では、基準点ｐがカメラＣiの視野から外れている場合は、外界ループＬxからのフィードバック量がゼロとなり、該当する基準点ｐについての外界ループＬxが作動しない。そして、該当する基準点ｐがカメラＣiの視野に入ってから、外界ループＬxが作動するように構成されている。この際、基準点ｐがカメラＣiの視野に入るまでの動作は、ロボット２に予めティーチングしておいても良い。かかるティーチングは、基準点ｐをカメラＣiの視野にまで移動させるだけで良いので、粗いもので足りる。ただし、カメラＣiの視野に入るまでの基準点ｐの軌跡をトラッキングして、基準点ｐをカメラＣiの視野に収めるように制御しても、もちろん構わない。

あるいは、基準点ｐがカメラＣiの視野に入っても、移動先ｐdから所定範囲の外にある間は、外界ループＬxを作動させずに、内界ループＬqのみを用いるように構成することもできる。この場合には、基準点ｐが移動先ｐdから所定範囲に入ってから外界ループＬxを作動させれば良い。かかる構成では、基準点ｐを移動先ｐdに移動させる最終段階での位置決めを、外界ループＬxを用いて高精度に実行することができる。

安定性について
続いては、数１５で与えられる制御則の安定性について考察する。ロボットダイナミクスは一般に次式で与えられる。

ここで、変数ξを次のように定義する。

数１７を数１６に代入すると次式を得る。

なお、次の条件が常に成立すると見なす。

ここで、リアプノフ関数候補として次の関数Ｖを考える。

αが適切に小さく、ｇ_ＩがＧ_ｐよりも十分に小さい場合には、Ｖ＞０が成立する。

また、関数Ｖの時間微分は次式で与えられる。

数２１を変形して、関数Ｖの時間微分は次式で与えられる。

ここで、変数λを次のように定義する。

この際、関数Ｖの時間微分について以下の不等式が得られる。

ただし、上の不等式を得るにあたって、以下の関係を利用した。

目標角度ｑdのまわりでテイラー展開することで、次式が得られる。

また、数２６より次式が得られる。

さらに、次式が成立するとする。

この際、関数Ｖの時間微分を与える数２４の第２項について次の不等式が得られる。

さらに、関数Ｖの時間微分を与える数２４の第３項について次の不等式が得られる。

この際、関数Ｖの時間微分が負となるために、数２４において次式が成立しうることが、重要となる。

ここで、次式が成立する。

したがって、数３１の条件は次の条件に変換できる。

結局、リアプノフ関数の時間微分について次の不等式を得る。

このとき、数３３のとおり次式が与えられる。

したがって、数３５の左辺が正定行列となって数３５が満たされれば、関数Ｖの時間微分は負となり得て、数１５で与えられる制御則において、各基準点ｐ1〜ｐ3はそれぞれの移動先ｐd1〜ｐd3において安定となる。そこで、続いては基準点ｐ1〜ｐ3の設定態様と安定性について考える。

上記第２実施形態では、関節Ｑ1〜Ｑ6を連結して６の自由度を持ったロボット２に対して、関節Ｑ3、Ｑ4の間に基準点ｐ1を設定し、エンドエフェクタ４に基準点ｐ2、ｐ3を設定していた。この場合には、基準点ｐ1の移動に関節Ｑ4〜Ｑ6は供しないため、次式が得られる。

したがって、数３５の左辺は、次式で与えられる。

ここで、次式が満たされるとする。

この際、数３７は次式と等しくなる。

ここで、重み係数δ1を他の重み係数δ2、δ3よりも十分に大きくとれば、次式が成立する。つまり、各基準点ｐ1〜ｐ3がそれぞれの移動先ｐd1〜ｐd3において安定となる。

ただし、次式が満たされているとする。

また、３点の基準点ｐ1〜ｐ3の全てがエンドエフェクタ４上に設定されている場合は、数３８の代わりに次式が成立する。したがって、次式の右辺の行列が正定行列となれば、各基準点ｐ1〜ｐ3がそれぞれの移動先ｐd1〜ｐd3において安定となる。

あるいは、基準点ｐ1の位置が関節Ｑ1〜Ｑ3で実現される３の自由度で制御されるとともに、その他の基準点ｐ2、ｐ3が関節Ｑ4〜Ｑ6で実現される３の自由度で主として制御されるとして、部分的ヤコビ行列を用いた次式を仮定することもできる。

この仮定において、数３５が満たされれば、数１５で与えられる制御則において、各基準点ｐ1〜ｐ3がそれぞれの移動先ｐd1〜ｐd3において安定となる。

その他
なお、本発明は上記した第２実施形態に限定されるものではなく、その趣旨を逸脱しない限りにおいて上述したもの以外に種々の変更を行うことが可能である。例えば、上記実施形態では、トルクの計算過程において、座標の回転行列Ｒの推定値が用いられていた。上記の説明では、回転行列Ｒの推定値を求める具体的手法については、特に言及しなかった。しかしながら、回転行列Ｒを取得する具体的手法として、種々のものが採用可能である。具体例を挙げると、方位センサ、姿勢センサあるいはジャイロセンサをロボット２およびカメラＣiに取り付けて、ロボット２とカメラＣiの位置関係をそれぞれのセンサ出力から算出し、回転行列Ｒの推定値を取得しても良い。かかる構成では、ロボット２やカメラＣiの配置状態によらず、これらの方位角が求められる。そのため、ユーザの操作性やロボット２の作業性に応じて適当にロボット２やカメラＣiを配置しても、それぞれの方位角から回転行列Ｒの推定値を取得できる。さらに、ロボット２とカメラＣiの位置関係の算出を、例えばプログラム７による自動計算により行っても良い。なお、ジャイロセンサを用いる場合は、ロボット２およびカメラＣiそれぞれのセンサの方向を同一方向に揃えた後に、各センサの積分値から方位角を求めれば良い。

また、上記実施形態では、特異点にあるロボット２がカメラＣ1、Ｃ2の視野から外れるようにカメラＣ1、Ｃ2を配置することで、特異点にあるロボット２の撮像結果をエンドエフェクタの位置制御にフィードバックしないように構成していた。しかしながら、かかる構成は必須ではなく、カメラＣ1、Ｃ2の視野に特異点にあるロボット２が含まれていても構わない。

また、カメラＣ1、Ｃ2の視野に特異点にあるロボット２が含まれる場合には、コントローラ５による制御態様を変更することで、特異点にあるロボット２の撮像結果がエンドエフェクタの位置制御にフィードバックされないように構成しても良い。これによって、特異点に起因して、ロボット２の制御が不安定になるのを抑制できる。

また、カメラ座標系から作業座標系への変換の具体的態様は、上述の内容に限られず、適宜変更することができる。

また、カメラＣiの配置や個数についても上述の内容に限られず、適宜変更することができる。

また、ロボット２を構成するリンクＬ1〜Ｌ3、３０や関節Ｑの具体的な個数、寸法、動作方向についても、上記実施形態で示したものから適宜変更が可能である。したがって、関節Ｑとして直動関節を用いることもできる。ちなみに、直動関節Ｑに対して本発明を適用するにあたっては、「角度」に代えて「直動方向への変位量」を検出し、「トルク」に代えて「力」を算出して、直動関節Ｑの動作を制御すればよい。

次に本発明の実施例を示す。ただし、本発明はもとより下記の実施例によって制限を受けるものではない。したがって、前後記の趣旨に適合しうる範囲で適当に変更を加えて実施することももちろん可能であり、それらはいずれも本発明の技術的範囲に含まれる。

以下では、図１のロボット２と同様の構成を備えるＭＭＳＥ（マン・マシン・シナジー・エフェクタズ）株式会社製の３ＤＯＦ(Three Degrees Of Freedom)ロボットユニットを用いて、エンドエフェクタ４を目標位置ｐdまで移動させる実験を行なった結果を示す。具体的には、カメラＣ1、Ｃ2の内部パラメータ、カメラＣ1、Ｃ2の外部パラメータ、カメラＣ1、Ｃ2の位置、およびロボット２とカメラＣ1、Ｃ2の間の関係について、いずれもキャリブレーションを行わない状態で、エンドエフェクタ４を目標位置ｐdまで移動できることが、この実験で確認された。

なお、ロボット２を制御するコントローラ５はIntel Core i7 960（ＣＰＵ）と４GBのＤＲＡＭ(Dynamic Random Access Memory)で構成し、カメラＣ1、Ｃ2は２００万画素のカメラを用いた。エンドエフェクタ４の位置ｐおよび目標位置ｐdの検出は、それぞれに設けられたマーカのエッジを抽出した結果に、テンプレートマッチングを適用して行った。ロボット２のキャリブレーションには誤差を持たせており、具体的には、回転角度ｑの値は９０度に対して５度の誤差を持ち、リンクＬ1〜Ｌ3の長さは±１００[mm]以内の誤差を持つ。

実施例１
実施例１の制御則を表す式は次のとおりである。

数４４に示すように実施例１では、作業座標系における位置偏差ΔｐのＸ軸成分として、カメラＣ1の座標系における位置偏差のＵ1軸成分を採用し、作業座標系における位置偏差ΔｐのＹ軸成分として、カメラＣ2の座標系における位置偏差のＵ2軸成分を採用し、作業座標系における位置偏差ΔｐのＺ軸成分として、カメラＣ1、Ｃ2の座標系における位置偏差のＵ1、Ｕ2軸成分の平均を採用している。また、上述のとおり、本発明によれば、ヤコビ行列には精度が求められないため、ヤコビ行列の各成分は適当な定数としておけば足りる。そこで、例えば目標角度ｑdにおけるヤコビ行列の各成分の値を用いることができる。

図６および図７は、実施例１での実験結果を示す図である。両図において、時間を横軸とするとともに位置偏差Δｐを縦軸とするグラフが上段に示され、時間を横軸とするとともに目標角度および回転角度を縦軸とするグラフが下段に示されている。位置偏差Δｐについては、Ｘ軸、Ｙ軸およびＺ軸の各成分が示されている。そして、図６と図７の違いは、フィードバックゲインにある。特に、図７の実験よりも図６の実験の方が、積分ゲインを大きく設定している。

図６の実験における各フィードバックゲインは、次のとおりである。

図７の実験における各フィードバックゲインは、次のとおりである。

両図の上段のグラフに示すように、いずれの実験結果においても、位置偏差Δｐは１ピクセル程度に収束しており、換言すれば、０．３[mm]程度の精度でエンドエフェクタ４の位置を制御することが可能となっている。つまり、カメラＣ1、Ｃ2の内部パラメータ、カメラＣ1、Ｃ2の外部パラメータ、カメラＣ1、Ｃ2の位置、およびロボット２とカメラＣ1、Ｃ2の間の関係について、いずれもキャリブレーションを行わなくとも、エンドエフェクタ４を目標位置ｐdまで移動することが可能となっている。

実施例２
図８ないし図１１は、実施例２での実験結果を示す図である。図８および図１０での表記は、図６および図７の上段の表記に相当し、図９および図１１での表記は、図６および図７の下段の表記に相当する。各図での時刻ｔsは、ロボット２の制御を開始した時刻である。この実施例２では、カメラＣ1、Ｃ2とエンドエフェクタ４の経路との位置関係が異なる２通りの実験結果を示す。キャリブレーションを省略している点は実施例１と同様である。

図８および図９に示す実験では、目標位置ｐdから３０[cm]の範囲をカメラＣ1、Ｃ2の視野が概ね捉えるようにカメラＣ1、Ｃ2を配置し、エンドエフェクタ４の移動はカメラＣ1、Ｃ2の視野内から開始した。図１０および図１１に示す実験では、目標位置ｐdから５[cm]の範囲をカメラＣ1、Ｃ2の視野が概ね捉えるようにカメラＣ1、Ｃ2を配置し、エンドエフェクタ４の移動はカメラＣ1、Ｃ2の視野外から開始した。

図８および図１０に示すように、カメラＣ1、Ｃ2と目標位置ｐdとの距離に依らず、位置偏差Δｐを収束させることが可能となっている。また、エンドエフェクタ４の移動を、カメラＣ1、Ｃ2の視野内から開始しても視野外から開始しても、位置偏差Δｐを収束させることが可能となっている。これらの結果から、カメラＣ1、Ｃ2とエンドエフェクタ４の経路との位置関係にあまり影響されずに位置偏差Δｐを収束させることができ、これらの位置関係は大雑把で良いことが判る。

実施例３
図１２および図１３は、実施例３での実験結果を示す図である。なお、図１２での表記は、図６および図７の上段の表記に相当し、図１３での表記は、図６および図７の下段の表記に相当する。各図での時刻ｔsは、ロボット２の制御を開始した時刻である。この実施例３では、位置偏差Δｐに対する比例ゲインがゼロに設定されており、位置偏差Δｐに対する比例制御が実行されない。なお、目標位置ｐdから５[cm]の範囲をカメラＣ1、Ｃ2の視野が概ね捉えるようにカメラＣ1、Ｃ2を配置し、エンドエフェクタ４の移動はカメラＣ1、Ｃ2の視野外から開始した。キャリブレーションを省略している点は実施例１と同様である。図１２に示すように、位置偏差Δｐに対する比例制御を行わなくても、位置偏差Δｐに対する積分制御によって、位置偏差Δｐを収束させられることが判る。

以上の実施例１〜３が、図１のロボット２と同様の構成を備えるロボットを用いてシミュレーションを行った結果である。続いては、図４のロボット２と同様の構成を備えるロボットを用いて、ロボット２に対して設定された基準点ｐを移動先ｐdまで移動させるシミュレーションを行なった結果を示す。具体的には、カメラの内部パラメータ、カメラの外部パラメータ、カメラの位置、およびロボット２とカメラの間の関係について、いずれもキャリブレーションを行わない状態で、基準点ｐを移動先ｐdへ収束させて、エンドエフェクタ４を目標位置まで移動できることが、このシミュレーションで確認された。

また、このシミュレーションを行うにあたっては、ロボット２を制御するコントローラ５としてIntel Core i7 960（ＣＰＵ）と４GBのＤＲＡＭ(Dynamic Random Access Memory)を用い、カメラＣ1、Ｃ2として２００万画素のカメラを用いることができる。ロボット２のキャリブレーションには誤差を持たせており、具体的には、関節角度および関節角速度それぞれに対して2.0％の誤差を導入し、リンクの長さに対して5.0%の誤差を導入した。なお、この実施例では、上記実施形態における「ｐ」を「ｘ」に変更して表記し、上記実施形態における「ｊ」を「ｉ」に変更して表記している。

実施例４
実施例４では、上記実施形態と同様に、基準点ｐ1（特定基準点）は関節Ｑ3、Ｑ4の間に設定され、基準点ｐ2、ｐ3はエンドエフェクタ４上に設定されている。実施例４の制御則を表す式は次のとおりである。

数４７における各種ゲインはいずれも対角行列であり、それぞれの対角成分は図１４の表に示すとおりである。ここで、図１４は、実施例４でのゲインの設定値を表として示す図である。同図の表では、関節角度ｑ1〜ｑ6それぞれに対応する各ゲインＧ_Ｐ…の対角成分が示されている。ここで、ゲインＧ_Ｄは、第５関節Ｑ5の振動を抑えるために大きく設定されている。また、位置偏差Δｐ1に対する比例ゲイン（＝0.20）が、位置偏差Δｐ2、Δｐ3に対する比例ゲイン（＝0.10）よりも大きく設定され、位置偏差Δｐ1に対する積分ゲイン（＝0.006）が、位置偏差Δｐ2、Δｐ3に対する積分ゲイン（＝0.004）よりも大きく設定されている。

図１５は、実施例４でのシミュレーション結果を示す図である。同図に示すように、位置偏差Δｐ1〜Δｐ3はいずれもゼロに収束しており、エンドエフェクタ４は目標位置に到達している。また、位置偏差Δｐ1に対する各ゲインが、位置偏差Δｐ2、Δｐ3に対する各ゲインよりも大きく設定されていることから、位置偏差Δｐ1の方が、位置偏差Δｐ2、Δｐ3よりも速く収束しているのが判る。

実施例５
実施例５の基準点ｐ1〜ｐ3の設定場所は、実施例４と同様である。一方、実施例５の制御側は、位置偏差Δｐに対する比例制御の項を有さず、数４７において位置偏差Δｐに対する比例ゲインをゼロに設定したものに相当する。具体的なゲインの設定値は次のとおりである。

図１６は、実施例５でのゲインの設定値を表として示す図である。同図の表では、関節角度ｑ1〜ｑ6それぞれに対応する各ゲインＧ_Ｐ…の対角成分が示されている。位置偏差Δｐ2に対する積分ゲイン（＝0.032）が、位置偏差Δｐ1、ｐ3に対する積分ゲイン（＝0.02）よりも大きく設定されている。

図１７は、実施例５でのシミュレーション結果を示す図である。同図に示すように、位置偏差Δｐ1〜Δｐ3はいずれもゼロに収束しており、エンドエフェクタ４は目標位置に到達可能となっている。

実施例６
実施例６では、基準点ｐ1〜ｐ3の全てがエンドエフェクタ４上に設定されている。実施例６の制御側は、実施例４で示した数４７と同様である。具体的なゲインの設定値は次のとおりである。

図１８は、実施例６でのゲインの設定値を表として示す図である。同図の表では、関節角度ｑ1〜ｑ6それぞれに対応する各ゲインＧ_Ｐ…の対角成分が示されている。ここで、ゲインＧ_Ｄは、第５関節Ｑ5の振動を抑えるために大きく設定されている。

図１９は、実施例６でのシミュレーション結果を示す図である。同図に示すように、位置偏差Δｐ1〜Δｐ3はいずれもゼロに収束しており、エンドエフェクタ４は目標位置に到達可能となっている。

実施例７
実施例７では、基準点ｐ1〜ｐ3の全てがエンドエフェクタ４上に設定されている。実施例７の制御側は、数４７と同様である。ただし、基準点ｐおよび移動先ｐdの検出は３台のカメラで実行され、カメラの座標系から作業座標系への座標変換は次式に基づいて実行される。

図２０は、実施例７でのゲインの設定値を表として示す図である。同図の表では、関節角度ｑ1〜ｑ6それぞれに対応する各ゲインＧ_Ｐ…の対角成分が示されている。図２１は、実施例７でのシミュレーション結果を示す図である。同図に示すように、位置偏差Δｐ1〜Δｐ3はいずれもゼロに収束しており、エンドエフェクタ４は目標位置に到達可能となっている。

実施例８
実施例８では、５点の基準点ｐ1〜ｐ5の全てがエンドエフェクタ４上に設定されている。実施例８の制御側は、次のとおりである。

また、基準点ｐおよび移動先ｐdの検出は３台のカメラで実行され、カメラの座標系から作業座標系への座標変換は次式に基づいて実行される。

図２２は、実施例８でのゲインの設定値を表として示す図である。同図の表では、関節角度ｑ1〜ｑ6それぞれに対応する各ゲインＧ_Ｐ…の対角成分が示されている。図２３は、実施例８でのシミュレーション結果を示す図である。同図に示すように、５点の基準点ｐ1〜ｐ5の位置偏差Δｐ1〜Δｐ5はいずれもゼロに収束しており、エンドエフェクタ４は目標位置に到達可能となっている。

この発明は、回転自在な関節を有して当該関節の回転に応じてエンドエフェクタを移動させるロボットに関する技術に好適に適用することができ、例えば、当該ロボットを用いて部品を組み立てたり、箱詰めを行ったりする場合に適用することができる。

１…ロボットシステム、 ２…ロボット、 ３…アーム、 ３０…リンク、 ４…エンドエフェクタ、 ５…コントローラ、 ６…記録媒体、 ７…プログラム、 Ｃ1…カメラ、 Ｃ2…カメラ、 Ｃi…カメラ、 Ｌ1…リンク、 Ｌ2…リンク、 Ｌ3…リンク、 Ｑ1〜Ｑ6…関節、 Ｑ…関節、 Ｑn…関節、 Ｅn…エンコーダ、 ｐ…位置（基準点）、 ｐd…目標位置（移動先）、 Δｐ…位置偏差、 ｑ…回転角度、 ｑd…目標角度、 Δｑ…角度偏差、 τ…トルク、 Ｌq…内界ループ、 Ｌx…外界ループ

Claims (15)

1. 変位自在な関節を有して前記関節の変位に応じてエンドエフェクタを移動させるロボットを制御するロボット制御装置において、
   前記ロボットに設けられた基準箇所と前記基準箇所の移動先を視野に捉えつつ前記基準箇所を撮像する撮像部と、
   前記関節の変位量を検出する変位量検出部と、
   前記撮像部の撮像結果に基づいて前記移動先との間の位置偏差を前記基準箇所について求める位置偏差取得部と、
   前記関節を変位させるために前記関節に印加する力あるいはトルクである印加量として、前記位置偏差に応じた値に積分演算を行った積分値を前記基準箇所について求めた結果から第１印加量を算出するとともに、前記基準箇所が対応する前記移動先に一致するときの前記関節の前記変位量である目標変位量と前記変位量検出部の検出変位量との変位量偏差に応じた値に比例演算を行って第２印加量を算出する印加量算出部と、
   前記第１印加量および前記第２印加量を前記関節に加えて、前記基準箇所をその前記移動先へ向けて移動させる駆動制御部と
   を備え、
   前記駆動制御部は、前記変位量偏差に基づく積分制御は、前記関節に対して実行しないロボット制御装置。
2. 前記関節の回転に応じて前記エンドエフェクタを移動させる前記ロボットを制御する請求項１に記載のロボット制御装置において、
   前記撮像部は、前記エンドエフェクタに設けられた前記基準箇所と前記基準箇所の前記移動先を捉えつつ前記基準箇所を撮像し、
   前記変位量検出部は、前記関節の回転角度を検出する角度検出部であり、
   前記印加量算出部は、前記位置偏差に応じた値に積分演算を行って第１トルクを前記第１印加量として算出するとともに、前記基準箇所が前記移動先にあるときの前記関節の前記回転角度である前記目標変位量としての目標角度と前記角度検出部の検出角度との角度偏差に応じた値に比例演算を行って第２トルクを前記第２印加量として算出するトルク算出部であり、
   前記駆動制御部は、前記第１トルクおよび前記第２トルクが前記関節に加わるように、前記関節を駆動する関節駆動機構を制御するロボット制御装置。
3. 前記トルク算出部が前記位置偏差に応じた値に比例演算を行って算出したトルクが前記関節にさらに加わるように、前記駆動制御部は前記関節駆動機構を制御する請求項２に記載のロボット制御装置。
4. 前記トルク算出部は、転置ヤコビ行列が乗ぜられた前記位置偏差に対して積分演算を行って前記第１トルクを算出する請求項２または３に記載のロボット制御装置。
5. 前記関節を連結して６以上の自由度を持ったアームの一端に前記エンドエフェクタを取り付けた前記ロボットを制御する請求項１に記載のロボット制御装置において、
   前記撮像部は、前記関節のうちの最も一端側の先端関節よりも前記エンドエフェクタ側に対して設定された少なくとも２箇所を含む、前記ロボットに対して設定された３箇所以上の前記基準箇所それぞれの前記移動先を視野に捉えつつ、前記基準箇所を撮像し、
   前記位置偏差取得部は、前記位置偏差を前記各基準箇所について求め、
   前記印加量算出部は、前記各関節について前記第１印加量および前記第２印加量を算出し、
   前記駆動制御部は、前記第１印加量および前記第２印加量を前記関節に加えて、前記３箇所以上の基準箇所をそれぞれの前記移動先へ向けて移動させることで、三次元での前記エンドエフェクタの位置および姿勢を制御するロボット制御装置。
6. 前記駆動制御部は、前記先端関節あるいは前記先端関節よりも前記一端側の逆側である他端側に対して特定基準箇所として設定された前記基準箇所を含む前記３箇所以上の基準箇所のそれぞれを、対応する前記移動先へ向けて移動させる請求項５に記載のロボット制御装置。
7. 前記複数の関節のうち前記一端側から順番に前記関節の自由度を数えた総計が３以上となる特定関節あるいは当該特定関節より他端側に前記特定基準箇所が設定されている請求項６に記載のロボット制御装置。
8. 前記複数の関節のうち前記他端側から順番に前記関節の自由度を数えた総計が３以上となる前記関節よりも前記一端側に前記特定基準箇所が設定されている請求項７に記載のロボット制御装置。
9. 前記印加量算出部は、前記特定基準箇所についての前記積分値の重み係数が前記先端関節よりも前記エンドエフェクタ側に対して設定された前記基準箇所についての前記積分値の重み係数よりも大きい加重加算を前記各基準箇所の前記積分値に行って、前記第１印加量を算出する請求項７または８に記載のロボット制御装置。
10. 前記印加量算出部は、前記位置偏差に応じた値に比例演算を行った比例値を前記各基準箇所について求めた結果から第３印加量を算出し、前記駆動制御部は、前記第３印加量を前記関節にさらに加える請求項５ないし９のいずれか一項に記載のロボット制御装置。
11. 前記印加量算出部は、転置ヤコビ行列が乗ぜられた前記位置偏差に対して積分演算を行って前記積分値を求めて、前記第１印加量を算出する請求項５ないし１０のいずれか一項に記載のロボット制御装置。
12. 変位自在な関節を有して前記関節の変位に応じてエンドエフェクタを移動させるロボットを制御するロボット制御方法において、
    前記ロボットに設けられた基準箇所と前記基準箇所の移動先を視野に捉えつつ前記基準箇所を撮像する工程と、
    前記基準箇所を撮像した結果に基づいて前記移動先との間の位置偏差を前記基準箇所について求める工程と、
    前記関節の変位量を検出して検出変位量を求める工程と、
    前記関節を変位させるために前記関節に印加する力あるいはトルクである印加量として、前記位置偏差に応じた値に積分演算を行った積分値を前記基準箇所について求めた結果から第１印加量を算出するとともに、前記基準箇所が対応する前記移動先に一致するときの前記関節の変位量である目標変位量と前記検出変位量との変位量偏差に応じた値に比例演算を行って第２印加量を算出する工程と、
    前記第１印加量および前記第２印加量を前記関節に加えて、前記基準箇所をその前記移動先へ向けて移動させる工程と
    を備え、
    前記変位量偏差に基づく積分制御は、前記関節に対して実行しないロボット制御方法。
13. 変位自在な関節を有して前記関節の変位に応じてエンドエフェクタを移動させるロボットをコンピュータに制御させるロボット制御プログラムにおいて、
    前記ロボットに設けられた基準箇所と前記基準箇所の移動先を視野に捉えつつ前記基準箇所を撮像する工程と、
    前記基準箇所を撮像した結果に基づいて前記移動先との間の位置偏差を前記基準箇所について求める工程と、
    前記関節の変位量を検出して検出変位量を求める工程と、
    前記関節を変位させるために前記関節に印加する力あるいはトルクである印加量として、前記位置偏差に応じた値に積分演算を行った積分値を前記基準箇所について求めた結果から第１印加量を算出するとともに、前記基準箇所が対応する前記移動先に一致するときの前記関節の変位量である目標変位量と前記検出変位量との変位量偏差に応じた値に比例演算を行って第２印加量を算出する工程と、
    前記第１印加量および前記第２印加量を前記関節に加えて、前記基準箇所をその前記移動先へ向けて移動させる工程と
    を、前記コンピュータに実行させ、
    前記関節に対する前記変位量偏差に基づく積分制御は、前記コンピュータに実行させないプログラム。
14. 請求項１３に記載のプログラムをコンピュータにより読み出し可能に記録した記録媒体。
15. 変位自在な関節を有して前記関節の変位に応じてエンドエフェクタを移動させるロボットと、
    前記ロボットに設けられた基準箇所と前記基準箇所の移動先を視野に捉えつつ前記基準箇所を撮像する撮像部と、
    前記関節の変位量を検出する変位量検出部と、
    前記撮像部の撮像結果に基づいて前記移動先との間の位置偏差を前記基準箇所について求める位置偏差取得部と、
    前記関節を変位させるために前記関節に印加する力あるいはトルクである印加量として、前記位置偏差に応じた値に積分演算を行った積分値を前記基準箇所について求めた結果から第１印加量を算出するとともに、前記基準箇所が対応する前記移動先に一致するときの前記関節の前記変位量である目標変位量と前記変位量検出部の検出変位量との変位量偏差に応じた値に比例演算を行って第２印加量を算出する印加量算出部と、
    前記第１印加量および前記第２印加量を前記関節に加えて、前記基準箇所をその前記移動先へ向けて移動させる駆動制御部と
    を備え、
    前記駆動制御部は、前記変位量偏差に基づく積分制御は、前記関節に対して実行しないロボットシステム。

Images