JP2735126B2

JP2735126B2 - ロボットの制御方法及びその制御装置

Info

Publication number: JP2735126B2
Application number: JP63327657A
Authority: JP
Inventors: 勝己石原; 正輝安原
Original assignee: Canon Inc
Current assignee: Canon Inc
Priority date: 1988-12-27
Filing date: 1988-12-27
Publication date: 1998-04-02
Anticipated expiration: 2013-04-02
Also published as: US5357598A; JPH02172685A

Description

【発明の詳細な説明】［産業上の利用分野］本発明は、ロボツト言語で記されたプログラムに従つ
て例えばロボツトアーム等の軌道を制御するためのロボ
ツト制御方法及びその装置に関し、より詳しくは、異な
る方向の移動動作を連続して行なうときのロボツト制御
の高速化と安全な軌道の両立を目指したものである。

［従来の技術］ロボツトの連続動作制御の高速化に関する従来技術と
して、例えば、特開昭58-177289号、特開昭60-132207
号、特開昭61-146487号等がある。

特開昭58-177289は、アーム位置を検出する手段と、
検出されたアーム位置を前もつて記憶された所定位置と
比較する手段を有し、アームがこの所定位置を通過した
ことをもつて、アーム動作途中でも、プログラムステツ
プを進行させることにより、制御の高速化を図つてい
る。更に詳しくいえば、この特開昭では、水平方向の移
動はDCサーボモータで行なわれ、垂直方向の移動はエア
シリンダにより行なわれる。エアシリンダの先端にはチ
ヤツク等が接続されている。水平方向の目的位置がＤ点
であれば、このＤ点の前方の点Ｄ′を通過して時点でエ
アシリンダを駆動して降下運動を開始する。このＤ′点
はエアシリンダが最下位位置に到達するのにかかる時間
から推定して決める。

また、特開昭60-132207の制御装置は３軸にDCサーボ
モータを用いている。そして、アーム位置を検出する手
段と、現在のアーム動作中に次の動作を開始してもよい
オーバラツプ領域を入力する手段と、アームがこのオー
バラツプ領域内にあるかを判断する手段を備え、オーバ
ラツプ領域にあるときは、現在の移動命令と次の移動命
令をオーバラツプさせることにより、制御の高速化を図
つている。このオーバラツプ領域の入力は操作者により
操作入力される。このオーバラツプ領域は移動方向の変
化点の近傍として定義されている。

また、特開昭61-146487は、互いに独立した垂直移動
制御回路と水平移動制御回路とを持ち、水平移動は垂直
移動からのタイミング信号で、又、垂直移動は水平移動
回路からのタイミング信号で移動を開始する。そして、
このタイミング信号は、下降移動が水平移動よりも後に
終了するように、水平移動の速度を検出して出力され
る。換言すれば、水平移動と下降移動が重なる場合に、
水平移動を先に終了させて、下降移動の終期には下降移
動のみを行なうようにして、制御の高速化とアームの破
損事故等の防止を図つている。

［発明が解決しようとしている課題］ところで、ロボツト制御においては、現在位置から目
標位置まで高速に移動すればよいというわけでなく、障
害物をクリアして目標位置に到達しなければならない。
換言すれば、障害物を回避できなければ、高速化には何
の意味もない。そもそも、上昇水平移動下降という
動作も、本来なら水平移動だけで目標地点に到達できる
ものの、その間に障害物があるために、このような動作
を強いられるのである。この事情は、特に産業用ロボツ
トにおいて、例えば複数の部品から完成品に組立てる場
合において、部品を把持したロボツトフインガーがその
部品の収容ケースを回避するように、さらに、フインガ
ー及び把持部品とが組立て途中の半完成品と衝突しない
ように制御しなければならないという点を考慮すれば了
解されるのであろう。

上記３つの従来例は、ある期間水平移動と垂直移動が
並行して行なわれる点に特徴がある。この２つの方向の
並行運動による軌跡は、その各々の方向の運動の速度の
合成により規定される。

例えば、第14図は特開昭61-146487で説明される２方
向の運動の軌跡である。垂直（Ｚ軸）方向で、移動を開
始してから目標位置に到達するまでにかかる時間をａと
し、水平（Ｘ）方向で速度ｖ₀から停止するまでにかか
る時間をｂとする。そして、降下運動の開始時点を、水
平方向の速度がｖになつた時点とすると、水平移動が終了した後に、アー
ムはｃだけ下降してから垂直移動が終了するようにな
る。ところで、上式は水平方向の減速運動が直線運動と
近似できると仮定して始めて成立する式であり、しかも
ｃが小さな量であることが大前提となつている。ところ
が、第14図に示すように、もし障害物100が前記並行運
動の軌跡途中に存在すればロボツトアームは破壊されて
しまう。そこで、従来では、オペレータが上記並行運動
の軌跡が障害物を回避できるようなものとなるように、
垂直方向の仮の目標位置を試行錯誤を繰返して設定し、
その後、最終の垂直方向目標位置までの垂直移動（の
み）を行なうようにしていた。ところが、この試行錯誤
の過程で、下降時の直線部が確保出来ない為に、ロボツ
トを治具やワークに衝突させる等の重大事故にもつなが
つていた。事故を避けるための一番安易な手法は、上記
並行運動の期間を短くすればよいのであるが、これでは
高速性に欠ける。これも、そもそも特開昭61-146487で
は並行運動期間を水平方向の減速速度と垂直方向の運動
にかかる時間とから定義するという点に主眼があるので
あり、障害物を回避して安全性を確保しつつ高速制御を
図るという観点が欠落しているからである。

特開昭58-177289の従来例では、前述したように、垂
直方向の移動はエアシリンダにより行なわれるのである
から、特開昭61-146487よりも安全性についてはさらに
不安定となる。

また、特開昭60-132207では、オーバラツプ領域は操
作者の勘により入力しなければならないから、安全性も
高速性も高いとはいえない。

そこで本発明は上記従来技術の欠点を解消するために
提案されたもので、その目的は異なる方向にロボツトを
連続動作制御するときに、短時間に目標位置に到達し、
且つ、ロボツトの破壊の潜在的可能性をなくしたロボツ
ト制御装置を提案するものである。

［課題を解決するための手段］上記課題を達成するための本発明の構成は、第１の方
向の移動から第２の方向の移動にロボツトを連続的に制
御させるときに、第１の方向の移動中に、所定の位置か
ら第１の方向と第２の方向の移動とを並列的に行なうよ
うに制御するロボツトの制御方法において、第１の方向
の目標位置と、第２の方向における最終目標位置と、第
２の方向で移動運動が行なわれる必要のある第１の距離
とに少なくとも基づいて、前記所定位置を決定すること
を特徴とする。

また、本発明の他の構成は、第１の方向の移動から第
２の方向の移動にロボツトを連続的に移動制御するとき
に、第１の方向の移動中に、所定の位置から第１の方向
と第２の方向の移動とを並列的に行なうように制御する
ロボツトの制御装置において、第１の方向の位置決め制
御部と、第１の方向とは独立して制御する第２の方向の
位置決め制御部と、第１の方向の移動における目標位置
と、第２の方向の移動における最終目標位置と、第２の
方向で移動運動が行なわれる必要のある第１の距離とに
基づいて、第２の方向の移動の開始から前記第１の距離
の先頭位置に到達するまでの期間を演算し、さらに、こ
の期間内の第１の方向の移動距離だけ、第１の方向の移
動における目標位置からさかのぼつた位置を演算する演
算手段とを備え、前記第２の位置決め制御部は、前記さ
かのぼつた位置を前記所定の位置とし、この所定の位置
を通過すると、第２の方向の制御を開始することを特徴
とする。

［実施例］以下添付図面を参照して、本発明を、XYZθ軸の４軸
制御のロボツト制御装置に適用した実施例を説明する。

〈実施例の概略〉第１図は、実施例のロボツトが使用される環境及びそ
のアームの動作を説明するものである。アームの構成を
第２図に示す。第１図中、４は部品２を複数個収納して
いるマガジンである。また、３は治具５に保持されたワ
ークであり、ロボツトはマガジン４から１つの部品２を
取出して、このワーク３に取付けるものである。ワーク
３は、その内部にキヤビテイ６が設けられており、部品
２はこのキヤビテイ６の奥の凹所７内に挿入される。

第１図では、説明を簡略化するために、水平移動はＸ
方向で、垂直移動はＺ軸方向で行なうものとする。上記
のような組み付け動作を行なうためには、アームは、Ｐ
₀から上昇を開始し、Ｐ₄Ｐ_GＰ₅を通過し、Ｐ₃で停
止する必要がある。このような動作を実現するためのプ
ログラムを第３図に示す。第３図において、PICK命令は
フインガー13が部品２を把持する命令である。SPEEDは
アームの移動速度を定義する命令、OUTは特定のソレノ
イド（第１図には不図示）等をオン／オフする命令、IN
Pはセンサ（第１図には不図示）等のステータスを入力
する命令である。また、ARCHUPは上昇命令、ARCHは水平
移動命令、ARCHDOWNは下降命令である。

第３図プログラムにおいて、移動命令は、 ARCHUP Ｐ₁;H₁ ARCH Ｐ₂;P₃;V₃;H₂ ARCHDOWN Ｐ₃;V₃ という形式をもつている。これらの命令は本実施例に特
有の命令であり、障害物を定義し、その障害物を回避す
るような軌跡の運動をアームに行なわせるようなもので
ある。

これらの命令を理解するために、第１図に戻つて説明
を続ける。

マガジン４は同図に示すようにフランジ８を有するす
るから、Ｐ₀から垂直移動を開始するときは、このフラ
ンジに衝突しないようにしなければならない。そこで、
Ｐ₀からＰ₄までは垂直移動のみを行ない、その間、水平
移動を禁止する必要がある。

ARCHUP Ｐ₁;H₁ という命令は、点Ｐ₁に向けて上昇し、距離Ｈ₁の間は上
昇のみを行ない、Ｈ₁の上昇を行なつたらその後は次の
命令を並行に実行してもよいという命令である。この次
の命令とは、第３図プログラムの例では、ライン４以下
の命令であり、実質的にはARCH命令である。点Ｐ₄は点
Ｐ₀から距離Ｈ₁上方の点と定義される。

一方、ワーク３は前述のような形状を持つているか
ら、アームは、第１図に示すように、最終目標位置Ｐ₃
から上方に距離Ｈ₂の間は少なくとも垂直移動のみを行
わなければ、ワークのフランジに衝突してしまう。そこ
で、 ARCH Ｐ₂;P₃;V₃;H₂ は点Ｐ₂に向けて水平移動を行なう命令であり、最終目
標位置Ｐ₃と降下速度Ｖ₃と上記距離Ｈ₂で規定される点
Ｐ_Gを経過した時点で、次のプログラム命令を並行に実
行してもよいという命令である。第３図の例では、次の
プログラム命令はライン８以下の命令であり、実質的に
は ARCHDOWN Ｐ₃;V₃ である。尚、点Ｐ₅は点Ｐ₃の上方Ｈ₂の点と定義され
る。

第４図は、第１図実施例のロボツトが第３図プログラ
ムに従つて動作した場合の、アームの運動の動きを図示
したものである。

さて、上記Ｐ_Gは次のような観点から決められなけれ
ばならない。

：水平移動運動と降下運動とは、Ｐ_G以降は並行して
行なわれる。

：水平移動運動は、Ｐ_Gで降下が開始しても継続し、
点Ｐ₂で終了する。

：水平移動運動は、降下運動が点Ｐ₅を通過した時点
で終了する。

：降下運動は点Ｐ₃で終了する。

この観点から決定された点Ｐ_Gは、点Ｐ₂からＸ方向に
△ｘだけ前方の点であり、この△ｘは、Ｚ方向の運動が
点Ｐ₂からＰ₅に移動するのに要する時間Ｔ₂₅間に、水平
方向に移動する距離である。ところで、Ｘ方向もＺ方向
の運動も、一般的には、単純な等速度運動ではなく、こ
の実施例でも、等加速度運動等速度運動等源速度運動を行なつている。そして、上記Ｔ₂₅は、点Ｐ₅が等加速
度運動区間に入るのか、等速度運動区間に入るのか、等
源速度運動区間に入るのかで、即ち、距離Ｐ₂Ｐ₃と距離
Ｈ₂との値により異なるものとなる。△ｘをどのように
決定するかは後述する。

〈制御装置の構成〉第５図は実施例のロボツト制御装置の構成を示す。図
中、20は第３図に示したようなプログラムを入力する入
力装置である。21は入力されたプログラムを解釈する部
分であり、マイクロコンピュータ制御部である。22はXY
Z軸とθ軸を独立に制御する位置決め制御部であり、具
体的には、サーボモータ（第６図のＭ）を、そのモータ
に直結されたエンコーダ（第６図のＥ）の出力（位置情
報）によりサーボ制御するものである。I/O制御部23は
フインガー13のソレノイド（第６図の25）を駆動した
り、このフインガーが部品を把持したか否かをチエツク
するためのセンサ（第６図の26）の出力を入力する部分
である。

第５図のプログラム解釈部21と位置決め制御部22とI/
O制御部23との関係を第６図に示す。解釈部21のプログ
ラム構成は、第３図の如きアプリケーシヨンプログラム
と、OS（オペレーテイングシステム）と、アプリケーシ
ヨンプログラムを解釈するインタプリタからなる。イン
タープリタの手順の一部を第７図に示す。プログラム解
釈部21と位置決め制御部22とI/O制御部23とはシステム
バス24により接続される。インタプリタは位置決め制御
部22に対し、第８図に示したようなI/Oコマンドを送
る。このI/Oコマンドは、前述の３つの命令、ARCHUP,AR
CH,ARCHDOWNに対応して、コマンドコードとパラメータ
とからなる。位置決め制御部22はI/Oコマンドを受ける
と、その実行を行なう。移動動作は第９図に示すよう
に、周知の台形制御、即ち、等加速度運動等速度運動等源速度運動を行なう。本実施例に用いられている台形制御では、等
速度運動における速度をＶ、このＶに至るまでの加速期
間をＴ_AC、速度Ｖから停止までの減速期間もＴ_ACとする
と、加減速時の加速度はで与えられる。また、速度“0"から速度Ｖまで加速する
までに移動する距離、若しくは、速度Ｖから速度“0"ま
で減速するまでに移動する距離は、 1/2VT_AC ……（２）で与えられる。第9B図は速度Ｖまで加速され、直ちに
“0"まで減速された状態を示す。第9B図の場合の総移動
距離は（２）式から、 VT_AC である。第9C図は加速度αで期間Ｔ（＜Ｔ_AC）だけ加速
され、その後“0"まで減速された状態を示す。このとき
の移動距離は、である。上記（１）〜（３）式は点Ｐ_Gを求める上でし
ばしば参照される。

〈命令のフエツチ実行シーケンス〉第７図に示したインタプリタプログラムの制御手順、
及び第10A図〜第10C図に示した位置決め制御部における
I/Oコマンド実行プログラムの制御手順を説明すること
により、実施例の制御装置において、ARCH等の命令が完
全に終了しない時点で、次の命令が命令が実行され得る
様子（所謂パイプライン実行制御）を説明する。こうす
ることにより、プログラマが、ARCH命令等が実行されて
いる最中に、他のARCH命令のみならず、OUT命令、INP命
令等を並列的に実行されるようにプログラムを記すこと
が可能となる。

第７図〜第10図を用いて、インタプリタプログラムと
I/O制御部におけるサーボ制御プログラムの動作につい
て説明する。ここで、第１図の各点の座標を定義する。
前述したように、XZ系のみで考えているから、Ｐ₀＝（ｘ₀,z₀）Ｐ₁＝（ｘ₀,z₁）Ｐ₄＝（ｘ₀,z₀＋Ｈ₁）Ｐ_G＝（ｘ_G,z₁）Ｐ₂＝（ｘ₂,z₁）Ｐ₅＝（ｘ₂,z₃＋Ｈ₂）Ｐ₃＝（ｘ₂,z₃）である。

第７図のステツプS1においては、プログラムカウンタ
PCをインクリメントし、ステツプS2でこのPCでポイント
された命令をフエツチする。ステツプS4では、フエツチ
された命令が、ARCH関連の命令（ステツプS8へ）か、I/
O関連の命令（ステツプS30へ）か、その他の命令かを判
断する。ARCH関連の命令であれば、ステツプS8〜ステツ
プS14で命令コードを設定（ARCH＝0;ARCHUP＝1;ARCHDOW
N＝２）し、ステツプS16では、命令毎に第８図のような
パラメータを付加してI/Oコマンドを作成する。そし
て、このI/Oコマンドを位置決め制御部22に送る。ステ
ツプS20では、位置決め制御部22からのACK信号を待つ。
このACK信号は、第４図及び後述のサーボ制御手順の説
明から分るように、ロボツトが点Ｐ₄やＰ₅等の中間点を
通過したときに位置決め制御部22から返される。このAC
Kを受けると、制御はメインルーチン（不図示）にリタ
ーンし、再び、ステツプS1から前述の手順を実行する。
即ち、ARCH関連命令では、ACKが位置決め制御部22から
返つてくれば、解釈部21はプログラム中の次の命令をフ
エツチして実行するので、ARCH関連命令の並列処理若し
くは、ARCH関連命令と他の命令の並列処理が可能にな
る。特に、本実施例では、第３図のような高級言語レベ
ルでの命令の並列処理が可能となつており、プログラマ
はARCH関連命令から別のARCH関連命令に進む前に、例え
ば、第３図に示したように、INP命令等を挿入すること
により、フインガー13が正常に動作していることを確認
してから後のARCH命令に進むようにすることができる。

一方、命令がI/Oデバイスのチエツク命令（INP）であ
れば、ステツプS30で、I/O制御部23に対しステータス情
報を要求し、ステツプS32で、そのステータスをチエツ
クする。もしステータス情報が正常であれば、メインル
ーチンにリターンして次の命令をフエツチする。

命令が例えばソレノイド等を駆動する命令（例えば、
命令OUT）であれば、ステツプS6に進み、当該命令を実
行する。即ち、I/O制御部23に対し、所定のソレノイド
を駆動するためのI/Oコマンドを送る。

位置決め制御部22側では、解釈部21から送られてきた
I/Oコマンドのコードにより、ARCHUP命令か、ARCH命令
か、ARCHDOWN命令かを区別する。

〈ARCHUP命令実行の詳細〉 ARCHUP Ｐ₁:H₁ とプログラムしたときの動作を説明する。ARCHUP命令は
第10A図に示される。ステツプS50では軌道計画を策定す
る。この軌道計画は、点Ｐ₀から点Ｐ₁に向つて進む際に
第９図に示した台形制御により制御した場合に、どこま
でが等加速度運動で，どこまでが等速度運動で、どこか
らが減速運動かを決定するものである。次に、ステツプ
S52〜ステツプS56のループと、ステツプS58〜ステツプS
60のループを並列実行処理する。

ステツプS52〜ステツプS56のループでは、Ｚ方向のサ
ーボモータの駆動（ステツプS52）と、エンコーダ出力
（ｚ_iの読み込み（ステツプS54）と、Ｚ方向でＰ₁を超
えた（ｚ_i＞ｚ₁）かを判定（ステツプS66）する。ステ
ツプS58〜ステツプS60のループでは、エンコーダ出力
（ｚ_i）の読み込み（ステツプS58）と、Ｚ方向にＰ₄を
超えた（ｚ_i＞ｚ₀＋Ｈ₁）かを判定（ステツプS60）す
る。第１図で上方向にＺ軸をとつた場合、ステツプS56
でYESと判断されるよりも、ステツプS60でYESと判断さ
れる方が早いので、ステツプS62が実行され、解釈部21
に対しACKが返される。このACKにより、解釈部21は次の
命令をフエツチすることが可能となるのは前述した通り
である。

〈ARCH命令実行の詳細〉インタプリタがARCH命令をフエツチすると、コード
“0"のI/Oコマンドが制御部22に送られる。位置決め制
御部22では、ステツプS72で点Ｐ_Gの算出を行なう。この
点が算出されると、ステツプS74〜ステツプS78のループ
と、ステツプS80〜ステツプS82のループを並列実行処理
する。

ステツプS74〜ステツプS78のループでは、Ｘ方向のサ
ーボモータに駆動（ステツプS74）と、エンコーダ出力
（ｘ_i）の読み込み（ステツプS76）と、Ｘ方向にＰ₂を
超えた（ｘ_i＞ｘ₂）かを判定（ステツプS78）する。ス
テツプS80〜ステツプS82のループでは、エンコーダ出力
（ｘ_i）の読み込み（ステツプS80）と、Ｘ方向にＰ_Gを
超えた（ｘ_i＞ｘ_G）かを判定（ステツプS82）する。第
１図で左方向にＸ軸をとつた場合、ｘ_G＜ｘ₂であるか
ら、ステツプS78でYESと判断されるよりも、ステツプS8
2でYESと判断される方が早いので、ステツプS84が実行
され、解釈部21に対しACKが返される。このACKにより、
解釈部21は次の命令をフエツチする。

第11図のフローチヤートを用いて、ステツプS72の点
Ｐ_Gの算出制御の詳細を説明する。この第11図の制御
は、前述したように、Ｚ方向の運動が点Ｐ₂からＰ₅に移
動するのに要する時間Ｔ₂₅内に、水平方向に移動する距
離△ｘを求めることに等価である。ステツプS100では、
軌道計画、I/Oコマンドのパラメータ等から次の諸量を
定義する。即ち、Ｔ_AZ：下降時の加速時間（＝減速時間）Ｖ₃：下降時の最大速度（等速運動時の速度） z:垂直方向移動距離（＝|P₃−Ｐ₂｜）である。ステツプS102では、ｚとＶ₃Ｔ_AZの大小を調べ
て、ステツプS70で策定された軌道計画中において、Ｐ₂
からＰ₃までの下降中に等速度運動があるか否かを調べ
る。

等速下降運動のある場合第９図に関連して説明したように、ｚ＞Ｖ₃・Ｔ_AZ であれば、等速下降運動が存在する場合である。このと
きは、ステツプS110,ステツプS112により、点Ｐ₅が等加
速度運動区間に入るのか、等速度運動区間に入るのか、
等減速度運動区間に入るのかを判断して場合分けをす
る。

1/2V₃・Ｔ_AZは減速期間Ｔ_AZ中にＺ方向に移動する距
離であるから、Ｈ₂≦1/2V₃・Ｔ_AZ （ステツプS110でNO）とは、減速がＰ₅よりも上方で開
始されることを意味し、Ｈ₂＞1/2V₃・Ｔ_AZ （ステツプS110でYES）とは減速がＰ₅よりも下方で開始
されることを意味する。ステツプS110でNOの場合は、Ｔ
_AZ間加速Ｖ₃で等速運動減速開始Ｐ₅通過Ｐ₃到
着となる。Ｐ₂Ｐ₃に要する時間は、また、Ｐ₅Ｐ₃に要する時間はであるから、Ｔ₂₅は、ステツプS114にあるように、となる。

減速がＰ₅より下で行なわれる場合（ステツプS110でY
ES）は、Ｐ₅が加速運動区間に入るかまたは等速運動区
間に入るかを、Ｈ₂と（ｚ−1/2V₃・Ｔ_AZ）との大小から
判断する。

Ｈ₂＞ｚ−1/2V₃・Ｔ_AZ であれば（ステツプS112でYES）、Ｔ₂₅の期間は、等加
速度Ｖ₃/T_AZで運動が行なわれるから、Ｔ₂₅はステツプS
118のように、となる。逆に、Ｐ₅が等速度運動区間にある場合には、
加速時間はＴ_AZ、等速度運動期間は、であるから、ステツプS116のように、となる。

等速下降運動が無い場合等速下降運動が無い場合はｚ＜Ｖ₃・Ｔ_AZとなりステ
ツプS104に進む。この場合、減速がＰ₅前に行なわれる
か（Ｈ₂≦1/2Z）、Ｐ₅の後で行なわれる（Ｈ₂＞1/2z）
かという２つの場合があり得る。前者の場合であれば、
Ｐ₂Ｐ₃に要する時間はであり、Ｐ₅Ｐ₃に要する時間はであるから、である。

こうして、各場合についてＴ₂₅が得られた。次に、Ｔ
₂₅の間にＸ方向に移動する距離△ｘを第11B図のフロー
チヤートに従つて計算する。ステツプS120では、各量の
定義を行なう。即ち、Ｔ_AX:X方向加速時間（＝減速時間）Ｖ₂：最大速度（等速運動時の速度） x:水平方向移動距離（＝|P₂−Ｐ₁｜）である。ステツプS122では、ｘとＶ₂・Ｔ_AXとを比較し
て、水平移動に等速運動区間が存在するか否かを調べ
る。

等速水平移動がある場合この場合は、ステツプS122でYESと判断され、ステツ
プS140に進む。加速と減速に要する期間は2T_AX、等速運
動の期間は（ｘ−Ｖ₂・Ｔ_AX）/V₂であるから、水平移動
でＰ₁からＰ₂に要する時間Ｔ_Tは、となる。ステツプS141では、Ｔ₂₅とこのＴ_Tとの大小を
比較する。もし、Ｔ₂₅＞Ｔ_T であれば、下降開始がＰ₁よりも左で行なわれることを
意味し、これはあり得ないので、プログラムミスとして
エラーとする。

ステツプS142ではＴ₂₅とＴ_AXとの大小を比較する。下
降開始点Ｐ_Gが水平移動の減速開始前にあれば、Ｔ₂₅＞Ｔ_T であり、開始後であれば、Ｔ₂₅≦Ｔ_T である。そして、第12A図に示すように、下降開始が等
速運動区間で行なわれれば、Ｔ₂₅≦Ｔ_T−Ｔ_AX である（ステツプS144でYES）から、Ｐ₁Ｐ_Gまでの距
離△ｘは、ステツプS148のように、 △ｘ＝ｘ−|P₂−Ｐ_G｜＝ｘ−｛Ｖ₂（Ｔ₂₅−Ｔ_AX）＋1/2V₂・Ｔ_AX｝＝ｘ−Ｖ₂・（Ｔ₂₅＋1/2T_AX）と得られる。

下降開始が第12B図に示されるように、加速期間中に
行なわるものであれば、Ｔ₂₅＞Ｔ_T−Ｔ_AX である（ステツプS144でNO）から、ステツプS150に進
む。この場合、 △ｘ＝|P_G−Ｐ₁｜であるから、となる。

一方、ステツプS142で、Ｔ₂₅≦Ｔ_AXと判断されたとき
はステツプS146に進む。このときは、降下開始が第12C
図に示すように減速期間に行なわれるから、 △ｘ＝|P_G−Ｐ₁｜＝|P₁−Ｐ₂｜−|P₂−Ｐ_G｜であるから、となる。

等速水平移動がない場合この場合は、ｘ≦Ｖ₂・Ｔ_AXであるから、ステツプS12
4に進む。第12D図から明らかなように、Ｔ_Tは、加速し
て1/2x移動するのに要する期間の２倍であるから、ステ
ツプS124のように、とする。ステツプS125はステツプS141と同じ意味であ
る。1/2T_Tは減速に要する時間であるから、Ｔ₂₅＞1/2T_T とは第12D図に示すように下降が減速開始前に行なわれ
ることを意味する。このときの△ｘはＰ₁とＰ_G間の距
離、即ち、期間（Ｔ_T−Ｔ₂₅）の間に加速して移動した
距離であるから、である。また、降下が水平方向の減速が開始されてから
始まる場合は、第12E図に示すように、△ｘ＝ｘ−|P_G−
Ｐ₂｜であるから、ステツプS128のように、となる。こうして各場合において△ｘが求めることがで
きた。次に、ステツプS152で、Ｐ_GのＸ座標位置ｘ_Gが計
算される。即ち、である。ここで（ｘ₂−ｘ₁/xは水平移動の方向を考慮し
た。

このようにして定められたＰ_Gにおいて、下降を開始
すれば、上述の〜の条件は満足される。即ち、点Ｐ
_Gは、点Ｐ₃に向けて下降開始を行なつた場合に障害物を
回避して最短時間で点Ｐ₃に到達し得る通過点である。
従つて、この点Ｐ_Gを通過した時点で、位置決め制御部2
2が解釈部に対しACKを返し、解釈部が次のARCHDOWN命令
を解釈してI/Oコマンドを位置決め制御部22に送ると、
ロボツトアームは障害物を回避して最短時間で点Ｐ₃に
到達する。

〈ARCHDOWN命令実行の詳細〉第３図のプログラムによれば、ARCH命令によりＰ_Gを
通過した時点でACKが解釈部に返されると、OUT命令、IF
INP命令、そしてARCHDOWN命令が実行される。このARCH
DOWN命令に対する位置決め制御部22の制御手順は第10C
図に示す通りである。

〈実施例の効果〉以上説明した実施例によれば、 I:高級ロボツト言語により、障害物を回避しつつ、異な
る方向の動作を並行して行なうようなロボツト制御プロ
グラムの記述が可能となる。従つて、プログラマによる
プログラミング及びテイーチングは容易になるととも
に、並行移動のためにロボツト移動が高速化する。

II:特に、ARCH命令において、障害物高さＨ₂、降下時の
最大速度Ｖ₃、目標降下位置Ｐ₃等を記述できるので、障
害物を回避し得るのみでなく、そのうちの最短時間で目
標Ｐ₃に到達するような通過点Ｐ_Gを制御装置自体がオペ
レータに替つて計算してくれるので、従来のように試行
錯誤を繰返してしか得られなかつた通過点Ｐ_Gは容易に
得られることとなつた。

III:ARCHUP,ARCH系統の命令では、異なる方向への移動
を開始する時点で、先の移動命令は疑似的に終了となる
ので、この先の移動命令の最中にプログラマは所望の手
順プログラムを挿入することができ、制御プログラムの
自由度が増す。

〈変形例〉本発明はその要旨を逸脱しない範囲で種々変形が可能
である。

例えば、前述の第１図実施例では、方向としてXZの２
方向としたが、他の方向であつてもよい。

また、前述の第１図実施例では工業用ロボツトを例に
して説明した。工業用ロボツトでは、障害物は高さ方向
で発生することが多い。ところが、第13図に示したよう
に、軌跡が回避しなければならない点として２つの点Ｓ
₁,S₂がある場合があるが、このような場合にも、本発明
を適用したARCH命令は、 ARCH Ｐ₂;P₃;V₃;H₂;H₃;H₄ となる。この命令において、Ｐ₃,H₂で規定される点Ｓ₂
は第１図で説明した通りの回避点である。点Ｓ₁は、点
Ｐ₃から高さＨ₃の位置にあり、Ｓ₂から水平方向に距離
Ｈ₄の位置にある点である。かかる命令において軌道制
御は次のようにして行なう。先ず点Ｓ₁について前記実
施例と同じ要領で軌道を生成し通過点Ｐ_G′を計算す
る。また、点Ｓ₁についても軌道を生成し通過点Ｐ_G″を
計算する。もし、Ｓ₁,S₂の一方について夫々独立して計
算された２つの軌道が両方とも他方の点と干渉しないの
であれば、点Ｐ₁により近い方のＰ_G′またはＰ_G″をＰ_G
として採用する。もし、Ｓ₁,S₂の一方について夫々独立
して計算された２つの軌道のうちの１つが他方の点と干
渉するのであれば、干渉のない方の軌道を採用する。こ
のようにして、２つの干渉点が存在するようなモデルに
対しても、本発明を拡張適用できる。

また、前述の第１図実施例では、ARCHUP命令及びARCH
命令に付加する直線部高さ情報としてＨ₁,H₂としていた
が、その替りに、ポイントＰ₄,P₅により表記してもよ
い。

また、前述の第１図実施例では解釈部はインタプリタ
タイプのものであつたが、プログラム解釈部があらかじ
めすべての命令を実行形式にしておく、いわゆるコンパ
イラタイプのプログラム方式では、水平移動命令に付加
している下降命令の目標値，速度は先読みしてコンパイ
ルした後に付加出来るので、水平移動命令中になくても
同様の効果が見られる。

また、下降開始可能な水平位置計算は水平移動軸で、
移動量が０でなければ、どの軸で行つても同様の効果が
得られる。

また、下降開始可能な水平位置計算のかわりに、サー
ボサイクル毎に位置計算を行う位置決め制御部22であれ
ば、時間を用いても同様の効果が得られる。

また、軌道計画は台形制御に限られず、他の制御であ
つてもよい。

また、下降開始可能な水平位置計算は位置決め制御部
22でなく、プログラム解釈部で行つても同様の効果が得
られる。

［発明の効果］以上説明したように本発明のロボツトの制御方法及び
制御装置によれば、ロボツトを高速に動作し得る機能は
そのままで、オペレータが容易に障害物の干渉を避ける
プログラミング及びテイーチングが出来る効果がある。

また、本発明の好適な実施態様によれば、各々の動作
命令は、独立に動作・記述出来るので、各々の命令の間
で停止させたり、各々の命令の間にロボツトの移動以外
の命令を記述することが出来るという今までの高速動作
方式では難しいとされていた事も容易に出来るようにな
る効果もある。

【図面の簡単な説明】

第１図は１実施例に係るロボツト制御装置による軌道の
側面図、第２図は制御装置のアーム部分のブロツク図、第３図は実施例装置に用いられるプログラムを記した
図、第４図は第１図実施例における軌道の変化を示したタイ
ミング図、第５図，第６図は実施例装置の構成を説明する図、第７図は実施例装置におけるインタプリタの手順を示す
フローチヤート、第８図は位置決め制御部に用いられるI/Oコマンドのフ
ォーマツト例を示す図、第9A図〜第9C図は夫々実施例の軌道計画に用いられる軌
道の一例を示す図、第10A図〜第10C図は夫々、位置決め制御部における制御
手順を示すフローチヤート、第11A図，第11B図は点Ｐ_Gを計算するためのフローチヤ
ート、第12A図〜第12E図は点Ｐ_Gを計算する手法を説明する
図、第13図は変形例を説明する図、第14図は従来例を説明する図である。図中、１……ロボツト軌道、２……部品、３……ワー
ク、４……マガジン、５……治具、６……キヤビテイ、
７……凹所、８……フランジ、10〜12……アーム、13…
…フインガー、20……プログラム入力装置、21……プロ
グラム解釈部、22……位置決め制御部、23……I/O制御
部、24……システムバスである。

Claims

(57)【特許請求の範囲】

【請求項１】第１の方向の移動から第２の方向の移動に
ロボツトを連続的に制御させるときに、第１の方向の移
動中に、所定の位置から第１の方向と第２の方向の移動
とを並列的に行なうように制御するロボツトの制御方法
において、第１の方向の目標位置と、第２の方向における最終目標
位置と、第２の方向で移動運動が行なわれる必要のある
第１の距離とに少なくとも基づいて、前記所定位置を決定することを特徴とするロボツトの制
御方法。
【請求項２】第１の方向は水平方向であり、第２の方向
は垂直方向であり、更に、水平移動の開始位置と終了位
置と、垂直移動の開始と最終目標位置とを入力し、水平移動の開始位置と終了位置に基づいて、第１の方向
における第１の軌道計画を設定し、垂直移動の開始と最終目標位置に基づいて第２の方向に
おける第２の軌道計画を設定し、この第２の軌道計画に従つて垂直方向への移動の開始か
ら前記第１の距離の先頭位置に到達するまでの期間に、
前記第１の軌道計画に従つて水平方向に移動する第２の
距離だけ水平移動の終了位置からさかのぼつた位置を、
前記所定位置とする事を特徴とする請求項の第１項に記
載のロボツトの制御方法。
【請求項３】第１の方向の移動から第２の方向の移動に
ロボツトを連続的に移動制御するときに、第１の方向の
移動中に、所定の位置から第１の方向と第２の方向の移
動とを並列的に行なうように制御するロボツトの制御装
置において、第１の方向の位置決め制御部と、第１の方向とは独立して制御する第２の方向の位置決め
制御部と、第１の方向の移動における目標位置と、第２の方向の移
動における最終目標位置と、第２の方向の移動運動が行
なわれる必要のある第１の距離とに基づいて、第２の方
向の移動の開始から前記第１の距離の先頭位置に到達す
るまでの期間を演算し、さらに、この期間内の第１の方
向の移動距離だけ、第１の方向の移動における目標位置
からさかのぼつた位置を演算する演算手段とを備え、前記第２の位置決め制御部は、前記さかのぼつた位置を
前記所定の位置とし、この所定の位置を通過すると、第
２の方向の制御を開始することを特徴とするロボツトの
制御装置。
【請求項４】前記演算手段は、第１の方向の移動におけ
る目標位置と、第２の方向の移動における最終目標位置
と、第２の方向で移動運動が行なわれる必要のある第１
の距離とを記述する命令を解釈する解釈手段を含む事を
特徴とする請求項の第３項に記載のロボツトの制御装
置。