JP5192925B2 - 負荷同定方法およびロボットの制御システム - Google Patents

Description

この発明は、モータにより駆動されるロボットにおいて、モータにかかる負荷を同定する技術に関するものである。

従来から、ロボットの駆動源としてモータが広く用いられており、例えば単軸ロボットでは、可動部材である可動テーブルがモータにより駆動されて移動する。また、このような可動部材を所望の速度で所望の位置に移動するために、速度制御ループや位置制御ループを備えた制御系によりモータを制御することが一般に行なわれている。この際、速度制御ループの速度ゲインや位置制御ループの位置ゲイン等のループゲインは、モータにかかる負荷に応じて設定され、例えば速度ゲインＫwspは、次式
Ｋwsp＝ωsc×Ｊ÷Ｋt 式（Ａ）
に基づいて設定することができる。ここで、ωscは目標速度応答周波数であってユーザにより設定される値であり、Ｊはモータにかかる負荷（イナーシャ）であり、Ｋtはモータのトルク定数である。

ところで、ロボットのモータにかかる負荷は、種々の原因により変動する。具体例を挙げると、このようなロボットは、ユーザの使用目的によって様々な荷重の搬送物を移載し、また設置態様も可動部材の可動方向が水平向きに設置されたり、可動部材の可動方向が鉛直向きに設置されたりするため、かかる搬送物や配置態様の変更によりモータ負荷が変動する場合がある。そして、このようにロボットの搬送物や設置態様が変更されることでモータにかかる負荷が変動してしまうと、動作中に振動や発振が発生したり、追従性が悪くなったり、制御が不安定になることがある。このため、負荷変動に応じて速度ゲイン（速度比例ゲイン、速度積分ゲイン）、位置比例ゲイン、加速度等を設定しなおす必要が生じる。このような場合、変動後の負荷を同定して、かかる同定結果から速度ゲイン等を算出して設定しなおすことができる。次に、このような負荷同定技術について説明する。

一般的に、モータのトルクが粘性摩擦とクーロン摩擦以外の要因で損失がない場合、モータにかかる負荷（イナーシャ）、モータトルクおよび摩擦トルク（粘性摩擦トルクとクーロン摩擦トルクの和）の間には、次式
[摩擦トルク]＋[イナーシャ×角加速度]＝[モータトルク] 式（Ｂ）
が成立する。なお、式（Ｂ）中の角加速度はモータの角加速度である。従来から、この関係式に基づいて、モータにかかるイナーシャを同定する種々の技術が提案されているが、特に特許文献１では、摩擦トルクを求めること無く簡便にイナーシャを同定することができる負荷同定技術が提案されている。具体的には、この負荷同定技術は、モータを加速した場合におけるモータトルクの積分量と、モータを減速した場合におけるモータトルクの積分量とを求めることで、摩擦トルクを求めることなく負荷同定を実行している。この負荷同定技術の原理は次の通りである。

モータの加速中におけるモータトルクの積分量は、式（Ｂ）を時間積分した次式
[摩擦トルクの積分量]a＋[イナーシャ×角速度]a＝[モータトルクの積分量]a 式（Ｃ）
を満たす。また、モータの減速中におけるモータトルクの積分量は、式（Ｂ）を時間積分した次式、
[摩擦トルクの積分量]d＋[イナーシャ×角速度]d＝[モータトルクの積分量]d 式（Ｄ）
を満たす。なお、式（Ｃ）の各項に付されたアルファベット「a」は各項の物理量がモータ加速中のものであることを示しており、式（Ｄ）の各項に付されたアルファベット「d」は各項の物理量がモータ減速中のものであることを示している。

また、一般に、摩擦トルクのうち粘性摩擦トルクは略モータ速度にのみ依存し、クーロン摩擦トルクは一定であると考えられており、モータ速度が同じであればモータにかかる摩擦トルクも略同じと推定される。したがって、特許文献１の図４等に示されるようにモータの加速と減速とを同じ時間をかけて同じ速度変化パターンで行なった場合、加速中にモータが受ける摩擦トルクを積分した積分量と、減速中にモータが受ける摩擦トルクを積分した積分量とは略等しくなる。なお、本明細書では、減速中のある時間範囲におけるモータの速度変化が、加速中の当該時間範囲におけるモータの速度変化の逆をたどるような（換言すれば、減速中のモータの速度変化パターンを時間反転させたものが、加速中のモータの速度変化パターンに重なるような）場合、この時間範囲において、加速中の速度変化パターンと減速中の速度変化パターンとが等しいと表現する。

そこで、この負荷同定技術は、同じ速度変化パターンで同じ時間をかけてモータの加速と減速とを行なうことで、次式
[摩擦トルクの積分量]a＝[摩擦トルクの積分量]d 式（Ｅ）
が成立するような条件を作り出している。つまり、このような条件下において、式（Ｃ）と式（Ｄ）の両辺を互いに引算することで、加速中および減速中それぞれの摩擦トルクの積分量が相殺されるとともに、イナーシャが[モータトルクの積分量]aと[モータトルクの積分量]dとの差から求められることを示す関係式が得られる。そして、従来技術の負荷同定方法は、こうして得られた関係式に基づいて、加速中でのモータトルクの積分量と減速中でのモータトルクの積分量との差から、摩擦トルクを求めること無く簡便にイナーシャを同定することを可能としている。

特開２００２−１５３０８８号公報（段落０００２等）

ところで、上述の負荷同定方法では、加速中および減速中の全区間でモータの駆動力を積分していたために、摩擦トルクがイナーシャの同定結果に影響してしまい、その結果、負荷を高精度に求めることができない場合があった。つまり、種々の理由により、加速中の全区間での速度変化パターンと、減速中の全区間での速度変化パターンとを比較した場合、これらの速度パターンが互いに同じにならない場合も多い。このような原因としては、上述のようなロボットの搬送物や配置変更に伴う負荷変動が大きく、変動後の負荷に対して最適な速度ゲイン等が実際の速度ゲイン等から大きくずれてしまったために、ロボットのモータ速度が制御系の速度指令に追従せず、モータ速度の立ち上がりが遅れたりする場合が挙げられる。そして、このような速度変化パターンの差が加速中と減速中との間で発生してしまうと、従来のように加速中および減速中の全区間でモータ駆動力を積分する負荷同定方法では、加速中にモータが受ける摩擦トルクの積分量と減速中にモータが受ける摩擦トルクの積分量とが十分に相殺されない場合があった。その結果、イナーシャの同定結果に摩擦トルクが影響してしまい、イナーシャを精度良く同定できない問題が発生する。このため、上述の負荷同定方法の場合、イナーシャを精度良く同定するには、あらかじめロボットのモータ速度が速度指令に追従する条件で駆動させるためのテスト駆動した後に負荷同定をしなければならず、作業が煩雑であった。

この発明は上記課題に鑑みなされたものであり、モータの速度変化パターンの差が加速中と減速中との間で発生した場合であっても、摩擦の影響を抑制してロボットで用いられるモータの負荷を高精度に同定することを可能とするロボットの負荷同定方法およびロボットの提供を目的としている。

この発明にかかる負荷同定方法は、モータにより駆動するロボットのモータにかかる負荷を同定する負荷同定方法であって、上記目的を達成するために、第１時間をかけてモータの駆動速度を第１駆動速度から第２駆動速度にまで加速した後でモータを第２時間をかけて第３駆動速度まで減速する、加減速動作を実行しながら、モータを加速している間に、モータの駆動速度が第１駆動速度および第３駆動速度よりも速く且つ第２駆動速度よりも遅い基準駆動速度となる時刻を含み且つ第１時間および第２時間よりも短い時間範囲を有する第１積分区間で、モータの駆動力を積分した値から加速中のモータ駆動力の積分量を求め、モータを減速している間に、モータの駆動速度が基準駆動速度となる時刻を含み且つ時間範囲を有する第２積分区間で、モータの駆動力を積分した値から、減速中のモータ駆動力の積分量を求め、加速中のモータ駆動力の積分量と、減速中のモータ駆動力の積分量との差に基づき前記負荷を同定することを特徴としている。

また、この発明にかかるロボットの制御システムは、モータを駆動源とするロボットに接続する制御システムであって、上記目的を達成するために、第１時間をかけてモータの駆動速度を第１駆動速度から第２駆動速度にまで加速した後でモータを第２時間をかけて第３駆動速度まで減速させるモータ制御部と、モータを加速している間に、モータの駆動速度が第１駆動速度および第３駆動速度よりも速く且つ第２駆動速度よりも遅い基準駆動速度となる時刻を含み且つ第１時間および第２時間よりも短い時間範囲を有する第１積分区間で、モータの駆動力を積分した値から加速中のモータ駆動力の積分量を求め、モータを減速している間に、モータの駆動速度が基準駆動速度となる時刻を含み且つ時間範囲を有する第２積分区間で、モータの駆動力を積分した値から減速中のモータ駆動力の積分量を求め、加速中のモータ駆動力の積分量と減速中のモータ駆動力の積分量との差に基づき負荷を同定する負荷同定部とを備えることを特徴としている。

このように構成された発明（負荷同定方法、ロボットの制御システム）は、第１時間をかけてモータの駆動速度を第１駆動速度から第２駆動速度にまで加速した後でモータを第２時間をかけて第３駆動速度まで減速する。したがって、「発明が解決しようとする課題」で説明したように、モータ加速中の全区間（第１時間）およびモータ減速中の全区間（第２時間）でモータの駆動力を積分した値から、加速中のモータ駆動力の積分量と減速中のモータ駆動力の積分量とを求めてしまうと、負荷を高精度に同定できない場合があった。

そこで、本発明は、基準駆動速度とモータの駆動速度とから決まる、第１積分区間と第２積分区間とでモータの駆動力を積分している。具体的には次の通りである。この基準駆動速度は、第１駆動速度および第３駆動速度よりも速く且つ第２駆動速度よりも遅い速度であり、言わば、加減速動作によってモータの駆動速度が変化する速度範囲内にある。そして、モータを加速している間に、モータの駆動速度が基準駆動速度となる時刻を含み且つ第１時間および第２時間よりも短い時間範囲を有する第１積分区間で、モータの駆動力を積分して加速中のモータ駆動力の積分量を求めている。また、モータを減速している間に、モータの駆動速度が基準駆動速度となる時刻を含み且つ先ほどの時間範囲（第１時間および第２時間よりも短い時間範囲）を有する第２積分区間で、モータの駆動力を積分して減速中のモータ駆動力の積分量を求めている。このように、本発明では、モータ加速中の全区間（第１時間）とモータ減速中の全区間（第２時間）でモータの駆動力を積分するのではなく、第１時間および第２時間よりも短い時間範囲を有する第１積分区間と第２積分区間でモータの駆動力を積分する。しかも、これら第１積分区間および第２積分区間はいずれも、駆動速度が基準駆動速度となる時刻を含んでおり、換言すれば、第１積分区間および第２積分区間のいずれにおいても、同じ基準駆動速度の周辺でモータの駆動速度は変化する。したがって、第１積分区間および第２積分区間の間では、駆動速度の変化パターンは近似する。そして、このように速度変化パターンが近似する第１積分区間と第２積分区間でモータ駆動力を積分した値から求められた、加速中のモータ駆動力の積分量と減速中のモータ駆動力の積分量との差をとることで、「発明が解決しようとする課題」で説明したような摩擦トルクの影響を、従来技術と比較してより確実に相殺することができるため、モータ速度が速度指令に追従する条件で駆動させるためのテスト駆動をする必要がない。こうして、テスト駆動を行なうことなく負荷同定のための作業を簡素化しつつ、摩擦の影響を抑制してロボットで用いられるモータの負荷を高精度に同定することが可能となっている。

また、加減速動作を往路方向および当該往路方向と逆向きの復路方向のそれぞれについて実行し、往路方向に加速するモータの駆動速度が基準駆動速度となる時刻を含み且つ上記時間範囲を有する第１積分区間と、復路方向に加速するモータの駆動速度が基準駆動速度となる時刻を含み且つ時間範囲を有する第１積分区間とで、モータの駆動力を積分した値から加速中のモータ駆動力の積分量を求め、往路方向に減速するモータの駆動速度が基準駆動速度となる時刻を含み且つ上記時間範囲を有する第２積分区間と、復路方向に減速するモータの駆動速度が基準駆動速度となる時刻を含み且つ上記時間範囲を有する第２積分区間とで、モータの駆動力を積分した値から減速中のモータ駆動力の積分量を求めるように構成しても良い。この構成では、往路方向および復路方向のそれぞれについてモータの加減速動作が実行される。そして、往路および復路のそれぞれの第１積分区間で加速中のモータ駆動力の積分値から加速中のモータ駆動力の積分量が求められるとともに、往路及び復路のそれぞれの第２積分区間での減速中のモータ駆動力の積分値から減速中のモータ駆動力の積分量が求められる。すなわち、この構成では、往路と復路のそれぞれでモータの駆動力を積分して加速中のモータ駆動力の積分量と減速中のモータ駆動力の積分量とを求めるとともに、これら加速中のモータ駆動力の積分量と減速中のモータ駆動力の積分量の差から負荷が同定される。したがって、例えば重力等の一方向にかかる力がロボットに作用しているような場合でも、かかる力が負荷の同定結果に与える影響を抑制することができる。

また、互いに異なる複数の基準駆動速度のそれぞれについて第１積分区間と第２積分区間とを求め、各第１積分区間で加速しているモータの駆動力を積分した値を足し合わせて加速中のモータ駆動力の積分量を求め、各第２積分区間で減速しているモータの駆動力を積分した値を足し合わせて減速中のモータ駆動力の積分量を求めるように構成しても良い。このような構成では、複数の基準駆動速度のそれぞれについて第１積分区間と第２積分区間とが求められており、同じ基準駆動速度から求められた第１積分区間と第２積分区間とでは、上述したとおり速度変化パターンは近似する。また、複数の第１積分区間それぞれでのモータ駆動力の積分値の和として加速中のモータ駆動力の積分量が求められ、複数の第２積分区間それぞれでのモータ駆動力の積分値の和として減速中のモータ駆動力の積分量が求められる。そして、加速中のモータ駆動力の積分量と減速中のモータ駆動力の積分量との差に基づいてモータにかかる負荷が同定される。このようにして求められた加速中のモータ駆動力と減速中のモータ駆動力との差を取った場合、同じ基準駆動速度から求められて速度変化パターンが近似する第１積分区間および第２積分区間でのモータ駆動力の積分値同士が引算されることとなり、摩擦トルクの影響を従来技術と比較してより確実に相殺することができる。したがって摩擦の影響を抑制してロボットで用いられるモータの負荷を高精度に同定することが可能となっている。さらに、かかる構成では、１つではなく複数の第１積分区間と第２積分区間とでモータ駆動力の積分量が求められており、負荷同定をより高精度に実行することができる。

以上のように、本発明によれば、モータ加速中の全区間（第１区間）とモータ減速中の全区間（第２時間）でモータの駆動力を積分するのではなく、第１時間および第２時間よりも短い時間範囲を有する第１積分区間と第２積分区間でモータの駆動力を積分する。しかも、これら第１積分区間および第２積分区間はいずれも、駆動速度が基準駆動速度となる時刻を含んでおり、換言すれば、第１積分区間および第２積分区間のいずれにおいても、同じ基準駆動速度の周辺でモータの駆動速度は変化する。したがって、第１積分区間および第２積分区間の間では、駆動速度の変化パターンは近似する。そして、このように速度変化パターンが近似する第１積分区間と第２積分区間でモータ駆動力を積分した値から求められた、加速中のモータ駆動力の積分量と減速中のモータ駆動力の積分量との差をとることで、「発明が解決しようとする課題」で説明したような摩擦トルクの影響を、従来技術と比較してより確実に相殺することができるため、モータ速度が速度指令に追従する条件で駆動させるためのテスト駆動をする必要がない。こうして、テスト駆動を行なうことなく負荷同定のための作業を簡素化しつつ、摩擦の影響を抑制してロボットで用いられるモータの負荷を高精度に同定することが可能となっている。

第１実施形態
図１は、本実施形態にかかる単軸ロボットの一例を示す平面図である。単軸ロボット１０は、Ｘ軸方向に伸びるベース部材２等からなるロボット本体１と、このロボット本体１に対してＸ軸方向に往復移動可能な可動テーブル３と、この可動テーブル３を移動させるための機構部とから構成されている。ベース部材２は、同図紙面の裏から表に向う方向に開口しており、その内部が中空に構成されている。また、ベース部材２には、Ｘ軸方向に延びる一対の固定レール６、６と、各固定レール６、６の間においてＸ軸方向に延びるボールネジ軸７とが配設されている。このボールネジ軸７は、その基端部が支持部材８に回転自在に支持されるとともに、その先端部が支持部材１５に回転自在に支持されている。Ｘ軸方向において、支持部材１５に対してボールネジ軸７の反対側にはモータ１１が配置されており、このモータ１１の出力軸とボールネジ軸７の基端部とが支持ブロック９の内部において、カップリング１２を介して連結されている。しかも、このボールネジ軸７には、可動テーブル３に取り付けられたボールナット２０が螺合している。したがって、モータ１１が正逆回転することで、可動テーブル３は往路（＋Ｘ）方向および復路（−Ｘ）方向に往復移動することができる。なお、本実施形態では、可動テーブル３の移動方向Ｘが水平方向となるようにロボット１０は配置されている。

図２は、図１の単軸ロボットが備える制御システム１００のブロック図を示す図である。この制御システムは、速度指令Ｖrefに基づいてモータの駆動を制御するモータ制御部１１０と、モータの負荷を同定する負荷同定部１２０とを備えている。以下では、先にモータ制御部１１０の構成を説明した後で、負荷同定部１２０の構成を説明する。

モータ制御部１１０は、速度ループのループゲイン（速度ゲイン）が設定される速度制御部１１２と、電流ループのループゲインが設定される電流制御部１１３とを備えており、速度制御部１１２および電流制御部１１３はそれぞれＰＩ制御を実行する。また、モータ制御部１１０は、これら速度制御部１１２および電流制御部１１３のほかに微分器１１１をさらに備えている。微分器１１１は、位置検出器３４が検出したモータ１１の位置を微分して算出したモータ１１の駆動速度Ｖfbkを速度制御部１１２の入力側にフィードバックする。そして、モータ駆動速度Ｖfbkと外部から入力される速度指令Ｖrefとの差が速度制御部１１２に入力され、この速度制御部１１２は速度指令Ｖrefとモータ駆動速度Ｖfbkとの差に基づいて電流指令Ｉrefを出力する。また、電流制御部１１３は、この電流指令Ｉrefに基づいて駆動電流をモータに出力する。つまり、電流制御部１１３が出力する駆動電流の電流値Ｉfbkは、電流制御部１１３の入力側にフィードバックされており、この駆動電流地Ｉfbkと電流指令Ｉrefとの差に基づいて駆動電流を出力する。そして、駆動電流を受けたモータ１１は、この駆動電流に応じたトルクを出力する。

次に負荷同定部１２０について説明する。先ずは、この負荷同定部１２０で実行される負荷同定技術の原理について説明する。この負荷同定技術は、モータ１１を加減速することで、ロボットの可動テーブル３を加減速させて、モータ１１にかかる負荷を同定するものである。つまり、加速中にモータ１１にかかるトルクをＴaと、減速中にモータ１１にかかるトルクをＴdとすると、次式

が成立する。なお、ここで、Ｊはイナーシャ（負荷）であり、θaはモータ加速中におけるモータの回転角度であり、θdはモータ減速中におけるモータの回転角度であり、Ｆｒｉｃｔｉｏｎaは加速中の摩擦トルクであり、Ｆｒｉｃｔｉｏｎdは減速中の摩擦トルクである。ここで、摩擦トルクを発生させる摩擦はクーロン摩擦と粘性摩擦とあるが、クーロン摩擦は速度に依らず一定であり、粘性摩擦は速度にのみ依存するとすると、速度が等しいときには、次式

が成立する。したがって、速度が等しいとの条件下で式（１）と式（２）との引算を取ると、次式

が得られる。なお、Ｉaは加速中のモータ１１の駆動電流Ｉfbkであり、Ｉdは減速中のモータ１１の駆動電流Ｉfbkであり、Ｋtはモータのトルク定数である。よって、加速中の速度変化パターンと減速中の速度変化パターンとが等しいときには、加速中のモータトルクＴaを加速中の時間範囲で積分した積分量と、減速中のモータトルクＴdを減速中の時間範囲で積分した積分量との差を取ることで、摩擦トルクの影響を抑制して負荷同定を実行できる。また、モータの駆動電流の積分量にトルク定数Ｋtをかけることでモータトルクの積分量は求められ、換言すれば、モータトルクの積分量はモータの駆動電流を積分することで実質的に求められる。そこで、本実施形態は、モータの駆動電流を積分することで、モータトルクの積分量を求めて負荷同定を行なう。つまり、加速中の速度変化パターンと減速中の速度変化パターンとが等しいとの条件下で、加速中において駆動電流Ｉaを積分した値Ｉa(Ｉfbk)と、加速中において角加速度を積分した値Ｉa(aclfbk)と、減速中において駆動電流Ｉdを積分した値Ｉd(Ｉfbk)と、減速中において角加速度を積分した値Ｉ（aclfbk_d）とは、次式
Ｊ＝Ｋt（Ｉa(Ｉfbk)−Ｉd(Ｉfbk)）／（Ｉa(aclfbk)−Ｉd(aclfbk)） （式Ｅ）
を満たすため、本実施形態の負荷同定技術は式（Ｅ）を用いて負荷を同定することとしている。より具体的な信号を用いて説明すると次の通りである。

図３は、モータを加減速するための速度指令と電流指令とを示す図であり、同図上段は速度指令Ｖrefの時間変化を示し、同図下段は電流指令Ｉrefの時間変化を示している。同図に示すように、速度指令は、ピークを中心に左右対称となっており、加速中における速度変化パターンと減速中における速度変化パターンは略等しい。しかしながら、「発明が解決しようとする課題」でも説明したが、次に例示するようにモータ１１の駆動速度Ｖfbkはこの速度指令Ｖrefに追従するとは限らない。

そこで、負荷同定部１２０は、出力記憶部１２１および積分区間決定部１２２を備えている。つまり、出力記憶部１２１は、モータ１１の駆動速度Ｖfbkと駆動電流値Ｉfbkとが入力されてこれらを記憶する。さらに、出力記憶部１２１は、モータ１１の駆動速度Ｖfbkを積分区間決定部１２２に出力する。そして、この積分区間決定部１２２は、モータ駆動速度Ｖfbkと、基準駆動速度ＲＶから積分区間を決定している。そして、負荷同定部１２０の負荷演算部１２３では。このように決定された積分区間でモータ駆動力を積分した値に基づいて、負荷を同定している。詳細は次の通りである。

図４は、モータの駆動速度と駆動電流値とを例示す図であり、同図上段は駆動速度Ｖfbkの時間変化を示し、同図下段は駆動電流指令Ｉfbkの時間変化を示している。同図が示すように、モータ１１は、時間Ｔ1（第１時間）をかけて駆動速度Ｖ1（＝０）（第１駆動速度）から駆動速度Ｖ2（第２駆動速度）にまで加速された後、時間Ｔ2（第２時間）をかけて駆動速度Ｖ2（第２駆動速度）から速度Ｖ3（第３駆動速度）まで減速されている。しかしながら、速度指令Ｖrefに対して駆動速度Ｖfbkは追従しておらず、その結果、加速中および減速中のそれぞれの全域で見ると、加速中での駆動速度Ｖfbkの速度変化パターンと減速中での駆動速度Ｖfbkの速度変化パターンとが異なっている。このような条件下では、加速中の全域（第１時間Ｔ1の全域）と減速中の全域（第２時間Ｔ2の全域）とで式（Ｂ）の積分値を求めてしまうと、イナーシャＪの同定結果に摩擦トルクの影響が残ってしまう場合があった。そこで、積分区間決定部１２２では、第１駆動速度Ｖ1および第３駆動速度Ｖ3よりも速く且つ第２駆動速度Ｖ2よりも遅い基準駆動速度ＲＶと、駆動速度Ｖfbkとが比較結果されて、第１時間Ｔ1よりも短い時間範囲inttを有する第１積分区間[ｔas、ｔae]と、第２時間Ｔ2よりも短い時間範囲inttを有する第２積分区間[ｔds、ｔde]とが求められる。つまり、基準駆動速度ＲＶと駆動速度Ｖfbkとが比較されて、加速中のモータ駆動速度Ｖfbkが基準駆動速度ＲＶとなる時刻ｔaが求められて、この時刻ｔaを中心として時間範囲inttを有する区間が第１積分区間[ｔas、ｔae]として決定される。一方、減速中のモータ駆動速度Ｖfbkが基準駆動速度ＲＶとなる時刻ｔdが求められて、この時刻ｔdを中心として時間範囲inttを有する区間が第２積分区間[ｔds、ｔde]として決定される。なお、本明細書において、第１積分区間は、モータ加速中に対応する積分区間であり、第２積分区間はモータ減速中に対応する積分区間である。

そして、このようにして求められた各積分区間とは負荷演算部１２３に入力されるとともに、さらにこの負荷演算部１２３には出力記憶部１２１から駆動電流Ｉfbkが入力される。そして、負荷演算部１２３では、各積分区間と駆動電流Ｉfbkに基づいて次のような演算処理が実行される。つまり、第１積分区間[ｔas、ｔae]で加速中の駆動電流Ｉfbkを積分した値Ｉa(Ｉfbk)にトルク定数Ｋtが乗ぜられて、加速中の駆動トルクの積分量が求められるとともに、第２積分区間[ｔds、ｔde]で減速中の駆動電流Ｉfbkを積分した値Ｉd(Ｉfbk)にトルク定数Ｋtが乗ぜられて、減速中の駆動トルクの積分量が求められる。また同時に、負荷演算部１２３は、出力記憶部１２１から受けとった駆動速度Ｖfbkを微分して角加速度を求める。そして、第１積分区間[ｔas、ｔae]で加速中の角加速度の積分量Ｉ(aclfbk)が求められるとともに、第２積分区間[ｔds、ｔde]で加速中の角加速度の積分量Ｉ(aclfbk)が求められる。そして、上記式（Ｂ）に基づいて、加速中の駆動トルクの積分量と減速中の駆動トルクの積分量との差を、加速中の角加速度の積分量Ｉ(aclfbk)と減速中の角加速度の積分量ＩＩ(aclfbk)の差で除算した値が、イナーシャＪとして求められる。

このように、第１実施形態では、モータ加速中の全区間（第１時間Ｔ1）とモータ減速中の全区間（第２時間Ｔ2）でモータの駆動力を積分するのではなく、第１時間Ｔ1および第２時間Ｔ2よりも短い時間範囲inttを有する第１積分区間[ｔas、ｔae]と第２積分区間[ｔds、ｔde]でモータの駆動力を積分する。しかも、これら第１積分区間[ｔas、ｔae]および第２積分区間[ｔds、ｔde]はいずれも、駆動速度Ｖfbkが基準駆動速度ＲＶとなる時刻ｔa、ｔdを含んでおり、換言すれば、第１積分区間[ｔas、ｔae]および第２積分区間[ｔas、ｔae]のいずれにおいても、同じ基準駆動速度ＲＶの周辺でモータの駆動速度Ｖfbkは変化する。したがって、第１積分区間[ｔas、ｔae]および第２積分区間[ｔds、ｔde]の間では、駆動速度Ｖfbkの変化パターンは近似する。そして、このように速度変化パターンが近似する第１積分区間[ｔas、ｔae]と第２積分区間[ｔds、ｔde]とでモータ駆動力を積分して求められた、加速中のモータ駆動力の積分量と減速中のモータ駆動力の積分量との差をとることで、「発明が解決しようとする課題」で説明したような摩擦トルクの影響を、従来技術と比較してより確実に相殺することができため、モータ速度が速度指令に追従する条件で駆動させるためのテスト駆動をする必要がない。こうして、テスト駆動を行なうことなく負荷同定のための作業を簡素化しつつ、摩擦の影響を抑制して単軸ロボット１０で用いられるモータ１１の負荷を高精度に同定することが可能となっている。

第２実施形態
上述の第１実施形態では、基準駆動速度ＲＶは１つだけ設けられていたが、基準駆動速度ＲＶの数は１つには限られず、２つ以上であっても良い。そこで、第２実施形態では、基準駆動速度が２つＲＶ1、ＲＶ2設けられている場合について説明する。なお、基準駆動速度の個数が２つであるという以外では、第１実施形態と第２実施形態は共通するため、以下では差異点を中心に説明する。

図５は、モータの駆動速度と駆動電流値とを例示す図であり、同図上段は駆動速度Ｖfbkの時間変化を示し、同図下段は駆動電流指令Ｉfbkの時間変化を示している。積分区間決定部１１２には、互いに異なる速度に駆動速度に対応する２つの基準駆動速度ＲＶ1、ＲＶ2が記憶されている。なお、各基準駆動速度ＲＶ1、ＲＶ2は、第１駆動速度Ｖ1および第３駆動速度Ｖ3よりも速く且つ第２駆動速度Ｖ2よりも遅い。積分区間決定部１２２では、各基準駆動速度ＲＶ1、ＲＶ2について第１積分区間および第２積分区間が決定され、結果として、第１積分区間と第２積分区間とは２つずつ求められることとなる。具体的には次の通りである。

第１基準駆動速度ＲＶ1と駆動速度Ｖfbkとが比較結果されて、加速中のモータ駆動速度Ｖfbkが第１基準駆動速度ＲＶ1となる時刻ｔa1が求められる。そして、この時刻ｔa1を中心として時間範囲inttを有する区間が１つ目の第１積分区間[ｔas1、ｔae1]として決定される。また、第２基準駆動速度ＲＶ2と駆動速度Ｖfbkとが比較結果されて、加速中のモータ駆動速度Ｖfbkが第２基準駆動速度ＲＶ2となる時刻ｔa2が求められる。そして、この時刻ｔa2を中心として時間範囲inttを有する区間が２つ目の第１積分区間[ｔas2、ｔae2]として決定される。こうして、加速中のモータ駆動速度を積分するための、２つの第１積分区間が決定される。

また、第１基準駆動速度ＲＶ1と駆動速度Ｖfbkとが比較結果されて、減速中のモータ駆動速度Ｖfbkが第１基準駆動速度ＲＶ1となる時刻ｔd1が求められる。そして、この時刻ｔd1を中心として時間範囲inttを有する区間が１つ目の第２積分区間[ｔds1、ｔde1]として決定される。また、第２基準駆動速度ＲＶ2と駆動速度Ｖfbkとが比較結果されて、減速中のモータ駆動速度Ｖfbkが第２基準駆動速度ＲＶ2となる時刻ｔd2が求めらる。そして、この時刻ｔd2を中心として時間範囲inttを有する区間が２つ目の第１積分区間[ｔds2、ｔde2]として決定される。なお、同図に示すように各積分区間は互いに重複しないように設定されている。

そして負荷演算部１２３では、２つの第１積分区間[ｔas1、ｔae1] 、[ｔas2、ｔae2]それぞれで駆動電流Ｉfbkを積分した積分値Ｉa(Ｉfbk)_1、Ｉa(Ｉfbk)_2を足し合わせた値が、加速中の駆動電流Ｉfbkの積分量Ｉfbk_aとして求められるとともに、２つの第２積分区間[ｔds1、ｔde1] 、[ｔds2、ｔde2]それぞれで駆動電流Ｉfbkを積分した積分値Ｉa(Ｉfbk)_1、Ｉa(Ｉfbk)_2を足し合わせた値が、減速中の駆動電流の積分量Ｉfbk_dとして求められる。さらに、２つの第１積分区間[ｔas1、ｔae1] 、[ｔas2、ｔae2]それぞれで角加速度を積分した積分値を足し合わせ値が、加速中の角加速度の積分量aclfbk_aとして求められるとともに、２つの第２積分区間[ｔds1、ｔde1] 、[ｔds2、ｔde2]それぞれで角加速度を積分した積分値を足し合わせた値が、減速中の角加速度の積分量aclfbk_dとして求められる。そして、次式
Ｊ＝Ｋt（Ｉfbk_a−Ｉfbk_d）／（aclfbk_a− aclfbk_d） 式（６）
に基づきイナーシャＪが同定される。

このように第２実施形態では、複数の基準駆動速度ＲＶ1、ＲＶ2のそれぞれについて第１積分区間と第２積分区間とが求められている。そして、複数の第１積分区間それぞれで、駆動電流Ｉfbkを積分した積分値の和が加速中の駆動電流Ｉfbkの積分量Ｉfbk_aとして求められるとともに、複数の第２積分区間それぞれで、駆動電流Ｉfbkを積分した積分値の和が減速中の駆動電流Ｉfbkの積分量Ｉfbk_ｄとして求められれている。こうして求まった加速中の駆動電流Ｉfbk_aの積分量と減速中の駆動電流Ｉfbk_dとの差から負荷が同定される。そして、第１実施形態でも説明したとおり、同じ基準駆動速度から求められた第１積分区間と第２積分区間（つまり、[ｔas1、ｔae1]と[ｔds1、ｔde1] 、あるいは[ｔds2、ｔde2]と[ｔas2、ｔae2]）とでは、上述したとおり速度変化パターンは近似する。したがって、加速中の駆動電流Ｉfbk_aと減速中の駆動電流Ｉfbk_dとの引算により、同じ基準駆動速度から求められて速度変化パターンが近似する第１積分区間および第２積分区間での駆動電流の積分値同士が引算されて、摩擦トルクの影響を従来技術と比較してより確実に相殺することができため、モータ速度が速度指令に追従する条件で駆動させるためのテスト駆動をする必要がない。こうして、テスト駆動を行なうことなく負荷同定のための作業を簡素化しつつ、摩擦の影響を抑制して単軸ロボット１０で用いられるモータ１１の負荷を高精度に同定することが可能となっている。さらに、かかる構成では、１つではなく複数の第１積分区間と第２積分区間とでモータ駆動力の積分量が求められている。したがって、より多くの駆動電流Ｉfbkの積分値に基づいて負荷同定ができ、負荷同定をより高精度に実行することができる。

第３実施形態
上記実施形態では、可動テーブルの移動方向（Ｘ方向）が水平向きとなるようにロボット１０が配置されている場合について説明したが、この第３実施形態では、可動テーブルの移動方向（Ｘ方向）が鉛直向きとなるようにロボット１０が配置されている場合に特に有効な負荷同定方法について説明する。つまり第３実施形態の負荷同定方法は、往路方向（＋Ｘ方向）および復路方向（−Ｘ方向）の両方向において可動テーブル３の加減速動作を実行することで、鉛直下向きに働く重力が負荷同定に与える影響を緩和するものである。また、往路方向（＋Ｘ方向）および復路方向（−Ｘ方向）の両方向において可動テーブル３の加減速動作を実行することで、ボールネジ軸などの微妙な歪みや加工精度などによって発生するクーロン摩擦のばらつきなどの影響を相殺することが出来る。このため、可動テーブルの移動方向（Ｘ方向）が水平向きとなる場合にも、より高精度な負荷同定が可能になる。なお、以下では上記実施形態と第３実施形態の差異点を中心に説明する。

図６は、モータを加減速するための速度指令と電流指令とを示す図であり、同図上段は速度指令Ｖrefの時間変化を示し、同図下段は電流指令Ｉrefの時間変化を示している。第３実施形態では往路方向へ加減速させる際の速度指令と復路方向へ加減速させる際の速度指令とは同じであるが、図６では往路方向と復路方向との違いを明確にするために、往路方向（＋Ｘ方向）を正方向とするとともに復路方向（−Ｘ方向）を負方向としており、往路方向（＋Ｘ方向）の速度指令と復路方向（−Ｘ方向）の速度指令とは時間軸に対して対称となっている。

上述の通り、可動テーブル３の移動方向（Ｘ方向）は鉛直方向に向けられている。したがって、電流指令Ｉrefからわかるように、第３実施形態では速度がゼロの場合の時刻（例えば時刻ゼロ）においても、重力に抗して可動テーブル３を静止させるため、駆動電流がモータ１１に供給されている。そして、かかる静止状態から往路方向の速度指令が出力された後、待機時間ＷＴだけ待って往路方向への速度指令が出力される。このように待機時間ＷＴを設ける理由は、往路方向の加減速の後で確実に可動テーブル３が静止したのを待って復路方向の加減速を実行するためである。

図７は、往路方向におけるモータの駆動速度と駆動電流値とを例示す図であり、同図上段は駆動速度Ｖfbkの時間変化を示し、同図下段は駆動電流指令Ｉfbkの時間変化を示している。また、図８は、復路方向におけるモータの駆動速度と駆動電流値とを例示す図であり、同図上段は駆動速度Ｖfbkの時間変化を示し、同図下段は駆動電流指令Ｉfbkの時間変化を示している。同図が示すように、往路において、モータ１１は、時間Ｔ1（第１時間）をかけて駆動速度Ｖ1（＝０）（第１駆動速度）から駆動速度Ｖ2（第２駆動速度）にまで加速された後、時間Ｔ2（第２時間）をかけて駆動速度Ｖ2（第２駆動速度）から速度Ｖ3（＝０）（第３駆動速度）まで減速されている。一方、復路において、モータ１１は、時間Ｔ3（第１時間）をかけて駆動速度Ｖ3（＝０）（第１駆動速度）から駆動速度Ｖ4（第２駆動速度）にまで加速された後、時間Ｔ4（第２時間）をかけて駆動速度Ｖ4（第２駆動速度）から速度Ｖ5（＝０）（第３駆動速度）まで減速されている。第３実施形態の積分区間決定部１１２には、２つの基準駆動速度ＲＶ1、ＲＶ2が往路および復路のそれぞれについて記憶されている。なお、復路方向を負方向としたことに対応して、図８では、基準駆動速度ＲＶ1、ＲＶ2はそれぞれ−ＲＶ1、−ＲＶ2と表されている。そして、往路方向および復路方向のそれぞれにおいて、第１積分区間と第２積分区間が決定される。各積分区間の決定方法は、第２実施形態と同様である。つまり、往路方向において、第１積分区間[ｔas1、ｔae1]、[ｔas2、ｔae2]が決定されるとともに、第２積分区間[ｔds1、ｔde1]、[ｔds2、ｔde2]が決定される。また、復路方向において、第１積分区間[ｔbs1、ｔbe1]、[ｔbs2、ｔbe2]が決定されるとともに、第２積分区間[ｔes1、ｔee1]、[ｔes2、ｔee2]が決定される。

そして負荷演算部１２３では、４つの第１積分区間[ｔas1、ｔae1] 、[ｔas2、ｔae2]、[ｔbs1、ｔbe1]、[ｔbs2、ｔbe2]それぞれで駆動電流Ｉfbkを積分した積分値Ｉa(Ｉfbk)_1、Ｉa(Ｉfbk)_2、Ｉb(Ｉfbk)_1、Ｉb(Ｉfbk)_2を足し合わせた値（なお、図７、図８のように復路方向を負方向とした場合は、Ｉb(Ｉfbk)_1、Ｉb(Ｉfbk)_2にマイナス１を乗じた後、これら４個の積分値を足し合わせた値）が、加速中の駆動電流Ｉfbkの積分量Ｉfbk_aとして求められる。さらに、４つの第２積分区間[ｔds1、ｔde1]、[ｔds2、ｔde2]、[ｔes1、ｔee1]、[ｔes2、ｔee2]それぞれで駆動電流Ｉfbkを積分した積分値Ｉd(Ｉfbk)_1、Ｉd(Ｉfbk)_2、Ｉe(Ｉfbk)_1、Ｉe(Ｉfbk)_2を足し合わせた値（なお、図７、図８のように復路方向を負方向とした場合は、Ｉe(Ｉfbk)_1、Ｉe(Ｉfbk)_2にマイナス１を乗じた後、これら４個の積分値を足し合わせた値）が、減速中の駆動電流Ｉfbkの積分量Ｉfbk_dとして求められる。

さらに、４つの第１積分区間[ｔas1、ｔae1] 、[ｔas2、ｔae2]、[ｔbs1、ｔbe1]、[ｔbs2、ｔbe2]それぞれで角加速度を積分した４個の積分値を足し合わせ値（なお、図７、図８のように復路方向を負方向とした場合は、復路方向における各積分値にマイナス１を乗じた後、これら４個の積分値を足し合わせた値）が、加速中の角加速度の積分量aclfbk_aとして求められる。また、４つの第２積分区間[ｔds1、ｔde1]、[ｔds2、ｔde2]、[ｔes1、ｔee1]、[ｔes2、ｔee2]それぞれで角加速度を積分した４個の積分値を足し合わせ値（なお、図７、図８のように復路方向を負方向とした場合は、復路方向における各積分値にマイナス１を乗じた後、これら４この積分値を足し合わせた値）が、減速中の角加速度の積分量aclfbk_dとして求められる。
そして、次式
Ｊ＝Ｋt（Ｉfbk_a−Ｉfbk_d）／（aclfbk_a− aclfbk_d） 式（６）
に基づきイナーシャＪが同定される。

このように第３実施形態では、往路と復路のそれぞれで加速中のモータ駆動力の積分量と減速中のモータ駆動力の積分量とが求めるとともに、これら加速中のモータ駆動力の積分量と減速中のモータ駆動力の積分量の差から負荷が同定される。したがって、ボールネジ軸の微妙な歪みや加工精度などによって発生するクーロン摩擦のばらつきなどの影響を抑制することができ、重力のように一方向にかかる力がロボット１０に作用しているような場合でも、かかる力が負荷の同定結果に与える影響を抑制することができる。

なお、本発明は上記実施形態に限定されるものではなく、その趣旨を逸脱しない限りにおいて上述したものに対して種々の変更を加えることが可能である。例えば、上記実施形態では、モータ１１によりボールネジを回転させて可動テーブル３を駆動するロボット１０に対して負荷同定を行なう場合について説明したが、所謂リニアモータにより可動テーブル３を駆動するロボットに対しても、同様に本発明の負荷同定技術を適用できる。

図９は、リニアモータを用いた単軸ロボットの一例を示す図である。この単軸ロボット２１１は、Ｘ軸方向に延びるケース２１２を備え、このケース２１２内にはリニアガイドと、このリニアガイドと同一方向に配された棒状のステータと、ステータが貫通する環状コイルを内蔵してリニアガイドに往復自在に支持されたスライダが設けられている。なお、同図では、リニアガイド、ステータおよび環状コイルは省略されている。また、このスライダには可動テーブル２５２が取り付けられている。そして、ステータと環状コイルとの磁気的相互作用により、環状コイルの移動に伴って可動テーブル２５２はＸ軸方向に移動することができる。つまり、ステータおよび環状コイル等がリニアモータとして機能する。

そして、基本的には、このようなリニアモータを用いた単軸ロボット２１１に対しても本発明を適用することができる。すなわち、可動テーブル２５２を加減速するとともに、可動テーブル２５２の駆動速度と基準駆動速度との比較から第１・第２積分区間を決定すし、第１・第２積分区間それぞれでの駆動電流の積分量の差から負荷を同定すればよい。但し、このようなリニアモータを用いた単軸ロボット２１１は、直進力Ｆにより可動テーブル２５２を駆動しており、回転力Ｔにより可動テーブルを駆動していた第１〜第３実施形態の単軸ロボット１０とは異なる。したがって、リニアモータを用いた単軸ロボット２１１において本発明の負荷同定方法を実施する場合には、イナーシャＪを求める式（４）の代わりに、リニアモータにかかる質量Ｍについての次式

を用いて負荷Ｍを求めることとなる。ここで、Ｆaは加速時にリニアモータが出力する力であり、Ｆdは減速時にリニアモータが出力する力であり、ｘはＸ軸方向における可動テーブル２５２の位置である。つまり、第１積分区間での駆動電流Ｉaの積分量と第２積分区間での駆動電流Ｉdの積分量との差に定数Ｋtを乗じた値を、第１積分区間での加速度の積分量と第２積分区間での加速度の積分量との差で割り算することで負荷Ｍが同定できる。

また、上記実施形態では、加速する際の最初の速度である第１駆動速度Ｖ1、Ｖ3はゼロに設定されているが、第１駆動速度Ｖ1、Ｖ3はゼロ以外の値であっても良い。

また、単軸ロボット以外のロボットについても本発明を適用することが可能であり、モータを駆動源とするロボット全般に対して本発明を適用することができる。

また、上記実施形態では、各積分区間は、駆動速度が基準駆動速度となる時刻を中心としていたが、当該時刻が積分区間の中心である必要は必ずしも無く、当該時刻が積分区間に含まれている限りにおいて当該時刻が積分区間の中心からずれるように積分区間を決定することもできる。

また、上記実施形態では、各積分区間は互いに重複しないように設定しているが、各積分区間を互いに重複するように設定してもよい。

また、第２実施形態では、積分区間の時間範囲inttは各積分区間で等しい。しかしながら、異なる基準駆動速度に基づいて求められた積分区間の間においては、時間範囲inttが異なっていても良い。

本実施形態にかかる単軸ロボットの一例を示す平面図である。 図１の単軸ロボットの動作を制御する制御システムのブロック図を示す図である。 モータを加減速するための速度指令と電流指令とを示す図である。 モータの駆動速度と駆動電流値とを例示す図である。 モータの駆動速度と駆動電流値とを例示す図である。 モータを加減速するための速度指令と電流指令とを示す図である。 往路方向におけるモータの駆動速度と駆動電流値とを例示す図である。 復路方向におけるモータの駆動速度と駆動電流値とを例示す図である。 リニアモータを用いた単軸ロボットの一例を示す図である。

符号の説明

１…ロボット本体
１０…単軸ロボット
１１…モータ
１１０…モータ制御部
１１２…速度制御部
１１３…電流制御部
１２０…負荷同定部
２１１…単軸ロボット
２５２…可動テーブル
３…可動テーブル
ＲＶ、ＲＶ1、ＲＶ2…基準駆動速度
Ｖ1、Ｖ2、Ｖ3、Ｖ4、Ｖ5…駆動速度
Ｘ…移動方向
intt…時間範囲

Claims (4)

1. モータにより駆動するロボットの前記モータにかかる負荷を同定する負荷同定方法であって、
   第１時間をかけて前記モータの駆動速度を第１駆動速度から第２駆動速度にまで加速した後で前記モータを第２時間をかけて第３駆動速度まで減速する、加減速動作を実行しながら、
   前記モータを加速している間に、前記モータの駆動速度が前記第１駆動速度および前記第３駆動速度よりも速く且つ前記第２駆動速度よりも遅い基準駆動速度となる時刻を含み且つ前記第１時間および前記第２時間よりも短い時間範囲を有する第１積分区間で、前記モータの駆動力を積分した値から加速中のモータ駆動力の積分量を求め、
   前記モータを減速している間に、前記モータの駆動速度が前記基準駆動速度となる時刻を含み且つ前記時間範囲を有する第２積分区間で、前記モータの駆動力を積分した値から、減速中のモータ駆動力の積分量を求め、
   前記加速中のモータ駆動力の積分量と、前記減速中のモータ駆動力の積分量との差に基づき前記負荷を同定することを特徴とする負荷同定方法。
2. 前記加減速動作を往路方向および当該往路方向と逆向きの復路方向のそれぞれについて実行し、
   前記往路方向に加速する前記モータの駆動速度が前記基準駆動速度となる時刻を含み且つ前記時間範囲を有する前記第１積分区間と、前記復路方向に加速する前記モータの駆動速度が前記基準駆動速度となる時刻を含み且つ前記時間範囲を有する前記第１積分区間とで、前記モータの駆動力を積分した値から、前記加速中のモータ駆動力の積分量を求め、
   前記往路方向に減速する前記モータの駆動速度が前記基準駆動速度となる時刻を含み且つ前記時間範囲を有する前記第２積分区間と、前記復路方向に減速する前記モータの駆動速度が前記基準駆動速度となる時刻を含み且つ前記時間範囲を有する前記第２積分区間とで、前記モータの駆動力を積分した値から、前記減速中のモータ駆動力の積分量を求める請求項１に記載の負荷同定方法。
3. 互いに異なる複数の前記基準駆動速度のそれぞれについて前記第１積分区間と前記第２積分区間とを求め、前記各第１積分区間で加速している前記モータの駆動力を積分した値から前記加速中のモータ駆動力の積分量を求め、前記各第２積分区間で減速している前記モータの駆動力を積分した値から前記減速中のモータ駆動力の積分量を求める請求項１または２に記載の負荷同定方法。
4. モータを駆動源とするロボットに接続する制御システムであって、
   第１時間をかけて前記モータの駆動速度を第１駆動速度から第２駆動速度にまで加速した後で前記モータを第２時間をかけて第３駆動速度まで減速させるモータ制御部と、
   前記モータを加速している間に、前記モータの駆動速度が前記第１駆動速度および前記第３駆動速度よりも速く且つ前記第２駆動速度よりも遅い基準駆動速度となる時刻を含み且つ前記第１時間および前記第２時間よりも短い時間範囲を有する第１積分区間で、前記モータの駆動力を積分した値から加速中のモータ駆動力の積分量を求め、
   前記モータを減速している間に、前記モータの駆動速度が前記基準駆動速度となる時刻を含み且つ前記時間範囲を有する第２積分区間で、前記モータの駆動力を積分した値から減速中のモータ駆動力の積分量を求め、
   前記加速中のモータ駆動力の積分量と前記減速中のモータ駆動力の積分量との差に基づき前記負荷を同定する負荷同定部とを備えることを特徴とするロボットの制御システム。
   

Images