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

Abstract

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

Description

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

〔従来の技術〕

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

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

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

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

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

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

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

このようｋ、第１２図に示すように任意曲面を倣わせる
には限界があるので、曲面の目標値を記述し、その記述
（教示）誤差や工具摩耗やワークのバラつき誤差を仮想
バネで吸収するようにｋ。

Ｃの値を設定したのが第１３図である。この例では、曲
面での上記誤差を吸収するためにＺ軸方向だけでなくＸ
軸方向にも仮想バネを入れる必要がある。従ってこの場
合、送り方向にバネの影響が入るので目標位置と実際の
位置と間の位置の偏差が増大してしまい、適切な作業が
できなくなるなどの不都合を生じる。特に曲面の変化の
大きい所では、位置の偏差の大きいまま送り方向が変わ
ることにより、削り過ぎ等の悪影響が出る。

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

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

上記目的は、少なくとも位置の３自由度を持つロボット
や工作機械等の多自由度作業機械の位置と力の制御装置
において、作業用ツールに加わる少なくとも直交３軸方
向の力を検出する第１の検出手段と、作業用ツールの位
置又は速度を検出する第２の検出手段と、第１の検出手
段で検出されたカデータから、作業用ツールがワーク表
面から受ける反力ベクトルｆを演算する第１の演算手段
と、第２の検出手段で検出された位置または速度データ
から作業用ツールの移動方向ベクトルマを演算する第２
の演算手段と、第１及び第２の演算手段で演算された反
力ベクトルｔと移動方向ベクトルＶとから互いに直交す
る単位ベクトル−１ｊ。

ｋを演算する第３の演算手段と、第３の演算手段で演算
された単位ベクトルｉ、ｊ、ｋを座標変換データとして
用い、その単位ベクトルｉ、ｊ、ｋを座標軸とする直交
座標系（ｓ、ｊ、ｋ）上で位置と力の制御演算を行う第
４の演算手段とを有することを特徴とする多自由度作業
ａｌｌの位置と力の制御装置によって達成される。

第３の演算手段は、−例として、第１の演算手段で演算
された反力ベクトルＩから単位ベクトルｋを演算し、こ
の単位ベクトルにと第２の演算手段で演算された移動方
向ベクトルＶとからこれら両ベクトルｋ、ｖに垂直な単
位ベクトルｊと、単位ベクトルｋ、マの張る平面上で単
位ベクトルｋに垂直な単位ベクトル１を演算することが
できる。

このとき第３の演算手段は、曲面倣い動作の場合には、
第１の演算手段で演算された反カベクトル賃からワーク
表面の法線ベクトルｎを演算し、この法線ベクトルｉか
ら法線方向の単位ベクトルｋを演算する。

また第３の演算手段は、他の手法として、第２の演算手
段で演算された移動方向ベクトルマから作業用ツールの
移動方向の単位ベクトルｉを演算し、この単位ベクトル
−と第１の演算手段で演算された反力ベクトルｆとから
両ベクトルｉ、ｔに垂直な単位ベクトルｊと、単位ベク
トルｉ、ｔの張る平面上で単位ベクトル１に垂直な単位
ベクトルにと演算し、これによりワーク表面座標系の直
交３軸の単位ベクトルｉ、ｊ、ｋを演算することができ
る。

第３の演算手段は、曲面倣い動作の場合には、第１の演
算手段で演算された反力ベクトルｆと第２の演算手段で
演算された移動方向ベクトルＶとから、作業用ツールの
先端を原点としワーク表面の法線及び接線を座標軸とす
るワーク表面座標系の単位ベクトルｓ、ｊ、ｋを演算す
る。

第３の演算手段で演算された単位ベクトル−１ｊ、ｋを
他の教示データと共に記憶する記憶手段と、この記憶し
たデータをプレイバック時に再生する再生手段を設けて
もよい。

〔作用〕

第３の演算手段で互いに直交する単位ベクトルｉ、ｊ、
ｋを演算し、第４の演算手段でその単位ベクトルｉ、ｊ
、ｋを座標変換データとして用い、単位ベクトルｉ、ｊ
、ｋを座標軸とする直交座標系（ｉ、ｊ、ｋ）上で位置
と力の制御演算を行うようにしたので、作業用ツールに
適切かつ効率的な動作を行なわせることが可能となる。

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

第３の演算手段で、第１の演算手段で演算された反力ベ
クトルｌから単位ベクトルｋを演算することにより、単
位ベクトルｉ、ｊ、ｋを簡便に求めることができる。こ
こで、上記ワーク表面座標系の単位ベクトルを求める場
合には、第１の演算手段で演算された反力ベクトルＩか
らワーク表面の法線ベクトルｎを演算し、この法線ベク
トルｎから法線方向の単位ベクトルｋを演算ずればよい
。

また第３の演算手段で、第２の演算手段で演算された移
動方向ベクトルＶから作業用ツールの移動方向の単位ベ
クトルｔを演算する場合には、反力データから直接、法
線方向の単位ベクトルを求めないので、例えばワーク表
面座標系の演算において、摩擦力等により反力の方向が
法線方向からずれていた場合でも正確にワーク表面座標
系の単位ベクトル−、ｊ、ｋを求めることができる。

第３の演算手段で演算された単位ベクトル−０ｊ、ｋを
他の教示データと共に記憶する記憶手段と、この記憶し
たデータをプレイバック時に再生する再生手段を設ける
ことにより、ワーク表面座標系を教示データとして与え
ておくことができ、この教示データを用いて多自由度作
業機械の動作を容易に制御することができる。また形状
認識等の他の用途に使用することもできる。

〔実施例〕

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

第１図は本発明の一実施例による位置と力の制御装置の
ワーク表面座標の演算部分を示し、第２図はその演算結
果を利用した制御装置本体部分の全体構成を示す。

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

第２図において、ロボット１の制御装置は全体的に符号
６で示され、力センサ−３及び角度計５はこの制御装置
６の一部を構成する。制御装置６はまた、手先効果器４
に加わる力及びモーメントの目標値ｆ「を設定する角目
標値設定部７と、手先効果器４の位置及び姿勢の目標値
を設定する位置目標値設定部８とを有する。角目標値設
定部７は力及びモーメントの目ＷＡ値を後述するワーク
表面座標系の値として設定し、位置目標値設定部８は位
置及び姿勢の目標値を後述するベース座標系の値として
設定する。

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

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

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

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

一方、ロボット１のモータに取付けたエンコーダ５から
の軸角度データは角度演算部１３に送られ、ここで関節
角０を算出する。この関節角０は位置演算部１４に送ら
れ、関節角０から後述するベース座標系での手先の位置
及び姿勢λ°が求められる。　位置演算部１４で演算さ
れた位置及び姿勢Ｘ°は位置偏差演算部１５で位置目標
［設定部８で設定された位置及び姿勢の目標値λ°「と
比軸され、偏差Δ入°が求められる。このベース座標系
で表した位置及び姿勢の偏差Δ入°は座標変換部１６で
後述するワーク表面座標系に変換され、Δλを算出する
。この変換は座標変換部１１での座標変換と同じ変換行
列を用いた方向のみの変換（回転変換）である。

力偏差演算部１２で演算したワーク表面座標系での力及
びモーメントの偏差Δ四と座標変換部１６でワーク表面
座標系に変換した位置及び姿勢の偏差Δλは位置と力の
制御演算部１７に送られる。

この位置と力の制御演算部１７では、偏差Δｆと偏差Δ
Ｘから、ワーク表面座標系での位置と力の制御演算を行
い、この座標系での指令速度Ｖを演算する０本実施例で
は、この演算部に仮想コンプライアンス制御による制御
演算を使用する。

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

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

特にｋｉ　＝Ｏにすれば位置のフィードバックをなくし
て、力制御のみを行うことも出来る。

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

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

位置と力の制御演算部１７で演算された速度指令値Ｖは
ワーク表面座標系での値なので、これをＳ標変換部２２
でベース座標系での速度Ｖ“に変換する。このベース座
標系での速度指令値Ｖ゛は角速度演算部２３で各関節の
角速度指令値Ｗに演算され、この角速度指令値Ｗからモ
ータ速度演算部２４で各駆動モータの速度指令値を演算
する。

モータ速度演算部２４で演算された速度指令値はサーボ
アンプ２５に送られ、各駆動モータの速度指令値によっ
てロボット１のモータを駆動する。

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

上述した座標変換部１１．１６．２２においては、ワー
ク表面座標系への座標変換又はワーク座標系からの座標
変換のため、第１図に示す演算部分で演算されたワーク
表面座標系のデータが用いられる。

第１図においてこのワーク座標系演算部分は、座標変換
部１０（第２図参照）で基準座標系（ベース座標系）に
変換された力のデータからワーク表面の法線方向ベクト
ルｎを求める法線方向演算部６０と、位置演算部１４（
第２図参照）で求められた手先の位置を手先の移動に従
って順次記憶する記憶部６１と、記憶部６１に記憶され
たデータをもとｋ、移動方向ベクトルＶを求める移動方
向演算部６２と、法線方向ベクトルｎと移動方向ベクト
ルＶとから、第３図の手順でワーク表面座標の座標軸ｉ
、、ｊ、ｋを演算する座標軸演算部６３とからなってい
る。記憶部６１で記憶する位置の個数は移動方向演算部
６２の演算で用いる範囲とされる０Ｍ標原軸算部６３で
演算されたワーク表面座標ノＦＭｌ［ｌｔｌ　ｉ　、　
ｊ　、　ｋｇ、ｔ、第２　図ニ示ｔ　Ｍ種変換部１１．
１６．２２のＵ標変換行列用のデータとされる。

次にこのように構成された制御装置の動作を説明する。

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

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

このように手先座標系に変換された力及びモーメントは
、座標変換部ｔｏ、ｔｔによってワーク表面座標系に変
換される。このワーク表面座標系は原点を手先座標系の
原点と同一にとり、その方向をワーク表面の接線及び法
線方向にとっている。

従ってこの方向はワークと同じ空間に固定された基準座
標系からの回転として記述するのが演算上都合が良い、
そこでその方向を、ロボットが据え付けられている空間
に固定されたベース座標系を利用し、このベース座標系
からの回転変換として記述する。この変換が座標変換１
１であり、ここでは後述する座標軸演算部６３の演算デ
ーター。

ｊ、ｋを用いる。この座標変換を行うにはベース座標系
への変換が必要なので、座標変換部１０によって手先座
標系からベース座標系（基準座標系）への変換を行う、
ここで言うベース座標系とは直交３軸の方向がベース座
標系と一致するという意味で、原点は手先座標系と同じ
である。即ち座標変換部１０では方向のみベース座標系
に変換する。

この座標変換は、ベース座標系に対する手先座標系の方
向によって与えられ、後述する手先の姿勢を表す行列と
同様なので、常に計算されており、簡単に変換を行うこ
とが出来る。

このようにして得られたワーク表面座標系での力ｆは、
山口標値設定部７で設定されたワーク表面座標系上の目
標力ｔ「と比較され、力偏差演算部１２によって方間差
出Δｔが演算される。

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

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

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

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

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

このようにして求められた位置及び姿勢の偏差Δ入°は
ベースＪｉＪｉｌｉ系で表わされているので、これを座
標変換１６によってワーク表面座標系に変換する。この
変換は、座標変換部１１の変換と同様、後述する座標軸
演算部６３の演算データー。

ｊ、ｋを用いて行い、かつ回転変換として与えられる。

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

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

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

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

この速度指令値Ｖはワーク表面座標系での値なので、こ
れを座標変換部２２で一旦ベース座標系のｒｉｖ’に変
換する。この座標変換には、後述する座標軸演算部６３
の演算データｋ、ｊ、ｋが用いられる。ベース座標系に
変換された速度指令値Ｖ゛は角速度演算部２３によって
ロボットの各関節の角速度Ｗに変換され、さらにモータ
速度演算部２４で、モータの回転速度に変換した後、サ
ーボアン１２５によってこの速度で動くようにロボット
１を制御する。

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

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

このように座標変換部１１．１６．２２での座標変換は
ワーク表面座標系の座標軸の単位ベクトルｓ、ｊ、にで
与えることが出来、第１図に示した演算部分で前述した
ように単位ベクトルｉ、ｊ。

ｋを演算すれば、ワーク表面座標系へのまたはワーク表
面座標系からの座標変換が可能である。

次に第１図に示すワーク座標系演算部分の動作を第３図
及び第４図を参照して説明する。

第４図はワーク表面座標系の与え方を示す図である。ワ
ーク表面座標系はワーク表面の法線及び接線方向を座標
軸にとるが、接線方向のとり方は一義的には決まらない
ので、本実施例では接平面上で手先の移動方向をＭｆｌ
ｉ軸の１つにとり、第４図（ａ）に示すようにｉ、ｊ、
ｋを座標軸にとる。

ここで１は移動方向にある接線方向の単位ベクトル、ｋ
は法線方向の単位ベクトル、ｊは１．ｋに垂直にとった
単位ベクトルであり、座標系（１゜ｊ、ｋ）が右手系と
なるようにとられる。

第４図（ｂ）はこの座標系（ｉ、ｊ、ｋ）の＜ｉ、ｋ）
平面について説明したもので、ロボット１の手先効果器
４がワーク表面から受ける反力ｔ、及び手先の移動方向
ベクトルＶはこの平面内にあるので、これを用いてｉ、
ｊ、ｋを決定する。

ワーク座標系演算部分は以上の考え方に基づきワーク座
標系の座標軸を演算するものである。即ち、力センサ−
３で検出した力は、前述したように力演算部９、座標変
換部１０によってベース座標系（基準座標系）での手先
効果器４に加わる力に変換される。この力は第４図（ｂ
）に示す反力Ｉに相当する。この力ｔから法線方向演算
部６０によって、ワークの法線方向ベクトルｎを演算す
る。この演算では、摩擦力の影響が問題となるので、こ
れを除去するようにする。一番正確に行うには、手先効
果器４自身を摩擦のないプローブで作ればよく、この場
合は上記の検出した反力ｆが法線方向になる。即ち、第
４図（ｂ）においては反力ｆと法線方向ベクトルれとが
一致する。摩擦力が出てしまう場合は、一定の動摩擦係
数を仮定すれば、第４図（ｂ）の１とｎのなす角が一定
となり、これによって法線方向ベクトルｎを求めること
が出来る。更に力の他にモーメントも検出すれば力の作
用線が分かり、手先効果器４の外形がわかればその接触
点を求めることが出来、これによって摩擦係数が変わっ
ても常に法線方向を求、められる。

一方、手先効果器４の位置は、前述したように角度演算
部１３、位置演算部１４で演算される。

この位置を記憶部６１に必要な個数だけ記憶し、移動方
向演算部６２で移動方向Ｖを演算する。このＶは移動速
度と同じだが、その方向が分かればよいので、大きさは
速度と同じでなくてよい、その方向を演算するための一
番単純な方法は、１つ（あるいはいくつか前）の値との
差分をとる方法である。その他、ノイズ等の誤差要因を
除くために数点の重み付き平均をとるなどいく通りかの
既知の方法がある。

このようにして法線方向演算部６０で求めた法線方向ベ
クトルｎと、移動方向演算部６２で求めた移動方向ベク
トルＶを用いてワーク表面座標座標軸演算部６３によっ
て、座標軸の単位ベクトルｓ、ｊ、ｋを求める。このｉ
、にの方向は、第４図（ｂ）に示すようにｎ、ｖの方向
と同一になるはずであるが、測定誤差やノイズによって
必ずしも同一にはならず、ｎとＶが直交しないことがあ
る。そこで、本実施例では力の値は比軸的正確に測定出
来たものとして法線方向ｎをに方向とし、ｉはｎとＶを
含む平面内でｋと垂直になるように定める。ｊはこれら
に垂直な方向である。この求め方を示したのが、第３図
のフローチャートである。

まず手順７０において、法線ベクトルｎをその大きさｆ
ｉｌで割って正規化し、長さ１の単位ベクトルにとする
１次に手順７１で、ｋとＶに垂直な方向を求めるためｋ
、外積ｋＸｖを演算する。

そして手順７２でこれを正規化した単位ベクトルをｊと
する０次に手順７３で、外積ｊＸｋを１とすれば、ｉは
ｎ、ｖを含む平面内でｋと垂直な単位ベクトルになる。

ここで、手順７１において、ｋＸｖを求めたが、これは
ｎｘｖでもよく、この場合ｎの正規化を行うのは手順７
１の後でもよい。

このようにして、ワーク表面の座標軸−１ｊｌｋを求め
たが、このｉ、ｊ、ｋを第２図に示す制御系の座標変換
部１１，１６．２２へ送り、ワーク表面座標系とベース
座標系との間の座標変換に使用する。

本実施例は以上のように構成したので、ロボット１にワ
ーク表面を倣って表面研磨やパリ取り等を行なわせる曲
面倣い動作をさせる場合には、作業用ツールの先端を原
点としワーク表面の法線及び接線を座標軸とするワーク
表面座標系で仮想コンプライアンス制御を行うことがで
き、第７図の従来例に対応して第５図に示すようｋ、ワ
ーク表面の法線方向を常に２軸、移動しようとする接線
方向を常にＸ軸方向にとることができる。これにより、
位置と力の制御演算部１７でＸ軸方向に位置制御、Ｚ軸
方向に力制御を行わせた場合には、曲面の勾配は座標設
定等の誤差によるもののみとなり、Ｚ軸方向の倣い速度
尾ＶＺは第７図の従来例のようには大きくならず、力の
偏差は小さくなる。従って、曲面の勾配に制約されるこ
となく、Ｘ軸方向の移動速度即ち送り速度ＶＸを大きく
することができる。

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

以上の実施例は、ワーク表面座標系演算部分での座標軸
−、ｊ、にの演算をロボットの動作と同時に逐次行う例
であるが、その演算結果を教示データとして格納してお
き、プレイバック時に座標変換データとして用いること
もできる。第７図はそのような実施例を示すもので、第
１図に示した実施例の構成に加えて、ワーク表面座標座
標軸演算部６３で演算したｉ、ｊ、ｋを、位置演算部１
４で演算した手先の位置及び姿勢と共に教示データとし
て記憶する記憶部８０と、記憶された一１ｊ、ｋをプレ
イバック時に再生し、補間演算等を行って座標変換デー
タとして座標変換部１１，１６．２２に送る再生部８１
と、記憶された位置及び姿勢をプレイバック時に再生し
、捕間演算等を行って位置及び姿勢の目標値として位置
目標値設定部８に送る再生部８２とを有している。

本実施例では、ワーク表面座標系を位置及び姿勢と共に
教示データとして与えておくことができ、この教示デー
タを用いて多自由度作業機械の動作を制御することがで
きる。またこの教示データを形状認識等の他の用途に使
用することもできる。

第８図に本発明のワーク座標系演算部分のさらに他の実
施例を示す６図中第１図に示す部材と同等の部材には同
じ符号を付しである。第１図に示す実施例では、座標変
換部１０で求められた力のベクトルからワーク表面の法
線方向ベクトルｎを演算し、これと移動方向ベクトルＶ
とから座標軸の単位ベクトルｓ、ｊ、ｋを求めたが、こ
の場合は摩擦力の影響が除去できないとｎが不正確とな
り、第４図（ｂ）に示すようにｉ、にの方向がｎ。

マの方向に正確に一致しなくなる。一方、移動方向Ｖは
比軸的正確に求めることができるので、これを基準とし
てｉ、ｊ、ｋを求めれば、摩擦力の影響を排除すること
ができる。第８図はこのような実施例を示すもので、第
１図の法線方向演算部６０は無く、座標変換部１０でベ
ース座標系に変換された反力ベクトルｔは直接ワーク表
面塵ｌｌ座標軸演算部９０に送られ、ここで、その力ベ
クトルｆと移動方向ベクトルＶとから、ワーク表面座標
系のＰｉ標原軸、ｊ、ｋを演算する。即ち、移動方向ベ
クトルＶを１方向とし、ｋは電とマを含む平面内でｉと
垂直になるように定め、ｊはこれらに垂直な方向として
定める。

座標軸演算部９０でのこの演算手順を示したのが第９図
のフローチャートである。まず手順９１で、移動方向ベ
クトルＶをその大きさＩＶＩで割って正規化し、長さ１
の単位ベクトルｉとする。

次に手順９２で、反力ベクトルｔと１に垂直な方向を求
めるために外積ｆＸｉを演算する０次に手順９３で、こ
れを正規化した単位ベクトルをｊとする０次に手順９４
で、外積ｉＸｊをｋとすれば、ｋはｔ、マを含む平面内
で１と垂直な単位ベクトルとなる。

ここで、手順９２においてｆＸｉを求めたが、これはｆ
Ｘｖでもよく、この場合Ｖの正規化を行うのは手順９２
の後でもよい。

なお第８図に示す実施例も、第１図の実施例について第
７図に示す実施例で行ったごとく、演算して求めた座標
軸データｓ、ｊ、ｋを教示データとして使用するように
することができ、第１０図はそのような実施例を示すも
のである６図中第７図に示す実施例と同等の部材には同
じ符号を付しである。この実施例においても第７図の実
施外と同様の効果を得ることができる。

以上の実施例においては、ワーク表面座標系の座標軸を
演算するのに使用する手先効果器４の移動方向ベクトル
Ｖを、手先効果器の位置を検出する角度計５の軸角度デ
ータから演算した。しかしながらこれは、手先効果器４
の移動速度を検出する速度計を設置し、この速度計の速
度データから移動方向ベクトルマを演算することもでき
る。

また以上の実施例は、ロボット１が行う作業として表面
倣い動作を想定し、位置と力の制御演算部１７において
ワーク表面座標系上で位置と力の制御演五を行った例で
あるが、この座標系としてはワーク表面座標系以外ｋ、
作業内容において適宜最適の座標系を設定することがで
きる０例えば３次元クランク回しなどの作業においては
、作業用ツールの拘束による反力の方向及び移動方向に
より拘束座標系を定め、この拘束座標系で位置と力の演
算制御をする。この場合は、第１図等において示す座標
系演算部分で当該拘束座標系の座標軸を演篤し、この拘
束座標系の座標軸データを座標変換部１１．１６．２２
に送り、座標変換用のデータとする。これによりその拘
束座標系で適切かつ効率的なりランク回し作業を行うこ
とができる。

〔発明の効果〕

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

（１）力の検出値と移動方向から位置と力の制御演算を
行う座標軸を自動的に算出することができるので、作業
内容に応じて最適の座標系を設定することができ、適切
かつ効率的な作業を行うことができる。

（２）曲面倣い動作の場合は、ワーク表面座標系の座標
軸を演算することにより、ワークの法線方向に押し付け
たり、この方向に仮想バネを設定したりすることが出来
、接線方向にはこれと独立して送ることが出来、適切か
つ効率的な作業を行うことができる。またワーク座標系
を常に演算しながら、未知の物体の表面をなぞって行く
ような倣い動作を容易に行うことができる。

（３）ワーク表面座標が演算できるので、ワークに対し
て工具の姿勢を一定に保つような姿勢制御も簡単に付加
することができる。

（４）演算された座標軸データを記憶し、再生できるよ
うにすることにより、そのｑ原糸の教示を簡単に行うこ
とができ、当該座標系を自由に使いこなすことができる
。またその教示データが記憶されているので、物体表面
の方向を容易に知ることができ、形状認識等の他の用途
に使用することもできる。

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

【図面の簡単な説明】

第１図は本発明の一実施例による位置と力の制御装置の
ワーク表面座標系演算部分を示すブロック図であり、第
２図はその制御装置の本体部分を示すブロック図であり
、第３図は第１図に示すワーク表面座標系演算部分にお
ける座標軸の演算手順を示すフローチャートであり、第
４図（ａ）及び（ｂ）はその座標軸の与え方を説明する
説明図であり、第５図はその制御装置を用いて一定押付
力制御で曲面を倣う例を示す説明図であり、第６図は同
制御装置を用いて仮想バネによって曲面を倣う例を示す
説明図であり、第７図は本発明の他の実施例によるワー
ク表面座標系演算部分を示すブロック図であり、第８図
は本発明のさらに他の実施例によるワーク表面座標系演
算部分を示すブロック図であり、第９図は第８図に示す
ワーク表面座標系演算部分における座標軸の演算手順を
示すフローチャートであり、第１０図は本発明のなおさ
らに他の実施例によるワーク表面座標系演算部分を示す
ブロック図であり、第１１図はロボットによって曲面を
倣う動作を示す概略図であり、第１２図は従来の制御方
法を用いて一定押付力制御により曲面を倣う例を示す説
明図であり、第１３図は従来の制御方法を用いて仮想バ
ネによって曲面を倣う例を示す説明図である。 符号の説明 １・・・ロボット　　　　　３・・・力センサ−４・・
・手先効果器（作業用ツール） ５・・・エンコーダ（位置検出手段） ６・・・制御装置 １０・・・座標変換部（第１の演算手段）１７・・・位
置と力の制御演算部（第４の演算手段）６０・・・法線
方向演算部（第１の演算手段）６２・・・移動方向演算
部（第２の演算手段）６３．９０・・・ワーク表面座標
座標軸演算部（第３の演算手段） ８０・・・記憶部　　　　　８１．８２・・・再生部代
理人　　弁理士　春　日　　譲 １−　ロボット ８１．８２−−−・再生部 第１図 第３図 第４図 （ａ） （ｂ） 第７図 第８図 第９図 第１０図 ５゜

Claims (6)

【特許請求の範囲】
1. （１）少なくとも位置の３自由度を持つロボットや工作
   機械等の多自由度作業機械の位置と力の制御装置におい
   て、 作業用ツールに加わる少なくとも直交３軸方向の力を検
   出する第１の検出手段と、作業用ツールの位置又は速度
   を検出する第２の検出手段と、第１の検出手段で検出さ
   れた力から、作業用ツールが受ける反力のベクトルｆを
   演算する第１の演算手段と、第２の検出手段で検出され
   た位置又は速度データから作業用ツールの移動方向ベク
   トルｖを演算する第２の演算手段と、第１の演算手段で
   演算された反力ベクトルｆと第２の演算手段で演算され
   た移動方向ベクトルｖとから互いに直交する単位ベクト
   ルｉ、ｊ、ｋを演算する第３の演算手段と、第３の演算
   手段で演算された単位ベクトルｉ、ｊ、ｋを座標変換デ
   ータとして用い、その単位ベクトルｉ、ｊ、ｋを座標軸
   とする直交座標系（ｉ、ｊ、ｋ）上で位置と力の制御演
   算を行う第４の演算手段とを有することを特徴とする多
   自由度作業機械の位置と力の制御装置。
2. （２）前記第３の演算手段は、第１の演算手段で演算さ
   れた反力ベクトルｆから単位ベクトルｋを演算し、この
   単位ベクトルｋと第２の演算手段で演算された移動方向
   ベクトルｖとからこれら両ベクトルｋ、ｖに垂直な単位
   ベクトルｊと、単位ベクトルｋ、ｖの張る平面上で単位
   ベクトルｋに垂直な単位ベクトルｉを演算することを特
   徴とする請求項１記載の多自由度作業機械の位置と力の
   制御装置。
3. （３）前記第３の演算手段は、第１の演算手段で演算さ
   れた反力ベクトルｆからワーク表面の法線ベクトルｎを
   演算し、この法線ベクトルｎから法線方向の単位ベクト
   ルｋを演算することを特徴とする請求項２記載の多自由
   度作業機械の位置と力の制御装置。
4. （４）前記第３の演算手段は、第２の演算手段で演算さ
   れた移動方向ベクトルｖから作業用ツールの移動方向の
   単位ベクトルｉを演算し、この単位ベクトルｉと第１の
   演算手段で演算された反力ベクトルｆとから両ベクトル
   ｉ、ｆに垂直な単位ベクトルｊと、単位ベクトルｉ、ｆ
   の張る平面上で単位ベクトルｉに垂直な単位ベクトルｋ
   を演算することを特徴とする請求項１記載の多自由度作
   業機械の位置と力の制御装置。
5. （５）前記第３の演算手段で演算された単位ベクトルｉ
   、ｊ、ｋを他の教示データと共に記憶する記憶手段と、
   この記憶したデータをプレイバック時に再生する再生手
   段とを有することを特徴とする請求項１、２、３、又は
   ４記載の多自由度作業機械の位置と力の制御装置。
6. （６）前記第３の演算手段は、第１の演算手段で演算さ
   れた反力ベクトルｆと第２の演算手段で演算された移動
   方向ベクトルｖとから、作業用ツールの先端を原点とし
   ワーク表面の法線及び接線を座標軸とするワーク表面座
   標系の単位ベクトルｉ、ｊ、ｋを演算することを特徴と
   する請求項１記載の多自由度作業機械の位置と力の制御
   装置。