JP5963515B2

JP5963515B2 - ツールベクトルの自動設定方法

Info

Publication number: JP5963515B2
Application number: JP2012096975A
Authority: JP
Inventors: 信高坪井
Original assignee: Mitsubishi Electric Corp
Current assignee: Mitsubishi Electric Corp
Priority date: 2012-04-20
Filing date: 2012-04-20
Publication date: 2016-08-03
Anticipated expiration: 2032-04-20
Also published as: JP2013223901A

Description

本発明は、産業用などのロボットにおけるエンドファクタの作業点、即ち、ツール先端点の自動設定方法に関する。

ロボットの手首のツール取付け面、即ち、フェイスプレートにツールを取付ける場合、フェイスプレートの中心点を原点とするフェイスプレート座標系におけるツール先端点の座標位置を設定しなければならない。測定器具によるツール測定や設計図等を必要とせず、ツール先端点を設定できる自動設定方式として、４回（４点）以上フェイスプレートの中心点座標位置を教示し、ツール先端点の座標位置を計算して求める技術が開示されている（例えば、特許文献１参照）。

特公平８−３８４号公報特許第２７７４９３９号公報

しかしながら、上記従来の技術によれば、求めるツール先端点の誤差が計算に使用する教示点の誤差より大きくなるため、正確な設定になっていないという問題があった。また、計算するだけで誤差の表示がなかったので、計算した値がどれだけ信用のおける数字か判断がつかないという問題があった。さらに、一点でも精度の悪い教示をすることにより全体に影響を及ぼし、設定を正確に求めることができない場合があり、その場合は初めから教示をやり直す必要があるという問題があった。

本発明は、上記に鑑みてなされたものであって、少ない回数の教示で高い精度のツールベクトルを得ることができるツールベクトルの自動設定方法を得ることを目的とする。

上述した課題を解決し、目的を達成するために、本発明は、ロボットアームのフェイスプレートに装着されたツールの先端点を、当該ロボットアームの姿勢および当該フェイスプレートの中心位置のいずれかが異なる少なくとも３つ以上の状態で空間上の任意の一点に位置づけることにより、各々の前記状態における前記ロボットアームの姿勢ベクトルおよび前記フェイスプレートの中心位置座標に基づいて、前記フェイスプレートの中心点を原点とするフェイスプレート座標系における前記ツールの先端点の座標位置であるツールベクトルを求めることを特徴とする。

本発明によれば、少ない回数の教示で高い精度のツールベクトルを得ることができるという効果を奏する。

図１は、ツール装着状態で様々な向きから同一のポイントを指し示す状態を教示する様子を示す図である。図２は、本実施の形態にかかるツールベクトルの自動設定方法のフローチャートの一例を示す図である。図３は、本実施の形態におけるベース座標系での教示の目標点、ロボットのアーム、およびツールベクトルの関係を示した図である。

以下に、本発明にかかるツールベクトルの自動設定方法の実施の形態を図面に基づいて詳細に説明する。なお、この実施の形態によりこの発明が限定されるものではない。

実施の形態．
産業用などのロボットにおいては、ロボットアームの先端のフェイスプレート（フランジとも呼ばれる）にツールを装着した後に、フェイスプレートの中心点を原点とするフェイスプレート座標系におけるツール先端点の座標位置（ツールベクトル）を設定しなければならない。このために、図１に示すように、ツール装着状態で様々な向きから同一のポイントを指し示す状態を教示する必要がある。

本実施の形態においては、ベース座標系でのフェイスプレートの中心点の座標（以下で述べる（Ｏ_X,Ｏ_Y,Ｏ_Z））に加えてロボットアームの姿勢の情報（以下で述べるアーム姿勢ベクトル１０，１１，１２）も利用して計算する。これにより、大きさの不明なツールをロボットに装着した状態で、ツール先端で同一のポイントを最低３回、即ち少なくとも３回以上教示することにより、ツール座標点、全体の誤差、各教示点の誤差を計算することができる。

本実施の形態にかかるツールベクトルの自動設定方法の一例を、図２に示したフローチャートと、ベース座標系６における教示の目標点４、ロボットのアーム１、およびツールベクトル８の関係を示した図３を用いて説明する。まず、図３の同一の教示の目標点４をツール先端が指すように少なくとも３回以上であるｎ回教示する（ステップＳ１）。そして、このｎ回の教示点に基づいて、以下に詳述するように、ツール座標点、全体の誤差、各教示点の誤差を順次算出する(ステップＳ２)。

（ツール座標点の求め方）
図３に示すように、ベース座標系６の原点から教示の目標点４へのベクトルをベクトル７、フェイスプレートの中心点３から教示の目標点４へ向うツールのベクトルをツールベクトル８とし、ロボットのアーム１の姿勢を示すベクトルをアーム姿勢ベクトル１０，１１，１２とする。ツールベクトル８を（Ｘ_H,Ｙ_H,Ｚ_H）、ベクトル７を（Ｘ_B,Ｙ_B,Ｚ_B）、アーム姿勢ベクトル１０，１１，１２を各々（ｌ_X,ｌ_Y,ｌ_Z），（ｍ_X,ｍ_Y,ｍ_Z），（ｎ_X,ｎ_Y,ｎ_Z）、フェイスプレートの中心点３の座標を（Ｏ_X,Ｏ_Y,Ｏ_Z）とすると、以下の式（１）が成り立つ。

ここで、求めたいのはツールベクトル８の成分（Ｘ_H,Ｙ_H,Ｚ_H）である。１〜ｎのｎ個の教示点のなかのｉ番目の教示点における姿勢ベクトルを（ｌ_Xi,ｌ_Yi,ｌ_Zi），（ｍ_Xi,ｍ_Yi,ｍ_Zi），（ｎ_Xi,ｎ_Yi,ｎ_Zi）、フェイスプレートの中心点３の座標を（Ｏ_Xi,Ｏ_Yi,Ｏ_Zi）とし、ｊ番目の教示点における姿勢ベクトルを（ｌ_Xj,ｌ_Yj,ｌ_Zj），（ｍ_Xj,ｍ_Yj,ｍ_Zj），（ｎ_Xj,ｎ_Yj,ｎ_Zj）、フェイスプレートの中心点３の座標を（Ｏ_Xj,Ｏ_Yj,Ｏ_Zj）とする。このとき、（Ｘ_B,Ｙ_B,Ｚ_B）と（Ｘ_H,Ｙ_H,Ｚ_H）は一定なので以下の式が成り立つ。

これを移項すると、以下の式（２）が成り立つ。

上の式（２）を簡単に表現すると以下の式（３）のようになる。

最小二乗法により、最もよく上の式（３）を満たす（Ｘ_H,Ｙ_H,Ｚ_H）は、以下の式（４）を最小化する。

式（４）をＸ_Hについて偏微分して０とおくと以下の式（５）が成り立つ。

Ｙ_HおよびＺ_Hについても同様の式を求め、さらにｉ≠ｊを満たす（ｉ，ｊ）について１〜ｎまで足し合わせると以下の式（６）が成り立つ。

上の式（６）の左辺の行列をＰとおくと、以下の式（７）のようにフェイスプレートの中心点を原点とするフェイスプレート座標系におけるツール先端点の座標位置であるツールベクトル８（Ｘ_H,Ｙ_H,Ｚ_H）が求められる。

（計算結果の全体の誤差の求め方）
計算結果の全体の誤差はスカラーである。上述した（ツール座標点の求め方）においては、偏りをなくすために全ての式について万遍なく合計したが、誤差を求める場合には独立した成分のみを計算する。式（３）には３つの等式があるが、そのうち独立した式は２つである。また、互いに異なる教示点の選び方は、（ｉ，ｊ）（ｉ，ｊ＝１〜ｎ）のうち（１，２），（２，３），・・・・，（ｎ−１，ｎ）の（ｎ−１）通りを考えれば十分である。

まず、始めに、（２ｎ−２）×３の行列Ｘを定義する。行列Ｘの第１，２行はｉ＝１，ｊ＝２とした場合の式（３）の

である。
行列Ｘの第３，４行はｉ＝２，ｊ＝３とした場合の式（３）の

である。
行列Ｘの第（２ｎ−３），（２ｎ−２）行はｉ＝ｎ−１，ｊ＝ｎとした場合の式（３）の

である。

最小二乗法による誤差分散を求める。最小二乗法による誤差分散σ²は以下の式（８）で求められる。

（「東京大学教養学部統計学教室編：基礎統計学III 自然科学の統計学」より式（２．４７）を引用）
ただし、Ｎは最小二乗法で使われる独立した式の数であり、この場合Ｎ＝２ｎ−２である。Ｓは残差平方和で、この場合は以下の式で表される。

ｒａｎｋ（Ｘ）は、求める未知数の数であり、この場合は３である。

そして、σ²を（Ｘ_H,Ｙ_H,Ｚ_H）の誤差に変換する。（Ｘ_H,Ｙ_H,Ｚ_H）の誤差（分散）は、

で求められる。ここで、Ｘ’はＸの転置行列であり、Ｔｒはトレース（対角和）である。
（「東京大学教養学部統計学教室編：基礎統計学III 自然科学の統計学」より式（２．４２）を引用）

上の式（１０）は（Ｘ_H,Ｙ_H,Ｚ_H）の分散なので、標準偏差にするために平方根をとる。最終的な（Ｘ_H,Ｙ_H,Ｚ_H）の標準偏差は式（１０）に式（８）を代入して、以下の式（１１）で求められる。

（各教示点の誤差の求め方）
各教示点の誤差は教示点ごとに生じ、教示点の数であるｎ個のベクトルで示される。（ツール座標点の求め方）で求めたツールベクトル８（Ｘ_H,Ｙ_H,Ｚ_H）を用いると、各教示点のツール先端点（Ｘ_Bi,Ｙ_Bi,Ｚ_Bi）は以下の式（１２）で求められる。

これからツール先端点の平均が以下の式（１３）で求められる。

このようにして得られた（Ｘ_Bav,Ｙ_Bav,Ｚ_Bav）を最も確からしいベース座標系６における教示の目標点４の座標（Ｘ_B,Ｙ_B,Ｚ_B）であると考えると、各教示点の誤差（Ｘ_er,Ｙ_er,Ｚ_er）は以下の式（１４）で求められる。

以上のようにしてステップＳ２を実行した後、ステップＳ２で求めた全体の誤差（式（８）、式（９）（誤差の二乗和）、式（１０）あるいは式（１１））が所定の精度を満たすか否か、例えば所定の閾値以下となるか否かを判定する（ステップＳ３）。ここで、全体の誤差としては、上述した以外のものを用いても構わない。例えば、教示毎の誤差に依存した何らかの非負の値を全ての教示について合計した数値であっても構わない。その結果、所定の精度を満たす場合（ステップＳ３：Ｙｅｓ）は終了であるが、所定の精度を満たさない場合（ステップＳ３：Ｎｏ）はさらにステップＳ４に進む。ステップＳ４では、ステップＳ２で求めた各教示点の誤差（式（１４））が所定の値（第２閾値）より大きいものがあるか否かを判定する。

教示点の誤差が所定の値より大きいものがある場合（ステップＳ４：Ｙｅｓ）は、当該教示点における姿勢ベクトル（ｌ_Xi,ｌ_Yi,ｌ_Zi），（ｍ_Xi,ｍ_Yi,ｍ_Zi），（ｎ_Xi,ｎ_Yi,ｎ_Zi）、およびフェイスプレートの中心点３の座標（Ｏ_Xi,Ｏ_Yi,Ｏ_Zi）を破棄し、そのような教示点の数だけ新たに教示を行って（ステップＳ５）、新たな姿勢ベクトル（ｌ_Xi,ｌ_Yi,ｌ_Zi），（ｍ_Xi,ｍ_Yi,ｍ_Zi），（ｎ_Xi,ｎ_Yi,ｎ_Zi）、およびフェイスプレートの中心点３の座標（Ｏ_Xi,Ｏ_Yi,Ｏ_Zi）を取得する。即ち、誤差が所定の値より大きい教示点のデータは破棄し、破棄した教示点の数だけ新たに教示してデータを得る。すなわち、各教示点の誤差のうち、誤差が特に大きい精度の悪い教示点がある場合、その教示点は教示の目標点４の教示に失敗した可能性が大きい。従って、精度を高めるために、そのような教示点については教示し直す。そして、ステップＳ２に再び入って、ツール座標点、全体の誤差、各教示点の誤差を再計算して更新する。

教示点の誤差が所定の値より大きいものがない場合（ステップＳ４：Ｎｏ）は、既に教示したｎ個の教示点に加えて新たな教示点を追加する（ステップＳ６）。即ち、既に得ているｎセットの姿勢ベクトル（ｌ_Xi,ｌ_Yi,ｌ_Zi），（ｍ_Xi,ｍ_Yi,ｍ_Zi），（ｎ_Xi,ｎ_Yi,ｎ_Zi）、およびフェイスプレートの中心点３の座標（Ｏ_Xi,Ｏ_Yi,Ｏ_Zi）（ｉ＝１〜ｎ）に加えて、新たな姿勢ベクトル（ｌ_Xk,ｌ_Yk,ｌ_Zk），（ｍ_Xk,ｍ_Yk,ｍ_Zk），（ｎ_Xk,ｎ_Yk,ｎ_Zk）、およびフェイスプレートの中心点３の座標を（Ｏ_Xk,Ｏ_Yk,Ｏ_Zk）を取得する。新たに追加する教示点は１つずつでもよいが、２以上の数ずつでもよい。そして、ステップＳ２に再び入って、ツール座標点、全体の誤差、各教示点の誤差を再計算して更新する。基本的に教示点の数が多ければ多いほどツール座標点の精度は高く（全体の誤差が小さく）なる。従って、全体の誤差が所定の精度を満たさない（ステップＳ３：Ｎｏ）にも拘わらず、教示点の誤差が所定の値より大きいものがない場合（ステップＳ４：Ｎｏ）は、全体の誤差を小さくして精度を高めるために、ステップＳ６にて新しい教示点を追加する。

以上述べたように、ステップＳ５およびＳ６からステップＳ２へのループを、全体の誤差が所定の精度を満たす（ステップＳ３：Ｙｅｓ）まで繰り返す。このようにしてフェイスプレートの中心点を原点とするフェイスプレート座標系におけるツール先端点の座標位置、即ち、ツールベクトルを自動設定することにより、得られたツールベクトルが要求する精度以上であることをユーザが確認してから、ツールを使用することができる。

さらに、本願発明は上記実施の形態に限定されるものではなく、実施段階ではその要旨を逸脱しない範囲で種々に変形することが可能である。また、上記実施の形態には種々の段階の発明が含まれており、開示される複数の構成要件における適宜な組み合わせにより種々の発明が抽出されうる。例えば、上記実施の形態に示される全構成要件からいくつかの構成要件が削除されても、発明が解決しようとする課題の欄で述べた課題が解決でき、発明の効果の欄で述べられている効果が得られる場合には、この構成要件が削除された構成が発明として抽出されうる。更に、異なる実施の形態にわたる構成要素を適宜組み合わせてもよい。

以上のように、本発明にかかるツールベクトルの自動設定方法は、産業用などのロボットにおけるエンドファクタの作業点、即ち、ツール先端点の自動設定に有用であり、特に、高い精度でツール先端点を自動設定する用途に適している。

１ロボットのアーム
３フェイスプレートの中心点
４教示の目標点
６ベース座標系
７ベクトル
８ツールベクトル
１０，１１，１２アーム姿勢ベクトル
Ｓ１〜Ｓ６ステップ

Claims

ロボットアームのフェイスプレートに装着されたツールの先端点を、当該ロボットアームの姿勢および当該フェイスプレートの中心位置のいずれかが異なる少なくとも３つ以上の状態で空間上の任意の一点に位置づけることにより、各々の前記状態における前記ロボットアームの姿勢ベクトルおよび前記フェイスプレートの中心位置座標に基づいて、前記フェイスプレートの中心位置を原点とするフェイスプレート座標系における前記ツールの先端点の座標位置であるツールベクトルを求めるツールベクトルの自動設定方法であって、
各々の前記状態における前記ロボットアームの姿勢ベクトルおよび前記ツールベクトルから得られる前記フェイスプレートの中心位置から前記任意の一点へ向けたベクトルと当該状態における前記フェイスプレートの中心位置座標の和の間の誤差に基づいて最小二乗法により前記ツールベクトルを求める
ことを特徴とするツールベクトルの自動設定方法。
各々の前記状態にわたる前記誤差に依存した非負の値の合計が所定の閾値より大きい場合は、前記ロボットアームの姿勢および前記フェイスプレートの中心位置のいずれかが各々の前記状態とは異なる新たな状態で前記ツールの先端点を前記任意の一点に位置づけることにより、当該新たな状態における前記ロボットアームの姿勢ベクトルおよび前記フェイスプレートの中心位置座標にも基づいて、前記ツールベクトルを求める
ことを特徴とする請求項１に記載のツールベクトルの自動設定方法。
各々の前記状態にわたる前記誤差に依存した非負の値の合計が所定の閾値より大きい場合で、かつ前記誤差が所定の第２閾値より大きい場合は、当該誤差に対応する前記ロボットアームの姿勢ベクトルおよび前記フェイスプレートの中心位置座標を用いないで前記ツールベクトルを求める
ことを特徴とする請求項２に記載のツールベクトルの自動設定方法。
前記誤差に依存した非負の値は、前記誤差の二乗である
ことを特徴とする請求項２または３に記載のツールベクトルの自動設定方法。