JPH03136747A - ワークコーナー部の倣い制御装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔産業上の利用分野〕 本発明はワークコーナー部の倣い制御装置に関し、特に
金型等を研磨する研磨ロボットや形状の計測等の倣い装
置などの多自由度作業機械に用いられ、コーナー部を有
するワークに対して連続的且つ滑らかに倣い動作を行わ
せるワークコーナー部の倣い制御装置に関する。

〔従来の技術〕

従来、金型を製作する際金型の切削加工作業はＮＣフラ
イス等によって自動化されているが、切削加工後におい
て加工跡を除去し加工面を鏡面程度まで磨き上げる研磨
作業は、はとんど人手にょって行われている。かかる研
磨作業では、研磨工具をワークに押し付けながら研磨す
べき面に沿って移動させることが必要であり、研磨工具
の制御において力制御が要求されるので、位置制御のみ
が行われる通常の工業用ロボットや工作機械では研磨作
業の自動化を図ることができなかった。そこで最近では
、空気圧等を利用して研磨工具をワークに押し付け、力
制御を行えるようにした研磨装置が提案されている。

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

ところが、現在開発されている力制御を行い得る研磨装
置は、その押付は方向が一定方向に制限され、押付は方
向をワーク形状の状況や作業状況に応じて自由に変更す
ることができないように構成されている。そのため、例
えばワークの被研磨部に垂直等の角度を有したコーナー
部が存在する場合には、このコーナー部を連続的に且つ
滑らかに研磨することができず、研磨作業を自動的に行
う装置としては不完全なものであった。

コーナー部を含むワーク面に対し連続的に且つ滑らかに
研磨を行えるようにするためには、コーナー部であるこ
とを正確に検知し、コーナー部で即座に送り方向と押付
は方向の制御状態を切換えること、またコーナー部の検
知をコーナー側壁面への接触によって行う場合には機器
の破損を避けるため柔らかく接触することが要求される
。

本発明の目的は、被研磨部材であるワークにおいて隣合
う壁面が直角等の角度を形成して隣接するコーナー部が
存在する場合に、コーナー部の手前で送り方向に関し力
制御モードに変換して移動を行い、これによりコーナー
側壁面から過大な反力を受けることなくコーナー部を検
出し、当該コーナー部の箇所も連続的に且つ滑らかに研
磨等の作業を実行することができるワークコーナー部の
倣い制御装置を提供することにある。

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

本発明に係るワークコーナー部の倣い制御装置は、ワー
ク面に設定された押付は力で押し付けられて所定作業を
行いながらワーク面上を移動する手先効果器と、手先効
果器に加わる力を検出する力検出手段と、手先効果器の
押付は力と移動を制御するための制御手段であり、力検
出手段の検出した力と目標とする力との偏差により移動
速度を決める力制御手段を含む前記制御手段とを備え、
第１の面と第２の面によって形成されたコーナー部を有
するワークのコーナー部を含むワーク面に対して前記所
定作業を行う少なくとも２自由度を有する多自由度作業
機械において、コーナー部の手前の位置で、移動方向に
関し位置制御モードから力制御モードに変更し、コーナ
ー部に向って手先効果器を力制御モードで移動させる移
動モード変更手段を備えるように構成される。

本発明に係るワークコーナー部の倣い制御装置は、前記
の構成において、更に、力検出手段の検出した手先効果
器の移動方向の力が設定値よりも大きいか否かを判定し
、前記移動方向の力が設定値よりも大きいときには手先
効果器の押付は方向と移動方向を変更する制御方向変更
手段を備えるように構成される。

〔作用〕

第１の構成を有する本発明によれば、移動モード変更手
段によって、コーナー部の側壁面に当接する直前に送り
方向が力制御の状態になるので手先効果器が側壁面に衝
突しても力制御で対応して過大な反力を受けない。その
ため、コーナー部で手先効果器等を損傷することなく滑
らかな制御状態の移行を行える。

第２の構成を有する本発明によれば、前記の移動モード
変更手段により変更された力制御モードでコーナー部に
柔らかく接触し、その際送り方向において生じる力の検
出に基づき制御方向変更手段によって手先効果器の押付
は方向と送り方向とを変更する。移動モード変更手段と
制御方向変更手段の組み合わせにより、コーナー部を含
むワーク面に対し手先効果器に連続且つ円滑な倣い動作
を行わせることができる。

〔実施例〕

以下に、本発明の実施例を添付図面に基づいて説明する
。

第１図は本発明に係るワークコーナー部の倣い制御装置
の第１実施例を示す工作機械本体及びその制御装置の構
成を示す。

第１図において、１はロボットの主要部をなす例えば研
磨装置の如き工作機械本体で、この工作機械本体１は、
ロボット機構部２と、駆動部であるモータ３と、モータ
３に付設されモータ駆動軸の移動量を検出するエンコー
ダ（又はリニアスケール等）４とから構成される。この
図示例ではロボット機構部２はワークの台座となるｘｙ
テーブル５と、所定の作業を実行する手先効果器を取り
付けるための２テーブル６を備える。ｘｙテーブル５で
はワークがＸ座標軸及びＸ座標軸の少なくともいずれか
一方に沿ってｘｙ平面内を移動し、２テーブル６では手
先効果器が２座標軸に沿って移動する。これらの移動は
前記モータ３の駆動動作によって行われる。各軸におけ
る移動動作はそれぞれ独立に行われるため、モータ３は
各軸ごとに設けられる。従って、エンコーダ４も各軸の
モータに対応して軸ごとに備えられる。しかし図示例で
は、説明の便宜上それぞれ１つのブロックで複数のモー
タ及びエンコーダを示している。Ｘ。

ｙ、ｚの３軸に関し所定のモータ３を動作させると、対
応する駆動対象にカｆｖが与えられ、駆動対象は移動す
る。駆動対象の移動量は位置情報として対応するエンコ
ーダ４によって得ることができる。２テーブル６はアー
ム７を備え、そのリスト部に力センサ８が備えられ、更
にこの方センサ８の先に例えばグラインダ等の作業用ツ
ールである手先効果器９が取り付けられる。

上記の構成によれば、ワークをｘｙテーブル５の上に固
定して配置し、モータ３を介してｘｙテーブル５上のワ
ーク及び２テーブル６上の手先効果器９の位置を適宜に
変更し、ワークに対し手先効果器９を所定の押付は方向
に押付けなから当接させ且つ所定の送り方向に移動させ
ることにより、ワークの表面にグラインダかけ等の作業
を行うことができる。この作業において、力センサ８に
対しては手先効果器９とワーク七の接触によって生じる
力及びトルクが作用し、力センサ８を介して手先効果器
９に反力として生じるカとトルクを検出することができ
る。

次に制御装置の回路構成を説明する。第１図において工
作機械本体１以外の部分１０が制御装置の主要部を構成
する。前述したエンコーダ４及び力センサ８も検出部と
して制御装置に含まれる。

第１図中、ブロック回路図で制御装置１０を構成する要
素を示す。制御装置１０は例えばマイクロコンピュータ
を利用したソフト的構成によって実現される。なお、ハ
ード的電気回路構成でも実現できるのは勿論である。１
１は力目標値設定部、１２は位置目標値設定部である。

力目標値設定部１１には”＋　　Ｙ＊　　Ｚの各座標軸
について押付は力の目標値ｆｒ　　（各軸の目標値を要
素とするベクトルとして表示される）が予め設定され、
力目標値設定部１１は設定された目標値ｆｒを減算器１
３に対し出力する。力目標値の設定には例えば教示デー
タが使用される。位置目標値設定部１２には各軸につい
て手先効果器９の位置の目標値ｐｒ（各軸の目標値を要
素とするベクトルとして表示される）が予め設定され、
位置目標値設定部１２は設定された目標値ｐ「を減算器
１４に対し出力する。位置目標値の設定には例えば教示
データを補間した値が使用される。力目標値設定部１１
と位置目標値設定部工２には、それぞれ、手先効果器９
の押付は方向における押付は力の目標値と送り方向にお
ける移動位置の目標値とが順次に設定される。

前記力センサ８で検出された力は力演鼻部１５に送給さ
れ、力演鼻部１５で、センサ座標系がら手先座標系であ
る基準座標系に変換すると共に手先効果器９の重力分を
差し引く重力補償を行うことによりワークと手先効果器
９の接触点での手先効果器９に加わるカｆを算出する。

力演鼻部１５で得られた力ｔのデータは減算器１３に与
えられる。一方、エンコーダ４で検出された各テーブル
における各軸方向の移動量に係る位置データは位置演算
部１６に送給され、この位置演算部１６で位置演算を行
うことにより手先効果器９での作業点（ワークとの接触
点）の位置ｐを算出する。位置演算部１６で得られた位
置ｐのデータは減算器１４に与えられる。

減算器１３では、力ずと力目標値ｆｒとが比較され、そ
の偏差Δｆ＝ｆ−ｆｔが出力される。

方、減算器１４では、位置ｐと位置目標値ｐｒが比較さ
れ、その偏差Δｐ＝ｐ−ｐｒが出力される。

このようにして得られた上記の各偏差ΔｔとΔｐは位置
・力制御演算部１７に入力される。

位置・力制御演算部１７は仮想コンプライアンス制御系
として構成され、任意に調整できる不感帯幅Ｗが設定さ
れた不感帯演算部１８と、調整可能なバネ定数行列（コ
ンプライアンス行列）ｋが設定されたバネ定数乗算部１
９と、減算器２０と、コントローラゲインｋｃが設定さ
れた特性補償演算部２１とから構成される。かかる構成
によって、不感帯演算部１８を通過したΔｆとバネ定数
乗算部１９を通過したΔｐは減算器２０で減算が行われ
、その後、特性補償演算部２１でコントローラゲインｋ
ｃが乗じられることにより目標速度Ｖを表す速度指令信
号が位置・力制御演算部１７から構成される装置・力制
御演算部１７の出力信号はモータ速度演算部２２に与え
られ、ここでモータ速度操作信号を発生し、サーボアン
プ２３を経てモータ３に供給される。モータ３は操作信
号により目標速度になるように制御される。

上記構成において、仮想コンプライアンス制御は、ソフ
トウェアで形成される制御方式によって手先効果器９に
見掛は上あたかもバネが存在するような動作をさせるも
ので、このバネ定数を制御軸ごとに自由に変更すること
ができる。仮想コンプライアンス制御によれば、その設
定値の与え方によって、位置制御又は力制御、両制御が
複合された制御のモードで動作させることができる位置
と力の制御方式である。バネ定数行列にやコントローラ
ゲインｋｃはＸ軸、ｙ軸、２軸の各軸に対応する値を対
角成分に有する対角行列であり、また不感帯演算部１８
は各軸に対応して設けられる。

例えば、不感帯演算部１８については、不感帯幅を各軸
ごとに任意に設定することができ、不感帯をなくすこと
もできるし、不感帯を大きくとることによってその出力
を常にＯにし、カフィードバックをなくして位置制御の
みを行うこともできる。

バネ定数乗算部１９については、各軸の対角成分ｋｉを
任意に設定することができ、特にｋｉを０にすれば位置
のフィードバックをなくして力制御のみを行うことがで
きる。このように、Ｘ１３’１２の各座標軸はそれぞれ
独立した制御特性（力制御特性又は位置制御特性、ある
いは両特性が複合された制御特性）を有するように設定
することが可能である。

２４は制御方向変更部であり、この制御方向変更部２４
には力演鼻部１５の出力信号が入力される。制御装置１
０によって制御される工作機械本体１の制御方向は、力
目標値設定部１１、位置目標値設定部１２、不感帯演算
部１８、バネ定数乗算部１９のそれぞれにおける設定値
によって決まる手先効果器９の押付は方向及び送り方向
に基づき特定される。従って、手先効果器９の押付は方
向と送り方向を変更すれば、制御方向が変更されたこと
になる。制御方向変更部２４は、第１の制御方向状態の
下で研磨作業を行っている手先効果器９を、その送り方
向の反力を監視しながら、その力が所定の条件を満たし
たときに、予め定められた制御手順に基づき力目標値設
定部１１、位置目標値設定部１２、不感帯演算部１８、
バネ定数乗算部１９におけるＸ軸、ｙ軸、ｚ軸の各設定
値のいずれかを選択的に変更し手先効果器９の押付は方
向及び送り方向を変更することにより第２の制御方向状
態に変更する機能を有する。変更−の仕方についての詳
細な説明は後述される。

制御方向変更部２４は具体的には第１図に示すように力
判定部２５と押付は方向及び送り方向の変更指示部２６
とからなる。力判定部２４は力演鼻部１５から与えられ
る手先効果器９における力情報のうち送り方向の力の大
きさを判定する。その力が設定値ｆ。よりも大きくなっ
た時、手先効果器９がワークコーナー部の壁面に当った
と判断して信号を出し、それに応じて変更指示部２６が
制御装置本体の力目標値設定部１１、位置目標値設定部
１２、不感帯演算部１８、バネ定数乗算部１９のうち作
業手順に基づいていずれかを適宜に選択して設定値変更
指令信号を出し、手先効果器９の押付は方向及び送り方
向を変更する。

次いで、２７は移動モード変更部である。この移動モー
ド変更部２７は位置演算部１６の出力信号である位置デ
ータｐを取込み、その位置データｐを用いて工作機械本
体１における手先効果器９の位置を監視する。そして予
め内部に記憶された制御手順に従い且つ予め教示された
ワークのコーナー部手前の所定の位置データに基づき、
入力された位置データｐを監視しつつ、手先効果器９の
移動位置がコーナー部手前の前記所定位置に達した時、
制御装置１゛０における不感帯演算部１８、バネ定数乗
算部１９、内口標値設定部１１、更に必要に応じて位置
目標値設定部１２の各値をそれぞれ変更し、送り方向の
移動モードを変更する。

具体的には、送り方向の軸に関して、不感帯演算部１８
におけるその不感帯幅を大きい値からほぼ零に近い小さ
い値に変更し、且つバネ定数乗算部１９におけるそのバ
ネ定数を大きい値から零にすると共に、内口標値設定部
１１におけるその力目標値を力制御による移動を可能に
する所要の値に設定することにより、移動モードを位置
制御のモードから力制御のモードに切換える。この場合
、移動モード変更部２７は、位置目標値設定部１２の送
り方向の軸に関する位置目標値が力制御の状態では使用
されないので、そのまま位置制御モードによる移動の値
を計算している状態に保持されても良い。しかし、通常
、送り方向の軸に関し位置制御モードで行われていた移
動計算は中止される。

なお、第１図において便宜的に内口標値設定部１１、位
置目標値設定部１２、不感帯演算部１８、バネ定数乗算
部１９の各設定値を変更するための信号を与える信号線
を、制御方向変更部２４と移動モード変更部で共用させ
ているが、別々に設けることも可能である。

次に、前記構成を有する工作機械本体１及び制御装置１
０の動作について説明する。特にワークコーナー部にお
ける倣い制御における動作を説明する。

先ず、制御装置１０による一般的な制御動作を説明する
。工作機械本体１にｘｙ子テーブル上のワーク表面を倣
って表面研磨等を行わせる曲面倣い動作を実行させた場
合、工作機械本体１の２テーブル６に取り付けた手先効
果器９には反力としての力が加わり、この力はリスト部
の力センサ８で検出される。検出された力は力センサ８
に固定された、センサの変形中心を原点とするセンサ座
標系の値になっているので、この値を、力演鼻部１５に
よって、手先効果器９の適当な場所を原点とし且つ手先
効果器９と共に移動する手先座標系（基準座標系）の値
に変換する。更に、力演鼻部１５では、手先効果器５の
重力分を差し引いて重力補償を行い、重力の影響を除去
する。こうして得られた力１は、内口標値設定部１１か
らの内口標値ｆｒと比較され、減算器１３で力偏差Δｆ
が算出される。

一方、位置については、モータ３に付設されたエンコー
ダ４から出力される値に基づき位置演算部１６によって
手先位置ｐを演算する。この手先位置は手先座標系の原
点位置を基準にしている。

位置演算部１６で得られた位置ｐは、位置目標値設定部
１２で設定された目標位置ｐｒと比較され、減算器１４
で位置偏差Δｐが計算される。

上記の力偏差Δｆと位置偏差Δｐは、位置・力制御演算
部１７に入力され、位置・力制御演算部１７において前
述したように仮想コンプライアンス制御演算が行われ、
速度指令値マが出力される。

この速度指令値Ｖは工作機械本体１において作業を実行
させるための指令値である。速度指令値は、モータ速度
演算部２２でモータ３の回転速度に変換され、サーボア
ンプ２３により当該回転速度でモータ３が動作するよう
に制御を行う。実際は、モータ３でｘｙテーブル５と２
テーブル６を移動させることにより、ワークと手先効果
器９の相対位置を変化させてワークの移動動作を行わせ
且つワークに対する押付は力を生じさせる。

次に前記制御装置１０によるワークコーナー部における
倣い制御を前記第１図と第２図乃至第４図に基づいて説
明する。第２図はコーナー部を含むワーク表面の研磨作
業において制御状態を変更するための手順を示すフロー
チャート、第３図はコーナー部の手先効果器９による作
業状態を示す斜視図、第４図はｚｘ平面内において送り
方向での位置制御モードと力制御モードの変更と、コー
ナー部における押付は方向と送り方向の変更とを説明す
るための図である。なお、第２図の制御手順は一例であ
って、基本的な制御状態の変更の仕方を示している。

第３図において、手先効果器９は研磨用振動工具９ａを
備え、ワーク３０の直角の角度を有するコーナー部付近
の表面を矢印Ａの方向に移動しながら研磨作業を行う。

従って手先効果器９はコーナー部の境界線３１を境に隣
合う壁部表面３２゜３３を連続的に研磨する。第３図中
に示された座標系から明らかなように、手先効果器９は
表面３２ではＸ軸方向に沿って移動しながらＺ軸方向に
押付は力が与えられ、表面３３では２軸方向に沿って移
動しながらＸ軸方向に押付は力が与えられる。その関係
を明確に示したのが第４図である。

第４図において、表面３２上に存する状態Ｂの振動工具
９ａは２軸方向に力ｆが与えられ、Ｘ軸方向に速度Ｖが
生じている。また、表面３３上に存する状態Ｃの振動工
具９ａはＸ軸方向に力ｆが与えられ、ｚ軸方向に速度Ｖ
が生じている。このように、振動工具９ａは、表面３２
上では押付は方向が２軸方向且つ送り方向がＸ軸方向の
第１の制御方向（状態Ｂ）にあり、表面３３上では押付
は方向がＸ軸方向且つ送り方向が２軸方向の第２の制御
方向（状態Ｃ）にある。従って、研磨用振動工具９ａが
状態Ｂから状態Ｃに移るワークコーナー部の研磨作業で
は、制御装置１０における制御状態も第１の制御方向か
ら第２の制御方向に変更する必要がある。このような制
御方向変更の制御の下で、振動工具９ａは、位置ＰＬ、
Ｐ２．Ｐ３の順序で移動していく。かかるコーナー部に
おける制御方向の変更は前記制御方向変更部２４によっ
て行われる。

次に、コーナー部の境界線の位置Ｐ２の直前の位置Ｐ４
は、送り方向であるＸ軸方向に関し位置制御モードが力
制御モードに切換わる位置である。

このワークコーナー部の倣い制御では、振動工具９ａが
コーナー部の表面３３に衝突する直前の位置Ｐ４で送り
方向の制御を位置制御モードから力制御モードに変更し
て、送り方向について力制御モードで表面３３に接触さ
せる。位置Ｐ４についての位置情報は予め教示作業等に
おいて教示させるようにする。位置Ｐ４での送り方向に
関する位置制御モードから力制御モードへの移動モード
の変更は移動モード変更部２７によって行われる。

なお、上記においてｘｙ座標系は第１図のｘｙ子テーブ
ル上に固定された座標系を使用する。Ｘ軸とｙ軸の方向
の移動については実際はｘｙ子テーブルが動くのである
が、説明の便宜上第３図と第４図に示したように手先効
果器９が移動するように仮定して説明する。

次に、第１図乃至第４図に基づいて制御方向変更部２４
と移動モード変更部２７を含む制御装置１０による制御
状態変更の基本的変更例について説明する。

手先効果器９がワーク３０の表面３２を研磨する第１の
制御状態（第４図中の状態Ｂ）では、Ｘ軸方向は、不感
帯演算部１８の不感帯幅Ｗｘを大きくして力のフィード
バックをな（し且つバネ定数乗算部１９のバネ定数ｋｘ
を大きくして位置制御モードにすると共に、速度Ｖで移
動するように位置目標値設定部１２の位置目標値ｌｐｔ
を時々刻々計算して出力する。一方、前記第１の制御状
態で２軸方向は、不感帯演算部１８の不感帯幅Ｗｚを０
（又は小さく）にし且つバネ定数乗算部１９のバネ定数
ｋ　’ｚを０にして位置のフィードバックをなくシ、力
制御モードにすると共に、押付は力ｆで押し付けられる
ように内口標値設定部１１で内口標値ｆｒを設定し出力
する。また、Ｘ軸方向における内口標値ｆｕと２軸方向
における位置目標値については特に値を指定する必要は
なく、任意の値を設定することができる。例えば内口標
値の場合、０でも、ｆｒのままでもよい。位置目標値の
場合、経路の目標値を設定しているが、別の値であって
もよい。このように、第２図のステツプ４０に示される
ように第１の制御状態に基づいて表面３２における手先
効果器９の押付は方向及び送り方向の設定がなされる。

第２図中、ステップ４０における各設定の内容は、注釈
Ｓ、に示される。注釈Ｓ１において“−”は任意の値が
設定されることを示す。従って、第１の制御状態の設定
によれば、手先効果器９は第４図に示されるように表面
３２を押付は力ｆで研磨しながら、ワークコーナー部の
表面３３に向かって所定の速度でＸ軸方向に送られる（
ステップ４１）。表面３２における手先効果器９の研磨
作業は本発明の前提となる従来から提案されている自動
的な研磨作業である。

上記のように、手先効果器９が表面３２の上でＸ軸方向
に速度Ｖの位置制御モードで送られ且つ２軸方向に押付
は力ｆの力制御モードで押付けられながら倣い動作して
いる間、移動モード変更部２７は次のように動作する。

移動モード変更部２７は位置演算部１６の出力を取込ん
で手先効果器９の位置を監視し、その位置が予め設定さ
れた位置Ｐ４に到達したか否かを判定する（ステップ４
２）。判定がＮＯである時はステップ４１．４２を繰り
返す。手先効果器９が位置Ｐ４に到達すると、制御の手
順はステップ４２からステップ４３に移行し、このステ
ップ４３において移動モード変更部２７は送り方向の制
御モードを位置制御モードから力制岨モードに変更する
。そして次のステップ４４では、手先効果器９は表面３
２の上でコーナー部に向って押付は動作及び力制御モー
ドによる送り動作で移動する。なおステップ４３におけ
る変更内容は注釈Ｓ２に記載される。

ここで、ステップ４４における力制御モードによる移動
について説明する。この実施例の如く位置・力制御演算
部１７で位置・力制御により速度指令値を算出して制御
する力制御においては、力制御が行われる方向に物体が
なく拘束されない状態である場合には、内口標値設定部
１１で設定された内口標値に応じた速度で手先効果器９
を移動させることができる。第１図において、移動モー
ド変更部２７の出力する変更指令信号に基づきＸ軸方向
に関しその不感帯幅を０、バネ定数を０にすると共に内
口標値をｆ”ｔとして適当に与えると、送り方向は力制
御モードに設定される。そして、この力制御の状態にお
いて送り方向についてワークから反力を受けない場合に
は、送り方向の速度指令値はコントローラゲインｋｃと
上記の如く設定された内口標値との積で与えられ、この
ため手先効果器９は当該速度で移動することになる。

つまり、送り方向において位置Ｐ４で位置制御モードか
ら力制御モードに変更されたとしても、手先効果器９は
送り方向（Ｘ軸方向）において内口標値設定部１１に設
定された適当な値の内口標値に基づき所要の速度で移動
することができる。このような力制御モードによる移動
では、移動方向に物体が存在した時、その物体に手先効
果器９が接触したとすると、手先効果器９は制御装置１
０による力制御に基づき設定された内口標値となるよう
に制御された状態で止まることになる。従って、手先効
果器９がコーナー部で他の壁面に当接したとしても大き
な衝撃を受けることなく、手先効果器等を含む工作機械
を安全に保持した状態で、次の制御方向変更の制御を実
行し研磨作業に移ることができる。このような力制御に
よる移動に比較して、位置制御モードによる移動では手
先効果器９とコーナー部の表面３３はまともに衝突して
しまうため、移動速度が大きい場合には過大な力が発生
し、その結果ワークの一部又は手先効果器９が破損する
おそれがある。本実施例の力制御モードによる移動の場
合にはこのようなことは全く起こらない。

上記の如く第４図において、位置Ｐ４からコーナー部ま
での間は力制御モードによる移動が行われる。この移動
において、内口標値設定部１１に設定される内口標値ｆ
−ｔは可能な限り小さく設定することにより移動速度を
小さく設定し、コーナー部における衝撃力を小さくする
ことが望ましい。

次いで、制御方向変更部２４の力判定部２５は力演鼻部
１５の出力において手先効果器９の送り方向であるＸ軸
方向において生じる力を検出し（ステップ４５）、送り
方向の力が設定値ｆ。より大きくなったか否かを監視し
ている（ステップ４６）。送り方向の力が設定値ｆ。よ
りも小さい間、すなわち第４図のＢで示される状態の時
はステップ４４．４５．４６を繰り返し、手先効果器９
はＸ軸方向へ所定速度で送られていく。

手先効果器９の振動工具９ａが表面３３に当接すると、
送り方向であるＸ軸方向についての力がｆ、よりも大き
くなる。すると、ステップ４６の判定によってステップ
４７に移り、手先効果器９が表面３３を研磨する第２の
制御方向状態に設定され、押付は方向及び送り方向が変
更される。第２の制御方向では、Ｘ軸方向は、不感帯演
算部１８の不感帯幅Ｗｘが０（又は小さく）及びバネ定
数乗算部１９のバネ定数ｋｘが０の前記力制御モードが
維持されると共に、押付は力ｆで押付けられるように内
口標値設定部１１で右目標値ｆ「を新たに設定される。

一方、第２の制御方向で２軸方向は、不感帯演算部１８
の不感帯幅Ｗｚを大きくし且つバネ定数乗算部１９のバ
ネ定数ｋｚを大きくして位置をフィードバックさせ、位
置制御のモードにすると共に、速度Ｖで移動するように
位置目標値設定部１２の位置目標値ｚｒを時々刻々計算
して出力する。前述と同様に、ｚ軸方向における右目標
値ｆｒ！とＸ軸方向における位置目標値ｐｒｘについて
は特に値を指定する必要はなく、任意の値が設定される
。第２図中、ステップ４７における各設定の内容は、注
釈Ｓ、に示される。

上記の如くステップ４７において制御方向が変更されて
、押付は方向及び送り方向が変更されると、第２の制御
方向に基づいて手先効果器９は、第４図で説明された通
り、Ｘ軸方向を押付は方向、ｚ軸方向を送り方向として
表面３３を研磨しながら移動する（ステップ４８）。ス
テップ４９では予め設定された研磨作業が終了したか否
かが判定され、終了しない場合にはステップ４８を継続
し、作業が終了した場合には、ワークコーナー部の倣い
制御は終了する。

上記において、制御方向変更部２４によって行われる押
付は方向と送り方向の変更に係る時間は、移動モード変
更部２７を備えるために、それほど短時間である必要は
ない。つまり、移動モード変更蔀２７の作用によってコ
ーナー部に衝突する直前に移動方向が力制御モードにな
っているため、コーナー部で押付は方向と送り方向との
切換えが多少遅れても移動方向の力は力制御モードの移
動時に設定された右目標値ｆ’ｒで制御されており、不
都合は生じない。

なお上記の実施例において、各軸の不感帯幅、バネ定数
、右目標値、位置目標値のそれぞれの設定値は、前記の
値に限定されるものではなく、実行しようとする制御の
目的に応じて任意に変更することができるものである。

次に第５図に基づいて本発明の第２実施例を説明する。

この実施例では、位置・力制御演算部１７がハイブリッ
ド制御を実行するように構成され、本発明に係るワーク
コーナー部の倣い制御装置はハイブリッド制御にも適用
できることを示すものである。そのため第１図に示され
た位置・力制御演算部１７の構成とは部分的に異なり、
不感帯演算部１８の代りに制御スイッチ（Ｓｆ）１１８
が配設されると共に、バネ定数乗算部１９の前段には制
御スイッチ（Ｓｐ）１１９が配設され、制御スイッチ１
１９はバネ定数乗算部１９に直列的に接続されている。

その他の基本的構成は第１図に示された構成と同じであ
る。また、制御方向変更部２４及び移動モード変更部２
７のそれぞれから出力される変更指令信号は、必要に応
じ内口標値設定部１１、位置目標値設定部１２、バネ定
数乗算部１９、制御スイッチ１１８，１１９に供給され
、それぞれにおいて設定される所定の各値を変更し得る
ように構成されている。制御スイッチ１１８．１１９の
それぞれのＳｆ、Ｓｐは下記の通り対角行列を表す。

上記５ｌｌｌ〜Ｓ、、、Ｓ□〜Ｓ□はそれぞれ１又は０
の値をとる。上記のハイブリッド制御の構成における制
御動作を、前記実施例による仮想コンプライアンス制御
と比較し対応させて説明する。

Ｘ軸方向に移動しながら２軸方向に押付は力が与えられ
ている時（第４図の状態Ｂ）には、制御スイッチ１１８
はＳｌ、とＳｆｖはＯ，Ｓ、、は１の状態に設定され、
制御スイッチ１１９はＳ　ｐｘと８９７は１、Ｓ□はＯ
の状態に設定される。この制御状態ではＸ軸方向におい
て位置制御、ｚ軸方向において力制御が行われる。

次に表面３２の上をコーナー部に向かっている間に移動
モード変更部２７が位置演算部１６の出力する位置信号
に基づき設定位置Ｐ４を検出すると、移動モード変更部
２７は制御スイッチ１１８のＳ　Ｌｘを１、制御スイッ
チ１１９のＳ□を０にする共に右目標値設定部１１にお
けるＸ軸に関する設定値を第１実施例で説明した前記所
要の値に変更する。かかる移動モード変更部２７による
変更制御動作により、設定位置Ｐ４からコーナー壁面ま
での間は、送り方向について力制御による移動が行われ
ることになる。

そして第４図に示されるＣの状態になる前のコーナー部
を通過する時点で、送り方向（Ｘ軸方向）の力を監視す
る制御方向変更部２４が、前述したように送り方向に関
し所定の力を検出し、この検出を条件に制御方向を変更
するための指令信号を出す。これにより、制御スイッチ
１１８はＳ１１を１、ＳｏとＳ１ｍを０の状態に設定さ
れ、制御スイッチ１１９はＳ□を０、Ｓ　ａｙとＳ　ｐ
ｇを１の状態に設定される。Ｓ　ＩｔとＳ　ｐｆについ
てはコーナー部の直前の位置Ｐ４で変更された値がその
まま維持される。そのため、前記状態ＣにおいてはＸ軸
方向において本来の押付は動作のための力制御、Ｚ軸方
向において位置制御が行われる。なお、制御方向変更部
２４からの指令信号は右目標値設定部１１とバネ定数乗
算部１９と位置目標値設定部１２にも与えられる。これ
らの要素の状態制御は第１実施例と同様に行われるもの
とする。

第１実施例では位置制御と力制御の切換えを不感帯の幅
とバネ定数を変更することで行ったが、前述のように本
実施例では制御スイッチ１１８゜１１９の対角成分の値
を変更して各軸に関しカフィードパックループと位置フ
ィードバックループを切換えることによって行うことが
できる。なお本実施例では、位置制御と力制御の切換え
は基本的に制御スイッチ１１８．１１９で行われるから
、バネ定数乗算部１９の状態を変更する必要性は必ずし
もない。従って各軸のばね定数は所定値に設定しておく
こともできる。

次に第６図と第７図に基づいて本発明の第３実施例を説
明する。第６図において、構成要素として第１図に示さ
れた構成に更に座標変換部５１゜５２．５３が追加され
る。座標変換部５１は力演鼻部１５と減算器１３の間、
座標変換部５２は減算器１４とバネ定数乗算部１９との
間、座標変換部５３は特性補償演算部２１とモータ演算
部２２との間にそれぞれ配置される。第６図中において
、２点鎖線り、の右側領域はセンサ座標系の領域であり
、２点鎖線Ｌ１とＬ２の間の領域は基準座標系の領域で
あり、２点鎖線Ｌ２で囲まれる領域は任意座標系の領域
である。上記の座標変換部５１゜５２．５３は任意座標
系で位置と力の制御演算をを行うためのものである。座
標変換部５１は、力演鼻部１５で演算された力をｘｙテ
ーブル５上に固定された基準座標系の値かＣ手先効果器
９の先端を原点とし、方向のみを回転した任意座標系の
値に変換する。座標変換部５２は、減算器１４で得られ
た位置の偏差を任意座標系の値に変換する。

座標変換部５３は、位置・力制御演算部１７で得られた
任意座標系における指令速度値を基準座標系における値
に変換する。本実施例では、制御方向変更部２４の変更
指令出力によって、座標変換部５１．５２．５３の各変
換値と位置目標値設定部１２の値を変更して制御方向、
すなわち押付は方向と送り方向を変更する。また移動モ
ード変更部２７は、同じく座標変換部５１，５２．５３
の各変換値と右目標値設定部１１と必要に応じて位置目
標値設定部１２の多値を変更して、コーナー部の手前の
所定の位置で送り方向に関し位置制御モードを力制御モ
ードに変更する。制御方向変更部２４と移動モード変更
部２７の各構成及び制御装置のその他の構成については
、第１図で説明した構成と同じであり、同一の要素には
同一の符号を付している。

次に上記構成を有する制御装置の作用について説明する
。

上記座標変換部５１．５２．５３は手先効果器９の先端
を中心に座標系を回転することができ、前述した通り、
位置と力の制御を任意方向の座標系で行うことを可能に
するために設けられた。これによって、ワークを倣う場
合通常ワークの法線方向に押付け、接線方向に移動させ
るのが望ましいので、ｘｙ子テーブルに固定された座標
系に対して傾いた斜面を有するワークや、複雑な形状で
その法線方向が変化するようなワークを倣う場合におい
て、座標系を回転して常にワークの法線方向に押付け、
且つ接線方向に送れるように座標変換を実行するための
ものである。

ここで、座標変換部５１，５２．５３で実行される回転
変換について更に詳しく説明する。基準座標系から見た
任意座標系の座標軸の単位ベクトルを”　＊　　Ｊｉ　
＋　　ｍｃｉ　とすると、３次元ベクトルＵの任意座標
系での表現Ｕ！は、 ｕｉ＝（ｉａ、ｊａ、ｋｇ）Ｔｕ となる。ここで、Ｔは転置行列を表す。すなわち、座標
変換行列は、原点が動かずに回転のみを行う場合は、←
ｉｓ、ｊａ、ｋａ）Ｔで表すことができる。

一方、座標変換部５３は任意座標系から基準座標系への
変換であり、上記の式を変形してｕ＝　［（ｉａ、ｊａ
、ｋｇ）Ｔ］−”ｕｓとなり、［（Ｍａ、ｊａ、１ｇ）
”］−’＝（ｉ！。

ｊｘ、ｋａ）となる。このように座標変換部５１゜５２
．５３での座標変換は、任意座標系の座標軸の単位ベク
トルで与えることができる。工作機械本体１にワーク表
面を倣って表面研磨等を行わせる曲面倣い動作をさせる
場合には、任意座標系として手先効果器９の先端を原点
としてワーク表面の法線及び接線を座標軸とするワーク
表面座標系を与える。この場合、ワーク表面座標系への
座標変換（座標変換部５１．５２）又はワーク表面座標
系からの座標変換（座標変換部５３）の変換行列にはワ
ーク表面の法線と接線の単位ベクトルを使用する。この
ような単位ベクトルは、手先が移動しようとする曲面に
沿って順次その値を内部の記憶部にストアしておくこと
により与えることが可能である。

次に上記第３実施施例による移動モード変更部２７によ
る移動時の制御モードの変更動作と制御方向変更部２４
による押付は方向及び送り方向の変更動作について説明
する。第７図は第５図に示した研磨作業状態図に類似し
た図である。６１は研磨用振動工具であり、手先効果器
９の先部に取り付けられている。ワーク３０は鈍角の角
度を形成する２つの表面６２．６３を有している。振動
工具６１はワーク３０のコーナー部を通過して表面６２
．６３を順次研磨する。コーナー部の手前で送り方向に
ついて位置制御モードから力制御モードに変更するため
の移動モード変更部２７にょる変更動作が行われ、更に
コーナー部を通過する際に座標等の変換が行われる。表
面６２を研磨する状態りは、第４図で説明した状態Ｂと
同じであり、ワーク３０の表面６２の法線方向に力ｆで
押付け、接線方向に速度Ｖで送っており、座標系は接線
方向をＸ軸、法線方向を２軸としている。また、Ｘ軸方
向及び２軸方向の不感帯、バネ定数、位置目標値、右目
標値の設定は第４図のＢの場合と同様である。

手先効果器９の振動工具６１がコーナー部手前のＰ４の
位置に到達すると、この位置は前述の通り移動モード変
換部２７によって検出され、この移動モード変換部２７
の変換指令によって第１実施例の場合と同様に位置Ｐ４
で送り方向について位置制御モードから力制御モードに
変換され、手先効果器９の振動工具６１は力制御モード
でコーナー部に向かって移動する。

コーナー部に来ると、送り方向（Ｘ軸方向）の力がｆ。

よりも大きくなり、制御方向変更部２４の力判定部２５
がこの状態を検出し、これを受けて変更指示部２６が変
更指令を出して、座標変換部５１，５２．５３の変換行
列と位置目標値設定部１２の設定値を変更し、振動工具
６１の押付は方向と送り方向を変更する。

上記の座標変換部５１，５２．５３はその単位ベクトル
を表面６３の接線及び法線の各方向に変更する。これら
の値は予め与えられることにより記憶部にストアしてお
くこともできるし、また振動工具６１の接触面の傾斜角
を与えたり又は教示時に表面６３の２点に手先効果器９
を接触させ傾斜角を自動的に算出させることにより、演
算装置で自動的に逐次算出させるように構成することも
できる。また、位置目標値設定部１２は、表面６３の研
磨作業においては前記実施例と同様に、振動工具６１が
速度Ｖで移動するよう位置目標値を時々刻々計算し設定
値として出力する。なお、Ｘ軸方向の制御モードは移動
モード変更部２７によってＰ４の位置から力制御モード
になっているので、Ｘ軸方向に関しては制御方向変更時
において、その不感帯幅とバネ定数を大きくするよう不
感帯幅演算部１８とバネ定数乗算部１９の値を変更し、
位置制御モードに設定する。またこの時力目標値設定部
１１の設定値は何でも良いが、通常０の値に設定する。

以上の制御方向変更部２４による変更動作によって、振
動工具６１の研磨作業の状態が変更され、振動工具６１
は、表面６３の接線方向をＸ軸、法線方向を２軸とした
座標系において、法線方向であるｚ軸方向に押付は力を
生じながら、且つ接線方向であるＸ軸方向に速度Ｖで移
動して、ワーク表面６３を倣っていく。本実施例の場合
には、コーナー部のなす角度が直角に限定されないので
、特に非直角のコーナー部に最適である。ただし、この
実施例においては振動工具６１の接触面の角度をコーナ
ー部の傾斜角度に対応させて傾斜を持たせであるが、こ
の構成は必ずしも必要ではなく、目的に応じてその周辺
の構成を適宜に変更することができる。

次に本発明の他の実施例を第８図に従って説明する。こ
の図では特に特徴のある要部のみの構成を取出して示し
ている。その他の構成は第１図に示された構成と同じで
ある。特徴的な点は、移動モード変更部２７で移動モー
ドを変更する位置を位置演算部１６の出力する位置信号
に基づいて検出するのではなく、近接スイッチ等の近接
センサ７０の検出信号を用いて検出するように構成した
点である。この近接センサは、工作機械本体１のアーム
等や手先効果器９の適当な箇所に取付けられる。この近
接センサ７０の出力信号によってコーナー部の手前の位
置を検出し、送り方向の移動モードを変更する。

上記実施例の有利な点は、コーナー部の側壁面との距離
を直接的に計測することができるので、モード変更する
位置をティーチングする必要が無く、ワークの据付誤差
や工具の磨耗等による誤差の影響を受けることがないた
め動作が確実となり、位置Ｐ４とコーナー部の距離を短
くとることも可能となる。

また本発明では、送り方向の力制御モードを、力の偏差
に比例した速度指令値を出すことにより行うが、その意
味するところは力のフィードバックがない非接触状態で
右目標値に応じた速度で移動する制御系を構成するもの
である。この制御系の中には、力偏差に応じた位置増分
ΔＸを計算して位置制御系の目標値を順次に加算してい
くものや、速度フィードバック或いはこれに類したフィ
ードバックを有するトルク制御系を用いるものなど、種
々な形態が含まれる。

前記の各実施例では３自由度の工作機械を例に挙げて説
明したが、本発明はこれに限定されるものではなく、多
自由度の産業用ロボットに適用できるものである。

また前記の各実施例の説明において、ｙ軸方向の動きに
ついては説明の便宜上できる限り触れないようにした。

しかしながら、第４図において、例えばワークの表面３
２を表面３３に向かって移動する手先効果器９が表面３
３に当った後、境界線３１に沿ってｙ軸方向に進む場合
、或いは表面３３に当った後先きた経路を戻る場合など
もある。

上記第１の例の場合には、ｙ軸方向が送り方向になるの
で位置制御が行われ、Ｘ軸方向及び／又はｚ軸方向につ
いて力制御が行われることになる。

従って、このような場合には、前述した制御方向変更部
２４の変更指令に基づきｘ、ｙ、ｚの各軸に関し手先効
果器の動作を定める所要の位置と力の制御が行われる。

〔発明の効果〕

以上の説明で明らかなように、本発明によれば次のよう
な効果が生じる。

力制御が要求される研磨等の作業のための多自由度工作
機械において、ワークに直角又は非直角の角度を有した
コーナー部が存在したとしても、コーナー部の手前で力
制御モードに切換えて力制御モードで移動するようにし
たため、手先効果器がコーナー側壁面で過大な力を受け
ることなく、従ってコーナー部でワークや工具等に損傷
を与えることなく、ワークを連続的に且つ滑らかに倣い
、研磨等の作業を行うことができる。また、コーナー部
を検出し手先効果器の押付は方向と送り方向を変更する
ように構成したため、コーナー部を含むワーク面を円滑
に研磨等することができる。更に、接触時のコーナー側
壁面への押付は力は、力制御モードにおける移動で設定
された右目標値により与えられるので、自由に選択する
ことができ、容易に最適の接触状態に制御することがで
きる。

上記のようにワークのコーナー部を連続的に且つ滑らか
に研磨等することができるようになったので、力制御が
必要とされる作業を行う多自由度工作機械を完全に自動
化することができる。

また制御方式に仮想コンプライアンス制御やハイブリッ
ド制御が適用された工作機械では、各軸ごとに制御のモ
ードが異なる場合においても、コーナー部において手先
効果器の押付は方向と送り方向を変更するとき、押付は
方向と送り方向における仮想コンプライアンス制御等の
設定状態を保持しつつ作業状況の変更に応じて押付は方
向と送り方向を変更することができる。

更に、座標変換部に設定された変換行列を利用して手先
効果器の押付は方向と送り方向を変更するように構成さ
れた倣い制御装置によれば、直角以外の角度を有するコ
ーナー部であっても倣い動作を行うことができる。

【図面の簡単な説明】

第１図は本発明の第１実施例に係るワークコーナー部の
倣い制御装置が適用された工作機械とその制御装置を示
す構成図、第２図は移動モード変更部と制御方向変更部
による基本の変更方法を示すフローチャート、第３図は
コーナー部における作業用ツールの移動状態を説明する
ための斜視図、第４図はコーナー部における各表面にお
ける押付は方向と送り方向を示す説明図、第５図は本発
明の第２実施例に係るワークコーナー部の倣い制御装置
が適用された工作機械とその制御装置を示す構成図、第
６図は本発明の第３実施例に係るワークコーナー部の倣
い制御装置が適用された工作機械とその制御装置を示す
構成図、第７図は第３実施例における第３図と同様な図
、第８図は本発明の他の実施例を示す要部構成図である
。 〔符号の説明〕 １・・・・・・工作機械本体 ２　・　− ３−・ ４　・　・ ８　・　・ ９　・　・ ０１１ １１　・ １２　・ １３゜ ５− １６　・ ７１１ １８　＠ １９　・ ２１　・ ２２　・ ２４　・ ２５　・ ２６　・ ・ロボット機構部 ・モータ 嶋エンコーダ ・力センサ ・手先効果器 ・制御装置 ・右目標値設定部 ・位置目標値設定部 ０ ・減算器 ・力演鼻部 ・位置演算部 ・位置・力制御演算部 ・不感帯演算部 ・バネ定数乗算部 ・特性補償演算部 ・モータ速度演算部 ・制御方向変更部 ・力判定部 ・押付は方向及び送り方向変更指 ２７　・　・　・　・ ３０　・　・　・　− ３３゜ ５２゜ ３２゜ ７０　・ ５１゜ 承部 ・移動モード変更部 ・ワーク ６２．６３ ・ワーク表面 ３ ・座標変換部 ・近接センサ

Claims (2)

【特許請求の範囲】
1. （１）ワーク面に設定された押付け力で押し付けられて
   所定作業を行いながら前記ワーク面上を移動する手先効
   果器と、前記手先効果器に加わる力を検出する力検出手
   段と、前記手先効果器の押付け力と移動を制御するため
   の制御手段であり、前記力検出手段の検出した力と目標
   とする力との偏差により移動速度を決める力制御手段を
   含む前記制御手段とを備え、第１の面と第２の面によっ
   て形成されたコーナー部を有するワークの前記コーナー
   部を含むワーク面に対して前記所定作業を行う少なくと
   も２自由度を有する多自由度作業機械において、前記コ
   ーナー部の手前の位置で移動方向に関し位置制御モード
   から力制御モードに変更し、コーナー部に向って前記手
   先効果器を力制御モードで移動させる移動モード変更手
   段を備えることを特徴とするワークコーナー部の倣い制
   御装置。
2. （２）請求項１において、前記力検出手段の検出した手
   先効果器の移動方向の力が設定値よりも大きいか否かを
   判定し、前記移動方向の力が設定値よりも大きいときに
   手先効果器の押付け方向と移動方向を変更する制御方向
   変更手段を備えることを特徴とするワークコーナー部の
   倣い制御装置。