特許 6916091 位置姿勢推定システム及び位置姿勢推定装置

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

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

(11)【特許番号】6916091

(24)【登録日】2021年7月19日

(45)【発行日】2021年8月11日

(54)【発明の名称】位置姿勢推定システム及び位置姿勢推定装置

(51)【国際特許分類】

   G05D 1/02 20200101AFI20210729BHJP

   G06T 7/00 20170101ALI20210729BHJP

【ＦＩ】

   G05D1/02 K

   G06T7/00 650A

【請求項の数】5

【全頁数】14

(21)【出願番号】特願2017-217482(P2017-217482)

(22)【出願日】2017年11月10日

(65)【公開番号】特開2019-91102(P2019-91102A)

(43)【公開日】2019年6月13日

【審査請求日】2020年3月23日

(73)【特許権者】

【識別番号】502324066

【氏名又は名称】株式会社デンソーアイティーラボラトリ

(74)【代理人】

【識別番号】100115808

【弁理士】

【氏名又は名称】加藤 真司

(74)【代理人】

【識別番号】100113549

【弁理士】

【氏名又は名称】鈴木 守

(74)【代理人】

【識別番号】230121430

【弁護士】

【氏名又は名称】安井 友章

(72)【発明者】

【氏名】関川 雄介

【審査官】 山村 秀政

(56)【参考文献】

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

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

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

【文献】 Sen Wang et al.，Deep VO : Towards End-to-End Visual Odmetory with Deep Recurrent Convolutional Neural Networks，2017 IEEE International Conference on Robotics and Automation (ICRA)，IEEE，２０１７年 ６月 ３日，pp. 2043-2050

【文献】 Aaron van den Oord et al.，WaveNet: A Generative Model for Raw Audio，<URL:https://arXiv.org/pdf/1609.03499.pdf>，２０１６年 ９月１９日，1609.03499v2[cs SD]，pp.1-15

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

Ｇ０５Ｄ １／０２

Ｇ０６Ｔ ７／００

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

【請求項1】

２次元の位置情報及び１次元の時間情報を含む３次元の光学センシングデータを生成する光学センシング装置と、
時系列に入力される前記光学センシングデータに基づいて、ビジュアルオドメトリによって前記光学センシング装置の位置姿勢を推定する位置姿勢推定装置と、
を備え、
前記位置姿勢推定装置は、
前記光学センシングデータからなる連続する複数のフレームの各々の前記位置情報をそれぞれ入力して、特徴量を出力する複数の２次元ＣＮＮモジュールからなる２次元畳込部と、
前記複数の２次元ＣＮＮモジュールの各々から出力される複数の前記特徴量を入力して、隣接する前記フレームの間の位置姿勢の変化量を局所変化量として出力する１次元ＣＮＮモジュールからなる１次元畳込部と、
前記複数のフレームの前記局所変化量を累積することで前記局所変化量の累積値を求め、位置姿勢の初期値に前記累積値を加えることで、前記複数のフレームの後の前記光学センシング装置の位置姿勢を求める累積部と、
を備え、
前記光学センシング装置は、車両の外側をセンシングするように該車両に固定され、
前記累積部は、モデルベースで前記局所変化量を累積し、前記局所変化量を直進変化量及び角度変化量のパラメータで表現する、位置姿勢推定システム。

【請求項2】

前記光学センシング装置は、イベントカメラである、請求項１に記載の位置姿勢推定システム。

【請求項3】

前記位置姿勢推定装置は、前記光学センシング装置から入力された前記光学センシングデータの時間解像度を低下させて、時間解像度が低下した前記複数のフレームを生成する前処理部をさらに含む、請求項１又は２に記載の位置姿勢推定システム。

【請求項4】

前記累積部は、前記パラメータについての第１のエラーと、前記局所変化量についての第２のエラーとをそれぞれ第１の重み及び第２の重みで重みづけして学習に用い、ここで、学習の初期には前記第２の重みを重くし、学習の後期には前記第１の重みを重くするように調整して学習を行う、請求項１〜３のいずれかに記載の位置姿勢推定システム。

【請求項5】

２次元の位置情報及び１次元の時間情報を含む３次元の光学センシングデータを生成する光学センシング装置とともに用いられ、時系列に入力される前記光学センシングデータに基づいて、ビジュアルオドメトリによって前記光学センシング装置の位置姿勢を推定する位置姿勢推定装置であって、
前記光学センシングデータからなる連続する複数のフレームの各々の前記位置情報をそれぞれ入力して、特徴量を出力する複数の２次元ＣＮＮモジュールからなる２次元畳込部と、
前記複数の２次元ＣＮＮモジュールの各々から出力される複数の前記特徴量を入力して、隣接する前記フレームの間の位置姿勢の変化量を局所変化量として出力する１次元ＣＮＮモジュールからなる１次元畳込部と、
前記複数のフレームの前記局所変化量を累積することで前記局所変化量の累積値を求め、位置姿勢の初期値に前記累積値を加えることで、前記複数のフレームの後の前記光学センシング装置の位置姿勢を求める累積部と、
を備え、
前記光学センシング装置は、車両の外側をセンシングするように該車両に固定され、
前記累積部は、モデルベースで前記局所変化量を累積し、前記局所変化量を直進変化量及び角度変化量のパラメータで表現する、位置姿勢推定装置。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、自走体からの観察により得られた光学センシングデータに基づいて該自走体の位置及び姿勢を推定する位置姿勢推定システム及び位置姿勢推定装置に関する。

【背景技術】

【0002】

従来、自走体（以下、車両を例として説明する）の位置及び姿勢を計測するのに全地球測位システム（Global Positioning System、以下「ＧＰＳ」という。）が用いられている。車両はＧＰＳ受信機を備えており、複数のＧＰＳ衛星からの信号をこのＧＰＳ受信機で受信することにより、コード測位方式又は搬送波測位方式で自車両の位置を測定できる。

【0003】

しかしながら、ＧＰＳ受信機がＧＰＳ衛星からの信号を受信できないトンネル内等の場所では、ＧＰＳによる自車両の位置測定ができない。自車両の位置を測定するＧＰＳ以外の方法の一つとして、ホイールオドメトリやビジュアルオドメトリがある。ホイールオドメトリは、自車両の車輪の方向と回転数とを積分することで自車両の移動軌跡を測定して自車両の位置及び姿勢を推定するものである。ビジュアルオドメトリは、自車両に固定されたカメラによる連続的な複数の画像に基づいて自車両の移動軌跡を推定することで、自車の位置及び姿勢を推定するものである。

【0004】

このビジュアルオドメトリについては、モデルベースの手法が長らく研究されてきたが、近年ディープニューラルネットワーク（Deep Neural Network、以下「ＤＮＮ」という。）を使った学習ベースの手法が着目されている（例えば、特許文献１）。

【先行技術文献】

【非特許文献】

【0005】

【非特許文献1】e.g., R. Clark, S. Wang, H. Wen, A. Markham, and N. Trigoni. Vinet: Visual-inertial odometry as a sequence-to-sequence learning problem. In AAAI, pages 3995-4001, 2017

【発明の概要】

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

【0006】

ビジュアルオドメトリにおいて、自車両の位置姿勢推定を精度良く行うには、時間的解像度の高いイメージセンサを用いて長時間の相関をモデル化する必要がある。そのような時間的解像度の高いイメージセンサとして、イベントカメラが注目されている。

【0007】

しかしながら、従来のＤＮＮは、短時間の相関を把握することはできたが、長時間の相関を把握するにはシステムの処理負荷（処理時間、使用メモリ容量）が過大になり、現実的ではなかった。

【0008】

そこで、本発明は、長時間の相関をモデル化できるＣＮＮ（Long Short-Term CNN、以下「ＬＳＴＣＮＮ」という。）を用いてビジュアルオドメトリを行う位置姿勢推定システム、位置姿勢推定方法、及び位置姿勢推定プログラムを提供することを目的とする。

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

【0009】

本願発明の位置姿勢推定装置は、ビジュアルオドメトリを行うためにイメージセンサのデータに対して行う時空間の３次元ＣＮＮを空間の２次元ＣＮＮと時間の１次元ＣＮＮとに分解して実行する。これにより、処理負荷を過大にすることなく、畳込処理が可能な時間範囲を長くすることができる。

【0010】

本発明の一態様の位置姿勢推定システムは、２次元の位置情報及び１次元の時間情報を含む３次元の光学センシングデータを生成する光学センシング装置と、時系列に入力される前記光学センシングデータに基づいて、ビジュアルオドメトリによって前記光学センシング装置の位置姿勢を推定する位置姿勢推定装置とを備える。前記位置姿勢推定装置は、前記光学センシングデータからなる連続する複数のフレームの各々の前記位置情報をそれぞれ入力して、特徴量を出力する複数の２次元ＣＮＮモジュールからなる２次元畳込部と、前記複数の２次元ＣＮＮモジュールの各々から出力される複数の前記特徴量を入力して、隣接する前記フレームの間の位置姿勢の変化量を局所変化量として出力する１次元ＣＮＮモジュールからなる１次元畳込部と、前記複数のフレームの前記局所変化量を累積することで前記局所変化量の累積値を求め、位置姿勢の初期値に前記累積値を加えることで、前記複数のフレームの後の前記光学センシング装置の位置姿勢を求める累積部とを備えている。

【0011】

この構成により、時系列に入力される２次元位置情報と１次元時間情報からなる光学センシングデータに対して実行すべき３次元ＣＮＮを位置情報に対する２次元ＣＮＮと時間情報に対する１次元ＣＮＮとに分けて実行するので、処理負荷を過大にすることなく、畳込処理可能な時間範囲を長く（光学センシングデータの時間方向の数を多く）することができる。

【0012】

上記の位置姿勢推定システムにおいて、前記光学センシング装置は、イベントカメラであってよい。イベントカメラの時間解像度は高く、単位時間当たりのフレーム数が多くなるが、この構成によれば、そのような多フレーム（長時間）についても畳込を有効に行って位置姿勢を推定できる。

【0013】

上記の位置姿勢推定システムにおいて、前記位置姿勢推定装置は、前記光学センシング装置から入力された前記光学センシングデータの時間解像度を低下させて、時間解像度が低下した前記複数のフレームを生成する前処理部をさらに含んでいてよい。この構成により、光学センシング装置からの光学センシングデータの時間解像度が高すぎて畳込処理における入力データが時間方向に疎（スパース）になりすぎることを回避できる。

【0014】

上記の位置姿勢推定システムにおいて、前記光学センシング装置は、車両の外側をセンシングするように該車両に固定されていてよく、前記累積部は、モデルベースで前記局所変化量を累積し、前記局所変化量を直進変化量及び角度変化量のパラメータで表現してよい。この構成により、車両の移動の制約を活かして少パラメータのモデルで局所変化慮鵜の累積を行うことができる。

【0015】

上記の位置姿勢推定システムにおいて、前記２次元ＣＮＮモジュールの各々は、ＬＳＴＭモジュールであってよい。

【0016】

本発明の一態様の位置姿勢推定装置は、２次元の位置情報及び１次元の時間情報を含む３次元の光学センシングデータを生成する光学センシング装置とともに用いられ、時系列に入力される前記光学センシングデータに基づいて、ビジュアルオドメトリによって前記光学センシング装置の位置姿勢を推定する位置姿勢推定装置であって、前記光学センシングデータからなる連続する複数のフレームの各々の前記位置情報をそれぞれ入力して、特徴量を出力する複数の２次元ＣＮＮモジュールからなる２次元畳込部と、前記複数の２次元ＣＮＮモジュールの各々から出力される複数の前記特徴量を入力して、隣接する前記フレームの間の位置姿勢の変化量を局所変化量として出力する１次元ＣＮＮモジュールからなる１次元畳込部と、前記複数のフレームの前記局所変化量を累積することで前記局所変化量の累積値を求め、位置姿勢の初期値に前記累積値を加えることで、前記複数のフレームの後の前記光学センシング装置の位置姿勢を求める累積部とを備えている。

【0017】

この構成によっても、時系列に入力される２次元位置情報と１次元時間情報からなる光学センシングデータに対して実行すべき３次元ＣＮＮを位置情報に対する２次元ＣＮＮと時間情報に対する１次元ＣＮＮとに分けて実行するので、処理負荷を過大にすることなく、畳込処理可能な時間範囲を長く（光学センシングデータの時間方向の数を多く）することができる。

【発明の効果】

【0018】

本発明によれば、時系列に入力される２次元位置情報と１次元時間情報からなる光学センシングデータに対して実行すべき３次元ＣＮＮを位置情報に対する２次元ＣＮＮと時間情報に対する１次元ＣＮＮとに分けて実行するので、処理負荷を過大にすることなく、畳込処理可能な時間範囲を長く（光学センシングデータの時間方向の数を多く）することができる。

【図面の簡単な説明】

【0019】

【図1】本発明の実施の形態の位置姿勢推定システムの構成を示すブロック図

【図2】本発明の実施の形態のイベントカメラによるイベントデータと通常のカメラによる画像との時間解像度を比較する図

【図3】本発明の実施の形態のビジュアルオドメトリにおけるネットワーク構造及びデータの流れを示す図

【図4】本発明の実施の形態の１次元ＣＮＮモジュール２３及び累積部２４のネットワーク構造を示す図

【図5】本発明の実施の形態の平行２輪車両のモデルを示す図

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

【0020】

以下、図面を参照して本発明の実施の形態を説明する。なお、以下に説明する実施の形態は、本発明を実施する場合の一例を示すものであって、本発明を以下に説明する具体的構成に限定するものではない。本発明の実施にあたっては、実施の形態に応じた具体的構成が適宜採用されてよい。

【0021】

図１は、本発明の実施の形態の位置姿勢推定システムの構成を示すブロック図である。位置姿勢推定システム１００は、光学センシング装置１０と位置姿勢推定装置２０とからなる。位置姿勢装置２０は、前処理部２１、２次元畳込部２２、１次元畳込部２３、及び累積部２４を備えている。光学センシング装置１０は、自走体である車両に固定されて、車両外部を光学的にセンシングすることで光学センシングデータとしてイベントフレームを生成し、生成したイベントフレームを時系列に順に位置姿勢推定装置２０に出力する。

【0022】

本実施の形態では、光学センシング装置１０として、生物学的知見に基づいた（biologically inspired）カメラとしてのイベントカメラを採用する。通常のカメラは、各ピクセルが所定の露光時間に蓄積した光子の数を測定して、すべてのピクセルの測定結果を１フレームとして同時に出力する。これに対して、イベントカメラは、各ピクセルが非同期で作動する。また、イベントカメラは、前の検出強度（明度）と現在の検出強度（明度）との相違を検出したときに、１フレームの光学センシングデータとしてイベントフレームを出力する。イベントフレーム（以下、単に「フレーム」ともいう。）を構成するイベントデータには、検出強度の相違が生じているピクセルの位置情報、時間情報としてのタイムスタンプ、及び検出強度が増加しているか減少しているかを示す極性情報が含まれる。

【0023】

図２は、イベントカメラによるイベントデータと通常のカメラによる画像との時間解像度を比較する図である。イベントカメラは、時間解像度がマイクロ秒オーダであり、通常のカメラ（例えば、３０フレーム／秒）と比較して時間的解像度が極めて高い。また、イベントカメラは、強度の絶対値を検出せず、強度変化の極性のみを検出するので、ダイナミックレンジが広い。例えば、通常のカメラのダイナミックレンジが５０ｄＢ程度であるのに対して、イベントカメラのダイナミックレンジは１２０ｄＢ程度である。さらに、通常のカメラは強度が比較的強い部分しか検出できないのに対して、イベントカメラは、暗部と明部とを同時に検出できる。

【0024】

イベントカメラの上記の特性から、イベントカメラは車両の自動運転のシーンで有効に活用される。ただし、上述のようにイベントカメラの時間解像度は高いので、位置姿勢推定装置２０は、長時間（多フレーム）の相関を扱える必要がある。

【0025】

位置姿勢推定装置２０は、光学センシング装置１０から時系列に並んだ複数のイベントフレームを取得して、それらのフレーム数を減少させてフレームｅ１〜ｅＫを抽出する。ここで、Ｋは、位置姿勢推定装置２０において一度に処理可能なフレーム数（以下、「許容フレーム数」ともいう。）である。位置姿勢推定装置２０は、複数のフレームｅ１〜ｅＫから、ビジュアルオドメトリによって１フレーム後〜Ｋフレーム後の自車両の位置姿勢ｐＫを求める。

【0026】

即ち、位置姿勢推定装置２０によって、下式（１）によるビジュアルオドメトリが実行される。

【数1】

ここで、

【数2】

は、タイムステップｋのフレーム（以下、「第ｋフレーム」等と表現する。）であり、Ｍ×Ｎは、イベントカメラの画素数である。また、ｍは、ホイールオドメトリ、慣性測定装置等のビジュアルオドメトリ以外の方法で得られた付加的なセンシングデータである。入力データにｍを含めるか否かは任意である。また、ｐｋは、第ｋフレームにおける光学センシング装置（が固定された自車両）の位置姿勢である。

【0027】

具体的には、位置姿勢推定装置２０は、隣り合うフレーム間における自車両の位置及び姿勢の変化（以下、「局所変化量」ともいう。）Δｐ１〜ΔｐＫを求め、それらを順に累積（連結）して、自車両の位置及び姿勢の初期値（以下、「初期位置姿勢」という。）ｐ０に加えることで、第Ｋフレームの自車両の位置姿勢ｐＫを算出する。これを式で表すと、下式（２）となる。

【数3】

【0028】

上記のように、イベントカメラは時間解像度が高いので、位置姿勢推定装置２０におけるＬＳＴＣＮＮは、長時間（多フレーム、例えば、数千フレーム）の相関を扱える必要がある。そこで、位置姿勢推定装置２０は、光学センシング装置１０から入力された複数のフレームデータに対して実行すべき時空間の３次元ＣＮＮを空間の２次元ＣＮＮと時間の１次元ＣＮＮとに分解して実行する。このために、位置姿勢推定装置２０は、光学センシング装置１０から入力される複数のイベントフレームの数を削減する前処理部２１を備えている。また、位置姿勢推定装置２０は、空間の２次元ＣＮＮを実行する２次元畳込部２２と、時間の１次元ＣＮＮを実行する１次元畳込部２３とを有し、複数のフレームについて行う３次元ＣＮＮを２次元ＣＮＮと１次元ＣＮＮとに分割して行う。

【0029】

前処理部２１について説明する。光学センシング装置１０から出力される複数のイベントフレームは、各ピクセルにおいて非同期であり、各イベントフレームは｛ｕ，ｖ，ｔ，ｐ｝の４次元のイベントデータからなる。ここで、ｕ、ｖはイベントが検出された位置であり、ｔはイベントが検出された時刻（タイムスタンプ）であり、ｐは検出されたイベントの極性である。これらのイベントデータは、前処理部２１において時空間のイベントフレームに変換される。

【0030】

前処理部２１は、イベントフレームを構成するために、イベントフレームの各データｕ、ｖ、ｔ、ｐを、３次元テンソルの対応する時空間位置に投影する。イベントカメラの時間解像度は、１マイクロ秒程度と非常に小さいので、３次元テンソルをその粒度で用意すると、２次元畳込部２２に入力される複数のフレームが疎（スパース）になりすぎてＣＮＮで処理するのに非効率的になる。そこで前処理部２１は、十分に粗く、ただし、通常のカメラのフレームレート（例えば、３０フレーム／秒）よりは小さい時間解像度τ（例えば、１，０００マイクロ秒程度）にまでイベントフレームの時間解像度を低下させる。

【0031】

前処理部２１は、光学センシング装置１０から得られた細かい時間情報を維持するために、各イベントに対する

【数4】

の３つの重み係数を下式（４）〜（６）によって計算する。

【数5】

ここで、ｔはイベントのタイムスタンプであり、

【数6】

は、ｔに最も近い離散化タイムスタンプであり、

【数7】

である。

【0032】

前処理部２１は、上記のようにして、時間解像度が光学センシング装置１０から入力される複数のイベントフレームより小さい複数のフレームを生成して２次元畳込部２２に入力する。

【0033】

２次元畳込部２２は、前処理部２１から入力される各フレームに対して、それぞれ２次元ＣＮＮを実行する複数の２次元ＣＮＮモジュール２２−１〜２２−Ｋからなり、１次元畳込部２３は、１次元ＣＮＮモジュールからなる。各２次元ＣＮＮモジュール２２−１〜２２−Ｋは、時分割されたＭ×Ｎ×Ｌの短時間のイベントフレームを処理し、１次元ＣＮＮモジュールは、Ｆ×１×Ｔのサイズの長時間の特徴量を処理する。ここで、Ｆは、各２次元ＣＮＮモジュール２２−１〜２２−Ｋから出力される特徴量の長さであり、Ｔは、Ｔ＝Ｋ／Ｌを満たす。

【0034】

図３は、本発明の実施の形態のビジュアルオドメトリにおけるネットワーク構造及びデータの流れを示す図である。Ｌは、入力されるフレームの特性に応じて１〜Ｋの間で任意に設定される。例えば、Ｋ＝３０００のときにＬ＝１００と設定してよい。

【0035】

各２次元ＣＮＮモジュール２２−１〜２２−Ｋは、空間（２次元）の畳み込みを行い、１次元ＣＮＮモジュール２３は、時間（１次元）の畳み込みを行う。各２次元ＣＮＮモジュール２２−１〜２２−Ｋの構造は、例えばＶＧＧ−１６（Simonyan, K., and isserman, A. 2014. Very deep convolutional networks for large-scale image recognition. CoRR abs/1409.1556.）ネットワークの畳込部分と同様であってよい。また、１次元ＣＮＮモジュール２３は、ＷａｖｅＮｅｔ（van den Oord, A.; Dieleman, S.; Zen, H.; Simonyan, K.; Vinyals, O.; Graves, A.; Kalchbrenner, N.; Senior, A. W.; and Kavukcuoglu, K. 2016. Wavenet: A generative model for raw audio. CoRR abs/1609.03499.）に似た通常の畳込モジュールを積み重ねて構成される。

【0036】

図４は、１次元ＣＮＮモジュール２３及び累積部２４のネットワーク構造を示す図である。１次元ＣＮＮモジュール２３は、ＷａｖｅＮｅｔで用いられているように、下式（３）で示されるゲート付き活性化関数（gated activation）ユニットを用いて、複雑な時間依存をモデル化する。

【数8】

【0037】

ここで、ｆは２次元ＣＮＮモジュールのネットワーク２２−１〜２２−Ｋであり、ｇは１次元ＣＮＮモジュール２３のネットワークである。１次元ＣＮＮネットワークｇはＯ層（Ｏは自然数）の層構造を有し、第（Ｏ−１）層の出力は、連続するフレームから推定される隣接フレーム間の変化（局所変化量）であり、第Ｏ層は、パラメータレスのモデルベース位置姿勢連結（Model-based Pose Concatenation、以下「ＭＰＣ」という。）である。ＭＰＣは累積部２４として実装される。ＭＰＣモジュール、即ち累積部２４は、タイムステップｋの局所変化量Δｐｋを用いてタイムステップｋの位置姿勢ｐｋを更新することでタイムステップｋ＋１の位置姿勢ｐｋ＋１を推定する。

【0038】

１次元畳込部２３の最終層では推定された位置姿勢のエラーが計算される。このエラーは、２次元畳込部２２及び１次元畳込部２３のパラメータの更新に用いられる。ＭＰＣモジュールとしての累積部２４は、最終的なエラーから安定したデリバティブ（derivative）（即ち、各ニューロンのパラメータに対する微分）を効果的に算出できる。このＭＰＣモジュールは、以下の知見から得られるものである。

【0039】

図５は、平行２輪車両のモデルを示す図である。図５に示すように、車両の動きには制約があり、リー代数ｓｅ（２）又はｓｅ（３）のパラメータセットよりも少ないパラメータで表現できる。すなわち、平行２輪車両モデルでは、局所的な移動は、直進速度ｖと角速度ωのパラメータによって表現できるが、本実施の形態の累積部２４は、直進変化ΔＬと角度変化Δθを用い、位置姿勢エラーをΣΔＬとΣΔθで定義する。このようなパラメータ化及び位置姿勢エラーの定義の変更によって、各局所変化量についてのエラー関数のデリバティブの計算を容易かつ安定的にすることができる。

【0040】

以下では、まず、簡単のために、車両が２次元平面を走行する（車両の高さ方向の移動を考慮しない）２次元の場合を説明する。２次元の場合には、車両の位置姿勢は、車両の位置及び直進角

【数9】

で表示できる。

【0041】

通常は、位置姿勢は、第ｋフレームにおける局所時間Δｔの間の局所変化量

【数10】

を用いて下式（７）で更新される。

【数11】

ここで、Δθｋは、

【数12】

によって与えられ、ΔＬは、ｖ及びωを用いて下式（８）により計算される。

【数13】

【0042】

タイムステップｋ−１のエラーはタイムステップｋのエラーに依存しているので、位置姿勢ｐＫの各局所変化量ｚｋに関するデリバティブは、非線形に式（７）及び式（８）に関連している。よって、計算負荷が高く、また、実装が困難であり、さらに、位置姿勢の累積が真の位置姿勢から遠くなっている場合に、デリバティブ自体が不安定となってしまう。

【0043】

従来の誤差の定義の場合には、タイムステップｋの位置姿勢の誤差は、タイムステップ１〜ｋ−１の位置姿勢のエラーに依存するので、その微分は過去の式（８）を経由して過去の微分に影響される。したがって、微分は位置姿勢の積分を行う区間全部のエラーの関数になってしまい、結果として計算負荷が大きくなる。これに対して、本実施の形態では、最終的に積分したエラーの各タイムステップ（時刻）の局所変化量に対する微分がタイムステップごとに独立になるので、計算が簡単で軽量になる。

【0044】

具体的には、本実施の形態の累積部２４では、局所変化量を

【数14】

と表現する代わりに、

【数15】

と表現し、累積された経路と角度のエラーを

【数16】

ではなく、

【数17】

と表記する。

【0045】

第（Ｏ−１）層、即ち１次元ＣＮＮ部２３の出力層は、

【数18】

を出力し、第Ｏ層はＭＰＣモジュールとして現在の車両の動きを式（７）を用いて更新する。ここで、ｑｋは、下式（９）のように定義できる。

【数19】

【0046】

ＭＰＣモジュールで計算される累積位置姿勢エラーＬａｃｃｕｍは、下式（１０）で定義される。

【数20】

ここで、

【数21】

は、累積された経路及び角度の真値である。

【0047】

【数22】

の局所変化量

【数23】

に関するヤコビ行列は、下式（１１）で計算される。

【数24】

【0048】

ＭＰＣモジュールは、上記の累積位置姿勢エラーＬａｃｃｕｍに加えて、局所変化量のエラーＬｌｏｃａｌも下式（１２）で計算する。

【数25】

【0049】

これらの累積位置姿勢エラーＬａｃｃｕｍと局所移動エラーＬｌｏｃａｌの合計

【数26】

は、ネットワークを学習するのに用いられる。調整パラメータλ１、λ２は、学習の初期にはＬｌｏｃａｌを強調し、学習の後期にはＬａｃｃｕｍを強調するように調整する。これにより、連結された位置姿勢の推定の精度を向上できる。

【0050】

次に、位置姿勢推定装置２０の学習について説明する。位置姿勢推定装置２０の学習を行う際には、入力データとして、許容フレーム数Ｋの２倍の長さの２Ｋフレーム分の連続するシーケンスがデータセットからランダムに抽出される。位置姿勢推定装置２０は、時分割された（Ｍ×Ｎ×Ｌ）の入力データを２Ｔ回にわたって２次元畳込部２２に入力することで、Ｆ×１×２Ｔの大きさのテンソルを取得する。

【0051】

これらのテンソルは１次元畳込部２３に入力され、１次元畳込部２３において位置姿勢及び位置姿勢のエラーが計算される。１次元畳込部２３は、このエラーを用いて更新される。２次元畳込部２２の直前のエラーは、Ｔこの短時間エラーに分割され、Ｔ回にわたって２次元畳込部２２の更新に用いられる。なお、２Ｋフレームは、１次元畳込部２３から有効なＴを取得する必要がある。

【0052】

本実施の形態では、通常の畳込層について水増しを行わないので、カーネルサイズの半分まで出力を減少させることができる。また、付加的なセンシングデータｍが利用できる場合には、それらは２次元畳込部２２の出力に連結され、その時間情報は２次元畳込部２２の出力とともに１次元畳込部２３によってモデル化される。

【0053】

２次元畳込部２２の各２次元ＣＮＮモジュール２２−１〜２２−Ｋ及び１次元畳込部２３の１次元ＣＮＮモジュールを最適化するために、Ａｄａｍ（Kingma, D., and Ba, J. 2014. Adam: A method for stochastic optimization. arXiv preprint arXiv:1412.6980.）をハイパーパラメータ（学習レートは、ｌ＝０．００１、β１＝０．９、β２＝０．９９９、ε＝０．０００００００１）とともに利用することができる。

【0054】

以上のように、本実施の形態の位置姿勢推定システム１００によれば、時系列に入力される２次元の位置情報と１次元の時間情報からなる光学センシングデータに対して実行すべき３次元ＣＮＮを位置情報に対する２次元ＣＮＮと時間情報に対する１次元ＣＮＮとに分けて実行するので、処理負荷を過大にすることなく、畳込処理可能な時間範囲を長く（光学センシングデータの時間方向の数を多く）することができる。

【産業上の利用可能性】

【0055】

本発明は、時系列に入力される２次元の位置情報と１次元の時間情報からなる光学センシングデータに対して実行すべき３次元ＣＮＮを位置情報に対する２次元ＣＮＮと時間情報に対する１次元ＣＮＮとに分けて実行するので、処理負荷を過大にすることなく、畳込処理可能な時間範囲を長く（光学センシングデータの時間方向の数を多く）することができ、自走体から撮影された画像に基づいて該自走体の位置及び姿勢を推定する位置姿勢推定システム等として有用である。

【符号の説明】

【0056】

１０ 光学センシング装置
２０ 位置姿勢推定装置
２１ 前処理部
２２ ２次元畳込部
２３ １次元畳込部
２４ 累積部
１００ 位置姿勢推定システム

【図1】

【図2】

【図3】

【図4】

【図5】