(57)【要約】【課題】自己位置の推定に用いる初期の環境地図を生成して精度良く自己位置を推定することができる自己位置推定装置、自己位置推定方法およびプログラムを提供する。【解決手段】本発明に係る自己位置推定装置は、画像入力部（カメラ）１１から順に入力された時系列画像から物体の特徴点を抽出して、抽出した特徴点を追跡して移動ベクトルを算出する。そして、算出された特徴点の移動ベクトルの長さ及び方向に基づいて、特徴点が移動物体であるか否かを判定し、特徴点が移動物体であると判定した場合、特徴点に対応する物体を時系列画像から除去し、除去されなかった特徴点の物体の世界座標系における座標を含む地図情報を生成し、生成した地図情報に基づいて自己位置を推定する。【選択図】図２
2016157197.tif 000002
【特許請求の範囲】
【請求項１】
移動中の画像を時系列に入力する画像入力部と、
前記画像入力部から入力された時系列画像から物体の特徴点を抽出して、前記画像入力部から前記特徴点までの距離を算出する距離算出部と、
前記画像入力部を基準とした座標系を示すカメラ座標系における前記特徴点の座標と、算出した前記特徴点までの距離とを用いて、世界座標系における前記特徴点の座標を算出する座標算出部と、
順に入力された前記時系列画像で、抽出した前記特徴点を追跡して、前記座標算出部で算出された世界座標系における前記特徴点の座標から前記特徴点の移動ベクトルを算出する移動ベクトル算出部と、
前記移動ベクトル算出部で算出された前記移動ベクトルの長さ及び方向に基づいて、前記特徴点が移動物体であるか否かを判定する移動物体判定部と、
前記特徴点が移動物体であると判定した場合、前記特徴点に対応する前記物体を前記時系列画像から除去する移動物体除去部と、
前記移動物体除去部で除去されなかった前記物体の前記世界座標系における前記座標を含む地図情報を生成する地図生成部と、
生成した前記地図情報に基づいて自己位置を推定する推定部と、
を備える、
ことを特徴とする自己位置推定装置。
【請求項２】
前記座標算出部は、複数の前記画像入力部から入力された前記時系列画像の視差画像から求められる前記物体の前記特徴点の視差の逆数を、前記特徴点までの距離に用いて、世界座標系における前記特徴点の座標を算出する、
ことを特徴とする請求項１に記載の自己位置推定装置。
【請求項３】
前記移動ベクトル算出部は、順に入力された前記時系列画像において、時刻ｔでの世界座標系における前記特徴点の座標と、時刻ｔ＋Δｔでの世界座標系における前記特徴点の座標とから前記特徴点の移動ベクトルを算出する、
ことを特徴とする請求項１に記載の自己位置推定装置。
【請求項４】
前記距離算出部は、前記時系列画像から複数の物体の特徴点を抽出して、前記画像入力部から、複数の前記特徴点までの距離をそれぞれ算出し、
前記座標算出部は、前記カメラ座標系における複数の前記特徴点の座標と、算出した複数の前記特徴点までの距離とを用いて、世界座標系における複数の前記特徴点の座標をそれぞれ算出し、
前記移動ベクトル算出部は、順に入力された前記時系列画像で、抽出した複数の前記特徴点を追跡して、前記座標算出部で算出された世界座標系における複数の前記特徴点の座標から複数の前記特徴点の移動ベクトルをそれぞれ算出し、
前記移動物体判定部は、前記移動ベクトル算出部で算出された複数の前記特徴点の移動ベクトルの長さ及び向きが同一または類似する前記移動ベクトルを１つのグループに統合し、統合されたそれぞれのグループに所属する移動ベクトルの数が他のグループよりも多いグループを静止物体の移動ベクトルのグループと判定し、
前記移動物体除去部は、判定された前記静止物体の移動ベクトルのグループを除く他のグループに所属する移動ベクトルの前記特徴点の前記物体を前記時系列画像から除去する、
ことを特徴とする請求項１に記載の自己位置推定装置。
【請求項５】
前記画像入力部が移動する速度及び車輪の操舵角を少なくとも用いて、前記画像入力部の移動速度と移動方向とを含む移動情報を算出する移動情報算出部をさらに備え、
前記移動ベクトル算出部は、前記特徴点の移動ベクトルを算出すると共に、前記移動情報を用いて、前記特徴点の移動ベクトルを予測し、
前記移動物体判定部は、算出した移動ベクトルと、予測した移動ベクトルとが同一または類似する場合、算出した移動ベクトルの前記特徴点の前記物体を静止物体と判定し、
前記移動物体除去部は、判定された前記静止物体の移動ベクトルを除く他の移動ベクトルの前記特徴点の前記物体を前記時系列画像から除去する、
ことを特徴とする請求項１に記載の自己位置推定装置。
【請求項６】
移動中の画像を時系列に入力する画像入力ステップと、
入力された時系列画像から物体の特徴点を抽出して、画像入力部から前記特徴点までの距離を算出する距離算出ステップと、
画像入力部を基準とした座標系を示すカメラ座標系における前記特徴点の座標と、算出した前記特徴点までの距離とを用いて、世界座標系における前記特徴点の座標を算出する座標算出ステップと、
順に入力された前記時系列画像で、抽出した前記特徴点を追跡して、前記座標算出ステップで算出された世界座標系における前記特徴点の座標から前記特徴点の移動ベクトルを算出する移動ベクトル算出ステップと、
算出された前記移動ベクトルの長さ及び方向に基づいて、前記特徴点が移動物体であるか否かを判定する移動物体判定ステップと、
前記特徴点が移動物体であると判定した場合、前記特徴点に対応する前記物体を前記時系列画像から除去する移動物体除去ステップと、
前記移動物体除去ステップで除去されなかった前記物体の前記世界座標系における前記座標を含む地図情報を生成する地図生成ステップと、
生成した前記地図情報に基づいて自己位置を推定する推定ステップと、
を含む、
ことを特徴とする自己位置推定方法。
【請求項７】
前記画像入力部が移動する速度及び車輪の操舵角を少なくとも用いて、前記画像入力部の移動速度と移動方向とを含む移動情報を算出する移動情報算出ステップをさらに含み、
前記移動ベクトル算出ステップは、前記特徴点の移動ベクトルを算出すると共に、前記移動情報を用いて、前記特徴点の移動ベクトルを予測し、
前記移動物体判定ステップは、算出した移動ベクトルと、予測した移動ベクトルとが同一または類似する場合、算出した移動ベクトルの前記特徴点の前記物体を静止物体と判定し、
前記移動物体除去ステップは、判定された前記静止物体の移動ベクトルを除く移動ベクトルの前記特徴点の前記物体を前記時系列画像から除去する、
ことを特徴とする請求項６に記載の自己位置推定方法。
【請求項８】
自己位置推定装置に、
移動中の画像を時系列に入力する画像入力ステップと、
入力された時系列画像から物体の特徴点を抽出して、画像入力部から前記特徴点までの距離を算出する距離算出ステップと、
画像入力部を基準とした座標系を示すカメラ座標系における前記特徴点の座標と、算出した前記特徴点までの距離とを用いて、世界座標系における前記特徴点の座標を算出する座標算出ステップと、
順に入力された前記時系列画像で、抽出した前記特徴点を追跡して、前記座標算出ステップで算出された世界座標系における前記特徴点の座標から前記特徴点の移動ベクトルを算出する移動ベクトル算出ステップと、
算出された前記移動ベクトルの長さ及び方向に基づいて、前記特徴点が移動物体であるか否かを判定する移動物体判定ステップと、
前記特徴点が移動物体であると判定した場合、前記特徴点に対応する前記物体を前記時系列画像から除去する移動物体除去ステップと、
前記移動物体除去ステップで除去されなかった前記物体の前記世界座標系における前記座標を含む地図情報を生成する地図生成ステップと、
生成した前記地図情報に基づいて自己位置を推定する推定ステップと、
を実行させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、自己位置推定装置、自己位置推定方法およびプログラムに関する。
【背景技術】
【０００２】
従来、自動車の走行時に現在位置や目的地への経路案内を行なうカーナビゲーションシステム（Ａｕｔｏｍｏｔｉｖｅ  Ｎａｖｉｇａｔｉｏｎ  Ｓｙｓｔｅｍ）は、自車位置の特定をＧＰＳ（Ｇｌｏｂａｌ  Ｐｏｓｉｔｉｏｎｉｎｇ  Ｓｙｓｔｅｍ：全地球測位システム）衛星からの位置情報により基本的に行っている。ＧＰＳの電波は直進性が強く、トンネルの中、高層ビルの間、高架道路の下、樹木の間などでは電波が受信しづらく、ＧＰＳだけでは誤差があることから、カーナビゲーションシステム内のジャイロセンサ、加速度計、タイヤの回転に伴う車速信号などの情報を用いた自立航法との併用で自車位置を特定することが行われている。
【０００３】
また、自立航法では、路面やタイヤの状況などにより誤差が生じてしまうため、マップマッチングと呼ばれるソフトウェア技術で、地図上の道路データに自車位置を当てはめることでＧＰＳやジャイロセンサなどの誤差を補うという方法も用いられている。
【０００４】
また、レーザーレンジセンサ（測域センサ）、カメラ、エンコーダなどを利用して自己位置を数〜数十ｃｍ精度で把握できる技術として、ＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ  Ｌｏｃａｌｉｚａｔｉｏｎ  Ａｎｄ  Ｍａｐｐｉｎｇ：自己位置推定と環境地図作成を同時に行う技術）が知られている。特に、カメラを利用したＳＬＡＭは、Ｖｉｓｕａｌ  ＳＬＡＭと呼ばれている。Ｖｉｓｕａｌ  ＳＬＡＭ技術では、上記のセンサによりリアルタイムに局所的な地図を作成して地図情報から自車位置を特定するため、ＧＰＳの電波が受信しづらい状況下においても精度良く自車位置を特定することができる。また、ジャイロセンサや加速度計とは異なり、外界の情報を利用しているため、路面やタイヤの状況によらず自車位置を特定することができる。
【０００５】
特許文献１には、Ｖｉｓｕａｌ  ＳＬＡＭ技術を使って被写体を撮影したカメラの被写体に対する位置及び姿勢を示す回転角（カメラポーズ）の推定精度を向上させることができるカメラポーズ推定装置が開示されている。特許文献１に記載のカメラポーズ推定装置は、テンプレート画像に対応する領域の三次元位置を推定する際に、当該領域に含まれる特徴点から外れ値を除去してからテンプレート画像の三次元位置を推定するので、外れ値の影響を抑えて三次元位置の精度を向上させることができるものである。
【発明の概要】
【発明が解決しようとする課題】
【０００６】
Ｖｉｓｕａｌ  ＳＬＡＭ技術を使って自己位置推定を行う場合、時間の経過とともに誤差が蓄積されるため、自己位置を推定するための基準となる静止物体の３次元座標情報だけを含んだ環境地図を任意のタイミングで初期化する必要がある。この環境地図には、建物や道路、標識などの静止物体の３次元座標情報のみが必要であり、人や車などの移動物体の３次元座標情報は含まないようにする必要がある。
【０００７】
しかしながら、特許文献１に記載のカメラポーズ推定装置では、移動物体の３次元点情報を含まない環境地図を事前に作成する際に、人や車などの移動物体の有無にかかわらず、任意のタイミングで環境地図を初期化して精度良く自己位置を推定することができない、という問題があった。
【０００８】
本発明は、上記に鑑みてなされたものであって、自己位置の推定に用いる初期の環境地図を生成して精度良く自己位置を推定することができる自己位置推定装置、自己位置推定方法およびプログラムを提供することを目的とする。
【課題を解決するための手段】
【０００９】
上述した課題を解決し、目的を達成するために、本発明に係る自己位置推定装置は、移動中の画像を時系列に入力する画像入力部と、前記画像入力部から入力された時系列画像から物体の特徴点を抽出して、前記画像入力部から前記特徴点までの距離を算出する距離算出部と、前記画像入力部を基準とした座標系を示すカメラ座標系における前記特徴点の座標と、算出した前記特徴点までの距離とを用いて、世界座標系における前記特徴点の座標を算出する座標算出部と、順に入力された前記時系列画像で、抽出した前記特徴点を追跡して、前記座標算出部で算出された世界座標系における前記特徴点の座標から前記特徴点の移動ベクトルを算出する移動ベクトル算出部と、前記移動ベクトル算出部で算出された前記移動ベクトルの長さ及び方向に基づいて、前記特徴点が移動物体であるか否かを判定する移動物体判定部と、前記特徴点が移動物体であると判定した場合、前記特徴点に対応する前記物体を前記時系列画像から除去する移動物体除去部と、前記移動物体除去部で除去されなかった前記物体の前記世界座標系における前記座標を含む地図情報を生成する地図生成部と、生成した前記地図情報に基づいて自己位置を推定する推定部と、を備える、ことを特徴とする。
【発明の効果】
【００１０】
本発明によれば、精度良く自己位置を推定することができる、という有利な効果を奏する。
【図面の簡単な説明】
【００１１】
【図１】図１は、本実施形態に係る自己位置推定装置のハードウェア構成の一例を示す図である。
【図２】図２は、自己位置推定装置の機能構成の一例を示すブロック図である。
【図３】図３は、視差画像を用いて特徴点までの距離を算出する方法の一例について説明する図である。
【図４】図４は、視差画像を用いて特徴点までの距離を算出する方法の一例について説明する図である。
【図５】図５は、カメラ座標系における特徴点の座標と、算出した特徴点までの距離とを用いて、世界座標系における特徴点の座標を算出する方法の一例を説明する図である。
【図６】図６は、移動ベクトルの一例について説明する図である。
【図７】図７は、入力される時系列画像の一例について説明する図である。
【図８】図８は、入力される時系列画像の一例について説明する図である。
【図９】図９は、時系列画像から移動ベクトルを算出する方法の一例について説明する図である。
【図１０】図１０は、時系列画像から移動ベクトルを算出する方法の一例について説明する図である。
【図１１】図１１は、時系列画像から移動ベクトルを算出する方法の一例について説明する図である。
【図１２】図１２は、移動ベクトルから移動物体を判定する方法の一例について説明する図である。
【図１３】図１３は、カメラの移動量について説明する図である。
【図１４】図１４は、カメラの移動量について説明する図である。
【図１５】図１５は、カメラの移動量について説明する図である。
【図１６】図１６は、本実施形態の自己位置推定装置の処理動作の一例について説明するフローチャート図である。
【図１７】図１７は、本実施形態の自己位置推定装置の他の処理動作の一例について説明するフローチャート図である。
【発明を実施するための形態】
【００１２】
以下、添付図面を参照しながら、本発明に係る自己位置推定装置、自己位置推定方法およびプログラムの一実施形態を詳細に説明する。なお、以下の実施形態により本発明が限定されるものではない。また、各実施形態は、内容を矛盾させない範囲で、適宜組み合わせることができる。
【００１３】
本実施形態の自己位置推定装置は、画像入力部から入力された時系列画像から物体の特徴点を抽出して、画像入力部から特徴点までの距離を算出する。次に、カメラ座標系における特徴点の座標と、算出した特徴点までの距離とを用いて、世界座標系における特徴点の座標を算出する。次に、順に入力された時系列画像で、抽出した特徴点を追跡して、世界座標系における特徴点の座標から特徴点の移動ベクトルを算出する。次に、算出された移動ベクトルの長さ及び方向に基づいて、特徴点が移動物体であるか否かを判定し、特徴点が移動物体であると判定した場合、特徴点に対応する物体を時系列画像から除去する。次に、除去されなかった物体の世界座標系における座標を含む地図情報を生成し、生成した地図情報に基づいて自己位置を推定する。また、自己位置推定装置は、新たに抽出した物体と、追跡している物体とが同じ物体であると判定した場合に、新たに抽出した物体を除去して追跡を中止する。自己位置推定装置は、除去されなかった物体の世界座標系における座標を含む地図情報を更新して記憶する。これにより、自己位置の推定に用いる初期の環境地図を生成して精度良く自己位置を推定することができる。
【００１４】
以下では、本実施形態の自己位置推定装置を、Ｖｉｓｕａｌ  ＳＬＡＭを用いる自己位置推定装置として実現する例を説明する。本実施形態の自己位置推定装置が適用可能な装置は、このような自己位置推定装置に限られるものではない。
【００１５】
最初に、図１を用いて、自己位置推定装置１００のハードウェア構成例について説明する。図１は、本実施形態の自己位置推定装置のハードウェア構成の一例を示す図である。
【００１６】
図１に示すように、自己位置推定装置１００は、カメラ１１と、システムバス１３０に接続されたＶｉｓｕａｌ  ＳＬＡＭ  Ｈ／Ｗ（ＨａｒｄＷａｒｅ）部１０２と、ＣＰＵ（Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）１０３と、ＲＯＭ（Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ）１０４と、ＲＡＭ（Ｒａｎｄｏｍ  Ａｃｃｅｓｓ  Ｍｅｍｏｒｙ）１０５と、ＨＤＤ（Ｈａｒｄ  Ｄｉｓｋ  Ｄｒｉｖｅ）１０６と、表示装置１０７と、入力装置１０８と、インタフェース装置１０９と、ドライブ装置１１０とを備える。ドライブ装置１１０には記録媒体１１１が接続される。なお、本実施形態のＶｉｓｕａｌ  ＳＬＡＭの処理は、Ｖｉｓｕａｌ  ＳＬＡＭ部１２０で構成されるハードウェアで行われる。
【００１７】
カメラ１１は、例えば、移動中の画像を時系列に入力するデジタルカメラやビデオカメラ等の画像センサを有する撮像装置（カメラ）である。本実施形態において、カメラ１１は、例えば、視差による距離画像が取得可能な複数台のカメラ（ステレオカメラなど）１１である。なお、カメラは、複数台のカメラ１１に限ることはなく、時系列に撮影された複数フレームの画像から距離画像を取得することができる単眼カメラ１１でもよい。
【００１８】
Ｖｉｓｕａｌ  ＳＬＡＭ  Ｈ／Ｗ部１０２は、Ｖｉｓｕａｌ  ＳＬＡＭの処理を行うハードウェアである。具体的には、例えば、自己位置の推定を行う前に移動物体の除去を行い、特徴点の移動からカメラ１１のポーズ推定と、特徴点の３次元座標（世界座標）を算出する処理を行うハードウェアである。
【００１９】
ＣＰＵ１０３は、自己位置推定装置１００の動作を統括的に制御する。ＣＰＵ１０３は、ＲＯＭ１０４やＨＤＤ１０６に格納されたプログラムやデータをＲＡＭ（メモリ）１０５上に読み出し、処理を実行することで、自己位置推定装置１００全体の動作を制御する。また、ＣＰＵ１０３は、各種機能を実現する。
【００２０】
ＲＯＭ１０４は、電源を切っても内部データを保持することができる不揮発性の半導体メモリ（記憶装置）である。ＲＯＭ１０４には、自己位置推定装置１００の起動時に実行されるＢＩＯＳ（Ｂａｓｉｃ  Ｉｎｐｕｔ／Ｏｕｔｐｕｔ  Ｓｙｓｔｅｍ）、情報処理システム設定、及びネットワーク設定などのプログラムやデータが格納されている。
【００２１】
ＲＡＭ１０５は、各種プログラムやデータを一時記憶する揮発性の半導体メモリ（記憶装置）である。また、ＲＡＭ１０５は、ＣＰＵ１０３がデータ処理時に使用するメモリでもある。なお、ＲＡＭ１０５の代わりに、ＳＤＲＡＭ、ＤＲＡＭ、ＳＲＡＭなどを使用してもよい。
【００２２】
ＨＤＤ１０６は、自己位置推定装置１００で実行される各種のプログラムやデータを格納する不揮発性の記憶装置である。また、ＨＤＤ１０６は、自己位置推定装置１００の各種機能を実現するために必要なプログラムやデータ等を格納する。格納されるプログラムやデータには、自己位置推定装置１００全体を制御する情報処理システム（例えば「Ｗｉｎｄｏｗｓ（登録商標）」や「ＵＮＩＸ（登録商標）」などの基本ソフトウェアであるＯＳ（Ｏｐｅｒａｔｉｎｇ  Ｓｙｓｔｅｍ））、及びシステム上において各種機能を提供するアプリケーションなどがある。また、ＨＤＤ１０６は、格納しているプログラムやデータを、所定のファイルシステム及び／又はＤＢ（Ｄａｔａ  Ｂａｓｅ）により管理している。
【００２３】
表示装置１０７は、プログラムによるＧＵＩ（Ｇｒａｐｈｉｃａｌ  Ｕｓｅｒ  Ｉｎｔｅｒｆａｃｅ）等を表示する。表示装置１０７は、例えば、画像などを表示するディスプレイなどを含み、自己位置推定装置１００による処理結果などを表示する。ディスプレイは、例えば、液晶や有機ＥＬなどのＦＰＤ（Ｆｌａｔ  Ｐａｎｅｌ  Ｄｉｓｐｌａｙ）である。
【００２４】
入力装置１０８は、自己位置推定装置１００に各操作信号、各種データを入力する入力インタフェースである。
【００２５】
また、入力装置１０８は、キーボード、マウス、ディスプレイ等で構成され、ユーザの様々な操作指示を受け付けるユーザインタフェースである。入力装置１０８は、例えば、タッチパネルや音声入力装置を入力装置１０８とすることもできる。
【００２６】
インタフェース装置１０９は、例えば、ネットワークに接続するためのインタフェースである。自己位置推定装置１００は、インタフェース装置１０９を介して、ネットワークに接続された外部装置とデータ通信を行うことができる。
【００２７】
ドライブ装置１１０は、着脱可能な記録媒体１１１とのインタフェースである。自己位置推定装置１００は、ドライブ装置１１０を介して、記録媒体１１１の読み取り及び／又は書き込みを行うことができる。記録媒体１１１には、例えば、フロッピー（登録商標）ディスク、ＣＤ（Ｃｏｍｐａｃｔ  Ｄｉｓｋ）、ＤＶＤ（Ｄｉｇｉｔａｌ  Ｖｅｒｓａｔｉｌｅ  Ｄｉｓｋ）、ＳＤメモリカード（ＳＤ  ｍｅｍｏｒｙ  ｃａｒｄ）、ＵＳＢメモリ（Ｕｎｉｖｅｒｓａｌ  Ｓｅｒｉａｌ  Ｂｕｓ  ｍｅｍｏｒｙ）などがある。
【００２８】
上記のＶｉｓｕａｌ  ＳＬＡＭ部１２０が行うＶｉｓｕａｌ  ＳＬＡＭ処理は、専用ハードウェアで全て行ってもよいし、一部をハードウェア又はソフトウェアで行ってもよい。なお、全ての処理をソフトウェアで実現することもできるが、組み込み向けに実装する場合には処理速度が遅くなるので現実的ではない。例えば、後述する図２の重複点除去部１５までを専用に開発したハードウェアで行い、それ以降の地図生成部１６、推定部１７をソフトウェアで行うこともできる。この場合、例えば、Ｖｉｓｕａｌ  ＳＬＡＭ処理のソフトウェアはＲＯＭ１０４に保存され、ＣＰＵ１０３がロードすることで処理を実行する。Ｖｉｓｕａｌ  ＳＬＡＭ処理で扱う情報量は多いため、Ｖｉｓｕａｌ  ＳＬＡＭ  Ｈ／Ｗ部１０２、ソフトウェア部は、ＲＡＭ１０５を用いてデータ処理を実行する。
【００２９】
なお、上記の自己位置推定装置１００用プログラムは、インストール可能な形式又は実行可能な形式のファイルで、記録メディア等のコンピュータで読み取り可能な記録媒体に記録して流通させるようにしてもよい。
【００３０】
次に、図２を用いて、自己位置推定装置１００の機能構成について説明する。図２は、自己位置推定装置の機能構成の一例を示すブロック図である。
【００３１】
図２に示すように、自己位置推定装置１００は、画像入力部１１と、特徴点抽出部１２と、特徴点追跡部１３と、物体除去部１４と、重複点除去部１５と、地図生成部１６と、推定部１７と、記憶制御部１８と、地図情報記憶部１９とを備える。また、物体除去部１４と連携する機能部として、移動情報算出部２６を備えている。上記各部については、これらの一部がソフトウェア（プログラム）であってもよいし、これらの一部又は全部がハードウェア回路であってもよい。
【００３２】
画像入力部１１は、移動中の画像を時系列に入力する。画像入力部１１は、例えば、移動中の画像を時系列に入力するデジタルカメラやビデオカメラ等の画像センサを有する撮像装置（カメラ）である。本実施形態において、カメラ１１は、例えば、視差による距離画像が取得可能な複数台のカメラ（ステレオカメラなど）１１である。ステレオカメラは、例えば、シャッタースピード、絞りなどが同期可能で、光軸や取り付け位置などが正確に設計されているステレオカメラである。なお、カメラ１１は、複数台のカメラ１１に限ることはなく、時系列に撮影された複数フレームの画像から距離画像を取得することができる単眼カメラでもよい。また、画像入力部１１は、映像データを構成する一連の画像データを取得できればよく、予め撮影された映像データを取得する装置などであってもよい。なお「画像入力部１１」は、図１の「カメラ１１」に対応する。以下では、画像入力部１１を「カメラ１１」とも表記する。
【００３３】
特徴点抽出部１２は、画像入力部１１から入力された時系列画像から物体の特徴点を抽出して、特徴点追跡部１３及び物体除去部１４に受け渡す。特徴点抽出部１２は、具体的には、連続して入力される時系列画像から物体（静止物体及び移動物体）を抽出し、抽出した物体の特徴点を抽出する。なお、特徴点は、例えば、建物の角などの画像上の特徴（コーナー）のことを指す。
【００３４】
また、画像から確実といえる特徴点を抽出し画像の中身を推測するために、特徴点を抽出する手法としては、例えば、特徴点をコーナーとしたＭｒａｖｅｃのコーナー検出、Ｈａｒｒｉｓのコーナー検出、ＦＡＳＴ（Ｆｅａｔｕｒｅｓ  ｆｒｏｍ  Ａｃｃｅｌｅｒａｔｅｄ  Ｓｅｇｍｅｎｔ  Ｔｅｓｔ）などが例示される。
【００３５】
また、スケールと特徴点を同時に検出するＤｉｆｆｅｒｅｎｃｅ−ｏｆ−Ｇａｕｓｓｉａｎ（ＤｏＧ）、近似Ｈｅｓｓｉａｎ−Ｌａｐｌａｃｅなどが例示される。また、局所特徴量は局所領域の勾配情報を実数で表現するＳｃａｌｅ−Ｉｎｖａｒｉａｎｔ  Ｆｅａｔｕｒｅ  Ｔｒａｎｓｆｏｒｍ  （ＳＩＦＴ）、Ｓｐｅｅｄｅｄ−Ｕｐ  Ｒｏｂｕｓｔ  Ｆｅａｔｕｒｅｓ（ＳＵＲＦ）などが例示される。また、輝度の大小関係をバイナリで表現するＢｉｎａｒｙ  Ｒｏｂｕｓｔ  Ｉｎｄｅｐｅｎｄｅｎｔ  Ｅｌｅｍｅｎｔａｒｙ  Ｆｅａｔｕｒｅｓ（ＢＲＩＥＦ）、Ｏｒｉｅｎｔｅｄ  ＦＡＳＴ  ａｎｄ  Ｒｏｔａｔｅｄ  ＢＲＩＥＦ（ＯＲＢ）などが例示される。
【００３６】
特徴点追跡部１３は、順に入力された時系列画像で、特徴点抽出部１２で抽出した特徴点を追跡して、その情報を物体除去部１４に受け渡す。
【００３７】
物体除去部１４は、距離算出部２１と、座標算出部２２と、移動ベクトル算出部２３と、移動物体判定部２４と、移動物体除去部２５とを備える。
【００３８】
距離算出部２１は、特徴点抽出部１２で抽出された物体の特徴点について、画像入力部（カメラ）１１から特徴点までの距離を算出する。距離算出部２１は、算出した特徴点の距離を座標算出部２２に受け渡す。なお、本実施形態では、特徴点の抽出を、上記の特徴点抽出部１２の機能が行う構成として説明しているが、これに限ることはなく、距離算出部２１の機能で特徴点の抽出を行うようにすることもできる。この場合、特徴点抽出部１２の構成はなくてもよい。
【００３９】
また、距離算出部２１は、画像入力部１１から同時に入力された２枚以上の画像から、特徴点までの距離を算出する。特徴点までの距離の算出は、画像入力部１１として、例えば、ステレオカメラを用いた場合には、視差を測定することで算出できる。視差の算出は、例えば、同時に入力された２枚の画像から、対象の特徴点が映っている場所を特定して算出する。視差を算出した後、２つのカメラ１１の位置関係（例えば、並進行列、回転行列で表される）を用いて、三角測量の原理で特徴点までの距離を算出する。
【００４０】
また、距離算出部２１は、時系列画像から複数の物体の特徴点を抽出して、画像入力部１１から、複数の特徴点までの距離をそれぞれ算出する。なお、特徴点の抽出は、特徴点抽出部１２で行うようにしてもよい。その場合、距離算出部２１は、特徴点の情報を特徴点抽出部１２から受け取る。
【００４１】
なお、画像入力部（ステレオカメラ）１１に、例えば、視差画像から視差を算出して出力する機能が搭載されている場合、距離算出部２１は算出された視差をそのまま用いることができる。そして、視差画像から特徴点の座標にある画素を抽出し、その座標値を距離として使用する。画像入力部（ステレオカメラ）１１に、視差を算出する機能がない場合、例えば、単に左右のカメラ１１で撮影した２枚の画像を出力するだけの場合は、上述したように特徴点の視差を算出することで特徴点までの距離を算出する。
【００４２】
ここで、時系列画像の視差画像を用いて特徴点までの距離の算出方法について説明する。図３及び図４は、視差画像を用いて特徴点までの距離を算出する方法の一例について説明する図である。図３に示すように、２台のカメラ（ステレオカメラ）１１があるとする。Ｘ座標における左カメラのレンズ中心はｘｌ、右カメラのレンズ中心はｘｒで、カメラ１１間の距離はＤである。（Ｘ，Ｙ，Ｚ）を右カメラの焦点を原点とした実際の空間の座標系、（ｘｌ，ｙｌ）、（ｘｒ，ｙｒ）をそれぞれ左カメラ、右カメラの撮像面上で、それぞれの光軸との交点を原点とした座標系とする。また、Ｘ，ｘｌ，ｘｒ軸はすべて、左カメラの焦点から右カメラの焦点に向かう方向に一致させる。また、図４に示すように、左カメラと右カメラとの視差はＥであるとする。この場合、ｙｌ＝ｙｒとなり、世界座標系における特徴点の３次元座標（Ｘ，Ｙ，Ｚ）とそれぞれのカメラ１１での投影点ｍｌ（ｘｌ，ｙｌ）、ｍｒ（ｘｒ，ｙｒ）は下記式（１）〜（３）により算出できる。
【００４３】
Ｚ＝Ｄｆ／（ｘｌ−ｘｒ）  ・・・（１）
Ｘ＝（Ｚ／ｆ）×ｘｒ      ・・・（２）
Ｙ＝（Ｚ／ｆ）×ｙｒ      ・・・（３）
なお、上記式（１）〜（３）中、Ｄはカメラ間の距離、ｆはカメラの焦点距離、ｘｌ−ｘｒは視差Ｅを表す。
【００４４】
上記式のＤ及びｆは、共に定数であるので、ｘｌ−ｘｒ（視差）を計測すれば、Ｚが求まり、右カメラでの像の位置（ｘｒ，ｙｒ）より、特徴点の世界座標系における３次元座標（Ｘ，Ｙ，Ｚ）が算出できる。このように、複数の画像入力部（カメラ）１１から入力された時系列画像の視差画像から特徴点の世界座標系における３次元座標（３次元点：Ｘ，Ｙ，Ｚ）が算出できる。そして、特徴点の３次元座標を用いてカメラ（画像入力部）１１から特徴点までの距離を算出する。
【００４５】
図２に戻って説明を続ける。座標算出部２２は、画像入力部１１を基準とした座標系を示すカメラ座標系における特徴点の座標と、距離算出部２１で算出した特徴点までの距離とを用いて、世界座標系における特徴点の座標を算出する。
【００４６】
また、座標算出部２２は、複数の画像入力部１１から入力された時系列画像の視差画像から求められる物体の特徴点の視差の逆数を、特徴点までの距離として用いて、世界座標系における特徴点の座標を算出する。
【００４７】
また、座標算出部２２は、カメラ座標系における複数の特徴点の座標と、算出した複数の特徴点までの距離とを用いて、世界座標系における複数の特徴点の座標をそれぞれ算出する。
【００４８】
ここで、図５を用いて、カメラ座標系における特徴点の座標と、距離算出部２１で算出した特徴点までの距離とを用いて、世界座標系における特徴点の座標を算出する方法について説明する。図５は、カメラ座標系における特徴点の座標と、算出した特徴点までの距離とを用いて、世界座標系における特徴点の座標を算出する方法の一例を説明する図である。
【００４９】
図５に示したのは、例えば、ピンホールカメラモデルが適用できるカメラ（魚眼レンズを用いたカメラには適用できない）１１で撮影された画像は、透視投影で、現実の世界の３次元座標（世界座標系）を２次元座標（カメラ座標系）に投影したものになる。そのため、単一の画像では、奥行き情報が失われている。しかし、奥行き情報（カメラ１１から特徴点までの距離情報）が分かれば画像から世界座標系の３次元座標を算出することができる。
【００５０】
図５の例では、カメラ座標系の投影面３１に、特徴点（×印）４１、４２、４３があると仮定する。そこで、投影中心と、各特徴点４１〜４３を通る直線を引いたとき、投影中心から距離算出部２１が算出した距離に、特徴点４１〜４３の世界座標系における３次元座標５１〜５３を算出することができる。
【００５１】
すなわち、物体（特徴点）のカメラ座標系の２次元座標（ｘ，ｙ）を、変換行列を用いて、世界座標系における３次元座標（Ｘ，Ｙ，Ｚ）に変換する。なお、２次元座標から３次元座標に変換するための変換行列は任意の式を適用できる。
【００５２】
図２に戻って説明を続ける。移動ベクトル算出部２３は、画像入力部１１から、順に入力された時系列画像で、特徴点抽出部１２又は距離算出部２１で、抽出した特徴点を追跡して、座標算出部２２で算出された世界座標系における特徴点の座標から特徴点の移動ベクトルを算出する。具体的には、移動ベクトル算出部２３は、特徴点抽出部１２又は距離算出部２１で抽出した物体の特徴点について、例えば、最初に入力された時系列画像（フレーム）を探索画像として順に入力された時系列画像で物体の特徴点の位置を追跡し、物体の特徴点が移動した移動ベクトル（オプティカルフロー）を算出する。なお、オプティカルフローとは、デジタル画像中の物体の動きをベクトルで表したものである。主に移動物体の検出や、その動作の解析などに用いられる。なお、本実施形態では、特徴点の追跡を、上記の特徴点追跡部１３の機能が行う構成として説明しているが、これに限ることはなく、移動ベクトル算出部２３の機能で特徴点の追跡を行うようにすることができる。この場合、特徴点追跡部１３の構成はなくてもよい。
【００５３】
また、移動ベクトル算出部２３は、順に入力された時系列画像において、時刻ｔでの世界座標系における特徴点の座標と、時刻ｔ＋Δｔでの世界座標系における特徴点の座標とから特徴点の移動ベクトルを算出する。
【００５４】
また、移動ベクトル算出部２３は、順に入力された時系列画像で、抽出した複数の特徴点を追跡して、座標算出部２２で算出された世界座標系における複数の特徴点の座標から複数の特徴点の移動ベクトルをそれぞれ算出する。
【００５５】
また、移動ベクトル算出部２３は、特徴点の移動ベクトルを算出すると共に、移動情報を用いて、特徴点の移動ベクトルを予測する。
【００５６】
なお、上記の特徴点抽出部１２、特徴点追跡部１３から受け取る特徴点情報には、例えば、カメラ座標系における画像上の２次元座標（ｘ，ｙ）の他に、特徴点を識別する識別情報（ＩＤ）、特徴点を追跡した追跡回数などの情報が付与されている。すなわち、複数の特徴点は、例えば、識別情報（ＩＤ）により識別されている。なお、追跡回数の一例として、例えば「０」を追跡失敗、「１」を新規な特徴点、「２」以上を追跡成功回数などと定義してもよい。
【００５７】
ここで、図６を用いて、移動ベクトルについて説明する。図６は、移動ベクトルの一例について説明する図である。まず、図６に示した、カメラ座標における投影面３２の特徴点（×印）４１は、時間軸で、時刻ｔの時の座標である。また、投影面３２の特徴点（×印）６１は、時刻ｔ＋Δｔの時の座標である。この場合、特徴点４１から特徴点６１に向かうベクトルがカメラ座標系における移動ベクトル８１となる。また、図５を用いて説明したように、投影中心と、特徴点４１、６１を通る直線を引いたとき、投影中心から距離算出部２１が算出した距離に、特徴点４１、６１の世界座標系における３次元座標５１、７１を算出できる。この場合、世界座標系の特徴点の３次元座標５１から特徴点の３次元座標７１に向かうベクトルが世界座標系における移動ベクトル９１となる。
【００５８】
すなわち、図６に示すように、例えば、カメラ座標系において、時刻ｔである前の画像の特徴点４１が、時刻ｔ＋Δｔである次の画像において別の場所、特徴点６１に動いたとき、その特徴点の移動を表すベクトル（特徴点４１から特徴点６１に向かうベクトル）が、この特徴点の２次元の移動ベクトル８１である。また、世界座標系においても同様に、時刻ｔである前の画像の特徴点の３次元座標５１が、時刻ｔ＋Δｔである次の画像において別の場所、特徴点の３次元座標７１に動いたとき、その特徴点の移動を表すベクトル（特徴点の３次元座標５１から特徴点の３次元座標７１に向かうベクトル）が、この特徴点の３次元の移動ベクトル９１である。なお、時間軸のｔは任意の時刻を表す。
【００５９】
このように、動画像の時間的、そして空間的な例えば、輝度分布の変化を用いて特徴点の画素ごとに移動ベクトルを求めてできた結果をオプティカルフローと呼ぶ。
【００６０】
次に、図７〜図１１を用いて、画像入力部１１から入力される時系列画像と、移動ベクトルについて説明する。図７及び図８は、入力される時系列画像の一例について説明する図である。
【００６１】
図７に示すように、例えば、自動車に搭載されたカメラ（画像入力部）１１が進行方向に向かって進んでいる場合、移動中の画像が時系列に入力される。図７の例では、時刻ｔでの画像と、時刻ｔ＋Δｔでの画像が時系列に入力されることになる。つまり、進行方向に向かって連続画像が入力される。
【００６２】
図８に示すように、例えば、自動車に搭載されたステレオカメラ（画像入力部）１１が直進する場合、図８の上図には時刻ｔにおける左カメラでの画像と右カメラでの画像が示されている。画像の直方体の角（エッジ）の特徴点（×印）の視差は、左カメラと右カメラにおいて、例えば、直方体Ａの視差は１１４（ｒａｄ）であり、直方体Ｂの視差は８６（ｒａｄ）であり、直方体Ｃの視差は５８（ｒａｄ）であり、直方体Ｄの視差は５９、８６（ｒａｄ）であるとする。
【００６３】
また、図８の下図には時刻ｔ＋Δｔにおける左カメラでの画像と右カメラでの画像が示されている。画像の直方体の角（エッジ）の特徴点（×印）の視差は、左カメラと右カメラにおいて、例えば、直方体Ａの視差は１３１（ｒａｄ）であり、直方体Ｂの視差は９３（ｒａｄ）であり、直方体Ｃの視差は６４（ｒａｄ）であり、直方体Ｄの視差は７１、１１３（ｒａｄ）であるとする。なお、上記の視差は、ピクセル精度で算出したものであるが、より精密にするために、サブピクセル精度で視差を算出するようにしてもよい。
【００６４】
図８において、直方体Ａ〜Ｃは地面に対して静止しており、直方体Ｄはステレオカメラ１１に対して接近するように移動している。従って、図８の下図の時刻ｔ＋Δｔでの直方体Ｄは、直方体Ａ〜Ｃと比べてステレオカメラ１１に近接しているので大きく写っている。つまり、ステレオカメラ（画像入力部）１１が移動するので、時刻ｔでの物体の画像は小さく（遠くにある）、時刻ｔ＋Δｔでの物体の画像は大きく（近くにある）なる。
【００６５】
図９〜図１１は、時系列画像から移動ベクトルを算出する方法の一例について説明する図である。
【００６６】
図９は、図８で示した時刻ｔでの右カメラでの画像を示す。説明を簡略化するため、水平方向の動きにのみ着目して説明する。図９において、まず、投影中心の座標を決め、撮影中心からある距離に映像面を決める。次に、映像面に特徴点の座標をマッピングする。具体的には、映像面の特徴点の座標は、直方体Ａ〜Ｄの特徴点（×印）から映像面に垂直に引いた点線が映像面と交わる点である。次に、映像面の特徴点の座標と投影中心を結ぶ直線を引く。そして、その直線と一致するように、投影中心から、視差から計算される距離の長さを持つベクトルを描く。この時、距離は上記の図８で仮定した視差の逆数を用いる。つまり、投影中心から映像面の特徴点の座標に対して描く直線（ベクトル）の長さを、特徴点の視差の逆数とし、ベクトルの長さの位置を特徴点の３次元座標とする。なお、長さの単位は考えない。このようにすると、特徴点の３次元座標を求めることができる。図９の例では、直方体Ａの特徴点のベクトルは１Ａ、直方体Ｂの特徴点のベクトルは１Ｂ、直方体Ｃの特徴点のベクトルは１Ｃ、直方体Ｄのベクトルは１Ｄ、２Ｄとなる。それぞれの特徴点のベクトルの長さ（矢印の先）が各特徴点の３次元座標である。
【００６７】
図１０は、図８で示した時刻ｔ＋Δｔでの右カメラでの画像を示す。図１０において、まず、投影中心の座標を決め、撮影中心からある距離に映像面を決める。次に、映像面に特徴点の座標をマッピングする。次に、図９で説明したものと同様にして投影中心から、視差から計算される距離の長さを持つベクトルを描く。この時、距離は上記の図８で仮定した視差の逆数を用いる。図１０の例では、直方体Ａの特徴点のベクトルは２Ａ、直方体Ｂの特徴点のベクトルは２Ｂ、直方体Ｃの特徴点のベクトルは２Ｃ、直方体Ｄのベクトルは３Ｄ、４Ｄとなる。それぞれの特徴点のベクトルの矢印の先が各特徴点の３次元座標である。
【００６８】
図１１は、図９及び図１０での特徴点のベクトルについて投影中心を合わせて合成したものを示す。すなわち、時系列画像において時刻ｔ、時刻ｔ＋Δｔでの特徴点のベクトルを合成したものである。時刻ｔと時刻ｔ＋Δｔの時の特徴点の３次元座標の差が、それぞれの特徴点の移動ベクトルとなる。図１１において、直方体Ａの特徴点の時刻ｔでのベクトル１Ａと時刻ｔ＋Δｔでのベクトル２Ａの差が、直方体Ａの特徴点の移動ベクトルＡＡである。また、直方体Ｂの特徴点の時刻ｔでのベクトル１Ｂと時刻ｔ＋Δｔでのベクトル２Ｂの差が、直方体Ｂの特徴点の移動ベクトルＢＢである。また、直方体Ｃの特徴点の時刻ｔでのベクトル１Ｃと時刻ｔ＋Δｔでのベクトル２Ｃの差が、直方体Ｃの特徴点の移動ベクトルＣＣである。また、直方体Ｄの特徴点の時刻ｔでのベクトル１Ｄ、２Ｄと時刻ｔ＋Δｔでのベクトル３Ｄ、４Ｄの差が、直方体Ｄの特徴点の移動ベクトル１ＤＤ、２ＤＤである。図８〜図１０を用いて説明したように、移動している直方体Ｄの特徴点のベクトル１ＤＤ、２ＤＤは、静止している直方体Ａ〜Ｃの特徴点の移動ベクトルＡＡ、ＢＢ、ＣＣと比較して、ベクトルの長さが長いことが分かる。よって、静止している物体と、移動している物体とは、移動ベクトルの長さで区別することができる。このようにして、物体の特徴点の移動ベクトルを算出することができる。
【００６９】
なお、説明を簡略化するために、水平成分のみを用いて説明したが、自動車などに本実施形態を適用する場合は、自動車と、街中を移動する他の物体は、ほとんどが地面の上を移動するので、移動ベクトルはすべて水平方向成分しか持っていないと仮定することができる。その場合は、上記で説明したように水平成分のみで移動物体を区別することが可能である。
【００７０】
図２に戻って説明を続ける。移動物体判定部２４は、移動ベクトル算出部２３で算出された移動ベクトルの長さ及び方向に基づいて、特徴点が移動物体であるか否かを判定する。具体的には、上記の図１１を用いて説明したように、静止している物体と、移動している物体とは、移動ベクトルの長さで区別することができるため、移動ベクトルの長さが長い特徴点の物体を移動物体と判定することができる。
【００７１】
また、移動物体判定部２４は、移動ベクトル算出部２３で算出された複数の特徴点の移動ベクトルの長さ及び向きが同一または類似する移動ベクトルを１つのグループに統合し、統合されたそれぞれのグループに所属する移動ベクトルの数が他のグループよりも多いグループを静止物体の移動ベクトルのグループと判定する。なお、「同一または類似する」とは、移動ベクトルの長さの差が所定範囲内であり、移動ベクトルの向きの差が所定範囲内であることを表す。また、所定範囲は任意に設定できる。以下の説明において「同一または類似する」は上記と同様である。
【００７２】
また、移動物体判定部２４は、算出した移動ベクトルと、予測した移動ベクトルとが同一または類似する場合、算出した移動ベクトルの特徴点の物体を静止物体と判定する。
【００７３】
ここで、図１２を用いて、移動ベクトルから移動物体を判定する方法の一例について説明する。図１２は、移動ベクトルから移動物体を判定する方法の一例について説明する図である。図１２の例は、図８〜図１１で説明したように、時系列画像での物体の特徴点の移動ベクトルを算出した一例である。図１２では、判定方法の説明を分かり易くするため、複数の特徴点の移動ベクトルの長さ及び向きが同一または類似する移動ベクトルを領域２０１〜２０３にそれぞれグループ化したものを示している。領域２０１〜２０３には、長さと方向が同一または類似している移動ベクトルが存在している。領域２０２には、領域２０１の移動ベクトルと長さは同一または類似しているが向きが異なる移動ベクトルが存在している。領域２０３には、領域２０１の移動ベクトルと向きは同一または類似しているが長さが異なる移動ベクトルが存在している。従って、領域２０１〜２０３は、特徴点の移動ベクトルの長さ及び向きが同一または類似する移動ベクトルをそれぞれ１つのグループに統合したものである。
【００７４】
まず、領域２０１〜２０３の各グループにおいて、カメラ（画像入力部）１１が直進している場合、静止物体の移動ベクトルは、視線方向に対して水平になるので、それ以外の向きのベクトル（領域２０２）を移動物体と判定して除外する。次に領域２０１、２０３において、上述したように移動ベクトルの長さが長い特徴点の物体を移動物体と判定して、領域２０３のベクトルを除外する。最後に残った領域２０１のベクトルが静止物体となる。なお、静止物体の移動ベクトルのグループ（領域２０１）は、長さ及び向きが同一または類似し、移動ベクトルの数が他のグループ（領域２０２、２０３）よりも多いグループになる。
【００７５】
移動ベクトルの長さによる除外は、例えば、ベクトル長さのヒストグラムを作成し、最もメジャーな長さのベクトル以外を除外するようにする。また、外部から与えられる車速情報から、静止物体の予測される移動ベクトルを計算して、予測ベクトルを基準として予測ベクトルと同一または類似しているベクトルを静止物体のベクトルと判定するようにしてもよい。また、カメラ（画像入力部）１１が移動する移動速度、及び回転情報が取得できる場合、静止物の移動量を予測することができるので、予測した静止物の移動量を基準にして静止物体の特徴点を判定することもできる。この場合、静止している物体の特徴点の数が少なくても、静止物のみを抽出することができる。
【００７６】
図２に戻って説明を続ける。移動物体除去部２５は、移動物体判定部２４で特徴点が移動物体であると判定した場合、特徴点に対応する物体を時系列画像から除去する。具体的には、上述したように、移動物体判定部２４では、物体の特徴点の移動ベクトルの長さ及び向きによって、移動物体か静止物体かを判定することができる。移動物体除去部２５は、移動物体判定部２４での判定結果に基づいて、画像入力部（カメラ）１１から入力された時系列画像から移動物体を除去する。すなわち、例えば、人や車などの移動物体の世界座標系における３次元座標（３次元点）情報を含まない初期の地図を生成することができる。
【００７７】
また、移動物体除去部２５は、移動物体判定部２４で判定された静止物体の移動ベクトルのグループを除く他のグループに所属する移動ベクトルの特徴点の物体を時系列画像から除去する。具体的には、まず、移動物体判定部２４は、移動ベクトル算出部２３で算出された複数の特徴点の移動ベクトルの長さ及び向きが同一または類似する移動ベクトルを１つのグループに統合し、統合されたそれぞれのグループに所属する移動ベクトルの数が他のグループよりも多いグループを静止物体の移動ベクトルのグループと判定する。そして、移動物体除去部２５は、移動物体判定部２４で判定された静止物体の移動ベクトルのグループを除く他のグループに所属する移動ベクトルの特徴点の物体を時系列画像から除去する。
【００７８】
また、移動物体除去部２５は、移動物体判定部２４で判定された静止物体の移動ベクトルを除く他の移動ベクトルの特徴点の物体を時系列画像から除去する。具体的には、まず、移動情報算出部２６で画像入力部（カメラ）１１が移動する速度及び車輪の操舵角を少なくとも用いて、画像入力部（カメラ）１１の移動速度と移動方向とを含む移動情報を算出する。そして、移動ベクトル算出部２３は、特徴点の移動ベクトルを算出すると共に、移動情報を用いて、特徴点の移動ベクトルを予測する。そして、移動物体判定部２４は、算出した移動ベクトルと、予測した移動ベクトルとが同一または類似する場合、算出した移動ベクトルの特徴点の物体を静止物体と判定する。そして、移動物体除去部２５は、移動物体判定部２４で判定された静止物体の移動ベクトルを除く他の移動ベクトルの特徴点の物体を時系列画像から除去する。
【００７９】
次に、物体除去部１４と連携する移動情報算出部２６について先に説明する。移動情報算出部２６は、画像入力部１１が移動する速度及び車輪の操舵角（ハンドル切れ角）を少なくとも用いて、画像入力部１１の移動速度と移動方向とを含む移動情報を算出する。具体的には、移動情報算出部２６は、車載ネットワークの基幹プロトコルとなっているＣＡＮ（Ｃｏｎｔｒｏｌｌｅｒ  Ａｒｅａ  Ｎｅｔｗｏｒｋ）などから得られた車両の速度情報及びタイヤの操舵角情報を少なくとも用いて、車両の移動速度と移動方向を算出する。例えば、車両に搭載されたカメラ１１が直進している場合は、静止物体の移動ベクトルは撮影された画像面に対して垂直になるが、カーブを走行中や、方向転換を行っている場合は、カメラ１１の回転量に応じて、撮影された画像面から傾く。この傾きを、車両（カメラ１１）自体の移動情報から計算することで、静止物体の移動ベクトルの方向を予測して静止物体の判定に利用することができる。つまり、時刻ｔから、時刻ｔ＋Δｔまでの間に、車両が回転した量が、移動ベクトルの撮影された画面に対する傾きと一致する。車両（カメラ１１）自体の移動情報を用いることで、静止物体及び移動物体の判定精度を向上させることができる。なお、車両に加速度計や、角速度計などが設置されており、他のデバイスでも利用できるように車載ネットワークに情報が出力されている場合は、その情報を利用することができる。
【００８０】
ここで、図１３〜図１５を用いて、カメラ（画像入力部）１１の移動速度と移動方向について説明する。図１３〜図１５は、カメラの移動量について説明する図である。
【００８１】
図１３では、カメラ１１と特徴点（×印）を鉛直上方から見た図を示している。三角形がカメラ１１を表している。三角形の下の頂点が、投影中心であり、対向する逆の辺が映像面である。図１３の例では、投影中心と、映像面と、追跡する特徴点の位置との関係を示している。以下、図１３に示した関係を基に説明する。
【００８２】
図１４では、カメラ１１が直進する場合の移動ベクトルを説明する。図１４は、時刻ｔ、時刻ｔ＋Δｔの時のカメラ１１の位置と特徴点の位置との関係を示している。図１４の左図において、時刻ｔの時の投影中心と特徴点（×印）を結んだ直線と、時刻ｔ＋Δｔの時の投影中心と特徴点（×印）を結んだ直線（点線）とを求める。次に、図１４の右図において、それぞれの三角形の投影中心が重なるように、移動させると、時刻ｔから時刻ｔ＋Δｔにおける特徴点の移動ベクトルを図示することができる。
【００８３】
このように、カメラ１１が直進している場合は、静止物体の移動ベクトルは、カメラ１１の移動量と一致する。よって、カメラ１１の移動量から、静止物体の移動ベクトルの大きさを予測することができる。
【００８４】
図１５では、カメラ１１が方向転換やカーブ走行中の時の移動ベクトルを説明する。図１５の左図において、時刻ｔの時の投影中心と特徴点（×印）を結んだ直線と、時刻ｔ＋Δｔの時の投影中心と特徴点（×印）を結んだ直線（点線）とを求める。次に、図１５の右図において、それぞれの三角形の投影中心が重なるように、回転して移動させると、時刻ｔから時刻ｔ＋Δｔにおける特徴点の移動ベクトルを図示することができる。この場合、移動ベクトルには、並進成分と回転成分が含まれることになる。なお、並進成分と回転成分は、移動速度及び車輪の操舵角（ハンドル切れ角）から算出することができるので、図１４で説明した直進する場合と同様に、静止物体の移動ベクトルの大きさを予測することができる。
【００８５】
ここで、並進成分と回転成分は、下記式（４）〜（７）で算出することができる。
回頭速度ωは、
ω＝ｖ×ｓｉｎ（ｓ）／Ｗ  ・・・（４）
【００８６】
また、時刻ｔの時の自己位置を原点とした場合、時刻ｔ＋Δｔの位置（ｘ，ｙ）と回転角（θ）は、
ｘｔ＋Δｔ＝（ｖ／ω）×ｓｉｎ（ωｄｔ）  ・・・（５）
ｙｔ＋Δｔ＝（ｖ／ω）−（ｖ／ω）×ｃｏｓ（ωｄｔ）  ・・・（６）
θｔ＋Δｔ＝ωｄｔ  ・・・（７）
上記式（４）〜（７）中、ωは回頭速度（ヨーレート）（ｒａｄ／ｓ）、ｖは速度（ｍ／ｓ）、ｓはステアリング角度（ｒａｄ）、Ｗは前後のタイヤの距離（ｍ）、ｔ＋Δｔは時刻を表す。
【００８７】
図２に戻って説明を続ける。重複点除去部１５は、特徴点抽出部１２又は距離算出部２１で新たに抽出した物体と、特徴点追跡部１３で追跡している物体とが同じ物体であるか否かの対応付けを行う。
【００８８】
また、重複点除去部１５は、特徴点抽出部１２又は距離算出部２１で新たに抽出した物体と、特徴点追跡部１３で追跡している物体とが同じ物体であると判定した場合に、新たに抽出した物体を除去して物体の追跡を中止する。すなわち、新たに抽出した物体と、現在追跡している物体とが同じである場合には重複している新たに抽出した物体を除去して、移動ベクトルを算出するための追跡を止める。
【００８９】
地図生成部１６は、移動物体除去部２５で除去されなかった物体の世界座標系における座標を含む地図情報を生成する。具体的には、時系列画像において、移動物体判定部２４で特徴点が移動物体であると判定された場合、移動物体除去部２５で特徴点に対応する物体を時系列画像から除去する。地図生成部１６は、時系列画像から移動物体が除去された後の静止物体のみの画像データを受け取り、静止物体の世界座標系における３次元座標（３次元点）情報だけを含んだ初期の地図を生成する。
【００９０】
また、地図生成部１６は、予め設定された間隔で初期の地図を更新する。つまり、地図生成部１６は、予め設定された間隔で自己位置を推定するための基準となる静止物体の３次元座標情報だけを含んだ環境地図を任意のタイミングで初期化する。すなわち、地図生成部１６は、予め設定された間隔で地図情報記憶部１９に記憶されている物体の地図情報（世界座標系における物体の座標）を初期化して地図情報を更新する。
【００９１】
地図生成部１６が更新した地図情報は、移動物体を除去した自己位置を推定するための基準となる初期の地図情報である。従って、地図生成部１６は、時系列画像から移動物体を除去した、静止物体（ランドマーク）の世界座標系における物体の座標の情報を含んだ、自己位置推定に適用可能な初期の地図情報を生成することができる。
【００９２】
推定部１７は、地図生成部１６で生成した地図情報に基づいて自己位置を推定する。また、推定部１７は、世界座標系における物体の座標（特徴点の３次元座標）に基づいて、世界座標系からカメラ（画像入力部）１１を基準とした座標系を示すカメラ座標系への変換行列を推定し、該変換行列に基づいて画像を入力したカメラ（画像入力部）１１の物体（特徴点）に対する位置及び姿勢を示す回転角を推定する。
【００９３】
つまり、推定部１７は、物体に対するカメラ（画像入力部）１１の位置及び向きを算出する。具体的には、推定部１７は、世界座標系における物体の位置座標を、カメラ座標系へ投影するための変換を行う変換パラメータ［Ｒ｜ｔ］行列を算出する。なお、Ｒは回転行列のことを表し、ｔは並進ベクトルのことを表す。
【００９４】
すなわち、推定部１７は、地図情報記憶部１９に記憶されている物体（特徴点）の世界座標系における座標（Ｘ，Ｙ，Ｚ）と、時系列画像における物体（特徴点）のカメラ座標系での座標（ｘ，ｙ）とを用いて、世界座標系をカメラ座標系へと線形変換するための変換パラメータ［Ｒ｜ｔ］行列を算出する。なお、３次元座標から２次元座標に変換するための変換パラメータ［Ｒ｜ｔ］行列は任意の式を適用できる。
【００９５】
記憶制御部１８は、移動物体除去部２５、及び重複点除去部１５で除去されなかった物体の世界座標系における座標を含む地図情報を地図情報記憶部１９に記憶させる制御を行う。すなわち、地図生成部１６で生成した初期の地図情報を地図情報記憶部１９に記憶させる。また、記憶制御部１８は、地図生成部１６で地図情報が更新された場合、更新後の地図情報を地図情報記憶部１９に記憶させる制御を行う。なお、記憶制御部１８は、地図情報を地図データベースとして記憶させるようにしてもよい。さらに、地図データベースは、地図情報記憶部１９が備えることに限ることはなく、例えば、通信可能な外部装置（サーバ装置）に備えるように構成することもできる。
【００９６】
また、記憶制御部１８は、現在の地図情報だけではなく過去に走行した際の地図情報を記憶させておくことで、例えば、雨や雪、夜間など不利な環境下においてもロバスト性を向上させることができる。
【００９７】
地図情報記憶部１９は、物体の世界座標系における座標を含む地図情報を記憶する。また、地図情報記憶部１９は、特徴点抽出部１２又は距離算出部２１で時系列画像から抽出された物体で、移動物体除去部２５で除去されなかった物体の世界座標系における座標を含む地図情報を記憶する。すなわち、地図情報記憶部１９は、記憶制御部１８の制御の下、地図生成部１６で生成した初期の地図情報を記憶する。また、地図情報記憶部１９は、記憶制御部１８の制御の下、地図生成部１６で地図情報が更新された場合、更新後の地図情報を記憶する。
【００９８】
次に、図１６を用いて、本実施形態の自己位置推定装置１００の処理動作について説明する。図１６は、本実施形態の自己位置推定装置の処理動作の一例について説明するフローチャート図である。
【００９９】
まず、自己位置推定装置１００の特徴点抽出部１２又は距離算出部２１は、画像入力部（カメラ）１１から入力された時系列画像から物体の特徴点（ｘ，ｙ）を抽出して（ステップＳ１）、距離算出部２１は、画像入力部（カメラ）１１から特徴点までの距離を算出する（ステップＳ２）。座標算出部２２は、画像入力部（カメラ）１１を基準とした座標系を示すカメラ座標系における特徴点の座標と、算出した特徴点までの距離とを用いて、世界座標系における特徴点の３次元座標（Ｘ，Ｙ，Ｚ）を算出する（ステップＳ３）。移動ベクトル算出部２３は、画像入力部（カメラ）１１から順に入力された時系列画像で、抽出した特徴点を追跡して、座標算出部２２で算出された世界座標系における特徴点の座標から特徴点の移動ベクトルを算出する（ステップＳ４）。ここで、最初に入力される時系列画像（フレーム）では、追跡対象の物体（特徴点）の情報が無いので、次に入力される時系列画像（２フレーム）以降に特徴点の移動ベクトルの算出処理が開始される。
【０１００】
次に、移動物体判定部２４は、移動ベクトル算出部２３で算出された移動ベクトルの長さ及び方向に基づいて、特徴点が移動物体であるか否かを判定する（ステップＳ５）。移動物体でない場合（ステップＳ５：Ｎｏ）、ステップＳ７に移行して処理を継続する。移動物体である場合（ステップＳ５：Ｙｅｓ）、移動物体除去部２５は、特徴点に対応する物体を時系列画像から除去する（ステップＳ６）。
【０１０１】
次に、重複点除去部１５は、特徴点抽出部１２又は距離算出部２１で新たに抽出した特徴点（物体）と、特徴点追跡部１３又は移動ベクトル算出部２３で追跡している特徴点（物体）との対応付けを行って、特徴点（物体）が同じ特徴点（物体）であるか否かを判定する（ステップＳ７）。同じ特徴点（物体）でないと判定した場合（ステップＳ７：Ｎｏ）、ステップＳ９に移行して処理を継続する。同じ特徴点（物体）であると判定した場合（ステップＳ７：Ｙｅｓ）、重複している新たに抽出した特徴点に対応する物体を時系列画像から除去する（ステップＳ８）。すなわち、新たに抽出した特徴点（物体）と、追跡している特徴点（物体）とが重複している場合、特徴点（物体）の移動ベクトルの算出を中止することで、自己位置推定装置１００の処理の負荷を軽減させることができる。
【０１０２】
次に、地図生成部１６は、移動物体除去部２５で除去されなかった特徴点に対応する物体の世界座標系における座標を含む地図情報を生成して、記憶制御部１８の制御の下で地図情報記憶部１９に記憶させる（ステップＳ９）。
【０１０３】
次に、推定部１７は、地図生成部１６で生成した地図情報に基づいて、自己位置を推定する（ステップＳ１０）。すなわち、推定部１７は、世界座標系における特徴点（物体）の３次元座標（Ｘ，Ｙ，Ｚ）と、カメラ（画像入力部）１１を基準とした座標系を示すカメラ座標系の物体（特徴点）の座標（ｘ，ｙ）から、画像を入力したカメラ（画像入力部）１１の特徴点（物体）に対する位置及び姿勢を示す回転角を推定する。その後、処理を終了する。
【０１０４】
次に、図１７を用いて、画像入力部（カメラ）１１の移動情報を用いた処理動作について説明する。図１７は、本実施形態の自己位置推定装置の他の処理動作の一例について説明するフローチャート図である。なお、図１６を用いて説明したものと同様の処理については、同じ符号を付し詳細な説明を省略する。具体的には、ステップＳ１〜ステップＳ４、ステップＳ７〜ステップＳ１０は図１６の処理と同様であるので、ステップＳ２１〜ステップＳ２６について説明する。
【０１０５】
まず、移動ベクトル算出部２３は、特徴点の移動ベクトルを算出する（ステップＳ４）。次に、移動情報算出部２６は、画像入力部（カメラ）１１が移動する速度及び車輪の操舵角を少なくとも用いて、画像入力部（カメラ）１１の移動速度と移動方向とを含む移動情報を算出する（ステップＳ２１）。次に、移動ベクトル算出部２３は、特徴点の移動ベクトルを算出すると共に、移動情報を用いて、特徴点の移動ベクトルを予測する（ステップＳ２２）。次に、移動物体判定部２４は、算出した移動ベクトルと、予測した移動ベクトルとが同一または類似するか否かを判定する（ステップＳ２３）。同一または類似しないと判定した場合（ステップＳ２３：Ｎｏ）、移動物体除去部２５は、算出した移動ベクトルの特徴点の物体を時系列画像から除去し（ステップＳ２６）、ステップＳ７に移行して処理を継続する。
【０１０６】
同一または類似すると判定した場合（ステップ２３：Ｙｅｓ）、移動物体判定部２４は、算出した移動ベクトルの特徴点の物体を静止物体と判定し（ステップＳ２４）、移動物体除去部２５は、判定された静止物体の移動ベクトルを除く他の移動ベクトルの特徴点の物体を時系列画像から除去する（ステップＳ２５）。その後、ステップＳ７〜ステップＳ１０の処理を行って処理を終了する。
【０１０７】
以上に説明したように、本実施形態では、自己位置推定装置１００に上記の処理を実行させることで、例えば、画像入力部（カメラ）１１から順に入力された時系列画像から物体の特徴点を抽出して、抽出した特徴点を追跡して移動ベクトルを算出する。そして、算出された特徴点の移動ベクトルの長さ及び方向に基づいて、特徴点が移動物体であるか否かを判定し、特徴点が移動物体であると判定した場合、特徴点に対応する物体を時系列画像から除去し、除去されなかった特徴点の物体の世界座標系における座標を含む地図情報を生成し、生成した地図情報に基づいて自己位置を推定する。これにより、本実施形態によれば、自己位置の推定に用いる初期の環境地図を生成して精度良く自己位置を推定することができる、という有利な効果を達成できる。
【０１０８】
以上、本発明に係る実施形態について説明したが、本発明は、上述の実施形態そのままに限定されるものではなく、実施段階ではその要旨を逸脱しない範囲で構成要素を変形して具体化できる。また、上述の実施形態に開示されている複数の構成要素の適宜な組み合わせにより、種々の発明を形成できる。例えば、実施の形態に示される全構成要素から幾つかの構成要素を削除してもよい。さらに、異なる実施形態にわたる構成要素を適宜組み合わせてもよい。
【０１０９】
また、上述した各実施形態の自己位置推定装置１００で実行されるプログラムは、インストール可能な形式または実行可能な形式のファイルでフロッピー（登録商標）ディスク、ＣＤ（Ｃｏｍｐａｃｔ  Ｄｉｓｋ）、ＣＤ−Ｒ（Ｃｏｍｐａｃｔ  Ｄｉｓｋ−Ｒｅｃｏｒｄａｂｌｅ）、ＣＤ−ＲＯＭ（Ｃｏｍｐａｃｔ  Ｄｉｓｃ  Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ）、ＤＶＤ（Ｄｉｇｉｔａｌ  Ｖｅｒｓａｔｉｌｅ  Ｄｉｓｋ）、ＳＤメモリカード（ＳＤ  ｍｅｍｏｒｙ  ｃａｒｄ）、ＵＳＢメモリ（Ｕｎｉｖｅｒｓａｌ  Ｓｅｒｉａｌ  Ｂｕｓ  ｍｅｍｏｒｙ）等のコンピュータで読み取り可能な記録媒体に記録して提供するように構成してもよいし、インターネット等のネットワーク経由で提供または配布するように構成してもよい。また、各種プログラムを、ＲＯＭ等に予め組み込んで提供するように構成してもよい。
【０１１０】
なお、本実施形態の自己位置推定装置１００の構成は一例であり、用途や目的に応じて様々な装置構成例があることは言うまでもない。
【符号の説明】
【０１１１】
１１  画像入力部（カメラ）
１２  特徴点抽出部
１３  特徴点追跡部
１４  物体除去部
１５  重複点除去部
１６  地図生成部
１７  推定部
１８  記憶制御部
１９  地図情報記憶部
３１、３２  投影面
４１、４２、４３、６１  特徴点（２次元座標）
５１、５２、５３、７１  ３次元座標
８１、９１  移動ベクトル
１００  自己位置推定装置
１０２  Ｖｉｓｕａｌ  ＳＬＡＭ  Ｈ／Ｗ部
１０３  ＣＰＵ
１０４  ＲＯＭ
１０５  ＲＡＭ
１０６  ＨＤＤ
１０７  表示装置
１０８  入力装置
１０９  インタフェース装置
１１０  ドライブ装置
１１１  記録媒体
１２０  Ｖｉｓｕａｌ  ＳＬＡＭ部
１３０  システムバス
２０１、２０２、２０３  領域
【先行技術文献】
【特許文献】
【０１１２】
【特許文献１】特開２０１４−０９５５５３号公報
【図１】
2016157197.tif 000003
【図２】
2016157197.tif 000004
【図３】
2016157197.tif 000005
【図４】
2016157197.tif 000006
【図５】
2016157197.tif 000007
【図６】
2016157197.tif 000008
【図１１】
2016157197.tif 000009
【図１２】
2016157197.tif 000010
【図１３】
2016157197.tif 000011
【図１４】
2016157197.tif 000012
【図１５】
2016157197.tif 000013
【図１６】
2016157197.tif 000014
【図１７】
2016157197.tif 000015
【図７】
2016157197.tif 000016
【図８】
2016157197.tif 000017
【図９】
2016157197.tif 000018
【図１０】
2016157197.tif 000019