JPH0538662A

JPH0538662A - 倣い制御装置

Info

Publication number: JPH0538662A
Application number: JP3193786A
Authority: JP
Inventors: Yasuyuki Nakada; 康之中田
Original assignee: Fujitsu Ltd
Current assignee: Fujitsu Ltd
Priority date: 1991-08-02
Filing date: 1991-08-02
Publication date: 1993-02-19
Anticipated expiration: 2015-08-21
Also published as: JP3078884B2

Abstract

(57)【要約】【目的】本発明は倣い制御装置に関し、未知の形状曲
面を有する対象物に対して、例えば、グラインダ作業、
バリ取り作業、研磨作業等を行う場合においても、正確
にある一定の押圧力で倣い動作を行う倣い制御装置を提
供することを目的としている。【構成】複数部材を互いに揺動自在に支持して関節部
を形成し、所定の制御対象を操作する操作手段、位置制
御手段、力制御手段を有する第１、及び第２のアーム
と、該各アームの有する該各位置制御手段、及び各力制
御手段にそれぞれ発行すべき制御指令を生成する制御指
令生成手段とを備えた倣い制御装置であって、前記２つ
のアームのうち、先行動作する第１のアームにより倣い
対象となる対象物の表面形状を測定し、該測定の結果情
報に基づいて、後に動作する第２のアームによる実作業
を行うように構成する。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、倣い制御装置に係り、
詳しくは、例えば、グラインダ作業、バリ取り作業、研
磨作業等の作業の自動化の分野に用いて好適な、対象物
の表面に沿って制御対象を倣い動作させて加工等を行う
倣い作業のための倣い制御装置に関する。

【０００２】近年、曲面を有する対象物体の表面に沿っ
て制御対象を倣い動作させることにより、例えば、グラ
インダ作業、バリ取り作業、研磨作業等の作業を行う倣
い制御装置が数多く開発されている。しかし、例えば、
図１３に示すように、倣い対象となる対象物の表面形状
が曲面で構成されている場合、アームの先端を一定した
圧力で押し付けて接触を保つ制御は難しい。

【０００３】そこで、アームの先端を対象物に対して予
め設定された圧力で正確に押圧する倣い制御装置が要求
される。

【０００４】

【従来の技術】従来のこの種の倣い制御装置としては、
例えば、図１１に示すようなブロック構成のものがあ
る。この倣い制御装置は、大別して、位置制御手段１、
力制御手段２、制御指令生成手段である制御指令生成部
３から構成され、位置制御手段１は位置検出部４、及び
位置制御部５から、力制御手段２は力検出部６、及び力
制御部７から構成されている。なお、８は操作部、９は
スカラ型ロボットのアームである。

【０００５】スカラ型ロボットの制御装置は、倣い作業
等を容易に行うために、位置と力とのハイブリッド制御
装置を備えている。位置と力とのハイブリッド制御は、
図１２に示すように、位置制御部５と力制御部７とによ
る制御であり、位置制御部５は、転置直交変換行列（Ｒ
^T）演算部１０、選択行列（Ｉ−Ｓ_f）演算部１１、直
交変換行列（Ｒ）演算部１２、位置フィードバックゲイ
ン（Ｃ_p）１３から構成され、力制御部７は、転置直交
行列（Ｒ^T）演算部１４、選択行列（Ｓ_f）演算部１
５、直交変換行列（Ｒ）演算部１６、力フィードバック
ゲイン（Ｃ_f）１７から構成されている。

【０００６】以上の構成において、まず、位置検出部４
によって制御対象であるアーム９の位置や姿勢が検出さ
れ、アーム９の位置や姿勢の情報が位置制御部５に出力
される。位置制御部５では、位置検出部４からの情報に
基づいて各演算部（転置直交変換行列演算部１０、選択
行列演算部１１、直交変換行列演算部１２、位置フィー
ドバックゲイン１３）により、アーム９の位置制御が行
われる。

【０００７】また、同様に、力検出部６によって制御対
象であるアーム９が受ける力も検出されるとともに、検
出した力の情報が力制御部７に出力され、力検出部６か
らの情報に基づいて各演算部（転置直交行列演算部１
４、選択行列演算部１５、直交変換行列演算部１６、力
フィードバックゲイン１７）により、アーム９の力制御
が行われる。

【０００８】すなわち、位置制御部５によってアーム９
の先端が対象物Ｍに接触するまでのロボットの位置移動
や、接触した後の力の制御方向、及び制御方向に直交す
る方向への位置移動を行うための操作指令が操作部８に
送られて、ロボットが制御され、力制御部７によって、
制御対象であるアームの先端に取り付けられた力検出部
６（６軸力覚センサ）で検出されたアームの先端と対象
物Ｍとの接触力Ｆを、所定の設定力にするように操作部
８へ操作指令が送られ、アーム９が制御される。

【０００９】ここで、力制御ロボットで倣い作業をする
場合、図１３に示すように、アーム９の先端と対象物Ｍ
との接触点Ｐ₁における対象物表面の法線方向と一致す
る、押しつけ方向ベクトルｎと、倣いをしながら移動す
る移動方向ベクトルｏで決定される倣い座標系（Ｏ_W−
Ｘ_WＹ_WＺ_W）との各情報を制御指令生成部３に与える
必要があり、予め対象物Ｍの表面状態の情報がない場
合、アーム９を対象物Ｍに接触させ、アーム９の先端位
置、及び対象物Ｍから受ける反力から実時間で倣い座標
系を作成していた。

【００１０】

【発明が解決しようとする課題】しかしながら、このよ
うな従来の倣い制御装置にあっては、アーム９の先端位
置、及び対象物Ｍから受ける反力から倣い座標系を作成
するという構成となっていたため、以下に述べるような
問題点があった。すなわち、倣い作業の例として、例え
ば、グラインダ作業、バリ取り作業、研磨作業等の作業
のように、手先効果器の刃先が対象物Ｍにくい込むよう
なことのある作業を行う場合、手先効果器の刃先が対象
物Ｍにくい込むときに生じる切削力や、抵抗等のため
に、アーム９と対象物Ｍに生じる接触力を正確に得るこ
とが困難である場合があり、このため、アーム９の先端
を対象物Ｍに対して予め設定された圧力で正確に押圧す
るという倣い動作精度が悪化することによって加工精度
が悪化するという問題点があった。

【００１１】［目的］そこで本発明は、未知の形状曲面
を有する対象物に対して、例えば、グラインダ作業、バ
リ取り作業、研磨作業等を行う場合においても、正確に
ある一定の押圧力で倣い動作を行う倣い制御装置を提供
することを目的としている。

【００１２】

【課題を解決するための手段】本発明による倣い制御装
置は上記目的達成のため、複数部材を互いに揺動自在に
支持して関節部を形成し、所定の制御対象を操作する操
作手段、並びに該制御対象の位置や姿勢を検出し、検出
した位置情報に基づいて該制御対象の位置を制御する位
置制御手段、及び該制御対象が受ける力を検出し、検出
した力情報に基づいて該制御対象によって外部に及ぼす
力を制御する力制御手段を有する第１、及び第２のアー
ムと、該各アームの有する該各位置制御手段、及び各力
制御手段にそれぞれ発行すべき制御指令を生成する制御
指令生成手段とを備えた倣い制御装置であって、前記２
つのアームのうち、先行動作する第１のアームにより倣
い対象となる対象物の表面形状を測定し、該測定の結果
情報に基づいて、後に動作する第２のアームによる実作
業を行うように構成している。

【００１３】この場合、前記第１のアームと前記対象物
との接触点、及び該接触点に対応する作業座標系を記憶
する倣い座標系記録部を設け、該第１のアームから前記
第２のアームに結果情報を受け渡す際、該第２のアーム
が該第１のアームの通過した位置に達した時点で、前記
倣い座標系記録部に記憶された情報を該第１のアームか
ら前記第２のアームに転送するように構成することは有
効である。

【００１４】

【作用】本発明では、位置と力との制御が行われる２つ
のアームのうち、先行動作する第１のアームにより倣い
対象となる対象物の表面形状が測定され、該測定の結果
情報に基づいて、後に動作する第２のアームによる実作
業が行われる。すなわち、未知の形状曲面を有する対象
物に対して、例えば、グラインダ作業、バリ取り作業、
研磨作業等を行う場合においても、対象物に対して予め
設定された圧力で正確にアームの先端が押圧され、一定
した押圧力で倣い動作がなされる。

【００１５】

【実施例】以下、本発明を図面に基づいて説明する。図
１〜８は本発明に係る倣い制御装置の一実施例を示す図
である。図１は本実施例の装置外観を示す図であり、図
２は本実施例の全体構成を示すブロック図である。

【００１６】まず、構成を説明する。なお、図１，２に
おいて、図１１，１３に示した従来例に付された番号と
同一番号は同一部分を示す。本実施例の倣い制御装置
は、２台のロボット、すなわち、先行動作する形状測定
用のアーム９ａ（第１のアーム）と、本動作用のアーム
９ｂ（第２のアーム）との２本のアーム９を有し、アー
ム９ａを対象物Ｍの表面形状を測定するために使用する
とともに、アーム９ｂを実作業を行うために使用し、各
アーム９ａ，９ｂを、表面形状を検出するものと、実作
業を行うものとに分離することによって、作業精度の向
上を図るものである。

【００１７】なお、図１中、１８はプローブ、１９は手
先効果器であり、図２中、２０は法線ベクトル算出部、
２１は移動方向ベクトル算出部、２２は倣い座標系記憶
部である。図３〜５は図２の詳細を示すブロック図であ
る。図３〜５に示すように、位置検出部４ａ，４ｂは、
それぞれエンコーダ＆カウンタ２３ａ，２３ｂ、タコメ
ータ２４ａ，２４ｂから構成され、力検出部６ａ，６ｂ
は、それぞれ力覚センサ２５ａ，２５ｂ、及び力覚セン
サ２５ａ，２５ｂで検出した力をプローブ１８、または
手先効果器１９の座標系からロボットの基準座標系へ変
換する座標変換部２６ａ，２６ｂから構成されている。

【００１８】操作部８ａ，８ｂは、それぞれサーボモー
タ２７ａ，２７ｂ、パワーアンプ２８ａ，２８ｂ、Ｄ／
Ａコンバータ２９ａ，２９ｂ、補償器３０ａ，３０ｂか
ら構成されている。ちなみに、力制御部７ａ，７ｂは力
制御動作時に、力検出部６ａ，６ｂにより検出された力
Ｆ₀、設定力（力指令Ｆ_r）、及び力制御パラメータに
基づいて力制御方向の速度指令信号Ｖ_fを発するもので
あり、位置制御部５ａ，５ｂは位置検出部４ａ，４ｂで
検出された位置Ｘ₀、設定位置（位置指令Ｘ_r）、及び
位置制御パラメータに基づいて速度指令信号Ｖ_pを発す
るものである。

【００１９】なお、３１ａ，３１ｂは加算部であり、位
置制御部５ａ，５ｂ、及び力制御部７ａ，７ｂから出力
された速度についての加算を行うものである。３２ａ，
３２ｂは逆ヤコビ変換部であり、加算部３１ａ，３１ｂ
で加算された速度をアーム９ａ，９ｂの各関節の角速度
に変換するものである。座標変換部３３ａ，３３ｂで
は、位置検出部４ａ，４ｂで検出されたアーム９ａ，９
ｂの関節角θ_sを基準座標系での位置Ｘ₀に変換するも
のである。

【００２０】制御指令生成部３では、アーム９ａ、９ｂ
の位置、及びロボット先端と対象物Ｍとの間で生じた接
触力を検出し、位置指令Ｘ_r、及び位置制御パラメータの転送、力指令Ｆ_r、及び力制御パラメータの転送、現在位置Ｘ₀の受信、現在時刻の接触力Ｆ₀の受信、を行うものであり、オペレータが与えた作業シーケン
ス、現在の位置、接触力にしたがって、位置指令Ｘ_r、
力指令Ｆ_r、制御パラメータを位置制御部５ａ，５ｂ、
及び力制御部７ａ，７ｂへそれぞれ出力するものであ
る。

【００２１】位置制御部５ａ，５ｂの具体的構成は、図
１２の従来例に示すように、転置直交変換行列（Ｒ^T）
演算部１０、選択行列（Ｉ−Ｓ_f）演算部１１、直交行
列（Ｒ）演算部１２、位置フィードバックゲイン
（Ｃ_p）演算部１３とからなり、一方、力制御部７ａ，
７ｂは転置直交変換行列（Ｒ^T）演算部１４と、選択行
列（Ｓ_f）演算部１５、直交行列（Ｒ）演算部１６、力
フィードバックゲイン（Ｃ_r）演算部１７とから構成さ
れている。なお、符号部分は偏差部である。

【００２２】次に作用を説明する。ベクトルｎ、ｏ、ａ
を用いてロボット基準座標系（Ｘ_o、Ｙ_O、Ｚ_o）から
倣い座標系（Ｘ_wＹ_wＺ_w）への座標変換する直交座標
変換行列Ｒは、次のように表される。図１に示すように、倣い座標系（Ｘ_wＹ_wＺ_w）のＸ
_w方向を力制御方向、Ｙ _w、Ｚ_w方向を位置制御方向と
すると、選択行列演算部１１，１５の選択行列Ｓ_fは、で与えられる。

【００２３】力フィードバックゲインＣ_rは、基準座標
系に関して、で与えれる。

【００２４】また、位置フィードバックゲインＣ_pは同
様にして、で与えられる。

【００２５】図１に示すように、形状測定用のアーム９
ａは、プローブ１８と対象物Ｍとの接触点Ｏ_Wで倣い座
標系を設定しながら対象物Ｍの表面を倣うものであり、
本動作用のアーム９ｂは、手先効果器１９に取り付けら
れた各種の作業用具によって対象物Ｍの表面に対して、
例えば、グラインダ作業、バリ取り作業、研磨作業等を
行うものである。

【００２６】ここで、倣い座標系Ｏ_W−Ｘ_WＹ_WＺ
_Wは、図１に示すように、対象物Ｍに対するアーム９の
先端部の位置・姿勢により決定される座標であり、ベク
トルｎ_A，ｏ_A，ａ_Aはそれぞれ倣い座標系の座標軸Ｘ
_W，Ｙ_W，Ｚ_Wについての単位ベクトルである。すなわ
ち、ベクトルｎ_Aは対象物Ｍへ力を加える時の押し付け
方向を示し、対象物Ｍの表面の法線ベクトルと同じ方向
であり、ベクトルｏ_Aはベクトルｎ_Aと直交関係にあ
り、倣い動作時のアーム９の先端の移動方向を示す。

【００２７】ベクトルａ_Aは、ベクトルｎ_A，ｏ_Aに直
交するように定められており、ａ_A＝ｎ_A×ｏ_A で与えられる。ベクトルｎ_A，ｏ_A，ａ_Aの基準座標系
Ｘ_OＹ_OＺ_Oについての成分表示を行うと、ｎ_A＝（ｎ_xｎ_yｎ_z）^T ｏ_A＝（ｏ_xｏ_yｏ_z）^T となる。ただし、Ｔは転置行列を示す。

【００２８】なお、倣い座標Ｏ_W−Ｘ_WＹ_WＺ_W、及び
単位ベクトルｎ_B，ｏ_B，ａ_Bは、実作業を行うアーム
９ｂについても同様に定められる。次に、曲面を有する
対象物Ｍと倣い座標との関係を図１３に基づいて説明す
る。ロボットは、対象物Ｍのある面上で倣い動作を行っ
ており、このとき、倣い座標系は図１の位置Ｐ₁のよう
に与えられているものとし、ベクトルｎは面に垂直であ
り、ベクトルｏ，ａは接平面上にあるものとする。

【００２９】ロボットが倣い動作を行ってベクトルｏの
方向へ移動すると、位置Ｐ₂に達した点で、ロボットと
対象物Ｍの接点での面の法線方向が位置Ｐ₁での押しつ
け方向ｎと一致しなくなる。面の法線方向と押しつけ方
向ｎが一致しないと、対象物Ｍに対して発生する押しつ
け力が設定した値からずれるため、正確な力を発生する
ためには、位置Ｐ₁の倣い座標系を位置Ｐ₂の倣い座標
系に切り換える必要がある。

【００３０】こうした倣い座標系を変更するか否かを判
断するための条件として、（１）一定時間経過したと
き、（２）対象物Ｍから受ける反力が設定力からある程
度以上ずれたとき、（３）ロボットが一定距離を移動し
たとき、等があげられるが、以下の説明では、（１）の
条件にしたがって、倣い座標を変更するものとする。こ
の場合、上記（２），または（３）の条件でも、基本的
な原理は（１）と同一である。

【００３１】本実施例では、形状測定用のアーム９ａで
測定した対象物Ｍり表面データを蓄え、この蓄えられた
データを、実作業を行うアーム９ｂが参照しながら倣い
作業を実行するように全体を制御する。なお、図９は設
定力算出方向を説明するための図であり、同図（ａ）は
手先効果器１９が対象物Ｍから受ける反力Ｆと、力覚セ
ンサ座標系Ｏ_S−Ｘ_SＹ_SＺ_Sの関係を示した図、同図
（ｂ）は目標力Ｆr を示す図である。

【００３２】図１０は形状測定用のアーム９ａで倣い動
作を行う際の倣い座標系とロボットの軌道の関係を示し
たものであり、各点における倣い座標系を説明するため
の図である。図６〜８は本実施例の動作例を示すフロー
チャートであり、以下、２台のロボットで曲面を有する
対象物Ｍへ倣い作業を行うための制御方法を説明する。〔１〕アームの倣い座標系の算出（図６参照）図１０に示す、始点Ｐ_Bでの倣い座標系の算出方法につ
いて説明する。

【００３３】（１）始点での法線ベクトルの算出始点Ｐ_Bでの法線ベクトルｎ_Aは以下に示すようにして
求められる。まず、力覚センサ６ａ，６ｂでは、反力Ｆ
のＸ_S、Ｙ_S、Ｚ_S方向のそれぞれの分力ｆ_X，ｆ_Y，
ｆ_Zが検出され（以下、力覚センサの検出するトルク成
分についての説明は省略する）、反力_SＦがベクトル表
示されると（Ｓは力覚センサ座標系Ｏ_S−Ｘ_SＹ_SＺ_S
で記述されていることを示す）、^S Ｆ＝（ｆ_X ｆ_Y ｆ_Z）^T となる。

【００３４】力覚センサ座標系で表した単位法線ベクト
ル^Sｎ_Aは、^SＦと逆向きのベクトルであり、成分表示
すると、

【００３５】

【数１】

【００３６】である。次に、法線ベクトル^Sｎ_Aを基準
座標系で表してみる。力覚センサ座標系の各座標軸
Ｘ_S，Ｙ_S，Ｚ_Sの基準座標軸系に対する単位ベクトル
を^Oｎ_S，^Oｏ_S，^Oａ_Sとし（Ｏは基準座標系で記述
されていることを示す）、成分表示すると、^O ｎ_s＝（ｎ_Sx ｎ_Sy ｎ_Sz）^T ^O ｏ_S＝（ｏ_Sx ｏ_Sy ｏ_Sz）^T ^O ａ_S＝（ａ_Sx ａ_Sy ａ_Sz）^T となる。

【００３７】このとき、力覚センサ座標系から基準座標
系への座標変換行列^OＡ_Sは、^O Ａ_S＝（^Oｎ_s ^Oｏ_S ^Oａ_S）で与えられ、基準座標系で表した法線ベクトル^Oｎ_Aは
座標変換行列^OＡ_Sを用いて、^O ｎ_A＝^OＡ_S ^Sｎ_A となる。

【００３８】法線ベクトルは、図２に示す法線ベクトル
算出部２０で算出され、始点以外の他の接触点での法線
ベクトルの算出も同様の操作で行われる。（２）始点での移動方向ベクトルの算出次に、オペレータが与えた移動方向ベクトルｏ_OPと、求
めた法線ベクトル^Oｎ _Aを用いて、始点Ｐ_Bにおける移
動方向ベクトルｏ_Aを算出する。ただし、ベクトルｏ_OP
とベクトル^Oｎ_Aは、ｏ_OP＝^Oｎ_A、またはｏ_OP＝−^Oｎ_A を満たさないものとする。

【００３９】また、移動方向ベクトルｏ_Aは、法線ベク
トル^Oｎ_Aに直交し、移動方向ベクトルｏ_OPと法線ベク
トル^Oｎ_Aのなす平面上にあるベクトルであり、このと
き、倣い座標系の座標軸を表す単位ベクトルの１つであ
るａ_Aは、^Oｎ_Aとｏ_OPを用いて、^O ａ_A＝（^Oｎ_A×ｏ_OP）／｜^Oｎ_A×ｏ_OP｜で表される。

【００４０】移動方向ベクトル^Oｏ_Aは、ベクトル^Oｎ
_A、^Oａ_Aとの直交関係により、^O ｏ_A＝^Oａ_A×^Oｎ_A で求められる。移動方向ベクトルは、図２に示す移動方
向ベクトル算出部２１で算出される。（３）点Ｐ_iでの法線ベクトルの算出次に、図１０の点Ｐ₁での倣い座標系の算出方法につい
て説明する。

【００４１】点Ｐ_iでの法線ベクトルｎ_iの算出方法
は、始点Ｐ_Bでの算出方法と同様であり、次式で表され
る。

【００４２】

【数２】

【００４３】^Oｎ_i＝^OＡ_Si ^Sｎ_i ただし、点Ｐ_iでの反力^SＦ_iを、^S Ｆ_i＝（ｆ_Xi ｆ_Yi ｆ_Zi）^T とする。^OＡ_Siは、点Ｐ_iの力覚センサ座標系から基準
座標系への座標変換行列である。

【００４４】（４）点Ｐ_iでの移動方向ベクトルの算出ロボットの先端位置は、ベクトルａ方向にも位置制御さ
れているので、先端位置が常にベクトルｎ、ｏがなす平
面上にあるように制御される。したがって、対象物Ｍの
表面に描く軌跡は、始点で与えられたベクトルｎ_A，ｏ
_Aのなす平面上の曲線となり、このことから点Ｐ_iでの
移動方向ベクトルｏ_iは、始点で求められたベクトルａ
_Aに垂直に与えられればよいことがわかる。

【００４５】また、移動方向ベクトルｏ_iは、法線ベク
トルｎ_iにも直交するので、ベクトル^Oｏ_iは、^O ｏ_i＝（^Oａ_A×^Oｎ_i）／｜^Oａ_A×^Oｎ_i｜のように表せる。（ただし、上式は^Oａ_B＝^Oｎ_i、ま
たは^Oａ_B＝−^Oｎ_iのときには成立しないが、このと
きは、点Ｐ_iの移動方向ベクトルにオペレータが与えた
移動方向ベクトルｏ_OPを用いると進行方向が維持でき
る。）倣い座標系の座標軸を表す単位ベクトルの１つで
あるａ_iは、ベクトル^Oｎ_i，^Oｏ_iとの直交関係によ
り、^O ａ_i＝^Oｎ_i×^Oｏ_i で求められる。

【００４６】以上のように、対象物Ｍ体上の点Ｐ_iに対
応して、倣い座標系^Oｎ_i，^Oｏ_i，^Oａ_iがほぼ実時
間で算出される。アーム９ａでは、算出した倣い座標系
にしたがって倣い動作が行われる。また、算出した倣い
座標系はアーム９ｂの作業時にも使用されるため、図２
に示したように、倣い座標系記憶部２２に記憶される。〔２〕アーム９ａの倣い動作の制御（図７参照）図１０に示す、点Ｐ_iでの倣い動作の制御方法について
説明する。

【００４７】（１）目標力Ｆ_rの発生点Ｐ_iで対象物Ｍに対して設定した力Ｆ_rを発生する時
は、力の大きさがＦ_rで、方向・向きがｎ_iの力を発生
すればよい。すなわち、設定力ベクトル^OＦ_rは、［数
１］で求めた^Oｎ_iを用いて、^O Ｆ_r＝Ｆ_r・^Oｎ_i で与えられる。

【００４８】（２）目標位置の設定点Ｐ_iでのロボットの進行方向は、点Ｐ_iからの相対位
置指令で与えられ、点Ｐ_iからの移動方向は、^Oｏ_A＝
^Oａ_A×^Oｎ_Aの移動方向ベクトル^Oｏ_iで与えられて
おり、^Oｏ_iを用いて相対位置を表すと、^O Ｘ_i＝α・^Oｏ_i となる。ここで、αは前述した倣い座標系の修正のため
の条件（１）〜（３）から決まる適当な定数である。

【００４９】目標力、目標位置の設定は図２に示す制御
指令生成部３で行われる。（３）倣い動作の終了以下に示す条件を満たした時に倣い動作を終了する。オペレータから終了の指令があったとき一定時間を経過したとき衝突したとき、または対象物Ｍから離れたときロボットの可動範囲を越えたとき〔３〕アーム９ｂの倣い作業の制御（図８参照）アーム９ｂの制御は、アーム９ａの倣い動作で求めた倣
い座標系を用いて行われる。

【００５０】すなわち、アーム９ｂの現在位置がＰ_iの
とき、倣い座標系の単位ベクトルは ^Oｎ_i，^Oｏ_i，^O
ａ_iであり、その情報は倣い座標系記憶部２２に格納さ
れている。したがって、アーム９ｂの力制御指令、位置
制御指令の生成は、アーム９ａと同様に行えばよい。

【００５１】（１）目標力Ｆ_rの発生点Ｐ_iで対象物Ｍに対して設定した力Ｆ_rを発生する時
は、力の大きさがＦ_rで、方向・向きがｎ_iの力を発生
すればよい。したがって、設定力ベクトル^OＦ_rは^Oｎ
_iを用いて、^O Ｆ_r＝Ｆ_r・^Oｎ_i で与えられる。

【００５２】（２）目標位置の設定点Ｐ_iでロボットの進行方向は、点Ｐ_iからの相対位置
指令で与えられる。すなわち、点Ｐ_iからの移動方向は
^Oｏ_iで与えられており、^Oｏ_iを用いて相対位置を表
すと、^O Ｘ_i＝α・^Oｏ_i となる。ここで、αは前述した倣い座標系の修正のため
の条件（１）〜（３）から決まる適当な定数である。

【００５３】目標力、目標位置の設定は図２に示す制御
指令生成部３で行われる。このように本実施例では、位
置と力との制御が行われる２つのアームのうち、先行動
作する第１のアームにより倣い対象となる対象物の表面
形状を測定し、この測定結果に基づいて、後に動作する
第２のアームによる実作業を行う。すなわち、未知の形
状曲面を有する対象物に対して、例えば、グラインダ作
業、バリ取り作業、研磨作業等を行う場合においても、
対象物に対して予め設定された圧力で正確にアームの先
端を押圧でき、一定した押圧力で倣い動作を行うことが
できる。

【００５４】したがって、設定された接触力により正確
に追従でき、未知形状の曲面を有する対象物へ対して力
の変動を受けやすい倣い作業を行う場合にも、力の変動
の影響を受けずに、正確な力を発生して作業を行うこと
ができる。

【００５５】

【発明の効果】本発明では、位置と力との制御を行う２
つのアームのうち、先行動作する第１のアームによって
倣い対象となる対象物の表面形状を測定でき、この測定
結果の情報に基づいて第２のアームによって実作業を行
うことができる。したがって、未知の形状曲面を有する
対象物に対して、例えば、グラインダ作業、バリ取り作
業、研磨作業等を行う場合においても、対象物に対して
予め設定された圧力で正確にアームの先端を押圧するこ
とができ、一定した押圧力で倣い動作を行うことができ
る。

【図面の簡単な説明】

【図１】本実施例の装置外観を示す図である。

【図２】本実施例の全体構成を示すブロック図である。

【図３】図２の詳細を示すブロック図である。

【図４】図２の詳細を示すブロック図である。

【図５】図２の詳細を示すブロック図である。

【図６】法線ベクトル算出部の処理を示すフローチャー
トである。

【図７】移動方向ベクトル算出部の処理を示すフローチ
ャートである。

【図８】倣い座標系記憶部の処理を示すフローチャート
である。

【図９】設定力算出方向を説明するための図である。

【図１０】各点における倣い座標系を説明するための図
である。

【図１１】従来例の全体構成を示すブロック図である。

【図１２】位置と力とのハイブリッド制御を説明するた
めのブロック図である。

【図１３】対象物とアームとの位置関係を説明するため
の図である。

【符号の説明】

１位置制御手段２力制御手段３制御指令生成部（制御指令生成手段）４位置検出部５位置制御部６力検出部７力制御部８操作部９アーム（操作手段）９ａアーム（第１のアーム）９ｂアーム（第２のアーム）１０転置直交変換行列演算部１１選択行列演算部１２直交変換行列演算部１３位置フィードバックゲイン１４転置直交行列演算部１５選択行列演算部１６直交変換行列演算部１７力フィードバックゲイン１８プローブ１９手先効果器２０法線ベクトル算出部２１移動方向ベクトル算出部２２倣い座標系記憶部２３ａ，２３ｂエンコーダ＆カウンタ２４ａ，２４ｂタコメータ２５ａ，２５ｂ力覚センサ２６ａ，２６ｂ座標変換部２７ａ，２７ｂサーボモータ２８ａ，２８ｂパワーアンプ２９ａ，２９ｂＤ／Ａコンバータ３０ａ，３０ｂ補償器３１ａ，３１ｂ加算部３２ａ，３２ｂ逆ヤコビ変換部

───────────────────────────────────────────────────── フロントページの続き (51)Int.Cl.⁵ 識別記号庁内整理番号ＦＩ技術表示箇所 // Ｂ２４Ｂ 27/00 Ａ 7908−3Ｃ

Claims

【特許請求の範囲】

【請求項１】複数部材を互いに揺動自在に支持して関節
部を形成し、所定の制御対象を操作する操作手段、並び
に該制御対象の位置や姿勢を検出し、検出した位置情報
に基づいて該制御対象の位置を制御する位置制御手段、
及び該制御対象が受ける力を検出し、検出した力情報に
基づいて該制御対象によって外部に及ぼす力を制御する
力制御手段を有する第１、及び第２のアームと、該各アームの有する該各位置制御手段、及び各力制御手
段にそれぞれ発行すべき制御指令を生成する制御指令生
成手段と、を備えた倣い制御装置であって、前記２つのアームのうち、先行動作する第１のアームに
より倣い対象となる対象物の表面形状を測定し、該測定
の結果情報に基づいて、後に動作する第２のアームによ
る実作業を行うことを特徴とする倣い制御装置。
【請求項２】前記第１のアームと前記対象物との接触
点、及び該接触点に対応する作業座標系を記憶する倣い
座標系記録部を設け、該第１のアームから前記第２のアームに結果情報を受け
渡す際、該第２のアームが該第１のアームの通過した位
置に達した時点で、前記倣い座標系記録部に記憶された
情報を該第１のアームから前記第２のアームに転送する
ことを特徴とする請求項１記載の倣い制御装置。