JPH06175716A - マニピュレータの物体把持作業における位置・姿勢補正方法 - Google Patents

Abstract

(57)【要約】 【目的】 センサデータによって仮想ツール座標系を定
義し、キャリブレーション誤差を軽減し得るマニピュレ
ータ位置・姿勢補正方法を提供する。 【構成】 （ａ）センサとマニピュレータにセンサ座標
系を設定し、（ｂ）該被把持物体の位置・姿勢のセンサ
データに準拠して定義されたツール座標系をマニピュレ
ータに設定した上で動作教示を行い、（ｃ）教示内容を
再生してマニピュレータを動作させるに際して、前記定
義されたツール座標系のデータを、該再生時の被把持物
体の位置・姿勢のセンサ実測データに書き換えて補正用
データを作成し、（ｄ）該補正用データと前記教示時に
教示された他のデータに基づいてマニピュレータの位置
・姿勢を制御する。また、教示に際して、ツール座標系
データ以外に、コンフィギュレーションデータ、冗長度
規定データ等を含む付帯データを合わせて教示し、座標
系再定義の段階時に該付帯データは非置換データとし、
再生段階時にそのまま利用した動作制御を行う。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、マニピュレータ（ロボ
ットマニピュレータ、以下、単にマニピュレータと言
う。）に対象物を把持させて該対象物に関連した作業を
実行する際に、作業空間内にセンシング領域を持つセン
サによって取得されるセンサデータに基づいて、物体を
把持したマニピュレータの位置・姿勢を補正する方法に
関し、特に、その為の教示・再生の方式に特徴を有する
前記方法に関する。

【０００２】

【従来の技術】マニピュレータにワーク等の物体を把持
させ、運搬、マウント、パレタイジング、アセンブリン
グ等の作業を実行する場合に、３次元視覚センサ等のセ
ンサを用いて該物体の位置・姿勢を検出し、検出データ
を用いてマニピュレータの位置・姿勢を補正して、以後
の動作をより正確なものとすることは、公知の技術課題
である。

【０００３】例えば、所定位置に次々と送られてくるワ
ークを１個づつ順次別の定位置へ移動させ、定位置に配
置された物体に対して、一定の姿勢で対面させる作業等
においては、各ワーク移動サイクル毎のマニピュレータ
の運動が正確に同一であったとしても、ワークの把持状
態（被把持物体の位置と姿勢）が厳密に同じとは限らな
いから、ワークとエフェクタの対面関係を一定に保つ為
には、このワーク把持状態の変動を含めた位置・姿勢変
動を打ち消すような位置・姿勢の補正が必要となる。

【０００４】従来より、このような位置・姿勢補正を行
う為の技術として、次の（１）及び（２）の２つの方法
が知られている。 （１）マニピュレータに物体を把持させた状態でマニピ
ュレータに動作教示を行う際に、センサにより該物体の
位置・姿勢を検出し、そのデータをセンサ内のメモリに
記憶させておき、マニピュレータの再生動作時に検出さ
れる物体の位置・姿勢のデータと前記教示時に取得され
たデータとのずれ量をマニピュレータに通知し、マニピ
ュレータ側で位置指令値を修正する方法。この変形とし
て、教示時のデータの記憶をセンサ側でなくマニピュレ
ータ側に行わせ、再生時にマニピュレータ側がデータ変
化量を計算して位置指令値を修正する方法がある。

【０００５】（２）センサデータをそのまま作業座標形
のデータと解釈して、マニピュレータを動作させる方
法。この場合、教示動作は行われない。

【０００６】

【発明が解決しようとする課題】しかしながら、上記の
ような従来の技術にはシステムを簡素化して、効率的な
作業を実行する上で無視出来ない問題点があった。即
ち、上記（１）に記された技術においては、センサ側あ
るいはマニピュレータ側でセンサデータを格納すること
が必要となり、また、教示時と再生時のデータ変化に見
合った補正量を位置指令値に対する補正値の形で求め、
位置指令値を補正量分だけ修正する為の特殊な処理機能
をマニピュレータ側に用意しなければならないという問
題点があった。

【０００７】一方、（２）の型の方法を採用した場合に
は、センサデータを表現しているセンサ座標系とマニピ
ュレータが準拠している基準座標系との関係が正確に認
識され、センサデータが、随時マニピュレータの準拠す
る基準座標系上で表現されたデータに変換可能になって
いなければならない。そして、マニピュレータの位置・
姿勢補正精度は、必然的に両座標系の関係を設定するキ
ャリブレーションの精度に左右されることになるが、一
般に、キャリブレーションの精度には限界があり、ま
た、高精度のキャリブレーションを実行するには、細心
の注意と煩雑な作業が伴うので、作業の効率化の観点か
ら見て好ましくない。

【０００８】本発明は、このような従来技術の問題点を
解消し、簡素な構成によって正確なマニピュレータの位
置・姿勢補正を効率的に行うことの出来るマニピュレー
タ物体把持作業の位置・姿勢補正方法を提供せんとする
ものである。

【０００９】

【課題を解決するための手段】本発明は、物体を把持し
て作業を行うマニピュレータの位置と姿勢の少なくとも
一方を、センサ手段の検出する前記被把持物体の位置と
姿勢の少なくとも一方のデータに基づいて補正する為
に、（ａ）センサ手段とマニピュレータが共有するセン
サ座標系を設定する段階と、（ｂ）マニピュレータに物
体を把持した状態で該マニピュレータに動作の教示を行
うに際して、該被把持物体の位置と姿勢の少なくとも一
方に関するデータをセンサ手段を利用して取得し、該セ
ンサ取得データに基づいて定義されたツール座標系をマ
ニピュレータに設定した上で動作教示を行う段階と、
（ｃ）教示内容を再生してマニピュレータを動作させる
に際して、前記定義されたツール座標系のデータを、該
再生時の被把持物体の位置と姿勢の少なくとも一方に関
してセンサ手段によって取得されるデータに書き換える
形でツール座標系を再定義して、マニピュレータの位置
と姿勢の少なくとも一方を補正する為のデータを提供す
る段階と、（ｄ）該補正用データと前記教示時に教示さ
れた他のデータとに基づいてマニピュレータの位置及び
姿勢の少なくとも一方を制御する段階とを含むマニピュ
レータの物体把持作業における位置・姿勢の補正方法を
提供することによって、上記課題を解決したものであ
る。

【００１０】また、上記（ｃ）の教示に際して、ツール
座標系データ以外に、マニピュレータコンフィギュレー
ションデータ、冗長自由度がある場合の冗長度規定デー
タ等を併せて教示し、上記（ｄ）のデータ置換時に、該
付帯データは非置換データとすることにより、再生に際
して、教示時のマニピュレータのコンフィギュレーショ
ンデータや冗長データ等をそのまま利用することを可能
にしたものである。

【００１１】

【作用】本発明の構成要件をなす上記（ａ）〜（ｄ）の
各段階について、その原理的側面を中心に、更に詳しく
説明する。 （ａ）センサ座標系設定（キャリブレーション） センサが物体の位置・姿勢を計測する時に基準とする座
標系をセンサ座標系として設定すれば良い。例えば、視
覚センサであれば、その視野内にセンサ座標系設定用の
治具、あるいはこれに代わる既知物体を置いてその特徴
点を観測して、センサ側での座標系定義を行う。

【００１２】マニピュレータに対する座標定義は、例え
ば、センサが計測した治具（既知物体）の特徴点を、マ
ニピュレータの作業座標系設定機能を利用してタッチア
ップして、センサに定義された座標系とマニピュレータ
が有している作業座標系の結合を行う。このようなセン
サ座標系の設定方法自体は公知の技術であり、本発明で
は、マニピュレータとセンサが共有する座標系を設定出
来るのであれば、いずれの方法を用いても構わない。

【００１３】上記の治具にタッチアップする方法の他
に、例えば、センサで治具上の特徴点を観測して、セン
サ側での座標系定義を行った後に、マニピュレータに該
治具を把持させ、治具の特徴点が視覚センサに定義した
座標系の原点等の特定点に対応したカメラ視野内位置に
観測されるようにマニピュレータを動かして、その時の
作業座標系上での位置をマニピュレータに認識させるこ
とによって、マニピュレータの有している作業座標系と
センサに設定された座標系との関係をマニピュレータに
認識させる方法がある。センサ座標系を表すデータは、
マニピュレータが有する作業座標系から見たセンサ座標
系を表現する同次変換行列の形で求めることが出来る
［下記（ｂ）の項及び図１参照］。

【００１４】（ｂ）把持状態での教示 センサ座標系設定済みのマニピュレータに、実際の作業
で把持させる物体あるいはそれに準じた物体（例えば、
同寸同形のダミー物体）を把持させた状態で、実際の作
業に相当した運動を行わせて位置・姿勢の教示を行う
が、その一方で、被把持物体をセンサ（センサ座標系設
定済み）で計測し、取得された被把持物体の位置・姿勢
データに基づきツール座標系を設定する為のデータを作
成しする。そして、重要な教示点については、該データ
を使ってマニピュレータにツール座標系を設定した上で
位置・姿勢の教示を行う。

【００１５】この教示方法の原理について、図１を用い
て説明する。図中ΣM ；ＯM ―ＸＹＺ、ΣS ；ＯS ―ｘ
ｙｚは各々マニピュレータが有している作業座標系、マ
ニピュレータとセンサが共有しているセンサ座標系であ
り、ＯM 、ＯS は各座標系の原点を表している。マニピ
ュレータはワークＷを把持しているものとする。Ｔはツ
ールポイントに対応するワークＷ上の特定点である。そ
して、点Ｔを原点として、ワークＷと一体をなすツール
座標系ΣT を定義された状態を考える。例えば、ワーク
Ｗの軸方向をツール座標系ΣT の１つの軸（γ軸とす
る。）に一致させ、該座標軸γの方向及び向きが作業座
標系ΣM の１つの座標軸Ｚの方向及び向きに一致した
時、他の軸αがＸ軸に、β軸がＹ軸に一致するように定
義されたものとする。（実際の定義はセンサデータで行
う。後述の説明参照。） ツール座標系が定義されると、以後、ワーク（被把持物
体）の位置・姿勢はツール座標系の位置・姿勢と１対１
対応することになる。従って、センサ座標系を設定済み
のセンサで、被把持ワークＷの位置・姿勢を計測するこ
とにより、センサ座標系ΣS 上で表されたツール座標系
データを取得することが出来る。このデータを、センサ
座標系からみたツール座標系を表現する同次変換行列
［ＴS ］で表すことにすると、次の関係式（１）が成立
する。

【００１６】 ［ＴM ］＝［Ｆ］・［ＴH ］＝［Ｋ］・［ＴS ］ ・・・（１） ここで、［ＴM ］、［Ｆ］、［Ｋ］は、各々作業座標系
ΣM から見たツール座標系、フェイスプレート面代表点
Ｈを原点とするハンド座標系ΣH 及びセンサ座標系ΣS
を表現する同次変換行列である。また、［ＴH ］は、ハ
ンド座標系ΣHから見たツール座標系ΣT の位置・姿勢
を表す同次変換行列である。

【００１７】式（１）を［ＴH ］について解き、［ＴS
］を含む式で表すと、下記（２）式となる。 ［ＴH ］＝inv.［Ｆ］・［Ｋ］・［ＴS ］ ・・・（２） （但し、inv.［Ａ］は任意の行列［Ａ］の逆行列を表
す。） （２）式は、フェイスプレート代表点Ｈを原点とするハ
ンド座標系ΣH から見たツール座標系ΣT の位置と姿勢
を表現したものであるから、（２）式の右辺の計算を行
う際に［ＴS ］のデータとして、センサで実測されたも
のを用いれば、センサによる実測値通りの位置と姿勢を
有する仮想的なツール座標系をハンド座標系の原点Ｈか
ら見た時の位置と姿勢を表すデータが得られることにな
る。この［ＴH ］の値は、ワークＷの把持状態自体に変
化が無い限り不変に保たれる性質を有している。

【００１８】従って、任意の教示位置において、ワーク
Ｗに実現すべき位置・姿勢をとらせた状態における
［Ｆ］の値と、上記センサ実測データに基づく［ＴH ］
の値を使って（１）式中辺の計算を実行し、［ＴM ］の
教示値として採用すれば、上記仮想ツールの位置と姿勢
の作業座標系ΣM 上でのデータを教示していることにな
る。

【００１９】つまり、センサデータに準拠した［ＴH ］
を与えることによって、上記仮想的なツール座標系が定
義・設定されたマニピュレータに対しては、該仮想的な
ツール座標系についての作業座標系ΣM 上でのデータを
指定する形で教示を行うことが可能になる。即ち、セン
サが計測したデータによって記述される仮想的な物体を
ツールとして扱うことが出来ることになる。

【００２０】このような考え方を導入したことにより、
センサデータが、通常の教示・再生動作の仕組みの中で
一貫して扱われる共通データとされ得るようになった点
が本発明の基本的かつ重要な特徴点である。

【００２１】位置・姿勢の教示時には、上記設定された
ツール座標系の（補正前の）目標値としての［ＴM ］を
教示点毎に教示するが、同時に、手首軸回転数やフリッ
プの有無（ＦＬＩＰ／ＮＯ ＦＬＩＰ）等のデータを含
むコンフィギュレーションデータＣ、冗長自由度を有す
るマニピュレータを使用する場合の逆運動解を作業に好
都合な唯一の解に定める為に指定する制約条件（例え
ば、肘角度）である冗長自由度規定データＲ等を教示す
る。マニピュレータが準拠する作業座標系ΣM 自体が未
設定の場合には、そのデータＭも教示時に併せて設定す
ることも考えられる。

【００２２】結局、ツール座標系設定を含めた教示時の
データとしては、各教示点についてこれらデータの組
｛［ＴM ］、［ＴH ］、Ｃ、Ｒ、Ｍ｝が用意乃至作成さ
れることになる。

【００２３】（ｃ）ツール座標系データ置換 本発明では、上記（ｂ）で説明した原理に従ってツール
座標系を設定して教示を行ったマニピュレータに対し
て、再生動作起動後に位置・姿勢補正を加えることによ
り、把持状態等に変化を生じたとしても被把持物体が企
図された通りの位置・姿勢を実現するようにする。その
為に、先ず、教示データ通りにマニピュレータを再生動
作させ、重要な教示点を通過する前に、センサにより被
把持物体を観測して位置・姿勢を計測する。

【００２４】そして、上記教示済みのデータの内、ツー
ル座標系データ相当部分のみを、適宜の時点で、上記再
生時点計測値（例えば、視覚センサを使用する場合には
被把持物体上の特徴点の位置データを用いる。この特徴
点は、必ずしも教示時の被計測特徴点と同一である必要
はない。要は、再生時位置・姿勢補正前におけるセンサ
データとしての［ＴH ］、乃至［ＴS ］が求められれば
良い。）に基づいて求められたツール座標系データに置
き換える。

【００２５】即ち、教示時の［ＴH ］のデータ［ＴH ］
teach を、再生時の［ＴS ］の実測値［ＴS ］replay
を使って（２）式右辺の計算を実行して求められる［Ｔ
H ］の値［ＴH ］’に置き換える。ここで、［Ｆ］のデ
ータは、当該教示点（位置・姿勢補正は未実行）に関す
る教示済みデータから取得する。

【００２６】そして、他のデータ部分［ＴM ］、Ｃ、
Ｒ、Ｍ等については、教示時のデータを書き換えること
なく保存する。

【００２７】つまり、教示データの内、ツール座標系デ
ータ部分のみを再生時の実測データに準拠したデータに
置き換える。このツール座標系データ置換によって、マ
ニピュレータの位置補正動作に必要なデータが用意され
たことになる。この位置補正用データを利用して補正さ
れた動作内容を決定する原理については、次の（ｄ）の
項で説明する。

【００２８】（ｄ）再生時の位置・姿勢補正 前記式（１）を［Ｆ］についての方程式と見て解くと直
ちに次式（３）が得られる。

【００２９】 ［Ｆ］＝［ＴM ］・inv.［ＴH ］ ・・・（３） ここで、教示時に指定された［ＴM ］の値は、最終的に
実現されるべき被把持物体の位置・姿勢に対応したツー
ル座標系ΣT の位置・姿勢を作業座標系ΣM 上で表現し
たものであるから、教示時と再生時（補正後）とで共通
した不変の値をとるべきである。また、［ＴH ］は、フ
ェイスプレート代表点Ｈから見たツール座標系ΣT の位
置・姿勢を表すから、教示時点と再生時点では変化して
も、再生動作開始後の補正動作によって変化することは
ない（マニピュレータによる物体把持自体は堅固であ
り、把持後に把持状態がぐらつくことはないものとす
る。）。これらの事を考慮に入れて上記（３）式を教示
時と再生時（補正後）について記してみると、次のよう
になる。

【００３０】 ［Ｆ］teach ＝［ＴM ］・inv.［ＴH ］teach ・・・（４） ［Ｆ］af.corr.＝［ＴM ］・inv.［ＴH ］’ ・・・（５） （５）式から［ＴM ］を消去すると、次の（６）式にな
る。 ［Ｆ］af.corr.＝［Ｆ］teach ・［ＴH ］teach ・inv.［ＴH ］’ ・・・（６） この（５）式または（６）式によって、最終的に実現さ
れるべきフェイスプレート代表点（ハンド座標系原点）
の位置とフェイスプレート（ハンド座標系）の姿勢が求
められることになる。（６）式右辺の［ＴH ］teach 及
びinv.［ＴH ］’は、いずれもセンサデータ［ＴS ］te
ach あるいは［ＴS ］’に基づいて、式（２）を用いて
計算される。（２）式における［Ｆ］としては、［ＴH
］teach計算時には、センサ計測時点における［Ｆ］te
ach が用いられるべきであり、また、inv.［ＴH ］’の
計算を行う際には再生時（補正前）のセンサ計測時点に
おける［Ｆ］bef.corr. が各々用いられるべきである。

【００３１】しかしながら、フェースプレートの位置・
姿勢の教示・再生にずれが無い限り、センサによるワー
クＷの計測実行位置においては、［Ｆ］teach ＝［Ｆ］
bef.corr. である。（新たなワークＷ’を把持して再生
段階に入っていても、ワークＷ’のセンサ実測データに
基づくツール座標系再定義を行うまでは、教示段階にお
いて定義されたツール座標系について、教示された通り
の位置・姿勢が再現される。）従って、（２）式の右辺
で、教示時と再生時（補正前）の違いが現れるのはセン
サデータである［ＴS ］（［ＴS ］teach と［ＴS
］’）のみである。ところで、センサ座標系設定時の
キャリブレーション誤差は、［ＴS ］teachと［ＴS ］r
eplayに共通の影響を及ぼしていると考えられるから、
その影響は（２）式の関係を介してそのまま［ＴH ］te
ach 及び［ＴH ］’に及んでいるとみるべきである。

【００３２】ところが、実現されるべき位置・姿勢を定
める（６）式の右辺には、ツール座標系の表現行列の逆
行列inv.［ＴH ］’が、［ＴH ］teach との乗算の形で
入っているので、センサ座標系設定誤差の影響の相当部
分は互いに打ち消し合うことになる。その結果、（６）
式右辺全体に対するセンサ座標系設定誤差の影響は極め
て小さくなっている。この誤差低減作用は、前述した従
来技術には期待し得ない、本発明の位置・姿勢補正方法
に固有の顕著な特徴である。

【００３３】このように、教示時に作成された教示デー
タの内、［ＴH ］のデータは新たなセンサデータに基づ
いて置換され、［ＴM ］のデータはそのまま補正動作内
容を決定するのに用いられる。そして、その他の保存デ
ータ、コンフィギュレーションデータＣ、冗長自由度規
定データＲ等も非置換データとして補正動作時にそのま
ま使用することが出来る。これによって、補正動作後に
おけるマニピュレータの状態を、手首軸回転数、フリッ
プの有無（ＦＬＩＰ／ＮＯ ＦＬＩＰ）、肘角度等に関
して教示時に指定された通り状態を実現することが可能
となる。

【００３４】実際にマニピュレータの運動を制御するに
あたっては、マニピュレータの各軸の位置θ＝（θ1 、
θ2 、θ3 ・・・θn ）［n は軸変数の数］を決定しな
ければならないが、上記（５）式または（６）式によっ
て実現されるべき［Ｆ］の値（行列成分）が定まってい
れば、公知の方法により、次式（７）で一般的に表記さ
れる逆運動演算を行うことで、各軸θ1 〜θn の値を定
めることが出来る。そして、それら値を制御目標値とす
る指令をマニピュレータに発することにより、（５）
式、（６）式を満足する補正動作が実行される。

【００３５】 θ＝Γ［Ｆ、Ｃ、Ｒ、θ(jb)］ ・・・（７） 但し、Гは逆運動学計算関数と呼ばれるもので、具体的
な内容はマニピュレータの機構に依存して定められる。
また、θ(jb)＝（θ1 (jb)、θ2 (jb)、・・・θn (j
b)）はマニピュレータに指令を出す直前における各軸位
置θの値である。

【００３６】

【実施例】上記の通り原理を中心に説明した本発明の位
置・姿勢補正方法を、図２に示されたシステムを用い
て、図３に例示された作業に適用する場合の具体的操作
及びデータ処理の手順の一例について、これらの図及び
図４のフローチャートを参照して順次説明する。図２
は、本発明の方法を実施する為に使用するシステムの１
例を示す概略ブロック図である。システム全体は、図中
番号１０で示された画像処理装置、２０で示されたマニ
ピュレータコントローラ及び図示を省略したマニピュレ
ータ本体から構成されている。

【００３７】画像処理装置１０は中央演算処理装置（以
下、ＣＰＵという。）１１を有し、該ＣＰＵ１１には、
フレームメモリ（画像メモリ）１２、ＲＯＭで構成され
たコントロールソフト用メモリ１３、ＲＡＭ等で構成さ
れたプログラムメモリ１４、不揮発性ＲＡＭで構成され
たデータメモリ１５、カメラインタフェイス１６、画像
処理プロセッサ１７及び通信インタフェイス１８がバス
１９を介して接続されている。

【００３８】カメラインタフェイス１６には、例えば、
製造ライン上に順次送り込まれて来る部品等をマニピュ
レータに把持された状態で撮像して、最高３次元の位置
・姿勢を検出するカメラ３０が接続されている。図で
は、カメラは１台のみを示してあるが、実際に接続され
るカメラの台数は、被計測対象物のとる位置・姿勢の自
由度、必要な情報内容やスリット投影系等の付属手段を
含めた検出系の型式によって異なる。カメラを複数台使
用する場合には、カメラインタフェイス１６内のコネク
タ番号を指定することにより、各カメラからの映像信号
を順次取り込むことが出来るようになっている。カメラ
３０は、作業空間内に固定的に設置された状態で、マニ
ピュレータに把持された物体を撮像するものとする。

【００３９】通信インタフェイス１５は、マニピュレー
タコントローラ２０側の通信インタフェイス２８に接続
されており、両者を介して相互にデータや指令を表す信
号が送受信される。

【００４０】カメラ３０の視野で捉えられた画像は、グ
レイスケールによる濃淡画像に変換されてフレームメモ
リ１２に格納することが出来る。画像処理プロセッサ１
７はフレームメモリ１２に格納された画像を解析処理し
て、物体のカメラ画素面上の位置等を認識する。コント
ロールソフト用メモリ１３には、ＣＰＵ１１が視覚セン
サを制御する為のコントロールプログラムが格納されて
おり、治具を用いてセンサ座標系ΣS を設定する為のキ
ャリブレーション用プログラム、画像処理プロセッサ１
７を用いて物体の位置や姿勢を求める為に必要な画像解
析プログラムや、適正なタイミングでマニピュレータ側
への計測データ送信を指令する為のプログラム等が格納
される。上記画像解析プログラムには、後述する図３の
作業に本発明を適用する為に、ワークＷの３個の特徴点
Ｔ1 、Ｔ2 、Ｔ3 の各像を画像中から抽出し、各点の位
置を決定する為のプログラムが含まれている。また、メ
モリ１９にはユーザが作成するプログラムが格納出来る
ようになっている。

【００４１】なお、カメラ３０の捉えた画像やフレーム
メモリ１２から呼び出された画像を視認する為のＴＶモ
ニタを図示を省略したモニタインタフェイスを介して接
続することも出来る。

【００４２】一方、マニピュレータコントローラ２０
は、中央演算処理装置（以下、ＣＰＵという。）２１を
有し、該ＣＰＵ２１には、制御プログラムを格納したＲ
ＯＭ２２、計算データの一時記憶等の為に利用されるＲ
ＡＭ２３、教示データやキャリブレーション用治具デー
タ等各種設定値が格納される不揮発性ＲＡＭで構成され
たメモリ２４、マニピュレータ本体４０の各軸を制御す
る軸制御器２５、マニピュレータの手動操作、座標系設
定、位置教示、自動運転（再生動作）指令、センサ起動
指令等の画像処理装置との交信を行う為の教示操作盤２
６及び画像処理装置１０側の通信インタフェイス１８及
び図３におけるエフェクタＥの制御部５０に接続された
通信インタフェイス２７がバス２８を介して接続されて
いる。

【００４３】画像処理装置１０及びマニピュレータコン
トローラ２０を含む上記システム構成は、従来のマニピ
ュレータ―センサシステムと基本的に変わるところはな
いが、本発明を実施する場合には、教示時にセンサデー
タに基づいてツール座標系を設定する為のプログラム、
再生時にツール座標系データを置換し、置換されたデー
タに基づいて補正動作を決定する補正データを作成する
為のプログラム（サブプログラム）等がＲＯＭ２２に格
納されると共に、それらプログラムを実行する際の各種
設定値（特徴点Ｔ1 、Ｔ2 、Ｔ3 の位置データに基づい
てツール座標系の原点位置と姿勢を定義するのに必要
な、ワークＷの寸法データ等）が格納されている点が従
来と異なる。

【００４４】以下、上記説明したシステムを使用し、図
３に示された作業を行う場合について述べる。図３は、
本発明の方法を適用する作業の簡単な一例を概念的に描
いたものである。図中、作業空間内の適宜位置に据え付
けられたマニピュレータ４０（アーム等は図示を省略）
は、順次搬入コンベア（図示省略）で送り込まれて来る
ワークＷを位置Ｇ1 で把持し、計測位置Ａを経て作業位
置Ｂに至り、次いで搬出コンベア（図示省略）上の位置
Ｇ2 にワークを置く作業を行う。搬入コンベアはリミッ
トスイッチ等を利用したシステムによって、位置Ｃ1 か
らワークＷが離れると自動的に次のワークを位置Ｃ1 に
送り込むように設定されており、一方搬出コンベアは、
教示作業開始後は、常時運転状態にあるものとする。

【００４５】センサ手段としては、カメラ３０（台数に
ついては、前述の通り）を含む視覚センサを使用し、計
測位置ＡでワークＷの３点Ｔ1 、Ｔ2 、Ｔ3 の位置を観
測することによって、ワークＷの特定点（例えば重心）
Ｔを原点とするツール座標系を設定する為のデータを得
るものとする。

【００４６】ここでは、ワークＷを直方体で代表させ、
３つの軸ＴＴ1 、ＴＴ2 、ＴＴ3 を設定されるべきツー
ル座標系ΣT のα、β、γ各軸に対応させるものとす
る。視覚センサにセンサ座標系ΣS が設定された後に
は、点Ｔ1 〜Ｔ3 の観測データに基づいて、点Ｔを原点
とするツール座標系ΣT （Ｔ―αβγ）に関するセンサ
座標系上データ（同次変換行列［ＴS ］の各成分デー
タ）が画像処理装置１０側で用意され、通信インタフェ
イス１８、２７を介してマニピュレータコントローラ２
０に伝送されるようになっている。作業位置Ｂでは、ワ
ークＷ上の特定点Ｐに対してエフェクタＥによる作業
（例えば、ワーク上の定点Ｐから垂直に穿たれた角孔に
対して、該角孔に整合した角形ピンをエフェクタＥを用
いて差し込む作業）が行われ、その際にワークＷの３次
元位置及び姿勢が厳密に制御される必要があるものとす
る（例えば、上記ピン差込み作業では、角孔の位置と姿
勢のいずれか一方にずれがあると差込み作業が正常に行
われない。）。また、位置Ｇ2 でワークＷを搬出コンベ
ア上に解放する際にもマニピュレータの位置・姿勢を正
確に保ち、ワークの転倒を防止することが要求されてい
るものとする。結局、図３の作業例において位置・姿勢
の補正が必要とされるのは、作業位置Ｂとワーク解放位
置Ｇ2 であるということになる。

【００４７】以下、作用の説明の欄の記述順序に準じ
て、上記例示された作業を実行する際のシステムの操作
及びデータ処理の手順について述べる。全体作業は大き
く（ａ）センサ座標系設定（キャリブレーション）、
（ｂ）把持状態での教示、（ｃ）再生時のツール座標系
データ置換による位置・姿勢補正用データの作成及び
（ｄ）補正された再生動作、に分けることが出来る。

【００４８】まず、（ａ）のキャリブレーション（ツー
ル座標系設定）の手順を図４のフローチャートに沿って
簡単に記すと、初めに、キャリブレーション用の治具を
定位置に配置し（ステップＣ１）、マニピュレータコン
トローラの教示操作盤２６を操作して、通信インタフェ
イス２７、１８を介してセンサを起動する。センサは、
治具の計測を行い（ステップＣ２）、コントロールソフ
ト用メモリ１３に格納されたキャリブレーション用プロ
グラムに従って、カメラ座標系とセンサ座標系ΣS の関
係を定める形でセンサ座標系の設定を実行する。その
際、カメラ座標系とセンサ座標系の関係を表すデータ
は、データメモリ１５に格納される（ステップＣ３）。

【００４９】治具及びセンサ座標系設定の方式には、種
々のものがあるが、本発明ではいずれの方式によっても
良い。例えば、カメラを３台配置し、ステップＣ２で原
点に区別された特徴を付与して既知間隔Ｌ、ｍ行ｎ列の
格子状配列（原点と行及び列が設定されるセンサ座標系
の原点、ｘ軸、ｙ軸等に対応している。）を形成する特
徴点群を有する治具を定位置に配置し、ステップＣ３で
この治具を各カメラで撮影する。そして、撮影された各
画像をフレームメモリ１２に記憶した上で、予め画像処
理装置１０のデータメモリ１５に設定しておいたデータ
（Ｌ，ｍ，ｎ）を使用して、コントロールソフト用メモ
リ１３に格納されているキャリブレーション用プログラ
ムを実行するように教示操作盤２６を操作することによ
り、各カメラのカメラ座標形とセンサ座標系との関係を
定めるという方法がある。

【００５０】センサ側のセンサ座標系設定が終了した
ら、マニピュレータ４０に座標設定用のツールを取り付
けて（ステップＣ４）、マニピュレータを起動し、手動
モードに設定する（ステップＣ５）。教示操作盤２６を
操作して、センサ計測を行った治具の特徴点を座標設定
用ツールで順次タッチアップし（ステップＣ６）、マニ
ピュレータにセンサ座標系ΣS を設定すると共に、作業
座標系ΣM とセンサ座標系ΣS の関係を表現する同次変
換行列［Ｋ］のデータをＲＡＭ２４に格納して（ステッ
プＣ７）、キャリブレーションを終了する。

【００５１】センサとマニピュレータが共有するセンサ
座標系ΣS が設定されたならば、マニピュレータに、実
際の作業で把持させる物体あるいはそれに準じた物体
（例えば、同寸同形のダミー物体）を把持させた状態
で、図３に示した作業に相当した運動を行わせて位置・
姿勢の教示を行う（ｂ）の段階に進むが、これを図５に
示したフローチャートに従って説明する。ここでは、教
示時・再生時の各々において把持する物体は、一貫して
同寸同形の「ワーク」であるとする。

【００５２】まず、教示操作盤２６から教示動作開始の
指令を発すると、マニピュレータとセンサのＣＰＵ１
１、２１は共に教示モードにおける処理動作を開始する
態勢に入る（スタート）。マニピュレータ側では、オペ
レータが教示操作盤２６を操作して、外部装置のコント
ローラ４０を介して位置Ｇ1 へワークＷを送り込む。オ
ペレータは、ワークＷの到着を確認した上で、教示操作
盤を操作し、マニピュレータを待機位置Ｇ0 からワーク
Ｗを正常に把持出来る位置Ｇ1 迄移動し（ステップＭＴ
１）、位置Ｇ1 に関して通常のやり方で位置・姿勢の教
示を行う（ステップＭＴ２）と共に、ワークＷを把持す
る（ステップＭＴ３）。センサ側では、教示動作の開始
後直ちに、マニピュレータからの信号を待つ受信待ちの
態勢に入っている（ステップＳＴ１）。

【００５３】続いて、教示操作盤２６を操作して、ワー
クＷを把持したマニピュレータを位置Ａへ移動させ、以
後の再生動作時において実現させたい位置と姿勢を選ん
で停止させ（ステップＭＴ４）、位置Ａに関して通常の
やり方で位置・姿勢の教示を行う（ステップＭＴ５）。
この状態で、教示操作盤２６を更に操作し、センサを起
動してワークＷの計測を開始する指令を、データ処理の
方式の指定等必要な付帯情報と共にセンサ側に送信する
（ステップＭＴ６）。マニピュレータコントローラ１０
は、送信終了後直ちにセンサからの信号を受信する態勢
に入る（ステップＭＴ７）。

【００５４】送信を受けたセンサコントローラ２０のＣ
ＰＵ１１（ステップＳＴ２）は、コントロールソフト用
メモリ１３から読み出される計測処理の為のプログラム
に従って、マニピュレータに把持されたワークＷの計測
処理を実行する（ステップＳＴ３）。即ち、カメラ３０
によって取得され、カメラインタフェイス１６を介して
フレームメモリ１２に格納された画像を画像処理プロセ
ッサ１７により解析し、画像中から３個の特徴点Ｔ1 、
Ｔ2 、Ｔ3 を抽出する。そして、これら特徴点の位置
を、データメモリ１５に格納されたセンサ座標系とカメ
ラ座標系の関係を表すデータを用いてセンサ座標系上の
座標値データの形で求め、データメモリ１５に格納する
ことで計測処理が終了する。

【００５５】計測処理の終了は、マニピュレータ側に送
信され（ステップＳＴ４）、マニピュレータはこれを受
信して（ステップＭＴ８）、その応答として、計測デー
タの送信を指令する（ステップＭＴ９）。ＣＰＵ２１
は、これを受信して（ステップＳＴ５）、データメモリ
１５に記憶されているＴ1 、Ｔ2 、Ｔ3 のセンサ座標系
上での座標値データをマニピュレータ側に送信し（ステ
ップＳＴ６）、マニピュレータコントローラ１０は通信
インタフェイス１８を介してこれを受信する（ステップ
ＭＴ１０）。教示段階におけるセンサの役割はこれで終
了する（センサ側エンド）。

【００５６】マニピュレータコントローラ１０のＣＰＵ
２１は、ＲＯＭ２０から読み出されるツール座標系設定
用のプログラムに従いツール座標系ΣT を設定する（ス
テップＭＴ１１）。即ち、ステップＭＴ１０で受信した
データに基づき、不揮発性メモリ２４に格納されている
設定値（ワークＷの寸法データ等）を使って、センサ座
標系ΣS から見たツール座標系ΣT の位置と姿勢を表わ
す同次変換行列［ＴS ］をセンサデータとして求め、該
センサデータと、先にキャリブレーション（ステップＣ
７）で設定された同次変換行列［Ｋ］のデータとに基づ
いて、前記（２）式の右辺、inv.［Ｆ］A ・［Ｋ］・
［ＴS ］を計算して、同次変換行列［ＴH ］A の成分デ
ータを決定し、ツール座標系表現データとして、不揮発
性メモリ２４に格納する。その際、［Ｆ］A のデータ
は、マニピュレータの現在位置データに含まれているも
のを読み出して利用する。

【００５７】以上述べたステップＭＴ１１が終了した
ら、ワークＷを把持したまま作業位置Ｂに向けてマニピ
ュレータを移動させ、エフェクタＥによる作業（孔Ｐへ
の角形ピンの差込み）が適正に行える位置で停止する
（ステップＭＴ１２）。位置Ｂにおける位置・姿勢の教
示は、位置Ａにおいて設定された仮想ツール（ツール座
標系ΣT ）に対しての位置・姿勢の教示を行う形をと
る。即ち、ステップＭＴ１１で求めた［ＴH ］のデータ
と位置Ｂ停止時における［Ｆ］B のデータに基づき、前
記（１）式の中辺を計算した結果に相当する［ＴM ］B
＝［Ｆ］B ・［ＴH ］のデータを、ツールポイントＴに
対する教示データとして教示する（ステップＭＴ１
３）。

【００５８】ステップＭＴ１３が終了したら、教示操作
盤２６を再び操作して、マニピュレータを位置Ｇ2 へ移
動させる（ステップＭＴ１４）。尚、作業位置Ｂにおけ
る最終的な停止位置を定めるに当たっては、教示操作盤
２６から外部装置４０に指令を発して実際にエフェクタ
Ｅを作動させ、ピンをワークＷの孔（Ｐ）に差し込む動
作を試験的に実行して、ワークＷが適正な位置と姿勢を
とっていることを確認することが好ましい。その場合に
は、ピンが差し込まれた状態でワークＷが位置ＢからＧ
2 に運ばれ、位置Ｂにおける教示に準じた教示が実行さ
れる（ステップＭＴ１５）。

【００５９】位置Ｇ2 におけるデータ教示に前後して、
教示操作盤２６から「ハンド開く」の指令を出してワー
クＷの把持状態を解除し、ワークＷを常時運転状態にあ
るコンベア上に載置する（ステップＭＴ１６）。ワーク
Ｗを解放したマニピュレータは、ホームポジションＧ0
へ戻される（ステップＭＴ１７）。そして、このホーム
ポジションＧ0 においても、位置ＢあるいはＧ2 に準じ
たデータ教示を行い（ステップＭＴ１８）、教示動作を
終了する（エンド）。

【００６０】以上の教示動作において、各位置Ａ、Ｂ、
Ｇ2 、Ｇ0 、Ｇ1 で与えた教示データの対応行番号をＮ
A 、ＮB 、ＮG2、ＮG0、ＮG1とすると、実際の作業サイ
クルがＧ0 を出発点として、Ｇ1 、Ａ，Ｂ、Ｇ2 を経
て、Ｇ0 に戻って来るものであることから、ＮG1＜ＮA
＜ＮB ＜ＮG2＜ＮG0の関係が成立している。

【００６１】上記説明した移動関連の教示の他に、マニ
ピュレータ制御プログラムの行番号ＮG1とＮA の間に
は、ワークＷを把持する為の命令（ハンド閉）が入る。
その行番号をＮCLで代表させる。ＮB とＮG2の間には、
通信インタフェイス２７を介してエフェクタ制御部５０
に位置Ｂへの到着を知らせてエフェクタＥを作動させ、
エフェクタによる作業（ピン差込み）の終了を受信・確
認する為の幾つかの命令が入る。これらを行番号ＮEFで
代表させる。また、ＮG2とＮG0の間には、ワークＷを解
放する為の命令（ハンド開）を挿入し、これを行番号Ｎ
OPで代表させる。

【００６２】更に、行番号ＮA とＮB の間には、作用の
説明の欄で述べた手順に従って、センサを起動して新た
に把持されたワークＷの位置・姿勢を計測し、その結果
に基づいてツール座標系を設定する為のプログラム（サ
ブプログラム）を実行する為に必要な命令が挿入され
る。その行番号をＮSBで代表させる。

【００６３】これらの命令を含めた行番号の大小関係
は、ＮG1＜ＮCL＜ＮA ＜ＮSB＜ＮB ＜ＮEF＜ＮG2＜ＮOP
＜ＮG0となる。その他、必要な命令がしかるべき行番号
部分に挿入されて、制御プログラム全体が完成される。

【００６４】また、各教示点位置における教示に際して
は、以上説明したデータの他に、コンフィグレーション
データとして手首回転数等を規定するデータＣ並びに冗
長度規定データとして肘角度を表すデータＲを教示する
ものとする。このデータＣ、Ｒの教示は、マニピュレー
タに実際の作業と同等の動きをさせて教示を行うことを
前提にして、マニピュレータの各位置到達時点における
Ｃ、Ｒのデータをそのまま教示する形で実行することが
出来る。

【００６５】結局、各教示位置Ｇ1 、Ａ、Ｂ、Ｇ2 、Ｇ
0 について、前述したツールポイントに関するデータ
［ＴM ］、［ＴH ］の他に、これらデータＣ、Ｒを含む
データセットＤG1、ＤA 、ＤB 、ＤG2、ＤG0が作成され
ることになる。各データセットの構成は、次のようにな
る。 ＤG1＝｛［ＴM ］G1、［ＴH ］bef.sensg.、ＣG1、ＲG
1、ΛG1｝ ＤA ＝｛［ＴM ］A 、［ＴH ］bef.sensg.、ＣA 、ＲA
、ΛA ｝ ＤB ＝｛［ＴM ］B 、［ＴH ］aft.sensg.、ＣB 、ＲB
、ΛB ｝ ＤG2＝｛［ＴM ］G2、［ＴH ］aft.sensg.、ＣG2、ＲG
2、ΛG2｝ ＤG0＝｛［ＴM ］G0、［ＴH ］aft.sensg.、ＣG0、ＲG
0、ΛG0｝ ここで、添字G1、A 、B 、G2、G0は各位置Ｇ1 、Ａ、
Ｂ、Ｇ2 、Ｇ0 についてのデータであることを表し、添
字bef.sensg.、aft.sensg.は、各々センサによる計測
前、計測後のデータを表す。Λは、各位置における他の
教示データ、例えば、移動速度、動作形式、位置決め形
式等を指定するデータである。

【００６６】尚、ＤG0については、精密な位置・姿勢制
御は特に必要がないことを考慮して、センサデータ［Ｔ
H ］aft.sensg.を用いずに、［ＴH ］bef.sensg.をその
まま使って、ＤG0＝｛［ＴM ］G0、［ＴH ］bef.sens
g.、ＣG0、ＲG0、ΛG0｝とすることも出来る。

【００６７】このようにして作成された制御プログラム
（補正動作の為のサブプログラムを含む）を再生し、実
際の作業を実行する場合の各ＣＰＵ１１、２１の処理手
順を、図６及び図７に示したフローチャートを参照して
説明する。尚、各フローチャート及び関連説明は、上述
のＮG1、ＮCL、ＮA 、ＮSB、ＮB 、ＮEF、ＮG2、ＮOP、
ＮG0に関連した命令の為の処理のみについて記載し、他
の処理については説明の便宜上記載をほぼ省略した。

【００６８】教示操作盤２６からの再生動作開始指令を
受けてマニピュレータのＣＰＵ２１は自動運転モードの
再生動作を開始させると共に（マニピュレータ側スター
ト）、センサの画像処理装置１０に再生動作開始を知ら
せる。センサ側では、これを受けて直ちにマニピュレー
タコントローラ２０からの信号の受信待ちの状態に入る
（ステップＳＲ１）。

【００６９】ＣＰＵ２１は、教示されたプログラムを１
行づつ読み出し、順次命令を実行することを開始する。
先ず、行番号ＮG1以前に実行すべき命令があれば、それ
を順次実行した後（以後、着目行番号間の命令について
は同様の扱いとし、このような文言を省く。）、行番号
ＮG1の命令を読出し（ステップＭＲ１）、教示されたＧ
1 位置へ移動する（ステップＭＲ２）。行番号ＮCLが読
み出されたら（ステップＭＲ３）、ハンドを閉じてワー
クＷ’を把持する（ステップＭＲ４）。

【００７０】次いで、行番号ＮA が読み出されると（ス
テップＭＲ５）、マニピュレータを教示されたＡ位置へ
移動させる（ステップＭＲ６）。センサ視野に入る位置
Ａに到着後、行番号ＮSBが読み出されると（ステップＭ
Ｒ７）、ＣＰＵ２１は補正用サブプログラムに従った処
理（ステップＭＲ８）を開始させる（図７、マニピュレ
ータ側スタート）。

【００７１】図７を参照して、補正用サブプログラムの
処理及び対応するセンサ側の処理（ステップＳＲ２）を
説明する。先ず、マニピュレータ側からセンサ起動指令
をセンサ側に送信し（ステップＭＱ１）、既にステップ
ＭＲ１で受信待ち状態にあるセンサコントローラ１０で
これを受信する（ステップＳＱ１）。ＣＰＵ１１は、前
述の教示段階と全く同じ処理手順（図５、ステップＳＴ
３の説明参照。）に従ってワークＷ’の計測処理を実行
する（ステップＳＱ２）。

【００７２】計測処理が終了したら、マニピュレータ側
に計測処理終了を送信する（ステップＳＱ３）。ステッ
プＭＱ２で受信待ち状態に入っているマニピュレータコ
ントローラ１０は、これを受信し（ステップＭＱ３）、
その応答として計測データの送信をセンサに指令・送信
する（ステップＭＱ４）。画像処理装置１０は、これを
受信し（ステップＳＱ４）、応答としてＷ’に関する計
測データをマニピュレータコントローラ２０に送信する
（ステップＳＱ５）。１作業サイクルに対応したセンサ
側の処理は、これですべて終了する（図６、図７、セン
サ側エンド）。

【００７３】マニピュレータ側では、ステップＭＱ５で
ワークＷ’の計測データを受信すると、ＣＰＵ２１は、
ステップＭＱ５で受信した新規なセンサデータに基づい
て、前述の（２）式を計算して［ＴH ］’を求め、不揮
発性メモリ２４に格納して（ステップＳＱ６）ステップ
ＭＲ８のサブプログラム処理を終了する（サブプログラ
ムエンド）。尚、（２）式を計算する際の［Ｆ］のデー
タ［Ｆ］B は、教示時のデータＤB に含まれているか
ら、それを用いれば良い。

【００７４】ＣＰＵ２１は、ステップＭＲ８が終了する
と、位置Ａから位置Ｂでなく、補正されたＢ位置（Ｂ’
とする。）へ移動する為の処理を行う。先ず、不揮発性
メモリ２４から行番号ＮB に対応するデータセットＤB
を読出してコピーし、複製物として得られたデータセッ
トＤB 中の［ＴH ］を上記補正用サブプログラム処理中
のステップＭＱ６で得られた［ＴH ］’に書き換えて、
更新されたデータセットＤB ’を作成する（ステップＭ
Ｒ９）。作用の説明の欄で詳しく述べたように、このツ
ール座標系データ［ＴH ］の［ＴH ］’への置換以外に
は、データの書換えは行われない。即ち、他のデータ、
［ＴM ］B 、ＲB 、ＣB 、ΛB は、ＤB’の中で教示値
通りのまま保存される。

【００７５】データセットＤB ’が作成されたら、位置
Ｂ’に関して前記式（５）に相当する計算を行う（ステ
ップＭＲ１０）。これを表記すれば、 ［Ｆ］B ’＝［ＴM ］B ・inv.［ＴH ］’ ・・・（８） となる。（８）式の右辺のデータはすべてＤB ’の中に
含まれている。

【００７６】この（８）式を計算して得られる［Ｆ］B
’が実現されるように、マニピュレータを移動させれ
ば、補正されたＢ位置（Ｂ’とする。）、即ち、エフェ
クタＥによる角ピン差込み処理が正しく行われる為のワ
ークＷ’の位置・姿勢）が実現されることになる。そこ
で、現在位置Ａ（［Ｆ］＝［Ｆ］A ）、目標位置Ｂ’
（［Ｆ］＝［Ｆ］B ’）間を適宜数の補間区間に分けて
補間計算を行い、各補間点について、順次前記式（７）
の計算を行うことによって、順次θの指令値を定めて、
位置Ｂ’へ移動する（ステップＭＲ１１）。

【００７７】即ち、補間点数をＢ’を含めてｑ個とする
と、ｉ番目の補間点に対する各軸値θ[i] を求める式
は、次のようになる。（i=0,1,2,3....q、θ[0] ＝θA
、θ[q] ＝θB ’とする。） θ[i] ＝Γ［Ｆ[i] 、Ｃ[i] 、Ｒ[i] 、θ[i-1] ］ ・・・（９） ここで、Ｆ[i] 、Ｃ[i] 、Ｒ[i] は、各々ｉ番目の補間
点に対応するＦ、Ｃ、Ｒの値を表し、θ[i-1] は、直前
各軸値として採用されるｉ―１番目の補間点の各軸値を
表す。従って、ｉ＝１から順を追って式（９）を計算す
れば、Ｂ’を含めて、全補間点の各軸値が求めることが
出来る。

【００７８】Ａ位置から、補正されたＢ位置である位置
Ｂ’への移動が終了したら、エフェクタＥによるピン差
込み作業を実行する為の処理を実行する（ステップＭＲ
１２）。この処理は、エフェクタ制御部５０へのＢ’位
置到着報知、エフェクタ作動、エフクタ制御部５０から
マニピュレータコントローラ２０への差込み作業完了報
知等を含んでいる。

【００７９】ピン差込み作業の終了後、位置Ｂ’から補
正されたＧ2 位置であるＧ2 ’への移動の為の処理を行
うが、ステップＭＲ９〜ステップＭＲ１１の処理に準じ
て実行される。即ち、行番号ＮG2に対応したデータセッ
トＤG2を読出して、コピーを作成してから、［ＴH ］を
センサデータによる［ＴH ］’に書き換えたＤG2’を作
成する（ステップＳ１３）。他のデータ、［ＴM ］B 、
ＲB 、ＣB 、ΛB は、ＤB ’の中で教示値通りのまま保
存される。

【００８０】次いで、ＤG2’に基づいて、位置Ｇ2 に関
して前記式（５）に相当する計算を行い、［Ｆ］B ’を
求める（ステップＭＲ１４）。これを表記すれば、 ［Ｆ］B ’＝［ＴM ］B ・inv.［ＴH ］’ ・・・（９） となる。（８）式の右辺のデータはすべてＤB ’の中に
含まれている。

【００８１】この（９）式を計算して得られる［Ｆ］G
2’を用いて現在位置Ａ（［Ｆ］＝［Ｆ］B'）、目標位
置Ｇ2 （［Ｆ］＝［Ｆ］G2）間を適宜数の補間区間に分
けて補間計算を行い、各補間点について、順次前記式
（７）の計算を行うことによって、順次θの指令値を定
めて、位置Ｇ2 ’へ移動する（ステップＭＲ１５）。各
補間点の各軸値が求め方は、位置Ａから位置Ｂ’への移
動の際と同様なので、繰り返して述べることを省略す
る。

【００８２】Ｇ2 ’に到着したら、行番号ＮOPの命令を
読み出し（ステップＭＲ１６）、ハンドを開いてワーク
Ｗ’を解放して搬出コンベア上に載置する（ステップＭ
Ｒ１７）。ここでも、ワークＷ’の位置と姿勢が教示時
と同一となるようにマニピュレータの位置と姿勢が補正
されている状態でハンドが開かれるから、コンベアへの
載置に際して、ワークが転がる等のトラブルが防止され
る。

【００８３】ワークＷ’の解放後、行番号ＮG0に対応し
たＤG0を読出し（ステップＭＲ１８）、Ｇ2 ’からＧ0
位置に至る軌道を計算し（ステップＭＲ１９）、Ｇ0 へ
移動して（ステップＭＲ２０）、１作業サイクルの再生
動作の処理を終了する（マニピュレータ側エンド）。

【００８４】２個目以降のワークＷ’、Ｗ”・・・を把
持して、作業を繰り返し実行する場合には、各作業サイ
クルにおいて、センサ計測位置（Ａ位置）までは前回の
再生過程で使用されたツール座標系データを利用し、Ａ
位置でツール座標系表現データを［ＴH ］”以下に毎回
書き換えて、上記説明した処理を繰り返して実行すれば
良い。

【００８５】尚、上記説明では、センサ計測位置Ａから
補正されたＢ位置Ｂ’へ向かう軌道をとらせたが、これ
に代えて、センサ計測位置Ａから補正前の位置Ｂへ一旦
移動した後で、Ｂ’へ移動する軌道を選択しても差し支
えないことは言うまでもない。

【００８６】また、図３の作業例において厳密な位置・
姿勢の制御が要求されるのはエフェクタによるピン差込
み作業を行うＢ位置であることを考慮して、Ｂ位置でセ
ンサによる計測を実行しても良い。一般には、再定義さ
れたツール座標系表現データを含むデータのセットＤB
’、ＤG2’等を用意出来る限り、センサ実測位置以降
の任意の教示位置で補正動作を実行することが可能であ
る。センサによる実測位置は、この事を考慮して、補正
の必要な位置を通過する以前の位置で計測に都合が良い
（把持物体を観測し易い等。）場所を選択して設定する
ことが好ましい。

【００８７】また、ワークの位置のみが問題であり、姿
勢について特に要求が無い場合等においては、センサは
ワークの位置のみを実測し、ツール座標系データの実質
的な部分を位置データのみで構成するようにすれば、マ
ニピュレータの位置のみを補正して作業行うケースに本
発明の方法を適用することが可能なことも特に説明を要
しないであろう。

【００８８】更に、本発明において使用されるセンサと
しては、視覚センサが最も一般的であるが、被把持物体
の形状、性質や作業の内容に応じて、視覚センサ以外の
電気的、磁気的あるいは機械的な原理を利用した各種の
センサを用いることも、本発明の原理に照らして全く差
し支えのないことである。

【００８９】

【発明の効果】本発明の効果（長所）を列挙すれば、次
のようになる。

【００９０】（イ）センサデータは教示時と再生時にお
けるツール座標系の定義・設定に使われるのみであるか
ら、ツール座標系の定義・設定が済めばセンサデータを
記憶・格納しておく必要がない。また、教示・再生時に
おけるデータ処理を同形にすることが可能であるから、
センサ側に用意される情報処理・記憶システムを簡素な
ものとすることが出来る。

【００９１】（ロ）作用の説明の欄で詳しく述べたよう
に、実現されるべきマニピュレータの位置・姿勢を規定
する式に、再生時のツール座標系の表現行列の逆行列で
あるinv.［ＴH ］’が、［ＴH ］teach との乗算の形で
入っているから、センサ座標系設定誤差の影響の相当部
分は互いに打ち消し合うことになる。その結果、多少の
センサ座標系設定誤差があったとしても、補正動作の精
度が大きく低下することが無い。この誤差低減作用は、
本発明の方法に固有な、特に顕著な特徴である。

【００９２】（ハ）教示時に作成される教示データは、
［ＴH ］のセンサデータを除いて、再生時にそのまま保
存されているから、マニピュレータの位置・姿勢を補正
しながら、コンフィギュレーションデータＣ、冗長自由
度規定データＲを使用することにより、手首軸回転数、
肘角度等に関して、マニピュレータに教示された通りの
状態を補正後も維持させることが出来る。

【００９３】（ニ）被把持物体のセンサによる計測は、
ツール座標系を定める為に行われるから、教示時と再生
時で実際に計測する特定点は必ずしも同一である必要は
ない。

【００９４】従って、例えば、把持ずれの生じ方が特定
されるような場合には、該ずれをより正確あるいは簡便
に計り得る特定点を再生時の計測点に選んでツール座標
系の再定義を行うことも可能となる。

【図面の簡単な説明】

【図１】本発明の原理を説明する為に、センサ座標系、
作業座標系、ツール座標系及びハンド座標系の関係を概
念的に表した図。

【図２】本発明を実施する際に使用されるマニピュレー
タコントローラ、視覚センサの画像処理装置を含むシス
テムの要部ブロック図。

【図３】本発明の方法を適用する作業の一例を説明する
概略図。

【図４】マニピュレータとセンサに共通のセンサ座標系
を設定するキャリブレーションの手順を説明する為のフ
ローチャート。

【図５】本発明の方法を図３の作業例に適用した場合の
教示段階における処理手順を説明する為のフローチャー
ト。

【図６】本発明の方法を図３の作業例に適用した場合の
再生動作段階における処理手順を説明する為のフローチ
ャート。

【図７】本発明の方法を図３の作業例に適用した場合の
再生動作段階における補正用サブプログラムの処理を説
明する為のフローチャート。

【符号の説明】

ΣS センサ座標系 ΣM 作業座標系 ΣT ツール座標系 ΣH ハンド座標系 Ｗ ワーク Ｔ ツール座標系原点 Ｔ1 、Ｔ2 、Ｔ3 ワーク上特徴点 Ｅ エフェクタ（角形ピン差込み装置） Ｐ 角孔 Ｇ0 マニピュレータ初期位置（ホームポジション） Ｇ1 ワーク把持位置 Ａ センサ計測位置 Ｂ 角形ピン差込み作業位置 Ｇ2 ワーク解放位置 １０ 画像処理装置 ２０ マニピュレータコントローラ ３０ カメラ ４０ マニピュレータ本体 ５０ エフェクタ制御部 １９ バス（画像処理装置） ２８ バス（マニピュレータコントローラ）

───────────────────────────────────────────────────── フロントページの続き (51)Int.Cl.⁵ 識別記号 庁内整理番号 ＦＩ 技術表示箇所 Ｇ０５Ｂ 19/18 Ｋ 9064−3Ｈ 19/403 Ｐ 9064−3Ｈ 19/42 Ｇ 9064−3Ｈ Ｗ 9064−3Ｈ Ｇ０５Ｄ 3/12 Ｋ 9179−3Ｈ Ｔ 9179−3Ｈ

Claims (2)

【特許請求の範囲】
1. 【請求項１】 物体を把持して作業を行うマニピュレー
   タの位置と姿勢の少なくとも一方を、センサ手段の検出
   する前記被把持物体の位置と姿勢の少なくとも一方のデ
   ータに基づいて補正する為に、（ａ）前記センサ手段と
   前記マニピュレータが共有するセンサ座標系を設定する
   段階と、（ｂ）前記マニピュレータに物体を把持した状
   態で該マニピュレータに動作の教示を行うに際して、該
   被把持物体の位置と姿勢の少なくとも一方に関するデー
   タを前記センサ手段を利用して取得し、該センサ取得デ
   ータに基づいて定義されたツール座標系を前記マニピュ
   レータに設定した上で動作教示を行う段階と、（ｃ）前
   記教示内容を再生して前記マニピュレータを動作させる
   に際して、前記定義されたツール座標系のデータを、該
   再生時の被把持物体の位置と姿勢の少なくとも一方に関
   して前記センサ手段によって取得されるデータに書き換
   える形でツール座標系を再定義して、マニピュレータの
   位置と姿勢の少なくとも一方を補正する為のデータを提
   供する段階と、（ｄ）該補正用データと前記教示時に教
   示された他のデータに基づいてマニピュレータの位置及
   び姿勢の少なくとも一方を制御する段階とを含むマニピ
   ュレータの物体把持作業における位置・姿勢補正方法。
2. 【請求項２】 前記段階（ｂ）における教示に際して、
   ツール座標系データ以外に、マニピュレータコンフィギ
   ュレーションデータと冗長度規定データの内の少なくと
   も一方を含む付帯データを合わせて教示し、前記（ｃ）
   のツール座標系再定義の段階時に該付帯データは非置換
   データとし、前記（ｄ）の再生段階時に、前記付帯デー
   タを利用した動作制御を行う請求項１に記載されたマニ
   ピュレータの物体把持作業における位置・姿勢補正方
   法。