JP6446009B2 - 位置姿勢推定方法 - Google Patents

Description

本発明は、ワークなどの対象物の３次元空間における位置および姿勢を測定する方法に関する。

カメラに測距センサを組み合わせて物体の位置を測定する方法が提案されている。カメラの光軸に対して垂直な基準面において、当該カメラによる物体上における既知の３点のそれぞれの位置の検出結果が、測距センサによる当該３点のそれぞれの測距結果に基づいて視差補正され、当該３点の３次元位置、ひいては当該物体の３次元位置を測定する方法が提案されている（特許文献１参照）。平面上の３個のマークのそれぞれのカメラによるＸ−Ｙ位置の測定結果および測距センサによるＺ位置の測定結果に基づき、当該平面の位置および姿勢を推定する方法が提案されている（特許文献２参照）。

しかし、測距センサの採用により測定系のコストが高くなるため、これを回避するために測距センサを用いずに撮像装置を用いて物体の３次元位置を測定する方法も提案されている。幾何学的関係が既知の４以上の複数のマークが存在する平面がカメラで撮像され、当該複数のマークの当該平面における位置および２次元画像座標系における位置に基づき、当該複数のマークの３次元位置を推定する方法が提案されている（特許文献３参照）。カメラにより撮像された２次元画像において対象物体の３つの特徴点が抽出され、当該抽出結果と、予め測定されている当該３つの特徴点間の距離とに基づき、当該対象物体の３次元位置を測定する方法が提案されている（特許文献４参照）。格子状に２次元配列された複数のマークがカメラにより撮像されて得られた複数のマークの２次元位置に基づき、所定の関係式にしたがって当該複数のマークの３次元位置を推定する方法が提案されている（特許文献５参照）。相互間距離が予め計測されている４つの目印が付されている対象物の、カメラにより撮像された画像における２次元位置に加えて、当該相互間距離に基づき、当該４つの目印の３次元位置を推定する方法が提案されている（特許文献６参照）。

特開平３−９２７１２号公報 特開平９−３０４０１３号公報 特開昭６２−１７５６０３号公報 特許第２５３４５１７号公報 特開平６−２５９５３６号公報 特許第４６０２７０４号公報

しかし、部品同士の組み付け状態を確認するため、撮像装置を用いて一の部品が撮像された際にそこに付されたマークが他の部品または部品群によって少なくとも部分的に隠されてしまい、当該確認作業が困難になる場合がある。

そこで、本発明は、対象物の一部が他の物体により隠されてしまう状況であっても当該対象物の３次元座標系における位置および姿勢を簡易かつ高精度で推定することができる方法を提供することを目的とする。

本発明の位置姿勢推定方法は、ＸＹＺ座標系により定義される実空間における位置および姿勢が固定され、Ｚ軸に対して平行な光軸を有する撮像装置を通じて対象物の撮像画像を取得し、対象物において定義されている、前記対象物の外観または構造に由来する少なくとも１つの指標領域を含む複数の指標領域のそれぞれの前記撮像画像におけるサイズを測定する第１サイズ測定処理と、前記第１サイズ測定処理における測定結果に基づき、前記複数の指標領域のそれぞれに含まれている複数の指標点のそれぞれのＺ座標値を推定する縦実空間位置推定処理と、前記複数の指標点のそれぞれのＸ座標値およびＹ座標値のうち少なくとも一方と、前記複数の指標点のそれぞれのＺ座標値の推定結果とに基づき、前記複数の指標点のうち対をなす指標点を通る指標直線の実空間における姿勢を第１実空間姿勢として推定する第１実空間姿勢推定処理と、前記第１実空間姿勢の推定結果に基づいて前記対象物の実空間姿勢を推定する実空間姿勢推定処理と、前記撮像画像における前記指標直線の延在方向について、前記少なくとも１つの指標領域のサイズを測定する第２サイズ測定処理と、前記第２サイズ測定処理における測定結果に基づき、前記少なくとも１つの指標領域の実空間姿勢を第２実空間姿勢として推定する第２実空間姿勢推定処理と、を含み、
前記第１サイズ測定処理において、前記実空間における位置および姿勢が固定されている照射装置を用いて、前記複数の指標点のうち第１指標点に対して光線を照射することで、前記複数の指標領域のうち少なくとも１つの指標領域として第１指標領域を形成し、
前記実空間姿勢推定処理において、前記第１実空間姿勢の推定結果および前記第２実空間姿勢の推定結果を総合することにより、前記対象物の実空間姿勢を推定することを特徴とする。

本発明の位置姿勢推定方法によれば、実空間のＺ軸に平行な光軸を有する撮像装置を通じて取得された対象物の各指標領域のサイズに応じて、各指標点のＺ座標値が推定される。これは、指標点が撮像装置（またはその撮像素子）に近くなるほどその測定サイズが大きくなり、指標点が撮像装置から遠くなるほどその測定サイズが小さくなるという定性的関係に依拠している。各指標点のＺ座標値に基づき、対をなす指標点を通る指標直線の第１実空間姿勢（方向ベクトルに応じて定まる。）が推定される。照射装置を用いて複数の指標点のうち第１指標点に対して光線が照射されることで、当該第１指標点を含む第１指標領域が形成かつ定義される。

撮像装置の撮像範囲において、対象物の一部が他の物体により隠されている状況であっても、第１指標点に対して当該他の物体を回避するようにまたは間隙を縫うように当該対象物の第１指標点に照射されることで、第１指標領域が形成される。同一の指標直線上にある複数の指標点のそれぞれを含む複数の指標領域を撮像画像に容易に含ませることができる。よって、このような状況であっても、対象物の少なくともＺ座標値により定義される実空間位置および少なくとも指標直線の第１実空間姿勢により定義される対象物の実空間姿勢を簡易かつ高精度で推定することができる。

当該位置姿勢推定方法によれば、撮像画像における少なくとも１つの指標領域の、指標直線の延在方向についてのサイズに応じて、当該指標領域の第２実空間姿勢（指標領域の垂線ベクトルに応じて定まる。）が推定される。第１実空間姿勢の推定結果および第２実空間姿勢の推定結果が統合されることで、対象物の実空間姿勢の推定精度の向上が図られる。

本発明の位置姿勢推定方法は、ＸＹＺ座標系により定義される実空間における位置および姿勢が固定され、Ｚ軸に対して平行な光軸を有する撮像装置を通じて対象物の撮像画像を取得し、対象物において定義されている複数の指標領域のそれぞれの前記撮像画像におけるサイズを測定する第１サイズ測定処理と、前記第１サイズ測定処理における測定結果に基づき、前記複数の指標領域のそれぞれに含まれている複数の指標点のそれぞれのＺ座標値を推定する縦実空間位置推定処理と、前記複数の指標点のそれぞれのＸ座標値およびＹ座標値のうち少なくとも一方と、前記複数の指標点のそれぞれのＺ座標値の推定結果とに基づき、前記複数の指標点のうち対をなす指標点を通る指標直線の実空間における姿勢を第１実空間姿勢として推定する第１実空間姿勢推定処理と、前記第１実空間姿勢の推定結果に基づいて前記対象物の実空間姿勢を推定する実空間姿勢推定処理と、を含み、前記第１サイズ測定処理において、前記実空間における位置および姿勢が固定されている照射装置を用いて、前記複数の指標点のうち第１指標点に対して光線を照射することで、前記複数の指標領域のうち少なくとも１つの指標領域として第１指標領域を形成し、前記実空間姿勢推定処理において、異なる対のそれぞれをなす指標点を通り、かつ、同一のまたは相互に平行な複数の前記指標直線のそれぞれの前記第１実空間姿勢の推定結果を総合することにより、前記対象物の実空間姿勢を推定することを特徴とする。

当該位置姿勢推定方法によれば、異なる対をなす指標点を通る複数の指標直線のそれぞれの実空間姿勢、ひいては対象物の実空間姿勢の推定精度の向上が図られる。

本発明の位置姿勢推定方法において、前記複数の指標点の異なる対によって、前記実空間において直交する一対の指標直線が定義されるように、前記複数の指標点が設定され、前記第１実空間姿勢推定処理において、前記一対の指標直線のそれぞれの実空間における姿勢を前記第１実空間姿勢として推定することが好ましい。

当該位置姿勢推定方法によれば、実空間において直交する２つの指標直線のそれぞれの、第１実空間姿勢（または第１実空間姿勢の複数の推定結果の統合結果、または第１実空間姿勢および第２実空間姿勢の統合結果）により定義される対象物の実空間姿勢が推定される。

本発明の位置姿勢推定方法において、前記複数の指標点のうち対象物座標系に固定されている第２指標点の前記撮像画像における位置に基づき、前記第２指標点のＸ座標値およびＹ座標値を推定する横実空間位置推定処理をさらに含むことが好ましい。

当該位置姿勢測定方法によれば、対象物座標系において固定されている第２指標点の撮像画像における位置に応じて当該指標点のＸ座標値およびＹ座標値が推定される。これにより、第２指標点のＺ座標値の推定結果に加えて、Ｘ座標値およびＹ座標値の推定結果により定義される対象物の実空間位置が高精度で推定される。

本発明の位置姿勢推定方法において、前記対象物において前記撮像装置を通じて認識可能な輪郭により画定される第２指標領域に含まれる点が、前記第２指標点として定義されていることが好ましい。

当該位置姿勢推定方法によれば、撮像画像における第２指標点の位置の測定精度の向上が図られることにより、対象物の実空間位置および実空間姿勢の推定精度の向上が図られる。

本発明の位置姿勢推定方法において、前記複数の指標点のうち少なくとも１つの指標点のＺ座標値を変化させながら、前記対象物の撮像画像における前記少なくとも１つの指標領域のサイズの変化を測定することにより、前記少なくとも１つの指標点のＺ座標値と前記撮像画像における前記少なくとも１つの指標領域のサイズとの相関関係を第１相関関係として定義する第１準備処理をさらに含み、前記縦実空間位置推定処理において、前記第１相関関係にしたがって前記複数の指標点のそれぞれのＺ座標値を推定することが好ましい。

当該位置姿勢推定方法によれば、事前に定義された第１相関関係にしたがって各指標点のＺ座標値が推定されるので、対象物の実空間位置および実空間姿勢の推定精度の向上が図られる。

本発明の位置姿勢推定方法において、前記少なくとも１つの指標領域の実空間姿勢を変化させながら、前記撮像画像における前記少なくとも１つの指標領域の、前記指標直線の延在方向についてのサイズの変化を測定することにより、前記少なくとも１つの指標領域の実空間姿勢と前記撮像画像における前記指標直線の延在方向についてのサイズとの相関関係を第２相関関係として定義する第２準備処理をさらに含み、前記第２実空間姿勢推定処理において、前記第２相関関係にしたがって前記第２実空間姿勢を推定することが好ましい。

当該位置姿勢推定方法によれば、事前に定義された第２相関関係にしたがって第２実空間姿勢が推定されるので、対象物の実空間位置および実空間姿勢の推定精度の向上が図られる。

本発明の第１実施形態としての位置姿勢推定方法に関する説明図。 基準状態における対象物の撮像画像および光線の照射および撮像態様（第１実施形態）に関する説明図。 任意状態における対象物の撮像画像および光線の照射および撮像態様（第１実施形態）に関する説明図。 第１相関関係に関する説明図。 第１相関関係に関する説明図。 第１相関関係に関する説明図。 第２相関関係に関する説明図。 第２相関関係に関する説明図。 第２相関関係に関する説明図。 基準状態における対象物の撮像画像および光線の照射および撮像態様（第２実施形態）に関する説明図。 任意状態における対象物の撮像画像および光線の照射および撮像態様（第２実施形態）に関する説明図。

（第１実施形態）
本発明の第１実施形態としての位置姿勢推定方法においては、実空間座標系（Ｘ，Ｙ，Ｚ）における対象物座標系（ｘ，ｙ，ｚ）の位置および姿勢がワークＷ（対象物）の実空間位置および実空間姿勢として推定される（図２Ａ下側、図２Ｂ下側、図５Ａ下側および図５Ｂ下側参照）。当該推定に際して、照射装置２および撮像装置４が用いられ、ワークＷの実空間における位置および姿勢が推定される。照射装置２はその光線２０（レーザービーム）の光軸がＺ軸に平行になるように、実空間における位置および姿勢が基台１により保持されている。撮像装置４はその光軸がＺ軸に平行になるように、実空間における位置および姿勢が基台１により保持されている。

ワークＷは、例えば、実空間における位置および姿勢が一定に維持されるように配置されている他のワークに対して組み付けられる部品であり、ワークＷのそれぞれが当該他のワークのそれぞれに対して組み付けられた際に、その実空間位置および姿勢のそれぞれの、目標位置および目標姿勢のそれぞれに対する偏差を評価するために実行される。

照射装置２の実空間位置および実空間姿勢が一定に維持されることにより、複数の指標点Ｐ_i（ｉ＝０，１，２，‥Ｎ）のうち、第１指標点のＸ座標値Ｘ_iおよびＹ座標値Ｙ_iはあらかじめ定められている。第２指標点は、対象物座標系における位置が固定されている。第１実施形態では、対をなす指標点Ｐ_iおよびＰ_jを通る複数の指標直線は相互に非平行である。例えば、図２Ａ上側および図２Ｂ上側に示されているように、指標点Ｐ₀およびＰ₁を通る第１指標直線（ここではｘ軸に一致している。）および指標点Ｐ₀およびＰ₂を通る第２指標直線（ここではｙ軸に一致している。）は、指標点Ｐ₀において直交する。

ワークＷの複数の指標点Ｐ_iのうち第１指標点Ｐ_i１に対して、照射装置２を用いて光線２０が照射される（図１／ＳＴＥＰ０２）。これにより、例えば図２Ａ上側および図２Ｂ上側に示されている第１指標点Ｐ₁およびＰ₂（ｉ₁＝１，２）のそれぞれに対して同時にまたは異なる時刻に光線が照射される。その結果、略円形の照射領域がワークＷの表面に第１指標領域Ａ₁およびＡ₂として形成かつ定義される。光線２０の照射領域の形状は略円形状のほか、略楕円形上などの他の形状であってもよく、略円環状などの環状であってもよい。照射領域の形状が環状である場合、当該環を輪郭とする領域が第１指標領域Ａ_i1として定義されてもよい。

撮像装置４により取得された撮像画像において識別可能な、ワークＷの外観または構造由来の領域が第２指定領域Ａ_i2として定義され、第２指定領域Ａ_i2の中心点またはその境界線上の点など、そこに含まれている点が第２指定点Ｐ_i2として定義されている。第２指定点Ｐ_i2は、その性質上、対象物座標系においてその位置が固定されている。図２Ａおよび図２Ｂに示されている例では、ワークＷに形成されている開口部の周縁により定義される第２指標領域Ａ₀（ｉ₂＝０）中心点が第２指標点Ｐ₀として定義されている。第１実施形態では、対をなす第１指標点Ｐ_i１および第２指標点Ｐ_i2を通るように指標直線Ｑ_i１i2が定義されている。なお、複数の指標点Ｐ_iに複数の第２指標点Ｐ_i2が含まれていてもよい。複数の指標点Ｐ_iの全てが第１指標点Ｐ_i１として定義され、全ての指標点Ｐ_iに対して光線２０が照射されてもよい。

続いて、撮像装置４を通じてワークＷの撮像画像が取得される（図１／ＳＴＥＰ０４）。これにより、例えば、図２Ａ上側および図２Ｂ上側のそれぞれに示されているように、指標領域Ａ₀〜Ａ₂（Ａ₀’〜Ａ₂’）が含まれているワークＷの撮像画像が取得される。

撮像画像に基づき「第１サイズ測定処理」が実行され、画像座標系における指標領域Ａ_iのサイズφ_i（または径）が測定される（図１／ＳＴＥＰ１０）。なお、第１サイズ測定処理として、後述する第２サイズ測定処理（図１／ＳＴＥＰ２０参照）と同様に、撮像画像における指標直線の延在方向について指標領域Ａ_iのサイズφ_iが測定されてもよい。

さらに「縦実空間位置推定処理」が実行され、画像座標系（ｕ，ｖ）における指標領域Ａ_iのサイズφ_iの測定結果に基づき、指標点Ｐ_iのＺ座標値Ｚ_i（Ｚ座標値）が推定される（図１／ＳＴＥＰ１２）。例えば、図３Ａ〜図３Ｃに概念的に示されているように、撮像装置４（またはその撮像素子）と指標点Ｐ_iとの間隔Ｄ_iが小さいほど撮像画像における指標領域Ａ_iのサイズφ_iが大きくなる（撮像装置４と指標点Ｐ_iとの間隔Ｄが大きいほど撮像画像における指標領域Ａ_iのサイズφ_iが小さくなる）というように、当該間隔Ｄ_iおよび当該サイズφ_iの間には相関関係（第１相関関係）が存在する。よって、画像座標系（ｕ，ｖ）における指標領域Ａ_iのサイズφ_iに基づき、当該第１相関関係にしたがって、間隔Ｄ_iに相当する指標点Ｐ_iのＺ座標値Ｚ_iが推定されうる。

撮像画像に基づき「横実空間位置推定処理」が実行され、画像座標系（ｕ，ｖ）における第２指標点Ｐ_i2の位置（ｕ_i2，ｖ_i2）に基づき、実空間における当該第２指標点Ｐ_i2のＸ座標値Ｘ_i2およびＹ座標値Ｙ_i2が測定される（図１／ＳＴＥＰ１４）。これにより、例えば、第２指標点Ｐ₀（ｉ₂＝０）の画像座標系における位置（ｕ₀，ｖ₀）に基づき、Ｘ座標値Ｘ₀およびＹ座標値Ｙ₀が測定される（図２Ａ上側および図２Ｂ上側参照）。

第２指標点Ｐ_i2に関する縦実空間位置推定処理の結果（図１／ＳＴＥＰ１２参照）および横実空間位置推定処理の結果（図１／ＳＴＥＰ１４参照）に基づき、「実空間位置推定処理」が実行されることで、当該第２指標点Ｐ_i2の実空間位置（Ｘ_i2，Ｙ_i2，Ｚ_i2）がワークＷの実空間位置として推定される（図１／ＳＴＥＰ１６参照）。これにより、例えばワークＷの代表点である第２指標点Ｐ₀（ｉ₂＝０）の実空間位置（Ｘ₀，Ｙ₀，Ｚ₀）が推定される（図２Ａおよび図２Ｂ参照）。

縦実空間位置推定処理の結果（図１／ＳＴＥＰ１２参照）に基づき、「第１実空間姿勢推定処理」が実行される（図１／ＳＴＥＰ１８）。具体的には、各指標点Ｐ_iのＸ座標値Ｘ_iおよびＹ座標値Ｙ_i（第１指定点については既知の値であり、第２指定点については推定値である。）ならびにＺ座標値Ｚ_iの推定結果とに基づき、複数の指標点Ｐ_iのうち対をなす指標点Ｐ_iおよびＰ_jを通る指標直線Ｑ_ijの実空間姿勢が第１実空間姿勢として推定される。例えば、指標直線Ｑ_ijの方向ベクトル（ａ_i1，ｂ_i1，ｃ_i1）に基づき、実空間のＸ軸に対する指標直線Ｑ_ijの角度θ_1X(i,j)＝ａｒｃｔａｎ（ｃ_i1／ａ_i1）および実空間のＹ軸に対する指標直線Ｑ_ijの角度θ_1Y(i,j)＝ａｒｃｔａｎ（ｃ_i1／ｂ_i1）のそれぞれが、第１実空間姿勢θ_1(i,j)として推定される。

対象物座標系のｘｙ平面の実空間のＸＹ平面に対する姿勢が基準姿勢（例えば平行）である基準状態における指標点Ｐ_iのＺ座標値Ｚ_i（０）に対する、対象物座標系のｘｙ平面の実空間のＸＹ平面に対する姿勢が不明である任意の状態における指標点Ｐ_iのＺ座標値Ｚ_i（ｋ）の偏差ΔＺ_i（ｋ）に基づき、関係式（０１）にしたがって第１実空間姿勢θ_1(i,j)が推定されてもよい。対象物座標系のｘｙ平面の実空間のＸＹ平面に対する姿勢が基準姿勢に調節された上で、指標点Ｐ_iのＺ座標値Ｚ_i（０）が予め測定されていてもよい。

θ_1(i,j)＝ａｒｃｔａｎ｛（ΔＺ_j（ｋ）−ΔＺ_i（ｋ））／Ｌ_ij｝‥（０１）。

例えば、図２Ａ下側に示されている状態では、ｘ軸がＸ軸に対して平行であり、かつ、ｙ軸がＹ軸に対して平行である。この状態では、対象物座標系のｘｙ平面が、実空間のＸＹ平面に対して平行であり、ＸＹ平面に対する第１指標直線（ｘ軸線）の第１実空間姿勢θ_1(0,1)および第２指標直線（ｙ軸線）の第１実空間姿勢θ_1(0,2)がともに０であると推定される。

これに対して、図２Ｂ下側に示されている状態では、対象物座標系のｘｙ平面が、実空間のＸＹ平面に対して傾斜している。この状態では、指標点Ｐ_iのＺ座標値Ｚ_i（ｋ）（縦実空間位置推定処理の結果）に基づき、関係式（０１）にしたがって、第１指標直線の第１実空間姿勢θ_1(0,1)および第２指標直線の第１実空間姿勢θ_1(0,2)が推定される。

撮像画像に基づき「第２サイズ測定処理」が実行され、画像座標系における指標直線の延在方向について、複数の指標領域Ａ_iのうち少なくとも１つの指標領域のサイズが測定される（図１／ＳＴＥＰ２０）。これにより、例えば、第１指標直線Ｑ₀₁（対象物座標系のｘ軸に相当する。）および第２指標直線Ｑ₀₂（対象物座標系のｙ軸に相当する。）のそれぞれの延在方向について、撮像画像における第２指標領域Ａ₂のサイズφ₀₁およびφ₀₂が測定される（図２Ａ上側および図２Ｂ上側参照）。

続いて、第２サイズ測定処理における測定結果に基づき「第２実空間姿勢推定処理」が実行され、少なくとも１つの指標領域Ａ_iの実空間姿勢が第２実空間姿勢として推定される（図１／ＳＴＥＰ２２）。指標領域Ａ_iの垂線ベクトル（ａ_i2，ｂ_i2，ｃ_i2）に基づき、実空間のＸ軸に対する指標直線Ｑ_ijの角度θ_2X(i,j)＝ａｒｃｔａｎ（ｃ_i2／ａ_i2）および実空間のＹ軸に対する指標直線Ｑ_ijの角度θ_2Y(i,j)＝ａｒｃｔａｎ（ｃ_i2／ｂ_i2）のそれぞれが、第２実空間姿勢θ_2(i,j)として推定される。

例えば、図４Ａ〜図４Ｃ上側に示されているように、実空間のＸＹ平面に対する指標直線Ｑ₀₁（この例では対象物座標系のｘ軸に相当する。）の傾斜角度ξが大きくなるほど、図４Ａ〜図４Ｃ下側に示されているように、画像座標系における指標領域Ａ_iの当該指標直線の延在方向（この例ではｘ方向）についてのサイズφ₀₁（ξ）が小さくなるというように、当該傾斜角度ξおよび当該サイズφ_ij（ξ）の間には相関関係（第２相関関係）が存在する。よって、画像座標系（ｕ，ｖ）における指標領域Ａ_iの指標直線Ｑ_{i j}の延在方向についてのサイズφ_ijに基づき、当該第２相関関係にしたがって、第２実空間姿勢θ_2(i,j)が推定されうる。

対象物座標系のｘｙ平面の実空間のＸＹ平面に対する姿勢が基準姿勢（例えば平行）である基準状態における指標領域Ａ_iのサイズφ_ij（ξ（０））に対する、対象物座標系のｘｙ平面の実空間のＸＹ平面に対する姿勢が不明である任意の状態における指標領域Ａ_iのサイズφ_ij（ξ（ｋ））の偏差Δφ_ij（ｋ）に基づき、関係式（０２）にしたがって第２実空間姿勢θ_2(i,j)が推定される。対象物座標系のｘｙ平面の実空間のＸＹ平面に対する姿勢が基準姿勢に調節された上で、指標領域Ａ_iのサイズφ_ij（ξ（０））が予め測定されていてもよい。

θ_2(i,j)＝ａｒｃｃｏｓ｛Δφ_ij（ｋ）／φ_ij（ｋ）｝‥（０２）。

第１実空間姿勢推定処理の結果（図１／ＳＴＥＰ１８参照）および第２実空間姿勢推定処理の結果（図１／ＳＴＥＰ２２参照）に基づき、「実空間姿勢推定処理」が実行される（図１／ＳＴＥＰ２４）。具体的には、前記のように推定された第１実空間姿勢θ_1(i,j)および第２実空間姿勢θ_2(i,j)に基づき、関係式（０４）にしたがってワークＷの実空間姿勢θ_(i,j)が推定される。

θ_(i,j)＝αθ_1(i,j)＋（１−α）θ_2(i,j) （０＜α＜１）‥（０４）。

重み係数αは、例えば「０．５」に設定されるが、第１実空間姿勢θ_1(i,j)および第２実空間姿勢θ_2(I,j)のそれぞれの推定精度の高低差に応じて、適宜変更されてもよい。例えば、撮像画像における光線由来の第１指標領域の認識精度が、ワークＷの構造または外観由来の第２指標領域の認識精度よりも高い場合、αが０．５より大きい値に設定されてもよい。

（第２実施形態）
本発明の第２実施形態としての位置姿勢推定方法においては、第２サイズ測定処理（図１／ＳＴＥＰ２０参照）および第２実空間姿勢推定処理（図１／ＳＴＥＰ２２参照）が省略されている点で第１実施形態と相違する。

第２実施形態では、複数の指標直線Ｑ_ijおよびＱ_i’j’は相互に平行に配置され、または、同一の直線である。例えば、図５Ａ上側および図５Ｂ上側に示されているように、指標点Ｐ₀およびＰ₁を通る第１指標直線Ｑ₀₁（ｉ＝０，ｊ＝１）および指標点Ｐ₀およびＰ₃を通る第３指標直線Ｑ₀₃（ｉ’＝０，ｊ’＝１）は、同一の直線（対象物座標系のｘ軸に相当する。）である。同様に、指標点Ｐ₀およびＰ₂を通る第２指標直線Ｑ₀₂（ｉ＝０，ｊ＝２）および指標点Ｐ₀およびＰ₄を通る第４指標直線Ｑ₀₄（ｉ’＝０，ｊ’＝４）は、同一の直線（対象物座標系のｙ軸に相当する。）である。第１指標点として定義されている４つの指標点Ｐ₁〜Ｐ₄のそれぞれに対して光線が照射されて第１指標領域Ａ₁〜Ａ₄が形成かつ定義される（図１／ＳＴＥＰ０２参照）。

第１サイズ測定処理、縦実空間位置推定処理および第１実空間姿勢推定処理（図１／ＳＴＥＰ１０→ＳＴＥＰ１２→ＳＴＥＰ１８参照）を経て、相互に平行である（または同一の直線である）複数の指標直線Ｑ_i1j1〜Ｑ_iMjM（Ｍ＝２，３，‥）のそれぞれのＸＹ平面に対する角度が第１実空間姿勢θ_1(i1,j1)〜θ_1(iM,jM)として推定される。そして、複数の第１実空間姿勢θ_1(i1,j1)〜θ_1(iM,jM)に基づき、関係式（０６）にしたがって「実空間姿勢推定処理」が実行される（図１／ＳＴＥＰ２４参照）。

θ＝Σ_m=1-Mβ_mθ_1(im,jm)) （０＜β_m＜１、Σβ_m＝１）‥（０６）。

例えば、図５Ａ下側に示されている基準状態では、ＸＹ平面に対する第１〜第４指標直線のそれぞれの第１実空間姿勢θ_1(0,1)〜θ_1(0,4)が０であると推定される（図２Ａ下側参照）。これに対して、図５Ｂ下側に示されている任意の状態では、対象物座標系のｘ軸線の実空間のＸＹ平面に対する角度θ_xが（θ_1(0,1)＋θ_1(0,3)）／２（β₀₁＝１／２、β₀₃＝１／２）であると推定され、対象物座標系のｙ軸線の実空間のＸＹ平面に対する角度θ_yが（θ_1(0,２)＋θ_1(0,４)）／２（β₀₂＝１／２、β₀₄＝１／２）であると推定される。

（本発明の作用効果）
本発明の位置姿勢推定方法によれば、実空間のＺ軸に平行な光軸を有する撮像装置４を通じて取得されたワークＷの各指標領域Ａ_iのサイズφ_iに応じて、各指標点Ｐ_iのＺ座標値が推定される（図１／ＳＴＥＰ０４→ＳＴＥＰ１０→ＳＴＥＰ１２、図２Ａ、図２Ｂ参照）。これは、指標点Ｐ_iが撮像装置４（またはその撮像素子）に近くなるほどその測定サイズが大きくなり、指標点Ｐ_iが撮像装置４から遠くなるほどその測定サイズが小さくなるという定性的関係に依拠している（図３Ａ〜図３Ｃ参照）。各指標点Ｐ_iのＺ座標値Ｚ_iに基づき、対をなす指標点Ｐ_iおよびＰ_jを通る指標直線Ｑ_ijの第１実空間姿勢θ_1(i,j)（方向ベクトルに応じて定まる。）が推定される（図１／ＳＴＥＰ１８→ＳＴＥＰ２４、関係式（０４）参照）。照射装置２を用いて複数の指標点Ｐ_iのうち第１指標点Ｐ_i1に対して光線２０が照射されることで、当該第１指標点Ｐ_i1を含む第１指標領域Ａ_i1が形成かつ定義される（図１／ＳＴＥＰ０２、図２Ａ上側、図２Ｂ上側、図５Ａ上側および図５Ｂ上側参照）。

撮像装置４の撮像範囲において、ワークＷの一部が他の物体により隠されている状況であっても、第１指標点Ｐ_i1に対して当該他の物体を回避するようにまたは間隙を縫うように当該ワークＷの第１指標点Ｐ_i1に照射されることで、第１指標領域Ａ_i1が形成される。同一の指標直線上にある複数の指標点Ｐ_iのそれぞれを含む複数の指標領域Ａ_iを撮像画像に容易に含ませることができる。よって、このような状況であっても、ワークＷの少なくともＺ座標値Ｚ_iにより定義される実空間位置および少なくとも指標直線Ｑ_ijの第１実空間姿勢θ_1(i,j)により定義されるワークＷの実空間姿勢を簡易かつ高精度で推定することができる。

第１実施形態によれば、撮像画像における少なくとも１つの指標領域Ｐ_iの、指標直線Ｑ_ijの延在方向についてのサイズφ_ijに応じて、当該指標領域Ｐ_iの第２実空間姿勢θ_2(i,j)（指標領域Ｐ_iの垂線ベクトルに応じて定まる。）が推定される（図１／ＳＴＥＰ２０→ＳＴＥＰ２２、関係式（０２）参照）。第１実空間姿勢θ_1(i,j)の推定結果および第２実空間姿勢θ_2(i,j)の推定結果が統合されることで、ワークＷの実空間姿勢θ_(i,j)の推定精度の向上が図られる（図１／ＳＴＥＰ２４、関係式（０４）参照）。

第２実施形態によれば、実空間姿勢推定処理（図１／ＳＴＥＰ２４参照）において、異なる対のそれぞれをなす指標点を通り、かつ、同一のまたは相互に平行な複数の指標直線Ｑ_i1j1〜Ｑ_iMjMのそれぞれの第１実空間姿勢θ_1(im,jm)の推定結果を総合することにより、ワークＷの実空間姿勢θ_(i,j)が推定される（関係式（０６）参照）。これにより、単一の指標直線の第１実空間姿勢θ_1(im,jm)のみに基づいてワークＷの実空間姿勢θ _(i,j)が推定されるよりも、当該推定精度の向上が図られる。

（他の実施形態）
前記実施形態において、横実空間位置推定処理（図１／ＳＴＥＰ１４参照）が省略されてもよい。この場合、少なくとも１つの指標点Ｐ_iのＺ座標値Ｚ_iの推定結果またはその統合結果がワークＷの実空間位置の推定結果とされる。

第２実施形態と同様に複数の指標点Ｐ_iが定義され、第１の対をなす第２指標点Ｐ₁およびＰ₃を通る指標直線Ｑ₁₃（対象物座標系のｘ軸に相当する。）および第２の対をなす第２指標点Ｐ₂およびＰ₄を通る指標直線Ｑ₂₄（対象物座標系のｙ軸に相当する。）のそれぞれのＸ軸およびＹ軸に対する角度が第１実空間姿勢θ_1(1,3)およびθ_1(2,4)として推定された上で、本発明の第１実施形態と同様の手順にしたがってワークＷの姿勢が推定されてもよい。

前記実施形態において「第１準備処理」が予め実行されていてもよい。具体的には、複数の指標点Ｐ_iのうち少なくとも１つの指標点Ｐ_kのＺ座標値Ｚ_kを変化させながら、ワークＷの撮像画像における少なくとも１つの指標領域Ａ_kのサイズφ_kの変化を測定することにより、当該Ｚ座標値Ｚ_kと当該サイズφ_kとの相関関係を第１相関関係として定義する。これにより、縦実空間位置推定処理（図１／ＳＴＥＰ１２）において、第１相関関係にしたがって各指標点Ｐ_iのＺ座標値Ｚ_iが推定される。

前記実施形態において「第２準備処理」が予め実行されていてもよい。具体的には、少なくとも１つの指標領域Ｐ_iの実空間姿勢θ_(i,j)を変化させながら、撮像画像における当該指標領域Ｐ_iの、指標直線Ｑ_ijの延在方向についてのサイズφ_ijの変化を測定することにより、当該実空間姿勢θ_(i,j)と撮像画像における指標直線Ｑ_ijの延在方向についてのサイズφ_ijとの相関関係を第２相関関係として定義する。これにより、第２実空間姿勢推定処理（図１／ＳＴＥＰ２２参照）において、第２相関関係にしたがって第２実空間姿勢θ_2(i,j)が推定される。

１‥基台、２‥照射装置、４‥撮像装置、２０‥光線、Ａ_i‥指標領域、Ｐ_i‥指標点、Ｗ‥ワーク（対象物）。

Claims (7)

1. ＸＹＺ座標系により定義される実空間における位置および姿勢が固定され、Ｚ軸に対して平行な光軸を有する撮像装置を通じて対象物の撮像画像を取得し、対象物において定義されている、前記対象物の外観または構造に由来する少なくとも１つの指標領域を含む複数の指標領域のそれぞれの前記撮像画像におけるサイズを測定する第１サイズ測定処理と、
   前記第１サイズ測定処理における測定結果に基づき、前記複数の指標領域のそれぞれに含まれている複数の指標点のそれぞれのＺ座標値を推定する縦実空間位置推定処理と、
   前記複数の指標点のそれぞれのＸ座標値およびＹ座標値のうち少なくとも一方と、前記複数の指標点のそれぞれのＺ座標値の推定結果とに基づき、前記複数の指標点のうち対をなす指標点を通る指標直線の実空間における姿勢を第１実空間姿勢として推定する第１実空間姿勢推定処理と、
   前記第１実空間姿勢の推定結果に基づいて前記対象物の実空間姿勢を推定する実空間姿勢推定処理と、
   前記撮像画像における前記指標直線の延在方向について、前記少なくとも１つの指標領域のサイズを測定する第２サイズ測定処理と、
   前記第２サイズ測定処理における測定結果に基づき、前記少なくとも１つの指標領域の実空間姿勢を第２実空間姿勢として推定する第２実空間姿勢推定処理と、を含み、
   前記第１サイズ測定処理において、前記実空間における位置および姿勢が固定されている照射装置を用いて、前記複数の指標点のうち第１指標点に対して光線を照射することで、前記複数の指標領域のうち少なくとも１つの指標領域として第１指標領域を形成し、
   前記実空間姿勢推定処理において、前記第１実空間姿勢の推定結果および前記第２実空間姿勢の推定結果を総合することにより、前記対象物の実空間姿勢を推定することを特徴とする位置姿勢推定方法。
2. ＸＹＺ座標系により定義される実空間における位置および姿勢が固定され、Ｚ軸に対して平行な光軸を有する撮像装置を通じて対象物の撮像画像を取得し、対象物において定義されている複数の指標領域のそれぞれの前記撮像画像におけるサイズを測定する第１サイズ測定処理と、
   前記第１サイズ測定処理における測定結果に基づき、前記複数の指標領域のそれぞれに含まれている複数の指標点のそれぞれのＺ座標値を推定する縦実空間位置推定処理と、
   前記複数の指標点のそれぞれのＸ座標値およびＹ座標値のうち少なくとも一方と、前記複数の指標点のそれぞれのＺ座標値の推定結果とに基づき、前記複数の指標点のうち対をなす指標点を通る指標直線の実空間における姿勢を第１実空間姿勢として推定する第１実空間姿勢推定処理と、
   前記第１実空間姿勢の推定結果に基づいて前記対象物の実空間姿勢を推定する実空間姿勢推定処理と、を含み、
   前記第１サイズ測定処理において、前記実空間における位置および姿勢が固定されている照射装置を用いて、前記複数の指標点のうち第１指標点に対して光線を照射することで、前記複数の指標領域のうち少なくとも１つの指標領域として第１指標領域を形成し、
   前記実空間姿勢推定処理において、異なる対のそれぞれをなす指標点を通り、かつ、同一のまたは相互に平行な複数の前記指標直線のそれぞれの前記第１実空間姿勢の推定結果を総合することにより、前記対象物の実空間姿勢を推定することを特徴とする位置姿勢推定方法。
3. 請求項１または２記載の位置姿勢推定方法において、
   前記複数の指標点の異なる対によって、前記実空間において直交する一対の指標直線が定義されるように、前記複数の指標点が設定され、
   前記第１実空間姿勢推定処理において、前記一対の指標直線のそれぞれの実空間における姿勢を前記第１実空間姿勢として推定することを特徴とする位置姿勢推定方法。
4. 請求項１〜３のうちいずれか１つに記載の位置姿勢推定方法において、
   前記複数の指標点のうち対象物座標系に固定されている第２指標点の前記撮像画像における位置に基づき、前記第２指標点のＸ座標値およびＹ座標値を推定する横実空間位置推定処理をさらに含むことを特徴とする位置姿勢測定方法。
5. 請求項４記載の位置姿勢推定方法において、
   前記対象物において前記撮像装置を通じて認識可能な輪郭により画定される第２指標領域に含まれる点が、前記第２指標点として定義されていることを特徴とする位置姿勢推定方法。
6. 請求項１記載の位置姿勢推定方法において、
   前記複数の指標点のうち少なくとも１つの指標点のＺ座標値を変化させながら、前記対象物の撮像画像における前記少なくとも１つの指標領域のサイズの変化を測定することにより、前記少なくとも１つの指標点のＺ座標値と前記撮像画像における前記少なくとも１つの指標領域のサイズとの相関関係を第１相関関係として定義する第１準備処理をさらに含み、
   前記縦実空間位置推定処理において、前記第１相関関係にしたがって前記複数の指標点のそれぞれのＺ座標値を推定することを特徴とする位置姿勢推定方法。
7. 請求項１記載の位置姿勢推定方法において、
   前記少なくとも１つの指標領域の実空間姿勢を変化させながら、前記撮像画像における前記少なくとも１つの指標領域の、前記指標直線の延在方向についてのサイズの変化を測定することにより、前記少なくとも１つの指標領域の実空間姿勢と前記撮像画像における前記指標直線の延在方向についてのサイズとの相関関係を第２相関関係として定義する第２準備処理をさらに含み、
   前記第２実空間姿勢推定処理において、前記第２相関関係にしたがって前記第２実空間姿勢を推定することを特徴とする位置姿勢推定方法。