工業用ロボット
【課題】 マニピュレータと補助装置を同時に動作させる工業用ロボットにおいて、ワーク上の指定された経路に対してツールを高精度で移動させる。
【解決手段】 制御装置１３が備える平滑化手段（Ｓ９）は、ポジショナ装置経路方程式（Ｊｐ（ｔ））により規定されるポジショナ１２の位置及び姿勢Ｊｐを限界加減速度Ａｐを下回るように平滑化し、平滑化位置姿勢Ｊｐ＿smoothを算出する。制御装置１３は、平滑化位置姿勢Ｊｐ＿smoothを使用して算出した関節角度Ｊｍをマニピュレータ１１に指令として出力し、平滑化位置姿勢Ｊｐ＿smoothをポジショナ１２に指令として出力する。
【解決手段】 制御装置１３が備える平滑化手段（Ｓ９）は、ポジショナ装置経路方程式（Ｊｐ（ｔ））により規定されるポジショナ１２の位置及び姿勢Ｊｐを限界加減速度Ａｐを下回るように平滑化し、平滑化位置姿勢Ｊｐ＿smoothを算出する。制御装置１３は、平滑化位置姿勢Ｊｐ＿smoothを使用して算出した関節角度Ｊｍをマニピュレータ１１に指令として出力し、平滑化位置姿勢Ｊｐ＿smoothをポジショナ１２に指令として出力する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は工業用ロボットに関する。特に、本発明は、ツールを備えるマニピュレータと、ワークを位置決めするポジショナやマニピュレータを移動させるスライダである補助装置とを同期して同時に動作させて加工作業を行う工業用ロボットに関する。
【背景技術】
【０００２】
工業用ロボットのマニピュレータは複数の自由度を有し、この自由度により手首先端に取り付けた溶接トーチ、塗装ガン等のツール（加工具）を任意の位置及び姿勢に位置決めしてワーク（加工対象物）を加工する。工業用ロボットには、マニピュレータに加え、ワークの姿勢変更やツールの作業領域拡大のための補助装置を備えるものがある。この補助装置には、図１６に示すワーク４を単一又は複数の軸周りに回転させるポジショナ（位置決め装置）２や、図１８に示すマニピュレータ１を単一又は複数の軸方向に直進移動させるスライダ（移動装置）３がある。
【０００３】
作業時間短縮と加工品質の向上を図るには、マニピュレータ１と補助装置の動作を同期させ、両者を同時に動作させながら加工作業を行う必要がある。例えば、アーク溶接の場合、ワーク４に対してツール５（溶接トーチ）が最適な下向きの姿勢を維持するようにマニピュレータ１に対してポジショナ２を同期して動作させることで連続溶接が可能となり、溶接作業時間の短縮と溶接品質の向上を図ることができる。また、スライダ３とマニピュレータ１を同期して動作させることで、マニピュレータ１のみでは作業領域が不足するような大型のワーク４の連続溶接が可能となる。
【０００４】
マニピュレータ１と補助装置を同期して動作させて加工作業を行う場合、ワーク４に対するツール５の位置や速度の精度が良好である必要がある。また、溶接、切断、シーリング等の用途では、加工品質を確保するために、ワークに対するツールの移動が等速度であることが必要である。
【０００５】
マニピュレータ１と補助装置が同期するように制御する方法は、例えば特許文献１，２に開示されている。これらの文献に記載の制御方法は、いずれもＰＴＰ型の工業用ロボットに関するものである。この従来の制御方法について、図１６に示すポジショナ２を備える工業用ロボットを例に説明する。マニピュレータ１とポジショナ２の配置、並びにマニピュレータ１及びポジショナ２の各リンク情報は既知である。
【０００６】
マニピュレータ１の自由度は自由度６である。換言すれば、ツール先端５ａの位置及び姿勢は関節角度Ｊｍ（θｍ１，θｍ２，θｍ３，θｍ４，θｍ５，θｍ６）で表される。また、マニピュレータ１に対して直交座標系（ベース座標系Σbase）を設定する。以下、ベース座標系Σbaseでの座標をＸｂ，Ｙｂ，Ｚｂ、座標軸周りの回転角度をαｂ，βｂ，γｂとする。
【０００７】
一方、ポジショナ２の自由度は自由度２である。換言すれば、ポジショナ２の位置及び姿勢は関節角度Ｊｐ（θｐ１，θｐ２）で表される。また、ワーク４に対して直交座標系（ワーク座標系Σwork）を設定する。ワーク座標系Σworkはポジショナ２と共に移動する。以下、ワーク座標系Σworkでの座標をＸｗ，Ｙｗ，Ｚｗ、座標軸周りの回転角度をαｗ，βｗ，γｗとする。
【０００８】
具体的な手順は以下の通りである。
【０００９】
（１）まず、教示位置Ｐｔｅとしてツール先端５ａの位置及び姿勢（関節角度Ｊｍｔｅ）とポジショナ２の位置及び姿勢（関節角度Ｊｐｔｅ）が与えられ（Ｐｔｅ＝Ｊｍｔｅ＋Ｊｐｔｅ）、これらからワーク座標系Σworkにおける教示位置Ｐｔｅでのツール先端５ａの位置及び姿勢workＰｔｅ(Ｘｗｔｅ，Ｙｗｔｅ，Ｚｂｗｔｅ，αｗｔｅ，βｗｔｅ，γｗｔｅ，)を計算する。
【００１０】
（２）次に、現在位置Ｐ０におけるツール先端５ａの位置及び姿勢（関節角度Ｊｍ０）とポジショナ２の位置及び姿勢（関節角度Ｊｐ０）をマニピュレータ１及びポジショナ３の角度センサから取得し（Ｐ０＝Ｊｍ０＋Ｊｐ０）、これらからワーク座標系Σworkにおける現在位置Ｐ０でのツール先端５ａの位置及び姿勢workＰ０(Ｘｗ０，Ｙｗ０，Ｚｗ０，αｗ０，βｗ０，γｗ０)を計算する。
【００１１】
（３）教示位置Ｐｔｅ及び現在位置Ｐ０でのツール先端５ａの位置及び姿勢workＰｔｅ，workＰ０に基づいて、ワーク座標系Σworkにおける教示位置Ｐｔから現在位置Ｐ０までのツール先端５ａの位置及び姿勢の時系列変化（ツール経路方程式workＰ（ｔ），「ｔ」は時刻を示す。）を決定する。また、ポジショナ２の時系列変化（ポジショナ経路方程式Ｊｐ（ｔ））を決定する。
【００１２】
（４）ツール経路方程式workＰ（ｔ）により、制御周期Ｔｃ毎にワーク座標系Σworkにおけるツール先端５ａの位置及び姿勢workＰ（Ｘｗ，Ｙｗ，Ｚｗ，αｗ，βｗ，γｗ）を更新する。また、移動経路Ｊｐ（ｔ）により、制御周期Ｔｃ毎にポジショナ２の関節角度Ｊｐ（θｐ１，θｐ２）を更新する。
【００１３】
（５）（４）で更新したワーク座標系Σworkにおけるツール先端５ａの位置及び姿勢workＰとポジショナ２の関節角度Ｊｐから、座標変換によりベース座標系Σbaseにおけるツール先端５ａの位置及び姿勢baseＰ(Ｘｂ，Ｙｂ，Ｚｂ，αｂ，βｂ，γｂ)を計算する。
【００１４】
（６）（５）で算出したツール先端５ａの位置及び姿勢baseＰの逆キネマティックを計算し、マニピュレータ１の関節角度Ｊｍに変換する。
【００１５】
（７）（６）で計算した関節角度Ｊｍを指令として出力してマニピュレータ１のサーボ駆動系を動作させ、かつ（４）で計算した関節角度Ｊｐを指令として出力してポジショナ２のサーボ駆動系を動作させる。
【００１６】
（８）教示位置Ｐｔｅに到達するまで（４）から（７）を繰り返す。
【００１７】
ワーク４上でのツール先端５ａの移動軌跡である加工線ないしは作業線Ｌが直線であり、かつツール先端５ａが作業線Ｌ上を等速で移動する場合、例えば図１７（Ａ），（Ｂ）において実線で示すように、動作開始時刻ｔ１から動作終了時刻ｔ２まで関節角度Ｊｐが等速（ｄＪｐ／ｄｔ＝Ｖ）となるように、ポジショナ２に対して指令が出力される。
【００１８】
図１８に示すマニピュレータ１とスライダ３を備える工業ロボットについても、前記（１）〜（８）と同様の方法で両者を同期して動作させることができる。現在位置Ｐ０において時刻ｔ１にアークを発生した後、ツール（溶接トーチ）５が教示位置Ｐｔｅに向かって直線の溶接線Ｌ上を一定の溶接速度Ｖで移動し、時刻ｔ２に教示位置Ｐｔｅに到達してアークを停止する場合、現在位置Ｐ０と教示位置Ｐｔｅでマニピュレータ１の姿勢（ベース座標系Σbaseにおけるツール先端５ａの位置及び姿勢）が同一であれば、図１９（Ｂ）に示すように、ベース座標系Σbaseにおけるツール先端５ａの速度は常に０であり、図１９（Ａ）において実線で示すように、Ｙ方向に等速で動作するようにスライダ３に対して指令が出力される。
【００１９】
しかし、この制御方法を採用した従来の工業用ロボットには以下の問題がある。
【００２０】
溶接、塗装、切断、シーリング等の線加工型用途では、加工品質を確保するために、ワーク上の作業線に対してツール先端を高精度で追従させ、かつツール先端を等速度で移動させる必要がある。そのため、動作開始時にはツール先端を停止状態からステップ状に急加速し、動作停止時にはツール先端をステップ状に急減速する必要がある。従来のこの種の工業用ロボットでは、図１７（Ｂ），１９（Ａ）の時刻ｔ１，ｔ２に示すように、マニピュレータ１のみでなくポジショナ２やスライダ３に対しても動作開始時と動作停止時に急激な加減速が指令される。しかし、ポジショナ２はワーク４を保持し、スライダ３はマニピュレータ１を保持しているので、比較的軽量なツール５を保持するマニピュレータ１と比較して移動させる必要のある重量が重く、慣性が大きい。そのため、ポジショナ２やスライダ３は、マニピュレータ１と比較して加速度性能が大幅に劣る。
【００２１】
具体的には、図１６に示すポジショナ２の場合、図１７（Ｂ）において実線で示すように、動作開始時（時刻ｔ１）に１回の制御周期ＴＣ間に速度０の状態から所定の速度Ｖまで関節角度Ｊｐを加速させる指令を出力しても、ポジショナ２の駆動系はこの指令に追従できず、実際の関節角度Ｊｐの速度ｄＪｐ／ｄｔは二点鎖線で示すようになり、応答遅れＴdelayが生じる。また、動作停止時（時刻ｔ２）に１回の制御周期ＴＣ間に速度Ｖから速度０まで関節角度Ｊｐの速度ｄＪｐ／ｄｔを減速させる指令を出力しても、実際の速度ｄＪｐ／ｄｔは二点鎖線で示すようになり、応答遅れＴdelayを生じる。同様に図１８に示すスライダ３の場合も、加減速の指令を駆動系が追従できず、図１９（Ａ）に示すように動作開始時及び動作停止（時刻ｔ１，ｔ２）に応答遅れＴｄｅｌａｙが生じる。スライダ３の動作開始（時刻ｔ１）から応答遅れＴdelayの間は溶接速度が遅いので、アーク溶接による熱入力が過剰となり溶接欠陥を生じる。また、スライダ３の動作停止時（時刻ｔ２）の応答遅れＴdelayのためにツール先端５ａが教示位置Ｐｔｅに到達する前に溶接が終了し、溶接長の不足が生じる。
【００２２】
補助装置で急激な加減速を実現するためには、良好な加速性能を有する高価なモータが必要となり高コストとなる。また、加速性能の良好なモータを使用したとしても、補助装置は一般に機械全体の剛性が低いので、急激な加減速時には大きな振動が生じる。この振動によりツール先端が位置ずれを起こして加工欠陥を生じるだけでなく、補助装置自体に過度の負荷が作用することになる。
【００２３】
動作開始時及び動作停止時以外の作業中で、ワーク４に対するツール先端５ａの速度が一定であっても、ポジショナ２やスライダ３の速度が教示点前後において大きく変動する場合がある。例えば、ポジショナ２の回転方向を反転させる場合や、それまで停止していたスライダ３が大きく移動する場合がある。これらの場合にも、ポジショナ２やスライダ３の加速度性能が低いので前述の応答遅れを生じる。その結果、ツール先端５ａの位置にずれが生じ、加工品質に悪影響を与える。
【００２４】
従来、この問題を解決するには、ポジショナ２やスライダ３の移動量が教示点間で大きく変化しないように、すなわちポジショナ２やスライダ３の速度が大きく変化しないように、操作者が注意深く教示作業を行う必要があった。しかし、このような教示作業は、形状が対称で作業線が滑らかに変化する特殊なワーク（例えばパイプ）を除いて困難である。
【００２５】
このように従来のこの種の工業用ロボットでは、補助装置の加速性能がマニピュレータよりも劣ることを考慮することなく両者を同期させて同時に動作させるので、ワークの作業線に対してツールを精度よく移動させることは困難であった。
【００２６】
【特許文献１】特公平７−１２５９７号公報
【特許文献２】特許第３０９８６１８号
【発明の開示】
【発明が解決しようとする課題】
【００２７】
本発明は、マニピュレータと補助装置を同期させて同時に動作させる工業用ロボットにおいて、ワーク上の指定された経路に対してツールを高精度で移動させることを課題とする。
【課題を解決するための手段】
【００２８】
本発明は、先端にツールを備え、かつこのツールの位置及び姿勢を三次元空間内で変化させるマニピュレータと、前記ツール及び／又は前記マニピュレータを前記三次元空間内で移動させる補助装置と、前記マニピュレータと前記補助装置を同期して同時に動作させる制御装置とを備える工業用ロボットであって、前記制御装置は、前記マニピュレータ自体に設定されたマニピュレータ座標系で与えられるマニピュレータの位置及び姿勢と、前記補助装置自体に設定された補助装置座標系で与えられる補助装置の位置及び姿勢として入力される教示位置から、前記ワークに設定されたワーク座標系におけるツール先端の目標位置及び姿勢を計算する第１の演算手段と、第１の検出器で検出された前記マニピュレータ座標系における前記マニピュレータの位置及び姿勢と、第２の検出器で検出された前記補助装置座標系における前記補助装置の位置及び姿勢とである現在位置から、前記ワーク座標系における前記ツール先端の現在位置及び姿勢を計算する第２の演算手段と、前記ワーク座標系におけるツール先端の目標位置及び姿勢と、前記ワーク座標系におけるツール先端の現在位置及び姿勢と、予め入力された前記ツール先端の移動特性とに基づいて、前記現在位置から前記目標位置までの前記ワーク座標系における前記ツール先端の位置及び姿勢の時系列変化を規定するツール経路方程式を計算する第１の経路算出手段と、前記ツール経路方程式で規定される前記ワーク座標系における前記ツール先端の位置及び姿勢の時系列変化と同期するように、前記現在位置から前記教示位置までの前記補助装置座標系における前記補助装置の位置及び姿勢の時系列変化を規定する補助装置経路方程式を計算する第２の経路算出手段と、前記補助装置経路方程式により規定される前記補助装置の位置及び姿勢の時系列変化を、その加減速度が限界加減速度を下回るように平滑化した平滑化位置姿勢を算出する平滑化手段と、前記平滑化位置姿勢を使用して、前記ツール経路方程式により規定された前記ワーク座標系における前記ツール先端の位置及び姿勢を、前記マニピュレータの基部に設定され、かつ前記三次元空間に対して固定されたベース座標系におけるツール先端の位置及び姿勢に変換する座標変換処理手段と、前記座標変換処理手段により算出された前記ベース座標系におけるツール先端の位置及び姿勢を前記マニピュレータ座標系におけるマニピュレータの位置及び姿勢に変換する逆キネマティクス処理手段と、前記逆キネマティクス処理手段により算出された前記マニピュレータ座標系におけるマニピュレータの位置及び姿勢に基づいて、前記マニピュレータを駆動するマニピュレータ駆動手段と、前記平滑化手段により算出された平滑化位置姿勢に基づいて、前記補助装置を駆動する補助装置駆動手段とを備えることを特徴とする工業用ロボットを提供する。
【００２９】
平滑化手段は、加減速度が限界加減速度を下回るように、補助装置経路方程式により規定される補助装置の位置及び姿勢を平滑化する。この平滑化位置姿勢を使用して、座標変換処理手段がツール経路方程式により規定されるワーク座標系における前記ツール先端の位置及び姿勢をベース座標系に変換する。ベース座標系に変換されたツール先端の位置及び姿勢は、逆キネマティクス処理手段でマニピュレータ座標系に変換された後にマニピュレータの駆動に使用される。一方、補助装置は平滑化位置姿勢に基づいて駆動される。従って、本発明の工業用ロボットでは、加速性能の低い補助装置に対しては限界加減速度を超えるような位置及び姿勢の変化を要求する指令は出力せず、補助装置の加減速度の不足をマニピュレータの動作で補う。そのため、ツール先端は指定された経路を高精度で移動することができ、加工品質が大幅に向上する。また、加速性能の良好な高価なモータ等を補助装置の駆動装置として使用する必要がなく、振動抑制のために補助装置の機械強度や剛性を高める必要もないので大幅なコスト低減を図ることができる。
【００３０】
具体的には、前記平滑化手段は、前記ツールが前記ツール経路方程式で規定される経路に沿って前記現在位置から前記目標位置に移動するまでに要する時間である移動時間と、前記教示位置での前記補助装置座標系における前記補助装置の位置及び姿勢と、前記現在位置での前記補助装置座標系における前記補助装置の位置及び姿勢と、前記補助装置の限界加減速度とに基づいて、前記平滑化位置姿勢を計算する。
【００３１】
さらに具体的には、例えば以下の式に基づいて、平滑化位置姿勢を計算することができる。
【００３２】
【数１】
Ｊps_smooth：平滑化位置姿勢
Ｗ（ｉ）：特性関数
Ｊps（ｉ）：補助装置経路方程式により規定される補助装置の位置及び姿勢
ｉ：１からｍの整数であって、現在の制御周期から数えて第何番目の制御周期であるかを示す。
ｍ：平均化点数
【００３３】
前記平均化点数ｍは、例えば下記の式に基づいて決定される。
【００３４】
【数２】
ΔＪｐｓ：補助装置の目標速度（rad/sec）
Ａｐｓ：補助装置の限界加減速度（rad/sec2）
ＴＣ：制御周期（msec）
【００３５】
目標速度ΔＪｐｓは、移動時間、教示位置での補助装置座標系における補助装置の位置及び姿勢、及び現在位置での補助装置座標系における補助装置の位置及び姿勢により定まる。
【００３６】
特性関数Ｗ（ｉ）として、例えば下記の式で定義される単位ステップ関数状の関数を使用することができる。
【００３７】
【数３】
【００３８】
補助装置経路方程式で規定される経路が等速である場合、この特性関数を使用して平滑化することにより、補助装置の動作開始時の加速度と動作停止時の減速度が限界加減速度未満に低下し、台形型の速度パターンとなる。
【００３９】
代案としては、下記の式で定義される二等辺三角形状の関数を特性関数Ｗ（ｉ）として使用することができる。
【００４０】
【数４】
【００４１】
補助装置経路方程式で規定される経路が等速である場合、この特性関数を使用すると、補助装置の動作開始時の速度の立ち上がりは滑らかな曲線状となる。同様に、補助装置の動作停止時には滑らかな曲線状に速度が低下する。
【００４２】
補助装置は、例えば、ワークを保持し、かつワークを単一又は複数の軸周りに回転させるポジショナと、マニピュレータを保持し、かつマニピュレータを単一又は複数の軸線方向に直線移動させるスライダのいずれか一方又は両方である。
【発明の効果】
【００４３】
本発明の工業用ロボットによれば、ツール先端はワーク上の指定された経路を高精度で移動することができ、加工品質を大幅に向上することができる。また、加速性能の良好な高価なモータ等を補助装置の駆動装置として使用する必要がなく、振動抑制のために補助装置の機械強度や剛性を高める必要もないので、大幅なコスト低減を図ることができる。
【発明を実施するための最良の形態】
【００４４】
次に、添付図面を参照して本発明の実施形態を詳細に説明する。
【００４５】
（第１実施形態）
図１から図５に示す第１実施形態は、マニピュレータ１１とポジショナ１２を備えるＰＴＰ型の工業用ロボットに本発明を適用した例である。
【００４６】
図１を参照すると、工業用ロボットは、マニピュレータ１１、ポジショナ１２、制御装置１３、及び教示装置１４を備える。
【００４７】
マニピュレータ１１は先端にツール１６を備え、かつツール１６の位置及び姿勢を三次元空間内で変化させる。マニピュレータ１１は６個の回転関節ＲＪｍ１，ＲＪｍ２，ＲＪｍ３，ＲＪｍ４，ＲＪｍ５，ＲＪｍ６を有する。回転関節ＲＪｍ１〜ＲＪｍ６間はリンクで連結され、最も基端側の回転関節ＲＪｍ１は台座１１ａに取り付けられている。各回転関節ＲＪｍ１〜ＲＪｍ６は、回転駆動のためのモータＭＯｍ１〜ＭＯｍ６と、関節角度θｍ１〜θｍ２を検出するための角度センサＳＥｍ１〜ＳＥｍ６を備える。
【００４８】
ポジショナ１２はワーク１７を保持するテーブル１２ａと、ワーク１７の位置及び姿勢を三次元空間内で変化させるための２個の回転関節ＲＪｐ１，ＲＪｐ２を備える。回転関節ＲＪｐ１，ＲＪｐ２は、回転駆動のためのモータＭＯｐ１，ＭＯｐ２と、関節角度θｐ１，θｐ２を検出するための角度センサＳＥｐ１，ＳＥｐ２を備える。
【００４９】
次に、マニピュレータ１１及びポジショナ１２の制御に使用する座標を説明する。
【００５０】
マニピュレータ１１に関し、２つの座標系を設定する。まず、６個の関節角度θｍ１〜θｍ６で与えられる回転関節座標系（マニピュレータ座標系）を設定する。マニピュレータ座標系でのマニピュレータ１１の位置及び姿勢（ツール先端１６ａの位置及び姿勢）は、関節角度Ｊｍ（θｍ１，θｍ２，θｍ３，θｍ４，θｍ５，θｍ６）で表記する。また、原点がマニピュレータ１１の台座１１ａに設定され、かつ三次元空間に対して固定された直交座標系（ベース座標系Σbase）を設定する。ベース座標系Σbaseにおけるマニピュレータ１１の位置及び姿勢（ツール先端１６ａの位置及び姿勢）、すなわちベース座標系Σbaseでの座標と座標軸周りの回転角をbaseＰ（Ｘｂ，Ｙｂ，Ｚｂ，αｂ，βｂ，γｂ）で表記する。
【００５１】
ポジショナ１２に関しては、２個の関節角度θｐ１，θｐ１で与えられる回転関節座標系（ポジショナ座標系）を設定する。ポジショナ座標系でのポジショナ１２の位置及び姿勢は、関節角度Ｊｐ（θｐ１，θｐ２）で表記する。
【００５２】
ワーク１７に対して固定された直交座標系（ワーク座標系Σwork）を設定する。ワーク座標系Σworkにおける座標と座標軸周りの回転角をworkＰ（Ｘｗ，Ｙｗ，Ｚｗ，αｗ，βｗ，γｗ）で表記する。
【００５３】
制御装置１３は、ＣＰＵ、後述するＦＩＦＯメモリ１８を含む各種メモリ、及び入出力ポート等を有するコンピュータを備える。制御装置１３は、予め記憶されたプログラムに従って、教示装置１４及び角度センサＳＥｍ１〜ＳＥｍ６，ＳＥｐ１，ＳＥｐ２からの入力に基づいて、モータＭＯｍ１〜ＭＯｍ６，ＭＯｐ１，ＭＯｐ２を駆動する。制御装置１３からマニピュレータ１１及びポジショナ１２のモータＭＯｍ１〜ＭＯｍ６，ＭＯｐ１，ＭＯｐ２に対して出力される指令は、関節角度Ｊｍ（θｍ１〜θｍ６），Ｊｐ（θｐ１，θｐ２）である。
【００５４】
教示装置１４を使用して制御装置１３に対して、制御に必要な条件が入力される。この条件には、教示位置Ｐｔｅに加え、ワーク座標系Σbaseにおけるツール先端１６ａの移動経路と移動速度Ｖset、機械設計上定まるポジショナ１２のモータＭＯｐ１，ＭＯｐ２が制御装置１３からの指令を追従できる限界の加減速度（限界加減速度Ａｐ）等の各種パラメータが含まれる。限界加減速度Ａｐは予め制御装置１３が記憶していてもよい。教示位置Ｐｔｅは、マニピュレータ１１の関節角度Ｊｍｔｅ（θｍｔｅ１，θｍｔｅ２，θｍｔｅ３，θｍｔｅ４，θｍｔｅ５，θｍｔｅ６）とポジショナ１２の関節角度Ｊｐｔｅ（θｐｔｅ１，θｐｔｅ２）として入力される。
【００５５】
次に、図２のフローチャートを参照して、制御装置１３により実行されるマニピュレータ１１及びポジショナ１２の制御を説明する。なお、理解を容易にするために、図３に示すようにワーク１７上の直線の作業線Ｌに沿ってツール先端１６ａが現在位置から目標位置まで等速で移動する場合を例に説明する。
【００５６】
まず、ステップＳ１において、教示位置Ｐｔｅ（Ｊｍｔｅ＋Ｊｐｔｅ）からワーク座標系Σworkにおけるツール先端１６ａの目標位置及び姿勢workＰｔｅ（Ｘｗｔｅ，Ｙｗｔｅ，Ｚｗｔｅ，αｗｔｅ，βｗｔｅ，γｗｔｅ）を計算する。具体的には、まず、教示位置Ｐｔｅでのマニピュレータ１１の関節角度Ｊｍｔｅの順キネマティクスを計算し、ベース座標系Σbaseにおけるツール先端１６ａの位置及び姿勢baseＰｔｅ（Ｘｂｔｅ，Ｙｂｔｅ，Ｚｂｔｅ，αｂｔｅ，βｂｔｅ，γｂｔｅ）に変換する。次に、教示位置Ｐｔｅにおけるポジショナ１２の関節角度Ｊｐｔｅを使用し、座標変換手段によってベース座標系Σbaseにおける教示位置Ｐｔｅでのツール先端１６ａの位置及び姿勢baseＰｔｅを、ワーク座標系Σworkにおけるツール先端１６ａの目標位置及び姿勢workＰｔｅに変換する。順キネマティクスは公知の算出式で算出できる。また、ベース座標系Σbaseからワーク座標系Σworkへの座標変換手段も公知のものを使用できる。機能から見ると、ステップＳ１は本発明における第１の演算手段を構成している。
【００５７】
次に、ステップＳ２において、現在位置Ｐ０におけるマニピュレータ１１の関節角度Ｊｍ０（θｍ１０，θｍ２０，θｍ３０，θｍ４０，θｍ５０，θｍ６０）とポジショナ１２の関節角度Ｊｐ０（θｐ１０，θｐ２０）を角度センサＳＥｍ１〜ＳＥｍ６，ＳＥｐ１，ＳＥｐ２から取得する。また、ステップＳ１と同様の順キネマティクスと座標変換により、現在位置Ｐ０（Ｊｍ０＋Ｊｐ０）からワーク座標系Σworkにおけるツール先端１６ａの現在位置及び姿勢workＰ０（Ｘｗ０，Ｙｗ０，Ｚｗ０，αｗ０，βｗ０，γｗ０）を計算する。機能から見ると、ステップＳ２は本発明における第２の演算手段を構成している。
【００５８】
続いて、ステップＳ３において、ワーク座標系Σworkにおける現在位置及び姿勢workＰ０から目標位置及び姿勢workＰｔｅまでのツール先端１６ａの位置及び姿勢workＰの時系列変化を規定する方程式（ツール経路方程式workＰ（ｔ）、符号ｔは時刻を示す。）を算出する。機能から見ると、ステップＳ３は第１の経路算出手段を構成している。
【００５９】
本実施形態では前述のようにツール先端１６ａは作業線Ｌ上を直線かつ等速で移動するので、ツール経路方程式workＰ（ｔ）は以下のように計算される。まず、ワーク座標系Σworkにおける現在位置及び姿勢workＰ０から目標位置及び姿勢workＰｔｅの間の距離Ｄは以下の式（１）で表される。
【００６０】
【数５】
【００６１】
ワーク座標系Σworkにおけるツール先端１６ａの移動速度の設定値（設定速度）をＶsetとすると、ツール先端１６ａが作業線Ｌに沿って現在位置及び姿勢workＰ０から目標位置及び姿勢workＰｔｅまで移動するのに要する時間（移動時間Ｔｎ）は、以下の式（２）で表される。
【００６２】
【数６】
【００６３】
また、ワーク座標系Σworkにおけるツール先端１６ａの位置及び姿勢workＰの変化の速度ΔworkＰは以下の式（３）で表される。
【００６４】
【数７】
【００６５】
従って、ツール経路方程式workＰ（ｔ）は以下の式（４）に示す通りである。
【００６６】
【数８】
【００６７】
次に、ステップＳ４において、ツール経路方程式workＰ（ｔ）で規定されるワーク座標系Σworkにおけるツール先端１６ａの位置及び姿勢の時系列変化と同期するように、ポジショナ座標系における現在位置Ｐ０から教示位置Ｐｔｅまでのポジショナ１２の関節角度Ｊｐの時系列変化を規定する方程式（ポジショナ経路方程式Ｊｐ（ｔ））を求める。機能から見ると、ステップＳ４は本発明における第２の経路算出手段を構成する。ポジショナ１２の位置及び姿勢の変化の速度ΔＪｐは、現在位置Ｐ０でのポジショナ１２の関節角度Ｊｐ０、教示位置Ｐｔｅでのポジショナ１２の関節角度Ｊｐｔｅ、及び移動時間Ｔｎを用いて以下の式（５）で表される。
【００６８】
【数９】
【００６９】
従って、ポジショナ経路方程式Ｊｐ（ｔ）は以下の式（６）に示す通りである。
【００７０】
【数１０】
【００７１】
次に、ステップＳ５において、時刻ｔを初期化する（ｔ＝０）。
【００７２】
ステップＳ６からステップＳ１２は、制御周期Ｔｃ毎に繰り返される。まず、ステップＳ６において時刻ｔを時刻ｔ＋Ｔｃに更新する。
【００７３】
次に、ステップＳ７において、ステップＳ６で更新した時刻ｔの時点でのワーク座標系Σworkにおけるツール先端１６ａの位置及び姿勢workＰを、ツール経路方程式workＰ（ｔ）により計算する。本実施形態では、式（４）に更新した時刻ｔを代入することで、ツール先端１６ａの位置及び姿勢workＰが得られる。
【００７４】
ステップＳ８では、ステップＳ６で更新した時刻ｔの時点でのポジショナ１２の関節角度Ｊｐを、ポジショナ経路方程式Ｊｐ（ｔ）により計算する。本実施形態では、式（６）に更新した時刻ｔを代入することで、ポジショナ１２の関節角度Ｊｐが得られる。
【００７５】
ステップＳ７，Ｓ８で算出したツール先端１６ａの位置及び姿勢workＰとポジショナ１２の関節角度Ｊｐをそのままマニピュレータ１１及びポジショナ１２に対する駆動指令の算出に使用すると、ポジショナ１２に対して限界加減速度Ａｐを超える急激な加減速を指令することになる。そこで、ステップＳ９では、ステップＳ８でポジショナ経路方程式Ｊｐ（ｔ）により算出したポジショナ１２の関節角度Ｊｐを平滑化し、ポジショナ１２の関節角度Ｊｐの加減速度が限界加減速度Ａｐ未満となる平滑化位置姿勢Ｊｐ＿smoothを算出している。機能から見ると、ステップＳ９は本発明における平滑化手段を構成する。
【００７６】
ステップＳ９の平滑化処理を詳細に説明する。図４を参照すると、制御装置１３は、先入れ先出し型のメモリ（ＦＩＦＯメモリ１８）を備える。このＦＩＦＯメモリ１８にはｍ個のポジショナ１２の位置及び姿勢Ｊｐ（１），Ｊｐ（２），Ｊｐ（３），・・・Ｊｐ（ｍ−１），Ｊｐ（ｍ）が記憶される。具体的には、ＦＩＦＯメモリ１８には、現在の制御周期におけるポジショナ１２の位置及び姿勢Ｊｐ（１）から、現在の制御周期から数えて第ｍ−１番目前の制御周期におけるポジショナ１２の位置及び姿勢Ｊｐ（ｍ）が記憶されている。また、ステップＳ６で時刻ｔが更新されてステップＳ８で新たなポジショナ１２の関節角度Ｊｐが算出される度に、その新たな関節角度ＪｐがＦＩＦＯメモリ１８に記憶され、かつ最も古い関節角度ＪｐがＦＩＦＯメモリ１８から消去される。これらＦＩＦＯメモリ１８に記憶されたｍ個のポジショナ１２の関節角度Ｊｐ（１）〜Ｊｐ（ｍ）に対して、以下の式（７）で定義される積和計算を実行して平滑化位置姿勢Ｊｐ＿smoothを算出する。
【００７７】
【数１１】
【００７８】
式（７）において、Ｊp_smoothはポジショナ１２の平滑化位置姿勢、Ｗ（ｉ）は特性関数、ｉは１からｍの整数であって、現在の制御周期から数えて第何番目前の制御周期であるかを示す。また、ｍは平均化点数であり、ＦＩＦＯメモリ１８が記憶するポジショナ１２の位置及び姿勢Ｊｐの個数に対応する。
【００７９】
図４に概略的に示すように、本実施形態では特性関数Ｗ（ｉ）として単位ステップ関数状の関数を使用して平滑化処理を行っている。本実施形態での特性関数は以下の式（８）で定義される。
【００８０】
【数１２】
【００８１】
式（８）の特性関数Ｗ（ｉ）を使用すると、単純移動平均による平滑化処理となる。ステップＳ８で算出したポジショナ経路方程式Ｊｐ（ｔ）により規定されるポジショナ１２の位置及び姿勢Ｊｐの時系列変化をそのままポジショナ１２に出力すると、図４において二点点線で示すように、動作開始時刻ｔ１及び動作開始時刻ｔ２にステップ状の急激な加減速をポジショナ１２に強いることになる。これに対して式（７），（８）により算出したポジショナ１２の平滑化位置姿勢Ｊｐ＿smoothは、図４において実線で示すように、動作開始時及び動作停止時の加減速度が低減された台形型速度パターンを呈する。詳細には、動作開始時には平滑化後のポジショナ１２の速度ｄＪｐ＿smooth／ｄｔは０から式（５）の速度ΔＪｐまで直線的に増加し、加速時間はｍ＊Ｔｃである（図５ではｍ＝５としている）。同様に、動作停止時には、平滑化後のポジショナ１２の速度ｄＪｐ＿smooth／ｄｔは速度ΔＪｐから０まで直線的に減少し、減速時間はｍ＊Ｔｃである。
【００８２】
加減速に要する時間はｍ＊Ｔｃで、制御周期ＴＣは定数（例えば２６０msec）である。従って、平滑化位置姿勢Ｊｐ＿smoothの加減速度が最適、すなわち
ポジショナ１２の限界加減速度Ａｐを超えない範囲で、加減速に要する時間が最短となるようにするには、平均化点数ｍを以下のように設定すればよい。なお、限界加減速度Ａｐの単位はrad／sec２、ポジショナ１２の位置及び姿勢の変化の速度ΔＪｐの単位はrad／sec、制御周期Ｔｃの単位はmsecとする。まず、ポジショナ１２の位置及び姿勢Ｊｐの最適加減速時間Ｔａｃｃは以下の式（９）で表される。
【００８３】
【数１３】
【００８４】
ポジショナ１２の関節角度Ｊｐ（ｉ）は、制御周期Ｔｃ毎に離散的に与えられるので、最適加減速時間Ｔａｃｃに対応する平均化点数ｍは以下の式（１０）で示す通りとなる。
【００８５】
【数１４】
【００８６】
また、式（１０）に式（９）を代入すると、最適な平均化点数ｍは以下の式（１１）で表される。
【００８７】
【数１５】
【００８８】
式（７），（１１）から明らかなように、ステップＳ９では、移動時間Ｔｎ、教示位置Ｐｔｅでのポジショナ１２の関節角度Ｊｐｔｅ、現在位置Ｐ０でのポジショナ１２の関節角度Ｊｐ０、並びに限界加減速度Ａｐに基づいて平滑化位置姿勢Ｊｐ＿smoothを算出している。式（１０），（１１）により決定した平均化点数ｍで、式（７）の積和計算を実行することにより、限界加減速度Ａｐを超えない範囲で加減速に要する時間が最短となる平滑化位置姿勢Ｊｐ＿smoothを求めることができる。
【００８９】
ステップＳ１０及びＳ１１は、マニピュレータ１１に対する指令を計算するための処理である。なお、ポジショナ１２に対する指令は平滑化位置姿勢Ｊｐ＿smoothが使用される。まず、ステップＳ１０において、ステップＳ７で計算したワーク座標系Σworkにおけるツール先端１６ａの位置及び姿勢workＰを、ベース座標系Σbaseにおけるツール先端１６ａの位置及び姿勢baseＰに変換する。この変換には、ステップＳ８で算出したポジショナ１２の位置及び姿勢Ｊｐではなく、ステップＳ９で算出した平滑化位置姿勢Ｊｐ＿smoothを使用し、前述の座標変換手段により行う。次に、ステップＳ１１において、ステップＳ１０で算出したベース座標系Σbaseにおけるツール先端１６ａの位置及び姿勢baseＰの逆キネマティクスを計算し、マニピュレータ座標系、すなわち関節角度Ｊｍに変換する。機能から見ると、ステップＳ１０は本発明における座標変換処理手段を構成し、ステップＳ１１は逆キネマティクス処理手段を構成する。
【００９０】
ステップＳ１２において、ステップＳ１１で算出した関節角度Ｊｍを指令としてマニピュレータ１１に出力する。また、ステップＳ９で算出した平滑化位置姿勢Ｊｐ＿smoothを指令としてポジショナ１２に出力する。これらの指令によりマニピュレータ１１のモータＭＯｍ１〜ＭＯｍ６とポジショナ１２のモータＭＯｐ１，ＭＯｐ２が作動し、マニピュレータ１１とポジショナ１２が同期して同時に動作する。機能から見ると、ステップＳ１２は本発明におけるマニピュレータ駆動手段及び補助装置駆動手段を構成する。
【００９１】
ステップＳ１３において教示位置Ｐｔｅに到達するまで、ステップＳ６からステップＳ１２の処理が繰り返される。
【００９２】
ステップＳ８でポジショナ経路方程式Ｊｐ（ｔ）から算出したポジショナ１２の位置及び姿勢Ｊｐをそのままポジショナ１２に指令として出力すると、図５において二点鎖線で示すように、ポジショナ１２に対して動作開始時及び動作停止時（時刻ｔ１，ｔ２）に急激な加減速を指令することになる。しかし、本実施形態では、ステップＳ９で算出した平滑化位置姿勢Ｊｐ＿smoothがポジショナ１２に対して指令として出力されるので、実線で示すような加減速が緩やかな台形型の速度パターンとなり、加速性能の低いポジショナ１２のモータＭＯｐ１，ＭＯｐ２に対して限界加減速度Ａｐを上回るような位置及び姿勢の変化を要求する指令は出力されない。従って、ポジショナ１２に応答遅れが生じない。
【００９３】
単に平滑化位置姿勢Ｊｐ＿smoothをポジショナ１２に対する指令として使用するだけでは、ワーク座標系Σworkにおけるツール先端１６ａの加減速度が不足し、ツール先端１６ａを正確に作業線Ｌ上で移動させることもできない。しかし、本実施形態では、平滑化位置姿勢Ｊｐ＿smoothを使用してワーク座標系Σworkからベース座標系Σbaseへの座標変換を行い（ステップＳ１０）、その逆キネマティクスである関節角度Ｊｍをマニピュレータ１１のモータＭＯｍ１〜ＭＯｍ６に指令として出力する（ステップＳ１１，Ｓ１２）。そのため、マニピュレータ１１は、ポジショナ１２の加減速度の不足を補い、ツール先端１６ａが作業線Ｌ上を設定速度Ｖsetで移動するように動作する。例えば、ツール先端１６ａは図５の時刻ｔ２に作業線Ｌの終点に到達するが、この時刻ｔ２からポジショナ１２の動作が完全に停止する時刻ｔ２’の間、ツール先端１６ａが作業線Ｌの終点に位置したままで保持されるように、ポジショナ１２の関節角度Ｊｐの変化に応じてマニピュレータ１１が作動する。
【００９４】
以上のように、加速性能の低いポジショナ１２に対しては急激な加減速の指令を出力せず、ポジショナ１２の加減速度の不足を加速性能の高いマニピュレータ１１の動作で補うので、ツール先端１６ａは良好な位置及び速度の精度を維持しつつワーク１７上の作業線Ｌを移動することができ、加工品質が大幅に向上する。また、加速性能の良好な高価なモータをポジショナ１２のモータＭＯｐ１，ＭＯｐ２として使用する必要がなく、振動抑制のためにポジショナ１２の機械強度や剛性を高める必要もないので大幅なコスト低減を図ることができる。
【００９５】
（第２実施形態）
図６から図９に示す第２実施形態は、マニピュレータ１１とスライダ２２を備えるＰＴＰ型の工業用ロボットに本発明を適用した例である。
【００９６】
スライダ２２は、互いに直交する方向に直進移動する３個の直動関節ＳＪ１，ＳＪ２，ＳＪ３を備え、直動関節ＳＪｐ１にマニピュレータ１１の台座１１ａが保持されている。スライダ２２に関し、座標軸がそれぞれ直動関節ＳＪｐ１〜ＳＪｐ３の移動方向に対応し、かつ三次元空間に固定された直交座標系（スライダ座標系）を設定する。スライダ座標系でのスライダ２２の位置及び姿勢は、関節位置Ｊｓ（Ｓｘ，Ｓｙ，Ｓｚ）で表記する。スライダ座標系の各軸の向きはベース座標系Σbaseの各軸の向きと一致している。各直動関節ＳＪｐ１〜ＳＪｐ３は、それぞれ駆動用のモータＭＯｓ１〜ＭＯＳ３と、関節位置Ｊｓを検出するための位置センサＳＥｓ１〜ＳＥｓ３を備える。
【００９７】
ワーク１７は三次元空間に固定されている。ワーク座標系Σworkの各軸の向きは、ベース座標系Σbase及びスライダ座標系の各軸の向きと一致している。
【００９８】
教示装置１４から制御装置１３には、教示位置Ｐｔｅに加え、ワーク座標系Σbaseにおけるツール先端１６ａの移動経路と移動速度Ｖset、スライダ２２のモータＭＯｓ１〜ＭＯｓ３の限界加減速度Ａｐ等を含む制御に必要な条件が入力される。教示位置Ｐｔｅは、マニピュレータ１１の関節角度Ｊｍｔｅ（θｍｔｅ１，θｍｔｅ２，θｍｔｅ３，θｍｔｅ４，θｍｔｅ５，θｍｔｅ６）とスライダ２２の関節位置Ｊｓｔｅ（Ｓｘｔｅ，Ｓｙｔｅ，Ｓｚｔｅ）として入力される。
【００９９】
図７に示す制御は、ポジショナ１２がスライダ２２に置き換わる点を除いて第１実施形態における制御（図２参照）と同様である。理解を容易にするために、図８に示すようにワーク１７上の作業線Ｌはベース座標系Σbase、ワーク座標系Σwork、及びスライダ座標系のＹ方向に延びる直線であり、この作業線Ｌに沿ってツール先端１６ａが設定速度Ｖsetで等速移動し、かつ現在位置Ｐ０と教示位置Ｐｔｅでのマニピュレータ座標系におけるマニピュレータ１１の位置及び姿勢が同一（Ｊｍ０＝Ｊｍｔｅ）である場合を例に、本実施形態における制御を説明する。
【０１００】
ステップＳ１’において教示位置Ｐｔｅ（Ｊｍｔｅ＋Ｊｓｔｅ）からワーク座標系Σworkにおけるツール先端１６ａ目標位置及び姿勢workＰｔｅを計算し、ステップＳ２’において現在位置Ｐ０からワーク座標系Σworkにおけるツール先端１６ａの現在位置及び姿勢workＰ０を計算する。現在位置Ｐ０は、角度センサＳＥｍ１〜ＳＥｍ６から取得したマニピュレータ１１の現在の関節角度Ｊｍ０と、位置センサＳＥｓ１〜ＳＥｓ３で取得したスライダ２２の現在の関節位置Ｊｓ０（Ｓｘ０，Ｓｙ０，Ｓｚ０）として与えられる。
【０１０１】
ステップＳ３’で求めるツール経路方程式wokrＰ（ｔ）は、第１実施形態と同一であり前述の式（４）で示す通りとなる。
【０１０２】
ステップＳ４’において、ワーク座標系Σworkにおけるツール先端１６ａの位置及び姿勢の時系列変化と同期するように、現在位置Ｐ０から教示位置Ｐｔｅまでのスライダ座標系におけるスライダ２２の位置及び姿勢の時系列変化を規定する方程式（スライダ経路方程式Ｊｓ（ｔ））を求める。スライダ２２の位置及び姿勢の変化の速度ΔＪｓは、現在位置Ｐ０でのスライダ２２の関節位置Ｊｓ０と教示位置Ｐｔｅでのポジショナ１２の関節位置Ｊｓｔｅ、及び移動時間Ｔｎを用いて以下の式（１２）で表される。
【０１０３】
【数１６】
【０１０４】
従って、スライダ経路方程式Ｊｓ（ｔ）は以下の式（１３）に示す通りである。
【０１０５】
【数１７】
【０１０６】
時刻ｔを初期化後（ステップＳ５’）、制御周期Ｔｃ毎に時刻ｔを更新しつつ、ツール経路方程式wokrＰ（ｔ）によりワーク座標系Σworkにおけるツール先端１６ａの位置及び姿勢workＰを計算し、スライダ経路方程式Ｊｓ（ｔ）によりスライダ２２の位置及び姿勢Ｊｐを計算する（ステップＳ７’，Ｓ８’）。また、ステップＳ９’において、加減速度が限界加減速度Ａｐを超えないように、スライダ２２の関節位置ＪＳを平滑化処理する。具体的には、式（９）〜（１１）で決まる最適な平均化点数ｍと、式（８）で定義される特性関数Ｗ（ｉ）を使用し、式（７）の積和演算を実行して平滑化位置姿勢Ｊｓ＿smoothを算出する。
【０１０７】
平滑化位置姿勢Ｊｓ＿smoothをスライダ２２に出力し、平滑化位置姿勢Ｊｓ＿smoothを用いてワーク座標系Σworkからベース座標系Σbaseに座標変換したツール先端の逆キネマティクス（関節角度Ｊｍ）をマニピュレータ１１に出力する（ステップＳ１０’〜Ｓ１２’）。
【０１０８】
図９（Ａ）はスライダ２２のＹ方向の移動速度の変化を示し、図９（Ｂ）はベース座標系Σbaseでのツール先端１６ａのＹ方向の移動速度の変化を示し、図９（Ｃ）はワーク座標系Σworkでのツール先端１６ａのＹ方向の移動速度（ワーク１７に対するツール先端１６ａの移動速度）を示す。
【０１０９】
図７のステップＳ８’でスライダ経路方程式Ｊｓ（ｔ）により算出したスライダ２２の関節位置Ｊｓをそのままスライダ２２に指令として出力すると、図９（Ａ）の二点鎖線で示すように、スライダ２２に対して動作開始時及び動作停止時（時刻ｔ１，ｔ２）に急激な加減速を指令することになる。しかし、本実施形態では、ステップＳ９’で算出した平滑化位置姿勢Ｊｓ＿smoothがスライダ２２に対して指令として出力されるので、実線で示すような加減速が緩やかな台形型の速度パターンとなり、加速性能の低いスライダ２２のモータＭＯｓ１〜ＭＯｓ３に対して限界加減速度Ａｐを上回るような位置及び姿勢の変化を要求する指令は出力されない。
【０１１０】
単に平滑化位置姿勢Ｊｓ＿smoothをスライダ２２に対する指令として使用するだけでは、ワーク座標系Σworkにおけるツール先端１６ａの加減速度が不足する。具体的には、動作開始（時刻ｔ１）からツール先端１６ａが設定速度Ｖsetまで加速するのに要する時間が長くなる。また、動作停止時に設定速度Ｖsetから速度０まで減速する時間が長くなり所望の時刻ｔ２までにツール先端１６ａが作業線Ｌの終端まで到達できない。
【０１１１】
しかし、本実施形態では、スライダ経路方程式Ｊｓ（ｔ）により算出したスライダ２２の位置及び姿勢Ｊｓではなく、平滑化位置姿勢Ｊｓ＿smoothを使用して、マニピュレータ１１に対する指令（関節角度Ｊｍ）を算出しているので（ステップＳ１０’）、スライダ２２の加減速度を制限したことによるツール先端１６ａの加減速度の不足を補うようにマニピュレータ１１が作動する。具体的には、動作開始時には時刻ｔ１’にスライダ２２が設定速度Ｖsetに達するまで、ツール先端１６ａがＹ方向に移動するようにマニピュレータ１１が動作する。その結果、スライダ２２の移動とマニピュレータ１１によるツール先端１６ａの移動の重ね合わせによって、加速度の不足が補われ、図９（Ｃ）に示すように１回の制御周期Ｔｃでワーク１７に対するツール先端１６ａの移動速度が設定速度Ｖsetまで上昇する。また、動作停止時には時刻ｔ２’にスライダ２２が完全に停止するまで、ツール先端１６ａがＹ軸方向に移動するようにマニピュレータ１１が動作する。その結果、スライダ２２の移動とマニピュレータ１１によるツール先端１６ａの移動の重ね合わせによって、ツール先端１６ａが作業線Ｌの終端に保持される。
【０１１２】
以上のように、加速性能の低いスライダ２２に対しては急激な加減速の指令を出力せず、スライダ２２の加減速度の不足を加速性能の高いマニピュレータ１１の動作で補うので、ツール先端１６ａは良好な位置及び速度の精度を維持しつつワーク１７上の作業線Ｌを移動することができ、加工品質が大幅に向上する。また、加速性能の良好な高価なモータをスライダ２２のモータＭＯｓ１〜ＭＯｓ３として使用する必要がなく、振動抑制のためにスライダ２２の機械強度や剛性を高める必要もないので大幅なコスト低減を図ることができる。
【０１１３】
第２実施形態のその他の構成及び作用は第１実施形態と同様であるので、同一の要素には同一の符号を付して説明を省略する。
【０１１４】
（第３実施形態）
図１０に示すように、マニピュレータ１１とポジショナ１２及びスライダ２２を備えるＰＴＰ型の工業用ロボットにも本発明を適用することができる。
【０１１５】
制御装置１３は、図１１のステップＳ１''〜Ｓ１３''に示す第１及び第２実施形態と同様の制御を実行する。教示位置Ｐｔｅと現在位置Ｐ０は、マニピュレータ１１の関節角度Ｊｍｔｅ，Ｊｍ０、ポジショナ１２の関節角度Ｊｐｔｅ，Ｊｐ０、及びスライダ２２の関節位置Ｊｓｔｅ，Ｊｓ０で与えられる（ステップＳ１''，Ｓ２''）。ポジショナ経路方程式Ｊｐ（ｔ）とスライダ経路方程式Ｊｓ（ｔ）の両方を求め（ステップＳ４''）、これらから計算した関節角度Ｊｐと関節位置Ｊｓをそれぞれ平滑化処理して平滑化位置姿勢Ｊｐ＿smooth，Ｊｓ＿smoothを計算する（ステップＳ８''，Ｓ９''）。また、両方の平滑化位置姿勢Ｊｐ＿smooth，Ｊｓ＿smoothを使用してツール先端１６ａの座標をワーク座標系Σworkからベース座標系Σbaseに変換し、逆キネマティクスを計算してマニピュレータ１１の関節角度Ｊｍを得る（ステップＳ１０''，Ｓ１１''）。
【０１１６】
スライダ２２とポジショナ１２に対して平滑化位置姿勢Ｊｐ＿smooth，Ｊｓ＿smoothを指令として出力し、マニピュレータ１１にはこれらから算出した関節角度Ｊｍを指令として出力する（ステップＳ１２''）。従って、加速性能の低いポジショナ１２とスライダ２２に対しては急激な加減速の指令は出力せず、ポジショナ１２及びスライダ２２の加減速度の不足を加速性能の高いマニピュレータ１１の動作で補うので、ツール先端１６ａは良好な位置及び速度の精度を維持しつつワーク１７上の作業線Ｌを移動することができる。
【０１１７】
第３実施形態のその他の構成及び作用は第１実施形態と同様である。同一の要素には同一の符号を付して説明を省略する。
【０１１８】
本発明は、前記実施形態に限定されず、種々の変形が可能である。例えば、図１２に示すピーク値が１の二等辺三角形状の特性を有する特性関数Ｗ（ｉ）を使用して式（７）の積和計算を実行することで、ポジショナ１２やスライダ２２の平滑化位置姿勢Ｊｐ＿smooth，Ｊｓ＿smoothを算出してもよい。この特性関数Ｗ（ｉ）は、以下の式（１４）で定義される。
【０１１９】
【数１８】
【０１２０】
この特性関数Ｗ（ｉ）を使用した場合、図１３において実線で示すようにポジショナ１２やスライダ２２の速度パターンは、動作開始時（時刻ｔ１）の速度の立ち上がりは滑らかな曲線状となる。同様に、ポジショナ１２やスライダ２２の動作停止時（時刻ｔ２）には滑らかな曲線状に速度が低下する。
【０１２１】
一般に特性関数は対象であればどのような形であってもよい。例えば、図１２の特性関数の形を少し変更した図１４に示す特性関数をであってもよい。この場合、図１５において実線で示すようにポジショナ１２やスライダ２２の速度パターンは、動作開始時（時刻ｔ１）の速度の立ち上がり及び動作停止時（時刻ｔ２）の速度低下がより一層滑らかな曲線状となる。
【図面の簡単な説明】
【０１２２】
【図１】本発明の第１実施形態に係る工業用ロボットを示す概略構成図。
【図２】本発明の第１実施形態に係る工業用ロボットの動作を説明するためのフローチャート。
【図３】マニピュレータとポジショナの同期動作を説明するための概略図。
【図４】平滑化処理を説明するための概略図。
【図５】第１実施形態におけるポジショナの速度変化を示す線図。
【図６】本発明の第２実施形態に係る工業用ロボットを示す概略構成図。
【図７】本発明の第２実施形態に係る工業用ロボットの動作を説明するためのフローチャート。
【図８】マニピュレータとスライダの同期動作を説明するための概略図。
【図９】（Ａ）はスライダの速度変化を示す線図、（Ｂ）はマニピュレータの速度変化を示す線図、（Ｃ）はスライダとマニピュレータの合成速度を示す線図。
【図１０】本発明の第３実施形態に係る工業用ロボットを示す概略構成図。
【図１１】本発明の第３実施形態に係る工業用ロボットの動作を説明するためのフローチャート。
【図１２】平滑処理の他の例を説明するための概略図。
【図１３】図１２の平滑処理を行った場合のポジショナ及びスライダの速度変化の一例を示す線図。
【図１４】平滑処理のさらに他の例を説明するための概略図。
【図１５】図１４の平滑処理を行った場合のポジショナ及びスライダの速度変化の一例を示す線図。
【図１６】従来の工業用ロボットの一例を示す概略斜視図。
【図１７】（Ａ）はマニピュレータの変位を示す線図、（Ｂ）はマニピュレータの速度変化を示す線図。
【図１８】従来の工業用ロボットの他の一例を示す概略平面図。
【図１９】（Ａ）はスライダの速度変化を示す線図、（Ｂ）はマニピュレータの速度変化を示す線図。
【符号の説明】
【０１２３】
１１ マニピュレータ
１１ａ 台座
１２ ポジショナ
１２ａ テーブル
１３ 制御装置
１４ 教示装置
１６ ツール
１６ａ ツール先端
１７ ワーク
１８ ＦＩＦＯメモリ
２２ スライダ
【技術分野】
【０００１】
本発明は工業用ロボットに関する。特に、本発明は、ツールを備えるマニピュレータと、ワークを位置決めするポジショナやマニピュレータを移動させるスライダである補助装置とを同期して同時に動作させて加工作業を行う工業用ロボットに関する。
【背景技術】
【０００２】
工業用ロボットのマニピュレータは複数の自由度を有し、この自由度により手首先端に取り付けた溶接トーチ、塗装ガン等のツール（加工具）を任意の位置及び姿勢に位置決めしてワーク（加工対象物）を加工する。工業用ロボットには、マニピュレータに加え、ワークの姿勢変更やツールの作業領域拡大のための補助装置を備えるものがある。この補助装置には、図１６に示すワーク４を単一又は複数の軸周りに回転させるポジショナ（位置決め装置）２や、図１８に示すマニピュレータ１を単一又は複数の軸方向に直進移動させるスライダ（移動装置）３がある。
【０００３】
作業時間短縮と加工品質の向上を図るには、マニピュレータ１と補助装置の動作を同期させ、両者を同時に動作させながら加工作業を行う必要がある。例えば、アーク溶接の場合、ワーク４に対してツール５（溶接トーチ）が最適な下向きの姿勢を維持するようにマニピュレータ１に対してポジショナ２を同期して動作させることで連続溶接が可能となり、溶接作業時間の短縮と溶接品質の向上を図ることができる。また、スライダ３とマニピュレータ１を同期して動作させることで、マニピュレータ１のみでは作業領域が不足するような大型のワーク４の連続溶接が可能となる。
【０００４】
マニピュレータ１と補助装置を同期して動作させて加工作業を行う場合、ワーク４に対するツール５の位置や速度の精度が良好である必要がある。また、溶接、切断、シーリング等の用途では、加工品質を確保するために、ワークに対するツールの移動が等速度であることが必要である。
【０００５】
マニピュレータ１と補助装置が同期するように制御する方法は、例えば特許文献１，２に開示されている。これらの文献に記載の制御方法は、いずれもＰＴＰ型の工業用ロボットに関するものである。この従来の制御方法について、図１６に示すポジショナ２を備える工業用ロボットを例に説明する。マニピュレータ１とポジショナ２の配置、並びにマニピュレータ１及びポジショナ２の各リンク情報は既知である。
【０００６】
マニピュレータ１の自由度は自由度６である。換言すれば、ツール先端５ａの位置及び姿勢は関節角度Ｊｍ（θｍ１，θｍ２，θｍ３，θｍ４，θｍ５，θｍ６）で表される。また、マニピュレータ１に対して直交座標系（ベース座標系Σbase）を設定する。以下、ベース座標系Σbaseでの座標をＸｂ，Ｙｂ，Ｚｂ、座標軸周りの回転角度をαｂ，βｂ，γｂとする。
【０００７】
一方、ポジショナ２の自由度は自由度２である。換言すれば、ポジショナ２の位置及び姿勢は関節角度Ｊｐ（θｐ１，θｐ２）で表される。また、ワーク４に対して直交座標系（ワーク座標系Σwork）を設定する。ワーク座標系Σworkはポジショナ２と共に移動する。以下、ワーク座標系Σworkでの座標をＸｗ，Ｙｗ，Ｚｗ、座標軸周りの回転角度をαｗ，βｗ，γｗとする。
【０００８】
具体的な手順は以下の通りである。
【０００９】
（１）まず、教示位置Ｐｔｅとしてツール先端５ａの位置及び姿勢（関節角度Ｊｍｔｅ）とポジショナ２の位置及び姿勢（関節角度Ｊｐｔｅ）が与えられ（Ｐｔｅ＝Ｊｍｔｅ＋Ｊｐｔｅ）、これらからワーク座標系Σworkにおける教示位置Ｐｔｅでのツール先端５ａの位置及び姿勢workＰｔｅ(Ｘｗｔｅ，Ｙｗｔｅ，Ｚｂｗｔｅ，αｗｔｅ，βｗｔｅ，γｗｔｅ，)を計算する。
【００１０】
（２）次に、現在位置Ｐ０におけるツール先端５ａの位置及び姿勢（関節角度Ｊｍ０）とポジショナ２の位置及び姿勢（関節角度Ｊｐ０）をマニピュレータ１及びポジショナ３の角度センサから取得し（Ｐ０＝Ｊｍ０＋Ｊｐ０）、これらからワーク座標系Σworkにおける現在位置Ｐ０でのツール先端５ａの位置及び姿勢workＰ０(Ｘｗ０，Ｙｗ０，Ｚｗ０，αｗ０，βｗ０，γｗ０)を計算する。
【００１１】
（３）教示位置Ｐｔｅ及び現在位置Ｐ０でのツール先端５ａの位置及び姿勢workＰｔｅ，workＰ０に基づいて、ワーク座標系Σworkにおける教示位置Ｐｔから現在位置Ｐ０までのツール先端５ａの位置及び姿勢の時系列変化（ツール経路方程式workＰ（ｔ），「ｔ」は時刻を示す。）を決定する。また、ポジショナ２の時系列変化（ポジショナ経路方程式Ｊｐ（ｔ））を決定する。
【００１２】
（４）ツール経路方程式workＰ（ｔ）により、制御周期Ｔｃ毎にワーク座標系Σworkにおけるツール先端５ａの位置及び姿勢workＰ（Ｘｗ，Ｙｗ，Ｚｗ，αｗ，βｗ，γｗ）を更新する。また、移動経路Ｊｐ（ｔ）により、制御周期Ｔｃ毎にポジショナ２の関節角度Ｊｐ（θｐ１，θｐ２）を更新する。
【００１３】
（５）（４）で更新したワーク座標系Σworkにおけるツール先端５ａの位置及び姿勢workＰとポジショナ２の関節角度Ｊｐから、座標変換によりベース座標系Σbaseにおけるツール先端５ａの位置及び姿勢baseＰ(Ｘｂ，Ｙｂ，Ｚｂ，αｂ，βｂ，γｂ)を計算する。
【００１４】
（６）（５）で算出したツール先端５ａの位置及び姿勢baseＰの逆キネマティックを計算し、マニピュレータ１の関節角度Ｊｍに変換する。
【００１５】
（７）（６）で計算した関節角度Ｊｍを指令として出力してマニピュレータ１のサーボ駆動系を動作させ、かつ（４）で計算した関節角度Ｊｐを指令として出力してポジショナ２のサーボ駆動系を動作させる。
【００１６】
（８）教示位置Ｐｔｅに到達するまで（４）から（７）を繰り返す。
【００１７】
ワーク４上でのツール先端５ａの移動軌跡である加工線ないしは作業線Ｌが直線であり、かつツール先端５ａが作業線Ｌ上を等速で移動する場合、例えば図１７（Ａ），（Ｂ）において実線で示すように、動作開始時刻ｔ１から動作終了時刻ｔ２まで関節角度Ｊｐが等速（ｄＪｐ／ｄｔ＝Ｖ）となるように、ポジショナ２に対して指令が出力される。
【００１８】
図１８に示すマニピュレータ１とスライダ３を備える工業ロボットについても、前記（１）〜（８）と同様の方法で両者を同期して動作させることができる。現在位置Ｐ０において時刻ｔ１にアークを発生した後、ツール（溶接トーチ）５が教示位置Ｐｔｅに向かって直線の溶接線Ｌ上を一定の溶接速度Ｖで移動し、時刻ｔ２に教示位置Ｐｔｅに到達してアークを停止する場合、現在位置Ｐ０と教示位置Ｐｔｅでマニピュレータ１の姿勢（ベース座標系Σbaseにおけるツール先端５ａの位置及び姿勢）が同一であれば、図１９（Ｂ）に示すように、ベース座標系Σbaseにおけるツール先端５ａの速度は常に０であり、図１９（Ａ）において実線で示すように、Ｙ方向に等速で動作するようにスライダ３に対して指令が出力される。
【００１９】
しかし、この制御方法を採用した従来の工業用ロボットには以下の問題がある。
【００２０】
溶接、塗装、切断、シーリング等の線加工型用途では、加工品質を確保するために、ワーク上の作業線に対してツール先端を高精度で追従させ、かつツール先端を等速度で移動させる必要がある。そのため、動作開始時にはツール先端を停止状態からステップ状に急加速し、動作停止時にはツール先端をステップ状に急減速する必要がある。従来のこの種の工業用ロボットでは、図１７（Ｂ），１９（Ａ）の時刻ｔ１，ｔ２に示すように、マニピュレータ１のみでなくポジショナ２やスライダ３に対しても動作開始時と動作停止時に急激な加減速が指令される。しかし、ポジショナ２はワーク４を保持し、スライダ３はマニピュレータ１を保持しているので、比較的軽量なツール５を保持するマニピュレータ１と比較して移動させる必要のある重量が重く、慣性が大きい。そのため、ポジショナ２やスライダ３は、マニピュレータ１と比較して加速度性能が大幅に劣る。
【００２１】
具体的には、図１６に示すポジショナ２の場合、図１７（Ｂ）において実線で示すように、動作開始時（時刻ｔ１）に１回の制御周期ＴＣ間に速度０の状態から所定の速度Ｖまで関節角度Ｊｐを加速させる指令を出力しても、ポジショナ２の駆動系はこの指令に追従できず、実際の関節角度Ｊｐの速度ｄＪｐ／ｄｔは二点鎖線で示すようになり、応答遅れＴdelayが生じる。また、動作停止時（時刻ｔ２）に１回の制御周期ＴＣ間に速度Ｖから速度０まで関節角度Ｊｐの速度ｄＪｐ／ｄｔを減速させる指令を出力しても、実際の速度ｄＪｐ／ｄｔは二点鎖線で示すようになり、応答遅れＴdelayを生じる。同様に図１８に示すスライダ３の場合も、加減速の指令を駆動系が追従できず、図１９（Ａ）に示すように動作開始時及び動作停止（時刻ｔ１，ｔ２）に応答遅れＴｄｅｌａｙが生じる。スライダ３の動作開始（時刻ｔ１）から応答遅れＴdelayの間は溶接速度が遅いので、アーク溶接による熱入力が過剰となり溶接欠陥を生じる。また、スライダ３の動作停止時（時刻ｔ２）の応答遅れＴdelayのためにツール先端５ａが教示位置Ｐｔｅに到達する前に溶接が終了し、溶接長の不足が生じる。
【００２２】
補助装置で急激な加減速を実現するためには、良好な加速性能を有する高価なモータが必要となり高コストとなる。また、加速性能の良好なモータを使用したとしても、補助装置は一般に機械全体の剛性が低いので、急激な加減速時には大きな振動が生じる。この振動によりツール先端が位置ずれを起こして加工欠陥を生じるだけでなく、補助装置自体に過度の負荷が作用することになる。
【００２３】
動作開始時及び動作停止時以外の作業中で、ワーク４に対するツール先端５ａの速度が一定であっても、ポジショナ２やスライダ３の速度が教示点前後において大きく変動する場合がある。例えば、ポジショナ２の回転方向を反転させる場合や、それまで停止していたスライダ３が大きく移動する場合がある。これらの場合にも、ポジショナ２やスライダ３の加速度性能が低いので前述の応答遅れを生じる。その結果、ツール先端５ａの位置にずれが生じ、加工品質に悪影響を与える。
【００２４】
従来、この問題を解決するには、ポジショナ２やスライダ３の移動量が教示点間で大きく変化しないように、すなわちポジショナ２やスライダ３の速度が大きく変化しないように、操作者が注意深く教示作業を行う必要があった。しかし、このような教示作業は、形状が対称で作業線が滑らかに変化する特殊なワーク（例えばパイプ）を除いて困難である。
【００２５】
このように従来のこの種の工業用ロボットでは、補助装置の加速性能がマニピュレータよりも劣ることを考慮することなく両者を同期させて同時に動作させるので、ワークの作業線に対してツールを精度よく移動させることは困難であった。
【００２６】
【特許文献１】特公平７−１２５９７号公報
【特許文献２】特許第３０９８６１８号
【発明の開示】
【発明が解決しようとする課題】
【００２７】
本発明は、マニピュレータと補助装置を同期させて同時に動作させる工業用ロボットにおいて、ワーク上の指定された経路に対してツールを高精度で移動させることを課題とする。
【課題を解決するための手段】
【００２８】
本発明は、先端にツールを備え、かつこのツールの位置及び姿勢を三次元空間内で変化させるマニピュレータと、前記ツール及び／又は前記マニピュレータを前記三次元空間内で移動させる補助装置と、前記マニピュレータと前記補助装置を同期して同時に動作させる制御装置とを備える工業用ロボットであって、前記制御装置は、前記マニピュレータ自体に設定されたマニピュレータ座標系で与えられるマニピュレータの位置及び姿勢と、前記補助装置自体に設定された補助装置座標系で与えられる補助装置の位置及び姿勢として入力される教示位置から、前記ワークに設定されたワーク座標系におけるツール先端の目標位置及び姿勢を計算する第１の演算手段と、第１の検出器で検出された前記マニピュレータ座標系における前記マニピュレータの位置及び姿勢と、第２の検出器で検出された前記補助装置座標系における前記補助装置の位置及び姿勢とである現在位置から、前記ワーク座標系における前記ツール先端の現在位置及び姿勢を計算する第２の演算手段と、前記ワーク座標系におけるツール先端の目標位置及び姿勢と、前記ワーク座標系におけるツール先端の現在位置及び姿勢と、予め入力された前記ツール先端の移動特性とに基づいて、前記現在位置から前記目標位置までの前記ワーク座標系における前記ツール先端の位置及び姿勢の時系列変化を規定するツール経路方程式を計算する第１の経路算出手段と、前記ツール経路方程式で規定される前記ワーク座標系における前記ツール先端の位置及び姿勢の時系列変化と同期するように、前記現在位置から前記教示位置までの前記補助装置座標系における前記補助装置の位置及び姿勢の時系列変化を規定する補助装置経路方程式を計算する第２の経路算出手段と、前記補助装置経路方程式により規定される前記補助装置の位置及び姿勢の時系列変化を、その加減速度が限界加減速度を下回るように平滑化した平滑化位置姿勢を算出する平滑化手段と、前記平滑化位置姿勢を使用して、前記ツール経路方程式により規定された前記ワーク座標系における前記ツール先端の位置及び姿勢を、前記マニピュレータの基部に設定され、かつ前記三次元空間に対して固定されたベース座標系におけるツール先端の位置及び姿勢に変換する座標変換処理手段と、前記座標変換処理手段により算出された前記ベース座標系におけるツール先端の位置及び姿勢を前記マニピュレータ座標系におけるマニピュレータの位置及び姿勢に変換する逆キネマティクス処理手段と、前記逆キネマティクス処理手段により算出された前記マニピュレータ座標系におけるマニピュレータの位置及び姿勢に基づいて、前記マニピュレータを駆動するマニピュレータ駆動手段と、前記平滑化手段により算出された平滑化位置姿勢に基づいて、前記補助装置を駆動する補助装置駆動手段とを備えることを特徴とする工業用ロボットを提供する。
【００２９】
平滑化手段は、加減速度が限界加減速度を下回るように、補助装置経路方程式により規定される補助装置の位置及び姿勢を平滑化する。この平滑化位置姿勢を使用して、座標変換処理手段がツール経路方程式により規定されるワーク座標系における前記ツール先端の位置及び姿勢をベース座標系に変換する。ベース座標系に変換されたツール先端の位置及び姿勢は、逆キネマティクス処理手段でマニピュレータ座標系に変換された後にマニピュレータの駆動に使用される。一方、補助装置は平滑化位置姿勢に基づいて駆動される。従って、本発明の工業用ロボットでは、加速性能の低い補助装置に対しては限界加減速度を超えるような位置及び姿勢の変化を要求する指令は出力せず、補助装置の加減速度の不足をマニピュレータの動作で補う。そのため、ツール先端は指定された経路を高精度で移動することができ、加工品質が大幅に向上する。また、加速性能の良好な高価なモータ等を補助装置の駆動装置として使用する必要がなく、振動抑制のために補助装置の機械強度や剛性を高める必要もないので大幅なコスト低減を図ることができる。
【００３０】
具体的には、前記平滑化手段は、前記ツールが前記ツール経路方程式で規定される経路に沿って前記現在位置から前記目標位置に移動するまでに要する時間である移動時間と、前記教示位置での前記補助装置座標系における前記補助装置の位置及び姿勢と、前記現在位置での前記補助装置座標系における前記補助装置の位置及び姿勢と、前記補助装置の限界加減速度とに基づいて、前記平滑化位置姿勢を計算する。
【００３１】
さらに具体的には、例えば以下の式に基づいて、平滑化位置姿勢を計算することができる。
【００３２】
【数１】
Ｊps_smooth：平滑化位置姿勢
Ｗ（ｉ）：特性関数
Ｊps（ｉ）：補助装置経路方程式により規定される補助装置の位置及び姿勢
ｉ：１からｍの整数であって、現在の制御周期から数えて第何番目の制御周期であるかを示す。
ｍ：平均化点数
【００３３】
前記平均化点数ｍは、例えば下記の式に基づいて決定される。
【００３４】
【数２】
ΔＪｐｓ：補助装置の目標速度（rad/sec）
Ａｐｓ：補助装置の限界加減速度（rad/sec2）
ＴＣ：制御周期（msec）
【００３５】
目標速度ΔＪｐｓは、移動時間、教示位置での補助装置座標系における補助装置の位置及び姿勢、及び現在位置での補助装置座標系における補助装置の位置及び姿勢により定まる。
【００３６】
特性関数Ｗ（ｉ）として、例えば下記の式で定義される単位ステップ関数状の関数を使用することができる。
【００３７】
【数３】
【００３８】
補助装置経路方程式で規定される経路が等速である場合、この特性関数を使用して平滑化することにより、補助装置の動作開始時の加速度と動作停止時の減速度が限界加減速度未満に低下し、台形型の速度パターンとなる。
【００３９】
代案としては、下記の式で定義される二等辺三角形状の関数を特性関数Ｗ（ｉ）として使用することができる。
【００４０】
【数４】
【００４１】
補助装置経路方程式で規定される経路が等速である場合、この特性関数を使用すると、補助装置の動作開始時の速度の立ち上がりは滑らかな曲線状となる。同様に、補助装置の動作停止時には滑らかな曲線状に速度が低下する。
【００４２】
補助装置は、例えば、ワークを保持し、かつワークを単一又は複数の軸周りに回転させるポジショナと、マニピュレータを保持し、かつマニピュレータを単一又は複数の軸線方向に直線移動させるスライダのいずれか一方又は両方である。
【発明の効果】
【００４３】
本発明の工業用ロボットによれば、ツール先端はワーク上の指定された経路を高精度で移動することができ、加工品質を大幅に向上することができる。また、加速性能の良好な高価なモータ等を補助装置の駆動装置として使用する必要がなく、振動抑制のために補助装置の機械強度や剛性を高める必要もないので、大幅なコスト低減を図ることができる。
【発明を実施するための最良の形態】
【００４４】
次に、添付図面を参照して本発明の実施形態を詳細に説明する。
【００４５】
（第１実施形態）
図１から図５に示す第１実施形態は、マニピュレータ１１とポジショナ１２を備えるＰＴＰ型の工業用ロボットに本発明を適用した例である。
【００４６】
図１を参照すると、工業用ロボットは、マニピュレータ１１、ポジショナ１２、制御装置１３、及び教示装置１４を備える。
【００４７】
マニピュレータ１１は先端にツール１６を備え、かつツール１６の位置及び姿勢を三次元空間内で変化させる。マニピュレータ１１は６個の回転関節ＲＪｍ１，ＲＪｍ２，ＲＪｍ３，ＲＪｍ４，ＲＪｍ５，ＲＪｍ６を有する。回転関節ＲＪｍ１〜ＲＪｍ６間はリンクで連結され、最も基端側の回転関節ＲＪｍ１は台座１１ａに取り付けられている。各回転関節ＲＪｍ１〜ＲＪｍ６は、回転駆動のためのモータＭＯｍ１〜ＭＯｍ６と、関節角度θｍ１〜θｍ２を検出するための角度センサＳＥｍ１〜ＳＥｍ６を備える。
【００４８】
ポジショナ１２はワーク１７を保持するテーブル１２ａと、ワーク１７の位置及び姿勢を三次元空間内で変化させるための２個の回転関節ＲＪｐ１，ＲＪｐ２を備える。回転関節ＲＪｐ１，ＲＪｐ２は、回転駆動のためのモータＭＯｐ１，ＭＯｐ２と、関節角度θｐ１，θｐ２を検出するための角度センサＳＥｐ１，ＳＥｐ２を備える。
【００４９】
次に、マニピュレータ１１及びポジショナ１２の制御に使用する座標を説明する。
【００５０】
マニピュレータ１１に関し、２つの座標系を設定する。まず、６個の関節角度θｍ１〜θｍ６で与えられる回転関節座標系（マニピュレータ座標系）を設定する。マニピュレータ座標系でのマニピュレータ１１の位置及び姿勢（ツール先端１６ａの位置及び姿勢）は、関節角度Ｊｍ（θｍ１，θｍ２，θｍ３，θｍ４，θｍ５，θｍ６）で表記する。また、原点がマニピュレータ１１の台座１１ａに設定され、かつ三次元空間に対して固定された直交座標系（ベース座標系Σbase）を設定する。ベース座標系Σbaseにおけるマニピュレータ１１の位置及び姿勢（ツール先端１６ａの位置及び姿勢）、すなわちベース座標系Σbaseでの座標と座標軸周りの回転角をbaseＰ（Ｘｂ，Ｙｂ，Ｚｂ，αｂ，βｂ，γｂ）で表記する。
【００５１】
ポジショナ１２に関しては、２個の関節角度θｐ１，θｐ１で与えられる回転関節座標系（ポジショナ座標系）を設定する。ポジショナ座標系でのポジショナ１２の位置及び姿勢は、関節角度Ｊｐ（θｐ１，θｐ２）で表記する。
【００５２】
ワーク１７に対して固定された直交座標系（ワーク座標系Σwork）を設定する。ワーク座標系Σworkにおける座標と座標軸周りの回転角をworkＰ（Ｘｗ，Ｙｗ，Ｚｗ，αｗ，βｗ，γｗ）で表記する。
【００５３】
制御装置１３は、ＣＰＵ、後述するＦＩＦＯメモリ１８を含む各種メモリ、及び入出力ポート等を有するコンピュータを備える。制御装置１３は、予め記憶されたプログラムに従って、教示装置１４及び角度センサＳＥｍ１〜ＳＥｍ６，ＳＥｐ１，ＳＥｐ２からの入力に基づいて、モータＭＯｍ１〜ＭＯｍ６，ＭＯｐ１，ＭＯｐ２を駆動する。制御装置１３からマニピュレータ１１及びポジショナ１２のモータＭＯｍ１〜ＭＯｍ６，ＭＯｐ１，ＭＯｐ２に対して出力される指令は、関節角度Ｊｍ（θｍ１〜θｍ６），Ｊｐ（θｐ１，θｐ２）である。
【００５４】
教示装置１４を使用して制御装置１３に対して、制御に必要な条件が入力される。この条件には、教示位置Ｐｔｅに加え、ワーク座標系Σbaseにおけるツール先端１６ａの移動経路と移動速度Ｖset、機械設計上定まるポジショナ１２のモータＭＯｐ１，ＭＯｐ２が制御装置１３からの指令を追従できる限界の加減速度（限界加減速度Ａｐ）等の各種パラメータが含まれる。限界加減速度Ａｐは予め制御装置１３が記憶していてもよい。教示位置Ｐｔｅは、マニピュレータ１１の関節角度Ｊｍｔｅ（θｍｔｅ１，θｍｔｅ２，θｍｔｅ３，θｍｔｅ４，θｍｔｅ５，θｍｔｅ６）とポジショナ１２の関節角度Ｊｐｔｅ（θｐｔｅ１，θｐｔｅ２）として入力される。
【００５５】
次に、図２のフローチャートを参照して、制御装置１３により実行されるマニピュレータ１１及びポジショナ１２の制御を説明する。なお、理解を容易にするために、図３に示すようにワーク１７上の直線の作業線Ｌに沿ってツール先端１６ａが現在位置から目標位置まで等速で移動する場合を例に説明する。
【００５６】
まず、ステップＳ１において、教示位置Ｐｔｅ（Ｊｍｔｅ＋Ｊｐｔｅ）からワーク座標系Σworkにおけるツール先端１６ａの目標位置及び姿勢workＰｔｅ（Ｘｗｔｅ，Ｙｗｔｅ，Ｚｗｔｅ，αｗｔｅ，βｗｔｅ，γｗｔｅ）を計算する。具体的には、まず、教示位置Ｐｔｅでのマニピュレータ１１の関節角度Ｊｍｔｅの順キネマティクスを計算し、ベース座標系Σbaseにおけるツール先端１６ａの位置及び姿勢baseＰｔｅ（Ｘｂｔｅ，Ｙｂｔｅ，Ｚｂｔｅ，αｂｔｅ，βｂｔｅ，γｂｔｅ）に変換する。次に、教示位置Ｐｔｅにおけるポジショナ１２の関節角度Ｊｐｔｅを使用し、座標変換手段によってベース座標系Σbaseにおける教示位置Ｐｔｅでのツール先端１６ａの位置及び姿勢baseＰｔｅを、ワーク座標系Σworkにおけるツール先端１６ａの目標位置及び姿勢workＰｔｅに変換する。順キネマティクスは公知の算出式で算出できる。また、ベース座標系Σbaseからワーク座標系Σworkへの座標変換手段も公知のものを使用できる。機能から見ると、ステップＳ１は本発明における第１の演算手段を構成している。
【００５７】
次に、ステップＳ２において、現在位置Ｐ０におけるマニピュレータ１１の関節角度Ｊｍ０（θｍ１０，θｍ２０，θｍ３０，θｍ４０，θｍ５０，θｍ６０）とポジショナ１２の関節角度Ｊｐ０（θｐ１０，θｐ２０）を角度センサＳＥｍ１〜ＳＥｍ６，ＳＥｐ１，ＳＥｐ２から取得する。また、ステップＳ１と同様の順キネマティクスと座標変換により、現在位置Ｐ０（Ｊｍ０＋Ｊｐ０）からワーク座標系Σworkにおけるツール先端１６ａの現在位置及び姿勢workＰ０（Ｘｗ０，Ｙｗ０，Ｚｗ０，αｗ０，βｗ０，γｗ０）を計算する。機能から見ると、ステップＳ２は本発明における第２の演算手段を構成している。
【００５８】
続いて、ステップＳ３において、ワーク座標系Σworkにおける現在位置及び姿勢workＰ０から目標位置及び姿勢workＰｔｅまでのツール先端１６ａの位置及び姿勢workＰの時系列変化を規定する方程式（ツール経路方程式workＰ（ｔ）、符号ｔは時刻を示す。）を算出する。機能から見ると、ステップＳ３は第１の経路算出手段を構成している。
【００５９】
本実施形態では前述のようにツール先端１６ａは作業線Ｌ上を直線かつ等速で移動するので、ツール経路方程式workＰ（ｔ）は以下のように計算される。まず、ワーク座標系Σworkにおける現在位置及び姿勢workＰ０から目標位置及び姿勢workＰｔｅの間の距離Ｄは以下の式（１）で表される。
【００６０】
【数５】
【００６１】
ワーク座標系Σworkにおけるツール先端１６ａの移動速度の設定値（設定速度）をＶsetとすると、ツール先端１６ａが作業線Ｌに沿って現在位置及び姿勢workＰ０から目標位置及び姿勢workＰｔｅまで移動するのに要する時間（移動時間Ｔｎ）は、以下の式（２）で表される。
【００６２】
【数６】
【００６３】
また、ワーク座標系Σworkにおけるツール先端１６ａの位置及び姿勢workＰの変化の速度ΔworkＰは以下の式（３）で表される。
【００６４】
【数７】
【００６５】
従って、ツール経路方程式workＰ（ｔ）は以下の式（４）に示す通りである。
【００６６】
【数８】
【００６７】
次に、ステップＳ４において、ツール経路方程式workＰ（ｔ）で規定されるワーク座標系Σworkにおけるツール先端１６ａの位置及び姿勢の時系列変化と同期するように、ポジショナ座標系における現在位置Ｐ０から教示位置Ｐｔｅまでのポジショナ１２の関節角度Ｊｐの時系列変化を規定する方程式（ポジショナ経路方程式Ｊｐ（ｔ））を求める。機能から見ると、ステップＳ４は本発明における第２の経路算出手段を構成する。ポジショナ１２の位置及び姿勢の変化の速度ΔＪｐは、現在位置Ｐ０でのポジショナ１２の関節角度Ｊｐ０、教示位置Ｐｔｅでのポジショナ１２の関節角度Ｊｐｔｅ、及び移動時間Ｔｎを用いて以下の式（５）で表される。
【００６８】
【数９】
【００６９】
従って、ポジショナ経路方程式Ｊｐ（ｔ）は以下の式（６）に示す通りである。
【００７０】
【数１０】
【００７１】
次に、ステップＳ５において、時刻ｔを初期化する（ｔ＝０）。
【００７２】
ステップＳ６からステップＳ１２は、制御周期Ｔｃ毎に繰り返される。まず、ステップＳ６において時刻ｔを時刻ｔ＋Ｔｃに更新する。
【００７３】
次に、ステップＳ７において、ステップＳ６で更新した時刻ｔの時点でのワーク座標系Σworkにおけるツール先端１６ａの位置及び姿勢workＰを、ツール経路方程式workＰ（ｔ）により計算する。本実施形態では、式（４）に更新した時刻ｔを代入することで、ツール先端１６ａの位置及び姿勢workＰが得られる。
【００７４】
ステップＳ８では、ステップＳ６で更新した時刻ｔの時点でのポジショナ１２の関節角度Ｊｐを、ポジショナ経路方程式Ｊｐ（ｔ）により計算する。本実施形態では、式（６）に更新した時刻ｔを代入することで、ポジショナ１２の関節角度Ｊｐが得られる。
【００７５】
ステップＳ７，Ｓ８で算出したツール先端１６ａの位置及び姿勢workＰとポジショナ１２の関節角度Ｊｐをそのままマニピュレータ１１及びポジショナ１２に対する駆動指令の算出に使用すると、ポジショナ１２に対して限界加減速度Ａｐを超える急激な加減速を指令することになる。そこで、ステップＳ９では、ステップＳ８でポジショナ経路方程式Ｊｐ（ｔ）により算出したポジショナ１２の関節角度Ｊｐを平滑化し、ポジショナ１２の関節角度Ｊｐの加減速度が限界加減速度Ａｐ未満となる平滑化位置姿勢Ｊｐ＿smoothを算出している。機能から見ると、ステップＳ９は本発明における平滑化手段を構成する。
【００７６】
ステップＳ９の平滑化処理を詳細に説明する。図４を参照すると、制御装置１３は、先入れ先出し型のメモリ（ＦＩＦＯメモリ１８）を備える。このＦＩＦＯメモリ１８にはｍ個のポジショナ１２の位置及び姿勢Ｊｐ（１），Ｊｐ（２），Ｊｐ（３），・・・Ｊｐ（ｍ−１），Ｊｐ（ｍ）が記憶される。具体的には、ＦＩＦＯメモリ１８には、現在の制御周期におけるポジショナ１２の位置及び姿勢Ｊｐ（１）から、現在の制御周期から数えて第ｍ−１番目前の制御周期におけるポジショナ１２の位置及び姿勢Ｊｐ（ｍ）が記憶されている。また、ステップＳ６で時刻ｔが更新されてステップＳ８で新たなポジショナ１２の関節角度Ｊｐが算出される度に、その新たな関節角度ＪｐがＦＩＦＯメモリ１８に記憶され、かつ最も古い関節角度ＪｐがＦＩＦＯメモリ１８から消去される。これらＦＩＦＯメモリ１８に記憶されたｍ個のポジショナ１２の関節角度Ｊｐ（１）〜Ｊｐ（ｍ）に対して、以下の式（７）で定義される積和計算を実行して平滑化位置姿勢Ｊｐ＿smoothを算出する。
【００７７】
【数１１】
【００７８】
式（７）において、Ｊp_smoothはポジショナ１２の平滑化位置姿勢、Ｗ（ｉ）は特性関数、ｉは１からｍの整数であって、現在の制御周期から数えて第何番目前の制御周期であるかを示す。また、ｍは平均化点数であり、ＦＩＦＯメモリ１８が記憶するポジショナ１２の位置及び姿勢Ｊｐの個数に対応する。
【００７９】
図４に概略的に示すように、本実施形態では特性関数Ｗ（ｉ）として単位ステップ関数状の関数を使用して平滑化処理を行っている。本実施形態での特性関数は以下の式（８）で定義される。
【００８０】
【数１２】
【００８１】
式（８）の特性関数Ｗ（ｉ）を使用すると、単純移動平均による平滑化処理となる。ステップＳ８で算出したポジショナ経路方程式Ｊｐ（ｔ）により規定されるポジショナ１２の位置及び姿勢Ｊｐの時系列変化をそのままポジショナ１２に出力すると、図４において二点点線で示すように、動作開始時刻ｔ１及び動作開始時刻ｔ２にステップ状の急激な加減速をポジショナ１２に強いることになる。これに対して式（７），（８）により算出したポジショナ１２の平滑化位置姿勢Ｊｐ＿smoothは、図４において実線で示すように、動作開始時及び動作停止時の加減速度が低減された台形型速度パターンを呈する。詳細には、動作開始時には平滑化後のポジショナ１２の速度ｄＪｐ＿smooth／ｄｔは０から式（５）の速度ΔＪｐまで直線的に増加し、加速時間はｍ＊Ｔｃである（図５ではｍ＝５としている）。同様に、動作停止時には、平滑化後のポジショナ１２の速度ｄＪｐ＿smooth／ｄｔは速度ΔＪｐから０まで直線的に減少し、減速時間はｍ＊Ｔｃである。
【００８２】
加減速に要する時間はｍ＊Ｔｃで、制御周期ＴＣは定数（例えば２６０msec）である。従って、平滑化位置姿勢Ｊｐ＿smoothの加減速度が最適、すなわち
ポジショナ１２の限界加減速度Ａｐを超えない範囲で、加減速に要する時間が最短となるようにするには、平均化点数ｍを以下のように設定すればよい。なお、限界加減速度Ａｐの単位はrad／sec２、ポジショナ１２の位置及び姿勢の変化の速度ΔＪｐの単位はrad／sec、制御周期Ｔｃの単位はmsecとする。まず、ポジショナ１２の位置及び姿勢Ｊｐの最適加減速時間Ｔａｃｃは以下の式（９）で表される。
【００８３】
【数１３】
【００８４】
ポジショナ１２の関節角度Ｊｐ（ｉ）は、制御周期Ｔｃ毎に離散的に与えられるので、最適加減速時間Ｔａｃｃに対応する平均化点数ｍは以下の式（１０）で示す通りとなる。
【００８５】
【数１４】
【００８６】
また、式（１０）に式（９）を代入すると、最適な平均化点数ｍは以下の式（１１）で表される。
【００８７】
【数１５】
【００８８】
式（７），（１１）から明らかなように、ステップＳ９では、移動時間Ｔｎ、教示位置Ｐｔｅでのポジショナ１２の関節角度Ｊｐｔｅ、現在位置Ｐ０でのポジショナ１２の関節角度Ｊｐ０、並びに限界加減速度Ａｐに基づいて平滑化位置姿勢Ｊｐ＿smoothを算出している。式（１０），（１１）により決定した平均化点数ｍで、式（７）の積和計算を実行することにより、限界加減速度Ａｐを超えない範囲で加減速に要する時間が最短となる平滑化位置姿勢Ｊｐ＿smoothを求めることができる。
【００８９】
ステップＳ１０及びＳ１１は、マニピュレータ１１に対する指令を計算するための処理である。なお、ポジショナ１２に対する指令は平滑化位置姿勢Ｊｐ＿smoothが使用される。まず、ステップＳ１０において、ステップＳ７で計算したワーク座標系Σworkにおけるツール先端１６ａの位置及び姿勢workＰを、ベース座標系Σbaseにおけるツール先端１６ａの位置及び姿勢baseＰに変換する。この変換には、ステップＳ８で算出したポジショナ１２の位置及び姿勢Ｊｐではなく、ステップＳ９で算出した平滑化位置姿勢Ｊｐ＿smoothを使用し、前述の座標変換手段により行う。次に、ステップＳ１１において、ステップＳ１０で算出したベース座標系Σbaseにおけるツール先端１６ａの位置及び姿勢baseＰの逆キネマティクスを計算し、マニピュレータ座標系、すなわち関節角度Ｊｍに変換する。機能から見ると、ステップＳ１０は本発明における座標変換処理手段を構成し、ステップＳ１１は逆キネマティクス処理手段を構成する。
【００９０】
ステップＳ１２において、ステップＳ１１で算出した関節角度Ｊｍを指令としてマニピュレータ１１に出力する。また、ステップＳ９で算出した平滑化位置姿勢Ｊｐ＿smoothを指令としてポジショナ１２に出力する。これらの指令によりマニピュレータ１１のモータＭＯｍ１〜ＭＯｍ６とポジショナ１２のモータＭＯｐ１，ＭＯｐ２が作動し、マニピュレータ１１とポジショナ１２が同期して同時に動作する。機能から見ると、ステップＳ１２は本発明におけるマニピュレータ駆動手段及び補助装置駆動手段を構成する。
【００９１】
ステップＳ１３において教示位置Ｐｔｅに到達するまで、ステップＳ６からステップＳ１２の処理が繰り返される。
【００９２】
ステップＳ８でポジショナ経路方程式Ｊｐ（ｔ）から算出したポジショナ１２の位置及び姿勢Ｊｐをそのままポジショナ１２に指令として出力すると、図５において二点鎖線で示すように、ポジショナ１２に対して動作開始時及び動作停止時（時刻ｔ１，ｔ２）に急激な加減速を指令することになる。しかし、本実施形態では、ステップＳ９で算出した平滑化位置姿勢Ｊｐ＿smoothがポジショナ１２に対して指令として出力されるので、実線で示すような加減速が緩やかな台形型の速度パターンとなり、加速性能の低いポジショナ１２のモータＭＯｐ１，ＭＯｐ２に対して限界加減速度Ａｐを上回るような位置及び姿勢の変化を要求する指令は出力されない。従って、ポジショナ１２に応答遅れが生じない。
【００９３】
単に平滑化位置姿勢Ｊｐ＿smoothをポジショナ１２に対する指令として使用するだけでは、ワーク座標系Σworkにおけるツール先端１６ａの加減速度が不足し、ツール先端１６ａを正確に作業線Ｌ上で移動させることもできない。しかし、本実施形態では、平滑化位置姿勢Ｊｐ＿smoothを使用してワーク座標系Σworkからベース座標系Σbaseへの座標変換を行い（ステップＳ１０）、その逆キネマティクスである関節角度Ｊｍをマニピュレータ１１のモータＭＯｍ１〜ＭＯｍ６に指令として出力する（ステップＳ１１，Ｓ１２）。そのため、マニピュレータ１１は、ポジショナ１２の加減速度の不足を補い、ツール先端１６ａが作業線Ｌ上を設定速度Ｖsetで移動するように動作する。例えば、ツール先端１６ａは図５の時刻ｔ２に作業線Ｌの終点に到達するが、この時刻ｔ２からポジショナ１２の動作が完全に停止する時刻ｔ２’の間、ツール先端１６ａが作業線Ｌの終点に位置したままで保持されるように、ポジショナ１２の関節角度Ｊｐの変化に応じてマニピュレータ１１が作動する。
【００９４】
以上のように、加速性能の低いポジショナ１２に対しては急激な加減速の指令を出力せず、ポジショナ１２の加減速度の不足を加速性能の高いマニピュレータ１１の動作で補うので、ツール先端１６ａは良好な位置及び速度の精度を維持しつつワーク１７上の作業線Ｌを移動することができ、加工品質が大幅に向上する。また、加速性能の良好な高価なモータをポジショナ１２のモータＭＯｐ１，ＭＯｐ２として使用する必要がなく、振動抑制のためにポジショナ１２の機械強度や剛性を高める必要もないので大幅なコスト低減を図ることができる。
【００９５】
（第２実施形態）
図６から図９に示す第２実施形態は、マニピュレータ１１とスライダ２２を備えるＰＴＰ型の工業用ロボットに本発明を適用した例である。
【００９６】
スライダ２２は、互いに直交する方向に直進移動する３個の直動関節ＳＪ１，ＳＪ２，ＳＪ３を備え、直動関節ＳＪｐ１にマニピュレータ１１の台座１１ａが保持されている。スライダ２２に関し、座標軸がそれぞれ直動関節ＳＪｐ１〜ＳＪｐ３の移動方向に対応し、かつ三次元空間に固定された直交座標系（スライダ座標系）を設定する。スライダ座標系でのスライダ２２の位置及び姿勢は、関節位置Ｊｓ（Ｓｘ，Ｓｙ，Ｓｚ）で表記する。スライダ座標系の各軸の向きはベース座標系Σbaseの各軸の向きと一致している。各直動関節ＳＪｐ１〜ＳＪｐ３は、それぞれ駆動用のモータＭＯｓ１〜ＭＯＳ３と、関節位置Ｊｓを検出するための位置センサＳＥｓ１〜ＳＥｓ３を備える。
【００９７】
ワーク１７は三次元空間に固定されている。ワーク座標系Σworkの各軸の向きは、ベース座標系Σbase及びスライダ座標系の各軸の向きと一致している。
【００９８】
教示装置１４から制御装置１３には、教示位置Ｐｔｅに加え、ワーク座標系Σbaseにおけるツール先端１６ａの移動経路と移動速度Ｖset、スライダ２２のモータＭＯｓ１〜ＭＯｓ３の限界加減速度Ａｐ等を含む制御に必要な条件が入力される。教示位置Ｐｔｅは、マニピュレータ１１の関節角度Ｊｍｔｅ（θｍｔｅ１，θｍｔｅ２，θｍｔｅ３，θｍｔｅ４，θｍｔｅ５，θｍｔｅ６）とスライダ２２の関節位置Ｊｓｔｅ（Ｓｘｔｅ，Ｓｙｔｅ，Ｓｚｔｅ）として入力される。
【００９９】
図７に示す制御は、ポジショナ１２がスライダ２２に置き換わる点を除いて第１実施形態における制御（図２参照）と同様である。理解を容易にするために、図８に示すようにワーク１７上の作業線Ｌはベース座標系Σbase、ワーク座標系Σwork、及びスライダ座標系のＹ方向に延びる直線であり、この作業線Ｌに沿ってツール先端１６ａが設定速度Ｖsetで等速移動し、かつ現在位置Ｐ０と教示位置Ｐｔｅでのマニピュレータ座標系におけるマニピュレータ１１の位置及び姿勢が同一（Ｊｍ０＝Ｊｍｔｅ）である場合を例に、本実施形態における制御を説明する。
【０１００】
ステップＳ１’において教示位置Ｐｔｅ（Ｊｍｔｅ＋Ｊｓｔｅ）からワーク座標系Σworkにおけるツール先端１６ａ目標位置及び姿勢workＰｔｅを計算し、ステップＳ２’において現在位置Ｐ０からワーク座標系Σworkにおけるツール先端１６ａの現在位置及び姿勢workＰ０を計算する。現在位置Ｐ０は、角度センサＳＥｍ１〜ＳＥｍ６から取得したマニピュレータ１１の現在の関節角度Ｊｍ０と、位置センサＳＥｓ１〜ＳＥｓ３で取得したスライダ２２の現在の関節位置Ｊｓ０（Ｓｘ０，Ｓｙ０，Ｓｚ０）として与えられる。
【０１０１】
ステップＳ３’で求めるツール経路方程式wokrＰ（ｔ）は、第１実施形態と同一であり前述の式（４）で示す通りとなる。
【０１０２】
ステップＳ４’において、ワーク座標系Σworkにおけるツール先端１６ａの位置及び姿勢の時系列変化と同期するように、現在位置Ｐ０から教示位置Ｐｔｅまでのスライダ座標系におけるスライダ２２の位置及び姿勢の時系列変化を規定する方程式（スライダ経路方程式Ｊｓ（ｔ））を求める。スライダ２２の位置及び姿勢の変化の速度ΔＪｓは、現在位置Ｐ０でのスライダ２２の関節位置Ｊｓ０と教示位置Ｐｔｅでのポジショナ１２の関節位置Ｊｓｔｅ、及び移動時間Ｔｎを用いて以下の式（１２）で表される。
【０１０３】
【数１６】
【０１０４】
従って、スライダ経路方程式Ｊｓ（ｔ）は以下の式（１３）に示す通りである。
【０１０５】
【数１７】
【０１０６】
時刻ｔを初期化後（ステップＳ５’）、制御周期Ｔｃ毎に時刻ｔを更新しつつ、ツール経路方程式wokrＰ（ｔ）によりワーク座標系Σworkにおけるツール先端１６ａの位置及び姿勢workＰを計算し、スライダ経路方程式Ｊｓ（ｔ）によりスライダ２２の位置及び姿勢Ｊｐを計算する（ステップＳ７’，Ｓ８’）。また、ステップＳ９’において、加減速度が限界加減速度Ａｐを超えないように、スライダ２２の関節位置ＪＳを平滑化処理する。具体的には、式（９）〜（１１）で決まる最適な平均化点数ｍと、式（８）で定義される特性関数Ｗ（ｉ）を使用し、式（７）の積和演算を実行して平滑化位置姿勢Ｊｓ＿smoothを算出する。
【０１０７】
平滑化位置姿勢Ｊｓ＿smoothをスライダ２２に出力し、平滑化位置姿勢Ｊｓ＿smoothを用いてワーク座標系Σworkからベース座標系Σbaseに座標変換したツール先端の逆キネマティクス（関節角度Ｊｍ）をマニピュレータ１１に出力する（ステップＳ１０’〜Ｓ１２’）。
【０１０８】
図９（Ａ）はスライダ２２のＹ方向の移動速度の変化を示し、図９（Ｂ）はベース座標系Σbaseでのツール先端１６ａのＹ方向の移動速度の変化を示し、図９（Ｃ）はワーク座標系Σworkでのツール先端１６ａのＹ方向の移動速度（ワーク１７に対するツール先端１６ａの移動速度）を示す。
【０１０９】
図７のステップＳ８’でスライダ経路方程式Ｊｓ（ｔ）により算出したスライダ２２の関節位置Ｊｓをそのままスライダ２２に指令として出力すると、図９（Ａ）の二点鎖線で示すように、スライダ２２に対して動作開始時及び動作停止時（時刻ｔ１，ｔ２）に急激な加減速を指令することになる。しかし、本実施形態では、ステップＳ９’で算出した平滑化位置姿勢Ｊｓ＿smoothがスライダ２２に対して指令として出力されるので、実線で示すような加減速が緩やかな台形型の速度パターンとなり、加速性能の低いスライダ２２のモータＭＯｓ１〜ＭＯｓ３に対して限界加減速度Ａｐを上回るような位置及び姿勢の変化を要求する指令は出力されない。
【０１１０】
単に平滑化位置姿勢Ｊｓ＿smoothをスライダ２２に対する指令として使用するだけでは、ワーク座標系Σworkにおけるツール先端１６ａの加減速度が不足する。具体的には、動作開始（時刻ｔ１）からツール先端１６ａが設定速度Ｖsetまで加速するのに要する時間が長くなる。また、動作停止時に設定速度Ｖsetから速度０まで減速する時間が長くなり所望の時刻ｔ２までにツール先端１６ａが作業線Ｌの終端まで到達できない。
【０１１１】
しかし、本実施形態では、スライダ経路方程式Ｊｓ（ｔ）により算出したスライダ２２の位置及び姿勢Ｊｓではなく、平滑化位置姿勢Ｊｓ＿smoothを使用して、マニピュレータ１１に対する指令（関節角度Ｊｍ）を算出しているので（ステップＳ１０’）、スライダ２２の加減速度を制限したことによるツール先端１６ａの加減速度の不足を補うようにマニピュレータ１１が作動する。具体的には、動作開始時には時刻ｔ１’にスライダ２２が設定速度Ｖsetに達するまで、ツール先端１６ａがＹ方向に移動するようにマニピュレータ１１が動作する。その結果、スライダ２２の移動とマニピュレータ１１によるツール先端１６ａの移動の重ね合わせによって、加速度の不足が補われ、図９（Ｃ）に示すように１回の制御周期Ｔｃでワーク１７に対するツール先端１６ａの移動速度が設定速度Ｖsetまで上昇する。また、動作停止時には時刻ｔ２’にスライダ２２が完全に停止するまで、ツール先端１６ａがＹ軸方向に移動するようにマニピュレータ１１が動作する。その結果、スライダ２２の移動とマニピュレータ１１によるツール先端１６ａの移動の重ね合わせによって、ツール先端１６ａが作業線Ｌの終端に保持される。
【０１１２】
以上のように、加速性能の低いスライダ２２に対しては急激な加減速の指令を出力せず、スライダ２２の加減速度の不足を加速性能の高いマニピュレータ１１の動作で補うので、ツール先端１６ａは良好な位置及び速度の精度を維持しつつワーク１７上の作業線Ｌを移動することができ、加工品質が大幅に向上する。また、加速性能の良好な高価なモータをスライダ２２のモータＭＯｓ１〜ＭＯｓ３として使用する必要がなく、振動抑制のためにスライダ２２の機械強度や剛性を高める必要もないので大幅なコスト低減を図ることができる。
【０１１３】
第２実施形態のその他の構成及び作用は第１実施形態と同様であるので、同一の要素には同一の符号を付して説明を省略する。
【０１１４】
（第３実施形態）
図１０に示すように、マニピュレータ１１とポジショナ１２及びスライダ２２を備えるＰＴＰ型の工業用ロボットにも本発明を適用することができる。
【０１１５】
制御装置１３は、図１１のステップＳ１''〜Ｓ１３''に示す第１及び第２実施形態と同様の制御を実行する。教示位置Ｐｔｅと現在位置Ｐ０は、マニピュレータ１１の関節角度Ｊｍｔｅ，Ｊｍ０、ポジショナ１２の関節角度Ｊｐｔｅ，Ｊｐ０、及びスライダ２２の関節位置Ｊｓｔｅ，Ｊｓ０で与えられる（ステップＳ１''，Ｓ２''）。ポジショナ経路方程式Ｊｐ（ｔ）とスライダ経路方程式Ｊｓ（ｔ）の両方を求め（ステップＳ４''）、これらから計算した関節角度Ｊｐと関節位置Ｊｓをそれぞれ平滑化処理して平滑化位置姿勢Ｊｐ＿smooth，Ｊｓ＿smoothを計算する（ステップＳ８''，Ｓ９''）。また、両方の平滑化位置姿勢Ｊｐ＿smooth，Ｊｓ＿smoothを使用してツール先端１６ａの座標をワーク座標系Σworkからベース座標系Σbaseに変換し、逆キネマティクスを計算してマニピュレータ１１の関節角度Ｊｍを得る（ステップＳ１０''，Ｓ１１''）。
【０１１６】
スライダ２２とポジショナ１２に対して平滑化位置姿勢Ｊｐ＿smooth，Ｊｓ＿smoothを指令として出力し、マニピュレータ１１にはこれらから算出した関節角度Ｊｍを指令として出力する（ステップＳ１２''）。従って、加速性能の低いポジショナ１２とスライダ２２に対しては急激な加減速の指令は出力せず、ポジショナ１２及びスライダ２２の加減速度の不足を加速性能の高いマニピュレータ１１の動作で補うので、ツール先端１６ａは良好な位置及び速度の精度を維持しつつワーク１７上の作業線Ｌを移動することができる。
【０１１７】
第３実施形態のその他の構成及び作用は第１実施形態と同様である。同一の要素には同一の符号を付して説明を省略する。
【０１１８】
本発明は、前記実施形態に限定されず、種々の変形が可能である。例えば、図１２に示すピーク値が１の二等辺三角形状の特性を有する特性関数Ｗ（ｉ）を使用して式（７）の積和計算を実行することで、ポジショナ１２やスライダ２２の平滑化位置姿勢Ｊｐ＿smooth，Ｊｓ＿smoothを算出してもよい。この特性関数Ｗ（ｉ）は、以下の式（１４）で定義される。
【０１１９】
【数１８】
【０１２０】
この特性関数Ｗ（ｉ）を使用した場合、図１３において実線で示すようにポジショナ１２やスライダ２２の速度パターンは、動作開始時（時刻ｔ１）の速度の立ち上がりは滑らかな曲線状となる。同様に、ポジショナ１２やスライダ２２の動作停止時（時刻ｔ２）には滑らかな曲線状に速度が低下する。
【０１２１】
一般に特性関数は対象であればどのような形であってもよい。例えば、図１２の特性関数の形を少し変更した図１４に示す特性関数をであってもよい。この場合、図１５において実線で示すようにポジショナ１２やスライダ２２の速度パターンは、動作開始時（時刻ｔ１）の速度の立ち上がり及び動作停止時（時刻ｔ２）の速度低下がより一層滑らかな曲線状となる。
【図面の簡単な説明】
【０１２２】
【図１】本発明の第１実施形態に係る工業用ロボットを示す概略構成図。
【図２】本発明の第１実施形態に係る工業用ロボットの動作を説明するためのフローチャート。
【図３】マニピュレータとポジショナの同期動作を説明するための概略図。
【図４】平滑化処理を説明するための概略図。
【図５】第１実施形態におけるポジショナの速度変化を示す線図。
【図６】本発明の第２実施形態に係る工業用ロボットを示す概略構成図。
【図７】本発明の第２実施形態に係る工業用ロボットの動作を説明するためのフローチャート。
【図８】マニピュレータとスライダの同期動作を説明するための概略図。
【図９】（Ａ）はスライダの速度変化を示す線図、（Ｂ）はマニピュレータの速度変化を示す線図、（Ｃ）はスライダとマニピュレータの合成速度を示す線図。
【図１０】本発明の第３実施形態に係る工業用ロボットを示す概略構成図。
【図１１】本発明の第３実施形態に係る工業用ロボットの動作を説明するためのフローチャート。
【図１２】平滑処理の他の例を説明するための概略図。
【図１３】図１２の平滑処理を行った場合のポジショナ及びスライダの速度変化の一例を示す線図。
【図１４】平滑処理のさらに他の例を説明するための概略図。
【図１５】図１４の平滑処理を行った場合のポジショナ及びスライダの速度変化の一例を示す線図。
【図１６】従来の工業用ロボットの一例を示す概略斜視図。
【図１７】（Ａ）はマニピュレータの変位を示す線図、（Ｂ）はマニピュレータの速度変化を示す線図。
【図１８】従来の工業用ロボットの他の一例を示す概略平面図。
【図１９】（Ａ）はスライダの速度変化を示す線図、（Ｂ）はマニピュレータの速度変化を示す線図。
【符号の説明】
【０１２３】
１１ マニピュレータ
１１ａ 台座
１２ ポジショナ
１２ａ テーブル
１３ 制御装置
１４ 教示装置
１６ ツール
１６ａ ツール先端
１７ ワーク
１８ ＦＩＦＯメモリ
２２ スライダ
【特許請求の範囲】
【請求項１】
先端にツールを備え、かつこのツールの位置及び姿勢を三次元空間内で変化させるマニピュレータと、前記ツール及び／又は前記マニピュレータを前記三次元空間内で移動させる補助装置と、前記マニピュレータと前記補助装置を同期して同時に動作させる制御装置とを備える工業用ロボットであって、
前記制御装置は、
前記マニピュレータ自体に設定されたマニピュレータ座標系で与えられるマニピュレータの位置及び姿勢と、前記補助装置自体に設定された補助装置座標系で与えられる補助装置の位置及び姿勢として入力される教示位置から、前記ワークに設定されたワーク座標系におけるツール先端の目標位置及び姿勢を計算する第１の演算手段と、
第１の検出器で検出された前記マニピュレータ座標系における前記マニピュレータの位置及び姿勢と、第２の検出器で検出された前記補助装置座標系における前記補助装置の位置及び姿勢とである現在位置から、前記ワーク座標系における前記ツール先端の現在位置及び姿勢を計算する第２の演算手段と、
前記ワーク座標系におけるツール先端の目標位置及び姿勢と、前記ワーク座標系におけるツール先端の現在位置及び姿勢と、予め入力された前記ツール先端の移動特性とに基づいて、前記現在位置から前記目標位置までの前記ワーク座標系における前記ツール先端の位置及び姿勢の時系列変化を規定するツール経路方程式を計算する第１の経路算出手段と、
前記ツール経路方程式で規定される前記ワーク座標系における前記ツール先端の位置及び姿勢の時系列変化と同期するように、前記現在位置から前記教示位置までの前記補助装置座標系における前記補助装置の位置及び姿勢の時系列変化を規定する補助装置経路方程式を計算する第２の経路算出手段と、
前記補助装置経路方程式により規定される前記補助装置の位置及び姿勢の時系列変化を、その加減速度が限界加減速度を下回るように平滑化した平滑化位置姿勢を算出する平滑化手段と、
前記平滑化位置姿勢を使用して、前記ツール経路方程式により規定された前記ワーク座標系における前記ツール先端の位置及び姿勢を、前記マニピュレータの基部に設定され、かつ前記三次元空間に対して固定されたベース座標系におけるツール先端の位置及び姿勢に変換する座標変換処理手段と、
前記座標変換処理手段により算出された前記ベース座標系におけるツール先端の位置及び姿勢を前記マニピュレータ座標系におけるマニピュレータの位置及び姿勢に変換する逆キネマティクス処理手段と、
前記逆キネマティクス処理手段により算出された前記マニピュレータ座標系におけるマニピュレータの位置及び姿勢に基づいて、前記マニピュレータを駆動するマニピュレータ駆動手段と、
前記平滑化手段により算出された平滑化位置姿勢に基づいて、前記補助装置を駆動する補助装置駆動手段と
を備えることを特徴とする工業用ロボット。
【請求項２】
前記平滑化手段は、
前記ツールが前記ツール経路方程式で規定される経路に沿って前記現在位置から前記目標位置に移動するまでに要する時間である移動時間と、
前記教示位置での前記補助装置座標系における前記補助装置の位置及び姿勢と、
前記現在位置での前記補助装置座標系における前記補助装置の位置及び姿勢と、
前記補助装置の限界加減速度と
に基づいて、前記平滑化位置姿勢を計算することを特徴とする請求項１に記載の工業用ロボット。
【請求項３】
前記平滑化手段は、下記の式に基づいて平滑化位置姿勢を計算することを特徴とする請求項２に記載の工業用ロボット。
【数１】
Ｊps_smooth：平滑化位置姿勢
Ｗ（ｉ）：特性関数
Ｊps（ｉ）：補助装置経路方程式により規定される補助装置の位置及び姿勢
ｉ：１からｍの整数であって、現在の制御周期から数えて第何番目の制御周期であるかを示す。
ｍ：平均化点数
【請求項４】
前記平均化点数ｍは、以下の式に基づいて決定されることを特徴とする請求項３に記載の工業用ロボット。
【数２】
ΔＪｐｓ：補助装置の目標速度（rad/sec）
Ａｐｓ：補助装置の限界加減速度（rad/sec2）
ＴＣ：制御周期（msec）
【請求項５】
前記特性関数Ｗ（ｉ）は、以下の式で定義されることを特徴とする請求項３又は請求項４に記載の工業用ロボット。
【数３】
【請求項６】
前記特性関数Ｗ（ｉ）は、以下の式で定義されることを特徴とする、請求項３又は請求項４に記載の工業用ロボット。
【数４】
【請求項７】
前記補助装置として、前記ワークを保持し、かつ前記ワークを単一又は複数の軸周りに回転させるポジショナを備えることを特徴とする請求項１から請求項６のいずれか１項に記載の工業用ロボット。
【請求項８】
前記補助装置として、前記マニピュレータを保持し、かつ前記マニピュレータを単一又は複数の軸線方向に直進移動させるスライダを備えることを特徴とする請求項１から請求項６のいずれか１項に記載の工業用ロボット。
【請求項９】
前記補助装置は、前記ワークを保持し、かつ前記ワークを単一又は複数の軸周りに回転させるポジショナと、前記マニピュレータを保持し、かつ前記マニピュレータを単一又は複数の軸線方向に直進移動させるスライダとを備えることを特徴とする請求項１から請求項６のいずれか１項に記載の工業用ロボット。
【請求項１】
先端にツールを備え、かつこのツールの位置及び姿勢を三次元空間内で変化させるマニピュレータと、前記ツール及び／又は前記マニピュレータを前記三次元空間内で移動させる補助装置と、前記マニピュレータと前記補助装置を同期して同時に動作させる制御装置とを備える工業用ロボットであって、
前記制御装置は、
前記マニピュレータ自体に設定されたマニピュレータ座標系で与えられるマニピュレータの位置及び姿勢と、前記補助装置自体に設定された補助装置座標系で与えられる補助装置の位置及び姿勢として入力される教示位置から、前記ワークに設定されたワーク座標系におけるツール先端の目標位置及び姿勢を計算する第１の演算手段と、
第１の検出器で検出された前記マニピュレータ座標系における前記マニピュレータの位置及び姿勢と、第２の検出器で検出された前記補助装置座標系における前記補助装置の位置及び姿勢とである現在位置から、前記ワーク座標系における前記ツール先端の現在位置及び姿勢を計算する第２の演算手段と、
前記ワーク座標系におけるツール先端の目標位置及び姿勢と、前記ワーク座標系におけるツール先端の現在位置及び姿勢と、予め入力された前記ツール先端の移動特性とに基づいて、前記現在位置から前記目標位置までの前記ワーク座標系における前記ツール先端の位置及び姿勢の時系列変化を規定するツール経路方程式を計算する第１の経路算出手段と、
前記ツール経路方程式で規定される前記ワーク座標系における前記ツール先端の位置及び姿勢の時系列変化と同期するように、前記現在位置から前記教示位置までの前記補助装置座標系における前記補助装置の位置及び姿勢の時系列変化を規定する補助装置経路方程式を計算する第２の経路算出手段と、
前記補助装置経路方程式により規定される前記補助装置の位置及び姿勢の時系列変化を、その加減速度が限界加減速度を下回るように平滑化した平滑化位置姿勢を算出する平滑化手段と、
前記平滑化位置姿勢を使用して、前記ツール経路方程式により規定された前記ワーク座標系における前記ツール先端の位置及び姿勢を、前記マニピュレータの基部に設定され、かつ前記三次元空間に対して固定されたベース座標系におけるツール先端の位置及び姿勢に変換する座標変換処理手段と、
前記座標変換処理手段により算出された前記ベース座標系におけるツール先端の位置及び姿勢を前記マニピュレータ座標系におけるマニピュレータの位置及び姿勢に変換する逆キネマティクス処理手段と、
前記逆キネマティクス処理手段により算出された前記マニピュレータ座標系におけるマニピュレータの位置及び姿勢に基づいて、前記マニピュレータを駆動するマニピュレータ駆動手段と、
前記平滑化手段により算出された平滑化位置姿勢に基づいて、前記補助装置を駆動する補助装置駆動手段と
を備えることを特徴とする工業用ロボット。
【請求項２】
前記平滑化手段は、
前記ツールが前記ツール経路方程式で規定される経路に沿って前記現在位置から前記目標位置に移動するまでに要する時間である移動時間と、
前記教示位置での前記補助装置座標系における前記補助装置の位置及び姿勢と、
前記現在位置での前記補助装置座標系における前記補助装置の位置及び姿勢と、
前記補助装置の限界加減速度と
に基づいて、前記平滑化位置姿勢を計算することを特徴とする請求項１に記載の工業用ロボット。
【請求項３】
前記平滑化手段は、下記の式に基づいて平滑化位置姿勢を計算することを特徴とする請求項２に記載の工業用ロボット。
【数１】
Ｊps_smooth：平滑化位置姿勢
Ｗ（ｉ）：特性関数
Ｊps（ｉ）：補助装置経路方程式により規定される補助装置の位置及び姿勢
ｉ：１からｍの整数であって、現在の制御周期から数えて第何番目の制御周期であるかを示す。
ｍ：平均化点数
【請求項４】
前記平均化点数ｍは、以下の式に基づいて決定されることを特徴とする請求項３に記載の工業用ロボット。
【数２】
ΔＪｐｓ：補助装置の目標速度（rad/sec）
Ａｐｓ：補助装置の限界加減速度（rad/sec2）
ＴＣ：制御周期（msec）
【請求項５】
前記特性関数Ｗ（ｉ）は、以下の式で定義されることを特徴とする請求項３又は請求項４に記載の工業用ロボット。
【数３】
【請求項６】
前記特性関数Ｗ（ｉ）は、以下の式で定義されることを特徴とする、請求項３又は請求項４に記載の工業用ロボット。
【数４】
【請求項７】
前記補助装置として、前記ワークを保持し、かつ前記ワークを単一又は複数の軸周りに回転させるポジショナを備えることを特徴とする請求項１から請求項６のいずれか１項に記載の工業用ロボット。
【請求項８】
前記補助装置として、前記マニピュレータを保持し、かつ前記マニピュレータを単一又は複数の軸線方向に直進移動させるスライダを備えることを特徴とする請求項１から請求項６のいずれか１項に記載の工業用ロボット。
【請求項９】
前記補助装置は、前記ワークを保持し、かつ前記ワークを単一又は複数の軸周りに回転させるポジショナと、前記マニピュレータを保持し、かつ前記マニピュレータを単一又は複数の軸線方向に直進移動させるスライダとを備えることを特徴とする請求項１から請求項６のいずれか１項に記載の工業用ロボット。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【公開番号】特開２００６−４３８３９（Ｐ２００６−４３８３９Ａ）
【公開日】平成１８年２月１６日（２００６．２．１６）
【国際特許分類】
【出願番号】特願２００４−２３０７０４（Ｐ２００４−２３０７０４）
【出願日】平成１６年８月６日（２００４．８．６）
【出願人】（０００００１１９９）株式会社神戸製鋼所 (5,860)
【Ｆターム（参考）】
【公開日】平成１８年２月１６日（２００６．２．１６）
【国際特許分類】
【出願日】平成１６年８月６日（２００４．８．６）
【出願人】（０００００１１９９）株式会社神戸製鋼所 (5,860)
【Ｆターム（参考）】
[ Back to top ]