(57)【要約】【課題】天井に蛍光灯やマーカのような特別な構造体を設けなくても室内移動体の位置及び姿勢の同定が可能な自己位置／姿勢同定装置を提供する。【解決手段】本発明による自己位置／姿勢同定装置は、部屋の内部で使用される移動体（２）の位置、及び／又は姿勢を同定する自己位置／姿勢同定装置である。当該自己位置／姿勢同定装置は、前記移動体（２）に搭載されたカメラ（３）と、複数の事前撮像データを保持するテンプレート画像データベース（７ａ）と、カメラ（３）によって撮像された撮像画像と前記複数の事前撮像データとに基づいて、前記撮像画像が撮像された時点における前記移動体（２）の位置、及び／又は姿勢を同定する演算装置（６）とを備えている。カメラ（３）は、部屋（１）の床面（１ａ）と実質的に垂直である天井方向に向けられている。複数の事前撮像データは、前記移動体（２）の位置、及び／又は姿勢の同定の前に前記カメラ（３）によって前記部屋（１）の複数の位置からそれぞれ撮像された複数の事前撮像画像からそれぞれ生成される。【選択図】　図１
2004012429.tif 000002
【特許請求の範囲】
【請求項１】
部屋の内部で使用される移動体の位置、及び／又は姿勢を同定する自己位置／姿勢同定装置であって、
前記移動体に搭載され、前記部屋の床面と実質的に垂直である天井方向に向けられたカメラと、
前記移動体の位置、及び／又は姿勢の同定の前に前記カメラによって前記部屋の複数の位置からそれぞれ撮像された複数の事前撮像画像からそれぞれ生成された複数の事前撮像データを保持するテンプレート画像データベースと、
前記カメラによって撮像された撮像画像と前記複数の事前撮像データとに基づいて、前記撮像画像が撮像された時点における前記移動体の位置、及び／又は姿勢を同定する演算装置
とを備えた
自己位置／姿勢同定装置。
【請求項２】
請求項１に記載の自己位置／姿勢同定装置において、
前記事前撮像画像が撮像される時の前記移動体の姿勢は、一定であり、
前記演算装置は、前記撮像画像を互いに異なる角度に回転した複数の回転画像と前記事前撮像画像とを対比することにより、前記移動体の位置及び姿勢を同定する
自己位置／姿勢同定装置。
【請求項３】
請求項１に記載の自己位置／姿勢同定装置において
前記複数の事前撮像画像は、同一の位置から前記移動体の姿勢が異なる状態で撮像された異姿勢事前撮像画像を含む
自己位置／姿勢同定装置。
【請求項４】
請求項１に記載の自己位置／姿勢同定装置において
前記複数の事前撮像画像は、互いに異なる環境条件で撮像された異環境条件撮像画像を含む
自己位置／姿勢同定装置。
【請求項５】
請求項１に記載の自己位置／姿勢同定装置において、
前記カメラの撮像対象は、前記複数の事前撮像画像と前記撮像画像とにおいて、実質的に円形である有効撮像領域に写され、
前記複数の事前撮像データは、前記有効撮像領域の中心を原点として定められる極座標系の円周方向に前記複数の事前撮像画像をそれぞれ微分することによって得られる複数の事前撮像エッジ画像から生成され、
前記演算装置は、前記撮像画像を前記極座標系の前記円周方向に微分することによってエッジ画像を生成し、前記エッジ画像と前記複数の事前撮像データとに基づいて前記移動体の位置、及び／又は姿勢を同定する
自己位置／姿勢同定装置。
【請求項６】
請求項５に記載の自己位置／姿勢同定装置において、
前記カメラは、魚眼レンズを含み、
前記撮像画像と前記複数の事前撮像画像とは、いずれも、前記魚眼レンズを通して撮影される
自己位置／姿勢同定装置。
【請求項７】
請求項５に記載の自己位置／姿勢同定装置において、
前記複数の事前撮像データは、前記複数の事前エッジ画像からそれぞれ抽出された前記複数の事前撮像画像の特徴量をそれぞれ示す複数の事前撮像画像特徴量データであり、
前記演算装置は、前記エッジ画像から、前記撮像画像の特徴量を示す特徴量データを抽出し、前記特徴量データと前記複数の事前撮像画像特徴量データとから前記移動体の位置、及び／又は姿勢を同定する
自己位置／姿勢同定装置。
【請求項８】
請求項５に記載の自己位置／姿勢同定装置において、
前記有効撮像領域には、複数の領域が規定され、
前記複数の事前撮像データは、前記複数の領域のそれぞれに含まれる、前記事前撮像画像に撮像されている構造体のエッジの量に対応した値で構成される事前撮像画像特徴量データであり、
前記演算装置は、前記エッジ画像から、前記複数の領域のそれぞれに含まれる前記撮像画像に撮像されている構造体のエッジの量に対応した値で構成される特徴量データを抽出し、前記特徴量データと前記事前撮像画像特徴量データとから、前記移動体の位置、及び／又は姿勢を同定する
自己位置／姿勢同定装置。
【請求項９】
請求項１に記載の自己位置／姿勢同定装置において、
更に、
過去の時点における前記移動体の過去位置と、前記移動体が前記過去位置に存在した存在時刻とを保存する記憶装置
を備え、
前記演算装置は、前記過去位置と前記存在時刻とに基づいて前記移動体の存在可能範囲を算出し、前記事前撮像画像のうち、前記存在可能範囲にある位置から撮像されたものから生成された存在可能範囲事前撮像データを、前記複数の事前撮像データのうちから選択し、前記存在可能範囲事前撮像データと前記撮像画像から前記移動体の位置、及び／又は姿勢を同定する
自己位置／姿勢同定装置。
【請求項１０】
請求項１に記載の自己位置／姿勢同定装置において、
前記複数の事前撮像データは、前記複数の事前撮像画像のそれぞれから切り出された複数の部分事前撮像画像から生成され、
前記演算装置は、前記撮像画像から切り出された部分撮像画像と、前記複数の事前撮像データとに基づいて前記移動体の位置、及び／又は姿勢を同定する
自己位置／姿勢同定装置。
【請求項１１】
請求項１０に記載の自己位置／姿勢同定装置において、
前記複数の事前撮像画像と前記撮像画像とにおいて、前記カメラの撮像対象は、実質的に円形である有効撮像領域に写され、
前記有効撮像領域には、中央部と前記中央部を取り囲む周辺部とが定められ、前記複数の部分事前撮像画像は、前記複数の事前撮像画像それぞれのうちの前記中央部に対応する部分であり、
前記部分撮像画像は、前記撮像画像のうちの前記中央部に対応する部分である
自己位置／姿勢同定装置。
【請求項１２】
部屋の内部で使用される移動体の位置、及び／又は姿勢を同定する自己位置／姿勢同定方法であって、
（Ａ）前記移動体の位置、及び／又は姿勢の同定の前に、前記部屋の複数の位置のそれぞれから、前記移動体に搭載され、且つ、前記部屋の床面と実質的に垂直である天井方向に向けられたカメラによって複数の事前撮像画像を撮像するステップと、
（Ｂ）前記複数の事前撮像画像のそれぞれから、複数の事前撮像データをそれぞれ生成して保存するステップと、
（Ｃ）前記カメラによって撮像画像を撮像するステップと、
（Ｄ）前記撮像画像と前記複数の事前撮像データとに基づいて、前記撮像画像が撮像された時点における前記移動体の位置、及び／又は姿勢を同定するステップ
とを備えた
自己位置／姿勢同定方法。
【請求項１３】
請求項１２に記載の自己位置／姿勢同定方法において、
前記カメラの撮像対象は、前記複数の事前撮像画像と前記撮像画像とにおいて、実質的に円形である有効撮像領域に写され、
前記（Ｂ）ステップは、
（Ｂ１）　前記複数の事前撮像画像を、前記有効撮像領域の中心を原点として定められる極座標系の円周方向にそれぞれ微分することによって複数の事前撮像エッジ画像をそれぞれ生成するステップと、
（Ｂ２）前記複数の事前撮像エッジ画像のそれぞれから、前記複数の事前撮像データをそれぞれ生成して保存するステップ
とを含み、
前記（Ｄ）ステップは、
（Ｄ１）前記撮像画像を前記極座標系の前記円周方向に微分することによってエッジ画像を生成するステップと、
（Ｄ２）前記エッジ画像と前記複数の事前撮像データとに基づいて前記移動体の位置、及び／又は姿勢を同定するステップ
とを含む
自己位置／姿勢同定方法。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、自己位置／姿勢同定装置に関する。本発明は、特に、部屋の天井が撮像された撮像画像を用いて自己の位置及び姿勢を同定する自己位置／姿勢同定装置に関する。
【０００２】
【従来の技術】
室内で使用される室内移動体の自律制御のために、自己の位置及び姿勢を同定する技術の開発が進められている。かかる技術の開発は、介護ロボットや家事ロボットのような、屋内で使用されるロボットの実用化の上で重要である。
【０００３】
室内移動体の位置及び姿勢の同定のために、天井を撮像した天井画像を用いる手法が広く用いられている。天井は、人や障害物が室内に存在しても移動体から隠れにくい。天井を撮像した天井画像の利用は、自己の位置及び姿勢を確実に同定することを可能にする。
【０００４】
例えば、公開特許公報（特開平６−４１２７）には、天井画像から天井に設けられた蛍光灯の位置を算出し、算出された蛍光灯の位置を元に自己の位置を算出する屋内移動体の自己位置測定装置が開示されている。
【０００５】
更に、公開特許公報（特開平１０−１７７４１４）には、移動体に搭載されたカメラによって撮像された天井画像から、該移動体の進行方向及び該移動体の位置を検出する走行状態認識装置が開示されている。該走行状態認識装置は、該天井画像から該移動体が進行すべき方向の直線成分を取り出し、更に、該直線成分から該移動体の進行方向及び該移動体が走行する通路上の横方向の位置を算出する。該走行状態認識装置は、更に、天井に設けられたマーカを用いて、該移動体の位置を算出する。
【０００６】
更に、”天井画像列を用いた屋内ナビゲーション”（紙　弘和他，第１８回日本ロボット学会学術講演会予稿集ｐｐ．３３１−３３２）には、天井画像列を利用したナビゲーション手法が開示されている。該ナビゲーション手法では、まず、移動体の教示走行が行われ、該教示走行の間に画像列が記録される。記録された画像と、該移動体の現在の画像との単純な比較を行うことで、該移動体の自己位置同定とナビゲーションとが行われる。
【０００７】
天井に蛍光灯やマーカのような特徴的な構造体が設けられていなくても、室内移動体の位置及び姿勢を同定できることが望まれる。
【０００８】
更に、天井に特徴が少ない環境でも、室内移動体の位置及び姿勢を同定できることが望まれる。
【０００９】
更に、室内移動体の位置及び姿勢は、一層に正確に同定されることが望まれる。
【００１０】
更に、室内移動体の位置及び姿勢を同定するために必要な演算量、及びデータ量が抑制されることが望まれる。
【００１１】
【発明が解決しようとする課題】
本発明の目的は、天井に蛍光灯やマーカのような特別な構造体を設けなくても室内移動体の位置及び姿勢の同定が可能な自己位置／姿勢同定装置を提供することにある。
【００１２】
本発明の他の目的は、天井に特徴が少ない環境でも、室内移動体の位置及び姿勢の同定が可能な自己位置／姿勢同定装置を提供することにある。
【００１３】
本発明の更に他の目的は、室内移動体の位置及び姿勢を一層に正確に同定できる自己位置／姿勢同定装置を提供することにある。
【００１４】
本発明の更に他の目的は、室内移動体の位置及び姿勢を同定するために必要な演算量、及びデータ量を抑制することが可能な自己位置／姿勢同定装置を提供することにある。
【００１５】
【課題を解決するための手段】
以下に、［発明の実施の形態］で使用される番号・符号を用いて、課題を解決するための手段を説明する。これらの番号・符号は、［特許請求の範囲］の記載と［発明の実施の形態］の記載との対応関係を明らかにするために付加されている。但し、付加された番号・符号は、［特許請求の範囲］に記載されている発明の技術的範囲の解釈に用いてはならない。
【００１６】
本発明による自己位置／姿勢同定装置は、部屋（１）の内部で使用される移動体（２）の位置、及び／又は姿勢を同定する自己位置／姿勢同定装置である。当該自己位置／姿勢同定装置は、前記移動体（２）に搭載されたカメラ（３）と、複数の事前撮像データ（Ｍｉｎｉｉ，ｊ，Ｍｉｎｉｉ，ｊ，ｋ）を保持するテンプレート画像データベース（７ａ，７ａ’）と、カメラ（３）によって撮像された撮像画像と前記複数の事前撮像データ（Ｍｉｎｉｉ，ｊ，Ｍｉｎｉｉ，ｊ，ｋ）とに基づいて、前記撮像画像が撮像された時点における前記移動体（２）の位置、及び／又は姿勢を同定する演算装置（６）とを備えている。カメラ（３）は、部屋（１）の床面（１ａ）と実質的に垂直である天井方向に向けられている。複数の事前撮像データ（Ｍｉｎｉｉ，ｊ，Ｍｉｎｉｉ，ｊ，ｋ）は、前記移動体（２）の位置、及び／又は姿勢の同定の前に前記カメラ（３）によって前記部屋（１）の複数の位置からそれぞれ撮像された複数の事前撮像画像からそれぞれ生成される。当該自己位置／姿勢同定装置は、カメラ（３）によって撮像された撮像画像と複数の事前撮像画像との対比によって前記移動体（２）の位置、及び／又は姿勢が同定され、天井に蛍光灯やマーカのような特別な構造体を設けなくても室内移動体の位置及び姿勢の同定が可能である。
【００１７】
複数の事前撮像データ（Ｍｉｎｉｉ，ｊ，Ｍｉｎｉｉ，ｊ，ｋ）は、複数の事前撮像画像そのものを示すデータであることが可能であり、また複数の事前撮像画像を加工することによって得られるデータであることも可能である。
【００１８】
当該自己位置／姿勢同定装置において、前記事前撮像画像が撮像される時の前記移動体（２）の姿勢は、一定であり、前記演算装置（６）は、前記撮像画像を互いに異なる角度に回転した複数の回転画像と前記事前撮像画像とを対比することにより、前記移動体（２）の位置及び姿勢を同定することが好ましい。前記撮像画像の回転画像と前記事前撮像画像とが対比されるため、前記移動体（２）の姿勢を変更して事前撮像画像を撮像する必要がない。
【００１９】
演算装置（６）の演算量を減少させるためには、当該前記複数の事前撮像画像は、同一の位置から前記移動体（２）の姿勢が異なる状態で撮像された異姿勢事前撮像画像を含むことが好ましい。このような自己位置／姿勢同定装置は、姿勢が異なる状態で事前撮像画像が撮像されているため、前記移動体（２）の位置及び姿勢の同定のために、前記撮像画像を回転する回転処理を行う必要がない。
【００２０】
前記複数の事前撮像画像は、互いに異なる環境条件で撮像された異環境条件撮像画像を含むことが好ましい。これにより、部屋（１）の環境条件が変化しても、前記移動体（２）の位置及び姿勢を正確に同定することができる。
【００２１】
前記カメラ（３）の撮像対象は、前記複数の事前撮像画像と前記撮像画像との両方において、実質的に円形である有効撮像領域（Ｒｅｆｆ）に写される場合がある。この場合、前記複数の事前撮像データ（Ｍｉｎｉｉ，ｊ，Ｍｉｎｉｉ，ｊ，ｋ）は、前記有効撮像領域（Ｒｅｆｆ）の中心を原点として定められる極座標系の円周方向に前記複数の事前撮像画像をそれぞれ微分することによって得られる複数の事前撮像エッジ画像から生成され、前記演算装置（６）は、前記撮像画像を前記極座標系の前記円周方向に微分することによってエッジ画像を生成し、前記エッジ画像と前記複数の事前撮像データ（Ｍｉｎｉｉ，ｊ，Ｍｉｎｉｉ，ｊ，ｋ）とに基づいて前記移動体（２）の位置、及び／又は姿勢を同定することが好ましい。円周方向に微分されて生成されたエッジ画像には、主として、実空間における天井方向の構造体のエッジが抽出されることになる。人工建造物の内部において天井方向に延伸する構造体は、その位置の変動が少ないものが多く存在する。例えば、隣接する２つの壁により形成される稜線や、柱は、典型的な、天井方向に延伸する構造体である。前記事前撮像画像から生成された前記事前撮像エッジ画像と、前記撮像画像とから生成されたエッジ画像には、いずれも、位置の変動が小さい構造体のエッジが抽出されることになる。前記事前撮像エッジ画像と前記エッジ画像を用いて移動体（２）の位置及び姿勢の同定を行うことにより、移動体（２）の位置及び姿勢の同定の精度を向上することができる。
【００２２】
この場合、カメラ（３）は、魚眼レンズを含み、前記撮像画像と前記複数の事前撮像画像とは、いずれも、前記魚眼レンズを通して撮影されることが好ましい。これにより、天井方向に延伸する構造体が、より確実に前記撮像画像と前記複数の事前撮像画像とに写される。
【００２３】
前記複数の事前撮像データ（Ｍｉｎｉｉ，ｊ，Ｍｉｎｉｉ，ｊ，ｋ）は、前記複数の事前エッジ画像からそれぞれ抽出された前記複数の事前撮像画像の特徴量をそれぞれ示す複数の事前撮像画像特徴量データであり、前記演算装置（６）は、前記エッジ画像から、前記撮像画像の特徴量を示す特徴量データを抽出し、前記特徴量データと前記複数の事前撮像画像特徴量データとから前記移動体（２）の位置、及び／又は姿勢を同定することが好ましい。
【００２４】
前記有効撮像領域には、複数の領域（Ｓｉｎｉｓ，ｔＳαｓ，ｔ、Ｓ０ｓ，ｔ）が規定され、前記複数の事前撮像データは、前記複数の領域（Ｓｉｎｉｓ，ｔＳαｓ，ｔ、Ｓ０ｓ，ｔ）のそれぞれに含まれる、前記事前撮像画像に撮像されている構造体のエッジの量に対応した値で構成される事前撮像画像特徴量データであり、前記演算装置（６）は、前記エッジ画像から、前記複数の領域のそれぞれに含まれる前記撮像画像に撮像されている構造体のエッジの量に対応した値で構成される特徴量データを抽出し、前記特徴量データと前記事前撮像画像特徴量データとから、前記移動体（２）の位置、及び／又は姿勢を同定することが好ましい。
【００２５】
当該自己位置／姿勢同定装置は、更に、過去の時点における前記移動体（２）の過去位置と、前記移動体（２）が前記過去位置に存在した存在時刻とを保存する記憶装置（７）を備え、前記演算装置（６）は、前記過去位置と前記存在時刻とに基づいて前記移動体（２）の存在可能範囲を算出し、前記事前撮像画像のうち、前記存在可能範囲にある位置から撮像されたものから生成された存在可能範囲事前撮像データを、前記複数の事前撮像データのうちから選択し、前記存在可能範囲事前撮像データと前記撮像画像から前記移動体（２）の位置、及び／又は姿勢を同定することが好ましい。
【００２６】
前記複数の事前撮像データは、前記複数の事前撮像画像のそれぞれから切り出された複数の部分事前撮像画像から生成され、前記演算装置（６）は、前記撮像画像から切り出された部分撮像画像と、前記複数の事前撮像データとに基づいて前記移動体（２）の位置、及び／又は姿勢を同定することが好ましい。
【００２７】
前記複数の事前撮像画像と前記撮像画像とにおいて、前記カメラ（３）の撮像対象は、実質的に円形である有効撮像領域（Ｒｅｆｆ）に写され、前記有効撮像領域有効撮像領域（Ｒｅｆｆ）には、中央部と前記中央部を取り囲む周辺部とが定められ、前記複数の部分事前撮像画像は、前記複数の事前撮像画像それぞれのうちの前記中央部に対応する部分であり、前記部分撮像画像は、前記撮像画像のうちの前記中央部に対応する部分であることが好ましい。
【００２８】
本発明による自己位置／姿勢同定方法は、部屋（１）の内部で使用される移動体（２）の位置、及び／又は姿勢を同定する自己位置／姿勢同定方法であって、（Ａ）前記移動体（２）の位置、及び／又は姿勢の同定の前に、前記部屋（１）の複数の位置のそれぞれから、前記移動体（２）に搭載され、且つ、前記部屋（１）の床面と実質的に垂直である天井方向に向けられたカメラ（３）によって複数の事前撮像画像を撮像するステップと、
（Ｂ）前記複数の事前撮像画像のそれぞれから、複数の事前撮像データをそれぞれ生成して保存するステップと、
（Ｃ）前記カメラ（３）によって撮像画像を撮像するステップと、
（Ｄ）前記撮像画像と前記複数の事前撮像データとに基づいて、前記撮像画像が撮像された時点における前記移動体（２）の位置、及び／又は姿勢を同定するステップ
とを備えている。
【００２９】
当該自己位置／姿勢同定方法において、前記カメラ（３）の撮像対象が、前記複数の事前撮像画像と前記撮像画像とにおいて、実質的に円形である有効撮像領域（Ｒｅｆｆ）に写される場合、
前記（Ｂ）ステップは、
（Ｂ１）　前記複数の事前撮像画像を、前記有効撮像領域の中心を原点として定められる極座標系の円周方向にそれぞれ微分することによって複数の事前撮像エッジ画像をそれぞれ生成するステップと、
（Ｂ２）前記複数の事前撮像エッジ画像のそれぞれから、前記複数の事前撮像データをそれぞれ生成して保存するステップ
とを含み、
前記（Ｄ）ステップは、
（Ｄ１）前記撮像画像を前記極座標系の前記円周方向に微分することによってエッジ画像を生成するステップと、
（Ｄ２）前記エッジ画像と前記複数の事前撮像データとに基づいて前記移動体（２）の位置、及び／又は姿勢を同定するステップ
とを含むことが好ましい。
【００３０】
【発明の実施の形態】
以下、添付図面を参照しながら、本発明による自己位置／姿勢同定装置の実施の一形態を説明する。
【００３１】
（実施の第１形態）
図２は、本発明による自己位置／姿勢同定装置の実施の第１形態を示す。実施の第１形態の自己位置／姿勢同定装置は、部屋１の中で使用される移動体２に搭載される。移動体２は、部屋１の床面１ａの上を自在に移動可能である。
【００３２】
部屋１には、ｘｙｚ直交座標系が定められている。ｘｙ面は、部屋１の床１ａ内に定められ、ｚ軸は、床面１ａに垂直な方向（以下、「天井方向」という。）に定められている。床面１ａには、マトリックス状に、位置（ｘ０，ｙ０）〜（ｘｐ，ｙｑ）が規定されている。ｐとｑとは、いずれも自然数である。
【００３３】
移動体２の位置は、座標（ｘ，ｙ）によって表現され、移動体２の姿勢は、図３に示されているように、移動体２上にｘｙ面に平行に規定された方向と、ｘ軸とがなす角θで表現される。角θは、以下、姿勢θと表現されることがある。移動体２の位置の座標（ｘ、ｙ）と姿勢θとが、該自己位置／姿勢同定装置によって同定される。
【００３４】
図１に示されているように、移動体２には、カメラ３と制御コンピュータ４とから構成される自己位置／姿勢同定装置が搭載される。カメラ３は、部屋１の天井方向（ｚ方向）に実質的に平行な方向に向けられ、移動体２から天井方向に向けて画像を撮像する。カメラ３のレンズ３ａは、ｚ軸に実質的に平行な主軸３ｂを有する凸レンズである。従って、図４に示されているように、カメラ３の撮像対象は、円形である領域Ｒｅｆｆに写される。この領域Ｒｅｆｆを、以下では、有効撮像領域Ｒｅｆｆと呼ぶ。図２を参照して、カメラ３の視野は、天井１ｂのみならず壁１ｃに及び、有効撮像領域Ｒｅｆｆには、壁１ｃも撮像される。
【００３５】
図１に示されているように、カメラ３は制御コンピュータ４に接続されている。制御コンピュータ４は、カメラ３が撮像した撮像画像から、移動体２の位置（ｘ、ｙ）と姿勢θとを同定する。
【００３６】
制御コンピュータ４は、インターフェース５と演算装置６と記憶装置７とを備えている。インターフェース５は、カメラ３が撮像した撮像画像を受け取り、記憶装置７に保存する。演算装置６は、移動体２の位置（ｘ、ｙ）と姿勢θとを同定するために必要な演算を行う。演算装置６は、カメラ３が撮像した撮像画像に対して画像処理を行い、その画像処理によって得られるデータから、移動体２の位置（ｘ、ｙ）と姿勢θとを同定する。記憶装置７は、テンプレート画像データベース７ａを保存する。
【００３７】
図５に示されているように、記憶装置７に保存されるテンプレート画像データベース７ａには、特徴量マトリックスＭｉｎｉ０，０〜Ｍｉｎｉｐ，ｑが蓄積される。特徴量マトリックスＭｉｎｉ０，０〜Ｍｉｎｉｐ，ｑは、該自己位置／姿勢同定装置の稼動の前に、事前に、カメラ３によって撮像される事前撮像画像から生成されるデータである。ｉを０以上ｐ以下の整数とし、ｊを０以上ｑ以下の整数としたとき、特徴量マトリックスＭｉｎｉｉ，ｊは、移動体２が位置（ｘｉ，ｙｊ）を有する状態でカメラ３によって撮像された事前撮像画像の特徴量が記述されたマトリックスである。事前撮像画像の撮像のときの移動体２の姿勢は、姿勢θ０（＝０）で共通である。特徴量マトリックスＭｉｎｉ０，０〜Ｍｉｎｉｐ，ｑは、移動体２の位置と姿勢との同定の際に撮影される撮像画像と、事前撮像画像との対比に使用される。特徴量マトリックスＭｉｎｉ０，０〜Ｍｉｎｉｐ，ｑの生成方法は、後述され、ここでは詳細には説明されない。
【００３８】
記憶装置７には、更に、自己位置／姿勢同定プログラム７ｂが記憶される。自己位置／姿勢同定プログラム７ｂは、移動体２の位置及び姿勢を同定する手順が記述されたコンピュータプログラムである。演算装置６が自己位置／姿勢同定プログラム７ｂを実行することにより、移動体２の位置及び姿勢が同定される。
【００３９】
記憶装置７は、更に、移動体２の位置及び姿勢を同定する過程で生成されるデータが記憶されるワークエリアとして使用される。例えば、カメラ３によって撮像された撮像画像と、該撮像画像に対して画像処理が行われた結果として生成されるデータとが、記憶装置７に保存される。
【００４０】
既述のように、該自己位置／姿勢同定装置を稼動するためには、稼動の前に、特徴量マトリックスＭｉｎｉ０，０〜Ｍｉｎｉｐ，ｑを生成し、テンプレート画像データベース７ａに保存する必要がある。以下では、特徴量マトリックスＭｉｎｉ０，０〜Ｍｉｎｉｐ，ｑを生成する手順が説明される。
【００４１】
ステップＳ０１：事前撮像画像取得
移動体２が位置（ｘ０，ｙ０）〜（ｘｐ，ｙｑ）をそれぞれ有する状態で、天井方向に向けられたカメラ３により事前撮像画像が予め撮像される。一の位置（ｘｉ，ｙｊ）に対応して一の事前撮像画像が撮像される。以下において、該一の事前撮像画像をＰｐｒｅｉ、ｊと記述することがある。事前撮像画像の撮像のときの移動体２の姿勢は、θ０（＝０）で共通である。撮像された事前撮像画像は、グレースケールの画像であり、典型的には、２５６階調を有する。撮像された事前撮像画像は、記憶装置７に保存される。既述のように、事前撮像画像のうちの有効撮像領域Ｒｅｆｆは、実質的に円形である。事前撮像画像には、有効撮像領域Ｒｅｆｆの中心を原点とする極座標（ｒ、ψ）が定められる。有効撮像領域Ｒｅｆｆの中心から所定の方向が、ψ＝０の方向であると定められる。
【００４２】
ステップＳ０２：エッジ画像生成
図６に示されているように、事前撮像画像のそれぞれを、有効撮像領域Ｒｅｆｆの中心を原点として規定される円周方向（ψ方向）に微分することによって、エッジ画像がそれぞれ生成される。一の位置（ｘｉ，ｙｊ）に対応して、一のエッジ画像が生成される。エッジ画像には、該エッジ画像が対応する事前撮像画像に含まれるエッジのうち、半径方向に延伸するエッジが抽出される。エッジ画像は、グレースケールの画像である。
【００４３】
部屋１の内部におけるｚ軸方向は、事前撮像画像では半径方向（ｒ方向）に写される。従って、ｚ軸方向に延伸する構造体のエッジは、確実にエッジ画像に抽出されることになる。人工建造物の内部の空間において、天井方向（ｚ軸方向）に延伸する構造体としては、例えば、隣接する壁１ｃにより形成される稜線１ｄや柱（図示されない）が挙げられる。これらの例から分かるように、人工建造物の内部の空間において、天井方向（ｚ軸方向）に延伸する構造体の位置は、変動することが少ない。従って、事前撮像画像を円周方向（ψ方向）に微分してエッジ画像を生成し、該エッジ画像から特徴量マトリックスＭｉｎｉ０，０〜Ｍｉｎｉｐ，ｑを生成することにより、位置が変動しにくい構造体に関するデータを特徴量マトリックスＭｉｎｉ０，０〜Ｍｉｎｉｐ，ｑに組み込むことが可能である。
【００４４】
ステップＳ０３：エッジ２値化
図７に示されているように、エッジ画像のそれぞれが、適切なしきい値を用いて２値化され、２値化エッジ画像が生成される。一の位置（ｘｉ，ｙｊ）に対応して、一の２値化エッジ画像が生成される。２値化エッジ画像は、エッジが存在する位置にあるピクセルを”１”、エッジが存在しない位置にあるピクセルを”０”として表現したデータである。以下では、エッジが存在する位置にあるピクセルをエッジ存在ピクセルと呼び、エッジが存在しない位置にあるピクセルをエッジ不存在ピクセルと呼ぶ。
【００４５】
ステップＳ０４：領域分割
図８に示されているように、２値化エッジ画像のうちの有効撮像領域に対応する有効領域が、半径方向（ｒ方向）にｎ個に、円周方向（ψ方向）にｍ個に分割される。分割により定められた領域を、それぞれ、セグメント領域Ｓｉｎｉ１，１〜Ｓｉｎｉｎ，ｍと呼ぶこととする。セグメント領域Ｓｉｎｉｓ、ｔ（ｓは、ｎ以下の自然数、ｔは、ｍ以下の自然数）は、有効撮像領域の半径をｒｅｆｆとして、
（ｓ−１）・Δｒ＜ｒ≦ｓ・Δｒ，
（ｔ−１）・Δψ＜ψ≦ｔ・Δψ，　　　…（１）
である領域である。但し、
Δｒ＝ｒｅｆｆ／ｎ，
Δψ＝３６０／ｍ，
である。
【００４６】
ステップＳ０５：度数算出
図９に示されているように、セグメント領域Ｓｉｎｉ１，１〜Ｓｉｎｉｎ，ｍのそれぞれについて、各セグメント領域が含むエッジ存在ピクセルの度数が算出される。エッジ存在ピクセルの度数は、各セグメント領域に写されたエッジの量を示す。エッジ存在ピクセルの度数の算出は、２値化エッジ画像のそれぞれについて行われる。図１０に示されているように、算出された度数から、ヒストグラムが生成される。
【００４７】
ステップＳ０６：特徴量マトリックスの生成
図１１に示されているように、セグメント領域Ｓｉｎｉｓ，ｔが含むエッジ存在ピクセルの度数から、特徴量マトリックスが生成される。特徴量マトリックスの生成は、位置（ｘ０，ｙ０）〜（ｘｐ，ｙｑ）のそれぞれについて行われる。位置（ｘｉ，ｙｊ）に対応して生成された２値化エッジ画像の、セグメント領域Ｓｉｎｉｓ，ｔに含まれるエッジ存在ピクセルの度数をＮｉｎｉｓ，ｔ（ｉ，ｊ）として、特徴量マトリックスＭｉｎｉｉ，ｊは、
【数１】
2004012429.tif 000003
として求められる。式（２）は、事前撮像画像Ｐｐｒｅｉ、ｊの特徴量として、度数Ｎｉｎｉ１，１（ｉ，ｊ）〜Ｎｉｎｉｎ，ｍ（ｉ，ｊ）が使用されることを意味する。
【００４８】
以上の過程により、特徴量マトリックスＭｉｎｉ０，０〜Ｍｉｎｉｐ，ｑが生成される。特徴量マトリックスＭｉｎｉ０，０〜Ｍｉｎｉｐ，ｑは、数値データであり、従って、その保存に必要な記憶領域は小さい。生成された特徴量マトリックスＭｉｎｉ０，０〜Ｍｉｎｉｐ，ｑは、テンプレート画像データベース７ａに保存される。特徴量マトリックスＭｉｎｉ０，０〜Ｍｉｎｉｐ，ｑがテンプレート画像データベース７ａに保存されると、テンプレート画像データベース７ａを用いて、移動体２の位置及び姿勢の算出が可能になる。
【００４９】
図１２は、移動体２の位置及び姿勢の算出のシーケンスを示すフローチャートである。該シーケンスが、自己位置／姿勢同定プログラム７ｂに記述されている。
【００５０】
まず、天井方向に向けられたカメラ３によって撮像画像が撮像され、制御コンピュータ４に入力される（ステップＳ１１）。撮像画像は、記憶領域７に保存される。既述のように、撮像画像のうちの有効撮像領域Ｒｅｆｆは、実質的に円形である。撮像画像には、事前撮像画像と同一の極座標（ｒ，ψ）が定められる。事前撮像画像について定められた極座標と同様に、該極座標の原点は、有効撮像領域Ｒｅｆｆの中心である。また、撮像画像について定められたψ＝０の方向と、事前撮像画像について定められたψ＝０の方向は一致する。
【００５１】
続いて、姿勢同定パラメータαが０°にセットされ、姿勢同定ループが開始される（ステップＳ１２）。姿勢同定ループは、後述のステップＳ１３〜Ｓ１８で構成される。ステップＳ１３〜Ｓ１８が１度実行される毎に、αがΔαだけ増加され、αが３６０°を超えるまでステップＳ１３〜Ｓ１８が繰り返して実行される。
【００５２】
姿勢同定ループは、撮像画像の特徴量マトリックスＭαの算出で開始される（ステップＳ１３）。特徴量マトリックスＭαとは、撮像画像が角度αだけ時計回りに回転された画像の特徴量が記述されたマトリックスである。特徴量マトリックスＭαの算出は、以下の過程で行われる。
【００５３】
ステップＳ１３−１：エッジ画像生成
ステップＳ０２と同様に、撮像画像を円周方向（ψ方向）に微分することによって、エッジ画像が生成される。撮像画像に含まれるエッジのうち、半径方向に延伸するエッジがエッジ画像に抽出される。既述のように、ｚ軸方向に延伸する構造体は、撮像画像の半径方向に延伸するように撮像されるため、該エッジ画像には、ｚ軸方向に延伸する構造体が確実に抽出されることになる。エッジ画像は、グレースケールの画像である。
【００５４】
ステップＳ１３−２：エッジ２値化
ステップＳ０３と同様に、エッジ画像が適切なしきい値を用いて２値化され、２値化エッジ画像が生成される。
【００５５】
ステップＳ１３−３：領域分割
ステップＳ０４と同様に、２値化エッジ画像のうちの有効撮像領域Ｒｅｆｆに対応する有効領域が、半径方向にｎ個、円周方向にｍ個のセグメント領域Ｓα１，１〜Ｓαｎ，ｍに分割される。但し、図１３に示されているように、事前撮像画像について定められたセグメント領域Ｓｉｎｉｓ，ｔとは異なり、撮像画像について定められるセグメント領域Ｓαｓ，ｔは、姿勢同定パラメータαを用いて、
（ｓ−１）・Δｒ＜ｒ≦ｓ・Δｒ，
（ｔ−１）・Δψ＋α＜ψ≦ｔ・Δψ＋α，　　　…式（３）
である領域として定められる。ここで、有効撮像領域の半径をｒｅｆｆとして、
Δｒ＝ｒｅｆｆ／ｎ，
Δψ＝３６０／ｍ，
である。α＝０のときのみ、撮像画像について定められたセグメント領域Ｓαｓ，ｔは、事前撮像画像について定められたセグメント領域Ｓｉｎｉｓ，ｔと一致する。
【００５６】
ステップＳ１３−４：
セグメント領域Ｓα１，１〜Ｓαｎ，ｍのそれぞれについて、各セグメント領域が含むエッジ存在ピクセルの度数が算出され、算出されたエッジ存在ピクセルの度数から、特徴量マトリックスＭαが算出される。既述のように、エッジ存在ピクセルとは、エッジが存在する位置にあるピクセルのことである。特徴量マトリックスＭαは、
【数２】
2004012429.tif 000004
により求められる。ここでＮαｓ，ｔは、セグメント領域Ｓαｓ，ｔに含まれるエッジ存在ピクセルの数である。
【００５７】
このようにして定められた特徴量マトリックスＭαは、撮像画像を角度αだけ時計回りに回転させた画像の特徴量が記述されることになる。上述の式（３）は、セグメント領域Ｓα１，１〜Ｓαｎ，ｍの決定の基準となる方向を、角度αだけ反時計回りに回転させることを意味する。これは、撮像画像を角度αだけ時計回りに回転するのと実質的に同一の画像処理を行うことを意味する。撮像画像が角度αだけ時計回りに回転された仮想的な画像を回転画像Ｐαと記載することとすると、式（４）は、回転画像Ｐαの特徴量として、Ｎα１，１〜Ｎαｎ，ｍが使用されることを意味している。
【００５８】
特徴量マトリックスＭαの算出が完了すると、位置同定ループが開始される（ステップＳ１４）。０≦ｉ≦ｐ，０≦ｊ≦ｑなる全てのｉ，ｊの組について、１度ずつ、ステップＳ１５〜Ｓ１７が実行される。
【００５９】
位置同定ループは、事前撮像画像ＰＰｒｅｉ、ｊから生成された特徴量マトリックスＭｉｎｉｉ、ｊの読み出しで開始される（ステップＳ１５）。特徴量マトリックスＭｉｎｉ０，０〜Ｍｉｎｉｐ，ｑのうちの特徴量マトリックスＭｉｎｉｉ、ｊが、テンプレート画像データベース７ａから読み出される。
【００６０】
続いて、類似度Ｌαｉ，ｊの算出が行われる（ステップＳ１６）。類似度Ｌαｉ，ｊは、回転画像Ｐαと事前撮像画像Ｐｐｒｅｉ，ｊとが類似している程度を示す値であり、これらが類似しているほど大きな値をとる。類似度Ｌαｉ，ｊの算出は、回転画像Ｐαの特徴量が記述された特徴量マトリックスＭαと、事前撮像画像Ｐｐｒｅｉ，ｊの特徴量が記述された特徴量マトリックスＭｉｎｉｉ、ｊとを用いて行われる。特徴量マトリックスＭαと特徴量マトリックスＭｉｎｉｉ、ｊとを用いて類似度Ｌαｉ，ｊを算出することは、回転画像Ｐαと事前撮像画像Ｐｐｒｅｉ，ｊとを対比することと等価である。
【００６１】
類似度Ｌαｉ，ｊの算出には、ＳＳＤＡ（Ｓｅｑｕｅｎｔｉａｌ　Ｓｉｍｉｌａｒｉｔｙ　Ｄｅｔｅｃｔｉｏｎ　Ａｌｇｏｒｉｔｈｍ）、ＳＡＤ（Ｓｕｍ　ｏｆ　Ａｂｓｏｌｕｔｅ　Ｄｉｆｆｅｒｅｎｃｅ）、相関法のいずれを用いることも可能である。ＳＳＤＡが使用される場合、類似度Ｌαｉ，ｊは、
【数３】
2004012429.tif 000005
により算出される。ここでＳＳＤＡαｉ，ｊは、
【数４】
2004012429.tif 000006
であり、εは、０で割る演算を防止するために用いられる所定の微小値である。
【００６２】
一方、ＳＡＤが使用される場合、類似度Ｌαｉ，ｊは、
【数５】
2004012429.tif 000007
により算出される。ここでＳＡＤαｉ，ｊは、
【数６】
2004012429.tif 000008
であり、εは、０で割る演算を防止するために用いられる所定の微小値である。
【００６３】
更に、相関法が使用される場合、類似度Ｌαｉ，ｊは、
【数７】
2004012429.tif 000009
により算出される。
【００６４】
算出された類似度Ｌαｉ，ｊは、記憶装置７に保存される（ステップＳ１７）。０≦ｉ≦ｐ，０≦ｊ≦ｑなる全てのｉ，ｊの組について、１度ずつ、ステップＳ１５〜Ｓ１７が実行されると、位置同定ループが完了する（ステップＳ１８）。
【００６５】
０°から３６０°までの範囲で、所定の間隔Δαで定められた全てのαについて、１度ずつ上述のステップＳ１３〜Ｓ１８が実行されると、姿勢同定ループが完了する（ステップＳ１９）。階層化された位置同定ループと姿勢同定ループとが実行されることにより、所定の間隔Δαで定められた全てのαについて、且つ、０≦ｉ≦ｐ，０≦ｊ≦ｑなる全てのｉ，ｊの組について、類似度Ｌαｉ，ｊが算出される。これは、所定の間隔Δαで定められた全てのαについて、且つ、０≦ｉ≦ｐ，０≦ｊ≦ｑなる全てのｉ，ｊの組について、回転画像Ｐαと事前撮像画像Ｐｐｒｅｉ，ｊとを対比することと等価である。
【００６６】
ステップＳ１２〜ステップＳ１９における類似度Ｌαｉ，ｊの算出が完了すると、類似度Ｌαｉ，ｊに基づいて、移動体２の位置（ｘ，ｙ）と、姿勢θとが同定される。類似度Ｌαｉ，ｊを最大にするα，ｉ，ｊをそれぞれα１，ｉ１，ｊ１として、移動体２の位置（ｘ，ｙ）は、位置（ｘｉ１，ｙｉ１）と同定され、姿勢θは、α１と同定される。
【００６７】
以上で、移動体２の位置（ｘ，ｙ）と姿勢θとを算出する処理が完了する。以上の処理は、演算装置６が自己位置／姿勢同定プログラム７ｂを実行することにより実現されている。
【００６８】
本実施の形態では、該自己位置／姿勢同定装置の稼動の前に予め撮影された事前撮影画像と、移動体２の位置及び姿勢の同定のために撮影された撮影画像とが対比され、この対比により、移動体２の位置及び姿勢の同定が行われる。当該自己位置／姿勢同定装置は、移動体２の位置及び姿勢の同定のために、蛍光灯やマーカのような特別な構造体を天井に設けることを必要としない。
【００６９】
更に、本実施の形態では、移動体２の位置及び姿勢の同定のときに撮影される撮像画像と事前撮像画像とを、極座標の円周方向に微分することによってエッジ画像が生成され、該エッジ画像が対比することによって、移動体２の位置及び姿勢の同定が行われる。これにより、移動体２の位置及び姿勢の同定の精度が向上されている。円周方向に微分されて生成されたエッジ画像には、主として、実空間における天井方向（ｚ軸方向）の構造体のエッジが抽出されることになる。人工建造物の内部において、天井方向（ｚ軸方向）に延伸する構造体は、隣接する壁１ｃにより形成される稜線１ｄや柱（図示されない）の例で分かるように、一般に、その位置の変動が少ない。従って、事前撮像画像と、移動体２の位置及び姿勢の同定のために撮影される撮像画像とから生成されたエッジ画像には、いずれも、位置の変動が小さい構造体のエッジが抽出されることになる。このようなエッジ画像を用いて移動体２の位置及び姿勢の同定を行うことにより、移動体２の位置及び姿勢の同定の精度を向上することができる。
【００７０】
本実施の形態において、カメラ３が有するレンズ３ａは、魚眼レンズであることが好ましい。レンズ３ａが魚眼レンズであることにより、カメラ３の撮像範囲が床面１ａに平行な方向に拡大され、隣接する壁１ｃにより形成される稜線１ｄや柱（図示されない）のように、天井方向（ｚ軸方向）に延伸する構造体を一層に確実に撮像することが可能になる。
【００７１】
なお、本実施の形態のステップＳ２０において、より詳細に移動体２の位置（ｘ，ｙ）と姿勢θとを同定するために、移動体２の位置（ｘ，ｙ）と姿勢θとは、類似度Ｌαｉ，ｊのうちの上位Ｎ番目までに基づいて算出されることも可能である。この場合、類似度Ｌαｉ，ｊが第δ番目に大きくなるようなα，ｉ，ｊを、それぞれαδ、ｉδ、ｊδとすると、移動体２の位置のｘ座標は、ｘｉ１〜ｘｉＮに、それぞれ類似度Ｌα１ｉ１，ｊ１〜ＬαＮｉＮ，ｊＮに対応した重み付けを与えて算出した平均値として算出される。同様に、移動体２の位置のｙ座標は、ｙｉ１〜ｙｉＮに、それぞれ類似度Ｌα１ｉ１，ｊ１〜ＬαＮｉＮ，ｊＮに対応した重み付けを与えて算出した平均値として算出される。同様に、姿勢θは、α１〜αｉＮにそれぞれ類似度Ｌα１ｉ１，ｊ１〜ＬαＮｉＮ，ｊＮに対応した重み付けを与えて算出した平均値として算出される。このような算出方法は、事前撮影画像が撮影された位置（ｘ０，ｙ０）〜（ｘｐ，ｙｑ）以外の位置の検出が可能である点で好ましい。
【００７２】
また、本実施の形態において、テンプレート画像データベース７ａには、特徴量マトリックスＭｉｎｉ０，０〜Ｍｉｎｉｐ，ｑの代わりに、事前撮影画像そのものを示すデータ、事前撮影画像から生成されたエッジ画像を示すデータ、又は、エッジ画像から生成された２値化エッジ画像を示すデータのいずれを保存しておくことが可能である。この場合、図１２のステップＳ１５において、テンプレート画像データベース７ａから事前撮影画像、エッジ画像、又は、２値化エッジ画像が読み出され、読み出された画像から、上述と同一の方法により、特徴量マトリックスＭｉｎｉｉ，ｊが算出される。ステップＳ１６における類似度Ｌαｉ，ｊの算出では、算出された特徴量マトリックスＭｉｎｉｉ，ｊが使用される。但し、特徴量マトリックスＭｉｎｉ０，０〜Ｍｉｎｉｐ，ｑは、データ量を小さくすることが容易であり、特徴量マトリックスＭｉｎｉ０，０〜Ｍｉｎｉｐ，ｑをテンプレート画像データベース７ａに記憶することは、大きな記憶領域を必要としない点で好ましい。
【００７３】
また、本実施の形態では、移動体２の位置及び姿勢の同定のときに撮影される撮像画像と事前撮影画像との対比は、有効撮像領域Ｒｅｆｆの全体について行われるが、有効撮像領域Ｒｅｆｆの一部を対比することによって移動体２の位置及び姿勢を同定することが可能である。これにより、撮像画像の撮影毎に、構造体の形状が変動する領域を除いて、移動体２の位置及び姿勢を同定することが可能になる。
【００７４】
この場合、有効撮像領域Ｒｅｆｆの中央部のみが対比に使用され、中央部を取り囲む周辺部は、対比に使用されないことが好ましい。即ち、有効撮像領域Ｒｅｆｆの半径をｒｅｆｆとし、ｒｍａｘをｒｅｆｆより小さい正数として、有効撮像領域Ｒｅｆｆのうち、
０≦ｒ≦ｒｍａｘ（＜ｒｅｆｆ），
なる領域のみが、移動体２の位置及び姿勢の同定のときに撮影される撮像画像と事前撮影画像とから切り出され、切り出された部分が対比されることが好ましい。有効撮像領域Ｒｅｆｆの周辺部は、床面１ａの近くに存在する構造体が写される。床面１ａの近くには、人のように、移動可能な構造体が存在することが多い。有効撮像領域Ｒｅｆｆの周辺部が対比から排除されることにより、人のような移動可能な構造体の存在の影響を排除することができる。
【００７５】
また、本実施の形態において、日照、照明、物の配置上等の環境条件が異なる状態で、一の位置から複数の事前撮像画像が撮像され、該事前撮像画像からそれぞれ生成された特徴量マトリックスが、移動体２の位置及び姿勢の同定のときに撮影される撮像画像と事前撮影画像との対比において使用されることが可能である。位置（ｘｉ，ｙｊ）について、第１〜第ｇ環境条件で撮像された事前撮像画像から撮像された特徴量マトリックスをそれぞれＭｉｎｉｉ，ｊ１〜Ｍｉｎｉｉ，ｊｇとすると、特徴量マトリックスＭｉｎｉｉ，ｊの代わりに特徴量マトリックスＭｉｎｉｉ，ｊ１〜Ｍｉｎｉｉ，ｊｇをそれぞれ使用することにより、特徴量マトリックスＭｉｎｉｉ，ｊ１〜Ｍｉｎｉｉ，ｊｇのそれぞれについて上述の類似度Ｌαｉ，ｊが算出される。図１２のステップＳ２０では、この類似度Ｌαｉ，ｊを最大にするような位置（ｘｉ，ｙｊ）が移動体２の位置（ｘ、ｙ）と定められ、類似度Ｌαｉ，ｊを最大にするようなαが移動体２の姿勢θであると定められる。
【００７６】
また、本実施の形態において、移動体２が存在し得る位置を考慮することにより、位置の探索範囲を減少することが可能である。この場合、図１４に示されているように、時刻を測定する時計装置８が制御コンピュータ４に追加される。移動体２の位置及び姿勢の同定が行われる毎に、同定された移動体２の位置と、該同定が行われた時刻が記憶装置７に記録される。新たに位置及び姿勢の同定が行われる場合、最も最近に同定された位置と、その位置が同定された時刻と、移動体２の最大移動速度とから、移動体２の存在し得る存在範囲が特定される。図１２のステップＳ１４〜Ｓ１８は、位置（ｘｉ、ｙｊ）が該存在範囲の内部にあるｉ，ｊについてのみ実行され、算出対象とされる類似度Ｌαｉ，ｊの数が減少される。これにより、移動体２の位置及び姿勢の同定に必要な演算量を減少することができる。
【００７７】
また、本実施の形態においては、移動体２の位置と姿勢の両方が同定されているが、必要に応じて、位置と姿勢のうちのいずれか一方のみが同定されることも可能である。
【００７８】
（実施の第２形態）
実施の第１形態では、セグメント領域Ｓα１，１〜Ｓαｎ，ｍの決定の基準となる方向を可変にすることにより、移動体２の位置及び姿勢の同定のときに撮像される撮像画像を実質的に回転する回転演算が行われているが、実施の第２形態では、撮像画像を回転する回転演算は行われない。その代り、同一の位置から複数の事前撮像画像が異なる姿勢θで撮像され、撮像された事前撮像画像と、移動体２の位置及び姿勢の同定のときに撮像される撮像画像とが対比されることにより、移動体２の位置及び姿勢の同定が行われる。
【００７９】
図１５は、実施の第２形態の自己位置／姿勢同定装置が搭載された移動体２を示している。実施の第２形態では、テンプレート画像データベース７ａの代わりに、テンプレート画像データベース７ａ’　が記憶装置７に記憶される。更に、移動体２の位置及び姿勢の同定の手順の変更に伴い、自己位置／姿勢同定プログラム７ｂの代わりに、自己位置／姿勢同定プログラム７ｂ’が記憶装置７に記憶される。
【００８０】
図１６は、実施の第２形態で使用されるテンプレート画像データベース７ａ’の内容を示している。テンプレート画像データベース７ａ’には、特徴量マトリックスＭｉｎｉ０，０，０〜Ｍｉｎｉｐ，ｑ，ｒが保存されている。特徴量マトリックスＭｉｎｉ０，０、０〜Ｍｉｎｉｐ，ｑ，ｒは、該自己位置／姿勢同定装置の稼動の前にカメラ３によって撮像された事前撮像画像から生成されるデータである。ｉを０以上ｐ以下の整数とし、ｊを０以上ｑ以下の整数とし、ｋを、０以上ｒ以下の整数としたとき、特徴量マトリックスＭｉｎｉｉ，ｊ，ｋは、移動体２が位置（ｘｉ，ｙｊ）を有し、且つ、姿勢θｋを有する状態でカメラ３によって撮像された事前撮像画像の特徴量が記述されたマトリックスである。
【００８１】
特徴量マトリックスＭｉｎｉ０，０，０〜Ｍｉｎｉｐ，ｑ，ｒの生成方法は、実施の第１形態で説明された特徴量マトリックスＭｉｎｉ０，０〜Ｍｉｎｉｐ，ｑの生成方法と同一である。実施の第１形態と同様にして、事前撮像画像からエッジ画像が抽出され、該エッジ画像から、２値化エッジ画像が生成される。２値化エッジ画像にセグメント領域Ｓｉｎｉ１，１〜Ｓｉｎｉｎ，ｍが定められ、セグメント領域Ｓｉｎｉ１，１〜Ｓｉｎｉｎ，ｍに含まれるエッジ存在ピクセルの度数を要素とする行列が、特徴量マトリックスとされる。特徴量マトリックスＭｉｎｉｉ，ｊ，ｋは、
【数８】
2004012429.tif 000010
により算出される。ここで、Ｎｉｎｉｓ，ｔ（ｉ，ｊ，ｋ）は、移動体２が位置（ｘｉ，ｙｊ）及び姿勢θｋを有する状態で撮像された事前撮像画像から生成された２値化エッジ画像のセグメント領域Ｓｉｎｉｓ，ｔに含まれるエッジ存在ピクセルの度数である。
【００８２】
移動体２の位置及び姿勢を同定する際には、カメラ３によって撮像画像が撮像される。該撮像画像から、特徴量マトリックスＭ０が生成される。特徴量マトリックスＭ０の生成方法は、実施の第１形態においてα＝０としたときの特徴量マトリックスＭαの生成方法と同一である。特徴量マトリックスＭ０は、
【数９】
2004012429.tif 000011
により算出される。Ｎ０ｓ，ｔは、式（３）にα＝０を代入することにより規定されるセグメント領域Ｓ０ｓ，ｔの内部に含まれるエッジ存在ピクセルの度数である。
【００８３】
特徴量マトリックスＭ０と特徴量マトリックスＭｉｎｉ０，０、０〜Ｍｉｎｉｐ，ｑ，ｒとを用いて、移動体２の位置及び姿勢の同定の際に撮影された撮像画像と、事前撮像画像との対比が行われる。撮像画像と事前撮像画像との対比のために、これらが類似している程度を示す類似度Ｌｉ，ｊ，ｋが算出される。類似度Ｌｉ，ｊ，ｋの算出は、ＳＳＤＡ（Ｓｅｑｕｅｎｔｉａｌ　Ｓｉｍｉｌａｒｉｔｙ　Ｄｅｔｅｃｔｉｏｎ　Ａｌｇｏｒｉｔｈｍ）、ＳＡＤ（Ｓｕｍ　ｏｆ　Ａｂｓｏｌｕｔｅ　Ｄｉｆｆｅｒｅｎｃｅ）、相関法のいずれを用いることも可能である。
【００８４】
ＳＳＤＡが使用される場合、類似度Ｌｉ，ｊ，ｋは、
【数１０】
2004012429.tif 000012
により算出される。ここでＳＳＤＡｉ，ｊ，ｋは、
【数１１】
2004012429.tif 000013
であり、εは、０で割る演算を防止するために用いられる所定の微小値である。
【００８５】
一方、ＳＡＤが使用される場合、類似度Ｌｉ，ｊ，ｋは、
【数１２】
2004012429.tif 000014
により算出される。ここでＳＡＤｉ，ｊ，ｋは、
【数１３】
2004012429.tif 000015
であり、εは、０で割る演算を防止するために用いられる所定の微小値である。
【００８６】
更に、相関法が使用される場合、類似度Ｌｉ，ｊ，ｋは、
【数１４】
2004012429.tif 000016
により算出される。
【００８７】
移動体２の位置及び姿勢は、類似度Ｌｉ，ｊ，ｋから同定される。類似度Ｌｉ，ｊ，ｋを最大にするｉ，ｊ，ｋをそれぞれ、ｉ１，ｊ１，ｋ１として、移動体２の位置（ｘ，ｙ）は、（ｘｉ１，ｙｊ１）と同定され、移動体２の姿勢θは、θｋ１と同定される。
【００８８】
以上で、移動体２の位置（ｘ，ｙ）と姿勢θとを算出する処理が完了する。以上の処理は、演算装置６が自己位置／姿勢同定プログラム７ｂ’を実行することにより実現されている。
【００８９】
実施の第２形態では、実施の第１形態と同様に、該自己位置／姿勢同定装置の稼動の前に予め撮影された事前撮影画像と、移動体２の位置及び姿勢の同定のために撮影された撮影画像とが対比され、この対比により、移動体２の位置及び姿勢の同定が行われる。移動体２の位置及び姿勢の同定のために、蛍光灯やマーカのような特別な構造体を天井に設けることを必要としない。
【００９０】
更に、実施の第２形態では、実施の第１形態と同様に、移動体２の位置及び姿勢の同定の際に撮影される撮像画像と事前撮像画像とを円周方向に微分することによってエッジ画像が生成され、該エッジ画像を使用して撮像画像と事前撮像画像との対比が行われる。このようなエッジ画像を用いて移動体２の位置及び姿勢の同定を行うことにより、移動体２の位置及び姿勢の同定の精度を向上することができる。
【００９１】
更に実施の第２形態では、移動体２の位置及び姿勢の同定のために撮影された撮影画像を回転する回転処理が行われず、実施の第１形態に比較して、移動体２の位置及び姿勢の同定のために必要な演算量の抑制が可能である。但し、実施の第２形態においてテンプレート画像データベース７ａ’に蓄積する必要があるデータ量は、実施の第１形態においてテンプレート画像データベース７ａに蓄積する必要があるデータ量に比べて大きい。記憶装置７の記憶容量を小さくする観点では、実施の第１形態が好ましい。
【００９２】
なお、実施の第２形態では、特徴量マトリックスＭｉｎｉ０，０，０〜Ｍｉｎｉｐ，ｑ，ｒは、位置（ｘ０，ｙ０）〜（ｘｐ，ｙｑ）のそれぞれについて、且つ、姿勢θ０〜θｒのそれぞれについて撮像された事前撮像画像から生成されているが、一の姿勢θ０についてのみ撮像された事前撮像画像からＭｉｎｉ０，０，０〜Ｍｉｎｉｐ，ｑ，０が生成され、姿勢θ０以外の姿勢については、該撮像された事前撮像画像を回転した画像から、特徴量マトリックスＭｉｎｉ０，０，１〜Ｍｉｎｉｐ，ｑ，ｒが生成されることも可能である。これにより、必要な事前撮像画像の数を減少することができる。
【００９３】
また、実施の第２形態において、より詳細に移動体２の位置（ｘ，ｙ）と姿勢θとを同定するために、移動体２の位置（ｘ，ｙ）と姿勢θとが、類似度Ｌｉ，ｊ，ｋのうちの上位Ｎ番目までに基づいて算出されることも可能である。この場合、類似度Ｌｉ，ｊ、ｋが第δ番目に大きくなるようなｉ，ｊ，ｋを、それぞれ、ｉδ、ｊδ、ｋδとして、移動体２の位置のｘ座標は、ｘｉ１〜ｘｉＮにそれぞれ類似度Ｌｉ１，ｊ１，ｋ１〜ＬｉＮ，ｊＮ，ｋＮに対応した重み付けを与えて算出した平均値として算出される。同様に、移動体２の位置のｙ座標は、ｙｉ１〜ｙｉＮに、それぞれ類似度Ｌｉ１，ｊ１，ｋ１〜ＬｉＮ，ｊＮ、ｋＮに対応した重み付けを与えて算出した平均値として算出される。同様に、姿勢θは、θｋ１〜θｋＮにそれぞれ類似度Ｌｉ１，ｊ１，ｋ１〜ＬｉＮ，ｊＮ、ｋＮに対応した重み付けを与えて算出した平均値として算出される。このような算出方法は、事前撮影画像が撮影された位置（ｘ０，ｙ０）〜（ｘｐ，ｙｑ）以外の位置の検出が可能である点で好ましい。
【００９４】
また、実施の第２形態において、テンプレート画像データベース７ａ’には、特徴量マトリックスＭｉｎｉ０，０，０〜Ｍｉｎｉｐ，ｑ，ｒの代わりに、事前撮影画像そのものを示すデータ、事前撮影画像から生成されたエッジ画像を示すデータ、又は、エッジ画像から生成された２値化エッジ画像を示すデータのいずれを保存しておくことが可能である。この場合、類似度Ｌｉ，ｊ、ｋの算出のときに、テンプレート画像データベース７ａ’から事前撮影画像、エッジ画像、又は、２値化エッジ画像が読み出され、読み出された画像から、上述と同一の方法により、特徴量マトリックスＭｉｎｉｉ，ｊ，ｋが算出される。類似度Ｌｉ，ｊ，ｋの算出では、算出された特徴量マトリックスＭｉｎｉｉ，ｊ，ｋが使用される。但し、特徴量マトリックスＭｉｎｉ０，０，０〜Ｍｉｎｉｐ，ｑ，ｒは、データ量を小さくすることが容易であり、特徴量マトリックスＭｉｎｉ０，０，０〜Ｍｉｎｉｐ，ｑ，ｒをテンプレート画像データベース７ａ’に記憶することは、大きな記憶領域を必要としない点で好ましい。
【００９５】
また、実施の第２形態でも、実施の第１形態と同様に、有効撮像領域Ｒｅｆｆの一部を対比することによって移動体２の位置及び姿勢を同定することが可能である。これにより、撮像画像の撮影毎に、構造体の形状が変動する領域を除いて、移動体２の位置及び姿勢を同定することが可能になる。
【００９６】
この場合、実施の第１形態と同様に、有効撮像領域Ｒｅｆｆの周辺部が対比に使用されないことが好ましい。即ち、有効撮像領域Ｒｅｆｆの半径をｒｅｆｆとし、ｒｍａｘをｒｅｆｆより小さい正数として、有効撮像領域Ｒｅｆｆのうち、
０≦ｒ≦ｒｍａｘ（＜ｒｅｆｆ），
なる領域のみが、移動体２の位置及び姿勢の同定のときに撮影される撮像画像と事前撮影画像との対比において使用されることが好ましい。有効撮像領域Ｒｅｆｆの周辺部が対比から排除されることにより、人のような移動可能な構造体の存在の影響を排除することができる。
【００９７】
また、実施の第２形態において、日照、照明、物の配置上等の環境条件が異なる状態で、一の位置から複数の事前撮像画像が撮像され、該事前撮像画像からそれぞれ生成された特徴量マトリックスが、移動体２の位置及び姿勢の同定のときに撮影される撮像画像と事前撮影画像との対比において使用されることが可能である。位置（ｘｉ，ｙｊ）及び姿勢θｋについて、第１〜第ｇ環境条件で撮像された事前撮像画像から撮像された特徴量マトリックスをそれぞれＭｉｎｉｉ，ｊ，ｋ１〜Ｍｉｎｉｉ，ｊ，ｋｇとすると、特徴量マトリックスＭｉｎｉｉ，ｊ，ｋの代わりに特徴量マトリックスＭｉｎｉｉ，ｊ，ｋ１〜Ｍｉｎｉｉ，ｊ，ｊｇをそれぞれ使用することにより、特徴量マトリックスＭｉｎｉｉ，ｊ，ｋ１〜Ｍｉｎｉｉ，ｊ，ｋｇのそれぞれについて上述の類似度Ｌｉ，ｊ，ｋが算出される。類似度Ｌｉ，ｊ，ｋを最大にするような位置（ｘｉ，ｙｊ）及び姿勢θｋが、それぞれ、移動体２の位置（ｘ、ｙ）及び姿勢θと定められる。
【００９８】
また、実施の第２形態において、実施の第１形態と同様に、移動体２が存在し得る位置を考慮することにより、位置の探索範囲を減少することが可能である。この場合、実施の第１形態と同様に、時刻を測定する時計装置が制御コンピュータ４に追加され、移動体２の位置及び姿勢の同定が行われる毎に、該同定が行われた時刻が記憶装置７に記録される。新たに位置及び姿勢の同定が行われる場合、最も最近に位置及び姿勢の同定が行われた時刻と、移動体２の最大移動速度とから、移動体２の存在し得る存在範囲が特定される。類似度Ｌｉ，ｊ，ｋの算出は、位置（ｘｉ、ｙｊ）が該存在範囲の内部にあるｉ，ｊについてのみ実行される。これにより、算出対象である類似度Ｌｉ，ｊ，ｋの数が減少され、移動体２の位置及び姿勢の同定に必要な演算量を減少することができる。
【００９９】
また、実施の第２形態においては、移動体２の位置と姿勢の両方が同定されているが、必要に応じて、位置と姿勢のうちのいずれか一方のみが同定されることも可能である。
【０１００】
【発明の効果】
本発明により、天井に蛍光灯やマーカのような特別な構造体を設けなくても室内移動体の位置及び姿勢の同定が可能な自己位置／姿勢同定装置が提供される。
【０１０１】
また、本発明により、室内移動体の位置及び姿勢を一層に正確に同定できる自己位置／姿勢同定装置が提供される。
【０１０２】
また、本発明により、室内移動体の位置及び姿勢を同定するために必要な演算量、及びデータ量を抑制することが可能な自己位置／姿勢同定装置が提供される。
【図面の簡単な説明】
【図１】図１は、本発明による自己位置／姿勢同定装置の実施の第１形態を示す。
【図２】図２は、実施の第１形態の自己位置／姿勢同定装置が搭載された移動体２が使用される部屋１を示す。
【図３】図３は、移動体２の姿勢θの定義を示す。
【図４】図４は、有効撮像領域Ｒｅｆｆを示す。
【図５】図５は、テンプレート画像データベース７ａの内容を示す。
【図６】図６は、エッジ画像が抽出される態様を示す。
【図７】図７は、エッジ画像が２値化されて２値化エッジ画像が生成される過程を示す。
【図８】図８は、セグメント領域Ｓｉｎｉｓ，ｔの決定方法を示す。
【図９】図９は、セグメント領域Ｓｉｎｉｓ，ｔに含まれるエッジ存在ピクセルの度数の算出方法を示す。
【図１０】図１０は、エッジ存在ピクセルの度数のヒストグラムを示す。
【図１１】図１１は、エッジ存在ピクセルの度数から、特徴量マトリックスが生成される過程を示す。
【図１２】図１２は、移動体２の位置及び姿勢が同定される過程を示すフローチャートである。
【図１３】図１３は、セグメント領域Ｓαｓ，ｔの決定方法と、セグメント領域Ｓαｓ，ｔに含まれるエッジ存在ピクセルの度数の算出方法とを示す。
【図１４】図１４は、実施の第１形態の自己位置／姿勢同定装置の変形例を示す。
【図１５】図１５は、本発明による自己位置／姿勢同定装置の実施の第２形態を示す。
【図１６】図１６は、テンプレート画像データベース７ａ’の内容を示す。
【符号の説明】
１：部屋
２：移動体
３：カメラ
４：制御コンピュータ
５：インターフェース
６：演算装置
７：記憶装置
７ａ、７ａ’：テンプレート画像データベース
７ｂ、７ｂ’：自己位置／姿勢同定プログラム
８：時計装置
【図１】
2004012429.tif 000017
【図２】
2004012429.tif 000018
【図３】
2004012429.tif 000019
【図４】
2004012429.tif 000020
【図５】
2004012429.tif 000021
【図６】
2004012429.tif 000022
【図７】
2004012429.tif 000023
【図８】
2004012429.tif 000024
【図９】
2004012429.tif 000025
【図１０】
2004012429.tif 000026
【図１１】
2004012429.tif 000027
【図１２】
2004012429.tif 000028
【図１３】
2004012429.tif 000029
【図１４】
2004012429.tif 000030
【図１５】
2004012429.tif 000031
【図１６】
2004012429.tif 000032