(57)【特許請求の範囲】
【請求項１】
手先部に取り付けられた力覚センサと、力覚センサに取り付けられたエンドエフェクタとを有する力制御ロボットであって、
前記エンドエフェクタは１対の加工ツールを有し、該１対の加工ツールは、工具回転軸に直交する１方向に間隔を隔て、工具回転軸まわりに回転可能に構成されており、
力制御ロボットは、前記手先部を３次元空間で位置と姿勢を移動可能な３次元ロボットと、３次元ロボットの位置と姿勢を制御するロボットコントローラと、力覚センサの検出データによりエンドエフェクタに対する位置指令値、速度指令値、又はトルク指令値を作成しロボットコントローラに入力する力制御装置とを備え、
前記１対の加工ツールを位置制御し、かつ前記１方向及び工具回転軸の軸方向に直交する加工方向に力制御するとともに、工具回転軸まわりにトルク制御する、ことを特徴とする力制御ロボット。
【請求項２】
手先部に取り付けられた力覚センサと、力覚センサに取り付けられたエンドエフェクタとを有する力制御ロボットの制御方法であって、
前記エンドエフェクタは１対の加工ツールを有し、該１対の加工ツールは、工具回転軸に直交する１方向に間隔を隔て、工具回転軸まわりに回転可能に構成されており、
前記１対の加工ツールを位置制御し、かつ前記１方向及び工具回転軸の軸方向に直交する加工方向に力制御するとともに、工具回転軸まわりにトルク制御する、ことを特徴とする力制御ロボットの制御方法。
【請求項３】
前記位置制御を前記１方向、工具回転軸の軸方向、又は前記１方向と工具回転軸の軸方向の合成方向に実施する、ことを特徴とする請求項２に記載の力制御ロボットの制御方法。
【請求項４】
前記位置制御をワークの加工面に沿った方向に実施する、ことを特徴とする請求項２に記載の力制御ロボットの制御方法。
【請求項５】
前記１対の加工ツールの回転軸は、互いに平行に配置されている、ことを特徴とする請求項２乃至４のいずれか１項に記載の力制御ロボットの制御方法。
【請求項６】
前記工具回転軸は、１対の加工ツールの一方の回転軸上に位置しており、前記トルク制御により他方の加工ツールによるワークに対する押付力を制御する、ことを特徴とする請求項２乃至５のいずれか１項に記載の力制御ロボットの制御方法。
【請求項７】
前記工具回転軸は、１対の加工ツールの回転軸の中点上に平行に位置しており、前記トルク制御によるトルクを０に制御する、ことを特徴とする請求項２乃至５のいずれか１項に記載の力制御ロボットの制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、研磨、バリ取り、面取りなどを行う力制御ロボットとその制御方法に関する。
【背景技術】
【０００２】
力制御ロボットは、力覚センサによるフィードバック信号を用い、力制御やコンプライアンス制御などを実施するものであり、例えば特許文献１〜４に開示されている。
すなわち、力制御やコンプライアンス制御により、部品に対して工具を押し付けて、加工形状のバラツキ、設置誤差、ロボット自体がもつ誤差などに対して、工具の軌道を補正しながら加工することができる。
このような力制御ロボットの場合、部品の輪郭線などに沿う方向（送り方向）には位置制御や速度制御により移動し、送り方向と直交する方向には、一定の力などで押し付ける力制御により研磨、バリ取り、面取りなどが行われる。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１２−２０３４８号公報
【特許文献２】特開平８−１１８２７８号公報
【特許文献３】特開平８−１１８２７６号公報
【特許文献４】特開平７−２６６２６９号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
上述した力制御ロボットによる加工（研磨、バリ取り、面取りなど）は、一般的に力覚センサからのフィードバック信号に基づく制御により行われるため、それぞれ固有の動特性を有しかつ応答遅れを伴う。この動特性と応答遅れは、力制御ロボット自体の性能や使用する工具などにより決まるため、自由に変更することは一般的に困難である。【０００５】
そのため生産性を高めるために、加工速度を上げ、送り速度を高速にすると、加工形状のバラツキ、設置誤差、ロボット自体がもつ誤差などが大きい場合に、変化に追従することができない。【０００６】
その結果、（１）工具を一定の力で押し付けることができなくなり（押付力が変動し）、安定した加工ができなくなる、（２）ワークから工具が離れて加工ができなくなる、（３）最悪の場合には、力を吸収できずに過大な力を受けてしまい、センサや装置が破損する、などの可能性がある。【０００７】
また、力制御の応答速度や精度は力制御を行う制御装置の能力（すなわち制御サイクル）や、力制御ロボット自体の機械的な応答性能に依存するため、制御装置の性能向上などの対策も考えられる。
しかし、制御装置の性能向上の場合、市販されている一般的な産業用汎用ロボットが利用できなくなり、特殊な装置が必要となり、設備コストが増大してしまう。【０００８】
また力制御ロボットのように複雑な動作を行う装置では、制御指令による複雑な動作が安定的に行われるように、制御指令部に不安定な入力を排除するフィルタ（遅延要素）などを入れている場合も多く、このフィルタによる遅延が支配的で、送り速度の大幅な高速化ができない場合も多い。【０００９】
本発明は、かかる問題点を解消するために創案されたものである。すなわち、本発明の目的は、従来の汎用ロボットを用いて、研磨、バリ取り、面取りなどの力制御による加工を、従来よりも高速で行うことができる力制御ロボットとその制御方法を提供することにある。
【課題を解決するための手段】
【００１０】
本発明によれば、手先部に取り付けられた力覚センサと、力覚センサに取り付けられたエンドエフェクタとを有する力制御ロボットであって、
前記エンドエフェクタは１対の加工ツールを有し、該１対の加工ツールは、工具回転軸に直交する１方向に間隔を隔て、工具回転軸まわりに回転可能に構成されており、
力制御ロボットは、前記手先部を３次元空間で位置と姿勢を移動可能な３次元ロボットと、３次元ロボットの位置と姿勢を制御するロボットコントローラと、力覚センサの検出データによりエンドエフェクタに対する位置指令値、速度指令値、又はトルク指令値を作成しロボットコントローラに入力する力制御装置とを備え、
前記１対の加工ツールを位置制御し、かつ前記１方向及び工具回転軸の軸方向に直交する加工方向に力制御するとともに、工具回転軸まわりにトルク制御する、ことを特徴とする力制御ロボットが提供される。【００１１】
また本発明によれば、手先部に取り付けられた力覚センサと、力覚センサに取り付けられたエンドエフェクタとを有する力制御ロボットの制御方法であって、
前記エンドエフェクタは１対の加工ツールを有し、該１対の加工ツールは、工具回転軸に直交する１方向に間隔を隔て、工具回転軸まわりに回転可能に構成されており、
前記１対の加工ツールを位置制御し、かつ前記１方向及び工具回転軸の軸方向に直交する加工方向に力制御するとともに、工具回転軸まわりにトルク制御する、ことを特徴とする力制御ロボットの制御方法が提供される。【００１２】
本発明の実施形態によれば、前記位置制御を前記１方向、工具回転軸の軸方向、又は前記１方向と工具回転軸の軸方向の合成方向に実施する。【００１３】
また、前記位置制御をワークの加工面に沿った方向に実施することが好ましい。【００１４】
また、前記１対の加工ツールの回転軸は、互いに平行に配置されている。【００１５】
また、前記工具回転軸は、１対の加工ツールの一方の回転軸上に位置しており、前記トルク制御により他方の加工ツールによるワークに対する押付力を制御する。【００１６】
また、前記工具回転軸は、１対の加工ツールの回転軸の中点上に平行に位置しており、前記トルク制御によるトルクを０に制御してもよい。
【発明の効果】
【００１７】
上記本発明の装置と方法によれば、エンドエフェクタが１対（２台）の加工ツールを有し、１対の加工ツールを工具回転軸まわりにトルク制御することで、１台の力制御ロボットで、２台の加工ツールの位置制御とワークに対する力制御を同時に実施することができる。【００１８】
従って、１台の加工ツールによる加工速度（研磨、バリ取り、面取りなど）は従来と同じであっても、２台の加工ツールで２倍の加工速度を達成することができる。
【図面の簡単な説明】
【００１９】
【図１】本発明による力制御ロボットの第１実施形態図である。
【図２】本発明の原理説明図である。
【図３Ａ】押付力の補正説明図である。
【図３Ｂ】押付力の別の補正説明図である。
【図３Ｃ】押付力の別の補正説明図である。
【図３Ｄ】押付力の別の補正説明図である。
【図４】本発明による力制御ロボットの制御ブロック図である。
【図５】工具径が異なる場合の模式図である。
【図６】ワークの加工面が曲面の場合の模式図である。
【図７】角度誤差の補正方法を示す模式図である。
【図８Ａ】本発明による力制御ロボットの第２実施形態図である。
【図８Ｂ】図８Ａの力制御ロボットにおいて、第１工具により粗仕上げした面を第２工具が精密仕上げする様子を示す。
【図９】第１工具と第２工具の中心間にＴＣＰ及び工具回転軸を設定した模式図である。
【図１０Ａ】本発明による力制御ロボットの第３実施形態図である。
【図１０Ｂ】本発明による力制御ロボットの第４実施形態図である。
【図１０Ｃ】本発明による力制御ロボットの第５実施形態図である。
【図１１Ａ】図１の工具回転軸をワークの加工面に沿って傾斜させた状態を示す正面図である。
【図１１Ｂ】図１の工具回転軸をワークの加工面に沿って傾斜させた状態を示す右側面図である。
【図１２Ａ】図１の工具回転軸をワークの加工面に対し傾斜させた状態を示す正面図である。
【図１２Ｂ】図１の工具回転軸をワークの加工面に対し傾斜させた状態を示す右側面図である。
【発明を実施するための形態】
【００２０】
以下、本発明の好ましい実施形態を添付図面に基づいて詳細に説明する。なお、各図において共通する部分には同一の符号を付し、重複した説明を省略する。
【００２１】
図１は、本発明による力制御ロボット１０の第１実施形態図である。
この図において、力制御ロボット１０は、３次元ロボット１２、ロボットコントローラ１４、力覚センサ１６、エンドエフェクタ１８、及び力制御装置２２を備える。
【００２２】
３次元ロボット１２は、この例ではロボットアーム１１の手先部１１ａを３次元空間で位置と姿勢を移動可能な多関節アームロボットである。なお、３次元ロボット１２は、手先部１１ａを３次元空間で位置と姿勢を移動可能な限りで、その他のロボットであってもよい。
【００２３】
ロボットコントローラ１４は、３次元ロボット１２の位置と姿勢を制御する制御装置である。
力覚センサ１６は、この例ではロボットアーム１１の手先部１１ａに取り付けられ、外部から受ける直交３軸方向の力と、直交３軸まわりのトルクとを検出する。
【００２４】
エンドエフェクタ１８は、力覚センサ１６に取り付けられた１対の加工ツール１８Ａ，１８Ｂを有する。１対の加工ツール１８Ａ，１８Ｂの回転軸は、好ましくは互いに平行に配置されている。
１対の加工ツール１８Ａ，１８Ｂは、この例ではそれぞれ円筒形の工具１９ａ、１９ｂと、工具１９ａ、１９ｂをそれぞれの軸心を中心に回転駆動するスピンドル２０ａ、２０ｂと、を有する。
１対の加工ツール１８Ａ，１８Ｂの全体は、工具回転軸２１のまわりに回転可能である。
【００２５】
１対のスピンドル２０ａ、２０ｂの回転軸は、工具回転軸２１に直交する１方向（図で左右方向）に間隔Ｌを隔て、工具回転軸２１のまわりに回転可能に構成されている。
工具回転軸２１は、１対の加工ツール１８Ａ，１８Ｂの一方（この例では１８Ａ）の回転軸上に位置している。
【００２６】
力制御装置２２は、力覚センサ１６の検出データによりエンドエフェクタ１８の位置指令値を作成しロボットコントローラ１４に入力する。
【００２７】
この例で、ロボットコントローラ１４は位置指令を受けて３次元ロボット１２を位置制御する位置制御タイプであるが、速度指令を受けて３次元ロボット１２を速度制御する速度制御タイプ、又はトルク指令（又は電流指令）を受けて３次元ロボット１２をトルク制御するトルク制御タイプであってもよい。
また、力制御装置２２からの出力は、この例では位置指令値であるが、ロボットコントローラ１４が速度制御タイプの場合には速度指令値、ロボットコントローラ１４がトルク制御タイプである場合にはトルク指令値である。
すなわち、力制御装置２２からの出力は、ロボットコントローラ１４の入力に合わせて、位置指令値、速度指令値、又はトルク指令値である。したがって、力制御装置２２は、力覚センサ１６の検出データによりエンドエフェクタ１８に対する位置指令値、速度指令値、又はトルク指令値を作成しロボットコントローラ１４に入力する。
言い換えれば、図４はロボットコントローラ１４が位置指令タイプの場合を示しているが、ロボットコントローラ１４が速度指令タイプ又はトルク指令タイプであっても、ロボットコントローラ１４内で実施されている速度制御計算や位置制御計算を力制御装置２２内に取り込むことで、同様の機能が実現できる。
また、力制御装置２２とロボットコントローラ１４は、必ずしも分離されている必要はなく、一体であってもよい。
【００２８】
力制御ロボット１０の制御方法では、力制御装置２２とロボットコントローラ１４は、１対の加工ツール１８Ａ，１８Ｂの全体を位置制御し、かつ工具回転軸２１に直交する１方向と工具回転軸２１の軸方向との両方に直交する加工方向（図１で紙面に直交する方向）に力制御するとともに、工具回転軸２１まわりにトルク制御する。
位置制御は、工具回転軸２１に直交する１方向（図１で左右方向）、工具回転軸２１の軸方向、又は前記１方向と工具回転軸２１の軸方向の合成方向に実施する。
【００２９】
図２は、図１のＡ−Ａ矢視図であり、本発明の原理説明図である。
この例で、ワーク１の加工面１ａは平面であり、１対の加工ツール１８Ａ，１８Ｂは互いに平行に配置されており、円筒形の工具１９ａ、１９ｂは同一の直径ｄを有する。更に、ＴＣＰ（工具基準点）と工具回転軸２１は、図２で左側の工具１９ａの中心に位置するものとする。
以下、この図２で左側の工具１９ａを第１工具１９ａ、右側の工具１９ｂを第２工具１９ｂと呼ぶ。
【００３０】
図２において、１対の加工ツール１８Ａ，１８Ｂを位置制御し、かつ１対の加工ツール１８Ａ，１８Ｂを加工方向Ｂ（図２で下方向）に力制御するとともに、工具回転軸２１まわりにトルク制御する。
位置制御は、工具回転軸２１に直交する１方向（以下「Ａ方向」、図２で左方向）、工具回転軸２１の軸方向、又は前記１方向と工具回転軸２１の軸方向の合成方向に実施する。
【００３１】
すなわち、基準となる第１工具１９ａに対して、Ａ方向と直交する加工方向Ｂに力覚センサ１６からのフィードバック情報を使って、一定の押付力Ｐ１（＝反力Ｒ１）になるように制御する。
また、第１工具１９ａの回転軸まわりのトルクＴ１を制御する。トルクＴ１は、第１工具１９ａの回転軸中心のトルクであるため、第１工具１９ａの押付力Ｐ１には影響を与えない。すなわちトルクＴ１は、第１工具１９ａの回転軸中心に第２工具１９ｂを回転させて、ワーク１の加工面１ａに対して押し付ける。
従って、図２に示すように、ワーク１の加工面１ａが平面であり、第１工具１９ａと第２工具１９ｂの直径（工具径）が同じ場合には、第２工具１９ｂによる押付力をＰ２とした場合、トルクＴ１＝Ｐ２・Ｌでトルク制御することで、所望の押付力Ｐ２が得られる。
【００３２】
図３Ａ〜図３Ｄは、押付力Ｐの補正説明図である。トルク制御によって発生する反力Ｒ２が、実際には押付力Ｐ１に影響するため、以下のように補正する必要がある。
【００３３】
図３Ａの状態を目標状態とすると、第１工具１９ａのＴＣＰ座標系（工具基準点を原点とするローカル座標系）で力とトルクを算出した場合には、第１工具１９ａに反力（Ｒ１＋Ｒ２）が作用している（図３Ｂ参照）。時系列的には、力覚センサ１６で計測された力とトルクを、第１工具１９ａのＴＣＰ座標系での力とトルクに変換すると、図３Ｂが得られ、これを使って制御演算する。
従って、押付力Ｐ１の目標値をＲ１（＝Ｐ１）とすると、ワーク１から離れる方向にフィードバックが働くため好ましくない。そこで、これを防ぐために、押付力Ｐ１の目標値を（Ｒ１＋Ｒ２）とすることで、図３Ａの状態に制御できる。
【００３４】
更に工具回転と、それによる反力を考慮すると図３Ｃのようになる。トルクＴ１の反力（反トルク）はＴ１’＝Ｒ２×Ｌ＋Ｒ１’×ｄ／２＋Ｒ２’×ｄ／２となり、工具回転による反力Ｒ１’、Ｒ２’によって生じるトルクが混入する。ここで、ｄは、工具の直径である。
ただし、力覚センサ１６の計測値からＴＣＰ座標系での力とトルクを算出すると、図３Ｄのようになり、工具回転による反力が（Ｒ１’＋Ｒ２’）として独立に計測できる。これを使って、反トルクＴ１’から、工具回転による反力成分を除去すれば、工具回転の影響を排除して、すなわち計測した反トルクＴ１’から、（Ｒ１’＋Ｒ２’）×ｄ／２を減算して、目標値（反力Ｒ２×Ｌ）でトルク制御すれば、図３Ｃの状態に制御できる。
【００３５】
従って、上述したように押付力Ｐ１とトルクＴ１を制御することで、２つの工具１９ａ、１９ｂの押付力Ｐ１、Ｐ２（すなわちワーク１からの反力Ｒ１、Ｒ２）をそれぞれ独立に制御することができる。
【００３６】
図２において、第１工具１９ａの空間的な位置と姿勢を決定するには空間６自由度（直交する３軸方向の位置と３軸まわりの回転角度）を与える必要がある。しかし、第１工具１９ａは回転工具であるため、回転工具の回転軸まわりの姿勢はどの方向をとっても加工が可能となる。このため実際には第１工具１９ａの位置と姿勢を決定するには５自由度を与えればよいことになる。そこで第１工具１９ａに対しては加工方向Ｂの並進１自由度を押付力Ｐ１で力制御し、残りの４自由度（並進２自由度＋回転２自由度）で送りのための位置制御を行う。
【００３７】
一方、本発明では３次元ロボット１２として一般的な多関節アームロボット（すなわち６軸ロボット）を想定している。６軸ロボットは空間６自由度を与えることが可能で上記状態に対して１冗長自由度を持つ構成となっている。そこで、残りの１自由度となる工具回転軸２１まわりの姿勢自由度でトルクＴ１のトルク制御することで、第２工具１９ｂに対して押付力Ｐ２を付与するようにしたものである。
【００３８】
なお、この例では第１工具１９ａの位置と姿勢を決定するのに５自由度を与えているが、加工されるワーク１の形状、すなわち工具１９ａ、１９ｂが描くべき軌跡によっては、更に自由度を減らすことが可能である。
従って、本発明は、６自由度以上の３次元ロボット１２を用いることは要件ではなく、加工に必要な自由度に対して＋１自由度の冗長自由度を持つことが要件となる。
【００３９】
図４は、本発明による力制御ロボット１０の制御ブロック図である。
【００４０】
この図において、３次元ロボット１２は力制御装置２２にワーク加工中のエンドエフェクタ１８の位置情報ａ１をフィードバックする。また、力覚センサ１６は、ワーク加工中のエンドエフェクタ１８に作用する力情報ｂ１（外部から受ける直交３軸方向の力と、直交３軸まわりのトルク）をフィードバックする。
力制御装置２２は、予めオフラインで生成された加工条件を記憶する記憶部２２ａと、位置制御及び力制御の成分抽出部２２ｂと、位置制御及び力制御を計算する計算部２２ｃとを有する。
【００４１】
記憶部２２ａは、位置軌道ａ２、押付方向ｃ、力情報ｂ２（力とトルク）を出力し、位置軌道ａ２とフィードバックされた位置情報ａ１の差Δａと、押付方向ｃと、力情報ｂ２とフィードバックされた力情報ｂ１の差Δｂとが成分抽出部２２ｂに入力される。
【００４２】
成分抽出部２２ｂは、Δａ、Δｂ、押付方向ｃから位置誤差Δｄと力誤差Δｅを出力する。出力された位置誤差Δｄと力誤差Δｅは、計算部２２ｃに入力される。この入力に基づく、計算部２２ｃからの出力に、フィードバックされた位置情報ａ１が加算され、その結果としての位置指令値ｆがロボットコントローラ１４に出力される。
【００４３】
この位置指令値ｆは、図２において、１対の加工ツール１８Ａ，１８ＢをＡ方向（工具回転軸２１に直交する１方向）に位置制御し、かつ１対の加工ツール１８Ａ，１８Ｂを加工方向Ｂに力制御するとともに、工具回転軸２１まわりにトルク制御するように設定されている。
【００４４】
従って、ロボットコントローラ１４により３次元ロボット１２の位置と姿勢を位置指令値ｆになるように制御することにより、１対の加工ツール１８Ａ，１８ＢをＡ方向に位置制御し、かつ１対の加工ツール１８Ａ，１８Ｂを加工方向Ｂに力制御するとともに、工具回転軸２１まわりにトルク制御することができる。
【００４５】
例えば、Ａ方向（ここでは、Ｘ軸方向とする）には、位置制御で、１対の加工ツール１８Ａ，１８Ｂの位置をＸ＝１００［ｍｍ］に制御し、加工方向Ｂ（ここでは、Ｙ方向とする）には、力制御で、１対の加工ツール１８Ａ，１８Ｂに作用する力をＦｙ＝５Ｎに制御し、１対の加工ツール１８Ａ，１８Ｂに付与される工具回転軸２１まわり（ここでは、Ｚ軸まわりとする）のトルクをＴｚ＝１［Ｎｍ］に制御する場合について説明する。
【００４６】
ここで、指令値ｆが位置指令値である場合には、例えば、指令値ｆを、以下の式で表わされるｆ＝｛ｆｘ［ｍｍ］，ｆｙ［ｍｍ］，ｆｚ［度］｝というベクトルにすることができる。以下の式における各数値は、一例である。
ｆｘ［ｍｍ］＝（位置目標値１００ｍｍ−Ｘ方向の計測位置９０ｍｍ）×位置制御ゲイン＋Ｘ方向の計測位置９０ｍｍ
ｆｙ［ｍｍ］＝（力目標値５Ｎ−Ｙ方向の計測した力４Ｎ）×力制御ゲイン＋Ｙ方向の計測位置４５ｍｍ
ｆｚ［度］＝（トルク目標値１Ｎｍ−計測したトルク０．８Ｎｍ）×トルク制御ゲイン＋Ｚ軸まわりの計測角度３５度
【００４７】
このようなｆｘ、ｆｙ、ｆｚが、力制御装置２２で生成されて、ロボットコントローラ１４へ入力される。
ロボットコントローラ１４にも、Ｘ方向の計測位置と、Ｙ方向の計測位置と、Ｚ軸まわりの計測角度が入力される。ロボットコントローラ１４は、入力されたＸ方向の計測位置が、入力されたｆｘになるように、Ｘ方向について１対の加工ツール１８Ａ，１８Ｂを制御し、入力されたＹ方向の計測位置が、入力されたｆｙになるように、Ｙ方向について１対の加工ツール１８Ａ，１８Ｂを制御し、入力されたＺ軸まわりの計測角度（すなわち、計測位置）が、入力されたｆｚになるように、Ｚ軸まわりについて１対の加工ツール１８Ａ，１８Ｂを制御する。その結果として、１対の加工ツール１８Ａ，１８Ｂは、Ｘ方向に位置制御され、加工方向Ｂに力制御され、工具回転軸２１まわりにトルク制御される。
【００４８】
なお、指令値ｆが速度指令値ｆである場合には、指令値ｆ＝｛ｆｘ［ｍｍ／ｓ］，ｆｙ［ｍｍ／ｓ］，ｆｚ［度／ｓ］｝は、例えば、次のように算出される。
ｆｘ［ｍｍ／ｓ］＝（位置目標値−Ｘ方向の計測位置）×位置制御ゲイン＋Ｘ方向の計測速度
ｆｙ［ｍｍ／ｓ］＝（力目標値−Ｙ方向の計測した力）×力制御ゲイン＋Ｙ方向の計測速度
ｆｚ［度／ｓ］＝（トルク目標値−計測したトルク）×トルク制御ゲイン＋Ｚ軸まわりの計測速度
このようなｆｘ、ｆｙ、ｆｚが、力制御装置２２で生成されて、ロボットコントローラ１４へ入力される。
ロボットコントローラ１４にも、Ｘ方向の計測速度と、Ｙ方向の計測速度と、Ｚ軸まわりの計測速度が入力される。ロボットコントローラ１４は、入力されたＸ方向の計測速度が、入力されたｆｘになるように、Ｘ方向について１対の加工ツール１８Ａ，１８Ｂを制御し、入力されたＹ方向の計測速度が、入力されたｆｙになるように、Ｙ方向について１対の加工ツール１８Ａ，１８Ｂを制御し、入力されたＺ軸まわりの計測速度が、入力されたｆｚになるように、Ｚ軸まわりについて１対の加工ツール１８Ａ，１８Ｂを制御する。その結果として、１対の加工ツール１８Ａ，１８Ｂは、Ｘ方向に位置制御され、加工方向Ｂに力制御され、工具回転軸２１まわりにトルク制御される。
【００４９】
指令値ｆがトルク指令値ｆである場合には、指令値ｆ＝｛ｆｘ［Ｎｍ］，ｆｙ［Ｎｍ］，ｆｚ［Ｎｍ］｝は、例えば、次のように算出される。
ｆｘ［Ｎｍ］＝（位置目標値−Ｘ方向の計測位置）×位置制御ゲイン＋Ｘ方向に関する計測トルク
ｆｙ［Ｎｍ］＝（力目標値−Ｙ方向の計測した力）×力制御ゲイン＋Ｙ方向に関する計測トルク
ｆｚ［Ｎｍ］＝（トルク目標値−計測したトルク）×トルク制御ゲイン＋Ｚ軸まわりの計測トルク
このようなｆｘ、ｆｙ、ｆｚが、力制御装置２２で生成されて、ロボットコントローラ１４へ入力される。
ロボットコントローラ１４にも、Ｘ方向に関する計測トルクと、Ｙ方向に関する計測トルクと、Ｚ軸まわりの計測トルクが入力される。ロボットコントローラ１４は、入力されたＸ方向に関する計測トルクが、入力されたｆｘになるように、Ｘ方向について１対の加工ツール１８Ａ，１８Ｂを制御し、入力されたＹ方向に関する計測トルクが、入力されたｆｙになるように、Ｙ方向について１対の加工ツール１８Ａ，１８Ｂを制御し、入力されたＺ軸まわりの計測トルクが、入力されたｆｚになるように、Ｚ軸まわりについて１対の加工ツール１８Ａ，１８Ｂを制御する。その結果として、１対の加工ツール１８Ａ，１８Ｂは、Ｘ方向に位置制御され、加工方向Ｂに力制御され、工具回転軸２１まわりにトルク制御される。
【００５０】
Ｒ２＝Ｔ１／Ｌ・・・（１）が成立するのは、ワーク１の加工面１ａが平面であり、かつ、第１工具１９ａと第２工具１９ｂの工具径（直径ｄ）が同じ場合である。しかし、実際には、工具径が違う場合や、異なる工具を使用する場合も考えられ、またワーク１の加工面１ａが曲率などをもつ場合も存在する。
【００５１】
図５は、工具径が異なる場合の図２と同様の模式図である。
この図において、ワーク１の加工面１ａは平面であり、第１工具１９ａの直径がｄ、第２工具１９ｂの直径がＤであり、Ｄ＞ｄの場合を示している。工具径が異なる場合、径差に起因する角度誤差が発生する。
図５において、第１工具１９ａの中心Ｏ１と第２工具１９ｂの中心Ｏ２を結ぶ線分（Ｏ１−Ｏ２）の長さがＬであり、線分（Ｏ１−Ｏ２）に直交し中心Ｏ２を通る直線と第２工具１９ｂの反力Ｒ２の方向とのなす角度をαとする。
この場合、第２工具１９ｂの反力Ｒ２はＲ２＝Ｔ１／Ｌ・ｃｏｓα・・・（２）で与えられる。
【００５２】
図６はワーク１の加工面１ａが曲面の場合の図２と同様の模式図である。
この図において、ワーク１の加工面１ａは曲面であり、第１工具１９ａと第２工具１９ｂの直径ｄは同一である。曲面に対応する場合、曲率などに起因する角度誤差が発生する。
図６において、図５と同様に、第１工具１９ａの中心Ｏ１と第２工具１９ｂの中心Ｏ２を結ぶ線分（Ｏ１−Ｏ２）の長さがＬであり、線分（Ｏ１−Ｏ２）に直交し中心Ｏ２を通る直線と第２工具１９ｂの反力Ｒ２の方向とのなす角度をαとする。
この場合、図５と同様に、第２工具１９ｂの反力Ｒ２はＲ２＝Ｔ１／Ｌ・ｃｏｓα・・・（２）で与えられる。
【００５３】
図５、図６の例において、角度αが十分小さい場合には、Ｒ２＝Ｔ１／Ｌ・・・（１）が実質的に成り立つ。従って、この場合、式（１）をそのまま用いることができる。また、トルクＴ１の値を調整して、式（１）を適用することもできる。
従って、角度αに応じて、第２工具１９ｂの反力Ｒ２を容易に算出することができる。
【００５４】
図７は、角度誤差の補正方法を示す図２と同様の模式図である。
この図において、ワーク１の加工面１ａは曲面であり、第１工具１９ａの直径がｄ、第２工具１９ｂの直径がＤであり、Ｄ＜ｄの場合を示している。この場合、工具径の相違と曲面に起因する角度誤差が発生する。
図７において、図５と同様に、第１工具１９ａの中心Ｏ１と第２工具１９ｂの中心Ｏ２を結ぶ線分（Ｏ１−Ｏ２）の長さがＬであり、線分（Ｏ１−Ｏ２）に直交し中心Ｏ２を通る直線と第２工具１９ｂの反力Ｒ２の方向とのなす角度をαとする。
【００５５】
この例において、使用する工具１９ａ、１９ｂの直径ｄ、Ｄは事前に把握できる。またワーク１の加工面１ａの形状も通常事前にわかっている。従って、トルク制御するトルク中心位置（すなわち工具回転軸２１の位置）を適宜補正しトルクＴ１の値を補正することで、第２工具１９ｂの反力Ｒ２を容易に求めることができる。
【００５６】
図５〜図７に示したように、角度誤差が十分に小さい場合には、第２工具１９ｂの反力Ｒ２を式（１）で表現できるが、角度誤差が大きく、加工や制御に対して影響などを与える場合には、工具径や曲率などの事前情報を使って補正する必要がある。
【００５７】
上述した第１実施形態において、１対の加工ツール１８Ａ，１８Ｂは、単一の力覚センサ１６に取り付けられているため、工具１９ａ、１９ｂの一方を位置制御することで、他方もほぼ同一の軌道を動くことになる。このためワーク１に対して予め適当な位置をとることで、工具１９ａ、１９ｂのそれぞれの押付力Ｐ１、Ｐ２を制御した状態で、工具１９ａ、１９ｂを移動し、ワーク１の加工面１ａの研磨、面取り、バリ取りなどの加工が可能となる。また工具回転軸２１まわりにトルク制御を行うことで、第１工具１９ａは自身の回転軸まわりに回転するが、第１工具１９ａ自体が回転工具であるために、回転しても加工に影響を与えない。
従って、上述した押付力Ｐ１とトルクＴ１を制御することで、２つの工具１９ａ、１９ｂの押付力Ｐ１、Ｐ２（すなわちワーク１からの反力Ｒ１、Ｒ２）をそれぞれ独立に制御することができる。
【００５８】
上述した第１実施形態では、ワーク１の表面の研磨作業を説明しているが、バリ取りや面取り（例えばＣ面取り）などのエッジ加工に対しても、同様の動作が可能である。
【００５９】
図８Ａと図８Ｂは、本発明による力制御ロボット１０の第２実施形態図である。
図８Ａにおいて、第１工具１９ａと第２工具１９ｂはそれぞれ円錐形工具であり、第１工具１９ａが切削工具、第２工具１９ｂが磨き工具である。
この例のように、第１工具１９ａと第２工具１９ｂが別工具であってもよい。例えば実際の加工では、最初に粒度の粗い工具を使って粗仕上げを行い、その後粒度の細かい工具を使って同じ面に対して最終・精密仕上げを行うことも多い。
すなわち、図８Ｂに示すように、第１工具１９ａに粗仕上げ工具を、第２工具１９ｂに精密仕上げ工具を使用することで、第１工具１９ａが先行して粗仕上げした面を、順次、第２工具１９ｂが精密仕上げをしていく構成が考えられる。
【００６０】
また、上述した第１実施形態では、第１工具１９ａを基準として、第１工具１９ａを一定の押付力Ｐ１で制御し、また第１工具１９ａの回転軸まわりのトルクＴ１を制御しているが、ＴＣＰ（工具基準点）及びトルクの回転軸を他に取ることも可能である。
【００６１】
図９は、第１工具１９ａと第２工具１９ｂの中心間にＴＣＰ及び工具回転軸２１を設定した図２と同様の模式図である。
この例において、ＴＣＰに対する押付力Ｐ１とトルクＴ１の合成からそれぞれの反力Ｒ１、Ｒ２を求めることができる。
第１工具１９ａと第２工具１９ｂが同じであり、ワーク１の加工面１ａが平面又は平面に近い面の場合、図９の構成において、Ｌ１＝Ｌ／２、Ｔ１＝０として、反力Ｒ１＝Ｒ２＝Ｐ１／２、回転はフリーにする構成が考えられる。
すなわち、工具回転軸２１が、１対の加工ツール１８Ａ，１８Ｂの回転軸の中点上に位置し、かつ、１対の加工ツール１８Ａ，１８Ｂの回転軸と平行であり、トルク制御によるトルクＴ１を０に制御する。
このように力覚センサ１６の計測中心、ＴＣＰ、工具回転軸２１を合わせることで、校正などが楽にでき、かつ校正計算による誤差などが発生しにくい効果がある。
【００６２】
図１０Ａ〜図１０Ｃは、本発明による力制御ロボット１０の第３〜５実施形態図である。
図１０Ａは、第３実施形態図である。この例は、図９に対応して、第１工具１９ａと第２工具１９ｂの中心間にＴＣＰ及び工具回転軸２１を設定し、Ｌ１＝Ｌ／２としたものである。
この図に示すように力覚センサ１６の計測中心と得たい反力位置などをうまく合わせることで、校正などが楽にでき、かつ校正計算による誤差などがのりにくくなる。
【００６３】
図１０Ｂは、第４実施形態図であり、１対のスピンドル２０ａ，２０ｂのＡ方向の間隔Ｌを位置決め装置２４（例えばスライド）により可変にした例である。
また、図１０Ｃは、第５実施形態図であり、第１工具１９ａに対する第２工具１９ｂの軸方向位置を位置決め装置２４（例えばスライド）により可変にした例である。
図１０Ｂと図１０Ｃに示すように第１工具１９ａと第２工具１９ｂの間に位置決め装置２４を導入することで、さまざまなワーク形状に対応することが可能となる。
【００６４】
図１１Ａと図１１Ｂは、図１の工具回転軸２１をワーク１の加工面１ａに沿って角度θ１傾斜させた状態を示す図であり、図１１Ａは正面図、図１１Ｂは右側面図である。
図１１Ａと図１１Ｂにおいて、１対の加工ツール１８Ａ，１８Ｂを加工面１ａに直交する方向（加工方向Ｂ）に力制御するとともに工具回転軸２１まわりにトルク制御する。
またこの場合、位置制御は、加工面１ａに沿った方向Ｃに制御するのがよい。
【００６５】
図１２Ａと図１２Ｂは、図１の工具回転軸２１をワーク１の加工面１ａに対し角度θ２傾斜させた状態を示す図であり、図１２Ａは正面図、図１２Ｂは右側面図である。
図１２Ａと図１２Ｂにおいて、１対の加工ツール１８Ａ，１８Ｂを加工面１ａに直交する方向（加工方向Ｂ）に力制御するとともに工具回転軸２１まわりにトルク制御する。
またこの場合、位置制御は、加工面１ａに沿った方向Ｃに制御するのがよい。
【００６６】
さらに、図１１Ａと図１１Ｂ、図１２Ａと図１２Ｂのパターンを合わせた状態の場合も、同様に、位置制御は、加工面１ａに沿った方向Ｃに制御するのがよい。
【００６７】
上述した本発明によれば、１台の加工ツールによる加工速度（すなわち送り速度）は従来と同じであっても、２台の加工ツール１８Ａ，１８Ｂで２倍の加工速度を達成することができる。
従って、１台のロボットで、２台の工具１９ａ、１９ｂを同時に運用できるため、１台の加工ツールによる加工速度を上げずに、加工の安定性を確保したまま、生産性（加工量）を高めることができる。また工具の台数は増えるが、ロボットの台数は増えないため、設備費もほとんど増大しない。
【００６８】
なお、本発明は上述した実施形態に限定されず、特許請求の範囲の記載によって示され、更に特許請求の範囲の記載と均等の意味及び範囲内でのすべての変更を含むものである。
【符号の説明】
【００６９】
１  ワーク、１ａ  加工面、１０  力制御ロボット、１１  ロボットアーム、１１ａ  手先部、１２  ３次元ロボット、１４  ロボットコントローラ、１６  力覚センサ、１８  エンドエフェクタ、１８Ａ，１８Ｂ  加工ツール、１９ａ  工具（第１工具）、１９ｂ  工具（第２工具）、２０ａ，２０ｂ  スピンドル、２１  工具回転軸、２２  力制御装置、２２ａ  記憶部、２２ｂ  成分抽出部、２２ｃ  計算部、２４  位置決め装置（例えばスライド）
【図１】
0005979618.tif 000002
【図２】
0005979618.tif 000003
【図３Ａ】
0005979618.tif 000004
【図３Ｂ】
0005979618.tif 000005
【図３Ｃ】
0005979618.tif 000006
【図３Ｄ】
0005979618.tif 000007
【図４】
0005979618.tif 000008
【図５】
0005979618.tif 000009
【図６】
0005979618.tif 000010
【図７】
0005979618.tif 000011
【図８Ａ】
0005979618.tif 000012
【図８Ｂ】
0005979618.tif 000013
【図９】
0005979618.tif 000014
【図１０Ａ】
0005979618.tif 000015
【図１０Ｂ】
0005979618.tif 000016
【図１０Ｃ】
0005979618.tif 000017
【図１１Ａ】
0005979618.tif 000018
【図１１Ｂ】
0005979618.tif 000019
【図１２Ａ】
0005979618.tif 000020
【図１２Ｂ】
0005979618.tif 000021