JPS594278B2 - 複腕ロボツトの衝突回避方式 - Google Patents

Description

【発明の詳細な説明】 〔発明の利用分野〕 本発明は、複腕ロボットの相互の腕の衝突回避９０方式
、特に、腕相互の距離を検出する手段として超音波セン
サを設けてなる複腕ロボットの衝突回避方式に関する。

〔従来技術〕

ロボットにおいて、ロボットによる作業性を向ソ５ 上
させる目的で、２本以上の腕を具えた複腕ロボットが提
供されている。

この複腕ロボットの制御は極めて複雑である。その理由
は、各腕の自由度が比較的多く、且つその自由度を決定
しているのが各腕に設けられている複数個のアーム部に
よつａ て行つている。従つて、複腕ロボットの制御に
際しては、各腕全体に対する制御と、谷腕の各アーム部
個々に対する制御とを互いに関連して、且つ独自の機能
を含めて、制御しなければならない。こうした点からも
複腕ロボットの制御は複雑とな３５つている。しかしな
がらこのような複腕ロボットの異常接近あるいは衝突防
止については何らの対策もなされていなかつた。〔発明
の目的〕 本発明の目的はこのような従来技術の欠点を改善し、安
全なロボツトの運転を可能にする複腕ロボツトの衝突回
避方式を提供することにある。

〔発明の概要〕本発明の特徴はロボツトのアームに接近
センサを取付けアームの異常接近が検知されたときは、
衝突しないように回避動作を卦こなうことにある。

さらに具体的には例えばアームに超音波センサを設けて
アーム間の距離を測定し、該測定された距離が異常接近
か否かを判定し、異常接近と判定されたときは衝突を避
ける方向に該アームを動作せしめる方式である。〔発明
の実施例〕 実施例の説明に先立ち、まず本発明の基礎となる事項に
ついて述べる。

複腕ロボツトの制御で特に重要なことは、腕相互の異常
な接近であり、この異常接近の検出に特に役割を果して
いるのが、上述した超音波センサである。

異常接近といつても、本来予期せぬ異常接近と、全体の
制御上は正しい思考のもとに制御を行つたとしてその結
果たまたま異常接近を起すという異常接近との２つの事
例が考えられる。この２つの形態の異常接近いずれに於
いても、腕相互が衝突等の事故を起した場合、作業の一
時定止や、作業内容が指示通り行われないという悪い結
果を生む。一方、超音波センサを複腕ロボツトの各腕に
取りつけるに際して、超音波センサは２つの目的のもと
に設置される。

第１は送波用であり、第２は受話用である。超音波セン
サとしては送波と受波とを兼用したものと、兼用させな
いで送波専用、受波専用という形で設ける場合とがある
。いずれに訃いても、超音波センサは、各腕の各アーム
部毎に設置されている。そして、各腕の各アーム部毎に
設けられた超音波センサは、各腕の各アーム間の接近距
離を測定するようにしている０この接近距離は、超音波
を送波してから受波するまでの時間をもつて検出してい
る〇本出願人は、現在までに、かかる方式に基づく各腕
の各アーム間の接近距離を検出する各種のやり方につい
ても提案し、出願している。

しかし、これらの出願は、距離測定と異常接近の検出ま
での過程に関するものであつて、異常接近後いかなる形
でＦｈＩ脚を行うかについては論及しなかつた。異常接
近の検出後の動きとしては、異常検出の発生と同時に該
当した接近アーム間の動作停止を行わせることが最も常
識的であるが、作業の一時中断の結果を生み、また、中
断した後どのような制御を行うかという別の制御の必要
性を生んでくる。本発明は、かかる観点の中から提案す
るものであつて、異常接近時には、異常接近と同時に、
回避動作を行わせるようにしたものである。これによつ
て、作業の中断時間の縮少と、作業の連続性とを維持で
きることになつた。更に、本発明では回避動作を行わせ
るに際して、回避動作上、最も効率的な回避方向（ベク
トル的に）をとるようにしてなる回避方式が提案されて
いる。以下、本発明を、図面により具体的に述べよう。
第１図は、複腕ロボツトの構成と超音波センサの取付状
況とより成る本発明の対象となるロボツト系の概念図を
示す。

図は、２つの腕Ａ，Ｂより成るロボツト系より成る。腕
Ａは、２つのアーム部１，２及びフインガ一５及び、関
節７，８，９を持つている。更に、アーム部１は超音波
発振器より成る２つの送波器１ａ，１ｂを持ち、アーム
部２は同様に２つの送波器２ａ，２ｂを持つている。即
ち、腕Ａは送信専用の役割を持たせている。アーム部３
は超音波受信器より成る２つの受波器３ａ，３ｂを持ち
、アーム部４は同様に２つの受波器４ａ，４ｂを持つて
いる。即ち、腕Ｂは受信専用の役割を持たせている。２
つの腕Ａ，Ｂ共に運動上の自由度は関節７，８，９及び
１０，１１，１２によつて決定される。

送波器，受波器は共に、超音波センサに該当する。各送
波器，受波器共に３６『全体の空間をカバーするように
構成されている。具体的には、互いに方向が１８♂異な
る２つの超音波素子を持つように構成している。かかる
構成に於ける距離測定の全体図を第２図に示す。デコー
ダ２５は、送波指令Ｓと送波器用アドレスＰとを入力と
し、アドレスＰを解読し、対応するドライバー１３，１
４，１５，１６を選択するものである。各ドライバー１
３，１４，１５，１６は各送波器１ａ，１ｂ，２ｃ，２
ｄに対応するドライバーである。また、センスアツプ１
７，１８，１９，２０は、受波器３ａ，３ｂ，４ｃ，４
ｄに対応するアンプである。更に、時間差検出回路２１
，２２，２３，２４は各センスアンプ１７，１８，１９
，２０に対応して｝り、送波してから受波するまでの時
間を測定し、その結果、送波器と受波器との距離ｔを検
出するものである。図より明らかなように、各送波器か
らの出力はすべての受波器によつて検出されるように構
成されている。先ず、超音波を発生するタイミングを与
える送波指令Ｓはデコーダ２５のエネーブル入力Ｅを通
して、送波器アドレスＰで指定された所定のドライバに
加わる。

該ドライバほ送波器１ａ，１ｂ，２ａ，２ｂの中で所定
の対応する送波器を駆動し、超音波が空中に放射される
。１つの送波器が駆動されると、この送波器からの距離
に比例した時間後に受波器３ａ，３ｂ，４ａ，４ｃが超
音波を受取る。

各受波器３ａ，３ｂ，４ａ，４ｄの出力は各センスアツ
プ１７，１８，１９，２０に加えられる。送波指令が生
じてから、センスアツプに出力が生ずるまでの時間は、
対応する送波器と受波器との間の超音波の伝搬時間を表
す。各時間差検出回路には、入力として送波指令とセン
スアツプの出力が加わり、両者の時間間隔を測定し、距
離ｔを出力する。時間差検出回路は第３図に示すように
、入力１をセツト入力、入力２をりセツト入力とするＲ
Ｓフリツプフロツプ２６、発振器２７、これらの出力の
論理和を求めるアンド回路２８、入力１でりセツトされ
、アンド回路２８の出力を計数するカウンタ２９、この
出力をデータ入力とし、入力２を書込指令とするレジス
タ３０から成つている。

この回路で、Ｒ−Ｓフリツプフロツプ２６は人力１が加
わつてから、入力２が加わるまでの間だけオンになる。
したがつて、アンド回路２８は、入力１から入力２まで
の間だけ、発振器２７の出力を通す。カウンタ２９は入
力１が加わつた時点でりセツトされてから、アンド回路
２８の出力を計数するので、入力２が加わつて、アンド
回路２８の出力が無くなつた時点では、入力１から入力
２までの間に発振器２７が発生したパルス数が計数され
ており、これは入力１から入力２までの時間に比例する
。したがつて、入力２によつてカウンタ２９の出力をレ
ジスタ３０に書込めば、この出力が時間差を表わす。と
ころで、空中に訃ける超音波の速度は温度の影響を無視
した場合、ほぼ一定になるので、以上のようにして測定
された時間差は距離出力と考えることができる。

このような距離は、１つの送波器を駆動する毎に４種類
得られる。したがつて、送波器アドレスを変更すること
により、４つの送波器を順次切替えて駆動すれば１６種
類の出力が得られる。第１表は以上のようにして得られ
る出力の状態を示したもので、送波器と受波器のすべて
の組合せについて距離出力Ｄａａ，ｄａｂ，・・・・・
・・・・，Ｄｄｄの１６個の出力が得られる。

これによつて、センサ同士の接近状態は検出できるが（
アーム部とセンサの接近状態を直接検出することはでき
ない。センサ同士の距離を表わす１６種類の値からセン
サとアーム部の距離を正確に算出することはできない六
この距離に対応する数値を次のように定義することは
可能である。

アーム部１と受波器３ｂに注目し、アーム部１の長さを
Ｌ１として次のような量を定義する。この量はセンサと
アーム部で形成する三角形の二辺の和と他の一辺の差で
あり、センサとアーム部が接近するほど小さくなり、接
触すると零になる。

したがつて、この量をセンサとアーム部の距離の目安と
して用いることができる。同じ量をアーム部とセンサの
すべての組合せについて求めれば、第２表のように１６
種類の値が得られる。次に、以上のようにして得られた
、センサ同士の距離を示す１６種類の値と、センサとア
ーム部の距離に対応する１６種類の値とを用いた、衝突
回避方式について説明する。

先ず、全体について説明し、次いでフインガ側のセンサ
と他のセンサでは異るので各回避方式について各々の実
施例をもとに説明しよう。先ず、全体を説明する前に各
腕が実際にどのような構成より成るかを述べよう。

第４図は、第１図に示した腕Ａについての構成図である
。本図では、各送波器は図面上から省略している。説明
を簡単にするためである。図で、第１図と同一記号は同
一内容を示し、駆動用として、モータ３１，３２，３３
，３４，３５，３６，３７が設けられている。これらの
モータの駆動方向の組合せによつて腕Ａ及びアーム部１
，２の運動方向としての自由度が決定される。Ｒｌ，ｒ
２，ｒ３，ｒ４，ｒ５，ｒ６，ｒ７はそれぞれ運動方向
を示し、Ｒｌ，ｒ２，ｒ３，ｒ４及びＲ６はモータ３１
，３２，３３，３４，３６の回転運動方向を示し、アー
ム部２の先端の２つの自由度はモータ３５，３６の回転
運動の和と差で駆動を受ける。即ち、フインガ５の左右
への振シは、和（Ｒ５＋Ｒ６）前後への振Ｄは差（Ｒ５
−Ｒ６）で駆動される。更にフインガ５の先端の自由度
はモータ３７による回転運動方向Ｒ７によつて設定され
る。以上の腕Ａの構成は、他の腕Ｂについても同様であ
る。

次に、各腕に対する制御系統を第５図に示そう。図で、
位置指令装置３８は、腕の運動方向を決定し、その運動
方向に沿つて各自由度についてその量を決定指令する装
置であり、制御系統の中で、最も中心となる装置である
。即ち、この装置は、各腕の制御の中心である。勿論、
各腕全体を通して制御する全体位置指令装置（図示せず
）の制御の管轄下で、上記位置指令装置３８は制御をう
けている。かかる位置指令装置３８は、各運動方向、即
ち第４図の事例に例えば７個の自由度Ｒｌ，ｒ２，ｒ３
，ｒ４，ｒ５，ｒ６，ｒ７についての操作量を計算し、
出力する。

この出力は、座標変換装置３９に入力する。該座標変換
装置３９は、上記出力をうけて、腕の実際の位置関係に
即した座標変換を行う。座標変換装置３９は、座標変換
に基づいて実際の自由度（運動量）Ｒｌ，ｒ２，ｒ３，
ｒ４，ｒ５，ｒ６，ｒ７を出力する。各自由度は、位置
決め装置４０，４１，４２，４３，４４，４５，４６に
送られる。各位置決め装置は、ほとんど同→構成より成
る。図では、位置決め装置４０の内部構成のみが示され
ているが、他についても同様な構成より成る。即ち位置
決め装置４０は、サーボアンプ４００、モータ３１（第
４図のモータ３１と同じもの）、位置検出器４旧、速度
検出器４０２よジ成る。この位置決め装置４０は、モー
タ軸に接続される位置検出器４０１、速度検出器４０２
の両出力と座標変換装置３９の出力ｒ１とを入力とする
サーボアンプ４００に基づいてモータ３１を駆動するよ
うに作動する。この位置決め装置の動作は、他の位置決
め装置についても同じである。次に、本発明の回避方式
の全体構成を第６図に示す。

図に於いて、異常接近検出装置４７は、第３図に示した
時間差検出回路からの出力に基づき第２表に示す距離を
検出し、異常接近かどうかの検出を行う装置である。異
常接近が検出されると、その時の該当する腕相互間の異
常接近した部分、例えばアーム部とフインガ一ならばこ
の該当するアーム部とフインガ一といつた具合に、異常
接近した部分を指示し、回避方向決定装置４８に送る。
回避方向決定装置４８は、その異常接近した部分ついて
の回避指令４８ａとその回避方向４８ｂとを設定し、回
避指令装置４９に送る。この回避指令装置４９は、回避
指令４８ａと回避方向４８ｂとを入力とし、実際の回避
の操作量を設定している。この回避指令装置４９の出力
である操作量は出力４９ａとして第５図に示した位置指
令装置３８等の該当する位置指令装置に送られる。この
後の動作は第５図の構成に従つてなされる。また、回避
指令装置４９は回避開始指令４９ｂを演算回路５０に送
る。この演算回路５０は、回避開始指令４９ｂに基づき
作動を開始するものであり、演算内容は以下の通Ｄであ
る。即ち、演算回路５０は、第５図に示した位置決め装
置、例えば該当する操作モータが位置決め装置４０のモ
ータ３１であれば装置４０からの動作状態を位置検出器
４０２に基づき検出し、位置検出信号５０ａとして取ジ
込む。

この取ね込みは極力、連続的に行われる。この取り込ん
でなる入力信号５０ａは、第２表及び（１）式に基づき
判定用のデータに変換され、例えば、（１）式に従えば
、相対距離Ｔｌｄが求められる。この相対距離は、回避
動作の過程で連続的に検出されていく。この検出してゆ
く過程で、上記相対距離は、回避方向に進んでいるかど
うかの判定が行われる。この判定のやり方は、上記相対
距離が回避動作の進行に従つて増加しているか減少して
いるかによつて決定して。いる０増加していれば、回避
動作は正常に行われていると判定Ｌ減少していれば、回
避動作を続けた場合、該当部分相互が衝突してしまうこ
とになるので、回避動作を停止させる。この時、異常接
近信号を発生する。正常判定の際には動作をそのまま続
行させる。そして、相対距離が異常接近距離より充分離
れたと認められる値になつた時、即ち、基準距離と相対
距離とを比較し、後者が大きくなつた時点で、回避動作
は終了したものとして、回避動作を完了させる。次に、
具体的な事例に即して回避動作を述べよう。

回避動作はどの部分相互が異常接近したかによつて若干
の相異を呈する。先ず、フインガ側のセンサとアーム部
との異常接近の事例を述べよう第７図には、腕Ａのアー
ム部２と腕Ｂのフインガ６に近い位置に取りつけられた
アーム部４の受波器４ｄとの異常接近の事例が示されて
いる。この時ごＤ異常接近方向、即ち進行方向は矢印Ｑ
の方向とする。この異常接近により、アーム２と受波器
４ｄとの間の距離は第２表に従えばＴ２ｄと定義できる
。異常接近に従つて、Ｔ２ｄは減少していく。異常接近
の判定値をａとすると、Ｔ２ｄが減少してゆき、Ｔ２ｄ
＜ａとなると、異常接近の判定がなされる。この判定は
、第６図の異常接近検出装置４７によつてなされる。次
いで、回避方向決定装置４８に、該異常接近した部分相
互間を明示する信号を送り、回避方向決定に供される。
ところで、複腕ロボツトを制御する場合、制御装置が確
実に把握でき、かつ制御できるのはフインガの進行方向
と、各駆動軸の動作量である。

したがつて、回避動作は、これだけの情報と操作量によ
つて行なわなければならない。回避動作は、異常接近と
判定された時点で進行していた方向Ｑと直交するのが効
果的である。第７図の円は進行方向Ｑと直交し、受波器
４ｄを中心とするものであへ回避方向はこの面上に選ぶ
。この方向を決めるために、次のような動作を行なう。
円周上にＸｌ，Ｘ２，Ｙ，，Ｙ２の４点を設け、Ｘ，−
Ｘ２とＹ１−Ｙ２が直交するようにする。ここで、最初
に受波器４ｄがＸ１の位置になるようにフインガを移動
し、この時のＴ２ｄの移動前との差をＴｘとする。同様
に、受波器４ｄがＹｌ，Ｘ２，Ｙ２の位置になるように
フインガを移動し、このときのＴ２ｄの移動前との差を
ＴＹｌ，ｔｘ２，ｔＹ２とする。この４つの量を用い、
次の手順で回避方向を決める。（１）ＴＸｌ，ｔＸ２，
ｔＹｌ，ｔＹ２の中で最大のものに対応する点を選ぶ。

たとえば、Ｔｘ，が最大とするとＸ，が選ばれる。（２
）上の点が乗つていない軸の中で大きいものに対応する
点を選ぶ。

上の例では、ＴＹｌと，／．Ｙ２の中で大きいものを選
ぶ。たとえば、ＴＹｌが大ならばＹ１になる。（３）上
のようにして選ばれた２点で囲まれた角度を対応する偏
差で配分する。

上の例では、角度θはとする。

？上のようにして回避動作が最も効果的に行なわれる方
向Ｒ（第７図参照）が選ばれる。

以上の経過のもとに回避方向Ｒが決定すると、次に、回
避指令装置４９が作動する。

先の第６図では、回避動作は簡単に述べたが、実際には
以下のように行われる。演算回路５０の動作を含めて述
べよう。動作は一回に行なう回避と進行の距離を決めて
おき、一回の回避を行なつた後、Ｔ２ｄ≦ａとなるまで
進行の動作を操わ返えす。またＴ２ｄ≦ａとなれば回避
動作に戻る。回避の後ではＴ２ｄが回避前よりも増加し
ているか否かをチエツクしているが、もへ回避を行なつ
てもＴ２ｄ力く増加しない場合には、制御不能と考えて
異常表示を行ない、人間の操作にまかせる。また、進行
の動作が・繰り返えされている間には、Ｉ！，２ｄがａ
より大きな一定値ｂより小さいか否かをチエツクし、小
さければ回避動作を続けるが、ｂを起えると、回避が完
了したと見做し、回避動作を打切つて、正常の動作に戻
る。以上が、フインガ側のセンサとアームが接近した場
合の回避動作について述べたが、他のセンサとアームの
接近の場合を別の実施例として説明する。

第８図はアーム部２と受波器４との接近の状態を示した
ものである。

ロボツトにおいては、制御すべきものはフインガの位置
であるが、第８図の場合、先に述べた実施例のような方
法で回避すると、フインガの位置が大きな変動を起す可
能性が有り望ましくない。したがつて、この場合には、
フインガの位置を固定して回避動作を行なう。第８図に
示すような関節形ロボツトでは、自由度が冗長度を持つ
ているため、フインガを固定して卦いて受波器４ｃの位
置を動かすのは可能である。回避を行なう場合の操作軸
は腕の取付点の軸を選び、この動きに応じて他の軸で補
正動作を行なうことにょ拡フィンガを一定位置に保つ、
腕の取付点が２つの自由度を有し、この自由度が他の腕
に向う方向δと、これに直交する方向θであるとする。
いま、Ｔ２。

が前もつて設定？値ａより小さくなるとロボツトの制御
装置は異常接近と判定し、回避動作を始める。回避動作
の最初に、回避の方向を決めるが、この場合には、θ軸
の両方向に一定量だけ動かし、Ｔ２。が小さくなる方向
を回避方向とする。以上の回避方向の決定を回避方向決
定装置４８によつて行う。回避方向が定まれば、先に述
べた事例と同動な操作によ如回避動作を行うことになる
。尚、以上の回避方式は、一般のデイジタル計算機によ
つて行うことも可能である。

この場合、デイジタル計算機での係り合いは、回避方向
決定以後とすることが望ましい。その理由は、回避方向
決定以後の作業は、上述した２つの典形的な事例から明
らかなように、どのような部分相互間の異常接近であつ
ても同様な処理形態をとるためである。即ち、回避方向
決定以後は同一のプログラムのもとで作業を進めればよ
いのである。このプログラムのフローチヤートを第９図
に示す。このフローチヤートの動きについては、先の説
明で充分理解できる故、省略しよう。〔発明の効果〕 以上の本発明によれば、複雑な軌跡で動作する複腕ロボ
ツトの衝突を自動的に回避可能になつた〇

【図面の簡単な説明】

第１図は本発明の対象とする複腕ロボツト系の概略図、
第２図は距離検出の回路例図、第３図は時間差検出回路
の回路例図、第４図はロボツトの腕の概観図、第５図は
その制御系統図、第６図は本発明の実施例図、第７図及
び第８図は本発明の実施例のための説明図、第９図は処
理のフローチヤートを示す図である。

Claims (1)

1. 【特許請求の範囲】 １ 複数のアーム部から成る腕を複数もつている複腕ロ
   ボットにおいて、該アーム部には少なくとも１つ設けら
   れ他の腕のアーム部との距離を検出する距離センサと、
   該距離センサからの信号により該距離があらかじめ定め
   られた距離よりも小さい異常接近か否かを判定する判定
   手段と、該判定手段によりあらかじめ定められた距離よ
   りも小さくなつたと判定されたとき該判定信号により異
   常接近を回避する指令と回避方向を決定する回避方向決
   定手段と、該回避方向決定手段の出力信号により該異常
   接近を回避すべく操作量信号を発生する回避指令手段と
   を有し該複腕の異常接近回避動作をおこなわせることを
   特徴とする複腕ロボットの衝突回避方式。 ２ 前記特許請求の範囲第１項記載において、該距離セ
   ンサは超音波素子であることを特徴とする複腕ロボット
   の衝突回避方式。 ３ 前記特許請求の範囲第２項において、相対する一組
   の複腕のうち一方の距離センサは超音波受波器であるこ
   とを特徴とする複腕ロボットの衝突回避方式。 ４ 前記特許請求の範囲第１項記載において、該異常接
   近か否かの判定手段は相対向する腕に設けられている複
   数のセンサの信号を用いて判定する手段であることを特
   徴とする複腕ロボットの衝突回避方式。 ５ 前記特許請求の範囲第１項記載の回避方向決定手段
   はあらかじめ定めた複数の方向に該腕を移動したとき最
   大の離脱距離を示す方向に決定する手段であることを特
   徴とする複腕ロボットの衝突回避方式。 ６ 前記特許請求の範囲第５項記載において、該あらか
   じめ定められた複数の方向を該腕の進行方向と垂直な面
   上に選ぶことを特徴とする複腕ロボットの衝突回避方式
   。