JP6252273B2 - ロボットの制御方法、ロボットの制御装置 - Google Patents

Description

本発明は、多関節型のロボットを制御するためのロボットの制御方法、ロボットの制御装置に関する。

多関節型のロボットにおいて、ユーザにより教示された複数の教示点間を曲線で補間し、その曲線に沿ってロボットを回転駆動する制御が行われている。具体的には、例えば始点、経由点および終点が教示された場合にそれら３つの教示点間を曲線で補間する制御が行われている。このような制御を可能とすることで、小数の教示点を設定するだけでロボットの動作軌跡全体を設定できるというメリットを受けることができる。
さて、産業用途においては、ロボットは、一般的に繰り返し作業を行うために設けられている。その場合、繰り返し作業一回当たりの動作時間を短縮することができれば、生産性を向上させることができる。そのため、例えば特許文献１のものは、始点から終点までロボットを回転駆動させる際の所要時間が最短となるように制御を行っている。

特開平１１−１９８０７２号公報

ところで、ロボットは、例えばフランジ構造の手先を回転駆動するとき、その回転方向を２つ設定することができる。具体的には、現在位置から例えば＋９０°となる回転位置まで回転駆動する際には、現在位置から＋９０°回転駆動する回転方向と、現在位置から−２７０°回転駆動する回転方向とが存在している。そして、いずれの回転方向で回転駆動されたとしても、最終的には、手先は現在位置から＋９０°の回転位置まで回転駆動されることになる。

しかしながら、従来では、上記したように所要時間を短縮するために、始点から終点までの姿勢の変化量が少なくなる回転方向が選択されていた。つまり、従来では、始点、経由点および終点の３点が教示された場合であっても、経由点での姿勢を考慮せずに、始点での姿勢から終点での姿勢まで回転駆動する際の回転量が小さくなる回転方向が選択されていた。その結果、１８０°を超えるような回転を行うことができず、実際に回転駆動されたときの経由点での姿勢が、教示された姿勢から大きくずれてしまうことがあった。

この場合、教示点の数を増やせば姿勢を細かく制御することが可能であるものの、そのようにしてしまうと、小数の教示点を設定するだけでロボットの動作軌跡全体を設定できるという補間による制御を行うメリットがなくなってしまう。
本発明は、上記事情に鑑みてなされたものであり、その目的は、補間による制御を行う場合において、教示点の数が少なくとも経由点での姿勢を教示されて姿勢に近づけることができるロボットの制御方法、ロボットの制御装置を提供することにある。

請求項１記載のロボットの制御方法の発明では、教示された始点、経由点および終点の３点の教示点間を曲線で補間して多関節型のロボットの動作軌跡を求める際に、始点での姿勢を示すベクトルのうち対象とする対象ベクトルＳと、始点から終点までの姿勢変化を表すオイラーベクトルＥＶとの外積Ｓ×ＥＶを求め、対象ベクトルＳと外積Ｓ×ＥＶとがなす平面Ｈを求める。続いて、経由点での姿勢を示すベクトルのうち対象となる対象ベクトルＫＶを平面Ｈに射影した正射影ベクトルＫＶ’と、終点での姿勢を示すベクトルのうち対象となる対象ベクトルＤを平面Ｈに射影した正射影ベクトルＤ’とを求める。そして、対象ベクトルＳと正射影ベクトルＤ’とがなす角αと、対象ベクトルＳと正射影ベクトルＫＶ’とがなす角βとを求める。これらα、βが、回転方向を判定する際の基礎データとなる。

このときαの絶対値がβの絶対値よりも大きく、且つ、αとβとの積が正であるか否かを判定する。このとき、αの絶対値がβの絶対値よりも大きければ、始点での姿勢から終点での姿勢まで最短で変化させる途中に経由点での姿勢が存在することになる。

また、基礎データであるαおよびβを上記したようにオイラーベクトルの正射影ベクトルから求めている。オイラーベクトルは、回転軸となるベクトル成分と、その回転軸周りの回転量成分とによりあらわされる。このため、αとβとの積が負であるということは、回転軸に対するαの回転方向と回転軸に対するβの回転方向とが一致していないこと、すなわち、回転量成分だけ回転させる際の回転方向が逆であることを示している。そして、回転方向が逆であるということは、そもそも姿勢変化というのが３６０°の値で表現されることから、始点から終点までを最短動作させた場合に中継点の姿勢を経由しない（つまり、教示された姿勢に近い姿勢が出現しない）ことになる。

そのため、αの絶対値がβの絶対値よりも大きく、且つ、αとβとの積が正である場合には、始点から終点まで姿勢を変化させた際の姿勢の変化量が相対的に小さくなる回転方向、つまり、始点から終点まで最短で回転駆動する場合と同じ向きの回転方向を、実際にロボットを回転駆動する際の回転方向に設定する。これにより、始点から終点までの動作の中に中継点の姿勢に近い姿勢を含むことができ、教示された姿勢に近い姿勢を取ることができる。

一方、αの絶対値がβの絶対値よりも小さい場合には、始点での姿勢から終点での姿勢まで最短で変化させる途中に経由点での姿勢が存在しないことになる。また、αとβとの積が負である場合には、上記したようにそもそもの回転方向が逆になっている。そのため、αの絶対値がβの絶対値よりも小さい場合、あるいは、αとβとの積が負である場合には、始点から終点まで姿勢を変化させた際の姿勢の変化量が相対的に大きくなる回転方向、つまり、始点から終点まで最短で回転駆動する場合とは逆向きの回転方向を、実際にロボットを回転駆動する際の回転方向に設定する。これにより、始点から終点までの動作の中に中継点の姿勢に近い姿勢を含むことができ、教示された姿勢に近い姿勢を取ることができる。

このように、正射影ベクトルのなす角の大きさに基づいて回転方向を設定することで、遠回りの回転方向が選択肢に組み込まれ、１８０°を超えるような回転が可能となる。そして、実際に回転駆動する際の回転方向を経由点での姿勢に近づける方向に設定することから、教示された経由点での姿勢に近くなる。その結果、補間による制御を行う場合において、経由点での姿勢を教示された姿勢に近づけることができる。つまり、経由点での姿勢が教示された姿勢から大きくずれることを抑制することができる。
この場合、教示点の数を増やすことなく経由点での姿勢を教示された姿勢に近づけることができるため、小数の教示点を設定するだけでロボットの動作軌跡全体を設定できるというメリットが損なわれることがない。

請求項２記載のロボットの制御方法の発明では、手先軸の姿勢を表す３次元ベクトルの全てを対象ベクトルとして回転方向を順次判定する。つまり、手先軸の姿勢が直交座標系におけるアプローチベクトル、オリエントベクトルおよびノーマルベクトルで表される場合、各ベクトルについて、上記した請求項１記載の制御方法を適用して回転方向の判定を順次行う。これにより、例えばアプローチベクトルに対する回転方向の判定と、オリエントベクトルやノーマルベクトルに対する回転方向の判定とが異なるような状態において、オリエントベクトルやノーマルベクトルに対する回転方向の判定を考慮した上で、実際のロボットの回転方向を設定することができる。
このとき、いずれかの対象ベクトルにおいて姿勢の変化量が相対的に大きくなる回転方向が設定された場合には、以降の判定を省略する。以降の判定を省略するので、不必要に判定時間が長期化することがない。

請求項３記載のロボットの制御装置の発明は、上記した請求項１記載のロボットの制御方法を実現するために、ベクトル取得手段、判定手段、および設定手段を備えている。これにより、上記した請求項１記載のロボットの制御方法を採用したロボットの制御装置を実現できる。したがって、請求項１記載のロボットの制御方法で述べたような効果を得ることができる。

請求項４記載のロボットの制御装置の発明では、姿勢を示すベクトルの全てを対象ベクトルとして回転方向の判定を順次行う一方、設定手段によりいずれかの対象ベクトルにおいて姿勢の変化量が相対的に大きくなる回転方向が設定された場合には、以降の判定を省略する。これにより、請求項２記載のロボットの制御方法で述べたような効果を得ることができる。

本発明の一実施形態によるロボットの構成を概略的に示す図 従来の補間制御におけるロボットの姿勢の変化を模式的に示す図 ロボットの回転方向を模式的に示す図 姿勢の異なる２つの座標系における各ベクトルを模式的に示す図 制御装置による回転方向設定処理の流れを示す図 制御装置による回転方向判定処理の流れを示す図 回転方向の判定ができない状態の一例を模式的に示す図 回転方向を判定するアルゴリズムをモデル化した状態を模式的に示す図 回転方向設定処理を行った場合の経由点姿勢を模式的に示す図

以下、本発明の一実施形態について、図１から図９を参照しながら説明する。
図１に示すように、ロボットシステム１は、多関節型のロボット２、ロボット２を制御するコントローラ３、コントローラ３に接続されたティーチングペンダント４を備えている。このロボットシステム１は、一般的な産業用に用いられている。

ロボット２は、いわゆる６軸の垂直多関節型ロボットとして周知の構成を備えており、ベース５上に、Ｚ方向の軸心を持つ第１軸（Ｊ１）を介してショルダ６が水平方向に回転可能に連結されている。ショルダ６には、Ｙ方向の軸心を持つ第２軸（Ｊ２）を介して上方に延びる下アーム７の下端部が垂直方向に回転可能に連結されている。下アーム７の先端部には、Ｙ方向の軸心を持つ第３軸（Ｊ３）を介して第一上アーム８が垂直方向に回転可能に連結されている。第一上アーム８の先端部には、Ｘ方向の軸心を持つ第４軸（Ｊ４）を介して第二上アーム９が捻り回転可能に連結されている。第二上アーム９の先端部には、Ｙ方向の軸心を持つ第５軸（Ｊ５）を介して手首１０が垂直方向に回転可能に連結されている。手首１０には、Ｘ方向の軸心を持つ第６軸（Ｊ６）を介してフランジ１１が捻り回転可能に連結されている。以下、第６軸を、便宜的に手先軸とも称する
ベース５、ショルダ６、下アーム７、第一上アーム８、第二上アーム９、手首１０およびフランジ１１は、ロボット２のアームとして機能し、アームの先端となるフランジ１１には、図示は省略するが、ハンド（エンドエフェクタとも呼ばれる）が取り付けられる。ハンドは、例えば図示しないワークを保持して移送したり、ワークを加工する工具等が取り付けられたりする。ロボット２に設けられている各軸（Ｊ１〜Ｊ６）には、それぞれに対応して駆動源となるモータ（図示省略）が設けられている。

コントローラ３は、ロボットの制御装置に相当し、図示しないＣＰＵ、ＲＯＭおよびＲＡＭ等で構成されたコンピュータからなる制御部においてコンピュータプログラムを実行することで、ロボット２を制御している。具体的には、コントローラ３は、インバータ回路等から構成された駆動部を備えており、各モータに対応して設けられているエンコーダで検知したモータの回転位置に基づいて例えばフィードバック制御によりモータを駆動する。このコントローラ３は、詳細は後述するが、ベクトル取得手段、判定手段、設定手段を構成している。

ティーチングペンダント４は、例えば概ね略矩形箱状に形成されており、ユーザが所持したまま操作可能な程度の大きさに形成されている。このティーチングペンダント４には、各種キースイッチやタッチパネル等が設けられており、ユーザは、それらキースイッチやタッチパネル等を用いてティーチングを行う。
このような構成のロボットシステム１では、ロボット２を制御する際の基準となる座標系が設定されている。本実施形態の場合、ベース５に対応する座標系が、基準座標系Σ_Ｂとなる。この基準座標系Σ_Ｂは、ロボット２がどのような姿勢を取ったとしても変化することがない座標系である。この基準座標系Σ_Ｂには、互いに直交するＸ_Ｂ軸、Ｙ_Ｂ軸およびＺ_Ｂ軸が設定されている。なお、Ｚ_Ｂ軸は設置面に垂直な軸となっている。

また、ロボット２では、手先軸（Ｊ６）に対応する座標系Σ_Ｆが設定されている。この座標系Σ_Ｆは、フランジ１１の向きを手先軸の原点を基準として示す座標系であり、互いに直交するＸ_Ｆ軸、Ｙ_Ｆ軸およびＺ_Ｆ軸が設定されている。このうち、Ｚ_Ｆ軸は、手先軸と同軸に設定されている。つまり、Ｚ_Ｆ軸の向きが、フランジ１１の向き、つまり、手先の向きを示すことになる。

また、ロボット２の姿勢を表す際には、基準座標系Σ_Ｂからみた手先の向きを表すベクトルが用いられる。具体的には、座標系Σ_ＦのＸ_Ｆ軸方向を表す３次元ベクトルであるノーマルベクトル（Ｎ）、基準座標系Σ_Ｂからみた座標系Σ_ＦのＹ_Ｆ軸方向を表す３次元ベクトルであるオリエントベクトル（Ｏ）、および、基準座標系Σ_Ｂからみた座標系Σ_ＦのＺ_Ｆ軸方向を表す３次元ベクトルであるアプローチベクトル（Ａ）が制御に用いられる。
これら各ベクトルは、以下の（１）式〜（３）式にて表される。

上記した（１）式〜（３）式に付されているＴは、以下の（４）式に示すように、基準座標系Σ_Ｂから座標系Σ_Ｆへの座標変換を行う４行４列の行列であり、同次変換行列と称される。この同次変換行列は、ロボット２の手先の位置と姿勢を表すために用いられる。

なお、（４）式に示されているＲは、ノーマルベクトル、オリエントベクトルおよびアプローチベクトルを纏めた３行３列の行列であり、Ｒ＝（Ｎ Ｏ Ａ）として表される。また、（４）式に示されているＰは、座標系Σ_Ｆの原点の位置を基準座標系Σ_Ｂで表した３次元ベクトルである。
ここで、始点、経由点および終点の３点が教示点として示された場合における従来の補間による制御について、図２を参照しながら説明する。以下、複数の教示点間を曲線にて補間する制御を、便宜的に補間制御と称する。なお、この補間制御は、一般的には円弧制御とも称されるが、必ずしも円弧に限らず、スプライン曲線等で補間されることもある。

例えば図２（ａ）に示すように、互いに異なり、且つ、直線上にない始点（Ｋ１）、経由点（Ｋ２）および終点（Ｋ３）の３点が、ユーザにより教示点として設定されたとする。このとき、各教示点での姿勢は、図２（ｂ）に示したようになっているものとする。なお、図２（ｂ）では、説明の簡略化のために、Ｋ１〜Ｋ３におけるアプローチベクトル（Ａ１〜Ａ３）にのみ符号を付している。以下、便宜的に、始点での姿勢を始点姿勢と称し、終点での姿勢を終点姿勢と称し、経由点での姿勢を経由点姿勢と称する。

このとき、従来の補間制御では、図２（ｃ）に示すように、経由点姿勢を考慮せずに、始点姿勢から終点姿勢となるまでの時間が最短となるようにロボット２を回転駆動していた。その結果、図２（ｄ）に示すように、回転駆動される際の経由点のアプローチベクトル（Ａ４）が、図２（ａ）に示した教示された経由点のアプローチベクトル（Ａ２）と一致しない状態、つまり、回転駆動した際の経由点姿勢が教示された姿勢から大きくずれた状態となることがあった。

これは、ロボット２を回転駆動する際には、その回転方向が２つ存在するためである。例えば、図３（ａ）に示すように、手先軸のアプローチベクトルがＡ１である始点姿勢から、アプローチベクトルがＡ３である終点姿勢となるように回転駆動する場合を考えてみる。この場合、始点から終点まで回転駆動する回転方向としては、相対的に回転量が少なく、回転駆動に要する時間が最短時間となる姿勢回転方向Ｄ１（以下、近回りと称する）と、相対的に回転量が多く始点から終点まで回転駆動するのに要する最短時間とはならない姿勢回転方向Ｄ２（以下、遠回りと称する）とが存在する。なお、図３では、説明の簡略化のために、アプローチベクトルＡ１、Ａ３の基点を重ねている。

このとき、従来では、始点姿勢と終点姿勢とだけを考慮し、始点から終点まで回転駆動させたときの時間が最短時間となるように、近回りが選択されていた。その結果、例えば図３（ｂ）に示すように、実際に回転駆動されたときの経由点でのアプローチベクトル（Ａ４）が、教示された経由点でのアプローチベクトル（Ａ２）に対して例えば逆向きになる等、経由点での姿勢が大きくずれるおそれがあった。そして、ロボット２の手先にはツールが取り付けられることから、手先の向きがずれてしまうと、ツールの向きもずれてしまう。その結果、ワークが意図しない形状に加工されたりするおそれがあった。この場合、アプローチベクトルに限らず、後述する図４に示すようなノーマルベクトルやオリエントベクトルが変化する場合には、同様に教示された姿勢から大きくずれる可能性があった。

また、教示点の数を増やし、ロボット２の軌跡を細かく制御することで、上記したずれが生じることを防止することができると考えられるが、そのような対処方法は、少ない教示点を設定することで軌跡全体を設定できるという補間制御の意義が薄れてしまう。
そこで、本実施形態では、補間制御を行う場合において、実際に回転駆動されたときの経由点姿勢とユーザにより教示された経由点姿勢とが大きくずれることを抑制している。
まず、姿勢行列を用いて２つの座標間の姿勢変化を表す手法について、図４を参照しながら説明する。図４に示すように、基準座標系Σ_Ｂに任意の２点が存在し、一方が座標系Σ_Ｆ１で表される例えば始点であり、他方が座標系Σ_Ｆ２で表される例えば終点であるとする。このとき、始点からみた終点の姿勢行列を_Ｆ１Ｒ^Ｆ２とすると、姿勢行列_Ｆ１Ｒ^Ｆ２は、以下の（５）式で表すことができる。

ただし、（５）式において、_ＢＲ^Ｆ１は、基準座標系Σ_Ｂから座標系Σ_Ｆ１への同次変換行列_ＢＴ^Ｆ１に含まれる姿勢行列であり、_ＢＲ^Ｆ２は、基準座標系Σ_Ｂから座標系Σ_Ｆ２への同次変換行列_ＢＴ^Ｆ２に含まれる姿勢行列である。

そして、座標系Σ_Ｆ１から座標系Σ_Ｆ２への姿勢の変化量ΔＲは、座標系Σ_Ｆ１から座標系Σ_Ｆ２への姿勢行列_Ｆ１Ｒ^Ｆ２と同義であるので、上記した（５）式から、姿勢の変化量ΔＲは、以下の（６）式にて表される。

さて、本実施形態では、後述するように、回転駆動する際の回転方向を、オイラーベクトル表現した際の回転量成分θにより判定する。オイラーベクトルは、周知のように、オイラーベクトルはある姿勢行列Ｒを、回転軸となる以下の（７）式で示される単位ベクトルのベクトル成分Ｖ（ｖ_ｘ，ｖ_ｙ，ｖ_ｚ）と、回転量成分θとで表現する手法である。

そして、上記した（６）式をオーラ−ベクトルの公式に適用することで、以下の（８）式および（９）式に示すように、座標系Σ_Ｆ１と座標系Σ_Ｆ２との間の姿勢の変化量（ΔＲ）を、ベクトル成分と回転量成分とで表現することができる。

これら姿勢変化をオイラーベクトル表現するための処理は、コントローラ３によって行われている。そして、コントローラ３は、姿勢変化をオイラーベクトル表現した際のベクトル成分（ｖ）と回転量成分（θ）とに基づいて、補間制御を行う際の回転駆動の回転方向を上記した近回りとするか遠回りとするかを判定する。より具体的には、本実施形態の場合、コントローラ３は、図２に示したような互いに異なり、且つ、直線上にない始点（Ｋ１）、経由点（Ｋ２）および終点（Ｋ３）の３点が教示点として設定されたとき、手先軸に対応するアプローチベクトル、オリエントベクトルおよびノーマルベクトルのそれぞれについて、図５に示す回転方向設定処理および図６に示す回転方向判定処理を実行することで回転方向を判定する。

コントローラ３は、まず回転方向設定処理において、回転方向の判定が可能であるかを判定する（Ｓ１）。本実施形態では、オイラーベクトルを後述する平面Ｈに射影することで回転方向を判定している。このとき、図７に示すようにオイラーベクトル（ＥＶ。より厳密には、オイラーベクトル表現した際のベクトル成分）が平面Ｈに対して垂直となった場合であって、且つ、始点での対象ベクトルＳ（アプローチベクトル、オリエントベクトル、ノーマルベクトルのいずれか）とオイラーベクトル（ＥＶ）との外積（Ｓ×ＥＶ）がオイラーベクトル（ＥＶ）に対して垂直となった場合には、オイラーベクトル（ＥＶ）を平面Ｈに射影したベクトルが零ベクトルとなり、回転方向の判定ができなくなる。

そのため、コントローラ３は、ステップＳ１にて回転方向の判定が可能であるかを判定し、回転方向の判定が不可能な場合には（Ｓ１：ＮＯ）、処理を終了する。なお、判定ができない場合に何も処理をしないと補間制御ができなくなるため、例えば従来と同様の手法にて回転方向を判定する例外処理を実行するようにしてもよい。

一方、コントローラ３は、回転方向の判定が可能な場合には（Ｓ１：ＹＥＳ）、まず、アプローチベクトルを対象として、回転方向判定処理を実行する（Ｓ２）。この回転方向判定処理において、コントローラ３は、まず、始点での対象ベクトルＳ（この場合、アプローチベクトル）と、始点から終点までの姿勢変化を表すオイラーベクトル（ＥＶ）とを求める（Ｓ１０）。このオイラーベクトル（ＥＶ）は、図８（ａ）に示すようなベクトルとなる。

次に、コントローラ３は、図８（ｂ）に示すように、対象ベクトルＳとＥＶとの外積（Ｓ×ＥＶ）を求め（Ｓ１１）、図８（ｃ）に示すように、Ｓと（Ｓ×ＥＶ）とがなす平面Ｈを求める（Ｓ１２）。続いて、コントローラ３は、教示された経由点の対象ベクトルＫＶを平面Ｈに射影した正射影ベクトルＫＶ’と、終点の対象ベクトルＤを平面Ｈに射影した正射影ベクトルＤ’とを求める（Ｓ１３）。つまり、コントローラ３は、ベクトル取得手段として機能する。

続いて、コントローラ３は、始点での対象ベクトルＳと正射影ベクトルＤ’とがなす角αと、始点での対象ベクトルＳと正射影ベクトルＫＶ’とがなす角βとを求める（Ｓ１４）。なお、各ベクトルがなす角は、図２に示したように角度の取り方によってはαまたは３６０−αとなるものの、このステップＳ１４では、最小値となる角度がなす角として選択される。そして、コントローラ３は、αの絶対値（｜α｜）がβの絶対値（｜β｜）よりも大きく、且つ、αとβの積（αβ）が正であるかを判定する（Ｓ１５）。つまり、コントローラ３は、判定手段として機能する。

ここで、αβが正であるかを判定しているのは、経由点の対象ベクトルの向きによっては、いずれの回転方向とするかを判定することが可能であるためである。すなわち、始点から終点に向かう回転方向を仮に正とすれば、αβが負となるのはβが負の場合であり、その状況は、始点から経由点に向かう回転方向が、始点から終点に向かう向きとは逆であることを示している。

また、｜α｜が｜β｜よりも大きい状態は、始点から終点まで回転駆動する途中に経由点での姿勢を取ることができることを意味している。より平易に言えば、始点と終点との間に経由点が位置していることになる。逆に、｜α｜が｜β｜よりも小さい状態は、経由点が終点よりも先にあることを意味している。つまり、始点から終点まで最短で回転駆動する途中には、経由点での姿勢を取ることができないことを意味している。

そのため、コントローラ３は、｜α｜が｜β｜よりも大きく、且つ、αβが正である場合には（Ｓ１５：ＹＥＳ）、姿勢回転方向を近回りと判定する（Ｓ１６）。つまり、始点から終点まで最短で回転駆動する途中で、経由点姿勢を取ることができると判定する。
これに対して、コントローラ３は、｜α｜が｜β｜よりも小さい場合、あるいは、αβが正でない場合には（Ｓ１５：ＮＯ）、姿勢回転方向を遠回りと判定する（Ｓ１６）。つまり、始点から終点まで最短で回転駆動する途中で、経由点姿勢を取ることができると判定する。

そして、コントローラ３は、姿勢回転方向をいずれにするかを判定すると、回転方向設定処理にリターンし、判定結果が近回りであるかを判定し（Ｓ３）、回転方向判定処理において遠回りと判定されていれば、判定結果が近回りではないので（Ｓ３：ＮＯ）、姿勢回転方向を遠回りに設定する（Ｓ９）。すなわち、始点姿勢から終点姿勢まで回転駆動する途中では経由点姿勢と同じあるいは近似する姿勢を取ることができないため、回転方向を遠回りに設定することで、すなわち、１８０°を超えるような回転方向を設定することで、補間制御にて実際に回転駆動する際の経由点姿勢を教示された経由点姿勢に近づけることができる。つまり、コントローラ３は、設定手段として機能する。

ところで、多関節型のロボット２の場合、当然のことながら、始点姿勢と終点姿勢とにおいてアプローチベクトル以外のベクトルも変化する可能性がある。その場合、アプローチベクトルに対する回転方向の判定のみで実際の回転方向を決定すると、オリエントベクトルやノーマルベクトルにおける回転方向の判定が行われず、実際の動作軌跡における経由点姿勢と教示された経由点姿勢とで例えばオリエントベクトル大きくずれてしまうといったことが起こり得る。つまり、アプローチベクトルだけを教示された経由点姿勢に近づけることができたとしても、他のベクトルでは教示された経由点姿勢から乖離してしまっている可能性がある。

そこで、コントローラ３は、アプローチベクトルを対象とした回転方向判定処理において近回りと判定された場合には（Ｓ３：ＹＥＳ）、オリエントベクトルを対象として回転方向判定処理を実行する（Ｓ４）。なお、ステップＳ４の回転方向判定処理では、対象がオリエントベクトルとなるものの上記したアプローチベクトルと同様の処理が行われるため、説明を省略する。

コントローラ３は、オリエントベクトルを対象とした回転方向判定処理において遠回りと判定されていれば（Ｓ５：ＮＯ）、姿勢回転方向を遠回りに設定する（Ｓ９）。一方、コントローラ３は、回転方向判定処理において近回りと判定されている場合には（Ｓ５：ＹＥＳ）、ノーマルベクトルを対象として、回転方向判定処理を実行する（Ｓ６）。
そして、コントローラ３は、ノーマルベクトルを対象とした回転方向判定処理において遠回りと判定されていれば（Ｓ７：ＮＯ）、姿勢回転方向を遠回りに設定する（Ｓ９）。一方、コントローラ３は、回転方向判定処理において近回りと判定されている場合には（Ｓ４：ＹＥＳ）、姿勢回転方向を近回りに設定する（Ｓ８）。

このように、コントローラ３は、手先軸の各ベクトルに対してそれぞれ回転方向を判定し、いずれかにおいて遠回りと判定された場合には、実際に回転駆動する際の回転方向を遠回りに設定し、いずれも近回りと判定された場合には、実際に回転駆動する際の回転方向を近回りに設定する。
これにより、例えば図９（ａ）に示すような教示点つまり姿勢がユーザにより教示されたとき、回転方向設定処理によって１８０°を超える回転方向を選択できるようになり、教示された経由点姿勢に近づく回転方向が選択されて、例えば図９（ｂ）に示すように教示されたアプローチベクトル（Ａ２）と実際に回転駆動する際のアプローチベクトル（Ａ４）が一致する等、実際の軌道における経由点姿勢を教示された経由点姿勢と同じあるいは近似した姿勢にすることができる。

以上説明した本実施形態によれば、次のような効果を得ることができる。
実施形態の制御方法では、始点での対象ベクトルＳと始点から終点までの姿勢変化を表すオイラーベクトルＥＶとの外積Ｓ×ＥＶを求め、対象ベクトルＳと外積Ｓ×ＥＶとがなす平面Ｈにおいて、経由点に対応する正射影ベクトルＫＶ’と、終点に対応する正射影ベクトルＤ’とを求める。そして、対象ベクトルＳと正射影ベクトルＤ’とがなす角αと対象ベクトルＳと正射影ベクトルＫＶ’とがなす角βとに基づいて、回転方向を経由点での姿勢に近づける方向に設定する。このように、正射影ベクトルのなす角に基づいてロボットを回転駆動する際の回転方向を設定する制御方法を採用することで、遠回りの回転方向が選択肢に組み込まれ、１８０°を超えるような回転が可能となる。そして、実際に回転駆動した際の経由点での姿勢を教示された姿勢に近づけることができる。したがって、補間による制御を行う場合において経由点での姿勢が教示された姿勢から大きくずれることを抑制できる。

また、教示点の数を増やすことなく経由点での姿勢を教示された姿勢に近づけることができるため、小数の教示点を設定するだけでロボットの動作軌跡全体を設定できるというメリットが損なわれることがない。
また、手先軸の姿勢が直交座標系におけるアプローチベクトル、オリエントベクトルおよびノーマルベクトルで表される場合、各ベクトルをそれぞれ対象ベクトルとして、回転方向を順次判定する。これにより、例えばアプローチベクトルに対する回転方向の判定と、オリエントベクトルやノーマルベクトルに対する回転方向の判定とが異なるような状態において、オリエントベクトルやノーマルベクトルに対する回転方向の判定を考慮した上で、実際のロボットの回転方向を設定することができる。

また、いずれかの対象ベクトルにおいて姿勢の変化量が相対的に大きくなる回転方向が設定された場合には、以降の判定を省略するので、不必要に判定時間が長期化することを抑制できる。
このとき、一実施形態では、アプローチベクトルに対する回転方向の判定を最初に行った後、他のベクトルに対する判定を行っている。アプローチベクトルは、手先の向きを示すベクトルであるため、アプローチベクトルが教示された姿勢に近ければ、その姿勢は、ユーザが所望する姿勢であると考えられる。そのため、アプローチベクトルに対する判定を最初に行うことにより、その時点で回転方向が遠回りと判定されれば以降の判定を省略しても教示された姿勢に近い姿勢を取ることができると考えられる。つまり、アプローチベクトルを最初に判定することは、以降の判定を省略する根拠なり得る。

コントローラ３は、上記した制御方法を採用したロボットの制御装置を実現するために、ベクトル取得手段、判定手段、および設定手段として機能する。これにより、上記した制御方法を採用したロボットの制御装置を実現でき、補間制御を行う際に実際にロボット２を制御する際に１８０°を超える回転方向を選択することができ、教示された姿勢に近づけることができる等、上記した制御方法で述べたような効果を得ることができる。

また、コントローラ３は、姿勢を示すベクトルの全てを対象ベクトルとして回転方向の判定を順次行う一方、いずれかの対象ベクトルにおいて姿勢の変化量が相対的に大きくなる回転方向が設定された場合には、以降の判定を省略する。これにより、上記したように不必要に判定時間が長期化することを抑制できる。
本発明は上記した一実施形態に限定されるものではなく、その要旨を逸脱しない範囲で任意に変形または拡張することができる。

図面中、２はロボット、３はコントローラ（ロボットの制御装置、ベクトル取得手段、判定手段、設定手段）を示す。

Claims (4)

1. 教示された始点、経由点および終点の３点の教示点間を曲線で補間して多関節型のロボットの動作軌跡を求める際に、
   始点での姿勢を示すベクトルのうち対象とする対象ベクトルＳと、始点から終点までの姿勢変化を表すオイラーベクトルＥＶとの外積Ｓ×ＥＶを求め、
   対象ベクトルＳと外積Ｓ×ＥＶとがなす平面Ｈを求め、
   経由点での姿勢を示すベクトルのうち対象となる対象ベクトルＫＶを平面Ｈに射影した正射影ベクトルＫＶ’と、終点での姿勢を示すベクトルのうち対象となる対象ベクトルＤを平面Ｈに射影した正射影ベクトルＤ’とを求め、
   対象ベクトルＳと正射影ベクトルＤ’とがなす角αと、対象ベクトルＳと正射影ベクトルＫＶ’とがなす角βとを求め、
   αの絶対値がβの絶対値よりも大きく、且つ、αとβとの積が正であるか否かを判定し、
   αの絶対値がβの絶対値よりも大きく、且つ、αとβとの積が正である場合には、始点から終点まで姿勢を変化させた際の姿勢の変化量が相対的に小さくなる回転方向を、ロボットを回転駆動する際の回転方向に設定し、
   αの絶対値がβの絶対値よりも小さい場合、あるいは、αとβとの積が負である場合には、始点から終点まで姿勢を変化させた際の姿勢の変化量が相対的に大きくなる回転方向を、ロボットを回転駆動する際の回転方向に設定することを特徴とするロボットの制御方法。
2. 手先軸の姿勢を表す３次元ベクトルの全てを対象ベクトルとして回転方向を順次判定する一方、いずれかの対象ベクトルにおいて姿勢の変化量が相対的に大きくなる回転方向が設定された場合には、以降の判定を省略することを特徴とする請求項１記載のロボットの制御方法。
3. 多関節型のロボットを制御するロボットの制御装置であって、
   始点での姿勢を示すベクトルのうち対象とする対象ベクトルＳと、始点から終点までの姿勢変化を表すオイラーベクトルＥＶとの外積Ｓ×ＥＶを求め、対象ベクトルＳと外積Ｓ×ＥＶとがなす平面Ｈを求め、経由点での姿勢を示すベクトルのうち対象となる対象ベクトルＫＶを平面Ｈに射影した正射影ベクトルＫＶ’と、終点での姿勢を示すベクトルのうち対象となる対象ベクトルＤを平面Ｈに射影した正射影ベクトルＤ’とを求めるベクトル取得手段と、
   前記ベクトル取得手段で求めた対象ベクトルＳと正射影ベクトルＤ’とがなす角αと、対象ベクトルＳと正射影ベクトルＫＶ’とがなす角βとを求め、αの絶対値がβの絶対値よりも大きく、且つ、αとβとの積が正であるか否かを判定する判定手段と、
   前記判定手段によってαの絶対値がβの絶対値よりも大きく、且つ、αとβとの積が正である場合には、始点から終点まで姿勢を変化させた際の姿勢の変化量が相対的に小さくなる回転方向を、ロボットを回転駆動する際の回転方向に設定し、αの絶対値がβの絶対値よりも小さい場合、あるいは、αとβとの積が負である場合には、始点から終点まで姿勢を変化させた際の姿勢の変化量が相対的に大きくなる回転方向を、ロボットを回転駆動する際の回転方向に設定する設定手段と、
   を備えることを特徴とするロボットの制御装置。
4. 手先軸の姿勢を表す３次元ベクトルの全てを対象ベクトルとして回転方向を順次判定する一方、前記設定手段によりいずれかの対象ベクトルにおいて姿勢の変化量が相対的に大きくなる回転方向が設定された場合には、以降の判定を省略することを特徴とする請求項３記載のロボットの制御装置。

Images