JP2009066713A - 姿勢制御方法および姿勢制御装置 - Google Patents

Abstract

【課題】簡易な制御により、丸管の処理面に対し、処理手段の姿勢を所望の姿勢で臨ませることができる姿勢制御方法および姿勢制御装置を提供する。
【解決手段】処理手段と、処理手段により処理が施される丸管の処理面と、の間の距離を４つの距離計測手段により計測して、処理手段の姿勢を制御する姿勢制御方法であって、第１計測距離から第２計測距離を引いた差分と、第３計測距離から第４計測距離を引いた差分とに基づいて、ピッチ軸周りの姿勢を制御し、第１計測距離から第３計測距離を引いた差分と、第２計測距離から第４計測距離を引いた差分とに基づいて、ヨー軸周りの姿勢を制御し、第２計測距離および第３計測距離を合算した第１合算距離と、第１計測距離および第４計測距離を合算した第２合算距離との差分とに基づいて、ロール軸周りの姿勢を制御する。
【選択図】 図２

Description

本発明は、丸管の処理面に対する処理手段の姿勢を制御する姿勢制御方法および姿勢制御装置に関するものである。

従来の姿勢制御装置として、蒸気発生器の水室内に立設した旋回支持部と、旋回支持部に設けられたスライドテーブルと、スライドテーブルに装着されたマニピュレーターと、マニピュレーターの先端に装着された先端工具（処理手段）とを備えた先端工具案内装置が知られている（特許文献１参照）。

この先端工具案内装置では、マニピュレーターを制御して、水室に設けられた管台の内周面のショットピーニング領域に沿って先端工具を移動（案内）している。このとき、先端工具はマニピュレーターによって内周面に押し付けられ、これにより、先端工具の姿勢は、管台の内周面に倣った姿勢となる。つまり、先端工具の姿勢は、マニピュレーターにより内周面に押し付けられて密着させることで、所定の姿勢となるように制御されている。

特開２００７−１８１９０９号公報

しかしながら、従来の先端工具案内装置によれば、マニピュレーターにより先端工具を内周面に押し付けてしまうと、押付方向によっては、先端工具案内装置の剛性が、内周面からの反力に耐えることができない。このため、先端工具案内装置が僅かに撓んだり、または、ねじれてしまったりすることで、押し付けて密着させたはずの先端工具が、僅かに浮き上がってしまう場合がある。この場合、先端工具の浮きにより、先端工具による処理不良を引き起こしてしまう虞がある。この問題を解消すべく、先端工具案内装置の剛性を高めることが考えられるが、先端工具案内装置は、水室内に搬入することを考慮して設計されているため、設計上における制約により、所定の剛性以上の剛性にすることが困難である。

そこで、本発明は、簡易な制御により、丸管の処理面に対し、処理手段の姿勢を所望の姿勢で臨ませることができる姿勢制御方法および姿勢制御装置を提供することを課題とする。

本発明の姿勢制御方法は、丸管の内周面または外周面に対して処理を施す処理手段と、処理手段により処理が施される丸管の処理面と、の間隙の距離を計測して、処理面に対する処理手段の姿勢を制御する姿勢制御方法であって、処理手段には、複数の距離計測手段が設けられると共に、少なくとも任意の４つの距離計測手段が矩形の４つの頂点の位置関係となって処理面に臨むように配置され、任意の４つの距離計測手段のうち、一方の対角に位置する２つの距離計測手段によりそれぞれ計測された計測距離を合算した第１合算距離と、他方の対角に位置する２つの距離計測手段によりそれぞれ計測された計測距離を合算した第２合算距離との差分に基づいて、ロール軸周りの回転方向における処理手段の姿勢を制御する姿勢制御工程とを備えたことを特徴とする。

この場合、複数の距離計測手段が臨む処理面には、処理面において管軸方向に延びる第１仮想境界線および第１仮想境界線に直交する直交方向に延びる第２仮想境界線に基づいて、４つの仮想区画領域が設定され、４つの仮想区画領域は、第１仮想区画領域と、第１仮想区画領域の管軸方向に隣接する第２仮想区画領域と、第１仮想区画領域の直交方向に隣接する第３仮想区画領域と、第２仮想区画領域の直交方向に隣接すると共に第３仮想区画領域の管軸方向に隣接する第４仮想区画領域とで構成されており、任意の４つの距離計測手段のうち、第１距離計測手段を第１仮想区画領域に臨ませ、第２距離計測手段を第２仮想区画領域に臨ませ、第３距離計測手段を第３仮想区画領域に臨ませ、第４距離計測手段を第４仮想区画領域に臨ませる計測手段位置決め工程をさらに備え、姿勢制御工程では、第２距離計測手段により計測した第２計測距離および第３距離計測手段により計測した第３計測距離を足し合わせた第１合算距離と、第１距離計測手段により計測した第１計測距離および第４距離計測手段により計測した第４計測距離を足し合わせた第２合算距離との差分に基づいて、ロール軸周りの回転方向における処理手段の姿勢を制御することが、好ましい。

また、本発明の他の姿勢制御方法は、丸管の内周面または外周面に対して処理を施す処理手段と、処理手段により処理が施される丸管の処理面と、の間隙の距離を計測して、処理面に対する処理手段の姿勢を制御する姿勢制御方法であって、処理手段には、矩形の４つの頂点の位置関係となって処理面に臨むように４つの距離計測手段が設けられ、４つの距離計測手段が臨む処理面には、処理面において管軸方向に延びる第１仮想境界線および第１仮想境界線に直交する直交方向に延びる第２仮想境界線に基づいて、４つの仮想区画領域が設定され、４つの仮想区画領域は、第１仮想区画領域と、第１仮想区画領域の管軸方向に隣接する第２仮想区画領域と、第１仮想区画領域の直交方向に隣接する第３仮想区画領域と、第２仮想区画領域の直交方向に隣接すると共に第３仮想区画領域の管軸方向に隣接する第４仮想区画領域とで構成されており、４つの距離計測手段のうち、第１距離計測手段を第１仮想区画領域に臨ませ、第２距離計測手段を第２仮想区画領域に臨ませ、第３距離計測手段を第３仮想区画領域に臨ませ、第４距離計測手段を第４仮想区画領域に臨ませる計測手段位置決め工程と、第１距離計測手段により計測した第１計測距離から第２距離計測手段により計測した第２計測距離を引いた差分と、第３距離計測手段により計測した第３計測距離から第４距離計測手段により計測した第４計測距離を引いた差分とに基づいて、ピッチ軸周りの回転方向における処理手段の姿勢を制御し、第１計測距離から第３計測距離を引いた差分と、第２計測距離から第４計測距離を引いた差分とに基づいて、ヨー軸周りの回転方向における処理手段の姿勢を制御し、第２計測距離および第３計測距離を足し合わせた第１合算距離と、第１計測距離および第４計測距離を足し合わせた第２合算距離との差分に基づいて、ロール軸周りの回転方向における処理手段の姿勢を制御する姿勢制御工程とを備えたことを特徴とする。

この場合、処理手段は、その姿勢を処理面に倣わせた状態で処理面に沿って移動する倣い動作を行うように構成され、処理手段の倣い動作時において、姿勢制御工程をリアルタイムに実行して、処理手段の姿勢をフィードバック制御することが、好ましい。

これらの場合、第１計測距離、第２計測距離、第３計測距離および第４計測距離の平均距離から予め設定した設定距離を引いた差分に基づいて、処理手段と処理面との間隙を設定距離とする間隙距離設定工程を、さらに備えたことが、好ましい。

また、これらの場合、姿勢制御工程では、予め設定したピッチ軸周りにおける処理手段の初期姿勢、予め設定したヨー軸周りにおける処理手段の初期姿勢、および予め設定したロール軸周りにおける処理手段の初期姿勢に基づいて、処理手段の姿勢を制御していることが、好ましい。

また、本発明の姿勢制御装置は、丸管の内周面または外周面に対して処理を施す処理手段の姿勢を制御する姿勢制御装置であって、処理手段に設けられ、処理手段と処理手段により処理が施される丸管の処理面との間隙の距離を計測する複数の距離計測手段と、複数の距離計測手段のうち、少なくとも任意の４つの距離計測手段の計測結果に基づいて、処理面に対する処理手段の姿勢を制御する姿勢制御手段と、を備え、任意の４つの距離計測手段は、矩形の４つの頂点の位置関係となって処理面に臨むように配置され、姿勢制御手段は、任意の４つの距離計測手段のうち、一方の対角に位置する２つの距離計測手段によりそれぞれ計測された計測距離を合算した第１合算距離と、他方の対角に位置する２つの距離計測手段によりそれぞれ計測された計測距離を合算した第２合算距離との差分に基づいて、ロール軸周りの回転方向における処理手段の姿勢を制御することを特徴とする。

この場合、複数の距離計測手段が臨む処理面には、処理面において管軸方向に延びる第１仮想境界線および第１仮想境界線に直交する直交方向に延びる第２仮想境界線に基づいて、４つの仮想区画領域が設定され、４つの仮想区画領域は、第１仮想区画領域と、第１仮想区画領域の管軸方向に隣接する第２仮想区画領域と、第１仮想区画領域の直交方向に隣接する第３仮想区画領域と、第２仮想区画領域の直交方向に隣接すると共に第３仮想区画領域の管軸方向に隣接する第４仮想区画領域とで構成され、任意の４つの距離計測手段のうち、第１距離計測手段を第１仮想区画領域に臨ませ、第２距離計測手段を第２仮想区画領域に臨ませ、第３距離計測手段を第３仮想区画領域に臨ませ、第４距離計測手段を第４仮想区画領域に臨ませた状態で、姿勢制御手段は、第２距離計測手段により計測した第２計測距離および第３距離計測手段により計測した第３計測距離を足し合わせた第１合算距離と、第１距離計測手段により計測した第１計測距離および第４距離計測手段により計測した第４計測距離を足し合わせた第２合算距離との差分に基づいて、ロール軸周りの回転方向における処理手段の姿勢を制御することが、好ましい。

また、本発明の他の姿勢制御装置は、丸管の内周面または外周面に対して処理を施す処理手段の姿勢を制御する姿勢制御装置であって、処理手段に設けられ、処理手段と処理手段により処理が施される丸管の処理面との間隙の距離を計測する４つの距離計測手段と、４つの距離計測手段の計測結果に基づいて、処理面に対する処理手段の姿勢を制御する姿勢制御手段と、を備え、４つの距離計測手段は、矩形の４つの頂点の位置関係となって処理面に臨むように配置され、４つの距離計測手段が臨む処理面には、処理面において管軸方向に延びる第１仮想境界線および第１仮想境界線に直交する直交方向に延びる第２仮想境界線に基づいて、４つの仮想区画領域が設定され、４つの仮想区画領域は、第１仮想区画領域と、第１仮想区画領域の管軸方向に隣接する第２仮想区画領域と、第１仮想区画領域の直交方向に隣接する第３仮想区画領域と、第２仮想区画領域の直交方向に隣接すると共に第３仮想区画領域の管軸方向に隣接する第４仮想区画領域とで構成され、姿勢制御手段は、４つの距離計測手段のうち、第１距離計測手段を第１仮想区画領域に臨ませ、第２距離計測手段を第２仮想区画領域に臨ませ、第３距離計測手段を第３仮想区画領域に臨ませ、第４距離計測手段を第４仮想区画領域に臨ませ、第１距離計測手段により計測した第１計測距離から第２距離計測手段により計測した第２計測距離を引いた差分と、第３距離計測手段により計測した第３計測距離から第４距離計測手段により計測した第４計測距離を引いた差分とに基づいて、ピッチ軸周りの回転方向における処理手段の姿勢を制御し、第１計測距離から第３計測距離を引いた差分と、第２計測距離から第４計測距離を引いた差分とに基づいて、ヨー軸周りの回転方向における処理手段の姿勢を制御し、第２計測距離および第３計測距離を足し合わせた第１合算距離と、第１計測距離および第４計測距離を足し合わせた第２合算距離との差分に基づいて、ロール軸周りの回転方向における処理手段の姿勢を制御することを特徴とする。

この場合、処理手段は、その姿勢を処理面に倣わせた状態で処理面に沿って移動する倣い動作を行うように構成され、姿勢制御手段は、処理手段の倣い動作時において、姿勢制御工程をリアルタイムに実行して、処理手段の姿勢をフィードバック制御することが、好ましい。

これらの場合、姿勢制御手段は、第１計測距離、第２計測距離、第３計測距離および第４計測距離の平均距離から予め設定した設定距離を引いた差分に基づいて、処理手段と処理面との間隙が設定距離となるように処理手段の姿勢を制御することが、好ましい。

また、これらの場合、姿勢制御手段は、予め設定したピッチ軸周りにおける処理手段の初期姿勢、予め設定したヨー軸周りにおける処理手段の初期姿勢、および予め設定したロール軸周りにおける処理手段の初期姿勢に基づいて、処理手段の姿勢を制御することが、好ましい。

請求項１の姿勢制御方法および請求項７の姿勢制御装置によれば、簡易な制御により、処理手段の姿勢を、ロール軸周りに制御することができる。このとき、任意の４つの距離計測手段を用いているため、例えば、処理手段の姿勢が、ピッチ軸周りの回転方向における角度成分、またはヨー軸周りの回転方向における角度成分にズレたとしても、このズレに影響されることなく、処理手段の姿勢を適切にロール軸周りに制御することができる。なお、少なくとも４つの距離計測手段を用いればよく、場合によっては、４以上の距離計測手段を用いて、処理手段の姿勢をロール軸周りに制御してもよい。

請求項２の姿勢制御方法および請求項８の姿勢制御装置によれば、任意の４つの距離計測手段を、４つの仮想区画領域に適切に臨ませることができる。つまり、対角に位置する２つの距離計測手段（例えば、第２距離計測手段および第３距離計測手段、または、第１距離計測手段および第４距離計測手段）が、第１仮想境界線または第２仮想境界線に重なったりすることがなく、また、１つの仮想区画領域に複数の距離計測センサが臨んだりすることがない。このため、処理手段の姿勢のロール軸周りにおけるズレを、第１合算距離と第２合算距離との差分により、所定の姿勢に戻すための方向が分かるため、処理手段の姿勢をロール軸周りに正確に制御することができる。

請求項３の姿勢制御方法および請求項９の姿勢制御装置によれば、簡易な制御により、処理手段の姿勢を、ピッチ軸周り、ヨー軸周りおよびロール軸周りに制御することができる。この制御により、処理手段を、丸管の処理面に倣わせた状態で臨ませることができる。このため、処理手段の姿勢を、処理面に倣った姿勢に制御することで、従来のように、マニピュレーターで先端工具を内周面（処理面）に押し付ける必要がなく、処理面からの反力が大きくならないので、先端工具案内装置の剛性を高める必要もない。

なお、処理手段としては、例えば、ショットピーニング処理を行うための先端工具、超音波等により検査を行う検査装置、顕微鏡等の撮像カメラ、または切削加工やフライス加工を行うための加工装置等がある。また、丸管としては、その断面が、断面円形、断面楕円形や断面卵形等のものがある。

請求項４の姿勢制御方法および請求項１０の姿勢制御装置によれば、処理手段の倣い動作時において、処理手段の姿勢をフィードバック制御することができるため、処理手段が移動しても、処理手段の姿勢を、常に処理面に倣った姿勢に維持することができる。

請求項５の姿勢制御方法および請求項１１の姿勢制御装置によれば、処理手段と処理面との間隙の距離を所望の距離とすることができるため、間隙を所定の距離に維持した状態で処理手段により処理を施すことができる。

請求項６の姿勢制御方法および請求項１２の姿勢制御装置によれば、処理手段を予め設定した初期姿勢とすることで、処理面に対し、処理手段を初期姿勢に維持した状態で処理手段により処理を施すことができる。

以下、添付した図面を参照して、本発明にかかる姿勢制御方法および姿勢制御装置について説明する。なお、この実施例によりこの発明が限定されるものではない。

ここで、図１は、本実施例にかかる先端工具案内装置を示す斜視図であり、図２は、先端工具を示す模式図である。また、図３は、計測センサ位置決め工程における説明図である。

先ず、図１を参照して、本実施例にかかる姿勢制御方法および姿勢制御装置を先端工具案内装置に適用した場合について説明する。先端工具案内装置１０は、加圧水型軽水炉原子力発電設備に設けられた蒸気発生器の水室１内に設置されるものである。そして、先端工具案内装置１０は、水室１の下部に設けられた管台２および管台２に接続した円筒管３（丸管）の間の溶接部分の内周面（以下、処理面４という）に各種処理を施すものである。

先端工具案内装置１０は、水室１の底面から天面にかけて立設する旋回支持部１４と、スライド機構を有すると共に旋回支持部１４に着脱自在に連結されたスライドテーブル１５と、スライドテーブル１５に着脱自在に装着されるマニピュレーター１６と、マニピュレーター１６の先端に着脱自在に装着される先端工具１７（処理手段）とを備えている。そして、先端工具案内装置１０は、旋回支持部１４、スライドテーブル１５、マニピュレーター１６および先端工具１７に、それぞれ解体可能に構成されると共に、組立可能に構成されている。また、先端工具案内装置１０には制御装置１８が接続されており、制御装置１８により先端工具案内装置１０の各種動作が制御されている。

旋回支持部１４は、水室１の底面に設置される下部ベース２０と、下部ベース２０の上部に設けられ、下部ベース２０に対して旋回移動可能な柱状の旋回部２１と、旋回部２１の上部に設けられ、旋回部２１に設けた昇降機構により下部ベース２０に対して昇降移動可能な上部支持部２２とを有している。

旋回支持部１４を設置する場合は、先ず、下部ベース２０を水室１の底面に設置し、この後、昇降機構により上部支持部２２を鉛直方向に上昇させて水室１の天面に押し付ける。これにより、旋回支持部１４は、水室１の底面および天面に対し、鉛直方向に突っ張った状態で固定される。

スライドテーブル１５は、基端部を旋回部２１の上部に連結するテーブル部３０と、テーブル部３０上をスライド移動するスライド部３１と、テーブル部３０の先端部に回動軸２４を介して回動自在に連結された支持部２３とを有しており、スライド部３１は、図示しない駆動部によって、スライド移動可能となっている。スライド部３１には、マニピュレーター１６を着脱自在に装着可能な第１ツールチェンジャー３２が設けられている。また、支持部２３は、その基端部が回動軸２４を中心に、収容位置と支持位置との間で移動可能となっており、支持部２３の基端部は、旋回部２１の下部に連結される。

スライドテーブル１５を旋回支持部１４に設置する場合は、テーブル部３０の基端部を旋回部２１の上部に連結した後、支持部２３の基端部を、収容位置から支持位置に回動させ、支持部２３の基端部を旋回部２１の下部に連結する。

マニピュレーター１６は、いわゆる７軸マニピュレーターであり、その基端部は、スライド部３１の第１ツールチェンジャー３２に着脱自在に連結されている。マニピュレーター１６の先端部には、先端工具１７を着脱自在に装着する第２ツールチェンジャー３５が設けられている。なお、マニピュレーター１６は、上記の制御装置１８により、その動作が制御されている。

先端工具１７は、例えば、上記の処理面４にショットピーニング処理を行う先端工具や、上記の処理面４を検査する先端工具等が用意されている。なお、以下の説明では、ショットピーニング処理を行う先端工具を用いた場合について説明する。

図２に示すように、先端工具１７は、直方体状の工具本体４０と、工具本体４０の側面に設けられたエアシリンダ４１と、処理面４に対向する工具本体４０の設置面に設けられた４つの全方向転動ローラ４２ａ，４２ｂ，４２ｃ，４２ｄとを備えている。また、先端工具１７には、４つの距離計測センサ４３ａ，４３ｂ，４３ｃ，４３ｄが設けられている。

エアシリンダ４１は、その基端部が上記のマニピュレーター１６の第２ツールチェンジャー３５に装着され、その先端部に工具本体４０が固定されている。このため、マニピュレーター１６により工具本体４０の４つの全方向転動ローラ４２ａ，４２ｂ，４２ｃ，４２ｄが処理面４に押し当てられると、エアシリンダ４１は処理面４からの反力によって収縮する一方、マニピュレーター１６により工具本体４０が処理面４から離間すると、エアシリンダ４１は伸長する。つまり、エアシリンダ４１は、処理面４に対する工具本体４０の離接に追従して伸縮幅を可変しており、これにより、工具本体４０と処理面４との間隙の誤差を吸収している。

４つの全方向転動ローラ４２ａ，４２ｂ，４２ｃ，４２ｄは、工具本体４０の設置面に設けられており、第１全方向転動ローラ４２ａ（図示右上）、第２全方向転動ローラ４２ｂ（図示右下）、第３全方向転動ローラ４２ｃ（図示左上）および第４全方向転動ローラ４２ｄ（図示左下）で構成され、長方形の４つの頂点の位置関係となるように配置されている。つまり、４つの全方向転動ローラ４２ａ，４２ｂ，４２ｃ，４２ｄは、長方形の格子状に配置されている。

４つの距離計測センサ４３ａ，４３ｂ，４３ｃ，４３ｄは、第１距離計測センサ４３ａ（図示右上）、第２距離計測センサ４３ｂ（図示右下）、第３距離計測センサ４３ｃ（図示左上）および第４距離計測センサ４３ｄ（図示左下）で構成され、先端工具１７と処理面４との間の距離を計測している。また、４つの距離計測センサ４３ａ，４３ｂ，４３ｃ，４３ｄは、工具本体４０の側面に付設されており、上記の４つの全方向転動ローラ４２ａ，４２ｂ，４２ｃ，４２ｄと同様に、長方形の４つの頂点の位置関係となるように配置されている。つまり、４つの距離計測センサ４３ａ，４３ｂ，４３ｃ，４３ｄも、長方形の格子状に配置されている。

距離計測センサとしては、例えば、ストローク式の接触型変位計を用いており、４つの距離計測センサ４３ａ，４３ｂ，４３ｃ，４３ｄの先端は、工具本体４０の設置面から突出するように設けられ、その突出量はそれぞれ同長となっている。この４つの距離計測センサ４３ａ，４３ｂ，４３ｃ，４３ｄによる計測結果は、制御装置１８に向けて出力される。なお、本実施例では、接触型変位計を用いたが、レーザー変位計等の非接触型変位計を用いてもよい。

次に、先端工具案内装置１０に接続された制御装置１８について説明する。制御装置１８は、主としてＣＰＵ、ＲＯＭおよびＲＡＭ等によって構成されており、これらが協働して先端工具案内装置１０を制御することにより、先端工具案内装置１０に様々な動作を行わせることが可能となっている。

制御装置１８は、先端工具１７を処理面４に沿って移動させるための移動軌跡を設定する移動軌跡設定部５０と、設定した移動軌跡に沿って先端工具１７の姿勢を処理面４に倣わせて移動させるようにマニピュレーター１６の動作を制御する動作制御部５１と、移動軌跡に沿って移動する先端工具１７の姿勢を補正するようにマニピュレーター１６の動作制御を補正する動作補正制御部５２と、を有している。

移動軌跡設定部５０は、各種データに基づいて、先端工具１７を、例えば、管台２と円筒管３との間の溶接部分の内周面（処理面４）の周方向全域（一周）に亘って移動するように、移動開始位置から移動終了位置までの移動軌跡を設定するものである。

動作制御部５１は、設定された移動軌跡上を、移動開始位置から移動終了位置まで、先端工具１７の姿勢が処理面４に倣って移動する倣い動作を行うように、すなわち、先端工具１７と処理面４（厳密には、処理面４は湾曲しているため、正確には、処理面４における接線方向）とが平行となるようにマニピュレーター１６の動作を制御するものである。

動作補正制御部５２は、移動軌跡上を処理面４に倣って移動する先端工具１７の姿勢が、誤差等によりずれてしまった場合、４つの距離計測センサ４３ａ，４３ｂ，４３ｃ，４３ｄの計測結果に基づいて、先端工具１７の姿勢を補正するようにマニピュレーター１６の動作を補正するものである（詳細は後述）。つまり、請求項で述べる姿勢制御手段は、動作補正制御部５２のことである。

ここで、先端工具案内装置１０により、管台２と円筒管３との間の溶接部分の内周面（処理面４）に処理を施す一連の処理制御動作について説明する。この処理制御動作は、移動軌跡設定工程と、計測センサ位置決め工程（計測手段位置決め工程）と、動作制御工程と、動作補正制御工程（姿勢制御工程）とから構成されている。

移動軌跡設定工程は、上記の移動軌跡設定部５０により移動開始位置から移動終了位置までの先端工具１７の移動軌跡を周方向全域に亘って設定する（図２参照）。

図３に示すように、計測センサ位置決め工程は、設定した移動軌跡の移動開始位置における処理面４に、先端工具１７およびこれに付設した４つの距離計測センサ４３ａ，４３ｂ，４３ｃ，４３ｄを臨ませる。このとき、移動開始位置における円筒管３の処理面４は、処理面４において管軸方向に延びる第１仮想境界線Ｋ１および第１仮想境界線Ｋ１に直交する接線方向（直交方向）に延びる第２仮想境界線Ｋ２により、４つの仮想区画領域Ｅ１，Ｅ２，Ｅ３，Ｅ４に区分けされている。４つの仮想区画領域Ｅ１，Ｅ２，Ｅ３，Ｅ４は、第１仮想区画領域Ｅ１（図示右上）と、第１仮想区画領域Ｅ１の管軸方向に隣接する第２仮想区画領域Ｅ２（図示右下）と、第１仮想区画領域Ｅ１の接線方向に隣接する第３仮想区画領域Ｅ３（図示左上）と、第２仮想区画領域Ｅ２の接線方向に隣接すると共に第３仮想区画領域Ｅ３の管軸方向に隣接する第４仮想区画領域Ｅ４（図示左下）とで構成されている。

そして、制御装置１８は、マニピュレーター１６を制御して、４つの距離計測センサ４３ａ，４３ｂ，４３ｃ，４３ｄの中の第１距離計測センサ４３ａを第１仮想区画領域Ｅ１に、第２距離計測センサ４３ｂを第２仮想区画領域Ｅ２に、第３距離計測センサ４３ｃを第３仮想区画領域Ｅ３に、第４距離計測センサ４３ｄを第４仮想区画領域Ｅ４に臨むように処理面４に接触させて臨ませる。このため、第１距離計測センサ４３ａおよび第４距離計測センサ４３ｄが第１仮想境界線Ｋ１または第２仮想境界線Ｋ２に重なったりすることなく、あるいは、第２距離計測センサ４３ｂおよび第３距離計測センサ４３ｃが、第１仮想境界線Ｋ１または第２仮想境界線Ｋ２に重なったりすることなく、４つの距離計測センサ４３ａ，４３ｂ，４３ｃ，４３ｄを適切に処理面４に接触させることができる。また、１つの仮想区画領域に複数の距離計測センサが臨んだりすることなく、４つの距離計測センサ４３ａ，４３ｂ，４３ｃ，４３ｄを適切に処理面４に接触させることができる。

動作制御工程は、制御装置１８の動作制御部５１によりマニピュレーター１６を制御して、処理面４に対し先端工具１７の姿勢を維持させた状態で、設定された移動軌跡に沿って、先端工具１７を移動開始位置から移動終了位置まで移動させる。

そして、動作補正制御工程は、移動軌跡上を移動する先端工具１７の倣い動作時において、動作補正制御部５２により、マニピュレーター１６の動作補正をリアルタイムに実行して、先端工具１７の姿勢をフィードバック制御している。

次に、本実施例の特徴部分である、先端工具１７の姿勢を補正するマニピュレーター１６の動作補正制御工程について詳細に説明する。

マニピュレーター１６の動作補正制御工程、すなわち、先端工具１７の姿勢制御工程は、接触させた４つの距離計測センサ４３ａ，４３ｂ，４３ｃ，４３ｄの計測結果に基づいて、接線方向に延びるピッチ軸周りの回転方向における先端工具１７の姿勢を制御するピッチ軸姿勢制御工程と、管軸方向に延びるヨー軸周りの回転方向における先端工具１７の姿勢を制御するヨー軸姿勢制御工程と、先端工具１７と処理面４との離間方向に延びるロール軸周りの回転方向における先端工具１７の姿勢を制御するロール軸姿勢制御工程と、から構成されている。なお、接線方向、管軸方向および離間方向は、Ｘ軸方向（管軸方向）、Ｙ軸方向（接線方向）およびＺ軸方向（離間方向）の関係と同じであり、先端工具１７を基準にした座標系である。また、以下の説明では、先端工具１７の姿勢が、接線方向、管軸方向および離間方向において平行な姿勢となるように制御する場合について説明する。

上記の計測センサ位置決め工程において、４つの距離計測センサ４３ａ，４３ｂ，４３ｃ，４３ｄを接触させると、４つの距離計測センサ４３ａ，４３ｂ，４３ｃ，４３ｄにより先端工具１７と処理面４との間の距離が計測される。

ピッチ軸姿勢制御工程は、第１距離計測センサ４３ａにより計測した第１計測距離Ｌ１から第２距離計測センサ４３ｂにより計測した第２計測距離Ｌ２を引いた差分（Ｌ１−Ｌ２）と、第３距離計測センサ４３ｃにより計測した第３計測距離Ｌ３から第４距離計測センサ４３ｄにより計測した第４計測距離Ｌ４を引いた差分（Ｌ３−Ｌ４）とに基づいて、ピッチ軸周りの回転方向における先端工具１７の姿勢を制御する。具体的には、数１の（１）に示す式により算出されるピッチ成分の制御量に基づいて制御される。このとき、Ｇｐはピッチ制御ゲインであり、動作環境に応じて適切なゲインが設定される。

例えば、ピッチ軸周りにおける先端工具１７の姿勢が処理面４と平行となった状態において、ピッチ軸周りに先端工具１７が回転する。すると、第１計測距離Ｌ１および第３計測距離Ｌ３は伸び、第２計測距離Ｌ２および第４計測距離Ｌ４は縮む。あるいは、第１計測距離Ｌ１および第３計測距離Ｌ３は縮み、第２計測距離Ｌ２および第４計測距離Ｌ４は伸びる。つまり、先端工具１７の姿勢が回転すると、第１計測距離Ｌ１から第２計測距離Ｌ２を引いた差分および第３計測距離Ｌ３から第４計測距離Ｌ４を引いた差分が大きくなる。このため、先端工具１７の姿勢を戻すように、つまり処理面４と平行となるように制御する場合は、第１計測距離Ｌ１から第２計測距離Ｌ２を引いた差分および第３計測距離Ｌ３から第４計測距離Ｌ４を引いた差分を、ゼロに近づけるようにマニピュレーター１６の動作を制御すればよい。

ヨー軸姿勢制御工程は、第１計測距離Ｌ１から第３計測距離Ｌ３を引いた差分（Ｌ１−Ｌ３）と、第２計測距離Ｌ２から第４計測距離Ｌ４を引いた差分（Ｌ２−Ｌ４）とに基づいて、ヨー軸周りの回転方向における先端工具１７の姿勢を制御する。具体的には、数１の（２）に示す式により算出されるヨー成分の制御量に基づいて制御される。このとき、Ｇｙはヨー制御ゲインであり、動作環境に応じて適切なゲインを設定する。

例えば、ヨー軸周りにおける先端工具１７の姿勢が処理面４と平行となった状態において、ヨー軸周りに先端工具１７が回転する。すると、第１計測距離Ｌ１および第２計測距離Ｌ２は伸び、第３計測距離Ｌ３および第４計測距離Ｌ４は縮む。あるいは、第１計測距離Ｌ１および第２計測距離Ｌ２は縮み、第３計測距離Ｌ３および第４計測距離Ｌ４は伸びる。つまり、先端工具１７の姿勢が回転すると、第１計測距離Ｌ１から第３計測距離Ｌ３を引いた差分および第２計測距離Ｌ２から第４計測距離Ｌ４を引いた差分が大きくなる。このため、先端工具１７の姿勢を戻すように、つまり処理面４と平行となるように制御する場合は、第１計測距離Ｌ１から第３計測距離Ｌ３を引いた差分および第２計測距離Ｌ２から第４計測距離Ｌ４を引いた差分を、ゼロに近づけるようにマニピュレーター１６の動作を制御すればよい。

ロール軸姿勢制御工程は、第２計測距離Ｌ２および第３計測距離Ｌ３を足し合わせた第１合算距離（Ｌ２＋Ｌ３）から、第１計測距離Ｌ１および第４計測距離Ｌ４を足し合わせた第２合算距離（Ｌ１＋Ｌ４）を引いた差分に基づいて、ロール軸周りの回転方向における先端工具１７の姿勢を制御する。具体的には、数１の（３）に示す式により算出されるロール成分の制御量に基づいて制御される。このとき、Ｇｒはロール制御ゲインであり、動作環境に応じて適切なゲインを設定する。

例えば、ロール軸周りにおける先端工具１７の姿勢が処理面４と平行となった状態において、ロール軸周りに先端工具１７が回転する。すると、第２計測距離Ｌ２および第３計測距離Ｌ３は伸び、第１計測距離Ｌ１および第４計測距離Ｌ４は縮む。あるいは、第２計測距離Ｌ２および第３計測距離Ｌ３は縮み、第１計測距離Ｌ１および第４計測距離Ｌ４は伸びる。つまり、先端工具１７の姿勢が回転すると、第１合算距離（Ｌ２＋Ｌ３）から第２合算距離（Ｌ１＋Ｌ４）を引いた差分が大きくなる。このため、先端工具１７の姿勢を戻すように制御する場合は、第１合算距離（Ｌ２＋Ｌ３）から第２合算距離（Ｌ１＋Ｌ４）を引いた差分を、ゼロに近づけるようにマニピュレーター１６の動作を制御すればよい。

上記の構成によれば、ピッチ軸姿勢制御工程、ヨー軸姿勢制御工程およびロール軸姿勢制御工程からなる動作補正制御工程（姿勢制御工程）を実行することにより、簡易な制御で、先端工具１７の姿勢を、ピッチ軸周り、ヨー軸周りおよびロール軸周りに制御することができる。また、先端工具１７の姿勢を、円筒管３の処理面４に倣わせた状態で、すなわち接線方向、管軸方向および離間方向において平行な姿勢となった状態で臨ませることができる。

さらに、動作補正制御工程をリアルタイムに実行することで、先端工具１７の倣い動作時において、先端工具１７の姿勢をフィードバック制御することができるため、先端工具１７が処理面４に沿って移動しても、先端工具１７の姿勢を、常に処理面４に倣った姿勢に維持することができる。

なお、上記の動作補正制御工程を、断面楕円形または断面卵形となる丸管の処理面４に対して実行する場合には、処理面４に応じて制御用ゲインＧｐ、Ｇｙ、Ｇｒを適宜可変させながら実行することで、先端工具１７の姿勢を制御することが可能となる。また、処理面４が丸管の外周面であった場合には、ロール軸姿勢制御工程における（３）の式の正負を逆とする（マイナスを乗算する）ことで、上記の動作補正制御工程を適用することができる。さらに、移動軌跡は、周方向全域に亘って設定する必要はなく、任意に設定してよい。

次に、本実施例にかかる先端工具案内装置１０の変形例１について説明する。なお、重複した記載を避けるべく、異なる部分についてのみ説明する。変形例１における先端工具案内装置１０では、先端工具１７と処理面４との間隙を所定の距離（設定距離）に維持した状態で、先端工具１７を移動軌跡上に沿うように移動させることができる。

具体的に、動作制御補正工程では、間隙距離設定工程をさらに有している。間隙距離設定工程では、第１計測距離Ｌ１、第２計測距離Ｌ２、第３計測距離Ｌ３および第４計測距離Ｌ４の平均距離成分から予め設定した設定距離成分Ｌｓを引いた差分に基づいて、先端工具１７と処理面４との間隙が設定距離成分Ｌｓとなるように制御している。つまり、数２の（４）に示す式により算出される押付成分の制御量に基づいて制御される。このとき、Ｇｓは、押付制御ゲインであり、動作環境に応じて適切なゲインが設定される。

例えば、先端工具１７の姿勢が処理面４に対し倣い姿勢となった状態において、先端工具１７と処理面４との間隙が設定距離成分Ｌｓとなっていない場合、平均距離成分から設定距離成分Ｌｓを引いた差分は大きくなる。このため、先端工具１７と処理面４との間隙を設定距離Ｌｓとする場合は、平均距離成分から設定距離成分Ｌｓを引いた差分を、ゼロに近づけるようにマニピュレーター１６の動作を制御すればよい。

以上の構成によれば、先端工具１７の姿勢を、ピッチ軸周り、ヨー軸周りおよびロール軸周りに制御可能であるのはもちろんのこと、先端工具１７と処理面４との間隙の距離を、設定距離Ｌｓに維持した状態で、先端工具１７の倣い動作を行わせることができる。この場合、先端工具１７に設けられたエアシリンダ４１および４つの全方向転動ローラ４２ａ，４２ｂ，４２ｃ，４２ｄを排した構成としてもよい。

次に、本実施例にかかる先端工具案内装置１０の変形例２について説明する。なお、この場合も、重複した記載を避けるべく、異なる部分についてのみ説明する。変形例２における先端工具案内装置１０では、処理面４に対し先端工具１７の姿勢を所望の初期姿勢とした状態で臨ませることができると共に、初期姿勢を維持した状態で先端工具１７を移動軌跡上に沿うように移動させることができる。

具体的には、ピッチ軸姿勢制御工程における（１）の式に、予め設定した先端工具１７のピッチ軸周りの回転方向における初期姿勢の角度成分Ｆｐを差し引いた数３の（５）に示す式に基づいて制御される。同様に、ヨー軸姿勢制御工程およびロール軸姿勢制御工程における（２）および（３）の式に、予め設定した先端工具１７のヨー軸周りおよびロール軸周りの回転方向における初期姿勢の角度成分Ｆｙ，Ｆｒを差し引いた数３の（６）および（７）に示す式に基づいて制御される。

例えば、ピッチ軸周りにおける先端工具１７の姿勢が初期姿勢Ｆｐとなった状態から、ピッチ軸周りに先端工具１７が回転する。すると、（１）の式によって求められる先端工具の姿勢から初期姿勢Ｆｐを引いた差分が大きくなる。このため、先端工具１７を初期姿勢に戻す場合は、（１）の式によって求められる先端工具の姿勢から初期姿勢Ｆｐを引いた差分を、すなわち（５）の式の値を、ゼロに近づけるようにマニピュレーター１６の動作を制御すればよい。なお、ヨー軸姿勢制御工程およびロール軸姿勢制御工程においても、同様であるため説明を省略する。

以上の構成によれば、先端工具１７の姿勢を、ピッチ軸周り、ヨー軸周りおよびロール軸周りに制御可能であるのはもちろんのこと、先端工具１７の姿勢を、初期姿勢に維持した状態で、先端工具１７の倣い動作を行わせることができる。この場合、各種処理に応じて、先端工具１７の姿勢を適切な初期姿勢とすることが好ましい。

以上のように、本発明にかかる姿勢制御方法および姿勢制御装置は、丸管の内周面および外周面に対して処理を施すものに有用であり、特に、丸管の溶接部分へ処理を施す場合に適している。

実施例１にかかる先端工具案内装置を示す斜視図である。 先端工具を示す模式図である。 計測センサ位置決め工程における説明図である。

符号の説明

３ 円筒管
４ 処理面
１０ 先端工具案内装置
１７ 先端工具
１８ 制御装置
４３ａ 第１距離計測センサ
４３ｂ 第２距離計測センサ
４３ｃ 第３距離計測センサ
４３ｄ 第４距離計測センサ
５２ 動作補正制御部
Ｋ１ 第１仮想境界線
Ｋ２ 第２仮想境界線
Ｅ１ 第１仮想区画領域
Ｅ２ 第２仮想区画領域
Ｅ３ 第３仮想区画領域
Ｅ４ 第４仮想区画領域
Ｌ１ 第１計測距離
Ｌ２ 第２計測距離
Ｌ３ 第３計測距離
Ｌ４ 第４計測距離
Ｌｓ 設定距離成分

Claims (12)

1. 丸管の内周面または外周面に対して処理を施す処理手段と、前記処理手段により処理が施される前記丸管の処理面と、の間隙の距離を計測して、前記処理面に対する前記処理手段の姿勢を制御する姿勢制御方法であって、
   前記処理手段には、複数の距離計測手段が設けられると共に、少なくとも任意の４つの距離計測手段が矩形の４つの頂点の位置関係となって前記処理面に臨むように配置され、
   前記任意の４つの距離計測手段のうち、一方の対角に位置する２つの距離計測手段によりそれぞれ計測された計測距離を合算した第１合算距離と、他方の対角に位置する２つの距離計測手段によりそれぞれ計測された計測距離を合算した第２合算距離との差分に基づいて、ロール軸周りの回転方向における前記処理手段の姿勢を制御する姿勢制御工程とを備えたことを特徴とする姿勢制御方法。
2. 前記複数の距離計測手段が臨む前記処理面には、前記処理面において管軸方向に延びる第１仮想境界線および前記第１仮想境界線に直交する直交方向に延びる第２仮想境界線に基づいて、４つの仮想区画領域が設定され、
   前記４つの仮想区画領域は、第１仮想区画領域と、前記第１仮想区画領域の前記管軸方向に隣接する第２仮想区画領域と、前記第１仮想区画領域の前記直交方向に隣接する第３仮想区画領域と、前記第２仮想区画領域の前記直交方向に隣接すると共に前記第３仮想区画領域の前記管軸方向に隣接する第４仮想区画領域とで構成されており、
   前記任意の４つの距離計測手段のうち、第１距離計測手段を前記第１仮想区画領域に臨ませ、第２距離計測手段を前記第２仮想区画領域に臨ませ、第３距離計測手段を前記第３仮想区画領域に臨ませ、第４距離計測手段を前記第４仮想区画領域に臨ませる計測手段位置決め工程をさらに備え、
   前記姿勢制御工程では、前記第２距離計測手段により計測した第２計測距離および前記第３距離計測手段により計測した第３計測距離を足し合わせた前記第１合算距離と、前記第１距離計測手段により計測した第１計測距離および前記第４距離計測手段により計測した第４計測距離を足し合わせた前記第２合算距離との差分に基づいて、ロール軸周りの回転方向における前記処理手段の姿勢を制御することを特徴とする請求項１に記載の姿勢制御方法。
3. 丸管の内周面または外周面に対して処理を施す処理手段と、前記処理手段により処理が施される前記丸管の処理面と、の間隙の距離を計測して、前記処理面に対する前記処理手段の姿勢を制御する姿勢制御方法であって、
   前記処理手段には、矩形の４つの頂点の位置関係となって前記処理面に臨むように４つの距離計測手段が設けられ、
   前記４つの距離計測手段が臨む前記処理面には、前記処理面において管軸方向に延びる第１仮想境界線および前記第１仮想境界線に直交する直交方向に延びる第２仮想境界線に基づいて、４つの仮想区画領域が設定され、
   前記４つの仮想区画領域は、第１仮想区画領域と、前記第１仮想区画領域の前記管軸方向に隣接する第２仮想区画領域と、前記第１仮想区画領域の前記直交方向に隣接する第３仮想区画領域と、前記第２仮想区画領域の前記直交方向に隣接すると共に前記第３仮想区画領域の前記管軸方向に隣接する第４仮想区画領域とで構成されており、
   前記４つの距離計測手段のうち、第１距離計測手段を前記第１仮想区画領域に臨ませ、第２距離計測手段を前記第２仮想区画領域に臨ませ、第３距離計測手段を前記第３仮想区画領域に臨ませ、第４距離計測手段を前記第４仮想区画領域に臨ませる計測手段位置決め工程と、
   前記第１距離計測手段により計測した第１計測距離から前記第２距離計測手段により計測した第２計測距離を引いた差分と、前記第３距離計測手段により計測した第３計測距離から前記第４距離計測手段により計測した第４計測距離を引いた差分とに基づいて、ピッチ軸周りの回転方向における前記処理手段の姿勢を制御し、
   前記第１計測距離から前記第３計測距離を引いた差分と、前記第２計測距離から前記第４計測距離を引いた差分とに基づいて、ヨー軸周りの回転方向における前記処理手段の姿勢を制御し、
   前記第２計測距離および前記第３計測距離を足し合わせた第１合算距離と、前記第１計測距離および前記第４計測距離を足し合わせた第２合算距離との差分に基づいて、ロール軸周りの回転方向における前記処理手段の姿勢を制御する姿勢制御工程とを備えたことを特徴とする姿勢制御方法。
4. 前記処理手段は、その姿勢を前記処理面に倣わせた状態で前記処理面に沿って移動する倣い動作を行うように構成され、
   前記処理手段の倣い動作時において、前記姿勢制御工程をリアルタイムに実行して、前記処理手段の姿勢をフィードバック制御することを特徴とする請求項３に記載の姿勢制御方法。
5. 前記第１計測距離、前記第２計測距離、前記第３計測距離および前記第４計測距離の平均距離から予め設定した設定距離を引いた差分に基づいて、前記処理手段と前記処理面との間隙を前記設定距離とする間隙距離設定工程を、さらに備えたことを特徴とする請求項３または４に記載の姿勢制御方法。
6. 前記姿勢制御工程では、
   予め設定した前記ピッチ軸周りにおける前記処理手段の初期姿勢、予め設定した前記ヨー軸周りにおける前記処理手段の初期姿勢、および予め設定した前記ロール軸周りにおける前記処理手段の初期姿勢に基づいて、前記処理手段の姿勢を制御していることを特徴とする請求項３ないし５のいずれかに１項に記載の姿勢制御方法。
7. 丸管の内周面または外周面に対して処理を施す処理手段の姿勢を制御する姿勢制御装置であって、
   前記処理手段に設けられ、前記処理手段と前記処理手段により処理が施される前記丸管の処理面との間隙の距離を計測する複数の距離計測手段と、
   前記複数の距離計測手段のうち、少なくとも任意の４つの距離計測手段の計測結果に基づいて、前記処理面に対する前記処理手段の姿勢を制御する姿勢制御手段と、を備え、
   前記任意の４つの距離計測手段は、矩形の４つの頂点の位置関係となって前記処理面に臨むように配置され、
   前記姿勢制御手段は、前記任意の４つの距離計測手段のうち、一方の対角に位置する２つの距離計測手段によりそれぞれ計測された計測距離を合算した第１合算距離と、他方の対角に位置する２つの距離計測手段によりそれぞれ計測された計測距離を合算した第２合算距離との差分に基づいて、ロール軸周りの回転方向における前記処理手段の姿勢を制御することを特徴とする姿勢制御装置。
8. 前記複数の距離計測手段が臨む前記処理面には、前記処理面において管軸方向に延びる第１仮想境界線および前記第１仮想境界線に直交する直交方向に延びる第２仮想境界線に基づいて、４つの仮想区画領域が設定され、
   前記４つの仮想区画領域は、第１仮想区画領域と、前記第１仮想区画領域の前記管軸方向に隣接する第２仮想区画領域と、前記第１仮想区画領域の前記直交方向に隣接する第３仮想区画領域と、前記第２仮想区画領域の前記直交方向に隣接すると共に前記第３仮想区画領域の前記管軸方向に隣接する第４仮想区画領域とで構成され、
   前記任意の４つの距離計測手段のうち、第１距離計測手段を前記第１仮想区画領域に臨ませ、第２距離計測手段を前記第２仮想区画領域に臨ませ、第３距離計測手段を前記第３仮想区画領域に臨ませ、第４距離計測手段を前記第４仮想区画領域に臨ませた状態で、
   前記姿勢制御手段は、前記第２距離計測手段により計測した前記第２計測距離および前記第３距離計測手段により計測した前記第３計測距離を足し合わせた前記第１合算距離と、前記第１距離計測手段により計測した前記第１計測距離および前記第４距離計測手段により計測した前記第４計測距離を足し合わせた前記第２合算距離との差分に基づいて、ロール軸周りの回転方向における前記処理手段の姿勢を制御することを特徴とする請求項７に記載の姿勢制御装置。
9. 丸管の内周面または外周面に対して処理を施す処理手段の姿勢を制御する姿勢制御装置であって、
   前記処理手段に設けられ、前記処理手段と前記処理手段により処理が施される前記丸管の処理面との間隙の距離を計測する４つの距離計測手段と、
   前記４つの距離計測手段の計測結果に基づいて、前記処理面に対する前記処理手段の姿勢を制御する姿勢制御手段と、を備え、
   前記４つの距離計測手段は、矩形の４つの頂点の位置関係となって前記処理面に臨むように配置され、
   前記４つの距離計測手段が臨む前記処理面には、前記処理面において管軸方向に延びる第１仮想境界線および前記第１仮想境界線に直交する直交方向に延びる第２仮想境界線に基づいて、４つの仮想区画領域が設定され、
   前記４つの仮想区画領域は、第１仮想区画領域と、前記第１仮想区画領域の前記管軸方向に隣接する第２仮想区画領域と、前記第１仮想区画領域の前記直交方向に隣接する第３仮想区画領域と、前記第２仮想区画領域の前記直交方向に隣接すると共に前記第３仮想区画領域の前記管軸方向に隣接する第４仮想区画領域とで構成され、
   前記４つの距離計測手段のうち、第１距離計測手段を前記第１仮想区画領域に臨ませ、第２距離計測手段を前記第２仮想区画領域に臨ませ、第３距離計測手段を前記第３仮想区画領域に臨ませ、第４距離計測手段を前記第４仮想区画領域に臨ませた状態で、
   前記姿勢制御手段は、
   前記第１距離計測手段により計測した第１計測距離から前記第２距離計測手段により計測した第２計測距離を引いた差分と、前記第３距離計測手段により計測した第３計測距離から前記第４距離計測手段により計測した第４計測距離を引いた差分とに基づいて、ピッチ軸周りの回転方向における前記処理手段の姿勢を制御し、
   前記第１計測距離から前記第３計測距離を引いた差分と、前記第２計測距離から前記第４計測距離を引いた差分とに基づいて、ヨー軸周りの回転方向における前記処理手段の姿勢を制御し、
   前記第２計測距離および前記第３計測距離を足し合わせた第１合算距離と、前記第１計測距離および前記第４計測距離を足し合わせた第２合算距離との差分に基づいて、ロール軸周りの回転方向における前記処理手段の姿勢を制御することを特徴とする姿勢制御装置。
10. 前記処理手段は、その姿勢を前記処理面に倣わせた状態で前記処理面に沿って移動する倣い動作を行うように構成され、
    前記姿勢制御手段は、前記処理手段の倣い動作時において、前記姿勢制御工程をリアルタイムに実行して、前記処理手段の姿勢をフィードバック制御することを特徴とする請求項９に記載の姿勢制御装置。
11. 前記姿勢制御手段は、
    前記第１計測距離、前記第２計測距離、前記第３計測距離および前記第４計測距離の平均距離から予め設定した設定距離を引いた差分に基づいて、前記処理手段と前記処理面との間隙が前記設定距離となるように前記処理手段の姿勢を制御することを特徴とする請求項９または１０に記載の姿勢制御装置。
12. 前記姿勢制御手段は、
    予め設定した前記ピッチ軸周りにおける前記処理手段の初期姿勢、予め設定した前記ヨー軸周りにおける前記処理手段の初期姿勢、および予め設定した前記ロール軸周りにおける前記処理手段の初期姿勢に基づいて、前記処理手段の姿勢を制御することを特徴とする請求項９ないし１１のいずれか１項に記載の姿勢制御装置。

Images