JPH0212504A - マニピュレータ用の視覚シーム追跡方法および装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔産業上の利用分野〕 本発明は、マニピュレータ用の三次元視覚シーム（ｓｅ
ａｍ）追跡システムに関し、特に動的に変化する方位角
を補償することができる如きシステムに関する。

〔従来の技術〕

本発明の視覚シーム追跡システムを備えたマニピュレー
タは、シール剤の塗布、輪郭マツピングを含む多くの処
理用途において、また一般に適応経路制御を必要とする
どんな処理用途においても使用することができる。−例
を示す目的のため、本発明はシーム溶接に対する用途に
おいて教示されることになろう。

マニピュレータが一旦１つのプログラムを教えられると
、マニピュレータは高い繰返し精度を以てプログラムさ
れた経路に沿って運動する。

このことは多くの用途において満足できるが、他の場合
は適応経路制御を必要とする。例えば、溶接作業におい
ては、もし接合される部品が製造公差、熱の歪み等によ
りシーム即ち接合場所において均一でなければ、例えシ
ームが他の場所にあってもマニピュレータはその教えら
れた経路に沿フて溶接を行なうことになる。

この問題を克服するため、またマニピュレータが使用で
きる用途を広げるため、従来技術の研究者達は多くの視
覚案内された追跡システムを考案してきた。米国特許第
４，５４２，２７９号および同第４，５９０．：１５６
号の教えるものは、このようなシステムの事例である。

一般に、従来技術の試みは、マニピュレータの案内のた
め視覚システムを必要とした。この案内の要件の故に、
従来技術のシステムでの１１約となる。更に、従来技術
のシステムは、一般にカメラと部品との間の一定の関係
を必要とし、追跡が生じる間方位の変化を禁止する。

視覚案内を備えた溶接システムの用途が複雑さを増すに
伴い、データのそれ以上の処理および解釈が要求される
。従来技術の研究者達は、視覚案内装置の処理能力を最
大化しようと試みた。しかし、この試みの主な欠点は、
必要な座標変換を行ないかつ溶接プロセス全体を制御す
るために膨大な量のマニピュレータ情報を必要とするこ
とにあり、その結果通信および性能の制約をもたらした
。更に他の問題として、この大量のマニピュレータ情報
は実時間で処理しなければならない。

本発明は、マニピュレータ制御によるマニピュレータを
案内し、カメラ座標におけるシームの位置に関するデー
タのみを視覚システムに要求するという概念に基いてい
る。本発明は、カメラ座標におけるカメラ・データを用
い、方位角とは独立的にカメラ座標をマニピュレータ座
標へ変換する手段を含む。方位角の変化により生じるカ
メラにおける姿勢の変化を許容するマトリックスが用い
られる。マニピュレータ；開部は、萌記変換マトリック
スの形成に必要な大量のマニピュレータ・データ（位置
、方位、等）へのアクセスおよびこれを処理する能力を
有する。

その結果は、動的に変動する方位角を補償することがで
きる視覚シーム追跡方法および装置となる。マニピュレ
ータの方位角は、追跡性能を損なうことなく所要の迎角
および押引き角度、等を達成するため溶接プロセスが必
要とする如くユーザがプログラムできる。この能力は、
もし方位角が固定されたままでなければならなければ行
なうことができる多数の部品の溶接を可能にする。

（課題を解決するための手段） 本発明によれば、マニピュレータのための三次元の視覚
シーム追跡方法および装置が提供される。マニピュレー
タは、溶接トーチの前方で溶接されるシームを予め検分
するレーザー・スキャナー／カメラ・ヘッドを含む視覚
システムが設けられている。この視覚システムからのデ
ータは、マニピュレータの制御部へ送られる。

このマニピュレータ制御部は、マニピュレータ座標から
カメラ座標における視覚システムのデータをマニピュレ
ータ座標へ変換することができるマトリックスを得る。

この変換されたデータは、必要となるまでＦＩＦＯバッ
ファ内に格納される。マニピュレータの方位角は、ユー
ザが予めプログラムすることができ、またシーム追跡機
能を損なうことなくシーム溶接プロセス全般にわたって
動的に変動し得る。

マニピュレ−タ制御部のシーム追跡ルーチンは、以降に
おけるＦＩＦＯのエントリを排除し、次いでこのエント
リを通過するようにマニピュレータの予めプログラムさ
れた経路を変更する。

〔実施例〕

本発明の視覚案内シーム追跡方法および装置の完全な理
解のためには、例示的なマニピュレータおよびその制御
部について述べることが必要であろう。この目的のため
、第１図は、本発明の譲受人であるＣ１ｎｃｉｎｎａｔ
ｉ　Ｍｉｌａｃｒｏｎ　Ｉｎｄｕｓヒｒｉｃｓ社製のも
のと対応するマニピュレータを示している。この図示さ
れたマニピュレータは事例に過ぎず、本発明は適当な６
自由度の機械的マニピュレータに対して適用し得るもの
である。望ましい実施悪様の詳細な説明は実際に実現す
ることを必然的に反映するものであるが、このような細
部は頭書の特許請求の範囲に規定される如き本発明に対
する限定と見做すべきではない。

第１図においては、事例ロボット即ちマニピュレータが
全体的に番号１で示されている。

マニピュレータ１は、直立するベース部材３を支持する
ベース板２を含む。上部アーム４は５枢着部５において
ベース部材３に対して枢着されている。前アーム６は枢
着部７において上部アーム４に枢動自在に取付けられて
いる。

前アーム６は、その自由前端部において手首部８を支持
している。当技術では周知の如く、手首部８は、前アー
ム６内部に回動自在に支持されたインテグラル軸部１０
と前アーム６の長手方向中心線と平行あるいは一致する
釉を有する第１ｍ部９を含む。手首部８は更に、第１節
部９に回動自在に取付けられかつ、インテグラル軸部１
２を有する第２の節部１１を含む。この１Ｍ１２は面板
１３で終る。面板１３に固定されているのは、以下に述
べる溶接工具！４と視覚システムのレーザー・スキャナ
ー／カメラ・ヘッド１５である。

マニピュレータ又は、その部材の運動が回動軸心を規定
するように構成されている。第１の軸心は、点線１６で
示されベース板２およびベース部材３の回動により規定
される垂直方向のベース軸心である。上部アーム４は、
枢着部Ｓの中心を通る水平肩部軸心１７の周囲に回動す
る。前アーム６は、枢着部７の中心を通って延長する水
平なエルボ−軸心１８の周囲に回動する。

手首部８は、更に３つの回動輪心を提供する。

その第１は、軸！Ｏの軸心の周囲における節部９の回動
運動である。その第２は、節部９および目の当接面に対
し直角の軸心周囲での節部ＩＩの回動運動である。第３
は、その周囲で面板１３が回動しかつこれが面仮に対し
て直角をなす軸心を含む。

ベースの軸心１６、肩部のｊ軸心１７およびエルボ−１
８の周囲の回動運動が、工具１４の中心点１９の作動エ
ンベロープ（包絡線）を規定する。手首部のｔＩｒ１部
９、手首部節部１１および面板１３のそれらの各軸心周
囲の回動運動は、プログラムされる方位角に従ってプロ
グラムされた場所における工具１４およびレーザー・ス
キャナー／カメラ・ヘッドＩ５の方位を制御する。

マニピュレータ制御部は５２０で略図的に示される。６
つの運動軸心は各々、指令信号およびフィードバック信
号を制御部２０のサーボ制御部２１に接続することによ
りｌｔ、ＩＪ御されるサーボである。

溶接工具１４の作動は、サイクル制御部２３により制御
されるプログラムされた作動サイクルに応答する機械の
インターフェース２２によって行なわれる。このサイク
ル制御部２３は、記憶された場所、速度および機能のデ
ータについて演算して、サーボ制御部２１おまび機械の
インターフェース２２に対する制御信号を生じる。この
ような特定の事例においては、工具１４は溶接トーチで
あり、また溶接プロセスの制御は記憶された機能信号に
応答して機械のインターフェースを介して行なわれる。

制御部２０はまた、レーザー・スキャナー／カメラ・ヘ
ッド１４、カメラ制御装置２５および信号処理用コンピ
ュータ２６からなる視覚システムから情報を受取る視覚
データ・プロセッサ２４を内蔵する。この視覚システム
については、以下において更に記述することにする。

ユーザ・アーム４および（ηアーム６の各々の軸心周囲
の回動運動は、それぞれナツト２９および３０を介する
ねじ２７および２８の直線状運動によって達成される。

ナツト２９および３０の回動運動は、それぞれ図示しな
い駆動モータによ）てブリー３１および３２を介して与
えられる。ベース板２およびベース部材３の回動運動は
伝動装置３３、駆動ビニオン３４を介して行なわれ、こ
の駆動とニオンは更にベース板２に固定されたリング歯
車３５を駆動する。伝動装置３３への駆動力はモータ３
６によって与えられる。

手首部の節部９および１１、および面板１３の回動運動
は、前アーム６内部の同心状トルク・チューブ（図示せ
ず）を介して行なわれ、伝動装置４０を介してモータ３
７．３８および３９により駆動される。各可動マニピユ
レータ部材に対する位置の信号は位置のトランスジュー
サ（レゾルバの如き）によって生じるが、このトランス
ジューサはマニピュレータの可動部材それ事態またはそ
のための駆動モータの運動により機械的に駆動され得る
。

次に、第１図の制御部２０のブロック図を構成する第２
図を参照する。この制御部２０は、共通のバスを介して
通信する複数のマイクロプロセッサを用いて実現される
。本出願人は、Ｉｎｊｅ１社製の市販される８０１８６
マイクロプロセツサを選定した。当業者には、以降に述
べる制御アルゴリズムを実現するため他の適当な汎用デ
ィジタル・コンピュータの使用が可能であることが理解
されよう。

上記のように、制御部２０は視覚データ・プロセッサ２
４を含む。この視覚データ・プロセッサ２４は、共に直
接にシステム・バス４３と接続されたマイクロプロセッ
サ４１およびメモリー４２を含む。

視覚データ・プロセッサ２４の目的については、更に詳
細に以下に述べよう。

サイクル１制御部２３は、共に直接にシステム・バス４
３と接続されたマイクロプロセッサ４４および割込み制
御部４５を含む。マイクロプロセッサ４４により実行さ
れるオペレーティング・システム・プログラムは、メモ
リー４６内に記憶され、モード制御４７、自動モード４
８および教示モニド４９として識別されるプログラムを
含む。ユーザが指定する場所、速度および機能を規定す
るデータは、データ・ストア５０に格納されている。

メモリー４６は、直接にシステム・バス４３と接続され
ている。自動モード４８として識別されるプログラムの
組は、マイクロプロセッサ４４によって使用されて、デ
ータ・ストア５０に格納されたデータにより規定される
作動サイクルを実行する。この自動モード・プログラム
の実行は、割込み制御部４５により処理される割込み信
号によって実行が割込まれるまで、独立的にマイクロプ
ロセッサ４４によって実施される。

サーボ１ｔｄＪ御部２＋（第１図）は第２図において５
１で示され、以下に述べるサイクル制御部２３およびサ
ーボ入出カモジュールにより生成され、かつマニピュレ
ータ軸心の駆動モータの実際の制御と関連するサーボ指
令信号を処理するサーボ監視部（スーパーバイザ）を含
む。このサーボ監視部は、マイクロプロセッサ５２、プ
ログラム可能タイマー５３およびサーボ制御処理プログ
ラム５５を含む局部メモリー５４からなっている。サー
ボ割込み信号は、プログラム可能タイマー５３により周
期的に生成される。割込み信号間の期間は、サイクル制
御部２３により実行される経路制御転送の各反復間隔を
規定する。サーボ監視部を構成する各要素は、反復間隔
において行なわれるべき回動軸心に対するマニピュレー
タの運動の増分を表わすマニピュレータ軸心指令信号を
受取る。

これらマニピュレータ軸心の指令信号は、サーボ監視部
により処理されて反復間隔の小分割に有効なマニピュレ
ータの各軸心に対する小増分を規定するサーボ指令信号
を生じる。このサーボ指令信号は、反復間隔の予め定め
た小間隔でサーボ入出カモジュールへ分散される。この
サーボ入出カモジュールは、位置のトランスジューサに
より生じるサーボ指令信号およびフィードバック信号を
用いてマニピュレータ軸心駆動モータを制御する。要素
５２．５３および５４は局部バス５６と接続され、この
バスは更に二重ボート５７を介してシステム・バス４３
と接続されることが判るであろう。

サーボ入出力は、駆動インターフェース回路５８および
マニピュレータ軸心位置測定トランスジューサ・インタ
ーフェース５ｇを介してマイクロプロセッサ５２および
メモリー５４によって行なわれる。これら要素は、局部
バス５６によって相互に連結されている、マイクロプロ
セッサ５２は、メモリー５４に格納されたサーボ入出カ
プログラム６２に従ってサーボ指令信号およびフィード
バック信号に応答して増巾器６０ａおよび１３０ｂを介
しマニピュレータ軸心駆動モータ６０および６！を制御
する。各マニピュレータ軸心サーボ指令信号は、位置の
トランスジューサ６３または８４により規定される如き
対応するその時の位置信号と比較されて位置の誤差信号
を生じ、この信号は次に駆動インターフェース回路５８
へ加えられ、関連するマニピュレータ軸心駆動モータに
適用するための駆動中制御信号を生じる。

第２図のブロック図はブロック６５および６７により示
されるサーボ入出力インターフェースを示すことが判る
であろう。ブロック６５および６７の各々が、駆動イン
ターフェース回路５８、および５ｇで示されるものと類
似する機ｔｉＩｉ〜心位置測定位置測定トランスジュー
サーフェースの双方を表わすことが理解されよう。更に
また、ブロック６６および６８が各々２つの駆動モータ
と１つの増巾器とその各々と関連する１つのレゾルバを
表わすことが理解されよう。サーボ指令信号は、数ミリ
秒内に行なわれるマニピュレータ軸心の運動の増分を表
わす。対照的に、マニピュレータ軸心の指令信号は、！
０乃至２０ミリ秒の反復間隔内で行なわれるべきマニピ
ユレータ部材の運動の増分を表わす。

マニピュレータ軸心の指令信号の生成に加えて、サイク
ル−制御部２３は、入力信号により表わされかつプログ
ラムされた場所で機能させられる工具１４（第１図）と
関連する諸機能の実行をＩ制御する。機能指令は、デー
タ・ストア５０におけるプログラム・データと共に格納
され、かつ自動モード・プログラム４８と関連する機能
実行サブルーチンに従つて実行される。マニピュレータ
機能のｉｌＪ御は、リミッＩ・・スイッチ、押しボタン
、リレーおよびソレノイドの如き入出力装置を介して行
なわれる。このマニピュレータ入出力装置は、Ｒ域の入
出力インターフェース回路７０と共働シ、てデータ人出
力装置の入出カポ−ドロ９により直接に制御される。デ
ータは二重ボート装置７１を介してシステム・バス４３
に関して出入りさせられる。この二重ボート装、１７１
は、ボード６９の局部バス７２と接続されている。

ボード６９の動作は、局部メモリー７４に格納されたプ
ログラムを実行するバス７２と接続された局部マイクロ
プロセッサ７３によってｆｉｌｌされる。

マニピュレータ入出力インターフェース回路７０のボー
ド６ｇに対する接・続は、直列インターフェース回路７
５を介して行なわれる。制御筐体のオペレータ・パネル
７６は、並列回線により並列インターフェース回路ア７
と接続されている。

インターフェース回路７５および７７の監視および制御
は、それぞれ局部メモリー７４に格納されたマニピュレ
ータ入出カプログラム７８およびオペレータ・パネル入
出カプログラム７９に応答してマニピュレータ７３によ
って行なわれる。マニピュレータ入出力装置のその時の
条件は、パネル６９から二重ボート装置７１を介してサ
イクル制御部２３に対して送られる装置の状態信号に反
映される。

格納された作動プログラムに従ってサイクル制御部２３
により発生される機能指令信号は、システム・バス４３
上を二重ボート７１を介して送られ、直列インターフェ
ース７５により最後に適当なマニピュレータ入出力イン
ターフェース装置に対して送られる。

マニピュレータ装置インターフェース間の信号交換に加
えて、制御部はパネル６９およびその関連するインター
フェースによるデータの交換を可能にする。サイクル制
御部２３が自動モード・プログラム４８の下に作動して
いる間、場所のデータは機能指令に応答して交換するこ
とができる。

このプロセス内のデータ交換は、パネル６９を介して視
覚システムの信号処理コンピュータ２６（これまた第１
図に示される）と制御部２０との間で生じる。データ・
ストア５０からの場所のデータは、システム・バス４３
からパネル６９へその二重ボート装置７１を介して転送
される。視覚システムの局部メモリー７４に格納された
データ入出カプログラム８１の制御下で作動するマイク
ロプロセッサ７３は、視覚システムの信号処理コンピュ
ータ２６へ送るため初期化データを二重ボート装置７１
から直列チャネル・インターフェース８２へ転送する。

反対に、視覚システム信号処理コンピュータ２６からの
データは、直列チャネル・インターフェース８２へ入力
され、またこれから局部バス７２上を二重ボート装置１
７１へ送られる。ここから、データはシステム・バス４
３上で視覚データ・プロセッサ２４に対して使用可能と
なる。

今述べた詐りのプロセス間のデータ交換に加えて、プロ
グラム・データは、大容量データ・トランシーバ８３を
介してメモリー４６のデータ・ストア５０と大容量デー
タ・ストアとの間で交換することができる。大容量デー
タ・ストアの事例は、逐次テープ装置およびデータ・デ
ィスク装置を含む。データは、メモリー７４に格納され
た大容量データ・トランシーバの入出カプログラム８４
に応答して作動するマイクロプロセッサ７３７３によっ
て交換される。データは、局部バス７２に対する直列チ
ャネル・インターフェース８２を介し、またシステム・
バス４３と接続された二重ボート装置７ｔを介して、大
容量データ・トランシーバに関して出入れされる。

最終データ入出力装置、即ち教示用ペンダント８５は、
サイクル制御部２３により教示モード・プログラム４９
の実行と関連付けられている。

プログラムされた作動サイクルを規定する場所および機
能のデータは、教示用ペンダント８５を用いてオペレー
タにより生成することができる。

このペンダントは、マニピュレータ１の手動操作、およ
び場所および機能データのデータ・ストア５０への格納
を可能にする。他のデータ入出力装置におけるように、
データは局部バス７２に対する直列チャネル・インター
フェース８２およびこれからシステム・バス４３に対す
る二重ボート装置７１を介して、ペンダント８５に関し
て出入れされる。教示操作モードは本発明の一部をなさ
ないため、そのこれ以上の詳細については本文では述べ
ない。教示モードにおけるマニピュレータの作動のこれ
以上の詳細は米国特許下３，９２０，９７２号に見出す
ことができる。

マニピユレータ部材の運動を制御してプログラムされた
場所間の直線経路に沿って工具の中心点１９の運動を生
じることが自動モード・プログラム４８の目的であるた
め、マニと”ユレータの概略図は自動モード制御アルゴ
リズムの説明において有効であり得る。このような概略
図が第３図に示されている。

第３図においては、第１図のマニピュレータの上部アー
ム４と対応する第１の線による節部８７が示されている
。第２の線による節部８Ｂは、第１図のマニピュレータ
の前アーム６と対応するように示されている。第１図の
肩部軸心１７、エルボ−軸心Ｉ８、手首部８および工具
中心点１ｇもまた第３図に示されている。手首部８およ
び工具中心点１９は、線の節部８９によフて接合された
状態で示されている。

第３図の線の節部８７．８８および８９は、それぞれ長
さＬＩ　　ＬｌおよびＬ３により寸法が示される。長さ
Ｌｌは、肩部軸心１７とエルボ−軸心１８との間の上部
アーム４の長さを示す。長さＬｌは、エルボ−軸心１８
から手首部８の中心点までの前アーム６の長さを表わす
。最後に、長さＬ３は、手首部８の中心と工具１４の中
心点１９との間の距離を表わしている。

第３図においては、線の節部８７．８８および８９は、
マニピュレータ１の直角座標系Ｘ、Ｙ、Ｚとの関連にお
いて示されている。マニピュレータの座標系は、しばし
ばマニピュレータの「ワールド（ｗｏｒｌｄ）Ｊ座標系
と呼ばれる。このＦｇ、標系の原点は、垂直方向のベー
ス軸心１６と水平の肩部軸心１７との交点に位置するマ
ニピュレータｌ上の！っの点と対応している。第３図か
ら、マニピュレータの座標系の垂直のＺ〜はベース軸心
１Ｂと同軸であることが判るであろう。マニピュレータ
１がその「ホームポジション」にある時、水平のＹ％は
肩部軸心１７と同〜であり、水平のＸ軸はベース「軸心
１６と肩部軸心１７の交点に対して直角をなしかつこれ
を通る。

第２のＦ５標系は、マニピュレータと関連して用いられ
る。この座ｌｌＡ系ｘ、ｙ、ｚは、その原点を手首部８
の中心点に有する。手首部８の中心点は、その３つの回
動輪心の交点として定義される。この第２の座標系はし
ばしば１手の」座ｊＩＡ；ｆ−と呼ばれる０手首部８の
中心点と工具！４の中心点１．９との間の距離を表わす
第３図の線の節部８９は、手の座標系におけるベクトル
からなっている。

如何なる時点でも、この３つの方位角がマニピュレータ
の１ｈ心系Ｘ、Ｙ、Ｚに対する手の軸心系ｘ、ｙ、ｚの
方位を定義し、従来技術において周知のように、マニピ
ュレータの座標系から手の座標系へ、また手の座標系か
らマニピュレータ座標系への変換は下記の如く行なわれ
説明される。即ち、 Ｘ　　　　　　　　　　　　　　　　　ｘＹ　　　　＝
　　　　　［Ｑ］　　　　　　　ｙＺ　　　　　　　　
　　　　　　　　　ｚマニピュレータ　　　　　　　　
　　　　　手ｘ　　　　　　　　　　　　　　　　　　
　　　　　　　　　　　　Ｘｙ　　　　＝　　　　　［
Ｑ］−’　　　　　Ｙｚ　　　　　　　　　　　　　　
　　　　　　　　　　　　　　　　２手　　　　　　　
　　　　　マニピュレータ但し、［Ｑ］は適当な変換マ
トリックスを示す。

第３図においては、マニピュレータのベース軸心１６の
周囲における回動運動は角度ａの大きさにより決定され
る。マニピュレータの上部アームの回動運動は角度すの
大きさによフて決定される。マニピュレータの前アーム
の回動運動は、角度Ｃの大きさにより決定される。

角度Ｃはマニピュレータの上部アームの中心線から測定
されることが判るであろう。当業者は、第３図から長さ
Ｌｍ、Ｌ２、および角度ａ、ｂ、Ｃの大きさが手首部８
の中心点の場所を完全に決定することが判るであろう。

もし線の節部８９が線の節部８８と同軸であるならば、
長さＬＩＬ２．Ｌ３は角度ａ、ｂ、ｃの大きさと共に、
工具の中心点１９の場所を完全に規定することになろう
。

手首部８の軸心における回動運動の誘起は、工具の中心
点１９を介する工具１４と関連する機能要素の方位を生
じる。手首部８は箪４図において拡大されて示される。

上記の如く、この手首部は、手首部８がマニピュレータ
の前アーム６に取付けられるそのインテグラル１ｌｌｄ
ｌ　１．０を持つ第１の節部９を有する。手首部は更に
、第２の節部１１と、面板１３を支持するそのインテグ
ラルＮ１２とを有する。この手首部８の第１の回動輪心
９０は、手首部節部９の回動運動によって規定される。

手首部８の第２の回動軸心は、手首部節部１１の回動運
動によって規定される。この軸心は９１で示・され、手
首部節部９および！！の当接面に対し直角をなす。手首
部８の第３の回動輪心は、面板１３の回動運動によって
規定される。この軸心は９２で示され、面板１３に対し
直角をなす。手首部の軸心ｇＯの周囲の回転運動はρｌ
で示される。

手首部の軸心９１の周囲の回動連動はρ２で示される。

最後に、面板１３の回動運動はρ３により示される。

第５図は、如何にして方位角が手の座標系に関して規定
されその原点を手首部８の中心に置くかを示している。

手のＰｉＰＭ＆　（ｘ　、　ｙ　、　ｚ　）の軸心は、
方位角がゼロ（０）である時、それぞれマニピュレータ
のくワールド）座標系の軸心Ｘ、Ｙ、Ｚに対して平行と
なる。方位角り、ＥおよびＲは、下記の如く手の座標系
に関して回動運動を規定する。即ち、 （１）Ｅは、それぞれＸおよびｙをＸ、およびｙＩとに
整合させるＺの周囲の回動の大きさであり、 （２）Ｄは、Ｘｌをｘ２と整合させｘ２およびＺをｚ２
と整合させるｙｌの周囲の回動の大きさであり。

（３）Ｒはｙｌおよびｚ２をＺ３と整合させるＸ、の周
囲の回動の大きさである。

当業者には、長さＬｌ、Ｌ２およびＬ３が既知である時
、工具の中心点１９の位置が３つの変位数値Ｘ、Ｙ、Ｚ
、および３つの方位角り、　Ｅ３よびＲに照して完全に
規定できることが理解されよう。

本発明の制御系統ににおいては、１つの経路が自動モー
ドにおいて計算されつつある時、あるいはマニピュレー
タ１が教示モードにおいて教えられつつある時、マニピ
ュレータの（ワールド）座標系Ｘ、Ｙ、Ｚ、Ｄ、Ｅ、Ｒ
が使用される。本発明のサーボ・システムに対する信号
は、しばしば「接合スペース」と呼ばれる接合角度位置
（即ち、モータ位置）に即したものでなければならない
。その結果、これらの信号は、ａ、ｂ、ｃ、およびｐｌ
　ｐ２、ｐ３に関連する。当技術において公知のように
、Ｘ、Ｙ、Ｚ、Ｄ、Ｅ、Ｒおよびａ、ｂ、ｃ、ｐｌ、ｐ
２．９２間の変換のためのマトリックス変換操作がある
。

次に、第６図の幾何学的概略図を参照する。同図におい
ては、直線の節部９３は、工具の中心点１９が格納され
た入力信号により規定される点ＰｏからＰｌへ移動進行
する運動経路を示している。自動モードにおけるマニピ
ュレータの制御は、この直線経路に沿フて中間点ｐ、を
補間し、増分間隔ΔＴに右ける経路運動を生じる。点ｐ
、、ｐ、、および中間点Ｐ、の座標は、全てマニピュレ
ータのくワールド）座標系に関して規定される。工具の
中心点１９が点Ｐ０において静止状態から始動し点Ｐ、
において再び静止するものとすれば、経路制御方式はプ
ログラムされた速度への、またほからの自動的加速およ
び自動的減速を生じる。

中間点Ｐ１により規定される増分は固定された増分間隔
の期間に従って生じるが、点Ｐｌ間の実際の増分距離は
、工具の中心点の速度が変化するに従って変化する。こ
のため、一定の増分間隔ΔＴの間に横切った距離と対応
する増分距離ΔＳは、運動の一定の速度部分と予め定め
た運動の加速および減速段階との間で変動することが判
る。

本発明の実施においては、ΔＴは１５ミリ秒に等しい。

このように、サーボ入出力ボード５１は、１５ミリ秒毎
にサイクル制御ボードからの位置の指令を受取り、サイ
クル制御ボード２３はこれらの信号を生じるに充分な速
さで計算する。サイクル制御部２３は、工具の中心点が
加速、減速およびプログラムされた速度等に基いてどれ
だけ線Ｐｏ、Ｐ、に沿って移動できるかを計算し、サー
ボ入出力ボード５１にこれを達成する適当な接合スペー
ス信号を与える。

自動モードにおけるマニピュレータ１の全作動サイクル
は、第７図のフローチャートによって示される。処理ス
テップ９４においては、前記制御部はプログラムされた
速度ＶＰにおけるプログラムされた点Ｐ０およびＰ、間
の運動を補間するため必要な間隔（ｓｐａｎ）データを
初期化する。このプログラムされた間隔長さＳＰおよび
座標成分比ＮＣＣに対する数値は、ステップ９４と関連
する手順により生成される。プロセス・ステップ９５に
おいては、経路に沿う増分が補間されて増分距離ΔＳお
よび累積増分比１！ＩＳＫを生成する。プロセス・ステ
ップ９６は、サブルーチンを呼出してマニピュレータ座
標から接合角度位置への補間された中間点の変換を行な
う。

プロセス・ステップ９６と関連するこのサブルーチンは
、軸心指令信号を生じ、これらの信号をサーボ割込み信
号の発生と同時にサーボ監視部によりアクセスするため
バッファに格納する。

プロセス・ステップ９６のサブルーチンの実行に続いて
、判断ステップ９７において減速が要求されないことを
示すフラッグが設定された通りであるかどうかを判定す
るためテストが行なわれる。もしこの非減速フラッグが
設定されなかったならば、プロセスは判断ステップ１０
２で続行し、ここで補間されつつあるこの時の増分がプ
ログラムされた場所間のその時の直線間隔の最後の増分
であるかどうか判定される。もしそうでなければ、別の
増分がコネクタＬ２を介して補間される。もしその時の
増分がこの間隔の最後の増分ならば、プロセスはコネク
タＬ３を介してプロセス・ステップまで継続し、ここで
間隔の終りと関連する機能が実行される。

その後、判断ステップ１０５が、この時の場所がプログ
ラムの最後の場所であるかどうかを判定する。もしそう
でなければ、更にプログラムされた場所のデータがコネ
クタＬ１に進むことにより処理されることになる。もし
これが！＆後の場所であれば、プログラムされた作動サ
イクル全体がターミナル１０６により示される如く再び
実行されることになる。

判断ステップ９７に戻って、非減速フラッグが設定され
なかったことが判定されたならば、この手順の実行はプ
ロセス・ステップ９ｇで続行する。このステップは、減
速停止が開始しなければならない点に先立ち、その時の
間隔における残りの距ａＳ３゜を計算する。判断ステッ
プ１００においては、この残りの距１！Ｉ　Ｓ　ＲＤの
大きさがその時の増分距離ΔＳに対してテストされ、残
りの距ｆｉｓ＋＋ｏがその時の増分距離ΔＳよりも小さ
いかどうかを判定する。もしこの残りの距離がその時の
増分距離よりも小さくなければ、プロセスはステップ１
０１で続行し、ここで速度修正サブルーチンの実行がサ
ブルーチン呼出しによりて開始される。速度修正サブル
ーチンはどんなプロセス・パラメータにも応答して、工
具の中心点の速度をプロセスの変化に適応させるために
、あるいはマニピュレータの軸心速度をそれらの関連し
た限度に拘束するために、増分速度における変化を発生
する。このサブルーチンの完了と同時に、手順はプロセ
ス・ステップ９５におけるループへ続行し、ここで増分
速度の最も後に生成された数値を用いて間隔の増分が補
間される。

判断ステップ１００に戻り、残りの距離ＳＲＤが増分距
離ΔＳよりも小さかったことが決定されたならば、手順
の実行は判断ステップ１０２において継続することにな
る。このステップは、その時の直線間隔の終りを規定し
ているプログラムされた記憶領域を介して運動が継続さ
れるべきかを判定する。もし工具の中心点がその時の間
隔の終りにおいて静止状態となるならば、判断ステップ
１０２は、プログラムされた場所が継続点でないことを
判定し、この手順の実行はプロセス・ステップ１０３で
続行し、ここで減速サブルーチンの実行は呼出しにより
最終の速度パラメータ信号ｖｆを０に等しく設定するよ
うに初期化される。減速サブルーチンの実行が終ると同
時に、この全手順の実行がプロセス・ステップ！０４に
おいて１ｍ続し、ここでその時の間隔（スパン）の終り
と関連した機能命令が実行されることになる。その後、
判断ステップ１０５において、その時のプログラムされ
た場所がプログラムの最後のプログラムされた場所であ
るかどうかを判定する。

もしそうであれば、プログラムの実行はプログラムが再
び循環すべきことを示すターミナル１０６を介して記憶
されたプログラムの第１の場所において開始することに
より再開される。

もしその時の場所が格納されたプログラムの最後の場所
でなければ、実行はコネクタＬ１を経て次のプログラム
された直線的間隔を補間する。

再び判断ステップ１０２において、その時の間隔の終り
に対するプログラムされた場所が工具の中心点の運動が
継続すべきものであると判定されたならば、全サイクル
ｆｌｄＪ御手順の実行がプロセス・ステップ１０７まで
進み、ここでその時の間隔に対する新たな終りの位置が
計算される。

補間された中間位置が増分間隔の期間および有効増分速
度により規定される増分走離だけ隔てられる故に、運動
がプログラムされた位置を経て続行する時、減速判定位
置からプログラムされた位置の終りまでの間隔において
残る距離は常にその時の増分距離の整数倍と等しくない
ことになる。この矛盾を勘案して、その時の間隔はプロ
グラムされた終りの位置に最も近いその時の増分距離の
整数倍で存効に終了される。このためには、残りの距ｍ
（ｓｐ−ｓｘ）が増分距離ΔＳで除され、整数の倍数お
よび残を得る。次いで、その時の間隔長さはこの残が差
引かれ、継続間隔長さＳｃＰを得る。従って、この距離
はその時の間隔の終りの位置および次の間隔の開始位置
を再び規定する。

判断ステップ１０Ｂにおいて、その時の間隔と次の間隔
との間に挟まれる角度が　１２０”より小さいかどうか
を判定する。もしこの挟角が１２０°より小さければ、
工具の中心点の運動がその時の間隔の最後の場所Ｐ、に
おいて終る。

この挟角の値の決定は、余弦則に依存している。

その時の間隔の初めにおけるプログラムされた場所と、
次の間隔の終了におけるプログラムされた場所との間の
間隔距離の２つの値は独立的に生成される。即ち、間隔
長さＳ７２は直角座標成分の２乗の和として計算される
が、値Ｓ　Ｔ”　（１２０°）はプロセス・ステップ１
０７において計算される間隔長さｓｅｐの２乗および次
の間隔の間隔長さＳ。の２乗の和、および次の間隔長さ
Ｓ、、とその時の間隔の間隔長さＳＣＰの積として計算
される。もしＳＴ２がＳ、２（１２０°）より小さけれ
ば、挟角は　１２０°より小さい。

もしこの挟角が１２０°より小さければ、進行以前に運
動を停止状態にすることが必要であり、このプロセスは
プロセス・ステップ１０３での減速手順の呼出しを通し
て続行することになる。

もし挟角が】２Ｏ４以上であるならば、このプロセスは
判断ステップ１０９で継続する。判断ステップ１０９は
、次の間隔のプログラム速度がその時のプログラムされ
た速度よりも大きいかあるいはこれと等しいかどうかを
判定する。もし次の間隔のプログラムされた速度がその
時の間隔のプロダラムされた速度よりも小さければ、次
の間隔にあける運動がこの間隔に対してプログラムされ
た速度において開始されるように、その時の間隔におけ
る減速を生じることが必要である。プロセス・ステップ
１１０により呼出される減速手順に続いて、全サイクル
がコネクタＬ３を経てプロセス・ステップ１０４におい
て続行する。

もし次の間隔におけるプログラムされた速度が事実その
時の間隔のプログラムされた速度よりも大きいかあるい
はこれに等しければ、判断ステップ１０９からのプロセ
スは非減速フラッグを設定するプロセス・ステップ１１
１で続行する。

その後、前に述べた全作動サイクルがコネクタＬ２を介
して再開される。

全サイクル制御が格納されたプログラムの反復実行を含
むことが明らかであろう。プログラムされた場所間の運
動は、その間の直線経路に沿った増分運動の相互積を含
む。この全作動サイクルは、マニピュレータの運動およ
びプログラムされた場所と関連するプログラムされた緒
機能の実行からなっている。

第８図においては、サーボ制御により実行されるサーボ
割込みサービス・ルーチンが判断ステップ１１２で開始
し、ここで軸心指令信号が一時的に格納されるバッファ
が空であるかどうかを判定する。もしこのバッファが空
であれば、サイクル制御は間隔の終りに達し、間隔信号
の終りがプロセス・ステップ！１３において送られる。

もし判断ステップ■２によってバッファが空でないこと
が判れば、軸心指令信号により表わされる軸心運動の増
分はステップ１１４においてバッファから検索される。

ターミナル１１５において、サーボ割込みルーチンが終
了され、サイクル制御部２３による全作動サイクルが再
開する。前に述べたように、サーボ監視部が軸心指令信
号により表わされる増分を、反復期間の予め定めた小期
間中にサーボ位置を指令するため用いられる小増分に分
割する。サーボ指令信号の分割および出力プロセスは、
サイクル制御部２３による自動モード手順全体の実行と
同時に継続する。

第７図のフローチャートと関連する主要セグメントおよ
びサブルーチンの記述は第９図乃至第１４図に関して行
なうことにする。第９図のフローチャートは、第７図の
プロセス・ステップ９４と対応している。プロセス・ス
テップ１１１３においては、予めプログラムされた入力
信号と対応するプログラムされた間隔（スパン）データ
がメモリー４１３のデータ・ストア５０から再び呼出さ
れる。始動点Ｐ。、終了点ＰＩおよびプログラムされた
速度ｖＰは、二［具の中心点１９により次に実行される
べき運動を定義する。メモリーから再び呼出されるプロ
グラムされた座標データは、プログラムされた点の表示
に対応してサブスクリプトで示される。処理ステップ！
１７においては、全間隔（スパン）長さが２つのプログ
ラムされた点の直角座標を用いて、座標成分の２乗の和
の平方根をとって計算される。プロセス・ステップ１１
８においては、プログラムされた座標系の軸内成分の比
が、間隔長さにより座標成分を除すことにより計算され
る。これらの比は、Ｎ　ＸＸｖ　Ｎ　ｖｖｓ　Ｎ　！２
ｓ　Ｎ　ｏｏ、　Ｎ　ＥＥおよびＮＲＲで表わされる。

方位角に対する軸心成分比は、直角座標釉における成分
比と同じように計算される。

−旦間隔変数のこの予備的な計算が完了すると、全作動
サイクルはターミナル１２０を経て東７図の処理ブロッ
ク９５へ続く。

次に第１０図においては、第７図のブロック９５と対応
する間隔の増分の補間処理ステップが更に詳細なフロー
チャートに拡張されている。判断ステップ１２＋から始
まり、判断は適応即ちプログラムされない速度フラッグ
が第７図の処理ステップ１０１により呼出されるサブル
ーチンによって設定されたかどうかについて行なわれる
。

もしその時の間隔（スパン）に対してプログラムされた
適応速度機能がないか、あるいは適応速度機能が増分速
度値への変化を結果として生じるならば、適応速度フラ
ッグは設定されず、プロセスは判断ステップ１２２にお
いて継続することになる。判断ステップ１２２は、その
時の増分速度信号ｖＫの値、即ちその時の反復増分速度
にをプログラムされた速度信号ｖＰの値に対してテスト
して、その時の増分速度がプログラムされた速度より小
さいかどうかを判定する。もしそうであれば、プロセス
はプロセス・ステップ１２３において継続し、これが加
速サブルーチンを呼出す。

加速サブルーチンの効果は、予め定めた加速機能に従っ
て増分速度に対する新たな値を生成すること、また対応
する距離の増分ΔＳを生成することである。このため、
工具の中心点が静止状態から始動する運動の初めに、増
分速度の初期値がゼロとなり、プログラムされた速度は
無論ゼロではなく、プロセス・ステップ１２３における
加速サブルーチンの呼出しが増分速度（ａ　Ｖ　Ｋの変
化を生じ始め、増分距離イ８号ΔＳを生じる。

増分速度ＶＫがプログラムされた速度■、よりも小さく
なければ、増分距離信号に対する新たな値を計算するこ
とは必要でなく、プロセス・ステップ１２５により示さ
れる如き反復カウンタを増進させるだけでよい。いずれ
の場合も、プロセスは最後にプロセス・ステップ１２６
を再開し、ここで累積された増分信号ＳＫの値がプロセ
ス・ステップ１２Ｂの右側に示した数式に従って計算さ
れる。全作動サイクルは、その時フローチャートの終り
１２７の「゛継続」により継続される。

この時、全作動サイクルは、第７図のプロセス・ステッ
プ９６の呼出しにより続行し、第１１図のフローチャー
トに示された変換サブルーチンを呼出す。

第１＋図においては、プロセス・ステップ１２８におい
て、累積された補間距離のマニピュレータ座標系におけ
る座標を示す終端点座標信号の値が計算される。これは
、に−１で示される終端点座標信号の前の値を、増分距
離信号ΔＳにより表わされる補間された増分距離の軸内
の成分に加算することにより行なわれる。この時、ステ
ップ！２９は以下に述べる座標調整ルーチンを呼出す。

このルーチンは、必要に応じて、例えばシームを追従す
るよう調整された座標を提供する。このルーチンもまた
、ユーザが指定するパラメータに従ってプログラムされ
た経路からの偏位置を制限する。その後、補間された点
のプログラム座標系に関する座標がステップ！３０によ
り接合角度位置ａ、ｂ、ｃ、およびｐＩ、ｐ２．ｐ３に
変換される。逆三角関数機能を避けるため、その時の接
合角度位置に基く誤す関数を生じる反復近似アルゴリズ
ムが用いられる。この試みの詳細については、米国特許
下３，９０９，８００号に示されるが、これはこの変換
アルゴリズムの説明に必要な程度に参考のため本文に付
言される。プロセス・ステップ！３１において、接合角
度位置における変化が計算され、プロセス・ステップ１
３２において、接合角度位置の増分差がサーボ割込みサ
ービス・ルーチンによるアクセスのためバッファに格納
される。変換ルーチンが完了すると、全作動サイクルの
処理がターミナル・ブロック１３３における「戻し」に
より再開される。次にこれは、第７図の判断ステップ９
７ヘプロセスを送る。ステップ９７に続いて、全作動サ
イクルはプロセス・ステップ９９において継続し、ここ
で減速間隔の初めまでの残る距離が計算され、このプロ
セス・ステップの詳細なフローチャートが第１２図に示
される。

第１２図においては、プロセス・ステップ１３４は、予
め定めた加速値を用いて減速に必要な期間を計算する。

このため、停止するため減速に必要なおよその時間はそ
の時の増分速度ｖＫおよび加速定数の商に等しい。プロ
セス・ステップ１３５においては、その時の増分速度の
半分に等しい平均速度がプロセス・ステップ１３４にお
いて計算された期間中に有効となることを仮定して、減
速に必要な距離を表わす減速距離信号が計算される。最
後に、プロセス・ステップ１３６において、間隔残の信
号Ｓ□。の値が計算され、その時の補間位置から丁度計
算された減速距離が開始する時点までのプログラムされ
た間隔長さ内の残りの距離を表わす。このため、距離の
残ｓｎ。

は、プログラムされた間隔長さＳＰから累積された補間
距離により表わされる間隔の部分ＳＲ１および減速に必
要な間隔の部分ＳＤを差引いたものに等しい。この計算
に続いて、全作動サイクルが第７図の判断ステップ１０
０において続行し、ここで残りの距１１１ｓＲＤがその
時の有効増分距離ΔＳより小さいかどうかが判定される
。もし残の距１１１ｉＳＲｏがその時の有効増分距離Δ
Ｓより小さくなければ、この距離の別の増分を補間する
ことができる。しかし、もしその時の有効増分距離ΔＳ
が計算された残りの距離Ｓｑｏより大きければ、即時減
速を開始することが必要である。この減速サブルーチン
は、第１３図のフローチャートに示されている。

この減速サブルーチンは、予め定めた加速関数に従って
、増分速度変数ＶＫの減少する値を反復的に生じる。加
速サブルーチンの初めに、増分速度が再計算されて、本
出願人が使用するコンピュータにより実行される整数演
算の丸め誤差について調整する。次に第１３図において
、プロセス・ステップ１３８が減速反復カウンタをゼロ
に等しくセットする。プロセス・ステップ１３９におい
ては、減速期間Ｔ０が未だ実行されない間隔長さおよび
その時の増分速度ｖＫの値とその時の間隔が終了すべき
最終速度Ｖ、−との間の速度の変化の商の２倍として計
算される。最終速度ｖＰは１、次のプログラムされた位
置が連続点でありかつその時の間隔と次の間隔との間の
挟角は　１２０”と等しいかこれより大きい時を除いて
、全ての場合にゼロとなる。プロセス・ステップ１４０
において、減速を生じるため必要な反復数に０は、減速
の計算された期間ＴＤと予め定めた増分間隔Δｔの商と
して計算される。プロセス・ステップ＋４１においては
、増分速度ｖＫの新たな値は、最終速度Ｖ、と、その時
の間隔の残りの距離および減速の計算された期間Ｔ、の
商の２倍との和として計算される。前に述べたように、
増分速度■、のこの再計算は、本出願人のディジタル・
コンピュータにおける整数演算の使用により必要とされ
る。また、プロセス・ステップ１４１において、速度信
号Δｖ８゜における増分の変化が計算される。この信号
は、減速サブルーチンの反復毎の速度の大きさの変化を
決定する。

速度信号の増分変化は、初期速度と最終速度との速度差
を減速の反復数Ｋｌ）で除すことにより計算される。プ
ロセス・ステップ！４３においては、減速の反復カウン
タが増進される。プロセス・ステップ！４５においては
、増分距離ΔＳが、その時と館の増分の平均増分速度と
、増分間隔信号Δｔの積として計算される。プロセス・
ステップ１４６においては、累積された補間距離ＳＫが
、全の補間距離ど増分距離ΔＳの和として計算される。

プロセス・ステップ１４７においては、変換サブルーチ
ンが呼出され、判断ステップ１４８において１反復カウ
ンタが前に計算された減速反復値との等価についてテス
トされる。もしこれらが等しければ、減速サブルーチン
が完了し、全作動サイクルはターミナル・ブロック１４
９の「戻し」により再開される。

しかし、もしこの反復カウンタが減速反復値と等しくな
ければ、新たな増分速度ＶＫＨがプロセス・ステップ１
４８ａにおいてその時の増分速度ｖＫと速度ＶＫＤにお
ける変化との間の差として計算される。その後、減速反
復ループは、プロセス・ステップ！４３へ処理を戻す戻
し線により反復される。

マニピュレータ１、その制御部２０およびその基本的な
作動モードについては既に述べたが、第１図を再び参照
し、特に視覚システム１５．２５および２６を参照する
。本出願人は、オランダ国デルフトの０ｐｔｉｓｃｈｅ
　Ｉｎｄｕｓｔ、ｒｉｅ　Ｄｅ　０ｕｄｅ　Ｄｅｌ１１
社製の商標ＳＥＡＭｌ’１ＬＯＴの下に市販される視覚
システムを選択した。当業者は、シームを走査してカメ
ラの座標においてシームに沿った点を有する視覚データ
・プロセッサ２４（第２図参照）を提供することができ
る、適当などんな視覚システムでも使用できることが理
解されよう。

周知のように、レーザー・スキャナー／カメラ・ヘッド
１５は、サーボ制御されたミラーに対して集束され、レ
ーザー・ビームのサイドツウサイド（ｓ　１ｄｅ−Ｌｏ
−ｓ　ｔｄｅ　）走査運動を生じるレーザーを含む。こ
のレーザー・スキャナー／カメラ・ヘッド１４のカメラ
部分は、レーザー・ビームが工作物に当たる場所を視認
する。このレーザー・スキャナー／カメラ・ヘッド１５
には、当技術においては全て周知の適当な冷却手段、使
い捨ての光学的ウィンドウの保護のための遮蔽ガス、等
が備えられている。

カメラ制御装置２５は、レーザーを投入遮断し、レーザ
ー・ビームの走査を制御し２！！当なカメラ・データを
信号処理用コンピュータ２６に対して送出する。信号処
理用コンピュータ２６は、走査レーザーにより得られる
工作物の各輪郭を視覚データ・プロセッサ・メモリー４
２（第２図参照）からマニピュレータのオペレータによ
り選択された輪郭（テンプレート）の仕様と比較する。

もしこの選択された輪郭が見出されるならば、輪郭上の
問題となる点の場所（シームの中心）が２信号処理用コ
ンピュータ２６によりカメラ座標において計算される。

このカメラ座標は、直角チャネル・インターフェース８
２、局部バス７２、二重ポート７Ｉおよびシステム・バ
ス４３を介して視覚データ・プロセッサ２４（第２図参
照）へ送られる。以下において明らかになるように、こ
の視覚システムの位置の値を用いて、工具１４を所要の
輪郭に案内するためにマニピュレータのプログラムされ
た経路をオフセットする。この情報はまた、部品の設置
誤差、製造公差のばら付き、部品の熱歪み、不正なシー
ム形状等の？Ａ整のため使用することができる。追跡中
、マニピュレータの位置は工具１４をシームに維持する
ように修正される。マニピュレータの方位値は、溶接プ
ロセスの必要に応じて追跡性能を損なうことなくプログ
ラムすることができる。この能力は、もし方位角が固定
された状態を維持しなければならなければ実施すること
ができない多数の部品の溶接を可能にする。

要素１５のカメラ・ヘッド（第１図参照）の手の座標系
に関する場所および方位が、変換マトリックスがカメラ
の座標をマニピュレータのくワールド）座標に関連付け
るよう形成することができるように決定されることが必
要である。手順の座標のマニピュレータ座ｉｐ＜ワール
ド座Ｓ）への変換については既に論述した。

手の座標におけるカメラの場所を決定するために、既知
の目標の場所のカメラ座標が決定され、且つマニピュレ
ータの作業ボリューム内に固定された校正部を用いる操
作の教示モードに格納される。この校正部のマニピュレ
ータの（ワールド）座標は、校正部をマニピュレータの
終端効果器（即ち、工具中心１９）に取付けることによ
りマニピュレータから得られる。同じ校正部の目標点は
、カメラにより３つの異なる場所から視認される。各場
所毎に、校正部の目標のカメラ座標および工具中心点の
マニピュレータの（ワールド）座標が格納される。各場
所から校正部の目標へのベクトルは、下式により計算さ
れて手の座標へ変換される。即ち、 校正部をマニピュレータの終端効果器（即ち、工具中心
１９）に取付けることによりマニピュレータから得られ
る。同じ校正部の目標点は、カメラにより３つの異なる
場所から視認される。各場所毎に、校正部の目標のカメ
ラ座標および工具中心点のマニピュレータの（ワールド
）座標が確定される。各場所から校正部の目標へのベク
トルは、下式により計算されて手の座標へ変換される。

即ち、 ｘ　　　　　　　　　　　　　　　　Ｘｙ　　　　−［
Ｑ］−’　　　　　Ｙ ｚ　　　　　　　　　　　　　　　　２手の座標　　　
　　　　　　マニピュレータ座標目標点に対する３組の
手の座標値、および目標点に対する対応する３組のカメ
ラ座標を用いて、その結果のカメラ座標系から手の座標
系への変換マトリックスを生成することができる。この
３組の座標を用いて、カメラ座標、および視認が行なわ
れた３つの場所により規定される面に直角の単位ベクト
ルの座標成分を含む第１の４Ｘ４マトリツクスを形成す
る。この３つの場所の手の座標、およびこの３つの場所
により規定される面と直角をなす単位ベクトルを含む第
２の４×４マトリツクスが形成される。次に、カメラの
座標を手の座標に変換するための変換マトリックスが、
第１の４×４マトリツクスの逆数と第２の４×４マトリ
ツクスとの積として形成される。その結果得られる変換
マトリックス［Ｍ］は下記の如く演算する。即ち、 ｘ　ｓｓ　　　　　　　　　　　　　　　　　　　　　
　　　　　　　ｘ　５ｃｙｓＨ［Ｍ］　　　　　　　’
Ｊｓｃ ｚ　ｓｏ　　　　　　　　　　　　　　　　　　　　　
　　　　　　　　ｚ　ｓｃ手　　　　　　　　　　　　
　　　　　カメラＸｓＣＸＳＭ ）／ｓｃ　　　　　　　　　　　　　　　　　［Ｍ］−
’　　　　　　　　　　　　　　Ｖ　　Ｓｒ量ｚ　ｓｃ
　　　　　　　　　　　　　　　　　　　　　　　　　
　　　　ｚ　ｓｓ左カメラ　　　　　　　　　　　　　
手［Ｑコマトリックスとは異なり、ＣＭ］マトリックス
は定数と見做すことができるが、これはカメラに物が当
たる等のことがない限り、カメラの座標と手の座標が変
化しないためである。手の座標とマニピュレータ（ワー
ルド）の座標との間の関係が要求される異なる方位角の
関数として変化することになる。

上記のように、視覚システムの信号処理用コンピュータ
はカメラの座標データを視覚データ・プロセッサ２４へ
送る。この視覚データ・プロセッサ２４は、カメラの座
標データをマニピュレータの（ワールド）座標データへ
変換する。

このことは、下記の如くに示すことができる。

即ち、 Ｘ　Ｓ　　　　　　　　　　　　　　　　　　Ｘ　！Ｉ
Ｉｃｙｓ　　　　　　［Ｑ］　　［Ｍ］　　　　　　　
ｙｓｃＺ　ｓ　　　　　　　　　　　　　　　　　　Ｚ
　ｇｃマニピュレータ　　　　　　　　　　カメラ特に
、カメラのデータは、これをカメラ座標から手の座標に
変換する［Ｍ］マトリックスを介して変換される。その
後、実際にカメラ走査が行なわわた時点におけるもので
あった方位の値を与えることにより、［Ｑ］マトリック
スを形成することができる。手の座標は［Ｑ］マトリッ
クスを介して送られ、マニピュレータのくワールド）座
標が結果として生じる。

本発明の実施において、レーザー走査／カメラ・ヘッド
１５は工具または溶接トーチ１４の前方のシームを予め
検分する。シームの輪郭が処理され、その場所が記憶さ
れる。ある時間の後、このシーム・データを用いて、工
具中心点１９を案内し、これが直接追跡点を通るように
する。

このことは、所要の追跡点についてＦＩＦＯバッファに
与えることにより実現される。

ＦＩＦＯは、シーム・データを受入れ、マニピュレ−タ
の（ワールド）座標における追跡点を計算する視覚デー
タ・プロセッサ２４の前記ル−チンにより書込まれる。

このＦＩＦＯは、次の点を呼出してこｔＵ”向って指向
するアルゴリズムにより読出される。Ｆ　Ｉ　ＦＯは、
視覚データ・プロセッサのメモリー４２の一部を構成す
る。

第１４図は、この形式の追跡を示している。

第１４図に勿いては、工具の中心点１９が位置ＰＫに置
かれると見做される。この同じ実施例で、Ｉノーザー・
スキャナー／カメラ・ヘッドが位置ＰＳＫにおける工具
の中心点の前方に示される予備走査を行ないつつある。

しかし、この工具の中心点は、その時の１つの走査後５
回の走査である走査ＰＳＫ−Ｓの間指定される追跡点に
向って追跡中である。このため、ＦＩＦＯは、この溶接
の実施に当たり少なくとも６要素の長さでなければなら
ない。工具中心点が位Ｊ　Ｐ　ｇに−９に達すると５レ
ーザー・スキャナー／カメラ・ヘッドは走査ＰｓＫや）
を開始する。従って、適正な追跡操作のためには２つの
ルーチンが視覚データ・プロセッサにおいて提供されね
ばならないことが明らかである。第１に、シーム処理ル
ーチンは視覚システムからデータを受入ね１．追跡点を
決定し、追跡点をカメラ座標からマニピュレータの（ワ
ールド）座標へ変換し、次いでＦＩＦＯにシーム点のＸ
、Ｙおよび２のワールド座標を格納する。

このシーム追跡ルーチンは、その時のロボット位置およ
びその時のシーム点のＦＩ　Ｆｌエントリを決定し、マ
ニピュレータ経路をこの所要のシーム点に向けるようオ
フセットし、この点に達すると、ＦＩＦＯから新たなシ
ーム点を選択する。このように、シーム処理ルーチンは
、レーザー・スキャナー／カメラ・ヘッドから受取る如
きシーム・データのマニピュレータの（ワールド）座標
への変換、およびこれらの結果をＦＩＦＯに与える任に
ある。シーム追跡ルーチンは、ある時間後にＦＩＦＯエ
ントリを取出し、次いでロボットの経路をこれらエント
リを通るように変更する任にある。

第１４図からは、レーザー・スキャナー／カメラ・ヘッ
ド１５と工具の中心点１９との間の距疎かＦＩＦＯの大
きさに重要な効果を有することが理解されよう。同様に
、この工具中心点１９の速度がＦＩＦＯの大きさに影！
を及ぼし、速度が遅くなると与えられた予備検分距離に
わたって更に長い間隔をとらせる。更に、もし測定があ
まり短い間隔であると、測定システムの精度不備が測定
される実際の距離程度に大きくなる。

ＦＩＦＯ寸法を実用限度内に保持するために、ＦＩＦＯ
が決して指温れするまで一杯の状態にならないように、
ある走査を選択的に捨てる方法を考えることが必要とな
る。

追跡中ＦＩＦＯの圧縮を実現するため、工具の中心点１
ｇと視野の中心との間の距離が計算さね。

る。この視野内の１つの点が視野中心から最大約５０．
８ｍｍ　（２インチ）オフセットできるので、この距離
に約５０．８１ＩＩ＋（２インチ）のオフセットを加え
て最７男の場合の比重を得る。その後、この合計をＦＩ
ＦＯエン］・り数（例えば、３５）で除して追跡距１ｎ
ｌｓｒｒを得、これがＦＩＦＯにロードされるシーム点
間の最小分離を定義する。

最後のＦｆＦＯエントリからの追跡距、１１！より以上
ではないカメラにより検出されるシーム点は、ＦＩＦＯ
に対してロードされない。

ＦＩＦＯは、シーム点を確保しこれらシーム点をＦＴＦ
Ｏに格納することにより、追跡アルゴリズムに対してロ
ードされる。シーム点の確保は、探索モードにおいて開
始し、シームが発見されないことを前提とする。このプ
ロセスは、視覚システムの信号処理用コンピュータ２６
を待機してデータ・メツセージを送出し、シームが発見
されてオペレータの選定したテンプレートに突合せられ
たかどうかを調べるためこれを検分する。

ＦＩＦＯにロードされる各追跡点は、［Ｍ］マトリック
スを用いてカメラ座標から手の座標へ変換される。この
データが約４５０ミリ秒而に開始した走査からのもので
あるため、これは略々３０回の更新前の正確であったマ
ニピュレータ座標と関連させられる。前のり、Ｅ、Ｈの
値から［Ｑ］マトリックスを形成することにより、追跡
点の場所はマニピュレータの（ワールド）座標に見出す
ことができる。この点は、待ち行列中のＲ＜１の点と比
較され、これが必要な最小距ｌ１ｌ（即ち、ＦＩＦＯの
追跡距離）を越えるならば、ＦＩＦＯに入れられる。も
しこのシームが３回の連続する走査に対して見出される
ならば、シームの初めは妥当なものであると仮定される
。更に、もしシームが３回の連続走査の後に無くなるな
らば、このシームは失われたと仮定される。シームの発
見とシームの追跡の両方においてプロセスの機能は実質
的に同じものであることは明らかであろう。データが受
取られ、カメラ座標から手の座標へ変換され、マニピュ
レータの（ワールド）座標に変換され、検査され、必要
に応じてＦＩＦＯに入れられる。もしプロセスが新たな
シーム点を完全に一杯の状態のＦＩＦＯに対して人ねよ
うとするならば、エラーが生じる結果となる。

ワールド座標におけるシーム場所データを生じるための
本発明の変換および制御手法を、特定のシームの初めあ
るいは終りの認識のための能力と共に用いて、ある特定
のマニピュレータ・プログラムが関連して実行されるべ
き工作物の諸特徴の場所を定義する座標データを生成す
ることができる。例えば、ある車両のボテイーの風防は
、風防の終端部の場所を包囲したシームの初めまたは終
りを検出することにより位置決めすることができる。次
に、これらの場所の座標は、風防挿入ルーチンに対する
プログラムされた座標を風防の開口の実際の位置に補正
するための必要なオフセット操作、置換あるいは回転を
規定するために使用することができる。

実際の追跡操作は、２つのサブプロセスに分けることが
できる。第１のサブプロセスは、ＦＩＦＯにおけるどの
点が指向する点であるかを決定し、次いでＦＩＦＯを然
るべく調整する。

もし工具中心点から第１のＦＩＦＯ位置までの距離が増
分距離ΔＳの２倍に等しいかあるいはこれより小さけれ
ば、第１のＦＩＦＯ位置は捨てられ、次のＦＩＦＯ位置
が再呼出し即ちポツプアップされる。適当なＦＩＦＯ位
置が選択された時、第２のサブプロセスがこの位置へ進
めるため必要な計算を行なう。この状態は第１５図に示
されている。第１５図においては、その時の工具の中心
点はＰ　Ｋ−１で示される。

点Ｐ３１は、シーム点取得プロセスにより見出された適
当なＦＩＦＯ点である。その時の工具の中心点の場所Ｐ
　Ｋ−１から光の工具中心点の場所Ｐ　Ｋ−２を差引く
ことにより、ベクトルＵｌを決定することができ、これ
がその時の移動方向を示す。ベクトル「「は、プログラ
ムされた方向ベクトルであるか、あるいはそうでなくて
もよい。

いずれにおいても、目標点Ｐａｌはベクトル「「上には
存在せず、その結果マニピュレータは工具の中心点の方
向を変更しなければならない。

このためには、その時の工具の中心点の場所ｐ　’ｘ　
−＋から目標点ＰＳＩへのベクトルＷｌが形成され、点
Ｐ　Ｋ−１と点Ｐｌｉ＋との間の目標距離が第１５図に
示される如く計算される。

上記のように、再生距ｍ　Ｓ　Ｔが増分距離ΔＳの２倍
以上でなければならないため、工具の中心点が唯１回の
操作では点Ｐ　Ｉｃ−１から点Ｐｇｌへ移動できないこ
とが判る。その結果、第１５図に示されるように、ベク
トルＴ「に沿フた一回の操作の距離ΔＳにおいて工具の
中心点が達する点、にの座標が計算される。次いで、マ
ニピュレータは工具の中心点を点ｐｓＫまで案内する。

もし点Ｐ。と点ｐｓ＋との間の残りの距離が依然として
増分距離ΔＳの２倍より大きければ、工具の中心点のそ
の時の位置、および目標点として点ｐｓ＋を用いて、同
じ計算が行なわれることになる。一方、もし点ＰＳＫと
点ｐｓｒとの間の距離が増分距離ΔＳの２倍を越えなけ
れば、点ＰＳＩは捨てられ、新たな目標点Ｐｓ２がＦＩ
ＦＯから取部される。次いで、目標点として点Ｐｓ□を
、また工具の中心点のその時の位置として点ＰＦｉＫを
用いてベクトルＷ２を確保するため、同じ計算が繰返さ
れる。

次に、視覚データ・プロセッサ２４により行なわれる反
復ループ全体を示す第１６図を参照する。

判断ステップ１６１において、視覚データ・プロセッサ
２４が視覚システムの信号処理用コンピュータ２６から
完全なメツセージを受取ったかどうかを調べるため検査
が行なわれる。もし完全なメツセージが受取られなかフ
たならば、プロセス・ステップ１６２において、視覚デ
ータ・プロセッサ２４は標準的なデータ通信プロトコル
を用いて視覚システムの４８号処理用コンピュータ２６
と通信することになる。

もし判断ステップＩＦｉｌ　において完全メツセージが
視覚システムの信号処理用コンピュータ２６から受取ら
れたことが判定されるならば、ＹＥＳ側をたどってプロ
セス・ステップ１６３へ進む。プロセス・ステップ１６
３は、視覚システムの信号処理用コンピュータからのシ
ーム座標が追跡するＦＩＦＯの距離と一致するかどうか
を調べるためプロセスのシーム・サブルーチンを呼出（
）、ＦＩＦＯへ入れられねばならない。判断ステップ１
６５はプロセス・ステップ＋６２または１６３に進み、
サイフルル制御部２３がメツセージを視覚データ・プロ
セッサ２４へ送ったかどうかを判定する。もし答えがＮ
ｏならば、今述べた詐りのプロセス・ステップが反復さ
れる。も（）答えがＹＥＳならば、視覚データ・プロセ
ッサ２４はプロセス・ステップ１６５においてサイクル
制御部２３からのメツセージに従う。

プロセス・ステップ１１５５におけるサイクル制御メツ
セージの性格に従って、プロセス・ステップ１６６％１
８７および＋６８の適当なものがこの時実行されること
になる。プロセス・ステップ１６６においては、適当な
テンプレートが視覚データ・プロセッサ２４により信号
処理用コンピュータに対して与えられ、追跡するＦＩＦ
Ｏの距離が計算されることになる。

もしプロセス・ステップ１６７が実行されるならば、視
覚データ・プロセッサ２４はＯＦＦ信号を視覚システム
の信号処理用コンピュータ２６に対して送出することに
なる。

もしプロセス・ステップ１６８が実行されるならば、適
当なＦＩＦＯエントリが還択され、次の工具の中心点Ｐ
ＳＫが計算されてサイクルＩＩＪ御部２３によりアクセ
スするため予め定めた場所に格納される。プロセス・ス
テップ１．６６　、１６７および１６８のどれかの終了
時Ｃ２視覚データ・プロセッサ２４は、プロセス・ステ
ップ＋６６　、１６７および１６８のどれが実施される
かに従って、プロセス・ステップ１６ｇにおいてサイク
ルＩＩＪ御部２３に応答して適当な信号を送出すること
になる。プロセス・ステップ１６９に続いて、第１６図
のルーチンが反復され、ループを完了する。

第１６図のプロセス・ステップ１６２と関連するサブル
ーチンが第１７図に示されている。これは、視覚データ
・プロセッサ２４が当技術において周知の如く、プロト
コルＤ　Ｉ　Ｎ　６６０１９に従って視覚システムの信
号処理用コンピュータと通信する唯１つのプロセス・ス
テップ１７０からなる簡単なサブルーチンである。

第１６図のプロセス・ステップ１６３と関連するサブル
ーチンが第１９図に示される。このサブルーチンにおい
ては、第１のプロセス・ステップ１７１は、視覚データ
・プロセッサ２４が視覚システムのセンサ・カメラによ
り与えら才する座［Ｘｑｃ、　ＹｓｃおよびＺ３Ｃをマ
ニピュレ−タの（ワールド）座標ｘｑ、ｙ、、ｚ、に変
換することを要求する。プロセス・ステップ！７２にお
いては、ＦＩＦＯにロードされた！＆後の点から視覚シ
ステムの点Ｘｓ％Ｙ、およびＺｓへのベクトルの座標Ｘ
ｃｓ、Ｙｃ５およびＺＣＳは、新たな視覚システムの点
Ｘ！Ｉ、Ｙ１１、Ｚ、の座標から前（７）Ｆ　Ｉ　ＦＯ
（７）点ＦＩＦＯＸ、ＦＩＦＯＹおよびＦＩＦＯＺの座
標を差引くことにより計算される。その後、プロセス・
ステップ１７２は、ベクトル座標成分Ｘ　Ｃｇｓ　Ｙ　
Ｃ！ｌおよびＺｃｓの２乗の和の平方根をとることによ
り、このベクトルの大きさ、即ち距ｍＳ３を計算する。

第１９図の判断ステップ１７３において、走査距離Ｓ３
は予め定めた追跡するＦＩＦＯの距離Ｓｔｐと比較され
る。もしＳｓｈ＜Ｓｒｒと等しくないかあるいはこれよ
り大きければ、第１９図のサブルーチンは第１６図のル
ーチンへその判断ステップ１６４の直前に戻る。もしＳ
３がｓｙｒと等しいかあるいはこれより大きければ、視
覚システムにより与えられる点の座＊Ｘｓ、Ｙｓおよび
Ｚ８がステップ１７４においてＦＩＦＯヘロード即ちブ
ツシュされる。ステップ＋７４に続いて、第１９図のサ
ブルーチンは終了して、第１６図のルーチンの判断ステ
ップ＋６４の直前へ戻る。

第１６図のプロセス・ステップ１６６と関連するサブル
ーチンが第１８図に示される。第１８図のこのサブルー
チンはプロセス・ステップ１７５で開始し、このステッ
プではユーザが選択したテンプレートが視覚データ・プ
ロセッサ２４によってそのメモリー４２から視覚システ
ムの信号処理用コンピュータ２６へ送られる。プロセス
・ステップ１７５の後には判断ステップ１７６が続き、
こわにおいては視見システムがオンであるかどうかが判
定される。もし：′７れに対する答えがＹＥＳであれば
、答えのＹＥＳのバイパス線は第１８図のサブルーチン
を終了させ、プロセスは第１６図のルーチンのプロセス
・ステップ＋６９の直前へ戻る。もし判断ステップ１７
６において視覚追跡システムがオンの状態にないと判定
されるならば、ステップ１７７において信号「ＯＮ」が
視覚データ・プロセッサ２４によって視覚システムの信
号処理用コンピュータ２６へ送られる。

ステップ１７８においては、ＦＩＦＯの追跡距離ＳＴ＋
−が第１８図に示されるように計算され、これにおいて
はｘｃ、ＹｃおよびＺｃは手の座標系におけるカメラの
視野の中心の座標であり、Ｘア、ＹＴおよびＺ７は手の
座標系に３ける工具の中心点の座標である。定数４は、
追跡点が位置する視野の中心からの約５０．８＋＋ｍ　
（２インチ）の２乗を表わす、数３５はＦＩＦＯ位置の
数を表わす。

第１８図のステップ＋７８の終りに、この特定のサブル
ーチンが終了され、プロセスは第１６図のルーチンのプ
ロセス・ステップ１６９の直前へ戻る。

第１６図のプロセス・ステップ１８７と関連するサブル
ーチンが第２０図に示される。これもまた、唯１つのプ
ロセス・ステップ１７９を含む簡単なサブルーチンであ
る。プロセス・ステップ！７９においては、視覚データ
・プロセッサ２４は信号ｒｏＦ　ＦＪを視覚システムの
信号処理用コンピュータ２６に送出する。この動作が完
了すると、このサブルーチンが終了され、プロセスを第
１６図のルーチンのプロセス・ステップ１６９の直前へ
戻す。

第２１図は、プロセス・ステップ１６８と関連したサブ
ルーチンを示している。第２１図のサブルーチンは、工
具の中心点の速度ｖＫがサイクル制御の自動モード４８
から再び呼出される第１のプロセス・ステップ１８０を
有する。このサブルーチンは、第２２図の計算補正サブ
ルーチンを呼出す第２のプロセス・ステップ１８１を有
する。

第２２図のサブルーチンは、増分距離ΔＳの２倍が目標
距ｍ　ｓ　Ｔ、即ち工具の中心点から活動シーム点Ｐ３
までの距離と等しいかあるいはこれより大きいかどうか
が判定される判断ステップ＋８２で始まる。もし答えが
ＮＯであれば、次の２つのステップがＮＯのバイパス線
を介して飛越される。もしこの答えがＹＥＳならば、検
査中のＦＩＦＯエントリが工具の中心点のその時の場所
に近過ぎることが知られる。判断ステップ！８２に対す
るＹＥＳの答えは判断ステップ１１１１３へ進み、ここ
でＦＩＦＯは空であるかどうかが調べられる。もしこの
ＦＩＦＯが空であるならば、ＹＥＳのバイパス線はプロ
セスを第２１図のサブルーチンの終りへ戻し、このサブ
ルーチンが更にプロセスを第＋６図のルーチンのプロセ
ス・ステップ１６９の直前に戻す。この状態が生じるな
らば、プロセス・ステップ１６９は、サイクル制御部２
３に対して、追跡データがなく、第１６図のルーチンが
その初めの点ヘルーブすることを知らせる。その間、マ
ニピュレータは工具の中心点を前に行なわれたオフセッ
トの補正を含むプログラムされた経路に沿って追跡する
ことになる。

第２２図の判断ステップ１８３へ戻り、ＦＩＦＯが空で
ないことが見出されるならば、検査中のその時のＦＩＦ
Ｏの座標は捨てられ、ＦＸＦＯ座標の次の組が使用でき
るよう取出されることになる。

判断ステップ１８２およびプロセス・ステップ１８４か
らのＮＯのバイパス線は共にプロセス・ステップ１８５
へ進む。ここで、使用されるべきＦＩＦＯエントリはＦ
ＩＦＯから取出され、新たな目標点の座標ＸＳ、ＹＳお
よびＺ３として一時的に格納される。プロセス・ステッ
プ１８６においては、工具の中心点が＋Ｔｈ後になった
座標（即ち、工具の中心点のその時の位置）が再び呼出
される。

次の３つのプロセス・ステップ１８７　、１８１１場よ
び１８ｇにおいては、第１５図に関して説明したものと
同様に計算が行なわれる。このため、プロセス・ステッ
プ１８７においては、工具の中心点の場所から目標点ま
で延長するベクトルの座標が決定される。この情報を用
いて、ステップ１８１１においては、工具の中心点の場
所と目標点との間の距離ＳＴが計算される。

ａ後に、工具の中心点が一回の反復により得る新たなベ
クトルに沿った次の位置点ｐｓｘの座標は、プロセス・
ステップ１８９において計算される。この場所の座標Ｘ
　ＳＫｓ　Ｙ　３Ｘ％　Ｚ　３には、サイクル制御部２
３によるアクセスのための予め定めた場所に格納される
。

ステップ１９０に続いて、第２２図のサブルーチンが終
了され、第２１図のサブルーチンの終りへ戻る。第２１
図のサブルーチンが終ると更に、第１５図のルーチンの
プロセス・ステップ１６つの直前に戻る。

第２３図は、サイクル制御部２３によって行なわれるサ
ブルーチンを示している。このサブルーチンは第１１図
のサブルーチンのステップ１２９により呼出されること
が思い出されよう。

視覚追跡システムと関連して視覚データ・プロセッサ２
４により呼出される補正は、マニピュレータのオペレー
タによりプログラムされる最大の補正よりも大きい。も
しこの状態が生じると、この補正を調整即ち縮減するこ
とが必要となる。

補正のこのような圧縮が、第２３図のサブルーチンの目
的である。

このサブルーチンは判断ステップ１９１で始まり、これ
によりシームの追跡が生じつつあるかどうかが判定され
る。も１ノこの答えがＮｏであれば、第２３図のサブル
ーチンは終了し、第１１図のサブルーチンへ戻る。もし
答えがＹＥＳならば、第２３図のサブルーチンはプロセ
ス・ステップ＋９２において状態検査を呼出すことにな
る。プロセス・ステップ１９２は、第２４図に示される
視覚データ・プロセッサの状態検査サブルーチンを呼出
す。第２４図のサブルーチンは判断ステップ＋９３で始
まり、このステップが視：（゛ｌデータ・ボードにおけ
る誤りを検査する。もし誤りが見出されなければ、Ｎｏ
バイパス線が第２４図のサブルーチンを終了し、これが
第２３図のサブルーチンのプロセス・ステップ１９２の
直後に戻る。

もし第２４図の判断ステップ！１）３が誤差が存在する
ことを判定するならば、誤差の性質に基いてプロセス・
ステップ１９４が適当に応答する。誤差に従って、プロ
セス・ステップ１９４はマニビュレータ゛のオペレータ
に対して誤差を知らせ、マニピュレータを停止し、警告
灯灯を投入し、マニピュレータを作動サイクルから外す
等が可能である。プロセス・ステップ１９４は第２４図
のサブルーチンを終了する。第２４図のサブルーチンは
、プロセス・ステップ９４の間生じる動作に従って、プ
ロセス・ステップ９４に続いて第２３図のサブルーチン
へ戻ることになる。

第２３図に戻り、第２４図のサブルーチンは第２３図の
プロセス・ステップ１９２の直後に戻るとすれば、この
サブルーチンの次のステップ１９５はサイクル制御部２
３により妥当な視覚データ処理座標が受取られたかどう
かを判定する判断ステップである。これらの座標は、ｘ
ｓＫ、ＹｓＫおよびＺＳＫで示される。もしこれらの座
標が妥当であることが判定されるならば、ステップ１９
６は、前のオフセットを含む第７図のステップ９５によ
り生じる最後の補間された点（座標ＸＫ　＋０ＦＦｘ　
ＹＫ＋ＯＦＦ、　　およびＺ、＋ＯＦＦ、を存する）か
ら新たな目標点Ｘｓに、ＹｓＫおよびＺＳＫに向けて延
長するベクトルの座標成分ＸΔ、ＹΔおよびＺ＾を計算
する。このベクトル座標の２乗の和の平方根をとること
により、これらの点間の距１！ＩＭＡＧΔ１が計算され
る。

プロセス・ステップ１９６の後には、ＭＡＧΔ１がプロ
グラムされた経路からの偏差に対して許容される限界を
越えるかどうかが判定される判断ステップ１９７が続く
。−時保持変数ＴＥＭＰは積ＭＡＸ　（ＶＰ）ΔＴとし
て定義されるが、ここでＭＡＸはマニピュレータのオペ
レータにより予めプログラムされる最大補正値（０から
１．５）、ＶＰはプログラムされた速度、およびΔＴは
１つの増分間隔の時間である。従って、（ｖｐ　）ΔＴ
はプログラムされた速度における増分距離をなし、ＴＥ
ＭＰはこの距離の倍数を表わす。

もしＴＥＭＰがＭＡＧＡＴよりも小さくなければ、次の
３つのプロセス・ステップ１９８　、１９９および２０
０がＮｏのバイパス線によりバイパスされる。もしＴ　
Ｅ　Ｍ　Ｐ　、６（Ｍ　Ａ　Ｇ＾、より小さければ、視
覚データ点Ｐ。線が予めプログラムされた限界よりも大
きなベクトルＸΔ、ＹΔおよびＺΔにおいである距離に
存在することが判る。プロセス・ステップ１９８および
１９９においてｘｇ１Ｙ３におよびＺ。に対する新たな
値が生じ、これを同じベクトル上の場所Ｐ３Ｋ、即ちＰ
ＫからのＴＥＭＰの大きさに置く。大きさＴＥＭＰを有
するこのベクトルの座標成分は、ＴＥＭＰと、大きさＭ
ＡＧΔ、に対する座標成分ＸΔ、ＹΔ、ＺΔの比との積
として定義される。効率の理由から、プロセス・ステッ
プ１９８においてＴＥＭＰをＭＡＧΔ１により除してＭ
ＡＧΔ２を生じることにより最初に共通の除算が行なわ
れる。

プロセス・ステップ１９９においては、ＸＳに、”　’
３Ｋｓ　Ｚ　ｇＫに対する１組の新たな値がＭＡＧΔ２
を用いて計算される。この新たな点Ｘ３ｇ、ＹＨＫ、Ｚ
ＳＫは、マニピュレータが工具の中心線をＴＥＭＰと等
しく置くことにな）た点からある距離ニある元のベクト
ルＸΔ、Ｙ＾、ＺΔに沿って存在する。

プロセス・ステップ２００は、点ＰＩＩＫと点ＰＫ−１
との間の実際の距！１ＭＡＧΔ３をΔＳに等しくなるよ
うにする。最初に、工具の中心点の場所、即？）ＸＫ−
、＋ＯＦＦ、　、ＹＫ−、＋０ＦＦＹ、ＺＫ−、＋０Ｆ
ＦＺと、新たに計算されたＸ５ＫｖＹ、、　　　Ｚ、、
との間の実際の距離は、座標成分の差ＸΔ、ＹΔ、Ｚ＾
を計算し、次いでそれらの２乗の和の実方根をとること
により計算される。

この距離は、ＭＡＧΔ３として示される。次に、Ｐ　Ｋ
−１からのＰ。へのベクトル上の距離が、ステップ１９
８および！９９に示されるものと同じ手順を用いて、増
分距離ΔＳに調整される。

特に、大きさＭＡＧＡ４はＭＡＧ＾３でΔを除すことに
より計算される。ＭＡＧΔ４を用いて、第３および最後
の目標点Ｘ　３に％　Ｙ　ＳＫ、２３にの座標が計算さ
れる。この点は、距離および速度の両方に関して補正さ
れるＲ後の目標点をなす。最後に、プロセス・ステップ
２０１において、Ｘ、　ＹおよびＺのオフセットが示さ
れるように泪算される。

もし判断ステップ１９５において妥当な座標が受取られ
なかったと判定されたならば、サイクルｉｖＪ御は単に
プログラムされた経路と平行でありかつこれからこの時
存在する何等かのオフセットだけずれた経路を計算する
ことになるに過ぎない。このことは、プロセス・ステッ
プ２０２において示される。プロセス・ステップ２０３
においては、調整された点が第１１図のサブルーチンの
点ＸＫ、ＹＫおよびＺＫに対し代入される。

プロセス・ステップ２０１または２０２のいずれかがこ
のサブルーチンを終了させ、このプロセスは第１１図の
サブルーチンへ戻される。

本発明についてはここに詳細に記述した。

本発明においては、その主旨から逸説することなく変更
が可能である。

【図面の簡単な説明】

第１図は本発明の視覚シーム追跡システムが設けられた
工業用マニピュレータおよびその制御部を示す概略図、
第２図は第１図の制御部のブロック図、第３図はマニピ
ュレータ座標に示されたマニピュレータの概略図、第４
図はマニピュレータの手首部およびその回動軸心を示す
部分断面立面図、第５図は手の座標系および方位角り、
Ｅ、Ｒの概略図、第６図は自動運転モードにおいて制御
されたマニピュレータにより行なわれる運動を示す概略
図、第７図および第８図は運動制御を生じる２つの主な
制御手順のフローチャート、第９図乃至第１３図は第７
図のフローチャートのサブルーチンおよび主要セグメン
トを示すフローチャート、第１４図はレーザー・スキャ
ナー／カメラ・ヘッドを示す概略図、第１５図は本発明
の視覚シーム追跡システムによる工具の中心点のプログ
ラムされた経路において行なわれる補正を示す概略図、
第１６図は視覚データ・プロセッサにより行なわれる全
体的反復ループを示すフローチャート、第１７図は視覚
データ・プロセッサのメツセージ処理手順を示すフロー
チャート、第１８図はユーザが定義したテンプレートを
用いる視覚システムを付勢する手順を示すフローチャー
ト、第１９図は視覚システムのシーム座標の最も後に生
成された組がＦＩＦＯにロードされるべきかどうかを判
定する手順を示すフローチャート、第２０図は視覚シス
テムを消勢する手順を示すフローチャート、第２１図は
直線状経路上のある中間点の座標をシーム座標に作る全
体的手順を示すフローチャート、第２２図はこの中間点
の座標を生じる詳細な手順を示すフローチャート、第２
３図は座標の調整ルーチンを示すフローチャート、およ
び第２１図は第２３図のサブルーチンを示すフローチャ
ートである。 １・・・マニピュレータ、２・・・ベース板、３・・・
ベース部材、４・・・上部アーム、５・・・枢着部、６
・・・萌アーム、７・・・枢着部、８・・・手首部、９
・・・第１節部、１０・・・軸部、１１・・・第２ｉＴ
ｉ部、１２・・・軸部、１３・・・面板、！４・・・溶
接工具、ｌ　５−・・レーザー・スキャナー／カメラ・
ヘッド、１６・・・ベース軸心、１７・・・水平肩部軸
心、１８・・・エルボ−軸心、１９・・・工具中心点、
２０・・・マニピユレータ１］御部、２１・−・サーボ
制御部、２２・−インターフェース、２３・・・サイク
ル制御部、２４−・・視覚データ・プロセッサ、２５−
・・カメラ−Ｊｍ装置、２ト・信号処理用コンピュータ
、２７．２訃−ねじ、２９．３０−・ナツト、３１．３
２・−プリー３３−・・伝動装置、３４−・・駆動ビニ
オン、３５−・・リング歯車、３６〜３９・・・モータ
、４０・・・伝動装置、４１・・・マイクロプロセッサ
、４２−・・メモリー、４３・・・システム・バス、４
４・・・マイクロプロセッサ、４５・・・割込み制御部
、４６・・・メモリー　４７−・・モード制御、４８−
・・自動モード、４９−・・教示モード、５０−・・デ
ータ・ストア、５２・・・マイクロプロセッサ、５３・
・・プログラム可能タイマー　５４−・・局部メモリー
　５５−・・サーボ制御処理プログラム、５日・・・局
部バス、５７・・・二重ボート、５８・・・駆動インタ
ーフェース回路、５９・・・マニピュレータ軸心位置測
定トランスジューサ・インターフェース、６０．６ト・
・マニピュレータ軸心駆動モータ、６２・・・サーボ入
出カプログラム、６３．６４・・・位置のトランスジュ
ーサ、６９−・・入出力ボード、７０−・・入出力イン
ターフェース回路、７１・・・二重ボート装置、７２・
・・局部バス、７３・・・局部マイクロプロセッサ、７
４・・・局部メモリー７５・・・直列インターフェース
回路、７６・・・オペレータ・パネル、７７・・・並列
インターフェース回路、７８−・・マニピュレータ入出
カプログラム、７９−・・オペレータ・パネル入出カプ
ログラム、ａ　ｉ−・・データ入出カプログラム、８２
−・・直列チャネル・インターフェース、８３・・・大
容量データ・トランシーバ、８４−・・入出カプログラ
ム、８５・・・教示用ペンダント、８７−・・第１節部
、８８・・・第２節部、８９．９３・・・節部、９０〜
ｇ２・−回動軸心。 ＦＩ　Ｇ。 ＦＩＧ、　　１ ＩＧ ＩＧ Ｕｌ：！〜−−−。 ＦＩＧ。 ＦＩＧ、　　＋６

Claims (1)

1. 【特許請求の範囲】 １、工具のベクトルが所要の姿勢に対し整合される間工
   具の中心点が予め定めた経路に追従するようにマニピュ
   レータを制御する方法であって、該工具の中心点および
   工具のベクトルがマニピュレータにより支持された機能
   要素と関連付けられ、該機能要素の運動が工作物の特徴
   により規定される所要の運動経路に 近似させる前記工具の中心点の予期された 運動経路を規定し、該工作物の特徴はマニピュレータに
   より支持されるスキャナ／カメラを含む視覚システムに
   より検出される特徴を呈する方法において、 （ａ）前記視覚システムが、所要の経路上の１点の場所
   の座標を表わす経路点の信号を周期的に生じ、該経路点
   の座標が前記スキャナ／カメラ・ヘッドと関連した視覚
   システムの基準フレームに対して規定され、該視覚シス
   テムの基準フレームが工具ベクトルの姿勢に対して固定
   された関係において前記工作物に対する姿勢を有し、 （ｂ）前記経路点の信号により表わされる座標が、前記
   マニピュレータの基準フレームに対する座標に変換され
   、 （ｃ）選択された変換済み経路点の座標を用いて、略々
   所要の経路上に工具の中心点の運動を生じることを特徴
   とする方法。 ２、前記工具の中心点により追従される経路とプログラ
   ムにより規定される予期された経路との間の偏差率が、
   オペレータにより設定される限界値に従って制限される
   ことを更に特徴とする請求項１記載の方法。 ３、経路点の信号により表わされる点間の距離が、工作
   物の特徴に関連するスキャナ／カメラ・ヘッドの速度お
   よび走査速度に依存し、前記の選択された変換経路点の
   信号により表わされる点が少なくとも予め定めた一方か
   ら他方への距離にあることを更に特徴とする請求項２記
   載の方法。 ４、（ａ）前記工具の中心点が予め定めた時間内に運動
   する増分を規定する増分距離が計算され、 （ｂ）ある選択された変換済み経路点の信号により表わ
   される各点が、前記増分距離の少なくとも２倍だけ選択
   の時点においてこの時の工具の中心点の場所から隔てら
   れていることを更に特徴とする請求項３記載の方法。 ５、選択された変換済み経路点の信号により表わされる
   点に向う前記工具の中心点の運動が、各々が長さにおけ
   る計算された増分距離である一連の運動において生じる
   ことを更に特徴とする請求項４記載の方法。 ６、工具のベクトルが所要の姿勢に整合される間、工具
   の中心点が予め定めた経路に追従するようにマニピュレ
   ータを制御する装置であって、該工具の中心点および工
   具のベクトルが前記マニピュレータにより支持される機
   能要素と関連付けられ、該機能要素の運動が、工作物の
   特徴により定義される所要の運動経路に近似させる工具
   の中心点の運動の予期される経路を規定するプログラム
   に応答して生じ、前記工作物の特徴は、前記マニピュレ
   ータにより支持されるスキャナ／カメラ・ヘッドを含む
   視覚システムにより検出される特徴を有する装置におい
   て、 （ａ）前記視覚システムが、所要の経路におけるある点
   の場所の座標を表わす経路点の信号を周期的に生じ、該
   経路点の座標は前記スキャナ／カメラ・ヘッドと関連付
   けられた視覚システムの基準フレームに対して規定され
   、該視覚システムの基準フレームが前記工具ベクトル姿
   勢に関して固定された関係において工作物に関連する姿
   勢を・有し、 （ｂ）前記経路点の信号により表わされる座標を前記マ
   ニピュレータに関する基準フレームに関する座標へ変換
   する手段が設けられ、 （ｃ）変換された経路点の座標を選択する手段が設けら
   れ、 （ｄ）前記工具の中心点が所要の経路に実質的に追従す
   るように、前記選択された変換済み経路点の座標に対応
   して前記工具の中心点の運動を生じる手段が設けられる
   ことを特徴とする装置。 ７、オペレータにより設定された限界値に従ってプログ
   ラムにより規定される予期される経路からの前記工具の
   中心点が追従する経路の偏差率を制限する手段が設けら
   れることを更に特徴とする請求項６記載の装置。 ８、経路点の座標により表わされる点間の距離が、前記
   工作物の特徴に関する前記スキャナ／カメラ・ヘッドの
   速度および走査速度に依存し、変換された経路点の座標
   を選択する装置が少なくとも予め定めた一方から他方へ
   の距離にある点を表わす変換された経路点の座標を選択
   することを更に特徴とする請求項７記載の装置。 ９、（ａ）前記工具の中心点がある予め定めた時間内に
   運動する増分を規定する増分距離を計算する手段を設け
   、 （ｂ）変換された経路点の座標を選択する前記手段が、
   少なくとも計算された増分距離の２倍だけ選択の時間に
   おけるこの時の工具の中心点の場所から隔てられた変換
   された経路点の座標を選択することを更に特徴とする請
   求項８記載の装置。 １０、選択された変換済み経路点の座標に応答して前記
   工具の中心点の運動を生じる前記手段が、各々が長さに
   関して計算された増分距離である一連の運動を生じるこ
   とを更に特徴とする請求項９記載の装置。