JP7845985B2

JP7845985B2 - 自己位置推定装置、自己位置推定方法、および自己位置推定プログラム

Info

Publication number: JP7845985B2
Application number: JP2022169676A
Authority: JP
Inventors: 篤志成重; 広器佐々木
Original assignee: Denso Corp; Toyota Motor Corp; Mirise Technologies Corp
Current assignee: Denso Corp; Toyota Motor Corp; Mirise Technologies Corp
Priority date: 2022-10-24
Filing date: 2022-10-24
Publication date: 2026-04-14
Anticipated expiration: 2042-10-24
Also published as: JP2024061982A

Description

本開示は、自己位置推定装置、自己位置推定方法、および自己位置推定プログラムに関する。

従来、例えば特許文献１に示すように、ＳＬＡＭ（ＳｉｍｕｌｔａｎｅｏｕｓＬｏｃａｌｉｚａｔｉｏｎＡｎｄＭａｐｐｉｎｇ）を用いて自己の位置を推定する方法が知られている。ＳＬＡＭは、カメラやセンサを搭載した移動体によって、ＧＰＳなどの衛星システムに依存せずに、自己位置の推定と３次元地図である環境地図の作成とを同時に実行する技術である。

特開２０２２－６２８０７号公報

しかし、上記従来技術においては、３次元地図の作成を行うため、自己位置推定に要する処理量が多く処理時間が長いという問題があった。本開示は、上記のような点に鑑みて創作されたものであり、その目的は、処理時間を短縮することが可能な自己位置推定装置、自己位置推定方法、および自己位置推定プログラムを提供することにある。

本開示は、以下の形態として実現することが可能である。

本開示の一形態によれば、自己位置推定装置が提供される。この自己位置推定装置は、移動体（ＭＣ）の自己位置を推定する自己位置推定装置であって、前記移動体に搭載された複数のカメラ（２１，２２，２３，２４）で構成されるマルチカメラ（２０）から出力された画像データを設定間隔毎に取得する画像データ取得部（１１）と、前記画像データ取得部により取得された前記画像データから特徴点を抽出する特徴点抽出部（１２）と、前記各画像データ内の前記各特徴点の位置情報、を記憶する記憶部（１３）と、最新の画像データにおける特徴点である現特徴点と、過去の画像データにおける特徴点である前特徴点と、を対応付けて、時系列毎に異なる前記画像データにおいて対応する特徴点同士の組であるフローを作成するフロー作成部（１４）と、前記フロー作成部により作成された前記フローのうち、前記移動体の予測される運動に基づいて計算されたエピポーラ拘束を含む予め定められた制約条件を用いて、前記制約条件を満たす前記フローを正しいフローとして選別する選別部（１５）と、前記選別部により選別された前記正しいフローの情報を用いて、前記移動体の位置および向きを算出する自己位置算出部（１６）と、を備える。前記制約条件は、時系列において連続する前記画像データにおける前記フローの長さであるフロー長の基準範囲を、前記フローの選別の指標として含む。前記選別部は、作成された前記フローのうち、前記フロー長が前記基準範囲内にあるものを選別する。前記フロー長の前記基準範囲は、前記複数のカメラごとに個別に設定される。
本開示の他の形態によれば、自己位置推定装置が提供される。この自己位置推定装置は、移動体（ＭＣ）の自己位置を推定する自己位置推定装置であって、前記移動体に搭載された複数のカメラ（２１，２２，２３，２４）で構成されるマルチカメラ（２０）から出力された画像データを設定間隔毎に取得する画像データ取得部（１１）と、前記画像データ取得部により取得された前記画像データから特徴点を抽出する特徴点抽出部（１２）と、前記各画像データ内の前記各特徴点の位置情報、を記憶する記憶部（１３）と、最新の画像データにおける特徴点である現特徴点と、過去の画像データにおける特徴点である前特徴点と、を対応付けて、時系列毎に異なる前記画像データにおいて対応する特徴点同士の組であるフローを作成するフロー作成部（１４）と、前記フロー作成部により作成された前記フローのうち、前記移動体の予測される運動に基づいて計算されたエピポーラ拘束を含む予め定められた制約条件を用いて、前記制約条件を満たす前記フローを正しいフローとして選別する選別部（１５）と、前記選別部により選別された前記正しいフローの情報を用いて、前記移動体の位置および向きを算出する自己位置算出部（１６）と、を備える。前記制約条件は、時系列において連続する前記画像データにおける前記フローの長さであるフロー長の基準範囲を、前記フローの選別の指標として含む。前記選別部は、作成された前記フローのうち、前記フロー長が前記基準範囲内にあるものを選別する。前記フロー長の前記基準範囲は、前記移動体の移動状態に応じて設定される。
本開示の他の形態によれば、自己位置推定装置が提供される。この自己位置推定装置は、移動体（ＭＣ）の自己位置を推定する自己位置推定装置であって、前記移動体に搭載された複数のカメラ（２１，２２，２３，２４）で構成されるマルチカメラ（２０）から出力された画像データを設定間隔毎に取得する画像データ取得部（１１）と、前記画像データ取得部により取得された前記画像データから特徴点を抽出する特徴点抽出部（１２）と、前記各画像データ内の前記各特徴点の位置情報、を記憶する記憶部（１３）と、最新の画像データにおける特徴点である現特徴点と、過去の画像データにおける特徴点である前特徴点と、を対応付けて、時系列毎に異なる前記画像データにおいて対応する特徴点同士の組であるフローを作成するフロー作成部（１４）と、前記フロー作成部により作成された前記フローのうち、前記移動体の予測される運動に基づいて計算されたエピポーラ拘束を含む予め定められた制約条件を用いて、前記制約条件を満たす前記フローを正しいフローとして選別する選別部（１５）と、前記選別部により選別された前記正しいフローの情報を用いて、前記移動体の位置および向きを算出する自己位置算出部（１６）と、を備える。前記選別部は、前記正しいフローとして選別した数が、予め定められた基準より少ない場合には、前記正しいフローの数が前記基準以上となるまで、従前に用いられた前記制約条件から変更された制約条件を用いて再度前記フローの選別を実行する。

この形態の自己位置推定装置によれば、画像データ取得部により取得されたマルチカメラから出力された画像データを用いて、３次元マップを作成することなく車両の自己位置を推定できる。そして、選別部により、エピポーラ拘束を含む予め定められた制約条件を用いて、制約条件を満たすフローを正しいフローとして選別する。このように制約条件を用いることで、最新の画像データ内の適切な現特徴点を特定する際に、適切な探索範囲を限定することができるため、現特徴点の予測が可能となり探索処理時間を少なくできる。以上により、自己位置推定装置における自己位置推定の処理時間を短縮できる。

第１実施形態の自己位置推定装置の概略的なシステム構成を示すブロック図である。自己位置推定装置が搭載される車両を示す平面図である。現在フレームの特徴点リストを得る手順を示すフローチャートである。エピポーラ拘束を説明するための図である。車両の運動情報を動的に示し、各カメラの位置変化を示す図である。フロー長の統計分布を示すヒストグラムである。フロー長の統計分布を示すヒストグラムである。フロー長の統計分布を示すヒストグラムである。フロー長の統計分布を示すヒストグラムである。車両の運動情報を動的に示し、各カメラの位置変化を示す図である。フロー長の統計分布を示すヒストグラムである。フロー長の統計分布を示すヒストグラムである。フロー長の統計分布を示すヒストグラムである。フロー長の統計分布を示すヒストグラムである。任意の現在フレームの一例を示しており、フレーム内に特徴点およびフローを図示した図である。任意の現在フレームの一例を示しており、フレーム内に特徴点およびフローを図示した図である。

以下、実施形態について、図１～図１６に基づいて説明する。
Ａ．第１実施形態：
Ａ１．自己位置推定装置１の構成：
第１実施形態の自己位置推定装置１の構成について、図１、図２を参照しつつ説明する。第１実施形態の自己位置推定装置１は、移動体としての自動運転車両ＭＣ（以下、単に「車両ＭＣ」という）に搭載され、車両ＭＣの自己位置を推定する。車両ＭＣは、自己位置推定装置１により推定された自己位置に基づいて自動運転による移動を行うことができる。なお、「自己位置」とは、移動体の位置および向きを含むものであり、位置姿勢を意味する。

図１に示すように、車両ＭＣは、自己位置推定装置１のほか、車両制御部１０と、カメラ２０と、を備える。車両制御部１０は、車両ＭＣの制御を行う機能部であり、車両制御部１０には、カメラ２０が電気的に接続されており、カメラ２０によって撮像された画像データ（以下、「フレーム」ともいう。）が通知される。その他、車両制御部１０には、図示しないＧＮＳＳ（ＧｌｏｂａｌＮａｖｉｇａｔｉｏｎＳａｔｅｌｌｉｔｅＳｙｓｔｅｍ）センサ等が電気的に接続されている。

図２に示すように、カメラ２０は、前カメラ２１と、後カメラ２２と、右カメラ２３と、左カメラ２４と、の合計４つのカメラを有した複眼のマルチカメラとして構成されている。前カメラ２１は、車両ＭＣの前方における周辺画像を取得する。後カメラ２２は、車両ＭＣの後方における周辺画像を取得する。

右カメラ２３は、車両ＭＣの進行方向に見たときの右方における周辺画像を取得する。左カメラ２４は、車両ＭＣの進行方向に見たとき左方における周辺画像を取得する。以下、各カメラ２１，２２，２３，２４を特に区別しないときは、単に「カメラ２０」という。カメラ２０は、魚眼を代表する広角カメラであり、４つのカメラの合計の画角は３６０度の視野をカバーする。

再び、図１を参照する。本実施形態の自己位置推定装置１は、車両ＭＣの車両制御部１０と電気的に接続されている。具体的には、本実施形態の自己位置推定装置１は、車両ＭＣのＯＢＤ２（ＯｎＢｏａｒｄＤｉａｇｎｏｓｔｉｃｓ２）に接続されているため、自己位置推定装置１は、車両制御部１０から車両ＭＣの各種の情報の受信が可能である。

なお、自己位置推定装置１と車両ＭＣとの接続はこれに限られず、例えば、無線通信による接続を用いてもよい。また、自己位置推定装置１と車両ＭＣとは接続されておらず、自己位置推定装置１が、カメラ２０を別途備えてもよい。本実施形態では、ＯＢＤ２から自己位置推定装置１へ電力が供給されるが、これに限られず、自己位置推定装置１は別途バッテリーを備えてもよく、車両ＭＣのシガーソケットから自己位置推定装置１の電力を供給してもよい。

自己位置推定装置１は、画像データ取得部１１と、特徴点抽出部１２と、記憶部１３と、フロー作成部１４と、選別部１５と、自己位置算出部１６と、の各機能モジュールを備える。自己位置推定装置１は、ＣＰＵと、ＲＯＭやＲＡＭなどのメモリと、を備える周知のコンピュータとして構成されている。各機能モジュール１１～１６は、メモリに記憶されたプログラムをＣＰＵによって実行することで実現される。

画像データ取得部１１は、車両ＭＣの周辺環境の画像データを取得する。具体的には、画像データ取得部１１は、上記カメラ２０から出力された画像データを設定間隔毎に車両ＭＣから取得する工程を実行する。

特徴点抽出部１２は、画像取得部１１により取得された画像データから特徴点を抽出する工程を実行する。特徴点は、例えば、エッジ、コーナー、色分布などを含む。特徴点抽出の手法は、ＯＲＢ，ＳＩＦＴ，ＳＵＲＦを代表するインダイレクト法であってもよいし、ＤＳＯ（ＤｉｒｅｃｔＳｐａｒｓｅＯｄｏｍｅｔｒｙ）を代表するダイレクト法であってもよい。

記憶部１３は、各画像データ内の各特徴点の位置情報として３次元位置座標を記憶する工程を実行する。その他、記憶部１３は、車両ＭＣの自己位置推定の一連の処理において用いられる各種データや、演算結果を記憶する。

フロー作成部１４は、撮影された時間毎に異なる画像データにおいて対応する特徴点同士の組であるフローを作成する工程を実行する。フロー作成部１４は、フローの作成に際して、記憶部１３に記憶された過去の画像データにおける特徴点である前特徴点と、最新の画像データにおける特徴点である現特徴点と、の対応付けを行う。具体的には、最新より一つ前の過去の画像データにおける前特徴点と、現特徴点との対応付けが、フレーム毎に実行される。

選別部１５は、予め定められた制約条件を用いて、フロー作成部１４により作成されたフローのうち、上記制約条件を満たすフローを正しいフローとして選別する工程を実行する。制約条件は、車両ＭＣの予測される運動に基づいて計算されたエピポーラ拘束を含む。なお、制約条件の詳細については、後述する自己位置推定方法において、制御フローチャートと併せて詳細に説明する。

自己位置算出部１６は、選別部１５により選別されたフローの情報を用いて、車両ＭＣの位置および向きを算出する工程を実行する。算出された車両ＭＣの位置および向き、すなわち位置姿勢が、車両ＭＣの現在位置の推定結果となる。ここで、自己位置算出に際しては、ＳＬＡＭ処理を用いる。ＳＬＡＭとは、ＳｉｍｕｌｔａｎｅｏｕｓＬｏｃａｌｉｚａｔｉｏｎＡｎｄＭａｐｐｉｎｇの頭字語であり、自己位置推定と環境地図作成とを同時に行う手法のことである。なお、本実施形態では、３次元マップとしての環境地図の作成は行わず、カメラ２０からの画像のみを用いて車両ＭＣの位置推定を行う。

Ａ２．自己位置推定方法：
次に、上記自己位置推定装置１による自己位置推定方法について説明する。図３は、現在（すなわち最新）の自己位置算出に用いるための、現在フレームの特徴点リストを得る手順を示すフローチャートである。「現在フレーム」は最新のフレームであり、以下、「前回フレーム」とは、現在フレームより一つ前のフレームであり、「前々回フレーム」とは、前回フレームよりもさらに一つ前のフレームである。「前回フレーム」および「前々回フレーム」は、いずれも過去の画像データに相当する。

特徴点リストには、上記フローの情報が含まれる。図３に示す処理は、上記自己位置推定装置１により、設定間隔毎に繰り返し実行される。ここでの設定間隔は、カメラ２０による撮影の設定間隔と同じである。

図３に示すように、Ｓ１０１において、車両ＭＣの角度変化量および移動量の前回推定値が読み込まれる。「前回推定値」は、前々回フレームと前回フレーム間における車両ＭＣの角度変化量および移動量であり、事前に計算されて記憶部１３に記憶されている。次に、Ｓ１０２において、Ｓ１０１において読み込んだ前回推定値を用いて、等速運動を前提として、前回フレームと現在フレームとの間の基本行列が作成される。基本行列は、エピポーラ拘束を数式で表すときに、用いられる行列である。

次に、Ｓ１０３において、エピポーラ線が作成される。エピポーラ線は、Ｓ１０２において作成された基本行列と、現在フレームにおける特徴点の３次元座標およびカメラ２０の焦点距離と、前回フレームにおける特徴点の３次元座標およびカメラ２０の焦点距離と、を用いて公知の方法により作成される。

そして、Ｓ１０４において、エピポーラ拘束による上限閾値と、フロー長の基準範囲が決定される。このＳ１０４の処理は、選別部１５により実行される。なお、Ｓ１０４の処理は、選別部１５に限らず、他の機能部が実行してもよい。なお、「フロー長」とは、前特徴点と、前特徴点と対応する現特徴点と、を結んだ一つのフローの長さである。「フロー長」は、時系列において連続する画像データ間におけるフローの長さであり、換言すると、１フレーム間のフローの長さのことをいう。Ｓ１０４において決定される、エピポーラ拘束による上限閾値と、フロー長の基準範囲を、併せて「制約条件」という。また、エピポーラ拘束による上限閾値を満たすとの制約条件を「第１条件」とし、フロー長の基準範囲を満たすとの制約条件を「第２条件」とする。

Ｓ１０４において制約条件が決定された後は、Ｓ１０５において、制約条件による判定が行われる。すなわち、上述の第１条件および第２条件を満たすか否かが判定される。具体的には、現特徴点の座標が、エピポーラ線からの所定距離の上限閾値内にあり、かつ、現特徴点と前特徴点とを結んだフロー長が、基準範囲内にあれば、制約条件を満たすと判定する。なお、第１条件と第２条件のうち、いずれか一つでも満たさない場合には、制約条件を満たさないと判定する。この判定は、フレーム内の全ての特徴点に対して実行される。なお、Ｓ１０４，Ｓ１０５における、制約条件の決定および判定についての詳細は、後述する。

次に、Ｓ１０６において、制約条件を満たす特徴点が「特徴点リスト」に追加される。「特徴点リスト」とは、本制御ルーチンの終了後に、自己位置推定の算出に用いられる特徴点およびフローの情報をまとめて格納するリストである。次に、Ｓ１０７において、特徴点リストに追加されたフローの数が基準以上であるか否かが判断される。フローの数の基準値は、適切な自己位置推定の算出に際して必要とされる数として、事前の試験により予め定められる。そして、Ｓ１０７において、フローの数が基準以上である場合には（Ｓ１０７：Ｙｅｓ）、Ｓ１０９に進み、特徴点リストが出力される。ここで出力される特徴点リストは、制約条件を満たさない特徴点およびフローの情報が間引きされたものである。Ｓ１０９の処理の後、本処理ルーチンは終了する。その後、出力された特徴点リストの情報を用いて、例えば２点法等により、自己位置推定が行われる。

一方、Ｓ１０７において、特徴点リストに追加されたフローの数が基準以上ではない場合には（Ｓ１０７：Ｎｏ）、Ｓ１０８に進み、制約条件が緩和される。フローの数があまりに少なく基準より少ない場合には、現在の自己位置推定の算出が行えないため、もしくは、精度が著しく低下するため、制約条件を緩和することで、特徴点リストのフローの数を増やすためである。

「制約条件を緩和する」とは、具体的には、エピポーラ線からの所定距離の上限閾値を大きくする、または、フロー長の基準範囲を大きくすることである。制約条件が緩和された後は、再びＳ１０５における判定処理に戻り、以下、Ｓ１０７においてフローの数が基準以上となるまで、Ｓ１０８、Ｓ１０５、Ｓ１０６の処理が繰り返される。

Ａ３．制約条件の詳細：
次に、上記Ｓ１０４，Ｓ１０５における、制約条件の決定および判定に関する詳細について説明する。まず、エピポーラ拘束による上限閾値について図４を用いて説明する。図４は、エピポーラ拘束を説明するための図であり、３次元空間を異なる位置のカメラから撮影した幾何を示す図である。図４において、点ｅ_Ｌおよび点ｅ_Ｒは、エピポール（Ｅｐｉｐｏｌｅ）であり、一方のカメラから他方のカメラが写る位置のことである。また、エピポーラ線Ｅは、「カメラＯ_Ｌから見てＸ_Ｌに物体が写っている」とき、「カメラＯ_Ｒから見てどこに物体が写るか」の候補となる線である。

図４に示すように、Ｘ_Ｐにある物体は、前回フレームにおけるカメラＯ_Ｌから見るとＸ_Ｌに写り、現在フレームにおけるカメラＯ_Ｒから見るとＸ_Ｒに写る。「カメラＯ_Ｌから見てＸ_Ｌに物体が写っている」ことしか分かっていない時点では、カメラＯ_Ｒでその物体が写る（Ｘ_Ｒがある）位置は、「エピポーラ線Ｅ上のどこか」に限定される。このことをエピポーラ拘束（ＥｐｉｐｏｌａｒＣｏｎｓｔｒａｉｎｔ）と呼ぶ。

従来の３次元マップを作成する手法の場合には、２枚のフレームから、Ｘｐの奥行き方向におけるおおまかな位置が分かるため、現在フレーム内で特徴点を探す際の探索範囲を、比較的小さい範囲に２次元的に定めることができる。しかし、本実施形態では、３次元マップを持たないため、Ｏ_ＬとＸ_Ｌを結ぶ直線であるＸ線の延長上にあることは分かるものの、Ｘ線上における奥行き方向の位置が不明である。すなわち、図４において、Ｘ_２やＸ_１やＸ_０にあるかも知れない。また、Ｘ_Ｒの近くにあることは分かるが、エピポーラ線Ｅの近傍といっても、特徴点の探索範囲は広く、定まらない。

そこで、本実施形態では、現在フレーム内で現特徴点を探す際において、エピポーラ線Ｅから所定の距離以内の範囲を探索範囲Ｓとする。言い換えると、探索範囲Ｓは、現在フレーム内においてエピポーラ線Ｅから両側に所定距離をとった２本の平行な直線Ｌ１，Ｌ２に挟まれた領域である。「エピポーラ線Ｅからの距離」は、図４において、２次元平面としての現在フレーム内における、エピポーラ線Ｅと直線Ｌ１，Ｌ２との１次元距離である。つまり、Ｓ１０４では、作成されたエピポーラ線と、予め設定された距離とに基づいて、エピポーラ拘束による上限閾値、すなわち上記探索範囲Ｓが決定される。

そして、Ｓ１０５において、選別部１５は、作成されたフローのうち、現在フレーム内の現特徴点の位置座標が、探索範囲Ｓ内にあるものを正しいフローとして選別する。エピポーラ拘束による上限閾値、すなわち、エピポーラ線Ｅからの所定距離を、フローの選別の指標としている。なお、距離の上限閾値は、探索処理時間や、特徴点の位置精度等を鑑みて、事前の試験データにより探索範囲Ｓがあまり広くなりすぎない程度に予め適宜設定される。

次に、フロー長の基準範囲の決定について、図５～図１４を参照して説明する。図５は、車両ＭＣの運動情報を動的に示し、各カメラ２１，２２，２３，２４の位置変化を示す図であり、任意の環境における左折時の形態を示している。図６～図９は、フロー長の度数分布を示すヒストグラムである。図６は、前カメラ２１により取得されたデータに基づいたヒストグラムであり、図７は、後カメラ２２により取得されたデータに基づいたヒストグラムである。図８は、右カメラ２３により取得されたデータに基づいたヒストグラムであり、図９は、左カメラ２４により取得されたデータに基づいたヒストグラムである。図６～図９において、横軸にフロー長を取り、縦軸に出現値を取っている。なお、フロー長の目盛は、フロー長の大小を見るためのデータ処理上の便宜上の単位のない数値である。以下、後述する図１１～図１４についても同様である。

図５～図９に示すように、車両ＭＣが左折するとき、各カメラ２１，２２，２３，２４により取得されるフロー長のヒストグラムはそれぞれ異なる。図６に示すように、前カメラ２１によるヒストグラムでは、フロー長が３０の出現値が約９０であり突出して大きく、次にフロー長が４０の出現値が約６０であり大きく、その他は小さくなっている。図７に示すように、後カメラ２２によるヒストグラムでは、フロー長が３０の出現値が６０であり突出して大きく、フロー長が２０，４０，５０の出現値が２０近傍であり、フロー長が３０の出現値に対して半数以下である。

また、図８に示すように、右カメラ２３によるヒストグラムでは、フロー長が５０および６０の出現値が突出して大きい。図９に示すように、左カメラ２４によるヒストグラムでは、フロー長が１０～４０と大きくなるにつれて出現値も大きくなり、フロー長が４０の出現値が６０であり最も大きい。一方、フロー長が５０，６０の出現値は１０以下であり小さくなっている。

以上のように、各カメラ２１～２４において、フロー長の統計分布の系統はそれぞれ異なっていることが分かった。また、出現値の大きいフロー、すなわち長いフローを用いて推定を行った方が、精度が良くなることが分かった。そこで、本実施形態では、これらの性質を利用し、フロー長の基準範囲を、各カメラ２１～２４において適宜設定する。また、フロー長の基準範囲を、右折や左折等の運転状況に応じて設定する。なお、右折や左折等の運転状況は、「移動体の移動状態」の一例に相当する。

本実施形態では、例えば、前カメラ２１については（図６参照）、フロー長の基準範囲を、最頻値である３０，４０近傍の「２５～４５」とする。後カメラ２２については（図７参照）、フロー長の基準範囲を、最頻値である３０近傍の「２５～３５」とする。右カメラ２３については（図８参照）、フロー長の基準範囲を、最頻値である５０，６０近傍の「４５～６０」とする。

すなわち、前カメラ２１および後カメラ２２における左折時のフロー長の基準範囲は、フロー長の最頻値を基準に設定される。「最頻値を基準に」とは、上記例示のように最頻値を中央値として、経験則に基づく任意の幅を上下にとる形態であってもよいし、最頻値から上または下のみに予め試験により定めた値をとる形態であってもよい。つまり、「最頻値」を指標として、基準範囲を設定する任意の形態のことを意味する。以下、「平均値を基準に」についても同義である。

左カメラ２４については（図９参照）、フロー長の基準範囲を、平均値である４０近傍の「２５～４５」とする。つまり、左カメラ２４におけるフロー長の基準範囲は、フロー長の平均値を基準に設定される。基準範囲として、最頻値を指標とするか、平均値を指標とするかについては、ヒストグラムの形態に応じてカメラ２１，２２，２３，２４ごとに適宜選択できる。また、最頻値や平均値からどのくらいの幅を許容範囲とするかは、事前の試験データ等から予め設定される。よって、上記基準範囲の具体的数値については適宜変更可能である。

以下、右折時の一例についても説明する。図１０は、車両ＭＣの運動情報を動的に示し、各カメラ２１，２２，２３，２４の位置変化を示す図であり、任意の環境における右折時の形態を示している。図１１～図１４は、フロー長の統計分布を示すヒストグラムである。図１１は、前カメラ２１により取得されたデータに基づいたヒストグラムであり、図１２は、後カメラ２２により取得されたデータに基づいたヒストグラムである。図１３は、右カメラ２３により取得されたデータに基づいたヒストグラムであり、図１４は、左カメラ２４により取得されたデータに基づいたヒストグラムである。

図６～図１４を適宜比較すると分かるように、各カメラ２１～２４に写る物体の見え方は、右折時と左折時においては異なるため、当然、フロー長のヒストグラムはそれぞれ異なる。図１１に示すように、右折時の前カメラ２１によるヒストグラムでは、出現値が５０以下と小さいものが分散している。ヒストグラムがこのような形態の場合には、最頻値を取るよりも、平均値を取る方が好ましい。この場合、例えば、前カメラ２１については、フロー長の基準範囲を、平均値である２０近傍の「１５～２５」とする。

一方、図１２に示すように、後カメラ２２によるヒストグラムでは、フロー長が２０の出現値が約２６０であり極めて突出して大きく、その他は小さくなっている。ヒストグラムがこのような形態の場合には、フロー長の基準範囲を、最頻値である２０近傍の「１５～２５」とするのが好ましい。同様にして、図１３、図１４に示すように、右カメラ２３および左カメラ２４によるヒストグラムでは、フロー長の基準範囲を、例えば平均値近傍に決定するのが好ましい。

以上のように、現在フレームにおける、各カメラ２１～２４によるヒストグラムを取得し、自己位置推定に際して、随時、フロー長の基準範囲を決定する。Ｓ１０５において、選別部１５は、作成されたフローのうち、現在フレームと前回フレーム間のフロー長が、基準範囲内にあるものを正しいフローとして選別する。すなわち、フロー長の統計であるヒストグラムから得た基準範囲を、フローの選別の指標としている。上記フロー分布は、車両ＭＣの移動速度および旋回方向に応じて変化する。なお、一般に、上記基準範囲は、車両ＭＣの移動速度が大きいほど大きくなる。また、車両ＭＣの旋回方向と逆に設けられたカメラにおける値の方が大きくなる傾向がある。

図１５、図１６は、任意の現在フレームの一例を示しており、フレーム内に特徴点およびフローを図示した図である。図１５は、左折中の前カメラ２１により得られたフレームを示している。図１６は、左折中の右カメラ２３により得られたフレームを示している。各図において、複数の白い丸で示すのは現特徴点Ｃであり、特徴点から引き出された黒色の線で示すのは誤フローＦｅであり、特徴点から引き出されたグレーの線で示すのは正フローＦｃである。「誤フロー」とは、Ｓ１０５における制約条件による判定により、制約条件を満たさないと判定されたフローである。「正フロー」とは、Ｓ１０５における制約条件による判定により、制約条件を満たすと判定されたフローである。

このように、正フローＦｃと誤フローＦｅとでは相違がある。第１実施形態では、抽出された現特徴点を用いて引いたフローの全てを自己位置推定に用いるのではなく、図１５、図１６に示される正フローＦｃの情報を用いて、車両ＭＣの自己位置推定が算出される。

［効果］
（１）上記第１実施形態の自己位置推定装置１によれば、センサ等によらず、マルチカメラとしてのカメラ２０によって撮影した画像のみから、３次元マップを作成することなく車両ＭＣの自己位置を推定できる。このため、自己位置推定装置１における処理時間を短縮できる。また、３次元マップを作成しないため、自己位置推定装置１におけるメモリリソースを削減できる。

（２）また、上記第１実施形態の自己位置推定装置１では、３次元マップを作成しないため、画像に対する奥行き方向の情報を現在フレームに反映させることができない。しかし、上記第１実施形態の自己位置推定装置１では、特徴点があると予想される範囲を、エピポーラ線からの所定距離として設定しており、エピポーラ線を基準とする２次元的な予測範囲を探索範囲Ｓとして設定できるため、特徴点の存在する範囲の予測ができ、探索処理時間を短縮することができる。

（３）上記第１実施形態の自己位置推定装置１によれば、車両ＭＣの予測される運動に基づいて計算されたエピポーラ線からの所定距離として予め定められた上限閾値を、フローの選別の指標として含んでいる。そして、選別部１５により、作成されたフローのうち、現特徴点の位置座標が、エピポーラ線からの所定距離として予め定められた上限閾値の範囲内にあるものが、正しいフローとして選別される。このため、車両ＭＣの自己位置推定の精度を向上させることができる。

（４）さらに、上記第１実施形態の自己位置推定装置１によれば、フロー長の統計としてのヒストグラムから得た基準範囲を、フローの選別の指標として含んでいる。そして、選別部１５により、作成されたフローのうち、フロー長が基準範囲内にあるものが、正しいフローとして選別される。このため、車両ＭＣの自己位置推定の精度を向上させることができる。

また、上記第１実施形態では、第１条件と第２条件を共に満たすフローのみ正しいフローとして選別し、自己位置推定に用いるため、さらに車両ＭＣの自己位置推定の精度を向上させることができる。

（５）上記第１実施形態の自己位置推定装置１によれば、第２条件は、前後左右の各カメラ２１～２４において取得されたデータに基づくヒストグラムの形態によって、各カメラ２１～２４において適宜個別に決定される。すなわち、ヒストグラムの形態から、最頻値近傍や平均値近傍など、適切な基準範囲が決定される。このため、より正確にフローの抽出を行うことができる。

（６）上記第１実施形態の自己位置推定装置１によれば、Ｓ１０２において、等速運動を前提として基本行列を作成しているため、基本行列の算出を容易に行うことができる。

（７）上記第１実施形態の自己位置推定装置１によれば、制約条件の判定（Ｓ１０５）を行ったのちに、判定条件をクリアしたフローの数が基準より少ない場合には（Ｓ１０７）、従前の制約条件を緩和して再度判定するようにしている（Ｓ１０８）。このため、自己位置推定に用いるフローの数が過度に少なくなることを回避できる。

Ｂ．他の実施形態：
（Ｂ１）上記第１実施形態の自己位置推定装置１は、車両ＭＣに搭載されるものとしたが、その他、船舶やドローン、自律移動型ロボットなどの物体や、人等の移動体に搭載されていてもよい。

（Ｂ２）上記第１実施形態の自己位置推定装置１において、カメラ２０は、前後左右の４つのカメラで構成したが、２つ以上の複眼カメラであればよい。

（Ｂ３）上記第１実施形態の自己位置推定装置１では、Ｓ１０２において、等速運動を前提として、前回フレームと現在フレームとの間の基本行列を作成したが、動作の予想は、前回計算値の利用の他、車両ＭＣのＣＡＮデータからのステアリング情報等を用いてもよい。

（Ｂ４）上記第１実施形態の自己位置推定装置１では、フロー長の基準範囲の決定に際して、ヒストグラム中の最頻値または平均値を利用したが、中央値を利用してもよい。例えば、統計において、フロー長が著しく大きく外れたものが存在するような場合には、平均値ではなく、中央値を適用するなど、適宜対応できる。

（Ｂ５）本開示に記載の自己位置推定装置１及びそれら手法は、コンピュータプログラムにより具体化された一つ乃至は複数の機能を実行するようにプログラムされたプロセッサ及びメモリを構成することによって提供された専用コンピュータにより、実現されてもよい。あるいは、本開示に記載の自己位置推定装置１及びそれら手法は、一つ以上の専用ハードウェア論理回路によってプロセッサを構成することによって提供された専用コンピュータにより実現されてもよい。もしくは、本開示に記載の自己位置推定装置１及びそれら手法は、一つ乃至は複数の機能を実行するようにプログラムされたプロセッサ及びメモリと一つ以上のハードウェア論理回路によって構成されたプロセッサとの組合わせにより構成された一つ以上の専用コンピュータにより実現されてもよい。また、コンピュータプログラム（自己位置推定プログラム）は、コンピュータにより実行されるインストラクションとして、コンピュータ読み取り可能な非遷移有形記録媒体に記憶されていてもよい。

本開示は、上述の実施形態に限られるものではなく、その趣旨を逸脱しない範囲において種々の構成で実現することができる。例えば、発明の概要の欄に記載した形態中の技術的特徴に対応する各実施形態中の技術的特徴は、上述の課題の一部又は全部を解決するために、あるいは、上述の効果の一部又は全部を達成するために、適宜、差し替えや、組み合わせを行うことが可能である。また、その技術的特徴が本明細書中に必須なものとして説明されていなければ、適宜、削除することが可能である。

１…自己位置推定装置、１０…車両制御部、１１…画像データ取得部、１２…特徴点抽出部、１３…記憶部、１４…フロー作成部、１５…選別部、１６…自己位置算出部、２０…カメラ、２１…前カメラ、２２…後カメラ、２３…右カメラ、２４…左カメラ、ＭＣ…車両

Claims

移動体（ＭＣ）の自己位置を推定する自己位置推定装置であって、
前記移動体に搭載された複数のカメラ（２１，２２，２３，２４）で構成されるマルチカメラ（２０）から出力された画像データを設定間隔毎に取得する画像データ取得部（１１）と、
前記画像データ取得部により取得された前記画像データから特徴点を抽出する特徴点抽出部（１２）と、
前記各画像データ内の前記各特徴点の位置情報、を記憶する記憶部（１３）と、
最新の画像データにおける特徴点である現特徴点と、過去の画像データにおける特徴点である前特徴点と、を対応付けて、時系列毎に異なる前記画像データにおいて対応する特徴点同士の組であるフローを作成するフロー作成部（１４）と、
前記フロー作成部により作成された前記フローのうち、前記移動体の予測される運動に基づいて計算されたエピポーラ拘束を含む予め定められた制約条件を用いて、前記制約条件を満たす前記フローを正しいフローとして選別する選別部（１５）と、
前記選別部により選別された前記正しいフローの情報を用いて、前記移動体の位置および向きを算出する自己位置算出部（１６）と、を備え、
前記制約条件は、時系列において連続する前記画像データにおける前記フローの長さであるフロー長の基準範囲を、前記フローの選別の指標として含み、
前記選別部は、作成された前記フローのうち、前記フロー長が前記基準範囲内にあるものを選別し、
前記フロー長の前記基準範囲は、前記複数のカメラごとに個別に設定される、自己位置推定装置。
移動体（ＭＣ）の自己位置を推定する自己位置推定装置であって、
前記移動体に搭載された複数のカメラ（２１，２２，２３，２４）で構成されるマルチカメラ（２０）から出力された画像データを設定間隔毎に取得する画像データ取得部（１１）と、
前記画像データ取得部により取得された前記画像データから特徴点を抽出する特徴点抽出部（１２）と、
前記各画像データ内の前記各特徴点の位置情報、を記憶する記憶部（１３）と、
最新の画像データにおける特徴点である現特徴点と、過去の画像データにおける特徴点である前特徴点と、を対応付けて、時系列毎に異なる前記画像データにおいて対応する特徴点同士の組であるフローを作成するフロー作成部（１４）と、
前記フロー作成部により作成された前記フローのうち、前記移動体の予測される運動に基づいて計算されたエピポーラ拘束を含む予め定められた制約条件を用いて、前記制約条件を満たす前記フローを正しいフローとして選別する選別部（１５）と、
前記選別部により選別された前記正しいフローの情報を用いて、前記移動体の位置および向きを算出する自己位置算出部（１６）と、を備え、
前記制約条件は、時系列において連続する前記画像データにおける前記フローの長さであるフロー長の基準範囲を、前記フローの選別の指標として含み、
前記選別部は、作成された前記フローのうち、前記フロー長が前記基準範囲内にあるものを選別し、
前記フロー長の前記基準範囲は、前記移動体の移動状態に応じて設定される、自己位置推定装置。
移動体（ＭＣ）の自己位置を推定する自己位置推定装置であって、
前記移動体に搭載された複数のカメラ（２１，２２，２３，２４）で構成されるマルチカメラ（２０）から出力された画像データを設定間隔毎に取得する画像データ取得部（１１）と、
前記画像データ取得部により取得された前記画像データから特徴点を抽出する特徴点抽出部（１２）と、
前記各画像データ内の前記各特徴点の位置情報、を記憶する記憶部（１３）と、
最新の画像データにおける特徴点である現特徴点と、過去の画像データにおける特徴点である前特徴点と、を対応付けて、時系列毎に異なる前記画像データにおいて対応する特徴点同士の組であるフローを作成するフロー作成部（１４）と、
前記フロー作成部により作成された前記フローのうち、前記移動体の予測される運動に基づいて計算されたエピポーラ拘束を含む予め定められた制約条件を用いて、前記制約条件を満たす前記フローを正しいフローとして選別する選別部（１５）と、
前記選別部により選別された前記正しいフローの情報を用いて、前記移動体の位置および向きを算出する自己位置算出部（１６）と、を備え、
前記選別部は、前記正しいフローとして選別した数が、予め定められた基準より少ない場合には、前記正しいフローの数が前記基準以上となるまで、従前に用いられた前記制約条件から変更された制約条件を用いて再度前記フローの選別を実行する、自己位置推定装置。
前記制約条件は、予測した前記移動体の運動に基づいて計算されたエピポーラ線からの距離として予め定められた上限閾値を、前記フローの選別の指標として含み、
前記選別部は、作成された前記フローのうち、前記現特徴点の位置座標が、前記最新の画像データ内において前記エピポーラ線から前記上限閾値の範囲内にあるものを選別する、請求項１から請求項３のいずれか一項に記載の自己位置推定装置。
前記フロー長の前記基準範囲は、前記フロー長の統計を用いて設定される、請求項１または請求項２に記載の自己位置推定装置。
前記フロー長の前記基準範囲は、前記フロー長の度数分布における最頻値を基準に設定される請求項５に記載の自己位置推定装置。
前記フロー長の前記基準範囲は、前記フロー長の度数分布における平均値を基準に設定される請求項５に記載の自己位置推定装置。
前記フロー長の前記基準範囲は、前記フロー長の度数分布における中央値を基準に設定される請求項５に記載の自己位置推定装置。
自己位置推定装置を用いて、移動体（ＭＣ）の自己位置を推定する自己位置推定方法であって、
前記自己位置推定装置は、画像データ取得部（１１）と、特徴点抽出部（１２）と、記憶部（１３）と、フロー作成部（１４）と、選別部（１５）と、自己位置算出部（１６）と、を備え、
前記画像データ取得部により、前記移動体に搭載された複数のカメラ（２１，２２，２３，２４）で構成されるマルチカメラ（２０）から出力された画像データを設定間隔毎に取得する工程と、
前記特徴点抽出部により、前記画像データ取得部により取得された前記画像データから特徴点を抽出する工程と、
前記記憶部により、前記各画像データ内の前記各特徴点の位置情報を記憶する工程と、
前記フロー作成部により、最新の画像データにおける特徴点である現特徴点と、過去の画像データにおける特徴点である前特徴点と、を対応付けて、時系列毎に異なる前記画像データにおいて対応する特徴点同士の組であるフローを作成する工程と、
前記選別部により、前記フロー作成部により作成された前記フローのうち、前記移動体の予測される運動に基づいて計算されたエピポーラ拘束を含む予め定められた制約条件を用いて、前記制約条件を満たす前記フローを正しいフローとして選別する工程と、
前記自己位置算出部により、前記選別部により選別された前記正しいフローの情報を用いて、前記移動体の位置および向きを算出する工程と、を備え、
前記制約条件は、時系列において連続する前記画像データにおける前記フローの長さであるフロー長の基準範囲を、前記フローの選別の指標として含み、
前記選別する工程は、作成された前記フローのうち、前記フロー長が前記基準範囲内にあるものを選別する工程を含み、
前記フロー長の前記基準範囲は、前記複数のカメラごとに個別に設定される、自己位置推定方法。
自己位置推定装置を用いて、移動体（ＭＣ）の自己位置を推定する自己位置推定方法であって、
前記自己位置推定装置は、画像データ取得部（１１）と、特徴点抽出部（１２）と、記憶部（１３）と、フロー作成部（１４）と、選別部（１５）と、自己位置算出部（１６）と、を備え、
前記画像データ取得部により、前記移動体に搭載された複数のカメラ（２１，２２，２３，２４）で構成されるマルチカメラ（２０）から出力された画像データを設定間隔毎に取得する工程と、
前記特徴点抽出部により、前記画像データ取得部により取得された前記画像データから特徴点を抽出する工程と、
前記記憶部により、前記各画像データ内の前記各特徴点の位置情報を記憶する工程と、
前記フロー作成部により、最新の画像データにおける特徴点である現特徴点と、過去の画像データにおける特徴点である前特徴点と、を対応付けて、時系列毎に異なる前記画像データにおいて対応する特徴点同士の組であるフローを作成する工程と、
前記選別部により、前記フロー作成部により作成された前記フローのうち、前記移動体の予測される運動に基づいて計算されたエピポーラ拘束を含む予め定められた制約条件を用いて、前記制約条件を満たす前記フローを正しいフローとして選別する工程と、
前記自己位置算出部により、前記選別部により選別された前記正しいフローの情報を用いて、前記移動体の位置および向きを算出する工程と、を備え、
前記制約条件は、時系列において連続する前記画像データにおける前記フローの長さであるフロー長の基準範囲を、前記フローの選別の指標として含み、
前記選別する工程は、作成された前記フローのうち、前記フロー長が前記基準範囲内にあるものを選別する工程を含み、
前記フロー長の前記基準範囲は、前記移動体の移動状態に応じて設定される、自己位置推定方法。
自己位置推定装置を用いて、移動体（ＭＣ）の自己位置を推定する自己位置推定方法であって、
前記自己位置推定装置は、画像データ取得部（１１）と、特徴点抽出部（１２）と、記憶部（１３）と、フロー作成部（１４）と、選別部（１５）と、自己位置算出部（１６）と、を備え、
前記画像データ取得部により、前記移動体に搭載された複数のカメラ（２１，２２，２３，２４）で構成されるマルチカメラ（２０）から出力された画像データを設定間隔毎に取得する工程と、
前記特徴点抽出部により、前記画像データ取得部により取得された前記画像データから特徴点を抽出する工程と、
前記記憶部により、前記各画像データ内の前記各特徴点の位置情報を記憶する工程と、
前記フロー作成部により、最新の画像データにおける特徴点である現特徴点と、過去の画像データにおける特徴点である前特徴点と、を対応付けて、時系列毎に異なる前記画像データにおいて対応する特徴点同士の組であるフローを作成する工程と、
前記選別部により、前記フロー作成部により作成された前記フローのうち、前記移動体の予測される運動に基づいて計算されたエピポーラ拘束を含む予め定められた制約条件を用いて、前記制約条件を満たす前記フローを正しいフローとして選別する工程と、
前記自己位置算出部により、前記選別部により選別された前記正しいフローの情報を用いて、前記移動体の位置および向きを算出する工程と、を備え、
前記選別する工程は、前記正しいフローとして選別した数が、予め定められた基準より少ない場合には、前記正しいフローの数が前記基準以上となるまで、従前に用いられた前記制約条件から変更された制約条件を用いて再度前記フローの選別を実行する工程を含む、自己位置推定方法。
移動体（ＭＣ）の自己位置を推定する自己位置推定プログラムであって、
前記移動体に搭載された複数のカメラ（２１，２２，２３，２４）で構成されるマルチカメラ（２０）から出力された画像データを設定間隔毎に取得する機能と、
前記画像データから特徴点を抽出する機能と、
前記各画像データ内の前記各特徴点の位置情報、を記憶する機能と、
最新の画像データにおける特徴点である現特徴点と、過去の画像データにおける特徴点である前特徴点と、を対応付けて、時系列毎に異なる前記画像データにおいて対応する特徴点同士の組であるフローを作成する機能と、
作成された前記フローのうち、前記移動体の予測される運動に基づいて計算されたエピポーラ拘束を含む予め定められた制約条件を用いて、前記制約条件を満たす前記フローを正しいフローとして選別する機能と、
選別された前記正しいフローの情報を用いて、前記移動体の位置および向きを算出する機能と、をコンピュータに実現させ、
前記制約条件は、時系列において連続する前記画像データにおける前記フローの長さであるフロー長の基準範囲を、前記フローの選別の指標として含み、
前記選別する機能は、作成された前記フローのうち、前記フロー長が前記基準範囲内にあるものを選別する機能を含み、
前記フロー長の前記基準範囲は、前記複数のカメラごとに個別に設定される、自己位置推定プログラム。
移動体（ＭＣ）の自己位置を推定する自己位置推定プログラムであって、
前記移動体に搭載された複数のカメラ（２１，２２，２３，２４）で構成されるマルチカメラ（２０）から出力された画像データを設定間隔毎に取得する機能と、
前記画像データから特徴点を抽出する機能と、
前記各画像データ内の前記各特徴点の位置情報、を記憶する機能と、
最新の画像データにおける特徴点である現特徴点と、過去の画像データにおける特徴点である前特徴点と、を対応付けて、時系列毎に異なる前記画像データにおいて対応する特徴点同士の組であるフローを作成する機能と、
作成された前記フローのうち、前記移動体の予測される運動に基づいて計算されたエピポーラ拘束を含む予め定められた制約条件を用いて、前記制約条件を満たす前記フローを正しいフローとして選別する機能と、
選別された前記正しいフローの情報を用いて、前記移動体の位置および向きを算出する機能と、をコンピュータに実現させ、
前記制約条件は、時系列において連続する前記画像データにおける前記フローの長さであるフロー長の基準範囲を、前記フローの選別の指標として含み、
前記選別する機能は、作成された前記フローのうち、前記フロー長が前記基準範囲内にあるものを選別する機能を含み、
前記フロー長の前記基準範囲は、前記移動体の移動状態に応じて設定される、自己位置推定プログラム。
移動体（ＭＣ）の自己位置を推定する自己位置推定プログラムであって、
前記移動体に搭載された複数のカメラ（２１，２２，２３，２４）で構成されるマルチカメラ（２０）から出力された画像データを設定間隔毎に取得する機能と、
前記画像データから特徴点を抽出する機能と、
前記各画像データ内の前記各特徴点の位置情報、を記憶する機能と、
最新の画像データにおける特徴点である現特徴点と、過去の画像データにおける特徴点である前特徴点と、を対応付けて、時系列毎に異なる前記画像データにおいて対応する特徴点同士の組であるフローを作成する機能と、
作成された前記フローのうち、前記移動体の予測される運動に基づいて計算されたエピポーラ拘束を含む予め定められた制約条件を用いて、前記制約条件を満たす前記フローを正しいフローとして選別する機能と、
選別された前記正しいフローの情報を用いて、前記移動体の位置および向きを算出する機能と、をコンピュータに実現させ、
前記選別する機能は、前記正しいフローとして選別した数が、予め定められた基準より少ない場合には、前記正しいフローの数が前記基準以上となるまで、従前に用いられた前記制約条件から変更された制約条件を用いて再度前記フローの選別を実行する機能を含む、自己位置推定プログラム。