JP6557973B2 - 地図生成装置、地図生成方法およびプログラム - Google Patents

Description

本発明は、地図生成装置、地図生成方法およびプログラムに関する。

従来、自動車の走行時に現在位置や目的地への経路案内を行なうカーナビゲーションシステム（Ａｕｔｏｍｏｔｉｖｅ Ｎａｖｉｇａｔｉｏｎ Ｓｙｓｔｅｍ）は、自車位置の特定をＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ：全地球測位システム）衛星からの位置情報により基本的に行っている。ＧＰＳの電波は直進性が強く、トンネルの中、高層ビルの間、高架道路の下、樹木の間などでは電波が受信しづらく、ＧＰＳだけでは誤差があることから、カーナビゲーションシステム内の加速度センサとジャイロ、タイヤの回転に伴う車速信号などの情報による自立航法と併用することで自車位置を特定することが行われている。

また、自立航法では、路面やタイヤの状況などにより誤差が生じてしまうため、マップマッチングと呼ばれるソフトウェア技術で、地図上の道路データに自車位置を当てはめることでＧＰＳやジャイロセンサなどの誤差を補うという方法も用いられている。

また、レーザーレンジセンサ（測域センサ）、カメラ、エンコーダなどを利用して自己位置を数〜数十ｃｍ精度で把握できる技術として、ＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ Ａｎｄ Ｍａｐｐｉｎｇ：自己位置推定と環境地図作成を同時に行う技術）が知られている。特に、カメラを利用したＳＬＡＭは、Ｖｉｓｕａｌ ＳＬＡＭと呼ばれている。Ｖｉｓｕａｌ ＳＬＡＭ技術では、上記のセンサにより局所的な地図を作成し、地図情報から自車位置を特定するため、ＧＰＳの電波が受信しづらい状況下においても精度良く自車位置を特定することができる。また、ジャイロセンサや加速度センサと異なり、外界の情報を利用しているため、路面やタイヤの状況によらず自車位置を特定することができる。

特許文献１には、Ｖｉｓｕａｌ ＳＬＡＭ技術を使って被写体を撮影したカメラの被写体に対する位置及び姿勢を示す回転角（カメラポーズ）の推定精度を向上させることができるカメラポーズ推定装置が開示されている。特許文献１に記載のカメラポーズ推定装置は、テンプレート画像に対応する領域の三次元位置を推定する際に、当該領域に含まれる特徴点から外れ値を除去してからテンプレート画像の三次元位置を推定するので、外れ値の影響を抑えて三次元位置の精度を向上させることができるものである。

Ｖｉｓｕａｌ ＳＬＡＭ技術を使って自車位置を特定しながら走行するには、初期の自己位置を推定するための地図を初期化しておく必要がある。この地図には、建物や道路、標識などの静止物体の３次元点情報のみが必要であり、人や車などの移動物体の３次元点情報は含まないようにする必要がある。

しかしながら、特許文献１に記載のカメラポーズ推定装置では、移動物体の３次元点情報を含まない地図を事前に作成する際に、人や車などの移動物体の有無にかかわらず、任意のタイミングで地図を初期化することができない、という問題があった。

本発明は、上記に鑑みてなされたものであって、自己位置の推定に適用可能な初期の地図を生成することができる地図生成装置、地図生成方法およびプログラムを提供することを目的とする。

上述した課題を解決し、目的を達成するために、本発明に係る地図生成装置は、移動中の画像を時系列に入力する画像入力部と、前記画像入力部から入力された時系列画像から物体を示す画像領域を抽出して、世界座標系における前記物体の座標を算出する第１の算出部と、順に入力された前記時系列画像で、抽出した前記画像領域の前記物体を追跡して前記物体のオプティカルフローを算出する第２の算出部と、前記第２の算出部で算出された前記オプティカルフローに基づいて、前記画像入力部の移動により生成した第１の消失点の座標と、前記物体の移動により生成した第２の消失点の座標との差を算出し、算出した前記差に基づいて前記物体が移動物体であると判定した場合、前記物体の画像領域を前記時系列画像から除去する移動物体除去部と、前記移動物体除去部で除去されなかった前記物体の前記世界座標系における前記座標を含む地図情報を記憶部に記憶させる記憶制御部と、前記第１の算出部で新たに抽出した物体と、前記第２の算出部で追跡している前記物体とが同じ物体であると判定した場合に、前記新たに抽出した物体を除去して前記追跡を中止する重複点除去部と、前記画像入力部を基準とした座標系を示すカメラ座標系における前記物体の座標と、前記地図情報の前記世界座標系における前記物体の座標を前記カメラ座標系に再投影した座標との誤差を算出し、算出した前記誤差が予め設定された閾値以上である場合、前記記憶部に記憶されている前記物体の前記地図情報を削除して前記地図情報を更新する更新部と、を備えることを特徴とする。

本発明によれば、初期の地図を生成することができる、という有利な効果を奏する。

図１は、本実施形態に係る自己位置推定装置のハードウェア構成の一例を示す図である。 図２は、本実施形態に係る自己位置推定装置の機能構成の一例を示すブロック図である。 図３は、視差画像から３次元点を算出する方法の一例について説明する図である。 図４は、視差画像から３次元点を算出する方法の一例について説明する図である。 図５は、オプティカルフローの一例について説明する図である。 図６は、動き消失点の一例について説明する図である。 図７は、動き消失点の一例について説明する図である。 図８は、世界座標系における物体の座標をカメラ座標系に再投影した座標について説明する図である。 図９は、時系列画像の画像領域の分割とオプティカルフローの統合について説明する図である。 図１０は、動き消失点の座標について説明する図である。 図１１は、動き消失点の座標について説明する図である。 図１２は、ステレオ距離画像と障害物マスク画像の一例について説明する図である。 図１３は、本実施形態の自己位置推定装置の処理動作の一例について説明するフローチャート図である。 図１４は、地図情報の更新処理の一例について説明するフローチャート図である。

以下、添付図面を参照しながら、本発明に係る地図生成装置、地図生成方法およびプログラムの一実施形態を詳細に説明する。なお、以下の実施形態により本発明が限定されるものではない。また、各実施形態は、内容を矛盾させない範囲で、適宜組み合わせることができる。

本実施形態の地図生成装置は、画像入力部から入力された時系列画像から物体を示す画像領域を抽出して、世界座標系における物体の座標を算出する。次に、地図生成装置は、順に入力された前記時系列画像で、抽出した前記画像領域の前記物体を追跡して前記物体のオプティカルフローを算出する。次に、算出されたオプティカルフロー（物体の移動ベクトル）に基づいて、画像入力部の移動により生成した第１の消失点の座標と、物体の移動により生成した第２の消失点の座標との差を算出する。次に、算出した差に基づいて物体が移動物体であると判定した場合、物体の画像領域を時系列画像から除去する。また、地図生成装置は、新たに抽出した物体と、追跡している物体とが同じ物体であると判定した場合に、新たに抽出した物体を除去して追跡を中止する。地図生成装置は、除去されなかった物体の世界座標系における座標を含む地図情報を更新して記憶する。これにより、Ｖｉｓｕａｌ ＳＬＡＭに適用可能な初期の地図を生成することができる。

以下では、本実施形態の地図生成装置を、Ｖｉｓｕａｌ ＳＬＡＭを用いる自己位置推定装置の一部の装置として実現する例を説明する。本実施形態の地図生成装置が適用可能な装置は、このような自己位置推定装置に限られるものではない。

最初に、図１を用いて、自己位置推定装置１００のハードウェア構成例について説明する。図１は、本実施形態の自己位置推定装置のハードウェア構成の一例を示す図である。

図１に示すように、自己位置推定装置１００は、システムバス１２０に接続された、入力装置１０１と、表示装置１０２と、ドライブ装置１０３と、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）１０４と、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）１０５と、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）１０６と、インタフェース装置１０７と、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）１０８とを備える。ドライブ装置１０３には記録媒体１０９が接続される。

入力装置１０１は、自己位置推定装置１００に各操作信号、各種データを入力する入力インタフェースである。入力装置１０１は、例えば、移動中の画像を時系列に入力するカメラである。本実施形態において、カメラは、例えば、視差による距離画像が取得可能な複数台のカメラ（ステレオカメラなど）である。なお、カメラは、複数台のカメラに限ることはなく、時系列に撮影された複数フレームの画像から距離画像を取得することができる単眼カメラでもよい。

また、入力装置１０１は、キーボード、マウス、ディスプレイ等で構成され、ユーザの様々な操作指示を受け付けるユーザインタフェースである。入力装置１０１は、例えば、タッチパネルや音声入力装置を入力装置１０１とすることもできる。

表示装置１０２は、プログラムによるＧＵＩ（Ｇｒａｐｈｉｃａｌ Ｕｓｅｒ Ｉｎｔｅｒｆａｃｅ）等を表示する。表示装置１０２は、例えば、画像などを表示するディスプレイなどを含み、自己位置推定装置１００による処理結果などを表示する。ディスプレイは、例えば、液晶や有機ＥＬなどのＦＰＤ（Ｆｌａｔ Ｐａｎｅｌ Ｄｉｓｐｌａｙ）である。

ドライブ装置１０３は、着脱可能な記録媒体１０９とのインタフェースである。自己位置推定装置１００は、ドライブ装置１０３を介して、記録媒体１０９の読み取り及び／又は書き込みを行うことができる。記録媒体１０９には、例えば、フロッピー（登録商標）ディスク、ＣＤ（Ｃｏｍｐａｃｔ Ｄｉｓｋ）、ＤＶＤ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｋ）、ＳＤメモリカード（ＳＤ ｍｅｍｏｒｙ ｃａｒｄ）、ＵＳＢメモリ（Ｕｎｉｖｅｒｓａｌ Ｓｅｒｉａｌ Ｂｕｓ ｍｅｍｏｒｙ）などがある。

ＲＡＭ１０４は、各種プログラムやデータを一時記憶する揮発性の半導体メモリ（記憶装置）である。また、ＲＡＭ１０４は、ＣＰＵ１０６がデータ処理時に使用するメモリでもある。なお、ＲＡＭ１０４の代わりに、ＳＤＲＡＭ、ＤＲＡＭ、ＳＲＡＭなどを使用してもよい。

ＲＯＭ１０５は、電源を切っても内部データを保持することができる不揮発性の半導体メモリ（記憶装置）である。ＲＯＭ１０５には、自己位置推定装置１００の起動時に実行されるＢＩＯＳ（Ｂａｓｉｃ Ｉｎｐｕｔ／Ｏｕｔｐｕｔ Ｓｙｓｔｅｍ）、情報処理システム設定、及びネットワーク設定などのプログラムやデータが格納されている。

ＣＰＵ１０６は、自己位置推定装置１００の動作を統括的に制御する。ＣＰＵ１０６は、ＲＯＭ１０５やＨＤＤ１０８に格納されたプログラムやデータをＲＡＭ（メモリ）１０４上に読み出し、処理を実行することで、自己位置推定装置１００全体の動作を制御する。また、ＣＰＵ１０６は、各種機能を実現する。

インタフェース装置１０７は、例えば、ネットワークに接続するためのインタフェースである。自己位置推定装置１００は、インタフェース装置１０７を介して、ネットワークに接続された外部装置とデータ通信を行うことができる。

ＨＤＤ１０８は、自己位置推定装置１００で実行される各種のプログラムやデータを格納する不揮発性の記憶装置である。また、ＨＤＤ１０８は、自己位置推定装置１００の各種機能を実現するために必要なプログラムやデータ等を格納する。格納されるプログラムやデータには、自己位置推定装置１００全体を制御する情報処理システム（例えば「Ｗｉｎｄｏｗｓ（登録商標）」や「ＵＮＩＸ（登録商標）」などの基本ソフトウェアであるＯＳ（Ｏｐｅｒａｔｉｎｇ Ｓｙｓｔｅｍ））、及びシステム上において各種機能を提供するアプリケーションなどがある。また、ＨＤＤ１０８は、格納しているプログラムやデータを、所定のファイルシステム及び／又はＤＢ（Ｄａｔａ Ｂａｓｅ）により管理している。

なお、上記の自己位置推定装置１００用プログラムは、インストール可能な形式又は実行可能な形式のファイルで、記録メディア等のコンピュータで読み取り可能な記録媒体に記録して流通させるようにしてもよい。

なお、後述する地図生成装置１０のハードウェア構成は、上記の自己位置推定装置１００と同様であるため、以下では詳細な説明は省略する。

次に、図２を用いて、自己位置推定装置１００の機能構成について説明する。図２は、自己位置推定装置の機能構成の一例を示すブロック図である。

図２に示すように、自己位置推定装置１００は、画像入力部１１と、第１の算出部１２と、第２の算出部１３と、移動物体除去部１４と、重複点除去部１５と、記憶制御部１６と、更新部１７と、推定部１８とを備える。また、移動物体除去部１４及び記憶制御部１６と連携する各機能部として、統合部２１と、消失点算出部２２と、移動情報算出部２３と、動き消失点取得部２４と、記憶部２５と、距離画像算出部２６と、路面特定部２７とを備えている。上記各部については、これらの一部がソフトウェア（プログラム）であってもよいし、これらの一部又は全部がハードウェア回路であってもよい。

画像入力部１１は、移動中の画像を時系列に入力する。画像入力部１１は、例えば、移動中の画像を時系列に入力するデジタルカメラやビデオカメラ等の画像センサを有する撮像装置（カメラ）である。本実施形態において、カメラは、例えば、視差による距離画像が取得可能な複数台のカメラ（ステレオカメラなど）である。なお、カメラは、複数台のカメラに限ることはなく、時系列に撮影された複数フレームの画像から距離画像を取得することができる単眼カメラでもよい。また、画像入力部１１は、映像データを構成する一連の画像データを取得できればよく、予め撮影された映像データを取得する装置などであってもよい。

第１の算出部１２は、画像入力部（カメラ）１１から入力された時系列画像から物体を示す画像領域を抽出して、世界座標系における物体の３次元位置座標を算出する。以下では、画像入力部を「カメラ」とも表記する。第１の算出部１２は、具体的には、連続して入力される時系列画像から物体（静止物体及び移動物体）を抽出し、抽出した物体の特徴点を抽出する。そして、複数の画像入力部（カメラ）１１から入力された時系列画像の視差画像から特徴点の世界座標系における３次元位置座標（３次元点：Ｘ，Ｙ，Ｚ）を算出する。なお、特徴点は、例えば、建物の角などの画像上の特徴（コーナー）のことを指す。

また、画像から確実といえる特徴点を抽出し画像の中身を推測するために、特徴点を抽出する手法としては、例えば、特徴点をコーナーとしたＭｒａｖｅｃのコーナー検出、Ｈａｒｒｉｓのコーナー検出、ＦＡＳＴ（Ｆｅａｔｕｒｅｓ ｆｒｏｍ Ａｃｃｅｌｅｒａｔｅｄ Ｓｅｇｍｅｎｔ Ｔｅｓｔ）などが例示される。

また、スケールと特徴点を同時に検出するＤｉｆｆｅｒｅｎｃｅ−ｏｆ−Ｇａｕｓｓｉａｎ（ＤｏＧ）、近似Ｈｅｓｓｉａｎ−Ｌａｐｌａｃｅなどが例示される。また、局所特徴量は局所領域の勾配情報を実数で表現するＳｃａｌｅ−Ｉｎｖａｒｉａｎｔ Ｆｅａｔｕｒｅ Ｔｒａｎｓｆｏｒｍ （ＳＩＦＴ）、Ｓｐｅｅｄｅｄ−Ｕｐ Ｒｏｂｕｓｔ Ｆｅａｔｕｒｅｓ（ＳＵＲＦ）などが例示される。また、輝度の大小関係をバイナリで表現するＢｉｎａｒｙ Ｒｏｂｕｓｔ Ｉｎｄｅｐｅｎｄｅｎｔ Ｅｌｅｍｅｎｔａｒｙ Ｆｅａｔｕｒｅｓ（ＢＲＩＥＦ）、Ｏｒｉｅｎｔｅｄ ＦＡＳＴ ａｎｄ Ｒｏｔａｔｅｄ ＢＲＩＥＦ（ＯＲＢ）などが例示される。

ここで、時系列画像の視差画像から特徴点の世界座標系における３次元位置座標（３次元点）の算出方法について説明する。図３及び図４は、視差画像から３次元点を算出する方法の一例について説明する図である。図３に示すように、２台のカメラ（ステレオカメラ）があるとする。Ｘ座標における左カメラのレンズ中心はｘｌ、右カメラのレンズ中心はｘｒで、カメラ間の距離はＤである。また、図４に示すように、左カメラと右カメラとの視差はＥである。この場合、３次元点（Ｘ、Ｙ、Ｚ）は下記式（１）〜（３）により算出される。

Ｚ＝Ｄｆ／（ｘｌ−ｘｒ） ・・・（１）
Ｘ＝（Ｚ／ｆ）×ｘｒ ・・・（２）
Ｙ＝（Ｚ／ｆ）×ｙｒ ・・・（３）
なお、上記式（１）〜（３）中、Ｄはカメラ間の距離、ｆはカメラの焦点距離、ｘｌ−ｘｒは視差Ｅを表す。

図２に戻って説明を続ける。第２の算出部１３は、画像入力部（カメラ）１１から順に入力された時系列画像で、第１の算出部１２で抽出した物体を追跡して物体のオプティカルフローを算出する。具体的には、第２の算出部１３は、第１の算出部１２で抽出した物体の特徴点について、例えば、最初に入力された時系列画像（フレーム）を探索画像として順に入力された時系列画像で物体の特徴点の位置を追跡し、物体の特徴点が移動した移動ベクトル（オプティカルフロー）を算出する。なお、オプティカルフローとは、デジタル画像中の物体の動きをベクトルで表したものである。主に移動物体の検出や、その動作の解析などに用いられる。

なお、各特徴点は、例えば、特徴点を識別する識別情報（ＩＤ）、特徴点を追跡した追跡回数、画像上の座標（ｘ，ｙ）などの情報が付与されている。すなわち、複数の特徴点は、例えば、識別情報（ＩＤ）により識別されている。なお、追跡回数の一例として、例えば「０」を追跡失敗、「１」を新規な特徴点、「２」以上を追跡成功回数などと定義してもよい。

ここで、図５を用いて、オプティカルフローの移動ベクトルについて説明する。図５は、オプティカルフローの一例について説明する図である。図５に示すように、例えば、時間軸ｔである前の画像の特徴点の画素Ａ（特徴点）が、時間軸ｔ＋Δｔである次の画像において別の場所、画素Ｂに動いたとき、その移動を表すベクトル（ＡからＢに向かうベクトル）が、この特徴点の移動ベクトルである。このように、オプティカルフローとは、動画像の時間的、そして空間的な例えば、輝度分布の変化を用いて画素ごとに移動ベクトルを求めてできた結果である。なお、時間軸のｔは任意の時刻を表す。

図２に戻って説明を続ける。移動物体除去部１４は、第２の算出部１３で算出されたオプティカルフロー（物体の移動ベクトル）に基づいて、画像入力部１１の移動により生成した第１の消失点（動き消失点）の座標と、物体の移動により生成した第２の消失点（移動物体消失点）の座標との差を算出し、算出した差に基づいて物体が移動物体であると判定した場合に、物体の画像領域を時系列画像から除去する。

具体的には、移動物体除去部１４は、第２の算出部１３で得られた移動ベクトルの情報から画像入力部（カメラ）１１の移動により生成した第１の消失点（動き消失点）の座標と、移動物体により生成した第２の消失点（移動物体消失点）の座標を求める。そして、それぞれの消失点の座標の差分が予め設定した閾値の範囲内であるか否かを判定し、静止物体であるのか、又は移動物体であるのかを判定する。そして、移動物体除去部１４は、画像入力部（カメラ）１１から入力された時系列画像から抽出した物体が移動物体であると判定した場合に、物体を示す画像領域を時系列画像から除去する。すなわち、例えば、人や車などの移動物体の世界座標系における３次元位置座標（３次元点）情報を含まない初期の地図を生成することができる。

また、移動物体除去部１４は、消失点の座標の距離が予め設定した閾値以下であるグループ同士をさらに統合し、統合されたグループについて、消失点を算出するためのオプティカルフロー（移動ベクトル）の数が少ないものを移動物体と判定して物体の画像領域を時系列画像から除去する。

具体的には、まず、統合部２１で、時系列画像を画像の種類ごとの画像領域に分割し、分割した画像領域に含まれるオプティカルフロー（移動ベクトル）を１つのグループとする。次に、消失点算出部２２で、それぞれのグループごとに、オプティカルフローの物体の移動ベクトルに基づいて消失点の座標を算出する。その後、移動物体除去部１４は、消失点の座標の距離が予め設定した閾値以下であるグループ同士をさらに統合し、統合されたグループについて、消失点を算出するためのオプティカルフローの移動ベクトルの数が少ないものを移動物体と判定して物体の画像領域を時系列画像から除去する。

また、複数カメラの距離画像の情報を用いて、例えば、画像領域の距離が近い場合には危険と判断して無条件で移動物体と判断してもよい。なお、詳細については、後述の図９を用いて、統合部２１、消失点算出部２２にて説明する。

また、移動物体除去部１４は、順に入力された時系列画像における物体の座標（追跡中の特徴点）と取得した第１の消失点（動き消失点）の座標とを結んだ方向のベクトルと、第２の算出部１３で算出されたオプティカルフローの移動ベクトルとの内積を計算した値に基づき、移動物体を判定して物体の画像領域を時系列画像から除去する。

詳細には、後述する図１１を用いて説明するが、移動物体除去部１４は、動き消失点と追跡中の特徴点とを結んだ方向のベクトルと、実際に特徴点の追跡により求められたオプティカルフローの移動ベクトルの差を、ベクトルの内積からｃｏｓθを算出し、算出したｃｏｓθが予め設定した閾値を超える場合（差が大きい場合、ズレが大きい場合）、物体を移動物体と判定して物体の画像領域を時系列画像から除去する。

ここで、図６及び図７を用いて、動き消失点について説明する。図６及び図７は、動き消失点の一例について説明する図である。図６に示すように、例えば、自動車に搭載されたカメラ（画像入力部）１１が進行方向に向かって進んでいる場合、移動中の画像が時系列に入力される。図６の例では、時刻ｔでの画像と、時刻ｔ＋Δｔでの画像が時系列に入力されることになる。つまり、進行方向に向かって連続画像が入力される。

また、図７に示すように、カメラ（画像入力部）１１が移動するので、時刻ｔでの物体の画像は小さく（遠くにある）、時刻ｔ＋Δｔでの物体の画像は大きく（近くにある）なる。図７において、カメラ１１から得られる時刻ｔでの物体の画像のコーナーと、時刻ｔ＋Δｔでの物体の画像のコーナーのオプティカルフローを延長した直線による交点を用いて、動きの消失点を求めることができる。なお、図７に示した消失点は画像上の消失点ではなく、動き消失点のことであり、画像上の消失点とは違うものである。図７に示した、動き消失点の軌跡を用いることによりカメラ１１自体の動きを推定することができる。

図２に戻って説明を続ける。移動物体除去部１４は、後述の図１２を用いて説明する記憶部２５に記憶されている障害物マスク画像に、距離画像算出部２６で算出した距離画像に含まれる障害物画像が重なり、かつ距離画像から障害物画像との距離が予め設定した距離以下の場合に、障害物画像を移動物体と判定して物体の画像領域を時系列画像から除去する。

また、移動物体除去部１４は、路面特定部２７から受け付けた路面の画像領域に基づいて、路面の画像領域と接する画像領域を移動物体と判定して物体の画像領域を時系列画像から除去する。

また、移動物体除去部１４は、ＲＡＮＳＡＣ（ＲＡＮＤｄｏｍ Ｓａｍｐｌｅ Ｃｏｎｓｅｎｓｕｓ）アルゴリズムを用いて、時系列画像に含まれる背景領域の特徴点を２次曲線に近似して、２次曲線から離れた特徴点を移動物体と判定し、特徴点に対応する物体の画像領域を時系列画像から除去する。すなわち、ＲＡＮＳＡＣ（ＲＡＮｄｏｍ ＳＡｍｐｌｅ Ｃｏｎｓｅｎｓｕｓ）アルゴリズムを用いて、背景領域のオプティカルフローはｘ方向に対して滑らかに変化する特性を２次曲線に近似して２次曲線の外れ値を移動物体と判断して物体を除去する。

重複点除去部１５は、第１の算出部１２で新たに抽出した物体と、第２の算出部１３で追跡している物体と同じ物体であるか否かの対応付けを行う。

また、重複点除去部１５は、第１の算出部１２で新たに抽出した物体と、第２の算出部１３で追跡している物体とが同じ物体であると判定した場合に、新たに抽出した物体を除去して物体の追跡を中止する。すなわち、新たに抽出した物体と、現在追跡している物体とが同じである場合には重複している新たに抽出した物体を除去して、オプティカルフローを算出するための追跡を止める。

記憶制御部１６は、移動物体除去部１４、及び重複点除去部１５で除去されなかった物体の世界座標系における座標を含む地図情報を記憶部２５に記憶させる制御を行う。すなわち、地図生成装置１０で生成した初期の地図情報を記憶部２５に記憶させる。また、記憶制御部１６は、更新部１７で地図情報が更新された場合、更新後の地図情報を記憶部２５に記憶させる制御を行う。なお、記憶制御部１６は、地図情報を地図データベースとして記憶させるようにしてもよい。さらに、地図データベースは、記憶部２５が備えることに限ることはなく、例えば、通信可能な外部装置（サーバ装置）に備えるように構成することもできる。

また、記憶制御部１６は、現在の地図情報だけではなく過去に走行した際の地図情報を記憶させておくことで、例えば、雨や雪、夜間など不利な環境下においてもロバスト性を向上させることができる。

更新部１７は、画像入力部１１を基準とした座標系を示すカメラ座標系における物体の座標と、記憶部２５に記憶されている地図情報の世界座標系における物体の座標をカメラ座標系に再投影した座標との誤差を算出し、算出した誤差が予め設定された閾値以上である場合、記憶部２５に記憶されている物体の地図情報（世界座標系における物体の座標）を削除して地図情報を更新する。

すなわち、更新部１７が更新した地図情報は、移動物体を除去した初期の地図情報である。従って、更新部１７は、時系列画像から移動物体を除去した、静止物体（ランドマーク）の世界座標系における物体の座標の情報を含んだ、自己位置推定に適用可能な初期の地図情報を生成することができる。

ここで、図８を用いて、世界座標系における物体の座標と、カメラ座標系に再投影した座標との誤差について説明する。図８は、世界座標系における物体の座標をカメラ座標系に再投影した座標について説明する図である。図８に示した３次元点Ｘｐ（Ｘ，Ｙ，Ｚ）の座標は、第１の算出部１２で算出された、画像入力部（カメラ）１１から入力された時系列画像から物体を示す画像領域を抽出して、算出した世界座標系における物体（特徴点）の３次元位置（３次元点）の座標に対応する。

まず、更新部１７は、画像入力部１１から入力された時系列画像から抽出した物体（特徴点）のカメラ座標系における座標ｘｐ（ｘ，ｙ）を取得する。次に、更新部１７は、記憶部２５に記憶されている地図情報から物体（特徴点）の世界座標系における座標Ｘｐ（Ｘ，Ｙ，Ｚ）を取得する。そして、座標Ｘｐ（Ｘ，Ｙ，Ｚ）をカメラ座標系の座標に変換し、変換した座標ｘ′ｐ（ｘ′，ｙ′）をカメラ座標系に再投影する。

すなわち、物体（特徴点）の世界座標系における３次元の座標Ｘｐ（Ｘ，Ｙ，Ｚ）を、変換行列を用いて、カメラ座標系の２次元の座標ｘ′ｐ（ｘ′，ｙ′）に変換する。なお、３次元座標から２次元座標に変換するための変換行列は任意の式を適用できる。

図８の例では、時系列画像から抽出した物体（特徴点）の座標ｘｐ（ｘ，ｙ）と、記憶部２５に記憶されている地図情報から物体（特徴点）の座標Ｘｐ（Ｘ，Ｙ，Ｚ）を変換して投影した座標ｘ′ｐ（ｘ′，ｙ′）とが同一の座標でなく誤差（投影誤差）がある場合を示している。

更新部１７は、図８に示した、座標ｘｐ（ｘ，ｙ）と、座標ｘ′ｐ（ｘ′，ｙ′）との誤差（投影誤差）を算出する。そして、更新部１７は、算出した誤差（投影誤差）が予め設定された閾値以上である場合、記憶部２５に記憶されている物体の地図情報（世界座標系における物体の座標）を削除（初期化）して地図情報を更新する。なお、誤差（投影誤差）の閾値は任意に設定できる。

なお、時系列画像における物体（特徴点）のカメラ座標系での座標ｘｐ（ｘ，ｙ）を、世界座標系における座標Ｘｐ（Ｘ，Ｙ，Ｚ）に変換する場合は、上記で説明した変換行列の逆変換によって、世界座標系における物体の座標Ｘｐ（Ｘ，Ｙ，Ｚ）を取得することができる。

また、更新部１７は、画像入力部（カメラ）１１から入力された時系列画像から新たに物体を抽出した場合、時系列画像における物体（特徴点）のカメラ座標系での座標ｘｐ（ｘ，ｙ）と、第１の算出部１２で算出された世界座標系における物体（特徴点）の３次元位置（３次元点）の座標を変換してカメラ座標系に投影した座標ｘ′ｐ（ｘ′，ｙ′）との誤差（投影誤差）が予め設定された閾値以上であり、かつ時系列画像の視差画像から得られる視差が予め設定された閾値以上である場合に、新たな物体（特徴点）を地図情報に追加して地図情報を更新する。なお、移動しながら物体（特徴点）の世界座標系における座標を地図情報に追加することで初期の地図を生成することができる。なお、誤差（投影誤差）の閾値、及び視差の閾値は任意に設定できる。

図２に戻って説明を続ける。推定部１８は、世界座標系における物体の座標（特徴点の３次元点）に基づいて、世界座標系からカメラ（画像入力部）１１を基準とした座標系を示すカメラ座標系への変換行列を推定し、該変換行列に基づいて画像を入力したカメラ（画像入力部）１１の物体（特徴点）に対する位置及び姿勢を示す回転角を推定する。

つまり、推定部１８は、物体に対するカメラ（画像入力部）１１の位置及び向きを算出する。具体的には、推定部１８は、世界座標系における物体の位置座標を、カメラ座標系へ投影するための変換を行う変換パラメータ［Ｒ｜ｔ］行列を算出する。なお、Ｒは回転行列のことを表し、ｔは並進ベクトルのことを表す。

すなわち、図８を用いて説明したように、推定部１８は、記憶部２５に記憶されている物体（特徴点）の世界座標系における座標Ｘｐ（Ｘ，Ｙ，Ｚ）と、時系列画像における物体（特徴点）のカメラ座標系での座標ｘｐ（ｘ，ｙ）とを用いて、世界座標系をカメラ座標系へと線形変換するための変換パラメータ［Ｒ｜ｔ］行列を算出する。なお、上記で説明したように、３次元座標から２次元座標に変換するための変換パラメータ［Ｒ｜ｔ］行列は任意の式を適用できる。

次に、移動物体除去部１４及び記憶制御部１６と連携する各機能部について説明する。

統合部２１は、時系列画像を画像の種類ごとの画像領域に分割し、分割した画像領域ごとにオプティカルフローを１つのグループに統合する。すなわち、第１の算出部１２で、時系列画像から複数の物体の特徴点を抽出し、複数の画像入力部１１から入力された視差画像から世界座標系における複数の物体の特徴点の座標を算出している。また、第２の算出部１３で、順に入力された時系列画像で、抽出した複数の物体の特徴点を追跡して複数の物体のオプティカルフローを算出している。そこで、統合部２１は、入力された時系列画像を、例えば、空、背景（左）、背景（右）、人、路面などといった画像の種類ごとに画像領域を分割する。次に統合部２１は、分割した画像領域ごとに、第２の算出部１３で算出したオプティカルフローを１つのグループに統合する。

ここで、時系列画像を画像の種類ごとの画像領域に分割し、分割した画像領域ごとにオプティカルフローを統合する流れについて説明する。図９は、時系列画像の画像領域の分割とオプティカルフローの統合について説明する図である。

図９に示すように、統合部２１は、例えば、空、背景（左）、背景（右）、人、路面といった画像領域に分割し、図９の例では、例えば、空の領域に含まれるオプティカルフロー（移動ベクトル）を１つのグループとし、背景（左）に含まれるオプティカルフロー（移動ベクトル）を１つのグループとし、背景（右）に含まれるオプティカルフロー（移動ベクトル）を１つのグループとする。また、人のオプティカルフロー（移動ベクトル）を１つのグループとし、路面のオプティカルフロー（移動ベクトル）を１つのグループとする。

なお、画像領域の分割の方法は、複数カメラによる距離画像により領域分割する方法や、画像の輝度値の勾配（エッジ）で領域分割する方法がある。領域がきれいに分割できない場合には、ノイズ除去や膨張・収縮などといった処理を必要に応じて行うものとする。この後に、後述する消失点算出部２２で、オプティカルフロー（移動ベクトル）を統合したグループごとに、消失点を算出する。

図２に戻って説明を続ける。消失点算出部２２は、上記の統合部２１で、画像領域ごとに、第２の算出部１３で算出したオプティカルフローを１つのグループに統合し、それぞれのグループごとに、オプティカルフロー（物体の移動ベクトル）に基づいて消失点の座標を算出する。そして、移動物体除去部１４は、消失点の座標が近いものについてはグループ同士を、さらに１つのグループに統合する。なお、グループ同士をさらに統合する機能は、移動物体除去部１４に限ることはなく、消失点算出部２２が機能を有してもよい。

なお、座標が近いか否かの判断は、消失点の座標の距離が予め設定した閾値以下である場合に近いと判断し、閾値を超える場合に遠いと判断するようにする。なお、閾値の設定は任意である。

消失点算出部２２で、消失点を算出した後に、移動物体除去部１４は、統合されたグループについて、移動物体に対して背景が占める割合が高いという条件から、消失点を算出するための移動ベクトルの数が少ないものを移動物体と判定して物体の画像領域を時系列画像から除去する。

図９の例では、例えば、人の移動物体消失点を算出するためのオプティカルフロー（移動ベクトル）は３つで示されているので、他のオプティカルフローより数が少ないと判断し、人を移動物体と判定して人（物体）の画像領域を時系列画像から除去する。

また、複数カメラの距離画像の情報を用いて、例えば、画像領域（人）の距離が近い場合には危険と判断して無条件で、人を移動物体と判断してもよい。

図２に戻って説明を続ける。移動情報算出部２３は、画像入力部（カメラ）１１が移動する速度及び車輪の操舵角を少なくとも用いて、画像入力部１１の移動速度と移動方向とを含む移動情報を算出する。具体的には、移動情報算出部２３は、車載ネットワークの基幹プロトコルとなっているＣＡＮ（Ｃｏｎｔｒｏｌｌｅｒ Ａｒｅａ Ｎｅｔｗｏｒｋ）などから得られた車両の速度情報及びタイヤの操舵角情報を少なくとも用いて、車両の移動速度と移動方向を算出する。

動き消失点取得部２４は、移動情報算出部２３で算出された移動情報に基づいて、順に入力された時系列画像から画像入力部（カメラ）１１の移動により生成した第１の消失点（動き消失点）の座標を取得する。図１０及び図１１は、動き消失点の座標について説明する図である。動き消失点取得部２４は、移動情報算出部２３で算出した、例えば、車両の移動速度と移動方向の移動情報に基づいて、図１０に示した動き消失点パターンから、例えば、車両自身の移動によって画像上に生成される動き消失点を検索して取得する。

動き消失点パターンとして、図１０に一例を示す。図１０の例では、画像入力部（カメラ）１１が直進する場合は、動き消失点は画像の中心部にある。また、画像入力部１１が左カーブする場合は、動き消失点は画像の中心から左側にある。また、画像入力部１１が右カーブする場合は、動き消失点は画像の中心から右側にある。なお、左折及び右折の場合には、動き消失点が画像上にないが、移動ベクトルの方向が左か右に一定となるため、その移動ベクトルの向きと異なる動きをしている移動ベクトルは、移動物体のオプティカルフロー（移動ベクトル）であると判断することができる。

また、図１１に示すように、移動物体除去部１４は、順に入力された時系列画像における物体の座標（追跡中の特徴点）と取得した第１の消失点（動き消失点）の座標とを結んだ方向のベクトルと、第２の算出部１３で算出されたオプティカルフローの移動ベクトルとの内積を計算した値に基づき、移動物体を判定して物体の画像領域を時系列画像から除去する。

図１１の例では、動き消失点と追跡中の特徴点を結んだ方向のベクトルと、実際に特徴点の追跡により求められたオプティカルフローのベクトルの差を、ベクトルの内積からｃｏｓθとして求めている。そして、移動物体除去部１４は、求めたｃｏｓθが予め設定した閾値を超える（差が大きい、ズレが大きい）場合、物体を移動物体と判定して物体の画像領域を時系列画像から除去する。なお、閾値は任意に設定できる。

図２に戻って説明を続ける。記憶部２５は、物体の世界座標系における座標を含む地図情報を記憶する。また、記憶部２５は、第１の算出部１２で時系列画像から抽出された物体で、移動物体除去部１４で除去されなかった物体の世界座標系における座標を含む地図情報を記憶する。

記憶部２５は、画像入力部（カメラ）１１の移動により生成した第１の消失点（動き消失点）の座標の移動ベクトルのパターン（動き消失点パターン）を記憶する。なお、動き消失点パターンについては、上記の図１０を用いて説明したものと同様である。

記憶部２５は、後述の図１２を用いて説明する、障害物の画像領域を示す障害物マスク画像を記憶する。なお、記憶部２５は、予め障害物マスク画像を記憶するようにしてもよい。なお、記憶部２５は、障害物マスク画像を、障害物エリア予測マスクのデータベースとして記憶するようにしてもよい。さらに、障害物エリア予測マスクのデータベースは、記憶部２５が備えることに限ることはなく、例えば、通信可能な外部装置（サーバ装置）に備えるように構成することもできる。

距離画像算出部２６は、画像入力部１１から入力された時系列画像の視差画像から求められる視差を用いて、障害物の画像領域を示す障害物画像の領域を含む距離画像を算出する。図１２は、ステレオ距離画像と障害物マスク画像の一例について説明する図である。距離画像算出部２６は、図１２に示すように、例えば、複数のカメラ（画像入力部）１１から入力された時系列画像の視差画像の視差から得られるステレオ距離画像を算出する。その際に、距離画像算出部２６では、例えば、車両や人、その他の障害物があった場合に、障害物領域を生成する。

次に、移動物体除去部１４は、上記の障害物マスク画像に、生成した障害物領域が重なり、かつステレオ距離画像から障害物領域との距離が予め設定した距離以下の場合に、障害物領域を移動物体と判定し、判定した移動物体の画像領域（障害物領域）を時系列画像から除去する。すなわち、移動物体除去部１４は、距離画像算出部２６で算出された上記の距離画像の情報と、記憶部２５に予め記憶されている障害物マスク画像との情報から、障害物マスク画像に障害物領域が一致し、かつ障害物領域との距離が近い画像領域を移動物体と判断する。

図１２の例では、移動情報算出部２３で、画像入力部１１の移動速度と移動方向とを含む移動情報を算出し、動き消失点取得部２４で、移動情報算出部２３で算出された移動情報に基づいて、画像入力部（カメラ）１１の移動により生成した第１の消失点（動き消失点）の座標を取得する。次に、距離画像算出部２６は、障害物の画像領域を示す障害物領域を含むステレオ距離画像を生成し、記憶部２５に記憶されている障害物マスク画像（図１２の例では路面の距離画像）に、距離画像算出部２６で生成したステレオ距離画像（路面の距離画像）に含まれる障害物領域が重なり、かつ距離画像から障害物領域との距離が予め設定した距離以下の場合に、障害物領域を移動物体と判定し、物体の画像領域を時系列画像から除去する。すなわち、距離画像算出部２６で生成したステレオ距離画像（路面の距離画像）と障害物マスク画像が一致し、ステレオ距離画像に含まれる障害物領域との距離が近いと判断された場合、障害物領域を移動物体と判定して物体の画像領域を時系列画像から除去する。

図２に戻って説明を続ける。路面特定部２７は、順に入力された時系列画像から路面の画像領域を特定する。路面特定部２７は、特定した路面の画像領域を移動物体除去部１４に受け渡すと共に記憶部２５に記憶させる。その後、移動物体除去部１４は、路面特定部２７から受け付けた路面の画像領域に基づいて、路面の画像領域と接する画像領域を移動物体と判定して物体の画像領域を時系列画像から除去する。これにより、例えば、路面上に静止している移動物体を判定して物体を除去することができる。

次に、図１３を用いて、本実施形態の自己位置推定装置１００の処理動作について説明する。図１３は、本実施形態の自己位置推定装置の処理動作の一例について説明するフローチャート図である。

まず、自己位置推定装置１００の第１の算出部１２は、画像入力部（カメラ）１１から入力された時系列画像から物体を示す画像領域（特徴点（ｘ，ｙ））を抽出して、世界座標系における物体の３次元位置座標（Ｘ，Ｙ，Ｚ）を算出する（ステップＳ１）。それと並行して、第２の算出部１３は、画像入力部（カメラ）１１から順に入力された時系列画像で、抽出した物体を追跡して物体のオプティカルフローを算出する（ステップＳ２）。ここで、最初に入力される時系列画像（フレーム）では、追跡対象の物体（特徴点）の情報が無いので次に入力される時系列画像（２フレーム）以降に物体のオプティカルフローの算出処理が開始される。

次に、重複点除去部１５は、第１の算出部１２で新たに抽出した物体（特徴点）と、第２の算出部１３で追跡している物体（特徴点）との対応付けを行って、物体（特徴点）が同じ物体であると判定した場合に、重複している新たに抽出した物体の画像領域を時系列画像から除去し（ステップＳ３）、物体の追跡を中止する。すなわち、新たに抽出した物体と、追跡している物体とが重複している場合、物体のオプティカルフローの算出を中止することで、自己位置推定装置１００の処理の負荷を軽減させることができる。

次に、推定部１８は、世界座標系における物体（特徴点）の３次元位置座標（Ｘ，Ｙ，Ｚ）と、カメラ（画像入力部）１１を基準とした座標系を示すカメラ座標系の物体（特徴点）の座標（ｘ，ｙ）から、画像を入力したカメラ（画像入力部）１１の物体（特徴点）に対する位置及び姿勢を示す回転角を推定する（ステップＳ４）。

次に、推定部１８は、カメラ（画像入力部）１１の位置及び姿勢の推定が成功したか否かを判定する（ステップＳ５）。推定に成功しない場合（ステップＳ５：Ｎｏ）、ステップＳ１、Ｓ２に戻り処理を継続する。

推定に成功した場合（ステップＳ５：Ｙｅｓ）、更新部１７は、推定したカメラ（画像入力部）１１の位置及び姿勢に基づき、地図情報を更新する（ステップＳ６）。すなわち、更新部１７は、カメラ座標系における物体（特徴点）の座標（ｘ，ｙ）と、記憶部２５に記憶されている地図情報の世界座標系における物体（特徴点）の３次元位置座標（Ｘ，Ｙ，Ｚ）をカメラ座標系に再投影した座標（ｘ′，ｙ′）との誤差を算出し、算出した誤差が予め設定された閾値以上である場合、記憶部２５に記憶されている物体（特徴点）の地図情報（世界座標系における物体の座標）を削除して地図情報を更新する。その後、処理を終了する。

次に、図１４を用いて、図１３のステップＳ６の処理の詳細について説明する。図１４は、地図情報の更新処理の一例について説明するフローチャート図である。更新部１７は、上記で推定したカメラ（画像入力部）１１の位置及び姿勢に基づき、物体（特徴点）が新たな物体（特徴点）であるか否かを判定する（ステップＳ１１）。新たな物体（特徴点）である場合（ステップＳ１１：Ｙｅｓ）、更新部１７は、時系列画像における物体（特徴点）のカメラ座標系での座標（ｘ，ｙ）と、世界座標系の３次元座標からカメラ座標系の２次元座標に変換する変換パラメータ［Ｒ｜ｔ］とから物体（特徴点）の３次元位置座標（Ｘ，Ｙ，Ｚ）を算出する（ステップＳ１２）。

次に、更新部１７は、カメラ座標系における物体（特徴点）の座標（ｘ，ｙ）と、記憶部２５に記憶されている地図情報の世界座標系における物体（特徴点）の３次元位置座標（Ｘ，Ｙ，Ｚ）を変換パラメータ［Ｒ｜ｔ］を用いてカメラ座標系に再投影した座標（ｘ′，ｙ′）との投影誤差、及び視差画像から視差を算出し（ステップＳ１３）、算出した投影誤差、及び視差が予め設定された閾値を満たすか否かを判断する（ステップＳ１４）。

投影誤差が閾値以上（閾値を満たさない）である場合（ステップＳ１４：Ｎｏ）、処理を終了する。投影誤差が閾値以下（閾値を満たす）である場合（ステップＳ１４：Ｙｅｓ）、記憶部２５に記憶されている地図情報に新たな物体（特徴点）の３次元位置座標（Ｘ，Ｙ，Ｚ）を追加して地図情報を更新（ステップＳ１５）して処理を終了する。

ステップＳ１１に戻り、新たな物体（特徴点）でない場合（ステップＳ１１：Ｎｏ）、更新部１７は、ステップＳ１３で説明した投影誤差を再度算出する（ステップＳ１６）。次に、更新部１７は、再度算出した投影誤差が予め設定された閾値を満たすか否かを判断する（ステップＳ１７）。

投影誤差が閾値以下（閾値を満たす）である場合（ステップＳ１７：Ｙｅｓ）、処理を終了する。投影誤差が閾値以上（閾値を満たさない）である場合（ステップＳ１７：Ｎｏ）、記憶部２５に記憶されている地図情報の物体（特徴点）の３次元位置座標（Ｘ，Ｙ，Ｚ）を削除して地図情報を更新（ステップＳ１８）して処理を終了する。

以上に説明したように、本実施形態では、自己位置推定装置１００に上記の処理を実行させることで、例えば、画像入力部（カメラ）１１から順に入力された時系列画像から物体（特徴点）を抽出して、抽出した物体を追跡して物体のオプティカルフローを算出する。そして、算出されたオプティカルフローに基づいて、順に入力された時系列画像から画像入力部（カメラ）１１の移動により生成した第１の消失点（動き消失点）の座標を取得し、第１の消失点（動き消失点）の座標と、オプティカルフロー（物体の移動ベクトル）に基づいて算出した第２の消失点（移動体消失点）の座標との差を算出する。そして、算出した差が大きい物体を移動物体と判定して時系列画像から除去し、除去されなかった物体の世界座標系における座標を含む地図情報を更新して記憶する。これにより、本実施形態によれば、自己位置の推定に適用可能な初期の地図を生成することができる、という有利な効果を達成できる。

以上、本発明に係る実施形態について説明したが、本発明は、上述の実施形態そのままに限定されるものではなく、実施段階ではその要旨を逸脱しない範囲で構成要素を変形して具体化できる。また、上述の実施形態に開示されている複数の構成要素の適宜な組み合わせにより、種々の発明を形成できる。例えば、実施の形態に示される全構成要素から幾つかの構成要素を削除してもよい。さらに、異なる実施形態にわたる構成要素を適宜組み合わせてもよい。

また、上述した各実施形態の自己位置推定装置１００及び地図生成装置１０で実行されるプログラムは、インストール可能な形式または実行可能な形式のファイルでフロッピー（登録商標）ディスク、ＣＤ（Ｃｏｍｐａｃｔ Ｄｉｓｋ）、ＣＤ−Ｒ（Ｃｏｍｐａｃｔ Ｄｉｓｋ−Ｒｅｃｏｒｄａｂｌｅ）、ＣＤ−ＲＯＭ（Ｃｏｍｐａｃｔ Ｄｉｓｃ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＤＶＤ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｋ）、ＳＤメモリカード（ＳＤ ｍｅｍｏｒｙ ｃａｒｄ）、ＵＳＢメモリ（Ｕｎｉｖｅｒｓａｌ Ｓｅｒｉａｌ Ｂｕｓ ｍｅｍｏｒｙ）等のコンピュータで読み取り可能な記録媒体に記録して提供するように構成してもよいし、インターネット等のネットワーク経由で提供または配布するように構成してもよい。また、各種プログラムを、ＲＯＭ等に予め組み込んで提供するように構成してもよい。

なお、本実施形態の地図生成装置１０を備えた自己位置推定装置１００の構成は一例であり、用途や目的に応じて様々な装置構成例があることは言うまでもない。

１０ 地図生成装置
１１ 画像入力部（カメラ）
１２ 第１の算出部
１３ 第２の算出部
１４ 移動物体除去部
１５ 重複点除去部
１６ 記憶制御部
１７ 更新部
１８ 推定部
２１ 統合部
２２ 消失点算出部
２３ 移動情報算出部
２４ 動き消失点取得部
２５ 記憶部
２６ 距離画像算出部
２７ 路面特定部
１００ 自己位置推定装置
１０１ 入力装置
１０２ 表示装置
１０３ ドライブ装置
１０４ ＲＡＭ
１０５ ＲＯＭ
１０６ ＣＰＵ
１０７ インタフェース装置
１０８ ＨＤＤ
１０９ 記録媒体

特開２０１４−０９５５５３号公報

Claims (11)

1. 移動中の画像を時系列に入力する画像入力部と、
   前記画像入力部から入力された時系列画像から物体を示す画像領域を抽出して、世界座標系における前記物体の座標を算出する第１の算出部と、
   順に入力された前記時系列画像で、抽出した前記画像領域の前記物体を追跡して前記物体のオプティカルフローを算出する第２の算出部と、
   前記第２の算出部で算出された前記オプティカルフローに基づいて、前記画像入力部の移動により生成した第１の消失点の座標と、前記物体の移動により生成した第２の消失点の座標との差を算出し、算出した前記差に基づいて前記物体が移動物体であると判定した場合、前記物体の画像領域を前記時系列画像から除去する移動物体除去部と、
   前記移動物体除去部で除去されなかった前記物体の前記世界座標系における前記座標を含む地図情報を記憶部に記憶させる記憶制御部と、
   前記第１の算出部で新たに抽出した物体と、前記第２の算出部で追跡している前記物体とが同じ物体であると判定した場合に、前記新たに抽出した物体を除去して前記追跡を中止する重複点除去部と、
   前記画像入力部を基準とした座標系を示すカメラ座標系における前記物体の座標と、前記地図情報の前記世界座標系における前記物体の座標を前記カメラ座標系に再投影した座標との誤差を算出し、算出した前記誤差が予め設定された閾値以上である場合、前記記憶部に記憶されている前記物体の前記地図情報を削除して前記地図情報を更新する更新部と、
   を備えることを特徴とする地図生成装置。
2. 移動中の画像を時系列に入力する画像入力部と、
   前記画像入力部から入力された時系列画像から物体を示す画像領域を抽出して、前記時系列画像から複数の物体の特徴点を抽出し、複数の前記画像入力部から入力された視差画像から世界座標系における前記物体の特徴点の座標を算出する第１の算出部と、
   順に入力された前記時系列画像で、抽出した前記画像領域の前記複数の物体の特徴点を追跡して前記複数の物体のオプティカルフローを算出する第２の算出部と、
   前記第２の算出部で算出された前記オプティカルフローに基づいて、前記画像入力部の移動により生成した第１の消失点の座標と、前記物体の移動により生成した第２の消失点の座標との差を算出し、算出した前記差に基づいて前記物体が移動物体であると判定した場合、前記物体の画像領域を前記時系列画像から除去する移動物体除去部と、
   前記移動物体除去部で除去されなかった前記物体の前記世界座標系における前記座標を含む地図情報を記憶部に記憶させる記憶制御部と、
   前記時系列画像を画像の種類ごとの画像領域に分割し、分割した画像領域ごとに前記オプティカルフローを１つのグループに統合する統合部と、
   前記グループごとに、前記オプティカルフローに基づいて消失点の座標を算出する消失点算出部と、
   を備え、
   前記移動物体除去部は、前記消失点の座標の距離が予め設定した閾値以下である前記グループ同士をさらに統合し、統合された前記グループについて、前記消失点を算出するための前記オプティカルフローの移動ベクトルの数が少ないものを移動物体と判定して前記物体の画像領域を前記時系列画像から除去する、
   ことを特徴とする地図生成装置。
3. 移動中の画像を時系列に入力する画像入力部と、
   前記画像入力部から入力された時系列画像から物体を示す画像領域を抽出して、世界座標系における前記物体の座標を算出する第１の算出部と、
   順に入力された前記時系列画像で、抽出した前記画像領域の前記物体を追跡して前記物体のオプティカルフローを算出する第２の算出部と、
   前記第２の算出部で算出された前記オプティカルフローに基づいて、前記画像入力部の移動により生成した第１の消失点の座標と、前記物体の移動により生成した第２の消失点の座標との差を算出し、算出した前記差に基づいて前記物体が移動物体であると判定した場合、前記物体の画像領域を前記時系列画像から除去する移動物体除去部と、
   前記移動物体除去部で除去されなかった前記物体の前記世界座標系における前記座標を含む地図情報を記憶部に記憶させる記憶制御部と、
   前記画像入力部が移動する速度及び車輪の操舵角を少なくとも用いて、前記画像入力部の移動速度と移動方向とを含む移動情報を算出する移動情報算出部と、
   前記移動情報に基づいて、順に入力された前記時系列画像から前記画像入力部の移動により生成した前記第１の消失点の座標を取得する動き消失点取得部と、
   を備え、
   前記移動物体除去部は、順に入力された前記時系列画像における前記物体の座標と取得した前記第１の消失点の座標とを結んだ方向のベクトルと、前記第２の算出部で算出された前記オプティカルフローの移動ベクトルとの内積を計算した値に基づき、移動物体を判定して前記物体の画像領域を前記時系列画像から除去する、
   ことを特徴とする地図生成装置。
4. 障害物の画像領域を示す障害物マスク画像を記憶する記憶部と、
   前記画像入力部から入力された前記時系列画像の視差画像から求められる視差を用いて、障害物の画像領域を示す障害物画像を含む距離画像を算出する距離画像算出部と、をさらに備え、
   前記移動物体除去部は、前記障害物マスク画像に前記障害物画像が重なり、かつ前記距離画像から前記障害物画像との距離が予め設定した距離以下の場合に、前記障害物画像を移動物体と判定して前記物体の画像領域を前記時系列画像から除去する、
   ことを特徴とする請求項２に記載の地図生成装置。
5. 前記時系列画像から路面の画像領域を特定する路面特定部を、さらに備え、
   前記移動物体除去部は、特定した前記路面の画像領域に接する画像領域を移動物体と判定して前記物体の画像領域を前記時系列画像から除去する、
   ことを特徴とする請求項２に記載の地図生成装置。
6. 移動中の画像を時系列に入力する画像入力ステップと、
   前記画像入力ステップで入力された時系列画像から物体を示す画像領域を抽出して、世界座標系における前記物体の座標を算出する第１の算出ステップと、
   順に入力された前記時系列画像で、抽出した前記画像領域の前記物体を追跡して前記物体のオプティカルフローを算出する第２の算出ステップと、
   前記第２の算出ステップで算出された前記オプティカルフローに基づいて、画像入力部の移動により生成した第１の消失点の座標と、前記物体の移動により生成した第２の消失点の座標との差を算出し、算出した前記差に基づいて前記物体が移動物体であると判定した場合、前記物体の画像領域を前記時系列画像から除去する移動物体除去ステップと、
   前記移動物体除去ステップで除去されなかった前記物体の前記世界座標系における前記座標を含む地図情報を記憶部に記憶させる記憶制御ステップと、
   前記第１の算出ステップで新たに抽出した物体と、前記第２の算出ステップで追跡している前記物体とが同じ物体であると判定した場合に、前記新たに抽出した物体を除去して前記追跡を中止する重複点除去ステップと、
   前記画像入力ステップを基準とした座標系を示すカメラ座標系における前記物体の座標と、前記地図情報の前記世界座標系における前記物体の座標を前記カメラ座標系に再投影した座標との誤差を算出し、算出した前記誤差が予め設定された閾値以上である場合、前記記憶部に記憶されている前記物体の前記地図情報を削除して前記地図情報を更新する更新ステップと、
   を含む、
   ことを特徴とする地図生成方法。
7. 移動中の画像を時系列に入力する画像入力ステップと、
   前記画像入力ステップで入力された時系列画像から物体を示す画像領域を抽出して、前記時系列画像から複数の物体の特徴点を抽出し、複数の画像入力部から入力された視差画像から世界座標系における前記物体の特徴点の座標を算出する第１の算出ステップと、
   順に入力された前記時系列画像で、抽出した前記画像領域の前記複数の物体の特徴点を追跡して前記複数の物体のオプティカルフローを算出する第２の算出ステップと、
   前記第２の算出ステップで算出された前記オプティカルフローに基づいて、画像入力部の移動により生成した第１の消失点の座標と、前記物体の移動により生成した第２の消失点の座標との差を算出し、算出した前記差に基づいて前記物体が移動物体であると判定した場合、前記物体の画像領域を前記時系列画像から除去する移動物体除去ステップと、
   前記移動物体除去ステップで除去されなかった前記物体の前記世界座標系における前記座標を含む地図情報を記憶部に記憶させる記憶制御ステップと、
   前記時系列画像を画像の種類ごとの画像領域に分割し、分割した画像領域ごとに前記オプティカルフローを１つのグループに統合する統合ステップと、
   前記グループごとに、前記オプティカルフローに基づいて消失点の座標を算出する消失点算出ステップと、
   を含み、
   前記移動物体除去ステップは、前記消失点の座標の距離が予め設定した閾値以下である前記グループ同士をさらに統合し、統合された前記グループについて、前記消失点を算出するための前記オプティカルフローの移動ベクトルの数が少ないものを移動物体と判定して前記物体の画像領域を前記時系列画像から除去する、
   ことを特徴とする地図生成方法。
8. 前記第１の算出ステップで新たに抽出した物体と、前記第２の算出ステップで追跡している前記物体とが同じ物体であると判定した場合に、前記新たに抽出した物体を除去して前記追跡を中止する重複点除去ステップと、
   前記画像入力部を基準とした座標系を示すカメラ座標系における前記物体の座標と、前記地図情報に記憶されている前記世界座標系における前記物体の座標を前記カメラ座標系に再投影した座標との誤差を算出し、算出した前記誤差が予め設定された閾値以上である場合、前記記憶部に記憶されている前記物体の前記地図情報を削除して前記地図情報を更新する更新ステップと、をさらに含む、
   ことを特徴とする請求項６または７に記載の地図生成方法。
9. 地図生成装置に、
   移動中の画像を時系列に入力する画像入力ステップと、
   前記画像入力ステップで入力された時系列画像から物体を示す画像領域を抽出して、世界座標系における前記物体の座標を算出する第１の算出ステップと、
   順に入力された前記時系列画像で、抽出した前記画像領域の前記物体を追跡して前記物体のオプティカルフローを算出する第２の算出ステップと、
   前記第２の算出ステップで算出された前記オプティカルフローに基づいて、画像入力部の移動により生成した第１の消失点の座標と、前記物体の移動により生成した第２の消失点の座標との差を算出し、算出した前記差に基づいて前記物体が移動物体であると判定した場合、前記物体の画像領域を前記時系列画像から除去する移動物体除去ステップと、
   前記移動物体除去ステップで除去されなかった前記物体の前記世界座標系における前記座標を含む地図情報を記憶部に記憶させる記憶制御ステップと、
   前記第１の算出ステップで新たに抽出した物体と、前記第２の算出ステップで追跡している前記物体とが同じ物体であると判定した場合に、前記新たに抽出した物体を除去して前記追跡を中止する重複点除去ステップと、
   前記画像入力ステップを基準とした座標系を示すカメラ座標系における前記物体の座標と、前記地図情報の前記世界座標系における前記物体の座標を前記カメラ座標系に再投影した座標との誤差を算出し、算出した前記誤差が予め設定された閾値以上である場合、前記記憶部に記憶されている前記物体の前記地図情報を削除して前記地図情報を更新する更新ステップと、
   を実行させるためのプログラム。
10. 地図生成装置に、
    移動中の画像を時系列に入力する画像入力ステップと、
    前記画像入力ステップで入力された時系列画像から物体を示す画像領域を抽出して、前記時系列画像から複数の物体の特徴点を抽出し、複数の画像入力部から入力された視差画像から世界座標系における前記物体の特徴点の座標を算出する第１の算出ステップと、
    順に入力された前記時系列画像で、抽出した前記画像領域の前記複数の物体の特徴点を追跡して前記複数の物体のオプティカルフローを算出する第２の算出ステップと、
    前記第２の算出ステップで算出された前記オプティカルフローに基づいて、画像入力部の移動により生成した第１の消失点の座標と、前記物体の移動により生成した第２の消失点の座標との差を算出し、算出した前記差に基づいて前記物体が移動物体であると判定した場合、前記物体の画像領域を前記時系列画像から除去する移動物体除去ステップと、
    前記移動物体除去ステップで除去されなかった前記物体の前記世界座標系における前記座標を含む地図情報を記憶部に記憶させる記憶制御ステップと、
    前記時系列画像を画像の種類ごとの画像領域に分割し、分割した画像領域ごとに前記オプティカルフローを１つのグループに統合する統合ステップと、
    前記グループごとに、前記オプティカルフローに基づいて消失点の座標を算出する消失点算出ステップと、
    を実行させ、
    前記移動物体除去ステップは、前記消失点の座標の距離が予め設定した閾値以下である前記グループ同士をさらに統合し、統合された前記グループについて、前記消失点を算出するための前記オプティカルフローの移動ベクトルの数が少ないものを移動物体と判定して前記物体の画像領域を前記時系列画像から除去する、
    ことを特徴とするプログラム。
11. 前記第１の算出ステップで新たに抽出した物体と、前記第２の算出ステップで追跡している前記物体とが同じ物体であると判定した場合に、前記新たに抽出した物体を除去して前記追跡を中止する重複点除去ステップと、
    前記画像入力部を基準とした座標系を示すカメラ座標系における前記物体の座標と、前記地図情報に記憶されている前記世界座標系における前記物体の座標を前記カメラ座標系に再投影した座標との誤差を算出し、算出した前記誤差が予め設定された閾値以上である場合、前記記憶部に記憶されている前記物体の前記地図情報を削除して前記地図情報を更新する更新ステップと、をさらに含む、
    ことを特徴とする請求項９または１０に記載のプログラム。

Images