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

Description

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

多関節型のロボットにおいて、ユーザにより教示された複数の教示点間を曲線で補間し、その曲線に沿ってロボットを回転駆動する制御が行われている。具体的には、例えば始点、経由点および終点が教示された場合にそれら３つの教示点間を曲線で補間する制御が行われている。このような制御を可能とすることで、小数の教示点を設定するだけでロボットの動作軌跡全体を設定できるというメリットを受けることができる。

さて、産業用途においては、ロボットは、一般的に繰り返し作業を行うために設けられている。その場合、繰り返し作業一回当たりの動作時間を短縮することができれば、生産性を向上させることができる。そのため、例えば特許文献１のものは、始点から終点までロボットを回転駆動させる際の所要時間が最短となるように制御を行っている。

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

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

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

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

請求項１記載のロボットの制御方法の発明では、教示された始点、経由点および終点の３点の教示点間を曲線で補間して多関節型のロボットの動作軌跡を求める際に、始点から終点までの姿勢変化を表すオイラーベクトルＶ１と、始点から経由点までの姿勢変化を表すオイラーベクトルＶ２とを求め、オイラーベクトルＶ１とオイラーベクトルＶ２との内積を求めて内積の正負を判定する。

オイラーベクトルは、回転軸となるベクトル成分と、その回転軸周りの回転量成分とにより表される。このとき、２つのオイラーベクトルの内積が正であるか負であるかは、オイラーベクトルによって示された回転軸の向きが一致しているか否か、すなわち、回転量成分だけ回転させる際の回転方向が一致するか否かを示している。つまり、内積の正負によって、姿勢を変化させた際の回転方向が一致するか否かを判定することができる。

そして、上記したオイラーベクトルＶ１とオイラーベクトルＶ２の場合では、内積が負となるのは、始点から終点まで最短で回転駆動する場合の回転方向と、始点から経由点まで最短で回転駆動する場合の回転方向とが一致しない場合である。一方、内積が正となるのは、始点から終点まで最短で回転駆動する場合の回転方向と、始点から経由点まで最短で回転駆動する場合の回転方向とが一致する場合である。

そのため、オイラーベクトルＶ１とオイラーベクトルＶ２との内積が負であれば、始点から終点まで最短で回転させた場合の回転方向と始点から経由点まで最短で回転駆動する場合の回転方向とが一致していないことから、始点から終点までを最短動作させた場合に経由点の姿勢を経由しない状態、すなわち、始点から終点までの動作中の任意の場所でも経由点で教示した姿勢に近い姿勢が出現しない状態となるので、始点から終点まで姿勢を変化させた際の姿勢の変化量が相対的に大きくなる回転方向、つまり、始点から終点まで最短で回転駆動する場合とは逆向きの回転方向を、実際にロボットを回転駆動する際の回転方向に設定する。逆向きの回転方向ということは、そもそも姿勢変化というのが３６０°の値で表現されることから、始点から終点までを最短動作させた場合に経由点の姿勢を経由しない（つまり、教示された姿勢に近い姿勢が出現しない）ことを考えれば、逆に始点から終点までの動作の中に経由点の姿勢に近い姿勢を含むということである。これにより、実際に回転駆動した際の経由点での姿勢を教示された経由点での姿勢に近づけることができる。そして、遠回りの回転方向が選択可能であるため、１８０°を超えるような回転が可能となる。したがって、補間による制御を行う場合において経由点での姿勢が教示された姿勢から大きくずれることを抑制できる。

さて、オイラーベクトルＶ１とオイラーベクトルＶ２との内積が正であれば、始点から終点まで最短で回転駆動する場合の回転方向と始点から経由点まで最短で回転駆動する場合の回転方向とが一致していることになる。ただし、内積が正になったとしても、必ずしも始点から終点までを最短動作させた場合に経由点の姿勢を経由する（つまり、教示された姿勢に近い姿勢が出現する）とは限らない。これは、経由点とは終点までに経由すべき点として教示される点であるものの、始点での姿勢から経由点での姿勢までの回転量成分が、始点での姿勢から終点での姿勢までの回転量成分よりも大きく変化していることもあり得るためである。

そこで、内積が正であった場合には、オイラーベクトルＶ１の回転量成分θ_αとオイラーベクトルＶ２の回転量成分θ_βとの大小関係を比較する。
このとき、回転量成分θ_αが回転量成分θ_βよりも大きければ、始点から終点まで姿勢を変化させた際の姿勢の変化量が相対的に小さくなる回転方向、つまり、始点から終点まで最短で回転駆動する場合と同じ回転方向を、実際にロボットを回転駆動する際の回転方向に設定する。これは、平易に言えば、始点から終点まで最短で姿勢を変化させる回転方向に回転駆動すれば、その途中で経由点での姿勢を取ることができるためである。

一方、回転量成分θ_αが回転量成分θ_βよりも小さければ、始点から終点まで姿勢を変化させた際の姿勢の変化量が相対的に大きくなる回転方向、つまり、始点から終点まで最短で回転駆動する場合とは逆向きの回転方向を、実際にロボットを回転駆動する際の回転方向に設定する。これは、平易に言えば、始点から終点まで最短で姿勢を変化させる回転方向に回転駆動すると、その途中では経由点での姿勢を取ることができないためである。

このように、オイラーベクトルの内積および回転量成分の大小関係に基づいてロボットを回転駆動する際の回転方向を設定することにより、遠回りの回転方向が選択肢に組み込まれ、１８０°を超えるような回転が可能となる。そして、経由点での姿勢に近づける方向に回転方向を設定するので、教示された経由点での姿勢に近くなり、補間による制御を行う場合において、経由点での姿勢を教示された姿勢に近づけることができる。つまり、経由点での姿勢が教示された姿勢から大きくずれることを抑制することができる。

この場合、回転量成分のスカラー値を直接用いて大小関係を判定しているので、処理を簡略化することができ、判定を短期間で行うことができる。
また、教示点の数を増やすことなく経由点での姿勢を教示された姿勢に近づけることができるため、小数の教示点を設定するだけでロボットの動作軌跡全体を設定できるというメリットが損なわれることもない。

請求項２記載のロボットの制御方法の発明では、手先軸の座標系を対象として回転方向の判定を行う。手先軸の座標系は、手先つまりツールの向きを示すものである。そこで、手先軸の座標系を対象として回転方向を判定すれば、手先軸の向きを教示された経由点での向きに近づけることができる。すなわち、実際の動作軌跡における経由点での姿勢を、教示された姿勢つまりはユーザが所望する姿勢に近づけることができる。

請求項３記載のロボットの制御装置の発明は、オイラーベクトルを求めるベクトル取得手段と、オイラーベクトルの内積を求めて内積の正負を判定する判定手段と、オイラーベクトルの回転量成分の大小関係を比較する比較手段と、判定手段の判定結果および比較手段の比較結果に基づいてロボットを回転駆動する際の回転方向を設定する設定手段と、を備える。これにより、上記した請求項１記載のロボットの制御方法を採用したロボットの制御装置を実現できる。そして、上記した請求項１記載のロボットの制御方法で述べたような効果を得ることができる。

請求項４記載のロボットの制御装置の発明では、ベクトル取得手段は、手先軸に対応する座標系における姿勢行列を対象としてオイラーベクトルを求める。つまり、手先軸を対象として回転方向の判定を行う。これにより、上記した請求項２記載のロボットの制御方法で述べたような効果を得ることができる。

本発明の一実施形態によるロボットの構成を概略的に示す図 従来の補間制御におけるロボットの姿勢の変化を模式的に示す図 ロボットの回転方向を模式的に示す図 姿勢の異なる２つの座標系における各ベクトルを模式的に示す図 制御装置による回転方向設定処理の流れを示す図 経由点姿勢とオイラーベクトルとの関係を模式的に示す図 経由点および終点に対する回転量成分の大小関係を模式的に示す図 回転方向設定処理を行った場合の経由点姿勢を模式的に示す図

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

ロボット２は、いわゆる６軸の垂直多関節型ロボットとして周知の構成を備えており、ベース５上に、Ｚ方向の軸心を持つ第１軸（Ｊ１）を介してショルダ６が水平方向に回転可能に連結されている。ショルダ６には、Ｙ方向の軸心を持つ第２軸（Ｊ２）を介して上方に延びる下アーム７の下端部が垂直方向に回転可能に連結されている。下アーム７の先端部には、Ｙ方向の軸心を持つ第３軸（Ｊ３）を介して第一上アーム８が垂直方向に回転可能に連結されている。第一上アーム８の先端部には、Ｘ方向の軸心を持つ第４軸（Ｊ４）を介して第二上アーム９が捻り回転可能に連結されている。第二上アーム９の先端部には、Ｙ方向の軸心を持つ第５軸（Ｊ５）を介して手首１０が垂直方向に回転可能に連結されている。手首１０には、Ｘ方向の軸心を持つ第６軸（Ｊ６）を介してフランジ１１が捻り回転可能に連結されている。以下、第６軸を、便宜的に手先軸とも称する

ベース５、ショルダ６、下アーム７、第一上アーム８、第二上アーム９、手首１０およびフランジ１１は、ロボット２のアームとして機能し、アームの先端となるフランジ１１には、図示は省略するが、ハンド（エンドエフェクタとも呼ばれる）が取り付けられる。ハンドは、例えば図示しないワークを保持して移送したり、ワークを加工する工具等が取り付けられたりする。ロボット２に設けられている各軸（Ｊ１〜Ｊ６）には、それぞれに対応して駆動源となるモータ（図示省略）が設けられている。
コントローラ３は、ロボットの制御装置に相当し、図示しないＣＰＵ、ＲＯＭおよびＲＡＭ等で構成されたコンピュータからなる制御部においてコンピュータプログラムを実行することで、ロボット２を制御している。具体的には、コントローラ３は、インバータ回路等から構成された駆動部を備えており、各モータに対応して設けられているエンコーダで検知したモータの回転位置に基づいて例えばフィードバック制御によりモータを駆動する。

また、コントローラ３は、詳細は後述するが、姿勢変化を表すオイラーベクトルを求めるベクトル取得手段、オイラーベクトルの内積を求めて内積の正負を判定する判定手段、オイラーベクトルの回転量成分の大小関係を比較する比較手段、および、判定手段の判定結果および比較手段の比較結果に基づいてロボット２を回転駆動する際の回転方向を設定する設定手段を構成している。
ティーチングペンダント４は、例えば概ね略矩形箱状に形成されており、ユーザが所持したまま操作可能な程度の大きさに形成されている。このティーチングペンダント４には、各種キースイッチやタッチパネル等が設けられており、ユーザは、それらキースイッチやタッチパネル等を用いてティーチングを行う。

このような構成のロボットシステム１では、ロボット２を制御する際の基準となる座標系が設定されている。本実施形態の場合、ベース５に対応する座標系が、基準座標系Σ_Ｂとなる。この基準座標系Σ_Ｂは、ロボット２がどのような姿勢を取ったとしても変化することがない座標系である。この基準座標系Σ_Ｂには、互いに直交するＸ_Ｂ軸、Ｙ_Ｂ軸およびＺ_Ｂ軸が設定されている。なお、Ｚ_Ｂ軸は設置面に垂直な軸となっている。
また、ロボット２では、手先軸（Ｊ６）に対応する座標系Σ_Ｆが設定されている。この座標系Σ_Ｆは、フランジ１１の向きを手先軸の原点を基準として示す座標系であり、互いに直交するＸ_Ｆ軸、Ｙ_Ｆ軸およびＺ_Ｆ軸が設定されている。このうち、Ｚ_Ｆ軸は、手先軸と同軸に設定されている。つまり、Ｚ_Ｆ軸の向きが、フランジ１１の向き、つまり、手先の向きを示すことになる。

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

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

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

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

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

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

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

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

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

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

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

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

本実施形態の場合、コントローラ３は、手先軸の座標系を対象として回転方向設定処理を実行する。これは、手先軸の座標系は手先つまりツールの向きを示すものであり、ワークを加工する際に重視されると考えられるためである。
この回転方向設定処理において、コントローラ３は、まず教示された始点、経由点および終点での姿勢を取得し、姿勢行列に基づいて、始点姿勢から終点姿勢への変化をオイラーベクトル表現した際のベクトルＶ１と、始点姿勢から経由点姿勢への変化をオイラーベクトル表現した際のベクトルＶ２とを求める。つまり、コントローラ３は、姿勢変化を表すオイラーベクトルを求めるベクトル取得手段として機能する。

オイラーベクトルを求めると、コントローラ３は、それらの内積（Ｖ１・Ｖ２）が負であるかを判定する（Ｓ１）。このステップＳ１では、始点から終点まで回転駆動する際の回転方向と、始点から経由点まで回転駆動する際の回転方向とが一致するか否かが判定されている。つまり、コントローラ３は、オイラーベクトルの内積を求めて内積の正負を判定する判定手段として機能する。

例えば、図６（ａ）に模式的に示すように、始点から終点まで最短で、つまり、回転量が少ない方向に回転駆動する際の回転方向Ｄ３と、始点から経由点まで最短で回転駆動する際の回転方向Ｄ４とが一致すれば、内積（Ｖ１・Ｖ２）は正となる。一方、図６（ｂ）に模式的に示すように、始点から終点まで回転駆動する際の回転方向Ｄ３と、始点から経由点まで回転駆動する際の回転方向Ｄ４とが一致していなければ、内積（Ｖ１・Ｖ２）は負となる。つまり、内積（Ｖ１・Ｖ２）の正負によって、経由点姿勢に近づけるための回転方向を判断することができる。

そのため、コントローラ３は、内積（Ｖ１・Ｖ２）が負である場合には（Ｓ１：ＹＥＳ）、姿勢回転方向を、遠回り方向つまり終点まで最短となる回転方向とは逆向きの回転方向に設定する（Ｓ４）。これにより、１８０°を超えるような回転方向を選択できるようになり、実際に回転駆動した際における経由点姿勢を、教示された経由点姿勢に近づけることができる。

これに対して、コントローラ３は、内積（Ｖ１・Ｖ２）が負でない場合には（Ｓ１：ＮＯ）、回転量θに基づいて回転方向を判定する。具体的には、コントローラ３は、始点姿勢から終点姿勢への変化をオイラーベクトル表現した際の回転量成分θ_αと、始点姿勢から経由点姿勢への変化をオイラーベクトル表現した際の回転量成分θ_βとを求め、その大小関係に基づいて回転方向を判定する。つまり、コントローラ３は、オイラーベクトルＶ１の回転量成分θ_αとオイラーベクトルＶ２の回転量成分θ_βとの大小関係を比較する比較手段として機能する。

コントローラ３は、図７（ａ）に示すように回転量成分θ_αが回転量成分θ_βよりも大きい場合には（Ｓ２：ＮＯ）、姿勢回転方向を、近回り方向つまり終点まで最短となる回転方向と同じ向き回転方向に設定する（Ｓ３）。この状態は、より平易に言えば、始点姿勢から終点姿勢まで回転駆動する途中で、経由点姿勢と同じあるいは近似する姿勢を取ることができる状態である。そのため、回転方向を近回り方向に設定することで、実際に回転駆動する際の経由点姿勢を教示された経由点姿勢に近づけることができる。

一方、コントローラ３は、図７（ｂ）に示すように回転量成分θ_αが回転量成分θ_βよりも小さい場合には（Ｓ２：ＹＥＳ）、姿勢回転方向を、遠回り方向に設定する（Ｓ４）。この場合、始点姿勢から終点姿勢まで最短で回転駆動すると、その途中において経由点姿勢と同じあるいは近似する姿勢を取ることができない。そのため、コントローラ３は、回転方向に遠回り方向を設定する。これにより、実際に回転駆動した際における経由点姿勢を、教示された経由点姿勢に近づけることができる。

このように、コントローラ３は、オイラーベクトルの内積および回転量成分の大小関係に基づいて、ロボット２を回転駆動する際の回転方向を設定する設定手段として機能する。

いずれかの回転方向を設定すると、コントローラ３は、回転方向設定処理を終了する。その後、教示点間を補間して動作軌跡を設定し、その動作軌跡に沿うようにロボット２を制御する。
これにより、例えば図８（ａ）に示すような教示点つまり姿勢がユーザにより教示されたとき、回転方向設定処理によって１８０°を超える回転方向を選択できるようになり、教示された経由点姿勢に近づく回転方向が選択されて、例えば図８（ｂ）に示すように教示されたアプローチベクトル（Ａ２）と実際に回転駆動する際のアプローチベクトル（Ａ４）が一致する等、実際の軌道における経由点姿勢を教示された経由点姿勢と同じあるいは近似した姿勢にすることができる。

以上説明した本実施形態によれば、次のような効果を得ることができる。
実施形態の制御方法では、始点から終点までの姿勢変化を表すオイラーベクトルＶ１と、始点から経由点までの姿勢変化を表すオイラーベクトルＶ２とを求め、オイラーベクトルＶ１とオイラーベクトルＶ２との内積を求め、まず、姿勢を変化させた際の回転方向が一致するか否かを判定する。そして、オイラーベクトルＶ１とオイラーベクトルＶ２との内積が負であれば、始点から終点まで最短で回転させた場合の回転方向と始点から経由点まで最短で回転させた場合の回転方向とが一致していないことから、始点から終点まで最短で回転させた場合とは逆向きの回転方向（遠回り方向）を、実際にロボットを回転駆動する際の回転方向に設定する。これにより、実際に回転駆動した際の経由点での姿勢を教示された経由点での姿勢に近づけることができる。そして、遠回りの回転方向が選択された場合には、１８０°を超えるような回転が可能となる。したがって、補間による制御を行う場合において経由点での姿勢が教示された姿勢から大きくずれることを抑制できる。

一方、オイラーベクトルＶ１とオイラーベクトルＶ２との内積が正であれば、始点から終点まで最短で回転させた場合の回転方向と始点から経由点まで最短で回転させた場合の回転方向とが一致することになるため、オイラーベクトルＶ１の回転量成分θ_αとオイラーベクトルＶ２の回転量成分θ_βとの大小関係を比較する。そして、回転量成分θ_αが回転量成分θ_βよりも大きければ、始点から終点まで最短で回転させた場合と同じ回転方向（近回り方向）を、実際にロボットを回転駆動する際の回転方向に設定する。また、回転量成分θ_αが回転量成分θ_βよりも小さければ、始点から終点まで最短で回転させた場合とは逆向きの回転方向（遠回り方向）を、実際にロボットを回転駆動する際の回転方向に設定する。

このように、オイラーベクトルの内積および回転量成分の大小関係に基づいてロボットを回転駆動する際の回転方向を経由点姿勢に近づける方向に設定することで、遠回りの回転方向が選択肢に組み込まれ、１８０°を超えるような回転が可能となる。そして、実際に回転駆動した際の経由点での姿勢を教示された経由点での姿勢に近づけることができる。したがって、補間による制御を行う場合において経由点での姿勢が教示された姿勢から大きくずれることを抑制できる。

また、回転量成分のスカラー値を直接用いて大小関係を判定している。これは、オイラーベクトル表現とすれば、回転量成分が負の値を取ることはないためである。そのため、絶対値を取る等の処理が不要となり、純粋に回転量成分のスカラー値の大小関係で判定を行うことができ、処理を簡略化することができる。そして、処理が簡略化されることで、判定を短期間で行うことができる。

また、教示点の数を増やすことなく経由点での姿勢を教示された姿勢に近づけることができるため、小数の教示点を設定するだけでロボットの動作軌跡全体を設定できるというメリットを享受することができる。つまり、ユーザの負担を増やすことなく、補間制御のためのティーチングを行うことができる。
また、本実施形態では、手先軸に対応する座標系を対象として回転方向の判定を行っている。このため、手先軸の向きが教示された経由点での手先軸の向きに近づくように回転方向を設定することで、教示された姿勢に近づけることができる。すなわち、実際の動作軌跡における経由点での姿勢を、よりユーザが希望する姿勢に近づけることができる。

実施形態のコントローラ３は、オイラーベクトルを求めるベクトル取得手段と、オイラーベクトルの内積を求めて内積の正負を判定する判定手段と、オイラーベクトルの回転量成分の大小関係を比較する比較手段と、判定手段の判定結果および比較手段の比較結果に基づいてロボットを回転駆動する際の回転方向を設定する設定手段として機能する。これにより、上記した制御方法を採用したロボットの制御装置を実現でき、実際にロボット２を制御する際に１８０°を超える回転方向を選択でき、教示された姿勢に近づけることができる等、上記した制御方法で述べたような効果を得ることができる。
る。つまり、上記した制御方法で得られる効果を得ることができる。

また、コントローラ３は、手先軸の座標系を対象として回転方向の判定を行っているため、上記したように、手先の姿勢を教示された姿勢に近づけることができる。
なお、本発明は一実施形態に限定されるものではなく、その要旨を逸脱しない範囲で任意に変形または拡張することができる。

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

Claims (4)

1. 教示された始点、経由点および終点の３点の教示点間を曲線で補間して多関節型のロボットの動作軌跡を求める際に、
   始点から終点までの姿勢変化を表すオイラーベクトルＶ１と、始点から経由点までの姿勢変化を表すオイラーベクトルＶ２とを求め、
   オイラーベクトルＶ１とオイラーベクトルＶ２との内積を求めて内積の正負を判定し、
   オイラーベクトルＶ１とオイラーベクトルＶ２との内積が負であれば、始点から終点まで姿勢を変化させた際の姿勢の変化量が相対的に大きくなる回転方向を、ロボットが回転駆動する際の回転方向に設定し、
   オイラーベクトルＶ１とオイラーベクトルＶ２との内積が正であれば、オイラーベクトルＶ１の回転量成分θ_αとオイラーベクトルＶ２の回転量成分θ_βとの大小関係を比較し、
   回転量成分θ_αが回転量成分θ_βよりも大きければ、始点から終点まで姿勢を変化させた際の姿勢の変化量が相対的に小さくなる回転方向を、ロボットが回転駆動する際の回転方向に設定する一方、回転量成分θ_αが回転量成分θ_βよりも小さければ、始点から終点まで姿勢を変化させた際の姿勢の変化量が相対的に大きくなる回転方向を、ロボットが回転駆動する際の回転方向に設定することを特徴とするロボットの制御方法。
2. 手先軸の座標系を対象として回転方向の判定を行うことを特徴とする請求項１記載のロボットの制御方法。
3. 多関節型のロボットを制御するロボットの制御装置であって、
   始点から終点までの姿勢変化を表すオイラーベクトルＶ１と、始点から経由点までの姿勢変化を表すオイラーベクトルＶ２とを求めるベクトル取得手段と、
   オイラーベクトルＶ１とオイラーベクトルＶ２との内積を求めて内積の正負を判定する判定手段と、
   オイラーベクトルＶ１の回転量成分θ_αとオイラーベクトルＶ２の回転量成分θ_βとの大小関係を比較する比較手段と、
   前記判定手段の判定結果および前記比較手段の比較結果に基づいて、オイラーベクトルＶ１とオイラーベクトルＶ２との内積が負であれば、始点から終点まで姿勢を変化させた際の変化量が相対的に大きくなる回転方向を、ロボットを回転駆動する際の回転方向に設定し、オイラーベクトルＶ１とオイラーベクトルＶ２との内積が正であれば、オイラーベクトルＶ１の回転量成分θ_αとオイラーベクトルＶ２の回転量成分θ_βとの大小関係を比較し、回転量成分θ_αが回転量成分θ_βよりも大きければ、始点から終点までの姿勢の変化量が相対的に小さくなる回転方向をロボットが回転駆動する際の回転方向に設定する一方、回転量成分θ_αが回転量成分θ_βよりも小さければ、始点から終点までの姿勢の変化量が相対的に大きくなる回転方向をロボットが回転駆動する際の回転方向に設定する設定手段と、
   を備えることを特徴とするロボットの制御装置。
4. 前記ベクトル取得手段は、手先軸に対応する座標系における姿勢行列を対象としてオイラーベクトルを求めることを特徴とする請求項３記載のロボットの制御装置。

Images