JP2000190262A

JP2000190262A - ロボットの制御装置

Info

Publication number: JP2000190262A
Application number: JP10364389A
Authority: JP
Inventors: Shinji Iida; 慎二飯田; Koji Kamiya; 孝二神谷
Original assignee: Denso Corp
Current assignee: Denso Corp
Priority date: 1998-12-22
Filing date: 1998-12-22
Publication date: 2000-07-11
Also published as: US6166504A

Abstract

(57)【要約】【課題】ロボット制御において、関節を予め定められ
た範囲を大きく越えて動作することを防止する。【解決手段】サンプリング時点が到来すると、次のサ
ンプリング時点での関節の位置と速度を求める。そし
て、その速度で動作する関節を予め設定された減速度で
減速したときの停止までの動作量を求め、関節が上記次
のサンプリング時点での位置から上記動作量だけ動作し
たときの位置を求める。この減速による停止位置が予め
定められた制限位置を越えるとき、実際に関節を所定の
減速度で減速して停止させる。このように、関節の位置
指令値が或る範囲を越えたとき、関節を減速停止させる
のではなく、関節の動作速度を時々刻々把握し、その動
作速度に応じて減速を開始すべき位置を変えるので、関
節は、定められた動作範囲の限界位置の近辺で確実に停
止する。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は関節が予め定められ
た動作許容範囲を越えて動作することのないようにした
ロボットの制御装置に関する。

【０００２】

【発明が解決しようとする課題】多関節形の産業用ロボ
ットでは、多数の可動部を有し、それら可動部が関節結
合された構造となっている。そして、各関節（可動部）
は、駆動源である直流サーボモータなどのモータから適
宜の伝動機構を介して駆動トルクを受けて旋回動作など
を行うようになっており、それぞれの関節の動作の総合
としてロボット先端のハンドがプログラムされた通りの
動作を行う。

【０００３】このような産業用ロボットにおいて、特
に、直線補間によってロボット先端が２点間をほぼ直線
的に移動するように制御するような場合、各関節がどの
ような軌跡を辿るかを動作プログラムの作成時点で予測
することは困難である。このため、ロボットの試運転を
行う場合などでは、関節が思わぬ挙動を取り、周辺機器
に衝突したりするおそれがある。

【０００４】そこで、産業用ロボットでは、各関節が動
作する範囲に制限を加えていることが多い。この動作範
囲の制限のために、従来では、例えば、関節に許される
動作範囲（動作許容範囲）の限界位置にストッパを設
け、関節がそのストッパを越えて動作することのないよ
うにすると共に、動作プログラム上においては、関節の
位置指令値が上記の動作許容範囲よりも内側に存在する
通常の動作範囲を越えるか否かを監視し、位置指令値が
その通常の動作範囲を越えたとき、関節にブレーキをか
けて該関節を予め設定された減速度で減速し、停止させ
るように構成している。

【０００５】しかしながら、この従来の動作範囲制限方
式では、ストッパと通常の動作範囲との間の距離を十分
に取らないと、関節が十分に減速されないままストッパ
に衝突することがあり、最悪の場合には、衝突時の衝撃
でストッパが折損したり、機構部分が破損してしまうお
それがある。本発明は上記の事情に鑑みてなされたもの
で、その目的は、関節が予め定められた動作範囲の外側
に動作しようとする場合、その関節を動作範囲の限界位
置で停止させることができることができるロボットの制
御装置を提供するにある。

【０００６】

【課題を解決するための手段】本発明のロボットの制御
装置によれば、所定の監視時点が到来すると、その時点
での関節の位置と速度が求められる。次いで、監視時点
における速度で動作する関節が予め設定された減速度で
減速されたとき、停止するまでの動作量が演算により求
められ、そして、関節が上記監視時点おける位置から上
記動作量だけ動作したときの停止位置が同じく演算によ
り求められる。この演算による停止位置が予め定められ
た動作範囲を越えるとき、実際に関節にブレーキをかけ
て該関節を所定の減速度で減速して停止させる。

【０００７】このように、関節の位置指令値が予め定め
られた動作範囲を越えたとき、関節にブレーキをかけて
減速停止させるのではなく、関節の動作速度を所定の監
視時点が到来する度に把握し、その動作速度に応じてブ
レーキをかける位置を変えるので、関節が定められた動
作範囲を越えて動作しようとするとき、関節をその動作
範囲の限界位置で確実に停止させることができる。従っ
て、上記動作範囲を動作許容範囲よりも内側に定めてお
くことにより、関節を動作許容範囲内で確実に停止させ
ることができる。

【０００８】

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

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

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

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

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

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

【００１４】ＣＰＵ１１は、上記の動作プログラムに記
録されたパラメータに基づいて、ロボット先端の現在位
置（動作開始位置）から目標位置（動作終了位置）まで
の速度パターンを例えば台形パターンに当てはめて決定
し、その速度パターンに基づいて各関節の所定時間経過
毎の速度と位置を演算し、これを速度指令値および位置
指令値として駆動回路１２に与えるようになっている。

【００１５】すなわち、台形速度パターンは、図２
（ａ）に示すように、加速過程（Ｔ１）、最高速度での
等速過程（Ｔ２）、減速過程（Ｔ３）からなるが、この
台形の速度パターンとなるような動作角度（位置）のパ
ターンは図２（ｂ）に示すようになる。図２（ｂ）に示
す角度パターンにおいて、ｔn 時点での関節の角度をθ
n、このｔn 時点からΔｔ時間後のｔ(n+1) 時点での角
度をθ(n+1) とすると、Δｔ時間での角度の変化がｔ(n
+1) 時点での関節の角速度Ｖ(n+1) となり、これが速度
指令値となる。

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

【００１７】この実施例のロボットでは、ロボット先端
の動作軌跡を動作プログラムに飛び飛びの点として設定
し、直線補間によってロボット先端がそれら点間をほぼ
直線的に移動するように各関節を制御するようにしてい
る。そして、ＣＰＵ１１は、一つの動作を制御する場
合、前述のように、動作プログラムに記録されているパ
ラメータに基づいてロボット先端の速度パターンを決定
し、この速度パターンに基づいてサンプリング時点毎に
次のサンプリング時点におけるロボット先端の速度と座
標位置を求める。次いで、次のサンプリング時点でのロ
ボット先端の座標位置から各関節について次のサンプリ
ング時点での位置を求めると共に、各関節について今回
と次回のサンプリング時点における位置の差から速度を
求めるようにしているのである。

【００１８】このような直線補間方式を採用するロボッ
トにおいては、各関節がどのように動くかを動作プログ
ラムの作成時点で予測することが困難であるため、所定
の関節、例えばショルダ部５、下アーム６および上アー
ム７については、危険防止の観点から、メカニズムおよ
びソフトウエアの両面で動作の範囲に制限が加えられて
いる。

【００１９】メカニズム上での動作の範囲の制限は、各
関節にストッパを設けることによってなされる。すなわ
ち、各関節５〜７を支える関節、すなわちショルダ部５
についてはベース４、下アーム６についてはショルダ部
５、上アーム７については下アーム６には、それぞれ図
４に示すようなストッパ１８および１９が突設されてい
る。そして、各関節５〜７がストッパ１８，１９によっ
て定められる限界位置を越えて旋回動作しようとした場
合、その関節がストッパ１８または１９に当たって強制
的に停止されるようになっている。このストッパ１８，
１９の位置は、関節の動作許容範囲の限界位置に設定さ
れ、関節が動作許容範囲の限界位置（図４に二点鎖線お
よび破線で示す）まで動作したとき、当該関節がストッ
パ１８，１９に軽く接するような位置に定められてい
る。なお、図４において、符号２０は関節５〜７の旋回
中心軸を示す。

【００２０】ソフトウエア面での動作範囲の制限は、往
復旋回する各関節５〜７について、前記動作許容範囲よ
りも内側に定められた通常動作範囲が設定されており、
関節の速度をリアルタイムで把握し、或る減速度で減速
させたとき、停止する位置がその通常動作範囲を越える
場合、その時点で当該関節を上記減速度で減速して停止
させることによって行われる。

【００２１】すなわち、関節５〜７の速度および位置
は、ＣＰＵ１１によって前記サンプリング時点毎に把握
されている。そして、この実施例では、所定の監視時点
を、極く短時間毎、例えば８ｍ秒毎に到来するサンプリ
ング時点に定め、ここで次のサンプリング時点での関節
の位置と速度を取得する。そして、各関節５〜７をそれ
ぞれに予め設定された減速度で減速したときの停止位置
を演算により求め、その演算により求めた停止位置が前
記通常の動作範囲を越えるとき、実際にその関節にブレ
ーキをかけて該関節を減速停止させるようにしている。

【００２２】ここで、関節を或る減速度で減速させるに
は、その駆動用のモータ１０に逆トルクを発生させて該
モータ１０をブレーキ手段として機能させることにより
行う。モータ１０に逆トルクを発生させることにより、
関節に対し、現在の動作方向とは逆方向の駆動力を作用
させて当該関節にブレーキをかけるものである。そし
て、この場合の関節の減速度はモータ１０に発生する逆
トルクと、モータ１０の回転を関節に伝達する伝動機
構、関節の重量などに応じて、関節毎に一定値に定めら
れている。

【００２３】上記ソフトウエア上での動作範囲の制限に
おいて、関節の現在速度をＶ0 、現在位置をＰ0 とする
と、減速度βで減速した場合の停止位置Ｐｓは次のよう
にして求めることができる。まず、減速を開始してから
停止するまでに要する時間ｔは次の（１）式で求めるこ
とができる。ｔ＝Ｖ／β …… （１）そして、減速してから停止までの動作量Ｌ（旋回角度）
は次の（２）式で求めることができる。Ｌ＝（１／２）βｔ^２ …… （２）停止位置は関節の旋回動作について時計回り方向をプラ
ス、反時計回り方向をマイナスと定めると、停止位置Ｐ
ｓは、関節の旋回動作方向が時計回り方向のとき下記
（３）式、反時計回り方向のとき下記（４）式で求める
ことができる。Ｐｓ＝Ｐ0 ＋（１／２）βｔ^２ …… （３）Ｐｓ＝Ｐ0 −（１／２）βｔ^２ …… （４）

【００２４】このようにして求められる停止位置Ｐｓが
予め定められた通常の動作範囲を越える場合、その時点
で実際に関節にブレーキをかけて停止させるようにすれ
ば、関節が通常動作範囲を大きく越えて動作することを
防止でき、その通常動作範囲の限界位置で確実に停止さ
せることができる。

【００２５】以下、制御の内容を図１のフローチャート
をも参照しながら説明する。ＣＰＵ１１は、一つの動作
に対する制御を開始すると、まず、ステップＳ１で、動
作プログラムから目標位置の座標、速度および加減速度
の両係数などのパラメータを読み出し、それらパラメー
タから速度パターンを作成する（速度パターン作成手
段）。

【００２６】速度パターンは、動作開始から終了までの
ロボット先端の移動量Ｌ、加速度α、最高速度Ｖmax 、
減速度βに基づいて図２（ａ）に示す台形パターンに当
てはめて決定される。ここで、加速度αと減速度βは、
モータ１０の最大許容加速度および最大許容減速度に加
減速係数を乗じて求められる。また、移動量Ｌは、パラ
メータの一つとして動作プログラムから得た目標位置と
検出回路１３の位置情報から求められる現在位置とから
演算される。

【００２７】速度パターンを決定すると、ＣＰＵ１１
は、所定時間Δｔが経過する都度、次の所定時間経過時
点（サンプリング時点；監視時点）でのロボット先端の
位置を演算し、このロボット先端の位置から各関節の位
置を演算する。そして、ＣＰＵ１１は、前述したように
して各関節の位置指令値および速度指令値を演算し（位
置取得手段、速度取得手段）、それらの指令値を駆動回
路１２に出力する（以上、ステップＳ２）。

【００２８】この後、ＣＰＵ１１は、動作範囲を確認す
る必要のある関節５〜７のうちから一つの関節を予め定
められた順番に従って選択し（ステップＳ３）、その選
択した関節がステップＳ１で求めた速度指令値で動作し
ているとし、該関節が減速度βで減速された場合、減速
開始から停止までに要する時間ｔを前記（１）式により
演算し、次に前記（２）式により減速開始時点から停止
時点までの関節の動作量（旋回角度）を演算する（以
上、ステップＳ４）。

【００２９】次に、ＣＰＵ１１は、関節の動作方向がプ
ラス方向であるか否かを判断し（ステップＳ５）、プラ
ス方向であるときには（ステップＳ５で「ＹＥＳ」）、
前記（３）式により、また、マイナス方向であるときに
は（ステップＳ５で「ＮＯ」）、前記（４）式によりそ
れぞれ関節の停止位置を演算する（ステップＳ６または
ステップＳ８）。

【００３０】その後、ＣＰＵ１１は、上述のようにして
演算により求めた停止位置が前記通常動作範囲のプラス
側限界である上限値、またはマイナス側限界である下限
値を越えているか否かを判断する（判定手段；ステップ
Ｓ７またはステップＳ９）。そして、停止位置が上限値
または下限値を越えている場合（ステップＳ７で「ＹＥ
Ｓ」またはステップＳ９で「ＹＥＳ」）、ＣＰＵ１１
は、ステップＳ１２に移行し、ここでエラー検出動作を
行って実際に全ての関節の駆動回路１２に減速停止指令
を出力し（停止制御手段）、エンドとなる。

【００３１】各駆動回路１２は減速停止指令を受ける
と、モータ１０を予め定められた大きさの逆トルクが発
生するように制御する。これにより、各関節にブレーキ
がかけられて一定の減速度で減速され、やがて停止す
る。この場合、演算により通常の動作範囲の上限値また
は下限値を越えると判定された関節は、演算で用いた減
速度βで減速されるようになっており、従って、その関
節は、通常動作範囲の限界位置近辺で停止し、ストッパ
１８，１９に衝突するような事態は生じない。

【００３２】一方、演算により求めた関節の停止位置が
通常動作範囲の上限値または下限値を越えていない場合
（ステップＳ７で「ＮＯ」またはステップＳ９で「Ｎ
Ｏ」）、ＣＰＵ１１は、ステップＳ１０に移行して動作
範囲の確認が必要とされた全ての関節について動作確認
したか否かを判断し、未だ確認されていない関節がある
場合には、ステップＳ１０で「ＮＯ」となって前記ステ
ップＳ２に戻り、次の関節について上述のような動作範
囲の確認を行う。

【００３３】そして、全ての関節について動作範囲の確
認を終了すると、ＣＰＵ１１は、ステップＳ１０で「Ｙ
ＥＳ」と判断してステップＳ１１に移行し、ここでロボ
ット先端が目標位置に到達したか否かを判断する。目標
位置到達前のときには、ＣＰＵ１１は、ステップＳ１１
で「ＮＯ」となって前記ステップＳ２に戻る。そして、
前述したと同様にして次のサンプリング時点でのロボッ
ト先端の位置を求めると共に、各関節の指令速度および
指令位置を演算し、上述したと同様にして各関節５〜７
の動作範囲の確認を実行する、という動作を目標位置に
到達するまで継続する。

【００３４】このように本実施例によれば、各関節の動
作速度をリアルタイムに把握し、その時点で所定の減速
度で減速したときの停止位置を演算し、その停止位置が
通常動作範囲を越える場合、その時点で関節にブレーキ
をかけて減速停止させるように構成したので、関節は通
常動作範囲をほんの僅か越えた位置で確実に停止するこ
ととなる。

【００３５】この場合、関節の位置が通常動作範囲を越
えないように構成することにより、関節の現在位置が通
常動作範囲の限界位置となると、それまでの減速度より
大きな減速度でブレーキをかけて瞬時に関節を停止させ
るようにすることで、通常動作範囲内で停止させること
ができる。以上のことから、関節が動作許容範囲を越え
て動作することを確実に防止することができる。

【００３６】このことを例示すれば、今、関節にブレー
キをかけた場合に停止する位置が通常動作範囲の限界位
置Ｐｄを越えると演算されたとき、そのときの速度指令
値がＶ１、或いはＶ２（Ｖ１＞Ｖ２）であるとした場
合、図３に示すように、速度Ｖ１のときには通常動作範
囲の限界位置ＰｄよりＬ１だけ手前、速度Ｖ２のときに
は通常動作範囲の限界位置ＰｄよりＬ２（Ｌ１＞Ｌ２）
だけ手前でブレーキがかかり、そして、関節が限界位置
Ｐｄに至ると大きなブレーキがかかって停止することと
なり、いずれの場合も、通常動作範囲の限界位置Ｐｄで
停止するようになる。

【００３７】なお、本発明は上記し且つ図面に示す実施
例に限定されるものではなく、以下のような拡張或いは
変更が可能である。動作量の演算に用いる関節の速度Ｖ
は、速度指令値に限られず、検出回路１３から与えられ
る現在速度であっても良い。同様に動作範囲の内外の判
定に用いる位置Ｐ０は、位置指令値に限られず、検出回
路１３から与えられる現在位置であっても良い。各関節
の減速度は、例えば動作速度、負荷、ロボット本体１の
位置、姿勢などの動作条件によってリアルタイムに切り
換えるように構成しても良い。本発明は、垂直多関節の
ロボットに限られず、水平多関節型、直交座標型など各
種のロボットに適用することができる。

【図面の簡単な説明】

【図１】本発明の一実施例における動作範囲の確認用の
フローチャート

【図２】速度パターンとその速度パターンでの動作角度
を示すグラフ

【図３】動作速度と減速開始位置との関係を示すグラフ

【図４】ストッパによる関節の動作範囲の制限を示す図

【図５】ロボット制御装置の電気的構成を示すブロック
図

【図６】ロボットの全体を示す斜視図

【符号の説明】

１はロボット本体、２はロボット制御装置、５〜９は可
動体（関節）、１０はモータ、１１はＣＰＵ（位置取得
手段、速度取得手段、動作量取得手段、判定手段、停止
制御手段）である。

Claims

【特許請求の範囲】

【請求項１】所定の監視時点の到来の都度、その監視
時点での関節の位置を求める位置取得手段と、前記監視時点での前記関節の速度を求める速度取得手段
と、この速度取得手段により求められた速度で動作する前記
関節を所定の減速度で減速したとき、減速開始から停止
までの当該関節の動作量を演算する動作量取得手段と、前記関節が前記位置取得手段により求められた位置から
前記動作量取得手段により求められた動作量だけ動作し
たとき、その停止位置が予め設定された動作範囲を越え
るか否かを判定する判定手段と、この判定手段により前記関節の停止位置が前記動作範囲
を越えると判定されたとき、前記関節を前記所定の減速
度で減速して停止させる停止制御手段とを具備してなる
ロボットの制御装置。