JP6815935B2 - 位置推定装置 - Google Patents

Description

本発明は、ロボットや自動車などの移動体の位置を推定する位置推定装置に関する。

従来、ロボットや自動車などの移動体がその周囲の情報を収集し、移動体の現在位置及び走行状態を推定し、移動体の走行を制御する自律走行技術及び運転支援技術が開発されている。移動体の周囲の情報を収集するためには様々なセンサが用いられる。一般に、周囲の情報を計測するためのセンサとしては、カメラなどの撮像装置、レーザセンサ、ミリ波レーダなどがある。移動体の位置を測定するためのセンサとしては、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）またはＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）が用いられる。

自律走行制御においては、移動体に搭載された制御装置が、例えばＩＭＵで算出した移動体の速度または角速度を積算したり、ＧＰＳ測位を用いたりして、移動体自身の位置（自己位置）を推定する。さらに、制御装置は、レーザセンサあるいはカメラなどで周囲の情報を収集し、位置推定の際の基準となる路面ペイントまたは標識のようなランドマーク等を検出する。そして、制御装置が、検出されたランドマークの位置を地図情報と比較することにより、移動体の現在位置を修正する。

また、駐車場やトンネルのような環境では、地図情報とランドマークがなく、ＧＰＳも使えない場合があるため、移動体の周囲に存在する物体との相対位置を推定するオドメトリ法が用いられる。画像でオドメトリ法を行う場合、第一のタイミングで画像を取得し、画像処理技術で取得した画像にある特徴点を抽出し、第二のタイミングで画像を取得し、第一のタイミングの画像で抽出した特徴点を第二のタイミングで取得した画像に追跡し、第一のタイミングと第二のタイミングの画像上での特徴点の移動量を用い、カメラの移動量を推定する。ただし、カメラの移動量を推定するために、最小二乗法や試行錯誤が必要となり、処理負荷が大きい。

ここで、例えば、「ア・ローバスト・ビズアル・オドメトリ・アンド・プレシピス・ディテクション・システム・ユージング・コンスーマー・グレード・モノキュラー・ビジョン（A Robust Visual Odometry and Precipice Detection System Using Consumer-grade Monocular Vision.）」は、画像オドメトリの処理負荷を軽減させるために、単眼カメラで取得した画像の領域を遠方領域と近傍領域とに分けて、三次元の画像オドメトリを行う。遠方領域から抽出した遠方特徴点の画像上での移動量は一定のため、遠方特徴点の移動量と一定のパラメータｋを用い、カメラの方位を推定する。次に、路面は平らだと前提し、近傍領域から抽出した近傍特徴点までの距離をカメラの設置高さと角度で幾何学的に算出し、遠方特徴点で推定した方位と近傍特徴点の移動量とを用いてカメラの移動量を推定する。

ジェーソン・キャンベル他（Jason Campbell et al.）、「ア・ローバスト・ビズアル・オドメトリ・アンド・プレシピス・ディテクション・システム・ユージング・コンスーマー・グレード・モノキュラー・ビジョン（A Robust Visual Odometry and Precipice Detection System Using Consumer-grade Monocular Vision.）」、アイクラー2005（ICRA2005.）

しかし、「ア・ローバスト・ビズアル・オドメトリ・アンド・プレシピス・ディテクション・システム・ユージング・コンスーマー・グレード・モノキュラー・ビジョン（A Robust Visual Odometry and Precipice Detection System Using Consumer-grade Monocular Vision.）」では、画像の遠方領域にある特徴点が、実際に遠方にある遠方特徴点であるとして処理をしているが、画像の遠方領域にある特徴点が必ずしも遠方特徴点であるとは限らない。例えば、走行環境によっては、画像の遠方領域に、近くにある立体物が写る場合があり、こうなると画像上での移動量が一定でなくなるため、一定のパラメータｋで推定した方位では誤差が大きいという問題があった。

本発明の目的は、精度よく移動体の位置を推定する位置推定装置を提供することにある。

本発明は上記の目的を達成するために、移動体の移動量に基づいて該移動体の位置を推定する位置推定装置であって、前記移動体の周囲の画像を取得する画像取得部と、前記画像取得部で取得した画像に基づいて前記移動体の移動量を推定する移動体移動量推定部と、前記移動体移動量推定部で推定した移動量に基づいて前記移動体の位置を推定する移動体位置推定部と、を備え、前記移動体移動量推定部は、第一のタイミングで前記画像取得部によって取得した第一の画像から第一の特徴点を抽出する第一の特徴点抽出部と、前記第一のタイミングより後の第二のタイミングで前記画像取得部によって取得した第二の画像から第二の特徴点を抽出する第二の特徴点抽出部と、前記第一の特徴点を前記第二の特徴点にトラッキングするトラッキング部と、前記トラッキング部でトラッキングした前記第二の特徴点の前記移動体からの距離を算出する距離算出部と、前記距離算出部で算出した前記第二の特徴点の前記移動体からの距離に基づいて、前記移動体の方位を推定する方位推定部と、前記方位推定部で推定した前記移動体の方位に基づいて、前記移動体の移動量を推定する移動量推定部と、を有し、前記方位推定部は、前記第二の特徴点の前記移動体からの距離に応じたパラメータを算出し、該パラメータを用いて前記移動体の方位を推定する、ことを特徴とする。

本発明によれば、精度よく移動体の位置を推定する位置推定装置を提供することができる。

上記した以外の課題、構成及び効果は、以下の実施形態の説明により明らかにされる。

本発明の実施例１に係る位置推定装置１の構成を示すブロック図。 図１に示した画像処理部１４の機能ブロック図。 図１に示した画像処理部１４が行う画像処理手順を示すフローチャート。 従来技術における一定のパラメータｋによる位置推定の原理について説明する図であって、移動体１００と立体物との位置関係を示すとともに立体物を撮像した画像を示す図。 従来技術における一定のパラメータｋによる位置推定の原理について説明する図であって、グラフ３６は特徴点の画像上での移動量と特徴点が抽出された立体物の距離ｄとの関係を表すグラフ、グラフ３８はパラメータｋと特徴点が抽出された立体物の距離ｄとの関係を表すグラフ。 実施例１による、立体物の距離ｄが距離ｄａより近い場合の位置推定方法について説明する図であって、移動体１００が移動量４１の移動をした場合を示す図。 実施例１による、立体物の距離ｄが距離ｄａより近い場合の位置推定方法について説明する図であって、移動体１００が移動量４５の移動をした場合を示す図。 実施例１のΔｘと特徴点への距離を用いた位置推定方法の詳細を説明する図であって、移動体１００と特徴点との位置関係を示すとともに特徴点を撮像した画像を示す図。 実施例１のΔｘと特徴点への距離を用いた位置推定方法の詳細を説明する図であって、グラフ５５はパラメータｋと特徴点の距離との関係を表すグラフ。 実施例１における、複数の特徴点を抽出した場合の処理を説明する図。 実施例１における、単眼カメラとステレオカメラとでの位置推定処理の違いについて説明する図である。

以下、添付図面を参照して本発明の実施例について説明する。なお、添付図面や以下の説明は、本発明の原理に則った具体的な実施例を示しているが、これらは本発明の理解のためのものであり、決して本発明を限定的に解釈するために用いられるものではない。

図１は、本発明の実施例１に係る位置推定装置１の構成を示すブロック図である。

位置推定装置１は、自動車やロボットなどの移動体１００に搭載されている。位置推定装置１は、一台以上の撮像装置１２（図１では、撮像装置１２ａ、１２ｂ、・・・１２ｎ）と、情報処理装置１３と、を有する。なお、以下において、撮像装置１２と表記した場合は、撮像装置１２ａ、１２ｂ、・・・１２ｎのそれぞれのことを指す。本実施例では、撮像装置１２によって撮像された画像に基づいて移動体１００の位置を推定するが、本発明はこれに限られるものではなく、例えば、移動体１００とは別の移動体や静止体によって移動体１００の周囲の画像を取得し、この画像に基づいて移動体１００の位置を推定するものであってもよい。

撮像装置１２は、例えばスチルカメラまたはビデオカメラでよい。撮像装置１２は単眼カメラ（一つの撮像部を有するカメラ）またはステレオカメラ（複数の撮像部を有して視差画像を撮像可能なカメラ）でもよい。

情報処理装置１３は、撮像装置１２で撮像された画像を処理して移動体１００の位置または移動量を算出する。情報処理装置１３は、算出された位置または移動量に応じた表示を行ってもよく、または移動体１００の制御に関する信号を出力してもよい。

情報処理装置１３は、例えば一般的なコンピュータであって、撮像装置１２によって撮像された画像を処理する画像処理部１４と、画像処理部１４による画像処理結果に基づく処理を行う制御部（ＣＰＵ）１５と、メモリ１６と、ディスプレイなどの表示部１７と、これら構成要素を相互に接続するバス１８と、を有する。情報処理装置１３は、画像処理部１４及び制御部１５が所定のコンピュータプログラムを実行することにより、以下の処理を行ってもよい。

撮像装置１２ａは、例えば、移動体１００の前方に設置されている。撮像装置１２ａのレンズは移動体１００の前方に向けられている。撮像装置１２ａは、例えば、移動体１００の前方の遠景を撮像する。他の撮像装置１２ｂ、・・・撮像装置１２ｎは、撮像装置１２ａと異なる位置に設置され、撮像装置１２ａと異なる撮像方向または領域を撮像する。撮像装置１２ｂは、例えば、移動体１００の後方で下方に向けて設置されていてもよい。撮像装置１２ｂは、移動体１００の後方の近景を撮像するものでよい。

撮像装置１２が単眼カメラの場合、路面が平らであれば、撮像画像上のピクセル位置と実際の位置関係（ｘ，ｙ）とが一定になるため、撮像装置１２による撮像画像に基づき、移動体から特徴点までの距離を幾何学的に計算できる。撮像装置１２がステレオカメラの場合、画像上の特徴点までの距離をより正確に計測できる。以下の説明では、単眼の標準レンズを有するカメラを採用した事例について説明するが、本発明はこれ以外のカメラ（広角レンズを有するカメラまたはステレオカメラなど）であってもよい。

また、撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎが、ある時刻で撮像する対象物は、それぞれ互いに異なるものでよい。例えば、撮像装置１２ａは、移動体１００の前方の遠景を撮像するものでよい。この場合、遠景を撮像した画像からは、障害物、または位置推定のためのランドマークなどの特徴点が抽出されるようにしてもよい。撮像装置１２ｂは、移動体１００の周辺の路面などの近景を撮像するようにしてもよい。この場合、近景を撮像した画像からは、移動体１００の周囲の白線、または路面ペイントなどの特徴点が検出されるようにしてもよい。

また、撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎは、同時に雨や日差しなどの環境外乱の影響を受けないような条件で移動体１００に設置されてもよい。例えば、撮像装置１２ａは移動体１００の前方で前向きに設置されるのに対して、撮像装置１２ｂは移動体１００の後方で後ろ向きまたは下向きに設置されてもよい。これにより、例えば、降雨時に撮像装置１２ａのレンズに雨滴が付着した場合でも、進行方向の逆向きまたは下向きの撮像装置１２ｂのレンズには雨滴が付着しにくい。このため、撮像装置１２ａが撮像した画像が雨滴の影響で不鮮明であっても、撮像装置１２ｂが撮像した画像は雨滴の影響を受けにくい。あるいは、日差しの影響で撮像装置１２ａの画像が不鮮明であっても、撮像装置１２ｂが撮像した画像は鮮明である場合がある。

また、撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎは、互いに異なる撮像条件（絞り値、ホワイトバランス、等）で撮影してもよい。例えば、明るい場所用にパラメータを調整した撮像装置と、暗い場所用にパラメータを調整した撮像装置とを搭載することで、環境の明暗によらず撮像可能としてもよい。

撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎは、制御部１５から撮像開始の指令を受けたとき、または一定の時間間隔で画像を撮像してもよい。撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎによって撮像された画像の画像データ及び撮像時刻は、メモリ１６に格納される。

メモリ１６は、情報処理装置１３の主記憶装置（メインメモリ）及びストレージなどの補助記憶装置を含む。

画像処理部１４は、メモリ１６に格納された画像データ及び撮像時刻に基づいて、様々な画像処理を行う。この画像処理では、例えば、中間画像が作成されてメモリ１６に保存される。中間画像は、画像処理部１４による処理の他、制御部１５などの判断や処理に利用されてもよい。

バス１８は、ＩＥＢＵＳ（Ｉｎｔｅｒ Ｅｑｕｉｐｍｅｎｔ Ｂｕｓ）やＬＩＮ（Ｌｏｃａｌ Ｉｎｔｅｒｃｏｎｎｅｃｔ Ｎｅｔｗｏｒｋ）やＣＡＮ（Ｃｏｎｔｒｏｌｌｅｒ Ａｒｅａ Ｎｅｔｗｏｒｋ）などで構成できる。

画像処理部１４は、撮像装置１２で撮像された画像に基づいて移動体１００の複数の位置候補を特定し、その複数の位置候補と移動体１００の移動速度とに基づいて移動体１００の位置を推定する。

画像処理部１４は、例えば、移動体１００の走行中に撮像装置１２が撮像した画像を処理して、移動体１００の位置を推定する。例えば、画像処理部１４は、撮像装置１２が撮像したビデオ画像で移動体１００の移動量を算出し、スタート地点に移動量を加算して現在位置を推定してもよい。

画像処理部１４は、ビデオ画像の各フレーム画像で特徴点を抽出してもよい。画像処理部１４は、さらに、次以降のフレーム画像で同じ特徴点を抽出する。そして、画像処理部１４は、特徴点のトラッキング（追跡）により移動体１００の移動量を算出してもよい。

制御部１５は、画像処理部１４の画像処理の結果に基づいて、移動体１００に対して移動速度に関する指令を出力してもよい。例えば、制御部１５は、画像内の障害物の画素数、画像内の特徴点のうちの外れ値の数または画像処理の種類等に応じて、移動体１００の移動速度を増加させる指令、減少させる指令または維持させる指令を出力してもよい。

図２は、図１に示した画像処理部１４の機能ブロック図である。

画像処理部１４は、撮像装置１２が撮像した画像に基づいて移動体１００の移動量を推定する移動体移動量推定部１０１と、移動体移動量推定部１０１で推定した移動量に基づいて移動体１００の位置を推定する移動体位置推定部１０２と、を備えている。

また、詳しくは後述するように、移動体移動量推定部１０１は、第一のタイミングで撮像装置１２によって撮像した第一の画像から第一の特徴点を抽出する第一の特徴点抽出部１０３と、第一のタイミングより後の第二のタイミングで撮像装置１２によって撮像した第二の画像から第二の特徴点を抽出する第二の特徴点抽出部１０４と、第一の特徴点を第二の特徴点にトラッキングするトラッキング部１０５と、トラッキング部１０５でトラッキングした第二の特徴点の移動体１００からの距離を算出する距離算出部１０６と、距離算出部１０６で算出した第二の特徴点の移動体１００からの距離に基づいて、移動体１００の方位を推定する方位推定部１０７と、方位推定部１０７で推定した移動体１００の方位に基づいて、移動体１００の移動量を推定する移動量推定部１０８と、を有する。

また、詳しくは後述するように、移動体移動量推定部１０１は、複数の第一の特徴点の各々及び複数の第二の特徴点の各々が、移動体１００とは別の、他の移動体（例えば、図７に示す移動体６３）から抽出した特徴点であるか否かを検出する検出部１０９を有し、方位推定部１０７は、検出部１０９で他の移動体６３から抽出した特徴点であると検出された特徴点を、複数の第一の特徴点、及び複数の第二の特徴点から削除する。

図３は、図１に示した画像処理部１４が行う画像処理手順を示すフローチャートである。

画像処理部１４は、撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎが撮像した画像データをメモリ１６から取得する（ステップＳ２１）。ステップＳ２１で取得する画像データは撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎが撮像した画像のうちの一枚のみ、または、複数の画像データでよい。また、ステップＳ２１は、撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎがそれぞれ撮像した最新の画像のみならず、過去撮像した画像でもよい。以下では、時系列で撮像したフレーム画像を取得した場合について説明する。

次に、画像処理部１４の第一の特徴点抽出部１０３及び第二の特徴点抽出部１０４は、取得した各フレーム画像内の特徴点を抽出する（ステップＳ２２）。特徴点は、例えば、画像内のエッジやコーナーやピクセル強度の最大値や最小値などでよい。特徴点の抽出には、Ｃａｎｎｙ、Ｓｏｂｅｌ、ＦＡＳＴ、Ｈｅｓｓｉａｎ、Ｇａｕｓｓｉａｎなどの技術を用いてもよい。具体的なアルゴリズムは、画像の特徴に応じて適宜選択される。

画像処理部１４のトラッキング部１０５は、フレーム画像の時系列に従って、各フレーム画像でそれぞれ抽出された特徴点をトラッキング（追跡）する（ステップＳ２３）。トラッキングにはＬｕｃａｓ−Ｋａｎａｄｅ法やＳｈｉ−Ｔｏｍａｓｉ法やＤｉｒｅｃｔ Ｍａｔｃｈｉｎｇ法などの技術を用いてもよい。また、ステップＳ２３のトラッキングは、直前や直後取得したフレームの特徴点に限らず、数フレーム間隔でもよい。具体的なアルゴリズムは、画像の特徴に応じて適宜選択される。

次に、画像処理部１４の距離算出部１０６は、ステップＳ２３でトラッキングした特徴点のそれぞれについて、特徴点までの距離（移動体１００から特徴点までの距離。以下、単に「特徴点までの距離」、「特徴点の距離」または「特徴点への距離」と記載したときは、移動体１００から特徴点までの距離を指す。）を算出する（ステップＳ２４）。撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎの設置高さと角度を用いて、幾何学的に特徴点の距離を算出してもよい。また、撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎがステレオカメラの場合、視差画像を用いて距離を算出してもよい。また、撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎは、単眼カメラやステレオカメラの組み合わせで構成してもよい。最終的に、画像に写る特徴点への距離を算出できる装置であれば、どのカメラや組み合わせでもよい。

次に、画像処理部１４の方位推定部１０７は、ステップＳ２４で算出した距離に基づいて移動体１００の方位を推定するためのパラメータｋを算出する（ステップＳ２５）。パラメータｋの詳細は後述する。

次に、画像処理部１４の移動量推定部１０８は、は、特徴点トラッキングの結果に基づいて、移動体１００の移動量を推定する（ステップＳ２６）。移動体１００の移動量推定の詳細は後述する。

次に、画像処理部１４は、前述ステップＳ２１からステップＳ２６で取得した画像、特徴点、特徴点への距離（移動体１００から特徴点までの距離）、推定した移動量などの情報を表示部１７に表示する（ステップＳ２７）。いずれかの情報のみを表示してもよいし、すべての情報を表示してもよい。

ここで、図４Ａ及び図４Ｂを用いて、従来技術である「ア・ローバスト・ビズアル・オドメトリ・アンド・プレシピス・ディテクション・システム・ユージング・コンスーマー・グレード・モノキュラー・ビジョン（A Robust Visual Odometry and Precipice Detection System Using Consumer-grade Monocular Vision.）」における一定のパラメータｋによる位置推定の原理について説明する。

図４Ａは、従来技術における一定のパラメータｋによる位置推定の原理について説明する図であって、移動体１００と立体物との位置関係を示すとともに立体物を撮像した画像を示す図である。

図４Ｂは、従来技術における一定のパラメータｋによる位置推定の原理について説明する図であって、グラフ３６は特徴点の画像上での移動量と特徴点が抽出された立体物の距離ｄとの関係を表すグラフであり、グラフ３８はパラメータｋと特徴点が抽出された立体物の距離ｄとの関係を表すグラフである。

まず、図４Ａを参照しながら説明する。移動量３１は移動体１００が走行した移動量である。ここでは、簡単のため、移動量３１を、移動体１００のｘ方向の変化分（Δｘ）、ｙ方向の変化分（Δｙ）及び方位（ｙａｗ）の変化分（Δｙａｗ）で構成するものとする。

距離３２は移動体１００が移動量３１を走行した後の位置から、遠方にある立体物までの距離である。

フレーム３３は移動体１００が移動量３１を走行した後の撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎが撮像した画像のうちの一枚である。なお、フレーム３３では、見やすさのため、移動体１００が走行前に撮像した立体物の位置を示し、走行による各特徴点の移動方向及び移動量を移動量３５ａ、移動量３５ｂ、移動量３５ｃの矢印で示している。
ステップＳ２１からステップＳ２３で抽出した特徴点３４ａ、特徴点３４ｂ、特徴点３４ｃの画像上での移動量はそれぞれ移動量３５ａ、移動量３５ｂ、移動量３５ｃとなる。これらの特徴点について移動前が第一の特徴点であり、移動後が第二の特徴点である。

移動体１００が移動量３１を走行した後、近傍にある特徴点３４ａの画像上での移動量３５ａはｕ≠０、ｖ≠０となるが、遠方にある特徴点３４ｂ、特徴点３４ｃの移動量３５ｂ、移動量３５ｃはｕ≠０、ｖはほぼ０となる。

また、特徴点３４ａが抽出された立体物（ただし、図４Ａ及び図４Ｂの例では立体物ではなく路面の白線である。）は、特徴点３４ｂと特徴点３４ｃが抽出されて立体物に比べて移動体１００に近いため、画像上での移動量３５ａ＞移動量３５ｂ、移動量３５ａ＞移動量３５ｃである。

従って、遠方の特徴点３４ｂ、特徴点３４ｃの画像上での移動量３５ｂ、移動量３５ｃは移動体１００のΔｙａｗのみに依存し、フレーム３３のｕ方向のみに移動するため、遠方特徴点の画像上での移動量だけで移動体１００のΔｙａｗを推定できる。

一方、近傍の特徴点３４ａの画像上での移動量３５ａは移動体１００のΔｙａｗと（Δｘ，Δｙ）に影響されるため、フレーム３３のｕ方向とｖ方向とに移動する。従って、近傍特徴点の画像上での移動量は、Δｙａｗと（Δｘ，Δｙ）が混ざっているため、それぞれを推定できない。

ここで、特徴点の画像上での移動量と特徴点が抽出された立体物の距離ｄ（移動体１００から立体物までの距離ｄ）との関係を表すグラフ３６を、図４Ｂの上のグラフで示している。グラフ３６において、縦軸は、特徴点の画像上での移動量（Δｕの曲線はｕ方向の移動量、Δｖの曲線はｖ方向の移動量）であり、横軸は、立体物の距離ｄである。

また、パラメータｋと特徴点が抽出された立体物の距離ｄとの関係を表すグラフ３８を、図４Ｂの下のグラフで示している。グラフ３８において、縦軸は、パラメータｋであり、横軸は、立体物の距離ｄである。

グラフ３６からも分かるように、ステップＳ２１からステップＳ２３で得られた特徴点の距離ｄが距離ｄａより近ければ、移動量３１と特徴点への距離ｄによる画像上でのΔｕとΔｖが大きく変わる。一方、ステップＳ２１からステップＳ２３で得られた特徴点の距離ｄが距離ｄａより遠ければ、特徴点への距離ｄに依存せず、画像上でのΔｕとΔｖが一定である。従って、ステップＳ２１からステップ２３で得られた特徴点の距離ｄが距離ｄａより遠い場合、グラフ３８と数１が示すようにΔｕと一定のパラメータｋで移動体１００の方位を推定できる。

一方、ステップＳ２１からステップＳ２３で得られた特徴点の距離ｄが距離ｄａより近ければ、画像上でのΔｕとΔｖが一定でないため、方位を推定する場合、一定のパラメータｋで精度よく方位を推定できない。

距離ｄａは、撮像装置１２ａ、撮像装置１２ｂ・・・撮像装置１２ｎの画質や画角によって異なるが、経験的に距離ｄａを設定する。

図５Ａは、実施例１による、立体物の距離ｄが距離ｄａより近い場合の位置推定方法について説明する図であって、移動体１００が移動量４１の移動をした場合を示す図である。

図５Ｂは、実施例１による、立体物の距離ｄが距離ｄａより近い場合の位置推定方法について説明する図であって、移動体１００が移動量４５の移動をした場合を示す図である。

図５Ａの移動量４１は移動体１００が走行した移動量である。簡単のため、移動体１００の移動量４１のΔｙａｗ、Δｘ、ΔｙをΔｙａｗ＝０、Δｘ＝０、Δｙ≠０とする。

画像４２ａは移動体１００が移動量４１を走行した後の撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎが撮像した画像のうちの一枚を表す。なお、画像４２ａでは、見やすさのため、移動体１００が走行前に撮像した立体物の位置を示し、走行による各特徴点の移動方向及び移動量を移動量４４ａ、移動量４４ｂの矢印で示している。

特徴点４３ａと特徴点４３ｂは、ステップＳ２１からステップＳ２３で得られた近傍特徴点である。

移動量４４ａと移動量４４ｂは、画像４２でのステップＳ２１からステップＳ２３で得られた近傍特徴点４３ａと特徴点４３ｂの移動量である。ここで、移動量４４ａと移動量４４ｂのΔｕは、Δｕ≠０であるため、上述した従来技術の式である数１にΔｕを代入すると、Δｙａｗ≠０となり、移動体１００のΔｙａｗは誤って推定される。一方、画像４２は移動体１００が移動量４１を走行した後の撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎが撮像した画像上でのΔｕの実際のΔｘは、Δｘ＝０である。

図５Ｂの移動量４５は移動体１００が走行した移動量である。この場合、移動体１００の移動量４５のΔｙａｗ、Δｘ、ΔｙをΔｙａｗ≠０、Δｘ≠０、Δｙ≠０とする。

画像４２ｂは移動体１００が移動量４５を走行した後の撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎが撮像した画像のうちの一枚を表す。なお、画像４２ｂでは、見やすさのため、移動体１００が走行前に撮像した立体物の位置を示し、走行による各特徴点の移動方向及び移動量を移動量４４ａ、移動量４４ｂの矢印で示している。

特徴点４６ａと特徴点４６ｂは、ステップＳ２１からステップＳ２３で得られた近傍特徴点である。

移動量４７ａと移動量４７ｂは、画像４２ｂでのステップＳ２１からステップＳ２３で得られた近傍特徴点４６ａと特徴点４６ｂの移動量である。ここで、移動量４７ａと移動量４７ｂのΔｕは、Δｕ≠０であるため、上述した従来技術の式である数１にΔｕを代入すると、Δｙａｗ≠０となるが、特徴点４６ｂへの距離は特徴点４６ａへの距離より近いため、移動量４７ａ＜移動量４７ｂとなり、移動量４７ａで推定するΔｙａｗと移動量４７ｂで推定するΔｙａｗとは異なる。一方、画像４２ｂは移動体１００が移動量４５を走行した後の撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎが撮像した画像上でのΔｕの実際のΔｘは、Δｘ≠０となる。

図５Ａ及び図５Ｂの説明から分かるように、Δｙａｗと共に変化するのは、従来技術で用いていたΔｕではなく、Δｘ（すなわち横方向の移動量）である。従って、近傍特徴点でΔｙａｗを推定する場合、特徴点への距離ｄと特徴点の実際のΔｘを用いるのがよい。

図６Ａは、実施例１のΔｘと特徴点への距離を用いた位置推定方法の詳細を説明する図であって、移動体１００と特徴点との位置関係を示すとともに特徴点を撮像した画像を示す図である。

図６Ｂは、実施例１のΔｘと特徴点への距離を用いた位置推定方法の詳細を説明する図であって、グラフ５５はパラメータｋと特徴点の距離との関係を表すグラフである。グラフ５５において、縦軸は、パラメータｋであり、横軸は、立体物の距離ｄである。グラフ５５の関係は、予め求めておき、例えばテーブル形式でメモリ１６に記憶しておいてもよいし、グラフ５５の関係の近似式をメモリ１６に記憶しておいてもよい。

移動量５１は、移動体１００が時間ｔ_ｎ−１から時間ｔ_ｎまでの間に走行する移動量である。移動量５１のΔｙａｗ、Δｘ、ΔｙをΔｙａｗ≠０、Δｘ≠０、Δｙ≠０とする。

点５２ａは、移動体１００の近傍にある点であり、時間ｔ_ｎ−１に撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎが取得した画像上でのステップＳ２１からステップＳ２３で得られた特徴点５２ｂに相当する点である。移動体１００が（Δｘ，Δｙ，Δｙａｗ）を移動すると、撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎが取得した画像上で、特徴点５２ｂはΔｙａｗによる移動で（Ｘ_ｎ−１，Ｙ_ｎ−１）から（Ｘ’ｎ−１，Ｙ’_ｎ−１）に移動し、点５３になる。次に、ΔｘとΔｙの移動で、点５３は特徴点５４になる。つまり、移動量５１による特徴点５２ｂから特徴点５４への移動は、数２と数３で表せる。

ただし、移動体１００はｘ方向に移動できない（例えば、真横に移動できない。）と前提条件を付けると、ΔｘはΔｘ＝０となる。従って、数２は数４となり、ステップＳ２１からステップＳ２３で得られた特徴点の移動量からＸ_ｎ、Ｘ_ｎ−１、Ｙ_ｎ、Ｙ_ｎ−１を数５に代入し、数５を解くとΔｙａｗ_ｎを算出できる。

また、ｙ方向の移動量がＧＰＳや他センサにより既知の場合、ステップＳ２１からステップＳ２３で得られた特徴点の移動量から方位による移動量とｙ方向による移動量を区別できるため、数５に代入せずに、ＧＰＳや他センサから得られた（Δｘ，Δｙ）を用いて、特徴点５４を点５３に直し、メモリ１６に記憶してある図６Ｂのグラフ５５の関係を用いて方位を推定する。ここで、時間ｔ_ｎ−１における点５２ａへの距離である距離ｄ_ｎ−１及び時間ｔ_ｎにおける点５２ａへの距離である距離ｄ_ｎを、数６及び数７により算出する。数６及び数７において、Ｓｑｒt（）は（）内の値の正の平方根を求める関数である。

従って、距離ｄ_ｎ−１とｄ_ｎをグラフ５５に代入すると、パラメータｋ_ｎ−１とｋ_ｎが得られる。パラメータｋ_ｎ−１とｋ_ｎの平均を算出し、新たなパラメータｋを算出し、数１からΔｙａｗを推定してもよい。また、時間ｔ_ｎ−１とｔ_ｎの間に移動体１００が移動するため、点５２ａへの距離がｄ_ｎ−１から距離ｄ_ｎへ変化していくため、数８によりΔｙａｗを推定してもよい。なお、数８において、パラメータｋは距離ｄの関数であり（すなわち、ｋ＝ｆ（ｄ））、数８では、距離ｄ_ｎ−１〜距離ｄ_ｎの範囲で、距離ｄに対して積分を算出する。

図７は、実施例１における、複数の特徴点を抽出した場合の処理を説明する図である。

移動量６１は移動体１００が走行する移動量である。

静止点６２は、移動体１００が移動量６１を走行する時の画像上でステップＳ２１からステップＳ２３で特徴点を抽出した点である。静止点６２は、例えば、路面上や建物にある点である。また、静止点６２は一点とは限らず、複数点であってもよい。ステップＳ２１からステップＳ２６で静止点６２を用いて位置推定行う場合、点毎に推定値の候補（Δｘ，Δｙ，Δｙａｗ）_１、（Δｘ，Δｙ，Δｙａｗ）_２、・・・（Δｘ，Δｙ，Δｙａｗ）_ｎが得られるため、最終的に推定値を決定する手段が必須である。ここで、ステップＳ２１からステップＳ２６で得られた推定値の候補（Δｘ，Δｙ，Δｙａｗ）_１、（Δｘ，Δｙ，Δｙａｗ）_２、・・・（Δｘ，Δｙ，Δｙａｗ）_ｎの平均を算出し、推定値を決定してもよい。また、ステップＳ２１からステップＳ２６で得られた推定値の候補（Δｘ，Δｙ，Δｙａｗ）_１、（Δｘ，Δｙ，Δｙａｗ）_２、・・・（Δｘ，Δｙ，Δｙａｗ）_ｎの中央値を算出し、推定値を決定してもよい。

移動体６３は、移動体１００の周囲を走行している移動体である。

動点６４は、移動体１００が移動量６１を走行する時の画像上でステップＳ２１からステップＳ２３で移動体６３から特徴点を抽出した点である。動点６４は一点とは限らず、複数点であってもよい。簡単のため動点６４を移動体６３にある点とするが、歩行者、他車、動物などの静止していない立体物であってもよい。移動体６３が静止していないため、移動体１００の撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎの画像上での動点６４の移動量は静止点６２の移動量と異なる。従って、移動体６３にある動点６４から得られた特徴点で移動体１００の位置推定を行う場合、推定した位置の誤差が大きい。

ここで、移動体６３の特徴点による影響を軽減させるための処理を説明する。

静止点６２の点数が動点６４の点数より多いという前提条件を付けて、ステップＳ２１からステップＳ２６で得られた推定値の候補（Δｘ，Δｙ，Δｙａｗ）_１、（Δｘ，Δｙ，Δｙａｗ）_２、・・・（Δｘ，Δｙ，Δｙａｗ）_ｎの平均や中央値を算出し、推定値を決定する。

一方、静止点６２の点数と動点６４の点数が未知の場合、移動体を障害物として検出する既知の障害物検出技術と組み合わせ、移動体６３が障害物として検出された後、ステップＳ２１からステップＳ２３で移動体６３から抽出した特徴点を削除し、移動体１００の位置を推定するときに削除された特徴点を利用しないようにしてもよい。例えば、画像処理部１４の検出部１０９は、各特徴点の各々が、移動体１００とは別の、他の移動体（例えば、移動体６３）から抽出した特徴点であるか否かを検出し、画像処理部１４の方位推定部１０７は、検出部１０９で他の移動体（例えば、移動体６３）から抽出した特徴点であると検出された特徴点を、方位推定の際に利用する特徴点から削除（除外）する。

また、過去時系列に推定した位置情報を保存しておき、保存してある過去の位置情報に基づいて次のフレームでの位置を予測し、最新フレームでステップＳ２１からステップＳ２６から得られた特徴点の移動量を、保存してある時系列の位置情報に基づき予測した位置と比較し、予測との差が大きい特徴点を削除するようにしてもよい。位置の予測は、例えば、過去推定した位置の多項式による補間やＫａｌｍａｎ Ｆｉｌｔｅｒのような確率的な手法で算出することで行うことができる。

図８は、実施例１における、単眼カメラとステレオカメラとでの位置推定処理の違いについて説明する図である。

撮像装置７１は、図１に示した撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎに相当する撮像装置である。ここでは、簡単のため、撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎのうちの一台に相当する撮像装置を、撮像装置７１として示して説明するが、撮像装置７１と同様のステレオカメラや単眼カメラを複数台備えた構成であってもよい。

路面７２は、移動体１００が走行中の路面である。高さＨは、撮像装置７１の路面７２に対する設置高さである。立体物７４は、路面７２の上にある静止立体物である。

点７５は、撮像装置７１でステップＳ２１からステップＳ２３で立体物７４から特徴点を抽出する点である。点７５ｂは、撮像装置７１から点７５と同じ方向で路面７２の上にある点である。点７５ｃは、立体物７４と路面７２の接触点である。点７６は撮像装置７１でステップＳ２１からステップＳ２３で路面７２から特徴点を抽出する点である。

撮像装置７１がステレオカメラである場合、点７５や点７６への距離を算出できるため、前述したようにメモリ１６に記憶してある図６Ｂのグラフ５５の関係を用いて点ごとにパラメータｋを算出し、このパラメータｋを用いて移動体１００の方位と移動量を推定するようにしてもよい。

一方、撮像装置７１が単眼カメラである場合、点７５や点７６への距離を直接算出できないため、他の技術を活用する。例えば、立体物が写っている画像と立体物への距離をＤｅｅｐ Ｌｅａｒｎｉｎｇ技術で学習し、撮像装置７１とステップＳ２１で取得した画像に写っている立体物の距離を推定し、図６Ｂのグラフ５５の関係を用いて点ごとにパラメータｋを算出し、このパラメータｋを用いて移動体１００の方位と移動量を推定するようにしてもよい。

また、撮像装置７１が単眼カメラである場合、撮像装置７１の路面７２に対する設置高さＨと、撮像装置７１の路面７２に対する設置角度とで、路面にある特徴点への距離を幾何学的に算出するようにしてもよい。一方、点７５のように路面７２以外にある点の場合、幾何学的に距離を算出する場合、点７５と、撮像装置７１から点７５と同じ方向で路面７２の上にある点７５ｂと、の区別ができないため、点７５ではなく、点７５ｂへの距離を誤って推定してしまうことになる。従って、単眼による障害物検出を用いて、立体物７４と路面７２との接触点である点７５ｃを算出し、この点７５ｃへの距離を算出し、点７５ｃと点７５を結ぶ直線は路面７２に対して垂直であると前提し、点７５ｃへの距離に基づいて点７５への距離を算出し、移動体１００の方位と移動量を推定するようにしてもよい。

また、撮像装置７１が単眼カメラである場合、Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ Ａｎｄ Ｍａｐｐｉｎｇ（ＳＬＡＭ）技術を用いて、移動体１００の移動量と点７５を数フレームにトラッキングし、点７５への距離を算出し、移動体１００の方位と移動量を推定するようにしてもよい。

＜付記１＞
なお、以上説明した本発明は、
１．
移動体の移動量に基づいて該移動体の位置を推定する位置推定装置であって、
前記移動体の周囲の画像を取得する画像取得部（例えば、撮像装置１２）と、
前記画像取得部で取得した画像に基づいて前記移動体の移動量を推定する移動体移動量推定部（例えば、移動体移動量推定部１０１）と、
前記移動体移動量推定部で推定した移動量に基づいて前記移動体の位置を推定する移動体位置推定部（例えば、移動体位置推定部１０２）と、
を備え、
前記移動体移動量推定部は、
第一のタイミングで前記画像取得部によって取得した第一の画像から第一の特徴点を抽出する第一の特徴点抽出部（例えば、第一の特徴点抽出部１０３）と、
前記第一のタイミングより後の第二のタイミングで前記画像取得部によって取得した第二の画像から第二の特徴点を抽出する第二の特徴点抽出部（例えば、第二の特徴点抽出部１０４）と、
前記第一の特徴点を前記第二の特徴点にトラッキングするトラッキング部（例えば、トラッキング部１０５）と、
前記トラッキング部でトラッキングした前記第二の特徴点の前記移動体からの距離を算出する距離算出部（例えば、距離算出部１０６）と、
前記距離算出部で算出した前記第二の特徴点の前記移動体からの距離に基づいて、前記移動体の方位を推定する方位推定部（例えば、方位推定部１０７）と、
前記方位推定部で推定した前記移動体の方位に基づいて、前記移動体の移動量を推定する移動量推定部（例えば、移動量推定部１０８）と、
を有する、
ことを特徴とする位置推定装置、としたので、
・精度よく移動体の位置を推定する位置推定装置を提供することができる。
・すなわち、移動体の方位の推定において、第二の特徴点の移動体からの距離を考慮することで、精度よく方位を推定することができる。

また、本発明は、
２．
１．に記載の位置推定装置において、
前記方位推定部は、前記第二の特徴点の前記移動体からの距離に応じたパラメータを算出し、該パラメータを用いて前記移動体の方位を推定する、
ことを特徴とする位置推定装置、としたので、
・第二の特徴点の移動体からの距離に応じたパラメータを用いることで、精度よく移動体の位置を推定することができる。

また、本発明は、
３．
１．に記載の位置推定装置において、
前記方位推定部は、前記第二の特徴点の前記移動体に対する横方向の移動量に基づいて、前記移動体の方位を推定する、
ことを特徴とする位置推定装置、としたので、
・第二の特徴点の移動体に対する横方向の移動量を用いることで、精度よく移動体の位置を推定することができる。

また、本発明は、
４．
１．に記載の位置推定装置において、
前記画像取得部は、前記移動体に搭載され、該移動体の周囲の画像を撮像して取得する撮像装置である、
ことを特徴とする位置推定装置、としたので、
・移動体に搭載された撮像装置によって移動体の周囲の画像を撮像して取得することで、移動体の周囲を確実に撮像することができる。

また、本発明は、
５．
１．に記載の位置推定装置において、
前記第一の特徴点は複数の第一の特徴点であり、
前記第二の特徴点は複数の第二の特徴点であり、
前記距離算出部は、前記複数の第一の特徴点の各々の前記移動体からの距離を算出するとともに、前記複数の第二の特徴点の各々の前記移動体からの距離を算出し、
前記方位推定部は、前記距離算出部で算出した前記複数の第一の特徴点の各々の前記移動体からの距離、及び前記距離算出部で算出した前記複数の第二の特徴点の各々の前記移動体からの距離に基づいて、前記移動体の方位を推定する、
ことを特徴とする位置推定装置、としたので、
・複数の第一の特徴点、複数の第二の特徴点の移動体からの距離を考慮することで、精度よく方位を推定することができる。

また、本発明は、
６．
５．に記載の位置推定装置において、
前記方位推定部は、前記複数の第一の特徴点の各々の前記移動体からの距離の各々、及び前記複数の第二の特徴点の各々の前記移動体からの距離の各々に応じた複数のパラメータを算出し、該複数のパラメータを用いて前記移動体の方位を推定する、
ことを特徴とする位置推定装置、としたので、
・複数の第一の特徴点、複数の第二の特徴点の移動体からの距離に応じたパラメータを用いることで、精度よく移動体の位置を推定することができる。

また、本発明は、
７．
５．に記載の位置推定装置において、
前記移動体移動量推定部は、前記複数の第一の特徴点の各々及び前記複数の第二の特徴点の各々が、前記移動体とは別の、他の移動体（例えば、移動体６３）から抽出した特徴点であるか否かを検出する検出部（例えば、検出部１０９）を有し、
前記方位推定部は、前記検出部で前記他の移動体から抽出した特徴点であると検出された特徴点を、前記複数の第一の特徴点、及び前記複数の第二の特徴点から削除する、
ことを特徴とする位置推定装置、としたので、
・他の移動体から抽出した特徴点を、複数の第一の特徴点、及び複数の第二の特徴点から削除することで、誤差を除外し、精度よく移動体の位置を推定することができる。

なお、本発明は、上記した実施例に限定されるものではなく、様々な変形例が含まれる。例えば、上記した実施例は本発明を分かりやすく説明するために詳細に説明したものであり、必ずしも説明した全ての構成を備えるものに限定されるものではない。

また、ある実施例の構成の一部を他の実施例の構成に置き換えることが可能であり、また、ある実施例の構成に他の実施例の構成を加えることも可能である。

また、各実施例の構成の一部について、他の構成の追加・削除・置換をすることが可能である。

また、上述した個々の実施例の各要素のいかなる組合せも本発明に含むものである。

１…位置推定装置、１２…撮像装置、１３…情報処理装置、１４…画像処理部、１５…制御部、１６…メモリ、１７…表示部、１００…移動体、１０１…移動体移動量推定部、１０２…移動体位置推定部、１０３…第一の特徴点抽出部、１０４…第二の特徴点抽出部、１０５…トラッキング部、１０６…距離算出部、１０７…方位推定部、１０８…移動量推定部、１０９…検出部。

Claims (2)

1. 移動体の移動量に基づいて該移動体の位置を推定する位置推定装置であって、
   前記移動体の周囲の画像を取得する画像取得部と、
   前記画像取得部で取得した画像に基づいて前記移動体の移動量を推定する移動体移動量推定部と、
   前記移動体移動量推定部で推定した移動量に基づいて前記移動体の位置を推定する移動体位置推定部と、
   を備え、
   前記移動体移動量推定部は、
   第一のタイミングで前記画像取得部によって取得した第一の画像から第一の特徴点を抽出する第一の特徴点抽出部と、
   前記第一のタイミングより後の第二のタイミングで前記画像取得部によって取得した第二の画像から第二の特徴点を抽出する第二の特徴点抽出部と、
   前記第一の特徴点を前記第二の特徴点にトラッキングするトラッキング部と、
   前記トラッキング部でトラッキングした前記第二の特徴点の前記移動体からの距離を算出する距離算出部と、
   前記距離算出部で算出した前記第二の特徴点の前記移動体からの距離に基づいて、前記移動体の方位を推定する方位推定部と、
   前記方位推定部で推定した前記移動体の方位に基づいて、前記移動体の移動量を推定する移動量推定部と、
   を有し、
   前記方位推定部は、前記第二の特徴点の前記移動体からの距離に応じたパラメータを算出し、該パラメータを用いて前記移動体の方位を推定する、
   ことを特徴とする位置推定装置。
2. 移動体の移動量に基づいて該移動体の位置を推定する位置推定装置であって、
   前記移動体の周囲の画像を取得する画像取得部と、
   前記画像取得部で取得した画像に基づいて前記移動体の移動量を推定する移動体移動量推定部と、
   前記移動体移動量推定部で推定した移動量に基づいて前記移動体の位置を推定する移動体位置推定部と、
   を備え、
   前記移動体移動量推定部は、
   第一のタイミングで前記画像取得部によって取得した第一の画像から第一の特徴点を抽出する第一の特徴点抽出部と、
   前記第一のタイミングより後の第二のタイミングで前記画像取得部によって取得した第二の画像から第二の特徴点を抽出する第二の特徴点抽出部と、
   前記第一の特徴点を前記第二の特徴点にトラッキングするトラッキング部と、
   前記トラッキング部でトラッキングした前記第二の特徴点の前記移動体からの距離を算出する距離算出部と、
   前記距離算出部で算出した前記第二の特徴点の前記移動体からの距離に基づいて、前記移動体の方位を推定する方位推定部と、
   前記方位推定部で推定した前記移動体の方位に基づいて、前記移動体の移動量を推定する移動量推定部と、
   を有し、
   前記第一の特徴点は複数の第一の特徴点であり、
   前記第二の特徴点は複数の第二の特徴点であり、
   前記距離算出部は、前記複数の第一の特徴点の各々の前記移動体からの距離を算出するとともに、前記複数の第二の特徴点の各々の前記移動体からの距離を算出し、
   前記方位推定部は、前記距離算出部で算出した前記複数の第一の特徴点の各々の前記移動体からの距離、及び前記距離算出部で算出した前記複数の第二の特徴点の各々の前記移動体からの距離に基づいて、前記移動体の方位を推定し、
   前記方位推定部は、前記複数の第一の特徴点の各々の前記移動体からの距離の各々、及び前記複数の第二の特徴点の各々の前記移動体からの距離の各々に応じた複数のパラメータを算出し、該複数のパラメータを用いて前記移動体の方位を推定する、
   ことを特徴とする位置推定装置。

Images