ところで、このように加速度センサにより検出された加速度量を用いる手法では、実際に振動が発生するアームの先端近傍に加速度センサを取り付ける必要がある。しかしながら、現実のロボットに適用する場合には、アームの先端近傍に加速度センサを取り付けるスペースや、加速度センサまでの配線の取り回し等が問題となり、実現が困難である。
本発明は、こうした実情に鑑みてなされたものであり、その主たる目的は、ロボットの振動を抑制することができ、且つ現実のロボットへの適用が容易であるロボットの制御装置を提供することにある。
上記課題を解決するための第1の手段は、
駆動部と、前記駆動部により駆動され水平面に沿って所定の軌道上を往復動する移動部と、鉛直方向に沿って往復動自在に前記移動部により支持され下部に物体が取り付けられた所定軸と、を備えるロボットを制御するロボットの制御装置であって、
前記移動部の直近の目標位置を算出する目標位置算出部と、
前記目標位置算出部により算出された前記目標位置が変化する際の加減速度を算出する加減速度算出部と、
前記所定軸及び前記物体の全体の重心と、鉛直方向における前記所定軸の位置とに基づいて、前記移動部が前記所定軸を支持する支持部から前記重心までの距離である重心距離を算出する重心距離算出部と、
前記目標位置算出部により算出された前記目標位置、前記所定軸の曲げに対する前記支持部のばね定数、前記全体の質量、前記加減速度算出部により算出された前記加減速度、及び前記重心距離算出部により算出された前記重心距離に基づいて、前記水平面上へ投影した前記重心の位置を前記目標位置へ近付けるように、前記目標位置を修正した修正目標位置を算出する修正目標位置算出部と、
前記修正目標位置算出部により算出された前記修正目標位置に前記移動部を移動させるように、前記駆動部を駆動する修正指令値を算出する修正指令値算出部と、
前記修正指令値算出部により算出された前記修正指令値に基づいて、前記駆動部を制御する駆動制御部と、
を備える。
上記構成によれば、ロボットは、駆動部と移動部と所定軸とを備えている。移動部は、駆動部により駆動され、水平面に沿って所定の軌道上を往復動する。所定軸は、鉛直方向に沿って往復動自在に移動部により支持されており、下部に物体が取り付けられている。
目標位置算出部により、移動部の直近の目標位置が算出される。加減速度算出部によって、目標位置算出部により算出された目標位置が変化する際の加減速度が算出される。
移動部が加減速する際には、移動部が所定軸を支持する支持部から所定軸及び物体の全体の重心までの距離である重心距離の位置に、この全体の質量がある場合と同様の慣性力が作用する。ここで、所定軸は、鉛直方向に沿って往復動自在に支持されているため、所定軸の動作状態に応じて重心距離が変化し、慣性力の作用する高さが変化することとなる。この点、重心距離算出部により、上記全体の重心と鉛直方向における所定軸の位置とに基づいて、上記重心距離が算出される。
上記慣性力により、所定軸の曲げに対する支持部のばね定数に応じて、所定の水平面上へ投影した上記重心の位置が目標位置から変位する。所定の水平面上へ投影した上記重心の位置と目標位置とがずれると、このずれが解消される際に所定軸に振動が発生する。この慣性力は、上記全体の質量と、目標位置の加減速度とに基づいて算出することができる。さらに、所定の水平面上へ投影した上記重心の位置が目標位置から変位する量は、上記慣性力と上記重心距離と上記ばね定数とに基づいて算出することができる。
したがって、修正目標位置算出部は、上記目標位置、上記ばね定数、上記全体の質量、目標位置が変化する際の加減速度、及び上記重心距離に基づいて、所定の水平面上へ投影した上記重心の位置を上記目標位置へ近付けるように、上記目標位置を修正した修正目標位置を算出する。すなわち、所定の水平面上へ投影した上記重心の位置と目標位置とのずれを縮小するように、目標位置を修正した修正目標位置が算出される。
修正指令値算出部は、修正目標位置算出部により算出された修正目標位置に移動部を移動させるように、駆動部を駆動する修正指令値を算出する。そして、駆動制御部は、算出された修正指令値に基づいて駆動部を制御する。このため、所定の水平面上へ投影した上記重心の位置と目標位置とのずれが解消される際に、所定軸に振動が発生することを抑制することができる。さらに、実際に振動が発生する部分に加速度センサを取り付ける必要がなく、目標位置と既知の値と目標位置の加減速度と重心距離とに基づいて修正目標位置、ひいては修正指令値を算出することができる。したがって、現実のロボットに容易に適用することができる。
第2の手段では、前記修正目標位置算出部は、前記移動部の加速時において、前記目標位置よりも前記移動部を進ませるように前記修正目標位置を算出する。
移動部が加速する際には、所定軸及び物体の全体に移動部の移動方向と反対方向の慣性力が作用する。このため、所定の水平面上へ投影した上記全体の重心は、移動部よりも遅れて移動する。この点、上記構成によれば、修正目標位置算出部は、移動部の加速時において、目標位置よりも移動部を進ませるように修正目標位置を算出する。したがって、所定の水平面上へ投影した上記重心の位置と目標位置とのずれを縮小するように、修正目標位置を算出することができる。
第3の手段では、前記修正目標位置算出部は、前記移動部の減速時において、前記目標位置よりも前記移動部を遅らせるように前記修正目標位置を算出する。
移動部が減速する際には、所定軸及び物体の全体に移動部の移動方向と同方向の慣性力が作用する。このため、所定の水平面上へ投影した上記全体の重心は、移動部よりも進んで移動する。この点、上記構成によれば、修正目標位置算出部は、移動部の減速時において、目標位置よりも移動部を遅らせるように修正目標位置を算出する。したがって、所定の水平面上へ投影した上記重心の位置と目標位置とのずれを縮小するように、修正目標位置を算出することができる。
第4の手段では、前記修正目標位置算出部は、前記水平面上へ投影した前記重心の位置を前記目標位置に一致させるように前記修正目標位置を算出する。
上記構成によれば、修正目標位置算出部は、所定の水平面上へ投影した重心の位置を目標位置に一致させるように修正目標位置を算出する。このため、所定の水平面上へ投影した上記重心の位置と目標位置とのずれを最小にすることができ、所定軸に振動が発生することを更に抑制することができる。
第5の手段では、前記修正目標位置算出部は、算出した前記修正目標位置をなました目標位置に前記修正目標位置を変更する。
修正指令値が急激に変化した場合、修正指令値に基づき制御される駆動部に、駆動部が発生し得る上限トルクよりも大きなトルクが要求されるおそれがある。この点、上記構成によれば、修正目標位置算出部は、算出した修正目標位置をなました目標位置に修正目標位置を変更する。このため、修正目標位置に移動部を移動させるように駆動部を駆動する修正指令値が、急激に変化することを抑制することができる。そして、駆動制御部は、修正指令値算出部により算出された修正指令値に基づいて、駆動部を制御する。したがって、上限トルクよりも大きなトルクが駆動部に要求されることを抑制することができる。
第6の手段では、前記ばね定数をK、前記全体の質量をM、前記目標位置をP、前記目標位置Pが変化する際の加減速度をa、前記重心距離をL、前記目標位置Pの補正量をΔPとして、前記修正目標位置算出部は、ΔP=M・a・L^2/Kの式により算出した前記補正量ΔPを前記目標位置Pに加えて、前記修正目標位置を算出する。
上記構成によれば、修正目標位置算出部は、ΔP=M・a・L^2/Kの式により算出した補正量ΔPを目標位置Pに加えて、修正目標位置を算出することができる。したがって、修正目標位置ひいては修正指令値を容易に算出することができ、現実のロボットに容易に適用することができる。
第7の手段では、前記ロボットは、前記駆動部としての第2駆動部により駆動される前記移動部としての第2移動部と、第1駆動部により駆動され水平面に沿って所定の第1軌道上を往復動する第1移動部と、を備え、前記第2移動部は、第1移動部により移動可能に支持され、前記第1移動部に対して水平面に沿って前記所定の軌道としての所定の第2軌道上を往復動し、前記目標位置算出部は、前記目標位置としての前記第2移動部の直近の目標位置を算出し、前記修正指令値算出部は、前記修正目標位置算出部により算出された前記修正目標位置に前記第2移動部を移動させるように、前記第1駆動部及び前記第2駆動部をそれぞれ駆動する第1修正指令値及び第2修正指令値を算出し、前記駆動制御部は、前記修正指令値算出部により算出された前記第1修正指令値及び前記第2修正指令値に基づいて、それぞれ前記第1駆動部及び前記第2駆動部を制御する。
上記構成によれば、ロボットは、駆動部としての第2駆動部により駆動される上記移動部としての第2移動部と、第1駆動部により駆動され水平面に沿って所定の第1軌道上を往復動する第1移動部と、を備えている。そして、第2移動部は、第1移動部により移動可能に支持され、第1移動部に対して水平面に沿って所定の軌道としての所定の第2軌道上を往復動する。
そして、目標位置算出部により、上記目標位置としての第2移動部の直近の目標位置が算出される。修正指令値算出部は、修正目標位置算出部により算出された修正目標位置に第2移動部を移動させるように、第1駆動部及び第2駆動部をそれぞれ駆動する第1修正指令値及び第2修正指令値を算出する。駆動制御部は、修正指令値算出部により算出された第1修正指令値及び第2修正指令値に基づいて、それぞれ第1駆動部及び第2駆動部を制御する。このため、第1移動部と、第1移動部により移動可能に支持された第2移動部とを備えるロボットにおいて、所定軸の振動を抑制することができる。
第8の手段では、前記第1移動部は、所定の中心軸線を中心として水平面に沿って、前記所定の第1軌道としての所定の第1円弧軌道上を往復動し、前記第2移動部は、前記第1移動部を中心として水平面に沿って、前記所定の第2軌道としての所定の第2円弧軌道上を往復動する。
上記構成によれば、ロボットにおいて、第1移動部は、所定の中心軸線を中心として水平面に沿って、所定の第1軌道としての第1円弧軌道上を往復動する。また、第2移動部は、第1移動部を中心として水平面に沿って、所定の第2軌道としての所定の第2円弧軌道上を往復動する。このため、ロボットとして、水平面に沿って回転する第1軸と、第1軸により水平面に沿って回転可能に支持された第2軸と、第2軸の先端部において鉛直方向に沿って往復動する第3軸と、を備える水平多関節ロボットを採用することができる。したがって、水平多関節ロボットにおいて、所定軸としての第3軸の振動を抑制することができる。
第9の手段では、前記修正目標位置算出部は、算出した前記修正目標位置が前記移動部の可動範囲よりも外側である場合に、前記修正目標位置に最も近い前記可動範囲内の位置に前記修正目標位置を変更する。
修正目標位置算出部により算出された修正目標位置が移動部の可動範囲よりも外側である場合、修正目標位置に移動部を移動させるように駆動部を駆動する修正指令値を算出できないおそれがある。その場合、ロボットの制御にエラーが生じ、ロボットの動作が停止するおそれがある。
この点、上記構成によれば、修正目標位置算出部は、算出した修正目標位置が移動部の可動範囲よりも外側である場合に、修正目標位置に最も近い可動範囲内の位置に修正目標位置を変更する。このため、ロボットの制御を継続することができる範囲内で、所定軸の振動をできるだけ抑制することができる。
以下、一実施形態について、図面を参照しつつ説明する。本実施形態は、機械組立工場などの組立ラインにて用いられる産業用ロボットに具体化している。
図1に示すように、水平多関節ロボット10は、基台20、第1軸21、第2軸22、第3軸23等を備えている。
基台20は、床に固定されている。基台20には、ベアリング(軸受け)等を介して、第1軸21が水平回転自在に連結されている。第1軸21は、軸線C1(所定の中心軸線に相当)を中心として水平回転自在となっている。なお、第1軸21の先端部により、第1移動部(移動部)が構成されている。そして、第1移動部は、軸線C1を中心として水平面に沿って所定の第1円弧軌道上を往復動する。
第1軸21の先端部には、ベアリング等を介して、第2軸22が水平回転自在に連結されている。第2軸22は、軸線C2を中心として水平回転自在となっている。なお、第2軸22の先端部により、第2移動部が構成されている。そして、第2移動部は、第1軸21の先端部(第1移動部)により移動可能に支持され、第1移動部を中心として水平面に沿って所定の第2円弧軌道(所定の円弧軌道、所定の軌道に相当)上を往復動する。
第2軸22の先端部には、第3軸23が上下動自在に連結されている。第3軸23(所定軸に相当)は、第2軸22の先端部(第2移動部)により、軸線C3(鉛直方向)に沿って往復動自在に支持されている。また、第3軸23は、軸線C3を中心として水平回転自在となっている。第3軸23の下部には、所定の動作を行うツールやワーク(部品)等に相当する物体Wが取り付けられている。
基台20には、第1軸21を水平回転駆動する第1駆動部31が固定されている。第1駆動部31(駆動部に相当)は、モータ、伝達機構、減速機構等を備えている。モータの出力軸は、伝達機構及び減速機構を介して第1軸21の基端部に接続されている。減速機構は、基台20に固定されている。
第1軸21には、第2軸22を水平回転駆動する第2駆動部32が固定されている。第2駆動部32(駆動部に相当)は、モータ、伝達機構、減速機構等を備えている。モータの出力軸は、伝達機構及び減速機構を介して第2軸22の基端部に接続されている。
第2軸22には、第3軸23を垂直往復駆動する第3駆動部33が固定されている。第3駆動部33は、モータ、伝達機構、減速機構等を備えている。モータの出力軸は、伝達機構及び減速機構を介して第3軸23に接続されている。伝達機構及び減速機構は、ベルト、プーリ、ボールねじナット等を備えている。
第2軸22には、第3軸23を水平回転駆動する第4駆動部が固定されている。第4駆動部(図示略)は、モータ、伝達機構、減速機構等を備えている。モータの出力軸は、伝達機構及び減速機構を介して第3軸23に接続されている。
こうした構成によれば、第1駆動部31によって、第1軸21が水平回転駆動される。第1軸21は、ベアリング等により支持されて水平回転させられる。第2駆動部32によって、第2軸22が水平回転駆動される。第2軸22は、ベアリング等により支持されて水平回転させられる。第3駆動部33によって、第3軸23が垂直往復動駆動される。第4駆動部によって、第3軸23が水平回転駆動される。
ロボット10の動作は、ロボットコントローラ50により制御される。コントローラ50(制御装置に相当)は、CPU、ROM、RAM、駆動回路、及び位置検出回路等を備えている。ROMは、ロボット10のシステムプログラムや動作プログラム等を記憶している。RAMは、これらのプログラムを実行する際にパラメータの値等を記憶する。位置検出回路には、ロボット10の各関節に設けられた各エンコーダ(図示略)の検出信号が、それぞれ入力される。位置検出回路は、各エンコーダの検出信号に基づいて、各関節に設けられたモータの回転角度位置を検出する。こうしたコントローラ50により、軌道生成部51、修正処理部52、及びサーボ機構53が構成されている。
コントローラ50は、ロボット10の動作プログラム等に基づいて、第2軸22の先端部の目標停止位置を算出する。そして、コントローラ50は、目標停止位置に基づいて、各軸の目標停止角度位置を算出する。軌道生成部51は、各軸の目標停止角度位置に基づいて、各軸の角度位置を目標停止角度位置まで変更する際の各軸の角速度パターンを生成する。軌道生成部51は、各軸の角速度パターンに基づいて、各軸の直近の目標角度位置である指令値θanを算出する。サーボ機構53(駆動制御部に相当)は、各エンコーダにより検出された各軸の角度位置θnと指令値θanとに基づいて、各軸の角度位置を指令値θanとするように、各駆動部31〜33を各駆動トルクTnで制御する。なお、修正処理部52については後述する。
ところで、第1軸21が水平回転駆動されたり、第2軸22が水平回転駆動されたりした場合に、第3軸23の先端が振動する場合がある。図2は、第3軸23の振動状態を示す模式図である。
第3軸23は第2軸22の支持部22aにより、軸線C3に沿って往復動自在に支持されている。支持部22a(第2移動部、移動部に相当)は、第3駆動部33の伝達機構及び減速機構等により構成されており、第2軸22のフレーム(被駆動部)や第3軸23と比べて剛性が低い。このため、支持部22aは、第3軸23を曲げる方向の力に対して、ばね定数Kのばね要素として作用する。したがって、第2軸22の先端部が加減速する際に、第3軸23及び物体Wに作用する慣性力により、支持部22aを中心として第3軸23の先端が振動することとなる。特に、物体Wの質量が大きいほど、支持部22aから物体Wまでの距離が長いほど、この振動が大きくなる。
そこで、本実施形態では、コントローラ50は、ロボット10を図3に示す動作モデルとして捉え、第3軸23の振動を抑制する制振制御を実行する。同図に示すように、ロボット10の水平回転軸である第1軸21及び第2軸22は、各軸21,22を支持する状態が変化しない固定モデルとする。これに対して、ロボット10の垂直往復動軸である第3軸23は、支持部22aにより第3軸23が支持される位置が変化する可変モデルとする。また、第3軸23と物体Wとを足し合わせて、第3軸23及び物体Wの全体WAとする。そして、全体WAの重心に、全体WAの質量Mが存在するものとして扱う。
図4は、第2軸22の動作時において、比較例及び本実施形態における目標位置と重心位置との関係を示す模式図である。図4(a)は、第1軸21及び第2軸22の指令値θa1及び指令値θa2に対応する破線の目標位置に、第2軸22の支持部22a(先端部)の位置を合わせるように制御した比較例を示している。図4(b)は、第1軸21及び第2軸22の指令値θa1及び指令値θa2に対応する破線の目標位置に、水平面H上に投影した全体WAの重心の位置を合わせるように制御した本実施形態を示している。
図4(a)に示すように、比較例では、支持部22aが加速度aで加速する際に、全体WAに支持部22aの移動方向と反対方向の慣性力−Maが作用する。このため、支持部22aが弾性変形し、水平面H上へ投影した全体WAの重心は、支持部22aよりも遅れて移動する。そして、支持部22aが等速時へ移行する際に、慣性力−Maが小さくなり、支持部22aの弾性変形が元に戻る。支持部22aの弾性変形が元に戻る際に、第3軸23に振動が発生する。
その後、支持部22aが減速度−aで減速する際に、全体WAに支持部22aの移動方向と同方向の慣性力Maが作用する。このため、支持部22aが弾性変形し、水平面H上へ投影した全体WAの重心は、支持部22aよりも進んで移動する。そして、支持部22aの移動が終了する際に、慣性力Maが小さくなり、支持部22aの弾性変形が元に戻る。支持部22aの弾性変形が元に戻る際に、第3軸23に振動が発生する。
図4(b)に示すように、本実施形態でも、支持部22aが加速度aで加速する際に、全体WAに支持部22aの移動方向と反対方向の慣性力−Maが作用する。ここで、本実施形態では、支持部22aの加速時において、破線の目標位置よりも支持部22aを進ませる。詳しくは、支持部22aが弾性変形する量を考慮して、破線の目標位置に、水平面H上に投影した全体WAの重心の位置を合わせる。そして、支持部22aが等速時へ移行する際に、慣性力−Maが小さくなり、支持部22aの弾性変形が元に戻るのに合わせて、目標位置に対する支持部22aの進み量を減少させる。このため、支持部22aの弾性変形が元に戻る際に、第3軸23に振動が発生することが抑制される。
その後、支持部22aが減速度−aで減速する際に、全体WAに支持部22aの移動方向と同方向の慣性力Maが作用する。ここで、本実施形態では、支持部22aの減速時において、破線の目標位置よりも支持部22aを遅らせる。詳しくは、支持部22aが弾性変形する量を考慮して、破線の目標位置に、水平面H上に投影した全体WAの重心の位置を合わせる。そして、支持部22aの移動が終了する際に、慣性力Maが小さくなり、支持部22aの弾性変形が元に戻るのに合わせて、目標位置に対する支持部22aの遅れ量を減少させる。このため、支持部22aの弾性変形が元に戻る際に、第3軸23に振動が発生することが抑制される。
図5に示すように、第2軸22の先端部が加減速する際には、第2軸22が第3軸23を支持する支持部22aから第3軸23及び物体Wの全体WAの重心Cwまでの距離である重心距離L3の位置に、この全体WAの質量Mがある場合と同様の慣性力が作用する。ここで、第3軸23は、鉛直方向に沿って往復動自在に支持されているため、第3軸23の動作状態に応じて重心距離L3が変化し、慣性力の作用する高さが変化することとなる。そこで、本実施形態では、修正処理部52(重心距離算出部に相当)は、全体WAの重心と鉛直方向における第3軸23の位置とに基づいて、重心距離L3を算出する。鉛直方向における第3軸23の位置は、第3軸23を駆動する指令値や、エンコーダの検出信号に基づいて算出することができる。全体WAの重心は、第3軸23の長さ及び質量、物体Wの高さ幅及び質量に基づいて算出することができる。
図6は、第1軸21及び第2軸22の長さ及び角度位置を示す平面図である。同図に示すように、第1軸21の回転中心である軸線C1から、第2軸22の回転中心である軸線C2までの長さはL1である。軸線C2から、第3軸23の中心軸線である軸線C3までの長さはL2である。基台20の正面方向をx軸として、x軸に対して第1軸21の長手方向のなす角度がθ1である。第1軸21の長手方向に対して、第2軸22の長手方向のなす角度がθ2である。
ここで、第1軸21及び第2軸22が水平回転する際に、第2軸22の先端部(軸線C3の位置)の加速度をaとする。第3軸23及び物体Wの全体WAに作用する慣性力Fは、全体WAの質量をMとして、以下の(1)式で表される。
F=M・a ・・・(1)
第2軸22の支持部22aに作用するトルクTは、上記重心距離をL3として、以下の(2)式で表される。
T=F・L3 ・・・(2)
慣性力Fによる全体WAの重心の水平面に沿った変位量Δdは、第3軸23の曲げに対する支持部22aのばね定数をKとして、以下の(3)式で表される。
Δd=L3・T/K ・・・(3)
変位量Δdを相殺するためには、それと等しい補正量ΔPを加えればよい。
(3)式に、上記(1)〜(2)式を適用して整理すると、補正量ΔPは以下の(4)式で表される。L3^2はL3の二乗である。
ΔP=M・L3^2・a/K ・・・(4)
すなわち、補正量ΔPは、既知の定数である質量M及びばね定数K、第2軸22の支持部22aの加減速度a、及び重心距離L3から算出することができる。
図7は、本実施形態における制振制御の手順を示すフローチャートである。この一連の処理は、コントローラ50によって、ロボットの10の全水平回転軸に対して一括して実行される。
まず、軌道生成部51は、全軸の目標停止角度位置を取得する(S20)。続いて、軌道生成部51は、全軸の目標停止角度位置に基づいて、全軸の角速度パターンを生成する(S21)。例えば、図8に破線で示す指令角速度のように、台形の角速度パターンをそれぞれ生成する。続いて、軌道生成部51は、生成した角速度パターンに基づいて、全軸の角度位置の指令値θaを算出する(S22)。指令値θaは、今回の制御周期における各軸の目標角度位置に相当する。
続いて、修正処理部52は、全軸の角度位置の指令値θaに基づいて、第2軸22の先端部(支持部22a)の目標位置Pa(x,y)を算出する(S23)。目標位置Pa(x,y)は、今回の制御周期における目標位置(直近の目標位置)に相当する。
続いて、目標位置Pa(x,y)に基づいて、目標位置Pa(x,y)の加減速度(ax,ay)を算出する(S24)。詳しくは、今回の目標位置Pa(k)のx成分であるx(k)と前回の目標位置Pa(k−1)のx成分であるx(k−1)との差を、加減速度axとする。同様にして、今回の目標位置Pa(k)のy成分であるy(k)と前回の目標位置Pa(k−1)のy成分であるy(k−1)との差を、加減速度ayとする。続いて、修正処理部52は、上述したように、重心距離L3を算出する(S25)。修正処理部52は、上記(4)式により、補正量ΔP(x,y)を算出する(S26)。そして、修正処理部52は、目標位置Pa(x,y)に補正量ΔP(x,y)を加えて、修正目標位置Pb(x,y)を算出する(S27)。
続いて、修正処理部52は、修正目標位置Pb(x,y)にローパスフィルタを掛ける(S28)。すなわち、算出した修正目標位置Pb(x,y)をなました目標位置に修正目標位置Pb(x,y)を変更する。
続いて、修正処理部52は、フィルタ処理後の修正目標位置Pb(x,y)に基づいて、全軸の修正指令値θbを算出する(S39)。修正指令値θb(第1修正指令値、第2修正指令値に相当)は、第2軸22の先端部を修正目標位置Pb(x,y)に移動させる各軸の指令値(目標角度位置)であり、修正目標位置Pb(x,y)に周知の変換処理を行うことにより算出することができる。そして、全軸の修正指令値θbをサーボ機構53に転送する(S30)。
続いて、修正処理部52は、全軸に設けられたエンコーダの検出信号に基づいて、全軸の角度位置が目標停止角度位置に到達したか否か判定する(S31)。この判定において、少なくとも1つの軸の角度位置が目標停止角度位置に到達していないと判定した場合(S31:NO)、S22の処理から再度実行する。一方、この判定において、全軸の角度位置が目標停止角度位置に到達したと判定した場合(S31:YES)、この一連の処理を終了する(END)。
なお、S23の処理が目標位置算出部としての処理に相当し、S24の処理が加減速度算出部としての処理に相当し、S25の処理が重心距離算出部としての処理に相当し、S26〜S28の処理が修正目標位置算出部としての処理に相当し、S29の処理が修正指令値算出部としての処理に相当する。
図8は、比較例及び本実施形態における角速度パターンを示すタイムチャートである。破線は、図7のS21の処理で生成された角速度パターンに相当し、指令角速度を示している。一点鎖線は、生成された角速度パターンに基づきS22の処理で算出された指令値θaを、そのままサーボ機構53に転送した場合に実現される角速度(角速度パターン)を示している。実線は、図7に示す本実施形態の制振制御を実行した場合に実現される角速度(角速度パターン)を示している。
同図に示すように、停止状態からの加速開始時に、本実施形態は比較例よりも角速度の上昇する時期が早くなっている。これにより、本実施形態では、第3軸23及び物体Wの全体WAの重心を水平面上に投影した位置が、第2軸22の支持部22a(先端部)の直近の目標位置に合わせられている。このとき、図7のS28の処理により、修正目標位置Pb(x,y)にローパスフィルタが掛けられるため、修正目標位置Pb(x,y)の急激な変化が抑制されている。そして、フィルタ処理後の修正目標位置Pb(x,y)に基づいて、修正指令値θbが算出される。その結果、修正指令値θbに基づいて各駆動部31,32が制御された際に、各軸21,22の角速度が急激に変化することが抑制されている。
加速時から等速時への移行時に、本実施形態は比較例よりも角速度が一定になる時期が遅くなっている。これにより、本実施形態では、第2軸22の支持部22aの弾性変形が元に戻るのに合わせて加速度が減少し、全体WAの重心を水平面上に投影した位置が、支持部22aの直近の目標位置に合わせられている。このときも、フィルタ処理後の修正目標位置Pb(x,y)に基づいて各駆動部31,32が制御された際に、各軸21,22の角速度が急激に変化することが抑制されている。
等速時から減速時への移行時に、本実施形態は比較例よりも角速度の低下する時期が早くなっている。これにより、これにより、本実施形態では、全体WAの重心を水平面上に投影した位置が、支持部22aの直近の目標位置に合わせられている。このときも、フィルタ処理後の修正目標位置Pb(x,y)に基づいて各駆動部31,32が制御された際に、各軸21,22の角速度が急激に変化することが抑制されている。
減速時から移動終了時への移行時に、本実施形態は比較例よりも角速度が低下する時期が遅くなっている。これにより、本実施形態では、第2軸22の支持部22aの弾性変形が元に戻るのに合わせて減速度が減少し、全体WAの重心を水平面上に投影した位置が、支持部22aの直近の目標位置に合わせられている。このときも、フィルタ処理後の修正目標位置Pb(x,y)に基づいて各駆動部31,32が制御された際に、各軸21,22の角速度が急激に変化することが抑制されている。
図9は、第2軸22の先端部の移動時を例として、比較例及び本実施形態における目標停止位置と重心の投影位置とのずれを示すタイムチャートである。比較例は、図8で説明したものと同一である。
同図に一点鎖線で示すように、比較例では、水平面上への全体WAの重心の投影位置と目標停止位置とのずれが、負側と正側とに交互に大きくなっている。すなわち、第2軸22の支持部22aを支点として、第3軸23が第2軸22の支持部22a(先端部)の移動方向の前後に振動している。これに対して、実線で示すように、本実施形態では、水平面上への全体WAの重心の投影位置と目標停止位置とのずれが0に近くなっている。すなわち、第2軸22の支持部22aを支点とする第3軸23の振動が、効果的に抑制されている。
以上詳述した本実施形態は、以下の利点を有する。
・第3軸23は、鉛直方向に沿って往復動自在に支持されているため、第3軸23の動作状態に応じて重心距離L3が変化し、慣性力の作用する高さが変化することとなる。この点、重心距離算出部(S25の処理)により、第3軸23及び物体Wの全体WAの重心と鉛直方向における第3軸23の位置とに基づいて、重心距離L3が算出される。修正目標位置算出部(S26,S27の処理)は、目標位置Pa、ばね定数K、全体WAの質量、目標位置Paが変化する加減速度a、及び重心距離L3に基づいて、所定の水平面上へ投影した全体WAの重心の位置を直近の目標位置へ近付けるように、目標位置Paを修正した修正目標位置Pbを算出する。修正指令値算出部(S29の処理)は、修正目標位置算出部により算出された修正目標位置Pbに第2軸22の支持部22a(先端部)を移動させるように、駆動部31,32を駆動する修正指令値θbを算出する。そして、サーボ機構53は、算出された修正指令値θbに基づいて駆動部31,32を制御する。このため、所定の水平面上へ投影した上記重心の位置と目標位置とのずれが解消される際に、第3軸23に振動が発生することを抑制することができる。さらに、実際に振動が発生する部分に加速度センサを取り付ける必要がなく、目標位置Paと既知の値と目標位置Paの加減速度aと重心距離L3とに基づいて修正目標位置Pb、ひいては修正指令値θbを算出することができる。したがって、現実のロボット10に容易に適用することができる。
・第2軸22の支持部22a(先端部)が加速する際には、第3軸23及び物体Wの全体WAに第2軸22の支持部22aの移動方向と反対方向の慣性力が作用する。このため、所定の水平面上へ投影した上記全体WAの重心は、第2軸22の支持部22aよりも遅れて移動する。この点、修正目標位置算出部は、第2軸22の支持部22aの加速時において、目標位置よりも第2軸22の支持部22aを進ませるように修正目標位置Pbを算出する。したがって、所定の水平面上へ投影した上記重心の位置と目標位置とのずれを縮小するように、修正目標位置Pbを算出することができる。
・第2軸22の支持部22aが減速する際には、第3軸23及び物体Wの全体WAに第2軸22の支持部22aの移動方向と同方向の慣性力が作用する。このため、所定の水平面上へ投影した上記全体WAの重心は、第2軸22の支持部22aよりも進んで移動する。この点、修正目標位置算出部は、第2軸22の支持部22aの減速時において、目標位置よりも第2軸22の支持部22aを遅らせるように修正目標位置Pbを算出する。したがって、所定の水平面上へ投影した上記重心の位置と目標位置とのずれを縮小するように、修正目標位置Pbを算出することができる。
・修正目標位置算出部は、所定の水平面上へ投影した上記重心の位置を目標位置に一致させるように修正目標位置Pbを算出する。このため、所定の水平面上へ投影した上記重心の位置と目標位置とのずれを最小にすることができ、第3軸23に振動が発生することを更に抑制することができる。
・修正指令値θbが急激に変化した場合、それぞれの修正指令値θbに基づき制御される駆動部31,32に、駆動部31,32が発生し得る上限トルクよりも大きなトルクが要求されるおそれがある。この点、修正目標位置算出部は、算出した修正目標位置Pbをなました目標位置に修正目標位置Pbを変更する。このため、修正目標位置Pbに第2軸22の支持部22aを移動させるように駆動部31,32を駆動する修正指令値θbが、急激に変化することを抑制することができる。そして、サーボ機構53は、修正指令値算出部により算出された修正指令値θbに基づいて、駆動部31,32を制御する。したがって、上限トルクよりも大きなトルクが駆動部31,32に要求されることを抑制することができる。
修正目標位置算出部は、ΔP=M・a・L^2/Kの式により算出した補正量ΔPを目標位置Paに加えて、修正目標位置Pbを算出することができる。したがって、修正目標位置Pbひいては修正指令値θbを容易に算出することができ、現実のロボット10に容易に適用することができる。
・ロボット10として、水平面に沿って回転する第1軸21と、第1軸21により水平面に沿って回転可能に支持された第2軸22と、第2軸22の先端部において鉛直方向に沿って往復動する第3軸と、を備える水平多関節ロボット10が採用されている。そして、水平多関節ロボット10において、第3軸23の振動を抑制することができる。
なお、上記実施形態を、以下のように変更して実施することもできる。
・図7のフローチャートにおいて、S28の修正目標位置Pbにローパスフィルタを掛ける処理を省略することもできる。その場合に、S29の処理で算出した修正指令値θbにローパスフィルタを掛けてもよい。また、修正指令値θbにローパスフィルタを掛ける処理を省略してもよい。
・ロボット10の制御方式として、いわゆるPTP(連続位置決め) 制御に限らず、いわゆるCP (連続経路) 制御を採用することもできる。その場合、全軸の指令値θaから直近の目標位置Pa(x,y)を算出しなくても、CP制御で用いられる直近の目標位置Pa(x,y)を図7のS23の処理で用いることもできる。
・図10に示すように、修正目標位置算出部により算出された修正目標位置Pbが第3軸23の支持部22a(軸線C3)の可動範囲よりも外側である場合、修正目標位置Pbに支持部22aを移動させるように駆動部31,32を駆動する修正指令値θbを算出することができない。この場合、ロボット10の制御にエラーが生じ、ロボット10の動作が停止するおそれがある。そこで、修正目標位置算出部は、算出した修正目標位置Pbが支持部22aの可動範囲よりも外側である場合に、修正目標位置Pbに最も近い可動範囲内の位置、すなわち同図の軸線C3の位置に修正目標位置Pbを変更する。こうした構成によれば、ロボット10の制御を継続することができる範囲内で、第3軸23の振動をできるだけ抑制することができる。
・上記実施形態では、修正目標位置算出部は、所定の水平面上へ投影した第3軸23及び物体Wの全体WAの重心の位置を目標位置に一致させるように修正目標位置Pbを算出した。これに対して、修正目標位置算出部は、所定の水平面上へ投影した全体WAの重心の位置と目標位置との中間位置に、上記重心の位置を一致させるように修正目標位置Pbを算出することもできる。すなわち、所定の水平面上へ投影した全体WAの重心の位置を直近の目標位置へ近付けるように、目標位置Paを修正した修正目標位置Pbを算出すれば、第3軸23に振動が発生することを抑制することができる。
・第3軸23の先端に物体Wが取り付けられていなくてもよい。その場合は、第3軸23を全体WAとすればよい。
・水平多関節ロボット10に限らず、水平面上のX方向レール沿って所定の第1軌道上を往復動する第1移動部と、第1移動部により移動可能に支持され、第1移動部を中心として水平面に沿って所定の第2円弧軌道上を往復動する第2移動部と、を備えるXRロボットに、上記制振制御を適用することもできる。その場合は、第1移動部について、角速度を速度に代え、角度位置を位置に代えて、制振制御を実行すればよい。
・水平多関節ロボット10に限らず、水平面上のX方向レール沿って所定の第1軌道上を往復動する第1移動部と、第1移動部により移動可能に支持され、水平面上のY方向レールに沿って所定の第2軌道上を往復動する第2移動部と、を備えるXYロボットに、上記制振制御を適用することもできる。その場合は、第1移動部及び第2移動部について、角速度を速度に代え、角度位置を位置に代えて、それぞれ制振制御を実行すればよい。
・上記第1移動部を備えず、上記第2移動部と、上記第3軸23とを備えるロボットに、上記制振制御を適用することもできる。