JPH01222315A - 多自由度作業機械の位置と力の制御装置 - Google Patents

Abstract

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

Description

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

〔従来の技術〕

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

また近年、「計測自動制御学界論文集ＪＶＯ１２２、Ｎ
ｏ３　（１９８６）、３４３〜３５０頁、特開昭６０−
３０１０号及び特開昭６１−７９０５号に記載のように
仮想コンプライアンス制御と呼ばれているものがある。

これは、バネ・マス・ダンパーを有する力学系モデルを
仮想的に実現しようとするもので、力学系の式 ％式％ のｍ、ｃ、にの値を任意に与えて、その動作をソフトウ
ェア上で模擬して、その動きを実現するように制御を行
うものである。これを多自由度系で行うために、座標系
の各軸筋にｍ、ｃ、にの値を変えられるようになってお
り、この値を適当に選べば各軸筋に特性を変えることが
出来る。またに＝０にすれば、位置のフィードバックが
なくなり力制御モードになり、ｋ、ｃの値を大きくして
カフィードバックをなくすようにすれば、位置制御モー
ドとなり、いわゆるハイブリッド制御を行うこともでき
る。このように仮想コンプライアンス制御は、ハイブリ
ッド制御とコンプライアンス制御を包含した制御方式と
いうことができる。

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

しかしながら、従来の仮想コンプライアンス制御を応用
した制御装置においては、制御演算の座標系の与え方に
ついて配慮がされておらず、作業内容によっては適切か
つ効率的な作業ができないという問題があった。

例えば、位置と力の制御によりロボットが行う作業の１
つにワーク表面を倣って表面研磨やパリ取り等を行なわ
せる曲面倣い動作がある。第５図はこのような曲面倣い
動作を模式的に示すもので、ロボット５０の手先効果器
５１がワーク表面５２を倣って動くように制御される９
手先効果器５１に加わる力及びモーメントは力センサ５
３により検出される。この曲面倣い動作を仮想コンプラ
イアンス制御を応用して行う場合、−例として第６図に
示すように行われる。第６図は簡単のためＸ−Ｚ平面で
説明するものであり、この例ではＸ軸方向にはに、ｃの
値を大きくしてかたくし、手先効果器５１を位置ｆｆ、
＋制御によって速度ＶＸで送り、Ｚ軸方向にはに＝ｏと
し位置の拘束を解除し、手先効果器５１を力制御により
目標力ｆ「で押し付けている。このとき、力センサ−５
３により検出される力をｆとすると、手先効果器５１は
Ｚ軸方向には ｍ※Ｚ　十ｃｖＺ　＝ｆ−ｆｒ の式を模擬して動いているので、速度変化のない定常状
態ではｃｖＺ＝ｆ−ｆｒとなり、曲面を倣う速度ｖＺを
発生するには、それに応じた力偏差Δｆ＝ｆ−ｆｒがな
ければならないことが分かる。

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

Ｃｖｘ となる。

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

このように、第６図に示すように任意曲面を倣わせるに
は限界があるので、曲面の目標値を記述し、その記述（
教示）誤差や工具摩耗やワークのバラつき誤差を仮想バ
ネで吸収するようにに、ｃの値を設定したのが第７図で
ある。この例では、曲面での上記誤差を吸収するために
２軸方向だけでなくＸ軸方向にも仮想バネを入れる必要
がある。

従ってこの場合、送り方向にバネの影響が入るので目標
位置と実際の位置と間の位置の偏差が増大してしまい、
適切な作業ができなくな、るなどの不都合を生じる。特
に曲面の変化の大きい所では、位置の偏差の大きいまま
送り方向が変わることにより、削り過ぎ等の悪影響が出
る。

本発明の目的は、座標系を任意に与えられるようにして
上述した従来の問題点を解決し、適切かつ効率的な作業
を確保することのできる多自由度作業機械の位置と力の
制御装置を提供することにある。

〔課題を解決するための手段〕

上記目的は、少なくとも２以上の位置の自由度を持つロ
ボットや工作機械等の多自由度作業機械の位置と力の制
御装置において、作業用ツールの目標位置及び姿勢を設
定する位置設定手段と、作業用ツールの現在の位置及び
姿勢を検出する位置検出手段と、任意に設定した座標系
での目標力及びモーメントを設定する力設定手段と、作
業用ツールに加わる力及びモーメントを検出する力検出
手段と、位置設定手段で設定された目標位置及び姿勢と
位置検出手段で検出された現在の位置及び姿勢との偏差
を求める第１の演算手段と、第１の演算手段で求められ
た位置及び姿勢の偏差を前記任意座標系の値に変換する
第２の演算手段と、力検出手段で検出された力及びモー
メントを同任意座標系の値に変換する第３の演算手段と
、第３の演算手段で変換された任意座標系での力及びモ
ーメントと力設定手段で設定された任意座標系での目標
力及びモーメントとの偏差を求める第４の演算手段と、
第２の演算手段で変換された任意座標系での位置及び姿
勢の偏差と第４の演算手段で求められた任意座標系での
目標力及びモーメントの偏差を用いて位置と力の制御演
算を行い、作業機械の制御量を求める第５の演算手段と
、第５の演算手段で求められた制御量を作業機械の駆動
系の動作指令値に変換する第６の演算手段とからなるこ
とを特徴とする多自由度作業機械の位置と力の制御装置
によって達成される。

曲面倣い動作を行う場合には、前記任意座標系は、工具
の先端を原点としワーク表面の法線及び接線を座標軸と
するワーク表面座標系とすると有利である。

前記第５の演算手段は、好ましくは、第２の演算手段で
求められた位置及び姿勢の偏差Δ入に前記任意座標の各
座標軸ごとに設定したばね定数からなるばね定数行列Ｋ
を乗じる手段と、前記第４の演算手段で求められた力及
びモーメントの偏差Δｔに前記任意座標の各座標軸ごと
に設定した不感帯の演算を行う手段と、不感帯手段の演
算値Δｆ゛から乗算手段の乗算値にΔ入を減算する手段
と、減算手段の減算値Δｆ！’−にΔｘに所定の特性補
償演算を行って前記制御量として速度指令値Ｖを求める
特性補償演算手段とで構成されている。

この場合、特性補償演算手段は、仮想質量行列Ｍと仮想
粘性係数行列Ｃとによって Ｍ會十Ｃｘ＝Δｆ’−にΔｘ の式から仮想コンプライアンス制御の演算を行い、速度
指令値Ｖを演算するようにしてもよい。

制御装置は、好ましくはさらに、位置検出手段で検出さ
れた現在の位置及び姿勢を、第１の演算手段で目標位置
及び姿勢との偏差を求める前にベース座標系の値に変換
する第７の演算手段と、力検出手段で検出された力及び
モーメントを、第３の演算手段で任意座標系の値に変換
する前にベース座標系の値に変換する第８の演算手段と
、第５の演算手段で求められた制御量を、第６の演算手
段で作業機械の駆動系の動作指令値に変換する前にベー
ス座標系の値に変換する第９の演算手段とを有し、第２
、第３及び第９の演算装置は、それぞれベース座標系か
ら任意座標系への変換及び任意座標系からベース座標系
への変換を、ベース座標系と任意座標系との姿勢の変換
行列で与えるようにする。

〔作用〕

第５の演算手段で、任意座標系での位置及び姿勢の偏差
と目標力及びモーメントの偏差を用いて位置と力の制御
演算を行い作業機械の制御量を求めるようにしたので、
作業機械の動作はその任意座標系での制御量に基づいて
制御される。従って、その任意座標系を作業内容に応じ
て最適の座標系に設定することにより、適切かつ効率的
な動作を行なわせることが可能となる。

特に曲面倣い動作を行わせる場合には、その任意座標系
として、作業用ツールの先端を原点としワーク表面の法
線及び接線を座標軸とするワーク表面座標系を設定する
ことにより、法線方向を常に２軸、移動しようとする接
線方向を常にＸ軸方向にとることができる。そこで第５
の演算手段で仮想コンプライアンス制御を採用し、Ｘ軸
方向に位置制御、Ｚ軸方向に力制御を行わせた場合には
、曲面の勾配は座標設定等の誤差によるもののみとなり
、Ｚ軸方向の力の偏差は小さく、Ｘ軸方向の移動速度即
ち送り速度ＶＸを大きくすることができる（第２図参照
）、またそのわずかの誤差をも吸収すべく仮想バネを入
れた場合には、Ｘ軸が接線方向であることからこの方向
に拘束されないので、Ｘ軸方向には仮想バネを入れる必
要がなく、通常の位置制御で送ることができ、送り方向
の位ｉ！ｆｆ１ｌｌ’ｉ差が増大することによる不都合
が生じない（第３図参照）。

〔実施例〕

以下、本発明の好適実施例を図面を参照して説明する。

第１図は本発明の一実施例による位置と力の制御装置の
全体構成を示すもので、符号１は関節型ロボットである
。ロボット１は平部２を有し、平部２に力センサ−３が
取り付けられ、力センサ−３の先に、工具や把持具でワ
ークに対して仕事を行う手先効果器４が取り付けられて
いる。力センサ−３は手先効果器４に加わる力及びモー
メントを検出する。ロボット１はまた、各関節廻りの駆
動を行う図示しない駆動モータを有し、手先効果器４の
位置及び姿勢はこの駆動モータを駆動することにより制
御される。各駆動モータには符号５で例示される角度計
、例えばエンコーダが取り付けられ、このエンコーダ５
により駆動モータの駆動量従って各関節の軸角度データ
を検出する。

ロボット１の制御装置は全体的に符号６で示され、力セ
ンサ−３及び角度計５はこの制御装置６の一部を構成す
る。制御装置６はまた、手先効果器４に加わる力及びモ
ーメントの目標値ｆ「を設定する角目標値設定部７と、
手先効果器４の位置及び姿勢の目ｔｌＩｆｆｉを設定す
る位置目標値設定部８とを有する。角目標値設定部７は
力及びモーメントの［１標値を後述する任意座標系の値
として設定し、位置目標値設定部８は位置及び姿勢の目
標値を後述するベース座標系の値として設定する。角目
標値設定部７は例えば教示データをを使用することがで
き、位置口＃５値設定部８は例えば教示データを補間し
た値を用いることができる。

力センサ−３で検出された力及びモーメントは力演鼻部
９に送られ、ここで後述するセンサ座標系から手先座標
系への変換を行うと共に、手先効果器４の重力骨を差し
引いて重力補償を・行う。

手先座標系で表わされた力及びモーメントは座標変換部
１０でベース座標系を利用した後述する基準座標系に変
換する。この基準座標系への変換は、後述するように方
向のみの変換（回転変換）である。

基準座標系で表した力及びモーメントは、さらに座標変
換部１１で任意座標系に変換する。この座標変換された
力及びモーメントを電とする。この変換も方向のみの変
換（回転変換）である。

座標変換部１１で座標変換された力及びモーメントｆは
、山口標Ｉ設定部７で設定された力及びモーメントの目
標値ｆ「と力偏差演算部１２で比較され、偏差Δ四が求
められる。即ち、Δ１＝１−ｔｒを演算する。

一方、ロボット１のモータに取付けなエン、コーグ５か
らの軸角度データは角度演算部１３に送られ、ここで関
節角０を算出する。この関節角ｅは位置演算部１４に送
られ、関節角０からベース座標系での手先の位置及び姿
勢工°が求められる。

位置演算部１４で演算された位置及び姿勢λ。

は位ｒＩｌ閤差演算部１５で位置目標値設定部８で設定
された位置及び姿勢の目標値入゛「と比較され、偏差Δ
ｘ°が求められる。このベース座標系で表した位置及び
姿勢の偏差Δ入′は座標変換部１６で任意座標系に変換
され、Δ工を算出する。この変換は座標変換部１１での
座標変換と同じ変換行列を用いた方向のみの変換（回転
変換）である。

力偏差演算部１２で演算した任意座、標系での力及びモ
ーメントの偏差Δｔと座標変換部１６で任意座標系に変
換した位置及び姿勢の偏差Δｘは位置と力の制ａＩＩ演
算部１７に送られる。この位置と力の制御演算部１７で
は、偏差Δｆと偏差Δλから、任意座標系での位置と力
の制御演算を行い、この座標系での指令速度Ｖを演算す
る０本実施例では、この演算部に仮想コンプライアンス
制御による制御演算を使用する。

位置と力の制御演算部１７は、具体的には不感帯演算部
１８と、バネ定数乗算部１９と、減算部２０と、特性補
償演算部２１とからなっている。

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

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

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

位置と力の制御演算部１７で演算された速度指令値Ｖは
任意座標系での値なので、これを座標変換部２２でベー
ス座標系での速度Ｖ°に変換する。

このベース座標系での速度指令値Ｖ°は角速度演算部２
３で各関節の角速度指令値Ｗに演算され、この角速度指
令値Ｗからモータ速度演算部２４で各駆動モータの速度
指令値を演算する。モータ速度演算部２４で演算された
速度指令値はサーボアンプ２５に送られ、各駆動モータ
の速度指令値によってロボット１のモータを駆動する。

この中にはモータに取り付けたタコジェネレータからの
速度フィードバックも含まれる。

次にこのように構成された制御装置の動作の詳細を、ロ
ボット１に６自由度の多関節型ロボットを使用した場合
を例にとり説明する。

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

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

このように手先座標系に変換された力及びモーメントは
、座標変換部１０．１１によって任意座標系に変換され
る。この任意座標系の原点は手先座標系の原点と同一に
とり、その方向を任意にとるものとする。この方向は例
えばワークの法線及び接線を座標軸にとるようにするの
で、任意座標系への変換はワークと同じ空間に固定され
た基準座標系からの回転として記述するのが演算上都合
が良い、そこでこの基準座標系として、ロボットが据え
付けられている空間に固定されたベース座標系を利用し
、任意座標系への変換をベース座標系からの回転変換と
して記述する。この変換が座標変換１１である。この座
標変換を行うにはベース座標系への変換が必要なので、
座標変換部１０によって手先座標系からベース座標系（
基準座標系）への変換を行う、ここで言うベース座標系
とは直交３軸の方向がベース座標系と一致するという意
味で、原点は手先座標系と同じである。即ち座標変換部
ｌＯでは方向のみベース座標系に変換する。この座標変
換は、ベース座標系に対する手先座標系の方向によって
与えられ、後述する手先の姿勢を表す行列と同様なので
、常に計算されており、簡単に変換を行うことが出来る
。

このようにして得られた任意座標系での力Ｉは、両目標
値設定部７で設定された任意座標系上の目標力ｆｒと比
較され、力偏差演算部１２によって方間差出Δｆが演算
される。

一方、位置及び姿勢に関しては、ロボット１のモータに
取り付けたエンコーダの値から角度演算部１３によって
関節角０を演算する。さらにこの関節角０から位置演算
部１４によって、ベース座標系における手先の位置及び
姿勢λ°を演算する。

この手先の位置及び姿勢は手先座標系の原点の位置及び
座標軸の方向で見る。第１図では、図を理解しやすいよ
うにベース座標系でのこの位置及び姿勢を６次元ベクト
ルＸ°で表わしており、この場合、６次元ベクトルＸ°
中の姿勢を表す３次元ベクトルは、ベース座標系に対す
る手先座標系の回転角度を表すベクトル（方向が回転軸
方向で大きさが回転角となるベクトル）となる。

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

このように位置演算部１４で演算された位置及び姿勢Ｘ
°は、位置目標値設定部８で設定された目標位置及び姿
勢入“ｒと比較され、位置偏差演算部１５で位置及び姿
勢の偏差Δλ°が計算される。

このうち姿勢の偏差は、目標姿勢と実際の姿勢との間の
回転角を表すベクトルで表わせ、目標手先座標系の姿勢
を表す行列を（＞　ｈｒ、　ｊ　ｈｒ、　ｋｈｒ）とす
れば姿勢の偏差は？ｅで表せる。ここでｅは回転軸の単
位ベクトルであり、市は軸廻りの回転角であり、それぞ
れ以下のように表わされる。

このようにして求められた位置及び姿勢の偏差Δλ°は
ベース座標系で表わされているので、これを座標変換１
６によってワーク表面座標系に変換する。この変換は、
座標変換部１１の変換と同じであり、回転変換として与
えられる。

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

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

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

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

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

この速度指令値Ｖは任意座標系での値なので、これを座
標変換部２２で一旦ベース座標系の値Ｖ。

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

ここで座標変換部１１，１６．２２で行われる回転変換
につきさらに詳しく説明する。

ベース座標系から見た任意座標系のｇ原軸の単位ベクト
ルをｉａ　、ｊａ　、ｋａとすると、３次元ベクトルＵ
の任意座標系での表現Ｕａは、ｕａ＝（ｉａ、ｊａ、ｋ
ａ）”ｕ となる、ここでＴは転置行列を表す、即ち、座標変換行
列は、原点が動かずに回転のみ行う場合は、（ｉａ　、
　ｊａ　、　ｋａ　）’で表せる。座標変換部１１．１
６で使用する行列は６Ｘ６行列であるが、これは並進と
回転、それぞれにこの変換を行えばとなる。一方、座標
変換部２２は任意座標系からベース座標系への変換であ
り、上記式を変形してｕ＝　［（ｉａ　、　ｊａ　、　
ｋａ　）’　］−１ｕａとなり、［（ｉａ　、　ｊａ　
、　ｋａ　）”　］”＝　（ｉａ　。

ｊａ　、　ｋａ　）となるので、この座標変換部２２の
変換行列は で表せる。このように座標変換部１１．１６．２２での
座標変換は任意座標系の座標軸の単位ベクトル−ａ　、
ｊａ　、ｋａで与えることが出来る。

ロボット１にワーク表面を倣って表面研磨やパリ取り等
を行なわせる曲面倣い動作をさせる場合には、任意座標
系として作業用ツールの先端を原点としワーク表面の法
線及び接線を座標軸とするワーク表面座標系を与える。

この場合、ワーク表面座標系への座標変換（座標変換部
１１．１６）又はワーク表面座標系からの座標変換（座
標変換部２２）の変換行列には、ワーク表面の法線及び
接線の単位ベクトルを用いる。これは例えば、手先が移
動しようとする曲面に沿って順次その値を記憶しておく
ことにより与えることができる。

このようにすれば、第６図の従来例に対応して第２図に
示すように、ワーク表面の法線方向を常に２軸、移動し
ようとする接線方向を常にＸ軸方向にとることができる
。これにより、位置と力の制ｔＩＩ演ｒＬ部１７でＸ軸
方向に位置制御、Ｚ軸方向に力制御を行わせた場合には
、曲面の勾配は８標設定等の誤差によるもののみとなり
、Ｚ軸方向の倣い速度ＶＺは第６図の従来例のようには
大きくならず、力の偏差は小さくなる。従って、曲面の
勾配に制約されることなく、Ｘ軸方向の移動速度即ち送
り速度ＶＸを大きくすることができる。

また第７図の従来例に対応して第３図に示すように、誤
差をも吸収すべく仮想バネを入れた場合には、Ｘ軸が接
線方向であることからこの方向に拘束されることがなく
、Ｘ軸方向には仮想バネを入れて柔らかくする必要がな
い、従っ、てＸ軸方向には通常の位置制御で送ることが
でき、送り方向の位置開基が増大することによる不都合
が生じることはない。

以上の実施例は本発明を６自由度のロボットの制御に適
用した例であるが、本発明は基本的には２以上の自由度
を持つものであれば、それ以外の多自由度作業機械に適
用できるものである。第４図は本発明を３軸の工作機械
に適用した例である。

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

第４図において、符号３０は工作機械本体であり、工作
機械本体３０−にはｘｙ子テーブル１と２軸駆動機横３
２とが取り付けられている。ｘｙ子テーブル１は、ｘ、
ｙの直交方向に動くテーブルであり、この上に加工すべ
きワークを置（、ｚ軸駆動機構３２は、Ｚ方向（上下方
向）に動き、この先に手先加工器４を取り付ける。２軸
駆！１ｌＩｌ！ｆＩ３２はｘｙ子テーブル１の運動によ
っては動がない。

このような構成の工作機械に対する位置と力の制御装置
において、第１の実施例と異なるのは、力演鼻部３３、
位置演算部３４及びモータ速度演算部３５の演算内容が
異なり、それに関連して第１図の実施例の座標変換部１
０、角度演算部１３及び角速度演算部２３が不要な点で
ある。即ち、力演鼻部３３は力センサ−４から検出され
た力をセンサー座標系から手先座標系へ変換し、手先部
に加わる力を算出する。このとき、工作機械が３次元構
成のためにモーメントの演算は行わず、また手先効果器
４の姿勢変化がないため、重力補償は一定のバイアスを
差し引くだけでよい、力演鼻部３３以下のベクトルも全
て３次元で並進ベクトルのみとなる。また基準座標系は
移動するｘｙ子テーブル１上にとれるので、手先座標系
と基準座標系はその方向に関して同じとなり、力演鼻部
３３で手先座標系に変換された力は基準座標系のものと
なっている。

位置演算部３４は、エンコーダまたはリニアスケールの
値を用いて手先の位置を演算する。これは、ｘｙ子テー
ブル１上に固定した基準座標系においての手先の位置に
なっている。

モータ速度演算部３５は、基準座標系での速度指令値か
ら各駆動モータの速度指令値を直接演算する。

このようにこの実施例では、第１図に示す実施例の場合
のように関節角を用いず、直接ｘｙｚ座標系を算出でき
、姿勢変化がないため、姿勢及びモーメントに関する演
算がない、従って図中のベクトルｘ、ｔ、ｖ等は３次元
ベクトルとなり、Ｋ。

Ｍ、Ｃや座標変換行列は３Ｘ３行列になり、演算は第１
図の実施例に比べて簡単なものになる。

本実施例においても、その動作は姿勢の自由度がない点
を除き第１図と実質的に同じであり、第２図及び第３図
を参照して説明した曲面倣い動作を同様に行うことがで
きるものである。

なお以上の実施例は、任意座標系としてワーク表面座標
系をとった場合につき説明したが、任意座標系はワーク
表面座標系に限らず、作業内容において適宜最適の座標
系を設定できるものである。

例えば３次元クランク回しなどの作業においては、作業
用ツールの拘束による反力の方向及び移動方向により拘
束座標系を定め、この拘束座標系を任意座標系にすれば
よく、これにより適切かつ効率的なりランク回し作業を
行うことができる。

〔発明の効果〕

本発明は、以上のように構成したので以下に列挙する効
果を奏することができる。

（１）位置と力の制御を行う座標軸方向を任意の方向に
とれ、これを作業内容に応じて最適の座標系に定めるこ
とができるので、適切かつ効率的な作業を行うことがで
きる。

（２）曲面倣い動作の場合は、任意座標系をワークの表
面座標系にとることによって、ワークの法線方向に押し
付けたり、この方向に仮想バネを設定したりすることが
出来、接線方向にはこれと独立して送ることが出来、適
切かつ効率的な作業ができる。

（３）任意座標としてワーク表面座標系に限らず、拘束
による反力と移動方向によって拘束座標系を定めること
により、３次元クランク回しなどの作業を行うことも出
来る。

（４）任意座標系への変換行列を基準座標系からの姿勢
の変換行列で与えることにより、その変換行列を任意座
標系の座標軸の単位ベクトルで記述することができ、取
扱が容易となる。

【図面の簡単な説明】

第１図は本発明の一実施例による位置と力の制御装置を
示すブロック図であり、第２図はその制御装置を用いて
一定押付力制御で曲面を倣う例を示す説明図であり、第
３図は同制御装置を用いて仮想バネによって曲面を倣う
例を示す説明図であり、第４図は本発明の他に実施例に
よる位置と力の制御装置を示すブロック図であり、第５
図はロボットによって曲面を倣う動作を示す概略図であ
り、第６図は従来の制御方法を用いて一定押付力制御に
より曲面を倣う例を示す説明図であり、第７図は従来の
制御方法を用いて仮想バネによって曲面を倣う例を示す
説明図である。 符号の説明 １・・・ロボット　　　　　３・・・力センサ−４・・
・手先効果器（作業用ツール） ５・・・エンコーダ（位置検出手段） ６・・・制御装ｗ！、７・・・力設定装置８・・・位置
設定装置

Claims (5)

【特許請求の範囲】
1. （１）少なくとも２以上の位置の自由度を持つロボット
   や工作機械等の多自由度作業機械の位置と力の制御装置
   において、 作業用ツールの目標位置及び姿勢を設定する位置設定手
   段と、作業用ツールの現在の位置及び姿勢を検出する位
   置検出手段と、任意に設定した座標系での目標力及びモ
   ーメントを設定する力設定手段と、作業用ツールに加わ
   る力及びモーメントを検出する力検出手段と、位置設定
   手段で設定された目標位置及び姿勢と位置検出手段で検
   出された現在の位置及び姿勢との偏差を求める第１の演
   算手段と、第１の演算手段で求められた位置及び姿勢の
   偏差を前記任意座標系の値に変換する第２の演算手段と
   、力検出手段で検出された力及びモーメントを同任意座
   標系の値に変換する第３の演算手段と、第３の演算手段
   で変換された任意座標系での力及びモーメントと力設定
   手段で設定された任意座標系での目標力及びモーメント
   との偏差を求める第４の演算手段と、第２の演算手段で
   変換された任意座標系での位置及び姿勢の偏差と第４の
   演算手段で求められた任意座標系での目標力及びモーメ
   ントの偏差を用いて位置と力の制御演算を行い、作業機
   械の制御量を求める第５の演算手段と、第５の演算手段
   で求められた制御量を作業機械の駆動系の動作指令値に
   変換する第６の演算手段とからなることを特徴とする多
   自由度作業機械の位置と力の制御装置。
2. （２）前記任意座標系を、作業用ツールの先端を原点と
   し、ワーク表面の法線及び接線を座標軸とするワーク表
   面座標系としたことを特徴とする請求項１記載の多自由
   作業機械の位置と力の制御装置。
3. （３）前記第５の演算手段が、前記第２の演算手段で求
   められた位置及び姿勢の偏差Δｘに前記任意座標の各座
   標軸ごとに設定したばね定数からなるばね定数行列Ｋを
   乗じる手段と、前記第４の演算手段で求められた力及び
   モーメントの偏差Δｆに前記任意座標の各座標軸ごとに
   設定した不感帯の演算を行う手段と、この手段の演算値
   Δｆ′から乗算手段の乗算値ＫΔｘを減算する手段と、
   この手段の減算値Δｆ′−ＫΔｘに所定の特性補償演算
   を行って前記制御量として速度指令値ｖを求める特性補
   償演算手段とからなることを特徴とする請求項１記載の
   多自由作業機械の位置と力の制御装置。
4. （４）前記特性補償演算手段が、仮想質量行列Ｍと仮想
   粘性係数行列Ｃとによって Ｍ■＋Ｃｘ＝Δｆ′−ＫΔｘ の式から仮想コンプライアンス制御の演算を行い速度指
   令値ｖを演算することを特徴とする請求項３記載の多自
   由度作業機械の位置と力の制御装置。
5. （５）前記位置検出手段で検出された現在の位置及び姿
   勢を、第１の演算手段で目標位置及び姿勢との偏差を求
   める前にベース座標系の値に変換する第７の演算手段と
   、力検出手段で検出された力及びモーメントを、第３の
   演算手段で任意座標系の値に変換する前にベース座標系
   の値に変換する第８の演算手段と、第５の演算手段で求
   められた制御量を、第６の演算手段で作業機械の駆動系
   の動作指令値に変換する前にベース座標系の値に変換す
   る第９の演算手段とを有し、第２、第３及び第９の演算
   装置は、それぞれベース座標系から任意座標系への変換
   及び任意座標系からベース座標系への変換を、ベース座
   標系と任意座標系との姿勢の変換行列で与えることを特
   徴とする請求項１記載の多自由度作業機械の位置と力の
   制御装置。