JP5217898B2 - 位置計測装置およびプログラム - Google Patents

Description

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

対象物の位置を計測する手段として、２台のカメラで対象物を撮影し、カメラ同士の間の距離を基線とした三角測量によって対象物の位置座標を算出する方法がよく用いられる。

特許文献１には、２点の座標から幾何的に一意に決定される座標集合上の画像のパターン分布を獲得し、それを距離について正規化した明度分布ベクトル獲得部と、予め認識対象の画像に対して明度分布ベクトル獲得部を適用して得られた複数の明度分布ベクトルから照合情報を作成する照合情報作成部と、入力した画像から明度分布ベクトル獲得部で得られた明度分布ベクトルと、照合情報作成部で作成した照合情報を用いて明度分布ベクトルの照合を行って対象物を認識する認識部を備えた物体認識装置が開示されている。

また、特許文献２には、物体に固有の、複数の特徴点より構成される物体識別情報コードと物体識別コードに対応する第１の幾何学的不変量を生成し、画像入力装置によって、複数の視点から、物体識別情報コードが付加された物体の複数の画像を取得し、複数の画像の中から、独立な複数の画像を発生し、独立な複数の画像のそれぞれに対して、物体識別情報コードを構成する特徴点位置を抽出し、抽出された特徴点位置から、第２の幾何学的不変量を計算し、計算された第２の幾何学的不変量に最も近い、第２の幾何学的不変量に対応する物体識別情報コードを検出する物体識別情報認識システムが開示されている。

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

対象物の位置を計測する装置においては、対象物の三次元位置や三次元角度を高精度で計測することが望まれる。
本発明の目的は、対象物に備えられた点光源を撮影し、撮影された画像における点光源の位置を高精度に計測することで、対象物の位置計測を高精度に行うことができる位置計測装置等を提供することにある。

請求項１に係る発明は、対象物に設けられた３点以上の第１基準点と１点以上の第２基準点からなる点光源の像を撮影する撮影部と、前記撮影部により撮影された前記第１基準点の像と前記第２基準点の像とを識別する識別部と、前記第１基準点の位置関係を取得する位置関係取得部と、前記位置関係取得部により取得された前記第１基準点の位置関係と前記識別部により識別された当該第１基準点の像の位置関係から前記第２基準点の前記対象物における位置を求め、当該第１基準点および当該第２基準点の位置関係より当該対象物の位置を求める演算部と、を備え、前記演算部は、前記識別部により識別された前記第１基準点の像の組を複数取得し、前記組毎に前記第１基準点の像の位置関係から前記第２基準点の前記対象物における位置候補を複数算出し、算出された前記位置候補の中から、前記第２基準点の位置の正解を求めることを特徴とする位置計測装置である。

請求項２に係る発明は、前記正解は、算出された前記位置候補の中から前記組毎の変動が最も少ないものを選択することで求めることを特徴とする請求項１に記載の位置計測装置である。
請求項３に係る発明は、前記第１基準点の像の組は、前記撮影部が撮影するフレーム毎に取得することを特徴とする請求項１または２に記載の位置計測装置である。
請求項４に係る発明は、前記第１基準点は、３点の発光点からなり、前記第２基準点は、１点の発光点からなることを特徴とする請求項１乃至３の何れか１項に記載の位置計測装置である。

請求項５に係る発明は、コンピュータに、撮影部により撮影された第１基準点の像と第２基準点の像とを識別する識別機能と、前記第１基準点の位置関係を取得する位置関係取得機能と、前記第１基準点の位置関係と前記第１基準点の像の位置関係から前記第２基準点の対象物における位置を求め、当該第１基準点および当該第２基準点の位置関係より当該対象物の位置を求める演算機能と、を実現させ、前記演算機能は、前記第１基準点の像の位置関係から前記第２基準点の前記対象物における位置候補を複数算出し、算出された前記位置候補の中から、前記第２基準点の位置の正解を求めることを特徴とするプログラムである。

請求項６に係る発明は、前記正解は、算出された前記位置候補の中から前記第２基準点の位置に関する参照値との差が最も少ないものを選択することで求めることを特徴とする請求項５に記載のプログラムである。
請求項７に係る発明は、前記識別機能は、前記第１基準点および前記第２基準点の位置関係により算出されるアフィン不変量を用いて、前記第１基準点の像と前記第２基準点の像とを識別することを特徴とする請求項５または６に記載のプログラムである。

請求項１の発明によれば、対象物の位置計測を高精度に行うことができる位置計測装置が実現できる。また第２基準点の対象物における位置が不明であっても、第１基準点の対象物における位置関係と第１基準点の像の位置関係から第２基準点の対象物における位置を求めることができる。
請求項２の発明によれば、本構成を採用しない場合に比較して、第２基準点の対象物における位置を、より高精度に求めることができる。
請求項３の発明によれば、本構成を採用しない場合に比較して、第１基準点の像の組を複数取得することが、より簡単にできる。
請求項４の発明によれば、本構成を採用しない場合に比較して、対象物の位置計測を行うことができる位置計測装置が、より簡易な構成で実現できる。

請求項５の発明によれば、本構成を採用しない場合に比較して、対象物の位置計測を高精度に行うことができる機能を、コンピュータにより実現できる。また本構成を採用しない場合に比較して、第１基準点の対象物における位置関係と第１基準点の像の位置関係から第２基準点の対象物における位置を、より高速に求めることができる。
請求項６の発明によれば、本構成を採用しない場合に比較して、第２基準点の対象物における位置を、より高精度に求めることができる。
請求項７の発明によれば、本構成を採用しない場合に比較して、撮影部により撮影された第１基準点の像と第２基準点の像との識別を、より簡単に行うことができる。

以下、添付図面を参照して、本発明の実施形態について詳細に説明する。
＜システム構成＞
図１は、本実施形態が適用される位置計測装置、および位置計測を行う対象物を含んだ位置計測システムを説明した図である。
図１に示した位置計測システムでは、位置計測対象である対象物１００と、対象物１００の三次元位置および三軸角度を計算する位置計測装置３００とを有する。そして、位置計測装置３００は、対象物１００に設けられた３点の第１基準点と１点の第２基準点からなる点光源の像を撮影する撮影部の一例としてのカメラ３０と、カメラ３０により撮影された画像に基づいて対象物１００の三次元位置および三軸角度を演算する演算装置４０とを備える。

図２（Ａ）〜（Ｂ）は、本実施の形態が適用される対象物１００の構成を示す図である。
図２（Ａ）に示すように、対象物１００には、対象物１００の三次元位置および三軸角度を計算するために用いられる第１基準点１１〜１３と第２基準点１４とが設けられている。ここで、第１基準点１１〜１３は、対象物１００上での互いの位置関係が予め定義されているが、第２基準点１４の対象物１００上での位置は不明である。そして、第１基準点１１〜１３および第２基準点１４は、ＬＥＤ（Light Emitting Diode）により構成される発光点で構成される。また対象物１００は、このＬＥＤの発光を制御する発光制御装置１６を内部に備える。

第１基準点１１〜１３および第２基準点１４は、対象物１００の表面に設けられる。
第１基準点１１〜１３により形成される三角形の形状は、例えば直角二等辺三角形とすることができる。図２に示す例では、第１基準点１１と第１基準点１２の間の長さ（即ち、ａ１ａ２の長さ）が５０ｍｍであり、第１基準点１２と第１基準点１３の間の長さ（即ち、ａ２ａ３の長さ）が同様に５０ｍｍである。そして、この間の角度が直角である直角二等辺三角形を形成するように第１基準点１１〜１３を配置している。
また、第２基準点１４は、後に詳述する第１基準点１１〜１３と第２基準点１４との識別の必要性から、配置に対し一定の条件を満たす必要がある。
例えば、第１基準点１１〜１３および第２基準点１４で形成される四角形（ａ１ａ２ａ３ｂ１）の対角線の交点をｐ（図示せず）とし、線分の比ａ１ｐ／ａ１ａ３＝ｔ、ａ２ｐ／ａ２ｂ１＝ｓを考える。このとき、ａ１ｐ＝ｔ、ａ２ｐ＝ｓ、ａ３ｐ＝１−ｔ、ｂ１ｐ＝１−ｓとなる。そして、第２基準点１４を、ｓ＜０．４７かつｔ＜０．４７を満たす位置に配置する。このとき線分の比の値であるｓ、ｔ、１−ｓ、１−ｔの値は、アフィン変換の前後で保存されるアフィン不変量である。

図３は、対象物１００上に第２基準点１４を配置可能な領域を示した例である。
図３に示した対象物１００は、横７０ｍｍ、縦６５ｍｍのカード状であり、説明の便宜上、対象物１００上のｘ方向位置とｙ方向位置を示す目盛りを付加している。ここで、第１基準点１１は、（ｘ，ｙ）＝（５０，０）の位置に配置し、この位置を基準として、第１基準点１２を、（０，０）の位置に、第１基準点１３を、（０，５０）の位置に設置しており、第１基準点１１〜１３により直角二等辺三角形が形成される。また第２基準点１４は、上述した条件、即ち、ｓ＜０．４７かつｔ＜０．４７を満たす条件で配置される。そして、図３における領域Ａがこの条件を満たす領域である。

なお、図１および図２には、対象物１００として第１基準点１１〜１３、第２基準点１４が設けられた直方体状のものが記載されている。しかし、本実施形態では、カメラ３０により撮影された第１基準点１１〜１３および第２基準点１４の点光源の画像に基づいて対象物１００の三次元位置および三軸角度を計算するものであるから、第１基準点１１〜１３および第２基準点１４が上記の位置関係をなすのであればよく、対象物１００自体の形状は何ら限定されない。

図４は、カメラ３０の構成例を示す図である。
カメラ３０は、第１基準点１１〜１３（図２参照）から出射された光を収束する光学系３１と、光学系３１により収束された光を検出する撮像素子の一例であるイメージセンサ３２とを備える。

光学系３１は、単一のレンズまたは複数のレンズを組み合わせて構成される。例えば、２つの半球レンズを使用し、その球面側を向かい合わせに組み合わせたツインレンズが用いられる。

イメージセンサ３２は、ＣＣＤ（Charge Coupled Device）やＣＭＯＳ（Complementary Metal Oxide Semiconductor）等を配列して構成される。このイメージセンサ３２の表面がカメラ３０の撮像面となる。イメージセンサ３２の前面に、第１基準点１１〜１３および第２基準点１４の発光波長に応じたフィルタを設置することにより、余計な光が排除され、第１基準点１１〜１３および第２基準点１４（図２参照）の像のみが撮影されることとなる。本実施の形態の場合、フィルタとして、赤外光を透過させるものを使用し、イメージセンサ３２により赤外光を撮影することが好ましい。このようにすることで、撮影された画像中で背景と第１基準点１１〜１３および第２基準点１４の像とのコントラストの差が生じやすく、点画像をより抽出しやすくなる。具体的には例えば、第１基準点１１〜１３および第２基準点１４の発光波長を９００ｎｍとした場合、イメージセンサ３２の前面に８５０ｎｍよりも長い波長だけを透過するフィルタを設置する。

図５は、演算装置４０の構成を説明した図である。
演算装置４０は、図５に示したように、第１基準点１１〜１３（図２参照）の位置関係を記憶する記憶部４１、記憶部４１から第１基準点１１〜１３の位置関係を取得する位置関係取得部４２、撮影部により撮影された入力画像を取り込み、第１基準点１１〜１３の像と第２基準点１４の像との位置関係に基づき、第１基準点１１〜１３の像と第２基準点１４の像とを識別する識別部４３、位置関係取得部４２により取得された第１基準点１１〜１３の位置関係と、識別部４３により識別された第１基準点１１〜１３の像の位置関係から第２基準点１４の対象物１００における位置を求め、第１基準点１１〜１３および第２基準点１４の位置関係より対象物１００の位置を求め、位置計測結果を出力する演算部４４とからなる。

また、図６は、演算装置４０の動作を説明したフローチャートである。
以下、図５と図６に基づき、演算装置４０の動作の概略を説明する。
演算装置４０は、例えばパーソナルコンピュータ等で実現される。識別部４３は、カメラ３０により撮影された点光源の入力画像を取得し（ステップ１０１）、入力画像中の各点光源の像が、第１基準点１１〜１３と第２基準点１４のうちどちらかに該当する像かをまず識別する（ステップ１０２）。
次に、位置関係取得部４２が、第１基準点１１〜１３の位置関係を記憶部４１から読み出す（ステップ１０３）。そして、演算部４４は、位置関係取得部４２から第１基準点１１〜１３の位置関係を受け取り、第１基準点１１〜１３の位置関係と、識別部４３により識別された第１基準点１１〜１３の像の位置関係から第２基準点１４の対象物１００における位置を求める（ステップ１０４）。更に、演算部４４は、対象物１００上の第１基準点１１〜１３の位置関係および第２基準点１４の位置関係より対象物１００の三次元位置、三軸角度を求める（ステップ１０５）。

なお、図５に示した演算装置４０では、記憶部４１はこの演算装置４０の内部に設けられていたが、これに限られるものではなく、例えば、演算装置４０の外部ではあるが、位置計測装置３００中に設けるようにしてもよい。また、位置計測装置３００の外部に設けるようにしてもよい。この場合、位置関係取得部４２は、予め用意された通信手段を通じ、第１基準点１１〜１３の位置関係を取得することができる。

＜画像における第１識別点と第２識別点の識別＞
次に、カメラ３０により撮影された画像から、第１基準点および第２基準点の各々の画像を識別する方法を説明する。
図７は、３点の第１基準点と１点の第２基準点を備える対象物の第１の例である。
図７に示した対象物１００の第１基準点１１〜１３および第２基準点１４は、図２で説明した場合と同様の配置を採る。即ち、第１基準点１１〜１３により形成される三角形の形状は、直角二等辺三角形である。そして、第１基準点１１と第１基準点１２の間の長さ（即ち、ａ１ａ２の長さ）が５０ｍｍであり、第１基準点１２と第１基準点１３の間の長さ（即ち、ａ２ａ３の長さ）が同様に５０ｍｍである。そして、この間の角度が直角である。また、第１基準点１１〜１３および第２基準点１４で形成される四角形（ａ１ａ２ａ３ｂ１）の対角線の交点をｐとし、線分の比ａ１ｐ／ａ１ａ３＝ｔ、ａ２ｐ／ａ２ｂ１＝ｓ、とする。このとき、ａ１ｐ＝ｔ、ａ２ｐ＝ｓ、ａ３ｐ＝１−ｔ、ｂ１ｐ＝１−ｓとなる。そして、第２基準点１４は、ｓ＜０．４７かつｔ＜０．４７を満たす位置に配置する。但し、第２基準点１４の位置は、対象物１００の表面に配されるということ以外は、不明である。

ここで、第１基準点１１〜１３が配される平面にｘ軸とｙ軸を配し、この平面に対し、垂直な方向をｚ軸とする対象物１００を基準とする座標系を考える。このとき第１基準点１１は、（５０，０，０）に位置し、第１基準点１２は、（０，０，０）に位置し、第１基準点１３は、（０，５０，０）に位置するものとして定義することができる（単位はｍｍ）。また第２基準点１４のｚ座標は０であるが、ｘ座標、ｙ座標はこの時点では不明である。

このとき線分の比の値であるｓ、ｔ、１−ｓ、１−ｔの値は、アフィン変換の前後で保存されるアフィン不変量である。
よって、カメラ３０により撮影された第１基準点１１〜１３および第２基準点１４の点光源の像の位置関係においても保存される。したがって、この幾何学的特徴に基づいて各点光源の像が第１基準点１１〜１３および第２基準点１４のいずれのものかを特定すればよい。

次に、カメラ３０により撮影された画像から第１基準点１１〜１３および第２基準点１４を識別する際の処理手順を説明する。
カメラ３０により撮影された画像には、第１基準点１１〜１３および第２基準点１４の点光源の像が４点撮影されている。
まず各像の交点で分けられる線分比を求める。このとき、線分比が、０．５より小さい線分であるａ２ｐとａ１ｐが検出できる。ここで、ａ２の位置にある第１基準点１２、ａ１の位置にある第１基準点１１、ｂ１の位置にある第２基準点１４は反時計回りに順に配置する。よって、この検出された２本の線分を基準にして反時計回りに辿っていったときに、最初に検出される線分は、ｂ１ｐである。即ち、このｂ１ｐの線分の端点であるｂ１に位置するのが第２基準点１４であることが検出できる。あとは、更に反時計回りに辿っていき次に検出される線分が、ａ３ｐである。よって、ａ３ｐの線分の端点であるａ３に位置するのが第１基準点１３であることが検出できる。

＜対象物１００の三次元位置、三軸角度の計算＞
次に、第１基準点１１〜１３および第２基準点１４の位置に基づいて対象物１００の三次元位置および三軸角度を計算する方法について説明する。

図８は、対象物１００の三次元位置および三軸角度を計算する方法を説明する図である。
まず、図８に示すように、カメラ３０により撮影された画像上（イメージセンサ３２上）の第１基準点１１〜１３の像に基づいて、カメラ３０の光学中心Ｏから第１基準点１１〜１３の各々への方向ベクトルｄｉ（ｉ＝１、２、３）を算出する。方向ベクトルｄｉは、カメラ３０の撮像面における第１基準点１１〜１３の像と光学中心Ｏとを結ぶ直線上にあり、光学中心Ｏを始点とする規格化された単位ベクトルである。また、第１基準点１１への方向を方向ベクトルｄ１（＝［ｘ１，ｙ１，ｚ１］）、第１基準点１２への方向を方向ベクトルｄ２（＝［ｘ２，ｙ２，ｚ２］）、第１基準点１３への方向を方向ベクトルｄ３（＝［ｘ３，ｙ３，ｚ３］）とする。

カメラ３０の光学中心Ｏを基準として、第１基準点１１の位置ベクトルをｐ１、第１基準点１２の位置ベクトルをｐ２、第１基準点１３の位置ベクトルをｐ３とする。これらの位置ベクトルｐｉ（ｉ＝１、２、３）は、それぞれ上記の方向ベクトルｄｉと重なる。そこで、各位置ベクトルｐｉの長さを表す係数をｔ１、ｔ２、ｔ３とすると、各位置ベクトルｐｉは次式で表せる。

第１基準点１１〜１３が形成する三角形の形状は予めわかっているので、三角形の各辺の長さ（各第１基準点１１〜１３間の長さ）を、ａ１ａ２＝Ｌ１、ａ２ａ３＝Ｌ２、ａ３ａ１＝Ｌ３とすると、次式が得られる。

これを整理すると、次式が得られ、

さらに次式が得られる。

ただし、
Ａ１＝ｘ１ｘ２＋ｙ１ｙ２＋ｚ１ｚ２、
Ａ２＝ｘ２ｘ３＋ｙ２ｙ３＋ｚ２ｚ３、
Ａ３＝ｘ３ｘ１＋ｙ３ｙ１＋ｚ３ｚ１
である。

実数解を持つために、次式の条件を満たす実数ｔ１、ｔ２、ｔ３をそれぞれ上記の数４式に代入し、数４式が成立する全てのｔ１、ｔ２、ｔ３を求める。

次に、この数５式を満たすｔ１、ｔ２、ｔ３の組を数４式に代入し、数４式が成立する全てのｔ１、ｔ２、ｔ３の組を算出する。そして数１式からｐ１、ｐ２、ｐ３を算出する。即ち、第１基準点１１〜１３の三次元位置を算出する。

以上の方法により算出した結果、明らかに正解でない虚数解を除去すると、２つの解が得られた。この２つの解であるＱ１１、Ｑ１２をそれぞれ表１および表２に示す。

これらの解Ｑ１１、Ｑ１２から第２基準点１４の対象物１００における座標を算出した。解Ｑ１１から算出した第２基準点１４の対象物１００における座標をＭ１１とし、解Ｑ１２から算出した第２基準点１４の対象物１００における座標をＭ１２とする。この２つの解が第２基準点１４の位置候補となる。ここで、カメラ３０が撮影するフレーム毎に順次第２基準点１４の対象物１００における座標を算出した。第１フレームにおける解は、上記のＭ１１、Ｍ１２であるが、本実施の形態では、第５フレームまでの解を算出した。このとき第２フレームにおける解を、Ｍ２１、Ｍ２２とし、第３フレームにおける解を、Ｍ３１、Ｍ３２とし、第４フレームにおける解を、Ｍ４１、Ｍ４２とし、第５フレームにおける解を、Ｍ５１、Ｍ５２とした。フレーム数をｉとし、算出された合計１０個の解を表３に示す。なおここでは、Ｚ座標は０であることがわかっているので、ｘ座標とｙ座標について（ｘ，ｙ）の書式で示している。

撮影された各フレームにおいて、いつも正解は１つ入っており、不正解の値は、ばらばらの値として出てくる。この場合、Ｍｉ１の各解は、各フレーム毎にいつも同じ値を示すのに対し、Ｍｉ２は、各フレーム毎に異なった値を示す。よって正解は（７０．００，５０．００）であり、これが第２基準点１４の対象物１００におけるｘ座標およびｙ座標となる。
なお、表３においてＭｉ１の解は、全て（７０．００，５０．００）となって一致したが、測定誤差等によって、一致しないものも算出されることがある。その場合は、組毎の解の変動の少ない方を正解とすることができる。
このようにして、識別部４３（図５参照）により識別された第１基準点１１〜１３の像の組を複数取得し、この組毎に第１基準点１１〜１３の像の位置関係から第２基準点１４の対象物１００における位置候補を複数算出し、算出された位置候補の中から、第２基準点１４の位置の正解を求めることができる。そして、この正解は、算出された位置候補の中から組毎の変動が最も少ないものを選択することで求めることができる。

以上のように、Ｍｉ１の解が正解であったので、第１基準点１１〜１３の三次元位置は、表１に示した座標値が正解となる。また第１基準点１１〜１３および第２基準点１４の三次元位置が求まったので対象物１００の三軸角度も求めることが可能となり、対象物１００の位置計測ができることになる。

また、上記のように算出された第２基準点１４の位置に、種々の情報を付与することができる。例えば、認識情報として用いることが考えられる。対象物１００をＩＤ（identification）番号を付与したカードとし、ＩＤ番号とそれに対応した第２基準点１４の位置が対応付けられているＩＤテーブルを参照することで、ＩＤ番号を認識することができる。

図９は、３点の第１基準点と１点の第２基準点を備える対象物の第２の例である。
図９に示した対象物１００上の第１基準点１１〜１３は、第１基準点１１〜１３により形成される三角形の形状が、二等辺三角形になるように配置される。具体的には、第１基準点１１は、（３０，０，０）に位置し、第１基準点１２は、（０，０，０）に位置し、第１基準点１３は、（１５，５０，０）に位置する（単位はｍｍ）。この場合、第１基準点１１と第１基準点１３の間の長さ（即ち、ａ１ａ３の長さ）と、第１基準点１２と第１基準点１３の間の長さ（即ち、ａ２ａ３の長さ）が同じとなる。また、第２基準点１４は、この、第１基準点１１〜１３により形成される二等辺三角形の内部に配置するが、そのｘ座標とｙ座標は不明である。但し、ｚ座標は、１５であることがわかっているものとする。よって、第２基準点１４の座標は（ｇ，ｈ，１５）と表すことができる。この場合、第２基準点１４は第１基準点１１〜１３により構成される平面上にない。このようにするには、例えば、対象物１００に脚を設けその脚の先端に、第２基準点１４を設けるようにすればよい。

この対象物１００をカメラ３０により撮影する。そして撮影された４つの像からまず、第２基準点１４の像を識別する。この方法として、例えば、第２基準点１４を、第１基準点１１〜１３により形成される三角形の重心付近に設けておく。この場合、三角形の重心は、アフィン変換の前後で保存される量、即ちアフィン不変量である。よってカメラ３０により撮影された４つの像の重心の位置を算出し、その位置から最も近い像を第２基準点１４の像とすることができる。

次に、残った３つの像が第１基準点１１〜１３の像であるが、この画像情報からは、どの像が第１基準点１１〜１３の像であるかは、図７で説明したような方法では、識別できない。
本実施の形態では、撮影された第１基準点１１〜１３の像を反時計回りに仮にｃ１、ｃ２、ｃ３（図示せず）とする。ａ１、ａ２、ａ３も反時計回りに配置するので、このｃ１、ｃ２、ｃ３に対応し得るａ１、ａ２、ａ３の組み合わせは３通りとなる。そして、これら３通りの場合について、それぞれ第１基準点１１〜１３の三次元位置を上述した方法で算出する。その結果、明らかに正解でない虚数解を除去すると、各々の場合に２つの解が第２基準点１４の位置候補として算出された。その計算結果を表４に示す。

ここで、Ｍ１１〜Ｍ１６は、ａ１、ａ２、ａ３の三次元位置を表す解である。
これら６つの計算結果に対して、上述した場合と同様に、第２基準点１４の像の位置から、第２基準点１４の三次元位置を計算した。これを例えば、カメラ３０の６フレーム毎にそれぞれ計算し、常に同じ値を示す第２基準点１４の座標を正解とした。よって、表４に示したＭ１１〜Ｍ１６の中から、この正解に対応する第１基準点１１〜１３の三次元位置の解が同様に正解となる。なお、表４において正解は、測定誤差等によって、常に同じ値を示すとは限らず一致しないものも算出されることがある。その場合は、解の変動の少ないものを正解とすることができる。
このようにして、識別部４３（図５参照）により識別された第１基準点１１〜１３の像の組を複数取得し、この組毎に第１基準点１１〜１３の像の位置関係から第２基準点１４の対象物１００における位置候補を複数算出し、算出された位置候補の中から、第２基準点１４の位置の正解を求めることができる。そして、この正解は、算出された位置候補の中から組毎の変動が最も少ないものを選択することで求めることができる。

以上のようにして、第１基準点１１〜１３および第２基準点１４の三次元位置が求まったので対象物１００の三軸角度も求めることが可能となり、対象物１００の位置計測ができることになる。

また、本実施の形態において、第２基準点１４は、第１基準点１１〜１３により形成される三角形の重心付近に設けていたが、他には、例えば、第２基準点１４を、第１基準点１１〜１３および第２基準点１４の４点の重心位置より最も遠い位置に設けるようにしてもよい。

また、第２基準点１４の対象物１００を基準とした位置は、上述した例では、不明であることになっていた。ここで、この座標値を参照値として用意し、それと照合することにより第２基準点１４の位置候補の中から第２基準点１４の正解を求め、それにより第１基準点１１〜１３の正解を求めることもできる。

表５は、参照値の一例を示した表である。ここでは、第２基準点１４をＩＤとして利用することを想定し、ＩＤ番号１〜６についてそれぞれ第２基準点１４の対象物１００を基準とした位置が用意されているものとする。ここでは、その位置を(ｘ１，ｙ１，ｚ１)、(ｘ２，ｙ２，ｚ２)、(ｘ３，ｙ３，ｚ３)、(ｘ４，ｙ４，ｚ４)、(ｘ５，ｙ５，ｚ５)、(ｘ６，ｙ６，ｚ６)としている。

ここで、例えば、図７や図８で示した対象物１００を使用し、同様にまず第１基準点１１〜１３および第２基準点１４の画像を識別した後、第２基準点１４の三次元位置を算出する。算出された複数の解について、その結果を、表５に示したような参照値と比較し、算出された解の中で一致したものが、正解となる。
この場合、カメラ３０により撮影するフレーム数が１回であっても、第２基準点１４の対象物１００における位置を予め定めたディスクリートな値に設定することで、正しい計測値を決定することができる。
なお、算出された解は、測定誤差等によって、常に参照値と一致するとは限らず一致しないものも算出されることがある。その場合は、参照値との差が最も少ない解を正解とすることができる。
このようにして、第１基準点１１〜１３の像の位置関係から第２基準点１４の対象物１００における位置候補を複数算出し、算出された位置候補の中から、第２基準点１４の位置の正解を求めることができる。そして、正解は、算出された位置候補の中から第２基準点１４の位置に関する参照値との差が最も少ないものを選択することで求めることができる。

以上のようにして、第１基準点１１〜１３および第２基準点１４の三次元位置が求まったので対象物１００の三軸角度も求めることが可能となり、対象物１００の位置計測ができることになる。
そして、この場合、表５のような参照値をＩＤテーブルとして、ＩＤ番号を取得することができる。

なお、本実施の形態では、第１基準点１１〜１３および第２基準点１４をＬＥＤにより構成される発光点としたが、これに限定するものではない。例えば、第１基準点１１〜１３および第２基準点１４としてＬＥＤの代わりに再帰反射板を設け、カメラ３０の近くに設けられた照明装置から光を照射し、再帰反射板による反射光をカメラ３０により撮影するようにしてもよい。
また、本実施の形態では、第１基準点１１〜１３は、３点であったが、３点以上であれば特に制限はない。但し、そのうち少なくとも３点の位置関係が予めわかっていることが必要である。この場合、位置関係が予めわかっている３点を抽出し、今まで詳述した方法を用いて対象物１００の位置計測を行うことができる。
更に、第２基準点１４は、１点であったが、これに限られるものでなく１点以上であってもよい。この場合、対象物１００をＩＤ番号を付与したカードとし、ＩＤ番号とそれに対応した２点以上の第２基準点１４の位置の組み合わせが対応付けられている参照値を参照することで、ＩＤ番号を認識することができる。

本実施形態が適用される位置計測装置、および位置計測を行う対象物を含んだ位置計測システムを説明した図である。 本実施の形態が適用される対象物の構成を示す図である。 対象物上に第２基準点を配置可能な領域を示した例である。 カメラの構成例を示す図である。 演算装置の構成を説明した図である。 演算装置の動作を説明したフローチャートである。 ３点の第１基準点と１点の第２基準点を備える対象物の第１の例である。 対象物の三次元位置および三軸角度を計算する方法を説明する図である。 ３点の第１基準点と１点の第２基準点を備える対象物の第２の例である。

符号の説明

１１〜１３…第１基準点、１４…第２基準点、３０…カメラ、３１…光学系、３２…イメージセンサ、４０…演算装置、４１…記憶部、４２…位置関係取得部、４３…識別部、４４…演算部、１００…対象物、３００…位置計測装置

Claims (7)

1. 対象物に設けられた３点以上の第１基準点と１点以上の第２基準点からなる点光源の像を撮影する撮影部と、
   前記撮影部により撮影された前記第１基準点の像と前記第２基準点の像とを識別する識別部と、
   前記第１基準点の位置関係を取得する位置関係取得部と、
   前記位置関係取得部により取得された前記第１基準点の位置関係と前記識別部により識別された当該第１基準点の像の位置関係から前記第２基準点の前記対象物における位置を求め、当該第１基準点および当該第２基準点の位置関係より当該対象物の位置を求める演算部と、
   を備え、
   前記演算部は、
   前記識別部により識別された前記第１基準点の像の組を複数取得し、
   前記組毎に前記第１基準点の像の位置関係から前記第２基準点の前記対象物における位置候補を複数算出し、
   算出された前記位置候補の中から、前記第２基準点の位置の正解を求めることを特徴とする位置計測装置。
2. 前記正解は、算出された前記位置候補の中から前記組毎の変動が最も少ないものを選択することで求めることを特徴とする請求項１に記載の位置計測装置。
3. 前記第１基準点の像の組は、前記撮影部が撮影するフレーム毎に取得することを特徴とする請求項１または２に記載の位置計測装置。
4. 前記第１基準点は、３点の発光点からなり、前記第２基準点は、１点の発光点からなることを特徴とする請求項１乃至３の何れか１項に記載の位置計測装置。
5. コンピュータに、
   撮影部により撮影された第１基準点の像と第２基準点の像とを識別する識別機能と、
   前記第１基準点の位置関係を取得する位置関係取得機能と、
   前記第１基準点の位置関係と前記第１基準点の像の位置関係から前記第２基準点の対象物における位置を求め、当該第１基準点および当該第２基準点の位置関係より当該対象物の位置を求める演算機能と、
   を実現させ、
   前記演算機能は、
   前記第１基準点の像の位置関係から前記第２基準点の前記対象物における位置候補を複数算出し、
   算出された前記位置候補の中から、前記第２基準点の位置の正解を求めることを特徴とするプログラム。
6. 前記正解は、算出された前記位置候補の中から前記第２基準点の位置に関する参照値との差が最も少ないものを選択することで求めることを特徴とする請求項５に記載のプログラム。
7. 前記識別機能は、前記第１基準点および前記第２基準点の位置関係により算出されるアフィン不変量を用いて、前記第１基準点の像と前記第２基準点の像とを識別することを特徴とする請求項５または６に記載のプログラム。

Images