JP2014000649A - ロボット制御装置、ロボット装置、ロボット制御方法、プログラム及び記録媒体 - Google Patents

Abstract

【課題】ロボットアーム本体の駆動再開時に正確にフレームの温度を推定して、ロボットアーム本体の先端を目標位置に正確に位置決めする。
【解決手段】フレーム温度演算部１０５は、ロボットアーム本体の駆動中に、フレームの熱特性、温度センサ１３０〜１３４による温度検出結果及びフレームの前回の推定温度に基づいてフレームの今回の推定温度を所定時間間隔で推定する。フレーム温度演算部１０５は、ロボットアーム本体の駆動を再開したときには、フレームの熱特性、ロボットアーム本体の駆動を停止していた駆動停止時間、及びロボットアーム本体の駆動を停止する直前に求めたフレームの推定温度に基づき、フレームの前回の推定温度を推定する。
【選択図】図３

Description

本発明は、ロボットアームを制御するロボット制御装置、ロボット制御装置を備えたロボット装置、ロボット制御方法、プログラム及び記録媒体に関する。

一般に、関節で連結された複数のフレームを有するロボットアームでは、各関節を駆動するモータがフレームの内部に配置された構成となっている。そして、各モータには、モータの過熱を防止するために、モータの温度を検出する温度センサが設けられている。

このように、モータは発熱体であるため、モータを内蔵したロボットアーム本体を駆動し続けると、モータの発熱によりフレームが熱膨張し、ロボットアーム本体の先端の位置がずれることがあった。

そこで従来、モータに設けられた温度センサの検出温度、及びフレームの前の推定温度に基づいてフレームの現在の温度を推定し、フレームの現在の推定温度及びフレームの熱膨張率に基づき、ロボットアーム本体の先端の位置ずれを計算していた。そして、ロボットアーム本体の先端の目標位置に対するずれ量を解消するように各モータの回転位置を制御していた（特許文献１参照）。

特開２００９−２９７８２９号公報

しかしながら、ロボットアーム本体の駆動を停止する（即ち、電源を切る）と、ロボットアーム本体が駆動停止している間は、ロボット制御装置におけるプログラムの実行が停止し、フレームの温度の計算は行われないのが一般的である。

したがって、従来の構成では、ロボットアーム本体が駆動停止している間は、駆動停止時間中のフレームの温度履歴が分からないため、ロボットアーム本体の駆動再開時にフレームの温度を正確に推定することはできなかった。そのため、従来、ロボットアーム本体の各フレームの温度を正確に推定するためには、各フレームの温度が雰囲気温度に収束するまでロボットアーム本体の駆動を再開するのを待つ必要があり、改良が求められていた。

そこで、本発明は、ロボットアーム本体の駆動再開時に正確にフレームの温度を推定して、ロボットアーム本体の先端を目標位置に正確に位置決めすることを目的とするものである。

本発明は、関節を有するロボットアーム本体と、前記ロボットアーム本体のフレームの内部に設けられ、前記関節を駆動するアクチュエータと、前記アクチュエータの温度を検出する温度センサと、を有するロボットアームを制御するロボット制御装置において、入力を受けた駆動指令に基づいて前記アクチュエータの駆動を制御する駆動制御部と、前記ロボットアーム本体の駆動中に、前記フレームの熱特性、前記温度センサによる温度検出結果及び前記フレームの前回の推定温度に基づいて前記フレームの今回の推定温度を所定時間間隔で推定し、前記フレームの今回の推定温度から前記ロボットアーム本体の先端の推定位置を推定し、前記ロボットアーム本体の先端の目標位置に対する推定位置のずれ量に基づいて、前記駆動指令を算出する演算部と、を備え、前記演算部は、前記ロボットアーム本体の駆動を再開したときには、前記フレームの熱特性、前記ロボットアーム本体の駆動を停止していた駆動停止時間、及び前記ロボットアーム本体の駆動を停止する直前に求めた前記フレームの推定温度に基づき、前記フレームの前回の推定温度を推定することを特徴とする。

また、本発明は、ロボットアームが、関節を有するロボットアーム本体、前記ロボットアーム本体のフレームの内部に設けられ、前記関節を駆動するアクチュエータ、及び前記アクチュエータの温度を検出する温度センサを有しており、入力を受けた駆動指令に基づいて、前記アクチュエータの駆動を制御する駆動制御部と、前記駆動制御部に前記駆動指令を出力する演算部と、を有するロボット制御装置を用いて、前記ロボットアームを制御するロボット制御方法において、前記演算部が、前記ロボットアーム本体の駆動中に、前記フレームの熱特性、前記温度センサによる温度検出結果及び前記フレームの前回の推定温度に基づいて前記フレームの今回の推定温度を所定時間間隔で推定する温度推定工程と、前記演算部が、前記フレームの今回の推定温度から前記ロボットアーム本体の先端の位置を推定し、前記ロボットアーム本体の先端の目標位置に対する推定位置のずれ量を算出するずれ量算出工程と、前記演算部が、前記ずれ量に基づいて前記駆動指令を算出する駆動指令算出工程と、前記演算部が、前記ロボットアーム本体の駆動を停止した後に前記ロボットアーム本体の駆動を再開した場合には、前記フレームの熱特性、前記ロボットアーム本体の駆動を停止していた駆動停止時間、及び前記ロボットアーム本体の駆動を停止する直前に求めた前記フレームの推定温度に基づき、前記フレームの前回の推定温度を推定する駆動再開時温度推定工程と、を備えたことを特徴とする。

本発明によれば、フレームの温度が雰囲気温度に下がり切る前にロボットアーム本体の駆動を再開しても、正確にフレームの温度を推定することができる。その結果、駆動再開時においてロボットアーム本体の先端を目標位置に正確に位置決めすることができる。

本発明の実施形態に係るロボット装置の概略構成を示す説明図である。 本発明の実施形態に係るロボット制御装置の構成を示すブロック図である。 本発明の実施形態に係るロボット制御装置の機能を示す機能ブロック図である。 本発明の実施形態に係るロボット制御装置のＣＰＵの制御動作を示すフローチャートである。 ロボットアーム本体の駆動を停止した時と駆動を再開した時とのモータ温度、フレーム温度及び周囲温度の温度変化を説明するための図である。 ロボットアーム本体の駆動中におけるモータ温度、フレーム温度及び周囲温度の温度変化を説明するための図である。

以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。図１は、本発明の実施形態に係るロボット装置の概略構成を示す説明図である。ロボット装置１００は、多関節のロボットアーム本体１１５を有するロボットアーム１１４と、ロボットアーム本体１１５の先端１１５ａに設けられた、エンドエフェクタとしてのロボットハンド１２９と、を備えている。また、ロボット装置１００は、ロボットアーム１１４及びロボットハンド１２９を制御するロボット制御装置１０１を備えている。

ロボットアーム１１４は、各関節１２１〜１２４で連結された複数のフレーム１１６〜１２０と、各関節１２１〜１２４を駆動する複数のアクチュエータとしての電動モータ（以下、「モータ」という）１２５〜１２８と、を有している。

具体的に説明すると、第２フレーム１１７は、第１フレーム１１６に対して第１関節１２１で回転可能（旋回可能）に連結されており、第３フレーム１１８は、第２フレーム１１７に対して第２関節１２２で回転可能（旋回可能）に連結されている。また、第４フレーム１１９は、第３フレーム１１８に対して第３関節１２３で回転可能に連結されており、第５フレーム１２０は、第４フレーム１１９に対して第４関節１２４で回転可能（旋回可能）に連結されている。

第１関節１２１を駆動する第１モータ１２５は、第１フレーム１１６の内部に配置されており、第２関節１２２を駆動する第２モータ１２６は、第２フレーム１１７の内部に配置されている。また、第３関節１２３を駆動する第３モータ１２７は、第３フレーム１１８の内部に配置されており、第４関節１２４を駆動する第４モータ１２８は、第４フレーム１１９の内部に配置されている。

更に、各モータ１２５〜１２８の内部には、各モータ１２５〜１２８の温度を検出するための熱電対、測温抵抗体、サーミスタ等の温度センサ１３１〜１３４がそれぞれ設置されている。

エンドエフェクタとしてのロボットハンド１２９は、第５フレーム１２０の先端に設置され、不図示のワークを把持する等、ワークに直接効果を与えるものである。なお、ロボットアーム１１４のロボットアーム本体１１５の周囲には、ロボットアーム本体１１５の周囲温度（雰囲気温度）を検出するための熱電対、測温抵抗体、サーミスタ等の温度センサ１３０が設けられている。

図２は、本発明の実施形態に係るロボット制御装置の構成を示すブロック図である。ロボット制御装置１０１は、演算部としてのＣＰＵ１０３、ＲＯＭ１４１、ＲＡＭ１４２、記憶部としてのＨＤＤ１０４、記録ディスクドライブ１４３及び各種のインタフェース１４４〜１４７を備えている。更に、ロボット制御装置１０１は、複数のモータ１２５〜１２８の駆動を制御する駆動制御部としてのモータ制御部１０２を有している。

ＣＰＵ１０３には、ＲＯＭ１４１、ＲＡＭ１４２、ＨＤＤ１０４、記録ディスクドライブ１４３及び各種のインタフェース１４４〜１４７が、バス１４８を介して接続されている。ＲＯＭ１４１には、ＢＩＯＳ等の基本プログラムが格納されている。ＲＡＭ１４２は、ＣＰＵ１０３の演算処理結果を一時的に記憶する記憶装置である。

ＨＤＤ１０４は、ＣＰＵ１０３の演算処理結果である各種のデータ等を記憶する記憶部であると共に、ＣＰＵ１０３に各種演算処理を実行させるためのプログラム１５１を記録するものである。ＣＰＵ１０３は、ＨＤＤ１０４に記録（格納）されたプログラム１５１に基づいて各種演算処理を実行する。

温度センサ１３０〜１３４は、インタフェース１４４に接続されており、ＣＰＵ１０３は、インタフェース１４４及びバス１４８を介して温度センサ１３０〜１３４からの温度検出結果（温度データ）の入力を受ける。

モータ制御部１０２は、インタフェース１４７に接続されている。ＣＰＵ１０３は、各モータ１２５〜１２８の回転角度の制御量を示すそれぞれの駆動指令のデータを所定時間間隔でバス１４８及びインタフェース１４７を介してモータ制御部１０２に出力する。モータ制御部１０２は、演算部としてのＣＰＵ１０３から入力を受けた各駆動指令に基づき、各モータ１２５〜１２８への電流の出力量を演算し、各モータ１２５〜１２８へ電流を供給して、ロボットアーム本体１１５の先端１１５ａの位置制御を行う。各駆動指令は、各モータ１２５〜１２８へ出力する電流値を示す電流指令である。

なお、インタフェース１４５には、モニタ１４９が接続されており、モニタ１４９には、各種画像が表示される。インタフェース１４６は、書き換え可能な不揮発性メモリや外付けＨＤＤ等の外部記憶装置１５０が接続可能に構成されている。記録ディスクドライブ１４３は、記録ディスク１５２に記録されたプログラム等を読み出すことができる。

図３は、本発明の実施形態に係るロボット制御装置の機能を示す機能ブロック図である。本実施形態では、演算部としてのＣＰＵ１０３は、操作者による電源オン等の起動操作により、プログラム１５１（図２）をＨＤＤ１０４から読み出して実行する。そして、ＣＰＵ１０３は、プログラム１５１を実行することにより、フレーム温度演算部１０５、フレーム膨張演算部１０６、順運動学演算部１０７及び逆運動学演算部１０８として機能する。なお、ＣＰＵ１０３は、操作者による電源オフ等の操作或いは緊急停止により、プログラム１５１の実行を停止する。

記憶部としてのＨＤＤ１０４は、互いに異なる記憶エリアを示す、データ記憶部１０９、温度計算式記憶部１１０、膨張計算式記憶部１１１及び軌道記憶部１１２を有している。なお、本実施形態では、ＨＤＤ１０４がこれら記憶部１０９〜１１２を有しているが、記憶媒体はＨＤＤ１０４に限らず、電源オフとなってもデータが消去されない書き換え可能な不揮発性の記憶媒体であればいかなる記憶媒体でもよい。また、記憶媒体の数も、１つに限定するものではなく、複数であってもよい。

フレーム温度演算部１０５は、ロボットアーム本体１１５の駆動中、各温度センサ１３０〜１３４による温度検出結果及び各フレーム１１６〜１１９の前回の推定温度に基づいて各フレーム１１６〜１１９の今回の推定温度を所定時間間隔で演算により推定する。ここで、「今回の推定温度」とは、「現在の推定温度」であり、「前回の推定温度」とは、「今回の推定温度」に対して所定時間間隔前に推定したものである。このフレーム温度演算部１０５は、複数のフレーム１１６〜１２０のうち、発熱体であるモータ１２５〜１２８が内部に設けられたフレーム１１６〜１１９の推定温度を推定している。

データ記憶部１０９には、フレーム温度演算部１０５により算出された各フレーム１１６〜１１９の推定温度が記憶される。また、データ記憶部１０９には、この推定温度に対応付けられ、この推定温度の算出処理を開始した時の時刻が記憶される。また、データ記憶部１０９には、温度センサ１３０により検出され、この推定温度を求めたときに使用された周囲温度が記憶される。つまり、これら推定温度、周囲温度及び時刻がデータとしてデータ記憶部１０９に記憶される。

なお、データ記憶部１０９には、各フレーム１１６〜１１９の推定温度が算出される度に各データが上書きされるようにするのが好ましいが、順次、各データを記憶させるようにしてもよい。いずれにしても、フレーム温度演算部１０５は、ロボットアーム本体１１５の駆動中は、データ記憶部１０９に記憶されている各フレーム１１６〜１１９の最新の推定温度を、各フレーム１１６〜１１９の今回の推定温度を求める際の前回の推定温度として読み出す。

温度計算式記憶部１１０には、各フレーム１１６〜１１９の温度の推定に使用する演算式が記憶されている。つまり、フレーム温度演算部１０５は、温度計算式記憶部１１０に記憶された演算式を用いて、各フレーム１１６〜１１９の推定温度を算出する。

膨張計算式記憶部１１１には、各フレーム１１６〜１１９の膨張量を演算する演算式が記憶されている。フレーム膨張演算部１０６は、フレーム温度演算部１０５から各フレーム１１６〜１１９の今回の推定温度を取得し、膨張計算式記憶部１１１に記憶された演算式を用いて、各フレーム１１６〜１１９の膨張量を演算により推定する。

軌道記憶部１１２には、ロボットアーム本体１１５の目標軌道（即ち、ロボットアーム本体１１５の先端１１５ａの目標位置）が記憶されている。

順運動学演算部１０７は、各フレーム１１６〜１１９の膨張量からロボットアーム本体１１５の先端１１５ａの推定位置を順運動学を用いて算出し、軌道記憶部１１２から読み込んだ目標位置と推定位置との差分である位置のずれ量を算出する。そして、このずれ量に基づいて、モータ制御部１０２に出力する駆動指令を算出する。

図４は、本発明の実施形態に係るロボット制御装置のＣＰＵの制御動作を示すフローチャートである。以下、ロボット制御装置１０１の動作について、図４に示すフローチャートを参照しながら詳細に説明する。

まず、フレーム温度演算部１０５は、ロボットアーム本体１１５の駆動を再開すると、データ記憶部１０９に記憶されている各フレーム１１６〜１１９の推定温度、周囲温度及び時刻のデータを読み出す（Ｓ１）。ここで、データ記憶部１０９から読み出すデータは、ロボットアーム本体１１５が駆動停止する直前の各フレーム１１６〜１１９の推定温度、これら推定温度を推定した時刻、及び推定温度を演算したときに用いた周囲温度である。

なお、ロボットアーム本体１１５の駆動を再開するとは、ロボット制御装置１０１のＣＰＵ１０３がプログラム１５１の実行を再開する（駆動指令の演算処理が可能となる状態になる）ことである。つまり、駆動を再開した時刻は、ＣＰＵ１０３がプログラム１５１に従って演算処理を再開した時刻である。また、ロボットアーム本体１１５の駆動を停止するとは、ＣＰＵ１０３がプログラム１５１の実行を停止することである。つまり、駆動を停止した時刻は、ＣＰＵ１０３がプログラム１５１に従う演算処理を停止した時刻である。

次に、フレーム温度演算部１０５は、現在の時刻（即ち、ロボットアーム本体１１５の駆動を再開した時の時刻）と、データ記憶部１０９から読み出してきた時刻との差分である経過時間（駆動停止時間）を算出する（Ｓ２）。つまり、このステップＳ２では、フレーム温度演算部１０５は、読み込んだ時刻と現在時刻との差分を算出することで、各フレーム１１６〜１１９の温度の算出が停止してから再開するまでの経過時間を見積もる。

また、フレーム温度演算部１０５は、現在の時刻（即ち、ロボットアーム本体１１５の駆動を再開した時の時刻）で温度センサ１３０から周囲温度を読み込む。（Ｓ３）。

次に、フレーム温度演算部１０５は、温度計算式記憶部１１０から以下の式（１）〜式（４）の計算式を読み出す。そして、フレーム温度演算部１０５は、データ記憶部１０９から読み出したデータから、ロボットアーム本体１１５の駆動再開時における各フレーム１１６〜１１９の推定温度を、式（１）〜式（４）を用いて算出する（Ｓ４：駆動再開時温度推定工程）。
α_１ｓ＝Ａ_１・α_１ｅ・ｅｘｐ（−Δｔ／Ｂ_１）＋（１−Ａ_１）・α_１ｅ・ｅｘｐ（−Δｔ／Ｃ_１）＋Δα_Ｒ ・・・（１）
α_２ｓ＝Ａ_２・α_２ｅ・ｅｘｐ（−Δｔ／Ｂ_２）＋（１−Ａ_２）・α_２ｅ・ｅｘｐ（−Δｔ／Ｃ_２）＋Δα_Ｒ ・・・（２）
α_３ｓ＝Ａ_３・α_３ｅ・ｅｘｐ（−Δｔ／Ｂ_３）＋（１−Ａ_３）・α_３ｅ・ｅｘｐ（−Δｔ／Ｃ_３）＋Δα_Ｒ ・・・（３）
α_４ｓ＝Ａ_４・α_４ｅ・ｅｘｐ（−Δｔ／Ｂ_４）＋（１−Ａ_４）・α_４ｅ・ｅｘｐ（−Δｔ／Ｃ_４）＋Δα_Ｒ ・・・（４）

式（１）は第１フレーム１１６、式（２）は第２フレーム１１７、式（３）は第３フレーム１１８、式（４）は第４フレーム１１９のロボットアーム本体１１５を駆動再開した時の推定温度を算出する計算式である。

ここで、α_ｉｓ（ｉ＝１，２，３，４）は、ロボットアーム本体１１５の駆動再開時の各フレーム１１６〜１１９の推定温度であり、後の演算に用いる、各フレーム１１６〜１１９の前回の推定温度となる。

α_ｉｅ（ｉ＝１，２，３，４）は、ステップＳ１で読み出した、ロボットアーム本体１１５の駆動を停止する直前の各フレーム１１６〜１１９の推定温度である。Δｔは、ステップＳ２で算出した駆動停止時間（経過時間）である。Δα_Ｒは、ステップＳ１でデータ記憶部１０９から読み出した周囲温度に対する、ステップＳ３で温度センサ１３０から読み込んだ現在（ロボットアーム本体１１５の駆動再開時）の周囲温度の変化分である。

また、Ａ_ｉ、Ｂ_ｉ、Ｃ_ｉ（ｉ＝１，２，３，４）は、各フレーム１１６〜１１９から大気への放熱特性と各モータ１２５〜１２８から各フレーム１１６〜１１９への伝熱特性を示す係数である。これら係数Ａ_ｉ、Ｂ_ｉ、Ｃ_ｉは、第１フレーム１１６から第４フレーム１１９の特性を予め測定することで決定する。

つまり、フレーム温度演算部１０５は、ステップＳ４において、各フレーム１１６〜１１９の熱特性（放熱特性及び伝熱特性）、駆動停止時間Δｔ、及び各フレーム１１６〜１１９の推定温度α_ｉｅに基づき、各フレーム１１６〜１１９の推定温度α_ｉｓを推定する。

ここで、ロボットアーム本体１１５の駆動再開時の第１フレーム１１６の温度推定方法を例に挙げて詳しく説明する。

図５は、ロボットアーム本体１１５の駆動を停止した時と駆動を再開した時とのモータ温度、フレーム温度及び周囲温度の温度変化を説明するための図である。なお、図５において、ｔは時間、ｔ_ｅはロボットアーム本体１１５の駆動を停止した（電源を切った）時刻、ｔ_ｓはロボットアーム本体１１５の駆動を再開した時刻である。また、αは上昇温度、α_１ｅはロボットアーム本体１１５の駆動を停止する直前の第１フレーム１１６の推定温度、α_１ｓは駆動再開時の第１フレーム１１６の推定温度、α_Ｒは周囲温度、Δα_Ｒは駆動停止時に対する駆動再開時の周囲温度の変化分である。

図５において、時刻０から時刻ｔ_ｅまでロボットアーム本体１１５を駆動させている。この間に第１モータ１２５の温度は、図５に示すように温度上昇し、第１フレーム１１６の今回の推定温度は、後述する方法で時刻ｔ_ｅまで、第１モータ１２５の検出温度及び第１フレーム１１６の前回の推定温度から推定する。

ロボットアーム本体１１５の駆動を停止した時刻ｔ_ｅからロボットアーム本体１１５の駆動を再開した時刻ｔ_ｓまでの駆動停止時間Δｔの間は、第１モータ１２５の検出温度の取得や第１フレーム１１６の推定温度の算出は行われない。ロボットアーム本体１１５の駆動を再開した時刻ｔ_ｓにおける第１フレーム１１６の温度α_１ｓは、時刻ｔ_ｅから時刻ｔ_ｓまでの第１モータ１２５の温度履歴と、周囲温度の変化分Δα_Ｒで決まるものである。そのため、ロボットアーム本体１１５の駆動を再開した時の第１モータ１２５の温度α_１Ｍでは第１フレーム１１６の温度α_１ｓを推定することはできない。

そこで、本実施形態では、フレーム温度演算部１０５は、上述した式（１）に、第１フレーム１１６の推定温度α_１ｅと、駆動停止時間Δｔと、周囲温度の変化分Δα_Ｒとの値を代入することで、第１フレーム１１６の推定温度α_１ｓを推定する。このように推定した第１フレーム１１６の推定温度α_１ｓは、後のステップＳ６の計算処理において、第１フレーム１１６の前回の推定温度として用いる。

次に、フレーム温度演算部１０５は、前回、各フレーム１１６〜１１９の推定温度を演算した時刻から所定時間間隔後に、温度センサ１３０〜１３４から、モータ１２５〜１２８の温度と、ロボットアーム本体１１５の周囲温度とを読み込む（Ｓ５）。ここで、前回、各フレーム１１６〜１１９の推定温度を演算した時刻には、駆動開始時に各フレーム１１６〜１１９の推定温度を演算した時刻α_ｉｓも含んでいる。

そして、フレーム温度演算部１０５は、温度計算式記憶部１１０から読み出した計算式を用いて、各フレーム１１６〜１１９の今回（現在）の推定温度を推定する演算処理を実行する（Ｓ６：温度推定工程）。算出した各フレーム１１６〜１１９の今回の推定温度は、フレーム膨張演算部１０６に出力する。

ここで、ステップＳ６において、フレーム温度演算部１０５により温度計算式記憶部１１０から読み出される計算式は、例えば以下のように記述される。
α’_１＝α_１＋ｄｔ（Ｄ_１・α_Ｍ１−Ｅ_１・α_１＋Ｆ_１・α_Ｒ） ・・・（５）
α’_２＝α_２＋ｄｔ（Ｄ_２・α_Ｍ２−Ｅ_２・α_２＋Ｆ_２・α_Ｒ） ・・・（６）
α’_３＝α_３＋ｄｔ（Ｄ_３・α_Ｍ３−Ｅ_３・α_３＋Ｆ_３・α_Ｒ） ・・・（７）
α’_４＝α_４＋ｄｔ（Ｄ_４・α_Ｍ４−Ｅ_４・α_４＋Ｆ_４・α_Ｒ） ・・・（８）

式（５）は第１フレーム１１６、式（６）は第２フレーム１１７、式（７）は第３フレーム１１８、式（８）は第４フレーム１１９の今回の推定温度を算出する計算式である。

ここで、式（５）乃至式（８）は以下のように導き出した。まず、各フレーム１１６〜１１９に伝熱される熱量をＱ_ｉｎ（Ｊ）、各フレーム１１６〜１１９から周囲への放熱量をＱ_ｏｕｔ（Ｊ）とすると、以下の式（９）及び式（１０）となる。
Ｑ_ｉｎ＝ｋ・Ｓ／ｘ（α_Ｍ−α）ｄｔ ・・・（９）
Ｑ_ｏｕｔ＝ｈＳ’（α−α_Ｒ）ｄｔ ・・・（１０）

ここで、ｋは各モータ１２５〜１２８から各フレーム１１６〜１１９への熱の伝わり易さを示す熱伝導率（Ｗ／ｍＫ）、Ｓは熱の伝わる断面積（ｍ^２）、ｘは熱の伝わる距離（ｍ）である。α_Ｍはモータ温度の基準値からの温度変化量（℃）、αはフレーム温度の基準値からの温度変化量（℃）、ｈは各フレーム１１６〜１１９から周囲への放熱のし易さを示す熱伝達率（Ｗ／ｍ^２Ｋ）である。Ｓ’は各フレーム１１６〜１１９から周囲へ放熱する表面積（ｍ^２）である。ｄｔは計算間隔（所定時間間隔）である。

更に、各フレーム１１６〜１１９の温度上昇は、Ｑ_ｉｎとＱ_ｏｕｔの差によるものであるため、以下の式（１１）で示すことができる。
Ｑ_ｉｎ−Ｑ_ｏｕｔ＝ｃρＶ（α’−α） ・・・（１１）

ここで、ｃは各フレーム１１６〜１１９の比熱（Ｊ／ｋｇＫ）、ρは各フレーム１１６〜１１９の密度（ｋｇ／ｍ^３）、Ｖは各フレーム１１６〜１１９の体積（ｍ^３）である。αは各フレーム１１６〜１１９の前回の推定温度（℃）、α’は各フレーム１１６〜１１９の今回の推定温度（℃）である。

次に式（９）、式（１０）、式（１１）を次式（１２）のように整理した。
Ｋ_１（α_Ｍ−α）ｄｔ−Ｋ_２（α−α_Ｒ）ｄｔ＝Ｋ_３（α’−α） ・・・（１２）

Ｋ_１はｋ・Ｓ／ｘ、Ｋ_２はｈＳ’、Ｋ_３はｃρＶとした。

更に式（１２）を整理すると、次式（１３）で表すことができる。
α’＝α＋ｄｔ（Ｄ・α_Ｍ−Ｅ・α＋Ｆ・α_Ｒ） ・・・（１３）

ＤはＫ_１／Ｋ_３、Ｅは（Ｋ_１＋Ｋ_２）／Ｋ_３、ＦはＫ_２／Ｋ_３とした。式（１３）は各フレーム１１６〜１１９に有るため、式（５）乃至式（８）の４つの式を用意した。Ｄ_ｉ、Ｅ_ｉ、Ｆ_ｉ（ｉ＝１，２，３，４）は、各フレーム１１６〜１１９において温度特性を予め測定することで決定する係数である。

つまり、フレーム温度演算部１０５は、ステップＳ６において、各フレーム１１６〜１１９の熱特性、温度センサ１３０〜１３４による温度検出結果及び各フレームの前回の推定温度α_ｉに基づいて、各フレームの今回の推定温度α’_ｉを推定する。

次に、第１フレーム１１６の温度算出方法を例に挙げて詳しく説明する。図６は、ロボットアーム本体１１５の駆動中におけるモータ温度、フレーム温度及び周囲温度の温度変化を説明するための図である。図６において、α_１は現在の時刻ｔ_ｎに対して所定時間間隔ｄｔ前の時刻ｔ_ｎ−１に算出した第１フレーム１１６の前回の推定温度である。α’_１は時刻ｔ_ｎにおいて算出した第１フレーム１１６の今回の推定温度である。α_Ｍ１は第１モータ１２５の温度、α_Ｒは周囲温度である。

第１フレーム１１６の推定温度α’_１は、前回算出した第１フレーム１１６の推定温度α_１と、現在の第１モータ温度α_Ｍ１及び周囲温度α_Ｒを用いて、式（５）により算出される。計算間隔（所定時間間隔）ｄｔは、例えば１秒である。

この計算を所定時間間隔ｄｔで繰り返すことにより、モータ温度の履歴、フレーム温度の履歴及び周囲温度の履歴を、式（５）〜式（８）に示すように、積分することになる。したがって、ロボットアーム本体１１５の駆動中に動作が変わり、モータ１２５〜１２８の発熱量が変わった場合や周囲温度が変動した場合でも、精度良くフレーム１１６〜１１９の温度を推定することができる。

なお、ロボットアーム本体１１５の駆動再開時は、このステップＳ６において各フレーム１１６〜１１９の今回の推定温度を求める際に用いる前回の推定温度を、ステップＳ１〜Ｓ４の処理によって求めている。

次に、フレーム温度演算部１０５は、推定した各フレーム１１６〜１１９の推定温度と、推定温度を推定した時刻と、推定温度を算出するのに用いた周囲温度とをデータ記憶部１０９へ送信し、これらデータをデータ記憶部１０９に記憶させる（Ｓ７）。

また、フレーム温度演算部１０５は、推定した各フレーム１１６〜１１９の今回の推定温度のデータを、フレーム膨張演算部１０６へ送る。

フレーム膨張演算部１０６は、膨張計算式記憶部１１１から以下の式（１４）〜式（１７）の計算式を読み出し、各フレーム１１６〜１１９の今回の推定温度から、式（１４）〜式（１７）の計算式を用いて各フレーム１１６〜１１９の膨張量を算出する（Ｓ８）。
ΔＬ_１＝α’_１×δ_１×Ｌ_１ ・・・（１４）
ΔＬ_２＝α’_２×δ_２×Ｌ_２ ・・・（１５）
ΔＬ_３＝α’_３×δ_３×Ｌ_３ ・・・（１６）
ΔＬ_４＝α’_４×δ_４×Ｌ_４ ・・・（１７）

式（１４）は第１フレームの膨張量を算出する計算式であり、式（１５）は第２フレーム、式（１６）は第３フレーム、式（１７）は第４フレームの計算式を示す。つまり、これら計算式には、フレーム１１６〜１１９の膨張率の情報と長さの情報が含まれている。

ここで、δ_ｉ（ｉ＝１，２，３，４）は、各フレーム材料の膨張係数であり、例えば一般的なアルミニウムの場合、２４×１０^−６／℃である。ここで用いる膨張係数δ_ｉは、フレーム１１６〜１１９の材質が複数で構成される場合はその平均値を用いても良い。

また、Ｌ_ｉ（ｉ＝１，２，３，４）は、各フレーム１１６〜１１９の長さであり、例えば軸間距離を示す。フレーム１１６〜１１９の長さＬ_ｉは、熱による膨張量ΔＬ_ｉを算出するための長さであるため、熱膨張に寄与しない部分があればその長さを除いても良い。

フレーム膨張演算部１０６は、演算した各フレーム１１６〜１１９の膨張量を、順運動学演算部１０７に送る。

順運動学演算部１０７は、順運動学の計算により、各フレーム１１６〜１１９の熱による膨張量のデータを用いて、ロボットアーム本体１１５の先端１１５ａの推定位置を算出する。そして、順運動学演算部１０７は、軌道記憶部１１２からロボットアーム本体１１５の先端１１５ａの目標位置を読み込み、読み込んだ目標位置に対する推定位置のずれ量を算出する（Ｓ９：ずれ量算出工程）。順運動学演算部１０７は、算出したずれ量のデータを逆運動学演算部１０８に送る。

次に、逆運動学演算部１０８は、ロボットアーム本体１１５の先端１１５ａの位置のずれ量から逆運動学計算により各関節１２１〜１２４の回転角度の補正量を算出し（Ｓ１０）、補正量を加味した制御量となる駆動指令を算出する（Ｓ１１）。つまり、逆運動学演算部１０８は、これらステップＳ１０，Ｓ１１により、先端１１５ａのずれ量に基づいて駆動指令を算出する（駆動指令算出工程）。逆運動学演算部１０８（つまり、ＣＰＵ１０３）は、モータ制御部１０２に算出した駆動指令を出力する。モータ制御部１０２は、駆動指令の入力を受けて各モータ１２５〜１２８に電流を供給し、各モータ１２５〜１２８は、駆動指令に応じた制御量で各関節１２１〜１２４を駆動する。

ＣＰＵ１０３は、補正制御がＯＮかどうかをチェックし（Ｓ１２）、ＯＮであれば（Ｓ１２：Ｙｅｓ）、ステップＳ５の演算処理に戻り、制御を繰り返す。補正制御がＯＦＦであれば（Ｓ１２：Ｎｏ）、補正制御を停止する。ここで、補正制御ＯＦＦの状態というのは、ロボットアーム本体１１５の駆動停止の状態であり、緊急停止をした場合や、ロボット装置１００の電源をＯＦＦにした場合などである。

そして、ＣＰＵ１０３は、ロボットアーム本体１１５の駆動を再開した場合、ステップＳ１から処理を実行し、上述したように、駆動再開時の各フレーム１１６〜１１９の推定温度を、データ記憶部１０９に記録されている駆動停止前のデータを用いて推定する。

以上、本実施形態によれば、ロボットアーム本体１１５の駆動停止後、フレーム１１６〜１１９の温度が雰囲気温度に下がり切る前にロボットアーム本体１１５の駆動を再開しても、正確にフレーム１１６〜１１９の温度を推定することができる。その結果、駆動再開時においてロボットアーム本体１１５の先端１１５ａを目標位置に正確に位置決めすることができる。

また、ロボットアーム本体１１５の周囲温度をもフレーム１１６〜１１９の温度推定計算に用いるため、周囲温度の変化によるフレーム１１６〜１１９の熱膨張の変化があっても、先端１１５ａの位置ずれの補正を精度良く行うことができる。

なお、本発明は、以上説明した実施形態に限定されるものではなく、多くの変形が本発明の技術的思想内で当分野において通常の知識を有する者により可能である。

上記実施形態では、ロボットアーム１１４が、垂直多関節ロボットである場合について説明したが、水平多関節ロボット、パラレルリンクロボットなどであってもよい。

また、上記実施形態では、周囲温度（雰囲気温度）が変化する場合について説明したが、これに限定するものではない。ロボットアーム１１４の置かれている環境が一定の温度環境であれば、各フレーム１１６〜１１９の温度推定の演算の際に、周囲温度の変化分を加味しなくてもよい。即ち、式（１）〜式（４）の演算式において、＋Δα_Ｒを省略すればよい。

また、上記実施形態では、アクチュエータが電動モータである場合について説明したが、アクチュエータはこれに限定するものではない。例えば関節が直動関節の場合には、アクチュエータとして、電動モータの代わりに電動リニアアクチュエータを用いることも可能であり、この場合でも本発明は適用可能である。特に、アクチュエータとして、電動アクチュエータを用いた場合には、通電による発熱量が多いため、本発明により効果的にロボットアーム本体の先端を目標位置に位置決めすることができる。

また、上記実施形態では、ロボットアーム本体の関節数が４つの場合について説明したが、これに限定するものではなく、ロボットアーム本体の関節数が１つ以上あればよい。

また、以上述べた実施形態の各処理動作は具体的にはロボット制御装置１０１の演算部としてのＣＰＵ１０３により実行されるものである。従って上述した機能を実現するプログラムを記録した記録媒体をロボット制御装置１０１に供給し、ロボット制御装置１０１のコンピュータ（ＣＰＵやＭＰＵ）が記録媒体に格納されたプログラムを読み出し実行することによって達成されるようにしてもよい。この場合、記録媒体から読み出されたプログラム自体が上述した実施形態の機能を実現することになり、プログラム自体及びそのプログラムを記録した記録媒体は本発明を構成することになる。

また、上記実施形態では、コンピュータ読み取り可能な記録媒体がＨＤＤ１０４であり、ＨＤＤ１０４にプログラム１５１が格納される場合について説明したが、これに限定するものではない。プログラム１５１は、コンピュータ読み取り可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。例えば、プログラムを供給するための記録媒体としては、図２に示すＲＯＭ１４１、外部記憶装置１５０、記録ディスク１５２等を用いてもよい。具体例を挙げて説明すると、記録媒体として、フレキシブルディスク、ハードディスク、光ディスク、光磁気ディスク、ＣＤ−ＲＯＭ、ＣＤ−Ｒ、磁気テープ、不揮発性のメモリカード、ＲＯＭ等を用いることができる。

また、上記実施形態におけるプログラムを、ネットワークを介してダウンロードしてコンピュータにより実行するようにしてもよい。

また、コンピュータが読み出したプログラムコードを実行することにより、上記実施形態の機能が実現されるだけに限定するものではない。そのプログラムコードの指示に基づき、コンピュータ上で稼働しているＯＳ（オペレーティングシステム）等が実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれる。

さらに、記録媒体から読み出されたプログラムコードが、コンピュータに挿入された機能拡張ボードやコンピュータに接続された機能拡張ユニットに備わるメモリに書き込まれてもよい。そのプログラムコードの指示に基づき、その機能拡張ボードや機能拡張ユニットに備わるＣＰＵ等が実際の処理の一部または全部を行い、その処理によって上記実施形態の機能が実現される場合も含まれる。

１００…ロボット装置、１０１…ロボット制御装置、１０２…モータ制御部（駆動制御部）、１０３…ＣＰＵ（演算部）、１０４…ＨＤＤ（記憶部）、１１４…ロボットアーム、１１５…ロボットアーム本体、１１５ａ…先端、１１６〜１１９…フレーム、１２１〜１２４…関節、１２５〜１２８…電動モータ（アクチュエータ）、１３０…温度センサ、１３１〜１３４…温度センサ

Claims (7)

1. 関節を有するロボットアーム本体と、前記ロボットアーム本体のフレームの内部に設けられ、前記関節を駆動するアクチュエータと、前記アクチュエータの温度を検出する温度センサと、を有するロボットアームを制御するロボット制御装置において、
   入力を受けた駆動指令に基づいて前記アクチュエータの駆動を制御する駆動制御部と、
   前記ロボットアーム本体の駆動中に、前記フレームの熱特性、前記温度センサによる温度検出結果及び前記フレームの前回の推定温度に基づいて前記フレームの今回の推定温度を所定時間間隔で推定し、前記フレームの今回の推定温度から前記ロボットアーム本体の先端の推定位置を推定し、前記ロボットアーム本体の先端の目標位置に対する推定位置のずれ量に基づいて、前記駆動指令を算出する演算部と、を備え、
   前記演算部は、前記ロボットアーム本体の駆動を再開したときには、前記フレームの熱特性、前記ロボットアーム本体の駆動を停止していた駆動停止時間、及び前記ロボットアーム本体の駆動を停止する直前に求めた前記フレームの推定温度に基づき、前記フレームの前回の推定温度を推定することを特徴とするロボット制御装置。
2. 前記演算部は、前記ロボットアーム本体の駆動を再開したときに求めた前記フレームの前回の推定温度を、前記駆動停止時間の経過により変化した前記ロボットアーム本体の周囲温度の変化分で補正することを特徴とする請求項１に記載のロボット制御装置。
3. 前記演算部が推定した前記フレームの推定温度を、推定した時刻と対応付けて記憶する記憶部を備え、
   前記演算部は、前記駆動停止時間として、前記記憶部に記憶されている前記ロボットアーム本体の駆動を停止する直前の前記フレームの推定温度に対応付けられた時刻と、前記ロボットアーム本体の駆動を再開した時刻との差分を演算することを特徴とする請求項１又は２に記載のロボット制御装置。
4. 関節を有するロボットアーム本体、前記ロボットアーム本体のフレームの内部に設けられ、前記関節を駆動するアクチュエータ、及び前記アクチュエータの温度を検出する温度センサを有するロボットアームと、
   請求項１乃至３のいずれか１項に記載のロボット制御装置と、を備えたロボット装置。
5. ロボットアームが、関節を有するロボットアーム本体、前記ロボットアーム本体のフレームの内部に設けられ、前記関節を駆動するアクチュエータ、及び前記アクチュエータの温度を検出する温度センサを有しており、入力を受けた駆動指令に基づいて、前記アクチュエータの駆動を制御する駆動制御部と、前記駆動制御部に前記駆動指令を出力する演算部と、を有するロボット制御装置を用いて、前記ロボットアームを制御するロボット制御方法において、
   前記演算部が、前記ロボットアーム本体の駆動中に、前記フレームの熱特性、前記温度センサによる温度検出結果及び前記フレームの前回の推定温度に基づいて前記フレームの今回の推定温度を所定時間間隔で推定する温度推定工程と、
   前記演算部が、前記フレームの今回の推定温度から前記ロボットアーム本体の先端の位置を推定し、前記ロボットアーム本体の先端の目標位置に対する推定位置のずれ量を算出するずれ量算出工程と、
   前記演算部が、前記ずれ量に基づいて前記駆動指令を算出する駆動指令算出工程と、
   前記演算部が、前記ロボットアーム本体の駆動を停止した後に前記ロボットアーム本体の駆動を再開した場合には、前記フレームの熱特性、前記ロボットアーム本体の駆動を停止していた駆動停止時間、及び前記ロボットアーム本体の駆動を停止する直前に求めた前記フレームの推定温度に基づき、前記フレームの前回の推定温度を推定する駆動再開時温度推定工程と、を備えたことを特徴とするロボット制御方法。
6. コンピュータに請求項５に記載のロボット制御方法の各工程を実行させるためのプログラム。
7. 請求項６に記載のプログラムを記録したコンピュータ読み取り可能な記録媒体。

Images