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

Description

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

〔従来の技術〕

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

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

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

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

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

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

このように、第12図に示すように任意曲面を倣わせる
には限界があるので、曲面の目標値を記述し、この記述
（教示）誤差や工具摩耗やワークのバラつき誤差を仮想
バネで吸収するようにk,cの値を設定したのが第13図で
ある。この例では、曲面での上記誤差を吸収するために
ｚ軸方向だけでなくｘ軸方向にも仮想バネを入れる必要
がある。従ってこの場合、送り方向にバネの影響が入る
ので目標位置と実際の位置との間の位置の偏差が増大し
てしまい、適切な作業ができなくなるなどの不都合を生
じる。特に曲面の変化の大きい所では、位置の偏差の大
きいまま送り方向が変わることにより、削り過ぎ等の悪
影響が出る。

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

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

上記目的は、少なくとも位置の３自由度を持つロボッ
トや工作機械等の多自由度作業機械の位置と力の制御装
置において、作業用ツールに加わる少なくとも直交３軸
方向の力を検出する第１の検出手段と、作業用ツールの
位置又は速度を検出する第２の検出手段と、第１の検出
手段で検出された力データから、作業用ツールがワーク
表面から受ける反力ベクトル を演算する第１の演算手段と、第２の検出手段で検出さ
れた位置または速度データから作業用ツールの移動方向
ベクトル を演算する第２の演算手段と、第１及び第２の演算手段
で演算された反力ベクトル とから互いに直交する単位ベクトル を演算する第３の演算手段と、第３の演算手段で演算さ
れた単位ベクトル を座標変換データとして用い、その単位ベクトル を座標軸とする直交座標系 上で位置と力の制御演算を行う第４の演算手段とを有す
ることを特徴とする多自由度作業機械の位置と力の制御
装置によって達成される。

第３の演算手段は、一例として、第１の演算手段で演
算された反力ベクトル を演算し、この単位ベクトル と第２の演算手段で演算された移動方向ベクトル とからこれら両ベクトル と、単位ベクトル に垂直な単位ベクトル を演算することができるこのとき第３の演算手段は、曲
面倣い動作の場合には、第１の演算手段で演算された反
力ベクトル を演算し、この法線ベクトル を演算する。

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

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

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

〔作用〕

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

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

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

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

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

〔実施例〕

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

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

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

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

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

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

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

座標変換部11で座標変換された力及びモーメント は、力目標値設定部７で設定された力及びモーメントの
目標値 と力偏差演算部12で比較され、偏差 を演算する。

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

力偏差演算部12で演算したワーク表面座標系での力及
びモーメントの偏差 と座標変換部16でワーク表面座標系に変換した位置及び
姿勢の偏差 は位置と力の制御演算部17に送られる。この位置と力の
制御演算部17では、偏差 から、ワーク表面座標系での位置と力の制御演算を行
い、この座標系での指令速度 を演算する。本実施例では、この演算部に仮想コンプラ
イアンス制御による制御演算を使用する。

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

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

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

位置と力の制御演算部17で演算された速度指令値 はワーク表面座標系での値なので、これを座標変換部22
でベース座標系での速度 に変換する。このベース座標系での速度指令値 は角速度演算部23で各関節の角速度指令値 からモータ速度演算部24で各駆動モータの速度指令値を
演算する。モータ速度演算部24で演算された速度指令値
はサーボアンプ25に送られ、各駆動モータの速度指令値
によってロボット１のモータを駆動する。この中にはモ
ータに取り付けたタコジェネレータからの速度フィード
バックも含まれる。

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

第１図においてこのワーク座標系演算部分は、座標変
換部10（第２図参照）で基準座標系（ベース座標系）に
変換された力のデータからワーク表面の法線方向ベクト
ル を求める法線方向演算部60と、位置演算部14（第２図参
照）で求められた手先の位置を手先の移動に従って順次
記憶する記憶部61と、記憶部61に記憶されたデータをも
とに、移動方向ベクトル を求める移動方向演算部62と、法線方向ベクトル とから、第３図の手順でワーク表面座標の座標軸 を演算する座標軸演算部63とからなっている。記憶部61
で記憶する位置の個数は移動方向演算部62の演算で用い
る範囲とされる。座標軸演算部63で演算されたワーク表
面座標の座標軸 は、第２図に示す座標変換部11,16,22の座標変換行列用
のデータとされる。

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

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

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

このようにして得られたワーク表面座標系での力 は、力目標値設定部７で設定されたワーク表面座標系上
の目標力 と比較され、力偏差演算部12によって力偏差出 が演算される。

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

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

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

このようにして求められた位置及び姿勢の偏差 はベース座標系で表わされているので、これを座標変換
16によってワーク表面座標系に変換する。この変換は、
座標変換部11の変換と同様、後述する座標軸演算部63の
演算データ を用いて行い、かつ回転変換として与えられる。

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

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

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

位置と力の制御演算部17で演算された速度指令値 はロボット１を動かすための指令値である。この速度指
令値 はワーク表面座標系での値なので、これを座標変換部22
で一旦ベース座標系の値 に変換する。この座標変換には、後述する座標軸演算部
63の演算データ が用いられる。ベース座標系に変換された速度指令値 は角速度演算部23によってロボットの各関節の角速度 に変換され、さらにモータ速度演算部24で、モータの回
転速度に変換した後、サーボアンプ25によってこの速度
で動くようにロボット１を制御する。

ここで座標変換部11,16,22で行われる回転変換につき
さらに詳しく説明する。

ベース座標系から見たワーク表面座標系の座標軸の単
位ベクトルは前述した となる。ここでＴは転置行列を表す。即ち、座標変換行
列は、原点が動かずに回転のみ行う場合は、 で表せる。座標変換部11,16で使用する行列は６×６行
列であるが、これは並進と回転、それぞれにこの変換を
行えばよく、その変換行列は となる。一方、座標変換部22はワーク表面座標系からベ
ース座標系への変換であり、上記式を変形して となり、 となるので、この座標変換部22の変換行列は で表せる。

このように座標変換部11,16,22での座標変換はワーク
表面座標系の座標軸の単位ベクトル で与えることが出来、第１図に示した演算部分で前述し
たように単位ベクトル を演算すれば、ワーク表面座標系へのまたはワーク表面
座標系からの座標変換が可能である。

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

第４図はワーク表面座標系の与え方を示す図である、
ワーク表面座標系はワーク表面の法線及び接線方向を座
標軸にとるが、接線方向のとり方は一義的には決まらな
いので、本実施例では接平面上で手先の移動方向を座標
軸の１つにとり、第４図（ａ）に示すように は移動方向にある接線方向の単位ベクトル、 に垂直にとった単位ベクトルであり、座標系 が右手系となるようにとられる。

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

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

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

このようにして法線方向演算部60で求めた法線方向ベ
クトル と、移動方向演算部62で求めた移動方向ベクトル を用いてワーク表面座標座標軸演算部63によって、座標
軸の単位ベクトル の方向は、第４図（ｂ）に示すように の方向と同一になるはずであるが、測定誤差やノイズに
よって必ずしも同一にはならず、 が直交しないことがある。そこで、本実施例では力の値
は比較的正確に測定出来たものとして法線方向 と垂直になるように定める。

はこれらに垂直な方向である。この求め方を示したの
が、第３図のフローチャートである。

まず手順70において、法線ベクトル で割って正規化し、長さ１の単位ベクトル に垂直な方向を求めるために、 を演算する。そして手順72でこれを正規化した単位ベク
トルを とすれば、 と垂直な単位ベクトルになる。

ここで、手順71において、 を求めたが、これは の正規化を行うのは手順71の後でもよい。

このようにして、ワーク表面の座標軸 を第２図に示す制御系の座標変換部11,16,22へ送り、ワ
ーク表面座標系とベース座標系との間の座標変換に使用
する。

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

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

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

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

第８図に本発明のワーク座標系演算部分のさらに他の
実施例を示す。図中第１図に示す部材と同等の部材には
同じ符号を付してある。第１図に示す実施例では、座標
変換部10で求められた力のベクトルからワーク表面の法
線方向ベクトル とから座標軸の単位ベクトル を求めたが、この場合は摩擦力の影響が除去できないと が不正確となり、第４図（ｂ）に示すように の方向に正確に一致しなくなる。一方、移動方向 は比較的正確に求めることができるので、これを基準と
して を求めれば、摩擦力の影響を排除することができる。第
８図はこのような実施例を示すもので、第１図の法線方
向演算部60は無く、座標変換部10でベース座標系に変換
された反力ベクトル は直接ワーク表面座標座標軸演算部90に送られ、ここ
で、その力ベクトル とから、ワーク表面座標系の座標軸 を演算する。即ち、移動方向ベクトル を含む平面内で はこれらに垂直な方向として定める。

座標軸演算部90でのこの演算手順を示したのが第９図
のフローチャートである。まず手順91で、移動方向ベク
トル で割って正規化し、長さ１の単位ベクトル とする。次に手順92で、反力ベクトル に垂直な方向を求めるために を演算する。次に手順93で、これを正規化した単位ベク
トルを とする。次に手順94で、 とすれば と垂直な単位ベクトルとなる。

ここで、手順92において を求めたが、これは の正規化を行うのは手順92の後でもよい。

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

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

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

〔発明の効果〕

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

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

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

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

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

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

【図面の簡単な説明】

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

───────────────────────────────────────────────────── フロントページの続き (72)発明者 山田 一徳 茨城県土浦市神立町650番地 日立建機 株式会社土浦工場内 (56)参考文献 特開 平１−222311（ＪＰ，Ａ) 特開 昭63−133208（ＪＰ，Ａ)

Claims (6)

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