特許 6790669 ロボットの制御装置

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】6790669

(24)【登録日】2020年11月9日

(45)【発行日】2020年11月25日

(54)【発明の名称】ロボットの制御装置

(51)【国際特許分類】

   B25J 13/00 20060101AFI20201116BHJP

【ＦＩ】

   B25J13/00 Z

【請求項の数】9

【全頁数】17

(21)【出願番号】特願2016-188042(P2016-188042)

(22)【出願日】2016年9月27日

(65)【公開番号】特開2018-51651(P2018-51651A)

(43)【公開日】2018年4月5日

【審査請求日】2019年2月21日

(73)【特許権者】

【識別番号】501428545

【氏名又は名称】株式会社デンソーウェーブ

(74)【代理人】

【識別番号】100121821

【弁理士】

【氏名又は名称】山田 強

(74)【代理人】

【識別番号】100125575

【弁理士】

【氏名又は名称】松田 洋

(72)【発明者】

【氏名】長松 健司

【審査官】 稲垣 浩司

(56)【参考文献】

【文献】 特開平１０−１０００８５（ＪＰ，Ａ）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｂ２５Ｊ １／００ − ２１／０２

(57)【特許請求の範囲】

【請求項1】

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

【請求項2】

前記修正目標位置算出部は、前記移動部の加速時において、前記移動部を加速させた後に、前記目標位置よりも前記移動部を進ませ、且つ前記全体に作用する慣性力が小さくなるのに合わせて、前記目標位置に対する前記移動部の進み量を減少させるように前記修正目標位置を算出する請求項１に記載のロボットの制御装置。

【請求項3】

前記修正目標位置算出部は、前記移動部の減速時において、前記移動部を減速させた後に、前記目標位置よりも前記移動部を遅らせ、且つ前記全体に作用する慣性力が小さくなるのに合わせて、前記目標位置に対する前記移動部の遅れ量を減少させるように前記修正目標位置を算出する請求項１又は２に記載のロボットの制御装置。

【請求項4】

前記修正目標位置算出部は、前記水平面上へ投影した前記重心の位置を前記目標位置に一致させるように前記修正目標位置を算出する請求項１〜３のいずれか１項に記載のロボットの制御装置。

【請求項5】

前記修正目標位置算出部は、算出した前記修正目標位置をなました目標位置に前記修正目標位置を変更する請求項１〜４のいずれか１項に記載のロボットの制御装置。

【請求項6】

前記ばね定数をＫ、前記全体の質量をＭ、前記目標位置をＰ、前記目標位置Ｐが変化する際の加減速度をａ、前記重心距離をＬ、前記目標位置Ｐの補正量をΔＰとして、
前記修正目標位置算出部は、ΔＰ＝Ｍ・ａ・Ｌ＾２／Ｋの式により算出した前記補正量ΔＰを前記目標位置Ｐに加えて、前記修正目標位置を算出する請求項１〜５のいずれか１項に記載のロボットの制御装置。

【請求項7】

前記ロボットは、前記駆動部としての第２駆動部により駆動される前記移動部としての第２移動部と、第１駆動部により駆動され水平面に沿って所定の第１軌道上を往復動する第１移動部と、を備え、
前記第２移動部は、第１移動部により移動可能に支持され、前記第１移動部に対して水平面に沿って前記所定の軌道としての所定の第２軌道上を往復動し、
前記目標位置算出部は、前記目標位置としての前記第２移動部の直近の目標位置を算出し、
前記修正指令値算出部は、前記修正目標位置算出部により算出された前記修正目標位置に前記第２移動部を移動させるように、前記第１駆動部及び前記第２駆動部をそれぞれ駆動する第１修正指令値及び第２修正指令値を算出し、
前記駆動制御部は、前記修正指令値算出部により算出された前記第１修正指令値及び前記第２修正指令値に基づいて、それぞれ前記第１駆動部及び前記第２駆動部を制御する請求項１〜６のいずれか１項に記載のロボットの制御装置。

【請求項8】

前記第１移動部は、所定の中心軸線を中心として水平面に沿って、前記所定の第１軌道としての所定の第１円弧軌道上を往復動し、
前記第２移動部は、前記第１移動部を中心として水平面に沿って、前記所定の第２軌道としての所定の第２円弧軌道上を往復動する請求項７に記載のロボットの制御装置。

【請求項9】

前記修正目標位置算出部は、算出した前記修正目標位置が前記移動部の可動範囲よりも外側である場合に、前記修正目標位置に最も近い前記可動範囲内の位置に前記修正目標位置を変更する請求項１〜８のいずれか１項に記載のロボットの制御装置。

【発明の詳細な説明】

【技術分野】

【0001】

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

【背景技術】

【0002】

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

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開平１０−１０００８５号公報

【発明の概要】

【発明が解決しようとする課題】

【0004】

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

【0005】

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

【課題を解決するための手段】

【0006】

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

【0007】

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

【0008】

目標位置算出部により、移動部の直近の目標位置が算出される。加減速度算出部によって、目標位置算出部により算出された目標位置が変化する際の加減速度が算出される。

【0009】

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

【0010】

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

【0011】

したがって、修正目標位置算出部は、上記目標位置、上記ばね定数、上記全体の質量、目標位置が変化する際の加減速度、及び上記重心距離に基づいて、所定の水平面上へ投影した上記重心の位置を上記目標位置へ近付けるように、上記目標位置を修正した修正目標位置を算出する。すなわち、所定の水平面上へ投影した上記重心の位置と目標位置とのずれを縮小するように、目標位置を修正した修正目標位置が算出される。

【0012】

修正指令値算出部は、修正目標位置算出部により算出された修正目標位置に移動部を移動させるように、駆動部を駆動する修正指令値を算出する。そして、駆動制御部は、算出された修正指令値に基づいて駆動部を制御する。このため、所定の水平面上へ投影した上記重心の位置と目標位置とのずれが解消される際に、所定軸に振動が発生することを抑制することができる。さらに、実際に振動が発生する部分に加速度センサを取り付ける必要がなく、目標位置と既知の値と目標位置の加減速度と重心距離とに基づいて修正目標位置、ひいては修正指令値を算出することができる。したがって、現実のロボットに容易に適用することができる。

【0013】

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

【0014】

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

【0015】

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

【0016】

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

【0017】

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

【0018】

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

【0019】

第５の手段では、前記修正目標位置算出部は、算出した前記修正目標位置をなました目標位置に前記修正目標位置を変更する。

【0020】

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

【0021】

第６の手段では、前記ばね定数をＫ、前記全体の質量をＭ、前記目標位置をＰ、前記目標位置Ｐが変化する際の加減速度をａ、前記重心距離をＬ、前記目標位置Ｐの補正量をΔＰとして、前記修正目標位置算出部は、ΔＰ＝Ｍ・ａ・Ｌ＾２／Ｋの式により算出した前記補正量ΔＰを前記目標位置Ｐに加えて、前記修正目標位置を算出する。

【0022】

上記構成によれば、修正目標位置算出部は、ΔＰ＝Ｍ・ａ・Ｌ＾２／Ｋの式により算出した補正量ΔＰを目標位置Ｐに加えて、修正目標位置を算出することができる。したがって、修正目標位置ひいては修正指令値を容易に算出することができ、現実のロボットに容易に適用することができる。

【0023】

第７の手段では、前記ロボットは、前記駆動部としての第２駆動部により駆動される前記移動部としての第２移動部と、第１駆動部により駆動され水平面に沿って所定の第１軌道上を往復動する第１移動部と、を備え、前記第２移動部は、第１移動部により移動可能に支持され、前記第１移動部に対して水平面に沿って前記所定の軌道としての所定の第２軌道上を往復動し、前記目標位置算出部は、前記目標位置としての前記第２移動部の直近の目標位置を算出し、前記修正指令値算出部は、前記修正目標位置算出部により算出された前記修正目標位置に前記第２移動部を移動させるように、前記第１駆動部及び前記第２駆動部をそれぞれ駆動する第１修正指令値及び第２修正指令値を算出し、前記駆動制御部は、前記修正指令値算出部により算出された前記第１修正指令値及び前記第２修正指令値に基づいて、それぞれ前記第１駆動部及び前記第２駆動部を制御する。

【0024】

上記構成によれば、ロボットは、駆動部としての第２駆動部により駆動される上記移動部としての第２移動部と、第１駆動部により駆動され水平面に沿って所定の第１軌道上を往復動する第１移動部と、を備えている。そして、第２移動部は、第１移動部により移動可能に支持され、第１移動部に対して水平面に沿って所定の軌道としての所定の第２軌道上を往復動する。

【0025】

そして、目標位置算出部により、上記目標位置としての第２移動部の直近の目標位置が算出される。修正指令値算出部は、修正目標位置算出部により算出された修正目標位置に第２移動部を移動させるように、第１駆動部及び第２駆動部をそれぞれ駆動する第１修正指令値及び第２修正指令値を算出する。駆動制御部は、修正指令値算出部により算出された第１修正指令値及び第２修正指令値に基づいて、それぞれ第１駆動部及び第２駆動部を制御する。このため、第１移動部と、第１移動部により移動可能に支持された第２移動部とを備えるロボットにおいて、所定軸の振動を抑制することができる。

【0026】

第８の手段では、前記第１移動部は、所定の中心軸線を中心として水平面に沿って、前記所定の第１軌道としての所定の第１円弧軌道上を往復動し、前記第２移動部は、前記第１移動部を中心として水平面に沿って、前記所定の第２軌道としての所定の第２円弧軌道上を往復動する。

【0027】

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

【0028】

第９の手段では、前記修正目標位置算出部は、算出した前記修正目標位置が前記移動部の可動範囲よりも外側である場合に、前記修正目標位置に最も近い前記可動範囲内の位置に前記修正目標位置を変更する。

【0029】

修正目標位置算出部により算出された修正目標位置が移動部の可動範囲よりも外側である場合、修正目標位置に移動部を移動させるように駆動部を駆動する修正指令値を算出できないおそれがある。その場合、ロボットの制御にエラーが生じ、ロボットの動作が停止するおそれがある。

【0030】

この点、上記構成によれば、修正目標位置算出部は、算出した修正目標位置が移動部の可動範囲よりも外側である場合に、修正目標位置に最も近い可動範囲内の位置に修正目標位置を変更する。このため、ロボットの制御を継続することができる範囲内で、所定軸の振動をできるだけ抑制することができる。

【図面の簡単な説明】

【0031】

【図1】水平多関節ロボット及びロボットコントローラを示す模式図。

【図2】第３軸の振動状態を示す模式図。

【図3】水平多関節ロボットの動作モデルを示す模式図。

【図4】第２軸の動作時において、比較例及び本実施形態における目標位置と重心位置との関係を示す模式図。

【図5】重心距離を示す正面図。

【図6】第１軸及び第２軸の長さ及び角度位置を示す平面図。

【図7】制振制御の手順を示すフローチャート。

【図8】比較例及び本実施形態における角速度パターンを示すタイムチャート。

【図9】比較例及び本実施形態における目標停止位置と重心の投影位置とのずれを示すタイムチャート。

【図10】修正目標位置を可動範囲内の位置に変更する態様を示す模式図。

【発明を実施するための形態】

【0032】

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

【0033】

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

【0034】

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

【0035】

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

【0036】

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

【0037】

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

【0038】

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

【0039】

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

【0040】

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

【0041】

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

【0042】

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

【0043】

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

【0044】

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

【0045】

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

【0046】

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

【0047】

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

【0048】

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

【0049】

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

【0050】

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

【0051】

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

【0052】

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

【0053】

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

【0054】

ここで、第１軸２１及び第２軸２２が水平回転する際に、第２軸２２の先端部（軸線Ｃ３の位置）の加速度をａとする。第３軸２３及び物体Ｗの全体ＷＡに作用する慣性力Ｆは、全体ＷＡの質量をＭとして、以下の（１）式で表される。

【0055】

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

【0056】

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

【0057】

Δｄ＝Ｌ３・Ｔ／Ｋ ・・・（３）
変位量Δｄを相殺するためには、それと等しい補正量ΔＰを加えればよい。

【0058】

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

【0059】

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

【0060】

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

【0061】

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

【0062】

続いて、修正処理部５２は、全軸の角度位置の指令値θａに基づいて、第２軸２２の先端部（支持部２２ａ）の目標位置Ｐａ（ｘ，ｙ）を算出する（Ｓ２３）。目標位置Ｐａ（ｘ，ｙ）は、今回の制御周期における目標位置（直近の目標位置）に相当する。

【0063】

続いて、目標位置Ｐａ（ｘ，ｙ）に基づいて、目標位置Ｐａ（ｘ，ｙ）の加減速度（ａｘ，ａｙ）を算出する（Ｓ２４）。詳しくは、今回の目標位置Ｐａ（ｋ）のｘ成分であるｘ（ｋ）と前回の目標位置Ｐａ（ｋ−１）のｘ成分であるｘ（ｋ−１）との差を、加減速度ａｘとする。同様にして、今回の目標位置Ｐａ（ｋ）のｙ成分であるｙ（ｋ）と前回の目標位置Ｐａ（ｋ−１）のｙ成分であるｙ（ｋ−１）との差を、加減速度ａｙとする。続いて、修正処理部５２は、上述したように、重心距離Ｌ３を算出する（Ｓ２５）。修正処理部５２は、上記（４）式により、補正量ΔＰ（ｘ，ｙ）を算出する（Ｓ２６）。そして、修正処理部５２は、目標位置Ｐａ（ｘ，ｙ）に補正量ΔＰ（ｘ，ｙ）を加えて、修正目標位置Ｐｂ（ｘ，ｙ）を算出する（Ｓ２７）。

【0064】

続いて、修正処理部５２は、修正目標位置Ｐｂ（ｘ，ｙ）にローパスフィルタを掛ける（Ｓ２８）。すなわち、算出した修正目標位置Ｐｂ（ｘ，ｙ）をなました目標位置に修正目標位置Ｐｂ（ｘ，ｙ）を変更する。

【0065】

続いて、修正処理部５２は、フィルタ処理後の修正目標位置Ｐｂ（ｘ，ｙ）に基づいて、全軸の修正指令値θｂを算出する（Ｓ３９）。修正指令値θｂ（第１修正指令値、第２修正指令値に相当）は、第２軸２２の先端部を修正目標位置Ｐｂ（ｘ，ｙ）に移動させる各軸の指令値（目標角度位置）であり、修正目標位置Ｐｂ（ｘ，ｙ）に周知の変換処理を行うことにより算出することができる。そして、全軸の修正指令値θｂをサーボ機構５３に転送する（Ｓ３０）。

【0066】

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

【0067】

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

【0068】

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

【0069】

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

【0070】

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

【0071】

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

【0072】

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

【0073】

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

【0074】

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

【0075】

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

【0076】

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

【0077】

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

【0078】

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

【0079】

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

【0080】

・修正指令値θｂが急激に変化した場合、それぞれの修正指令値θｂに基づき制御される駆動部３１，３２に、駆動部３１，３２が発生し得る上限トルクよりも大きなトルクが要求されるおそれがある。この点、修正目標位置算出部は、算出した修正目標位置Ｐｂをなました目標位置に修正目標位置Ｐｂを変更する。このため、修正目標位置Ｐｂに第２軸２２の支持部２２ａを移動させるように駆動部３１，３２を駆動する修正指令値θｂが、急激に変化することを抑制することができる。そして、サーボ機構５３は、修正指令値算出部により算出された修正指令値θｂに基づいて、駆動部３１，３２を制御する。したがって、上限トルクよりも大きなトルクが駆動部３１，３２に要求されることを抑制することができる。

【0081】

修正目標位置算出部は、ΔＰ＝Ｍ・ａ・Ｌ＾２／Ｋの式により算出した補正量ΔＰを目標位置Ｐａに加えて、修正目標位置Ｐｂを算出することができる。したがって、修正目標位置Ｐｂひいては修正指令値θｂを容易に算出することができ、現実のロボット１０に容易に適用することができる。

【0082】

・ロボット１０として、水平面に沿って回転する第１軸２１と、第１軸２１により水平面に沿って回転可能に支持された第２軸２２と、第２軸２２の先端部において鉛直方向に沿って往復動する第３軸と、を備える水平多関節ロボット１０が採用されている。そして、水平多関節ロボット１０において、第３軸２３の振動を抑制することができる。

【0083】

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

【0084】

・図７のフローチャートにおいて、Ｓ２８の修正目標位置Ｐｂにローパスフィルタを掛ける処理を省略することもできる。その場合に、Ｓ２９の処理で算出した修正指令値θｂにローパスフィルタを掛けてもよい。また、修正指令値θｂにローパスフィルタを掛ける処理を省略してもよい。

【0085】

・ロボット１０の制御方式として、いわゆるＰＴＰ(連続位置決め) 制御に限らず、いわゆるＣＰ (連続経路) 制御を採用することもできる。その場合、全軸の指令値θａから直近の目標位置Ｐａ（ｘ，ｙ）を算出しなくても、ＣＰ制御で用いられる直近の目標位置Ｐａ（ｘ，ｙ）を図７のＳ２３の処理で用いることもできる。

【0086】

・図１０に示すように、修正目標位置算出部により算出された修正目標位置Ｐｂが第３軸２３の支持部２２ａ（軸線Ｃ３）の可動範囲よりも外側である場合、修正目標位置Ｐｂに支持部２２ａを移動させるように駆動部３１，３２を駆動する修正指令値θｂを算出することができない。この場合、ロボット１０の制御にエラーが生じ、ロボット１０の動作が停止するおそれがある。そこで、修正目標位置算出部は、算出した修正目標位置Ｐｂが支持部２２ａの可動範囲よりも外側である場合に、修正目標位置Ｐｂに最も近い可動範囲内の位置、すなわち同図の軸線Ｃ３の位置に修正目標位置Ｐｂを変更する。こうした構成によれば、ロボット１０の制御を継続することができる範囲内で、第３軸２３の振動をできるだけ抑制することができる。

【0087】

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

【0088】

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

【0089】

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

【0090】

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

【0091】

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

【符号の説明】

【0092】

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

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】