(57)【特許請求の範囲】
【請求項１】
平面上を移動する移動体の位置を検出する移動体位置検出システムであって、
前記平面上に配置された複数のドットからなるドットパターンと、
前記移動体に備えられ、前記移動体が位置する平面上の領域を撮像する撮像手段と、
前記撮像手段により取得した画像中のドットと、このドットに対応するポールスターアルゴリズムのポールスターデータベース中のドットとの間の誤差を吸収するための尤度を設定したポールスター特徴量を用いてポールスター照合を行うポールスター照合手段と、
前記ポールスター照合手段により照合された照合ドット候補から所定の照合ドットに絞り込む絞り込み手段と、
前記絞り込み手段により絞り込んだ所定の照合ドットからなるドットパターンの位置関係に基づいて、前記移動体の現在位置および方向を検出する検出手段と、
を備え、
前記絞り込み手段は、前記ポールスター照合手段により照合された照合ドット候補について照合ドット候補が存在する所定の共通領域を求め、前記共通領域内に存在する照合ドットに絞り込む処理を含むことを特徴とする移動体位置検出システム。
【請求項２】
前記ポールスター照合手段は、前記ポールスターデータベースにおける照合範囲を複数の領域に分割してこの各領域でポールスター照合を行い、照合率が最大となる領域を求めることを特徴とする請求項１に記載の移動体位置検出システム。
【請求項３】
平面上を移動する移動体の位置を検出する移動体位置検出方法であって、
前記平面上には複数のドットからなるドットパターンが配置され、
前記移動体には前記移動体が位置する平面上の領域を撮像する撮像手段が備えられ、
前記撮像手段により取得した画像中のドットと、このドットに対応するポールスターアルゴリズムのポールスターデータベース中のドットとの間の誤差を吸収するための尤度を設定したポールスター特徴量を用いてポールスター照合を行い、照合された照合ドット候補について照合ドット候補が存在する所定の共通領域を求め、前記共通領域内に存在する所定の照合ドットに絞り込み、絞り込んだ所定の照合ドットからなるドットパターンの位置関係に基づいて、前記移動体の現在位置および方向を検出することを特徴とする移動体位置検出方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体位置検出システムおよび方法に関するものである。
【背景技術】
【０００２】
従来、平面上に配置したパターン（模様）を利用して、平面上を移動する移動体の位置の検出を行う方法が知られている（例えば、特許文献１参照）。
【０００３】
これに対し、本発明者内の一部を含む発明者らは、既に特願２００８−２７４６８０号および特願２００８−２７４６８１号の移動体位置検出システムを提案している。
【０００４】
この移動体位置検出システムは、図１７および図１８に示すように、既知のドットパターン３が描画してある床面２上に配置した移動体４から床面２を撮像し、撮像した画像中にあるドット５のドットパターン３の特徴量を、床面２全体のドットパターン３の特徴量が格納されたデータベース６と照合することで、撮像した画像中のドットが床面２上のどのドットに相当するのかを割り出し、床面２上における移動体４の位置を検出するものである。
【０００５】
一方、カメラによる撮像位置および方向を判別する技術として、スウェーデンのアノト社が開発したデジタルペン技術が知られている（関連文献として特許文献１がある）。これは、図１９に示すように、ユニークなドットパターン８０が印刷された紙とカメラ８１内蔵のペン８２を用い、文字を書く際のペン先８３の紙面上での位置を検出し、これをデジタルデータとして記録するものである。ドットパターン８０は、ＸＹ位置座標値を符号化パターンにしたものであり、逆に符号化パターンを復元することでＸＹ座標値を得ることができる。ただし、この技術は、規定のコードを表現するドットパターン８０を正確に描く必要があり、自然発生的なドットパターンを利用した位置検出はできないという問題がある。
【０００６】
また、図２０に示すように、２次元情報がコード化された模様８４を平面８５上に描画し、それをカメラ８６で読み取って位置を検出する技術が知られている（例えば、特許文献２参照）。この技術は、数種類の図形でパターンを描画することが前提となっており、デザイン上の制約がある。例えば、ドットのみでパターンを構成することはできない。
【０００７】
また、宇宙空間で宇宙機８７の姿勢同定に使われる機器（スターセンサー）に用いる撮像恒星点群の恒星配置図８８（スターマップ）への照合アルゴリズムとして、ポールスターアルゴリズムが知られている（例えば、非特許文献１参照）。図２１に示すように、スターセンサーは、宇宙機８７に搭載されたカメラで天空８９を撮像し、撮像領域の恒星点の配列を既知のスターマップ８８と照合することによりカメラの向いている姿勢を特定するセンサーである。恒星の配置は自然発生的であり必ずしも均一に点は分布していないが、ポールスターアルゴリズムは、このような対象に対する照合に有効である。
【０００８】
このポールスターアルゴリズムは、平面上にランダムに広く点が分散しているとき、一部分の点分布から、その部分の平面上の位置を特定する方法である。このアルゴリズムの概略の処理手順は、図２２に示すように、画像取得して画像中の点にラベリングをし（ステップＳ１）、この画像点群を用いてポールスター照合を行い（ステップＳ２）、星点候補群について多角形照合を行って（ステップＳ３）、カメラ姿勢の計算をする（ステップＳ４）という手順である。
【０００９】
ここで、ステップＳ２のポールスター照合においては、平面上の任意の点を中心として規定半径の円内に存在する周辺点までの直線は、複数存在するが、その複数の直線の長さの組み合わせは、その中心点に関して特有のものであることを利用している。このような各点周辺の点分布を表す特徴量をポールスター特徴量として定義する。この特徴量に関するデータベースは、図２３に示すステップＳ５〜Ｓ８の処理により、オフラインで事前に作成しておく。
【００１０】
そして、取得画像中の各画像点のポールスター特徴量をデータベースと照合することで照合星点（データベース点）候補を求める。ステップＳ３の多角形照合においては、画像点を結んで作った多角形による照合により照合候補点群から幾何学的な関係が画像点群と整合性のある候補を選別する。そして、ステップＳ４において、残った画像点群と照合点群の座標値の組み合わせから撮像カメラ姿勢・位置を計算する。
【００１１】
次に、図２３の各ステップの処理内容について、図２４〜図２９を参照しながら具体的に説明する。
（１）ポールスター特徴量の計算（ステップＳ６）
図２４に示すように、ある点（Ｐｎ）を中心として規定の分布半径Ｒ内に存在する周辺点までの直線の長さ（以下、アーム長という。）を求め、アーム長を単位長さＤで分割してインデックス化する（以下、アーム長インデックスという。）。
【００１２】
ａｉ＝ｆｉｘ（ｒｉ／Ｄ）＋１
ａｍａｘ＝ｆｉｘ（Ｒ／Ｄ）＋１
ここで、ｒｉ：アーム長、ａｉ：アーム長インデックス、Ｄ：インデックス単位長さ、Ｒ：分布半径、ａｍａｘ：最大アーム長インデックスである。また、ｆｉｘは小数点以下を切り捨てて整数値を返す演算子である。
【００１３】
アーム長インデックスの組み合わせをその点周辺の点分布を表す特徴量（以下、ポールスター特徴量という。）とする。ここで、中心点に対して同一のアーム長インデックスが複数計算されても、ポールスター特徴量としてのアーム長インデックス列は変わらない。
【００１４】
（２）ポールスターデータベースの作成（ステップＳ７〜Ｓ８：オフライン）
対象とする点（以下、データベース点という。）全てについて、上記の（１）のポールスター特徴量を計算し、ルックアップテーブルとして整理し蓄積する（ステップＳ７）。
【００１５】
ルックアップテーブルは、図２５に示すように、１〜ａｍａｘの各アーム長インデックスに対して、そのアーム長インデックスをポールスター特徴量中にもつデータベース点を羅列することで整理した表である。
【００１６】
（３）ポールスター照合（ステップＳ２：オンライン）
カメラで画像を取得し、取得した画像中の各点（以下、画像点という。）の位置を抽出する。図２６に示すように、取得画像の画像点についてポールスター特徴量を計算する。
【００１７】
画像点のポールスター特徴量中の各アーム長インデックスについて、図２５のルックアップテーブルを使って、図２７に示すように、そのアーム長インデックスをもつデータベース点（複数）を調べる。
【００１８】
また、図２８に示すように、各データベース点（Ｐ１〜Ｐｍａｘ）に対応したカウンタを用意し、上記の処理でデータベース点が出てくる毎に対応する点のカウンタをインクリメントする。
【００１９】
最大カウンタ値をとる点が画像点に対応する照合点候補となる。各画像点のポールスター特徴量について同様の処理を行うことで各画像点に対する照合点候補が求められる。カウンタには下限を設定し、最大カウンタ値がこの下限以下の場合には、当該の画像点に関して照合不能とし、その画像点は以降の照合の対象から除く。
【００２０】
（４）多角形照合（ステップＳ３：オンライン）
上記の（３）で得られた各画像点に対応するデータベース点に対して、図２９に示すように、いくつかの画像点を結んで作った多角形と、それらの画像点に対応するデータベース点を結んで作った多角形とが一致する点の組み合わせを選ぶことで、一致確度の高い画像点とデータベース点の組み合わせを求める。図２９においては、１、３、４、７の点の組が最終的に照合されている。
【００２１】
（５）カメラ姿勢計算による位置・方向の算出（ステップＳ４：オンライン）
上記の（４）で得られた３点以上の画像点と照合点のＸＹ座標値の組み合わせから、カメラの姿勢（位置・方向）を求める。
【先行技術文献】
【特許文献】
【００２２】
【特許文献１】特開２００６−１４１０６１号公報
【特許文献２】特開２００８−５２４０３号公報
【非特許文献】
【００２３】
【非特許文献１】E. SILANI, M. LOVERA : Ploitecnico di Milano Italy, “Star Identification Algorithms : Novel Approach & Comparison Study”, IEEE Transactions on Aerospace and Electronic Systems, Vol.42, No.4 October 2006
【発明の概要】
【発明が解決しようとする課題】
【００２４】
ところで、上記の従来の移動体位置検出システムの平面上の点群照合のアルゴリズムとして上記のポールスターアルゴリズムをそのまま適用すると、誤った点と照合する問題（以下、誤照合という。）、照合ができない問題（以下、照合不能という。）、多くの点と照合して特定できない問題（以下、多照合という。）が発生するおそれがある。このような照合不良は、上記のスターマップ照合と平面上の点群照合の前提条件の違いにより引き起こると考えられる。これら照合の前提条件およびその違いは以下のとおりである。
【００２５】
［スターマップ照合の前提条件］
（１）レンズの歪やカメラの姿勢に起因する撮像面上の恒星点間の相対的位置関係の変動は極めて少ない。
（２）上記の（１）により、恒星点の位置を画像中で極めて細かい分解能で計算できる。
（３）スターマップ中の恒星点数（データベース点）は、等級（輝度を表す）により選別でき、おおよそ数千程度である。
【００２６】
［平面上の点群照合の前提条件］
（１）広角レンズを用いるためレンズ歪が大きく、かつ、点群のある平面までの対物距離が短いため、カメラ姿勢変動による点間の相対的位置関係の変動は無視できないほど大きい。
（２）処理速度の向上を図るため、撮像解像度を高くできないとともに、上記の（１）により、点位置を細かい分解能で算出できない。
（３）点群数（データベース点）は、対象とする平面の広さによるが、約２０平米程度で数万個とスターマップに比べて多い。
【００２７】
上記の（１）の条件の違いにより、誤照合および照合不能の問題が発生し、上記の（２）、（３）の条件の違いにより、多照合の問題が発生するおそれがある。
【００２８】
本発明は、上記に鑑みてなされたものであって、ポールスターアルゴリズムを用いた際の照合不良を起こし難くして、検出の信頼性を確保した移動体位置検出システムおよび方法を提供することを目的とする。
【課題を解決するための手段】
【００２９】
上記した課題を解決し、目的を達成するために、本発明の請求項１に係る移動体位置検出システムは、平面上を移動する移動体の位置を検出する移動体位置検出システムであって、前記平面上に配置された複数のドットからなるドットパターンと、前記移動体に備えられ、前記移動体が位置する平面上の領域を撮像する撮像手段と、前記撮像手段により取得した画像中のドットと、このドットに対応するポールスターアルゴリズムのポールスターデータベース中のドットとの間の誤差を吸収するための尤度を設定したポールスター特徴量を用いてポールスター照合を行うポールスター照合手段と、前記ポールスター照合手段により照合された照合ドット候補から所定の照合ドットに絞り込む絞り込み手段と、前記絞り込み手段により絞り込んだ所定の照合ドットからなるドットパターンの位置関係に基づいて、前記移動体の現在位置および方向を検出する検出手段と、を備え、前記絞り込み手段は、前記ポールスター照合手段により照合された照合ドット候補について照合ドット候補が存在する所定の共通領域を求め、前記共通領域内に存在する照合ドットに絞り込む処理を含むことを特徴とする。
【００３０】
また、本発明の請求項２に係る移動体位置検出システムは、上述した請求項１において、前記ポールスター照合手段は、前記ポールスターデータベースにおける照合範囲を複数の領域に分割してこの各領域でポールスター照合を行い、照合率が最大となる領域を求めることを特徴とする。
【００３１】
また、本発明の請求項３に係る移動体位置検出方法は、平面上を移動する移動体の位置を検出する移動体位置検出方法であって、前記平面上には複数のドットからなるドットパターンが配置され、前記移動体には前記移動体が位置する平面上の領域を撮像する撮像手段が備えられ、前記撮像手段により取得した画像中のドットと、このドットに対応するポールスターアルゴリズムのポールスターデータベース中のドットとの間の誤差を吸収するための尤度を設定したポールスター特徴量を用いてポールスター照合を行い、照合された照合ドット候補について照合ドット候補が存在する所定の共通領域を求め、前記共通領域内に存在する所定の照合ドットに絞り込み、絞り込んだ所定の照合ドットからなるドットパターンの位置関係に基づいて、前記移動体の現在位置および方向を検出することを特徴とする。
【発明の効果】
【００３３】
本発明によれば、平面上を移動する移動体の位置を検出する移動体位置検出システムであって、前記平面上に配置された複数のドットからなるドットパターンと、前記移動体に備えられ、前記移動体が位置する平面上の領域を撮像する撮像手段と、前記撮像手段により取得した画像中のドットと、このドットに対応するポールスターアルゴリズムのポールスターデータベース中のドットとの間の誤差を吸収するための尤度を設定したポールスター特徴量を用いてポールスター照合を行うポールスター照合手段と、前記ポールスター照合手段により照合された照合ドット候補から所定の照合ドットに絞り込む絞り込み手段と、前記絞り込み手段により絞り込んだ所定の照合ドットからなるドットパターンの位置関係に基づいて、前記移動体の現在位置および方向を検出する検出手段と、を備える。このため、ポールスターアルゴリズムを用いた際の照合不良を起こし難くして、検出の信頼性を確保することができるという効果を奏する。
【図面の簡単な説明】
【００３４】
【図１】図１は、本発明に係る移動体位置検出システムの実施例を示す構成図である。
【図２】図２は、ドットパターンと取得画像の一例を示す図である。
【図３】図３は、取得画像から位置を検出する概念を説明する図である。
【図４】図４は、本発明の実施例１を示すフローチャート図である。
【図５】図５は、本発明の実施例２を示すフローチャート図である。
【図６】図６は、照合点存在領域を説明する図である。
【図７】図７は、本発明の実施例３を示すフローチャート図である。
【図８】図８は、照合床エリアの分割を説明する図である。
【図９】図９は、本発明の実施例４を示すフローチャート図である。
【図１０】図１０は、従来の在庫ロケーション管理（１）を示す平面図である。
【図１１】図１１は、従来の在庫ロケーション管理（２）、（３）を示す側面図である。
【図１２】図１２は、従来の在庫ロケーション管理（２）、（３）を示す平面図である。
【図１３】図１３は、従来の在庫ロケーション管理（２）、（３）を示す平面図である。
【図１４】図１４は、本発明に係る移動体位置検出システムを在庫ロケーション管理に応用した場合の斜視図である。
【図１５】図１５は、本発明に係る移動体位置検出システムを在庫ロケーション管理に応用した場合の平面図である。
【図１６】図１６は、本発明に係る移動体位置検出システムを在庫ロケーション管理に応用した場合の斜視図である。
【図１７】図１７は、従来の移動体位置検出システムの概略構成図である。
【図１８】図１８は、従来の移動体位置検出システムのドットパターンと取得画像の一例を示す図である。
【図１９】図１９は、従来のデジタルペン技術を示す図である。
【図２０】図２０は、従来のコード化された模様による位置検出技術を示す図である。
【図２１】図２１は、従来の宇宙機の姿勢同定に使われるスターセンサー技術を示す図である。
【図２２】図２２は、従来のスターセンサー技術に用いられるポールスターアルゴリズムの要部フローチャート図である。
【図２３】図２３は、従来のスターセンサー技術に用いられるポールスターアルゴリズムのフローチャート図である。
【図２４】図２４は、ポールスター特徴量を説明する図である。
【図２５】図２５は、ルックアップテーブルを示す図である。
【図２６】図２６は、画像点のポールスター特徴量を説明する図である。
【図２７】図２７は、アーム長インデックスとデータベース点の対応を示す図である。
【図２８】図２８は、ルックアップテーブルを使った照合を説明する図である。
【図２９】図２９は、多角形照合の概念を説明する図である。
【図３０】図３０は、ポールスタービットを説明する図である。
【図３１】図３１は、ポールスターデータベース行列の作成を説明する図である。
【図３２】図３２は、ポールスター特徴量とポールスタービットを説明する図である。
【図３３】図３３は、ポールスターデータベース行列による演算式を示す図である。
【図３４】図３４は、尤度を±１と設定した場合のポールスターデータベースを説明する図である。
【図３５】図３５は、画像点と対応する照合点候補を説明する図である。
【図３６】図３６は、ｉｍｇＡｒｍＬｓＭｔｘを示す図である。
【図３７】図３７は、ｆｌｒＡｒｍＬｓＭｔｘを示す図である。
【図３８】図３８は、ソースコードの一例を示す図である。
【図３９】図３９は、ｍｃｈＡｒｍＬｓＭｔｘを示す図である。
【図４０】図４０は、列方向の和算の繰り返しを説明する図である。
【図４１】図４１は、残った画像点群−データベース点群の組を示す図である。
【図４２】図４２は、位置・方向を求める行列演算式を示す図である。
【図４３】図４３は、アドレス行列の作成を説明する図である。
【図４４】図４４は、照合率の計算を説明する図である。
【発明を実施するための形態】
【００３５】
以下に、本発明に係る移動体位置検出システムおよび方法の実施例を図面に基づいて詳細に説明する。なお、この実施例によりこの発明が限定されるものではない。
【００３６】
図１に示すように、本発明に係る移動体位置検出システム１０は、建物内の床面（平面）１２上に配置されたドットパターン１４と、移動体１６に備えられ、移動体１６とともに床面１２上を移動する移動体位置検出装置１８とから構成されている。ここで、移動体１６とは、例えば搬送台車、ロボット、人間等である。ドットパターン１４は、印刷や描画等の手段によって床面１２上にランダムに配置されている。
【００３７】
移動体位置検出装置１８は、移動体１６が位置する床面１２上の領域を撮像する撮像手段２０と、ポールスター特徴量格納手段２２と、ポールスター照合手段２４と、絞り込み手段２６と、撮像手段２０によって撮像した画像に基づいて移動体１６の位置を検出する検出手段２８と、を備える。
【００３８】
撮像手段２０は、ＣＣＤカメラ等の画像取得装置である。撮像手段２０は移動体１６に固定されており、移動体１６とともに移動しながら、移動体１６の位置する床面１２上の領域を所定の時間間隔で撮像し、図２に示したような画像を取得する。本実施の形態では、高速度カメラを用いて、例えば１０ｍｓ程度の時間間隔で画像を取得している。また、図１に示すように、撮像手段２０と床面１２との間の距離は数センチメートルであり、照明３０を用いることで、外乱光の影響の少ない画像を取得することができる。
【００３９】
ポールスター特徴量格納手段２２は、図３に示すように、移動体１６の検出を行う前に、床面１２上のドットパターン１４におけるすべてのドット３２のポールスター特徴量を予め算出し、これをポールスターデータベース３４に格納するためのものである。
【００４０】
ポールスター照合手段２４は、取得画像中の複数のドット３２から所定のドット３２ａを選択し、ドット３２ａや周辺に位置する複数のドット３２ｂ１〜３２ｂ５等から得られるポールスター特徴量を、予め作成したポールスターデータベース３４と照合するためのものである。
【００４１】
ここで、このポールスター照合手段２４は、尤度を設定したポールスター特徴量を用いて、ポールスター照合を行うものである。ここで、この尤度は、撮像手段２０により取得した画像中のドットと、このドットに対応するポールスターアルゴリズムのポールスターデータベース３４中のドットとの間の誤差を吸収するためのものである。
【００４２】
絞り込み手段２６は、ポールスター照合手段２４により照合された照合ドット候補から所定の照合ドットに絞り込むためのものである。
【００４３】
検出手段２８は、絞り込み手段２６により絞り込んだ所定の照合ドットからなるドットパターンの位置関係に基づいて、移動体１６の床面１２上での現在位置および方向を検出するというものである。
【００４４】
以下に、本発明の実施例１〜４について説明する。
【００４５】
［実施例１］
図４に示すように、本発明の実施例１は、画像取得して画像中の点にラベリングをし（ステップＳ１１）、この画像点群を用いてポールスター照合手段２４で尤度を導入したポールスター照合を行い（ステップＳ１２）、照合床点候補群について絞り込み手段２６で点群相互間距離の組み合わせの照合を行って（ステップＳ１３）、検出手段２８でカメラ位置・方向の計算をし（ステップＳ１４）、照合率による確認をする（ステップＳ１５）という手順である。
【００４６】
次に、実施例１の具体的な処理手順について図３０〜図４４を参照しながら説明する。
（１）ポールスター特徴量のビット表現
図３０に示すように、ポールスター特徴量をビットで表現したポールスタービットを作成する。
【００４７】
（２）ポールスターデータベース行列（以下、ＰＳＤ行列という。）の作成
図３１に示すように、各データベース点（Ｐ１〜Ｐｍａｘ）のポールスター特徴量を計算する。それぞれビット表現して縦方向に並べることで、ＰＳＤ行列を作成する。
【００４８】
（３）画像点のポールスター照合
図３２に示すように、ある画像点についてポールスター特徴量を計算する。
次に、前述のＰＳＤ行列を用いて、図３３に示すような演算を行う。各データベース点と注目している画像点のアーム長インデックスの一致数が一致アームカウンタとして計算される。一致アームカウンタの中で、最大値をとる要素が、照合点となる。
【００４９】
（４）尤度の追加
実際の各画像点には誤差があるので、ポールスター特徴量のアーム長インデックスもポールスターデータベース中の対応データベース点とは異なっている。この誤差を吸収するために、ポールスターデータベースに尤度（幅）を持たせる。
【００５０】
例えば、図３４に示すように、ＰＳＤ行列の各データベース点のポールスタービットに対して１に隣接する左右のビットに尤度に応じた幅の１を設定することで実現する。
【００５１】
このことにより、誤差は吸収され、誤差のために画像点のポールスタービットの１の位置が尤度範囲内でずれたとしても、一致アームカウンタの対応するデータベース点の位置に正照合時と同じアーム長インデックス一致数が出力される。ただし、アーム長インデックスの一致は全体として増加するため、最大一致数をとるデータベース点が正しい対応データベース点以外に複数あらわれることが考えられる。
【００５２】
（５）ポールスター照合後の絞込み
データベースによる照合時点では、一つの画像点に対して複数のデータベース点が照合する可能性がある。また、ゴミや点の消失などがあると照合したデータベース点の中には間違えが紛れ込んでいる可能性もある。そこで、正しく対応している画像点群とデータベース点群の間では、任意の２つの画像点の距離とその画像点に対応する２つのデータベース点の距離は一致することを利用して、多くの照合点候補から正しい照合点を絞り込む。
【００５３】
（５−１）画像点群データとデータベース点群データの準備
図３５に示すように、データベース点候補と画像点が一対一となるように画像点群データとデータベース点群データを準備する。
【００５４】
画像点は、データベース点候補に対応して設定する。つまり、１つの画像点に複数のデータベース点候補があった場合には、画像点も同一の画像点をデータベース点候補に対応して複数設ける。
【００５５】
画像点間の相互距離を計算して、図３６に示すような行列ｉｍｇＡｒｍＬｓＭｔｘが得られる。また、対応する照合データベース点間の相互距離を計算して、図３７に示すような行列ｆｌｒＡｒｍＬｓＭｔｘが得られる。ここで、各要素は、行に相当する点と列に相当する点の間の距離を表す。ただし、同一の画像点に対応したデータベース点間の距離はＮＡとする。
【００５６】
次に、ｉｍｇＡｒｍＬｓＭｔｘと、ｆｌｒＡｒｍＬｓＭｔｘで距離差を求め、尤度の範囲内の場合には要素を１とする。この演算は、例えば、図３８に示すようなソースコードを用いて行うことができる。なお、ＮＡの要素については、０とする。この演算により、図３９に示すような行列ＭｃｈＡｒｍＬｓＭｔｘが得られる。
【００５７】
図４０に示すように、ＭｃｈＡｒｍＬｓＭｔｘを列方向に和算すると、各データベース点についてそれを起点とした他のデータベース点までの距離が、対応する画像点と他の画像点までの距離と一致した数が算出される。行列の中で、距離の一致数が最も少ない行および列を削除する。
【００５８】
再度、列方向に和算すると、残ったデータベース点に対して同様の一致数が計算される。この操作を全ての一致数が等しくなるまで繰り返す。
【００５９】
画像点間相互距離と対応するデータベース点間相互距離が全て一致する場合には、一致数は、点数−１となり、残った画像点群とデータベース点群の組は、それぞれの点群の相対位置関係が幾何学的に一致していると考えられる。なお、これは鏡映関係の場合も含む。
【００６０】
（６）カメラ位置・方向の算出
図４１に示すように、画像点群と床点群の座標値を用いてカメラの位置・方向を算出する。図４２に示すような演算式から、床座標系からみた画像座標原点の位置・方向が求められる。
【００６１】
画像点群の位置・方向からの移動体の位置および方向は、以下の演算式により算定することができる。
位置＝［ｘ，ｙ］
方向θ＝ｔａｎ−１（（−ｃ１×ｃ２／ｓ１×ｓ２）１／２）
【００６２】
（７）照合率の計算
（ア）アドレス行列の作成（オフライン）
図４３に示すように、事前に床を升目状のブロックにエリア分割し、ブロック単位でアドレスを振る。各アドレスでの点の有無を調べて、アドレスに対応した行列（以下、アドレス行列という。）を用意し、アドレスでの点が有る場合には１を、ない場合には０を代入しておく。
【００６３】
（イ）照合率
図４４に示すように、求められた移動体位置・方向を元に、画像中の画像点全てを床座標系へ座標変換する。変換された座標に対応するアドレス行列に点が存在する割合を求めて、照合率とする。
【００６４】
このため、本発明の実施例１によれば、点分布照合に幅を持たせることで照合過程で正しい照合が排除されることを防ぎ、従来のアルゴリズムで発生するおそれがある誤照合や照合不能といった照合不良を起こし難くすることができる。また、取得画像中の点群相互間距離と床の照合点候補群相互間距離がすべて一致する組み合わせを求める手法により、画像点群と床照合点群の組を抽出し、多照合といった照合不良を起こし難くすることができる。
【００６５】
また、照合結果としてのカメラ位置方向検出値を使って、画像中の全点（画像座標系）を床座標系へ座標変換し、各点の座標位置に対応したアドレス行列の要素が１である割合（＝照合率）を算出する。この割合が規定以下の場合には不確実な検出として排除することで、出力値の信頼性を確保することができる。
【００６６】
また、本発明では自然発生的なランダム点群パターンを位置検出に利用できるため、以下の（１）、（２）の効果が見込まれる。
【００６７】
（１）チップ材混入による床パターンへの応用
工場などで利用が多い塗り床のように、床上への印刷が困難、あるいは、コストがかかるものに対しても、塗り床材にチップを混入することによりランダム点群パターンを作成することで、位置検出用の床として利用することが可能となる。また、床タイルについても、製造工程の中でチップ材を混入し表面にランダム点群パターンを作成することができるタイプのものがあるが、こういったタイプの床タイルにも適用可能となる。
【００６８】
（２）広い床への対応
一般にタイル印刷の場合には、印刷の版の種類が限定的になるため、広い範囲で異なる点分布パターンを実現することは困難である。自然発生的なランダム点群パターンでは、類似性のない点分布が広い範囲で実現できるため、広い床における位置検出に対応することができる。
【００６９】
［実施例２］
本発明の実施例２は、正しい候補点群は、撮像中の点分布の範囲に収まって分布していることを利用して、正しい候補点のあるデータベース中の床領域を絞り込むものである。
【００７０】
具体的の処理手順としては、図５に示すように、本発明の実施例２は、画像取得して画像中の点にラベリングをし（ステップＳ２１）、この画像点群を用いてポールスター照合手段２４で尤度を導入したポールスター照合を行い（ステップＳ２２）、照合床点候補群１について絞り込み手段２６で床点分布可能エリアの計算を行い（ステップＳ２３）、各分布可能エリアを求め（ステップＳ２４）、分布可能エリア内の床点を抽出する（ステップＳ２５）。
【００７１】
そして、照合床点候補群２について絞り込み手段２６で点群相互間距離の組み合わせの照合を行って（ステップＳ２６）、検出手段２８でカメラ位置・方向の計算をし（ステップＳ２７）、照合率の計算と最大照合率エリアの抽出を行って（ステップＳ２８）、最大照合率のエリアのカメラ位置・方向を求め（ステップＳ２９）、最大照合率の確認をする（ステップＳ３０）という手順である。
【００７２】
つまり、絞り込み手段２６が、ポールスター照合手段２４により照合された照合ドット候補について照合ドット候補が存在する所定の共通領域を求め、共通領域内に存在する照合ドットに絞り込む処理を含むようにしたものである。
【００７３】
ところで、上記の実施例１の処理では、場合によっては、多照合による必要な計算メモリが増大する可能性が考えられる。つまり、取得画像中の画像点数が少ない、あるいは、画像点の点分布が一様に近い場合には、ポールスター特徴量を構成するアーム長インデックスの数が少ないケースなどが発生する。この場合、データベースの多くの点が照合候補となるため、多くの計算メモリが必要となり、実質上計算の継続が困難になる場合があった。
【００７４】
そこで、この実施例２の処理においては、正しい候補点群は、撮像中の点分布の範囲に収まって分布していることを利用して、正しい候補点のあるデータベース中の床領域を絞り込む。任意の画像点を中心とし、それと最も遠い距離にある画像点までの直線を半径とする円内に、他の画像点が存在している。このとき、対応するデータベース点についても同様の関係が成り立つことを利用して、対応するデータベース点の存在する領域を求め、照合候補データベース点をその領域内のものに制限することで絞り込みを行う。このようにすることで、上記の実施例１の課題を解決することができる。
【００７５】
［照合点存在共通領域］
例えば、図６に示すように、画像点Ｃ１を中心として最も遠い距離にある点はＣ４であり、その距離がＲだったとすると、Ｃ１に対応するデータベース点ｆ１を中心として半径Ｒの円内に他画像点に対応するデータベース点は存在する。したがって、各画像点に対して、同様の他点群存在範囲半径を求め、対応する各照合候補データベース点を中心とした円を描き、その和領域を照合点存在領域とすると、正しく対応する点群は、各画像点に対する照合点存在領域の重なりが最も多く重なっている領域（＝照合点存在共通領域）に入っている。
【００７６】
ステップＳ２２のポールスター照合＋尤度の処理後の照合点候補についてこの共通領域を求め、共通領域内に存在する点に絞り込むことで、照合点候補を大幅に削減することができる。
【００７７】
［重なり数の設定］
また、ゴミ点が画像点に紛れ込んでいる場合には、誤った照合点存在領域があるため、最大の重なり領域では、正しい対応データベース点が排除される可能性があるため、照合点存在共通領域とする最小の重なり数は、画像点数から混入するであろうゴミの数を引いた数より小さく設定する。
【００７８】
［複数共通領域の取り扱い］
照合点存在共通領域は、複数存在する可能性がある。その場合には、各領域について個別に照合候補点群を求め、照合率の計算を行う。最後に各領域での照合率の計算結果から、最も照合率の高い領域の照合点群を答えとする。
【００７９】
この実施例２によれば、多照合により必要な計算メモリの増大を抑えることができるために、以下の効果を奏することができる。
（１）広い床範囲を対象とした位置検出が可能となる。
（２）比較的位置誤差の大きな画像点入力でも位置検出が可能となる。
（３）画像分解能が低いカメラでも位置検出が可能となる。
【００８０】
［実施例３］
本発明の実施例３は、照合するデータベースの床エリアをあらかじめ分割し、その分割エリア内での照合を行うものである。全分割エリアの照合後、計算された各分割エリアでの照合率を元に、最も照合率の高いエリアの結果を正しい答えとして出力する。
【００８１】
具体的の処理手順としては、図７に示すように、画像取得して画像中の点にラベリングをし（ステップＳ３１）、ポールスター照合手段２４で探索床エリアを分割するとともに（ステップＳ３２）、各探索エリアについて尤度を導入したポールスター照合を行う（ステップＳ３３、Ｓ３４）。
【００８２】
そして、照合床点候補群２について絞り込み手段２６で点群相互間距離の組み合わせの照合を行って（ステップＳ３５）、照合率の計算と最大照合率エリアの抽出を行って（ステップＳ３６）、検出手段２８で最大照合率のエリアのカメラ位置・方向を求め（ステップＳ３７）、最大照合率の確認をする（ステップＳ３８）という手順である。
【００８３】
つまり、ポールスター照合手段２４が、ポールスターデータベース３４における照合範囲を複数の領域に分割してこの各領域でポールスター照合を行い、照合率が最大となる領域を求めるものである。
【００８４】
ところで、上記の実施例１の処理では、場合によっては、多照合による必要な計算メモリが増大する可能性が考えられる。つまり、取得画像中の画像点数が少ない、あるいは、画像点の点分布が一様に近い場合には、ポールスター特徴量を構成するアーム長インデックスの数が少ないケースなどが発生する。この場合、データベースの多くの点が照合候補となるため、多くの計算メモリが必要となり、実質上計算の継続が困難になる場合があった。
【００８５】
しかしながら、この実施例３の処理においては、照合するデータベースの床エリアをあらかじめ分割し、その分割エリア内での照合を行い、全分割エリアの照合後、計算された各分割エリアでの照合率を元に、最も照合率の高いエリアの結果を正しい答えとして出力するので、上記の実施例１の課題を解決することができる。
【００８６】
［照合床エリアの分割］
図８に示すように、対象とする照合床エリアを適当な大きさに分割する。例えば、画像視野の２５倍くらいの大きさとする。
【００８７】
［照合および結果］
各照合床分割エリアで、ポールスター照合＋尤度および点群相互間距離組み合わせ照合を行い、照合率を計算する。最大の照合率を得た領域の検出値を答えとする。
【００８８】
この実施例３によれば、多照合により必要な計算メモリの増大を抑えることができるために、以下の効果を奏することができる。
（１）広い床範囲を対象とした位置検出が可能となる。
（２）比較的位置誤差の大きな画像点入力でも位置検出が可能となる。
（３）画像分解能が低いカメラでも位置検出が可能となる。
【００８９】
［実施例４］
本発明の実施例４は、上記の実施例３の照合床エリアの分割と、実施例２の点群存在共通領域の絞り込みを組み合わせた手法であり、上記の実施例１の課題を解決することができる。
【００９０】
具体的な処理手順としては、図９に示すように、画像取得して画像中の点にラベリングをし（ステップＳ４１）、ポールスター照合手段２４で探索床エリアを分割するとともに（ステップＳ４２）、各探索エリアについて尤度を導入したポールスター照合を行う（ステップＳ４３、Ｓ４４）。照合床点候補群１について絞り込み手段２６で床点分布エリアの計算を行い（ステップＳ４５）、各分布エリアを求め（ステップＳ４６）、分布エリア内の床点を抽出する（ステップＳ４７）。
【００９１】
そして、照合床点候補群２について点群相互間距離の組み合わせの照合を行って（ステップＳ４８）、検出手段２８でカメラ位置・方向の計算をし（ステップＳ４９）、照合率の計算と最大照合率エリアの抽出を行って（ステップＳ５０）、最大照合率のエリアのカメラ位置・方向を求め（ステップＳ５１）、探索エリアでの最大照合率とその時のカメラ位置・方向の保存を行い（ステップＳ５２、Ｓ５３）、最大照合率の確認をする（ステップＳ５４）という手順である。
【００９２】
つまり、照合床エリアを適切な大きさで分割して、各照合床分割エリアの照合について、尤度を考慮したポールスター照合の後、点群存在共通領域（複数）を計算する。各点群存在共通領域内の照合候補点群について点群相互間距離組み合わせ照合を行い、カメラの位置・方向の計算を実施後、照合率の計算をする。全ての点群存在領域について計算し、当該の照合分割エリアでの最大照合率とその時のカメラ位置・方向とする。これを全ての照合分割エリアについて実施し、最終的に、全ての照合分割エリアで最も照合率の高かったカメラの位置・方向を検出結果とする。
【００９３】
この実施例４によれば、多照合により必要な計算メモリの増大を抑えることができるために、以下の効果を奏することができる。
（１）広い床範囲を対象とした位置検出が可能となる。
（２）比較的位置誤差の大きな画像点入力でも位置検出が可能となる。
（３）画像分解能が低いカメラでも位置検出が可能となる。
【００９４】
次に、本発明の応用例１、応用例２について説明する。本発明に係る移動体位置検出システム１０は、病院や工場等における搬送システムに応用可能である。
【００９５】
［本発明の応用例１］
まず、本発明の応用例１として、ロボットを用いた病院デリバリーサービスに応用する場合について説明する。
【００９６】
まず、従来技術の課題について説明する。従来、病院の入院患者が雑誌などの商品を購入したい場合には、通常、患者自身が病院内の売店に移動して購入するが、大型病院においては、患者が居る病室から売店までの距離が遠い場合があり、患者にとって利便性が低い。また、患者が自力で移動できない場合もある。こうした環境下にある患者に対応するために、売店での購入・配達を代行するデリバリーサービスを導入している病院がある。しかし、このような病院においては、サービスに係る人件費が発生したり、サービスを享受できる人数に限りがあり、患者に迅速に配達できないなどの問題があった。
【００９７】
そこで、本発明に係る移動体位置検出システム１０をこのデリバリーサービスに応用し、患者に対する新たなサービスを提供する。具体的には、移動体位置検出システム１０における移動体１６を商品配達ロボットして用い、患者は、病室のベッドサイドに設けた端末機器からデリバリーしたい商品を注文する。すると、売店のロボットは、注文を受けた商品を、端末機器の置かれたベッドサイドまで配達するように移動する。この移動は、例えば、売店から端末機器までの移動ルートを、床面上に描いたドットパターンを用いて端末機器毎に紐付けしておき、所定の端末機器からの注文信号を受けた際に、その所定の端末機器にロボットが自動的に移動するように設定しておくことで可能である。これにより、病室のベッドサイドまでロボットが入って行くことができる。なお、ロボットの移動ルートや移動に関するルールは適宜自由に設定してもよい。
【００９８】
このように、本発明を病院内のデリバリーサービスに適用すると、ロボットを床面に描いたドットパターンで誘導することができる。このため、従来必要であった配達ロボットを誘導するための誘導線を廊下、病室内に敷設する大掛かりな工事は必要なくなる。また、床面に描いたドットパターンで誘導することから、将来的な建物内のレイアウト変更にも柔軟に対応することができる。また、人ではなくロボットが配達することから、従来の病院内のデリバリーサービスに比べて人件費が掛からず、ランニングコストを抑えることができる。さらに、ロボットによる配達距離を短くすることで、リードタイムを短縮でき、患者の満足度を向上することができる。
【００９９】
［本発明の応用例２］
次に、本発明の応用例２として、平倉庫フリーロケーション管理に応用する場合について説明する。
【０１００】
まず、従来技術の課題について説明する。従来、平倉庫において固定棚を用いずにパレット等を保管する場合の在庫ロケーション管理方法として主に以下の技術が知られている。
【０１０１】
（１）図１０に示すように、床にライン５０を引き、エリアを区画して柱５２に合わせた柱番号などで保管エリアＩＤを認識し、荷物と紐付けを行う。
【０１０２】
（２）図１１に示すように、決められた荷姿の荷物６０（パレットなど）に合わせてグリッドを設定し、その天井に固有のＩＤを発信する超音波発信器６２を設置する。フォークリフト７０で荷物６０をグリッドに置いたときのその天井にある超音波発信器６２から発信されるＩＤを、フォークリフト７０に設置したリーダー６４でロケーションを読み取り、荷物と紐付けを行う。
（３）上記の（２）と同様であるが、図１１に示すように、超音波発信器６２でなく床にパッシブ式ＲＦＩＤ６６を埋め込み、フォークリフト７０に設置したリーダー６８でＩＤを読み取る。
【０１０３】
しかし、上記の（１）では、エリアには複数の荷物が存在するので、荷物の詳細な位置を迅速に把握することができない。また、人手による作業を伴うので人為的ミスによるロケーションの間違いが発生するおそれがある。
【０１０４】
また、上記の（２）、（３）では、図１２（ａ）に示すように、荷物を一定方向に向けて配置する必要がある。荷物６０とフォークリフト７０に取り付けたリーダー６４（６８）の位置に差があるためである。図１２（ｂ）に示すように、横から置くと超音波発信器６２やＲＦＩＤ６６の位置とリーダー６４、６８に位置ずれが生じ対応できなくなる。さらに、図１３（ａ）に示すように、荷物の荷姿が同じ形態でなければならず、図１３（ｂ）に示すように、超音波発信器６２やＲＦＩＤ６６の配置に対して、異なる寸法のパレットの混在やパレットとカゴ車など別の種類の荷物には対応できない。
【０１０５】
そこで、本発明に係る移動体位置検出システム１０をこの平倉庫フリーロケーション管理に応用する。具体的には、図１４および図１５に示すように、移動体位置検出システム１０における移動体１６をフォークリフト７０や人として、カメラ（撮像手段２０）をこれらに取り付ける。一方、床面または天井面にドットパターン３２を描画して、カメラ２０が撮像した画像によりフォークリフト７０や人の位置や向きを検出し、平倉庫のどこに荷物を置いたかを自動的に認識する。
【０１０６】
また、荷物６０の平面中心とカメラ２０の距離を予め所定の値に設定しておき、カメラ２０の撮像画像によって荷物の中心を認識させることで、図１６（ａ）、（ｂ）に示すように、どの方向からも荷物の位置を認識できるようになり、形態が異なる荷物を様々な方向から平倉庫内に置くことができる。このため、平倉庫の利用態様の自由度が高まり、入出庫作業の効率化が図れる。また、上記の従来技術においては、施工時にパレットの数だけ超音波発信器またはＲＦＩＤを設置して、ＩＤとロケーションを登録する必要があったが、これをする必要がなくなる。
【０１０７】
以上説明したように、本発明によれば、平面上を移動する移動体の位置を検出する移動体位置検出システムであって、前記平面上に配置された複数のドットからなるドットパターンと、前記移動体に備えられ、前記移動体が位置する平面上の領域を撮像する撮像手段と、前記撮像手段により取得した画像中のドットと、このドットに対応するポールスターアルゴリズムのポールスターデータベース中のドットとの間の誤差を吸収するための尤度を設定したポールスター特徴量を用いてポールスター照合を行うポールスター照合手段と、前記ポールスター照合手段により照合された照合ドット候補から所定の照合ドットに絞り込む絞り込み手段と、前記絞り込み手段により絞り込んだ所定の照合ドットからなるドットパターンの位置関係に基づいて、前記移動体の現在位置および方向を検出する検出手段と、を備える。このため、ポールスターアルゴリズムを用いた際の照合不良を起こし難くして、検出の信頼性を確保することができる。
【産業上の利用可能性】
【０１０８】
以上のように、本発明に係る移動体位置検出システムおよび方法は、建物内の床面上などの平面上に配置した移動体の位置検出に有用であり、特に、位置検出アルゴリズムとしてポールスターアルゴリズムを用いた際の照合不良を起こし難くして、検出の信頼性を確保するのに適している。
【符号の説明】
【０１０９】
１０  移動体位置検出システム
１２  床面（平面）
１４  ドットパターン
１６  移動体
１８  移動体位置検出装置
２０  撮像手段
２２  ポールスター特徴量格納手段
２４  ポールスター照合手段
２６  絞り込み手段
２８  検出手段
３０  照明
３２  ドット
３４  ポールスターデータベース
【図１】
0005483090.tif 000002
【図２】
0005483090.tif 000003
【図３】
0005483090.tif 000004
【図４】
0005483090.tif 000005
【図５】
0005483090.tif 000006
【図６】
0005483090.tif 000007
【図７】
0005483090.tif 000008
【図８】
0005483090.tif 000009
【図９】
0005483090.tif 000010
【図１０】
0005483090.tif 000011
【図１１】
0005483090.tif 000012
【図１２】
0005483090.tif 000013
【図１３】
0005483090.tif 000014
【図１４】
0005483090.tif 000015
【図１５】
0005483090.tif 000016
【図１６】
0005483090.tif 000017
【図１７】
0005483090.tif 000018
【図１８】
0005483090.tif 000019
【図１９】
0005483090.tif 000020
【図２０】
0005483090.tif 000021
【図２１】
0005483090.tif 000022
【図２２】
0005483090.tif 000023
【図２３】
0005483090.tif 000024
【図２４】
0005483090.tif 000025
【図２５】
0005483090.tif 000026
【図２６】
0005483090.tif 000027
【図２７】
0005483090.tif 000028
【図２８】
0005483090.tif 000029
【図２９】
0005483090.tif 000030
【図３０】
0005483090.tif 000031
【図３１】
0005483090.tif 000032
【図３２】
0005483090.tif 000033
【図３３】
0005483090.tif 000034
【図３４】
0005483090.tif 000035
【図３５】
0005483090.tif 000036
【図３６】
0005483090.tif 000037
【図３７】
0005483090.tif 000038
【図３８】
0005483090.tif 000039
【図３９】
0005483090.tif 000040
【図４０】
0005483090.tif 000041
【図４１】
0005483090.tif 000042
【図４２】
0005483090.tif 000043
【図４３】
0005483090.tif 000044
【図４４】
0005483090.tif 000045