JP6865064B2 - 移動体の位置推定装置及び方法 - Google Patents

Description

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

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

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

また、駐車場やトンネルのような環境では、地図情報とランドマークがなく、ＧＰＳも使えない場合があるため、移動体の周囲に存在する物体との相対位置を推定するオドメトリ法が用いられる。

例えば、特許文献１は、第１の自車位置推定部により自車の周囲環境の画像に基づき地上固定座標系上での自車の位置と姿勢の推定値及び推定精度を推定する。第２の自車位置推定部により自車の操作量及び運動状態に基づき推定される自車位置の変化量を積算することで基準点及び方位と自車位置との相対的な位置関係を推定し、目標走行経路上で第１の自車位置推定部による位置推定の難度が高い区間を推定する。難度が高いと推定された区間の手前側領域内で周囲環境撮影部の撮影範囲に入る可能性が高いランドマークを地図情報から検出する。検出されたランドマークに基づき位置推定難度の高い区間を走行中、自車位置推定切替部により第１の自車位置推定部から第２の自車位置推定部に切り替える。位置推定難度の高い区間を走行中、自車位置推定切替部の結果に基づく第２の自車位置推定部の出力に基づき自車の走行制御を実行し、位置推定難度の高い区間以外の区間では自車位置推定切替部の結果に基づく第１の自車位置推定部の出力に基づき自車の走行制御を実行する。

位置推定難度の高い区間を走行中、第１の自車位置推定部から第２の自車位置推定部に切り替え、位置推定難度の高い区間を走行中、自車位置推定切替部の結果に基づく第２の自車位置推定部の出力に基づき自車の走行制御を実行する。位置推定難度の高い区間以外の区間では自車位置推定切替部の結果に基づく第１の自車位置推定部の出力に基づき自車の走行制御を実行する。

特開２０１４−３４２５１

ところで、移動体は移動及び停止を繰り返すので、移動体が観察する周囲の状況はめまぐるしく変化する。移動体の自律走行技術または運転支援技術において、どのような状況であっても精度良く現在位置を推定することが重要である。

本発明の一つの実施態様によれば、移動体の位置推定装置は、前記移動体に取り付けられた撮像装置と、前記撮像装置で撮像された画像に基づいて前記移動体の複数の位置候補を特定し、前記複数の位置候補と前記移動体の移動速度とに基づいて前記移動体の位置を推定する画像処理部と、を有する。

本発明の実施形態に係る移動体の位置推定装置の構成図 画像処理部１４が行う画像処理手順を示すフローチャート 撮像装置１２の撮像範囲から定まる移動体１００の最大移動速度の説明図 撮影されたビデオ画像の中の一つのフレーム画像の一例 速度による重み付けの具体例 障害物による重み付けの実施例。 制御部１５の制御により移動体１００の速度が変化する例 撮像装置１２が一台の場合の例

以下、本発明の実施形態に係る移動体の位置推定装置について、図面を用いて説明する。

図１は、本発明の一実施形態に係る位置推定装置１の構成図である。

位置推定装置１は、自動車またはロボットなどの移動体１００に搭載されている。位置推定装置１は、一台以上の撮像装置１２（１２ａ、１２ｂ、・・・１２ｎ）と、情報処理装置１３とを有する。

撮像装置１２は、例えばスチルカメラまたはビデオカメラでよい。撮像装置１２は単願カメラまたはステレオカメラでもよい。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

画像処理部１４は、複数の移動体位置候補算出処理２１（２１ａ、２１ｂ、・・・２１ｎ）と、複数の移動体位置候補から移動体位置を推定する推定処理２６とを行う。

複数の移動体位置候補算出処理２１（２１ａ、２１ｂ、・・・２１ｎ）は、互いに並列に実行されてもよい。例えば、移動体位置候補算出処理２１ａでは、撮像装置１２ａによって撮像された画像が処理されてもよい。同様に、移動体位置候補算出処理２１ｂ、・・・２１ｎでは、撮像装置１２ｂ、・・・撮像装置１２ｎによって撮像された画像が処理されてもよい。これにより、画像処理部１４は、各撮像装置１２が撮像した画像からそれぞれの移動体１００の位置候補を特定してもよい。

また、撮像装置１２ａが撮像した画像が前方の遠景であるときは、移動体位置候補算出処理２１ａでは移動体１００の推進方向を推定しやすい遠方特徴点を用いて推進方向が推定されてもよい。撮像装置１２ｂが撮像した画像が後方の近景であるときは、移動体位置候補算出処理２１ｂでは移動体１００の移動量を推定しやすい近傍特徴点を用いて移動量が推定されてもよい。

図示するように、複数の移動体位置候補算出処理２１（２１ａ、２１ｂ、・・・２１ｎ）は共通のステップを実行する。以下、各ステップについて説明する。

画像処理部１４は、メモリ１６から画像データを取得する（Ｓ２２ａ、Ｓ２２ｂ、・・・Ｓ２２ｎ）。ここで取得する画像データは複数のフレーム画像のデータでよい。

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

画像処理部１４は、フレーム画像の時系列に従って、各フレーム画像でそれぞれ抽出された特徴点をトラッキングする（Ｓ２４ａ、Ｓ２４ｂ、・・・Ｓ２４ｎ）。トラッキングにはＬｕｃａｓ−Ｋａｎａｄｅ法やＳｈｉ−Ｔｏｍａｓｉ法やＤｉｒｅｃｔ Ｍａｔｃｈｉｎｇ法などの技術を用いてもよい。具体的なアルゴリズムは、画像の特徴に応じて適宜選択される。

画像処理部１４は、特徴点トラッキングの結果に基づいて、移動体１００の移動量を算出して、移動体１００の現在位置の候補を算出する（Ｓ２５ａ）。移動量の算出には、例えばＲｉｇｉｄ Ｂｏｄｙ Ｔｒａｎｓｆｏｒｍａｔｉｏｎ、Ｓｌｉｄｉｎｇ Ｗｉｎｄｏｗ、基礎行列、中央値などを利用することができる。また、中央値フィルタ、ＲＡＮＳＡＣ、バンドル調整、キーフレーム調整等の外れ値検出と最適化処理をしてもよい。

複数の移動体位置候補算出処理２１ａ、２１ｂ、・・・２１ｎでそれぞれ異なるビデオ画像を対象とした処理が行われることで、複数の位置候補が算出される。

ステップＳ２６の位置推定処理は、それぞれの移動体位置候補算出処理２１ａ、２１ｂ、・・・２１ｎによって算出された位置候補を統合して、移動体１００の現在位置を推定する。位置推定処理の詳細は後述する。

図３を用いて撮像装置１２の撮像範囲から定まる移動体１００の最大移動速度について説明する。

同図は、移動体１００に取り付けられた撮像装置１２を模式的に示す側面図である。つまり、撮像装置１２は路面３０から高さＨの位置に、光軸が路面３０に対して角度αとなるように設置されている。さらに、撮像装置１２の画角がγ、撮像装置１２から路面までの最大撮像距離がＤ、最大撮像範囲がＲである。

ここで、ビデオ画像で特徴点のトラッキングを行うためには、少なくとも時間的に隣り合う２枚のフレーム画像に共通の特徴点が写っている必要がある。従って、フレーム間の時間間隔Δｔでの移動体１００の移動距離がＲ以下であることが必要である。

また、撮像装置１２が撮像するビデオ画像のフレームレートをＦＰＳとすると、フレーム間の時間間隔Δｔは式（１）で表される。
Δｔ＝２／ＦＰＳ （１）

従って、移動体１００の移動中に撮像されたビデオ画像で特徴点トラッキングを可能とするためには、移動体１００の移動速度に上限がある。その上限である最大移動速度Ｖｍａｘは以下の式（２）で定まる。
Ｖｍａｘ ＝ Ｒ／Δｔ ＝ ［Ｄｃｏｓ（α）−Ｈ／ｔａｎ（α＋γ）］ＦＰＳ／２ （２）

次に、特徴点トラッキングの精度を低下させる要因について説明する。

移動体１００が最大移動速度Ｖｍａｘ以下で走行している場合であっても、移動体１００の移動速度が増すほどトラッキングの精度が低下し、誤差が生じやすくなる。そこで、画像処理部１４が複数の移動体位置候補算出処理２１ａ、２１ｂ、・・・２１ｎで算出された位置候補を用いて現在位置を推定する際には、ステップＳ２６において移動体１００の移動速度に応じた位置推定を行っても良い。例えば、画像処理部１４は、速度による重み係数Ｗ_ｖを用いて、位置推定を行う。速度による重み係数Ｗ_ｖは、例えば、以下の式（３）で定まる。
Ｗ_ｖ ＝ １−Ｖ／Ｖｍａｘ（３）

また、撮影された画像の特性に応じて特徴点トラッキングの精度が低下する場合がある。

図４は、撮影されたビデオ画像の中の一つのフレーム画像Ｉの一例を示す。

同図は、画像３１内に障害物３４が写っている例を示す。障害物３４は特徴点抽出の対象とはならない。従って、画像内で障害物３４が占める面積が大きくなるほど画像３１から得られる情報量が減少する。つまり、障害物３４が占める面積が大きくなると画像３１から検出される特徴点数も減り、トラッキングの精度が低下する。そこで、画像処理部１４は、ステップＳ２６で障害物の面積に応じた位置推定をしてもよい。例えば、画像処理部１４は、障害物による重み係数Ｗ_ｏｂを用いて位置推定を行ってもよい。

ここで、画像３１のｕ軸の画素数をＰｕ、ｖ軸の画素数をＰ_ｖとする。さらに、障害物３４が占める面積の画素数をＡ（Ａ１、Ａ２、Ａ３）とする。このとき、障害物による重み係数Ｗ_ｏｂは、例えば、以下の式（４）で定まる。なお、式（４）は一例であり、式（４）以外の障害物による重み係数を用いてもよい。
Ｗ_ｏｂ ＝ １−（Ａ１＋Ａ２＋・・・＋Ａｋ）／（Ｐｕ・Ｐ_ｖ）（４）

また、図４の画像３１内にはさらに複数の特徴点３６、３７があり、これが検出されている。同図の例では、特徴点には、有効な特徴点（インライア）３６と有効でない特徴点である外れ値（アウトライア）３７とを含む。画像処理部１４が特徴点３６、３７を検出した後、所定の外れ値検出処理により外れ値３７を特定してもよい。

ここで、外れ値３７が多いほど、特徴点トラッキングに使える特徴点が少なくなり、トラッキングの精度が低下する。そこで、画像処理部１４は、ステップ２６で画像３１内の特徴点のうちの外れ値３７に応じた位置推定を行っても良い。例えば、画像処理部１４は、外れ値による重み係数Ｗ_ｆｐを用いて位置推定を行う。画像３１における有効な特徴点の個数をｉ、外れ値の個数をｏとすると、外れ値による重み係数Ｗ_ｆｐは、例えば、以下の式（５）で定まる。なお、式（５）は一例であり、式（５）以外の外れ値による重み係数を用いてもよい。
Ｗ_ｆｐ ＝ １−ｏ／（ｏ＋ｉ）（５）

以上説明したように、一つのビデオ画像に対して、特徴点トラッキングの精度を低下させる要因が複数ある。そして、上述のように、各フレーム画像Ｉに対して、速度による重み係数Ｗ_ｖ、障害物に関する重み係数Ｗ_ｏｂ及び特徴点の外れ値に関する重み係数Ｗ_ｆｐのそれぞれが算出可能である。画像処理部１４は、ステップＳ２６において、これらの１つ以上を用いることができる。例えば、画像処理部１４は、これらのうちの１つの重み係数を個別に用いてもよいし、２つ以上の重み係数を統合して、各フレーム画像Ｉに対して一つの重み係数Ｗを特定してもよい。ある時刻ｔのフレーム画像Ｉ（ｔ）に対する重み係数Ｗ（ｔ）は、同じ時刻の速度による重み係数Ｗ_ｖ（ｔ）、障害物に関する重み係数Ｗ_ｏｂ（ｔ）、及び特徴点の外れ値に関する重み係数Ｗ_ｆｐ（ｔ）に基づいて、例えば、以下の式（６）で定まる。

Ｗ（ｔ）＝ａｖｅｒａｇｅ（Ｗ_ｖ（ｔ），Ｗ_ｏｂ（ｔ），Ｗ_ｆｐ（ｔ）） （６）

上述のように、複数の撮像装置１２で撮像した画像に基づき複数の位置候補が算出される。従って、画像処理部１４は、ステップＳ２６において複数の自車位置の候補から自車位置を推定する。例えば、画像処理部１４は、ある時刻ｔの自車位置Ｐｏｓ（ｔ）を推定するために、その時刻のフレーム画像Ｉ（ｔ）に対する重み係数Ｗ（ｔ）を用いる。ある時刻ｔの自車位置Ｐｏｓ（ｔ）は、例えば、式（７）で算出されてもよい。

Ｐｏｓ（ｔ）＝（Ｐａ（ｔ）Ｗａ（ｔ）＋Ｐｂ（ｔ）Ｗｂ（ｔ）＋・・・＋Ｐｎ（ｔ）Ｗｎ（ｔ））／（Ｗａ（ｔ）＋Ｗｂ（ｔ）＋・・・Ｗｎ（ｔ）） （７）

これにより、複数の画像からそれぞれ算出された現在位置候補を統合して移動体１００の現在位置を推定できる。

次に、速度による重み付けの具体例を示す。

図５Ａは、撮像装置１２ａ及び撮像装置１２ｂを搭載した移動体１００の一例を示す。

撮像装置１２ａは、移動体１００の前方に向けて設置され、遠景を撮像する撮像装置である。撮像装置１２ａは撮像範囲Ｒ_Ｓを有する。

撮像装置１２ｂは、移動体１００の後方で下方に向けて設置され、近景を撮像する撮像装置である。撮像装置１２ｂは撮像範囲Ｒ_Ａを有する。

図５Ｂは、時刻ｔと自車速度Ｖの関係を表すグラフである。

速度Ｖｍａｘ_Ｓは、撮像装置１２ａで撮像した画像で特徴点トラッキングが可能な最高速度であり、速度Ｖｍａｘ_Ａは、撮像装置１２ｂで撮像した画像で特徴点トラッキングが可能な最高速度である。

図５Ｃは、時刻ｔと速度による重み係数Ｗ_ｖの関係を表すグラフである。時刻ｔ_Ｓは、移動体１００の速度がＶｍａｘ_Ｓになる時刻である。時刻ｔ_Ａは、移動体１００の速度がＶｍａｘ_Ａになる時刻である。

重みＷ_ｖＳは撮像装置１２ａの画像に対する自車速度Ｖによる重みである。重みＷ_ｖＡは撮像装置１２ｂの画像に対する自車速度Ｖによる重みである。

ｔ＝０のとき、移動体１００は止まっているため、式（３）に従い、重み係数Ｗ_ｖＳ及びＷ_ｖＡはいずれも１である。

ｔ＞０のとき、移動体１００が移動している。従って速度がＶ＞０であり、式（３）によればＷ_ｖＳ＜１、Ｗ_ｖＡ＜１になる。

ｔ＝ｔ_Ａになると、移動体１００の移動速度がＶ＝Ｖｍａｘ_Ａになるので、式（３）に従いＷ_ｖＡ＝０となる。ｔ＞ｔ_Ａにおいて、Ｗ_ｖＡ＝０である。

同様に、ｔ＝ｔ_Ｓになると、移動体１００の移動速度がＶ＝Ｖｍａｘ_Ｓになるので、式（３）に従いＷ_ｖＳ＝０となる。ｔ＞ｔ_Ｓにおいて、Ｗ_ｖＳ＝０である。

次に、障害物による重み付けの具体例を示す。

図６Ａは、図５Ａと同様に撮像装置１２ａ及び撮像装置１２ｂを搭載した移動体１００の一例を示す。この例ではさらに、撮像装置１２ａの撮像範囲に障害物５１が存在し得る。

図６Ｂは、図５Ｂと同様に、時刻ｔと自車速度Ｖの関係を表すグラフである。

図６Ｃは、時刻ｔと速度による重み係数Ｗ_ｖ及び障害物による重み係数Ｗ_ｏｂとの関係を表すグラフである。

速度による重み係数Ｗ_ｖは、図５Ｃの場合と同じである。

時刻ｔ_ｏｂは障害物５１が撮像範囲Ｒ_Ｓに入り、撮像される画像に写り始める時刻である。時間ｔ_ｏｂ’は、障害物５１が撮像範囲Ｒ_Ｓから外れ、撮像される画像に写らなくなる時刻である。

０＜ｔ＜ｔ_ｏｂのとき、撮像装置１２ａの撮像範囲Ｒ_Ｓに障害物５１はないため、Ｗ_ｏｂｓ＝１である。

ｔ＝ｔ_ｏｂになると、障害物５１は撮像装置１２ａの撮像範囲Ｒ_Ｓに入るため、式（４）に従ってＷ_ｏｂｓ＜１になる。ここでは簡単のために画像全体の画素数に占める障害物の画素数は一定とする。

ｔ＝ｔ_ｏｂ’になると、障害物５１が撮像装置１２ａの撮像範囲Ｒ_Ｓから外れ、画像に写らなくなるため、式（４）によってＷ_ｏｂｓ＝１になる。

このように、障害物が映り込むと障害物による重み係数Ｗ_ｏｂが低下し、その画像による位置推定の精度が低下する。そこで、制御部１５が画像処理部１４の処理結果に応じて移動体１００を制御するようにしてもよい。図６にその一例を示す。ここでは、画像処理部１４が障害物を検出すると、制御部１５の指令により移動体１００の速度が一定に保たれる例を示す。ここでは、障害物の検出をトリガーとしているが、これ以外のトリガーであってもよい。例えば、特徴点のうち外れ値数の割合が所定以上であることをトリガーとしてもよい。

図７は、図６と同じ状況下で障害物の検出をトリガーとする制御部１５の制御により、移動体１００の速度が変化する例を示す。

図７Ａは、図５Ｂ、図６Ｂと同様に、時刻ｔと自車速度Ｖの関係を表すグラフである。

０＜ｔ＜ｔ_ｏｂのとき、移動体１００が加速する。ここでは移動体１００の速度Ｖ＞０であるので、式（３）によりＷ_ｖＳ＜１、Ｗ_ｖＡ＜１になる。

ｔ＝ｔ_ｏｂになると、撮像装置１２ａの撮像範囲Ｒ_Ｓの画像に障害物５１が入り、画像処理部１４がこれを検出して、制御部１５へ通知する。この通知を行う基準は、例えば、障害物５１の画素数が所定以上でもよいし、画像全体の画素数に占める障害物５１の画素数の割合が所定以上でもよい。制御部１５は、この通知を受けると、そのときの速度を維持する指令を出力する。これにより、移動体１００の速度が維持されるので、速度による重み係数Ｗ_ｖも維持される。なお、移動体の速度を維持する代わりに、移動体の速度を低下させてもよい。

時刻ｔ＝ｔ_ｏｂ’になると、撮像装置１２ａの撮像範囲Ｒ_Ｓの画像から障害物５１が消える。画像処理部１４がこれを検出して、制御部１５へ通知する。この通知を行う基準は、例えば、障害物５１の画素数が所定以下でもよいし、画像全体の画素数に占める障害物５１の画素数の割合が所定以下でもよい。制御部１５は、この通知を受けると、それまでの速度維持を解除する指令を出力する。これにより、移動体１００は再び加速する。速度の増加に伴って、速度による重み係数も増加する。

これにより、移動体１００が観察する周囲の状況に応じて、精度良く現在位置を推定できる。特に、障害物が検出されているときは速度を一定に保つかまたは減少させ、速度上昇による位置推定の精度の更なる低下を防止する。

次に、図８は、撮像装置１２が一台の場合の例を示す。

図８Ａは、一台の撮像装置１２ａを搭載した移動体１００の一例を示す。この例では、撮像装置１２ａが移動体１００の前方に取り付けられている例で説明するが、撮像装置の取付位置は前方に限らない。

撮像装置１２が複数台あるときは、各撮像装置１２が撮像した画像別に移動体位置候補算出処理２１を適用して画像ごとに位置候補を算出したのに対して、撮像装置１２が一台の場合は、共通の画像に対して異なる移動体位置候補算出処理２１を適用する。

以下の説明では、撮像装置１２ａが撮像したビデオ画像に移動体位置候補算出処理２１ａ及び移動体位置候補算出処理２１ｂの２種類の処理が適用され、これらの処理の精度による重み付けを行う例を示す。ここで、移動体位置候補算出処理２１ａは移動体位置候補算出処理２１ｂと比べると、位置推定の精度は高いが、障害物の影響を受けやすい、ロバストでない処理である。移動体位置候補算出処理２１ｂは移動体位置候補算出処理２１ａと比べると、位置推定にあたり障害物の影響を受けにくいが、位置推定の精度は低いロバストな処理である。

図６と同様に、時刻ｔ_ｏｂに障害物５１が撮像範囲Ｒ_Ｓに入り、時間ｔ_ｏｂ’に障害物５１が撮像範囲Ｒ_Ｓから外れる。ここでは簡単のために画像全体の画素数に占める障害物５１の画素数は一定とする。

図８Ｂは、時刻ｔと、移動体位置候補算出処理２１ａの精度との関係を表す。図８Ｃは、時刻ｔと、移動体位置候補算出処理２１ｂの精度との関係を表す。つまり、移動体位置候補算出処理２１ａ、２１ｂは、画像の特徴に応じて現在位置候補算出の精度が変化する。

ここで、０＜ｔ＜ｔ_ｏｂのときは、撮像装置１２ａの撮像範囲Ｒ_Ｓに障害物５１が写っていないため、移動体位置候補算出処理２１ａの精度Ｐｒ_１と移動体位置候補算出処理２１ｂの精度Ｐｒ_２はともに高い。

この場合、画像処理部１４はステップＳ２６において、式（８）で示すように、移動体位置候補算出処理２１ａで推定された位置Ｐ_１と移動体位置候補算出処理２１ｂで推定された位置Ｐ_２のいずれか一方を推定位置Ｐｏｓ（ｔ）としてもよい。
Ｐｏｓ（ｔ）＝Ｐ_１、ｉｆ Ｐｒ_１＞Ｐｒ_２
Ｐｏｓ（ｔ）＝Ｐ_２、ｉｆ Ｐｒ_１＜Ｐｒ_２ （８）

あるいは、画像処理部１４はステップＳ２６において、式（９）に示すように、移動体位置候補算出処理２１ａで推定された位置Ｐ_１と移動体位置候補算出処理２１ｂで推定された位置Ｐ_２に、移動体位置候補算出処理２１ａと移動体位置候補算出処理２１ｂの精度に応じた重み付けをした平均位置を算出して、これを推定位置Ｐｏｓ（ｔ）としてもよい。
Ｐｏｓ（ｔ）＝（Ｐ_１・Ｗ_１＋Ｐ_２・Ｗ_２）／（Ｗ_１＋Ｗ_２） （９）
Ｗ_１：Ｐｒ_１に応じた重み係数
Ｗ_２：Ｐｒ_２に応じた重み係数

ｔ_ｏｂ＜ｔ＜ｔ_ｏｂ’では、障害物５１が撮像装置１２ａの撮像範囲Ｒ_Ｓに写っているため、ステップＳ２３ａ、ステップＳ２３ｂで抽出できる特徴点が減少する。これにより、移動体位置候補算出処理２１ａの精度がＰｒ_１’に、移動体位置候補算出処理２１ｂの精度がＰｒ_２’にそれぞれ低下する。そして、ロバストでない移動体位置候補算出処理２１ａがロバストである移動体位置候補算出処理２１ｂよりも大きな影響を受け、Ｐｒ_２’＞Ｐｒ_１’となる。

この場合、画像処理部１４はステップＳ２６で、より精度の高い移動体位置候補算出処理２１ｂで算出された位置候補Ｐ_２を推定位置Ｐｏｓ（ｔ）としてもよい。

Ｐｏｓ（ｔ）＝ Ｐ_２ （１０）

ｔ＞ｔ_ｏｂ’では、障害物５１が写っていないので、０＜ｔ＜ｔ_ｏｂと同じである。

これにより、一台の撮像装置で撮像された共通の画像から算出された複数の位置候補を統合して精度高く現在位置を推定することができる。

なお、上記の例では、共通の画像に対して障害物による影響の受けやすさが異なる２つの処理を適用する例を示したが、これ以外でもよい。例えば、移動体の速度による影響の受けやすさが異なる２つの処理の場合には、移動体の速度に応じていずれかの処理結果を優先させるようにしてもよい。あるいは、外れ値の個数による影響の受けやすさが異なる２つの処理の場合には、外れ値の個数に応じていずれかの処理結果を優先させるようにしてもよい。さらに、これらを組合わせてもよい。

また、処理数は２以上であればいくつでもよい。

１ 位置推定装置
１２ 撮像装置
１３ 情報処理装置
１４ 画像処理部
１５ 制御部
１６ メモリ
１７ 表示部
１００ 移動体

Claims (4)

1. 移動体の位置推定装置であって、
   前記移動体に取り付けられた撮像装置と、
   前記撮像装置で撮像された画像に基づいて前記移動体の複数の位置候補を特定し、前記複数の位置候補と前記移動体の移動速度とに基づいて前記移動体の位置を推定する画像処理部と、を有し、
   前記移動体に取り付けられた撮像装置を複数有し、
   前記画像処理部は、各撮像装置の画像に対してそれぞれ前記移動体の位置候補を特定し、
   前記画像処理部は、前記複数の位置候補に、それぞれの画像を撮影した前記撮像装置についての特徴点トラッキングを可能とするための前記移動体の移動速度の上限である最大移動速度に対する前記移動体の移動速度に応じた重み付けをして、前記移動体の位置を推定する、
   位置推定装置。
2. 前記画像処理部は、前記移動体の移動速度が、前記撮像装置についての前記最大移動速度以上である場合に撮影された画像により特定された前記位置候補に対する重み付けの重みを０とする
   請求項１に記載の位置推定装置。
3. 移動体の位置推定装置が行う方法であって、
   画像処理部が、
   前記移動体に取り付けられた撮像装置で撮像された画像に基づいて前記移動体の複数の位置候補を特定する第１ステップと、
   前記複数の位置候補と前記移動体の移動速度とに基づいて前記移動体の位置を推定する第２ステップと、を行い、
   前記第１ステップでは、前記移動体に取り付けられた複数の撮像装置の各撮像装置の画像に対してそれぞれ前記移動体の位置候補を特定し、
   前記第２ステップでは、前記複数の位置候補に、それぞれの画像を撮影した前記撮像装置についての特徴点トラッキングを可能とするための前記移動体の移動速度の上限である最大移動速度に対する前記移動体の移動速度に応じた重み付けをして、前記移動体の位置を推定する、
   方法。
4. 前記第２ステップでは、前記移動体の移動速度が、前記撮像装置についての前記最大移動速度以上である場合に撮影された画像により特定された前記位置候補に対する重み付けの重みを０とする
   請求項３に記載の方法。

Images