JP3776345B2 - 自動原点合わせ方法及び自動原点確認方法 - Google Patents
自動原点合わせ方法及び自動原点確認方法 Download PDFInfo
- Publication number
- JP3776345B2 JP3776345B2 JP2001309155A JP2001309155A JP3776345B2 JP 3776345 B2 JP3776345 B2 JP 3776345B2 JP 2001309155 A JP2001309155 A JP 2001309155A JP 2001309155 A JP2001309155 A JP 2001309155A JP 3776345 B2 JP3776345 B2 JP 3776345B2
- Authority
- JP
- Japan
- Prior art keywords
- origin
- automatic
- offset angle
- midpoint
- value
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Images
Landscapes
- Manipulator (AREA)
- Control Of Position Or Direction (AREA)
Description
【０００１】
【発明の属する技術分野】
この発明は、関節等の自動原点合わせ方法及び自動原点確認方法に係り、特に、温度ドリフトの影響を受けずに高精度な自動原点合わせ及び自動原点確認を実現可能にする自動原点合わせ方法及び自動原点確認方法に関する。
【０００２】
【従来の技術】
ロボット等の関節の制御では、予め決められた原点位置から相対的に関節を移動させている。したがって、必要に応じて原点合わせを正確に行う必要がある。また、温度ドリフトや機構の誤差によって原点検出のずれなどが生じる場合があるので、その補償を行う必要もある。特に、特開平５−２３７７７５号公報等に開示された自立移動する脚式歩行ロボットのように精密な制御が必要なものほど、原点合わせも高精度に行えるようにする必要がある。
なお、温度ドリフトとは、近接スイッチなど位置検出手段の温度特性などによって生じる現象の一種であり、温度によって位置検出手段による検出位置が変わることをいう。
【０００３】
【発明が解決しようとする課題】
この原点合わせを必要に応じて毎回人間が治具を用いて行うことも可能であるが、その作業は繁雑で時間がかかり、早急に起動したい時などに対応できないこともある。したがって、自動原点合わせ機構が必要不可欠である。
従来の自動原点合わせ方法として、予め合わせた原点から近接スイッチが感知するまでの角度と、原点位置におけるアブソリュートエンコーダ値をメモリに記憶しておき、次回の原点合わせ時に近接スイッチの感知位置から前記角度だけ関節を戻す方法がある。この方法では、戻された位置のアブソリュートエンコーダ値と予め記憶されたアブソリュートエンコーダ値とが比較され、その差分だけさらに原点へ合わせる補正が実行される。しかしながら、モータの高出力化に伴い関節部の温度上昇が以前より大きくなるにしたがって、温度ドリフトが増大し、高精度な原点合わせが困難になった。
【０００４】
例えば、３００倍の減速比で関節を回転させる場合、モータの回転角度は関節の角度変化の３００倍となるが、温度ドリフトが増大しているときに前記方法で原点合わせを行うと、モータにおける回転角度で半回転分以上のずれが原点に生じる場合もある。このような場合は、アブソリュートエンコーダ値がどちらにずれたかが分からないので、高精度な自動原点合わせができない。
そこで、この発明は、温度ドリフトの影響を受けず、高精度に自動原点合わせ及び自動原点確認が可能な自動原点合わせ方法及び自動原点確認方法を提供するものである。
【０００５】
【課題を解決するための手段】
上記課題を解決するために、請求項１に記載した発明は、被検出部（例えば、後述する実施の形態におけるドック７）を備えた部材（例えば、後述する実施の形態における下腿部２）を原点から検出部（例えば、後述する実施の形態における近接スイッチ８）に向かって複数方向から接近させて複数の検出位置を検出し、前記原点から前記複数の検出位置の中点までの第１の位置変位（例えば、後述する実施の形態におけるオフセット角度θ０）を予め記憶しておき、
自動原点合わせ時には、前記方法で改めて複数の検出位置の中点までの第２の位置変位（例えば、後述する実施の形態におけるオフセット角度θａ）を検出し、前記第１及び第２の位置変位を比較し、その差分を補正して前記部材を前記原点まで移動させることを特徴とする自動原点合わせ方法である。
【０００６】
検出した複数の検出位置の中点は、温度ドリフトの大小にかかわらず常に検出部の中央に位置する。したがって、前記第１及び第２の位置変位を比較することにより、温度ドリフトの影響を受けずに原点のずれ（差）を検知することができ、そのずれ（差）を補正して前記部材を原点に移動させているので、原点を高精度に合わせることが可能になる。
【０００７】
請求項２に記載した発明は、請求項１記載の自動原点合わせ方法であって、
前記部材の前記原点における位置情報（例えば、後述する実施の形態におけるアブソリュートエンコーダ値）も予め記憶しておき、
自動原点合わせ動作によって移動した後の前記部材の位置情報と前記予め記憶しておいた位置情報とを比較し、その差分だけ前記部材をさらに前記原点方向に移動させることを特徴とする自動原点合わせ方法である。
このように構成することにより、原点における位置情報を比較してさらに補正をしているので、さらに高精度に原点を合わせることが可能になる。
【０００８】
請求項３に記載した発明は、第１部材（例えば、後述する実施の形態における大腿部１）と第２部材（例えば、後述する実施の形態における下腿部２）が相対回転可能に連結され、前記第２部材と同期回転する回転体（例えば、後述する実施の形態における回転体４）がモータ（例えば、後述する実施の形態におけるモータ５）により回転駆動され、前記回転体に設けられた被検出部（例えば、後述する実施の形態におけるドック７）を検出する検出手段（例えば、後述する実施の形態における近接スイッチ８）が前記第１部材に設置され、前記モータの回転角度を検出するエンコーダ（例えば、後述する実施の形態におけるアブソリュートエンコーダ１０，インクリメンタルエンコーダ２０）の検出値に基づいて前記第１部材と前記第２部材の相対角度が制御される自動原点合わせ方法であって、前記回転体を原点から前記検出手段に向かって回転させて前記被検出部の第１及び第２の端点を検出し、該２つの端点検出位置の中点を算出し、前記原点から前記中点までのオフセット角度（例えば、後述する実施の形態におけるオフセット角度θ０）を予め記憶しておき、
自動原点合わせ時には、前記の方法で改めて第１及び第２の端点検出位置の中点までのオフセット角度（例えば、後述する実施の形態におけるオフセット角度θａ）を算出し、該オフセット角度と前記予め記憶したオフセット角度とを比較し、その差分を補正して前記第２部材を前記原点まで移動させることを特徴とする自動原点合わせ方法である。
【０００９】
第１の端点検出位置と第２の端点検出位置の中点は、温度ドリフトの大小にかかわらず常に検出手段の中央に位置する。したがって、自動原点合わせ時に算出したオフセット角度と予め記憶しておいたオフセット角度とを比較することにより、温度ドリフトの影響を受けずに原点のずれ（差）を検知することができ、そのずれ（差）を補正して前記第２部材を原点に移動させているので、第２部材を原点に高精度に合わせることが可能になる。
【００１０】
請求項４に記載した発明は、請求項３記載の自動原点合わせ方法であって、
前記第２部材の前記原点におけるエンコーダ値（例えば、後述する実施の形態におけるアブソリュートエンコーダ値）も予め記憶しておき、
自動原点合わせ動作によって移動した後の前記第２部材のエンコーダ値と前記予め記憶しておいたエンコーダ値とを比較し、その差分だけ前記第２部材をさらに前記原点方向に移動させることを特徴とする自動原点合わせ方法である。
このように構成することにより、原点におけるエンコーダ値を比較してさらに補正をしているので、第２部材を原点にさらに高精度に合わせることが可能になる。
【００１１】
請求項５に記載した発明は、被検出部（例えば、後述する実施の形態におけるドック７）を備えた部材（例えば、後述する実施の形態における下腿部２）を原点から検出部（例えば、後述する実施の形態における近接スイッチ８）に向かって複数方向から接近させて複数の検出位置を検出し、前記原点から前記複数の検出位置の中点までの第１の位置変位（例えば、後述する実施の形態におけるオフセット角度θ０）を予め記憶しておき、
自動原点確認時には、前記方法で改めて複数の検出位置の中点までの第２の位置変位（例えば、後述する実施の形態におけるオフセット角度θａ）を検出し、前記第１及び第２の位置変位の差分と所定値（例えば、後述する実施の形態におけるβ１）との比較によって、前記部材の原点が正確に維持されているか否かを判定することを特徴とする自動原点確認方法である。
検出した複数の検出位置の中点は、温度ドリフトの大小にかかわらず常に検出部の中央に位置する。したがって、前記第１及び第２の位置変位の差分と前記所定値とを比較することにより、温度ドリフトの影響を受けることなく、前記部材の原点が正確に維持されているか否かを正確に判定することが可能になる。
【００１２】
請求項６に記載した発明は、第１部材（例えば、後述する実施の形態における大腿部１）と第２部材（例えば、後述する実施の形態における下腿部２）が相対回転可能に連結され、前記第２部材と同期回転する回転体（例えば、後述する実施の形態における回転体４）がモータ（例えば、後述する実施の形態におけるモータ５）により回転駆動され、前記回転体に設けられた被検出部（例えば、後述する実施の形態におけるドック７）を検出する検出手段（例えば、後述する実施の形態における近接スイッチ８）が前記第１部材に設置され、前記モータの回転角度を検出するエンコーダ（例えば、後述する実施の形態におけるアブソリュートエンコーダ１０，インクリメンタルエンコーダ２０）の検出値に基づいて前記第１部材と前記第２部材の相対角度が制御される自動原点確認方法であって、
前記回転体を原点から前記検出手段に向かって回転させて前記被検出部の第１及び第２の端点を検出し、該２つの端点検出位置の中点を算出し、前記原点から前記中点までのオフセット角度（例えば、後述する実施の形態におけるオフセット角度θ０）を予め記憶しておき、
自動原点確認時には、前記の方法で改めて第１及び第２の端点検出位置の中点までのオフセット角度（例えば、後述する実施の形態におけるオフセット角度θａ）を算出し、該オフセット角度と前記予め記憶したオフセット角度の差分値を算出し、該差分値と所定値（例えば、後述する実施の形態におけるβ１）との比較によって、前記第２部材の原点が正確に維持されているか否かを判定することを特徴とする自動原点確認方法である。
【００１３】
第１の端点検出位置と第２の端点検出位置の中点は、温度ドリフトの大小にかかわらず常に検出手段の中央に位置する。したがって、自動原点確認時に算出したオフセット角度と予め記憶しておいたオフセット角度との差分値を前記所定値と比較することにより、温度ドリフトの影響を受けることなく、第２部材の原点が正確に維持されているか否かを正確に判定することが可能になる。
【００１４】
【発明の実施の形態】
以下、この発明に係る自動原点合わせ方法及び自動原点確認方法の一実施の形態を図１から図９の図面を参照して説明する。なお、この実施の形態は、脚式歩行ロボットにおける大腿部と下腿部とを回動可能に連結する関節に適用した態様である。
【００１５】
初めに、図１を参照して、自動原点合わせシステムの構成を説明する。脚式歩行ロボット（以下、単にロボットという）の大腿部（第１部材）１と下腿部（第２部材）２は関節軸３を介して回動可能に連結されており、回転体４の回転が関節軸３を介して下腿部２に伝達され、これにより下腿部２が動くようになっている。この回転体４は大腿部１に設置されたモータ５を駆動源とし、ベルトやギヤあるいは減速機等の伝達手段を介して回転駆動されるようになっている。なお、図１では、ベルト６を伝達手段として図示している。
【００１６】
回転体４には、原点合わせ用の指標である金属板からなるドック（被検出部）７が取り付けられており、このドック７を検出する検出手段としての近接スイッチ８が大腿部１に固定されている。
一方、モータ５には、モータ５の出力軸の絶対位置を検出するアブソリュートエンコーダ１０と、モータ５の出力軸の相対位置を検出するインクリメンタルエンコーダ２０が設けられている。アブソリュートエンコーダ１０は、例えば、３６０度で２０４８パルス発生させるものを使用することができる。インクリメンタルエンコーダ２０は、モータ５の出力軸に固定されたパルス発生部（図示せず）とコントローラ２１からなり、コントローラ２１は、パルス発生部が発生したパルスをカウントするカウンタ２２と、カウンタ２２のカウント値（以下、インクリメンタルエンコーダ値という）を記憶する揮発性メモリＭ１を備えている。これらエンコーダ１０，２０の検出値に基づいて、大腿部１と下腿部２の相対角度が制御されるようになっている。
【００１７】
インクリメンタルエンコーダ２０のコントローラ２１は、バックアップバッテリ３１とメインバッテリ３２から給電可能になっており、メインバッテリ３２がＯＮのときはメインバッテリ３２から給電され、メインバッテリ３２のＯＦＦ時にはバックアップバッテリ３１から給電されるようになっている。
また、インクリメンタルエンコーダ２０のコントローラ２１はメインコントローラ３３に接続されている。メインコントローラ３３は、ロボットの各関節に電気的に接続されていて、人間による指令や、予め与えられたプログラムに従って指令信号を出力する。大腿部１と下腿部２の関節におけるモータ５もこの指令信号に応じて回転駆動され、下腿部２を動かすこととなる。
さらに、前記したアブソリュートエンコーダ１０のアブソリュートエンコーダ値はメインコントローラ３３によって読み出され、不揮発性メモリＭ２に記憶することができるようになっている。メインコントローラ３３はメモリＭ３（ＲＡＭ）を有しており、後述するインクリメンタルエンコーダ値Ｅ１，Ｅ２等を格納することもできる。さらに、メインコントローラ３３は後述するフローチャートの処理を実行する。
【００１８】
この実施の形態の自動原点合わせ方法は、近接スイッチ８の動作特性を巧みに利用しているが、これについて初めに説明する。回転体４が回転したときに近接スイッチ８がドック７を検出する形態には二通りある。すなわち、回転体４が図１において時計回り方向に回転（以下、この回転を正転とする）したときにドック７の始端を検出する形態と、図１において反時計回り方向に回転（以下、この回転を逆転とする）したときにドック７の終端を検出する形態である。
ここで、説明の都合上、回転体４の正転時に近接スイッチ８がドック７の始端７ａを検出しＯＮしたときを第１の端点検出（図２（Ａ）参照）と称し、この第１の端点検出後、さらに回転体４を正転させ近接スイッチ８をＯＦＦとし（図２（Ｂ），（Ｃ）参照）、その後、回転体４を逆転させて、回転体４の逆転時に近接スイッチ８がドック７の終端７ｂを検出したときを第２の端点検出（図２（Ｄ）参照）と称するものとする。
【００１９】
一方、回転体４の回転に伴いドック７が近接スイッチ８に接近していったときに近接スイッチ８がＯＮするタイミングは、近接スイッチ８の磁気特性等に起因する温度ドリフトにより、温度が高いほど早くなる。すなわち、近接スイッチ８から離れた位置でＯＮするようになる。つまり、図３に示すように、近接スイッチ８をＯＮさせる領域（以下、この領域をＯＮ作動領域と称し、ＯＮ作動領域に対応する回転角度をＯＮ作動領域角Ｒと称す）はドック７の全長に対応する角度Ｓだけではなく、これと、ドック７の右側に生じる第１の端点検出時（正転時）のドリフト角ｄ１と、ドック７の左側に生じる第２の端点検出時（逆転時）のドリフト角ｄ２とを加算した角度（Ｒ＝Ｓ＋ｄ１＋ｄ２）になり、温度が高いほどＯＮ作動領域角Ｒが拡大する。なお、正転時のドリフト角ｄ１と逆転時のドリフト角ｄ２はほぼ等しいこと（ｄ１＝ｄ２）が確認されている。
【００２０】
このような温度ドリフト特性を有する近接スイッチ８においては、ｄ１＝ｄ２であることから、ドリフト角ｄ１，ｄ２の大きさが温度によって変化しても、ドリフト角ｄ１，ｄ２を加味したＯＮ作動領域の中点はドック７の中点Ｃ0に常に一致するはずである。この実施の形態の自動原点合わせ方法では、近接スイッチ８のこの特性を利用して、温度ドリフトの影響を受けることなく、高精度に原点チェック（原点確認）及び原点合わせを行うようにしている。
【００２１】
次に、図４、図５，図７，図８に示すフローチャートにしたがって自動原点合わせ制御を説明する。
図４は、全体フローチャートである。まず、ステップＳ１０１において、ロボットの出荷時あるいはロボットの補修直後か否か判定する。ステップＳ１０１における判定結果が「ＹＥＳ」である（出荷時あるいは補修直後である）場合はステップＳ１０２に進み、ロボットの原点合わせを手動で行う。これを初期原点合わせという。したがって、初期原点合わせは最初にロボットを組み立てた後、及び、モータ５やベルト６の交換等のメンテナンスを実施した後に実行される。初期原点合わせは、例えば図９（Ａ）に示すように、ロボットを起立させて大腿部１と下腿部２がほぼ同一直線上に位置したときを原点姿勢とし、所定の原点合わせ用の治具を用いて正確に実行する。この初期原点合わせによる原点位置が真の原点位置となる。
【００２２】
次に、図４のステップＳ１０３に進み、インクリメンタルエンコーダ２０のカウンタ２２のカウント値（検出値）、すなわちインクリメンタルエンコーダ値をリセットして「０」にするとともに、真の原点位置におけるアブソリュートエンコーダ１０の検出値をアブソリュートエンコーダ値の初期値として不揮発性メモリＭ２に記憶する。
【００２３】
次に、ステップＳ１０４に進み、モータ５を正転駆動させて、オフセット角度θ０を算出する。このオフセット角度θ０を求める方法を図５及び図６を参照して説明する。図５はオフセット角度θ０を算出する際の処理の流れを示すフローチャートである。
まず、モータ５を正転駆動し（図５のステップＳ４０１）、次に、近接スイッチ８がＯＮか否か判定する（ステップＳ４０２）。ステップＳ４０２における判定結果が「ＮＯ」（近接スイッチ８がＯＦＦ）である場合はステップＳ４０１に戻ってモータ５の正転駆動を継続する。一方、判定結果が「ＹＥＳ」（近接スイッチ８がＯＮ）である場合はステップＳ４０３に進んで、近接スイッチ８がＯＮした時にメモリＭ１から読み出したインクリメンタルエンコーダ値Ｅ１をメモリＭ３に記憶する。つまり、ステップＳ４０３では、第１の端点検出時（図２（Ａ）の状態）のインクリメンタルエンコーダ値Ｅ１を記憶することになる。
【００２４】
そして、ステップＳ４０３の処理後、さらにモータ５を正転駆動させ（ステップＳ４０４）、近接スイッチ８がＯＦＦか否か判定する（ステップＳ４０５）。ステップＳ４０５における判定結果が「ＮＯ」（近接スイッチ８がＯＮのまま）である場合はステップＳ４０４に戻ってモータ５の正転駆動を継続する。一方、判定結果が「ＹＥＳ」（近接スイッチ８がＯＦＦになった：図２（Ｂ））である場合はステップＳ４０６に進んで、さらにモータ５の正転駆動を継続する。
【００２５】
次に、ステップＳ４０５で「ＹＥＳ」判定してから回転体４を予め設定した所定角度γだけ回転したか否かを判定し（ステップＳ４０７）、判定結果が「ＮＯ」（所定角度γ回転していない）である場合はステップＳ４０６に戻ってモータ５の正転駆動を継続し、判定結果が「ＹＥＳ」（所定角度γ回転した：図２（Ｃ））である場合はステップＳ４０８に進んで、モータ５を逆転駆動する。
【００２６】
次に、近接スイッチ８がＯＮか否か判定し（ステップＳ４０９）、判定結果が「ＮＯ」（近接スイッチ８がＯＦＦのまま）である場合はステップＳ４０８に戻ってモータ５の逆転駆動を継続し、判定結果が「ＹＥＳ」（近接スイッチ８がＯＮ）である場合はステップＳ４１０に進んで、近接スイッチ８がＯＮした時にメモリＭ１から読み出したインクリメンタルエンコーダ値Ｅ２をメモリＭ３に記憶する。つまり、ステップＳ４１０では、第２の端点検出時（図２（Ｄ）の状態）のインクリメンタルエンコーダ値Ｅ２を記憶することになる。
【００２７】
次に、ステップＳ４１１に進んでオフセット角度θ０を算出する。図６に示すように、このオフセット角度θ０はインクリメンタルエンコーダ値Ｅ１及びＥ２の中央値、すなわち、（Ｅ１＋Ｅ２）／２に相当する角度である。この算出されたオフセット角度θ０は、図４のステップＳ１０５においてメモリＭ２に記憶される。なお、ステップＳ１０５の処理が終了した後は本ルーチンは終了となる。
【００２８】
一方、図４のステップＳ１０１における判定結果が「ＮＯ」である（出荷時あるいは補修直後でない）場合は、ステップＳ１０６に進む。
ステップＳ１０６においてメインバッテリ３２がＯＮされると、ステップＳ１０７に進み、原点情報がインクリメンタルエンコーダ値として保持されているか否か判定する。原点情報が保持されているか否かは、バックアップバッテリ３１の電圧が所定値以上あるか否かで判定することができる。すなわち、バックアップバッテリ３１の電圧が所定値以下の時には揮発性メモリＭ１に記憶されていたインクリメンタルエンコーダ値が消失していると考えられるので、原点情報が保持されていないと判定し、バックアップバッテリ３１の電圧が所定値を越えている時には揮発性メモリＭ１に記憶されていたインクリメンタルエンコーダ値が消失されずに残っていると考えられるので、原点情報が保持されていると判定する。
【００２９】
ステップＳ１０７における判定結果が「ＹＥＳ」である（原点情報が保持されている）場合は、Ｓ２００に進んで原点チェック（原点確認）処理を実行し、さらにステップＳ１０８に進んで原点チェック処理の結果が「ＯＫ」（良好）か否か判定し、ステップＳ１０８における判定結果が「ＹＥＳ」（ＯＫ）である場合は、本ルーチンの実行を一旦終了する。
【００３０】
一方、ステップＳ１０７における判定結果が「ＮＯ」である（原点情報が保持されていない）場合、及び、ステップＳ１０８における判定結果が「ＮＯ」（原点チェックの結果がＮＧ）である場合には、ステップＳ３００に進んで自動原点合わせ処理を実行した後、本ルーチンの実行を一旦終了する。
本ルーチンの実行終了により、関節の原点合わせが完了したことになる。したがって、センサやメインバッテリ３２の容量等の他のシステムチェックの結果が良好な場合には、プログラムや通信指令に基づいてロボットは歩行などの動作が可能となる。
【００３１】
次に、原点チェック処理（図４のステップＳ２００）を図７を参照して説明する。
まず、ステップＳ４００では図５のフローに従い改めてオフセット角度を算出する。なお、説明の便宜上、ステップＳ４００で算出されたオフセット角度をオフセット角度θａとする。また、オフセット角度θａの算出開始時におけるドック７の開始位置（始端７ａ、終端７ｂ、ドック７の中点いずれを用いてもよい）を「仮原点位置」とし、図４のステップＳ１０２でセットされた時のドック７の位置を「真の原点位置」と称す。
【００３２】
ステップＳ４００で算出されたオフセット角度θａは、メモリＭ３に記憶される（ステップＳ２０１）。ステップＳ２０１の処理が終了した時点では、ドック７は図６に示すように第２の端点（終端）検出位置にある。
ステップＳ２０１に続くステップＳ２０２では、図４のステップＳ１０５で記憶されたオフセット角度θ０がメモリＭ２から読み出される。そして、次にステップＳ２０３の処理が実行される。
【００３３】
ステップＳ２０３では、オフセット角度θ０とオフセット角度θａとが比較される。そして、差分｜θａ−θ０｜が所定値β１未満の場合は、下腿部２の原点ずれは許容範囲内に維持されており、原点チェックの結果は「ＯＫ」であると判断され（ステップＳ２０４）、本ルーチンは終了となる。
一方、前記差分が所定値β１以上である場合は、原点チェックの結果は「ＮＧ」であると判断され（ステップＳ２０５）、本ルーチンは終了となり、続いて図４のステップＳ３００に示す自動原点合わせの処理が実行される。
【００３４】
次に、自動原点合わせ処理（ステップＳ３００）を図８にしたがって説明する。
前述したように、自動原点合わせ処理（ステップＳ３００）は、ステップＳ１０７における判定結果が「ＮＯ」であった場合（すなわち、揮発性メモリＭ１に記憶されていたデータが消失して原点保持がされていないとき）、あるいは、ステップＳ１０８における判定結果が「ＮＯ」であった場合（すなわち、原点チェックの結果が「ＮＧ」であったとき）に実行される。
【００３５】
自動原点合わせ処理では、初めに、インクリメンタルエンコーダ値をリセットして「０」にする（ステップＳ３０１）。
次に、ステップＳ３０２に進んで、改めて仮原点位置から、オフセット角度θａ算出処理を実行する。オフセット角度θａ算出処理は、前述した原点チェック処理（図５のステップ）で実行したオフセット角度θａ算出処理と同じであるので、ここでの説明は省略する。
なお、オフセット角度θａ算出処理の実行を終了した時点において、回転体４は第２の端点（終端）検出位置にある。
【００３６】
オフセット角度θａ算出処理（ステップＳ３０２）を実行してオフセット角度θａを算出した後、モータ５を逆転駆動して、回転体４をθａ−θ０の位置に戻して（ステップＳ３０３）、そのときのアブソリュートエンコーダ１０のアブソリュートエンコーダ値を読み込む（ステップＳ３０４）。例えば、θ０＝９０゜，θａ＝７０゜の場合、回転体４はマイナス２０゜の位置、すなわち、真の原点位置に合わせられる。
【００３７】
次に、ステップＳ３０５に進み、予め記憶しておいた真の原点位置におけるアブソリュートエンコーダ値とステップＳ３０４において読み込んだアブソリュートエンコーダ値との差を算出し、この差が予め設定した所定範囲β２にあるか否か判定する（ステップＳ３０６）。
ステップＳ３０６における判定結果が「ＹＥＳ」（｜差｜＜β２）である場合は、真の原点位置におけるアブソリュートエンコーダ値とステップＳ３０４において読み込んだアブソリュートエンコーダ値の差は許容誤差範囲内であり、回転体４が原点に位置していると判断して、本ルーチンの実行を一旦終了する。
【００３８】
一方、ステップＳ３０６における判定結果が「ＮＯ」（｜差｜≧β２）である場合は、真の原点位置におけるアブソリュートエンコーダ値とステップＳ３０４において読み込んだアブソリュートエンコーダ値の差が許容誤差範囲から外れており、回転体４が原点に位置していないと判断して、原点補正を行う（ステップＳ３０７）。
この実施の形態の場合、原点補正は、アブソリュートエンコーダ値の差がゼロになるようにモータ５を回転駆動し、差がゼロになったときにインクリメンタルエンコーダ２０をリセットして、インクリメンタルエンコーダ値を「０」にする。
【００３９】
このように、この実施の形態の自動原点合わせ方法、及び、自動原点確認方法によれば、温度ドリフトの影響を受けることなく原点チェックを高精度に行うことができる。また、原点チェックをした結果、原点が狂っていたときや、揮発性メモリＭ１に記憶されていたデータが消失したときには、温度ドリフトの影響を受けることなく原点合わせ（原点出し）を高精度に行うことができる。
【００４０】
〔他の実施の形態〕
なお、この発明は前述した実施の形態に限られるものではない。
例えば、インクリメンタルエンコーダ２０をリセットする場合に、前述した実施の形態ではインクリメンタルエンコーダ値を「０」にしたが、その時のアブソリュートエンコーダ１０のアブソリュートエンコーダ値をインクリメンタルエンコーダ値にセットするようにしてもよい。
【００４１】
また、インクリメンタルエンコーダ２０のリセットでアブソリュートエンコーダ値をセットするようにしたときには、自動原点合わせ処理（ステップＳ３００）において原点補正（ステップＳ３０７）を行う場合に、記憶しているアブソリュートエンコーダ値をインクリメンタルエンコーダ値にセットすることで原点補正を行うようにしてもよい。このようにすると、原点補正において誤差分を回転駆動する必要がなくなり、誤差が生じなくなる。
【００４２】
さらに、前述した実施の形態では、第１の端点検出と第２の端点検出を近接スイッチ８がＯＦＦからＯＮになるときとしたが、これと逆のＯＮからＯＦＦになるときを検出端点としてもよい。また、近接スイッチ８がＯＮからＯＦＦになるときとＯＦＦからＯＮになるときのヒステリシス誤差が無視できる程度である場合には、近接スイッチ８がＯＦＦからＯＮになるときを第１の端点検出位置とし、その後、ＯＦＦからＯＮになるときを第２の端点検出位置として、その中点を基準にして、オフセット角度θａ，θ０を算出してもよい。
【００４３】
なお、この発明における原点チェック方法は、図４の処理全体が終了した後、すなわち、ロボットの動作時においても使用することができる。例えば、歩行における関節屈折時に２つの端点を検知できるようにスイッチ等を設置すれば、原点ずれが生じたことを指令者に通知したり、動作を強制停止させることでロボットの転倒等の不正制御を回避することができる。
また、この発明の原理は、回転運動を行うドック７と近接スイッチ８の組み合わせに限らず、直線運動等の他の平面運動や立体運動を行う被検出部を２点以上で検出手段が検出して、高精度な温度ドリフト補正に生かす構成としてもよい。
【００４４】
【発明の効果】
以上説明するように、請求項１に記載した発明によれば、温度ドリフトの影響を受けることなく、高精度に原点合わせを行うことができるという優れた効果が奏される。
請求項２に記載した発明によれば、さらに高精度に原点合わせを行うことができるという効果が奏される。
請求項３に記載した発明によれば、温度ドリフトの影響を受けることなく、第２部材に対し高精度に原点合わせを行うことができるという優れた効果が奏される。
【００４５】
請求項４に記載した発明によれば、第２部材に対しさらに高精度に原点合わせを行うことができるという効果が奏される。
請求項５に記載した発明によれば、温度ドリフトの影響を受けることなく、部材の原点が正確に維持されているか否かを正確に判定することができるという優れた効果が奏される。
請求項６に記載した発明によれば、温度ドリフトの影響を受けることなく、第２部材の原点が正確に維持されているか否かを正確に判定することができるという優れた効果が奏される。
【図面の簡単な説明】
【図１】 この発明に係る自動原点合わせ方法及び自動原点確認方法が適用可能な関節の一実施の形態におけるシステム構成図である。
【図２】 前記実施の形態における第１及び第２の端点検出方法を説明する図である。
【図３】 近接スイッチのＯＮ作動領域を説明する図である。
【図４】 前記実施の形態における自動原点合わせ制御の全体フローチャートである。
【図５】 前記実施の形態におけるオフセット角度θ０算出処理のフローチャートである。
【図６】 前記実施の形態におけるオフセット角度θ０，θａの算出方法を説明する図である。
【図７】 前記実施の形態における原点チェック処理のフローチャートである。
【図８】 前記実施の形態における自動原点合わせ処理のフローチャートである。
【図９】 前記実施の形態における初期原点合わせを説明する図である。
【符号の説明】
１ 大腿部（第１部材）
２ 下腿部（第２部材）
３ 関節軸
４ 回転体
５ モータ
７ ドック（被検出部）
７ａ 始端
７ｂ 終端
８ 近接スイッチ（検出部、検出手段）
１０ アブソリュートエンコーダ
２０ インクリメンタルエンコーダ
Ｍ１，Ｍ２，Ｍ３ メモリ
θ０，θａ オフセット角度（位置変位）
【発明の属する技術分野】
この発明は、関節等の自動原点合わせ方法及び自動原点確認方法に係り、特に、温度ドリフトの影響を受けずに高精度な自動原点合わせ及び自動原点確認を実現可能にする自動原点合わせ方法及び自動原点確認方法に関する。
【０００２】
【従来の技術】
ロボット等の関節の制御では、予め決められた原点位置から相対的に関節を移動させている。したがって、必要に応じて原点合わせを正確に行う必要がある。また、温度ドリフトや機構の誤差によって原点検出のずれなどが生じる場合があるので、その補償を行う必要もある。特に、特開平５−２３７７７５号公報等に開示された自立移動する脚式歩行ロボットのように精密な制御が必要なものほど、原点合わせも高精度に行えるようにする必要がある。
なお、温度ドリフトとは、近接スイッチなど位置検出手段の温度特性などによって生じる現象の一種であり、温度によって位置検出手段による検出位置が変わることをいう。
【０００３】
【発明が解決しようとする課題】
この原点合わせを必要に応じて毎回人間が治具を用いて行うことも可能であるが、その作業は繁雑で時間がかかり、早急に起動したい時などに対応できないこともある。したがって、自動原点合わせ機構が必要不可欠である。
従来の自動原点合わせ方法として、予め合わせた原点から近接スイッチが感知するまでの角度と、原点位置におけるアブソリュートエンコーダ値をメモリに記憶しておき、次回の原点合わせ時に近接スイッチの感知位置から前記角度だけ関節を戻す方法がある。この方法では、戻された位置のアブソリュートエンコーダ値と予め記憶されたアブソリュートエンコーダ値とが比較され、その差分だけさらに原点へ合わせる補正が実行される。しかしながら、モータの高出力化に伴い関節部の温度上昇が以前より大きくなるにしたがって、温度ドリフトが増大し、高精度な原点合わせが困難になった。
【０００４】
例えば、３００倍の減速比で関節を回転させる場合、モータの回転角度は関節の角度変化の３００倍となるが、温度ドリフトが増大しているときに前記方法で原点合わせを行うと、モータにおける回転角度で半回転分以上のずれが原点に生じる場合もある。このような場合は、アブソリュートエンコーダ値がどちらにずれたかが分からないので、高精度な自動原点合わせができない。
そこで、この発明は、温度ドリフトの影響を受けず、高精度に自動原点合わせ及び自動原点確認が可能な自動原点合わせ方法及び自動原点確認方法を提供するものである。
【０００５】
【課題を解決するための手段】
上記課題を解決するために、請求項１に記載した発明は、被検出部（例えば、後述する実施の形態におけるドック７）を備えた部材（例えば、後述する実施の形態における下腿部２）を原点から検出部（例えば、後述する実施の形態における近接スイッチ８）に向かって複数方向から接近させて複数の検出位置を検出し、前記原点から前記複数の検出位置の中点までの第１の位置変位（例えば、後述する実施の形態におけるオフセット角度θ０）を予め記憶しておき、
自動原点合わせ時には、前記方法で改めて複数の検出位置の中点までの第２の位置変位（例えば、後述する実施の形態におけるオフセット角度θａ）を検出し、前記第１及び第２の位置変位を比較し、その差分を補正して前記部材を前記原点まで移動させることを特徴とする自動原点合わせ方法である。
【０００６】
検出した複数の検出位置の中点は、温度ドリフトの大小にかかわらず常に検出部の中央に位置する。したがって、前記第１及び第２の位置変位を比較することにより、温度ドリフトの影響を受けずに原点のずれ（差）を検知することができ、そのずれ（差）を補正して前記部材を原点に移動させているので、原点を高精度に合わせることが可能になる。
【０００７】
請求項２に記載した発明は、請求項１記載の自動原点合わせ方法であって、
前記部材の前記原点における位置情報（例えば、後述する実施の形態におけるアブソリュートエンコーダ値）も予め記憶しておき、
自動原点合わせ動作によって移動した後の前記部材の位置情報と前記予め記憶しておいた位置情報とを比較し、その差分だけ前記部材をさらに前記原点方向に移動させることを特徴とする自動原点合わせ方法である。
このように構成することにより、原点における位置情報を比較してさらに補正をしているので、さらに高精度に原点を合わせることが可能になる。
【０００８】
請求項３に記載した発明は、第１部材（例えば、後述する実施の形態における大腿部１）と第２部材（例えば、後述する実施の形態における下腿部２）が相対回転可能に連結され、前記第２部材と同期回転する回転体（例えば、後述する実施の形態における回転体４）がモータ（例えば、後述する実施の形態におけるモータ５）により回転駆動され、前記回転体に設けられた被検出部（例えば、後述する実施の形態におけるドック７）を検出する検出手段（例えば、後述する実施の形態における近接スイッチ８）が前記第１部材に設置され、前記モータの回転角度を検出するエンコーダ（例えば、後述する実施の形態におけるアブソリュートエンコーダ１０，インクリメンタルエンコーダ２０）の検出値に基づいて前記第１部材と前記第２部材の相対角度が制御される自動原点合わせ方法であって、前記回転体を原点から前記検出手段に向かって回転させて前記被検出部の第１及び第２の端点を検出し、該２つの端点検出位置の中点を算出し、前記原点から前記中点までのオフセット角度（例えば、後述する実施の形態におけるオフセット角度θ０）を予め記憶しておき、
自動原点合わせ時には、前記の方法で改めて第１及び第２の端点検出位置の中点までのオフセット角度（例えば、後述する実施の形態におけるオフセット角度θａ）を算出し、該オフセット角度と前記予め記憶したオフセット角度とを比較し、その差分を補正して前記第２部材を前記原点まで移動させることを特徴とする自動原点合わせ方法である。
【０００９】
第１の端点検出位置と第２の端点検出位置の中点は、温度ドリフトの大小にかかわらず常に検出手段の中央に位置する。したがって、自動原点合わせ時に算出したオフセット角度と予め記憶しておいたオフセット角度とを比較することにより、温度ドリフトの影響を受けずに原点のずれ（差）を検知することができ、そのずれ（差）を補正して前記第２部材を原点に移動させているので、第２部材を原点に高精度に合わせることが可能になる。
【００１０】
請求項４に記載した発明は、請求項３記載の自動原点合わせ方法であって、
前記第２部材の前記原点におけるエンコーダ値（例えば、後述する実施の形態におけるアブソリュートエンコーダ値）も予め記憶しておき、
自動原点合わせ動作によって移動した後の前記第２部材のエンコーダ値と前記予め記憶しておいたエンコーダ値とを比較し、その差分だけ前記第２部材をさらに前記原点方向に移動させることを特徴とする自動原点合わせ方法である。
このように構成することにより、原点におけるエンコーダ値を比較してさらに補正をしているので、第２部材を原点にさらに高精度に合わせることが可能になる。
【００１１】
請求項５に記載した発明は、被検出部（例えば、後述する実施の形態におけるドック７）を備えた部材（例えば、後述する実施の形態における下腿部２）を原点から検出部（例えば、後述する実施の形態における近接スイッチ８）に向かって複数方向から接近させて複数の検出位置を検出し、前記原点から前記複数の検出位置の中点までの第１の位置変位（例えば、後述する実施の形態におけるオフセット角度θ０）を予め記憶しておき、
自動原点確認時には、前記方法で改めて複数の検出位置の中点までの第２の位置変位（例えば、後述する実施の形態におけるオフセット角度θａ）を検出し、前記第１及び第２の位置変位の差分と所定値（例えば、後述する実施の形態におけるβ１）との比較によって、前記部材の原点が正確に維持されているか否かを判定することを特徴とする自動原点確認方法である。
検出した複数の検出位置の中点は、温度ドリフトの大小にかかわらず常に検出部の中央に位置する。したがって、前記第１及び第２の位置変位の差分と前記所定値とを比較することにより、温度ドリフトの影響を受けることなく、前記部材の原点が正確に維持されているか否かを正確に判定することが可能になる。
【００１２】
請求項６に記載した発明は、第１部材（例えば、後述する実施の形態における大腿部１）と第２部材（例えば、後述する実施の形態における下腿部２）が相対回転可能に連結され、前記第２部材と同期回転する回転体（例えば、後述する実施の形態における回転体４）がモータ（例えば、後述する実施の形態におけるモータ５）により回転駆動され、前記回転体に設けられた被検出部（例えば、後述する実施の形態におけるドック７）を検出する検出手段（例えば、後述する実施の形態における近接スイッチ８）が前記第１部材に設置され、前記モータの回転角度を検出するエンコーダ（例えば、後述する実施の形態におけるアブソリュートエンコーダ１０，インクリメンタルエンコーダ２０）の検出値に基づいて前記第１部材と前記第２部材の相対角度が制御される自動原点確認方法であって、
前記回転体を原点から前記検出手段に向かって回転させて前記被検出部の第１及び第２の端点を検出し、該２つの端点検出位置の中点を算出し、前記原点から前記中点までのオフセット角度（例えば、後述する実施の形態におけるオフセット角度θ０）を予め記憶しておき、
自動原点確認時には、前記の方法で改めて第１及び第２の端点検出位置の中点までのオフセット角度（例えば、後述する実施の形態におけるオフセット角度θａ）を算出し、該オフセット角度と前記予め記憶したオフセット角度の差分値を算出し、該差分値と所定値（例えば、後述する実施の形態におけるβ１）との比較によって、前記第２部材の原点が正確に維持されているか否かを判定することを特徴とする自動原点確認方法である。
【００１３】
第１の端点検出位置と第２の端点検出位置の中点は、温度ドリフトの大小にかかわらず常に検出手段の中央に位置する。したがって、自動原点確認時に算出したオフセット角度と予め記憶しておいたオフセット角度との差分値を前記所定値と比較することにより、温度ドリフトの影響を受けることなく、第２部材の原点が正確に維持されているか否かを正確に判定することが可能になる。
【００１４】
【発明の実施の形態】
以下、この発明に係る自動原点合わせ方法及び自動原点確認方法の一実施の形態を図１から図９の図面を参照して説明する。なお、この実施の形態は、脚式歩行ロボットにおける大腿部と下腿部とを回動可能に連結する関節に適用した態様である。
【００１５】
初めに、図１を参照して、自動原点合わせシステムの構成を説明する。脚式歩行ロボット（以下、単にロボットという）の大腿部（第１部材）１と下腿部（第２部材）２は関節軸３を介して回動可能に連結されており、回転体４の回転が関節軸３を介して下腿部２に伝達され、これにより下腿部２が動くようになっている。この回転体４は大腿部１に設置されたモータ５を駆動源とし、ベルトやギヤあるいは減速機等の伝達手段を介して回転駆動されるようになっている。なお、図１では、ベルト６を伝達手段として図示している。
【００１６】
回転体４には、原点合わせ用の指標である金属板からなるドック（被検出部）７が取り付けられており、このドック７を検出する検出手段としての近接スイッチ８が大腿部１に固定されている。
一方、モータ５には、モータ５の出力軸の絶対位置を検出するアブソリュートエンコーダ１０と、モータ５の出力軸の相対位置を検出するインクリメンタルエンコーダ２０が設けられている。アブソリュートエンコーダ１０は、例えば、３６０度で２０４８パルス発生させるものを使用することができる。インクリメンタルエンコーダ２０は、モータ５の出力軸に固定されたパルス発生部（図示せず）とコントローラ２１からなり、コントローラ２１は、パルス発生部が発生したパルスをカウントするカウンタ２２と、カウンタ２２のカウント値（以下、インクリメンタルエンコーダ値という）を記憶する揮発性メモリＭ１を備えている。これらエンコーダ１０，２０の検出値に基づいて、大腿部１と下腿部２の相対角度が制御されるようになっている。
【００１７】
インクリメンタルエンコーダ２０のコントローラ２１は、バックアップバッテリ３１とメインバッテリ３２から給電可能になっており、メインバッテリ３２がＯＮのときはメインバッテリ３２から給電され、メインバッテリ３２のＯＦＦ時にはバックアップバッテリ３１から給電されるようになっている。
また、インクリメンタルエンコーダ２０のコントローラ２１はメインコントローラ３３に接続されている。メインコントローラ３３は、ロボットの各関節に電気的に接続されていて、人間による指令や、予め与えられたプログラムに従って指令信号を出力する。大腿部１と下腿部２の関節におけるモータ５もこの指令信号に応じて回転駆動され、下腿部２を動かすこととなる。
さらに、前記したアブソリュートエンコーダ１０のアブソリュートエンコーダ値はメインコントローラ３３によって読み出され、不揮発性メモリＭ２に記憶することができるようになっている。メインコントローラ３３はメモリＭ３（ＲＡＭ）を有しており、後述するインクリメンタルエンコーダ値Ｅ１，Ｅ２等を格納することもできる。さらに、メインコントローラ３３は後述するフローチャートの処理を実行する。
【００１８】
この実施の形態の自動原点合わせ方法は、近接スイッチ８の動作特性を巧みに利用しているが、これについて初めに説明する。回転体４が回転したときに近接スイッチ８がドック７を検出する形態には二通りある。すなわち、回転体４が図１において時計回り方向に回転（以下、この回転を正転とする）したときにドック７の始端を検出する形態と、図１において反時計回り方向に回転（以下、この回転を逆転とする）したときにドック７の終端を検出する形態である。
ここで、説明の都合上、回転体４の正転時に近接スイッチ８がドック７の始端７ａを検出しＯＮしたときを第１の端点検出（図２（Ａ）参照）と称し、この第１の端点検出後、さらに回転体４を正転させ近接スイッチ８をＯＦＦとし（図２（Ｂ），（Ｃ）参照）、その後、回転体４を逆転させて、回転体４の逆転時に近接スイッチ８がドック７の終端７ｂを検出したときを第２の端点検出（図２（Ｄ）参照）と称するものとする。
【００１９】
一方、回転体４の回転に伴いドック７が近接スイッチ８に接近していったときに近接スイッチ８がＯＮするタイミングは、近接スイッチ８の磁気特性等に起因する温度ドリフトにより、温度が高いほど早くなる。すなわち、近接スイッチ８から離れた位置でＯＮするようになる。つまり、図３に示すように、近接スイッチ８をＯＮさせる領域（以下、この領域をＯＮ作動領域と称し、ＯＮ作動領域に対応する回転角度をＯＮ作動領域角Ｒと称す）はドック７の全長に対応する角度Ｓだけではなく、これと、ドック７の右側に生じる第１の端点検出時（正転時）のドリフト角ｄ１と、ドック７の左側に生じる第２の端点検出時（逆転時）のドリフト角ｄ２とを加算した角度（Ｒ＝Ｓ＋ｄ１＋ｄ２）になり、温度が高いほどＯＮ作動領域角Ｒが拡大する。なお、正転時のドリフト角ｄ１と逆転時のドリフト角ｄ２はほぼ等しいこと（ｄ１＝ｄ２）が確認されている。
【００２０】
このような温度ドリフト特性を有する近接スイッチ８においては、ｄ１＝ｄ２であることから、ドリフト角ｄ１，ｄ２の大きさが温度によって変化しても、ドリフト角ｄ１，ｄ２を加味したＯＮ作動領域の中点はドック７の中点Ｃ0に常に一致するはずである。この実施の形態の自動原点合わせ方法では、近接スイッチ８のこの特性を利用して、温度ドリフトの影響を受けることなく、高精度に原点チェック（原点確認）及び原点合わせを行うようにしている。
【００２１】
次に、図４、図５，図７，図８に示すフローチャートにしたがって自動原点合わせ制御を説明する。
図４は、全体フローチャートである。まず、ステップＳ１０１において、ロボットの出荷時あるいはロボットの補修直後か否か判定する。ステップＳ１０１における判定結果が「ＹＥＳ」である（出荷時あるいは補修直後である）場合はステップＳ１０２に進み、ロボットの原点合わせを手動で行う。これを初期原点合わせという。したがって、初期原点合わせは最初にロボットを組み立てた後、及び、モータ５やベルト６の交換等のメンテナンスを実施した後に実行される。初期原点合わせは、例えば図９（Ａ）に示すように、ロボットを起立させて大腿部１と下腿部２がほぼ同一直線上に位置したときを原点姿勢とし、所定の原点合わせ用の治具を用いて正確に実行する。この初期原点合わせによる原点位置が真の原点位置となる。
【００２２】
次に、図４のステップＳ１０３に進み、インクリメンタルエンコーダ２０のカウンタ２２のカウント値（検出値）、すなわちインクリメンタルエンコーダ値をリセットして「０」にするとともに、真の原点位置におけるアブソリュートエンコーダ１０の検出値をアブソリュートエンコーダ値の初期値として不揮発性メモリＭ２に記憶する。
【００２３】
次に、ステップＳ１０４に進み、モータ５を正転駆動させて、オフセット角度θ０を算出する。このオフセット角度θ０を求める方法を図５及び図６を参照して説明する。図５はオフセット角度θ０を算出する際の処理の流れを示すフローチャートである。
まず、モータ５を正転駆動し（図５のステップＳ４０１）、次に、近接スイッチ８がＯＮか否か判定する（ステップＳ４０２）。ステップＳ４０２における判定結果が「ＮＯ」（近接スイッチ８がＯＦＦ）である場合はステップＳ４０１に戻ってモータ５の正転駆動を継続する。一方、判定結果が「ＹＥＳ」（近接スイッチ８がＯＮ）である場合はステップＳ４０３に進んで、近接スイッチ８がＯＮした時にメモリＭ１から読み出したインクリメンタルエンコーダ値Ｅ１をメモリＭ３に記憶する。つまり、ステップＳ４０３では、第１の端点検出時（図２（Ａ）の状態）のインクリメンタルエンコーダ値Ｅ１を記憶することになる。
【００２４】
そして、ステップＳ４０３の処理後、さらにモータ５を正転駆動させ（ステップＳ４０４）、近接スイッチ８がＯＦＦか否か判定する（ステップＳ４０５）。ステップＳ４０５における判定結果が「ＮＯ」（近接スイッチ８がＯＮのまま）である場合はステップＳ４０４に戻ってモータ５の正転駆動を継続する。一方、判定結果が「ＹＥＳ」（近接スイッチ８がＯＦＦになった：図２（Ｂ））である場合はステップＳ４０６に進んで、さらにモータ５の正転駆動を継続する。
【００２５】
次に、ステップＳ４０５で「ＹＥＳ」判定してから回転体４を予め設定した所定角度γだけ回転したか否かを判定し（ステップＳ４０７）、判定結果が「ＮＯ」（所定角度γ回転していない）である場合はステップＳ４０６に戻ってモータ５の正転駆動を継続し、判定結果が「ＹＥＳ」（所定角度γ回転した：図２（Ｃ））である場合はステップＳ４０８に進んで、モータ５を逆転駆動する。
【００２６】
次に、近接スイッチ８がＯＮか否か判定し（ステップＳ４０９）、判定結果が「ＮＯ」（近接スイッチ８がＯＦＦのまま）である場合はステップＳ４０８に戻ってモータ５の逆転駆動を継続し、判定結果が「ＹＥＳ」（近接スイッチ８がＯＮ）である場合はステップＳ４１０に進んで、近接スイッチ８がＯＮした時にメモリＭ１から読み出したインクリメンタルエンコーダ値Ｅ２をメモリＭ３に記憶する。つまり、ステップＳ４１０では、第２の端点検出時（図２（Ｄ）の状態）のインクリメンタルエンコーダ値Ｅ２を記憶することになる。
【００２７】
次に、ステップＳ４１１に進んでオフセット角度θ０を算出する。図６に示すように、このオフセット角度θ０はインクリメンタルエンコーダ値Ｅ１及びＥ２の中央値、すなわち、（Ｅ１＋Ｅ２）／２に相当する角度である。この算出されたオフセット角度θ０は、図４のステップＳ１０５においてメモリＭ２に記憶される。なお、ステップＳ１０５の処理が終了した後は本ルーチンは終了となる。
【００２８】
一方、図４のステップＳ１０１における判定結果が「ＮＯ」である（出荷時あるいは補修直後でない）場合は、ステップＳ１０６に進む。
ステップＳ１０６においてメインバッテリ３２がＯＮされると、ステップＳ１０７に進み、原点情報がインクリメンタルエンコーダ値として保持されているか否か判定する。原点情報が保持されているか否かは、バックアップバッテリ３１の電圧が所定値以上あるか否かで判定することができる。すなわち、バックアップバッテリ３１の電圧が所定値以下の時には揮発性メモリＭ１に記憶されていたインクリメンタルエンコーダ値が消失していると考えられるので、原点情報が保持されていないと判定し、バックアップバッテリ３１の電圧が所定値を越えている時には揮発性メモリＭ１に記憶されていたインクリメンタルエンコーダ値が消失されずに残っていると考えられるので、原点情報が保持されていると判定する。
【００２９】
ステップＳ１０７における判定結果が「ＹＥＳ」である（原点情報が保持されている）場合は、Ｓ２００に進んで原点チェック（原点確認）処理を実行し、さらにステップＳ１０８に進んで原点チェック処理の結果が「ＯＫ」（良好）か否か判定し、ステップＳ１０８における判定結果が「ＹＥＳ」（ＯＫ）である場合は、本ルーチンの実行を一旦終了する。
【００３０】
一方、ステップＳ１０７における判定結果が「ＮＯ」である（原点情報が保持されていない）場合、及び、ステップＳ１０８における判定結果が「ＮＯ」（原点チェックの結果がＮＧ）である場合には、ステップＳ３００に進んで自動原点合わせ処理を実行した後、本ルーチンの実行を一旦終了する。
本ルーチンの実行終了により、関節の原点合わせが完了したことになる。したがって、センサやメインバッテリ３２の容量等の他のシステムチェックの結果が良好な場合には、プログラムや通信指令に基づいてロボットは歩行などの動作が可能となる。
【００３１】
次に、原点チェック処理（図４のステップＳ２００）を図７を参照して説明する。
まず、ステップＳ４００では図５のフローに従い改めてオフセット角度を算出する。なお、説明の便宜上、ステップＳ４００で算出されたオフセット角度をオフセット角度θａとする。また、オフセット角度θａの算出開始時におけるドック７の開始位置（始端７ａ、終端７ｂ、ドック７の中点いずれを用いてもよい）を「仮原点位置」とし、図４のステップＳ１０２でセットされた時のドック７の位置を「真の原点位置」と称す。
【００３２】
ステップＳ４００で算出されたオフセット角度θａは、メモリＭ３に記憶される（ステップＳ２０１）。ステップＳ２０１の処理が終了した時点では、ドック７は図６に示すように第２の端点（終端）検出位置にある。
ステップＳ２０１に続くステップＳ２０２では、図４のステップＳ１０５で記憶されたオフセット角度θ０がメモリＭ２から読み出される。そして、次にステップＳ２０３の処理が実行される。
【００３３】
ステップＳ２０３では、オフセット角度θ０とオフセット角度θａとが比較される。そして、差分｜θａ−θ０｜が所定値β１未満の場合は、下腿部２の原点ずれは許容範囲内に維持されており、原点チェックの結果は「ＯＫ」であると判断され（ステップＳ２０４）、本ルーチンは終了となる。
一方、前記差分が所定値β１以上である場合は、原点チェックの結果は「ＮＧ」であると判断され（ステップＳ２０５）、本ルーチンは終了となり、続いて図４のステップＳ３００に示す自動原点合わせの処理が実行される。
【００３４】
次に、自動原点合わせ処理（ステップＳ３００）を図８にしたがって説明する。
前述したように、自動原点合わせ処理（ステップＳ３００）は、ステップＳ１０７における判定結果が「ＮＯ」であった場合（すなわち、揮発性メモリＭ１に記憶されていたデータが消失して原点保持がされていないとき）、あるいは、ステップＳ１０８における判定結果が「ＮＯ」であった場合（すなわち、原点チェックの結果が「ＮＧ」であったとき）に実行される。
【００３５】
自動原点合わせ処理では、初めに、インクリメンタルエンコーダ値をリセットして「０」にする（ステップＳ３０１）。
次に、ステップＳ３０２に進んで、改めて仮原点位置から、オフセット角度θａ算出処理を実行する。オフセット角度θａ算出処理は、前述した原点チェック処理（図５のステップ）で実行したオフセット角度θａ算出処理と同じであるので、ここでの説明は省略する。
なお、オフセット角度θａ算出処理の実行を終了した時点において、回転体４は第２の端点（終端）検出位置にある。
【００３６】
オフセット角度θａ算出処理（ステップＳ３０２）を実行してオフセット角度θａを算出した後、モータ５を逆転駆動して、回転体４をθａ−θ０の位置に戻して（ステップＳ３０３）、そのときのアブソリュートエンコーダ１０のアブソリュートエンコーダ値を読み込む（ステップＳ３０４）。例えば、θ０＝９０゜，θａ＝７０゜の場合、回転体４はマイナス２０゜の位置、すなわち、真の原点位置に合わせられる。
【００３７】
次に、ステップＳ３０５に進み、予め記憶しておいた真の原点位置におけるアブソリュートエンコーダ値とステップＳ３０４において読み込んだアブソリュートエンコーダ値との差を算出し、この差が予め設定した所定範囲β２にあるか否か判定する（ステップＳ３０６）。
ステップＳ３０６における判定結果が「ＹＥＳ」（｜差｜＜β２）である場合は、真の原点位置におけるアブソリュートエンコーダ値とステップＳ３０４において読み込んだアブソリュートエンコーダ値の差は許容誤差範囲内であり、回転体４が原点に位置していると判断して、本ルーチンの実行を一旦終了する。
【００３８】
一方、ステップＳ３０６における判定結果が「ＮＯ」（｜差｜≧β２）である場合は、真の原点位置におけるアブソリュートエンコーダ値とステップＳ３０４において読み込んだアブソリュートエンコーダ値の差が許容誤差範囲から外れており、回転体４が原点に位置していないと判断して、原点補正を行う（ステップＳ３０７）。
この実施の形態の場合、原点補正は、アブソリュートエンコーダ値の差がゼロになるようにモータ５を回転駆動し、差がゼロになったときにインクリメンタルエンコーダ２０をリセットして、インクリメンタルエンコーダ値を「０」にする。
【００３９】
このように、この実施の形態の自動原点合わせ方法、及び、自動原点確認方法によれば、温度ドリフトの影響を受けることなく原点チェックを高精度に行うことができる。また、原点チェックをした結果、原点が狂っていたときや、揮発性メモリＭ１に記憶されていたデータが消失したときには、温度ドリフトの影響を受けることなく原点合わせ（原点出し）を高精度に行うことができる。
【００４０】
〔他の実施の形態〕
なお、この発明は前述した実施の形態に限られるものではない。
例えば、インクリメンタルエンコーダ２０をリセットする場合に、前述した実施の形態ではインクリメンタルエンコーダ値を「０」にしたが、その時のアブソリュートエンコーダ１０のアブソリュートエンコーダ値をインクリメンタルエンコーダ値にセットするようにしてもよい。
【００４１】
また、インクリメンタルエンコーダ２０のリセットでアブソリュートエンコーダ値をセットするようにしたときには、自動原点合わせ処理（ステップＳ３００）において原点補正（ステップＳ３０７）を行う場合に、記憶しているアブソリュートエンコーダ値をインクリメンタルエンコーダ値にセットすることで原点補正を行うようにしてもよい。このようにすると、原点補正において誤差分を回転駆動する必要がなくなり、誤差が生じなくなる。
【００４２】
さらに、前述した実施の形態では、第１の端点検出と第２の端点検出を近接スイッチ８がＯＦＦからＯＮになるときとしたが、これと逆のＯＮからＯＦＦになるときを検出端点としてもよい。また、近接スイッチ８がＯＮからＯＦＦになるときとＯＦＦからＯＮになるときのヒステリシス誤差が無視できる程度である場合には、近接スイッチ８がＯＦＦからＯＮになるときを第１の端点検出位置とし、その後、ＯＦＦからＯＮになるときを第２の端点検出位置として、その中点を基準にして、オフセット角度θａ，θ０を算出してもよい。
【００４３】
なお、この発明における原点チェック方法は、図４の処理全体が終了した後、すなわち、ロボットの動作時においても使用することができる。例えば、歩行における関節屈折時に２つの端点を検知できるようにスイッチ等を設置すれば、原点ずれが生じたことを指令者に通知したり、動作を強制停止させることでロボットの転倒等の不正制御を回避することができる。
また、この発明の原理は、回転運動を行うドック７と近接スイッチ８の組み合わせに限らず、直線運動等の他の平面運動や立体運動を行う被検出部を２点以上で検出手段が検出して、高精度な温度ドリフト補正に生かす構成としてもよい。
【００４４】
【発明の効果】
以上説明するように、請求項１に記載した発明によれば、温度ドリフトの影響を受けることなく、高精度に原点合わせを行うことができるという優れた効果が奏される。
請求項２に記載した発明によれば、さらに高精度に原点合わせを行うことができるという効果が奏される。
請求項３に記載した発明によれば、温度ドリフトの影響を受けることなく、第２部材に対し高精度に原点合わせを行うことができるという優れた効果が奏される。
【００４５】
請求項４に記載した発明によれば、第２部材に対しさらに高精度に原点合わせを行うことができるという効果が奏される。
請求項５に記載した発明によれば、温度ドリフトの影響を受けることなく、部材の原点が正確に維持されているか否かを正確に判定することができるという優れた効果が奏される。
請求項６に記載した発明によれば、温度ドリフトの影響を受けることなく、第２部材の原点が正確に維持されているか否かを正確に判定することができるという優れた効果が奏される。
【図面の簡単な説明】
【図１】 この発明に係る自動原点合わせ方法及び自動原点確認方法が適用可能な関節の一実施の形態におけるシステム構成図である。
【図２】 前記実施の形態における第１及び第２の端点検出方法を説明する図である。
【図３】 近接スイッチのＯＮ作動領域を説明する図である。
【図４】 前記実施の形態における自動原点合わせ制御の全体フローチャートである。
【図５】 前記実施の形態におけるオフセット角度θ０算出処理のフローチャートである。
【図６】 前記実施の形態におけるオフセット角度θ０，θａの算出方法を説明する図である。
【図７】 前記実施の形態における原点チェック処理のフローチャートである。
【図８】 前記実施の形態における自動原点合わせ処理のフローチャートである。
【図９】 前記実施の形態における初期原点合わせを説明する図である。
【符号の説明】
１ 大腿部（第１部材）
２ 下腿部（第２部材）
３ 関節軸
４ 回転体
５ モータ
７ ドック（被検出部）
７ａ 始端
７ｂ 終端
８ 近接スイッチ（検出部、検出手段）
１０ アブソリュートエンコーダ
２０ インクリメンタルエンコーダ
Ｍ１，Ｍ２，Ｍ３ メモリ
θ０，θａ オフセット角度（位置変位）
Claims (6)
- 被検出部を備えた部材を原点から検出部に向かって複数方向から接近させて複数の検出位置を検出し、前記原点から前記複数の検出位置の中点までの第１の位置変位を予め記憶しておき、
自動原点合わせ時には、前記方法で改めて複数の検出位置の中点までの第２の位置変位を検出し、前記第１及び第２の位置変位を比較し、その差分を補正して前記部材を前記原点まで移動させることを特徴とする自動原点合わせ方法。 - 請求項１記載の自動原点合わせ方法であって、
前記部材の前記原点における位置情報も予め記憶しておき、
自動原点合わせ動作によって移動した後の前記部材の位置情報と前記予め記憶しておいた位置情報とを比較し、その差分だけ前記部材をさらに前記原点方向に移動させることを特徴とする自動原点合わせ方法。 - 第１部材と第２部材が相対回転可能に連結され、前記第２部材と同期回転する回転体がモータにより回転駆動され、前記回転体に設けられた被検出部を検出する検出手段が前記第１部材に設置され、前記モータの回転角度を検出するエンコーダの検出値に基づいて前記第１部材と前記第２部材の相対角度が制御される自動原点合わせ方法であって、
前記回転体を原点から前記検出手段に向かって回転させて前記被検出部の第１及び第２の端点を検出し、該２つの端点検出位置の中点を算出し、前記原点から前記中点までのオフセット角度を予め記憶しておき、
自動原点合わせ時には、前記の方法で改めて第１及び第２の端点検出位置の中点までのオフセット角度を算出し、該オフセット角度と前記予め記憶したオフセット角度とを比較し、その差分を補正して前記第２部材を前記原点まで移動させることを特徴とする自動原点合わせ方法。 - 請求項３記載の自動原点合わせ方法であって、
前記第２部材の前記原点におけるエンコーダ値も予め記憶しておき、
自動原点合わせ動作によって移動した後の前記第２部材のエンコーダ値と前記予め記憶しておいたエンコーダ値とを比較し、その差分だけ前記第２部材をさらに前記原点方向に移動させることを特徴とする自動原点合わせ方法。 - 被検出部を備えた部材を原点から検出部に向かって複数方向から接近させて複数の検出位置を検出し、前記原点から前記複数の検出位置の中点までの第１の位置変位を予め記憶しておき、
自動原点確認時には、前記方法で改めて複数の検出位置の中点までの第２の位置変位を検出し、前記第１及び第２の位置変位の差分と所定値との比較によって、前記部材の原点が正確に維持されているか否かを判定することを特徴とする自動原点確認方法。 - 第１部材と第２部材が相対回転可能に連結され、前記第２部材と同期回転する回転体がモータにより回転駆動され、前記回転体に設けられた被検出部を検出する検出手段が前記第１部材に設置され、前記モータの回転角度を検出するエンコーダの検出値に基づいて前記第１部材と前記第２部材の相対角度が制御される自動原点確認方法であって、
前記回転体を原点から前記検出手段に向かって回転させて前記被検出部の第１及び第２の端点を検出し、該２つの端点検出位置の中点を算出し、前記原点から前記中点までのオフセット角度を予め記憶しておき、
自動原点確認時には、前記の方法で改めて第１及び第２の端点検出位置の中点までのオフセット角度を算出し、該オフセット角度と前記予め記憶したオフセット角度の差分値を算出し、該差分値と所定値との比較によって、前記第２部材の原点が正確に維持されているか否かを判定することを特徴とする自動原点確認方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2001309155A JP3776345B2 (ja) | 2001-10-04 | 2001-10-04 | 自動原点合わせ方法及び自動原点確認方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2001309155A JP3776345B2 (ja) | 2001-10-04 | 2001-10-04 | 自動原点合わせ方法及び自動原点確認方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2003114721A JP2003114721A (ja) | 2003-04-18 |
| JP3776345B2 true JP3776345B2 (ja) | 2006-05-17 |
Family
ID=19128352
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2001309155A Expired - Fee Related JP3776345B2 (ja) | 2001-10-04 | 2001-10-04 | 自動原点合わせ方法及び自動原点確認方法 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP3776345B2 (ja) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2012147557A1 (ja) * | 2011-04-27 | 2012-11-01 | 日本電産サンキョー株式会社 | 回転機構、産業用ロボットおよび回転体の原点位置復帰方法 |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP3813102B2 (ja) * | 2002-03-15 | 2006-08-23 | ソニー株式会社 | 脚式ロボットの姿勢制御装置及び姿勢制御方法 |
| JP4506512B2 (ja) * | 2005-03-07 | 2010-07-21 | トヨタ自動車株式会社 | 支持脚をもつ歩行ロボットのオフセット調整システム及びオフセット調整方法 |
| JP5418029B2 (ja) * | 2009-07-15 | 2014-02-19 | 日本精工株式会社 | 原点位置設定装置、原点位置設定方法、リンク機構及び脚車輪型ロボット |
-
2001
- 2001-10-04 JP JP2001309155A patent/JP3776345B2/ja not_active Expired - Fee Related
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2012147557A1 (ja) * | 2011-04-27 | 2012-11-01 | 日本電産サンキョー株式会社 | 回転機構、産業用ロボットおよび回転体の原点位置復帰方法 |
| JP2012228753A (ja) * | 2011-04-27 | 2012-11-22 | Nidec Sankyo Corp | 回転機構、産業用ロボットおよび回転体の原点位置復帰方法 |
| US9278450B2 (en) | 2011-04-27 | 2016-03-08 | Nidec Sankyo Corporation | Rotating mechanism, industrial robot and method for returning rotating body to original position |
Also Published As
| Publication number | Publication date |
|---|---|
| JP2003114721A (ja) | 2003-04-18 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| AU2005211244B2 (en) | Verification system for robot pose | |
| US4722063A (en) | Method of calculating actual arm lengths and compensating for angular errors | |
| JPH03136780A (ja) | スカラ型ロボットの機構誤差補正方法 | |
| JP6743597B2 (ja) | たわみ角検出装置 | |
| US4743823A (en) | Method and device for correcting backlash | |
| JP3776345B2 (ja) | 自動原点合わせ方法及び自動原点確認方法 | |
| US7089085B2 (en) | Method of calibrating robot | |
| JP6647308B2 (ja) | 多関節ロボットのティーチングシステム | |
| CN110682287A (zh) | 自动机械以及控制装置 | |
| JPH0260473B2 (ja) | ||
| JP2012006125A (ja) | 水平多関節ロボットの座標値補正方法 | |
| JPH0549165B2 (ja) | ||
| JP3636952B2 (ja) | 数値制御装置 | |
| JP2561145B2 (ja) | 産業用ロボット | |
| JP4301154B2 (ja) | 多関節ロボット | |
| JP2803399B2 (ja) | ロボットの原点補正量検出方法および治具 | |
| JPS624471Y2 (ja) | ||
| JPH0260474B2 (ja) | ||
| CN117921646A (zh) | 零点标定方法、存储介质、控制器、关节结构及机器人 | |
| JP7360342B2 (ja) | ツール位置検出装置及び該装置を備えたロボット | |
| KR20030008575A (ko) | 지그를 이용한 다관절로봇의 오차 보정방법 | |
| KR102148160B1 (ko) | 레이저 측정의 시야확보를 위한 구형재귀반사경 구동시스템 및 이를 이용한 시야확보방법 | |
| JP2003291082A (ja) | 多関節脚制御装置 | |
| JPH05177565A (ja) | ロボットのキャリブレーション装置 | |
| KR100227030B1 (ko) | 로보트의 제조 및 조립오차에 따른 오차보상방법 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20051227 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20060214 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20060222 |
|
| R150 | Certificate of patent or registration of utility model |
Ref document number: 3776345 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20100303 Year of fee payment: 4 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20100303 Year of fee payment: 4 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20110303 Year of fee payment: 5 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20110303 Year of fee payment: 5 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120303 Year of fee payment: 6 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20130303 Year of fee payment: 7 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20130303 Year of fee payment: 7 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20140303 Year of fee payment: 8 |
|
| LAPS | Cancellation because of no payment of annual fees |