JP6245858B2

JP6245858B2 - 制御装置

Info

Publication number: JP6245858B2
Application number: JP2013132288A
Authority: JP
Inventors: 重人水浦
Original assignee: Daihen Corp
Current assignee: Daihen Corp
Priority date: 2013-06-25
Filing date: 2013-06-25
Publication date: 2017-12-13
Anticipated expiration: 2033-06-25
Also published as: JP2015006705A

Description

本発明は、減速機の角度伝達誤差に起因するマニピュレータの手先位置の軌跡誤差を低減する制御装置に関する。

産業用ロボットで広く用いられている減速機には、機械構造に起因する角度伝達誤差が存在する。ここでの角度伝達誤差とは、減速機の入力側に接続された動力機構（例えば、サーボモータ等）の角度と、減速機の出力側に接続された負荷の角度との間に生ずる周期的な誤差のことである。例えば、動力側を任意の角度からΔθだけ回転させても、負荷側の回転量がΔθにはならないのは、この角度伝達誤差によるものである。

減速機の角度伝達誤差は、産業用ロボットのロボット手先位置（ＴＣＰ）軌跡精度に影響し、特に低速度でＴＣＰを移動させるアプリケーションでは、減速機の角度伝達誤差がＴＣＰ軌跡精度により強く影響することが知られている。

そのような角度伝達誤差を補償する方法として、外界センサにて計測されたＴＣＰと、制御装置の内部センサにて取得されたロボット各軸の位置指令、位置フィードバックより、補正振幅と各軸の位置フィードバックとの関係を数式モデル化し、その数式モデルから得られる補正信号を用いて位置指令を補正することによって、角度伝達誤差によるＴＣＰ軌跡の誤差を低減する手法が提案されている（例えば、特許文献１参照）。

特開２０１１−２２４６６２号公報

上述の従来技術においては、角度伝達誤差によるＴＣＰの「うねり」を外界センサで計測するとともに、各軸の位置指令と位置フィードバックを計測し、位置指令とＴＣＰのうねりとの関係式を数値モデル化し、このモデルから得られる補正信号を各軸の指令位置に合成することで、ロボットＴＣＰの軌跡精度を向上させている。しかしながら、その従来技術では、外界センサにて計測されるＴＣＰの「うねり」情報と、ロボット制御装置の内部センサより取得される位置指令及び位置フィードバックとの間に生じる時間差によって、減速機の角度伝達誤差の数式モデルに誤差が生じる。具体的には、外界センサから得られるうねりの振幅量と、ロボット各軸の位置指令との関連が時間軸でずれてしまうという問題があった。

本発明は、上記問題点を解決するためになされたものであり、外界センサを使用せず、ロボットの内部センサから取得される情報を用いて減速機の角度伝達誤差の数式モデル化をより高精度に実現でき、角度伝達誤差をより適切に低減させることができる制御装置を提供することを目的とする。

上記目的を達成するため、本発明による制御装置は、減速機を介してモータにより駆動される関節によって連結された複数のアームを有するマニピュレータを制御する制御装置であって、モータの位置指令値を受け付ける指令値受付部と、モータのフィードバックを受け付けるフィードバック受付部と、減速機の角度伝達誤差の周波数が記憶される記憶部と、モータを単軸動作させる単軸動作制御部と、単軸動作制御部がモータを単軸動作させている際に受け付けられたフィードバックをフーリエ変換するフーリエ変換部と、フーリエ変換部によってフーリエ変換された周波数領域におけるフィードバックにおいて、記憶部で記憶されている周波数に応じた振幅である誤差振幅が小さくなるように、角度伝達誤差の補償で用いられる振幅である補償振幅を取得する振幅取得部と、指令値受付部が受け付けた位置指令値と、位置指令値に応じてフィードバック受付部が受け付けた位置フィードバックとを用いて、減速機の角度伝達誤差の位相を取得する位相取得部と、記憶部で記憶されている周波数と、振幅取得部が取得した補償振幅と、位相取得部が取得した位相とを用いて、位置指令値における減速機の角度伝達誤差成分を補償するための補償量を算出する補償量算出部と、補償量算出部が算出した補償量を用いて、位置指令値の角度伝達誤差成分の補償を行う補償部と、を備え、振幅取得部は、補償後のフィードバックに応じた誤差振幅が小さくなるように補償振幅を取得する、ものである。
このような構成により、外界センサを用いないで角度伝達誤差の数式モデルを構築することができ、その数式モデルを用いて角度伝達誤差を低減させることができる。また、内部センサから得られる情報を用いて数式モデルを構築しているため、従来技術のように、外界センサで取得された情報と内部センサによって取得された情報との間に生じる時間差が発生せず、その時間差に応じた誤差が数式モデルに生じないというメリットもある。

また、本発明による制御装置では、単軸動作制御部は、モータを一定速度で単軸動作させてもよい。
このような構成により、誤差振幅を取得するための処理や、位相を取得するための処理が容易になりうる。

また、本発明による制御装置では、記憶部では、標準的な補償振幅である標準振幅も記憶されており、振幅取得部は、標準振幅を、補償後のフィードバックに応じた誤差振幅が小さくなるように変更した補償振幅を取得してもよい。
このような構成により、標準振幅を用いて補償振幅を取得できるため、より容易に目的とする補償振幅を取得することができるようになる。

本発明による制御装置によれば、外界センサを用いずに角度伝達誤差に起因する軌跡誤差を低減させることができ、産業用ロボットの作業品質を向上させることができる。

本発明の実施の形態１による制御装置の構成を示すブロック図同実施の形態によるサーボコントローラの構成を示すブロック図同実施の形態による制御装置の動作を示すフローチャート同実施の形態による制御装置の動作を示すフローチャート同実施の形態による位相の取得について説明するためのグラフを示す図

以下、本発明による制御装置について、実施の形態を用いて説明する。なお、以下の実施の形態において、同じ符号を付した構成要素及びステップは同一または相当するものであり、再度の説明を省略することがある。

（実施の形態１）
本発明の実施の形態１による制御装置について、図面を参照しながら説明する。本実施の形態による制御装置は、外界センサを用いずに角度伝達誤差をモデル化し、そのモデルを用いて角度伝達誤差を補償するものである。

図１は、本実施の形態による産業用ロボット１００の構成を示すブロック図である。本実施の形態による産業用ロボット１００は、制御装置１と、サーボコントローラ２と、マニピュレータ３とを備える。制御装置１は、モータにより駆動される関節によって連結された複数のアームを有するマニピュレータ３を、サーボコントローラ２を介して制御する。そのマニピュレータ３において、各モータとアームとは減速機を介して接続されている。また、直列に接続されたアームの先端に、手先効果器（エンドエフェクタ）が設けられていてもよい。マニピュレータ３は、例えば、垂直多関節ロボットのマニピュレータであってもよく、水平多関節ロボットのマニピュレータであってもよい。また、そのマニピュレータ３の軸数は問わない。また、制御装置１は、ティーチングプレイバック方式によりマニピュレータ３を制御してもよく、または、自律方式によりマニピュレータ３を制御してもよい。本実施の形態による産業用ロボット１００は、例えば、搬送ロボットであってもよく、溶接ロボットであってもよく、組立ロボットであってもよく、塗装ロボットであってもよく、または、その他の用途のロボットであってもよい。図１では、説明の便宜上、１個のサーボコントローラ２のみを示しているが、通常、産業用ロボット１００は、マニピュレータ３の軸数と同数のサーボコントローラを備えている。したがって、サーボコントローラ２は、各軸に対応するサーボコントローラの集合であると考えてもよい。サーボコントローラ２の詳細な構成については、図２を用いて後述する。

図１で示されるように、制御装置１は、位置指令生成部１１と、指令値受付部１２と、フィードバック受付部１３と、単軸動作制御部１４と、フーリエ変換部１５と、記憶部１６と、振幅取得部１７と、位相取得部１８と、補償量算出部１９と、補償部２０とを備える。

位置指令生成部１１は、モータの位置指令値を生成し、指令値受付部１２と補償量算出部１９と補償部２０とに渡す。位置指令生成部１１は、例えば、教示情報等を用いることによって、モータの位置を算出し、その位置を示す位置指令値を指令値受付部１２と補償量算出部１９と補償部２０とに渡してもよい。なお、モータの位置は、厳密にはモータの角度であってもよい。また、位置指令生成部１１は、単軸動作制御部１４による制御に応じて、位置指令値を生成し、出力してもよい。

指令値受付部１２は、モータの位置指令値を受け付ける。指令値受付部１２は、受け付けた位置指令値のうち、後段の処理で必要になる期間に応じた位置指令値を図示しない記録媒体で一時的に記憶してもよい。

フィードバック受付部１３は、モータのフィードバックを受け付ける。そのモータは、指令値受付部１２が受け付けた位置指令値に対応するモータと同じである。そのモータのフィードバックは、通常、指令値受付部１２が受け付けたモータの位置指令値に応じた位置フィードバックである。なお、フィードバック受付部１３は、速度フィードバックをも受け付けてもよい。モータの位置フィードバックは、モータの現在位置であり、モータの速度フィードバックは、モータの現在速度である。フィードバック受付部１３は、受け付けたフィードバックのうち、後段の処理で必要になる期間に応じたフィードバックを図示しない記録媒体で一時的に記憶してもよい。

単軸動作制御部１４は、モータを単軸動作させる。単軸動作とは、マニピュレータ３を駆動する複数軸のうち、ある軸のみを動作させることである。したがって、単軸動作時には、単軸動作制御部１４は、あるモータのみを動作させてもよい。その単軸動作時に、単軸動作制御部１４は、モータを一定速度で単軸動作させてもよく、または、そうでなくてもよい。本実施の形態では、前者の場合について主に説明する。なお、ある軸について単軸動作を行う場合には、単軸動作制御部１４は、その軸から手先までのロボットアームが水平となるように単軸動作を行うことが好適である。単軸動作を行う軸に対する重力トルクが略一定となるからである。

フーリエ変換部１５は、単軸動作制御部１４がモータを単軸動作させている際に受け付けられたフィードバックをフーリエ変換する。なお、厳密に言えば、フーリエ変換部１５は、位置（角度）に応じて変化する位置フィードバックをフーリエ変換する。ここで、フィードバック受付部１３が受け付けるのは、位置フィードバックの時間変化である。したがって、フーリエ変換部１５は、モータの位置と、位置フィードバックとの関係（例えば、後述する図５の実線のグラフで示される関係）を取得し、それに対してフーリエ変換を行ってもよい。そのフーリエ変換の結果、フーリエ変換部１５は、位置フィードバックの周波数と、その周波数に応じた振幅との関係を取得することができる。位置フィードバックには、通常、角度伝達誤差に起因するうねり成分が含まれている。したがって、フーリエ変換をすることによって、後段の処理において、そのうねり成分の振幅を特定できるようになる。なお、周波数は、時間の周波数（ｓｅｃ^−１）ではなく、位置（角度）の周波数である。したがって、その周波数の単位は、例えば、角度がラジアン（ｒａｄ）である場合には、ｒａｄ^−１となり、角度が度（ｄｅｇ）である場合には、ｄｅｇ^−１となる。

ここで、モータの位置と、位置フィードバックとの関係を取得する方法について簡単に説明する。フーリエ変換部１５は、位置フィードバックの時間変化（時間と位置フィードバックとの対応関係）と、位置指令値の時間変化（時間と位置指令値との対応関係）とを用い、媒介変数である時間を消去することによって、位置と位置フィードバックとの関係を取得できる。なお、フーリエ変換部１５が上述したのと同様のフーリエ変換結果を得る方法は問わない。例えば、フーリエ変換部１５は、上述した位置フィードバックの時間変化に対して、時間領域におけるフーリエ変換を行い、その結果と、位置指令値の時間変化とを用いることによって、上述したのと同様のフーリエ変換結果を取得してもよい。ただし、そのようなフーリエ変換結果の取得を行う場合には、単軸動作中の速度が一定である必要がある。また、単軸動作時の位置指令値は通常、あらかじめ分かっているため、フーリエ変換部１５は、指令値受付部１２が受け付けた位置指令値を用いるのではなく、あらかじめ図示しない記録媒体で記憶されている位置指令値を用いてフーリエ変換を行ってもよい。また、単軸動作時にモータを一定速度で動作させる場合には、位置指令値に代えて、その速度を図示しない記録媒体で記憶しておき、フーリエ変換部１５は、その速度を用いてフーリエ変換を行ってもよい。なお、ここでは、フーリエ変換部１５が、位置フィードバックをフーリエ変換する場合について説明したが、前述のように、フィードバック受付部１３によって速度フィードバックをも受け付けられる場合には、その速度フィードバックをフーリエ変換してもよい。その場合には、例えば、フーリエ変換部１５は、モータの位置と、速度フィードバックとの関係を取得し、その取得した関係についてフーリエ変換を行ってもよい。なお、単軸動作が一定速度で行われる場合には、フーリエ変換部１５は、例えば、速度フィードバックを時間領域においてフーリエ変換し、その結果と、位置指令値の時間変化とを用いることによって、フーリエ変換結果を取得してもよい。また、フーリエ変換部１５は、フーリエ変換の結果を図示しない記録媒体で記憶してもよい。

記憶部１６では、減速機の角度伝達誤差の周波数が記憶される。その周波数は、前述のように通常、位置（角度）における周波数である。減速機の角度伝達誤差の周波数は、その減速機の機械構造に応じて一意に決定されるため、その機械構造に応じた周波数が記憶部１６に蓄積される。また、その周波数は、減速機の機械構造の種類、すなわち減速機の型式（種類）ごとに異なるため、記憶部１６では、例えば、減速機の型式を示す情報と、その型式の減速機の周波数とが対応付けられて記憶されていてもよい。また、記憶部１６で記憶されている周波数は、結果として周波数を知ることができる情報であれば、周波数以外の情報、例えば周期や角周波数であってもよい。したがって、周波数以外の周期等が記憶部１６で記憶されている場合であっても、周波数が記憶されていると考えることができる。また、記憶部１６では、その記憶部１６で記憶されている周波数に応じた減速機の標準的な補償振幅である標準振幅も記憶されていてもよい。補償振幅については後述する。記憶部１６での記憶は、ＲＡＭ等における一時的な記憶でもよく、あるいは、長期的な記憶でもよい。記憶部１６は、所定の記録媒体（例えば、半導体メモリや磁気ディスク、光ディスクなど）によって実現されうる。

振幅取得部１７は、フーリエ変換部１５によってフーリエ変換された周波数領域におけるフィードバックにおいて、記憶部１６で記憶されている周波数に応じた振幅である誤差振幅が小さくなるように、角度伝達誤差の補償で用いられる振幅である補償振幅を取得する。記憶部１６で複数の周波数が記憶されている場合には、振幅取得部１７は、誤差振幅の取得対象の減速機の型式に対応する周波数を読み出してもよい。そして、振幅取得部１７は、フーリエ変換後の周波数領域におけるフィードバックにおいて、読み出した周波数に対応する誤差振幅を特定する。なお、振幅取得部１７は、後述する補償が行われた補償後のフィードバックに応じた誤差振幅が小さくなるように、補償振幅を取得するものとする。補償後のフィードバックに応じた誤差振幅は、その補償によっても打ち消すことができなかった角度伝達誤差成分の値である。したがって、その誤差振幅が小さくなるようにすることによって、角度伝達誤差を適切に打ち消すことができるようになる。振幅取得部１７は、例えば、その誤差振幅があらかじめ決められたしきい値よりも小さくなるように、補償振幅を取得してもよい。その補償振幅の取得は、例えば、次のように行われる。まず、振幅取得部１７が、補償振幅を取得する。振幅取得部１７は、補償振幅の初期値として、記憶部１６で記憶されている標準振幅を取得してもよい。減速機の型式に応じて補償振幅は似たような値になると考えられるため、減速機の型式ごとの標準振幅を用いることによって、補償振幅が適切な値により早く収束するようになると考えられる。一方、標準振幅が記憶部１６で記憶されていない場合には、振幅取得部１７は、補償振幅の初期値として、ランダムな補償振幅を取得してもよい。そして、その補償振幅を用いて算出された補償量を用いた角度伝達誤差の補償を行いながら単軸動作が行われ、それに応じた誤差振幅が特定される。その誤差振幅がしきい値より小さければ、その時点の補償振幅が最終的な補償振幅となる。一方、その誤差振幅がしきい値より小さくなければ、振幅取得部１７は、新たな補償振幅を取得し、上述の処理を繰り返す。その新たな補償振幅は、例えば、標準振幅が、補償後のフィードバックに応じた誤差振幅が小さくなるように変更された補償振幅であってもよい。具体的には、振幅取得部１７は、補償振幅「α・Ａ」や、補償振幅「Ａ＋β」を取得してもよい。ここで、Ａは、標準振幅であり、α、βは、標準振幅の変更のために用いられる実数のパラメータである。産業用ロボット１００の個体差によって補償振幅の値は変わりうるが、通常、あまり大きくは変わらないと考えられる。したがって、そのパラメータαやβに、ロボットの個体差に応じた範囲をあらかじめ決めておき、振幅取得部１７は、その範囲内でαやβを変化させることによって、補償振幅を取得してもよい。このようにすることで、より容易に適切な補償振幅を取得することができるようになる。なお、誤差振幅を取得するために用いられるフィードバックは、指令値受付部１２が受け付ける位置指令値に応じたフィードバックである位置フィードバックでなくてもよい。具体的には、速度フィードバックに応じた誤差振幅が特定されてもよい。誤差振幅は、角度伝達誤差成分が残っているかどうかを確認するために用いられるものだからである。なお、位置フィードバックに応じた誤差振幅のしきい値と、速度フィードバックに応じた誤差振幅のしきい値とは異なっていてもよい。また、振幅取得部１７は、位置フィードバックと速度フィードバックとの両方を用いて補償振幅を取得してもよい。その場合には、振幅取得部１７は、例えば、両フィードバックの誤差振幅がそれぞれしきい値より小さくなるように、補償振幅を取得してもよい。

位相取得部１８は、指令値受付部１２が受け付けた位置指令値と、その位置指令値に応じてフィードバック受付部１３が受け付けた位置フィードバックとを用いて、減速機の角度伝達誤差の位相を取得する。角度伝達誤差が存在しない場合、すなわち、角度伝達誤差が存在しない場合の理想的な位置指令値θ_ｃｍｄと、位置フィードバックθ_ａｃｔとの関係は、図５のグラフにおける一点鎖線のようになる。一方、角度伝達誤差が存在する場合には、位置フィードバックが図５のグラフにおける実線のように、理想的な位置フィードバックに対してうねりの成分を有することになる。そのうねりの成分のずれを示すものが位相である。したがって、位相取得部１８は、図５のグラフで示されるように、位置指令値が入力されてから、位置フィードバックθ_ａｃｔが、理想的な位置フィードバックと交わるまでの位置（角度）の変化である位相φを取得してもよい。または、位相取得部１８は、例えば、位置指令値θ_ｃｍｄがサーボコントローラ２に入力されてから、その位置指令値に応じた位置フィードバックθ_ａｃｔが位置指令値θ_ｃｍｄと一致するまでの時間を測定し、その時間を、それに応じた位置（角度）に変換することによって、減速機の角度伝達誤差の位相φを取得してもよい。なお、一定速度の単軸動作が行われる場合には、その測定した時間に速度を掛けることによって位相φを算出できる。一方、一定速度でない単軸動作が行われる場合には、その測定した時間にわたって速度を積分することによって位相φを算出できる。

補償量算出部１９は、記憶部１６で記憶されている周波数と、振幅取得部１７が取得した振幅と、位相取得部１８が取得した位相とを用いて、位置指令値における減速機の角度伝達誤差成分を補償するための補償量を算出する。具体的には、補償量算出部１９は、次式のように補償量を算出してもよい。このように、本実施の形態では、外界センサを用いることなく、内部センサから得られる値を用いて数式モデルを構築することができる。
補償量＝−Ｂ・ｓｉｎ（ω・θ_ｃｍｄ＋φ）

ここで、Ｂは、振幅取得部１７が取得した補償振幅であり、位置（角度）の次元を有している。また、ω（＝２πｆ）は、角周波数である。ただし、ｆは、記憶部１６で記憶されている周波数である。また、θ_ｃｍｄは、位置指令値であり、φは、位相取得部１８が取得した位相である。したがって、補償量算出部１９は、図示しない記録媒体で記憶されている上記式を読み出し、補償振幅Ｂ等に値を代入することによって、補償量を算出してもよい。その算出された補償量は、図示しない記録媒体で記憶されてもよい。

補償部２０は、補償量算出部１９が算出した補償量を用いて、位置指令値の角度伝達誤差成分の補償を行う。その補償は、位置指令生成部１１が生成した位置指令値に、角度伝達誤差を打ち消すように補償量を加算または減算することによって行われてもよい。加算または減算は、例えば、補償量の全体に−１を掛けるどうかによって異なるため、角度伝達誤差が低減するように適宜、決定されるものである。補償部２０による補償後の位置指令値は、サーボコントローラ２に入力される。

図２は、サーボコントローラ２の詳細な構成を示すブロック図である。本実施の形態によるサーボコントローラ２は、速度指令生成器２１と、電流指令生成器２２と、トルク指令生成器２３とを備える。

速度指令生成器２１は、モータの位置フィードバック（現在位置）と、位置指令生成部１１から受け取った位置指令値とに応じて速度指令値を生成する。速度指令生成器２１は、モータの位置フィードバックが位置指令値に近づくようにフィードバック制御をするための速度指令値を生成する。

電流指令生成器２２は、モータの速度フィードバック（現在速度）と、速度指令生成器２１が生成した速度指令値とに応じて電流指令値を生成する。電流指令生成器２２は、モータの現在速度が速度指令値に近づくようにフィードバック制御をするための電流指令値を生成する。

トルク指令生成器２３は、モータの電流フィードバック（現在電流）と、電流指令生成器２２が生成した電流指令値とに応じてトルク指令値を生成し、モータに出力する。トルク指令生成器２３は、モータの現在電流が電流指令値に近づくようにフィードバック制御をするためのトルク指令値を生成する。モータは、このトルク指令値によって動作する。

なお、これらのフィードバック制御はすでに公知であり、その詳細な説明を省略する。また、モータの電流フィードバックは、トルク指令生成器２３に入力される。また、モータの速度フィードバックは、電流指令生成器２２に入力される。また、モータの位置フィードバックは、速度指令生成器２１に入力される。なお、モータの速度フィードバックは、モータの位置フィードバックが時間微分されたものであってもよい。また、マニピュレータ３が複数のモータを有する場合には、そのモータごとに図２で示されるサーボコントローラ２が存在すると考えてもよい。

なお、図１では、サーボコントローラ２に一の位置指令値を出力し、サーボコントローラ２から一のフィードバックを受け取る場合について示しているが、これは説明の便宜のためであり、制御装置１は、マニピュレータ３が有する軸数、すなわちモータの数だけ位置指令値を出力し、その数だけフィードバックを受け取ってもよい。すなわち、制御装置１の処理は、モータごとに行われてもよい。

次に、制御装置１の動作について図３，図４のフローチャートを用いて説明する。図３のフローチャートは、補償量の算出のために用いる振幅や位相を取得する制御装置１の動作を示すフローチャートである。この処理は、例えば、産業用ロボット１００の製造後に工場等において一度だけ行われてもよく、その産業用ロボット１００の減速機が交換されるなどのように、ロボットの構成が変更された際にも行われてもよい。

（ステップＳ１０１）単軸動作制御部１４は、単軸動作を行うための制御を行う。例えば、単軸動作制御部１４は、単軸動作を行うモータが一定速度で動作するための位置指令値を出力するように、位置指令生成部１１を制御してもよい。その結果、単軸動作を行うための位置指令値が位置指令生成部１１によって生成され、その位置指令値に応じて、あるモータが単軸動作を行う。なお、この場合には、補償部２０による補償は行われないものとする。

（ステップＳ１０２）指令値受付部１２は、位置指令値を受け付け、図示しない記録媒体に蓄積する。また、フィードバック受付部１３は、位置フィードバックを受け付け、図示しない記録媒体に蓄積する。

（ステップＳ１０３）単軸動作制御部１４は、単軸動作の制御を終了するかどうか判断する。そして、終了する場合には、ステップＳ１０４に進み、単軸動作の制御を継続する場合には、ステップＳ１０１に戻る。なお、単軸動作制御部１４は、例えば、あらかじめ決められた時間だけ単軸動作を行った場合に、単軸動作の制御を終了すると判断してもよく、その他のイベントの発生に応じて単軸動作の制御を終了すると判断してもよい。

（ステップＳ１０４）位相取得部１８は、指令値受付部１２がステップＳ１０２で受け付けた位置指令値と、フィードバック受付部１３がステップＳ１０２で受け付けた位置フィードバックとを用いて、角度伝達誤差の位相を取得する。
なお、ステップＳ１０１〜Ｓ１０４では、角度伝達誤差の補償を行わない単軸動作において、補償量を算出するために必要な位相が算出される。

（ステップＳ１０５）振幅取得部１７は、記憶部１６で記憶されている標準振幅を取得する。その標準振幅は、補償振幅の初期値である。

（ステップＳ１０６）単軸動作制御部１４は、単軸動作を行うための制御を行う。その結果、単軸動作を行うための位置指令値が位置指令生成部１１によって生成され、その位置指令値に応じて、あるモータが単軸動作を行う。なお、そのモータは、ステップＳ１０１で単軸動作を行ったモータである。また、この場合には、後述するように、補償部２０による補償が行われるものとする。

（ステップＳ１０７）指令値受付部１２は、位置指令値を受け付け、図示しない記録媒体に蓄積する。

（ステップＳ１０８）補償量算出部１９は、ステップＳ１０４で取得された位相と、ステップＳ１０５またはステップＳ１１５で取得された最新の補償振幅と、記憶部１６で記憶されている周波数と、位置指令生成部１１が生成した位置指令値とを用いて、上述の式で示されるように、補償量を算出する。

（ステップＳ１０９）補償部２０は、位置指令生成部１１が生成した位置指令値を、補償量算出部１９が算出した補償量を用いて補正し、サーボコントローラ２に渡す。

（ステップＳ１１０）フィードバック受付部１３は、サーボコントローラ２から位置フィードバックを受け付け、図示しない記録媒体に蓄積する。

（ステップＳ１１１）単軸動作制御部１４は、単軸動作の制御を終了するかどうか判断する。そして、終了する場合には、ステップＳ１１２に進み、単軸動作の制御を継続する場合には、ステップＳ１０６に戻る。なお、単軸動作制御部１４は、例えば、あらかじめ決められた時間だけ単軸動作を行った場合に、単軸動作の制御を終了すると判断してもよく、その他のイベントの発生に応じて単軸動作の制御を終了すると判断してもよい。

（ステップＳ１１２）フーリエ変換部１５は、フィードバック受付部１３が受け付けた位置フィードバックをフーリエ変換する。この処理において、フーリエ変換部１５は、前述のように、指令値受付部１２が受け付けた位置指令値を用いてもよい。

（ステップＳ１１３）振幅取得部１７は、フーリエ変換後の周波数領域におけるフィードバックにおいて、記憶部１６で記憶されている周波数に応じた誤差振幅を取得する。

（ステップＳ１１４）振幅取得部１７は、取得した誤差振幅が、あらかじめ決められているしきい値よりも小さいかどうか判断する。そして、誤差振幅がしきい値よりも小さい場合には、補償振幅や位相を取得する一連の処理は終了となり、小さくない場合には、ステップＳ１１５に進む。

（ステップＳ１１５）振幅取得部１７は、誤差振幅が小さくなるようにするための補償振幅を取得する。そして、ステップＳ１０６に戻る。なお、誤差振幅を小さくするために、補償振幅を増加させればよいのか、または減少させればよいのかが分かっている場合には、振幅取得部１７は、そのように変化させた補償振幅を取得するものとする。一方、そうでない場合には、振幅取得部１７は、例えば、ランダムに補償振幅を変更してもよい。

なお、ステップＳ１０５〜Ｓ１１５では、角度伝達誤差に応じた位置フィードバックの変化が小さくなるように、補償量を算出するために用いられる補償振幅が調整される。したがって、この調整後の補償振幅を用いることによって、角度伝達誤差の影響を低減した制御を実現できることになる。

また、図３のフローチャートでは、位相の取得を１回だけ行う場合について示しているが、そうでなくてもよい。例えば、ステップＳ１０１からステップＳ１０４までの位相の取得の処理を２回以上行い、その処理によって取得された２以上の位相の平均値としての位相を補償量の算出で用いてもよい。そのようにすることで、モデル精度を高めることができると考えられる。なお、位相を取得するタイミングは問わない。例えば、ステップＳ１１４でＹｅｓと判断された後に、位相の取得を行い、それまでに取得した位相との平均値を求め、その位相の平均値を用いた補償量によって、それ以降の補償を行ってもよい。

図４のフローチャートは、産業用ロボット１００の動作時における制御装置１の動作を示すフローチャートである。
（ステップＳ２０１）位置指令生成部１１は、位置指令値を生成するタイミングであるかどうか判断する。そして、位置指令値を生成するタイミングである場合には、ステップＳ２０２に進み、そうでない場合には、位置指令値を生成するタイミングとなるまでステップＳ２０１の処理を繰り返す。なお、位置指令生成部１１は、例えば、あらかじめ決められた時間間隔ごとに、位置指令値を生成すると判断してもよく、その他のタイミングで位置指令値を生成すると判断してもよい。

（ステップＳ２０２）位置指令生成部１１は、位置指令値を生成する。その位置指令値の生成は、例えば、教示情報に応じた位置指令値の生成であってもよい。

（ステップＳ２０３）補償量算出部１９は、ステップＳ１０４で取得された位相と、ステップＳ１０５またはステップＳ１１５で取得された最新の補償振幅と、記憶部１６で記憶されている周波数と、位置指令生成部１１が生成した位置指令値とを用いて、上述の式で示されるように、補償量を算出する。

（ステップＳ２０４）補償部２０は、位置指令生成部１１が生成した位置指令値における角度伝達誤差成分を、補償量算出部１９が算出した補償量で補償した位置指令値をサーボコントローラ２に渡す。そして、ステップＳ２０１に戻る。
なお、図４のフローチャートにおいて、電源オフや処理終了の割り込みにより処理は終了する。また、図４のフローチャートには、サーボコントローラ２に関する処理は含まれていないが、それらの処理は公知であり、その詳細な説明を省略する。

以上のように、本実施の形態による制御装置１によれば、外界センサを用いることなく角度伝達誤差を低減するための数式モデルを構築することができ、その数式モデルを用いて、角度伝達誤差の影響を低減することができる。その結果、軌跡精度の高い産業用ロボット１００を実現することが可能となる。なお、減速機の出力側の運動は、内部センサで観測することはできない。しかしながら、その減速機の出力側の運動は、減速機を経由して減速機の入力軸に結合されているサーボモータに伝達される。したがって、本実施の形態のように、サーボモータのフィードバックを観測することによって、減速機の角度伝達誤差に起因する出力軸の運動を間接的に観測することができ、その結果、外界センサを用いることなく角度伝達誤差に応じた数式モデルを構築することができる。また、角度伝達誤差の影響が小さくなるように補償振幅を調整し、その調整後の補償振幅を用いて角度伝達誤差の補償を行うため、時間軸でのずれのない数学モデルを構築することができ、より適切に角度伝達誤差の影響を低減できることになる。また、単軸動作時にモータを一定速度で動作させることによって、誤差振幅や位相の取得の処理が簡単になりうる。

また、本実施の形態では、フーリエ変換結果が、位置（角度）の周波数領域におけるフィードバックである場合について説明したが、そうでなくてもよい。一定速度の単軸動作が行われる場合には、フーリエ変換結果は、時間の周波数領域におけるフィードバックであってもよい。その場合には、振幅取得部１７は、記憶部１６で記憶されている周波数（ｒａｄ^−１）等に、単軸動作時の速度（ｒａｄ／ｓｅｃ）を掛けることによって、時間の周波数（ｓｅｃ^−１）を算出し、フーリエ変換結果において、その時間の周波数に対応する誤差振幅を取得してもよい。したがって、そのような場合には、フーリエ変換部１５は、フィードバック受付部１３が受け付けたフィードバックをそのままフーリエ変換してもよい。また、このようなフーリエ変換を行う場合には、ステップＳ１０７における位置指令値の受け付けの処理を行わなくてもよい。

また、上記実施の形態において、各処理または各機能は、単一の装置または単一のシステムによって集中処理されることによって実現されてもよく、あるいは、複数の装置または複数のシステムによって分散処理されることによって実現されてもよい。

また、上記実施の形態において、各構成要素間で行われる情報の受け渡しは、例えば、その情報の受け渡しを行う２個の構成要素が物理的に異なるものである場合には、一方の構成要素による情報の出力と、他方の構成要素による情報の受け付けとによって行われてもよく、あるいは、その情報の受け渡しを行う２個の構成要素が物理的に同じものである場合には、一方の構成要素に対応する処理のフェーズから、他方の構成要素に対応する処理のフェーズに移ることによって行われてもよい。

また、上記実施の形態において、各構成要素が実行する処理に関係する情報、例えば、各構成要素が受け付けたり、取得したり、選択したり、生成したり、送信したり、受信したりした情報や、各構成要素が処理で用いるしきい値や数式、アドレス等の情報等は、上記説明で明記していなくても、図示しない記録媒体において、一時的に、あるいは長期にわたって保持されていてもよい。また、その図示しない記録媒体への情報の蓄積を、各構成要素、あるいは、図示しない蓄積部が行ってもよい。また、その図示しない記録媒体からの情報の読み出しを、各構成要素、あるいは、図示しない読み出し部が行ってもよい。

また、上記実施の形態において、各構成要素等で用いられる情報、例えば、各構成要素が処理で用いるしきい値やアドレス、各種の設定値等の情報がユーザによって変更されてもよい場合には、上記説明で明記していなくても、ユーザが適宜、それらの情報を変更できるようにしてもよく、あるいは、そうでなくてもよい。それらの情報をユーザが変更可能な場合には、その変更は、例えば、ユーザからの変更指示を受け付ける図示しない受付部と、その変更指示に応じて情報を変更する図示しない変更部とによって実現されてもよい。その図示しない受付部による変更指示の受け付けは、例えば、入力デバイスからの受け付けでもよく、通信回線を介して送信された情報の受信でもよく、所定の記録媒体から読み出された情報の受け付けでもよい。

また、上記実施の形態において、制御装置に含まれる２以上の構成要素が通信デバイスや入力デバイス等を有する場合に、２以上の構成要素が物理的に単一のデバイスを有してもよく、あるいは、別々のデバイスを有してもよい。

また、上記実施の形態において、各構成要素は専用のハードウェアにより構成されてもよく、あるいは、ソフトウェアにより実現可能な構成要素については、プログラムを実行することによって実現されてもよい。例えば、ハードディスクや半導体メモリ等の記録媒体に記録されたソフトウェア・プログラムをＣＰＵ等のプログラム実行部が読み出して実行することによって、各構成要素が実現され得る。その実行時に、プログラム実行部は、記憶部や記録媒体にアクセスしながらプログラムを実行してもよい。また、そのプログラムは、サーバなどからダウンロードされることによって実行されてもよく、所定の記録媒体（例えば、ＣＤ−ＲＯＭなどの光ディスクや磁気ディスク、半導体メモリなど）に記録されたプログラムが読み出されることによって実行されてもよい。また、このプログラムは、プログラムプロダクトを構成するプログラムとして用いられてもよい。また、そのプログラムを実行するコンピュータは、単数であってもよく、複数であってもよい。すなわち、集中処理を行ってもよく、あるいは分散処理を行ってもよい。

また、本発明は、以上の実施の形態に限定されることなく、種々の変更が可能であり、それらも本発明の範囲内に包含されるものであることは言うまでもない。

以上より、本発明による制御装置によれば、角度伝達誤差の補償を行うことができ、マニピュレータの制御装置として有用である。

１制御装置
２サーボコントローラ
３マニピュレータ
１１位置指令生成部
１２指令値受付部
１３フィードバック受付部
１４単軸動作制御部
１５フーリエ変換部
１６記憶部
１７振幅取得部
１８位相取得部
１９補償量算出部
２０補償部
１００産業用ロボット

Claims

減速機を介してモータにより駆動される関節によって連結された複数のアームを有するマニピュレータを制御する制御装置であって、
前記モータの位置指令値を受け付ける指令値受付部と、
当該モータのフィードバックを受け付けるフィードバック受付部と、
前記減速機の角度伝達誤差の周波数が記憶される記憶部と、
前記モータを単軸動作させる単軸動作制御部と、
前記単軸動作制御部が前記モータを単軸動作させている際に受け付けられたフィードバックをフーリエ変換するフーリエ変換部と、
前記フーリエ変換部によってフーリエ変換された周波数領域におけるフィードバックにおいて、前記記憶部で記憶されている周波数に応じた振幅である誤差振幅が小さくなるように、角度伝達誤差の補償で用いられる振幅である補償振幅を取得する振幅取得部と、
前記指令値受付部が受け付けた位置指令値と、当該位置指令値に応じて前記フィードバック受付部が受け付けた位置フィードバックとを用いて、前記減速機の角度伝達誤差の位相を取得する位相取得部と、
前記記憶部で記憶されている周波数と、前記振幅取得部が取得した補償振幅と、前記位相取得部が取得した位相とを用いて、前記位置指令値における前記減速機の角度伝達誤差成分を補償するための補償量を算出する補償量算出部と、
前記補償量算出部が算出した補償量を用いて、前記位置指令値の角度伝達誤差成分の補償を行う補償部と、を備え、
前記振幅取得部は、補償後の前記フィードバックに応じた誤差振幅が小さくなるように前記補償振幅を取得する、制御装置。
前記フーリエ変換部は、位置指令値と、前記フィードバック受付部によって受け付けられたフィードバックとを用いて、前記モータの位置とフィードバックとの関係を取得し、当該関係に対してフーリエ変換を行い、
前記単軸動作制御部は、前記モータを一定ではない速度で単軸動作させる、請求項１記載の制御装置。
前記記憶部では、前記減速機の型式に応じた標準的な補償振幅である標準振幅も記憶されており、
前記振幅取得部は、当該標準振幅を、補償後の前記フィードバックに応じた誤差振幅が小さくなるように変更した補償振幅を取得する、請求項１または請求項２記載の制御装置。