JP6046994B2 - 制御装置 - Google Patents

Description

本発明は、教示されたルートを曲線補間する制御装置等に関する。

従来、開始教示点から目標教示点までマニピュレータが移動するように制御する場合に、搬送対象の重量やアームの自重等に起因して発生するアームの撓み等の外乱を考慮して、両点間に経由教示点を設定し、その経由教示点をマニピュレータが通過するように制御することが行われていた。また、そのように経由教示点を通過した場合でも、マニピュレータが開始教示点から目標教示点まで移動する時間である目標時間で、開始教示点から目標教示点までを経由教示点を介して移動させる制御も行われていた（例えば、特許文献１参照）。

特開２００８−２２９７９６号公報

しかしながら、従来例の場合には、経由教示点を通過する際に動作方向が急激に変化することになるため、その経由教示点で大きな加速度が発生することがある。その結果、振動が引き起こされ、所要の精度が得られなかったり、無理な力が加わることによってウェハやガラス基板等の搬送対象や機械本体の破損を招いたりするという問題の生じるおそれがあった。

本発明は、上記課題を解決するためになされたものであり、外乱を考慮して設定された経由教示点に応じてマニピュレータを移動させる場合であっても、速度変化を滑らかにすることができる制御装置等を提供することを目的とする。

上記目的を達成するため、本発明による制御装置は、モータによって駆動される関節によって連結された複数のアームを有するマニピュレータを制御する制御装置であって、マニピュレータの移動の開始点である開始教示点、マニピュレータの移動の目標点である目標教示点、開始教示点から目標教示点までの間において、両点間をマニピュレータが移動するように外乱を考慮して設定された１以上の経由教示点、及びマニピュレータが開始教示点から目標教示点まで移動する時間である目標時間を含む教示情報が記憶される教示情報記憶部と、開始教示点、１以上の経由教示点、及び目標教示点を順に直線で結ぶ教示ルートの各経由教示点について、経由教示点と経由教示点の直前の教示点との間の線分上の点である曲線補間開始点から、経由教示点と経由教示点の直後の教示点との間の線分上の点である曲線補間終了点までの間を曲線補間した開始教示点から目標教示点までのルートである曲線補間教示ルートを生成する曲線補間部と、開始教示点から目標教示点までを直線で結ぶ目標ルートを区画する複数の目標セクションごとに目標時間が分割されたセクション目標時間で、セクション目標時間に対応する目標セクションに応じた曲線補間教示ルートの区画である教示セクションをマニピュレータが移動するように、モータの制御を行うサーボコントローラを制御する軌道制御部と、を備えたものである。
このような構成により、開始教示点から目標教示点までマニピュレータが移動する経路において、移動方向が急激に変化しないようにすることができる。その結果、速度変化を滑らかにすることができ、搬送対象や機械本体の破損を防止することができる。また、開始教示点から目標教示点までの２点間の移動時間である目標時間によってマニピュレータの移動を制御することができるため、マニピュレータの移動の制御が容易になる。

また、本発明による制御装置では、曲線補間は、２次ベジェ曲線による補間であってもよい。
このような構成により、２次ベジェ曲線によって、曲線補間開始点から曲線補間終了点までを滑らかにつなぐことができる。また、２次ベジェ曲線は、パラメトリック曲線であるため、目標ルート上の点に対応する曲線軌道上の点を算出する処理が容易になる。

また、本発明による制御装置では、目標セクションの境界点は、曲線補間開始点に対応する目標ルート上の点及び／または曲線補間終了点に対応する目標ルート上の点であってもよい。
このような構成により、目標セクションの境界点が曲線補間された曲線軌道の端点となるため、その境界点を別途、算出する必要がなくなり、処理負荷を軽減することができる。

また、本発明による制御装置では、目標セクションの境界点は、経由教示点から目標ルートへの垂線の足であってもよい。

また、本発明による制御装置では、セクション目標時間は、目標ルートを加速及び減速を含んで移動する場合の目標セクションごとの移動時間であってもよい。
このような構成により、例えば、開始教示点や目標教示点の付近において加速や減速を行う場合であっても、適切に対応することができる。

本発明による制御装置等によれば、外乱を考慮して設定された経由教示点に応じてマニピュレータを移動させる場合であっても、速度変化を滑らかにすることができる。

本発明の実施の形態１による産業用ロボットの構成を示すブロック図 同実施の形態における目標ルートと教示ルートとの一例を示す図 同実施の形態における目標ルートと曲線補間教示ルートとの一例を示す図 同実施の形態における曲線補間について説明するための図 同実施の形態における曲線補間について説明するための図 同実施の形態における目標セクションと教示セクションとの一例を示す図 同実施の形態における目標セクションと教示セクションとの一例を示す図 同実施の形態における目標セクションと教示セクションとの一例を示す図 同実施の形態における目標セクションと教示セクションとの一例を示す図 同実施の形態における時刻と距離との対応の一例を示す図 同実施の形態における時刻と距離との対応の一例を示す図 同実施の形態による制御装置の動作を示すフローチャート

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

（実施の形態１）
本発明の実施の形態１によるマニピュレータの制御装置について、図面を参照しながら説明する。本実施の形態によるマニピュレータの制御装置は、外乱を考慮して設定された経由教示点に応じてマニピュレータの移動を制御するものである。

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

図１で示されるように、制御装置１は、教示情報記憶部１１と、曲線補間部１２と、軌道制御部１３とを備える。
教示情報記憶部１１では、教示情報が記憶される。その教示情報には、教示点と、目標時間とが含まれる。その教示点には、開始教示点と、目標教示点と、経由教示点とがある。開始教示点は、マニピュレータ３の移動の開始点の教示点である。目標教示点は、マニピュレータ３の移動の目標点の教示点である。経由教示点は、開始教示点から目標教示点までの間において、両点間をマニピュレータ３が移動するように外乱を考慮して設定された１以上の教示点である。通常、経由教示点は、開始教示点から目標教示点までの間に、２個以上設定される。その外乱は、例えば、ウェハやガラス基板等の搬送対象や、手先効果器が把持している部品等の重量、あるいは、マニピュレータ３の自重等に応じた外乱である。そのような外乱の存在の下、マニピュレータ３を開始教示点から目標教示点まで、１以上の経由教示点を経由しながら移動させることによって、結果として、マニピュレータ３が、開始教示点から目標教示点までを直線で結ぶ目標ルート上を移動することになる。図２Ａは、開始教示点Ｐ_ｓと、目標教示点Ｐ_ｅと、経由教示点Ｐ_１'、Ｐ_２'、Ｐ_３'との一例を示す図である。図２Ａにおいて、一点鎖線で示される直線のルートが目標ルートＧである。また、実線で示される折線のルートが教示ルートＫである。教示ルートＫは、開始教示点Ｐ_ｓ、１以上の経由教示点Ｐ_１'、Ｐ_２'、Ｐ_３'、及び目標教示点Ｐ_ｅを順に直線で結ぶルートである。すなわち、教示ルートＫは、分岐のない連続した線分の集合である。なお、経由教示点Ｐ_ｎ'から目標ルートへの垂線の足をシフト経由教示点Ｐ_ｎと呼ぶことがある。ｎは１以上の整数である。図２Ａにおいて、通常、教示ルートＫの各線分は、目標ルートＧに対して３次元のねじれの位置になるが、そうでなくてもよい。また、目標時間は、マニピュレータ３が開始教示点から目標教示点まで移動する時間である。なお、教示情報記憶部１１では、開始教示点、目標教示点、両点間の１以上の経由教示点、及び目標時間のセットが１個記憶されていてもよく、または、２個以上記憶されていてもよい。なお、そのセットが２個以上記憶されている場合には、通常、あるセットの目標教示点が、次のセットの開始教示点となる。したがって、教示情報記憶部１１では、マニピュレータ３が通過する教示点である固定教示点と、連続した２個の固定教示点間において外乱を考慮して設定された１以上の経由教示点と、連続した２個の固定教示点間の移動時間である目標時間とが記憶されていてもよい。その固定教示点は、その固定教示点を終点とする軌道に対しては目標教示点となり、その固定教示点を始点とする軌道に対しては開始教示点となる。また、教示情報記憶部１１で記憶されている目標時間は、結果として、開始教示点から目標教示点までの移動時間を知ることができる情報であれば、どのような情報であってもよい。その移動時間そのものであってもよく、または、マニピュレータ３が開始教示点の位置に存在する時刻と、マニピュレータ３が目標教示点の位置に存在する時刻とであってもよい。後者の場合には、目標教示点の時刻から開始教示点の時刻を減算することにより、移動時間を算出することができる。したがって、それらの時刻が教示情報に含まれている場合にも、目標時間が教示情報に含まれていると考えることができる。なお、開始教示点や目標教示点、経由教示点は、通常、マニピュレータ３の先端である手先効果器の所定の位置の教示点である。また、教示情報記憶部１１では、各教示点におけるマニピュレータ３の手先効果器の姿勢を示す情報も記憶されていてもよい。また、開始教示点から目標教示点までを直線で結ぶ目標ルートに応じた速さを示す情報も教示情報記憶部１１で記憶されてもよい。その速さを示す情報は、例えば、５０％や１００％等のように、最大の速さに対する割合であってもよく、速度の時間変化を示す速度波形であってもよい。また、教示情報記憶部１１では、教示情報以外の情報が記憶されてもよい。例えば、曲線補間部１２が補間した曲線軌道を示す式等が教示情報記憶部１１で記憶されてもよい。

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

曲線補間部１２は、教示ルートの各経由教示点について、経由教示点とその経由教示点の直前の教示点（この教示点は、経由教示点または開始教示点である）との間の線分上の点である曲線補間開始点から、その経由教示点と経由教示点の直後の教示点（この教示点は、経由教示点または目標教示点である）との間の線分上の点である曲線補間終了点までの間を曲線補間した開始教示点から目標教示点までのルートである曲線補間教示ルートを生成する。図２Ｂは、その曲線補間教示ルートＫ'の一例を示す図である。図２Ｂにおいて、曲線補間教示ルートＫ'は、開始教示点Ｐ_ｓから曲線補間開始点Ｍ_１'までの直線軌道と、曲線補間開始点Ｍ_１'から曲線補間終了点Ｒ_１'までの曲線軌道と、曲線補間終了点Ｒ_１'から曲線補間開始点Ｍ_２'までの直線軌道と、曲線補間開始点Ｍ_２'から曲線補間終了点Ｒ_２'までの曲線軌道と、曲線補間終了点Ｒ_２'から曲線補間開始点Ｍ_３'までの直線軌道と、曲線補間開始点Ｍ_３'から曲線補間終了点Ｒ_３'までの曲線軌道と、曲線補間終了点Ｒ_３'から目標教示点Ｐ_ｅまでの直線軌道とを有することになる。その曲線補間教示ルートＫ'では、移動方向が急に変化する点が存在しない。そのため、マニピュレータ３を曲線補間教示ルートＫ'に沿って移動させるように制御することにより、マニピュレータ３の速度変化が滑らかになる。なお、曲線補間開始点から曲線補間終了点までの曲線の種類は問わない。ただし、曲線補間開始点と曲線補間終了点とにおいて、曲線軌道が教示ルートＫと滑らかに接続され、また曲線補間開始点から曲線補間終了点までの曲線軌道が滑らかな曲線であることが好適である。また通常、その曲線の外側に経由教示点が位置するように曲線補間が行われることが好適である。その曲線補間は、例えば、２次ベジェ曲線による補間であってもよく、スプライン曲線による補間であってもよく、円弧による補間であってもよく、エルミート曲線による補間であってもよく、その他の曲線による補間であってもよい。いずれの補間であっても、その曲線の曲線補間開始点における接線が、曲線補間開始点における教示ルートＫの方向となり、その曲線の曲線補間終了点における接線が、曲線補間終了点における教示ルートＫの方向となることが好適である。なお、曲線補間部１２が曲線補間教示ルートを生成するとは、曲線補間教示ルートＫ'の全体を示す情報を教示情報記憶部１１に蓄積することであってもよく、曲線補間によって生成された１以上の曲線軌道を示す情報を教示情報記憶部１１に蓄積することであってもよい。後者の場合であっても、その曲線軌道以外の軌道については教示情報から知ることができるため、実質的に曲線補間教示ルートＫ'を生成したと考えることができる。

ここで、曲線補間部１２が曲線補間を行う方法について、図３Ａを用いて説明する。図３Ａにおいて、点Ｐ_ｎ−１'、Ｐ_ｎ'、Ｐ_ｎ＋１'は連続した経由教示点であり、点Ｐ_ｎ−１、Ｐ_ｎ、Ｐ_ｎ＋１は、それらの経由教示点にそれぞれ対応するシフト経由教示点である。シフト経由教示点は、前述のように、経由教示点から目標ルートへの垂線の足である。ｎは１からＤまでの任意の整数であり、Ｄは経由教示点の個数である。なお、Ｐ_０'は開始教示点であり、Ｐ_Ｄ＋１'は目標教示点であるとする。また、点Ｍ_ｎ'は、経由教示点Ｐ_ｎ−１'、Ｐ_ｎ'を１−ｍ：ｍに内分する曲線補間開始点であり、点Ｍ_ｎは、その曲線補間開始点Ｍ_ｎ'に対応する目標ルート上の点であって、シフト経由教示点Ｐ_ｎ−１、Ｐ_ｎを１−ｍ：ｍに内分する点である。その点Ｍ_ｎをシフト曲線補間開始点と呼ぶこともある。また、点Ｒ_ｎ'は、経由教示点Ｐ_ｎ'、Ｐ_ｎ＋１'をｒ：１−ｒに内分する曲線補間終了点であり、点Ｒ_ｎは、その曲線補間終了点Ｒ_ｎ'に対応する目標ルート上の点であって、シフト経由教示点Ｐ_ｎ、Ｐ_ｎ＋１をｒ：１−ｒに内分する点である。その点Ｒ_ｎをシフト曲線補間終了点と呼ぶこともある。また、ＶＰ_ｎ等を点Ｐ_ｎ等のベクトル（または座標）とすると、曲線補間開始点等は次のようになる。
ＶＭ_ｎ'＝（１−ｍ）ＶＰ_ｎ'＋ｍＶＰ_ｎ−１'
ＶＲ_ｎ'＝（１−ｒ）ＶＰ_ｎ'＋ｒＶＰ_ｎ＋１'
ＶＭ_ｎ＝（１−ｍ）ＶＰ_ｎ＋ｍＶＰ_ｎ−１
ＶＲ_ｎ＝（１−ｒ）ＶＰ_ｎ＋ｒＶＰ_ｎ＋１

ここで、０＜ｍ≦１、０＜ｒ≦１であり、このｍ、ｒを決定することにより、曲線補間開始点Ｍ_ｎ'、及び曲線補間終了点Ｒ_ｎ'が決まる。このｍ、ｒの値はあらかじめ図示しない記録媒体で記憶されていてもよい。また、その曲線補間開始点Ｍ_ｎ'、及び曲線補間終了点Ｒ_ｎ'において軌道が不連続とならないため、
ｍ＋ｒ≦１
であるとする。また、ｍ、ｒが次式をみたす場合には、曲線補間教示ルートは、開始教示点から１個目の曲線補間開始点までの間、及び、最後の曲線補間終了点から目標教示点までの間以外は、曲線となる。
ｍ＋ｒ＝１

また、目標ルート上の点Ｐが点Ｍ_ｎから点Ｒ_ｎまでの間にある場合には、その点Ｐに対応する曲線補間教示ルート上の点Ｐ'は、点Ｍ_ｎ'から点Ｒ_ｎ'までの曲線上に存在することになる。その場合に、目標ルート上の点Ｐは、次のようになる。ただし、０≦ｔ≦１であり、点Ｐは、ｔ＝０のときには点Ｍ_ｎと一致し、ｔ＝１のときには点Ｒ_ｎと一致することになる。
ＶＰ＝（１−ｔ）ＶＭ_ｎ＋ｔＶＲ_ｎ

ここで、上記式をｔについて解くと、次のようになる。なお、｜ＶＰ_ｎ−ＶＭ_ｎ｜等は、ベクトル（ＶＰ_ｎ−ＶＭ_ｎ）等の長さ（ノルム）である。
ｔ＝｜ＶＰ_ｎ−ＶＭ_ｎ｜／｜ＶＲ_ｎ−ＶＭ_ｎ｜ （１）

また、曲線補間開始点Ｍ_ｎ'から曲線補間終了点Ｒ_ｎ'までの曲線上の点Ｐ'は、次のようになる。ただし、０≦ｔ≦１であり、ｔ＝０のときには点Ｍ_ｎ'と一致し、ｔ＝１のときには点Ｒ_ｎ'と一致することになる。また、その曲線は、２次ベジェ曲線であるとしている。曲線補間開始点Ｍ_ｎ'から曲線補間終了点Ｒ_ｎ'までの曲線軌道が次式で示される場合には、その曲線軌道は、曲線補間開始点Ｍ_ｎ'、曲線補間終了点Ｒ_ｎ'、経由教示点Ｐ_ｎ'を含む平面上に存在することになるが、その曲線が２次ベジェ曲線でない場合であっても、その曲線は、それらの３点を含む平面上に存在することが好適である。
ＶＰ'＝（１−ｔ）^２ＶＭ_ｎ'＋２ｔ（１−ｔ）ＶＰ_ｎ'＋ｔ^２ＶＲ_ｎ' （２）
そして、上記ＶＰ'の式に、前述のｔを代入することにより、図３Ｂで示されるように、目標ルート上の点Ｐと、曲線補間教示ルート上の点Ｐ'との対応関係が決まることになる。なお、図３Ｂにおいて、曲線補間開始点Ｍ_ｎ'から曲線補間終了点Ｒ_ｎ'までの曲線軌道は、２次ベジェ曲線である。

なお、上述の説明では、曲線補間教示ルートの曲線軌道上の点と、目標ルート上の点との対応関係について説明したが、曲線補間教示ルートの直線軌道上の点と、目標ルート上の点との対応関係についても、同様になる。例えば、曲線補間終了点Ｒ_ｎ−１'から曲線補間開始点Ｍ_ｎ'までの直線区間上の点Ｐ'は、次のようになる。なお、０≦ｔ≦１であり、点Ｐ'は、ｔ＝０のときには点Ｒ_ｎ−１'と一致し、ｔ＝１のときには点Ｍ_ｎ'と一致する。
ＶＰ'＝（１−ｔ）ＶＲ_ｎ−１'＋ｔＶＭ_ｎ' （３）

ここで、目標ルート上の点Ｐが点Ｒ_ｎ−１から点Ｍ_ｎまでの間にある場合には、目標ルート上の点Ｐは、次のようになる。なお、０≦ｔ≦１であり、点Ｐは、ｔ＝０のときには点Ｒ_ｎ−１と一致し、ｔ＝１のときには点Ｍ_ｎと一致する。
ＶＰ＝（１−ｔ）ＶＲ_ｎ−１＋ｔＶＭ_ｎ （４）
したがって、前述した曲線軌道の場合と同様に、直線軌道の場合も、式（４）をｔについて解き、そのｔを式（３）に代入することによって、目標ルート上の点Ｐと、曲線補間教示ルート上の点Ｐ'との対応関係を求めることができる。

また、上述の説明では、曲線軌道が２次ベジェ曲線である場合について説明したが、前述のように、曲線軌道は２次ベジェ曲線以外であってもよい。なお、２次ベジェ曲線以外の曲線軌道であっても、曲線補間開始点から曲線補間終了点までを０≦ｔ≦１のパラメータを用いて示すことができるパラメトリック曲線である場合には、前述の説明と同様になる。また、曲線軌道がパラメトリック曲線でなかったとしても、その曲線軌道上の点と、目標ルート上の点との対応関係が決められているのであれば、上述の説明と同様にして、目標ルート上の点Ｐと、曲線補間教示ルート上の点Ｐ'との対応を知ることができることになる。

また、上述の説明では、ｍ、ｒがすべて同じである場合について説明したが、そうでなくてもよい。曲線軌道ごとに、ｍ、ｒがそれぞれ異なっていてもよい。その場合には、例えば、図３Ａにおいて、ｍ、ｒがそれぞれｍ_ｎ、ｒ_ｎとなってもよい。軌道制御部１３は、そのｍ_ｎ、ｒ_ｎの値を、例えば、教示ルートにおける経由教示点Ｐ_ｎ'での折れ曲がりが大きい場合には、大きい値に設定し、教示ルートにおける経由教示点Ｐ_ｎ'での折れ曲がりが小さい場合、すなわち、経由教示点Ｐ_ｎ'で教示ルートが直線に近い場合には、小さい値に設定してもよい。または、そのｍ_ｎ、ｒ_ｎ等の値は、あらかじめ設定されていてもよい。また、ｍ、ｒがそれぞれｍ_ｎ、ｒ_ｎである場合には、曲線補間開始点Ｍ_ｎ'、及び曲線補間終了点Ｒ_ｎ'において軌道が不連続とならないために、次の式がみたされる必要がある。ただし、ｎは、１からＤまでの整数であり、Ｄは、経由教示点の個数である。また、ｒ_０＝０であり、ｍ_Ｄ＋１＝０とする。開始教示点Ｐ_ｓから１個目の経由教示点Ｐ_１'までには曲線補間終了点が存在せず、最後の経由教示点Ｐ_Ｄ'から目標教示点Ｐ_ｅまでには曲線補間開始点が存在しないからである。
ｒ_ｎ−１＋ｍ_ｎ≦１

軌道制御部１３は、曲線補間教示ルートをマニピュレータ３が移動するように、サーボコントローラ２を制御する。その結果、マニピュレータ３の先端が、その曲線補間教示ルートに応じて移動することになる。なお、前述のように、教示ルートは、外乱を考慮して設定されたものであるため、軌道制御部１３によって、曲線補間教示ルートをマニピュレータ３が移動するように制御されることによって、結果として、マニピュレータ３の先端が目標ルートまたは目標ルートの近傍を移動することになる。その制御において、軌道制御部１３は、セクション目標時間で、そのセクション目標時間に対応する教示セクションをマニピュレータ３が移動するようにサーボコントローラ２を制御する。ここで、目標ルートは、複数の目標セクションに区画される。すなわち、その複数の目標セクションをつなげたものが、目標ルートとなる。また、その目標セクションごとに目標時間が分割されたものがセクション目標時間である。すなわち、ある目標ルートにおいて、セクション目標時間を全目標セクションについて合計した時間が目標時間となる。また、教示セクションは、目標セクションに応じた曲線補間教示ルートの区画である。なお、教示セクションと、目標セクションとは一対一に対応しているものとする。

ここで、目標ルートＧと曲線補間教示ルートＫ'とが図２Ｂで示されるときの目標セクションと、教示セクションとの対応について説明する。なお、図４Ａ〜図４Ｄにおいて、１個のセクションの両端を黒丸で示している。
（１）セクションの境界点がシフト曲線補間開始点及び曲線補間開始点である場合
この場合には、目標セクションの境界点はシフト曲線補間開始点Ｍ_ｎとなり、教示セクションの境界点は曲線補間開始点Ｍ_ｎ'となる。したがって、目標セクションｇｓ（１）〜ｇｓ（４）、及び教示セクションｔｓ（１）〜ｔｓ（４）は、図４Ａで示されるようになる。なお、ｎ＝１〜４について、目標セクションｇｓ（ｎ）と、教示セクションｔｓ（ｎ）とが互いに対応するセクションである。

（２）セクションの境界点がシフト曲線補間終了点及び曲線補間終了点である場合
この場合には、目標セクションの境界点はシフト曲線補間終了点Ｒ_ｎとなり、教示セクションの境界点は曲線補間終了点Ｒ_ｎ'となる。したがって、目標セクションｇｓ（１）〜ｇｓ（４）、及び教示セクションｔｓ（１）〜ｔｓ（４）は、図４Ｂで示されるようになる。なお、ｎ＝１〜４について、目標セクションｇｓ（ｎ）と、教示セクションｔｓ（ｎ）とが互いに対応するセクションである。

（３）セクションの境界点がシフト曲線補間開始点・シフト曲線補間終了点及び曲線補間開始点・曲線補間終了点である場合
この場合には、目標セクションの境界点はシフト曲線補間開始点Ｍ_ｎ及びシフト曲線補間終了点Ｒ_ｎとなり、教示セクションの境界点は曲線補間開始点Ｍ_ｎ'及び曲線補間終了点Ｒ_ｎ'となる。したがって、目標セクションｇｓ（１）〜ｇｓ（７）、及び教示セクションｔｓ（１）〜ｔｓ（７）は、図４Ｃで示されるようになる。なお、ｎ＝１〜７について、目標セクションｇｓ（ｎ）と、教示セクションｔｓ（ｎ）とが互いに対応するセクションである。この場合には、曲線軌道がｔ（０≦ｔ≦１）で示されるパラメトリック曲線であれば、各セクション上の点を１個のパラメータを用いて示すことが可能となる。例えば、各セクションの開始点でｔ＝０となり、そのセクションの終了点でｔ＝１となるｔ（０≦ｔ≦１）を用いて、各目標セクション上の点、及び各教示セクション上の点を示すことが可能となる。

（１）〜（３）で説明したように、教示セクションの境界点は、曲線補間開始点及び／または曲線補間終了点であってもよい。また、目標セクションの境界点は、曲線補間開始点に対応する目標ルート上の点（シフト曲線補間開始点）及び／または曲線補間終了点に対応する目標ルート上の点（シフト曲線補間終了点）であってもよい。その曲線補間開始点や曲線補間終了点に対応する目標ルート上の点は、図３Ａで説明したように、連続する２個のシフト経由教示点を、曲線補間開始点や曲線補間終了点が教示ルートの各線分を内分する内分比で内分した点であってもよい。

（４）セクションの境界点がシフト経由教示点及びそれに対応する曲線軌道上の点である場合
この場合には、目標セクションの境界点はシフト経由教示点Ｐ_ｎとなり、教示セクションの境界点は、そのシフト経由教示点Ｐ_ｎに対応する曲線軌道上の点となる。その曲線軌道上の点は、例えば、目標ルートに垂直なシフト経由教示点Ｐ_ｎを含む平面と、曲線軌道との交点Ｐ_ｎｃ'であってもよい。なお、その平面は、目標ルートを法線とする平面である。したがって、目標セクションｇｓ（１）〜ｇｓ（４）、及び教示セクションｔｓ（１）〜ｔｓ（４）は、図４Ｄで示されるようになる。なお、ｎ＝１〜４について、目標セクションｇｓ（ｎ）と、教示セクションｔｓ（ｎ）とが互いに対応するセクションである。

なお、ここでは、目標セクションと教示セクションとの対応に関する４つの例（１）〜（４）について説明したが、各セクションは上記以外のものであってもよい。例えば、目標ルートをあらかじめ決められた個数や、あらかじめ決められた長さごとに目標セクションに区画し、その目標セクションの境界点に対応する曲線補間教示ルート上の点である教示セクションの境界点を特定することによって、目標セクション及び教示セクションを設定してもよい。その場合に、目標セクションの境界点と教示セクションの境界点との対応は、上述の（４）における両境界点の対応と同様であってもよい。

次に、セクション目標時間の算出方法について説明する。この説明において、（Ａ）移動が等速である場合と、（Ｂ）移動が加速及び減速を含む場合とに分けて説明する。なお、この説明では、開始教示点から目標教示点までの移動において、開始教示点の距離を０とし、目標ルートの距離をＳとする。また、開始教示点の時刻を０とし、目標教示点の時刻をＴとする。すなわち、目標時間をＴとしていることになる。また、この説明では、上記（３）の場合について説明するが、他の（１）、（２）、（４）の場合も、同様である。

（Ａ）移動が等速である場合
等速で移動する場合には、時刻を横軸に取り、距離を縦軸に取ると、開始教示点から目標教示点までの目標ルートの移動における時間と距離との対応は、図５Ａで示されるようになる。なお、図５Ａにおいて、ｓ（ｎ）は、開始教示点から、目標セクションｇｓ（ｎ）の目標教示点に近い側の端点までの距離である。また、ｔ（ｎ）は、ｓ（ｎ）に対応する時刻である。また、Ｓ（＝ｓ（７））は、目標ルートの距離、すなわち、開始教示点Ｐ_ｓから目標教示点Ｐ_ｅまでの距離であり、Ｔ（＝ｔ（７））は、目標時間である。このように、目標ルートを等速で移動する場合には、軌道制御部１３は、各目標セクションの境界点に対応する時刻を算出することができる。また、軌道制御部１３は、隣接する２個の時刻の差を算出することにより、各目標セクションに対応するセクション目標時間を算出できる。具体的には、目標セクションｇｓ（ｎ）に対応するセクション目標時間は、ｔ（ｎ）−ｔ（ｎ−１）となる。なお、ｔ（０）＝０とする。

（Ｂ）移動が加速及び減速を含む場合
加速及び減速を含む場合には、時刻を横軸に取り、距離を縦軸に取ると、開始教示点から目標教示点までの移動における時間と距離との対応は、図５Ｂで示されるようになる。したがって、ｓ（１）等を（Ａ）の場合と同様にすると、軌道制御部１３は、ｓ（１）等にそれぞれ対応する時刻ｔ（１）等を求めることができる。そして、軌道制御部１３は、隣接する２個の時刻の差を算出することにより、各目標セクションに対応するセクション目標時間を算出できる。この場合も、目標セクションｇｓ（ｎ）に対応するセクション目標時間は、ｔ（ｎ）−ｔ（ｎ−１）となる。なお、ｔ（０）＝０である。この場合には、目標ルートを加速及び減速を含んで移動する場合の目標セクションごとの移動時間がセクション目標時間となる。

また、ここでは、図５Ａ，図５Ｂを用いて、移動が等速である場合と、移動が加速等を含む場合とについて説明したが、マニピュレータ３の移動の速度は、これらに限定されるものではない。例えば、その速度は、教示情報記憶部１１で記憶されている速度波形に応じたものであってもよい。

このようにして、軌道制御部１３は、各セクション目標時間を算出することができる。そして、軌道制御部１３は、教示セクションｔｓ（ｎ）をマニピュレータ３が移動する場合に、その教示セクションｔｓ（ｎ）に対応する目標セクションｇｓ（ｎ）のセクション目標時間で、教示セクションｔｓ（ｎ）の始点から終点まで移動するように、サーボコントローラ２を制御する。

具体的には、軌道制御部１３は、教示セクションｔｓ（ｎ）ごとに、その教示セクションｔｓ（ｎ）に対応するセクション目標時間で移動するように補間を行い、補間周期ごとのマニピュレータ３の先端の位置を算出してもよい。そして、軌道制御部１３は、補間周期ごとに、マニピュレータ３の先端が教示セクションの補間された位置となるように制御してもよい。そのようにすることで、結果として、曲線補間教示ルートの開始教示点から目標教示点までを、目標時間で移動するように制御できる。また、結果として、教示セクションごとに、その教示セクションに対応するセクション目標時間で移動するようにする制御するためには、各教示セクションｔｓ（ｎ）の終点を通過する時刻が、その終点に対応する時刻ｔ（ｎ）となるように制御すればよいことになる。ただし、その時刻は、曲線補間教示ルートごとに０から始まるものである。したがって、軌道制御部１３は、このような制御を行うことによって、結果として、マニピュレータ３が、教示セクションをそれに対応するセクション目標時間で移動するように制御してもよい。この場合には、軌道制御部１３は、セクション目標時間の算出に代えて、教示セクションｔｓ（ｎ）の終点に対応する時刻ｔ（ｎ）を算出してもよい。

また、軌道制御部１３は、目標ルートにおいて開始教示点から目標教示点までを目標時間で移動するように補間を行い、補間周期ごとの目標ルート上の位置を曲線補間教示ルート上の位置に変換してもよい。そして、軌道制御部１３は、補間周期ごとに、マニピュレータ３の先端が変換後の曲線補間教示ルート上の位置となるように制御してもよい。この場合にも、曲線補間教示ルートの開始教示点から目標教示点までを、目標時間で移動するようにできる。また、目標ルート上で補間を行っているため、各教示セクションを、その教示セクションに対応する目標セクションのセクション目標時間で移動できていることは当然である。なお、ここでは、目標ルートにおいて補間を行う場合について説明したが、目標ルートの目標セクションごとに補間を行ってもよい。

また、軌道制御部１３は、ある教示セクションにおいて、その教示セクションに対応するセクション目標時間での移動を行うことができなかった場合、例えば、移動が速すぎた場合や遅すぎた場合には、それに応じて、それ以降の教示セクションにおける移動の制御を変更することによって、結果として、マニピュレータ３が目標時間で開始教示点から目標教示点までを移動するように制御してもよい。

なお、その制御時に、軌道制御部１３は、補間周期ごとのマニピュレータ３の先端の位置に対して逆キネマティクス変換を行い、マニピュレータ３の先端以外の各軸の位置指令値を算出してもよい。このようにして、軌道制御部１３は、補間周期ごとのマニピュレータ３の全軸の位置指令値を算出でき、その位置指令値をサーボコントローラ２に出力することによって、サーボコントローラ２を制御することができる。なお、この軌道制御部１３による処理は、産業用ロボットにおけるサーボコントローラの制御としてすでに公知であり、その詳細な説明を省略する。

また、ここでは、軌道制御部１３がセクション目標時間を算出する場合について説明したが、このセクション目標時間は、他の構成要素によって、軌道制御部１３による軌道の制御が行われるまでに算出されてもよい。また、目標ルート上で補間を行う場合などのように、セクション目標時間を用いないで軌道の制御を行う場合には、セクション目標時間が算出されなくてもよい。

ここで、サーボコントローラ２について簡単に説明する。サーボコントローラ２は、制御装置１から、マニピュレータ３の各軸の位置の指令を受け取る。サーボコントローラ２は、その各軸の位置の指令に応じて、フィードバック制御により各軸のモータに対応するトルク指令値を生成する。マニピュレータ３の各モータは、このトルク指令値によって動作する。なお、このサーボコントローラ２によるフィードバック制御はすでに公知であり、その詳細な説明を省略する。なお、サーボコントローラ２による制御が行われ、またマニピュレータ３が外乱の影響を受けることによって、結果として、マニピュレータ３の先端は、目標ルートまたは目標ルートの近傍を移動することになる。

次に、制御装置１の動作について図６のフローチャートを用いて説明する。
（ステップＳ１０１）曲線補間部１２は、カウンタｉを１に設定する。

（ステップＳ１０２）曲線補間部１２は、教示情報記憶部１１で記憶されている教示情報に含まれるｉ番目の経由教示点について、曲線補間を行う。すなわち、そのｉ番目の経由教示点と、直前の教示点との間の曲線補間開始点から、そのｉ番目の経由教示点と、直後の教示点との間の曲線補間終了点までの間を曲線補間する。この曲線補間の処理は、曲線軌道を示す式を生成し、教示情報記憶部１１に蓄積することであってもよい。

（ステップＳ１０３）曲線補間部１２は、カウンタｉを１だけインクリメントする。

（ステップＳ１０４）曲線補間部１２は、教示情報記憶部１１で記憶されている教示情報にｉ番目の経由教示点が含まれるかどうか判断する。そして、ｉ番目の経由教示点が含まれる場合には、ステップＳ１０２に戻り、含まれない場合、すなわち、教示情報に含まれるすべての経由教示点について曲線補間が行われた場合には、ステップＳ１０５に進む。

（ステップＳ１０５）軌道制御部１３は、各目標ルートにおける目標セクションごとに、セクション目標時間を算出する。この算出されたセクション目標時間は、教示情報記憶部１１で記憶されてもよい。

（ステップＳ１０６）軌道制御部１３は、曲線補間後の曲線補間教示ルートと、各教示セクションに対応するセクション目標時間とを用いて、補間周期ごとにマニピュレータ３の先端の位置を特定し、その特定した位置に逆キネマティクス処理を行うことによって、マニピュレータ３の各軸の位置を算出する。そして、軌道制御部１３は、その補間周期ごとに各軸の位置を示す位置指令値をサーボコントローラ２に出力する。その結果、マニピュレータ３は、サーボコントローラ２による制御によって、各目標ルートを移動する。また、各目標ルートの移動時間は、あらかじめ設定された目標時間となる。

なお、図６のフローチャートでは、軌道の制御の直前にセクション目標時間を算出する場合について説明したが、そうでなくてもよい。曲線補間を行わなくてもセクション目標時間を算出できる場合には、曲線補間の前にセクション目標時間を算出してもよい。また、目標ルート上で補間を行い、その補間周期ごとの位置を曲線補間教示ルート上の位置に変換する場合には、セクション目標時間を算出する必要がないため、ステップＳ１０５の処理を行わなくてもよく、ステップＳ１０６において、そのセクション目標時間を用いた軌道の制御を行わなくてもよい。

次に、本実施の形態による制御装置１の動作について、具体例を用いて説明する。この具体例において、軌道制御部１３が、目標ルートにおいて補間を行い、その補間周期ごとの目標ルート上の位置を曲線補間教示ルート上の位置に変換することによって、制御を行う場合について説明する。すなわち、セクション目標時間の算出（ステップＳ１０５）は行わないものとする。また、教示情報記憶部１１では、次の教示情報が記憶されているものとする。
Ｐｏｓｉｔｉｏｎ（１）、Ｔｉｍｅ（１）
Ｐｏｓｉｔｉｏｎ（１−１）
Ｐｏｓｉｔｉｏｎ（１−２）
Ｐｏｓｉｔｉｏｎ（１−３）
Ｐｏｓｉｔｉｏｎ（２）、Ｔｉｍｅ（２）
Ｐｏｓｉｔｉｏｎ（２−１）
Ｐｏｓｉｔｉｏｎ（２−２）
Ｐｏｓｉｔｉｏｎ（２−３）
Ｐｏｓｉｔｉｏｎ（２−４）
Ｐｏｓｉｔｉｏｎ（３）、Ｔｉｍｅ（３）
Ｐｏｓｉｔｉｏｎ（３−１）
：
：

この教示情報において、Ｐｏｓｉｔｉｏｎ（Ｆ）は、目標ルートの端点におけるマニピュレータ３の先端の位置を示す情報である。すなわち、Ｐｏｓｉｔｉｏｎ（Ｆ）は、開始教示点及び／または目標教示点である。例えば、Ｐｏｓｉｔｉｏｎ（２）は、Ｐｏｓｉｔｉｏｎ（１）からＰｏｓｉｔｉｏｎ（２）までの目標ルートについては目標教示点となり、Ｐｏｓｉｔｉｏｎ（２）からＰｏｓｉｔｉｏｎ（３）までの目標ルートについては開始教示点となる。なお、Ｆは１以上の整数である。また、Ｐｏｓｉｔｉｏｎ（Ｆ−Ｅ）は、Ｐｏｓｉｔｉｏｎ（Ｆ）からＰｏｓｉｔｉｏｎ（Ｆ＋１）までの間の経由教示点である。なお、Ｅは１以上の整数である。また、Ｔｉｍｅ（Ｆ）は、教示点Ｐｏｓｉｔｉｏｎ（Ｆ）からＰｏｓｉｔｉｏｎ（Ｆ＋１）までの目標ルートに対応する目標時間である。例えば、マニピュレータ３の先端は、Ｐｏｓｉｔｉｏｎ（１）からＰｏｓｉｔｉｏｎ（２）までを、目標時間Ｔｉｍｅ（１）で移動するように制御される。また、この具体例では、曲線補間は、２次ベジェ曲線による補間であるとする。また、この具体例では、目標ルート及び曲線補間教示ルートが、前述の（３）のようにセクションに分割されるものとする。すなわち、例えば、図４Ｃで示されるように目標セクションや教示セクションが設定されるものとする。また、前述のｍ、ｒは、すべての経由教示点について共通であり、それらの値は、軌道制御部１３がアクセス可能な図示しない記録媒体で記憶されているものとする。

まず、曲線補間部１２は、１番目の経由教示点であるＰｏｓｉｔｉｏｎ（１−１）について、曲線補間を行う（ステップＳ１０１，Ｓ１０２）。具体的には、Ｐｏｓｉｔｉｏｎ（１）とＰｏｓｉｔｉｏｎ（１−１）との間の曲線補間開始点Ｍ_１'から、Ｐｏｓｉｔｉｏｎ（１−１）とＰｏｓｉｔｉｏｎ（１−２）との間の曲線補間終了点Ｒ_１'までの教示セクションｔｓ（２）に対応する式（２）を教示情報記憶部１１に蓄積する。なお、曲線補間部１２は、そのような曲線補間を、経由教示点Ｐｏｓｉｔｉｏｎ（１−２）、Ｐｏｓｉｔｉｏｎ（１−３）についても行い、さらに、経由教示点Ｐｏｓｉｔｉｏｎ（２−１）以降についても行う（ステップＳ１０２〜Ｓ１０４）。

その後、軌道制御部１３は、曲線補間後の曲線補間教示ルートを用いて、補間周期ごとにマニピュレータ３の先端の位置を特定し、その特定した位置に逆キネマティクス処理を行うことによって、マニピュレータ３の各軸の位置を算出し、その各軸の位置を示す位置指令値をサーボコントローラ２に出力する（ステップＳ１０６）。具体的には、軌道制御部１３は、まず、Ｐｏｓｉｔｉｏｎ（１）からＰｏｓｉｔｉｏｎ（２）までの目標ルートについて、その目標ルートに対応する目標時間Ｔｉｍｅ（１）を用いた補間を行う。そして、軌道制御部１３は、その補間位置のうち、目標セクションｇｓ（１）における各補間位置を、その目標セクションｇｓ（１）に対応する教示セクションｔｓ（１）上の点に変換する。具体的には、軌道制御部１３は、各補間位置に対応する式（４）のｔの値を算出し、そのｔの値を式（３）に代入することにより、各補間位置に対応する教示セクションｔｓ（１）上の点を求める。次に、軌道制御部１３は、目標セクションｇｓ（２）における各補間位置を、その目標セクションｇｓ（２）に対応する教示セクションｔｓ（２）上の点に変換する。この場合には、教示セクションｔｓ（２）が曲線軌道であるため、軌道制御部１３は、あらかじめ蓄積しておいた教示セクションｔｓ（２）に対応する式（２）を読み出し、各補間位置に対応する式（１）の値を算出し、そのｔの値を式（２）に代入することにより、各補間位置に対応する教示セクションｔｓ（２）上の点を求める。このような処理を繰り返すことによって、軌道制御部１３は、曲線補間教示ルート上の各教示セクション上の位置を特定することができ、その位置が補間周期ごとのマニピュレータ３の先端の位置となる。その後、軌道制御部１３は、補間周期ごとの曲線補間教示ルート上の位置から、各軸の位置を算出し、それらの位置指令値をサーボコントローラ２に出力することによって、マニピュレータ３の先端が曲線補間教示ルート上を移動するように制御する。なお、このように制御することによって、結果として、マニピュレータ３の先端が、セクション目標時間で、そのセクション目標時間に対応する教示セクションを移動するように制御され、それに外乱が加わることによって、マニピュレータ３の先端が目標ルートを移動することになるのは前述の通りである。

なお、この具体例では、目標ルート上の補間位置を曲線補間教示ルート上の位置に変換することによって、マニピュレータ３の先端が、セクション目標時間で教示セクションを移動するように制御する場合について説明したが、前述のように、教示セクションごとに、セクション目標時間で移動するように補間を行ってもよいことは言うまでもない。

以上のように、本実施の形態による制御装置１によれば、マニピュレータ３の先端が曲線補間教示ルートを移動する場合であっても、あらかじめ決められた目標時間でマニピュレータ３が開始教示点から目標教示点までを移動するように制御できる。その結果、目標ルートよりも移動距離が長い曲線補間教示ルートを移動する場合であっても、開始教示点から目標教示点までの移動に遅れが発生することがなく、タクトタイムが長くなることを防止することができる。また、マニピュレータ３の先端が曲線補間教示ルートを移動する場合であっても、開始教示点から目標教示点までの移動の時間である目標時間を用いて移動制御を行うことができ、その移動制御が容易になる。さらに、本実施の形態では、マニピュレータ３の先端が曲線補間教示ルートを移動するように制御するため、そのルート上において移動方向が急激に変化しないようにすることができる。その結果、速度変化を滑らかにすることができ、搬送対象や機械本体の破損を防止することができる。また、曲線補間の際に、２次ベジェ曲線のようなパラメトリック曲線を用いることによって、目標ルート上の点と、曲線補間教示ルート上の点との対応が明確になり、一方のルート上の点から、その点に対応する他方のルート上の点を特定する計算が簡単になる。

なお、本実施の形態では、曲線軌道の式やセクション目標時間が教示情報記憶部１１に蓄積される場合について説明したが、そうでなくてもよい。それらの情報は、他の図示しない記録媒体に蓄積されてもよい。その場合には、軌道制御部１３は、その図示しない記録媒体から曲線軌道の式等を読み出して用いるものとする。

また、本実施の形態による制御装置１を含む産業用ロボットの種類を問わないことは言うまでもない。例えば、産業用ロボットは、円筒座標ロボットであってもよい。その場合であっても、デカルト座標系における射影変換に加えて、関節軸で表す空間上で上述した処理と同様の処理を行ってもよい。

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

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

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

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

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

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

以上より、本発明による制御装置等によれば、移動方向が急激に変化しないように制御することができるという効果が得られ、マニピュレータを制御する装置等として有用である。

１ 制御装置
２ サーボコントローラ
３ マニピュレータ
１１ 教示情報記憶部
１２ 曲線補間部
１３ 軌道制御部

Claims (3)

1. モータによって駆動される関節によって連結された複数のアームを有するマニピュレータを制御する制御装置であって、
   前記マニピュレータの移動の開始点である開始教示点、前記マニピュレータの移動の目標点である目標教示点、前記開始教示点から前記目標教示点までの間において、両点間を前記マニピュレータが移動するように外乱を考慮して設定された１以上の経由教示点、及び前記マニピュレータが前記開始教示点から前記目標教示点まで移動する時間である目標時間を含む教示情報が記憶される教示情報記憶部と、
   前記開始教示点、前記１以上の経由教示点、及び前記目標教示点を順に直線で結ぶ教示ルートの各経由教示点について、前記経由教示点と当該経由教示点の直前の教示点との間の線分上の点である曲線補間開始点から、当該経由教示点と当該経由教示点の直後の教示点との間の線分上の点である曲線補間終了点までの間を曲線補間した前記開始教示点から前記目標教示点までのルートである曲線補間教示ルートを生成する曲線補間部と、
   前記開始教示点から前記目標教示点までを直線で結ぶ目標ルートを区画する複数の目標セクションごとに前記目標時間が分割されたセクション目標時間で、当該セクション目標時間に対応する目標セクションに応じた前記曲線補間教示ルートの区画である教示セクションを前記マニピュレータが移動するように、前記モータの制御を行うサーボコントローラを制御する軌道制御部と、を備え、
   前記目標セクションの境界点は、曲線補間開始点に対応する目標ルート上の点及び／または曲線補間終了点に対応する目標ルート上の点である、制御装置。
2. 前記曲線補間は、２次ベジェ曲線による補間である、請求項１記載の制御装置。
3. 前記セクション目標時間は、前記目標ルートを加速及び減速を含んで移動する場合の目標セクションごとの移動時間である、請求項１または請求項２記載の制御装置。

Images