角速度または角度の検出方法及びロボットの制御方法
【課題】ドリフト現象による誤差と感度の変動による誤差とを較正する方法を提供する。
【解決手段】角速度センサーを用いた角度検出方法にかかわる。角速度センサーの姿勢を第１姿勢から第２姿勢へ変更し、その間の角速度センサーの出力を積分した第１出力積分１６ａと変更に要した第１経過時間１７とを検出する。角速度センサーの姿勢を第２姿勢から第１姿勢と同じ姿勢の第３姿勢へ変更し、第１姿勢から第３姿勢１３へ変更する間の角速度センサーの出力を積分した第２出力積分１６ｂと第１姿勢から第３姿勢へ変更する間の第２経過時間１８とを検出する。第２出力積分１６ｂを第２経過時間１８にて除算してオフセット補正係数を演算する。第１出力積分１６ａからオフセット補正係数と第１経過時間１７との乗算値を引き算した第１正味出力積分１９ａを演算し、第１姿勢と第２姿勢との回転角度の差を第１正味出力積分１９ａにて除算して感度係数を演算する。
【解決手段】角速度センサーを用いた角度検出方法にかかわる。角速度センサーの姿勢を第１姿勢から第２姿勢へ変更し、その間の角速度センサーの出力を積分した第１出力積分１６ａと変更に要した第１経過時間１７とを検出する。角速度センサーの姿勢を第２姿勢から第１姿勢と同じ姿勢の第３姿勢へ変更し、第１姿勢から第３姿勢１３へ変更する間の角速度センサーの出力を積分した第２出力積分１６ｂと第１姿勢から第３姿勢へ変更する間の第２経過時間１８とを検出する。第２出力積分１６ｂを第２経過時間１８にて除算してオフセット補正係数を演算する。第１出力積分１６ａからオフセット補正係数と第１経過時間１７との乗算値を引き算した第１正味出力積分１９ａを演算し、第１姿勢と第２姿勢との回転角度の差を第１正味出力積分１９ａにて除算して感度係数を演算する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、角速度または角度の検出方法及びロボットの制御方法にかかわり、特に角速度センサーを用いて角速度または角度を検出する方法に関するものである。
【背景技術】
【０００２】
多軸ロボットはアーム（以後、腕部と称す）を複数備えている。そして、腕部を回転させるサーボモーター等のアクチュエーターに角度センサーを配置して腕部の姿勢を検出している。そして、腕部を移動させるときに腕部が振動するので、腕部の振動を少なくする駆動方法が求められている。そして、腕部の振動を少なくする方法が特許文献１に開示されている。それによると、腕部に角速度センサーを配置し、アクチュエーターの角度センサーと腕部の角速度センサーとを用いて、腕部の姿勢制御を行っている。
【０００３】
角速度センサーの較正方法が特許文献２に開示されている。これによると、加速度センサーを用いて重力方向を検出する。次に、角速度センサーを用いてロボットの姿勢を算出し、重力方向との差を検出している。そして、角速度センサーの出力のドリフト現象による誤差を較正していた。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特許第３８８３５４４号公報
【特許文献２】特開２００４−２６４０６０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
角速度センサーにはドリフト現象による誤差に加え、感度が変動することによる誤差がある。そこで、ドリフト現象による誤差と感度の変動による誤差を分離して較正する方法が望まれていた。
【課題を解決するための手段】
【０００６】
本発明は、上述の課題の少なくとも一部を解決するためになされたものであり、以下の形態または適用例として実現することが可能である。
【０００７】
［適用例１］
本適用例にかかる角速度または角度の検出方法は、角速度センサーを用いた角速度または角度の検出方法であって、前記角速度センサーの姿勢を第１姿勢から第２姿勢へ変更する第１変更工程と、前記第１姿勢から前記第２姿勢へ変更する間の前記角速度センサーの出力を積分した第１出力積分と、前記第１姿勢から前記第２姿勢へ変更するのに要した時間である第１経過時間と、を検出する第１検出工程と、前記角速度センサーの姿勢を前記第２姿勢から前記第１姿勢と同じ姿勢である第３姿勢へ変更する第２変更工程と、前記第１姿勢から前記第２姿勢を経て前記第３姿勢へ変更する間の前記角速度センサーの出力を積分した第２出力積分と、前記第１姿勢から前記第２姿勢を経て前記第３姿勢へ変更するのに要した時間である第２経過時間と、を検出する第２検出工程と、前記角速度センサーの角速度が零のときの出力をオフセット補正係数とし、前記角速度センサーの角速度変化量と前記角速度センサーの出力の関係を示す係数を感度係数とするとき、前記第２出力積分を前記第２経過時間にて除算して前記オフセット補正係数を演算し、前記第１出力積分から前記オフセット補正係数と前記第１経過時間との乗算値を引き算した第１正味出力積分を演算し、前記第１姿勢と前記第２姿勢との回転角度の差を前記第１正味出力積分にて除算して感度係数を演算する係数算出工程と、前記角速度センサーが第３経過時間の間に姿勢を変更するとき、変更する間における前記角速度センサーの出力と前記オフセット補正係数と感度係数とを用いて前記角速度センサーの角速度または姿勢を検出する姿勢検出工程と、を有することを特徴とする。
【０００８】
この角速度または角度の検出方法によれば、第１姿勢と第３姿勢とにおいて角速度センサーを同じ姿勢にしている。そして、第２経過時間の間に変化した角速度センサーの出力の差を第２経過時間にて除算することにより、オフセット補正係数を算出している。
【０００９】
第１姿勢から第２姿勢へ変更する間のオフセットによる誤差はオフセット補正係数と第１経過時間とを乗算して算出される。そして、角速度センサーの出力を積分した値からオフセットによる誤差を引き算して第１正味出力積分を算出することによりオフセットによる誤差を除いている。次に、第１姿勢と第２姿勢との回転角度の差を第１正味出力積分にて除算して感度係数を演算している。
【００１０】
従って、角速度センサーの出力のうちオフセットによる誤差を除いている。そして、オフセット誤差のない角速度センサーの出力を用いて角速度センサーの感度係数を演算している。その結果、精度良く角速度センサーの角速度及び姿勢を検出することができる。
【００１１】
［適用例２］
上記適用例にかかる角速度または角度の検出方法において、前記角速度センサーはセンサー支持部に配置され、前記センサー支持部は、回転軸と前記センサー支持部の回転角度を検出する回転角度検出器とを有し、前記角速度センサーの姿勢は前記回転角度検出器を用いて検出することを特徴とする。
【００１２】
この角速度または角度の検出方法によれば、角速度センサーが静止しているときの角速度センサーの回転角度は回転角度検出器を用いて所望のときに検出することができる。
【００１３】
［適用例３］
上記適用例にかかる角速度または角度の検出方法において、前記角速度センサーは振動子型ジャイロスコープであることを特徴とする。
【００１４】
この角速度または角度の検出方法によれば、角速度センサーに振動子型ジャイロスコープを用いている。回転型ジャイロスコープに比べて振動子型ジャイロスコープは回転機構を用いないので小型にすることができる。従って、小型のセンサーにて回転角度の変動を検出することができる。
【００１５】
［適用例４］
上記適用例にかかる角速度または角度の検出方法において、前記回転角度検出器はロータリーエンコーダーを用いて回転角度を検出することを特徴とする。
【００１６】
この角速度または角度の検出方法によれば、回転角度検出器にロータリーエンコーダーを用いている。ロータリーエンコーダーは目盛りを配置した円板と目盛りの検出器を備えている。そして、目盛りの個数を多くすることにより、高い分解能を実現することができる。従って、高い分解能のロータリーエンコーダーを用いることにより精度良く角速度センサーの回転角度を検出することができる。
【００１７】
［適用例５］
本適用例にかかるロボットの制御方法は、角速度センサーを用いて可動部の姿勢を検出するロボットの制御方法であって、前記角速度センサーを較正する較正工程と、前記可動部の姿勢を検出する姿勢検出工程と、を有し、前記較正工程は、前記角速度センサーの姿勢を第１姿勢から第２姿勢へ変更する第１変更工程と、前記第１姿勢から前記第２姿勢へ変更する間の前記角速度センサーの出力を積分した第１出力積分と、前記第１姿勢から前記第２姿勢へ変更するのに要した時間である第１経過時間と、を検出する第１検出工程と、前記角速度センサーの姿勢を前記第２姿勢から前記第１姿勢と同じ姿勢である第３姿勢へ変更する第２変更工程と、前記第１姿勢から前記第２姿勢を経て前記第３姿勢へ変更する間の前記角速度センサーの出力を積分した第２出力積分と、前記第１姿勢から前記第２姿勢を経て前記第３姿勢へ変更するのに要した時間である第２経過時間と、を検出する第２検出工程と、時間の経過にともなって変化する前記角速度センサーの前記出力の変化率をオフセット補正係数とし、前記角速度センサーの角度変化量と前記角速度センサーの出力の関係を示す係数を感度係数とするとき、前記第２出力積分を前記第２経過時間にて除算して前記オフセット補正係数を演算し、前記第１出力積分からオフセット補正係数と前記第１経過時間との乗算値を引き算した第１正味出力積分を演算し、前記第１姿勢と前記第２姿勢との回転角度の差を前記第１正味出力積分にて除算して感度係数を演算する係数算出工程と、を有し、前記姿勢検出工程は、前記角速度センサーが第３経過時間の間に姿勢を変更するとき、変更する間における前記角速度センサーの出力と前記オフセット補正係数と感度係数とを用いて前記角速度センサーの角速度または姿勢を検出する姿勢検出工程を有し、検出した前記角速度センサーの角速度の情報を用いて前記可動部の角速度検出、または検出した前記角速度センサーの姿勢の情報を用いて前記可動部の姿勢検出をすることを特徴とする。
【００１８】
このロボットの制御方法によれば、較正工程において、オフセット補正係数と角速度センサーの感度係数とを演算している。そして、オフセット誤差のない角速度センサーの出力を用いて角速度センサーの感度係数を演算している。従って、精度良く角速度センサーの姿勢を検出することができる為、精度良くロボットの姿勢を制御することができる。
【００１９】
［適用例６］
上記適用例にかかるロボットの制御方法において、前記ロボットが作業を行う作業工程を有し、前記較正工程は前記作業工程と並行して行われることを特徴とする。
【００２０】
このロボットの制御方法によれば、較正工程と作業工程とが並行して行われる。従って、較正工程と作業工程とを別々に行う方法に比べて生産性良く角速度センサーの較正を行うことができる。
【００２１】
［適用例７］
上記適用例にかかるロボットの制御方法において、前記第１姿勢と前記第３姿勢とにおいて前記角速度センサーは同じ場所に位置することを特徴とする。
【００２２】
このロボットの制御方法によれば、第１姿勢と第３姿勢とにおいて角速度センサーの姿勢が同じ姿勢のとき角速度センサーが同じ場所に位置している。角速度センサーが同じ場所に位置させるときにロボットの姿勢を同じ姿勢となるように駆動することにより角速度センサーの姿勢を同じにすることができる。従って容易に角速度センサーの姿勢を同じ姿勢にすることができる。
【図面の簡単な説明】
【００２３】
【図１】第１の実施形態にかかわり、（ａ）は、角度検出装置の構成を示す概略斜視図、（ｂ）は、角度検出装置の構成を示す模式断面図。
【図２】腕部の角度を検出する工程を示すフローチャート。
【図３】腕部の角度を検出する方法を説明するための模式図。
【図４】回転角度検出器及び角速度センサーの出力と演算結果の推移を示すタイムチャート。
【図５】（ａ）は、腕部の角度を検出する方法を説明するための模式図であり、図（ｂ）は、角速度センサーの出力と演算結果の推移を示すタイムチャート。
【図６】第２の実施形態にかかわり、（ａ）は、ロボットシステムの構成を示す概略斜視図、（ｂ）は、ロボットの構成を示す模式断面図。
【図７】ロボットの姿勢を説明するための模式図。
【図８】部品の移動工程を示すフローチャート。
【発明を実施するための形態】
【００２４】
以下、実施形態について図面に従って説明する。
尚、各図面における各部材は、各図面上で認識可能な程度の大きさとするため、各部材毎に縮尺を異ならせて図示している。
（第１の実施形態）
本実施形態における角度検出装置とこの角度検出装置を用いた特徴的な角度検出方法について図１〜図５に従って説明する。
【００２５】
図１（ａ）は、角度検出装置の構成を示す概略斜視図であり、図１（ｂ）は、角度検出装置の構成を示す模式断面図である。図１に示すように、角度検出装置１は平板状に形成された基台２を備えている。基台２の水平面上の１方向をＸ方向とする。そして、重力方向と逆の方向をＺ方向とし、Ｘ方向及びＺ方向と直交する方向をＹ方向とする。
【００２６】
基台２上には支持台３が配置されている。支持台３の内部には空洞が形成され、この空洞は支持板４により上下に分割されている。支持板４の下側にはモーター５が配置され、モーター５の下側には回転角度検出器６が配置されている。モーター５は電気信号によって回転方向を制御可能であればよく、直流モーター、パルスモーター、超音波モーター等のモーターを用いることができる。本実施形態では、例えば、直流モーターを採用している。回転角度検出器６は、モーター５の駆動軸の回転角度を検出可能であれば良く、光学式エンコーダー、磁気式エンコーダー等のロータリーエンコーダーを用いることができる。本実施形態では、例えば、光学式のロータリーエンコーダーを採用している。
【００２７】
支持板４の上側には減速機７が配置され、モーター５の回転軸５ａと接続されている。そして、減速機７の上側には出力軸７ａが配置されている。そして、モーター５の回転軸５ａの回転速度を減速した回転速度にて出力軸７ａが回転する。減速機７は歯車減速機、ハーモニックドライブ（登録商標）、ローラドライブ（登録商標）等を採用することができる。本実施形態では、例えば、ハーモニックドライブ（登録商標）を採用している。支持台３の上面には孔部３ａが形成され、孔部３ａから出力軸７ａが突出して配置されている。
【００２８】
出力軸７ａと接続して略直方体状のセンサー支持部としての腕部８が配置され、腕部８は出力軸７ａを中心に回転する。腕部８において出力軸７ａと反対側には角速度センサー９が配置されている。角速度センサー９の種類は特に限定されず、回転型ジャイロスコープ、振動型ジャイロスコープ、ガス型ジャイロスコープ、リングレーザジャイロ等のジャイロスコープを用いることができる。本実施形態では、例えば、振動型ジャイロスコープに属する振動子型ジャイロスコープを採用している。
【００２９】
モーター５が回転するとき、出力軸７ａが回転することにより、腕部８が回転させられる。角速度センサー９は腕部８と共に回転するので、角速度センサー９は腕部８の回転速度を検出する。そして、角速度センサー９の回転速度を所定の間で積分することにより腕部８が回転する角度を検出することができる。また、モーター５の回転角度は回転角度検出器６を用いて検出できるので、回転角度検出器６を用いて腕部８の回転角度を検出することができる。そして、回転角度検出器６を用いて検出した腕部８の回転角度と角速度センサー９を用いて検出した腕部８の回転角度とを比較することにより、角速度センサー９の較正を行うことができる。
【００３０】
基台２の図中右側には制御装置１０が配置されている。制御装置１０には中央演算装置、記憶部、インターフェース、モーター５を駆動するモーター駆動回路、入力装置、表示装置等を備えている。回転角度検出器６及び角速度センサー９はインターフェースを介して中央演算装置と接続されている。モーター５はモーター駆動回路と接続され、モーター駆動回路はインターフェースを介して中央演算装置と接続されている。記憶部には角度検出装置１を制御する動作手順を示したプログラムソフトや角度データが記憶されている。そして、中央演算装置はモーター５を所定の角度回転させて、回転角度検出器６及び角速度センサー９の出力を記憶部に記憶することができる。さらに、中央演算装置は回転角度検出器６及び角速度センサー９の出力を比較して較正するための演算を行う。
【００３１】
（角度検出方法）
次に、上述した角度検出装置１を用いて、腕部８の角度を検出する方法について図２〜図５にて説明する。図２は、腕部の角度を検出する工程を示すフローチャートである。図３〜図５は、腕部の角度を検出する方法を説明するための模式図及びタイムチャートである。
【００３２】
図２に示すフローチャートにおいて、ステップＳ１とステップＳ２とは並行して行われる。ステップＳ１は、第１変更工程に相当し、腕部を回転して角速度センサーの姿勢を第１姿勢から第２姿勢へと変更する工程である。次にステップＳ３に移行する。ステップＳ２は、第１検出工程に相当する。回転角度検出器の出力と角速度センサーの出力とを検出する。そして、角速度センサーの姿勢が第１姿勢から第２姿勢へと変更する間の角速度センサーの出力を積分した第１出力積分と、角速度センサーの出力を積分する間の時間とを検出する工程である。次にステップＳ３に移行する。ステップＳ３は、第２変更工程に相当し、腕部を回転して角速度センサーの姿勢を第２姿勢から第１姿勢と同じ姿勢である第３の姿勢へと変更する工程である。次にステップＳ５に移行する。ステップＳ４はステップＳ１及びステップＳ３と並行して行なわれる。ステップＳ４は、第２検出工程に相当する。回転角度検出器の出力と角速度センサーの出力とを検出する。そして、角速度センサーの姿勢が第１姿勢から第２姿勢を経て第３の姿勢へと変更する間の角速度センサーの出力を積分した第２出力積分と、角速度センサーの出力を積分する間の時間とを検出する工程である。次にステップＳ５に移行する。ステップＳ５は、係数算出工程に相当する。時間の経過にともなって変化する角速度センサーの出力の積分値の変化率であるオフセット補正係数を演算する。さらに、角速度センサーの角速度変化量と角速度センサーの出力の関係を示す感度係数を演算する。ステップＳ１〜ステップＳ５は、ステップＳ１１の較正工程に相当し、角速度センサーの係数を較正する工程である。次にステップＳ６及びステップＳ７に移行する。
【００３３】
ステップＳ６とステップＳ７とは並行して行われる。ステップＳ６は、第３変更工程に相当し、腕部を回転して角速度センサーの姿勢を変更する工程である。ステップＳ７は、第３検出工程に相当し、角速度センサーの出力を用いて角速度センサーの姿勢を検出することにより腕部の角度を検出する工程である。ステップＳ６及びステップＳ７は、ステップＳ１２の姿勢検出工程に相当し、姿勢が変化する角速度センサーの角速度を検出する。さらに、角速度センサー及び腕部の姿勢を検出する工程である。以上により、腕部の角度を検出する方法を終了する。
【００３４】
次に、図３〜図５を用いて、図２に示したステップと対応させて、腕部の角度を検出する方法を詳細に説明する。図３（ａ）はステップＳ１の第１変更工程とステップＳ２の第１検出工程とに対応する図である。図３（ａ）において、支持台３のＹ方向に角速度センサー９が位置するように制御装置１０は腕部８を配置する。このときの角速度センサー９の姿勢を第１姿勢１１とする。制御装置１０はモーター５を駆動して腕部８を時計回りに回転させる。その結果、図３（ｂ）に示すように、支持台３のＸ方向に角速度センサー９が位置するように腕部８が配置される。角速度センサー９のこの姿勢を第２姿勢１２とする。角速度センサー９が第１姿勢１１から第２姿勢１２になる間の角速度センサー９の出力を制御装置１０は入力して積分する。そして、積分した結果の推移を記憶部に記憶する。さらに、腕部８が回転する間の回転角度検出器６の出力を制御装置１０は入力して記憶部に記憶する。さらに、腕部８が回転する間の時間の経過を制御装置１０は記憶部に記憶する。
【００３５】
ステップＳ３の第２変更工程において、制御装置１０はモーター５を駆動して腕部８を反時計回りに回転させる。その結果、図３（ｃ）に示すように角速度センサー９の姿勢が第３姿勢１３になる。この第３姿勢１３は第１姿勢１１と同じ姿勢となっている。ステップＳ４の第２検出工程では、角速度センサー９の姿勢が第１姿勢１１から第２姿勢１２を経て第３姿勢１３になる間の角速度センサー９の出力を制御装置１０が入力して積分する。そして、積分した結果の推移を記憶部に記憶する。さらに、腕部８が回転する間の回転角度検出器６の出力を制御装置１０は入力して記憶部に記憶する。さらに、腕部８が回転する間の時間の経過を制御装置１０は記憶部に記憶する。
【００３６】
図４は、ステップＳ１〜ステップＳ４とステップＳ５の係数算出工程に対応する図であり、回転角度検出器及び角速度センサーの出力と演算結果の推移を示すタイムチャートである。図４において、横軸は時間の経過を示し、図中左から右へ時間が推移する。縦軸には、４つの要素が配置されている。図中上側から順に角度、角速度、角度、角度が配置されている。縦軸の角度は図中下側より上側が時計回りの角度が大きくなっている。縦軸の角速度は図中下側より上側が時計回り方向の角速度が大きくなっている。
【００３７】
上段の角度検出器出力推移線１４は回転角度検出器６の出力を示している。縦軸の角度は横軸と交差する場所が零点であり、腕部８がＹ方向に延在するときの角度を零とする。そして、角度検出器出力推移線１４が横軸の上側では腕部８がＹ方向から時計回りの方向に回転した場所に位置することを示している。
【００３８】
ステップＳ１の第１変更工程において、まず、角速度センサー９の姿勢は第１姿勢１１となっており、角度検出器出力推移線１４は零となっている。次に、腕部８が時計回りに回転することにより角速度センサー９の姿勢が第１姿勢１１から第２姿勢１２に移行する。従って、角度検出器出力推移線１４は上側へ推移する。このときの角度検出器出力推移線１４の値を回転角度差１４ａとする。回転角度差１４ａは第１姿勢１１と第２姿勢１２とにおける回転角度検出器６の回転角度差を示している。
【００３９】
ステップＳ３の第２変更工程において腕部８が反時計回りに回転することにより角速度センサー９が第２姿勢１２から第３姿勢１３に移行する。従って、角度検出器出力推移線１４は下側へ推移する。そして、第３姿勢１３は第１姿勢１１と同じであり、腕部８がＹ方向に延在するので角度検出器出力推移線１４が零となる。
【００４０】
上から２段目の角速度センサー出力推移線１５は角速度センサー９の出力を示している。縦軸の角速度は横軸と交差する場所が零点である。腕部８が静止しているとき、角速度センサー９の出力は所定の値となっており、この値をオフセット値と称す。オフセット値は角速度センサー９の温度等の環境により変動する。ステップＳ１の第１変更工程においてモーター５が腕部８を回転させる前では角速度センサー９が静止しているので、角速度センサー出力推移線１５はオフセット出力１５ａとなっている。
【００４１】
ステップＳ１の第１変更工程において、腕部８が時計回りに回転することにより、角速度センサー９が時計回りに回転する。そして、回転速度と比例した値が角速度センサー９から出力される。角速度センサー９が時計回りに回転するとき角速度センサー出力推移線１５はオフセット出力１５ａの上側を推移する。腕部８の回転速度と対応して角速度センサー９が角速度値を出力する。そして、角速度センサー出力推移線１５は腕部８の回転速度と対応した形状になる。ステップＳ３の第２変更工程では腕部８が反時計回りに回転する。そして、角速度センサー９が反時計回りに回転するとき角速度センサー出力推移線１５はオフセット出力１５ａの下側を推移し、腕部８の回転速度と対応した形状になる。
【００４２】
上から３段目の角速度出力積分推移線１６は角速度センサー出力推移線１５を時間軸において積分した値を示した線である。角速度出力積分推移線１６はステップＳ２の第１検出工程及びステップＳ４の第２検出工程にて演算される。角度検出装置１が第１姿勢１１から第２姿勢１２に変化する間に角速度出力積分推移線１６が変化した量を第１出力積分１６ａとする。そして、角度検出装置１が第１姿勢１１から第２姿勢１２に変化する間の時間を第１経過時間１７とする。ステップＳ２では第１出力積分１６ａ及び第１経過時間１７が制御装置１０の記憶部に記憶される。尚、第１出力積分１６ａ及び第１経過時間１７は角速度センサー９の出力の振動が安定したときの値を用いるのが好ましい。このとき、腕部８が振動することの影響を少なくすることができる為、精度良く較正することができる。
【００４３】
角度検出装置１が第１姿勢１１から第３姿勢１３に変化する間に角速度出力積分推移線１６が変化した量を第２出力積分１６ｂとする。そして、角度検出装置１が第１姿勢１１から第３姿勢１３に変化する間の時間を第２経過時間１８とする。ステップＳ４では第２出力積分１６ｂ及び第２経過時間１８が制御装置１０の記憶部に記憶される。
【００４４】
ステップＳ５の係数算出工程において、角速度センサー９の角速度が零のときの出力であるオフセット補正係数を演算する。オフセット補正係数は第２出力積分１６ｂを第２経過時間１８にて除算して算出する。従って、オフセット補正係数は第２経過時間１８におけるオフセット出力１５ａの平均値に相当する。オフセット線１６ｃはオフセット補正係数による角度の推移を示す線である。オフセット線１６ｃは第１姿勢１１と第３姿勢１３とにおいて角速度出力積分推移線１６と一致する直線となっている。
【００４５】
上から４段目の正味出力積分推移線１９は、角速度出力積分推移線１６の縦軸の値からオフセット線１６ｃの示す縦軸の値を引いた曲線である。つまり、正味出力積分推移線１９は、角速度出力積分推移線１６からオフセットによる角度誤差を引いた値が示す線となっている。角速度センサー９が第２姿勢１２のときにおける正味出力積分推移線１９の値を第１正味出力積分１９ａとする。第１正味出力積分１９ａは第１出力積分１６ａからオフセット補正係数と第１経過時間１７との乗算値を引き算した値となる。次に、回転角度差１４ａを第１正味出力積分１９ａにて除算した値を感度係数とする。感度係数は回転角度検出器６の出力が示す角速度と角速度センサー９の出力が示す角速度との関係を示す係数である。感度係数は回転角度検出器６の出力が示す角度と角速度センサー９の出力の積分値が示す角度とから算出することができる。
【００４６】
図５（ａ）はステップＳ６の第３変更工程に対応する図である。図５（ａ）に示すように、ステップＳ６において、制御装置１０はモーター５を駆動して腕部８を時計回りに回転させる。その結果、支持台３のＸ方向に対して斜めの方向に角速度センサー９が位置するように腕部８が配置される。このときの、角速度センサー９の姿勢を第４姿勢２２とする。従って、ステップＳ６では角速度センサー９の姿勢が第３姿勢１３から第４姿勢２２へと変更される。
【００４７】
図５（ｂ）はステップＳ７の第３検出工程に対応する図であり、角速度センサーの出力と演算結果の推移を示すタイムチャートである。図５（ｂ）において、横軸は時間の経過を示し、図中左から右へ時間が推移する。縦軸には、４つの要素が配置されている。図中上側から順に、角速度、角度、角度、角度が配置されている。縦軸の角度は図中下側より上側が時計回りの角度が大きくなっている。縦軸の角速度は図中下側より上側が時計回り方向の角速度が大きくなっている。
【００４８】
上段の角速度センサー出力推移線２３は角速度センサー９の出力を示している。縦軸の角速度は横軸と交差する場所が零点である。角度検出装置１が第３姿勢１３のとき腕部８は停止しており、角速度センサー９の出力はオフセット出力２３ａとなっている。そして、モーター５は加速して減速することにより、角度検出装置１の姿勢を第４姿勢２２に変更する。従って、角速度センサー出力推移線２３は山形に推移する。
【００４９】
上から２段目の角速度出力積分推移線２４は角速度センサー出力推移線２３を時間軸において積分した値を示した線である。角速度出力積分推移線２４はステップＳ７の第３検出工程にて演算される。角度検出装置１が第３姿勢１３から第４姿勢２２に変化する間に角速度出力積分推移線２４が変化した量を第３出力積分２４ａとする。そして、角速度センサー９が第３姿勢１３から第４姿勢２２に変化する間の時間を第３経過時間２５とする。ステップＳ７では第３出力積分２４ａ及び第３経過時間２５が制御装置１０の記憶部に記憶される。
【００５０】
オフセット線２４ｂはオフセット補正係数による角度の推移を示す線である。オフセット補正係数と第３経過時間２５とを乗算した値をオフセット補正量２４ｃとする。オフセット補正量２４ｃは第３経過時間２５に対応するオフセットによる誤差である。そして、第３出力積分２４ａからオフセット補正量２４ｃを引き算した値を第２正味出力積分２４ｄとする。第２正味出力積分２４ｄは角速度センサー９からオフセットによる誤差を除いた値である。
【００５１】
上から３段目の正味出力積分推移線２６は角速度出力積分推移線２４の縦軸の値からオフセット線２４ｂの示す縦軸の値を引いた曲線である。つまり、正味出力積分推移線２６は、角速度出力積分推移線２４からオフセットによる誤差を除いた線となっている。そして、角速度センサー９が第４姿勢２２のとき、第３経過時間２５の値は第２正味出力積分２４ｄとなる。
【００５２】
上から４段目のセンサー角度推移線２７は、正味出力積分推移線２６の縦軸の値に感度係数を乗算した値からなる曲線である。そして、センサー角度推移線２７は角速度センサー９の出力が示す角速度センサー９の回転角度の推移を示している。第４姿勢角度２７ａは、角度検出装置１が第４姿勢２２のときに角速度センサー９が示す角速度センサー９の角度である。そして、第４姿勢角度２７ａは第４姿勢２２におけるセンサー角度推移線２７の値となっている。
【００５３】
つまり、第３出力積分２４ａからオフセット補正量２４ｃを引き算して第２正味出力積分２４ｄを演算する。次に、第２正味出力積分２４ｄと感度係数とを乗算して第４姿勢角度２７ａを算出する。さらに、角速度センサー出力推移線２３から角速度を算出する。つまり、角速度センサー出力推移線２３からオフセット出力２３ａを減算する。次に、減算した値に感度係数を乗算することにより、角速度を算出する。以上の工程により腕部の角度を検出する工程を終了する。
【００５４】
上述したように、本実施形態によれば、以下の効果を有する。
（１）本実施形態によれば、ステップＳ５の係数算出工程において角速度センサー９の出力のうちオフセット出力１５ａによる誤差を除いている。そして、オフセット誤差のない角速度センサー９の出力を用いて角速度センサー９の感度係数を演算している。従って、精度良く角速度センサー９の姿勢を検出することができる。
【００５５】
（２）本実施形態によれば、角度検出装置１は回転角度検出器６を備えている。そして、回転角度検出器６が検出する角度と角速度センサー９の角度とは相関関係があるように角速度センサー９と回転角度検出器６とが配置されている。従って、角速度センサー９の回転角度は回転角度検出器６を用いて所望のときに検出することができる。そして、角速度センサー９が所望の場所に静止して位置するときに角速度センサー９を較正することができる。
【００５６】
（３）本実施形態によれば、角速度センサー９に振動子型ジャイロスコープを用いている。回転型ジャイロスコープに比べて振動子型ジャイロスコープは回転機構を用いないので小型にすることができる。従って、小型のセンサーにて回転角度の変動を検出することができる。そして、小型であることから、腕部８に配置する場所を設定し易くすることができる。
【００５７】
（４）本実施形態によれば、回転角度検出器６にロータリーエンコーダーを用いている。ロータリーエンコーダーは目盛りを配置した円板と目盛りの検出器を備えている。そして、目盛りの個数を多くすることにより、高い分解能を実現することができる。従って、高い分解能のロータリーエンコーダーを用いることにより精度良く回転角度検出器６の回転角度を検出することができる。
【００５８】
（５）本実施形態によれば、角速度センサー９は腕部８の一端に配置されている。そして、モーター５が腕部８を回転させるとき、腕部８及び減速機７が変形する。このとき、角速度センサー９は腕部８が変形する変位の影響を含んだ回転角度を検出することができる。
【００５９】
（第２の実施形態）
次に、ロボットシステムの一実施形態について図６〜図７を用いて説明する。本実施形態は第１の実施形態をロボットシステムの制御に応用した１例である。尚、第１の実施形態と同じ点については説明を省略する。
【００６０】
図６（ａ）は、ロボットシステムの構成を示す概略斜視図であり、図６（ｂ）は、ロボットの構成を示す模式断面図である。すなわち、本実施形態では、図６に示すように、ロボットシステム３０はワーク供給装置３１、ロボット３２、ワーク収納装置３３及び制御装置３４を備えている。
【００６１】
ワーク供給装置３１はワークとしての部品３５を供給する装置である。部品３５の形状や材質は特に限定されない、例えば、実施形態において部品３５は金属製の直方体となっている。
【００６２】
ワーク供給装置３１は部品整列装置３６と搬送装置３７とを備えている。部品整列装置３６は円錐状の皿部３６ａ及び皿部３６ａを支持する支持台３６ｂ等から構成されている。そして、皿部３６ａと支持台３６ｂの間には図示しない振動装置が配置されている。皿部３６ａの内側には螺旋状の段差が形成されている。段差は所定の幅の平坦部を有し、平坦部は部品３５が通過する通路になっている。平坦部は皿部３６ａの底から上部まで連続して形成されている。そして、振動装置が皿部３６ａを振動させるとき部品３５が平坦部に沿って移動するようになっている。平坦部の幅は部品３５が１個に限って通過可能な幅に形成されているので、部品３５が通路を通過することにより部品３５は１列に配列する。
【００６３】
搬送装置３７の上側にはベルトコンベア３７ａが配置されている。ベルトコンベア３７ａは１方向に長く延在して配置されている。この方向をＹ方向とする。そして水平方向においてＹ方向と直交する方向をＸ方向とし、鉛直方向をＺ方向とする。搬送装置３７は内部にステップモーター及びプーリーを備え、ベルトコンベア３７ａを移動及び停止することができる。ベルトコンベア３７ａの一端は部品整列装置３６の上部と接続されている。皿部３６ａの上部まで移動した部品３５はベルトコンベア３７ａの上に移動する。そして、部品３５はベルトコンベア３７ａにより順次図中右側へ移動し、所定の場所に停止するようになっている。従って、ベルトコンベア３７ａ上には部品３５が配列して配置される。
【００６４】
ロボット３２は平板状に形成された基台３８を備えている。基台３８上には支持台３９が配置されている。支持台３９の内部は空洞が形成され、この空洞は支持板４０により上下に分割されている。支持板４０の下側には第１モーター４１が配置され、第１モーター４１の下側には第１回転角度検出器４２が配置されている。第１回転角度検出器４２は第１モーター４１の回転角度を検出する。
【００６５】
支持板４０の上側には第１減速機４３が配置され、第１モーター４１の回転軸４１ａと接続されている。そして、第１減速機４３の上側には出力軸４３ａが配置されている。そして、第１モーター４１の回転軸４１ａの回転速度を減速した回転速度にて出力軸４３ａが回転する。第１減速機４３は各種の減速機を採用することができる。本実施形態では、例えば、ハーモニックドライブ（登録商標）を採用している。支持台３９の上面には孔部３９ａが形成され、孔部３９ａから出力軸４３ａが突出して配置されている。
【００６６】
出力軸４３ａと接続して略直方体状の可動部としての第１腕部４４が配置され、第１腕部４４は出力軸４３ａを中心に回転する。第１腕部４４において出力軸４３ａと反対側には第１角速度センサー４５が配置されている。従って、第１モーター４１が回転するとき、第１腕部４４が回転させられる。そして、第１腕部４４が回転する角度を第１回転角度検出器４２及び第１角速度センサー４５が検出可能になっている。
【００６７】
第１腕部４４上において第１角速度センサー４５の第１モーター４１側には第２減速機４６、第２モーター４７、第２回転角度検出器４８がこの順に重ねて配置されている。そして、第２減速機４６の出力軸４６ａが図中下方向に配置されている。第１腕部４４には第２減速機４６と対向する場所に孔部４４ａが形成され、孔部４４ａから出力軸４６ａが突出して配置されている。
【００６８】
第２モーター４７の回転軸には第２回転角度検出器４８が接続され、第２回転角度検出器４８は第２モーター４７の回転軸の回転角度を検出する。さらに、第２モーター４７の回転軸は第２減速機４６と接続されている。そして、第２モーター４７の回転軸の回転速度を減速した回転速度にて出力軸４６ａが回転する。
【００６９】
第１モーター４１及び第２モーター４７は電気信号によって回転方向を制御可能であればよく、各種のモーターを用いることができる。本実施形態では、例えば、直流モーターを採用している。第１回転角度検出器４２は第１モーター４１の駆動軸の回転角度を検出可能であれば良く、第２回転角度検出器４８は第２モーター４７の駆動軸の回転角度を検出可能であれば良い。第１回転角度検出器４２及び第２回転角度検出器４８には各種のロータリーエンコーダーを用いることができる。本実施形態では、例えば、光学式のロータリーエンコーダーを採用している。
【００７０】
出力軸４６ａと接続して略直方体状の可動部としての第２腕部４９が配置され、第２腕部４９は出力軸４６ａを中心に回転する。第２腕部４９において出力軸４６ａと反対側には第２角速度センサー５０が配置されている。第１角速度センサー４５及び第２角速度センサー５０の種類は特に限定されず、各種のジャイロスコープを用いることができる。本実施形態では、例えば、振動子型ジャイロスコープを採用している。
【００７１】
第２腕部４９上において第２角速度センサー５０の第２モーター４７側には回転装置５１が配置されている。そして、第２腕部４９には回転装置５１と対向する場所に孔部４９ａが形成され、孔部４９ａから回転装置５１の回転軸５１ａが突出して配置されている。第２腕部４９の下側において回転装置５１と対向する側には昇降装置５２が配置され、昇降装置５２は回転軸５１ａと接続されている。従って、回転装置５１は昇降装置５２を回転することができる。
【００７２】
昇降装置５２の下側には手部５３が配置されている。昇降装置５２は上下に移動する直動機構を有し、手部５３を昇降することができる。手部５３は略直方体状の２つの指部５３ａと直動機構とを有し、直動機構が２つの指部５３ａの間隔を変更することができる。そして、手部５３は指部５３ａの間に部品３５を挟んで保持することが可能になっている。
【００７３】
ロボット３２の図中右上にはワーク収納装置３３が配置されている。ワーク収納装置３３には複数の容器５６が配置されている。容器５６は外形が略直方体であり、深さが浅く形成されている。容器５６には仕切板が配置され、４つの部屋に分割されている。そして、各部屋に部品３５が配置される。
【００７４】
ワーク収納装置３３は内部に供給用昇降装置５７、除材用昇降装置５８とが並んで配置されている。供給用昇降装置５７及び除材用昇降装置５８の上側には容器５６が重ねて配置されている。そして、供給用昇降装置５７及び除材用昇降装置５８は上下方向に移動する直動機構を備え、容器５６を上昇及び下降することが可能になっている。供給用昇降装置５７の上側の容器５６は空になっている。そして、除材用昇降装置５８の上側の容器５６には部品３５が配置されている。
【００７５】
ワーク収納装置３３において図中左側の側面には直動機構を備えた押出装置５９が配置されている。押出装置５９は供給用昇降装置５７の上側に重ねて配置された容器５６の内最上段の容器５６を図中右下方向に押し出す。そして、押出装置５９は容器５６を供給用昇降装置５７の上側から除材用昇降装置５８の上側へ移動させる。
【００７６】
ロボット３２が供給用昇降装置５７の上側に位置する容器５６内に部品３５を移動させることにより、容器５６が部品３５で満たされる。そして、容器５６を押出装置５９が供給用昇降装置５７の上側から除材用昇降装置５８の上側へ移動させる。その後、除材用昇降装置５８が容器５６を下降させる。続いて、供給用昇降装置５７が空の容器５６を上昇させる。従って、ロボット３２が部品３５を空の容器５６内に移動させることができる。
【００７７】
昇降装置５２、手部５３、供給用昇降装置５７、除材用昇降装置５８、押出装置５９に配置された直動機構の種類は特に限定されない。例えば、直動機構にボールネジとパルスモーターとのを組み合わせたユニット、リニアモーター、エアーシリンダー等を用いることができる。本実施形態では、例えば、昇降装置５２、手部５３、供給用昇降装置５７、除材用昇降装置５８の直動機構にボールネジとパルスモーターとを組み合わせたユニットを採用した。そして、押出装置５９の直動機構には、例えば、エアーシリンダーを採用した。
【００７８】
ロボット３２の図中左側には制御装置３４が配置されている。制御装置３４は中央演算装置、記憶部、インターフェース、第１モーター４１及び第２モーター４７を駆動するモーター駆動回路を備えている。他にも、制御装置３４は、回転装置５１と昇降装置５２と手部５３とを駆動する駆動回路、入力装置、表示装置等を備えている。第１回転角度検出器４２、第１角速度センサー４５、第２回転角度検出器４８、第２角速度センサー５０はインターフェースを介して中央演算装置と接続されている。第１モーター４１及び第２モーター４７はモーター駆動回路と接続され、モーター駆動回路はインターフェースを介して中央演算装置と接続されている。さらに、ワーク供給装置３１及びワーク収納装置３３もインターフェースを介して中央演算装置と接続されている。
【００７９】
記憶部にはロボットシステム３０を制御するための動作手順を示したプログラムソフトや角度データ等が記憶されている。そして、中央演算装置は第１モーター４１及び第２モーター４７を所定の角度回転させて、第１回転角度検出器４２、第１角速度センサー４５、第２回転角度検出器４８、第２角速度センサー５０の出力を記憶部に記憶することができる。さらに、中央演算装置は第１回転角度検出器４２及び第１角速度センサー４５の出力を比較して較正するための演算を行う。他にも、中央演算装置は第２回転角度検出器４８及び第２角速度センサー５０の出力を比較して較正するための演算等を行う。
【００８０】
（ロボット制御方法）
次に、上述したロボットシステム３０においてロボット３２を制御する方法について図７及び図８にて説明する。尚、部品３５をワーク供給装置３１からワーク収納装置３３へロボット３２が移動させる１例を用いて説明する。図７は、ロボットの姿勢を説明するための模式図であり、図８は、部品の移動工程を示すフローチャートである。
【００８１】
図７（ａ）において、手部５３がベルトコンベア３７ａ上の部品３５を把持する予定の場所と対向する場所に位置している。このときのロボット３２の姿勢をロボット第１姿勢６０及びロボット第３姿勢６１とする。ロボット第１姿勢６０とロボット第３姿勢６１とは同じ姿勢である。
【００８２】
ロボット３２がロボット第１姿勢６０のときの第１角速度センサー４５の姿勢を第１センサー第１姿勢４５ａとし、第２角速度センサー５０の姿勢を第２センサー第１姿勢５０ａとする。そして、ロボット３２がロボット第３姿勢６１のときの第１角速度センサー４５の姿勢を第１センサー第３姿勢４５ｂとし、第２角速度センサー５０の姿勢を第２センサー第３姿勢５０ｂとする。第１角速度センサー４５及び第２角速度センサー５０の場所と姿勢とはロボット第１姿勢６０とロボット第３姿勢６１とで同じになっている。従って、第１センサー第１姿勢４５ａと第１センサー第３姿勢４５ｂとは同じ姿勢であり、第２センサー第１姿勢５０ａと第２センサー第３姿勢５０ｂとは同じ姿勢となっている。
【００８３】
図７（ｂ）において、手部５３がワーク収納装置３３上の容器５６と対向する場所に位置している。容器５６において左上の場所を第１の場所５６ａ、右上の場所を第２の場所５６ｂ、右下の場所を第３の場所５６ｃ、左下の場所を第４の場所５６ｄとする。そして、手部５３が部品３５を第１の場所５６ａに置くときに、手部５３が第１の場所５６ａと対向する場所に位置する。このときのロボット３２の姿勢をロボット第２姿勢６２とする。同様に、手部５３が第２の場所５６ｂ、第３の場所５６ｃ、第４の場所５６ｄと対向する場所に位置するときのロボット３２の姿勢をそれぞれ、ロボット第４姿勢、ロボット第５姿勢、ロボット第６姿勢とする。そして、ロボット３２の姿勢がロボット第２姿勢６２のときの第１角速度センサー４５の姿勢を第１センサー第２姿勢４５ｃとし、第２角速度センサー５０の姿勢を第２センサー第２姿勢５０ｃとする。
【００８４】
ロボット３２が部品３５をベルトコンベア３７ａから容器５６に移動させるとき、制御装置３４はロボット３２の姿勢をロボット第１姿勢６０からロボット第２姿勢６２へ変化させる。そして、ロボット３２がベルトコンベア３７ａ上の部品３５を把持するとき、制御装置３４はロボット３２の姿勢をロボット第２姿勢６２からロボット第３姿勢６１へ変化させる。次に、ロボット３２が第２の場所５６ｂに部品３５を置くときには、制御装置３４はロボット３２の姿勢をロボット第１姿勢６０からロボット第４姿勢へ変化させる。同様に、ロボット３２が第３の場所５６ｃに部品３５を置くときには、制御装置３４はロボット３２の姿勢をロボット第１姿勢６０からロボット第５姿勢へ変化させる。そして、ロボット３２が第４の場所５６ｄに部品３５を置くときには、制御装置３４はロボット３２の姿勢をロボット第１姿勢６０からロボット第６姿勢へ変化させる。
【００８５】
ロボット３２の姿勢がロボット第１姿勢６０からロボット第２姿勢６２を経てロボット第３姿勢６１に移行する。このとき、第１角速度センサー４５の姿勢は第１センサー第１姿勢４５ａから第１センサー第２姿勢４５ｃを経て第１センサー第３姿勢４５ｂに移行する。そして、第２角速度センサー５０の姿勢は第２センサー第１姿勢５０ａから第２センサー第２姿勢５０ｃを経て第２センサー第３姿勢５０ｂに移行する。
【００８６】
次に、図８に示すフローチャートを用いて、部品の移動工程におけるロボット３２の制御方法を説明する。ステップＳ２１は、第１較正工程に相当する。第１の実施形態におけるステップＳ１１の較正工程と同様の方法を用いて、第１角速度センサー４５及び第２角速度センサー５０を較正する。そして、第１角速度センサー４５及び第２角速度センサー５０のオフセット補正係数及び感度係数を演算する工程である。
【００８７】
次にステップＳ２２に移行する。ステップＳ２２は、移動工程に相当する。制御装置３４はロボット３２を駆動してロボット３２の姿勢をロボット第１姿勢６０にする。このとき、第１角速度センサー４５の姿勢は第１センサー第１姿勢４５ａとなり、第２角速度センサー５０の姿勢は第２センサー第１姿勢５０ａとなる。そして、手部５３が部品３５を把持する予定の場所と対向する場所に、手部５３を移動させる工程である。次にステップＳ２３に移行する。
【００８８】
ステップＳ２３は、ワーク供給工程に相当し、ワーク供給装置３１がベルトコンベア３７ａ上の所定の場所に部品３５を配置する。そして、手部５３と対応する場所に部品３５を配置する工程である。次にステップＳ２４に移行する。
【００８９】
ステップＳ２４は、ワーク把持工程に相当し、制御装置３４が手部５３に部品３５を把持させる工程である。制御装置３４は手部５３を駆動して、一対の指部５３ａの間隔を開く。そして、制御装置３４は回転装置５１を駆動して、一対の指部５３ａの間と対向する場所に部品３５が位置するように手部５３を回転させる。次に、制御装置３４は昇降装置５２を駆動して、手部５３を下降させる。続いて、制御装置３４は手部５３を駆動して、指部５３ａの間隔を狭くすることにより、手部５３に部品３５を把持させる。次に、制御装置３４は昇降装置５２を駆動して、手部５３を上昇させる。次にステップＳ２５、ステップＳ２６、ステップＳ２７、ステップＳ３０に移行する。ステップＳ２５とステップＳ２６、ステップＳ２７とは並行して行われる。
【００９０】
ステップＳ２５は、第１変更工程に相当し、制御装置３４がロボット３２の姿勢をロボット第１姿勢６０からロボット第２姿勢６２へ変更させる工程である。制御装置３４はロボット３２を駆動して手部５３をベルトコンベア３７ａと対向する場所から容器５６と対向する場所に移動させる。このとき、第１角速度センサー４５の姿勢は第１センサー第１姿勢４５ａから第１センサー第２姿勢４５ｃに移行し、第２角速度センサー５０の姿勢は第２センサー第１姿勢５０ａから第２センサー第２姿勢５０ｃに移行する。次にステップＳ２８に移行する。尚、第１の場所５６ａに部品３５が配置されているときには、ロボット第４姿勢に移行する。第１の場所５６ａ及び第２の場所５６ｂに部品３５が配置されているときには、ロボット第５姿勢に移行する。第１の場所５６ａ〜第３の場所５６ｃに部品３５が配置されているときには、ロボット第６姿勢に移行する。
【００９１】
ステップＳ２６は、第１検出工程に相当し、回転角度検出器の出力と角速度センサーの出力とを検出する工程である。第１角速度センサー４５の姿勢が第１センサー第１姿勢４５ａから第１センサー第２姿勢４５ｃに移行する間で、第１角速度センサー４５の出力を積分した第１出力積分を検出する。さらに、第２角速度センサー５０の姿勢が第２センサー第１姿勢５０ａから第２センサー第２姿勢５０ｃに移行する間で、第２角速度センサー５０の出力を積分した第１出力積分を検出する。さらに、第１角速度センサー４５及び第２角速度センサー５０の出力を積分する間の時間である第１経過時間を検出する。さらに、制御装置３４は第１回転角度検出器４２及び第２回転角度検出器４８の出力を検出して記憶する。この工程は第１の実施形態におけるステップＳ２の第１検出工程と略同様の工程となっている。次にステップＳ２８に移行する。
【００９２】
ステップＳ２７は、姿勢検出工程に相当し、第１腕部４４及び第２腕部４９の姿勢を検出する工程である。そして、第１角速度センサー４５及び第２角速度センサー５０における出力を積分する。次に、積分した値からオフセット補正係数と経過時間との乗算値を引き算した第２正味出力積分を演算する。そして、第２正味出力積分と感度係数とを乗算して第１角速度センサー４５及び第２角速度センサー５０の姿勢を検出する。また、第１角速度センサー４５及び第２角速度センサー５０の出力からオフセット補正量を減算した後、感度係数を乗算することにより、各センサーの角速度を算出する。次に、第１角速度センサー４５及び第２角速度センサー５０の姿勢の情報を用いて第１腕部４４及び第２腕部４９の姿勢を検出する。尚、第１角速度センサー４５及び第２角速度センサー５０の出力は第１腕部４４及び第２腕部４９の捩れ等による振動の影響を受けた出力となっている。第１腕部４４及び第２腕部４９の移動中においても随時角度及び角速度を検出する。この工程は第１の実施形態におけるステップＳ７の第３検出工程と略同様の工程となっている。次にステップＳ２８に移行する。
【００９３】
ステップＳ２８は、ワーク配置工程に相当し、部品３５を容器５６上に配置する工程である。制御装置３４は昇降装置５２を駆動して、手部５３を下降させる。続いて、制御装置３４は手部５３を駆動して、指部５３ａの間隔を広くする。そして、制御装置３４は部品３５を手部５３から離れさせて、部品３５を容器５６上に載置する。次に、制御装置３４は昇降装置５２を駆動して、手部５３を上昇させる。容器５６の４箇所総てに部品３５が載置されたときには、ワーク収納装置３３は容器５６を入れ替える。次にステップＳ２９に移行する。
【００９４】
ステップＳ２９は、第２変更工程に相当し、制御装置３４がロボット３２の姿勢をロボット第２姿勢６２からロボット第３姿勢６１へ変更させる工程である。制御装置３４はロボット３２を駆動して手部５３を容器５６と対向する場所からベルトコンベア３７ａと対向する場所に移動させる。このとき、第１角速度センサー４５の姿勢は第１センサー第２姿勢４５ｃから第１センサー第３姿勢４５ｂに移行し、第２角速度センサー５０の姿勢は第２センサー第２姿勢５０ｃから第２センサー第３姿勢５０ｂに移行する。次にステップＳ３１に移行する。
【００９５】
ステップＳ２４のワーク把持工程、ステップＳ２５の第１変更工程、ステップＳ２８のワーク配置工程、ステップＳ２９の第２変更工程を合わせてステップＳ４１の作業工程とする。作業工程は部品３５を移動する作業を行う工程となっている。
【００９６】
ステップＳ３０は、第２検出工程に相当し、回転角度検出器の出力と角速度センサーの出力とを検出する工程である。ステップＳ３０はステップＳ２５、ステップＳ２８、ステップＳ２９と並行して行われる。第１角速度センサー４５の姿勢が第１センサー第１姿勢４５ａから第１センサー第２姿勢４５ｃを経て第１センサー第３姿勢４５ｂに移行する間で、第１角速度センサー４５の出力を積分した第２出力積分を検出する。さらに、第２角速度センサー５０の姿勢が第２センサー第１姿勢５０ａから第２センサー第２姿勢５０ｃを経て第２センサー第３姿勢５０ｂに移行する間で、第２角速度センサー５０の出力を積分した第２出力積分を検出する。さらに、第１角速度センサー４５及び第２角速度センサー５０の出力を積分する間の時間とを検出する。さらに、制御装置３４は第１回転角度検出器４２及び第２回転角度検出器４８の出力を検出して記憶する。この工程は第１の実施形態におけるステップＳ４の第２検出工程と略同様の工程となっている。次にステップＳ３１に移行する。
【００９７】
ステップＳ３１は、係数算出工程に相当し、オフセット補正係数と感度係数とを演算する工程である。オフセット補正係数と感度係数との演算方法は第１の実施形態におけるステップＳ５の係数算出工程と略同様の方法にて演算する。オフセット補正係数と感度係数とを用いてステップＳ２７では第１腕部４４及び第２腕部４９の姿勢を検出する。次にステップＳ３２に移行する。
【００９８】
ステップＳ２５の第１変更工程、ステップＳ２６の第１検出工程、ステップＳ２９の第２変更工程、ステップＳ３０の第２検出工程を合わせてステップＳ４２の第２較正工程とする。第２較正工程では制御装置３４が、制御装置３４がオフセット補正係数と感度係数とを演算する工程となっている。ステップＳ４１の一部とステップＳ４２の第２較正工程とは並行して行われる。
【００９９】
ステップＳ３２は、終了判断工程に相当し、作業を継続するか終了するかを判断する工程である。予定した個数の部品３５を移動していないとき作業を継続する。次に、ステップＳ２３に移行する。予定した個数の部品３５を移動したとき終了する。以上の工程により部品の移動工程を終了する。
【０１００】
上述したように、本実施形態によれば、以下の効果を有する。
（１）本実施形態によれば、ステップＳ２１の第１較正工程とステップＳ３１の係数算出工程において、オフセット補正係数と感度係数とを演算している。そして、オフセット誤差のない角速度センサーの出力を用いて角速度センサーの感度係数を演算している。従って、精度良く第１角速度センサー４５および第２角速度センサー５０の姿勢を検出することができる為、精度良くロボット３２の姿勢を制御することができる。
【０１０１】
（２）本実施形態によれば、ステップＳ４２の第２較正工程とステップＳ４１の作業工程の一部とが並行して行われる。従って、第２較正工程と作業工程とを別々に行う方法に比べて生産性良く第１角速度センサー４５及び第２角速度センサー５０の較正を行うことができる。
【０１０２】
（３）本実施形態によれば、ロボット３２の姿勢はロボット第１姿勢６０からロボット第２姿勢６２を経てロボット第３姿勢６１へ移動している。そして、ロボット３２のロボット第１姿勢６０とロボット第３姿勢６１とが同じ姿勢に設定されている。ロボット３２の姿勢が同じ姿勢のとき、第１角速度センサー４５及び第２角速度センサー５０の位置と姿勢とは同じになる。従って、第１角速度センサー４５及び第２角速度センサー５０の姿勢を容易に同じ姿勢にすることができる。
【０１０３】
尚、本実施形態は上述した実施形態に限定されるものではなく、種々の変更や改良を加えることも可能である。変形例を以下に述べる。
（変形例１）
前記第１の実施形態では、ステップＳ２の第１検出工程とステップＳ４の第２検出工程とを並行して行なった。これに限定されず、ステップＳ２とステップＳ４とは別に行なっても良い。工程設計の都合に合わせても良い。同様に、前記第２の実施形態において、ステップＳ２６の第１検出工程とステップＳ３０の第２検出工程とは別に行なっても良い。工程設計の都合に合わせても良い。
【０１０４】
（変形例２）
前記第１の実施形態では、制御装置１０の記憶部内に動作手順に沿ったプログラムソフトを記憶し、プログラムにより角度検出装置１の制御を行なったが、これに限らず、電気回路にて構成される制御装置にて制御しても良い。周辺機器が手順通りに制御されれば良い。この内容は、前記第２の実施形態にも適用できる。制御装置３４は電気回路にて構成される制御装置にて制御しても良い。
【０１０５】
（変形例３）
前記第２の実施形態では、ロボット３２がロボット第１姿勢６０のとき、第１角速度センサー４５の姿勢を第１センサー第１姿勢４５ａとし、第２角速度センサー５０の姿勢を第２センサー第１姿勢５０ａとした。第１センサー第１姿勢４５ａ及び第２センサー第１姿勢５０ａはロボット３２が他の姿勢のときでも良い。手部５３が別の場所に位置するときに第１センサー第１姿勢４５ａ及び第２センサー第１姿勢５０ａを設定しても良い。この場合にも、第１センサー第１姿勢４５ａを第１センサー第３姿勢４５ｂと同じ姿勢に設定する。そして、第２センサー第１姿勢５０ａを第２センサー第３姿勢５０ｂと同じ姿勢に設定することにより前記第１の実施形態と同様の方法を用いることができる。
【０１０６】
（変形例４）
前記第２の実施形態では、ステップＳ２６の第１検出工程はロボット３２の姿勢がロボット第１姿勢６０からロボット第２姿勢６２に変更される間にて行なったが、ロボット第２姿勢６２以外の姿勢に移行する場合で行なっても良い。ロボット３２の姿勢がロボット第４姿勢、ロボット第５姿勢、ロボット第６姿勢の各姿勢に移行するときに行なっても良い。そして、較正する頻度を多くしても良い。較正する頻度が多い方が第１角速度センサー４５及び第２角速度センサー５０の測定精度を良くすることができる。
【０１０７】
（変形例５）
前記第２の実施形態では、ロボット３２がベルトコンベア３７ａから容器５６に部品３５を移動させた。これに限らず、ロボット３２が容器５６からベルトコンベア３７ａに部品３５を移動させるときに上述の較正方法を用いても良い。このとき、ロボット３２の姿勢がロボット第２姿勢６２のときに第１角速度センサー４５の姿勢を第１センサー第１姿勢４５ａ及び第１センサー第３姿勢４５ｂにする。そして、第２角速度センサー５０の姿勢を第２センサー第１姿勢５０ａ及び第２センサー第３姿勢５０ｂにする。ロボット３２の姿勢がロボット第１姿勢６０のときに第１角速度センサー４５の姿勢を第１センサー第２姿勢４５ｃにする。そして、第２角速度センサー５０の姿勢を第２センサー第２姿勢５０ｃにする。そして、容器５６が入れ替わるときにも、ロボット３２がロボット第２姿勢６２となるときにステップＳ４２の第２較正工程を行うようにしても良い。この場合にも前記第２の実施形態と同様に第１角速度センサー４５及び第２角速度センサー５０の較正を行うことができる。
【０１０８】
（変形例６）
前記第２の実施形態では、ロボット３２は第１腕部４４と第２腕部４９との２つの腕部を備えたが、腕部の数は２つに限定されない。腕部の数は１つでも良く、３つ以上でも良い。この場合にも、同様の方法を用いて角速度センサーの較正を行うことができる。
【０１０９】
（変形例７）
前記第１の実施形態では、ステップＳ１の第１変更工程とステップＳ３の第２変更工程において、腕部８が回転する間の回転角度検出器６の出力を制御装置１０は入力して記憶部に記憶した。制御装置１０は、腕部８が回転する間の総ての間において回転角度検出器６の出力を入力しなくとも良い。制御装置１０は、第１姿勢１１、第２姿勢１２、第３姿勢１３における回転角度検出器６の出力を入力して記憶部に記憶すれば良い。記憶部の記憶容量を減らすことができる。
【０１１０】
（変形例８）
前記第１の実施形態では、ステップＳ５の係数算出工程において、角速度センサー出力推移線１５を積分して角速度出力積分推移線１６を演算した後、正味出力積分推移線１９を算出した。角速度センサー出力推移線１５からオフセット出力１５ａを減算した後、積分の演算をすることにより正味出力積分推移線１９を算出しても良い。同様に正味出力積分推移線１９を算出することができる。この内容は前記第２の実施形態におけるステップＳ２１の第１較正工程及びステップＳ３１の係数算出工程においても適用することができる。
【０１１１】
（変形例９）
前記第２の実施形態では、ステップＳ４１の作業工程を行う毎にステップＳ４２の第２較正工程を行ったが、ステップＳ４２は必ずしも毎回行わなくとも良い。適宜設定した頻度にて行っても良い。第２較正工程を省略することにより、演算する時間が減る為、作業速度をあげることができる。
【０１１２】
（変形例１０）
ステップＳ２５の第１変更工程及びステップＳ２９の第２変更工程において第１腕部４４及び第２腕部４９の駆動制御を行うとき、第１回転角度検出器４２と第２回転角度検出器４８とに加えて第１角速度センサー４５及び第２角速度センサー５０を用いても良い。そして、駆動制御の演算にステップＳ３１にて算出したオフセット補正係数と感度係数を用いても良い。第１腕部４４及び第２腕部４９の駆動制御を行うとき、第１回転角度検出器４２と第２回転角度検出器４８とを用いて駆動制御する場合より位置精度良く制御することができる。もしくは、制振性良く制御することができる。
【０１１３】
（変形例１１）
前記第１の実施形態では、ステップＳ５の係数算出工程において、回転角度差１４ａを第１正味出力積分１９ａにて除算することにより感度係数を算出した。感度係数は他の方法にて算出しても良い。角度検出器出力推移線１４を時間にて微分することにより、角度微分推移線を算出する。次に、角速度センサー出力推移線１５からオフセット出力１５ａを減算することにより、正味角速度センサー出力推移線を算出する。続いて、角度微分推移線の各時刻の値を正味角速度センサー出力推移線の各時刻の値にて除算することにより各時刻の感度係数を算出する。このとき、角度微分推移線及び正味角速度センサー出力推移線の値が零でないときの値を用いるのが好ましい。感度係数の精度を向上することができる。次に、各時刻の感度係数の平均を算出して感度係数とする。つまり、回転角度検出器６の出力から算出した回転角速度と角速度センサー９の出力とを用いて感度係数を算出しても良い。この場合にも、同様に感度係数を算出することができる。この内容は前記第２の実施形態にも適用することができる。
【０１１４】
（変形例１２）
前記第１の実施形態では、ステップＳ７の第３検出工程において、正味出力積分推移線２６に感度係数を乗算してセンサー角度推移線２７を算出した。センサー角度推移線２７は他の方法にて算出しても良い。角速度センサー出力推移線２３からオフセット出力２３ａを除算した後、除算結果に感度係数を乗算して角速度推移線を算出する。次に、この角速度推移線を積分することにより、センサー角度推移線２７しても良い。この場合にも、同様にセンサー角度推移線２７を算出することができる。この内容は前記第２の実施形態にも適用することができる。
【符号の説明】
【０１１５】
６…回転角度検出器、８…センサー支持部としての腕部、９…角速度センサー、１１…第１姿勢、１２…第２姿勢、１３…第３姿勢、１６ａ…第１出力積分、１６ｂ…第２出力積分、１７…第１経過時間、１８…第２経過時間、１９ａ…第１正味出力積分、２４ｄ…第２正味出力積分、３２…ロボット、４４…可動部としての第１腕部、４５…第１角速度センサー、４５ａ…第１センサー第１姿勢、４５ｂ…第１センサー第３姿勢、４５ｃ…第１センサー第２姿勢、４９ａ…可動部としての孔部、５０…第２角速度センサー、５０ａ…第２センサー第１姿勢、５０ｂ…第２センサー第３姿勢、５０ｃ…第２センサー第２姿勢。
【技術分野】
【０００１】
本発明は、角速度または角度の検出方法及びロボットの制御方法にかかわり、特に角速度センサーを用いて角速度または角度を検出する方法に関するものである。
【背景技術】
【０００２】
多軸ロボットはアーム（以後、腕部と称す）を複数備えている。そして、腕部を回転させるサーボモーター等のアクチュエーターに角度センサーを配置して腕部の姿勢を検出している。そして、腕部を移動させるときに腕部が振動するので、腕部の振動を少なくする駆動方法が求められている。そして、腕部の振動を少なくする方法が特許文献１に開示されている。それによると、腕部に角速度センサーを配置し、アクチュエーターの角度センサーと腕部の角速度センサーとを用いて、腕部の姿勢制御を行っている。
【０００３】
角速度センサーの較正方法が特許文献２に開示されている。これによると、加速度センサーを用いて重力方向を検出する。次に、角速度センサーを用いてロボットの姿勢を算出し、重力方向との差を検出している。そして、角速度センサーの出力のドリフト現象による誤差を較正していた。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特許第３８８３５４４号公報
【特許文献２】特開２００４−２６４０６０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
角速度センサーにはドリフト現象による誤差に加え、感度が変動することによる誤差がある。そこで、ドリフト現象による誤差と感度の変動による誤差を分離して較正する方法が望まれていた。
【課題を解決するための手段】
【０００６】
本発明は、上述の課題の少なくとも一部を解決するためになされたものであり、以下の形態または適用例として実現することが可能である。
【０００７】
［適用例１］
本適用例にかかる角速度または角度の検出方法は、角速度センサーを用いた角速度または角度の検出方法であって、前記角速度センサーの姿勢を第１姿勢から第２姿勢へ変更する第１変更工程と、前記第１姿勢から前記第２姿勢へ変更する間の前記角速度センサーの出力を積分した第１出力積分と、前記第１姿勢から前記第２姿勢へ変更するのに要した時間である第１経過時間と、を検出する第１検出工程と、前記角速度センサーの姿勢を前記第２姿勢から前記第１姿勢と同じ姿勢である第３姿勢へ変更する第２変更工程と、前記第１姿勢から前記第２姿勢を経て前記第３姿勢へ変更する間の前記角速度センサーの出力を積分した第２出力積分と、前記第１姿勢から前記第２姿勢を経て前記第３姿勢へ変更するのに要した時間である第２経過時間と、を検出する第２検出工程と、前記角速度センサーの角速度が零のときの出力をオフセット補正係数とし、前記角速度センサーの角速度変化量と前記角速度センサーの出力の関係を示す係数を感度係数とするとき、前記第２出力積分を前記第２経過時間にて除算して前記オフセット補正係数を演算し、前記第１出力積分から前記オフセット補正係数と前記第１経過時間との乗算値を引き算した第１正味出力積分を演算し、前記第１姿勢と前記第２姿勢との回転角度の差を前記第１正味出力積分にて除算して感度係数を演算する係数算出工程と、前記角速度センサーが第３経過時間の間に姿勢を変更するとき、変更する間における前記角速度センサーの出力と前記オフセット補正係数と感度係数とを用いて前記角速度センサーの角速度または姿勢を検出する姿勢検出工程と、を有することを特徴とする。
【０００８】
この角速度または角度の検出方法によれば、第１姿勢と第３姿勢とにおいて角速度センサーを同じ姿勢にしている。そして、第２経過時間の間に変化した角速度センサーの出力の差を第２経過時間にて除算することにより、オフセット補正係数を算出している。
【０００９】
第１姿勢から第２姿勢へ変更する間のオフセットによる誤差はオフセット補正係数と第１経過時間とを乗算して算出される。そして、角速度センサーの出力を積分した値からオフセットによる誤差を引き算して第１正味出力積分を算出することによりオフセットによる誤差を除いている。次に、第１姿勢と第２姿勢との回転角度の差を第１正味出力積分にて除算して感度係数を演算している。
【００１０】
従って、角速度センサーの出力のうちオフセットによる誤差を除いている。そして、オフセット誤差のない角速度センサーの出力を用いて角速度センサーの感度係数を演算している。その結果、精度良く角速度センサーの角速度及び姿勢を検出することができる。
【００１１】
［適用例２］
上記適用例にかかる角速度または角度の検出方法において、前記角速度センサーはセンサー支持部に配置され、前記センサー支持部は、回転軸と前記センサー支持部の回転角度を検出する回転角度検出器とを有し、前記角速度センサーの姿勢は前記回転角度検出器を用いて検出することを特徴とする。
【００１２】
この角速度または角度の検出方法によれば、角速度センサーが静止しているときの角速度センサーの回転角度は回転角度検出器を用いて所望のときに検出することができる。
【００１３】
［適用例３］
上記適用例にかかる角速度または角度の検出方法において、前記角速度センサーは振動子型ジャイロスコープであることを特徴とする。
【００１４】
この角速度または角度の検出方法によれば、角速度センサーに振動子型ジャイロスコープを用いている。回転型ジャイロスコープに比べて振動子型ジャイロスコープは回転機構を用いないので小型にすることができる。従って、小型のセンサーにて回転角度の変動を検出することができる。
【００１５】
［適用例４］
上記適用例にかかる角速度または角度の検出方法において、前記回転角度検出器はロータリーエンコーダーを用いて回転角度を検出することを特徴とする。
【００１６】
この角速度または角度の検出方法によれば、回転角度検出器にロータリーエンコーダーを用いている。ロータリーエンコーダーは目盛りを配置した円板と目盛りの検出器を備えている。そして、目盛りの個数を多くすることにより、高い分解能を実現することができる。従って、高い分解能のロータリーエンコーダーを用いることにより精度良く角速度センサーの回転角度を検出することができる。
【００１７】
［適用例５］
本適用例にかかるロボットの制御方法は、角速度センサーを用いて可動部の姿勢を検出するロボットの制御方法であって、前記角速度センサーを較正する較正工程と、前記可動部の姿勢を検出する姿勢検出工程と、を有し、前記較正工程は、前記角速度センサーの姿勢を第１姿勢から第２姿勢へ変更する第１変更工程と、前記第１姿勢から前記第２姿勢へ変更する間の前記角速度センサーの出力を積分した第１出力積分と、前記第１姿勢から前記第２姿勢へ変更するのに要した時間である第１経過時間と、を検出する第１検出工程と、前記角速度センサーの姿勢を前記第２姿勢から前記第１姿勢と同じ姿勢である第３姿勢へ変更する第２変更工程と、前記第１姿勢から前記第２姿勢を経て前記第３姿勢へ変更する間の前記角速度センサーの出力を積分した第２出力積分と、前記第１姿勢から前記第２姿勢を経て前記第３姿勢へ変更するのに要した時間である第２経過時間と、を検出する第２検出工程と、時間の経過にともなって変化する前記角速度センサーの前記出力の変化率をオフセット補正係数とし、前記角速度センサーの角度変化量と前記角速度センサーの出力の関係を示す係数を感度係数とするとき、前記第２出力積分を前記第２経過時間にて除算して前記オフセット補正係数を演算し、前記第１出力積分からオフセット補正係数と前記第１経過時間との乗算値を引き算した第１正味出力積分を演算し、前記第１姿勢と前記第２姿勢との回転角度の差を前記第１正味出力積分にて除算して感度係数を演算する係数算出工程と、を有し、前記姿勢検出工程は、前記角速度センサーが第３経過時間の間に姿勢を変更するとき、変更する間における前記角速度センサーの出力と前記オフセット補正係数と感度係数とを用いて前記角速度センサーの角速度または姿勢を検出する姿勢検出工程を有し、検出した前記角速度センサーの角速度の情報を用いて前記可動部の角速度検出、または検出した前記角速度センサーの姿勢の情報を用いて前記可動部の姿勢検出をすることを特徴とする。
【００１８】
このロボットの制御方法によれば、較正工程において、オフセット補正係数と角速度センサーの感度係数とを演算している。そして、オフセット誤差のない角速度センサーの出力を用いて角速度センサーの感度係数を演算している。従って、精度良く角速度センサーの姿勢を検出することができる為、精度良くロボットの姿勢を制御することができる。
【００１９】
［適用例６］
上記適用例にかかるロボットの制御方法において、前記ロボットが作業を行う作業工程を有し、前記較正工程は前記作業工程と並行して行われることを特徴とする。
【００２０】
このロボットの制御方法によれば、較正工程と作業工程とが並行して行われる。従って、較正工程と作業工程とを別々に行う方法に比べて生産性良く角速度センサーの較正を行うことができる。
【００２１】
［適用例７］
上記適用例にかかるロボットの制御方法において、前記第１姿勢と前記第３姿勢とにおいて前記角速度センサーは同じ場所に位置することを特徴とする。
【００２２】
このロボットの制御方法によれば、第１姿勢と第３姿勢とにおいて角速度センサーの姿勢が同じ姿勢のとき角速度センサーが同じ場所に位置している。角速度センサーが同じ場所に位置させるときにロボットの姿勢を同じ姿勢となるように駆動することにより角速度センサーの姿勢を同じにすることができる。従って容易に角速度センサーの姿勢を同じ姿勢にすることができる。
【図面の簡単な説明】
【００２３】
【図１】第１の実施形態にかかわり、（ａ）は、角度検出装置の構成を示す概略斜視図、（ｂ）は、角度検出装置の構成を示す模式断面図。
【図２】腕部の角度を検出する工程を示すフローチャート。
【図３】腕部の角度を検出する方法を説明するための模式図。
【図４】回転角度検出器及び角速度センサーの出力と演算結果の推移を示すタイムチャート。
【図５】（ａ）は、腕部の角度を検出する方法を説明するための模式図であり、図（ｂ）は、角速度センサーの出力と演算結果の推移を示すタイムチャート。
【図６】第２の実施形態にかかわり、（ａ）は、ロボットシステムの構成を示す概略斜視図、（ｂ）は、ロボットの構成を示す模式断面図。
【図７】ロボットの姿勢を説明するための模式図。
【図８】部品の移動工程を示すフローチャート。
【発明を実施するための形態】
【００２４】
以下、実施形態について図面に従って説明する。
尚、各図面における各部材は、各図面上で認識可能な程度の大きさとするため、各部材毎に縮尺を異ならせて図示している。
（第１の実施形態）
本実施形態における角度検出装置とこの角度検出装置を用いた特徴的な角度検出方法について図１〜図５に従って説明する。
【００２５】
図１（ａ）は、角度検出装置の構成を示す概略斜視図であり、図１（ｂ）は、角度検出装置の構成を示す模式断面図である。図１に示すように、角度検出装置１は平板状に形成された基台２を備えている。基台２の水平面上の１方向をＸ方向とする。そして、重力方向と逆の方向をＺ方向とし、Ｘ方向及びＺ方向と直交する方向をＹ方向とする。
【００２６】
基台２上には支持台３が配置されている。支持台３の内部には空洞が形成され、この空洞は支持板４により上下に分割されている。支持板４の下側にはモーター５が配置され、モーター５の下側には回転角度検出器６が配置されている。モーター５は電気信号によって回転方向を制御可能であればよく、直流モーター、パルスモーター、超音波モーター等のモーターを用いることができる。本実施形態では、例えば、直流モーターを採用している。回転角度検出器６は、モーター５の駆動軸の回転角度を検出可能であれば良く、光学式エンコーダー、磁気式エンコーダー等のロータリーエンコーダーを用いることができる。本実施形態では、例えば、光学式のロータリーエンコーダーを採用している。
【００２７】
支持板４の上側には減速機７が配置され、モーター５の回転軸５ａと接続されている。そして、減速機７の上側には出力軸７ａが配置されている。そして、モーター５の回転軸５ａの回転速度を減速した回転速度にて出力軸７ａが回転する。減速機７は歯車減速機、ハーモニックドライブ（登録商標）、ローラドライブ（登録商標）等を採用することができる。本実施形態では、例えば、ハーモニックドライブ（登録商標）を採用している。支持台３の上面には孔部３ａが形成され、孔部３ａから出力軸７ａが突出して配置されている。
【００２８】
出力軸７ａと接続して略直方体状のセンサー支持部としての腕部８が配置され、腕部８は出力軸７ａを中心に回転する。腕部８において出力軸７ａと反対側には角速度センサー９が配置されている。角速度センサー９の種類は特に限定されず、回転型ジャイロスコープ、振動型ジャイロスコープ、ガス型ジャイロスコープ、リングレーザジャイロ等のジャイロスコープを用いることができる。本実施形態では、例えば、振動型ジャイロスコープに属する振動子型ジャイロスコープを採用している。
【００２９】
モーター５が回転するとき、出力軸７ａが回転することにより、腕部８が回転させられる。角速度センサー９は腕部８と共に回転するので、角速度センサー９は腕部８の回転速度を検出する。そして、角速度センサー９の回転速度を所定の間で積分することにより腕部８が回転する角度を検出することができる。また、モーター５の回転角度は回転角度検出器６を用いて検出できるので、回転角度検出器６を用いて腕部８の回転角度を検出することができる。そして、回転角度検出器６を用いて検出した腕部８の回転角度と角速度センサー９を用いて検出した腕部８の回転角度とを比較することにより、角速度センサー９の較正を行うことができる。
【００３０】
基台２の図中右側には制御装置１０が配置されている。制御装置１０には中央演算装置、記憶部、インターフェース、モーター５を駆動するモーター駆動回路、入力装置、表示装置等を備えている。回転角度検出器６及び角速度センサー９はインターフェースを介して中央演算装置と接続されている。モーター５はモーター駆動回路と接続され、モーター駆動回路はインターフェースを介して中央演算装置と接続されている。記憶部には角度検出装置１を制御する動作手順を示したプログラムソフトや角度データが記憶されている。そして、中央演算装置はモーター５を所定の角度回転させて、回転角度検出器６及び角速度センサー９の出力を記憶部に記憶することができる。さらに、中央演算装置は回転角度検出器６及び角速度センサー９の出力を比較して較正するための演算を行う。
【００３１】
（角度検出方法）
次に、上述した角度検出装置１を用いて、腕部８の角度を検出する方法について図２〜図５にて説明する。図２は、腕部の角度を検出する工程を示すフローチャートである。図３〜図５は、腕部の角度を検出する方法を説明するための模式図及びタイムチャートである。
【００３２】
図２に示すフローチャートにおいて、ステップＳ１とステップＳ２とは並行して行われる。ステップＳ１は、第１変更工程に相当し、腕部を回転して角速度センサーの姿勢を第１姿勢から第２姿勢へと変更する工程である。次にステップＳ３に移行する。ステップＳ２は、第１検出工程に相当する。回転角度検出器の出力と角速度センサーの出力とを検出する。そして、角速度センサーの姿勢が第１姿勢から第２姿勢へと変更する間の角速度センサーの出力を積分した第１出力積分と、角速度センサーの出力を積分する間の時間とを検出する工程である。次にステップＳ３に移行する。ステップＳ３は、第２変更工程に相当し、腕部を回転して角速度センサーの姿勢を第２姿勢から第１姿勢と同じ姿勢である第３の姿勢へと変更する工程である。次にステップＳ５に移行する。ステップＳ４はステップＳ１及びステップＳ３と並行して行なわれる。ステップＳ４は、第２検出工程に相当する。回転角度検出器の出力と角速度センサーの出力とを検出する。そして、角速度センサーの姿勢が第１姿勢から第２姿勢を経て第３の姿勢へと変更する間の角速度センサーの出力を積分した第２出力積分と、角速度センサーの出力を積分する間の時間とを検出する工程である。次にステップＳ５に移行する。ステップＳ５は、係数算出工程に相当する。時間の経過にともなって変化する角速度センサーの出力の積分値の変化率であるオフセット補正係数を演算する。さらに、角速度センサーの角速度変化量と角速度センサーの出力の関係を示す感度係数を演算する。ステップＳ１〜ステップＳ５は、ステップＳ１１の較正工程に相当し、角速度センサーの係数を較正する工程である。次にステップＳ６及びステップＳ７に移行する。
【００３３】
ステップＳ６とステップＳ７とは並行して行われる。ステップＳ６は、第３変更工程に相当し、腕部を回転して角速度センサーの姿勢を変更する工程である。ステップＳ７は、第３検出工程に相当し、角速度センサーの出力を用いて角速度センサーの姿勢を検出することにより腕部の角度を検出する工程である。ステップＳ６及びステップＳ７は、ステップＳ１２の姿勢検出工程に相当し、姿勢が変化する角速度センサーの角速度を検出する。さらに、角速度センサー及び腕部の姿勢を検出する工程である。以上により、腕部の角度を検出する方法を終了する。
【００３４】
次に、図３〜図５を用いて、図２に示したステップと対応させて、腕部の角度を検出する方法を詳細に説明する。図３（ａ）はステップＳ１の第１変更工程とステップＳ２の第１検出工程とに対応する図である。図３（ａ）において、支持台３のＹ方向に角速度センサー９が位置するように制御装置１０は腕部８を配置する。このときの角速度センサー９の姿勢を第１姿勢１１とする。制御装置１０はモーター５を駆動して腕部８を時計回りに回転させる。その結果、図３（ｂ）に示すように、支持台３のＸ方向に角速度センサー９が位置するように腕部８が配置される。角速度センサー９のこの姿勢を第２姿勢１２とする。角速度センサー９が第１姿勢１１から第２姿勢１２になる間の角速度センサー９の出力を制御装置１０は入力して積分する。そして、積分した結果の推移を記憶部に記憶する。さらに、腕部８が回転する間の回転角度検出器６の出力を制御装置１０は入力して記憶部に記憶する。さらに、腕部８が回転する間の時間の経過を制御装置１０は記憶部に記憶する。
【００３５】
ステップＳ３の第２変更工程において、制御装置１０はモーター５を駆動して腕部８を反時計回りに回転させる。その結果、図３（ｃ）に示すように角速度センサー９の姿勢が第３姿勢１３になる。この第３姿勢１３は第１姿勢１１と同じ姿勢となっている。ステップＳ４の第２検出工程では、角速度センサー９の姿勢が第１姿勢１１から第２姿勢１２を経て第３姿勢１３になる間の角速度センサー９の出力を制御装置１０が入力して積分する。そして、積分した結果の推移を記憶部に記憶する。さらに、腕部８が回転する間の回転角度検出器６の出力を制御装置１０は入力して記憶部に記憶する。さらに、腕部８が回転する間の時間の経過を制御装置１０は記憶部に記憶する。
【００３６】
図４は、ステップＳ１〜ステップＳ４とステップＳ５の係数算出工程に対応する図であり、回転角度検出器及び角速度センサーの出力と演算結果の推移を示すタイムチャートである。図４において、横軸は時間の経過を示し、図中左から右へ時間が推移する。縦軸には、４つの要素が配置されている。図中上側から順に角度、角速度、角度、角度が配置されている。縦軸の角度は図中下側より上側が時計回りの角度が大きくなっている。縦軸の角速度は図中下側より上側が時計回り方向の角速度が大きくなっている。
【００３７】
上段の角度検出器出力推移線１４は回転角度検出器６の出力を示している。縦軸の角度は横軸と交差する場所が零点であり、腕部８がＹ方向に延在するときの角度を零とする。そして、角度検出器出力推移線１４が横軸の上側では腕部８がＹ方向から時計回りの方向に回転した場所に位置することを示している。
【００３８】
ステップＳ１の第１変更工程において、まず、角速度センサー９の姿勢は第１姿勢１１となっており、角度検出器出力推移線１４は零となっている。次に、腕部８が時計回りに回転することにより角速度センサー９の姿勢が第１姿勢１１から第２姿勢１２に移行する。従って、角度検出器出力推移線１４は上側へ推移する。このときの角度検出器出力推移線１４の値を回転角度差１４ａとする。回転角度差１４ａは第１姿勢１１と第２姿勢１２とにおける回転角度検出器６の回転角度差を示している。
【００３９】
ステップＳ３の第２変更工程において腕部８が反時計回りに回転することにより角速度センサー９が第２姿勢１２から第３姿勢１３に移行する。従って、角度検出器出力推移線１４は下側へ推移する。そして、第３姿勢１３は第１姿勢１１と同じであり、腕部８がＹ方向に延在するので角度検出器出力推移線１４が零となる。
【００４０】
上から２段目の角速度センサー出力推移線１５は角速度センサー９の出力を示している。縦軸の角速度は横軸と交差する場所が零点である。腕部８が静止しているとき、角速度センサー９の出力は所定の値となっており、この値をオフセット値と称す。オフセット値は角速度センサー９の温度等の環境により変動する。ステップＳ１の第１変更工程においてモーター５が腕部８を回転させる前では角速度センサー９が静止しているので、角速度センサー出力推移線１５はオフセット出力１５ａとなっている。
【００４１】
ステップＳ１の第１変更工程において、腕部８が時計回りに回転することにより、角速度センサー９が時計回りに回転する。そして、回転速度と比例した値が角速度センサー９から出力される。角速度センサー９が時計回りに回転するとき角速度センサー出力推移線１５はオフセット出力１５ａの上側を推移する。腕部８の回転速度と対応して角速度センサー９が角速度値を出力する。そして、角速度センサー出力推移線１５は腕部８の回転速度と対応した形状になる。ステップＳ３の第２変更工程では腕部８が反時計回りに回転する。そして、角速度センサー９が反時計回りに回転するとき角速度センサー出力推移線１５はオフセット出力１５ａの下側を推移し、腕部８の回転速度と対応した形状になる。
【００４２】
上から３段目の角速度出力積分推移線１６は角速度センサー出力推移線１５を時間軸において積分した値を示した線である。角速度出力積分推移線１６はステップＳ２の第１検出工程及びステップＳ４の第２検出工程にて演算される。角度検出装置１が第１姿勢１１から第２姿勢１２に変化する間に角速度出力積分推移線１６が変化した量を第１出力積分１６ａとする。そして、角度検出装置１が第１姿勢１１から第２姿勢１２に変化する間の時間を第１経過時間１７とする。ステップＳ２では第１出力積分１６ａ及び第１経過時間１７が制御装置１０の記憶部に記憶される。尚、第１出力積分１６ａ及び第１経過時間１７は角速度センサー９の出力の振動が安定したときの値を用いるのが好ましい。このとき、腕部８が振動することの影響を少なくすることができる為、精度良く較正することができる。
【００４３】
角度検出装置１が第１姿勢１１から第３姿勢１３に変化する間に角速度出力積分推移線１６が変化した量を第２出力積分１６ｂとする。そして、角度検出装置１が第１姿勢１１から第３姿勢１３に変化する間の時間を第２経過時間１８とする。ステップＳ４では第２出力積分１６ｂ及び第２経過時間１８が制御装置１０の記憶部に記憶される。
【００４４】
ステップＳ５の係数算出工程において、角速度センサー９の角速度が零のときの出力であるオフセット補正係数を演算する。オフセット補正係数は第２出力積分１６ｂを第２経過時間１８にて除算して算出する。従って、オフセット補正係数は第２経過時間１８におけるオフセット出力１５ａの平均値に相当する。オフセット線１６ｃはオフセット補正係数による角度の推移を示す線である。オフセット線１６ｃは第１姿勢１１と第３姿勢１３とにおいて角速度出力積分推移線１６と一致する直線となっている。
【００４５】
上から４段目の正味出力積分推移線１９は、角速度出力積分推移線１６の縦軸の値からオフセット線１６ｃの示す縦軸の値を引いた曲線である。つまり、正味出力積分推移線１９は、角速度出力積分推移線１６からオフセットによる角度誤差を引いた値が示す線となっている。角速度センサー９が第２姿勢１２のときにおける正味出力積分推移線１９の値を第１正味出力積分１９ａとする。第１正味出力積分１９ａは第１出力積分１６ａからオフセット補正係数と第１経過時間１７との乗算値を引き算した値となる。次に、回転角度差１４ａを第１正味出力積分１９ａにて除算した値を感度係数とする。感度係数は回転角度検出器６の出力が示す角速度と角速度センサー９の出力が示す角速度との関係を示す係数である。感度係数は回転角度検出器６の出力が示す角度と角速度センサー９の出力の積分値が示す角度とから算出することができる。
【００４６】
図５（ａ）はステップＳ６の第３変更工程に対応する図である。図５（ａ）に示すように、ステップＳ６において、制御装置１０はモーター５を駆動して腕部８を時計回りに回転させる。その結果、支持台３のＸ方向に対して斜めの方向に角速度センサー９が位置するように腕部８が配置される。このときの、角速度センサー９の姿勢を第４姿勢２２とする。従って、ステップＳ６では角速度センサー９の姿勢が第３姿勢１３から第４姿勢２２へと変更される。
【００４７】
図５（ｂ）はステップＳ７の第３検出工程に対応する図であり、角速度センサーの出力と演算結果の推移を示すタイムチャートである。図５（ｂ）において、横軸は時間の経過を示し、図中左から右へ時間が推移する。縦軸には、４つの要素が配置されている。図中上側から順に、角速度、角度、角度、角度が配置されている。縦軸の角度は図中下側より上側が時計回りの角度が大きくなっている。縦軸の角速度は図中下側より上側が時計回り方向の角速度が大きくなっている。
【００４８】
上段の角速度センサー出力推移線２３は角速度センサー９の出力を示している。縦軸の角速度は横軸と交差する場所が零点である。角度検出装置１が第３姿勢１３のとき腕部８は停止しており、角速度センサー９の出力はオフセット出力２３ａとなっている。そして、モーター５は加速して減速することにより、角度検出装置１の姿勢を第４姿勢２２に変更する。従って、角速度センサー出力推移線２３は山形に推移する。
【００４９】
上から２段目の角速度出力積分推移線２４は角速度センサー出力推移線２３を時間軸において積分した値を示した線である。角速度出力積分推移線２４はステップＳ７の第３検出工程にて演算される。角度検出装置１が第３姿勢１３から第４姿勢２２に変化する間に角速度出力積分推移線２４が変化した量を第３出力積分２４ａとする。そして、角速度センサー９が第３姿勢１３から第４姿勢２２に変化する間の時間を第３経過時間２５とする。ステップＳ７では第３出力積分２４ａ及び第３経過時間２５が制御装置１０の記憶部に記憶される。
【００５０】
オフセット線２４ｂはオフセット補正係数による角度の推移を示す線である。オフセット補正係数と第３経過時間２５とを乗算した値をオフセット補正量２４ｃとする。オフセット補正量２４ｃは第３経過時間２５に対応するオフセットによる誤差である。そして、第３出力積分２４ａからオフセット補正量２４ｃを引き算した値を第２正味出力積分２４ｄとする。第２正味出力積分２４ｄは角速度センサー９からオフセットによる誤差を除いた値である。
【００５１】
上から３段目の正味出力積分推移線２６は角速度出力積分推移線２４の縦軸の値からオフセット線２４ｂの示す縦軸の値を引いた曲線である。つまり、正味出力積分推移線２６は、角速度出力積分推移線２４からオフセットによる誤差を除いた線となっている。そして、角速度センサー９が第４姿勢２２のとき、第３経過時間２５の値は第２正味出力積分２４ｄとなる。
【００５２】
上から４段目のセンサー角度推移線２７は、正味出力積分推移線２６の縦軸の値に感度係数を乗算した値からなる曲線である。そして、センサー角度推移線２７は角速度センサー９の出力が示す角速度センサー９の回転角度の推移を示している。第４姿勢角度２７ａは、角度検出装置１が第４姿勢２２のときに角速度センサー９が示す角速度センサー９の角度である。そして、第４姿勢角度２７ａは第４姿勢２２におけるセンサー角度推移線２７の値となっている。
【００５３】
つまり、第３出力積分２４ａからオフセット補正量２４ｃを引き算して第２正味出力積分２４ｄを演算する。次に、第２正味出力積分２４ｄと感度係数とを乗算して第４姿勢角度２７ａを算出する。さらに、角速度センサー出力推移線２３から角速度を算出する。つまり、角速度センサー出力推移線２３からオフセット出力２３ａを減算する。次に、減算した値に感度係数を乗算することにより、角速度を算出する。以上の工程により腕部の角度を検出する工程を終了する。
【００５４】
上述したように、本実施形態によれば、以下の効果を有する。
（１）本実施形態によれば、ステップＳ５の係数算出工程において角速度センサー９の出力のうちオフセット出力１５ａによる誤差を除いている。そして、オフセット誤差のない角速度センサー９の出力を用いて角速度センサー９の感度係数を演算している。従って、精度良く角速度センサー９の姿勢を検出することができる。
【００５５】
（２）本実施形態によれば、角度検出装置１は回転角度検出器６を備えている。そして、回転角度検出器６が検出する角度と角速度センサー９の角度とは相関関係があるように角速度センサー９と回転角度検出器６とが配置されている。従って、角速度センサー９の回転角度は回転角度検出器６を用いて所望のときに検出することができる。そして、角速度センサー９が所望の場所に静止して位置するときに角速度センサー９を較正することができる。
【００５６】
（３）本実施形態によれば、角速度センサー９に振動子型ジャイロスコープを用いている。回転型ジャイロスコープに比べて振動子型ジャイロスコープは回転機構を用いないので小型にすることができる。従って、小型のセンサーにて回転角度の変動を検出することができる。そして、小型であることから、腕部８に配置する場所を設定し易くすることができる。
【００５７】
（４）本実施形態によれば、回転角度検出器６にロータリーエンコーダーを用いている。ロータリーエンコーダーは目盛りを配置した円板と目盛りの検出器を備えている。そして、目盛りの個数を多くすることにより、高い分解能を実現することができる。従って、高い分解能のロータリーエンコーダーを用いることにより精度良く回転角度検出器６の回転角度を検出することができる。
【００５８】
（５）本実施形態によれば、角速度センサー９は腕部８の一端に配置されている。そして、モーター５が腕部８を回転させるとき、腕部８及び減速機７が変形する。このとき、角速度センサー９は腕部８が変形する変位の影響を含んだ回転角度を検出することができる。
【００５９】
（第２の実施形態）
次に、ロボットシステムの一実施形態について図６〜図７を用いて説明する。本実施形態は第１の実施形態をロボットシステムの制御に応用した１例である。尚、第１の実施形態と同じ点については説明を省略する。
【００６０】
図６（ａ）は、ロボットシステムの構成を示す概略斜視図であり、図６（ｂ）は、ロボットの構成を示す模式断面図である。すなわち、本実施形態では、図６に示すように、ロボットシステム３０はワーク供給装置３１、ロボット３２、ワーク収納装置３３及び制御装置３４を備えている。
【００６１】
ワーク供給装置３１はワークとしての部品３５を供給する装置である。部品３５の形状や材質は特に限定されない、例えば、実施形態において部品３５は金属製の直方体となっている。
【００６２】
ワーク供給装置３１は部品整列装置３６と搬送装置３７とを備えている。部品整列装置３６は円錐状の皿部３６ａ及び皿部３６ａを支持する支持台３６ｂ等から構成されている。そして、皿部３６ａと支持台３６ｂの間には図示しない振動装置が配置されている。皿部３６ａの内側には螺旋状の段差が形成されている。段差は所定の幅の平坦部を有し、平坦部は部品３５が通過する通路になっている。平坦部は皿部３６ａの底から上部まで連続して形成されている。そして、振動装置が皿部３６ａを振動させるとき部品３５が平坦部に沿って移動するようになっている。平坦部の幅は部品３５が１個に限って通過可能な幅に形成されているので、部品３５が通路を通過することにより部品３５は１列に配列する。
【００６３】
搬送装置３７の上側にはベルトコンベア３７ａが配置されている。ベルトコンベア３７ａは１方向に長く延在して配置されている。この方向をＹ方向とする。そして水平方向においてＹ方向と直交する方向をＸ方向とし、鉛直方向をＺ方向とする。搬送装置３７は内部にステップモーター及びプーリーを備え、ベルトコンベア３７ａを移動及び停止することができる。ベルトコンベア３７ａの一端は部品整列装置３６の上部と接続されている。皿部３６ａの上部まで移動した部品３５はベルトコンベア３７ａの上に移動する。そして、部品３５はベルトコンベア３７ａにより順次図中右側へ移動し、所定の場所に停止するようになっている。従って、ベルトコンベア３７ａ上には部品３５が配列して配置される。
【００６４】
ロボット３２は平板状に形成された基台３８を備えている。基台３８上には支持台３９が配置されている。支持台３９の内部は空洞が形成され、この空洞は支持板４０により上下に分割されている。支持板４０の下側には第１モーター４１が配置され、第１モーター４１の下側には第１回転角度検出器４２が配置されている。第１回転角度検出器４２は第１モーター４１の回転角度を検出する。
【００６５】
支持板４０の上側には第１減速機４３が配置され、第１モーター４１の回転軸４１ａと接続されている。そして、第１減速機４３の上側には出力軸４３ａが配置されている。そして、第１モーター４１の回転軸４１ａの回転速度を減速した回転速度にて出力軸４３ａが回転する。第１減速機４３は各種の減速機を採用することができる。本実施形態では、例えば、ハーモニックドライブ（登録商標）を採用している。支持台３９の上面には孔部３９ａが形成され、孔部３９ａから出力軸４３ａが突出して配置されている。
【００６６】
出力軸４３ａと接続して略直方体状の可動部としての第１腕部４４が配置され、第１腕部４４は出力軸４３ａを中心に回転する。第１腕部４４において出力軸４３ａと反対側には第１角速度センサー４５が配置されている。従って、第１モーター４１が回転するとき、第１腕部４４が回転させられる。そして、第１腕部４４が回転する角度を第１回転角度検出器４２及び第１角速度センサー４５が検出可能になっている。
【００６７】
第１腕部４４上において第１角速度センサー４５の第１モーター４１側には第２減速機４６、第２モーター４７、第２回転角度検出器４８がこの順に重ねて配置されている。そして、第２減速機４６の出力軸４６ａが図中下方向に配置されている。第１腕部４４には第２減速機４６と対向する場所に孔部４４ａが形成され、孔部４４ａから出力軸４６ａが突出して配置されている。
【００６８】
第２モーター４７の回転軸には第２回転角度検出器４８が接続され、第２回転角度検出器４８は第２モーター４７の回転軸の回転角度を検出する。さらに、第２モーター４７の回転軸は第２減速機４６と接続されている。そして、第２モーター４７の回転軸の回転速度を減速した回転速度にて出力軸４６ａが回転する。
【００６９】
第１モーター４１及び第２モーター４７は電気信号によって回転方向を制御可能であればよく、各種のモーターを用いることができる。本実施形態では、例えば、直流モーターを採用している。第１回転角度検出器４２は第１モーター４１の駆動軸の回転角度を検出可能であれば良く、第２回転角度検出器４８は第２モーター４７の駆動軸の回転角度を検出可能であれば良い。第１回転角度検出器４２及び第２回転角度検出器４８には各種のロータリーエンコーダーを用いることができる。本実施形態では、例えば、光学式のロータリーエンコーダーを採用している。
【００７０】
出力軸４６ａと接続して略直方体状の可動部としての第２腕部４９が配置され、第２腕部４９は出力軸４６ａを中心に回転する。第２腕部４９において出力軸４６ａと反対側には第２角速度センサー５０が配置されている。第１角速度センサー４５及び第２角速度センサー５０の種類は特に限定されず、各種のジャイロスコープを用いることができる。本実施形態では、例えば、振動子型ジャイロスコープを採用している。
【００７１】
第２腕部４９上において第２角速度センサー５０の第２モーター４７側には回転装置５１が配置されている。そして、第２腕部４９には回転装置５１と対向する場所に孔部４９ａが形成され、孔部４９ａから回転装置５１の回転軸５１ａが突出して配置されている。第２腕部４９の下側において回転装置５１と対向する側には昇降装置５２が配置され、昇降装置５２は回転軸５１ａと接続されている。従って、回転装置５１は昇降装置５２を回転することができる。
【００７２】
昇降装置５２の下側には手部５３が配置されている。昇降装置５２は上下に移動する直動機構を有し、手部５３を昇降することができる。手部５３は略直方体状の２つの指部５３ａと直動機構とを有し、直動機構が２つの指部５３ａの間隔を変更することができる。そして、手部５３は指部５３ａの間に部品３５を挟んで保持することが可能になっている。
【００７３】
ロボット３２の図中右上にはワーク収納装置３３が配置されている。ワーク収納装置３３には複数の容器５６が配置されている。容器５６は外形が略直方体であり、深さが浅く形成されている。容器５６には仕切板が配置され、４つの部屋に分割されている。そして、各部屋に部品３５が配置される。
【００７４】
ワーク収納装置３３は内部に供給用昇降装置５７、除材用昇降装置５８とが並んで配置されている。供給用昇降装置５７及び除材用昇降装置５８の上側には容器５６が重ねて配置されている。そして、供給用昇降装置５７及び除材用昇降装置５８は上下方向に移動する直動機構を備え、容器５６を上昇及び下降することが可能になっている。供給用昇降装置５７の上側の容器５６は空になっている。そして、除材用昇降装置５８の上側の容器５６には部品３５が配置されている。
【００７５】
ワーク収納装置３３において図中左側の側面には直動機構を備えた押出装置５９が配置されている。押出装置５９は供給用昇降装置５７の上側に重ねて配置された容器５６の内最上段の容器５６を図中右下方向に押し出す。そして、押出装置５９は容器５６を供給用昇降装置５７の上側から除材用昇降装置５８の上側へ移動させる。
【００７６】
ロボット３２が供給用昇降装置５７の上側に位置する容器５６内に部品３５を移動させることにより、容器５６が部品３５で満たされる。そして、容器５６を押出装置５９が供給用昇降装置５７の上側から除材用昇降装置５８の上側へ移動させる。その後、除材用昇降装置５８が容器５６を下降させる。続いて、供給用昇降装置５７が空の容器５６を上昇させる。従って、ロボット３２が部品３５を空の容器５６内に移動させることができる。
【００７７】
昇降装置５２、手部５３、供給用昇降装置５７、除材用昇降装置５８、押出装置５９に配置された直動機構の種類は特に限定されない。例えば、直動機構にボールネジとパルスモーターとのを組み合わせたユニット、リニアモーター、エアーシリンダー等を用いることができる。本実施形態では、例えば、昇降装置５２、手部５３、供給用昇降装置５７、除材用昇降装置５８の直動機構にボールネジとパルスモーターとを組み合わせたユニットを採用した。そして、押出装置５９の直動機構には、例えば、エアーシリンダーを採用した。
【００７８】
ロボット３２の図中左側には制御装置３４が配置されている。制御装置３４は中央演算装置、記憶部、インターフェース、第１モーター４１及び第２モーター４７を駆動するモーター駆動回路を備えている。他にも、制御装置３４は、回転装置５１と昇降装置５２と手部５３とを駆動する駆動回路、入力装置、表示装置等を備えている。第１回転角度検出器４２、第１角速度センサー４５、第２回転角度検出器４８、第２角速度センサー５０はインターフェースを介して中央演算装置と接続されている。第１モーター４１及び第２モーター４７はモーター駆動回路と接続され、モーター駆動回路はインターフェースを介して中央演算装置と接続されている。さらに、ワーク供給装置３１及びワーク収納装置３３もインターフェースを介して中央演算装置と接続されている。
【００７９】
記憶部にはロボットシステム３０を制御するための動作手順を示したプログラムソフトや角度データ等が記憶されている。そして、中央演算装置は第１モーター４１及び第２モーター４７を所定の角度回転させて、第１回転角度検出器４２、第１角速度センサー４５、第２回転角度検出器４８、第２角速度センサー５０の出力を記憶部に記憶することができる。さらに、中央演算装置は第１回転角度検出器４２及び第１角速度センサー４５の出力を比較して較正するための演算を行う。他にも、中央演算装置は第２回転角度検出器４８及び第２角速度センサー５０の出力を比較して較正するための演算等を行う。
【００８０】
（ロボット制御方法）
次に、上述したロボットシステム３０においてロボット３２を制御する方法について図７及び図８にて説明する。尚、部品３５をワーク供給装置３１からワーク収納装置３３へロボット３２が移動させる１例を用いて説明する。図７は、ロボットの姿勢を説明するための模式図であり、図８は、部品の移動工程を示すフローチャートである。
【００８１】
図７（ａ）において、手部５３がベルトコンベア３７ａ上の部品３５を把持する予定の場所と対向する場所に位置している。このときのロボット３２の姿勢をロボット第１姿勢６０及びロボット第３姿勢６１とする。ロボット第１姿勢６０とロボット第３姿勢６１とは同じ姿勢である。
【００８２】
ロボット３２がロボット第１姿勢６０のときの第１角速度センサー４５の姿勢を第１センサー第１姿勢４５ａとし、第２角速度センサー５０の姿勢を第２センサー第１姿勢５０ａとする。そして、ロボット３２がロボット第３姿勢６１のときの第１角速度センサー４５の姿勢を第１センサー第３姿勢４５ｂとし、第２角速度センサー５０の姿勢を第２センサー第３姿勢５０ｂとする。第１角速度センサー４５及び第２角速度センサー５０の場所と姿勢とはロボット第１姿勢６０とロボット第３姿勢６１とで同じになっている。従って、第１センサー第１姿勢４５ａと第１センサー第３姿勢４５ｂとは同じ姿勢であり、第２センサー第１姿勢５０ａと第２センサー第３姿勢５０ｂとは同じ姿勢となっている。
【００８３】
図７（ｂ）において、手部５３がワーク収納装置３３上の容器５６と対向する場所に位置している。容器５６において左上の場所を第１の場所５６ａ、右上の場所を第２の場所５６ｂ、右下の場所を第３の場所５６ｃ、左下の場所を第４の場所５６ｄとする。そして、手部５３が部品３５を第１の場所５６ａに置くときに、手部５３が第１の場所５６ａと対向する場所に位置する。このときのロボット３２の姿勢をロボット第２姿勢６２とする。同様に、手部５３が第２の場所５６ｂ、第３の場所５６ｃ、第４の場所５６ｄと対向する場所に位置するときのロボット３２の姿勢をそれぞれ、ロボット第４姿勢、ロボット第５姿勢、ロボット第６姿勢とする。そして、ロボット３２の姿勢がロボット第２姿勢６２のときの第１角速度センサー４５の姿勢を第１センサー第２姿勢４５ｃとし、第２角速度センサー５０の姿勢を第２センサー第２姿勢５０ｃとする。
【００８４】
ロボット３２が部品３５をベルトコンベア３７ａから容器５６に移動させるとき、制御装置３４はロボット３２の姿勢をロボット第１姿勢６０からロボット第２姿勢６２へ変化させる。そして、ロボット３２がベルトコンベア３７ａ上の部品３５を把持するとき、制御装置３４はロボット３２の姿勢をロボット第２姿勢６２からロボット第３姿勢６１へ変化させる。次に、ロボット３２が第２の場所５６ｂに部品３５を置くときには、制御装置３４はロボット３２の姿勢をロボット第１姿勢６０からロボット第４姿勢へ変化させる。同様に、ロボット３２が第３の場所５６ｃに部品３５を置くときには、制御装置３４はロボット３２の姿勢をロボット第１姿勢６０からロボット第５姿勢へ変化させる。そして、ロボット３２が第４の場所５６ｄに部品３５を置くときには、制御装置３４はロボット３２の姿勢をロボット第１姿勢６０からロボット第６姿勢へ変化させる。
【００８５】
ロボット３２の姿勢がロボット第１姿勢６０からロボット第２姿勢６２を経てロボット第３姿勢６１に移行する。このとき、第１角速度センサー４５の姿勢は第１センサー第１姿勢４５ａから第１センサー第２姿勢４５ｃを経て第１センサー第３姿勢４５ｂに移行する。そして、第２角速度センサー５０の姿勢は第２センサー第１姿勢５０ａから第２センサー第２姿勢５０ｃを経て第２センサー第３姿勢５０ｂに移行する。
【００８６】
次に、図８に示すフローチャートを用いて、部品の移動工程におけるロボット３２の制御方法を説明する。ステップＳ２１は、第１較正工程に相当する。第１の実施形態におけるステップＳ１１の較正工程と同様の方法を用いて、第１角速度センサー４５及び第２角速度センサー５０を較正する。そして、第１角速度センサー４５及び第２角速度センサー５０のオフセット補正係数及び感度係数を演算する工程である。
【００８７】
次にステップＳ２２に移行する。ステップＳ２２は、移動工程に相当する。制御装置３４はロボット３２を駆動してロボット３２の姿勢をロボット第１姿勢６０にする。このとき、第１角速度センサー４５の姿勢は第１センサー第１姿勢４５ａとなり、第２角速度センサー５０の姿勢は第２センサー第１姿勢５０ａとなる。そして、手部５３が部品３５を把持する予定の場所と対向する場所に、手部５３を移動させる工程である。次にステップＳ２３に移行する。
【００８８】
ステップＳ２３は、ワーク供給工程に相当し、ワーク供給装置３１がベルトコンベア３７ａ上の所定の場所に部品３５を配置する。そして、手部５３と対応する場所に部品３５を配置する工程である。次にステップＳ２４に移行する。
【００８９】
ステップＳ２４は、ワーク把持工程に相当し、制御装置３４が手部５３に部品３５を把持させる工程である。制御装置３４は手部５３を駆動して、一対の指部５３ａの間隔を開く。そして、制御装置３４は回転装置５１を駆動して、一対の指部５３ａの間と対向する場所に部品３５が位置するように手部５３を回転させる。次に、制御装置３４は昇降装置５２を駆動して、手部５３を下降させる。続いて、制御装置３４は手部５３を駆動して、指部５３ａの間隔を狭くすることにより、手部５３に部品３５を把持させる。次に、制御装置３４は昇降装置５２を駆動して、手部５３を上昇させる。次にステップＳ２５、ステップＳ２６、ステップＳ２７、ステップＳ３０に移行する。ステップＳ２５とステップＳ２６、ステップＳ２７とは並行して行われる。
【００９０】
ステップＳ２５は、第１変更工程に相当し、制御装置３４がロボット３２の姿勢をロボット第１姿勢６０からロボット第２姿勢６２へ変更させる工程である。制御装置３４はロボット３２を駆動して手部５３をベルトコンベア３７ａと対向する場所から容器５６と対向する場所に移動させる。このとき、第１角速度センサー４５の姿勢は第１センサー第１姿勢４５ａから第１センサー第２姿勢４５ｃに移行し、第２角速度センサー５０の姿勢は第２センサー第１姿勢５０ａから第２センサー第２姿勢５０ｃに移行する。次にステップＳ２８に移行する。尚、第１の場所５６ａに部品３５が配置されているときには、ロボット第４姿勢に移行する。第１の場所５６ａ及び第２の場所５６ｂに部品３５が配置されているときには、ロボット第５姿勢に移行する。第１の場所５６ａ〜第３の場所５６ｃに部品３５が配置されているときには、ロボット第６姿勢に移行する。
【００９１】
ステップＳ２６は、第１検出工程に相当し、回転角度検出器の出力と角速度センサーの出力とを検出する工程である。第１角速度センサー４５の姿勢が第１センサー第１姿勢４５ａから第１センサー第２姿勢４５ｃに移行する間で、第１角速度センサー４５の出力を積分した第１出力積分を検出する。さらに、第２角速度センサー５０の姿勢が第２センサー第１姿勢５０ａから第２センサー第２姿勢５０ｃに移行する間で、第２角速度センサー５０の出力を積分した第１出力積分を検出する。さらに、第１角速度センサー４５及び第２角速度センサー５０の出力を積分する間の時間である第１経過時間を検出する。さらに、制御装置３４は第１回転角度検出器４２及び第２回転角度検出器４８の出力を検出して記憶する。この工程は第１の実施形態におけるステップＳ２の第１検出工程と略同様の工程となっている。次にステップＳ２８に移行する。
【００９２】
ステップＳ２７は、姿勢検出工程に相当し、第１腕部４４及び第２腕部４９の姿勢を検出する工程である。そして、第１角速度センサー４５及び第２角速度センサー５０における出力を積分する。次に、積分した値からオフセット補正係数と経過時間との乗算値を引き算した第２正味出力積分を演算する。そして、第２正味出力積分と感度係数とを乗算して第１角速度センサー４５及び第２角速度センサー５０の姿勢を検出する。また、第１角速度センサー４５及び第２角速度センサー５０の出力からオフセット補正量を減算した後、感度係数を乗算することにより、各センサーの角速度を算出する。次に、第１角速度センサー４５及び第２角速度センサー５０の姿勢の情報を用いて第１腕部４４及び第２腕部４９の姿勢を検出する。尚、第１角速度センサー４５及び第２角速度センサー５０の出力は第１腕部４４及び第２腕部４９の捩れ等による振動の影響を受けた出力となっている。第１腕部４４及び第２腕部４９の移動中においても随時角度及び角速度を検出する。この工程は第１の実施形態におけるステップＳ７の第３検出工程と略同様の工程となっている。次にステップＳ２８に移行する。
【００９３】
ステップＳ２８は、ワーク配置工程に相当し、部品３５を容器５６上に配置する工程である。制御装置３４は昇降装置５２を駆動して、手部５３を下降させる。続いて、制御装置３４は手部５３を駆動して、指部５３ａの間隔を広くする。そして、制御装置３４は部品３５を手部５３から離れさせて、部品３５を容器５６上に載置する。次に、制御装置３４は昇降装置５２を駆動して、手部５３を上昇させる。容器５６の４箇所総てに部品３５が載置されたときには、ワーク収納装置３３は容器５６を入れ替える。次にステップＳ２９に移行する。
【００９４】
ステップＳ２９は、第２変更工程に相当し、制御装置３４がロボット３２の姿勢をロボット第２姿勢６２からロボット第３姿勢６１へ変更させる工程である。制御装置３４はロボット３２を駆動して手部５３を容器５６と対向する場所からベルトコンベア３７ａと対向する場所に移動させる。このとき、第１角速度センサー４５の姿勢は第１センサー第２姿勢４５ｃから第１センサー第３姿勢４５ｂに移行し、第２角速度センサー５０の姿勢は第２センサー第２姿勢５０ｃから第２センサー第３姿勢５０ｂに移行する。次にステップＳ３１に移行する。
【００９５】
ステップＳ２４のワーク把持工程、ステップＳ２５の第１変更工程、ステップＳ２８のワーク配置工程、ステップＳ２９の第２変更工程を合わせてステップＳ４１の作業工程とする。作業工程は部品３５を移動する作業を行う工程となっている。
【００９６】
ステップＳ３０は、第２検出工程に相当し、回転角度検出器の出力と角速度センサーの出力とを検出する工程である。ステップＳ３０はステップＳ２５、ステップＳ２８、ステップＳ２９と並行して行われる。第１角速度センサー４５の姿勢が第１センサー第１姿勢４５ａから第１センサー第２姿勢４５ｃを経て第１センサー第３姿勢４５ｂに移行する間で、第１角速度センサー４５の出力を積分した第２出力積分を検出する。さらに、第２角速度センサー５０の姿勢が第２センサー第１姿勢５０ａから第２センサー第２姿勢５０ｃを経て第２センサー第３姿勢５０ｂに移行する間で、第２角速度センサー５０の出力を積分した第２出力積分を検出する。さらに、第１角速度センサー４５及び第２角速度センサー５０の出力を積分する間の時間とを検出する。さらに、制御装置３４は第１回転角度検出器４２及び第２回転角度検出器４８の出力を検出して記憶する。この工程は第１の実施形態におけるステップＳ４の第２検出工程と略同様の工程となっている。次にステップＳ３１に移行する。
【００９７】
ステップＳ３１は、係数算出工程に相当し、オフセット補正係数と感度係数とを演算する工程である。オフセット補正係数と感度係数との演算方法は第１の実施形態におけるステップＳ５の係数算出工程と略同様の方法にて演算する。オフセット補正係数と感度係数とを用いてステップＳ２７では第１腕部４４及び第２腕部４９の姿勢を検出する。次にステップＳ３２に移行する。
【００９８】
ステップＳ２５の第１変更工程、ステップＳ２６の第１検出工程、ステップＳ２９の第２変更工程、ステップＳ３０の第２検出工程を合わせてステップＳ４２の第２較正工程とする。第２較正工程では制御装置３４が、制御装置３４がオフセット補正係数と感度係数とを演算する工程となっている。ステップＳ４１の一部とステップＳ４２の第２較正工程とは並行して行われる。
【００９９】
ステップＳ３２は、終了判断工程に相当し、作業を継続するか終了するかを判断する工程である。予定した個数の部品３５を移動していないとき作業を継続する。次に、ステップＳ２３に移行する。予定した個数の部品３５を移動したとき終了する。以上の工程により部品の移動工程を終了する。
【０１００】
上述したように、本実施形態によれば、以下の効果を有する。
（１）本実施形態によれば、ステップＳ２１の第１較正工程とステップＳ３１の係数算出工程において、オフセット補正係数と感度係数とを演算している。そして、オフセット誤差のない角速度センサーの出力を用いて角速度センサーの感度係数を演算している。従って、精度良く第１角速度センサー４５および第２角速度センサー５０の姿勢を検出することができる為、精度良くロボット３２の姿勢を制御することができる。
【０１０１】
（２）本実施形態によれば、ステップＳ４２の第２較正工程とステップＳ４１の作業工程の一部とが並行して行われる。従って、第２較正工程と作業工程とを別々に行う方法に比べて生産性良く第１角速度センサー４５及び第２角速度センサー５０の較正を行うことができる。
【０１０２】
（３）本実施形態によれば、ロボット３２の姿勢はロボット第１姿勢６０からロボット第２姿勢６２を経てロボット第３姿勢６１へ移動している。そして、ロボット３２のロボット第１姿勢６０とロボット第３姿勢６１とが同じ姿勢に設定されている。ロボット３２の姿勢が同じ姿勢のとき、第１角速度センサー４５及び第２角速度センサー５０の位置と姿勢とは同じになる。従って、第１角速度センサー４５及び第２角速度センサー５０の姿勢を容易に同じ姿勢にすることができる。
【０１０３】
尚、本実施形態は上述した実施形態に限定されるものではなく、種々の変更や改良を加えることも可能である。変形例を以下に述べる。
（変形例１）
前記第１の実施形態では、ステップＳ２の第１検出工程とステップＳ４の第２検出工程とを並行して行なった。これに限定されず、ステップＳ２とステップＳ４とは別に行なっても良い。工程設計の都合に合わせても良い。同様に、前記第２の実施形態において、ステップＳ２６の第１検出工程とステップＳ３０の第２検出工程とは別に行なっても良い。工程設計の都合に合わせても良い。
【０１０４】
（変形例２）
前記第１の実施形態では、制御装置１０の記憶部内に動作手順に沿ったプログラムソフトを記憶し、プログラムにより角度検出装置１の制御を行なったが、これに限らず、電気回路にて構成される制御装置にて制御しても良い。周辺機器が手順通りに制御されれば良い。この内容は、前記第２の実施形態にも適用できる。制御装置３４は電気回路にて構成される制御装置にて制御しても良い。
【０１０５】
（変形例３）
前記第２の実施形態では、ロボット３２がロボット第１姿勢６０のとき、第１角速度センサー４５の姿勢を第１センサー第１姿勢４５ａとし、第２角速度センサー５０の姿勢を第２センサー第１姿勢５０ａとした。第１センサー第１姿勢４５ａ及び第２センサー第１姿勢５０ａはロボット３２が他の姿勢のときでも良い。手部５３が別の場所に位置するときに第１センサー第１姿勢４５ａ及び第２センサー第１姿勢５０ａを設定しても良い。この場合にも、第１センサー第１姿勢４５ａを第１センサー第３姿勢４５ｂと同じ姿勢に設定する。そして、第２センサー第１姿勢５０ａを第２センサー第３姿勢５０ｂと同じ姿勢に設定することにより前記第１の実施形態と同様の方法を用いることができる。
【０１０６】
（変形例４）
前記第２の実施形態では、ステップＳ２６の第１検出工程はロボット３２の姿勢がロボット第１姿勢６０からロボット第２姿勢６２に変更される間にて行なったが、ロボット第２姿勢６２以外の姿勢に移行する場合で行なっても良い。ロボット３２の姿勢がロボット第４姿勢、ロボット第５姿勢、ロボット第６姿勢の各姿勢に移行するときに行なっても良い。そして、較正する頻度を多くしても良い。較正する頻度が多い方が第１角速度センサー４５及び第２角速度センサー５０の測定精度を良くすることができる。
【０１０７】
（変形例５）
前記第２の実施形態では、ロボット３２がベルトコンベア３７ａから容器５６に部品３５を移動させた。これに限らず、ロボット３２が容器５６からベルトコンベア３７ａに部品３５を移動させるときに上述の較正方法を用いても良い。このとき、ロボット３２の姿勢がロボット第２姿勢６２のときに第１角速度センサー４５の姿勢を第１センサー第１姿勢４５ａ及び第１センサー第３姿勢４５ｂにする。そして、第２角速度センサー５０の姿勢を第２センサー第１姿勢５０ａ及び第２センサー第３姿勢５０ｂにする。ロボット３２の姿勢がロボット第１姿勢６０のときに第１角速度センサー４５の姿勢を第１センサー第２姿勢４５ｃにする。そして、第２角速度センサー５０の姿勢を第２センサー第２姿勢５０ｃにする。そして、容器５６が入れ替わるときにも、ロボット３２がロボット第２姿勢６２となるときにステップＳ４２の第２較正工程を行うようにしても良い。この場合にも前記第２の実施形態と同様に第１角速度センサー４５及び第２角速度センサー５０の較正を行うことができる。
【０１０８】
（変形例６）
前記第２の実施形態では、ロボット３２は第１腕部４４と第２腕部４９との２つの腕部を備えたが、腕部の数は２つに限定されない。腕部の数は１つでも良く、３つ以上でも良い。この場合にも、同様の方法を用いて角速度センサーの較正を行うことができる。
【０１０９】
（変形例７）
前記第１の実施形態では、ステップＳ１の第１変更工程とステップＳ３の第２変更工程において、腕部８が回転する間の回転角度検出器６の出力を制御装置１０は入力して記憶部に記憶した。制御装置１０は、腕部８が回転する間の総ての間において回転角度検出器６の出力を入力しなくとも良い。制御装置１０は、第１姿勢１１、第２姿勢１２、第３姿勢１３における回転角度検出器６の出力を入力して記憶部に記憶すれば良い。記憶部の記憶容量を減らすことができる。
【０１１０】
（変形例８）
前記第１の実施形態では、ステップＳ５の係数算出工程において、角速度センサー出力推移線１５を積分して角速度出力積分推移線１６を演算した後、正味出力積分推移線１９を算出した。角速度センサー出力推移線１５からオフセット出力１５ａを減算した後、積分の演算をすることにより正味出力積分推移線１９を算出しても良い。同様に正味出力積分推移線１９を算出することができる。この内容は前記第２の実施形態におけるステップＳ２１の第１較正工程及びステップＳ３１の係数算出工程においても適用することができる。
【０１１１】
（変形例９）
前記第２の実施形態では、ステップＳ４１の作業工程を行う毎にステップＳ４２の第２較正工程を行ったが、ステップＳ４２は必ずしも毎回行わなくとも良い。適宜設定した頻度にて行っても良い。第２較正工程を省略することにより、演算する時間が減る為、作業速度をあげることができる。
【０１１２】
（変形例１０）
ステップＳ２５の第１変更工程及びステップＳ２９の第２変更工程において第１腕部４４及び第２腕部４９の駆動制御を行うとき、第１回転角度検出器４２と第２回転角度検出器４８とに加えて第１角速度センサー４５及び第２角速度センサー５０を用いても良い。そして、駆動制御の演算にステップＳ３１にて算出したオフセット補正係数と感度係数を用いても良い。第１腕部４４及び第２腕部４９の駆動制御を行うとき、第１回転角度検出器４２と第２回転角度検出器４８とを用いて駆動制御する場合より位置精度良く制御することができる。もしくは、制振性良く制御することができる。
【０１１３】
（変形例１１）
前記第１の実施形態では、ステップＳ５の係数算出工程において、回転角度差１４ａを第１正味出力積分１９ａにて除算することにより感度係数を算出した。感度係数は他の方法にて算出しても良い。角度検出器出力推移線１４を時間にて微分することにより、角度微分推移線を算出する。次に、角速度センサー出力推移線１５からオフセット出力１５ａを減算することにより、正味角速度センサー出力推移線を算出する。続いて、角度微分推移線の各時刻の値を正味角速度センサー出力推移線の各時刻の値にて除算することにより各時刻の感度係数を算出する。このとき、角度微分推移線及び正味角速度センサー出力推移線の値が零でないときの値を用いるのが好ましい。感度係数の精度を向上することができる。次に、各時刻の感度係数の平均を算出して感度係数とする。つまり、回転角度検出器６の出力から算出した回転角速度と角速度センサー９の出力とを用いて感度係数を算出しても良い。この場合にも、同様に感度係数を算出することができる。この内容は前記第２の実施形態にも適用することができる。
【０１１４】
（変形例１２）
前記第１の実施形態では、ステップＳ７の第３検出工程において、正味出力積分推移線２６に感度係数を乗算してセンサー角度推移線２７を算出した。センサー角度推移線２７は他の方法にて算出しても良い。角速度センサー出力推移線２３からオフセット出力２３ａを除算した後、除算結果に感度係数を乗算して角速度推移線を算出する。次に、この角速度推移線を積分することにより、センサー角度推移線２７しても良い。この場合にも、同様にセンサー角度推移線２７を算出することができる。この内容は前記第２の実施形態にも適用することができる。
【符号の説明】
【０１１５】
６…回転角度検出器、８…センサー支持部としての腕部、９…角速度センサー、１１…第１姿勢、１２…第２姿勢、１３…第３姿勢、１６ａ…第１出力積分、１６ｂ…第２出力積分、１７…第１経過時間、１８…第２経過時間、１９ａ…第１正味出力積分、２４ｄ…第２正味出力積分、３２…ロボット、４４…可動部としての第１腕部、４５…第１角速度センサー、４５ａ…第１センサー第１姿勢、４５ｂ…第１センサー第３姿勢、４５ｃ…第１センサー第２姿勢、４９ａ…可動部としての孔部、５０…第２角速度センサー、５０ａ…第２センサー第１姿勢、５０ｂ…第２センサー第３姿勢、５０ｃ…第２センサー第２姿勢。
【特許請求の範囲】
【請求項１】
角速度センサーを用いた角速度または角度の検出方法であって、
前記角速度センサーの姿勢を第１姿勢から第２姿勢へ変更する第１変更工程と、
前記第１姿勢から前記第２姿勢へ変更する間の前記角速度センサーの出力を積分した第１出力積分と、前記第１姿勢から前記第２姿勢へ変更するのに要した時間である第１経過時間と、を検出する第１検出工程と、
前記角速度センサーの姿勢を前記第２姿勢から前記第１姿勢と同じ姿勢である第３姿勢へ変更する第２変更工程と、
前記第１姿勢から前記第２姿勢を経て前記第３姿勢へ変更する間の前記角速度センサーの出力を積分した第２出力積分と、前記第１姿勢から前記第２姿勢を経て前記第３姿勢へ変更するのに要した時間である第２経過時間と、を検出する第２検出工程と、
前記角速度センサーの角速度が零のときの出力をオフセット補正係数とし、前記角速度センサーの角速度変化量と前記角速度センサーの出力の関係を示す係数を感度係数とするとき、
前記第２出力積分を前記第２経過時間にて除算して前記オフセット補正係数を演算し、前記第１出力積分から前記オフセット補正係数と前記第１経過時間との乗算値を引き算した第１正味出力積分を演算し、前記第１姿勢と前記第２姿勢との回転角度の差を前記第１正味出力積分にて除算して感度係数を演算する係数算出工程と、
前記角速度センサーが第３経過時間の間に姿勢を変更するとき、変更する間における前記角速度センサーの出力と前記オフセット補正係数と感度係数とを用いて前記角速度センサーの角速度または姿勢を検出する姿勢検出工程と、を有することを特徴とする角速度または角度の検出方法。
【請求項２】
請求項１に記載の角速度または角度の検出方法であって、
前記角速度センサーはセンサー支持部に配置され、
前記センサー支持部は、回転軸と前記センサー支持部の回転角度を検出する回転角度検出器とを有し、前記角速度センサーの姿勢は前記回転角度検出器を用いて検出することを特徴とする角速度または角度の検出方法。
【請求項３】
請求項２に記載の角速度または角度の検出方法であって、
前記角速度センサーは振動子型ジャイロスコープであることを特徴とする角速度または角度の検出方法。
【請求項４】
請求項３に記載の角速度または角度の検出方法であって、
前記回転角度検出器はロータリーエンコーダーを用いて回転角度を検出することを特徴とする角速度または角度の検出方法。
【請求項５】
角速度センサーを用いて可動部の姿勢を検出するロボットの制御方法であって、
前記角速度センサーを較正する較正工程と、前記可動部の姿勢を検出する姿勢検出工程と、を有し、
前記較正工程は、
前記角速度センサーの姿勢を第１姿勢から第２姿勢へ変更する第１変更工程と、
前記第１姿勢から前記第２姿勢へ変更する間の前記角速度センサーの出力を積分した第１出力積分と、前記第１姿勢から前記第２姿勢へ変更するのに要した時間である第１経過時間と、を検出する第１検出工程と、
前記角速度センサーの姿勢を前記第２姿勢から前記第１姿勢と同じ姿勢である第３姿勢へ変更する第２変更工程と、
前記第１姿勢から前記第２姿勢を経て前記第３姿勢へ変更する間の前記角速度センサーの出力を積分した第２出力積分と、前記第１姿勢から前記第２姿勢を経て前記第３姿勢へ変更するのに要した時間である第２経過時間と、を検出する第２検出工程と、
時間の経過にともなって変化する前記角速度センサーの前記出力の変化率をオフセット補正係数とし、前記角速度センサーの角速度変化量と前記角速度センサーの出力の関係を示す係数を感度係数とするとき、
前記第２出力積分を前記第２経過時間にて除算して前記オフセット補正係数を演算し、前記第１出力積分から前記オフセット補正係数と前記第１経過時間との乗算値を引き算した第１正味出力積分を演算し、前記第１姿勢と前記第２姿勢との回転角度の差を前記第１正味出力積分にて除算して感度係数を演算する係数算出工程と、を有し、
前記姿勢検出工程は、前記角速度センサーが第３経過時間の間に姿勢を変更するとき、変更する間における前記角速度センサーの出力と前記オフセット補正係数と感度係数とを用いて前記角速度センサーの角速度または姿勢を検出する姿勢検出工程を有し、検出した前記角速度センサーの角速度の情報を用いて前記可動部の角速度検出、または検出した前記角速度センサーの姿勢の情報を用いて前記可動部の姿勢検出をすることを特徴とするロボットの制御方法。
【請求項６】
請求項５に記載のロボットの制御方法であって、
前記ロボットが作業を行う作業工程を有し、
前記較正工程の少なくとも一部は前記作業工程と並行して行われることを特徴とするロボットの制御方法。
【請求項７】
請求項６に記載のロボットの制御方法であって、
前記第１姿勢と前記第３姿勢とにおいて前記角速度センサーは同じ場所に位置することを特徴とするロボットの制御方法。
【請求項１】
角速度センサーを用いた角速度または角度の検出方法であって、
前記角速度センサーの姿勢を第１姿勢から第２姿勢へ変更する第１変更工程と、
前記第１姿勢から前記第２姿勢へ変更する間の前記角速度センサーの出力を積分した第１出力積分と、前記第１姿勢から前記第２姿勢へ変更するのに要した時間である第１経過時間と、を検出する第１検出工程と、
前記角速度センサーの姿勢を前記第２姿勢から前記第１姿勢と同じ姿勢である第３姿勢へ変更する第２変更工程と、
前記第１姿勢から前記第２姿勢を経て前記第３姿勢へ変更する間の前記角速度センサーの出力を積分した第２出力積分と、前記第１姿勢から前記第２姿勢を経て前記第３姿勢へ変更するのに要した時間である第２経過時間と、を検出する第２検出工程と、
前記角速度センサーの角速度が零のときの出力をオフセット補正係数とし、前記角速度センサーの角速度変化量と前記角速度センサーの出力の関係を示す係数を感度係数とするとき、
前記第２出力積分を前記第２経過時間にて除算して前記オフセット補正係数を演算し、前記第１出力積分から前記オフセット補正係数と前記第１経過時間との乗算値を引き算した第１正味出力積分を演算し、前記第１姿勢と前記第２姿勢との回転角度の差を前記第１正味出力積分にて除算して感度係数を演算する係数算出工程と、
前記角速度センサーが第３経過時間の間に姿勢を変更するとき、変更する間における前記角速度センサーの出力と前記オフセット補正係数と感度係数とを用いて前記角速度センサーの角速度または姿勢を検出する姿勢検出工程と、を有することを特徴とする角速度または角度の検出方法。
【請求項２】
請求項１に記載の角速度または角度の検出方法であって、
前記角速度センサーはセンサー支持部に配置され、
前記センサー支持部は、回転軸と前記センサー支持部の回転角度を検出する回転角度検出器とを有し、前記角速度センサーの姿勢は前記回転角度検出器を用いて検出することを特徴とする角速度または角度の検出方法。
【請求項３】
請求項２に記載の角速度または角度の検出方法であって、
前記角速度センサーは振動子型ジャイロスコープであることを特徴とする角速度または角度の検出方法。
【請求項４】
請求項３に記載の角速度または角度の検出方法であって、
前記回転角度検出器はロータリーエンコーダーを用いて回転角度を検出することを特徴とする角速度または角度の検出方法。
【請求項５】
角速度センサーを用いて可動部の姿勢を検出するロボットの制御方法であって、
前記角速度センサーを較正する較正工程と、前記可動部の姿勢を検出する姿勢検出工程と、を有し、
前記較正工程は、
前記角速度センサーの姿勢を第１姿勢から第２姿勢へ変更する第１変更工程と、
前記第１姿勢から前記第２姿勢へ変更する間の前記角速度センサーの出力を積分した第１出力積分と、前記第１姿勢から前記第２姿勢へ変更するのに要した時間である第１経過時間と、を検出する第１検出工程と、
前記角速度センサーの姿勢を前記第２姿勢から前記第１姿勢と同じ姿勢である第３姿勢へ変更する第２変更工程と、
前記第１姿勢から前記第２姿勢を経て前記第３姿勢へ変更する間の前記角速度センサーの出力を積分した第２出力積分と、前記第１姿勢から前記第２姿勢を経て前記第３姿勢へ変更するのに要した時間である第２経過時間と、を検出する第２検出工程と、
時間の経過にともなって変化する前記角速度センサーの前記出力の変化率をオフセット補正係数とし、前記角速度センサーの角速度変化量と前記角速度センサーの出力の関係を示す係数を感度係数とするとき、
前記第２出力積分を前記第２経過時間にて除算して前記オフセット補正係数を演算し、前記第１出力積分から前記オフセット補正係数と前記第１経過時間との乗算値を引き算した第１正味出力積分を演算し、前記第１姿勢と前記第２姿勢との回転角度の差を前記第１正味出力積分にて除算して感度係数を演算する係数算出工程と、を有し、
前記姿勢検出工程は、前記角速度センサーが第３経過時間の間に姿勢を変更するとき、変更する間における前記角速度センサーの出力と前記オフセット補正係数と感度係数とを用いて前記角速度センサーの角速度または姿勢を検出する姿勢検出工程を有し、検出した前記角速度センサーの角速度の情報を用いて前記可動部の角速度検出、または検出した前記角速度センサーの姿勢の情報を用いて前記可動部の姿勢検出をすることを特徴とするロボットの制御方法。
【請求項６】
請求項５に記載のロボットの制御方法であって、
前記ロボットが作業を行う作業工程を有し、
前記較正工程の少なくとも一部は前記作業工程と並行して行われることを特徴とするロボットの制御方法。
【請求項７】
請求項６に記載のロボットの制御方法であって、
前記第１姿勢と前記第３姿勢とにおいて前記角速度センサーは同じ場所に位置することを特徴とするロボットの制御方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【公開番号】特開２０１０−２４７３０３（Ｐ２０１０−２４７３０３Ａ）
【公開日】平成２２年１１月４日（２０１０．１１．４）
【国際特許分類】
【出願番号】特願２００９−１０１６５４（Ｐ２００９−１０１６５４）
【出願日】平成２１年４月２０日（２００９．４．２０）
【出願人】（０００００２３６９）セイコーエプソン株式会社 (51,324)
【Ｆターム（参考）】
【公開日】平成２２年１１月４日（２０１０．１１．４）
【国際特許分類】
【出願日】平成２１年４月２０日（２００９．４．２０）
【出願人】（０００００２３６９）セイコーエプソン株式会社 (51,324)
【Ｆターム（参考）】
[ Back to top ]