(57)【要約】【課題】マニピュレータの制御性を向上させる。【解決手段】補正値計算部６０４は、マニピュレータの位置制御中に、マニピュレータに生じる負荷の測定値τ１〜τ６を取得する。動力学計算部６０２は、ロボットモデル５０３を用いた動力学の計算により、マニピュレータに生じる負荷の計算値τ１＾〜τ６＾を求める。補正値計算部６０４は、測定値τ１〜τ６と計算値τ１＾〜τ６＾との差に基づいて補正値τｃｏｍｐ１〜τｃｏｍｐ６を求めておく。インピーダンス制御部６０１は、マニピュレータの推力を制御する際に、教示データに基づくトルク指令値τＦｄ１〜τＦｄ６を求める。指令値補正部６０３は、トルク指令値τＦｄ１〜τＦｄ６を補正値τｃｏｍｐ１〜τｃｏｍｐ６で補正することで指令値τｒｅｆ１〜τｒｅｆ６を求める。第２制御ユニットは、指令値τｒｅｆ１〜τｒｅｆ６と測定値τ１〜τ６との差に基づき、マニピュレータの推力を制御する。【選択図】図７
2020151824.tif 000002
【特許請求の範囲】
【請求項１】
マニピュレータの位置又は推力を制御する制御部を備え、
前記制御部は、
前記マニピュレータの位置を制御しているときに、前記マニピュレータに生じる負荷の第１測定値を取得し、前記マニピュレータのモデルデータを用いた動力学の計算により、前記マニピュレータに生じる負荷の計算値を求め、前記第１測定値と前記計算値との差に基づいて補正値を求めておき、
前記マニピュレータの推力を制御する際に、教示データに基づく仮の指令値を求め、前記仮の指令値を前記補正値で補正することで指令値を求め、前記マニピュレータに生じる負荷の第２測定値を取得し、前記指令値と前記第２測定値との差に基づき、前記マニピュレータの推力を制御することを特徴とする制御装置。
【請求項２】
前記制御部は、前記マニピュレータの推力として、前記マニピュレータの関節の出力トルクを制御することを特徴とする請求項１に記載の制御装置。
【請求項３】
前記制御部は、前記マニピュレータを目標位置に位置決めした際に前記第１測定値を取得して、前記補正値を求めることを特徴とする請求項１又は２に記載の制御装置。
【請求項４】
前記制御部は、前記マニピュレータの位置として、前記マニピュレータの先端部の位置を制御することを特徴とする請求項１乃至３のいずれか１項に記載の制御装置。
【請求項５】
前記制御部は、
前記指令値を出力する第１制御ユニットと、
前記第１制御ユニットから前記指令値を取得し、前記指令値と前記第２測定値との差に基づき、前記マニピュレータの推力を制御する第２制御ユニットと、を含むことを特徴とする請求項１乃至４のいずれか１項に記載の制御装置。
【請求項６】
マニピュレータの位置又は推力を制御する制御部を備え、
前記制御部は、
前記マニピュレータの位置を制御しているときに前記マニピュレータに生じる負荷の第１測定値に基づくオフセット値を求めておき、
前記マニピュレータの推力を制御する際に、教示データに基づいて指令値を求め、前記マニピュレータに生じる負荷の第２測定値を取得し、前記指令値と前記第２測定値との差に基づいてフィードバック値を求め、前記フィードバック値に、前記オフセット値と、前記指令値に応じたフィードフォワード値と、を加算した値に基づいて、前記マニピュレータの推力を制御することを特徴とする制御装置。
【請求項７】
前記制御部は、前記マニピュレータのモデルデータを用いた動力学の計算により、前記指令値に応じた、前記マニピュレータに生じる負荷の計算値を求め、前記計算値から基準値を引いた値を、前記フィードフォワード値とすることを特徴とする請求項６に記載の制御装置。
【請求項８】
前記計算値が第１計算値であり、前記指令値が第１指令値であり、
前記基準値は、前記第１計算値よりも前に、前記第１指令値よりも前の第２指令値に応じて求められた、前記マニピュレータに生じる負荷の第２計算値であることを特徴とする請求項７に記載の制御装置。
【請求項９】
前記第２指令値は、前記マニピュレータの推力の制御を開始したときに求められることを特徴とする請求項８に記載の制御装置。
【請求項１０】
前記制御部は、
前記指令値を出力する第１制御ユニットと、
前記第１制御ユニットから前記指令値を取得して前記マニピュレータの推力を制御する第２制御ユニットと、を含むことを特徴とする請求項６乃至９のいずれか１項に記載の制御装置。
【請求項１１】
第１制御ユニットと、
前記第１制御ユニットの指令に基づいてマニピュレータの位置又は推力を制御する第２制御ユニットと、を備え、
前記第１制御ユニットは、
前記第２制御ユニットが前記マニピュレータの位置を制御しているときに、前記マニピュレータに生じる負荷の第１測定値を取得し、前記マニピュレータのモデルデータを用いた動力学の計算により、前記マニピュレータに生じる負荷の計算値を求め、前記第１測定値と前記計算値との差に基づいて補正値を求めておき、
前記第２制御ユニットに前記マニピュレータの推力を制御させる際に、教示データに基づく仮の指令値を求め、前記仮の指令値を前記補正値で補正することで指令値を求めて、前記第２制御ユニットに前記指令値を出力し、
前記第２制御ユニットは、
前記マニピュレータの位置を制御しているときに前記第１測定値に基づくオフセット値を求めておき、
前記マニピュレータの推力を制御する際に、前記マニピュレータに生じる負荷の第２測定値を取得し、前記指令値と前記第２測定値との差に基づいてフィードバック値を求め、前記フィードバック値に、前記オフセット値と、前記指令値に応じたフィードフォワード値と、を加算した値に基づいて前記マニピュレータの推力を制御することを特徴とする制御装置。
【請求項１２】
請求項１乃至１１のいずれか１項に記載の制御装置と、
前記マニピュレータと、を備えるロボット装置。
【請求項１３】
制御部が、マニピュレータの位置又は推力を制御する制御方法であって、
前記制御部が、前記マニピュレータの位置を制御しているときに、前記マニピュレータに生じる負荷の第１測定値を取得し、前記マニピュレータのモデルデータを用いた動力学の計算により、前記マニピュレータに生じる負荷の計算値を求め、前記測定値と前記計算値との差に基づいて補正値を求めておき、
前記制御部が、前記マニピュレータの推力を制御する際に、教示データに基づく仮の指令値を求め、前記仮の指令値を前記補正値で補正することで指令値を求め、前記マニピュレータに生じる負荷の第２測定値を取得し、前記指令値と前記第２測定値との差に基づき、前記マニピュレータの推力を制御することを特徴とする制御方法。
【請求項１４】
制御部が、マニピュレータの位置又は推力を制御する制御方法であって、
前記制御部が、
前記マニピュレータの位置を制御しているときに前記マニピュレータに生じる負荷の第１測定値に基づくオフセット値を求めておき、
前記マニピュレータの推力を制御する際に、教示データに基づいて指令値を求め、前記マニピュレータに生じる負荷の第２測定値を取得し、前記指令値と前記第２測定値との差に基づいてフィードバック値を求め、前記フィードバック値に、前記オフセット値と、前記指令値に応じたフィードフォワード値と、を加算した値に基づいて、前記マニピュレータの推力を制御することを特徴とする制御方法。
【請求項１５】
第１制御ユニットと、前記第１制御ユニットの指令に基づいてマニピュレータの位置又は推力を制御する第２制御ユニットとによる制御方法であって、
前記第１制御ユニットが、
前記第２制御ユニットが前記マニピュレータの位置を制御しているときに、前記マニピュレータに生じる負荷の第１測定値を取得し、前記マニピュレータのモデルデータを用いた動力学の計算により、前記マニピュレータに生じる負荷の計算値を求め、前記第１測定値と前記計算値との差に基づいて補正値を求めておき、
前記第２制御ユニットに前記マニピュレータの推力を制御させる際に、教示データに基づく仮の指令値を求め、前記仮の指令値を前記補正値で補正することで指令値を求めて、前記第２制御ユニットに前記指令値を出力し、
前記第２制御ユニットが、
前記マニピュレータの位置を制御しているときに前記第１測定値に基づくオフセット値を求めておき、
前記マニピュレータの推力を制御する際に、前記マニピュレータに生じる負荷の第２測定値を取得し、前記指令値と前記第２測定値との差に基づいてフィードバック値を求め、前記フィードバック値に、前記オフセット値と、前記指令値に応じたフィードフォワード値と、を加算した値に基づいて前記マニピュレータの推力を制御することを特徴とする制御方法。
【請求項１６】
請求項１３乃至１５のいずれか１項に記載の制御方法により前記マニピュレータを制御して、物品を製造する物品の製造方法。
【請求項１７】
請求項１３乃至１５のいずれか１項に記載の制御方法をコンピュータに実行させるためのプログラム。
【請求項１８】
請求項１７に記載のプログラムを記録した、コンピュータにより読取可能な記録媒体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、マニピュレータの制御に関する。
【背景技術】
【０００２】
産業用ロボットにおけるマニピュレータの制御として、マニピュレータの位置を制御する位置制御と、マニピュレータの推力を制御する力制御とが知られている。例えば、物品の一例として組立品を製造する際、マニピュレータの先端部にワークを保持させておき、位置制御によりマニピュレータの先端部を目標位置に位置決めした後、力制御によりワークを組み付け対象物に組み付けることが行われている。特許文献１には、マニピュレータの力制御として、トルク指令値と測定されたトルク値との差に基づいてマニピュレータの関節をフィードバック制御する技術が開示されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１７−５６５４９号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかし、マニピュレータを力制御しているときにマニピュレータが振動するなど、マニピュレータの挙動が不安定になることがあった。マニピュレータに精密な作業を行わせる場合、マニピュレータの挙動が不安定になると精密な作業に支障をきたすことがあるため、改善が求められていた。
【０００５】
本発明は、マニピュレータの制御性を向上させることを目的とする。
【課題を解決するための手段】
【０００６】
本発明の制御装置は、マニピュレータの位置又は推力を制御する制御部を備え、前記制御部は、前記マニピュレータの位置を制御しているときに、前記マニピュレータに生じる負荷の第１測定値を取得し、前記マニピュレータのモデルデータを用いた動力学の計算により、前記マニピュレータに生じる負荷の計算値を求め、前記第１測定値と前記計算値との差に基づいて補正値を求めておき、前記マニピュレータの推力を制御する際に、教示データに基づく仮の指令値を求め、前記仮の指令値を前記補正値で補正することで指令値を求め、前記マニピュレータに生じる負荷の第２測定値を取得し、前記指令値と前記第２測定値との差に基づき、前記マニピュレータの推力を制御することを特徴とする。
【発明の効果】
【０００７】
本発明によれば、マニピュレータの制御性が向上する。
【図面の簡単な説明】
【０００８】
【図１】第１実施形態に係るロボット装置の概略構成を示す斜視図である。
【図２】第１実施形態に係る制御装置の構成を示すブロック図である。
【図３】第１実施形態に係る制御装置の構成を示すブロック図である。
【図４】第１実施形態に係るロボット装置の制御システムを示す制御ブロック図である。
【図５】第１実施形態における制御装置の処理のフローチャートである。
【図６】第１実施形態における制御装置の処理のフローチャートである。
【図７】第１実施形態に係る第１制御ユニットのブロック図である。
【図８】第１実施形態におけるロボットアームの概略図である。
【図９】（ａ）は、第１実施形態における現実のマニピュレータの動力学パラメータの一例を示す図である。（ｂ）は、第１実施形態におけるロボットモデルの動力学パラメータの一例を示す図である。
【図１０】（ａ）及び（ｂ）は、第１実施形態におけるマニピュレータによる組付作業を説明するためのマニピュレータの模式図である。
【図１１】（ａ）は、第１実施形態における教示点のパラメータを示す図である。（ｂ）は、第１実施形態における力制御の機械インピーダンスのパラメータを示す図である。
【図１２】第１実施形態に係る物品の製造方法を説明するフローチャートである。
【図１３】（ａ）及び（ｂ）は、第１実施形態における実験結果を示すグラフである。（ｃ）は、比較例における実験結果を示すグラフである。
【図１４】第２実施形態に係るロボット装置の概略構成を示す斜視図である。
【図１５】第２実施形態に係る制御装置の構成を示すブロック図である。
【図１６】第２実施形態に係る制御装置の構成を示すブロック図である。
【図１７】第２実施形態に係るロボット装置の制御システムを示す制御ブロック図である。
【図１８】第２実施形態に係る第２制御ユニットのブロック図である。
【図１９】第２実施形態に係る物品の製造方法を説明するフローチャートである。
【図２０】第２実施形態における実験結果を示すグラフである。
【図２１】第３実施形態に係る第２制御ユニットを示すブロック図である。
【図２２】第３実施形態における実験結果を示すグラフである。
【図２３】第４実施形態に係るロボット装置の概略構成を示す斜視図である。
【発明を実施するための形態】
【０００９】
以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。
［第１実施形態］
図１は、第１実施形態に係るロボット装置の概略構成を示す斜視図である。図１に示すように、ロボット装置１００は、マニピュレータ２００と、マニピュレータ２００の位置又は推力を制御する制御部５００を有する制御装置１０００と、を備えている。また、ロボット装置１００は、制御装置１０００に教示データを送信する教示装置としての教示ペンダント６００を備えている。教示ペンダント６００は、操作者が操作するものであり、マニピュレータ２００や制御装置１０００の動作を指定するのに用いる。
【００１０】
マニピュレータ２００は、ロボットアーム２５１と、マニピュレータ２００の先端部を構成する、エンドエフェクタの一例であるハンド２５２と、を備える。ロボットアーム２５１は、本実施形態では垂直多関節のロボットアームである。
【００１１】
ロボットアーム２５１の先端部には、ハンド２５２が取り付けられている。以下、エンドエフェクタがハンド２５２である場合について説明するが、これに限定するものではなく、ツール等であってもよい。ロボットアーム２５１の基端部は、台座Ｂに固定されている。ハンド２５２は、部品やツール等のワークを把持するものである。
【００１２】
マニピュレータ２００、即ちロボットアーム２５１は、１つ以上の関節を有していればよく、本実施形態では、複数の関節、例えば６つの関節Ｊ１〜Ｊ６を有している。ロボットアーム２５１は、各関節Ｊ１〜Ｊ６を各軸Ａ１〜Ａ６まわりに回転駆動するための複数（６つ）のサーボ機構部２０１〜２０６を有している。
【００１３】
なお、本実施形態では、関節Ｊ１〜Ｊ６は、回転関節であるが、直動関節であってもよい。関節が回転関節の場合、関節の位置とは、関節の角度、即ち回転位置を表す。関節が直動関節の場合、関節の位置とは、直動位置を表す。また、関節が回転関節の場合、関節の推力とは、関節の推進トルク、即ち出力トルクを表す。関節が直動関節の場合、関節の推力とは、関節の直動推力を表す。
【００１４】
ロボットアーム２５１は、複数のリンク２１００〜２１０６を有する。リンク２１００〜２１０６は、関節Ｊ１〜Ｊ６で回転可能に連結されている。リンク２１００〜２１０６は、基端側から先端側に向かって、順に直列に連結されている。ロボットアーム２５１は、マニピュレータ２００の先端部、即ちハンド２５２を、可動範囲内の任意の位置に移動させることができる。
【００１５】
マニピュレータ２００の位置、即ち姿勢は、座標系Ｔｏで表現することができる。座標系Ｔｏは、ロボットアーム２５１の基端、即ち台座Ｂに固定した座標系を表し、座標系Ｔｅはマニピュレータ２００の先端部、即ちハンド２５２と連動する座標系を表す。
【００１６】
制御部５００は、第１制御ユニットである制御ユニット３００と、第２制御ユニットである制御ユニット４００と、を有する。制御ユニット３００は、ロボット装置１００を統括的に制御するものである。制御ユニット４００は、制御ユニット３００の指令に基づいて、マニピュレータ２００の位置又は推力を制御するものである。制御ユニット４００は、マニピュレータ２００の各サーボ機構部２０１〜２０６を制御するサーボ制御装置である。
【００１７】
制御部５００について具体的に説明する。図２は、第１実施形態に係る制御装置の構成を示すブロック図である。制御部５００は、上述したように、制御ユニット３００，４００を有する。図２には、制御ユニット３００のハードウェア構成について詳細に図示している。
【００１８】
制御ユニット３００は、コンピュータで構成されており、処理部としてのＣＰＵ（Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）３０１を備える。また、制御ユニット３００は、記憶部として、ＲＯＭ（Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ）３０２、ＲＡＭ（Ｒａｎｄｏｍ  Ａｃｃｅｓｓ  Ｍｅｍｏｒｙ）３０３、ＨＤＤ（Ｈａｒｄ  Ｄｉｓｋ  Ｄｒｉｖｅ）３０４を備えている。また、制御ユニット３００は、記録ディスクドライブ３０５、及びインタフェース３０６〜３０９を備えている。
【００１９】
ＣＰＵ３０１、ＲＯＭ３０２、ＲＡＭ３０３、ＨＤＤ３０４、記録ディスクドライブ３０５、インタフェース３０６〜３０９は、相互に通信可能にバス３１０で接続されている。ＲＯＭ３０２には、ＣＰＵ３０１に後述する制御方法の処理を実行させるためのプログラム３３０が格納されている。ＣＰＵ３０１は、ＲＯＭ３０２に記録（格納）されたプログラム３３０に基づいて、後述する制御方法を実行する。ＲＡＭ３０３は、ＣＰＵ３０１の演算処理結果等、各種データを一時的に記憶する記憶装置である。
【００２０】
ＨＤＤ３０４は、ＣＰＵ３０１の演算処理結果や外部から取得した各種データ等を記憶可能な記憶装置である。記録ディスクドライブ３０５は、記録ディスク３３１に記録された各種データやプログラム等を読み出すことができる。
【００２１】
教示ペンダント６００は、インタフェース３０６に接続されている。ＣＰＵ３０１は、インタフェース３０６及びバス３１０を介して教示ペンダント６００からマニピュレータ２００の教示データを取得する。
【００２２】
制御ユニット４００は、インタフェース３０９に接続されている。ＣＰＵ３０１は、指令値のデータを所定の制御周期でバス３１０及びインタフェース３０９を介して制御ユニット４００に出力する。
【００２３】
インタフェース３０７には、モニタ３２１が接続されており、モニタ３２１には、ＣＰＵ３０１の制御の下、各種画像が表示される。インタフェース３０８は、書き換え可能な不揮発性メモリや外付けＨＤＤ等の記憶装置である外部記憶装置３２２が接続可能に構成されている。外部記憶装置３２２には、教示データや、マニピュレータ２００のモデルデータが格納可能である。
【００２４】
なお、第１実施形態では、コンピュータによって読取可能な記録媒体がＲＯＭ３０２であり、ＲＯＭ３０２にプログラム３３０が記録されているが、これに限定するものではない。プログラム３３０は、コンピュータによって読取可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。プログラム３３０を供給するための記録媒体としては、例えば、フレキシブルディスク、ハードディスク、光ディスク、光磁気ディスク、磁気テープ、不揮発性メモリ等を用いることができる。
【００２５】
図３は、第１実施形態に係る制御装置の構成を示すブロック図である。図３には、制御ユニット４００のハードウェア構成について詳細に図示している。
【００２６】
制御ユニット４００は、コンピュータで構成されている。制御ユニット４００は、処理部であるＣＰＵ４０１と、記憶部であるＲＯＭ４０２及びＲＡＭ４０３と、インタフェース４０６〜４０９と、を備えている。ＣＰＵ４０１、ＲＯＭ４０２、ＲＡＭ４０３、インタフェース４０６〜４０９は、相互に通信可能にバス４１０で接続されている。ＲＯＭ４０２には、ＣＰＵ４０１に、後述する制御方法の処理を実行させるためのプログラム４３０が格納されている。
【００２７】
インタフェース４０６には、後述するモータ２１１〜２１６が接続されている。インタフェース４０７には、後述する位置センサ２３１〜２３６が接続されている。インタフェース４０８には、後述するトルクセンサ２４１〜２４６が接続されている。インタフェース４０９には、制御ユニット３００が接続されている。
【００２８】
なお、第１実施形態では、コンピュータによって読取可能な記録媒体がＲＯＭ４０２であり、ＲＯＭ４０２にプログラム４３０が記録されているが、これに限定するものではない。プログラム４３０は、コンピュータによって読取可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。プログラム４３０を供給するための記録媒体としては、例えば、フレキシブルディスク、ハードディスク、光ディスク、光磁気ディスク、磁気テープ、不揮発性メモリ等を用いることができる。
【００２９】
図４は、第１実施形態に係るロボット装置の制御システムを示す制御ブロック図である。図２に示す制御ユニット３００のＣＰＵ３０１は、プログラム３３０を実行することにより、図４に示すトルク指令値生成部５０４及び位置指令値生成部５０５として機能する。図３に示す制御ユニット４００のＣＰＵ４０１は、プログラム４３０を実行することにより、図４に示すトルク制御部５１１〜５１６、位置制御部５２１〜５２６、切替部５３１〜５３６、及びモータ制御部５４１〜５４６として機能する。サーボ機構部２０１〜２０６は、モータ２１１〜２１６、減速機２２１〜２２６、位置センサ２３１〜２３６、及びトルクセンサ２４１〜２４６を有する。
【００３０】
各モータ２１１〜２１６は、マニピュレータ２００の各関節Ｊ１〜Ｊ６を駆動する駆動源の一例であり、例えば電動モータである。各減速機２２１〜２２６は、各モータ２１１〜２１６の回転軸の回転を減速して出力するものであり、例えば波動歯車減速機である。各関節Ｊ１〜Ｊ６で連結された一対のリンクのうち、一方のリンクに対して他方のリンクを、各モータ２１１〜２１６、及び各減速機２２１〜２２６によって駆動することができる。なお、モータの回転軸が減速機の入力軸に直接連結されていてもよいが、モータと減速機との間に、ベルト機構などの不図示の伝達機構が配置されていてもよい。
【００３１】
各位置センサ２３１〜２３６は、各関節Ｊ１〜Ｊ６の位置（角度）に応じた信号を出力するセンサであり、例えばエンコーダである。エンコーダは、ロータリエンコーダであるのが好ましい。各位置センサ２３１〜２３６は、各減速機２２１〜２２６の入力軸側、即ち各モータ２１１〜２１６に設けられ、各モータ２１１〜２１６の回転軸の位置（角度）に応じた信号を、制御ユニット４００に出力する。制御ユニット４００は、各位置センサ２３１〜２３６から取得した信号に基づき、各モータ２１１〜２１６の回転軸の位置（角度）の測定値θ１〜θ６を求め、各測定値θ１〜θ６から各関節Ｊ１〜Ｊ６の位置（角度）の測定値ｑ１〜ｑ６を求める。本実施形態では、減速機２２１〜２２６の減速比に基づき、測定値θ１〜θ６を測定値ｑ１〜ｑ６に変換する。制御ユニット４００は、各関節Ｊ１〜Ｊ６の位置（角度）の測定値ｑ１〜ｑ６を所定の制御周期で制御ユニット３００に出力する。
【００３２】
各トルクセンサ２４１〜２４６は、マニピュレータ２００に作用する負荷の一例として各関節Ｊ１〜Ｊ６に作用する負荷トルクに応じた信号を、制御ユニット４００に出力する。制御ユニット４００は、各トルクセンサ２４１〜２４６から取得した信号に基づき、各関節Ｊ１〜Ｊ６にかかる負荷トルクの測定値τ１〜τ６を求める。制御ユニット４００は、各関節Ｊ１〜Ｊ６の負荷トルクの測定値τ１〜τ６を所定の制御周期で制御ユニット３００に出力する。
【００３３】
外部記憶装置３２２には、ユーザが予め作成しておいた教示データとしての力教示データ５０１及び位置教示データ５０２と、マニピュレータ２００のモデルデータであるロボットモデル５０３と、が格納されている。なお、これらデータが格納される記憶装置は、外部記憶装置３２２に限定するものではない。例えば、これらデータは、ＨＤＤ３０４、又は教示ペンダント６００に含まれる不図示の記憶装置に格納されていてもよい。
【００３４】
力教示データ５０１には、目標力Ｆｄが含まれている。位置教示データ５０２には、目標位置Ｐｄが含まれている。目標力Ｆｄは、マニピュレータ２００のハンド２５２にかかる力の目標値（目標力）であり、操作者が教示ペンダント６００を用いて設定することができる。目標位置Ｐｄは、マニピュレータ２００のハンド２５２の位置の目標値（目標位置）であり、操作者が教示ペンダント６００を用いて設定することができる。制御ユニット３００は、力教示データ５０１から目標力Ｆｄを読み出すことができ、位置教示データ５０２から目標位置Ｐｄを読み出すことができる。
【００３５】
ロボットモデル５０３は、マニピュレータ２００を構成する要素の質量データ、重心位置データ、重心まわりの慣性モーメントデータなどの設計データを含む、ユーザが作成した、仮想的なモデルである。
【００３６】
制御ユニット３００は、予め設定されているロボットプログラムに従って、マニピュレータ２００の位置を制御する位置制御モードと、マニピュレータ２００の推力を制御する力制御モードとを切り替えて、制御ユニット４００に指令を送る。即ち、制御ユニット３００は、位置制御モードに切り替わっているときには位置指令値生成部５０５として機能して位置指令値を送り、力制御モードに切り替わっているときには、トルク指令値生成部５０４として機能してトルク指令値を送る。
【００３７】
位置指令値生成部５０５は、目標位置Ｐｄに基づいて、関節Ｊ１〜Ｊ６に対応する位置指令値ｑｒｅｆ１〜ｑｒｅｆ６を生成する。具体的には、位置指令値生成部５０５は、ロボットモデル５０３を用いた逆運動学計算により、目標位置Ｐｄを関節Ｊ１〜Ｊ６に対応する位置指令値ｑｒｅｆ１〜ｑｒｅｆ６に変換する。位置指令値生成部５０５は、生成した位置指令値ｑｒｅｆ１〜ｑｒｅｆ６を、制御ユニット４００の位置制御部５２１〜５２６に出力する。位置制御部５２１〜５２６は、測定値ｑ１〜ｑ６と位置指令値ｑｒｅｆ１〜ｑｒｅｆ６との差に基づくフィードバック演算、例えばＰＩＤ演算により、トルク指令値τｑ１〜τｑ６を生成する。位置制御部５２１〜５２６は、トルク指令値τｑ１〜τｑ６を切替部５３１〜５３６に出力する。
【００３８】
トルク指令値生成部５０４は、目標力Ｆｄ、目標位置Ｐｄ、測定値τ１〜τ６、及び測定値ｑ１〜ｑ６に基づいて、関節Ｊ１〜Ｊ６に対応するトルク指令値τｒｅｆ１〜τｒｅｆ６を生成する。トルク指令値生成部５０４の具体的な処理は、後述する。トルク指令値生成部５０４は、生成したトルク指令値τｒｅｆ１〜τｒｅｆ６を、制御ユニット４００のトルク制御部５１１〜５１６に出力する。トルク制御部５１１〜５１６は、負荷トルクの測定値τ１〜τ６とトルク指令値τｒｅｆ１〜τｒｅｆ６との差に基づくフィードバック演算、例えばＰＩＤ演算により、トルク指令値τＦ１〜τＦ６を生成する。トルク制御部５１１〜５１６は、トルク指令値τＦ１〜τＦ６を切替部５３１〜５３６に出力する。
【００３９】
切替部５３１〜５３６は、トルク指令値τＦ１〜τＦ６の入力を受けた場合は、入力を受けたトルク指令値τＦ１〜τＦ６をトルク指令値τＭ１〜τＭ６としてモータ制御部５４１〜５４６に出力する。また、切替部５３１〜５３６は、トルク指令値τｑ１〜τｑ６の入力を受けた場合は、入力を受けたトルク指令値τｑ１〜τｑ６をトルク指令値τＭ１〜τＭ６としてモータ制御部５４１〜５４６に出力する。
【００４０】
モータ制御部５４１〜５４６は、トルク指令値τＭ１〜τＭ６に応じた電流Ｃｕｒ１〜Ｃｕｒ６をモータ２１１〜２１６に通電する。
【００４１】
以上、制御ユニット４００は、制御ユニット３００から位置指令値ｑｒｅｆ１〜ｑｒｅｆ６を取得した場合、位置指令値ｑｒｅｆ１〜ｑｒｅｆ６と測定値ｑ１〜ｑ６との差に基づき、マニピュレータ２００の位置を制御する。また制御ユニット４００は、制御ユニット３００からトルク指令値τｒｅｆ１〜τｒｅｆ６を取得した場合、トルク指令値τｒｅｆ１〜τｒｅｆ６と測定値τ１〜τ６との差に基づき、マニピュレータ２００の推力、即ち関節Ｊ１〜Ｊ６の出力トルクを制御する。
【００４２】
力制御モードにおけるマニピュレータ２００の力制御、即ちマニピュレータ２００の関節のトルク制御について説明する。図５は、第１実施形態において制御部５００がマニピュレータ２００を力制御する処理のフローチャートである。まず、トルク指令値生成部５０４は、目標力Ｆｄ及び目標位置Ｐｄを外部記憶装置３２２から取得する（Ｓ１）。トルク指令値生成部５０４は、目標力Ｆｄ、目標位置Ｐｄ、負荷トルクの測定値τ１〜τ６、位置の測定値ｑ１〜ｑ６に基づき、ロボットモデル５０３を用いてトルク指令値τｒｅｆ１〜τｒｅｆ６を算出する（Ｓ２）。トルク制御部５１１〜５１６は、トルク指令値τｒｅｆ１〜τｒｅｆ６と負荷トルクの測定値τ１〜τ６との差に基づくＰＩＤ演算により、トルク指令値τＦ１〜τＦ６を算出する（Ｓ３）。切替部５３１〜５３６は、モータ制御部５４１〜５４６にトルク指令値τＦ１〜τＦ６をトルク指令値τＭ１〜τＭ６として出力する。モータ制御部５４１〜５４６は、トルク指令値τＭ１〜τＭ６に応じてモータ２１１〜２１６を通電制御する（Ｓ４）。モータ２１１〜２１６は、通電に応じて関節Ｊ１〜Ｊ６に出力トルクを発生する（Ｓ５）。制御ユニット３００，４００は、負荷トルクの測定値τ１〜τ６及び関節の位置の測定値ｑ１〜ｑ６を取得する（Ｓ６）。制御ユニット３００は、駆動が終了したか否かを判定し（Ｓ７）、終了していない場合は（Ｓ７：Ｎｏ）、ステップＳ２〜Ｓ６を繰り返す。制御部５００は、駆動が終了した場合（Ｓ７：Ｙｅｓ）、制御処理を終了する。以上のフローチャートに従ってモータ２１１〜２１６を駆動することで、マニピュレータ２００のハンド２５２にかかる力Ｆを所望の目標力Ｆｄに制御することが可能である。なお、図５に示すフローチャートの順番に限定するものではなく、他の順番でも力制御は可能である。
【００４３】
次に、位置制御モードにおけるマニピュレータ２００の位置制御、即ちマニピュレータ２００のハンド２５２の位置制御について説明する。図６は、第１実施形態において制御部５００がマニピュレータ２００を位置制御する処理のフローチャートである。まず、位置指令値生成部５０５は、目標位置Ｐｄを外部記憶装置３２２から取得する（Ｓ１１）。目標位置Ｐｄは、マニピュレータ２００を位置決めする目標位置である。位置指令値生成部５０５は、目標位置Ｐｄに基づき、ロボットモデル５０３を用いて関節Ｊ１〜Ｊ６の位置指令値ｑｒｅｆ１〜ｑｒｅｆ６を算出する（Ｓ１２）。位置制御部５２１〜５２６は、位置指令値ｑｒｅｆ１〜ｑｒｅｆ６と関節の位置の測定値ｑ１〜ｑ６との差に基づくＰＩＤ演算により、トルク指令値τｑ１〜τｑ６を算出する（Ｓ１３）。なお、ステップＳ１３においては、測定値ｑ１〜ｑ６の代わりに測定値θ１〜θ６を用いてもよい。この場合、位置指令値ｑｒｅｆ１〜ｑｒｅｆ６は、減速機２２１〜２２６の減速比で位置指令値θｒｅｆ１〜θｒｅｆ６に変換すればよい。切替部５３１〜５３６は、モータ制御部５４１〜５４６にトルク指令値τｑ１〜τｑ６をトルク指令値τＭ１〜τＭ６として出力する。モータ制御部５４１〜５４６は、トルク指令値τＭ１〜τＭ６に応じてモータ２１１〜２１６を通電制御する（Ｓ１４）。モータ２１１〜２１６は、通電に応じて関節Ｊ１〜Ｊ６に出力トルクを発生する（Ｓ１５）。制御ユニット３００，４００は、関節の位置の測定値ｑ１〜ｑ６を取得する（Ｓ１６）。制御ユニット３００は、駆動が終了したか否かを判定し（Ｓ１７）、終了していない場合は（Ｓ１７：Ｎｏ）、ステップＳ１２〜Ｓ１６を繰り返す。制御部５００は、駆動が終了した場合（Ｓ１７：Ｙｅｓ）、制御処理を終了する。以上のフローチャートに従ってモータ２１１〜２１６を駆動することで、マニピュレータ２００のハンド２５２の位置を所望の目標位置Ｐｄに制御することが可能である。なお、図６に示すフローチャートの順番に限定するものではなく、他の順番でも位置制御は可能である。
【００４４】
ところで、マニピュレータ２００の力学モデルであるロボットモデル５０３と実機とは必ずしも一致せず、誤差が生じる。この誤差をモデル化誤差という。このモデル化誤差は、マニピュレータ２００の経年劣化などでも変化する。また、トルクセンサ２４１〜２４６の測定値τ１〜τ６には、マニピュレータが置かれている環境（例えば温度）の変化の影響で、オフセット誤差が重畳する。これらの誤差が大きいと、マニピュレータを力制御しているときにマニピュレータが振動するなど、マニピュレータの挙動が不安定になることがある。モデル化誤差とオフセット誤差とを求め、これら誤差に基づいて、モデル及び測定値を校正することも考えられるが、校正の作業を度々行わなければならない。マニピュレータ２００に連続運転で作業を行わせる場合には、校正作業の時間の分、サイクルタイムが増加してしまう。
【００４５】
そこで、本実施形態では、組立作業などの生産作業においてマニピュレータ２００を動作させている制御の中で、モデル化誤差及びオフセット誤差をキャンセルさせる。図７は、第１実施形態に係る第１制御ユニットの処理の一部を示すブロック図である。図７には、トルク指令値生成部５０４の処理について図示している。トルク指令値生成部５０４は、インピーダンス制御部６０１、動力学計算部６０２、指令値補正部６０３、補正値計算部６０４として機能し、例えば図２に示すＲＡＭ３０３が補正値記憶部６０５として機能する。以下、各部の機能について説明する。
【００４６】
動力学計算部６０２は、制御ユニット４００がマニピュレータ２００の位置を制御しているときに、マニピュレータ２００の位置の測定値である関節Ｊ１〜Ｊ６の位置の測定値ｑ１〜ｑ６を取得する。本実施形態では、動力学計算部６０２は、力制御を開始する位置を目標位置Ｐｄとし、マニピュレータ２００を目標位置Ｐｄに位置決めした際に、測定値ｑ１〜ｑ６を取得する。動力学計算部６０２は、取得したこれらの位置情報を微分して、速度、及び加速度を求める。動力学計算部６０２は、ロボットモデル５０３と関節Ｊ１〜Ｊ６の位置、速度、及び加速度の情報に基づいて、公知の動力学計算手法であるニュートン・オイラー法により、関節Ｊ１〜Ｊ６にかかる負荷トルクの計算値τ＾１〜τ＾６を算出する。即ち、動力学計算部６０２は、マニピュレータ２００の位置を制御しているときに、ロボットモデル５０３を用いた動力学の計算により、マニピュレータ２００に生じる負荷としての負荷トルクの計算値τ＾１〜τ＾６を求める。計算値τ＾１〜τ＾６には、モデル化誤差が含まれる。なお、動力学の計算は、これに限定するものではなく、例えばラグランジュの定式化による導出方法などでもよい。
【００４７】
補正値計算部６０４は、制御ユニット４００がマニピュレータ２００の位置を制御しているときに、マニピュレータ２００に生じる負荷の第１測定値として、負荷トルクの測定値τ１〜τ６を取得する。本実施形態では、補正値計算部６０４は、マニピュレータ２００を目標位置Ｐｄに位置決めした際に、測定値τ１〜τ６を取得する。測定値τ１〜τ６には、オフセット誤差が含まれている。補正値計算部６０４は、測定値τ１〜τ６と計算値τ＾１〜τ＾６との差に基づいて補正値τｃｏｍｐ１〜τｃｏｍｐ６を求める。具体的には、補正値計算部６０４は、位置制御モードから力制御モードに切り替わる直前に、計算値τ＾１〜τ＾６から測定値τ１〜τ６を減算することで、補正値τｃｏｍｐ１〜τｃｏｍｐ６を求める。例えば補正値計算部６０４は、位置制御モード時に、計算値τ＾１〜τ＾６から測定値τ１〜τ６を減算する処理を、所定の周期で行い、位置制御モードから力制御モードに切り替わったときに、その直前の減算値を補正値τｃｏｍｐ１〜τｃｏｍｐ６とする。補正値計算部６０４は、補正値τｃｏｍｐ１〜τｃｏｍｐ６を補正値記憶部６０５に記憶させておく。なお、補正値τｃｏｍｐ１〜τｃｏｍｐ６には、モデル化誤差とオフセット誤差が含まれている。
【００４８】
インピーダンス制御部６０１は、マニピュレータ２００のハンド２５２に所望の機械インピーダンスを持たせるよう、ロボットモデル５０３に基づく関節Ｊ１〜Ｊ６のトルク指令値τＦｄ１〜τＦｄ６を、以下の式（１）に従って算出する。式（１）は、インピーダンスの制御式である。機械インピーダンスは、例えば剛性インピーダンス、粘性インピーダンス、及び力インピーダンスである。トルク指令値τＦｄ１〜τＦｄ６は、補正前の仮の指令値であり、モデル化誤差が含まれている。
【数１】
2020151824.tif 000003
【００４９】
式（１）中、Ｍは６×６の慣性行列、Ｃは６×１のコリオリ力・遠心力のベクトル、Ｇは６×１の重力負荷トルクのベクトルである。Ｋｄは、所望の機械インピーダンスの剛性の値、Ｄｄは所望の機械インピーダンスの粘性の値を表わし、それぞれ６×６の行列である。Ｊａｃは、関節Ｊ１〜Ｊ６の速度と、座標系Ｔｏ（図１）基準で表現されたハンド２５２の速度とを関連付ける６×６のヤコビ行列である。なお、ヤコビ行列Ｊａｃの添え字Ｔは行列の転置を表わしている。
【００５０】
このように、インピーダンス制御部６０１は、制御ユニット４００がマニピュレータ２００の推力を制御する際に、式（１）を用いて、教示データ５０１，５０２に基づく仮の指令値であるトルク指令値τＦｄ１〜τＦｄ６を求める。動力学計算部６０２は、Ｍ、Ｃ、Ｇを、ロボットモデル５０３と、関節Ｊ１〜Ｊ６の位置及び速度に基づいて、ニュートン・オイラー法で計算する。
【００５１】
指令値補正部６０３は、トルク指令値τＦｄ１〜τＦｄ６を補正値τｃｏｍｐ１〜τｃｏｍｐ６で補正し、トルク制御部５１１〜５１６に出力する指令値であるトルク指令値τｒｅｆ１〜τｒｅｆ６を求める。具体的には、指令値補正部６０３は、補正値記憶部６０５に記憶されている補正値τｃｏｍｐ１〜τｃｏｍｐ６をインピーダンス制御部６０１で算出したトルク指令値τＦｄ１〜τＦｄ６から減算することで、トルク指令値τｒｅｆ１〜τｒｅｆ６を生成する。これにより、マニピュレータ２００の実機とモデルとの差であるモデル化誤差がキャンセルされる。
【００５２】
トルク指令値τｒｅｆ１〜τｒｅｆ６には、オフセット誤差が含まれている。図４に示すトルク制御部５１１〜５１６は、制御ユニット４００がマニピュレータ２００の推力を制御する際に、マニピュレータ２００に生じる負荷の第２測定値である関節Ｊ１〜Ｊ６の負荷トルクの測定値τ１〜τ６を取得する。この測定値τ１〜τ６には、オフセット誤差が含まれている。トルク制御部５１１〜５１６は、トルク指令値τｒｅｆ１〜τｒｅｆ６と測定値τ１〜τ６との差を求め、この差に基づき、フィードバック値であるトルク指令値τＦ１〜τＦ６を求める。このように、マニピュレータ２００の推力を制御するのに用いられるトルク指令値τＦ１〜τＦ６において、オフセット誤差がキャンセルされることになる。
【００５３】
以上、本実施形態によれば、マニピュレータ２００の推力の制御中、モデル化誤差とオフセット誤差が共にキャンセルされるので、マニピュレータ２００の制御が安定し、マニピュレータ２００の制御性が向上する。
【００５４】
以下、マニピュレータ２００の制御の実験結果について説明する。図８は、第１実施形態におけるロボットアーム２５１の概略図である。図８中、Ｔ１〜Ｔ６はリンク２１００〜２１０６に固定された座標系である。なお、各関節Ｊ１〜Ｊ６の回転軸はＺ軸で、回転方向は右手方向である。図８に示したロボットアーム２５１の姿勢を、関節Ｊ１〜Ｊ６の角度の測定値ｑ１〜ｑ６が０度の時の姿勢とする。また、リンク２１００〜２１０６の寸法は、図８に示す通りである。
【００５５】
図９（ａ）は、第１実施形態における現実のマニピュレータ２００の動力学パラメータの一例を示す図である。図９（ａ）中、ｍ＾は、当該リンクの実際の質量である。ｓ＾ｘ、ｓ＾ｙ、ｓ＾ｚは、それぞれ当該リンクの座標原点から見たＸ軸、Ｙ軸、Ｚ軸方向の実際の重心位置である。Ｉ＾ｘｘ、Ｉ＾ｙｙ、Ｉ＾ｚｚは、それぞれ当該リンクの重心まわりのＸ軸、Ｙ軸、Ｚ軸に対する実際の慣性モーメントである。
【００５６】
図９（ｂ）は、第１実施形態におけるロボットモデル５０３の動力学パラメータの一例を示す図である。ロボットモデル５０３は、ロボットアーム２５１の設計値からＣＡＤで事前に算出し、外部記憶装置３２２に記憶しておく。図９（ｂ）中、ｍは当該リンクの設計上の質量である。ｓｘ、ｓｙ、ｓｚはそれぞれ当該リンクの座標原点から見たＸ軸、Ｙ軸、Ｚ軸方向の設計上の重心位置である。Ｉｘｘ、Ｉｙｙ、Ｉｚｚはそれぞれ当該リンクの重心まわりのＸ軸、Ｙ軸、Ｚ軸に対する設計上の慣性モーメントである。本実施形態では、ロボットモデル５０３の各リンクの質量が、現実の各リンク２１０１〜２１０６の質量の１割の誤差があるものとした。
【００５７】
図１０（ａ）及び図１０（ｂ）は、第１実施形態におけるマニピュレータ２００による組付作業を説明するためのマニピュレータ２００の模式図である。本実施形態では、ハンド２５２で把持したワークＷ１をワークＷ２に組み付ける組み付け作業を行う場合について説明する。ワークＷ１は、円筒形状の凸部を有する部品であり、ワークＷ２は円筒形状の凹部を有する部品である。図１０（ａ）中、Ｐ１は、マニピュレータ２００のハンド２５２の座標系Ｔｅで示す力制御開始時の教示点であり、Ｐ２は、マニピュレータ２００のハンド２５２の座標系Ｔｅで示す力制御の目標位置を示す教示点である。
【００５８】
図１０（ｂ）は、マニピュレータ２００のハンド２５２の座標系Ｔｅが教示点Ｐ１にあるときのワークＷ１とワークＷ２との位置関係を説明するための拡大図である。ワークＷ２のとば口ＷＡは、面取りされている。ワークＷ１が座標系ＴｏのＸ軸方向に力制御で２０ｍｍ進んだ時、ワークＷ１がとば口ＷＡの内側にあれば、力制御の柔軟性により、ワークＷ１がワークＷ２の内面に倣い、組み付けが完了する。ワークＷ１がとば口ＷＡの内側にあるとは、ワークＷ１の中心軸が座標系ＴｏのＺ軸方向の１９５ｍｍ〜２０５ｍｍの範囲内にあることである。このとき、ワークＷ１の傾き、及び座標系ＴｏのＹ軸方向への移動は、組み付け作業には影響がないものとする。
【００５９】
図１１（ａ）は、第１実施形態における教示点のパラメータを示す図である。図１１（ｂ）は、第１実施形態における力制御の機械インピーダンスのパラメータを示す図である。図１１（ａ）に、教示点Ｐ１，Ｐ２の座標系Ｔｏを基準とする位置及び姿勢の値を示す。なお、図１１（ａ）中のα、β、γは、それぞれＺ軸、Ｙ軸、Ｘ軸まわりの回転量を表わす。また、図１１（ｂ）に、機械インピーダンスのパラメータとして剛性Ｋｄ、粘性Ｄｄ、力Ｆｄの値を示す。
【００６０】
図１２は、第１実施形態に係る物品の製造方法を説明するフローチャートである。まず、制御部５００は、マニピュレータ２００の位置制御を開始し、マニピュレータ２００のハンド２５２（座標系Ｔｅ）を、力制御を開始する位置である教示点Ｐ１へ移動させる（Ｓ５１）。
【００６１】
制御部５００は、ロボットモデル５０３と関節Ｊ１〜Ｊ６の位置、速度、及び加速度の情報に基づいて、現在の関節Ｊ１〜Ｊ６の負荷トルクの計算値τ＾１〜τ＾６を算出する（Ｓ５２）。このとき、マニピュレータ２００は必ずしも停止している必要はない。
【００６２】
制御部５００は、第１測定値である、関節Ｊ１〜Ｊ６の負荷トルクの測定値τ１〜τ６を取得する（Ｓ５３）。このとき、測定値τ１〜τ６には、トルクセンサ２４１〜２４６のオフセット誤差が含まれ、例えばマニピュレータ２００の関節Ｊ１における測定値τ１および関節Ｊ２における測定値τ２に、−１．０Ｎｍのオフセット誤差があるものとする。
【００６３】
制御部５００は、計算値τ＾１〜τ＾６から測定値τ１〜τ６を減算することで、補正値τｃｏｍｐ１〜τｃｏｍｐ６を算出する（Ｓ５４）。このとき、算出されたトルク補正値τｃｏｍｐ１〜τｃｏｍｐ６は、補正値記憶部６０５に記憶される。
【００６４】
制御部５００は、位置制御モードから力制御モードへ切り替え、力制御を開始する（Ｓ５５）。制御部５００は、マニピュレータ２００のハンド２５２に所望の機械インピーダンス（剛性Ｋｄ、粘性Ｄｄ、力Ｆｄ）を持たせるような関節Ｊ１〜Ｊ６のトルク指令値τＦｄ１〜τＦｄ６を式（１）に従って算出する（Ｓ５６）。
【００６５】
制御部５００は、ステップＳ５６で算出したトルク指令値τＦｄ１〜τＦｄ６から補正値記憶部６０５に記憶されている補正値τｃｏｍｐ１〜τｃｏｍｐ６を減算してトルク指令値τｒｅｆ１〜τｒｅｆ６を算出する（Ｓ５７）。制御部５００は、トルク指令値τｒｅｆ１〜τｒｅｆ６と、第２測定値である測定値τ１〜τ６との差に基づくＰＩＤ演算により、トルク指令値τＦ１〜τＦ６を算出する（Ｓ５８）。
【００６６】
制御部５００は、トルク指令値τＦ１〜τＦ６をトルク指令値τＭ１〜τＭ６とし、トルク指令値τＭ１〜τＭ６に応じてモータ２１１〜２１６を通電制御する（Ｓ５９）。モータ２１１〜２１６は、通電に応じて関節Ｊ１〜Ｊ６に出力トルクを発生する（Ｓ６０）。制御部５００は、負荷トルクの測定値τ１〜τ６及び関節の位置の測定値ｑ１〜ｑ６を取得する（Ｓ６１）。制御部５００は、駆動が終了したか否かを判定し（Ｓ６２）、終了していない場合は（Ｓ６２：Ｎｏ）、ステップＳ５６〜Ｓ６２を繰り返す。制御部５００は、駆動終了と判断したら（Ｓ６２：Ｙｅｓ）、制御処理を終了する。本実施形態では、ステップＳ５５で力制御を開始してから５秒経過した時点で、制御処理を終了する。
【００６７】
図１３（ａ）、図１３（ｂ）及び図１３（ｃ）は、実験結果を示すグラフである。図１３（ａ）は、第１実施形態において、力制御を行っているときの時間に対するハンド２５２のＸ軸方向の位置を示すグラフである。
【００６８】
インピーダンス制御の目標位置Ｐｄは、図１３（ａ）に示すように、教示点Ｐ１から座標系ＴｏでＸ軸方向に５０ｍｍ移動した位置である。インピーダンス制御を開始した時点から３秒間、Ｘ軸方向に５０ｍｍ線形変化するものとした。
【００６９】
図１３（ｂ）のグラフは、第１実施形態おいて、マニピュレータ２００を力制御したときのマニピュレータ２００のハンド２５２の座標系Ｔｅの位置変化を示している。横軸は、座標系Ｔｏにおける座標系ＴｅのＸ軸方向の位置である。縦軸は、座標系Ｔｏにおける座標系ＴｅのＺ軸方向の位置である。ワークＷ１とワークＷ２との接触による位置変化は考慮しないものとする。
【００７０】
図１３（ｂ）に示す通り、座標系Ｔｅは、ワークＷ１がとば口ＷＡに入る範囲、即ち座標系Ｔｅが座標系ＴｏのＸ軸方向の位置２７０ｍｍに到達したときに、座標系ＴｏのＺ軸方向の位置１９５ｍｍ〜２０５ｍｍの範囲内にある。したがってワークＷ２に対するワークＷ１の組み付けが成功する。
【００７１】
一方、ステップＳ５７においてトルク指令値τＦ１〜τＦ６を補正しない場合、即ち、トルク指令値τＦ１〜τＦ６がトルク指令値τｒｅｆ１〜τｒｅｆ６である場合を比較例とする。図１３（ｃ）のグラフは、比較例おいて、マニピュレータ２００を力制御したときのマニピュレータ２００のハンド２５２の座標系Ｔｅの位置変化を示している。
【００７２】
図１３（ｃ）に示すように、座標系Ｔｅが、座標系ＴｏのＸ軸方向の位置２７０ｍｍに到達したときに、座標系ＴｏのＺ軸方向の位置１８５ｍｍ以下にあり、組み付け可能な位置１９５ｍｍ〜２０５ｍの範囲から１０ｍｍ以上ずれている。したがってワークＷ２に対するワークＷ１の組み付けが失敗する。
【００７３】
このように、本実施形態では、マニピュレータ２００の実機とモデルとの差が不明であっても、トルク指令値τＦｄ１〜τＦｄ６を補正値τｃｏｍｐ１〜τｃｏｍｐ６で補正することで、モデル化誤差をキャンセルすることができる。また、負荷トルクの測定値τ１〜τ６に含まれるオフセット誤差が不明であっても、フィードバック演算において、トルク指令値τｒｅｆ１〜τｒｅｆ６と測定値τ１〜τ６との差を求めることで、オフセット誤差をキャンセルすることができる。よって、関節Ｊ１〜Ｊ６のトルクの急変を防止することができる。
【００７４】
また、ロボットモデル５０３は、厳密に現実のマニピュレータ２００に近付ける必要がなくなるので、ロボットモデル５０３の推定などの準備にかかる手間や時間を減らすことができる。さらに、環境変化や経時変化によるトルクセンサ２４１〜２４６のオフセット誤差も事前に調べる必要がなくなり、校正作業にかかる手間や時間も減らすことができる。所定の校正作業を事前に行う必要なしに、力制御の作業ごとにモデル化誤差およびセンサオフセット誤差による制御性能の低下や不測の動作を防止することができる。
【００７５】
［第２実施形態］
第２実施形態に係るロボット装置について説明する。図１４は、第２実施形態に係るロボット装置の概略構成を示す斜視図である。第２実施形態のロボット装置１００Ａにおいて、第１実施形態のロボット装置１００と同様の構成については、同一符号を付して詳細な説明は省略する。図１４に示すように、ロボット装置１００Ａは、第１実施形態と同様の構成のマニピュレータ２００と、マニピュレータ２００の位置又は推力を制御する制御部５００Ａを有する制御装置１０００Ａと、を備えている。また、ロボット装置１００Ａは、制御装置１０００Ａに教示データを送信する教示装置としての教示ペンダント６００を備えている。
【００７６】
制御部５００Ａは、第１制御ユニットである制御ユニット３００Ａと、第２制御ユニットである制御ユニット４００Ａと、を有する。制御ユニット３００Ａは、ロボット装置１００Ａを統括的に制御するものである。制御ユニット４００Ａは、制御ユニット３００Ａからの指令に基づいて、マニピュレータ２００の位置又は推力を制御するものである。制御ユニット４００Ａは、マニピュレータ２００の各サーボ機構部２０１〜２０６を制御するサーボ制御装置である。
【００７７】
制御部５００Ａについて具体的に説明する。図１５及び図１６は、第２実施形態に係る制御部５００Ａの構成を示すブロック図である。制御部５００Ａは、上述したように、制御ユニット３００Ａ，４００Ａを有する。図１５には、制御ユニット３００Ａの詳細な構成、図１６には、制御ユニット４００Ａの詳細な構成について図示している。
【００７８】
制御ユニット３００Ａは、コンピュータで構成されている。制御ユニット３００Ａのハードウェアは、第１実施形態と同様であり、ＣＰＵ３０１、ＲＯＭ３０２、ＲＡＭ３０３、ＨＤＤ３０４、記録ディスクドライブ３０５、及びインタフェース３０６〜３０９を備えている。ＣＰＵ３０１、ＲＯＭ３０２、ＲＡＭ３０３、ＨＤＤ３０４、記録ディスクドライブ３０５、インタフェース３０６〜３０９は、相互に通信可能にバス３１０で接続されている。ＲＯＭ３０２には、ＣＰＵ３０１に制御方法の処理を実行させるためのプログラム３３０Ａが格納されている。
【００７９】
なお、第２実施形態では、コンピュータによって読取可能な記録媒体がＲＯＭ３０２であり、ＲＯＭ３０２にプログラム３３０Ａが記録されているが、これに限定するものではない。プログラム３３０Ａは、コンピュータによって読取可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。プログラム３３０Ａを供給するための記録媒体としては、例えば、フレキシブルディスク、ハードディスク、光ディスク、光磁気ディスク、磁気テープ、不揮発性メモリ等を用いることができる。
【００８０】
図１６に示す制御ユニット４００Ａは、コンピュータで構成されている。制御ユニット４００Ａのハードウェアは、第１実施形態と同様であり、ＣＰＵ４０１と、ＲＯＭ４０２と、ＲＡＭ４０３と、インタフェース４０６〜４０９と、を備えている。ＣＰＵ４０１、ＲＯＭ４０２、ＲＡＭ４０３、インタフェース４０６〜４０９は、相互に通信可能にバス４１０で接続されている。ＲＯＭ４０２には、ＣＰＵ４０１に制御方法の処理を実行させるためのプログラム４３０Ａが格納されている。
【００８１】
インタフェース４０６には、モータ２１１〜２１６が接続されている。インタフェース４０７には、位置センサ２３１〜２３６が接続されている。インタフェース４０８には、トルクセンサ２４１〜２４６が接続されている。インタフェース４０９には、制御ユニット３００Ａが接続されている。
【００８２】
なお、第２実施形態では、コンピュータによって読取可能な記録媒体がＲＯＭ４０２であり、ＲＯＭ４０２にプログラム４３０Ａが記録されているが、これに限定するものではない。プログラム４３０Ａは、コンピュータによって読取可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。プログラム４３０Ａを供給するための記録媒体としては、例えば、フレキシブルディスク、ハードディスク、光ディスク、光磁気ディスク、磁気テープ、不揮発性メモリ等を用いることができる。
【００８３】
図１７は、第２実施形態に係るロボット装置の制御システムを示す制御ブロック図である。図１５に示す制御ユニット３００ＡのＣＰＵ３０１は、プログラム３３０Ａを実行することにより、図１７に示すトルク指令値生成部５０４Ａ及び位置指令値生成部５０５として機能する。図１６に示す制御ユニット４００ＡのＣＰＵ４０１は、プログラム４３０Ａを実行することにより、図１７に示すトルク制御部５１Ａ１〜５１Ａ６、位置制御部５２１〜５２６、切替部５３１〜５３６、及びモータ制御部５４１〜５４６として機能する。サーボ機構部２０１〜２０６は、モータ２１１〜２１６、減速機２２１〜２２６、位置センサ２３１〜２３６、及びトルクセンサ２４１〜２４６を有する。
【００８４】
トルク指令値生成部５０４Ａは、ロボットモデル５０３、目標力Ｆｄ、目標位置Ｐｄ、関節Ｊ１〜Ｊ６の位置の測定値ｑ１〜ｑ６、関節Ｊ１〜Ｊ６のトルクの測定値τ１〜τ６が入力される。そして、トルク指令値生成部５０４Ａは、これらの値を用いて、関節Ｊ１〜Ｊ６に対するトルク指令値τｒｅｆ１〜τｒｅｆ６を求める。トルク指令値生成部５０４Ａは、求めたトルク指令値τｒｅｆ１〜τｒｅｆ６を、トルク制御部５１Ａ１〜５１Ａ６に出力する。
【００８５】
トルク制御部５１Ａ１〜５１Ａ６は、関節Ｊ１〜Ｊ６のトルク指令値τｒｅｆ１〜τｒｅｆ６と、関節Ｊ１〜Ｊ６のトルクの測定値τ１〜τ６との差が小さくなるように、トルク指令値τＦ１〜τＦ６を求め、切替部５３１〜５３６に出力する。トルク制御部５１Ａ１〜５１Ａ６の詳細については後述する。なお、位置制御部５２１〜５２６、切替部５３１〜５３６、モータ制御部５４１〜５４６は、第１実施形態と同様の処理を行うため、詳細な説明は省略する。
【００８６】
以上、制御ユニット４００Ａは、制御ユニット３００Ａから位置指令値ｑｒｅｆ１〜ｑｒｅｆ６を取得した場合、位置指令値ｑｒｅｆ１〜ｑｒｅｆ６と測定値ｑ１〜ｑ６との差に基づき、マニピュレータ２００の位置を制御する。制御ユニット４００Ａは、制御ユニット３００Ａからトルク指令値τｒｅｆ１〜τｒｅｆ６を取得した場合、トルク指令値τｒｅｆ１〜τｒｅｆ６と測定値τ１〜τ６との差に基づき、マニピュレータ２００の推力、即ち関節Ｊ１〜Ｊ６の出力トルクを制御する。
【００８７】
ところで、マニピュレータで精密な組立作業を行う場合、マニピュレータを位置制御する位置制御モードにて組立を開始する所定位置にマニピュレータを移動させる。その後、マニピュレータを力制御する力制御モードに切り替えてからマニピュレータに精密組立を行わせることがある。位置制御モードから力制御モードに制御モードを切り替える際、仮に、マニピュレータの関節を駆動するモータへのトルク指令値が急激に変化すると、関節が急峻な動作をし、その結果、マニピュレータのハンドが振動的になることがある。これは、位置制御モードから力制御モードへの切り替え時に、フィードバック値が一旦ゼロにリセットされるためである。
【００８８】
図１８は、第２実施形態に係る第２制御ユニットの処理の一部を示すブロック図である。図１８には、トルク制御部５１Ａ１〜５１Ａ６の処理について図示している。トルク制御部５１Ａ１〜５１Ａ６は、主に、フィードバック制御部６０１〜６０６、フィードフォワード演算部６２１〜６２６、及び加算部６３１〜６３６として機能する。また、図１６に示すＲＡＭ４０３は、第１記憶部６１１〜６１６及び第２記憶部７１１〜７１６として機能する。以下、各部の機能について説明する。
【００８９】
第１記憶部６１１〜６１６は、力制御モードに切り替わる前の位置制御モードにおいて、第１測定値である、トルクセンサ２４１〜２４６を用いて測定した負荷トルクの測定値τ１〜τ６を、オフセット値τｏｆｆｓｅｔ１〜τｏｆｆｓｅｔ６として記憶する。即ち、トルク制御部５１Ａ１〜５１Ａ６は、位置制御部５２１〜５２６がマニピュレータ２００の位置を制御しているときにマニピュレータ２００に生じる負荷の第１測定値に基づくオフセット値τｏｆｆｓｅｔ１〜τｏｆｆｓｅｔ６を求める。トルク制御部５１Ａ１〜５１Ａ６は、第１記憶部６１１〜６１６に、位置制御時に取得したオフセット値τｏｆｆｓｅｔ１〜τｏｆｆｓｅｔ６を記憶させておく。第１記憶部６１１〜６１６に記憶されたオフセット値τｏｆｆｓｅｔ１〜τｏｆｆｓｅｔ６は、位置制御モードから力制御モードに切り替えた後、フィードバック値τＦＢ１〜τＦＢ６に加算される。
【００９０】
本実施形態では、マニピュレータ２００の位置が目標位置に到達したのをトリガとし、その時の測定値τ１〜τ６をオフセット値τｏｆｆｓｅｔ１〜τｏｆｆｓｅｔ６とする。なお、オフセット値τｏｆｆｓｅｔ１〜τｏｆｆｓｅｔ６は、位置制御中、所定の制御周期で随時更新してもよい。この場合、位置制御モードから力制御モードに切り替わるときには、最後に更新された値が第１記憶部６１１〜６１６に保持されることとなる。
【００９１】
フィードバック制御部６０１〜６０６は、力制御モードにおいて、所定の制御周期で、トルク指令値生成部５０４Ａからトルク指令値τｒｅｆ１〜τｒｅｆ６と、トルクセンサ２４１〜２４６を用いて測定した負荷トルクの測定値τ１〜τ６と、を取得する。フィードバック制御部６０１〜６０６は、トルク指令値τｒｅｆ１〜τｒｅｆ６と、測定値τ１〜τ６との差に基づき、フィードバック値τＦＢ１〜τＦＢ６を算出する。フィードバック制御部６０１〜６０６は、フィードバック演算の一例として、比例演算、積分演算、及び微分演算とからなるＰＩＤ演算を行う。
【００９２】
フィードフォワード演算部６２１〜６２６は、力制御時において、トルク指令値τｒｅｆ１〜τｒｅｆ６に応じたフィードフォワード値τＦＦ１〜τＦＦ６を求める。加算部６３１〜６３６は、フィードバック値τＦＢ１〜τＦＢ６に、オフセット値τｏｆｆｓｅｔ１〜τｏｆｆｓｅｔ６と、フィードフォワード値τＦＦ１〜τＦＦ６とを加算し、その値を、トルク指令値τＦ１〜τＦ６として切替部５３１〜５３６に出力する。
【００９３】
フィードフォワード演算部６２１〜６２６の処理について具体例を挙げて説明する。ここで、トルク制御部５１Ａ１〜５１Ａ６が入力を受けるトルク指令値τｒｅｆ１〜τｒｅｆ６のうち、力制御時に最初に入力を受ける指令値（第２指令値）を、トルク指令値τ０ｒｅｆ１〜τ０ｒｅｆ６とする。つまり、トルク指令値生成部５０４Ａは、トルク指令値τ０ｒｅｆ１〜τ０ｒｅｆ６を、マニピュレータ２００の推力の制御を開始したときに最初に求める。また、トルク制御部５１Ａ１〜５１Ａ６がトルク指令値τ０ｒｅｆ１〜τ０ｒｅｆ６の後に入力を受ける指令値（第１指令値）をトルク指令値τ１ｒｅｆ１〜τ１ｒｅｆ６とする。つまり、トルク指令値生成部５０４Ａは、トルク指令値τ０ｒｅｆ１〜τ０ｒｅｆ６の後にトルク指令値τ１ｒｅｆ１〜τ１ｒｅｆ６を求める。よって、トルク指令値τ０ｒｅｆ１〜τ０ｒｅｆ６は、トルク制御部５１Ａ１〜５１Ａ６において、トルク指令値τ１ｒｅｆ１〜τ１ｒｅｆ６の前に入力を受けるトルク指令値である。
【００９４】
フィードフォワード演算部６２１〜６２６は、本実施形態では、モデルトルク演算部７０１〜７０６と、減算部７３１〜７３６とを含む。力制御を行う最初の制御サイクルについて説明する。モデルトルク演算部７０１〜７０６は、ロボットモデル５０３を用いた動力学の計算により、トルク指令値τ０ｒｅｆ１〜τ０ｒｅｆ６に応じた、マニピュレータ２００に生じる負荷である負荷トルクの第２計算値である計算値τ０ＦＦ１〜τ０ＦＦ６を求める。ロボットモデル５０３には、モータ２１１〜２１６の負荷イナーシャＪＭ及び粘性係数ＤＭ、並びに減速機２２１〜２２６の剛性値Ｋ及び減速比Ｎなどのパラメータが含まれる。本実施形態では、モデルトルク演算部７０１〜７０６は、トルク指令値τ０ｒｅｆ１〜τ０ｒｅｆ６と、ロボットモデル５０３に含まれるパラメータＪＭ、ＤＭ、Ｋ、Ｎと、に基づいて、計算値τ０ＦＦ１〜τ０ＦＦ６を以下の式（２）に従って求める。ｓはラプラス演算子である。
【数２】
2020151824.tif 000004
【００９５】
第２記憶部７１１〜７１６は、計算値τ０ＦＦ１〜τ０ＦＦ６を、オフセット値である基準値τ０ＦＦ１〜τ０ＦＦ６として記憶する。第２記憶部７１１〜７１６は、力制御が継続される間、計算値τ０ＦＦ１〜τ０ＦＦ６を基準値τ０ＦＦ１〜τ０ＦＦ６として保持する。力制御モードから位置制御モードに切り替わったときには、第２記憶部７１１〜７１６は、リセットされる。
【００９６】
減算部７３１〜７３６は、計算値τ０ＦＦ１〜τ０ＦＦ６から基準値τ０ＦＦ１〜τ０ＦＦ６を引いた値を、フィードフォワード値τＦＦ１〜τＦＦ６とし、加算部６３１〜６３６に出力する。したがって、本実施形態では、フィードフォワード値τＦＦ１〜τＦＦ６の初期値はゼロである。
【００９７】
次に、最初の制御サイクルの次の制御サイクル以降について説明する。モデルトルク演算部７０１〜７０６は、ロボットモデル５０３を用いた動力学の計算により、トルク指令値τ１ｒｅｆ１〜τ１ｒｅｆ６に応じた、マニピュレータ２００に生じる負荷である負荷トルクの第１計算値である計算値τ１ＦＦ１〜τ１ＦＦ６を求める。本実施形態では、モデルトルク演算部７０１〜７０６は、トルク指令値τ１ｒｅｆ１〜τ１ｒｅｆ６と、ロボットモデル５０３に含まれるパラメータＪＭ、ＤＭ、Ｋ、Ｎと、に基づいて、計算値τ１ＦＦ１〜τ１ＦＦ６を以下の式（３）に従って求める。減算部７３１〜７３６は、計算値τ１ＦＦ１〜τ１ＦＦ６から基準値τ０ＦＦ１〜τ０ＦＦ６を引いた値を、フィードフォワード値τＦＦ１〜τＦＦ６とし、加算部６３１〜６３６に出力する。
【数３】
2020151824.tif 000005
【００９８】
つまり、マニピュレータ２００の関節Ｊ１〜Ｊ６において、作用する負荷の変動で捩れ量が変動すると、ハンド２５２が振動する。本実施形態では、力制御を開始する時点での負荷を基準とし、負荷変動分（２つの計算値の差分）をフィードフォワード値τＦＦ１〜τＦＦ６とすることで、力制御の応答性を高め、その結果、ハンド２５２の振動が抑制される。
【００９９】
なお、第２実施形態においては、第２記憶部７１１〜７１６に記憶される基準値は、マニピュレータ２００の推力の制御を開始して最初に求められる初期値である計算値τ０ＦＦ１〜τ０ＦＦ６であるのが好ましいが、これに限定するものではない。例えば、初期値の次の値などを用いてもよい。即ち、基準値は、ある計算値τ１ＦＦ１〜τ１ＦＦ６よりも前に、トルク指令値τ１ＦＦ１〜τ１ＦＦ６よりも前のトルク指令値に応じて求められた、マニピュレータ２００に生じる負荷の計算値であればよい。
【０１００】
図１９は、第２実施形態に係る物品の製造方法を説明するフローチャートである。なお、第２実施形態では、第１実施形態と同様、図１０（ａ）に示すように、ワークＷ１をハンド２５２に把持させ、ワークＷ１をワークＷ２に組み付けることで物品が製造されるものとする。
【０１０１】
まず、制御部５００Ａは、マニピュレータ２００の位置制御を開始し（Ｓ７１）、力制御を開始する位置、即ち図１０（ａ）に示す教示点Ｐ１に到達するまで、位置制御でマニピュレータ２００を駆動する（Ｓ７２）。
【０１０２】
マニピュレータ２００が力制御を開始する位置まで駆動したかどうかを判定する（Ｓ７３）。制御部５００Ａは、マニピュレータ２００がその位置まで移動した場合（Ｓ７３：Ｙｅｓ）、当該位置におけるトルクの測定値τ１〜τ６を取得する。制御部５００Ａは、図１８に示す第１記憶部６１１〜６１６に、測定値τ１〜τ６をオフセット値τｏｆｆｓｅｔ１〜τｏｆｆｓｅｔ６として記憶させる（Ｓ７４）。
【０１０３】
次に、制御部５００Ａは、位置制御モードから力制御モードへマニピュレータ２００を制御する制御モードを切り替え（Ｓ７５）、マニピュレータ２００の力制御を開始する（Ｓ７６）。力制御開始後、制御部５００Ａは、トルク指令値τ０ｒｅｆ１〜τ０ｒｅｆ６を入力として、関節Ｊ１〜Ｊ６における負荷トルクの計算値τ０ＦＦ１〜τ０ＦＦ６を基準値として第２記憶部７１１〜７１６に記憶させる（Ｓ７７）。
【０１０４】
制御部５００Ａは、フィードバック値τＦＢ１〜τＦＢ６に、オフセット値τｏｆｆｓｅｔ１〜τｏｆｆｓｅｔ６とフィードフォワード値τＦＦ１〜τＦＦ６とを加算した値に基づいて、マニピュレータ２００の推力を制御する。これにより、制御部５００Ａは、マニピュレータ２００のハンド２５２に働く力が目標力となるように駆動する（Ｓ７８）。
【０１０５】
制御部５００Ａは、駆動が終了したかどうか、つまり組付けが完了したかどうかを判定する（Ｓ７９）。制御部５００Ａは、駆動が終了していない場合（Ｓ７９：Ｎｏ）、組付けが完了するまで、ステップＳ７８及びＳ７９のループを繰り返し実行する。制御部５００Ａは、駆動終了と判断したら（Ｓ７９：Ｙｅｓ）、制御処理を終了する。
【０１０６】
以上、第２実施形態によれば、位置制御モードから力制御モードへ切り替わる直前に、第１記憶部６１１〜６１６にトルクの測定値τ１〜τ６が、オフセット値τｏｆｆｓｅｔ１〜τｏｆｆｓｅｔ６として記憶される。力制御モードに切り替わった後は、フィードバック値τＦＢ１〜τＦＢ６に、オフセット値τｏｆｆｓｅｔ１〜τｏｆｆｓｅｔ６が加算される。これにより、位置制御モードから力制御モードに制御が切り替わった際に、トルク指令値τＭ１〜τＭ６の連続性が補償される。モータトルク指令値τＭ１〜τＭ６の連続性が補償されるので、モータへ供給される電流Ｃｕｒ１〜Ｃｕｒ６が急峻に変動するのが防止される。よって、マニピュレータ２００の各関節において急峻な動作が防止され、マニピュレータ２００のハンド２５２が振動するのを防止することができる。特に、マニピュレータ２００を用いて精密な組立て作業を行う場合、マニピュレータ２００のハンド２５２の振動が防止されるので、組立ワーク又は被組立ワークが変形したり破壊したりするのを防止することができる。また、組立不良を低減することができ、リトライ動作などが少なくなり、物品の生産性が向上する。
【０１０７】
また、第２実施形態によれば、フィードバック値τＦＢ１〜τＦＢ６に、フィードフォワード値τＦＦ１〜τＦＦ６が加算されるので、マニピュレータ２００における関節Ｊ１〜Ｊ６の制御の応答性が向上する。このフィードフォワード値τＦＦ１〜τＦＦ６は、負荷トルクの計算値τ１ＦＦ１〜τ１ＦＦ６から、オフセットしている分として基準値τ０ＦＦ１〜τ０ＦＦ６が除去されることで求められるので、トルク指令値τＭ１〜τＭ６が急峻に変動するのが防止される。したがって、制御モードの切替時にハンド２５２が振動するのを効果的に防止することができる。以上、第２実施形態によれば、位置制御モードから力制御モードへ切り替わる際のマニピュレータ２００の制御性が向上する。
【０１０８】
図２０は、第２実施形態における実験結果を示すグラフである。図２０において、横軸は時間、縦軸はトルクである。図２０には、関節Ｊ１のトルク指令値τＭ１と、関節Ｊ１の負荷トルクの測定値τ１について図示している。
【０１０９】
制御モードの切り替え時、トルク指令値τＭ１は、トルク指令値τｑ１からトルク指令値τＦ１へ切り替わるが、急峻な変化をさせることなく滑らかに切り替わっている。このため、制御モードを切り替えた後のトルクの測定値τ１の振動が小さい。また、力制御時は、フィードバック値τＦＢ１にフィードフォワード値τＦＦ１を加算しているため、測定値τ１が応答性良く、トルク指令値τＭ１に追従していることが確認できる。
【０１１０】
以上より、第２実施形態によれば、位置制御モードから力制御モードに制御モードを切り替える際において、制御モードの切り替え時のトルク指令値τＭ１〜τＭ６の連続性を補償し、かつ力制御モード時の応答性を向上させることができる。これにより、制御モードを切り替える場合においても、マニピュレータ２００の高精度かつ高応答な力制御が可能となる。
【０１１１】
［第３実施形態］
次に、第３実施形態のロボット装置について説明する。なお、ロボット装置の構成は、第２実施形態と同様であるため、説明を省略する。
【０１１２】
第２実施形態では、第２記憶部７１１〜７１６に記憶させておく基準値を、モデルトルク演算部７０１〜７０６にて算出した値の初期値とした。制御モードの切り替え直後は、フィードフォワード値τＦＦ１〜τＦＦ６はゼロとなる。よって、この場合、トルク指令値τＭ１〜τＭ６は、僅かな期間ではあるが一定値となり、滑らかに推移しない。
【０１１３】
図２１は、第３実施形態に係る第２制御ユニットの処理の一部を示すブロック図である。第３実施形態では、モデルトルク演算部７０１〜７０６は、力制御実行の前の位置制御実行時に、第２計算値を求める演算処理を並行して行い、この値を第２記憶部７１１〜７１６に記憶させておく。
【０１１４】
具体的には、トルク指令値生成部５０４Ａは、位置制御が実行されている時に、力制御が実行される時の第１指令値であるトルク指令値τ１ｒｅｆ１〜τ１ｒｅｆ６よりも前に、第２指令値であるトルク指令値τ２ｒｅｆ１〜τ２ｒｅｆ６を求める。トルク指令値生成部５０４Ａは、位置制御中に目標力Ｆｄ、目標位置Ｐｄ、測定値τ１〜τ６、及び測定値ｑ１〜ｑ６を取得し、トルク指令値τ２ｒｅｆ１〜τ２ｒｅｆ６を、取得したこれら値に基づいて生成する。
【０１１５】
トルク指令値生成部５０４Ａは、位置制御が実行されている時に、トルク指令値τ２ｒｅｆ１〜τ２ｒｅｆ６をモデルトルク演算部７０１〜７０６に出力する。そして、モデルトルク演算部７０１〜７０６は、取得したトルク指令値τ２ｒｅｆ１〜τ２ｒｅｆ６に応じた負荷トルクの第２計算値である計算値τ２ＦＦ１〜τ２ＦＦ６を求める。そして、モデルトルク演算部７０１〜７０６は、この計算値τ２ＦＦ１〜τ２ＦＦ６を基準値τ２ＦＦ１〜τ２ＦＦ６として第２記憶部７１１〜７１６に記憶させる。なお、位置制御時、フィードバック制御部６０１〜６０６、減算部７３１〜７３６、及び加算部６３１〜６３６は、演算処理を行う必要がないため行わない。
【０１１６】
力制御を実行するときには、フィードバック制御部６０１〜６０６は、トルク指令値τ１ｒｅｆ１〜τ１ｒｅｆ６と測定値τ１〜τ６との差に基づき、フィードバック値τＦＢ１〜τＦＢ６を求める。モデルトルク演算部７０１〜７０６は、トルク指令値τ１ｒｅｆ１〜τ１ｒｅｆ６に応じた第１計算値である計算値τ１ＦＦ１〜τ１ＦＦ６を求める。減算部７３１〜７３６は、計算値τ１ＦＦ１〜τ１ＦＦ６から基準値τ２ＦＦ１〜τ２ＦＦ６を減算して、フィードフォワード値τＦＦ１〜τＦＦ６を求める。そして、加算部６３１〜６３６は、フィードバック値τＦＢ１〜τＦＢ６に、オフセット値τｏｆｆｓｅｔ１〜τｏｆｆｓｅｔ６、及びフィードフォワード値τＦＦ１〜τＦＦ６を加算することで、トルク指令値τＦ１〜τＦ６を求める。
【０１１７】
これにより、力制御モードに切り替え直後であっても、トルク指令値τＦ１〜τＦ６、即ち図１７に示すトルク指令値τＭ１〜τＭ６は、時間変化に対して滑らかに推移する。その結果、制御モード切り替え時にマニピュレータ２００のハンド２５２が振動するのをさらに低減することができる。
【０１１８】
図２２は、第３実施形態における実験結果を示すグラフである。なお、図２２のグラフには、第２実施形態における実験結果も図示している。図２２には、関節Ｊ１のトルク指令値τＭ１を図示している。第１実施形態におけるトルク指令値τＭ１の時間変化率は、位置制御モードにおいては一定であるが、位置制御モードから力制御モードに切り替えた直後、一定とならずに変化する。一方、第２実施形態におけるトルク指令値τＭ１の時間変化率は、位置制御モードから力制御モードに切り替えた直後であっても、一定となる。
【０１１９】
第３実施形態によれば、位置制御モードから力制御モードに制御モードを切り替える際において、制御モードの切替時のトルク指令値τＭ１〜τＭ６を連続かつ滑らかにすることができる。その結果、制御モードの切替時に、マニピュレータ２００のハンド２５２の振動をさらに抑制でき、高精度かつ高応答な力制御が可能となる。
【０１２０】
［第４実施形態］
第４実施形態のロボット装置について説明する。図２３は、第４実施形態に係るロボット装置の概略構成を示す斜視図である。第４実施形態のロボット装置１００Ｂにおいて、第１、第２又は第３実施形態のロボット装置と同様の構成については、同一符号を付して詳細な説明は省略する。図２３に示すように、ロボット装置１００Ｂは、第１実施形態と同様の構成のマニピュレータ２００と、マニピュレータ２００を制御する制御部５００Ｂを有する制御装置１０００Ｂと、を備えている。また、ロボット装置１００Ｂは、制御装置１０００Ｂに教示データを送信する教示装置としての教示ペンダント６００を備えている。
【０１２１】
制御部５００Ｂは、第１実施形態と同様の制御ユニット３００と、第２又は第３実施形態と同様の制御ユニット４００Ａと、を有する。即ち、制御ユニット３００は、第１実施形態において説明した処理を行い、制御ユニット４００Ａは、第２又は第３実施形態において説明した処理を行う。
【０１２２】
制御ユニット３００は、制御ユニット４００Ａがマニピュレータ２００の位置を制御しているときに、補正値τｃｏｍｐ１〜τｃｏｍｐ６（図７）を求めておく。制御ユニット３００は、制御ユニット４００Ａにマニピュレータ２００の推力を制御させる際に、教示データである力教示データ５０１及び位置教示データ５０２に基づく仮の指令値であるトルク指令値τＦｄ１〜τＦｄ６を求める。そして、制御ユニット３００は、トルク指令値τＦｄ１〜τＦｄ６を補正値τｃｏｍｐ１〜τｃｏｍｐ６で補正することでトルク指令値τｒｅｆ１〜τｒｅｆ６を求めて、制御ユニット４００Ａにトルク指令値τｒｅｆ１〜τｒｅｆ６を出力する。
【０１２３】
一方、制御ユニット４００Ａは、マニピュレータ２００の位置を制御しているときに、図１８に示すように、オフセット値τｏｆｆｓｅｔ１〜τｏｆｆｓｅｔ６を求めておく。制御ユニット４００Ａは、マニピュレータ２００の推力を制御する際に、フィードバック値τＦＢ１〜τＦＢ６及びフィードフォワード値τＦＦ１〜τＦＦ６を求める。そして、制御ユニット４００Ａは、フィードバック値τＦＢ１〜τＦＢ６に、オフセット値τｏｆｆｓｅｔ１〜τｏｆｆｓｅｔ６と、フィードフォワード値τＦＦ１〜τＦＦ６と、を加算して、トルク指令値τＦ１〜τＦ６を生成する。制御ユニット４００Ａは、このトルク指令値τＦ１〜τＦ６に基づいてマニピュレータ２００の推力を制御する。
【０１２４】
このように、第４実施形態では、制御部５００Ｂが、第１実施形態で説明した制御処理と、第２又は第３実施形態で説明した制御処理とを組み合わせて制御処理を実行することで、マニピュレータ２００の制御性が更に向上する。
【０１２５】
なお、本発明は、以上説明した実施形態に限定されるものではなく、本発明の技術的思想内で多くの変形が可能である。また、実施形態に記載された効果は、本発明から生じる最も好適な効果を列挙したに過ぎず、本発明による効果は、実施形態に記載されたものに限定されない。
【０１２６】
上述の実施形態では、ロボットアーム２５１が垂直多関節のロボットアームである場合について説明したが、これに限定するものではない。例えば、水平多関節のロボットアーム、パラレルリンクのロボットアーム、直交ロボット等、種々のロボットアームであってもよい。
【０１２７】
（その他の実施例）
本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。
【符号の説明】
【０１２８】
１００…ロボット装置
２００…マニピュレータ
３００…制御ユニット（第１制御ユニット）
４００…制御ユニット（第２制御ユニット）
５００…制御部
１０００…制御装置
【図１】
2020151824.tif 000006
【図２】
2020151824.tif 000007
【図３】
2020151824.tif 000008
【図４】
2020151824.tif 000009
【図５】
2020151824.tif 000010
【図６】
2020151824.tif 000011
【図７】
2020151824.tif 000012
【図８】
2020151824.tif 000013
【図９】
2020151824.tif 000014
【図１０】
2020151824.tif 000015
【図１１】
2020151824.tif 000016
【図１２】
2020151824.tif 000017
【図１３】
2020151824.tif 000018
【図１４】
2020151824.tif 000019
【図１５】
2020151824.tif 000020
【図１６】
2020151824.tif 000021
【図１７】
2020151824.tif 000022
【図１８】
2020151824.tif 000023
【図１９】
2020151824.tif 000024
【図２０】
2020151824.tif 000025
【図２１】
2020151824.tif 000026
【図２２】
2020151824.tif 000027
【図２３】
2020151824.tif 000028