JP6790668B2 - ロボットの制御装置 - Google Patents

Description

本発明は、ロボットの動作を制御する制御装置に関する。

従来、ロボットのアームの先端近傍に加速度量を検出する加速度センサを取り付け、加速度センサにより検出された加速度量に基づいて、アームの先端に生じた振動を抑制するように各関節部の駆動部を制御する指令値を補償するものがある（特許文献１参照）。

特開平１０−１０００８５号公報

ところで、このように加速度センサにより検出された加速度量を用いる手法では、実際に振動が発生するアームの先端近傍に加速度センサを取り付ける必要がある。しかしながら、現実のロボットに適用する場合には、アームの先端近傍に加速度センサを取り付けるスペースや、加速度センサまでの配線の取り回し等が問題となり、実現が困難である。

本発明は、こうした実情に鑑みてなされたものであり、その主たる目的は、ロボットの振動を抑制することができ、且つ現実のロボットへの適用が容易であるロボットの制御装置を提供することにある。

上記課題を解決するための第１の手段は、
駆動部と、前記駆動部により駆動され水平面に沿って所定の軌道上を往復動する移動部と、鉛直方向に沿って往復動自在に前記移動部により支持され下部に物体が取り付けられた所定軸と、を備えるロボットを制御するロボットの制御装置であって、
前記移動部を直近の目標位置に移動させるように、前記駆動部を駆動する指令値を算出する指令値算出部と、
前記指令値算出部により算出された前記指令値が変化する際の加減速度を算出する加減速度算出部と、
前記所定軸及び前記物体の全体の重心と、鉛直方向における前記所定軸の位置とに基づいて、前記移動部が前記所定軸を支持する支持部から前記重心までの距離である重心距離を算出する重心距離算出部と、
前記指令値算出部により算出された前記指令値、前記所定軸の曲げに対する前記支持部のばね定数、前記全体の質量、前記加減速度算出部により算出された前記加減速度、及び前記重心距離算出部により算出された前記重心距離に基づいて、前記軌道上へ投影した前記重心の位置を前記目標位置へ近付けるように、前記指令値算出部により算出された前記指令値を修正した修正指令値を算出する修正指令値算出部と、
前記修正指令値算出部により算出された前記修正指令値に基づいて、前記駆動部を制御する駆動制御部と、
を備える。

上記構成によれば、ロボットは、駆動部と移動部と所定軸とを備えている。移動部は、駆動部により駆動され、水平面に沿って所定の軌道上を往復動する。所定軸は、鉛直方向に沿って往復動自在に移動部により支持されており、下部に物体が取り付けられている。

指令値算出部により、移動部を直近の目標位置に移動させるように、駆動部を駆動する指令値が算出される。加減速度算出部によって、指令値算出部により算出された指令値が変化する際の加減速度が算出される。指令値が変化する際の加減速度は、移動部を直近の目標位置に移動させる加減速度に相関している。

移動部が加減速する際には、移動部が所定軸を支持する支持部から所定軸及び物体の全体の重心までの距離である重心距離の位置に、この全体の質量がある場合と同様の慣性力が作用する。ここで、所定軸は、鉛直方向に沿って往復動自在に支持されているため、所定軸の動作状態に応じて重心距離が変化し、慣性力の作用する高さが変化することとなる。この点、重心距離算出部により、上記全体の重心と鉛直方向における所定軸の位置とに基づいて、上記重心距離が算出される。

上記慣性力により、所定軸の曲げに対する支持部のばね定数に応じて、所定の軌道上へ投影した上記重心の位置が目標位置から変位する。所定の軌道上へ投影した上記重心の位置と目標位置とがずれると、このずれが解消される際に所定軸に振動が発生する。この慣性力は、上記全体の質量と、移動部の加減速度に相関する指令値が変化する際の加減速度とに基づいて算出することができる。さらに、所定の軌道上へ投影した上記重心の位置が目標位置から変位する量は、上記慣性力と上記重心距離と上記ばね定数とに基づいて算出することができる。

したがって、修正指令値算出部は、上記指令値、上記ばね定数、上記全体の質量、指令値が変化する際の加減速度、及び上記重心距離に基づいて、所定の軌道上へ投影した上記重心の位置を上記目標位置へ近付けるように、指令値算出部により算出された指令値を修正した修正指令値を算出する。すなわち、所定の軌道上へ投影した上記重心の位置と目標位置とのずれを縮小するように、指令値を修正した修正指令値が算出される。そして、駆動制御部は、算出された修正指令値に基づいて駆動部を制御する。このため、所定の軌道上へ投影した上記重心の位置と目標位置とのずれが解消される際に、所定軸に振動が発生することを抑制することができる。さらに、実際に振動が発生する部分に加速度センサを取り付ける必要がなく、指令値と既知の値と指令値の加減速度と重心距離とに基づいて修正指令値を算出することができるため、現実のロボットに容易に適用することができる。

第２の手段では、前記修正指令値算出部は、前記移動部の加速時において、前記目標位置よりも前記移動部を進ませるように前記修正指令値を算出する。

移動部が加速する際には、所定軸及び物体の全体に移動部の移動方向と反対方向の慣性力が作用する。このため、所定の軌道上へ投影した上記全体の重心は、移動部よりも遅れて移動する。この点、上記構成によれば、修正指令値算出部は、移動部の加速時において、目標位置よりも移動部を進ませるように修正指令値を算出する。したがって、所定の軌道上へ投影した上記重心の位置と目標位置とのずれを縮小するように、修正指令値を算出することができる。

第３の手段では、前記修正指令値算出部は、前記移動部の減速時において、前記目標位置よりも前記移動部を遅らせるように前記修正指令値を算出する。

移動部が減速する際には、所定軸及び物体の全体に移動部の移動方向と同方向の慣性力が作用する。このため、所定の軌道上へ投影した上記全体の重心は、移動部よりも進んで移動する。この点、上記構成によれば、修正指令値算出部は、移動部の減速時において、目標位置よりも移動部を遅らせるように修正指令値を算出する。したがって、所定の軌道上へ投影した上記重心の位置と目標位置とのずれを縮小するように、修正指令値を算出することができる。

第４の手段では、前記修正指令値算出部は、前記軌道上へ投影した前記重心の位置を前記目標位置に一致させるように前記修正指令値を算出する。

上記構成によれば、修正指令値算出部は、所定の軌道上へ投影した重心の位置を目標位置に一致させるように修正指令値を算出する。このため、所定の軌道上へ投影した上記重心の位置と目標位置とのずれを最小にすることができ、所定軸に振動が発生することを更に抑制することができる。

第５の手段では、前記修正指令値算出部は、算出した前記修正指令値をなました指令値に前記修正指令値を変更する。

指令値の加減速度が急激に変化すると、指令値の加減速度に基づき算出される修正指令値が急激に変化するおそれがある。その場合、修正指令値に基づき制御される駆動部に、駆動部が発生し得る上限トルクよりも大きなトルクが要求されるおそれがある。この点、上記構成によれば、修正指令値算出部は、算出した修正指令値をなました指令値に修正指令値を変更する。したがって、修正指令値が急激に変化することを抑制することができ、上限トルクよりも大きなトルクが駆動部に要求されることを抑制することができる。

第６の手段では、前記移動部は、所定の中心軸線を中心として水平面に沿って所定の円弧軌道上を往復動し、前記ばね定数をＫ、前記全体の質量をＭ、前記目標位置に対応する前記中心軸線を中心とした前記移動部の目標角度位置をθ、前記目標角度位置θが変化する角加減速度をａ、前記重心距離をＬ、前記目標角度位置θの補正量をΔθとして、前記指令値算出部は、前記指令値として前記目標角度位置θを算出し、前記修正指令値算出部は、Δθ＝Ｍ・ａ・Ｌ＾２／Ｋの式により算出した前記補正量Δθを前記目標角度位置θに加えて、前記修正指令値を算出する。

上記構成によれば、移動部は、所定の中心軸線を中心として水平面に沿って所定の円弧軌道上を往復動する。この場合、移動部を直近の目標位置に移動させるように、駆動部を駆動する指令値として、目標位置に対応する上記中心軸線を中心とした移動部の目標角度位置θを採用することができる。そして、修正指令値算出部は、Δθ＝Ｍ・ａ・Ｌ＾２／Ｋの式により算出した補正量Δθを目標角度位置θに加えて、修正指令値を算出することができる。したがって、修正指令値を容易に算出することができ、現実のロボットに容易に適用することができる。

第７の手段では、前記ロボットは、前記移動部としての第２移動部と、所定の中心軸線を中心として水平面に沿って所定の第１円弧軌道上を往復動する第１移動部と、を備え、前記第２移動部は、前記第１移動部により移動可能に支持され、前記第１移動部を中心として水平面に沿って前記所定の円弧軌道としての所定の第２円弧軌道上を往復動し、前記第１移動部及び前記第２移動部に対してそれぞれ、前記指令値算出部、前記加減速度算出部、前記重心距離算出部、前記修正指令値算出部、及び前記駆動制御部による処理を実行する。

上記構成によれば、ロボットは、上記移動部としての第２移動部と、所定の中心軸線を中心として水平面に沿って所定の第１円弧軌道上を往復動する第１移動部と、を備えている。そして、第２移動部は、第１移動部により移動可能に支持され、第１移動部を中心として水平面に沿って所定の円弧軌道としての所定の第２円弧軌道上を往復動する。このため、ロボットとして、水平面に沿って回転する第１軸と、第１軸により水平面に沿って回転可能に支持された第２軸と、第２軸の先端部において鉛直方向に沿って往復動する第３軸と、を備える水平多関節ロボットを採用することができる。

そして、第１移動部及び前記第２移動部に対してそれぞれ、指令値算出部、加減速度算出部、重心距離算出部、修正指令値算出部、及び駆動制御部による処理が実行される。したがって、水平多関節ロボットにおいて、所定軸としての第３軸の振動を抑制することができる。

水平多関節ロボット及びロボットコントローラを示す模式図。 第３軸の振動状態を示す模式図。 水平多関節ロボットの動作モデルを示す模式図。 第２軸の動作時において、比較例及び本実施形態における目標位置と重心位置との関係を示す模式図。 重心距離を示す正面図。 第１軸及び第２軸の長さ及び角度位置を示す平面図。 制振制御の手順を示すフローチャート。 比較例及び本実施形態における角速度パターンを示すタイムチャート。 比較例及び本実施形態における目標停止位置と重心の投影位置とのずれを示すタイムチャート。

以下、一実施形態について、図面を参照しつつ説明する。本実施形態は、機械組立工場などの組立ラインにて用いられる産業用ロボットに具体化している。

図１に示すように、水平多関節ロボット１０は、基台２０、第１軸２１、第２軸２２、第３軸２３等を備えている。

基台２０は、床に固定されている。基台２０には、ベアリング（軸受け）等を介して、第１軸２１が水平回転自在に連結されている。第１軸２１は、軸線Ｃ１（所定の中心軸線に相当）を中心として水平回転自在となっている。なお、第１軸２１の先端部により、第１移動部（移動部）が構成されている。そして、第１移動部は、軸線Ｃ１を中心として水平面に沿って所定の第１円弧軌道上を往復動する。

第１軸２１の先端部には、ベアリング等を介して、第２軸２２が水平回転自在に連結されている。第２軸２２は、軸線Ｃ２を中心として水平回転自在となっている。なお、第２軸２２の先端部により、第２移動部が構成されている。そして、第２移動部は、第１軸２１の先端部（第１移動部）により移動可能に支持され、第１移動部を中心として水平面に沿って所定の第２円弧軌道（所定の円弧軌道、所定の軌道に相当）上を往復動する。

第２軸２２の先端部には、第３軸２３が上下動自在に連結されている。第３軸２３（所定軸に相当）は、第２軸２２の先端部（第２移動部）により、軸線Ｃ３（鉛直方向）に沿って往復動自在に支持されている。また、第３軸２３は、軸線Ｃ３を中心として水平回転自在となっている。第３軸２３の下部には、所定の動作を行うツールやワーク（部品）等に相当する物体Ｗが取り付けられている。

基台２０には、第１軸２１を水平回転駆動する第１駆動部３１が固定されている。第１駆動部３１（駆動部に相当）は、モータ、伝達機構、減速機構等を備えている。モータの出力軸は、伝達機構及び減速機構を介して第１軸２１の基端部に接続されている。減速機構は、基台２０に固定されている。

第１軸２１には、第２軸２２を水平回転駆動する第２駆動部３２が固定されている。第２駆動部３２（駆動部に相当）は、モータ、伝達機構、減速機構等を備えている。モータの出力軸は、伝達機構及び減速機構を介して第２軸２２の基端部に接続されている。

第２軸２２には、第３軸２３を垂直往復駆動する第３駆動部３３が固定されている。第３駆動部３３は、モータ、伝達機構、減速機構等を備えている。モータの出力軸は、伝達機構及び減速機構を介して第３軸２３に接続されている。伝達機構及び減速機構は、ベルト、プーリ、ボールねじナット等を備えている。

第２軸２２には、第３軸２３を水平回転駆動する第４駆動部が固定されている。第４駆動部（図示略）は、モータ、伝達機構、減速機構等を備えている。モータの出力軸は、伝達機構及び減速機構を介して第３軸２３に接続されている。

こうした構成によれば、第１駆動部３１によって、第１軸２１が水平回転駆動される。第１軸２１は、ベアリング等により支持されて水平回転させられる。第２駆動部３２によって、第２軸２２が水平回転駆動される。第２軸２２は、ベアリング等により支持されて水平回転させられる。第３駆動部３３によって、第３軸２３が垂直往復動駆動される。第４駆動部によって、第３軸２３が水平回転駆動される。

ロボット１０の動作は、ロボットコントローラ５０により制御される。コントローラ５０（制御装置に相当）は、ＣＰＵ、ＲＯＭ、ＲＡＭ、駆動回路、及び位置検出回路等を備えている。ＲＯＭは、ロボット１０のシステムプログラムや動作プログラム等を記憶している。ＲＡＭは、これらのプログラムを実行する際にパラメータの値等を記憶する。位置検出回路には、ロボット１０の各関節に設けられた各エンコーダ（図示略）の検出信号が、それぞれ入力される。位置検出回路は、各エンコーダの検出信号に基づいて、各関節に設けられたモータの回転角度位置を検出する。こうしたコントローラ５０により、軌道生成部５１、修正処理部５２、及びサーボ機構５３が構成されている。

コントローラ５０は、ロボット１０の動作プログラム等に基づいて、第２軸２２の先端部の目標停止位置を算出する。そして、コントローラ５０は、目標停止位置に基づいて、各軸の目標停止角度位置を算出する。軌道生成部５１は、各軸の目標停止角度位置に基づいて、各軸の角度位置を目標停止角度位置まで変更する際の各軸の角速度パターンを生成する。軌道生成部５１は、各軸の角速度パターンに基づいて、各軸の直近の目標角度位置である指令値θａｎを算出する。サーボ機構５３（駆動制御部に相当）は、各エンコーダにより検出された各軸の角度位置θｎと指令値θａｎとに基づいて、各軸の角度位置を指令値θａｎとするように、各駆動部３１〜３３を各駆動トルクＴｎで制御する。なお、修正処理部５２については後述する。

ところで、第１軸２１が水平回転駆動されたり、第２軸２２が水平回転駆動されたりした場合に、第３軸２３の先端が振動する場合がある。図２は、第３軸２３の振動状態を示す模式図である。

第３軸２３は第２軸２２の支持部２２ａにより、軸線Ｃ３に沿って往復動自在に支持されている。支持部２２ａ（第２移動部、移動部に相当）は、第３駆動部３３の伝達機構及び減速機構等により構成されており、第２軸２２のフレーム（被駆動部）や第３軸２３と比べて剛性が低い。このため、支持部２２ａは、第３軸２３を曲げる方向の力に対して、ばね定数Ｋのばね要素として作用する。したがって、第２軸２２の先端部が加減速する際に、第３軸２３及び物体Ｗに作用する慣性力により、支持部２２ａを中心として第３軸２３の先端が振動することとなる。特に、物体Ｗの質量が大きいほど、支持部２２ａから物体Ｗまでの距離が長いほど、この振動が大きくなる。

そこで、本実施形態では、コントローラ５０は、ロボット１０を図３に示す動作モデルとして捉え、第３軸２３の振動を抑制する制振制御を実行する。同図に示すように、ロボット１０の水平回転軸である第１軸２１及び第２軸２２は、各軸２１，２２を支持する状態が変化しない固定モデルとする。これに対して、ロボット１０の垂直往復動軸である第３軸２３は、支持部２２ａにより第３軸２３が支持される位置が変化する可変モデルとする。また、第３軸２３と物体Ｗとを足し合わせて、第３軸２３及び物体Ｗの全体ＷＡとする。そして、全体ＷＡの重心に、全体ＷＡの質量Ｍが存在するものとして扱う。

図４は、第２軸２２の動作時において、比較例及び本実施形態における目標位置と重心位置との関係を示す模式図である。図４（ａ）は、第２軸２２の指令値θａに対応する破線の目標位置に、第２軸２２の支持部２２ａ（先端部）の位置を合わせるように制御した比較例を示している。図４（ｂ）は、第２軸２２の指令値θａに対応する破線の目標位置に、軌道Ｒ上に投影した全体ＷＡの重心の位置を合わせるように制御した本実施形態を示している。

図４（ａ）に示すように、比較例では、支持部２２ａが加速度ａで加速する際に、全体ＷＡに支持部２２ａの移動方向と反対方向の慣性力−Ｍａが作用する。このため、支持部２２ａが弾性変形し、軌道Ｒ上へ投影した全体ＷＡの重心は、支持部２２ａよりも遅れて移動する。そして、支持部２２ａが等速時へ移行する際に、慣性力−Ｍａが小さくなり、支持部２２ａの弾性変形が元に戻る。支持部２２ａの弾性変形が元に戻る際に、第３軸２３に振動が発生する。

その後、支持部２２ａが減速度−ａで減速する際に、全体ＷＡに支持部２２ａの移動方向と同方向の慣性力Ｍａが作用する。このため、支持部２２ａが弾性変形し、軌道Ｒ上へ投影した全体ＷＡの重心は、支持部２２ａよりも進んで移動する。そして、支持部２２ａの移動が終了する際に、慣性力Ｍａが小さくなり、支持部２２ａの弾性変形が元に戻る。支持部２２ａの弾性変形が元に戻る際に、第３軸２３に振動が発生する。

図４（ｂ）に示すように、本実施形態でも、支持部２２ａが加速度ａで加速する際に、全体ＷＡに支持部２２ａの移動方向と反対方向の慣性力−Ｍａが作用する。ここで、本実施形態では、支持部２２ａの加速時において、破線の目標位置よりも支持部２２ａを進ませる。詳しくは、支持部２２ａが弾性変形する量を考慮して、破線の目標位置に、軌道Ｒ上に投影した全体ＷＡの重心の位置を合わせる。そして、支持部２２ａが等速時へ移行する際に、慣性力−Ｍａが小さくなり、支持部２２ａの弾性変形が元に戻るのに合わせて、目標位置に対する支持部２２ａの進み量を減少させる。このため、支持部２２ａの弾性変形が元に戻る際に、第３軸２３に振動が発生することが抑制される。

その後、支持部２２ａが減速度−ａで減速する際に、全体ＷＡに支持部２２ａの移動方向と同方向の慣性力Ｍａが作用する。ここで、本実施形態では、支持部２２ａの減速時において、破線の目標位置よりも支持部２２ａを遅らせる。詳しくは、支持部２２ａが弾性変形する量を考慮して、破線の目標位置に、軌道Ｒ上に投影した全体ＷＡの重心の位置を合わせる。そして、支持部２２ａの移動が終了する際に、慣性力Ｍａが小さくなり、支持部２２ａの弾性変形が元に戻るのに合わせて、目標位置に対する支持部２２ａの遅れ量を減少させる。このため、支持部２２ａの弾性変形が元に戻る際に、第３軸２３に振動が発生することが抑制される。

図５に示すように、第２軸２２の先端部が加減速する際には、第２軸２２が第３軸２３を支持する支持部２２ａから第３軸２３及び物体Ｗの全体ＷＡの重心Ｃｗまでの距離である重心距離Ｌ３の位置に、この全体ＷＡの質量Ｍがある場合と同様の慣性力が作用する。ここで、第３軸２３は、鉛直方向に沿って往復動自在に支持されているため、第３軸２３の動作状態に応じて重心距離Ｌ３が変化し、慣性力の作用する高さが変化することとなる。そこで、本実施形態では、修正処理部５２（重心距離算出部に相当）は、全体ＷＡの重心と鉛直方向における第３軸２３の位置とに基づいて、重心距離Ｌ３を算出する。鉛直方向における第３軸２３の位置は、第３軸２３を駆動する指令値や、エンコーダの検出信号に基づいて算出することができる。全体ＷＡの重心は、第３軸２３の長さ及び質量、物体Ｗの高さ幅及び質量に基づいて算出することができる。

図６は、第１軸２１及び第２軸２２の長さ及び角度位置を示す平面図である。同図に示すように、第１軸２１の回転中心である軸線Ｃ１から、第２軸２２の回転中心である軸線Ｃ２までの長さはＬ１である。軸線Ｃ２から、第３軸２３の中心軸線である軸線Ｃ３までの長さはＬ２である。基台２０の正面方向をｘ軸として、ｘ軸に対して第１軸２１の長手方向のなす角度がθ１である。第１軸２１の長手方向に対して、第２軸２２の長手方向のなす角度がθ２である。

ここで、第１軸２１及び第２軸２２が水平回転する際に、第２軸２２の先端部（軸線Ｃ３の位置）の加速度をａとし、第２軸２２の回転の角加減速度をａωとする。軸線Ｃ１から軸線Ｃ３までの長さをＬｒとする。その場合、以下の（１）式が成立する。

ａ＝ａω・Ｌｒ ・・・（１）
第３軸２３及び物体Ｗの全体ＷＡに作用する慣性力Ｆは、全体ＷＡの質量をＭとして、以下の（２）式で表される。

Ｆ＝Ｍ・ａ ・・・（２）
第２軸２２の支持部２２ａに作用するトルクＴは、上記重心距離をＬ３として、以下の（３）式で表される。

Ｔ＝Ｆ・Ｌ３ ・・・（３）
慣性力Ｆによる全体ＷＡの重心の軌道に沿った変位量Δｄは、第３軸２３の曲げに対する支持部２２ａのばね定数をＫとして、以下の（４）式で表される。

Δｄ＝Ｌ３・Ｔ／Ｋ ・・・（４）
変位量Δｄを相殺するための支持部２２ａの回転角度量である補正量Δθは、以下の（５）式で表される。

Δθ＝Δｄ／Ｌｒ ・・・（５）
（５）式に、上記（１）〜（４）式を適用して整理すると、補正量Δθは以下の（６）式で表される。Ｌ３＾２はＬ３の二乗である。

Δθ＝Ｍ・Ｌ３＾２・ａω／Ｋ ・・・（６）
すなわち、補正量Δθは、既知の定数である質量Ｍ及びばね定数Ｋ、第２軸２２の回転の角加減速度ａω、及び重心距離Ｌ３から算出することができる。

図７は、本実施形態における制振制御の手順を示すフローチャートである。この一連の処理は、コントローラ５０によって、ロボットの１０の各水平回転軸に対してそれぞれ実行される。ここでは、制御対象の水平回転軸を第ｎ軸とする。

まず、軌道生成部５１は、第ｎ軸の目標停止角度位置を取得する（Ｓ１０）。続いて、軌道生成部５１は、第ｎ軸の目標停止角度位置に基づいて、第ｎ軸の角速度パターンを生成する（Ｓ１１）。例えば、図８に破線で示す指令角速度のように、台形の角速度パターンを生成する。続いて、軌道生成部５１は、生成した角速度パターンに基づいて、第ｎ軸の角度位置の指令値θａを算出する（Ｓ１２）。指令値θａは、今回の制御周期における第ｎ軸の目標角度位置に相当する。なお、各水平回転軸がそれぞれ指令値θａに制御されることで、第２軸２２の先端部が今回の制御周期における目標位置（直近の目標位置）に制御される。

続いて、修正処理部５２は、第ｎ軸の回転の角速度ωに基づいて、第ｎ軸の回転の角加減速度ａωを算出する（Ｓ１３）。詳しくは、今回の角速度ω（ｋ）と前回の角速度ω（ｋ−１）との差を、角加減速度ａωとする。すなわち、角加減速度ａωは、指令値θａが変化する際の加減速度に相当する。続いて、修正処理部５２は、上述したように、重心距離Ｌ３を算出する（Ｓ１４）。修正処理部５２は、上記（６）式により、補正量Δθを算出する（Ｓ１５）。そして、修正処理部５２は、指令値θａに補正量Δθを加えて、修正指令値θｂを算出する（Ｓ１６）。

続いて、修正処理部５２は、修正指令値θｂにローパスフィルタを掛け（Ｓ１７）、フィルタ処理後の修正指令値θｂをサーボ機構５３に転送する（Ｓ１８）。すなわち、算出した修正指令値θｂをなました指令値に修正指令値θｂを変更して、サーボ機構５３に転送する。

続いて、修正処理部５２は、第ｎ軸に設けられたエンコーダの検出信号に基づいて、第ｎ軸の角度位置が目標停止角度位置に到達したか否か判定する（Ｓ１９）。この判定において、第ｎ軸の角度位置が目標停止角度位置に到達していないと判定した場合（Ｓ１９：ＮＯ）、Ｓ１２の処理から再度実行する。一方、この判定において、第ｎ軸の角度位置が目標停止角度位置に到達したと判定した場合（Ｓ１９：ＹＥＳ）、この一連の処理を終了する（ＥＮＤ）。

なお、Ｓ１２の処理が指令値算出部としての処理に相当し、Ｓ１３の処理が加減速度算出部としての処理に相当し、Ｓ１４の処理が重心距離算出部としての処理に相当し、Ｓ１５〜Ｓ１８の処理が修正指令値算出部としての処理に相当する。

図８は、比較例及び本実施形態における角速度パターンを示すタイムチャートである。破線は、図７のＳ１１の処理で生成された角速度パターンに相当し、指令角速度を示している。一点鎖線は、生成された角速度パターンに基づきＳ１２の処理で算出された指令値θａを、そのままサーボ機構５３に転送した場合に実現される角速度（角速度パターン）を示している。実線は、図７に示す本実施形態の制振制御を実行した場合に実現される角速度（角速度パターン）を示している。

同図に示すように、停止状態からの加速開始時に、本実施形態は比較例よりも角速度の上昇する時期が早くなっている。これにより、本実施形態では、第３軸２３及び物体Ｗの全体ＷＡの重心を軌道上に投影した位置が、第２軸２２の支持部２２ａ（先端部）の直近の目標位置に合わせられている。このとき、図７のＳ１７の処理により、修正指令値θｂにローパスフィルタが掛けられるため、修正指令値θｂの急激な変化が抑制されている。その結果、修正指令値θｂに基づいて各駆動部３１，３２が制御された際に、各軸２１，２２の角速度が急激に変化することが抑制されている。

加速時から等速時への移行時に、本実施形態は比較例よりも角速度が一定になる時期が遅くなっている。これにより、本実施形態では、第２軸２２の支持部２２ａの弾性変形が元に戻るのに合わせて加速度が減少し、全体ＷＡの重心を軌道上に投影した位置が、支持部２２ａの直近の目標位置に合わせられている。このときも、フィルタ処理後の修正指令値θｂに基づいて各駆動部３１，３２が制御された際に、各軸２１，２２の角速度が急激に変化することが抑制されている。

等速時から減速時への移行時に、本実施形態は比較例よりも角速度の低下する時期が早くなっている。これにより、これにより、本実施形態では、全体ＷＡの重心を軌道上に投影した位置が、支持部２２ａの直近の目標位置に合わせられている。このときも、フィルタ処理後の修正指令値θｂに基づいて各駆動部３１，３２が制御された際に、各軸２１，２２の角速度が急激に変化することが抑制されている。

減速時から移動終了時への移行時に、本実施形態は比較例よりも角速度が低下する時期が遅くなっている。これにより、本実施形態では、第２軸２２の支持部２２ａの弾性変形が元に戻るのに合わせて減速度が減少し、全体ＷＡの重心を軌道上に投影した位置が、支持部２２ａの直近の目標位置に合わせられている。このときも、フィルタ処理後の修正指令値θｂに基づいて各駆動部３１，３２が制御された際に、各軸２１，２２の角速度が急激に変化することが抑制されている。

図９は、第２軸２２の先端部の移動時を例として、比較例及び本実施形態における目標停止位置と重心の投影位置とのずれを示すタイムチャートである。比較例は、図８で説明したものと同一である。

同図に一点鎖線で示すように、比較例では、軌道上への全体ＷＡの重心の投影位置と目標停止位置とのずれが、負側と正側とに交互に大きくなっている。すなわち、第２軸２２の支持部２２ａを支点として、第３軸２３が第２軸２２の支持部２２ａ（先端部）の移動方向の前後に振動している。これに対して、実線で示すように、本実施形態では、軌道上への全体ＷＡの重心の投影位置と目標停止位置とのずれが０に近くなっている。すなわち、第２軸２２の支持部２２ａを支点とする第３軸２３の振動が、効果的に抑制されている。

以上詳述した本実施形態は、以下の利点を有する。

・第３軸２３は、鉛直方向に沿って往復動自在に支持されているため、第３軸２３の動作状態に応じて重心距離Ｌ３が変化し、慣性力の作用する高さが変化することとなる。この点、重心距離算出部（Ｓ１４の処理）により、第３軸２３及び物体Ｗの全体ＷＡの重心と鉛直方向における第３軸２３の位置とに基づいて、重心距離Ｌ３が算出される。修正指令値算出部（Ｓ１５，Ｓ１６の処理）は、指令値θａ、ばね定数Ｋ、全体ＷＡの質量、指令値θａが変化する角加減速度ａω、及び重心距離Ｌ３に基づいて、所定の軌道上へ投影した全体ＷＡの重心の位置を直近の目標位置へ近付けるように、指令値算出部（Ｓ１２の処理）により算出された指令値θａを修正した修正指令値θｂを算出する。そして、サーボ機構５３は、算出された修正指令値θｂに基づいて駆動部３１，３２を制御する。このため、所定の軌道上へ投影した上記重心の位置と目標位置とのずれが解消される際に、第３軸２３に振動が発生することを抑制することができる。さらに、実際に振動が発生する部分に加速度センサを取り付ける必要がなく、指令値θａと既知の値と指令値θａの角加減速度ａωと重心距離Ｌ３とに基づいて修正指令値θｂを算出することができるため、現実のロボット１０に容易に適用することができる。

・第２軸２２の支持部２２ａ（先端部）が加速する際には、第３軸２３及び物体Ｗの全体ＷＡに第２軸２２の支持部２２ａの移動方向と反対方向の慣性力が作用する。このため、所定の軌道上へ投影した上記全体ＷＡの重心は、第２軸２２の支持部２２ａよりも遅れて移動する。この点、修正指令値算出部は、第２軸２２の支持部２２ａの加速時において、目標位置よりも第２軸２２の支持部２２ａを進ませるように修正指令値θｂを算出する。したがって、所定の軌道上へ投影した上記重心の位置と目標位置とのずれを縮小するように、修正指令値θｂを算出することができる。

・第２軸２２の支持部２２ａが減速する際には、第３軸２３及び物体Ｗの全体ＷＡに第２軸２２の支持部２２ａの移動方向と同方向の慣性力が作用する。このため、所定の軌道上へ投影した上記全体ＷＡの重心は、第２軸２２の支持部２２ａよりも進んで移動する。この点、修正指令値算出部は、第２軸２２の支持部２２ａの減速時において、目標位置よりも第２軸２２の支持部２２ａを遅らせるように修正指令値θｂを算出する。したがって、所定の軌道上へ投影した上記重心の位置と目標位置とのずれを縮小するように、修正指令値θｂを算出することができる。

・修正指令値算出部は、所定の軌道上へ投影した上記重心の位置を目標位置に一致させるように修正指令値θｂを算出する。このため、所定の軌道上へ投影した上記重心の位置と目標位置とのずれを最小にすることができ、第３軸２３に振動が発生することを更に抑制することができる。

・指令値θａの角加減速度ａωが急激に変化すると、指令値θａの角加減速度ａωに基づき算出される修正指令値θｂが急激に変化するおそれがある。その場合、それぞれの修正指令値θｂに基づき制御される駆動部３１，３２に、駆動部３１，３２が発生し得る上限トルクよりも大きなトルクが要求されるおそれがある。この点、修正指令値算出部は、算出した修正指令値θｂをなました指令値に修正指令値θｂを変更する。したがって、修正指令値θｂが急激に変化することを抑制することができ、上限トルクよりも大きなトルクが駆動部３１，３２に要求されることを抑制することができる。

・第２軸２２の支持部２２ａは、軸線Ｃ２を中心として水平面に沿って所定の円弧軌道上を往復動する。この場合、第２軸２２の支持部２２ａを直近の目標位置に移動させるように、第２駆動部３２を駆動する指令値θａとして、目標位置に対応する上記軸線Ｃ２を中心とした支持部２２ａの目標角度位置θを採用することができる。そして、修正指令値算出部は、Δθ＝Ｍ・ａ・Ｌ＾２／Ｋの式により算出した補正量Δθを指令値θａに加えて、修正指令値θｂを算出することができる。したがって、修正指令値θｂを容易に算出することができ、現実のロボット１０に容易に適用することができる。

・ロボット１０として、水平面に沿って回転する第１軸２１と、第１軸２１により水平面に沿って回転可能に支持された第２軸２２と、第２軸２２の先端部において鉛直方向に沿って往復動する第３軸と、を備える水平多関節ロボット１０が採用されている。そして、第１軸２１及び第２軸２２に対してそれぞれ、指令値算出部、加減速度算出部、重心距離算出部、修正指令値算出部、及びサーボ機構５３による処理が実行される。したがって、水平多関節ロボット１０において、第３軸２３の振動を抑制することができる。

なお、上記実施形態を、以下のように変更して実施することもできる。

・図７のフローチャートにおいて、Ｓ１７の修正指令値θｂにローパスフィルタを掛ける処理を省略することもできる。

・上記実施形態では、修正指令値算出部は、所定の軌道上へ投影した第３軸２３及び物体Ｗの全体ＷＡの重心の位置を目標位置に一致させるように修正指令値θｂを算出した。これに対して、修正指令値算出部は、所定の軌道上へ投影した全体ＷＡの重心の位置と目標位置との中間位置に、上記重心の位置を一致させるように修正指令値θｂを算出することもできる。すなわち、所定の軌道上へ投影した全体ＷＡの重心の位置を直近の目標位置へ近付けるように、指令値θａを修正した修正指令値θｂを算出すれば、第３軸２３に振動が発生することを抑制することができる。

・第３軸２３の先端に物体Ｗが取り付けられていなくてもよい。その場合は、第３軸２３を全体ＷＡとすればよい。

・水平多関節ロボット１０に限らず、水平面上のＸ方向レール沿って所定の第１軌道上を往復動する第１移動部と、第１移動部により移動可能に支持され、第１移動部を中心として水平面に沿って所定の第２円弧軌道上を往復動する第２移動部と、を備えるＸＲロボットに、上記制振制御を適用することもできる。その場合は、第１移動部について、角速度を速度に代え、角度位置を位置に代えて、制振制御を実行すればよい。

・水平多関節ロボット１０に限らず、水平面上のＸ方向レール沿って所定の第１軌道上を往復動する第１移動部と、第１移動部により移動可能に支持され、水平面上のＹ方向レールに沿って所定の第２軌道上を往復動する第２移動部と、を備えるＸＹロボットに、上記制振制御を適用することもできる。その場合は、第１移動部及び第２移動部について、角速度を速度に代え、角度位置を位置に代えて、それぞれ制振制御を実行すればよい。

・上記第１移動部を備えず、上記第２移動部と、上記第３軸２３とを備えるロボットに、上記制振制御を適用することもできる。

１０…水平多関節ロボット（ロボット）、２１…第１軸、２２…第２軸、２２ａ…支持部（移動部、第２移動部）、２３…第３軸、５０…ロボットコントローラ（制御装置）、５１…軌道生成部（指令値算出部）、５２…修正処理部（加減速度算出部、重心距離算出部、修正指令値算出部）、５３…サーボ機構（駆動制御部）。

Claims (7)

1. 駆動部と、前記駆動部により駆動され水平面に沿って所定の軌道上を往復動する移動部と、鉛直方向に沿って往復動自在に前記移動部により支持され下部に物体が取り付けられた所定軸と、を備えるロボットを制御する制御装置であって、
   前記移動部を直近の目標位置に移動させるように、前記駆動部を駆動する指令値を算出する指令値算出部と、
   前記指令値算出部により算出された前記指令値が変化する際の加減速度を算出する加減速度算出部と、
   前記所定軸及び前記物体の全体の重心と、鉛直方向における前記所定軸の位置とに基づいて、前記移動部が前記所定軸を支持する支持部から前記重心までの距離である重心距離を算出する重心距離算出部と、
   前記指令値算出部により算出された前記指令値、前記所定軸の曲げに対する前記支持部のばね定数、前記全体の質量、前記加減速度算出部により算出された前記加減速度、及び前記重心距離算出部により算出された前記重心距離に基づいて、前記軌道上へ投影した前記重心の位置を前記目標位置へ近付けるように、前記指令値算出部により算出された前記指令値を修正した修正指令値を算出し、前記全体に作用する慣性力が小さくなるのに合わせて、前記指令値を修正する量を減少させる修正指令値算出部と、
   前記修正指令値算出部により算出された前記修正指令値に基づいて、前記駆動部を制御する駆動制御部と、
   を備えるロボットの制御装置。
2. 前記修正指令値算出部は、前記移動部の加速時において、前記目標位置よりも前記移動部を進ませ、且つ前記全体に作用する慣性力が小さくなるのに合わせて、前記目標位置に対する前記移動部の進み量を減少させるように前記修正指令値を算出する請求項１に記載のロボットの制御装置。
3. 前記修正指令値算出部は、前記移動部の減速時において、前記目標位置よりも前記移動部を遅らせ、且つ前記全体に作用する慣性力が小さくなるのに合わせて、前記目標位置に対する前記移動部の遅れ量を減少させるように前記修正指令値を算出する請求項１又は２に記載のロボットの制御装置。
4. 前記修正指令値算出部は、前記軌道上へ投影した前記重心の位置を前記目標位置に一致させるように前記修正指令値を算出する請求項１〜３のいずれか１項に記載のロボットの制御装置。
5. 前記修正指令値算出部は、算出した前記修正指令値をなました指令値に前記修正指令値を変更する請求項１〜４のいずれか１項に記載のロボットの制御装置。
6. 前記移動部は、所定の中心軸線を中心として水平面に沿って所定の円弧軌道上を往復動し、
   前記ばね定数をＫ、前記全体の質量をＭ、前記目標位置に対応する前記中心軸線を中心とした前記移動部の目標角度位置をθ、前記目標角度位置θが変化する角加減速度をａ、前記重心距離をＬ、前記目標角度位置θの補正量をΔθとして、
   前記指令値算出部は、前記指令値として前記目標角度位置θを算出し、
   前記修正指令値算出部は、Δθ＝Ｍ・ａ・Ｌ＾２／Ｋの式により算出した前記補正量Δθを前記目標角度位置θに加えて、前記修正指令値を算出する請求項１〜５のいずれか１項に記載のロボットの制御装置。
7. 前記ロボットは、前記移動部としての第２移動部と、所定の中心軸線を中心として水平面に沿って所定の第１円弧軌道上を往復動する第１移動部と、を備え、
   前記第２移動部は、前記第１移動部により移動可能に支持され、前記第１移動部を中心として水平面に沿って前記所定の円弧軌道としての所定の第２円弧軌道上を往復動し、
   前記第１移動部及び前記第２移動部に対してそれぞれ、前記指令値算出部、前記加減速度算出部、前記重心距離算出部、前記修正指令値算出部、及び前記駆動制御部による処理を実行する請求項６に記載のロボットの制御装置。

Images