JP2016177388A - 移動体位置姿勢計測装置 - Google Patents

Abstract

【課題】移動体位置姿勢計測装置において、周囲に移動物体がある環境でも精度の高い環境地図を生成できるようにする。
【解決手段】移動を検出する移動検出部と、外界を撮影する撮像部と、記憶部と、撮像部で撮影した画像から画像特徴点を抽出しその情報を記憶部に保存する画像特徴点抽出部と、撮像部で撮影される時系列の各画像において、保存されている画像特徴点の情報が示す画像特徴点を追跡し、最新の画像特徴点の情報となるよう更新する画像特徴点追跡部と、時系列の各画像中の背景画像および該画像中の移動物体をマスクするための前景マスクを生成し更新する前景マスク生成部と、移動体が停止状態から運動状態になったと判断される場合、記憶部の画像特徴点の情報が示す画像特徴点のうち前景マスクでマスクされない画像特徴点を用いて環境地図を生成する環境地図生成部を備える。
【選択図】図１

Description

本発明は、移動体位置姿勢計測装置に関する。

車などの移動体の位置・姿勢を、この移動体に取り付けたカメラを用いて検出するＶｉｓｕａｌ−ＳＬＡＭ（Ｖｉｓｕａｌ−Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ）と呼ばれる技術がある。Ｖｉｓｕａｌ−ＳＬＡＭとは、時系列の映像から、環境地図と呼ばれる周囲の静止物体の３次元位置情報を含む地図の生成と、移動体の位置・姿勢の推定を同時に行う手法である。Ｖｉｓｕａｌ−ＳＬＡＭでは、移動体の位置・姿勢推定に環境地図を利用するため、精度の良い位置・姿勢を得るためには、生成する環境地図の精度が高いことが望ましい。しかし、この環境地図は、周囲がすべて静止物体で構成されているという前提があり、移動体や人物などの移動物体が、環境地図を生成する範囲内に入ってしまうと環境地図の精度が悪くなる。

Ｖｉｓｕａｌ−ＳＬＡＭの１手法として、画像特徴点を用いる手法がある。画像特徴点を用いる手法では、環境地図は３次元の座標を持った３次元空間内の点の集合として定義され、車両の位置・姿勢は、その３次元点の座標と、その３次元点が観測された画像上の座標とをもとに推定される。

ところが、Ｖｉｓｕａｌ−ＳＬＡＭの特性上、画像特徴点を用いる手法では、位置推定を開始する時点において、Ｖｉｓｕａｌ−ＳＬＡＭによって環境地図を生成することはできないので、予め環境地図を何かしらの方法で生成する必要がある。このＶｉｓｕａｌ−ＳＬＡＭ処理の前に生成する環境地図は初期マップと呼ばれる。

初期マップの生成方法として、単眼カメラを並行移動させて十分視差が出た移動前の画像と移動後の画像間で対応づけられる画像特徴点のオプティカルフローから移動物体を除いて、３次元復元することにより初期マップを生成する方法や、ステレオカメラを用いて一度の撮影で初期マップを生成する方法が知られている。

また、特許文献１では、撮影手段によって撮影した画像データと、位置検知手段によって検知した撮影時の位置情報と、地図情報とを関連付けてマップデータを作成することにより、精度の高い環境地図を生成するマップデータ作成装置が提案されている。

しかしながら、上記の単眼カメラを用いて初期マップを生成する方法では、人混みや交差点など、周囲に移動物体が多い環境においては移動物体の除去が不十分となり、移動物体の画像特徴点のオプティカルフローの影響で精度の高い環境地図が生成できないことがあるという問題があった。

また、ステレオカメラを用いて一度の撮影で初期マップを生成する方法では、一度だけの撮影では移動物体と静止物体との区別がつかないため、初期マップに移動物体が入ってしまうことがあり、周囲に移動物体が多いような場面では環境地図の精度が悪くなる問題があった。また、特許文献１に開示の手法においても、周囲に移動物体がある環境では精度の高い環境地図を生成することが困難である問題は解消できていない。

本発明は、上記に鑑みてなされたものであって、Ｖｉｓｕａｌ−ＳＬＡＭによる移動体位置姿勢計測装置において、周囲に移動物体がある環境でも精度の高い環境地図を生成できるようにすることを目的とする。

上述した課題を解決し、目的を達成するために、本発明は、移動体の移動を検出する移動検出部と、前記移動体の外界を撮影する撮像部と、記憶部と、前記撮像部で撮影した画像から画像特徴点を抽出し該画像特徴点の情報を前記記憶部に保存する画像特徴点抽出部と、前記撮像部で撮影される時系列の各画像において、前記記憶部に保存されている前記画像特徴点の情報が示す画像特徴点を追跡し、前記記憶部の画像特徴点の情報が最新の画像特徴点の情報となるよう更新する画像特徴点追跡部と、前記撮像部で撮影される時系列の各画像中の背景画像および該画像中の移動物体をマスクするための前景マスクを生成し更新する前景マスク生成部と、前記移動検出部の検出結果から得られる前記移動体の移動に関する情報から前記移動体が停止状態から運動状態になったと判断される場合、前記記憶部の画像特徴点の情報が示す画像特徴点のうち前記前景マスクでマスクされない画像特徴点を用いて環境地図を生成する環境地図生成部と、前記環境地図生成部が生成した環境地図を基に前記移動体の位置姿勢計測を行う移動体位置計測部と、を備えたことを特徴とする。

本発明によれば、Ｖｉｓｕａｌ−ＳＬＡＭによる移動体位置姿勢計測装置において、周囲に移動物体がある環境でも精度の高い環境地図を生成することができ、その結果、移動体の位置姿勢計測の精度を高めることができるという効果を奏する。

図１は、第１の実施形態の移動体位置姿勢計測装置の全体構成を示すブロック図である。 図２は、前景マスク生成部による前景マスク更新処理を説明するフローチャートである。 図３は、背景画像の一例を示す図である。 図４は、ある時点で撮影されたフレームの一例を示す図である。 図５は、図４に例示したフレームを前景マスクでマスクした状態を示した図である。 図６は、環境地図生成部にかかる動作について説明するフローチャートである。 図７は、環境地図生成処理を説明するフローチャートである。 図８は、第２の実施形態として、ステレオカメラを用いた移動体位置姿勢計測装置の全体構成を示すブロック図である。 図９Ａは、第２の実施形態における前景マスク生成部での前景マスク更新処理について説明するフローチャートである。 図９Ｂは、第２の実施形態における前景マスク生成部での背景距離画像更新処理について説明するフローチャートである。 図１０は、背景距離画像の一例を示す図である。 図１１は、ある時点で撮影されたフレームに対する距離画像の一例を示す図である。 図１２は、図１０の例に対応するフレームに対し、前景マスクでマスクした状態を示した図である。

以下に添付図面を参照して、移動体位置姿勢計測装置の諸実施形態を詳細に説明する。

（第１の実施形態）
本実施形態の移動体位置姿勢計測装置は、Ｖｉｓｕａｌ−ＳＬＡＭ（Ｖｉｓｕａｌ−Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ）における環境地図の生成処理に特徴を有する。具体的には、本実施形態の移動体位置姿勢計測装置は、当該移動体位置姿勢計測装置を備える移動体（以下、自己の移動体と表現する）が停止していると判断した場合に、カメラ（撮像部）で撮影した画像を背景と移動物体の領域に分離できるようにする。そして、本実施形態の移動体位置姿勢計測装置は、自己の移動体が移動を始めたと判断した場合に、その直前に撮影した画像の背景領域にある画像特徴点のみから環境地図（この段階では初期マップ）を作成することが特徴になっている。以下に、本実施形態の移動体位置姿勢計測装置の詳細について説明する。

図１は、本実施形態の移動体位置姿勢計測装置の全体構成を示すブロック図である。

本実施形態の移動体位置姿勢計測装置１は、移動検出部１１、撮像部１２、画像特徴点抽出部１３、画像特徴点追跡部１４、前景マスク生成部１５、環境地図生成部１６、および、移動体位置計測部１７からなる。

移動検出部１１は、ホイールエンコーダなどの速度を計測するセンサ、または、速度を計測するセンサと加速度センサ・ジャイロセンサなどからなる内界センサ、または、ＧＰＳなどの絶対位置を計測するセンサから構成され、移動体位置姿勢計測装置１を備える移動体自身の移動を検出する装置である。ここでは内界センサを用いるものとする。

撮像部１２は、移動体位置姿勢計測装置１を備える移動体の外界（周囲）を撮影し、時系列の画像データを順次出力するカメラ（ここでは単眼カメラ）である。

画像特徴点抽出部１３は、一例としてｈａｒｒｉｓ作用素（Ａ ＣＯＭＢＩＮＥＤ ＣＯＲＮＥＲ ＡＮＤ ＥＤＧＥ ＤＥＴＥＣＴＯＲ Ｃｈｒｉｓ Ｈａｒｒｉｓ ＆ Ｍｉｋｅ Ｓｔｅｐｈｅｎｓ Ｐｌｅｓｓｅｙ Ｒｅｓｅａｒｃｈ Ｒｏｋｅ Ｍａｎｏｒ， Ｕｎｉｔｅｄ Ｋｉｎｇｄｏｍ， １９８８）を用いて、撮像部１２が撮影した画像から画像特徴点（以下、特徴点と略記す）を抽出し、画像特徴点情報２１として、ＲＡＭ，Ｆｌａｓｈ ＲＯＭ等の書き換え可能なメモリ等からなる記憶部２０に保存する。なお、特徴点の抽出方法は、ｈａｒｒｉｓ作用素によるものに限らず、それ以外の特徴点抽出方法を採用することができる。

画像特徴点追跡部１４は、一例としてブロックマッチングを用いて、記憶部２０に保存されている下記の画像特徴点情報２１が示す特徴点を、撮像部１２が撮影した各画像において追跡し、記憶部２０の画像特徴点情報２１を、追跡した特徴点の最新の座標等を含むものとなるよう更新する。この画像特徴点追跡部１４における特徴点の追跡（対応付け）は、Ｋａｎａｄｅ−Ｌｕｃａｓ追跡器や、特徴量記述子のＳＨＩＦＴ（Ｓｃａｌｅ−Ｉｎｖａｒｉａｎｔ Ｆｅａｔｕｒｅ Ｔｒａｎｓｆｏｒｍ）などを用いて行うことができる。

本実施形態では、画像特徴点の情報として、画像上で観測された特徴点の座標を含む情報である画像特徴点情報２１を用いる。なお、この座標は、画像上の座標でもよいし、事前に求められたカメラパラメタを用いて、ピンホールカメラモデルに正規化された座標でもよい。また、画像特徴点情報２１は、上記座標に加え、画像特徴点追跡部１４がブロックマッチングによる追跡を行う場合は、ブロックマッチングで利用する特徴点の周囲の局所画像を含み、特徴量記述子を用いて行う場合には、特徴点の特徴量記述子を含むものとする。

前景マスク生成部１５は、前景マスク２２を生成し、生成した前景マスク２２を記憶部２０に保存する（詳細は後述）。本実施形態における前景マスク２２は、撮像部１２にて撮影された画像を複数の小領域に分割したそれぞれの領域ついて、その領域が前景であるか否かを示すデータを含む情報である。また、前景マスク生成部１５は、撮像部１２で撮影される時系列の各画像中の背景画像を生成し更新する。

環境地図生成部１６は、移動検出部１１の検出結果に応じた、画像特徴点情報２１と前景マスク２２とから環境地図２３を生成するための処理を行い、生成した環境地図２３は記憶部２０に保存する（処理の詳細は後述）。本実施形態における環境地図２３は、特徴点の各々の３次元位置関係を示すデータを含む情報である。

移動体位置計測部１７は、移動体の位置姿勢計測の処理として、Ｖｉｓｕａｌ−ＳＬＡＭにおいてＰｅｒｓｐｅｃｔｉｖｅ−ｎ−Ｐｏｉｎｔ（ＰｎＰ）問題と呼ばれる非線形最小化問題を解く処理を行う。この処理により、移動体位置計測部１７は、環境地図２３と画像特徴点情報２１とから移動体の位置・姿勢を計測し、計測した位置・姿勢の情報を移動体位置姿勢情報２４として記憶部２０に保存する。この移動体位置計測部１７での処理は、Ｐ３Ｐアルゴリズム（Ｘ． Ｇａｏ， Ｘ． Ｈｏｕ， Ｊ． Ｔａｎｇ， ａｎｄ Ｈ． Ｃｈｅｎｇ． Ｃｏｍｐｌｅｔｅ ｓｏｌｕｔｉｏｎ ｃｌａｓｓｉｆｉｃａｔｉｏｎ ｆｏｒ ｔｈｅ ｐｅｒｓｐｅｃｔｉｖｅ−ｔｈｒｅｅ−ｐｏｉｎｔ ｐｒｏｂｌｅｍ． ＩＥＥＥ ＴＰＡＭＩ， ２５（８）：９３０−９４３， ２００３）や、ＥＰｎＰアルゴリズム（Ｖｉｎｃｅｎｔ Ｌｅｐｅｔｉｔ， Ｆｒａｎｃｅｓｃ Ｍｏｒｅｎｏ−Ｎｏｇｕｅｒ， ａｎｄ Ｐａｓｃａｌ Ｆｕａ． ２００９． ＥＰｎＰ： Ａｎ Ａｃｃｕｒａｔｅ Ｏ（ｎ） Ｓｏｌｕｔｉｏｎ ｔｏ ｔｈｅ ＰｎＰ Ｐｒｏｂｌｅｍ． Ｉｎｔ． Ｊ． Ｃｏｍｐｕｔ． Ｖｉｓｉｏｎ ８１， ２ （Ｆｅｂｒｕａｒｙ ２００９）， １５５−１６６． ＤＯＩ＝１０．１００７／ｓ１１２６３−００８−０１５２−６ ｈｔｔｐ：／／ｄｘ．ｄｏｉ．ｏｒｇ／１０．１００７／ｓ１１２６３−００８−０１５２−６）などに開示の公知の手法を利用する。

ここで、本実施形態における前景マスク生成部１５における処理について説明する。図２は、前景マスク生成部１５による前景マスク更新処理を説明するフローチャートである。

前景マスク生成部１５は、撮像部１２が撮影を行う度に、以下の処理を繰り返す。

ここでは、この処理の時点で撮像部１２により撮影されたフレーム（画像）をフレームＮとし（図４に、撮影されたフレームの一例を示す）、フレームＮと背景画像とを複数の小領域に分け、その領域に１対１で対応する前景マスク２２の領域を定義する。前景マスク生成部１５は、その前景マスク２２の全領域について以下の処理を繰り返し行う（Ｓ１０１）。

まず、前景マスク生成部１５は、前景マスク２２の各領域に対応するフレームＮと背景画像の各領域について、フレームＮの画像と背景画像との輝度値の差分を計算する（Ｓ１０２）。なお、繰り返し処理の最初の段階では、背景画像として、初期背景画像（停止状態になった時点で撮影された輝度画像）を用いる。また、各領域の大きさは、例えば１ピクセルとする。その他の実施例として、各領域を、近接する複数のピクセルで形成される矩形の領域として定義してもよい。各々の領域の輝度値は、領域を複数のピクセルで構成する場合、その領域に含まれるピクセルの持つ輝度値の重み付き平均で表現するものとする。重みの付け方は、事前の試験結果等に基づき、適宜定めるものとする。

続いて、前景マスク生成部１５は、輝度値の差分（ここでは絶対値）と予め定めた閾値を比較し、この差分が閾値以上である場合（Ｓ１０３でＹｅｓ）、その該当領域は背景の前方に物体が現れたとして、前景マスク２２の該当領域をＯＮに設定する（Ｓ１０４）。そうでない場合（Ｓ１０３でＮｏ）、前景マスク生成部１５は、前景マスク２２の該当領域をＯＦＦにする（Ｓ１０５）。

全領域について、小領域毎の更新処理（Ｓ１０１）が終了すると、前景マスク生成部１５は、背景画像を生成し更新する背景画像更新処理を行う（Ｓ１０６）。この背景画像更新処理としては、例えば、移動物体像の抽出技術（情報処理学会論文誌 ２８（４），３９５−４０２，１９８７−０４−１５）に記載の方法がある。この方法は、自己が静止しているような環境であれば、単眼のカメラを用いても背景画像を生成することができる。図３に、背景画像の一例を示す。なお、本実施形態において、背景画像は、各ピクセルが輝度情報（輝度値）を持った画像とする。

上記のようにして、前景マスク生成部１５は、背景画像および前景マスク２２を生成し更新する。

本実施形態では、Ｓ１０３で、前景マスク生成部１５が、フレームＮの画像と背景画像との輝度値の差分を基に前景マスク２２を生成することにより、デジタルサイネージなどで表示された画像なども背景と分離できるようになる。また、領域を複数のピクセルで構成し、各領域の輝度値を、その領域に含まれるピクセルの持つ輝度値の重み付き平均で表現する場合には、さらに背景画像が見た目に安定した輝度値をもつものとして更新されていくので、本実施形態の移動体位置姿勢計測装置１は、デジタルサイネージなどで表示された映像を背景以外として取り除くことができるようになる。その結果、後述のように前景マスク２２を用いて生成される環境地図２３の精度がさらに上がる。

図４に、ある時点で撮影されたフレームの一例を示す。また、図５に、図４に例示したフレームを、上記のようにして生成した前景マスク２２でマスクした状態を示す。同図において、横線で描画された領域がマスク領域である。なお、図５では、カメラの振動、露光の変化、照明の変化など、量子化誤差などによって生じるノイズを除去するため、背景画像に対し、膨張・縮小処理を繰り返す処理を行っている。

次に、本実施形態における環境地図生成部１６にかかる動作について説明する。図６は、環境地図生成部１６にかかる動作について説明するフローチャートである。

環境地図生成部１６は、自己が停止状態か運動状態かを示す状態情報（自己の移動体の移動に関する情報：運動状態／停止状態）を保持しており、この状態情報を基に、自己の状態に応じて振る舞いを変える。そのため、移動検出部１１による検出結果から自己の移動体が動いたか停止したかを判定し、状態情報を更新する処理（状態情報更新処理：Ｓ２０１）を行う。状態情報の更新は、例えば、予め定めた時間間隔または撮像部１２での撮影周期で行うようにする。なお、環境地図生成部１６は、自己の移動体の状態に関して、例えば、移動検出部１１に備わる速度を計測するセンサから求めた自己の速度が０であり、かつ、内界センサの出力値が一定の閾値以下の変化をした場合には移動してない（停止状態）と判定し、そうでない場合は移動している（運動状態）と判定するものとする。

また、環境地図生成部１６は、Ｓ２０１の状態情報更新処理を行う際、この処理による状態情報の更新内容を判定し（Ｓ２０２）、その判定結果に応じて、以下の動作を実行する。

（１）状態情報が停止状態から停止状態となっている場合、すなわち、停止状態のままである場合、環境地図生成部１６は、前景マスク生成部１５により更新されている前景マスク２２を取得する前景マスク更新処理を行う（Ｓ２０３）。前景マスク生成部１５は、前述のように、背景画像および前景マスク２２を更新し続けている。自己の移動体が停止している状態においては、更新される背景画像の精度が高まり、それに応じて前景マスク２２の精度があがる。その結果、下記の（２）の条件下で生成される環境地図２３の精度が上がる。

（２）状態情報が停止状態から運動状態に変化した場合、環境地図生成部１６は、前景マスク２２を使用して環境地図生成処理（後述）を行う（Ｓ２０４）。このとき背景画像および前景マスク２２は最新のものに更新されており、これらを用いて初期マップを生成することになる。

（３）状態情報が運動状態から停止状態に変化した場合、環境地図生成部１６は、前景マスク２２をクリアし（Ｓ２０５）、初期背景画像を設定する（Ｓ２０６）。ここでは、例えば、環境地図生成部１６が、前景マスク生成部１５に指示することにより初期背景画像を設定する。なお、このときの初期背景画像は、停止状態になった時点で撮影された輝度画像とする。

（４）運動状態から運動状態となっている場合、すなわち、状態情報が運動状態のままである場合、環境地図生成部１６は、前景マスク２２を使用せずに環境地図生成処理（後述）を行う（Ｓ２０７）。

続いて、上記Ｓ２０４およびＳ２０７で実施される環境地図生成処理について説明する。図７は、環境地図生成処理を説明するフローチャートである。

環境地図生成部１６による環境地図生成処理では、画像に含まれるすべての特徴点について、環境地図への特徴点追加処理（Ｓ３０１）として、以下の処理を繰り返す。

まず、環境地図生成部１６は、特徴点が背景のものなのか否かを判定する（Ｓ３０２）。前述のＳ２０４の場合（状態情報が停止状態から運動状態に変化した場合）、特徴点が背景の点であると判定する基準として、特徴点の座標が前景マスク２２のＯＮ領域でないこと、かつ、この特徴点が画像上で移動していないことを満たせば、該当の特徴点は、背景にある特徴点であるとする。なお、特徴点が画像上で移動していないことは、画像特徴点追跡部１４による特徴点の追跡結果により判断できる。また、前述のＳ２０７の場合（状態情報が運動状態のままである場合）は、前景マスク２２を使用しないので、特徴点が画像上で移動していないことを満たせば、該当の特徴点は、背景にある特徴点であるとする。

続いて、環境地図生成部１６は、該当の特徴点が背景にある特徴点であると判定される場合（Ｓ３０３でＹｅｓ）、この特徴点の深度を算出する（Ｓ３０４）。ここでは、例えば、移動検出部１１の出力から得られる移動体の移動量から求められるカメラの位置と、画像上の特徴点の動きから、三角測量の原理により特徴点の深度を求める。一方、該当の特徴点が背景にある特徴点でないと判定されると（Ｓ３０３でＮｏ）、下段の処理を省略し、繰り返し処理の最初に戻る。

次に、環境地図生成部１６は、深度計算の精度が良いか否か判定し（Ｓ３０５）、良ければ（Ｓ３０５でＹｅｓ）、該当の特徴点を環境地図２３に追加する（Ｓ３０６）。ここで深度計算の精度が良いか否かの判断は、再投影誤差が十分小さいか等に基づいて行う。一方、深度計算の精度が良くない場合（Ｓ３０５でＮｏ）、環境地図生成部１６は、下段の処理を省略し、処理を繰り返し処理の最初に戻す。

以上の繰り返し処理を終えると、環境地図生成部１６は、環境地図２３に十分な数の特徴点が環境地図２３にあるか判定する（Ｓ３０７）。環境地図２３に十分な数の特徴点が環境地図２３にあれば（Ｓ３０７でＹｅｓ）、環境地図生成部１６は、上記一連の処理を終了とする。この処理を終了する点数の条件は、移動体位置計測部１７で利用するアルゴリズムによって利用する最低限の点数とする。例えばＰ３Ｐアルゴリズムを用いる場合は３点、ＥＰＮＰアルゴリズムを用いる場合は６点などとする。

一方、環境地図２３に十分な数の特徴点がない場合（Ｓ３０７でＮｏ）、環境地図生成部１６は、環境地図生成続行可能であるか判定し（Ｓ３０８）、環境地図生成処理が続行可能であれば（Ｓ３０８でＹｅｓ）、Ｓ３０１に処理を戻し一連の処理を繰り返す。環境地図生成処理が続行不可能と判断されるのは、背景画像の特徴点の数が環境地図生成を成功と判断する終了基準となる数より少なくなった場合であり、その場合は（Ｓ３０８でＮｏ）、環境地図生成部１６は、環境地図生成処理を続行不可能として上記一連の処理を終了する。

以上、本実施形態における環境地図生成部１６の動作について説明した。

本実施形態の移動体位置姿勢計測装置１は、自己の移動体が停止状態から運動状態に変わる際に、環境地図生成部１６が、前景マスク２２でマスクされない特徴点、つまり移動物体など背景の前方にある物体を除く静止物体のみからなる背景上の特徴点を使って環境地図２３（初期マップ）を生成する。この特徴から、本実施形態の移動体位置計測装置は、周囲に移動物体がある環境でも精度の高い環境地図２３を生成することができ、その結果、移動体の位置姿勢計測の精度を高めることが可能となる。

（第２の実施形態）
図８は、第２の実施形態として、ステレオカメラを用いた移動体位置姿勢計測装置１ｂの全体構成を示すブロック図である。

本実施形態の移動体位置姿勢計測装置１ｂは、前述の第１の実施形態と異なり、移動体の外界を撮影する撮像部を複数（図８の例では２つ）備え、それらがステレオカメラとして配置される。以下、図８に示す撮像部１２ａおよび１２ｂの組をステレオカメラと称す。また、本実施形態の移動体位置姿勢計測装置１ｂは、ステレオカメラの撮像画像から距離画像（＝奥行き方向の距離についての情報を各ピクセルが持つ画像）を生成する距離画像生成部１８をさらに備える。前景マスク生成部１５ｂは、距離画像生成部１８が生成した距離画像から前景マスク２２を生成して、記憶部２０に格納する。

なお、距離画像生成部１８は、距離画像の生成を、例えば、三次元の距離情報を有する距離画像を求めることができるステレオカメラ装置について記載している特許第５２７８８１４号公報に開示の手法などを用いて行う。また、距離画像の生成は、この例の他に、ＴｏＦ（Ｔｉｍｅ−Ｏｆ−Ｆｌｉｇｈｔ）センサやレーザレンジファインダなどを利用して行うようにしてもよい。また、距離画像として視差画像（＝右画像を基準に左画像で同じものが画像上で何ピクセルずれているかの情報（視差情報）を各ピクセルが持つ画像）を用いてもよい。視差情報は、その値が大きいものほど、視差が小さく距離が近いことを意味する。

続いて、本実施形態の前景マスク生成部１５ｂによる処理について説明する。図９Ａは、本実施形態における前景マスク生成部１５ｂによる前景マスク更新処理について、図９Ｂは、同前景マスク生成部１５ｂによる背景距離画像更新処理について説明するフローチャートである。なお、ここでは、前景マスク生成部１５ｂが、距離画像生成部１８が生成した距離画像から背景距離画像を生成しているものとして説明する。

前景マスク生成部１５ｂは、ステレオカメラが撮影を行う度に、図９Ａに示す前景マスク更新処理として、以下の処理を繰り返す。

ここでは、この処理の時点でステレオカメラにより撮影された一対のフレーム（画像データ）をフレームＮとし、フレームＮと背景距離画像とを複数の小領域に分け、その領域に１対１で対応する前景マスク２２の領域を定義する。前景マスク生成部１５ｂは、その前景マスク２２の全領域について以下の処理（小領域毎の更新処理）を繰り返し行う（Ｓ４０１）。

まず、前景マスク生成部１５ｂは、前景マスク２２の各領域に対応するフレームＮと背景画像の各領域について、フレームＮの距離画像と背景距離画像との距離の差分を計算する（Ｓ４０２）。なお、各領域（小領域）の大きさは、例えば１ピクセルとする。その他の実施例として、各領域を、近接する複数のピクセルで形成される矩形の領域として定義してもよい。領域を複数のピクセルで構成する場合、各々の領域の距離はその領域に含まれるピクセルの持つ距離の重み付き平均で表現するものとする。重みの付け方は、事前の試験結果等に基づき、適宜定めるものとする。

上記で算出した距離の差分によって、ある領域が背景より近いと判断される場合（Ｓ４０３でＹｅｓ）、前景マスク生成部１５ｂは、その該当領域について背景の前方に物体が現れたとして、対応する前景マスク２２の小領域をＯＮに設定する（Ｓ４０４）。

そうでない場合、すなわち上記のある領域が背景より遠いと判断される場合は（Ｓ４０３でＮｏ）、前景マスク生成部１５ｂは、該当領域の前景マスク２２をＯＦＦにする（Ｓ４０５）。背景より近いかの判断は、上記距離の差分（ここでは絶対値）が一定の閾値を超えるか否かで判断する。このように、本実施形態では、距離画像を用いることにより、自己の移動体からの距離を基に後方にあるものを背景と判断でき、環境地図２３から背景以外の特徴点を除外しやすくなり、その結果、生成する環境地図２３の精度が上がる。

また、前景マスク生成部１５ｂは、ステレオカメラが撮影を行う度に、図９Ｂに示す背景距離画像更新処理として、以下の処理を繰り返す。ここでは、小領域をピクセルとする。

ここでは、背景距離画像の全ピクセルについて、前景マスク生成部１５ｂが以下の処理（ピクセル毎の更新処理）を繰り返し行う（Ｓ４１１）。

はじめに、前景マスク生成部１５ｂは、フレームＮの距離画像と背景距離画像との距離の差分を計算する（Ｓ４１２）。

そして、該当領域が背景画像より遠いと判断される場合に（Ｓ４１３でＹｅｓ）、前景マスク生成部１５ｂは、例えば背景距離画像の該当ピクセルの距離を、フレームＮの距離画像における該当ピクセルの距離に代えて、すなわち置き換えることで背景距離画像を更新する（Ｓ４１４）。

一方、該当領域が背景画像より近いと判断される場合には（Ｓ４１３でＮｏ）、前景マスク生成部１５ｂは、下段の処理を省略し、背景距離画像の該当ピクセルはそのままとする。

以上の背景距離画像更新処理によって、背景距離画像がこれまでで観測した中で最も遠方にあるものとして更新され、前景マスク更新中に移動状態から静止状態に変化した移動物体も、更新された背景距離画像と前景マスク２２によって排除できるようになるので、その結果、生成する環境地図２３の精度が上がる。

図１０に、背景距離画像の一例を示す。また、図１１に、ある時点で撮影されたフレームに対する距離画像の一例を示す。また、図１２に、図１０の例に対応するフレームに対し、上記のようにして生成した前景マスク２２でマスクした状態を示す。同図において横線で描画された領域がマスク領域である。なお、図１１では、カメラの振動、露光の変化、照明の変化など、量子化誤差などによって生じるノイズを除去するため、背景画像を膨張・縮小処理を繰り返す処理を行っている。また、ここでの背景画像は、ステレオカメラで同時に撮影された一対のフレームの一方（例えば右画像）である。

次に、本実施形態における環境地図生成処理について説明する。なお、本実施形態における環境地図生成部１６の基本的な動作は、前述した第１の実施形態において図７を用いて説明したものと同様である。ただし、第２の実施形態では、第１の実施形態における初期背景画像を初期背景距離画像に代える。ここで、初期背景距離画像は、停止状態になった時点で撮影された画像から得られる距離画像を用いるものとする。以下では、図７を参照し、本実施形態における環境地図生成処理について説明する。

本実施形態における環境地図生成処理では、環境地図生成部１６が、画像に含まれるすべての特徴点について、以下の処理を繰り返す（Ｓ３０１）。

まず、環境地図生成部１６は、背景特徴点判定処理によって、特徴点が背景のものなのか否かを判定する。第１の実施形態と同様に、特徴点が背景の点であると判定する基準として、状態情報が停止状態から運動状態に変化した場合については、特徴点の座標が前景マスク２２のＯＮ領域でないこと、かつ、この特徴点が画像上で移動していないことを満たせば、該当の特徴点は背景にある特徴点であるとする。また、状態情報が運動状態のままである場合については、前景マスク２２を使用しないので、特徴点が画像上で移動していないことを満たせば、該当の特徴点は、背景にある特徴点であるとする。

該当の特徴点が背景にある特徴点であると判定される場合（Ｓ３０３でＹｅｓ）、環境地図生成部１６は、この特徴点の深度を算出する（Ｓ３０４）。ここでの深度の計測は、背景距離画像を基に計測する。

次に、環境地図生成部１６は、深度計算の精度が良いか否か判定し（Ｓ３０５）、良ければ（Ｓ３０５でＹｅｓ）、該当の特徴点を環境地図２３に追加する（Ｓ３０６）。ここでの深度計算の精度が良いか否かの判断は、視差が十分にあるか等に基づいて行う。一方、深度計算の精度が良くない場合（Ｓ３０５でＮｏ）、環境地図生成部１６は、下段の処理を省略し、処理を繰り返し処理の最初に戻す。

以上の繰り返し処理を終えると、環境地図生成部１６は、環境地図２３に十分な数の特徴点が環境地図２３にあるか判定する（Ｓ３０７）。環境地図２３に十分な点数の特徴点が環境地図２３にあれば（Ｓ３０７でＹｅｓ）、環境地図生成部１６は、上記一連の処理を終了とする。処理を終了する点数の条件は、移動体位置計測部１７で利用するアルゴリズムによって利用する最低限の点数とする。例えばＰ３Ｐアルゴリズムを用いる場合３点、ＥＰＮＰアルゴリズムを用いる場合は６点などとする。

一方、環境地図２３に十分な数の特徴点がない場合（Ｓ３０７でＮｏ）、環境地図生成部１６は、環境地図生成続行可能であるか判定し（Ｓ３０８）、環境地図生成処理が続行可能であれば（Ｓ３０８でＹｅｓ）、Ｓ３０１に処理を戻し一連の処理を繰り返す。環境地図生成処理が続行不可能と判断されるのは、背景画像の特徴点の数が環境地図生成を成功と判断する終了基準となる数より少なくなった場合であり、その場合は（Ｓ３０８でＮｏ）、環境地図生成部１６は、環境地図生成処理を続行不可能として上記一連の処理を終了する。

以上により、本実施形態の移動体位置計測装置は、第１の実施形態と同様に、周囲に移動物体がある環境でも精度の高い環境地図２３を生成することができ、その結果、移動体の位置姿勢計測の精度を高めることが可能となる。

以上、移動体位置計測装置の諸実施形態について説明した。上述した移動体位置姿勢計測装置１，１ｂによれば、周囲に移動物体がある環境でも精度の高い環境地図２３を生成できるようになる。さらに、デジタルサイネージなどで表示された画像なども背景と分離されるため、環境地図２３の精度が上がる。その結果、上述の移動体位置姿勢計測装置１，１ｂによれば、精度の良い位置・姿勢を得ることができる。

なお、前述の諸実施形態の移動体位置姿勢計測装置１は、ＣＰＵと、処理プログラムを記憶する補助記憶装置および主記憶装置（記憶部２０）等を備える情報処理装置と同様のハードウェア（いずれも図示せず）を含んで構成される。処理プログラムは、上述した各部（画像特徴点抽出部１３、画像特徴点追跡部１４、前景マスク生成部１５，１５ｂ、環境地図生成部１６、および移動体位置計測部１７）の機能を実現するためのモジュールを含むモジュール構成となっており、ＣＰＵが上記補助記憶装置から上記処理プログラムを主記憶装置上にロードすることにより、画像特徴点抽出部１３、画像特徴点追跡部１４、前景マスク生成部１５，１５ｂ、環境地図生成部１６、および移動体位置計測部１７が主記憶装置上に生成されるようになっている。

また、上述した諸実施形態における画像特徴点抽出部１３、画像特徴点追跡部１４、前景マスク生成部１５，１５ｂ、環境地図生成部１６、および移動体位置計測部１７の機能を実現する処理プログラムは、ＲＯＭ、ＨＤＤ等の補助記憶装置に予め組み込まれて提供される。また、上記処理プログラムは、インストール可能な形式又は実行可能な形式のファイルでＣＤ−ＲＯＭ、フレキシブルディスク（ＦＤ）、ＣＤ−Ｒ、ＤＶＤ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｋ）等のコンピュータで読み取り可能な記録媒体に記録して提供するように構成してもよい。さらに、上記処理プログラムを、インターネット等のネットワークに接続されたコンピュータ上に格納し、インターネット等のネットワーク経由で提供または配布するように構成しても良い。

１ 移動体位置姿勢計測装置
１１ 移動検出部
１２、１２ａ、１２ｂ 撮像部
１３ 画像特徴点抽出部
１４ 画像特徴点追跡部
１５、１５ｂ 前景マスク生成部
１６ 環境地図生成部
１７ 移動体位置計測部
１８ 距離画像生成部
２０ 記憶部
２１ 画像特徴点情報
２２ 前景マスク
２３ 環境地図
２４ 移動体位置姿勢情報

特表２００８−５００５６１号公報

Claims (4)

1. 移動体の移動を検出する移動検出部と、
   前記移動体の外界を撮影する撮像部と、
   記憶部と、
   前記撮像部で撮影した画像から画像特徴点を抽出し該画像特徴点の情報を前記記憶部に保存する画像特徴点抽出部と、
   前記撮像部で撮影される時系列の各画像において、前記記憶部に保存されている前記画像特徴点の情報が示す画像特徴点を追跡し、前記記憶部の画像特徴点の情報が最新の画像特徴点の情報となるよう更新する画像特徴点追跡部と、
   前記撮像部で撮影される時系列の各画像中の背景画像および該画像中の移動物体をマスクするための前景マスクを生成し更新する前景マスク生成部と、
   前記移動検出部の検出結果から得られる前記移動体の移動に関する情報から前記移動体が停止状態から運動状態になったと判断される場合、前記記憶部の画像特徴点の情報が示す画像特徴点のうち前記前景マスクでマスクされない画像特徴点を用いて環境地図を生成する環境地図生成部と、
   前記環境地図生成部が生成した環境地図を基に前記移動体の位置姿勢計測を行う移動体位置計測部と、
   を備えたことを特徴とする移動体位置姿勢計測装置。
2. 前記環境地図生成部は、前記環境地図を生成するための処理として、さらに、
   前記移動体の移動に関する情報から前記移動体が運動状態から停止状態に変化したと判断される場合に、前記前景マスクをクリアし、初期背景画像を設定し、
   前記移動体の移動に関する情報から前記移動体が停止したままであると判断される場合に、前記前景マスク生成部により生成される背景画像および前記前景マスクを最新のものとする更新を繰り返し、
   前記移動体の移動に関する情報から前記移動体が運動状態のままであると判断される場合に、前記前景マスクを使用せずに前記記憶部の画像特徴点の情報が示す画像特徴点を用いて環境地図を生成する
   ことを特徴とする請求項１に記載の移動体位置姿勢計測装置。
3. 前記撮像部としてのステレオカメラと、
   前記ステレオカメラが出力する一対の画像データから対応する距離画像を生成する距離画像生成部と、を備え、
   前記前景マスク生成部は、前記距離画像生成部が生成する距離画像から、背景距離画像と前記前景マスクとを生成し更新する
   ことを特徴とする請求項１または請求項２に記載の移動体位置姿勢計測装置。
4. 前記前景マスク生成部は、前記背景距離画像と前記距離画像生成部が生成した距離画像とを複数の小領域に分割した各領域について、対応する領域を比較し、前記距離画像のある領域の方が前記背景距離画像の該当領域より遠いと判断される場合には、前記背景距離画像の該当領域を前記距離画像の該当領域のものに代えて更新することを特徴とする請求項３に記載の移動体位置姿勢計測装置。