位置計測システム
【課題】 特定のカメラの撮影範囲外にある対象物であっても別のカメラを介することによりその対象物の位置を計測することができる位置計測システムを提供する。
【解決手段】 位置計測システムは、位置関係の分かっている３つ以上の基本標識を有する標識セット１と、標識セット１を撮像するカメラ１０と、カメラ１０に取り付けられる位置関係の分かっている４つ以上の基本標識を有する標識セット２と、標識セット２を撮像するカメラ２０と、カメラ１０で撮像した標識セット１の画像から標識セット１の位置を演算し、カメラ２０で撮像した標識セット２の画像からカメラ１０の位置と角度を演算し、このカメラ１０の位置と角度に基づいて、カメラ１０のカメラ座標１５を基準とした標識セット１の位置をカメラ２０のカメラ座標２５を基準とした位置として演算する演算部３０とを備える。
【解決手段】 位置計測システムは、位置関係の分かっている３つ以上の基本標識を有する標識セット１と、標識セット１を撮像するカメラ１０と、カメラ１０に取り付けられる位置関係の分かっている４つ以上の基本標識を有する標識セット２と、標識セット２を撮像するカメラ２０と、カメラ１０で撮像した標識セット１の画像から標識セット１の位置を演算し、カメラ２０で撮像した標識セット２の画像からカメラ１０の位置と角度を演算し、このカメラ１０の位置と角度に基づいて、カメラ１０のカメラ座標１５を基準とした標識セット１の位置をカメラ２０のカメラ座標２５を基準とした位置として演算する演算部３０とを備える。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、位置計測システムに関する。
【背景技術】
【０００２】
対象物の３次元位置を計測する手段としては、デジタルカメラ２台で発光体を撮影し、カメラ２台間の距離を基線とした三角測量の原理で発光体の座標を算出する方法がよく用いられる。また、対象物の位置計測を高精度に行う方法として光干渉法がよく用いられる。代表的な光干渉法としては、レーザ光源から出た光をビームスプリッターなどを用いて２つに分割し、その一方を対象物に照射し、他方を参照光としてミラーに照射して元の光路に戻し、対象物からの反射光と参照光とを重ね合わせて干渉させる方法がある。
【０００３】
特許文献１には、互いの位置関係が判明している４個以上の光源と、各光源からの光を透過して球面収差により光リング像を形成する光学レンズ系と、光学レンズ系により形成された光リング像を検出する撮像素子と、撮像素子により検出された光リング像の検出情報および各光源の位置関係に基づいて各光源の位置を計測する演算装置とを備えた位置計測システムが記載されている。
【特許文献１】特開２００８−５８２０４号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
このようなカメラを用いる位置計測システムにおいては、特定のカメラの撮影範囲内にある対象物だけでなくその撮影範囲外にある対象物であってもその位置を計測可能であることが望まれる。
本発明の目的は、特定のカメラの撮影範囲外にある対象物であっても別のカメラを介することによりその対象物の位置を計測することができる位置計測システムを提供することにある。
【課題を解決するための手段】
【０００５】
本発明は、上記目的を達成するために、以下の位置計測システムを提供する。
（１）対象物に取り付けられる位置関係の分かっている３つ以上の基本標識を有する第１の標識セットと、前記第１の標識セットを撮像する２次元撮像素子を有する第１のカメラと、前記第１のカメラに取り付けられる位置関係の分かっている４つ以上の基本標識を有する第２の標識セットと、前記第２の標識セットを撮像する２次元撮像素子を有する第２のカメラと、前記第１のカメラで撮像した第１の標識セットの画像から前記第１の標識セットの位置を演算し、前記第２のカメラで撮像した前記第２の標識セットの画像から前記第１のカメラの位置と角度を演算し、前記第１のカメラの位置と角度に基づいて、前記第１のカメラのカメラ座標を基準とした前記第１の標識セットの位置を前記第２のカメラのカメラ座標を基準とした位置として演算する演算部とを備えた位置計測システム。
（２）前記第２の標識セットの４つ以上の基本標識のうち、少なくとも３つ以上の基本標識が同一平面上にあり、少なくとも１つ以上の基本標識が前記平面上から離隔した地点にある上記（１）記載の位置計測システム。
（３）前記第１の標識セットが、前記対象物を特定するための識別標識を有する上記（１）または（２）記載の位置計測システム。
（４）前記対象物と前記第１の標識セットの組が複数設けられる上記（３）記載の位置計測システム。
（５）前記第１のカメラが移動可能とされ、前記第２のカメラが固定される上記（１）〜（４）のいずれかに記載の位置計測システム。
（６）前記第１のカメラと前記第２の標識セットの組が多段に設けられる上記（１）〜（５）のいずれかに記載の位置計測システム。
【発明の効果】
【０００６】
請求項１に係る位置計測システムによれば、特定のカメラの撮影範囲外にある対象物であっても別のカメラを介することによりその対象物の位置を計測することができる。
請求項２に係る位置計測システムによれば、別のカメラに取り付ける標識セットの基本標識をすべて同一平面上に置く必要がなくなり、基本標識の取り付け位置の自由度を増すことができる。
請求項３に係る位置計測システムによれば、位置を計測した対象物の内容が何であるかを知ることができる。
請求項４に係る位置計測システムによれば、位置を計測した複数の対象物の内容がそれぞれ何であるかを知ることができる。
請求項５に係る位置計測システムによれば、対象物が撮影範囲に入るように別のカメラを動かすことで、特定のカメラは動かす必要がなくなる。
請求項６に係る位置計測システムによれば、対象物が複雑な経路の奥まったところにあっても、複数の別のカメラを介して特定のカメラのカメラ座標で位置計測することができる。
【発明を実施するための最良の形態】
【０００７】
図１は、本発明に係る位置計測システムの一実施例を示す図である。本実施例は、図示のように、対象物３に取り付けられる位置関係の分かっている３つの基本標識（基本マーカー）ａ１，ａ２，ａ３を有する標識セット１と、標識セット１を撮像するレンズ１２および２次元撮像素子１４を有するカメラ１０と、カメラ１０に取り付けられる位置関係の分かっている４つの基本標識ａ１，ａ２，ａ３，ａ４を有する標識セット２と、標識セット２を撮像するレンズ２２および２次元撮像素子２４を有するカメラ２０と、カメラ１０で撮像した標識セット１の画像から標識セット１の位置を演算し、カメラ２０で撮像した標識セット２の画像からカメラ１０の位置と角度を演算し、このカメラ１０の位置と角度に基づいて、カメラ１０のカメラ座標１５を基準とした標識セット１の位置をカメラ２０のカメラ座標２５を基準とした位置として演算する演算部３０とを備える。これにより、カメラ座標２５において標識セット１の位置が特定される。すなわち、固定されたカメラ２０の撮影範囲外にある対象物３の位置計測が、移動可能なカメラ１０を介することで可能となる。
【０００８】
本実施例では、標識セット１の基本標識数は３つとしたが３つ以上でもよく、標識セット２の基本標識数は４つとしたが４つ以上でもよい。また、標識セット１，２を構成する各基本標識は、例えば発光ダイオード（ＬＥＤ）等の光源を用いることができる。この場合、光源の発光を制御する発光制御装置１８（配線は図示省略）が対象物３に配置される。また、図示はしないが、上述の光源の代わりに再帰反射板を用い、その再帰反射板を照明するための照明装置を備えるようにしてもよい。しかし基本標識は、ＬＥＤ等の光源や再帰反射板に限定されず、これ以外のものを用いてもよい。また、カメラ１０，２０は、例えばＣＣＤやＣＭＯＳセンサ等を搭載したデジタルカメラが用いられるが、これに限定されない。演算部３０は、図示のように、カメラ１０（２次元撮像素子１４）およびカメラ２０（２次元撮像素子２４）と有線あるいは無線で接続され、カメラ１０および２０で撮像された画像を受信できるように構成される。演算部３０は、例えばパーソナルコンピュータ（ＰＣ）等のコンピュータが用いられるが、これに限定されない。
【０００９】
以下、カメラ１０で撮像した標識セット１の画像から標識セット１の位置を演算し、カメラ２０で撮像した標識セット２の画像からカメラ１０の位置と角度を演算し、このカメラ１０の位置と角度に基づいて、カメラ１０のカメラ座標１５を基準とした標識セット１の位置をカメラ２０のカメラ座標２５を基準とした位置として演算する方法について説明する。
【００１０】
カメラ１０のカメラ座標１５を基準とした標識セット１の位置をカメラ２０のカメラ座標２５を基準とした位置として演算するには、座標系の回転ｒ（角度θ）と平行移動ｔを組み合わせた座標変換行列Ｍｒｔ（式１）を用いる。本実施例のカメラ座標２５とカメラ座標１５との間の座標変換行列Ｍｒｔは、標識セット２の位置と角度から求めることができる。これを用いると、カメラ座標１５を基準とした標識セット１の各基本標識の位置（ｘ１，ｙ１，ｚ１）と、カメラ座標２５を基準とした位置（ｘ２、ｙ２、ｚ２）との関係は、座標変換行列Ｍｒｔを用いて式２のように表される。
【００１１】
【数１】
【００１２】
図２は、３つの基本標識を有する標識セットの３次元位置の演算方法の一例を示す図である。図において、基本標識ａ１，ａ２，ａ３の画像面２７上の画像位置ｃ１，ｃ２，ｃ３とカメラの光学中心２８との関係から、カメラ座標系における基本標識位置の方向ベクトルｄｉ（ｉ＝１，２，３）を算出する。ｄｉは規格化した単位ベクトルとする。
【００１３】
基本標識ａ１，ａ２，ａ３の空間の位置ベクトルをｐ１，ｐ２，ｐ３とすると、これらはｄｉの延長線上に存在するので、その係数をｔ１，ｔ２，ｔ３として、
p1=t1・d1
p2=t2・d2 式３
p3=t3・d3
で表すことができる。
【００１４】
三角形の形状は最初からわかっており、その長さを各々
p1p2=L1
p2p3=L2 式４
p3p1=L3
とすると次式が得られる。式中「^」は累乗を表す。
(t1x1-t2x2)^2+(t1y1-t2y2)^2+(t1z1-t2z2)^2=L1^2
(t2x2-t3x3)^2+(t2y2-t3y3)^2+(t2z2-t3z3)^2=L2^2 式５
(t3x3-t1x1)^2+(t3y3-t1y1)^2+(t3z3-t1z1)^2=L3^2
【００１５】
整理すると
t1^2-2t1t2(x1x2+y1y2+z1z2)+t2^2-L1^2=0
t2^2-2t2t3(x2x3+y2y3+z2z3)+t3^2-L2^2=0 式６
t3^2-2t3t1(x3x1+y3y1+z3z1)+t1^2-L3^2=0
が得られ、次式となる。式中「sqrt」は平方根を表す。
t1=A1・t2±sqrt((A1^2-1)・t2^2+L1^2)
t2=A2・t3±sqrt((A2^2-1)・t3^2+L2^2) 式７
t3=A3・t1±sqrt((A3^2-1)・t1^2+L3^2)
ここで、A1,A2,A3は次式のとおりである。
A1=x1x2+y1y2+z1z2
A2=x2x3+y2y3+z2z3 式８
A3=x3x1+y3y1+z3z1
【００１６】
実数解を持つために、式７の平方根の中が正になる。
t1≦ sqrt(L3^2/(1-A3^2))
t2≦ sqrt(L1^2/(1-A1^2)) 式９
t3≦ sqrt(L2^2/(1-A2^2))
【００１７】
この条件を満たす実数ｔ１、ｔ２、ｔ３を順次、式７に代入し、式７が成立するすべてのｔ１，ｔ２，ｔ３を算出する。次に上記の式３からｐ１，ｐ２，ｐ３を、即ち、基本標識の３次元位置を算出する。
上述の計算方法に従って実際に計測した結果を以下の表に示す。表１は、基本標識ａｉ（ｉ＝１〜３）の座標系を示す。
【００１８】
【表１】
【００１９】
表１中の基本標識ａ１，ａ２，ａ３は、図３に示すように、ａ２を直角の頂点とし、ａ１，ａ３が他の２つの頂点となる直角二等辺三角形を形成する。即ち、標識座標系において、
a1=(50,0,0)
a2=(0,0,0)
a3=(0,50,0)
となる。これは線分ａ１ａ２とａ２ａ３の長さが５０ｍｍであることを意味する。
【００２０】
これをある空間位置に配置し、カメラで撮像した。この時のａｉの画像点の位置を表２に示す。この画像は通常のカメラの画像を左右反転させているので注意を要する。カメラとしては、６４０画素×４８０画素のＣＭＯＳセンサを搭載した１３９４カメラ（ＩＥＥＥ１３９４でＰＣと接続可能なデジタルカメラ）を使用した。
【００２１】
【表２】
【００２２】
この画像点の位置を用いて式３〜式９から解を求めると、基本標識の空間位置として二つの解Ｑ１，Ｑ２がカメラ座標１５の値として得られる。これらを表３と表４に示す。第１の解Ｑ１と第２の解Ｑ２のどちらが正解であるかは不明であるが、これらの値の差は比較的小さい。よって、その平均値を基本標識の空間位置として近似することができる。このように本例では、３つの基本標識の場合、二つの解の平均値を基本標識の位置とする。
【００２３】
【表３】
【００２４】
【表４】
【００２５】
表３，４に示すように、基本標識が３つの場合には、空間位置として解が二つできたが、基本標識が正方形を形成するように配置された場合などでは、その空間位置として、最大４個でてくることがある。しかし、この場合も各々の値の平均値を空間位置として近似することができる。
【００２６】
基本標識が３つの場合には、上述のとおり二つの解ができたが、基本標識が４つある場合には、正解を一つに特定することができる。その方法は、４つの基本標識から３点を選ぶ組み合わせを２つ考える。各々の３点を用いて、前述と同様の計算から２つの解を導出する。その２つの解の内一つは基本標識位置が全く同じ値を示すので、それを正解とする。このように、基本標識が４点以上ある場合には、答えを一つに同定することができるので、４つの基本標識ａ１，ａ２，ａ３，ａ４を有する標識セットの位置と角度を決定することができる。この位置と角度は、カメラ座標２５とカメラ座標１５の間の上記式１に示す座標変換行列Ｍｒｔのrijとti（i,j = 1〜3の整数）を示している。そして上記式２を用いて、カメラ座標１５を基準とした標識セット１の各基本標識の位置（ｘ１，ｙ１，ｚ１）を、カメラ座標２５を基準とした位置（ｘ２、ｙ２、ｚ２）として演算する。これにより、カメラ２０のカメラ座標２５において標識セット１の位置、すなわちそれを取り付けた対象物３の位置を計測することができる。
【００２７】
４つ以上の基本標識がある場合の上記説明はそれらが同一平面上にある場合を想定して記述したが、４つ以上の基本標識のうち、少なくとも３つ以上の基本標識が同一平面上にあり、少なくとも１つ以上の基本標識がその平面上から離隔した地点にある場合も同様に演算することができる。図４は、３つの基本標識ａ１，ａ２，ａ３が同一平面上にあり、１つの基本標識ａ４’がその平面上から離隔した地点にある場合の一例を示す図である。基本標識ａ４’は、例えばカメラ１０に取り付けられた標識セット２の平面上から突出する突出部１９の先端に設けられる。この突出部１９の位置と高さは既知で、例えば３つの基本標識ａ１，ａ２，ａ３の重心に置くことができるが、これに限定されない。この場合も、同一平面上の３つの基本標識ａ１，ａ２，ａ３を用いて、前述と同様の計算を行い２つの解を導出する。そして、この２つの解の基本標識ａ１，ａ２，ａ３の位置から基本標識ａ４’のそれぞれの位置を計算する。この計算値とカメラで撮影した基本標識ａ４’の画像面上の位置の実際値とを比較し、この計算値と実際値とが対応する解を正解とする。このような方法を用いても、上述のような座標変換行列Ｍｒｔを決定することができる。
【００２８】
図５は、本発明に係る位置計測システムの他の実施例を示す図である。本実施例が図１の実施例と異なる点は、識別標識（ＩＤマーカー）付きの標識セット１’をそれぞれ付けた複数の荷物３１の位置とそのＩＤ番号を特定可能とするように構成したところにあり、その他の点は図１の実施例の場合と同様である。
【００２９】
本実施例では、図５に示すように、荷物を特定するための識別標識付きの標識セット１’をそれぞれ付けた複数の荷物３１（すなわち、荷物と標識セット１の複数の組）が棚３２に配置されている。各識別標識は対応する標識セット１’内の３つの基本標識との位置関係からＩＤ番号が付与され、荷物３１の中身と関連付けられてＰＣの記憶装置に保存されている。この識別標識付きの標識セット１を例えば移動可能なカメラ１０で撮影し、その位置とＩＤ番号を算出する。カメラ１０には、実施例１の場合と同様に４つの標識を有する標識セット２が付いており、これを例えば天井に固定したカメラ２０で撮影する。これにより、カメラ２０のカメラ座標２５における各荷物３１の位置とそのＩＤ番号を求める。荷物（標識セット１’）の位置の演算方法は上述したとおりである。以下、ＩＤ番号の求め方について述べる。
【００３０】
図６は、標識セット１内の識別標識に係るＩＤ番号の算出方法の一例を説明するための図である。本実施例では、３つの基本標識と１つの識別標識は同一平面上にあるものとする。標識セット１’の３つの基本標識ａ１，ａ２，ａ３は、図６に示すように、ａ２を直角の頂点とし、ａ１，ａ３が他の２つの頂点となる直角二等辺三角形を形成する。即ち、標識座標系において、
a1=(50,0,0)
a2=(0,0,0)
a3=(0,50,0)
となる。これは線分ａ１ａ２とａ２ａ３の長さが５０ｍｍであることを意味する。
【００３１】
図中のｂ１は識別標識であり、基本標識がなす平面と同一平面にある。ａ２ｂ１とａ１ａ３との交点をｗとし、ｓとｔを以下とする。
a2w:wb1=s:1-s
a1w:wa3=t:1-t
【００３２】
この時、ｂ１は例えばｓ＜０．４７、ｔ＜０．４７、ｓ＜ｔを満足するような位置に配置する。これを満足するｂ１領域を図７に示す。この標識セットを撮像した４つの画像点はアフィン変換で近似できるので、これらの画像点の交点で分けられる直線上の線分比を出す。２本の直線におけるそれぞれ短い方の線分であるａ２ｗとａ１ｗを検出する。この２本の線分比の内のさらに小さい方がｓ、即ちａ２ｗであるので、そこから反時計回りにａ２，ａ１，ｂ１，ａ３を検出することができる。ＩＤ番号付与には、ｓ：ｔの比率を用いる。この比率に対して予めＩＤ番号を対応させＰＣの記憶装置に保存しておくことで、容易にＩＤ番号を取得することができる。
【００３３】
図８は、本発明に係る位置計測システムのさらに他の実施例を示す図である。本実施例が図５の実施例と異なる点は、４つの基本標識を有する標識セット２を取り付けたカメラ１０の他に、それと同様の４つの基本標識を有する標識セット２’を取り付けた別のカメラ１０’を設けたところにあり、その他の点は図５の実施例の場合と同様である。本実施例の場合、カメラ１０で撮像した複数の識別標識付きの標識セット１’の画像から標識セット１’の位置を演算し、そして次段のカメラ１０’で撮像した標識セット２の画像からカメラ１０の位置と角度を演算し、このカメラ１０の位置と角度に基づいて、カメラ１０のカメラ座標１５を基準とした標識セット１’の位置をカメラ１０’のカメラ座標を基準とした位置として演算し、さらにカメラ２０で撮像した標識セット２’の画像からカメラ１０’の位置と角度を演算し、このカメラ１０’の位置と角度に基づいて、カメラ１０’のカメラ座標１５’を基準とした標識セット１’の位置をカメラ２０のカメラ座標２５を基準とした位置として演算する。これにより、カメラ２０のカメラ座標２５で標識セット１’を取り付けた荷物３１の位置が特定される。本実施例では、４つの基本標識を有する標識セットを取り付けたカメラを２つとしたが、上述の２つのカメラ１０，１０’の関係にならって３つ以上の同様なカメラを多段に設けることもできる。
【００３４】
以上の実施例では、標識セット１の基本標識（基本マーカー）数は３つとしたが、これに限定されず、４つ以上でもよく、また識別標識（ＩＤマーカー）数は１つとしたが、２つ以上でもよい。また、標識セット２の基本標識（基本マーカー）数は４つとしたが、これに限定されず、５つ以上でもよい。
【図面の簡単な説明】
【００３５】
【図１】本発明に係る位置計測システムの一実施例を示す図である。
【図２】３つの基本標識を有する標識セットの３次元位置の演算方法の一例を示す図である。
【図３】基本標識ａ１，ａ２，ａ３の配置関係の一例を示す図である。
【図４】３つの基本標識ａ１，ａ２，ａ３が同一平面上にあり、１つの基本標識ａ４’がその平面上から離隔した地点にある場合の一例を示す図である。
【図５】本発明に係る位置計測システムの他の実施例を示す図である。
【図６】標識セット１内の識別標識に係るＩＤ番号の算出方法の一例を説明するための図である。
【図７】標識セット１内の識別標識の配置領域の一例を示す図である。
【図８】本発明に係る位置計測システムのさらに他の実施例を示す図である。
【符号の説明】
【００３６】
１、２ 識別セット
３ 対象物
１０、２０ カメラ
１２、２２ レンズ
１４、２４ ２次元撮像素子
１５、２５ カメラ座標
１８ 発光制御装置
１９ 突起部
２７ 画像面
２８ カメラの光学中心
３０ 演算部（ＰＣ）
３１ 荷物
３２ 棚
【技術分野】
【０００１】
本発明は、位置計測システムに関する。
【背景技術】
【０００２】
対象物の３次元位置を計測する手段としては、デジタルカメラ２台で発光体を撮影し、カメラ２台間の距離を基線とした三角測量の原理で発光体の座標を算出する方法がよく用いられる。また、対象物の位置計測を高精度に行う方法として光干渉法がよく用いられる。代表的な光干渉法としては、レーザ光源から出た光をビームスプリッターなどを用いて２つに分割し、その一方を対象物に照射し、他方を参照光としてミラーに照射して元の光路に戻し、対象物からの反射光と参照光とを重ね合わせて干渉させる方法がある。
【０００３】
特許文献１には、互いの位置関係が判明している４個以上の光源と、各光源からの光を透過して球面収差により光リング像を形成する光学レンズ系と、光学レンズ系により形成された光リング像を検出する撮像素子と、撮像素子により検出された光リング像の検出情報および各光源の位置関係に基づいて各光源の位置を計測する演算装置とを備えた位置計測システムが記載されている。
【特許文献１】特開２００８−５８２０４号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
このようなカメラを用いる位置計測システムにおいては、特定のカメラの撮影範囲内にある対象物だけでなくその撮影範囲外にある対象物であってもその位置を計測可能であることが望まれる。
本発明の目的は、特定のカメラの撮影範囲外にある対象物であっても別のカメラを介することによりその対象物の位置を計測することができる位置計測システムを提供することにある。
【課題を解決するための手段】
【０００５】
本発明は、上記目的を達成するために、以下の位置計測システムを提供する。
（１）対象物に取り付けられる位置関係の分かっている３つ以上の基本標識を有する第１の標識セットと、前記第１の標識セットを撮像する２次元撮像素子を有する第１のカメラと、前記第１のカメラに取り付けられる位置関係の分かっている４つ以上の基本標識を有する第２の標識セットと、前記第２の標識セットを撮像する２次元撮像素子を有する第２のカメラと、前記第１のカメラで撮像した第１の標識セットの画像から前記第１の標識セットの位置を演算し、前記第２のカメラで撮像した前記第２の標識セットの画像から前記第１のカメラの位置と角度を演算し、前記第１のカメラの位置と角度に基づいて、前記第１のカメラのカメラ座標を基準とした前記第１の標識セットの位置を前記第２のカメラのカメラ座標を基準とした位置として演算する演算部とを備えた位置計測システム。
（２）前記第２の標識セットの４つ以上の基本標識のうち、少なくとも３つ以上の基本標識が同一平面上にあり、少なくとも１つ以上の基本標識が前記平面上から離隔した地点にある上記（１）記載の位置計測システム。
（３）前記第１の標識セットが、前記対象物を特定するための識別標識を有する上記（１）または（２）記載の位置計測システム。
（４）前記対象物と前記第１の標識セットの組が複数設けられる上記（３）記載の位置計測システム。
（５）前記第１のカメラが移動可能とされ、前記第２のカメラが固定される上記（１）〜（４）のいずれかに記載の位置計測システム。
（６）前記第１のカメラと前記第２の標識セットの組が多段に設けられる上記（１）〜（５）のいずれかに記載の位置計測システム。
【発明の効果】
【０００６】
請求項１に係る位置計測システムによれば、特定のカメラの撮影範囲外にある対象物であっても別のカメラを介することによりその対象物の位置を計測することができる。
請求項２に係る位置計測システムによれば、別のカメラに取り付ける標識セットの基本標識をすべて同一平面上に置く必要がなくなり、基本標識の取り付け位置の自由度を増すことができる。
請求項３に係る位置計測システムによれば、位置を計測した対象物の内容が何であるかを知ることができる。
請求項４に係る位置計測システムによれば、位置を計測した複数の対象物の内容がそれぞれ何であるかを知ることができる。
請求項５に係る位置計測システムによれば、対象物が撮影範囲に入るように別のカメラを動かすことで、特定のカメラは動かす必要がなくなる。
請求項６に係る位置計測システムによれば、対象物が複雑な経路の奥まったところにあっても、複数の別のカメラを介して特定のカメラのカメラ座標で位置計測することができる。
【発明を実施するための最良の形態】
【０００７】
図１は、本発明に係る位置計測システムの一実施例を示す図である。本実施例は、図示のように、対象物３に取り付けられる位置関係の分かっている３つの基本標識（基本マーカー）ａ１，ａ２，ａ３を有する標識セット１と、標識セット１を撮像するレンズ１２および２次元撮像素子１４を有するカメラ１０と、カメラ１０に取り付けられる位置関係の分かっている４つの基本標識ａ１，ａ２，ａ３，ａ４を有する標識セット２と、標識セット２を撮像するレンズ２２および２次元撮像素子２４を有するカメラ２０と、カメラ１０で撮像した標識セット１の画像から標識セット１の位置を演算し、カメラ２０で撮像した標識セット２の画像からカメラ１０の位置と角度を演算し、このカメラ１０の位置と角度に基づいて、カメラ１０のカメラ座標１５を基準とした標識セット１の位置をカメラ２０のカメラ座標２５を基準とした位置として演算する演算部３０とを備える。これにより、カメラ座標２５において標識セット１の位置が特定される。すなわち、固定されたカメラ２０の撮影範囲外にある対象物３の位置計測が、移動可能なカメラ１０を介することで可能となる。
【０００８】
本実施例では、標識セット１の基本標識数は３つとしたが３つ以上でもよく、標識セット２の基本標識数は４つとしたが４つ以上でもよい。また、標識セット１，２を構成する各基本標識は、例えば発光ダイオード（ＬＥＤ）等の光源を用いることができる。この場合、光源の発光を制御する発光制御装置１８（配線は図示省略）が対象物３に配置される。また、図示はしないが、上述の光源の代わりに再帰反射板を用い、その再帰反射板を照明するための照明装置を備えるようにしてもよい。しかし基本標識は、ＬＥＤ等の光源や再帰反射板に限定されず、これ以外のものを用いてもよい。また、カメラ１０，２０は、例えばＣＣＤやＣＭＯＳセンサ等を搭載したデジタルカメラが用いられるが、これに限定されない。演算部３０は、図示のように、カメラ１０（２次元撮像素子１４）およびカメラ２０（２次元撮像素子２４）と有線あるいは無線で接続され、カメラ１０および２０で撮像された画像を受信できるように構成される。演算部３０は、例えばパーソナルコンピュータ（ＰＣ）等のコンピュータが用いられるが、これに限定されない。
【０００９】
以下、カメラ１０で撮像した標識セット１の画像から標識セット１の位置を演算し、カメラ２０で撮像した標識セット２の画像からカメラ１０の位置と角度を演算し、このカメラ１０の位置と角度に基づいて、カメラ１０のカメラ座標１５を基準とした標識セット１の位置をカメラ２０のカメラ座標２５を基準とした位置として演算する方法について説明する。
【００１０】
カメラ１０のカメラ座標１５を基準とした標識セット１の位置をカメラ２０のカメラ座標２５を基準とした位置として演算するには、座標系の回転ｒ（角度θ）と平行移動ｔを組み合わせた座標変換行列Ｍｒｔ（式１）を用いる。本実施例のカメラ座標２５とカメラ座標１５との間の座標変換行列Ｍｒｔは、標識セット２の位置と角度から求めることができる。これを用いると、カメラ座標１５を基準とした標識セット１の各基本標識の位置（ｘ１，ｙ１，ｚ１）と、カメラ座標２５を基準とした位置（ｘ２、ｙ２、ｚ２）との関係は、座標変換行列Ｍｒｔを用いて式２のように表される。
【００１１】
【数１】
【００１２】
図２は、３つの基本標識を有する標識セットの３次元位置の演算方法の一例を示す図である。図において、基本標識ａ１，ａ２，ａ３の画像面２７上の画像位置ｃ１，ｃ２，ｃ３とカメラの光学中心２８との関係から、カメラ座標系における基本標識位置の方向ベクトルｄｉ（ｉ＝１，２，３）を算出する。ｄｉは規格化した単位ベクトルとする。
【００１３】
基本標識ａ１，ａ２，ａ３の空間の位置ベクトルをｐ１，ｐ２，ｐ３とすると、これらはｄｉの延長線上に存在するので、その係数をｔ１，ｔ２，ｔ３として、
p1=t1・d1
p2=t2・d2 式３
p3=t3・d3
で表すことができる。
【００１４】
三角形の形状は最初からわかっており、その長さを各々
p1p2=L1
p2p3=L2 式４
p3p1=L3
とすると次式が得られる。式中「^」は累乗を表す。
(t1x1-t2x2)^2+(t1y1-t2y2)^2+(t1z1-t2z2)^2=L1^2
(t2x2-t3x3)^2+(t2y2-t3y3)^2+(t2z2-t3z3)^2=L2^2 式５
(t3x3-t1x1)^2+(t3y3-t1y1)^2+(t3z3-t1z1)^2=L3^2
【００１５】
整理すると
t1^2-2t1t2(x1x2+y1y2+z1z2)+t2^2-L1^2=0
t2^2-2t2t3(x2x3+y2y3+z2z3)+t3^2-L2^2=0 式６
t3^2-2t3t1(x3x1+y3y1+z3z1)+t1^2-L3^2=0
が得られ、次式となる。式中「sqrt」は平方根を表す。
t1=A1・t2±sqrt((A1^2-1)・t2^2+L1^2)
t2=A2・t3±sqrt((A2^2-1)・t3^2+L2^2) 式７
t3=A3・t1±sqrt((A3^2-1)・t1^2+L3^2)
ここで、A1,A2,A3は次式のとおりである。
A1=x1x2+y1y2+z1z2
A2=x2x3+y2y3+z2z3 式８
A3=x3x1+y3y1+z3z1
【００１６】
実数解を持つために、式７の平方根の中が正になる。
t1≦ sqrt(L3^2/(1-A3^2))
t2≦ sqrt(L1^2/(1-A1^2)) 式９
t3≦ sqrt(L2^2/(1-A2^2))
【００１７】
この条件を満たす実数ｔ１、ｔ２、ｔ３を順次、式７に代入し、式７が成立するすべてのｔ１，ｔ２，ｔ３を算出する。次に上記の式３からｐ１，ｐ２，ｐ３を、即ち、基本標識の３次元位置を算出する。
上述の計算方法に従って実際に計測した結果を以下の表に示す。表１は、基本標識ａｉ（ｉ＝１〜３）の座標系を示す。
【００１８】
【表１】
【００１９】
表１中の基本標識ａ１，ａ２，ａ３は、図３に示すように、ａ２を直角の頂点とし、ａ１，ａ３が他の２つの頂点となる直角二等辺三角形を形成する。即ち、標識座標系において、
a1=(50,0,0)
a2=(0,0,0)
a3=(0,50,0)
となる。これは線分ａ１ａ２とａ２ａ３の長さが５０ｍｍであることを意味する。
【００２０】
これをある空間位置に配置し、カメラで撮像した。この時のａｉの画像点の位置を表２に示す。この画像は通常のカメラの画像を左右反転させているので注意を要する。カメラとしては、６４０画素×４８０画素のＣＭＯＳセンサを搭載した１３９４カメラ（ＩＥＥＥ１３９４でＰＣと接続可能なデジタルカメラ）を使用した。
【００２１】
【表２】
【００２２】
この画像点の位置を用いて式３〜式９から解を求めると、基本標識の空間位置として二つの解Ｑ１，Ｑ２がカメラ座標１５の値として得られる。これらを表３と表４に示す。第１の解Ｑ１と第２の解Ｑ２のどちらが正解であるかは不明であるが、これらの値の差は比較的小さい。よって、その平均値を基本標識の空間位置として近似することができる。このように本例では、３つの基本標識の場合、二つの解の平均値を基本標識の位置とする。
【００２３】
【表３】
【００２４】
【表４】
【００２５】
表３，４に示すように、基本標識が３つの場合には、空間位置として解が二つできたが、基本標識が正方形を形成するように配置された場合などでは、その空間位置として、最大４個でてくることがある。しかし、この場合も各々の値の平均値を空間位置として近似することができる。
【００２６】
基本標識が３つの場合には、上述のとおり二つの解ができたが、基本標識が４つある場合には、正解を一つに特定することができる。その方法は、４つの基本標識から３点を選ぶ組み合わせを２つ考える。各々の３点を用いて、前述と同様の計算から２つの解を導出する。その２つの解の内一つは基本標識位置が全く同じ値を示すので、それを正解とする。このように、基本標識が４点以上ある場合には、答えを一つに同定することができるので、４つの基本標識ａ１，ａ２，ａ３，ａ４を有する標識セットの位置と角度を決定することができる。この位置と角度は、カメラ座標２５とカメラ座標１５の間の上記式１に示す座標変換行列Ｍｒｔのrijとti（i,j = 1〜3の整数）を示している。そして上記式２を用いて、カメラ座標１５を基準とした標識セット１の各基本標識の位置（ｘ１，ｙ１，ｚ１）を、カメラ座標２５を基準とした位置（ｘ２、ｙ２、ｚ２）として演算する。これにより、カメラ２０のカメラ座標２５において標識セット１の位置、すなわちそれを取り付けた対象物３の位置を計測することができる。
【００２７】
４つ以上の基本標識がある場合の上記説明はそれらが同一平面上にある場合を想定して記述したが、４つ以上の基本標識のうち、少なくとも３つ以上の基本標識が同一平面上にあり、少なくとも１つ以上の基本標識がその平面上から離隔した地点にある場合も同様に演算することができる。図４は、３つの基本標識ａ１，ａ２，ａ３が同一平面上にあり、１つの基本標識ａ４’がその平面上から離隔した地点にある場合の一例を示す図である。基本標識ａ４’は、例えばカメラ１０に取り付けられた標識セット２の平面上から突出する突出部１９の先端に設けられる。この突出部１９の位置と高さは既知で、例えば３つの基本標識ａ１，ａ２，ａ３の重心に置くことができるが、これに限定されない。この場合も、同一平面上の３つの基本標識ａ１，ａ２，ａ３を用いて、前述と同様の計算を行い２つの解を導出する。そして、この２つの解の基本標識ａ１，ａ２，ａ３の位置から基本標識ａ４’のそれぞれの位置を計算する。この計算値とカメラで撮影した基本標識ａ４’の画像面上の位置の実際値とを比較し、この計算値と実際値とが対応する解を正解とする。このような方法を用いても、上述のような座標変換行列Ｍｒｔを決定することができる。
【００２８】
図５は、本発明に係る位置計測システムの他の実施例を示す図である。本実施例が図１の実施例と異なる点は、識別標識（ＩＤマーカー）付きの標識セット１’をそれぞれ付けた複数の荷物３１の位置とそのＩＤ番号を特定可能とするように構成したところにあり、その他の点は図１の実施例の場合と同様である。
【００２９】
本実施例では、図５に示すように、荷物を特定するための識別標識付きの標識セット１’をそれぞれ付けた複数の荷物３１（すなわち、荷物と標識セット１の複数の組）が棚３２に配置されている。各識別標識は対応する標識セット１’内の３つの基本標識との位置関係からＩＤ番号が付与され、荷物３１の中身と関連付けられてＰＣの記憶装置に保存されている。この識別標識付きの標識セット１を例えば移動可能なカメラ１０で撮影し、その位置とＩＤ番号を算出する。カメラ１０には、実施例１の場合と同様に４つの標識を有する標識セット２が付いており、これを例えば天井に固定したカメラ２０で撮影する。これにより、カメラ２０のカメラ座標２５における各荷物３１の位置とそのＩＤ番号を求める。荷物（標識セット１’）の位置の演算方法は上述したとおりである。以下、ＩＤ番号の求め方について述べる。
【００３０】
図６は、標識セット１内の識別標識に係るＩＤ番号の算出方法の一例を説明するための図である。本実施例では、３つの基本標識と１つの識別標識は同一平面上にあるものとする。標識セット１’の３つの基本標識ａ１，ａ２，ａ３は、図６に示すように、ａ２を直角の頂点とし、ａ１，ａ３が他の２つの頂点となる直角二等辺三角形を形成する。即ち、標識座標系において、
a1=(50,0,0)
a2=(0,0,0)
a3=(0,50,0)
となる。これは線分ａ１ａ２とａ２ａ３の長さが５０ｍｍであることを意味する。
【００３１】
図中のｂ１は識別標識であり、基本標識がなす平面と同一平面にある。ａ２ｂ１とａ１ａ３との交点をｗとし、ｓとｔを以下とする。
a2w:wb1=s:1-s
a1w:wa3=t:1-t
【００３２】
この時、ｂ１は例えばｓ＜０．４７、ｔ＜０．４７、ｓ＜ｔを満足するような位置に配置する。これを満足するｂ１領域を図７に示す。この標識セットを撮像した４つの画像点はアフィン変換で近似できるので、これらの画像点の交点で分けられる直線上の線分比を出す。２本の直線におけるそれぞれ短い方の線分であるａ２ｗとａ１ｗを検出する。この２本の線分比の内のさらに小さい方がｓ、即ちａ２ｗであるので、そこから反時計回りにａ２，ａ１，ｂ１，ａ３を検出することができる。ＩＤ番号付与には、ｓ：ｔの比率を用いる。この比率に対して予めＩＤ番号を対応させＰＣの記憶装置に保存しておくことで、容易にＩＤ番号を取得することができる。
【００３３】
図８は、本発明に係る位置計測システムのさらに他の実施例を示す図である。本実施例が図５の実施例と異なる点は、４つの基本標識を有する標識セット２を取り付けたカメラ１０の他に、それと同様の４つの基本標識を有する標識セット２’を取り付けた別のカメラ１０’を設けたところにあり、その他の点は図５の実施例の場合と同様である。本実施例の場合、カメラ１０で撮像した複数の識別標識付きの標識セット１’の画像から標識セット１’の位置を演算し、そして次段のカメラ１０’で撮像した標識セット２の画像からカメラ１０の位置と角度を演算し、このカメラ１０の位置と角度に基づいて、カメラ１０のカメラ座標１５を基準とした標識セット１’の位置をカメラ１０’のカメラ座標を基準とした位置として演算し、さらにカメラ２０で撮像した標識セット２’の画像からカメラ１０’の位置と角度を演算し、このカメラ１０’の位置と角度に基づいて、カメラ１０’のカメラ座標１５’を基準とした標識セット１’の位置をカメラ２０のカメラ座標２５を基準とした位置として演算する。これにより、カメラ２０のカメラ座標２５で標識セット１’を取り付けた荷物３１の位置が特定される。本実施例では、４つの基本標識を有する標識セットを取り付けたカメラを２つとしたが、上述の２つのカメラ１０，１０’の関係にならって３つ以上の同様なカメラを多段に設けることもできる。
【００３４】
以上の実施例では、標識セット１の基本標識（基本マーカー）数は３つとしたが、これに限定されず、４つ以上でもよく、また識別標識（ＩＤマーカー）数は１つとしたが、２つ以上でもよい。また、標識セット２の基本標識（基本マーカー）数は４つとしたが、これに限定されず、５つ以上でもよい。
【図面の簡単な説明】
【００３５】
【図１】本発明に係る位置計測システムの一実施例を示す図である。
【図２】３つの基本標識を有する標識セットの３次元位置の演算方法の一例を示す図である。
【図３】基本標識ａ１，ａ２，ａ３の配置関係の一例を示す図である。
【図４】３つの基本標識ａ１，ａ２，ａ３が同一平面上にあり、１つの基本標識ａ４’がその平面上から離隔した地点にある場合の一例を示す図である。
【図５】本発明に係る位置計測システムの他の実施例を示す図である。
【図６】標識セット１内の識別標識に係るＩＤ番号の算出方法の一例を説明するための図である。
【図７】標識セット１内の識別標識の配置領域の一例を示す図である。
【図８】本発明に係る位置計測システムのさらに他の実施例を示す図である。
【符号の説明】
【００３６】
１、２ 識別セット
３ 対象物
１０、２０ カメラ
１２、２２ レンズ
１４、２４ ２次元撮像素子
１５、２５ カメラ座標
１８ 発光制御装置
１９ 突起部
２７ 画像面
２８ カメラの光学中心
３０ 演算部（ＰＣ）
３１ 荷物
３２ 棚
【特許請求の範囲】
【請求項１】
対象物に取り付けられる位置関係の分かっている３つ以上の基本標識を有する第１の標識セットと、前記第１の標識セットを撮像する２次元撮像素子を有する第１のカメラと、前記第１のカメラに取り付けられる位置関係の分かっている４つ以上の基本標識を有する第２の標識セットと、前記第２の標識セットを撮像する２次元撮像素子を有する第２のカメラと、前記第１のカメラで撮像した第１の標識セットの画像から前記第１の標識セットの位置を演算し、前記第２のカメラで撮像した前記第２の標識セットの画像から前記第１のカメラの位置と角度を演算し、前記第１のカメラの位置と角度に基づいて、前記第１のカメラのカメラ座標を基準とした前記第１の標識セットの位置を前記第２のカメラのカメラ座標を基準とした位置として演算する演算部とを備えた位置計測システム。
【請求項２】
前記第２の標識セットの４つ以上の基本標識のうち、少なくとも３つ以上の基本標識が同一平面上にあり、少なくとも１つ以上の基本標識が前記平面上から離隔した地点にある請求項１記載の位置計測システム。
【請求項３】
前記第１の標識セットが、前記対象物を特定するための識別標識を有する請求項１または２記載の位置計測システム。
【請求項４】
前記対象物と前記第１の標識セットの組が複数設けられる請求項３記載の位置計測システム。
【請求項５】
前記第１のカメラが移動可能とされ、前記第２のカメラが固定される請求項１〜４のいずれかに記載の位置計測システム。
【請求項６】
前記第１のカメラと前記第２の標識セットの組が多段に設けられる請求項１〜５のいずれかに記載の位置計測システム。
【請求項１】
対象物に取り付けられる位置関係の分かっている３つ以上の基本標識を有する第１の標識セットと、前記第１の標識セットを撮像する２次元撮像素子を有する第１のカメラと、前記第１のカメラに取り付けられる位置関係の分かっている４つ以上の基本標識を有する第２の標識セットと、前記第２の標識セットを撮像する２次元撮像素子を有する第２のカメラと、前記第１のカメラで撮像した第１の標識セットの画像から前記第１の標識セットの位置を演算し、前記第２のカメラで撮像した前記第２の標識セットの画像から前記第１のカメラの位置と角度を演算し、前記第１のカメラの位置と角度に基づいて、前記第１のカメラのカメラ座標を基準とした前記第１の標識セットの位置を前記第２のカメラのカメラ座標を基準とした位置として演算する演算部とを備えた位置計測システム。
【請求項２】
前記第２の標識セットの４つ以上の基本標識のうち、少なくとも３つ以上の基本標識が同一平面上にあり、少なくとも１つ以上の基本標識が前記平面上から離隔した地点にある請求項１記載の位置計測システム。
【請求項３】
前記第１の標識セットが、前記対象物を特定するための識別標識を有する請求項１または２記載の位置計測システム。
【請求項４】
前記対象物と前記第１の標識セットの組が複数設けられる請求項３記載の位置計測システム。
【請求項５】
前記第１のカメラが移動可能とされ、前記第２のカメラが固定される請求項１〜４のいずれかに記載の位置計測システム。
【請求項６】
前記第１のカメラと前記第２の標識セットの組が多段に設けられる請求項１〜５のいずれかに記載の位置計測システム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【公開番号】特開２０１０−７１６７７（Ｐ２０１０−７１６７７Ａ）
【公開日】平成２２年４月２日（２０１０．４．２）
【国際特許分類】
【出願番号】特願２００８−２３６４９８（Ｐ２００８−２３６４９８）
【出願日】平成２０年９月１６日（２００８．９．１６）
【出願人】（０００００５４９６）富士ゼロックス株式会社 (21,908)
【Ｆターム（参考）】
【公開日】平成２２年４月２日（２０１０．４．２）
【国際特許分類】
【出願日】平成２０年９月１６日（２００８．９．１６）
【出願人】（０００００５４９６）富士ゼロックス株式会社 (21,908)
【Ｆターム（参考）】
[ Back to top ]