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

Description

本発明は、回転モータにより減速機を介してロボット本体の関節を駆動する際に、関節のねじれを求めて、関節の角度を制御するものに関する。

一般に、ロボットアーム等のロボット本体の関節の駆動には、ＡＣサーボモータやＤＣブラシレスサーボモータ等のサーボモータが用いられており、高出力のトルクを得るためにサーボモータの出力側に減速機が設けられ、リンク等の構造部材へ結合されている。

従来、ロボットアームの各関節の角度制御は、サーボモータ軸に直結されたロータリーエンコーダ（エンコーダ）でサーボモータ軸の回転角度を検出して行っていた。しかしながら、サーボモータに結合した減速機は、構造上、ねじれやガタが生じるため、関節にねじれが生じ、サーボモータ軸の回転角度では、正確に関節の角度を求めることができなかった。そのため、ロボットアーム先端等、ロボット本体の位置誤差となっていた。

近年、ロボット本体の位置誤差を小さくするために、減速機の出力軸にアブソリュート型のエンコーダを設けて位置を検出するものが提案されている（特許文献１参照）。アブソリュート型のエンコーダを用いることで、関節の角度を直接検出することができるため、ロボット本体の位置誤差も小さくすることができる。

特開２０１０−２３８１７４号公報

しかしながら、上記特許文献１のように、減速機の出力軸にアブソリュート型のエンコーダを設ける場合、アブソリュート型のエンコーダには、非常に高い分解能が要求される。

具体的に説明すると、モータの回転軸にアブソリュート型のエンコーダを実装する方式では、減速機で減速比分の分解能が緩和されるため、低分解能のエンコーダを用いることができる。一方、減速機の出力軸側に設けられるアブソリュート型のエンコーダには、モータに実装するエンコーダの分解能に対して減速比を乗じた分の高い分解能が要求される。例えば、減速機の減速比が５０〜１６０程度で設計されている場合、減速機の出力軸側に設けられるアブソリュート型のエンコーダには、モータ側に実装されるエンコーダに対して、分解能が５０〜１６０倍程度必要となる。

アブソリュート型のエンコーダでは、高分解能になるほど、発光素子や受光素子等の検出素子の数が増加するため、構造が複雑となり、また高価で大型となる。このため、ロボット本体の各関節に、関節角度検出用にアブソリュート型のエンコーダを実装すると、ロボット装置がコスト高で大型になるという問題があった。

そこで、本発明は、ローコストで小型のロボット装置とすることができ、ロボット本体の関節を高精度に目標関節角度に制御できるものを提供することを目的とする。

本発明のロボット装置は、回転モータと、前記回転モータの出力を減速する減速機と、前記回転モータにより前記減速機を介して駆動される関節を有するロボット本体と、前記関節の角度が目標関節角度となるように、前記回転モータによる前記関節の駆動を制御する制御部と、前記減速機の入力側に設けられ、前記回転モータの回転軸の回転に伴って入力側パルス信号を生成する入力側エンコーダ部と、前記減速機の出力側に設けられ、前記関節の駆動に伴って前記関節の角度に応じた第１出力側パルス信号を生成する矩形波発生部と、前記減速機の出力側に設けられ、前記関節の駆動に伴って第２出力側パルス信号を生成する、前記矩形波発生部よりも高い分解能のインクリメンタル型の出力側エンコーダ部と、を備え、前記制御部は、前記入力側パルス信号の示す前記回転モータの回転軸の回転角度及び前記減速機の減速比から、前記関節にねじれがない理想状態における前記関節の角度を推定し、推定関節角度が前記目標関節角度になるように前記回転モータを動作させる第１制御処理と、前記第１出力側パルス信号と、前記理想状態において前記矩形波発生部から得られると想定されるパルス波形とのずれ量に相当する前記関節のねじれ角度を演算するねじれ角度演算処理と、前記推定関節角度が前記ねじれ角度の分補正されるように、前記回転モータを動作させる補正制御処理と、を実行し、前記ねじれ角度演算処理において、前記制御部は、前記第１制御処理による前記回転モータの動作中に、前記第１出力側パルス信号のエッジをトリガとして前記第２出力側パルス信号をカウントし第１角度を求める第１角度演算処理と、前記推定関節角度を前記第１出力側パルス信号のピッチ角度で割り算した際の余りである第２角度を求める第２角度演算処理と、前記ねじれ角度として、前記第１角度と前記第２角度との差分を演算する差分演算処理と、を実行することを特徴とする。

本発明によれば、矩形波発生部の検出結果に基づいて関節のねじれ角度を求めて回転モータを制御しているので、関節の角度を目標関節角度に高精度に制御することができる。しかも、アブソリュート型のエンコーダを実装する構成に比べ、小型でローコストのロボット装置を実現することができる。

第１実施形態に係るロボット装置の概略構成を示す斜視図である。 図１に示す関節近傍のロボットアームの断面図である。 関節エンコーダスケールの平面図である。 ロボット装置の制御装置の構成を示すブロック図である。 第１実施形態に係るロボット装置の制御系を示す機能ブロック図である。 入力側エンコーダ部の原点出しを行うフローチャートである。 第１実施形態におけるＣＰＵの関節の角度の制御動作を示すフローチャートである。 関節にねじれがない理想状態のパルス信号を示す波形図である。 関節にねじれがある状態のパルス信号を示す波形図である。 ＣＰＵによるねじれ角度演算処理の処理動作を示すフローチャートである。 関節にねじれがある状態のパルス信号を示す波形図である。 ロボットアームを水平方向に伸ばしたときの姿勢を示す斜視図である。 関節エンコーダの設計フローを示すフローチャートである。 第２実施形態に係るロボット装置の制御系を示す機能ブロック図である。 第２実施形態におけるＣＰＵの関節の角度の制御動作を示すフローチャートである。 第３実施形態に係るロボット装置の制御系を示す機能ブロック図である。 第３実施形態におけるＣＰＵの関節の角度の制御動作を示すフローチャートである。 第４実施形態に係るロボット装置の制御系を示す機能ブロック図である。 第４実施形態におけるＣＰＵの関節の角度の制御動作を示すフローチャートである。

以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。

［第１実施形態］
図１は、本発明の第１実施形態に係るロボット装置の概略構成を示す斜視図である。図１に示すロボット装置５００は、例えば組立作業等を行う産業ロボットであり、垂直６軸多関節型のロボットアーム１００と、ロボットアーム１００を制御する制御装置２００と、制御装置２００に接続されたティーチングペンダント３００と、を備えている。

ロボットアーム１００は、作業台に固定されるベース部１０１と、変位や力を伝達する複数のリンク１２１〜１２６と、各リンク１２１〜１２６を旋回又は回転可能に連結する複数の関節１１１〜１１６とを有するロボット本体１００Ａを備えている。

また、ロボットアーム１００の先端、即ちリンク１２６には、ハンド１０２が連結されている。ハンド１０２は、任意のワークＷに対して把持等の作業を行うエンドエフェクタである。

各関節１１１〜１１６には、回転モータである電磁モータとしてのサーボモータと、サーボモータの回転軸の回転角度を検出するエンコーダと、サーボモータのトルクを増大させるためにサーボモータの出力を減速する減速機と、が設けられている。減速機には、小型軽量で減速比の大きい波動歯車減速機を用いるのが望ましい。

以下、各関節１１１〜１１６を代表して、関節１１２を例に説明する。図２は、図１に示す関節１１２近傍のロボットアーム１００の断面図である。

図２に示すように、関節１１２には、サーボモータ１と、サーボモータ１の出力（回転角度）を減速する減速機１５とが設けられている。サーボモータ１は、例えばブラシレスＤＣサーボモータやＡＣサーボモータである。

サーボモータ１は、回転軸２とロータマグネット３とで構成された回転部を有している。モータハウジング４，５には、回転軸２を回転自在に支持する軸受６，７が設けられており、モータハウジング４には、ステータコイル８が取付けられている。

回転軸２の一端部には、回転軸２の回転角度を検出するロータリエンコーダである入力側エンコーダ部９が設けられている。つまり、この入力側エンコーダ部９は、減速機１５の入力側に設けられている。そして、サーボモータ１の回転軸２の回転に伴って、入力側パルス信号を生成し、図１の制御装置２００に出力する。入力側エンコーダ部９は、インクリメンタル型のロータリエンコーダでもよいが、サーボモータ１の所定の回転位置を原点とした絶対角度が検出できるアブソリュート型のロータリエンコーダであるのが望ましい。

入力側エンコーダ部９は、エンコーダスケール１０と、エンコーダスケール１０に対向して配置されたエンコーダヘッド１１と、エンコーダスケール１０を回転軸２に固定するフランジ１２と、防塵用のエンコーダカバー１３とで構成されている。サーボモータ１はモータカバー１４で囲われている。

必要に応じて、サーボモータ１と入力側エンコーダ部９との間に、不図示のブレーキユニットを設けてもよい。ブレーキユニットの主な目的は電源ＯＦＦ時のロボット本体１００Ａの姿勢の保持にある。

サーボモータ１の回転軸２の他端部には、減速比Ｎの波動歯車減速機である減速機１５が取付けられている。

この減速機１５は、入力部であるウェブジェネレータ１６と、サーキュラスプライン１７と、ウェブジェネレータ１６とサーキュラスプライン１７との間に配置されたフレックススプライン１８と、を備えている。

フレックススプライン１８は、リンク１２１に連結されており、減速機１５の減速比Ｎで減速される。減速機１５の出力部であるサーキュラスプライン１７は、リンク１２２に結合されている。ウェブジェネレータ１６は、サーボモータ１の回転軸２に結合されている。

サーボモータ１の回転軸２とウェブジェネレータ１６との結合部が、減速機１５の入力軸、フレックススプライン１８とリンク１２１との結合部が減速機１５の出力軸となる。減速機１５の出力軸の回転角度が、出力角度、即ち関節１１２の角度である。

リンク１２１とリンク１２２との間には、クロスローラベアリング２２が設けられており、リンク１２１とリンク１２２とは、クロスローラベアリング２２を介して回転自在に結合されている。このように、サーボモータ１の回転軸２は、減速機１５を介して１／Ｎに減速され、リンク１２１とリンク１２２とが相対的に回転する。

以上の構成により、ロボット本体１００Ａの関節１１２は、サーボモータ１により減速機１５を介して駆動される。一般的な産業用ロボットでは、サーボモータと減速機の間にタイミングベルトを設けて動力を伝達する場合もある。タイミングベルトに用いるプーリ直径が異なると、モータ出力を減速或いは増速する。当然ながら、タイミングベルト等の動力伝達要素もねじれやガタが発生する。また、タイミングベルトの替りに歯車等の伝達要素を設けても同様である。本実施形態では、サーボモータと減速機の間の伝達要素も減速機の一部として扱う。

関節エンコーダ３１は、リンク１２１とリンク１２２との相対角度を検出するものであり、矩形波発生部４１及びインクリメンタル型の出力側エンコーダ部４２からなる。関節エンコーダ３１は、減速機１５の出力側に設けられている。矩形波発生部４１は、関節１１２の駆動に伴って関節１１２の角度に応じた第１出力側パルス信号を生成し、制御装置２００（図１）に出力する。また、出力側エンコーダ部４２は、関節１１２の駆動に伴って第２出力側パルス信号を生成し、制御装置２００（図１）に出力する。

関節エンコーダ３１は、関節エンコーダスケール３２と、第１検出ヘッド３３と、第２検出ヘッド３４から構成されている。本第１実施形態では、関節エンコーダ３１は、反射型のロータリエンコーダである場合について説明するが、透過型のロータリエンコーダであってもよい。

関節エンコーダスケール３２は、リンク１２１の表面に固定して配置され、検出ヘッド３３，３４は、関節エンコーダスケール３２に対向するようにリンク１２２の表面に固定して配置されている。

図３は、関節エンコーダスケール３２の平面図である。関節エンコーダスケール３２は、円板形状又はリング形状に形成され、内周側に形成された第１スケール３５と外周側に形成された第２スケール３６とで構成されている。第１スケール３５には、第１ピッチ角度θ_ａで第１パターン３５ａが形成されている。第２スケール３６には、第２ピッチ角度θ_ｂで第２パターン３６ａが形成されている。これらパターン３５ａ，３６ａは、関節の回転中心に対して放射状に形成されている。

第１検出ヘッド３３は、関節エンコーダスケール３２の第１スケール３５に対して減速機１５の出力軸の回転に伴って相対的に回転し、第１パターン３５ａを読み取って第１出力側パルス信号を制御装置２００（図１）に出力する。第２検出ヘッド３４は、関節エンコーダスケール３２の第２スケール３６に対して減速機１５の出力軸の回転に伴って相対的に回転し、第２パターン３６ａを読み取って第２出力側パルス信号を制御装置２００（図１）に出力する。第１検出ヘッド３３及び第１スケール３５により矩形波発生部４１が構成されている。また、第２検出ヘッド３４及び第２スケール３６により出力側エンコーダ部４２が構成されている。

矩形波発生部４１は、関節１１２の角度を第１ピッチ角度θ_ａで分割検出することができる。同様に、出力側エンコーダ部４２は、関節１１２の角度を第２ピッチ角度θ_ｂで分割検出することができる。

本第１実施形態では、第２ピッチ角度θ_ｂは第１ピッチ角度θ_ａよりも小さい。即ち、第１パターン３５ａの間隔は、第２パターン３６ａの間隔よりも広い。したがって、出力側エンコーダ部４２は、矩形波発生部４１よりも高い分解能である。

また、第１ピッチ角度θ_ａは第２ピッチ角度θ_ｂの整数倍であり、出力側エンコーダ部４２は、矩形波発生部４１の整数倍の分解能で関節１１２の角度を検出することができる。

なお、検出ヘッド３３，３４は、回転方向の判別ができるように２相出力が望ましい。また、検出ヘッド３３，３４の分解能を向上させるために、電気的な分割手段を更に加えても良い。

更に図３を用いて、関節エンコーダ３１の原点を説明する。関節１１２の角度を制御するために、任意の位置に原点を設け、この原点を基準に角度制御を行う。

原点の位置を明示するために、原点の位置でリンク１２１とリンク１２２に貫通する穴を設ける場合もある。この穴に軸を通すことでリンク１２１とリンク１２２と間の原点を特定できる。

図３中の線分Ｄ−Ｄは、図２を用いて説明した第１検出ヘッド３３と第２検出ヘッド３４で検出する位置を表している。この線分Ｄ−Ｄの位置が関節１１２の原点位置になるように関節エンコーダスケール３２と第１検出ヘッド３３と第２検出ヘッド３４との検出位置を調整して配置する。

図４は、ロボット装置５００の制御装置２００の構成を示すブロック図である。制御装置２００は、制御部としてのＣＰＵ２０１、ＲＯＭ２０２、ＲＡＭ２０３、記憶部としてのＨＤＤ２０４、記録ディスクドライブ２０５及び各種のインタフェース２１１〜２１５を備えている。

ＣＰＵ２０１には、ＲＯＭ２０２、ＲＡＭ２０３、ＨＤＤ２０４、記録ディスクドライブ２０５及び各種のインタフェース２１１〜２１５が、バス２１６を介して接続されている。ＲＯＭ２０２には、ＢＩＯＳ等の基本プログラムが格納されている。ＲＡＭ２０３は、ＣＰＵ２０１の演算処理結果を一時的に記憶する記憶装置である。記録ディスクドライブ２０５にはティーチングペンダントから取得した教示データや、動作手順の動作プログラムを記憶する。

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

記録ディスクドライブ２０５は、記録ディスク３３１に記録された各種データやプログラム等を読み出すことができる。

インタフェース２１１には、ティーチングペンダント３００が接続されている。ティーチングペンダント３００は、ユーザが操作するものであり、ユーザの操作により各関節１１１〜１１６の目標関節角度を順次出力する。ＣＰＵ２０１は、インタフェース２１１及びバス２１６を介してティーチングペンダント３００からの目標関節角度のデータ信号の入力を受ける。

インタフェース２１２には、エンコーダ部９，４２及び矩形波発生部４１が接続されている。エンコーダ部９，４２及び矩形波発生部４１からは、角度を示すパルス信号が出力される。ＣＰＵ２０１は、インタフェース２１２及びバス２１６を介してエンコーダ部９，４２及び矩形波発生部４１からのパルス信号の入力を受ける。

インタフェース２１３には、モニタ３１１が接続されており、モニタ３１１には、各種画像が表示される。インタフェース２１４には、書き換え可能な不揮発性メモリや外付けＨＤＤ等の外部記憶装置３１２が接続されている。

インタフェース２１５には、サーボ制御装置３１３が接続されている。ＣＰＵ２０１は、サーボモータ１の回転軸２の回転角度の制御量を示す駆動指令のデータを所定時間間隔でバス２１６及びインタフェース２１５を介してサーボ制御装置３１３に出力する。サーボ制御装置３１３は、制御部としてのＣＰＵ２０１から入力を受けた駆動指令に基づき、サーボモータ１への電流の出力量を演算し、サーボモータ１へ電流を供給して、ロボット本体１００Ａの関節１１１〜１１６の関節角度制御を行う。

即ち、ＣＰＵ２０１は、サーボ制御装置３１３を介して、関節１１２の角度が目標関節角度となるように、サーボモータ１による関節１１２の駆動を制御する。

図５は、本発明の第１実施形態に係るロボット装置の制御系を示す機能ブロック図である。ここで、図４に示す制御部としてのＣＰＵ２０１は、プログラム３３０を実行することにより、図５に示す各部２４２，２４８，２４９，２５４，２５５として機能する。以下、各部の動作について、大まかに説明する。

リセット部２５５は、入力側エンコーダ部９のエンコーダの値をクリアする。関節位相演算部２４２は、矩形波発生部４１と出力側エンコーダ部４２から、第１出力側パルス信号及び第２出力側パルス信号の入力を受ける。そして、関節位相演算部２４２は、第１出力側パルス信号を基準として、第２出力側パルス信号をカウントし、カウント値に相当する角度、即ち関節位相角度を演算する。

基準位相演算部２４８は、入力側エンコーダ部９のパルス信号から得られる回転角度、減速機１５の減速比Ｎ、及びピッチ角度θ_ａから、理想状態の推定関節位相角度を演算する。つまり、基準位相演算部２４８は、入力側エンコーダ部９により検出された入力側パルス信号から、サーボモータ１の回転軸２の回転角度を求め、この回転角度を減速比Ｎで割り算することで、関節１１２の角度を推定する。この推定関節角度は、関節１１２にねじれがないとした場合の理想状態の関節角度である。この推定関節角度をピッチ角度θ_ａで割ったときの余りが推定関節位相角度である。つまり、理想状態とは、減速機１５にねじれがないと仮定した理想の状態である。この理想状態では、入力軸の回転角度を減速比Ｎで割った値が関節角度と一致する。

関節誤差演算部２４９は、基準位相演算部２４８の演算結果と関節位相演算部２４２の演算結果から関節誤差、即ち関節１１２のねじれ角度を求める。推定関節角度と実際の関節角度との差分が、関節１１２のねじれ角度（関節誤差）である。関節誤差が生じる理由としては、減速機１５の弾性ねじれ変形やガタ、モータ軸のねじれ等、複数の要因がある。

モータ制御部２５４は、入力側エンコーダ部９のパルス信号が示す回転角度の値と関節誤差からモータの制御量を決定して、サーボモータ１の駆動を制御する。

ここで、ロボット装置５００による組立作業等の動作に先立って行う、入力側エンコーダ部９の原点出しの方法について説明する。

図６は、入力側エンコーダ部９の原点出しを行うフローチャートである。まず、ユーザは、リンク１２１とリンク１２２に貫通する穴に軸部材を通すことで、関節１１２を原点位置に固定する（Ｓ１）。

次に、ユーザは、ティーチングペンダント３００を操作することで、ＣＰＵ２０１に指令を送り、サーボモータ１の逆転トルクが最小となるモータ回転位置を探索する（Ｓ２）。なお、この逆転トルクはモータの駆動電流からも推定できる。

次に、ユーザは、ティーチングペンダント３００を操作することで、ＣＰＵ２０１に指令を送り、リセット部２５５（ＣＰＵ２０１）が入力側エンコーダ部９をリセットする（Ｓ３）。

ステップＳ１〜Ｓ３の作業は、矩形波発生部４１及び出力側エンコーダ部４２を備える関節エンコーダ３１が原点位置であり、且つ、関節誤差が無い場合にリセット部２５５でリセットして、入力側エンコーダ部９の原点出しを行う。

次に、本第１実施形態の制御部であるＣＰＵ２０１によるロボット制御方法、即ち関節１１２の角度の制御動作について説明する。図７は、第１実施形態におけるＣＰＵ２０１の関節１１２の角度の制御動作を示すフローチャートである。なお、図７に示す各処理ステップは、ＣＰＵ２０１がプログラム３３０を読み出してプログラム３３０を実行することにより実行されるものである。

まず、図５中、モータ制御部２５４として機能するＣＰＵ２０１は、サーボモータ１の制御中、入力側エンコーダ部９からの入力側パルス信号に基づき、サーボモータ１の回転軸２の回転角度を逐次求めている。そして、ＣＰＵ２０１は、サーボモータ１の回転軸２の回転角度及び減速機１５の減速比Ｎから、関節１１２にねじれがない理想状態における関節１１２の角度（推定関節角度）を逐次推定している。具体的には、ＣＰＵ１０２は、回転角度を減速比Ｎで割り算することで、推定関節角度を求める。

ＣＰＵ１０２は、この推定関節角度が動作プログラムやティーチングペンダント３００から取得した目標関節角度になるようにサーボモータを動作させる第１制御処理を実行する（Ｓ１１：第１制御ステップ）。

次に、関節位相演算部２４２、基準位相演算部２４８、及び関節誤差演算部２４９として機能するＣＰＵ１０２は、関節１１２のねじれ角度θを演算するねじれ角度演算処理を実行する（Ｓ１２：ねじれ角度演算ステップ）。このねじれ角度（関節誤差）θは、矩形波発生部４１から得られる第１出力側パルス信号と、理想状態において矩形波発生部４１から得られると想定されるパルス波形とのずれ量に相当する。

図８は、関節１１２にねじれがない理想状態のパルス信号を示す波形図、図９は、関節１１２にねじれがある状態のパルス信号を示す波形図である。

図８及び図９において、波形Ａは、矩形波発生部４１から出力される第１出力側パルス信号のパルス波形である。また、波形Ｂは、出力側エンコーダ部４２から出力される第２出力側パルス信号のパルス波形である。波形Ｃは、理想状態において矩形波発生部４１から得られると想定されるパルス波形である。即ち、波形Ｃは、入力側パルス信号に基づいて求めた推定関節角度をピッチ角度θ_ａで分割した演算波形である。

図８に示すように、関節１１２のねじれ角度が０、即ち関節誤差のない理想的な状態では、波形Ａと波形Ｃとは一致する。関節１１２にねじれが生じた場合、図９に示すように、波形Ａと波形Ｂとは位相がねじれ角度θの分ずれる。したがって、ＣＰＵ１０２は、このねじれ角度θを求めればよい。

次に、モータ制御部２５４として機能するＣＰＵ２０１は、推定関節角度がねじれ角度θの分補正されるように、サーボモータ１を動作させる補正制御処理を実行する（Ｓ１３：補正制御ステップ）。

なお、ＣＰＵ１０２は、次の目標関節角度のデータの入力を受けている場合は、再度、ステップＳ１１〜Ｓ１３を繰り返し実行する。

次に、ステップＳ１２のねじれ角度演算処理について詳細に説明する。図１０は、ＣＰＵ１０２によるねじれ角度演算処理の処理動作を示すフローチャートである。

関節位相演算部２４２として機能するＣＰＵ１０２は、ステップＳ１１の第１制御処理によるサーボモータ１の動作中に、矩形波発生部４１から取得した第１出力側パルス信号のエッジをトリガとして第２出力側パルス信号のパルス数をカウントする。そして、ＣＰＵ１０２は、カウントした結果に基づき、推定関節角度と第１出力側パルス信号のエッジの間の第１角度（関節位相角度）Ｐｊを求める第１角度演算処理を実行する（Ｓ２１：第１角度演算ステップ）。

図１１は、関節１１２にねじれがある状態のパルス信号を示す波形図である。図１１中、Ｑ，Ｒ，Ｓは、入力側エンコーダ部９から出力される入力側パルス信号より求められる推定関節角度である。また、図１１中、ｔｇ１，ｔｇ２は、波形Ａの立ち上がりエッジ、ｔｇ３，ｔｇ４は波形Ｃの立ち上がりエッジである。入力側エンコーダ部９からの入力側パルス信号より求まった角度が推定関節角度Ｑから推定関節角度Ｒに変化する場合について説明する。

関節位相演算部２４２は、アップダウンカウンタとして機能する。アップダウンカウンタは、カウント値をクリアするリセット部と、入力パルス信号のパルス数を計測するカウント部と、カウントのアップダウン指令の符号部からなる。本第１実施形態では、第１検出ヘッド３３が９０°位相の２相センサで構成されており、方向判別信号を出力できる。関節位相演算部２４２のリセット部は、波形Ａのエッジｔｇ１を入力し、カウント部は、波形Ｂを入力し、符号部は、方向判別信号を入力する。

つまり、関節位相演算部２４２は、エッジｔｇ１を入力してから出力側エンコーダ部４２から得た第２出力側パルス信号のパルスのカウントを開始する。そして、関節位相演算部２４２は、入力側エンコーダ部９から得た入力側パルス信号から求まる関節１１２の推定角度が、推定関節角度Ｒとなったときにパルスのカウントを終了して、カウント値を得る。そして、関節位相演算部２４２は、カウント値を角度に換算することで、第１角度（関節位相角度）Ｐｊを求める。このように、第１出力側パルス信号のパルスエッジをトリガとして、高分解能の第２出力側パルス信号のパルスをカウントすることで、正確に第１角度Ｐｊを求めることができる。

また、基準位相演算部２４８として機能するＣＰＵ１０２は、推定関節角度Ｒを第１出力側パルス信号のピッチ角度θ_ａで割り算した際の余りである第２角度Ｐｍを求める第２角度演算処理を実行する（Ｓ２２：第２角度演算ステップ）。つまり、基準位相演算部２４８は、波形Ｃを求めることなく、Ｒ÷ａの余りを求めることで、簡単に第２角度（推定関節位相角度）Ｐｍを計算することができる。

次に、関節誤差演算部２４９として機能するＣＰＵ１０２は、ねじれ角度θとして、第１角度Ｐｊと第２角度Ｐｍとの差分を演算する差分演算処理を実行する（Ｓ２３：差分演算ステップ）。つまり、関節誤差演算部２４９は、θ＝Ｐｍ−Ｐｊの計算を実行する。

以上のステップＳ２１〜Ｓ２３で、推定関節角度がＱからＲになったときの関節誤差の演算手段を述べたが、推定関節角度がＲからＳになった場合も同様の演算を行う。この場合、ステップＳ２１では、立ち上がりエッジｔｇ２をトリガとして第２出力側パルス信号のパルスのカウントを開始する。

ここで、本第１実施形態では、ピッチ角度θ_ａは、関節１１２における最大ねじれ角度の２倍よりも大きい角度に設定されている。以下、関節１１２の最大ねじれ角度について説明する。

図１２は、ロボットアーム１００を水平方向に伸ばしたときの姿勢を示す斜視図である。支配的な関節誤差θの要因は関節１１２の弾性であり、この弾性を図１２を用いて説明する。図１２に示すロボット本体１００Ａの姿勢が、関節１１２に与えるモーメント最大条件となる。

関節１１２の最大ねじれ角度は、関節１１２にかかる最大のモーメントを、減速機１５のねじれ剛性で割り算した値よりも大きい値、即ちマージンを上乗せした値にすればよい。このときのモーメントをＭ、減速機１５のねじれ剛性をｋとすれば、ねじれ角度はＭ／ｋとなる。実際には、モータ１の回転軸２等のねじれや減速機１５のガタにより、更にねじれるので、マージンを上乗せした値Ａｊを最大ねじれ角度とする。関節１１２を１８０°反転させると最大ねじれ角度は−Ａｊとなる。関節誤差θは±Ａｊ以下となる。

また、減速機１５の許容トルクが定められている場合があり、この許容トルクから最大関節ねじれ角度を算出してもよい。すなわち、最大ねじれ角度は、減速機１５の許容トルクを、減速機１５のねじれ剛性で割り算した値よりも大きい値、即ち、マージンを上乗せした値とすればよい。

図８、図９及び図１１に示すピッチ角度θ_ａは、波形Ａも波形Ｃも常に一定であり、以下のように設定する。
±θ_ａ／２＞±Ａｊ
θ_ａ＞２Ａｊ

つまり、ピッチ角度θ_ａは最大ねじれ角度Ａｊの２倍よりも大きく設定するのがよい。このような条件にすることにより、θ±θ_ａ×ｎ（ｎ＝１、２、３・・）のように複数解をもつことなく、一つ解となる。ねじれ角度θは、波形Ａと波形Ｃとの位相差であり、モータ制御部２５４での制御補正量でもある。

次に、関節エンコーダ３１の設計例について説明する。図１３は、関節エンコーダ３１の設計フローを示すフローチャートである。まず、関節１１２の仕様と必要な関節分解能を決める（Ｓ３１）。次に、関節１１２の最大ねじれ角度Ａｊを見積もる（Ｓ３２）。この結果を基に、矩形波発生部４１の第１スケール３５の第１パターン３５ａのピッチ角度θ_ａを決定する（Ｓ３３）。次に、関節分解能とピッチ角度θ_ａより、出力側エンコーダ部４２の第２スケール３６の第２パターン３６ａのピッチ角度θ_ｂを決定する（Ｓ３４）。

以下、設計フローの各ステップＳ３１〜Ｓ３４を更に詳細に説明する。まずステップＳ３１について説明する。図１２において、関節１１２の回転中心からハンド１０２までの距離を０．７［ｍ］、関節１１２からリンク１２６までのロボットアームの質量を１２［ｋｇ］、重力角速度を９．８［ｍ／ｓ^２］とする。関節１１２からリンク１２６までのロボットアームの重心は、関節１１２の回転中心からハンド１０２までの距離０．７［ｍ］の真ん中にあるとする。また、ハンド１０２とワークＷの質量を４［ｋｇ］とする。ハンド１０２の位置で必要な関節分解能の角度をθｄ、ハンド１０２の位置での必要分解距離をΔＬ、回転中心からハンド１０２の位置までの回転半径をＲｈとすれば、θｄは近似的に以下となる。
θｄ＝ΔＬ÷Ｒｈ

必要分解距離ΔＬを０．００１［ｍｍ］にすれば、関節分解能の角度θｄが以下のように求まる。
θｄ＝０．００１［ｍｍ］÷７００［ｍｍ］＝１．４３×１０^−６［ｒａｄ］（＝８．１９×１０^−５［°］）

よって、一回転当たりの必要分解能は、２π÷１．４３×１０^−６＝４．４×１０^６以上となる。

次に、設計フローのステップＳ３２について説明する。図１２では、関節１１２のねじれ角度が最大となる想定姿勢であり、水平方向にロボットアーム１００を伸ばしている。ハンド１０２とワークＷの質量と重心位置が最大のとき、関節１１２に与えるモーメント最大条件となる。ロボットアームの関節１１２の最大ねじれ角度は、モーメント最大条件のモーメントを減速機のねじれ剛性で割り算することで求められる。関節１１２に働くモーメントは以下になる。
Ｍ＝１２［ｋｇ］×９．８［ｍ／ｓ^２］×０．７［ｍ］÷２
＋４［ｋｇ］×９．８［ｍ／ｓ^２］×０．７［ｍ］＝６８．６［Ｎｍ］

減速機１５を波動歯車減速機として、ハーモニックドライブ（登録商標）のＣＳＧ−２０−１００−２Ａを選択すれば、カタログより剛性値（ばね定数）は１．６×１０^４［Ｎｍ／ｒａｄ］〜２．９×１０^４［Ｎｍ／ｒａｄ］になる。最も剛性値の小さいｋ＝１．６×１０^４［Ｎｍ／ｒａｄ］を採用すれば、最大たわみ角度Ａｊは以下になる。
Ａｊ＝６８．６［Ｎｍ］÷１．６×１０^４［Ｎｍ／ｒａｄ］
＝４．２８８×１０^−３［ｒａｄ］（＝０．２４６［°］）

関節誤差の支配的な要因である波動歯車減速機のねじれのみを対象に計算してあるが、サーボモータ１の回転軸２のねじれや、ベルト駆動時のベルト剛性やガタ、設計マージン等も実際に考慮する必要がある。このため、実験で求めても良い。

次に設計フローのステップＳ３３について説明する。±θ_ａ／２＞±Ａｊの関係があるため、θ_ａ＞４．２８８×１０^−３［ｒａｄ］×２（＝０．２４６［°］×２＝０．４９２［°］）の必要がある。

図３に示すような第１パターン３５ａを構成するには、ピッチ角度θ_ａで割り切れる必要があるため、θ_ａ＝０．５［°］とする。ゆえに、第１スケール３５では、第１パターン３５ａを１回転当たり７２０分割する。

次に、設計フローのステップＳ３４について説明する。ピッチ角度θ_ｂは８．１９×１０^−５［°］未満であり、ａ÷ｂが整数である必要がる。ｂ＝８×１０^−５［°］とすれば、ａ÷ｂ＝６２５０となる。よって、条件を満たすのでθ_ｂ＝８×１０^−５［°］とする。ゆえに、第２スケール３６では１回転当たり、６２５０×７２０＝４５０００００分割する。出力側エンコーダ部４２は、インクリメンタル型のエンコーダの構成であり、１回転当たり４５００分割の検出を更に電気的な分割手段で１０００倍にすることで達成できる。

次に、推定関節角度Ｒ＝１２３．４５［°］、関節位相角度Ｐｊを検出するアップダウンカウンタ（図５中、関節位相演算部２４２）が２０００パルスとして、関節誤差θを求める具体例について述べる。

推定関節位相角度Ｐｍは、Ｒ÷ａの余りを求めれば良いので、１２３．４５［°］÷０．５［°］の余りは０．４５［°］、即ち、Ｐｍ＝０．４５［°］になる。

関節位相角度Ｐｊはアップダウンカウンタの検出値２０００パルスとピッチ角度が６２５０パルスであることから、比例配分で求めると以下になる。
Ｐｊ＝２０００÷６２５０×０．５［°］＝０．１６［°］

従って、関節誤差θは以下となる。
θ＝Ｐｍ−Ｐｊ＝０．４５［°］−０．１６［°］＝０．２９［°］

以上、本第１実施形態によれば、制御部としてのＣＰＵ１０２が矩形波発生部４１の検出結果に基づいて関節１１２のねじれ角度θを求めてサーボモータ１を制御している。したがって、減速機１５の剛性不足やガタにより関節誤差が発生しても、関節１１２の角度を目標関節角度に高精度に制御することができる。しかも、矩形波発生部４１及び出力側エンコーダ部４２は、インクリメンタル型のロータリエンコーダであり、アブソリュート型のロータリエンコーダを実装する構成に比べ、小型でローコストのロボット装置５００を実現することができる。

以上の説明では、ロボット本体１００Ａの関節１１２について説明したが、他の関節１１１，１１３〜１１６についても同様の構成であり、同様に制御可能である。この場合も、関節１１１，１１３〜１１６の角度を目標関節角度に高精度に制御することができる。従って、ロボット本体１００Ａの高精度な位置決めが可能となる。これにより、直進性などの動作精度を高めることが可能となる。

［第２実施形態］
次に、本発明の第２実施形態に係るロボット装置について説明する。図１４は、本発明の第２実施形態に係るロボット装置の制御系を示す機能ブロック図である。なお、本第２実施形態において、上記第１実施形態と同様の構成については、同一符号を付して説明を省略する。本第２実施形態において、装置構成は上記第１実施形態と同様であり、図４に示す制御装置２００のＣＰＵ２０１における制御動作、即ち、プログラム３３０の内容が上記第１実施形態と異なるものである。

本第２実施形態のＣＰＵ２０１（図４参照）は、信号切替部２５６及び関節誤差判断部２５７の機能を更に有している。

信号切替部２５６は、出力側エンコーダ部４２からの第２出力側パルス信号を、関節位相演算部２４２またはモータ制御部２５４へ切り替える。モータ制御部２５４へ切替えた場合は、出力側エンコーダ部４２の第２出力側パルス信号のみの値で関節の角度制御を行う。関節誤差判断部２５７は、電源状態等から関節誤差の演算が完了しているか否かを判断する。

図１５は、第２実施形態におけるＣＰＵの関節の角度の制御動作を示すフローチャートである。なお、図１５に示す各処理ステップは、ＣＰＵ２０１（図４参照）がプログラム３３０を読み出してプログラム３３０を実行することにより実行されるものである。

まず、ＣＰＵ２０１は、上記第１実施形態と同様、第１制御処理（Ｓ４１：第１制御ステップ）、ねじれ角度演算処理（Ｓ４２：ねじれ角度演算ステップ）、補正制御処理（Ｓ４３：補正制御ステップ）を実行する。

なお、ステップＳ４２の終了後は、ねじれ角度、即ち関節誤差の計算は完了しているので、関節誤差判断部２５７は、完了したと判断する。そして、信号切替部２５６は、パルス信号送信先を関節位相演算部２４２からモータ制御部２５４へ切り替える。したがって、出力側エンコーダ部４２により出力されたパルス信号は、モータ制御部２５４に送られることとなる。

次に、ＣＰＵ２０１は、ステップＳ４３で関節の角度が補正されたときの出力側エンコーダ部４２から得た第２出力側パルス信号のカウント値を目標関節角度に対応付ける位置出し処理を実行する（Ｓ４４：位置出しステップ）。補正直後の関節角度を（第２）出力側パルス信号に換算し、この換算値を（第２）出力側パルス信号として更新すれば、以降、関節１１２の角度を第２出力側パルス信号で検出できるものである。このように、ＣＰＵ２０１は、関節角度の補正により、出力側エンコーダ部４２の位置出しを行う。

次に、ＣＰＵ２０１は、次の目標関節角度があるか否か、即ち、ティーチングペンダント３００（図１）、或いは動作プログラムから次の目標関節角度のデータの入力を受けているか否かを判断する（Ｓ４５）。

ＣＰＵ２０１は、次の目標関節角度がない場合は（Ｓ４５：ＮＯ）、制御を終了し、次の目標関節角度がある場合は（Ｓ４５：ＹＥＳ）、第２制御処理を実行する（Ｓ４６：第２制御ステップ）。つまり、ＣＰＵ２０１は、次の目標関節角度に関節の角度を制御する際に、第２出力側パルス信号のカウント値に基づいてサーボモータ１を動作させる。

このように、既に関節誤差が求まっているので、次回以降は、出力側エンコーダ部４２が出力する第２出力側パルス信号をカウントすれば関節の角度を直接検出することができる。

また、このステップＳ４５における第２制御ステップでは、同時に関節位相演算部２４２、基準位相演算部２４８、関節誤差演算部２４９等の関節誤差演算に関与する部分の演算処理が休止する休止モードとなる。このように休止モードにすることにより、使用するＣＰＵ１０２の演算処理を削減できる。

以上、本第２実施形態によれば、ＣＰＵ１０２の関節誤差演算後の演算処理が低減する。特に関節が高速に回転する場合に有効である。

［第３実施形態］
次に、本発明の第３実施形態に係るロボット装置について説明する。図１６は、本発明の第３実施形態に係るロボット装置の制御系を示す機能ブロック図である。なお、本第３実施形態において、上記第１実施形態と同様の構成については、同一符号を付して説明を省略する。本第３実施形態において、装置構成は上記第１実施形態と同様であり、図４に示す制御装置２００のＣＰＵ２０１における制御動作、即ち、プログラム３３０の内容が上記第１実施形態と異なるものである。

所定時間、ロボットアームの電源をＯＦＦし再度ＯＮした場合、演算した関節誤差が揮発或いは不定になっている場合がある。また、ロボットアームの電源がＯＦＦの間に人為的に動かされる場合もある。このため、電源ＯＦＦし電源ＯＮ直後にロボットアームに記憶されている関節誤差を使うことはできない。

即ち、電源ＯＦＦし電源ＯＮした場合、矩形波発生部４１のパルス信号のエッジが検出されるまで関節が回転しないと、関節誤差がない状態での制御になる。例えば、図１１で推定関節角度Ｒの状態だったとする。関節が回転して、エッジｔｇ１或いはエッジｔｇ２を検出しなければ、関節誤差θを検出できない。

そこで、本第３実施形態では、ＣＰＵ１０２（図４）は、上記第１実施形態の各部の他に、関節誤差演算が終了したか否かの関節誤差判断部２５７の機能を更に有する。

そして、本第３実施形態では、ティーチングペンダント３００が、ユーザに報知可能な報知部として機能する。このティーチングペンダント３００は、ユーザに関節誤差の演算状態を通知する。

図１７は、第３実施形態におけるＣＰＵの関節の角度の制御動作を示すフローチャートである。なお、図１７に示す各処理ステップは、ＣＰＵ２０１（図４参照）がプログラム３３０を読み出してプログラム３３０を実行することにより実行されるものである。

電源立ち上げ後、まず、ＣＰＵ２０１は、関節誤差判断部２５７として機能する。即ち、ＣＰＵ２０１は、上記第１実施形態で説明した第１制御処理によるサーボモータ１の動作開始から動作終了までの間に、矩形波発生部４１からの第１出力側パルス信号のエッジを検知したか否かを判断する判断処理を実行する（Ｓ５１）。

次に、ＣＰＵ２０１は、ステップＳ５１の判断処理にて第１出力側パルス信号のエッジを検知していないと判断した場合（Ｓ５１：ＮＯ）、報知部であるティーチングペンダント３００を動作させて関節駆動をユーザに促す報知処理を実行する（Ｓ５２）。例えば、ティーチングペンダント３００の表示部に表示を行う。また、音声やブザー音等を出力してもよい。

ユーザがティーチングペンダント３００を操作することにより、ＣＰＵ２０１は、各関節を駆動する（Ｓ５３）。

次に、ＣＰＵ２０１は、関節誤差の計算が終えたか否か、即ち計算ＯＫか否かを判断する（Ｓ５４）。ＣＰＵ２０１は、関節誤差の計算が終えていない場合は（Ｓ５４：ＮＯ）、計算が終えるまで待つ。ＣＰＵ２０１は、関節誤差の計算が終えた場合は（Ｓ５４：ＹＥＳ）、通常モードに移行し（Ｓ５５）、上記第１実施形態と同様に動作し、処理が完了したら、終了する。

以上、本第３実施形態によれば、ティーチングペンダント３００がユーザへ無効の旨を伝える報知部として機能させることができるので、ユーザへ関節誤差が有効か否かを通知でき、使い勝手が向上する。なお、報知部としては、ティーチングペンダント３００以外にも、表示装置や音出力装置、光出力装置等であってもよい。

［第４実施形態］
次に、本発明の第４実施形態に係るロボット装置について説明する。図１８は、本発明の第４実施形態に係るロボット装置の制御系を示す機能ブロック図である。なお、本第４実施形態において、上記第１実施形態と同様の構成については、同一符号を付して説明を省略する。本第４実施形態において、装置構成は上記第１実施形態と同様であり、図４に示す制御装置２００のＣＰＵ２０１における制御動作、即ち、プログラム３３０の内容が上記第１実施形態と異なるものである。

本第４実施形態では、ＣＰＵ１０２（図４）は、上記第１実施形態の各部の他に、関節誤差演算が終了したか否かの関節誤差判断部２５７の機能を更に有する。また、ＣＰＵ１０２（図４）は、上記第１実施形態の各部の他に、関節誤差の演算値が不定のときに、ロボットアーム姿勢等の情報から暫定関節誤差を演算する暫定関節誤差演算部２６０の機能を更に有する。

図１９は、第４実施形態におけるＣＰＵの関節の角度の制御動作を示すフローチャートである。なお、図１９に示す各処理ステップは、ＣＰＵ２０１（図４参照）がプログラム３３０を読み出してプログラム３３０を実行することにより実行されるものである。

電源立ち上げ後、まず、ＣＰＵ２０１は、関節誤差判断部２５７として機能する。即ち、ＣＰＵ２０１は、上記第１実施形態で説明した第１制御処理によるサーボモータ１の動作開始から動作終了までの間に、矩形波発生部４１からの第１出力側パルス信号のエッジを検知したか否かを判断する判断処理を実行する（Ｓ６１）。

次に、ＣＰＵ２０１は、暫定関節誤差演算部２６０として機能する。即ち、ＣＰＵ２０１は、ステップＳ６１の判断処理にて第１出力側パルス信号のエッジを検知していないと判断した場合（Ｓ６１：ＮＯ）、ロボット本体の姿勢から、関節の推定ねじれ角度を推定する推定処理を実行する（Ｓ６２）。そして、ＣＰＵ２０１は、ステップＳ６１でＮＯと判断した場合は、上記第１実施形態で説明した補正制御処理では、ねじれ角度θの代わりに、ステップＳ６２で計算した推定ねじれ角度を用いる。即ち、ステップＳ６２では、ワーク、ハンド等を考慮したロボット本体の姿勢から各関節のねじれ角度を演算することで、推定ねじれ角度（暫定関節誤差）を求め、関節誤差記憶部（例えば、図４中、ＨＤＤ２０４）に記憶させる。そして、ＣＰＵ２０１は、ねじれ角度の代わりに、推定ねじれ角度（暫定関節誤差）を用いて、補正制御処理を実行する。

ユーザがティーチングペンダント３００を操作することにより、ＣＰＵ２０１は、各関節を駆動する（Ｓ６３）。

次に、ＣＰＵ２０１は、関節誤差の計算が終えたか否か、即ち計算ＯＫか否かを判断する（Ｓ６４）。ＣＰＵ２０１は、関節誤差の計算が終えていない場合は（Ｓ６４：ＮＯ）、計算が終えるまで待つ。ＣＰＵ２０１は、関節誤差の計算が終えた場合は（Ｓ６４：ＹＥＳ）、通常モードに移行し（Ｓ６５）、上記第１実施形態と同様に動作し、処理が完了したら、終了する。

以上、本第４実施形態では、ロボット本体の姿勢から各関節の推定ねじれ角度を演算することで、電源立ち上げ直後等のロボット本体の動作を安定させることができる。

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

上記実施形態では、ロボット本体１００Ａが、垂直多関節ロボットである場合について説明したが、水平多関節ロボット（スカラロボット）、パラレルリンクロボットなどであってもよい。

また、上記実施形態では、入力側エンコーダ部９を、サーボモータ１の回転軸２に設ける場合について説明したが、減速機１５の入力軸へ直接設けても良い。

また、減速機が波動歯車減速機であり、波動歯車減速機のねじれが支配的な場合、このねじれ角度は関節のトルクに対応するため、関節誤差から関節のトルク制御を行うこともできる。

また、図３で説明した線分Ｄ−Ｄに第１検出ヘッド３３と第２検出ヘッド３４の検出位置が一致しない場合、各々のずれをオフセット値として記憶し、波形Ａと波形Ｂのシフト演算を行えばよい。また、エッジとして立ち上がりエッジを検知する場合について説明したが、波形Ａと波形Ｂに周期性があり、その基準を特定できれば、立ち上がりエッジに限らず、立下りエッジであってもよい。

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

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

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

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

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

１…サーボモータ（回転モータ）、２…回転軸、９…入力側エンコーダ部、１５…減速機、４１…矩形波発生部、４２…出力側エンコーダ部、１００Ａ…ロボット本体、１１１〜１１６…関節、２０１…ＣＰＵ（制御部）、３３０…プログラム、５００…ロボット装置

Claims (13)

1. 回転モータと、
   前記回転モータの出力を減速する減速機と、
   前記回転モータにより前記減速機を介して駆動される関節を有するロボット本体と、
   前記関節の角度が目標関節角度となるように、前記回転モータによる前記関節の駆動を制御する制御部と、
   前記減速機の入力側に設けられ、前記回転モータの回転軸の回転に伴って入力側パルス信号を生成する入力側エンコーダ部と、
   前記減速機の出力側に設けられ、前記関節の駆動に伴って前記関節の角度に応じた第１出力側パルス信号を生成する矩形波発生部と、
   前記減速機の出力側に設けられ、前記関節の駆動に伴って第２出力側パルス信号を生成する、前記矩形波発生部よりも高い分解能のインクリメンタル型の出力側エンコーダ部と、を備え、
   前記制御部は、
   前記入力側パルス信号の示す前記回転モータの回転軸の回転角度及び前記減速機の減速比から、前記関節にねじれがない理想状態における前記関節の角度を推定し、推定関節角度が前記目標関節角度になるように前記回転モータを動作させる第１制御処理と、
   前記第１出力側パルス信号と、前記理想状態において前記矩形波発生部から得られると想定されるパルス波形とのずれ量に相当する前記関節のねじれ角度を演算するねじれ角度演算処理と、
   前記推定関節角度が前記ねじれ角度の分補正されるように、前記回転モータを動作させる補正制御処理と、を実行し、
   前記ねじれ角度演算処理において、前記制御部は、
   前記第１制御処理による前記回転モータの動作中に、前記第１出力側パルス信号のエッジをトリガとして前記第２出力側パルス信号をカウントし第１角度を求める第１角度演算処理と、
   前記推定関節角度を前記第１出力側パルス信号のピッチ角度で割り算した際の余りである第２角度を求める第２角度演算処理と、
   前記ねじれ角度として、前記第１角度と前記第２角度との差分を演算する差分演算処理と、を実行する、
   ことを特徴とするロボット装置。
2. 前記制御部は、
   前記補正制御処理で前記関節の角度が補正されたときの前記第２出力側パルス信号のカウント値を前記目標関節角度に対応付ける位置出し処理と、
   次の目標関節角度に前記関節の角度を制御する際に、前記第２出力側パルス信号のカウント値に基づいて前記回転モータを動作させる第２制御処理と、を実行することを特徴とする請求項１に記載のロボット装置。
3. 前記出力側エンコーダ部の分解能は、前記矩形波発生部の分解能の整数倍であることを特徴とする請求項１又は２に記載のロボット装置。
4. ユーザに報知可能な報知部を備え、
   前記制御部は、
   前記第１制御処理による前記回転モータの動作開始から動作終了までの間に前記第１出力側パルス信号のエッジを検知したか否かを判断する判断処理と、
   前記判断処理にて前記第１出力側パルス信号のエッジを検知していないと判断した場合、前記報知部を動作させる報知処理と、を実行することを特徴とする請求項１乃至３の何れか１項に記載のロボット装置。
5. 前記制御部は、
   前記第１制御処理による前記回転モータの動作開始から動作終了までの間に前記第１出力側パルス信号のエッジを検知したか否かを判断する判断処理と、
   前記判断処理にて前記第１出力側パルス信号のエッジを検知していないと判断した場合、前記ロボット本体の姿勢から、前記関節の推定ねじれ角度を推定する推定処理と、を実行し、
   前記補正制御処理では、前記判断処理にて前記第１出力側パルス信号のエッジを検知していないと判断した場合は、前記ねじれ角度の代わりに前記推定ねじれ角度を用いることを特徴とする請求項１乃至３のいずれか１項に記載のロボット装置。
6. 前記矩形波発生部は、
   前記関節における最大ねじれ角度の２倍よりも大きいピッチ角度でパターンが形成されたスケールと、
   前記スケールに対して前記減速機の出力軸の回転に伴って相対的に回転し、前記パターンを読み取って前記第１出力側パルス信号を出力する検出ヘッドと、を有することを特徴とする請求項１乃至５のいずれか１項に記載のロボット装置。
7. 前記最大ねじれ角度は、前記関節にかかる最大のモーメントを、前記減速機のねじれ剛性で割り算した値よりも大きい値であることを特徴とする請求項６に記載のロボット装置。
8. 前記最大ねじれ角度は、前記減速機の許容トルクを、前記減速機のねじれ剛性で割り算した値よりも大きい値であることを特徴とする請求項６に記載のロボット装置。
9. 前記入力側エンコーダ部は、アブソリュート型のロータリエンコーダであることを特徴とする請求項１乃至８のいずれか１項に記載のロボット装置。
10. 回転モータと、前記回転モータの出力を減速する減速機と、前記回転モータにより前記減速機を介して駆動される関節を有するロボット本体と、前記減速機の入力側に設けられ、前記回転モータの回転軸の回転に伴って入力側パルス信号を生成する入力側エンコーダ部と、前記減速機の出力側に設けられ、前記関節の駆動に伴って前記関節の角度に応じた第１出力側パルス信号を生成する矩形波発生部と、前記減速機の出力側に設けられ、前記関節の駆動に伴って第２出力側パルス信号を生成する、前記矩形波発生部よりも高い分解能のインクリメンタル型の出力側エンコーダ部と、を有するロボット装置の制御部により、前記関節の角度が目標関節角度となるように、前記回転モータによる前記関節の駆動を制御するロボット制御方法において、
    前記制御部が、前記入力側パルス信号の示す前記回転モータの回転軸の回転角度及び前記減速機の減速比から、前記関節にねじれがない理想状態における前記関節の角度を推定し、推定関節角度が前記目標関節角度になるように前記回転モータを動作させる第１制御ステップと、
    前記制御部が、前記第１出力側パルス信号と、前記理想状態において前記矩形波発生部から得られると想定されるパルス波形とのずれ量に相当する前記関節のねじれ角度を演算するねじれ角度演算ステップと、
    前記制御部が、前記推定関節角度が前記ねじれ角度の分補正されるように、前記回転モータを動作させる補正制御ステップと、を備え、
    前記ねじれ角度演算ステップは、
    前記制御部が、前記第１制御ステップによる前記回転モータの動作中に、前記第１出力側パルス信号のエッジをトリガとして前記第２出力側パルス信号をカウントし第１角度を求める第１角度演算ステップと、
    前記推定関節角度を前記第１出力側パルス信号のピッチ角度で割り算した際の余りである第２角度を求める第２角度演算ステップと、
    前記ねじれ角度として、前記第１角度と前記第２角度との差分を演算する差分演算ステップと、を備えたことを特徴とするロボット制御方法。
11. 前記制御部が、前記補正制御ステップで前記関節の角度が補正されたときの前記第２出力側パルス信号のカウント値を前記目標関節角度に対応付ける位置出しステップと、
    前記制御部が、次の目標関節角度に前記関節の角度を制御する際に、前記第２出力側パルス信号のカウント値に基づいて前記回転モータを動作させる第２制御ステップと、を備えたことを特徴とする請求項１０に記載のロボット制御方法。
12. コンピュータに請求項１０又は１１に記載のロボット制御方法の各ステップを実行させるためのプログラム。
13. 請求項１２に記載のプログラムを記録したコンピュータ読み取り可能な記録媒体。

Images