JPH01255009A - 倣い制御ロボット - Google Patents

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。

Description

【発明の詳細な説明】 ［産業上の利用分野］ 本発明は、次に作業する位置のワーク形状をセンサによ
って測定しながら作業するロボットに関する。

［従来の技術］ 従来より産業用ロボットとして、種々のタイプのものが
使用されている。

そのひとつに、倣い線などワーク上の基準をセンサによ
って追跡し作業するロボットがある。

この種のロボットの制御方式の１つとして、ワーク自身
の形状誤差・セツティング誤差を上記基準を工具に先行
するセンサで計測することによって検出し、この誤差を
用いて軌跡補間値を逐次的に補正し、倣い動作を行うと
いう制御方式がある。

この制御方式では、倣い精度の点で確実な作業を行うた
めに、センサが検出した計測データをいかなるタイミン
グで軌跡補間値の修正に用いるかという点が問題となる
。

すなわち、作業スピードの変化、ロボットのサーボ系の
遅れなどの変動要因のため、センサが計測した時刻Ｔ１
とセンサの計測点に工具が到達する時刻Ｔ２の関係をリ
アルタイムで把握することが難しいからである。

これを解決しようとするものとして、センサが検出した
ワーク形状データをＦＩＦＯバッファ（先入れ先出しバ
ッファ）に−旦蓄え、そのバッファ内の最も古いデータ
を順次取出して、センサに後続する工具の軌跡補正値を
算出する方式が提案されている（第４回日本ロボット学
会学術講演会、講演予稿集２６０３；　２６０４）。

この方式では、ロボットの動作中の作業スピードが一定
である限り、異なる作業スピードであっても確実な倣い
制御が行える。

すなわち、センサが計測したワーク形状データをバッフ
ァに蓄えるとともに、バッファに蓄えら＝４− れたワーク形状データを軌跡補正に用いるスピードと、
ロボットの作業スピードとが同じであれは゛、軌跡補正
をする始点のタイミングを制御することにより、正しい
倣いが行えるのである。

［発明の解決しようとする課題］ しかしながら、上記の制御方式では、ロボットの動作中
に作業スピードが変化すると正しい倣いが行えなくなる
。

すなわち、ロボットの動作中に作業スピードが変化する
と、バッファに蓄えられたワーク形状データを軌跡補正
に用いるタイミングと、ロボットの実際の作業位置との
関係が崩れてしまい、もはや正しい倣いが行えない。

このロボットの動作中におけるスピード変更は以下の場
合に必要となる。

■　−船釣にロボットは、出発時あるいは停止時に不要
な振動を避けるため、加減速動作を行う。

■　直線部とコーナ一部のように作業部位によって、作
業スピードを変化させる必要がある。

■　レーザーカッティング等の作業では、ワーりにおい
て平坦かつ直線部では高速動作させ、−方曲率大の部分
は、低速動作させることによって、総作業時間を短縮で
きる。動作中の速度が可変でないと、全て低速動作とな
る。

また、経路変更は、作業において一本の直線上の動作す
る作業は一般にありえず、複数のティーチング点による
標準軌跡が与えられることが一般的であるという理由か
ら作業中に必ず起こることといえる。

本発明は、作業中に作業スピードの変化、経路の変更が
生じても、常に適切な倣い動作を行うロボットを提供す
ることを課題とする。

［課題を解決するための手段］ 本発明は上記課題を達成するためになされたものであり
、次の手段を採用した。

即ち、本発明の要旨は第１図に例示する如く、予め記憶
された位置データにより得られる標準軌跡上を動作し、
手先Ｒ１に工具Ｒ２と、該工具Ｒ２より工具進行方向に
所定の距離だけ先行して装着したワーク形状を計測する
センサＲ３とを有する倣い制御ロボッ）Ｒ４において、 前記工具Ｒ２の作業と同時とこ前記センサＲ３が検出し
た計測データによりワーク形状近似データを生成し一時
記憶するワーク形状記憶手段Ｒ５と、前記工具Ｒ２が次
に移動すべき前記標準軌跡上の標準軌跡補間点を出力す
る標準軌跡補間点生成手段Ｒ６と、 前記工具Ｒ２が次に移動すべき標準軌跡補間点に対応し
た近似基準点を生成するとともに、該近似基準点に対し
空間的に最も距離の小さい関係にある前記ワーク形状近
似データを選択するワークデータ選択手段Ｒ７と、 前記ワークデータ選択手段Ｒ７により選択されたワーク
形状近似データを入力し、次に移動すべき工具Ｒ２の作
業点位置および工具姿勢を出力する軌跡補正手段Ｒ８と
、 前記軌跡補正手段Ｒ８から逐次出力される工具Ｒ２の作
業点列へ、工具Ｒ２を逐次到達させるよう制御するロボ
ット制御手段Ｒ９と、 を備え、工具Ｒ２の標準軌跡からのずれをリアー〇− ルタイムで補正しながら動作することを特徴とする倣い
制御ロボットにある。

にで、標準軌跡とは、ワークの加工順序、ワークに対す
る工具Ｒ２の向き、作業スピードを規定するものであり
、ティーチング操作等により予め記憶された位置データ
により生成される。

ワーク形状を計測するセンサＲ３としては、ロボッ）Ｒ
４の手先Ｒ１に設けられ、センサＲ３とワークとの距離
を測定するものや、あるいは単に距離だけではなくワー
ク表面の法線ベクトル等の形状を測定するものが使用で
きる。具体的には、光等を用いて非接触に距離、形状を
測定するセンサＲ３や、ワイヤ等をワークに接触させて
その距離、形状を測定するセンサＲ３等が使用できる。

にでロボッ）Ｒ４としては、工具Ｒ２の姿勢を固定とし
て、最低限度、手先Ｒ１と、ワークとの距離（高さ）方
向およびこの方向に直交する方向への自由度を持つ２軸
型のロボットや、工具Ｒ２の姿勢を含む５軸あるいは６
軸のロボットなど、種々の多軸、多関節ロボットを考え
ることが出来る。また工具Ｒ２で行う作業としては、溶
接、塗装、トリミング等の切断、穴明けその他レーザ加
工等の種々の加工作業の外、組立作業、さらには加工寸
法の計測や塗装ムラ、ピンホール等の検査等を考えるこ
とが出来る。

ワーク形状記憶手段Ｒ５は、上記センサＲ３で検出した
情報を一時蓄えるものであり、具体的には、論理演算回
路とメモリによって構成される。

標準軌跡補間点生成手段Ｒ６は、工具Ｒ２の次に移動す
べき標準軌跡補間点を生成する。この生成された標準軌
跡補間点は、上記標準軌跡を生成する位置データに含ま
れる作業スピード、経路変更等を反映したものとなる。

ワークデータ選択手段Ｒ７は、先ず、上記生成された標
準軌跡補間点に対応する近似基準点を生成する。

この近似基準点は、補正に最も適したワーク形状近似デ
ータを選択する基準となる点であって、例えは、上記標
準軌跡補間点をそのまま近似基準点として用いたり、あ
るいは標準軌跡上の現在工真位置に対応する点を始点と
し標準軌跡補間点を終点とするベクトルを生成し、その
ベクトルの始点を現在工具点位置としたときの終点を近
似基準・　　点とする。

そして、ワークデータ選択手段Ｒ７は、この近似基準点
と空間的に最も近いワーク形状データを軌跡を補正する
ために選択する。

軌跡補正手段Ｒ８は、上記選択されたワーク形状データ
に対して、予め定められたワークと工具との位置・姿勢
関係等を考慮するととにより、次に移動すべき工具の作
業点および工具姿勢を求め出力する。

ロボット制御手段Ｒ９は、上記軌跡補正手段Ｒ８から逐
次出力される工具の作業点列へ工具を逐次到達させるよ
う制御するものであり、従来より使用されるロボットの
制御と同様に行えばよい。

にで、前述のワークデータ選択手段Ｒ７の好ましい態様
として次のような構成をあげることができる。

すなわち、前記ワークデータ選択手段Ｒ７が、少なくと
も前記ワーク形状近似データもしくは標準軌跡のいずれ
か一方より基準直線を生成する基準直線生成手段と、 予め演算決定した射影始点から前記基準直線上に射影す
る射影手段と、 前記射影手段により得られる射影点と前記基準直線上の
予め定められる距離比較点との距離が最も小さくなるワ
ーク形状近似データを選択する距離比較手段と を具備することを特徴とする構成である。

さらにこの態様の好ましいものとして、前記基準直線は
、前記ワーク形状近似データより生成し、 前記射影始点は、前記標準軌跡および実軌跡から推定さ
れる次に工具が到達すべき推定工具端に等しい前記近似
基準点とし、 前記距離比較点は、前記センサが計測した計測データと
する構成と、 前記基準直線は、前記標準軌跡とし、 前記射影始点は、前記センサが計測した計測デ−タとし
、 前記距離比較点は、前記標準軌跡補間点に等しい近似基
準点とする構成をあげることができる。

そして、前記基準直線の好ましい態様として、前記基準
直線は、隣合う前記計測データを逐次結んで得られるベ
クトル、あるいは所定の間隔を有する２つの前記計測デ
ータを逐次結んで得られるベクトル、あるいは前記計測
データと該計測データを検出した時点での工具端データ
を結んで得られるベクトルのいずれか一つで生成される
構成をあげることができる。

また、前述のワーク形状記憶手段Ｒ５の好ましい態様と
して次のような構成をあげることができる。

すなわち、前記ワーク形状記憶手段Ｒ５が、前記軌跡補
正手段からの次に工具が移動すべき目標の作業点位置デ
ータを入力し、前記目標の作業点位置データより前の不
要となったワーク形状近似データを消去することを特徴
とする構成である。

［作用］ 上記構成を有する本発明の倣い制御ロボットＲ４は、次
のようにして工具Ｒ２の設けられた手先Ｒ１を制御する
。

（１）　ワーク形状記憶手段Ｒ５は、ワーク形状を計測
するセンサＲ３を用いて、工具Ｒ２に先行してワーク形
状近似データを生成し、記憶する。

（２）　標準軌跡補間点生成手段Ｒ６は、工具Ｒ２が次
に動くべき標準軌跡上の標準軌跡補間点を生成する。

（３）　ワークデータ選択手段Ｒ７は、標準補間点に対
応する近似基準点を生成すると共に、近似基準点を用い
てワーク形状近似データの中で軌跡を補正するのに最も
適切なものを選択する。

（４）　軌跡補正手段Ｒ８にて選択されたワーク形状近
似データから次に移動すべき工具Ｒ２の作業点位置およ
び工具姿勢を求め、ロボット制御手段Ｒ９にてこの工具
Ｒ２の作業点列へ工具Ｒ２を到達させるよう手先Ｒ１を
制御する。

すなわち、本発明では、第２図の説明図のように、次に
工具Ｒ２が移動すべき位置・姿勢を求める際に、 作業のスピード（すなわち工具Ｒ２の移動スピード）お
よび経路を決定する標準軌跡Ｓ上で工具Ｒ２が次に移動
すべき標準軌跡補間点Ｓｄに対応して生成された近似基
準点Ｎｄを求め、この近似基準点Ｎｄに空間的に最も距
離の小さい関係にあるワーク形状近似データＷｄを選択
し、その選択されたワーク形状近似データＷｄより、次
に移動すべき工具Ｒ２の作業点位置および工具姿勢を演
算して工具Ｒ２の軌跡を補正する。

したがって、作業中に作業のスピードおよび経路の変更
を行っても、その情報は標準軌跡補間点に直接反映され
るため、工具Ｒ２はその変更された作業スピードおよび
経路に正確に追従して移動することができ、正しい倣い
を行うことができる。

［実施例］ 本発明の第１実施例を説明する。

本実施例は、次に作業する位置のワーク形状をセンサに
よって測定しなからレーザ光により作業する６軸型のロ
ボットに関する。

第３図の斜視図にこのロボット１０の構成を示す。

このロボット１０は、該ロボット１０の動作制御部であ
るロボットコントローラ１２、該ロボットコントローラ
１２に設けられ作業者がロボット１′０に仕事を教示す
るティーチングボックス１４、ロボットコントローラ１
２からの信号によって動作する６軸の動作Ｍ′Ｊ１６、
動作部１６の手先１８に設けられた工具２０およびワー
クＷの形状を検出するセンサヘッド２２、該センサヘッ
ド２２からの信号を処理するセンサコントローラ２４を
備える。また、ロボットコントローラ１２は人出力のた
めのターミナルＴ、プリンターＰを備える。動作部１６
、手先１８の構成の詳細については後述する。

にで工具２０はレーザ加工具であって、図示されない外
部の発振器で発振された大出力のレーザ光を、ライトガ
イド２６により、工具２０の作業端まで導き、ワークＷ
上に集光させている。レーザ加工具２０の他の要素であ
るミラー、プリズム、レンズ等は、本発明の要旨に直接
関与しないので図示しない。

センサヘッド２２は、距離検出センサを３つ絹み合わせ
てワークＷの位置姿勢を検出する。ごのセンサヘッド２
２の出力はセンサケーブル２８によって、センサコント
ローラ２４に送られ、処理される。このセンサヘッド２
２、センサコントローラ２４の詳しい内容についても後
述する。

第４図を用いて、ロボット１０の動作部１６を詳細に説
明する。なお、第４図（ａ）は動作部１６の平面図、　
（ｂ）は正面図、　（ｃ）は左側面図である。

この動作部１６は、第１軸から第６軸までの６個の軸を
有する。この内、第１軸から第３軸によって手先１８の
ワークＷに対する位置が制御され、第４軸から第６軸に
よって手先１８のワークに対する姿勢が制御される。

各軸には、各々の軸を中心として、回転駆動するモータ
、その回転を停止させるブレーキ、その回転量を測定す
るエンコーダが設けられている。

即ち、第１軸には、第１軸用モータ３０、第１軸用ブレ
ーキ３２、第１軸用エンコーダ３４が、第２軸には、第
２軸用モータ４０、第２軸用ブレーキ４２、第２軸用エ
ンコーダ４４が、第３軸には、第３軸用モータ５０、第
３軸用ブレーキ５２、第３軸用エンコーダ５４が、第４
軸には、第４軸用モータ６０、第４軸用ブレーキ６２、
第４軸用エンコーダ６４が、第５軸には、第５軸用モー
タ７０、第５軸用ブレーキ７２、第５軸用エンコーダ７
４が、第６軸には、第６軸用モータ８０、第６軸用ブレ
ーキ８２、第６軸用エンコーダ８４が、各々設けられて
いる。第２軸、第３軸は、各々そのモータ４０．５０の
回転運動をポールネジフィード９０．９２により直線運
動に変換し、これを平行リンク機構の軸回転運動に変換
することにより、駆動される。他の軸は、その軸のモー
タの回転をトルクチューブで伝え、平歯車、ハスバ歯車
等を用いて減速して駆動される。

第５図の斜視図を用いて、動作部１６の手先１８を詳細
に説明する。

この手先１８には、前述の如くワークＷを加工するレー
ザ加工具２０と、ワークＷの形状を検出するセンサヘッ
ド２２とが、所定の距離を隔てて配置されている。そし
て、加工具２０とセンサヘッド２２との相対的な位置関
係は手先１８が移動しても常に一定である。このセンサ
ヘッド２２には、前述の如く、３個の距離検出センサ１
０２．１０４．１０６が設けられている。

ロボットｌＯは、この手先１８を、センサヘッド２２が
レーザ加工具２０ζこ先行するように移動させながら、
ワークＷを加工する。即ち、センサヘッド２２は、レー
ザ加工具２０が現在加工している点ＰＯではなく、将来
レーザ加工具２０が加工するであろう点Ｐ１の形状を測
定する。

この測定は次のようにして行われる。センサヘッド２２
に設けられた距離検出センサ１０２．１０４．１０６は
、将来レーザ加工具２０が加工するであろう点Ｐ１の近
傍にある点ｐＨ、ＰＩ３、ＰＩ３と距離検出センサ１０
２．１０４．１０６との各々の相対的方向、距離を後述
の手法により測定し、その結果から、センサコントロー
ラ２４は前述の点Ｐ１の形状（位置、傾き等）を、算出
する。尚、便宜上、第５図において、点Ｐ１と点ｐＨ、
ＰＩ３、ＰＩ３とが隔たっているように描いであるが、
実際には僅かな距離しか離れていない。

ごこて、第６図を用い、距離検出センサ１０２．１０４
．１０６とセンサコントローラ２４との構成についで更
に詳細に説明し、併せて本実施例における距離検出の手
法について簡略に説明する。

距離検出センサ１０２．１０４．１０６は、ワークＷに
向けて発射されたレーザ光のワークＷ上の光点の結像位
置によってワークＷまでの距離を検出するものである。

これらはいずれも同じ構成であるので、距離検出センサ
１０２についてのみ説明する。

距離検出センサ１０２は、約７５０　ｕｍのレーザ光を
発射するレーザダイオード２００、このレーザダイオー
ド２００を所定のデユーティでオンオフ制御即するパル
ス変調器２０２、発射されたレーザ光を平行光とするコ
リメータレンズ２０４、ワークＷ上に形成されるレーザ
光のスポットからの乱反射を入光し長さ２Ｌの１次元Ｐ
　Ｓ　Ｄ　（Ｐｏｓｉｔｉｏｎ　５ｅｎｓｉ−ｔｉｖｅ
　Ｄｅｖｉｃｅ）　２０６上に結像させる受光レンズ２
０８およびＰ　Ｓ　Ｄ　２０６の光生成型９ｍ　Ｉ　Ｏ
を検出し電圧信号に変換する２つの前置増幅器２１０．
２１２を備える。Ｐ　Ｓ　Ｄ　２０６はシリコンホトダ
イオードを応用した光検出器であり、高抵抗シリコン基
板（１層）の表面にＰ型紙抗層を設け、その両端に信号
取り出し用の２つの電極を、高抵抗シリコン基板の裏面
に共通電極を各々設けた構造を有する。このＰ型紙抗層
に光が入射すると、この位置で発生する光生成電流Ｉｏ
は、人別位置から両端の電極までの抵抗値に逆比例して
分割され、各々の電極から取り出される。即ち、長さ２
ＬのＰＳＤ２０６の中心から距離ΔＸだけ変位した位置
にワークＷ上のスポットが結像された場合には、両端の
電極から取り出され前置増幅器２１０．２１２に入力さ
れる電流の大きさは、各々、 となる。

さらに、レーザダイオード２００、コリメータレンズ２
０４、受光レンズ２０８、ＰＳＤ２Ｑ６からなる光学系
は、 （１）　発射光軸と反射光軸とが角度δをなす場合に反
射光軸がＰＳＤ２０６の中心を通ること、（２）　この
場合に反射光軸とＰＳＤ２０６とのなす角θが、光点Ｌ
Ｃの位置によらず光点ＬＣがＰ　Ｓ　Ｄ　２０６上に正
しく焦点を結ぶような角度に選ばれていること、 の２点を満足させるよう構成されている。従って、受光
レンズ２０８の焦点距離をｆ、発射光軸と反別光軸とが
角度δをなすときの光点ＬＣから受光レンズ２０８まで
の距離をＡｏ、同じく受光レンズ２０８からＰ　Ｓ　Ｄ
、２０６の中心までの距離をＢｏとすると、距離検出セ
ンサとワークＷとの距離の変位ＸとＰ　Ｓ　Ｉ）２０６
上の変位ΔＸとは、ΔＸ＝ ・・・（２） の関係を有する。従って、式（１）および焦点距離ｆに
ついての関係式（，１／　ｆ　＝　１／Ａ　ｏ　＋　１
／Ｂ　ｏ　）から、次式（３）を得る。

Δｘ／Ｌ＝（ＩＩ−Ｉ２）／、（１１＋ｌ２）＝に＋ｘ
／　（１＋α・Ｘ）　　・・・（３）但し、　Ｋ＝ｃｏ
ｓ　　δ　（Ｂｏ−ｆ）／（Ｌ＋ｃｏｓ　　θ　（Ａｏ
−ｆ））α＝ＣＯ８δ／（Ａｏ−ｆ） 従って、Ｐ　Ｓ　Ｄ　２０６の出力電流■１、Ｉ２を知
れは、ワークＷと距離検出センサとの距離の偏差Ｘを知
ることができるが、本実施例で、式（３）に相当する演
算を行うのがセンサコントローラ２４の３つの演算部２
１４．２１６．２１８である。この演算部２１４．２１
６．２１８はいずれも同じ構成であるので演算部２１４
についてのみ説明する。

センサコントローラ２４の演算ＫＩＪ２１４は、ＰＳＤ
２０６の出力電流■１、Ｉ２に相当する信号を、各々入
力バッファ２２０．２２２を介して位置演算器２２４に
入力し、（ＩＩ−Ｉ２）／（１１＋Ｉ２）に相当する演
算を実行する。

位置演算器２２４の出力（Δｘ／Ｌに比例）はリニアリ
ティ補正回路２２６に入力され、にでワークＷの変位Ｘ
と上述したΔｘ／Ｌとの関係が直線となるよう補正が行
われる。これは先に式（３）として示したように、両者
は非直線的な関係を有するためである。

リニアリティ補正回路２２６の出力は、Ａ／Ｄ変換器２
２８を通った後に、ロボットコントローラ１２内のセン
サ入力ボート２３０に渡される。

次に、第７図によって、６軸の動作ｉｉＢ　１６を制御
するとともに、ティーチング装置としても働くロボット
コントロ−ラ ロボットコントローラ１２は、周知の第１ＣＰＵ３００
、第２　Ｃ　Ｐ　Ｕ３０１、Ｒ　Ｏ　Ｍ２Ｏ３、ＲＡＭ
３０４、バックアップＲ　Ａ　Ｍ２Ｏ［３等を中心に、
バス３０８により相互に接続された人出力ボート・回路
と共に算術論理演算回路として構成されている。

第１ＣＰＵ３００、第２ＣＰＵ３０１等と共にロボット
コントローラ１２を構成する人出力ポート・回路として
は、バブルメモリ３１０にデータを記憶したり読み出す
といった処理を行うバブルメモリインターフェイス３１
２、ＣＲＴデイスプレィとキーボードとを備えたターミ
ナルＴとデータの授受を行うターミナル人出力ボート３
１４、プリンタＰにデータを出力するプリンタ用出力ボ
ート３１６、ティーチングボックス１４の操作状態を入
力するティーチングボックス入力ボート３１８、センサ
１０２、１０４、１０６の出力する電気信号をセンサコ
ントローラ２４を介して入力しＣＰＵ３０１に対して割
り込みを発生させるセンサ入力ボート２３０、および上
述した６軸（第１軸、第２軸、第３軸、第４軸、第５軸
、第６軸）用のサーボ回路３２０、３２２、３２４、３
２６、３２８、３３０が備えられている。

各軸周サーボ回路３２０ないし３３０は、何れも個々に
ＣＰＵを備えたインテリジェントタイプのものであり、
第１ＣＰＵ３００からバス３０８を介して、レーザ加工
具２０先端の目標位置および目標姿勢のデータが与えら
れると、第１軸ないし第６軸用エンコーダ３４、４４、
５４、６４、７４、８４から各軸の制御量を読み込み、
第１軸ないし第６軸用ドライバ３３２、３３４、３３６
、３３８、３４０、３４２を介して第１軸ないし第６軸
用モータ３０、４０、５０、６０、７０、８０を駆動し
、レーザ加工具２０の先端を目標位置まで移送し、更に
目標姿勢に同時に制御する。尚、第１軸ないし第６軸用
サーボ回路３２０、３２２、３２４、３２６、３２８、
３３０は、各々第１軸ないし第６軸用ブレーキ３２、４
２、５２、６２、７２、８２も適時制御して、非常停止
などでサーボ電源（モータ電源）が切られてもアームが
自重で動作または、下へ垂れることを防いでいる。

本ロボットコントローラ１２は、第８図の概略構成図に
示すように、 予めティーチングボックス１４を用いてティーチング動
作により教示された位置データＰ　Ｔ　ｔｃｈを標準軌
跡として記憶する標準軌跡記憶部４００と、標準軌跡記
憶部４００に記憶される位置データより、次に工具２０
の移動すべき標準軌跡上の標準軌跡補間点である作業点
目標データＰ　Ｔ　ｐａ．ｔｈを出力する標準軌跡補間
点生成部４０２と、 センサ２２からワークＷの計測データＳ　ｓｅｎおよび
手先１８のフランジ端データＦｒａｎｇｅ−ｃｅｎｔｅ
ｒ−ｄａｔａを入力し、ワーク形状データＰＳｗａｌｌ
、作業点データＰ　Ｔ　ｔｏｏｌ、Ｐ’ｌ”ｔｏｏｌか
らＰＳｗａｌｌに至るベクトルＬおよびベクトルＬの長
さＩＬＩを算出するワーク形状近似部４０４と、 −ク形状記憶部４０６と、 前述の標準軌跡補間点生成部４０２から出力される作業
目標点データＰＴｐａｔｈと後述する軌跡補正部４１４
から出力される補正量ΔＰＴとから、次の工具作業推定
点Ｐ　Ｔ　ｐｍｏｃｊｄａｓｈを算出する推定工具点演
算部４０８と、 ワーク形状記憶部４０６に格納されるワーク形状を近似
するベクトルＬに対して、推定工具点演算部４０８から
出力されるＰ　Ｔ　ｐｍｏｃｊｄａｓｈを射影した選択
する推定工具点射影演算判定部４１２と、上記選択され
たベクトルに対応するワーク点データＰＳ１ｗａｌｌと
標準軌跡補間点生成部４０２で生成されたＰ　Ｔ　ｐａ
ｔｈとから軌跡補正値ＰＴｍｏｄおよび補正量ΔＰＴを
算出する軌跡補正ｇｉＰＪ４１４と、軌跡補正部４１４
で算出された軌跡補正値ＰＴｍ。

ｄに工具２０を移動するよう制御するロボット制御部４
１６と を備える。

第９図は、ワークＷ、手先１８、工具２０、センサ２２
および上記各員の関係を示す図である。

そして、本実施例は、上記のような構成のロボットコン
Ｉ・ローラ１２を用いるため、以下のように動作する。

■　ワーク形状近似部４０４で得られたワーク点データ
ＰＳｗａｌｌや近似ベクトルＬをワーク形状記・塗部４
０６に格納する。

■　推定工具点演算部４０８において、推定工具点Ｐ　
Ｔ　ｐＩＴｌｏｄ−ｄａｓｈを作業点目標データＰ　Ｔ
　ｐａｔｈと前回の軌跡補正で用いた補正量ΔＰＴから
求める。

■　推定工具点射影演算部４１０において、推定工具点
Ｐ　Ｔ　ｐｍｏｄ−ｄａｓｈを近似ベクトル群に射影し
、近似ベクトルＬの長さと射影点までの長さの差分を求
める。

■　推定工具点射影演算判定部４１２において、長さの
差分のもっとも小さいときのワーク点データＰＳ１ｗａ
ｌｌを選択する。

■　軌跡補正部４１４において、選択したワーク点デー
タＰＳ１ｗａｌｌから軌跡補正量ΔＰＴを求め、作業点
目標データＰ　Ｔ　ｐａｔｈとから新たな軌跡指令値Ｐ
Ｔｍｏｄを作る。

■　ロボット制御部４１６において、軌跡指令値ＰＴｍ
ｏｄに基づいて手先１８を制御する。

第１０図のブロック図を用いて、ロボットコントローラ
１２の動作をさらに詳細に説明する。

また、第１１図に本ロボットコントローラ１２で使用す
る各員の関係を示す。

先ず、ロボット１０を動作させる前に従来のロボットと
同じくティーチングボックス１４を用いて、ワークＷに
対してレーザ加工具の作業点２１の通過すべき希望位置
とその時の姿勢をロボット１０に教示し、メモリに格納
しておく。教示方法は、例えば、レーザ加工具２０の先
端にオフセットバーをつけ、オフセットバーをワークに
接触させることによって行う。このとき、ロボットはロ
ボットの基準位置からオフセットバー先端までの関係を
次の形式で標準軌跡記・塗部４００に格納しておく。

このｍ番目の教示データは次に示すマトリックスで現さ
れる。

・・・（４） ことで、 ＮＸ、ＮＹ、ＮＺは、ノーマルベクトルの各要素、○Ｘ
、０■、ＯＺは、オリエントベクトルの各要素、ＡＸ、
ＡＶ、ＡＺは、アプローチベクトルの各要素、Ｘ、Ｙ、
Ｚ　は、教示点の位置ベクトルの各要素を表すものであ
って、これらは何れもロボットの動作部１６の動作の基
準点を原点とする座標系〇−×■ｚであられされたデー
タである。また、ノーマルベクトル、オリエントベクト
ル、アプローチベクトルとは、各々直交するベクトルで
あって、上記マトリックスで表される教示点での工具の
加工の方向を表すものである。尚、以下の説明に用いら
れる各点を示す変数もこのマトリックスと同様の形式で
ある。

上記の教示が終了すると、作業開始信号により、ロボッ
ト１０は作業を開始する。

先ず、上記バブルメモリ３１０中に確保された標準軌跡
記憶部４００から出力された標準軌跡ＰＴｔｃｈを、標
準軌跡補間点生成部４０２で補間して、工具２０の軌跡
補間点データＰＴｐａｔｈを算出し、ＲＡＭ３０４中の
所定領域である軌跡補間点記憶器４２０へ入力する。

軌跡補正部４１４は、軌跡補間点記憶器４２０から軌跡
補間点データＰ　Ｔ　ｐａｔｈを受は取ると、軌跡補間
点データＰＴｐａｔｈに後述する補正量ΔＰＴおよび目
標オフセットＯｆｓを目標オフセット加算器４１４ｄに
て以下の式４，５に示すように加え、実際の作業点修正
データＰ　Ｔ　ｍｏｄとし、ロボット制御部４１６のジ
ヨイント逆変換回路４１６ａに入力する。とこで、Ｏｆ
ｓは、工具端座標系でみた作業点座標値のことで作業点
と工具端座標との相対関係を与えるものである。

Ｐ　Ｔ　ｐｍｏｄ　＝　Ｐ　Ｔ　ｐａｔｈ　◆ΔＰＴ−
（４）ＰＴｍｏｄ　＝ＰＴｐｍｏｄ◆Ｏｆｓ　　−（５
）この時、Ｐ　Ｔ　ｐａｔｈ、ΔＰＴおよびＰＴｍｏｄ
の間の関係は前述の第９図に示すようになる。

ジヨイント逆変換回路４１６ａは、前述の作業点修正デ
ータＰＴｍｏｄを各軸の移動角度θｈｏｓ　ｔに変換し
、各軸周サーボ回路３２０〜３３０に出力し、各軸周ド
ライバー回路３３２〜３４２を介して各軸周モータ３０
．４０．５０．６０．７０．８０を駆動して、工具端を
作業点修正データＰ　Ｔ　ＩＴｌｏｄの位置に移動させ
るよう制御する。

次に、上述の補正量ΔＰＴの算出について順次説明する
。

先ず、ワーク形状近似部４０４には、ワークＷの計測デ
ータ５ｓｅｎおよび手先１８のフランジ端データＦｒａ
ｎｇｅ−ｃｅｎｔｅｒ−ｄａｔａが入力される。

フランジ端データＦｒａｎｇｅ−ｃｅｎｔｅｒ−ｄａｔ
ａは、各軸周エンコーダ３４．４４．５４．６４．７４
．８４から入力されるジョイント角データθｅｎｃを、
ジヨイント順変換回路４２２でフランジ端データＦｒａ
ｎｇｅ−ｃｅｎｔｅｒ−ｄａｔａに変換することで得ら
れる。即ち、以下の式（６）を実行することにより得ら
れる。

Ｆ　ｒａｎｇｅ−ｃｅｎｔｅｒ　ｄａｔａ　＝　Ａ　［
θｅｎｃｌ　−（６）にで、Δはジヨイント順変換 ［］はベペクトを表す 次いで、ワーク形状近似部４０４で、このフランジ端デ
ータＦｒａｎｇｃ、ｃｅｎｔｅｒ−ｄａｔａからセンサ
一端データＰＳおよび工具作業点実データＰＴｅｎｃを
以下の式（７）、　　（８’）により求める。

Ｐ　Ｓ　＝　Ｆ　ｒａｎｇｅ　ｃｅｎｔｅｒ−ｄａｔａ
　◆ｌ１ｆ−＋Ｓ−・−（７） Ｐ　Ｔｅｎｃ　＝　Ｆｒａｎｇｅ−ｃｅｎｔｅｒ−ｄａ
ｔａ　　÷口ｆ＋Ｔ・・・（８） にでＩ”Ｉｆ＋Ｓはフランジ端に対するセンサ一端の位
置を示す位置・姿勢マトリクスであり、口Ｔ→Ｓはフラ
ンジ端に対する工具作業点の位置を示す位置・姿勢マト
リクスである。

一方、ワーク形状近似部４０４にセンサコントローラ２
４から入力された測定点のセンサ２２とワークＷとの相
対位置５、形状に関するセンシングデータＳ　ｓｅｎは
、上記センサ端データＰＳを用いて、ワーク形状データ
ＰＳｗａｌｌに変換され、ワーク形状記憶部４０６コこ
入力される。尚、このセンシングデータＳ　ｓｅｎは非
常に短いサンプリング間隔で得られたデータを平均化し
たものでも良い。

ワーク形状データＰＳｗａｌｌは次式（９）によって求
まる。

ＰＳｗａｌｌ＝ＰＳ　＋５ｓｅｎ　　　　−（９）次い
で、同じくワーク形状近似部４０４において、工具２０
、ワークＷ間の位置姿勢の基準値○ｆｓと工具作業点実
データＰ　Ｔ　ｅｎｃとを用いて作業点座標ＰＴｔｏｏ
ｌを以下の式（１０）により算出し、ＰＴｔ。

ｏｌの位置成分をワーク形状記・塗部４０６に入力する
。

Ｔｔｏｏｌ ＝ＰＴｅｎｃ・○ｆｓ−”　　　　　−・−（１０）に
で、Ｏｆｓは、加工条件等の外部からの情報や、ロボッ
ト１０を制御するプログラム中の命令により変更される
データである。

また、ワーク形状の近似法の１手段として、前記ワーク
形状データＰＳｗａｌｌの位置成分（Ｘｗａｌｌ、Ｙｗ
ａｌｌ、Ｚｗａｌｌ）と作業点座標ＰＴｔｏｏｌの位置
成分（Ｘｔｏｏｌ、Ｙｔｏｏｌ、　　Ｚｔｏｏｌ）より
ＰＴｔｏｏｌからＰ＝　（Ｘｗａｌｌ、ＹｗａｌＬＺｗ
ａｌｌ）−（Ｘｔｏｏｌ、Ｙｔｏｏｌ、Ｚｔｏｏｌ）　
　−（１１）ワーク形状近似部４０４で得られた同時刻
におけるデータはワーク形状記憶部４０６の内部では第
１１図のように収納されている。同時刻のデータがそろ
うとバッファリングカウンタ部４２４でカウンタを１だ
けインクリメントする。なお、第１１図中においてＫＨ
ｌ、ＫＨＩ　−ｎは時系列を示すものであり、ＫＨＩが
現在であり、ＫＨＩ−ｎはｎサンプル前を意味する。

次いで推定工具点滴算器４０８乙こおいて、作業点目標
データＰＴｐａｔｈと直前の軌跡補正において軌跡補正
部４１４の補正量演算器４１４ａで得られた補正量ΔＰ
Ｔを用いて、次の工具作業推定点ＰＴｐｍｏｃｌ−ｄａ
ｓｈを式（１２）により求める。

Ｐ　Ｔ　ｐｍｏｃｊｄａｓｌ］＝　Ｐ　Ｔ　ｐａｔｈ　
・△Ｐ　Ｔ　（ＭＨＩ−１）・・・（１２） にで、Ｋｔ−１−１は１サンプル前を意味する。

作業のスピード変化は一般に標準軌跡で与えられるもの
であるため、軌跡補間点データＰＴｐａｔｈによって与
えられる速度と工具端が現在に至るまでの過去の軌跡、
即ち補正量△Ｐ　Ｔ　（Ｋｌｌｌ−１）から次ぎに工具
端が到達すべき位置Ｐ　Ｔ　ｐｎ＋ｏｄ−ｔｌａｓｈを
推定することによりスピード変化に対応した工具端位置
が推定でき、かつこの工具作業推定点ＰＴρｍｏｔｊｄ
ａｓｈに距離的に最も近いワーク形状データＰＳｗａｌ
ｌを後述の手段によって求めるため、ロボット１０の動
作中のスピード変更しこ柔軟こ乙対応できる。

推定工具点射膨部４１０では、先ずベクトル演算器４１
０ａにおいて上記工具作業推定点ＰＴρ…ｏｄ−ｄ＝ａ
ｓｈの位置成分（Ｘｐｍｏｄ−ｄａｓｈ　、　　Ｙｐｍ
ｏｄ−ｄａｓｈ　。

Ｚ　ｐｍｏｃｊｄａｓｈ　）と、ワーク形状記憶部４０
６から取出した作業点座標ＰＴｔｏｏｌの位置成分（Ｘ
ｔｏｏｌ。

Ｙ　ｔｏｏｌ、　　Ｚ　ｔｏｏｌ）より、ＰＴｔｏｏｌ
からＰ　Ｔ　ｐｍｏｄ−ｄａｓｈに至るベクトル■を式
（１３）により求める。

■ ＝　（Ｘｐ＋ｎｏｄ−ｄａｓｈ　、　　Ｙｐｍｏｄ−ｄ
ａｓｈ　、　　Ｚｐｍｏｄ−ｄａｓｈ　）−（Ｘｔｏｏ
ｌ、Ｙｔｏｏｌ、Ｚｔｏｏｌ）　　−（１３）次いで、
工具作業推定点Ｐ　Ｔ　ｐｍｏｃｊｄａｓｈに空間的に
最も距離の小さい関係にあるワーク形状データＰＳ１ｗ
ａｌｌを算出するために、以下の処理を行う。

先ず、全てのワーク形状データＰＳｗａｌｌについて上
記位置関係を調べるのは計算量を増大させるたけである
ので、比較する時系列の範囲である囲い込み区間［ａ、
ｂｌを設定する。ａの初期値は現在よりｎサンプル前の
時系列ＫＨＩ　−ｎであり、ｂの初期値は現在の時系列
ＫＨＩである。なお、上記ｎは作業スピードおよび工具
・センサ間の距離など、作業条件により決定される値で
ある。

次いで角度演算器４１０ｂにおいて、ワーク形状記憶Ｍ
ＪＪ４０６から取り出した時系列ａにおけるワーク形状
近似ベクトルＬ（以下、各時系列のデータめる。

・・・（１５） 次いで三角関数演算器４１０Ｃにおいて、上記式でさら
に、差分演算器４１０ｄにおいて式（１６）で（ａ）の
大きさＩＬ（ａ）ｌの差を式（１７）より求める。

ｆ（ａ）　　＝　ｌ　Ｌ（ａ）　　ｌ　−ｌ　　ＩＬ（
ａ）　　Ｉ−（１７）次いで、ワーク形状記憶部４０６
に格納される時系列すにおけるデータを使用して、ベク
トル演算器４１０ａから差分演算器４１０ｄまでを繰り
返し、ｆ（ｂ）の値を算出する。

そして、上記推定工具点射影演算部４１０で得られたｆ
（ａ）、　　ｆ（ｂ）の値を推定工具点射影演算判定部
４１２の差分比較器（１）　４１２ａに入力し、式（１
７）によって比較する。

式（１７）が成立しない場合には、制御を異常処理部４
２６に移し、直前の軌跡補正において補正量演算器４１
４ａで得られた補正量ΔＰＴ　（Ｈｌ−１）を補正量Δ
ＰＴとして軌跡補正部４１４の軌跡指令値演算器４１４
ｂに渡す。

式（１７）が成立する場合には、囲い込み区間チエツク
器４１２ｂにおいて、囲い込み区間である時系列ａとｂ
とを式（１８）によって比較する。

ｂ−ａ≦１　・・・（１８） 上式が成立する場合には、時系列ａおよびｂにおける関
数ｆ　（Ｘ）の値の小さい方の時系列で収納されている
第１１図のワーク形状データＰＳｗａｌｌを軌跡補正に
用いるワーク形状データＰＳ１ｗａｌｌとする。

一方、式（１８）が成立しない場合には、囲い込み区間
中点演算器４１２ｃにおいて時系列ａとｂの中間点Ｃを
求める。

そして、求めた中間点Ｃについて、推定工具点射影演算
部４１０のベクトル演算器４１０ａから差分演算器４１
０ｄまでを繰り返し、ｆ　（ｃ）を求めるよう、読み出
し信号をワーク形状記憶部４０６に送る。

ついで、差分比較器（２）　４１２ｄｔこおいて、中間
点Ｃにおける関数ｆ　（ｃ）の値を調べ、ｆ（ｃ）＞Ｏ
ならば、上記すをＣに置き換え、ｆ（ｃ）＜Ｏならは、
上記ａをＣに置き換え、上記処理を繰り返して軌跡補正
に用いるワーク形状データＰＳ１ｗａｌｌを求める。

また、上述の時系列ｋＨ１における選択されたワーク形
状データＰＳ１ｗａｌｌは、以下のようにして求めるこ
ともできる。

最も小さいｋｌ２が得られた場合には、３つのワーク形
状データＰ　Ｓ　ｗａｌ　１（ＫＨ２−１）、Ｐ　Ｓ　
ｗａ　Ｉ　Ｉ　（ｋｌ２）、Ｐ　Ｓ　ｗａ　ｌ　ｌ　（
ＫＨ２＋１）を内挿することによってＰＳｌｗａｌ　Ｉ
（ｋｌ−１１）を求めてもよい。例えば、位置ペクト・
・・（２０） Ｐ　Ｔ　ｐａｔｈはＰＴｐａｔｈの位置ベクトル＋　Ｖ
　３　◆Ｐ　Ｓｗａｌｌ（ＫＨ２＋１）　　　−（２２
）このようにして、ＰＳｌｗａｌｌを求めるのは、ロボ
ッＩ・コントローラ１２がサーボ回路３３２〜３４２に
目標点データを出力する間隔よりも、センサコントロー
ラ２４から測定点の位置、形状データが入力される間隔
の方が短いためであったり、ロボット１０の速度が低速
から高速に変化したりするためである。即ち、ワーク形
状記・連部４０６には、一般に、１個以上のワーク形状
データが格納されているのである。そのため、その多数
のデータの中から最も適切なデータを選ぶ必要があるの
である。

また、この様に構成することにより、ロボット１０の作
業スピードを変化させた場合も乙も適正な作業目標点座
標を設定できる。例えば、センサコントローラ２４から
出力されるデータ数とロボットコントローラ１２から出
力されるデータ数とが同程度であると、作業中にロボッ
ト１０の作業スピードが高速から低速に変化する場合に
、目標点に対応するワークＷの情報が得られない場合が
ある。しかし、本実施例では、センサコントローラ２４
から出力された疎らなデータを内挿補間して用いるので
、その様な問題は起とらない。

上述のようにして、ワーク形状記憶部４０６から選択さ
れたワーク形状データＰＳ１ｗａｌｌが出力されると、
このＰＳｌｗａｌｌと前述の軌跡補間点記・憶器４２０
によって得られた作業点目標データＰＴｐａｔｈとの差
Ｒｏｆｓを次式により求める。

　　ｏｆｓ ＝　Ｐ　Ｔｐａｔｈ−’　◆Ｐ　Ｓ　１　ｗａｌｌ・・
・（２３） そして、補正要素選択器４１４Ｃによってセンサ２２が
計測した要素に対応するマトリックス成分のみを、Ｒｏ
ｆｓのデータを用いて、ΔＰＴを以下のように求める。

例えは、 ■　工具端座標系のＺ方向の距離のみを計測している場
合は、 ■　工具端座標系のＹ方向の距離のみを計測している場
合は、 ■　工具端姿勢と２方向の距離のみを計測している場合
は、 −ｏｆｓ ＝　Ｐ　Ｔｐａｔｈ　”　◆Ｐ　Ｓ　１　ｗａｌｌ　　
　−（２６）このようにして求めた補正量ΔＰＴは、異
常検出器４２８に入力され所定値βと比較される。

ΔＰＴの大きさ１ΔＰＴＩがβより大きいときはロボッ
ト１０が暴走している可能性があるので、例えば動作停
止という非常処理を行う。

逆に小さい時はΔＰＴを用いて軌跡指令値演算器４１４
ｂにおいて初段目標工具端座標Ｐ　Ｔ　ｐｍｏｄを式４
により求める。

また、ΔＰＴの補正要素に姿勢要素が含まれている場合
、１回の移動量が大きいと振動を発生する可能性がある
ので以下のような処理を行う。

■　作業点修正データＰ　Ｔ　ｍｏｄ（Ｋｌ−１１）と
ＰＴｍｏｄ（ＫＨＩ−１）の２つのマトリクスから主軸
回転角度θを求め、このθがロボット１０の最大加速度
で動作可＝４４− 能な角度α以上の場合はθをαに置き換えて新たにＰ　
Ｔ　ｍａｄ（ＫＨＩ）を作る。

■　主軸回転角度が例えば２°以下の場合は、姿勢補正
しない。

上述の処理を、第１ＣＰＵ３００Ｓ第２ＣＰＵ３０１で
実行する治れ図を第１３図、第１４図に示す。

本実施例に用いているロボットコントローラ１２は、既
ζこ述べたように、第７図に示す２つのＣＰＵ３００．
３０１を有し、この２つのＣＰＵ３００．３０１が並列
的に処理を行っている。第１３図に第１ＣＰＵ３００の
動作を示す流れ図を示し、第１４図に第２ＣＰＵ３０１
の動作を示す流れ図を示す。

作業開始信号により、ロボット１０が作業を開始すると
、第１ＣＰＵは、以下のステップを順次実行する。

ステップ５００：標準軌跡記憶部４００に記憶されてい
る標準軌跡の位置データＰＴｔｃｈを補間して、時系列
ＫＨＩにおける軌跡補間点Ｐ　Ｔ　ｐａｔｈを算出する
。

ステップ５０２：軌跡補間点データＰＴｐａｔｈを第２
ＣＰＵ！こ送り、第２ＣＰＵに作業点修正データＰＴ＋
ｎｏｄを要求する。

ステップ５０４：第２ＣＰＵから作業点修正データＰＴ
ｍｏｄを入力する。

ステップ５０６：目標点データＰＴｍｏｄを各軸の移動
角度θｈｏｓｔに変換する。

ステップ５０８：　θｈｏｓｔを各軸周サーボ回路３２
０〜３３０に出力する。

ステップ５１０：時系列ＫＨＩを１だけインクリメント
し、上記ステップ５００以降を繰り返して実行すること
により、次の時系列の処理を行う。

第２ＣＰＵ３０１は以下の処理を行う。尚、ステップ６
０２〜６０４とステップ６１０〜６３８は、並行してマ
ルチタスクで実行され、ステップ６０２〜６０４は、セ
ンサーコントローラ２４のサイクルタイムで実行され、
ステップ６１０〜６３８はロボットコントローラ１２の
サイクルタイムで実行される。

ステップ６００：第２ＣＰＵボードの初期設定を行う。

ステップ６０２：センサコントローラ２４から入力され
た測定点の位置、形状データＳ　ｓｅｎを入力する。

ステップ６０４：センサ２２の計測開始と同時に、各軸
のエンコーダ３４．４４．５４．６４．７４．８４から
ジヨイント角θｅｎｃを入力する。

ステップ６１０：第１ＣＰＵから作業点データＰＴ　ｐ
ａｔｈを入力する。

、　　ステップ６１２：作業点実データＰ　Ｔ　ｅｎｃ
、作業点座標ＰＴｔｏｏｌの位置成分、ワーク形状デー
タＰめ、ワーク形状記憶部４０６の時系列ＫＨＩに対応
する領域に格納する。

ステップ６１４：推定工具点Ｐ　Ｔ　ｐｍｏｄ−ｄａｓ
ｈを、前述の式により軌跡補間点データＰ　Ｔ　ｐａｔ
ｈおよび直前の軌跡補正に用いた補正量△ＰＴから求め
る。

ステップ６１６：ワーク形状記憶部４０６からＰＳｌｗ
ａｌｌを選択するための探索区間［ａ、　　ｂ］を求め
る。前述のように初期値は、ａ＝ＫＨ１−ｎ。

ｂ二ＫＨＩである。

ステップ６１８：ワーク形状記憶部４０６からＰＴステ
ップ６２０：データ探索区間［ａ、　　ｂ］において ｆ（ａ）＜ＩＬＩ ｆ（ｂ）＞０ の場合、ステップ６２２へ制御を移し、異常処理を行う
。

そうでなければ、ステップ６２４へ制御を移す。

ステップ６２２：前回算出したΔＰ　Ｔ　（ＫＨＩ−１
）を今回の処理のΔＰＴとし、制御をステップ６３６に
移す。

ステップ６２４：工具作業推定点Ｐ　Ｔ　ｐｍｏｃｊｄ
ａｓｈに空間的に最も近いワーク形状データが発見でき
たかチエツクする。

ｂ−ａ≦１の成立条件は、探索区間の両端の点が同一点
であるか、または隣合った場合であり、成立すると後述
のステップ６３２に制御を移し、成立しない場合には、
ステップ６２６に制御を移す。

ステップ６２６：データ探索区間内を１つづつチエツク
していては膨大な時間がかかるので、囲い込み手法を用
いて時間短縮を行う。そのため、データ探索区間［ａ、
　　ｂ］の中間点Ｃを求める。

ステップ６２８：中間点Ｃにおいてｆ　（ｃ）を求める
。

ステップ６３０　：　　ｆ（ｃ）　＞０　　ならば、Ｃ
を新たなりとし ｆ（ｃ）＜Ｏならば、Ｃを新たなａとして、ステップ６
２４に戻る。

ステップ６３２：ワーク形状記憶部４０６から選択され
た時系列のＰＳｗａｌｌを読みだし、このＰＳｗａｌｌ
をＰＳｌｗａｌｌとする。

ステップ６３４：選択されたワーク形状データＰＳｌｗ
ａｌｌと、軌跡補間点データＰＴｐａｔｈとの差Ｒｏｆ
ｓを前述の式により求め、Ｒｏｆｓと補正要素選択器４
１４ｃとを用いて、前述の式によって補正量ΔＰＴを求
め、異常検出器４２８で異常チエツクを行う。

ステップ６３６：軌跡補間点データＰＴｐａｔｈと補正
量ΔＰＴおよび目標オフセラ’ｐ　ｏｆｓから作業点修
正データＰＴｍｏｄを算出する。

ステップ６３８二作業点修正データＰＴｎ＋ｏｄを第１
ＣＰＵへ転送し、ステップ６１０に制御を移す。

以上の処理を繰り返すことによって、ロボッＩ・１０は
、次に作業するワーク上の点を測定しながら作業を行っ
ていく。

以上のような構成にすると、作業点位置姿勢記憶メモリ
は無限の容量を持つ二とが必要となるが、実際にはメモ
リへの人出力をリングバッファ形式として構成するので
、その心配はない。

また、従来のロボット制御では、標準軌跡補間点生成部
で補間された作業点目標データを直接ロボット制御部に
送り、ロボットを動作させていた。

それに対して、本実施例ではセンサー情報を基に作業点
データを軌跡補正部において新たに軌跡を作り直しロボ
ット制御している。

上述の如き構成を有する本実施例は次の効果を有する。

（１）　目標点を先に検出しているので高速な作業がで
きる。具体的には、従来のセンサによって倣い制御を行
うロボットの作業速度は、最大で、５０〜１００　ｍｍ
／ｓｅｃであったのに対し、本実施例のロボットは５０
０　ｍｍ／ｓｅｃの作業速度を持つ。

（２）　目標点を定める際に、センサ２２からの情報だ
けを基にせず、ティーチング点を参照しているので暴走
することがない。

（３）　時系列的に求められた目標点によって制御する
ので、正確な制御が出来る。例えは、現在作業を行って
いる点とセンサ２２で測定している点との間に急峻に変
化する部分があっても、その部分の目標点データは既に
あるので、正しく倣い作業が出来る。

（４）　スピード変化は一般に標準軌跡によって与えら
れるが、本実施例では逐次標準軌跡を参照することによ
り、スピード変化を瞬時に知りスピード変化に柔軟に対
応することができ、精度良く作業できる。

（５）　センサ２２からの情報を位置・姿勢マトリック
スで演算しているため、ある方向のみの１次元補正から
姿勢を含む３次元の補正まで汎用的に行える。

本発明の第２実施例を説明する。

本実施例は、上記ロボット１０を第１実施例と異なる処
理を行うロボットコントローラで制御するものである。

ロボット１０の構成は第１実施例と同じであるので説明
を省略する。

本実施例のロボットコントローラ１２は、第１５図の概
略構成図に示すように、 予めティーチングボックス１４を用いてティーチング動
作により教示された位置データＰ　Ｔ　ｔｅｌ］を標準
軌跡として記憶する標準軌跡記憶部７００と、標準軌跡
記・塗部７００に記憶される位置データより、次に工具
２０の移動すべき標準軌跡上の標準軌跡補間点である作
業点目標データＰ　Ｔ　ｐａｔｈを出力する標準軌跡補
間点生成部７０２と、 センサ２２からワークＷの計測データＳ　ｓｅｎおよび
手先１８のフランジ端データＦｒａｎｇｃ、ｃｅｎｔｅ
ｒ−ｄａｔａを入力し、ワーク形状データＰＳｗａｌｌ
を算出するワーク形状近似部７０４と、 ＰＳｗａｌｌを記憶するワーク形状記憶Ｈ７０６と、教
示の開始点から作業点目標データＰ　Ｔ　ｐａｔｈにい
たるベクトルＬ上に、ワーク形状記憶部７０６にるワー
ク形状工具点射影演算判定部７１０と、上記選択された
ベクトルに対応するワーク点データＰＳ１ｗａｌｌと標
準軌跡補間点生成部７０２で生成されたＰ　Ｔ　ｐａｔ
ｈとから軌跡補正値ＰＴｍｏｄおよび補正量ΔＰＴを算
出する軌跡補正部７１２と、軌跡補正部７１２で算出さ
れた軌跡補正値ＰＴｍ。

ｄに工具２０を移動するよう制御するロボット制御部７
１４と を備える。

上記ワークＷ５手先１８、工具２０、センサ２２および
上記各員の関係は前述の第９図に示したものと同じであ
る。

そして、本実施例は、上記のような構成のロボットコン
トローラ１２を用いるため、以下のように動作する。

■　ワーク形状近似部７０４で得られたワーク点データ
ＰＳｗａｌｌをワーク形状記憶部７０６に格納する。

■　ワーク形状射影演算部７０８において、ワーク点デ
ータ群を、標準軌跡上に射影し、射影点と作業点目標デ
ータとの距離の差分を求める。

■　ワーク形状演算判定部７１０において、距離の差分
の最も小さいときのワーク点データＰＳ１ｗａｌｌを選
択する。

■　軌跡補正部７１２において、選択したワーク点デー
タＰＳ１ｗａｌｌから軌跡補正量ΔＰＴを求め、作業点
目標データＰ　Ｔ　ｐａｔｈとから新たな軌跡指令値を
ＰＴｍｏｄを作る。

■　ロボット制御ｇｉＰＪ７１４において、軌跡指令値
ＰＴｎ＋ｏｄに基づいて手先１８を制御する。

第１６図のブロック図を用いて、ロボッ［・コントロー
ラ１２の動作をさらに詳細に説明する。

また、第１８図乙こ米処理で使用する各員の関係を示す
。

先ず、バブルメモリ３１０中に確保された標準軌跡記憶
部７００から出力された標準軌跡Ｐ　Ｔ　ｔｃｈを、標
準軌跡補間点生成部７０２で補間して、工具２０の軌跡
補間点データＰ　Ｔ　ｐａｔｈを算出し、ＲＡ　Ｍ２Ｏ
３中の所定領域である軌跡補間点記憶器７２０へ入力す
る。

軌跡補正部７１２は、軌跡補間点記憶器７２０から軌跡
補間点データＰ　Ｔ　ｐａｔｈを受は取ると、軌跡補間
点データＰＴｐａｔｈに後述する補正量ΔＰＴおよび目
標オフセラｆ−Ｏｆｓを目標オフセット加算器７１２ａ
にて上記式４．５乙こ示すよう乙こ加え、実際の作業点
修正データＰ　Ｔ　ｍｏｄとし、ロボット制御部７１４
のジヨイント逆変換回路７１４ａに入力し、第１実施例
と同様にして制御する。

次に、上述の補正量ΔＰＴの算出について順次説明する
。

先ず、ワーク形状近似部７０４には、ワークＷの計測デ
ータ５ｓｅｎおよび、ジヨイント順変換回路７２２を介
して手先１８のフランジ端データＦ　ｒａｎｇｅ−ｃｅ
ｎｔｅｒ−ｄａｔａが入力される。

そして、第１実施例と同様にしてワーク形状データＰＳ
ｗａｌｌを求め、ワーク形状記憶部７０６に記憶する。

次いで、作業点目標データＰ　Ｔ　ｐａｔｈに空間的む
こ最も距離の小さい関係にあるワーク形状データＰＳ１
ｗａｌｌを以下のようにして求める。

先ず、全てのワーク形状データＰＳｗａｌｌについて上
記位置関係を調べるのは計算量を増大させるだけである
ので、比較する時系列の範囲である囲い込み区間［ａ、
　　ｂ］を第１実施例と同様にして設定する。ａの初回
値は現在よりｎサンプル前の時系列Ｋ　Ｉ（１−ｎであ
り、ｂの初期値は現在の時系列ＫＨＩである。なお、上
記ｎは作業スピードおよび工具・センサ間の距離など、
作業条件により決定される値である。

次いで、ワーク形状射影演算部７０８のベクトル演算器
７０８ａにおいて、作業点目標データＰ　Ｔ　ｐａｔｈ
の位置成分（Ｘ　ｐａｔｈ、　　Ｙ　ｐａｔｈ、　　Ｚ
　ｐａｔｈ）と教示点の始点データの位置成分（ＯＸ、
　　Ｏｙ、　　０２　）より、教示点の始点からＰＴｐ
ａ廿にいたるベクトルＬを式２９により求める。

Ｌ＝　（Ｘｐａｔｈ、　　Ｙｐａｔｈ、　　Ｚｐａｔｈ
）−（○ｘ、　　Ｏｙ、　　０２　）　　−（２９）そ
して、ワーク形状記憶部７０６から取り出した時系列ａ
におけるワーク形状データＰ　Ｓ　ｗａｌ　１（ａ）の
位置成分（Ｘｗａｌｌ（ａ）　、　　Ｙｗａｌｌ（ａ）
　、　　Ｚｗａｌｌ（ａ））と教示点の始点データの位
置成分（Ｏｘ、　　Ｏｙ。

０２）より、教示点の始点からＰＳｗａｌｌにいたるベ
クトルＩ　（ａ）を式３０により求める。

Ｉ（ａ）　＝　（Ｘｗａｌｌ（ａ）　、　　Ｙｗａｌｌ
（ａ）　、　　Ｚｗａｌｌ（ａ）　）（ａ）との間の角
度ＣＯ８θを第１実施例と同様にして求め、三角関数演
算器７０８Ｃにおいて、上記式さらに、差分演算器７０
８ｄにおいて、上記ＩＩＬ（ａ）１とＩＬ＋の差ｆ　（
ａ）を第１実施例と同様にして求める。

次いで、ワーク形状記憶部７０６に格納される時系列す
におけるデータを使用して、ベクトル演算器７０８ａか
ら差分演算器７０８ｄまでを繰り返し、ｆ（ｂ）の値を
算出する。

そして、第１実施例の推定工具点射影演算部４１２と同
様の構成であるワーク形状射影演算判定部７１０によっ
て、第１実施例と同様にしてワーク形状データＰＳ１ｗ
ａｌｌを選択する。

続いて、第１実施例と同様の構成による軌跡補正部７１
２によって軌跡補正値ＰＴｍｏｄを算出し、ロボット制
御部７１４に送る。

また、第１実施例と同様の構成の異常処理部７２６、異
常検出器７２８によって、異常データによって制御され
ないようにする点についても第１実施例と同様である。

上述の処理を、第１　ＣＰ　Ｕ３００、第２ＣＰＵ３０
１で実行する手順を説明する。なお、第１ＣＰＵの処理
は第１実施例と全く同じであるので、説明を省略する。

第１９図に第２ＣＰＵ３０１の動作を示す流れ図を示す
。

第２ＣＰＵ３０１は以下の処理を行う。尚、ステップ８
０２〜８０４とステップ８１０〜８３６は、並行してマ
ルチタスクで実行され、ステップ８０２〜８０４は、セ
ンサーコントローラ２４のサイクルタイムで実行され、
ステップ８１０〜８３６はロボットコントローラ１２の
サイクルタイムで実行される。

ステップ８００：第２ＣＰＵボードの初期設定を行う。

ステップ８０２：センサコントローラ２４から入力され
た測定点の位置、形状データＳ　ｓｅｎを入力する。

ステップ８０４：センサ２２の計測開始と同時に、各軸
のエンコーダ３４．４４．５４．６４．７４．８４から
シロ０− コイント角θｅｎｃを入力する。

ステップ８１０：第１ＣＰＵから作業点データＰＴ　ｐ
ａｔｈを入力する。

ステップ８１２：ワーク形状データＰＳｗａｌｌを前記
式にしたがって計算により求め、ワーク形状記憶部７０
６の時系列ＫＨＩに対応する領域に格納する。

ステップ８１４：ワーク形状記憶部７０６からＰＳｌｗ
ａｌｌを選択するための探索区間［ａ、　　ｂ］を求め
る。前述のように初期値は、ａ”ＫＨＩ　−’ｎ。

ｂ＝ＫＨ１である。

ステップ８１６：ワーク形状記憶部７０ＧからＰＴ（ａ
）に至るベクトルＩ　（ａ）とその距離ＩＩ（ａ）Ｉを
前述の式によって求め、教示点の始点から作業点口（ａ
）と、その距離ＩＩＬ（ａ）ｌを、前述の式によっで求
める。

ステップ８１８：データ探索区間［ａ、　　ｂ］におい
て ｆ（ａ）＜ＩＬ＋ ｆ（ｂ）＞０　　。

の場合、ステップ８２ＣＩへ制御を移し、異常処理を行
う。

そうでなければ、ステップ８２２へ制御を移す。

ステップ８２０：前回算出したΔＰ　Ｔ　（ｋＨｌ−１
）を今回の処理のΔＰＴとし、制御をステップ８３４に
移す。

ステップ８２２：最も作業点目標データＰＴｐａｔｈに
近いワーク形状データが発見できたかチエツクする。

ｂ−ａ≦１の成立条件は、探索区間の両端の点が同一点
であるか、または隣合った場合であり、成立すると後述
のステップ８３０に制御を移し、成立しない場合には、
ステップ８２４に制御を移す。

ステップ８２４：データ探索区間内を１つづつチエツク
していては膨大な時間がかかるので、囲い込み手法を用
いて時間短縮を行う。そのため、データ探索区間［ａ、
　　ｂ］の中間点Ｃを求める。

ステップ８２６：中間点Ｃにおいてｆ　（ｃ）を求める
。

ステップ８２８　：　ｆ（ｃ）　＞Ｑ　　ならば、Ｃを
新たなりとし ｆ（ｃ）＜０　　ならは、Ｃを新たなａとして、ステッ
プ８２２に戻る。

ステップ８３０：ワーク形状記憶部７０６から選択され
た時系列のＰＳｗａｌｌを読みだし、このＰＳｗａｌｌ
をＰＳｌｗａｌｌとする。

ステップ８３２：選択されたワーク形状データＰＳ１ｗ
ａｌｌと、軌跡補間点データＰ　Ｔ　ｐａｔｈとの差Ｒ
ｏｆｓを前述の式により求め、Ｒｏｆｓと補正要素選択
器７１２ｂとを用いて、前述の式によって補正量ΔＰＴ
を求め、異常検出器７２８で異常チエツクを行う。

ステップ８３４：軌跡補間点データＰＴｐａｔｈと補正
量ΔＰＴおよび目標オフセットｏｆｓから作業点修正デ
ータＰＴｍｏｄを算出する。

ステップ８３６：作業点修正データＰＴｍｏｄを第１Ｃ
ＰＵへ転送し、ステップ８１０に制御を移す。

以上のように構成された本実施例の倣い制御ロボットも
第１実施例と同様の効果を発揮する。゛ざらに、本実施
例では、ワーク形状記憶部７０６に記憶されるのは、ワ
ーク形状データＰＳｗａｌｌだけであるので、第１実施
例よりも記憶容量が少なくてよい。

［発明の効果］ 上述の如き構成を有する本発明の倣い制御ロボットは次
の効果を有する。

（１）　目標点を先に検出しているので高速な作業がで
きる。

（２）　目標点を定める際に、センサからの情報だけを
基にせず、ティーチング点を参照しているので暴走する
ことがない。

（３）　時系列的に求められた目標点によって制御する
ので、正確な制御が出来る。

（４）　作業中に速度変化、経路変更を行っても常乙こ
正しい倣いが行える。

一６３＝

【図面の簡単な説明】

第１図は本発明の構成を例示する構成図、第２図は本発
明の詳細な説明図、 第３図は本発明の実施例の斜視図、 第４図はその動作部１６を説明する図であって、（ａ）
は平面図、（ｂ）は正面図、（ｃ）は左側面図、第５図
はその手先１８を説明する斜視図、第６図はそのセンサ
コントローラ２４の構成と距離検出の手法を説明する説
明図、 第７図はそのロボットコントローラ１２の構成図、第８
図は本発明の第１実施例の概略構成図、第９図はそれに
使用する各点の位置関係を示す説明図、 第１０図はその構成図、 第１１図はそのワーク形状記憶部４０６の説明図、第１
２図はそれに使用する各点の位置関係を示す説明図、 第１３図、第１４図は各々第１ＣＰＵ３００、第２ＣＰ
Ｕ３０１で実行する流れ図、 第１５図は本発明の第２実施例の概略構成図、第１６図
はその構成図、 第１７図はそのワーク形状記・塗部７０６の説明図、第
１８図はそれに使用する各点の位置関係を示す説明図、 第１９図は第２ＣＰＵ３０１で実行する流れ図を示す。 Ｒ１，１８・・・手先 Ｒ２，２０・・・工具 Ｒ３，２２・・・センサ Ｒ４，１０・・−ロボット Ｒ５・・・ワーク形状記憶手段 Ｒ６・・・標準軌跡補間点生成手段 Ｒ７・・・ワークデータ選択手段 Ｒ８・・・軌跡補正手段 Ｒ９−・・ロボット制御手段 Ｗ・・・ワーク １２・・・ロボットコントローラ ２４・・・センサコントローラ ３００・・・第１ＣＰＵ ３０１・・−第２ＣＰＵ ４００，７００・・・標準軌跡記憶部 ４０２．７０２・・・標準軌跡補間点生成部４０４．７
０４・・・ワーク形状近似部４０６．７０６・・・ワー
ク形状記憶部４０８・・・推定工具点演算部 ４１０・・・推定工具点射影演算部 ４１２・・・推定工具点射影演算判定部４１４．７１２
・・・軌跡補正部 ４１６．７１４・・・ロボット制御部 ７０８・・・ワーク形状射影演算部 ７１０・・・ワーク形状工具点躬影演算判定部代理人　
弁理士　定立　勉（他２名） 第６図 時系列 ＫＨｌ−ｎ 第１７図 ワーク形状記憶部７０６

Claims (1)

1. 【特許請求の範囲】 １　予め記憶された位置データにより得られる標準軌跡
   上を動作し、手先に工具と、該工具より工具進行方向に
   所定の距離だけ先行して装着したワーク形状を計測する
   センサとを有する倣い制御ロボットにおいて、 前記工具の作業と同時に前記センサが検出した計測デー
   タによりワーク形状近似データを生成し一時記憶するワ
   ーク形状記憶手段と、 前記工具が次に移動すべき前記標準軌跡上の標準軌跡補
   間点を出力する標準軌跡補間点生成手段と、 前記工具が次に移動すべき標準軌跡補間点に対応した近
   似基準点を生成するとともに、該近似基準点に対し空間
   的に最も距離の小さい関係にある前記ワーク形状近似デ
   ータを選択するワークデータ選択手段と、 前記ワークデータ選択手段により選択されたワーク形状
   近似データを入力し、次に移動すべき工具の作業点位置
   および工具姿勢を出力する軌跡補正手段と、 前記軌跡補正手段から逐次出力される工具の作業点列へ
   、工具を逐次到達させるよう制御するロボット制御手段
   と、 を備え、工具の標準軌跡からのずれをリアルタイムで補
   正しながら動作することを特徴とする倣い制御ロボット
   。 ２　前記ワークデータ選択手段が、 少なくとも前記ワーク形状近似データもしくは標準軌跡
   のいずれか一方より基準直線を生成する基準直線生成手
   段と、 予め演算決定した射影始点を前記基準直線上に射影する
   射影手段と、 前記射影手段により得られる射影点と前記基準直線上の
   予め定められる距離比較点との距離が最も小さくなるワ
   ーク形状近似データを選択する距離比較手段と を具備することを特徴とする請求項１記載の倣い制御ロ
   ボット。 ３　前記基準直線は前記ワーク形状近似データより生成
   し、 前記射影始点は前記標準軌跡および実軌跡から推定され
   る次に工具が到達すべき推定工具端に等しい前記近似基
   準点とし、 前記距離比較点は前記センサが計測した計測データとす
   ることを特徴とする請求項２記載の倣い制御ロボット。 ４　前記基準直線は、隣合う前記計測データを逐次結ん
   で得られるベクトル、あるいは所定の間隔を有する２つ
   の前記計測データを逐次結んで得られるベクトル、ある
   いは前記計測データと該計測データを検出した時点での
   工具端データを結んで得られるベクトルのいずれか一つ
   で生成されることを特徴とする請求項３記載の倣い制御
   ロボット。 ５　前記基準直線は前記標準軌跡とし、 前記射影始点は前記センサが計測した計測データとし、 前記距離比較点は前記標準軌跡補間点に等しい近似基準
   点とすることを特徴とする請求項２記載の倣い制御ロボ
   ット。