JP5381039B2 - 多関節マニピュレータの先端位置制御方法および多関節マニピュレータ - Google Patents

Description

本発明は、産業用ロボット、特に据付型産業用ロボットの多関節ロボットアームとして用いられる多関節マニピュレータの先端位置制御方法および多関節マニピュレータに関する。

産業用ロボットの多関節ロボットアームとして用いられる多関節マニピュレータは、一般に、鉛直または水平な軸回りに回転する複数の関節軸と、これらの関節軸からその径方向に延伸する複数のアームとを備えた構成となっている。
このような多関節マニピュレータの先端を所定の位置に位置制御する方法としては、各関節軸の軸トルクをアームのリスト部に装着された力覚センサにより検出すると共に、各関節軸の角度を関節角度センサにより検出して、多関節マニピュレータの先端を位置制御する方法が知られている（特許文献１参照）。
特開平１−２８９６８８号公報

しかしながら、関節軸の軸トルクをアームのリスト部に装着された力覚センサにより検出しようとすると、アームが鉛直方向に回動動作した場合には力覚センサが重力の影響を受けてしまうため、多関節マニピュレータの先端を精度よく位置制御することは困難である。
本発明は上述した問題点に鑑みてなされたもので、多関節マニピュレータの先端を所定の位置に精度よく位置制御することのできる多関節マニピュレータの先端位置制御方法を提供することを目的とするものである。また、本発明は各アームの先端を所定の位置に精度よく位置制御することのできる多関節マニピュレータを提供することを目的とするものである。

上記課題を解決するため、本発明の一態様による多関節マニピュレータの先端位置制御方法は、軸回りに回転する第１関節軸および第２関節軸を含む複数の関節軸と、前記第１関節軸から前記第１関節軸の径方向に延伸する第１アームおよび前記第２関節軸から前記第２関節軸の径方向に延伸する第２アームを含む複数のアームとを備えた多関節マニピュレータの先端を位置制御する方法であって、
前記第１関節軸の回転角度を目標角度に回転させる第１目標位置指令信号に、前記第１アームに装着された第１ジャイロセンサの出力から得られる前記第１関節軸の回転角度をフィードバックし、前記第１アームの先端を目標速度とするために前記第１関節軸を回転させる第１目標速度指令信号に、前記第１ジャイロセンサの出力から得られる前記第１関節軸の回転速度をフィードバックして前記第１関節軸の回転を制御するとともに、前記第２関節軸の回転角度を目標角度に回転させる信号と前記第１目標位置指令信号とを加算した信号である第２目標位置指令信号に、前記第２アームに装着された第２ジャイロセンサの出力から得られる前記第２関節軸の回転角度をフィードバックし、前記第２関節軸の回転速度を目標速度で回転させる信号と前記第１目標速度指令信号とを加算した信号である第２目標速度指令信号に、前記第２ジャイロセンサの出力から得られる前記第２関節軸の回転速度をフィードバックして前記第２関節軸の回転を制御することによって、前記多関節マニピュレータの先端を位置制御することを特徴とするものである。

本発明の一態様による多関節マニピュレータの先端位置制御方法によると、アームの角速度を検出する手段して加速度センサを用いた場合のようにアームが鉛直方向に回動動作しても重力の影響を受けることがないので、多関節マニピュレータの先端を所定の位置に精度よく位置制御することができる。
また、関節軸の回転位置をエンコーダやレゾルバ等の位置検出器の出力から求める必要がなく、位置検出器が不要となるので、多関節マニピュレータの先端を比較的簡単な構成により位置制御することができる。

本発明の一態様による多関節マニピュレータは、軸回りに回転する第１関節軸および第２関節軸を含む複数の関節軸と、前記第１関節軸から前記第１関節軸の径方向に延伸する第１アームおよび前記第２関節軸から前記第２関節軸の径方向に延伸する第２アームを含む複数のアームとを備えた多関節マニピュレータであって、
前記アームに装着された複数のジャイロセンサと、
前記多関節マニピュレータの先端を位置制御する位置制御手段と、を設け、
前記位置制御手段は、前記第１関節軸の回転角度を目標角度に回転させる第１目標位置指令信号に、前記第１アームに装着された第１ジャイロセンサの出力から得られる前記第１関節軸の回転角度をフィードバックし、前記第１アームの先端を目標速度とするために前記第１関節軸を回転させる第１目標速度指令信号に、前記第１ジャイロセンサの出力から得られる前記第１関節軸の回転速度をフィードバックして前記第１関節軸の回転を制御するとともに、前記第２関節軸の回転角度を目標角度に回転させる信号と前記第１目標位置指令信号とを加算した信号である第２目標位置指令信号に、前記第２アームに装着された第２ジャイロセンサの出力から得られる前記第２関節軸の回転角度をフィードバックし、前記第２関節軸の回転速度を目標速度で回転させる信号と前記第１目標速度指令信号とを加算した信号である第２目標速度指令信号に、前記第２ジャイロセンサの出力から得られる前記第２関節軸の回転速度をフィードバックして前記第２関節軸の回転を制御することを特徴とするものである。

本発明の一態様による多関節マニピュレータは、アームの角速度を検出する手段して加速度センサを用いた場合のようにアームが鉛直方向に回動動作しても重力の影響を受けることがないので、各アームの先端位置を所定の位置に精度よく位置制御することができる。
また、関節軸の回転位置をエンコーダやレゾルバ等の位置検出器の出力から求める必要がなく、位置検出器が不要となるので、各アームの先端を比較的簡単な構成により位置制御することができる。
本発明の一態様による多関節マニピュレータは、固定基台と、
前記固定基台に対して回転する第１関節軸を介して前記固定基台に連結された第１アームと、
前記第１関節軸を回転させる第１関節軸駆動モータと、
前記第１アームに装着された第１ジャイロセンサと、
前記第１アームに対して回転する第２関節軸を介して前記第１アームに連結された第２アームと、
前記第２関節軸を回転させる第２関節軸駆動モータと、
前記第２アームに装着された第２ジャイロセンサと、
前記第１関節軸駆動モータおよび前記第２関節軸駆動モータの駆動を制御して、前記第１アームおよび前記第２アームの先端位置をそれぞれ位置制御する位置制御手段と、を有し、
位置制御手段は、前記第１関節軸の回転角度を目標角度に回転させ、前記第１関節軸駆動モータに供給する第１目標位置指令信号に、前記第１ジャイロセンサの出力から得られる前記第１関節軸の回転角度をフィードバックし、前記第１アームの先端を目標速度とするために前記第１関節軸駆動モータに供給する第１目標速度指令信号に、前記第１ジャイロセンサの出力から得られる前記第１関節軸の回転速度をフィードバックして前記第１関節軸の回転を制御するとともに、前記第２関節軸の回転角度を目標角度に回転させる信号と前記第１目標位置指令信号とを加算した信号であり、前記第２関節軸駆動モータに供給する第２目標位置指令信号に、前記第２ジャイロセンサの出力から得られる前記第２関節軸の回転角度をフィードバックし、前記第２関節軸の回転速度を目標速度で回転させる信号と前記第１目標速度指令信号とを加算した信号であり、前記第２関節軸駆動モータに供給する第２目標速度指令信号に、前記第２ジャイロセンサの出力から得られる前記第２関節軸の回転速度をフィードバックして前記第２関節軸の回転を制御することを特徴とするものである。

以下、添付図面を参照して本発明の実施形態について説明する。
本発明に係る多関節マニピュレータの先端位置制御方法が適用される産業用ロボットの一例を図１及び図２に示す。図１及び図２に示される産業用ロボットは、アーム１，３、関節軸２，４、固定基台６、関節軸駆動モータ７，８およびジャイロセンサ９，１０を備えている。
アーム（第２アーム）１は先端部を有しており、このアーム１の先端部と反対側の端部に、鉛直な軸回りに回転する関節軸（第２関節軸）２が設けられている。
アーム（第１アーム）３は関節軸２を介してアーム１に連結された先端部を有しており、このアーム３の先端部と反対側の端部に、鉛直な軸回りに回転する関節軸（第１関節軸）４が設けられている。
固定基台６はアーム１，３および関節軸２，４から形成される多関節マニピュレータ５を図示しない固定物に据え付けるためのものであり、この固定基台６の図中上端部に関節軸４が鉛直に設けられている。

関節軸駆動モータ（第２関節軸駆動モータ）７は図示しない減速機構を介して関節軸２を鉛直な軸回りに駆動するものであり、図１及び図２に示される産業用ロボットでは、アーム１の内部に関節軸駆動モータ７が設けられている。
関節軸駆動モータ（第１関節軸駆動モータ）８は図示しない減速機構を介して関節軸４を鉛直な軸回りに駆動するものであり、図１及び図２に示される産業用ロボットでは、固定基台６の内部に関節軸駆動モータ８が設けられている。
ジャイロセンサ９，１０はアーム１，３の角速度（関節軸２，４の回転角度）を検出するためのものであり、これらのジャイロセンサ９，１０はアーム１，３の先端部に装着されている。
なお、アーム１の先端部には中空軸１１が軸回りに回転可能に且つ軸方向に移動可能に設けられ、この中空軸１１の下端部にエンドエフェクタ等が下端部に装着されるようになっている。

多関節マニピュレータ５の先端を位置制御する位置制御回路（位置制御手段）の一例を図３に示す。図３に示される位置制御回路１３は、第１の目標位置指示器１４、第１の積分器１５、第１の減算器１６、第１の位置補償器１７、第１の目標速度指示器１８、第１の加減算器１９、第１の速度補償器２０、第１の電流制御器２１、第２の目標位置指示器２２、第１の加算器２３、第２の積分器２４、第２の減算器２５、第２の位置補償器２６、第２の目標速度指示器２７、第２の加算器２８、第２の加減算器２９、第２の速度補償器３０、第２の電流制御器３１、電力増幅器３２，３３を備えている。
第１の目標位置指示器１４はアーム３の先端目標位置（関節軸４の目標回転角度）を指示するものであり、この第１の目標位置指示器１４で指示された先端目標位置は、目標位置指令信号（第１目標位置指令信号）として第１の減算器１６に供給されるようになっている。

第１の積分器１５はジャイロセンサ（第１ジャイロセンサ）１０の出力を積分してアーム３の実際の先端位置（関節軸４の実際の回転角度）を算出するものであり、この第１の積分器１５で算出されたアーム３の実際の先端位置は、実位置信号として第１の減算器１６に供給されるようになっている。
第１の減算器１６は第１の目標位置指示器１４で指示されたアーム３の先端目標位置と第１の積分器１５で算出されたアーム３の実際の先端位置との位置偏差を算出するものであり、この第１の減算器１６で算出された位置偏差は、位置偏差信号として第１の位置補償器１７に供給されるようになっている。
第１の位置補償器１７は第１の減算器１６で算出された位置偏差を補償するアーム３の位置補償速度を算出するものであり、この第１の位置補償器１７で算出されたアーム３の位置補償速度は、位置補償信号として第１の加減算器１９に供給されるようになっている。

第１の目標速度指示器１８はアーム３の先端目標速度（関節軸４の目標回転速度）を指示するものであり、この第１の目標速度指示器１８で指示されたアーム３の先端目標速度は、目標速度指令信号として第１の加減算器１９に供給されるようになっている。
第１の加減算器１９は第１の位置補償器１７で算出されたアーム３の位置補償速度と第１の目標速度指示器１８で指示されたアーム３の先端目標速度との加算値からジャイロセンサ１０で検出された角速度を差し引いた速度偏差を算出するものであり、この第１の加減算器１９で算出された速度偏差は、速度偏差信号として第１の速度補償器２０に供給されるようになっている。
第１の速度補償器２０は第１の加減算器１９で算出された速度偏差を補償するトルクを算出するものであり、この第１の速度補償器２０で算出されたトルクは、トルク指令信号として第１の電流制御器２１に供給されるようになっている。

第２の目標位置指示器２２はアーム１の先端目標位置（関節軸３の目標回転角度）を指示するものであり、この第２の目標位置指示器２２で指示されたアーム１の先端目標位置は、目標位置信号として第１の加算器２３に供給されるようになっている。
第１の加算器２３は第１の目標位置指示器１４で指示されたアーム３の先端目標位置と第２の目標位置指示器２２で指示されたアーム１の先端目標位置との合計目標位置を算出するものであり、この第１の加算器２３で算出された先端目標位置は、目標位置指令信号（第２目標位置指令信号）として第２の減算器２５に供給されるようになっている。
第２の積分器２４はジャイロセンサ（第２ジャイロセンサ）９の出力を積分してアーム１の実際の先端位置（関節軸２の実際の回転角度）を算出するものであり、この第２の積分器２４で算出されたアーム１の先端位置は、実位置信号として第２の減算器２５に供給されるようになっている。

第２の減算器２５は第１の加算器２３で算出された合計目標位置と第２の積分器２４で算出されたアーム１の先端位置との位置偏差を算出するものであり、この第２の減算器２５で算出された位置偏差は、位置偏差信号として第２の位置補償器２６に供給されるようになっている。
第２の位置補償器２６は第２の減算器２５で算出された位置偏差を補償するアーム１の位置補償速度を算出するものであり、この第２の位置補償器２６で算出されたアーム１の位置補償速度は、位置補償信号として第２の加減算器２９に供給されるようになっている。
第２の目標速度指示器２７はアーム１の先端目標速度（関節軸２の目標回転速度）を指示するものであり、この第２の目標速度指示器２７で指示されたアーム１の先端目標速度は、目標速度信号として第２の加算器２８に供給されるようになっている。

第２の加算器２８は第１の目標速度指示器１８で指示されたアーム３の先端目標速度と第２の目標速度指示器２７で指示されたアーム１の先端目標速度との合計目標速度を算出するものであり、この第２の加算器２８で算出された合計目標速度は、合計目標速度指令信号として第２の加減算器２９に供給されるようになっている。
第２の加減算器２９は第２の位置補償器２６で算出されたアーム１の位置補償速度と第２の加算器２８で算出された合計目標速度との加算値からジャイロセンサ９で検出された角速度を差し引いた速度偏差を算出するものであり、この第２の加減算器２９で算出された速度偏差は、速度偏差信号として第２の速度補償器３０に供給されるようになっている。

第２の速度補償器３０は第２の加減算器２９で算出された速度偏差を補償するトルクを算出するものであり、この第２の速度補償器３０で算出されたトルクは、トルク指令信号として第２の電流制御器３１に供給されるようになっている。
第１の電流制御器２１及び第２の電流制御器３１は電力増幅器３２、３３を介して関節軸駆動モータ７，８を制御するものであり、従って、図３に示される位置制御回路１３は、アーム１，３の各先端部に装着されたジャイロセンサ９，１０の出力から関節軸２，４の回転角度と回転速度を求めて多関節マニピュレータ５の先端を位置制御するように構成されている。

上述のように、アーム１，３の各先端部に装着されたジャイロセンサ９，１０の出力から関節軸２，４の回転角度と回転速度を求めて多関節マニピュレータ５の先端を位置制御すると、アーム１，３の角速度を検出する手段して加速度センサを用いた場合のようにアームが鉛直方向に回動動作しても重力の影響を受けることがないので、多関節マニピュレータ５の先端を所定の位置に精度よく位置制御することができる。
また、関節軸２，４の回転位置をエンコーダやレゾルバ等の位置検出器の出力から求める必要がなく、位置検出器が不要となるので、多関節マニピュレータ５の先端を比較的簡単な構成により位置制御することができる。

なお、本発明に係る多関節マニピュレータの先端位置制御方法が適用される産業用ロボットとして、関節軸２，４が鉛直な軸回りに回転するものを例示したが、これに限られるものではなく、関節軸が水平な軸回りに回転する多関節マニピュレータについても本発明を適用できることは勿論である。
また、多関節マニピュレータ５の先端を位置制御する位置制御回路の構成についても図３に示されるものに限られるものではない。例えば、アームの撓り量を加味して多関節マニピュレータの先端を位置制御してもよいし、多関節マニピュレータ先端の既知位置を撮像装置や近接センサを用いて検出し、検出した既知位置を基に積分器の累積誤差を補正して多関節マニピュレータの先端を位置制御してもよい。

本発明に係る多関節マニピュレータの先端位置制御方法が適用される産業用ロボットの一例を示す斜視図である。 図１に示す産業用ロボットの側面図である。 多関節マニピュレータの先端を位置制御する位置制御回路の一例を示す図である。

符号の説明

１，３…アーム、２，４…関節軸、５…多関節マニピュレータ、６…固定基台、７，８…関節軸駆動モータ、９，１０…ジャイロセンサ、１１…中空軸、１３…位置制御回路、１４…第１の目標位置指示器、１５…第１の積分器、１６…第１の減算器、１７…第１の位置補償器、１８…第１の目標速度指示器、１９…第１の加減算器、２０…第１の速度補償器、２１…第１の電流制御器、２２…第２の目標位置指示器、２３…第１の加算器、２４…第２の積分器、２５…第２の減算器、２６…第２の位置補償器、２７…第２の目標速度指示器、２８…第２の加算器、２９…第２の加減算器、３０…第２の速度補償器、３１…第２の電流制御器、３２、３３…電力増幅器。

Claims (3)

1. 軸回りに回転する第１関節軸および第２関節軸を含む複数の関節軸と、前記第１関節軸から前記第１関節軸の径方向に延伸する第１アームおよび前記第２関節軸から前記第２関節軸の径方向に延伸する第２アームを含む複数のアームとを備えた多関節マニピュレータの先端を位置制御する方法であって、
   前記第１関節軸の回転角度を目標角度に回転させる第１目標位置指令信号に、前記第１アームに装着された第１ジャイロセンサの出力から得られる前記第１関節軸の回転角度をフィードバックし、前記第１アームの先端を目標速度とするために前記第１関節軸を回転させる第１目標速度指令信号に、前記第１ジャイロセンサの出力から得られる前記第１関節軸の回転速度をフィードバックして前記第１関節軸の回転を制御するとともに、前記第２関節軸の回転角度を目標角度に回転させる信号と前記第１目標位置指令信号とを加算した信号である第２目標位置指令信号に、前記第２アームに装着された第２ジャイロセンサの出力から得られる前記第２関節軸の回転角度をフィードバックし、前記第２関節軸の回転速度を目標速度で回転させる信号と前記第１目標速度指令信号とを加算した信号である第２目標速度指令信号に、前記第２ジャイロセンサの出力から得られる前記第２関節軸の回転速度をフィードバックして前記第２関節軸の回転を制御することによって、前記多関節マニピュレータの先端を位置制御することを特徴とする多関節マニピュレータの先端位置制御方法。
2. 軸回りに回転する第１関節軸および第２関節軸を含む複数の関節軸と、前記第１関節軸から前記第１関節軸の径方向に延伸する第１アームおよび前記第２関節軸から前記第２関節軸の径方向に延伸する第２アームを含む複数のアームとを備えた多関節マニピュレータであって、
   前記アームに装着された複数のジャイロセンサと、
   前記多関節マニピュレータの先端を位置制御する位置制御手段と、を設け、
   前記位置制御手段は、前記第１関節軸の回転角度を目標角度に回転させる第１目標位置指令信号に、前記第１アームに装着された第１ジャイロセンサの出力から得られる前記第１関節軸の回転角度をフィードバックし、前記第１アームの先端を目標速度とするために前記第１関節軸を回転させる第１目標速度指令信号に、前記第１ジャイロセンサの出力から得られる前記第１関節軸の回転速度をフィードバックして前記第１関節軸の回転を制御するとともに、前記第２関節軸の回転角度を目標角度に回転させる信号と前記第１目標位置指令信号とを加算した信号である第２目標位置指令信号に、前記第２アームに装着された第２ジャイロセンサの出力から得られる前記第２関節軸の回転角度をフィードバックし、前記第２関節軸の回転速度を目標速度で回転させる信号と前記第１目標速度指令信号とを加算した信号である第２目標速度指令信号に、前記第２ジャイロセンサの出力から得られる前記第２関節軸の回転速度をフィードバックして前記第２関節軸の回転を制御することを特徴とする多関節マニピュレータ。
3. 固定基台と、
   前記固定基台に対して回転する第１関節軸を介して前記固定基台に連結された第１アームと、
   前記第１関節軸を回転させる第１関節軸駆動モータと、
   前記第１アームに装着された第１ジャイロセンサと、
   前記第１アームに対して回転する第２関節軸を介して前記第１アームに連結された第２アームと、
   前記第２関節軸を回転させる第２関節軸駆動モータと、
   前記第２アームに装着された第２ジャイロセンサと、
   前記第１関節軸駆動モータおよび前記第２関節軸駆動モータの駆動を制御して、前記第１アームおよび前記第２アームの先端位置をそれぞれ位置制御する位置制御手段と、を有し、
   位置制御手段は、前記第１関節軸の回転角度を目標角度に回転させ、前記第１関節軸駆動モータに供給する第１目標位置指令信号に、前記第１ジャイロセンサの出力から得られる前記第１関節軸の回転角度をフィードバックし、前記第１アームの先端を目標速度とするために前記第１関節軸駆動モータに供給する第１目標速度指令信号に、前記第１ジャイロセンサの出力から得られる前記第１関節軸の回転速度をフィードバックして前記第１関節軸の回転を制御するとともに、前記第２関節軸の回転角度を目標角度に回転させる信号と前記第１目標位置指令信号とを加算した信号であり、前記第２関節軸駆動モータに供給する第２目標位置指令信号に、前記第２ジャイロセンサの出力から得られる前記第２関節軸の回転角度をフィードバックし、前記第２関節軸の回転速度を目標速度で回転させる信号と前記第１目標速度指令信号とを加算した信号であり、前記第２関節軸駆動モータに供給する第２目標速度指令信号に、前記第２ジャイロセンサの出力から得られる前記第２関節軸の回転速度をフィードバックして前記第２関節軸の回転を制御することを特徴とする多関節マニピュレータ。

Images