JP2014065096A - 開始時点特定装置、制御装置、及び開始時点特定方法 - Google Patents

Abstract

【課題】動作を滑らかにするために連続した２個の速度波形に移動平均処理を行うと、指定した軌道の終点や内回り開始位置をマニピュレータが通過しない。
【解決手段】開始時点特定部２３は、教示点を示す教示情報が記憶される教示情報記憶部２１と、教示情報が示す第１の軌道に応じた速度の時間変化である第１の速度波形に対して、第１の軌道に続く第２の軌道に応じた速度の時間変化である第２の速度波形の開始時点を特定する開始時点特定部２３とを備える。その特定された開始時点を用いて接続された第１及び第２の速度波形に有限インパルス応答フィルタ処理を行って新たな速度波形を得る。この速度波形を用いてマニピュレータ３を制御すると、マニピュレータ３が、第１の軌道の終点や内回り開始位置等を通過する。すなわち、滑らかな動作を実現しながらも指定位置を通過することにより精度も確保することができる。
【選択図】図１

Description

本発明は、連続した軌道に応じた２個の速度波形の時間的な位置関係を決定する装置等に関する。

産業用ロボットにおいて、ある経路とその次の経路との間で移動方向や速さが急激に変化する場合には、その変化点で大きな加速度が発生する。その結果、振動が引き起こされ、所要の精度が得られなかったり、無理な力が加わることによってウェハやガラス基板等の搬送対象や機械本体の破損を招いたりするという問題が生じる。そのような問題を解決するため、移動平均処理などを用いて２個の経路の速度変化を滑らかにつなぐ方法が知られている（例えば、特許文献１参照）。

特開２００３−２４１８１１号公報

しかしながら、連続した第１及び第２の速度波形に移動平均処理を行った場合には、図１２Ａの破線の波形で示されるように、速度波形の接続部分が重ね合わされたようになる。その結果、図１２Ｂで示されるように、移動平均処理の内回り軌道を通ることになり、第１の軌道の終点ＥＰを通過しないことになる。なお、第１の軌道に対応する速度波形が第１の速度波形であり、第２の軌道に対応する速度波形が第２の速度波形であるとしている。以下の説明においても同様であるとする。また、図１２Ａにおいて、第１及び第２の速度波形は、厳密には３次元座標系における速度ベクトルの時間変化であり、それぞれの方向は異なっているが、説明の便宜上、速度ベクトルの大きさを同じグラフ上に示している。このことは、他の速度波形のグラフについても同様である。

また、内回りを行うように重ね合わされた第１及び第２の速度波形に移動平均処理を行った場合には、図１２Ｃの破線の波形で示されるように、速度波形の合成部分が重ね合わされたようになる。その結果、図１２Ｄで示されるように、第１の軌道の内回り開始距離よりも手前の位置ＭＳＰから移動平均処理の内回り軌道が開始されることになり、第１の軌道の内回り開始位置ＳＰを通過しないことになる。

ウェハや液晶基板等の搬送対象を搬送する搬送ロボットは、狭小な空間で搬送対象をスロットなどに搬送する必要があり、想定していない内回りが発生した場合には、干渉の発生するおそれがある。また、搬送対象をあらかじめ決められた位置に正確に載置するため、軌道の終点に到達することが必要な場合もあり、そのような場合に想定していない内回りが発生すると、搬送対象の正確な載置を行うことができなくなる。このことは、溶接ロボットや組み立てロボット等の搬送ロボット以外の産業用ロボットについても同様である。
一般的に言えば、移動平均処理等のフィルタ処理を行う場合であっても、マニピュレータが軌道のあらかじめ決められた位置を通過するようにしたいと言う要望があった。

本発明は、上記事情に応じてなされたものであり、移動平均処理等のフィルタ処理を行う場合であっても、マニピュレータがあらかじめ決められた位置を通過できるようにするための装置等を提供することを目的とする。

上記目的を達成するため、本発明による開始時点特定装置は、モータによって駆動される関節によって連結された複数のアームを有するマニピュレータの３個以上の教示点を示す教示情報が記憶される教示情報記憶部と、教示情報によって示される連続する２個の教示点間の第１の軌道に応じた速度の時間変化である第１の速度波形に対して、第１の軌道に続く第２の軌道に応じた速度の時間変化である第２の速度波形の開始時点を特定する開始時点特定部と、を備え、開始時点特定部は、特定した開始時点を用いて接続された第１及び第２の速度波形に対して有限インパルス応答フィルタ処理を１回以上実行した場合でも、マニピュレータが第１の軌道におけるあらかじめ決められた位置を通過するように、あらかじめ決められた位置に対応する時点より後の時点である第２の速度波形の開始時点を特定する、ものである。
このような構成により、第１の軌道のあらかじめ決められた位置をマニピュレータが通過するための制御で用いられる、第２の速度波形の開始時点を特定できる。その結果、例えば、干渉を防止したり、所定の位置への正確な移動を実現したりすることができる。

また、本発明による開始時点特定装置では、あらかじめ決められた位置は、第１の軌道の終点の教示点であり、開始時点特定部は、第１の速度波形において終点に到達する時点である終点時点から、有限インパルス応答フィルタの１段あたりの遅延時間に段数を掛けた時間を有限インパルス応答フィルタ処理の実行回数に応じて加算した時間だけ後の時点である第２の速度波形の開始時点を特定してもよい。
このような構成により、第１の軌道の終点をマニピュレータが通過するための制御で用いられる、第２の速度波形の開始時点を特定することができる。

また、本発明による開始時点特定装置では、あらかじめ決められた位置は、第１の軌道における内回り開始位置であり、開始時点特定部は、有限インパルス応答フィルタ処理を実行回数だけ実行した第１の速度波形における第２の速度波形の開始時点に応じた第１の軌道の位置から、第１の軌道の終点までの距離が、第１の軌道の終点から内回り開始位置までの距離である内回り開始距離と等しくなるように、第２の速度波形の開始時点を特定してもよい。
このような構成により、第１の軌道の内回り開始位置をマニピュレータが通過するための制御で用いられる、第２の速度波形の開始時点を特定することができる。

また、本発明による開始時点特定装置では、開始時点特定部は、有限インパルス応答フィルタ処理を実行回数だけ実行した第１の速度波形を積分した位置の時間変化を用いて、第１の軌道の終点から内回り開始距離だけ始点側の位置に応じた時点である第２の速度波形の開始時点を特定してもよい。
このような構成により、第１の速度波形に対して実際に有限インパルス応答フィルタ処理を行わなくても、第２の速度波形の開始時点を特定できる。その結果、例えば、第２の速度波形の開始時点を特定するための処理を軽くすることができる。

また、本発明による制御装置は、開始時点特定装置と、開始時点特定装置が特定した第２の速度波形の開始時点を用いて第１及び第２の速度波形が接続された速度波形に対して有限インパルス応答フィルタ処理を１回以上実行するＦＩＲフィルタ処理部と、ＦＩＲフィルタ処理部によって有限インパルス応答フィルタ処理の実行された速度波形を用いて、マニピュレータの各モータの制御を行うサーボコントローラを制御する軌道制御部と、を備えたものである。
このような構成により、第１の軌道のあらかじめ決められた位置をマニピュレータが通過するように制御できる。

本発明による開始時点特定装置等によれば、第１の軌道のあらかじめ決められた位置をマニピュレータが通過するための制御で用いられる、第２の速度波形の開始時点を特定できる。

本発明の実施の形態１による産業用ロボットの構成を示すブロック図 同実施の形態におけるＦＩＲフィルタ処理部の構成の一例を示す図 同実施の形態における開始時点の特定について説明するための図 同実施の形態におけるＦＩＲフィルタ処理前後の速度波形の一例を示す図 同実施の形態におけるＦＩＲフィルタ処理後の軌道の一例を示す図 同実施の形態における開始時点の特定について説明するための図 同実施の形態におけるＦＩＲフィルタ処理後の軌道の一例を示す図 同実施の形態における速度波形等の波形の一例を示す図 同実施の形態における位置、速度、加速度の波形の一例を示す図 同実施の形態における速度波形等の波形の一例を示す図 同実施の形態における位置、速度、加速度の波形の一例を示す図 同実施の形態における開始時点の特定について説明するための図 同実施の形態による制御装置の動作を示すフローチャート 同実施の形態による開始時点特定装置の動作を示すフローチャート 従来のＦＩＲフィルタ処理前後の速度波形を示す図 従来のＦＩＲフィルタ処理前後の軌道を示す図 従来のＦＩＲフィルタ処理前後の速度波形を示す図 従来のＦＩＲフィルタ処理前後の軌道を示す図

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

（実施の形態１）
本発明の実施の形態１によるマニピュレータの制御装置について、図面を参照しながら説明する。本実施の形態によるマニピュレータの制御装置は、ある軌道におけるあらかじめ決められた位置をマニピュレータが通過するように制御するため、その軌道に続く軌道に応じた速度波形の開始時点を特定するものである。

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

図１で示されるように、本実施の形態による制御装置１は、開始時点特定装置１１と、ＦＩＲフィルタ処理部１２と、軌道制御部１３とを備える。また、開始時点特定装置１１は、教示情報記憶部２１と、速度波形生成部２２と、開始時点特定部２３とを備える。

教示情報記憶部２１では、教示情報が記憶される。教示情報は、モータによって駆動される関節によって連結された複数のアームを有するマニピュレータ３の３個以上の教示点を示す情報である。この教示点は、通常、マニピュレータ３の先端である手先効果器の教示点である。また、教示情報は、教示点におけるマニピュレータ３の先端の姿勢をも示す情報であってもよい。すなわち、教示情報にマニピュレータ３の手先効果器の姿勢を示す情報が含まれていてもよい。また、各教示点には、その教示点において内回りをするかどうかを示すフラグ等の情報が設定されていてもよい。その内回りを行う場合に、内回り開始位置を示す情報、例えば、内回り開始位置の座標や、内回り開始距離が教示情報に含まれていてもよい。また、連続する２個の教示点を結ぶ軌道に応じた速さを示す情報が教示情報に含まれていてもよい。ここで、連続する２個の教示点とは、時間的に隣接した２個の教示点である。その速さを示す情報は、例えば、５０％や１００％等のように、最大の速さに対する割合であってもよく、速度の時間変化を示す速度波形であってもよい。本実施の形態では、速度波形が教示情報に含まれておらず、速度波形生成部２２が速度波形を生成する場合について主に説明する。

教示情報記憶部２１に教示情報が記憶される過程は問わない。例えば、記録媒体を介して教示情報が教示情報記憶部２１で記憶されるようになってもよく、通信回線等を介して送信された教示情報が教示情報記憶部２１で記憶されるようになってもよく、あるいは、入力デバイスを介して入力された教示情報が教示情報記憶部２１で記憶されるようになってもよい。教示情報記憶部２１での記憶は、ＲＡＭ等における一時的な記憶でもよく、あるいは、長期的な記憶でもよい。教示情報記憶部２１は、所定の記録媒体（例えば、半導体メモリや磁気ディスクなど）によって実現されうる。

速度波形生成部２２は、教示情報記憶部２１で記憶されている教示情報を用いて、連続する２個の教示点間の軌道に応じた速度波形を生成する。速度波形生成部２２は、教示情報で示される各軌道について、例えば、加速区間、等速区間、減速区間を有する台形状の速度波形を生成してもよい。加速区間や減速区間における加速度は、あらかじめ決められていてもよく、または、軌道の状況に応じて速度波形生成部２２が決定してもよい。また、例えば、教示情報において、ある軌道について１００％の速度が設定されている場合には、速度波形生成部２２は、等速区間において、あらかじめ決められた速度の１００％となるように、速度波形を生成してもよい。速度波形生成部２２は、生成した速度波形を教示情報記憶部２１に蓄積してもよく、または、図示しない記録媒体に蓄積してもよい。本実施の形態では、前者の場合について説明する。その蓄積の際に、速度波形に応じた軌道との対応が分かるように蓄積されることが好適である。

開始時点特定部２３は、教示情報記憶部２１で記憶されている教示情報によって示される連続した２個の軌道にそれぞれ対応した２個の速度波形のうち、時間的に前の速度波形である第１の速度波形に対して、第２の速度波形の開始時点を特定する。開始時点特定部２３は、その処理を、教示情報によって示される連続した複数の軌道のうち、連続した軌道について順次、繰り返して行うが、ここでは説明の便宜上、第１の軌道と、それに続く第２の軌道とに関する処理について説明する。ここで、各軌道は、教示情報によって示される、連続する２個の教示点間の軌道である。また、第１の軌道に応じた速度波形が第１の速度波形であり、第２の軌道に応じた速度波形が第２の速度波形であるとする。なお、ある軌道に応じた速度波形とは、その軌道をマニピュレータ３の先端が移動する際のマニピュレータ３の先端の速度の時間変化を示す波形である。

従来、この第２の速度波形の開始時点は、内回りを行わない場合には、第１の速度波形においてマニピュレータ３が第１の軌道の終点に到達した時点であり、内回りを行う場合には、第１の速度波形においてマニピュレータ３が第１の軌道の内回り開始位置に到達した時点である。一方、そのような開始時点を用いて第１及び第２の速度波形を接続し、その接続後の第１及び第２の速度波形に対して移動平均処理等の有限インパルス応答フィルタ処理を行った場合には、前述のように、あらかじめ決められた位置、例えば、軌道の終点や内回り開始位置をマニピュレータ３が通過しなくなる。そこで、開始時点特定部２３は、マニピュレータ３があらかじめ決められた位置を通過するように、第１及び第２の速度波形を接続する際の第２の速度波形の開始時点を特定する。すなわち、開始時点特定部２３は、特定した開始時点を用いて接続された第１及び第２の速度波形に対して有限インパルス応答フィルタ処理を１回以上実行した場合でも、マニピュレータ３が第１の軌道におけるあらかじめ決められた位置を通過するように第２の速度波形の開始時点を特定する。その第２の速度波形の開始時点は、通常、あらかじめ決められた位置に対応する時点、すなわち、あらかじめ決められた位置をマニピュレータ３が通過する時点より後の時点である。なお、あらかじめ決められた位置は、軌道の終点であってもよく、軌道の内回り開始位置であってもよく、または、その他の点であってもよい。本実施の形態では、内回りを行わない軌道では、あらかじめ決められた位置が軌道の終点であり、内回りを行う軌道では、あらかじめ決められた位置が内回り開始位置である場合について主に説明する。その結果、内回りを行わない軌道では、その軌道の終点をマニピュレータ３が通過することになり、内回りを行う軌道では、その軌道の内回り開始位置をマニピュレータ３が通過することになる。また、ある位置をマニピュレータ３が通過するとは、厳密には、マニピュレータ３のあらかじめ決められた点がその位置を通過することである。そのあらかじめ決められた点は、通常、マニピュレータ３の先端である。また、第２の速度波形の開始時点の特定は、例えば、第２の速度波形の開始時刻を特定することであってもよく、所定の時点を基準として、第２の速度波形の開始時点を特定することであってもよい。その所定の時点は、例えば、第１の速度波形において終点に到達する時点である終点時点であってもよく、第１の速度波形において内回り開始位置に到達する時点であってもよく、従来の開始時点であってもよく、または、その他の時点であってもよい。なお、所定の時点を基準とした第２の速度波形の開始時点の特定は、例えば、その所定の時点と、第２の速度波形の開始時点との間の相対的な時間を特定することであってもよい。また、従来の開始時点とは、例えば、内回りを行わない場合には、第１の軌道の終点に対応する第１の速度波形の時点であり、内回りを行う場合には、第１の軌道の内回り開始位置に対応する第１の速度波形の時点である。その従来の開始時点を基準とした第２の速度波形の開始時点の特定は、例えば、その従来の開始時点に対する開始時点の遅延時間の特定であってもよい。その従来の開始時点に対する開始時点の遅延時間とは、あらかじめ決められた位置をマニピュレータ３が通過するようにするために第２の速度波形が遅延される時間のことである。なお、開始時点特定部２３による開始時点の特定方法については後述する。

ＦＩＲフィルタ処理部１２は、開始時点特定装置１１が特定した第２の速度波形の開始時点を用いて第１及び第２の速度波形が接続された速度波形に対して、有限インパルス応答（ＦＩＲ：Ｆｉｎｉｔｅ Ｉｍｐｕｌｓｅ Ｒｅｓｐｏｎｓｅ）フィルタ処理を１回以上実行する。その有限インパルス応答フィルタ処理を実行する回数は、あらかじめ決められているものとする。そして、その回数がＦＩＲフィルタ処理部１２に設定されていてもよく、または、ＦＩＲフィルタ処理部１２がその回数に応じた構成となっていてもよい。また、第２の速度波形の開始時点を用いて第１及び第２の速度波形を接続するとは、第１の速度波形に対して、第２の速度波形の開始時点が開始時点特定部２３によって特定された時点となる時間的な位置関係で、両速度波形を接続したものである。内回りを行う場合には、通常、両波形が重ね合わされることになる。一方、内回りを行わない場合には、通常、両波形が間隔をおいてつながれることになる。

図２は、ＦＩＲフィルタ処理部１２の構成を示す図である。ＦＩＲフィルタ処理部１２は、Ｎ個の遅延器３１−１〜３１−Ｎと、入力値及びＮ個の遅延器３１−１〜３１−Ｎで遅延された値にそれぞれ乗算値を乗算するＮ＋１個の乗算器３２−０〜３２−Ｎと、乗算後の値を加算するＮ個の加算器３３−１〜３３−Ｎとを備えている。ここで、Ｎは２以上の整数である。このＮの値を、有限インパルス応答フィルタの段数やタップ数と呼ぶこともある。すなわち、図２の有限インパルス応答フィルタは、段数が「Ｎ」となる。Ｎ＋１個の乗算器３２−０〜３２−Ｎの各乗算値は、同じであってもよく、または、異なっていてもよい。なお、図２では、有限インパルス応答フィルタ処理を１回行うＦＩＲフィルタ処理部１２を示しているが、ＦＩＲフィルタ処理部１２が接続後の速度波形に対して有限インパルス応答フィルタ処理を行う回数は問わない。その回数は、例えば、１回であってもよく、または、２回以上であってもよい。また、ＦＩＲフィルタ処理部１２が複数回の有限インパルス応答フィルタ処理を行う場合に、各有限インパルス応答フィルタ処理における有限インパルス応答フィルタの段数は同じであってもよく、または、異なっていてもよい。また、ＦＩＲフィルタ処理部１２が複数回の有限インパルス応答フィルタ処理を行う場合に、各有限インパルス応答フィルタ処理における乗算器３２−０〜３２−Ｎが乗算する値である乗算値は同じであってもよく、または、異なっていてもよい。ここで、複数回の有限インパルス応答フィルタ処理が行われる場合であって、ＦＩＲフィルタ処理部１２がハードウェアで実現される場合には、ＦＩＲフィルタ処理部１２は、２個以上の有限インパルス応答フィルタ回路を有してもよい。また、複数回の有限インパルス応答フィルタ処理が行われる場合であって、ＦＩＲフィルタ処理部１２がソフトウェアで実現される場合には、ＦＩＲフィルタ処理部１２には、その回数と、各有限インパルス応答フィルタ処理における段数や各乗算器３２−０〜３２−Ｎの乗算値とが設定されていてもよい。また、ＦＩＲフィルタ処理部１２の各遅延器３１−１〜３１−Ｎの遅延時間は、通常、制御装置１の補間周期と同じであるが、そうでなくてもよい。前者の場合には、例えば、補間周期が１０ｍｓである場合に、各遅延器３１−１〜３１−Ｎの遅延時間も１０ｍｓとなる。本実施の形態では、ＦＩＲフィルタ処理部１２の各遅延器３１−１〜３１−Ｎの遅延時間が、制御装置１の補間周期と同じである場合について主に説明する。また、ＦＩＲフィルタ処理部１２による有限インパルス応答フィルタ処理の内容は問わない。例えば、有限インパルス応答フィルタ処理は、移動平均処理であってもよく、その他のフィルタ処理であってもよい。ＦＩＲフィルタ処理部１２が移動平均処理を行う場合には、例えば、各乗算器３２−０〜３２−Ｎは、入力された信号に１／（Ｎ＋１）を乗算してもよい。また、ＦＩＲフィルタ処理部１２が移動平均処理を行う場合には、Ｎ＋１個の乗算器３２−０〜３２−Ｎに代えて、最後段の加算器３３−Ｎの後段で１／（Ｎ＋１）を乗算する１個の乗算器を備えてもよい。このように、ＦＩＲフィルタ処理部１２は、実行する有限インパルス応答フィルタ処理に応じた構成とすればよい。なお、その有限インパルス応答フィルタ処理の構成はすでに公知であり、その説明を省略する。また、特定された第２の速度波形の開始時点を用いて接続された第１及び第２の速度波形は、厳密には、速度ベクトルの時間変化である。したがって、ＦＩＲフィルタ処理部１２は、その速度ベクトルの各成分に対して有限インパルス応答フィルタ処理を行う。

軌道制御部１３は、ＦＩＲフィルタ処理部１２によって有限インパルス応答フィルタ処理の実行された速度波形を用いて、サーボコントローラ２を制御する。具体的には、有限インパルス応答フィルタ処理の実行された速度波形を用いて、軌道制御部１３は、補間周期ごとの補間点の位置を算出する。この位置は、３次元座標系におけるマニピュレータ３の先端の位置である。また、軌道制御部１３は、その補間周期ごとのマニピュレータ３の先端の位置に対して逆キネマティクス変換を行い、マニピュレータ３の先端以外の各軸の位置指令値を算出する。このようにして、軌道制御部１３は、補間周期ごとのマニピュレータ３の全軸の位置指令値を算出でき、その位置指令値をサーボコントローラ２に出力することによって、サーボコントローラ２を制御する。なお、この軌道制御部１３による処理は、産業用ロボットにおけるサーボコントローラの制御としてすでに公知であり、その詳細な説明を省略する。

ここで、サーボコントローラ２について簡単に説明する。サーボコントローラ２は、制御装置１から、マニピュレータ３の各軸の位置の指令を受け取る。サーボコントローラ２は、その各軸の位置の指令に応じて、フィードバック制御により各軸のモータに対応するトルク指令値を生成する。マニピュレータ３の各モータは、このトルク指令値によって動作する。なお、このサーボコントローラ２によるフィードバック制御はすでに公知であり、その詳細な説明を省略する。

次に、開始時点特定部２３が第２の速度波形の開始時点を特定する処理について説明する。その第２の速度波形の開始時点を特定する処理について、あらかじめ決められた位置が、軌道の終点である場合と、内回り開始位置である場合とで、第２の速度波形の開始時点の特定方法が異なるため、それぞれに分けて説明する。なお、あらかじめ決められた位置は、軌道の終点及び内回り開始位置以外の位置であってもよい。

（１）あらかじめ決められた位置が軌道の終点である場合
第１の速度波形に１回以上の有限インパルス応答フィルタ処理を実行すると、図３で示されるように、第１の軌道の終点に対応する時点は、終点遅延時間ＥＤＴだけ遅れることになる。その終点遅延時間ＥＤＴは、有限インパルス応答フィルタ処理の１回の実行に応じた遅延時間を、有限インパルス応答フィルタ処理の実行回数に応じて加算した時間である。なお、有限インパルス応答フィルタ処理の１回の実行に応じた遅延時間は、有限インパルス応答フィルタの１段あたりの遅延時間「Ｔ」にｉ回目の有限インパルス応答フィルタ処理に応じた段数「Ｎ_ｉ」を掛けた時間「Ｔ×Ｎ_ｉ」となる。なお、各Ｎ_ｉは、２以上の整数である。その時間「Ｔ×Ｎ_ｉ」を、有限インパルス応答フィルタ処理の実行回数分だけ加算することによって、終点遅延時間ＥＤＴが算出される。したがって、ＥＤＴは、次のようになる。ここで、Ｍは、有限インパルス応答フィルタ処理の実行回数を示す１以上の整数である。

したがって、あらかじめ決められた位置が第１の軌道の終点の教示点である場合には、開始時点特定部２３は、第１の速度波形において終点に到達する時点である終点時点から、終点遅延時間ＥＤＴだけ後の時点である第２の速度波形の開始時点を特定する。その結果、第２の速度波形の開始時点は、図３で示されるように、初期の開始時点から、特定された開始時点まで遅延することになる。なお、その遅延の時間は、前述の終点遅延時間ＥＤＴである。また、終点時点は、有限インパルス応答フィルタ処理を行っていない第１の速度波形における終点に応じた時点である。また、有限インパルス応答フィルタ処理の実行の有無に関わらず、速度波形の開始時点は変わらないため、図３において、有限インパルス応答フィルタ処理を行っていない第２の速度波形を示している。そのようにして特定された第２の速度波形の開始時点を用いて第１及び第２の速度波形が接続され、有限インパルス応答フィルタ処理が行われると、図４Ａで示されるようになる。図４Ａにおいて、実線によって、有限インパルス応答フィルタ処理前の第１及び第２の速度波形が示されており、破線によって、第１及び第２の速度波形に有限インパルス応答フィルタ処理を実行した後の速度波形が示されている。図４Ａから分かるように、有限インパルス応答フィルタ処理前において、第１の軌道の終点に応じた時点と、第２の軌道の始点に応じた時点との期間が、終点遅延時間ＥＤＴとなっている。このように、第２の速度波形を終点遅延時間ＥＤＴだけ遅らせることによって、図４Ｂで示されるように、マニピュレータ３の先端は、内回り軌道を通ることなく、あらかじめ決められた位置である第１の軌道の終点ＥＰを通ることになる。

（２）あらかじめ決められた位置が内回り開始位置である場合
内回りを行う場合には、従来、図５で示されるように、内回り開始距離と面積Ｓ１とが等しくなる内回り開始時点ＳＴから第２の速度波形が重ね合わされる。ここで、内回り開始距離は、第１の軌道の終点から内回り開始位置までの距離である。一方、第１の速度波形に１回以上の有限インパルス応答フィルタ処理を実行すると、図５で示されるように、第１の軌道の終点に対応する終点時点は、ＥＴからＭＥＴになる。したがって、面積Ｓ１＝面積Ｓ２となるように、遅延した内回り開始時点ＭＳＴを特定すると、そのＭＳＴが、開始時点特定部２３が特定する第２の速度波形の開始時点となる。この場合には、第２の速度波形の開始時点は、有限インパルス応答フィルタ処理を行わない場合と比較して、内回り遅延時間ＳＤＴだけ遅延することになる。

したがって、あらかじめ決められた位置が内回り開始位置である場合には、開始時点特定部２３は、有限インパルス応答フィルタ処理を実行回数だけ実行した第１の速度波形における第２の速度波形の開始時点（ＭＳＴ）に応じた第１の軌道の位置から、第１の軌道の終点までの距離が内回り開始距離と等しくなるように、第２の速度波形の開始時点（ＭＳＴ）を特定する。そのようにして特定された第２の速度波形の開始時点を用いて第１及び第２の速度波形が接続され、有限インパルス応答フィルタ処理が行われると、図６で示されるように、マニピュレータ３の先端は、本来の内回り開始位置ＳＰから内回り軌道を通るようになり、想定している以上の内回りとなることを回避できる。

なお、第２の速度波形の開始時点ＭＳＴを特定するためには、開始時点特定部２３は、有限インパルス応答フィルタ処理後の第１の速度波形を知らなくてはならない。その処理後の第１の速度波形は、第１の速度波形に実際に有限インパルス応答フィルタ処理を実行することによって得られるが、開始時点特定部２３は、その処理よりも軽い処理によって第２の速度波形の開始時点ＭＳＴを特定してもよい。そのような第２の速度波形の開始時点の特定方法について、（Ａ）有限インパルス応答フィルタ処理を１回行う場合と、（Ｂ）２回行う場合と、（Ｃ）Ｍ回行う場合とに分けて説明する。なお、以下の説明では、説明の便宜上、有限インパルス応答フィルタ処理が移動平均処理であるとしている。

（Ａ）有限インパルス応答フィルタ処理を１回行う場合
有限インパルス応答フィルタ処理を１回行う場合について、図７Ａを用いて説明する。図７Ａ（ａ）は、第１の速度波形を示す図である。通常、第１の速度波形は、図７Ａ（ａ）で示されるように、加速区間、等速区間、減速区間を有する台形状のグラフである。その第１の速度波形を時間で微分した加速度波形は、図７Ａ（ｂ）で示されるようになる。ここで、ある波形に移動平均処理を行ってから微分することは、その波形を微分してから移動平均処理を行うことと同じである。また、図７Ａ（ｂ）で示されるような階段状の時間変化に対して移動平均処理を行うと、図７Ａ（ｃ）で示されるように、台形状の時間変化になる。例えば、時刻Ｔ１０１からＴ１０２までの期間が加速度Ａ１０１であり、それ以外が０である加速度波形に移動平均処理を行うと、移動平均処理後の加速度は、時刻Ｔ１０１で０となり、そこから等加加速度で変化して時刻Ｔ１０１＋Ｔ１でＡ１０１となって時刻Ｔ１０２まではＡ１０１であり、その後、等加加速度で変化して時刻Ｔ１０２＋Ｔ１で０となるように変化する。図７Ａ（ｃ）で示されるように、Ａ１０１が正である場合には、移動平均処理後の加速度は、上底の方が短い台形状となり、Ａ１０１が負である場合には、移動平均処理後の加速度は、下底の方が短い台形状となる。ここで、Ｔ１は、移動平均処理における１段あたりの遅延時間「Ｔ」に段数「Ｎ_１」を掛けた値である。すなわち、Ｔ１＝Ｔ×Ｎ_１である。したがって、図７Ａ（ａ）の第１の速度波形に移動平均処理を行った後の加速度波形は、図７Ａ（ｃ）で示されるようになる。この図７Ａ（ｃ）の加速度波形を積分すると速度波形が得られ、その速度波形を積分すると、位置の時間変化が得られる。すなわち、図７Ａ（ｃ）の時間変化のグラフを２回積分すると、位置の時間変化が得られる。なお、その位置の時間変化は、時間について１次の関数である加速度波形を時間で２回積分しているため、時間について３次の関数となる。その加速度、速度、位置のそれぞれの時間変化は、図７Ｂで示されるようになる。したがって、開始時点特定部２３は、上述のようにして位置の時間変化を算出し、その算出した位置の時間変化を用いることによって、第２の速度波形の開始時点ＭＳＴを特定することができる。その処理については後述する。

（Ｂ）有限インパルス応答フィルタ処理を２回行う場合
有限インパルス応答フィルタ処理を２回行う場合について、図８Ａを用いて説明する。図８Ａ（ａ）は、第１の速度波形を示す図である。その第１の速度波形を時間で微分した加速度波形は、図８Ａ（ｂ）で示されるようになる。また、その加速度波形に移動平均処理を行うと、図８Ａ（ｃ）で示されるようになる。また、その移動平均後の加速度波形を時間で微分した加加速度波形は、図８Ａ（ｄ）で示されるようになる。さらに、その加加速度波形に移動平均処理を行うと、図８Ａ（ｅ）で示されるようになる。なお、Ｔ１＝Ｔ×Ｎ_１であり、Ｔ２＝Ｔ×Ｎ_２である。したがって、この図８Ａ（ｅ）の時間変化のグラフを１回積分すると加速度波形が得られ、２回積分すると速度波形が得られ、３回積分すると位置の時間変化が得られる。なお、その位置の時間変化は、時間について１次の関数である加加速度波形を時間で３回積分しているため、時間について４次の関数となる。その加速度、速度、位置のそれぞれの時間変化は、図８Ｂで示されるようになる。したがって、開始時点特定部２３は、上述のようにして位置の時間変化を算出し、その算出した位置の時間変化を用いることによって、第２の速度波形の開始時点ＭＳＴを特定することができる。その処理については後述する。

（Ｃ）有限インパルス応答フィルタ処理をＭ回行う場合
有限インパルス応答フィルタ処理をＭ回行う場合には、波形を微分して移動平均処理を行うことを、速度波形からはじめてＭ回繰り返す。そして、その処理後の波形をＭ＋１回積分することによって、位置の時間変化を取得することができる。

なお、有限インパルス応答フィルタ処理を移動平均処理に限らない場合には、開始時点特定部２３は、第１の速度波形に対して、微分と、その微分後の波形に対する有限インパルス応答フィルタ処理に応じた変形とを有限インパルス応答フィルタ処理の実行回数（＝Ｍ回）だけ繰り返して行い、その繰り返した後の波形に対して積分を有限インパルス応答フィルタ処理の実行回数に１を加算した回数（＝Ｍ＋１回）だけ繰り返して第１の軌道に応じた位置の時間変化を算出してもよい。ここで、有限インパルス応答フィルタ処理に応じた変形とは、移動平均処理の場合には、１段あたりの遅延時間「Ｔ」に段数「Ｎ_ｉ」を掛けた値を用いて、波形を台形状に変形することであり、その他の有限インパルス応答フィルタ処理の場合には、波形に対して、その有限インパルス応答フィルタ処理に応じた変形を行うことである。

次に、算出された位置の時間変化を用いた第２の速度波形の開始時点の特定について説明する。算出された位置の時間変化が、図９で示されるものであったとする。すると、開始時点特定部２３は、内回り開始位置ＳＰを特定する。その特定は、例えば、第１の軌道の終点ＥＰから内回り開始距離だけ始点側の位置を特定することによって行うことができる。その後、開始時点特定部２３は、算出した位置の時間変化において、内回り開始位置ＳＰに応じた時点を特定することによって、有限インパルス応答フィルタ処理を実行回数だけ実行した後の第１の速度波形における第２の速度波形の開始時点、すなわち、内回り開始時刻を特定できる。その内回り開始時刻が、前述の遅延した内回り開始時点ＭＳＴである。このように、開始時点特定部２３は、有限インパルス応答フィルタ処理を実行回数だけ実行した第１の速度波形を積分した位置の時間変化を用いて、第１の軌道の終点から内回り開始距離だけ始点側の位置に応じた時点である第２の速度波形の開始時点を特定してもよい。なお、通常、内回り開始位置ＳＰは、第１の軌道における減速開始位置と、終点ＥＰとの間に存在するため、開始時点特定部２３は、減速開始位置以降についてのみ、位置の時間変化を算出してもよい。そうすることにより、計算量を削減できる。

なお、有限インパルス応答フィルタ処理をＭ回行う場合であって、各回の有限インパルス応答フィルタ処理における段数や各乗算器の乗算値が決まっている場合には、位置の時間変化は決まったものになる。ここで、Ｍは、上述した説明と同様に、１以上の整数であるとする。したがって、開始時点特定部２３は、図示しない記録媒体で記憶されている位置の時間変化の関数を用いて、第２の速度波形の開始時点を特定してもよい。その図示しない記録媒体には、例えば、有限インパルス応答フィルタ処理の実行回数ごとに、その位置の時間変化の関数が記憶されていてもよい。その場合には、開始時点特定部２３は、ＦＩＲフィルタ処理部１２が有限インパルス応答フィルタ処理を実行する回数に応じた位置の時間変化を読み出して、内回り開始位置に応じた開始時点の特定に用いてもよい。例えば、有限インパルス応答フィルタ処理である移動平均処理を１回行う場合には、その関数は、次のようになる。なお、Ｄは、内回り開始距離であり、Ｔ１は、移動平均処理における各段の遅延時間に段数を掛けたものである。また、−αは、減速区間における加速度であり、Ｔ３は、減速開始から速度が０になるまでの時間である。ここで、αは通常、正の値である。また、時間ｔは、移動平均処理後の速度波形における終点に対応する時点ＭＥＴから、過去に向かう方向にとっている。また、位置の関数Ｆ（ｔ）も、軌道の終点ＥＰを原点として、始点に向かうほど値が増加するように取っている。すなわち、ｔ＝０に対応する位置Ｆ（０）＝０を軌道の終点ＥＰであるとしている。

（ｉ）Ｄ＜αＴ１^２／６である場合
位置の時間変化の関数は、次のようになる。

したがって、Ｄ＝Ｆ（ｔ）をｔについて解くことによって、図５におけるＭＥＴからＭＳＴまでの時間を求めることができる。
（ｉｉ）αＴ１^２／６≦Ｄ＜αＴ１^２／６−αＴ１Ｔ３／２＋αＴ３^２／２である場合
位置の時間変化の関数は、次のようになる。

したがって、Ｄ＝Ｆ（ｔ）をｔについて解くことによって、図５におけるＭＥＴからＭＳＴまでの時間を求めることができる。
（ｉｉｉ）αＴ１^２／６−αＴ１Ｔ３／２＋αＴ３^２／２≦Ｄ≦αＴ３（Ｔ１＋Ｔ３）／２である場合
位置の時間変化の関数は、次のようになる。

したがって、Ｄ＝Ｆ（ｔ）をｔについて解くことによって、図５におけるＭＥＴからＭＳＴまでの時間を求めることができる。

このように、位置の時間変化に関する式をあらかじめ記憶しておくことによって、Ｔ１やＴ３、α、Ｄが分かれば、図５におけるＭＥＴからＭＳＴまでの時間を求めることができる。なお、ｔの方程式を解く際には、時間ｔが４次までであれば、解の公式を用いて解を求めてもよく、あるいは、数値解析により解を求めてもよく、時間ｔが５次以上であれば、数値解析により解を求めてもよい。また、上記説明では、減速区間に応じた位置の時間変化のみについて示しているが、それ以外の区間に応じた位置の時間変化も用いてもよい。

ここで、あらかじめ決められた位置が軌道の終点及び内回り開始位置でない場合について簡単に説明する。あらかじめ決められた位置が終点等でない場合には、開始時点特定部２３は、前述した内回り開始位置のときと同様にして、有限インパルス応答フィルタ処理を実行回数だけ実行した第１の速度波形における第２の速度波形の開始時点に応じた第１の軌道の位置から、第２の軌道の終点までの距離が、第１の軌道の終点からあらかじめ決められた位置までの距離と等しくなる時点を特定する。そして、第２の速度波形の初期の開始時点が、その特定した時点よりも後であれば、開始時点特定部２３は、その初期の開始時点を、第２の速度波形の開始時点として特定する。一方、その初期の開始時点が、特定した時点よりも前であれば、開始時点特定部２３は、その特定した時点を、第２の速度波形の開始時点とする。なお、初期の開始時点とは、有限インパルス応答フィルタ処理に応じた第２の速度波形の遅延を行う前の第２の速度波形の開始時点のことである。したがって、内回りを行わない場合には、第１の速度波形の終点時点であり、内回りを行う場合には、第１の速度波形の内回り開始時点ＳＴである。

また、ＦＩＲフィルタ処理部１２の各遅延器３１−１〜３１−Ｎの遅延時間が補間周期と異なる場合における第２の速度波形の開始時点の特定について簡単に説明する。その場合には、開始時点特定部２３は、前述のようにして開始時点を特定する。その開始時点をここでは、暫定開始時点と呼ぶことにする。その後、開始時点特定部２３は、その暫定開始時点よりも時間的に後であり、かつ、補間周期に合致する時点を、第２の速度波形の最終的な開始時点として特定する。具体的には、例えば、各遅延器３１−１〜３１−Ｎの遅延時間が７ｍｓであり、補間周期が１０ｍｓであったとする。また、ＦＩＲフィルタ処理部１２の段数Ｎ＝５であり、有限インパルス応答フィルタ処理は、１回だけ実行されるように設定されていたとする。すると、暫定終点遅延時間ＥＤＴ＝３５ｍｓとなるが、開始時点特定部２３は、その暫定終点遅延時間ＥＤＴ「３５ｍｓ」よりも大きく、補間周期１０ｍｓの倍数となる最終的な終点遅延時間ＥＤＴ＝４０ｍｓを求め、それを用いて第２の速度波形の開始時点を特定する。このようにすることで、遅延器の遅延時間と補間周期とが異なる場合であっても、マニピュレータ３があらかじめ決められた位置を通過するように制御することができる。

次に、制御装置１の動作について図１０のフローチャートを用いて説明する。
（ステップＳ１０１）速度波形生成部２２は、速度波形をまだ生成していない軌道のうち、時間的に最も前の軌道の情報を教示情報記憶部２１で記憶されている教示情報から読み出す。

（ステップＳ１０２）速度波形生成部２２は、ステップＳ１０１で読み出した軌道に対応する速度波形を生成し、教示情報記憶部２１に蓄積する。

（ステップＳ１０３）開始時点特定部２３は、教示情報記憶部２１で記憶されている教示情報に、次の軌道の情報が含まれているかどうか判断する。そして、含まれている場合には、ステップＳ１０４に進み、含まれていない場合には、すべての軌道について開始時点の特定が行われたことになるため、ステップＳ１０５に進む。

（ステップＳ１０４）開始時点特定部２３は、ステップＳ１０２で速度波形生成部２２が生成した速度波形である第１の速度波形において、次の速度波形である第２の速度波形の開始時点を特定する。開始時点特定部２３は、内回りが行われない場合には、第１の速度波形に対応する第１の軌道の終点をマニピュレータ３の先端が通過するように第２の速度波形の開始時点を特定し、内回りが行われる場合には、その第１の軌道における内回り開始位置をマニピュレータ３の先端が通過するように第２の速度波形の開始時点を特定してもよい。開始時点特定部２３は、特定した第２の速度波形の開始時点を、教示情報記憶部２１に蓄積する。その際に、第１の軌道の教示点に対応付けられて第２の速度波形の開始時点が蓄積されることが好適である。そして、ステップＳ１０１に戻る。

（ステップＳ１０５）ＦＩＲフィルタ処理部１２は、開始時点特定部２３が特定した第２の速度波形の開始時点を用いて接続された連続した２個の速度波形について、あらかじめ決められた有限インパルス応答フィルタ処理をＭ回実行する。その有限インパルス応答フィルタ処理を行った後の速度波形は、図示しない記録媒体で記憶されてもよい。なお、ＦＩＲフィルタ処理部１２は、すべての軌道に応じた速度波形について、この処理を行うものとする。

（ステップＳ１０６）軌道制御部１３は、有限インパルス応答フィルタ処理後の速度波形を用いて、補間周期ごとにマニピュレータ３の先端の位置を算出し、その算出した位置に対して逆キネマティクス処理を行うことによって、マニピュレータ３の各軸の位置を算出する。そして、軌道制御部１３は、その補間周期ごとに各軸の位置を示す位置指令値をサーボコントローラ２に出力する。その結果、マニピュレータ３は、サーボコントローラ２による制御によって、教示情報が示す教示点や内回り開始位置を通過するように順次、動作する。

なお、図１０のフローチャートでは、第２の速度波形の開始時点をすべて特定した後に、軌道の制御を行う場合について説明したが、そうでなくてもよい。第２の速度波形の開始時点の特定と、軌道の制御とを逐次的に実行してもよい。また、図１０のフローチャートでは、すべての速度波形に対して有限インパルス応答フィルタ処理を実行し、すべての軌道に対応する速度波形を生成した後に、その速度波形を用いて軌道の制御を行う場合について説明したが、そうでなくてもよい。有限インパルス応答フィルタ処理と、軌道の制御とを逐次的に実行してもよい。

次に、内回りが行われる場合において、第２の速度波形の開始時点を特定する動作について、図１１のフローチャートを用いて説明する。
（ステップＳ２０１）開始時点特定部２３は、第１の軌道に応じた位置の時間変化を、前述のようにして取得する。なお、その位置の時間変化は、例えば、位置の時間変化の式に加速度等を代入することによって取得してもよく、または、速度波形に対して、微分と、その微分後の波形に対する有限インパルス応答フィルタ処理に応じた変形とを、有限インパルス応答フィルタ処理の実行回数に応じて繰り返して行った結果の波形に積分を繰り返すことによって取得してもよい。その算出された位置の時間変化は、図示しない記録媒体で記憶されてもよい。なお、図１１のフローチャートでは、前述のように、第１の軌道の減速開始位置から終点までの範囲に応じた位置の時間変化が取得されるものとする。

（ステップＳ２０２）開始時点特定部２３は、ステップＳ２０１で算出された位置の時間変化の範囲に、内回り開始位置が含まれるかどうか判断する。そして、その範囲に内回り開始位置が含まれる場合には、ステップＳ２０３に進み、そうでない場合には、ステップＳ２０６に進む。

（ステップＳ２０３）開始時点特定部２３は、ニュートン法を用いて、内回り開始位置に対応する時点である、第２の速度波形の開始時点を特定する処理を行う。

（ステップＳ２０４）開始時点特定部２３は、ステップＳ２０３の処理において、第２の速度波形の開始時点を特定できたかどうか判断する。特定できた場合には、第２の速度波形の開始時点を特定する処理は終了となり、上位の処理に戻り、特定できなかった場合には、ステップＳ２０５に進む。

（ステップＳ２０５）開始時点特定部２３は、二分法を用いて、内回り開始位置に対応する時点である、第２の速度波形の開始時点を特定する。そして、上位の処理に戻る。

（ステップＳ２０６）開始時点特定部２３は、あらかじめ決められた位置が軌道の終点であるとして第２の速度波形の開始時点の特定を行う。ステップＳ２０２において、位置の時間変化の範囲に内回り開始位置が含まれない場合には、内回り開始位置等の設定において何らかのエラーが起こっている可能性が高いと考えられるからである。そして、上位の処理に戻る。

次に、本実施の形態による制御装置１の動作について、具体例を用いて説明する。この具体例において、教示情報記憶部２１に、次の教示情報が記憶されているものとする。
Ｐｏｓｉｔｉｏｎ（１）、Ｄｉｓｔ（１）
Ｐｏｓｉｔｉｏｎ（２）、Ｄｉｓｔ（２）
Ｐｏｓｉｔｉｏｎ（３）、Ｄｉｓｔ（３）
Ｐｏｓｉｔｉｏｎ（４）、Ｄｉｓｔ（４）
：
：

この教示情報において、Ｐｏｓｉｔｉｏｎ（Ｋ）は、マニピュレータ３の先端の位置を示す情報である。ここで、Ｋは１以上の整数であり、各教示点に対応している。また、Ｄｉｓｔ（Ｋ）は、Ｋ番目の教示点における内回り開始距離である。したがって、Ｄｉｓｔ（Ｋ）＝０の場合には、内回りを行わないことになり、Ｄｉｓｔ（Ｋ）＞０の場合には、内回りを行うことになる。なお、教示情報には、Ｋ番目の教示点について、マニピュレータ３の手先効果器の姿勢を示す情報Ｐｏｓｅ（Ｋ）が含まれていてもよいが、ここでは説明の便宜のため省略している。また、教示情報には、各軌道に応じたスピードの情報（例えば、５０％や１００％等）も含まれていてもよいが、ここでは省略している。したがって、この具体例では、すべて同じ最高速度の速度波形が生成されるものとする。また、１個目の教示点の内回り開始距離Ｄｉｓｔ（１）は０であるとする。また、この具体例では、Ｄｉｓｔ（２）＝０，Ｄｉｓｔ（３）＞０であるとする。

また、この具体例では、ＦＩＲフィルタの段数Ｎは、５であるとする。また、ＦＩＲフィルタ処理部１２の各乗算器３２−０〜３２−５は、１／６を乗算するものとする。すなわち、ＦＩＲフィルタ処理部１２は、移動平均処理を行うものとする。また、補間周期が１０ｍｓであり、ＦＩＲフィルタ処理部１２の各遅延器３１−１〜３１−５の遅延時間も１０ｍｓであるとする。また、ＦＩＲフィルタ処理部１２は、有限インパルス応答フィルタ処理を１回だけ行うものとする。

まず、速度波形生成部２２は、まだ速度波形を生成していない軌道の情報、すなわち、１個目の軌道であるＰｏｓｉｔｉｏｎ（１）からＰｏｓｉｔｉｏｎ（２）までの情報を読み出す（ステップＳ１０１）。具体的には、速度波形生成部２２は、Ｐｏｓｉｔｉｏｎ（１）と、Ｐｏｓｉｔｉｏｎ（２）とを読み出す（ステップＳ１０１）。そして、速度波形生成部２２は、１番目の教示点から２番目の教示点までの軌道に応じた速度波形Ｓｐｅｅｄ（２）を生成し、教示情報記憶部２１に蓄積する（ステップＳ１０２）。その結果、教示情報記憶部２１で記憶されている教示情報は次のようになる。
Ｐｏｓｉｔｉｏｎ（１）、Ｄｉｓｔ（１）
Ｐｏｓｉｔｉｏｎ（２）、Ｄｉｓｔ（２）、Ｓｐｅｅｄ（２）
Ｐｏｓｉｔｉｏｎ（３）、Ｄｉｓｔ（３）
：
：

次に、開始時点特定部２３は、新たな速度波形Ｓｐｅｅｄ（２）が蓄積されたことを検知すると、その次の軌道が存在するかどうか判断する（ステップＳ１０３）。この場合には、次のＰｏｓｉｔｉｏｎ（３）が存在するため、開始時点特定部２３は、次の軌道が存在すると判断し、次の軌道の開始時点を特定する処理を行う（ステップＳ１０４）。具体的には、開始時点特定部２３は、Ｄｉｓｔ（２）を読み出し、それが０であるかどうか判断する。この場合にはＤｉｓｔ（２）＝０であるため、開始時点特定部２３は、あらかじめ決められた位置が終点である場合の開始時点の特定を行う。すなわち、開始時点特定部２３は、移動平均処理の１回の実行に応じた遅延時間である１０ｍｓ×５＝５０ｍｓを算出する。そして、開始時点特定部２３は、次軌道の開始を５０ｍｓだけ遅らせることを示す遅延情報Ｄｅｌａｙ（２）＝５０ｍｓを生成し、教示情報記憶部２１で記憶されている教示情報の２番目の教示点に対応付けて蓄積する。なお、遅延情報Ｄｅｌａｙ（Ｋ）は、Ｓｐｅｅｄ（Ｋ）の終点時点に対するＳｐｅｅｄ（Ｋ＋１）の開始時点の遅延時間を示す情報である。その結果、教示情報記憶部２１で記憶されている教示情報は次のようになる。
Ｐｏｓｉｔｉｏｎ（１）、Ｄｉｓｔ（１）
Ｐｏｓｉｔｉｏｎ（２）、Ｄｉｓｔ（２）、Ｓｐｅｅｄ（２）、Ｄｅｌａｙ（２）
Ｐｏｓｉｔｉｏｎ（３）、Ｄｉｓｔ（３）
：
：

その後、速度波形生成部２２は、新たな遅延情報が蓄積されたことを検知すると、まだ速度波形を生成していない軌道の情報、すなわち、Ｐｏｓｉｔｉｏｎ（３）を読み出す（ステップＳ１０１）。そして、前回の速度波形の生成で読み出したＰｏｓｉｔｉｏｎ（２）と、このたび読み出したＰｏｓｉｔｉｏｎ（３）とを用いて、速度波形生成部２２は、２番目の教示点から３番目の教示点までの軌道に応じた速度波形Ｓｐｅｅｄ（３）を生成し、教示情報記憶部２１に蓄積する（ステップＳ１０２）。その結果、教示情報記憶部２１で記憶されている教示情報は次のようになる。
Ｐｏｓｉｔｉｏｎ（１）、Ｄｉｓｔ（１）
Ｐｏｓｉｔｉｏｎ（２）、Ｄｉｓｔ（２）、Ｓｐｅｅｄ（２）、Ｄｅｌａｙ（２）
Ｐｏｓｉｔｉｏｎ（３）、Ｄｉｓｔ（３）、Ｓｐｅｅｄ（３）
：
：

その後、開始時点特定部２３は、新たな速度波形Ｓｐｅｅｄ（３）が蓄積されたことを検知すると、その次の軌道が存在するかどうか判断する（ステップＳ１０３）。この場合には、次のＰｏｓｉｔｉｏｎ（４）が存在するため、開始時点特定部２３は、次の軌道が存在すると判断し、次の軌道の開始時点を特定する処理を行う（ステップＳ１０４）。具体的には、開始時点特定部２３は、Ｄｉｓｔ（３）を読み出し、それが０であるかどうか判断する。この場合にはＤｉｓｔ（３）＞０であるため、開始時点特定部２３は、あらかじめ決められた位置が内回り開始位置である場合の開始時点の特定を行う（ステップＳ２０１）。すなわち、開始時点特定部２３は、図示しない記録媒体から位置の時間変化に関する式を読み出し、加速度等を代入することによって、Ｓｐｅｅｄ（３）に移動平均処理を行って積分した関数に対応する位置の時間変化を取得する。その後、開始時点特定部２３は、位置の時間変化において、内回り開始位置に応じた時刻ＭＳＴを特定し、その時刻ＭＳＴから、軌道の終点に対応する時刻ＭＥＴまでの時間間隔を算出する（ステップＳ２０２〜Ｓ２０４）。そして、開始時点特定部２３は、１回の移動平均処理に応じた遅延時間５０ｍｓから、その算出した時間間隔を減算することによって、軌道の終点時点に対する次軌道の開始時点の遅延に関する遅延情報Ｄｅｌａｙ（３）を生成し、教示情報記憶部２１で記憶されている教示情報の３番目の教示点に対応付けて蓄積する。その遅延情報Ｄｅｌａｙ（３）は、図５のＥＴからＭＳＴまでの時間間隔を示すものである。その結果、教示情報記憶部２１で記憶されている教示情報は次のようになる。なお、Ｄｅｌａｙ（３）＞０の場合には、速度波形Ｓｐｅｅｄ（４）の開始時点が速度波形Ｓｐｅｅｄ（３）の終点時点よりも遅くなる。一方、Ｄｅｌａｙ（３）＜０の場合には、速度波形Ｓｐｅｅｄ（４）の開始時点が速度波形Ｓｐｅｅｄ（３）の終点時点よりも以前になり、両速度波形の重ね合わせが行われることになる。
Ｐｏｓｉｔｉｏｎ（１）、Ｄｉｓｔ（１）
Ｐｏｓｉｔｉｏｎ（２）、Ｄｉｓｔ（２）、Ｓｐｅｅｄ（２）、Ｄｅｌａｙ（２）
Ｐｏｓｉｔｉｏｎ（３）、Ｄｉｓｔ（３）、Ｓｐｅｅｄ（３）、Ｄｅｌａｙ（３）
Ｐｏｓｉｔｉｏｎ（４）、Ｄｉｓｔ（４）
：
：

このようにして、速度波形と、遅延情報との生成がすべての軌道について行われ、その後、ＦＩＲフィルタ処理部１２は、教示情報記憶部２１で記憶されている各遅延情報を用いて各速度波形を接続し、その接続後の速度波形に対して移動平均処理を行う（ステップＳ１０５）。また、軌道制御部１３は、移動平均処理後の速度波形を用いて、各軸の位置指令値を生成してサーボコントローラ２に渡す（ステップＳ１０６）。その結果、各軸に応じたモータが、位置指令値に応じた位置になるように制御され、マニピュレータ３が教示情報に応じた動作を行うことになる。なお、その動作において、内回りを行わない軌道については、手先効果器が軌道の終点を通過し、内回りを行う軌道については、手先効果器が軌道の内回り開始位置を通過することになる。

なお、この具体例では、速度波形の終点に対する差分によって次軌道の開始時点を特定する場合について説明したが、そうでなくてもよいことは言うまでもない。例えば、内回りを行う場合には、従来の内回り開始時点ＳＴから遅延した内回り開始時点ＭＳＴまでの内回り遅延時間ＳＤＴによって次軌道の開始時点を特定してもよい。

以上のように、本実施の形態による制御装置１によれば、各軌道のあらかじめ決められた位置である終点や内回り開始位置をマニピュレータ３が通過するように、速度波形を接続する際に用いられる開始時点を特定することができる。そして、その開始時点を用いて速度波形を接続することによって、有限インパルス応答フィルタ処理を実行しても、そのあらかじめ決められた位置をマニピュレータ３が通過するように制御できる。したがって、想定していない干渉が発生したり、搬送対象の載置位置がずれたりする事態を回避することができ、正確な移動を実現することができるようになる。また、有限インパルス応答フィルタ処理を行うことによって、移動方向や速さが急激に変化することを防止することもできる。

なお、本実施の形態では、開始時点特定装置１１が速度波形生成部２２を備える場合について説明したが、そうでなくてもよい。例えば、教示情報に速度波形が含まれる場合には、開始時点特定装置１１は、速度波形生成部２２を備えていなくてもよい。

また、本実施の形態では、制御装置１が開始時点特定装置１１を含んでいる場合について説明したが、そうでなくてもよい。開始時点特定装置１１と、速度波形に有限インパルス応答フィルタ処理を実行し、マニピュレータ３を制御する制御装置とは、異なる装置であってもよい。その場合には、例えば、開始時点特定装置１１は、あらかじめ決められた位置を通過するように、第２の速度波形の開始時点を特定し、その第２の速度波形の開始時点を教示情報に追加したり、マニピュレータ３を制御する制御装置に渡したりしてもよい。したがって、開始時点特定装置１１と、マニピュレータ３を制御する制御装置とが異なる装置である場合には、開始時点特定装置１１は、特定した第２の速度波形の開始時点を蓄積したり、制御装置に渡したりする図示しない出力部を備えていてもよい。また、開始時点特定装置１１と、マニピュレータ３を制御する制御装置とが異なる装置である場合に、開始時点特定装置１１の有する教示情報記憶部２１は、制御装置で記憶されている教示情報が一時的に記憶される記憶部であってもよく、あるいは、そうでなくてもよい。

また、本実施の形態では、あらかじめ決められた位置が内回り開始位置である場合に、開始時点特定部２３が、位置の時間変化を用いて、第２の速度波形の開始時点を数値解析によって特定する場合について主に説明したが、そうでなくてもよい。開始時点特定部２３は、可能な場合には、代数的に第２の速度波形の開始時点を算出してもよい。

また、本実施の形態では、開始時点特定部２３がすべての軌道についてあらかじめ決められた位置をマニピュレータ３が通過するように次軌道に応じた速度波形の開始時点を特定する場合について説明したが、そうでなくてもよい。あらかじめ決められた軌道についてのみ、その開始時点の特定を行ってもよい。終点や内回り開始位置を必ずしも通過しなくてもよい軌道もありうるからである。

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

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

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

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

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

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

以上より、本発明による開始時点特定装置等によれば、有限インパルス応答フィルタ処理を行う場合であっても、あらかじめ決められた位置をマニピュレータが通過するようにできるという効果が得られ、そのような制御を行うための速度波形の開始時点を特定する装置等として有用である。

１ 制御装置
２ サーボコントローラ
３ マニピュレータ
１１ 開始時点特定装置
１２ ＦＩＲフィルタ処理部
１３ 軌道制御部
２１ 教示情報記憶部
２２ 速度波形生成部
２３ 開始時点特定部
３１−１〜３１−Ｎ 遅延器
３２−０〜３２−Ｎ 乗算器
３３−１〜３３−Ｎ 加算器

Claims (6)

1. モータによって駆動される関節によって連結された複数のアームを有するマニピュレータの３個以上の教示点を示す教示情報が記憶される教示情報記憶部と、
   前記教示情報によって示される連続する２個の教示点間の第１の軌道に応じた速度の時間変化である第１の速度波形に対して、当該第１の軌道に続く第２の軌道に応じた速度の時間変化である第２の速度波形の開始時点を特定する開始時点特定部と、を備え、
   前記開始時点特定部は、特定した開始時点を用いて接続された前記第１及び第２の速度波形に対して有限インパルス応答フィルタ処理を１回以上実行した場合でも、前記マニピュレータが前記第１の軌道におけるあらかじめ決められた位置を通過するように、当該あらかじめ決められた位置に対応する時点より後の時点である前記第２の速度波形の開始時点を特定する、開始時点特定装置。
2. 前記あらかじめ決められた位置は、前記第１の軌道の終点の教示点であり、
   前記開始時点特定部は、前記第１の速度波形において終点に到達する時点である終点時点から、前記有限インパルス応答フィルタの１段あたりの遅延時間に段数を掛けた時間を当該有限インパルス応答フィルタ処理の実行回数に応じて加算した時間だけ後の時点である前記第２の速度波形の開始時点を特定する、請求項１記載の開始時点特定装置。
3. 前記あらかじめ決められた位置は、前記第１の軌道における内回り開始位置であり、
   前記開始時点特定部は、前記有限インパルス応答フィルタ処理を実行回数だけ実行した前記第１の速度波形における前記第２の速度波形の開始時点に応じた前記第１の軌道の位置から、前記第１の軌道の終点までの距離が、前記第１の軌道の終点から前記内回り開始位置までの距離である内回り開始距離と等しくなるように、前記第２の速度波形の開始時点を特定する、請求項１記載の開始時点特定装置。
4. 前記開始時点特定部は、前記有限インパルス応答フィルタ処理を実行回数だけ実行した前記第１の速度波形を積分した位置の時間変化を用いて、前記第１の軌道の終点から前記内回り開始距離だけ始点側の位置に応じた時点である前記第２の速度波形の開始時点を特定する、請求項３記載の開始時点特定装置。
5. 請求項１から請求項４のいずれか記載の開始時点特定装置と、
   前記開始時点特定装置が特定した前記第２の速度波形の開始時点を用いて前記第１及び第２の速度波形が接続された速度波形に対して前記有限インパルス応答フィルタ処理を１回以上実行するＦＩＲフィルタ処理部と、
   前記ＦＩＲフィルタ処理部によって有限インパルス応答フィルタ処理の実行された速度波形を用いて、前記マニピュレータの各モータの制御を行うサーボコントローラを制御する軌道制御部と、を備えた制御装置。
6. モータによって駆動される関節によって連結された複数のアームを有するマニピュレータの３個以上の教示点を示す教示情報が記憶される教示情報記憶部で記憶されている教示情報によって示される連続する２個の教示点間の第１の軌道に応じた速度の時間変化である第１の速度波形に対して、当該第１の軌道に続く第２の軌道に応じた速度の時間変化である第２の速度波形の開始時点を特定する開始時点特定ステップを備え、
   前記開始時点特定ステップでは、特定した開始時点を用いて接続された前記第１及び第２の速度波形に対して有限インパルス応答フィルタ処理を１回以上実行した場合でも、前記マニピュレータが前記第１の軌道におけるあらかじめ決められた位置を通過するように、当該あらかじめ決められた位置に対応する時点より後の時点である前記第２の速度波形の開始時点を特定する、開始時点特定方法。

Images