JP5458769B2 - ロボットの制御装置 - Google Patents

Description

本発明は、ロボットを実際に動作させたときの移動軌跡が教示点上を通過するような制御を行うことができるロボットの制御装置に関する。

従来からロボットの実際の動作制御は、前もって演算したサンプリングタイム毎の目標位置または目標速度に達するようにサーボモータを制御することによって行われる。
一方、間欠的に与えられた教示点を通ってロボット先端が移動するように制御する方法として、ＰＴＰ（point-to-point）制御方式とＣＰ（continuous path）制御方式とがある。ＰＴＰ制御方式は、教示点の相互間でロボット先端を直線的に移動させるもので、ロボット先端は教示点間で移動停止を繰り返す。このため、ＰＴＰ制御方式では、ロボットの動作時間が長くなる。

これに対し、ＣＰ制御方式は、ロボット先端を、教示点を通過するように連続的に移動させるもので、動作時間は短縮される。しかしながら、ＣＰ制御方式では、ロボット先端の移動軌跡が教示点上を必ずしも通過しない。これは、ＰＴＰ制御方式では、例えば、図４（ａ）および（ｂ）に示すように、教示点Ｐｎ，Ｐｎ＋１，Ｐｎ＋２の相互間で直線移動するような台形状の速度パターンを定め、この速度パターンから所定のサンプリング時間毎の位置を算出してロボットを動作させるが、ＣＰ制御方式では、図４（ｃ）に示すように、例えばＰＴＰ制御方式における教示点Ｐｎから教示点Ｐｎ＋１までの速度パターンにおける減速域と、教示点Ｐｎ＋１から教示点Ｐｎ＋２までの速度パターンにおける加速域とを一部ラップさせて速度パターンを決定するため、２つの速度パターンの減速域と加速域とがラップする範囲内に教示点Ｐｎ＋１が存在することとなってしまい、当該教示点Ｐｎ＋１を通らない移動軌跡が生成されてしまうからである。
この問題を解消し、ロボットを動作させたときのロボット先端の実際の移動軌跡を教示点にできるだけ近付けるようにする制御方式として、特許文献１に開示されたものがある。

特開平１−１０６１０７号公報

特許文献１に開示された制御方式は、教え込まれた複数の教示点を通過するように演算された目標経路によりロボットを動かしたとしたとき、この目標経路と教示点との間に生ずる誤差を求め、この誤差に基づいて教示点の位置を補正しておき、実際にロボットを動かすとき、補正後の教示点を通過するように目標経路を演算し、この目標経路により実際にロボットを動かしたとき、実際経路が複数の教示点上を通過するようにしたものである。
この教示点の位置の補正を行うためのフローチャートを図７に示す。なお、図７のフローチャートは、特許文献１に開示された発明に従ったものであるが、現実のロボットにおいて制御する場合のもので、特許文献１の図８に示されたフローチャートとは若干異なる。

図７のフローチャートによって特許文献１の制御方式を説明する前に、予備的知識としてロボット先端の回転位置の制御の一例を説明する。図１０（ａ）には６軸の垂直多関節型ロボットの先端部分が示されており、図１０（ａ）中、符号Ｆはロボット先端としてのフランジを示す。このフランジＦは、０°の位置から時計回り方向に３６０°、０°の位置から反時計回り方向に３６０°回転可能になっている。従って、フランジＦは合計２回転できるが、その２回転内の回転位置を複数の形態によって定義している。

この回転位置を定義する形態を説明するに、まず、フランジＦの基準位置を０°とし、この基準位置から時計回り方向を（−）、反時計回り方向を（＋）に定める。そして、基準位置から時計回り方向に半回転および反時計回り方向に半回転の１回転の範囲を−１８０°＜θ≦＋１８０°と定義し（形態１）、この形態１の範囲を超える時計回り方向の半回転を−３６０°＜θ≦−１８０°、反時計回り方向の半回転を＋１８０°＜θ≦＋３６０°（形態２）と定義する。

そして、実際にロボットを動かす場合、フランジＦの１回転の回転位置の定義を形態１で行うか、形態２で行うかを予め設定しておく。これにより、フランジＦの１つの姿勢が回転位置で例えば＋９０°と−２７０°の２つで定義されるところ、例えば形態１で制御すると予め決めておくことにより、＋９０°に一義的に決められるので、フランジＦの回転制御が円滑に行われるようになる。

さて、図７のフローチャートの実行に入ると、ロボットの制御装置は、まず、記憶部に記憶されている教示点データから、各教示点の位置と姿勢を取得する（ステップＢ１）。このとき、例えば教示点が図４（ａ）に示すＰｎ，Ｐｎ＋１，Ｐｎ＋２の３点であったとする（図８の式７）。なお、教示点データにおいて、教示点の位置はロボット座標のｘｙｚ座標値で表され、姿勢はロボット先端に定義された固有座標のｘｙｚ座標のｘｙｚ軸のロボット座標上での方向（座標上の値）で表される。

次に、制御装置は、教示点Ｐｎ，Ｐｎ＋１，Ｐｎ＋２の位置と姿勢のデータを同次変換して同次変換行列を取得する（ステップＢ２: 図８の式８）。式８において、ノーマルベクトル、オリエントベクトルおよびアプローチベクトルとは、例えば図１０（ａ）に示すロボット先端の３次元座標において、ロボット先端の捻り回転の中心軸（ｚ軸）方向の単位ベクトルをノーマルベクトル、このノーマルベクトルと直交する２つ軸（ｘ軸およびｙ軸）方向の単位ベクトルをそれぞれオリエントベクトルおよびアプローチベクトルという。また、位置ベクトルとは、ロボット座標において、当該ロボット座標の原点からロボット先端の座標の原点Ｏまでのベクトルをいう。

続いて、制御装置は、教示点Ｐｎ，Ｐｎ＋１，Ｐｎ＋２の位置と姿勢のデータを逆変換し、ロボット先端を教示点（姿勢を含む）に位置させるための各アームの回転角度（軸値）を求める（ステップＢ３：図８の式９）。この逆変換時において、解が２つ求まり、制御不能となる場合がある。これを避けるために、解が２つ求まった場合、いずれか一方の解を採用するように予めプログラミングされている。

ステップＢ３で各アームの軸値を求めた後、制御装置は、次に、ロボット先端を、ＰＴＰ制御方式によって教示点Ｐｎから教示点Ｐｎ＋１に直線移動させる場合の速度パターンおよび教示点Ｐｎ＋１から教示点Ｐｎ＋２に移動させる場合の速度パターンを求め、この速度パターンを使用してＣＰ制御方式によって教示点Ｐｎから教示点Ｐｎ＋１を経てＰｎ＋２まで連続移動（パス動作）させる場合のパス動作用速度パターンを生成し、このパス動作用速度パターンを用いて連続移動軌跡を生成する（ステップＢ４）。

このときの連続移動軌跡の生成は、パス動作用速度パターンから、所定のサンプリングタイムが経過する毎の各サンプリング時点での目標角度（または速度）を定め、この目標角度（または速度）となるように各アームの駆動モータを制御することによって行われる。そして、この制御によってロボット先端が通過する移動軌跡データをサンプリング時点毎に記憶しておく。この場合、移動軌跡データは、サンプリング時点毎の各アームの軸値で記憶される。

次に、制御装置は、生成した移動軌跡上の各点（サンプリング時点）を同次変換し、この同次変換行列式をステップＢ２で求めた教示点Ｐｎ＋１の同次変換行列式と比較して、移動軌跡上の各点のうち、教示点Ｐｎ＋１との間の距離がもっとも小さい点（近傍点）を探索する（ステップＢ５）。そして、制御装置は、探索した近傍点（図８の式１０）と教示点Ｐｎ＋１との位置と姿勢の差ｄＬおよび姿勢ｄＯ，ｄＡ（姿勢は２軸のベクトルの方向で特定できる。）を求め（図８の式１１）、その差が所定の閾値以内であるか否かにより、近傍点の位置および姿勢が教示点Ｐｎ＋１の位置および姿勢に十分に近いか否かを判定する（ステップＢ６）。

制御装置は、近傍点の位置および姿勢と教示点Ｐｎ＋１の位置および姿勢との差が所定の閾値を超えている場合には、当該近傍点の位置および姿勢と教示点Ｐｎ＋１の位置および姿勢との差に補正係数αを乗じて補正量を求める（ステップＢ７:図８の式１２）。次いで、教示点Ｐｎ＋１の位置および姿勢に上記補正量を加えた点を補正候補点とし、この補正候補点の位置および姿勢を求める（ステップＳ８: 図８の式１３）。

続いて、制御装置は、補正候補点でのロボットの形態をチェックし、補正候補点が教示点Ｐｎ＋１の補正教示点となったときに、実際にロボットを動かす際に、サンプリング時点毎の目標位置（または速度）を前もって演算するために補正候補点の位置および姿勢を逆変換したとき、解が２つ求まることのないこと、およびＰｎ，Ｐｎ＋１に代わる補正候補点，Ｐｎ＋２を通るように実際にロボットの動作制御を行ったとき、フランジが急に大きく回転するというような予期しない急な動作（以下、ひっくり返るような動作という。）を行わないことを確認し、解が２つ求まったり、ひっくり返るような動作を行ったりする恐れがあるならば、そのような事態が起きないように必要な処置を行う（ステップＢ９）。

このひっくり返るような動作を行う場合の例を説明すると、フランジが例えば形態１（−１８０＜θ≦＋１８０）の角度範囲で制御されているとき、Ｐｎ＋１でのフランジの回転位置が−１８０°に近い角度であり、補正候補点でのフランジの回転位置が−１８０°と＋１８０°の境界を越えて＋１８０°よりもやや小さい角度になったとする。すると、ＰｎからＰｎ＋１の補正候補点を経てＰｎ＋２に至る連続移動軌跡を生成した際、ＰｎからＰｎ＋１の補正候補点までの多数のサンプリング時点のうち、いずれかの隣合う２つのサンプリング時点で−１８０°と＋１８０°の境界を挟んだ２つの回転位置（図１０（ｂ）に示すａ位置とｂ位置）が現出される。説明を簡単にするために、ａ位置が−１７９度で、ｂ位置が＋１７９度であったとすると、形態１での制御では、−１８０°と＋１８０°の境界を通過できないので、フランジは、ａ位置から０°の位置を通過してｂ位置までほぼ１回転（３５８度）する。この１回転に近い回転は急峻に行われるので、予期しないひっくり返るような動作となる。

ロボットの形態をチェックした後、制御装置は、補正候補点の位置および姿勢を逆変換してロボット先端を補正候補点に位置させるための各アームの軸値を求め（ステップＢ１０: 図８の式１４）、その後、前述のステップＢ４に移行する。このステップＢ４で制御装置は、前述したと同様に、ロボット先端を、ＰＴＰ制御方式により、教示点Ｐｎから補正候補点に直線移動させる場合の速度パターンおよび補正候補点から教示点Ｐｎ＋２に直線移動させる場合の速度パターンを求め、この速度パターンを使用してＣＰ制御方式によって教示点Ｐｎ，補正候補点，Ｐｎ＋２を連続移動させた場合の連続移動軌跡を生成する。そして、前述のステップＢ５〜７を実行し、再度求めた近傍点の位置および姿勢を補正教示点とするか否かを判定し、補正教示点とすることができない場合には、補正教示点とすることができる近傍点が求まるまで、上述したと同様にして、ステップＢ８〜１１およびステップＢ４〜７を繰り返し実行する。

補正教示点とすることができる近傍点が求まった場合、制御装置は、その補正教示点の軸値データを順変換して当該補正教示点の位置および姿勢を座標上の値で求める（ステップＢ１１: 図８の式１５）。そして、実際にロボットを動作させる場合、Ｐｎ，Ｐｎ＋１に代わる補正教示点，Ｐｎ＋２の各点の位置および姿勢を逆変換し、そして、各アームの軸値を用いてＣＰ制御方式の速度パターンを生成し、この速度パターンを基にして各サンプリングタイム毎の目標角度（または速度）を演算し、ロボットを制御する。これにより、実際にロボットを動かした場合、ロボット先端がＰｎ，Ｐｎ＋１，Ｐｎ＋２を通過するようになる。

しかしながら、以上のような特許文献１による制御方式では、求めた補正候補点での位置および姿勢を形態チェックし、補正候補点でロボットが予期しないひっくり返るような動作をしないように必要な処置をしなければならないという不具合があった。

本発明は上記の事情に鑑みてなされたもので、その目的は、形態チェックを行わずとも、ロボットが予期しないひっくり返るような動作を行う恐れのないロボットの制御装置を提供することにある。

本発明では、各アームの回転角からロボット先端の位置および姿勢を求めることができること、つまり各アームの回転角はロボット先端の位置および姿勢を含んでいることを利用している。そして、実際にロボットを動かすに先立って、各アームの回転角を用いて連続移動軌跡を生成した際、補正教示点に設定された点を通る連続移動軌跡の各サンプリング時点における各アームの回転角データを、実際にロボットを動作させる際の目標回転角としてそのまま用いるので、ロボットの形態チェックをしなくと、ロボットが予期しない動作を行う恐れがない。

本発明の一実施形態における経路補正の原理説明図 補正教示点を求めるためのフローチャート 図２の各ステップにて使用する数式を示す図 サンプリング時点での目標位置の設定過程を示すもので、（ａ）は教示点を示す図、（ｂ）はＰＴＰ制御方式の速度パターン図、（ｃ）はＣＰ制御方式の速度パターン図、（ｄ）はサンプリング時点で設定される目標位置を示す図 ロボットの斜視図 制御装置を示すブロック図 公知の補正教示点を求めるためのフローチャート 図７の各ステップにて使用する数式を示す図その１ 図７の各ステップにて使用する数式を示す図その２ （ａ）はロボット先端の１回転の定義を説明するための斜視図、（ｂ）はひっくり返りを説明するための図

以下、本発明の一実施形態を図面に基づいて説明する。図５には、産業用ロボット装置１が示されている。この産業用ロボット装置１は、ロボット２と、このロボット２を制御する制御装置３と、この制御装置３に接続されティーチング作業を行なう際に使用するティーチングペンダント（ティーチング手段）４とから構成されている。このティーチングペンダント４は、各種の操作部４ａおよび表示器４ｂを備えている。

ロボット２は、例えば６軸の垂直多関節型ロボットからなるもので、ベース５と、このベース５に第１軸Ｌｃ−１を中心に水平方向に旋回可能に支持されたショルダ部６と、このショルダ部６に第２軸Ｌｃ−２を中心に上下方向に旋回可能に支持された下アーム７と、この下アーム７に第３軸Ｌｃ−３を中心に上下方向に旋回可能に支持された第１の上アーム８と、この第１の上アーム８の先端部に第４軸Ｌｃ−４を中心に捻り回転可能に支持された第２の上アーム９と、この第２の上アーム９に第５軸Ｌｃ−５を中心に上下方向に回転可能に支持された手首１０と、この手首１０に第６軸Ｌｃ−６を中心に回転（捻り動作）可能に支持されたフランジ１１とから構成されている。

ベース５、ショルダ部６、下アーム７、第１の上アーム８、第２の上アーム９、手首１０、フランジ１１は、ロボットにおけるアームとして機能し、ベース５を除く各アームは、下段のアームに対し関節、例えば回転関節によって順次回転可能に連結されている。なお、ロボット先端であるフランジ１１には、ワークを把持するハンドや、視覚検査のために用いるカメラなどのエンドエフェクタ（図示せず）が取り付けられるようになっている。

そして、各アーム６〜１１は、それぞれモータ１２（図６参照）により減速装置を介して回転されるようになっている。なお、図６では、モータ１２は１台しか示されていないが、実際には、各アーム６〜１１に１台ずつ設けられているものである。

ロボット２の動作を制御するロボット制御装置３は、図６に示すように、ＣＰＵを主体とする制御部１３と、記憶部１４と、位置検出部１５を備えている。制御部１３は、産業用ロボット装置１の全体を統括制御するもので、この制御部１３には、記憶部１４および位置検出部１５の他、前記ティーチングペンダント４およびモータ１２の駆動回路１６が接続されている。記憶部１４は、各アーム６〜１１を駆動するモータ１２についての最大加速度（＋α）、最大減速度（−α）、最高速度Ｖｍａｘなどのパラメータ、ロボット制御用の各種ソフトウエアが予め記憶されており、また、実際のロボット作業に際しては、ティーチングペンダント４により設定される動作プログラムやロボットを実際に動作させるに先立って生成されるロボット先端の移動軌跡情報などが記憶される。

位置検出部１５には、各アーム６〜１１の駆動源である各モータ１２の回転軸（図示せず）に連結されたロータリエンコーダ（回転位置検出手段）１７が接続されている。前記位置検出部１５は、ロータリエンコーダ１７から入力される回転検出信号に基づいてモータ１２の回転角度（軸値）を検出し、その回転位置情報を制御部１３に与える。制御部１３は、各アーム６〜１１のモータ１２の回転角度情報から各アーム６〜１１の回転角を演算し、各アーム６〜１１の回転角を目標角度と比較してその差分に応じた電流指令値を駆動回路１６に与える。すると、駆動回路１６は、与えられた電流指令値に応じた電流をモータ１２に供給し、これによりモータ１２の回転が制御され、アーム６〜１１が目標角度に回転される。なお、各アーム６〜１１の回転角は、モータ１２の回転数を減速装置の減速比で除することによって求めることができ、逆に、各アーム６〜１１を所望角度回転させるには、各アーム６〜１１の回転角度に減速装置の減速比を乗じた回転数で各モータ１２を回転させれば良い。

ベース５および各アーム６〜１１には、３次元の座標が規定されている。このうち、床面に据え付けられるベース５の座標系は、不動の座標系としてロボット座標（基準座標）とされるものである。アーム６〜１１の座標系は、各アーム６〜１１の先端部に規定されており、アーム６〜１１の回転によりロボット座標上での位置と向きが変化する。記憶部には、ロボット座標上におけるショルダ部６の座標位置、ショルダ部６の座標上における下アーム７の座標位置、下アーム７の座標上における第１の上アーム８の座標位置、第１の上アーム８の座標上における第２の上アーム９の座標位置、第２の上アーム９の座標上における手首１０の座標位置、手首１０の座標上におけるフランジ１１の座標位置、および各アーム６〜１１の長さが記憶されている。

そして、制御部１３は、各アーム６〜１１の座標の位置と向きとを、各アーム６〜１１の回転位置と各アーム６〜１１の長さと各アーム６〜１１の座標の相互位置関係から、座標変換の計算機能によりロボット座標上での位置と姿勢とに変換して認識することができるようになっている。なお、各アーム６〜１１の姿勢は、各アーム６〜１１に固有の座標軸上の単位ベクトルについてのロボット座標上での向きで表される。

また、逆に、ＣＰＵ１５は、動作プログラムからロボット先端であるフランジ１１の座標のロボット座標上での位置と姿勢が与えられると、各アーム６〜１１のモータ１２の回転位置を制御することにより、各アーム６〜１１の座標のロボット座標上での位置と姿勢を制御してフランジ１１が動作プログラム通りの位置と姿勢を取るように各アーム６〜１１を動作させることができるようにもなっている。

ところで、ロボット２を動作させるには、まず、ティーチングペンダント４をティーチングモードに設定し、ロボット先端が通過すべき点（教示点）の位置と、その位置で取るべき姿勢を教示する。このとき、教示点の位置と姿勢は、ロボット先端であるフランジ１１のロボット座標でのｘｙｚ座標値とフランジ１１の固有座標のオリエントベクトルおよびアプローチベクトルのロボット座標上での向き（ロボット座標上の値）で表される。教示点には、ロボット先端の移動開始点および移動終了点を含み、教示点の位置および姿勢は、教示データ記憶手段としての制御装置３の記憶部１４に記憶される。図４（ａ）は教示点のうち、移動開始点および移動終了点の間の途中の３つの教示点Ｐｎ，Ｐｎ＋１，Ｐｎ＋２を示しており、Ｐｎが移動開始点側の教示点、Ｐｎ＋２が移動終了点側の教示点である。

教示作業を終了した後、ティーチングペンダント４を学習モードに設定する。すると、制御装置３の制御部１３が図２に示されたフローチャートに従ってロボット先端の連続移動軌跡を演算する。この図２のフローチャートの概略を述べれば、まず、連続移動軌跡が当初教え込まれた教示点を用いて演算され、その演算された連続移動軌跡が教示点からずれていた場合には、教示点の位置を補正し、補正された教示点を用いて連続移動軌跡を再度演算するという動作を、連続移動軌跡が当初教え込まれた教示点を通過するようになるまで繰り返す。そして、最終的に求められた連続移動軌跡を辿るようにロボットの動作を制御すれば、実際のロボット先端の移動軌跡が当初教え込まれた教示点を通過するようになるというものである。

さて、学習モードに入って図２のフローチャートを実行し始めると、制御部（教示点補正手段）１３は、まず、記憶部１４に記憶されている複数の教示点の位置および姿勢のデータ（教示点データ）を読み出す（ステップＡ１）。この教示点データは、図３の式１に示すように、ロボット座標上での位置と姿勢よって表されているので、次に制御部（回転角取得手段）１３は、教示点データを逆変換して各教示点（移動開始点，…，Ｐｎ，Ｐｎ＋１，Ｐｎ＋２，…，移動終了点）における各アーム６〜１１の回転角度（軸値）を求める（ステップＡ２：図３の式２）。この逆変換時に、解が２つ求まる場合があるが、本実施形態では、＋側と−側のうち、例えば＋側を解として採用するようにプログラミングされており、これにより、解が２つ存在することによって制御不能に陥ることが防止される。

続いて、制御部（移動軌跡取得手段）１３は、この軸値を用いて連続移動軌跡を生成する（ステップＡ３）。なお、以下では、連続移動軌跡の生成を図４に示す３つの教示点Ｐｎ，Ｐｎ＋１，Ｐｎ＋２に適用して説明する。連続移動軌跡の生成は、ロボット先端を、ＰＴＰ制御方式によって移動開始点側の教示点Ｐｎから次の教示点Ｐｎ＋１に直線移動させる場合の角速度パターン、同じく教示点Ｐｎ＋１から次の教示点Ｐｎ＋２に直線移動させる場合の角速度パターンを求めることから始められる。角速度パターンは、三角パターン或は台形パターンに生成される。図４（ｂ）は、教示点Ｐｎと教示点Ｐｎ＋１との間の角速度パターンおよび教示点Ｐｎ＋１と教示点Ｐｎ＋２との間の角速度パターンが共に台形パターンで生成された例を示している。勿論、このような２つの角速度パターンは、各アーム６〜１１毎に生成される。

角速度パターンは、基本的には台形パターンとなる。台形パターンは、加速域、定速域、減速域からなり、その台形によって囲まれた面積が２つの教示点間の距離となるように、加速域、定速域、減速域の時間Ｔ１，Ｔ２，Ｔ３が決められる。この場合の加速域および減速域での加速度および減速度は記憶部１４に予め記憶されている加速度α、減速度−α、最高速度Ｖｍａｘが用いられる。そして、２つの教示点間の距離が短い場合には、加速域の終了時点でＶｍａｘに達すれば、定速域がなく直ちに減速域に入り、或は、加速域でＶｍａｘに達する前に減速域に入ることがあり、この場合には、角速度パターンは三角パターンとなる。

次に、制御部１３は、教示点Ｐｎと教示点Ｐｎ＋１との間の角速度パターン、教示点Ｐｎ＋１と教示点Ｐｎ＋２との間の角速度パターンを使用して、ロボット先端をＣＰ制御方式により教示点Ｐｎから教示点Ｐｎ＋１を経由してＰｎ＋２まで連続移動（パス動作）させる場合の角速度パターンを生成する。このパス動作用の角速度パターンは、ＰＴＰ制御方式によるものとして求めた２つの角速度パターンの形態、つまり、２つとも台形パターンであるか、一方が台形パターンで他方が三角パターンであるか、或は２つとも三角パターンであるか、更には加速域、定速域、減速域の所要時間によってどのような角速度パターンとするかが決められている。図４（ｃ）は（ｂ）に示されている２つの角速度パターンの減速域と加速域の一部をラップさせるようにしてパス動作用の角速度パターンを生成した例を示している。

パス動作用の角速度パターンを生成すると、制御部１３は、その角速度パターンを所定のサンプリング時間（Δｔ）毎に分けて、各サンプリング時点毎（…，ｔｎ，ｔｎ＋１，…）に軸値（…，θｎ，θｎ＋１，…）を求める（図４（ｃ），（ｄ）参照）。このサンプリング時点毎の軸値によってパス動作用の連続移動軌跡が生成されたこととなる。図１（ａ）に、生成された連続移動軌跡Ｌ０を示す。

連続移動軌跡を生成すると、次に、制御部（近傍点探索手段）１３は、連続移動軌跡の各点（サンプリング時点毎の軸値）のうちから、ステップＡ２で求めたＰｎ＋１の軸値に最も近い点を探索し、この点を近傍点とする（ステップＡ４）。この近傍点は図３の式３によって表される。
次に、制御部（比較手段、判定手段）１３は、Ｐｎ＋１の軸値と近傍点の軸値との差を、図３の式４により求め、この差が予め定められた閾値以下であるか否かを判定する（ステップＡ５）。

差が閾値を超えていた場合、制御部１３は、次に、近傍点における各アーム６〜１１の軸値と教示点Ｐｎ＋１における各アーム６〜１１の軸値との差に補正係数αを乗じて補正量を求める（ステップＡ６：図３の式５）。なお、αは経験的に定められる所定値である。

次に、制御部１３は、教示点Ｐｎ＋１における各アーム６〜１１の軸値に上記補正量を加えた点（補正候補点Ｐ′ｎ＋１）における各アーム６〜１１の軸値を求める（ステップＡ７）。この補正量を教示点Ｐｎ＋１の軸値に加える演算式は、図３の式６に示されている。図１（ｂ）には、求めた補正候補点をＰ′ｎ＋１で示している。

続いて、制御部１３は、前述のステップＡ３に戻り、教示点Ｐｎ＋１を補正候補点Ｐ′ｎ＋１に替えて前述したと同様に教示点Ｐｎ、補正候補点Ｐ′ｎ＋１、教示点Ｐｎ＋２を用いて連続移動軌跡を生成する。新たに生成された連続移動軌跡を図１（ｃ）にＬ１で示している。そして、この連続移動軌跡の各サンプリング時点の軸値のうち教示点Ｐｎ＋１に最も近い軸値を近傍点とし、教示点Ｐｎ＋１と近傍点との軸値の差を求め、その差が閾値以下であるか否かを判定する（ステップＡ４、ステップＡ５）。

差が閾値を超えていた場合には、制御部（再試行実施手段）１３は、再度教示点Ｐｎ＋１と近傍点の軸値の差に補正係数αを乗じて補正量を求め、前回の補正候補点Ｐ′ｎ＋１における各アーム６〜１１の軸値に上記補正量を加えた新たな補正候補点Ｐ′ｎ＋１を求める。そして、教示点Ｐｎ、新たな補正候補点Ｐ′ｎ＋１、教示点Ｐｎ＋２の軸値を用いて連続移動軌跡を生成し、近傍点を求めて当該近傍点と教示点Ｐｎ＋１の差が閾値内であるか否かを判定する、という動作を近傍点と教示点Ｐｎ＋１の軸値の差が閾値内となるまで繰り返す（ステップＡ６，７，ステップＡ３，４，５）。

そして、近傍点と教示点Ｐｎ＋１の軸値の差が閾値内となった場合（ステップＡ５で「ＹＥＳ」）、制御部１３は、その近傍点を補正教示点と定め、教示点Ｐｎ、教示点Ｐｎ＋１に変えて補正教示点、教示点Ｐｎ＋２を用いて求めた連続移動軌跡の各サンプリング時点における各アーム６〜１１の軸値を、記憶部１４に目標値生成データとして記憶させる（ステップＡ８）。図１（ｄ）に、近傍点が教示点Ｐｎ＋１と閾値以下に近付いた連続移動軌跡Ｌｎを示している。

以上により学習モードを終了する。そして、実際にロボット２を動作させる場合、ロボット動作制御手段としての制御部１３は、記憶部１４に記憶された目標値生成データからサンプリング時点毎に各アーム６〜１１の軸値を読み出してこれを目標軸値とし、各アーム６〜１１の軸値が目標軸値となるように各モータ１２を制御する。これにより、実際にロボット２を動作させた時、ロボット先端の連続移動軌跡が教示点Ｐｎ、Ｐｎ＋１、Ｐｎ＋２を通過するようになる。

このように本実施形態によれば、学習モード時に、座標上の値で記憶された教示点データを逆変換することによって教示点の位置と姿勢を各アーム６〜１１の軸値に変換し、そして、連続移動軌跡を同じく軸値により求める。そして、連続移動軌跡の各点のうちから教示点に最も近い近傍点を探索する場合に、連続移動軌跡の各点における各アーム６〜１１の軸値と、教示点における各アーム６〜１１の軸値と比較するので、図７に示した特許文献１の制御方式とは異なり、連続移動軌跡の各点をロボット座標上の位置と姿勢に変換する必要がなく、制御部１３による演算を簡素化することができる。

しかも、実際にロボットを動かす場合に、学習モードにおいて軸値で求めた連続移動軌跡の各点における各アーム６〜１１の軸値を、そのまま各サンプリング時点における各アーム６〜１１の目標軸値として用いて、各アーム６〜１１のモータ１２を制御するので、学習モードにおいて、軸値で求められている補正教示点を順変換してロボット座標上の位置と姿勢に変換し、そして、実際にロボットを動かす際、サンプリング時点毎の目標軸値を求めるために、教示点Ｐｎ、補正教示点、教示点Ｐｎ＋２を逆変換する特許文献１の制御方式のものとは異なり、逆変換の必要がないことから、学習モードにおいて形態チェックの必要がなく、形態チェックをしておかなくとも、逆変換の解が２つ求まるといった不具合は本来的に生じない。

ところで、特許文献１の制御方式では、図１０に示すように、フランジの回転位置が例えば形態１で定義されている場合に、連続移動軌跡上の隣接する２点（ａ，ｂ）が−１８０度と＋１８０度との境界を越えることがあり、この場合には、ａ位置（−１７９度）からｂ位置（＋１７９度）に回転するとき、フランジがａ位置から０°を経由してｂ位置に至るため、フランジが急に１回転に近い角度回転するという予期しないひっくり返り動作を行う恐れがある（既述）。

このひっくり返りを防止するには、フランジの回転位置を、ａ位置では形態１で制御し、ｂ位置では形態２で制御するように変更すればよい。すると、ａ位置は形態１で制御され、ｂ位置は形態２で制御されるので、フランジはａ位置からｂ位置へ２度だけ回転するだけでよいこととなる。しかしながら、このようにするためには、連続移動軌跡を演算しながら補正教示点を求めて行く過程で、ロボットの形態チェックを行わねばならない。 この形態チェックは、連続移動軌跡の各点における各アームの軸値から座標上の位置と姿勢に変換して逐次チェックしてゆかなければならず、制御装置の負担が非常に重くなる。

これに対し、本実施形態では、学習モードで見つけた補正教示点を含む連続移動軌跡の各点における各アームの軸値を、実際にロボット２を動かす際の目標軸値としてそのまま使用するので、図１０に示す形態１で１回転を定義したり、形態２で１回転を定義したりしてフランジ１１の回転位置を制御せずとも済む。このため、ａ位置からｂ位置への移動に際しても、ｂ位置は−１８１度としてａ位置（−１７９度）に連続する軸値で与えられているので、学習モードのときに形態チェックを行わずとも、実際にロボット２を動かした際、フランジ１１がひっくり返るといった予期しない動作が起きる恐れがない。

なお、本発明は上記し且つ図面に示す実施形態に限定されるものではなく、以下のような拡張或は変更が可能である。
アームの軸値は、モータの回転角に比例するので、アームの軸値をモータの回転角に置換して演算するようにしても良い。
ティーチングペンダント４によって教示点の位置および姿勢を教示する場合、ロボット座標のｘｙｚ座標値とフランジ１１の座標のｘｙｚ座標軸のベクトルのロボット座標上の向きで記憶部１４に記憶させるのではなく、その位置および姿勢のデータを各アーム６〜１１の軸値で記憶させるようにしても良い。このようにした場合には、ステップＡ１で記憶部１４に記憶されている各アーム６〜１１の回転角を取得し、そして、ステップＡ３の各アーム６〜１１の軸値を用いて連続移動軌跡を生成するステップへ移行する。従って、ステップＡ２のステップは省略される。
６軸の垂直多関節型ロボットに限られない。

図面中、３は制御装置、４はティーチングペンダント、５はベース、６はショルダ部、７は下アーム、８は第１の上アーム、９は第２の上アーム、１０は手首、１１はフランジ（ロボット先端）、１２はモータ、１３は制御部、１４は記憶部（教示データ記憶手段、ロボット動作制御手段、教示点補正手段、回転角取得手段、移動軌跡取得手段、近傍点探索手段、比較手段、判定手段、再試行実施手段）、１５は位置検出部、１６は駆動回路、１７はロータリエンコーダを示す。

Claims (1)

1. 複数の回転動作するアームを備え、各アームをそれぞれの駆動モータにより駆動するロボットの制御装置において、
   ロボット先端が通過するとして教示された３点以上の教示点の位置および姿勢を、座標上の値または前記各アームの回転角にて示される教示データとして記憶する教示データ記憶手段と、
   前記ロボット先端が前記教示点のうち移動開始点となる教示点から移動終了点となる教示点に向かって連続的に移動するパス動作を行うように教示データを用いて前記ロボットを制御するロボット動作制御手段と、
   前記パス動作を行う前記ロボット先端の連続移動軌跡が前記途中の教示点に重なるように、当該途中の教示点の教示データを前記各アームの回転角を用いて補正して補正教示点を求めるとともに、当該補正教示点の教示データを用いてロボット動作制御手段前記パス動作の制御を行わせる教示点補正手段と、を備え、
   前記教示点補正手段は、
   前記教示データ記憶手段に教示データが前記座標上の値で記憶されているときには当該教示データ記憶手段に記憶されている教示データを逆変換して前記教示点での前記各アームの回転角を求め、または前記教示データ記憶手段に教示データが前記各アームの回転角で記憶されているときには当該教示データ記憶手段から前記教示点での前記各アームの回転角を取得する回転角取得手段と、
   前記パス動作をさせたとした場合における前記ロボット先端の前記連続移動軌跡を、教示データに基づいて求める移動軌跡取得手段と、
   前記移動軌跡取得手段が取得した前記連続移動軌跡上の各点のうち、前記各アームの回転角が前記途中の教示点の前記各アームの回転角に最も近い点を探索し、当該最も近い点を近傍点とする近傍点探索手段と、
   前記近傍点探索手段が探索した前記近傍点での前記各アームの回転角と前記回転角取得手段が取得した前記途中の教示点での前記各アームの回転角とを比較し、それらの回転角の差を比較結果として求める比較手段と、
   前記比較手段による比較結果に基づいて、前記近傍点と前記教示点との回転角の差が予め定められている閾値以下であるかを判定し、閾値以下であれば当該近傍点を前記補正教示点に設定すると判定する判定手段と、
   前記判定手段が前記近傍点を前記補正教示点に設定しなかった場合、前記近傍点の前記各アームの回転角と当該近傍点に対応する教示点の前記各アームの回転角との差に基づいて補正量を求め、当該補正量を前記連続移動軌跡を求めた教示点の回転角に加算することで補正候補点を求め、当該補正候補点の教示データを用いて前記移動軌跡取得手段に前記連続移動軌跡を求める動作を実行させるとともに、近傍点探索手段による前記近傍点の探索、前記比較手段による回転角の差の比較、および前記判定手段による閾値以下であるかの判定ならびに補正教示点に設定するかの判定を再度行わせる再試行実施手段と、を備え、
   前記ロボット動作制御手段に、当該補正教示点の回転角にて示される教示データを用いて前記パス動作の制御を行わせることを特徴とするロボットの制御装置。

Images