(57)【要約】【課題】ローコストで小型のロボット装置とすることができ、ロボット本体の関節を高精度に目標関節角度に制御する。【解決手段】ロボット装置が、減速機１５の出力側に設けられ、関節の駆動に伴って第１出力側パルス信号を生成する矩形波発生部４１を備えている。制御部は、演算部２４２，２４８，２４９と、モータ制御部２５４と、を有する。モータ制御部２５４は、入力側パルス信号を用いて推定関節角度を推定し、この推定関節角度が目標関節角度になるようにサーボモータ１を動作させる。各演算部２４２，２４８，２４９は、第１出力側パルス信号とその理想状態のパルス波形とのずれ量に相当する関節のねじれ角度を演算する。モータ制御部２５４は、推定関節角度がねじれ角度の分補正されるように、サーボモータ１を動作させる。【選択図】図５
2014065097.tif 000002
【特許請求の範囲】
【請求項１】
回転モータと、
前記回転モータの出力を減速する減速機と、
前記回転モータにより前記減速機を介して駆動される関節を有するロボット本体と、
前記関節の角度が目標関節角度となるように、前記回転モータによる前記関節の駆動を制御する制御部と、
前記減速機の入力側に設けられ、前記回転モータの回転軸の回転に伴って入力側パルス信号を生成する入力側エンコーダ部と、
前記減速機の出力側に設けられ、前記関節の駆動に伴って前記関節の角度に応じた第１出力側パルス信号を生成する矩形波発生部と、を備え、
前記制御部は、
前記入力側パルス信号の示す前記回転モータの回転軸の回転角度及び前記減速機の減速比から、前記関節にねじれがない理想状態における前記関節の角度を推定し、推定関節角度が前記目標関節角度になるように前記回転モータを動作させる第１制御処理と、
前記第１出力側パルス信号と、前記理想状態において前記矩形波発生部から得られると想定されるパルス波形とのずれ量に相当する前記関節のねじれ角度を演算するねじれ角度演算処理と、
前記推定関節角度が前記ねじれ角度の分補正されるように、前記回転モータを動作させる補正制御処理と、を実行することを特徴とするロボット装置。
【請求項２】
前記減速機の出力側に設けられ、前記関節の駆動に伴って第２出力側パルス信号を生成する、前記矩形波発生部よりも高い分解能のインクリメンタル型の出力側エンコーダ部を備え、
前記制御部は、前記ねじれ角度演算処理として、
前記第１制御処理による前記回転モータの動作中に、前記第１出力側パルス信号のエッジをトリガとして前記第２出力側パルス信号をカウントし第１角度を求める第１角度演算処理と、
前記推定関節角度を前記第１出力側パルス信号のピッチ角度で割り算した際の余りである第２角度を求める第２角度演算処理と、
前記ねじれ角度として、前記第１角度と前記第２角度との差分を演算する差分演算処理と、を実行することを特徴とする請求項１に記載のロボット装置。
【請求項３】
前記制御部は、
前記補正制御処理で前記関節の角度が補正されたときの前記第２出力側パルス信号のカウント値を前記目標関節角度に対応付ける位置出し処理と、
次の目標関節角度に前記関節の角度を制御する際に、前記第２出力側パルス信号のカウント値に基づいて前記回転モータを動作させる第２制御処理と、を実行することを特徴とする請求項２に記載のロボット装置。
【請求項４】
前記出力側エンコーダ部の分解能は、前記矩形波発生部の分解能の整数倍であることを特徴とする請求項２又は３に記載のロボット装置。
【請求項５】
ユーザに報知可能な報知部を備え、
前記制御部は、
前記第１制御処理による前記回転モータの動作開始から動作終了までの間に前記第１出力側パルス信号のエッジを検知したか否かを判断する判断処理と、
前記判断処理にて前記第１出力側パルス信号のエッジを検知していないと判断した場合、前記報知部を動作させる報知処理と、を実行することを特徴とする請求項１乃至４の何れか１項に記載のロボット装置。
【請求項６】
前記制御部は、
前記第１制御処理による前記回転モータの動作開始から動作終了までの間に前記第１出力側パルス信号のエッジを検知したか否かを判断する判断処理と、
前記判断処理にて前記第１出力側パルス信号のエッジを検知していないと判断した場合、前記ロボット本体の姿勢から、前記関節の推定ねじれ角度を推定する推定処理と、を実行し、
前記補正制御処理では、前記判断処理にて前記第１出力側パルス信号のエッジを検知していないと判断した場合は、前記ねじれ角度の代わりに前記推定ねじれ角度を用いることを特徴とする請求項１乃至４のいずれか１項に記載のロボット装置。
【請求項７】
前記矩形波発生部は、
前記関節における最大ねじれ角度の２倍よりも大きいピッチ角度でパターンが形成されたスケールと、
前記スケールに対して前記減速機の出力軸の回転に伴って相対的に回転し、前記パターンを読み取って前記第１出力側パルス信号を出力する検出ヘッドと、を有することを特徴とする請求項１乃至６のいずれか１項に記載のロボット装置。
【請求項８】
前記最大ねじれ角度は、前記関節にかかる最大のモーメントを、前記減速機のねじれ剛性で割り算した値よりも大きい値であることを特徴とする請求項７に記載のロボット装置。
【請求項９】
前記最大ねじれ角度は、前記減速機の許容トルクを、前記減速機のねじれ剛性で割り算した値よりも大きい値であることを特徴とする請求項７に記載のロボット装置。
【請求項１０】
前記入力側エンコーダ部は、アブソリュート型のロータリエンコーダであることを特徴とする請求項１乃至９のいずれか１項に記載のロボット装置。
【請求項１１】
回転モータと、前記回転モータの出力を減速する減速機と、前記回転モータにより前記減速機を介して駆動される関節を有するロボット本体と、前記減速機の入力側に設けられ、前記回転モータの回転軸の回転に伴って入力側パルス信号を生成する入力側エンコーダ部と、前記減速機の出力側に設けられ、前記関節の駆動に伴って前記関節の角度に応じた第１出力側パルス信号を生成する矩形波発生部と、を有するロボット装置の制御部により、前記関節の角度が目標関節角度となるように、前記回転モータによる前記関節の駆動を制御するロボット制御方法において、
前記制御部が、前記入力側パルス信号の示す前記回転モータの回転軸の回転角度及び前記減速機の減速比から、前記関節にねじれがない理想状態における前記関節の角度を推定し、推定関節角度が前記目標関節角度になるように前記回転モータを動作させる第１制御ステップと、
前記制御部が、前記第１出力側パルス信号と、前記理想状態において前記矩形波発生部から得られると想定されるパルス波形とのずれ量に相当する前記関節のねじれ角度を演算するねじれ角度演算ステップと、
前記制御部が、前記推定関節角度が前記ねじれ角度の分補正されるように、前記回転モータを動作させる補正制御ステップと、を備えたことを特徴とするロボット制御方法。
【請求項１２】
前記ロボット装置が、前記減速機の出力側に設けられ、前記関節の駆動に伴って第２出力側パルス信号を生成する、前記矩形波発生部よりも高い分解能のインクリメンタル型の出力側エンコーダ部を有しており、
前記ねじれ角度演算ステップは、
前記制御部が、前記第１制御ステップによる前記回転モータの動作中に、前記第１出力側パルス信号のエッジをトリガとして前記第２出力側パルス信号をカウントし第１角度を求める第１角度演算ステップと、
前記推定関節角度を前記第１出力側パルス信号のピッチ角度で割り算した際の余りである第２角度を求める第２角度演算ステップと、
前記ねじれ角度として、前記第１角度と前記第２角度との差分を演算する差分演算ステップと、を有することを特徴とする請求項１１に記載のロボット制御方法。
【請求項１３】
前記制御部が、前記補正制御ステップで前記関節の角度が補正されたときの前記第２出力側パルス信号のカウント値を前記目標関節角度に対応付ける位置出しステップと、
前記制御部が、次の目標関節角度に前記関節の角度を制御する際に、前記第２出力側パルス信号のカウント値に基づいて前記回転モータを動作させる第２制御ステップと、を備えたことを特徴とする請求項１２に記載のロボット制御方法。
【請求項１４】
コンピュータに請求項１１乃至１３のいずれか１項に記載のロボット制御方法の各ステップを実行させるためのプログラム。
【請求項１５】
請求項１４に記載のプログラムを記録したコンピュータ読み取り可能な記録媒体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、回転モータにより減速機を介してロボット本体の関節を駆動する際に、関節のねじれを求めて、関節の角度を制御するものに関する。
【背景技術】
【０００２】
一般に、ロボットアーム等のロボット本体の関節の駆動には、ＡＣサーボモータやＤＣブラシレスサーボモータ等のサーボモータが用いられており、高出力のトルクを得るためにサーボモータの出力側に減速機が設けられ、リンク等の構造部材へ結合されている。
【０００３】
従来、ロボットアームの各関節の角度制御は、サーボモータ軸に直結されたロータリーエンコーダ（エンコーダ）でサーボモータ軸の回転角度を検出して行っていた。しかしながら、サーボモータに結合した減速機は、構造上、ねじれやガタが生じるため、関節にねじれが生じ、サーボモータ軸の回転角度では、正確に関節の角度を求めることができなかった。そのため、ロボットアーム先端等、ロボット本体の位置誤差となっていた。
【０００４】
近年、ロボット本体の位置誤差を小さくするために、減速機の出力軸にアブソリュート型のエンコーダを設けて位置を検出するものが提案されている（特許文献１参照）。アブソリュート型のエンコーダを用いることで、関節の角度を直接検出することができるため、ロボット本体の位置誤差も小さくすることができる。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２０１０−２３８１７４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、上記特許文献１のように、減速機の出力軸にアブソリュート型のエンコーダを設ける場合、アブソリュート型のエンコーダには、非常に高い分解能が要求される。
【０００７】
具体的に説明すると、モータの回転軸にアブソリュート型のエンコーダを実装する方式では、減速機で減速比分の分解能が緩和されるため、低分解能のエンコーダを用いることができる。一方、減速機の出力軸側に設けられるアブソリュート型のエンコーダには、モータに実装するエンコーダの分解能に対して減速比を乗じた分の高い分解能が要求される。例えば、減速機の減速比が５０〜１６０程度で設計されている場合、減速機の出力軸側に設けられるアブソリュート型のエンコーダには、モータ側に実装されるエンコーダに対して、分解能が５０〜１６０倍程度必要となる。
【０００８】
アブソリュート型のエンコーダでは、高分解能になるほど、発光素子や受光素子等の検出素子の数が増加するため、構造が複雑となり、また高価で大型となる。このため、ロボット本体の各関節に、関節角度検出用にアブソリュート型のエンコーダを実装すると、ロボット装置がコスト高で大型になるという問題があった。
【０００９】
そこで、本発明は、ローコストで小型のロボット装置とすることができ、ロボット本体の関節を高精度に目標関節角度に制御できるものを提供することを目的とする。
【課題を解決するための手段】
【００１０】
本発明のロボット装置は、回転モータと、前記回転モータの出力を減速する減速機と、前記回転モータにより前記減速機を介して駆動される関節を有するロボット本体と、前記関節の角度が目標関節角度となるように、前記回転モータによる前記関節の駆動を制御する制御部と、前記減速機の入力側に設けられ、前記回転モータの回転軸の回転に伴って入力側パルス信号を生成する入力側エンコーダ部と、前記減速機の出力側に設けられ、前記関節の駆動に伴って前記関節の角度に応じた第１出力側パルス信号を生成する矩形波発生部と、を備え、前記制御部は、前記入力側パルス信号の示す前記回転モータの回転軸の回転角度及び前記減速機の減速比から、前記関節にねじれがない理想状態における前記関節の角度を推定し、推定関節角度が前記目標関節角度になるように前記回転モータを動作させる第１制御処理と、前記第１出力側パルス信号と、前記理想状態において前記矩形波発生部から得られると想定されるパルス波形とのずれ量に相当する前記関節のねじれ角度を演算するねじれ角度演算処理と、前記推定関節角度が前記ねじれ角度の分補正されるように、前記回転モータを動作させる補正制御処理と、を実行することを特徴とする。
【発明の効果】
【００１１】
本発明によれば、矩形波発生部の検出結果に基づいて関節のねじれ角度を求めて回転モータを制御しているので、関節の角度を目標関節角度に高精度に制御することができる。しかも、アブソリュート型のエンコーダを実装する構成に比べ、小型でローコストのロボット装置を実現することができる。
【図面の簡単な説明】
【００１２】
【図１】第１実施形態に係るロボット装置の概略構成を示す斜視図である。
【図２】図１に示す関節近傍のロボットアームの断面図である。
【図３】関節エンコーダスケールの平面図である。
【図４】ロボット装置の制御装置の構成を示すブロック図である。
【図５】第１実施形態に係るロボット装置の制御系を示す機能ブロック図である。
【図６】入力側エンコーダ部の原点出しを行うフローチャートである。
【図７】第１実施形態におけるＣＰＵの関節の角度の制御動作を示すフローチャートである。
【図８】関節にねじれがない理想状態のパルス信号を示す波形図である。
【図９】関節にねじれがある状態のパルス信号を示す波形図である。
【図１０】ＣＰＵによるねじれ角度演算処理の処理動作を示すフローチャートである。
【図１１】関節にねじれがある状態のパルス信号を示す波形図である。
【図１２】ロボットアームを水平方向に伸ばしたときの姿勢を示す斜視図である。
【図１３】関節エンコーダの設計フローを示すフローチャートである。
【図１４】第２実施形態に係るロボット装置の制御系を示す機能ブロック図である。
【図１５】第２実施形態におけるＣＰＵの関節の角度の制御動作を示すフローチャートである。
【図１６】第３実施形態に係るロボット装置の制御系を示す機能ブロック図である。
【図１７】第３実施形態におけるＣＰＵの関節の角度の制御動作を示すフローチャートである。
【図１８】第４実施形態に係るロボット装置の制御系を示す機能ブロック図である。
【図１９】第４実施形態におけるＣＰＵの関節の角度の制御動作を示すフローチャートである。
【発明を実施するための形態】
【００１３】
以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。
【００１４】
［第１実施形態］
図１は、本発明の第１実施形態に係るロボット装置の概略構成を示す斜視図である。図１に示すロボット装置５００は、例えば組立作業等を行う産業ロボットであり、垂直６軸多関節型のロボットアーム１００と、ロボットアーム１００を制御する制御装置２００と、制御装置２００に接続されたティーチングペンダント３００と、を備えている。
【００１５】
ロボットアーム１００は、作業台に固定されるベース部１０１と、変位や力を伝達する複数のリンク１２１〜１２６と、各リンク１２１〜１２６を旋回又は回転可能に連結する複数の関節１１１〜１１６とを有するロボット本体１００Ａを備えている。
【００１６】
また、ロボットアーム１００の先端、即ちリンク１２６には、ハンド１０２が連結されている。ハンド１０２は、任意のワークＷに対して把持等の作業を行うエンドエフェクタである。
【００１７】
各関節１１１〜１１６には、回転モータである電磁モータとしてのサーボモータと、サーボモータの回転軸の回転角度を検出するエンコーダと、サーボモータのトルクを増大させるためにサーボモータの出力を減速する減速機と、が設けられている。減速機には、小型軽量で減速比の大きい波動歯車減速機を用いるのが望ましい。
【００１８】
以下、各関節１１１〜１１６を代表して、関節１１２を例に説明する。図２は、図１に示す関節１１２近傍のロボットアーム１００の断面図である。
【００１９】
図２に示すように、関節１１２には、サーボモータ１と、サーボモータ１の出力（回転角度）を減速する減速機１５とが設けられている。サーボモータ１は、例えばブラシレスＤＣサーボモータやＡＣサーボモータである。
【００２０】
サーボモータ１は、回転軸２とロータマグネット３とで構成された回転部を有している。モータハウジング４，５には、回転軸２を回転自在に支持する軸受６，７が設けられており、モータハウジング４には、ステータコイル８が取付けられている。
【００２１】
回転軸２の一端部には、回転軸２の回転角度を検出するロータリエンコーダである入力側エンコーダ部９が設けられている。つまり、この入力側エンコーダ部９は、減速機１５の入力側に設けられている。そして、サーボモータ１の回転軸２の回転に伴って、入力側パルス信号を生成し、図１の制御装置２００に出力する。入力側エンコーダ部９は、インクリメンタル型のロータリエンコーダでもよいが、サーボモータ１の所定の回転位置を原点とした絶対角度が検出できるアブソリュート型のロータリエンコーダであるのが望ましい。
【００２２】
入力側エンコーダ部９は、エンコーダスケール１０と、エンコーダスケール１０に対向して配置されたエンコーダヘッド１１と、エンコーダスケール１０を回転軸２に固定するフランジ１２と、防塵用のエンコーダカバー１３とで構成されている。サーボモータ１はモータカバー１４で囲われている。
【００２３】
必要に応じて、サーボモータ１と入力側エンコーダ部９との間に、不図示のブレーキユニットを設けてもよい。ブレーキユニットの主な目的は電源ＯＦＦ時のロボット本体１００Ａの姿勢の保持にある。
【００２４】
サーボモータ１の回転軸２の他端部には、減速比Ｎの波動歯車減速機である減速機１５が取付けられている。
【００２５】
この減速機１５は、入力部であるウェブジェネレータ１６と、サーキュラスプライン１７と、ウェブジェネレータ１６とサーキュラスプライン１７との間に配置されたフレックススプライン１８と、を備えている。
【００２６】
フレックススプライン１８は、リンク１２１に連結されており、減速機１５の減速比Ｎで減速される。減速機１５の出力部であるサーキュラスプライン１７は、リンク１２２に結合されている。ウェブジェネレータ１６は、サーボモータ１の回転軸２に結合されている。
【００２７】
サーボモータ１の回転軸２とウェブジェネレータ１６との結合部が、減速機１５の入力軸、フレックススプライン１８とリンク１２１との結合部が減速機１５の出力軸となる。減速機１５の出力軸の回転角度が、出力角度、即ち関節１１２の角度である。
【００２８】
リンク１２１とリンク１２２との間には、クロスローラベアリング２２が設けられており、リンク１２１とリンク１２２とは、クロスローラベアリング２２を介して回転自在に結合されている。このように、サーボモータ１の回転軸２は、減速機１５を介して１／Ｎに減速され、リンク１２１とリンク１２２とが相対的に回転する。
【００２９】
以上の構成により、ロボット本体１００Ａの関節１１２は、サーボモータ１により減速機１５を介して駆動される。一般的な産業用ロボットでは、サーボモータと減速機の間にタイミングベルトを設けて動力を伝達する場合もある。タイミングベルトに用いるプーリ直径が異なると、モータ出力を減速或いは増速する。当然ながら、タイミングベルト等の動力伝達要素もねじれやガタが発生する。また、タイミングベルトの替りに歯車等の伝達要素を設けても同様である。本実施形態では、サーボモータと減速機の間の伝達要素も減速機の一部として扱う。
【００３０】
関節エンコーダ３１は、リンク１２１とリンク１２２との相対角度を検出するものであり、矩形波発生部４１及びインクリメンタル型の出力側エンコーダ部４２からなる。関節エンコーダ３１は、減速機１５の出力側に設けられている。矩形波発生部４１は、関節１１２の駆動に伴って関節１１２の角度に応じた第１出力側パルス信号を生成し、制御装置２００（図１）に出力する。また、出力側エンコーダ部４２は、関節１１２の駆動に伴って第２出力側パルス信号を生成し、制御装置２００（図１）に出力する。
【００３１】
関節エンコーダ３１は、関節エンコーダスケール３２と、第１検出ヘッド３３と、第２検出ヘッド３４から構成されている。本第１実施形態では、関節エンコーダ３１は、反射型のロータリエンコーダである場合について説明するが、透過型のロータリエンコーダであってもよい。
【００３２】
関節エンコーダスケール３２は、リンク１２１の表面に固定して配置され、検出ヘッド３３，３４は、関節エンコーダスケール３２に対向するようにリンク１２２の表面に固定して配置されている。
【００３３】
図３は、関節エンコーダスケール３２の平面図である。関節エンコーダスケール３２は、円板形状又はリング形状に形成され、内周側に形成された第１スケール３５と外周側に形成された第２スケール３６とで構成されている。第１スケール３５には、第１ピッチ角度θａで第１パターン３５ａが形成されている。第２スケール３６には、第２ピッチ角度θｂで第２パターン３６ａが形成されている。これらパターン３５ａ，３６ａは、関節の回転中心に対して放射状に形成されている。
【００３４】
第１検出ヘッド３３は、関節エンコーダスケール３２の第１スケール３５に対して減速機１５の出力軸の回転に伴って相対的に回転し、第１パターン３５ａを読み取って第１出力側パルス信号を制御装置２００（図１）に出力する。第２検出ヘッド３４は、関節エンコーダスケール３２の第２スケール３６に対して減速機１５の出力軸の回転に伴って相対的に回転し、第２パターン３６ａを読み取って第２出力側パルス信号を制御装置２００（図１）に出力する。第１検出ヘッド３３及び第１スケール３５により矩形波発生部４１が構成されている。また、第２検出ヘッド３４及び第２スケール３６により出力側エンコーダ部４２が構成されている。
【００３５】
矩形波発生部４１は、関節１１２の角度を第１ピッチ角度θａで分割検出することができる。同様に、出力側エンコーダ部４２は、関節１１２の角度を第２ピッチ角度θｂで分割検出することができる。
【００３６】
本第１実施形態では、第２ピッチ角度θｂは第１ピッチ角度θａよりも小さい。即ち、第１パターン３５ａの間隔は、第２パターン３６ａの間隔よりも広い。したがって、出力側エンコーダ部４２は、矩形波発生部４１よりも高い分解能である。
【００３７】
また、第１ピッチ角度θａは第２ピッチ角度θｂの整数倍であり、出力側エンコーダ部４２は、矩形波発生部４１の整数倍の分解能で関節１１２の角度を検出することができる。
【００３８】
なお、検出ヘッド３３，３４は、回転方向の判別ができるように２相出力が望ましい。また、検出ヘッド３３，３４の分解能を向上させるために、電気的な分割手段を更に加えても良い。
【００３９】
更に図３を用いて、関節エンコーダ３１の原点を説明する。関節１１２の角度を制御するために、任意の位置に原点を設け、この原点を基準に角度制御を行う。
【００４０】
原点の位置を明示するために、原点の位置でリンク１２１とリンク１２２に貫通する穴を設ける場合もある。この穴に軸を通すことでリンク１２１とリンク１２２と間の原点を特定できる。
【００４１】
図３中の線分Ｄ−Ｄは、図２を用いて説明した第１検出ヘッド３３と第２検出ヘッド３４で検出する位置を表している。この線分Ｄ−Ｄの位置が関節１１２の原点位置になるように関節エンコーダスケール３２と第１検出ヘッド３３と第２検出ヘッド３４との検出位置を調整して配置する。
【００４２】
図４は、ロボット装置５００の制御装置２００の構成を示すブロック図である。制御装置２００は、制御部としてのＣＰＵ２０１、ＲＯＭ２０２、ＲＡＭ２０３、記憶部としてのＨＤＤ２０４、記録ディスクドライブ２０５及び各種のインタフェース２１１〜２１５を備えている。
【００４３】
ＣＰＵ２０１には、ＲＯＭ２０２、ＲＡＭ２０３、ＨＤＤ２０４、記録ディスクドライブ２０５及び各種のインタフェース２１１〜２１５が、バス２１６を介して接続されている。ＲＯＭ２０２には、ＢＩＯＳ等の基本プログラムが格納されている。ＲＡＭ２０３は、ＣＰＵ２０１の演算処理結果を一時的に記憶する記憶装置である。記録ディスクドライブ２０５にはティーチングペンダントから取得した教示データや、動作手順の動作プログラムを記憶する。
【００４４】
ＨＤＤ２０４は、ＣＰＵ２０１の演算処理結果である各種のデータ等を記憶する記憶部であると共に、ＣＰＵ２０１に、後述する各種演算処理を実行させるためのプログラム３３０を記録するものである。ＣＰＵ２０１は、ＨＤＤ２０４に記録（格納）されたプログラム３３０に基づいて各種演算処理を実行する。
【００４５】
記録ディスクドライブ２０５は、記録ディスク３３１に記録された各種データやプログラム等を読み出すことができる。
【００４６】
インタフェース２１１には、ティーチングペンダント３００が接続されている。ティーチングペンダント３００は、ユーザが操作するものであり、ユーザの操作により各関節１１１〜１１６の目標関節角度を順次出力する。ＣＰＵ２０１は、インタフェース２１１及びバス２１６を介してティーチングペンダント３００からの目標関節角度のデータ信号の入力を受ける。
【００４７】
インタフェース２１２には、エンコーダ部９，４２及び矩形波発生部４１が接続されている。エンコーダ部９，４２及び矩形波発生部４１からは、角度を示すパルス信号が出力される。ＣＰＵ２０１は、インタフェース２１２及びバス２１６を介してエンコーダ部９，４２及び矩形波発生部４１からのパルス信号の入力を受ける。
【００４８】
インタフェース２１３には、モニタ３１１が接続されており、モニタ３１１には、各種画像が表示される。インタフェース２１４には、書き換え可能な不揮発性メモリや外付けＨＤＤ等の外部記憶装置３１２が接続されている。
【００４９】
インタフェース２１５には、サーボ制御装置３１３が接続されている。ＣＰＵ２０１は、サーボモータ１の回転軸２の回転角度の制御量を示す駆動指令のデータを所定時間間隔でバス２１６及びインタフェース２１５を介してサーボ制御装置３１３に出力する。サーボ制御装置３１３は、制御部としてのＣＰＵ２０１から入力を受けた駆動指令に基づき、サーボモータ１への電流の出力量を演算し、サーボモータ１へ電流を供給して、ロボット本体１００Ａの関節１１１〜１１６の関節角度制御を行う。
【００５０】
即ち、ＣＰＵ２０１は、サーボ制御装置３１３を介して、関節１１２の角度が目標関節角度となるように、サーボモータ１による関節１１２の駆動を制御する。
【００５１】
図５は、本発明の第１実施形態に係るロボット装置の制御系を示す機能ブロック図である。ここで、図４に示す制御部としてのＣＰＵ２０１は、プログラム３３０を実行することにより、図５に示す各部２４２，２４８，２４９，２５４，２５５として機能する。以下、各部の動作について、大まかに説明する。
【００５２】
リセット部２５５は、入力側エンコーダ部９のエンコーダの値をクリアする。関節位相演算部２４２は、矩形波発生部４１と出力側エンコーダ部４２から、第１出力側パルス信号及び第２出力側パルス信号の入力を受ける。そして、関節位相演算部２４２は、第１出力側パルス信号を基準として、第２出力側パルス信号をカウントし、カウント値に相当する角度、即ち関節位相角度を演算する。
【００５３】
基準位相演算部２４８は、入力側エンコーダ部９のパルス信号から得られる回転角度、減速機１５の減速比Ｎ、及びピッチ角度θａから、理想状態の推定関節位相角度を演算する。つまり、基準位相演算部２４８は、入力側エンコーダ部９により検出された入力側パルス信号から、サーボモータ１の回転軸２の回転角度を求め、この回転角度を減速比Ｎで割り算することで、関節１１２の角度を推定する。この推定関節角度は、関節１１２にねじれがないとした場合の理想状態の関節角度である。この推定関節角度をピッチ角度θａで割ったときの余りが推定関節位相角度である。つまり、理想状態とは、減速機１５にねじれがないと仮定した理想の状態である。この理想状態では、入力軸の回転角度を減速比Ｎで割った値が関節角度と一致する。
【００５４】
関節誤差演算部２４９は、基準位相演算部２４８の演算結果と関節位相演算部２４２の演算結果から関節誤差、即ち関節１１２のねじれ角度を求める。推定関節角度と実際の関節角度との差分が、関節１１２のねじれ角度（関節誤差）である。関節誤差が生じる理由としては、減速機１５の弾性ねじれ変形やガタ、モータ軸のねじれ等、複数の要因がある。
【００５５】
モータ制御部２５４は、入力側エンコーダ部９のパルス信号が示す回転角度の値と関節誤差からモータの制御量を決定して、サーボモータ１の駆動を制御する。
【００５６】
ここで、ロボット装置５００による組立作業等の動作に先立って行う、入力側エンコーダ部９の原点出しの方法について説明する。
【００５７】
図６は、入力側エンコーダ部９の原点出しを行うフローチャートである。まず、ユーザは、リンク１２１とリンク１２２に貫通する穴に軸部材を通すことで、関節１１２を原点位置に固定する（Ｓ１）。
【００５８】
次に、ユーザは、ティーチングペンダント３００を操作することで、ＣＰＵ２０１に指令を送り、サーボモータ１の逆転トルクが最小となるモータ回転位置を探索する（Ｓ２）。なお、この逆転トルクはモータの駆動電流からも推定できる。
【００５９】
次に、ユーザは、ティーチングペンダント３００を操作することで、ＣＰＵ２０１に指令を送り、リセット部２５５（ＣＰＵ２０１）が入力側エンコーダ部９をリセットする（Ｓ３）。
【００６０】
ステップＳ１〜Ｓ３の作業は、矩形波発生部４１及び出力側エンコーダ部４２を備える関節エンコーダ３１が原点位置であり、且つ、関節誤差が無い場合にリセット部２５５でリセットして、入力側エンコーダ部９の原点出しを行う。
【００６１】
次に、本第１実施形態の制御部であるＣＰＵ２０１によるロボット制御方法、即ち関節１１２の角度の制御動作について説明する。図７は、第１実施形態におけるＣＰＵ２０１の関節１１２の角度の制御動作を示すフローチャートである。なお、図７に示す各処理ステップは、ＣＰＵ２０１がプログラム３３０を読み出してプログラム３３０を実行することにより実行されるものである。
【００６２】
まず、図５中、モータ制御部２５４として機能するＣＰＵ２０１は、サーボモータ１の制御中、入力側エンコーダ部９からの入力側パルス信号に基づき、サーボモータ１の回転軸２の回転角度を逐次求めている。そして、ＣＰＵ２０１は、サーボモータ１の回転軸２の回転角度及び減速機１５の減速比Ｎから、関節１１２にねじれがない理想状態における関節１１２の角度（推定関節角度）を逐次推定している。具体的には、ＣＰＵ１０２は、回転角度を減速比Ｎで割り算することで、推定関節角度を求める。
【００６３】
ＣＰＵ１０２は、この推定関節角度が動作プログラムやティーチングペンダント３００から取得した目標関節角度になるようにサーボモータを動作させる第１制御処理を実行する（Ｓ１１：第１制御ステップ）。
【００６４】
次に、関節位相演算部２４２、基準位相演算部２４８、及び関節誤差演算部２４９として機能するＣＰＵ１０２は、関節１１２のねじれ角度θを演算するねじれ角度演算処理を実行する（Ｓ１２：ねじれ角度演算ステップ）。このねじれ角度（関節誤差）θは、矩形波発生部４１から得られる第１出力側パルス信号と、理想状態において矩形波発生部４１から得られると想定されるパルス波形とのずれ量に相当する。
【００６５】
図８は、関節１１２にねじれがない理想状態のパルス信号を示す波形図、図９は、関節１１２にねじれがある状態のパルス信号を示す波形図である。
【００６６】
図８及び図９において、波形Ａは、矩形波発生部４１から出力される第１出力側パルス信号のパルス波形である。また、波形Ｂは、出力側エンコーダ部４２から出力される第２出力側パルス信号のパルス波形である。波形Ｃは、理想状態において矩形波発生部４１から得られると想定されるパルス波形である。即ち、波形Ｃは、入力側パルス信号に基づいて求めた推定関節角度をピッチ角度θａで分割した演算波形である。
【００６７】
図８に示すように、関節１１２のねじれ角度が０、即ち関節誤差のない理想的な状態では、波形Ａと波形Ｃとは一致する。関節１１２にねじれが生じた場合、図９に示すように、波形Ａと波形Ｂとは位相がねじれ角度θの分ずれる。したがって、ＣＰＵ１０２は、このねじれ角度θを求めればよい。
【００６８】
次に、モータ制御部２５４として機能するＣＰＵ２０１は、推定関節角度がねじれ角度θの分補正されるように、サーボモータ１を動作させる補正制御処理を実行する（Ｓ１３：補正制御ステップ）。
【００６９】
なお、ＣＰＵ１０２は、次の目標関節角度のデータの入力を受けている場合は、再度、ステップＳ１１〜Ｓ１３を繰り返し実行する。
【００７０】
次に、ステップＳ１２のねじれ角度演算処理について詳細に説明する。図１０は、ＣＰＵ１０２によるねじれ角度演算処理の処理動作を示すフローチャートである。
【００７１】
関節位相演算部２４２として機能するＣＰＵ１０２は、ステップＳ１１の第１制御処理によるサーボモータ１の動作中に、矩形波発生部４１から取得した第１出力側パルス信号のエッジをトリガとして第２出力側パルス信号のパルス数をカウントする。そして、ＣＰＵ１０２は、カウントした結果に基づき、推定関節角度と第１出力側パルス信号のエッジの間の第１角度（関節位相角度）Ｐｊを求める第１角度演算処理を実行する（Ｓ２１：第１角度演算ステップ）。
【００７２】
図１１は、関節１１２にねじれがある状態のパルス信号を示す波形図である。図１１中、Ｑ，Ｒ，Ｓは、入力側エンコーダ部９から出力される入力側パルス信号より求められる推定関節角度である。また、図１１中、ｔｇ１，ｔｇ２は、波形Ａの立ち上がりエッジ、ｔｇ３，ｔｇ４は波形Ｃの立ち上がりエッジである。入力側エンコーダ部９からの入力側パルス信号より求まった角度が推定関節角度Ｑから推定関節角度Ｒに変化する場合について説明する。
【００７３】
関節位相演算部２４２は、アップダウンカウンタとして機能する。アップダウンカウンタは、カウント値をクリアするリセット部と、入力パルス信号のパルス数を計測するカウント部と、カウントのアップダウン指令の符号部からなる。本第１実施形態では、第１検出ヘッド３３が９０°位相の２相センサで構成されており、方向判別信号を出力できる。関節位相演算部２４２のリセット部は、波形Ａのエッジｔｇ１を入力し、カウント部は、波形Ｂを入力し、符号部は、方向判別信号を入力する。
【００７４】
つまり、関節位相演算部２４２は、エッジｔｇ１を入力してから出力側エンコーダ部４２から得た第２出力側パルス信号のパルスのカウントを開始する。そして、関節位相演算部２４２は、入力側エンコーダ部９から得た入力側パルス信号から求まる関節１１２の推定角度が、推定関節角度Ｒとなったときにパルスのカウントを終了して、カウント値を得る。そして、関節位相演算部２４２は、カウント値を角度に換算することで、第１角度（関節位相角度）Ｐｊを求める。このように、第１出力側パルス信号のパルスエッジをトリガとして、高分解能の第２出力側パルス信号のパルスをカウントすることで、正確に第１角度Ｐｊを求めることができる。
【００７５】
また、基準位相演算部２４８として機能するＣＰＵ１０２は、推定関節角度Ｒを第１出力側パルス信号のピッチ角度θａで割り算した際の余りである第２角度Ｐｍを求める第２角度演算処理を実行する（Ｓ２２：第２角度演算ステップ）。つまり、基準位相演算部２４８は、波形Ｃを求めることなく、Ｒ÷ａの余りを求めることで、簡単に第２角度（推定関節位相角度）Ｐｍを計算することができる。
【００７６】
次に、関節誤差演算部２４９として機能するＣＰＵ１０２は、ねじれ角度θとして、第１角度Ｐｊと第２角度Ｐｍとの差分を演算する差分演算処理を実行する（Ｓ２３：差分演算ステップ）。つまり、関節誤差演算部２４９は、θ＝Ｐｍ−Ｐｊの計算を実行する。
【００７７】
以上のステップＳ２１〜Ｓ２３で、推定関節角度がＱからＲになったときの関節誤差の演算手段を述べたが、推定関節角度がＲからＳになった場合も同様の演算を行う。この場合、ステップＳ２１では、立ち上がりエッジｔｇ２をトリガとして第２出力側パルス信号のパルスのカウントを開始する。
【００７８】
ここで、本第１実施形態では、ピッチ角度θａは、関節１１２における最大ねじれ角度の２倍よりも大きい角度に設定されている。以下、関節１１２の最大ねじれ角度について説明する。
【００７９】
図１２は、ロボットアーム１００を水平方向に伸ばしたときの姿勢を示す斜視図である。支配的な関節誤差θの要因は関節１１２の弾性であり、この弾性を図１２を用いて説明する。図１２に示すロボット本体１００Ａの姿勢が、関節１１２に与えるモーメント最大条件となる。
【００８０】
関節１１２の最大ねじれ角度は、関節１１２にかかる最大のモーメントを、減速機１５のねじれ剛性で割り算した値よりも大きい値、即ちマージンを上乗せした値にすればよい。このときのモーメントをＭ、減速機１５のねじれ剛性をｋとすれば、ねじれ角度はＭ／ｋとなる。実際には、モータ１の回転軸２等のねじれや減速機１５のガタにより、更にねじれるので、マージンを上乗せした値Ａｊを最大ねじれ角度とする。関節１１２を１８０°反転させると最大ねじれ角度は−Ａｊとなる。関節誤差θは±Ａｊ以下となる。
【００８１】
また、減速機１５の許容トルクが定められている場合があり、この許容トルクから最大関節ねじれ角度を算出してもよい。すなわち、最大ねじれ角度は、減速機１５の許容トルクを、減速機１５のねじれ剛性で割り算した値よりも大きい値、即ち、マージンを上乗せした値とすればよい。
【００８２】
図８、図９及び図１１に示すピッチ角度θａは、波形Ａも波形Ｃも常に一定であり、以下のように設定する。
±θａ／２＞±Ａｊ
θａ＞２Ａｊ
【００８３】
つまり、ピッチ角度θａは最大ねじれ角度Ａｊの２倍よりも大きく設定するのがよい。このような条件にすることにより、θ±θａ×ｎ（ｎ＝１、２、３・・）のように複数解をもつことなく、一つ解となる。ねじれ角度θは、波形Ａと波形Ｃとの位相差であり、モータ制御部２５４での制御補正量でもある。
【００８４】
次に、関節エンコーダ３１の設計例について説明する。図１３は、関節エンコーダ３１の設計フローを示すフローチャートである。まず、関節１１２の仕様と必要な関節分解能を決める（Ｓ３１）。次に、関節１１２の最大ねじれ角度Ａｊを見積もる（Ｓ３２）。この結果を基に、矩形波発生部４１の第１スケール３５の第１パターン３５ａのピッチ角度θａを決定する（Ｓ３３）。次に、関節分解能とピッチ角度θａより、出力側エンコーダ部４２の第２スケール３６の第２パターン３６ａのピッチ角度θｂを決定する（Ｓ３４）。
【００８５】
以下、設計フローの各ステップＳ３１〜Ｓ３４を更に詳細に説明する。まずステップＳ３１について説明する。図１２において、関節１１２の回転中心からハンド１０２までの距離を０．７［ｍ］、関節１１２からリンク１２６までのロボットアームの質量を１２［ｋｇ］、重力角速度を９．８［ｍ／ｓ２］とする。関節１１２からリンク１２６までのロボットアームの重心は、関節１１２の回転中心からハンド１０２までの距離０．７［ｍ］の真ん中にあるとする。また、ハンド１０２とワークＷの質量を４［ｋｇ］とする。ハンド１０２の位置で必要な関節分解能の角度をθｄ、ハンド１０２の位置での必要分解距離をΔＬ、回転中心からハンド１０２の位置までの回転半径をＲｈとすれば、θｄは近似的に以下となる。
θｄ＝ΔＬ÷Ｒｈ
【００８６】
必要分解距離ΔＬを０．００１［ｍｍ］にすれば、関節分解能の角度θｄが以下のように求まる。
θｄ＝０．００１［ｍｍ］÷７００［ｍｍ］＝１．４３×１０−６［ｒａｄ］（＝８．１９×１０−５［°］）
【００８７】
よって、一回転当たりの必要分解能は、２π÷１．４３×１０−６＝４．４×１０６以上となる。
【００８８】
次に、設計フローのステップＳ３２について説明する。図１２では、関節１１２のねじれ角度が最大となる想定姿勢であり、水平方向にロボットアーム１００を伸ばしている。ハンド１０２とワークＷの質量と重心位置が最大のとき、関節１１２に与えるモーメント最大条件となる。ロボットアームの関節１１２の最大ねじれ角度は、モーメント最大条件のモーメントを減速機のねじれ剛性で割り算することで求められる。関節１１２に働くモーメントは以下になる。
Ｍ＝１２［ｋｇ］×９．８［ｍ／ｓ２］×０．７［ｍ］÷２
＋４［ｋｇ］×９．８［ｍ／ｓ２］×０．７［ｍ］＝６８．６［Ｎｍ］
【００８９】
減速機１５を波動歯車減速機として、ハーモニックドライブ（登録商標）のＣＳＧ−２０−１００−２Ａを選択すれば、カタログより剛性値（ばね定数）は１．６×１０４［Ｎｍ／ｒａｄ］〜２．９×１０４［Ｎｍ／ｒａｄ］になる。最も剛性値の小さいｋ＝１．６×１０４［Ｎｍ／ｒａｄ］を採用すれば、最大たわみ角度Ａｊは以下になる。
Ａｊ＝６８．６［Ｎｍ］÷１．６×１０４［Ｎｍ／ｒａｄ］
＝４．２８８×１０−３［ｒａｄ］（＝０．２４６［°］）
【００９０】
関節誤差の支配的な要因である波動歯車減速機のねじれのみを対象に計算してあるが、サーボモータ１の回転軸２のねじれや、ベルト駆動時のベルト剛性やガタ、設計マージン等も実際に考慮する必要がある。このため、実験で求めても良い。
【００９１】
次に設計フローのステップＳ３３について説明する。±θａ／２＞±Ａｊの関係があるため、θａ＞４．２８８×１０−３［ｒａｄ］×２（＝０．２４６［°］×２＝０．４９２［°］）の必要がある。
【００９２】
図３に示すような第１パターン３５ａを構成するには、ピッチ角度θａで割り切れる必要があるため、θａ＝０．５［°］とする。ゆえに、第１スケール３５では、第１パターン３５ａを１回転当たり７２０分割する。
【００９３】
次に、設計フローのステップＳ３４について説明する。ピッチ角度θｂは８．１９×１０−５［°］未満であり、ａ÷ｂが整数である必要がる。ｂ＝８×１０−５［°］とすれば、ａ÷ｂ＝６２５０となる。よって、条件を満たすのでθｂ＝８×１０−５［°］とする。ゆえに、第２スケール３６では１回転当たり、６２５０×７２０＝４５０００００分割する。出力側エンコーダ部４２は、インクリメンタル型のエンコーダの構成であり、１回転当たり４５００分割の検出を更に電気的な分割手段で１０００倍にすることで達成できる。
【００９４】
次に、推定関節角度Ｒ＝１２３．４５［°］、関節位相角度Ｐｊを検出するアップダウンカウンタ（図５中、関節位相演算部２４２）が２０００パルスとして、関節誤差θを求める具体例について述べる。
【００９５】
推定関節位相角度Ｐｍは、Ｒ÷ａの余りを求めれば良いので、１２３．４５［°］÷０．５［°］の余りは０．４５［°］、即ち、Ｐｍ＝０．４５［°］になる。
【００９６】
関節位相角度Ｐｊはアップダウンカウンタの検出値２０００パルスとピッチ角度が６２５０パルスであることから、比例配分で求めると以下になる。
Ｐｊ＝２０００÷６２５０×０．５［°］＝０．１６［°］
【００９７】
従って、関節誤差θは以下となる。
θ＝Ｐｍ−Ｐｊ＝０．４５［°］−０．１６［°］＝０．２９［°］
【００９８】
以上、本第１実施形態によれば、制御部としてのＣＰＵ１０２が矩形波発生部４１の検出結果に基づいて関節１１２のねじれ角度θを求めてサーボモータ１を制御している。したがって、減速機１５の剛性不足やガタにより関節誤差が発生しても、関節１１２の角度を目標関節角度に高精度に制御することができる。しかも、矩形波発生部４１及び出力側エンコーダ部４２は、インクリメンタル型のロータリエンコーダであり、アブソリュート型のロータリエンコーダを実装する構成に比べ、小型でローコストのロボット装置５００を実現することができる。
【００９９】
以上の説明では、ロボット本体１００Ａの関節１１２について説明したが、他の関節１１１，１１３〜１１６についても同様の構成であり、同様に制御可能である。この場合も、関節１１１，１１３〜１１６の角度を目標関節角度に高精度に制御することができる。従って、ロボット本体１００Ａの高精度な位置決めが可能となる。これにより、直進性などの動作精度を高めることが可能となる。
【０１００】
［第２実施形態］
次に、本発明の第２実施形態に係るロボット装置について説明する。図１４は、本発明の第２実施形態に係るロボット装置の制御系を示す機能ブロック図である。なお、本第２実施形態において、上記第１実施形態と同様の構成については、同一符号を付して説明を省略する。本第２実施形態において、装置構成は上記第１実施形態と同様であり、図４に示す制御装置２００のＣＰＵ２０１における制御動作、即ち、プログラム３３０の内容が上記第１実施形態と異なるものである。
【０１０１】
本第２実施形態のＣＰＵ２０１（図４参照）は、信号切替部２５６及び関節誤差判断部２５７の機能を更に有している。
【０１０２】
信号切替部２５６は、出力側エンコーダ部４２からの第２出力側パルス信号を、関節位相演算部２４２またはモータ制御部２５４へ切り替える。モータ制御部２５４へ切替えた場合は、出力側エンコーダ部４２の第２出力側パルス信号のみの値で関節の角度制御を行う。関節誤差判断部２５７は、電源状態等から関節誤差の演算が完了しているか否かを判断する。
【０１０３】
図１５は、第２実施形態におけるＣＰＵの関節の角度の制御動作を示すフローチャートである。なお、図１５に示す各処理ステップは、ＣＰＵ２０１（図４参照）がプログラム３３０を読み出してプログラム３３０を実行することにより実行されるものである。
【０１０４】
まず、ＣＰＵ２０１は、上記第１実施形態と同様、第１制御処理（Ｓ４１：第１制御ステップ）、ねじれ角度演算処理（Ｓ４２：ねじれ角度演算ステップ）、補正制御処理（Ｓ４３：補正制御ステップ）を実行する。
【０１０５】
なお、ステップＳ４２の終了後は、ねじれ角度、即ち関節誤差の計算は完了しているので、関節誤差判断部２５７は、完了したと判断する。そして、信号切替部２５６は、パルス信号送信先を関節位相演算部２４２からモータ制御部２５４へ切り替える。したがって、出力側エンコーダ部４２により出力されたパルス信号は、モータ制御部２５４に送られることとなる。
【０１０６】
次に、ＣＰＵ２０１は、ステップＳ４３で関節の角度が補正されたときの出力側エンコーダ部４２から得た第２出力側パルス信号のカウント値を目標関節角度に対応付ける位置出し処理を実行する（Ｓ４４：位置出しステップ）。補正直後の関節角度を（第２）出力側パルス信号に換算し、この換算値を（第２）出力側パルス信号として更新すれば、以降、関節１１２の角度を第２出力側パルス信号で検出できるものである。このように、ＣＰＵ２０１は、関節角度の補正により、出力側エンコーダ部４２の位置出しを行う。
【０１０７】
次に、ＣＰＵ２０１は、次の目標関節角度があるか否か、即ち、ティーチングペンダント３００（図１）、或いは動作プログラムから次の目標関節角度のデータの入力を受けているか否かを判断する（Ｓ４５）。
【０１０８】
ＣＰＵ２０１は、次の目標関節角度がない場合は（Ｓ４５：ＮＯ）、制御を終了し、次の目標関節角度がある場合は（Ｓ４５：ＹＥＳ）、第２制御処理を実行する（Ｓ４６：第２制御ステップ）。つまり、ＣＰＵ２０１は、次の目標関節角度に関節の角度を制御する際に、第２出力側パルス信号のカウント値に基づいてサーボモータ１を動作させる。
【０１０９】
このように、既に関節誤差が求まっているので、次回以降は、出力側エンコーダ部４２が出力する第２出力側パルス信号をカウントすれば関節の角度を直接検出することができる。
【０１１０】
また、このステップＳ４５における第２制御ステップでは、同時に関節位相演算部２４２、基準位相演算部２４８、関節誤差演算部２４９等の関節誤差演算に関与する部分の演算処理が休止する休止モードとなる。このように休止モードにすることにより、使用するＣＰＵ１０２の演算処理を削減できる。
【０１１１】
以上、本第２実施形態によれば、ＣＰＵ１０２の関節誤差演算後の演算処理が低減する。特に関節が高速に回転する場合に有効である。
【０１１２】
［第３実施形態］
次に、本発明の第３実施形態に係るロボット装置について説明する。図１６は、本発明の第３実施形態に係るロボット装置の制御系を示す機能ブロック図である。なお、本第３実施形態において、上記第１実施形態と同様の構成については、同一符号を付して説明を省略する。本第３実施形態において、装置構成は上記第１実施形態と同様であり、図４に示す制御装置２００のＣＰＵ２０１における制御動作、即ち、プログラム３３０の内容が上記第１実施形態と異なるものである。
【０１１３】
所定時間、ロボットアームの電源をＯＦＦし再度ＯＮした場合、演算した関節誤差が揮発或いは不定になっている場合がある。また、ロボットアームの電源がＯＦＦの間に人為的に動かされる場合もある。このため、電源ＯＦＦし電源ＯＮ直後にロボットアームに記憶されている関節誤差を使うことはできない。
【０１１４】
即ち、電源ＯＦＦし電源ＯＮした場合、矩形波発生部４１のパルス信号のエッジが検出されるまで関節が回転しないと、関節誤差がない状態での制御になる。例えば、図１１で推定関節角度Ｒの状態だったとする。関節が回転して、エッジｔｇ１或いはエッジｔｇ２を検出しなければ、関節誤差θを検出できない。
【０１１５】
そこで、本第３実施形態では、ＣＰＵ１０２（図４）は、上記第１実施形態の各部の他に、関節誤差演算が終了したか否かの関節誤差判断部２５７の機能を更に有する。
【０１１６】
そして、本第３実施形態では、ティーチングペンダント３００が、ユーザに報知可能な報知部として機能する。このティーチングペンダント３００は、ユーザに関節誤差の演算状態を通知する。
【０１１７】
図１７は、第３実施形態におけるＣＰＵの関節の角度の制御動作を示すフローチャートである。なお、図１７に示す各処理ステップは、ＣＰＵ２０１（図４参照）がプログラム３３０を読み出してプログラム３３０を実行することにより実行されるものである。
【０１１８】
電源立ち上げ後、まず、ＣＰＵ２０１は、関節誤差判断部２５７として機能する。即ち、ＣＰＵ２０１は、上記第１実施形態で説明した第１制御処理によるサーボモータ１の動作開始から動作終了までの間に、矩形波発生部４１からの第１出力側パルス信号のエッジを検知したか否かを判断する判断処理を実行する（Ｓ５１）。
【０１１９】
次に、ＣＰＵ２０１は、ステップＳ５１の判断処理にて第１出力側パルス信号のエッジを検知していないと判断した場合（Ｓ５１：ＮＯ）、報知部であるティーチングペンダント３００を動作させて関節駆動をユーザに促す報知処理を実行する（Ｓ５２）。例えば、ティーチングペンダント３００の表示部に表示を行う。また、音声やブザー音等を出力してもよい。
【０１２０】
ユーザがティーチングペンダント３００を操作することにより、ＣＰＵ２０１は、各関節を駆動する（Ｓ５３）。
【０１２１】
次に、ＣＰＵ２０１は、関節誤差の計算が終えたか否か、即ち計算ＯＫか否かを判断する（Ｓ５４）。ＣＰＵ２０１は、関節誤差の計算が終えていない場合は（Ｓ５４：ＮＯ）、計算が終えるまで待つ。ＣＰＵ２０１は、関節誤差の計算が終えた場合は（Ｓ５４：ＹＥＳ）、通常モードに移行し（Ｓ５５）、上記第１実施形態と同様に動作し、処理が完了したら、終了する。
【０１２２】
以上、本第３実施形態によれば、ティーチングペンダント３００がユーザへ無効の旨を伝える報知部として機能させることができるので、ユーザへ関節誤差が有効か否かを通知でき、使い勝手が向上する。なお、報知部としては、ティーチングペンダント３００以外にも、表示装置や音出力装置、光出力装置等であってもよい。
【０１２３】
［第４実施形態］
次に、本発明の第４実施形態に係るロボット装置について説明する。図１８は、本発明の第４実施形態に係るロボット装置の制御系を示す機能ブロック図である。なお、本第４実施形態において、上記第１実施形態と同様の構成については、同一符号を付して説明を省略する。本第４実施形態において、装置構成は上記第１実施形態と同様であり、図４に示す制御装置２００のＣＰＵ２０１における制御動作、即ち、プログラム３３０の内容が上記第１実施形態と異なるものである。
【０１２４】
本第４実施形態では、ＣＰＵ１０２（図４）は、上記第１実施形態の各部の他に、関節誤差演算が終了したか否かの関節誤差判断部２５７の機能を更に有する。また、ＣＰＵ１０２（図４）は、上記第１実施形態の各部の他に、関節誤差の演算値が不定のときに、ロボットアーム姿勢等の情報から暫定関節誤差を演算する暫定関節誤差演算部２６０の機能を更に有する。
【０１２５】
図１９は、第４実施形態におけるＣＰＵの関節の角度の制御動作を示すフローチャートである。なお、図１９に示す各処理ステップは、ＣＰＵ２０１（図４参照）がプログラム３３０を読み出してプログラム３３０を実行することにより実行されるものである。
【０１２６】
電源立ち上げ後、まず、ＣＰＵ２０１は、関節誤差判断部２５７として機能する。即ち、ＣＰＵ２０１は、上記第１実施形態で説明した第１制御処理によるサーボモータ１の動作開始から動作終了までの間に、矩形波発生部４１からの第１出力側パルス信号のエッジを検知したか否かを判断する判断処理を実行する（Ｓ６１）。
【０１２７】
次に、ＣＰＵ２０１は、暫定関節誤差演算部２６０として機能する。即ち、ＣＰＵ２０１は、ステップＳ６１の判断処理にて第１出力側パルス信号のエッジを検知していないと判断した場合（Ｓ６１：ＮＯ）、ロボット本体の姿勢から、関節の推定ねじれ角度を推定する推定処理を実行する（Ｓ６２）。そして、ＣＰＵ２０１は、ステップＳ６１でＮＯと判断した場合は、上記第１実施形態で説明した補正制御処理では、ねじれ角度θの代わりに、ステップＳ６２で計算した推定ねじれ角度を用いる。即ち、ステップＳ６２では、ワーク、ハンド等を考慮したロボット本体の姿勢から各関節のねじれ角度を演算することで、推定ねじれ角度（暫定関節誤差）を求め、関節誤差記憶部（例えば、図４中、ＨＤＤ２０４）に記憶させる。そして、ＣＰＵ２０１は、ねじれ角度の代わりに、推定ねじれ角度（暫定関節誤差）を用いて、補正制御処理を実行する。
【０１２８】
ユーザがティーチングペンダント３００を操作することにより、ＣＰＵ２０１は、各関節を駆動する（Ｓ６３）。
【０１２９】
次に、ＣＰＵ２０１は、関節誤差の計算が終えたか否か、即ち計算ＯＫか否かを判断する（Ｓ６４）。ＣＰＵ２０１は、関節誤差の計算が終えていない場合は（Ｓ６４：ＮＯ）、計算が終えるまで待つ。ＣＰＵ２０１は、関節誤差の計算が終えた場合は（Ｓ６４：ＹＥＳ）、通常モードに移行し（Ｓ６５）、上記第１実施形態と同様に動作し、処理が完了したら、終了する。
【０１３０】
以上、本第４実施形態では、ロボット本体の姿勢から各関節の推定ねじれ角度を演算することで、電源立ち上げ直後等のロボット本体の動作を安定させることができる。
【０１３１】
なお、本発明は、以上説明した実施形態に限定されるものではなく、多くの変形が本発明の技術的思想内で当分野において通常の知識を有する者により可能である。
【０１３２】
上記実施形態では、ロボット本体１００Ａが、垂直多関節ロボットである場合について説明したが、水平多関節ロボット（スカラロボット）、パラレルリンクロボットなどであってもよい。
【０１３３】
また、上記実施形態では、入力側エンコーダ部９を、サーボモータ１の回転軸２に設ける場合について説明したが、減速機１５の入力軸へ直接設けても良い。
【０１３４】
また、減速機が波動歯車減速機であり、波動歯車減速機のねじれが支配的な場合、このねじれ角度は関節のトルクに対応するため、関節誤差から関節のトルク制御を行うこともできる。
【０１３５】
また、図３で説明した線分Ｄ−Ｄに第１検出ヘッド３３と第２検出ヘッド３４の検出位置が一致しない場合、各々のずれをオフセット値として記憶し、波形Ａと波形Ｂのシフト演算を行えばよい。また、エッジとして立ち上がりエッジを検知する場合について説明したが、波形Ａと波形Ｂに周期性があり、その基準を特定できれば、立ち上がりエッジに限らず、立下りエッジであってもよい。
【０１３６】
また、上記実施形態の各処理動作は具体的には制御装置２００の制御部としてのＣＰＵ２０１により実行されるものである。従って上述した機能を実現するプログラムを記録した記録媒体を制御装置２００に供給し、制御装置２００のコンピュータ（ＣＰＵやＭＰＵ）が記録媒体に格納されたプログラムを読み出し実行することによって達成されるようにしてもよい。この場合、記録媒体から読み出されたプログラム自体が上述した実施形態の機能を実現することになり、プログラム自体及びそのプログラムを記録した記録媒体は本発明を構成することになる。
【０１３７】
また、上記実施形態では、コンピュータ読み取り可能な記録媒体がＨＤＤ２０４であり、ＨＤＤ２０４にプログラム３３０が格納される場合について説明したが、これに限定するものではない。プログラム３３０は、コンピュータ読み取り可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。例えば、プログラムを供給するための記録媒体としては、図４に示すＲＯＭ２０２、外部記憶装置３１２、記録ディスク３３１等を用いてもよい。具体例を挙げて説明すると、記録媒体として、フレキシブルディスク、ハードディスク、光ディスク、光磁気ディスク、ＣＤ−ＲＯＭ、ＣＤ−Ｒ、磁気テープ、不揮発性のメモリカード、ＲＯＭ等を用いることができる。
【０１３８】
また、上記実施形態におけるプログラムを、ネットワークを介してダウンロードしてコンピュータにより実行するようにしてもよい。
【０１３９】
また、コンピュータが読み出したプログラムコードを実行することにより、上記実施形態の機能が実現されるだけに限定するものではない。そのプログラムコードの指示に基づき、コンピュータ上で稼働しているＯＳ（オペレーティングシステム）等が実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれる。
【０１４０】
さらに、記録媒体から読み出されたプログラムコードが、コンピュータに挿入された機能拡張ボードやコンピュータに接続された機能拡張ユニットに備わるメモリに書き込まれてもよい。そのプログラムコードの指示に基づき、その機能拡張ボードや機能拡張ユニットに備わるＣＰＵ等が実際の処理の一部または全部を行い、その処理によって上記実施形態の機能が実現される場合も含まれる。
【符号の説明】
【０１４１】
１…サーボモータ（回転モータ）、２…回転軸、９…入力側エンコーダ部、１５…減速機、４１…矩形波発生部、４２…出力側エンコーダ部、１００Ａ…ロボット本体、１１１〜１１６…関節、２０１…ＣＰＵ（制御部）、３３０…プログラム、５００…ロボット装置
【図１】
2014065097.tif 000003
【図２】
2014065097.tif 000004
【図３】
2014065097.tif 000005
【図４】
2014065097.tif 000006
【図５】
2014065097.tif 000007
【図６】
2014065097.tif 000008
【図７】
2014065097.tif 000009
【図８】
2014065097.tif 000010
【図９】
2014065097.tif 000011
【図１０】
2014065097.tif 000012
【図１１】
2014065097.tif 000013
【図１２】
2014065097.tif 000014
【図１３】
2014065097.tif 000015
【図１４】
2014065097.tif 000016
【図１５】
2014065097.tif 000017
【図１６】
2014065097.tif 000018
【図１７】
2014065097.tif 000019
【図１８】
2014065097.tif 000020
【図１９】
2014065097.tif 000021