JP2004012429A - 自己位置／姿勢同定装置、及び自己位置／姿勢同定方法 - Google Patents

Abstract

【課題】天井に蛍光灯やマーカのような特別な構造体を設けなくても室内移動体の位置及び姿勢の同定が可能な自己位置／姿勢同定装置を提供する。
【解決手段】本発明による自己位置／姿勢同定装置は、部屋の内部で使用される移動体（２）の位置、及び／又は姿勢を同定する自己位置／姿勢同定装置である。当該自己位置／姿勢同定装置は、前記移動体（２）に搭載されたカメラ（３）と、複数の事前撮像データを保持するテンプレート画像データベース（７ａ）と、カメラ（３）によって撮像された撮像画像と前記複数の事前撮像データとに基づいて、前記撮像画像が撮像された時点における前記移動体（２）の位置、及び／又は姿勢を同定する演算装置（６）とを備えている。カメラ（３）は、部屋（１）の床面（１ａ）と実質的に垂直である天井方向に向けられている。複数の事前撮像データは、前記移動体（２）の位置、及び／又は姿勢の同定の前に前記カメラ（３）によって前記部屋（１）の複数の位置からそれぞれ撮像された複数の事前撮像画像からそれぞれ生成される。
【選択図】　図１

Description

【０００１】
【発明の属する技術分野】
本発明は、自己位置／姿勢同定装置に関する。本発明は、特に、部屋の天井が撮像された撮像画像を用いて自己の位置及び姿勢を同定する自己位置／姿勢同定装置に関する。
【０００２】
【従来の技術】
室内で使用される室内移動体の自律制御のために、自己の位置及び姿勢を同定する技術の開発が進められている。かかる技術の開発は、介護ロボットや家事ロボットのような、屋内で使用されるロボットの実用化の上で重要である。
【０００３】
室内移動体の位置及び姿勢の同定のために、天井を撮像した天井画像を用いる手法が広く用いられている。天井は、人や障害物が室内に存在しても移動体から隠れにくい。天井を撮像した天井画像の利用は、自己の位置及び姿勢を確実に同定することを可能にする。
【０００４】
例えば、公開特許公報（特開平６−４１２７）には、天井画像から天井に設けられた蛍光灯の位置を算出し、算出された蛍光灯の位置を元に自己の位置を算出する屋内移動体の自己位置測定装置が開示されている。
【０００５】
更に、公開特許公報（特開平１０−１７７４１４）には、移動体に搭載されたカメラによって撮像された天井画像から、該移動体の進行方向及び該移動体の位置を検出する走行状態認識装置が開示されている。該走行状態認識装置は、該天井画像から該移動体が進行すべき方向の直線成分を取り出し、更に、該直線成分から該移動体の進行方向及び該移動体が走行する通路上の横方向の位置を算出する。該走行状態認識装置は、更に、天井に設けられたマーカを用いて、該移動体の位置を算出する。
【０００６】
更に、”天井画像列を用いた屋内ナビゲーション”（紙　弘和他，第１８回日本ロボット学会学術講演会予稿集ｐｐ．３３１−３３２）には、天井画像列を利用したナビゲーション手法が開示されている。該ナビゲーション手法では、まず、移動体の教示走行が行われ、該教示走行の間に画像列が記録される。記録された画像と、該移動体の現在の画像との単純な比較を行うことで、該移動体の自己位置同定とナビゲーションとが行われる。
【０００７】
天井に蛍光灯やマーカのような特徴的な構造体が設けられていなくても、室内移動体の位置及び姿勢を同定できることが望まれる。
【０００８】
更に、天井に特徴が少ない環境でも、室内移動体の位置及び姿勢を同定できることが望まれる。
【０００９】
更に、室内移動体の位置及び姿勢は、一層に正確に同定されることが望まれる。
【００１０】
更に、室内移動体の位置及び姿勢を同定するために必要な演算量、及びデータ量が抑制されることが望まれる。
【００１１】
【発明が解決しようとする課題】
本発明の目的は、天井に蛍光灯やマーカのような特別な構造体を設けなくても室内移動体の位置及び姿勢の同定が可能な自己位置／姿勢同定装置を提供することにある。
【００１２】
本発明の他の目的は、天井に特徴が少ない環境でも、室内移動体の位置及び姿勢の同定が可能な自己位置／姿勢同定装置を提供することにある。
【００１３】
本発明の更に他の目的は、室内移動体の位置及び姿勢を一層に正確に同定できる自己位置／姿勢同定装置を提供することにある。
【００１４】
本発明の更に他の目的は、室内移動体の位置及び姿勢を同定するために必要な演算量、及びデータ量を抑制することが可能な自己位置／姿勢同定装置を提供することにある。
【００１５】
【課題を解決するための手段】
以下に、［発明の実施の形態］で使用される番号・符号を用いて、課題を解決するための手段を説明する。これらの番号・符号は、［特許請求の範囲］の記載と［発明の実施の形態］の記載との対応関係を明らかにするために付加されている。但し、付加された番号・符号は、［特許請求の範囲］に記載されている発明の技術的範囲の解釈に用いてはならない。
【００１６】
本発明による自己位置／姿勢同定装置は、部屋（１）の内部で使用される移動体（２）の位置、及び／又は姿勢を同定する自己位置／姿勢同定装置である。当該自己位置／姿勢同定装置は、前記移動体（２）に搭載されたカメラ（３）と、複数の事前撮像データ（Ｍ^ｉｎｉ _ｉ，ｊ，Ｍ^ｉｎｉ _{ｉ，ｊ，ｋ}）を保持するテンプレート画像データベース（７ａ，７ａ’）と、カメラ（３）によって撮像された撮像画像と前記複数の事前撮像データ（Ｍ^ｉｎｉ _ｉ，ｊ，Ｍ^ｉｎｉ _{ｉ，ｊ，ｋ}）とに基づいて、前記撮像画像が撮像された時点における前記移動体（２）の位置、及び／又は姿勢を同定する演算装置（６）とを備えている。カメラ（３）は、部屋（１）の床面（１ａ）と実質的に垂直である天井方向に向けられている。複数の事前撮像データ（Ｍ^ｉｎｉ _ｉ，ｊ，Ｍ^ｉｎｉ _{ｉ，ｊ，ｋ}）は、前記移動体（２）の位置、及び／又は姿勢の同定の前に前記カメラ（３）によって前記部屋（１）の複数の位置からそれぞれ撮像された複数の事前撮像画像からそれぞれ生成される。当該自己位置／姿勢同定装置は、カメラ（３）によって撮像された撮像画像と複数の事前撮像画像との対比によって前記移動体（２）の位置、及び／又は姿勢が同定され、天井に蛍光灯やマーカのような特別な構造体を設けなくても室内移動体の位置及び姿勢の同定が可能である。
【００１７】
複数の事前撮像データ（Ｍ^ｉｎｉ _ｉ，ｊ，Ｍ^ｉｎｉ _{ｉ，ｊ，ｋ}）は、複数の事前撮像画像そのものを示すデータであることが可能であり、また複数の事前撮像画像を加工することによって得られるデータであることも可能である。
【００１８】
当該自己位置／姿勢同定装置において、前記事前撮像画像が撮像される時の前記移動体（２）の姿勢は、一定であり、前記演算装置（６）は、前記撮像画像を互いに異なる角度に回転した複数の回転画像と前記事前撮像画像とを対比することにより、前記移動体（２）の位置及び姿勢を同定することが好ましい。前記撮像画像の回転画像と前記事前撮像画像とが対比されるため、前記移動体（２）の姿勢を変更して事前撮像画像を撮像する必要がない。
【００１９】
演算装置（６）の演算量を減少させるためには、当該前記複数の事前撮像画像は、同一の位置から前記移動体（２）の姿勢が異なる状態で撮像された異姿勢事前撮像画像を含むことが好ましい。このような自己位置／姿勢同定装置は、姿勢が異なる状態で事前撮像画像が撮像されているため、前記移動体（２）の位置及び姿勢の同定のために、前記撮像画像を回転する回転処理を行う必要がない。
【００２０】
前記複数の事前撮像画像は、互いに異なる環境条件で撮像された異環境条件撮像画像を含むことが好ましい。これにより、部屋（１）の環境条件が変化しても、前記移動体（２）の位置及び姿勢を正確に同定することができる。
【００２１】
前記カメラ（３）の撮像対象は、前記複数の事前撮像画像と前記撮像画像との両方において、実質的に円形である有効撮像領域（Ｒ_ｅｆｆ）に写される場合がある。この場合、前記複数の事前撮像データ（Ｍ^ｉｎｉ _ｉ，ｊ，Ｍ^ｉｎｉ _{ｉ，ｊ，ｋ}）は、前記有効撮像領域（Ｒ_ｅｆｆ）の中心を原点として定められる極座標系の円周方向に前記複数の事前撮像画像をそれぞれ微分することによって得られる複数の事前撮像エッジ画像から生成され、前記演算装置（６）は、前記撮像画像を前記極座標系の前記円周方向に微分することによってエッジ画像を生成し、前記エッジ画像と前記複数の事前撮像データ（Ｍ^ｉｎｉ _ｉ，ｊ，Ｍ^ｉｎｉ _{ｉ，ｊ，ｋ}）とに基づいて前記移動体（２）の位置、及び／又は姿勢を同定することが好ましい。円周方向に微分されて生成されたエッジ画像には、主として、実空間における天井方向の構造体のエッジが抽出されることになる。人工建造物の内部において天井方向に延伸する構造体は、その位置の変動が少ないものが多く存在する。例えば、隣接する２つの壁により形成される稜線や、柱は、典型的な、天井方向に延伸する構造体である。前記事前撮像画像から生成された前記事前撮像エッジ画像と、前記撮像画像とから生成されたエッジ画像には、いずれも、位置の変動が小さい構造体のエッジが抽出されることになる。前記事前撮像エッジ画像と前記エッジ画像を用いて移動体（２）の位置及び姿勢の同定を行うことにより、移動体（２）の位置及び姿勢の同定の精度を向上することができる。
【００２２】
この場合、カメラ（３）は、魚眼レンズを含み、前記撮像画像と前記複数の事前撮像画像とは、いずれも、前記魚眼レンズを通して撮影されることが好ましい。これにより、天井方向に延伸する構造体が、より確実に前記撮像画像と前記複数の事前撮像画像とに写される。
【００２３】
前記複数の事前撮像データ（Ｍ^ｉｎｉ _ｉ，ｊ，Ｍ^ｉｎｉ _{ｉ，ｊ，ｋ}）は、前記複数の事前エッジ画像からそれぞれ抽出された前記複数の事前撮像画像の特徴量をそれぞれ示す複数の事前撮像画像特徴量データであり、前記演算装置（６）は、前記エッジ画像から、前記撮像画像の特徴量を示す特徴量データを抽出し、前記特徴量データと前記複数の事前撮像画像特徴量データとから前記移動体（２）の位置、及び／又は姿勢を同定することが好ましい。
【００２４】
前記有効撮像領域には、複数の領域（Ｓ^ｉｎｉ _ｓ，ｔＳ^α _ｓ，ｔ、Ｓ^０ _ｓ，ｔ）が規定され、前記複数の事前撮像データは、前記複数の領域（Ｓ^ｉｎｉ _ｓ，ｔＳ^α _ｓ，ｔ、Ｓ^０ _ｓ，ｔ）のそれぞれに含まれる、前記事前撮像画像に撮像されている構造体のエッジの量に対応した値で構成される事前撮像画像特徴量データであり、前記演算装置（６）は、前記エッジ画像から、前記複数の領域のそれぞれに含まれる前記撮像画像に撮像されている構造体のエッジの量に対応した値で構成される特徴量データを抽出し、前記特徴量データと前記事前撮像画像特徴量データとから、前記移動体（２）の位置、及び／又は姿勢を同定することが好ましい。
【００２５】
当該自己位置／姿勢同定装置は、更に、過去の時点における前記移動体（２）の過去位置と、前記移動体（２）が前記過去位置に存在した存在時刻とを保存する記憶装置（７）を備え、前記演算装置（６）は、前記過去位置と前記存在時刻とに基づいて前記移動体（２）の存在可能範囲を算出し、前記事前撮像画像のうち、前記存在可能範囲にある位置から撮像されたものから生成された存在可能範囲事前撮像データを、前記複数の事前撮像データのうちから選択し、前記存在可能範囲事前撮像データと前記撮像画像から前記移動体（２）の位置、及び／又は姿勢を同定することが好ましい。
【００２６】
前記複数の事前撮像データは、前記複数の事前撮像画像のそれぞれから切り出された複数の部分事前撮像画像から生成され、前記演算装置（６）は、前記撮像画像から切り出された部分撮像画像と、前記複数の事前撮像データとに基づいて前記移動体（２）の位置、及び／又は姿勢を同定することが好ましい。
【００２７】
前記複数の事前撮像画像と前記撮像画像とにおいて、前記カメラ（３）の撮像対象は、実質的に円形である有効撮像領域（Ｒ_ｅｆｆ）に写され、前記有効撮像領域有効撮像領域（Ｒ_ｅｆｆ）には、中央部と前記中央部を取り囲む周辺部とが定められ、前記複数の部分事前撮像画像は、前記複数の事前撮像画像それぞれのうちの前記中央部に対応する部分であり、前記部分撮像画像は、前記撮像画像のうちの前記中央部に対応する部分であることが好ましい。
【００２８】
本発明による自己位置／姿勢同定方法は、部屋（１）の内部で使用される移動体（２）の位置、及び／又は姿勢を同定する自己位置／姿勢同定方法であって、（Ａ）前記移動体（２）の位置、及び／又は姿勢の同定の前に、前記部屋（１）の複数の位置のそれぞれから、前記移動体（２）に搭載され、且つ、前記部屋（１）の床面と実質的に垂直である天井方向に向けられたカメラ（３）によって複数の事前撮像画像を撮像するステップと、
（Ｂ）前記複数の事前撮像画像のそれぞれから、複数の事前撮像データをそれぞれ生成して保存するステップと、
（Ｃ）前記カメラ（３）によって撮像画像を撮像するステップと、
（Ｄ）前記撮像画像と前記複数の事前撮像データとに基づいて、前記撮像画像が撮像された時点における前記移動体（２）の位置、及び／又は姿勢を同定するステップ
とを備えている。
【００２９】
当該自己位置／姿勢同定方法において、前記カメラ（３）の撮像対象が、前記複数の事前撮像画像と前記撮像画像とにおいて、実質的に円形である有効撮像領域（Ｒ_ｅｆｆ）に写される場合、
前記（Ｂ）ステップは、
（Ｂ１）　前記複数の事前撮像画像を、前記有効撮像領域の中心を原点として定められる極座標系の円周方向にそれぞれ微分することによって複数の事前撮像エッジ画像をそれぞれ生成するステップと、
（Ｂ２）前記複数の事前撮像エッジ画像のそれぞれから、前記複数の事前撮像データをそれぞれ生成して保存するステップ
とを含み、
前記（Ｄ）ステップは、
（Ｄ１）前記撮像画像を前記極座標系の前記円周方向に微分することによってエッジ画像を生成するステップと、
（Ｄ２）前記エッジ画像と前記複数の事前撮像データとに基づいて前記移動体（２）の位置、及び／又は姿勢を同定するステップ
とを含むことが好ましい。
【００３０】
【発明の実施の形態】
以下、添付図面を参照しながら、本発明による自己位置／姿勢同定装置の実施の一形態を説明する。
【００３１】
（実施の第１形態）
図２は、本発明による自己位置／姿勢同定装置の実施の第１形態を示す。実施の第１形態の自己位置／姿勢同定装置は、部屋１の中で使用される移動体２に搭載される。移動体２は、部屋１の床面１ａの上を自在に移動可能である。
【００３２】
部屋１には、ｘｙｚ直交座標系が定められている。ｘｙ面は、部屋１の床１ａ内に定められ、ｚ軸は、床面１ａに垂直な方向（以下、「天井方向」という。）に定められている。床面１ａには、マトリックス状に、位置（ｘ_０，ｙ_０）〜（ｘ_ｐ，ｙ_ｑ）が規定されている。ｐとｑとは、いずれも自然数である。
【００３３】
移動体２の位置は、座標（ｘ，ｙ）によって表現され、移動体２の姿勢は、図３に示されているように、移動体２上にｘｙ面に平行に規定された方向と、ｘ軸とがなす角θで表現される。角θは、以下、姿勢θと表現されることがある。移動体２の位置の座標（ｘ、ｙ）と姿勢θとが、該自己位置／姿勢同定装置によって同定される。
【００３４】
図１に示されているように、移動体２には、カメラ３と制御コンピュータ４とから構成される自己位置／姿勢同定装置が搭載される。カメラ３は、部屋１の天井方向（ｚ方向）に実質的に平行な方向に向けられ、移動体２から天井方向に向けて画像を撮像する。カメラ３のレンズ３ａは、ｚ軸に実質的に平行な主軸３ｂを有する凸レンズである。従って、図４に示されているように、カメラ３の撮像対象は、円形である領域Ｒ_ｅｆｆに写される。この領域Ｒ_ｅｆｆを、以下では、有効撮像領域Ｒ_ｅｆｆと呼ぶ。図２を参照して、カメラ３の視野は、天井１ｂのみならず壁１ｃに及び、有効撮像領域Ｒ_ｅｆｆには、壁１ｃも撮像される。
【００３５】
図１に示されているように、カメラ３は制御コンピュータ４に接続されている。制御コンピュータ４は、カメラ３が撮像した撮像画像から、移動体２の位置（ｘ、ｙ）と姿勢θとを同定する。
【００３６】
制御コンピュータ４は、インターフェース５と演算装置６と記憶装置７とを備えている。インターフェース５は、カメラ３が撮像した撮像画像を受け取り、記憶装置７に保存する。演算装置６は、移動体２の位置（ｘ、ｙ）と姿勢θとを同定するために必要な演算を行う。演算装置６は、カメラ３が撮像した撮像画像に対して画像処理を行い、その画像処理によって得られるデータから、移動体２の位置（ｘ、ｙ）と姿勢θとを同定する。記憶装置７は、テンプレート画像データベース７ａを保存する。
【００３７】
図５に示されているように、記憶装置７に保存されるテンプレート画像データベース７ａには、特徴量マトリックスＭ^ｉｎｉ _０，０〜Ｍ^ｉｎｉ _ｐ，ｑが蓄積される。特徴量マトリックスＭ^ｉｎｉ _０，０〜Ｍ^ｉｎｉ _ｐ，ｑは、該自己位置／姿勢同定装置の稼動の前に、事前に、カメラ３によって撮像される事前撮像画像から生成されるデータである。ｉを０以上ｐ以下の整数とし、ｊを０以上ｑ以下の整数としたとき、特徴量マトリックスＭ^ｉｎｉ _ｉ，ｊは、移動体２が位置（ｘ_ｉ，ｙ_ｊ）を有する状態でカメラ３によって撮像された事前撮像画像の特徴量が記述されたマトリックスである。事前撮像画像の撮像のときの移動体２の姿勢は、姿勢θ_０（＝０）で共通である。特徴量マトリックスＭ^ｉｎｉ _０，０〜Ｍ^ｉｎｉ _ｐ，ｑは、移動体２の位置と姿勢との同定の際に撮影される撮像画像と、事前撮像画像との対比に使用される。特徴量マトリックスＭ^ｉｎｉ _０，０〜Ｍ^ｉｎｉ _ｐ，ｑの生成方法は、後述され、ここでは詳細には説明されない。
【００３８】
記憶装置７には、更に、自己位置／姿勢同定プログラム７ｂが記憶される。自己位置／姿勢同定プログラム７ｂは、移動体２の位置及び姿勢を同定する手順が記述されたコンピュータプログラムである。演算装置６が自己位置／姿勢同定プログラム７ｂを実行することにより、移動体２の位置及び姿勢が同定される。
【００３９】
記憶装置７は、更に、移動体２の位置及び姿勢を同定する過程で生成されるデータが記憶されるワークエリアとして使用される。例えば、カメラ３によって撮像された撮像画像と、該撮像画像に対して画像処理が行われた結果として生成されるデータとが、記憶装置７に保存される。
【００４０】
既述のように、該自己位置／姿勢同定装置を稼動するためには、稼動の前に、特徴量マトリックスＭ^ｉｎｉ _０，０〜Ｍ^ｉｎｉ _ｐ，ｑを生成し、テンプレート画像データベース７ａに保存する必要がある。以下では、特徴量マトリックスＭ^ｉｎｉ _０，０〜Ｍ^ｉｎｉ _ｐ，ｑを生成する手順が説明される。
【００４１】
ステップＳ０１：事前撮像画像取得
移動体２が位置（ｘ_０，ｙ_０）〜（ｘ_ｐ，ｙ_ｑ）をそれぞれ有する状態で、天井方向に向けられたカメラ３により事前撮像画像が予め撮像される。一の位置（ｘ_ｉ，ｙ_ｊ）に対応して一の事前撮像画像が撮像される。以下において、該一の事前撮像画像をＰ^ｐｒｅ _ｉ、ｊと記述することがある。事前撮像画像の撮像のときの移動体２の姿勢は、θ_０（＝０）で共通である。撮像された事前撮像画像は、グレースケールの画像であり、典型的には、２５６階調を有する。撮像された事前撮像画像は、記憶装置７に保存される。既述のように、事前撮像画像のうちの有効撮像領域Ｒ_ｅｆｆは、実質的に円形である。事前撮像画像には、有効撮像領域Ｒ_ｅｆｆの中心を原点とする極座標（ｒ、ψ）が定められる。有効撮像領域Ｒ_ｅｆｆの中心から所定の方向が、ψ＝０の方向であると定められる。
【００４２】
ステップＳ０２：エッジ画像生成
図６に示されているように、事前撮像画像のそれぞれを、有効撮像領域Ｒ_ｅｆｆの中心を原点として規定される円周方向（ψ方向）に微分することによって、エッジ画像がそれぞれ生成される。一の位置（ｘ_ｉ，ｙ_ｊ）に対応して、一のエッジ画像が生成される。エッジ画像には、該エッジ画像が対応する事前撮像画像に含まれるエッジのうち、半径方向に延伸するエッジが抽出される。エッジ画像は、グレースケールの画像である。
【００４３】
部屋１の内部におけるｚ軸方向は、事前撮像画像では半径方向（ｒ方向）に写される。従って、ｚ軸方向に延伸する構造体のエッジは、確実にエッジ画像に抽出されることになる。人工建造物の内部の空間において、天井方向（ｚ軸方向）に延伸する構造体としては、例えば、隣接する壁１ｃにより形成される稜線１ｄや柱（図示されない）が挙げられる。これらの例から分かるように、人工建造物の内部の空間において、天井方向（ｚ軸方向）に延伸する構造体の位置は、変動することが少ない。従って、事前撮像画像を円周方向（ψ方向）に微分してエッジ画像を生成し、該エッジ画像から特徴量マトリックスＭ^ｉｎｉ _０，０〜Ｍ^ｉｎｉ _ｐ，ｑを生成することにより、位置が変動しにくい構造体に関するデータを特徴量マトリックスＭ^ｉｎｉ _０，０〜Ｍ^ｉｎｉ _ｐ，ｑに組み込むことが可能である。
【００４４】
ステップＳ０３：エッジ２値化
図７に示されているように、エッジ画像のそれぞれが、適切なしきい値を用いて２値化され、２値化エッジ画像が生成される。一の位置（ｘ_ｉ，ｙ_ｊ）に対応して、一の２値化エッジ画像が生成される。２値化エッジ画像は、エッジが存在する位置にあるピクセルを”１”、エッジが存在しない位置にあるピクセルを”０”として表現したデータである。以下では、エッジが存在する位置にあるピクセルをエッジ存在ピクセルと呼び、エッジが存在しない位置にあるピクセルをエッジ不存在ピクセルと呼ぶ。
【００４５】
ステップＳ０４：領域分割
図８に示されているように、２値化エッジ画像のうちの有効撮像領域に対応する有効領域が、半径方向（ｒ方向）にｎ個に、円周方向（ψ方向）にｍ個に分割される。分割により定められた領域を、それぞれ、セグメント領域Ｓ^ｉｎｉ _１，１〜Ｓ^ｉｎｉ _ｎ，ｍと呼ぶこととする。セグメント領域Ｓ^ｉｎｉ _ｓ、ｔ（ｓは、ｎ以下の自然数、ｔは、ｍ以下の自然数）は、有効撮像領域の半径をｒ_ｅｆｆとして、
（ｓ−１）・Δｒ＜ｒ≦ｓ・Δｒ，
（ｔ−１）・Δψ＜ψ≦ｔ・Δψ，　　　…（１）
である領域である。但し、
Δｒ＝ｒ_ｅｆｆ／ｎ，
Δψ＝３６０／ｍ，
である。
【００４６】
ステップＳ０５：度数算出
図９に示されているように、セグメント領域Ｓ^ｉｎｉ _１，１〜Ｓ^ｉｎｉ _ｎ，ｍのそれぞれについて、各セグメント領域が含むエッジ存在ピクセルの度数が算出される。エッジ存在ピクセルの度数は、各セグメント領域に写されたエッジの量を示す。エッジ存在ピクセルの度数の算出は、２値化エッジ画像のそれぞれについて行われる。図１０に示されているように、算出された度数から、ヒストグラムが生成される。
【００４７】
ステップＳ０６：特徴量マトリックスの生成
図１１に示されているように、セグメント領域Ｓ^ｉｎｉ _ｓ，ｔが含むエッジ存在ピクセルの度数から、特徴量マトリックスが生成される。特徴量マトリックスの生成は、位置（ｘ_０，ｙ_０）〜（ｘ_ｐ，ｙ_ｑ）のそれぞれについて行われる。位置（ｘ_ｉ，ｙ_ｊ）に対応して生成された２値化エッジ画像の、セグメント領域Ｓ^ｉｎｉ _ｓ，ｔに含まれるエッジ存在ピクセルの度数をＮ^ｉｎｉ _ｓ，ｔ（ｉ，ｊ）として、特徴量マトリックスＭ^ｉｎｉ _ｉ，ｊは、
【数１】

として求められる。式（２）は、事前撮像画像Ｐ^ｐｒｅ _ｉ、ｊの特徴量として、度数Ｎ^ｉｎｉ _１，１（ｉ，ｊ）〜Ｎ^ｉｎｉ _ｎ，ｍ（ｉ，ｊ）が使用されることを意味する。
【００４８】
以上の過程により、特徴量マトリックスＭ^ｉｎｉ _０，０〜Ｍ^ｉｎｉ _ｐ，ｑが生成される。特徴量マトリックスＭ^ｉｎｉ _０，０〜Ｍ^ｉｎｉ _ｐ，ｑは、数値データであり、従って、その保存に必要な記憶領域は小さい。生成された特徴量マトリックスＭ^ｉｎｉ _０，０〜Ｍ^ｉｎｉ _ｐ，ｑは、テンプレート画像データベース７ａに保存される。特徴量マトリックスＭ^ｉｎｉ _０，０〜Ｍ^ｉｎｉ _ｐ，ｑがテンプレート画像データベース７ａに保存されると、テンプレート画像データベース７ａを用いて、移動体２の位置及び姿勢の算出が可能になる。
【００４９】
図１２は、移動体２の位置及び姿勢の算出のシーケンスを示すフローチャートである。該シーケンスが、自己位置／姿勢同定プログラム７ｂに記述されている。
【００５０】
まず、天井方向に向けられたカメラ３によって撮像画像が撮像され、制御コンピュータ４に入力される（ステップＳ１１）。撮像画像は、記憶領域７に保存される。既述のように、撮像画像のうちの有効撮像領域Ｒ_ｅｆｆは、実質的に円形である。撮像画像には、事前撮像画像と同一の極座標（ｒ，ψ）が定められる。事前撮像画像について定められた極座標と同様に、該極座標の原点は、有効撮像領域Ｒ_ｅｆｆの中心である。また、撮像画像について定められたψ＝０の方向と、事前撮像画像について定められたψ＝０の方向は一致する。
【００５１】
続いて、姿勢同定パラメータαが０°にセットされ、姿勢同定ループが開始される（ステップＳ１２）。姿勢同定ループは、後述のステップＳ１３〜Ｓ１８で構成される。ステップＳ１３〜Ｓ１８が１度実行される毎に、αがΔαだけ増加され、αが３６０°を超えるまでステップＳ１３〜Ｓ１８が繰り返して実行される。
【００５２】
姿勢同定ループは、撮像画像の特徴量マトリックスＭ^αの算出で開始される（ステップＳ１３）。特徴量マトリックスＭ^αとは、撮像画像が角度αだけ時計回りに回転された画像の特徴量が記述されたマトリックスである。特徴量マトリックスＭ^αの算出は、以下の過程で行われる。
【００５３】
ステップＳ１３−１：エッジ画像生成
ステップＳ０２と同様に、撮像画像を円周方向（ψ方向）に微分することによって、エッジ画像が生成される。撮像画像に含まれるエッジのうち、半径方向に延伸するエッジがエッジ画像に抽出される。既述のように、ｚ軸方向に延伸する構造体は、撮像画像の半径方向に延伸するように撮像されるため、該エッジ画像には、ｚ軸方向に延伸する構造体が確実に抽出されることになる。エッジ画像は、グレースケールの画像である。
【００５４】
ステップＳ１３−２：エッジ２値化
ステップＳ０３と同様に、エッジ画像が適切なしきい値を用いて２値化され、２値化エッジ画像が生成される。
【００５５】
ステップＳ１３−３：領域分割
ステップＳ０４と同様に、２値化エッジ画像のうちの有効撮像領域Ｒ_ｅｆｆに対応する有効領域が、半径方向にｎ個、円周方向にｍ個のセグメント領域Ｓ^α _１，１〜Ｓ^α _ｎ，ｍに分割される。但し、図１３に示されているように、事前撮像画像について定められたセグメント領域Ｓ^ｉｎｉ _ｓ，ｔとは異なり、撮像画像について定められるセグメント領域Ｓ^α _ｓ，ｔは、姿勢同定パラメータαを用いて、
（ｓ−１）・Δｒ＜ｒ≦ｓ・Δｒ，
（ｔ−１）・Δψ＋α＜ψ≦ｔ・Δψ＋α，　　　…式（３）
である領域として定められる。ここで、有効撮像領域の半径をｒ_ｅｆｆとして、
Δｒ＝ｒ_ｅｆｆ／ｎ，
Δψ＝３６０／ｍ，
である。α＝０のときのみ、撮像画像について定められたセグメント領域Ｓ^α _ｓ，ｔは、事前撮像画像について定められたセグメント領域Ｓ^ｉｎｉ _ｓ，ｔと一致する。
【００５６】
ステップＳ１３−４：
セグメント領域Ｓ^α _１，１〜Ｓ^α _ｎ，ｍのそれぞれについて、各セグメント領域が含むエッジ存在ピクセルの度数が算出され、算出されたエッジ存在ピクセルの度数から、特徴量マトリックスＭ^αが算出される。既述のように、エッジ存在ピクセルとは、エッジが存在する位置にあるピクセルのことである。特徴量マトリックスＭ^αは、
【数２】

により求められる。ここでＮ^α _ｓ，ｔは、セグメント領域Ｓ^α _ｓ，ｔに含まれるエッジ存在ピクセルの数である。
【００５７】
このようにして定められた特徴量マトリックスＭ^αは、撮像画像を角度αだけ時計回りに回転させた画像の特徴量が記述されることになる。上述の式（３）は、セグメント領域Ｓ^α _１，１〜Ｓ^α _ｎ，ｍの決定の基準となる方向を、角度αだけ反時計回りに回転させることを意味する。これは、撮像画像を角度αだけ時計回りに回転するのと実質的に同一の画像処理を行うことを意味する。撮像画像が角度αだけ時計回りに回転された仮想的な画像を回転画像Ｐ^αと記載することとすると、式（４）は、回転画像Ｐ^αの特徴量として、Ｎ^α _１，１〜Ｎ^α _ｎ，ｍが使用されることを意味している。
【００５８】
特徴量マトリックスＭ^αの算出が完了すると、位置同定ループが開始される（ステップＳ１４）。０≦ｉ≦ｐ，０≦ｊ≦ｑなる全てのｉ，ｊの組について、１度ずつ、ステップＳ１５〜Ｓ１７が実行される。
【００５９】
位置同定ループは、事前撮像画像Ｐ^Ｐｒｅ _ｉ、ｊから生成された特徴量マトリックスＭ^ｉｎｉ _ｉ、ｊの読み出しで開始される（ステップＳ１５）。特徴量マトリックスＭ^ｉｎｉ _０，０〜Ｍ^ｉｎｉ _ｐ，ｑのうちの特徴量マトリックスＭ^ｉｎｉ _ｉ、ｊが、テンプレート画像データベース７ａから読み出される。
【００６０】
続いて、類似度Ｌ^α _ｉ，ｊの算出が行われる（ステップＳ１６）。類似度Ｌ^α _ｉ，ｊは、回転画像Ｐ^αと事前撮像画像Ｐ^ｐｒｅ _ｉ，ｊとが類似している程度を示す値であり、これらが類似しているほど大きな値をとる。類似度Ｌ^α _ｉ，ｊの算出は、回転画像Ｐ^αの特徴量が記述された特徴量マトリックスＭ^αと、事前撮像画像Ｐ^ｐｒｅ _ｉ，ｊの特徴量が記述された特徴量マトリックスＭ^ｉｎｉ _ｉ、ｊとを用いて行われる。特徴量マトリックスＭ^αと特徴量マトリックスＭ^ｉｎｉ _ｉ、ｊとを用いて類似度Ｌ^α _ｉ，ｊを算出することは、回転画像Ｐ^αと事前撮像画像Ｐ^ｐｒｅ _ｉ，ｊとを対比することと等価である。
【００６１】
類似度Ｌ^α _ｉ，ｊの算出には、ＳＳＤＡ（Ｓｅｑｕｅｎｔｉａｌ　Ｓｉｍｉｌａｒｉｔｙ　Ｄｅｔｅｃｔｉｏｎ　Ａｌｇｏｒｉｔｈｍ）、ＳＡＤ（Ｓｕｍ　ｏｆ　Ａｂｓｏｌｕｔｅ　Ｄｉｆｆｅｒｅｎｃｅ）、相関法のいずれを用いることも可能である。ＳＳＤＡが使用される場合、類似度Ｌ^α _ｉ，ｊは、
【数３】

により算出される。ここでＳＳＤＡ^α _ｉ，ｊは、
【数４】

であり、εは、０で割る演算を防止するために用いられる所定の微小値である。
【００６２】
一方、ＳＡＤが使用される場合、類似度Ｌ^α _ｉ，ｊは、
【数５】

により算出される。ここでＳＡＤ^α _ｉ，ｊは、
【数６】

であり、εは、０で割る演算を防止するために用いられる所定の微小値である。
【００６３】
更に、相関法が使用される場合、類似度Ｌ^α _ｉ，ｊは、
【数７】

により算出される。
【００６４】
算出された類似度Ｌ^α _ｉ，ｊは、記憶装置７に保存される（ステップＳ１７）。０≦ｉ≦ｐ，０≦ｊ≦ｑなる全てのｉ，ｊの組について、１度ずつ、ステップＳ１５〜Ｓ１７が実行されると、位置同定ループが完了する（ステップＳ１８）。
【００６５】
０°から３６０°までの範囲で、所定の間隔Δαで定められた全てのαについて、１度ずつ上述のステップＳ１３〜Ｓ１８が実行されると、姿勢同定ループが完了する（ステップＳ１９）。階層化された位置同定ループと姿勢同定ループとが実行されることにより、所定の間隔Δαで定められた全てのαについて、且つ、０≦ｉ≦ｐ，０≦ｊ≦ｑなる全てのｉ，ｊの組について、類似度Ｌ^α _ｉ，ｊが算出される。これは、所定の間隔Δαで定められた全てのαについて、且つ、０≦ｉ≦ｐ，０≦ｊ≦ｑなる全てのｉ，ｊの組について、回転画像Ｐ^αと事前撮像画像Ｐ^ｐｒｅ _ｉ，ｊとを対比することと等価である。
【００６６】
ステップＳ１２〜ステップＳ１９における類似度Ｌ^α _ｉ，ｊの算出が完了すると、類似度Ｌ^α _ｉ，ｊに基づいて、移動体２の位置（ｘ，ｙ）と、姿勢θとが同定される。類似度Ｌ^α _ｉ，ｊを最大にするα，ｉ，ｊをそれぞれα１，ｉ１，ｊ１として、移動体２の位置（ｘ，ｙ）は、位置（ｘ_ｉ１，ｙ_ｉ１）と同定され、姿勢θは、α１と同定される。
【００６７】
以上で、移動体２の位置（ｘ，ｙ）と姿勢θとを算出する処理が完了する。以上の処理は、演算装置６が自己位置／姿勢同定プログラム７ｂを実行することにより実現されている。
【００６８】
本実施の形態では、該自己位置／姿勢同定装置の稼動の前に予め撮影された事前撮影画像と、移動体２の位置及び姿勢の同定のために撮影された撮影画像とが対比され、この対比により、移動体２の位置及び姿勢の同定が行われる。当該自己位置／姿勢同定装置は、移動体２の位置及び姿勢の同定のために、蛍光灯やマーカのような特別な構造体を天井に設けることを必要としない。
【００６９】
更に、本実施の形態では、移動体２の位置及び姿勢の同定のときに撮影される撮像画像と事前撮像画像とを、極座標の円周方向に微分することによってエッジ画像が生成され、該エッジ画像が対比することによって、移動体２の位置及び姿勢の同定が行われる。これにより、移動体２の位置及び姿勢の同定の精度が向上されている。円周方向に微分されて生成されたエッジ画像には、主として、実空間における天井方向（ｚ軸方向）の構造体のエッジが抽出されることになる。人工建造物の内部において、天井方向（ｚ軸方向）に延伸する構造体は、隣接する壁１ｃにより形成される稜線１ｄや柱（図示されない）の例で分かるように、一般に、その位置の変動が少ない。従って、事前撮像画像と、移動体２の位置及び姿勢の同定のために撮影される撮像画像とから生成されたエッジ画像には、いずれも、位置の変動が小さい構造体のエッジが抽出されることになる。このようなエッジ画像を用いて移動体２の位置及び姿勢の同定を行うことにより、移動体２の位置及び姿勢の同定の精度を向上することができる。
【００７０】
本実施の形態において、カメラ３が有するレンズ３ａは、魚眼レンズであることが好ましい。レンズ３ａが魚眼レンズであることにより、カメラ３の撮像範囲が床面１ａに平行な方向に拡大され、隣接する壁１ｃにより形成される稜線１ｄや柱（図示されない）のように、天井方向（ｚ軸方向）に延伸する構造体を一層に確実に撮像することが可能になる。
【００７１】
なお、本実施の形態のステップＳ２０において、より詳細に移動体２の位置（ｘ，ｙ）と姿勢θとを同定するために、移動体２の位置（ｘ，ｙ）と姿勢θとは、類似度Ｌ^α _ｉ，ｊのうちの上位Ｎ番目までに基づいて算出されることも可能である。この場合、類似度Ｌ^α _ｉ，ｊが第δ番目に大きくなるようなα，ｉ，ｊを、それぞれαδ、ｉδ、ｊδとすると、移動体２の位置のｘ座標は、ｘ_ｉ１〜ｘ_ｉＮに、それぞれ類似度Ｌ^α１ _{ｉ１，ｊ１}〜Ｌ^αＮ _{ｉＮ，ｊＮ}に対応した重み付けを与えて算出した平均値として算出される。同様に、移動体２の位置のｙ座標は、ｙ_ｉ１〜ｙ_ｉＮに、それぞれ類似度Ｌ^α１ _{ｉ１，ｊ１}〜Ｌ^αＮ _{ｉＮ，ｊＮ}に対応した重み付けを与えて算出した平均値として算出される。同様に、姿勢θは、α１〜αｉＮにそれぞれ類似度Ｌ^α１ _{ｉ１，ｊ１}〜Ｌ^αＮ _{ｉＮ，ｊＮ}に対応した重み付けを与えて算出した平均値として算出される。このような算出方法は、事前撮影画像が撮影された位置（ｘ_０，ｙ_０）〜（ｘ_ｐ，ｙ_ｑ）以外の位置の検出が可能である点で好ましい。
【００７２】
また、本実施の形態において、テンプレート画像データベース７ａには、特徴量マトリックスＭ^ｉｎｉ _０，０〜Ｍ^ｉｎｉ _ｐ，ｑの代わりに、事前撮影画像そのものを示すデータ、事前撮影画像から生成されたエッジ画像を示すデータ、又は、エッジ画像から生成された２値化エッジ画像を示すデータのいずれを保存しておくことが可能である。この場合、図１２のステップＳ１５において、テンプレート画像データベース７ａから事前撮影画像、エッジ画像、又は、２値化エッジ画像が読み出され、読み出された画像から、上述と同一の方法により、特徴量マトリックスＭ^ｉｎｉ _ｉ，ｊが算出される。ステップＳ１６における類似度Ｌ^α _ｉ，ｊの算出では、算出された特徴量マトリックスＭ^ｉｎｉ _ｉ，ｊが使用される。但し、特徴量マトリックスＭ^ｉｎｉ _０，０〜Ｍ^ｉｎｉ _ｐ，ｑは、データ量を小さくすることが容易であり、特徴量マトリックスＭ^ｉｎｉ _０，０〜Ｍ^ｉｎｉ _ｐ，ｑをテンプレート画像データベース７ａに記憶することは、大きな記憶領域を必要としない点で好ましい。
【００７３】
また、本実施の形態では、移動体２の位置及び姿勢の同定のときに撮影される撮像画像と事前撮影画像との対比は、有効撮像領域Ｒ_ｅｆｆの全体について行われるが、有効撮像領域Ｒ_ｅｆｆの一部を対比することによって移動体２の位置及び姿勢を同定することが可能である。これにより、撮像画像の撮影毎に、構造体の形状が変動する領域を除いて、移動体２の位置及び姿勢を同定することが可能になる。
【００７４】
この場合、有効撮像領域Ｒ_ｅｆｆの中央部のみが対比に使用され、中央部を取り囲む周辺部は、対比に使用されないことが好ましい。即ち、有効撮像領域Ｒ_ｅｆｆの半径をｒ_ｅｆｆとし、ｒ_ｍａｘをｒ_ｅｆｆより小さい正数として、有効撮像領域Ｒ_ｅｆｆのうち、
０≦ｒ≦ｒ_ｍａｘ（＜ｒ_ｅｆｆ），
なる領域のみが、移動体２の位置及び姿勢の同定のときに撮影される撮像画像と事前撮影画像とから切り出され、切り出された部分が対比されることが好ましい。有効撮像領域Ｒ_ｅｆｆの周辺部は、床面１ａの近くに存在する構造体が写される。床面１ａの近くには、人のように、移動可能な構造体が存在することが多い。有効撮像領域Ｒ_ｅｆｆの周辺部が対比から排除されることにより、人のような移動可能な構造体の存在の影響を排除することができる。
【００７５】
また、本実施の形態において、日照、照明、物の配置上等の環境条件が異なる状態で、一の位置から複数の事前撮像画像が撮像され、該事前撮像画像からそれぞれ生成された特徴量マトリックスが、移動体２の位置及び姿勢の同定のときに撮影される撮像画像と事前撮影画像との対比において使用されることが可能である。位置（ｘ_ｉ，ｙ_ｊ）について、第１〜第ｇ環境条件で撮像された事前撮像画像から撮像された特徴量マトリックスをそれぞれＭ^ｉｎｉ _ｉ，ｊ ^１〜Ｍ^ｉｎｉ _ｉ，ｊ ^ｇとすると、特徴量マトリックスＭ^ｉｎｉ _ｉ，ｊの代わりに特徴量マトリックスＭ^ｉｎｉ _ｉ，ｊ ^１〜Ｍ^ｉｎｉ _ｉ，ｊ ^ｇをそれぞれ使用することにより、特徴量マトリックスＭ^ｉｎｉ _ｉ，ｊ ^１〜Ｍ^ｉｎｉ _ｉ，ｊ ^ｇのそれぞれについて上述の類似度Ｌ^α _ｉ，ｊが算出される。図１２のステップＳ２０では、この類似度Ｌ^α _ｉ，ｊを最大にするような位置（ｘ_ｉ，ｙ_ｊ）が移動体２の位置（ｘ、ｙ）と定められ、類似度Ｌ^α _ｉ，ｊを最大にするようなαが移動体２の姿勢θであると定められる。
【００７６】
また、本実施の形態において、移動体２が存在し得る位置を考慮することにより、位置の探索範囲を減少することが可能である。この場合、図１４に示されているように、時刻を測定する時計装置８が制御コンピュータ４に追加される。移動体２の位置及び姿勢の同定が行われる毎に、同定された移動体２の位置と、該同定が行われた時刻が記憶装置７に記録される。新たに位置及び姿勢の同定が行われる場合、最も最近に同定された位置と、その位置が同定された時刻と、移動体２の最大移動速度とから、移動体２の存在し得る存在範囲が特定される。図１２のステップＳ１４〜Ｓ１８は、位置（ｘ_ｉ、ｙ_ｊ）が該存在範囲の内部にあるｉ，ｊについてのみ実行され、算出対象とされる類似度Ｌ^α _ｉ，ｊの数が減少される。これにより、移動体２の位置及び姿勢の同定に必要な演算量を減少することができる。
【００７７】
また、本実施の形態においては、移動体２の位置と姿勢の両方が同定されているが、必要に応じて、位置と姿勢のうちのいずれか一方のみが同定されることも可能である。
【００７８】
（実施の第２形態）
実施の第１形態では、セグメント領域Ｓ^α _１，１〜Ｓ^α _ｎ，ｍの決定の基準となる方向を可変にすることにより、移動体２の位置及び姿勢の同定のときに撮像される撮像画像を実質的に回転する回転演算が行われているが、実施の第２形態では、撮像画像を回転する回転演算は行われない。その代り、同一の位置から複数の事前撮像画像が異なる姿勢θで撮像され、撮像された事前撮像画像と、移動体２の位置及び姿勢の同定のときに撮像される撮像画像とが対比されることにより、移動体２の位置及び姿勢の同定が行われる。
【００７９】
図１５は、実施の第２形態の自己位置／姿勢同定装置が搭載された移動体２を示している。実施の第２形態では、テンプレート画像データベース７ａの代わりに、テンプレート画像データベース７ａ’　が記憶装置７に記憶される。更に、移動体２の位置及び姿勢の同定の手順の変更に伴い、自己位置／姿勢同定プログラム７ｂの代わりに、自己位置／姿勢同定プログラム７ｂ’が記憶装置７に記憶される。
【００８０】
図１６は、実施の第２形態で使用されるテンプレート画像データベース７ａ’の内容を示している。テンプレート画像データベース７ａ’には、特徴量マトリックスＭ^ｉｎｉ _{０，０，０}〜Ｍ^ｉｎｉ _{ｐ，ｑ，ｒ}が保存されている。特徴量マトリックスＭ^ｉｎｉ _{０，０、０}〜Ｍ^ｉｎｉ _{ｐ，ｑ，ｒ}は、該自己位置／姿勢同定装置の稼動の前にカメラ３によって撮像された事前撮像画像から生成されるデータである。ｉを０以上ｐ以下の整数とし、ｊを０以上ｑ以下の整数とし、ｋを、０以上ｒ以下の整数としたとき、特徴量マトリックスＭ^ｉｎｉ _{ｉ，ｊ，ｋ}は、移動体２が位置（ｘ_ｉ，ｙ_ｊ）を有し、且つ、姿勢θ_ｋを有する状態でカメラ３によって撮像された事前撮像画像の特徴量が記述されたマトリックスである。
【００８１】
特徴量マトリックスＭ^ｉｎｉ _{０，０，０}〜Ｍ^ｉｎｉ _{ｐ，ｑ，ｒ}の生成方法は、実施の第１形態で説明された特徴量マトリックスＭ^ｉｎｉ _０，０〜Ｍ^ｉｎｉ _ｐ，ｑの生成方法と同一である。実施の第１形態と同様にして、事前撮像画像からエッジ画像が抽出され、該エッジ画像から、２値化エッジ画像が生成される。２値化エッジ画像にセグメント領域Ｓ^ｉｎｉ _１，１〜Ｓ^ｉｎｉ _ｎ，ｍが定められ、セグメント領域Ｓ^ｉｎｉ _１，１〜Ｓ^ｉｎｉ _ｎ，ｍに含まれるエッジ存在ピクセルの度数を要素とする行列が、特徴量マトリックスとされる。特徴量マトリックスＭ^ｉｎｉ _{ｉ，ｊ，ｋ}は、
【数８】

により算出される。ここで、Ｎ^ｉｎｉ _ｓ，ｔ（ｉ，ｊ，ｋ）は、移動体２が位置（ｘ_ｉ，ｙ_ｊ）及び姿勢θ_ｋを有する状態で撮像された事前撮像画像から生成された２値化エッジ画像のセグメント領域Ｓ^ｉｎｉ _ｓ，ｔに含まれるエッジ存在ピクセルの度数である。
【００８２】
移動体２の位置及び姿勢を同定する際には、カメラ３によって撮像画像が撮像される。該撮像画像から、特徴量マトリックスＭ^０が生成される。特徴量マトリックスＭ^０の生成方法は、実施の第１形態においてα＝０としたときの特徴量マトリックスＭ^αの生成方法と同一である。特徴量マトリックスＭ^０は、
【数９】

により算出される。Ｎ^０ _ｓ，ｔは、式（３）にα＝０を代入することにより規定されるセグメント領域Ｓ^０ _ｓ，ｔの内部に含まれるエッジ存在ピクセルの度数である。
【００８３】
特徴量マトリックスＭ^０と特徴量マトリックスＭ^ｉｎｉ _{０，０、０}〜Ｍ^ｉｎｉ _{ｐ，ｑ，ｒ}とを用いて、移動体２の位置及び姿勢の同定の際に撮影された撮像画像と、事前撮像画像との対比が行われる。撮像画像と事前撮像画像との対比のために、これらが類似している程度を示す類似度Ｌ_{ｉ，ｊ，ｋ}が算出される。類似度Ｌ_{ｉ，ｊ，ｋ}の算出は、ＳＳＤＡ（Ｓｅｑｕｅｎｔｉａｌ　Ｓｉｍｉｌａｒｉｔｙ　Ｄｅｔｅｃｔｉｏｎ　Ａｌｇｏｒｉｔｈｍ）、ＳＡＤ（Ｓｕｍ　ｏｆ　Ａｂｓｏｌｕｔｅ　Ｄｉｆｆｅｒｅｎｃｅ）、相関法のいずれを用いることも可能である。
【００８４】
ＳＳＤＡが使用される場合、類似度Ｌ_{ｉ，ｊ，ｋ}は、
【数１０】

により算出される。ここでＳＳＤＡ_{ｉ，ｊ，ｋ}は、
【数１１】

であり、εは、０で割る演算を防止するために用いられる所定の微小値である。
【００８５】
一方、ＳＡＤが使用される場合、類似度Ｌ_{ｉ，ｊ，ｋ}は、
【数１２】

により算出される。ここでＳＡＤ_{ｉ，ｊ，ｋ}は、
【数１３】

であり、εは、０で割る演算を防止するために用いられる所定の微小値である。
【００８６】
更に、相関法が使用される場合、類似度Ｌ_{ｉ，ｊ，ｋ}は、
【数１４】

により算出される。
【００８７】
移動体２の位置及び姿勢は、類似度Ｌ_{ｉ，ｊ，ｋ}から同定される。類似度Ｌ_{ｉ，ｊ，ｋ}を最大にするｉ，ｊ，ｋをそれぞれ、ｉ１，ｊ１，ｋ１として、移動体２の位置（ｘ，ｙ）は、（ｘ_ｉ１，ｙ_ｊ１）と同定され、移動体２の姿勢θは、θ_ｋ１と同定される。
【００８８】
以上で、移動体２の位置（ｘ，ｙ）と姿勢θとを算出する処理が完了する。以上の処理は、演算装置６が自己位置／姿勢同定プログラム７ｂ’を実行することにより実現されている。
【００８９】
実施の第２形態では、実施の第１形態と同様に、該自己位置／姿勢同定装置の稼動の前に予め撮影された事前撮影画像と、移動体２の位置及び姿勢の同定のために撮影された撮影画像とが対比され、この対比により、移動体２の位置及び姿勢の同定が行われる。移動体２の位置及び姿勢の同定のために、蛍光灯やマーカのような特別な構造体を天井に設けることを必要としない。
【００９０】
更に、実施の第２形態では、実施の第１形態と同様に、移動体２の位置及び姿勢の同定の際に撮影される撮像画像と事前撮像画像とを円周方向に微分することによってエッジ画像が生成され、該エッジ画像を使用して撮像画像と事前撮像画像との対比が行われる。このようなエッジ画像を用いて移動体２の位置及び姿勢の同定を行うことにより、移動体２の位置及び姿勢の同定の精度を向上することができる。
【００９１】
更に実施の第２形態では、移動体２の位置及び姿勢の同定のために撮影された撮影画像を回転する回転処理が行われず、実施の第１形態に比較して、移動体２の位置及び姿勢の同定のために必要な演算量の抑制が可能である。但し、実施の第２形態においてテンプレート画像データベース７ａ’に蓄積する必要があるデータ量は、実施の第１形態においてテンプレート画像データベース７ａに蓄積する必要があるデータ量に比べて大きい。記憶装置７の記憶容量を小さくする観点では、実施の第１形態が好ましい。
【００９２】
なお、実施の第２形態では、特徴量マトリックスＭ^ｉｎｉ _{０，０，０}〜Ｍ^ｉｎｉ _{ｐ，ｑ，ｒ}は、位置（ｘ_０，ｙ_０）〜（ｘ_ｐ，ｙ_ｑ）のそれぞれについて、且つ、姿勢θ_０〜θ_ｒのそれぞれについて撮像された事前撮像画像から生成されているが、一の姿勢θ_０についてのみ撮像された事前撮像画像からＭ^ｉｎｉ _{０，０，０}〜Ｍ^ｉｎｉ _{ｐ，ｑ，０}が生成され、姿勢θ_０以外の姿勢については、該撮像された事前撮像画像を回転した画像から、特徴量マトリックスＭ^ｉｎｉ _{０，０，１}〜Ｍ^ｉｎｉ _{ｐ，ｑ，ｒ}が生成されることも可能である。これにより、必要な事前撮像画像の数を減少することができる。
【００９３】
また、実施の第２形態において、より詳細に移動体２の位置（ｘ，ｙ）と姿勢θとを同定するために、移動体２の位置（ｘ，ｙ）と姿勢θとが、類似度Ｌ_{ｉ， ｊ，ｋ}のうちの上位Ｎ番目までに基づいて算出されることも可能である。この場合、類似度Ｌ_{ｉ，ｊ、ｋ}が第δ番目に大きくなるようなｉ，ｊ，ｋを、それぞれ、ｉδ、ｊδ、ｋδとして、移動体２の位置のｘ座標は、ｘ_ｉ１〜ｘ_ｉＮにそれぞれ類似度Ｌ_{ｉ１，ｊ１，ｋ１}〜Ｌ_{ｉＮ，ｊＮ，ｋＮ}に対応した重み付けを与えて算出した平均値として算出される。同様に、移動体２の位置のｙ座標は、ｙ_ｉ１〜ｙ_ｉＮに、それぞれ類似度Ｌ_{ｉ１，ｊ１，ｋ１}〜Ｌ_{ｉＮ，ｊＮ、ｋＮ}に対応した重み付けを与えて算出した平均値として算出される。同様に、姿勢θは、θ_ｋ１〜θ_ｋＮにそれぞれ類似度Ｌ_{ｉ１，ｊ１，ｋ１}〜Ｌ_{ｉＮ，ｊＮ、ｋＮ}に対応した重み付けを与えて算出した平均値として算出される。このような算出方法は、事前撮影画像が撮影された位置（ｘ_０，ｙ_０）〜（ｘ_ｐ，ｙ_ｑ）以外の位置の検出が可能である点で好ましい。
【００９４】
また、実施の第２形態において、テンプレート画像データベース７ａ’には、特徴量マトリックスＭ^ｉｎｉ _{０，０，０}〜Ｍ^ｉｎｉ _{ｐ，ｑ，ｒ}の代わりに、事前撮影画像そのものを示すデータ、事前撮影画像から生成されたエッジ画像を示すデータ、又は、エッジ画像から生成された２値化エッジ画像を示すデータのいずれを保存しておくことが可能である。この場合、類似度Ｌ_{ｉ，ｊ、ｋ}の算出のときに、テンプレート画像データベース７ａ’から事前撮影画像、エッジ画像、又は、２値化エッジ画像が読み出され、読み出された画像から、上述と同一の方法により、特徴量マトリックスＭ^ｉｎｉ _{ｉ，ｊ，ｋ}が算出される。類似度Ｌ_{ｉ，ｊ，ｋ}の算出では、算出された特徴量マトリックスＭ^ｉｎｉ _{ｉ，ｊ，ｋ}が使用される。但し、特徴量マトリックスＭ^ｉｎｉ _{０，０，０}〜Ｍ^ｉｎｉ _{ｐ，ｑ，ｒ}は、データ量を小さくすることが容易であり、特徴量マトリックスＭ^ｉｎｉ _{０，０，０}〜Ｍ^ｉｎｉ _{ｐ，ｑ，ｒ}をテンプレート画像データベース７ａ’に記憶することは、大きな記憶領域を必要としない点で好ましい。
【００９５】
また、実施の第２形態でも、実施の第１形態と同様に、有効撮像領域Ｒ_ｅｆｆの一部を対比することによって移動体２の位置及び姿勢を同定することが可能である。これにより、撮像画像の撮影毎に、構造体の形状が変動する領域を除いて、移動体２の位置及び姿勢を同定することが可能になる。
【００９６】
この場合、実施の第１形態と同様に、有効撮像領域Ｒ_ｅｆｆの周辺部が対比に使用されないことが好ましい。即ち、有効撮像領域Ｒ_ｅｆｆの半径をｒ_ｅｆｆとし、ｒ_ｍａｘをｒ_ｅｆｆより小さい正数として、有効撮像領域Ｒ_ｅｆｆのうち、
０≦ｒ≦ｒ_ｍａｘ（＜ｒ_ｅｆｆ），
なる領域のみが、移動体２の位置及び姿勢の同定のときに撮影される撮像画像と事前撮影画像との対比において使用されることが好ましい。有効撮像領域Ｒ_ｅｆｆの周辺部が対比から排除されることにより、人のような移動可能な構造体の存在の影響を排除することができる。
【００９７】
また、実施の第２形態において、日照、照明、物の配置上等の環境条件が異なる状態で、一の位置から複数の事前撮像画像が撮像され、該事前撮像画像からそれぞれ生成された特徴量マトリックスが、移動体２の位置及び姿勢の同定のときに撮影される撮像画像と事前撮影画像との対比において使用されることが可能である。位置（ｘ_ｉ，ｙ_ｊ）及び姿勢θ_ｋについて、第１〜第ｇ環境条件で撮像された事前撮像画像から撮像された特徴量マトリックスをそれぞれＭ^ｉｎｉ _{ｉ，ｊ，ｋ} ^１〜Ｍ^ｉｎｉ _{ｉ，ｊ，ｋ} ^ｇとすると、特徴量マトリックスＭ^ｉｎｉ _{ｉ，ｊ，ｋ}の代わりに特徴量マトリックスＭ^ｉｎｉ _{ｉ，ｊ，ｋ} ^１〜Ｍ^ｉｎｉ _{ｉ，ｊ，ｊ} ^ｇをそれぞれ使用することにより、特徴量マトリックスＭ^ｉｎｉ _{ｉ，ｊ，ｋ} ^１〜Ｍ^ｉｎｉ _{ｉ，ｊ，ｋ} ^ｇのそれぞれについて上述の類似度Ｌ_{ｉ，ｊ，ｋ}が算出される。類似度Ｌ_{ｉ，ｊ，ｋ}を最大にするような位置（ｘ_ｉ，ｙ_ｊ）及び姿勢θ_ｋが、それぞれ、移動体２の位置（ｘ、ｙ）及び姿勢θと定められる。
【００９８】
また、実施の第２形態において、実施の第１形態と同様に、移動体２が存在し得る位置を考慮することにより、位置の探索範囲を減少することが可能である。この場合、実施の第１形態と同様に、時刻を測定する時計装置が制御コンピュータ４に追加され、移動体２の位置及び姿勢の同定が行われる毎に、該同定が行われた時刻が記憶装置７に記録される。新たに位置及び姿勢の同定が行われる場合、最も最近に位置及び姿勢の同定が行われた時刻と、移動体２の最大移動速度とから、移動体２の存在し得る存在範囲が特定される。類似度Ｌ_{ｉ，ｊ，ｋ}の算出は、位置（ｘ_ｉ、ｙ_ｊ）が該存在範囲の内部にあるｉ，ｊについてのみ実行される。これにより、算出対象である類似度Ｌ_{ｉ，ｊ，ｋ}の数が減少され、移動体２の位置及び姿勢の同定に必要な演算量を減少することができる。
【００９９】
また、実施の第２形態においては、移動体２の位置と姿勢の両方が同定されているが、必要に応じて、位置と姿勢のうちのいずれか一方のみが同定されることも可能である。
【０１００】
【発明の効果】
本発明により、天井に蛍光灯やマーカのような特別な構造体を設けなくても室内移動体の位置及び姿勢の同定が可能な自己位置／姿勢同定装置が提供される。
【０１０１】
また、本発明により、室内移動体の位置及び姿勢を一層に正確に同定できる自己位置／姿勢同定装置が提供される。
【０１０２】
また、本発明により、室内移動体の位置及び姿勢を同定するために必要な演算量、及びデータ量を抑制することが可能な自己位置／姿勢同定装置が提供される。
【図面の簡単な説明】
【図１】図１は、本発明による自己位置／姿勢同定装置の実施の第１形態を示す。
【図２】図２は、実施の第１形態の自己位置／姿勢同定装置が搭載された移動体２が使用される部屋１を示す。
【図３】図３は、移動体２の姿勢θの定義を示す。
【図４】図４は、有効撮像領域Ｒ_ｅｆｆを示す。
【図５】図５は、テンプレート画像データベース７ａの内容を示す。
【図６】図６は、エッジ画像が抽出される態様を示す。
【図７】図７は、エッジ画像が２値化されて２値化エッジ画像が生成される過程を示す。
【図８】図８は、セグメント領域Ｓ^ｉｎｉ _ｓ，ｔの決定方法を示す。
【図９】図９は、セグメント領域Ｓ^ｉｎｉ _ｓ，ｔに含まれるエッジ存在ピクセルの度数の算出方法を示す。
【図１０】図１０は、エッジ存在ピクセルの度数のヒストグラムを示す。
【図１１】図１１は、エッジ存在ピクセルの度数から、特徴量マトリックスが生成される過程を示す。
【図１２】図１２は、移動体２の位置及び姿勢が同定される過程を示すフローチャートである。
【図１３】図１３は、セグメント領域Ｓ^α _ｓ，ｔの決定方法と、セグメント領域Ｓ^α _ｓ，ｔに含まれるエッジ存在ピクセルの度数の算出方法とを示す。
【図１４】図１４は、実施の第１形態の自己位置／姿勢同定装置の変形例を示す。
【図１５】図１５は、本発明による自己位置／姿勢同定装置の実施の第２形態を示す。
【図１６】図１６は、テンプレート画像データベース７ａ’の内容を示す。
【符号の説明】
１：部屋
２：移動体
３：カメラ
４：制御コンピュータ
５：インターフェース
６：演算装置
７：記憶装置
７ａ、７ａ’：テンプレート画像データベース
７ｂ、７ｂ’：自己位置／姿勢同定プログラム
８：時計装置

Claims (13)

1. 部屋の内部で使用される移動体の位置、及び／又は姿勢を同定する自己位置／姿勢同定装置であって、
   前記移動体に搭載され、前記部屋の床面と実質的に垂直である天井方向に向けられたカメラと、
   前記移動体の位置、及び／又は姿勢の同定の前に前記カメラによって前記部屋の複数の位置からそれぞれ撮像された複数の事前撮像画像からそれぞれ生成された複数の事前撮像データを保持するテンプレート画像データベースと、
   前記カメラによって撮像された撮像画像と前記複数の事前撮像データとに基づいて、前記撮像画像が撮像された時点における前記移動体の位置、及び／又は姿勢を同定する演算装置
   とを備えた
   自己位置／姿勢同定装置。
2. 請求項１に記載の自己位置／姿勢同定装置において、
   前記事前撮像画像が撮像される時の前記移動体の姿勢は、一定であり、
   前記演算装置は、前記撮像画像を互いに異なる角度に回転した複数の回転画像と前記事前撮像画像とを対比することにより、前記移動体の位置及び姿勢を同定する
   自己位置／姿勢同定装置。
3. 請求項１に記載の自己位置／姿勢同定装置において
   前記複数の事前撮像画像は、同一の位置から前記移動体の姿勢が異なる状態で撮像された異姿勢事前撮像画像を含む
   自己位置／姿勢同定装置。
4. 請求項１に記載の自己位置／姿勢同定装置において
   前記複数の事前撮像画像は、互いに異なる環境条件で撮像された異環境条件撮像画像を含む
   自己位置／姿勢同定装置。
5. 請求項１に記載の自己位置／姿勢同定装置において、
   前記カメラの撮像対象は、前記複数の事前撮像画像と前記撮像画像とにおいて、実質的に円形である有効撮像領域に写され、
   前記複数の事前撮像データは、前記有効撮像領域の中心を原点として定められる極座標系の円周方向に前記複数の事前撮像画像をそれぞれ微分することによって得られる複数の事前撮像エッジ画像から生成され、
   前記演算装置は、前記撮像画像を前記極座標系の前記円周方向に微分することによってエッジ画像を生成し、前記エッジ画像と前記複数の事前撮像データとに基づいて前記移動体の位置、及び／又は姿勢を同定する
   自己位置／姿勢同定装置。
6. 請求項５に記載の自己位置／姿勢同定装置において、
   前記カメラは、魚眼レンズを含み、
   前記撮像画像と前記複数の事前撮像画像とは、いずれも、前記魚眼レンズを通して撮影される
   自己位置／姿勢同定装置。
7. 請求項５に記載の自己位置／姿勢同定装置において、
   前記複数の事前撮像データは、前記複数の事前エッジ画像からそれぞれ抽出された前記複数の事前撮像画像の特徴量をそれぞれ示す複数の事前撮像画像特徴量データであり、
   前記演算装置は、前記エッジ画像から、前記撮像画像の特徴量を示す特徴量データを抽出し、前記特徴量データと前記複数の事前撮像画像特徴量データとから前記移動体の位置、及び／又は姿勢を同定する
   自己位置／姿勢同定装置。
8. 請求項５に記載の自己位置／姿勢同定装置において、
   前記有効撮像領域には、複数の領域が規定され、
   前記複数の事前撮像データは、前記複数の領域のそれぞれに含まれる、前記事前撮像画像に撮像されている構造体のエッジの量に対応した値で構成される事前撮像画像特徴量データであり、
   前記演算装置は、前記エッジ画像から、前記複数の領域のそれぞれに含まれる前記撮像画像に撮像されている構造体のエッジの量に対応した値で構成される特徴量データを抽出し、前記特徴量データと前記事前撮像画像特徴量データとから、前記移動体の位置、及び／又は姿勢を同定する
   自己位置／姿勢同定装置。
9. 請求項１に記載の自己位置／姿勢同定装置において、
   更に、
   過去の時点における前記移動体の過去位置と、前記移動体が前記過去位置に存在した存在時刻とを保存する記憶装置
   を備え、
   前記演算装置は、前記過去位置と前記存在時刻とに基づいて前記移動体の存在可能範囲を算出し、前記事前撮像画像のうち、前記存在可能範囲にある位置から撮像されたものから生成された存在可能範囲事前撮像データを、前記複数の事前撮像データのうちから選択し、前記存在可能範囲事前撮像データと前記撮像画像から前記移動体の位置、及び／又は姿勢を同定する
   自己位置／姿勢同定装置。
10. 請求項１に記載の自己位置／姿勢同定装置において、
    前記複数の事前撮像データは、前記複数の事前撮像画像のそれぞれから切り出された複数の部分事前撮像画像から生成され、
    前記演算装置は、前記撮像画像から切り出された部分撮像画像と、前記複数の事前撮像データとに基づいて前記移動体の位置、及び／又は姿勢を同定する
    自己位置／姿勢同定装置。
11. 請求項１０に記載の自己位置／姿勢同定装置において、
    前記複数の事前撮像画像と前記撮像画像とにおいて、前記カメラの撮像対象は、実質的に円形である有効撮像領域に写され、
    前記有効撮像領域には、中央部と前記中央部を取り囲む周辺部とが定められ、前記複数の部分事前撮像画像は、前記複数の事前撮像画像それぞれのうちの前記中央部に対応する部分であり、
    前記部分撮像画像は、前記撮像画像のうちの前記中央部に対応する部分である
    自己位置／姿勢同定装置。
12. 部屋の内部で使用される移動体の位置、及び／又は姿勢を同定する自己位置／姿勢同定方法であって、
    （Ａ）前記移動体の位置、及び／又は姿勢の同定の前に、前記部屋の複数の位置のそれぞれから、前記移動体に搭載され、且つ、前記部屋の床面と実質的に垂直である天井方向に向けられたカメラによって複数の事前撮像画像を撮像するステップと、
    （Ｂ）前記複数の事前撮像画像のそれぞれから、複数の事前撮像データをそれぞれ生成して保存するステップと、
    （Ｃ）前記カメラによって撮像画像を撮像するステップと、
    （Ｄ）前記撮像画像と前記複数の事前撮像データとに基づいて、前記撮像画像が撮像された時点における前記移動体の位置、及び／又は姿勢を同定するステップ
    とを備えた
    自己位置／姿勢同定方法。
13. 請求項１２に記載の自己位置／姿勢同定方法において、
    前記カメラの撮像対象は、前記複数の事前撮像画像と前記撮像画像とにおいて、実質的に円形である有効撮像領域に写され、
    前記（Ｂ）ステップは、
    （Ｂ１）　前記複数の事前撮像画像を、前記有効撮像領域の中心を原点として定められる極座標系の円周方向にそれぞれ微分することによって複数の事前撮像エッジ画像をそれぞれ生成するステップと、
    （Ｂ２）前記複数の事前撮像エッジ画像のそれぞれから、前記複数の事前撮像データをそれぞれ生成して保存するステップ
    とを含み、
    前記（Ｄ）ステップは、
    （Ｄ１）前記撮像画像を前記極座標系の前記円周方向に微分することによってエッジ画像を生成するステップと、
    （Ｄ２）前記エッジ画像と前記複数の事前撮像データとに基づいて前記移動体の位置、及び／又は姿勢を同定するステップ
    とを含む
    自己位置／姿勢同定方法。

Images