JPH07104692B2 - 予見追跡制御型ロボツト - Google Patents

Description

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

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

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

例えば、アーク溶接用ロボットでは、溶接棒に流れる電
流によって部材の接合部を検出し、工具の位置を制御す
る。

あるいは、１度目の動作でセンサによってワーク上の倣
い線等をセンシングし、その時のデータを取り込んでお
き、２度目の動作でそのデータを基に実際に工具端を倣
わせて動作制御するロボットがある。

しかし、溶接棒に流れる電流から部材の接合部を検出す
る場合には、現在作業を行っている位置において、その
作業位置が正しい位置にあるか否かを検出してから工具
を制御する。従って、ロボットの作業スピードが遅いと
きにはよいが、ロボットを高速化すると、確実な作業が
行われなくなる。即ち、制御が終了した時には、その制
御が必要であった位置を通り過ぎているのである。

また、１度目の動作でワーク上の倣い線をセンシングす
るものは、実際の作業は高速で行えるが、１回の作業を
行うために同一箇所を２回移動する必要があり、結果と
してロボットの作業スピードを上げることは出来ない。

上述の如き問題点を解決するために、特開昭59−223817
の如く、スリット光投光と２次元カメラを使用して部材
の継目の軌跡を倣い工具を制御するものがある。

［発明の解決しようとする問題点］ このロボットは、次に作業する点をスリット光と２次元
カメラにより測定しつつ作業を行う。従って、上記アー
ク溶接用ロボットよりは高速な作業が行える。しかし、
上述のロボットはいくつかの問題点を有する。

このロボットは、現在作業している点と、次に作業
する点とを結ぶ直線に沿って作業を行う。従って、現在
作業している点と、次に作業する点との間に急峻に変化
する部分があると、その部分が、直線上の測定領域に入
らないので測定がなされず、作業が停止してしまう。こ
れを防止するためには、測定や移動を極めて短くしなく
てはならず、作業が遅延する。

このロボットは、スタート点、エンド点を教示する
のみで、その間の基準となるデータを教示しないで、セ
ンサの情報のみを使って制御している。従って、センサ
が故障又は誤認識した場合でも、ロボットは出力してく
る値のまま追従動作をするため、工具が倣い線を徐々に
ずれていっても検出できず、ワークとの干渉などが発生
してもそれを検出することもできない。

このようなロボット以外に、予め作業位置を教示してお
き、その教示位置に移動する際に、所定距離先の次の教
示位置に該当する位置のワーク形状を検出し、この検出
データに基づき次の教示位置を補正した後、その教示位
置を目標位置として移動するロボットが知られている
（特開昭60−62487号，特開昭54−102260）。

ところが、操作者の作業効率向上等の観点から、教示位
置は比較的間隔の広いものとし、その教示データを補間
することにより自ら目標作業位置を算出しつつ移動させ
ようとする場合がある。この場合は次のような問題が生
じた。

即ち、補間目標位置は、所定時間以後にロボットが到達
できる位置を補間位置として、演算し決定している。こ
の所定時間は、目標位置を決定したり、各種の必要な処
理を実施するための時間である。この時間がなくては次
の作業に移れない。

ロボットが一定速度で移動しているならば、時間でなく
距離でもよいが、ロボットの速度が刻々と変化している
状況では、距離よりも時間でなければ、次の目標位置が
定まらないため誤動作を生じる可能性がある。

上述した教示位置を目標位置として移動するロボットで
は、所定距離先行する位置の形状データを促えているに
過ぎず、何等ロボットの移動速度変化を考慮していな
い。

ましてや補間しつつ移動する場合には、少しの移動速度
変化でも、誤動作は必至である。

［問題点を解決するための手段］ 本発明は上記問題点を解決するために次の手段を採用し
た。

即ち、本発明の要旨は第１図に例示する如く、 手先に工具と、該工具より工具進行方向に所定の距離だ
け先行して装着したワーク形状検出センサとを有し、予
め記憶された位置データに基づいて補間して得られる標
準軌跡上を移動して、上記センサにて上記工具が将来移
動する位置のワーク形状を検出しつつ、上記工具にて作
業を行う予見追跡制御型ロボットであって、 将来工具が移動する位置のワークの形状を前記センサか
ら時間経過に応じて逐次検出し、この検出された各ワー
ク形状に対応する工具の３次元的な位置および３次元的
な姿勢のデータを求めるとともに、このデータの内か
ら、標準軌跡上の将来の作業点の３次元的な位置および
３次元的な姿勢に近接しているデータを選択する選択手
段と、 該選択手段により選択されたデータと前記将来の作業点
の３次元的な位置および３次元的な姿勢とを比較し、偏
差量を求める偏差量検出手段と、 該偏差量に基づいてロボットの手先の３次元的な位置お
よび３次元的な姿勢を修正するロボット制御手段と、 を備えることにより、標準軌跡からのずれをリアルタイ
ムで補正しながら作業することを特徴とする予見追跡制
御型ロボットにある。

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

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

選択手段は、ワーク形状検出センサで時間経過に応じて
逐次検出し、この検出された各ワーク形状に対応する工
具の３次元的な位置および３次元的な姿勢のデータを求
める。これとともに、このデータの内から、標準軌跡上
の将来の作業点の３次元的な位置および３次元的な姿勢
に近接しているデータを選択する。

ここで、上記選択手段の好ましい態様として次のような
構成をあげることが出来る。

即ち、上記選択手段が、 ロボットに設けられ、作業点位置より工具進行方向に所
定の距離だけ先行して装着したセンサにより時間経過に
応じて逐次検出されたワークとセンサとの間の相対的位
置関係と、ロボットの手先の現在の位置および姿勢とに
より、前記先行位置のワーク上の作業点の位置および姿
勢を推定する作業点位置・姿勢推定手段と、 該推定された位置・姿勢データを逐次記憶する記憶手段
と、 標準軌跡上の次の作業点の位置・姿勢が上記逐次記憶さ
れる位置・姿勢データのいずれかに略一致したかを判定
する比較手段と、 からなる構成である。

また、上記偏差量検出手段において、偏差量が所定範囲
外であれば異常処理をする異常検出手段を備える構成と
すると、ロボットあるいはワークが、ロボットの異常動
作によって破壊される、いわゆるロボットの暴走防止が
でき好ましい。

この異常処理としては、 偏差量検出手段の異常検出手段にて異常であると判定さ
れると、ロボットの動作を停止させるとともに、異常信
号を出力するような処理や、 あるいは、同じく偏差量検出手段の異常検出手段にて異
常であると判定されると、上記センサで検出されたワー
ク形状情報を用いずに、予め定められた標準軌跡上の位
置および姿勢に基づいてロボットの手先の状態を制御す
るような処理をあげることが出来る。

上記各手段は、各々ディスクリートな回路構成により実
現してもよいが、論理演算回路として一体的に構成して
もよい。

［作用］ 本予見追跡制御型ロボットは、予め記憶された位置デー
タに基づいて補間して得られる標準軌跡上を移動して、
上記センサにて上記工具が将来移動する位置のワーク形
状を検出しつつ、上記工具にて作業を行う。しかも検出
は時間経過に応じて逐次なされている。

いかなる位置が補間目標位置になるかは、その時のロボ
ットの速度によって異なることになる。したがって、そ
の移動の際に先行して測定されつつあるワークの位置デ
ータの内、いずれのデータが補間目標位置の補正に適切
なものであるかは、作業の最中にしか判らない。

このため選択手段は、標準軌跡上の将来の作業点の３次
元的な位置および３次元的な姿勢に近接しているデータ
を選択する。検出は時間経過に応じて逐次なさとれてい
るため、ロボットの移動速度が一定でない場合も、適切
なデータが得られやすい。

勿論、予め記憶された位置データ（教示された目標位
置）またはそこから補間した目標位置を基準としている
ので、予め記憶された位置からの偏差が判明し、その大
きさがチェックできる。

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

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

第２図の斜視図にこのロボット10の構成を示す。

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

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

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

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

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

各軸には、各々の軸を中心として、回転駆動するモー
タ、その回転を停止させるブレーキ、その回転量を測定
するエンコーダが設けられている。即ち、第１軸には、
第１軸用モータ30、第１軸用ブレーキ32、第１軸用エン
コーダ34が、第２軸には、第２軸用モータ40、第２軸用
ブレーキ42、第２軸用エンコーダ44が、第３軸には、第
３軸用モータ50、第３軸用ブレーキ52、第３軸用エンコ
ーダ54が、第４軸には、第４軸用モータ60、第４軸用ブ
レーキ62、第４軸用エンコーダ64が、第５軸には、第５
軸用モータ70、第５軸用ブレーキ72、第５軸用エンコー
ダ74が、第６軸には、第６軸用モータ80、第６軸用ブレ
ーキ82、第６軸用エンコーダ84が、各々設けられてい
る。第２軸、第３軸は、各々そのモータ40、50の回転運
動をボールネジフィード90、92により直線運動に変換
し、これを平行リンク機構の軸回転運動に変換すること
により、駆動される。他の軸は、その軸のモータの回転
をトルクチューブで伝え、平歯車、ハスバ歯車等を用い
て減速して駆動される。

第４図の斜視図を用いて、動作部16の手先18を詳細に説
明する。

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

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

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

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

距離検出センサ102、104、106は、ワークＷに向けて発
射されたレーザ光のワークＷ上の光点の結像位置によっ
てワークＷまての距離を検出するものである。これらは
いずれも同じ構成であるので、距離検出センサ102につ
いてのみ説明する。

距離検出センサ102は、約750μｍのレーザ光を発射する
レーザダイオード200、このレーザダイオード200を所定
のデューティでオンオフ制御するパルス変調器202、発
射されたレーザ光を平行光とするコリメータレンズ20
4、ワークＷ上に形成されるレーザ光のスポットからの
乱反射を入光し長さ2Lの１次元PSD（Position Sensitiv
e Device）206上に結像させる受光レンズ208及びPSD206
の光生成電流Ioを検出し電圧信号に変換する２つの前置
増幅器210、212を備える。PSD206はシリコンホトダイオ
ードを応用した光検出器であり、高抵抗シリコン基板
（ｉ層）の表面にＰ型抵抗層を設け、その両端に信号取
り出し用の２つの電極を、高抵抗シリコン基板の裏面に
共通電極を各々設けた構造を有する。このＰ型抵抗層に
光が入射すると、この位置で発生する光生成電流Ioは、
入射位置から両端の電極までの抵抗値に逆比例して分割
され、各々の電極から取り出される。即ち、長さ2LのPS
D206の中心から距離Δｘだけ変位した位置にワークＷ上
のスポットが結像された場合には、両端の電極から取り
出され前置増幅器210、212に入力される電流の大きさ
は、各々、 となる。

さらに、レーザダイオード200、コリメータレンズ204、
受光レンズ208、PSD206からなる光学系は、 （１）発射光軸と反射光軸とが角度δをなす場合に反射
光軸がPSD206の中心を通ること、 （２）この場合に反射光軸とPSD206とのなす角θが、光
点LCの位置によらず光点LCがPSD206上に正しく焦点を結
ぶような角度に選ばれていること、 の２点を満足させるよう構成されている。従って、受光
レンズ208の焦点距離をｆ、発射光軸と反射光軸とが角
度δをなすときの光点LCから受光レンズ208までの距離
をAo、同じく受光レンズ208からPSD206の中心までの距
離をBoとすると、距離検出センサとワークＷとの距離の
変位ｘとPSD206上の変位Δｘとは、 の関係を有する。従って、式（１）及び焦点距離ｆにつ
いての関係式（1/f＝1/Ao＋1/Bo）から、次式（３）を
得る。

Δx/L＝（I1−I2）／（I1＋I2） ＝Ｋ・x/（１＋α・ｘ） …（３） 但し、Ｋ＝COS δ（Bo−ｆ） ／｛Ｌ・COS θ（Ao−ｆ）｝ α＝COS δ／（Ao−ｆ） 従って、PSD206の出力電流I1、I2を知れば、ワークＷと
距離検出センサとの距離の偏差ｘを知ることができる
が、本実施例で、式（３）に相当する演算を行うのがセ
ンサコントローラ24の３つの演算部214、216、218であ
る。この演算部214、216、218はいずれも同じ構成であ
るので演算部214についてのみ説明する。

センサコントローラ24の演算部214は、PSD206の出力電
流I1、I2に相当する信号を、各々入力バッファ220、222
を介して位置演算器224に入力し、（I1−I2）／（I1＋I
2）に相当する演算を実行する。

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

リニアリティ補正回路226の出力は、A/D変換器228を通
った後に、ロボットコントローラ12内のセンサ入力ポー
ト230に渡される。

次に、第６図によって、６軸の動作部16を制御するとと
もに、ティーチング装置としても働くロボットコントロ
ーラ12の構成について説明する。ロボットコントローラ
12は、周知の第1CPU300、第2CPU301、ROM302、RAM304、
バックアップRAM306等を中心に、バス308により相互に
接続された入出力ポート・回路と共に算術論理演算回路
として構成されている。

第1CPU300、第2CPU301等と共にロボットコントローラ12
を構成する入出力ポート・回路としては、バブルメモリ
310にデータを記憶したり読み出すといった処理を行う
バブルメモリインターフェイス312、CRTディスプレイと
キーボードとを備えたターミナルＴとデータの授受を行
うターミナル入出力ポート314、プリンタＰにデータを
出力するプリンタ用出力ポート316、ティーチングボッ
クス14の操作状態を入力するティーチングボックス入力
ポート318、センサ102、104、106の出力する電気信号を
センサコントローラ24を介して入力しCPU301に対して割
り込みを発生させるセンサ入力ポート230、及び上述し
た６軸（第１軸、第２軸、第３軸、第４軸、第５軸、第
６軸）用のサーボ回路320、322、324、326、328、330が
備えられている。

各軸用サーボ回路320ないし330は、何れも個々にCPUを
備えたインテイジェントタイプのものであり、第1CPU30
0からバス308を介して、レーザ加工具20先端の目標位置
及び目標姿勢のデータが与えられると、第１軸ないし第
６軸用エンコーダ34、44、54、64、74、84から各軸の制
御量を読み込み、第１軸ないし第６軸用ドライバ332、3
34、336、338、340、342を介して第１軸ないし第６軸用
モータ30、40、50、60、70、80を駆動し、レーザ加工具
20の先端を目標位置まで移送し、更に目標姿勢に同時に
制御する。尚、第１軸ないし第６軸用サーボ回路320、3
22、324、326、328、330は、各々第１軸ないし第６軸用
ブレーキ32、42、52、62、72、82も適時制御して、非常
停止などでサーボ電源（モータ電源）が切られてもアー
ムが自重で動作又は、下へ垂れることを防いでいる。

このロボットコントローラ10の動作を第２図と共に説明
する。

このロボットコントローラ12の制御は第７図に示すよう
に大きく分けて２つの部分からなる。

１つは、予めティーチングにより教示されたデータを後
述の補正量ΔPT（KH1）にて補正して動作部16に出力す
る部分であり、第1CPU300が受け持つ。

他の一つは、センサによって得られたワーク形状データ
から上記補正量ΔPT（KH1）を算出する部分であり、第2
CPU301が受け持つ。

以下に、第1CPU300が受け持つ制御部分について述べ
る。

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

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

ここで、 NX、NY、NZは、ノーマルベクトルの各要素、 OX、OY、OZは、オリエントベクトルの各要素、 AX、AY、AZは、アプローチベクトルの各要素、 Ｘ、Ｙ、Ｚは、教示点の位置ベクトルの各要素を表すも
のであって、これらは何れもロボットの動作部16の動作
の基準点を原点とする座標系Ｏ−XYZであらわされたデ
ータである。又、ノーマルベクトル、エリエントベクト
ル、アプローチベクトルとは、各々直交するベクトルで
あって、上記マトリックスで表される教示点での工具の
加工の方向を表すものである。尚、以下の説明に用いら
れる各点を示す変数もこのマトリックスと同様の形式で
ある。

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

このロボット10の基本的な動作は従来のプレイバックロ
ボットと似ている。この基本的な動作を第７図によって
説明する。

即ち、ティーチング点KHOが実作業点KH1よりも疎である
ため、ティーチング点（メモリ）400から出力されたKHO
番目のティーチング点データPTtch（KHO）を、軌跡補間
回路402で補間してレーザ加工作業点21の目標点データ
（作業点目標データ）PTpath（KH1）を算出する。

この目標点データに後述する補正量ΔPT（KH1）及び目
標オフセットOfs（KH1）412を加え、実際の作業点修正
データPTmod（KH1）とし、ジョイント逆変換回路404に
入力する。ここで、Ofs（KH1）は、工具端座標形でみた
作業点座標値のことで作業点と工具端座標との相対関係
を与えるものである。この時、PTpath（KH1）、ΔPT（K
H1）及びPTmod（KH1）の間の関係は第８図に示すように
なる。ジョイント逆変換回路404は、作業点修正データP
Tmod（KH1）を各軸の移動角度θhost（KH1）に変換し、
各軸用サーボ回路320〜330に出力し、各軸用ドライバー
回路332〜342を介して各軸用モータ30、40、50、60、7
0、80を駆動して、レーザ加工作業点21を、作業点修正
データPTmod（KH1）目標点データ位置に移動させるよう
制御する。

上述の補正量ΔPT（KH1）は、第2CPU301で求められる。

この補正量ΔPT（KH1）が、第1CPU300に入力されると、
異常検出回路405で所定値βと比較される。ΔPT（KH1）
の大きさ‖ΔPT（KH1）‖（こで、‖ΔPT（KH1）‖はΔ
PT（KH1）のユークリッドノルムl2normである。以下同
じ）がβより大きいときはロボットが暴走している可能
性があるので、例えば動作停止という非常処理を行う。
逆に小さい時はこの補正量ΔPT（KH1）と前述の作業点
目標データPTpath（KH1）とを用いて実際の作業点修正
データPTmod（KH1）を求める。

PTpmod（KH1）＝PTpath（KH1）・ΔPT（KH1） …（５） PTmod（KH1）＝PTpmod（KH1）・Ofs（KH1） …（６） ここで、PTpmod（KH1）は初段目標工具端座標である。

次に、上述の補正量ΔPT（KH1）を求める第2CPU301の部
分について、第７図、第９図を参照しながら説明する。

第2CPU301は、第1CPU300と並列的に動作しており、第1C
PU300が作業点修正データPTmod（KH1）を出力する間隔
と第2CPU301が補正量ΔPT（KH1）を出力する間隔とは、
同一タイミングで演算を行っている。一方、第2CPU301
が、壁形状をセンシングし、壁形状記憶メモリにその情
報を格納するタイミングは、一般に上記タイミングより
短いかあるいは等しい。即ち、以下の説明に用いられ
る、時系列を示す変数KH2は、上述の時系列を示す変数K
H1より、一般にその間隔が短いかあるいは等しい。

第2CPU301の処理が開始されると、第2CPU301は、先ず、
KH2時における各軸用エンコーダ34、44、54、64、74、8
4からジョイント角データθenc（KH2）を入力し、ジョ
イント順変換回路406でレーザ加工作業点実データPTenc
（KH2）に変換する。即ち、以下の式（７）を実行す
る。

PTenc（KH2）＝Λ［θenc（KH2）］ …（７） ここで、Λはジョイント順変換 ［］はベクトルを表す 次いで、座標変換回路408で、この工具先端座標データP
Tenc（KH2）からセンサ端データPS（KH2）を求める。即
ち、以下の式（８）を実行する。

PS（KH2）＝PTenc（KH2）・Π_Ｔ→_Ｓ …（８） ここでΠ_Ｔ→_Ｓはレーザ加工作業点21に対するセンサ端
の位置を示す位置・姿勢マトリックスである。

一方、センサコントローラ24から入力された測定点のセ
ンサとワークとの相対位置、形状に関するセンシングデ
ータSsen（KH2）は上記センサ端データPS（KH2）を用い
てセンサヘッド22によって測定されている点の作業点位
置姿勢、ワーク形状データPSwall（KH2）に変換され
て、作業点位置姿勢記憶メモリ410 ２に入力される。
尚、このセンシングデータSsen（KH2）は非常に短いサ
ンプリング間隔で得られたデータを平均化したものでも
良い。

ワーク形状データPSwall（KH2）は次式（９）によって
求まる。

PSwall（KH2）＝PS（KH2）・Ssen（KH2） …（９） 所定時間後、比較回路410 ３は以下の処理を行うこと
により、次に作業すると推定されるワークＷ上の選択さ
れたワーク形状データPS1wall（KH1）を算出する。

この選択されたワーク形状データPS1wall（KH1）は、以
下のようにして求められる。

工具20、ワークＷ間の位置姿勢の基準値Ofs（KH1）と工
具作業点実データPTenc（KH2）とを用いて作業点座標PT
tool（KH2）を以下の式（10）により算出する。

PTtool（KH2） ＝PTenc（KH2）・Ofs（KH1）^−１ …（10） ここで、Ofs（KH1）は、加工条件等の外部からの情報
や、ロボット10を制御するプログラム中の命令により変
更されるデータである。

前記ワーク形状データPSwall（KH2）の位置成分（Xwal
l,Ywall,Zwall）とKH2時における作業点座標PTtool（KH
2）の位置成分（Xtool,Ytool,Ztool）よりPTtool（KH
2）からPSwall（KH2）にいたるベクトル（KH2）を、
式（11）により求める。

（KH2） ＝（Xwall,Ywall,Zwall） −（Xtool,Ytool,Ztool） …（11） 次いで、上記現在の作業点修正データPTmod（KH1−１）
の位置成分（Xmod,Ymod,Zmod）とKH2時における作業点
データPTtool（KH2）の位置成分（Xtool,Ytool,Ztool）
よりPTtool（KH2）からPTmod（KH1−１）にいたるベク
トル（KH2lKH1）を、式（12）により求める。

（KH2lKH1） ＝（Xmod,Ymod,Zmod） −（Xtool,Ytool,Ztool） …（12） この（KH2）と（KH2lKH1）とから（KH2lKH1）を
（KH2）に射影したベクトル▲▼（KH2lKH1）の大
きさを求め。▲▼（KH2lKH1）の大きさ‖▲▼
（KH2lKH1）‖は次式でもとめられる。

‖IL（KH2lKH1）‖ ＝‖（KH2lKH1）・COSθ（KH2lKH1）‖ …（13） ここで、 さらに、▲▼の１サンプル間での変化量ΔILを求め
る。

Δ▲▼（KH2lKH1） ＝▲▼（KH2lKH1）−▲▼（KH2lKH1−１） …
（14） そして、‖（KH2）‖−（‖▲▼（KH2lKH1）‖＋
‖Δ▲▼（KH2lKH1）‖）が最もＯに近いPSwall（K
H2）を、上記PS1wall（KH1）とする。あるいは、‖
（KH2）‖−（‖▲▼（KH2lKH1）‖＋‖Δ▲▼
（KH2lKH1）‖）の最も小さいKH2が得られた場合には３
つのワーク形状データPSwall（KH2−１）、PSwall（KH
2）、PSwall（KH2＋１）を内挿することによってPS1wal
l（KH1）を求めてもよい。例えば、位置ベクトルだけ、
次の方法で内挿することが考えられる。

α１＝（‖▲▼wall（KH2−１）−▲▼Tpath
（KH1）‖）^−１ α２＝（‖▲▼wall（KH2）−▲▼path（KH1）
‖）^−１ α３＝（‖▲▼wall（KH2＋１）−▲▼path（K
H1）‖）^−１ ただし、▲▼wallはPSwallの位置ベクトル、 ▲▼pathはPTpathの位置ベクトル とした時次式でPS1wallの位置ベクトル▲▼1wallを
得ることができる。

このようにして、PS1wall（KH1）を求めるのは、ロボッ
トコントローラ12がサーボ回路332〜342に目標点データ
を出力する間隔よりも、センサコントローラ24から測定
点の位置、形状データが入力される間隔の方が短いため
であったり、ロボットの速度が低速から高速に変化しし
たりするためである。即ち、作業点位置姿勢記憶メモリ
410_2には、一般に、１個以上のワーク形状データが格
納されているのである。そのため、その多数のデータの
中から最も適切なデータを選ぶ必要があるのである。

特に前述のごとく内挿処理を採用することにより、ワー
ク形状データが疎らな状態でロボット10の作業スピード
を変化させた場合にも、より一層適正な作業目標点座標
を設定できる。例えば、センサコントローラ24から出力
されるデータ数とロボットコントローラ12から出力され
るデータ数とが同程度であると、作業中にロボット10の
作業スピードが高速から低速に変化する場合に、目標点
に対応するワークの形状データが少し離れた位置のデー
タとなる場合がある。しかし、本実施例で特に内挿処理
を採用する場合は、センサコントローラ24から出力され
た疎らなデータを内挿補間して用いるので、その様な問
題は起こらない。

上述のようにして、比較回路410_3からPS1wall（KH1）
が出力されると、このPS1wall（KH1）と前述の軌跡補間
回路402によって得られた作業点目標データPTpath（KH
1）との差R_ofs（KH1）を次式により求める。

Ｒ ofs（KH1） ＝PTpath（KH1）^−１・PS1wall（KH1） …（15） そして、補正要素選択回路410 ４によってセンサが計
測した要素に対応するマトリックス成分のみを、Ｒ of
s（KH1）のデータを用いて、ΔPT（KH1）を以下のよう
に求める。

例えば、 工具端座標系のＺ方向の距離のみを計測している場
合は、 工具端座標系のＹ方向の距離のみを計測している場
合は、 工具端姿勢とＺ方向の距離のみを計測している場合
は、 Ｒ ofs（KH1） ＝PTpath（KH1）^−１・PS1wall（KH1） このようにして求めた補正量ΔPT（KH1）は、異常検出
回路405に入力され所定値βと比較される。ΔPT（KH1）
の大きさ‖ΔPT（KH1）‖がβより大きいときはロボッ
トが暴走している可能性があるので、例えば動作停止と
いう非常処理を行う。逆に小さい時はΔPT（KH1）を用
いて次の作業点修正データPTmod（KH1）を求める。

上述の処理を第1CPU300、第2CPU301で実行する流れ図を
第10図、第11図に示す。

本実施例に用いているロボットコントローラ12は、既に
述べたように、第６図に示す２つのCPU300、301を有
し、この２つのCPU300、301が並列的に処理を行ってい
る。第10図に第1CPU300の動作を示す流れ図を示し、第1
1図に第2CPU301の動作を示す流れ図を示す。

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

ステップ500:メモリに記憶されているティーチング点デ
ータPTtch（KH0）を、補間して作業点目標データPTpath
（KH1）を算出する。

ステップ502:作業点目標データPTpath（KH1）と１つ前
の作業点修正データPTmod（KH1−１）とを、第2CPUに送
り、第2CPUに補正量ΔPT（KH1）を、要求する。

ステップ504:第2CPUから補正量ΔPT（KH1）を入力す
る。

ステップ506:補正量ΔPT（KH1）を、所定量βと比較す
る。

‖ΔPT（KH1）‖＞＝β であれば、処理はステップ508
の異常処理へ移行し、 ‖ΔPT（KH1）‖＜β であれば、処理はステップ510へ
移行する。ここでは、補正量ΔPT（KH1）を請求の範囲
の偏差量としてその範囲が所定範囲外か否かを判定して
いるが、勿論、直接的に対応している差Ｒ ofs（KH1）
を偏差量として、所定範囲外か否かを判定してもよい。

ステップ508:ロボット10の動作を停止して、処理を終
る。

ステップ510:作業点修正データPTmod（KH1）を、PTpath
（KH1）、ΔPT（KH1）及び目標オフセットOfs（KH1）か
ら算出する。

ステップ512:目標点データPTmod（KH1）を各軸の移動角
度θhost（KH1）に変換する。

ステップ514:θhost（KH1）を、各軸用サーボ回路320〜
330に出力する。

ステップ516:KH1を、１だけインクリメントし、上記ス
テップ500以降を繰り返して実行する。

第2CPU301は以下の処理を行う。尚、ステップ600〜604
とステップ606〜623は、並行してマルチタスクで実行さ
れる。

ステップ600:センシングカウンタKH2の内容をクリアす
る。

ステップ601:センサコントローラ24から入力された測定
点の位置、形状データSsen（KH2）を入力する。

ステップ602:各軸のエンコーダ34、44、54、64、74、84
からジョイント角θenc（KH2）を入力する。ただし、セ
ンサが計測を開始した瞬間にエンコーダから値を入力す
る必要がある。

ステップ603:前記各式に従って、作業点実データPTenc
（KH2）、該工具端座標PTenc（KH2）及び工具20とワー
クＷとの間の位置姿勢の基準値Ofs（KH1）を用いて作業
点座標PTtool（KH2）、センシング点PSwall（KH2）、さ
らに作業点実データPTtool（KH2）から、センシング点P
Swall（KH2）にいたるベクトル（KH2）と、その距離
‖（KH2）‖を求め、作業点位置姿勢記憶メモリに格
納する。

ステップ604:センシングカウンタKH2を１だけインクリ
メントする。

ステップ606:比較カウンタｉの内容をクリアする。

ステップ607:第1CPU300から作業点目標データPTpath（K
H1）、１つ前の作業点修正データPTmod（KH1−１）を入
力する。

ステップ608:作業点位置姿勢記憶メモリからPTtool
（ｉ）を読みだし、PTtool（ｉ）からPTmod（KH1−１）
に至るベクトル（ilKH1）とその距離‖（ilKH1）‖
を前述の式によって求める。

ステップ609:ワーク形状記憶メモリから（ｉ）を読み
だし、（ilKH1）と（ｉ）とのなす角cosθ（ilKH
1）を前述の式によって求める。

ステップ610:（ilKH1）を（ｉ）に射影したベクト
ル▲▼（ilKH1）と、その距離‖▲▼（ilKH1）
‖を、前述の式によって求める。

ステップ611:▲▼（ilKH1）の変化量Δ▲▼（i
lKH1）とその距離‖Δ▲▼（ilKH1）‖を前述の式
によつて求める。

ステップ612:‖（ｉ）‖が、（‖▲▼（ilKH1）
‖＋‖Δ▲▼（ilKH1）‖）とほぼ等しいか、否か
を判定する。即ち、‖（ｉ）‖と（‖▲▼（ilKH
1）‖＋‖Δ▲▼（ilKH1）‖）との差の絶対値が、
判定のための所定値εより小さい時にほぼ等しいと判定
する。そして、 ‖（ｉ）‖ ≒（‖▲▼（ilKH1）‖＋‖Δ▲▼（ilKH1）
‖）ならばステップ618に、そうでないならばステップ6
15に制御が移る。

ステップ615:比較カウンタｉとセンシングカウンタKH2
とを比較する。

ｉ＞＝KH2 ならばステップ622へ行き、 ｉ＜KH2 ならばステップ616へ行く。

ステップ616:比較カウンタｉを１だけインクリメント
し、ステップ608に戻る。

ステップ618:作業点位置姿勢記憶メモリからPS1wall
（ｉ）を読みだし、PS1wall（ｉ）をPS1wall（KH1）と
する。

ステップ619:ワーク形状推定データPS1wall（KH1）と、
作業点目標データPTpath（KH1）との差Ｒ ofs（KH1）
を、前述の式により求める。

ステップ620:R oft（KH1）と、補正要素選択回路とを
用いて、前述の式によって補正量ΔPT（KH1）を求め
る。

ステップ622:前回の補正量ΔPT（KH1−１）を、次の補
正量ΔPT（KH1）とする。

ステップ623:第1CPUに補正量ΔPT（KH1）を送る。

ステップ624:比較カウンタｉを１だけインクリメントす
る。

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

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

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

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

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

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

（４）時間経過に応じて逐次ワーク形状を先に検出する
とともに、前述のごとくの比較回路410 ３があるため
に作業スピードが変化しても正確な作業が出来る。

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

［発明の効果］ 上述の如き構成を有する本発明の予見追跡制御型ロボッ
トは次の効果を有する。

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

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

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

【図面の簡単な説明】

第１図は本発明の構成を例示する構成図、 第２図は本発明の一実施例の斜視図、 第３図はその動作部16を説明する図であって、 （ａ）は平面図、（ｂ）は正面図、（ｃ）は左側面図、 第４図はその手先18を説明する斜視図、 第５図は、そのセンサコントローラ24との構成と距離検
出の手法を説明する説明図、 第６図は、そのロボットコントローラ12の構成図、 第７図は、ロボットコントローラ12の制御ブロック図、 第８図、第９図は各点の位置関係を示す説明図、 第10図、第11図は各々第1CPU300、第2CPU301で実行する
流れ図を示す。 10……ロボット 12……ロボットコントローラ 14……ティーチングボックス 16……動作部 18……手先 20……工具 21……作業点 22……センサヘッド 24……センサコントローラ 26……ライトガイド 28……センサケーブル 30、40、50、60、70、80……モータ 32、42、52、62、72、82……ブレーキ 34、44、54、64、74、84……エンコーダ 90、92……ボールネジフィード 102、104、106……距離検出センサ 200……レーザダイオード 202……パルス変調器 204……コリメータレンズ 206……PSD 208……受光レンズ 210、212……前置増幅器 214、216、218……演算部 220、222……入力バッファ 224……位置演算器 226……リニアリティ補正回路 228……A/D変換器 230……センサ入力ポート 300……第1CPU 301……第2CPU 302……ROM 304……RAM 306……バックアップRAM 308……バス 310……バブルメモリ 312……バブルメモリインターフェイス 314……ターミナル入出力ポート 316……プリンタ用出力ポート 318……ティーチングボックス入力ポート 320、322、324、326、328、330……サーボ回路 332、334、336、338、340、342……ドライバ 400……ティーチング点（メモリ） 402……軌跡補間回路 404……ジョイント逆変換回路 405……異常検出回路 406……ジョイント順変換回路 408……座標変換回路 410 １……ワーク位置姿勢推定回路 410 ２……作業点位置姿勢記憶メモリ 410 ３……比較回路 410 ４……補正要素選択回路 412……目標オフセット Ｐ……プリンター Ｔ……ターミナル Ｗ……ワーク

フロントページの続き (72)発明者 竹田 史章 愛知県愛知郡長久手町大字長湫字横道41番 地の１ 株式会社豊田中央研究所内 (72)発明者 小出 光男 愛知県愛知郡長久手町大字長湫字横道41番 地の１ 株式会社豊田中央研究所内 (72)発明者 久野 敏孝 愛知県愛知郡長久手町大字長湫字横道41番 地の１ 株式会社豊田中央研究所内 (72)発明者 中野 冠 愛知県愛知郡長久手町大字長湫字横道41番 地の１ 株式会社豊田中央研究所内 (56)参考文献 特開 昭59−223817（ＪＰ，Ａ) 特開 昭60−120408（ＪＰ，Ａ) 実開 昭58−80192（ＪＰ，Ｕ)

Claims (6)

【特許請求の範囲】
1. 【請求項１】手先に工具と、該工具より工具進行方向に
   所定の距離だけ先行して装着したワーク形状検出センサ
   とを有し、予め記憶された位置データに基づいて補間し
   て得られる標準軌跡上を移動して、上記センサにて上記
   工具が将来移動する位置のワーク形状を検出しつつ、上
   記工具にて作業を行う予見追跡制御型ロボットであっ
   て、 将来工具が移動する位置のワークの形状を前記センサか
   ら時間経過に応じて逐次検出し、この検出された各ワー
   ク形状に対応する工具の３次元的な位置および３次元的
   な姿勢のデータを求めるとともに、このデータの内か
   ら、標準軌跡上の将来の作業点の３次元的な位置および
   ３次元的な姿勢に近接しているデータを選択する選択手
   段と、 該選択手段により選択されたデータと前記将来の作業点
   の３次元的な位置および３次元的な姿勢とを比較し、偏
   差量を求める偏差量検出手段と、 該偏差量に基づいてロボットの手先の３次元的な位置お
   よび３次元的な姿勢を修正するロボット制御手段と、 を備え、標準軌跡からのずれをリアルタイムで補正しな
   がら作業することを特徴とする予見追跡制御型ロボッ
   ト。
2. 【請求項２】選択手段が、 ロボットに設けられ、作業点位置より工具進行方向に所
   定の距離だけ先行して装着したセンサにより時間経過に
   応じて逐次検出されたワークとセンサとの間の相対的位
   置関係と、ロボットの手先の現在の位置および姿勢とに
   より、前記先行位置のワーク上の作業点の位置および姿
   勢を推定する作業点位置・姿勢推定手段と、 該推定された位置・姿勢データを逐次記憶する記憶手段
   と、 標準軌跡上の次の作業点の位置・姿勢が上記逐次記憶さ
   れる位置・姿勢データのいずれかに略一致したかを判定
   する比較手段と、 を備えたことを特徴とする特許請求の範囲第１項記載の
   予見制御型ロボット。
3. 【請求項３】上記偏差量検出手段において、偏差量が所
   定範囲外であれば異常処理をする異常検出手段を備え、
   ロボットの暴走防止ができることを特徴とする特許請求
   の範囲第１項または第２項記載の予見追跡制御型ロボッ
   ト。
4. 【請求項４】上記異常処理が、ロボットの動作停止及び
   異常信号の出力であることを特徴とする特許請求の範囲
   第３項記載の予見追跡制御型ロボット。
5. 【請求項５】上記異常処理が、上記ワーク形状情報を用
   いずに、予め定められた標準軌跡上の位置および姿勢に
   基づいてロボットの手先の状態を制御することを特徴と
   する特許請求の範囲第３項記載の予見追跡制御型ロボッ
   ト。
6. 【請求項６】上記選択手段が、複数のデータを選択する
   とともに、 上記偏差量検出手段が、前記複数のデータを内挿して得
   られたデータと前記将来の作業点の３次元的な位置およ
   び３次元的な姿勢とを比較し、偏差量を求めることを特
   徴とする特許請求の範囲第１〜５項のいずれか記載の予
   見追跡制御型ロボット。