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

Abstract

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

Description

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

〔従来の技術〕

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

また近年、［計測自動制御学界論文集Ｊ　ｖｏｌ　２２
、Ｎｏ３　（１９８６）、３４３〜３５０頁、特開昭６
０−３０１０号及び特開昭６１−７９０５号に記載のよ
うに仮想コンプライアンス制御と呼ばれているものがあ
る。これは、バネ・マス・ダンパーを有する力学系モデ
ルを仮想的に実現しようとするもので、力学系の式 ｍ大十Ｃ交十にΔｘ＝ｆ のｍ、ｃ、にの値を任意に与えて、その動作をソフトウ
ェア上で模擬して、その動きを実現するように制御を行
うものである。これを多自由度系で行うために、座標系
の各軸毎にｍ、ｃ、にの値を変えられるようになってお
り、この値を適当に選べば各軸毎に特性を変えることが
出来る。またに＝０にすれば、位置のフィードバックが
なくなり力制御モードになり、ｋ、　　ｃの値を大きく
してカフィードバックをなくすようにすれば、位置制御
モードとなり、いわゆるハイブリッド制御を行うことも
できる。このように仮想コンプライアンス制御は、ハイ
ブリッド制御とコンプライアンス制御を包含した制御方
式ということができる。

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

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

例えば、位置と力の制御により、ロボットが行う作業の
１つにワーク表面を倣って表面研磨やパリ取り等を行な
わせる曲面倣い動作がある。この場合、ロボットの作業
用ツールがワーク表面を倣って動くように制御される。

作業用ツールに加わる力及びモーメントは力センサによ
り検出される。

この曲面倣い動作を仮想コンプライアンス制御を応用し
て行う場合を考える。ここで、作業用ツールの移動方向
をＸ軸方向、作業用ツールの軸線方向を２軸方向とし、
簡単のためｘ−ｚ平面で説明する。例えばＸ軸方向には
に、　　ｃの値を大きくしてかたくし、作業用ツールを
位置制御によって速度Ｖ！で送り、２軸方向にはに＝０
とし位置の拘束を解除し、作業用ツールを力制御により
目標力ｆｒで押し付ける。このとき、力センサにより検
出される力をｆとすると、作業用ツールはｚ軸方向には ｍ※ｔ　　＋ｃｖｔ　　＝ｆ−ｆｒ の式を模擬して動いているので、速度変化のない定常状
態ではＣＶ！　＝ｆ−ｆｔとなり、曲面を倣う速度ｖｘ
を発生するには、それに応じた力偏差Δｆ＝ｆ−ｆｒが
なければならないことが分かる。

ここでＸ方向の送り速度ｖｘを一定とすると、速度ｖ寞
は曲面の勾配に比例する。従って力偏差Δｆは曲面の勾
配に比例した値となり、押付カーｆは曲面の勾配によっ
て変化する。今、倣い動作で許容される力偏差をΔｆＯ
とすれば、定常状態で追従できる勾配は Ｃｖ　ｘ となる。

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

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

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

〔課題を解決するための手段および作用〕上記目的を達
成するため、本発明によれば、ワーク面から作業用ツー
ルに加わる力を検出する手段、この検出した力と予め設
定した作業用ツールの押付は力の目標値ｉｏとの偏差を
とる手段、この偏差に基づいて作業用ツールの速度指令
値ｕｚを演算する手段、この速度指令値ｕｘに基づいて
多自由度作業機械を動かす手段を含む少なくとも１つの
力制御ループと、作業用ツールの少なくとも１つの速度
指令値ｕｘを指令する手段と、この速度指令値ｕｘに基
づいて多自由度作業機械を動かす手段とを備えた、少な
くとも２自由度を持つ多自由度作業機械を用いた倣い制
御装置において、作業用ツールをワーク面上に前記力の
目標値ｆｒ。

で押付けながら前記速度指令値ｕｘで送るときに、その
送り方向に対するワーク面の勾配ｔａｎαにより生じる
作業用ツールの押付は方向の移動速度ＶＸｏ］を与える
（ｐ「ｏｖｉｄｅする）第１の制御手段と、この押付は
方向の移動速度ｖｚｏｌを用いて前記力制御ループの制
御量ｆ＋ｏまたはｕｚを補正し、その速度ｖ　ｘｏｆに
よる力ｆｏ［を相殺する（ｃａｎｃｅｌする）第２の制
御手段とを有する任意曲面の倣い制御装置が提供される
。

一般に、本発明の係わる倣い制御装置においては、作業
用ツールをワーク面上に力の目標値ｆｒ。

で押付けながら速度指令値ｕｘの基づく送り速度Ｖ！で
倣わせる場合、その送り方向に対するワーク面の勾配ｆ
ａｎαにより、押付は方向にｖｚｏｆ　＝Ｖ　１１１１
１αの速度を生じ、目標値ｆｒｏの力とは別に、その速
度Ｖ　！ｉ１１によるｆｏｆ＝ｃｖｘｌａｎα（Ｃは定
数）という押付は方向の力が発生する。

本発明においては、第１の制御手段ではその移動速度ｖ
ｚｏｆが与えられ（ｐｒｏｗｉｄｅされ）、第２の制御
手段ではこの移動速度ｖｘｏｒを用いて力制御ループの
制御量を補正し、その速度ｖ　ｗｏｆによる力ｆ、ｏｌ
を相殺する。このため、作業用ツールの押付は力は目標
値ｆｒｏにほぼ一致し、速度指令値Ｖ！にしたがった所
望の速度で倣うことが可能となる。

なお、「作業用ツールの少なくとも１つの速度指令値ｕ
ｘを指令する手段」は種々の形態をとることができる。

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

第１の制御手段は、前記ワーク面の勾配ｔａｎαを演算
する第１の演算手段と、前記速度指令値ＵＸで送られる
前記作業用ツールの送り速度ｖｘを演算する第２の演算
手段と、前記第１の演算手段で求めた勾配ｔｊｎαと前
記第２の演算手段で求めた送り速度ｖｘとから前記押付
は方向の移動速度ｖｓｏ［を演算する第３の演算手段と
で構成することができる。

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

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

また、第１の制御手段は、前記ワーク面の勾配ｔａｎα
を演算する第４の演算手段と、前記作業用ツールの送り
目標速度ｖ　ｘ７ｏを設定する手段と、前記第４の演算
手段で求めた勾配ｔａｎαと前記送り目標速度Ｖ　Ｉ７
０とから前記押付は方向の移動速度Ｖ　！０１を演算す
る第５の演算手段とで構成してもよい。

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

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

以上は、第１の制御手段はワーク面の勾配ｔａｎαを演
算し、この勾配ｔａｎαを用いて移動速度Ｖｖｏｌを演
算したが、勾配Ｕｎαを用いずに移動速度ｖｖｏ［を演
算してもよい。例えば、第１の制御手段は、ワーク面上
を移動する作業用ツールの位置を検出する手段と、前記
検出手段で検出した作業用ツールのワーク面上の現在点
を含む２点の位置とその２点の位置の移動時間とから前
記押付は方向の移動速度Ｖ　１０１を演算する第６の演
算手段とで構成してもよい。この場合は、勾配を演算す
る必要がないので、演算が容易となり、高速演算処理が
可能となる。

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

また、第１の制御手段は、前記ワーク面の勾配ｔａｎα
を予め記憶する手段と、前記作業用ツールの送り目標速
度ｖ　ｘＴｏを設定する手段と、前記予め記憶した勾配
ｆａｎαと前記送り目標速度ｖ　ｘｙ。

とから押付は方向の移動速度ｖ！０１を演算する第７の
演算手段とで構成してもよい。この場合、予め記憶した
勾配ｔｉｎαを用いるので、リアルタイムでワーク面の
勾配Ｉ！ｎαを演算する上述の場合よりも演算量が少な
くて済み、高速演算処理が可能となる。

一方、第２の制御手段は、前記押付は方向の移動速度ｖ
　ｔｏｊによって生じる力ｆａｔを演算する第８の演算
手段と、この方ｆｏｆを前記力制御ループの押付は力の
目標値ｆｒｏから差し引いて新しい押付は力の目標値ｆ
「を演算する第９の演算手段とで構成することができる
。

また、前記第２の制御手段は、前記押付は方向の移動速
度ｖ　ｔｏｌを速度補正値ＶＩＣとし、この速度補正値
ｖｚｃを前記力制御ループの速度指令値Ｕ！０に加算し
、新たな速度指令値ｕｚを演算する第１０の演算手段で
構成してもよい。

前述したように、作業用ツールをｆａｎαという勾配の
ワーク面上で速度Ｖｌで倣わせた場合、押付は方向にｖ
ｔｏ１＝ｖｘ　ｔａｎαの速度を生じ、その速度ｖ　！
ｏｆによりｆｏｆ＝ｃｖｘ　Ｉｎｎ　ａ　（Ｃは定数）
という力が発生するのであるから、その速度ｖｚｏｆに
よる力ｆｏｆを相殺する（ｃａｎｃｅｌする）には、そ
の制御量である押付は力の目標値ｆｒｏを補正してもよ
いし、速度指令値ｕ！、を補正してもよい。従って、第
２の制御手段は、力ｆｏｆを押付は力の目標値ｆｒｏか
ら差し引いて新しい押付は力の目標値ｆｒを演算するこ
とにより力の目標値ｆｔ。

で押付けながらワーク面を倣わせることができ、また、
押付は方向の移動速度ｖ　！ｏｆを速度補正値ｖｚｃと
し、この速度補正値ｖｘＣを速度指令値ｕｘＯに加算し
、新たな速度指令値ｕｚを演算することによっても同様
の結果を得ることができる。

〔実施例〕

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

第１図において、多自由度作業機械の一例である６自由
度の関節型ロボット１は平部２を有し、平部２には作業
用ツール４が取り付けられている。

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

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

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

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

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

特性補償演算部２１は、減算部の出力Δｔ′にΔ入に制
御上の特性補償演算を行って速度指令値Ｕを出力するも
ので、仮想コンプライアンス制御では、仮想質量行列Ｍ
と仮想粘性係数行列Ｃによって、 Ｍｕ＋Ｃｕ＝Δｆ’　　−にΔｘ　　　　・（１）を模
擬して動くように速度指令値を演算する。

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

力センサ３、力演鼻部９、座標変換部１０、力目標値設
定値７、力目標値補正部７Ａ、力偏差演算部１２、制御
演算部１７、角速度演算部２３、モータ速度演算部２４
、サーボアンプ部２５は力制御ループを構成し、エンコ
ーダ５、角度演算部１３、位置演算部１４、位置目標値
設定部８、位置偏差演算部１５、制御演算部１７、角速
度演算部２３、モータ速度演算部２４、サーボアンプ部
２５は位置制御ループを構成する。

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

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

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

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

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

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

さらにこの力演鼻部９では、作業用ツール４の重力分を
差し引いて重力補償を行い、手先の姿勢によって変化す
る作業用ツール４の重力の影響を取り除く。

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

このようにして得られたベース座標系での力ｔは、押付
は力演鼻部６３で求められた押付は力の目標値ｆｒと比
較され、力偏差演算部１２によって力偏差Δｔが演算さ
れる。

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

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

このように位置演算部１４で演算された位置及び姿勢Ｘ
は、設定部８で設定された位置及び姿勢目標値Ｘ「と比
較され、演算部１５で位置及び姿勢の偏差ΔＸが計算さ
れる。このうち姿勢の偏差は、目標姿勢と実際の姿勢と
の間の回転角を表すベクトルで表わせ、目標手先座標系
の姿勢を表す行列を（ｉｈｔ、　ｊｈｒ、　　ｋｈｒ）
とすれば姿勢の偏差はＩＦｅで表せる。ここでｅは回転
軸の単位ベクトルであり、マは軸廻りの回転角であり、
それぞれ以下のように表わされる。

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

即ち、不感帯演算部１８では力偏差Δｆの各座標軸毎に
不感帯演算をしてΔｆ′　を算出し、バネ定数乗算部１
９では各座標軸毎に設定した仮想バネ定数行列Ｋを位置
偏差ΔＸに乗じてにΔλを演算し、減算部２０では不感
帯演算を行った力の偏差Δ１′からにΔＸを減算する。

特性補償演算部２１では、減算部の出力Δｆ’　　−に
Δ入に制御上の特性補償演算を行って速度指令値Ｕを出
力し、仮想コンプライアンス制御では、仮想頁行列Ｍと
仮想粘性係数行列Ｃによって前述した（＋）式を模擬し
て動くように速度指令値を演算する。この演算は、実際
は例えば（１）式を離散系で変形したＵ　＝ΔｔＭ’（
Δｆ’　−にΔＸ） ＋（１−Δｔ　Ｍ−’Ｃ）　ｕ　　　　−（２）の形で
行う。ここでΔｔはサンプリング周期であり、Ｕ　はｎ
回目のサンプリングを表す。またＵは並進速度及び回転
速度を表わす６次元ベクトル、Δ貫′　は力及びモーメ
ントの偏差を表わす６次元ベクトル、Δλは位置及び姿
勢の偏差を表す６次元ベクトル、Ｍ、Ｃ，には６×６の
行列で、特に対角行列を用いる。

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

位置と力の制御演算部１７で演算された速度指令値Ｕは
ロボット１を動かすための指令値である。

この速度指令値Ｕは角速度演算部２３によってロボット
の各関節の角速度台に変換され、さらにモータ速度演算
部２４で、モータの回転速度に変換した後、サーボアン
プ２５によってこの速度で動くようにロボット１を制御
する。

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

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

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

一方、位置制御ループでは、位置及び姿勢の目標値ＸＩ
と検出値に基づく位置及び姿勢入との偏差Δ入から制御
演算部１７で速度指令値ＵのＸ軸成分ｕｘが演算され、
作業用ツール４はこの速度指令値Ｕ！に基づいた送り速
度Ｖ！でＸ軸方向に送られる。

このような状態において、速度指令値Ｕ！により得られ
る作業用ツール４の送り方向に対するワーク面の傾斜角
をαとし、その勾配を【■αで表わすと、作業用ツール
４はワーク面上に力の目標値ｆｒｏで押付けられながら
速度指令値ｕｘに基づく速度Ｖ！で送られているのであ
るから、作業用ツール４には送り方向のワーク面の勾配
ｔａｎαによりｚ軸方向に ｖ　ｚｏｆ　＝　ｖ　ｘｊａｎα の速度を生じる。従って、この速度Ｖ！０１によって、
目標値ｆｉＯの力とは別に ｆ　ｏｌ＝　Ｃｖ　！ｏｆ という押付は力が発生する。但し、Ｃは仮想コンプライ
アンス制御で設定した粘性減衰係数である。

このため、力目標値設定部７で押付は力の目標値ｆｒＯ
を設定したにも係わらず、実際にはｆ　＝　ｆ　ｒ。

＋　ｆ　ｏｆの押付は力が生じる。従って、設定した押
付は力ｆｒｏを得るためには、押付は力の目標値ｆｒｏ
から力ｆｏｆを差し引いた値を押付は力の指令値とすれ
ばよい。即ち、この押付は力の指令値をｆｒとすれば、
その指令値ｆｒは次式で求まる。

ｆ　ｒ　＝　ｆ　ｔｏ−Ｃｖｘｌａｎα本実施例は以上
の知見に基づくものであり、第１図に示す勾配演算部６
１、送り速度演算部６２及び押付は力演鼻部６３で上記
演算を行ってやることにより、作業用ツール４を一定の
押付は力でワーク面を倣うように制御する。

即ち、第４図に示すように、ワーク５０の表面を倣いな
がら記憶部６０に記憶された現在の位置を点Ｐｉ　　（
ｘｊ、ｙｊ、ｚｉ）とし、その１つ前の位置を点Ｐｉ　
　（ｘｉ、ｙｉ、ｚｉ）とすると、点Ｐｊでのワーク面
の勾配ｔａｎαは次式で近似することができる。

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

又、第４図において、点Ｐｊでの作業用ツール４の送り
速度ＶのＸ軸方向成分Ｖｌ及びｙ軸方向成分ｖ７は、 Ｖｌ　：　（ｘｉ　−ｘｉ　）　／ΔｔＶＴ　＝Ｃ’ｌ
ｒ　−３’ｉ　）　／Δｔで表わされる。ここでΔｔは
、点Ｐｉ　と点Ｐｊの間の検出時間で・ある。Δｔとし
ては、記憶部６０で一定の時間間隔Δｔで手先の位置を
順次記憶するようにし、その時間間隔Δｔを送り速度演
算部６２に記憶しておけばよい。従って、ｘ−ｙ平面に
平行な方向の送り速度の大きさＶＩＦは次式で近似する
ことができる。

ｖｘ７＝ｆ■丁０ゴｊコ１− 送り速度演算部６２は上式の演算を行う。なお、第３図
に示す上述した例では送り速度のＸ軸方向成分のみが演
算されている。

そして押付は力演鼻部６３においては、勾配演算部６１
で演算した勾配ｔａｎαと送り速度演算部６２で演算し
た送り速度ＶＩ７と予め設定した粘性減衰係数Ｃとから
速度ｖ！ｙによって２軸方向に生じる力ｆ　ｏｆ＝　Ｃ
ｖ　ｘ７１ｘｎαを演算し、この力ｆｏｆと内口標値設
定部７で設定した内口標値ｆ＋ｏとから新たな押付は力
の目標値’ｆｔを、上述した式ｆ　ｔ　＝　ｆ　ｒｏ−
ＣＶ！！　ｌａｍαより演算する。

このようにして押付は力演鼻部６３で求めた新たな押付
は力の目標値ｆｔは、内口標値ｆｒとして力偏差演算部
１２に送られ、前述した力偏差Δｔを求めるのに使用さ
れる。

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

又、曲面倣い動作を行う場合、ワーク面に対して一定の
押付は力で作業用ツール４を押し付ける手法としては移
動のための座標変換を行う等の手法も考えられるが（特
願昭６３−４８４８６−ＵＳ　　５ｅｒｉａｌ　　Ｎｏ
、　　３１６．　６９６及びＥＰＣｐａｌｅｎｔ　ａｐ
ｐｌｉｃａｔｉｏｎ　ｎｏ、８９２００４９５Ｊ対応−
参照）、本実施例ではワーク面の形状に合わせて押付は
力のみを制御するため、そのような手法に比べて座標変
換等の複雑な演算を必要とせず、高速演算処理が可能と
なる。

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

本実施例の倣い制御装置における内口標値補正部７Ｂは
、第５図に示すように、第２図に示す第１の実施例の構
成に加えて、作業用ツール４の倣い動作開始時の送り目
標位置を記憶する記憶部７０と、位置演算部１４で演算
された位置と記憶部７０に記憶された送り目標位置から
送り方向ベクトル１を演算する送り方向ベクトル演算部
７１と、作業用ツールの倣い動作開始時の送り目標速度
ＶＨａを設定する設定部６２Ａとを有している。勾配演
算部７２は、第１の実施例における勾配演算部６１の機
能に加えて、第１図に示す力演鼻部９及び座標変換部１
０で求めた作業用ツール４に作用する反力ベクトルｔと
送り方向ベクトル演算部７１で求めた送り方向ベクトル
１とからワーク面の勾配ｔａｎαを演算する機能を有し
、押付は力演鼻部６３は、勾配演算部７２で求めたワー
ク面の勾配ｊｔｎαと、送り速度演算部６２で求めた送
り速度ｖｘ７又は設定部６２Ａに設定された送り目標速
度Ｖ　！ＦＯと、内口標値設定部７に設定された内口標
値ｆｒｏとから新たな押付は力の目標値ｔ「を演算する
。

なお、設定部６２Ａの送り目標速度Ｖ　１７０は位置目
標値設定部８に送られ、現在の目標位置にτｖ　！Ｙｏ
を加算し、新たな目標位置ｘｒをサンプリングタイムτ
毎に順次計算する。この演算は後述する第１７図に示す
実施例の移動位置目標値演算部９７と同じであるので、
詳細は省略する。

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

次に、本実施例の動作を第６図を参照して説明する。第
６図は本実施例における内口標値補正部７Ｂにおけるワ
ーク面の勾配ｔａｎαの与え方を示す図であり、作業用
ツールをワーク５０の表面に押付け、倣い動作を開始す
る状態を示している。

第６図において、位置演算部１４で演算された現在の位
置を点Ｐ　（ｘｐ、ｙｐ、ｚｐ）とし、記憶部７０で記
憶された作業用ツールの送り目標位置を点Ｑ　（ＸＱ、
ＶＱ、ＺＱ）とする。この点Ｐ及び点Ｑから作業用ツー
ル４の送り方向を示す送り方向ベクトル１を求めると、
ｒｔｎ　（ＸＱ　−ｘｐ　。

ｙｑ　−ｙｐ　、Ｏ）となる。送り方向ベクトル演算部
７１ではこの演算を行う。

次に、点Ｐにおけるワーク面の押付反力ベクトルｔを法
線ベクトル１に一致すると仮定すると、法線ベクトルｎ
の成分は反力ベクトルｔのｘｙｚ方向の３分力（ｆｘ　
、ｆ７　、ｆｚ）で表わされる。

又、点Ｐにおけるワーク面上での移動方向の単位ベクト
ルをｔとし、その方向余弦を（ｔｘ、ｔ７゜ｔＩ）で表
わす。ベクトルｔは点Ｐにおけるワーク面の接線ベクト
ルであるから、法線方向ベクトルｎと直交する。従って
、互いに直交するベクトルの内積は０であるから、次式
が成り立つ。

１・１＝０ よって、 ｆｘ　　・ｔｘ　　＋ｆ７　−ｔ７　　＋ｆｘ　　ｅｔ
ｔ　　＝０・・・（３） 今、ｘ−ｙ平面に垂直で点Ｑを通る直線と、ベクトルｔ
の延長線が交わる点をＱ′　とし、点Ｐと点Ｑ′　の距
離をＥとすると、ベクトルＰＱ’　の成分はＣ１ｔｘ、
ｌ　ｔｙ、ｌ　ｔｓ）で表わされる。

従って、点Ｐにおけるワーク面の勾配ｔａｎαは送り方
向ベクトル１に対するベクトルｔのなす勾配に一致し、
ワーク面の勾配ｆａｌｌαは次式で求めることができる
。

ｔａｎ　　ａ＝ｌ　　ｔｓ　／’　　　（ｌｔｘ　）　
２＋　（ｌｔ７）’・・・（４） ここで（３）式の両辺にｌをかけてＡ’ｔｔについて整
理すると、 ｌ　　ｔ！＝　−１（ｆｔ　　ｔｘ　　＋ｆ７　　ＬＹ
　）　／ｆ！・・・（５） 又、Ａ’ｌ、ｊ！ｔＹは法線方向ベクトルｎの成分より ｌ　　ｔｘ　　＝ｘｑ　　−ｘｐ ｌ　　ｔｙ　　＝ｙｑ　　−ｙｌｌ　　　　　　　・・
・（６）従って、（５）式及び（６）式を用いて（４）
式を整理すると、 ｔａｎ　　α ・・・（７） が得られる。

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

押付は力演鼻部６３においては、勾配演算部７２で上記
のようにして求めた送り方向に対するワーク面の勾配ｆ
ａｎαと送り目標速度設定部６２Ａに設定された送り目
標速度ｖ　ｘ７ｏと上目標値設定部７に設定された上目
標値ｆｒｏとを用いて、第１の実施例と同様に、ｆ　ｔ
　＝　ｆ　ｒｏ−Ｃｖｘｙｏ　Ｕｎ　ａを演算し、押付
は力の指令値として新たな押付は力の目標値ｆｒを演算
する。従って、第１の実施例と同様、送り目標速度Ｖ　
！７Ｇによって生じる押付は力の変化分が差し引かれ、
作業用ツール４を目標値ｔｒｏの力で押付けながらワー
ク面を倣わせることができる。

以上のようにして、倣い動作開始時における作業用ツー
ル４が停止状態から移動開始直後までは、記憶部７０に
記憶された送り目標位置と設定部６２Ａに設定された送
り目標速度ｖ　ｘ７ｏを用いて押付は力の目標値を演算
し、位置と力の制御を行う。

−旦作業用ツール４が動き始めた後は、第１の実施例と
同様な位置と力の制御を行う。即ち、勾配演算部７２は
、記憶部６０に記憶された位置データをもとに送り方向
に対するワーク面の勾配ｔａｎαを演算し、送り方向速
度演算部６２は、記憶部６０で記憶した位置情報を用い
て送り速度ｖＢを演算し、押付は力演鼻部６３は、勾配
演算部７２で求めたワーク面の移動方向の勾配ｔａｎα
と送り速度演算部６２で求めた送り速度ｖ！Ｙと上目標
値設定部７に設定された上目標値ｔｒｏとを用いて押付
は力の指令値ｆ＋を演算する。

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

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

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

本実施例の倣い制御装置における上目標値補正部７Ｃは
、第７図に示すように、位置演算部１４で演算された位
置を順次記憶する記憶部６０と、記憶部６０で記憶した
位置から作業用ツール４の押付は方向に垂直な方向の送
り速度ベクトルＶを演算する送り速度演算部８０と、力
演鼻部９及び座標変換部１０で演算された作業用ツール
４に作用する反力ベクトル１の方向と演算部８０で求め
た送り速度ベクトルＶの方向とから送り方向に対するワ
ーク面の勾配ｔａｎαを演算する勾配演算部８１と、演
算部８１で求めた勾配ｔａｎαと演算部８０で求めた送
り速度ベクトルＶの大きさと方間標値設定部７に設定さ
れた力目標値ｆ＋ｏとから新たな押付は力の目標値１「
を演算する押付は力演鼻部６３とからなる。押付は力演
鼻部６３で求めた新たな押付は力の目標値ｆｒは力偏差
演算部１２に出力される。

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

次に、本実施例の動作を説明する。第４図を再び参照し
て、作業用ツール４でワーク５０の表面を倣っている状
態を考える。このとき、記憶部６０には任意の時間Δを
毎に位置演算部１４で演算された作業用ツール４の位置
が記憶される。また、第４図において、Ｐｉは作業用ツ
ール４の現在の位置、Ｐｉはそ°の１つ前の位置である
。この２つの位置から、ｘ−ｙ平面に平行な方向の送り
速度ベクトルＶの成分（ｖｘ　、　　ｖｙ　、　　ＶＺ
　）は、ｖｘ　＝　（ｘｉ　−ｘｉ　）　／Δｔｖｙ　
＝　（ｙｉ　−ｙｉ　）　／Δｔ■２＝０　　　　　　
　　　　　　・・・（８）となる。又、その平均速度は １ｖｌ＝Ｆ７「７下■〒”　　　　　　　−（９）とな
る。これは送り速度ベクトルＶの大きさに一致し、第１
の実施例で説明した送り速度ｖｘｙと同じ値である。従
って、以下、送り速度をＶＺ７と表示する。送り速度演
算部８０では以上の演算を行う。

第８図は本実施例の勾配演算部８１におけるワーク面の
勾配ｔａｎαの与え方を示す図である。第８図において
、現在の検出位置を第４図と同様に点Ｐｉとし、Ｐｊ点
の送り速度ベクトルを上述のＶ　（Ｖｌ、ｖ７，０）で
表わしている。今、Ｐｊ点におけるワーク面の押付は反
力ベクトルｆ（ｆｘ、ｆｙ、ｆｘ）を法線方向ベクトル
と仮定する。

又、Ｐｊ点におけるワーク面上の移動方向単位ベクトル
を１、その方向余弦を（ｔｘ　、　　ｔｙ　、　　ｔｘ
　）で表わす。ベクトルｔは点Ｐｉにおけるワーク面の
接線ベクトルであるから、反力ベクトルｆと直交する。

従って、互いに直交するベクトルの内積は０であるから
、次式が成り立つ。

１・１＝０ よって、 ｆｘ　　φｌ　　＋ｆ７　　＊　　ｔ７　　＋ｆｔ　　
・ｔｚ　　＝０・・・（１０） 次ニ、単位ベクトル１にｌをかけ、ベクトルｌｔをｘ−
ｙ平面に投影したときの値が送り速度ベクトルＶと一致
するようにｌを定める。このときベクトル１１の成分は
（Ａ’　ｔｘ、ｌ　ｔ７．ｌ　ｔｘ）で表わされる。従
って、点Ｐｉにおける勾配ｔａｎαは送り速度ベクトル
Ｖに対するベクトルｔのなす勾配に一致し、倣い勾配ｔ
ａｎαは次式で求めることができる。

ｆａｎ　ａ＝ｌ　Ｌｘ　／　　　ｌ　ｔｘ　）　２＋　
（１ｔｙ）２・・・（１１） ここで（１０）式の両辺にｌをかけてｌｔ！について整
理すると、 ｌ　ｔ！＝　−１（ｆｘ　ｔｘ　＋ｆｙ　ｔｙ　）　／
ｆｚ・・・（１２） 又、ｌｔｘ、Ａ’ｔｙは送り速度ベクトルＶの成分に一
致することから、 ｌ　ｔ！＝ｖｘ １１　＝ｖｒ　　　　　　　　　−（１３）従って、（
１２）式及び（１３）式を用いて（１１）式を整理する
と、 ｔａｎ　　α １（ｘｉ　　−ｘｉ）＋ｆｒ（Ｙ　ｊ　　　ｙｉ）・・
・（１４） が得られる。勾配演算部８１では以上の演算を行う。

押付は力演鼻部６３においては、第１の実施例と同様に
、送り速度Ｖ１７と勾配ｔａｎαと力目標値ｔｔｏとか
らｆ　ｒ　＝　ｆ　ｔｏ−Ｃｖｘ７１ａｎａを演算し、
新たな押付は力の目標値ｔ【を求め、作業用ツール４を
一定の押付は力でワーク面を倣うように制御する。

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

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

本実施例の倣い制御装置における力目標値補正部７Ｄは
、位置演算部１４で求めた手先の位置及び姿勢Ｘの位置
データを手先の移動に従って一定時間ごとに順次記憶す
る記憶部６０と、記憶部６０に記憶された位置データを
もとに作業用ツール４の押付は方向の移動速度ｖｘｏｌ
を演算する移動速度演算部８２と、移動速度演算部８２
で求めた移動速度ｖ　ｘｏｌ及び力目標値設定部７で設
定した押付は力の目標値ｔｒｏとから新たな押付は力の
目標値ｆ＋を演算する押付は力演鼻部８３とからなる。

押付は力演鼻部８３で求めた新たな押付は力の目標値ｆ
ｒは力偏差演算部１２に出力される。

ここで、記憶部６０で記憶される位置データは、一定の
時間毎に記憶され、その数は移動速度演算部８２で用い
る範囲とする。

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

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

ここでΔｔは点Ｐｉ　と点Ｐｉの間の時間である。

時間Δｔは移動速度演算部８２に予め記憶しておく。移
動速度演算部８２は上式の演算を行う。

押付は力演鼻部８３においては、移動速度演算部８２で
演算した移動速度ｖｚｏｌと予め設定した粘性減衰係数
Ｃとから送り速度によって２軸方向に生じる力ｆ　ｏｌ
＝　Ｃｖ　ｘｏｌを演算し、この方ｆｏｆと力目標値設
定部７で設定した力目標値ｆｒｏとから新たな押付は力
の目標値ｆ「を式ｆｔ＝ｆ＋ｏ−Ｃｖｚより演算する。

即ち、第１〜第３の実施例で勾配ｊａｎαと送り速度Ｖ
Ｄを用いてＺ軸方向に生じる力を演算した代わりに、ｚ
軸方向の移動速度Ｖ　冨０１から直接２軸方向に生じる
力ｆｏｆを演算するものである。

このようにして押付は力演鼻部８３で求めた新たな押付
は力の目標値ｆｒは力目標値ｔ「とし力偏差演算部１２
に送られ、力偏差ΔＩを求めるのに使用される。

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

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

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

第１０図において、本実施例の倣い制御装置６Ａは、制
御演算部１７の特性補償演算部２１で速度指令値ＵＯを
演算し、この速度指令値ｕｏを速度指令値補正部９０Ａ
で補正し、新たな速度指令値Ｕを得て、この速度指令値
Ｕが角速度演算部２３で使用される。偏差演算部１２に
は力目標値設定部７から直接力目標値ｔ【が送られる。

速度指令値補正部９０Ａは、第１１図に示すように、第
２図に示す第１の実施例の力目標値補正部７Ａと同様、
位置演算部１４で求めた手先の位置及び姿勢Ｘの位置デ
ータを手先の移動に従って順次記憶する記憶部６０と、
記憶部６０に記憶された位置データをもとに作業用ツー
ル４の送り方向に対するワーク面の勾配ｔａｎαを演算
する勾配演算部６１と、位置演算部１４で求めた位置と
その検出時間Δｔとから作業用ツール４の送り速度ｖｘ
７を演算する送り速度演算部６２とを有している。速度
指令値補正部９０Ａは、さらに、勾配演算部６１で求め
た勾配ｔａｎαと送り速度演算部６２で求めた送り速度
ｖＨとから押付は方向の速度補正値ｖｚｃを演算する補
正速度演算部９０と、制御演算部１７で求めた速度指令
値ｕｏの２軸成分ｕｓｏに演算部９０で求めた速度補正
値ＶＺＣを加算して新たな速度指令値Ｕを求め、これを
角速度演算部２３に出力する加算器９１とを有している
。

第３図及び第４図を参照して説明したように、力目標値
７で２軸方向に押付は力の目標値ｆｒを与えながらｘ−
ｙ平面に平行な方向に速度ｖｘ７で作業用ツール４を移
動させるとき、送り方向の勾配ｔａｌｌαにより作業用
ツール４にはＶ２Ｏ３＝ｖｘｙｌａｎａ の２軸方向の速度を生じ、この速度ｖ　ｇｏｔによって ｆ　ｏｆ＝　ＣＶ　！０１ という力が発生する。本実施例では、第１１図に示す演
算部分で直接この速度ｖ！０１を演算し、これを押付は
方向の速度補正値として加算部９１に加えることにより
、２軸方向の速度変化分ｖ　ｓｏｌの影響を除去し、ワ
ーク面を倣わせるものである。

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

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

補正速度演算部９０では、演算部６１で演算された勾配
ｔａｎαと演算部６２で演算された送り速度ＶＩＦとか
ら次式の計算によりＺ軸方向の速度Ｖ！ｏ１を求める。

Ｖ　！０１　＝　Ｖ　！ｙＩａｎａ そして、この速度ｖ　ｗｏ［を速度補正値ｖｘｃと置き
、これを加算部９１で加える。

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

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

第６の実施例 本発明の第６の実施例を第１２図を参照して説明する。

図中、第５図、第１０図及び第１１図に示す部材と同等
の部材には同じ符号を付している。

本実施例は、第５の実施例の速度指令値補正部における
勾配ｔｉｎαの演算手段を第５図に示した第２の実施例
における勾配ｔｉｎαの演算手段に置き換えたものであ
る。即ち、本実施例の速度指令値補正部９０Ｂは、第１
２図に示すように、第５図に示した実施例の力目標値補
正部７Ｂと同様、記憶部６０と、送り速度演算部６２と
、送り目標位置を記憶する記憶部７０と、位置演算部１
４で演算された位置と記憶部７０に記憶された送り目標
位置から送り方向ベクトルｍを演算する送り方向ベクト
ル演算部７１と、力演鼻部９及び座標変換部１０で演算
した作業用ツール４に作用する反力ベクトル１と演算部
７１で求めた送り方向ベクトルｍとからワーク面の勾配
ｔａｎαを演算する勾配演算部７２と、送り目標速度Ｖ
　１７０を設定する設定部６２Ａとを有している。速度
指令値補正部９０Ｂは、さらに、勾配演算部６１で求め
た勾配ｔａｎαと送り速度演算部６２で求めた送り速度
Ｖ１？又は設定部６２Ａに設定された送り目標速度Ｖｘ
ｙｏとから押付は方向の速度補正値ｖ！ｃを演算する補
正速度演算部９０と、制御演算部１７で求めた速度指令
値ｕｏの２軸成分ｕ！Ｏに演算部９０で求めた速度補正
値ｖ！ｃを加算して新たな速度指令値Ｕを求め、これを
角速度演算部２３に出力する加算器９１とを有している
。

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

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

第７の実施例 本発明の第７の実施例を第１３図を参照して説明する。

図中、第７図、第１０図及び第１１図に示す部材と同等
の部材には同じ符号を付している。

本実施例は、第５の実施例の速度指令値補正部における
勾配ｔａｎαの演算手段を第７図に示した第３の実施例
における勾配計■αの演算手段に置き換えたものである
。即ち、本実施例の速度指令値補正部９０Ｃは、第１３
図に示すように、第７図に示す実施例の力目標値補正部
７Ｃと同様、位置演算部１４で演算された位置を順次記
憶する記憶部６０と、記憶部６０で記憶された位置から
作業用ツール４の送り速度ベクトルＶを演算する送り速
度演算部８０と、力演鼻部９及び座標変換部１０で演算
した作業用ツール４に作用する反力ベクトルｆの方向と
送り速度ベクトルマの方向とからワーク面の勾配ｌ！ｎ
αを演算する勾配演算部８１とを有している。速度指令
値補正部９０Ｃは、さらに、勾配演算部８１で求めた勾
配ｔａｎαと送り速度演算部８０で求めた送り速度ベク
トルＶの大きさとから押付は方向の速度補正値ｖＩＣを
演算する補正速度演算部９０と、制御演算部１７で求め
た速度指令値ｕｏの２軸成分ｕ！ｏに演算部９０で求め
た速度補正値ｖｘｃを加算して新たな速度指令値Ｕを求
め、これを角速度演算部２３に出力する加算器９１とを
有している。

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

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

第８の実施例 本発明の第８の実施例を第１４図を参照して説明する。

図中、第９図、第１０図及び第１１図に示す部材と同等
・の部材には同じ符号を付している。

本実施例は、第５の実施例の速度指令値補正部における
速度補正値ｖ寞Ｃの演算手段を第９図に示した第４の実
施例における作業用ツールの押付は方向の移動速度ｖｓ
ｏｆを演算する手段に置き換えたものである。即ち、本
実施例の速度指令値補正部９０Ｄは、第１４図に示すよ
うに、位置演算部１４で求めた手先の位置及び姿勢λの
位置データを手先の移動に従って一定時間ごとに順次記
憶する記憶部６０と、記憶部６０に記憶された位置デー
タをもとに作業用ツール４の押付は方向の移動速度ｖｚ
ｏｆを演算し、これを速度補正値ｖｓｃとする移動速度
演算部８２と、制御演算部１７で求めた速度指令値ｕＯ
の２構成分ｕスＯに演算部８２で求めた速度補正値ｖＩ
Ｃを加算して新たな速度指令値Ｕを求め、これを角速度
演算部２３に出力する加算器９１とを有している。

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

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

第９の実施例 本発明の第９の実施例を第１５図を参照して説明する。

図中、第１図、第２図及び第５図に示す部材と同等の部
材には同じ符号を付している。

第１の実施例では、位置演算部１４で求めた位置から送
り速度演算部６２において作業用ツール４の実際の送り
速度ｖＢを演算し、この送り速度ＶＸ７を用いて演算部
６３で押付は力の目標値ｆｒを演算した。本実施例は、
送り速度演算部６２を用いずに、送り目標速度設定部６
２Ａを用いるものであり、その機能は第２の実施例の送
り目標速度設定部６２Ａと同じである。押付は力演鼻部
６３では、演算部６１で求めた勾配ｔａｎαと設定部６
２Ａで設定した送り目標速度ｖ　ｘ７ｏと上目標値設定
部７で設定した押付は力の目標値ｔｔｏとから新たな押
付は力の目標値ｆｒを演算する。

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

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

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

第１６図において、本実施例の倣い制御装置６Ｃは、ワ
ーク面の勾配と作業用ツールの移動軌跡を予め教示デー
タとして記憶しておく位置及び内口標値演算部７Ｆを有
し、この目標値演算部７Ｆでは、再生時に、記憶した教
示データと、設定部７に予め設定した作業用ツール４の
押付力の目標値ｆｒｏと、送り目標速度設定部６２Ａに
予め設定した作業用ツール４の送り目標速度Ｖ　１７０
とから力及びモーメントの目標値ｉｔと移動位置目標値
Ｘｒとを演算する。演算部７Ｆで演算した力及びモーメ
ントの目標値１ｒと移動位置目標値ｘｒとはそれぞれ力
偏差演算部１２及び位置偏差演算部１５に送られ、力偏
差Δを及び位置偏差ΔＸが求められる。倣い制御装置６
Ｃの他の構成は第１の実施例と同じである。

位置及び内口標値演算部７Ｆは、第１７図に示すように
、位置演算部１４で求めた手先の位置及び姿勢Ｘの位置
データを手先の移動に従って順次記憶する記憶部６０と
、位置演算部１４で求めた位置及び姿勢入の位置データ
をもとに作業用ツール４の送り方向に対するワーク面の
勾配ｔａｎαを演算する勾配演算部６１と、勾配演算部
６１で求めた勾装置１１ｎαを作業用ツール４の移動に
従って順次記憶する勾配記憶部９５と、記憶部９５で記
憶された勾配ｔａｎαと送り目標速度設定部６２Ａに設
定された送り目標速度Ｖ　！７０と上目標値設定部７に
設定した押付は力の目標値１「０とから新たな押付は力
の目標値ｆ＋を演算する押付は力演鼻部６３と、記憶部
６０に記憶された位置データＸと送り目標速度設定部６
２Ａに設定された送り目標速度Ｖ　１７Ｇとから、作業
用ツール４の送り速度が当該設定された送り目標速度と
なるように１サイクル毎の移動位置目標値ｘｒを演算す
る移動位置目標値演算部９７とからなっている。

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

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

次に、記憶部９５．６０に記憶された勾配及び位置デー
タを用いて再生を行う。即ち、押付は力演環部６３では
、記憶部９５で記憶された勾装置ａｎαと送り目標速度
設定部６２Ａに設定された送り目標速度Ｖ　１７Ｇと上
目標値設定部７に設定された押付は力の目標値ｔｕとを
用いて、ワーク面を目標値ｆｒｏの力で押付けるのに必
要な目標値ｆｒを前述した実施例と同様に、ｔ　ｒ　＝
　ｆ　ｔｏ　−Ｃｖ　ｘｌｏ　ｔａｎαの式から演算す
る。

又、移動位置目標値演算部９７では、記憶部６０に記憶
された位置を通り、送り目標速度設定部６２Ａに設定さ
れた送り速度Ｖ　１７０で作業用ツール４が移動するよ
うに移動位置目標値ｘｒを演算する。

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

実際には上記移動ベクトルｉｘ７を考慮する必要があり
、移動ベクトル１！ｙの長さと各軸方向成分の比をとり
、移動ベクトルＩＢ方向の移動量ΔｌのＸ軸及びｙ軸方
向の成分を演算する。即ち、！　！ Δ／ｘ＝ｖＢｏ・τ・ａ□工。口。

を演算し、各軸の方向成分としての移動量を求める。そ
してこれを位置データに加算することにより、移動位置
目標値Ｘｒを求める。

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

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

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

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

〔発明の効果〕

本発明によれば、ワーク面の勾配ｆａｎαにより生じる
作業用ツールの押付は方向の移動速度ｖｔ。

１を与え、この押付は方向の移動速度ｖ　ｔｏｌを用い
て力制御ループの制御量ｆｒｏまたはｖｚを補正するの
で、作業用ツールの押付は力は目標値ｆ＋。

にほぼ一致し、曲面の勾配のいかんに係わらず所望の送
り速度で倣うことができ、適切かつ効率的な倣い作業を
行うことができる。

【図面の簡単な説明】

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

Claims (10)

【特許請求の範囲】
1. （１）ワーク面から作業用ツールに加わる力を検出する
   手段、この検出した力と予め設定した作業用ツールの押
   付け力の目標値ｆ＿ｒ＿ｏとの偏差をとる手段、この偏
   差に基づいて作業用ツールの速度指令値ｕ＿ｚを演算す
   る手段、この速度指令値ｕ＿ｚに基づいて多自由度作業
   機械を動かす手段を含む少なくとも１つの力制御ループ
   と、作業用ツールの少なくとも１つの速度指令値ｕ＿ｘ
   を指令する手段と、この速度指令値ｕ＿ｘに基づいて多
   自由度作業機械を動かす手段とを備えた、少なくとも２
   自由度を持つ多自由度作業機械を用いた倣い制御装置に
   おいて、 作業用ツールをワーク面上に前記力の目標値ｆ＿ｒ＿ｏ
   で押付けながら前記速度指令値ｕ＿ｘで送るときに、そ
   の送り方向に対するワーク面の勾配ｔａｎαにより生じ
   る作業用ツールの押付け方向の移動速度ｖ＿ｚ＿ｏ＿ｆ
   を与える（ｐｒｏｖｉｄｅする）第１の制御手段と、 この押付け方向の移動速度ｖ＿ｚ＿ｏ＿ｆを用いて前記
   力制御ループの制御量ｆ＿ｒ＿ｏまたはｕ＿ｘを補正し
   、その速度ｖ＿ｚ＿ｏ＿ｆによる力ｆ＿ｏ＿ｆを相殺す
   る（ｃａｎｃｅｌする）第２の制御手段と を有する倣い制御装置。
2. （２）請求項１記載の倣い制御装置において、前記第１
   の制御手段は、 前記ワーク面の勾配ｔａｎαを演算する第１の演算手段
   と、 前記速度指令値ｕ＿ｘで送られる前記作業用ツールの送
   り速度ｖ＿ｘを演算する第２の演算手段と、前記第１の
   演算手段で求めた勾配ｔａｎαと前記第２の演算手段で
   求めた送り速度ｖ＿ｘとから前記押付け方向の移動速度
   ｖ＿ｚ＿ｏ＿ｆを演算する第３の演算手段と を含む倣い制御装置。
3. （３）請求項２記載の倣い制御装置において、前記第１
   の演算手段は、 ワーク面上を移動する作業用ツールの位置を検出する手
   段と、 前記検出手段で検出した作業用ツールのワーク面上の現
   在点を含む２点の位置から前記ワーク面の勾配ｔａｎα
   を演算する手段と を含む倣い制御装置。
4. （４）請求項２記載の倣い制御装置において、前記第１
   の演算手段は、 ワーク面上を移動する作業用ツールの位置を検出する手
   段と、 前記検出手段で検出した作業用ツールの現在点を含む２
   点の位置から前記作業用ツールの送り速度ベクトルｖの
   方向を演算する手段と、 前記送り速度ベクトルｖの方向と前記力検出手段で検出
   した力の方向とから前記ワーク面の勾配ｔａｎαを演算
   する手段と を含む倣い制御装置。
5. （５）請求項１記載の倣い制御装置において、前記第１
   の制御手段は、 前記ワーク面の勾配ｔａｎαを演算する第４の演算手段
   と、 前記作業用ツールの送り目標速度ｖ＿ｘ＿ｙ＿ｏを設定
   する手段と、 前記第４の演算手段で求めた勾配ｔａｎαと前記送り目
   標速度ｖ＿ｘ＿ｙ＿ｏとから前記押付け方向の移動速度
   ｖ＿ｚ＿ｏ＿ｆを演算する第５の演算手段とを含む倣い
   制御装置。
6. （６）請求項５記載の倣い制御装置において、前記第４
   の演算手段は、 ワーク面上を移動する作業用ツールの位置を検出する手
   段と、 作業用ツールの送り目標位置を予め設定する手段と、 前記検出手段で検出した作業用ツールのワーク面上の現
   在点の位置と前記設定手段で予め設定した送り目標位置
   との２点の位置から作業用ツールの押付け方向に垂直な
   方向の送り方向ベクトル■を演算する手段と、 前記送り方向ベクトル■と前記力検出手段で検出した力
   の方向とから前記ワーク面の勾配ｔａｎαを演算する手
   段と を含む倣い制御装置。
7. （７）請求項１記載の倣い制御装置において、前記第１
   の制御手段は、 ワーク面上を移動する作業用ツールの位置を検出する手
   段と、 前記検出手段で検出した作業用ツールのワーク面上の現
   在点を含む２点の位置とその２点の位置の移動時間とか
   ら前記押付け方向の移動速度ｖ＿ｚ＿ｏ＿ｆを演算する
   第６の演算手段と を含む倣い制御装置。
8. （８）請求項１記載の倣い制御装置において、前記第１
   の制御手段は、 前記ワーク面の勾配ｔａｎαを予め記憶する手段と、 前記作業用ツールの送り目標速度ｖ＿ｘ＿ｙ＿ｏを設定
   する手段と、 前記予め記憶した勾配ｔａｎαと前記送り目標速度ｖ＿
   ｘ＿ｙ＿ｏとから押付け方向の移動速度ｖ＿ｚ＿ｏ＿ｆ
   を演算する第７の演算手段と を含む倣い制御装置。
9. （９）請求項１記載の倣い制御装置において、前記第２
   の制御手段は、 前記押付け方向の移動速度ｖ＿ｚ＿ｏ＿ｆによって生じ
   る力ｆ＿ｏ＿ｆを演算する第８の演算手段と、この力ｆ
   ＿ｏ＿ｆを前記力制御ループの押付け力の目標値ｆ＿ｒ
   ＿ｏから差し引いて新しい押付け力の目標値ｆ＿ｒを演
   算する第９の演算手段と を含む倣い制御装置。
10. （１０）請求項１記載の倣い制御装置において、前記第
    ２の制御手段は、 前記押付け方向の移動速度ｖ＿ｚ＿ｏ＿ｆを速度補正値
    ｖ＿ｚ＿ｃとし、この速度補正値ｖ＿ｚ＿ｃを前記力制
    御ループの速度指令値ｕ＿ｚ＿ｏに加算し、新たな速度
    指令値ｕ＿ｚを演算する第１０の演算手段 を含む倣い制御装置。