以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。
[第1実施形態]
図1は、第1実施形態に係るロボット装置の概略構成を示す斜視図である。図1に示すように、ロボット装置100は、マニピュレータ200と、マニピュレータ200の位置又は推力を制御する制御部500を有する制御装置1000と、を備えている。また、ロボット装置100は、制御装置1000に教示データを送信する教示装置としての教示ペンダント600を備えている。教示ペンダント600は、操作者が操作するものであり、マニピュレータ200や制御装置1000の動作を指定するのに用いる。
マニピュレータ200は、ロボットアーム251と、マニピュレータ200の先端部を構成する、エンドエフェクタの一例であるハンド252と、を備える。ロボットアーム251は、本実施形態では垂直多関節のロボットアームである。
ロボットアーム251の先端部には、ハンド252が取り付けられている。以下、エンドエフェクタがハンド252である場合について説明するが、これに限定するものではなく、ツール等であってもよい。ロボットアーム251の基端部は、台座Bに固定されている。ハンド252は、部品やツール等のワークを把持するものである。
マニピュレータ200、即ちロボットアーム251は、1つ以上の関節を有していればよく、本実施形態では、複数の関節、例えば6つの関節J1~J6を有している。ロボットアーム251は、各関節J1~J6を各軸A1~A6まわりに回転駆動するための複数(6つ)のサーボ機構部201~206を有している。
なお、本実施形態では、関節J1~J6は、回転関節であるが、直動関節であってもよい。関節が回転関節の場合、関節の位置とは、関節の角度、即ち回転位置を表す。関節が直動関節の場合、関節の位置とは、直動位置を表す。また、関節が回転関節の場合、関節の推力とは、関節の推進トルク、即ち出力トルクを表す。関節が直動関節の場合、関節の推力とは、関節の直動推力を表す。
ロボットアーム251は、複数のリンク2100~2106を有する。リンク2100~2106は、関節J1~J6で回転可能に連結されている。リンク2100~2106は、基端側から先端側に向かって、順に直列に連結されている。ロボットアーム251は、マニピュレータ200の先端部、即ちハンド252を、可動範囲内の任意の位置に移動させることができる。
マニピュレータ200の位置、即ち姿勢は、座標系Toで表現することができる。座標系Toは、ロボットアーム251の基端、即ち台座Bに固定した座標系を表し、座標系Teはマニピュレータ200の先端部、即ちハンド252と連動する座標系を表す。
制御部500は、第1制御ユニットである制御ユニット300と、第2制御ユニットである制御ユニット400と、を有する。制御ユニット300は、ロボット装置100を統括的に制御するものである。制御ユニット400は、制御ユニット300の指令に基づいて、マニピュレータ200の位置又は推力を制御するものである。制御ユニット400は、マニピュレータ200の各サーボ機構部201~206を制御するサーボ制御装置である。
制御部500について具体的に説明する。図2は、第1実施形態に係る制御装置の構成を示すブロック図である。制御部500は、上述したように、制御ユニット300,400を有する。図2には、制御ユニット300のハードウェア構成について詳細に図示している。
制御ユニット300は、コンピュータで構成されており、処理部としてのCPU(Central Processing Unit)301を備える。また、制御ユニット300は、記憶部として、ROM(Read Only Memory)302、RAM(Random Access Memory)303、HDD(Hard Disk Drive)304を備えている。また、制御ユニット300は、記録ディスクドライブ305、及びインタフェース306~309を備えている。
CPU301、ROM302、RAM303、HDD304、記録ディスクドライブ305、インタフェース306~309は、相互に通信可能にバス310で接続されている。ROM302には、CPU301に後述する制御方法の処理を実行させるためのプログラム330が格納されている。CPU301は、ROM302に記録(格納)されたプログラム330に基づいて、後述する制御方法を実行する。RAM303は、CPU301の演算処理結果等、各種データを一時的に記憶する記憶装置である。
HDD304は、CPU301の演算処理結果や外部から取得した各種データ等を記憶可能な記憶装置である。記録ディスクドライブ305は、記録ディスク331に記録された各種データやプログラム等を読み出すことができる。
教示ペンダント600は、インタフェース306に接続されている。CPU301は、インタフェース306及びバス310を介して教示ペンダント600からマニピュレータ200の教示データを取得する。
制御ユニット400は、インタフェース309に接続されている。CPU301は、指令値のデータを所定の制御周期でバス310及びインタフェース309を介して制御ユニット400に出力する。
インタフェース307には、モニタ321が接続されており、モニタ321には、CPU301の制御の下、各種画像が表示される。インタフェース308は、書き換え可能な不揮発性メモリや外付けHDD等の記憶装置である外部記憶装置322が接続可能に構成されている。外部記憶装置322には、教示データや、マニピュレータ200のモデルデータが格納可能である。
なお、第1実施形態では、コンピュータによって読取可能な記録媒体がROM302であり、ROM302にプログラム330が記録されているが、これに限定するものではない。プログラム330は、コンピュータによって読取可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。プログラム330を供給するための記録媒体としては、例えば、フレキシブルディスク、ハードディスク、光ディスク、光磁気ディスク、磁気テープ、不揮発性メモリ等を用いることができる。
図3は、第1実施形態に係る制御装置の構成を示すブロック図である。図3には、制御ユニット400のハードウェア構成について詳細に図示している。
制御ユニット400は、コンピュータで構成されている。制御ユニット400は、処理部であるCPU401と、記憶部であるROM402及びRAM403と、インタフェース406~409と、を備えている。CPU401、ROM402、RAM403、インタフェース406~409は、相互に通信可能にバス410で接続されている。ROM402には、CPU401に、後述する制御方法の処理を実行させるためのプログラム430が格納されている。
インタフェース406には、後述するモータ211~216が接続されている。インタフェース407には、後述する位置センサ231~236が接続されている。インタフェース408には、後述するトルクセンサ241~246が接続されている。インタフェース409には、制御ユニット300が接続されている。
なお、第1実施形態では、コンピュータによって読取可能な記録媒体がROM402であり、ROM402にプログラム430が記録されているが、これに限定するものではない。プログラム430は、コンピュータによって読取可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。プログラム430を供給するための記録媒体としては、例えば、フレキシブルディスク、ハードディスク、光ディスク、光磁気ディスク、磁気テープ、不揮発性メモリ等を用いることができる。
図4は、第1実施形態に係るロボット装置の制御システムを示す制御ブロック図である。図2に示す制御ユニット300のCPU301は、プログラム330を実行することにより、図4に示すトルク指令値生成部504及び位置指令値生成部505として機能する。図3に示す制御ユニット400のCPU401は、プログラム430を実行することにより、図4に示すトルク制御部511~516、位置制御部521~526、切替部531~536、及びモータ制御部541~546として機能する。サーボ機構部201~206は、モータ211~216、減速機221~226、位置センサ231~236、及びトルクセンサ241~246を有する。
各モータ211~216は、マニピュレータ200の各関節J1~J6を駆動する駆動源の一例であり、例えば電動モータである。各減速機221~226は、各モータ211~216の回転軸の回転を減速して出力するものであり、例えば波動歯車減速機である。各関節J1~J6で連結された一対のリンクのうち、一方のリンクに対して他方のリンクを、各モータ211~216、及び各減速機221~226によって駆動することができる。なお、モータの回転軸が減速機の入力軸に直接連結されていてもよいが、モータと減速機との間に、ベルト機構などの不図示の伝達機構が配置されていてもよい。
各位置センサ231~236は、各関節J1~J6の位置(角度)に応じた信号を出力するセンサであり、例えばエンコーダである。エンコーダは、ロータリエンコーダであるのが好ましい。各位置センサ231~236は、各減速機221~226の入力軸側、即ち各モータ211~216に設けられ、各モータ211~216の回転軸の位置(角度)に応じた信号を、制御ユニット400に出力する。制御ユニット400は、各位置センサ231~236から取得した信号に基づき、各モータ211~216の回転軸の位置(角度)の測定値θ1~θ6を求め、各測定値θ1~θ6から各関節J1~J6の位置(角度)の測定値q1~q6を求める。本実施形態では、減速機221~226の減速比に基づき、測定値θ1~θ6を測定値q1~q6に変換する。制御ユニット400は、各関節J1~J6の位置(角度)の測定値q1~q6を所定の制御周期で制御ユニット300に出力する。
各トルクセンサ241~246は、マニピュレータ200に作用する負荷の一例として各関節J1~J6に作用する負荷トルクに応じた信号を、制御ユニット400に出力する。制御ユニット400は、各トルクセンサ241~246から取得した信号に基づき、各関節J1~J6にかかる負荷トルクの測定値τ1~τ6を求める。制御ユニット400は、各関節J1~J6の負荷トルクの測定値τ1~τ6を所定の制御周期で制御ユニット300に出力する。
外部記憶装置322には、ユーザが予め作成しておいた教示データとしての力教示データ501及び位置教示データ502と、マニピュレータ200のモデルデータであるロボットモデル503と、が格納されている。なお、これらデータが格納される記憶装置は、外部記憶装置322に限定するものではない。例えば、これらデータは、HDD304、又は教示ペンダント600に含まれる不図示の記憶装置に格納されていてもよい。
力教示データ501には、目標力Fdが含まれている。位置教示データ502には、目標位置Pdが含まれている。目標力Fdは、マニピュレータ200のハンド252にかかる力の目標値(目標力)であり、操作者が教示ペンダント600を用いて設定することができる。目標位置Pdは、マニピュレータ200のハンド252の位置の目標値(目標位置)であり、操作者が教示ペンダント600を用いて設定することができる。制御ユニット300は、力教示データ501から目標力Fdを読み出すことができ、位置教示データ502から目標位置Pdを読み出すことができる。
ロボットモデル503は、マニピュレータ200を構成する要素の質量データ、重心位置データ、重心まわりの慣性モーメントデータなどの設計データを含む、ユーザが作成した、仮想的なモデルである。
制御ユニット300は、予め設定されているロボットプログラムに従って、マニピュレータ200の位置を制御する位置制御モードと、マニピュレータ200の推力を制御する力制御モードとを切り替えて、制御ユニット400に指令を送る。即ち、制御ユニット300は、位置制御モードに切り替わっているときには位置指令値生成部505として機能して位置指令値を送り、力制御モードに切り替わっているときには、トルク指令値生成部504として機能してトルク指令値を送る。
位置指令値生成部505は、目標位置Pdに基づいて、関節J1~J6に対応する位置指令値qref1~qref6を生成する。具体的には、位置指令値生成部505は、ロボットモデル503を用いた逆運動学計算により、目標位置Pdを関節J1~J6に対応する位置指令値qref1~qref6に変換する。位置指令値生成部505は、生成した位置指令値qref1~qref6を、制御ユニット400の位置制御部521~526に出力する。位置制御部521~526は、測定値q1~q6と位置指令値qref1~qref6との差に基づくフィードバック演算、例えばPID演算により、トルク指令値τq1~τq6を生成する。位置制御部521~526は、トルク指令値τq1~τq6を切替部531~536に出力する。
トルク指令値生成部504は、目標力Fd、目標位置Pd、測定値τ1~τ6、及び測定値q1~q6に基づいて、関節J1~J6に対応するトルク指令値τref1~τref6を生成する。トルク指令値生成部504の具体的な処理は、後述する。トルク指令値生成部504は、生成したトルク指令値τref1~τref6を、制御ユニット400のトルク制御部511~516に出力する。トルク制御部511~516は、負荷トルクの測定値τ1~τ6とトルク指令値τref1~τref6との差に基づくフィードバック演算、例えばPID演算により、トルク指令値τF1~τF6を生成する。トルク制御部511~516は、トルク指令値τF1~τF6を切替部531~536に出力する。
切替部531~536は、トルク指令値τF1~τF6の入力を受けた場合は、入力を受けたトルク指令値τF1~τF6をトルク指令値τM1~τM6としてモータ制御部541~546に出力する。また、切替部531~536は、トルク指令値τq1~τq6の入力を受けた場合は、入力を受けたトルク指令値τq1~τq6をトルク指令値τM1~τM6としてモータ制御部541~546に出力する。
モータ制御部541~546は、トルク指令値τM1~τM6に応じた電流Cur1~Cur6をモータ211~216に通電する。
以上、制御ユニット400は、制御ユニット300から位置指令値qref1~qref6を取得した場合、位置指令値qref1~qref6と測定値q1~q6との差に基づき、マニピュレータ200の位置を制御する。また制御ユニット400は、制御ユニット300からトルク指令値τref1~τref6を取得した場合、トルク指令値τref1~τref6と測定値τ1~τ6との差に基づき、マニピュレータ200の推力、即ち関節J1~J6の出力トルクを制御する。
力制御モードにおけるマニピュレータ200の力制御、即ちマニピュレータ200の関節のトルク制御について説明する。図5は、第1実施形態において制御部500がマニピュレータ200を力制御する処理のフローチャートである。まず、トルク指令値生成部504は、目標力Fd及び目標位置Pdを外部記憶装置322から取得する(S1)。トルク指令値生成部504は、目標力Fd、目標位置Pd、負荷トルクの測定値τ1~τ6、位置の測定値q1~q6に基づき、ロボットモデル503を用いてトルク指令値τref1~τref6を算出する(S2)。トルク制御部511~516は、トルク指令値τref1~τref6と負荷トルクの測定値τ1~τ6との差に基づくPID演算により、トルク指令値τF1~τF6を算出する(S3)。切替部531~536は、モータ制御部541~546にトルク指令値τF1~τF6をトルク指令値τM1~τM6として出力する。モータ制御部541~546は、トルク指令値τM1~τM6に応じてモータ211~216を通電制御する(S4)。モータ211~216は、通電に応じて関節J1~J6に出力トルクを発生する(S5)。制御ユニット300,400は、負荷トルクの測定値τ1~τ6及び関節の位置の測定値q1~q6を取得する(S6)。制御ユニット300は、駆動が終了したか否かを判定し(S7)、終了していない場合は(S7:No)、ステップS2~S6を繰り返す。制御部500は、駆動が終了した場合(S7:Yes)、制御処理を終了する。以上のフローチャートに従ってモータ211~216を駆動することで、マニピュレータ200のハンド252にかかる力Fを所望の目標力Fdに制御することが可能である。なお、図5に示すフローチャートの順番に限定するものではなく、他の順番でも力制御は可能である。
次に、位置制御モードにおけるマニピュレータ200の位置制御、即ちマニピュレータ200のハンド252の位置制御について説明する。図6は、第1実施形態において制御部500がマニピュレータ200を位置制御する処理のフローチャートである。まず、位置指令値生成部505は、目標位置Pdを外部記憶装置322から取得する(S11)。目標位置Pdは、マニピュレータ200を位置決めする目標位置である。位置指令値生成部505は、目標位置Pdに基づき、ロボットモデル503を用いて関節J1~J6の位置指令値qref1~qref6を算出する(S12)。位置制御部521~526は、位置指令値qref1~qref6と関節の位置の測定値q1~q6との差に基づくPID演算により、トルク指令値τq1~τq6を算出する(S13)。なお、ステップS13においては、測定値q1~q6の代わりに測定値θ1~θ6を用いてもよい。この場合、位置指令値qref1~qref6は、減速機221~226の減速比で位置指令値θref1~θref6に変換すればよい。切替部531~536は、モータ制御部541~546にトルク指令値τq1~τq6をトルク指令値τM1~τM6として出力する。モータ制御部541~546は、トルク指令値τM1~τM6に応じてモータ211~216を通電制御する(S14)。モータ211~216は、通電に応じて関節J1~J6に出力トルクを発生する(S15)。制御ユニット300,400は、関節の位置の測定値q1~q6を取得する(S16)。制御ユニット300は、駆動が終了したか否かを判定し(S17)、終了していない場合は(S17:No)、ステップS12~S16を繰り返す。制御部500は、駆動が終了した場合(S17:Yes)、制御処理を終了する。以上のフローチャートに従ってモータ211~216を駆動することで、マニピュレータ200のハンド252の位置を所望の目標位置Pdに制御することが可能である。なお、図6に示すフローチャートの順番に限定するものではなく、他の順番でも位置制御は可能である。
ところで、マニピュレータ200の力学モデルであるロボットモデル503と実機とは必ずしも一致せず、誤差が生じる。この誤差をモデル化誤差という。このモデル化誤差は、マニピュレータ200の経年劣化などでも変化する。また、トルクセンサ241~246の測定値τ1~τ6には、マニピュレータが置かれている環境(例えば温度)の変化の影響で、オフセット誤差が重畳する。これらの誤差が大きいと、マニピュレータを力制御しているときにマニピュレータが振動するなど、マニピュレータの挙動が不安定になることがある。モデル化誤差とオフセット誤差とを求め、これら誤差に基づいて、モデル及び測定値を校正することも考えられるが、校正の作業を度々行わなければならない。マニピュレータ200に連続運転で作業を行わせる場合には、校正作業の時間の分、サイクルタイムが増加してしまう。
そこで、本実施形態では、組立作業などの生産作業においてマニピュレータ200を動作させている制御の中で、モデル化誤差及びオフセット誤差をキャンセルさせる。図7は、第1実施形態に係る第1制御ユニットの処理の一部を示すブロック図である。図7には、トルク指令値生成部504の処理について図示している。トルク指令値生成部504は、インピーダンス制御部601、動力学計算部602、指令値補正部603、補正値計算部604として機能し、例えば図2に示すRAM303が補正値記憶部605として機能する。以下、各部の機能について説明する。
動力学計算部602は、制御ユニット400がマニピュレータ200の位置を制御しているときに、マニピュレータ200の位置の測定値である関節J1~J6の位置の測定値q1~q6を取得する。本実施形態では、動力学計算部602は、力制御を開始する位置を目標位置Pdとし、マニピュレータ200を目標位置Pdに位置決めした際に、測定値q1~q6を取得する。動力学計算部602は、取得したこれらの位置情報を微分して、速度、及び加速度を求める。動力学計算部602は、ロボットモデル503と関節J1~J6の位置、速度、及び加速度の情報に基づいて、公知の動力学計算手法であるニュートン・オイラー法により、関節J1~J6にかかる負荷トルクの計算値τ^1~τ^6を算出する。即ち、動力学計算部602は、マニピュレータ200の位置を制御しているときに、ロボットモデル503を用いた動力学の計算により、マニピュレータ200に生じる負荷としての負荷トルクの計算値τ^1~τ^6を求める。計算値τ^1~τ^6には、モデル化誤差が含まれる。なお、動力学の計算は、これに限定するものではなく、例えばラグランジュの定式化による導出方法などでもよい。
補正値計算部604は、制御ユニット400がマニピュレータ200の位置を制御しているときに、マニピュレータ200に生じる負荷の第1測定値として、負荷トルクの測定値τ1~τ6を取得する。本実施形態では、補正値計算部604は、マニピュレータ200を目標位置Pdに位置決めした際に、測定値τ1~τ6を取得する。測定値τ1~τ6には、オフセット誤差が含まれている。補正値計算部604は、測定値τ1~τ6と計算値τ^1~τ^6との差に基づいて補正値τcomp1~τcomp6を求める。具体的には、補正値計算部604は、位置制御モードから力制御モードに切り替わる直前に、計算値τ^1~τ^6から測定値τ1~τ6を減算することで、補正値τcomp1~τcomp6を求める。例えば補正値計算部604は、位置制御モード時に、計算値τ^1~τ^6から測定値τ1~τ6を減算する処理を、所定の周期で行い、位置制御モードから力制御モードに切り替わったときに、その直前の減算値を補正値τcomp1~τcomp6とする。補正値計算部604は、補正値τcomp1~τcomp6を補正値記憶部605に記憶させておく。なお、補正値τcomp1~τcomp6には、モデル化誤差とオフセット誤差が含まれている。
インピーダンス制御部601は、マニピュレータ200のハンド252に所望の機械インピーダンスを持たせるよう、ロボットモデル503に基づく関節J
1~J
6のトルク指令値τ
Fd1~τ
Fd6を、以下の式(1)に従って算出する。式(1)は、インピーダンスの制御式である。機械インピーダンスは、例えば剛性インピーダンス、粘性インピーダンス、及び力インピーダンスである。トルク指令値τ
Fd1~τ
Fd6は、補正前の仮の指令値であり、モデル化誤差が含まれている。
式(1)中、Mは6×6の慣性行列、Cは6×1のコリオリ力・遠心力のベクトル、Gは6×1の重力負荷トルクのベクトルである。Kdは、所望の機械インピーダンスの剛性の値、Ddは所望の機械インピーダンスの粘性の値を表わし、それぞれ6×6の行列である。Jacは、関節J1~J6の速度と、座標系To(図1)基準で表現されたハンド252の速度とを関連付ける6×6のヤコビ行列である。なお、ヤコビ行列Jacの添え字Tは行列の転置を表わしている。
このように、インピーダンス制御部601は、制御ユニット400がマニピュレータ200の推力を制御する際に、式(1)を用いて、教示データ501,502に基づく仮の指令値であるトルク指令値τFd1~τFd6を求める。動力学計算部602は、M、C、Gを、ロボットモデル503と、関節J1~J6の位置及び速度に基づいて、ニュートン・オイラー法で計算する。
指令値補正部603は、トルク指令値τFd1~τFd6を補正値τcomp1~τcomp6で補正し、トルク制御部511~516に出力する指令値であるトルク指令値τref1~τref6を求める。具体的には、指令値補正部603は、補正値記憶部605に記憶されている補正値τcomp1~τcomp6をインピーダンス制御部601で算出したトルク指令値τFd1~τFd6から減算することで、トルク指令値τref1~τref6を生成する。これにより、マニピュレータ200の実機とモデルとの差であるモデル化誤差がキャンセルされる。
トルク指令値τref1~τref6には、オフセット誤差が含まれている。図4に示すトルク制御部511~516は、制御ユニット400がマニピュレータ200の推力を制御する際に、マニピュレータ200に生じる負荷の第2測定値である関節J1~J6の負荷トルクの測定値τ1~τ6を取得する。この測定値τ1~τ6には、オフセット誤差が含まれている。トルク制御部511~516は、トルク指令値τref1~τref6と測定値τ1~τ6との差を求め、この差に基づき、フィードバック値であるトルク指令値τF1~τF6を求める。このように、マニピュレータ200の推力を制御するのに用いられるトルク指令値τF1~τF6において、オフセット誤差がキャンセルされることになる。
以上、本実施形態によれば、マニピュレータ200の推力の制御中、モデル化誤差とオフセット誤差が共にキャンセルされるので、マニピュレータ200の制御が安定し、マニピュレータ200の制御性が向上する。
以下、マニピュレータ200の制御の実験結果について説明する。図8は、第1実施形態におけるロボットアーム251の概略図である。図8中、T1~T6はリンク2100~2106に固定された座標系である。なお、各関節J1~J6の回転軸はZ軸で、回転方向は右手方向である。図8に示したロボットアーム251の姿勢を、関節J1~J6の角度の測定値q1~q6が0度の時の姿勢とする。また、リンク2100~2106の寸法は、図8に示す通りである。
図9(a)は、第1実施形態における現実のマニピュレータ200の動力学パラメータの一例を示す図である。図9(a)中、m^は、当該リンクの実際の質量である。s^x、s^y、s^zは、それぞれ当該リンクの座標原点から見たX軸、Y軸、Z軸方向の実際の重心位置である。I^xx、I^yy、I^zzは、それぞれ当該リンクの重心まわりのX軸、Y軸、Z軸に対する実際の慣性モーメントである。
図9(b)は、第1実施形態におけるロボットモデル503の動力学パラメータの一例を示す図である。ロボットモデル503は、ロボットアーム251の設計値からCADで事前に算出し、外部記憶装置322に記憶しておく。図9(b)中、mは当該リンクの設計上の質量である。sx、sy、szはそれぞれ当該リンクの座標原点から見たX軸、Y軸、Z軸方向の設計上の重心位置である。Ixx、Iyy、Izzはそれぞれ当該リンクの重心まわりのX軸、Y軸、Z軸に対する設計上の慣性モーメントである。本実施形態では、ロボットモデル503の各リンクの質量が、現実の各リンク2101~2106の質量の1割の誤差があるものとした。
図10(a)及び図10(b)は、第1実施形態におけるマニピュレータ200による組付作業を説明するためのマニピュレータ200の模式図である。本実施形態では、ハンド252で把持したワークW1をワークW2に組み付ける組み付け作業を行う場合について説明する。ワークW1は、円筒形状の凸部を有する部品であり、ワークW2は円筒形状の凹部を有する部品である。図10(a)中、P1は、マニピュレータ200のハンド252の座標系Teで示す力制御開始時の教示点であり、P2は、マニピュレータ200のハンド252の座標系Teで示す力制御の目標位置を示す教示点である。
図10(b)は、マニピュレータ200のハンド252の座標系Teが教示点P1にあるときのワークW1とワークW2との位置関係を説明するための拡大図である。ワークW2のとば口WAは、面取りされている。ワークW1が座標系ToのX軸方向に力制御で20mm進んだ時、ワークW1がとば口WAの内側にあれば、力制御の柔軟性により、ワークW1がワークW2の内面に倣い、組み付けが完了する。ワークW1がとば口WAの内側にあるとは、ワークW1の中心軸が座標系ToのZ軸方向の195mm~205mmの範囲内にあることである。このとき、ワークW1の傾き、及び座標系ToのY軸方向への移動は、組み付け作業には影響がないものとする。
図11(a)は、第1実施形態における教示点のパラメータを示す図である。図11(b)は、第1実施形態における力制御の機械インピーダンスのパラメータを示す図である。図11(a)に、教示点P1,P2の座標系Toを基準とする位置及び姿勢の値を示す。なお、図11(a)中のα、β、γは、それぞれZ軸、Y軸、X軸まわりの回転量を表わす。また、図11(b)に、機械インピーダンスのパラメータとして剛性Kd、粘性Dd、力Fdの値を示す。
図12は、第1実施形態に係る物品の製造方法を説明するフローチャートである。まず、制御部500は、マニピュレータ200の位置制御を開始し、マニピュレータ200のハンド252(座標系Te)を、力制御を開始する位置である教示点P1へ移動させる(S51)。
制御部500は、ロボットモデル503と関節J1~J6の位置、速度、及び加速度の情報に基づいて、現在の関節J1~J6の負荷トルクの計算値τ^1~τ^6を算出する(S52)。このとき、マニピュレータ200は必ずしも停止している必要はない。
制御部500は、第1測定値である、関節J1~J6の負荷トルクの測定値τ1~τ6を取得する(S53)。このとき、測定値τ1~τ6には、トルクセンサ241~246のオフセット誤差が含まれ、例えばマニピュレータ200の関節J1における測定値τ1および関節J2における測定値τ2に、-1.0Nmのオフセット誤差があるものとする。
制御部500は、計算値τ^1~τ^6から測定値τ1~τ6を減算することで、補正値τcomp1~τcomp6を算出する(S54)。このとき、算出されたトルク補正値τcomp1~τcomp6は、補正値記憶部605に記憶される。
制御部500は、位置制御モードから力制御モードへ切り替え、力制御を開始する(S55)。制御部500は、マニピュレータ200のハンド252に所望の機械インピーダンス(剛性Kd、粘性Dd、力Fd)を持たせるような関節J1~J6のトルク指令値τFd1~τFd6を式(1)に従って算出する(S56)。
制御部500は、ステップS56で算出したトルク指令値τFd1~τFd6から補正値記憶部605に記憶されている補正値τcomp1~τcomp6を減算してトルク指令値τref1~τref6を算出する(S57)。制御部500は、トルク指令値τref1~τref6と、第2測定値である測定値τ1~τ6との差に基づくPID演算により、トルク指令値τF1~τF6を算出する(S58)。
制御部500は、トルク指令値τF1~τF6をトルク指令値τM1~τM6とし、トルク指令値τM1~τM6に応じてモータ211~216を通電制御する(S59)。モータ211~216は、通電に応じて関節J1~J6に出力トルクを発生する(S60)。制御部500は、負荷トルクの測定値τ1~τ6及び関節の位置の測定値q1~q6を取得する(S61)。制御部500は、駆動が終了したか否かを判定し(S62)、終了していない場合は(S62:No)、ステップS56~S62を繰り返す。制御部500は、駆動終了と判断したら(S62:Yes)、制御処理を終了する。本実施形態では、ステップS55で力制御を開始してから5秒経過した時点で、制御処理を終了する。
図13(a)、図13(b)及び図13(c)は、実験結果を示すグラフである。図13(a)は、第1実施形態において、力制御を行っているときの時間に対するハンド252のX軸方向の位置を示すグラフである。
インピーダンス制御の目標位置Pdは、図13(a)に示すように、教示点P1から座標系ToでX軸方向に50mm移動した位置である。インピーダンス制御を開始した時点から3秒間、X軸方向に50mm線形変化するものとした。
図13(b)のグラフは、第1実施形態おいて、マニピュレータ200を力制御したときのマニピュレータ200のハンド252の座標系Teの位置変化を示している。横軸は、座標系Toにおける座標系TeのX軸方向の位置である。縦軸は、座標系Toにおける座標系TeのZ軸方向の位置である。ワークW1とワークW2との接触による位置変化は考慮しないものとする。
図13(b)に示す通り、座標系Teは、ワークW1がとば口WAに入る範囲、即ち座標系Teが座標系ToのX軸方向の位置270mmに到達したときに、座標系ToのZ軸方向の位置195mm~205mmの範囲内にある。したがってワークW2に対するワークW1の組み付けが成功する。
一方、ステップS57においてトルク指令値τF1~τF6を補正しない場合、即ち、トルク指令値τF1~τF6がトルク指令値τref1~τref6である場合を比較例とする。図13(c)のグラフは、比較例おいて、マニピュレータ200を力制御したときのマニピュレータ200のハンド252の座標系Teの位置変化を示している。
図13(c)に示すように、座標系Teが、座標系ToのX軸方向の位置270mmに到達したときに、座標系ToのZ軸方向の位置185mm以下にあり、組み付け可能な位置195mm~205mの範囲から10mm以上ずれている。したがってワークW2に対するワークW1の組み付けが失敗する。
このように、本実施形態では、マニピュレータ200の実機とモデルとの差が不明であっても、トルク指令値τFd1~τFd6を補正値τcomp1~τcomp6で補正することで、モデル化誤差をキャンセルすることができる。また、負荷トルクの測定値τ1~τ6に含まれるオフセット誤差が不明であっても、フィードバック演算において、トルク指令値τref1~τref6と測定値τ1~τ6との差を求めることで、オフセット誤差をキャンセルすることができる。よって、関節J1~J6のトルクの急変を防止することができる。
また、ロボットモデル503は、厳密に現実のマニピュレータ200に近付ける必要がなくなるので、ロボットモデル503の推定などの準備にかかる手間や時間を減らすことができる。さらに、環境変化や経時変化によるトルクセンサ241~246のオフセット誤差も事前に調べる必要がなくなり、校正作業にかかる手間や時間も減らすことができる。所定の校正作業を事前に行う必要なしに、力制御の作業ごとにモデル化誤差およびセンサオフセット誤差による制御性能の低下や不測の動作を防止することができる。
[第2実施形態]
第2実施形態に係るロボット装置について説明する。図14は、第2実施形態に係るロボット装置の概略構成を示す斜視図である。第2実施形態のロボット装置100Aにおいて、第1実施形態のロボット装置100と同様の構成については、同一符号を付して詳細な説明は省略する。図14に示すように、ロボット装置100Aは、第1実施形態と同様の構成のマニピュレータ200と、マニピュレータ200の位置又は推力を制御する制御部500Aを有する制御装置1000Aと、を備えている。また、ロボット装置100Aは、制御装置1000Aに教示データを送信する教示装置としての教示ペンダント600を備えている。
制御部500Aは、第1制御ユニットである制御ユニット300Aと、第2制御ユニットである制御ユニット400Aと、を有する。制御ユニット300Aは、ロボット装置100Aを統括的に制御するものである。制御ユニット400Aは、制御ユニット300Aからの指令に基づいて、マニピュレータ200の位置又は推力を制御するものである。制御ユニット400Aは、マニピュレータ200の各サーボ機構部201~206を制御するサーボ制御装置である。
制御部500Aについて具体的に説明する。図15及び図16は、第2実施形態に係る制御部500Aの構成を示すブロック図である。制御部500Aは、上述したように、制御ユニット300A,400Aを有する。図15には、制御ユニット300Aの詳細な構成、図16には、制御ユニット400Aの詳細な構成について図示している。
制御ユニット300Aは、コンピュータで構成されている。制御ユニット300Aのハードウェアは、第1実施形態と同様であり、CPU301、ROM302、RAM303、HDD304、記録ディスクドライブ305、及びインタフェース306~309を備えている。CPU301、ROM302、RAM303、HDD304、記録ディスクドライブ305、インタフェース306~309は、相互に通信可能にバス310で接続されている。ROM302には、CPU301に制御方法の処理を実行させるためのプログラム330Aが格納されている。
なお、第2実施形態では、コンピュータによって読取可能な記録媒体がROM302であり、ROM302にプログラム330Aが記録されているが、これに限定するものではない。プログラム330Aは、コンピュータによって読取可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。プログラム330Aを供給するための記録媒体としては、例えば、フレキシブルディスク、ハードディスク、光ディスク、光磁気ディスク、磁気テープ、不揮発性メモリ等を用いることができる。
図16に示す制御ユニット400Aは、コンピュータで構成されている。制御ユニット400Aのハードウェアは、第1実施形態と同様であり、CPU401と、ROM402と、RAM403と、インタフェース406~409と、を備えている。CPU401、ROM402、RAM403、インタフェース406~409は、相互に通信可能にバス410で接続されている。ROM402には、CPU401に制御方法の処理を実行させるためのプログラム430Aが格納されている。
インタフェース406には、モータ211~216が接続されている。インタフェース407には、位置センサ231~236が接続されている。インタフェース408には、トルクセンサ241~246が接続されている。インタフェース409には、制御ユニット300Aが接続されている。
なお、第2実施形態では、コンピュータによって読取可能な記録媒体がROM402であり、ROM402にプログラム430Aが記録されているが、これに限定するものではない。プログラム430Aは、コンピュータによって読取可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。プログラム430Aを供給するための記録媒体としては、例えば、フレキシブルディスク、ハードディスク、光ディスク、光磁気ディスク、磁気テープ、不揮発性メモリ等を用いることができる。
図17は、第2実施形態に係るロボット装置の制御システムを示す制御ブロック図である。図15に示す制御ユニット300AのCPU301は、プログラム330Aを実行することにより、図17に示すトルク指令値生成部504A及び位置指令値生成部505として機能する。図16に示す制御ユニット400AのCPU401は、プログラム430Aを実行することにより、図17に示すトルク制御部51A1~51A6、位置制御部521~526、切替部531~536、及びモータ制御部541~546として機能する。サーボ機構部201~206は、モータ211~216、減速機221~226、位置センサ231~236、及びトルクセンサ241~246を有する。
トルク指令値生成部504Aは、ロボットモデル503、目標力Fd、目標位置Pd、関節J1~J6の位置の測定値q1~q6、関節J1~J6のトルクの測定値τ1~τ6が入力される。そして、トルク指令値生成部504Aは、これらの値を用いて、関節J1~J6に対するトルク指令値τref1~τref6を求める。トルク指令値生成部504Aは、求めたトルク指令値τref1~τref6を、トルク制御部51A1~51A6に出力する。
トルク制御部51A1~51A6は、関節J1~J6のトルク指令値τref1~τref6と、関節J1~J6のトルクの測定値τ1~τ6との差が小さくなるように、トルク指令値τF1~τF6を求め、切替部531~536に出力する。トルク制御部51A1~51A6の詳細については後述する。なお、位置制御部521~526、切替部531~536、モータ制御部541~546は、第1実施形態と同様の処理を行うため、詳細な説明は省略する。
以上、制御ユニット400Aは、制御ユニット300Aから位置指令値qref1~qref6を取得した場合、位置指令値qref1~qref6と測定値q1~q6との差に基づき、マニピュレータ200の位置を制御する。制御ユニット400Aは、制御ユニット300Aからトルク指令値τref1~τref6を取得した場合、トルク指令値τref1~τref6と測定値τ1~τ6との差に基づき、マニピュレータ200の推力、即ち関節J1~J6の出力トルクを制御する。
ところで、マニピュレータで精密な組立作業を行う場合、マニピュレータを位置制御する位置制御モードにて組立を開始する所定位置にマニピュレータを移動させる。その後、マニピュレータを力制御する力制御モードに切り替えてからマニピュレータに精密組立を行わせることがある。位置制御モードから力制御モードに制御モードを切り替える際、仮に、マニピュレータの関節を駆動するモータへのトルク指令値が急激に変化すると、関節が急峻な動作をし、その結果、マニピュレータのハンドが振動的になることがある。これは、位置制御モードから力制御モードへの切り替え時に、フィードバック値が一旦ゼロにリセットされるためである。
図18は、第2実施形態に係る第2制御ユニットの処理の一部を示すブロック図である。図18には、トルク制御部51A1~51A6の処理について図示している。トルク制御部51A1~51A6は、主に、フィードバック制御部601~606、フィードフォワード演算部621~626、及び加算部631~636として機能する。また、図16に示すRAM403は、第1記憶部611~616及び第2記憶部711~716として機能する。以下、各部の機能について説明する。
第1記憶部611~616は、力制御モードに切り替わる前の位置制御モードにおいて、第1測定値である、トルクセンサ241~246を用いて測定した負荷トルクの測定値τ1~τ6を、オフセット値τoffset1~τoffset6として記憶する。即ち、トルク制御部51A1~51A6は、位置制御部521~526がマニピュレータ200の位置を制御しているときにマニピュレータ200に生じる負荷の第1測定値に基づくオフセット値τoffset1~τoffset6を求める。トルク制御部51A1~51A6は、第1記憶部611~616に、位置制御時に取得したオフセット値τoffset1~τoffset6を記憶させておく。第1記憶部611~616に記憶されたオフセット値τoffset1~τoffset6は、位置制御モードから力制御モードに切り替えた後、フィードバック値τFB1~τFB6に加算される。
本実施形態では、マニピュレータ200の位置が目標位置に到達したのをトリガとし、その時の測定値τ1~τ6をオフセット値τoffset1~τoffset6とする。なお、オフセット値τoffset1~τoffset6は、位置制御中、所定の制御周期で随時更新してもよい。この場合、位置制御モードから力制御モードに切り替わるときには、最後に更新された値が第1記憶部611~616に保持されることとなる。
フィードバック制御部601~606は、力制御モードにおいて、所定の制御周期で、トルク指令値生成部504Aからトルク指令値τref1~τref6と、トルクセンサ241~246を用いて測定した負荷トルクの測定値τ1~τ6と、を取得する。フィードバック制御部601~606は、トルク指令値τref1~τref6と、測定値τ1~τ6との差に基づき、フィードバック値τFB1~τFB6を算出する。フィードバック制御部601~606は、フィードバック演算の一例として、比例演算、積分演算、及び微分演算とからなるPID演算を行う。
フィードフォワード演算部621~626は、力制御時において、トルク指令値τref1~τref6に応じたフィードフォワード値τFF1~τFF6を求める。加算部631~636は、フィードバック値τFB1~τFB6に、オフセット値τoffset1~τoffset6と、フィードフォワード値τFF1~τFF6とを加算し、その値を、トルク指令値τF1~τF6として切替部531~536に出力する。
フィードフォワード演算部621~626の処理について具体例を挙げて説明する。ここで、トルク制御部51A1~51A6が入力を受けるトルク指令値τref1~τref6のうち、力制御時に最初に入力を受ける指令値(第2指令値)を、トルク指令値τ0ref1~τ0ref6とする。つまり、トルク指令値生成部504Aは、トルク指令値τ0ref1~τ0ref6を、マニピュレータ200の推力の制御を開始したときに最初に求める。また、トルク制御部51A1~51A6がトルク指令値τ0ref1~τ0ref6の後に入力を受ける指令値(第1指令値)をトルク指令値τ1ref1~τ1ref6とする。つまり、トルク指令値生成部504Aは、トルク指令値τ0ref1~τ0ref6の後にトルク指令値τ1ref1~τ1ref6を求める。よって、トルク指令値τ0ref1~τ0ref6は、トルク制御部51A1~51A6において、トルク指令値τ1ref1~τ1ref6の前に入力を受けるトルク指令値である。
フィードフォワード演算部62
1~62
6は、本実施形態では、モデルトルク演算部70
1~70
6と、減算部73
1~73
6とを含む。力制御を行う最初の制御サイクルについて説明する。モデルトルク演算部70
1~70
6は、ロボットモデル503を用いた動力学の計算により、トルク指令値τ0
ref1~τ0
ref6に応じた、マニピュレータ200に生じる負荷である負荷トルクの第2計算値である計算値τ0
FF1~τ0
FF6を求める。ロボットモデル503には、モータ21
1~21
6の負荷イナーシャJ
M及び粘性係数D
M、並びに減速機22
1~22
6の剛性値K及び減速比Nなどのパラメータが含まれる。本実施形態では、モデルトルク演算部70
1~70
6は、トルク指令値τ0
ref1~τ0
ref6と、ロボットモデル503に含まれるパラメータJ
M、D
M、K、Nと、に基づいて、計算値τ0
FF1~τ0
FF6を以下の式(2)に従って求める。sはラプラス演算子である。
第2記憶部711~716は、計算値τ0FF1~τ0FF6を、オフセット値である基準値τ0FF1~τ0FF6として記憶する。第2記憶部711~716は、力制御が継続される間、計算値τ0FF1~τ0FF6を基準値τ0FF1~τ0FF6として保持する。力制御モードから位置制御モードに切り替わったときには、第2記憶部711~716は、リセットされる。
減算部731~736は、計算値τ0FF1~τ0FF6から基準値τ0FF1~τ0FF6を引いた値を、フィードフォワード値τFF1~τFF6とし、加算部631~636に出力する。したがって、本実施形態では、フィードフォワード値τFF1~τFF6の初期値はゼロである。
次に、最初の制御サイクルの次の制御サイクル以降について説明する。モデルトルク演算部70
1~70
6は、ロボットモデル503を用いた動力学の計算により、トルク指令値τ1
ref1~τ1
ref6に応じた、マニピュレータ200に生じる負荷である負荷トルクの第1計算値である計算値τ1
FF1~τ1
FF6を求める。本実施形態では、モデルトルク演算部70
1~70
6は、トルク指令値τ1
ref1~τ1
ref6と、ロボットモデル503に含まれるパラメータJ
M、D
M、K、Nと、に基づいて、計算値τ1
FF1~τ1
FF6を以下の式(3)に従って求める。減算部73
1~73
6は、計算値τ1
FF1~τ1
FF6から基準値τ0
FF1~τ0
FF6を引いた値を、フィードフォワード値τ
FF1~τ
FF6とし、加算部63
1~63
6に出力する。
つまり、マニピュレータ200の関節J1~J6において、作用する負荷の変動で捩れ量が変動すると、ハンド252が振動する。本実施形態では、力制御を開始する時点での負荷を基準とし、負荷変動分(2つの計算値の差分)をフィードフォワード値τFF1~τFF6とすることで、力制御の応答性を高め、その結果、ハンド252の振動が抑制される。
なお、第2実施形態においては、第2記憶部711~716に記憶される基準値は、マニピュレータ200の推力の制御を開始して最初に求められる初期値である計算値τ0FF1~τ0FF6であるのが好ましいが、これに限定するものではない。例えば、初期値の次の値などを用いてもよい。即ち、基準値は、ある計算値τ1FF1~τ1FF6よりも前に、トルク指令値τ1FF1~τ1FF6よりも前のトルク指令値に応じて求められた、マニピュレータ200に生じる負荷の計算値であればよい。
図19は、第2実施形態に係る物品の製造方法を説明するフローチャートである。なお、第2実施形態では、第1実施形態と同様、図10(a)に示すように、ワークW1をハンド252に把持させ、ワークW1をワークW2に組み付けることで物品が製造されるものとする。
まず、制御部500Aは、マニピュレータ200の位置制御を開始し(S71)、力制御を開始する位置、即ち図10(a)に示す教示点P1に到達するまで、位置制御でマニピュレータ200を駆動する(S72)。
マニピュレータ200が力制御を開始する位置まで駆動したかどうかを判定する(S73)。制御部500Aは、マニピュレータ200がその位置まで移動した場合(S73:Yes)、当該位置におけるトルクの測定値τ1~τ6を取得する。制御部500Aは、図18に示す第1記憶部611~616に、測定値τ1~τ6をオフセット値τoffset1~τoffset6として記憶させる(S74)。
次に、制御部500Aは、位置制御モードから力制御モードへマニピュレータ200を制御する制御モードを切り替え(S75)、マニピュレータ200の力制御を開始する(S76)。力制御開始後、制御部500Aは、トルク指令値τ0ref1~τ0ref6を入力として、関節J1~J6における負荷トルクの計算値τ0FF1~τ0FF6を基準値として第2記憶部711~716に記憶させる(S77)。
制御部500Aは、フィードバック値τFB1~τFB6に、オフセット値τoffset1~τoffset6とフィードフォワード値τFF1~τFF6とを加算した値に基づいて、マニピュレータ200の推力を制御する。これにより、制御部500Aは、マニピュレータ200のハンド252に働く力が目標力となるように駆動する(S78)。
制御部500Aは、駆動が終了したかどうか、つまり組付けが完了したかどうかを判定する(S79)。制御部500Aは、駆動が終了していない場合(S79:No)、組付けが完了するまで、ステップS78及びS79のループを繰り返し実行する。制御部500Aは、駆動終了と判断したら(S79:Yes)、制御処理を終了する。
以上、第2実施形態によれば、位置制御モードから力制御モードへ切り替わる直前に、第1記憶部611~616にトルクの測定値τ1~τ6が、オフセット値τoffset1~τoffset6として記憶される。力制御モードに切り替わった後は、フィードバック値τFB1~τFB6に、オフセット値τoffset1~τoffset6が加算される。これにより、位置制御モードから力制御モードに制御が切り替わった際に、トルク指令値τM1~τM6の連続性が補償される。モータトルク指令値τM1~τM6の連続性が補償されるので、モータへ供給される電流Cur1~Cur6が急峻に変動するのが防止される。よって、マニピュレータ200の各関節において急峻な動作が防止され、マニピュレータ200のハンド252が振動するのを防止することができる。特に、マニピュレータ200を用いて精密な組立て作業を行う場合、マニピュレータ200のハンド252の振動が防止されるので、組立ワーク又は被組立ワークが変形したり破壊したりするのを防止することができる。また、組立不良を低減することができ、リトライ動作などが少なくなり、物品の生産性が向上する。
また、第2実施形態によれば、フィードバック値τFB1~τFB6に、フィードフォワード値τFF1~τFF6が加算されるので、マニピュレータ200における関節J1~J6の制御の応答性が向上する。このフィードフォワード値τFF1~τFF6は、負荷トルクの計算値τ1FF1~τ1FF6から、オフセットしている分として基準値τ0FF1~τ0FF6が除去されることで求められるので、トルク指令値τM1~τM6が急峻に変動するのが防止される。したがって、制御モードの切替時にハンド252が振動するのを効果的に防止することができる。以上、第2実施形態によれば、位置制御モードから力制御モードへ切り替わる際のマニピュレータ200の制御性が向上する。
図20は、第2実施形態における実験結果を示すグラフである。図20において、横軸は時間、縦軸はトルクである。図20には、関節J1のトルク指令値τM1と、関節J1の負荷トルクの測定値τ1について図示している。
制御モードの切り替え時、トルク指令値τM1は、トルク指令値τq1からトルク指令値τF1へ切り替わるが、急峻な変化をさせることなく滑らかに切り替わっている。このため、制御モードを切り替えた後のトルクの測定値τ1の振動が小さい。また、力制御時は、フィードバック値τFB1にフィードフォワード値τFF1を加算しているため、測定値τ1が応答性良く、トルク指令値τM1に追従していることが確認できる。
以上より、第2実施形態によれば、位置制御モードから力制御モードに制御モードを切り替える際において、制御モードの切り替え時のトルク指令値τM1~τM6の連続性を補償し、かつ力制御モード時の応答性を向上させることができる。これにより、制御モードを切り替える場合においても、マニピュレータ200の高精度かつ高応答な力制御が可能となる。
[第3実施形態]
次に、第3実施形態のロボット装置について説明する。なお、ロボット装置の構成は、第2実施形態と同様であるため、説明を省略する。
第2実施形態では、第2記憶部711~716に記憶させておく基準値を、モデルトルク演算部701~706にて算出した値の初期値とした。制御モードの切り替え直後は、フィードフォワード値τFF1~τFF6はゼロとなる。よって、この場合、トルク指令値τM1~τM6は、僅かな期間ではあるが一定値となり、滑らかに推移しない。
図21は、第3実施形態に係る第2制御ユニットの処理の一部を示すブロック図である。第3実施形態では、モデルトルク演算部701~706は、力制御実行の前の位置制御実行時に、第2計算値を求める演算処理を並行して行い、この値を第2記憶部711~716に記憶させておく。
具体的には、トルク指令値生成部504Aは、位置制御が実行されている時に、力制御が実行される時の第1指令値であるトルク指令値τ1ref1~τ1ref6よりも前に、第2指令値であるトルク指令値τ2ref1~τ2ref6を求める。トルク指令値生成部504Aは、位置制御中に目標力Fd、目標位置Pd、測定値τ1~τ6、及び測定値q1~q6を取得し、トルク指令値τ2ref1~τ2ref6を、取得したこれら値に基づいて生成する。
トルク指令値生成部504Aは、位置制御が実行されている時に、トルク指令値τ2ref1~τ2ref6をモデルトルク演算部701~706に出力する。そして、モデルトルク演算部701~706は、取得したトルク指令値τ2ref1~τ2ref6に応じた負荷トルクの第2計算値である計算値τ2FF1~τ2FF6を求める。そして、モデルトルク演算部701~706は、この計算値τ2FF1~τ2FF6を基準値τ2FF1~τ2FF6として第2記憶部711~716に記憶させる。なお、位置制御時、フィードバック制御部601~606、減算部731~736、及び加算部631~636は、演算処理を行う必要がないため行わない。
力制御を実行するときには、フィードバック制御部601~606は、トルク指令値τ1ref1~τ1ref6と測定値τ1~τ6との差に基づき、フィードバック値τFB1~τFB6を求める。モデルトルク演算部701~706は、トルク指令値τ1ref1~τ1ref6に応じた第1計算値である計算値τ1FF1~τ1FF6を求める。減算部731~736は、計算値τ1FF1~τ1FF6から基準値τ2FF1~τ2FF6を減算して、フィードフォワード値τFF1~τFF6を求める。そして、加算部631~636は、フィードバック値τFB1~τFB6に、オフセット値τoffset1~τoffset6、及びフィードフォワード値τFF1~τFF6を加算することで、トルク指令値τF1~τF6を求める。
これにより、力制御モードに切り替え直後であっても、トルク指令値τF1~τF6、即ち図17に示すトルク指令値τM1~τM6は、時間変化に対して滑らかに推移する。その結果、制御モード切り替え時にマニピュレータ200のハンド252が振動するのをさらに低減することができる。
図22は、第3実施形態における実験結果を示すグラフである。なお、図22のグラフには、第2実施形態における実験結果も図示している。図22には、関節J1のトルク指令値τM1を図示している。第1実施形態におけるトルク指令値τM1の時間変化率は、位置制御モードにおいては一定であるが、位置制御モードから力制御モードに切り替えた直後、一定とならずに変化する。一方、第2実施形態におけるトルク指令値τM1の時間変化率は、位置制御モードから力制御モードに切り替えた直後であっても、一定となる。
第3実施形態によれば、位置制御モードから力制御モードに制御モードを切り替える際において、制御モードの切替時のトルク指令値τM1~τM6を連続かつ滑らかにすることができる。その結果、制御モードの切替時に、マニピュレータ200のハンド252の振動をさらに抑制でき、高精度かつ高応答な力制御が可能となる。
[第4実施形態]
第4実施形態のロボット装置について説明する。図23は、第4実施形態に係るロボット装置の概略構成を示す斜視図である。第4実施形態のロボット装置100Bにおいて、第1、第2又は第3実施形態のロボット装置と同様の構成については、同一符号を付して詳細な説明は省略する。図23に示すように、ロボット装置100Bは、第1実施形態と同様の構成のマニピュレータ200と、マニピュレータ200を制御する制御部500Bを有する制御装置1000Bと、を備えている。また、ロボット装置100Bは、制御装置1000Bに教示データを送信する教示装置としての教示ペンダント600を備えている。
制御部500Bは、第1実施形態と同様の制御ユニット300と、第2又は第3実施形態と同様の制御ユニット400Aと、を有する。即ち、制御ユニット300は、第1実施形態において説明した処理を行い、制御ユニット400Aは、第2又は第3実施形態において説明した処理を行う。
制御ユニット300は、制御ユニット400Aがマニピュレータ200の位置を制御しているときに、補正値τcomp1~τcomp6(図7)を求めておく。制御ユニット300は、制御ユニット400Aにマニピュレータ200の推力を制御させる際に、教示データである力教示データ501及び位置教示データ502に基づく仮の指令値であるトルク指令値τFd1~τFd6を求める。そして、制御ユニット300は、トルク指令値τFd1~τFd6を補正値τcomp1~τcomp6で補正することでトルク指令値τref1~τref6を求めて、制御ユニット400Aにトルク指令値τref1~τref6を出力する。
一方、制御ユニット400Aは、マニピュレータ200の位置を制御しているときに、図18に示すように、オフセット値τoffset1~τoffset6を求めておく。制御ユニット400Aは、マニピュレータ200の推力を制御する際に、フィードバック値τFB1~τFB6及びフィードフォワード値τFF1~τFF6を求める。そして、制御ユニット400Aは、フィードバック値τFB1~τFB6に、オフセット値τoffset1~τoffset6と、フィードフォワード値τFF1~τFF6と、を加算して、トルク指令値τF1~τF6を生成する。制御ユニット400Aは、このトルク指令値τF1~τF6に基づいてマニピュレータ200の推力を制御する。
このように、第4実施形態では、制御部500Bが、第1実施形態で説明した制御処理と、第2又は第3実施形態で説明した制御処理とを組み合わせて制御処理を実行することで、マニピュレータ200の制御性が更に向上する。
なお、本発明は、以上説明した実施形態に限定されるものではなく、本発明の技術的思想内で多くの変形が可能である。また、実施形態に記載された効果は、本発明から生じる最も好適な効果を列挙したに過ぎず、本発明による効果は、実施形態に記載されたものに限定されない。
上述の実施形態では、ロボットアーム251が垂直多関節のロボットアームである場合について説明したが、これに限定するものではない。例えば、水平多関節のロボットアーム、パラレルリンクのロボットアーム、直交ロボット等、種々のロボットアームであってもよい。
(その他の実施例)
本発明は、上述の実施形態の1以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける1つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、1以上の機能を実現する回路(例えば、ASIC)によっても実現可能である。