JP7391523B2 - 制御装置、ロボットシステム、制御方法、物品の製造方法、プログラム、及び記録媒体 - Google Patents

Description

本発明は、マニピュレータの制御に関する。

産業用ロボットにおけるマニピュレータの制御として、マニピュレータの位置を制御する位置制御と、マニピュレータの推力を制御する力制御とが知られている。例えば、物品の一例として組立品を製造する際、マニピュレータの先端部にワークを保持させておき、位置制御によりマニピュレータの先端部を目標位置に位置決めした後、力制御によりワークを組み付け対象物に組み付けることが行われている。特許文献１には、マニピュレータの力制御として、トルク指令値と測定されたトルク値との差に基づいてマニピュレータの関節をフィードバック制御する技術が開示されている。

特開２０１７－５６５４９号公報

しかし、マニピュレータを力制御しているときにマニピュレータが振動するなど、マニピュレータの挙動が不安定になることがあった。マニピュレータに精密な作業を行わせる場合、マニピュレータの挙動が不安定になると精密な作業に支障をきたすことがあるため、改善が求められていた。

本発明は、マニピュレータの制御性を向上させることを目的とする。

本開示の第１態様は、ロボットを制御可能な制御部を備え、前記制御部は、前記ロボットの位置に関する位置測定値と、前記ロボットのモデルと、に基づき前記ロボットの力に関する力計算値を取得し、前記力計算値と、前記ロボットの力に関する力測定値と、に基づき補正値を取得し、前記ロボットの目標位置と、前記モデルと、前記ロボットの目標力に基づき、前記ロボットを制御する前記ロボットの力に関する第１力指令値を取得し、前記第１力指令値を前記補正値に基づき更新する、ことを特徴とする制御装置である。
本開示の第２態様は、ロボットの制御方法であって、前記ロボットの位置に関する位置測定値と、前記ロボットのモデルと、に基づき前記ロボットの力に関する力計算値を取得し、前記力計算値と、前記ロボットの力に関する力測定値と、に基づき補正値を取得し、前記ロボットの目標位置と、前記モデルと、前記ロボットの目標力に基づき、前記ロボットを制御する前記ロボットの力に関する第１力指令値を取得し、前記第１力指令値を前記補正値に基づき更新する、ことを特徴とする制御方法である。

本発明によれば、マニピュレータの制御性が向上する。

第１実施形態に係るロボット装置の概略構成を示す斜視図である。 第１実施形態に係る制御装置の構成を示すブロック図である。 第１実施形態に係る制御装置の構成を示すブロック図である。 第１実施形態に係るロボット装置の制御システムを示す制御ブロック図である。 第１実施形態における制御装置の処理のフローチャートである。 第１実施形態における制御装置の処理のフローチャートである。 第１実施形態に係る第１制御ユニットのブロック図である。 第１実施形態におけるロボットアームの概略図である。 （ａ）は、第１実施形態における現実のマニピュレータの動力学パラメータの一例を示す図である。（ｂ）は、第１実施形態におけるロボットモデルの動力学パラメータの一例を示す図である。 （ａ）及び（ｂ）は、第１実施形態におけるマニピュレータによる組付作業を説明するためのマニピュレータの模式図である。 （ａ）は、第１実施形態における教示点のパラメータを示す図である。（ｂ）は、第１実施形態における力制御の機械インピーダンスのパラメータを示す図である。 第１実施形態に係る物品の製造方法を説明するフローチャートである。 （ａ）及び（ｂ）は、第１実施形態における実験結果を示すグラフである。（ｃ）は、比較例における実験結果を示すグラフである。 第２実施形態に係るロボット装置の概略構成を示す斜視図である。 第２実施形態に係る制御装置の構成を示すブロック図である。 第２実施形態に係る制御装置の構成を示すブロック図である。 第２実施形態に係るロボット装置の制御システムを示す制御ブロック図である。 第２実施形態に係る第２制御ユニットのブロック図である。 第２実施形態に係る物品の製造方法を説明するフローチャートである。 第２実施形態における実験結果を示すグラフである。 第３実施形態に係る第２制御ユニットを示すブロック図である。 第３実施形態における実験結果を示すグラフである。 第４実施形態に係るロボット装置の概略構成を示す斜視図である。

以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。
［第１実施形態］
図１は、第１実施形態に係るロボット装置の概略構成を示す斜視図である。図１に示すように、ロボット装置１００は、マニピュレータ２００と、マニピュレータ２００の位置又は推力を制御する制御部５００を有する制御装置１０００と、を備えている。また、ロボット装置１００は、制御装置１０００に教示データを送信する教示装置としての教示ペンダント６００を備えている。教示ペンダント６００は、操作者が操作するものであり、マニピュレータ２００や制御装置１０００の動作を指定するのに用いる。

マニピュレータ２００は、ロボットアーム２５１と、マニピュレータ２００の先端部を構成する、エンドエフェクタの一例であるハンド２５２と、を備える。ロボットアーム２５１は、本実施形態では垂直多関節のロボットアームである。

ロボットアーム２５１の先端部には、ハンド２５２が取り付けられている。以下、エンドエフェクタがハンド２５２である場合について説明するが、これに限定するものではなく、ツール等であってもよい。ロボットアーム２５１の基端部は、台座Ｂに固定されている。ハンド２５２は、部品やツール等のワークを把持するものである。

マニピュレータ２００、即ちロボットアーム２５１は、１つ以上の関節を有していればよく、本実施形態では、複数の関節、例えば６つの関節Ｊ_１～Ｊ_６を有している。ロボットアーム２５１は、各関節Ｊ_１～Ｊ_６を各軸Ａ_１～Ａ_６まわりに回転駆動するための複数（６つ）のサーボ機構部２０_１～２０_６を有している。

なお、本実施形態では、関節Ｊ_１～Ｊ_６は、回転関節であるが、直動関節であってもよい。関節が回転関節の場合、関節の位置とは、関節の角度、即ち回転位置を表す。関節が直動関節の場合、関節の位置とは、直動位置を表す。また、関節が回転関節の場合、関節の推力とは、関節の推進トルク、即ち出力トルクを表す。関節が直動関節の場合、関節の推力とは、関節の直動推力を表す。

ロボットアーム２５１は、複数のリンク２１０_０～２１０_６を有する。リンク２１０_０～２１０_６は、関節Ｊ１～Ｊ６で回転可能に連結されている。リンク２１０_０～２１０_６は、基端側から先端側に向かって、順に直列に連結されている。ロボットアーム２５１は、マニピュレータ２００の先端部、即ちハンド２５２を、可動範囲内の任意の位置に移動させることができる。

マニピュレータ２００の位置、即ち姿勢は、座標系Ｔｏで表現することができる。座標系Ｔｏは、ロボットアーム２５１の基端、即ち台座Ｂに固定した座標系を表し、座標系Ｔｅはマニピュレータ２００の先端部、即ちハンド２５２と連動する座標系を表す。

制御部５００は、第１制御ユニットである制御ユニット３００と、第２制御ユニットである制御ユニット４００と、を有する。制御ユニット３００は、ロボット装置１００を統括的に制御するものである。制御ユニット４００は、制御ユニット３００の指令に基づいて、マニピュレータ２００の位置又は推力を制御するものである。制御ユニット４００は、マニピュレータ２００の各サーボ機構部２０_１～２０_６を制御するサーボ制御装置である。

制御部５００について具体的に説明する。図２は、第１実施形態に係る制御装置の構成を示すブロック図である。制御部５００は、上述したように、制御ユニット３００，４００を有する。図２には、制御ユニット３００のハードウェア構成について詳細に図示している。

制御ユニット３００は、コンピュータで構成されており、処理部としてのＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）３０１を備える。また、制御ユニット３００は、記憶部として、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）３０２、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）３０３、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）３０４を備えている。また、制御ユニット３００は、記録ディスクドライブ３０５、及びインタフェース３０６～３０９を備えている。

ＣＰＵ３０１、ＲＯＭ３０２、ＲＡＭ３０３、ＨＤＤ３０４、記録ディスクドライブ３０５、インタフェース３０６～３０９は、相互に通信可能にバス３１０で接続されている。ＲＯＭ３０２には、ＣＰＵ３０１に後述する制御方法の処理を実行させるためのプログラム３３０が格納されている。ＣＰＵ３０１は、ＲＯＭ３０２に記録（格納）されたプログラム３３０に基づいて、後述する制御方法を実行する。ＲＡＭ３０３は、ＣＰＵ３０１の演算処理結果等、各種データを一時的に記憶する記憶装置である。

ＨＤＤ３０４は、ＣＰＵ３０１の演算処理結果や外部から取得した各種データ等を記憶可能な記憶装置である。記録ディスクドライブ３０５は、記録ディスク３３１に記録された各種データやプログラム等を読み出すことができる。

教示ペンダント６００は、インタフェース３０６に接続されている。ＣＰＵ３０１は、インタフェース３０６及びバス３１０を介して教示ペンダント６００からマニピュレータ２００の教示データを取得する。

制御ユニット４００は、インタフェース３０９に接続されている。ＣＰＵ３０１は、指令値のデータを所定の制御周期でバス３１０及びインタフェース３０９を介して制御ユニット４００に出力する。

インタフェース３０７には、モニタ３２１が接続されており、モニタ３２１には、ＣＰＵ３０１の制御の下、各種画像が表示される。インタフェース３０８は、書き換え可能な不揮発性メモリや外付けＨＤＤ等の記憶装置である外部記憶装置３２２が接続可能に構成されている。外部記憶装置３２２には、教示データや、マニピュレータ２００のモデルデータが格納可能である。

なお、第１実施形態では、コンピュータによって読取可能な記録媒体がＲＯＭ３０２であり、ＲＯＭ３０２にプログラム３３０が記録されているが、これに限定するものではない。プログラム３３０は、コンピュータによって読取可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。プログラム３３０を供給するための記録媒体としては、例えば、フレキシブルディスク、ハードディスク、光ディスク、光磁気ディスク、磁気テープ、不揮発性メモリ等を用いることができる。

図３は、第１実施形態に係る制御装置の構成を示すブロック図である。図３には、制御ユニット４００のハードウェア構成について詳細に図示している。

制御ユニット４００は、コンピュータで構成されている。制御ユニット４００は、処理部であるＣＰＵ４０１と、記憶部であるＲＯＭ４０２及びＲＡＭ４０３と、インタフェース４０６～４０９と、を備えている。ＣＰＵ４０１、ＲＯＭ４０２、ＲＡＭ４０３、インタフェース４０６～４０９は、相互に通信可能にバス４１０で接続されている。ＲＯＭ４０２には、ＣＰＵ４０１に、後述する制御方法の処理を実行させるためのプログラム４３０が格納されている。

インタフェース４０６には、後述するモータ２１_１～２１_６が接続されている。インタフェース４０７には、後述する位置センサ２３_１～２３_６が接続されている。インタフェース４０８には、後述するトルクセンサ２４_１～２４_６が接続されている。インタフェース４０９には、制御ユニット３００が接続されている。

なお、第１実施形態では、コンピュータによって読取可能な記録媒体がＲＯＭ４０２であり、ＲＯＭ４０２にプログラム４３０が記録されているが、これに限定するものではない。プログラム４３０は、コンピュータによって読取可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。プログラム４３０を供給するための記録媒体としては、例えば、フレキシブルディスク、ハードディスク、光ディスク、光磁気ディスク、磁気テープ、不揮発性メモリ等を用いることができる。

図４は、第１実施形態に係るロボット装置の制御システムを示す制御ブロック図である。図２に示す制御ユニット３００のＣＰＵ３０１は、プログラム３３０を実行することにより、図４に示すトルク指令値生成部５０４及び位置指令値生成部５０５として機能する。図３に示す制御ユニット４００のＣＰＵ４０１は、プログラム４３０を実行することにより、図４に示すトルク制御部５１_１～５１_６、位置制御部５２_１～５２_６、切替部５３_１～５３_６、及びモータ制御部５４_１～５４_６として機能する。サーボ機構部２０_１～２０_６は、モータ２１_１～２１_６、減速機２２_１～２２_６、位置センサ２３_１～２３_６、及びトルクセンサ２４_１～２４_６を有する。

各モータ２１_１～２１_６は、マニピュレータ２００の各関節Ｊ_１～Ｊ_６を駆動する駆動源の一例であり、例えば電動モータである。各減速機２２_１～２２_６は、各モータ２１_１～２１_６の回転軸の回転を減速して出力するものであり、例えば波動歯車減速機である。各関節Ｊ_１～Ｊ_６で連結された一対のリンクのうち、一方のリンクに対して他方のリンクを、各モータ２１_１～２１_６、及び各減速機２２_１～２２_６によって駆動することができる。なお、モータの回転軸が減速機の入力軸に直接連結されていてもよいが、モータと減速機との間に、ベルト機構などの不図示の伝達機構が配置されていてもよい。

各位置センサ２３_１～２３_６は、各関節Ｊ_１～Ｊ_６の位置（角度）に応じた信号を出力するセンサであり、例えばエンコーダである。エンコーダは、ロータリエンコーダであるのが好ましい。各位置センサ２３_１～２３_６は、各減速機２２_１～２２_６の入力軸側、即ち各モータ２１_１～２１_６に設けられ、各モータ２１_１～２１_６の回転軸の位置（角度）に応じた信号を、制御ユニット４００に出力する。制御ユニット４００は、各位置センサ２３_１～２３_６から取得した信号に基づき、各モータ２１_１～２１_６の回転軸の位置（角度）の測定値θ_１～θ_６を求め、各測定値θ_１～θ_６から各関節Ｊ_１～Ｊ_６の位置（角度）の測定値ｑ_１～ｑ_６を求める。本実施形態では、減速機２２_１～２２_６の減速比に基づき、測定値θ_１～θ_６を測定値ｑ_１～ｑ_６に変換する。制御ユニット４００は、各関節Ｊ_１～Ｊ_６の位置（角度）の測定値ｑ_１～ｑ_６を所定の制御周期で制御ユニット３００に出力する。

各トルクセンサ２４_１～２４_６は、マニピュレータ２００に作用する負荷の一例として各関節Ｊ_１～Ｊ_６に作用する負荷トルクに応じた信号を、制御ユニット４００に出力する。制御ユニット４００は、各トルクセンサ２４_１～２４_６から取得した信号に基づき、各関節Ｊ_１～Ｊ_６にかかる負荷トルクの測定値τ_１～τ_６を求める。制御ユニット４００は、各関節Ｊ_１～Ｊ_６の負荷トルクの測定値τ_１～τ_６を所定の制御周期で制御ユニット３００に出力する。

外部記憶装置３２２には、ユーザが予め作成しておいた教示データとしての力教示データ５０１及び位置教示データ５０２と、マニピュレータ２００のモデルデータであるロボットモデル５０３と、が格納されている。なお、これらデータが格納される記憶装置は、外部記憶装置３２２に限定するものではない。例えば、これらデータは、ＨＤＤ３０４、又は教示ペンダント６００に含まれる不図示の記憶装置に格納されていてもよい。

力教示データ５０１には、目標力Ｆ_ｄが含まれている。位置教示データ５０２には、目標位置Ｐ_ｄが含まれている。目標力Ｆ_ｄは、マニピュレータ２００のハンド２５２にかかる力の目標値（目標力）であり、操作者が教示ペンダント６００を用いて設定することができる。目標位置Ｐ_ｄは、マニピュレータ２００のハンド２５２の位置の目標値（目標位置）であり、操作者が教示ペンダント６００を用いて設定することができる。制御ユニット３００は、力教示データ５０１から目標力Ｆ_ｄを読み出すことができ、位置教示データ５０２から目標位置Ｐ_ｄを読み出すことができる。

ロボットモデル５０３は、マニピュレータ２００を構成する要素の質量データ、重心位置データ、重心まわりの慣性モーメントデータなどの設計データを含む、ユーザが作成した、仮想的なモデルである。

制御ユニット３００は、予め設定されているロボットプログラムに従って、マニピュレータ２００の位置を制御する位置制御モードと、マニピュレータ２００の推力を制御する力制御モードとを切り替えて、制御ユニット４００に指令を送る。即ち、制御ユニット３００は、位置制御モードに切り替わっているときには位置指令値生成部５０５として機能して位置指令値を送り、力制御モードに切り替わっているときには、トルク指令値生成部５０４として機能してトルク指令値を送る。

位置指令値生成部５０５は、目標位置Ｐ_ｄに基づいて、関節Ｊ_１～Ｊ_６に対応する位置指令値ｑ_ｒｅｆ１～ｑ_ｒｅｆ６を生成する。具体的には、位置指令値生成部５０５は、ロボットモデル５０３を用いた逆運動学計算により、目標位置Ｐ_ｄを関節Ｊ_１～Ｊ_６に対応する位置指令値ｑ_ｒｅｆ１～ｑ_ｒｅｆ６に変換する。位置指令値生成部５０５は、生成した位置指令値ｑ_ｒｅｆ１～ｑ_ｒｅｆ６を、制御ユニット４００の位置制御部５２_１～５２_６に出力する。位置制御部５２_１～５２_６は、測定値ｑ_１～ｑ_６と位置指令値ｑ_ｒｅｆ１～ｑ_ｒｅｆ６との差に基づくフィードバック演算、例えばＰＩＤ演算により、トルク指令値τ_ｑ１～τ_ｑ６を生成する。位置制御部５２_１～５２_６は、トルク指令値τ_ｑ１～τ_ｑ６を切替部５３_１～５３_６に出力する。

トルク指令値生成部５０４は、目標力Ｆ_ｄ、目標位置Ｐ_ｄ、測定値τ_１～τ_６、及び測定値ｑ_１～ｑ_６に基づいて、関節Ｊ_１～Ｊ_６に対応するトルク指令値τ_ｒｅｆ１～τ_ｒｅｆ６を生成する。トルク指令値生成部５０４の具体的な処理は、後述する。トルク指令値生成部５０４は、生成したトルク指令値τ_ｒｅｆ１～τ_ｒｅｆ６を、制御ユニット４００のトルク制御部５１_１～５１_６に出力する。トルク制御部５１_１～５１_６は、負荷トルクの測定値τ_１～τ_６とトルク指令値τ_ｒｅｆ１～τ_ｒｅｆ６との差に基づくフィードバック演算、例えばＰＩＤ演算により、トルク指令値τ_Ｆ１～τ_Ｆ６を生成する。トルク制御部５１_１～５１_６は、トルク指令値τ_Ｆ１～τ_Ｆ６を切替部５３_１～５３_６に出力する。

切替部５３_１～５３_６は、トルク指令値τ_Ｆ１～τ_Ｆ６の入力を受けた場合は、入力を受けたトルク指令値τ_Ｆ１～τ_Ｆ６をトルク指令値τ_Ｍ１～τ_Ｍ６としてモータ制御部５４_１～５４_６に出力する。また、切替部５３_１～５３_６は、トルク指令値τ_ｑ１～τ_ｑ６の入力を受けた場合は、入力を受けたトルク指令値τ_ｑ１～τ_ｑ６をトルク指令値τ_Ｍ１～τ_Ｍ６としてモータ制御部５４_１～５４_６に出力する。

モータ制御部５４_１～５４_６は、トルク指令値τ_Ｍ１～τ_Ｍ６に応じた電流Ｃｕｒ_１～Ｃｕｒ_６をモータ２１_１～２１_６に通電する。

以上、制御ユニット４００は、制御ユニット３００から位置指令値ｑ_ｒｅｆ１～ｑ_ｒｅｆ６を取得した場合、位置指令値ｑ_ｒｅｆ１～ｑ_ｒｅｆ６と測定値ｑ_１～ｑ_６との差に基づき、マニピュレータ２００の位置を制御する。また制御ユニット４００は、制御ユニット３００からトルク指令値τ_ｒｅｆ１～τ_ｒｅｆ６を取得した場合、トルク指令値τ_ｒｅｆ１～τ_ｒｅｆ６と測定値τ_１～τ_６との差に基づき、マニピュレータ２００の推力、即ち関節Ｊ_１～Ｊ_６の出力トルクを制御する。

力制御モードにおけるマニピュレータ２００の力制御、即ちマニピュレータ２００の関節のトルク制御について説明する。図５は、第１実施形態において制御部５００がマニピュレータ２００を力制御する処理のフローチャートである。まず、トルク指令値生成部５０４は、目標力Ｆ_ｄ及び目標位置Ｐ_ｄを外部記憶装置３２２から取得する（Ｓ１）。トルク指令値生成部５０４は、目標力Ｆ_ｄ、目標位置Ｐ_ｄ、負荷トルクの測定値τ_１～τ_６、位置の測定値ｑ_１～ｑ_６に基づき、ロボットモデル５０３を用いてトルク指令値τ_ｒｅｆ１～τ_ｒｅｆ６を算出する（Ｓ２）。トルク制御部５１_１～５１_６は、トルク指令値τ_ｒｅｆ１～τ_ｒｅｆ６と負荷トルクの測定値τ_１～τ_６との差に基づくＰＩＤ演算により、トルク指令値τ_Ｆ１～τ_Ｆ６を算出する（Ｓ３）。切替部５３_１～５３_６は、モータ制御部５４_１～５４_６にトルク指令値τ_Ｆ１～τ_Ｆ６をトルク指令値τ_Ｍ１～τ_Ｍ６として出力する。モータ制御部５４_１～５４_６は、トルク指令値τ_Ｍ１～τ_Ｍ６に応じてモータ２１_１～２１_６を通電制御する（Ｓ４）。モータ２１_１～２１_６は、通電に応じて関節Ｊ_１～Ｊ_６に出力トルクを発生する（Ｓ５）。制御ユニット３００，４００は、負荷トルクの測定値τ_１～τ_６及び関節の位置の測定値ｑ_１～ｑ_６を取得する（Ｓ６）。制御ユニット３００は、駆動が終了したか否かを判定し（Ｓ７）、終了していない場合は（Ｓ７：Ｎｏ）、ステップＳ２～Ｓ６を繰り返す。制御部５００は、駆動が終了した場合（Ｓ７：Ｙｅｓ）、制御処理を終了する。以上のフローチャートに従ってモータ２１_１～２１_６を駆動することで、マニピュレータ２００のハンド２５２にかかる力Ｆを所望の目標力Ｆ_ｄに制御することが可能である。なお、図５に示すフローチャートの順番に限定するものではなく、他の順番でも力制御は可能である。

次に、位置制御モードにおけるマニピュレータ２００の位置制御、即ちマニピュレータ２００のハンド２５２の位置制御について説明する。図６は、第１実施形態において制御部５００がマニピュレータ２００を位置制御する処理のフローチャートである。まず、位置指令値生成部５０５は、目標位置Ｐ_ｄを外部記憶装置３２２から取得する（Ｓ１１）。目標位置Ｐ_ｄは、マニピュレータ２００を位置決めする目標位置である。位置指令値生成部５０５は、目標位置Ｐ_ｄに基づき、ロボットモデル５０３を用いて関節Ｊ_１～Ｊ_６の位置指令値ｑ_ｒｅｆ１～ｑ_ｒｅｆ６を算出する（Ｓ１２）。位置制御部５２_１～５２_６は、位置指令値ｑ_ｒｅｆ１～ｑ_ｒｅｆ６と関節の位置の測定値ｑ_１～ｑ_６との差に基づくＰＩＤ演算により、トルク指令値τ_ｑ１～τ_ｑ６を算出する（Ｓ１３）。なお、ステップＳ１３においては、測定値ｑ_１～ｑ_６の代わりに測定値θ_１～θ_６を用いてもよい。この場合、位置指令値ｑ_ｒｅｆ１～ｑ_ｒｅｆ６は、減速機２２_１～２２_６の減速比で位置指令値θ_ｒｅｆ１～θ_ｒｅｆ６に変換すればよい。切替部５３_１～５３_６は、モータ制御部５４_１～５４_６にトルク指令値τ_ｑ１～τ_ｑ６をトルク指令値τ_Ｍ１～τ_Ｍ６として出力する。モータ制御部５４_１～５４_６は、トルク指令値τ_Ｍ１～τ_Ｍ６に応じてモータ２１_１～２１_６を通電制御する（Ｓ１４）。モータ２１_１～２１_６は、通電に応じて関節Ｊ_１～Ｊ_６に出力トルクを発生する（Ｓ１５）。制御ユニット３００，４００は、関節の位置の測定値ｑ_１～ｑ_６を取得する（Ｓ１６）。制御ユニット３００は、駆動が終了したか否かを判定し（Ｓ１７）、終了していない場合は（Ｓ１７：Ｎｏ）、ステップＳ１２～Ｓ１６を繰り返す。制御部５００は、駆動が終了した場合（Ｓ１７：Ｙｅｓ）、制御処理を終了する。以上のフローチャートに従ってモータ２１_１～２１_６を駆動することで、マニピュレータ２００のハンド２５２の位置を所望の目標位置Ｐ_ｄに制御することが可能である。なお、図６に示すフローチャートの順番に限定するものではなく、他の順番でも位置制御は可能である。

ところで、マニピュレータ２００の力学モデルであるロボットモデル５０３と実機とは必ずしも一致せず、誤差が生じる。この誤差をモデル化誤差という。このモデル化誤差は、マニピュレータ２００の経年劣化などでも変化する。また、トルクセンサ２４_１～２４_６の測定値τ_１～τ_６には、マニピュレータが置かれている環境（例えば温度）の変化の影響で、オフセット誤差が重畳する。これらの誤差が大きいと、マニピュレータを力制御しているときにマニピュレータが振動するなど、マニピュレータの挙動が不安定になることがある。モデル化誤差とオフセット誤差とを求め、これら誤差に基づいて、モデル及び測定値を校正することも考えられるが、校正の作業を度々行わなければならない。マニピュレータ２００に連続運転で作業を行わせる場合には、校正作業の時間の分、サイクルタイムが増加してしまう。

そこで、本実施形態では、組立作業などの生産作業においてマニピュレータ２００を動作させている制御の中で、モデル化誤差及びオフセット誤差をキャンセルさせる。図７は、第１実施形態に係る第１制御ユニットの処理の一部を示すブロック図である。図７には、トルク指令値生成部５０４の処理について図示している。トルク指令値生成部５０４は、インピーダンス制御部６０１、動力学計算部６０２、指令値補正部６０３、補正値計算部６０４として機能し、例えば図２に示すＲＡＭ３０３が補正値記憶部６０５として機能する。以下、各部の機能について説明する。

動力学計算部６０２は、制御ユニット４００がマニピュレータ２００の位置を制御しているときに、マニピュレータ２００の位置の測定値である関節Ｊ_１～Ｊ_６の位置の測定値ｑ_１～ｑ_６を取得する。本実施形態では、動力学計算部６０２は、力制御を開始する位置を目標位置Ｐ_ｄとし、マニピュレータ２００を目標位置Ｐ_ｄに位置決めした際に、測定値ｑ_１～ｑ_６を取得する。動力学計算部６０２は、取得したこれらの位置情報を微分して、速度、及び加速度を求める。動力学計算部６０２は、ロボットモデル５０３と関節Ｊ_１～Ｊ_６の位置、速度、及び加速度の情報に基づいて、公知の動力学計算手法であるニュートン・オイラー法により、関節Ｊ_１～Ｊ_６にかかる負荷トルクの計算値τ＾_１～τ＾_６を算出する。即ち、動力学計算部６０２は、マニピュレータ２００の位置を制御しているときに、ロボットモデル５０３を用いた動力学の計算により、マニピュレータ２００に生じる負荷としての負荷トルクの計算値τ＾_１～τ＾_６を求める。計算値τ＾_１～τ＾_６には、モデル化誤差が含まれる。なお、動力学の計算は、これに限定するものではなく、例えばラグランジュの定式化による導出方法などでもよい。

補正値計算部６０４は、制御ユニット４００がマニピュレータ２００の位置を制御しているときに、マニピュレータ２００に生じる負荷の第１測定値として、負荷トルクの測定値τ_１～τ_６を取得する。本実施形態では、補正値計算部６０４は、マニピュレータ２００を目標位置Ｐ_ｄに位置決めした際に、測定値τ_１～τ_６を取得する。測定値τ_１～τ_６には、オフセット誤差が含まれている。補正値計算部６０４は、測定値τ_１～τ_６と計算値τ＾_１～τ＾_６との差に基づいて補正値τ_{ｃｏｍｐ１}～τ_{ｃｏｍｐ６}を求める。具体的には、補正値計算部６０４は、位置制御モードから力制御モードに切り替わる直前に、計算値τ＾_１～τ＾_６から測定値τ_１～τ_６を減算することで、補正値τ_{ｃｏｍｐ１}～τ_{ｃｏｍｐ６}を求める。例えば補正値計算部６０４は、位置制御モード時に、計算値τ＾_１～τ＾_６から測定値τ_１～τ_６を減算する処理を、所定の周期で行い、位置制御モードから力制御モードに切り替わったときに、その直前の減算値を補正値τ_{ｃｏｍｐ１}～τ_{ｃｏｍｐ６}とする。補正値計算部６０４は、補正値τ_{ｃｏｍｐ１}～τ_{ｃｏｍｐ６}を補正値記憶部６０５に記憶させておく。なお、補正値τ_{ｃｏｍｐ１}～τ_{ｃｏｍｐ６}には、モデル化誤差とオフセット誤差が含まれている。

インピーダンス制御部６０１は、マニピュレータ２００のハンド２５２に所望の機械インピーダンスを持たせるよう、ロボットモデル５０３に基づく関節Ｊ_１～Ｊ_６のトルク指令値τ_Ｆｄ１～τ_Ｆｄ６を、以下の式（１）に従って算出する。式（１）は、インピーダンスの制御式である。機械インピーダンスは、例えば剛性インピーダンス、粘性インピーダンス、及び力インピーダンスである。トルク指令値τ_Ｆｄ１～τ_Ｆｄ６は、補正前の仮の指令値であり、モデル化誤差が含まれている。

式（１）中、Ｍは６×６の慣性行列、Ｃは６×１のコリオリ力・遠心力のベクトル、Ｇは６×１の重力負荷トルクのベクトルである。Ｋ_ｄは、所望の機械インピーダンスの剛性の値、Ｄ_ｄは所望の機械インピーダンスの粘性の値を表わし、それぞれ６×６の行列である。Ｊａｃは、関節Ｊ_１～Ｊ_６の速度と、座標系Ｔｏ（図１）基準で表現されたハンド２５２の速度とを関連付ける６×６のヤコビ行列である。なお、ヤコビ行列Ｊａｃの添え字Ｔは行列の転置を表わしている。

このように、インピーダンス制御部６０１は、制御ユニット４００がマニピュレータ２００の推力を制御する際に、式（１）を用いて、教示データ５０１，５０２に基づく仮の指令値であるトルク指令値τ_Ｆｄ１～τ_Ｆｄ６を求める。動力学計算部６０２は、Ｍ、Ｃ、Ｇを、ロボットモデル５０３と、関節Ｊ_１～Ｊ_６の位置及び速度に基づいて、ニュートン・オイラー法で計算する。

指令値補正部６０３は、トルク指令値τ_Ｆｄ１～τ_Ｆｄ６を補正値τ_{ｃｏｍｐ１}～τ_{ｃｏｍｐ６}で補正し、トルク制御部５１_１～５１_６に出力する指令値であるトルク指令値τ_ｒｅｆ１～τ_ｒｅｆ６を求める。具体的には、指令値補正部６０３は、補正値記憶部６０５に記憶されている補正値τ_{ｃｏｍｐ１}～τ_{ｃｏｍｐ６}をインピーダンス制御部６０１で算出したトルク指令値τ_Ｆｄ１～τ_Ｆｄ６から減算することで、トルク指令値τ_ｒｅｆ１～τ_ｒｅｆ６を生成する。これにより、マニピュレータ２００の実機とモデルとの差であるモデル化誤差がキャンセルされる。

トルク指令値τ_ｒｅｆ１～τ_ｒｅｆ６には、オフセット誤差が含まれている。図４に示すトルク制御部５１_１～５１_６は、制御ユニット４００がマニピュレータ２００の推力を制御する際に、マニピュレータ２００に生じる負荷の第２測定値である関節Ｊ_１～Ｊ_６の負荷トルクの測定値τ_１～τ_６を取得する。この測定値τ_１～τ_６には、オフセット誤差が含まれている。トルク制御部５１_１～５１_６は、トルク指令値τ_ｒｅｆ１～τ_ｒｅｆ６と測定値τ_１～τ_６との差を求め、この差に基づき、フィードバック値であるトルク指令値τ_Ｆ１～τ_Ｆ６を求める。このように、マニピュレータ２００の推力を制御するのに用いられるトルク指令値τ_Ｆ１～τ_Ｆ６において、オフセット誤差がキャンセルされることになる。

以上、本実施形態によれば、マニピュレータ２００の推力の制御中、モデル化誤差とオフセット誤差が共にキャンセルされるので、マニピュレータ２００の制御が安定し、マニピュレータ２００の制御性が向上する。

以下、マニピュレータ２００の制御の実験結果について説明する。図８は、第１実施形態におけるロボットアーム２５１の概略図である。図８中、Ｔ_１～Ｔ_６はリンク２１０_０～２１０_６に固定された座標系である。なお、各関節Ｊ_１～Ｊ_６の回転軸はＺ軸で、回転方向は右手方向である。図８に示したロボットアーム２５１の姿勢を、関節Ｊ_１～Ｊ_６の角度の測定値ｑ_１～ｑ_６が０度の時の姿勢とする。また、リンク２１０_０～２１０_６の寸法は、図８に示す通りである。

図９（ａ）は、第１実施形態における現実のマニピュレータ２００の動力学パラメータの一例を示す図である。図９（ａ）中、ｍ＾は、当該リンクの実際の質量である。ｓ＾_ｘ、ｓ＾_ｙ、ｓ＾_ｚは、それぞれ当該リンクの座標原点から見たＸ軸、Ｙ軸、Ｚ軸方向の実際の重心位置である。Ｉ＾_ｘｘ、Ｉ＾_ｙｙ、Ｉ＾_ｚｚは、それぞれ当該リンクの重心まわりのＸ軸、Ｙ軸、Ｚ軸に対する実際の慣性モーメントである。

図９（ｂ）は、第１実施形態におけるロボットモデル５０３の動力学パラメータの一例を示す図である。ロボットモデル５０３は、ロボットアーム２５１の設計値からＣＡＤで事前に算出し、外部記憶装置３２２に記憶しておく。図９（ｂ）中、ｍは当該リンクの設計上の質量である。ｓ_ｘ、ｓ_ｙ、ｓ_ｚはそれぞれ当該リンクの座標原点から見たＸ軸、Ｙ軸、Ｚ軸方向の設計上の重心位置である。Ｉ_ｘｘ、Ｉ_ｙｙ、Ｉ_ｚｚはそれぞれ当該リンクの重心まわりのＸ軸、Ｙ軸、Ｚ軸に対する設計上の慣性モーメントである。本実施形態では、ロボットモデル５０３の各リンクの質量が、現実の各リンク２１０_１～２１０_６の質量の１割の誤差があるものとした。

図１０（ａ）及び図１０（ｂ）は、第１実施形態におけるマニピュレータ２００による組付作業を説明するためのマニピュレータ２００の模式図である。本実施形態では、ハンド２５２で把持したワークＷ１をワークＷ２に組み付ける組み付け作業を行う場合について説明する。ワークＷ１は、円筒形状の凸部を有する部品であり、ワークＷ２は円筒形状の凹部を有する部品である。図１０（ａ）中、Ｐ１は、マニピュレータ２００のハンド２５２の座標系Ｔｅで示す力制御開始時の教示点であり、Ｐ２は、マニピュレータ２００のハンド２５２の座標系Ｔｅで示す力制御の目標位置を示す教示点である。

図１０（ｂ）は、マニピュレータ２００のハンド２５２の座標系Ｔｅが教示点Ｐ１にあるときのワークＷ１とワークＷ２との位置関係を説明するための拡大図である。ワークＷ２のとば口ＷＡは、面取りされている。ワークＷ１が座標系ＴｏのＸ軸方向に力制御で２０ｍｍ進んだ時、ワークＷ１がとば口ＷＡの内側にあれば、力制御の柔軟性により、ワークＷ１がワークＷ２の内面に倣い、組み付けが完了する。ワークＷ１がとば口ＷＡの内側にあるとは、ワークＷ１の中心軸が座標系ＴｏのＺ軸方向の１９５ｍｍ～２０５ｍｍの範囲内にあることである。このとき、ワークＷ１の傾き、及び座標系ＴｏのＹ軸方向への移動は、組み付け作業には影響がないものとする。

図１１（ａ）は、第１実施形態における教示点のパラメータを示す図である。図１１（ｂ）は、第１実施形態における力制御の機械インピーダンスのパラメータを示す図である。図１１（ａ）に、教示点Ｐ１，Ｐ２の座標系Ｔｏを基準とする位置及び姿勢の値を示す。なお、図１１（ａ）中のα、β、γは、それぞれＺ軸、Ｙ軸、Ｘ軸まわりの回転量を表わす。また、図１１（ｂ）に、機械インピーダンスのパラメータとして剛性Ｋ_ｄ、粘性Ｄｄ、力Ｆ_ｄの値を示す。

図１２は、第１実施形態に係る物品の製造方法を説明するフローチャートである。まず、制御部５００は、マニピュレータ２００の位置制御を開始し、マニピュレータ２００のハンド２５２（座標系Ｔｅ）を、力制御を開始する位置である教示点Ｐ１へ移動させる（Ｓ５１）。

制御部５００は、ロボットモデル５０３と関節Ｊ_１～Ｊ_６の位置、速度、及び加速度の情報に基づいて、現在の関節Ｊ_１～Ｊ_６の負荷トルクの計算値τ＾_１～τ＾_６を算出する（Ｓ５２）。このとき、マニピュレータ２００は必ずしも停止している必要はない。

制御部５００は、第１測定値である、関節Ｊ_１～Ｊ_６の負荷トルクの測定値τ_１～τ_６を取得する（Ｓ５３）。このとき、測定値τ_１～τ_６には、トルクセンサ２４_１～２４_６のオフセット誤差が含まれ、例えばマニピュレータ２００の関節Ｊ_１における測定値τ_１および関節Ｊ_２における測定値τ_２に、－１．０Ｎｍのオフセット誤差があるものとする。

制御部５００は、計算値τ＾_１～τ＾_６から測定値τ_１～τ_６を減算することで、補正値τ_{ｃｏｍｐ１}～τ_{ｃｏｍｐ６}を算出する（Ｓ５４）。このとき、算出されたトルク補正値τ_{ｃｏｍｐ１}～τ_{ｃｏｍｐ６}は、補正値記憶部６０５に記憶される。

制御部５００は、位置制御モードから力制御モードへ切り替え、力制御を開始する（Ｓ５５）。制御部５００は、マニピュレータ２００のハンド２５２に所望の機械インピーダンス（剛性Ｋ_ｄ、粘性Ｄ_ｄ、力Ｆ_ｄ）を持たせるような関節Ｊ１～Ｊ６のトルク指令値τ_Ｆｄ１～τ_Ｆｄ６を式（１）に従って算出する（Ｓ５６）。

制御部５００は、ステップＳ５６で算出したトルク指令値τ_Ｆｄ１～τ_Ｆｄ６から補正値記憶部６０５に記憶されている補正値τ_{ｃｏｍｐ１}～τ_{ｃｏｍｐ６}を減算してトルク指令値τ_ｒｅｆ１～τ_ｒｅｆ６を算出する（Ｓ５７）。制御部５００は、トルク指令値τ_ｒｅｆ１～τ_ｒｅｆ６と、第２測定値である測定値τ_１～τ_６との差に基づくＰＩＤ演算により、トルク指令値τ_Ｆ１～τ_Ｆ６を算出する（Ｓ５８）。

制御部５００は、トルク指令値τ_Ｆ１～τ_Ｆ６をトルク指令値τ_Ｍ１～τ_Ｍ６とし、トルク指令値τ_Ｍ１～τ_Ｍ６に応じてモータ２１_１～２１_６を通電制御する（Ｓ５９）。モータ２１_１～２１_６は、通電に応じて関節Ｊ_１～Ｊ_６に出力トルクを発生する（Ｓ６０）。制御部５００は、負荷トルクの測定値τ_１～τ_６及び関節の位置の測定値ｑ_１～ｑ_６を取得する（Ｓ６１）。制御部５００は、駆動が終了したか否かを判定し（Ｓ６２）、終了していない場合は（Ｓ６２：Ｎｏ）、ステップＳ５６～Ｓ６２を繰り返す。制御部５００は、駆動終了と判断したら（Ｓ６２：Ｙｅｓ）、制御処理を終了する。本実施形態では、ステップＳ５５で力制御を開始してから５秒経過した時点で、制御処理を終了する。

図１３（ａ）、図１３（ｂ）及び図１３（ｃ）は、実験結果を示すグラフである。図１３（ａ）は、第１実施形態において、力制御を行っているときの時間に対するハンド２５２のＸ軸方向の位置を示すグラフである。

インピーダンス制御の目標位置Ｐ_ｄは、図１３（ａ）に示すように、教示点Ｐ１から座標系ＴｏでＸ軸方向に５０ｍｍ移動した位置である。インピーダンス制御を開始した時点から３秒間、Ｘ軸方向に５０ｍｍ線形変化するものとした。

図１３（ｂ）のグラフは、第１実施形態おいて、マニピュレータ２００を力制御したときのマニピュレータ２００のハンド２５２の座標系Ｔｅの位置変化を示している。横軸は、座標系Ｔｏにおける座標系ＴｅのＸ軸方向の位置である。縦軸は、座標系Ｔｏにおける座標系ＴｅのＺ軸方向の位置である。ワークＷ１とワークＷ２との接触による位置変化は考慮しないものとする。

図１３（ｂ）に示す通り、座標系Ｔｅは、ワークＷ１がとば口ＷＡに入る範囲、即ち座標系Ｔｅが座標系ＴｏのＸ軸方向の位置２７０ｍｍに到達したときに、座標系ＴｏのＺ軸方向の位置１９５ｍｍ～２０５ｍｍの範囲内にある。したがってワークＷ２に対するワークＷ１の組み付けが成功する。

一方、ステップＳ５７においてトルク指令値τ_Ｆ１～τ_Ｆ６を補正しない場合、即ち、トルク指令値τ_Ｆ１～τ_Ｆ６がトルク指令値τ_ｒｅｆ１～τ_ｒｅｆ６である場合を比較例とする。図１３（ｃ）のグラフは、比較例おいて、マニピュレータ２００を力制御したときのマニピュレータ２００のハンド２５２の座標系Ｔｅの位置変化を示している。

図１３（ｃ）に示すように、座標系Ｔｅが、座標系ＴｏのＸ軸方向の位置２７０ｍｍに到達したときに、座標系ＴｏのＺ軸方向の位置１８５ｍｍ以下にあり、組み付け可能な位置１９５ｍｍ～２０５ｍの範囲から１０ｍｍ以上ずれている。したがってワークＷ２に対するワークＷ１の組み付けが失敗する。

このように、本実施形態では、マニピュレータ２００の実機とモデルとの差が不明であっても、トルク指令値τ_Ｆｄ１～τ_Ｆｄ６を補正値τ_{ｃｏｍｐ１}～τ_{ｃｏｍｐ６}で補正することで、モデル化誤差をキャンセルすることができる。また、負荷トルクの測定値τ_１～τ_６に含まれるオフセット誤差が不明であっても、フィードバック演算において、トルク指令値τ_ｒｅｆ１～τ_ｒｅｆ６と測定値τ_１～τ_６との差を求めることで、オフセット誤差をキャンセルすることができる。よって、関節Ｊ_１～Ｊ_６のトルクの急変を防止することができる。

また、ロボットモデル５０３は、厳密に現実のマニピュレータ２００に近付ける必要がなくなるので、ロボットモデル５０３の推定などの準備にかかる手間や時間を減らすことができる。さらに、環境変化や経時変化によるトルクセンサ２４_１～２４_６のオフセット誤差も事前に調べる必要がなくなり、校正作業にかかる手間や時間も減らすことができる。所定の校正作業を事前に行う必要なしに、力制御の作業ごとにモデル化誤差およびセンサオフセット誤差による制御性能の低下や不測の動作を防止することができる。

［第２実施形態］
第２実施形態に係るロボット装置について説明する。図１４は、第２実施形態に係るロボット装置の概略構成を示す斜視図である。第２実施形態のロボット装置１００Ａにおいて、第１実施形態のロボット装置１００と同様の構成については、同一符号を付して詳細な説明は省略する。図１４に示すように、ロボット装置１００Ａは、第１実施形態と同様の構成のマニピュレータ２００と、マニピュレータ２００の位置又は推力を制御する制御部５００Ａを有する制御装置１０００Ａと、を備えている。また、ロボット装置１００Ａは、制御装置１０００Ａに教示データを送信する教示装置としての教示ペンダント６００を備えている。

制御部５００Ａは、第１制御ユニットである制御ユニット３００Ａと、第２制御ユニットである制御ユニット４００Ａと、を有する。制御ユニット３００Ａは、ロボット装置１００Ａを統括的に制御するものである。制御ユニット４００Ａは、制御ユニット３００Ａからの指令に基づいて、マニピュレータ２００の位置又は推力を制御するものである。制御ユニット４００Ａは、マニピュレータ２００の各サーボ機構部２０_１～２０_６を制御するサーボ制御装置である。

制御部５００Ａについて具体的に説明する。図１５及び図１６は、第２実施形態に係る制御部５００Ａの構成を示すブロック図である。制御部５００Ａは、上述したように、制御ユニット３００Ａ，４００Ａを有する。図１５には、制御ユニット３００Ａの詳細な構成、図１６には、制御ユニット４００Ａの詳細な構成について図示している。

制御ユニット３００Ａは、コンピュータで構成されている。制御ユニット３００Ａのハードウェアは、第１実施形態と同様であり、ＣＰＵ３０１、ＲＯＭ３０２、ＲＡＭ３０３、ＨＤＤ３０４、記録ディスクドライブ３０５、及びインタフェース３０６～３０９を備えている。ＣＰＵ３０１、ＲＯＭ３０２、ＲＡＭ３０３、ＨＤＤ３０４、記録ディスクドライブ３０５、インタフェース３０６～３０９は、相互に通信可能にバス３１０で接続されている。ＲＯＭ３０２には、ＣＰＵ３０１に制御方法の処理を実行させるためのプログラム３３０Ａが格納されている。

なお、第２実施形態では、コンピュータによって読取可能な記録媒体がＲＯＭ３０２であり、ＲＯＭ３０２にプログラム３３０Ａが記録されているが、これに限定するものではない。プログラム３３０Ａは、コンピュータによって読取可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。プログラム３３０Ａを供給するための記録媒体としては、例えば、フレキシブルディスク、ハードディスク、光ディスク、光磁気ディスク、磁気テープ、不揮発性メモリ等を用いることができる。

図１６に示す制御ユニット４００Ａは、コンピュータで構成されている。制御ユニット４００Ａのハードウェアは、第１実施形態と同様であり、ＣＰＵ４０１と、ＲＯＭ４０２と、ＲＡＭ４０３と、インタフェース４０６～４０９と、を備えている。ＣＰＵ４０１、ＲＯＭ４０２、ＲＡＭ４０３、インタフェース４０６～４０９は、相互に通信可能にバス４１０で接続されている。ＲＯＭ４０２には、ＣＰＵ４０１に制御方法の処理を実行させるためのプログラム４３０Ａが格納されている。

インタフェース４０６には、モータ２１_１～２１_６が接続されている。インタフェース４０７には、位置センサ２３_１～２３_６が接続されている。インタフェース４０８には、トルクセンサ２４_１～２４_６が接続されている。インタフェース４０９には、制御ユニット３００Ａが接続されている。

なお、第２実施形態では、コンピュータによって読取可能な記録媒体がＲＯＭ４０２であり、ＲＯＭ４０２にプログラム４３０Ａが記録されているが、これに限定するものではない。プログラム４３０Ａは、コンピュータによって読取可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。プログラム４３０Ａを供給するための記録媒体としては、例えば、フレキシブルディスク、ハードディスク、光ディスク、光磁気ディスク、磁気テープ、不揮発性メモリ等を用いることができる。

図１７は、第２実施形態に係るロボット装置の制御システムを示す制御ブロック図である。図１５に示す制御ユニット３００ＡのＣＰＵ３０１は、プログラム３３０Ａを実行することにより、図１７に示すトルク指令値生成部５０４Ａ及び位置指令値生成部５０５として機能する。図１６に示す制御ユニット４００ＡのＣＰＵ４０１は、プログラム４３０Ａを実行することにより、図１７に示すトルク制御部５１Ａ_１～５１Ａ_６、位置制御部５２_１～５２_６、切替部５３_１～５３_６、及びモータ制御部５４_１～５４_６として機能する。サーボ機構部２０_１～２０_６は、モータ２１_１～２１_６、減速機２２_１～２２_６、位置センサ２３_１～２３_６、及びトルクセンサ２４_１～２４_６を有する。

トルク指令値生成部５０４Ａは、ロボットモデル５０３、目標力Ｆ_ｄ、目標位置Ｐ_ｄ、関節Ｊ_１～Ｊ_６の位置の測定値ｑ_１～ｑ_６、関節Ｊ_１～Ｊ_６のトルクの測定値τ_１～τ_６が入力される。そして、トルク指令値生成部５０４Ａは、これらの値を用いて、関節Ｊ_１～Ｊ_６に対するトルク指令値τ_ｒｅｆ１～τ_ｒｅｆ６を求める。トルク指令値生成部５０４Ａは、求めたトルク指令値τ_ｒｅｆ１～τ_ｒｅｆ６を、トルク制御部５１Ａ_１～５１Ａ_６に出力する。

トルク制御部５１Ａ_１～５１Ａ_６は、関節Ｊ_１～Ｊ_６のトルク指令値τ_ｒｅｆ１～τ_ｒｅｆ６と、関節Ｊ_１～Ｊ_６のトルクの測定値τ_１～τ_６との差が小さくなるように、トルク指令値τ_Ｆ１～τ_Ｆ６を求め、切替部５３_１～５３_６に出力する。トルク制御部５１Ａ_１～５１Ａ_６の詳細については後述する。なお、位置制御部５２_１～５２_６、切替部５３_１～５３_６、モータ制御部５４_１～５４_６は、第１実施形態と同様の処理を行うため、詳細な説明は省略する。

以上、制御ユニット４００Ａは、制御ユニット３００Ａから位置指令値ｑ_ｒｅｆ１～ｑ_ｒｅｆ６を取得した場合、位置指令値ｑ_ｒｅｆ１～ｑ_ｒｅｆ６と測定値ｑ_１～ｑ_６との差に基づき、マニピュレータ２００の位置を制御する。制御ユニット４００Ａは、制御ユニット３００Ａからトルク指令値τ_ｒｅｆ１～τ_ｒｅｆ６を取得した場合、トルク指令値τ_ｒｅｆ１～τ_ｒｅｆ６と測定値τ_１～τ_６との差に基づき、マニピュレータ２００の推力、即ち関節Ｊ_１～Ｊ_６の出力トルクを制御する。

ところで、マニピュレータで精密な組立作業を行う場合、マニピュレータを位置制御する位置制御モードにて組立を開始する所定位置にマニピュレータを移動させる。その後、マニピュレータを力制御する力制御モードに切り替えてからマニピュレータに精密組立を行わせることがある。位置制御モードから力制御モードに制御モードを切り替える際、仮に、マニピュレータの関節を駆動するモータへのトルク指令値が急激に変化すると、関節が急峻な動作をし、その結果、マニピュレータのハンドが振動的になることがある。これは、位置制御モードから力制御モードへの切り替え時に、フィードバック値が一旦ゼロにリセットされるためである。

図１８は、第２実施形態に係る第２制御ユニットの処理の一部を示すブロック図である。図１８には、トルク制御部５１Ａ_１～５１Ａ_６の処理について図示している。トルク制御部５１Ａ_１～５１Ａ_６は、主に、フィードバック制御部６０_１～６０_６、フィードフォワード演算部６２_１～６２_６、及び加算部６３_１～６３_６として機能する。また、図１６に示すＲＡＭ４０３は、第１記憶部６１_１～６１_６及び第２記憶部７１_１～７１_６として機能する。以下、各部の機能について説明する。

第１記憶部６１_１～６１_６は、力制御モードに切り替わる前の位置制御モードにおいて、第１測定値である、トルクセンサ２４_１～２４_６を用いて測定した負荷トルクの測定値τ_１～τ_６を、オフセット値τ_{ｏｆｆｓｅｔ１}～τ_{ｏｆｆｓｅｔ６}として記憶する。即ち、トルク制御部５１Ａ_１～５１Ａ_６は、位置制御部５２_１～５２_６がマニピュレータ２００の位置を制御しているときにマニピュレータ２００に生じる負荷の第１測定値に基づくオフセット値τ_{ｏｆｆｓｅｔ１}～τ_{ｏｆｆｓｅｔ６}を求める。トルク制御部５１Ａ_１～５１Ａ_６は、第１記憶部６１_１～６１_６に、位置制御時に取得したオフセット値τ_{ｏｆｆｓｅｔ１}～τ_{ｏｆｆｓｅｔ６}を記憶させておく。第１記憶部６１_１～６１_６に記憶されたオフセット値τ_{ｏｆｆｓｅｔ１}～τ_{ｏｆｆｓｅｔ６}は、位置制御モードから力制御モードに切り替えた後、フィードバック値τ_ＦＢ１～τ_ＦＢ６に加算される。

本実施形態では、マニピュレータ２００の位置が目標位置に到達したのをトリガとし、その時の測定値τ_１～τ_６をオフセット値τ_{ｏｆｆｓｅｔ１}～τ_{ｏｆｆｓｅｔ６}とする。なお、オフセット値τ_{ｏｆｆｓｅｔ１}～τ_{ｏｆｆｓｅｔ６}は、位置制御中、所定の制御周期で随時更新してもよい。この場合、位置制御モードから力制御モードに切り替わるときには、最後に更新された値が第１記憶部６１_１～６１_６に保持されることとなる。

フィードバック制御部６０_１～６０_６は、力制御モードにおいて、所定の制御周期で、トルク指令値生成部５０４Ａからトルク指令値τ_ｒｅｆ１～τ_ｒｅｆ６と、トルクセンサ２４_１～２４_６を用いて測定した負荷トルクの測定値τ_１～τ_６と、を取得する。フィードバック制御部６０_１～６０_６は、トルク指令値τ_ｒｅｆ１～τ_ｒｅｆ６と、測定値τ_１～τ_６との差に基づき、フィードバック値τ_ＦＢ１～τ_ＦＢ６を算出する。フィードバック制御部６０_１～６０_６は、フィードバック演算の一例として、比例演算、積分演算、及び微分演算とからなるＰＩＤ演算を行う。

フィードフォワード演算部６２_１～６２_６は、力制御時において、トルク指令値τ_ｒｅｆ１～τ_ｒｅｆ６に応じたフィードフォワード値τ_ＦＦ１～τ_ＦＦ６を求める。加算部６３_１～６３_６は、フィードバック値τ_ＦＢ１～τ_ＦＢ６に、オフセット値τ_{ｏｆｆｓｅｔ１}～τ_{ｏｆｆｓｅｔ６}と、フィードフォワード値τ_ＦＦ１～τ_ＦＦ６とを加算し、その値を、トルク指令値τ_Ｆ１～τ_Ｆ６として切替部５３_１～５３_６に出力する。

フィードフォワード演算部６２_１～６２_６の処理について具体例を挙げて説明する。ここで、トルク制御部５１Ａ_１～５１Ａ_６が入力を受けるトルク指令値τ_ｒｅｆ１～τ_ｒｅｆ６のうち、力制御時に最初に入力を受ける指令値（第２指令値）を、トルク指令値τ０_ｒｅｆ１～τ０_ｒｅｆ６とする。つまり、トルク指令値生成部５０４Ａは、トルク指令値τ０_ｒｅｆ１～τ０_ｒｅｆ６を、マニピュレータ２００の推力の制御を開始したときに最初に求める。また、トルク制御部５１Ａ_１～５１Ａ_６がトルク指令値τ０_ｒｅｆ１～τ０_ｒｅｆ６の後に入力を受ける指令値（第１指令値）をトルク指令値τ１_ｒｅｆ１～τ１_ｒｅｆ６とする。つまり、トルク指令値生成部５０４Ａは、トルク指令値τ０_ｒｅｆ１～τ０_ｒｅｆ６の後にトルク指令値τ１_ｒｅｆ１～τ１_ｒｅｆ６を求める。よって、トルク指令値τ０_ｒｅｆ１～τ０_ｒｅｆ６は、トルク制御部５１Ａ_１～５１Ａ_６において、トルク指令値τ１_ｒｅｆ１～τ１_ｒｅｆ６の前に入力を受けるトルク指令値である。

フィードフォワード演算部６２_１～６２_６は、本実施形態では、モデルトルク演算部７０_１～７０_６と、減算部７３_１～７３_６とを含む。力制御を行う最初の制御サイクルについて説明する。モデルトルク演算部７０_１～７０_６は、ロボットモデル５０３を用いた動力学の計算により、トルク指令値τ０_ｒｅｆ１～τ０_ｒｅｆ６に応じた、マニピュレータ２００に生じる負荷である負荷トルクの第２計算値である計算値τ０_ＦＦ１～τ０_ＦＦ６を求める。ロボットモデル５０３には、モータ２１_１～２１_６の負荷イナーシャＪ_Ｍ及び粘性係数Ｄ_Ｍ、並びに減速機２２_１～２２_６の剛性値Ｋ及び減速比Ｎなどのパラメータが含まれる。本実施形態では、モデルトルク演算部７０_１～７０_６は、トルク指令値τ０_ｒｅｆ１～τ０_ｒｅｆ６と、ロボットモデル５０３に含まれるパラメータＪ_Ｍ、Ｄ_Ｍ、Ｋ、Ｎと、に基づいて、計算値τ０_ＦＦ１～τ０_ＦＦ６を以下の式（２）に従って求める。ｓはラプラス演算子である。

第２記憶部７１_１～７１_６は、計算値τ０_ＦＦ１～τ０_ＦＦ６を、オフセット値である基準値τ０_ＦＦ１～τ０_ＦＦ６として記憶する。第２記憶部７１_１～７１_６は、力制御が継続される間、計算値τ０_ＦＦ１～τ０_ＦＦ６を基準値τ０_ＦＦ１～τ０_ＦＦ６として保持する。力制御モードから位置制御モードに切り替わったときには、第２記憶部７１_１～７１_６は、リセットされる。

減算部７３_１～７３_６は、計算値τ０_ＦＦ１～τ０_ＦＦ６から基準値τ０_ＦＦ１～τ０_ＦＦ６を引いた値を、フィードフォワード値τ_ＦＦ１～τ_ＦＦ６とし、加算部６３_１～６３_６に出力する。したがって、本実施形態では、フィードフォワード値τ_ＦＦ１～τ_ＦＦ６の初期値はゼロである。

次に、最初の制御サイクルの次の制御サイクル以降について説明する。モデルトルク演算部７０_１～７０_６は、ロボットモデル５０３を用いた動力学の計算により、トルク指令値τ１_ｒｅｆ１～τ１_ｒｅｆ６に応じた、マニピュレータ２００に生じる負荷である負荷トルクの第１計算値である計算値τ１_ＦＦ１～τ１_ＦＦ６を求める。本実施形態では、モデルトルク演算部７０_１～７０_６は、トルク指令値τ１_ｒｅｆ１～τ１_ｒｅｆ６と、ロボットモデル５０３に含まれるパラメータＪ_Ｍ、Ｄ_Ｍ、Ｋ、Ｎと、に基づいて、計算値τ１_ＦＦ１～τ１_ＦＦ６を以下の式（３）に従って求める。減算部７３_１～７３_６は、計算値τ１_ＦＦ１～τ１_ＦＦ６から基準値τ０_ＦＦ１～τ０_ＦＦ６を引いた値を、フィードフォワード値τ_ＦＦ１～τ_ＦＦ６とし、加算部６３_１～６３_６に出力する。

つまり、マニピュレータ２００の関節Ｊ_１～Ｊ_６において、作用する負荷の変動で捩れ量が変動すると、ハンド２５２が振動する。本実施形態では、力制御を開始する時点での負荷を基準とし、負荷変動分（２つの計算値の差分）をフィードフォワード値τ_ＦＦ１～τ_ＦＦ６とすることで、力制御の応答性を高め、その結果、ハンド２５２の振動が抑制される。

なお、第２実施形態においては、第２記憶部７１_１～７１_６に記憶される基準値は、マニピュレータ２００の推力の制御を開始して最初に求められる初期値である計算値τ０_ＦＦ１～τ０_ＦＦ６であるのが好ましいが、これに限定するものではない。例えば、初期値の次の値などを用いてもよい。即ち、基準値は、ある計算値τ１_ＦＦ１～τ１_ＦＦ６よりも前に、トルク指令値τ１_ＦＦ１～τ１_ＦＦ６よりも前のトルク指令値に応じて求められた、マニピュレータ２００に生じる負荷の計算値であればよい。

図１９は、第２実施形態に係る物品の製造方法を説明するフローチャートである。なお、第２実施形態では、第１実施形態と同様、図１０（ａ）に示すように、ワークＷ１をハンド２５２に把持させ、ワークＷ１をワークＷ２に組み付けることで物品が製造されるものとする。

まず、制御部５００Ａは、マニピュレータ２００の位置制御を開始し（Ｓ７１）、力制御を開始する位置、即ち図１０（ａ）に示す教示点Ｐ１に到達するまで、位置制御でマニピュレータ２００を駆動する（Ｓ７２）。

マニピュレータ２００が力制御を開始する位置まで駆動したかどうかを判定する（Ｓ７３）。制御部５００Ａは、マニピュレータ２００がその位置まで移動した場合（Ｓ７３：Ｙｅｓ）、当該位置におけるトルクの測定値τ_１～τ_６を取得する。制御部５００Ａは、図１８に示す第１記憶部６１_１～６１_６に、測定値τ_１～τ_６をオフセット値τ_{ｏｆｆｓｅｔ１}～τ_{ｏｆｆｓｅｔ６}として記憶させる（Ｓ７４）。

次に、制御部５００Ａは、位置制御モードから力制御モードへマニピュレータ２００を制御する制御モードを切り替え（Ｓ７５）、マニピュレータ２００の力制御を開始する（Ｓ７６）。力制御開始後、制御部５００Ａは、トルク指令値τ０_ｒｅｆ１～τ０_ｒｅｆ６を入力として、関節Ｊ_１～Ｊ_６における負荷トルクの計算値τ０_ＦＦ１～τ０_ＦＦ６を基準値として第２記憶部７１_１～７１_６に記憶させる（Ｓ７７）。

制御部５００Ａは、フィードバック値τ_ＦＢ１～τ_ＦＢ６に、オフセット値τ_{ｏｆｆｓｅｔ１}～τ_{ｏｆｆｓｅｔ６}とフィードフォワード値τ_ＦＦ１～τ_ＦＦ６とを加算した値に基づいて、マニピュレータ２００の推力を制御する。これにより、制御部５００Ａは、マニピュレータ２００のハンド２５２に働く力が目標力となるように駆動する（Ｓ７８）。

制御部５００Ａは、駆動が終了したかどうか、つまり組付けが完了したかどうかを判定する（Ｓ７９）。制御部５００Ａは、駆動が終了していない場合（Ｓ７９：Ｎｏ）、組付けが完了するまで、ステップＳ７８及びＳ７９のループを繰り返し実行する。制御部５００Ａは、駆動終了と判断したら（Ｓ７９：Ｙｅｓ）、制御処理を終了する。

以上、第２実施形態によれば、位置制御モードから力制御モードへ切り替わる直前に、第１記憶部６１_１～６１_６にトルクの測定値τ_１～τ_６が、オフセット値τ_{ｏｆｆｓｅｔ１}～τ_{ｏｆｆｓｅｔ６}として記憶される。力制御モードに切り替わった後は、フィードバック値τ_ＦＢ１～τ_ＦＢ６に、オフセット値τ_{ｏｆｆｓｅｔ１}～τ_{ｏｆｆｓｅｔ６}が加算される。これにより、位置制御モードから力制御モードに制御が切り替わった際に、トルク指令値τ_Ｍ１～τ_Ｍ６の連続性が補償される。モータトルク指令値τ_Ｍ１～τ_Ｍ６の連続性が補償されるので、モータへ供給される電流Ｃｕｒ_１～Ｃｕｒ_６が急峻に変動するのが防止される。よって、マニピュレータ２００の各関節において急峻な動作が防止され、マニピュレータ２００のハンド２５２が振動するのを防止することができる。特に、マニピュレータ２００を用いて精密な組立て作業を行う場合、マニピュレータ２００のハンド２５２の振動が防止されるので、組立ワーク又は被組立ワークが変形したり破壊したりするのを防止することができる。また、組立不良を低減することができ、リトライ動作などが少なくなり、物品の生産性が向上する。

また、第２実施形態によれば、フィードバック値τ_ＦＢ１～τ_ＦＢ６に、フィードフォワード値τ_ＦＦ１～τ_ＦＦ６が加算されるので、マニピュレータ２００における関節Ｊ_１～Ｊ_６の制御の応答性が向上する。このフィードフォワード値τ_ＦＦ１～τ_ＦＦ６は、負荷トルクの計算値τ１_ＦＦ１～τ１_ＦＦ６から、オフセットしている分として基準値τ０_ＦＦ１～τ０_ＦＦ６が除去されることで求められるので、トルク指令値τ_Ｍ１～τ_Ｍ６が急峻に変動するのが防止される。したがって、制御モードの切替時にハンド２５２が振動するのを効果的に防止することができる。以上、第２実施形態によれば、位置制御モードから力制御モードへ切り替わる際のマニピュレータ２００の制御性が向上する。

図２０は、第２実施形態における実験結果を示すグラフである。図２０において、横軸は時間、縦軸はトルクである。図２０には、関節Ｊ１のトルク指令値τ_Ｍ１と、関節Ｊ１の負荷トルクの測定値τ_１について図示している。

制御モードの切り替え時、トルク指令値τ_Ｍ１は、トルク指令値τ_ｑ１からトルク指令値τ_Ｆ１へ切り替わるが、急峻な変化をさせることなく滑らかに切り替わっている。このため、制御モードを切り替えた後のトルクの測定値τ_１の振動が小さい。また、力制御時は、フィードバック値τ_ＦＢ１にフィードフォワード値τ_ＦＦ１を加算しているため、測定値τ_１が応答性良く、トルク指令値τ_Ｍ１に追従していることが確認できる。

以上より、第２実施形態によれば、位置制御モードから力制御モードに制御モードを切り替える際において、制御モードの切り替え時のトルク指令値τ_Ｍ１～τ_Ｍ６の連続性を補償し、かつ力制御モード時の応答性を向上させることができる。これにより、制御モードを切り替える場合においても、マニピュレータ２００の高精度かつ高応答な力制御が可能となる。

［第３実施形態］
次に、第３実施形態のロボット装置について説明する。なお、ロボット装置の構成は、第２実施形態と同様であるため、説明を省略する。

第２実施形態では、第２記憶部７１_１～７１_６に記憶させておく基準値を、モデルトルク演算部７０_１～７０_６にて算出した値の初期値とした。制御モードの切り替え直後は、フィードフォワード値τ_ＦＦ１～τ_ＦＦ６はゼロとなる。よって、この場合、トルク指令値τ_Ｍ１～τ_Ｍ６は、僅かな期間ではあるが一定値となり、滑らかに推移しない。

図２１は、第３実施形態に係る第２制御ユニットの処理の一部を示すブロック図である。第３実施形態では、モデルトルク演算部７０_１～７０_６は、力制御実行の前の位置制御実行時に、第２計算値を求める演算処理を並行して行い、この値を第２記憶部７１_１～７１_６に記憶させておく。

具体的には、トルク指令値生成部５０４Ａは、位置制御が実行されている時に、力制御が実行される時の第１指令値であるトルク指令値τ１_ｒｅｆ１～τ１_ｒｅｆ６よりも前に、第２指令値であるトルク指令値τ２_ｒｅｆ１～τ２_ｒｅｆ６を求める。トルク指令値生成部５０４Ａは、位置制御中に目標力Ｆ_ｄ、目標位置Ｐ_ｄ、測定値τ_１～τ_６、及び測定値ｑ_１～ｑ_６を取得し、トルク指令値τ２_ｒｅｆ１～τ２_ｒｅｆ６を、取得したこれら値に基づいて生成する。

トルク指令値生成部５０４Ａは、位置制御が実行されている時に、トルク指令値τ２_ｒｅｆ１～τ２_ｒｅｆ６をモデルトルク演算部７０_１～７０_６に出力する。そして、モデルトルク演算部７０_１～７０_６は、取得したトルク指令値τ２_ｒｅｆ１～τ２_ｒｅｆ６に応じた負荷トルクの第２計算値である計算値τ２_ＦＦ１～τ２_ＦＦ６を求める。そして、モデルトルク演算部７０_１～７０_６は、この計算値τ２_ＦＦ１～τ２_ＦＦ６を基準値τ２_ＦＦ１～τ２_ＦＦ６として第２記憶部７１_１～７１_６に記憶させる。なお、位置制御時、フィードバック制御部６０_１～６０_６、減算部７３_１～７３_６、及び加算部６３_１～６３_６は、演算処理を行う必要がないため行わない。

力制御を実行するときには、フィードバック制御部６０_１～６０_６は、トルク指令値τ１_ｒｅｆ１～τ１_ｒｅｆ６と測定値τ_１～τ_６との差に基づき、フィードバック値τ_ＦＢ１～τ_ＦＢ６を求める。モデルトルク演算部７０_１～７０_６は、トルク指令値τ１_ｒｅｆ１～τ１_ｒｅｆ６に応じた第１計算値である計算値τ１_ＦＦ１～τ１_ＦＦ６を求める。減算部７３_１～７３_６は、計算値τ１_ＦＦ１～τ１_ＦＦ６から基準値τ２_ＦＦ１～τ２_ＦＦ６を減算して、フィードフォワード値τ_ＦＦ１～τ_ＦＦ６を求める。そして、加算部６３_１～６３_６は、フィードバック値τ_ＦＢ１～τ_ＦＢ６に、オフセット値τ_{ｏｆｆｓｅｔ１}～τ_{ｏｆｆｓｅｔ６}、及びフィードフォワード値τ_ＦＦ１～τ_ＦＦ６を加算することで、トルク指令値τ_Ｆ１～τ_Ｆ６を求める。

これにより、力制御モードに切り替え直後であっても、トルク指令値τ_Ｆ１～τ_Ｆ６、即ち図１７に示すトルク指令値τ_Ｍ１～τ_Ｍ６は、時間変化に対して滑らかに推移する。その結果、制御モード切り替え時にマニピュレータ２００のハンド２５２が振動するのをさらに低減することができる。

図２２は、第３実施形態における実験結果を示すグラフである。なお、図２２のグラフには、第２実施形態における実験結果も図示している。図２２には、関節Ｊ_１のトルク指令値τ_Ｍ１を図示している。第１実施形態におけるトルク指令値τ_Ｍ１の時間変化率は、位置制御モードにおいては一定であるが、位置制御モードから力制御モードに切り替えた直後、一定とならずに変化する。一方、第２実施形態におけるトルク指令値τ_Ｍ１の時間変化率は、位置制御モードから力制御モードに切り替えた直後であっても、一定となる。

第３実施形態によれば、位置制御モードから力制御モードに制御モードを切り替える際において、制御モードの切替時のトルク指令値τ_Ｍ１～τ_Ｍ６を連続かつ滑らかにすることができる。その結果、制御モードの切替時に、マニピュレータ２００のハンド２５２の振動をさらに抑制でき、高精度かつ高応答な力制御が可能となる。

［第４実施形態］
第４実施形態のロボット装置について説明する。図２３は、第４実施形態に係るロボット装置の概略構成を示す斜視図である。第４実施形態のロボット装置１００Ｂにおいて、第１、第２又は第３実施形態のロボット装置と同様の構成については、同一符号を付して詳細な説明は省略する。図２３に示すように、ロボット装置１００Ｂは、第１実施形態と同様の構成のマニピュレータ２００と、マニピュレータ２００を制御する制御部５００Ｂを有する制御装置１０００Ｂと、を備えている。また、ロボット装置１００Ｂは、制御装置１０００Ｂに教示データを送信する教示装置としての教示ペンダント６００を備えている。

制御部５００Ｂは、第１実施形態と同様の制御ユニット３００と、第２又は第３実施形態と同様の制御ユニット４００Ａと、を有する。即ち、制御ユニット３００は、第１実施形態において説明した処理を行い、制御ユニット４００Ａは、第２又は第３実施形態において説明した処理を行う。

制御ユニット３００は、制御ユニット４００Ａがマニピュレータ２００の位置を制御しているときに、補正値τ_{ｃｏｍｐ１}～τ_{ｃｏｍｐ６}（図７）を求めておく。制御ユニット３００は、制御ユニット４００Ａにマニピュレータ２００の推力を制御させる際に、教示データである力教示データ５０１及び位置教示データ５０２に基づく仮の指令値であるトルク指令値τ_Ｆｄ１～τ_Ｆｄ６を求める。そして、制御ユニット３００は、トルク指令値τ_Ｆｄ１～τ_Ｆｄ６を補正値τ_{ｃｏｍｐ１}～τ_{ｃｏｍｐ６}で補正することでトルク指令値τ_ｒｅｆ１～τ_ｒｅｆ６を求めて、制御ユニット４００Ａにトルク指令値τ_ｒｅｆ１～τ_ｒｅｆ６を出力する。

一方、制御ユニット４００Ａは、マニピュレータ２００の位置を制御しているときに、図１８に示すように、オフセット値τ_{ｏｆｆｓｅｔ１}～τ_{ｏｆｆｓｅｔ６}を求めておく。制御ユニット４００Ａは、マニピュレータ２００の推力を制御する際に、フィードバック値τ_ＦＢ１～τ_ＦＢ６及びフィードフォワード値τ_ＦＦ１～τ_ＦＦ６を求める。そして、制御ユニット４００Ａは、フィードバック値τ_ＦＢ１～τ_ＦＢ６に、オフセット値τ_{ｏｆｆｓｅｔ１}～τ_{ｏｆｆｓｅｔ６}と、フィードフォワード値τ_ＦＦ１～τ_ＦＦ６と、を加算して、トルク指令値τ_Ｆ１～τ_Ｆ６を生成する。制御ユニット４００Ａは、このトルク指令値τ_Ｆ１～τ_Ｆ６に基づいてマニピュレータ２００の推力を制御する。

このように、第４実施形態では、制御部５００Ｂが、第１実施形態で説明した制御処理と、第２又は第３実施形態で説明した制御処理とを組み合わせて制御処理を実行することで、マニピュレータ２００の制御性が更に向上する。

なお、本発明は、以上説明した実施形態に限定されるものではなく、本発明の技術的思想内で多くの変形が可能である。また、実施形態に記載された効果は、本発明から生じる最も好適な効果を列挙したに過ぎず、本発明による効果は、実施形態に記載されたものに限定されない。

上述の実施形態では、ロボットアーム２５１が垂直多関節のロボットアームである場合について説明したが、これに限定するものではない。例えば、水平多関節のロボットアーム、パラレルリンクのロボットアーム、直交ロボット等、種々のロボットアームであってもよい。

（その他の実施例）
本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。

１００…ロボット装置
２００…マニピュレータ
３００…制御ユニット（第１制御ユニット）
４００…制御ユニット（第２制御ユニット）
５００…制御部
１０００…制御装置

Claims (25)

1. ロボットを制御可能な制御部を備え、
   前記制御部は、
   前記ロボットの位置に関する位置測定値と、前記ロボットのモデルと、に基づき前記ロボットの力に関する力計算値を取得し、
   前記力計算値と、前記ロボットの力に関する力測定値と、に基づき補正値を取得し、
   前記ロボットの目標位置と、前記モデルと、前記ロボットの目標力に基づき、前記ロボットを制御する前記ロボットの力に関する第１力指令値を取得し、
   前記第１力指令値を前記補正値に基づき更新する、
   ことを特徴とする制御装置。
2. 前記制御部は、
   前記ロボットにおいて位置制御を実行している際に前記位置測定値と前記力測定値とを取得する、
   ことを特徴とする請求項１に記載の制御装置。
3. 前記制御部は、
   前記ロボットの位置を制御する位置制御と、前記ロボットの推力を制御する力制御とを切替える
   ことを特徴とする請求項１または２に記載の制御装置。
4. 前記制御部は、
   前記更新は、前記第１力指令値から、前記補正値を減算することである、
   ことを特徴とする請求項１から３のいずれか１項に記載の制御装置。
5. 前記制御部は、
   前記第１力指令値を、前記目標位置と前記モデルとに基づく動力学計算を行うことで取得する、
   ことを特徴とする請求項１から４のいずれか１項に記載の制御装置。
6. 前記制御部は、
   前記目標位置と、前記モデルとに基づき位置指令値を取得し、
   前記位置指令値と、前記位置測定値に基づくフィードバック演算により、第２力指令値を取得する、
   ことを特徴とする請求項３に記載の制御装置。
7. 前記制御部は、
   前記力計算値を、前記位置測定値と前記モデルとに基づく動力学計算を行うことで取得する、
   ことを特徴とする請求項１から６のいずれか１項に記載の制御装置。
8. 前記制御部は、更新された前記第１力指令値と前記力測定値との差を取得し、前記差に基づき前記第１力指令値をさらに更新する、
   ことを特徴とする請求項１から７のいずれか１項に記載の制御装置。
9. 前記制御部は、
   前記力として、前記ロボットの関節の出力トルクに関する情報を取得する、
   ことを特徴とする請求項１から８のいずれか１項に記載の制御装置。
10. 前記制御部は、
    前記ロボットを所定位置に位置決めした際に前記位置測定値と前記力測定値とを取得して、前記補正値を取得する、
    ことを特徴とする請求項１から９のいずれか１項に記載の制御装置。
11. 前記制御部は、
    前記位置として、前記ロボットの所定部位の位置を取得する、
    ことを特徴とする請求項１から１０のいずれか１項に記載の制御装置。
12. 前記所定部位にはエンドエフェクタが設けられる、
    ことを特徴とする請求項１１に記載の制御装置。
13. 前記制御部は、前記補正値を前記力測定値と前記力計算値との差に基づいて取得することを特徴とする請求項１から１２のいずれか１項に記載の制御装置。
14. 前記制御部は、前記第１力指令値を前記ロボットの機械インピーダンスの剛性の値および前記ロボットの機械インピーダンスの粘性の値を用いて取得することを特徴とする請求項１から１３のいずれか１項に記載の制御装置。
15. 前記制御部は、
    位置制御を実行している際に取得した前記力測定値に基づくオフセット値を取得し、
    更新された前記第１力指令値と、力制御を実行している際に取得した前記力測定値とに基づいてフィードバック値を取得し、
    前記フィードバック値と、前記オフセット値と、更新された前記第１力指令値に応じたフィードフォワード値と、に基づいて、前記ロボットの推力を制御する力制御を実行する、
    ことを特徴とする請求項１から１４のいずれか１項に記載の制御装置。
16. 前記目標位置はユーザにより設定できる、
    ことを特徴とする請求項１から１５のいずれか１項に記載の制御装置。
17. 前記目標力はユーザにより設定できる、
    ことを特徴とする請求項１から１６のいずれか１項に記載の制御装置。
18. 前記補正値は、前記位置制御から前記力制御に切り換える前に取得する、
    ことを特徴とする請求項３に記載の制御装置。
19. 前記動力学計算は、ニュートン・オイラー法である、
    ことを特徴とする請求項５または７に記載の制御装置。
20. 前記モデルは、前記ロボットに関する質量データ、前記ロボットに関する重心位置データ、前記ロボットに関する重心まわりの慣性モーメントデータ、の少なくとも１つを含む、
    ことを特徴とする請求項１から１９のいずれか１項に記載の制御装置。
21. 請求項１から２０のいずれか１項に記載の制御装置と前記ロボットとを備えたロボットシステム。
22. 請求項２１に記載のロボットシステムを用いて物品の製造を行うことを特徴とする物品の製造方法。
23. ロボットの制御方法であって、
    前記ロボットの位置に関する位置測定値と、前記ロボットのモデルと、に基づき前記ロボットの力に関する力計算値を取得し、
    前記力計算値と、前記ロボットの力に関する力測定値と、に基づき補正値を取得し、
    前記ロボットの目標位置と、前記モデルと、前記ロボットの目標力に基づき、前記ロボットを制御する前記ロボットの力に関する第１力指令値を取得し、
    前記第１力指令値を前記補正値に基づき更新する、
    ことを特徴とする制御方法。
24. 請求項２３に記載の制御方法をコンピュータに実行させるためのプログラム。
25. 請求項２４に記載のプログラムを記録した、コンピュータにより読取可能な記録媒体。

Images