(57)【要約】【課題】補間による制御を行う場合において、教示点の数が少なくとも経由点での姿勢を教示されて姿勢に近づけることができるロボットの制御方法、ロボットの制御装置を提供する。【解決手段】始点から終点までの姿勢変化を表すオイラーベクトルＶ１と始点から経由点までの姿勢変化を表すオイラーベクトルＶ２とを求め、Ｖ１とＶ２との内積の正負を判定し、ロボットを回転駆動する際の回転方向を、内積が負であれば姿勢の変化量が相対的に大きくなる回転方向（遠回り方向）に設定し、内積が正であれば、Ｖ１の回転量成分θαとＶ２の回転量成分θβとの大小関係を比較し、ロボットが回転駆動する際の回転方向を、θαがθβよりも大きければ姿勢の変化量が相対的に小さくなる回転方向（近回り方法）に設定する一方、θαがθβよりも小さければ姿勢の変化量が相対的に大きくなる回転方向（遠回り方向）に設定する。【選択図】図５
2015182147.tif 000002
【特許請求の範囲】
【請求項１】
教示された始点、経由点および終点の３点の教示点間を曲線で補間して多関節型のロボットの動作軌跡を求める際に、
始点から終点までの姿勢変化を表すオイラーベクトルＶ１と、始点から経由点までの姿勢変化を表すオイラーベクトルＶ２とを求め、
オイラーベクトルＶ１とオイラーベクトルＶ２との内積を求めて内積の正負を判定し、
オイラーベクトルＶ１とオイラーベクトルＶ２との内積が負であれば、始点から終点まで姿勢を変化させた際の姿勢の変化量が相対的に大きくなる回転方向を、ロボットが回転駆動する際の回転方向に設定し、
オイラーベクトルＶ１とオイラーベクトルＶ２との内積が正であれば、オイラーベクトルＶ１の回転量成分θαとオイラーベクトルＶ２の回転量成分θβとの大小関係を比較し、
回転量成分θαが回転量成分θβよりも大きければ、始点から終点まで姿勢を変化させた際の姿勢の変化量が相対的に小さくなる回転方向を、ロボットが回転駆動する際の回転方向に設定する一方、回転量成分θαが回転量成分θβよりも小さければ、始点から終点まで姿勢を変化させた際の姿勢の変化量が相対的に大きくなる回転方向を、ロボットが回転駆動する際の回転方向に設定することを特徴とするロボットの制御方法。
【請求項２】
手先軸の座標系を対象として回転方向の判定を行うことを特徴とする請求項１記載のロボットの制御方法。
【請求項３】
多関節型のロボットを制御するロボットの制御装置であって、
始点から終点までの姿勢変化を表すオイラーベクトルＶ１と、始点から経由点までの姿勢変化を表すオイラーベクトルＶ２とを求めるベクトル取得手段と、
オイラーベクトルＶ１とオイラーベクトルＶ２との内積を求めて内積の正負を判定する判定手段と、
オイラーベクトルＶ１の回転量成分θαとオイラーベクトルＶ２の回転量成分θβとの大小関係を比較する比較手段と、
前記判定手段の判定結果および前記比較手段の比較結果に基づいて、オイラーベクトルＶ１とオイラーベクトルＶ２との内積が負であれば、始点から終点まで姿勢を変化させた際の変化量が相対的に大きくなる回転方向を、ロボットを回転駆動する際の回転方向に設定し、オイラーベクトルＶ１とオイラーベクトルＶ２との内積が正であれば、オイラーベクトルＶ１の回転量成分θαとオイラーベクトルＶ２の回転量成分θβとの大小関係を比較し、回転量成分θαが回転量成分θβよりも大きければ、始点から終点までの姿勢の変化量が相対的に小さくなる回転方向をロボットが回転駆動する際の回転方向に設定する一方、回転量成分θαが回転量成分θβよりも小さければ、始点から終点までの姿勢の変化量が相対的に大きくなる回転方向をロボットが回転駆動する際の回転方向に設定する設定手段と、
を備えることを特徴とするロボットの制御装置。
【請求項４】
前記ベクトル取得手段は、手先軸に対応する座標系における姿勢行列を対象としてオイラーベクトルを求めることを特徴とする請求項３記載のロボットの制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、多関節型のロボットを制御するためのロボットの制御方法、ロボットの制御装置に関する。
【背景技術】
【０００２】
多関節型のロボットにおいて、ユーザにより教示された複数の教示点間を曲線で補間し、その曲線に沿ってロボットを回転駆動する制御が行われている。具体的には、例えば始点、経由点および終点が教示された場合にそれら３つの教示点間を曲線で補間する制御が行われている。このような制御を可能とすることで、小数の教示点を設定するだけでロボットの動作軌跡全体を設定できるというメリットを受けることができる。
【０００３】
さて、産業用途においては、ロボットは、一般的に繰り返し作業を行うために設けられている。その場合、繰り返し作業一回当たりの動作時間を短縮することができれば、生産性を向上させることができる。そのため、例えば特許文献１のものは、始点から終点までロボットを回転駆動させる際の所要時間が最短となるように制御を行っている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平１１−１９８０７２号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
ところで、ロボットは、例えばフランジ構造の手先を回転駆動するとき、その回転方向を２つ設定することができる。具体的には、現在位置から例えば＋９０°となる回転位置まで回転駆動する際には、現在位置から＋９０°回転駆動する回転方向と、現在位置から−２７０°回転駆動する回転方向とが存在している。そして、いずれの回転方向で回転駆動されたとしても、最終的には、手先は現在位置から＋９０°の回転位置まで回転駆動されることになる。
【０００６】
しかしながら、従来では、上記したように所要時間を短縮するために、始点から終点までの姿勢の変化量が少なくなる回転方向が選択されていた。つまり、従来では、始点、経由点および終点の３点が教示された場合であっても、経由点での姿勢を考慮せずに、始点での姿勢から終点での姿勢まで回転駆動する際の回転量が小さくなる回転方向が選択されていた。その結果、１８０°を超えるような回転を行うことができず、実際に回転駆動されたときの経由点での姿勢が、教示された姿勢から大きくずれてしまうことがあった。
【０００７】
この場合、教示点の数を増やせば姿勢を細かく制御することが可能であるものの、そのようにしてしまうと、小数の教示点を設定するだけでロボットの動作軌跡全体を設定できるという補間による制御を行うメリットがなくなってしまう。
本発明は、上記事情に鑑みてなされたものであり、その目的は、補間による制御を行う場合において、教示点の数が少なくとも経由点での姿勢を教示されて姿勢に近づけることができるロボットの制御方法、ロボットの制御装置を提供することにある。
【課題を解決するための手段】
【０００８】
請求項１記載のロボットの制御方法の発明では、教示された始点、経由点および終点の３点の教示点間を曲線で補間して多関節型のロボットの動作軌跡を求める際に、始点から終点までの姿勢変化を表すオイラーベクトルＶ１と、始点から経由点までの姿勢変化を表すオイラーベクトルＶ２とを求め、オイラーベクトルＶ１とオイラーベクトルＶ２との内積を求めて内積の正負を判定する。
【０００９】
オイラーベクトルは、回転軸となるベクトル成分と、その回転軸周りの回転量成分とにより表される。このとき、２つのオイラーベクトルの内積が正であるか負であるかは、オイラーベクトルによって示された回転軸の向きが一致しているか否か、すなわち、回転量成分だけ回転させる際の回転方向が一致するか否かを示している。つまり、内積の正負によって、姿勢を変化させた際の回転方向が一致するか否かを判定することができる。
【００１０】
そして、上記したオイラーベクトルＶ１とオイラーベクトルＶ２の場合では、内積が負となるのは、始点から終点まで最短で回転駆動する場合の回転方向と、始点から経由点まで最短で回転駆動する場合の回転方向とが一致しない場合である。一方、内積が正となるのは、始点から終点まで最短で回転駆動する場合の回転方向と、始点から経由点まで最短で回転駆動する場合の回転方向とが一致する場合である。
【００１１】
そのため、オイラーベクトルＶ１とオイラーベクトルＶ２との内積が負であれば、始点から終点まで最短で回転させた場合の回転方向と始点から経由点まで最短で回転駆動する場合の回転方向とが一致していないことから、始点から終点までを最短動作させた場合に経由点の姿勢を経由しない状態、すなわち、始点から終点までの動作中の任意の場所でも経由点で教示した姿勢に近い姿勢が出現しない状態となるので、始点から終点まで姿勢を変化させた際の姿勢の変化量が相対的に大きくなる回転方向、つまり、始点から終点まで最短で回転駆動する場合とは逆向きの回転方向を、実際にロボットを回転駆動する際の回転方向に設定する。逆向きの回転方向ということは、そもそも姿勢変化というのが３６０°の値で表現されることから、始点から終点までを最短動作させた場合に経由点の姿勢を経由しない（つまり、教示された姿勢に近い姿勢が出現しない）ことを考えれば、逆に始点から終点までの動作の中に経由点の姿勢に近い姿勢を含むということである。これにより、実際に回転駆動した際の経由点での姿勢を教示された経由点での姿勢に近づけることができる。そして、遠回りの回転方向が選択可能であるため、１８０°を超えるような回転が可能となる。したがって、補間による制御を行う場合において経由点での姿勢が教示された姿勢から大きくずれることを抑制できる。
【００１２】
さて、オイラーベクトルＶ１とオイラーベクトルＶ２との内積が正であれば、始点から終点まで最短で回転駆動する場合の回転方向と始点から経由点まで最短で回転駆動する場合の回転方向とが一致していることになる。ただし、内積が正になったとしても、必ずしも始点から終点までを最短動作させた場合に経由点の姿勢を経由する（つまり、教示された姿勢に近い姿勢が出現する）とは限らない。これは、経由点とは終点までに経由すべき点として教示される点であるものの、始点での姿勢から経由点での姿勢までの回転量成分が、始点での姿勢から終点での姿勢までの回転量成分よりも大きく変化していることもあり得るためである。
【００１３】
そこで、内積が正であった場合には、オイラーベクトルＶ１の回転量成分θαとオイラーベクトルＶ２の回転量成分θβとの大小関係を比較する。
このとき、回転量成分θαが回転量成分θβよりも大きければ、始点から終点まで姿勢を変化させた際の姿勢の変化量が相対的に小さくなる回転方向、つまり、始点から終点まで最短で回転駆動する場合と同じ回転方向を、実際にロボットを回転駆動する際の回転方向に設定する。これは、平易に言えば、始点から終点まで最短で姿勢を変化させる回転方向に回転駆動すれば、その途中で経由点での姿勢を取ることができるためである。
【００１４】
一方、回転量成分θαが回転量成分θβよりも小さければ、始点から終点まで姿勢を変化させた際の姿勢の変化量が相対的に大きくなる回転方向、つまり、始点から終点まで最短で回転駆動する場合とは逆向きの回転方向を、実際にロボットを回転駆動する際の回転方向に設定する。これは、平易に言えば、始点から終点まで最短で姿勢を変化させる回転方向に回転駆動すると、その途中では経由点での姿勢を取ることができないためである。
【００１５】
このように、オイラーベクトルの内積および回転量成分の大小関係に基づいてロボットを回転駆動する際の回転方向を設定することにより、遠回りの回転方向が選択肢に組み込まれ、１８０°を超えるような回転が可能となる。そして、経由点での姿勢に近づける方向に回転方向を設定するので、教示された経由点での姿勢に近くなり、補間による制御を行う場合において、経由点での姿勢を教示された姿勢に近づけることができる。つまり、経由点での姿勢が教示された姿勢から大きくずれることを抑制することができる。
【００１６】
この場合、回転量成分のスカラー値を直接用いて大小関係を判定しているので、処理を簡略化することができ、判定を短期間で行うことができる。
また、教示点の数を増やすことなく経由点での姿勢を教示された姿勢に近づけることができるため、小数の教示点を設定するだけでロボットの動作軌跡全体を設定できるというメリットが損なわれることもない。
【００１７】
請求項２記載のロボットの制御方法の発明では、手先軸の座標系を対象として回転方向の判定を行う。手先軸の座標系は、手先つまりツールの向きを示すものである。そこで、手先軸の座標系を対象として回転方向を判定すれば、手先軸の向きを教示された経由点での向きに近づけることができる。すなわち、実際の動作軌跡における経由点での姿勢を、教示された姿勢つまりはユーザが所望する姿勢に近づけることができる。
【００１８】
請求項３記載のロボットの制御装置の発明は、オイラーベクトルを求めるベクトル取得手段と、オイラーベクトルの内積を求めて内積の正負を判定する判定手段と、オイラーベクトルの回転量成分の大小関係を比較する比較手段と、判定手段の判定結果および比較手段の比較結果に基づいてロボットを回転駆動する際の回転方向を設定する設定手段と、を備える。これにより、上記した請求項１記載のロボットの制御方法を採用したロボットの制御装置を実現できる。そして、上記した請求項１記載のロボットの制御方法で述べたような効果を得ることができる。
【００１９】
請求項４記載のロボットの制御装置の発明では、ベクトル取得手段は、手先軸に対応する座標系における姿勢行列を対象としてオイラーベクトルを求める。つまり、手先軸を対象として回転方向の判定を行う。これにより、上記した請求項２記載のロボットの制御方法で述べたような効果を得ることができる。
【図面の簡単な説明】
【００２０】
【図１】本発明の一実施形態によるロボットの構成を概略的に示す図
【図２】従来の補間制御におけるロボットの姿勢の変化を模式的に示す図
【図３】ロボットの回転方向を模式的に示す図
【図４】姿勢の異なる２つの座標系における各ベクトルを模式的に示す図
【図５】制御装置による回転方向設定処理の流れを示す図
【図６】経由点姿勢とオイラーベクトルとの関係を模式的に示す図
【図７】経由点および終点に対する回転量成分の大小関係を模式的に示す図
【図８】回転方向設定処理を行った場合の経由点姿勢を模式的に示す図
【発明を実施するための形態】
【００２１】
以下、本発明の一実施形態について、図１から図８を参照しながら説明する。
図１に示すように、ロボットシステム１は、多関節型のロボット２、ロボット２を制御するコントローラ３、コントローラ３に接続されたティーチングペンダント４を備えている。このロボットシステム１は、一般的な産業用に用いられている。
【００２２】
ロボット２は、いわゆる６軸の垂直多関節型ロボットとして周知の構成を備えており、ベース５上に、Ｚ方向の軸心を持つ第１軸（Ｊ１）を介してショルダ６が水平方向に回転可能に連結されている。ショルダ６には、Ｙ方向の軸心を持つ第２軸（Ｊ２）を介して上方に延びる下アーム７の下端部が垂直方向に回転可能に連結されている。下アーム７の先端部には、Ｙ方向の軸心を持つ第３軸（Ｊ３）を介して第一上アーム８が垂直方向に回転可能に連結されている。第一上アーム８の先端部には、Ｘ方向の軸心を持つ第４軸（Ｊ４）を介して第二上アーム９が捻り回転可能に連結されている。第二上アーム９の先端部には、Ｙ方向の軸心を持つ第５軸（Ｊ５）を介して手首１０が垂直方向に回転可能に連結されている。手首１０には、Ｘ方向の軸心を持つ第６軸（Ｊ６）を介してフランジ１１が捻り回転可能に連結されている。以下、第６軸を、便宜的に手先軸とも称する
【００２３】
ベース５、ショルダ６、下アーム７、第一上アーム８、第二上アーム９、手首１０およびフランジ１１は、ロボット２のアームとして機能し、アームの先端となるフランジ１１には、図示は省略するが、ハンド（エンドエフェクタとも呼ばれる）が取り付けられる。ハンドは、例えば図示しないワークを保持して移送したり、ワークを加工する工具等が取り付けられたりする。ロボット２に設けられている各軸（Ｊ１〜Ｊ６）には、それぞれに対応して駆動源となるモータ（図示省略）が設けられている。
コントローラ３は、ロボットの制御装置に相当し、図示しないＣＰＵ、ＲＯＭおよびＲＡＭ等で構成されたコンピュータからなる制御部においてコンピュータプログラムを実行することで、ロボット２を制御している。具体的には、コントローラ３は、インバータ回路等から構成された駆動部を備えており、各モータに対応して設けられているエンコーダで検知したモータの回転位置に基づいて例えばフィードバック制御によりモータを駆動する。
【００２４】
また、コントローラ３は、詳細は後述するが、姿勢変化を表すオイラーベクトルを求めるベクトル取得手段、オイラーベクトルの内積を求めて内積の正負を判定する判定手段、オイラーベクトルの回転量成分の大小関係を比較する比較手段、および、判定手段の判定結果および比較手段の比較結果に基づいてロボット２を回転駆動する際の回転方向を設定する設定手段を構成している。
ティーチングペンダント４は、例えば概ね略矩形箱状に形成されており、ユーザが所持したまま操作可能な程度の大きさに形成されている。このティーチングペンダント４には、各種キースイッチやタッチパネル等が設けられており、ユーザは、それらキースイッチやタッチパネル等を用いてティーチングを行う。
【００２５】
このような構成のロボットシステム１では、ロボット２を制御する際の基準となる座標系が設定されている。本実施形態の場合、ベース５に対応する座標系が、基準座標系ΣＢとなる。この基準座標系ΣＢは、ロボット２がどのような姿勢を取ったとしても変化することがない座標系である。この基準座標系ΣＢには、互いに直交するＸＢ軸、ＹＢ軸およびＺＢ軸が設定されている。なお、ＺＢ軸は設置面に垂直な軸となっている。
また、ロボット２では、手先軸（Ｊ６）に対応する座標系ΣＦが設定されている。この座標系ΣＦは、フランジ１１の向きを手先軸の原点を基準として示す座標系であり、互いに直交するＸＦ軸、ＹＦ軸およびＺＦ軸が設定されている。このうち、ＺＦ軸は、手先軸と同軸に設定されている。つまり、ＺＦ軸の向きが、フランジ１１の向き、つまり、手先の向きを示すことになる。
【００２６】
また、ロボット２の姿勢を表す際には、基準座標系ΣＢからみた手先の向きを表すベクトルが用いられる。具体的には、座標系ΣＦのＸＦ軸方向を表す３次元ベクトルであるノーマルベクトル（Ｎ）、基準座標系ΣＢからみた座標系ΣＦのＹＦ軸方向を表す３次元ベクトルであるオリエントベクトル（Ｏ）、および、基準座標系ΣＢからみた座標系ΣＦのＺＦ軸方向を表す３次元ベクトルであるアプローチベクトル（Ａ）が制御に用いられる。
これら各ベクトルは、以下の（１）式〜（３）式にて表される。
【００２７】
【数１】
2015182147.tif 000003
【００２８】
上記した（１）式〜（３）式に付されているＴは、以下の（４）式に示すように、基準座標系ΣＢから座標系ΣＦへの座標変換を行う４行４列の行列であり、同次変換行列と称される。この同次変換行列は、ロボット２の手先の位置と姿勢を表すために用いられる。
【００２９】
【数２】
2015182147.tif 000004
【００３０】
なお、（４）式に示されているＲは、ノーマルベクトル、オリエントベクトルおよびアプローチベクトルを纏めた３行３列の行列であり、Ｒ＝（Ｎ  Ｏ  Ａ）として表される。また、（４）式に示されているＰは、座標系ΣＦの原点の位置を基準座標系ΣＢで表した３次元ベクトルである。
ここで、始点、経由点および終点の３点が教示点として示された場合における従来の補間による制御について、図２を参照しながら説明する。以下、複数の教示点間を曲線にて補間する制御を、便宜的に補間制御と称する。なお、この補間制御は、一般的には円弧制御とも称されるが、必ずしも円弧に限らず、スプライン曲線等で補間されることもある。
【００３１】
例えば図２（ａ）に示すように、互いに異なり、且つ、直線上にない始点（Ｋ１）、経由点（Ｋ２）および終点（Ｋ３）の３点が、ユーザにより教示点として設定されたとする。このとき、各教示点での姿勢は、図２（ｂ）に示したようになっているものとする。なお、図２（ｂ）では、説明の簡略化のために、Ｋ１〜Ｋ３におけるアプローチベクトル（Ａ１〜Ａ３）にのみ符号を付している。以下、便宜的に、始点での姿勢を始点姿勢と称し、終点での姿勢を終点姿勢と称し、経由点での姿勢を経由点姿勢と称する。
【００３２】
このとき、従来の補間制御では、図２（ｃ）に示すように、経由点姿勢を考慮せずに、始点姿勢から終点姿勢となるまでの時間が最短となるようにロボット２を回転駆動していた。その結果、図２（ｄ）に示すように、回転駆動される際の経由点のアプローチベクトル（Ａ４）が、図２（ａ）に示した教示された経由点のアプローチベクトル（Ａ２）と一致しない状態、つまり、回転駆動した際の経由点姿勢が教示された姿勢から大きくずれた状態となることがあった。
【００３３】
これは、ロボット２を回転駆動する際には、その回転方向が２つ存在するためである。例えば、図３（ａ）に示すように、手先軸のアプローチベクトルがＡ１である始点姿勢から、アプローチベクトルがＡ３である終点姿勢となるように回転駆動する場合を考えてみる。この場合、始点から終点まで回転駆動する回転方向としては、相対的に回転量が少なく、回転駆動に要する時間が最短時間となる姿勢回転方向Ｄ１（以下、近回り方向と称する）と、相対的に回転量が多く始点から終点まで回転駆動するのに要する最短時間とはならない姿勢回転方向Ｄ２（以下、遠回り方向と称する）とが存在する。なお、図３では、説明の簡略化のために、アプローチベクトルＡ１、Ａ３の基点を重ねている。
【００３４】
このとき、従来では、始点姿勢と終点姿勢とだけを考慮し、始点から終点まで回転駆動させたときの時間が最短時間となるように、近回り方向が選択されていた。その結果、図３（ｂ）に示すように、実際に回転駆動されたときの経由点でのアプローチベクトル（Ａ４）が、教示された経由点でのアプローチベクトル（Ａ２）に対して例えば逆向きになる等、経由点での姿勢が大きくずれるおそれがあった。そして、ロボット２の手先にはツールが取り付けられることから、手先の向きがずれてしまうと、ツールの向きもずれてしまう。その結果、ワークが意図しない形状に加工されたりするおそれがあった。
【００３５】
この場合、教示点の数を増やし、ロボット２の軌跡を細かく制御することで、上記したずれが生じることを防止することができると考えられる。しかし、そのような対処方法は、少ない教示点を設定することで軌跡全体を設定できるという補間制御の意義が薄れてしまう。
そこで、本実施形態では、補間制御を行う場合において、実際に回転駆動されたときの経由点姿勢とユーザにより教示された経由点姿勢とが大きくずれることを抑制している。
まず、姿勢行列を用いて２つの座標間の姿勢変化を表す手法について、図４を参照しながら説明する。図４に示すように、基準座標系ΣＢに任意の２点が存在し、一方が座標系ΣＦ１で表される例えば始点であり、他方が座標系ΣＦ２で表される例えば終点であるとする。このとき、始点からみた終点の姿勢行列をＦ１ＲＦ２とすると、姿勢行列Ｆ１ＲＦ２は、以下の（５）式で表すことができる。
【００３６】
【数３】
2015182147.tif 000005
【００３７】
ただし、（５）式において、ＢＲＦ１は、基準座標系ΣＢから座標系ΣＦ１への同次変換行列ＢＴＦ１に含まれる姿勢行列であり、ＢＲＦ２は、基準座標系ΣＢから座標系ΣＦ２への同次変換行列ＢＴＦ２に含まれる姿勢行列である。
そして、座標系ΣＦ１から座標系ΣＦ２への姿勢の変化量ΔＲは、座標系ΣＦ１から座標系ΣＦ２への姿勢行列Ｆ１ＲＦ２と同義であるので、上記した（５）式から、姿勢の変化量ΔＲは、以下の（６）式にて表される。
【００３８】
【数４】
2015182147.tif 000006
【００３９】
さて、本実施形態では、後述するように、回転駆動する際の回転方向を、オイラーベクトル表現した際の回転量成分θにより判定する。オイラーベクトルは、周知のように、オイラーベクトルはある姿勢行列Ｒを、回転軸となる以下の（７）式で示される単位ベクトルのベクトル成分Ｖ（ｖｘ，ｖｙ，ｖｚ）と、回転量成分θとで表現する手法である。
【００４０】
【数５】
2015182147.tif 000007
【００４１】
そして、上記した（６）式をオーラ−ベクトルの公式に適用することで、以下の（８）式および（９）式に示すように、座標系ΣＦ１と座標系ΣＦ２との間の姿勢の変化量（ΔＲ）を、ベクトル成分と回転量成分とで表現することができる。
【００４２】
【数６】
2015182147.tif 000008
【００４３】
これら姿勢変化をオイラーベクトル表現するための処理は、コントローラ３によって行われている。そして、コントローラ３は、姿勢変化をオイラーベクトル表現した際のベクトル成分（ｖ）と回転量成分（θ）とに基づいて、補間制御を行う際の回転駆動の回転方向を上記した近回り方向とするか遠回り方向とするかを判定する。より具体的には、本実施形態の場合、コントローラ３は、図２に示したような互いに異なり、且つ、直線上にない始点（Ｋ１）、経由点（Ｋ２）および終点（Ｋ３）の３点が教示点として設定されたとき、図５に示す回転方向設定処理を実行することで回転方向を判定する。
【００４４】
本実施形態の場合、コントローラ３は、手先軸の座標系を対象として回転方向設定処理を実行する。これは、手先軸の座標系は手先つまりツールの向きを示すものであり、ワークを加工する際に重視されると考えられるためである。
この回転方向設定処理において、コントローラ３は、まず教示された始点、経由点および終点での姿勢を取得し、姿勢行列に基づいて、始点姿勢から終点姿勢への変化をオイラーベクトル表現した際のベクトルＶ１と、始点姿勢から経由点姿勢への変化をオイラーベクトル表現した際のベクトルＶ２とを求める。つまり、コントローラ３は、姿勢変化を表すオイラーベクトルを求めるベクトル取得手段として機能する。
【００４５】
オイラーベクトルを求めると、コントローラ３は、それらの内積（Ｖ１・Ｖ２）が負であるかを判定する（Ｓ１）。このステップＳ１では、始点から終点まで回転駆動する際の回転方向と、始点から経由点まで回転駆動する際の回転方向とが一致するか否かが判定されている。つまり、コントローラ３は、オイラーベクトルの内積を求めて内積の正負を判定する判定手段として機能する。
【００４６】
例えば、図６（ａ）に模式的に示すように、始点から終点まで最短で、つまり、回転量が少ない方向に回転駆動する際の回転方向Ｄ３と、始点から経由点まで最短で回転駆動する際の回転方向Ｄ４とが一致すれば、内積（Ｖ１・Ｖ２）は正となる。一方、図６（ｂ）に模式的に示すように、始点から終点まで回転駆動する際の回転方向Ｄ３と、始点から経由点まで回転駆動する際の回転方向Ｄ４とが一致していなければ、内積（Ｖ１・Ｖ２）は負となる。つまり、内積（Ｖ１・Ｖ２）の正負によって、経由点姿勢に近づけるための回転方向を判断することができる。
【００４７】
そのため、コントローラ３は、内積（Ｖ１・Ｖ２）が負である場合には（Ｓ１：ＹＥＳ）、姿勢回転方向を、遠回り方向つまり終点まで最短となる回転方向とは逆向きの回転方向に設定する（Ｓ４）。これにより、１８０°を超えるような回転方向を選択できるようになり、実際に回転駆動した際における経由点姿勢を、教示された経由点姿勢に近づけることができる。
【００４８】
これに対して、コントローラ３は、内積（Ｖ１・Ｖ２）が負でない場合には（Ｓ１：ＮＯ）、回転量θに基づいて回転方向を判定する。具体的には、コントローラ３は、始点姿勢から終点姿勢への変化をオイラーベクトル表現した際の回転量成分θαと、始点姿勢から経由点姿勢への変化をオイラーベクトル表現した際の回転量成分θβとを求め、その大小関係に基づいて回転方向を判定する。つまり、コントローラ３は、オイラーベクトルＶ１の回転量成分θαとオイラーベクトルＶ２の回転量成分θβとの大小関係を比較する比較手段として機能する。
【００４９】
コントローラ３は、図７（ａ）に示すように回転量成分θαが回転量成分θβよりも大きい場合には（Ｓ２：ＮＯ）、姿勢回転方向を、近回り方向つまり終点まで最短となる回転方向と同じ向き回転方向に設定する（Ｓ３）。この状態は、より平易に言えば、始点姿勢から終点姿勢まで回転駆動する途中で、経由点姿勢と同じあるいは近似する姿勢を取ることができる状態である。そのため、回転方向を近回り方向に設定することで、実際に回転駆動する際の経由点姿勢を教示された経由点姿勢に近づけることができる。
【００５０】
一方、コントローラ３は、図７（ｂ）に示すように回転量成分θαが回転量成分θβよりも小さい場合には（Ｓ２：ＹＥＳ）、姿勢回転方向を、遠回り方向に設定する（Ｓ４）。この場合、始点姿勢から終点姿勢まで最短で回転駆動すると、その途中において経由点姿勢と同じあるいは近似する姿勢を取ることができない。そのため、コントローラ３は、回転方向に遠回り方向を設定する。これにより、実際に回転駆動した際における経由点姿勢を、教示された経由点姿勢に近づけることができる。
【００５１】
このように、コントローラ３は、オイラーベクトルの内積および回転量成分の大小関係に基づいて、ロボット２を回転駆動する際の回転方向を設定する設定手段として機能する。
【００５２】
いずれかの回転方向を設定すると、コントローラ３は、回転方向設定処理を終了する。その後、教示点間を補間して動作軌跡を設定し、その動作軌跡に沿うようにロボット２を制御する。
これにより、例えば図８（ａ）に示すような教示点つまり姿勢がユーザにより教示されたとき、回転方向設定処理によって１８０°を超える回転方向を選択できるようになり、教示された経由点姿勢に近づく回転方向が選択されて、例えば図８（ｂ）に示すように教示されたアプローチベクトル（Ａ２）と実際に回転駆動する際のアプローチベクトル（Ａ４）が一致する等、実際の軌道における経由点姿勢を教示された経由点姿勢と同じあるいは近似した姿勢にすることができる。
【００５３】
以上説明した本実施形態によれば、次のような効果を得ることができる。
実施形態の制御方法では、始点から終点までの姿勢変化を表すオイラーベクトルＶ１と、始点から経由点までの姿勢変化を表すオイラーベクトルＶ２とを求め、オイラーベクトルＶ１とオイラーベクトルＶ２との内積を求め、まず、姿勢を変化させた際の回転方向が一致するか否かを判定する。そして、オイラーベクトルＶ１とオイラーベクトルＶ２との内積が負であれば、始点から終点まで最短で回転させた場合の回転方向と始点から経由点まで最短で回転させた場合の回転方向とが一致していないことから、始点から終点まで最短で回転させた場合とは逆向きの回転方向（遠回り方向）を、実際にロボットを回転駆動する際の回転方向に設定する。これにより、実際に回転駆動した際の経由点での姿勢を教示された経由点での姿勢に近づけることができる。そして、遠回りの回転方向が選択された場合には、１８０°を超えるような回転が可能となる。したがって、補間による制御を行う場合において経由点での姿勢が教示された姿勢から大きくずれることを抑制できる。
【００５４】
一方、オイラーベクトルＶ１とオイラーベクトルＶ２との内積が正であれば、始点から終点まで最短で回転させた場合の回転方向と始点から経由点まで最短で回転させた場合の回転方向とが一致することになるため、オイラーベクトルＶ１の回転量成分θαとオイラーベクトルＶ２の回転量成分θβとの大小関係を比較する。そして、回転量成分θαが回転量成分θβよりも大きければ、始点から終点まで最短で回転させた場合と同じ回転方向（近回り方向）を、実際にロボットを回転駆動する際の回転方向に設定する。また、回転量成分θαが回転量成分θβよりも小さければ、始点から終点まで最短で回転させた場合とは逆向きの回転方向（遠回り方向）を、実際にロボットを回転駆動する際の回転方向に設定する。
【００５５】
このように、オイラーベクトルの内積および回転量成分の大小関係に基づいてロボットを回転駆動する際の回転方向を経由点姿勢に近づける方向に設定することで、遠回りの回転方向が選択肢に組み込まれ、１８０°を超えるような回転が可能となる。そして、実際に回転駆動した際の経由点での姿勢を教示された経由点での姿勢に近づけることができる。したがって、補間による制御を行う場合において経由点での姿勢が教示された姿勢から大きくずれることを抑制できる。
【００５６】
また、回転量成分のスカラー値を直接用いて大小関係を判定している。これは、オイラーベクトル表現とすれば、回転量成分が負の値を取ることはないためである。そのため、絶対値を取る等の処理が不要となり、純粋に回転量成分のスカラー値の大小関係で判定を行うことができ、処理を簡略化することができる。そして、処理が簡略化されることで、判定を短期間で行うことができる。
【００５７】
また、教示点の数を増やすことなく経由点での姿勢を教示された姿勢に近づけることができるため、小数の教示点を設定するだけでロボットの動作軌跡全体を設定できるというメリットを享受することができる。つまり、ユーザの負担を増やすことなく、補間制御のためのティーチングを行うことができる。
また、本実施形態では、手先軸に対応する座標系を対象として回転方向の判定を行っている。このため、手先軸の向きが教示された経由点での手先軸の向きに近づくように回転方向を設定することで、教示された姿勢に近づけることができる。すなわち、実際の動作軌跡における経由点での姿勢を、よりユーザが希望する姿勢に近づけることができる。
【００５８】
実施形態のコントローラ３は、オイラーベクトルを求めるベクトル取得手段と、オイラーベクトルの内積を求めて内積の正負を判定する判定手段と、オイラーベクトルの回転量成分の大小関係を比較する比較手段と、判定手段の判定結果および比較手段の比較結果に基づいてロボットを回転駆動する際の回転方向を設定する設定手段として機能する。これにより、上記した制御方法を採用したロボットの制御装置を実現でき、実際にロボット２を制御する際に１８０°を超える回転方向を選択でき、教示された姿勢に近づけることができる等、上記した制御方法で述べたような効果を得ることができる。
る。つまり、上記した制御方法で得られる効果を得ることができる。
【００５９】
また、コントローラ３は、手先軸の座標系を対象として回転方向の判定を行っているため、上記したように、手先の姿勢を教示された姿勢に近づけることができる。
なお、本発明は一実施形態に限定されるものではなく、その要旨を逸脱しない範囲で任意に変形または拡張することができる。
【符号の説明】
【００６０】
図面中、２はロボット、３はコントローラ（ロボットの制御装置、ベクトル取得手段、判定手段、比較手段、設定手段）を示す。
【図１】
2015182147.tif 000009
【図２】
2015182147.tif 000010
【図３】
2015182147.tif 000011
【図４】
2015182147.tif 000012
【図５】
2015182147.tif 000013
【図６】
2015182147.tif 000014
【図７】
2015182147.tif 000015
【図８】
2015182147.tif 000016