JP5423520B2

JP5423520B2 - 位置計測システム、位置計測装置及び位置計測プログラム

Info

Publication number: JP5423520B2
Application number: JP2010068628A
Authority: JP
Inventors: 和敏谷田; 宏之堀田; 泰之佐口
Original assignee: Fuji Xerox Co Ltd; Fujifilm Business Innovation Corp
Current assignee: Fujifilm Business Innovation Corp
Priority date: 2010-03-24
Filing date: 2010-03-24
Publication date: 2014-02-19
Anticipated expiration: 2030-03-24
Also published as: JP2011200331A

Description

本発明は、位置計測システム、位置計測装置及び位置計測プログラムに関する。

従来、作業者の所定の部位にマーカーを設置し、このマーカーを撮影した画像に基づいて作業者の動作を観測するシステムが知られている（例えば、特許文献１参照）。

特許文献１に記載の作業指示システムは、作業者の腕、肩、頭などの部位にマーカーを設置し、このマーカーをビデオカメラで撮影した映像記録に基づいて作業者の行動を把握し、把握した作業者の行動の情報を基準の情報と比較し、比較の結果に基づいて作業者への指示を行うように構成されている。

特開２００６−２３９０１４号公報

本発明の目的は、標識装置が装着された腕部と把持対象物を把持する把持部を有する手部とが屈曲した場合でも、その屈曲を考慮して把持部の位置を演算することができる位置計測システム、位置計測装置及び位置計測プログラムを提供することにある。

本発明の一態様は、上記目的を達成するため、以下の位置計測システム、位置計測装置及び位置計測プログラムを提供する。
［１］把持対象物を把持する把持部を有する手部が屈曲可能に連結された腕部に装着され、少なくとも３つの位置標識を有する標識装置と、前記標識装置を撮影する撮影装置と、前記撮影装置により撮影した画像における前記少なくとも３つの位置標識の位置関係に基づいて前記腕部の位置及び向きを演算する第１の演算手段と、前記第１の演算手段が演算した前記腕部の位置及び向きに基づき、前記腕部の向きに応じた前記腕部と前記手部との屈曲を考慮して、前記把持部の位置を演算する第２の演算手段とを備えた位置計測システム。

［２］前記第２の演算手段は、前記第１の演算手段で演算した前記腕部の向きが設定角度よりも上向きであるとき、前記腕部と前記手部との屈曲がないとした場合の前記把持部の位置よりも前記把持部の位置が低くなるように補正を行い、前記第１の演算手段で演算した前記腕部の向きが設定角度よりも下向きであるとき、前記腕部と前記手部との屈曲がないとした場合の前記把持部の位置よりも前記把持部の位置が高くなるように補正を行う前記［１］に記載の位置計測システム。

［３］前記第２の演算手段は、前記把持対象物が載せられた支持部材の傾斜を考慮に加えて前記把持部の位置を演算する前記［１］又は［２］に記載の位置計測システム。

［４］把持対象物を把持する把持部を有する手部が屈曲可能に連結された腕部に装着される少なくとも３つの位置標識を有する標識装置を撮影した画像を取得する画像取得手段と、前記画像取得手段により取得した画像における前記少なくとも３つの位置標識の位置関係に基づいて前記腕部の位置及び向きを演算する第１の演算手段と、前記第１の演算手段が演算した前記腕部の位置及び向きに基づき、前記腕部の向きに応じた前記腕部と前記手部との屈曲を考慮して、前記把持部の位置を演算する第２の演算手段とを備えた位置計測装置。

［５］把持対象物を把持する把持部を有する手部が屈曲可能に連結された腕部に装着される少なくとも３つの位置標識を有する標識装置を撮影した画像を取得する画像取得ステップと、前記画像取得手段により取得した画像における前記少なくとも３つの位置標識の位置関係に基づいて前記腕部の位置及び向きを演算する第１の演算ステップと、前記第１の演算手段が演算した前記腕部の位置及び向きに基づき、前記腕部の向きに応じた前記腕部と前記手部との屈曲を考慮して、前記把持部の位置を演算する第２の演算ステップとを備えた位置計測プログラム。

請求項１，４，５に記載の発明によれば、標識装置が装着された腕部と把持対象物を把持する把持部とが屈曲した場合でも、その屈曲を考慮して把持部の位置を演算することができる。

請求項２に記載の発明によれば、腕部の向きに応じて把持部の位置を補正することができる。

請求項３に記載の発明によれば、把持対象物が載せられた支持部材が傾斜している場合でも、その傾斜に応じて把持部の位置を演算することができる。

図１は、本発明の第１の実施形態に係る位置測定システムを示す全体図である。図２は、本発明の第１の実施形態に係る撮影装置から見た標識装置を示す平面図である。図３は、本発明の第１の実施形態に係る位置計測装置の概略の構成例を示す構成図である。図４は、本発明の第１の実施形態に係る標識装置の位置及び向きの演算方法を説明するために示す説明図である。図５は、本発明の第１の実施形態に係る第２の演算手段が把持部９６ａの位置の座標を補正する際の補正量の一例を示すグラフである。図６は、本発明の第１の実施形態に係る位置計測システムの動作手順を示すフローチャートである。図７は、本発明の第２の実施形態に係る位置測定システムを示す全体図である。図８は、本発明の第３の実施形態に係る撮影装置から見た標識装置を示す平面図である。

［第１の実施の形態］
図１は、本発明の第１の実施形態における位置測定システムを示す図である。

この位置計測システム１００は、作業者９０の前腕部９５に装着された標識装置１と、標識装置１を作業者９０の上方から撮影する撮影装置２と、撮影装置２で撮影した画像に基づいて演算を行う位置計測装置３とを備えている。なお、以下では人間である作業者９０の動作を位置計測システム１００で観測する場合について説明するが、人間の形態を模したロボットを観測対象とすることも可能である。

作業者９０は、床面８に立った状態で部品棚４に置かれた複数種類（本実施の形態では３種類）の第１〜第３の組付部品４１，４２，４３の何れかを把持して部品棚４から取り出し、図略の被組立物に組付ける。

部品棚４には、第１〜第３の組付部品４１，４２，４３をそれぞれ支持する支持部材としての第１〜第３の棚板４ａ，４ｂ，４ｃが設けられている。第１〜第３の棚板４ａ，４ｂ，４ｃは、水平方向に沿って設けられ、床面８からの高さがそれぞれ異なっている。

第１の棚板４ａに支持された第１の組付部品４１は、作業者９０の頭部９１に対応する高さに位置している。第２の棚板４ｂに支持された第２の組付部品４２は、作業者９０の胸部９２に対応する高さに位置している。第３の棚板４ｃに支持された第３の組付部品４３は、作業者９０の腹部９３に対応する高さに位置している。なお、第１〜第３の棚板４ａ，４ｂ，４ｃには、作業者９０が部品を取り出した後に同種の部品が供給されるものとする。

作業者９０の上腕部９４は、第１の連結部としての肩関節９ａを介して胸部９２に連結され、胸部９２に対して屈曲可能である。また、作業者９０の前腕部９５は、第２の連結部としての肘関節９ｂを介して上腕部９４に連結され、上腕部９４に対して屈曲可能である。また、作業者の手部９６は、第３の連結部としての手関節９ｃを介して前腕部９５に連結され、前腕部９５に対して屈曲可能である。

手部９６は、第１〜第３の組付部品４１，４２，４３を把持することが可能な把持部の一例である。また、前腕部９５は、手部９６に対して屈曲可能に連結された腕部の一例である。

撮影装置２は、例えばビデオカメラにより実現され、作業者９０の上方にて標識装置１を撮影可能な位置に固定されている。撮影装置２で撮影した画像の情報は、位置計測装置３に送られる。

また、以下の説明では、床面８に垂直な鉛直方向をＺ軸とし、部品棚４の奥行き方向をＹ軸とする。また、Ｙ軸及びＺ軸に垂直な方向（部品棚４の幅方向）をＸ軸とする。

図２は、撮影装置２から見た標識装置１を示す図である。標識装置１は、作業者９０が第１〜第３の組付部品４１，４２，４３を把持する際に標識装置１の影像が第１〜第３の棚板４ａ，４ｂ，４ｃによって遮られないよう、前腕部９５の長手方向の中心位置よりも上腕部９４側に装着されている。標識装置１は、前腕部９５にベルト１０によって固定されているが、これに限らず、例えば作業者９０の衣服に接着等により一体に固定してもよい。

標識装置１には、位置標識としての第１〜第４のＬＥＤ（Light Emitting Diode）１１〜１４が配置されている。第１〜第４のＬＥＤ１１〜１４は、例えば撮影装置２に内蔵された図略の電池から電源が供給され、位置計測システム１００の使用時において発光する光源として構成されている。

第１〜第４のＬＥＤ１１〜１４は、同一平面上に配置され、例えばそれぞれのＬＥＤが正方形の頂点をなすように等間隔で配置されている。標識装置１は、第１〜第４のＬＥＤ１１〜１４が含まれる平面が前腕部９５の仮想的な中心軸に対して平行になるように前腕部９５に装着される。

図２の手部９６に破線で示す領域は、第１〜第３の組付部品４１，４２，４３を把持する際に第１〜第３の組付部品４１，４２，４３の上面に接触する領域である。以下、この領域を把持部９６ａという。

第１〜第４のＬＥＤ１１〜１４の中心位置から前腕部９５の長手方向に沿った手関節９ｃまでの距離Ｌ_１、及び手関節９ｃから把持部９６ａの中心までの距離Ｌ_２は、予め測定されているものとする。

（位置計測装置３の構成）
図３は、位置計測装置３の概略の構成例を示す構成図である。位置計測装置３は、ＣＰＵ（Central Processing Unit）等の電子部品によって構成された制御部３１と、磁気ディスクやＲＯＭ（Read Only Memory），ＲＡＭ（Random Access Memory）等によって構成された記憶部３２と、撮影装置２との通信を行う通信部３３とを有して構成されている。

制御部３１は、記憶部３２に記憶された位置計測プログラム３２０に従ってＣＰＵが動作することにより、画像取得手段３１１，第１の演算手段３１２，第２の演算手段３１３，判定手段３１４，及び照合手段３１５等として機能する。

記憶部３２は、位置計測プログラム３２０，撮影装置２によって撮影した画像の情報である画像情報３２１，標識装置１における第１〜第４のＬＥＤ１１〜１４の相対的な位置関係を示す標識装置情報３２２，作業者９０の体格に関する情報である作業者情報３２３，把持部９６ａの位置の補正のための補正量情報３２４，第１〜第３の組付部品４１〜４３の位置を示す組付部品位置情報３２５，被組立物へ組み付けるべき組付部品の種別を示す組付部品情報３２６等を記憶する。

標識装置情報３２２は、第１〜第４のＬＥＤ１１〜１４が成す正方形の各辺の長さに相当する第１〜第４のＬＥＤ１１〜１４の間の相互の距離の情報を含む。また、作業者情報３２３は、上記の距離Ｌ_１及び距離Ｌ_２の情報を含む。なお、距離Ｌ_１及び距離Ｌ_２は、平均的な作業者の体格を考慮して予め設定された値として作業者情報３２３として記憶されていてもよい。

画像取得手段３１１は、通信部３３を介して撮影装置２が撮影した画像の情報を予め定められた時間（例えば０．１秒）ごとに取得し、記憶部３２に記憶する。

第１の演算手段３１２は、記憶部３２に画像情報３２１として記憶された画像に第１〜第４のＬＥＤ１１〜１４の全てが含まれている場合、その画像における第１〜第４のＬＥＤ１１〜１４の位置関係に基づいて、標識装置１の三次元の位置及び向きを演算する。

標識装置１の三次元の位置及び向きは、画像情報３２１の画像における第１〜第４のＬＥＤ１１〜１４の位置関係に基づいて幾何学的な演算を行うことにより可能である。以下にその演算方法の一例を説明する。

図４は、標識装置１の三次元の位置及び向きの演算方法を説明するために示す説明図である。

まず、撮影装置２の画素面２１における標識装置１の４つのＬＥＤのうちの３つ（例えば第１〜第３のＬＥＤ１１〜１３）の画像位置ｃ１，ｃ２，ｃ３と撮影装置２の光学中心２０との位置関係から、光学中心２０を基準とした座標系における第１〜第３のＬＥＤ１１〜１３の位置の方向ベクトルｄ１，ｄ２，ｄ３を算出する。方向ベクトルｄ１，ｄ２，ｄ３は、規格化された単位ベクトルであり、Ｘ軸方向のベクトルを示すｘ１，ｘ２，ｘ３、Ｙ軸方向のベクトルを示すｙ１，ｙ２，ｙ３、及びＺ軸方向のベクトルを示すｚ１，ｚ２，ｚ３で表される。

第１〜第３のＬＥＤ１１〜１３の三次元空間の位置ベクトルをｐ_１，ｐ_２，ｐ_３とすると、これらは方向ベクトルｄ_１，ｄ_２，ｄ_３の延長線上に存在するので、光学中心２０からの距離をそれぞれｔ_１，ｔ_２，ｔ_３とすると、位置ベクトルｐ_１，ｐ_２，ｐ_３は次式で表される。
(数１)
ｐ_１＝ｔ_１・ｄ_１
ｐ_２＝ｔ_２・ｄ_２
ｐ_３＝ｔ_３・ｄ_３

標識装置１における実際の第１〜第３のＬＥＤ１１〜１３の間の距離は記憶部３２に標識装置情報３２２として記憶されており、第１のＬＥＤ１１と第２のＬＥＤ１２の距離、第２のＬＥＤ１２と第３のＬＥＤ１３の距離、第３のＬＥＤ１３と第１のＬＥＤ１１の距離をそれぞれｉ_１，ｉ_２，ｉ_３とすると、次式が得られる。
(数２)
(ｔ_１・ｘ_１-ｔ_２・ｘ_２)^２+(ｔ_１・ｙ_１-ｔ_２・ｙ_２)^２+(ｔ_１・ｚ_１-ｔ_２・ｚ_２)^２=ｉ_１ ^２
(ｔ_２・ｘ_２-ｔ_３・ｘ_３)^２+(ｔ_２・ｙ_２-ｔ_３・ｙ_３)^２+(ｔ_２・ｚ_２-ｔ_３・ｚ_３)^２=ｉ_２ ^２
(ｔ_３・ｘ_３-ｔ_１・ｘ_１)^２+(ｔ_３・ｙ_３-ｔ_１・ｙ_１)^２+(ｔ_３・ｚ_３-ｔ_１・ｚ_１)^２=ｉ_３ ^２

この式を変形して次式が得られる。
(数３)
ｔ_１ ^２-２・ｔ_１・ｔ_２・（ｘ_１・ｘ_２+ｙ_１・ｙ_２+ｚ_１・ｚ_２)+ｔ_２ ^２-ｉ_１ ^２=０
ｔ_２ ^２-２・ｔ_２・ｔ_３・（ｘ_２・ｘ_３+ｙ_２・ｙ_３+ｚ_２・ｚ_３)+ｔ_３ ^２-ｉ_２ ^２=０
ｔ_３ ^２-２・ｔ_３・ｔ_１・（ｘ_３・ｘ_１+ｙ_３・ｙ_１+ｚ_３・ｚ_１)+ｔ_１ ^２-ｉ_３ ^２=０

この式に基づいて次式が得られる。
(数４)
ｔ_１=Ａ_１・ｔ_２±√((Ａ_１ ^２-１)・ｔ_２ ^２+ｉ_１ ^２)
ｔ_２=Ａ_２・ｔ_３±√((Ａ_２ ^２-１)・ｔ_３ ^２+ｉ_２ ^２)
ｔ_３=Ａ_３・ｔ_１±√((Ａ_３ ^２-１)・ｔ_１ ^２+ｉ_３ ^２)

ただし、この数４の数式におけるＡ_１，Ａ_２，Ａ_３は次式のとおりである。
(数５)
Ａ_１=ｘ_１・ｘ_２+ｙ_１・ｙ_２+ｚ_１・ｚ_２
Ａ_２=ｘ_２・ｘ_３+ｙ_２・ｙ_３+ｚ_２・ｚ_３
Ａ_３=ｘ_３・ｘ_１+ｙ_３・ｙ_１+ｚ_３・ｚ_１

ここで、ｔ_１，ｔ_２，ｔ_３は実数であるため、数４のルート演算子の中が正である条件として次式が得られる。
(数６)
ｔ_１≦√(ｉ_３ ^２／(１−Ａ_３ ^２)
ｔ_２≦√(ｉ_１ ^２／(１−Ａ_１ ^２)
ｔ_３≦√(ｉ_２ ^２／(１−Ａ_２ ^２)

次に、上記の数６の数式を満たすことを条件として、上記数４の全ての数式を満たすｔ_１，ｔ_２，ｔ_３を算出する。こうして求めたｔ_１，ｔ_２，ｔ_３を数１の数式に適用して第１〜第３のＬＥＤ１１〜１３の三次元の位置を示す座標が得られる。

上記の演算手順では、第１〜第３のＬＥＤ１１〜１３の座標として、２つの解が得られる。そこで、第１〜第４のＬＥＤ１１〜１４のうち、上記の第１回目の演算で用いたＬＥＤとは異なる組み合わせのＬＥＤ（例えば、第２〜第４のＬＥＤ１２〜１４）を用いて上記と同様の演算手順により第２回目の演算を行い、第１回目の演算で得られた座標と一致する座標の組み合わせを正しい解とする。これにより、第１〜第４のＬＥＤ１１〜１４の座標を算出できる。

なお、標識装置１のＬＥＤの数が３つである場合には、第１の演算手段３１２は、近似解として、第１回目の演算で得られた２つの解の座標を平均して各ＬＥＤの三次元の座標とすることができる。

第１の演算手段３１２は、上記の演算方法によって算出した第１〜第４のＬＥＤ１１〜１４の座標の中心点の座標を演算し、その中心点から第１〜第４のＬＥＤ１１〜１４を含む平面の法線方向に前腕部９５の半径に相当する距離だけ移動した位置の座標を演算する。

この移動量は、記憶部３２に作業者情報３２３として記憶されており、例えば予め測定した作業者９０の前腕部９５の標識装置１の装着部位の太さに基づいて算出された値でもよく、平均的な作業者の前腕部の太さを考慮して予め設定された固定値を用いてもよい。こうして演算した座標は、標識装置１が装着された部位における前腕部９５の位置を表す。

また、第１の演算手段３１２は、この第１〜第４のＬＥＤ１１〜１４の座標に基づいて、前腕部９５の向きを演算する。前腕部９５の向きは、例えば第１のＬＥＤ１１と第２のＬＥＤ１２の中間位置から第３のＬＥＤ１１と第４のＬＥＤ１４の中間位置に向かう単位ベクトルで表される。

第２の演算手段３１３は、第１の演算手段３１２が演算した前腕部９５の位置及び向きに基づき、前腕部９５の向きに応じた前腕部９５と手部９６との屈曲を考慮して、手部９６の把持部９６ａの位置を演算する。以下にこの演算の手順の一例を説明する。

第２の演算手段３１３は、第１の演算手段３１２が演算した前腕部９５の標識装置１が装着された部位の位置から前腕部９５の向きに沿って、距離Ｌ_１（図２参照）だけ移動した位置の座標を演算する。この座標は、手関節９ｃの座標を表す。

また、第２の演算手段３１３は、手関節９ｃの位置から第１の演算手段３１２が演算した前腕部９５の向きに沿って、さらに図２に示す距離Ｌ_２（図２参照）だけ移動した位置の座標を演算する。この座標は、前腕部９５と手部９６との屈曲がないとした場合の把持部９６ａの中心位置の仮の座標を示す。

次に、第２の演算手段３１３は、前腕部９５の向きに応じて把持部９６ａの中心位置の座標を補正する。この補正は、前腕部９５の向きに応じた補正量を把持部９６ａの中心位置の座標に加算又は減算することにより行う。

図５は、第２の演算手段３１３が把持部９６ａの中心位置の座標を補正する際の補正量の一例を示すグラフである。このグラフは、前腕部９５の向きと補正量との関係を示している。このグラフに示される情報は、補正量情報３２４の一例である。

この図に示すように、前腕部９５の向きが水平方向である場合には、把持部９６ａの中心位置の補正量はゼロであり、前腕部９５の向きが上向きになるにつれてマイナスの補正量が大きくなる。また、前腕部９５の向きが水平方向から下向きになるにつれてプラスの補正量が大きくなる。またさらに、前腕部９５の向きが水平方向に近い領域では前腕部９５の向きに対する補正値の変化量が大きく、前腕部９５の向きが上向き又は下向きの角度が大きくなるにつれて前腕部９５の向きに対する補正値の変化量が小さくなる。

この補正量は、作業者９０が部品棚４から第１〜第３の組付部品４１〜４３を把持して取り出す際の前腕部９５と手部９６との屈曲度合いを示す屈曲角を考慮して設定されている。この屈曲角について、図１を参照して説明する。

図１に実線で示すように、作業者９０が胸部９２に対応する高さの第２の組付部品４２を把持する際、前腕部９５の向きは略水平方向であり、前腕部９５と手部９６とは殆ど屈曲せず、この状態における前腕部９５の仮想的な中心軸９５２と手部９６の仮想的な中心軸９６２とは略平行な状態である。この状態では、ＹＺ平面における中心軸９５２と中心軸９６２とがなす下側の角度である屈曲角θ_２が略１８０°である。

また、図１に破線で示すように、作業者９０が頭部９１に対応する高さの第１の組付部品４１を把持する際、前腕部９５の向きは水平方向よりも上向きとなり、手部９６は前腕部９５に対して屈曲し、この状態における前腕部９５の仮想的な中心軸９５１と手部９６の仮想的な中心軸９６１との屈曲角θ_１は例えば１５０°となる。

また、図１に破線で示すように、作業者９０が腹部９３に対応する高さの第３の組付部品４３を把持する際、前腕部９５の向きは水平方向よりも上向きとなり、手部９６は前腕部９５に対して逆方向に屈曲し、この状態における前腕部９５の仮想的な中心軸９５３と手部９６の仮想的な中心軸９６３との屈曲角θ_３は例えば２１０°となる。

このように、前腕部９５の向きに応じて前腕部９５と手部９６との屈曲角が変化する。前腕部９５の向きが水平方向よりも上向きである場合には、前腕部９５と手部９６とが屈曲しないとした場合の把持部９６ａ位置よりも実際の把持部９６ａの位置は下方になる。

また、前腕部９５の向きが水平方向よりも下向きである場合には、前腕部９５と手部９６とが屈曲しないとした場合の把持部９６ａ位置よりも実際の把持部９６ａの位置は上方になる。図５に示す補正値は、この前腕部９５の向きと屈曲角による把持部９６ａの位置のずれ量との関係に基づいて設定されている。

第２の演算手段３１３は、前腕部９５と手部９６との屈曲がないとした場合の把持部９６ａの中心位置の仮の座標のＺ軸の座標値に、図５のグラフに示されるような補正量を加算して補正を行い、把持部９６ａの位置を示す座標を演算する。つまり、第２の演算手段３１３は、前腕部９５の向きが水平方向に対応した設定角度よりも上向きであるときに把持部９６ａの位置が低くなるように補正を行い、前腕部９５の向きが水平方向に対応した設定角度よりも下向きであるときに把持部９６ａの位置が高くなるように補正を行う。

なお、前腕部９５の向きと補正量との関係は、例えば対応表の形式で補正量情報３２４として記憶されていてもよく、前腕部９５の向きと補正量との相関関係を示す関係式が補正量情報３２４として記憶されていてもよい。

判別手段３１４は、第２の演算手段３１３が演算した把持部９６ａの位置を示す座標に基づいて、作業者９０が第１〜第３の組付部品４１〜４３のうち、何れの組付部品を把持したかを判別する。判別手段３１４は、例えば記憶部３２の組付部品位置情報３２５に示される第１〜第３の組付部品４１〜４３の配置位置と、把持部９６ａの座標との相対的な距離を演算し、その距離が最も短い組付部品を作業者９０が把持したと判別する。

照合手段３１５は、判別手段３１４が判定した組付部品と、被組立物に組み付けるべき組付部品とを照合し、作業者９０が適切な組付部品を把持したか否かを判定する。被組立物に組み付けるべき組付部品の情報は、記憶部３２の組付部品情報３２６に記憶されている。また、照合手段３１５は、作業者９０が不適切な組付部品を把持したと判定した場合には、例えばブザーや表示によって警報を発し、作業者９０に報知する。

（位置計測システム１００の動作）
次に、図１から図６を参照して位置計測システム１００の動作を説明する。
図６は、位置計測システム１００の動作手順を示すフローチャートである。

画像取得手段３１１は、通信部３３を介して撮影装置２が撮影した画像の情報を取得し、記憶部３２に画像情報３２１として記憶する（Ｓ１０）。

次に、第１の演算手段３１２は、ステップＳ１０で記憶された画像情報３２１の画像に第１〜第４のＬＥＤ１１〜１４の全てが含まれているかを判定する（Ｓ１１）。

画像情報３２１の画像に第１〜第４のＬＥＤ１１〜１４の全てが含まれている場合（Ｓ１１；Ｙｅｓ）、第１の演算手段３１２は、画像情報３２１の画像における第１〜第４のＬＥＤ１１〜１４の位置に基づいて、識別装置１の位置及び向きを演算する（Ｓ１２）。

次に、第１の演算手段３１２は、ステップＳ１２で演算した識別装置１の位置及び向き、並びに作業者情報３２３を参照して得た作業者９０の前腕部９５の太さに応じた移動量に基づいて、前腕部９５の位置を演算する（Ｓ１３）。

次に、第２の演算手段３１３は、ステップＳ１３で演算した前腕部９５の位置、ステップＳ１２で演算した識別装置１の向き、及び作業者情報３２３を参照して得た距離Ｌ_１及び距離Ｌ_２（図２参照）に基づいて、前腕部９５と手部９６との屈曲がないとした場合の把持部９６ａの位置を演算する（Ｓ１４）。

次に、第２の演算手段３１３は、記憶部３２の補正量情報３２４を参照し、ステップＳ１４で演算した把持部９６ａの位置を補正する（Ｓ１５）。

次に、判別手段３１４は、ステップＳ１５で補正演算した把持部９６ａの位置を示す座標に基づいて、作業者９０が第１〜第３の組付部品４１〜４３のうち、何れの組付部品を把持したかを判別する（Ｓ１６）。

次に、照合手段３１５は、判別手段３１４が判定した組付部品と、被組立物に組み付けるべき組付部品とを照合し、作業者９０が適切な組付部品を把持したか否かを判定する（Ｓ１７）。

照合手段３１５は、ステップ１７で適切な組付部品を把持していないと判定した場合には（Ｓ１７；Ｎｏ）、作業者９０にブザーや表示等により把持した組付部品が適切でないことを報知する（Ｓ１８）。

作業者９０は、この報知によって組付部品を確認し、適切な組付部品を再度把持して部品棚４から取り出し、被組立物への組み付けを行う。

［第２の実施の形態］
次に、本発明の第２の実施の形態について説明する。本実施の形態では、作業者９０を側面から見たＹＺ平面における前腕部９５の向きに応じて把持部９６ａの位置の補正を行う。

図１に示したように、作業者９０が第１〜第３の組付部品４１〜４３の何れかを把持する際、前腕部９５の向きは第１〜第３の組付部品４１〜４３の高さ方向の位置に応じて向きが変わるが、手部９６の向きは略水平方向に沿った向きである。従って、前腕部９５の仮想的な中心軸と手部９６の仮想的な中心軸とがなす角度は、ＹＺ平面における前腕部９５の仮想的な中心軸とＹ軸とのなす角度で近似して求めることができる。

従って、本実施の形態における第２の演算手段３１３は、第１の演算手段３１２が演算した前腕部９５の向きに基づいてＹＺ平面における前腕部９５の仮想的な中心軸とＹ軸とのなす角度を演算し、その角度を用いて幾何学的な演算を行うことにより把持部９６ａの位置の補正を行う。

より詳細には、本実施の形態における第２の演算手段３１３は、ＹＺ平面における前腕部９５の仮想的な中心軸とＹ軸とのなす角度をθとすると、手関節９ｃから把持部９６ａの中心までの距離Ｌ_２にｓｉｎθを乗算した積に相当する距離を前腕部９５と手部９６との屈曲がないとした場合の把持部９６ａの中心位置の仮の座標に加算又は減算することで補正を行う。

なお、第１の演算手段３１２が演算した手関節９ｃの座標に、距離Ｌ_２とｃｏｓθを乗算した積に相当する距離をＹ軸方向に延伸することによっても、同様の値を得ることができる。

また、部品棚の棚板が水平方向に対して傾斜している場合には、その傾斜角に応じて補正量を変更することができる。この場合の演算方法について、以下に説明する。

図７は、第１〜第３の棚板４ａ，４ｂ，４ｃが傾斜した部品棚４Ａから第１〜第４の組付部品４１〜４３を取り出す場合の位置計測システム１００を示す構成図である。この図に示すように、部品棚４Ａの第１〜第３の棚板４ａ，４ｂ，４ｃとＺ軸とのなす角度αが９０°よりも小さくなっている。

このように棚板が傾斜している場合には、第２の演算手段３１３は、ＹＺ平面における前腕部９５の仮想的な中心軸とＹ軸とのなす角度θから、第１〜第３の棚板４ａ，４ｂ，４ｃの水平方向に対する傾斜角（９０°−α）を減算する。そして、減算の結果得られた角度（以下、この角度を「φ」とする）を用いて、距離Ｌ_２にｓｉｎφを乗算した積に相当する距離を前腕部９５と手部９６との屈曲がないとした場合の把持部９６ａの中心位置の仮の座標に加算又は減算することで補正を行う。

なお、上記のように、前腕部９５の仮想的な中心軸と手部９６の仮想的な中心軸とがなす角度をＹＺ平面における前腕部９５の仮想的な中心軸とＹ軸とのなす角度で近似する演算方法に限らず、第１の演算手段３１２が演算した前腕部９５の向きと、前腕部９５の仮想的な中心軸と手部９６の仮想的な中心軸とがなす角の対応関係を示す対応表又は演算式に基づいて把持部９６ａの位置の補正を行ってもよい。

［第３の実施の形態］
次に、本発明の第３の実施の形態について説明する。上記の実施の形態では、把持部９６ａの位置の鉛直方向（Ｚ軸方向）の補正を行ったが、本実施の形態では、把持部９６ａのＸ軸方向の位置の補正も行う。

図８は、手部９６の把持部９６ａが矢印Ａの方向（Ｘ軸に沿った方向）に動いた状態を示す図である。この図に示すように、水平方向に配置された複数の組付部品の何れか１つを把持する際、前腕部９５の向きは把持部９６ａの矢印Ａ方向の位置に応じて大きく変化するが、手部９６の向きの変化量は前腕部９５の向きの変化量よりも小さい。特に、複数の組付部品の間に間仕切りがある場合にはこの傾向が顕著となる。従って、前腕部９５と手部９６とが屈曲する。

本実施の形態に係る第２の演算手段３１３は、第１の演算手段３１２で演算した前腕部９５のＸＹ平面上における向きに応じて前腕部９５の仮想的な中心軸と手部９６の仮想的な中心軸とのなす角度を演算により、又は対応表を参照して求め、その角度に応じて第１又は第２の実施形態と同様の演算方法によって把持部９６ａの位置を補正する。

そして、補正した把持部９６ａの位置に応じて判別手段３１４が複数の種類の組付部品のうち何れの組付部品が把持されたかを判別し、照合手段３１５がその組付部品が適切か否かの判定を行い、不適切であれば作業者９０に報知する。

１…標識装置、２…撮影装置、３…位置計測装置、４，４Ａ…部品棚、４ａ，４ｂ，４ｃ…第１〜第３の棚板、８…床面、９ａ…肩関節、９ｂ…肘関節、９ｃ…手関節、１０…ベルト、１１〜１４…第１〜第４のＬＥＤ、２０…光学中心、２１…画素面、３１…制御部、３２…記憶部、３３…通信部、４１〜４３…第１〜第３の組付部品、９０…作業者、９１…頭部、９２…胸部、９３…腹部、９４…上腕部、９５…前腕部、９６…手部、９６ａ…把持部、１００…位置計測システム、３１１…画像記憶手段、３１２…第１の演算手段、３１３…第２の演算手段、３１４…判別手段、３１５…照合手段、３２０…位置計測プログラム、３２１…画像情報、３２２…標識装置情報、３２３…作業者情報、３２４…補正量情報、３２５…組付部品位置情報、３２６…組付部品情報、９５１〜９５３，９６１〜９６３…中心軸ｃ１，ｃ２，ｃ３…画像位置、ｄ１，ｄ２，ｄ３…方向ベクトル、α…角度、θ_１，θ_２，θ_３…屈曲角

Claims

把持対象物を把持する把持部を有する手部が屈曲可能に連結された腕部に装着され、少なくとも３つの位置標識を有する標識装置と、
前記標識装置を撮影する撮影装置と、
前記撮影装置により撮影した画像における前記少なくとも３つの位置標識の位置関係に基づいて前記腕部の位置及び向きを演算する第１の演算手段と、
前記第１の演算手段が演算した前記腕部の位置及び向きに基づき、前記腕部の向きに応じた前記腕部と前記手部との屈曲を考慮して、前記把持部の位置を演算する第２の演算手段とを備えた位置計測システム。
前記第２の演算手段は、前記第１の演算手段で演算した前記腕部の向きが設定角度よりも上向きであるとき、前記腕部と前記手部との屈曲がないとした場合の前記把持部の位置よりも前記把持部の位置が低くなるように補正を行い、前記第１の演算手段で演算した前記腕部の向きが設定角度よりも下向きであるとき、前記腕部と前記手部との屈曲がないとした場合の前記把持部の位置よりも前記把持部の位置が高くなるように補正を行う請求項１に記載の位置計測システム。
前記第２の演算手段は、前記把持対象物が載せられた支持部材の傾斜を考慮に加えて前記把持部の位置を演算する請求項１又は２に記載の位置計測システム。
把持対象物を把持する把持部を有する手部が屈曲可能に連結された腕部に装着される少なくとも３つの位置標識を有する標識装置を撮影した画像を取得する画像取得手段と、
前記画像取得手段により取得した画像における前記少なくとも３つの位置標識の位置関係に基づいて前記腕部の位置及び向きを演算する第１の演算手段と、
前記第１の演算手段が演算した前記腕部の位置及び向きに基づき、前記腕部の向きに応じた前記腕部と前記手部との屈曲を考慮して、前記把持部の位置を演算する第２の演算手段とを備えた位置計測装置。
把持対象物を把持する把持部を有する手部が屈曲可能に連結された腕部に装着される少なくとも３つの位置標識を有する標識装置を撮影した画像を取得する画像取得ステップと、
前記画像取得手段により取得した画像における前記少なくとも３つの位置標識の位置関係に基づいて前記腕部の位置及び向きを演算する第１の演算ステップと、
前記第１の演算手段が演算した前記腕部の位置及び向きに基づき、前記腕部の向きに応じた前記腕部と前記手部との屈曲を考慮して、前記把持部の位置を演算する第２の演算ステップとを備えた位置計測プログラム。