JP4736607B2 - ロボット制御装置 - Google Patents

Description

本発明は間隔をおいて与えられた複数の指令位置をスプライン補間機能により曲線補間してロボットアーム先端の移動経路を設定するようにしたロボット制御装置に関する。

プレイバック型のロボットでは、通常、ティーチング装置により、ハンドが取り付けられるロボットアーム先端の移動の始点、通過点、終点を指令位置として教示する。ロボットの制御装置は、教示された指令位置の間を曲線補間してロボットアーム先端を指令位置の点列に沿って滑らかに移動させる。この曲線補間の方式としては、従来から、スプライン関数を用いて補間する方式が良く採用されている（例えば、特許文献１、特許文献２）。

スプライン関数とは、パラメータｔの変動する範囲を例えば０から１としたとき、この範囲を幾つかの区間に分割し、各区間の曲線をそれぞれ異なった式で定義することによって、全体として０から１までの範囲で曲線を定義しようとするものである。この場合、一区間の曲線（スプライン曲線セグメント）は、その両端での条件（位置と接線ベクトル）だけによって指定される。そして、各スプライン曲線セグメントを次の３つの条件により接続して指令位置の点列を順番に通過する滑らかなスプライン曲線（移動経路）を求めるようにしている。
１）接続点が一致していること。
２）接続点で接線ベクトルが一致していること。
３）接続点で接線ベクトルの変化率が一致していること。
特開昭６４−８１０１２号公報 特開平７−６４６２０号公報

上記のように複数の指令位置を教示すると、ロボットの制御装置が指令位置の点列を順番に通過する滑らかなスプライン曲線を設定する。ところが、教示する指令位置の間隔が大小異なると、間隔の狭い区間で図７に示すようにループ曲線になることがまま生じるという問題があった。指令位置の間隔が等間隔ならば、このようなループ曲線となることはない。しかしながら、ロボットアーム先端の移動経路として、直線的な緩やかなカーブを描けばよい個所では、指令位置の間隔は大きく取っても良いが、しかし、急なカーブを描かねばならない個所では、指令位置の間隔は小さくしなければならない。

このように、ロボットアーム先端の移動経路として、緩やかなカーブの個所と急なカーブの個所とを含む場合に、ループを描く部分のないスプライン曲線を得るためには、緩やかなカーブの個所であっても、急なカーブの個所と同じような狭い間隔で指令位置を教示してゆかねばならなくなる。このことは、指令位置を教示する場合に、多数点を教示してゆかねばならないことを意味し、教示作業に多くの時間がかかるという問題があった。

本発明は上記の事情に鑑みてなされたもので、その目的は、複数の指令位置を教示する場合、指令位置間の間隔が大小異なっていても、スプライン曲線を設定した場合にループとなる個所を生成することのないロボット制御装置を提供することにある。

本発明者は、指令位置の間隔が大小異なる場合、指令位置間の間隔の狭い区間においてループを描くスプライン曲線となる原因が、各区間の曲線を接続する際、接続点で接線ベクトルが一致し、且つ、接続点で接線ベクトルの変化率（曲線の曲率）が一致するように接線ベクトルを決定するという制約条件にあることを見出した。

そこで、上記の目的を達成するために、本発明は、間隔をおいて与えられた複数の指令位置を曲線補間してロボットアーム先端の単位時間当たりの移動量を制御するロボット制御装置において、前記曲線補間のために前記指令位置を結ぶスプライン曲線を設定する設定手段と、設定された前記スプライン曲線上を前記ロボットアーム先端が移動するようにその単位時間当たりの移動量を演算する補間手段とを備え、前記設定手段は、隣り合う２つの前記指令位置の間を一区間としてそれら区間毎に、両端の前記指令位置を結ぶ直線の長さに比例する大きさのベクトルであって、当該区間の両端の前記指令位置を結ぶ直線と前の一区間についての両端の前記指令位置を結ぶ直線とがなす角を２分する方向を向く始点側接線ベクトル、および当該区間の両端の前記指令位置を結ぶ直線と後の一区間についての両端の前記指令位置を結ぶ直線とがなす角を２分する方向を向く終点側接線ベクトルを演算する接線ベクトル演算部と、前記区間毎に、前記接線ベクトル演算部によって演算された当該区間の前記始点側接線ベクトルおよび終点側接線ベクトルに基づいて前記スプライン曲線を演算する曲線演算部とを備えてなることとしたものである。

この構成によれば、各区間の始点（ロボットアーム先端の移動開始側の指令位置）および終点（ロボットアーム先端の移動終了側の指令位置）での接線ベクトルを、当該区間の始終点間を結ぶ直線の長さに比例する大きさで、且つ、当該区間の始終点間を結ぶ直線と、前の区間の始終点間を結ぶ直線、後の区間の始終点間を結ぶ直線とのなす角をそれぞれ二分する方向を向くベクトルに定めたので、各区間のスプライン曲線セグメントを滑らかに接続することができる。しかも、接続点の接線ベクトルの向きだけが一致することを条件とし、大きさおよび変化率の一致を条件としていないので、指令位置間の間隔が大小異なっていても、短い区間でループ曲線を描く恐れがない。

以下、本発明の一実施例を図１ないし図６を参照しながら説明する。
ロボットは、図６に示すように、ロボット本体１と、このロボット本体１を制御する制御装置２と、ティーチング装置としてのティーチングペンダント３を備えている。この実施例において、制御装置２の制御対象であるロボット本体１は、例えば垂直多関節型の作業用ロボットとして構成され、ベース４と、このベース４に水平方向に旋回可能に設けられたショルダ部５と、このショルダ部５に上下方向に旋回可能に設けられた下アーム６と、この下アーム６に上下方向に旋回可能に且つ捻り回転可能に設けられた上アーム７と、この上アーム７に上下方向に旋回可能に設けられた手首８とを備えており、手首８は先端部に捻り回転可能なフランジ９を備えている。このフランジ９には、図示はしないが、ワークを把持するハンド（図示せず）が取り付けられるようになっている。

そして、ショルダ部５の水平方向の旋回動作、下アーム６の上下方向の旋回動作、上アーム７の上下方向の旋回動作、上アーム７の回転動作、手首８の上下方向の旋回動作、フランジ９の回転動作は、それぞれ関節が、駆動源である例えば直流サーボモータからなるモータ１０（図５参照）により図示しない適宜の伝動機構を介して回転駆動されることにより行われる。

一方、図５に示すように、制御装置２は、制御部としてのＣＰＵ１１、各関節のモータ１０を駆動する駆動手段としての駆動回路１２、検出回路１３などを備えている。そして、ＣＰＵ１１には、ロボット全体のシステムプログラム等を記憶したＲＯＭ１４、ロボット本体１の動作プログラム等を記憶したＲＡＭ１５および前記ティーチングペンダント３を接続するためのインターフェース１６が接続されている。なお、図５では、ショルダ部５、下アーム６、上アーム７、手首８およびフランジ９を可動部として一つのブロックで示し、これに応じてそれらの関節の駆動源であるモータ１０も一台だけ示した。

上記検出回路１３は、各関節の現在位置（回転角度）および現在速度（回転速度）を検出するためのもので、この検出回路１３には、各関節を駆動するモータ１０に設けられたロータリエンコーダ１７が接続されている。ロータリエンコーダ１７は位置センサおよび速度センサを兼用するもので、各モータ１０の回転角度に応じたパルス信号を出力し、そのパルス信号は検出回路１３に与えられる。検出回路１３は、各ロータリエンコーダ１７からのパルス信号に基づいて各モータ１０ひいては各関節の現在位置を検出すると共に、単位時間当たり各ロータリエンコーダ１７から入力されるパルス信号数に基づいて各モータ１０ひいては各関節の現在速度を検出し、その位置および速度の情報は、各モータ１０の駆動回路１２およびＣＰＵ１１に与えられるようになっている。

そして、各駆動回路１２は、ＣＰＵ１１から与えられる位置指令値および速度指令値と検出回路１３から与えられる現在位置および現在速度とを比較し、その偏差に応じた電流を各モータ１０に供給してそれらを駆動する。これにより、ロボットアーム先端であるフランジ９の中心部が指令位置を順に通る経路を辿って動作し、種々の作業を行うものである。

さて、動作プログラムには、各モータ１０の速度係数および加減速度係数などのパラメータが記録されている。このうち、速度係数および加減速度係数とは、動作の最高速度および加減速度を各モータ１０の許容最大速度および許容最大加減速度に対する割合で定めたもので、許容最大速度および許容最大加減速度は、各モータ１０の負荷トルクが例えば許容最大トルクを越えることのないように、各モータ１０の回転を関節に伝達する伝動機構や各モータ１０の性能を考慮して設定されている。

ＣＰＵ１１は、上記の動作プログラムに記録されたパラメータに基づいて、ロボット先端の移動の開始位置から終了位置までの速度パターンを例えば台形パターンに当てはめて決定し、その速度パターンに基づいて各関節の所定時間経過毎の速度と位置を演算し、これを速度指令値および位置指令値として駆動回路１２に与えるようになっている。
すなわち、台形速度パターンは、図４（ａ）に示すように、加速過程（Ｔ１）、最高速度での等速過程（Ｔ２）、減速過程（Ｔ３）からなるが、この台形の速度パターンとなるような動作角度（位置）のパターンは図４（ｂ）に示すようになる。図４（ｂ）に示す角度パターンにおいて、ｔｎ時点での関節の角度をθｎ 、このｔｎ時点からΔｔ時間後のｔ（ｎ＋１）時点での角度をθ（ｎ＋１）とすると、Δｔ時間での角度の変化がｔ（ｎ＋１）時点での関節の角速度Ｖ（ｎ＋１）となり、これが速度指令値となる。

そして、ｔｎ時点からΔｔ時間後の関節の角度θ（ｎ＋１）は、ｔｎ時点の角度θｎ に、速度パターンから得られるｔ（ｎ＋１）時点の角速度Ｖ（ｎ＋１） にΔｔを乗じた値を加算することによって求めることができる。従って、速度パターンに基づいて、動作開始時点から所定のサンプリング時間（Δｔ）経過毎に、その経過時点を現在として次のサンプリング時点（Δｔ経過後に相当する時点）の速度を算出し、これにサンプリング時間を乗じた値を順次加算してゆけば、動作開始時点から動作終了時点までの間、サンプリング時間経過毎の角度を順次求めることができる。

この実施例のロボットでは、ロボットアーム先端の移動経路は、ティーチングペンダント３を用いて行う教示作業によって与えられる。この教示作業では、ロボットアーム先端が辿るべき軌跡上の複数の位置が指令位置として順に教示されると共に、各指令位置でのロボットアーム先端の姿勢が教示され、その教示された指令位置と姿勢は、ＲＡＭ１５に記憶される。そして、制御装置２は、実際のロボット作業に際して、与えられた複数の指令位置間を曲線補間してそれら指令位置を順に滑らかに辿る曲線を設定し（設定手段）、ロボットアーム先端がその曲線上を移動するように制御（補間手段）する。

なお、ロボットアーム先端の位置は、フランジ９に固定された３次元座標の原点がロボット座標上でどの位置にあるかによって示される。また、ロボットアーム先端の姿勢は、フランジ９に固定の３次元座標の３軸のうち、所定の２軸上の単位ベクトルがロボット座標上で示す向きによって定義される。この姿勢も、移動経路と同様な曲線補間によって滑らかに変化するように制御され、間欠的に与えられた指令位置で教示された姿勢を取るように、その姿勢を滑らかに変化させるようになっている。

さて、上記の複数の指令位置の曲線補間は、スプライン補間方式によって行われる。このスプライン補間の一例を図２によって説明する。図２において、Ｒ０，Ｒ１，……Ｒｎは、（ｎ＋１）個の指令位置であり、Ｒ０が移動開始位置、Ｒｎが移動終了位置である。また、指令位置Ｒ０〜Ｒｎの相互間を結んでいる曲線Ｇ１，Ｇ２……Ｇｎは、指令位置Ｒ０〜Ｒｎの相互間を一区間として各区間について定義されるスプライン曲線セグメントである。スプライン補間では、スプライン曲線セグメントに対応するスプラインセグメント関数により、指令位置を通るスプライン曲線を定義する。

各スプライン曲線セグメント上の各点は、３次元のロボット座標上の点として次の（１）〜（３）によって定めることができる。なお、ｔはパラメータである。
Ｘ（ｔ）＝ａ₃ｔ³＋ａ₂ｔ²＋ａ₁ｔ＋ａ ……（１）
Ｙ（ｔ）＝ｂ₃ｔ³＋ｂ₂ｔ²＋ｂ₁ｔ＋ｂ ……（２）
Ｚ（ｔ）＝ｃ₃ｔ³＋ｃ₂ｔ²＋ｃ₁ｔ＋ｃ ……（３）
この（１）〜（３）式をまとめると、スプライン曲線セグメント上の各点の位置は、次の（４）式に示す３次の一般式（スプラインセグメント関数）によって定義される。

よって、上記（４）式は、次の（９）式のようになる。

以上のように指令位置相互間を一区画としたとき、各区画のスプライン曲線セグメントは上記（９）式の一般式によって表されるが、それら各区画のスプライン曲線セグメントを接続するための条件として、本発明では、接線ベクトルＴ（ｋ−１）、Ｔｋを次の条件（ア）〜（ウ）により定める。

（ア）接続点が一致していること。
（イ）一区画の両端（始点側指令位置および終点側指令位置）の接線ベクトルの大きさは、両端を結ぶ直線の長さに比例する大きさに定め、且つ、両端の接線ベクトルの向きは、両端を結ぶ直線が、前の一区間についての両端を結ぶ直線および後の一区間について両端を結ぶ直線となす角を、それぞれ２分する方向に定めること。
（ウ）上記のように定めた始点側接線ベクトルの向きが前の区画の終点側接線ベクトルの向きと一致し、終点側接線ベクトルの向きが後の区画の始点側接線ベクトルの向きと一致すること。

この（ア）〜（ウ）の条件を具体的に図１に示す第２の区間（ｋ＝２；指令位置Ｒ１とＲ２の区間）を中心にして具体的に説明する。図１は図２の指令位置Ｒ０〜Ｒ３までを拡大して示している。この図１において、第１の区間（ｋ＝１）の両端（指令位置Ｒ０とＲ１）間を結ぶ直線をＬ１、第２の区間（ｋ＝２）の両端（指令位置Ｒ１とＲ２）間を結ぶ直線をＬ２、第３の区間（ｋ＝３）の両端（指令位置Ｒ２とＲ３）間を結ぶ直線をＬ３とする。

また、第１の区間の両端Ｒ０，Ｒ１の接線ベクトルをそれぞれＴ１ｓ（始点側接線ベクトル），Ｔ１ｅ（終点側接線ベクトル）、第２の区間の両端Ｒ１，Ｒ２の接線ベクトルをそれぞれＴ２ｓ（始点側接線ベクトル），Ｔ２ｅ（終点側接線ベクトル）、第３の区間の両端Ｒ２，Ｒ３の接線ベクトルをそれぞれＴ３ｓ（始点側接線ベクトル），Ｔ３ｅ（終点側接線ベクトル）とする。

すると、第１の区間の終点側接線ベクトルベクトルＴ１ｅ、第２の区間の始点側接線ベクトルＴ２ｓおよび終点側接線ベクトルＴ２ｅ、第３の区間の始点側接線ベクトルＴ２ｓおよび終点側接線ベクトルＴ２ｅは、次の（１０）式〜（１３）式のようになる。ここで、第１の区間は前の区間がないので、始点側接線ベクトルＴ１ｓについては、例えば大きさを直線Ｌ１の長さに比例係数ｒを乗じた値に定め、向きを直線Ｌ１と同じに定める。

上記（１０）式〜（１３）式において、ｒはＴ１ｓの大きさを定める際に使用したと同じ値の比例係数である。この比例係数ｒは、この実施例では１と定めている。
ここで、矢印付きのＰ０〜Ｐ３は、指令位置Ｒ０〜Ｒ３の位置ベクトルであるから、（１０）式および（１１）式において、小括弧内の第１式は直線Ｌ１に一致する方向の単位ベクトル、第２式は、直線Ｌ２に一致する方向の単位ベクトルをそれぞれ求めるものであり、同様に（１２）式および（１３）式において、小括弧内の第１式は直線Ｌ２に一致する方向の単位ベクトル、第２式は、直線Ｌ３に一致する方向の単位ベクトルをそれぞれ求めるものである。従って、（１０）式〜（１３）式において、小括弧内の式を演算することによって直線Ｌ１とＬ２とのなす角の差、直線Ｌ２とＬ３とのなす角の差が求められ、それに０．５を乗ずることによって直線Ｌ１とＬ２とのなす角を二分する方向、直線Ｌ２とＬ３とのなす角を二分する方向がそれぞれ求められる。

このようにして各区間についてそれぞれ始点側接線ベクトルおよび終点側接線ベクトルが求められる。そして、各区間について両端の接線ベクトルを求めた後、前述のようにして各区間のスプライン曲線セグメントを表す関数式の４つのスプラインパラメータを求める。すると、各区間の始点側接線ベクトルおよび終点側接線ベクトルが、それぞれ前の区間の終点側接線ベクトルおよび後の区間の始点側接線ベクトルに向きと一致する関数を求めることができ、その結果、各接続点（指令位置）において滑らかに連続するスプライン曲線（スプライン関数）を求めることができる。

次に、上記構成においてロボット作業を行う場合の作用を説明する。ロボット作業を行わせるに先立って、ティーチングペンダント３を用いて実際にロボット本体１を動かしてそのロボットアーム先端の移動開始位置、通過点および移動終了位置を指令位置として教示する。この指令位置および姿勢は、ＲＡＭ１５（記憶手段）に記憶される。なお、以下では説明を簡潔にするために、姿勢の制御についての説明は省略し、主として移動経路の制御について説明する。

ロボット作業を行わせるべく、スタート操作を行うと、ロボットの制御装置２は、まず、指令位置をＲＡＭ１５から読み出し（ステップＳ１）、移動経路の両端、つまり移動開始位置および移動終了位置の接線ベクトルを設定する（ステップＳ２：始終端ベクトル設定手段）。この場合、移動開始位置での接線ベクトルは、移動開始位置と次の指令位置とを結ぶ直線と同じ方向を向き、次の指令位置までの直線の長さに比例する大きさとする。また、移動終点位置での接線ベクトルは、直ぐ前の指令位置と移動終点位置とを結ぶ直線と同じ方向を向き、直ぐ前の指令位置と移動終点位置とを結ぶ直線の長さに比例する大きさとする。なお、この移動開始位置および移動終了位置での接線ベクトルの向きと大きさはこれに限られるものではなく、前後の作業を考慮して定めることができ、また、大きさは０であっても良い。

次に、制御装置２は、移動開始位置から移動終了位置までを指令位置で複数区画に分割し、区間毎に前述の（１０）式〜（１３）式によって始点側接線ベクトルと終点側接線ベクトルとを演算する（ステップＳ３：接線ベクトル演算部）。そして、制御装置２は、各区間のスプライン曲線セグメントの関数式（スプラインセグメント関数）のベクトル係数を前述した（５）式〜（８）式によって求める（ステップＳ４：曲線演算部）。

このようにして各区間のスプラインセグメント関数のベクトル係数が求まると、移動始点位置から移動終点位置までの全指令位置を滑らかな曲線で結ぶスプライン曲線が求められたこととなるので、その後、制御装置２は、このスプラインセグメント関数である（９）式を用いて、具体的には（９）式をｔ（例えば０≦ｔ≦１）で微分して求めた式を０から１まで積分することにより、移動開始位置から移動終了位置までの総曲線距離を計算する（ステップＳ５）。

求められた総曲線距離はロボットアーム先端の移動開始位置から移動終了位置までの移動距離であるから、次に、制御装置２は、総曲線距離から図４（ａ）に示す速度パターンを生成する（ステップＳ６）。この速度パターンは、ロボットアーム先端の速度が台形となるように定めるもので、ロボットアーム先端が移動開始位置から予め定められた一定の加速度で予め定められた最高速度となるまで速度を上昇させながら移動し、予め定められた最高速度に達したら、その最高速度で等速度移動し、そして、予め定められた減速度で減速して移動終了位置に至るような速度パターンを生成する。

速度パターンを生成すると、制御装置２は、速度パターンから一定のサンプリング時間内でのロボットアーム先端の移動距離を求め、今回のサンプリング時間内での移動距離を前のサンプリング時間内での移動終了位置にプラスして、今回のサンプリング時間でのスプライン曲線上の移動終了位置を求めると共に、ロボットアーム先端の姿勢を計算する（ステップＳ７）。そして、制御装置２は、サンプリング時間内でロボットアーム先端が移動開始位置から移動終了位置まで移動するように各アームの角速度および位置を計算し（ステップＳ８）、各アームのモータ１０に速度指令値および位置指令値を出力する（ステップＳ９）。そして、制御装置２は、上記のステップＳ７〜ステップＳ９を繰り返すことによって、ロボットアーム先端を移動終了位置まで動作させるものである。

このように本実施例によれば、各区間のスプライン曲線セグメントの両端（指令位置）の接線ベクトルを、その両端を結ぶ直線の長さに比例する大きさのベクトルであって、その両端を結ぶ直線が、前の一区間についての両端の前記指令位置を結ぶ直線および後の一区間についての両端を結ぶ直線となす角を、それぞれ２分する方向を向くように定めたので、指令位置間の間隔が大小異なっていても、スプライン曲線にループを描く部分が生ずることを防止できる。

これは、始端側接線ベクトルの向きが、より終点に近い方を向くように定まること、一区間の始点側接線ベクトルと終点側接線ベクトルとは、両端を結ぶ直線の長さに比例した同じ大きさとなるので、前後の区間の直線とのなす角が同じであれば、両端の間隔によらずスプライン曲線セグメントは相似となること、などによる。なお、本発明でループを生じないスプライン曲線を作成できることは、数学的にも証明される。

なお、本発明は上記し且つ図面に示す実施例に限定されるものではなく、以下のような拡張或いは変更が可能である。
比例係数ｒは、１に限られない。ｒが所定の大きさの範囲にあればループを描くことを防止できる。
垂直多関節型のロボットに限られず、ロボット一般に広く適用できる。

本発明の一実施例を示すもので、接線ベクトルを求める方法を示す図 指令位置とそれら指令位置を連ねた曲線を示す図 ロボットアーム先端の移動経路制御のフローチャート 速度パターンを示すグラフ ロボットの電気的構成を示すブロック図 ロボットの斜視図 従来の問題点を示す図２相当図

符号の説明

図面中、１はロボット本体、２は制御装置（設定手段、補間手段、接線ベクトル演算部、曲線演算部）、３はティーチングペンダント、９はフランジ（ロボットアーム先端）である。

Claims (1)

1. 間隔をおいて与えられた複数の指令位置を曲線補間してロボットアーム先端の単位時間当たりの移動量を制御するロボット制御装置において、
   前記曲線補間のために前記指令位置を結ぶスプライン曲線を設定する設定手段と、設定された前記スプライン曲線上を前記ロボットアーム先端が移動するようにその単位時間当たりの移動量を演算する補間手段とを備え、
   前記設定手段は、
   隣り合う２つの前記指令位置の間を一区間としてそれら区間毎に、両端の前記指令位置を結ぶ直線の長さに比例する大きさのベクトルであって、当該区間の両端の前記指令位置を結ぶ直線と前の一区間についての両端の前記指令位置を結ぶ直線とがなす角を２分する方向を向く始点側接線ベクトル、および当該区間の両端の前記指令位置を結ぶ直線と後の一区間についての両端の前記指令位置を結ぶ直線とがなす角を２分する方向を向く終点側接線ベクトルを演算する接線ベクトル演算部と、
   前記区間毎に、前記接線ベクトル演算部によって演算された当該区間の前記始点側接線ベクトルおよび終点側接線ベクトルに基づいて前記スプライン曲線を演算する曲線演算部とを備えてなることを特徴とするロボット制御装置。

Images