JP5505155B2

JP5505155B2 - ロボットシステムおよびロボット制御方法

Info

Publication number: JP5505155B2
Application number: JP2010161720A
Authority: JP
Inventors: 江桁劉
Original assignee: Fuji Electric Co Ltd
Current assignee: Fuji Electric Co Ltd
Priority date: 2010-07-16
Filing date: 2010-07-16
Publication date: 2014-05-28
Anticipated expiration: 2030-07-16
Also published as: JP2012020388A

Description

本発明は、補正によりオフラインで教示されたロボットアーム先端の位置および姿勢を実際の位置と姿勢に一致するようにロボットを制御するロボットシステム、およびロボット制御方法に関するものである。

ロボットを用いたワーク搬送装置は、ワーク供給パレットに整列された複数の対象ワークを、対象ワークごとに教示したロボット先端の位置と姿勢のデータを用い、ロボットで対象ワークを順次取出して所定の位置に搬送する。しかし、供給パレットに大量のワークが配列された時に、全てのワークを個々に教示すると膨大な時間がかかり効率的ではない。

そこで、ワーク供給パレットにある部品の相対位置関係を用いてオフライン教示する方法が盛んになった。これらの従来技術の例として、特許文献１に記載の技術が知られている。

特許文献１に記載の技術は、作業座標系を決定する複数の基準点を記憶している基準点記憶部と、前記基準点に基づいて、前記作業座標系内の作業点の座標を算出する作業座標計算部とを備えたロボット座標系の補正装置において、少なくとも一つの補正用基準点を記憶する補正用基準記憶部と、前記補正用基準点と該補正用基準点に対応する前記作業座標計算部により算出される座標点とに基づいて、補正ベクトルを算出する補正ベクトル計算部と、前記補正ベクトルに基づいて、前記作業点の補正を行う補正作業座標計算部とを備えているロボット座標系の補正装置である。

ここで図７は、特許文献１の技術を説明するための図であり、基準点Ｄ１が作業座標系の原点、基準点Ｄ２およびＤ３がそれぞれ横方向の座標軸Ｘおよび縦方向の座標軸Ｙを決定する点、基準点Ｄ４は補正用基準点、ΔＰが補正ベクトルである。

特開昭６３−７０３０５号公報

しかしながら、多関節の機構をもつロボットは、ロボットの各関節角の基準誤差及びアーム長さの誤差によりロボットの理論上の位置（オフラインで教示した位置）と実際の位置とは必ずしも一致しない。特許文献１に記載した技術では、図８のように全ての対象ワークが同じ平面上（ＸＹ平面）に配置されていることを前提とし、基準点から最も離れた対角点の位置を教示し、その教示した座標データと理論上の座標データと比較して、ロボットの絶対座標と理論値を補正している。しかし、セル生産システムや一つのパレットに複数品種のワークを配置する場合には空間内（Ｘ，Ｙ，Ｚ空間）での位置の誤差やロボット先端の姿勢の誤差を補正する必要があるが、特許文献１に記載された技術は、空間座標系での位置誤差やロボット先端の姿勢誤差については考慮されていないという問題があった。

そこで本発明は、ロボットアーム先端の３次元空間の位置誤差および姿勢誤差を補正して制御するロボットシステムおよびロボット制御方法を提供することにある。

上記課題を解決するために、本発明に係るロボットシステムは、ロボット動作領域内の位置および姿勢を、Ｘ，Ｙ，Ｚ軸座標値とＸ，Ｙ，Ｚ軸回転角度で定めるロボットシステムにおいて、ロボットの動作領域内に、座標系の各平面とそれぞれ平行な面を有する直方体領域を予め定め、前記直方体領域の任意の１頂点を基準頂点とし、オフラインにて前記直方体領域の８頂点の座標系における理論座標値を記憶する第１座標記憶部と、前記理論座標値から前記直方体領域を規定する３辺の長さＬ，Ｗ，Ｈを求める辺長演算部と、オンラインにてロボット先端を前記８頂点に設置された基準マーカに順次一致させ、ロボット先端のロボット座標系の座標値をロボット座標系の実測座標値として記憶する第２座標記憶部と、前記３辺の長さＬ，Ｗ，Ｈと前記実測座標値とから、前記直方体領域内におけるオフラインでの座標値をオンラインでの座標値に補正する座標値補正係数を演算する座標値補正係数演算部と、前記３辺の長さＬ，Ｗ，Ｈと前記実測座標値とから、前記直方体領域内におけるオフラインでの姿勢をオンラインでの姿勢に補正する角度補正係数を演算する角度補正係数演算部と、前記直方体領域内のオフラインで教示された位置および姿勢を前記座標値補正係数と前記角度補正係数とを用いて補正し、補正した位置および姿勢を用いて前記ロボット先端の位置および姿勢を制御するロボット制御部と、を備えたことを特徴とする。

また、本発明に係るロボット制御方法では、ロボット動作領域内の位置および姿勢を、座標系のＸ，Ｙ，Ｚ軸座標値と座標系のＸ，Ｙ，Ｚ軸回転角度で定めるロボットシステムにおいて、ロボットの動作領域内に、座標系の各平面とそれぞれ平行な面を有する直方体領域を予め定め、前記直方体領域の任意の１頂点を基準頂点とし、オフラインにて前記直方体領域の８頂点のロボット座標系における理論座標値を第１座標として記憶し、前記理論座標値から前記直方体領域を規定する３辺の長さＬ，Ｗ，Ｈを求め、オンラインにてロボット先端を前記８頂点に順次一致させ、ロボット先端のロボット座標系の座標値をロボット座標系の実測座標値として第２座標として記憶し、前記３辺の長さＬ，Ｗ，Ｈと前記実測座標値とから、前記直方体領域内におけるオフラインでの座標値をオンラインでの座標値に補正する座標値補正係数を演算し、前記３辺の長さＬ，Ｗ，Ｈと前記実測座標値とから、前記直方体領域内におけるオフラインでの姿勢をオンラインでの姿勢に補正する角度補正係数を演算し、前記直方体領域内のオフラインで教示された位置および姿勢を前記座標値補正係数と前記角度補正係数とを用いて補正し、補正した位置および姿勢を用いて前記ロボット先端の位置および姿勢を制御することを特徴とする。

本発明によれば、予め定められた直方体領域内の８頂点のオフラインでの座標とオンラインで実測した座標とに基づいて、前記直方体領域内の６自由度からなる座標系における位置と姿勢を全て補正することができる。

本発明の実施形態に係るロボットシステムの概略構成図である。ワークを配置した状態の供給パレットの図である。ワークが無い状態の供給パレットの図である。作業座標系と直方体領域の関係図である。治具を配置した状態の供給パレットの図である。オフラインでの頂点座標とオンラインでの頂点座標との関係を示した図である。従来技術における各基準点を示した図である。従来技術におけるワークの状態を示した図である。

本発明に係る実施形態について図を用いて説明する。
図１はロボットシステムのブロック図である。図１におけるロボットシステムは、ロボット２００、ロボットコントローラ３００とから構成されている。

ロボット２００は、ロボット先端位置を所定の３次元領域内で移動させることができると共に、ロボット先端の姿勢を任意の姿勢とすることができる。例えばロボット２００は、６自由度を持った垂直多関節ロボットを用いることができるし、より多くの自由度を有しているロボットを用いることもできる。

ここで、ロボット２００は、ロボット座標系を用いてロボット先端の位置および姿勢が制御される。ロボット座標系におけるロボット先端の位置・姿勢は（Ｘ_R，Ｙ_R，Ｚ_R，θ_XR，θ_YR，θ_ZR）として表現できる。ここで、Ｘ_Rはロボット座標系のＸ座標、Ｙ_Rはロボット座標系のＹ座標、Ｚ_Rはロボット座標系のＺ座標、θ_XRはロボット座標系のＸ軸廻りの回転角度、θ_YRはロボット座標系のＹ軸廻りの回転角度、θ_ZRはロボット座標系のＺ軸廻りの回転角度である。

また、順運動学においてロボット座標系におけるロボット先端の位置・姿勢は、同次変換行列として表現できる。姿勢を表す回転行列^RＲと位置を表すベクトル^RＰで表す事ができ、具体的には数式（１）の行列として表される。

なお、ロボット座標系におけるロボット先端の位置・姿勢（Ｘ_R，Ｙ_R，Ｚ_R，θ_XR，θ_YR，θ_ZR）を上記同次変換行列の形式に変換すること、および、同次変換行列で表現されたロボット先端の位置・姿勢を（Ｘ_R，Ｙ_R，Ｚ_R，θ_XR，θ_YR，θ_ZR）に変換すること、は周知技術であり本発明の主要部分ではないので説明は省略する。

ロボットの動作範囲内にロボット座標系とは異なる作業座標系を設定することができる。作業座標系におけるロボット先端の位置および姿勢は（Ｘ_W，Ｙ_W，Ｚ_W，θ_XW，θ_YW，θ_ZW）とすることができる。ここで、Ｘ_Wは作業座標系のＸ座標、Ｙ_Wは作業座標系のＹ座標、Ｚ_Wは作業座標系のＺ座標、θ_XWは作業座標系のＸ軸廻りの回転角度、θ_YWは作業座標系のＹ軸廻りの回転角度、θ_ZWは作業座標系のＺ軸廻りの回転角度である。

また、順運動学において作業座標系におけるロボット先端の位置・姿勢は、同次変換行列として表現できる。姿勢を表す回転行列^WＲと位置を表すベクトル^WＰで表す事ができ、具体的には数式（２）の行列として表される。

作業座標系は、ロボット座標系で位置および姿勢を表現するよりも、独自の座標系で表現した方がより柔軟に位置および姿勢を表現できる場合に用いられる。例えばワークが供給パレットで供給される場合、供給パレットに作業座標系を設定することで、供給パレット内のワークを把持するためのロボット先端は作業座標系で表現することができる。このようにする事で、ロボット座標系における作業座標系の位置・姿勢が変更されても、ロボット座標系の作業座標系の位置・姿勢を把握すれば、ロボット座標系におけるワークの位置および姿勢を容易に表現できるため、ロボットシステムにおいては通常用いられている。

なお、ロボット座標系における作業座標系の位置および姿勢を同次変換行列で^RＴ_Wとすると、ロボット座標系におけるロボット先端の位置および姿勢^RＴ_Eは、数式（３）となる。

^RＴ_E＝^RＴ_W・^WＴ_E ・・・（３）
図２はワーク供給パレットにワークが置かれた状態の図である。図２において、ワーク供給パレットは１０１、複数種類のワークは１１１，１１２，１１３である。ワーク供給パレットには複数種類のワーク１１１，１１２，１１３が所定の位置に置かれる。ワーク供給パレット１０１は、図３のように、それぞれのワークに対応するワーク固定溝１０６，０１７，１０８を有している。当該溝にワーク１１１，１１２，１１３をはめ込みことで、ワーク供給パレット１０１上においてワーク１１１，１１２，１１３は常に同じ位置に置かれた状態となる。

なお、ワーク供給パレット１０１は、ワーク供給装置によりロボット座標系の常に同じ位置・姿勢にて供給される。これにより、ロボット座標系および作業座標系においてワーク１１１，１１２，１１３は常に同じ位置、同じ姿勢にて供給されることとなる。なお、ロボット座標系における作業座標系の位置・姿勢は（Ｘ_RW，Ｙ_RW，Ｚ_RW，θ_RWX，θ_RWY，θ_RWZ）と表現できる。なお、同次変換行列だと^RＴ_Wと表現される。

ワーク供給パレット１０１には作業座標系が設定されており、ワーク１１１，１１２，１１３をロボットで把持する際のロボット先端の位置・姿勢を作業座標系で表現できる。例えば作業データ７００のワーク１１１のデータは、（Ｘ_W１１１，Ｙ_W１１１，Ｚ_W１１１，θ_XW１１１，θ_YW１１１，θ_ZW１１１）となる。同次変換行列だと^WＴ₁₁₁と表現される。

また、ロボット２００は、後に詳細に記載するロボットコントローラ３００と動力線や信号線等により接続され、ロボットコントローラ３００によりロボット２００のモータ等の動力機器等を制御することでロボット先端の位置と姿勢が制御されている。

ロボットコントローラ３００は、ロボット制御部４００、補正・座標変換部５００、作業シーケンス６００、作業データ７００、第１座標記憶部８００、第２座標記憶部９００から構成されている。

ロボット制御部４００は、補正・座標変換部５００からロボット先端の位置・姿勢データを受け取り、受け取った位置・姿勢データに基づいてロボット２００の動力機器等を制御する。ロボット制御部４００は、ロボット座標系におけるロボット先端の位置・姿勢を補正・座標変換部５００から受け取る。また、ロボット制御部４００は、図示しないがロボット２００の各関節に取り付けられたエンコーダ等の位置情報取得部からロボット２００の各関節の位置情報を受け取り、ロボット座標系のロボット先端の位置・姿勢を求めることができる。

補正・座標変換部５００は、作業シーケンス６００、作業データ７００、第１座標記憶部８００、第２座標記憶部９００から所定のデータを受け取り、そのデータに基づいてロボット先端位置・姿勢を演算し、ロボット制御部４００に出力する。また、ロボット制御部４００から受け取ったロボット座標系のロボット先端の位置・姿勢を作業座標系のロボット先端の位置・姿勢に変換し、第２座標系記憶部９００に記憶させることができる。なお、補正・座標変換部５００は後に詳細に説明する。

作業シーケンス６００は、ロボット２００の予め定められた動作内容および順番である。ロボットコントローラ３００は、作業シーケンス６００に定められた動作内容および手順にてロボット２００が動作するように制御する。例えば、ワーク供給位置にあるワーク１をロボットで把持して所定の位置に搬送する作業シーケンス６００は以下のステップ１〜５のように規定されている。

ステップ１：ロボット待機位置ＰＯＳ１からワーク供給位置ＰＯＳ２にロボット先端を移動する。
ステップ２：ワーク供給位置ＰＯＳ２にて、ワークを把持する。

ステップ３：ワークを把持した状態で、ロボット先端をワーク供給位置ＰＯＳ２から所定位置ＰＯＳ３に移動する。
ステップ４：所定位置ＰＯＳ３にて、ワークを開放する。

ステップ５：所定位置ＰＯＳ３からロボット待機位置ＰＯＳ１にロボット先端を移動する。
作業データ７００は、前記作業シーケンスにて定められた動作内容におけるロボット先端の位置および姿勢のデータである。上記の例を用いれば、ＰＯＳ１〜ＰＯＳ３のそれぞれのロボット先端の位置・姿勢のデータである。なお、作業データ７００に記憶されているロボット先端の位置・姿勢のデータは、ロボット座標系でも作業座標系でも良い。いずれの座標系のデータで記憶されているかがわかるようにされていれば良い。

第１座標記憶部８００は、オフラインにおけるロボット２００の動作領域内に予め定められている直方体領域の８つの頂点の座標を記憶している。直方体領域は、ロボット２００の有する誤差を補正し、ロボット先端の位置・姿勢を制御したい領域である。ここで、直方体領域の一つの頂点は作業座標系の原点と一致し、直方体領域の各平面は作業座標系のＸＹ平面、ＹＺ平面、ＺＸ平面の何れかと平行とする。第１座標記憶部８００に記憶される座標は、オフラインにおける設計上の８つの頂点座標であり、換言すれば実際のロボットに必ず発生する各種の誤差を含まない理想座標といえる。この第１座標記憶部８００に記憶された８つの頂点Ｐ１〜Ｐ８は下記のように表現される。

Ｐ１（ｘｉ１，ｙｉ１，ｚｉ１）
Ｐ２（ｘｉ２，ｙｉ２，ｚｉ２）
Ｐ３（ｘｉ３，ｙｉ３，ｚｉ３）
Ｐ４（ｘｉ４，ｙｉ４，ｚｉ４）
Ｐ５（ｘｉ５，ｙｉ５，ｚｉ５）
Ｐ６（ｘｉ６，ｙｉ６，ｚｉ６）
Ｐ７（ｘｉ７，ｙｉ７，ｚｉ７）
Ｐ８（ｘｉ８，ｙｉ８，ｚｉ８）
図４は本実施形態における作業座標系と直方体領域との関係を示す参考図である。図４においては、作業座標系の各平面と直方体領域の各平面は平行となっている。さらに、Ｐ１と作業座標系の原点が一致しており、作業座標系においてＰ１の座標は（０，０，０）となる。

直方体領域の８頂点は図５のように治具１２１〜１２７を用いて規定する。より具体的には、図２に示す供給パレット１０１の治具用固定穴１０２〜１０５に治具１２１〜１２７を固定し、治具１２１〜１２７のそれぞれ上端面中心点を直方体領域の８頂点とする。本例においては、治具１２１の上端面中心がＰ１、治具１２２の上端面中心がＰ２、治具１２３の上端面中心がＰ３、治具１２４の上端面中心がＰ５、治具１２５の上端面中心がＰ４、治具１２６の上端面中心がＰ６、治具１２７の上端面中心がＰ７、治具１２８の上端面中心がＰ８となる。勿論、ロボット２００で実測できる他の方法により８頂点を規定してもよい。

第２座標記憶部９００は、オンラインにおけるロボット２００の直方体領域の８つの頂点について実測した座標を記憶する。オンラインにおける８つの頂点の実測した座標は、ロボットのアーム先端を前記頂点と一致させた時にロボット制御部４００で得られるロボット先端の座標であり、換言すれば実際のロボットに必ず発生する各種の一切の誤差を含む実測座標といえる。なお、この実測座標は、ロボット制御部４００から得たロボット座標系におけるロボット先端の位置・姿勢を、補正・座標変換部５００で作業座標系のロボット先端の位置・姿勢に変換したものである。この第２座標記憶部９００に記憶された８つの頂点Ｐ１〜Ｐ８は下記のように表現される。

Ｐ１（ｘｔ１，ｙｔ１，ｚｔ１）
Ｐ２（ｘｔ２，ｙｔ２，ｚｔ２）
Ｐ３（ｘｔ３，ｙｔ３，ｚｔ３）
Ｐ４（ｘｔ４，ｙｔ４，ｚｔ４）
Ｐ５（ｘｔ５，ｙｔ５，ｚｔ５）
Ｐ６（ｘｔ６，ｙｔ６，ｚｔ６）
Ｐ７（ｘｔ７，ｙｔ７，ｚｔ７）
Ｐ８（ｘｔ８，ｙｔ８，ｚｔ８）
ここで、図６は本実施形態における第１座標記憶部に記憶された理想座標と第２座標記憶部に記憶された実測座標との関係を示す参考図である。図６では、理想座標による直方体領域を細い実線または細い点線で、実測座標における直方体領域を太い点線で描いている。つまり、実測座標と理想座標は誤差によるズレを生じるため、図６のようにロボット先端の直方体領域の各頂点の実測座標は理想座標とは通常一致しないこととなる。なお本実施形態においては、頂点Ｐ１の理想座標と実測座標が一致しているものとしている。

図５の供給パレットの場合には、アーム先端を治具１２１〜１２８の上端中心点に順次一致させ、そのときの作業座標系の座標を記憶すれば良い。
補正・座標変換部５００は、辺長演算部５１０、座標値補正係数演算部５２０、角度補正係数演算部５３０を有する。なお図示しないが、補正・座標変換部５００は、ロボット先端の位置・姿勢について、作業座標系からロボット座標系への変換、ロボット座標系から作業座標系に変換ができる座標変換機能を有している。

辺長演算部５１０は、第１座標記憶部に記憶された理論座標から直方体領域を規定する３辺の長さＬ，Ｗ，Ｈを求める。ここで、ＬはＸ軸方向の辺長、ＷはＹ軸方向の辺長、ＨはＺ軸方向の辺長とする。図４には、直方体領域の頂点Ｐ１〜Ｐ８と辺長Ｌ，Ｗ，Ｈとの関係が示されている。図４の例においては、Ｌ＝（ｘｉ２−ｘｉ１）、Ｗ＝（ｙｉ３−ｙｉ１）、Ｈ＝（ｚｉ４−ｚｉ１）として求めることができる。

座標値補正演算部５２０は、３辺の長さＬ，Ｗ，Ｈと実測座標とから、直方体領域内におけるオフラインでの座標値を補正する座標値補正係数を演算する。より詳細には、座標値補正演算部５２０は、軸移動補正係数演算部５２１、平面移動補正係数演算部５２２、空間移動補正係数演算部５２３を有し、座標補正係数を演算している。

軸移動補正係数演算部５２１は、基準頂点を含む２頂点を結んだ直線がＸ，Ｙ，Ｚ軸のそれぞれと平行となる３組の２頂点を選択し、前記３組の２頂点の前記実測座標値と前記３辺の長さＬ，Ｗ，Ｈとから、各組に対応するＸ軸補正係数、Ｙ軸補正係数、Ｚ軸補正係数を求める。

以下、Ｘ軸補正係数、Ｙ軸補正係数、Ｚ軸補正係数の導出方法について説明する。なお、説明を簡単にするために頂点Ｐ１の理論座標と実測座標は一致しているものとして説明する。

直方体領域の基準頂点Ｐ１のロボット座標系の座標を（ｘｉ１＝ｘｔ１＝０，ｙｉ１＝ｙｔ１＝０，ｚｉ１＝ｚｔ１＝０）とすると、理論座標（オフライン）においてはＸ軸方向へＬだけ移動した点がＰ２となり、その作業座標系の座標は（ｘｉ２＝Ｌ，ｙｉ２＝０，ｚｉ２＝０）となるが、実測座標（オンライン）におけるＰ２の作業座標系の座標は（ｘｔ２，ｙｔ２，ｚｔ２）となる。一方、誤差が無い場合には、Ｐ２の座標はＰ１の座標を用いて表現すれば（ｘｔ１＋Ｌ，ｙｔ１，ｚｔ１）となる。そこで、Ｘ軸方向のＬ移動分に対する誤差は数式（４）となる。

直方体領域内のＸ軸方向の誤差が移動距離と比例していると仮定し、作業座標系における理論座標の点（Ｘ_W，０，０）における実測座標で座標は数式（５）となる。

つまり、直方体領域内のＸ軸方向の移動における補正係数は数式（６）となる。

Ｘ軸方向の移動と同様にＹ軸方向およびＺ軸方向の移動における補正係数は数式（７）（８）となる。

平面移動補正係数演算部５２２は、前記基準点を含む４頂点から構成される平面がＸＹ，ＹＺ，ＺＸ平面と平行となる３組の４頂点を選択し、前記３組の４頂点の前記実測座標値と前記３辺の長さＬ，Ｗ，Ｈとから、各組に対応するＸＹ平面補正係数、ＹＺ平面補正係数、ＺＸ平面補正係数、を求める。

数式（６）〜（８）は、Ｘ，Ｙ，Ｚ軸の各軸方向に移動する際の補正係数であるが、例えばＸＹ平面上を移動する際にさらに誤差を生じることがある。ここで、代表としてＸＹ平面補正係数の算出方法を説明する。理論座標でＰ１からＰ５に移動する場合、Ｘ方向に理論座標でＬ、Ｙ方向へ理論座標でＷ、だけ移動することとなるので、Ｐ５の座標値はＰ１の理論座標にＰ１からＰ２へのベクトルとＰ１からＰ３へのベクトルとのベクトル和になる。そこで、Ｐ５におけるＸＹ平面上を移動する際の誤差は数式（９）のように表される。

さらに、直方体領域内のＸＹ平面の誤差が移動距離と比例していると仮定し、直方体領域内のＸＹ平面の移動に伴う補正係数は数式（１０）となる。

同様にＹＺ平面補正係数はＰ１，Ｐ３，Ｐ４，Ｐ７より、ＺＸ平面補正係数はＰ１，Ｐ２，Ｐ４，Ｐ６より、数式（１１）（１２）を求めることができる。

空間移動補正係数演算部５２３は、前記８頂点と前記３辺の長さＬ，Ｗ，Ｈとから、ＸＹＺ空間補正係数を求める。

数式（６）〜（８）および数式（１０）〜（１２）は、Ｘ，Ｙ，Ｚ軸の各軸方向に移動する際の補正係数、ＸＹ平面、ＹＺ平面、ＺＸ平面を移動する際の補正係数であるが、ＸＹＺ空間を移動する際にさらに誤差を生じることがある。ここで、理論座標でＰ１からＰ８に移動する場合、Ｘ方向に理論座標でＬ、Ｙ方向へ理論座標でＷ、Ｚ方向へ理論座標でＨだけ移動することとなる。さらに、大きさがＬ，Ｗ，Ｈとなるベクトルの点が重複しないようにすると、Ｐ８の座標値はＰ１の理論座標にＰ４からＰ６へのベクトルとＰ２からＰ５へのベクトルとＰ３からＰ７へのベクトルとのベクトル和になる。そこで、Ｐ８におけるＸＹＺ空間を移動する際の誤差は数式（１０）のように表される。

そこで、直方体領域内のＸＹ平面の移動に伴う補正係数は数式（１４）となる。

ここで、直方体領域内での作業座標系の理論座標の点（Ｘ_W，Ｙ_W，Ｚ_W）における各軸毎の補正量を（ｄＸ_W，ｄＹ_W，ｄＺ_W）とすると、上記の数式（６）〜（８）、数式（１０）〜（１２）および数式（１４）を用いて数式（１５）のように表される。なお、ここで、Ｐ１の理想座標（ｘｉ１，ｙｉ１，ｚｉ１）と実測座標（ｘｔ１，ｙｔ１，ｚｔ１）とは、一致しているものとしている。

角度補正係数演算部５３０は、Ｘ軸廻り角度補正係数、Ｙ軸廻り角度補正係数、Ｚ軸廻り角度補正係数を求めるに際し、Ｘ，Ｙ，Ｚ軸から第１軸を選択し、前記第１軸に直交する１つの軸をＸ，Ｙ，Ｚ軸から重複せずに第２軸として選択し、２頂点を結んだ直線が前記第２軸と平行となる４組の２頂点を選択し、前記選択された組ごとの前記第１軸の座標値成分を除く２頂点の実測座標値の差分値と、前記第２軸方向の辺の長さと等しいものを除く前記３辺の長さＬ，Ｗ，Ｈのいずれか２つとから、第１軸廻りの角度補正係数を演算する。

まず、Ｚ軸周りの角度補正係数の算出方法について説明する。
Ｘ，Ｙ，Ｚ軸からＺ軸を第１軸として選択する。さらに、ここでは、第１軸（Ｚ軸）に直交するＸ軸を第２軸として選択する。なお、第２軸は重複して選択できない。つまり、その後に第１軸にＹ軸を選択した場合には、Ｘ軸が選択できなくなる。次に、２頂点を結んだ直線が第２軸であるＸ軸と平行となる｛（Ｐ１，Ｐ２），（Ｐ３，Ｐ５），（Ｐ４，Ｐ６），（Ｐ７，Ｐ８）｝の４組の２頂点を選択する。

次に、基準頂点Ｐ１を含む２頂点の組（Ｐ１，Ｐ２）を用いて、第１軸であるＺ軸周りの第２軸であるＸ軸に対する、基準頂点Ｐ１における回転角度を数式（１６）により求める。

さらに、回転基準点が基準頂点Ｐ１よりＹ軸方向に移動することによる補正係数を、（Ｐ３，Ｐ５）と（Ｐ１，Ｐ２）の２組の２頂点より数式（１７）により求める。

なおさらに、回転基準点が基準頂点Ｐ１よりＺ軸方向に移動することによる補正係数を、（Ｐ４，Ｐ６）と（Ｐ１，Ｐ２）の２組の２頂点より数式（１８）により求める。

また、回転基準点が基準頂点Ｐ１よりＹＺ平面内に移動することによる補正係数を、（Ｐ７，Ｐ８）と（Ｐ４，Ｐ６）と（Ｐ１，Ｐ２）の３組の２頂点より数式（１９）により求める。

直方体領域内での作業座標系の理論座標の点（Ｘ_W，Ｙ_W，Ｚ_W）におけるＺ軸廻りの角度補正量ｄθ_ZWは、上記数式（１７）〜（１９）を用いると数式（２０）となる。

同様に、作業座標系の理論座標の点（Ｘ_W，Ｙ_W，Ｚ_W）におけるＸ軸廻りの角度補正量ｄθ_XWおよびＹ軸廻りの角度補正量ｄθ_YWは数式（２１）（２２）となる。

以上より、作業座標系における理論座標および理論姿勢（Ｘ_W，Ｙ_W，Ｚ_W，θ_XW，θ_YW，θ_ZW）を補正した作業座標系におけるロボット先端の補正座標および補正姿勢（Ｘｒ_W，Ｙｒ_W，Ｚｒ_W，θｒ_XW，θｒ_YW，θｒ_ZW）は、数式（１５）（２０）（２１）（２２）を用いて数式（２３）となる。

数式（２３）で求めた作業座標系におけるロボット先端の補正座標および補正姿勢（Ｘｒ_W，Ｙｒ_W，Ｚｒ_W，θｒ_XW，θｒ_YW，θｒ_ZW）を補正・座標変換部５００でロボット座標系に変換し、ロボット制御部４００に入力してロボットを制御することで、直方体領域内におけるロボット先端の位置・姿勢をロボット２００が有する各種の誤差を補正した位置・姿勢として制御することができる。

なお、作業座標系におけるロボット先端の補正後の位置・姿勢を同次変換行列で^WＴ_Erとすると、ロボット座標系におけるロボット先端の補正後の位置・姿勢^RＴ_Erは、前記数式（３）を利用すると数式（２４）で変換できる。

^RＴ_Er＝^RＴ_W・^WＴ_Er・・・（２４）

２００ロボット
３００ロボットコントローラ
４００ロボット制御部
５００補正・座標変換部
６００作業シーケンス
７００作業データ
８００第１座標記憶部
９００第２座標記憶部

Claims

ロボット動作領域内の位置および姿勢を、直交座標系のＸ，Ｙ，Ｚ軸座標値とＸ，Ｙ，Ｚ軸廻りの回転角度で定めるロボットシステムにおいて、
ロボットの動作領域内に、座標系の各平面とそれぞれ平行な面を有する直方体領域を予め定め、前記直方体領域の任意の１頂点を基準頂点とし、オフラインにて前記直方体領域の８頂点の座標系における理論座標値を記憶する第１座標記憶部と、
前記理論座標値から前記直方体領域を規定する３辺の長さＬ，Ｗ，Ｈを求める辺長演算部と、
オンラインにてロボット先端を前記８頂点に設置された基準マーカに順次一致させ、ロボット先端のロボット座標系の座標値をロボット座標系の実測座標値として記憶する第２座標記憶部と、
前記３辺の長さＬ，Ｗ，Ｈと前記実測座標値とから、前記直方体領域内におけるオフラインでの座標値をオンラインでの座標値に補正する座標値補正係数を演算する座標値補正係数演算部と、
前記３辺の長さＬ，Ｗ，Ｈと前記実測座標値とから、前記直方体領域内におけるオフラインでの姿勢をオンラインでの姿勢に補正する角度補正係数を演算する角度補正係数演算部と、
前記直方体領域内のオフラインで教示された位置および姿勢を前記座標値補正係数と前記角度補正係数とを用いて補正し、補正した位置および姿勢を用いて前記ロボット先端の位置および姿勢を制御するロボット制御部と、
を備えたことを特徴とするロボットシステム。
前記座標補正係数演算部は、
前記基準頂点を含む２頂点を結んだ直線がＸ，Ｙ，Ｚ軸と平行となる３組の２頂点を選択し、前記３組の２頂点の前記実測座標値と前記３辺の長さＬ，Ｗ，Ｈとから、各組に対応するＸ軸補正係数、Ｙ軸補正係数、Ｚ軸補正係数を求める軸移動補正係数演算部と、
前記基準点を含む４頂点から構成される平面がＸＹ，ＹＺ，ＺＸ平面と平行となる３組の４頂点を選択し、前記３組の４頂点の前記実測座標値と前記３辺の長さＬ，Ｗ，Ｈとから、各組に対応するＸＹ平面補正係数、ＹＺ平面補正係数、ＺＸ平面補正係数、を求める平面移動補正係数演算部と、
前記８頂点と前記３辺の長さＬ，Ｗ，Ｈとから、ＸＹＺ空間補正係数を求める空間移動補正係数演算部と、
から構成されていることを特徴とする請求項１に記載のロボットシステム。
前記軸移動補正係数演算部は、各組ごとに前記実測座標値における前記２頂点の座標値の差分値をオンラインで前記選択された２頂点と等しい長さである前記３辺の長さＬ，Ｗ，Ｈのいずれか１つで除算し、各組に対応するＸ軸補正係数、Ｙ軸補正係数、Ｚ軸補正係数を求めること特徴とする請求項２に記載のロボットシステム。
前記平面移動補正係数演算部は、各組ごとに前記基準頂点と前記軸移動補正係数演算部で選択されていない頂点の前記実測座標値を和算し、残りの２頂点の前記実測座標値を差算した値をオンラインで前記組に含まれる４頂点から構成される長方形の辺と等しい長さである前記３辺の長さＬ，Ｗ，Ｈのいずれか２つで除算し、各組に対応するＸＹ平面補正係数、ＹＺ平面補正係数、ＺＸ平面補正係数を求めること特徴とする請求項２に記載のロボットシステム。
空間移動補正係数演算部は、前記基準頂点と前記軸移動補正係数演算部で選択されていない頂点とを除く頂点の前記実測座標値を加算し、前記基準頂点と前記軸移動補正係数演算部で選択されていない頂点との前記実測座標値を差算した値を前記３辺の長さＬ，Ｗ，Ｈで除算し、ＸＹＺ空間補正係数を求めること特徴とする請求項２に記載のロボットシステム。
前記角度補正係数演算部は、Ｘ軸廻り角度補正係数、Ｙ軸廻り角度補正係数、Ｚ軸廻り角度補正係数を求めるに際し、
Ｘ，Ｙ，Ｚ軸から第１軸を選択し、
前記第１軸に直交する１つの軸をＸ，Ｙ，Ｚ軸から重複せずに第２軸として選択し、
２頂点を結んだ直線が前記第２軸と平行となる４組の２頂点を選択し、
前記選択された組ごとの前記第１軸の座標値成分を除く２頂点の実測座標値の差分値と、前記第２軸方向の辺の長さと等しいものを除く前記３辺の長さＬ，Ｗ，Ｈのいずれか２つとから、第１軸廻りの角度補正係数を演算すること、を特徴とする請求項１ないし５のいずれか１項に記載のロボットシステム。
動作領域内の位置および姿勢を、直交座標系のＸ，Ｙ，Ｚ軸座標値とＸ，Ｙ，Ｚ軸廻りの回転角度で定めるロボットの制御方法において、
ロボットの動作領域内に、座標系の各平面とそれぞれ平行な面を有する直方体領域を予め定め、前記直方体領域の任意の１頂点を基準頂点とし、オフラインにて前記直方体領域の８頂点のロボット座標系における理論座標値として記憶し、
前記理論座標値から前記直方体領域を規定する３辺の長さＬ，Ｗ，Ｈを求め、
オンラインにてロボット先端を前記８頂点に順次一致させ、ロボット先端のロボット座標系の座標値をロボット座標系の実測座標値として記憶し、
前記３辺の長さＬ，Ｗ，Ｈと前記実測座標値とから、前記直方体領域内におけるオフラインでの座標値をオンラインでの座標値に補正する座標値補正係数を演算し、
前記３辺の長さＬ，Ｗ，Ｈと前記実測座標値とから、前記直方体領域内におけるオフラインでの姿勢をオンラインでの姿勢に補正する角度補正係数を演算し、
前記直方体領域内のオフラインで教示された位置および姿勢を前記座標値補正係数と前記角度補正係数とを用いて補正し、補正した位置および姿勢を用いて前記ロボット先端の位置および姿勢を制御するロボット制御方法。