(57)【要約】【課題】移動体の現在位置を精度良く推定する。【解決手段】  移動体の位置推定装置１は、移動体１００に取り付けられた撮像装置１２と、撮像装置１２で撮像された画像に基づいて移動体１００の複数の位置候補を特定し、複数の位置候補と移動体１００の移動速度とに基づいて移動体１００の位置を推定する画像処理部１４とを有する。【選択図】図１
2018146326.tif 000002
【特許請求の範囲】
【請求項１】
移動体の位置推定装置であって、
前記移動体に取り付けられた撮像装置と、
前記撮像装置で撮像された画像に基づいて前記移動体の複数の位置候補を特定し、前記複数の位置候補と前記移動体の移動速度とに基づいて前記移動体の位置を推定する画像処理部と、を有する位置推定装置。
【請求項２】
前記移動体に取り付けられた撮像装置を複数有し、
前記画像処理部は、各撮像装置の画像に対してそれぞれ前記移動体の位置候補を特定する、請求項１記載の位置推定装置。
【請求項３】
前記画像処理部は、前記複数の位置候補に、それぞれの画像の画角及び前記移動体の移動速度に応じた重み付けをして、前記移動体の位置を推定する、請求項２記載の位置推定装置。
【請求項４】
前記画像処理部は、前記複数の位置候補に、それぞれの画像内の障害物に応じた重み付けをして、前記移動体の位置を推定する、請求項２記載の位置推定装置。
【請求項５】
前記画像処理部は、前記複数の位置候補に、それぞれの画像内の特徴点のうちの外れ値に応じた重み付けをして、前記移動体の位置を推定する、請求項２記載の位置推定装置。
【請求項６】
前記画像内の障害物の画素数または前記画像内の特徴点のうちの外れ値数に応じて、前記移動体に対して移動速度に関する指令を出力する制御部をさらに有する、請求項１記載の位置推定装置。
【請求項７】
前記画像処理部は、共通の画像に対して異なる処理をして前記複数の位置候補を特定する、請求項１記載の位置推定装置。
【請求項８】
移動体の位置推定装置が行う方法であって、
画像処理部が、
前記移動体に取り付けられた撮像装置で撮像された画像に基づいて前記移動体の複数の位置候補を特定するステップと、
前記複数の位置候補と前記移動体の移動速度とに基づいて前記移動体の位置を推定するステップと、を行う方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットや自動車などの移動体の位置を推定する技術に関する。
【背景技術】
【０００２】
ロボット及び自動車などの移動体がその周囲の情報を収集し、移動体の現在位置及び走行状態を推定し、移動体の走行を制御する自律走行技術及び運転支援技術が開発されている。移動体の周囲の情報検出のためには様々なセンサが用いられる。一般に、周囲の情報を計測するためのセンサとしては、カメラなどの撮像装置、レーザセンサ、ミリ波レーダなどがある。移動体の位置を測定するためのセンサとしては、ＧＰＳ（Ｇｌｏｂａｌ  Ｐｏｓｉｔｉｏｎｉｎｇ  Ｓｙｓｔｅｍ）またはＩＭＵ（Ｉｎｅｒｔｉａｌ  Ｍｅａｓｕｒｅｍｅｎｔ  Ｕｎｉｔ）が用いられる。
【０００３】
自律走行制御においては、移動体に搭載された制御装置が、例えばＩＭＵで算出した移動体の速度または角速度を積算したり、ＧＰＳ測位を用いたりして、移動体自身の位置（自己位置）を推定する。さらに、制御装置は、レーザセンサあるいはカメラなどで周囲の情報を収集し、位置推定の際の基準となる路面ペイントまたは標識のようなランドマーク等を検出する。そして、制御装置が、検出されたランドマークの位置を地図情報と比較することにより、移動体の現在位置を修正する。
【０００４】
また、駐車場やトンネルのような環境では、地図情報とランドマークがなく、ＧＰＳも使えない場合があるため、移動体の周囲に存在する物体との相対位置を推定するオドメトリ法が用いられる。
【０００５】
例えば、特許文献１は、第１の自車位置推定部により自車の周囲環境の画像に基づき地上固定座標系上での自車の位置と姿勢の推定値及び推定精度を推定する。第２の自車位置推定部により自車の操作量及び運動状態に基づき推定される自車位置の変化量を積算することで基準点及び方位と自車位置との相対的な位置関係を推定し、目標走行経路上で第１の自車位置推定部による位置推定の難度が高い区間を推定する。難度が高いと推定された区間の手前側領域内で周囲環境撮影部の撮影範囲に入る可能性が高いランドマークを地図情報から検出する。検出されたランドマークに基づき位置推定難度の高い区間を走行中、自車位置推定切替部により第１の自車位置推定部から第２の自車位置推定部に切り替える。位置推定難度の高い区間を走行中、自車位置推定切替部の結果に基づく第２の自車位置推定部の出力に基づき自車の走行制御を実行し、位置推定難度の高い区間以外の区間では自車位置推定切替部の結果に基づく第１の自車位置推定部の出力に基づき自車の走行制御を実行する。
【０００６】
位置推定難度の高い区間を走行中、第１の自車位置推定部から第２の自車位置推定部に切り替え、位置推定難度の高い区間を走行中、自車位置推定切替部の結果に基づく第２の自車位置推定部の出力に基づき自車の走行制御を実行する。位置推定難度の高い区間以外の区間では自車位置推定切替部の結果に基づく第１の自車位置推定部の出力に基づき自車の走行制御を実行する。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特開２０１４−３４２５１
【発明の概要】
【発明が解決しようとする課題】
【０００８】
ところで、移動体は移動及び停止を繰り返すので、移動体が観察する周囲の状況はめまぐるしく変化する。移動体の自律走行技術または運転支援技術において、どのような状況であっても精度良く現在位置を推定することが重要である。
【課題を解決するための手段】
【０００９】
本発明の一つの実施態様によれば、移動体の位置推定装置は、前記移動体に取り付けられた撮像装置と、前記撮像装置で撮像された画像に基づいて前記移動体の複数の位置候補を特定し、前記複数の位置候補と前記移動体の移動速度とに基づいて前記移動体の位置を推定する画像処理部と、を有する。
【図面の簡単な説明】
【００１０】
【図１】本発明の実施形態に係る移動体の位置推定装置の構成図
【図２】画像処理部１４が行う画像処理手順を示すフローチャート
【図３】撮像装置１２の撮像範囲から定まる移動体１００の最大移動速度の説明図
【図４】撮影されたビデオ画像の中の一つのフレーム画像の一例
【図５】速度による重み付けの具体例
【図６】障害物による重み付けの実施例。
【図７】制御部１５の制御により移動体１００の速度が変化する例
【図８】撮像装置１２が一台の場合の例
【発明を実施するための形態】
【００１１】
以下、本発明の実施形態に係る移動体の位置推定装置について、図面を用いて説明する。
【００１２】
図１は、本発明の一実施形態に係る位置推定装置１の構成図である。
【００１３】
位置推定装置１は、自動車またはロボットなどの移動体１００に搭載されている。位置推定装置１は、一台以上の撮像装置１２（１２ａ、１２ｂ、・・・１２ｎ）と、情報処理装置１３とを有する。
【００１４】
撮像装置１２は、例えばスチルカメラまたはビデオカメラでよい。撮像装置１２は単願カメラまたはステレオカメラでもよい。
【００１５】
情報処理装置１３は、撮像装置１２で撮像された画像を処理して移動体１００の位置または移動量を算出する。情報処理装置１３は、算出された位置または移動量に応じた表示を行ってもよく、または移動体１００の制御に関する信号を出力してもよい。
【００１６】
情報処理装置１３は、例えば一般的なコンピュータであって、撮像装置１２によって撮像された画像を処理する画像処理部１４と、画像処理結果に基づく処理を行う制御部（ＣＰＵ）１５と、メモリ１６と、ディスプレイなどの表示部１７と、これら構成要素を相互に接続するバス１８とを有する。情報処理装置１３は、画像処理部１４及び制御部１５が所定のコンピュータプログラムを実行することにより、以下の処理を行ってもよい。
【００１７】
撮像装置１２ａは、例えば、移動体１００の前方に設置されている。撮像装置１２ａのレンズは移動体１００の前方に向けられている。撮像装置１２ａは、例えば、移動体１００の前方の遠景を撮像する。他の撮像装置１２ｂ、・・・撮像装置１２ｎは、撮像装置１２ａと異なる位置に設置され、撮像装置１２ａと異なる撮像方向または領域を撮像する。撮像装置１２ｂは、例えば、移動体１００の後方で下方に向けて設置されていてもよい。撮像装置１２ｂは、移動体１００後方の近景を撮像するものでよい。
【００１８】
撮像装置１２が単眼カメラの場合、路面が平らであれば、画像上のピクセル位置と実際の位置関係（ｘ、ｙ）が一定になるため、撮像装置１２から特徴点までの距離を幾何学的に計算できる。撮像装置１２がステレオカメラの場合、画像上の特徴点までの距離をより正確に計測できる。以下の説明では、単願の標準レンズを有するカメラを採用した事例について説明するが、これ以外のカメラ（広角レンズを有するカメラまたはステレオカメラなど）でもよい。
【００１９】
また、撮像装置１２ａ、撮像装置１２ｂ・・・撮像装置１２ｎが、ある時刻で撮像する対象物は、それぞれ互いに異なるものでよい。例えば、撮像装置１２ａは、移動体１００の前方の遠景を撮像するものでよい。この場合、遠景を撮像した画像からは、障害物、または位置推定のためのランドマークなどの特徴点が抽出されるようにしてもよい。撮像装置１２ｂは、移動体１００周辺の路面などの近景を撮像するようにしてもよい。この場合、近景を撮像した画像からは、移動体１００の周囲の白線、または路面ペイントなどが検出されるようにしてもよい。
【００２０】
また、撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎは、同時に雨や日差しなどの環境外乱の影響を受けないような条件で移動体１００に設置されてもよい。例えば、撮像装置１２ａは移動体１００の前方で前向きに設置されるのに対して、撮像装置１２ｂは移動体１００の後方で後ろ向きまたは下向きに設置されてもよい。これにより、例えば、降雨時に撮像装置１２ａのレンズに雨滴が付着した場合でも、進行方向の逆向きまたは下向きの撮像装置１２ｂのレンズには雨滴が付着しにくい。このため、撮像装置１２ａが撮像した画像が雨滴の影響で不鮮明であっても、撮像装置１２ｂが撮像した画像は雨滴の影響を受けにくい。あるいは、日差しの影響で撮像装置１２ａの画像が不鮮明であっても、撮像装置１２ｂが撮像した画像は鮮明である場合がある。
【００２１】
また、撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎは、互いに異なる撮像条件（絞り値、ホワイトバランス、等）で撮影してもよい。例えば、明るい場所用にパラメータを調整した撮像装置と、暗い場所用にパラメータを調整した撮像装置とを搭載することで、環境の明暗によらず撮像可能としてもよい。
【００２２】
撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎは、制御部１５から撮影開始の指令を受けたとき、または一定の時間間隔で画像を撮像してもよい。撮像された画像のデータ及び撮像時刻は、メモリ１６に格納される。
【００２３】
メモリ１６は、情報処理装置１３の主記憶装置（メインメモリ）およびストレージなどの補助記憶装置を含む。
【００２４】
画像処理部１４がメモリ１６に格納された画像データ及び撮像時刻に基づいて、様々な画像処理を行う。この画像処理では、例えば、中間画像が作成されてメモリ１６に保存される。中間画像は、画像処理部１４による処理の他、制御部１５などの判断や処理に利用されてもよい。
【００２５】
バス１８は、ＩＥＢＵＳ（Ｉｎｔｅｒ  Ｅｑｕｉｐｍｅｎｔ  Ｂｕｓ）やＬＩＮ（Ｌｏｃａｌ  Ｉｎｔｅｒｃｏｎｎｅｃｔ  Ｎｅｔｗｏｒｋ）やＣＡＮ（Ｃｏｎｔｒｏｌｌｅｒ  Ａｒｅａ  Ｎｅｔｗｏｒｋ）などで構成できる。
【００２６】
画像処理部１４は、撮像装置１２で撮像された画像に基づいて複数の移動体の位置候補を特定し、その複数の位置候補と移動体１００の移動速度とに基づいて移動体１００の位置を推定する。
【００２７】
画像処理部１４は、例えば、移動体１００の走行中に撮像装置１２が撮像した画像を処理して、移動体１００の位置を推定する。例えば、画像処理部１４は、撮像装置１２が撮像したビデオ画像で移動体１００の移動量を算出し、スタート地点に移動量を加算して現在位置を推定してもよい。
【００２８】
画像処理部１４は、ビデオ画像の各フレーム画像で特徴点を抽出してもよい。画像処理部１４は、さらに、次以降のフレーム画像で同じ特徴点を抽出する。そして、画像処理部１４は、特徴点のトラッキングにより移動体１００の移動量を算出してもよい。
【００２９】
制御部１５は、画像処理部１４の画像処理の結果に基づいて、移動体１００に対して移動速度に関する指令を出力してもよい。例えば、制御部１５は、画像内の障害物の画素数、画像内の特徴点のうちの外れ値の数または画像処理の種類等に応じて、移動体１００の移動速度を増加させる指令、減少させる指令または維持させる指令を出力してもよい。
【００３０】
図２は、画像処理部１４が行う画像処理手順を示すフローチャートある。
【００３１】
画像処理部１４は、複数の移動体位置候補算出処理２１（２１ａ、２１ｂ、・・・２１ｎ）と、複数の移動体位置候補から移動体位置を推定する推定処理２６とを行う。
【００３２】
複数の移動体位置候補算出処理２１（２１ａ、２１ｂ、・・・２１ｎ）は、互いに並列に実行されてもよい。例えば、移動体位置候補算出処理２１ａでは、撮像装置１２ａによって撮像された画像が処理されてもよい。同様に、移動体位置候補算出処理２１ｂ、・・・２１ｎでは、撮像装置１２ｂ、・・・撮像装置１２ｎによって撮像された画像が処理されてもよい。これにより、画像処理部１４は、各撮像装置１２が撮像した画像からそれぞれの移動体１００の位置候補を特定してもよい。
【００３３】
また、撮像装置１２ａが撮像した画像が前方の遠景であるときは、移動体位置候補算出処理２１ａでは移動体１００の推進方向を推定しやすい遠方特徴点を用いて推進方向が推定されてもよい。撮像装置１２ｂが撮像した画像が後方の近景であるときは、移動体位置候補算出処理２１ｂでは移動体１００の移動量を推定しやすい近傍特徴点を用いて移動量が推定されてもよい。
【００３４】
図示するように、複数の移動体位置候補算出処理２１（２１ａ、２１ｂ、・・・２１ｎ）は共通のステップを実行する。以下、各ステップについて説明する。
【００３５】
画像処理部１４は、メモリ１６から画像データを取得する（Ｓ２２ａ、Ｓ２２ｂ、・・・Ｓ２２ｎ）。ここで取得する画像データは複数のフレーム画像のデータでよい。
【００３６】
次に、画像処理部１４は、取得した各フレーム画像内の特徴点を抽出する（Ｓ２３ａ、Ｓ２３ｂ、・・・Ｓ２３ｎ）。特徴点は、例えば、画像内のエッジやコーナーなどでよい。特徴点の抽出には、Ｃａｎｎｙ、Ｓｏｂｅｌ、ＦＡＳＴ、Ｈｅｓｓｉａｎ、Ｇａｕｓｓｉａｎなどの技術を用いてもよい。具体的なアルゴリズムは、画像の特徴に応じて適宜選択される。
【００３７】
画像処理部１４は、フレーム画像の時系列に従って、各フレーム画像でそれぞれ抽出された特徴点をトラッキングする（Ｓ２４ａ、Ｓ２４ｂ、・・・Ｓ２４ｎ）。トラッキングにはＬｕｃａｓ−Ｋａｎａｄｅ法やＳｈｉ−Ｔｏｍａｓｉ法やＤｉｒｅｃｔ  Ｍａｔｃｈｉｎｇ法などの技術を用いてもよい。具体的なアルゴリズムは、画像の特徴に応じて適宜選択される。
【００３８】
画像処理部１４は、特徴点トラッキングの結果に基づいて、移動体１００の移動量を算出して、移動体１００の現在位置の候補を算出する（Ｓ２５ａ）。移動量の算出には、例えばＲｉｇｉｄ  Ｂｏｄｙ  Ｔｒａｎｓｆｏｒｍａｔｉｏｎ、Ｓｌｉｄｉｎｇ  Ｗｉｎｄｏｗ、基礎行列、中央値などを利用することができる。また、中央値フィルタ、ＲＡＮＳＡＣ、バンドル調整、キーフレーム調整等の外れ値検出と最適化処理をしてもよい。
【００３９】
複数の移動体位置候補算出処理２１ａ、２１ｂ、・・・２１ｎでそれぞれ異なるビデオ画像を対象とした処理が行われることで、複数の位置候補が算出される。
【００４０】
ステップＳ２６の位置推定処理は、それぞれの移動体位置候補算出処理２１ａ、２１ｂ、・・・２１ｎによって算出された位置候補を統合して、移動体１００の現在位置を推定する。位置推定処理の詳細は後述する。
【００４１】
図３を用いて撮像装置１２の撮像範囲から定まる移動体１００の最大移動速度について説明する。
【００４２】
同図は、移動体１００に取り付けられた撮像装置１２を模式的に示す側面図である。つまり、撮像装置１２は路面３０から高さＨの位置に、光軸が路面３０に対して角度αとなるように設置されている。さらに、撮像装置１２の画角がγ、撮像装置１２から路面までの最大撮像距離がＤ、最大撮像範囲がＲである。
【００４３】
ここで、ビデオ画像で特徴点のトラッキングを行うためには、少なくとも時間的に隣り合う２枚のフレーム画像に共通の特徴点が写っている必要がある。従って、フレーム間の時間間隔Δｔでの移動体１００の移動距離がＲ以下であることが必要である。
【００４４】
また、撮像装置１２が撮像するビデオ画像のフレームレートをＦＰＳとすると、フレーム間の時間間隔Δｔは式（１）で表される。
Δｔ＝２／ＦＰＳ  （１）
【００４５】
従って、移動体１００の移動中に撮像されたビデオ画像で特徴点トラッキングを可能とするためには、移動体１００の移動速度に上限がある。その上限である最大移動速度Ｖｍａｘは以下の式（２）で定まる。
Ｖｍａｘ ＝  Ｒ／Δｔ ＝  ［Ｄｃｏｓ（α）−Ｈ／ｔａｎ（α＋γ）］ＦＰＳ／２  （２）
【００４６】
次に、特徴点トラッキングの精度を低下させる要因について説明する。
【００４７】
移動体１００が最大移動速度Ｖｍａｘ以下で走行している場合であっても、移動体１００の移動速度が増すほどトラッキングの精度が低下し、誤差が生じやすくなる。そこで、画像処理部１４が複数の移動体位置候補算出処理２１ａ、２１ｂ、・・・２１ｎで算出された位置候補を用いて現在位置を推定する際には、ステップＳ２６において移動体１００の移動速度に応じた位置推定を行っても良い。例えば、画像処理部１４は、速度による重み係数Ｗｖを用いて、位置推定を行う。速度による重み係数Ｗｖは、例えば、以下の式（３）で定まる。
Ｗｖ  ＝  １−Ｖ／Ｖｍａｘ（３）
【００４８】
また、撮影された画像の特性に応じて特徴点トラッキングの精度が低下する場合がある。
【００４９】
図４は、撮影されたビデオ画像の中の一つのフレーム画像Ｉの一例を示す。
【００５０】
同図は、画像３１内に障害物３４が写っている例を示す。障害物３４は特徴点抽出の対象とはならない。従って、画像内で障害物３４が占める面積が大きくなるほど画像３１から得られる情報量が減少する。つまり、障害物３４が占める面積が大きくなると画像３１から検出される特徴点数も減り、トラッキングの精度が低下する。そこで、画像処理部１４は、ステップＳ２６で障害物の面積に応じた位置推定をしてもよい。例えば、画像処理部１４は、障害物による重み係数Ｗｏｂを用いて位置推定を行ってもよい。
【００５１】
ここで、画像３１のｕ軸の画素数をＰｕ、ｖ軸の画素数をＰｖとする。さらに、障害物３４が占める面積の画素数をＡ（Ａ１、Ａ２、Ａ３）とする。このとき、障害物による重み係数Ｗｏｂは、例えば、以下の式（４）で定まる。なお、式（４）は一例であり、式（４）以外の障害物による重み係数を用いてもよい。
Ｗｏｂ  ＝  １−（Ａ１＋Ａ２＋・・・＋Ａｋ）／（Ｐｕ・Ｐｖ）（４）
【００５２】
また、図４の画像３１内にはさらに複数の特徴点３６、３７があり、これが検出されている。同図の例では、特徴点には、有効な特徴点（インライア）３６と有効でない特徴点である外れ値（アウトライア）３７とを含む。画像処理部１４が特徴点３６、３７を検出した後、所定の外れ値検出処理により外れ値３７を特定してもよい。
【００５３】
ここで、外れ値３７が多いほど、特徴点トラッキングに使える特徴点が少なくなり、トラッキングの精度が低下する。そこで、画像処理部１４は、ステップ２６で画像３１内の特徴点のうちの外れ値３７に応じた位置推定を行っても良い。例えば、画像処理部１４は、外れ値による重み係数Ｗｆｐを用いて位置推定を行う。画像３１における有効な特徴点の個数をｉ、外れ値の個数をｏとすると、外れ値による重み係数Ｗｆｐは、例えば、以下の式（５）で定まる。なお、式（５）は一例であり、式（５）以外の外れ値による重み係数を用いてもよい。
Ｗｆｐ  ＝  １−ｏ／（ｏ＋ｉ）（５）
【００５４】
以上説明したように、一つのビデオ画像に対して、特徴点トラッキングの精度を低下させる要因が複数ある。そして、上述のように、各フレーム画像Ｉに対して、速度による重み係数Ｗｖ、障害物に関する重み係数Ｗｏｂ及び特徴点の外れ値に関する重み係数Ｗｆｐのそれぞれが算出可能である。画像処理部１４は、ステップＳ２６において、これらの１つ以上を用いることができる。例えば、画像処理部１４は、これらのうちの１つの重み係数を個別に用いてもよいし、２つ以上の重み係数を統合して、各フレーム画像Ｉに対して一つの重み係数Ｗを特定してもよい。ある時刻ｔのフレーム画像Ｉ（ｔ）に対する重み係数Ｗ（ｔ）は、同じ時刻の速度による重み係数Ｗｖ（ｔ）、障害物に関する重み係数Ｗｏｂ（ｔ）、及び特徴点の外れ値に関する重み係数Ｗｆｐ（ｔ）に基づいて、例えば、以下の式（６）で定まる。
【００５５】
Ｗ（ｔ）＝ａｖｅｒａｇｅ（Ｗｖ（ｔ），Ｗｏｂ（ｔ），Ｗｆｐ（ｔ））  （６）
【００５６】
上述のように、複数の撮像装置１２で撮像した画像に基づき複数の位置候補が算出される。従って、画像処理部１４は、ステップＳ２６において複数の自車位置の候補から自車位置を推定する。例えば、画像処理部１４は、ある時刻ｔの自車位置Ｐｏｓ（ｔ）を推定するために、その時刻のフレーム画像Ｉ（ｔ）に対する重み係数Ｗ（ｔ）を用いる。ある時刻ｔの自車位置Ｐｏｓ（ｔ）は、例えば、式（７）で算出されてもよい。
【００５７】
Ｐｏｓ（ｔ）＝（Ｐａ（ｔ）Ｗａ（ｔ）＋Ｐｂ（ｔ）Ｗｂ（ｔ）＋・・・＋Ｐｎ（ｔ）Ｗｎ（ｔ））／（Ｗａ（ｔ）＋Ｗｂ（ｔ）＋・・・Ｗｎ（ｔ））  （７）
【００５８】
これにより、複数の画像からそれぞれ算出された現在位置候補を統合して移動体１００の現在位置を推定できる。
【００５９】
次に、速度による重み付けの具体例を示す。
【００６０】
図５Ａは、撮像装置１２ａ及び撮像装置１２ｂを搭載した移動体１００の一例を示す。
【００６１】
撮像装置１２ａは、移動体１００の前方に向けて設置され、遠景を撮像する撮像装置である。撮像装置１２ａは撮像範囲ＲＳを有する。
【００６２】
撮像装置１２ｂは、移動体１００の後方で下方に向けて設置され、近景を撮像する撮像装置である。撮像装置１２ｂは撮像範囲ＲＡを有する。
【００６３】
図５Ｂは、時刻ｔと自車速度Ｖの関係を表すグラフである。
【００６４】
速度ＶｍａｘＳは、撮像装置１２ａで撮像した画像で特徴点トラッキングが可能な最高速度であり、速度ＶｍａｘＡは、撮像装置１２ｂで撮像した画像で特徴点トラッキングが可能な最高速度である。
【００６５】
図５Ｃは、時刻ｔと速度による重み係数Ｗｖの関係を表すグラフである。時刻ｔＳは、移動体１００の速度がＶｍａｘＳになる時刻である。時刻ｔＡは、移動体１００の速度がＶｍａｘＡになる時刻である。
【００６６】
重みＷｖＳは撮像装置１２ａの画像に対する自車速度Ｖによる重みである。重みＷｖＡは撮像装置１２ｂの画像に対する自車速度Ｖによる重みである。
【００６７】
ｔ＝０のとき、移動体１００は止まっているため、式（３）に従い、重み係数ＷｖＳ及びＷｖＡはいずれも１である。
【００６８】
ｔ＞０のとき、移動体１００が移動している。従って速度がＶ＞０であり、式（３）によればＷｖＳ＜１、ＷｖＡ＜１になる。
【００６９】
ｔ＝ｔＡになると、移動体１００の移動速度がＶ＝ＶｍａｘＡになるので、式（３）に従いＷｖＡ＝０となる。ｔ＞ｔＡにおいて、ＷｖＡ＝０である。
【００７０】
同様に、ｔ＝ｔＳになると、移動体１００の移動速度がＶ＝ＶｍａｘＳになるので、式（３）に従いＷｖＳ＝０となる。ｔ＞ｔＳにおいて、ＷｖＳ＝０である。
【００７１】
次に、障害物による重み付けの具体例を示す。
【００７２】
図６Ａは、図５Ａと同様に撮像装置１２ａ及び撮像装置１２ｂを搭載した移動体１００の一例を示す。この例ではさらに、撮像装置１２ａの撮像範囲に障害物５１が存在し得る。
【００７３】
図６Ｂは、図５Ｂと同様に、時刻ｔと自車速度Ｖの関係を表すグラフである。
【００７４】
図６Ｃは、時刻ｔと速度による重み係数Ｗｖ及び障害物による重み係数Ｗｏｂとの関係を表すグラフである。
【００７５】
速度による重み係数Ｗｖは、図５Ｃの場合と同じである。
【００７６】
時刻ｔｏｂは障害物５１が撮像範囲ＲＳに入り、撮像される画像に写り始める時刻である。時間ｔｏｂ’は、障害物５１が撮像範囲ＲＳから外れ、撮像される画像に写らなくなる時刻である。
【００７７】
０＜ｔ＜ｔｏｂのとき、撮像装置１２ａの撮像範囲ＲＳに障害物５１はないため、Ｗｏｂｓ＝１である。
【００７８】
ｔ＝ｔｏｂになると、障害物５１は撮像装置１２ａの撮像範囲ＲＳに入るため、式（４）に従ってＷｏｂｓ＜１になる。ここでは簡単のために画像全体の画素数に占める障害物の画素数は一定とする。
【００７９】
ｔ＝ｔｏｂ’になると、障害物５１が撮像装置１２ａの撮像範囲ＲＳから外れ、画像に写らなくなるため、式（４）によってＷｏｂｓ＝１になる。
【００８０】
このように、障害物が映り込むと障害物による重み係数Ｗｏｂが低下し、その画像による位置推定の精度が低下する。そこで、制御部１５が画像処理部１４の処理結果に応じて移動体１００を制御するようにしてもよい。図６にその一例を示す。ここでは、画像処理部１４が障害物を検出すると、制御部１５の指令により移動体１００の速度が一定に保たれる例を示す。ここでは、障害物の検出をトリガーとしているが、これ以外のトリガーであってもよい。例えば、特徴点のうち外れ値数の割合が所定以上であることをトリガーとしてもよい。
【００８１】
図７は、図６と同じ状況下で障害物の検出をトリガーとする制御部１５の制御により、移動体１００の速度が変化する例を示す。
【００８２】
図７Ａは、図５Ｂ、図６Ｂと同様に、時刻ｔと自車速度Ｖの関係を表すグラフである。
【００８３】
０＜ｔ＜ｔｏｂのとき、移動体１００が加速する。ここでは移動体１００の速度Ｖ＞０であるので、式（３）によりＷｖＳ＜１、ＷｖＡ＜１になる。
【００８４】
ｔ＝ｔｏｂになると、撮像装置１２ａの撮像範囲ＲＳの画像に障害物５１が入り、画像処理部１４がこれを検出して、制御部１５へ通知する。この通知を行う基準は、例えば、障害物５１の画素数が所定以上でもよいし、画像全体の画素数に占める障害物５１の画素数の割合が所定以上でもよい。制御部１５は、この通知を受けると、そのときの速度を維持する指令を出力する。これにより、移動体１００の速度が維持されるので、速度による重み係数Ｗｖも維持される。なお、移動体の速度を維持する代わりに、移動体の速度を低下させてもよい。
【００８５】
時刻ｔ＝ｔｏｂ’になると、撮像装置１２ａの撮像範囲ＲＳの画像から障害物５１が消える。画像処理部１４がこれを検出して、制御部１５へ通知する。この通知を行う基準は、例えば、障害物５１の画素数が所定以下でもよいし、画像全体の画素数に占める障害物５１の画素数の割合が所定以下でもよい。制御部１５は、この通知を受けると、それまでの速度維持を解除する指令を出力する。これにより、移動体１００は再び加速する。速度の増加に伴って、速度による重み係数も増加する。
【００８６】
これにより、移動体１００が観察する周囲の状況に応じて、精度良く現在位置を推定できる。特に、障害物が検出されているときは速度を一定に保つかまたは減少させ、速度上昇による位置推定の精度の更なる低下を防止する。
【００８７】
次に、図８は、撮像装置１２が一台の場合の例を示す。
【００８８】
図８Ａは、一台の撮像装置１２ａを搭載した移動体１００の一例を示す。この例では、撮像装置１２ａが移動体１００の前方に取り付けられている例で説明するが、撮像装置の取付位置は前方に限らない。
【００８９】
撮像装置１２が複数台あるときは、各撮像装置１２が撮像した画像別に移動体位置候補算出処理２１を適用して画像ごとに位置候補を算出したのに対して、撮像装置１２が一台の場合は、共通の画像に対して異なる移動体位置候補算出処理２１を適用する。
【００９０】
以下の説明では、撮像装置１２ａが撮像したビデオ画像に移動体位置候補算出処理２１ａ及び移動体位置候補算出処理２１ｂの２種類の処理が適用され、これらの処理の精度による重み付けを行う例を示す。ここで、移動体位置候補算出処理２１ａは移動体位置候補算出処理２１ｂと比べると、位置推定の精度は高いが、障害物の影響を受けやすい、ロバストでない処理である。移動体位置候補算出処理２１ｂは移動体位置候補算出処理２１ａと比べると、位置推定にあたり障害物の影響を受けにくいが、位置推定の精度は低いロバストな処理である。
【００９１】
図６と同様に、時刻ｔｏｂに障害物５１が撮像範囲ＲＳに入り、時間ｔｏｂ’に障害物５１が撮像範囲ＲＳから外れる。ここでは簡単のために画像全体の画素数に占める障害物５１の画素数は一定とする。
【００９２】
図８Ｂは、時刻ｔと、移動体位置候補算出処理２１ａの精度との関係を表す。図８Ｃは、時刻ｔと、移動体位置候補算出処理２１ｂの精度との関係を表す。つまり、移動体位置候補算出処理２１ａ、２１ｂは、画像の特徴に応じて現在位置候補算出の精度が変化する。
【００９３】
ここで、０＜ｔ＜ｔｏｂのときは、撮像装置１２ａの撮像範囲ＲＳに障害物５１が写っていないため、移動体位置候補算出処理２１ａの精度Ｐｒ１と移動体位置候補算出処理２１ｂの精度Ｐｒ２はともに高い。
【００９４】
この場合、画像処理部１４はステップＳ２６において、式（８）で示すように、移動体位置候補算出処理２１ａで推定された位置Ｐ１と移動体位置候補算出処理２１ｂで推定された位置Ｐ２のいずれか一方を推定位置Ｐｏｓ（ｔ）としてもよい。
Ｐｏｓ（ｔ）＝Ｐ１、ｉｆ  Ｐｒ１＞Ｐｒ２
Ｐｏｓ（ｔ）＝Ｐ２、ｉｆ  Ｐｒ１＜Ｐｒ２  （８）
【００９５】
あるいは、画像処理部１４はステップＳ２６において、式（９）に示すように、移動体位置候補算出処理２１ａで推定された位置Ｐ１と移動体位置候補算出処理２１ｂで推定された位置Ｐ２に、移動体位置候補算出処理２１ａと移動体位置候補算出処理２１ｂの精度に応じた重み付けをした平均位置を算出して、これを推定位置Ｐｏｓ（ｔ）としてもよい。
Ｐｏｓ（ｔ）＝（Ｐ１・Ｗ１＋Ｐ２・Ｗ２）／（Ｗ１＋Ｗ２）  （９）
Ｗ１：Ｐｒ１に応じた重み係数
Ｗ２：Ｐｒ２に応じた重み係数
【００９６】
ｔｏｂ＜ｔ＜ｔｏｂ’では、障害物５１が撮像装置１２ａの撮像範囲ＲＳに写っているため、ステップＳ２３ａ、ステップＳ２３ｂで抽出できる特徴点が減少する。これにより、移動体位置候補算出処理２１ａの精度がＰｒ１’に、移動体位置候補算出処理２１ｂの精度がＰｒ２’にそれぞれ低下する。そして、ロバストでない移動体位置候補算出処理２１ａがロバストである移動体位置候補算出処理２１ｂよりも大きな影響を受け、Ｐｒ２’＞Ｐｒ１’となる。
【００９７】
この場合、画像処理部１４はステップＳ２６で、より精度の高い移動体位置候補算出処理２１ｂで算出された位置候補Ｐ２を推定位置Ｐｏｓ（ｔ）としてもよい。
【００９８】
Ｐｏｓ（ｔ）＝  Ｐ２  （１０）
【００９９】
ｔ＞ｔｏｂ’では、障害物５１が写っていないので、０＜ｔ＜ｔｏｂと同じである。
【０１００】
これにより、一台の撮像装置で撮像された共通の画像から算出された複数の位置候補を統合して精度高く現在位置を推定することができる。
【０１０１】
なお、上記の例では、共通の画像に対して障害物による影響の受けやすさが異なる２つの処理を適用する例を示したが、これ以外でもよい。例えば、移動体の速度による影響の受けやすさが異なる２つの処理の場合には、移動体の速度に応じていずれかの処理結果を優先させるようにしてもよい。あるいは、外れ値の個数による影響の受けやすさが異なる２つの処理の場合には、外れ値の個数に応じていずれかの処理結果を優先させるようにしてもよい。さらに、これらを組合わせてもよい。
【０１０２】
また、処理数は２以上であればいくつでもよい。
【符号の説明】
【０１０３】
１  位置推定装置
１２  撮像装置
１３  情報処理装置
１４  画像処理部
１５  制御部
１６  メモリ
１７  表示部
１００  移動体
【図１】
2018146326.tif 000003
【図２】
2018146326.tif 000004
【図３】
2018146326.tif 000005
【図４】
2018146326.tif 000006
【図５】
2018146326.tif 000007
【図６】
2018146326.tif 000008
【図７】
2018146326.tif 000009
【図８】
2018146326.tif 000010