JP2012223871A - ロボットの関節の原点補正方法およびそのシステム - Google Patents

Abstract

【課題】高精度且つ安価にロボットの関節を原点補正する。
【解決手段】第１の姿勢Ｐ_１（ｌ）のロボット１２のマーカー２２が撮影画像に写る位置にカメラ１６を設置する。各関節用のエンコーダの出力値に基づいて、原点補正対象の関節１２ｌに予め対応付けされた関節のみを動かすことにより、カメラ１６の撮影画像上において第１の姿勢時のマーカー２２の位置の近傍にマーカー２２が写るような第２の姿勢Ｐ_２（ｌ）に変更する。ロボット１２の姿勢変更によるカメラ１６の撮影画像上におけるマーカー２２の変位に基づいて、原点補正対象の関節１２ｌの原点補正を行う。
【選択図】図７

Description

本発明は、ロボットの関節の原点を補正する方法およびそのシステムに関する。

従来より、複数のリンクと、リンク間に設けられた関節と、一方のリンクに対して他方のリンクを関節軸中心に回転させる駆動源（モータ）と、一方のリンクと他方のリンクとの間の関節角度を検出する関節角度検出手段（エンコーダ）とを有する多関節型のロボットが知られている。

このようなロボットを所望の位置姿勢に制御するとき、エンコーダの出力値に基づいて、具体的にはエンコーダの出力値が所望の位置姿勢（所望の関節角度）に対応する値になるように、モータが制御される。そのため、エンコーダの出力値と関節角度とが所定の対応関係に維持されている必要がある。例えば、ゼロの関節角度に所定に対応付けされたエンコーダの出力値が所定の一定値に維持されている必要がある。

ところが、現実には、エンコーダの出力値と関節角度との対応関係は、ロボットの調整誤差や、修理のためにロボットの構成部品（特にモータやエンコーダ）が交換されるなど、ロボットの状態が変わることによって変化することがある。例えば、ゼロの関節角度に所定に対応付けされたエンコーダの出力値が、ロボットの状態が変わることによって異なる値になることがある。

当然ながら、エンコーダの出力値と関節角度との対応関係が変わると、エンコーダの出力値に基づいて制御されるロボットは、要求された位置姿勢と異なる位置姿勢になる。すなわちロボットの位置姿勢に関する精度が悪化する。

したがって、ロボットの位置姿勢に関する精度を高く維持するためには、精度調整の一環として、ロボットの状態が変わる度に、または定期的に、エンコーダの出力値と関節角度との対応関係を新たに調べる必要がある。そして、ロボットの状態が変わった後は、調べたエンコーダの出力値と関節角度との対応関係に基づいてロボットを制御する必要がある。このような精度調整は、関節角度がゼロになるときのエンコーダの出力値を新たに調べることから、零点調整や原点補正と呼ばれる。

一例として、特許文献１には、カメラを用いるロボットの関節の原点補正方法が開示されている。特許文献１の方法では、ロボットの先端にマーカーを取付け、マーカーが撮影画像に写る位置にカメラを設置する。そして、ロボットの複数の姿勢と、各姿勢時のカメラの撮影画像上のマーカーの位置とに基づいて、関節の原点補正を実行する。

特開２００６−１１０７０５号公報

しかしながら、特許文献１に記載する原点補正方法の場合、マーカーを大きく移動させてカメラの撮影画像上に定義された複数の基準点それぞれにマーカーを位置合わせするため、カメラをロボットから大きく離す必要がある。そのため、カメラによるマーカー位置の検出精度が低い（マーカーから大きく離れたカメラは、マーカーの微小な変位を捉えることが困難である）。したがって、高精度に、ロボットの関節の原点補正を行うことが困難である。

また、特許文献１に記載する原点補正方法は、撮影画像上において基準点にマーカーを正確に位置合わせするため、高価な高精度カメラが必要であり、また、カメラのキャリブレーションが必要である。

そこで、本発明は、高精度に且つ安価に、ロボットの関節の原点補正を実行することを課題とする。

上述の課題を解決するために、本発明の第１の態様によれば、
多関節型のロボットの関節角度を検出するエンコーダの出力値と該関節角度の実値との対応関係を補正するロボットの関節の原点補正方法であって、
第１の姿勢のロボットのマーカーが撮影画像に写る位置にカメラを設置し、
各関節用のエンコーダの出力値に基づいて、原点補正対象の関節に予め対応付けされた関節のみを動かすことにより、カメラの撮影画像上において第１の姿勢時のマーカーの位置の近傍にマーカーが写るような第２の姿勢に第１の姿勢から変更し、
第１の姿勢から第２の姿勢への変更によるカメラの撮影画像上におけるマーカーの変位に基づいて、原点補正対象の関節の原点補正を行う、ロボットの関節の原点補正方法が提供される。

本発明の第２の態様によれば、
原点補正対象は曲げ関節であって、
曲げ関節を挟んで第１および第２のねじり関節があって、
曲げ関節の関節角度が基準値のときは第１および第２のねじり関節の関節軸それぞれが同一直線上に位置し、
曲げ関節用のエンコーダが基準値と所定の対応関係の出力値を出力するように曲げ関節の関節角度を変更することによってロボットを第１の姿勢とし、
各関節用のエンコーダの出力値に基づいて、
（１）曲げ関節の関節角度が第１の姿勢時と同一であり、
（２）第１および第２のねじり関節の関節角度それぞれが第１の姿勢時から１８０度変更され、且つ、
（３）残りの関節の関節角度が第１の姿勢時と同一である、第２の姿勢に第１の姿勢から変更する、第１の態様に記載のロボットの関節の原点補正方法が提供される。

本発明の第３の態様によれば、
原点補正対象はねじり関節であって、
ねじり関節を挟んで第１および第２の曲げ関節があって、
ねじり関節の関節角度が基準値のときは第１および第２の曲げ関節の関節軸が互いに平行に位置し、
ねじり関節用のエンコーダが基準値と所定の対応関係の出力値を出力するようにねじり関節の関節角度を変更すること、且つ、ロボットの先端側の第１の曲げ関節の関節角度を、一方の方向に所定の角度変更することによってロボットを第１の姿勢とし、
各関節用のエンコーダの出力値に基づいて、
（１）ねじり関節の関節角度が第１の姿勢時と同一であり、
（２）第１の曲げ関節の関節角度が第１の姿勢時から他方の方向に変更され、
（３）第２の曲げ関節の関節角度が第１の姿勢時から変更され、且つ
（４）残りの関節の関節角度が第１の姿勢時と同一である、第２の姿勢に第１の姿勢から変更する、第１の態様に記載のロボットの関節の原点補正方法が提供される。

本発明の第４の態様によれば、
原点補正対象は第１のねじり関節であって、
第１のねじり関節の反先端側に別の第２のねじり関節があって、
第２のねじり関節の反先端側に曲げ関節があって、
マーカーが第１のねじり関節の関節軸上以外に位置し、
第１のねじり関節の関節角度が基準値のときはマーカーが曲げ関節の関節軸と直交し、且つ第２のねじり関節の関節軸を含む平面上に位置し、
第１のねじり関節用のエンコーダが基準値と所定の対応関係の出力値を出力するように第１のねじり関節の関節角度を変更することによってロボットを第１の姿勢とし、
各関節用のエンコーダの出力値に基づいて、
（１）第１および第２のねじり関節の関節角度の変更量の合計が１８０度になるように該第１および第２のねじり関節の関節角度が第１の姿勢時から変更され、
（２）曲げ関節の関節角度が第１の姿勢時から変更され、且つ、
（３）残りの関節の関節角度が第１の姿勢時と同一である、第２の姿勢に第１の姿勢から変更する、第１の態様に記載のロボットの関節の原点補正方法が提供される。

本発明の第５の態様によれば、
原点補正対象は関節軸が互いに平行な第１および第２の曲げ関節であって、
反先端側の第２の曲げ関節のさらに反先端側に第１のねじり関節があって、
第１の曲げ関節の先端側に第３の曲げ関節があって、
第３の曲げ関節の先端側に第２のねじり関節があって、
（ａ）各関節用のエンコーダの出力値に基づいて第１および第２のねじり関節の関節軸が同一直線上に位置するようにロボットを第１の姿勢にし、
各関節用のエンコーダの出力値に基づいて、
（１）第１および第２のねじり関節の関節角度それぞれが第１の姿勢時から１８０度変更され、且つ、
（２）残りの関節の関節角度が第１の姿勢時と同一である、第２の姿勢に第１の姿勢から変更し、
（ｂ）各関節用のエンコーダの出力値に基づいて第１および第２のねじり関節の関節軸が同一直線上に位置するようにロボットを第３の姿勢にし、
各関節用のエンコーダの出力値に基づいて、
（１）第１および第２のねじり関節の関節軸が同一直線上に位置するように維持し、
（２）第１、第２、および第３の曲げ関節の関節角度それぞれが第３の姿勢時から変更され、且つ、
（３）残りの関節の関節角度が第３の姿勢時と同一である、第４の姿勢に第３の姿勢から変更し、
第１の姿勢から第２の姿勢への変更によるカメラの撮影画像上におけるマーカーの第１の変位と、第３の姿勢から第４の姿勢への変更によるカメラの撮影画像上におけるマーカーの第２の変位とに基づいて、原点補正対象の関節の原点補正を行う、第１の態様に記載のロボットの関節の原点補正方法が提供される。

本発明の第６の態様によれば、
多関節型のロボットの関節角度を検出するエンコーダの出力値と該関節角度の実値との対応関係を補正するロボットの関節の原点補正システムであって、
第１の姿勢のロボットのマーカーを撮影するカメラと、
各関節用のエンコーダの出力値に基づいて、原点補正対象の関節に予め対応付けされた関節のみを動かすことにより、カメラの撮影画像上において第１の姿勢時のマーカーの位置の近傍にマーカーが写るような第２の姿勢に第１の姿勢のロボットを制御するロボット制御手段と、
第１の姿勢から第２の姿勢への変更によるカメラの撮影画像上におけるマーカーの変位量および変位方向を算出するマーカー変位算出手段と、
マーカー変位算出手段が算出したマーカーの変位量および変位方向に基づいて、原点補正対象の関節の原点補正を行う原点補正手段とを有する、ロボットの関節の原点補正システムが提供される。

本発明によれば、第１の姿勢から第２の姿勢（カメラの撮影画像上において第１の姿勢時のマーカーの位置の近傍にマーカーが写るような姿勢）への姿勢変更による、カメラの撮影画像上におけるマーカーの変位に基づいて、原点補正対象の関節の原点補正が行われる。マーカーの変位量が小さいので該マーカーの近くにカメラを設置することできるため、分解能の低い安価なカメラであっても、高精度に関節の原点補正を実行することができる。

また、第１の姿勢から第２の姿勢への変更によるカメラの撮影画像上におけるマーカーの変位と、原点補正対象の関節用のエンコーダの出力値に所定に対応付けされた関節角度と該関節の実際の関節角度の間のずれ量との相関性を利用するため、また、原点補正対象の関節のみに原点補正を実行するため、原点補正対象以外の関節の関節角度やロボットの機械パラメータ（リンクの長さなど）を用いて複雑な計算を行う必要がない。したがって、複雑な計算を行う必要がなく、処理能力が低い安価なコンピュータ１８であっても、容易に計算することができる。

その結果、高精度に且つ安価に、ロボットの関節の原点補正を実行することができる。

本発明の実施の形態に係る、ロボットの関節の原点補正を実行するシステムを概略的に示す図である。 ２つのねじり関節に挟まれた曲げ関節の原点補正を説明するための図である。 ２つのねじり関節に挟まれた曲げ関節の原点補正を説明するための別の図である。 ２つの曲げ関節に挟まれたねじり関節の原点補正を説明するための図である。 ２つの曲げ関節に挟まれたねじり関節の原点補正を説明するための別の図である。 先端点と別のねじり関節とに挟まれたねじり関節であって、別のねじり関節の反先端点側に曲げ関節が存在するねじり関節の原点補正を説明するための図である。 先端点と別のねじり関節とに挟まれたねじり関節であって、別のねじり関節の反先端点側に曲げ関節が存在するねじり関節の原点補正を説明するための別の図である。 関節軸が互いに平行な２つの曲げ関節の原点補正を説明するための図である。 関節軸が互いに平行な２つの曲げ関節の原点補正を説明するための別の図である。 図２〜７に対応する原点補正の流れを示す図である。 図８〜９に対応する原点補正の流れを示す図である。 別の実施の形態の関節の原点補正を説明するための図である。

図１は、本発明の実施の形態に係る、ロボットの関節の原点補正を実行する原点補正システムを概略的に示している。

図１に示すように、原点補正システム１０は、多関節型のロボット１２と、ロボット１２の姿勢を制御するコントローラ１４と、ロボット１２を撮影するカメラ１６と、コントローラ１４とカメラ１６とに接続されたコンピュータ１８とを有する。

ロボット１２は、複数のリンク１２ａ〜１２ｆと、リンク間に設けられた関節１２ｇ〜１２ｌとを有する多関節型のロボットである。また、ロボット１２は、その先端のフランジ１２ｍにスプレーガンや溶接トーチなどの作業用ツールを取り付けることにより、塗装作業や溶接作業などの作業を実行する。ここでは、ロボット１２の関節原点補正のための原点補正用ツール２０がフランジ１２ｍに取り付けられている。

なお、関節１２ｇ、１２ｊ、１２ｌは関節軸Ａ１、Ａ４、Ａ６を備えるねじり関節であり、関節１２ｈ、１２ｉ、１２ｋは関節軸Ａ２、Ａ３、Ａ５を備える曲げ関節である。また、図１に示すロボット１２では、ねじり関節１２ｇの関節軸Ａ１と曲げ関節１２ｈの関節軸Ａ２とが交差せず、また曲げ関節１２ｉの関節軸Ａ３とねじり関節１２ｊの関節軸Ａ４とが交差していないが、このようなロボット１２に本発明は限定しない。すなわち、ねじり関節１２ｇの関節軸Ａ１と曲げ関節１２ｈの関節軸Ａ２とが交差し、また曲げ関節１２ｉの関節軸Ａ３とねじり関節１２ｊの関節軸Ａ４とが交差するロボットでも、本発明は実施可能である。

ロボット１２の先端点として、原点補正用ツール２０は、先端にマーカー２２を備える。マーカー２２は、例えば発光ダイオードであって、後述するカメラ１６によって撮影される。マーカー２２はまた、フランジ１２ｍに最も近いねじり関節１２ｌの関節軸Ａ６上を外れて位置するようにフランジ１２ｍに固定されている。なお、本実施の形態においては、フランジ１２ｍに対するマーカー２２の位置は未知でもよい。すなわち本実施の形態は、マーカー２２の空間上の位置を算出することなく、マーカー２２を用いてロボット１２の関節の原点を補正する。

なお、本明細書において、「位置」や「姿勢」は、特段の記載がない限り、ロボット１２のベース１２ｎに予め定義されているベース座標系ΣＢにおける位置や姿勢を言う。

コントローラ１４は、関節１２ｇ〜１２ｌそれぞれのエンコーダの出力値に基づいて、関節１２ｇ〜１２ｌそれぞれを駆動する複数のモータ（図示せず）を制御することにより、ロボット１２の姿勢を制御するように構成されている。具体的には、コントローラ１４は、任意の姿勢を実現する関節角度と所定の対応関係の出力値を、関節１２ｇ〜１２ｌそれぞれのエンコーダが出力するように各関節のモータを制御する。

コントローラ１４はまた、詳細は後述するが、外部装置であるコンピュータ１８から教示データ（関節１２ｇ〜１２ｌそれぞれの関節角度のデータ）を受け取り、受け取った教示データに対応する姿勢にロボット１２を制御するように構成されている。具体的には、コントローラ１４は、教示データが示す関節角度と所定の対応関係の出力値をエンコーダが出力するように、関節１２ｇ〜１２ｌを駆動するモータ（図示せず）を制御する。

カメラ１６は、例えばＣＣＤカメラであって、ロボット１２周辺の任意の位置に配置されて、原点補正用ツール２０のマーカー２２を撮影するためのものである。詳細は後述するが、カメラ１６は、ロボット１２の姿勢変更にともなうマーカー２２の変位量および変位方向を検出するために使用される。また、カメラ１６は、マーカー２２の撮影画像をデータとしてコンピュータ１８に出力するように構成されている。

コンピュータ１８は、カメラ１６から受け取ったマーカー２２の撮影画像データを画像処理することにより、撮影画像上におけるマーカー２２の位置を算出するように構成されている。また、コンピュータ１８は、具体的には後述するが、ロボット１２が姿勢変更する前に撮影された撮影画像と姿勢変更後に撮影された撮影画像とから、ロボット１２の姿勢変更にともなう撮影画像上のマーカー２２の変位量および変位方向を算出するように構成されている。

コンピュータ１８はまた、ロボット１２の教示データを作成し、作成した教示データをコントローラ１４に送信するように構成されている。

ここからは、具体的に、ロボット１２の関節の原点補正方法について説明する。まず、その概念を説明する。

本発明に係るロボット１２の関節の原点補正方法では、関節１２ｇ〜１２ｌ全てを同時に原点補正するのではなく、個々にまたは多くても２つ同時に原点補正を行う。

原点補正とは、エンコーダの出力値に所定に対応付けされた関節角度と実際の関節角度とのずれ量（すなわち、エンコーダの検出誤差量）をゼロにすることである。ずれ量をゼロにするために、本発明に係るロボット１２の関節の原点補正方法では、原点補正対象の関節それぞれに、予め第１および第２の姿勢を設定し、第１の姿勢から第２の姿勢に変更することによるマーカー２２の変位量および変位方向を計測する。

なお、本明細書で言う「第２の姿勢」は、具体的には後述するが、カメラ１６の撮影画像上において、マーカー２２の位置が、第１の姿勢時の位置の近傍に位置するような姿勢（第１および第２の姿勢時のマーカー２２の位置の間の距離が最小（ゼロも含む）になるような姿勢）を言う。「第２の姿勢」はまた、具体的には後述するが、原点補正対象の関節に対応付けされた関節のみを動かすことにより実現される。

また、マーカー２２の変位量および変位方向の計測は、カメラ１６によって行う。具体的には、カメラ１６の撮影画像上におけるマーカー２２の変位量および変位方向を、第１の姿勢時の撮影画像と第２の姿勢時の撮影画像とを画像処理することによって算出する。撮影画像上のマーカー２２の変位量は、画素数で示される。

以下、ロボット１２の関節１２ｈ〜１２ｌそれぞれの原点補正方法について具体的に説明する。

図２および図３は、図１に示す曲げ関節１２ｋの原点補正方法を説明するための図である。また、図２および図３は、曲げ関節１２ｋ用のエンコーダ（図示せず）の出力値は同一であるが、曲げ関節１２ｋの実際の関節角度が異なる状態（角度Δθ_ｋずれている状態）を示している。図２は正常状態を示し、図３は異常状態を示している。

なお、本明細書でいう「正常」は、原点補正を必要としない状態、すなわちエンコーダの出力値と実際の関節角度が所定の対応関係にある状態を言う。また、「異常」は、原点補正を必要とする状態、すなわち、すなわちエンコーダの出力値と実際の関節角度が所定の対応関係にない状態を言う。

図２（Ａ）に示すように、曲げ関節１２ｋは、ねじり関節１２ｊの関節軸Ａ４とねじり関節１２ｌの関節軸Ａ６とが同一直線上に位置するときを原点とし、そのときの関節角度をゼロ度と設定している。なお、ゼロ度に所定に対応付けされたエンコーダの出力値はＳ_ｋである。

曲げ関節１２ｋの原点補正のための第１の姿勢Ｐ_１（ｋ）を実現するために、曲げ関節１２ｋのエンコーダが出力値Ｓ_ｋを出力するように、曲げ関節１２ｋの関節角度を調整する（図２（Ａ）に示す姿勢を実現する）。

次に、ねじり関節１２ｊの関節角度を図２（Ａ）に示す第１の姿勢Ｐ_１（ｋ）時から１８０度変更すると、図２（Ｂ）に示すように、マーカー２２は、関節軸Ａ４（Ａ６）対称の位置に移動する。

続いて、ねじり関節１２ｌの関節角度を１８０度変更する（曲げ関節１２ｋの原点補正のための第２の姿勢Ｐ_２（ｋ）を実現する）。図２（Ｃ）に示すように、マーカー２２は、ねじり関節１２ｊの関節角度とねじれ関節１２ｌの関節角度とを１８０度変更する前の位置（図２（Ａ）に示す第１の姿勢Ｐ_１（ｋ）時の位置）と同一の位置に移動する。

これに対して、図３（Ａ）に示すように、第１の姿勢Ｐ_１（ｋ）を実現するために、曲げ関節１２ｋのエンコーダが出力値Ｓ_ｋを出力するように、曲げ関節１２ｋの関節角度を調整すると、ねじり関節１２ｊの関節軸Ａ４とねじり関節１２ｌの関節軸Ａ６とは同一直線上に位置しない。これは、エンコーダの出力値はＳ_ｋであるが、曲げ関節１２ｋの実際の関節角度がゼロ度ではなく、ゼロ度からΔθ_ｋだけずれているために起こる。

次に、ねじり関節１２ｊの関節角度を図３（Ａ）に示す第１の姿勢Ｐ_１（ｋ）時から１８０度変更する（図３（Ｂ））。

続いて、ねじり関節１２ｌの関節角度を１８０度変更する（第２の姿勢Ｐ_２（ｋ）を実現する）と、図３（Ｃ）に示すように、マーカー２２は、ねじり関節１２ｊの関節角度とねじれ関節１２ｌの関節角度とを１８０度変更する前の位置（図３（Ａ）に示す第１の姿勢Ｐ_１（ｋ）時の位置）から変位量Ｄ（ｋ）だけ変位する。

したがって、第１の姿勢Ｐ_１（ｋ）時および第２の姿勢Ｐ_２（ｋ）時にカメラ１６によってマーカー２２を撮影し、その撮影画像上でマーカー２２が変位しなければ、エンコーダが出力値Ｓ_ｋを出力するときは、曲げ関節１２ｋの実際の関節角度がゼロ度であることがわかる。

一方、第１の姿勢Ｐ_１（ｋ）から第２の姿勢Ｐ_２（ｋ）への姿勢変更によって撮影画像上でマーカー２２が変位した場合、エンコーダが出力値Ｓ_ｋを出力するときは、曲げ関節１２ｋの実際の関節角度がゼロ度以外の角度であることがわかる。

また、第１の姿勢Ｐ_１（ｋ）から第２の姿勢Ｐ_２（ｋ）への姿勢変更による撮影画像上におけるマーカー２２の変位量は、実際のマーカー２２の変位量Ｄ（ｋ）と対応し、実際のマーカー２２の変位量Ｄ（ｋ）はずれ量Δθ_ｋに相関する。また、撮影画像上におけるマーカー２２の変位方向は、ずれ量Δθ_ｋのずれ方向に対応する。したがって、撮影画像上のマーカー２２の変位量および変位方向に基づいて、ずれ量Δθ_ｋとそのずれ方向とを算出することが可能である。

ただし、カメラ１６の撮影画像上におけるマーカー２２の変位量とずれ量Δθ_ｋとは相関するものの、上述したようにカメラ１６が任意の位置に配置される場合、撮影画像上におけるマーカー２２の変位量からずれ量Δθ_ｋを算出することはできない。撮影画像上におけるマーカー２２の変位量からずれ量Δθ_ｋを算出するためには、カメラ１６の設置位置を規定し、撮影画像上におけるマーカー２２の変位量および変位方向と曲げ関節１２ｋの関節角度の変化量との対応関係（例えば、関節角度の１度の増加に対する撮影画像上におけるマーカー２２の変位量および変位方向）を予め調べておく必要がある。なお、カメラ１６が任意の位置に配置される場合、すなわちずれ量Δθ_ｋを算出することなく、ロボット１２の曲げ関節１２ｋの原点を補正する方法（ずれ量Δθ_ｋをゼロにする方法）についてはフローチャートを用いて後述する。

また、曲げ関節１２ｋの原点補正にあたり、ねじり関節１２ｊ，１２ｌはその関節角度を１８０度変更するだけであるため、ねじり関節１２ｊ，１２ｌそれぞれが正常であるか否か（それぞれにおいて、エンコーダの出力値と実際の関節角度とが所定の対応関係であるか否か）は問題にならない。なぜなら、回転量は、エンコーダの出力値の変化量に基づいて制御されるためである。

さらに、ねじり関節１２ｊの関節角度とねじれ関節１２ｌの関節角度とを同時に１８０度変更してもよい。すなわち、第１の姿勢Ｐ_１（ｋ）から第２の姿勢Ｐ_２（ｋ）に直接姿勢変更してもよい。

さらにまた、曲げ関節１２ｋの原点補正中、ねじり関節１２ｇ、曲げ関節１２ｈ、および曲げ関節１２ｉは、固定した状態である。すなわち、第１の姿勢Ｐ_１（ｋ）から第２の姿勢Ｐ_２（ｋ）に姿勢変更する際に、ねじり関節１２ｇ、曲げ関節１２ｈ、および曲げ関節１２ｉの関節角度を変更しない。

図４および図５は、図１に示すねじり関節１２ｊの原点補正方法を説明するための図である。また、図４および図５は、ねじり関節１２ｊ用のエンコーダ（図示せず）の出力値は同一であるが、ねじり関節１２ｊの実際の関節角度が異なる状態（角度Δθ_ｊずれている状態）を示している。図４は正常状態を示し、図５は異常状態を示している。

図４（Ａ）に示すように、ねじり関節１２ｊは、曲げ関節１２ｉの関節軸Ａ３と曲げ関節１２ｋの関節軸Ａ５とが互いに平行になるときを原点とし、そのときの関節角度をゼロ度と設定している。なお、ゼロ度に所定に対応するエンコーダの出力値はＳ_ｊである。

ねじり関節１２ｊの原点補正のための第１の姿勢Ｐ_１（ｊ）を実現するために、図４（Ａ）および図５（Ａ）に示すように、まず、ねじり関節１２ｊのエンコーダが出力値Ｓ_ｊを出力するように、ねじり関節１２ｊの関節角度を調整する。また、曲げ関節１２ｋの関節角度を、原点（ゼロ度）から一方の方向（正方向）に予め決められた角度（例えば９０度）変更する。

次に、図４（Ｂ）および図５（Ｂ）に示すように、曲げ関節１２ｋの関節角度を、図４（Ａ）および図５（Ａ）に示す第１の姿勢Ｐ_１（ｊ）時から他方の方向（負方向）に予め決められた角度（例えば１８０度）変更する。なお、図４（Ａ）と図４（Ｂ）とに示す（図５（Ａ）と図５（Ｂ）とに示す）曲げ関節１２ｋの関節角度は、原点に対して＋９０度、−９０度であるが、すなわち絶対値は同一であるが、これに限定されるわけではない。

続いて、図４（Ｃ）および図５（Ｃ）に示すように、マーカー２２が第１の姿勢Ｐ_１（ｊ）時（図４（Ａ）および図５（Ａ）に示す姿勢時）の位置にほぼ戻るように、すなわちマーカー２２の位置が第１の姿勢Ｐ_１（ｊ）時のマーカー２２の位置との間の距離が最小になるような角度に曲げ関節１２ｉの関節角度を変更し、ねじり関節１２ｊの原点補正のための第２の姿勢Ｐ_２（ｊ）を実現する。

なお、図４（Ｃ）に示すようには、第１の姿勢Ｐ_１（ｊ）時のマーカー２２の位置と第２の姿勢Ｐ_２（ｊ）時のマーカー２２の位置は完全に一致しなくてもよい。

また、マーカー２２の位置が第１の姿勢Ｐ_１（ｊ）時のマーカー２２の位置との間の距離が最小になるような第２の姿勢Ｐ_２（ｊ）は、カメラ１６の撮影画像に基づいて実現される。具体的には、コンピュータ１８からの曲げ関節１２ｉの関節角度が異なる複数の教示データに基づいて、コントローラ１４がロボット１２に曲げ関節１２ｉの関節角度が異なる複数の姿勢を実現させる。ロボット１２の各姿勢についてカメラ１６がマーカー２２を撮影し、コンピュータ１８が各姿勢について撮影画像上におけるマーカー２２の位置を算出する。そして、コンピュータ１８は、第１の姿勢Ｐ_１（ｊ）時に撮影された撮影画像上におけるマーカー２２の位置との距離が最小になるような位置にマーカー２２が写る撮影画像に対する姿勢を第２の姿勢Ｐ_２（ｊ）と特定する。

ねじり関節１２ｊのエンコーダが出力値Ｓ_ｊを出力するときに実際の関節角度がゼロ度であれば、図４（Ｃ）に示すように、第１の姿勢Ｐ_１（ｊ）時のマーカー２２の位置と第２の姿勢Ｐ_２（ｊ）時のマーカー２２の位置とはＸ軸方向に関しては同一である。なお、このＸ軸方向は、曲げ関節１２ｉの関節軸Ａ３方向である。

これに対して、エンコーダが出力値Ｓ_ｊを出力するときに実際の関節角度がゼロ度からΔθ_ｊだけずれている場合、図５（Ｃ）に示すように、第１の姿勢Ｐ_１（ｊ）時のマーカー２２の位置と第２の姿勢Ｐ_２（ｊ）時のマーカー２２の位置は、Ｘ軸方向に変位量Ｄ（ｊ）だけずれる。これは、曲げ関節１２ｉの関節軸Ａ３と曲げ関節１２ｋの関節軸Ａ５とが平行でないために起こる。

したがって、カメラ１６の視準方向がＸ軸方向（関節軸Ａ３方向）と平行にならないように該カメラ１６を配置し、第１の姿勢Ｐ_１（ｊ）時および第２の姿勢Ｐ_２（ｊ）時にカメラ１６によってマーカー２２を撮影し、その撮影画像上でマーカー２２がＸ軸方向に対応する方向に変位しなければ、エンコーダが出力値Ｓ_ｊを出力するときは、ねじり関節１２ｊの実際の関節角度がゼロ度であることがわかる。

一方、第１の姿勢Ｐ_１（ｊ）から第２の姿勢Ｐ_２（ｊ）への姿勢変更によって撮影画像上でマーカー２２がＸ軸方向に対応する方向に変位した場合、エンコーダが出力値Ｓ_ｊを出力するときは、ねじり関節１２ｊの実際の関節角度がゼロ度以外の角度であることがわかる。

また、第１の姿勢Ｐ_１（ｊ）から第２の姿勢Ｐ_２（ｊ）への姿勢変更による撮影画像上でのマーカー２２の変位量は、実際のマーカー２２のＸ軸方向の変位量Ｄ（ｊ）と対応し、実際のマーカー２２のＸ軸方向の変位量Ｄ（ｊ）はずれ量Δθ_ｊと相関する。また、撮影画像上におけるマーカー２２の変位方向は、ずれ量Δθ_ｊのずれ方向に対応する。したがって、撮影画像上のマーカー２２の変位量および変位方向に基づいて、ずれ量Δθ_ｊとそのずれ方向とを算出することが可能である。

ただし、カメラ１６の撮影画像上におけるマーカー２２の変位量とずれ量Δθ_ｊとは相関するものの、上述したようにカメラ１６が任意の位置に配置される場合、撮影画像上におけるマーカー２２の変位量からずれ量Δθ_ｊを算出することはできない。撮影画像上におけるマーカー２２の変位量からずれ量Δθ_ｊを算出するためには、カメラ１６の設置位置を規定し、撮影画像上におけるマーカー２２の変位量および変位方向とねじり関節１２ｊの関節角度の変化量との対応関係（例えば、関節角度の１度の増加に対する撮影画像上におけるマーカー２２の変位量および変位方向）を予め調べておく必要がある。なお、カメラ１６が任意の位置に配置される場合、すなわちずれ量Δθ_ｊを算出することなく、ロボット１２のねじり関節１２ｊの原点を補正する方法（ずれ量Δθ_ｊをゼロにする方法）についてはフローチャートを用いて後述する。

また、ねじり関節１２ｊを原点補正する前に、曲げ関節１２ｋの原点補正を実行するのが好ましい。上述したように、ねじり関節１２ｊの原点補正を実行するにあたり、曲げ関節１２ｋの関節角度を使用するからである。

さらに、ねじり関節１２ｊの原点補正中、ねじり関節１２ｇ、曲げ関節１２ｈ、およびねじり関節１２ｌは固定した状態である。すなわち、第１の姿勢Ｐ_１（ｊ）から第２の姿勢Ｐ_２（ｊ）に姿勢変更する際に、ねじり関節１２ｇ、曲げ関節１２ｈ、およびねじり関節１２ｌの関節角度を変更しない。

図６および図７は、図１に示すねじり関節１２ｌの原点補正方法を説明するための図である。また、図６および図７は、ねじり関節１２ｌ用のエンコーダの出力値は同一であるが、ねじり関節１２ｌの実際の関節角度が異なる状態（角度Δθ_ｌずれている状態）を示している。図６は正常状態を示し、図７は異常状態を示している。

図６（Ａ）は、ねじり関節１２ｌの実際の関節角度がゼロ度のときを示している。マーカー２２は、図６（Ａ）に示すように、ねじり関節１２ｌの実際の関節角度がゼロ度のとき、ねじり関節１２ｌの関節軸Ａ６を含んで且つ曲げ関節１２ｉの関節軸Ａ３と直交する平面上に位置し、且つ関節軸Ａ６上以外に位置するように取り付けられている。なお、ゼロ度に所定に対応するエンコーダの出力値はＳ_ｌである。

これに対して、図７（Ａ）は、エンコーダの出力値はＳ_ｌであるが、ねじり関節１２ｌの実際の関節角度がゼロ度から角度Δθ_ｌずれた状態を示している。

ねじり関節１２ｌの原点補正のための第１の姿勢Ｐ_１（ｌ）を実現するために、図６（Ａ）および図７（Ａ）に示すように、まず、ねじり関節１２ｌのエンコーダが出力値Ｓ_ｌを出力するように、ねじり関節１２ｌの関節角度を調整する。また、曲げ関節１２ｋの関節角度を、ねじり関節１２ｌの関節軸Ａ６とねじり関節１２ｊの関節軸Ａ４とが同一直線上に位置するように調整する（すなわち、曲げ関節１２ｋの関節角度を原点（ゼロ度）にする）。

次に、図６（Ｂ）および図７（Ｂ）に示すように、ねじり関節１２ｊとねじり関節１２ｌの関節角度の変更量の合計が１８０度になるように、ねじり関節１２ｊとねじり関節１２ｌの関節角度を変更する。なお、図６（Ｂ）および図７（Ｂ）は、ねじり関節１２ｊの関節角度を１８０度変更し、ねじり関節１２ｌの関節角度は変更しない様子を示している。そして、図６（Ｃ）および図７（Ｃ）に示すように、マーカー２２が第１の姿勢Ｐ_１（ｌ）時の位置にほぼ戻るように、すなわちマーカー２２の位置が第１の姿勢Ｐ_１（ｌ）時のマーカー２２の位置との間の距離が最小になるような角度に曲げ関節１２ｉの関節角度を変更し、ねじり関節１２ｌの原点補正のための第２の姿勢Ｐ_２（ｌ）を実現する。

なお、図６（Ｃ）に示すようには、第１の姿勢Ｐ１（ｌ）時のマーカー２２の位置と第２の姿勢Ｐ２（ｌ）時のマーカー２２の位置は完全に一致しなくてもよい。

また、マーカー２２の位置が第１の姿勢Ｐ_１（ｌ）時のマーカー２２の位置との間の距離が最小になるような第２の姿勢Ｐ_２（ｌ）は、カメラ１６の撮影画像に基づいて実現される。具体的には、コンピュータ１８からの曲げ関節１２ｉの関節角度が異なる複数の教示データに基づいて、コントローラ１４がロボット１２に曲げ関節１２ｉの関節角度が異なる複数の姿勢を実現させる。ロボット１２の各姿勢についてカメラ１６がマーカー２２を撮影し、コンピュータ１８が各姿勢について撮影画像上におけるマーカー２２の位置を算出する。そして、コンピュータ１８は、第１の姿勢Ｐ_１（ｌ）時に撮影された撮影画像上におけるマーカー２２の位置との距離が最小になるような位置にマーカー２２が写る撮影画像に対する姿勢を第２の姿勢Ｐ_２（ｌ）と特定する。

ねじり関節１２ｌのエンコーダが出力値Ｓ_ｌを出力するときに実際の関節角度がゼロ度であれば、図６（Ｃ）に示すように、第１の姿勢Ｐ_１（ｌ）時のマーカー２２の位置と第２の姿勢Ｐ_２（ｌ）時のマーカー２２の位置とはＸ軸方向に関しては同一である。なお、このＸ軸方向は、曲げ関節１２ｉの関節軸Ａ３方向である。

これに対して、エンコーダが出力値Ｓ_ｌを出力するときに実際の関節角度がゼロ度から角度Δθ_ｌだけずれている場合、図７（Ｃ）に示すように、第１の姿勢Ｐ_１（ｌ）時のマーカー２２の位置と第２の姿勢Ｐ_２（ｌ）時のマーカー２２の位置は、Ｘ軸方向に変位量Ｄ（ｌ）だけずれる。

したがって、カメラ１６の視準方向がＸ軸方向（関節軸Ａ３方向）と平行にならないように該カメラ１６を配置し、第１の姿勢Ｐ_１（ｌ）時および第２の姿勢Ｐ_２（ｌ）時にカメラ１６によってマーカー２２を撮影し、その撮影画像上でマーカー２２がＸ軸方向に対応する方向に変位しなければ、エンコーダが出力値Ｓ_ｌを出力するときは、ねじり関節１２ｌの実際の関節角度がゼロ度であることがわかる。

一方、第１の姿勢Ｐ_１（ｌ）から第２の姿勢Ｐ_２（ｌ）への姿勢変更によって撮影画像上でマーカー２２がＸ軸方向に対応する方向に変位した場合、エンコーダが出力値Ｓ_ｌを出力するときは、ねじり関節１２ｌの実際の関節角度がゼロ度以外の角度であることがわかる。

第１の姿勢Ｐ_１（ｌ）から第２の姿勢Ｐ_２（ｌ）への姿勢変更による撮影画像上でのマーカー２２の変位量は、実際のマーカー２２のＸ軸方向の変位量Ｄ（ｌ）と対応し、実際のマーカー２２のＸ軸方向の変位量Ｄ（ｌ）はずれ量Δθ_ｌと相関する。また、撮影画像上におけるマーカー２２の変位方向は、ずれ量Δθ_ｌのずれ方向に対応する。したがって、撮影画像上のマーカー２２の変位量および変位方向に基づいて、ずれ量Δθ_ｌとそのずれ方向とを算出することが可能である。

ただし、カメラ１６の撮影画像上におけるマーカー２２の変位量とずれ量Δθ_ｌとは相関するものの、上述したようにカメラ１６が任意の位置に配置される場合、撮影画像上におけるマーカー２２の変位量からずれ量Δθ_ｌを算出することはできない。撮影画像上におけるマーカー２２の変位量からずれ量Δθ_ｌを算出するためには、カメラ１６の設置位置を規定し、撮影画像上におけるマーカー２２の変位量および変位方向とねじり関節１２ｌの関節角度の変化量との対応関係（例えば、関節角度の１度の増加に対する撮影画像上におけるマーカー２２の変位量および変位方向）を予め調べておく必要がある。なお、カメラ１６が任意の位置に配置される場合、すなわちずれ量Δθ_ｌを算出することなく、ロボット１２のねじり関節１２ｌの原点を補正する方法（ずれ量Δθ_ｌをゼロにする方法）についてはフローチャートを用いて後述する。

また、ねじり関節１２ｌを原点補正する前に、曲げ関節１２ｋの原点補正を実行するのが好ましい。上述したように、ねじり関節１２ｌの原点補正を実行するにあたり、曲げ関節１２ｋの関節角度をゼロ度にし、ねじり関節１２ｌの関節軸Ａ６とねじり関節１２ｊの関節軸Ａ４とを同一直線上に位置させる必要があるからである。

さらに、ねじり関節１２ｌの原点補正中、ねじり関節１２ｇ、曲げ関節１２ｈ、および曲げ関節１２ｋは固定した状態である。すなわち、第１の姿勢Ｐ_１（ｌ）から第２の姿勢Ｐ_２（ｌ）に姿勢変更する際に、ねじり関節１２ｇ、曲げ関節１２ｈ、および曲げ関節１２ｋの関節角度を変更しない。

図８および図９は、図１に示すように、それぞれの関節軸Ａ２と関節軸Ａ３とが互いに平行である曲げ関節１２ｈ，１２ｉそれぞれの原点補正方法を説明するための図である。また、図８および図９は、曲げ関節１２ｈ用のエンコーダの出力値は同一であるが曲げ関節１２ｈの実際の関節角度が異なる状態（角度Δθ_ｈずれている状態）を示すとともに、曲げ関節１２ｉ用のエンコーダの出力値は同一であるが曲げ関節１２ｉの実際の関節角度が異なる状態（角度Δθ_iずれている状態）を示している。図８は正常状態を示し、図９は異常状態を示している。

曲げ関節１２ｈ，１２ｉそれぞれの原点補正のための第１の姿勢Ｐ_１（ｈ，ｉ）を実現するために、図８（Ａ）に示すように、ねじり関節１２ｌの関節軸Ａ６をねじり関節１２ｇの関節軸Ａ１上に位置合わせする（関節軸Ａ１とＡ６とを同一直線上に位置合わせする）。なお、このとき、曲げ関節１２ｈのエンコーダの出力値はＳ_ｈであって、曲げ関節１２ｉのエンコーダの出力値はＳ_ｉである。また、出力値Ｓ_ｈと所定の対応関係の関節角度はθ_ｈであり、出力値Ｓ_ｉと所定の対応関係の関節角度はθｉである。

図８（Ａ）は、曲げ関節１２ｈのエンコーダの出力値がＳ_ｈ、曲げ関節１２ｉのエンコーダの出力値がＳ_ｉであって、曲げ関節１２ｈの関節角度がθ_ｈ、曲げ関節１２ｉの関節角度がθ_ｉの場合に、ねじり関節１２ｌの関節軸Ａ６がねじり関節１２ｇの関節軸Ａ１上に位置する状態を示している。

これに対して、図９（Ａ）は、曲げ関節１２ｈのエンコーダの出力値がＳ_ｈ、曲げ関節１２ｉのエンコーダの出力値がＳ_ｉであるが、曲げ関節１２ｈの関節角度がθ_ｈ＋Δθ_ｈ、曲げ関節１２ｉの関節角度がθ_ｉ−Δθ_ｉであるために、ねじり関節１２ｌの関節軸Ａ６がねじり関節１２ｇの関節軸Ａ１上に位置しない状態を示している。

曲げ関節１２ｈ，１２ｉそれぞれの原点補正のための第２の姿勢Ｐ_２（ｈ，ｉ）を実現するために、図８（Ｂ）および図９（Ｂ）に示すように、ねじり関節１２ｇの関節角度を１８０度変更する。同様に、ねじり関節１２ｌの関節角度も１８０度変更する。

図８（Ａ）に示すようにねじり関節１２ｌの関節軸Ａ６がねじり関節１２ｇの関節軸Ａ１上に位置する場合、第１の姿勢Ｐ_１（ｈ，ｉ）から第２の姿勢Ｐ_２（ｈ，ｉ）に姿勢変更すると、第２の姿勢Ｐ_２（ｈ，ｉ）時のマーカー２２の位置は、図８（Ｂ）に示すように、第１の姿勢Ｐ_１（ｈ，ｉ）時のマーカー２２の位置と一致する。

これに対して、図９（Ａ）に示すようにねじり関節１２ｌの関節軸Ａ６がねじり関節１２ｇの関節軸Ａ１上に位置しない場合、第１の姿勢Ｐ_１（ｈ，ｉ）から第２の姿勢Ｐ_２（ｈ，ｉ）に姿勢変更すると、第２の姿勢Ｐ_２（ｈ，ｉ）時のマーカー２２は、図９（Ｂ）に示すように第１の姿勢Ｐ_１（ｈ，ｉ）時のマーカー２２の位置から変位量Ｄ（ｈ，ｉ）だけずれる。

しかしながら、変位量Ｄ（ｈ，ｉ）に対応するカメラ１６の撮影画像上におけるマーカー２２の変位量および変位方向だけでは、曲げ関節１２ｈのずれ量Δθ_ｈと曲げ関節１２ｉのずれ量Δθ_ｉとを算出することができない。その理由は、第１の姿勢Ｐ_１（ｈ，ｉ）から第２の姿勢Ｐ_２（ｈ，ｉ）への姿勢変更により、マーカー２２が一方向（ねじれ関節１２ｇの関節軸Ａ１と直交する方向）にしか移動せず、そのため、求める２つのずれ量Δθ_ｈ，Δθ_ｉに対して１つの変位量Ｄ（ｈ，ｉ）しか得られないためである。

この対処として、曲げ関節１２ｈ，１２ｉの原点補正では、更なる姿勢変更を実行し、その姿勢変更によるマーカー２２の変位量Ｄ（ｈ，ｉ）’を利用する。

図８（Ｃ）および図９（Ｃ）には、更なる姿勢変更を実行する前の第３の姿勢Ｐ_３（ｈ，ｉ）を示している。なお、図に示すように、第３の姿勢Ｐ_３（ｈ，ｉ）は、第１の姿勢Ｐ_１（ｈ，ｉ）と同一であるが、第２の姿勢Ｐ_２（ｈ，ｉ）であってもよい。

図８（Ｃ）および図９（Ｃ）に示す第３の姿勢Ｐ_３（ｈ，ｉ）から、ねじり関節１２ｌの関節軸Ａ６とねじり関節１２ｇの関節軸Ａ１とが同一直線上に位置する状態を維持するように、曲げ関節１２ｈ，１２ｉ、および１２ｋの関節角度のみを変更する（第４の姿勢Ｐ_４（ｈ，ｉ）を実現する）。

図８（Ｄ）に示すように、曲げ関節１２ｈ、１２ｉそれぞれの実際の関節角度がエンコーダの出力値と所定の対応関係の関節角度であるときは、第４の姿勢Ｐ_４（ｈ，ｉ）時のマーカー２２の位置は、第３の姿勢Ｐ_３（ｈ，ｉ）時のマーカー２２の位置と一致する。

これに対して、図９（Ｄ）に示すように、曲げ関節１２ｈの実際の関節角度がエンコーダの出力値と所定の対応関係の関節角度からΔθ_ｈずれた角度であって、曲げ関節１２ｉ実際の関節角度がエンコーダの出力値と所定の対応関係の関節角度からΔθ_ｉずれた角度である場合、第４の姿勢Ｐ_４（ｈ，ｉ）時のマーカー２２は、第３の姿勢Ｐ_３（ｈ，ｉ）時のマーカー２２の位置から変位量Ｄ（ｈ，ｉ）’だけずれる。

これにより、図９（Ｂ）に示す変位量Ｄ（ｈ，ｉ）と、図９（Ｄ）に示す変位量Ｄ（ｈ，ｉ）’とに基づいて、曲げ関節１２ｈのずれ量Δθ_ｈと曲げ関節１２ｉのずれ量Δθ_ｉとを算出することが可能になる。すなわち、２つのずれ量Δθ_ｈ，Δθ_ｉを未知数とする連立方程式を得ることができる。

第１の姿勢Ｐ_１（ｈ，ｉ）から第２の姿勢Ｐ_２（ｈ，ｉ）への姿勢変更および第３の姿勢Ｐ_３（ｈ，ｉ）から第４の姿勢Ｐ_４（ｈ，ｉ）への姿勢変更による撮影画像上でのマーカー２２の変位量は、実際のマーカー２２の変位量Ｄ（ｈ，ｉ）およびＤ（ｈ，ｉ）’と対応し、実際のマーカー２２の変位量Ｄ（ｈ，ｉ）およびＤ（ｈ，ｉ）’はずれ量Δθ_ｈおよびΔθ_ｉと相関する。また、撮影画像上におけるマーカー２２の変位方向は、ずれ量Δθ_ｈおよびΔθ_ｉのずれ方向に対応する。したがって、撮影画像上のマーカー２２の変位量および変位方向に基づいて、ずれ量Δθ_ｈおよびΔθ_ｉとそのずれ方向とを算出することが可能である。

ただし、カメラ１６の撮影画像上におけるマーカー２２の変位量とずれ量Δθ_h，Δθ_iは相関するものの、上述したようにカメラ１６が任意の位置に配置される場合、撮影画像上におけるマーカー２２の変位量からずれ量Δθ_h，Δθ_iを算出することはできない。撮影画像上におけるマーカー２２の変位量からずれ量Δθ_h，Δθ_iを算出するためには、カメラ１６の設置位置を規定し、撮影画像上におけるマーカー２２の変位量および変位方向とねじり関節１２ｌの関節角度の変化量との対応関係（例えば、関節角度の１度の増加に対する撮影画像上におけるマーカー２２の変位量および変位方向）を予め調べておく必要がある。なお、カメラ１６が任意の位置に配置される場合、すなわちずれ量Δθ_h，Δθ_iを算出することなく、ロボット１２のねじり関節１２ｌの原点を補正する方法（ずれ量Δθ_h，Δθ_iをゼロにする方法）についてはフローチャートを用いて後述する。曲げ関節１２ｈ，１２ｉの原点補正を行う前に、曲げ関節１２ｋとねじり関節１２ｊの原点補正を完了しておくのが好ましい。

また、曲げ関節１２ｈ，１２ｉの原点補正中、ねじり関節１２ｊは固定した状態である。すなわち、第１の姿勢Ｐ_１（ｈ，ｉ）から第２の姿勢Ｐ_２（ｈ，ｉ）に姿勢変更する際に、また第３の姿勢Ｐ３（ｈ，ｉ）から第４の姿勢Ｐ４（ｈ，ｉ）に姿勢変更する際に、ねじり関節１２ｊの関節角度を変更しない。

なお、曲げ関節１２ｈおよび１２ｉの原点補正は、図８や図９に示すようにマーカー２２が反ねじり関節１２ｇ側に向く姿勢で行われるが、マーカー２２がねじり関節１２ｇに向く姿勢でも可能である。

さらに、図８や図９に示す状態からねじり関節１２ｊと曲げ関節１２ｋそれぞれの関節角度を９０度変更させた姿勢、すなわちマーカー２２が図面直交方向に向く姿勢でも、曲げ関節１２ｈおよび１２ｉの原点補正は可能である。ただし、この場合、第１の姿勢から第２姿勢に変更する際に、曲げ関節１２ｋの関節角度を１８０度変更する必要がある。

ここまでは、ロボット１２の関節の原点補正方法を概念的に説明してきた。ここからは、本実施の形態に係る、具体的なロボット１２の関節の原点補正方法の流れについて説明する。図１０は、ねじり関節１２ｊ、曲げ関節１２ｋ、およびねじり関節１２ｌの原点補正の流れを示している。図１１は、曲げ関節１２ｈ，１２ｉの原点補正の流れを示している。なお、図１０および図１１に示す原点補正は、カメラ１６を任意の位置に配置する場合を想定したものである。

まず、図１０を参照しながら、ねじり関節１２ｊ、曲げ関節１２ｋ、およびねじり関節１２ｌに適用される原点補正方法の流れを説明する。

最初のステップＳ１０において、コンピュータ１８は、第１の姿勢に対応する教示データをロボット１２のコントローラ１４に送信する。コントローラ１４は、コンピュータ１８から送信された教示データに基づいて、ロボット１２を第１の姿勢に変更する（ねじり関節１２ｊ、曲げ関節１２ｋ、およびねじり関節１２ｌそれぞれの第１の姿勢についての詳細は上述参照）。

次にステップＳ１２において、コンピュータ１８は、第１の姿勢時のマーカー２２の撮影画像をカメラ１６から取得する。

続いて、ステップＳ１４において、コンピュータ１８は、第２の姿勢に対応する教示データをロボット１２のコントローラ１４に送信する。コントローラ１４は、コンピュータ１８から送信された教示データに基づいて、ロボット１２を第２の姿勢に変更する（ねじり関節１２ｊ、曲げ関節１２ｋ、およびねじり関節１２ｌそれぞれの第２の姿勢についての詳細は上述および図２〜７参照）。

ステップＳ１６において、コンピュータ１８は、第２の姿勢時のマーカー２２の撮影画像をカメラ１６から取得する。

ステップＳ１８において、コンピュータ１８は、ステップＳ１２とステップＳ１６で取得した２つの撮影画像に基づいて、第１の姿勢から第２の姿勢への姿勢変更による、撮影画像上におけるマーカー２２の変位量および変位方向を算出する。

ステップＳ２０において、コンピュータ１８は、ステップ１８で算出した、撮影画像上におけるマーカー２２の変位量が、所定の画素数（例えば５画素）に比べて小さいか否かを判定する。すなわち、関節の原点補正を行う必要があるか否かを判定する。変位量が所定の画素数に比べて小さい場合、原点補正を行う必要がないので、コンピュータ１８は原点補正を終了する。

一方、変位量が所定の画素数に比べて大きい場合、原点補正を行う必要があるため、ステップＳ２２において、コンピュータ１８は、ステップＳ１８で算出したマーカー２２の変位量および変位方向に基づいて、ずれ量（すなわち、エンコーダの検出誤差量）を減少させるための関節角度の補正量を算出する。この補正量は、記憶装置（図示せず）に記憶されている、エンコーダの出力値と関節角度の実値との対応関係を示すデータ（対応関係データ）における、各エンコーダの出力値に対応する関節角度の値を補正する量である。

例えば、ステップＳ１８で算出した変位量が大きいほど大きい補正量をコンピュータ１８は算出する。また、変位方向に基づいて、コンピュータ１８は、補正量の正負（プラスマイナス）を決定する。

なお、代わりとして、補正量は、一定の値（例えば、０．０５度）であってもよい。

ステップＳ２４において、コンピュータ１８は、コントローラ１４に対して、ステップＳ２２で算出した補正量に基づく関節の原点補正命令（エンコーダの原点オフセット指令）を出力する。コントローラ１４は、コンピュータ１８からの原点補正命令に従い、記憶装置に記憶されている、対応関係データを、ずれ量がゼロに近づくように補正する。そして、ステップＳ１０に戻る。

図１０に示す原点補正の流れによれば、第１の姿勢から第２の姿勢への姿勢変更による撮影画像上におけるマーカー２２の変位量が所定の画素数に比べて小さくなると、原点補正は完了する。

次に、図１１を参照しながら、曲げ関節１２ｈおよび曲げ関節１２ｉの原点補正の流れを説明する。

最初のステップＳ１１０において、コンピュータ１８は、第１の姿勢に対応する教示データをロボット１２のコントローラ１４に送信する。コントローラ１４は、コンピュータ１８から送信された教示データに基づいて、ロボット１２を第１の姿勢に変更する（曲げ関節１２ｈおよび曲げ関節１２ｉそれぞれの第１の姿勢についての詳細は上述および図８〜９参照）。

次に、ステップＳ１１２において、コンピュータ１８は、第１の姿勢時のマーカー２２の撮影画像をカメラ１６から取得する。

続いて、ステップＳ１１４において、コンピュータ１８は、第２の姿勢に対応する教示データをロボット１２のコントローラ１４に送信する。コントローラ１４は、コンピュータ１８から送信された教示データに基づいて、ロボット１２を第２の姿勢に変更する（曲げ関節１２ｈおよび曲げ関節１２ｉそれぞれの第２の姿勢についての詳細は上述および図８〜９参照）。

ステップＳ１１６において、コンピュータ１８は、第２の姿勢時のマーカー２２の撮影画像をカメラ１６から取得する。

ステップＳ１１８において、コンピュータ１８は、ステップＳ１１２とステップＳ１１６で取得した２つの撮影画像に基づいて、第１の姿勢から第２の姿勢への姿勢変更による、撮影画像上におけるマーカー２２の第１の変位量および変位方向を算出する。

ステップＳ１２０において、コンピュータ１８は、第３の姿勢に対応する教示データをロボット１２のコントローラ１４に送信する。コントローラ１４は、コンピュータ１８から送信された教示データに基づいて、ロボット１２を第３の姿勢に変更する（曲げ関節１２ｈおよび曲げ関節１２ｉそれぞれの第３の姿勢についての詳細は上述および図８〜９参照）。

ステップＳ１２２において、コンピュータ１８は、第３の姿勢時のマーカー２２の撮影画像をカメラ１６から取得する。

ステップＳ１２４において、コンピュータ１８は、第４の姿勢に対応する教示データをロボット１２のコントローラ１４に送信する。コントローラ１４は、コンピュータ１８から送信された教示データに基づいて、ロボット１２を第４の姿勢に変更する（曲げ関節１２ｈおよび曲げ関節１２ｉそれぞれの第４の姿勢についての詳細は上述および図８〜９参照）。

ステップＳ１２６において、コンピュータ１８は、第４の姿勢時のマーカー２２の撮影画像をカメラ１６から取得する。

ステップＳ１２８において、コンピュータ１８は、ステップＳ１２２とステップＳ１２６で取得した２つの撮影画像に基づいて、第３の姿勢から第４の姿勢への姿勢変更による、撮影画像上におけるマーカー２２の第２の変位量および変位方向を算出する。

ステップＳ１３０において、コンピュータ１８は、ステップＳ１１８で算出した第１の変位量およびステップＳ１２８で算出した第２の変異量が、所定の画素数（例えば５画素）に比べて小さいか否かを判定する。すなわち、関節の原点補正を行う必要があるか否かを判定する。第１および第２の変位量それぞれが所定の画素数に比べて小さい場合、原点補正を行う必要がないので、コンピュータ１８は原点補正を終了する。

一方、第１および第２の変位量が所定の画素数に比べて大きい場合、原点補正を行う必要があるため、ステップＳ１３２において、コンピュータ１８は、ステップＳ１１８で算出したマーカー２２の第１の変位量および変位方向と、ステップＳ１３０で算出したマーカー２２の第２の変位量および変位方向とに基づいて、曲げ関節１２ｈ，１２ｊのずれ量Δθ_ｈ，Δ_θｉを減少させるための該曲げ関節１２ｈ，１２ｉそれぞれの補正量を算出する。

ステップＳ１３４において、コンピュータ１８は、ステップＳ１３２で算出した補正量に基づく曲げ関節１２ｈ，１２ｉの原点補正命令（エンコーダの原点オフセット指令）を出力する。コントローラ１４は、コンピュータ１８からの原点補正命令に従い、記憶装置に記憶されている、対応関係データを、ずれ量Δθ_ｈ，Δθ_ｉがゼロに近づくように補正する。そして、ステップＳ１１０に戻る。

図１１に示す原点補正の流れによれば、第１の姿勢から第２の姿勢への姿勢変更による撮影画像上におけるマーカー２２の第１の変位量および第３の姿勢から第４の姿勢への姿勢変更による撮影画像上におけるマーカー２２の第２の変位量が所定の画素数に比べて小さくなると、原点補正は完了する。

本実施の形態によれば、第１の姿勢から第２の姿勢（カメラ１６の撮影画像上において第１の姿勢時のマーカー２２の位置の近傍にマーカー２２が写るような姿勢）への姿勢変更による、カメラ１６の撮影画像上におけるマーカー２２の変位に基づいて、原点補正対象の関節の原点補正が行われる。マーカー２２の変位量が小さいので該マーカー２２の近くにカメラ１６を設置することできるため、分解能の低い安価なカメラ１６であっても、高精度に関節の原点補正を実行することができる。

また、第１の姿勢から第２の姿勢への変更によるカメラ１６の撮影画像上におけるマーカー２２の変位と、原点補正対象の関節用のエンコーダの出力値に所定に対応付けされた関節角度と該関節の実際の関節角度の間のずれ量との相関性を利用するため、また、原点補正対象の関節のみに原点補正を実行するため、原点補正対象以外の関節の関節角度やロボット１２の機械パラメータ（リンク１２ａ〜１２ｆの長さなど）を用いて複雑な計算を行う必要がない。したがって、複雑な計算を行う必要がなく、処理能力が低い安価なコンピュータ１８であっても、容易に計算することができる。

その結果、高精度に且つ安価に、ロボット１２の関節の原点補正を実行することができる。

以上、上述の実施の形態を挙げて本発明を説明したが、本発明は上述の実施の形態に限定されない。

例えば、上述の実施の形態では、図１に示すようにカメラは１台であるが、複数台あってもよい。ただし、各関節に対して原点補正を実行するときの原点補正用ツール２０のマーカー２２の位置が１台のカメラに撮影視野に収まるのであれば、カメラは１台であるのが好ましい。

また、上述の実施の形態では、一種類の原点補正用ツール２０、具体的にはねじり関節１２ｌの関節軸Ａ６上以外にマーカー２２が位置する原点補正用ツールが使用されるが、複数種類の原点補正用ツールが使用されてもよい。

例えば、図１２（Ａ）に示すように、マーカー２２’がねじり関節１２ｊの関節軸Ａ６上に位置する原点補正用ツールが使用されてもよい。

一例として、図１２（Ａ）に示す原点補正用ツールを用いる場合の曲げ関節１２ｋの原点補正について説明する。

図１２（Ｂ）は、エンコーダの出力値に所定に対応付けされた関節角度と実際の関節角度とがずれ量Δθ_ｋずれている曲げ関節１２ｋを示している。

図１２（Ｂ）に示すように、ねじり関節１２ｊの関節角度を１８０度変更すると、マーカーは変位量Ｄ（ｋ）’ずれる。この場合、曲げ関節１２ｋの関節軸Ａ５からマーカー２２’までの距離をＬとすると、数式１の関係が成り立つ。

数式１を用いれば、ずれ量Δθｋを算出することができる。ただし、この場合、カメラ１６とロボット１２の相対的な位置姿勢が既知であること、且つげ関節１２ｋの関節軸Ａ５からマーカー２２’までの距離Ｌが既知であることが条件である。カメラ１６がその視準方向がねじり関節１２ｊの関節軸Ａ４に一致するように固定されることにより、撮影画像上におけるマーカー２２の変位量から、実際のマーカー２２’の変位量Ｄ（ｋ）’を算出することが可能になる。なお、この場合、カメラ１６とロボット１２の相対的な位置姿勢を予め調べる代わりに次のようにしてもよい。すなわち、ロボット１２を動作させて所定の方向に所定の変位量だけマーカー２２を変位させ、そのときの撮影画像上におけるマーカー２２の変位方向および変位量をそれぞれ基準変位方向、基準変位量として記録する。次に第１の姿勢から第２の姿勢への変更による撮影画像上におけるマーカー２２の変位量および変位方向と、記録した基準変位方向および基準変位量とを比較することにより、マーカー２２の変位量および変位方向を求める。そして求めた変位量および変位方向に基づいて原点補正を行う。この方法によると、原点補正前に基準変位方向および基準変位量を設定するので、基準変位方向および基準変位量に誤差が含まれる可能性があるが、上記の方法を繰り返すことによって基準変位方向および基準変位量に含まれる誤差と原点のずれをともに０に収束させることができる。

なお、このように、エンコーダの出力値に所定に対応付けされた関節角度と実際の関節角度との間のずれ量を算出することができれば、関節の原点補正は短時間で済む。

説明すると、上述の実施の形態では、カメラ１６が任意の位置に配置されるため、またロボット１２のフランジ１２ｍに対するマーカー２２の位置が未知であるため、ずれ量を算出することができず、図１０や図１１に示すように、ずれ量がゼロに近づくような補正量を繰り返し算出するとともに、繰り返しロボット１２の姿勢変更を行う必要がある。しかし、ずれ量を算出することができれば、ずれ量を一度でゼロにする補正量は一度の計算で算出することができる。その結果、原点補正に必要な時間は、ずれ量が算出できない場合に比べて、短時間になる。

さらに、これに関連して、ロボット１２のフランジ１２ｍに対するマーカー２２の位置が既知であれば、マーカー２２の空間上の位置（例えば、ベース座標系ΣＢにおける座標）を算出することができる。これにより、フランジ１２ｍに対するマーカー２２の位置が未知である場合に比べて、関節の原点補正は短時間になる。

説明すると、図４および図５に示すねじり関節１２ｊの原点補正および図６および図７に示すねじり関節１２ｌの原点補正において、カメラの撮影画像上において第１の姿勢時の先端点の位置との距離が最小になる位置に先端点が写るような第２の姿勢は、曲げ関節１２ｉの関節角度が異なる複数の姿勢をロボット１２が取ることにより、またその姿勢毎にカメラ１６がマーカー２２を撮影することにより、さらにはカメラ１６が撮影した撮影画像毎にその撮影画像に写るマーカー２２の位置と第１の姿勢時のマーカー２２の位置との距離をコンピュータ１８が算出することにより行われる。すなわち、第２の姿勢を特定するのに時間が必要である。

これに対して、マーカー２２の空間上の位置を算出することができれば、第１の姿勢時のマーカー２２の位置を算出し、その算出した第１の姿勢時のマーカー２２の位置との距離が最小（ゼロを含む）になる位置にマーカー２２が位置する第２の姿勢（すなわち第２の姿勢を実現する関節角度）を算出することができる。それにより、第１の姿勢から第２の姿勢に直接的に姿勢変更することができ、その結果として、フランジ１２ｍに対するマーカー２２の位置が未知である場合に比べて、関節の原点補正は短時間になる。

最後に、図１においてロボット１２、具体的には、ベース１２ｎから順に、ねじり関節１２ｇ、曲げ関節１２ｈ、曲げ関節１２ｉ、ねじり関節１２ｊ、曲げ関節１２ｋ、ねじり関節１２ｌを有するロボット１２の関節の原点補正は、上述に説明した順番どおり、すなわち、（１）曲げ関節１２ｋ、（２）ねじり関節１２ｊ、（３）曲げ関節１２ｈ，１２ｉ、（４）ねじり関節１２ｌの順番で行うのが好ましい。この順番で補正を行えば、原点補正の精度が高くなるためである。その理由は、例えば、ねじり関節１２ｊの原点補正が、図４や図５に示すように、曲げ関節１２ｋを使用するためである。

本発明は、図１に示すような順番で曲げ関節やねじり関節を有するロボット以外にも適用可能である。また、曲げ関節やねじり関節に加えて並進関節を有するロボットにも適用可能であることは明らかである。

１２ ロボット
１２ｌ 関節（ねじり関節）
１６ カメラ
Ｐ_１（ｌ） 第１の姿勢
Ｐ_２（ｌ） 第２の姿勢
Δθ_ｌ ずれ量

Claims (6)

1. 多関節型のロボットの関節角度を検出するエンコーダの出力値と該関節角度の実値との対応関係を補正するロボットの関節の原点補正方法であって、
   第１の姿勢のロボットのマーカーが撮影画像に写る位置にカメラを設置し、
   各関節用のエンコーダの出力値に基づいて、原点補正対象の関節に予め対応付けされた関節のみを動かすことにより、カメラの撮影画像上において第１の姿勢時のマーカーの位置の近傍にマーカーが写るような第２の姿勢に第１の姿勢から変更し、
   第１の姿勢から第２の姿勢への変更によるカメラの撮影画像上におけるマーカーの変位に基づいて、原点補正対象の関節の原点補正を行うロボットの関節の原点補正方法。
2. 原点補正対象は曲げ関節であって、
   曲げ関節を挟んで第１および第２のねじり関節があって、
   曲げ関節の関節角度が基準値のときは第１および第２のねじり関節の関節軸それぞれが同一直線上に位置し、
   曲げ関節用のエンコーダが基準値と所定の対応関係の出力値を出力するように曲げ関節の関節角度を変更することによってロボットを第１の姿勢とし、
   各関節用のエンコーダの出力値に基づいて、
   （１）曲げ関節の関節角度が第１の姿勢時と同一であり、
   （２）第１および第２のねじり関節の関節角度それぞれが第１の姿勢時から１８０度変更され、且つ、
   （３）残りの関節の関節角度が第１の姿勢時と同一である、第２の姿勢に第１の姿勢から変更する請求項１に記載のロボットの関節の原点補正方法。
3. 原点補正対象はねじり関節であって、
   ねじり関節を挟んで第１および第２の曲げ関節があって、
   ねじり関節の関節角度が基準値のときは第１および第２の曲げ関節の関節軸が互いに平行に位置し、
   ねじり関節用のエンコーダが基準値と所定の対応関係の出力値を出力するようにねじり関節の関節角度を変更すること、且つ、ロボットの先端側の第１の曲げ関節の関節角度を、一方の方向に所定の角度変更することによってロボットを第１の姿勢とし、
   各関節用のエンコーダの出力値に基づいて、
   （１）ねじり関節の関節角度が第１の姿勢時と同一であり、
   （２）第１の曲げ関節の関節角度が第１の姿勢時から他方の方向に変更され、
   （３）第２の曲げ関節の関節角度が第１の姿勢時から変更され、且つ
   （４）残りの関節の関節角度が第１の姿勢時と同一である、第２の姿勢に第１の姿勢から変更する請求項１に記載のロボットの関節の原点補正方法。
4. 原点補正対象は第１のねじり関節であって、
   第１のねじり関節の反先端側に別の第２のねじり関節があって、
   第２のねじり関節の反先端側に曲げ関節があって、
   マーカーが第１のねじり関節の関節軸上以外に位置し、
   第１のねじり関節の関節角度が基準値のときはマーカーが曲げ関節の関節軸と直交し、且つ第２のねじり関節の関節軸を含む平面上に位置し、
   第１のねじり関節用のエンコーダが基準値と所定の対応関係の出力値を出力するように第１のねじり関節の関節角度を変更することによってロボットを第１の姿勢とし、
   各関節用のエンコーダの出力値に基づいて、
   （１）第１および第２のねじり関節の関節角度の変更量の合計が１８０度になるように該第１および第２のねじり関節の関節角度が第１の姿勢時から変更され、
   （２）曲げ関節の関節角度が第１の姿勢時から変更され、且つ、
   （３）残りの関節の関節角度が第１の姿勢時と同一である、第２の姿勢に第１の姿勢から変更する請求項１に記載のロボットの関節の原点補正方法。
5. 原点補正対象は関節軸が互いに平行な第１および第２の曲げ関節であって、
   反先端側の第２の曲げ関節のさらに反先端側に第１のねじり関節があって、
   第１の曲げ関節の先端側に第３の曲げ関節があって、
   第３の曲げ関節の先端側に第２のねじり関節があって、
   （ａ）各関節用のエンコーダの出力値に基づいて第１および第２のねじり関節の関節軸が同一直線上に位置するようにロボットを第１の姿勢にし、
   各関節用のエンコーダの出力値に基づいて、
   （１）第１および第２のねじり関節の関節角度それぞれが第１の姿勢時から１８０度変更され、且つ、
   （２）残りの関節の関節角度が第１の姿勢時と同一である、第２の姿勢に第１の姿勢から変更し、
   （ｂ）各関節用のエンコーダの出力値に基づいて第１および第２のねじり関節の関節軸が同一直線上に位置するようにロボットを第３の姿勢にし、
   各関節用のエンコーダの出力値に基づいて、
   （１）第１および第２のねじり関節の関節軸が同一直線上に位置するように維持し、
   （２）第１、第２、および第３の曲げ関節の関節角度それぞれが第３の姿勢時から変更され、且つ、
   （３）残りの関節の関節角度が第３の姿勢時と同一である、第４の姿勢に第３の姿勢から変更し、
   第１の姿勢から第２の姿勢への変更によるカメラの撮影画像上におけるマーカーの第１の変位と、第３の姿勢から第４の姿勢への変更によるカメラの撮影画像上におけるマーカーの第２の変位とに基づいて、原点補正対象の関節の原点補正を行う請求項１に記載のロボットの関節の原点補正方法。
6. 多関節型のロボットの関節角度を検出するエンコーダの出力値と該関節角度の実値との対応関係を補正するロボットの関節の原点補正システムであって、
   第１の姿勢のロボットのマーカーを撮影するカメラと、
   各関節用のエンコーダの出力値に基づいて、原点補正対象の関節に予め対応付けされた関節のみを動かすことにより、カメラの撮影画像上において第１の姿勢時のマーカーの位置の近傍にマーカーが写るような第２の姿勢に第１の姿勢のロボットを制御するロボット制御手段と、
   第１の姿勢から第２の姿勢への変更によるカメラの撮影画像上におけるマーカーの変位量および変位方向を算出するマーカー変位算出手段と、
   マーカー変位算出手段が算出したマーカーの変位量および変位方向に基づいて、原点補正対象の関節の原点補正を行う原点補正手段とを有するロボットの関節の原点補正システム。

Images