JP2018139084A - 装置、移動体装置及び方法 - Google Patents

Abstract

【課題】 環境地図の生成や移動体に関する情報の計測を安定して精度良く行うことができる装置を提供する。【解決手段】 地図生成・位置計測装置１００は、車両（移動体）に搭載される装置であって、車両の周囲の３次元情報を取得するステレオカメラ２００（第１の取得手段）と、車両の周囲の３次元情報を取得する単眼カメラ３００（第２の取得手段）と、ステレオカメラ２００で取得された３次元情報に基づいて、単眼カメラ３００が３次元情報を取得する領域である取得領域を制御するカメラ姿勢制御部３００ｂ（制御手段）と、単眼カメラ３００で取得された、取得領域の３次元情報に基づいて、車両の周囲の環境地図の生成及び車両に関する情報の計測を行う位置計測部５２０（手段）と、を備える装置である。【選択図】図３

Description

本発明は、装置、移動体装置及び方法に関する。

従来、移動体の周囲の３次元情報を取得し、移動体の周囲の環境地図の生成や、移動体の位置、姿勢、速度等の移動体に関する情報の計測を行う装置が知られている（例えば特許文献１参照）。

しかしながら、特許文献１に開示されている装置では、移動体の周囲の環境地図の生成や移動体に関する情報の計測を安定して精度良く行うことに関して改善の余地があった。

本発明は、移動体に搭載される装置であって、前記移動体の周囲の３次元情報を取得する第１の取得手段と、前記第１の取得手段が３次元情報を取得する領域である第１の取得領域の少なくとも一部の３次元情報を取得する第２の取得手段と、前記第１の取得手段で取得された、前記第１の取得領域の３次元情報に基づいて、前記第２の取得手段が前記第１の取得領域において３次元情報を取得する領域である第２の取得領域を制御する制御手段と、前記第１の取得手段で取得された、前記第１の取得領域の３次元情報と、前記第２の取得手段で取得された、前記第２の取得領域の３次元情報とに基づいて、前記移動体の周囲の環境地図の生成及び前記移動体に関する情報の計測の少なくとも一方を行う手段と、を備える装置である。

本発明によれば、移動体の周囲の環境地図の生成や移動体に関する情報の計測を安定して精度良く行うことができる。

図１（ａ）及び図１（ｂ）は、それぞれ本発明の一実施形態の移動体装置の外観を概略的に示す図である。 図１のステレオカメラの撮影範囲と初期姿勢にある単眼カメラの撮影範囲を示す図である。 図１の地図生成・位置計測装置の構成例を示すブロック図である。 地図生成・位置計測処理について説明するためのフローチャートである。 特徴点抽出処理１について説明するためのフローチャートである。 特徴点抽出処理２について説明するためのフローチャートである。 特徴点対応付け処理について説明するためのフローチャートである。 基準カメラの座標系から単眼カメラの座標系への座標変換について説明するための図である。 地図生成・位置計測装置により立体物が検出された領域を示す図である。 地図生成・位置計測装置により抽出された特徴点を示す図である。 地図生成・位置計測装置により抽出された特徴点を立体物特徴点と非立体物特徴点に区別して示す図である。 特徴点分類処理を説明するためのフローチャートである。 位置推定処理について説明するためのフローチャートである。 環境地図生成処理について説明するためのフローチャートである。 カメラ姿勢制御処理について説明するためのフローチャートである。 カメラ姿勢制御処理後の単眼カメラの撮影範囲を示す図である。 図１７（ａ）及び図１７（ｂ）は、それぞれ変形例１の移動体装置の外観を概略的に示す図である。 変形例１のライダの投光範囲と初期姿勢にある単眼カメラの撮影範囲を示す図である。 変形例１のライダの投光範囲と姿勢変更後の単眼カメラの撮影範囲を示す図である。 変形例１のライダ（走査型）の投光系及び受光系の構成を説明するための図である。 変形例１のライダ（非走査型）の投光系及び受光系の構成と、光源又は受光素子と検出領域の関係を説明するための図である。 変形例２のライダの投光範囲（初期状態）と単眼カメラの撮影範囲を示す図である。 変形例２のライダの投光範囲（変更後）と単眼カメラの撮影範囲を示す図である。 変形例３のライダの投光範囲（初期状態）とステレオカメラの撮影範囲を示す図である。 変形例３のライダの投光範囲（変更後）とステレオカメラの撮影範囲を示す図である。 ステレオカメラとライダが一体的に構成される例について説明するための図である。

以下、本発明の一実施形態について図面を参照して説明する。図１（ａ）には、一実施形態の移動体装置１の側面図が模式的に示されている。図１（ｂ）には、移動体装置１の正面図が概略的に示されている。

《装置構成》
移動体装置１は、図１に示されるように、移動体としての車両１０と、該車両１０に搭載される、本発明の装置の一例としての地図生成・位置計測装置１００とを備えている。車両１０は、自動車である。

地図生成・位置計測装置１００は、自己位置すなわち車両１０の位置を計測するとともに車両１０周辺の環境地図を生成する装置であり、ステレオカメラ２００及び単眼カメラ３００を含んで構成される。

ステレオカメラ２００及び単眼カメラ３００は、車両１０のバックミラー近傍に設けられている。

詳述すると、ステレオカメラ２００及び単眼カメラ３００は、上下に離間するように同一の支持部材を介して車両１０の天井に吊り下げ支持されている。

ステレオカメラ２００は、左側撮像部である撮像部２００ａと、右側撮像部である撮像部２００ｂと、視差画像生成部２００ｃ（図３参照）とを有する。撮像部２００ａ、２００ｂは、イメージセンサ（ＣＭＯＳやＣＣＤ）を含む。

ステレオカメラ２００は、図２に示されるように、撮影範囲（左眼画角（撮像部２００ａの画角）と右眼画角（撮像部２００ｂの画角）を併せた範囲）が車両１０前方となるように上記支持部材に固定されている。

視差画像生成部２００ｃは、撮像部２００ａ、２００ｂで撮影した画像（輝度画像）に対して平行ステレオ法を用いて視差画像を生成する。

単眼カメラ３００は、姿勢変更可能に設けられた撮像部３００ａと、該撮像部３００ａの姿勢を制御するカメラ姿勢制御部３００ｂ（図３参照）と、撮像部３００ａの姿勢を計測するカメラ姿勢計測部３００ｃ（図３参照）とを有する。なお、図２において、撮像部３００ａは、撮影範囲がステレオカメラ２００の撮影範囲と略一致する初期姿勢にある。ここでは、撮像部３００ａは、ステレオカメラ２００の撮影範囲に対して画角が同程度のものが用いられているが、画角が大きいものや小さいものを用いても良い。

撮像部３００ａは、ステレオカメラ２００の撮影範囲を含む所定範囲内で撮影範囲が変更可能となるようにパン方向（垂直軸周り）及びチルト方向（水平軸周り）に回転可能に（姿勢変更可能に）保持する保持機構を介して上記支持部材に取り付けられている。撮像部３００ａは、イメージセンサ（ＣＭＯＳやＣＣＤ）を含む。

カメラ姿勢制御部３００ｂは、上記保持機構を駆動して撮像部３００ａをパン方向及び／又はチルト方向に回転させる駆動部を有し、後述する記憶部５３０に格納されたカメラ姿勢情報５３０ｂを基に撮像部３００ａの姿勢制御を行う。上記駆動部は、パン方向回転用のステッピングモータ及びチルト方向回転用のステッピングモータを含む。

カメラ姿勢計測部３００ｃは、各ステッピングモータのステップ数（各ステッピングモータに印加される駆動パルスのパルス数）をカウントするカウンタを有し、撮像部３００ａのパン方向及びチルト方向の回転角（回転位置）すなわち撮像部３００ａの姿勢を計測する。

なお、上記駆動部にステッピングモータに代えて、サーボモータを用いても良い。この場合、カメラ姿勢計測部は、サーボモータのエンコーダから、撮像部３００ａのパン方向及びチルト方向の回転角（回転位置）を得ることができる。

また、ステレオカメラ２００と単眼カメラ３００の位置関係は、図１の位置関係に限らず、要は、ステレオカメラ２００の撮影範囲を含む所定範囲内で単眼カメラ３００の撮影範囲が変更可能となる位置関係であれば良い。

地図生成・位置計測装置１００は、ステレオカメラ２００及び単眼カメラ３００に加えて、制御装置５００を備えている。制御装置５００は、例えばＣＰＵやＦＰＧＡによって実現される。

制御装置５００は、ステレオカメラ２００から出力された輝度画像及び視差画像と、単眼カメラ３００から出力された輝度画像とから画像中の特徴点を抽出する画像処理部５１０と、該画像処理部５１０が抽出した特徴点を基に車両１０の位置を計測するとともに車両１０周辺の環境地図を生成する位置計測部５２０と、該位置計測部５２０が計測した車両１０の位置情報、画像処理部５１０が抽出した特徴点の３次元情報等を記憶する記憶部５３０と、ステレオカメラ２００の撮像部２００ａ、２００ｂ及び単眼カメラ３００の撮像部３００ａを制御する撮像制御部５４０とを含む。

画像処理部５１０は、画像中にある立体物を検出する立体物検出部５１０ａと、画像中にある特徴点を抽出する特徴点抽出部５１０ｂと、同一のカメラで異なる時刻に撮影した画像間で同じ特徴点を探索する特徴点追跡部５１０ｃと、異なるカメラで同一時刻に撮影された画像間で同じ特徴点を探索する特徴点マッチング部５１０ｄとを含む。

立体物検出部５１０ａでは、例えば特開２０１５‐２０７２８１号公報に開示されている立体物抽出方法を用いることができる。

特徴点抽出部５１０ｂでは、ＳＨＩＦＴ法やＦＡＳＴ法などの公知の技術を用いることができる。

特徴点追跡部５１０ｃでは、ＳＨＩＦＴ法などの特徴量記述を用いた手法や、ブロックマッチングのような画像テンプレートベースの手法を用いることができる。

特徴点マッチング部５１０ｄは、単眼カメラ３００で撮影した画像において、ステレオカメラ２００で抽出された特徴点を探索する。

位置計測部５２０は、画像処理部５１０から得られる特徴点情報及び立体物情報を基に特徴点を分類する特徴点分類部５２０ａと、車両１０の位置を推定し、その推定結果を車両制御装置７００に出力する位置推定部５２０ｂと、環境地図生成部５２０ｃとを含む。位置計測部５２０は、ＧＮＳＳ（衛星測位システム）６００から自車両周辺の環境地図のベースとなる基準データを受信し、該基準データを基に自車両周辺の環境地図をリアルタイムに生成するとともに自車両の位置情報を取得し、生成した環境地図のデータと取得した自車両の位置情報を車両制御装置７００に出力する。

車両制御装置７００は、受信した環境地図のデータと自車両の位置情報に基づいて自車両の制御（例えばオートブレーキ、オートステアリング、オートアクセル等）や、運転者への注意喚起（例えば警告音の発生、カーナビの画面上における警告表示等）を行う。

記憶部５３０には、特徴点がどのカメラでどの時刻に画像上のどの位置に撮影されたかを示す情報が含まれる特徴点観測情報５３０ａと、各時刻での単眼カメラ３００の姿勢情報であるカメラ姿勢情報５３０ｂと、特徴点のある基準の座標系での３次元位置の情報を含む特徴点３次元情報５３０ｃと、位置計測部５２０で計測された車両の位置情報と、ＧＮＳＳ６００から位置計測部５２０に送信された上記基準データとが格納される。

《地図生成・位置計測処理》
以下、地図生成・位置計測装置１００で実施される地図生成・位置計測処理の概要について、図４のフローチャートを参照して説明する。地図生成・位置計測処理は、位置計測部５２０が車両制御装置７００から処理開始要求を受信したときに開始される。車両制御装置７００は、例えば地図生成・位置計測装置１００が搭載された車両１０の電気系統がＯＮになったときに位置計測部５２０に処理開始要求を送信する。

最初のステップＳ１では、特徴点抽出処理１を実施する。特徴点抽出処理１の詳細については後述する。

次のステップＳ２では、特徴点抽出処理２を実施する。特徴点抽出処理２の詳細については後述する。なお、図４では特徴点抽出処理２が特徴点抽出処理１の後となっているが、実際には特徴点抽出処理１、２は同時に開始され、一部の処理が並行して行われる。従って、図４において、特徴点抽出処理１、２の順序は逆でも良い。

次のステップＳ３では、特徴点対応付け処理を実施する。特徴点対応付け処理の詳細については後述する。

次のステップＳ４では、特徴点追跡処理を実施する。特徴点追跡処理は、特徴点追跡部５１０ｃにより、ＳＨＩＦＴ法などの特徴量記述を用いた手法や、ブロックマッチングのような画像テンプレートベースの手法で行われる。

次のステップＳ５では、特徴点分類処理を実施する。特徴点分類処理の詳細については後述する。

次のステップＳ６では、位置推定処理を実施する。位置推定処理の詳細については後述する。

次のステップＳ７では、環境地図生成処理を実施する。環境地図生成処理の詳細については後述する。

次のステップＳ８では、カメラ姿勢制御処理を実施する。カメラ姿勢制御処理の詳細については後述する。

ステップＳ９では、処理を終了するか否かを判断する。ここでの判断は、位置計測部５２０が車両制御装置７００から処理終了要求を受信しているときに肯定され、受信していないときに否定される。車両制御装置７００は、例えば、地図生成・位置計測装置１００が搭載された車両１０の電気系統がＯＦＦになったときに位置計測部５２０に処理終了要求を送る。ステップＳ９での判断が肯定されるとフローは終了し、否定されるとステップＳ１に戻る。

《特徴点抽出処理１》
次に、特徴点抽出処理１（図４のステップＳ１）について、図５のフローチャートを参照して詳細に説明する。

最初のステップＳ１．１では、ステレオカメラ２００で撮影を行う。具体的には、ステレオカメラ２００の撮像部２００ａ、２００ｂが自車両前方を撮影し、撮像部２００ａ、２００ｂの撮影画像（輝度画像）のデータを視差画像生成部２００ｃに送信するとともに撮像部２００ｂの撮影画像（輝度画像）を特徴点抽出部５１０ｂに送信する。ステレオカメラ２００は、撮像制御部５４０から撮影開始信号を受信したときに撮影を開始する。撮像制御部５４０は、撮影開始信号をステレオカメラ２００と単眼カメラ３００に同期して送信する。

次のステップＳ１．２では、視差画像を生成する。具体的には、視差画像生成部２００ｃが、受信した２つの輝度画像のデータを基に平行化のためのリマップ処理などを行って視差画像を生成し、該視差画像のデータを立体物検出部５１０ａに送信する。

次のステップＳ１．３では、立体物を検出する。具体的には、立体物検出部５１０ａが、受信した視差画像のデータから、上述した立体物抽出方法を用いて立体物を検出し、その検出結果を特徴点分類部５２０ａに送信する。

最後のステップＳ１．４では、画像中の特徴点を抽出する。具体的には、特徴点抽出部５１０ｂが、受信した撮像部２００ｂの輝度画像のデータ（３次元情報）から、立体物及び非立体物の特徴点を抽出する。

《特徴点抽出処理２》
次に、特徴点抽出処理２（図４のステップＳ２）について、図６のフローチャートを参照して詳細に説明する。

最初のステップＳ２．１では、第ｋ姿勢（ｋ≧１）にある単眼カメラ３００で撮影を行う。具体的には、単眼カメラ３００の撮像部３００ａで自車両前方を撮影し、得られた輝度画像のデータ（３次元情報）を特徴点抽出部５１０ｂに送信する。なお、単眼カメラ３００は、撮像制御部５４０から撮影開始信号を受信したときに撮影を開始する。すなわち、ステップＳ２．１は、ステレオカメラ２００での撮影（上記ステップＳ１．１）と同時に開始される。

ここで、「第ｋ姿勢」は、単眼カメラ３００の既知の複数種類（Ｋ種類）の姿勢のうち任意の一の姿勢であって、図４の地図生成・位置計測処理のｋ番目のループにおける姿勢を意味する。地図生成・位置計測処理の１番目（最初）のループにおいて特徴点抽出処理２が実施されるときの単眼カメラ３００の姿勢は、初期姿勢である。

次のステップＳ２．２では、単眼カメラ３００の姿勢を計測する。具体的には、カメラ姿勢計測部３００ｃが各ステッピングモータのステップ数をカウントして、単眼カメラ３００の現在の姿勢がＫ種類の姿勢のうちいずれの姿勢であるかを判別する。この際、カウントしたステップ数を、カメラ姿勢情報５３０ｂとして記憶部５３０に格納する。なお、図４の地図生成・位置計測処理の最初のループにおいて特徴点抽出処理２が実施されるときの各ステッピングモータのステップ数は０であり、単眼カメラ３００の姿勢は初期姿勢である。

最後のステップＳ２．３では、画像中の特徴点を抽出する。具体的には、特徴点抽出部５１０ｂが、受信した単眼カメラ３００の撮影画像（輝度画像）のデータ（３次元情報）から、立体物及び非立体物の特徴点を抽出する。

《特徴点対応付け処理》
次に、特徴点対応付け処理（図４のステップＳ３）について、図７のフローチャートを参照して詳細に説明する。特徴点対応付け処理は、特徴点マッチング部５１０ｄにより実施される。

特徴点対応付け処理は、ステレオカメラ２００の輝度画像から抽出された特徴点を単眼カメラ３００の座標系上で探索する処理である。すなわち、特徴点対応付け処理は、ステレオカメラ２００で得られた特徴点について、単眼カメラ３００の座標系上でどこにあるかを、ステレオカメラ２００の輝度画像での観測座標と、その視差値から求める処理である。

最初のステップＳ３．１では、ｎに１をセットする。

次のステップＳ３．２では、ステレオカメラ２００の輝度画像（より詳細には撮像部２００ｂの輝度画像）から抽出された第ｎ特徴点の初期位置を算出する。その算出方法については、後述する。

次のステップＳ３．３では、抽出された第ｎ特徴点の初期位置近傍のｉピクセルの画素ブロック内に単眼カメラ３００の輝度画像から抽出された特徴点があるか否かを判断する。ここでの判断が肯定されるとステップＳ３．４に移行し、否定されるとステップＳ３．７に移行する。このように第ｎ特徴点の初期位置周辺にある特徴点に対してマッチングを行うことで、類似パターンが続くような環境でロバストに特徴点のマッチングを行うことができる。

次のステップＳ３．４では、コスト値を算出する。ここで「コスト値」とは、ステレオカメラ２００の輝度画像から抽出された特徴点に対する、単眼カメラ３００の輝度画像から抽出された特徴点の一致度を表す評価値である。

次のステップＳ３．５では、コスト値が閾値以下であるか否かを判断する。ここでの判断が肯定されるとステップＳ３．６に移行し、否定されるとステップＳ３．７に移行する。

次のステップＳ３．６では、単眼カメラ３００の輝度画像上における第ｎ特徴点の位置情報（以下では「単眼カメラ３００の観測情報」とも呼ぶ）を特徴点観測情報５３０ａに追加する。すなわち、第ｎ特徴点の初期位置近傍のｉピクセルの画素ブロック内にある特徴点のコスト値が閾値以下であれば単眼カメラ３００の観測情報を特徴点観測情報５３０ａとして記憶部５３０に追加する。第ｎ特徴点の初期位置近傍のｉピクセルの画素ブロック内に特徴点が存在しない場合、又は閾値以下のコスト値を持つ特徴点が存在しない場合は、特徴点観測情報５３０ａに単眼カメラ３００の観測情報を追加しない。

次のステップＳ３．７では、ｎ＜Ｎであるか否かを判断する。Ｎは、ステレオカメラ２００の輝度画像から抽出された特徴点の総数である。ここでの判断が肯定されるとステップＳ３．８に移行し、否定されるとフローは終了する。

ステップＳ３．８では、ｎをインクリメントする。ステップＳ３．８が実行されると、ステップＳ３．２に戻る。

次に、上記ステップＳ３．２における第ｎ特徴点の初期位置の算出方法について図８を参照して説明する。

ここで、ステレオカメラ２００の左右２つの撮像部２００ａ、２００ｂのうち右側の撮像部２００ｂを基準カメラとする。基準カメラで撮影された特徴点の基準カメラ座標系（基準カメラの座標系）での３次元座標P(Xi,Yi,Zi)は、該特徴点の基準カメラのセンサ座標系（イメージセンサの座標系）での２次元座標p(xi,yi)と、その視差値diを用いて次の（１）式で表す事ができる。

但し、bはステレオカメラ２００の基線長、f_sはステレオカメラ２００の焦点距離を示す値、cx_s,cy_sはステレオカメラ２００の中心を示す値である。これらの値は事前に校正されており、このシステムが動作する時点では既知のものとする。

基準カメラ座標系での座標P(Xi,Yi,Zi)の、単眼カメラ３００の座標系での座標(Xi^p,
Yi^p,Zi^p)への変換は、次の（２）式で行うことができる（図８参照）。

pRrは、基準カメラ座標系から単眼カメラ３００の座標系へ座標変換を行う回転成分である。pRrは、カメラ姿勢計測部３００ｃによって計測することが可能である。
pTrは、基準カメラ座標系から単眼カメラ３００の座標系へ座標変換を行う平行移動成分である。基準カメラ座標における単眼カメラ３００の位置が既知であれば、pTr=pRr^-1pTとして求めることが可能である。

ステレオカメラ２００で観測された特徴点の、単眼カメラ３００で撮影された画像上で観測される位置を次の（３）式によって求めることができる。

但し、fpは単眼カメラ３００の焦点距離を示す値、cxp,cypは単眼カメラ３００のセンサ座標系（イメージセンサの座標系）の中心を示す値である。これらの値は事前に校正されており、このシステムが動作する時点では既知のものとする。

《立体物の検出》
次に、上記ステップＳ１．３における立体物の検出について具体的に説明する。
図９において、矩形破線で囲まれた領域が画像の中で立体物が検出された領域（以下では「立体物検出領域」とも呼ぶ）である。
例えば特開２０１５‐２０７２８１号公報などの既知の方法を利用することで、画像の中で人や車などの路面上にある立体物を検出することが可能である。図９における矩形破線のように、例えば凸型の４角形によって立体物検出領域を定義することができる。立体物検出領域の情報としては、例えば矩形破線の４つの頂点の画像上の座標を出力する。

図１０において、白丸は画像中から抽出された特徴点を示す。また、立体物検出領域は矩形破線で示されている。そこで、上記ステップＳ５の特徴点分類処理によって、図１１に示されるように、特徴点の座標と立体物検出領域の情報から、特徴点を立体物特徴点（立体物の特徴点）と非立体物特徴点（立体物以外の特徴点）に分類することができる。

《特徴点分類処理》
次に、特徴点分類処理（図４のステップＳ５）について、図１２のフローチャートを参照して詳細に説明する。特徴点分類処理は、特徴点分類部５２０ａにより実施される。

最初のステップＳ５．１では、ｎに１をセットする。

次のステップＳ５．２では、ステレオカメラ２００の輝度画像（より詳細には撮像部２００ｂの輝度画像）から抽出された第ｎ特徴点が立体物検出領域内にあるか否かを判断する。具体的には、矩形の立体物検出領域の内部に特徴点が存在するか否かを計算する。このために、立体物検出領域を２つの三角形に分離し、特徴点が２つの三角形のいずれに含まれているかを判定する。ある点がある三角形の内部にあるか外部にあるかを判定する処理は、一般的に三角形の内外判定処理と呼ばれ、外積を用いた手法が知られており、それを用いればよい。ステップＳ５．２での判断が肯定されるとステップＳ５．３に移行し、否定されるとステップＳ５．４に移行する。

ステップＳ５．３では、第ｎ特徴点を立体物特徴点として記憶部５３０の特徴点３次元情報５３０ｃに登録する。

ステップＳ５．４では、第ｎ特徴点を非立体物特徴点として記憶部５３０の特徴点３次元情報５３０ｃに登録する。

ステップＳ５．５では、ｎ＜Ｎであるか否かを判断する。Ｎは、ステレオカメラ２００の輝度画像から抽出された特徴点の総数である。ここでの判断が肯定されるとステップＳ５．６に移行し、否定されるとフローは終了する。

ステップＳ５．６では、ｎをインクリメントする。ステップＳ５．６が実行されると、ステップＳ５．２に戻る。

《位置推定処理》
次に、位置推定処理（図４のステップＳ６）について図１３を参照して説明する。位置推定処理は、位置推定部５２０ｂにより実施される。

最初のステップＳ６．１では、ＲＡＮＳＡＣによるインライア選別を行う。すなわち、ＲＡＮＳＡＣ法により、非立体物特徴点の中から静止特徴点を抽出する。特徴点の中にはステレオカメラ２００での視差計算での誤マッチングや、特徴点追跡部５１０ｃでの誤マッチングなどが起こりうるため、誤マッチングの影響を低減するためにＲＡＮＳＡＣ法を用いる。

ここで、カメラの位置姿勢(R,t)をパラメータとするコスト関数は、ある特徴点に対して次の（４）式で求めることができる。

ここで、ex_i ^r及びey_i ^rは、それぞれ基準カメラ（右側の撮像部２００ｂ）でのx方向及びy方向の再投影誤差である。ex_i ^lは、左カメラ（左側の撮像部２００ａ）でのx方向の再投影誤差である。ex_i ^p及びey_i ^pは単眼カメラ３００でのx座標とy座標の再投影誤差である。W_i ^rは、基準カメラでの重みであり、基準カメラで追跡成功した特徴点には1が、それ以外には0がセットされる。W_i ^pは、単眼カメラ３００での重みであり、単眼カメラ３００で検出された特徴点には1がセットされ、それ以外には0がセットされる。

ここで、基準カメラ座標系での点の位置を[X_i ^r(R,t),Y_i ^r(R,t),Z_i ^r(R,t)]、地図座標系での点の位置を[X_i ^w,Y_i ^w,Z_i ^w]、単眼カメラ３００の座標系での点の位置を[X_i ^p(R,t),Y_i ^p(R,t),Z_i ^p(R,t)]とすると、次の（５）式及び（６）式でそれぞれ変換することができる。

次のステップＳ６．２では、インライアの特徴点で車両１０の位置を推定する。具体的には、ＲＡＮＳＡＣ法でインライアの特徴点が抽出された後、インライアの特徴点のみで車両１０の位置の推定を行う。位置推定は上述した特徴点毎の再投影誤差のコストの最小化問題として定式化することができ、Levemberg-Marquart法などの非線形の最小化法を用いて最適な位置を推定することができる。

次のステップＳ６．３では、立体物特徴点から静止特徴点を抽出する。具体的には、ステップＳ６．２で推定された位置情報を用いて立体物特徴点について再投影誤差を算出し、閾値計算により静止特徴点か否かを判定する。これにより、静止していた車両が動き出した場合などの静止していた特徴点が運動を始めた場合の影響を抑えることができる。

次のステップＳ６．４では、静止特徴点で車両１０の位置を推定する。具体的には、立体物特徴点のうち静止していると判定された特徴点について、再度位置推定のコスト関数（上記（４）式）に組み込み、非線形の最小化問題を解くことにより最適な位置を求める。

《環境地図生成処理》
次に、環境地図生成処理（図４のステップＳ７）について、図１４を参照して説明する。環境地図生成処理は、ＧＮＳＳ６００から送信される、環境地図のベースとなる基準データに対して、特徴点情報を追加することにより、精度の高い環境地図を生成する処理である。環境地図生成処理は、環境地図生成部５２０ｃによって実施される。

最初のステップＳ７．１では、ｎに１をセットする。

次のステップＳ７．２では、ステレオカメラ２００の輝度画像（より詳細には撮像部２００ｂの輝度画像）から抽出された第ｎ特徴点が環境地図上（但しｎ＝１のときは基準データ上）に存在するか否かを判断する。ここでの判断が否定されるとステップＳ７．３に移行し、肯定されるとステップＳ７．４に移行する。

ステップＳ７．３では、第ｎ特徴点が非立体物特徴点であるか否かを判断する。ここでの判断が肯定されるとステップＳ７．５に移行し、否定されるとステップＳ７．８に移行する。

ステップＳ７．４では、第ｎ特徴点がインライアとして判定されたか否かを判断する。ここでの判断が肯定されるとステップＳ７．６に移行し、否定されるとステップＳ７．７に移行する。

ステップＳ７．５では、環境地図（但しｎ＝１のときは基準データ）に、第ｎ特徴点の３次元情報（非立体物特徴点であること）を特徴点３次元情報５３０ｃとして追加するとともに第ｎ特徴点の観測情報（単眼カメラ３００の輝度画像上における第ｎ特徴点の位置情報）を特徴点観測情報５３０ａとして追加する。すなわち、第ｎ特徴点が環境地図に無い場合、第ｎ特徴点が非立体物特徴点であればステレオカメラ２００から得られる視差値を用いて３次元再構成を行い、立体物特徴点であれば環境地図への追加は行わない。ステップＳ７．５が実行されると、ステップＳ７．８に移行する。

ステップＳ７．６では、環境地図（但しｎ＝１のときは基準データ）に第ｎ特徴点の観測情報（単眼カメラ３００の輝度画像上における第ｎ特徴点の位置情報）を特徴点観測情報５３０ａとして追加する。ステップＳ７．６が実行されると、ステップＳ７．８に移行する。

ステップＳ７．７では、環境地図（但しｎ＝１のときは基準データ）から第ｎ特徴点を除外する。ステップＳ７．７が実行されると、ステップＳ７．８に移行する。

ステップＳ７．８では、バンドル調整を行う。すなわち、特徴点の情報が環境地図に追加された後、バンドル調整法を用いて全体の再投影誤差の最小化を行い車両１０の位置情報と、特徴点の３次元情報を最適化する。

ステップＳ７．９では、ｎ＜Ｎであるか否かを判断する。Ｎは、ステレオカメラ２００の輝度画像から抽出された特徴点の総数である。ここでの判断が肯定されるとステップＳ７．９５に移行し、否定されるとフローは終了する。

ステップＳ７．９５では、ｎをインクリメントする。ステップＳ７．９５が実行されると、ステップＳ７．２に戻る。

《カメラ姿勢制御処理》
次に、図４のステップＳ８のカメラ姿勢制御処理について、図１５のフローチャートを参照して説明する。カメラ姿勢制御処理は、カメラ姿勢制御部３００ｂにより実施される。なお、単眼カメラ３００がとり得る既知の姿勢が予め複数種類（Ｋ種類）用意されているものとする。単眼カメラ３００の姿勢は、静止した特徴点がなるべく多く画角に入る姿勢であることが好ましい。画像中の特徴点の中で、非立体物特徴点は、通常、静止した特徴点であるため、単眼カメラ３００の姿勢は、非立体物特徴点がなるべく多く画角に入る姿勢であることが望ましい。

最初のステップＳ８．１では、ｋに１をセットする。

次のステップＳ８．２では、第ｋ姿勢にある単眼カメラ３００で撮影可能な特徴点の数Ｎａを算出する。Ｎａは、座標変換とセンサ座標面への投影の式（上記（２）式や（３）式）を用いることにより算出することができる。

次のステップＳ８．３では、立体物特徴点のうち第ｋ姿勢にある単眼カメラ３００で撮影可能なものの数Ｎｓを算出する。Ｎｓは、座標変換とセンサ座標面への投影の式（上記（２）式や（３）式）を用いることにより算出することができる。

次のステップＳ８．４では、姿勢スコアＳを算出する。ここでは、Ｓ＝Ｎａ−Ｎｓと定義する。

次のステップＳ８．５では、ｋ＜Ｋであるか否かを判断する。ここでの判断が肯定されるとステップＳ８．２に戻り、否定されるとステップＳ８．６に移行する。

ステップＳ８．６では、予め用意された単眼カメラ３００のＫ種類の姿勢のうち姿勢スコアＳが最大となる姿勢を目標姿勢として選択する。

ステップＳ８．７では、許容角速度を計算する。すなわち、単眼カメラ３００の姿勢制御における最大角速度を算出する。この最大角速度は、単眼カメラ３００の回転運動時にモーションブラーが発生しない単眼カメラ３００の角速度の上限である。

単眼カメラ３００のフレーム間の特徴点の動き[v_x,v_y]は、フレーム間の回転運動R＝[r₁,r₂,r₃]がわかっている場合、次の（７）式で算出することができる。

但し、xは、単眼カメラ３００から見た特徴点の位置[x,y,z]である。fx,fyは、単眼カメラ３００の内部パラメータとしての、ピクセル単位で表される焦点距離である。

単眼カメラ３００の場合、x軸回り（チルト方向）の回転とY軸回り（パン方向）の回転なので、X軸周りにθ回転させた場合の特徴点の動き[v_x,v_y]は、次の（８）式で表現される。

但し、cはcosθ,sはsinθである。

同様にY軸周りにφ回転させた場合の特徴点の動き[v_x,v_y]は、次の（９）式で表現される。

但し、cはcosφ,sはsinφである。

モーションブラーが発生しないようにするためには、[v_x,v_y]それぞれが１以下の値であれば良い。

[v_x,v_y]のいずれもが１以下であるθとφの範囲は、例えば０．１度刻みでcosとsinの値を変化させ[v_x,v_y]の値を算出することにより求めることができる。

次のステップＳ８．８では、単眼カメラ３００の姿勢をステップＳ８．６で選択した姿勢に移行させる。具体的には、カメラ姿勢制御部３００ｂが、PID（Proportional-Integral-Differential）法を用いて、許容角速度（最大角速度）を超えない範囲でX軸回りの回転とY軸回りの回転それぞれで目標姿勢に近づくように単眼カメラ３００の姿勢制御を行う。このとき、カメラ姿勢制御部３００ｂは、上記ステップＳ２．２で計測されカメラ姿勢情報５３０ｂとして記憶部５３０に格納された単眼カメラ３００の姿勢情報（チルト方向、パン方向の回転角）から、選択した姿勢に移行させるための各ステッピングモータのステップ数を計算して単眼カメラ３００の姿勢制御を行う。

図１６には、姿勢制御後の単眼カメラ３００の撮影範囲の一例が示されている。図１６では、単眼カメラ３００の撮影範囲（画角）から、立体物かつ移動物体である先行車両が外れているのが分かる。

以上のようにしてカメラ姿勢制御処理（ステップＳ８）が実施された後、地図生成・位置計測処理を継続する場合（ステップＳ９でＮＯの場合）には、カメラ姿勢制御処理後の最適な姿勢にある単眼カメラ３００で次のループにおけるステップＳ２（特徴点抽出処理２）が実施される。このとき、単眼カメラ３００の撮影範囲内であってステレオカメラ２００の撮影範囲外の領域の３次元情報を特徴点抽出の対象から除外することが好ましい。この領域では、特徴点の性質（静止点か否かや立体物が否か）の判定が精度良く行えないからである。カメラ姿勢制御処理（ステップＳ８）により、次のループにおけるステップＳ２以後の処理（ステップＳ３〜Ｓ５）の負荷が軽減され、ステップＳ６（位置推定処理）で高精度な位置情報が得られ、ステップＳ７（環境地図生成処理）で高精度な環境地図を生成できる。

なお、上記実施形態の地図生成・位置計測装置１００では、ステレオカメラ２００と姿勢変更可能な単眼カメラ３００の組み合わせが採用されているが、これに限られない。例えば、図１７に示される変形例１の地図生成・位置計測装置１００´のように、ライダ（Ｌｉｄａｒ：Ｌｉｇｈｔ ｄｅｔｅｃｔｉｎｇ ａｎｄ ｒａｎｇｉｎｇ）２００と姿勢変更可能な単眼カメラ３００の組み合わせを採用しても良い。図１７（ａ）は、地図生成・位置計測装置１００´が搭載された車両１０を側面から見た模式図である。図１７（ｂ）は、車両１０の正面図である。

ライダ４００は、図１７に示されるように、車両１０の前側のナンバープレート近傍に投光範囲が車両１０の前方を向くように取り付けられている。

ライダ４００は、レーザ光を出射して反射光を検知することで測距を行う測距装置であり、レーザレーダとも呼ばれる。ライダ４００の測定結果から、車両１０の前方の３次元情報が得られる。

単眼カメラ３００は、車両１０のバックミラー近傍に姿勢変更可能（パン方向・チルト方向に回転可能）に取り付けられている。

地図生成・位置計測装置１００´では、図１８に示されるように、例えばライダ４００の投光範囲と単眼カメラ３００の画角を同等にし、上記実施形態と同様にして単眼カメラ３００の姿勢制御（図１９参照）、環境地図の生成、車両１０の位置計測を行う。なお、ライダ４００の測定結果である３次元情報における特徴点の抽出や、抽出された特徴点の分類も上記実施形態と同様にして行うことができる。立体物の検出は、ライダ４００の測定結果を用いて行うことができる。

ライダ４００の構成は、一例として図２０に示されるように、半導体レーザ（ＬＤ）及びカップリングレンズを含む投光系と、反射ミラー、結像光学系及び光検出器を含む受光系とが走査ミラーを共用する走査型であっても良い。

また、ライダ４００の構成は、一例として図２１に示されるように、光源（例えば半導体レーザ）を含む投光系と、結像光学系及び受光素子を含む受光系とを含む非走査型であっても良い。この場合、光源の発光点及び受光素子の受光エリアの少なくとも一方を複数にすることで、検出領域を分割して検出することが可能となる。

また、図２２に示される変形例２の地図生成・位置計測装置１００´´のように、ライダ４００と固定（非可動）の単眼カメラ３００´の組み合わせを採用しても良い。この場合、立体物の検出は、ライダ４００の測定結果を用いて行うことができる。

地図生成・位置計測装置１００´´では、ライダ４００を制御する、例えばＦＰＧＡやＣＰＵによって実現される制御手段がライダ４００の投光範囲を変更することにより、ライダ４００が３次元情報を取得する領域を制御できる（図２３参照）。投光範囲の変更は、ライダ４００が走査型の場合は、走査位置毎に光源の点灯／消灯を制御することにより行うことができ、ライダ４００が非走査型の場合は、光源における点灯させる発光点を選択することにより行うことができる。

また、図２４に示される変形例３の地図生成・位置計測装置１００´´´のように、ライダ４００とステレオカメラ２００の組み合わせを採用しても良い。この場合、立体物の検出は、ステレオカメラ２００が生成する視差画像やライダ４００の測定結果を用いて行うことができる。

地図生成・位置計測装置１００´´´では、ライダ４００を制御する、例えばＦＰＧＡやＣＰＵによって実現される制御手段がライダ４００の投光範囲を変更することにより、ライダ４００が３次元情報を取得する領域を制御できる（図２５参照）。

また、地図生成・位置計測装置の構成として、ステレオカメラ２００とライダ４００が一体的に設けられた構成（図２６参照）や、可動又は非可動の単眼カメラとライダが一体的に設けられた構成を採用しても良い。このように一体的に設けられた２つの取得手段は、例えば車両のバックミラー近傍や、車両の前側のナンバープレート近傍に取り付けることが可能である。

以上の説明から分かるように、本発明の「装置」の一例である地図生成・位置計測装置においては、車両周辺の３次元情報を取得する取得手段の組み合わせとして、多様な組み合わせを採用することが可能である。無論、上記実施形態及び各変形例の組み合わせ以外の組み合わせも採用可能である。特に、立体物を精度良く検出する観点からは、組み合わされた２つの取得手段の少なくとも一方が距離情報を精度良く検出可能なもの（例えばステレオカメラ、ライダ等）であることが好ましい。なお、単眼カメラで時系列に撮影された複数フレームの画像から距離情報を取得することとしても良い。この場合、取得手段の組み合わせとして、２つの単眼カメラの組み合わせを採用することもできる。

以上説明した上記実施形態及び各変形例の地図生成・位置計測装置は、第１の観点からすると、移動体（例えば車両）に搭載される装置であって、移動体の周囲の３次元情報（より詳細には移動体の周囲に存在する物体の３次元位置情報）を取得する第１の取得手段と、該第１の取得手段が３次元情報を取得する領域である第１の取得領域の少なくとも一部の３次元情報を取得する第２の取得手段と、第１の取得手段で取得された、第１の取得領域の３次元情報に基づいて、第２の取得手段が第１の取得領域において３次元情報を取得する領域である第２の取得領域を制御する制御手段と、第１の取得手段で取得された、第１の取得領域の３次元情報と、第２の取得手段で取得された、第２の取得領域の３次元情報とに基づいて、移動体の周囲の環境地図を生成する生成手段と、を備える装置である。

また、上記実施形態及び各変形例の地図生成・位置計測装置は、第２の観点からすると、移動体（例えば車両）に搭載される装置であって、移動体の周囲の３次元情報（より詳細には移動体の周囲に存在する物体の３次元位置情報）を取得する第１の取得手段と、該第１の取得手段が３次元情報を取得する領域である第１の取得領域の少なくとも一部の３次元情報を取得する第２の取得手段と、第１の取得手段で取得された、第１の取得領域の３次元情報に基づいて、第２の取得手段が第１の取得領域において３次元情報を取得する領域である第２の取得領域を制御する制御手段と、第１の取得手段で取得された、第１の取得領域の３次元情報と、第２の取得手段で取得された、第２の取得領域の３次元情報とに基づいて、移動体の位置を推定する推定手段と、を備える装置である。なお、地図生成・位置計測装置は、移動体の位置に加えて又は代えて、移動体の姿勢（例えばロール角、ピッチ角、ヨー角）や移動体の速度を計測しても良い。移動体の位置に加えて、移動体の姿勢及び速度を計測することにより、車両制御を更に精度良く行うことができる。本明細書中、移動体の位置、姿勢及び速度等の移動体に関する情報を「移動体情報」とも呼ぶ。

なお、上記実施形態及び各変形例では、環境地図の生成及び移動体情報の計測の双方を行う地図生成・位置計測装置を例にとって説明したが、本発明の「装置」は、環境地図の生成のみを行う装置であっても良いし、移動体情報の計測のみを行う装置であっても良い。

総括すると、本発明の装置は、移動体に搭載される装置であって、移動体の周囲の３次元情報（より詳細には移動体の周囲に存在する物体の３次元位置情報）を取得する第１の取得手段と、第１の取得手段が３次元情報を取得する領域である第１の取得領域の少なくとも一部の３次元情報を取得する第２の取得手段と、第１の取得手段で取得された、第１の取得領域の３次元情報に基づいて、第２の取得手段が第１の取得領域において３次元情報を取得する領域である第２の取得領域を制御する制御手段と、第１の取得手段で取得された、第１の取得領域の３次元情報と、第２の取得手段で取得された、第２の取得領域の３次元情報とに基づいて、移動体の周囲の環境地図の生成及び移動体に関する情報の計測の少なくとも一方を行う手段と、を備える装置である。

上記実施形態及び各変形例の地図生成・位置計測装置によれば、第２の取得手段が３次元情報を取得する領域である第２の取得領域を、例えば第１の取得領域のうち静止点が相対的に多い領域に設定することができる。

この結果、移動体の周囲の環境地図の生成や移動体に関する情報（移動体情報）の計測を安定して精度良く行うことができる。

また、制御手段は、第１の取得領域の３次元情報における特徴点を立体物の特徴点と非立体物の特徴点に分類し、その分類結果に基づいて取得領域を制御することが好ましい。

また、制御手段は、非立体物の特徴点を相対的に多く含む領域を第２の取得領域としても良い。非立体物は、立体物に比べて移動物体である可能性が極めて低く、環境地図の生成や移動体情報の計測に用いるのに適しているからである。

また、制御手段は、非立体物の特徴点を相対的に多く含む領域を一の第２の取得領域とし、立体物の特徴点の中で再投影誤差が少ない特徴点を相対的に多く含む領域を他の第２の取得領域とし、一及び他の第２の取得領域の３次元情報を互いに異なるタイミングで第２の取得手段に取得させても良い。

この場合、非立体物の特徴点に加えて、立体物の特徴点のうち移動物体である可能性が低い特徴点（再投影誤差が少ない特徴点）を第２の取得領域に含ませることができ、環境地図の生成や移動体情報の計測に用いる特徴点の数が増え、環境地図の生成や移動体情報の計測を更に精度良く行うことができる。

また、制御手段は、第１の取得領域の３次元情報から、基準面（例えば車両が走行する路面や歩道の路面）からの高さが３．０ｍ以下の高さの物体のみを立体物として抽出して上記分類を行うことが好ましい。なお、立体物として抽出する物体の高さの上限は、３．０ｍに限らず、例えば２．５ｍ、２．０ｍ、１．５ｍ等としても良い。

この場合、歩行者や前方車両などの移動物体の特徴点を排除しつつ、比較的高さのある建物や設置物などの特徴点を非立体物の特徴点と同様に静止物体の特徴点として扱うことができ、環境地図の生成や移動体情報の計測に用いる特徴点の数が増え、環境地図の生成や移動体情報の計測を更に精度良く行うことができる。

また、第２の取得手段は姿勢変更可能な撮像装置（例えば単眼カメラ）であり、制御手段が撮像装置の姿勢を制御しても良い。

また、制御手段は、撮像装置の既知の複数の姿勢のうち、第２の取得領域の３次元情報における特徴点の数から、該特徴点のうち立体物の特徴点の数を差し引いた数が最も多い姿勢を目標姿勢として撮像装置の姿勢を制御することが好ましい。

この場合、撮像装置の撮影範囲（画角）に非立体物の特徴点が多く入るため、環境地図の生成や移動体情報の計測を更に精度良く行うことができる。

また、制御手段は、第２の取得領域の３次元情報における特徴点の位置と撮像装置の内部パラメータとから算出される移動予測値が１以下となる角速度で撮像装置の姿勢を制御することが好ましい。

この場合、モーションブラーが発生しにくくなり、第１及び第２の取得手段でそれぞれ取得された３次元情報における特徴点の対応付けが容易になる。

また、第２の取得手段はライダであり、制御手段がライダの投光範囲を制御しても良い。この場合、第２の取得領域を制御するために、光源の発光タイミングや発光点の数を制御すれば良いので、撮像装置の姿勢を変更する場合に用いる機構（例えばヒンジ等）や駆動部（例えばモータ）を設ける必要がない。

また、上記実施形態又は各変形例の地図生成・位置計測装置と、該地図生成・位置計測装置が搭載される移動体（例えば車両）と、を備える移動体装置によれば、移動体の制御を安定して高精度に行うことができる。

また、上記実施形態及び各変形例の地図生成・位置計測方法は、移動体（例えば車両）の周囲の３次元情報（より詳細には移動体の周囲に存在する物体の３次元位置情報）を取得する第１の取得工程と、第１の取得工程で３次元情報が取得される領域である第１の取得領域の少なくとも一部の３次元情報を取得する第２の取得工程と、第１の取得工程で取得された、第１の取得領域の３次元情報に基づいて、第２の取得工程で第１の取得領域において３次元情報が取得される領域である第２の取得領域を制御する工程と、第１の取得工程で取得された、第１の取得領域の３次元情報と、第２の取得工程で取得された、第２の取得領域の３次元情報とに基づいて、移動体の周囲の環境地図の生成及び前記移動体に関する情報の計測を行う工程と、を含む方法である。

上記実施形態及び各変形例の地図生成・位置計測方法によれば、第２の取得工程で３次元情報が取得される領域である第２の取得領域を、例えば第１の取得領域のうち静止点が相対的に多い領域に設定することができる。

この結果、環境地図の生成や移動体に関する情報（移動体情報）の計測を安定して精度良く行うことができる。

なお、上記実施形態及び各変形例では、第２の取得工程で取得された、取得領域の３次元情報に基づいて、移動体の周囲の環境地図の生成及び移動体に関する情報の計測の双方を行っているが、一方のみを行うこととしても良い。

すなわち、本発明の「方法」は、移動体の周囲の３次元情報（より詳細には移動体の周囲に存在する物体の３次元位置情報）を取得する第１の取得工程と、前記第１の取得工程で３次元情報が取得される領域である第１の取得領域の少なくとも一部の３次元情報を取得する第２の取得工程と、前記第１の取得工程で取得された、前記第１の取得領域の３次元情報に基づいて、前記第２の取得工程で前記第１の取得領域において３次元情報が取得される領域である第２の取得領域を制御する工程と、前記第１の取得工程で取得された、前記第１の取得領域の３次元情報と、前記第２の取得工程で取得された、前記第２の取得領域の３次元情報とに基づいて、前記移動体の周囲の環境地図の生成及び前記移動体に関する情報の計測の少なくとも一方を行う工程と、を含む方法である。

また、上記実施形態や各変形例では、地図生成・位置計測装置を自動車に搭載する場合を例にとって説明したが、これに限られない。例えば上記実施形態や各変形例の地図生成・位置計測装置を、自律して移動可能なロボット、自動車以外の車両、船舶、航空機等の移動体に搭載しても良い。この場合も、上記実施形態や各変形例と同様の効果が得られる。

また、上記実施形態及び各変形例の説明で用いた数値、形状等は、本発明の趣旨を逸脱しない範囲で適宜変更可能である。

以下に、発明者が上記実施形態及び各変形例を発案するに至った思考プロセスについて説明する。

従来、車両などの移動体の自動運転において移動体自身の位置、姿勢、速度等の移動体情報を計測する必要があり、そのための一手法として、移動体に取り付けられたカメラを用いて移動体情報を検出するＶｉｓｕａｌ−ＳＬＡＭと呼ばれる技術がある。Ｖｉｓｕａｌ−ＳＬＡＭとは、時系列の映像から環境地図と呼ばれる周囲の静止物体の３次元位置情報を含む環境地図の生成と、移動体情報の推定を同時に行う手法である。

一般的なＶｉｓｕａｌ−ＳＬＡＭは画像特徴点を用いて行われ、環境地図にはステレオカメラやマルチビューステレオで３次元計測された画像特徴点の３次元位置情報が含まれている。その画像特徴点の３次元の座標と、その画像特徴点が観測された画像中の座標とを基に移動体情報を推定する手法が既に知られている。

しかし、従来のＶｉｓｕａｌ−ＳＬＡＭにおいては、環境地図に含まれている画像特徴点は絶対的に静止しているという前提があるため、実環境、特に周囲の人や車両等の移動物体が多く存在する環境においては、その前提が崩れてしまい移動体情報の推定精度が低下することが懸念される。

特に、車両に関する情報（例えば車両の位置、姿勢、速度等）を計測する場合や車両周辺の環境地図を生成する場合は、車両前方に先行車両があったり、車両前方を横断する車両があったりすると、画像の大半が速度を持つ画像特徴点で埋め尽くされてしまうことがあるため、取得する画像特徴点の中に３次元位置が不確かな画像特徴点が多く混ざってしまい環境地図の生成や車両に関する情報の計測の精度が著しく低下することが懸念される。すなわち、移動物体の特徴点を用いて環境地図の生成や移動体情報の計測を行うと、移動物体の３次元位置が不確かであるため、環境地図の生成や移動体情報の計測の精度が低下することが懸念される。

そこで、カメラに広角のレンズを利用し、画像中で動いている物体の領域を相対的に小さくする方法を用いることが考えられるが、この場合、空間分解能が下がり、位置、姿勢、速度推定の分解能が下がることが懸念される。

また、特許文献１（特開２０１６−１５７１９７号公報）は、自己位置の推定に用いる初期の環境地図を生成して精度良く自己位置を推定することができる自己位置推定装置、自己位置推定方法およびプログラムを提供することを目的としている。特許文献１では、この目的を達成するために、画像入力部（カメラ）１１から順に入力された時系列画像から物体の特徴点を抽出して、抽出した特徴点を追跡して移動ベクトルを算出する。そして、算出された特徴点の移動ベクトルの長さ及び方向に基づいて、特徴点が移動物体であるか否かを判定し、特徴点が移動物体であると判定した場合、特徴点に対応する物体を時系列画像から除去し、除去されなかった特徴点の物体の世界座標系における座標を含む地図情報を生成し、生成した地図情報に基づいて自己位置を推定している。すなわち、特許文献１では、移動物体を環境地図生成や移動体情報計測のための特徴点抽出領域から除外している。

しかし、特許文献１においても、画像の大半を移動物体が占めているような状況において、静止した特徴点を抽出できず、地図情報や自己位置推定の精度が低下することが懸念される。

そこで、発明者は、Ｖｉｓｕａｌ−ＳＬＡＭにおいて、移動体の周囲に移動物体がある環境でも効率的に精度の高い環境地図を生成し、精度の高い位置推定を実現すべく上記実施形態及び各変形例を発案するに至った。

例えば、上記実施形態の地図生成・位置計測装置１００は、ステレオカメラ２００と該ステレオカメラ２００の視野内を撮影する単眼カメラ３００とを備え、ステレオカメラ２００で撮影された画像特徴点を静止している特徴点と運動している特徴点に分類し、画像中の領域内で静止している特徴点の数に応じて、単眼カメラ３００の向きを制御し、単眼カメラ３００で撮影した画像からステレオカメラ２００で観測された特徴点を検出する処理を行い、ステレオカメラ２００の観測情報と単眼カメラ３００の観測情報を用いて、環境地図の生成や自己位置検出を行う。

１…移動体装置、１０…車両（移動体）、１００…地図生成・位置計測装置（装置）、２００…ステレオカメラ、３００、３００´…単眼カメラ、３００ｂ…カメラ姿勢制御部（制御手段）、４００…ライダ、位置計測部５２０（手段）。

特開２０１６−１５７１９７号公報

Claims (10)

1. 移動体に搭載される装置であって、
   前記移動体の周囲の３次元情報を取得する第１の取得手段と、
   前記第１の取得手段が３次元情報を取得する領域である第１の取得領域の少なくとも一部の３次元情報を取得する第２の取得手段と、
   前記第１の取得手段で取得された、前記第１の取得領域の３次元情報に基づいて、前記第２の取得手段が前記第１の取得領域において３次元情報を取得する領域である第２の取得領域を制御する制御手段と、
   前記第１の取得手段で取得された、前記第１の取得領域の３次元情報と、前記第２の取得手段で取得された、前記第２の取得領域の３次元情報とに基づいて、前記移動体の周囲の環境地図の生成及び前記移動体に関する情報の計測の少なくとも一方を行う手段と、を備える装置。
2. 前記制御手段は、前記第１の取得領域の３次元情報における特徴点を立体物の特徴点と非立体物の特徴点に分類し、その分類結果に基づいて前記第２の取得領域を制御することを特徴とする請求項１に記載の装置。
3. 前記制御手段は、前記非立体物の特徴点を相対的に多く含む領域を前記第２の取得領域とすることを特徴とする請求項２に記載の装置。
4. 前記制御手段は、
   前記非立体物の特徴点を相対的に多く含む領域を一の前記第２の取得領域とし、
   前記立体物の特徴点の中で再投影誤差が少ない特徴点を相対的に多く含む領域を他の前記第２の取得領域とし、
   前記一及び他の第２の取得領域の３次元情報を互いに異なるタイミングで前記第２の取得手段に取得させることを特徴とする請求項２に記載の装置。
5. 前記制御手段は、前記第１の取得領域の３次元情報から、基準面からの高さが３．０ｍ以下の高さの物体のみを立体物として抽出して前記分類を行うことを特徴とする請求項２〜４のいずれか一項に記載の装置。
6. 前記第２の取得手段は、姿勢変更可能な撮像装置であり、
   前記制御手段は、前記撮像装置の姿勢を制御することを特徴とする請求項２〜５のいずれか一項に記載の装置。
7. 前記制御手段は、前記撮像装置の既知の複数の姿勢のうち、前記第２の取得領域の３次元情報における特徴点の数から、該特徴点のうち前記立体物の特徴点の数を差し引いた数が最も多い姿勢を目標姿勢として前記撮像装置の姿勢を制御することを特徴とする請求項６に記載の装置。
8. 前記制御手段は、前記第２の取得領域の３次元情報における特徴点の位置と前記撮像装置の内部パラメータとから算出される移動予測値が１以下となる角速度で前記撮像装置の姿勢を制御することを特徴とする請求項６又は７に記載の装置。
9. 請求項１〜８のいずれか一項に記載の装置と、
   前記装置が搭載される移動体と、を備える移動体装置。
10. 移動体の周囲の３次元情報を取得する第１の取得工程と、
    前記第１の取得工程で３次元情報が取得される領域である第１の取得領域の少なくとも一部の３次元情報を取得する第２の取得工程と、
    前記第１の取得工程で取得された、前記第１の取得領域の３次元情報に基づいて、前記第２の取得工程で前記第１の取得領域において３次元情報が取得される領域である第２の取得領域を制御する工程と、
    前記第１の取得工程で取得された、前記第１の取得領域の３次元情報と、前記第２の取得工程で取得された、前記第２の取得領域の３次元情報とに基づいて、前記移動体の周囲の環境地図の生成及び前記移動体に関する情報の計測の少なくとも一方を行う工程と、を含む方法。

Images