JP3001211B2 - 任意曲面の倣い制御装置 - Google Patents

Description

【発明の詳細な説明】 〔産業上の利用分野〕 本発明は少なくとも２自由度を持つロボットや工作機
械等の多自由度作業機械を用いた倣い制御装置に係わ
り、特にバリ取り、曲面研磨等の曲面の倣い動作、教
示、形状計測、その他の動作を行うロボットや工作機械
等の多自由度作業機械を位置と力の制御で制御し、任意
曲面を倣わせる倣い制御装置に関する。

〔従来の技術〕

現在の産業用ロボットや自動工作機械のほとんどは、
位置情報をもとに位置の制御で動作している。しかしな
がら、はめ合い作業や研磨作業に代表されるような作業
では力加減を制御する必要が生じてきている。力加減を
制御するには位置と力の両方をうまく制御する必要があ
り、このために種々の研究がなされている。その代表的
なものに、位置制御と力制御を座標軸ごとに切換えて制
御するハイブリット制御や、位置と力の間をバネで関係
ずけるコンプライアンス制御がある。

また近年、「計測自動制御学界論文集」vol 22,No3
（1986）,343〜350頁、特開昭60−3010号及び特開昭61
−7905号に記載のように仮想コンプライアンス制御と呼
ばれているものがある。これば、バネ・マス・ダンパー
を有する力学系モデルを仮想的に実現しようとするもの
で、力学系の式 ｍ＋ｃ＋ｋΔｘ＝ｆ のm,c,kの値を任意に与えて、その動作をソフトウェア
上で模擬して、その動きを実現するように制御を行うも
のである。これを多自由度系で行うために、座標系の各
軸毎にm,c,kの値を変えられるようになっており、この
値を適当に選べば各軸毎に特性を変えることが出来る。
またｋ＝０にすれば、位置のフィードバックがなくなり
力制御モードになり、k,cの値を大きくして力フィード
バックをなくすようにすれば、位置制御モードとなり、
いわゆるハイブリッド制御を行うこともできる。このよ
うに仮想コンプライアンス制御は、ハイブリッド制御と
コンプライアンス制御を包含した制御方式ということが
できる。

〔発明が解決しようとする課題〕

しかしながら、従来の仮想コンプライアンス制御を各
自由度作業機械を用い倣い制御装置に応用した場合に
は、任意曲面を倣わせるには限界があり、適切かつ効率
的な作業ができないという問題があった。

例えば、位置と力の制御によりロボットが行う作業の
１つにワーク表面を倣って表面研磨やバリ取り等を行な
わせる曲面倣い動作がある。この場合、ロボットの作業
用ツールがワーク表面の倣って動くように制御される。
作業用ツールに加わる力及びモーメントは力センサによ
り検出される。この曲面倣い動作を仮想コンプライアン
ス制御を応用して行う場合を考える。ここで、作業用ツ
ールの移動方向をｘ軸方向、作業用ツールの軸線方向を
ｚ軸方向とし、簡単のためｘ−ｚ平面で説明する。例え
ばｘ軸方向にはk,cの値を大きくしてかたくし、作業用
ツールを位置制御によって速度ｖxで送り、ｚ軸方向に
はｋ＝０とし位置の拘束を解除し、作業用ツールを力制
御により目標力ｆrで押し付ける。このとき、力センサ
により検出される力をｆとすると、作業用ツールはｚ軸
方向には ｍｚ＋cvz＝ｆ−ｆr の式を模擬して動いているので、速度変化のない定常状
態ではcvz＝ｆ−ｆrとなり、曲面を倣う速度ｖzを発生
するには、それに応じた力偏差Δｆ＝ｆ−ｆrがなけれ
ばならないことが分かる。ここでｘ方向の送り速度ｖx
を一定とすると、速度ｖzは曲面の勾配に比例する。従
って力偏差Δｆは曲面の勾配に比例した値となり、押付
力−ｆは曲面の勾配によって変化する。今、倣い動作で
許容される力偏差をΔｆoとすれば、定常状態で追従で
きる勾配は となる。

ここで1/cを大きくすれば、追従できる勾配も大きく
なるが、1/cは力に対する速度のゲイン（ｖ＝1/c・ｆ）
になるので、これを大きくするとハンチングを起こして
しまう。そのため1/cは小さくせねばならず、大きな勾
配に対しては送り速度ｖxを小さくして対応するしかな
く、効率的でなく実用性に乏しくなる。

このように、従来の倣い制御装置では任意曲面を倣わ
せるには限界がある。

本発明の目的は、曲面の勾配のいかんに係わらず必要
な送り速度で送ることができ、適切かつ効率的な作業を
行うことのできる多自由度作業機械を用いた倣い制御装
置を提供することにある。

〔課題を解決するための手段および作用〕

上記目的を達成するため、本発明の第１の概念によれ
ば、作業用ツールの押付け力の目標値ｆroを設定する第
１の設定手段、ワーク面から作業用ツールに加わる力を
検出する第１の検出手段、この検出した力と前記作業用
ツールの押付け力の目標値との偏差をとる比較手段、こ
の力の偏差に基づいて作業用ツールの押付け方向の速度
指令値ｕzを演算する制御演算手段、作業用ツールの送
り方向の速度指令値ｕxyを指令する手段、前記押付け方
向及び送り方向の速度指令値ｕz,uxyに基づいて多自由
度作業機械を動かす演算駆動手段を備え、前記第１の設
定手段、第１の検出手段、比較手段、制御演算手段、演
算駆動手段により力制御ループを構成する、少なくとも
２自由度を持つ多自由度作業機械を用いた倣い制御装置
において、前記力制御ループにおける押付け力の目標値
ｆroを補正する力目標値補正手段を更に備え、前記力目
標値補正手段は、前記作業用ツールをワーク面上に前記
力の目標値ｆroで押付けながら前記速度指令値ｕxyで送
るときに、その送り方向に対するワーク面の勾配tan α
により生じる作業用ツールの押付け方向の移動速度ｖzo
fを求める第１の演算手段と、その押付け方向の移動速
度ｖzofにより生じる押付け力の変化量を求め、前記押
付け力の目標値ｆroからこの押付け力の変化量を差し引
き、その値を新たな押付け力の目標値として前記比較手
段に出力する第２の演算手段とを有することを特徴とす
る倣い制御装置が提供される。

また、上記目的を達成するため、本発明の第２の概念
によれば、作業用ツールの押付け力の目標値ｆroを設定
する第１の設定手段、ワーク面から作業用ツールに加わ
る力を検出する第１の検出手段、この検出した力と前記
作業用ツールの押付け力の目標値との偏差をとる比較手
段、この力の偏差に基づいて作業用ツールの押付け方向
の速度指令値ｕzを演算する制御演算手段、作業用ツー
ルの送り方向の速度指令値ｕxyを指令する手段、前記押
付け方向及び送り方向の速度指令値ｕz,uxyに基づいて
多自由度作業機械を動かす演算駆動手段を備え、前記第
１の設定手段、第１の検出手段、比較手段、制御演算手
段、演算駆動手段により力制御ループを構成する、少な
くとも２自由度を持つ多自由度作業機械を用いた倣い制
御装置において、前記力制御ループにおける押付け方向
の速度指令値ｕzを補正する速度指令値補正手段を更に
備え、前記速度指令値補正手段は、前記作業用ツールを
ワーク面上に前記力の目標値ｆroで押付けながら前記速
度指令値ｕxyで送るときに、その送り方向に対するワー
ク面の勾配tan αにより生じる作業用ツールの押付け方
向の移動速度ｖzofを求める第１の演算手段と、前記第
１の演算手段で求めた押付け方向の移動速度ｖzofを前
記制御演算手段で求めた押付け方向の速度指令値ｕzに
加算し、その値を新たな速度指令値として前記駆動手段
に出力する第２の演算手段とを有することを特徴とする
倣い制御装置が提供される。

一般に、本発明の係わる倣い制御装置において、作業
用ツールをワーク面上に力の目標値ｆroで押付けながら
速度指令値ｕxyの基づく送り速度ｖxyで倣わせる場合、
その送り方向に対するワーク面の勾配tan αにより、押
付け方向にｖzof＝ｖxy tan αの速度を生じ、目標値ｆ
roの力とは別に、その速度ｖzofによるｆof＝C v xy ta
n α（Ｃは定数）という押付け方向の力が発生する。

本発明の第１の概念においては、力目標値補正手段の
第１の演算手段により上記作業用ツールの押付け方向の
移動速度ｖzofを求め、第２の演算手段によりその押付
け方向の移動速度ｖzofにより生じる押付け力の変化量
を求め、押付け力の目標値ｆroからこの押付け力の変化
量を差し引き、その値を新たな押付け力の目標値として
比較手段に出力する。このため、作業用ツールの押付け
力は目標値ｆroにほぼ一致し、曲面の勾配のいかんに係
わらず速度指令値ｖxyにしたがった所望の速度で倣うこ
とが可能となる。

また、本発明の第２の概念においては、速度指令値補
正手段の第１の演算手段により上記作業用ツールの押付
け方向の移動速度ｖzofを求め、第２の演算手段により
第１の演算手段で求めた押付け方向の移動速度ｖzofを
制御演算手段で求めた押付け方向の速度指令値ｕzに加
算し、その値を新たな速度指令値として駆動手段に出力
する。このため、作業用ツールの押付け力は目標値ｆro
にほぼ一致し、曲面の勾配のいかんに係わらず速度指令
値ｖxyにしたがった所望の速度で倣うことが可能とな
る。

なお、「作業用ツールの送り方向の速度指令値ｕxyを
指令する手段」は種々の形態をとることができる。例え
ば、ワーク面上を移動する作業用ツールの位置を検出
し、この検出した位置と予め設定した作業用ツールの位
置の目標値との偏差をとり、この偏差に基づいて作業用
ツールの送り方向の速度指令値ｕxyを演算する位置制御
ループでの手段であってもよく、ワーク面上を移動する
作業用ツールの速度を検出し、この検出した速度と予め
設定した作業用ツールの速度の目標値との偏差をとり、
この偏差に基づいて作業用ツールの送り方向の速度指令
値ｕxyを演算する速度制御ループでの手段であってもよ
い。また、これらの場合、「予め設定した作業用ツール
の位置の目標値」、「予め設定した作業用ツールの速度
の目標値」の代わりにそれぞれ教示データとして記憶し
た位置情報から求めた「作業用ツールの位置の目標
値」、「作業用ツールの速度の目標値」を用いてもよ
い。さらに、ジョイスティック、キーボード等、オペレ
ータの操作により直接速度指令値ｕxyを指令できる手段
であってもよい。

前記第１の演算手段は、前記ワーク面の勾配tan αを
演算する手段と、前記速度指令値ｕxyで送られる前記作
業用ツールの送り速度ｖxyを演算する手段と、前記勾配
tan αと送り速度ｖxyとから前記押付け方向の移動速度
ｖzofを演算する手段とを含む構成とすることができ
る。

ここで、好ましくは、速度指令値ｕxyを指令する手段
はワーク面上を移動する作業用ツールの位置を検出する
第２の検出手段を有し、第１の演算手段の勾配を演算す
る手段は、前記第２の検出手段で検出した作業用ツール
のワーク面上の現在点を含む２点の位置から前記ワーク
面の勾配tan αを演算する手段を含む構成とする。この
構成は、検出値としてのワーク面上の２点の位置を用い
るのであるから倣い動作中の演算に適している。

また、速度指令値ｕxyを指令する手段はワーク面上を
移動する作業用ツールの位置を検出する第２の検出手段
を有し、第１の演算手段の勾配を演算する手段は、前記
第２の検出手段で検出した作業用ツールの現在点を含む
２点の位置から前記作業用ツールの送り速度ベクトルｖ
の方向を演算すると手段と、前記送り速度ベクトルｖの
方向と前記第１の検出手段で検出した力の方向とから前
記ワーク面の勾配tan αを演算する手段とを含む構成と
してもよい。この場合も、倣い動作中の演算に適してい
る。

また、速度指令値ｕxyを指令する手段は前記作業用ツ
ールの送り目標値ｖxyoを設定する手段を有し、第１の
演算手段は、前記ワーク面の勾配tan αを演算する手段
と、前記勾配tan αと前記送り目標速度ｖxyoとから前
記押付け方向の移動速度ｖzofを演算する手段とを含む
構成としてもよい。

この場合、好ましくは、速度指令値ｕxyを指令する手
段はワーク面上を移動する作業用ツールの位置を検出す
る第２の検出手段を更に有し、第１の演算手段の勾配を
演算する手段は、作業用ツールの送り目標位置を予め設
定する手段と、前記第２の検出手段で検出した作業用ツ
ールのワーク面上の現在点の位置と前記設定手段で予め
設定した送り目標位置との２点の位置から作業用ツール
の押付け方向に垂直な方向の送り方向ベクトルｍを演算
する手段と、前記送り方向ベクトルｍと前記第１の検出
手段で検出した力の方向とから前記ワーク面の勾配tan
αを演算する手段とを含む構成とする。これは、２点の
位置の１つとして予め設定した送り目標位置を用いるの
であるから、作業用ツールがまだ移動していない倣い動
作開始時の演算に適している。

なお、以上において、勾配tan αを演算するのに第１
の検出手段で検出した力の方向を用いる場合は、力とし
て３軸方向の力が必要であり、従って、第１の検出手段
としては作業用ツールに加わる少なくとも直交３軸方向
の力を検出できるものを用いる。

以上は、第１の演算手段はワーク面の勾配tan αを演
算し、この勾配tan αを用いて移動速度ｖvofを演算し
たが、勾配tan αを用いずに移動速度ｖvofを演算して
もよい。例えば、速度指令値ｕxyを指令する手段はワー
ク面上を移動する作業用ツールの位置を検出する第２の
検出手段を有し、第１の演算手段は、前記第２の検出手
段で検出した作業用ツールのワーク面上の現在点を含む
２点の位置とその２点の位置の移動時間とから前記押付
け方向の移動速度ｖzofを演算する手段を含む構成とし
てもよい。この場合は、勾配を演算する必要がないの
で、演算が容易となり、高速演算処理が可能となる。

さらに、第１の演算手段は、ワーク面の形状が既知
で、押付け方向の移動速度ｖzofが予め予測できる場合
にはその予測した値を記憶しておき、この値を用いる構
成としてもよい。

また、作業用ツールの送り方向の制御に関し、前記作
業用ツールの送り目標速度ｖxyoを設定する手段を更に
備え、第１の演算手段は、前記ワーク面の勾配tan αを
予め記憶する手段と、前記予め記憶した勾配tan αと前
記送り目標速度ｖxyoとから押付け方向の移動速度ｖzof
を演算する手段とを含む構成としてもよい。この場合、
予め記憶した勾配tan αを用いるので、リアルタイムで
ワーク面の勾配tan αを演算する上述の場合よりも演算
量が少なくて済み、高速演算処理が可能となる。

〔実施例〕

第１の実施例 本発明の第１の実施例を第１図〜第４図により説明す
る。

第１図において、多自由度作業機械の一例である６自
由度の関節型ロボット１は手部２を有し、手部２には作
業用ツール４が取り付けられている。作業用ツール４
は、ロボット１の各関節廻りの駆動を行う図示しない駆
動モータを駆動することにより、その位置及び姿勢が制
御される。このようなロボット１に対する本実施例の倣
い制御装置６は、検出手段として、手部２と作業用ツー
ル４の間に取り付けられ、作業用ツール４に加わる力及
びモーメントを検出する力センサー３と、ロボット１の
各関節廻りの駆動を行う図示しない駆動モータに取り付
けられ、駆動モータの駆動量、従って、各関節軸の軸角
度データを検出する角度計、例えばエンコーダ５とを有
し、設定手段として、作業用ツール４の押付け力及びモ
ーメントの目標値（以下、押付け力の目標値と略す） をベース座標系の値として設定する力目標値設定部７
と、作業用ツール４の位置及び姿勢の目標値 をベース座標系の値として設定する位置目標値設定部８
とを有している。なお、ベース座標系とはロボットが据
え付けられた空間に固定の座標系のことである。

また、倣い制御装置６は、力制御ループの構成要素と
して、力センサー３で検出した力及びモーメントをセン
サ座標系から手先座標系へ変換すると共に、作業用ツー
ル４の重力分を差し引く重力補償を行う力演算部９と、
手先座標系で表わされた力及びモーメントを、手先座標
系と原点を同じにとったベース座標系に変換する座標変
換部10と、座標交換部10で座標変換された力及びモーメ
ント と後述する力目標補正部7Aで求めた力及びモーメントの
目標値 と比較し、 を演算して偏差 を求める力偏差演算部12とを有し、位置制御ループの構
成要素として、ロボット１のモータに取付けたエンコー
ダ５からの軸角度データを入力し、関節角 を算出する角度演算部13と、その関節角 からベース座標系での手先の位置及び姿勢 を求める位置演算部14と、位置演算部14で演算した位置
及び姿勢 と位置目標値設定部８で設定した位置及び姿勢の目標値 とを比較し、偏差 を求める位置偏差演算部15とを有している。

力偏差演算部12で演算したベース座標系での力及びモ
ーメントの偏差 と位置偏差演算部15で演算したベース座標系での位置及
び姿勢の偏差 は位置と力の制御演算部17に入力され、制御演算部17で
はこれらの情報をもとにベース座標系での位置と力の制
御演算を行い、ベース座標系での速度指令値 を演算する。本実施例では、この位置と力の制御演算部
17には仮想コンプライアンス制御による制御演算を利用
する。

即ち、制御演算部17は、不感帯演算部18と、バネ定数
乗算部19と、減算部20と、特性補償演算部21とからなっ
ている。不感帯演算部18は、力偏差演算部12で演算した
力偏差 に不感帯を持たせ、 を算出するもので、ノイズや外乱などの微小力に反応し
ないように設定することができる。この不感帯幅は各座
標軸毎に自由に設定することが出来、不感帯をなくすこ
とも出来るし、不感帯幅を大きくとることによってこの
出力を常に０にし、力フィードバックをなくして、位置
制御のみを行うことも出来る。バネ定数乗算部19は各座
標軸毎に設定した仮想バネ定数行列Ｋを位置偏差 に乗じて を演算する。Ｋは対角行列で、成分ｋiは各座標軸毎の
仮想バネ定数で、任意に設定出来る。特にｋi＝０にす
れば位置のフィードバックをなくして、力制御のみを行
うことも出来る。

減算部20は、力の偏差 に不感帯演算を行った値 から位置の偏差 に仮想バネ定数行列Ｋを乗じた値 を減算する。

特性補償演算部21は、減算部の出力 に制御上の特性補償演算を行って速度指令値 を出力するもので、仮想コンプライアンス制御では、仮
想質量行列Ｍと仮想粘性係数行列Ｃによって、 を模擬して動くように速度指令値を演算する。

倣い制御装置６は、最後にその出力部分として、位置
と力の制御演算部17で演算された速度指令値 から各関節の角速度指令値 を演算する角速度演算部23と、この角速度指令値 から各駆動モータの速度指令値を演算するモータ速度演
算部24とを有し、モータ速度演算部24で演算された速度
指令値はサーボアンプ25に送られ、各駆動モータの速度
指令値によってロボット１のモータを駆動する。この中
にはモータに取り付けたタコジェネレータからの速度フ
ィードバックも含まれる。

力センサ３、力演算部９、座標変換部10、力目標値設
定値７、力目標値補正部7A、力偏差演算部12、制御演算
部17、角速度演算部23、モータ速度演算部24、サーボア
ンプ部25は力制御ループを構成し、エンコーダ５、角度
演算部13、位置演算部14、位置目標値設定部８、位置偏
差演算部15、制御演算部17、角速度演算部23、モータ速
度演算部24、サーボアンプ部25は位置制御ループを構成
する。

力目標補正部7Aの機能を第２図により説明する。

第２図において、角度演算部13で算出された関節角 は位置演算部14に送られ、関節角 からベース座標系での手先の位置及び姿勢 が求められる。力目標値設定部７には押付け力の目標値 がベース座標系の値として設定されている。

力目標値補正部7Aは、位置演算部14で求めた手先の位
置及び姿勢 の位置データを手先の移動に従って順次記憶する記憶部
60と、記憶部60に記憶された位置データをもとに作業用
ツール４の押付け方向に垂直な平面に対する作業用ツー
ル４の移動方向のワーク面の勾配を演算する勾配演算部
61と、位置演算部14で求めた位置とその検出時間とから
押付け方向に垂直な平面に投影した作業用ツール４の移
動速度、即ち、送り速度を演算する送り速度演算部62
と、勾配演算部61で求めた勾配、送り速度演算部62で求
めた送り速度及び力目標値設定部７で設定した押付け力
の目標値 から押付け力の指令値として新たな押付け力の目標値 を演算する押付け力演算部63とからなる。

押付け力演算部63で求めた新たな押付け力の目標値 は力偏差演算部12に出力される。なお、記憶部60で記憶
される位置データは、任意の時間あるいは移動距離毎に
記憶され、その数は勾配演算部61で用いる範囲とする。

次に、このように構成された倣い制御装置６の動作を
説明する。まず、第１図に示す制御装置全体の動作を説
明する。

ロボット１にワーク表面を倣って表面研磨やバリ取り
等を行なわせる曲面倣い動作をさせた場合、ロボット１
に取り付けられた作業用ツール４には押付け力の反力と
しての力及びモーメントが加わる。力制御ループでは、
この力及びモーメントを力センサー３で検出する。この
検出された力及びモーメントはセンサーに固定された、
センサーの変形中心を原点とするセンサー座標系の値に
なっているので、これを力演算部９で作業用ツールの適
当な場所を原点とし、作業用ツールに固定されて手先と
共に移動する手先座標系に変換する。この原点は普通、
力の加わる着力点付近にとり、センサー座標系からの交
換行列は定数行列になる。さらにこの力演算部９では、
作業用ツール４の重力分を差し引いて重力補償を行い、
手先の姿勢によって変化する作業用ツール４の重力の影
響を取り除く。

このように手先座標系に変換された力及びモーメント
は、座標変換部10によって手先座標系と原点を同じにす
るベース座標系に変換される。即ち、ここで言うベース
座標系とは直交３軸の方向がベース座標系と一致すると
いう意味であり、座標変換部10では方向のみベース座標
系に変換（回転変換）する。この座標変換は、ベース座
標系に対する手先座標系の方向によって与えられ、後述
する手先の姿勢を表す行列と同様なので、常に計算され
ており、簡単に変換を行うことが出来る。

このようにして得られたベース座標系での力 は、押付け力演算部63で求められた押付け力の目標値 と比較され、力偏差演算部12によって力偏差 が演算される。

一方、位置制御ループに関しては、ロボット１のモー
タに取り付けたエンコーダの値から角度演算部13によっ
て関節角 を演算する。さらに、この関節角 から位置演算部14によって、ベース座系標における手先
の位置及び姿勢 を演算する。この手先の位置及び姿勢は手先座標系の原
点の位置及び座標軸の方向で見る。第１図では、図を理
解しやすいようにベース座標系でのこの位置及び姿勢を
６次元ベクトル で表わしており、この場合、６次元ベクトル 中の姿勢を表す３次元ベクトルは、ベース座標系に対す
る手先座標系の回転角度を表すベクトル（方向が回転軸
方向で大きさが回転角となるベクトル）となる。

実際の演算では、この姿勢は上記のように表記せず、
手先座標系の各座標軸方向の単位ベクトルをベース座標
系で表した からなる３×３行列 で表す。

このように位置演算部14で演算された位置及び姿勢 は、設定部８で設定された位置及び姿勢目標値 と比較され、演算部15で位置及び姿勢の偏差 が計算される。このうち姿勢の偏差は、目標姿勢と実際
の姿勢との間の回転角を表すベクトルで表わせ、目標手
先座標系の姿勢を表す行列を とすれば姿勢の偏差は で表せる。ここで は回転軸の単位ベクトルであり、Ψは軸廻りの回転角で
あり、それぞれ以下のように表わされる。

このようにして得られたベース座標系での力の偏差 と位置偏差 を用いて位置と力の制御演算部17で制御演算を行う。本
実施例ではこの演算の具体例として、前述したように要
素18〜21による仮想コンプライアンス制御演算を行う。

即ち、不感帯演算部18では力偏差 の各座標軸毎に不感帯演算をして を算出し、バネ定数乗算部19では各座標軸毎に設定した
仮想バネ定数行列Ｋを位置偏差 に乗じて を演算し、減算部20では不感帯演算を行った力の偏差 から を減算する。特性補償演算部21では、減算部の出力 に制御上の特性補償演算を行って速度指令値 を出力し、仮想コンプライアンス制御では、仮想質行列
Ｍと仮想粘性係数行列Ｃによって前述した（１）式を模
擬して動くように速度指令値を演算する。この演算は、
実際は例えば（１）式を離散系で変形した の形で行う。ここでΔｔはサンプリング周期であり、 はｎ回目のサンプリングを表す。また は並進速度及び回転速度を表わす６次元ベクトル、 は力及びモーメントの偏差を表わす６次元ベクトル、 は位置及び姿勢の偏差を表す６次元ベクトル、M,C,Kは
６×６の行列で、特に対角行列を用いる。

ここで、各要素のパラメータK,M,Cや不感帯幅はベー
ス座標系上で与えられており、位置と力の制御演算をこ
のベース座標系上で行う。

位置と力の制御演算部17で演算された速度指令値 はロボット１を動かすための指令値である。この速度指
令値 は角速度演算部23によってロボットの角関節の角速度 に変換され、さらにモータ速度演算部24で、モータの回
転速度に変換した後、サーボアンプ25によってこの速度
で動くようにロボット１を制御する。

次に、力目標値補正部7Aの作用を説明する。

まず、第３図及び第４図を参照して押付け力の演算に
関する考え方を説明する。

第３図は押付け力の目標値の与え方を示す図であり、
作業用ツール４でワーク50の表面を倣う場合の例を示
す。説明の便宜上、第１図の制御演算部17でｘ軸方向の
不感帯幅を大きくとって位置制御とし、ｚ軸方向のばね
定数を０として力制御とする。また、力目標設定部７で
ｚ軸方向に一定の押付け力の目標値ｆroを設定し、これ
を直接力偏差演算部12に出力するものとする。この場
合、力制御ループでは、力の目標値ｆroと検出値に基づ
く力及びモーメント との偏差 から制御演算部17で速度指令値 のｚ軸成分ｕzが演算され、作業用ツール４はこの速度
指令値ｕzにより力の目標値ｆroでワーク面に押付けら
れる。一方、位置制御ループでは、位置及び姿勢の目標
値 と検出値に基づく位置及び姿勢 との偏差 から制御演算部17で速度指令値 のｘ軸成分ｕxが演算され、作業用ツール４はこの速度
指令値ｕxに基づいた送り速度ｖxでｘ軸方向に送られ
る。

このような状態において、速度指令値ｕxにより得ら
れる作業用ツール４の送り方向に対するワーク面の傾斜
角をαとし、その勾配をtan αで表わすと、作業用ツー
ル４はワーク面上に力の目標値ｆroで押付けられながら
速度指令値ｕxに基づく速度ｖxで送られているのである
から、作業用ツール４には送り方向のワーク面の勾配ta
n αによりｚ軸方向に ｖzof＝ｖx tan α の速度を生じる。従って、この速度ｖzofによって、目
標値ｆroの力とは別に ｆof＝C v zof という押付け力が発生する。但し、Ｃは仮想コンプライ
アンス制御で設定した粘性減衰係数である。このため、
力目標値設定部７で押付け力の目標値ｆofを設定したに
も係わらず、実際にはｆ＝ｆro＋ｆofの押付け力が生じ
る。従って、設定した押付け力ｆroを得るためには、押
付け力の目標値ｆroから力ｆofを差し引いた値を押付け
力の指令値とすればよい。即ち、この押付け力の指令値
をｆrとすれば、その指定値ｆrは次式で求まる。

ｆr＝ｆro−C v x tan α 本実施例は以上の知見に基づくものであり、第２図に
示す勾配演算部61、送り速度演算部62及び押付け力演算
部63で上記演算を行ってやることにより、作業用ツール
４を一定の押付け力でワーク面を倣うように制御する。

即ち、第４図に示すように、ワーク50の表面を倣いな
がら記憶部60に記憶された現在の位置を点Pj（ｘj,yj,z
j）とし、その１つ前の位置を点Pi（ｘi,yi,zi）とする
と、点Pjでのワーク面の勾配tan αは次式で近似するこ
とができる。

勾配演算部61では上式の演算を行う。

又、第４図において、点Pjでの作業用ツール４の送り
速度 のｘ軸方向成分ｖx及びｙ軸方向成分ｖyは、 ｖx＝（ｘj−ｘi）／Δｔ ｖy＝（ｙj−ｙi）／Δｔ で表わされる。ここでΔｔは、点Piと点Pjの間の検出時
間である。Δｔとしては、記憶部60で一定の時間間隔Δ
ｔで手先の位置を順次記憶するようにし、その時間間隔
Δｔを送り速度演算部62に記憶しておけばよい。従っ
て、ｘ−ｙ平面に平行な方向の送り速度の大きさｖxyは
次式で近似することができる。

送り速度演算部62は上式の演算を行う。なお、第３図
に示す上述した例では送り速度のｘ軸方向成分のみが演
算されている。

そして押付け力演算部63においては、勾配演算部61で
演算した勾配tan αと送り速度演算部62で演算した送り
速度ｖxyと予め設定した粘性減衰係数Ｃとから速度ｖxy
によってｚ軸方向に生じる力ｆof＝C v xy tanαを演算
し、この力ｆofと力目標値設定部７で設定した力目標値
ｆroとから新たな押付け力の目標値ｆrを、上述した式
ｆr＝ｆro−C v xy tanαより演算する。

このようにして押付け力演算部63で求めた新たな押付
け力の目標値ｆrは、力目標値 として力偏差演算部12に送られ、前述した力偏差 を求めるのに使用される。

本実施例は、以上のように構成したので、ロボット１
でワーク表面を倣って表面研磨やバリ取り等を行なわせ
る曲面倣い動作を行う場合、ワーク面に対し目標値ｆro
の力で押付けながら作業用ツール４を移動させることが
できるため、未知の形状のワークに対しても滑らかでか
つ効率的な作業を行うことができる。

又、曲面倣い動作を行う場合、ワーク面に対して一定
の押付け力で作業用ツール４を押し付ける手法としては
移動のための座標変換を行う等の手法も考えられるが
（特願昭63−48486−US Serial No.316,696及びEPC
Patent application no.89200495.3対応−参照）、本実
施例ではワーク面の形状に合わせて押付け力のみを制御
するため、そのような手法に比べて座標変換等の複雑な
演算を必要とせず、高速演算処理が可能となる。

第２の実施例 本発明の第２の実施例を第５図及び第６図を参照して
説明する。図中、第１図及び第２図に示す部材と同等の
部材には同じ符号を付している。

本実施例の倣い制御装置における力目標値補正部7B
は、第５図に示すように、第２図に示す第１の実施例の
構成に加えて、作業用ツール４の倣い動作開始時の送り
目標位置を記憶する記憶部70と、位置演算部14で演算さ
れた位置と記憶部70に記憶された送り目標位置から送り
方向ベクトル を演算する送り方向ベクトル演算部71と、作業用ツール
の倣い動作開始時の送り目標速度ｖxyoを設定する設定
部62Aとを有している。勾配演算部72は、第１の実施例
における勾配演算部61の機能に加えて、第１図に示す力
演算部９及び座標変換部10で求めた作業用ツール４に作
用する反力ベクトル と送り方向ベクトル演算部71で求めた送り方向ベクトル とからワーク面の勾配tan αを演算する機能を有し、押
付け力演算部63は、勾配演算部72で求めたワーク面の勾
配tan αと、送り速度演算部62で求めた送り速度ｖxy又
は設定部62Aに設定された送り目標速度ｖxyoと、力目標
値設定部７に設定された力目標値 とから新たな押付け力の目標値 を演算する。

なお、設定部62Aの送り目標速度ｖxyoは位置目標設定
部８に送られ、現在の目標位置にτｖxyoを加算し、新
たな目標位置 をサンプリングタイムτ毎に順次計算する。この演算は
後述する第17図に示す実施例の移動位置目標演算部97と
同じであるので、詳細は省略する。

倣い制御装置の他の構成は第１図に示す第１の実施例
と同じである。

次に、本実施例の動作を第６図を参照して説明する。
第６図は本実施例における力目標値補正部7Bにおけるワ
ーク面の勾配tan αの与え方を示す図であり、作業用ツ
ールをワーク50の表面に押付け、倣い動作を開始する状
態を示している。

第６図において、位置演算部14で演算された現在の位
置を点Ｐ（ｘp,yp,zp）とし、記憶部70で記憶された作
業用ツールの送り目標位置を点Ｑ（ｘq,yq,zq）とす
る。この点Ｐ及び点Ｑから作業用ツール４の送り方向を
示す送り方向ベクトル を求めると、 となる。送り方向ベクトル演算部71ではこの演算を行
う。

次に、点Ｐにおけるワーク面の押付反力ベクトル を法線ベクトル に一致すると仮定すると、法線ベクトル の成分は反力ベクトル のxyz方向の３分力（ｆx,fy,fz）で表わされる。又、点
Ｐにおけるワーク面上での移動方向の単位ベクトルを とし、その方向余弦を（ｔx,ty,tz）で表わす。ベクト
ル は点Ｐにおけるワーク面の接線ベクトルであるから、法
線方向ベクトル と直交する。従って、互いに直交するベクトルの内積は
０であるから、次式が成り立つ。

よって、 ｆx・ｔx＋ｆy・ｔy＋ｆz・ｔz＝０ …（３） 今、ｘ−ｙ平面に垂直で点Ｑを通る直線と、ベクトル の延長線が交わる点をＱ′とし、点Ｐと点Ｑ′の距離を
とすると、ベクトルPQ′の成分は（ｔx，ｔy，
ｔz）で表わされる。従って、点Ｐにおけるワーク面の
勾配tan αは送り方向ベクトル に対するベクトル のなす勾配に一致し、ワーク面の勾配tan αは次式で求
めることができる。

ここで（３）式の両辺をかけてｔzについて整理
すると、 ｔz＝−（ｆx tx＋ｆy ty）/fz …（５） 又、ｔx，ｔyは法線方向ベクトル の成分より ｔx＝ｘq−ｘp ｔy＝ｙq−ｙp …（６） 従って、（５）式及び（６）式を用いて（４）式を整
理すると、 が得られる。

勾配演算部72では、送り方向ベクトル演算部71で求め
た送り方向ベクトルｍと座標変換部10で求めた反力ベク
トルｆとを用いて以上の演算を行い、ワーク面の移動方
向の勾配tan αを求める。

押付け力演算部63においては、勾配演算部72で上記の
ようにして求めた送り方向に対するワーク面の勾配tan
αと送り目標速度設定部62Aに設定された送り目標速度
ｖxyoと力目標値設定部７に設定された力目標値 とを用いて、第１の実施例と同様に、ｆr＝ｆro−C v x
yo tan αを演算し、押付け力の指令値として新たな押
付け力の目標値 を演算する。従って、第１の実施例と同様、送り目標速
度ｖxyoによって生じる押付け力の変化分が差し引か
れ、作業用ツール４を目標値 の力で押付けながらワーク面を倣わせることができる。

以上のようにして、倣い動作開始時における作業用ツ
ール４が停止状態から移動開始直後までは、記憶部70に
記憶された送り目標位置と設定部62Aに設定された送り
目標速度ｖxyoを用いて押付け力の目標値を演算し、位
置と力の制御を行う。一旦作業用ツール４が動き始めた
後は、第１の実施例と同様な位置と力の制御を行う。即
ち、勾配演算部72は、記憶部60に記憶された位置データ
をもとに送り方向に対するワーク面の勾配tan αを演算
し、送り方向速度演算部62は、記憶部60で記憶した位置
情報を用いて送り速度ｖxyを演算し、押付け力演算部63
は、勾配演算部72で求めたワーク面の移動方向の勾配ta
n αと送り速度演算部62で求めた送り速度ｖxyと力目標
値設定部７に設定された力目標値 とを用いて押付け力の指令値ｆrを演算する。

このように本実施例においては、位置又は速度の検出
値を用いることなく、記憶部70に記憶した倣い軌跡の位
置情報と設定部62Aに設定した送り速度ｖxyoを用いて押
付け力の目標値を演算するので、倣い動作開始時の停止
時においても押付け力の目標値を演算することができ、
倣い動作開始時にも押付け力を制御することができる。

なお、送り目標速度設定部62Aにおける送り目標速度
の設定の仕方としては、オペレータがジョイスティッ
ク、キーボード等を操作して送り目標速度を指令する方
法、教示データ等の既知の２点を予め記憶しておく方法
等がある。

第３の実施例 本発明の第３の実施例を第７図及び第８図を参照して
説明する。図中、第１図及び第２図に示す部材と同等の
部材には同じ符号を付している。

本実施例の倣い制御装置における力目標値補正部7C
は、第７図に示すように、位置演算部14で演算された位
置を順次記憶する記憶部60と、記憶部60で記憶した位置
から作業用ツール４の押付け方向に垂直な方向の送り速
度ベクトル を演算する送り速度演算部80と、力演算部９及び座標変
換部10で演算された作業用ツール４に作用する反力ベク
トル の方向と演算部80で求めた送り速度ベクトル の方向とから送り方向に対するワーク面の勾配tan αを
演算する勾配演算部81と、演算部81で求めた勾配tan α
と演算部80で求めた送り速度ベクトル の大きさと力目標値設定部７に設定された力目標値 とから新たな押付け力の目標値 を演算する押付け力演算部63とからなる。押付け力演算
部63で求めた新たな押付け力の目標値 は力偏差演算部12に出力される。

倣い制御装置の他の構成は第１図に示す第１の実施例
と同じである。

次に、本実施例の動作を説明する。第４図を再び参照
して、作業用ツール４でワーク50の表面を倣っている状
態を考える。このとき、記憶部60には任意の時間Δｔ毎
に位置演算部14で演算された作業用ツール４の位置が記
憶される。また、第４図において、Pjは作業用ツール４
の現在の位置、Piはその１つ前の位置である。この２つ
の位置から、ｘ−ｙ平面に平行な方向の送り速度ベクト
ル の成分（ｖx,vy,vz）は、 ｖx＝（ｘj−ｘi）／Δｔ ｖy＝（ｙj−ｙi）／Δｔ ｖz＝０ …（８） となる。又、その平均速度は となる。これは送り速度ベクトル の大きさに一致し、第１の実施例で説明した送り速度ｖ
xyと同じ値である。従って、以下、送り速度をｖxyと表
示する。送り速度演算部80では以上の演算を行う。

第８図は本実施例の勾配演算部81におけるワーク面の
勾配tan αの与え方を示す図である。第８図において、
現在の検出位置を第４図と同様に点Pjとし、Pj点の送り
速度ベクトルを上述の で表わしている。今、Pj点におけるワーク面の押付け反
力ベクトル を法線方向ベクトルと仮定する。又、Pj点におけるワー
ク面上の移動方向単位ベクトルを その方向余弦を（ｔx,ty,tz）で表わす。ベクトル は点Pjにおけるワーク面の接線ベクトルであるから、反
力ベクトル と直交する。従って、互いに直交するベクトルの内積は
０であるから、次式が成り立つ。

よって、 ｆx・ｔx＋ｆy・ｔy＋ｆz・ｔz＝０ …（10） 次に、単位ベクトル にをかけ、ベクトル をｘ−ｙ平面に投影したときの値が送り速度ベクトル と一致するようにを定める。このときベクトル の成分は（ｔx，ｔy，ｔz）で表わされる。従っ
て、点Pjにおける勾配tan αは送り速度ベクトル に対するベクトル のなす勾配に一致し、倣い勾配tan αは次式で求めるこ
とができる。

ここで（10）式の両辺にをかけてｔzについて整
理すると、 ｔz＝−（ｆx tx＋ｆy ty）/fz …（12） 又、ｔx，ｔyは速度ベクトル の成分に一致することから、 ｔx＝ｖx ｔy＝ｖy …（13） 従って、（12）式及び（13）式を用いて（11）式を整
理すると、 が得られる。勾配演算部81では以上の演算を行う。

押付け力演算部63においては、第１の実施例と同様
に、送り速度ｖxyと勾配tan αと力目標値 とからｆr＝ｆro−C v xy tan αを演算し、新たな押付
け力の目標値 を求め、作業用ツール４の一定の押付け力でワーク面を
倣うように制御する。

本実施例においては、第１の実施例とは異なる手法で
勾配tan αを求め、第１の実施例と同様の効果を得るこ
とができる。

第４の実施例 本発明の第４の実施例を第９図を参照して説明する。
図中、第１図及び第２図に示す部材と同等の部材には同
じ符号を付している。

本実施例の倣い制御装置における力目標値補正部7D
は、位置演算部14で求めた手先の位置及び姿勢 の位置データを手先の移動に従って一定時間ごとに順次
記憶する記憶部60と、記憶部60に記憶された位置データ
をもとに作業用ツール４の押付け方向の移動速度ｖzof
を演算する移動速度演算部82と、移動速度演算部82で求
めた移動速度ｖzof及び力目標値設定部７で設定した押
付け力の目標値 とから新たな押付け力の目標値 を演算する押付け力演算部83とからなる。押付け力演算
部83で求めた新たな押付け力の目標値 は力偏差演算部12に出力される。ここで、記憶部60で記
憶される位置データは、一定の時間毎に記憶され、その
数は移動速度演算部82で用いる範囲とする。

倣い制御装置の他の構成は第１図に示す第１の実施例
と同じである。

本実施例の動作を第４図を再び参照して説明する。第
４図において、手先効果器４をｚ軸方向に押付けながら
ワーク面を倣うとき、前述したように記憶部60に記憶さ
れた現在位置を点Pj（ｘj,yj,zj）とし、その１つの前
の位置を点Pi（ｘi,yi,zi）とすると、点Pjでのｚ方向
速度ｖzofは次式で近似できる。

ここでΔｔは点Piと点Pjの間の時間である。時間Δｔ
は移動速度演算部82に予め記憶しておく。移動速度演算
部82は上式の演算を行う。

押付け力演算部83においては、移動速度演算部82で演
算した移動速度ｖzofと予め設定した粘性減衰係数Ｃと
から送り速度によってｚ軸方向に生じる力ｆof＝C v zo
fを演算し、この力ｆofと力目標値設定部７で設定した
力目標値ｆroとから新たな押付け力の目標値ｆrを式ｆr
＝ｆro−C v zより演算する。即ち、第１〜第３の実施
例で勾配tan αと送り速度ｖxyを用いてｚ軸方向に生じ
る力を演算した代わりに、ｚ軸方向の移動速度ｖzofか
ら直接ｚ軸方向に生じる力ｆofを演算するものである。

このようにして押付け力演算部83で求めた新たな押付
け力の目標値ｆrは力目標値 とし力偏差演算部12に送られ、力偏差 を求めるのに使用される。

従って本実施例においても、第１の実施例と同様に作
業ツール４を目標値ｆroの力で押付けながらワーク面を
倣わせることができる。

又、本実施例においては、第１〜第３の実施例におけ
る勾配を演算する手法に比べて演算が容易なので、一層
の高速演算処理が可能となる。

第５の実施例 本発明の第５の実施例を第10図及び第11図を参照して
説明する。図中、第１図及び第２図に示す部材と同等の
部材には同じ符号を付している。

第10図において、本実施例の倣い制御装置6Aは、制御
演算部17の特性補償演算部21で速度指令値 を演算し、この速度指令値 を速度指令値補正部90Aで補正し、新たな速度指令値 を得て、この速度指令値 が角速度演算部23で使用される。偏差演算部12には力目
標値設定部７から直接力目標値 が送られる。

速度指令値補正部90Aは、第11図に示すように、第２
図に示す第１の実施例の力目標値補正部7Aと同様、位置
演算部14で求めた手先の位置及び姿勢 の位置データを手先の移動に従って順次記憶する記憶部
60と、記憶部60に記憶された位置データをもとに作業用
ツール４の送り方向に対するワーク面の勾配tan αを演
算する勾配演算部61と、位置演算部14で求めた位置とそ
の検出時間Δｔとから作業用ツール４の送り速度ｖxyを
演算する送り速度演算部62とを有している。速度指令値
補正部90Aは、さらに、勾配演算部61で求めた勾配tan
αと送り速度演算部62で求めた送り速度ｖxyとから押付
け方向の速度補正値ｖzcを演算する補正速度演算部90
と、制御演算部17で求めた速度指令値 のｚ軸成分ｕzoに演算部90で求めた速度補正値ｖzcを加
算して新たな速度指令値 を求め、これを角速度演算部23に出力する加算器91とを
有している。

第３図及び第４図を参照して説明したように、力目標
値７でｚ軸方向に押付け力の目標値ｆrを与えながらｘ
−ｙ平面に平行な方向に速度ｖxyで作業用ツール４を移
動させるとき、送り方向の勾配tan αにより作業用ツー
ル４には ｖzof＝ｖxy tan α のｚ軸方向の速度を生じ、この速度ｖzofによって ｆof＝C v zof という力が発生する。本実施例では、第11図に示す演算
部分で直接この速度ｖzofを演算し、これを押付け方向
の速度補正値として加算部91に加えることにより、ｚ軸
方向の速度変化分ｖzofの影響を除去し、ワーク面を倣
わせるものである。

即ち、第４図において、ワーク面を倣いながら記憶部
60に記憶された現在点Pjとそれより１つ前の点Piの２点
を用いて勾配演算部61では点Pjでのワーク面の勾配tan
αを演算する。演算の手法は第１の実施例で説明したの
と同じである。

一方、送り速度演算部62では、第１の実施例と同様に
２点の位置Pj,Piの位置情報からｘ−ｙ平面に平行な方
向の送り速度ｖxyを演算する。

補正速度演算部90では、演算部61で演算された勾配ta
n αと演算部62で演算された送り速度ｖxyとから次式の
計算によりｚ軸方向の速度ｖzofを求める。

ｖzof＝ｖxy tanα そして、この速度ｖzofを速度補正値ｖzcと置き、こ
れを加算部91で加える。

このように本実施例においては、力目標値 を補正するのではなく、制御演算部17で演算された速度
指令値 を補正することにより、第１の実施例と同様、作業用ツ
ール４を目標値ｆrの力で押付けながらワーク面を倣わ
せることができる。

従って、本実施例においても第１の実施例と同じ効果
を得ることができる。

第６の実施例 本発明の第６の実施例を第12図を参照して説明する。
図中、第５図、第10図及び第11図に示す部材と同等の部
材には同じ符号を付している。

本実施例は、第５の実施例の速度指令値補正部におけ
る勾配tan αの演算手段を第５図に示した第２の実施例
における勾配tan αの演算手段に置き換えたものであ
る。即ち、本実施例の速度指令値補正部90Bは、第12図
に示すように、第５図に示した実施例の力目標値補正部
7Bと同様、記憶部60と、送り速度演算部62と、送り目標
位置を記憶する記憶部70と、位置演算部14で演算された
位置と記憶部70に記憶された送り目標位置から送り方向
ベクトル を演算する送り方向ベクトル演算部71と、力演算部９及
び座標変換部10で演算した作業用ツール４に作用する反
力ベクトル と演算部71で求めた送り方向ベクトル とからワーク面の勾配tan αを演算する勾配演算部72
と、送り目標速度ｖxyoを設定する設定部62Aとを有して
いる。速度指令値補正部90Bは、さらに、勾配演算部72
で求めた勾配tan αと送り速度演算部62で求めた送り速
度ｖxy又は設定部62Aに設定された送り目標速度Ｖxyoと
から押付け方向の速度補正値ｖzcを演算する補正速度演
算部90と、制御演算部17で求めた速度指令値 のｚ軸成分ｕzoに演算部90で求めた速度補正値ｖzcを加
算して新たな速度指令値 を求め、これを角速度演算部23に出力する加算器91とを
有している。

反力ベクトル を送り方向ベクトル とからワーク面の移動方向の勾配tan αを演算する勾配
演算部72の演算内容は、第５図に示す第２の実施例と同
じであり、補正速度演算部90の演算内容は第11図に示し
た第５の実施例と同じである。

従って、本実施例においても、第５の実施例と同様
に、制御演算部17で演算された速度指令値 を速度補正値ｖzcで補正することにより作業用ツール４
を目標値ｆrの力で押付けながらワーク面を倣うように
制御することができると共に、第２の実施例と同様、こ
の制御を作業用ツールが停止している倣い動作開始時か
ら行うことができる。

第７の実施例 本発明の第７の実施例を第13図を参照して説明する。
図中、第７図、第10図及び第11図に示す部材と同等の部
材には同じ符号を付している。

本実施例は、第５の実施例の速度指令値補正部におけ
る勾配tan αの演算手段を第７図に示した第３の実施例
における勾配tan αの演算手段に置き換えたものであ
る。即ち、本実施例の速度指令値補正部90Cは、第13図
に示すように、第７図に示した実施例の力目標値補正部
7Cと同様、位置演算部14で演算された位置を順次記憶す
る記憶部60と、記憶部60で記憶された位置から作業用ツ
ール４の送り速度ベクトル を演算する送り速度演算部80と、力演算部９及び座標変
換部10で演算した作業用ツール４に作用する反力ベクト
ル の方向と送り速度ベクトル の方向とからワーク面の勾配tan αを演算する勾配演算
部81とを有している。速度指令値補正部90Cは、さら
に、勾配演算部81で求めた勾配tan αと送り速度演算部
80で求めた送り速度ベクトル の大きさとから押付け方向の速度補正値ｖzcを演算する
補正速度演算部90と、制御演算部17で求めた速度指令値 のｚ軸成分ｕzoに演算部90で求めた速度補正値ｖzcを加
算して新たな速度指令値 を求め、これを角速度演算部23に出力する加算器91とを
有している。

送り速度演算部80及び勾配演算部81で行われる演算内
容は第７図に示す第３の実施例と同じであり、補正速度
演算部90で行われる演算内容は第11図に示す第５の実施
例と同じである。

従って、本実施例においても、第３の実施例で説明し
たのと同様の考えでワーク面の移動方向の勾配tan αを
演算しながら、第５の実施例と同様に制御演算部17で演
算された速度指令値 を速度補正値ｖzcで補正することにより作業用ツール４
を目標値ｆrの力で押付けながらワーク面を倣うように
制御することができる。

第８の実施例 本発明の第８の実施例を第14図を参照して説明する。
図中、第９図、第10図及び第11図に示す部材と同等の部
材には同じ符号を付している。

本実施例は、第５の実施例の速度指令値補正部におけ
る速度補正値ｖzcの演算手段を第９図に示した第４の実
施例における作業用ツールの押付け方向の移動速度ｖzo
fを演算する手段に置き換えたものである。即ち、本実
施例の速度指令値補正部90Dは、第14図に示すように、
位置演算部14で求めた手先の位置及び姿勢 の位置データを手先の移動に従って一定時間ごとに順次
記憶する記憶部60と、記憶部60に記憶された位置データ
をもとに作業用ツール４の押付け方向の移動速度ｖzof
を演算し、これを速度補正値ｖzcとする移動速度演算部
82と、制御演算部17で求めた速度指令値 のｚ軸成分ｕzoに演算部82で求めた速度補正値ｖzcを加
算して新たな速度指令値 を求め、これを角速度演算部23に出力する加算器91とを
有している。

移動速度演算部82で行われる演算内容は第９図に示す
第４の実施例と同じである。

従って、本実施例においても、第４の実施例で説明し
たのと同様に比較的容易な演算を行いながら、第５の実
施例と同様に制御演算部17で演算された速度指令値 を速度補正値ｖzcで補正することにより作業用ツール４
を目標値ｆrの力で押付けながらワーク面を倣うように
制御することができる。

第９の実施例 本発明の第９の実施例を第15図を参照して説明する。
図中、第１図、第２図及び第５図に示す部材と同等の部
材には同じ符号を付している。

第１の実施例では、位置演算部14で求めた位置から送
り速度演算部62において作業用ツール４の実際の送り速
度ｖxyを演算し、この送り速度ｖxyを用いて演算部63で
押付け力の目標値 を演算した。本実施例は、送り速度演算部62を用いず
に、送り目標速度設定部62Aを用いるものであり、その
機能は第２の実施例の送り目標速度設定部62Aと同じで
ある。押付け力演算部63では、演算部61で求めた勾配ta
n αと設定部62Aで設定した送り目標速度ｖxyoと力目標
値設定部７で設定した押付け力の目標値 とから新たな押付け力の目標値 を演算する。

本実施例においても、第１の実施例と同様の効果が得
られることは明らかである。

なお、図示はしないが、第11図に示した第５の実施例
においても、同様に、移動速度演算部62に代えて送り目
標速度設定部62Aを用いることができる。

第10の実施例 本発明の第10の実施例を第16図及び第17図により説明
する。図中、第１図及び第２図に示す部材と同等の部材
には同じ符号を付している。

第16図において、本実施例の倣い制御装置6Cは、ワー
ク面の勾配と作業用ツールの移動軌跡を予め教示データ
として記憶しておく位置及び力目標演算部7Fを有し、こ
の目標値演算部7Fでは、再生時に、記憶した教示データ
と、設定部７に予め設定した作業用ツール４の押付力の
目標値 と、送り目標速度設定部62Aに予め設定した作業用ツー
ル４の送り目標速度ｖxyoとから力及びモーメントの目
標値 と移動位置目標値 とを演算する。演算部7Fで演算した力及びモーメントの
目標値 と移動位置目標値 とはそれぞれ力偏差演算部12及び位置偏差演算部15に送
られ、力偏差 及び位置偏差 が求められる。倣い制御装置6Cの他の構成は第１の実施
例と同じである。

位置及び力目標値演算部7Fは、第17図に示すように、
位置演算部14で求めた手先の位置及び姿勢 の位置データを手先の移動に従って順次記憶する記憶部
60と、位置演算部14で求めた位置及び姿勢 の位置データをもとに作業用ツール４の送り方向に対す
るワーク面の勾配tan αを演算する勾配演算部61と、勾
配演算部61で求めた勾配tan αを作業用ツール４の移動
に従って順次記憶する勾配記憶部95と、記憶部95で記憶
された勾配tan αと送り目標速度設定部62Aに設定され
た送り目標速度ｖxyoと力目標値設定部７に設定した押
付け力の目標値 とから新たな押付け力の目標値 を演算するに押付け力演算部63と、記憶部60に記憶され
た位置データ と送り目標速度設定部62Aに設定された送り目標速度ｖx
yoとから、作業用ツール４の送り速度が当該設定された
送り目標速度となるように１サイクル毎の移動位置目標
値 を演算する移動位置目標値演算部97とからなっている。

教示作業に際して、位置演算部14で求められた作業用
ツール４の位置データ は勾配演算部61に順次送られ、勾配演算部61では隣接す
る２点の位置からその２点間の作業用ツール４が移動す
る方向のワーク面の勾配tan αを演算し、その結果を順
次記憶部95に記憶する。勾配演算部61の演算内容は第２
図に示す第１の実施例の勾配演算部61と同じである。

同時に、位置演算部14で求められた作業用ツール４の
位置 を手先の移動に従って順次記憶部60に記憶する。このと
き記憶される位置データは、任意の時間あるいは移動距
離毎に記憶され、その数は移動位置目標値演算部97で用
いる範囲とする。

次に、記憶部95,60に記憶された勾配及び位置データ
を用いで再生を行う。即ち、押付け力演算部63では、記
憶部95で記憶された勾配tan αと送り目標速度設定部62
Aに設定された送り目標速度ｖxyoと力目標値設定部７に
設定された押付け力の目標値 とを用いて、ワーク面を目標値 の力で押付けるのに必要な目標値 を前述した実施例と同様に、 の式から演算する。

又、移動位置目標値演算部97では、記憶部60に記憶さ
れた位置を通り、送り目標速度設定部62Aに設定された
送り速度ｖxyoで作業用ツール４が移動するように移動
位置目標値 を演算する。

この演算は例えば次のように行われる。記憶部60に記
憶されたｉ番目の位置データ とｉ＋１番目の位置データ からｘ−ｙ平面上での移動ベクトル を求める。また のｘ軸及びｙ軸方向の成分をx，yとする。ここで、
これら２点間を与えられた送り目標速度ｖxyoで移動す
るためには、１サンプリングタイム当りの移動量を各サ
ンプル毎に現在の目標値に付加すれば良い。その移動量
ΔはΔ＝ｖvyo・τ（τ：サンプリングタイム）で
ある。

実際には上記移動ベクトル を考慮する必要があり、移動ベクトル の長さと各軸方向成分の比をとり、移動ベクトル 方向の移動量Δのｘ軸及びｙ軸方向の成分を演算す
る。即ち、 を演算し、各軸の方向成分としての移動量を求める。そ
してこれを位置データに加算することにより、移動位置
目標値 を求める。

従って、本実施例によっても、再生時に押付け力の目
標値 と移動位置目標値 をそれぞれ演算して与えてやることにより、作業用ツー
ルを目標値 の力で押付けながらワーク面を倣わせることができる。

また、本実施例によれば、演算したワーク面の勾配ta
n αを教示データとして一旦記憶し、再生時にこのデー
タを用いて押付け力の目標値を演算するので、リアルタ
イムでワーク面の勾配tan αも演算しながら押付け力の
目標値を演算する前述の実施例よりも、制御時の演算量
が少なくて済み、応答性の良好な倣い制御が可能であ
る。

なお、以上の幾つかの実施例では、作業用ツールの位
置データか送り速度ｖxyを演算したが、速度指令値 を平均化し、その値を送り速度ｖxyの代わりに用いても
よい。

また、以上の実施例では、全て、作業ツールをロボッ
トで把持し、ワークをベースに固定して倣い作業を行う
場合を説明したが、ワークをロボットで把持し、作業用
ツールをベースに固定して倣い作業を行う場合にも、本
発明は同様に適用できるものである。

〔発明の効果〕

本発明によれば、作業用ツールの送り方向に対するワ
ーク面の勾配tan αにより生じる作業用ツールの押付け
方向の移動速度ｖzofを求め、その押付け方向の移動速
度ｖzofにより生じる押付け力の変化量を求め、押付け
力の目標値ｆroからこの押付け力の変化量を差し引き、
その値を新たな押付け力の目標値とするので、作業用ツ
ールの押付け力は目標値ｆroにほぼ一致し、曲面の勾配
のいかんに係わらず所望の速度で倣うことができ、適切
かつ好適な倣い作業を行うことができる。

また、本発明によれば、作業用ツールの送り方向に対
するワーク面の勾配tan αにより生じる作業用ツールの
押付け方向の移動速度ｖzofを求め、この押付け方向の
移動速度ｖzofを押付け方向の速度指令値ｕzに加算し、
その値を新たな速度指令値とするので、作業用ツールの
押付け力は目標値ｆroにほぼ一致し、曲面の勾配のいか
んに係わらず所望の速度で倣うことができ、適切かつ効
率的な倣い作業を行うことができる。

【図面の簡単な説明】

第１図は本発明の第１の実施例による倣い制御装置の全
体構成を示す機能ブロック図であり、第２図はその倣い
制御装置の力目標値補正部の構成を示す機能ブロック図
であり、第３図は倣い動作時に発生する力を示す説明図
であり、第４図は本実施例における勾配の演算方法を示
す説明図であり、第５図は本発明の第２の実施例による
倣い制御装置の力目標補正部の構成を示す機能ブロック
図であり、第６図はその倣い制御装置における勾配の演
算方法を示す説明図であり、第７図は本発明の第３の実
施例による倣い制御装置の力目標値補正部の構成を示す
機能ブロック図であり、第８図はその倣い制御装置にお
ける送り速度の演算方法を示す説明図であり、第９図は
本発明の第４の実施例による倣い制御装置の力目標値補
正部の構成を示す機能ブロック図であり、第10図は本発
明の第５の実施例による倣い制御装置の全体構成を示す
機能ブロック図であり、第11図はその倣い制御装置の速
度指令値補正部の構成を示す機能ブロック図であり、第
12図は本発明の第６の実施例による倣い制御装置の速度
指令値補正部の構成を示す機能ブロック図であり、第13
図は本発明の第７の実施例による倣い制御装置の速度指
令値補正部の構成を示す機能ブロック図であり、第14図
は本発明の第８の実施例による倣い制御装置の速度指令
値補正部の構成を示す機能ブロック図であり、第15図は
本発明の第９の実施例による倣い制御装置の力目標値補
正部の構成を示す機能ブロック図であり、第16図は本発
明の第10の実施例による倣い制御装置の全体構成を示す
機能ブロック図であり、第17図はその倣い制御装置の位
置及び力目標値演算部の構成を示す機能ブロック図であ
る。 符号の説明 １……ロボット（多自由度作業機械） ３……力センサ ４……作業用ツール ５……エンコーダ ６……倣い制御装置 ７……力目標値設定部 7A……力目標値補正部 ８……位置目標値設定部 12……力偏差演算部 17……制御演算部 25……サーボアンプ 50……ワーク 61……勾配演算部（第１の演算手段） 62……送り速度演算部（第１の演算手段） 63……押付け力演算部（第１および第２の演算手段）

───────────────────────────────────────────────────── フロントページの続き (56)参考文献 特開 平１−146645（ＪＰ，Ａ) 特開 平１−109056（ＪＰ，Ａ) (58)調査した分野(Int.Cl.⁷，ＤＢ名) B23Q 33/00 - 35/48 B25J 13/00 - 13/08

Claims (9)

(57)【特許請求の範囲】
1. 【請求項１】作業用ツールの押付け力の目標値ｆroを設
   定する第１の設定手段、ワーク面から作業用ツールに加
   わる力を検出する第１の検出手段、この検出した力と前
   記作業用ツールの押付け力の目標値との偏差をとる比較
   手段、この力の偏差に基づいて作業用ツールの押付け方
   向の速度指令値ｕzを演算する制御演算手段、作業用ツ
   ールの送り方向の速度指令値ｕxyを指令する手段、前記
   押付け方向及び送り方向の速度指令値ｕz,uxyに基づい
   て多自由度作業機械を動かす演算駆動手段を備え、前記
   第１の設定手段、第１の検出手段、比較手段、制御演算
   手段、演算駆動手段により制御ループを構成する、少な
   くとも２自由度を持つ多自由度作業機械を用いた倣い制
   御装置において、 前記力制御ループにおける押付け力の目標値ｆroを補正
   する力目標値補正手段を更に備え、前記力目標値補正手
   段は、前記作業用ツールをワーク面上に前記力の目標値
   ｆroで押付けながら前記速度指令値ｕxyで送るときに、
   その送り方向に対するワーク面の勾配tan αにより生じ
   る作業用ツールの押付け方向の移動速度ｖzofを求める
   第１の演算手段と、その押付け方向の移動速度ｖzofに
   より生じる押付け力の変化量を求め、前記押付け力の目
   標値ｆroからこの押付け力の変化量を差し引き、その値
   を新たな押付け力の目標値として前記比較手段に出力す
   る第２の演算手段とを有することを特徴とする倣い制御
   装置。
2. 【請求項２】作業用ツールの押付け力の目標値ｆroを設
   定する第１の設定手段、ワーク面から作業用ツールに加
   わる力を検出する第１の検出手段、この検出した力と前
   記作業用ツールの押付け力の目標値との偏差をとる比較
   手段、この力の偏差に基づいて作業用ツールの押付け方
   向の速度指令値ｕzを演算する制御演算手段、作業用ツ
   ールの送り方向の速度指令値ｕxyを指令する手段、前記
   押付け方向及び送り方向の速度指令値ｕz,uxyに基づい
   て多自由度作業機械を動かす演算駆動手段を備え、前記
   第１の設定手段、第１の検出手段、比較手段、制御演算
   手段、演算駆動手段により力制御ループを構成する、少
   なくとも２自由度を持つ多自由度作業機械を用いた倣い
   制御装置において、 前記力制御ループにおける押付け方向の速度指令値ｕz
   を補正する速度指令値補正手段を更に備え、前記速度指
   令値補正手段は、前記作業用ツールをワーク面上に前記
   力の目標値ｆroで押付けながら前記速度指令値ｕxyで送
   るときに、その送り方向に対するワーク面の勾配tan α
   により生じる作業用ツールの押付け方向の移動速度ｖzo
   fを求める第１の演算手段と、前記第１の演算手段で求
   めた押付け方向の移動速度ｖzofを前記制御演算手段で
   求めた押付け方向の速度指令値ｕzに加算し、その値を
   新たな速度指令値として前記駆動手段に出力する第２の
   演算手段とを有することを特徴とする倣い制御装置。
3. 【請求項３】請求項１または２記載の倣い制御装置にお
   いて、前記第１の演算手段は、前記ワーク面の勾配tan
   αを演算する手段と、前記速度指令値ｕxyで送られる前
   記作業用ツールの送り速度ｖxyを演算する手段と、前記
   勾配tan αと送り速度ｖxyとから前記押付け方向の移動
   速度ｖzofを演算する手段とを含む倣い制御装置。
4. 【請求項４】請求項３記載の倣い制御装置において、前
   記速度指令値ｕxyを指令する手段はワーク面上を移動す
   る作業用ツールの位置を検出する第２の検出手段を有
   し、前記第１の演算手段の勾配を演算する手段は、前記
   第２の検出手段で検出した作業用ツールのワーク面上の
   現在点を含む２点の位置から前記ワーク面の勾配tan α
   を演算する手段を含む倣い制御装置。
5. 【請求項５】請求項３記載の倣い制御装置において、前
   記速度指令値ｕxyを指令する手段はワーク面上を移動す
   る作業用ツールの位置を検出する第２の検出手段を有
   し、前記第１の演算手段の勾配を演算する手段は、前記
   第２の検出手段で検出した作業用ツールの現在点を含む
   ２点の位置から前記作業用ツールの送り速度ベクトルｖ
   の方向を演算する手段と、前記送り速度ベクトルｖの方
   向と前記第１の検出手段で検出した力の方向とから前記
   ワーク面の勾配tan αを演算する手段とを含む倣い制御
   装置。
6. 【請求項６】請求項１または２記載の倣い制御装置にお
   いて、前記速度指令値ｕxyを指令する手段は前記作業用
   ツールの送り目標速度ｖxyoを設定する手段を有し、前
   記第１の演算手段は、前記ワーク面の勾配tan αを演算
   する手段と、前記勾配tan αと前記送り目標速度ｖxyo
   とから前記押付け方向の移動速度ｖzofを演算する手段
   とを含む倣い制御装置。
7. 【請求項７】請求項６記載の倣い制御装置において、前
   記速度指令値ｕxyを指令する手段はワーク面上を移動す
   る作業用ツールの位置を検出する第２の検出手段を更に
   有し、前記第１の演算手段の勾配を演算する手段は、作
   業用ツールの送り目標位置を予め設定する手段と、前記
   第２の検出手段で検出した作業用ツールのワーク面上の
   現在点の位置と前記設定手段で予め設定した送り目標位
   置との２点の位置から作業用ツールの押付け方向に垂直
   な方向の送り方向ベクトルｍを演算する手段と、前記送
   り方向ベクトルｍと前記第１の検出手段で検出した力の
   方向とから前記ワーク面の勾配tan αを演算する手段と
   を含む倣い制御装置。
8. 【請求項８】請求項１または２記載の倣い制御装置にお
   いて、前記速度指令値ｕxyを指令する手段はワーク面上
   を移動する作業用ツールの位置を検出する第２の検出手
   段を有し、前記第１の演算手段は、前記第２の検出手段
   で検出した作業用ツールのワーク面上の現在点を含む２
   点の位置とその２点の位置の移動時間とから前記押付け
   方向の移動速度ｖzofを演算する手段を含む倣い制御装
   置。
9. 【請求項９】請求項１または２記載の倣い制御装置にお
   いて、前記作業用ツールの送り方向の制御に関し、前記
   作業用ツールの送り目標速度ｖxyoを設定する手段を更
   に備え、前記第１の演算手段は、前記ワーク面の勾配ta
   n αを予め記憶する手段と、前記予め記憶した勾配tan
   αと前記送り目標速度ｖxyoとから押付け方向の移動速
   度ｖzofを演算する手段とを含む倣い制御装置。