ロボットの制御装置
【課題】円弧状部分から直線部分に入る位置を正しくティーチングできず、直線部分に入った位置をティーチングしてしまっても、ツール先端に円弧状部分により近い形状の軌跡を辿らせる。
【解決手段】４つのティーチング位置Ｐ１〜Ｐ４のうち、Ｐ１からＰ２の間、Ｐ３からＰ４の間は直線補完する。Ｐ２が作業対象部分の直線部分から円弧状部分に入った位置に正しくティーチングし、Ｐ３は円弧状部分から直線部分に入った位置をティーチングした場合、Ｐ２とＰ３との間は楕円で曲線補間する。この場合、Ｐ２には楕円の長径部分が接するので円に近く、Ｐ３には短径部分が接するので直線に近くなり、結果として作業対象部分の形状に近くなる。
【解決手段】４つのティーチング位置Ｐ１〜Ｐ４のうち、Ｐ１からＰ２の間、Ｐ３からＰ４の間は直線補完する。Ｐ２が作業対象部分の直線部分から円弧状部分に入った位置に正しくティーチングし、Ｐ３は円弧状部分から直線部分に入った位置をティーチングした場合、Ｐ２とＰ３との間は楕円で曲線補間する。この場合、Ｐ２には楕円の長径部分が接するので円に近く、Ｐ３には短径部分が接するので直線に近くなり、結果として作業対象部分の形状に近くなる。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ティーチングされた複数位置の相互間を直線および曲線で補間するロボットの制御装置に関する。
【背景技術】
【０００２】
プレイバック型のロボットでは、ティーチング装置によりロボットアームを実際に動かしてロボットアーム先端に取り付けられたエンドエフェクタの位置を順次ティーチングし、プレイバック時にティーチング位置の相互間を補間演算し、その補間位置を目標位置としてロボットアームを動作させる。
【０００３】
特許文献１には、ティーチング位置の相互間を曲線補間するロボットの補間演算装置が開示されている。この特許文献１による曲線補間は、例えば順次与えられた４つのティーチング位置Ｐ１〜Ｐ４があった場合、第２のティーチング位置Ｐ２と第３のティーチング位置Ｐ３との間を滑らかな曲線で補間するために、第２のティーチング位置Ｐ２において、移動方向を示す直線に接し且つ次の第３のティーチング位置Ｐ３を通る第１の円を演算し、第３のティーチング位置Ｐ３において、移動方向を示す直線に接し且つ前の第２のティーチング位置Ｐ２を通る第２の円を演算し、これら２つの円の間を滑らかな曲線（渡り曲線）で結ぶことにより、第２のティーチング位置Ｐ２と第３のティーチング位置Ｐ３との間を、第１の円から渡り曲線を経て第２の円に至る滑らかな曲線で補間するというものである。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平５−３４１８３３号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
現在のプレイバック型ロボットでは、移動軌跡が直線、曲線にかかわらず、ティーチング位置を細かく与えている。この場合、直線はロボットアームを真直ぐに動かしてティーチング位置を与えれば良いので比較的簡単であるが、曲線はロボットアームに曲がり動作させてティーチング位置を与えねばならないので手間がかかる。
【０００６】
曲線ティーチングは、例えば、成形型で製造した矩形状のプラスチック容器の縁に生じたバリをバリ取り用カッターで切除するような作業、或いは矩形状の容器と蓋との間にシール剤をノズルで塗布するような作業を行う場合に、容器のコーナー部分の移動軌跡をティーチングする際に必要となってくる。
このような場合、容器のコーナー部分は、比較的半径の小さな曲線であることが多いため、特にティーチング位置の間隔を狭くしなければならず、作業者にとっては格別に細かい作業を要求され、非常の手間のかかる作業となるのである。
【０００７】
これに対し、上述の特許文献１に開示された補間演算装置を適用すれば、容器の一辺（直線部分）からコーナー部分（円弧状部分）に移る位置と、コーナー部分から他の辺（直線部分）に移る位置をティーチングするだけで、後はティーチングされた２位置間が自動的に曲線によって補間されるので、少ないティーチング位置数でバリ取り用カッターやノズルの先端をコーナー部分に沿って移動させることができるようになる。
【０００８】
ところで、実際のティーチング作業はバリ取り用カッターやノズルなどのツール（エンドエフェクタ）先端を容器に沿って移動させながら行うのであるが、直線部分から円弧状部分に入る位置（直線部分の終了点、円弧状部分の開始点）は、それまで容器の直線部分に沿って直線移動させていたツール先端が容器から離れるので、その容器から離れる位置を見つけることで、比較的容易に検知できる。これに対し、円弧状部分から直線部分に入る位置（円弧状部分の終了点、直線部分の開始点）は、ツール先端が容器から離れるところというような見つけ方ができず、しかも、人の習性として、円弧状部分の途中を円弧状部分が終了して直線部分に入った位置と見誤ることは殆どなく、直線部分にやや入ったところ円弧状部分が終了して直線部分に入ったところと判断し勝ちになる、という事情がある。
【０００９】
そして、円弧状部分から直線部分に入った後の位置を円弧状部分の終了点（直線部分の開始点）としてティーチングすると、特許文献１の補間装置では、補間曲線がコーナー部分の外側を通るようになって実際にロボットを動作させたとき、ツール先端がコーナー部分の外側を当該コーナー部分から離れて移動するようになる。すると、バリが除去できずに部分的に残ったり、シール剤が容器と蓋の間に塗られない部分ができたりするという不具合を生ずる。
【００１０】
円弧状部分から直線部分に入る位置を正しく見つけ出すことができなかった場合、円弧状部分の前後の直線部分が平行で、円弧状部分が半円状であれば、直線部分から円弧状部分に入る位置を直線部分と垂直方向に移動させることで、反対側の円弧状部分から直線部分に入る位置を修正することができる。しかし、多くは円弧状部分の前後の直線部分は平行ではないので、ティーチング位置の誤りを修正することは困難で、バリ取り作業やシール作業が円弧状部分で正常に行われなくなって不良製品を製造する結果を招く。
【００１１】
本発明は上記した事情に鑑みてなされたもので、その目的は、２つの直線部分の間に円弧状部分が存在するような部分の作業対象部分に対し、一方の直線部分から円弧状部分を経て他方の直線部分へとティーチングを進めて行く場合に、円弧状部分から直線部分に入る位置を正しくティーチングできず、直線部分に入った後の位置をティーチングしてしまったとしても、エンドエフェクタ先端に作業対象部分の円弧状部分により近い形状の軌跡を辿らせることができるロボットの制御装置を提供することにある。
【課題を解決するための手段】
【００１２】
本発明は、第１〜第４のティーチング位置のうち、第１のティーチング位置と第２のティーチング位置との間、および第３のティーチング位置と第４のティーチング位置との間を直線補間し、第２のティーチング位置と前記第３のティーチング位置との間を円または楕円によって曲線補間する。
【００１３】
２つの直線部分の間に円弧状部分が存在する作業対象部分において、エンドエフェクタの先端を作業対象部分に沿って移動させながらティーチング作業を行う場合、上述のように、一方の直線部分から円弧状部分に入る位置は正確にティーチングできるが、曲線部分から他方の直線部分に入る位置は直線部分に入ってからのところをティーチングしてしまうという誤りを起こし勝ちである。
【００１４】
この誤りの場合、第２のティーチング位置と第３のティーチング位置との間は楕円で補間されるが、正しくティーチングされた第２のティーチング位置では円に近い楕円の長径部分が与えられ、円弧状部分から直線部分に入ってからの位置に誤ってティーチングされた第３のティーチング位置では直線に近い楕円の短径部分が与えられるので、エンドエフェクタ先端を本来与えるべき移動軌跡により近い軌跡に沿って移動させることができ、プレイバック時にエンドエフェクタ先端が円弧状部分の外側を通るといった不具合の発生を極力防止できる。
【図面の簡単な説明】
【００１５】
【図１】本発明の一実施形態において、補間曲線が楕円となる場合の楕円算出用の座標を示す図
【図２】補間曲線の演算の内容を示すフローチャート
【図３】補間曲線が円となる場合の円算出用座標図
【図４】補間曲線が図１とは別の楕円となる場合の楕円算出用の座標を示す図
【図５】ロボットシステムの全体を示す斜視図
【図６】ロボットシステムの電気的構成を示すブロック図
【発明を実施するための形態】
【００１６】
以下に本発明の一実施形態を図面に基づいて説明する。図５はロボットのシステム１の外観構成を示している。このロボットシステム１は、ロボットアーム２と、ロボットアーム２を制御するコントローラ（ロボット制御装置）３と、コントローラ３に接続されたティーチングペンダント４とから構成されている。
【００１７】
ロボットアーム２は、６軸の垂直多関節型ロボットとして構成されている。このロボットアーム２は、ベース５と、このベース５に第１軸Ｌ１を中心に水平方向に旋回可能に支持されたショルダ部６と、このショルダ部６に第２軸Ｌ２を中心に上下方向に旋回可能に支持された下アーム７と、この下アーム７に第３軸Ｌ３を中心に上下方向に旋回可能に支持された第１の上アーム８と、この第１の上アーム８の先端部に第４軸Ｌ４を中心に捻り回転可能に支持された第２の上アーム９と、この第２の上アーム９に第５軸Ｌ５を中心に上下方向に旋回可能に支持された手首１０と、この手首１０に第６軸Ｌ６を中心に捻り回転可能に支持されたフランジ１１とから構成されている。なお、ロボット先端であるフランジ１１には、バリ取りなどの各種作業を行うためのツール、ワークを把持するためのハンドや、視覚検査のために用いるカメラなどのエンドエフェクタ（図示せず）が取り付けられるようになっている。
【００１８】
ベース５、ショルダ部６、下アーム７、第１の上アーム８、第２の上アーム９、手首１０、フランジ１１は、ロボットアーム２におけるアームとして機能し、固定アームであるベース５を除く各アーム６〜１１は、前段のアームに回転可能に支持された回転関節軸である第１軸Ｌ１〜第６軸Ｌ６に固定連結されている。
【００１９】
次に、電気的なブロック構成を示す図６において、ロボットアーム２には、上記した各アーム６〜１１の回転関節軸である第１軸Ｌ１〜第６軸Ｌ６の周りに回転駆動させるためのサーボモータからなるモータＭ１〜Ｍ６が設けられ、このモータＭ１〜Ｍ６の回転により、図示しない減速機構を介して各アーム６〜１１が回転駆動されるように構成されている。また、各モータＭ１〜Ｍ６には、第１軸Ｌ１〜第６軸Ｌ６に連結されたエンコーダＳ１〜Ｓ６が設けられ、回転検出信号をコントローラ３に出力するように構成されている。
【００２０】
ロボットアーム２の動作を制御するコントローラ３は、マイコンを主体とした構成であり、ＣＰＵ１２、ＲＯＭ１３およびＲＡＭ１４を備えている。ＲＯＭ１３は、一般的な読み出し専用のメモリに加えて、電気的に書き換え可能なフラッシュメモリ（ＥＥＰＲＯＭの一種）などのメモリも含むものである。このＲＯＭ１３には、後述するティーチングに関する制御プログラムを含めてロボットアーム２を駆動制御するための各種制御プログラムが記憶されている。
【００２１】
また、コントローラ３には、モータＭ１〜Ｍ６を駆動させるための駆動回路１５が設けられている。コントローラ３には、ロボットアーム２側の各エンコーダＳ１〜Ｓ６からの検出信号に基づいて各モータＭ１〜Ｍ６の回転角を検出する回転位置検出回路が設けられており、ＣＰＵ１２は、その回転角情報に基づいて駆動回路１５に位置姿勢を制御するための制御信号を出力するように構成されている。
【００２２】
次に、ロボットアーム２の各アーム６〜１１の６つの回転関節軸である第１軸Ｌ１〜第６軸Ｌ６について説明する。ベース５および各アーム６〜１１には、３次元の座標が規定されている。このうち、床面に据え付けられるベース５の座標系（図５に座標軸ＸＹＺで示す）は、不動の座標系で基準座標（ロボット座標）とされる。アーム６〜１１の座標は、各アーム６〜１１の回転関節軸の第１軸Ｌ１〜第６軸Ｌ６の回転中心軸線上に設定されており、アーム６〜１１の回転によりロボット座標上での位置と姿勢が変化する。
【００２３】
ＲＯＭ１３には、ロボット座標上におけるショルダ部６の座標位置、ショルダ部６の座標上における下アーム７の座標位置、下アーム７の座標上における第１の上アーム８の座標位置、第１の上アーム８の座標上における第２の上アーム９の座標位置、第２の上アーム９の座標上における手首１０の座標位置、手首１０の座標上におけるフランジ１１の座標位置、各アーム６〜１１の長さなどの各種のパラメータが記憶されている。なお、以下では、各アーム６〜１１の座標のロボット座標上の位置を、単に、各アーム６〜１１の位置ということとする。
【００２４】
ロボットアーム先端であるフランジ１１の座標の原点は、当該フランジ１１の先端面の回転中心に定められている。そして、ＣＰＵ１２は、座標変換の計算機能を有し、ロボットアーム先端の位置（姿勢を含む；以下同じ）が与えられると、当該与えられた位置をロボットアーム先端が取るような各アーム６〜１１の回転角を演算（逆変換）できるようになっており、また、各アーム６〜１１の回転角が与えられると、各アーム６〜１１の位置を演算（順変換）できるようにもなっている。
【００２５】
ロボットアーム２はプレイバック型として構成され、ティーチング時に、ティーチングペンダント４により実際に動かされてロボットアーム先端の移動軌跡上の複数位置をティーチング位置としてＲＡＭ（記憶手段）１４に記憶させる。コントローラ３は、プレイバック時に、内部に記憶された制御プログラムに基づいて、ティーチング位置の相互間を直線或いは曲線によって補間し、補間位置を目標位置として各アーム６〜１１を駆動し、ロボットアーム先端がティーチング位置を通るように移動させる。
【００２６】
ここで、実際のロボット作業時には、ロボットアーム先端（フランジ１１）にはツール（エンドエフェクタ）が取り付けられ、コントローラ３にはフランジ１１の座標上におけるツール先端の位置が与えられる。これにより、フランジ１１の座標がツール先端に並行移動したと同等に扱われ、ティーチング時には、ツール先端の位置がティーチング位置としてコントローラ３に与えられるようになる。
【００２７】
次にコントローラ３が実行する補間の内容につき説明する。ここでは、プラスチック成形された矩形状の容器のバリ取りを行う場合に適用し、バリ取り用ツール先端を容器に沿って移動させながらティーチング位置を与えたものとする。図１はツール先端の移動方向に沿って順に与えられた第１のティーチング位置Ｐ１〜第４のティーチング位置Ｐ４の一例を示す。
【００２８】
これらティーチング位置のうち、第１のティーチング位置Ｐ１は容器の直線部分である一辺の途中位置、第２のティーチング位置Ｐ２は直線部分から円弧状部分（容器のコーナー部分）に入る位置（直線部分の終了位置で且つ円弧状部分の開始位置）、第３のティーチング位置Ｐ３は円弧状部分から別の直線部分である上記一辺と隣合う他の辺に入る位置（円弧状部分の終了位置で且つ直線部分の開始位置）、第４のティーチング位置Ｐ４は上記別の直線部分の途中位置として与えられたものとする。
【００２９】
ただし、ツール先端を容器に沿って移動させてティーチングした関係で、第２のティーチング位置Ｐ２は直線移動させたツール先端が容器から離れる瞬間の位置として与えられ、このことから、実際の容器の直線部分の終了位置で円弧状部分の開始位置に正しく一致しているが、第３のティーチング位置Ｐ３は円弧状部分から直線部分に移る位置として見分け難いため、直線部分にやや入った位置を与えられたものとする。
なお、第１のティーチング位置Ｐ１〜第４のティーチング位置Ｐ４は一平面上に存在するが、説明の簡単化のために、ロボット座標のＸＹ平面上、或いはＸＹ平面と平行の平面上にあるものとする。
【００３０】
上記第１のティーチング位置Ｐ１〜第４のティーチング位置Ｐ４は、第１のティーチング位置Ｐ１と第２のティーチング位置Ｐ２を通る第１の直線Ａと、第３のティーチング位置Ｐ３と第４のティーチング位置Ｐ４を通る第２の直線Ｂが、第２のティーチング位置Ｐ２と第３のティーチング位置Ｐ３との間で交差する、つまり第１の直線Ａと第２の直線Ｂの交点Ｐｓが第２のティーチング位置Ｐ２に関して第１のティーチング位置Ｐ１とは反対側で、且つ第３のティーチング位置Ｐ３に関し第４のティーチング位置Ｐ４とは反対側に存在する位置関係にある。
【００３１】
プレイバック時にティーチング位置間を補間する場合、コントローラ３は、第１のティーチング位置Ｐ１〜第４のティーチング位置Ｐ４が上述の位置関係にあるかどうかを、第１の直線Ａと第２の直線Ｂを求めて両直線Ａ，Ｂの交点Ｐｓの位置を演算（交点検出手段）することによって判別する。
【００３２】
第１のティーチング位置Ｐ１〜第４のティーチング位置Ｐ４が上述のような位置関係にある場合、コントローラ３は第１のティーチング位置Ｐ１と第２のティーチング位置Ｐ２との間、および第３のティーチング位置Ｐ３と第４のティーチング位置Ｐ４との間を直線で補間し、残る第２のティーチング位置Ｐ２と第３のティーチング位置Ｐ３との間は曲線で補間する（補間演算手段）。
【００３３】
図２は第２のティーチング位置Ｐ２と第３のティーチング位置Ｐ３との間を曲線補間する場合の制御内容（補間曲線演算手段）を示すフローチャートである。即ち、曲線補間動作に入ると、コントローラ３は第２のティーチング位置Ｐ２を通り第１の直線Ａに直交する第３の直線Ｃと、第３のティーチング位置Ｐ３を通り第２の直線Ｂに直交する第４の直線Ｄとを求める（図２のステップＳ１，２：直線設定手段）。
【００３４】
次にコントローラ３は第３の直線Ｃと第４の直線Ｄとの交点Ｐｏを求め（ステップ３）、そして、この交点Ｐｏから第２のティーチング位置Ｐ２までの距離ｅ２と第３のティーチング位置Ｐ３までの距離ｅ３とを求め（ステップ４，５：長さ検出手段）、それらの距離ｅ２とｅ３とを比較する（ステップ６：比較手段）。
【００３５】
ここでは、上述のように、第２のティーチング位置Ｐ２は容器の直線部分から円弧状部分に入った位置に正しく一致し、第３のティーチング位置Ｐ３は円弧状部分の終了位置から直線部分にやや入った位置を与えられたものであるから、ｅ２の方がｅ３よりも長いこととなり、コントローラ３は、交点Ｐｏまでの距離が長い第２のティーチング位置Ｐ２が存在する第３の直線Ｃを選択し、当該第３の直線Ｃ上に中心Ｐｃがあって第２のティーチング位置Ｐ２と第３のティーチング位置Ｐ３の双方に接する楕円を演算する（ステップ７：補間曲線演算手段）。
【００３６】
この場合の楕円の演算方法を述べる。まず、楕円の計算をし易くするために、第１のティーチング位置Ｐ１〜第４のティーチング位置Ｐ４が存在する平面上に計算用ｘｙ座標を設定する。この計算用ｘｙ座標は、原点を第２のティーチング位置Ｐ２、楕円の中心が存在する第３の直線Ｃをｘ軸、これと直交する第１の直線Ａをｙ軸に定めている。
そして、第１のティーチング位置Ｐ１〜第４のティーチング位置Ｐ４の計算用ｘｙ座標上の位置をＰｎ＝（ｘn，ｙn）とすると、ｙ軸上にある第１のティーチング位置Ｐ１の座標（ｘ１，ｙ１）は（０，ｙ１）、原点である第２のティーチング位置Ｐ２の座標（ｘ２，ｙ２）は（０，０）、第３のティーチング位置Ｐ３は（ｘ３，ｙ３）、第４のティーチング位置Ｐ４は（ｘ４，ｙ４）となる。また、楕円の中心はｘ軸上にあり、その楕円は第１の直線Ａ（ｙ軸）に接するのであるから、当該楕円の長径がｘ軸に重なることとなる。従って、楕円の長径の長さをａ、短径の長さをｂとすると、楕円の中心Ｐｃの計算用ｘｙ座標上の位置を表す座標（ｘｃ、ｙｃ）は（ａ，０）となる。
【００３７】
ティーチング時にロボット座標で与えられた第１のティーチング位置Ｐ１〜第４のティーチング位置Ｐ４は次の（１）式の同時変換行列を用いることで計算用ｘｙ座標上の位置に変換できる。
【００３８】
【数１】
【００３９】
ここで、（１）式のψはロボット座標に対する計算用ｘｙ座標の傾き、ｘ０、ｙ０はロボット座標に対する計算用ｘｙ座標のＸ軸方向およびＹ軸方向のずれ量を示すもので、ｃｏｓψ、ｓｉｎψ、ｘ０、ｙ０は次の（２）式〜（５）式によって求めることができる。
【００４０】
【数２】
【００４１】
但し、上記（２）式〜（５）式において、Ｘ１，Ｘ２，Ｙ１，Ｙ２はロボット座標上のティーチング位置Ｐ１，Ｐ２のＸＹ座標値である。
【００４２】
さて、Ｐ３とＰ４との差を、ｕ＝ｘ４−ｘ３、ν＝ｙ４−ｙ３と定義する。
また、媒介変数τを用いて楕円を表現すると、
ｘ＝ａ＋ａ・ｃｏｓτ、ｙ＝ｂ・ｓｉｎτとなる。ただし、τは楕円の偏角を意味していない。
【００４３】
また、ｄｘ／ｄτ＝−ａ・ｓｉｎτ、ｄｙ／ｄτ＝ｂ・ｃｏｓτ
から、楕円の傾きはｄｙ／ｄｘ＝−ｂ／ａ（ｃｏｓτ／ｓｉｎτ）となる。このとき、下記（６）式で示すように、ｄｙ／ｄｘ−ｙ／ｘ＝ｄｙ／ｄｘ（１／ｃｏｓτ）であるので、ｃｏｓτは下記（７）式のようになる。
【００４４】
【数３】
【００４５】
【数４】
【００４６】
また、ｂ＞０であるので、ｓｉｎτは下記（８）式のようになる。
【００４７】
【数５】
【００４８】
ここで、第３のティーチング位置Ｐ３に着目すると、ｘ３＝ａ＋ａ・ｃｏｓτ３＝ａ（１＋ｃｏｓτ３）、ｙ３＝ｂ・ｓｉｎτ３、ｄｙ３／ｄｘ３＝−ｂ・ｃｏｓτ３／ａ・ｓｉｎτ３＝ν／ｕであるので、（７）式からｃｏｓτ３は下記（９）式のようになる。
【００４９】
【数６】
【００５０】
よって、ｃｏｓτ３、ｓｉｎτ３は次の（１０）式、（１１）式で表わすことができる。
【００５１】
【数７】
【００５２】
従って、楕円の長径ａおよび短径ｂは下記（１２）式、（１３）式によって求めることができる。
【００５３】
【数８】
【００５４】
以上により、長径の長さがａで短径の長さがｂの楕円であって、計算用ｘｙ座標で（ａ，０）の位置に中心Ｐｃをもつ楕円であって、第２のティーチング位置Ｐ２において第１の直線Ａに接し且つ第３のティーチング位置Ｐ３において第２の直線Ｂに接する楕円を求めることができる。
【００５５】
このようにして求めた楕円は計算用ｘｙ座標上のものであるから、次の（１４）式で示す同時変換行列により、この楕円をロボット座標上の楕円に変換することができる。
【００５６】
【数９】
【００５７】
そして、この楕円によって第２のティーチング位置Ｐ２から第３のティーチング位置Ｐ３に至る曲線を描き、これを補間曲線に定めてエンドとなる（ステップ１０）。
【００５８】
一方、ティーチング位置Ｐ１〜Ｐ４の与え方によっては、ｅ２とｅ３とが等しい場合（第２のティーチング位置Ｐ２が直線部分の終了位置で且つ円弧状部分の開始位置に正しく一致し、第３のティーチング位置Ｐ３が円弧状部分の終了位置で且つ直線部分の開始位置に正しく一致した場合を含む）や、ｅ３の方がｅ２よりも長い場合（第２のティーチング位置Ｐ２が直線部分の終了位置で且つ円弧状部分の開始位置を正しく与えられ、第３のティーチング位置Ｐ３が円弧状部分の終了位置（直線部分の開始位置）よりも手前の円弧状部分に与えられた場合を含む）がある。
【００５９】
ｅ２とｅ３とが等しい場合には、コントローラ３は、中心が前記交点Ｐｏにあって、半径が交点Ｐｏから第２のティーチング位置Ｐ２までの距離ｅ２（交点Ｐｏから第３のティーチング位置Ｐ３までの距離ｅ３と同じ）を半径とする円を演算し、この第２のティーチング位置Ｐ３において第１の直線Ａに接し、第３のティーチング位置Ｐ３において第２の直線Ｂに接する円を、第２のティーチング位置Ｐ２から第３のティーチング位置Ｐ３に至る補間曲線とする（ステップ８）。
【００６０】
ｅ３の方がｅ２よりも長かった場合、コントローラ３は第４の直線Ｄ上に中心があって第２のティーチング位置Ｐ２と第３のティーチング位置Ｐ３で第１の直線Ａと第２の直線Ｂに接する楕円を演算する（ステップ９）。この場合、計算用ｘｙ座標の中心を第３のティーチング位置Ｐ３、楕円の中心があるｘ軸を第４の直線Ｄ、第２の直線Ｂをｙ軸とする。そして、楕円の長径ａ、短径ｂを次の（１５）式〜（１８）式によって求めるものである。
【００６１】
【数１０】
【００６２】
そして、その楕円を、第２のティーチング位置Ｐ２と第３のティーチング位置Ｐ３との間を補間する補間曲線と定め（ステップ１０）、エンドとなる。
【００６３】
このように本実施形態の曲線補間によれば、ティーチング一位置として最も起き易いパターン、つまり直線部分から円弧状部分に入る位置（ティーチング位置Ｐ２）は正確に教示できるが、円弧状部分から直線部分に入る位置は円弧状部分の終了位置から直線部分にやや入った位置（第３のティーチング位置Ｐ３）を教示してしまうパターンについては、ツール先端に作業対象部分の円弧状部分の形状により近い移動軌跡を辿らせることができる。
【００６４】
即ち、前述の特許文献１では、第１のティーチング位置Ｐ１から第２のティーチング位置Ｐ２までは直線移動、第２のティーチング位置Ｐ２から第３のティーチング位置Ｐ３までは曲線移動、第３のティーチング位置Ｐ３から第４のティーチング位置Ｐ４までは直線移動する場合、第２のティーチング位置Ｐ２での移動方向を第１のティーチング位置Ｐ１からの直線移動方向、つまり第１の直線Ａと同じ方向、第３のティーチング位置Ｐ３での移動方向を第４のティーチング位置Ｐ４に向かう直線移動方向、つまり第２の直線Ｂと同じ方向として、第２のティーチング位置Ｐ２と第３のティーチング位置Ｐ３との間は、第２のティーチング位置Ｐ２での移動方向を示す直線（第１の直線Ａ）に接し且つ第２のティーチング位置Ｐ２と第３のティーチング位置Ｐ３とを通る第１の円と、第３のティーチング位置Ｐ３での移動方向を示す直線（第２の直線Ｂ）に接し且つ第２のティー位置Ｐ２と第３のティー位置Ｐ３とを通る第２の円とを演算し、そして、第１の円と第２の円とを結ぶ滑らかな曲線(渡り曲線)を演算し、以上により、第２のティーチング位置Ｐ２と第３のティーチング位置Ｐ３との間を、第１の円から渡り曲線を経て第２の円に至る滑らかな曲線上を補間曲線と定め、ツール先端がこの補間曲線上を移動するようにしている。このため、第２のティーチング位置Ｐ２と第３のティーチング位置Ｐ３の近くでは良いが、それらの間の部分では、ツール先端が、実際の円弧状部分の外縁よりも外側を通る軌跡で移動することとなる。
【００６５】
しかしながら、本実施形態では、第２のティーチング位置Ｐ２が正しく直線部分から円弧状部分に入る位置にティーチングされ、第３のティーチング位置Ｐ３が円弧状部分から直線部分に入ってからの位置にティーチングされたとすると、正しくティーチングされた第２のティーチング位置Ｐ２では円に近い楕円の長径部分が与えられ、円弧状部分から直線部分に入ってからの位置に誤ってティーチングされた第３のティーチング位置Ｐ３では直線に近い楕円の短径部分が与えられるので、ツール先端を本来与えるべき移動軌跡（四分の一円）により近い軌跡に沿い、且つ本来与えるべき移動軌跡よりも内側を移動させることができる。
【００６６】
このため、バリが部分的に残ったり、シール剤が容器と蓋の間に正しく塗られない部分が生じたりするという不具合が生ずることを防止できる。
なお、ｅ２がｅ３に等しい場合には、第２のティーチング位置Ｐ２と第３のティーチング位置Ｐ３との間は円によって曲線補間されるので、ツール先端は円弧状部分の外縁に沿って移動するので良いが、ｅ３の方がｅ２よりも長い場合、例えば第２のティーチング位置Ｐ２が容器の直線部分から円弧状部分に入る位置を正しく与えられ、第３のティーチング位置Ｐ３が円弧状部分の終了位置から直線部分に入る位置よりも手前の円弧状部分に与えられた場合には、ツール先端が円弧状部分の外側を通る軌跡を辿ることになるが、このような教示パターンは殆ど生じないであるので特に問題はない。
【００６７】
なお、本発明は上記し且つ図面に示す実施形態に限定されるものではなく、以下のような拡張或いは変更が可能である。
第１のティーチング位置Ｐ１〜第４のティーチング位置Ｐ４が位置する一平面は必ずしもロボット座標のＸＹ平面或いはＸＹ平面と平行な面である必要ない。ロボット座標のＸＹ平面に対し傾いていても良く、この場合、座標変換の（１）式、（２）式を、３次元を表現する周知の座標変換の式に置き換えれば良い。
ロボットアームの先端に取り付けるエンドエフェクタとしては、バリ取り用カッターに限られない。
【符号の説明】
【００６８】
図面中、２はロボットアーム、３はコントローラ（ロボット制御装置、補間演算手段、補間曲線演算手段）、１１はフランジ（ロボットアーム先端）を示す。
【技術分野】
【０００１】
本発明は、ティーチングされた複数位置の相互間を直線および曲線で補間するロボットの制御装置に関する。
【背景技術】
【０００２】
プレイバック型のロボットでは、ティーチング装置によりロボットアームを実際に動かしてロボットアーム先端に取り付けられたエンドエフェクタの位置を順次ティーチングし、プレイバック時にティーチング位置の相互間を補間演算し、その補間位置を目標位置としてロボットアームを動作させる。
【０００３】
特許文献１には、ティーチング位置の相互間を曲線補間するロボットの補間演算装置が開示されている。この特許文献１による曲線補間は、例えば順次与えられた４つのティーチング位置Ｐ１〜Ｐ４があった場合、第２のティーチング位置Ｐ２と第３のティーチング位置Ｐ３との間を滑らかな曲線で補間するために、第２のティーチング位置Ｐ２において、移動方向を示す直線に接し且つ次の第３のティーチング位置Ｐ３を通る第１の円を演算し、第３のティーチング位置Ｐ３において、移動方向を示す直線に接し且つ前の第２のティーチング位置Ｐ２を通る第２の円を演算し、これら２つの円の間を滑らかな曲線（渡り曲線）で結ぶことにより、第２のティーチング位置Ｐ２と第３のティーチング位置Ｐ３との間を、第１の円から渡り曲線を経て第２の円に至る滑らかな曲線で補間するというものである。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平５−３４１８３３号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
現在のプレイバック型ロボットでは、移動軌跡が直線、曲線にかかわらず、ティーチング位置を細かく与えている。この場合、直線はロボットアームを真直ぐに動かしてティーチング位置を与えれば良いので比較的簡単であるが、曲線はロボットアームに曲がり動作させてティーチング位置を与えねばならないので手間がかかる。
【０００６】
曲線ティーチングは、例えば、成形型で製造した矩形状のプラスチック容器の縁に生じたバリをバリ取り用カッターで切除するような作業、或いは矩形状の容器と蓋との間にシール剤をノズルで塗布するような作業を行う場合に、容器のコーナー部分の移動軌跡をティーチングする際に必要となってくる。
このような場合、容器のコーナー部分は、比較的半径の小さな曲線であることが多いため、特にティーチング位置の間隔を狭くしなければならず、作業者にとっては格別に細かい作業を要求され、非常の手間のかかる作業となるのである。
【０００７】
これに対し、上述の特許文献１に開示された補間演算装置を適用すれば、容器の一辺（直線部分）からコーナー部分（円弧状部分）に移る位置と、コーナー部分から他の辺（直線部分）に移る位置をティーチングするだけで、後はティーチングされた２位置間が自動的に曲線によって補間されるので、少ないティーチング位置数でバリ取り用カッターやノズルの先端をコーナー部分に沿って移動させることができるようになる。
【０００８】
ところで、実際のティーチング作業はバリ取り用カッターやノズルなどのツール（エンドエフェクタ）先端を容器に沿って移動させながら行うのであるが、直線部分から円弧状部分に入る位置（直線部分の終了点、円弧状部分の開始点）は、それまで容器の直線部分に沿って直線移動させていたツール先端が容器から離れるので、その容器から離れる位置を見つけることで、比較的容易に検知できる。これに対し、円弧状部分から直線部分に入る位置（円弧状部分の終了点、直線部分の開始点）は、ツール先端が容器から離れるところというような見つけ方ができず、しかも、人の習性として、円弧状部分の途中を円弧状部分が終了して直線部分に入った位置と見誤ることは殆どなく、直線部分にやや入ったところ円弧状部分が終了して直線部分に入ったところと判断し勝ちになる、という事情がある。
【０００９】
そして、円弧状部分から直線部分に入った後の位置を円弧状部分の終了点（直線部分の開始点）としてティーチングすると、特許文献１の補間装置では、補間曲線がコーナー部分の外側を通るようになって実際にロボットを動作させたとき、ツール先端がコーナー部分の外側を当該コーナー部分から離れて移動するようになる。すると、バリが除去できずに部分的に残ったり、シール剤が容器と蓋の間に塗られない部分ができたりするという不具合を生ずる。
【００１０】
円弧状部分から直線部分に入る位置を正しく見つけ出すことができなかった場合、円弧状部分の前後の直線部分が平行で、円弧状部分が半円状であれば、直線部分から円弧状部分に入る位置を直線部分と垂直方向に移動させることで、反対側の円弧状部分から直線部分に入る位置を修正することができる。しかし、多くは円弧状部分の前後の直線部分は平行ではないので、ティーチング位置の誤りを修正することは困難で、バリ取り作業やシール作業が円弧状部分で正常に行われなくなって不良製品を製造する結果を招く。
【００１１】
本発明は上記した事情に鑑みてなされたもので、その目的は、２つの直線部分の間に円弧状部分が存在するような部分の作業対象部分に対し、一方の直線部分から円弧状部分を経て他方の直線部分へとティーチングを進めて行く場合に、円弧状部分から直線部分に入る位置を正しくティーチングできず、直線部分に入った後の位置をティーチングしてしまったとしても、エンドエフェクタ先端に作業対象部分の円弧状部分により近い形状の軌跡を辿らせることができるロボットの制御装置を提供することにある。
【課題を解決するための手段】
【００１２】
本発明は、第１〜第４のティーチング位置のうち、第１のティーチング位置と第２のティーチング位置との間、および第３のティーチング位置と第４のティーチング位置との間を直線補間し、第２のティーチング位置と前記第３のティーチング位置との間を円または楕円によって曲線補間する。
【００１３】
２つの直線部分の間に円弧状部分が存在する作業対象部分において、エンドエフェクタの先端を作業対象部分に沿って移動させながらティーチング作業を行う場合、上述のように、一方の直線部分から円弧状部分に入る位置は正確にティーチングできるが、曲線部分から他方の直線部分に入る位置は直線部分に入ってからのところをティーチングしてしまうという誤りを起こし勝ちである。
【００１４】
この誤りの場合、第２のティーチング位置と第３のティーチング位置との間は楕円で補間されるが、正しくティーチングされた第２のティーチング位置では円に近い楕円の長径部分が与えられ、円弧状部分から直線部分に入ってからの位置に誤ってティーチングされた第３のティーチング位置では直線に近い楕円の短径部分が与えられるので、エンドエフェクタ先端を本来与えるべき移動軌跡により近い軌跡に沿って移動させることができ、プレイバック時にエンドエフェクタ先端が円弧状部分の外側を通るといった不具合の発生を極力防止できる。
【図面の簡単な説明】
【００１５】
【図１】本発明の一実施形態において、補間曲線が楕円となる場合の楕円算出用の座標を示す図
【図２】補間曲線の演算の内容を示すフローチャート
【図３】補間曲線が円となる場合の円算出用座標図
【図４】補間曲線が図１とは別の楕円となる場合の楕円算出用の座標を示す図
【図５】ロボットシステムの全体を示す斜視図
【図６】ロボットシステムの電気的構成を示すブロック図
【発明を実施するための形態】
【００１６】
以下に本発明の一実施形態を図面に基づいて説明する。図５はロボットのシステム１の外観構成を示している。このロボットシステム１は、ロボットアーム２と、ロボットアーム２を制御するコントローラ（ロボット制御装置）３と、コントローラ３に接続されたティーチングペンダント４とから構成されている。
【００１７】
ロボットアーム２は、６軸の垂直多関節型ロボットとして構成されている。このロボットアーム２は、ベース５と、このベース５に第１軸Ｌ１を中心に水平方向に旋回可能に支持されたショルダ部６と、このショルダ部６に第２軸Ｌ２を中心に上下方向に旋回可能に支持された下アーム７と、この下アーム７に第３軸Ｌ３を中心に上下方向に旋回可能に支持された第１の上アーム８と、この第１の上アーム８の先端部に第４軸Ｌ４を中心に捻り回転可能に支持された第２の上アーム９と、この第２の上アーム９に第５軸Ｌ５を中心に上下方向に旋回可能に支持された手首１０と、この手首１０に第６軸Ｌ６を中心に捻り回転可能に支持されたフランジ１１とから構成されている。なお、ロボット先端であるフランジ１１には、バリ取りなどの各種作業を行うためのツール、ワークを把持するためのハンドや、視覚検査のために用いるカメラなどのエンドエフェクタ（図示せず）が取り付けられるようになっている。
【００１８】
ベース５、ショルダ部６、下アーム７、第１の上アーム８、第２の上アーム９、手首１０、フランジ１１は、ロボットアーム２におけるアームとして機能し、固定アームであるベース５を除く各アーム６〜１１は、前段のアームに回転可能に支持された回転関節軸である第１軸Ｌ１〜第６軸Ｌ６に固定連結されている。
【００１９】
次に、電気的なブロック構成を示す図６において、ロボットアーム２には、上記した各アーム６〜１１の回転関節軸である第１軸Ｌ１〜第６軸Ｌ６の周りに回転駆動させるためのサーボモータからなるモータＭ１〜Ｍ６が設けられ、このモータＭ１〜Ｍ６の回転により、図示しない減速機構を介して各アーム６〜１１が回転駆動されるように構成されている。また、各モータＭ１〜Ｍ６には、第１軸Ｌ１〜第６軸Ｌ６に連結されたエンコーダＳ１〜Ｓ６が設けられ、回転検出信号をコントローラ３に出力するように構成されている。
【００２０】
ロボットアーム２の動作を制御するコントローラ３は、マイコンを主体とした構成であり、ＣＰＵ１２、ＲＯＭ１３およびＲＡＭ１４を備えている。ＲＯＭ１３は、一般的な読み出し専用のメモリに加えて、電気的に書き換え可能なフラッシュメモリ（ＥＥＰＲＯＭの一種）などのメモリも含むものである。このＲＯＭ１３には、後述するティーチングに関する制御プログラムを含めてロボットアーム２を駆動制御するための各種制御プログラムが記憶されている。
【００２１】
また、コントローラ３には、モータＭ１〜Ｍ６を駆動させるための駆動回路１５が設けられている。コントローラ３には、ロボットアーム２側の各エンコーダＳ１〜Ｓ６からの検出信号に基づいて各モータＭ１〜Ｍ６の回転角を検出する回転位置検出回路が設けられており、ＣＰＵ１２は、その回転角情報に基づいて駆動回路１５に位置姿勢を制御するための制御信号を出力するように構成されている。
【００２２】
次に、ロボットアーム２の各アーム６〜１１の６つの回転関節軸である第１軸Ｌ１〜第６軸Ｌ６について説明する。ベース５および各アーム６〜１１には、３次元の座標が規定されている。このうち、床面に据え付けられるベース５の座標系（図５に座標軸ＸＹＺで示す）は、不動の座標系で基準座標（ロボット座標）とされる。アーム６〜１１の座標は、各アーム６〜１１の回転関節軸の第１軸Ｌ１〜第６軸Ｌ６の回転中心軸線上に設定されており、アーム６〜１１の回転によりロボット座標上での位置と姿勢が変化する。
【００２３】
ＲＯＭ１３には、ロボット座標上におけるショルダ部６の座標位置、ショルダ部６の座標上における下アーム７の座標位置、下アーム７の座標上における第１の上アーム８の座標位置、第１の上アーム８の座標上における第２の上アーム９の座標位置、第２の上アーム９の座標上における手首１０の座標位置、手首１０の座標上におけるフランジ１１の座標位置、各アーム６〜１１の長さなどの各種のパラメータが記憶されている。なお、以下では、各アーム６〜１１の座標のロボット座標上の位置を、単に、各アーム６〜１１の位置ということとする。
【００２４】
ロボットアーム先端であるフランジ１１の座標の原点は、当該フランジ１１の先端面の回転中心に定められている。そして、ＣＰＵ１２は、座標変換の計算機能を有し、ロボットアーム先端の位置（姿勢を含む；以下同じ）が与えられると、当該与えられた位置をロボットアーム先端が取るような各アーム６〜１１の回転角を演算（逆変換）できるようになっており、また、各アーム６〜１１の回転角が与えられると、各アーム６〜１１の位置を演算（順変換）できるようにもなっている。
【００２５】
ロボットアーム２はプレイバック型として構成され、ティーチング時に、ティーチングペンダント４により実際に動かされてロボットアーム先端の移動軌跡上の複数位置をティーチング位置としてＲＡＭ（記憶手段）１４に記憶させる。コントローラ３は、プレイバック時に、内部に記憶された制御プログラムに基づいて、ティーチング位置の相互間を直線或いは曲線によって補間し、補間位置を目標位置として各アーム６〜１１を駆動し、ロボットアーム先端がティーチング位置を通るように移動させる。
【００２６】
ここで、実際のロボット作業時には、ロボットアーム先端（フランジ１１）にはツール（エンドエフェクタ）が取り付けられ、コントローラ３にはフランジ１１の座標上におけるツール先端の位置が与えられる。これにより、フランジ１１の座標がツール先端に並行移動したと同等に扱われ、ティーチング時には、ツール先端の位置がティーチング位置としてコントローラ３に与えられるようになる。
【００２７】
次にコントローラ３が実行する補間の内容につき説明する。ここでは、プラスチック成形された矩形状の容器のバリ取りを行う場合に適用し、バリ取り用ツール先端を容器に沿って移動させながらティーチング位置を与えたものとする。図１はツール先端の移動方向に沿って順に与えられた第１のティーチング位置Ｐ１〜第４のティーチング位置Ｐ４の一例を示す。
【００２８】
これらティーチング位置のうち、第１のティーチング位置Ｐ１は容器の直線部分である一辺の途中位置、第２のティーチング位置Ｐ２は直線部分から円弧状部分（容器のコーナー部分）に入る位置（直線部分の終了位置で且つ円弧状部分の開始位置）、第３のティーチング位置Ｐ３は円弧状部分から別の直線部分である上記一辺と隣合う他の辺に入る位置（円弧状部分の終了位置で且つ直線部分の開始位置）、第４のティーチング位置Ｐ４は上記別の直線部分の途中位置として与えられたものとする。
【００２９】
ただし、ツール先端を容器に沿って移動させてティーチングした関係で、第２のティーチング位置Ｐ２は直線移動させたツール先端が容器から離れる瞬間の位置として与えられ、このことから、実際の容器の直線部分の終了位置で円弧状部分の開始位置に正しく一致しているが、第３のティーチング位置Ｐ３は円弧状部分から直線部分に移る位置として見分け難いため、直線部分にやや入った位置を与えられたものとする。
なお、第１のティーチング位置Ｐ１〜第４のティーチング位置Ｐ４は一平面上に存在するが、説明の簡単化のために、ロボット座標のＸＹ平面上、或いはＸＹ平面と平行の平面上にあるものとする。
【００３０】
上記第１のティーチング位置Ｐ１〜第４のティーチング位置Ｐ４は、第１のティーチング位置Ｐ１と第２のティーチング位置Ｐ２を通る第１の直線Ａと、第３のティーチング位置Ｐ３と第４のティーチング位置Ｐ４を通る第２の直線Ｂが、第２のティーチング位置Ｐ２と第３のティーチング位置Ｐ３との間で交差する、つまり第１の直線Ａと第２の直線Ｂの交点Ｐｓが第２のティーチング位置Ｐ２に関して第１のティーチング位置Ｐ１とは反対側で、且つ第３のティーチング位置Ｐ３に関し第４のティーチング位置Ｐ４とは反対側に存在する位置関係にある。
【００３１】
プレイバック時にティーチング位置間を補間する場合、コントローラ３は、第１のティーチング位置Ｐ１〜第４のティーチング位置Ｐ４が上述の位置関係にあるかどうかを、第１の直線Ａと第２の直線Ｂを求めて両直線Ａ，Ｂの交点Ｐｓの位置を演算（交点検出手段）することによって判別する。
【００３２】
第１のティーチング位置Ｐ１〜第４のティーチング位置Ｐ４が上述のような位置関係にある場合、コントローラ３は第１のティーチング位置Ｐ１と第２のティーチング位置Ｐ２との間、および第３のティーチング位置Ｐ３と第４のティーチング位置Ｐ４との間を直線で補間し、残る第２のティーチング位置Ｐ２と第３のティーチング位置Ｐ３との間は曲線で補間する（補間演算手段）。
【００３３】
図２は第２のティーチング位置Ｐ２と第３のティーチング位置Ｐ３との間を曲線補間する場合の制御内容（補間曲線演算手段）を示すフローチャートである。即ち、曲線補間動作に入ると、コントローラ３は第２のティーチング位置Ｐ２を通り第１の直線Ａに直交する第３の直線Ｃと、第３のティーチング位置Ｐ３を通り第２の直線Ｂに直交する第４の直線Ｄとを求める（図２のステップＳ１，２：直線設定手段）。
【００３４】
次にコントローラ３は第３の直線Ｃと第４の直線Ｄとの交点Ｐｏを求め（ステップ３）、そして、この交点Ｐｏから第２のティーチング位置Ｐ２までの距離ｅ２と第３のティーチング位置Ｐ３までの距離ｅ３とを求め（ステップ４，５：長さ検出手段）、それらの距離ｅ２とｅ３とを比較する（ステップ６：比較手段）。
【００３５】
ここでは、上述のように、第２のティーチング位置Ｐ２は容器の直線部分から円弧状部分に入った位置に正しく一致し、第３のティーチング位置Ｐ３は円弧状部分の終了位置から直線部分にやや入った位置を与えられたものであるから、ｅ２の方がｅ３よりも長いこととなり、コントローラ３は、交点Ｐｏまでの距離が長い第２のティーチング位置Ｐ２が存在する第３の直線Ｃを選択し、当該第３の直線Ｃ上に中心Ｐｃがあって第２のティーチング位置Ｐ２と第３のティーチング位置Ｐ３の双方に接する楕円を演算する（ステップ７：補間曲線演算手段）。
【００３６】
この場合の楕円の演算方法を述べる。まず、楕円の計算をし易くするために、第１のティーチング位置Ｐ１〜第４のティーチング位置Ｐ４が存在する平面上に計算用ｘｙ座標を設定する。この計算用ｘｙ座標は、原点を第２のティーチング位置Ｐ２、楕円の中心が存在する第３の直線Ｃをｘ軸、これと直交する第１の直線Ａをｙ軸に定めている。
そして、第１のティーチング位置Ｐ１〜第４のティーチング位置Ｐ４の計算用ｘｙ座標上の位置をＰｎ＝（ｘn，ｙn）とすると、ｙ軸上にある第１のティーチング位置Ｐ１の座標（ｘ１，ｙ１）は（０，ｙ１）、原点である第２のティーチング位置Ｐ２の座標（ｘ２，ｙ２）は（０，０）、第３のティーチング位置Ｐ３は（ｘ３，ｙ３）、第４のティーチング位置Ｐ４は（ｘ４，ｙ４）となる。また、楕円の中心はｘ軸上にあり、その楕円は第１の直線Ａ（ｙ軸）に接するのであるから、当該楕円の長径がｘ軸に重なることとなる。従って、楕円の長径の長さをａ、短径の長さをｂとすると、楕円の中心Ｐｃの計算用ｘｙ座標上の位置を表す座標（ｘｃ、ｙｃ）は（ａ，０）となる。
【００３７】
ティーチング時にロボット座標で与えられた第１のティーチング位置Ｐ１〜第４のティーチング位置Ｐ４は次の（１）式の同時変換行列を用いることで計算用ｘｙ座標上の位置に変換できる。
【００３８】
【数１】
【００３９】
ここで、（１）式のψはロボット座標に対する計算用ｘｙ座標の傾き、ｘ０、ｙ０はロボット座標に対する計算用ｘｙ座標のＸ軸方向およびＹ軸方向のずれ量を示すもので、ｃｏｓψ、ｓｉｎψ、ｘ０、ｙ０は次の（２）式〜（５）式によって求めることができる。
【００４０】
【数２】
【００４１】
但し、上記（２）式〜（５）式において、Ｘ１，Ｘ２，Ｙ１，Ｙ２はロボット座標上のティーチング位置Ｐ１，Ｐ２のＸＹ座標値である。
【００４２】
さて、Ｐ３とＰ４との差を、ｕ＝ｘ４−ｘ３、ν＝ｙ４−ｙ３と定義する。
また、媒介変数τを用いて楕円を表現すると、
ｘ＝ａ＋ａ・ｃｏｓτ、ｙ＝ｂ・ｓｉｎτとなる。ただし、τは楕円の偏角を意味していない。
【００４３】
また、ｄｘ／ｄτ＝−ａ・ｓｉｎτ、ｄｙ／ｄτ＝ｂ・ｃｏｓτ
から、楕円の傾きはｄｙ／ｄｘ＝−ｂ／ａ（ｃｏｓτ／ｓｉｎτ）となる。このとき、下記（６）式で示すように、ｄｙ／ｄｘ−ｙ／ｘ＝ｄｙ／ｄｘ（１／ｃｏｓτ）であるので、ｃｏｓτは下記（７）式のようになる。
【００４４】
【数３】
【００４５】
【数４】
【００４６】
また、ｂ＞０であるので、ｓｉｎτは下記（８）式のようになる。
【００４７】
【数５】
【００４８】
ここで、第３のティーチング位置Ｐ３に着目すると、ｘ３＝ａ＋ａ・ｃｏｓτ３＝ａ（１＋ｃｏｓτ３）、ｙ３＝ｂ・ｓｉｎτ３、ｄｙ３／ｄｘ３＝−ｂ・ｃｏｓτ３／ａ・ｓｉｎτ３＝ν／ｕであるので、（７）式からｃｏｓτ３は下記（９）式のようになる。
【００４９】
【数６】
【００５０】
よって、ｃｏｓτ３、ｓｉｎτ３は次の（１０）式、（１１）式で表わすことができる。
【００５１】
【数７】
【００５２】
従って、楕円の長径ａおよび短径ｂは下記（１２）式、（１３）式によって求めることができる。
【００５３】
【数８】
【００５４】
以上により、長径の長さがａで短径の長さがｂの楕円であって、計算用ｘｙ座標で（ａ，０）の位置に中心Ｐｃをもつ楕円であって、第２のティーチング位置Ｐ２において第１の直線Ａに接し且つ第３のティーチング位置Ｐ３において第２の直線Ｂに接する楕円を求めることができる。
【００５５】
このようにして求めた楕円は計算用ｘｙ座標上のものであるから、次の（１４）式で示す同時変換行列により、この楕円をロボット座標上の楕円に変換することができる。
【００５６】
【数９】
【００５７】
そして、この楕円によって第２のティーチング位置Ｐ２から第３のティーチング位置Ｐ３に至る曲線を描き、これを補間曲線に定めてエンドとなる（ステップ１０）。
【００５８】
一方、ティーチング位置Ｐ１〜Ｐ４の与え方によっては、ｅ２とｅ３とが等しい場合（第２のティーチング位置Ｐ２が直線部分の終了位置で且つ円弧状部分の開始位置に正しく一致し、第３のティーチング位置Ｐ３が円弧状部分の終了位置で且つ直線部分の開始位置に正しく一致した場合を含む）や、ｅ３の方がｅ２よりも長い場合（第２のティーチング位置Ｐ２が直線部分の終了位置で且つ円弧状部分の開始位置を正しく与えられ、第３のティーチング位置Ｐ３が円弧状部分の終了位置（直線部分の開始位置）よりも手前の円弧状部分に与えられた場合を含む）がある。
【００５９】
ｅ２とｅ３とが等しい場合には、コントローラ３は、中心が前記交点Ｐｏにあって、半径が交点Ｐｏから第２のティーチング位置Ｐ２までの距離ｅ２（交点Ｐｏから第３のティーチング位置Ｐ３までの距離ｅ３と同じ）を半径とする円を演算し、この第２のティーチング位置Ｐ３において第１の直線Ａに接し、第３のティーチング位置Ｐ３において第２の直線Ｂに接する円を、第２のティーチング位置Ｐ２から第３のティーチング位置Ｐ３に至る補間曲線とする（ステップ８）。
【００６０】
ｅ３の方がｅ２よりも長かった場合、コントローラ３は第４の直線Ｄ上に中心があって第２のティーチング位置Ｐ２と第３のティーチング位置Ｐ３で第１の直線Ａと第２の直線Ｂに接する楕円を演算する（ステップ９）。この場合、計算用ｘｙ座標の中心を第３のティーチング位置Ｐ３、楕円の中心があるｘ軸を第４の直線Ｄ、第２の直線Ｂをｙ軸とする。そして、楕円の長径ａ、短径ｂを次の（１５）式〜（１８）式によって求めるものである。
【００６１】
【数１０】
【００６２】
そして、その楕円を、第２のティーチング位置Ｐ２と第３のティーチング位置Ｐ３との間を補間する補間曲線と定め（ステップ１０）、エンドとなる。
【００６３】
このように本実施形態の曲線補間によれば、ティーチング一位置として最も起き易いパターン、つまり直線部分から円弧状部分に入る位置（ティーチング位置Ｐ２）は正確に教示できるが、円弧状部分から直線部分に入る位置は円弧状部分の終了位置から直線部分にやや入った位置（第３のティーチング位置Ｐ３）を教示してしまうパターンについては、ツール先端に作業対象部分の円弧状部分の形状により近い移動軌跡を辿らせることができる。
【００６４】
即ち、前述の特許文献１では、第１のティーチング位置Ｐ１から第２のティーチング位置Ｐ２までは直線移動、第２のティーチング位置Ｐ２から第３のティーチング位置Ｐ３までは曲線移動、第３のティーチング位置Ｐ３から第４のティーチング位置Ｐ４までは直線移動する場合、第２のティーチング位置Ｐ２での移動方向を第１のティーチング位置Ｐ１からの直線移動方向、つまり第１の直線Ａと同じ方向、第３のティーチング位置Ｐ３での移動方向を第４のティーチング位置Ｐ４に向かう直線移動方向、つまり第２の直線Ｂと同じ方向として、第２のティーチング位置Ｐ２と第３のティーチング位置Ｐ３との間は、第２のティーチング位置Ｐ２での移動方向を示す直線（第１の直線Ａ）に接し且つ第２のティーチング位置Ｐ２と第３のティーチング位置Ｐ３とを通る第１の円と、第３のティーチング位置Ｐ３での移動方向を示す直線（第２の直線Ｂ）に接し且つ第２のティー位置Ｐ２と第３のティー位置Ｐ３とを通る第２の円とを演算し、そして、第１の円と第２の円とを結ぶ滑らかな曲線(渡り曲線)を演算し、以上により、第２のティーチング位置Ｐ２と第３のティーチング位置Ｐ３との間を、第１の円から渡り曲線を経て第２の円に至る滑らかな曲線上を補間曲線と定め、ツール先端がこの補間曲線上を移動するようにしている。このため、第２のティーチング位置Ｐ２と第３のティーチング位置Ｐ３の近くでは良いが、それらの間の部分では、ツール先端が、実際の円弧状部分の外縁よりも外側を通る軌跡で移動することとなる。
【００６５】
しかしながら、本実施形態では、第２のティーチング位置Ｐ２が正しく直線部分から円弧状部分に入る位置にティーチングされ、第３のティーチング位置Ｐ３が円弧状部分から直線部分に入ってからの位置にティーチングされたとすると、正しくティーチングされた第２のティーチング位置Ｐ２では円に近い楕円の長径部分が与えられ、円弧状部分から直線部分に入ってからの位置に誤ってティーチングされた第３のティーチング位置Ｐ３では直線に近い楕円の短径部分が与えられるので、ツール先端を本来与えるべき移動軌跡（四分の一円）により近い軌跡に沿い、且つ本来与えるべき移動軌跡よりも内側を移動させることができる。
【００６６】
このため、バリが部分的に残ったり、シール剤が容器と蓋の間に正しく塗られない部分が生じたりするという不具合が生ずることを防止できる。
なお、ｅ２がｅ３に等しい場合には、第２のティーチング位置Ｐ２と第３のティーチング位置Ｐ３との間は円によって曲線補間されるので、ツール先端は円弧状部分の外縁に沿って移動するので良いが、ｅ３の方がｅ２よりも長い場合、例えば第２のティーチング位置Ｐ２が容器の直線部分から円弧状部分に入る位置を正しく与えられ、第３のティーチング位置Ｐ３が円弧状部分の終了位置から直線部分に入る位置よりも手前の円弧状部分に与えられた場合には、ツール先端が円弧状部分の外側を通る軌跡を辿ることになるが、このような教示パターンは殆ど生じないであるので特に問題はない。
【００６７】
なお、本発明は上記し且つ図面に示す実施形態に限定されるものではなく、以下のような拡張或いは変更が可能である。
第１のティーチング位置Ｐ１〜第４のティーチング位置Ｐ４が位置する一平面は必ずしもロボット座標のＸＹ平面或いはＸＹ平面と平行な面である必要ない。ロボット座標のＸＹ平面に対し傾いていても良く、この場合、座標変換の（１）式、（２）式を、３次元を表現する周知の座標変換の式に置き換えれば良い。
ロボットアームの先端に取り付けるエンドエフェクタとしては、バリ取り用カッターに限られない。
【符号の説明】
【００６８】
図面中、２はロボットアーム、３はコントローラ（ロボット制御装置、補間演算手段、補間曲線演算手段）、１１はフランジ（ロボットアーム先端）を示す。
【特許請求の範囲】
【請求項１】
ティーチング時に、ロボットアーム先端に取り付けられたエンドエフェクタのロボット座標上での移動位置をティーチングし、プレイバック時に、前記ティーチング位置の相互間を補間演算し演算された補間位置を前記エンドエフェクタの移動目標位置にして前記ロボットアームを動作させるロボット制御装置において、
移動方向に沿って順にティーチングされた第１〜第４のティーチング位置であって、前記第１のティーチング位置と前記第２のティーチング位置を通る第１の直線と、前記第３のティーチング位置と前記第４のティーチング位置を通る第２の直線が前記第２のティーチング位置と前記第３のティーチング位置との間で交差する位置関係にある前記第１〜第４のティーチング位置を対象に、前記第１のティーチング位置と前記第２のティーチング位置との間を直線補間し、前記第２のティーチング位置と前記第３のティーチング位置との間を曲線補間し、前記第３のティーチング位置と第４のティーチング位置との間を直線補間する補間演算手段を備え、
前記補間演算手段は、
前記第２のティーチング位置を通り前記第１の直線に直交する第３の直線と、前記第３のティーチング位置を通り前記第２の直線に直交する第４の直線とを求める直線設定手段と、
前記第３の直線と前記第４の直線との交点を求め、この交点から前記第２のティーチング位置までの長さと前記第３のティーチング位置までの長さを比較する比較手段と、
この比較手段の比較結果に応じて前記第２のティーチング位置と前記第３のティーチング位置との間の前記補間曲線を設定する補間曲線演算手段と
を備え、
前記補間曲線演算手段は、
前記第３の直線と前記第４の直線との交点から前記第２のティーチング位置までの長さと前記第３のティーチング位置までの長さが同じである場合には、前記第３の直線と前記第４の直線との交点を中心とし、当該中心から前記第２のティーチング位置までの長さを半径として前記第１の直線および第２の直線に接する円を補間曲線とし、
前記第３の直線と前記第４の直線の交点から前記第２のティーチング位置までの長さの方が前記第３のティーチング位置までの長さよりも長い場合には、前記第２のティーチング位置を原点、前記第３の直線をｘ軸、前記第１の直線をｙ軸とする計算用ｘｙ座標を設定し、前記ロボット座標で与えられた前記第３のティーチング位置および第４のティーチング位置を座標変換行列によって前記計算用ｘｙ座標上の位置に変換し、当該計算用ｘｙ座標上での前記第３のティーチング位置の座標値が（ｘ３，ｙ３）、前記第４のティーチング位置の座標値が（ｘ４，ｙ４）であるとき、長径の長さａが下記（i）式、短径の長さｂが下記（ii）式で示され、前記長径が前記ｘ軸上にあって前記原点から前記長径の長さａだけ離れた位置に中心を有して前記第１の直線および第２の直線に接する楕円を求め、前記計算用ｘｙ座標上で求めた前記楕円を同時変換行列によって前記ロボット座標上の楕円に変換して当該楕円を補間曲線とし、
前記第３の直線と前記第４の直線の交点から前記第３のティーチング位置までの長さの方が前記第２のティーチング位置までの長さよりも長い場合には、前記第３のティーチング位置を原点、前記第４の直線をｘ軸、前記第２の直線をｙ軸とする計算用ｘｙ座標を設定し、前記ロボット座標で与えられた前記第２のティーチング位置および第１のティーチング位置を座標変換行列によって前記計算用ｘｙ座標上の位置に変換し、当該計算用ｘｙ座標上での前記第３のティーチング位置の座標値が（ｘ２，ｙ２）、前記第４のティーチング位置の座標値が（ｘ１，ｙ１）であるとき、長径の長さａが下記（iii）式、短径の長さｂが下記（iv）式で示され、前記長径が前記ｘ軸上にあって前記原点から前記長径の長さａだけ離れた位置に中心を有して前記第１の直線および第２の直線に接する楕円を求め、前記計算用ｘｙ座標上で求めた前記楕円を同時変換行列によって前記ロボット座標上の楕円に変換して当該楕円を補間曲線とすることを特徴とするロボットの制御装置。
ａ＝ｘ３／（１＋cos τ３）…（i）
ただし、cos τ３＝（ｘ３ｙ４−ｘ３ｙ３）／（ｘ３ｙ４−ｘ４ｙ３）
ｂ＝ｙ３／sin τ３…（ii）
ただし、sin τ３＝signｙ３（１−cos２τ３）１／２
sign ｙ３は、ｙ３＞のとき１、ｙ３＝０のとき０、ｙ３＜のとき−１
ａ＝ｘ２／（１＋cos τ２）…（iii）
ただし、cos τ２＝（ｘ２ｙ１−ｘ２ｙ２）／（ｘ２ｙ１−ｘ１ｙ２）
ｂ＝ｙ２／sin τ２…（iv）
ただし、sin τ２＝signｙ２（１−cos２ τ２）１／２
Sign ｙ２は、ｙ２＞１のとき１、ｙ２＝０のとき０、ｙ２＜1のとき−１
【請求項１】
ティーチング時に、ロボットアーム先端に取り付けられたエンドエフェクタのロボット座標上での移動位置をティーチングし、プレイバック時に、前記ティーチング位置の相互間を補間演算し演算された補間位置を前記エンドエフェクタの移動目標位置にして前記ロボットアームを動作させるロボット制御装置において、
移動方向に沿って順にティーチングされた第１〜第４のティーチング位置であって、前記第１のティーチング位置と前記第２のティーチング位置を通る第１の直線と、前記第３のティーチング位置と前記第４のティーチング位置を通る第２の直線が前記第２のティーチング位置と前記第３のティーチング位置との間で交差する位置関係にある前記第１〜第４のティーチング位置を対象に、前記第１のティーチング位置と前記第２のティーチング位置との間を直線補間し、前記第２のティーチング位置と前記第３のティーチング位置との間を曲線補間し、前記第３のティーチング位置と第４のティーチング位置との間を直線補間する補間演算手段を備え、
前記補間演算手段は、
前記第２のティーチング位置を通り前記第１の直線に直交する第３の直線と、前記第３のティーチング位置を通り前記第２の直線に直交する第４の直線とを求める直線設定手段と、
前記第３の直線と前記第４の直線との交点を求め、この交点から前記第２のティーチング位置までの長さと前記第３のティーチング位置までの長さを比較する比較手段と、
この比較手段の比較結果に応じて前記第２のティーチング位置と前記第３のティーチング位置との間の前記補間曲線を設定する補間曲線演算手段と
を備え、
前記補間曲線演算手段は、
前記第３の直線と前記第４の直線との交点から前記第２のティーチング位置までの長さと前記第３のティーチング位置までの長さが同じである場合には、前記第３の直線と前記第４の直線との交点を中心とし、当該中心から前記第２のティーチング位置までの長さを半径として前記第１の直線および第２の直線に接する円を補間曲線とし、
前記第３の直線と前記第４の直線の交点から前記第２のティーチング位置までの長さの方が前記第３のティーチング位置までの長さよりも長い場合には、前記第２のティーチング位置を原点、前記第３の直線をｘ軸、前記第１の直線をｙ軸とする計算用ｘｙ座標を設定し、前記ロボット座標で与えられた前記第３のティーチング位置および第４のティーチング位置を座標変換行列によって前記計算用ｘｙ座標上の位置に変換し、当該計算用ｘｙ座標上での前記第３のティーチング位置の座標値が（ｘ３，ｙ３）、前記第４のティーチング位置の座標値が（ｘ４，ｙ４）であるとき、長径の長さａが下記（i）式、短径の長さｂが下記（ii）式で示され、前記長径が前記ｘ軸上にあって前記原点から前記長径の長さａだけ離れた位置に中心を有して前記第１の直線および第２の直線に接する楕円を求め、前記計算用ｘｙ座標上で求めた前記楕円を同時変換行列によって前記ロボット座標上の楕円に変換して当該楕円を補間曲線とし、
前記第３の直線と前記第４の直線の交点から前記第３のティーチング位置までの長さの方が前記第２のティーチング位置までの長さよりも長い場合には、前記第３のティーチング位置を原点、前記第４の直線をｘ軸、前記第２の直線をｙ軸とする計算用ｘｙ座標を設定し、前記ロボット座標で与えられた前記第２のティーチング位置および第１のティーチング位置を座標変換行列によって前記計算用ｘｙ座標上の位置に変換し、当該計算用ｘｙ座標上での前記第３のティーチング位置の座標値が（ｘ２，ｙ２）、前記第４のティーチング位置の座標値が（ｘ１，ｙ１）であるとき、長径の長さａが下記（iii）式、短径の長さｂが下記（iv）式で示され、前記長径が前記ｘ軸上にあって前記原点から前記長径の長さａだけ離れた位置に中心を有して前記第１の直線および第２の直線に接する楕円を求め、前記計算用ｘｙ座標上で求めた前記楕円を同時変換行列によって前記ロボット座標上の楕円に変換して当該楕円を補間曲線とすることを特徴とするロボットの制御装置。
ａ＝ｘ３／（１＋cos τ３）…（i）
ただし、cos τ３＝（ｘ３ｙ４−ｘ３ｙ３）／（ｘ３ｙ４−ｘ４ｙ３）
ｂ＝ｙ３／sin τ３…（ii）
ただし、sin τ３＝signｙ３（１−cos２τ３）１／２
sign ｙ３は、ｙ３＞のとき１、ｙ３＝０のとき０、ｙ３＜のとき−１
ａ＝ｘ２／（１＋cos τ２）…（iii）
ただし、cos τ２＝（ｘ２ｙ１−ｘ２ｙ２）／（ｘ２ｙ１−ｘ１ｙ２）
ｂ＝ｙ２／sin τ２…（iv）
ただし、sin τ２＝signｙ２（１−cos２ τ２）１／２
Sign ｙ２は、ｙ２＞１のとき１、ｙ２＝０のとき０、ｙ２＜1のとき−１
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図２】
【図３】
【図４】
【図５】
【図６】
【公開番号】特開２０１２−１９８６００（Ｐ２０１２−１９８６００Ａ）
【公開日】平成２４年１０月１８日（２０１２．１０．１８）
【国際特許分類】
【出願番号】特願２０１１−６０６２０（Ｐ２０１１−６０６２０）
【出願日】平成２３年３月１８日（２０１１．３．１８）
【出願人】（５０１４２８５４５）株式会社デンソーウェーブ (1,155)
【Ｆターム（参考）】
【公開日】平成２４年１０月１８日（２０１２．１０．１８）
【国際特許分類】
【出願日】平成２３年３月１８日（２０１１．３．１８）
【出願人】（５０１４２８５４５）株式会社デンソーウェーブ (1,155)
【Ｆターム（参考）】
[ Back to top ]