(57)【要約】【課題】確実に、第１部材の突起を第２部材の開口に挿入することができるロボット制御装置、ロボットおよびロボット制御方法を提供する。【解決手段】本発明のロボット制御装置は、ロボットアームと力覚センサーとを備え、ロボットアームに装着されたエンドエフェクターにより、突起を有する第１部材と開口を有する第２部材との一方を支持し、突起を開口に挿入する作業を行うロボットの作動を制御し、突起を開口に挿入する際のそれらの相対的な移動方向はＺ軸方向であり、作業の際、エンドエフェクターをＸ軸方向に移動させ、力覚センサーにより検出された各軸方向の力を変位に変換し、各変位に基づいて力制御により、ロボットアームの作動を制御する制御部と、Ｘ軸方向およびＹ軸方向の変位に基づいて開口または突起の位置を特定する位置特定部とを備え、力制御のＺ軸方向の弾性項は、Ｘ軸方向およびＹ軸方向の弾性項よりも大きく設定される。【選択図】図１
2015168050.tif 000002
【特許請求の範囲】
【請求項１】
エンドエフェクターが装着可能なロボットアームと、
互いに直交するＸ軸、Ｙ軸およびＺ軸を想定したとき、前記Ｘ軸方向、前記Ｙ軸方向および前記Ｚ軸方向の力を検出し、前記ロボットアームと前記エンドエフェクターとの間に設けられた力覚センサーと、を備え、
前記エンドエフェクターにより、突起を有する第１部材と開口を有する第２部材との一方を支持し、前記第１部材の前記突起を前記第２部材の前記開口に挿入する作業を行うロボットの作動を制御するロボット制御装置であって、
前記突起を前記開口に挿入する際の前記突起と前記開口との相対的な移動方向は前記Ｚ軸方向であり、
前記力覚センサーにより検出された前記Ｘ軸方向、前記Ｙ軸方向および前記Ｚ軸方向の力をそれぞれ変位に変換する力変位変換処理部を有し、前記作業の際、前記エンドエフェクターにより前記第１部材または前記第２部材を支持し、前記エンドエフェクターを前記Ｘ軸方向に移動させ、前記力変位変換処理部により変換された前記各変位に基づいて、力制御により、前記ロボットアームの作動を制御する制御部と、
前記Ｘ軸方向の変位および前記Ｙ軸方向の変位に基づいて、前記開口または前記突起の位置を特定する位置特定部と、を備え、
前記力制御の前記Ｚ軸方向の弾性項は、前記Ｘ軸方向の弾性項および前記Ｙ軸方向の弾性項よりも大きく設定されることを特徴とするロボット制御装置。
【請求項２】
前記エンドエフェクターを前記Ｘ軸方向に移動させる際は、前記エンドエフェクターを前記Ｚ軸方向に押し付ける請求項１に記載のロボット制御装置。
【請求項３】
前記エンドエフェクターを前記Ｘ軸方向に移動させる走査を、前記Ｙ軸方向の位置を変更して複数回行う請求項１または２に記載のロボット制御装置。
【請求項４】
前記力変位変換処理部により変換された前記Ｘ軸方向の変位の絶対値と第１の閾値とを比較する第１比較部を有し、
前記位置特定部は、前記走査の際、前記突起が前記開口の前記Ｙ軸方向の中央部を通過する場合は、前記Ｘ軸方向の変位の絶対値が前記第１の閾値を超えているときに、前記突起が前記開口の前記Ｘ軸方向の縁部に位置していると判定する請求項３に記載のロボット制御装置。
【請求項５】
前記エンドエフェクターは、前記第１部材を支持し、前記位置特定部により、前記開口の位置を特定するよう構成されており、
前記力変位変換処理部により変換された前記Ｘ軸方向の変位の絶対値と第１の閾値とを比較する第１比較部を有し、
前記位置特定部は、前記走査の際、前記突起が前記開口の前記Ｙ軸方向の中央部を通過する場合は、前記Ｘ軸方向の変位の絶対値が前記第１の閾値を超えているときに、前記突起が前記開口の前記Ｘ軸方向の進行方向下流側の縁部に位置していると判定する請求項３に記載のロボット制御装置。
【請求項６】
前記力変位変換処理部により変換された前記Ｙ軸方向の変位の絶対値と第２の閾値とを比較する第２比較部を有し、
前記位置特定部は、前記走査の際、前記突起が前記開口の前記Ｙ軸方向の縁部を通過する場合は、前記Ｙ軸方向の変位の絶対値が前記第２の閾値を超えているときに、前記突起が前記開口の前記Ｘ軸方向の中央部に位置していると判定する請求項３ないし５のいずれか１項に記載のロボット制御装置。
【請求項７】
前記制御部は、前記力覚センサーから出力される信号を処理するローパスフィルターを有する請求項１ないし６のいずれか１項に記載のロボット制御装置。
【請求項８】
前記力制御の前記Ｘ軸方向の弾性項と前記Ｙ軸方向の弾性項とは等しく設定される請求項１ないし７のいずれか１項に記載のロボット制御装置。
【請求項９】
エンドエフェクターが装着可能なロボットアームと、
互いに直交するＸ軸、Ｙ軸およびＺ軸を想定したとき、前記Ｘ軸方向、前記Ｙ軸方向および前記Ｚ軸方向の力を検出し、前記ロボットアームと前記エンドエフェクターとの間に設けられた力覚センサーと、を備え、
前記エンドエフェクターにより、突起を有する第１部材と開口を有する第２部材との一方を支持し、前記第１部材の前記突起を前記第２部材の前記開口に挿入する作業を行うロボットであって、
前記突起を前記開口に挿入する際の前記突起と前記開口との相対的な移動方向は前記Ｚ軸方向であり、
前記ロボットの作動を制御する請求項１ないし８のいずれか１項に記載のロボット制御装置を備えることを特徴とするロボット。
【請求項１０】
前記ロボットは、双腕ロボットである請求項９に記載のロボット。
【請求項１１】
エンドエフェクターが装着可能なロボットアームと、
互いに直交するＸ軸、Ｙ軸およびＺ軸を想定したとき、前記Ｘ軸方向、前記Ｙ軸方向および前記Ｚ軸方向の力を検出し、前記ロボットアームと前記エンドエフェクターとの間に設けられた力覚センサーと、を備え、
前記エンドエフェクターにより、突起を有する第１部材と開口を有する第２部材との一方を支持し、前記第１部材の前記突起を前記第２部材の前記開口に挿入する作業を行うロボットの作動を制御するロボット制御方法であって、
前記突起を前記開口に挿入する際の前記突起と前記開口との相対的な移動方向は前記Ｚ軸方向であり、
前記作業の際、前記エンドエフェクターにより前記第１部材または前記第２部材を支持し、前記エンドエフェクターを前記Ｘ軸方向に移動させ、前記力覚センサーにより検出された前記Ｘ軸方向、前記Ｙ軸方向および前記Ｚ軸方向の力をそれぞれ変位に変換し、前記各変位に基づいて、力制御により、前記ロボットアームの作動を制御する工程と、
前記Ｘ軸方向の変位および前記Ｙ軸方向の変位に基づいて、前記開口または前記突起の位置を特定する工程と、を備え、
前記力制御の前記Ｚ軸方向の弾性項は、前記Ｘ軸方向の弾性項および前記Ｙ軸方向の弾性項よりも大きく設定されることを特徴とするロボット制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボット制御装置、ロボットおよびロボット制御方法に関するものである。
【背景技術】
【０００２】
従来、ロボットアームを備えたロボットが知られている。ロボットアームは複数のリンクが関節部を介して連結され、最も先端側のリンクには、エンドエフェクターとして、例えば、ハンドが装着される。関節部はモーターにより駆動され、その関節部の駆動により、リンクが回動する。そして、ロボットは、例えば、ハンドで突起を有する挿入物を把持し、その突起を挿入対象物に設けられた開口（穴）に挿入する作業を行う。
【０００３】
また、最も先端側のリンクとハンドとの間には、力覚センサーが設けられており、ロボットは、前記作業の際は、力覚センサーにより、力やモーメントを検出し、その力覚センサーの検出結果に基づいて、インピーダンス制御（力制御）を行う（例えば、特許文献１参照）。また、ロボットは、力覚センサーの出力信号に基づいて、開口の位置を把握し、挿入物の突起を挿入対象物の開口に挿入する。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０１０−１３７２９９号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、従来のロボットでは、次のような問題点がある。
力覚センサーの出力信号は、ノイズが多く、Ｓ／Ｎ比が低い。力覚センサーは、検出機構部の変形を電気信号に変換する原理であり、その感度を高めるためには、検出機構部の剛性が低い構造にしなければならないが、この場合、検出機構部が変形し易くなり、力やモーメントによってハンドが変位してしまい、正確な位置決めができない。そのため、前記検出機構部の変形を抑制するために検出機構部の剛性を高める設計が通常なされるが、この場合、力覚センサーの感度が低下し、Ｓ／Ｎ比が低下する。そのため、真の力やモーメントを示す信号がノイズに埋もれ、正確に力やモーメントを検出することは困難である。特に、力覚センサーの出力信号について、閾値に対する大小を正しく判定することは困難である。
【０００６】
また、力覚センサーの出力信号からノイズを除去するため、ローパスフィルターを導入する場合、低いＳ／Ｎ比を改善するのに十分な効果を持つ狭い帯域のローパスフィルターは、力覚センサーの出力信号の波形をなまらせてしまい（速い変化の成分を減衰させてしまい）、特に、高速に移動しながらの検出を正確に行うことは困難である。
以上の問題のため、特に、力覚センサーの出力信号について、閾値に対する大小を正しく判定することは困難であり、したがって開口の位置を正確に求めることはできない。
本発明の目的は、確実に、第１部材の突起を第２部材の開口に挿入することができるロボット制御装置、ロボットおよびロボット制御方法を提供することにある。
【課題を解決するための手段】
【０００７】
このような目的は、下記の本発明により達成される。
（適用例１）
本発明に係わるロボット制御装置は、エンドエフェクターが装着可能なロボットアームと、
互いに直交するＸ軸、Ｙ軸およびＺ軸を想定したとき、前記Ｘ軸方向、前記Ｙ軸方向および前記Ｚ軸方向の力を検出し、前記ロボットアームと前記エンドエフェクターとの間に設けられた力覚センサーと、を備え、
前記エンドエフェクターにより、突起を有する第１部材と開口を有する第２部材との一方を支持し、前記第１部材の前記突起を前記第２部材の前記開口に挿入する作業を行うロボットの作動を制御するロボット制御装置であって、
前記突起を前記開口に挿入する際の前記突起と前記開口との相対的な移動方向は前記Ｚ軸方向であり、
前記力覚センサーにより検出された前記Ｘ軸方向、前記Ｙ軸方向および前記Ｚ軸方向の力をそれぞれ変位に変換する力変位変換処理部を有し、前記作業の際、前記エンドエフェクターにより前記第１部材または前記第２部材を支持し、前記エンドエフェクターを前記Ｘ軸方向に移動させ、前記力変位変換処理部により変換された前記各変位に基づいて、力制御により、前記ロボットアームの作動を制御する制御部と、
前記Ｘ軸方向の変位および前記Ｙ軸方向の変位に基づいて、前記開口または前記突起の位置を特定する位置特定部と、を備え、
前記力制御の前記Ｚ軸方向の弾性項は、前記Ｘ軸方向の弾性項および前記Ｙ軸方向の弾性項よりも大きく設定されることを特徴とする。
【０００８】
これにより、確実に、第２部材の開口または第１部材の突起の位置を特定することができ、第１部材の突起を第２部材の開口に挿入することができる。
例えば、エンドエフェクターにより第１部材を支持して作業を行う場合、第２部材の開口の位置を求める際、力制御のＺ軸方向の弾性項は、Ｘ軸方向の弾性項およびＹ軸方向の弾性項よりも大きいので、第１部材の突起の先端部が開口に到達すると、突起の先端部が容易、迅速かつ確実に開口に挿入されてゆく。これによって、開口により、第１部材の突起がＸ軸方向、Ｙ軸方向に拘束され、力覚センサーにより、Ｘ軸方向、Ｙ軸方向の力が確実に検出される、すなわち、第１部材の突起のＸ軸方向、Ｙ軸方向の目標位置からの変位を確実に検出することができる。また、力覚センサーにより検出された力を変位に変換することにより、ノイズを低減することができ、Ｓ／Ｎ比を高くすることができる。これにより、確実に、第２部材の開口の位置を求めることができる。
【０００９】
（適用例２）
本発明に係わるロボット制御装置では、前記エンドエフェクターを前記Ｘ軸方向に移動させる際は、前記エンドエフェクターを前記Ｚ軸方向に押し付けることが好ましい。
これにより、より確実に、第２部材の開口または第１部材の突起の位置を特定することができ、第１部材の突起を第２部材の開口に挿入することができる。
【００１０】
（適用例３）
本発明に係わるロボット制御装置では、前記エンドエフェクターを前記Ｘ軸方向に移動させる走査を、前記Ｙ軸方向の位置を変更して複数回行うことが好ましい。
これにより、より確実に、第２部材の開口または第１部材の突起の位置を特定することができ、第１部材の突起を第２部材の開口に挿入することができる。
【００１１】
（適用例４）
本発明に係わるロボット制御装置では、前記力変位変換処理部により変換された前記Ｘ軸方向の変位の絶対値と第１の閾値とを比較する第１比較部を有し、
前記位置特定部は、前記走査の際、前記突起が前記開口の前記Ｙ軸方向の中央部を通過する場合は、前記Ｘ軸方向の変位の絶対値が前記第１の閾値を超えているときに、前記突起が前記開口の前記Ｘ軸方向の縁部に位置していると判定することが好ましい。
これにより、より確実に、第２部材の開口または第１部材の突起の位置を特定することができ、第１部材の突起を第２部材の開口に挿入することができる。
【００１２】
（適用例５）
本発明に係わるロボット制御装置では、前記エンドエフェクターは、前記第１部材を支持し、前記位置特定部により、前記開口の位置を特定するよう構成されており、
前記力変位変換処理部により変換された前記Ｘ軸方向の変位の絶対値と第１の閾値とを比較する第１比較部を有し、
前記位置特定部は、前記走査の際、前記突起が前記開口の前記Ｙ軸方向の中央部を通過する場合は、前記Ｘ軸方向の変位の絶対値が前記第１の閾値を超えているときに、前記突起が前記開口の前記Ｘ軸方向の進行方向下流側の縁部に位置していると判定することが好ましい。
これにより、より確実に、第２部材の開口の位置を特定することができ、第１部材の突起を第２部材の開口に挿入することができる。
【００１３】
（適用例６）
本発明に係わるロボット制御装置では、前記力変位変換処理部により変換された前記Ｙ軸方向の変位の絶対値と第２の閾値とを比較する第２比較部を有し、
前記位置特定部は、前記走査の際、前記突起が前記開口の前記Ｙ軸方向の縁部を通過する場合は、前記Ｙ軸方向の変位の絶対値が前記第２の閾値を超えているときに、前記突起が前記開口の前記Ｘ軸方向の中央部に位置していると判定することが好ましい。
これにより、より確実に、第２部材の開口または第１部材の突起の位置を特定することができ、第１部材の突起を第２部材の開口に挿入することができる。
【００１４】
（適用例７）
本発明に係わるロボット制御装置では、前記制御部は、前記力覚センサーから出力される信号を処理するローパスフィルターを有することが好ましい。
これにより、力覚センサーから出力される信号に含まれるノイズを低減することができ、Ｓ／Ｎ比を高くすることができる。
【００１５】
（適用例８）
本発明に係わるロボット制御装置では、前記力制御の前記Ｘ軸方向の弾性項と前記Ｙ軸方向の弾性項とは等しく設定されることが好ましい。
これにより、力制御においてＸ軸方向とＹ軸方向とを同等に扱うことができ、制御を容易に行うことができる。
【００１６】
（適用例９）
本発明に係わるロボットは、エンドエフェクターが装着可能なロボットアームと、
互いに直交するＸ軸、Ｙ軸およびＺ軸を想定したとき、前記Ｘ軸方向、前記Ｙ軸方向および前記Ｚ軸方向の力を検出し、前記ロボットアームと前記エンドエフェクターとの間に設けられた力覚センサーと、を備え、
前記エンドエフェクターにより、突起を有する第１部材と開口を有する第２部材との一方を支持し、前記第１部材の前記突起を前記第２部材の前記開口に挿入する作業を行うロボットであって、
前記突起を前記開口に挿入する際の前記突起と前記開口との相対的な移動方向は前記Ｚ軸方向であり、
前記ロボットの作動を制御する本発明のロボット制御装置を備えることを特徴とする。
【００１７】
これにより、確実に、第２部材の開口または第１部材の突起の位置を特定することができ、第１部材の突起を第２部材の開口に挿入することができる。
例えば、エンドエフェクターにより第１部材を支持して作業を行う場合、第２部材の開口の位置を求める際、力制御のＺ軸方向の弾性項は、Ｘ軸方向の弾性項およびＹ軸方向の弾性項よりも大きいので、第１部材の突起の先端部が開口に到達すると、突起の先端部が容易、迅速かつ確実に開口に挿入されてゆく。これによって、開口により、第１部材の突起がＸ軸方向、Ｙ軸方向に拘束され、力覚センサーにより、Ｘ軸方向、Ｙ軸方向の力が確実に検出される、すなわち、第１部材の突起のＸ軸方向、Ｙ軸方向の目標位置からの変位を確実に検出することができる。また、力覚センサーにより検出された力を変位に変換することにより、ノイズを低減することができ、Ｓ／Ｎ比を高くすることができる。これにより、確実に、第２部材の開口の位置を求めることができる。
【００１８】
（適用例１０）
本発明に係わるロボットでは、前記ロボットは、双腕ロボットであることが好ましい。
これにより、多彩な動作を行うことができ、多彩な作業を行うことができる。
（適用例１１）
本発明に係わるロボット制御方法は、エンドエフェクターが装着可能なロボットアームと、
互いに直交するＸ軸、Ｙ軸およびＺ軸を想定したとき、前記Ｘ軸方向、前記Ｙ軸方向および前記Ｚ軸方向の力を検出し、前記ロボットアームと前記エンドエフェクターとの間に設けられた力覚センサーと、を備え、
前記エンドエフェクターにより、突起を有する第１部材と開口を有する第２部材との一方を支持し、前記第１部材の前記突起を前記第２部材の前記開口に挿入する作業を行うロボットの作動を制御するロボット制御方法であって、
前記突起を前記開口に挿入する際の前記突起と前記開口との相対的な移動方向は前記Ｚ軸方向であり、
前記作業の際、前記エンドエフェクターにより前記第１部材または前記第２部材を支持し、前記エンドエフェクターを前記Ｘ軸方向に移動させ、前記力覚センサーにより検出された前記Ｘ軸方向、前記Ｙ軸方向および前記Ｚ軸方向の力をそれぞれ変位に変換し、前記各変位に基づいて、力制御により、前記ロボットアームの作動を制御する工程と、
前記Ｘ軸方向の変位および前記Ｙ軸方向の変位に基づいて、前記開口または前記突起の位置を特定する工程と、を備え、
前記力制御の前記Ｚ軸方向の弾性項は、前記Ｘ軸方向の弾性項および前記Ｙ軸方向の弾性項よりも大きく設定されることを特徴とする。
【００１９】
これにより、確実に、第２部材の開口または第１部材の突起の位置を特定することができ、第１部材の突起を第２部材の開口に挿入することができる。
例えば、エンドエフェクターにより第１部材を支持して作業を行う場合、第２部材の開口の位置を求める際、力制御のＺ軸方向の弾性項は、Ｘ軸方向の弾性項およびＹ軸方向の弾性項よりも大きいので、第１部材の突起の先端部が開口に到達すると、突起の先端部が容易、迅速かつ確実に開口に挿入されてゆく。これによって、開口により、第１部材の突起がＸ軸方向、Ｙ軸方向に拘束され、力覚センサーにより、Ｘ軸方向、Ｙ軸方向の力が確実に検出される、すなわち、第１部材の突起のＸ軸方向、Ｙ軸方向の目標位置からの変位を確実に検出することができる。また、力覚センサーにより検出された力を変位に変換することにより、ノイズを低減することができ、Ｓ／Ｎ比を高くすることができる。これにより、確実に、第２部材の開口の位置を求めることができる。
【図面の簡単な説明】
【００２０】
【図１】本発明のロボットの第１実施形態におけるロボット本体の概略図である。
【図２】図１に示すロボットのブロック図である。
【図３】図１に示すロボットのブロック図である。
【図４】図１に示すロボットの主要部の動作を示す側面図である。
【図５】図１に示すロボットの主要部の動作を示す側面図である。
【図６】図１に示すロボットの力覚センサーの出力信号を示す図である。
【図７】図１に示すロボットの力覚センサーの出力信号を示す図である。
【図８】図１に示すロボットの力覚センサーの出力信号を示す図である。
【図９】図１に示すロボットにおける挿入物の先端の目標位置からの変位量を示す図である。
【図１０】図１に示すロボットにおける挿入物の先端の目標位置からの変位量を示す図である。
【図１１】本発明のロボットの第２実施形態におけるロボット本体の概略図である。
【発明を実施するための形態】
【００２１】
以下、本発明のロボット制御装置、ロボットおよびロボット制御方法を添付図面に示す好適な実施形態に基づいて詳細に説明する。
図１は、本発明のロボットの第１実施形態におけるロボット本体の概略図である。図２は、図１に示すロボットのブロック図である。図３は、図１に示すロボットのブロック図である。図４および図５は、それぞれ、図１に示すロボットの主要部の動作を示す側面図である。図６〜図８は、それぞれ、図１に示すロボットの力覚センサーの出力信号を示す図である。図９および図１０は、それぞれ、図１に示すロボットにおける挿入物の先端の目標位置からの変位量を示す図である。
【００２２】
なお、以下では、説明の都合上、図１中の上側を「上」または「上方」、下側を「下」または「下方」と言う。また、図１中の基台側を「基端」、その反対側を「先端」と言う。また、図３には、インピーダンス制御（力制御）および挿入対象物４６の穴４７の位置を求める際に使用される回路部分が示されており、インピーダンス制御に使用される回路部分については、各駆動源制御部のうちの第１駆動源制御部の部分が代表的に示されている。
【００２３】
図１〜図３に示すロボット（産業用ロボット）１は、例えば腕時計のような精密機器等を製造する製造工程で用いることができ、ロボット本体（本体部）１０と、ロボット本体１０（ロボット１）の作動を制御するロボット制御装置（制御部）２０とを有している。ロボット本体１０と、ロボット制御装置２０とは、それぞれ、電気的に接続されている。また、ロボット制御装置２０は、例えば、ＣＰＵ（Central Processing Unit）が内蔵されたパーソナルコンピューター（ＰＣ）等で構成することができる。なお、ロボット本体１０とロボット制御装置２０とは、一体であってもよく、また、別体であってもよい。なお、ロボット制御装置２０については、後で詳述する。
【００２４】
ロボット本体１０は、基台（支持部）１１と、５本のリンク（腕部）１２、１３、１４、１５、１６と、２本のリンク（腕部）１８、１９を有するリスト１７と、７つの駆動源４０１、４０２、４０３、４０４、４０５、４０６、４０７とを備えるロボットアーム５を有している。すなわち、ロボット１は、基台１１と、リンク１２、１３、１４、１５、１６と、リスト１７とが基端側から先端側に向ってこの順に連結された垂直多関節（７軸）ロボットである。なお、リンク１２を「第１リンク」、リンク１３を「第２リンク」、リンク１４を「第３リンク」、リンク１５を「第４リンク」、リンク１６を「第５リンク」、リスト１７を「第６リンク、第７リンク」と分けて言うことができる。リスト１７にはエンドエフェクター等を取り付けることができる。
【００２５】
図１に示すように、リンク１２〜１６、リスト１７は、それぞれ、基台１１に対し独立して変位可能に支持されている。なお、リンク１２〜１６、リスト１７の長さは、それぞれ、特に限定されず、諸条件に応じて適宜設定される。
基台１１と第１リンク１２とは、関節（ジョイント）１７１を介して連結されている。関節１７１は、互いに連結された基台１１と第１リンク１２のうちの一方を他方に対し回動可能に支持する機構を有している。この場合、第１リンク１２は、基台１１に対し、鉛直方向と平行な第１回転軸Ｏ１を回転中心（軸中心）とし、その第１回転軸Ｏ１回りに回動自在となっている。第１回転軸Ｏ１は、ロボット１の設置面である床１０１の上面の法線と一致している。この第１回転軸Ｏ１回りの回動は、第１駆動源４０１の駆動によりなされる。また、第１駆動源４０１はモーター４０１Ｍとケーブル（図示せず）によって駆動され、このモーター４０１Ｍは電気的に接続されたモータードライバー３０１を介してロボット制御装置２０により制御される。第１駆動源４０１はモーター４０１Ｍとともに設けた減速機（図示せず）によってモーター４０１Ｍからの駆動を伝達するように構成されていてもよく、また、減速機が省略されていてもよい。なお、ロボット本体１０の基台１１には、例えば、モーター４０１Ｍやモータードライバー３０１〜３０７が収納されている。
【００２６】
第１リンク１２と第２リンク１３とは、関節（ジョイント）１７２を介して連結されている。関節１７２は、互いに連結された第１リンク１２と第２リンク１３のうちの一方を他方に対し回動可能に支持する機構を有している。この場合、第２リンク１３は、第１リンク１２に対し、第２回転軸Ｏ２を回転中心とし、その第２回転軸Ｏ２回りに回動自在となっている。第２回転軸Ｏ２は、第１回転軸Ｏ１と直交している。この第２回転軸Ｏ２回りの回動は、第２駆動源４０２の駆動によりなされる。また、第２駆動源４０２は、モーター４０２Ｍとケーブル（図示せず）によって駆動され、このモーター４０２Ｍは電気的に接続されたモータードライバー３０２を介してロボット制御装置２０により制御される。第２駆動源４０２はモーター４０２Ｍの他に設けた減速機（図示せず）によってモーター４０２Ｍからの駆動を伝達するように構成されていてもよく、また、減速機が省略されていてもよい。なお、第２回転軸Ｏ２は、第１回転軸Ｏ１に直交する軸と平行であってもよい。なお、第１リンク１２には、例えば、モーター４０２Ｍが収納されている。
【００２７】
第２リンク１３と第３リンク１４とは、関節（ジョイント）１７３を介して連結されている。関節１７３は、互いに連結された第２リンク１３と第３リンク１４のうちの一方を他方に対し回動可能に支持する機構を有している。この場合、第３リンク１４は、第２リンク１３に対し、第３回転軸Ｏ３を回転中心とし、その第３回転軸Ｏ３回りに回動自在となっている。第３回転軸Ｏ３は、第２回転軸Ｏ２と直交している。この第３回転軸Ｏ３回りの回動は、第３駆動源４０３の駆動によりなされる。また、第３駆動源４０３は、モーター４０３Ｍとケーブル（図示せず）によって駆動され、このモーター４０３Ｍは電気的に接続されたモータードライバー３０３を介してロボット制御装置２０により制御される。第３駆動源４０３はモーター４０３Ｍの他に設けた減速機（図示せず）によってモーター４０３Ｍからの駆動を伝達するように構成されていてもよく、また、減速機が省略されていてもよい。なお、第３回転軸Ｏ３は、第２回転軸Ｏ２に直交する軸と平行であってもよい。なお、第２リンク１３には、例えば、モーター４０３Ｍが収納されている。
【００２８】
第３リンク１４と第４リンク１５とは、関節（ジョイント）１７４を介して連結されている。関節１７４は、互いに連結された第３リンク１４と第４リンク１５のうちの一方を他方に対し回動可能に支持する機構を有している。この場合、第４リンク１５は、第３リンク１４に対し、第４回転軸Ｏ４を回転中心とし、その第４回転軸Ｏ４回りに回動自在となっている。第４回転軸Ｏ４は、第３回転軸Ｏ３と直交している。この第４回転軸Ｏ４回りの回動は、第４駆動源４０４の駆動によりなされる。また、第４駆動源４０４は、モーター４０４Ｍとケーブル（図示せず）によって駆動され、このモーター４０４Ｍは電気的に接続されたモータードライバー３０４を介してロボット制御装置２０により制御される。第４駆動源４０４はモーター４０４Ｍとともに設けた減速機（図示せず）によってモーター４０４Ｍからの駆動を伝達するように構成されていてもよく、また、減速機が省略されていてもよい。なお、第４回転軸Ｏ４は、第３回転軸Ｏ３に直交する軸と平行であってもよい。なお、第３リンク１４には、例えば、モーター４０４Ｍが収納されている。
【００２９】
第４リンク１５と第５リンク１６とは、関節（ジョイント）１７５を介して連結されている。関節１７５は、互いに連結された第４リンク１５と第５リンク１６のうちの一方を他方に対し回動可能に支持する機構を有している。この場合、第５リンク１６は、第４リンク１５に対し、第５回転軸Ｏ５を回転中心とし、その第５回転軸Ｏ５回りに回動自在となっている。第５回転軸Ｏ５は、第４回転軸Ｏ４と直交している。この第５回転軸Ｏ５回りの回動は、第５駆動源４０５の駆動によりなされる。また、第５駆動源４０５は、モーター４０５Ｍとケーブル（図示せず）によって駆動され、このモーター４０５Ｍは電気的に接続されたモータードライバー３０５を介してロボット制御装置２０により制御される。第５駆動源４０５はモーター４０５Ｍとともに設けた減速機（図示せず）によってモーター４０５Ｍからの駆動を伝達するように構成されていてもよく、また、減速機が省略されていてもよい。なお、第５回転軸Ｏ５は、第４回転軸Ｏ４に直交する軸と平行であってもよい。なお、第４リンク１５には、例えば、モーター４０５Ｍが収納されている。
【００３０】
リスト１７は、第６リンク１８と第７リンク１９とを有している。このリスト１７には、その先端部に、エンドエフェクターとして、例えば、腕時計等のような精密機器を把持するハンド９１が着脱自在に装着される。なお、ハンド９１としては、特に限定されず、例えば、複数本の指部（フィンガー）を有する構成のものが挙げられる。ハンド９１の指部の本数は、図示の構成では２本であるが、２本に限定されず、３本以上であってもよい。このロボット１は、ハンド９１で精密機器を把持したまま、リンク１２〜１６やリスト１７等の動作を制御することにより、当該精密機器を搬送することができる。
【００３１】
また、第５リンク１６とリスト１７の第６リンク１８とは、関節（ジョイント）１７６を介して連結されている。関節１７６は、互いに連結された第５リンク１６とリスト１７の第６リンク１８のうちの一方を他方に対し回動可能に支持する機構を有している。この場合、リスト１７の第６リンク１８は、第５リンク１６に対し、第６回転軸Ｏ６を回転中心とし、その第６回転軸Ｏ６回りに回動自在となっている。第６回転軸Ｏ６は、第５回転軸Ｏ５と直交している。この第６回転軸Ｏ６回りの回動は、第６駆動源４０６の駆動によりなされる。また、第６駆動源４０６は、モーター４０６Ｍとケーブル（図示せず）によって駆動され、このモーター４０６Ｍは電気的に接続されたモータードライバー３０６を介してロボット制御装置２０により制御される。第６駆動源４０６はモーター４０６Ｍとともに設けた減速機（図示せず）によってモーター４０６Ｍからの駆動を伝達されてもよく、また、減速機が省略されていてもよい。
【００３２】
また、リスト１７の第６リンク１８と第７リンク１９とは、関節（ジョイント）１７７を介して連結されている。関節１７７は、互いに連結されたリスト１７の第６リンク１８と第７リンク１９のうちの一方を他方に対し回動可能に支持する機構を有している。この場合、リスト１７の第７リンク１９は、第６リンク１８に対し、第７回転軸Ｏ７を回転中心とし、その第７回転軸Ｏ７回りに回動自在となっている。第７回転軸Ｏ７は、第６回転軸Ｏ６と直交している。この第７回転軸Ｏ７回りの回動は、第７駆動源４０７駆動によりなされる。また、第７駆動源４０７の駆動は、モーター４０７Ｍとケーブル（図示せず）によって駆動され、このモーター４０７Ｍは電気的に接続されたモータードライバー３０７を介してロボット制御装置２０により制御される。第７駆動源４０７はモーター４０７Ｍとともに設けた減速機（図示せず）によってモーター４０７Ｍからの駆動を伝達するように構成されていてもよく、また、減速機が省略されていてもよい。なお、第６回転軸Ｏ６は、第５回転軸Ｏ５に直交する軸と平行であってもよく、また、第７回転軸Ｏ７は、第６回転軸Ｏ６に直交する軸と平行であってもよい。なお、第５リンク１６には、例えば、モーター４０６Ｍ、４０７Ｍが収納されている。
【００３３】
駆動源４０１〜４０７には、それぞれのモーター４０１Ｍ〜４０７Ｍまたは減速機に、第１角度センサー４１１、第２角度センサー４１２、第３角度センサー４１３、第４角度センサー４１４、第５角度センサー４１５、第６角度センサー４１６、第７角度センサー４１７が設けられている。これらの角度センサー４１１〜４１７としては、例えば、エンコーダー、ロータリーエンコーダー等を用いることができる。これらの角度センサー４１１〜４１７により、それぞれ、駆動源４０１〜４０７のモーター４０１Ｍ〜４０７Ｍあるいは減速機の回転軸の回転角度を検出する。この角度センサー４１１〜４１７の検出結果、すなわち、角度センサー４１１〜４１７から出力される信号は、ロボット制御装置２０に入力される。なお、この駆動源４０１〜４０７のモーター４０１Ｍ〜４０７Ｍとしては、それぞれ、特に限定されず、例えば、ＡＣサーボモーター、ＤＣサーボモーター等のサーボモーターを用いるのが好ましい。
【００３４】
また、ロボット本体１０は、ロボットアーム５のリスト１７のリンク１９の先端部（リスト１７のリンク１９とハンド９１との間）に設けられた力覚センサー８１を有している。なお、力覚センサー８１は、前記の位置に限らず、例えば、ハンド９１の基端部等に設けられていてもよい。
力覚センサー８１は、ハンド９１が把持した後述する挿入物４１を介して受ける反力等の力やモーメントを検出するものである。この力覚センサー８１としては、特に限定されず、各種のものを用いることができるが、その１例としては、例えば、互いに直交する３軸（Ｘ軸、Ｙ軸、Ｚ軸）の各軸方向の力および各軸回りのモーメントを検出する６軸力覚センサー等が挙げられる。なお、以下では、力とモーメントとを含めて力と言う。この力覚センサー８１の検出結果、すなわち、力覚センサー８１から出力される信号は、ロボット制御装置２０に入力される。
【００３５】
ロボット本体１０は、ロボット制御装置２０と電気的に接続されている。すなわち、駆動源４０１〜４０７、角度センサー４１１〜４１７、力覚センサー８１は、それぞれ、ロボット制御装置２０と電気的に接続されている。
そして、ロボット制御装置２０は、リンク１２〜１６、リスト１７をそれぞれ独立して作動させることができる、すなわち、モータードライバー３０１〜３０７を介して、駆動源４０１〜４０７をそれぞれ独立して制御することができる。この場合、ロボット制御装置２０は、角度センサー４１１〜４１７、力覚センサー８１等により検出を行い、その検出結果に基づいて、駆動源４０１〜４０７の駆動、例えば、角速度や回転角度等をそれぞれ制御する。この場合、ロボット制御装置２０は、例えば、インピーダンス制御（力制御）、位置制御等の所定の制御を行う。この制御プログラムは、ロボット制御装置２０に内蔵された記録媒体に予め記憶されている。
【００３６】
図４および図５に示すように、このロボット１は、ロボット制御装置２０により、ロボット本体１０（腕部１２〜１６、リスト１７、ハンド９１等）の作動を制御することにより、ハンド９１で、突起（挿入部）４２を有する第１部材（第１組立対象物）である挿入物４１と、穴（開口）４７を有する第２部材（第２組立対象物）である挿入対象物４６との一方を把持（支持）し、突起４２を穴４７に挿入する作業（動作）を行う。なお、ハンド９１で、挿入物４１を把持してもよく、また、挿入対象物４６を把持してもよいが、本実施形態では、代表的に、挿入物４１を把持する場合を図示して説明する。なお、ハンド９１で挿入物４１を把持する場合は、穴４７の位置を求め（特定し）、挿入物４１の突起４２をその穴４７の位置に移動させ、突起４２を穴４７に挿入する。また、ハンド９１で挿入対象物４６を把持する場合は、突起４２の位置を求め（特定し）、挿入対象物４６の穴４７をその突起４２の位置に移動させ、突起４２を穴４７に挿入する。
【００３７】
挿入物４１の形状、挿入対象物４６の形状は、それぞれ、特に限定されないが、本実施形態では、代表的に、挿入物４１、挿入対象物４６の穴４７の形状をそれぞれ円筒状、挿入対象物４６の全体形状を直方体状とした場合を図示して説明する。なお、図示の構成では、挿入物４１自体が突起４２であり、その挿入物４１の先端部は、尖っている。
また、挿入対象物４６に形成された穴４７には、貫通穴と、貫通していない穴、すなわち、有底の穴（凹部）とが含まれるが、本実施形態では、代表的に、貫通穴を図示して説明する。なお、穴４７が貫通穴の場合は、作業において、挿入物４１は、その貫通穴を通過してしまってもよい。
【００３８】
次に、図１〜図３を参照し、ロボット制御装置２０の構成について説明する。
ロボット制御装置２０は、ロボット本体１０全体、すなわち、第１駆動源４０１、第２駆動源４０２、第３駆動源４０３、第４駆動源４０４、第５駆動源４０５、第６駆動源４０６、第７駆動源４０７、リスト１７に装着されたハンド９１の駆動源等の作動をそれぞれ制御する装置である。
【００３９】
図２に示すように、ロボット制御装置２０は、第１駆動源４０１の作動を制御する第１駆動源制御部（制御部）２０１と、第２駆動源４０２の作動を制御する第２駆動源制御部（制御部）２０２と、第３駆動源４０３の作動を制御する第３駆動源制御部（制御部）２０３と、第４駆動源４０４の作動を制御する第４駆動源制御部（制御部）２０４と、第５駆動源４０５の作動を制御する第５駆動源制御部（制御部）２０５と、第６駆動源４０６の作動を制御する第６駆動源制御部（制御部）２０６と、第７駆動源４０７の作動を制御する第７駆動源制御部（制御部）２０７とを有している。
【００４０】
ここで、ロボット制御装置２０は、ロボット本体１０が行う処理の内容に基づいてリスト１７の先端部の目標位置、すなわち、リスト１７に装着されたハンド９１の目標位置を求め、その目標位置にハンド９１を移動させるための軌道を生成する。そして、ロボット制御装置２０は、その生成した軌道に沿ってハンド９１（リスト１７）が移動するように、各駆動源４０１〜４０７の回転角度を所定の制御周期ごとに測定し、この測定結果に基づいて演算した値をそれぞれ各駆動源４０１〜４０７の位置指令Ｐｃとして駆動源制御部２０１〜２０７に出力する。なお、前記および以下では、「値が入力、出力」等と表記しているが、これは、「その値に対応する信号が入力、出力」の意味である。
【００４１】
第１駆動源制御部２０１には、第１駆動源４０１の位置指令Ｐｃの他、第１角度センサー４１１から検出信号が入力される。第１駆動源制御部２０１は、第１角度センサー４１１の検出信号から算出される第１駆動源４０１の回転角度（位置フィードバック値Ｐｆｂ）が位置指令Ｐｃになり、かつ、後述する角速度フィードバック値ωｆｂが後述する角速度指令ωｃになるように、各検出信号を用いたフィードバック制御によって第１駆動源４０１を駆動する。
【００４２】
すなわち、第１駆動源制御部２０１の第１減算器（図示せず）には、位置指令Ｐｃが入力され、また、後述する位置フィードバック値Ｐｆｂが入力される。第１駆動源制御部２０１では、第１角度センサー４１１から入力されるパルス数がカウントされるとともに、そのカウント値に応じた第１駆動源４０１の回転角度が位置フィードバック値Ｐｆｂとして第１減算器に出力される。第１減算器は、これら位置指令Ｐｃと位置フィードバック値Ｐｆｂとの偏差（第１駆動源４０１の回転角度の目標値から位置フィードバック値Ｐｆｂを減算した値）を出力する。
【００４３】
また、第１駆動源制御部２０１は、第１減算器から入力された偏差と、予め定められた係数である比例ゲイン等を用いた所定の演算処理を行うことで、その偏差に応じた第１駆動源４０１の角速度の目標値を演算する。そして、その第１駆動源４０１の角速度の目標値（指令値）を示す信号を角速度指令（第１角速度指令）ωｃとして第２減算器（図示せず）に出力する。なお、ここでは、本実施形態では、フィードバック制御として、比例制御（Ｐ制御）がなされるが、これに限定されるものではない。
【００４４】
また、第１駆動源制御部２０１では、第１角度センサー４１１から入力されるパルス信号の周波数に基づいて、第１駆動源４０１の角速度が算出され、その角速度が角速度フィードバック値ωｆｂとして第２減算器に出力される。
第２減算器には、角速度指令ωｃが入力され、また、角速度フィードバック値ωｆｂが入力される。第２減算器は、これら角速度指令ωｃと角速度フィードバック値ωｆｂとの偏差（第１駆動源４０１の角速度の目標値から角速度フィードバック値ωｆｂを減算した値）を出力する。
【００４５】
また、第１駆動源制御部２０１は、第２減算器から入力された偏差と、予め定められた係数である比例ゲイン、積分ゲイン等を用い、積分を含む所定の演算処理を行うことで、その偏差に応じた第１駆動源４０１の角加速度（トルク）の目標値を演算する。そして第１駆動源制御部２０１は、その第１駆動源４０１の角加速度の目標値（指令値）を示す信号を角加速度指令（トルク指令）として生成する。なお、ここでは、本実施形態では、フィードバック制御として、ＰＩ制御がなされるが、これに限定されるものではない。
【００４６】
第１駆動源制御部２０１は、その角加速度指令に基づいて、第１駆動源４０１の駆動信号（駆動電流）を生成し、モータードライバー３０１を介してモーター４０１Ｍに供給する。
このようにして、第１駆動源４０１の角加速度、すなわち、トルクがその目標値と可及的に等しくなり、かつ、位置フィードバック値Ｐｆｂが位置指令Ｐｃと可及的に等しくなるとともに、角速度フィードバック値ωｆｂが角速度指令ωｃと可及的に等しくなるように、フィードバック制御がなされ、第１駆動源４０１の駆動電流が制御される。
【００４７】
なお、第２駆動源制御部２０２〜第７駆動源制御部２０７については、それぞれ、前記第１駆動源制御部２０１と同様であるので、その説明は省略する。
次に、ロボット１がそのハンド９１で挿入物４１を把持し、その挿入物４１の突起４２を挿入対象物４６の穴４７に挿入する作業におけるロボット制御装置２０の制御動作について説明する。なお、第１駆動源制御部２０１〜第７駆動源制御部２０７の制御動作は、同様であるので、以下では、代表的に、第１駆動源制御部２０１の制御動作について説明する。
【００４８】
図３に示すように、ロボット制御装置２０は、減算器５１と、制御回路５２と、ローパスフィルター５３と、力変位変換部５４と、加算器５５と、第１比較部５６１および第２比較部５６２を有する比較回路５６と、位置特定部５７とを備えている。減算器５１、制御回路５２、ローパスフィルター５３、力変位変換部５４および加算器５５は、制御部の一部を構成する。なお、互いに直交するＸ軸、Ｙ軸、Ｚ軸を想定する（図４、図５参照）。この場合、作業において挿入対象物４６の穴４７の位置を求める際にハンド９１（挿入物４１）を移動させる方向（走査方向）がＸ軸方向であり、挿入物４１の突起４２を挿入対象物４６の穴４７に挿入する際の突起４２と穴４７との相対的な移動方向がＺ軸方向である。
【００４９】
このロボット制御装置２０の各駆動源制御部２０１〜２０７は、それぞれ、作業の際、力覚センサー８１により検出されたＸ軸方向、Ｙ軸方向およびＺ軸方向の力をそれぞれ力変位変換関数により変位（変位量）に変換し、各変位に基づいて、インピーダンス制御（力制御）により、ロボット本体１０（ロボットアーム５）の作動、すなわち、各駆動源４０１〜４０７の作動を制御する。インピーダンス制御を行うことにより、挿入物４１、挿入対象物４６等の物体同士を倣わせることができ、挿入物４１が他の物体に接触した接触状態においてもその挿入物４１に無理な力が加わることを防止することができる。
【００５０】
このインピーダンス制御（コンプライアンス制御）は、制御が複雑であるものの、汎用性や安全性が高いという利点がある。インピーダンス制御の１つであるコンプライアンス制御を説明する。コンプライアンスはバネ定数の逆数を意味し、バネ定数が硬さを表すのに対して、コンプライアンスは柔らかさを意味する。ロボット１と環境との間に相互作用が働くときに、機械的柔軟性であるコンプライアンスを与える制御をコンプライアンス制御と呼ぶ。例えば、ロボット１のアーム（ロボットアーム５）には力覚センサー８１が取り付けられていて、このロボット１のアームは、力覚センサー８１で得られたセンサー情報（力・トルク情報）に応じて姿勢が変わるようにプログラムされている。具体的には仮想的なバネが、あたかもアームの先端に取り付けられているかのようにロボット１を制御する。例えば仮想的なバネのバネ定数が１００Ｋｇ／ｍであったとする。これに５Ｋｇの力で押せば、仮想的なバネは５ｃｍだけ縮む。逆に言えば、５ｃｍだけ縮んでいれば、５Ｋｇの力で押されているといえる。つまり、力情報と位置情報とが線形かつ対称に対応づけられている。
【００５１】
コンプライアンス制御では、この仮想的なバネがアームの先端に取り付けられているかのような制御が行われる。具体的には、ロボット１は、力覚センサー８１の入力に応答して動作し、例示した５Ｋｇの加重に対して、５ｃｍだけ後退するように制御され、力情報に対応して位置情報が変化するように制御される。
このような単純なコンプライアンス制御では時間項を含まないが、時間項を含み、その２次の項までを考慮した制御が、インピーダンス制御である。具体的には、２次の項は質量項であり、１次の項は粘性項であり、インピーダンス制御のモデルは下式（１）に示すような運動方程式で表すことができる。
【００５２】
【数１】
2015168050.tif 000003
【００５３】
上式（１）において、ｍは質量、μは粘性項、ｋは弾性項、ｆは力、ｘは目標位置からの変位である。また、ｘの１次微分、２次微分は、各々、速度、加速度に対応する。インピーダンス制御では、上式（１）の特性をアームの先端であるエンドエフェクター部（ハンド９１）に持たせるための制御系を構成する。即ち上式（１）で表される仮想質量、仮想粘性項、仮想弾性項を、あたかもアームの先端が持っているかのように制御を行う。
このように、インピーダンス制御は、アームの先端の質量に粘性要素と弾性要素が各方向に接続されるモデルにおいて、目的として設定された粘性項と弾性項で物体に接触するようにする制御である。
【００５４】
インピーダンス制御のＺ軸方向の弾性項は、Ｘ軸方向の弾性項およびＹ軸方向の弾性項よりも大きく設定される。これにより、挿入対象物４６の穴４７の位置を求める際、挿入物４１の突起４２の先端部が穴４７に到達すると、突起４２の先端部が容易、迅速かつ確実に穴４７に挿入されてゆく。これによって、穴４７により、挿入物４１の突起４２がＸ軸方向、Ｙ軸方向に拘束され、力覚センサー８１により、Ｘ軸方向、Ｙ軸方向の力が確実に検出される、すなわち、挿入物４１の突起４２のＸ軸方向、Ｙ軸方向の目標位置からの変位を確実に発生させることができる。これにより、穴４７の位置を正確に求めることができる。
また、インピーダンス制御のＸ軸方向の弾性項とＹ軸方向の弾性項との大小については、特に限定されず、Ｘ軸方向の弾性項とＹ軸方向の弾性項とは、等しくてもよく、また、異なっていてもよい。なお、例えば、インピーダンス制御のＸ軸方向の弾性項とＹ軸方向の弾性項を等しく設定することにより、制御をより容易に行うことができる。
【００５５】
また、Ｚ軸方向の弾性項（Ａｚ）とＸ軸方向の弾性項（Ａｘ）との比（Ａｚ／Ａｘ）、Ｚ軸方向の弾性項（Ａｚ）とＹ軸方向の弾性項（Ａｙ）との比（Ａｚ／Ａｙ）は、それぞれ、特に限定されず、諸条件に応じて適宜設定されるが、１．５以上、１０以下であることが好ましく、２以上、５以下であることがより好ましい。これにより、挿入対象物４６の穴４７の位置を求める際、挿入物４１の突起４２の先端部が穴４７に到達すると、突起４２の先端部がより容易、迅速かつ確実に穴４７に挿入されてゆき、これにより、挿入物４１の突起４２のＸ軸方向、Ｙ軸方向の変位をより確実に発生させることができる。これにより、穴４７の位置をより正確に求めることができる。
【００５６】
図４に示すように、作業の際は、まず、挿入対象物４６の穴４７の位置を求めるために、ハンド９１（挿入物４１）をＸ軸方向に移動させる動作、すなわち、Ｘ軸方向の走査を、Ｙ軸方向の位置を変更して、複数回行う。また、この走査は、ハンド９１をＺ軸方向に押し付け、これにより、挿入物４１の突起４２の先端部を押し当て面に押し当てつつ行う。また、前記押し当て面は、挿入対象物４６またはロボット１の稼働範囲に設置された構造物に設けられたものである。なお、前記構造物としては、例えば、作業テーブル等が挙げられる。
【００５７】
そして、作業の際は、力覚センサー８１により、力の検出が行われ、その力覚センサー８１から信号（データ）、すなわち、Ｘ軸方向、Ｙ軸方向およびＺ軸方向の力を示す信号（データ）が出力される。図６に示すように、力覚センサー８１から出力されるＸ軸方向、Ｙ軸方向およびＺ軸方向の力を示す信号には、それぞれ、真の信号の他にノイズが含まれている。力覚センサー８１から出力されるＸ軸方向、Ｙ軸方向およびＺ軸方向の力を示す信号は、それぞれ、ローパスフィルター５３において高周波数成分が除去され、力変位変換部５４に入力される。ローパスフィルター５３により、信号に含まれるノイズを低減することができる。なお、以下では、Ｘ軸方向、Ｙ軸方向およびＺ軸方向の力を示す信号（データ）等の３軸方向の信号（データ）を、単に、「信号（データ）」とも言う。
【００５８】
なお、図７には、挿入物４１の突起４２が穴４７のＹ軸方向の中央部を通過する場合の力覚センサー８１から出力されるＸ軸方向、Ｙ軸方向およびＺ軸方向の力を示す信号が図示されている。また、図８には、挿入物４１の突起４２が穴４７のＹ軸方向の縁部近傍を通過する場合の力覚センサー８１から出力されるＸ軸方向、Ｙ軸方向およびＺ軸方向の力を示す信号が図示されている。
【００５９】
力変位変換部５４では、入力されたデータに対して力変位変換関数を用いた演算処理が行われ、力に関するデータ（信号）が変位に関するデータ（信号）に変換される。この変換は通常２階積分、または、２次遅れ特性（２次ローパスフィルターと同等）であるので、ノイズのような周波数の高い成分は周波数の２乗に比例して減衰する。そのため、この変換により、ノイズを大幅に低減することができる。この変換により、ノイズを低減することができる。以下、前記力に関するデータ（信号）を「力データ（力信号）」とも言い、また、前記変位に関するデータ（信号）を「変位データ（変位信号）」とも言う。
【００６０】
次に、加算器５５で、変位信号と、角度センサー４１１から出力された信号とが加算され、位置フィードバック値Ｐｆｂとされる。この位置フィードバック値Ｐｆｂは、前述したように、減算器５１に入力され、減算器５１は、これら位置指令Ｐｃと位置フィードバック値Ｐｆｂとの偏差を出力する。この偏差は、減算器５１の後段の制御回路５２に入力され、制御回路５２において、前述した各処理が行われる。
【００６１】
また、Ｘ軸方向およびＹ軸方向の変位信号は、それぞれ、力変位変換部５４から比較回路５６に入力され、比較回路５６において、所定の閾値と比較され、その比較回路５６から比較信号が出力される。比較回路５６は、変位信号の大きさの絶対値が閾値よりも大きい場合は、出力する比較信号のレベルをハイレベルにし、変位信号の大きさの絶対値が閾値よりも小さい場合は、出力する比較信号のレベルをローレベルにする。これにより、比較回路５６からは、矩形状の比較信号が出力される（図９、図１０参照）。この比較回路５６から出力された比較信号は、位置特定部５７に入力される。これについては、以下、詳細に説明する。
【００６２】
まず、Ｚ方向のインピーダンス制御の弾性項は、大きいので、挿入物４１の突起４２が穴４７のＹ軸方向のいずれの位置を通過する場合でも、挿入物４１の突起４２の先端部が穴４７に到達すると、突起４２の先端部は、追従性良く、迅速に穴４７の中に挿入されていく。
そして、図９に示すように、挿入物４１の突起４２が穴４７のＹ軸方向の中央部を通過する場合は、突起４２が穴４７のＸ軸方向の進行方向下流側の縁部近傍に到達すると、突起４２の先端部が穴４７に引っかかり移動しにくくなるため、Ｘ軸方向の変位量（変位）、すなわち、Ｘ軸方向の変位信号は、急激に増大する。このため、Ｘ軸方向の変位信号と比較する所定の第１の閾値を設定しておき、Ｘ軸方向の変位信号の大きさ（変位）の絶対値が、その第１の閾値を超えているとき、突起４２が穴４７のＸ軸方向の進行方向下流側の縁部近傍に位置していると判定することができる。
【００６３】
また、このとき、ロボットアーム５のＹ軸周りの回転（モーメント）に対するインピーダンス制御の弾性項を高く設定しておくことが好ましい。これにより、挿入物４１の突起４２が穴４７から逃げにくくなり、挿入物４１がＸ軸方向に移動するとともに、挿入物４１のＸ軸方向の変位量は移動量にほぼ等しい増加量を示す。
なお、Ｙ軸方向には力が発生しないため、Ｙ軸方向の変位量（変位）、すなわち、Ｙ軸方向の変位信号の大きさの絶対値は、ほぼ０である。
【００６４】
また、図１０に示すように、挿入物４１の突起４２が穴４７のＹ軸方向の縁部近傍を通過する場合は、穴４７の縁部近傍では、挿入物４１の突起４２は、穴４７にわずかしか挿入されず、挿入物４１の進行方向であるＸ軸方向に対して傾斜した面に当接するので、Ｘ軸方向に発生する力はわずかであり、これに比例してＸ軸方向の変位量も小さい。
一方、Ｙ軸方向の変位量、すなわち、Ｙ軸方向の変位信号は、大きく、穴４７のＸ軸方向の中央部でほぼ最大となる。このため、Ｙ軸方向の変位信号と比較する所定の第２の閾値を設定しておき、Ｙ軸方向の変位信号の大きさの絶対値が、その第２の閾値を超えているとき、突起４２が穴４７のＸ軸方向の中央部に位置していると判定することができる。
【００６５】
ここで、比較回路５６において、Ｘ軸方向の変位信号と比較する第１の閾値は、挿入物４１の突起４２が穴４７のＹ軸方向の中央部を通過する場合は、Ｘ軸方向の変位信号の大きさの絶対値よりも小さく、挿入物４１の突起４２が穴４７のＹ軸方向の縁部（端部）近傍を通過する場合は、Ｘ軸方向の変位信号の大きさの絶対値より大きくなる値に設定される。
【００６６】
また、比較回路５６において、Ｙ軸方向の変位信号と比較する第２の閾値の絶対値は、挿入物４１の突起４２が穴４７のＹ軸方向の中央部を通過する場合は、Ｙ軸方向の変位信号の大きさの絶対値よりも大きく、挿入物４１の突起４２が穴４７のＹ軸方向の縁部近傍を通過する場合は、Ｙ軸方向の変位信号の大きさの絶対値より小さくなる値に設定される。
【００６７】
そして、図９に示すように、挿入物４１の突起４２が穴４７のＹ軸方向の中央部を通過する場合は、Ｘ軸方向の比較信号については、矩形状の信号は、突起４２が穴４７のＸ軸方向の進行方向下流側の縁部近傍に位置したときに生じる。また、Ｙ軸方向の比較信号については、矩形状の信号は、生じない。これにより、位置特定部５７は、Ｘ軸方向の比較信号が矩形状のとき、突起４２が穴４７のＸ軸方向の進行方向下流側の縁部近傍に位置していると判定する。すなわち、第１比較部５６１は、Ｘ軸方向の変位信号の大きさの絶対値と第１の閾値とを比較し、位置特定部５７は、Ｘ軸方向の変位信号の大きさの絶対値が第１の閾値を超えているときに、突起４２が穴４７のＸ軸方向の進行方向下流側の縁部に位置していると判定する。
【００６８】
また、図１０に示すように、挿入物４１の突起４２が穴４７のＹ軸方向の縁部近傍を通過する場合は、Ｘ軸方向の比較信号については、矩形状の信号は、生じない。また、Ｙ軸方向の比較信号については、矩形状の信号は、突起４２が穴４７のＸ軸方向の中央部に位置したときに生じる。これにより、位置特定部５７は、Ｙ軸方向の比較信号が矩形状のとき、突起４２が穴４７のＸ軸方向の中央部に位置していると判定する。すなわち、第２比較部５６２は、Ｙ軸方向の変位信号の大きさの絶対値と第２の閾値とを比較し、位置特定部５７は、Ｙ軸方向の変位信号の大きさの絶対値が第２の閾値を超えているときに、突起４２が穴４７のＸ軸方向の中央部に位置していると判定する。
【００６９】
位置特定部５７は、比較回路５６から入力されるＸ軸方向の比較信号およびＹ軸方向の比較信号に基づいて、穴４７の位置、特に、穴４７の中心の位置を求める（特定する）。
なお、位置特定部５７の図示しない記憶部には、予め、例えば、穴４７の寸法を加味し、Ｘ軸方向の比較信号について矩形状の信号が生じた位置、Ｙ軸方向の比較信号について矩形状の信号が生じた位置と、穴４７の中心の位置との関係を示すテーブルが記憶されている。位置特定部５７は、比較回路５６から入力されるＸ軸方向の比較信号が生じた位置、Ｙ軸方向の比較信号が生じた位置、前記テーブルに基づいて、穴４７の中心の位置を求める。
【００７０】
次に、図５に示すように、ロボットアーム５、ハンド９１を作動させ、挿入物４１の突起４２を穴４７に挿入する。なお、この際もインピーダンス制御を行う。
以上説明したように、このロボット１によれば、ハンド９１により挿入物４１を支持して作業を行う場合、挿入対象物４６の穴４７の位置を求める際、インピーダンス制御のＺ軸方向の弾性項は、Ｘ軸方向の弾性項およびＹ軸方向の弾性項よりも大きいので、挿入物４１の突起４２の先端部が穴４７に到達すると、突起４２の先端部が容易、迅速かつ確実に穴４７に挿入されてゆく。これによって、穴４７により、挿入物４１の突起４２がＸ軸方向、Ｙ軸方向に拘束され、力覚センサー８１により、Ｘ軸方向、Ｙ軸方向の力が確実に検出される、すなわち、挿入物４１の突起４２のＸ軸方向、Ｙ軸方向の目標位置からの変位を確実に検出することができる。また、力覚センサー８１により検出された力を変位に変換することにより、ノイズを低減することができ、Ｓ／Ｎ比を高くすることができる。これにより、確実に、挿入対象物４６の穴４７の位置を求めることができ、挿入物４１の突起４２を穴４７に挿入することができる。
【００７１】
＜第２実施形態＞
図１１は、本発明のロボットの第２実施形態におけるロボット本体の概略図である。
以下、第２実施形態について、前述した第１実施形態との相違点を中心に説明し、同様の事項については、その説明を省略する。
図１１に示すように、第２実施形態のロボット１は、双腕ロボットであり、そのロボット本体１０は、２つのロボットアーム５と、各ロボットアーム５を支持する基台（支持部）としての胴部１１０とを備えている。このロボット１は、例えば、２つのハンド９１の一方で挿入物４１を把持し、他方で挿入対象物４６を把持して作業を行う。
【００７２】
このロボット１によれば、前述した第１実施形態と同様の効果が得られる。
そして、このロボット１は、双腕ロボットであるので、多彩な動作を行うことができ、多彩な作業を行うことができる。
なお、ロボットアームの数は、２つに限らず、３つ以上であってもよい。
以上、本発明のロボット制御装置、ロボットおよびロボット制御方法を、図示の実施形態に基づいて説明したが、本発明はこれに限定されるものではなく、各部の構成は、同様の機能を有する任意の構成のものに置換することができる。また、本発明に、他の任意の構成物や工程が付加されていてもよい。
【００７３】
また、本発明は、前記各実施形態のうちの、任意の２以上の構成（特徴）を組み合わせたものであってもよい。
また、各駆動源のモーターとしては、それぞれ、前記サーボモーターの他、例えば、ステッピングモーター等が挙げられる。また、モーターとしてステッピングモーターを用いる場合は、角度センサーとして、例えば、ステッピングモーターへ入力する駆動パルスの数を計測することで、モーターの回転角度を検出するものを用いてもよい。
【００７４】
また、前記実施形態では、ロボットアームの回転軸の数は、７つであるが、本発明では、これに限定されず、ロボットアームの回転軸の数は、例えば、２つ、３つ、４つ、５つ、６つまたは８つ以上でもよい。すなわち、本実施形態では、リストが２本のリンクを有しているので、ロボットアームのリンクの本数は、７本であるが、本発明では、これに限定されず、ロボットアームのリンクの本数は、例えば、２本、３本、４本、５本、６本または８本以上でもよい。
また、本発明では、ロボット（ロボット本体）は、他の形式のロボット、例えば、脚部を有する脚式歩行（走行）ロボット等であってもよい。
【符号の説明】
【００７５】
１……ロボット（産業用ロボット）、１０……ロボット本体  １１０……胴部、１１……基台  １２、１３、１４、１５、１６、１８、１９……リンク  １７……リスト  １７１、１７２、１７３、１７４、１７５、１７６、１７７……関節（ジョイント）  ２０……ロボット制御装置  １０１……床  ２０１、２０２、２０３、２０４、２０５、２０６、２０７……駆動源制御部  ３０１、３０２、３０３、３０４、３０５、３０６、３０７……モータードライバー  ４０１、４０２、４０３、４０４、４０５、４０６、４０７……駆動源  ４０１Ｍ、４０２Ｍ、４０３Ｍ、４０４Ｍ、４０５Ｍ、４０６Ｍ、４０７Ｍ……モーター  ４１１、４１２、４１３、４１４、４１５、４１６、４１７……角度センサー  ５……ロボットアーム  ４１……挿入物  ４２……突起  ４６……挿入対象物  ４７……穴  ５１……減算器  ５２……制御回路  ５３……ローパスフィルター  ５４……力変位変換部  ５５……加算器  ５６……比較回路  ５６１……第１比較部  ５６２……第２比較部  ５７……位置特定部  ８１……力覚センサー  ９１……ハンド  Ｏ１、Ｏ２、Ｏ３、Ｏ４、Ｏ５、Ｏ６、０７……回転軸
【図１】
2015168050.tif 000004
【図２】
2015168050.tif 000005
【図３】
2015168050.tif 000006
【図４】
2015168050.tif 000007
【図５】
2015168050.tif 000008
【図６】
2015168050.tif 000009
【図７】
2015168050.tif 000010
【図８】
2015168050.tif 000011
【図９】
2015168050.tif 000012
【図１０】
2015168050.tif 000013
【図１１】
2015168050.tif 000014