(57)【要約】  センサを使用せずに小さな外力であってもロボットマニピュレータを倣わせることができる安定な力制御方法を提供する。  第１の演算手段（１０１）で手首軸回転倣い動作時のＴＣＰ速度偏差ベクトルＶerrを計算し、第２の演算手段（１０２）でＴＣＰ速度偏差ベクトルＶerrの要素を、符号を含めて選択し、第３の演算手段（１０３）で選択された速度偏差ベクトルを関節速度ωerrに分解し、第４の演算手段（１０４）で関節速度ωerrを積算して位置修正量θaddを計算し、位置修正量θaddをトルク制限付き位置制御手段（１０５ａ〜ｆ）にフィードバックする。
2009034957.tif 000002
【特許請求の範囲】
【請求項１】
ツールを備えたロボットと、
前記ツールの先端の第１速度偏差ベクトルを算出する演算手段であって、前記第１速度偏差ベクトルは、前記ロボットが前記ツールに作用した外力に倣った結果生ずるものである第１の演算手段と、
前記第１速度偏差ベクトルと位置指令とに基づいて、前記ロボットの駆動モータを位置制御する位置制御手段と、を備えたことを特徴とするロボットシステム。
【請求項２】
基本軸および手首軸を少なくとも備えたロボットと、
前記ロボットのツールの先端の第１速度偏差ベクトルを算出する演算手段であって、前記第１速度偏差ベクトルは、前記ロボットがその先端に設けられたツールに作用した外力に倣った結果生ずるものである第１の演算手段と、
前記算出された速度偏差ベクトルのうち、並進動作に関するベクトル要素を有する第２速度偏差ベクトルを出力する第２の演算手段と、
前記出力された第２速度偏差ベクトルを前記基本軸の関節速度ベクトルに分解する第３の演算手段と、
前記分解された関節速度ベクトルを積算して位置修正量ベクトルを算出する第４の演算手段と、
前記位置修正量ベクトルが加算された位置指令に基づいて、前記マニピュレータの駆動モータを位置制御する位置制御手段と、を備えたことを特徴とするロボットシステム。
【請求項３】
基本軸および手首軸を少なくとも備えたロボットと、
前記ロボットのツールの先端の第１速度偏差ベクトルを算出する演算手段であって、前記前記第１速度偏差ベクトルは、前記ロボットの先端に設けられたツールに作用した外力に倣った結果生ずるものである第１の演算手段と、
前記算出された速度偏差ベクトルのうち、回転動作に関するベクトル要素を選択し、当該選択した要素の大きさをゼロとしたベクトルを第２速度偏差ベクトルとして出力する第２の演算手段と、
前記出力された第２速度偏差ベクトルを前記基本軸と手首軸の関節速度ベクトルに分解する第３の演算手段と、
前記分解された関節速度ベクトルを積算して位置修正量ベクトルを算出する第４の演算手段と、
前記位置修正量ベクトルが加算された位置指令に基づいて、前記マニピュレータの駆動モータを位置制御する位置制御手段と、を備えたことを特徴とするロボットシステム。
【請求項４】
基本軸および手首軸を少なくとも備えたロボットと、
前記ロボットのツールの先端の第１速度偏差ベクトルを算出する演算手段であって、前記前記第１速度偏差ベクトルは、前記ロボットの先端に設けられたツールに作用した外力に倣った結果生ずるものである第１の演算手段と、
前記算出された速度偏差ベクトルのうち、回転動作に関するベクトル要素を選択し、当該選択した要素の符号を反転したベクトルを第２速度偏差ベクトルとして出力する第２の演算手段と、
前記出力された第２速度偏差ベクトルを前記基本軸と手首軸の関節速度ベクトルに分解する第３の演算手段と、
前記分解された関節速度ベクトルを積算して位置修正量ベクトルを算出する第４の演算手段と、
前記位置修正量ベクトルが加算された位置指令に基づいて、前記マニピュレータの駆動モータを位置制御する位置制御手段と、を備えたことを特徴とするロボットシステム。
【請求項５】
前記第１の演算手段は、前記第１速度偏差ベクトルを、位置指令から計算された位置ベクトルと位置フィードバックから計算された位置ベクトルとの差を時間微分して算出することを特徴とする請求項１乃至４いずれかに記載のロボットシステム。
【請求項６】
前記第１の演算手段が算出する前記第１速度偏差ベクトルは、直交座標系における速度偏差ベクトルであることを特徴とする請求項１乃至４いずれかに記載のロボットシステム。
【請求項７】
ロボットのツールの先端の第１速度偏差ベクトルを算出する演算手段であって、前記第１速度偏差ベクトルは、前記ツールに作用した外力に倣った結果生ずるものである第１の演算手段と、
前記第１速度偏差ベクトルと位置指令とに基づいて、前記ロボットの駆動モータを位置制御する位置制御手段と、を備えたことを特徴とするロボット制御装置。
【請求項８】
基本軸および手首軸を少なくとも備えたロボットのツールの先端の第１速度偏差ベクトルを算出する演算手段であって、前記第１速度偏差ベクトルは、前記ツールに作用した外力に倣った結果生ずるものである第１の演算手段と、
前記算出された速度偏差ベクトルのうち、並進動作に関するベクトル要素を有する第２速度偏差ベクトルを出力する第２の演算手段と、
前記出力された第２速度偏差ベクトルを前記基本軸の関節速度ベクトルに分解する第３の演算手段と、
前記分解された関節速度ベクトルを積算して位置修正量ベクトルを算出する第４の演算手段と、
前記位置修正量ベクトルが加算された位置指令に基づいて、前記マニピュレータの駆動モータを位置制御する位置制御手段と、を備えたことを特徴とするロボット制御装置。
【請求項９】
基本軸および手首軸を少なくとも備えたロボットのツールの先端の第１速度偏差ベクトルを算出する演算手段であって、前記第１速度偏差ベクトルは、前記ツールに作用した外力に倣った結果生ずるものである第１の演算手段と、
前記算出された速度偏差ベクトルのうち、回転動作に関するベクトル要素を選択し、当該選択したベクトル要素の大きさをゼロとしたベクトルを第２速度偏差ベクトルとして出力する第２の演算手段と、
前記出力された第２速度偏差ベクトルを前記基本軸と手首軸の関節速度ベクトルに分解する第３の演算手段と、
前記分解された関節速度ベクトルを積算して位置修正量ベクトルを算出する第４の演算手段と、
前記位置修正量ベクトルが加算された位置指令に基づいて、前記マニピュレータの駆動モータを位置制御する位置制御手段と、を備えたことを特徴とするロボット制御装置。
【請求項１０】
基本軸および手首軸を少なくとも備えたロボットのツールの先端の第１速度偏差ベクトルを算出する演算手段であって、前記第１速度偏差ベクトルは、前記ツールに作用した外力に倣った結果生ずる、第１の演算手段と、
前記算出された速度偏差ベクトルのうち、回転動作に関するベクトル要素を選択し、当該選択した要素の符号を反転したベクトルを第２速度偏差ベクトルとして出力する第２の演算手段と、
前記出力された第２速度偏差ベクトルを前記基本軸と手首軸の関節速度ベクトルに分解する第３の演算手段と、
前記分解された関節速度ベクトルを積算して位置修正量ベクトルを算出する第４の演算手段と、
前記位置修正量ベクトルが加算された位置指令に基づいて、前記マニピュレータの駆動モータを位置制御する位置制御手段と、を備えたことを特徴とするロボット制御装置。
【請求項１１】
前記第１の演算手段は、前記第１速度偏差ベクトルを、位置フィードバックから計算される位置ベクトルから位置指令から計算される位置ベクトルを減算して算出することを特徴とする請求項７乃至１０いずれかに記載のロボット制御装置。
【請求項１２】
前記第１の演算手段が算出する前記第１速度偏差ベクトルは、直交座標系における速度偏差ベクトルであることを特徴とする請求項７乃至１０いずれかに記載のロボット制御装置。
【請求項１３】
ツールを備えたロボットが、前記ツールに作用した外力に倣った結果生ずる、前記ツール先端の速度偏差ベクトルを算出し、
前記速度偏差ベクトルに基づいて前記位置指令を修正し、
前記修正された位置指令に基づいて、前記ロボットの駆動モータを制御することを特徴とするロボットの力制御方法。
【請求項１４】
基本軸および手首軸を少なくとも備えたロボットが、ツールに作用した外力に倣った結果生ずる、前記ツール先端の直交座標における速度偏差ベクトルを算出し、
前記速度偏差ベクトルを基本軸および手首軸についての関節速度ベクトルに分解し、
前記関節速度ベクトルを時間積分して位置修正量ベクトルを算出し、
前記位置修正量ベクトルにより前記ロボットの駆動モータの位置指令を修正することを特徴とするロボットの力制御方法。
【請求項１５】
前記速度偏差ベクトルは、位置指令から計算された前記手首軸の手首点から前記ツール先端への位置ベクトルと位置フィードバックから計算された前記手首軸の手首点から前記ツール先端への位置ベクトルとの差を時間微分して求められることを特徴とする請求項１３または１４に記載のロボットの力制御方法。
【請求項１６】
ツールを備えたロボットと、
前記ツールの先端の第１速度偏差ベクトルを算出する演算器であって、前記第１速度偏差ベクトルは、前記ロボットが前記ツールに作用した外力に倣った結果生ずるものである第１の演算器と、
前記第１速度偏差ベクトルと位置指令とに基づいて、前記ロボットの駆動モータを位置制御する位置制御器と、を備えたことを特徴とするロボットシステム。
【請求項１７】
基本軸および手首軸を少なくとも備えたロボットと、
前記ロボットのツールの先端の第１速度偏差ベクトルを算出する演算器であって、前記第１速度偏差ベクトルは、前記ロボットがその先端に設けられたツールに作用した外力に倣った結果生ずるものである第１の演算器と、
前記算出された速度偏差ベクトルのうち、並進動作に関するベクトル要素を有する第２速度偏差ベクトルを出力する第２の演算器と、
前記出力された第２速度偏差ベクトルを前記基本軸の関節速度ベクトルに分解する第３の演算器と、
前記分解された関節速度ベクトルを積算して位置修正量ベクトルを算出する第４の演算器と、
前記位置修正量ベクトルが加算された位置指令に基づいて、前記マニピュレータの駆動モータを位置制御する位置制御器と、を備えたことを特徴とするロボットシステム。
【請求項１８】
基本軸および手首軸を少なくとも備えたロボットと、
前記ロボットのツールの先端の第１速度偏差ベクトルを算出する演算手段であって、前記前記第１速度偏差ベクトルは、前記ロボットの先端に設けられたツールに作用した外力に倣った結果生ずるものである第１の演算器と、
前記算出された速度偏差ベクトルのうち、回転動作に関するベクトル要素を選択し、当該選択した要素の大きさをゼロとしたベクトルを第２速度偏差ベクトルとして出力する第２の演算器と、
前記出力された第２速度偏差ベクトルを前記基本軸と手首軸の関節速度ベクトルに分解する第３の演算器と、
前記分解された関節速度ベクトルを積算して位置修正量ベクトルを算出する第４の演算器と、
前記位置修正量ベクトルが加算された位置指令に基づいて、前記マニピュレータの駆動モータを位置制御する位置制御器と、を備えたことを特徴とするロボットシステム。
【請求項１９】
基本軸および手首軸を少なくとも備えたロボットと、
前記ロボットのツールの先端の第１速度偏差ベクトルを算出する演算手段であって、前記前記第１速度偏差ベクトルは、前記ロボットの先端に設けられたツールに作用した外力に倣った結果生ずるものである第１の演算器と、
前記算出された速度偏差ベクトルのうち、回転動作に関するベクトル要素を選択し、当該選択した要素の符号を反転したベクトルを第２速度偏差ベクトルとして出力する第２の演算器と、
前記出力された第２速度偏差ベクトルを前記基本軸と手首軸の関節速度ベクトルに分解する第３の演算器と、
前記分解された関節速度ベクトルを積算して位置修正量ベクトルを算出する第４の演算器と、
前記位置修正量ベクトルが加算された位置指令に基づいて、前記マニピュレータの駆動モータを位置制御する位置制御器と、を備えたことを特徴とするロボットシステム。
【請求項２０】
基本軸および手首軸を少なくとも備えたロボットのツールの先端の第１速度偏差ベクトルを算出する演算手段であって、前記第１速度偏差ベクトルは、前記ツールに作用した外力に倣った結果生ずるものである第１の演算器と、
前記算出された速度偏差ベクトルのうち、並進動作に関するベクトル要素を有する第２速度偏差ベクトルを出力する第２の演算器と、
前記出力された第２速度偏差ベクトルを前記基本軸の関節速度ベクトルに分解する第３の演算器と、
前記分解された関節速度ベクトルを積算して位置修正量ベクトルを算出する第４の演算器と、
前記位置修正量ベクトルが加算された位置指令に基づいて、前記マニピュレータの駆動モータを位置制御する位置制御器と、を備えたことを特徴とするロボット制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットマニピュレータの力制御に関するものである。
【背景技術】
【０００２】
ロボットが部品を穴に挿入する動作をする場合、ロボットマニピュレータに加わった外力に逆らうと、ロボットがうまくその部品を挿入することができない。そのため、マニピュレータはその外力に逆らわずに倣う動作をする必要がある。マニピュレータがこのような動作をするためには、一般的には、その外力を検出する力センサを設ける必要がある。しかし、力センサを使わずにそのような動作をさせることもできる。力センサを使わずに、外力に対してロボットマニピュレータを倣わせるための技術が以下の文献に開示されている。
特許文献１では、次の技術が開示されている。モータの静止時の電流と駆動時の電流との変化分をモータ位置指令にフィードバックする。このフィードバックは、ロボットが動作していないときのみ行う。そうすると、マニピュレータに外力が加わった瞬間、ロボットのいずれかのモータの電流変化を捉えることができるので、ロボットマニピュレータを上述の外力に倣わせることができる。
特許文献２では、次の技術が開示されている。ロボットアームのツールの先端に外力が加わった際に、手首軸（ツールの姿勢を決定する軸）の駆動モータにおける回転速度と駆動電流とからモータ軸に作用する外乱トルクを推定する。この推定された外乱トルクからツールの先端に加わる外力を推定する。この推定された外力をもとに力制御系が構成されている。静止摩擦の小さな手首軸に加わる推定された外乱トルクを利用するので、小さな外力であってもロボットマニピュレータを外力に倣わせることができる。
特許文献３では、次の技術が開示されている。ロボットの手首軸のゲインを下げてコンプライアンス状態にし、ツールに対して移動方向（外力に倣う方向）を予め設定する。ロボットの初期位置および初期姿勢をメモリに記憶する。外力によってツールの先端が倣ったとき、位置指令の内、移動方向についての位置指令のみを現在位置に置き換え、姿勢指令については初期姿勢を保った状態を目標位置姿勢とする位置制御が構成されている。これにより、ツール姿勢を一定に保ったまま、小さな力であってもロボットマニピュレータを倣わせることができる。
【特許文献１】特開平6−39760号公報（第２頁、図１）
【特許文献２】特開平11−42574号公報（第２頁）
【特許文献３】特許第2619227号公報（第１−２頁、図７）
【発明の開示】
【発明が解決しようとする課題】
【０００３】
特許文献１では、電流フィードバックをロボット停止時のみ有効としているので、ワーク挿入作業のように、ロボットが動作しながらロボットマニピュレータに外力が作用する作業には適用できない。静止時電流値との偏差をフィードバックするだけなので、ロボット動作中に電流フィードバックを有効にするとロボットを駆動するための電流（慣性力や粘性摩擦）もフィードバックすることになり、力制御ができなくなるからである。
特許文献２では、モータの回転速度と電流値から外乱トルクを推定しているので、特許文献１の上記課題を解決することができる。しかしながら、外乱トルクの推定値には位相遅れが必ず存在するので、外乱推定値に基づいた力制御系にはフードバックループ内に位相遅れが含まれる。そのため、ゲイン設定によっては力制御が不安定になって、マニピュレータが発振する。
一方、特許文献３では、移動方向には小さな外力に対してロボットマニピュレータを倣わせることができる。しかし、移動方向と垂直な方向については外力に倣うことができず、ツール先端が初期位置に戻ろうとするので、ロボット動作中に垂直な方向に接触するとワークおよびロボットに過大な力が発生し、故障の原因となる。
本発明は上記課題に鑑みてなされたものであり、センサを使用せずに小さな外力であってもロボットマニピュレータを倣わせる力制御方法およびその制御装置を提供することを目的とする。
【課題を解決するための手段】
【０００４】
上記問題を解決するため、本発明は次のように構成したのである。
請求項１に記載の発明は、ツールを備えたロボットと、前記ツールの先端の第１速度偏差ベクトルを算出する演算手段であって、前記第１速度偏差ベクトルは、前記ロボットが前記ツールに作用した外力に倣った結果生ずるものである第１の演算手段と、前記第１速度偏差ベクトルと位置指令とに基づいて、前記ロボットの駆動モータを位置制御する位置制御手段と、を備えたことを特徴とするものである。
請求項２に記載の発明は、基本軸および手首軸を少なくとも備えたロボットと、前記ロボットのツールの先端の第１速度偏差ベクトルを算出する演算手段であって、前記第１速度偏差ベクトルは、前記ロボットがその先端に設けられたツールに作用した外力に倣った結果生ずるものである第１の演算手段と、前記算出された速度偏差ベクトルのうち、並進動作に関するベクトル要素を有する第２速度偏差ベクトルを出力する第２の演算手段と、前記出力された第２速度偏差ベクトルを前記基本軸の関節速度ベクトルに分解する第３の演算手段と、前記分解された関節速度ベクトルを積算して位置修正量ベクトルを算出する第４の演算手段と、前記位置修正量ベクトルが加算された位置指令に基づいて、前記マニピュレータの駆動モータを位置制御する位置制御手段と、を備えたことを特徴とするものである。
請求項３に記載の発明は、基本軸および手首軸を少なくとも備えたロボットと、前記ロボットのツールの先端の第１速度偏差ベクトルを算出する演算手段であって、前記前記第１速度偏差ベクトルは、前記ロボットの先端に設けられたツールに作用した外力に倣った結果生ずるものである第１の演算手段と、前記算出された速度偏差ベクトルのうち、回転動作に関するベクトル要素を選択し、当該選択した要素の大きさをゼロとしたベクトルを第２速度偏差ベクトルとして出力する第２の演算手段と、前記出力された第２速度偏差ベクトルを前記基本軸と手首軸の関節速度ベクトルに分解する第３の演算手段と、前記分解された関節速度ベクトルを積算して位置修正量ベクトルを算出する第４の演算手段と、前記位置修正量ベクトルが加算された位置指令に基づいて、前記マニピュレータの駆動モータを位置制御する位置制御手段と、を備えたことを特徴とするものである。
請求項４に記載の発明は、基本軸および手首軸を少なくとも備えたロボットと、前記ロボットのツールの先端の第１速度偏差ベクトルを算出する演算手段であって、前記前記第１速度偏差ベクトルは、前記ロボットの先端に設けられたツールに作用した外力に倣った結果生ずるものである第１の演算手段と、前記算出された速度偏差ベクトルのうち、回転動作に関するベクトル要素を選択し、当該選択した要素の符号を反転したベクトルを第２速度偏差ベクトルとして出力する第２の演算手段と、前記出力された第２速度偏差ベクトルを前記基本軸と手首軸の関節速度ベクトルに分解する第３の演算手段と、前記分解された関節速度ベクトルを積算して位置修正量ベクトルを算出する第４の演算手段と、前記位置修正量ベクトルが加算された位置指令に基づいて、前記マニピュレータの駆動モータを位置制御する位置制御手段と、を備えたことを特徴とするものである。
請求項５に記載の発明は、前記第１の演算手段は、前記第１速度偏差ベクトルを、位置指令から計算された位置ベクトルと位置フィードバックから計算された位置ベクトルとの差を時間微分して算出することを特徴とするものである。
請求項６に記載の発明は、前記第１の演算手段が算出する前記第１速度偏差ベクトルは、直交座標系における速度偏差ベクトルであることを特徴とするものである。
請求項７に記載の発明は、ロボットのツールの先端の第１速度偏差ベクトルを算出する演算手段であって、前記第１速度偏差ベクトルは、前記ツールに作用した外力に倣った結果生ずるものである第１の演算手段と、
前記第１速度偏差ベクトルと位置指令とに基づいて、前記ロボットの駆動モータを位置制御する位置制御手段と、を備えたことを特徴とするものである。
請求項８に記載の発明は、基本軸および手首軸を少なくとも備えたロボットのツールの先端の第１速度偏差ベクトルを算出する演算手段であって、前記第１速度偏差ベクトルは、前記ツールに作用した外力に倣った結果生ずるものである第１の演算手段と、前記算出された速度偏差ベクトルのうち、並進動作に関するベクトル要素を有する第２速度偏差ベクトルを出力する第２の演算手段と、前記出力された第２速度偏差ベクトルを前記基本軸の関節速度ベクトルに分解する第３の演算手段と、前記分解された関節速度ベクトルを積算して位置修正量ベクトルを算出する第４の演算手段と、前記位置修正量ベクトルが加算された位置指令に基づいて、前記マニピュレータの駆動モータを位置制御する位置制御手段と、を備えたことを特徴とするものである。
請求項９に記載の発明は、基本軸および手首軸を少なくとも備えたロボットのツールの先端の第１速度偏差ベクトルを算出する演算手段であって、前記第１速度偏差ベクトルは、前記ツールに作用した外力に倣った結果生ずるものである第１の演算手段と、前記算出された速度偏差ベクトルのうち、回転動作に関するベクトル要素を選択し、当該選択したベクトル要素の大きさをゼロとしたベクトルを第２速度偏差ベクトルとして出力する第２の演算手段と、前記出力された第２速度偏差ベクトルを前記基本軸と手首軸の関節速度ベクトルに分解する第３の演算手段と、前記分解された関節速度ベクトルを積算して位置修正量ベクトルを算出する第４の演算手段と、前記位置修正量ベクトルが加算された位置指令に基づいて、前記マニピュレータの駆動モータを位置制御する位置制御手段と、を備えたことを特徴とするものである。
請求項１０に記載の発明は、基本軸および手首軸を少なくとも備えたロボットのツールの先端の第１速度偏差ベクトルを算出する演算手段であって、前記第１速度偏差ベクトルは、前記ツールに作用した外力に倣った結果生ずる、第１の演算手段と、前記算出された速度偏差ベクトルのうち、回転動作に関するベクトル要素を選択し、当該選択した要素の符号を反転したベクトルを第２速度偏差ベクトルとして出力する第２の演算手段と、前記出力された第２速度偏差ベクトルを前記基本軸と手首軸の関節速度ベクトルに分解する第３の演算手段と、前記分解された関節速度ベクトルを積算して位置修正量ベクトルを算出する第４の演算手段と、前記位置修正量ベクトルが加算された位置指令に基づいて、前記マニピュレータの駆動モータを位置制御する位置制御手段と、を備えたことを特徴とするものである。
請求項１１に記載の発明は、前記第１の演算手段は、前記第１速度偏差ベクトルを、位置フィードバックから計算される位置ベクトルから位置指令から計算される位置ベクトルを減算して算出することを特徴とするものである。
請求項１２に記載の発明は、前記第１の演算手段が算出する前記第１速度偏差ベクトルは、直交座標系における速度偏差ベクトルであることを特徴とするものである。
請求項１３に記載の発明は、ツールを備えたロボットが、前記ツールに作用した外力に倣った結果生ずる、前記ツール先端の速度偏差ベクトルを算出し、前記速度偏差ベクトルに基づいて前記位置指令を修正し、前記修正された位置指令に基づいて、前記ロボットの駆動モータを制御することを特徴とするものである。
請求項１４に記載の発明は、基本軸および手首軸を少なくとも備えたロボットが、ツールに作用した外力に倣った結果生ずる、前記ツール先端の直交座標における速度偏差ベクトルを算出し、前記速度偏差ベクトルを基本軸および手首軸についての関節速度ベクトルに分解し、前記関節速度ベクトルを時間積分して位置修正量ベクトルを算出し、前記位置修正量ベクトルにより前記ロボットの駆動モータの位置指令を修正することを特徴とするものである。
請求項１５に記載の発明は、前記速度偏差ベクトルは、位置指令から計算された前記手首軸の手首点から前記ツール先端への位置ベクトルと位置フィードバックから計算された前記手首軸の手首点から前記ツール先端への位置ベクトルとの差を時間微分して求められることを特徴とするものである。
請求項１６に記載の発明は、ツールを備えたロボットと、前記ツールの先端の第１速度偏差ベクトルを算出する演算器であって、前記第１速度偏差ベクトルは、前記ロボットが前記ツールに作用した外力に倣った結果生ずるものである第１の演算器と、前記第１速度偏差ベクトルと位置指令とに基づいて、前記ロボットの駆動モータを位置制御する位置制御器と、を備えたことを特徴とするものである。
請求項１７に記載の発明は、基本軸および手首軸を少なくとも備えたロボットと、前記ロボットのツールの先端の第１速度偏差ベクトルを算出する演算器であって、前記第１速度偏差ベクトルは、前記ロボットがその先端に設けられたツールに作用した外力に倣った結果生ずるものである第１の演算器と、前記算出された速度偏差ベクトルのうち、並進動作に関するベクトル要素を有する第２速度偏差ベクトルを出力する第２の演算器と、前記出力された第２速度偏差ベクトルを前記基本軸の関節速度ベクトルに分解する第３の演算器と、前記分解された関節速度ベクトルを積算して位置修正量ベクトルを算出する第４の演算器と、前記位置修正量ベクトルが加算された位置指令に基づいて、前記マニピュレータの駆動モータを位置制御する位置制御器と、を備えたことを特徴とするものである。
請求項１８に記載の発明は、基本軸および手首軸を少なくとも備えたロボットと、前記ロボットのツールの先端の第１速度偏差ベクトルを算出する演算手段であって、前記前記第１速度偏差ベクトルは、前記ロボットの先端に設けられたツールに作用した外力に倣った結果生ずるものである第１の演算器と、前記算出された速度偏差ベクトルのうち、回転動作に関するベクトル要素を選択し、当該選択した要素の大きさをゼロとしたベクトルを第２速度偏差ベクトルとして出力する第２の演算器と、前記出力された第２速度偏差ベクトルを前記基本軸と手首軸の関節速度ベクトルに分解する第３の演算器と、前記分解された関節速度ベクトルを積算して位置修正量ベクトルを算出する第４の演算器と、前記位置修正量ベクトルが加算された位置指令に基づいて、前記マニピュレータの駆動モータを位置制御する位置制御器と、を備えたことを特徴とするものである。
請求項１９に記載の発明は、基本軸および手首軸を少なくとも備えたロボットと、前記ロボットのツールの先端の第１速度偏差ベクトルを算出する演算手段であって、前記前記第１速度偏差ベクトルは、前記ロボットの先端に設けられたツールに作用した外力に倣った結果生ずるものである第１の演算器と、前記算出された速度偏差ベクトルのうち、回転動作に関するベクトル要素を選択し、当該選択した要素の符号を反転したベクトルを第２速度偏差ベクトルとして出力する第２の演算器と、前記出力された第２速度偏差ベクトルを前記基本軸と手首軸の関節速度ベクトルに分解する第３の演算器と、前記分解された関節速度ベクトルを積算して位置修正量ベクトルを算出する第４の演算器と、前記位置修正量ベクトルが加算された位置指令に基づいて、前記マニピュレータの駆動モータを位置制御する位置制御器と、を備えたことを特徴とするものである。
請求項２０に記載の発明は、基本軸および手首軸を少なくとも備えたロボットのツールの先端の第１速度偏差ベクトルを算出する演算手段であって、前記第１速度偏差ベクトルは、前記ツールに作用した外力に倣った結果生ずるものである第１の演算器と、前記算出された速度偏差ベクトルのうち、並進動作に関するベクトル要素を有する第２速度偏差ベクトルを出力する第２の演算器と、前記出力された第２速度偏差ベクトルを前記基本軸の関節速度ベクトルに分解する第３の演算器と、前記分解された関節速度ベクトルを積算して位置修正量ベクトルを算出する第４の演算器と、前記位置修正量ベクトルが加算された位置指令に基づいて、前記マニピュレータの駆動モータを位置制御する位置制御器と、を備えたことを特徴とするものである。
【発明の効果】
【０００５】
本発明によると、静止摩擦の小さな手首軸の回転倣い動作によるＴＣＰの動作を基本軸にフィードバックして手首点を並進倣い動作させているので、小さい外力でＴＣＰ（Ｔｏｏｌ  Ｃｅｎｔｅｒ  Ｐｏｉｎｔ）をリニアに倣わせることができ、嵌合作業を力センサレスで実現できるという効果がある。また、倣い状態における手首軸の駆動モータはオープンループ状態なので安定な制御系を実現できるという効果もある。さらに、嵌合方向の衝突力についても、各軸のトルクリミットで吸収できるので、倣い方向以外（嵌合方向）の過大力を防止できるという効果もある。
また、並進倣い動作時のＴＣＰ姿勢変化をより小さくできるので、嵌合作業中に加わるモーメントを軽減でき、より精密な嵌合作業を実現できるという効果がある。
【図面の簡単な説明】
【０００６】
【図１】本発明によるロボットの力制御方法のブロック図
【図２】本発明によるロボットの力制御の動作原理を説明する図
【図３】本発明による力制御方法を嵌合作業に適用した場合の模式図
【図４】嵌合作業の成否を説明する図
【図５】本発明の実施の形態におけるＴＣＰ姿勢変化を説明する図
【図６】一般的な産業用ロボットの制御装置の構成図
【符号の説明】
【０００７】
１０１ 第１の演算手段
１０２ 第２の演算手段
１０３ 第３の演算手段
１０４ 第４の演算手段
１０５ａ〜ｆ トルク制限付き位置制御手段（Ｓ〜Ｔ軸）
２０１ ＴＣＰに加わる外力（Ｘ方向）
２０２ 位置指令から計算したＴＣＰ位置ベクトル
２０３ 位置フィードバックから計算したＴＣＰ位置ベクトル
２０４ ＴＣＰ位置ベクトルの偏差
２０５ ロボット座標原点から見たＴＣＰ位置偏差ベクトル
２０６ ＴＣＰ以外のリンクに加わる外力
３０１ 嵌合部品
３０２ 被嵌合部品
４０１ 並進倣い動作
５０１ 下腕
５０２ 手首点
６０１ ロボット
６０２ａ〜６０２ｆ 関節軸（Ｓ〜Ｔ軸）
６０３ ツール（ハンド）
６０４ 制御装置
６０５ 教示操作盤
【発明を実施するための最良の形態】
【０００８】
以下、本発明の方法の具体的実施例について、説明する。【０００９】
図６は一般的な産業用ロボットの構成図を示している。図６において、ロボット６０１は、関節軸６０２ａから６０２ｆを備えた６自由度マニピュレータである。関節軸６０２ａから６０２ｆにはそれぞれエンコーダ付の駆動モータが内蔵されており、各関節軸を独立に動作させることができる。関節軸６０２ａから６０２ｆまでを順番にＳ軸、Ｌ軸、Ｕ軸、Ｒ軸、Ｂ軸、Ｔ軸と呼ぶ。また、Ｓ軸、Ｌ軸、Ｕ軸の３軸を基本軸と呼び、Ｒ軸、Ｂ軸、Ｔ軸の３軸を手首軸と呼ぶ。基本軸は手首点（Ｒ、Ｂ、Ｔ軸の回転軸が交差する点）の位置を決定する。手首軸はツール（ハンド）６０３の姿勢を決定する。ロボット６０１の制御装置６０４は、各軸の駆動モータのエンコーダ信号をもとにフィードバック制御手段を備え、駆動モータの動作を制御する。教示操作盤６０５は、教示者がロボットを操作したり、動作プログラムを教示、編集したりするための装置である。
なお、以降は上述した６自由度マニピュレータに本発明を適用した場合の実施の形態を説明する。ただし、少なくとも基本軸と手首軸を備えていれば、マニピュレータの自由度は任意で良く、６自由度以上のマニピュレータに対しても本発明を同様に適用できる。
【実施例１】
【００１０】
図１は本発明におけるロボットの力制御方法の制御ブロック図を示している。
図１において、第１の演算手段１０１は、前記ツールの先端の第１速度偏差ベクトルを算出する演算手段である。第１速度偏差ベクトルは、前記ロボットが前記ツールに作用した外力に倣った結果生ずる速度偏差ベクトルである。詳しく説明すると、第１の演算手段１０１は、関節位置指令と関節位置フィードバックをもとに、ツールの先端（Ｔｏｏｌ  Ｃｅｎｔｅｒ  Ｐｏｉｎｔ、以下、「ＴＣＰ」という。）の直交座標系における速度偏差ベクトル、すなわち、ＴＣＰ速度偏差ベクトル（第１速度偏差ベクトル）Ｖerrを計算する。第１の演算手段１０１は、演算器である。この演算器は、例えば、加算器またはＣＰＵを動作させるソフトウェアで実現することができる。
第２の演算手段１０２は、算出された第１速度偏差ベクトルのうち、並進動作に関するベクトル要素を有する第２速度偏差ベクトルを出力する演算手段である。第２の演算手段１０２は、後述するように、第１の演算手段１０１で計算されたＴＣＰ速度偏差ベクトルＶerrに選択行列を積算し、第２速度偏差ベクトルを算出する。第２の演算手段１０２は、演算器である。この演算器は、例えば、乗算器またはＣＰＵを動作させるソフトウェアで実現することができる。
第３の演算手段１０３は、出力された第２速度偏差ベクトルを前記基本軸の関節速度ベクトルに分解する演算手段である。第３の演算手段１０３は、後述するように、第２速度偏差ベクトルをロボット６０１の関節空間（Ｊｏｉｎｔ  Ｓｐａｃｅ）における速度偏差ベクトル（関節速度ベクトル）ωerrに分解し、出力する。第３の演算手段１０３は、演算器である。この演算器は、例えば、乗算器またはＣＰＵを動作させるソフトウェアで実現することができる。
第４の演算手段１０４は、出力された関節速度ベクトルを積算して位置修正量ベクトルを算出する演算手段である。第４の演算手段１０４は、関節速度偏差ベクトルωerrを積分して位置修正量θaddを計算し、出力する。第４の演算手段１０４は、演算器である。この演算器は、例えば、積分器またはＣＰＵを動作させるソフトウェアで実現することができる。
位置制御手段１０５ａ〜１０５ｆは、位置指令に基づいて、前記ロボットの駆動モータを位置制御する。位置制御手段１０５ａから１０５ｆは、それぞれＳ、Ｌ、Ｕ、Ｒ、Ｂ、Ｔ軸（６０２ａ〜６０２ｆ）の駆動モータに対して構成された位置制御手段である。これらは、重力モーメントの補償機能と、駆動モータの発生トルクを制限するトルクリミット機能を備えている。図１においては、Ｓ軸についてのみ具体的な位置制御手段を示しているが、その他の軸についても同様の構成となっている。ロボット６０１のリンクに外力が加わってある関節軸の外乱トルク（モーメント）がトルクリミットに達すると、その関節軸は外乱トルクの方向に回転してこれに従うことになる。位置制御手段１０５ａ〜１０５ｆは、位置制御器である。この位置制御器は、例えば、加算器と乗算器で実現することができる。また、ＣＰＵを動作させるソフトウェアで実現することもできる。【００１１】
次に、ロボットが外力に対してどのように動作するかを図１および図２に基づいて説明する。
図２は本発明におけるロボットの力制御方法の動作原理を示している。
図２（ａ）に示すように、ツール６０３の先端にＸ軸方向の外力Ｆx（２０１）が加わると、基本軸（Ｓ、Ｌ、Ｕ軸）に比べて静止摩擦の小さな手首軸６０２ｅ（Ｂ軸）がトルクリミットにかかり、Ｂ軸が倣う（外力に従って回転する）。ＴＣＰ位置ベクトルＰtcp_ref２０２は、位置指令から計算される。ＴＣＰ位置ベクトルＰtcp_fb２０３は、位置フィードバック（倣った結果の現在位置）から計算される。２つのＴＣＰ位置ベクトルＰtcp_fb２０３とＰtcp_ref２０２の偏差Ｐerr２０４は、次式で計算される。
Ｐerr ＝ Ｐtcp_fb − Ｐtcp_ref       (1)
第１の演算手段１０１は、ＴＣＰ位置ベクトルの偏差Ｐerr２０４を計算し、さらにそれを時間微分してＴＣＰ速度偏差ベクトルＶerrを求める。なお、第１実施例では、ＴＣＰ速度偏差ベクトルＶerrは次式で表される並進（位置３自由度）のみの３次元ベクトルであり、姿勢３自由度については考慮しない。
Ｖerr ＝ [ ｖex、 ｖey、 ｖez ] T       (2)
なお、添え字のTはベクトルおよび行列の転置を表す。【００１２】
第２の演算手段１０２は、ＴＣＰ速度偏差ベクトルＶerrに対して、次式（３）で表されるサイズ３×３（３行３列）の選択行列を左から掛ける。
Ｍsel ＝ [ Ｉ33 ]       (3)
ここで、Ｉ33はサイズ３×３の単位行列を表す。
この演算結果が、第２速度偏差ベクトルである。【００１３】
第３の演算手段１０３は、基本軸（Ｓ、Ｌ、Ｕ軸）に関する手首点に対するヤコビ行列Ｊ33（サイズ３×３）を計算する。第３の演算手段１０３は、第２速度偏差ベクトルにこのヤコビ行列Ｊ33の逆行列を掛けて、関節空間の速度偏差ベクトル（関節速度ベクトル）ωerrをつぎのように計算する。
ωerr ＝ inv(Ｊ33 )・Ｍsel・Ｖerr       (4)
なお、inv(Ｘ)は正方行列Ｘの逆行列を表す。【００１４】
第４の演算手段１０４は、関節空間の速度偏差ベクトルωerrを時間積分し、位置修正量ベクトルθaddを計算する。
図１において位置修正量ベクトルθaddの各成分がＳ、Ｌ、Ｕ軸の位置制御手段の位置指令θrefに加算されると、図２（ｂ）に示すように、手首点がＴＣＰ速度偏差ベクトルＶerrに倣うように直進（並進倣い動作）する。
なお、ＴＣＰ位置ベクトル２０２および２０３は手首点を始点としＴＣＰを終点とするベクトルである。図２（ｃ）に示すように、常にそれぞれの始点を一致させて偏差ベクトル２０４が計算されている。このように計算する代わりに、単純にロボット座標系原点から見たＴＣＰの位置偏差２０５を時間微分してＴＣＰ速度偏差ベクトルＶerrを計算することも可能である。しかし、図２（ｂ）に示すように、ツール以外のリンクに外力Ｆx2（２０６）が加わってＴＣＰに位置偏差が生じた場合、時間微分によって計算されたＴＣＰ速度偏差ベクトルＶerrが急に増加して動作限界（特異点）に達する恐れがあるため注意が必要である。【００１５】
図３は、本発明を嵌合作業に適用する場合の模式図を示している。図６と同じ符号については、図の表現は異なるが同じ物を示している。非接触状態からツール６０３で把持した嵌合部品３０１を被嵌合部品３０２の穴に向かって接近させると、部品同士が接触しＴＣＰに外力が作用する。
図３（ａ）のようにツール６０３が取り付けられている場合、Ｘ方向の外力ＦxによってＢ軸６０２ｅにモーメントＭxが加わる。Ｙ方向の外力ＦyによってＲ軸６０２ｄにモーメントＭyが加わる。したがって、Ｒ、Ｂ軸の回転倣い動作によって手首点がＸ軸およびＹ軸方向に並進倣い動作するので、図４（ａ）の４０１に示すように嵌合部品３０１が被嵌合部品３０２の穴の中心軸方向に並進倣いする。その結果、嵌合部品３０１と被嵌合部品３０２の穴の中心軸が一致し、嵌合部品３０１はスムーズに挿入される。Ｚ方向の外力Ｆzは、Ｕ軸６０２ｃの回転倣い動作で吸収される。したがって、特許文献３のように接触時に過大な外力が加わり部品、ツールまたはロボットを破損させることはない。なお、手首点を並進倣いさせずに、Ｒ、Ｂ軸の回転倣い動作だけで挿入しようとすると、図４（ｂ）に示すように軸が一致しないので、嵌合部品３０１が被嵌合部品３０２の穴に噛み付き、挿入できない。【００１６】
図３（ｂ）は、ツール６０３の取り付け角度が関節軸６０２ｆの回転軸に対して９０度異なっている点が、図３（ａ）と相違する。図３（ｂ）のようにツール６０３が取り付けられている場合、Ｂ、Ｔ軸によってＸＹ方向の外力によるモーメントに回転倣い動作する。Ｚ方向の外力Ｆzは、Ｕ軸およびＢ軸の回転倣い動作で吸収されるが、Ｕ軸よりＢ軸の方が、静止摩擦が小さいので、実質的にはＢ軸で吸収される。
外力への倣いは手首軸の駆動モータのトルクリミットにより実現されている。倣い動作した状態は、モーメントを受けた軸のトルクが一定値に制限されていることを意味する。したがって、制御系はオープンループ状態であり、本質的に安全な制御系になっている。
以上説明したように、本実施形態によれば、静止摩擦の小さな手首軸の回転倣い動作によるＴＣＰの動作を基本軸にフィードバックして手首点を並進倣い動作させているので、小さい外力であってもＴＣＰをリニアに倣わせることがでる。さらに、嵌合作業を力センサを使用せずに実現できるという効果がある。また、倣い状態における手首軸の駆動モータの制御系はオープンループ状態なので、安定な制御系を実現できるという効果もある。さらに、挿入方向の衝突力についても、各軸のトルクリミットで吸収できるので、倣い方向以外（挿入方向）の過大力を防止できるという効果もある。
【実施例２】
【００１７】
第２実施例におけるロボットの力制御方法の制御ブロック図は、第１実施例と同様に図１で表される。動作原理（図２）についても第１実施例とほぼ同じであるが、計算方法が異なる。
第２実施例では、ＴＣＰ速度偏差ベクトルＶerrは、６次元ベクトル（位置３自由度＋姿勢３自由度）として、次式（５）で計算される。
Ｖerr ＝ [ ｖex、 ｖey、 ｖez、 ｒex、 ｒey、 ｒez ] T       (5)
ここで、（ｒex、 ｒey、 ｒez ）はＴＣＰ位置ベクトル２０２からＴＣＰ位置ベクトル２０３への回転速度ベクトルｒである（図２（ｃ））。
第２の演算手段１０２は、ＴＣＰ速度偏差ベクトルＶerrに対して、次式（６）のサイズ６×６の選択行列を左からかける。【００１８】
2009034957.tif 000003
【００１９】
ここで、Ｏ33はサイズ３×３のゼロ行列である。
また、第３の演算手段１０３は、全軸（Ｓ、Ｌ、Ｕ、Ｒ、Ｂ、Ｔ）に関するＴＣＰに対するヤコビ行列Ｊ66（サイズ６×６）を計算し、Ｊ66を使って関節空間の速度偏差ベクトルωerrを次式（７）のように計算する。
ωerr ＝ inv(Ｊ66 )・Ｍsel2・Ｖerr       (7)【００２０】
第４の演算手段１０４は、関節空間の速度偏差ベクトルωerrを時間積分し、位置修正量ベクトルθaddを計算する。位置修正量ベクトルθaddの各成分は全軸の位置制御系の位置指令θrefに加算される。
第２実施例においては、制御系は、手首軸の回転倣い動作によって発生するＴＣＰの並進速度に対して、基本軸だけでなく全軸を使って追従するように作用する。その際、選択行列Ｍsel2で回転速度ベクトルｒの影響を無視している（回転速度ベクトルｒをゼロとしている）ので、並進倣い動作によってＴＣＰが余計に回転しないように（姿勢が変化しないように）動作する。したがって、第１実施例よりもＴＣＰの姿勢変化が小さくなる。【００２１】
図５はＴＣＰ姿勢変化の相違を模式的に説明している。まず、図５（ａ）（ｂ）はそれぞれ第１実施例、第２実施例における並進倣い動作時のＴＣＰ姿勢変化を示している。
図５において、５０１はロボット６０１の下腕、５０２は手首点である。ここで、実線は関節位置指令から計算した下腕５０１とツール６０３の状態（姿勢）を示している。また、点線は手首点の並進倣い動作あるいは手首軸の回転倣い動作によって変化した下腕５０１とツール６０３の姿勢を示している。αは位置指令から計算した下腕５０１とツール６０３の成す角である。βはＴＣＰに外力が作用したときに手首軸（Ｂ軸）の回転倣い動作によるツールの回転角である。γは併進倣い動作により手首点が移動した結果生ずる下腕の回転角度である。第１実施例では、基本軸（Ｓ、Ｌ、Ｕ）のみを使って並進倣い動作を実現しており手首軸の位置指令は修正されないので、位置指令から計算した下腕５０１とツール６０３の成す角はαのままである。したがって、ＴＣＰ姿勢はβに加えγだけ余計に変化している。
一方、第２実施例では、全軸を使用してＴＣＰ姿勢変化が生じないように並進倣い動作を実現しているので、手首軸の位置指令も修正され、その結果、図５（ｂ）に示すように位置指令から計算した下腕５０１とツール６０３の成す角はα−γになる。したがって、下腕の回転γによる影響が手首軸の位置指令修正により打ち消され、手首軸の回転倣い動作β以外にＴＣＰに余計な姿勢変化は生じない。
以上説明したように、本発明による第２実施例によれば、並進倣い動作時のＴＣＰ姿勢変化を第１実施例より小さくできるので、嵌合作業中に加わるモーメントを軽減でき、より精密な嵌合作業を実現できるという効果がある。
【実施例３】
【００２２】
第３実施例におけるロボットの制御方法の制御ブロック図も、第１および第２実施例と同様に図１で表される。動作原理（図２）についてもほぼ同様であるが、計算方法の詳細が若干異なる。
第３実施例では、ＴＣＰ速度偏差ベクトルＶerrは、第２実施例と同様に式（５）に基づいて求められる。
第２の演算手段１０２は、ＴＣＰ速度偏差ベクトルＶerrに対して、次式（８）のサイズ６×６の選択行列を左からかける。【００２３】
2009034957.tif 000004
【００２４】
また、第３の演算手段１０３は、全軸（Ｓ、Ｌ、Ｕ、Ｒ、Ｂ、Ｔ）に関するＴＣＰに対するヤコビ行列Ｊ66（サイズ６×６）を計算し、Ｊ66を使って関節空間の速度偏差ベクトルωerrを次式（９）のように計算する。
ωerr ＝ inv(Ｊ66 )・Ｍsel3・Ｖerr       (9)
第４の演算手段１０４は、関節空間の速度偏差ベクトルωerrを時間積分し、位置修正量ベクトルθaddを計算する。位置修正量ベクトルθaddの各成分は全軸の位置制御系の位置指令θrefに加算される。
第３実施例と第２実施例では選択行列が異なる。第２実施例では、選択行列Ｍｓｅｌ２の右下ブロックをＯ33（サイズ３×３のゼロ行列）として手首軸の回転倣い動作によるＴＣＰ姿勢変化速度を無視していた。第３実施例では、式（８）に示すように、選択行列Ｍｓｅｌ３の右下のブロックを−Ｉ33とすることで、手首軸の回転倣い動作によるＴＣＰ姿勢変化速度の向き（符号）を逆にしてＴＣＰを並進倣い動作させる。手首軸の回転倣い動作による速度偏差のうち、回転動作に関する速度偏差の向き（符号）を逆にすることは、手首軸の回転倣い動作によるＴＣＰ姿勢変化を打ち消すように全軸にフィードバック動作をかけることになる。【００２５】
図５（ｃ）は、第３実施例における並進倣い動作時のＴＣＰ姿勢変化を示している。並進倣い動作の結果、位置指令から計算した下腕５０１とツール６０３の成す角はα−β−γとなる。下腕の回転角度γだけでなく、手首軸の回転倣い動作によるツール回転角β（ＴＣＰ姿勢変化）も打ち消される。したがって、倣い動作時のＴＣＰの姿勢変化はほとんどなくなる。
以上説明したように、本発明による第３実施例によれば、並進倣い動作時のＴＣＰ姿勢変化を第２実施例より小さくできるので、嵌合作業中に加わるモーメントをさらに軽減でき、より精密な嵌合作業を実現できるという効果がある。
【産業上の利用可能性】
【００２６】
本発明は、産業用ロボットによる組立作業に広く適用することができる。
【図１】
2009034957.tif 000005
【図２】
2009034957.tif 000006
【図３】
2009034957.tif 000007
【図４】
2009034957.tif 000008
【図５】
2009034957.tif 000009
【図６】
2009034957.tif 000010
【国際調査報告】
2009034957.tif 000011
2009034957.tif 000012
2009034957.tif 000013