(57)【要約】【課題】  対象物に配置する基本標識の数が４つ以上の場合に比べ基本標識の設置領域を小さくすることができる位置計測用演算装置、位置計測システムおよびプログラムを提供する。【解決手段】  位置計測システムは、対象物１に配置された位置関係の分かっている３つの基本標識ａ１，ａ２，ａ３と、２次元撮像素子２を有する撮像装置３と、撮像装置３により撮像した３つの基本標識ａ１，ａ２，ａ３の画像に基づいて対象物１の３次元位置候補を１つ乃至４つ算出し、この算出した３次元位置候補を用いて対象物１の３次元位置および角度の少なくとも一方を求める演算を行う位置計測用演算装置４とを備える。【選択図】図１
2010210437.tif 000002
【特許請求の範囲】
【請求項１】
２次元撮像素子を有する撮像装置により対象物に配置された位置関係の分かっている３つの基本標識を撮像した画像に基づいて前記対象物の３次元位置候補を１つ乃至４つ算出する算出部と、前記算出した３次元位置候補を用いて前記対象物の３次元位置および角度の少なくとも一方を導出する導出部とを備えた位置計測用演算装置。
【請求項２】
前記導出部が、前記対象物の３次元位置および角度の少なくとも一方を前記算出した３次元位置候補の平均値から導出する請求項１記載の位置計測用演算装置。
【請求項３】
前記導出部が、前記対象物の初期の３次元位置が既知の場合、前記対象物の３次元位置および角度の少なくとも一方を前記算出した３次元位置候補のうちで前記初期の３次元位置に近い値から導出する請求項１記載の位置計測用演算装置。
【請求項４】
前記導出部が、前記対象物の３次元位置および角度の少なくとも一方を前記算出した３次元位置候補のうちで前回演算した３次元位置に近い値から導出する請求項１記載の位置計測用演算装置。
【請求項５】
前記導出部が、前記対象物の３次元位置および角度の少なくとも一方を前記算出した３次元位置候補のうちで前回までに演算した３次元位置に基づいて予測した３次元位置に近い値から導出する請求項１記載の位置計測用演算装置。
【請求項６】
コンピュータに、２次元撮像素子を有する撮像装置により対象物に配置された位置関係の分かっている３つの基本標識を撮像した画像に基づいて前記対象物の３次元位置候補を１つ乃至４つ算出する手順、前記算出した３次元位置候補を用いて前記対象物の３次元位置および角度の少なくとも一方を求める手順を実行させるプログラム。
【請求項７】
対象物に配置された位置関係の分かっている３つの基本標識と、２次元撮像素子を有する撮像装置と、前記撮像装置により撮像した前記３つの基本標識の画像に基づいて前記対象物の３次元位置候補を１つ乃至４つ算出し、前記算出した３次元位置候補を用いて前記対象物の３次元位置および角度の少なくとも一方を求める演算を行う演算装置とを備えた位置計測システム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、位置計測用演算装置、位置計測システムおよびプログラムに関するものである。
【背景技術】
【０００２】
基板やカード等の対象物に位置計測の基本標識となる複数の光源を取り付け、この複数の光源をカメラ等の撮像装置で撮像し、その撮像した画像に基づいて対象物の３次元位置を求める技術が知られている。例えば、特許文献１に記載の位置計測システムは、互いの位置関係が判明している４個以上の光源と、各光源からの光を透過して球面収差により光リング像を形成する光学レンズ系と、光学レンズ系により形成された光リング像を検出する撮像素子と、撮像素子により検出された光リング像の検出情報および各光源の位置関係に基づいて各光源の位置を計測する演算装置とを備えて構成される。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００８−５８２０４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
本発明の目的は、対象物に配置する基本標識の数が４つ以上の場合に比べ基本標識の設置領域を小さくすることができる位置計測用演算装置、位置計測システムおよびプログラムを提供することにある。
【課題を解決するための手段】
【０００５】
本発明は、上記目的を達成するために、以下の位置計測用演算装置、位置計測システムおよびプログラムを提供する。
（１）２次元撮像素子を有する撮像装置により対象物に配置された位置関係の分かっている３つの基本標識を撮像した画像に基づいて前記対象物の３次元位置候補を１つ乃至４つ算出する算出部と、前記算出した３次元位置候補を用いて前記対象物の３次元位置および角度の少なくとも一方を導出する導出部とを備えた位置計測用演算装置。
（２）前記導出部が、前記対象物の３次元位置および角度の少なくとも一方を前記算出した３次元位置候補の平均値から導出する上記（１）記載の位置計測用演算装置。
（３）前記導出部が、前記対象物の初期の３次元位置が既知の場合、前記対象物の３次元位置および角度の少なくとも一方を前記算出した３次元位置候補のうちで前記初期の３次元位置に近い値から導出する上記（１）記載の位置計測用演算装置。
（４）前記導出部が、前記対象物の３次元位置および角度の少なくとも一方を前記算出した３次元位置候補のうちで前回演算した３次元位置に近い値から導出する上記（１）記載の位置計測用演算装置。
（５）前記導出部が、前記対象物の３次元位置および角度の少なくとも一方を前記算出した３次元位置候補のうちで前回までに演算した３次元位置に基づいて予測した３次元位置に近い値から導出する上記（１）記載の位置計測用演算装置。
（６）コンピュータに、２次元撮像素子を有する撮像装置により対象物に配置された位置関係の分かっている３つの基本標識を撮像した画像に基づいて前記対象物の３次元位置候補を１つ乃至４つ算出する手順、前記算出した３次元位置候補を用いて前記対象物の３次元位置および角度の少なくとも一方を求める手順を実行させるプログラム。
（７）対象物に配置された位置関係の分かっている３つの基本標識と、２次元撮像素子を有する撮像装置と、前記撮像装置により撮像した前記３つの基本標識の画像に基づいて前記対象物の３次元位置候補を１つ乃至４つ算出し、前記算出した３次元位置候補を用いて前記対象物の３次元位置および角度の少なくとも一方を求める演算を行う演算装置とを備えた位置計測システム。
【発明の効果】
【０００６】
請求項１に記載の位置計測用演算装置によれば、対象物に配置する基本標識の数が４つ以上の場合に比べ基本標識の設置領域を小さくすることができる。
請求項２に記載の位置計測用演算装置によれば、本構成を有していない場合に比べ簡単な演算で対象物の３次元位置や角度を求めることができる。
することができる。
請求項３に記載の位置計測用演算装置によれば、本構成を有していない場合に比べ簡単に対象物の３次元位置や角度を求めることができる。
請求項４に記載の位置計測用演算装置によれば、本構成を有していない場合に比べ簡単に対象物の３次元位置や角度を求めることができる。
請求項５に記載の位置計測用演算装置によれば、本構成を有していない場合に比べ正確に対象物の３次元位置や角度を求めることができる。
請求項６に記載のプログラムによれば、対象物に配置する基本標識の数が４つ以上の場合に比べ基本標識の設置領域を小さくすることができる位置計測システムを提供することができる。
請求項７に記載の位置計測システムによれば、対象物に配置する基本標識の数が４つ以上の場合に比べ基本標識の設置領域を小さくすることができる。
【図面の簡単な説明】
【０００７】
【図１】本発明に係る位置計測用演算装置を用いた位置計測システムの一実施例を示す図である。
【図２】３つの基本標識の画像に基づいて対象物の３次元位置を演算する方法の一例を示す図である。
【図３】位置計測用演算装置としてパーソナルコンピュータ（ＰＣ）を用いた場合の一例を示すブロック図である。
【図４】（ａ）、（ｂ）はそれぞれ位置計測用演算装置で行われる演算の例を示すフロー図である。
【図５】（ａ）、（ｂ）はそれぞれ位置計測用演算装置で行われる演算の他の例を示すフロー図である。
【発明を実施するための形態】
【０００８】
図１は、本発明に係る位置計測用演算装置を用いた位置計測システムの一実施例を示す図である。本実施例は、図示のように、対象物１に配置された位置関係の分かっている３つの基本標識ａ１，ａ２，ａ３と、２次元撮像素子２を有する撮像装置３と、撮像装置３により撮像した３つの基本標識ａ１，ａ２，ａ３の画像に基づいて対象物１の３次元位置候補を１つ乃至４つ算出し、この算出した３次元位置候補を用いて対象物１の３次元位置および角度の少なくとも一方を求める演算を行う位置計測用演算装置４とを備える。位置計測用演算装置４の構成例については後述する。このように、本実施例では、位置関係の分かっている基本標識が同一平面上に３つ配置されており、これらを撮像した画像から対象物１の３次元位置および角度の少なくとも一方を求める。対象物に配置する基本標識は３つなので４つ以上配置する場合に比べ設置領域は小さくて済む。
【０００９】
対象物１は本例ではカードや基板等の平板状のものであるが、これに限定されない。また、対象物１は本例では三角形であるが、これに限定されない。基本標識ａ１，ａ２，ａ３は、撮像装置３で撮像して画像情報が得られるものであれば特に限定されない。例えば基本標識ａ１，ａ２，ａ３を対象物１に印刷あるいは貼付することができる。また、基本標識としてＬＥＤ等の光源を用いることができ、さらに光源の代わりに再帰反射板を用い、その再帰反射板を照明するための照明装置を備えるようにしてもよい。撮像装置３は、例えばＣＣＤやＣＭＯＳセンサ等の２次元撮像素子２を搭載したデジタルカメラが用いられるが、これに限定されない。図１中の画像面５は２次元撮像素子２の素子面を示す。演算装置４は、撮像装置３の図示しない通信手段と有線あるいは無線で接続され、撮像装置３と通信できるように構成される。演算装置４は、例えばパーソナルコンピュータ（ＰＣ）等のコンピュータが用いられるが、これに限定されない。以下、演算装置４における演算の一例を説明する。
【００１０】
図２は、３つの基本標識の画像に基づいて対象物の３次元位置を演算する方法の一例を示す図である。以下、基本標識をＬＥＤ等で構成された光源、そして撮像装置を２次元撮像素子を有するカメラとして説明する。本例では、３つの光源をそれぞれ三角形の角に配置する。これらの光源の３点を用いて、対象物１の３次元位置候補を１つ乃至４つ算出する。そして、この算出した３次元位置候補を用いて対象物１の３次元位置や角度を求める。
【００１１】
まず、図２において、光源（基本標識）ａ１，ａ２，ａ３の画像面（カメラの２次元撮像素子面）５上の画像位置ｃ１，ｃ２，ｃ３とカメラの光学中心６との関係から、カメラ座標系における光源位置の方向ベクトルＤｉ（ｉ＝１，２，３）を算出する。すなわち、光源の画像位置をＸｋｋ１，Ｘｋｋ２，Ｘｋｋ３とし、その座標を［ｘＸ１，ｘＹ１］，［ｘＸ２，ｘＹ２］，［ｘＸ３，ｘＹ３］とする。光源画像のｚ座標の値を−１として３次元座標［ｘＸ１，ｘＹ１，−１］，［ｘＸ２，ｘＹ２，−１］，［ｘＸ３，ｘＹ３，−１］に変換する。
【００１２】
光源画像から光源の存在する方向へのベクトルをそれぞれＤ１，Ｄ２，Ｄ３とすると次式で表わすことができる。ただし、各ベクトルは規格化されたものとする。
D1=(x1,y1,z1)
D2=(x2,y2,z2)
D3=(x3,y3,z3)          (1)
【００１３】
光源の位置ベクトルをＰ１，Ｐ２，Ｐ３とすると次式で表わすことができる。
P1=(px1,py1,pz1)
P2=(px2,py2,pz2)
P3=(px3,py3,pz3)      (2)
【００１４】
光源Ｐｉは方向ベクトルＤｉ上に存在する未知係数をｔ１，ｔ２，ｔ３として以下のように表わすことができる。
P1=t1D1
P2=t2D2
P3=t3D3                (3)
【００１５】
座標の間には次の関係が成り立つ。
px1=t1x1
py1=t1y1
・・・
pz3=t3z3              (4)
【００１６】
光源間の距離｜Ｐ１Ｐ２｜は次のように表される。
|P1P2|2=(px1-px2)2+(py1-py2)2+(pz1-pz2)2
=(t1x1-t2x2)2+(t1y1-t2y2)2+(t1z1-t2z2)2
=x12t12-(2x1x2)t1t2+x22t22+y12t12-(2y1y2)t1t2+y22t22+z12t12-(2z1y2)t1t2+z22t22
=t12-2(x1x2+y1y2+z1z2)t1t2+t22          (5)
【００１７】
ここで、Ａ１２＝ｘ１ｘ２＋ｙ１ｙ２＋ｚ１ｚ２，｜Ｐ１Ｐ２｜＝Ｌ１２などとおくと
t12-2A12t1t2+t22-L122=0
t22-2A23t2t3+t32-L232=0
t32-2A31t3t1+t32-L312=0                (6)
移項して、
L122=t12+t22-2t1t2A12
L232=t22+t32-2t2t3A23
L312=t32+t12-2t3t1A31                  (7)
【００１８】
そして
t2=x*t1,t3=y*t1,A12=F,A23=G,A31=H      (8)
とおくと、
L312=t12+y2t12-2t12yH                  (9)
L122=t12+x2t12-2t12xF                (10)
L232=x2t12+y2t12-2t12xyG              (11)
【００１９】
式９と１１から
L232(1+y2-2yH)=L312(x2+y2-2xyG)      (12)
式１０と１１から
L232(1+x2-2fF)=L122(x2+y2-2xyG)      (13)
【００２０】
ここで
【数１】
2010210437.tif 000003
とおく。
【００２１】
式１２と１４から
0=y2(1-K)+2y(KH-xG)+(x2-K)          (15)
式１３と１４から
0=y2+2y(-xG)+x2(1-L)+2LFx-L        (16)
式１５と１６は次の形をしている。
0=my2+py+q                        (17)
0=m’y2+p’y+q’                  (18)
【００２２】
式１７にｍ’を式１８にｍをかけてひくと
0=(pm’-p’m)y+(m’q-mq’)        (19)
式１７にｑ’を式１８にｑをかけてひくと
0=(m’q-mq’)y2+(p’q-pq’)y
0=(m’q-mq’)y+(p’q-pq’)        (20)
【００２３】
今、ｍ’ｑ≠ｍｑ’、すなわち
(x2-K)≠x2(1-K)(1-L)+2xL(1-K)F-(1-K)L      (21)
とする。このとき、式１９は式１７と式２０は式１８と等しくなる。
式１９に（ｍ’ｑ−ｍｑ’）をかけ、式２０に（ｐｍ’−ｐ’ｍ）
をかけてひくと、
0=(m’q-mq’)2-(pm’-p’m)(p’q-pq’)      (22)
【００２４】
式２２を展開して整理すると次のｘの４次方程式になる。
0=G4x4+G3x3+G2x2+G1x2+G0                    (23)
ここで
G4=(KL-K-L)2-4KLG2                          (24)
G3=4[KL-K-L]L(1-K)F+4KG[(KL+L-K)H+2LFG]      (25)
G2=[2L(1-K)F]2+2[KL+K-L][KL-K-L]+4K[(K-L)G2+(1-L)KH2-2L(1+K)FGH]    (26)
G1=4(KL+K-L)L(1-K)F+4K[(KL-K+L)GH+2KLFH2]      (27)
G0=(KL+K=L)2-4K2LH2                          (28)
【００２５】
４次方程式は最大４つの解を持つから、ｘは４つ求まる。すなわちベクトルｔ１，ｔ２，ｔ３の組が４つ求まる。以下、ｘからｔ１，ｔ２，ｔ３の求め方を示す。
【００２６】
式１０から
【数２】
2010210437.tif 000004
【００２７】
また、式８より
t2=t1x                                        (30)
【００２８】
ｍ’ｑ≠ｍｑ’ならば、式２０より
【数３】
2010210437.tif 000005
ｍ’ｑ＝ｍｑ’ならば、式３１は不定になる。式９よりｙは以下の２つの解をもつ。
【数４】
2010210437.tif 000006
式８より
t3=y*t1                                        (33)
【００２９】
上記で求めた係数ｔ１，ｔ２，ｔ３を式３に入れ、光源の位置ベクトルＰ１，Ｐ２，Ｐ３をそれぞれ算出する。上記より、ｔ１，ｔ２，ｔ３の組は１つ乃至４つ求めることができるので、Ｐ１，Ｐ２，Ｐ３の組も１つ乃至４つ算出することができる。ここで算出したＰ１，Ｐ２，Ｐ３の組はそれぞれ対象物の３次元位置候補となる。この３次元位置候補を用いて対象物の３次元位置や角度を求めることができる。
【００３０】
例えば、算出した３次元位置候補の平均値を対象物の３次元位置とする。これは、それほど精度がいらない場合に特に有効である。また、対象物の初期の３次元位置が既知の場合、算出した３次元位置候補のうちで初期の３次元位置に近い値、例えば一番近い値を対象物の３次元位置とする。これは、初期値が同定できる場合には特に有効である。また、算出した３次元位置候補のうちで前回演算した３次元位置に近い値、例えば一番近い値を対象物の３次元位置とする。これも、初期値が同定できる場合に特に有効である。さらに、算出した３次元位置候補のうちで前回までに演算した３次元位置に基づいて予測した３次元位置に近い値、例えば一番近い値を対象物の３次元位置とする。この３次元位置の予測は、例えばカルマンフィルター（kalman filter）の値を使って実施することができるが、これに限定されない。この予測を用いる方法は、対象物が急激に移動した場合は前回演算した位置に一番近い値を使った場合はずれが大きくなりやすいが、これを防ぐ方法として有効である。算出した３次元位置候補を用いて行う対象物の３次元位置の求め方は上記に限定されず、これ以外の方法で求めてもよい。対象物の角度は上記で求めた３次元位置から対象物の向く方向として求めることができる。
【００３１】
図３は、位置計測用演算装置としてパーソナルコンピュータ（ＰＣ）を用いた場合の一例を示すブロック図である。位置計測用演算装置４は、撮像装置３の２次元撮像素子２で撮像した３つの基本標識の画像を入力する入力部３１と、入力した画像に基づいて対象物の３次元位置候補を１つ乃至４つ算出する算出部３５並びに前記算出した３次元位置候補を用いて前記対象物の３次元位置および角度の少なくとも一方を導出する導出部３６を有する演算部（ＣＰＵ）３２と、導出した前記対象物の３次元位置および角度の少なくとも一方を例えばモニタ等の表示装置に出力する出力部３３とを備える。演算部３２には記憶部３４が接続され、両者間で情報の授受が行われる。記憶部３４は演算部３２で実行されるプログラムやそこで用いられる各種情報を格納するものであり、内部メモリとして構成することができるが、これに限定されず、外部に接続した記憶装置でもよい。
【００３２】
以上の手順は、コンピュータに次のプログラムを実行させることで実施することができる。すなわち、このプログラムは、コンピュータに、２次元撮像素子を有する撮像装置により対象物に配置された位置関係の分かっている３つの基本標識を撮像した画像に基づいて対象物の３次元位置候補を１つ乃至４つ算出する手順、この算出した３次元位置候補を用いて対象物の３次元位置および角度の少なくとも一方を求める手順を実行させるものである。本例では、プログラムを位置計測用演算装置の記憶部に格納した実施形態として説明したが、このプログラムをＣＤＲＯＭ等の記憶媒体に格納して又は通信手段によって提供することも可能である。
【００３３】
図４（ａ）、（ｂ）はそれぞれ位置計測用演算装置で行われる演算の例を示すフロー図である。図４（ａ）は平均値を用いる方法で、まずステップ４１にて、撮像装置で撮像した画像情報を入力する。ステップ４２にて、画像情報に基づいて対象物の３次元位置候補を１つ乃至４つ算出する。ステップ４３にて、算出した３次元位置候補の平均値を算出する。ステップ４４にて、この平均値を対象物の３次元位置とし、これをもとに対象物の角度を求める。図４（ｂ）は記憶部に格納された対象物の３次元位置の初期値を用いる方法で、まずステップ５１にて、撮像装置で撮像した画像情報を入力する。ステップ５２にて、画像情報に基づいて対象物の３次元位置候補を１つ乃至４つ算出する。ステップ５３にて、対象物の３次元位置の初期値を記憶部から読み出す。ステップ５４にて、この初期値と各３次元位置候補を比較する。ステップ５５にて、各３次元位置候補のうち初期値に近い値、例えば一番近い値を対象物の３次元位置とし、これをもとに対象物の角度を求める。
【００３４】
図５（ａ）、（ｂ）はそれぞれ位置計測用演算装置で行われる演算の他の例を示すフロー図である。図５（ａ）は記憶部に格納された前回演算した３次元位置の値を用いる方法で、まずステップ６１にて、撮像装置で撮像した画像情報を入力する。ステップ６２にて、画像情報に基づいて対象物の３次元位置候補を１つ乃至４つ算出する。ステップ６３にて、前回演算した３次元位置の値を記憶部から読み出す。ステップ６４にて、この前回の値と各３次元位置候補を比較する。ステップ６５にて、各３次元位置候補のうち前回の値に近い値、例えば一番近い値を対象物の３次元位置とし、これをもとに対象物の角度を求める。この場合、当初の前回の値は対象物の３次元位置の初期値である。図５（ｂ）は記憶部に格納された前回までに演算した３次元位置の値を用いる方法で、まずステップ７１にて、撮像装置で撮像した画像情報を入力する。ステップ７２にて、画像情報に基づいて対象物の３次元位置候補を１つ乃至４つ算出する。ステップ７３にて、前回までに演算した３次元位置の値を記憶部から読み出す。ステップ７４にて、この前回までに演算した値を用いて３次元位置を予測する。この予測は例えばカルマンフィルターを用いて行うことができるが、これに限定されない。ステップ７５にて、各３次元位置候補のうち予測値に近い値、例えば一番近い値を対象物の３次元位置とし、これをもとに対象物の角度を求める。以上、位置計測用演算装置で行われる演算の例を示したが、演算方法はこれに限定されず、他の方法を用いてもよい。
【符号の説明】
【００３５】
１  対象物
２  ２次元撮像素子
３  撮像装置
４  位置計測用演算装置
５  画像面
６  カメラの光学中心
【図１】
2010210437.tif 000007
【図２】
2010210437.tif 000008
【図３】
2010210437.tif 000009
【図４】
2010210437.tif 000010
【図５】
2010210437.tif 000011