特許 7173471 ３次元位置推定装置及びプログラム

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

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

(11)【特許番号】

(24)【登録日】2022-11-08

(45)【発行日】2022-11-16

(54)【発明の名称】３次元位置推定装置及びプログラム

(51)【国際特許分類】

   G01B 11/00 20060101AFI20221109BHJP

   G06T 7/00 20170101ALI20221109BHJP

   G06T 7/70 20170101ALI20221109BHJP

   G06T 7/579 20170101ALI20221109BHJP

【ＦＩ】

G01B11/00 H

G06T7/00 650Z

G06T7/70 Z

G06T7/579

【請求項の数】 3

(21)【出願番号】P 2019015930

(22)【出願日】2019-01-31

(65)【公開番号】P2020122754

(43)【公開日】2020-08-13

【審査請求日】2021-12-16

(73)【特許権者】

【識別番号】000003609

【氏名又は名称】株式会社豊田中央研究所

(73)【特許権者】

【識別番号】000000011

【氏名又は名称】株式会社アイシン

(74)【代理人】

【識別番号】100079049

【弁理士】

【氏名又は名称】中島 淳

(74)【代理人】

【識別番号】100084995

【弁理士】

【氏名又は名称】加藤 和詳

(72)【発明者】

【氏名】浅井 彰司

(72)【発明者】

【氏名】浅海 周

(72)【発明者】

【氏名】早川 和孝

【審査官】眞岩 久恵

(56)【参考文献】

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

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

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

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

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

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

【文献】米国特許出願公開第２００９／０２２８２０４（ＵＳ，Ａ１）

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

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

Ｇ０１Ｃ ３／００－ ３／３２

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

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

【請求項1】

移動体に搭載された撮影装置により撮影された複数の画像の各々から複数の特徴点を検出する特徴点検出部と、
前記移動体に関するセンサ情報に基づいて、前記移動体の位置及び姿勢の変化量を計算する位置姿勢変化量計算部と、
前記特徴点検出部により検出された特徴点の特徴を示す特徴量を用いて、前記複数の画像間における特徴点を対応付ける特徴点対応付け部と、
前記特徴点対応付け部により対応付けられた特徴点の各々についての３次元位置と、前記複数の画像の各々についての前記撮影装置の位置及び姿勢とを取得する３次元位置取得部と、
前記特徴点の各々についての、前記特徴点の３次元位置を規定する特徴点ノードと、前記複数の画像の各々についての、前記画像を撮影した前記撮影装置の位置及び姿勢を規定する撮影ノードと、前記特徴点ノードと前記撮影ノードとの間の各々を結ぶ、前記撮影ノードの前記画像上の、前記特徴点ノードの特徴点の位置を規定する特徴点エッジと、隣接する前記撮影ノード間の各々を結ぶ、前記撮影ノードの画像間における前記移動体の位置及び姿勢の変化量を規定する撮影エッジとから構成されるグラフ構造データを生成するグラフ構造データ生成部と、
前記グラフ構造データ生成部により生成された前記グラフ構造データに含まれる前記特徴点エッジの各々について、前記特徴点エッジで結ばれた前記特徴点ノードの特徴点を、前記撮影ノードが規定する前記撮影装置の位置及び姿勢を用いて前記撮影ノードの画像上に投影したときの投影誤差を表す第１誤差関数を設定すると共に、前記グラフ構造データに含まれる前記撮影エッジの各々について、前記撮影エッジが規定する前記位置及び姿勢の変化量と位置姿勢変化量計算部による計算結果との誤差を表す第２誤差関数を設定する誤差関数設定部と、
前記特徴点エッジの各々についての前記第１誤差関数と、前記撮影エッジの各々についての前記第２誤差関数との和が最小となるように、前記グラフ構造データの前記特徴点ノードの各々が規定する前記特徴点の３次元位置、及び前記撮影ノードの各々が規定する前記撮影装置の位置及び姿勢を最適化するグラフ構造データ最適化部と、
を含み、
前記誤差関数設定部は、前記撮影エッジの各々について、前記撮影エッジに対応する前記位置姿勢変化量計算部による前記移動体の姿勢の変化量が小さいほど大きな重みを含んで表される前記第２誤差関数を設定する
３次元位置推定装置。

【請求項2】

前記位置姿勢変化量計算部は、前記移動体に搭載された操舵角センサにより検出された操舵角に基づいて、前記移動体のヨー角変化量を計算し、
前記誤差関数設定部は、前記位置姿勢変化量計算部による計算結果に基づいて得られる、前記撮影エッジに対応する前記移動体のヨー角変化量が小さいほど大きな重みを含んで表される前記第２誤差関数を設定する
請求項１記載の３次元位置推定装置。

【請求項3】

コンピュータを、
移動体に搭載された撮影装置により撮影された複数の画像の各々から複数の特徴点を検出する特徴点検出部、
前記移動体に関するセンサ情報に基づいて、前記移動体の位置及び姿勢の変化量を計算する位置姿勢変化量計算部、
前記特徴点検出部により検出された特徴点の特徴を示す特徴量を用いて、前記複数の画像間における特徴点を対応付ける特徴点対応付け部、
前記特徴点対応付け部により対応付けられた特徴点の各々についての３次元位置と、前記複数の画像の各々についての前記撮影装置の位置及び姿勢とを取得する３次元位置取得部、
前記特徴点の各々についての、前記特徴点の３次元位置を規定する特徴点ノードと、前記複数の画像の各々についての、前記画像を撮影した前記撮影装置の位置及び姿勢を規定する撮影ノードと、前記特徴点ノードと前記撮影ノードとの間の各々を結ぶ、前記撮影ノードの前記画像上の、前記特徴点ノードの特徴点の位置を規定する特徴点エッジと、隣接する前記撮影ノード間の各々を結ぶ、前記撮影ノードの画像間における前記移動体の位置及び姿勢の変化量を規定する撮影エッジとから構成されるグラフ構造データを生成するグラフ構造データ生成部、
前記グラフ構造データ生成部により生成された前記グラフ構造データに含まれる前記特徴点エッジの各々について、前記特徴点エッジで結ばれた前記特徴点ノードの特徴点を、前記撮影ノードが規定する前記撮影装置の位置及び姿勢を用いて前記撮影ノードの画像上に投影したときの投影誤差を表す第１誤差関数を設定すると共に、前記グラフ構造データに含まれる前記撮影エッジの各々について、前記撮影エッジが規定する前記位置及び姿勢の変化量と位置姿勢変化量計算部による計算結果との誤差を表す第２誤差関数を設定する誤差関数設定部、及び
前記特徴点エッジの各々についての前記第１誤差関数と、前記撮影エッジの各々についての前記第２誤差関数との和が最小となるように、前記グラフ構造データの前記特徴点ノードの各々が規定する前記特徴点の３次元位置、及び前記撮影ノードの各々が規定する前記撮影装置の位置及び姿勢を最適化するグラフ構造データ最適化部
として機能させ、
前記誤差関数設定部は、前記撮影エッジの各々について、前記撮影エッジに対応する前記位置姿勢変化量計算部による前記移動体の姿勢の変化量が小さいほど大きな重みを含んで表される前記第２誤差関数を設定する
プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、３次元位置推定装置及びプログラムに関する。

【背景技術】

【0002】

従来から、車両に搭載されたカメラで周辺環境を撮影し、撮影された画像から環境地図を作成する技術がある（非特許文献１）。

【0003】

非特許文献１では、画像から特徴点を検出し、当該特徴点の局所特徴量を用いて、複数地点で撮影された画像間の特徴点を対応付け、対応付けた特徴点の画像上の位置から、カメラの位置・姿勢と特徴点の３次元位置を算出し、複数の特徴点の３次元位置を規定する特徴点ノードと、複数の撮影地点におけるカメラの位置・姿勢を規定するカメラノードと、各撮影地点における画像上の特徴点位置を規定する特徴点エッジと、隣接するカメラノード間における車両の位置・姿勢の変化量を規定する撮影エッジとから構成されるグラフ構造データを生成し、特徴点エッジと撮影エッジから算出される誤差関数の和が最小となるようにグラフ構造データを最適化することにより、環境地図を作成する。

【先行技術文献】

【非特許文献】

【0004】

【文献】Jing Wang, Zongying Shi, Yisheng Zhong, "Visual SLAM Incorporating Wheel odometer for Indoor Robots", 2017 36th Chinese Control Conference (CCC), 2017.

【発明の概要】

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

【0005】

しかし、上記非特許文献１では、撮影エッジの誤差関数の重み（非特許文献中のｉｍｆｏｒｍａｔｉｏｎ ｍａｔｒｉｘ Σ_Ｂ）の設定方法について記述されていない。

【0006】

このため、車両旋回時のような、オドメトリにより車両の位置・姿勢の変化量を高い精度で取得できない場合、車両が旋回したときの精度が低い車両の位置変化量の計測値に対して撮影エッジの誤差関数が小さくなるようにグラフ構造データを最適化するため、車載カメラの画像から作成した環境地図の歪みが大きくなってしまう、という問題があった。

【0007】

本発明は、上記問題点を解決するためになされたもので、車両の位置・姿勢の変化量を高い精度で取得できない場合であっても、特徴点の３次元位置を精度良く推定することができる３次元位置推定装置及びプログラムを提供することを目的とする。

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

【0008】

上記の目的を達成するために、本発明の３次元位置推定装置は、移動体に搭載された撮影装置により撮影された複数の画像の各々から複数の特徴点を検出する特徴点検出部と、前記移動体に関するセンサ情報に基づいて、前記移動体の位置及び姿勢の変化量を計算する位置姿勢変化量計算部と、前記特徴点検出部により検出された特徴点の特徴を示す特徴量を用いて、前記複数の画像間における特徴点を対応付ける特徴点対応付け部と、前記特徴点対応付け部により対応付けられた特徴点の各々についての３次元位置と、前記複数の画像の各々についての前記撮影装置の位置及び姿勢とを取得する３次元位置取得部と、前記特徴点の各々についての、前記特徴点の３次元位置を規定する特徴点ノードと、前記複数の画像の各々についての、前記画像を撮影した前記撮影装置の位置及び姿勢を規定する撮影ノードと、前記特徴点ノードと前記撮影ノードとの間の各々を結ぶ、前記撮影ノードの前記画像上の、前記特徴点ノードの特徴点の位置を規定する特徴点エッジと、隣接する前記撮影ノード間の各々を結ぶ、前記撮影ノードの画像間における前記移動体の位置及び姿勢の変化量を規定する撮影エッジとから構成されるグラフ構造データを生成するグラフ構造データ生成部と、前記グラフ構造データ生成部により生成された前記グラフ構造データに含まれる前記特徴点エッジの各々について、前記特徴点エッジで結ばれた前記特徴点ノードの特徴点を、前記撮影ノードが規定する前記撮影装置の位置及び姿勢を用いて前記撮影ノードの画像上に投影したときの投影誤差を表す第１誤差関数を設定すると共に、前記グラフ構造データに含まれる前記撮影エッジの各々について、前記撮影エッジが規定する前記位置及び姿勢の変化量と位置姿勢変化量計算部による計算結果との誤差を表す第２誤差関数を設定する誤差関数設定部と、前記特徴点エッジの各々についての前記第１誤差関数と、前記撮影エッジの各々についての前記第２誤差関数との和が最小となるように、前記グラフ構造データの前記特徴点ノードの各々が規定する前記特徴点の３次元位置、及び前記撮影ノードの各々が規定する前記撮影装置の位置及び姿勢を最適化するグラフ構造データ最適化部と、を含み、前記誤差関数設定部は、前記撮影エッジの各々について、前記撮影エッジに対応する前記位置姿勢変化量計算部による前記移動体の姿勢の変化量が小さいほど大きな重みを含んで表される前記第２誤差関数を設定するように構成されている。

【0009】

また、本発明のプログラムは、コンピュータを、移動体に搭載された撮影装置により撮影された複数の画像の各々から複数の特徴点を検出する特徴点検出部、前記移動体に関するセンサ情報に基づいて、前記移動体の位置及び姿勢の変化量を計算する位置姿勢変化量計算部、前記特徴点検出部により検出された特徴点の特徴を示す特徴量を用いて、前記複数の画像間における特徴点を対応付ける特徴点対応付け部、前記特徴点対応付け部により対応付けられた特徴点の各々についての３次元位置と、前記複数の画像の各々についての前記撮影装置の位置及び姿勢とを取得する３次元位置取得部、前記特徴点の各々についての、前記特徴点の３次元位置を規定する特徴点ノードと、前記複数の画像の各々についての、前記画像を撮影した前記撮影装置の位置及び姿勢を規定する撮影ノードと、前記特徴点ノードと前記撮影ノードとの間の各々を結ぶ、前記撮影ノードの前記画像上の、前記特徴点ノードの特徴点の位置を規定する特徴点エッジと、隣接する前記撮影ノード間の各々を結ぶ、前記撮影ノードの画像間における前記移動体の位置及び姿勢の変化量を規定する撮影エッジとから構成されるグラフ構造データを生成するグラフ構造データ生成部、前記グラフ構造データ生成部により生成された前記グラフ構造データに含まれる前記特徴点エッジの各々について、前記特徴点エッジで結ばれた前記特徴点ノードの特徴点を、前記撮影ノードが規定する前記撮影装置の位置及び姿勢を用いて前記撮影ノードの画像上に投影したときの投影誤差を表す第１誤差関数を設定すると共に、前記グラフ構造データに含まれる前記撮影エッジの各々について、前記撮影エッジが規定する前記位置及び姿勢の変化量と位置姿勢変化量計算部による計算結果との誤差を表す第２誤差関数を設定する誤差関数設定部、及び前記特徴点エッジの各々についての前記第１誤差関数と、前記撮影エッジの各々についての前記第２誤差関数との和が最小となるように、前記グラフ構造データの前記特徴点ノードの各々が規定する前記特徴点の３次元位置、及び前記撮影ノードの各々が規定する前記撮影装置の位置及び姿勢を最適化するグラフ構造データ最適化部として機能させ、前記誤差関数設定部は、前記撮影エッジの各々について、前記撮影エッジに対応する前記位置姿勢変化量計算部による前記移動体の姿勢の変化量が小さいほど大きな重みを含んで表される前記第２誤差関数を設定するためのプログラムである。

【0010】

本発明の３次元位置推定装置及びプログラムによれば、特徴点検出部が、移動体に搭載された撮影装置により撮影された複数の画像の各々から複数の特徴点を検出し、位置姿勢変化量計算部が、当該移動体に関するセンサ情報に基づいて、当該移動体の位置及び姿勢の変化量を計算し、特徴点対応付け部が、当該特徴点検出部により検出された特徴点の特徴を示す特徴量を用いて、当該複数の画像間における特徴点を対応付け、３次元位置取得部が、当該特徴点対応付け部により対応付けられた特徴点の各々についての３次元位置と、当該複数の画像の各々についての当該撮影装置の位置及び姿勢とを取得し、グラフ構造データ生成部が、当該特徴点の各々についての、当該特徴点の３次元位置を規定する特徴点ノードと、当該複数の画像の各々についての、当該画像を撮影した当該撮影装置の位置及び姿勢を規定する撮影ノードと、当該特徴点ノードと当該撮影ノードとの間の各々を結ぶ、当該撮影ノードの当該画像上の、当該特徴点ノードの特徴点の位置を規定する特徴点エッジと、隣接する当該撮影ノード間の各々を結ぶ、当該撮影ノードの画像間における当該移動体の位置及び姿勢の変化量を規定する撮影エッジとから構成されるグラフ構造データを生成する。

【0011】

そして、誤差関数設定部が、グラフ構造データ生成部により生成された当該グラフ構造データに含まれる当該特徴点エッジの各々について、当該特徴点エッジで結ばれた当該特徴点ノードの特徴点を、当該撮影ノードが規定する当該撮影装置の位置及び姿勢を用いて当該撮影ノードの画像上に投影したときの投影誤差を表す第１誤差関数を設定すると共に、当該グラフ構造データに含まれる当該撮影エッジの各々について、当該撮影エッジが規定する当該位置及び姿勢の変化量と位置姿勢変化量計算部による計算結果との誤差を表す第２誤差関数を設定し、グラフ構造データ最適化部が、当該特徴点エッジの各々についての当該第１誤差関数と、当該撮影エッジの各々についての当該第２誤差関数との和が最小となるように、当該グラフ構造データの当該特徴点ノードの各々が規定する当該特徴点の３次元位置、及び当該撮影ノードの各々が規定する当該撮影装置の位置及び姿勢を最適化し、当該誤差関数設定部は、当該撮影エッジの各々について、当該撮影エッジに対応する当該位置姿勢変化量計算部による当該移動体の姿勢の変化量が小さいほど大きな重みを含んで表される当該第２誤差関数を設定する。

【0012】

このように、特徴点の３次元位置を規定する特徴点ノードと、撮影装置の位置及び姿勢を規定する撮影ノードと、特徴点ノードと撮影ノードとの間の各々を結ぶ、撮影ノードの画像上の、特徴点ノードの特徴点の位置を規定する特徴点エッジと、隣接する撮影ノード間の各々を結ぶ、撮影ノードの画像間における移動体の位置及び姿勢の変化量を規定する撮影エッジとから構成されるグラフ構造データを生成し、特徴点エッジで結ばれた特徴点ノードの特徴点を、撮影ノードが規定する撮影装置の位置及び姿勢を用いて撮影ノードの画像上に投影したときの投影誤差を表す第１誤差関数を設定すると共に、撮影エッジが規定する位置及び姿勢の変化量の誤差を表す第２誤差関数を、当該撮影エッジに対応する当該移動体の姿勢の変化量が小さいほど大きな重みを含んで表されるように設定し、第１誤差関数と、第２誤差関数との和が最小となるように、グラフ構造データの特徴点の３次元位置、及び撮影装置の位置及び姿勢を最適化することにより、車両の位置・姿勢の変化量を高い精度で取得できない場合であっても、特徴点の３次元位置を精度良く推定することができる。

【0013】

また、本発明の３次元位置推定装置の前記位置姿勢変化量計算部は、前記移動体に搭載された操舵角センサにより検出された操舵角に基づいて、前記移動体のヨー角変化量を計算し、前記誤差関数設定部は、前記位置姿勢変化量計算部による計算結果に基づいて得られる、前記撮影エッジに対応する前記移動体のヨー角変化量が小さいほど大きな重みを含んで表される前記第２誤差関数を設定することができる。

【発明の効果】

【0014】

以上説明したように、本発明の３次元位置推定装置及びプログラムによれば、特徴点の３次元位置を規定する特徴点ノードと、撮影装置の位置及び姿勢を規定する撮影ノードと、特徴点ノードと撮影ノードとの間の各々を結ぶ、撮影ノードの画像上の、特徴点ノードの特徴点の位置を規定する特徴点エッジと、隣接する撮影ノード間の各々を結ぶ、撮影ノードの画像間における移動体の位置及び姿勢の変化量を規定する撮影エッジとから構成されるグラフ構造データを生成し、特徴点エッジで結ばれた特徴点ノードの特徴点を、撮影ノードが規定する撮影装置の位置及び姿勢を用いて撮影ノードの画像上に投影したときの投影誤差を表す第１誤差関数を設定すると共に、撮影エッジが規定する位置及び姿勢の変化量の誤差を表す第２誤差関数を、当該撮影エッジに対応する当該移動体の姿勢の変化量が小さいほど大きな重みを含んで表されるように設定し、第１誤差関数と、第２誤差関数との和が最小となるように、グラフ構造データの特徴点の３次元位置、及び撮影装置の位置及び姿勢を最適化することにより、車両の位置・姿勢の変化量を高い精度で取得できない場合であっても、特徴点の３次元位置を精度良く推定することができる、という効果が得られる。

【図面の簡単な説明】

【0015】

【図1】本発明の実施の形態における概略構成を示すブロック図である。

【図2】車両の位置・ヨー角の変化量を示す図である。

【図3】特徴点の３次元位置の算出を示す図である。

【図4】カメラ位置・姿勢の変化量の算出を示す図である。

【図5】グラフ構造データの構成を示す図である。

【図6】本発明の実施の形態における環境地図作成処理ルーチンの内容を示すフローチャートである。

【図7】本発明の実施の形態における３次元位置計算処理ルーチンの内容を示すフローチャートである。

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

【0016】

以下、図面を参照して本発明の実施の形態を詳細に説明する。

【0017】

＜地図作成システムの構成＞
図１に示すように、本発明の実施の形態に係る地図作成システム１０は、撮影装置１００と、車輪速センサ２００と、操舵角センサ２１０と、地図作成装置３００とを備えて構成される。

【0018】

撮影装置１００は、移動体に搭載された周辺環境を撮影する。本実施の形態では、移動体は、車両である場合を例に説明する。

【0019】

具体的には、撮影装置１００は、車両の上部等に設けられた単眼カメラであり、車両の前方や後方等を撮影する。撮影装置１００は、例えば、車幅方向の略中央部付近に設けられ、かつ撮影装置１００の光軸が水平方向より若干下側を向くように配置されている。

【0020】

また、本実施形態では、撮影装置１００として単眼カメラを例示して説明するが、これに限られず他の形態のカメラ、例えばステレオカメラ等であってもよい。

【0021】

そして、撮影装置１００は、撮影した画像を、地図作成装置３００に渡す。

【0022】

車輪速センサ２００は、車両の４輪の車輪速を検出し、検出した車輪速を、地図作成装置３００に渡す。

【0023】

操舵角センサ２１０は、車両の操舵角を検出し、検出した車両の操舵角を、地図作成装置３００に渡す。

【0024】

地図作成装置３００は、ＣＰＵと、ＲＡＭと、後述する姿勢角補正誤差推定処理ルーチン及び自己位置推定処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。

【0025】

地図作成装置３００は、センサ情報入力部３１０と、オドメトリ計算部３２０と、画像入力部３３０と、キーフレーム選択部３４０と、特徴点検出部３５０と、特徴量算出部３６０と、特徴点対応付け部３７０と、３次元位置計算部３８０と、グラフ構造データ生成部３９０と、誤差関数設定部４００と、グラフ構造データ最適化部４１０と、環境地図作成部４２０と、出力部４３０と、を備えて構成される。

【0026】

センサ情報入力部３１０は、車輪速センサ２００により検出された車輪速、及び操舵角センサ２１０により検出された操舵角の入力を受け付ける。

【0027】

そして、センサ情報入力部３１０は、受け付けた車輪速及び操舵角を、オドメトリ計算部３２０に渡す。

【0028】

オドメトリ計算部３２０は、車両に関するセンサ情報に基づいて、車両の位置変化量（ΔＸ_ｖ，ΔＹ_ｖ）及びヨー角変化量Δθ_ｖを計算する。

【0029】

具体的には、オドメトリ計算部３２０は、まず、車輪速に基づいて、車両の走行距離を算出すると共に、操舵角に基づいて、車両の旋回半径を算出する。

【0030】

次に、オドメトリ計算部３２０は、図２に示すように、車両の走行距離及び旋回半径から、車両座標系での車両の位置変化量（ΔＸ_ｖ，ΔＹ_ｖ）とヨー角変化量Δθ_ｖとを計算する。

【0031】

そして、オドメトリ計算部３２０は、算出した位置変化量（ΔＸ_ｖ，ΔＹ_ｖ）とヨー角変化量Δθ_ｖとを、３次元位置計算部３８０、及び誤差関数設定部４００に渡す。また、オドメトリ計算部３２０は、各時刻における車両の走行距離を、キーフレーム選択部３４０に渡す。

【0032】

画像入力部３３０は、撮影装置１００から画像の入力を受け付ける。

【0033】

そして、画像入力部３３０は、受け付けた画像を、キーフレーム選択部３４０に渡す。

【0034】

キーフレーム選択部３４０は、撮影装置１００から入力された画像のうち、環境地図を作成するために用いるキーフレームを複数選択する。

【0035】

具体的には、キーフレーム選択部３４０は、まず、最初に撮影装置１００により受け付けた画像を１番目のキーフレームとして選択する。

【0036】

次に、キーフレーム選択部３４０は、オドメトリ計算部３２０から取得した車両の走行距離が所定値（例えば０．１ｍ）を超える度に、所定値を超えた時刻に撮影装置１００により受け付けた画像を次のキーフレームとして選択する。

【0037】

そして、キーフレーム選択部３４０は、選択した複数のキーフレームを、特徴点検出部３５０に渡す。

【0038】

特徴点検出部３５０は、車両に搭載された撮影装置１００により撮影された複数のキーフレームの各々から複数の特徴点を検出する。

【0039】

具体的には、特徴点検出部３５０は、ＦＡＳＴ（参考文献１）等のアルゴリズムを用いて各キーフレームの画像ピラミッドから複数の特徴点を検出する。
［参考文献１］Raul Mur-Artal, J. M. M. Montiel and Juan D. Tardos, "ORB-SLAM: A Versatile and Accurate Monocular SLAM System.", IEEE Transactions on Robotics, vol.31, no.5, 2015, pp.1147-1163.

【0040】

そして、特徴点検出部３５０は、複数のキーフレームの各々について、当該キーフレームの複数の特徴点を、特徴量算出部３６０に渡す。

【0041】

特徴量算出部３６０は、複数のキーフレームの各々について、当該キーフレームの複数の特徴点の各々の特徴量を算出する。

【0042】

具体的には、特徴量算出部３６０は、複数のキーフレームの各々についての複数の特徴点から、ＯＲＢ（参考文献１）等の局所特徴量を算出する。

【0043】

そして、特徴量算出部３６０は、複数のキーフレームの各々について、当該キーフレームの複数の特徴点と、当該複数の特徴点の各々についての特徴量を、特徴点対応付け部３７０に渡す。

【0044】

特徴点対応付け部３７０は、特徴量算出部３６０により算出された特徴点の特徴を示す特徴量を用いて、連続するキーフレーム間における特徴点を対応付ける。

【0045】

具体的には、特徴点対応付け部３７０は、特徴点の各々についての特徴量を用いて、連続するキーフレーム間での特徴点を対応付ける。

【0046】

そして、特徴点対応付け部３７０は、複数のキーフレームの各々について、対応付けた特徴点を、３次元位置計算部３８０に渡す。

【0047】

３次元位置計算部３８０は、特徴点対応付け部３７０により対応付けられた特徴点の各々についての３次元位置と、複数の画像の各々についての撮影装置１００の位置及び姿勢とを計算する。

【0048】

具体的には、３次元位置計算部３８０は、２番目のキーフレームから順番に、一つ前のキーフレームとの間の特徴点の対応付けに基づいて、各特徴点の３次元位置を計算する。

【0049】

３次元位置計算部３８０は、２番目のキーフレームの場合、まず、１番目のキーフレームとの間で、８点アルゴリズムとＲＡＮＳＡＣ（Ｒａｎｄｏｍ ｓａｍｐｌｅ ｃｏｎｓｅｎｓｕｓ）とを用いて、１番目のキーフレームの撮影位置と、２番目のキーフレームの撮影位置とにおける撮影装置１００の位置及び姿勢の変化量（無次元量）を計算し、車両の位置変化量（ΔＸ_ｖ，ΔＹ_ｖ）を用いて有次元化（絶対的なスケールを決定）する。

【0050】

次に、３次元位置計算部３８０は、図３に示すように、対応付けた特徴点のキーフレーム上の位置と撮影装置１００の位置及び姿勢の変化量とから、三角測量の原理により当該特徴点の３次元位置（ワールド座標系）を計算する。

【0051】

３次元位置計算部３８０は、３番目以降のキーフレームの場合、図４に示すように、まず、１つ前のキーフレームとの間で対応付けた特徴点のうち、３次元位置が既知の特徴点を抽出し、その再投影誤差の和が最小となる撮影装置１００の位置及び姿勢の変化量を計算する。

【0052】

次に、３次元位置計算部３８０は、１つ前のキーフレームとの間で対応付けた特徴点のうち、３次元位置が未知の特徴点について、２番目のキーフレームと同様に、キーフレーム上の特徴点の位置と撮影装置１００の位置及び姿勢の変化量とから三角測量の原理により当該特徴点の３次元位置を計算する。

【0053】

そして、３次元位置計算部３８０は、計算して得られた複数の特徴点の各々についての３次元位置と、複数のキーフレームの各々についての、当該キーフレームの１つ前のキーフレームの撮影位置と、当該キーフレームの撮影位置とにおける撮影装置１００の位置及び姿勢の変化量とを、グラフ構造データ生成部３９０に渡す。

【0054】

グラフ構造データ生成部３９０は、特徴点の各々についての、当該特徴点の３次元位置を規定する特徴点ノードと、複数のキーフレームの各々についての、当該キーフレームを撮影した撮影装置１００の位置及び姿勢を規定する撮影ノードと、特徴点ノードと撮影ノードとの間の各々を結ぶ、当該撮影ノードのキーフレーム上の、当該特徴点ノードの特徴点の位置を規定する特徴点エッジと、隣接する撮影ノード間の各々を結ぶ、当該撮影ノードのキーフレーム間における車両の位置及び姿勢の変化量を規定する撮影エッジとから構成されるグラフ構造データを生成する。

【0055】

具体的には、グラフ構造データ生成部３９０は、図５に示すように、特徴点Ｐ_ｉの３次元位置を規定する特徴点ノードと、キーフレームＫＦ_ｊのカメラ位置・姿勢を規定するカメラノードと、キーフレームＫＦ_ｊ上の特徴点Ｐ_ｉの位置を規定する特徴点エッジと、隣接するキーフレーム間における車両の位置変化量とヨー角変化量を規定する撮影エッジとから構成されるグラフ構造データを生成する。

【0056】

具体的には、グラフ構造データ生成部３９０は、３次元位置計算部３８０により得られた各特徴点の３次元位置を用いて、グラフ構造データを生成する。

【0057】

そして、グラフ構造データ生成部３９０は、生成したグラフ構造データを、誤差関数設定部４００に渡す。

【0058】

誤差関数設定部４００は、グラフ構造データ生成部３９０により生成されたグラフ構造データに含まれる特徴点エッジの各々について、特徴点エッジで結ばれた特徴点ノードの特徴点を、撮影ノードが規定する撮影装置１００の位置及び姿勢を用いて撮影ノードの画像上に投影したときの投影誤差を表す第１誤差関数Ｃ_Ｐｉｊを設定すると共に、グラフ構造データに含まれる撮影エッジの各々について、当該撮影エッジが規定する位置及び姿勢の変化量とオドメトリ計算部３２０による計算結果との誤差を表す第２誤差関数Ｃ_ＫＦｊを設定する。

【0059】

具体的には、誤差関数設定部４００は、特徴点エッジの第１誤差関数Ｃ_Ｐｉｊを下記式（１）、（２）のように設定する。

【0060】

【数1】

【0061】

ｅ_ＰｉｊはキーフレームＫＦ_ｊ上における特徴点Ｐ_ｉの（ｘ，ｙ）座標の再投影誤差を表す２行１列のベクトルであり、

と表すことができる。ここで、（ｘ_Ｐｉｊ，ｙ_Ｐｉｊ）は、特徴点Ｐ_ｉの実際の座標であり、’＾’（ハット記号）付きは画像上に投影したときの計算値である。

【0062】

Ｗ_Ｐｉｊは再投影誤差に対する重みであり、画像ピラミッドの解像度が高い階層から検出した特徴点に対しては大きく設定する。

【0063】

また、誤差関数設定部４００は、撮影エッジの第２誤差関数Ｃ_ＫＦｊを下記式（３）、（４）のように設定する。

【0064】

【数2】

【0065】

ここで、ｅ_ＫＦｊはキーフレームＫＦ_ｊ－１とＫＦ_ｊの間おける、カメラ位置・姿勢の変化量から換算した車両の位置変化量とヨー角変化量の、計測値に対する誤差を表す３行１列のベクトルであり、

と表すことができる。ここで、’＾’付きは計算値である。

【0066】

また、ｗ_Ｘｊは、車両のＸ方向の位置変化量の誤差に対する重みであり、ｗ_Ｙｊは、車両のＹ方向の位置変化量の誤差に対する重みであり、ｗ_θｊは、車両のヨー角変化量の誤差に対する重みである。

【0067】

ここで、誤差関数設定部４００は、撮影エッジの各々について、撮影エッジに対応するオドメトリ計算部３２０による車両のヨー角変化量Δθ_ｖが小さいほど大きな重みｗ_Ｘｊ、ｗ_Ｙｊ、及びｗ_θｊを含んで表される第２誤差関数Ｃ_ＫＦｊを設定する。

【0068】

すなわち、誤差関数設定部４００は、車両のヨー角変化量Δθ_ｖが小さい撮影エッジほど重みｗ_Ｘｊ、ｗ_Ｙｊ、及びｗ_θｊを大きく設定する。

【0069】

ここで、ｗ_Ｘｊ＝ｗ_Ｙｊとなるように設定してもよい。これにより、メモリ容量を節約することができる。

【0070】

そして、誤差関数設定部４００は、グラフ構造データと、第１誤差関数Ｃ_Ｐｉｊと、第２誤差関数Ｃ_ＫＦｊとを、グラフ構造データ最適化部４１０に渡す。

【0071】

グラフ構造データ最適化部４１０は、特徴点エッジの各々についての第１誤差関数Ｃ_Ｐｉｊと、撮影エッジの各々についての第２誤差関数Ｃ_ＫＦｊとの和が最小となるように、グラフ構造データの特徴点ノードの各々が規定する当該特徴点の３次元位置、及び撮影ノードの各々が規定する撮影装置１００の位置及び姿勢を最適化する。

【0072】

具体的には、グラフ構造データ最適化部４１０は、非特許文献１と同様に、特徴点エッジの各々についての第１誤差関数Ｃ_Ｐｉｊと、撮影エッジの各々についての第２誤差関数Ｃ_ＫＦｊとの和が最小となるように、グラフ構造データの特徴点ノードの各々が規定する当該特徴点の３次元位置、及び撮影ノードの各々が規定する撮影装置１００の位置及び姿勢を最適化する。

【0073】

ここで、車両のヨー角変化量Δθ_ｖが小さい撮影エッジほど第２誤差関数Ｃ_ＫＦｊの重みｗ_Ｘｊ、ｗ_Ｙｊ、及びｗ_θｊが大きく設定されていることにより、車両の位置変化量（ΔＸ_ｖ，ΔＹ_ｖ）の取得精度が比較的高い撮影エッジの誤差関数が小さくなるようにグラフ構造データは最適化される。これにより、グラフ構造データを構成する特徴点３次元位置とカメラ位置・姿勢の精度を全体的に高めることができる。

【0074】

そして、グラフ構造データ最適化部４１０は、最適化したグラフ構造データを、環境地図作成部４２０に渡す。

【0075】

環境地図作成部４２０は、グラフ構造データ最適化部４１０により最適化されたグラフ構造データに基づいて、各特徴点の３次元位置情報を含んで構成される環境地図を作成する。

【0076】

具体的には、環境地図作成部４２０は、非特許文献１と同様に、グラフ構造データ最適化部４１０により最適化されたグラフ構造データに基づいて、各特徴点の３次元位置情報を含んで構成される環境地図を作成する。ここで、非特許文献１に比べて特徴点の３次元位置の精度が高いため、歪みの小さい地図を作成できる。

【0077】

そして、環境地図作成部４２０は、作成した環境地図を、出力部４３０に渡す。

【0078】

出力部４３０は、環境地図作成部４２０により作成された環境地図を出力する。

【0079】

＜地図作成装置３００の動作＞
次に、図６を参照して、本実施形態の地図作成装置３００の地図作成処理ルーチンについて説明する。

【0080】

まず、ステップＳ１００において、地図作成装置３００は、ｉ＝１とする。ここで、ｉは、選択するキーフレームの数を数えるカウンタである。

【0081】

ステップＳ１１０において、センサ情報入力部３１０は、車輪速センサ２００により検出された車輪速、及び操舵角センサ２１０により検出された操舵角の入力を受け付ける。

【0082】

ステップＳ１２０において、オドメトリ計算部３２０は、上記ステップＳ１１０で得られた車両に関するセンサ情報に基づいて、車両の位置変化量（ΔＸ_ｖ，ΔＹ_ｖ）及びヨー角変化量Δθ_ｖを計算する。

【0083】

ステップＳ１３０において、画像入力部３３０は、撮影装置１００から画像の入力を受け付ける。

【0084】

ステップＳ１４０において、キーフレーム選択部３４０は、ｉ＝１であるか否かを判定する。

【0085】

ｉ＝１である場合（上記ステップＳ１４０のＹＥＳ）、ステップＳ１６０に進む。

【0086】

一方、ｉ＝１でない場合（上記ステップＳ１４０のＮＯ）、ステップＳ１５０において、キーフレーム選択部３４０は、車両の走行距離が所定値以上であるか否かを判定する。

【0087】

走行距離が所定値以上である場合（上記ステップＳ１５０のＹＥＳ）、ステップＳ１６０に進む。

【0088】

一方、走行距離が所定値以上でない場合（上記ステップＳ１５０のＮＯ）、ステップＳ１１０に戻る。

【0089】

ステップＳ１６０において、キーフレーム選択部３４０は、上記ステップＳ１３０により取得した画像をｉ番目のキーフレームとして選択する。

【0090】

ステップＳ１７０において、キーフレーム選択部３４０は、全てのキーフレームを選択したか否かを判定する。

【0091】

全てのキーフレームを選択していない場合（上記ステップＳ１７０のＮＯ）、ステップＳ１８０において、キーフレーム選択部３４０は、ｉに１を加算し、ステップＳ１１０に戻る。

【0092】

一方、全てのキーフレームを選択している場合（上記ステップＳ１７０のＹＥＳ）、ステップＳ１９０において、特徴点検出部３５０は、複数のキーフレームの各々から複数の特徴点を検出する。

【0093】

ステップＳ２００において、特徴量算出部３６０は、複数のキーフレームの各々について、当該キーフレームの複数の特徴点の各々の特徴量を算出する。

【0094】

ステップＳ２１０において、特徴点対応付け部３７０は、上記ステップＳ２００により算出された特徴点の特徴を示す特徴量を用いて、複数のキーフレーム間における特徴点を対応付ける。

【0095】

ステップＳ２２０において、３次元位置計算部３８０は、上記ステップＳ２１０により対応付けられた特徴点の各々についての３次元位置と、複数のキーフレームの各々についての撮影装置１００の位置及び姿勢とを計算する。

【0096】

ステップＳ２３０において、グラフ構造データ生成部３９０は、特徴点の各々についての、当該特徴点の３次元位置を規定する特徴点ノードと、複数のキーフレームの各々についての、当該キーフレームを撮影した撮影装置１００の位置及び姿勢を規定する撮影ノードと、特徴点ノードと撮影ノードとの間の各々を結ぶ、当該撮影ノードのキーフレーム上の、当該特徴点ノードの特徴点の位置を規定する特徴点エッジと、隣接する撮影ノード間の各々を結ぶ、当該撮影ノードのキーフレーム間における車両の位置及び姿勢の変化量を規定する撮影エッジとから構成されるグラフ構造データを生成する。

【0097】

ステップＳ２４０において、誤差関数設定部４００は、上記ステップＳ２３０により生成されたグラフ構造データに含まれる特徴点エッジの各々について、特徴点エッジで結ばれた特徴点ノードの特徴点を、撮影ノードが規定する撮影装置１００の位置及び姿勢を用いて撮影ノードのキーフレーム上に投影したときの投影誤差を表す第１誤差関数Ｃ_Ｐｉｊを設定すると共に、グラフ構造データに含まれる撮影エッジの各々について、当該撮影エッジが規定する位置及び姿勢の変化量とオドメトリ計算部３２０による計算結果との誤差を表す第２誤差関数Ｃ_ＫＦｊを設定する。

【0098】

ステップＳ２５０において、グラフ構造データ最適化部４１０は、特徴点エッジの各々についての第１誤差関数Ｃ_Ｐｉｊと、撮影エッジの各々についての第２誤差関数Ｃ_ＫＦｊとの和が最小となるように、グラフ構造データの特徴点ノードの各々が規定する当該特徴点の３次元位置、及び撮影ノードの各々が規定する撮影装置１００の位置及び姿勢を最適化する。

【0099】

ステップＳ２６０において、環境地図作成部４２０は、上記ステップＳ２５０により最適化されたグラフ構造データに基づいて、各特徴点の３次元位置情報を含んで構成される環境地図を作成する。

【0100】

ステップＳ２７０において、出力部４３０は、上記ステップＳ２６０により作成された環境地図を出力する。

【0101】

また、上記ステップＳ２２０は、図７に示す３次元位置計算処理ルーチンによって実現される。

【0102】

ステップＳ２２１において、３次元位置計算部３８０は、ｉ＝２とする。ここで、ｉは、キーフレームが撮影された番号を示すカウンタである。

【0103】

ステップＳ２２２において、３次元位置計算部３８０は、ｉ番目のキーフレームを選択する。

【0104】

ステップＳ２２３において、３次元位置計算部３８０は、ｉ＝２であるか否かを判定する。

【0105】

ｉ＝２である場合（上記ステップＳ２２７のＹＥＳ）、ステップＳ２２４において、３次元位置計算部３８０は、１番目のキーフレームとの間で、８点アルゴリズムとＲＡＮＳＡＣ（Ｒａｎｄｏｍ ｓａｍｐｌｅ ｃｏｎｓｅｎｓｕｓ）とを用いて、１番目のキーフレームの撮影位置と、２番目のキーフレームの撮影位置とにおける撮影装置１００の位置及び姿勢の変化量（無次元量）を計算し、車両の位置変化量（ΔＸ_ｖ，ΔＹ_ｖ）を用いて有次元化し、ステップＳ２２６に進む。

【0106】

一方、ｉ＝２でない場合（上記ステップＳ２２７のＮＯ）、ステップＳ２２５において、３次元位置計算部３８０は、ｉ－１番目のキーフレームとの間で対応付けた特徴点のうち、３次元位置が既知の特徴点を抽出し、その再投影誤差の和が最小となる撮影装置１００の位置及び姿勢の変化量を計算し、ステップＳ２２６に進む。

【0107】

ステップＳ２２６において、３次元位置計算部３８０は、ｉ－１番目のキーフレームとの間で対応付けた特徴点のうち、３次元位置が未知の特徴点について、ｉ番目のキーフレーム上の特徴点の位置と撮影装置１００の位置及び姿勢の変化量とから三角測量の原理により当該特徴点の３次元位置を計算する。

【0108】

ステップＳ２２７において、３次元位置計算部３８０は、全てのキーフレームについて処理を行ったか否かを判定する。

【0109】

全てのキーフレームについて処理を行っていない場合（上記ステップＳ２２７のＮＯ）、ステップＳ２２８において、３次元位置計算部３８０は、ｉに１を加算し、ステップＳ２２２に戻る。

【0110】

一方、全てのキーフレームについて処理を行っている場合（上記ステップＳ２２７のＹＥＳ）、リターンする。

【0111】

以上説明したように、本発明の実施の形態に係る地図作成装置によれば、特徴点の３次元位置を規定する特徴点ノードと、撮影装置の位置及び姿勢を規定する撮影ノードと、特徴点ノードと撮影ノードとの間の各々を結ぶ、撮影ノードの画像上の、特徴点ノードの特徴点の位置を規定する特徴点エッジと、隣接する撮影ノード間の各々を結ぶ、撮影ノードの画像間における移動体の位置及び姿勢の変化量を規定する撮影エッジとから構成されるグラフ構造データを生成し、特徴点エッジで結ばれた特徴点ノードの特徴点を、撮影ノードが規定する撮影装置の位置及び姿勢を用いて撮影ノードの画像上に投影したときの投影誤差を表す第１誤差関数を設定すると共に、撮影エッジが規定する位置及び姿勢の変化量の誤差を表す第２誤差関数を、当該撮影エッジに対応する当該移動体の姿勢の変化量が小さいほど大きな重みを含んで表されるように設定し、第１誤差関数と、第２誤差関数との和が最小となるように、グラフ構造データの特徴点の３次元位置、及び撮影装置の位置及び姿勢を最適化することにより、車両の位置・姿勢の変化量を高い精度で取得できない場合であっても、特徴点の３次元位置を精度良く推定することができる。

【0112】

なお、本発明は、上述した実施の形態に限定されるものではなく、この発明の要旨を逸脱しない範囲内で様々な変形や応用が可能である。

【0113】

例えば、上記実施の形態では、地図作成装置３００が車両に搭載されている場合を例に説明したが、地図作成装置３００がインターネット上のサーバとして構成されてもよい。

【0114】

この場合、撮影装置１００、車輪速センサ２００、及び操舵角センサ２１０は、無線通信などにより、インターネットを介して地図作成装置３００に画像、車輪速、及び操舵角をそれぞれ送信するように構成すればよい。

【0115】

また、本願明細書中において、プログラムが予めインストールされている実施形態として説明したが、当該プログラムを、コンピュータ読み取り可能な記録媒体に格納して提供することも可能である。

【符号の説明】

【0116】

１０ 地図作成システム
１００ 撮影装置
２００ 車輪速センサ
２１０ 操舵角センサ
３００ 地図作成装置
３１０ センサ情報入力部
３２０ オドメトリ計算部
３３０ 画像入力部
３４０ キーフレーム選択部
３５０ 特徴点抽出部
３６０ 特徴量算出部
３７０ 特徴点対応付け部
３８０ ３次元位置計算部
３９０ グラフ構造データ生成部
４００ 誤差関数設定部
４１０ グラフ構造データ最適化部
４２０ 環境地図作成部
４３０ 出力部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】