特許 7577600 ３次元マップ推定装置および障害物検出装置

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2024-10-25

(45)【発行日】2024-11-05

(54)【発明の名称】３次元マップ推定装置および障害物検出装置

(51)【国際特許分類】

   G01B 11/22 20060101AFI20241028BHJP

   G06T 7/70 20170101ALI20241028BHJP

   G08G 1/16 20060101ALI20241028BHJP

【ＦＩ】

G01B11/22 H

G06T7/70 Z

G08G1/16 C

【請求項の数】 16

(21)【出願番号】P 2021071762

(22)【出願日】2021-04-21

(65)【公開番号】P2022166505

(43)【公開日】2022-11-02

【審査請求日】2023-09-07

(73)【特許権者】

【識別番号】000003078

【氏名又は名称】株式会社東芝

(73)【特許権者】

【識別番号】317011920

【氏名又は名称】東芝デバイス＆ストレージ株式会社

(74)【代理人】

【識別番号】100111121

【弁理士】

【氏名又は名称】原 拓実

(74)【代理人】

【識別番号】100200104

【弁理士】

【氏名又は名称】渡邊 実

(72)【発明者】

【氏名】西山 学

【審査官】佐田 宏史

(56)【参考文献】

【文献】特開２０１６－１９７０８３（ＪＰ，Ａ）

【文献】国際公開第２０１７／０５７０５４（ＷＯ，Ａ１）

【文献】国際公開第２０１９／１８８３９１（ＷＯ，Ａ１）

【文献】国際公開第２０１５／０１５５４２（ＷＯ，Ａ１）

【文献】特表２０１５－５０１４７１（ＪＰ，Ａ）

【文献】Danping Zou et al.，"CoSLAM: Collaborative Visual SLAM in Dynamic Environments"，IEEE Transactions on Pattern Analysis and Machine Intelligence，米国，IEEE，2012年05月01日，Vol.35, No.2，pp.354-366

【文献】岩崎 亘典，“広範囲が高解像度に ドローン空撮画像から３Ｄ測量の研究”，Ｉｎｔｅｒｆａｃｅ，日本，ＣＱ出版株式会社，2020年04月01日，Vol.46, No.4，pp.130-139

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｇ０１Ｂ １１／００－１１／３０

Ｇ０６Ｔ １／００，７／００－７／９０

Ｇ０６Ｖ １０／００－１０／９８

Ｇ０８Ｇ １／１６

(57)【特許請求の範囲】

【請求項1】

移動体に搭載された、複数の画像を撮影する複数の撮像装置のうち１つ以上の撮像装置を選択する第１選択部と、
前記第１選択部が選択した前記撮像装置の撮影画像に基づいて、前記撮像装置ごとに位置姿勢推定を行い、前記撮像装置ごとの第１位置姿勢推定結果を出力する第１位置姿勢推定部と、
前記複数の第１位置姿勢推定結果の類似度を求め、前記類似度を基に必要な前記第１位置姿勢推定結果を１つ以上選択する第２選択部と、
１つ以上の前記第１位置姿勢推定結果から、第２位置姿勢推定結果を出力する第２位置姿勢推定部と、
前記第２位置姿勢推定結果と前記画像から前記移動体の周囲の３次元マップを推定する３次元マップ推定部と、
を備える３次元マップ推定装置。

【請求項2】

前記第２選択部は、合計でN個（Nは自然数）の前記第１位置姿勢推定結果から、前記第１位置姿勢推定結果を１個以上、N－１個以下の数だけ選択する、
請求項１に記載の３次元マップ推定装置。

【請求項3】

前記移動体にはセンサがさらに搭載され、前記第１選択部には、前記センサが取得したセンサ情報がさらに入力される請求項１又は２に記載の３次元マップ推定装置。

【請求項4】

前記第１選択部は、前記移動体の周囲の３次元マップを推定する第１のタイミングよりも前の前記第１位置姿勢推定結果と、前記移動体の移動方向と、前記移動体の移動速度と、を含む情報に基づいて前記撮像装置を選択する請求項１乃至３いずれか１つに記載の３次元マップ推定装置。

【請求項5】

前記第１選択部は、前記移動体の周囲の３次元マップを推定する第１のタイミングよりも前に推定された前記３次元マップに基づいて前記撮像装置を選択する、請求項１乃至３いずれか１つに記載の３次元マップ推定装置。

【請求項6】

前記第１選択部は、前記移動体の周囲に存在する他の移動体の位置情報に基づいて前記撮像装置を選択する請求項１乃至３いずれか１つに記載の３次元マップ推定装置。

【請求項7】

前記第２位置姿勢推定部は入力された複数の前記第１位置姿勢推定結果の重み付き和により前記第２位置姿勢推定結果を出力する請求項１乃至６いずれか１つに記載の３次元マップ推定装置。

【請求項8】

前記重み付き和の重み係数は、前記撮像装置の信頼度、または前記第１位置姿勢推定結果の信頼度によって決定される請求項７に記載の３次元マップ推定装置。

【請求項9】

前記３次元マップ推定部は、前記画像と、前記第１位置姿勢推定結果と異なる時刻の画像間の特徴点を用いて、前記画像の３次元マップを推定することを特徴とする請求項１乃至８いずれか１つに記載の３次元マップ推定装置。

【請求項10】

前記３次元マップ推定部は、前記第２位置姿勢推定結果と、前記第１選択部で選択されなかった前記撮像装置が取得した画像の特徴点と、を用いて３次元マップを推定することを特徴とする請求項１乃至８いずれか１つに記載の３次元マップ推定装置。

【請求項11】

前記３次元マップ推定部は、前記第２位置姿勢推定結果と、前記第２選択部で選択されなかった記第１位置姿勢推定結果の算出に用いられた画像の特徴点と、を用いて３次元マップを推定することを特徴とする請求項１に記載の３次元マップ推定装置。

【請求項12】

前記３次元マップ推定部は距離情報を取得可能な外部センサ情報から、前記撮像装置が取得した画像の特徴点と前記移動体との距離を算出し、前記移動体の周囲の３次元マップを推定することを特徴とする請求項１乃至８いずれか１つに記載の３次元マップ推定装置。

【請求項13】

請求項１乃至１２いずれか１つに記載の３次元マップ推定装置で推定された３次元マップを利用して障害物を検出する障害物検出部を有する障害物検出装置。

【請求項14】

請求項９乃至１２いずれか１つに記載の３次元マップ推定装置で推定された３次元マップの前記特徴点と、前記移動体との距離が閾値以下である場合に障害物を検出する障害物検出部を有する障害物検出装置。

【請求項15】

移動体の前後左右にそれぞれ少なくとも一つずつ搭載された、複数の画像を撮影する複数の撮像装置と、
前記撮像装置のうち１つ以上の撮像装置を選択する第１選択部と、
前記第１選択部が選択した前記撮像装置の撮影画像に基づいて、前記撮像装置ごとに位置姿勢推定を行い、前記撮像装置ごとの第１位置姿勢推定結果を出力する第１位置姿勢推定部と、
前記複数の撮像装置のうち一つの前記撮像装置の位置姿勢の変化を、他の前記撮像装置の位置姿勢の変化に変換する座標変換部と、
１つ以上の前記第１位置姿勢推定結果から、第２位置姿勢推定結果を出力する第２位置姿勢推定部と、
前記第２位置姿勢推定結果と前記画像から前記移動体の周囲の３次元マップを推定する３次元マップ推定部と、
を備え、
前記３次元マップ推定部は、前記第２位置姿勢推定結果と、前記第１選択部で選択されなかった前記撮像装置が取得した画像の特徴点と、を用いて３次元マップを推定することを特徴とする、
３次元マップ推定装置。

【請求項16】

移動体に搭載された、複数の画像を撮影する複数の撮像装置のうち１つ以上の撮像装置を選択する第１選択部と、
前記第１選択部が選択した前記撮像装置の撮影画像に基づいて、前記撮像装置ごとに位置姿勢推定を行い、前記撮像装置ごとの第１位置姿勢推定結果を出力する第１位置姿勢推定部と、
前記複数の撮像装置のうち第１の撮像装置の位置姿勢の変化を、前記第１の撮像装置とは視野の重複しない第２の撮像装置の位置姿勢の変化に変換する座標変換部と、
１つ以上の前記第１位置姿勢推定結果から、第２位置姿勢推定結果を出力する第２位置姿勢推定部と、
前記第２位置姿勢推定結果と前記画像から前記移動体の周囲の３次元マップを推定する３次元マップ推定部と、
を備え、
前記３次元マップ推定部は、前記第２位置姿勢推定結果と、前記第１選択部で選択されなかった前記撮像装置が取得した画像の特徴点と、を用いて３次元マップを推定することを特徴とする、
３次元マップ推定装置。

【発明の詳細な説明】

【技術分野】

【0001】

本発明の実施形態は、３次元マップ推定装置および障害物検出装置に関する。

【背景技術】

【0002】

車両等に搭載したカメラを用いて車両等の位置姿勢を推定し、周辺の３次元マップを推定する技術がある。車両等の位置姿勢推定は、例えば、Ｖｉｓｕａｌ ＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ Ａｎｄ Ｍａｐｐｉｎｇ）が使用される。

【0003】

３次元マップの推定には位置姿勢推定の精度が重要である。しかし、カメラが搭載された車両等の動きやカメラに映っているものによって位置姿勢推定の精度が低くなることがある。また、カメラに映るものが遠方にある場合、物体の動きが小さいため、位置姿勢推定に多くのフレームを要することがある。

【先行技術文献】

【特許文献】

【0004】

【文献】特許第４７６０８３１号公報

【発明の概要】

【発明が解決しようとする課題】

【0005】

本発明が解決しようとする課題は、位置姿勢推定の精度が良い３次元マップ推定装置および障害物検出装置を提供することである。

【課題を解決するための手段】

【0006】

実施形態の３次元マップ推定装置は、第１選択部と、第１位置姿勢推定部と、第２選択部と、第２位置姿勢推定部と、３次元マップ推定部と、を備える。第１選択部は、移動体に搭載された、複数の画像を撮影する複数の撮像装置のうち１つ以上の撮像装置を選択する。第１位置姿勢推定部は、第１選択部が選択した撮像装置の撮影画像に基づいて、撮像装置ごとに位置姿勢推定を行い、撮像装置ごとの第１位置姿勢推定結果を出力する。第２選択部は、複数の第１位置姿勢推定結果の類似度を求め、類似度を基に必要な前記第１位置姿勢推定結果を１つ以上選択する。第２位置姿勢推定部は、１つ以上の第１位置姿勢推定結果から、第２位置姿勢推定結果を出力する。３次元マップ推定部は、第２位置姿勢推定結果と画像から移動体の周囲の３次元マップを推定する。

【図面の簡単な説明】

【0007】

【図1】第１の実施形態に係る３次元マップ推定装置１００、及び障害物検出装置２００の機能を示すブロック図。

【図2】車両１に取り付けられたカメラ１０の配置について説明する上面図。

【図3】第１の実施形態に係る３次元マップ推定装置１００、及び障害物検出装置２００の機能を示すフローチャート。

【図4】Ｖｉｓｕａｌ ＳＬＡＭを用いた、時刻ｔ１における位置姿勢を推定する方法を示す図。

【図5】（ａ）障害物２のある環境の一例を示す図。（ｂ）図５（ａ）に示した環境下で車両１を走行させた時の、車両１の走行軌跡の一例を示す図。

【図6】第１の実施形態に係る３次元マップ推定装置１００、及び障害物検出装置２００の実装例を示す図。

【発明を実施するための形態】

【0008】

以下、本発明の実施形態について、図面を参照しながら説明する。この説明に際し、全図にわたり、共通する部分には共通する符号を付す。なお、本実施形態は、本発明を限定するものではない。

【0009】

［第１の実施形態］
（３次元マップ推定装置１００、及び障害物検出装置２００の機能構成の例）
第１の実施形態に係る３次元マップ推定装置１００、及び障害物検出装置２００について、図１を参照して説明する。図１は、第１の実施形態に係る３次元マップ推定装置１００、及び障害物検出装置２００の機能を示すブロック図である。

【0010】

３次元マップ推定装置１００は、自動車等の移動体の内外に搭載されたカメラ１０、およびカメラ以外のセンサ２０から移動体の周辺部の３次元マップを推定する。障害物検出装置２００は、３次元マップ推定装置１００で推定された３次元マップを基に、障害物２を検出する。３次元マップ推定装置１００は、各カメラ１０、およびセンサ２０と接続された第１選択部３０と、第１位置姿勢推定部４０と、第２選択部５０と、第２位置姿勢推定部６０と、座標変換部７０と、３次元マップ推定部８０と、を有する。さらに、障害物検出装置２００は、３次元マップ推定装置１００に加え、障害物検出部８２と、を有する。

【0011】

以下の説明においては、自動車（車両１）にカメラ１０、およびカメラ以外のセンサ２０が搭載されている場合について説明する。自動車に限らず、ロボット、ドローン等にカメラ１０、およびカメラ以外のセンサ２０を搭載し、周囲の３次元マップを推定し、障害物２を検出することが可能である。

【0012】

本明細書において、車両１の位置姿勢とは、例えば車両１の位置と向きである。位置とは取得デバイスの絶対的な位置であっても、ある特定の点を基準とする相対的な位置であってもよい。

【0013】

また、本明細書において、障害物２とは、ある基準点からの高さがある閾値よりも大きい物体である。例えば、この閾値は路面を基準点とし、路面に対して鉛直方向における、路面から車体の最も低い部分までの最短距離を元に設定する。この閾値を超えない物体については、車両１の走行中に車両１と衝突しない物体として障害物２の検出対象から除くことができる。なお、高さに限らず、路面水平方向の物体の大きさによって障害物２としてもよい。

【0014】

カメラ１０は、図２に示すように、車両１に複数備えられ、例えば、車両１の前後左右に設けられる。センサ２０は、例えば、超音波センサ、レーザー距離センサ、速度センサ、角速度センサなどであり、車両１内外にそれぞれ設けられる。

【0015】

各カメラ１０、及び各センサ２０は、ある時刻ｔにおいて、それぞれ画像、及びセンサ情報を取得する。

【0016】

第１選択部３０は、複数のカメラ１０、及びセンサ２０のうち、いずれのカメラ１０、及びセンサ２０を用いて車両１の位置姿勢推定を行うかを選択する。第１選択部３０では、後述する選択方法によって位置姿勢推定精度が良くなるようなカメラ１０、およびセンサ２０を選択する。

【0017】

第１位置姿勢推定部４０は、第１選択部３０が選択したカメラ１０の画像、及びセンサ２０が取得したセンサ情報に基づいて、個々に位置姿勢を推定し、第１位置姿勢推定結果を求める。第１位置姿勢推定部４０は、例えば、Ｖｉｓｕａｌ ＳＬＡＭの技術を用いて、第１選択部３０が選択したカメラ１０の画像、およびセンサ２０の情報から車両１の位置姿勢を推定する。なお、本実施形態では、第１位置姿勢推定部４０は、カメラ１０及びセンサ２０ごとに対応して設けられる。これは、後述する３次元マップ推定において、第２位置姿勢推定結果の精度をより向上させるためである。また図１では、第１位置姿勢推定部４０を各ブロックに分けて記述しているが、第１位置姿勢推定部４０を一のブロックとして構成し、第１選択部３０からの入力信号によって、各カメラ１０に対応する位置姿勢推定を個別に行ってもよい。

【0018】

第２選択部５０は、各第１位置姿勢推定部４０で求められた複数の第１位置姿勢推定結果から、第１位置姿勢推定結果どうしの類似度を求める。第２選択部５０は、類似度を基に、必要な第１位置姿勢推定結果を１つ以上選択する。例えば、類似度が相対的に外れている第１位置姿勢推定結果を除き、それ以外の第１位置姿勢推定結果を１つ以上選択する。

【0019】

第２位置姿勢推定部６０は、第２選択部５０が選択した１つ以上の第１位置姿勢推定結果の重み付き和により、車両１の第２位置姿勢推定結果を求める。

【0020】

座標変換部７０は、あるカメラ１０の動きを、他のカメラ１０の動きに変換する。補正が必要な例として、第１選択部３０、及び第２選択部５０で選択されなかったカメラ１０が取得した画像を後述する３次元マップ推定に用いる場合がある。第１選択部３０、及び第２選択部５０で選択されなかったカメラ１０が取得した画像からは、第１位置姿勢推定結果または第２位置姿勢推定結果が求められないため、カメラ１０の位置姿勢の変化を他のカメラ１０の位置姿勢の変化から求める必要がある。ここで、車両１が旋回する時、旋回の中心が車両１の後輪軸中心付近に位置する。カメラ１０はそれぞれ旋回中心からの距離が異なるため、旋回による動きの大きさが異なる。そのため、例えば車両１前方に設けられたカメラ１０の位置姿勢の変化を車両１後方に設けられたカメラ１０の位置姿勢の変化に変換する必要がある。カメラ１０の位置姿勢の変化は回転を表す行列と並進(平行移動)を表すベクトルとで表現できる。座標変換部７０ではあるカメラ１０の回転行列、並進ベクトルに線形変換を行い、別のカメラ１０の回転行列、並進ベクトルを求めることができる。

【0021】

３次元マップ推定部８０は、車両１の第２位置姿勢推定結果と、カメラ１０が取得した異なる時刻の画像間の特徴点とを用いて車両１周辺の３次元マップを推定する。３次元マップ推定部８０は、カメラ１０の動きと異なる時刻の画像における同一の特徴点の位置関係から三角測量の原理を用いて車両１と特徴点との距離を算出し、３次元マップを推定する。また、前述したように、３次元マップはカメラ１０の画像の特徴点を用いて行うが、カメラ１０の画像に特徴点が少ない場合や存在しない場合には３次元マップの密度が低下する。そのため、第１選択部３０で選択されなかったカメラ１０が取得した画像も３次元マップ推定部８０に入力することで、利用する特徴点を増やす。これにより、密度の高い３次元マップが推定される。同様に、第２選択部５０で選択されなかった（除外された）第１位置姿勢推定結果の算出に用いられた画像を用いて３次元マップを推定してもよい。３次元マップ推定部８０は、距離情報を取得可能な外部センサ情報、例えば超音波センサ、レーザー距離センサが取得した外部センサ情報を入力し、車両１周囲の３次元マップを推定してもよい。

【0022】

障害物検出部８２は、車両１の進行方向や速度といった走行情報と、３次元マップ推定部８０で算出された３次元マップに基づき障害物２を検出する。この検出結果は、例えば、図示しない警報装置などに出力され、運転者へ障害物２の有無をアラートする。

【0023】

（３次元マップ推定装置１００、及び障害物検出装置２００の処理の例）
３次元マップ推定装置１００、及び障害物検出装置２００の処理の流れについて、図３を参照して説明する。図３は、第１の実施形態に係る３次元マップ推定装置１００、及び障害物検出装置２００の処理の流れを示すフローチャートである。

【0024】

＜ステップＳ１０１＞
ステップＳ１０１では、第１選択部３０が、車両１の位置姿勢推定精度が良くなるような位置姿勢推定に使用するカメラ１０、およびセンサ２０を選択する。

【0025】

第１選択部３０は、例えば、ｔ２（第２のタイミング）に取得された画像と、時刻ｔ２の時刻のあとの時刻ｔ１（第１のタイミング）に取得された画像とを比較し、画像中の物体の移動変化量が適切なカメラ１０、及びセンサ２０を１つ以上選択する。

【0026】

一例として、車両１と周辺の障害物との位置関係に基づき、異なる時刻における画像間の物体の移動変化量が適切なカメラ１０、及びセンサ２０を選択する。第１選択部３０は、時刻ｔ２に取得された画像、及びセンサ情報に基づく３次元マップより取得された車両１周辺の障害物２と車両１との距離について、あらかじめ設けられた第１閾値と、第２閾値との間の距離にあるカメラ１０を選択する。すなわち、障害物２と車両１との距離が大きすぎず、且つ小さすぎないカメラ１０を選択する。

【0027】

また、車両１の速度（車速）に基づき、カメラ１０、及びセンサ２０を選択してもよい。図２において、例えば車両１が左方向に旋回する場合、車両１の左側にあるカメラ１０に映る障害物２ａの異なる時刻における画像間の移動変化量は小さくなる。一方、車両１の前後のカメラ１０に映る障害物２ｂの異なる時刻における画像間の移動変化量は大きくなる。この場合、車速に閾値をあらかじめ設定してもよい。第１選択部３０は、車速が閾値以下の場合において、直進時は車両１左右に搭載されたカメラ１０を選択し、旋回時は車両１前後に搭載されたカメラ１０を選択するようにする。一方で、車速が閾値以上の場合には、車両１前後に搭載されたカメラ１０を選択する。

【0028】

さらに、パターン認識で歩行者などの他の移動体を検出し、他の移動体が少ないカメラ１０を選択してもよい。後述するＶｉｓｕａｌ ＳＬＡＭは、カメラ１０の画像上の物体が静止していることを前提として位置姿勢推定、及び３次元マップ推定を行うため、視界内の移動体が少ない方がより精度が高いためである。

【0029】

また、カメラ１０が初めて画像を取得した時刻ｔ３の画像を用いて３次元マップを推定する場合、時刻ｔ３に取得された画像、及びセンサ情報に基づく３次元マップが存在しない。そのため、第１選択部３０は、前述した時刻ｔ２での車速や角速度、他の移動体の有無に基づきカメラ１０を選択する。

【0030】

＜ステップＳ１０２＞
ステップＳ１０２では、ステップＳ１０１において選択されたカメラ１０の画像、及びセンサ２０のセンサ情報に基づき、各第１位置姿勢推定部４０が対応したカメラ１０、およびセンサ２０ごとに位置姿勢推定を行う。

【0031】

時刻ｔ１における位置姿勢を推定する方法のうち、Ｖｉｓｕａｌ ＳＬＡＭを用いた手法について、図４を参照して説明する。まず、第１位置姿勢推定部４０は、第１選択部３０で選択されたカメラ１０から、時刻ｔ２と、時刻ｔ１の画像を取得する。次に、時刻ｔ２と、時刻ｔ１におけるそれぞれの画像について、２か所以上の特徴点群を抽出する。そして、時刻ｔ１およびｔ２で抽出された特徴点群同士でマッチングを行い、ｔ１とｔ２の特徴点の対応関係を求める。特徴点群同士のマッチングは、例えば特徴点周辺の画像パターンに対しＮＣＣ（Ｎｏｒｍａｌｉｚｅｄ Ｃｒｏｓｓ－Ｃｏｒｒｅｌａｔｉｏｎ）などを用いて算出された類似する値等を用いる。対応付けされた特徴点ペアの動きから、時刻ｔ２からｔ１の車両１の位置姿勢の変化を推定する。

【0032】

第１位置姿勢推定結果は、車両１の時刻ｔ２からｔ１の並進運動（３次元）、及び回転運動（３次元）から構成され、６次元の第１位置姿勢推定結果が求められる。

【0033】

＜ステップＳ１０３＞
ステップＳ１０３では、第１位置姿勢推定部４０がステップＳ１０２において推定された６次元の第１位置姿勢推定結果を、第２選択部５０に出力する。

【0034】

＜ステップＳ１０４＞
ステップＳ１０４では、第２選択部５０が、ステップＳ１０３における出力結果を基に各カメラ１０の第１位置姿勢推定結果どうしの類似度を求める。その後、求めた類似度を基に、第２選択部５０は必要な第１位置姿勢推定結果を１つ以上選択する。例えば、相対的に外れている第１位置姿勢推定結果（外れ値）を除き、それ以外の第１位置姿勢推定結果を１つ以上選択する。

【0035】

類似度は、例えば、各カメラ１０の並進運動を表す並進ベクトルの比較や、ベクトルどうしの内積を利用する。回転運動同士の類似度を定義し、それを利用してもよい。

【0036】

＜ステップＳ１０５＞
ステップＳ１０５では、ステップＳ１０４において第２選択部５０が選択した１つ以上の第１位置姿勢推定結果を、第２位置姿勢推定部６０に出力する。

【0037】

＜ステップＳ１０６＞
ステップＳ１０６では、第２位置姿勢推定部６０が、ステップＳ１０５において出力された第１位置姿勢推定結果から第２位置姿勢推定結果を求め、座標変換部７０に出力する。これにより、時刻ｔ２からｔ１における車両１の位置姿勢の変化が推定される。

【0038】

第２位置姿勢推定結果は、各第１位置姿勢推定結果の重み付き和である。各第１位置姿勢推定結果にかかる重み係数は、例えば、第２位置姿勢推定部６０で各第１位置姿勢推定結果の信頼度を算出し、信頼度に応じて決定してもよい。信頼度の一例として、３次元空間における特徴点の分布のばらつきの大きさがある。信頼度が大きいときは、特徴点の分布のばらつきの大きさが大きくなる。３次元空間における特徴点の分布のばらつきが小さい場合、特徴点のある一部分は後述するステップＳ１０８においてカメラ１０と特徴点との距離の算出を正確に行うことができるが、カメラ１０と他の部分にある特徴点との距離は正確に算出できないため、３次元マップ推定の精度が下がる。そのため、３次元空間における特徴点の分布のばらつきが大きい第１位置姿勢推定結果の信頼度を大きくする。また、マッチングが取れた特徴点ペアの数が多いほど信頼度を高くする、マッチングが取れた特徴点ペアの類似度の平均値が大きいほど信頼度を高くするなどの方法もある。

【0039】

＜ステップＳ１０７＞
ステップＳ１０７では、座標変換部７０は、第２位置姿勢推定結果に対して座標変換を行うことで、カメラ１０の各カメラ１０の位置姿勢の変化を算出する。

【0040】

＜ステップＳ１０８＞
ステップＳ１０８では、ステップＳ１０７において算出された各カメラ１０の位置姿勢の変化と、時刻ｔ２およびｔ１における特徴点の対応関係とから特徴点の３次元座標を算出する。そして、座標値が算出された３次元点群を車両１周囲の３次元マップとして出力する。

【0041】

Ｖｉｓｕａｌ ＳＬＡＭを用いたカメラ１０の周囲の３次元マップを推定する方法について、図４を参照して説明する。ステップＳ１０２で抽出された時刻ｔ２とｔ１のそれぞれの画像中の対応する特徴点ａと、ステップＳ１０７で出力されたカメラ１０の位置姿勢の変化を用いて、三角測量の原理から、特徴点ａの３次元位置を推定する。

【0042】

さらに、３次元マップ推定部８０は、第１選択部３０、及び第２選択部５０で選択されなかったカメラ１０についても特徴点の対応関係を求め、ステップＳ１０７において算出された各カメラ１０の位置姿勢の変化を用いて特徴点の３次元位置を推定する。

【0043】

＜ステップＳ１０９＞
ステップＳ１０９では、ステップＳ１０８において推定された１つ以上の３次元マップを障害物検出部８２に出力する。

【0044】

また、車両１と特徴点との距離についての閾値を設定しておく。特徴点のうち、車両１との距離が閾値以下の値である場合には障害物２であると判定する。

【0045】

図５は、障害物２のある環境で、車両１を走行させた実験結果を示している。図５（ａ）は障害物２のある環境の一例であり、図５（ｂ）は図５（ａ）に示した環境下で鎖線Ａの経路で車両１を走行させた時の、車両１の走行軌跡の一例を示している。

【0046】

図５（ａ）に示したような、複数の障害物２と１台の車両１が設けられている環境下で、車両１を走行させる。各時刻においてステップＳ１０１からステップＳ１０６を実行し、各時刻において出力された第２位置姿勢推定結果をグラフにプロットすると、図５（ｂ）のような車両１の走行軌跡が得られる。図５において、車両１の進行方向をＸ方向、及びＹ方向とし、車両１の高さ方向をＺ方向としている。なお、第２位置姿勢推定結果は３次元で出力されるが、図５（ｂ）に示した車両１の走行軌跡の１例では、Ｚ方向の第２位置姿勢推定結果を省略して示している。また、図５（ｂ）において、車両１の出発位置を原点０として車両１の走行軌跡を示している。

【0047】

（第１の実施形態の効果）
第１の実施形態に係る３次元マップ推定装置１００、及び障害物検出装置２００には、車両１の位置姿勢推定に使用するカメラ１０、及びセンサ２０を選択する第１選択部３０が設けられている。また、第１の実施形態に係る３次元マップ推定装置１００、及び障害物検出装置２００は、車両１の位置姿勢推定を行うために、カメラ１０が取得した画像に加え、センサ２０のセンサ情報を用いる。さらに、第１の実施形態に係る３次元マップ推定装置１００、及び障害物検出装置２００は、３次元マップを推定するために、第１選択部３０、及び第２選択部５０で選択されなかったカメラ１０が取得した画像、及びセンサ２０が取得したセンサ情報を用いて推定を行う。これにより、本実施形態は２つの効果を有する。

【0048】

１つめは、車両１の位置姿勢推定を安定かつ精度良くすることができる。第１選択部３０では、異なる時刻における画像間の物体の移動変化量が適切なカメラ１０、及びセンサ２０を１つ以上選択する。異なる時刻における画像間の物体の移動変化量が１つのピクセルより小さい場合、位置姿勢推定精度が悪くなる。しかし、第１選択部３０では、異なる時刻における画像間の物体の移動変化量が適切なカメラ１０、およびセンサ２０を選択しているため、車両１の位置姿勢推定精度が改善することが期待できる。

【0049】

２つ目は、３次元マップ推定ができない場面を減らすことができる。第１の実施形態に係る３次元マップ推定装置１００、及び障害物検出装置２００は、カメラ１０に加えて、センサ２０が取得したセンサ情報も利用して位置姿勢推定を行う。これにより、例えば、車両１が動き出した直後などカメラ１０の画像を用いた位置姿勢推定が難しい場合にも、位置姿勢推定を行うことができる。３次元マップ推定は第２位置姿勢推定結果を利用するため、３次元マップ推定ができない場面を減らすことができる。

【0050】

さらに、第１の実施形態に係る３次元マップ推定装置１００、及び障害物検出装置２００は、第１選択部３０において選択されなかったカメラ１０、及びセンサ２０が取得した画像、及びセンサ情報も用いて３次元マップの推定を行う。同様に、第２選択部５０において選択されなかった位置姿勢推定結果の算出に用いられた画像も用いる。既に述べた通り、Ｖｉｓｕａｌ ＳＬＡＭを用いた３次元マップの推定は、第２位置姿勢推定結果とカメラ１０が取得した画像上の特徴点から推定する。そのため、位置姿勢の変化の推定精度が低い場合、３次元マップの推定精度が低下する。第１の実施形態に係る３次元マップ推定装置１００、及び障害物検出装置２００は、精度良く位置姿勢の変化を算出できるカメラ１０やセンサ２０を選択し、それを元に３次元マップを推定するため、選択されなかったカメラ１０であっても精度の高い３次元マップの推定を期待することができる。

【0051】

（３次元マップ推定装置１００、及び障害物検出装置２００の実装例）
図６は、第１の実施形態に係る３次元マップ推定装置１００、及び障害物検出装置２００のハードウェア実装の例を示すブロック図である。３次元マップ推定装置１００の実装例を説明するが、３次元マップ推定装置１００を障害物検出装置２００に置き換え、障害物検出装置２００においても同様に実装することができる。

【0052】

３次元マップ推定装置１００は、プロセッサ９１と、主記憶装置９２と、補助記憶装置９３と、ネットワークインタフェース９４と、デバイスインタフェース９５と、を備え、これらがバス９６を介して接続され、実現できる。３次元マップ推定装置１００は、それ自体が独立して起動できるコンピュータ装置であってもよいし、独立して起動するコンピュータ装置に組み込まれ、又は、接続されたアクセラレータであってもよい。

【0053】

なお、図６の３次元マップ推定装置１００は、各構成要素を一つ備えているが、同じ構成要素を複数備えていてもよい。また、１台の３次元マップ推定装置１００が示されているが、ソフトウェアが複数のコンピュータ装置にインストールされて、当該複数のコンピュータ装置それぞれがソフトウェアの異なる一部の処理を実行してもよい。

【0054】

プロセッサ９１は、３次元マップ推定装置１００の制御装置および演算装置を含む処理回路として動作する電子回路である。プロセッサ９１は、３次元マップ推定装置１００の内部構成の各装置などから入力されたデータやプログラムに基づいて演算処理を行い、演算結果や制御信号を各装置などに出力する。具体的には、プロセッサ９１は、３次元マップ推定装置１００のＯＳ（Ｏｐｅｒａｔｉｎｇ Ｓｙｓｔｅｍ）や、アプリケーションなどを実行することにより、３次元マップ推定装置１００を構成する各構成要素を制御する。プロセッサ９１は、上記の処理を行うことができれば特に限られるものではない。３次元マップ推定装置１００、及びその各構成要素は、プロセッサ９１により実現される。

【0055】

主記憶装置９２は、プロセッサ９１が実行する命令および各種データなどを記憶する記憶装置であり、主記憶装置９２に記憶された情報がプロセッサ９１により直接読み出される。補助記憶装置９３は、主記憶装置９２以外の記憶装置である。なお、これらの記憶装置は、電子情報を格納可能な任意の電子部品を意味するものとし、メモリでもストレージでもよい。また、メモリには、揮発性メモリと、不揮発性メモリがあるが、いずれでもよい。３次元マップ推定装置１００内において各種データを保存するためのメモリは、主記憶装置９２または補助記憶装置９３により実現されてもよい。例えば、記憶部は、この主記憶装置９２又は補助記憶装置９３に実装されていてもよい。別の例として、アクセラレータが３次元マップ推定装置１００においてさらに備えられている場合には、記憶部は、当該アクセラレータに備えられているメモリ内に実装されていてもよい。

【0056】

ネットワークインタフェース９４は、無線または有線により、通信ネットワーク９８に接続するためのインタフェースである。ネットワークインタフェース９４は、既存の通信規格に適合したものを用いればよい。ネットワークインタフェース９４により、通信ネットワーク９８を介して通信接続された外部装置９９Ａと情報のやり取りが行われてもよい。

【0057】

外部装置９９Ａは、例えば、ステレオカメラ、モーションキャプチャ、出力先デバイス、外部のセンサ、入力元デバイスなどが含まれる。また、外部装置９９Ａは、３次元マップ推定装置１００の構成要素の一部の機能を有する装置でもよい。そして、３次元マップ推定装置１００は、３次元マップ推定装置１００の処理結果の一部を、クラウドサービスのように通信ネットワーク９８を介して送受信してもよい。

【0058】

デバイスインタフェース９５は、外部装置９９Ｂと直接接続するＵＳＢ（Ｕｎｉｖｅｒｓａｌ ＳｅｒｉａｌＢｕｓ）などのインタフェースである。外部装置９９Ｂは、外部記憶媒体でもよいし、ストレージ装置でもよい。記憶部は、外部装置９９Ｂにより実現されてもよい。

【0059】

外部装置９９Ｂは出力装置でもよい。出力装置は、例えば、画像を表示するための表示装置でもよいし、音声などを出力する装置などでもよい。例えば、ＬＣＤ（Ｌｉｑｕｉｄ Ｃｒｙｓｔａｌ Ｄｉｓｐｌａｙ）、ＣＲＴ（Ｃａｔｈｏｄｅ Ｒａｙ Ｔｕｂｅ）、ＰＤＰ（Ｐｌａｓｍａ Ｄｉｓｐｌａｙ Ｐａｎｅｌ）、スピーカなどがあるが、これらに限られるものではない。また、ＣＡＮを介して制御される自動車の構成要素であってもよい。

【0060】

なお、外部装置９９Ｂは入力装置でもよい。入力装置は、キーボード、マウス、タッチパネルなどのデバイスを備え、これらのデバイスにより入力された情報を３次元マップ推定装置１００に与える。入力装置からの信号はプロセッサ９１に出力される。

【0061】

このように、上記の全ての記載において、３次元マップ推定装置１００、及び障害物検出装置２００の少なくとも一部はハードウェアで構成されていてもよいし、ソフトウェアで構成され、ソフトウェアの情報処理によりＣＰＵ等が実施をしてもよい。ソフトウェアで構成される場合には、３次元マップ推定装置１００、及び障害物検出装置２００及びその少なくとも一部の機能を実現するプログラムをフレキシブルディスクやＣＤ－ＲＯＭ等の記憶媒体に収納しコンピュータに読み込ませて実行させるものであってもよい。記憶媒体は、磁気ディスクや光ディスク等の着脱可能なものに限定されず、ハードディスク装置やメモリなどの固定型の記憶媒体であってもよい。すなわち、ソフトウェアによる情報処理がハードウェア資源を用いて具体的に実装されるものであってもよい。さらに、ソフトウェアによる処理は、ＦＰＧＡ等の回路に実装され、ハードウェアが実行するものであってもよい。

【0062】

例えば、コンピュータが読み取り可能な記憶媒体に記憶された専用のソフトウェアをコンピュータが読み出すことにより、コンピュータを上記の実施形態の装置とすることができる。記憶媒体の種類は特に限定されるものではない。また、通信ネットワークを介してダウンロードされた専用のソフトウェアをコンピュータがインストールすることにより、コンピュータを上記の実施形態の装置とすることができる。こうして、ソフトウェアによる情報処理が、ハードウェア資源を用いて、具体的に実装される。

【0063】

本発明のいくつかの実施形態を説明したが、これらの実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。これら新規な実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。これら実施形態やその変形は、発明の範囲や要旨に含まれるとともに、特許請求の範囲に記載された発明とその均等の範囲に含まれる。

【符号の説明】

【0064】

１ 車両
２、２ａ、２ｂ 障害物
３ 特徴点
１０ カメラ
２０ センサ
３０ 第１選択部
４０ 第１位置姿勢推定部
５０ 第２選択部
６０ 第２位置姿勢推定部
７０ 座標変換部
８０ ３次元マップ推定部
８２ 障害物検出部
９１ プロセッサ
９２ 主記憶装置
９３ 補助記憶装置
９４ ネットワークインタフェース
９５ デバイスインタフェース
９６ バス
９８ 通信ネットワーク
９９Ａ、９９Ｂ 外部装置
１００ ３次元マップ推定装置
２００ 障害物検出装置

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】