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

Description

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

〔従来の技術〕

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

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

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

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

例えば、位置と力の制御によりロボットが行う作業の
１つにワーク表面を倣って表面研磨やバリ取り等を行な
わせる曲面倣い動作がある。第５図はこのような曲面倣
い動作を模式的に示すもので、ロボット50の手先効果器
51がワーク表面52を倣って動くように制御される。手先
効果器51に加わる力及びモーメントは力センサ53により
検出される。この曲面倣い動作を仮想コンプライアンス
制御を応用して行う場合、一例として第６図に示すよう
に行われる。第６図は簡単のためｘ−ｚ平面で説明する
ものであり、この例ではｘ軸方向には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を小さくして対応するしかな
く、効率的でなく実用性に乏しくなる。

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

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

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

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

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

前記第５の演算手段は、好ましくは、第２の演算手段
で求められた位置及び姿勢の偏差 に前記任意座標の各座標軸ごとに設定したばね定数から
なるばね定数行列Ｋを乗じる手段と、前記第４の演算手
段で求められた力及びモーメントの偏差 に前記任意座標の各座標軸ごとに設定した不感帯の演算
を行う手段と、不感帯手段の演算値 を減算する手段と、減算手段の減算値 に所定の特性補償演算を行って前記制御量として速度指
令値 を求める特性補償演算手段とで構成されている。この場
合、特性補償演算手段は、仮想質量行列Ｍと仮想粘性係
数行列Ｃとによって の式から仮想コンプライアンス制御の演算を行い、速度
指令値 を演算するようにしてもよい。

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

〔作用〕

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

特に曲面倣い動作を行わせる場合には、その任意座標
系として、作業用ツールの先端を原点としてワーク表面
の法線及び接線を座標軸とするワーク表面座標系を設定
することにより、法線方向を常にｚ軸、移動しようとす
る接線方向を常にｘ軸方向にとることができる。そこで
第５の演算手段で仮想コンプライアンス制御を採用し、
ｘ軸方向に位置制御、ｚ軸方向に力制御を行わせた場合
には、曲面の勾配は座標設定等の誤差によるもののみと
なり、ｚ軸方向の力の偏差は小さく、ｘ軸方向の移動速
度即ち送り速度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に送られ、各駆動モータの速度指令値
によってロボット１のモータを駆動する。この中にはモ
ータに取り付けたタコジェネレータからの速度フィード
バックも含まれる。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

ロボット１にワーク表面を倣って表面研磨やバリ取り
等を行なわせる曲面倣い動作をさせる場合には、任意座
標系として作業用ツールの先端を原点としワーク表面の
法線及び接線を座標軸とするワーク表面座標系を与え
る。この場合、ワーク表面座標系への座標変換（座標変
換部11,16）又はワーク表面座標系からの座標変換（座
標変換部22）の変換行列には、ワーク表面の法線及び接
線の単位ベクトルを用いる。これは例えば、手先が移動
しようとする曲面に沿って順次その値を記憶しておくこ
とにより与えることができる。

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

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

以上の実施例は本発明を６自由度のロボットの制御に
適用した例であるが、本発明は基本的には２以上の自由
度を持つものであれば、それ以外の多自由度作業機械に
適用できるものである。第４図は本発明を３軸の工作機
械に適用した例である。図中第１図に示す部材と同等の
部材には同じ符号を付している。

第４図において、符号30は工作機械本体であり、工作
機械本体30にはxyテーブル31とｚ軸駆動機構32とが取り
付けられている。xyテーブル31は、x,yの直交方向に動
くテーブルであり、この上に加工すべきワークを置く。
ｚ軸駆動機構32は、ｚ方向（上下方向）に動き、この先
に手先加工器４を取り付ける。ｚ軸駆動機構32はxyテー
ブル31の運動によって動かない。

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

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

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

このようにこの実施例では、第１図に示す実施例の場
合のように関節角を用いず、直接xyz座標系を算出で
き、姿勢変化がないため、姿勢及びモーメントに関する
演算がない。従って図中のベクトル 等は３次元ベクトルとなり、K,M,Cや座標変換行列は３
×３行列になり、演算は第１図の実施例に比べて簡単な
ものになる。

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

なお以上の実施例は、作業内容が作業用ツールをワー
ク表面に倣って移動させるものであったため、任意座標
系としてワーク表面座標系をとった場合につき説明した
が、任意座標系はワーク表面座標系に限らず、作業内容
において適宜最適の座標系を設定できるものである。例
えば３次元クランク回しなどの作業においては、作業用
ツールの拘束による反力の方向及び移動方向により拘束
座標系を定め、この拘束座標系を任意座標系にすればよ
く、これにより適切かつ効率的なクランク回し作業を行
うことができる。

〔発明の効果〕

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

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

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

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

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

【図面の簡単な説明】

第１図は本発明の一実施例による位置と力の制御装置を
示すブロック図であり、第２図はその制御装置を用いて
一定押付力制御で曲面を倣う例を示す説明図であり、第
３図は同制御装置を用いて仮想バネによって曲面を倣う
例を示す説明図であり、第４図は本発明の他に実施例に
よる位置と力の制御装置を示すブロック図であり、第５
図はロボットによって曲面を倣う動作を示す概略図であ
り、第６図は従来の制御方法を用いて一定押付力制御に
より曲面を倣う例を示す説明図であり、第７図は従来の
制御方法を用いて仮想バネによって曲面を倣う例を示す
説明図である。 符号の説明 １……ロボット、３……力センサー ４……手先効果器（作業用ツール） ５……エンコーダ（位置検出手段） ６……制御装置、７……力設定装置 ８……位置設定装置 10……座標変換部（第８の演算手段） 11……座標変換部（第３の演算手段） 12……力偏差演算部（第４の演算手段） 14……位置演算部（第７の演算手段） 15……位置偏差演算部（第１の演算手段） 16……座標変換部（第２の演算手段） 17……位置と力の制御演算部（第５の演算手段） 18……不感帯演算部、19……バネ定数演算部 20……減算部、21……特性補償演算部 22……座標変換部（第９の演算手段） 23……角速度演算部（第６の演算手段）

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

Claims (5)

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