(57)【要約】【課題】エンドエフェクタが物体を支持した後、ロボットの手先の位置が発散又は振動するのを抑制する。【解決手段】ロボット２００が、ロボットアーム２５１と、ロボットアーム２５１の先端に設けられ、ワークＷ１を把持するロボットハンド２５２とを有する。ロボット２００に設けられた力検知部により、ロボット２００の手先にかかる手先力が検知される。制御部は、手先の手先負荷モデルの負荷値を用いてロボット２００の動作を制御する。制御部は、手先において負荷変動が生じた際に力検知部により検知された手先力を用いて手先負荷モデルの推定値φｂを求める。制御部は、負荷変動の後の手先負荷モデルの負荷値を、負荷変動の前の手先負荷モデルの負荷値φａから経時的に漸次、推定値φｂに向かって変化するように設定する。【選択図】図６
2017056525.tif 000002
【特許請求の範囲】
【請求項１】
多関節のロボットアームと、前記ロボットアームの先端に設けられ、物体を支持するエンドエフェクタと、前記エンドエフェクタが物体を支持している状態又は前記エンドエフェクタが物体を支持していない状態である手先にかかる手先力を検知する力検知部とを有するロボットと、
前記手先の手先負荷モデルの負荷値を用いて前記ロボットの動作を制御する制御部と、を備え、
前記制御部は、
前記手先において負荷変動が生じた際に前記力検知部により検知された手先力を用いて前記手先負荷モデルの推定値を求める推定処理と、
前記負荷変動の後の前記手先負荷モデルの負荷値を、前記負荷変動の前の前記手先負荷モデルの負荷値から経時的に漸次、前記推定値に向かって変化するように設定する設定処理と、を実行することを特徴とするロボット装置。
【請求項２】
前記負荷変動とは、前記エンドエフェクタが前記物体を支持していない状態から前記物体を支持している状態に変化することである請求項１に記載のロボット装置。
【請求項３】
前記制御部は、前記推定処理において、前記手先を移動させている状態で前記力検知部により検知された手先力を用いて前記推定値を求めることを特徴とする請求項１又は２に記載のロボット装置。
【請求項４】
前記制御部は、前記推定処理において、前記手先に並進力又は回転力を付与した状態で前記力検知部により検知された手先力を用いて前記推定値を求めることを特徴とする請求項３に記載のロボット装置。
【請求項５】
前記制御部は、前記推定処理において、前記推定値を複数回求めることを特徴とする請求項３又は４に記載のロボット装置。
【請求項６】
前記エンドエフェクタのエンドエフェクタ負荷モデルの第１負荷値を予め記憶する記憶部を備え、
前記制御部は、前記推定処理として、
前記エンドエフェクタが前記物体を支持していない状態で所定位置姿勢のときに前記力検知部にて検知された第１手先力と、前記エンドエフェクタが前記物体を支持している状態で前記所定位置姿勢のときに前記力検知部にて検知された第２手先力との差から、前記エンドエフェクタが支持した前記物体の物体負荷モデルの第２負荷値を算出し、
前記第１負荷値と前記第２負荷値とを加算して前記推定値を算出することを特徴とする請求項２に記載のロボット装置。
【請求項７】
前記エンドエフェクタのエンドエフェクタ負荷モデルの第１負荷値を予め記憶する記憶部を備え、
前記制御部は、前記推定処理として、
前記エンドエフェクタが前記物体を支持していない状態で第１位置姿勢のときに前記力検知部にて検知された手先力から、前記エンドエフェクタが前記物体を支持していない状態で第２位置姿勢のときの第１手先力を推定し、
前記第１手先力と、前記エンドエフェクタが前記物体を支持している状態で前記第２位置姿勢のときに前記力検知部にて検知された第２手先力との差から、前記エンドエフェクタが支持した前記物体の物体負荷モデルの第２負荷値を算出し、
前記第１負荷値と前記第２負荷値とを加算して前記推定値を算出することを特徴とする請求項２に記載のロボット装置。
【請求項８】
前記手先負荷モデルは、前記手先の質量、重心位置若しくは慣性テンソル又はこれらの組み合わせであることを特徴とする請求項１乃至７のいずれか１項に記載のロボット装置。
【請求項９】
前記負荷変動の前の前記手先負荷モデルの負荷値をφａとし、
前記推定値をφｂとし、
０以上１以下の間で変化する修正係数をｋとしたとき、
前記制御部は、前記設定処理にて
【数１】
2017056525.tif 000014
の式に従い、前記修正係数ｋを経時的に漸次０から１に単調増加させることで、前記負荷変動の後の前記手先負荷モデルの負荷値を算出することを特徴とする請求項１乃至８のいずれか１項に記載のロボット装置。
【請求項１０】
前記修正係数ｋは、経時時間に対し線形に変化することを特徴とする請求項９に記載のロボット装置。
【請求項１１】
前記修正係数ｋは、経時時間に対し非線形に変化することを特徴とする請求項９に記載のロボット装置。
【請求項１２】
前記修正係数ｋは、時間で微分可能な時間の関数であることを特徴とする請求項１１に記載のロボット装置。
【請求項１３】
前記ロボットアームは、各関節をそれぞれ駆動する複数のモータと、前記各モータに対する各トルク指令値に基づいて前記各モータの駆動を制御する駆動制御部と、を有し、
前記制御部は、前記力検知部により検知された手先力及び前記負荷モデルを用いて前記各トルク指令値を求めることを特徴とする請求項１乃至１２のいずれか１項に記載のロボット装置。
【請求項１４】
ロボットが、多関節のロボットアームと、前記ロボットアームの先端に設けられ、物体を支持するエンドエフェクタと、前記エンドエフェクタが物体を支持している状態又は前記エンドエフェクタが物体を支持していない状態である手先にかかる手先力を検知する力検知部とを有しており、制御部が、前記手先の手先負荷モデルの負荷値を用いて前記ロボットの動作を制御するロボット制御方法であって、
前記制御部が、前記手先において負荷変動が生じた際に前記力検知部により検知された手先力を用いて前記手先負荷モデルの推定値を求める推定工程と、
前記制御部が、前記負荷変動の後の前記手先負荷モデルの負荷値を、前記負荷変動の前の前記手先負荷モデルの負荷値から経時的に漸次、前記推定値に向かって変化するように設定する設定工程と、を備えたことを特徴とするロボット制御方法。
【請求項１５】
請求項１４に記載のロボット制御方法の各工程をコンピュータに実行させるためのプログラム。
【請求項１６】
請求項１５に記載のプログラムが記録された、コンピュータが読み取り可能な記録媒体。
【請求項１７】
ロボットが、多関節のロボットアームと、前記ロボットアームの先端に設けられ、物体を支持するエンドエフェクタと、前記エンドエフェクタが物体を支持している状態又は前記エンドエフェクタが物体を支持していない状態である手先にかかる手先力を検知する力検知部とを有しており、制御部が、前記手先の手先負荷モデルを用いて前記ロボットの動作を制御して、組立部品を製造する組立部品の製造方法であって、
前記制御部が、前記エンドエフェクタに第１部品を支持させたときに前記力検知部により検知された手先力を用いて前記手先負荷モデルの推定値を求める推定工程と、
前記制御部が、前記エンドエフェクタに前記第１部品を支持させたことによる負荷変動の後の前記手先負荷モデルの負荷値を、前記負荷変動の前の前記手先負荷モデルの負荷値から経時的に漸次、前記推定値に向かって変化するように設定する設定工程と、
前記制御部が、前記設定工程にて設定された前記手先負荷モデルの負荷値を用いて前記ロボットの動作を制御し、前記第１部品を第２部品に組付ける組付工程と、を備えたことを特徴とする組立部品の製造方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、手先負荷モデルを用いてロボットの動作を制御するロボット装置、ロボット制御方法、プログラム、記録媒体及び組立部品の製造方法に関する。
【背景技術】
【０００２】
搬送や組立などに用いられる産業用ロボットは、組立作業等の作業時における速度等の制御性能向上が求められている。
【０００３】
このような制御性能を向上する方法として、ロボットアームの先端に取り付けられたエンドエフェクタ、又はエンドエフェクタ及び支持物体からなる手先負荷モデルに基づいたモデルベース制御方法がある。手先負荷モデル（手先の動力学モデル）とは、手先の質量、重心位置若しくは慣性テンソル、又はこれらの組み合わせである。
【０００４】
モデルベース制御方法は、重力やロボット自身の動作による慣性力や遠心力、コリオリ力などの動力学的な非線形要素を補償するようなフィードフォワード量をロボット動作に反映する方法として知られている。組立に必要な力制御においては、モデルベース制御によって接触作業時の安定性と速度の向上が図ることができる。すなわち、手先負荷モデルが正確であればロボットを高速に駆動させても安定した動作を実現できる。
【０００５】
しかし手先負荷モデルの情報に基づくモデルベース制御において、モデル化誤差があると制御性能に大きな影響を与える。また、ロボットの各関節の制限負荷内で作業を行うためには正確な手先負荷モデルが必要となる。
【０００６】
特許文献１では、ワークを微小量持ち上げ、把持前後の手先にかかる力・モーメントの差からワークの質量・重心位置等の負荷モデルを推定する方法が提案されている。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特許第５３２７７２２号公報
【発明の概要】
【発明が解決しようとする課題】
【０００８】
しかしながら、特許文献１では、ロボットの各関節に発生する負荷トルクから手先負荷モデルの推定が可能だが、推定したワークの質量・重心位置をロボットの制御に反映する際に、手先負荷モデルの値がワークの支持前後で大きく変化する。手先負荷モデルの値がワークの支持前後で大きく変化するので、手先負荷モデルに基づいて制御するモデルベース制御方法では、ロボットの手先の位置が発散又は振動する可能性があった。ロボットの手先の位置が発散又は振動すると、ロボット又はロボットが支持している物体が、周辺の物体に衝突するおそれがあった。
【０００９】
そこで、本発明は、エンドエフェクタが物体を支持した後、ロボットの手先の位置が発散又は振動するのを抑制することを目的とする。
【課題を解決するための手段】
【００１０】
本発明のロボット装置は、多関節のロボットアームと、前記ロボットアームの先端に設けられ、物体を支持するエンドエフェクタと、前記エンドエフェクタが物体を支持している状態又は前記エンドエフェクタが物体を支持していない状態である手先にかかる手先力を検知する力検知部とを有するロボットと、前記手先の手先負荷モデルの負荷値を用いて前記ロボットの動作を制御する制御部と、を備え、前記制御部は、前記手先において負荷変動が生じた際に前記力検知部により検知された手先力を用いて前記手先負荷モデルの推定値を求める推定処理と、前記負荷変動の後の前記手先負荷モデルの負荷値を、前記負荷変動の前の前記手先負荷モデルの負荷値から経時的に漸次、前記推定値に向かって変化するように設定する設定処理と、を実行することを特徴とする。
【発明の効果】
【００１１】
本発明によれば、ロボットの作業中に手先負荷が急激に変動した際にロボットの手先の位置が発散したり振動したりするのを抑制することができる。
【図面の簡単な説明】
【００１２】
【図１】第１実施形態に係るロボット装置の概略構成を示す斜視図である。
【図２】第１実施形態に係るロボット装置のロボット制御装置を示すブロック図である。
【図３】第１実施形態に係るロボット装置の制御系を示す制御ブロック図である。
【図４】第１実施形態においてロボットを力制御する際のフローチャートである。
【図５】第１実施形態においてロボットを位置制御する際のフローチャートである。
【図６】（ａ）はロボットによる組付作業を説明するためのロボットの模式図である。（ｂ）は手先負荷モデルの負荷値φａを説明するための図である。（ｃ）は手先負荷モデルの負荷値φｂを説明するための図である。
【図７】第１実施形態に係る組立部品の製造方法を示すフローチャートである。
【図８】（ａ）は、第１実施形態において時間に対する鉛直方向の手先の目標位置と現在位置を示すグラフである。（ｂ）は、第１実施形態において時間に対する修正係数ｋ（ｔ）を示すグラフである。（ｃ）は、第１実施形態において時間に対する手先負荷モデルの負荷値φ（ｔ）を示すグラフである。
【図９】（ａ）は、比較例において時間に対する手先負荷モデルの負荷値φ（ｔ）を示すグラフである。（ｂ）は、比較例において時間に対する鉛直方向の手先の目標位置と現在位置を示すグラフである。
【図１０】第２実施形態に係る組立部品の製造方法を示すフローチャートである。
【図１１】（ａ）は、第２実施形態において時間に対する修正係数ｋ（ｔ）を示すグラフである。（ｂ）は、第２実施形態において時間に対する手先負荷モデルの負荷値φ（ｔ）を示すグラフである。（ｃ）は、第２実施形態において時間に対する鉛直方向の手先の目標位置と現在位置を示すグラフである。
【図１２】モデル誤差が含まれる場合の時間に対する鉛直方向の手先の目標位置と現在位置を示すグラフである。
【図１３】第３実施形態に係る組立部品の製造方法を示すフローチャートである。
【図１４】（ａ）は、第４実施形態において時間に対する修正係数ｋ（ｔ）を示すグラフである。（ｂ）は、第４実施形態において時間に対する手先負荷モデルの負荷値φ（ｔ）を示すグラフである。（ｃ）は、第４実施形態において時間に対する鉛直方向の手先の目標位置と現在位置を示すグラフである。
【図１５】第５実施形態に係る組立部品の製造方法を示すフローチャートである。
【図１６】第６実施形態に係る組立部品の製造方法を示すフローチャートである。
【発明を実施するための形態】
【００１３】
以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。
【００１４】
［第１実施形態］
図１は、第１実施形態に係るロボット装置の概略構成を示す斜視図である。図１に示すように、ロボット装置１００は、多関節のロボット２００と、ロボット２００の動作を制御するロボット制御装置３００と、を備えている。また、ロボット装置１００は、ロボット制御装置３００に教示データを送信する教示装置としてのティーチングペンダント４００を備えている。ティーチングペンダント４００は、操作者が操作するものであり、ロボット２００やロボット制御装置３００の動作を指定するのに用いる。
【００１５】
ロボット２００は、多関節のロボットアーム２５１と、ロボットアーム２５１の先端に取り付けられたエンドエフェクタとしてのロボットハンド２５２と、を備えている。ロボットアーム２５１は、第１実施形態では垂直多関節である。以下、エンドエフェクタがロボットハンド２５２である場合について説明するが、これに限定するものではなく、ツール等であってもよい。ロボットアーム２５１の基端は、台座Ｂに固定されている。ロボットハンド２５２は、物体（部品やツール等）を把持（支持）するものである。
【００１６】
ロボット２００、即ちロボットアーム２５１は、複数の関節、例えば６つ関節（６軸）Ｊ１〜Ｊ６を有している。ロボットアーム２５１は、各関節Ｊ１〜Ｊ６を各関節軸Ａ１〜Ａ６まわりにそれぞれ回転駆動する複数（６つ）のサーボモータ２０１〜２０６を有している。
【００１７】
ロボットアーム２５１は、複数のリンク（フレーム）２１００〜２１０６が各関節Ｊ１〜Ｊ６で回転可能に連結されている。ここで、基端側から先端側に向かって、リンク２１００〜２１０６が順に直列に連結されている。ロボットアーム２５１は、可動範囲の中であれば、任意の３次元位置で任意の３方向の姿勢に、ロボット２００の手先（ロボットアーム２５１の先端）を向けることができる。
【００１８】
ロボットアーム２５１の位置及び姿勢は、座標系で表現することができる。座標系Ｔｏは、ロボットアーム２５１の基端、即ち台座Ｂに固定した座標系を表し、座標系Ｔｅはロボット２００の手先（ロボットアーム２５１の先端）に固定した座標系を表す。
【００１９】
ここで、ロボット２００の手先とは、第１実施形態では、ロボットハンド２５２が物体を把持（支持）していない場合には、ロボットハンド２５２のことである。ロボットハンド２５２が物体を把持（支持）している場合は、ロボットハンド２５２と把持（支持）している物体（例えば部品やツール等）とを含めてロボット２００の手先という。つまり、ロボットハンド２５２が物体を把持している状態であるか物体を把持していない状態であるかにかかわらず、ロボットアーム２５１の先端から先を手先という。
【００２０】
各サーボモータ２０１〜２０６は、各関節Ｊ１〜Ｊ６をそれぞれ駆動する電動モータ（モータ）２１１〜２１６と、各電動モータ２１１〜２１６にそれぞれ接続されたセンサ部２２１〜２２６とを有している。各センサ部２２１〜２２６は、各関節Ｊ１〜Ｊ６の位置（角度）を検出する位置センサ（角度センサ）と、各関節Ｊ１〜Ｊ６のトルクを検出するトルクセンサとを有する。また、各サーボモータ２０１〜２０６は、不図示の減速機を有し、直接、又は不図示のベルトやベアリング等の伝達部材を介して各関節Ｊ１〜Ｊ６で駆動されるフレームに接続されている。
【００２１】
ロボットアーム２５１の内部には、各サーボモータ２０１〜２０６の電動モータ２１１〜２１６の駆動を制御する駆動制御部としてのサーボ制御部２３０が配置されている。
【００２２】
サーボ制御部２３０は、入力した各関節Ｊ１〜Ｊ６に対応する各トルク指令値に基づき、各関節Ｊ１〜Ｊ６のトルクがトルク指令値に追従するよう、各電動モータ２１１〜２１６に電流を出力し、各電動モータ２１１〜２１６の駆動を制御する。なお、第１実施形態では、サーボ制御部２３０が１つの制御装置で構成されているものとして説明しているが、各電動モータ２１１〜２１６にそれぞれ対応した複数の制御装置の集合体で構成されていてもよい。また、第１実施形態では、サーボ制御部２３０は、ロボットアーム２５１の内部に配置されているが、ロボット制御装置３００の筐体内部に配置されていてもよい。
【００２３】
次に、ロボット制御装置３００について説明する。図２は、第１実施形態に係るロボット装置のロボット制御装置を示すブロック図である。ロボット制御装置３００は、コンピュータで構成されており、制御部（処理部）としてのＣＰＵ（Central  Processing  Unit）３０１を備えている。また、ロボット制御装置３００は、記憶部として、ＲＯＭ（Read  Only  Memory）３０２、ＲＡＭ（Random  Access  Memory）３０３、ＨＤＤ（Hard  Disk  Drive）３０４を備えている。また、ロボット制御装置３００は、記録ディスクドライブ３０５、各種のインタフェース３０６〜３０９を備えている。
【００２４】
ＣＰＵ３０１には、ＲＯＭ３０２、ＲＡＭ３０３、ＨＤＤ３０４、記録ディスクドライブ３０５、各種のインタフェース３０６〜３０９が、バス３１０を介して接続されている。ＲＯＭ３０２には、ＢＩＯＳ等の基本プログラムが格納されている。ＲＡＭ３０３は、ＣＰＵ３０１の演算処理結果等、各種データを一時的に記憶する記憶装置である。
【００２５】
ＨＤＤ３０４は、ＣＰＵ３０１の演算処理結果や外部から取得した各種データ等を記憶する記憶装置であると共に、ＣＰＵ３０１に、後述する演算処理を実行させるためのプログラム３３０を記録するものである。ＣＰＵ３０１は、ＨＤＤ３０４に記録（格納）されたプログラム３３０に基づいてロボット制御方法の各工程を実行する。
【００２６】
記録ディスクドライブ３０５は、記録ディスク３３１に記録された各種データやプログラム等を読み出すことができる。
【００２７】
ティーチングペンダント４００はインタフェース３０６に接続されている。ＣＰＵ３０１はインタフェース３０６及びバス３１０を介してティーチングペンダント４００からの教示データの入力を受ける。
【００２８】
サーボ制御部２３０は、インタフェース３０９に接続されている。ＣＰＵ３０１は、サーボ制御部２３０、インタフェース３０９及びバス３１０を介して各センサ部２２１〜２２６から検知結果を取得する。また、ＣＰＵ３０１は、各関節のトルク指令値のデータを所定時間間隔でバス３１０及びインタフェース３０９を介してサーボ制御部２３０に出力する。
【００２９】
インタフェース３０７には、モニタ３２１が接続されており、モニタ３２１には、ＣＰＵ３０１の制御の下、各種画像が表示される。インタフェース３０８は、書き換え可能な不揮発性メモリや外付けＨＤＤ等の記憶部である外部記憶装置３２２が接続可能に構成されている。
【００３０】
なお、第１実施形態では、コンピュータ読み取り可能な記録媒体がＨＤＤ３０４であり、ＨＤＤ３０４にプログラム３３０が格納される場合について説明するが、これに限定するものではない。プログラム３３０は、コンピュータ読み取り可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。例えば、プログラム３３０を供給するための記録媒体としては、図２に示すＲＯＭ３０２、記録ディスク３３１、外部記憶装置３２２等を用いてもよい。具体例を挙げて説明すると、記録媒体として、フレキシブルディスク、ハードディスク、光ディスク、光磁気ディスク、ＣＤ−ＲＯＭ、ＣＤ−Ｒ、磁気テープ、不揮発性メモリ、ＲＯＭ等を用いることができる。
【００３１】
図３は、第１実施形態に係るロボット装置の制御系を示す制御ブロック図である。ロボット制御装置３００のＣＰＵ３０１は、プログラム３３０を実行することにより、力検出部５０４、力制御部５０５及び位置目標値生成部５０６として機能する。サーボ制御部２３０は、複数（６関節であるので６つ）の切替制御部５１１〜５１６と、複数（６関節であるので６つ）の位置制御部５２１〜５２６と、複数（６関節であるので６つ）のモータ制御部５３１〜５３６として機能する。
【００３２】
各センサ部２２１〜２２６は、各角度センサ（位置センサ）５５１〜５５６と、各トルクセンサ５４１〜５４６とを有する。各角度センサ５５１〜５５６は、各電動モータ２１１〜２１６又は各関節Ｊ１〜Ｊ６の角度（位置）をそれぞれ検知するものである。
【００３３】
第１実施形態では、各角度センサ５５１〜５５６は、各電動モータ２１１〜２１６の角度θ１〜θ６を直接検知する。なお、各関節Ｊ１〜Ｊ６の角度ｑ１〜ｑ６は、不図示の減速機の減速比等に基づき、各角度θ１〜θ６から求めることができる。したがって、各角度センサ５５１〜５５６は、間接的に各関節Ｊ１〜Ｊ６の角度ｑ１〜ｑ６を検知していることになる。
【００３４】
各トルクセンサ５４１〜５４６は、各関節Ｊ１〜Ｊ６のトルクτ１〜τ６をそれぞれ検知するものである。複数の角度センサ５５１〜５５６でロボット２００の手先の位置Ｐを検知する位置検知部５５０が構成されている。また、複数のトルクセンサ５４１〜５４６でロボット２００の手先に作用する力（手先力）Ｆを検知する力検知部５４０が構成されている。
【００３５】
ティーチングペンダント４００は、操作者の操作により、力教示データ５０１に含まれる力目標値Ｆｒｅｆと、位置教示データ５０２に含まれる位置目標値ＰｒｅｆとをＣＰＵ３０１に出力する。力目標値Ｆｒｅｆは、ロボット２００の手先力の目標値であり、操作者がティーチングペンダント４００を用いて設定する。位置目標値Ｐｒｅｆは、ロボット２００の手先位置の目標値であり、操作者がティーチングペンダント４００を用いて設定する。外部記憶装置３２２にはロボットモデル５０３が記憶されている。
【００３６】
力検出部５０４は、各トルクセンサ５４１〜５４６により検知されたトルクτ１〜τ６及び各角度センサ５５１〜５５６により検知された角度ｑ１〜ｑ６を用いて、ロボット２００の手先の現在位置Ｐ（ｔ）とそのときのロボット２００の手先力Ｆを算出する。
【００３７】
力制御部５０５は、手先負荷モデルを含むロボットモデル５０３、力目標値Ｆｒｅｆ、位置目標値Ｐｒｅｆ、現在位置Ｐ（ｔ）及び力Ｆを入力する。そして、力制御部５０５は、これら値を用いて、各関節Ｊ１〜Ｊ６に対するトルク指令値τＭＦｒｅｆ１〜τＭＦｒｅｆ６を求める。このとき、手先力Ｆと力目標値Ｆｒｅｆとの力偏差、手先の位置Ｐ（ｔ）と位置目標値Ｐｒｅｆとの位置偏差が小さくなるようにトルク指令値τＭＦｒｅｆ１〜τＭＦｒｅｆ６を求める。力制御部５０５は、求めた各トルク指令値τＭＦｒｅｆ１〜τＭＦｒｅｆ６を各切替制御部５１１〜５１６に出力する。
【００３８】
位置目標値生成部５０６は、手先の位置目標値Ｐｒｅｆから逆運動学計算により各関節Ｊ１〜Ｊ６の角度指令値（位置指令値）ｑｒｅｆ１〜ｑｒｅｆ６を求め、各角度指令値ｑｒｅｆ１〜ｑｒｅｆ６を各位置制御部５２１〜５２６に出力する。
【００３９】
各位置制御部５２１〜５２６は、各関節Ｊ１〜Ｊ６の角度ｑ１〜ｑ６と各関節Ｊ１〜Ｊ６の角度指令値ｑｒｅｆ１〜ｑｒｅｆ６との角度偏差が小さくなるようトルク指令値τＭＰｒｅｆ１〜τＭＰｒｅｆ６を求める。なお、この角度偏差を小さくすることは、各電動モータ２１１〜２１６の角度と角度指令値ｑｒｅｆ１〜ｑｒｅｆ６を減速機の減速比等で換算した角度指令値との角度偏差を小さくすることと等価である。各位置制御部５２１〜５２６は、各トルク指令値τＭＰｒｅｆ１〜τＭＰｒｅｆ６を各切替制御部５１１〜５１６に出力する。
【００４０】
各切替制御部５１１〜５１６は、トルクベース力制御を行う力制御モードと、位置制御を行う位置制御モードとを切り替え制御する。各切替制御部５１１〜５１６は、力制御モード時は、各トルク指令値τＭＦｒｅｆ１〜τＭＦｒｅｆ６を各トルク指令値τＭｒｅｆ１〜τＭｒｅｆ６として各モータ制御部５３１〜５３６に出力する。また、各切替制御部５１１〜５１６は、位置制御モード時は、各トルク指令値τＭＰｒｅｆ１〜τＭＰｒｅｆ６を各トルク指令値τＭｒｅｆ１〜τＭｒｅｆ６として各モータ制御部５３１〜５３６に出力する。
【００４１】
各モータ制御部５３１〜５３６は、各電動モータ２１１〜２１６の角度（位置）θ１〜θ６に基づき、各トルク指令値τＭｒｅｆ１〜τＭｒｅｆ６を実現するように各電流Ｃｕｒ１〜Ｃｕｒ６を各電動モータ２１１〜２１６に通電する。
【００４２】
次に、ロボット２００の力制御について説明する。図４は、第１実施形態においてロボット２００を力制御する際のフローチャートである。
【００４３】
まず、操作者が力目標値Ｆｒｅｆと位置目標値Ｐｒｅｆとをティーチングペンダント４００に入力する（Ｓ１）。力目標値Ｆｒｅｆは、力教示データ５０１に格納され、位置目標値Ｐｒｅｆは、位置教示データ５０２に格納される。位置目標値Ｐｒｅｆは、力制御動作を開始する位置である。
【００４４】
力制御部５０５は、ロボット２００の手先にかかる手先力Ｆが力目標値Ｆｒｅｆに倣うようロボットモデル５０３に基づいて各電動モータ２１１〜２１６に対するトルク指令値（力）τＭＦｒｅｆ１〜τＭＦｒｅｆ６を算出する（Ｓ２）。ロボットモデル５０３には、後述する手先負荷モデルが含まれている。
【００４５】
各切替制御部５１１〜５１６は、各モータ制御部５３１〜５３６にトルク指令値（力）τＭＦｒｅｆ１〜τＭＦｒｅｆ６をトルク指令値τＭｒｅｆ１〜τＭｒｅｆ６として出力する（Ｓ３）。
【００４６】
各モータ制御部５３１〜５３６は、各電動モータ２１１〜２１６の角度θ１〜θ６に基づいて、各トルク指令値τＭｒｅｆ１〜τＭｒｅｆ６を実現するよう通電制御する（Ｓ４）。
【００４７】
各電動モータ２１１〜２１６は、通電されることにより各関節トルクτ１〜τ６を発生する（Ｓ５）。
【００４８】
各角度センサ５５１〜５５６は、各関節Ｊ１〜Ｊ６の角度ｑ１〜ｑ６（各電動モータ２１１〜２１６の角度θ１〜θ６）を検知する。各トルクセンサ５４１〜５４６は、各関節Ｊ１〜Ｊ６のトルクτ１〜τ６を検知する（Ｓ６）。各関節Ｊ１〜Ｊ６の角度ｑ１〜ｑ６と各関節Ｊ１〜Ｊ６のトルクτ１〜τ６は、ロボット制御装置３００のＣＰＵ３０１にフィードバックされる。
【００４９】
力検出部５０４（ＣＰＵ３０１）は、ロボットモデル５０３と各関節Ｊ１〜Ｊ６の角度ｑ１〜ｑ６に基づいて各関節Ｊ１〜Ｊ６のトルクτ１〜τ６を、現在の位置Ｐ（ｔ）にいてロボット２００の手先にかかる手先力Ｆに変換する（Ｓ７）。なお、関節Ｊ１〜Ｊ６の角度ｑ１〜ｑ６の代わりに電動モータ２１１〜２１６の角度θ１〜θ６を用いてもよい。
【００５０】
ＣＰＵ３０１は、駆動が終了したか否かを判定し（Ｓ８）、終了していない場合は（Ｓ８：Ｎｏ）、ステップＳ２〜Ｓ７を繰り返す。上記フローに従って各電動モータ２１１〜２１６を駆動することで、ロボット２００の手先にかかる力Ｆを所望の力目標値Ｆｒｅｆに制御することが可能である。なお、図４に示すフローチャートの順番に限定するものではなく、他の順番でも力制御は可能である。
【００５１】
次に、ロボット２００の位置制御について説明する。図５は、第１実施形態においてロボット２００を位置制御する際のフローチャートである。
【００５２】
まず、操作者が位置目標値Ｐｒｅｆをティーチングペンダント４００に入力する（Ｓ１１）。位置目標値Ｐｒｅｆは、位置教示データ５０２に格納される。
【００５３】
位置目標値生成部５０６は、ロボットモデル５０３に基づき、位置目標値Ｐｒｅｆを各関節Ｊ１〜Ｊ６の角度指令値ｑｒｅｆ１〜ｑｒｅｆ６に変換する（Ｓ１２）。
【００５４】
各位置制御部５２１〜５２６は、各関節Ｊ１〜Ｊ６の角度ｑ１〜ｑ６が各関節Ｊ１〜Ｊ６の角度指令値ｑｒｅｆ１〜ｑｒｅｆ６に倣うよう各電動モータ２１１〜２１６のトルク指令値（角度）τＭＰｒｅｆ１〜τＭＰｒｅｆ６を算出する（Ｓ１３）。各関節Ｊ１〜Ｊ６の角度を示す信号としては、角度ｑ１〜ｑ６の代わりに電動モータ２１１〜２１６の角度θ１〜θ６を用いてもよい。
【００５５】
各切替制御部５１１〜５１６は、各モータ制御部５３１〜５３６にトルク指令値（角度）τＭＰｒｅｆ１〜τＭＰｒｅｆ６をトルク指令値τＭｒｅｆ１〜τＭｒｅｆ６として出力する（Ｓ１４）。
【００５６】
なお、ステップＳ１５，Ｓ１６，Ｓ１７，Ｓ１８は、ステップＳ４，Ｓ５，Ｓ６，Ｓ８と同じであり、説明を省略する。上記フローに従って各電動モータ２１１〜２１６を駆動することで、ロボット２００の手先位置Ｐを所望の位置目標値Ｐｒｅｆに倣うよう制御することが可能である。
【００５７】
ステップＳ１〜Ｓ８の力制御とステップＳ１１〜Ｓ１８の位置制御とは、切替制御部５１１〜５１６にて切り替えられ、作業に応じていずれか一方の制御が選択される。
【００５８】
次に、手先負荷モデルの推定について説明する。手先負荷モデルとは、ロボットアーム２５１の先端に取り付けられたエンドエフェクタであるロボットハンド２５２及び把持物体からなる手先負荷の動力学モデルである。動力学モデルとは、対象の質量、重心位置若しくは慣性テンソル、又はこれらの組み合わせである。第１実施形態では、手先負荷モデルは、手先の質量、重心位置及び慣性テンソルである。
【００５９】
第１実施形態では、手先負荷の変動後のロボット２００の手先力Ｆを取得し、その情報（手先力Ｆ）を用いてロボット２００の作業中に手先負荷モデルを推定する。
【００６０】
第１実施形態では、手先負荷モデル（手先負荷の動力学モデル）が変動し、バラつきのあるワーク（第１部品）を搬送して、ワークを治具（第２部品）に組付ける（具体的には、嵌合する）組付工程（嵌合工程）を例に説明する。
【００６１】
組付工程においては組付動作の速度向上が求められるが、手先に所望の力が働くような位置指令値をサーボ制御部２３０に与える位置ベース力制御では制御性能が位置制御の帯域、環境剛性に支配され、高速な動作が難しい。
【００６２】
そこで第１実施形態では、ロボット２００の手先に所望の力が働くような各関節Ｊ１〜Ｊ６のトルク指令値を以下のインピーダンス制御式（１）に従って算出し、サーボ制御部２３０に与えるトルクベース力制御を行う。つまり、図４に示すフローチャートに従って、ロボット２００を力制御する。
【００６３】
【数１】
2017056525.tif 000003
【００６４】
なお、式（１）のＧ、Λ、νはそれぞれ重力負荷トルクベクトル、デカルト空間における慣性行列、デカルト空間におけるコリオリ力・遠心力行列である。これらＧ、Λ、νは、ロボットアーム２５１と手先負荷の動力学モデルおよび各関節Ｊ１〜Ｊ６の位置、速度、加速度から計算される。また、Λｄ、Ｋｄ、Ｄｄはそれぞれ所望の機械インピーダンスの質量、剛性、粘性の値を表わす。Ｊはロボットアーム２５１の各関節Ｊ１〜Ｊ６の速度と、ロボットアーム２５１の根元に固定された座標系Ｔｏ基準で表現されたロボットアーム２５１の手先の速度とを関連付ける６×６のヤコビ行列である。
【００６５】
図６（ａ）は、ロボット２００による組付作業を説明するためのロボット２００の模式図である。ロボットハンド２５２でステージＳｔ１上の第１部品であるワークＷ１をステージＳｔ２上の第２部品である治具Ｗ２に組付ける組付作業を行う場合について説明する。
【００６６】
ここで、ワークＷ１は、ロボットハンド２５２で把持可能な重さ０．５［ｋｇ］の直方体の剛体である。ロボットハンド２５２は、ロボットアーム２５１の先端に取り付けられた２指のグリッパであり、ワークＷ１を把持することができる。また、治具Ｗ２はワークＷ１が嵌る直方体の穴が形成された剛体であり、ステージＳｔ２に固定されている。
【００６７】
図６（ａ）中、教示点Ｐ１は、ステージＳｔ１上のワークＷ１把持前の上空位置の教示点である。教示点Ｐ２は、ワークＷ１の把持を行う位置の教示点である。教示点Ｐ３は、ワークＷ１をステージＳｔ２の上空へ移動した後の位置の教示点である。教示点Ｐ４は、力制御でワークＷ１を治具Ｗ２に組付ける際の位置の教示点である。教示点Ｐ１と教示点Ｐ２及び教示点Ｐ３と教示点Ｐ４は、それぞれ鉛直方向に０．１［ｍ］、教示点Ｐ１と教示点Ｐ３は水平方向に０．２［ｍ］離れた配置とする。
【００６８】
ここで、ロボットアーム２５１及びロボットハンド２５２の動力学モデルを含むロボットモデル５０３（図３）は、既知であるものとする。ロボットモデル５０３は、事前に複数の姿勢における各関節の位置、速度、加速度、トルクの情報からラグランジュの定式化に基づく同定方法で推定される。
【００６９】
ロボットハンド２５２は、今回推定される手先負荷モデルに含まれ、手先負荷モデルの初期値として扱う。第１実施形態ではロボットハンド２５２は、０．５［ｋｇ］とする。
【００７０】
ワーク把持前の手先負荷モデルの負荷値をφａ、ワーク把持後に推定した手先負荷モデルの推定値をφｂとする。図６（ｂ）は手先負荷モデルの負荷値φａを説明するための図であり、図６（ｃ）は手先負荷モデルの負荷値φｂを説明するための図である。
【００７１】
手先負荷モデルの負荷値φａは、ロボットハンド２５２の部分の負荷値であり、手先負荷モデルの負荷値φｂは、ロボットハンド２５２とロボットハンド２５２に把持されたワークＷ１との合計の負荷値である。
【００７２】
図７は、第１実施形態に係る組立部品の製造方法を示すフローチャートである。ＣＰＵ３０１は、ロボットアーム２５１を制御してロボットハンド２５２を教示点Ｐ１の位置から教示点Ｐ２のワークＷ１を把持する把持位置へ移動させる（Ｓ５１）。このとき、ＣＰＵ３０１は、予め設定された、ロボット２００の手先の手先負荷モデルの負荷値φａを用いて、ロボット２００の動作を制御する。
【００７３】
次に、ＣＰＵ３０１は、ロボットハンド２５２にワークＷ１を把持させる（Ｓ５２）。
【００７４】
次に、ＣＰＵ３０１は、ロボットアーム２５１を動作制御して、次の教示点Ｐ３へロボットハンド２５２の移動（第１実施形態では、並進移動）を開始する（Ｓ５３）。このとき、ロボット２００の手先は、ワークＷ１を把持したことにより、負荷変動が生じる。
【００７５】
ＣＰＵ３０１は、手先において負荷変動が生じた際に手先を教示点Ｐ２から教示点Ｐ３へ移動させている状態で、トルクセンサ５４１〜５４６により検知された手先力Ｆを用いて手先負荷モデルの推定値φｂを求める（Ｓ５４：推定処理，推定工程）。
【００７６】
第１実施形態で負荷変動とは、ロボットハンド２５２がワークＷ１を把持（支持）していない状態からワークＷ１を把持（支持）している状態に変化することである。
【００７７】
第１実施形態では、ロボット２００の手先を移動（並進移動）させている状態で力検知部５４０にて検知された手先力を用いて推定値φｂを求める。したがって、力検知部５４０から手先力Ｆのデータを取得する際に、ロボット２００の手先には、並進力が付与されることになる。
【００７８】
このように、ＣＰＵ３０１は、現在の手先力Ｆをもとに手先負荷モデルの推定値φｂを求める。手先力Ｆを検知するタイミングは、第１実施形態のようにワークＷ１を持ち上げるような場合は、ワークＷ１が持ち上がった瞬間、即ちロボット２００の手先に加速度が付与されたときであればよい。第１実施形態では、動作開始後、ワークＷ１が鉛直上向きに０．０１５［ｍ］持ち上がったタイミングで、手先力Ｆの検知を行う。
【００７９】
図８（ａ）は、第１実施形態において時間に対する鉛直方向の手先の目標位置と現在位置を示すグラフである。教示点Ｐ２の位置からワークＷ１が持ち上がったタイミングをｔａで示している。図８（ａ）ではｔａは０．５［秒］である。このとき、ロボットアーム２５１の動作は停止させない。
【００８０】
ステップＳ５４の推定処理について詳細に説明する。まず、ＣＰＵ３０１は、手先力を算出する（Ｓ５４−１）。ここで、時刻ｔａにおける現在の手先力をＦｅｘｔ、各関節Ｊ１〜Ｊ６にかかる外力トルクをτｅｘｔとすると、手先力Ｆｅｘｔは以下の式（２）で計算される。外力トルクとは手先力が働いたときにロボット２００（ロボットアーム２５１）の各関節Ｊ１〜Ｊ６に発生するトルクである。
【００８１】
【数２】
2017056525.tif 000004
【００８２】
式（２）の添字の−Ｔは逆行列の転置を表わす。
【００８３】
また、外力トルクτｅｘｔは、トルクセンサ５４１〜５４６で検出した各関節Ｊ１〜Ｊ６の実トルクτからトルク指令値τｒｅｆを減ずることで計算される。ただし、外力トルクを計算する方法はこの限りでなく、例えば各関節の実トルクτを電動モータ２１１〜２１６の電流値から推定して外力トルクτｅｘｔを計算してもよい。あるいは外乱オブザーバを用いてトルク指令値τｒｅｆとそれによってロボットアーム２５１を駆動させたときの各関節の位置・速度・加速度から外乱トルクを計算してもよい。
【００８４】
続いて、現在の手先力Ｆｅｘｔから手先負荷モデルを算出する方法について説明する。まず、ＣＰＵ３０１は、手先負荷の質量を算出する（Ｓ５４−２）。ロボット座標系ＴｏのＺ軸が鉛直上向きであったとすると、手先負荷の質量ｍｌは手先力ＦｅｘｔのＺ軸方向成分Ｆｚおよび重力加速度ｇから以下の式（３）のように計算できる。
【００８５】
【数３】
2017056525.tif 000005
【００８６】
式（３）のａｚは座標系Ｔｏ基準で表現されたロボット２００の手先の並進方向の加速度ａのＺ軸方向成分である。加速度ａはロボット２００の各軸の速度・加速度から順運動学で算出する。
【００８７】
ＣＰＵ３０１は、手先負荷の重心位置を算出する（Ｓ５４−３）。手先負荷の重心位置は手先力の回転モーメントを用いて以下の式（４）のように計算できる。
【００８８】
【数４】
2017056525.tif 000006
【００８９】
式（４）のｓｌｘ、ｓｌｙ、ｓｌｚはそれぞれ手先負荷の重心位置のＸ軸、Ｙ軸、Ｚ軸方向の位置を意味し、Ｍｘ、Ｍｙ、Ｍｚはそれぞれ手先力ＦｅｘｔのＸ軸、Ｙ軸、Ｚ軸まわりの回転モーメントを意味する。
【００９０】
ＣＰＵ３０１は、手先負荷の慣性テンソルを算出する（Ｓ５４−４）。ここで、手先負荷の慣性テンソルＩｌを以下のように表わすこととし、その求め方を説明する。
【００９１】
【数５】
2017056525.tif 000007
【００９２】
式（５）のＩｘｘ、Ｉｙｙ、Ｉｚｚはそれぞれ手先負荷のＸ軸、Ｙ軸、Ｚ軸まわりの慣性モーメントであり、Ｉｘｙ、Ｉｘｚ、Ｉｙｚは手先負荷の慣性乗積である。手先負荷の慣性モーメントＩｘｘ、Ｉｙｙ、Ｉｚｚはそれぞれ以下の式（６）のように計算できる。
【００９３】
【数６】
2017056525.tif 000008
【００９４】
式（６）のωｘ、ωｙ、ωｚはそれぞれ手先負荷のＸ軸、Ｙ軸、Ｚ軸まわりの角速度であり、その時間微分（角加速度）はロボットアーム２５１の各関節の速度・加速度およびヤコビ行列Ｊから順運動学で算出する。
【００９５】
さらに、手先負荷の力の回転モーメントＭと慣性テンソルＩｌには以下の関係が成り立つ。
【００９６】
【数７】
2017056525.tif 000009
【００９７】
慣性テンソルＩｌの内、慣性モーメントＩｘｘ、Ｉｙｙ、Ｉｚｚは式（６）で計算されているので、式（７）を解いて残りの慣性乗積Ｉｘｙ、Ｉｘｚ、Ｉｙｚを計算し、慣性テンソルが求まる。
【００９８】
ここで、推定値φｂにおける質量は正確に推定され、実際の手先負荷の質量１［ｋｇ］であったとする。
【００９９】
次に、ＣＰＵ３０１は、ステップＳ５４で推定した手先負荷モデルの推定値φｂを実際にロボット２００の制御で用いる手先負荷モデルの負荷値φに反映させる（Ｓ５５）。具体的には、ＣＰＵ３０１は、負荷変動の後の手先負荷モデルの負荷値φ（ｔ）を、負荷変動の前の手先負荷モデルの負荷値φａから経時的に漸次、推定値φｂに向かって変化するように設定する（設定処理，設定工程）。
【０１００】
即ち、第１実施形態では、推定した手先負荷モデルの推定値φｂを一度に反映せず、以下の式（８）のように、ワーク把持後に推定値φｂとワーク把持前の手先負荷モデルの負荷値φａとの差に修正係数ｋを乗じて負荷値φａに加算する。その計算結果を、ワーク把持後の制御で用いる手先負荷モデルの負荷値φとする。
【０１０１】
【数８】
2017056525.tif 000010
【０１０２】
ここで、修正係数ｋは、教示点Ｐ２から教示点Ｐ３への移動量と移動時間から決定される０以上１以下のスカラ量であり、経時的に漸次０から１に単調増加する。手先負荷モデルの負荷値φには、手先の質量、重心位置及び慣性テンソルの各値が含まれている。
【０１０３】
図８（ｂ）は、第１実施形態において時間に対する修正係数ｋ（ｔ）を示すグラフである。図８（ｃ）は、第１実施形態において時間に対する手先負荷モデルの負荷値φ（ｔ）を示すグラフである。第１実施形態では、修正係数ｋ（ｔ）は、図８（ｂ）に示すように、教示点Ｐ２から移動開始して時間ｔａ経過した時点からの経過時間に対し線形に変化するものである。なお、修正係数ｋ（ｔ）の変化時間（勾配）は、トルク指令値に対する手先位置の応答が以下の伝達関数（式（９））で表わされる２次系であるとしたときの整定時間（０．５秒）を目安としている。整定時間は応答が目標値の±５％に収まるまでの時間とした。
【０１０４】
【数９】
2017056525.tif 000011
【０１０５】
式（９）のζは減衰係数、ωは固有角周波数を表わし、それぞれ０．３、２０．０とした。
【０１０６】
次に、ＣＰＵ３０１は、修正係数ｋが１に到達したか否かを判断し（Ｓ５６）、達していなければ（Ｓ５６：Ｎｏ）、修正係数ｋを更新し（Ｓ５７）、ステップＳ５５に戻って推定値φｂを修正係数ｋで修正して制御に用いる手先負荷モデルの負荷値φを求める。手先負荷モデルの負荷値φ（ｔ）は、図８（ｃ）に示すように、負荷変動前の負荷値φａから推定値φｂに経時的に単調増加する。
【０１０７】
ＣＰＵ３０１は、修正係数ｋが１に到達した場合（Ｓ５６：Ｙｅｓ）、手先負荷モデルの負荷値φ（ｔ）が推定値φｂに到達したので、手先負荷モデルに推定値を反映させる反映処理を終了し、教示点Ｐ３にロボット２００の手先を到達させる（Ｓ５８）。
【０１０８】
そして、ＣＰＵ３０１は、教示点Ｐ４に手先を移動させて手先負荷モデルの負荷値φｂに基づく力制御で組付作業を行い（Ｓ５９：組付工程）、処理を終了する。即ち、ステップＳ５９では、ＣＰＵ３０１が手先負荷モデルの負荷値φｂを用いてロボット２００の動作を制御し、ワークＷ１を治具Ｗ２に組付ける。
【０１０９】
なお、第１実施形態では、修正係数ｋのプロファイルを、ロボット２００の手先が教示点Ｐ３に到達すると同時に１となるか、到達する以前に１になる場合について説明したが、修正係数ｋのプロファイルの決定方法はこの限りではない。例えば、手先負荷の変化が大きいことが予想されるような場合は、より安定した手先負荷モデルの推定ができるように手先が教示点Ｐ３に到達した後に修正係数が１になるように決定してもよい。
【０１１０】
以上説明したステップＳ５１〜Ｓ５９の処理を行うことにより、ロボット２００の作業中に手先負荷が急激に変化した際にロボット２００の手先の位置が発散したり振動したりするのを抑制することができる。
【０１１１】
次に、比較例として、手先負荷モデルの負荷値φ（ｔ）を、負荷値φａから推定値φｂにステップ状に変化させた場合について説明する。図９（ａ）は、比較例において時間に対する手先負荷モデルの負荷値φ（ｔ）を示すグラフである。図９（ｂ）は、比較例において時間に対する鉛直方向の手先の目標位置と現在位置を示すグラフである。
【０１１２】
図９（ａ）に示すように、手先負荷モデルの負荷値φ（ｔ）は、ステップ状に変化するため、各電動モータ２１１〜２１６のトルク指令値もステップ状の変化となる。トルク指令値がステップ状に変化すると、各関節のトルクをトルク指令値に追従させる制御を行っているので、各関節のトルクが急激に変化する。よって、ロボットアーム２５１が加振し、図９（ｂ）に示すように、機械的共振周波数で振動する。特に各電動モータ２１１〜２１６が不図示の減速機を介してロボットアーム２５１のリンクを駆動する場合、減速機の剛性は一般的に低いため、各電動モータ２１１〜２１６の急峻な変動により加振されやすい。
【０１１３】
一方、第１実施形態では、図８（ｃ）に示すように、手先負荷モデルの負荷値φ（ｔ）は、修正係数ｋが０から１になる間に、負荷値（初期値）φａから推定値φｂへ徐々に近づいていく。これにより、力制御部５０５で求まるトルク指令値τＭＦｒｅｆ１〜τＭＦｒｅｆ６も徐々に変化することになる。
【０１１４】
よって、第１実施形態では、手先負荷モデルの負荷値φ（ｔ）が時間に対して徐々に推定値φｂに変化していくため、急峻なモデルの変化、即ち各電動モータ２１１〜２１６の急峻なトルク変動を抑制できる。したがって、図８（ａ）に示すように、図９（ｂ）に示すような手先の振動（又は発散）が抑制されている。これにより、ロボット２００の振動や発散に起因してロボット２００又はワークＷ１が周辺の物体に接触するのを防止することができる。
【０１１５】
また、ロボットアーム２５１を動作させながらモデル修正ができるので、シームレスな動作が可能となり、サイクルタイムの短縮につながる。
【０１１６】
なお、第１実施形態では、修正係数ｋが、移動開始からの経過時間に対して変化させる場合について説明したが、教示点Ｐ２からの移動量に対して変化するようにしてもよい。この場合であっても、手先モデルの負荷値φは、初期値φａから推定値φｂへ経時的に漸次変化することとなる。
【０１１７】
さらに、第１実施形態では、各軸の負荷トルクτ１〜τ６を基に手先負荷を推定するため、手先に付加したエンドエフェクタの動力学モデルも合わせて修正することができる。これにより、エンドエフェクタが取り換えられた場合やエンドエフェクタ自体の変形による動力学モデルの変化にも対応することができる。
【０１１８】
［第２実施形態］
次に、第２実施形態に係るロボット装置について説明する。図１０は、第２実施形態に係る組立部品の製造方法を示すフローチャートである。第２実施形態では、第１実施形態の手先負荷モデルの推定手順において、再度、手先を教示点Ｐ１の位置へ移動させて、より正確な手先負荷モデルを算出する手順を追加したものである。ロボット装置の構成及びその他の制御動作については第１実施形態と同様であるので、符号を同じくして詳細な説明は省略する。以下、図１０に示すフローチャートに沿って説明する。
【０１１９】
ＣＰＵ３０１は、現在のロボット２００の手先位置が教示点Ｐ１にあるときの手先力Ｆｅｘｔ＿ａを記憶部（例えばＨＤＤ３０４等）に記憶させる（Ｓ６１）。つまり、記憶部であるＨＤＤ３０４は、ロボットハンド２５２がワークＷ１を支持していない状態で教示点Ｐ１に位置している所定位置姿勢のときに力検知部５４０にて検知された手先力（第１手先力）Ｆｅｘｔ＿ａを記憶する。ここで、記憶部であるＨＤＤ３０４は、ロボットハンド２５２がワークＷ１を把持（支持）していない状態でのロボットハンド２５２の負荷モデル（エンドエフェクタ負荷モデル）の負荷値（第１負荷値、初期値）φａを予め記憶している。
【０１２０】
次に、ＣＰＵ３０１は、ロボットアーム２５１を制御してロボットハンド２５２を教示点Ｐ１の位置から教示点Ｐ２の位置へ移動させる（Ｓ６２）。次に、ＣＰＵ３０１は、ロボットハンド２５２にワークＷ１を把持させる（Ｓ６３）。
【０１２１】
次に、ＣＰＵ３０１は、ロボットアーム２５１を動作制御して、ロボットハンド２５２の教示点Ｐ１への移動を開始する（Ｓ６４）。このとき、ロボット２００の手先は、ワークＷ１を把持したことにより、負荷変動が生じる。
【０１２２】
ＣＰＵ３０１は、手先において負荷変動が生じた際に手先を教示点Ｐ２から教示点Ｐ１へ移動させている状態で、トルクセンサ５４１〜５４６により検知された手先力Ｆを用いて手先負荷モデルの推定値φｂを求める（Ｓ６５：推定処理，推定工程）。第２実施形態で負荷変動とは、ロボットハンド２５２がワークＷ１を把持（支持）していない状態からワークＷ１を把持（支持）している状態に変化することである。このステップＳ６５の推定処理は、第１実施形態で説明したステップＳ５４の推定処理と同じである。
【０１２３】
ここで、説明のため、推定された手先負荷モデル（質量）の推定値φｂには、モデル化誤差が入り、実際の手先負荷の質量１［ｋｇ］から０．５［ｋｇ］ずれた１．５［ｋｇ］であったとする。
【０１２４】
ＣＰＵ３０１は、現在のロボットアーム２５１の各関節の角度ｑ１〜ｑ６から、デカルト空間におけるロボット２００の手先の現在位置Ｐと教示点Ｐ１と間の距離Ｌを算出する（Ｓ６６）。
【０１２５】
ＣＰＵ３０１は、算出した距離Ｌが、予め設定した一定時間、予め設定した閾値Ｌｔｈ以下であるか否かを判断する（Ｓ６７）。
【０１２６】
距離Ｌが一定時間、閾値以下であるということは、ロボット２００の手先が所定位置姿勢（閾値以下の誤差を含む）、第２実施形態では教示点Ｐ１に移動したことを示す。一定時間は例えば０．２［秒］であり、閾値Ｌｔｈは例えば０．０１５［ｍ］とする。
【０１２７】
ＣＰＵ３０１は、一定時間、閾値Ｌｔｈ以下であれば（Ｓ６７：Ｙｅｓ）ステップＳ７１の処理へ進み、そうでなければ（Ｓ６７：Ｎｏ）ステップＳ６８の処理へ進む。
【０１２８】
ステップＳ６８では、ＣＰＵ３０１は、ステップＳ５５と同様、手先負荷モデルの負荷値φ（ｔ）を、負荷変動の前の手先負荷モデルの負荷値φａから経時的に漸次、推定値φｂに向かって変化するように設定する（設定処理，設定工程）。
【０１２９】
ステップＳ６９では、ＣＰＵ３０１は、ステップＳ５６と同様、修正係数ｋが１に到達したか否かを判断する。ＣＰＵ３０１は、修正係数ｋが１に達していれば（Ｓ６９：Ｙｅｓ）、ステップＳ６６に戻って距離Ｌを計算し、修正係数ｋが１に達していなければ（Ｓ６９：Ｎｏ）、修正係数ｋを更新し（Ｓ７０）、ステップＳ６６に戻って距離Ｌを計算する。
【０１３０】
ステップＳ７０では、ＣＰＵ３０１は、ステップＳ６１で記憶した手先力Ｆｅｘｔ＿ａと現在の手先力Ｆｅｘｔ＿ｂの差から、より精密な手先負荷モデルの推定値φｃを計算する（推定処理、推定工程）。
【０１３１】
ＣＰＵ３０１は、手先力Ｆｅｘｔ＿ａと、ロボットハンド２５２がワークＷ１を把持（支持）している状態で教示点Ｐ１に位置しているときに力検知部５４０にて検知された手先力（第２手先力）Ｆｅｘｔ＿ｂとの差ΔＦｅｘｔを計算する。
【０１３２】
ＣＰＵ３０１は、求めた差ΔＦｅｘｔから、ロボットハンド２５２が把持したワークＷ１の負荷モデルである物体負荷モデルの負荷値（第２負荷値）を算出し、第１負荷値φａと第２負荷値とを加算して推定値φｃを算出する。
【０１３３】
ここで、ワークＷ１の質量をｍｗ、重心位置をｓｗとし、手先力Ｆｅｘｔ＿ａとＦｅｘｔ＿ｂの差をΔＦｅｘｔ＝Ｆｅｘｔ＿ｂ−Ｆｅｘｔ＿ａとする。ワークＷ１の質量ｍｗ及び重心位置ｓｗは、第１実施形態で説明したステップＳ５４−２、Ｓ５４−３のＦｅｘｔをΔＦｅｘｔと置き換えて同様の手順で算出できる。
【０１３４】
次に、これらの推定したワークＷ１の質量ｍｗ、重心位置ｓｗを、手先負荷モデル（質量と重心位置）の負荷値φａにそれぞれ加算し、新たな手先負荷モデル（質量と重心位置）の負荷値φｃとする。質量、重心位置以外のモデル情報、具体的には、慣性テンソルは、負荷値φｂのままとする。
【０１３５】
ここで、手先負荷モデルの推定値φｃにおける質量の値は、正確に推定され、実際の手先負荷の質量１［ｋｇ］であったとする。
【０１３６】
ＣＰＵ３０１は、ステップＳ７１で推定した、より精密な手先負荷モデルの推定値φｃを実際にロボット２００の制御で用いる手先負荷モデルの負荷値φに反映させる（Ｓ７２）。処理内容はステップＳ５５の手先負荷モデルの負荷値φａ及び推定値φｂをそれぞれ負荷値φｂ及び推定値φｃで置き換えたものと同じである。具体的には、ＣＰＵ３０１は、負荷変動の後の手先負荷モデルの負荷値φ（ｔ）を、負荷変動の前の手先負荷モデルの負荷値φｂから経時的に漸次、推定値φｃに向かって変化するように設定する（設定処理，設定工程）。
【０１３７】
図１１（ａ）は、第２実施形態において時間に対する修正係数ｋ（ｔ）を示すグラフである。図１１（ｂ）は、第２実施形態において時間に対する手先負荷モデルの負荷値φ（ｔ）を示すグラフである。図１１（ｃ）は、第２実施形態において時間に対する鉛直方向の手先の目標位置と現在位置を示すグラフである。修正係数ｋは図１１（ｂ）のような変化とし、手先負荷モデルの負荷値φの変化は図１１（ｃ）のような変化となる。
【０１３８】
ステップＳ７３〜Ｓ７６の処理は、第１実施形態で説明した図７のステップＳ５６〜Ｓ５９と同じであるため、説明を省略する。
【０１３９】
以上説明したステップＳ７１の処理をＣＰＵ３０１が行うことにより、ワークＷの把持前後の手先力の差から、より正確な手先負荷モデルの推定値φｃを求めることができる。
【０１４０】
図１２は、モデル誤差が含まれる場合の時間に対する鉛直方向の手先の目標位置と現在位置を示すグラフである。図１２において、破線は第１実施形態の手順において、ステップＳ５４で推定した手先負荷モデルの推定値φｂと実際の手先負荷とに誤差があった場合の手先の位置を表わしている。図１２では推定したモデルに誤差が含まれるため、定常偏差が残っていることが分かる。なお、ステップＳ５４で推定した手先負荷モデルφｂの質量は１．５［ｋｇ］で、実際の手先負荷の質量は１［ｋｇ］である。
【０１４１】
これに対し、図１１（ｃ）の破線は、第２実施形態の手順において、教示点Ｐ１でワークＷ１の把持前後の手先力の差から手先負荷モデルを再推定した場合の手先の位置を表わしている。図１１（ｃ）に示すように、図１２に示すような手先の位置の定常偏差が低減されていることが分かる。
【０１４２】
これにより、第２実施形態によれば、手先負荷モデルにおける手先の質量及び重心位置をより正確に求めることができ、モデル誤差をより低減することができる。
【０１４３】
［第３実施形態］
次に、第３実施形態に係るロボット装置について説明する。図１３は、第３実施形態に係る組立部品の製造方法を示すフローチャートである。第３実施形態では第２実施形態の手先負荷モデルの推定手順において、再度手先を教示点Ｐ１の位置へ移動させずに、次の移動先である教示点Ｐ３へ移動させる点が異なる。ロボット装置の構成及びその他の制御動作については第１、第２実施形態と同様であるので、符号を同じくして詳細な説明は省略する。以下、図１３に示すフローチャートに沿って説明する。
【０１４４】
ただし、ステップＳ８１〜Ｓ８３、Ｓ８５、Ｓ８８〜Ｓ９０、Ｓ９３〜Ｓ９７は第２実施形態と同様であるのでこれらの説明は省略し、追加又は変更したステップについてのみ詳細に説明する。
【０１４５】
ＣＰＵ３０１は、ステップＳ８１において、現在のロボット２００の手先位置が教示点Ｐ１にあるときの手先力を記憶部（例えばＨＤＤ３０４等）に記憶させる。つまり、記憶部であるＨＤＤ３０４は、ロボットハンド２５２がワークＷ１を支持していない状態で教示点Ｐ１に位置している第１位置姿勢のときに力検知部５４０にて検知された手先力（第１手先力）を記憶する。ここで、記憶部であるＨＤＤ３０４には、ロボットハンド２５２がワークＷ１を把持していない状態でのロボットハンド２５２の負荷モデル（エンドエフェクタ負荷モデル）の負荷値（第１負荷値、初期値）φａを予め記憶している。
【０１４６】
ＣＰＵ３０１は、ステップＳ８４において、ロボットアーム２５１を動作制御して、教示点Ｐ２の次の教示点Ｐ３へロボットハンド２５２の移動を開始する。
【０１４７】
ＣＰＵ３０１は、ステップＳ８６において、現在のロボットアーム２５１の各関節の角度ｑ１〜ｑ６から、デカルト空間におけるロボット２００の手先の現在位置Ｐと教示点Ｐ３と間の距離Ｌを算出する。
【０１４８】
ＣＰＵ３０１は、ステップＳ８７において、算出した距離Ｌが、予め設定した一定時間、予め設定した閾値Ｌｔｈ以下であるか否かを判断する。距離Ｌが一定時間、閾値以下であるということは、ロボット２００の手先が第２位置姿勢（閾値以下の誤差を含む）、第３実施形態では教示点Ｐ３に移動したことを示す。
【０１４９】
ＣＰＵ３０１は、一定時間、閾値Ｌｔｈ以下であれば（Ｓ８７：Ｙｅｓ）ステップＳ９１の処理へ進み、そうでなければ（Ｓ８７：Ｎｏ）ステップＳ８８の処理へ進む。
【０１５０】
ＣＰＵ３０１は、ステップＳ９１において、ステップＳ８１でＨＤＤ３０４に記憶させた手先力から、ロボットハンド２０２がワークＷ１を支持していない状態で教示点Ｐ３の位置姿勢のときの手先力（第１手先力）を推定する。具体的には、ＣＰＵ３０１は、ステップＳ８１で記憶した教示点Ｐ１での手先力に基づいて、教示点Ｐ３におけるワーク把持前相当の手先力Ｆｅｘｔ３を以下の式（１０）で算出する。
【０１５１】
【数１０】
2017056525.tif 000012
【０１５２】
なお、Ｆｅｘｔ１は教示点Ｐ１におけるワーク把持前の手先力であり、Ｒ３−１は教示点Ｐ３から教示点Ｐ１への回転行列を表わす。ただし、教示点Ｐ３での姿勢が教示点Ｐ１での姿勢と同じであれば回転行列Ｒ３−１は３×３の単位行列となり、式（１０）での計算は不要となる。
【０１５３】
次に、ＣＰＵ３０１は、ステップＳ９１において算出した、教示点Ｐ３におけるワーク把持前相当の手先力と現在の実際の手先力の差から手先負荷モデルを再推定する（Ｓ９２）。
【０１５４】
即ち、ＣＰＵ３０１は、Ｆｅｘｔ３と、ロボットハンド２５２がワークＷ１を把持している状態で教示点Ｐ３の位置姿勢のときに力検知部５４０にて検知された手先力（第２手先力）Ｆｅｘｔ＿ｂとの差ΔＦｅｘｔを計算する。
【０１５５】
ＣＰＵ３０１は、求めた差ΔＦｅｘｔから、ロボットハンド２５２が把持したワークＷ１の負荷モデルである物体負荷モデルの負荷値（第２負荷値）を算出し、第１負荷値φａと第２負荷値とを加算して推定値φｃを算出する。
【０１５６】
以上、ステップＳ９１，Ｓ９２の処理をＣＰＵ３０１が行うことにより、教示点Ｐ３におけるワークＷ１の把持前相当の手先力と現在の実際の手先力の差から、より正確な手先負荷モデルの推定値φｃを求めることができる。
【０１５７】
また、手先負荷モデルの再推定のために再度ロボット２００の手先をワークＷ１の把持前位置に移動させず直接次の移動場所へ移動することができるので、サイクルタイムを増加させることなく作業ができる。
【０１５８】
［第４実施形態］
次に、第４実施形態に係るロボット装置について説明する。図１４（ａ）は、第４実施形態において時間に対する修正係数ｋ（ｔ）を示すグラフである。
【０１５９】
第４実施形態では、修正係数ｋ（ｔ）が経時時間に対し非線形に変化するプロファイルにした点で第１実施形態と異なる。ロボット装置の構成及びその他の制御動作については第１実施形態と同様であるので、符号を同じくして詳細な説明は省略する。
【０１６０】
図１４（ａ）には、修正係数ｋ（ｔ）が５次多項式のプロファイルとなる場合を図示している。修正係数ｋ（ｔ）は、時間で微分可能な時間の関数である。修正係数ｋ（ｔ）は、０．５［秒］から１．０［秒］の間にかけて滑らかに単調増加させている。５次多項式ｋ（ｔ）は以下の式（１１）で与えられる。
【０１６１】
【数１１】
2017056525.tif 000013
【０１６２】
ｋｆはｋ（ｔ）の終点の値、ｔ０、ｔｆはそれぞれ始点、終点の時刻を意味する。
【０１６３】
図１４（ｂ）は、第４実施形態において時間に対する手先負荷モデルの負荷値φ（ｔ）を示すグラフである。図１４（ｃ）は、第４実施形態において時間に対する鉛直方向の手先の目標位置と現在位置を示すグラフである。
【０１６４】
図１４（ｂ）に示すように、手先負荷モデルの負荷値φ（ｔ）は、修正係数ｋ（ｔ）の変化と同様に滑らかに変化していることが分かる。また、図１４（ｃ）を見ると、図１２における手先位置の変化に比べ、より振動が低減されていることが分かる。
【０１６５】
以上説明したように、第４実施形態によれば、修正係数ｋ（ｔ）を滑らかな非線形の関数としたので、ロボット２００の振動がより低減される。なお、微分可能な修正係数ｋ（ｔ）のプロファイルはこの限りでなく、シグモイド関数やスプライン曲線などであってもよい。また、第４実施形態の修正係数を第２又は第３実施形態の修正係数に適用することでより正確かつ安定した手先負荷モデルの推定が図られる。
【０１６６】
［第５実施形態］
次に、第５実施形態に係るロボット装置について説明する。図１５は、第５実施形態に係る組立部品の製造方法を示すフローチャートである。第５実施形態では、手先負荷モデルの推定値を複数回求める点が第１実施形態と異なる。ロボット装置の構成及びその他の制御動作については第１実施形態と同様であるので、符号を同じくして詳細な説明は省略する。以下、図１５に示すフローチャートに沿って説明する。
【０１６７】
第５実施形態では、ステップＳ１０１〜Ｓ１０９の処理内容は第１実施形態で説明した図７のステップＳ５１〜Ｓ５９と同様であるが、第５実施形態ではステップＳ１０７の後にステップＳ１０４へ移る。
【０１６８】
即ち第１実施形態では、ロボット２００の手先の移動開始直後に手先負荷モデルの推定を１度だけ行うのに対し、第５実施形態では修正係数が１に到達するまで複数回行い、手先負荷モデルを収束させる。
【０１６９】
ステップＳ１０４においては、ＣＰＵ３０１は、複数回推定した過去の手先負荷モデルの推定値の平均を計算し、それを次のステップＳ１０５で用いる手先負荷モデルの推定値φｂとする。
【０１７０】
以上、第５実施形態によれば、ＣＰＵ３０１は、推定値の収束計算を行うことにより、精密な手先負荷モデルの推定値φｂが求まる。なお、ステップＳ１０４における複数回推定した過去の手先負荷モデルの平均の計算方法は全体平均、移動平均のどちらでもよい。また、平均ではなく中央値を計算し、それを用いてもよい。
【０１７１】
［第６実施形態］
次に、第６実施形態に係るロボット装置について説明する。図１６は、第６実施形態に係る組立部品の製造方法を示すフローチャートである。第６実施形態では、ロボット２００の手先の移動時に、手先の回転運動を同時に行う点で第１実施形態と異なる。ロボット装置の構成及びその他の制御動作については第１実施形態と同様であるので、符号を同じくして詳細な説明は省略する。以下、図１６に示すフローチャートに沿って説明する。
【０１７２】
第６実施形態では、ステップＳ１１１〜Ｓ１１２，Ｓ１１４〜Ｓ１１９の処理内容は第１実施形態で説明した図７のステップＳ５１〜Ｓ５２，Ｓ５４〜Ｓ５９と同様である。
【０１７３】
ステップＳ１１３では、教示点Ｐ２から教示点Ｐ３へ移動を開始するが、このとき手先に並進力だけでなく回転力が働くように手先をＸＹＺ軸まわりに複数回（３回程度）回転させる。教示点Ｐ３において、元の角度に戻るような回転運動を移動時に追加する。
【０１７４】
このような回転運動を手先に加えることで手先に並進方向の加速度だけでなく、回転方向の角加速度が発生し、手先力の回転成分も発生する。これにより、式（４）〜（７）で計算されるような手先負荷の重心位置や慣性テンソルが収束しやすくなり、より精密な手先負荷モデルの推定が図られる。
【０１７５】
また、第６実施形態ではステップＳ１１７の後に、第５実施形態と同様、ステップＳ１１４へ移る。すなわち、第１実施形態では手先の移動開始直後に手先負荷モデルの推定を１度だけ行うのに対し、第６実施形態では修正係数が１に到達するまで複数回行い、手先負荷モデルを収束させる。
【０１７６】
ステップＳ１１４においては、ＣＰＵ３０１は、複数回推定した過去の手先負荷モデルの推定値の平均を計算し、それを次のステップＳ１１５で用いる手先負荷モデルの推定値φｂとする。
【０１７７】
以上、第６実施形態によれば、ＣＰＵ３０１は、推定値の収束計算を行うことにより、精密な手先負荷モデルの推定値φｂが求まる。なお、ステップＳ１１４における複数回推定した過去の手先負荷モデルの平均の計算方法は全体平均、移動平均のどちらでもよい。また、平均ではなく中央値を計算し、それを用いてもよい。
【０１７８】
なお、第６実施形態では教示点Ｐ２から移動する際に回転運動を追加したものとして説明したが、第２実施形態のように一度教示点Ｐ１へ移動して収束処理をした後に第６実施形態の追加の回転運動を行ってさらにモデルを収束させてもよい。
【０１７９】
本発明は、以上説明した実施形態に限定されるものではなく、本発明の技術的思想内で多くの変形が可能である。また、本発明の実施形態に記載された効果は、本発明から生じる最も好適な効果を列挙したに過ぎず、本発明による効果は、本発明の実施形態に記載されたものに限定されない。
【０１８０】
本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。
【０１８１】
また、上述の実施形態では、ロボットアーム２５１が垂直多関節の場合について説明したが、これに限定するものではない。ロボットアームが、例えば、水平多関節のロボットアーム、パラレルリンクのロボットアーム、直交ロボット等、種々のロボットアームであってもよい。つまり、関節の駆動方向は、回転方向の駆動だけに限らず、直動方向の駆動（伸縮駆動）も含まれる。さらに、関節を駆動するのが電動モータである場合について説明したが、これに限定するものではなく、例えば人工筋肉等であってもよい。また、上述の実施形態では、ロボットアームが６つの関節を有する場合について説明したが、関節の数はこれに限定されるものではない。
【０１８２】
また、上述の実施形態では、力検知部が、複数のトルクセンサで構成される場合について説明したが、これに限定するものではなく、ロボットアームの先端に設けられた力覚センサであってもよい。
【符号の説明】
【０１８３】
１００…ロボット装置、２００…ロボット、２５１…ロボットアーム、２５２…ロボットハンド（エンドエフェクタ）、３０１…ＣＰＵ（制御部）、５４０…力検知部
【図１】
2017056525.tif 000015
【図２】
2017056525.tif 000016
【図３】
2017056525.tif 000017
【図４】
2017056525.tif 000018
【図５】
2017056525.tif 000019
【図６】
2017056525.tif 000020
【図７】
2017056525.tif 000021
【図８】
2017056525.tif 000022
【図９】
2017056525.tif 000023
【図１０】
2017056525.tif 000024
【図１１】
2017056525.tif 000025
【図１２】
2017056525.tif 000026
【図１３】
2017056525.tif 000027
【図１４】
2017056525.tif 000028
【図１５】
2017056525.tif 000029
【図１６】
2017056525.tif 000030