図面 (/)

技術 状態推定装置、情報端末、情報処理システム、及びプログラム

出願人 株式会社豊田中央研究所
発明者 武山洪二郎城殿清澄
出願日 2016年2月29日 (4年11ヶ月経過) 出願番号 2016-037818
公開日 2017年9月7日 (3年5ヶ月経過) 公開番号 2017-156154
状態 特許登録済
技術分野 航行(Navigation) 無線による位置決定 イメージ分析 画像処理
主要キーワード 系列データ数 時刻インデックス 蓄積データ数 最適軌跡 タイムステップ毎 各観測値 キャリブレーション行列 ピッチ角φ
関連する未来課題
重要な関連分野

この項目の情報は公開日時点(2017年9月7日)のものです。
また、この項目は機械的に抽出しているため、正しく解析できていない場合があります

図面 (18)

課題

車両の周辺画像情報と車両が取得した衛星情報とを考慮して、自車両の軌跡及び自車両の姿勢を精度良く推定することができるようにする。

解決手段

画像取得部22及び衛星情報取得部30が、GPS装置19によって受信された各時刻の衛星情報、及びカメラ12によって撮像された各時刻の車両の周辺の画像を車両データとして取得する。特徴点追跡部36が、画像の各々から特徴点を抽出し各時刻の画像の各々において各特徴点を追跡する。軌跡姿勢推定部42が、追跡された各特徴点の画像の各々における位置と、車両の各時刻の位置、車両の各時刻の姿勢、及び追跡された各特徴点の3次元位置から求まる、各特徴点の画像の各々における位置との差分、並びに取得された各時刻の衛星情報と、車両の各時刻の姿勢との整合性を表すコストを最小化するように、車両の各時刻の位置及び車両の各時刻の姿勢を推定する。

概要

背景

従来、2時刻以上の連続するカメラ画像上で対応し合う特徴点の情報を利用することで自車両の姿勢角(3軸)と並進方向速度(3軸)とをそれぞれ求める技術が知られている。しかし、画像の特徴点の対応に誤りがある場合、推定結果に大きな誤差を生じる可能性がある。

これに対し、特許文献1に記載の従来手法では、まず地磁気センサ加速度センサなどを用いて姿勢角を既知とすることで、特徴点の誤差の影響を受けにくい状態で並進速度を推定する。次に、並進成分を既知とすることで誤対応を含む特徴点を検出・除外する。そして、最後に残った正しい特徴点を用いて移動体姿勢及び並進方向速度を再計算することで、特徴点の対応に誤りがある場合でも精度の良い姿勢角および並進方向速度を得る。

概要

車両の周辺画像情報と車両が取得した衛星情報とを考慮して、自車両の軌跡及び自車両の姿勢を精度良く推定することができるようにする。画像取得部22及び衛星情報取得部30が、GPS装置19によって受信された各時刻の衛星情報、及びカメラ12によって撮像された各時刻の車両の周辺の画像を車両データとして取得する。特徴点追跡部36が、画像の各々から特徴点を抽出し各時刻の画像の各々において各特徴点を追跡する。軌跡姿勢推定部42が、追跡された各特徴点の画像の各々における位置と、車両の各時刻の位置、車両の各時刻の姿勢、及び追跡された各特徴点の3次元位置から求まる、各特徴点の画像の各々における位置との差分、並びに取得された各時刻の衛星情報と、車両の各時刻の姿勢との整合性を表すコストを最小化するように、車両の各時刻の位置及び車両の各時刻の姿勢を推定する。

目的

本発明は、上記の事情に鑑みてなされたもので、移動体の周辺の画像情報と移動体が取得した衛星情報とを考慮して、移動体の軌跡及び移動体の姿勢を精度良く推定することができる状態推定装置情報端末情報処理システム、及びプログラムを提供する

効果

実績

技術文献被引用数
0件
牽制数
0件

この技術が所属する分野

(分野番号表示ON)※整理標準化データをもとに当社作成

ライセンス契約や譲渡などの可能性がある特許掲載中! 開放特許随時追加・更新中 詳しくはこちら

請求項1

動体に搭載された受信手段によって受信された各時刻衛星情報、及び移動体に搭載された撮像手段によって撮像された各時刻の前記移動体の周辺の画像を移動体情報として取得する取得手段と、前記取得手段によって取得された各時刻の前記画像の各々から特徴点を抽出し、各時刻の前記画像の各々において各特徴点を追跡する特徴点追跡手段と、前記特徴点追跡手段によって追跡された各特徴点の、各時刻の前記画像の各々における位置と、前記移動体の各時刻の位置、前記移動体の各時刻の姿勢、及び前記特徴点追跡手段によって追跡された各特徴点の3次元位置から求まる、各特徴点の、各時刻の前記画像の各々における位置との差分、並びに前記取得手段によって取得された各時刻の前記衛星情報と、前記移動体の各時刻の姿勢との整合性を表すコストを最小化するように、前記移動体の各時刻の位置及び前記移動体の各時刻の姿勢を推定する状態推定手段と、を含む状態推定装置

請求項2

前記取得手段によって取得された各時刻の前記衛星情報に基づいて、各衛星についてのクロックドリフトを算出するクロックドリフト算出手段と、前記取得手段によって取得された各時刻の前記衛星情報に含まれる、各衛星の各時刻のドップラーシフトと、前記クロックドリフト算出手段によって算出された各衛星についてのクロックドリフト、及び前記移動体の各時刻の姿勢とに基づいて、前記ドップラーシフトから求められる前記移動体と各衛星との相対速度の観測値と、前記クロックドリフト及び前記移動体の各時刻の姿勢から求められる前記移動体と各衛星との相対速度の推定値との差分を表すドップラーコストを算出するドップラーコスト演算手段とを更に含み、前記状態推定手段は、前記差分と、前記ドップラーコスト演算手段によって算出された前記ドップラーコストとを含む前記コストを最小化するように、前記移動体の各時刻の位置及び前記移動体の各時刻の姿勢を推定する請求項1に記載の状態推定装置。

請求項3

ピッチ角算出手段を更に含み、前記取得手段は、前記移動体に搭載された速度センサによって検出された各時刻の速度と、前記移動体に搭載された加速度センサによって検出された各時刻の加速度と、前記移動体に搭載された角速度センサによって検出された各時刻の角速度とを前記移動体情報として更に取得し、前記ピッチ角算出手段は、前記取得手段によって取得された各時刻の前記速度及び各時刻の前記加速度に基づいて、前記移動体の各時刻のピッチ角を算出し、前記ピッチ角算出手段によって算出された前記移動体の各時刻のピッチ角と、前記移動体の姿勢のうちのピッチ角との差分を表すピッチ変化コストを算出するピッチ変化コスト演算手段と、前記取得手段によって取得された各時刻の前記速度から求められる移動体の位置の変化と、前記移動体の位置の変化との差分を表す位置変化コストを算出する位置変化コスト演算手段と、前記取得手段によって取得された各時刻の前記角速度から求められる移動体の方位角の変化と、前記移動体の姿勢のうちの方位角の変化との差分を表す方位変化コストを算出する方位変化コスト演算手段と、前記状態推定手段は、前記差分と、前記ドップラーコスト演算手段によって算出された前記ドップラーコストと、前記ピッチ変化コスト演算手段によって算出された前記ピッチ変化コストと、前記位置変化コスト演算手段によって算出された前記位置変化コストと、前記方位変化コスト演算手段によって算出された前記方位変化コストとを含む前記コストを最小化するように、前記移動体の各時刻の位置及び前記移動体の各時刻の姿勢を推定する請求項2に記載の状態推定装置。

請求項4

前記状態推定手段は、予め定められた収束条件を満たすまで、前記移動体の各時刻の位置、及び前記移動体の各時刻の姿勢を推定することを繰り返す請求項1〜請求項3の何れか1項に記載の状態推定装置。

請求項5

前記取得手段は、複数の前記移動体の各々について、前記移動体情報を取得し、前記状態推定手段は、複数の前記移動体の各々についての前記コストの総和を最小化するように、複数の前記移動体の各々について、前記移動体の各時刻の位置及び前記移動体の各時刻の姿勢を推定する請求項1〜請求項4の何れか1項に記載の状態推定装置。

請求項6

測位手段を更に含み、前記取得手段は、定点観測地点において観測された各時刻の衛星情報及び前記定点観測地点に関する情報を更に取得し、前記測位手段は、複数の前記移動体の各々について、前記取得手段によって取得された前記定点観測地点の各時刻の前記衛星情報及び前記定点観測地点に関する情報に基づいて、前記移動体の各時刻の位置を推定し、前記状態推定手段は、複数の前記移動体の各々についての、前記測位手段によって推定された前記移動体の各時刻の位置と、前記移動体の各時刻の位置との差分を含む前記コストの総和を最小化するように、複数の前記移動体の各々について、前記移動体の各時刻の位置及び前記移動体の姿勢を推定する請求項5に記載の状態推定装置。

請求項7

移動体に搭載された情報端末であって、前記移動体に搭載された受信手段によって受信された各時刻の衛星情報、前記移動体に搭載された撮像手段によって撮像された各時刻の前記移動体の周辺の画像、前記移動体に搭載された速度センサによって検出された各時刻の速度、前記移動体に搭載された加速度センサによって検出された各時刻の加速度、及び前記移動体に搭載された角速度センサによって検出された各時刻の角速度の少なくとも1つを前記移動体情報として取得する情報取得手段と、前記情報取得手段によって取得された前記移動体情報を、請求項1〜請求項6の何れか1項に記載の状態推定装置へ送信する通信手段と、を含む情報端末。

請求項8

請求項1〜請求項6の何れか1項に記載の状態推定装置と、請求項7に記載の情報端末とを含む情報処理システム

請求項9

コンピュータを、移動体に搭載された受信手段によって受信された各時刻の衛星情報、及び移動体に搭載された撮像手段によって撮像された各時刻の前記移動体の周辺の画像を移動体情報として取得する取得手段、前記取得手段によって取得された各時刻の前記画像の各々から特徴点を抽出し、各時刻の前記画像の各々において各特徴点を追跡する特徴点追跡手段、及び前記特徴点追跡手段によって追跡された各特徴点の、各時刻の前記画像の各々における位置と、前記移動体の各時刻の位置、前記移動体の各時刻の姿勢、及び前記特徴点追跡手段によって追跡された各特徴点の3次元位置から求まる、各特徴点の、各時刻の前記画像の各々における位置との差分、並びに前記取得手段によって取得された各時刻の前記衛星情報と、前記移動体の各時刻の姿勢との整合性を表すコストを最小化するように、前記移動体の各時刻の位置及び前記移動体の各時刻の姿勢を推定する状態推定手段として機能させるためのプログラム

技術分野

0001

本発明は、状態推定装置情報端末情報処理システム、及びプログラムに関する。

背景技術

0002

従来、2時刻以上の連続するカメラ画像上で対応し合う特徴点の情報を利用することで自車両の姿勢角(3軸)と並進方向速度(3軸)とをそれぞれ求める技術が知られている。しかし、画像の特徴点の対応に誤りがある場合、推定結果に大きな誤差を生じる可能性がある。

0003

これに対し、特許文献1に記載の従来手法では、まず地磁気センサ加速度センサなどを用いて姿勢角を既知とすることで、特徴点の誤差の影響を受けにくい状態で並進速度を推定する。次に、並進成分を既知とすることで誤対応を含む特徴点を検出・除外する。そして、最後に残った正しい特徴点を用いて移動体姿勢及び並進方向速度を再計算することで、特徴点の対応に誤りがある場合でも精度の良い姿勢角および並進方向速度を得る。

先行技術

0004

特開2004−78315号公報

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

0005

しかし、一般的に、複数画像を用いて姿勢・並進速度の推定を行う際には収束演算などを用いて観測方程式解く必要があるが、もし特徴点の誤対応が少ない場合であったとしても、収束演算の初期値などの影響によって局所解に陥り大きな推定誤差が発生することがある。

0006

従来技術では外部センサを利用することで特徴点の誤対応の影響を軽減しているが、最終的に画像の特徴点情報のみを用いて姿勢・並進成分を推定するため、その際の初期値の設定などにより上記問題が生じ、最終的に大きく誤った推定結果が得られる可能性がある、という問題がある。

0007

本発明は、上記の事情に鑑みてなされたもので、移動体の周辺画像情報と移動体が取得した衛星情報とを考慮して、移動体の軌跡及び移動体の姿勢を精度良く推定することができる状態推定装置、情報端末、情報処理システム、及びプログラムを提供することを目的とする。

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

0008

上記目的を達成するために、本発明の状態推定装置は、移動体に搭載された受信手段によって受信された各時刻の衛星情報、及び移動体に搭載された撮像手段によって撮像された各時刻の前記移動体の周辺の画像を移動体情報として取得する取得手段と、前記取得手段によって取得された各時刻の前記画像の各々から特徴点を抽出し、各時刻の前記画像の各々において各特徴点を追跡する特徴点追跡手段と、前記特徴点追跡手段によって追跡された各特徴点の、各時刻の前記画像の各々における位置と、前記移動体の各時刻の位置、前記移動体の各時刻の姿勢、及び前記特徴点追跡手段によって追跡された各特徴点の3次元位置から求まる、各特徴点の、各時刻の前記画像の各々における位置との差分、並びに前記取得手段によって取得された各時刻の前記衛星情報と、前記移動体の各時刻の姿勢との整合性を表すコストを最小化するように、前記移動体の各時刻の位置及び前記移動体の各時刻の姿勢を推定する状態推定手段と、を含んで構成されている。

0009

また、本発明のプログラムは、コンピュータを、移動体に搭載された受信手段によって受信された各時刻の衛星情報、及び移動体に搭載された撮像手段によって撮像された各時刻の前記移動体の周辺の画像を移動体情報として取得する取得手段、前記取得手段によって取得された各時刻の前記画像の各々から特徴点を抽出し、各時刻の前記画像の各々において各特徴点を追跡する特徴点追跡手段、及び前記特徴点追跡手段によって追跡された各特徴点の、各時刻の前記画像の各々における位置と、前記移動体の各時刻の位置、前記移動体の各時刻の姿勢、及び前記特徴点追跡手段によって追跡された各特徴点の3次元位置から求まる、各特徴点の、各時刻の前記画像の各々における位置との差分、並びに前記取得手段によって取得された各時刻の前記衛星情報と、前記移動体の各時刻の姿勢との整合性を表すコストを最小化するように、前記移動体の各時刻の位置及び前記移動体の各時刻の姿勢を推定する状態推定手段として機能させるためのプログラムである。

0010

本発明によれば、取得手段が、移動体に搭載された受信手段によって受信された各時刻の衛星情報、及び移動体に搭載された撮像手段によって撮像された各時刻の移動体の周辺の画像を移動体情報として取得する。

0011

そして、特徴点追跡手段が、取得手段によって取得された各時刻の画像の各々から特徴点を抽出し、各時刻の画像の各々において各特徴点を追跡する。

0012

そして、状態推定手段が、特徴点追跡手段によって追跡された各特徴点の、各時刻の画像の各々における位置と、移動体の各時刻の位置、移動体の各時刻の姿勢、及び特徴点追跡手段によって追跡された各特徴点の3次元位置から求まる、各特徴点の、各時刻の画像の各々における位置との差分、並びに取得手段によって取得された各時刻の衛星情報と、移動体の各時刻の姿勢との整合性を表すコストを最小化するように、移動体の各時刻の位置及び移動体の各時刻の姿勢を推定する。

0013

このように、各時刻の自車両の周辺の画像の各々から特徴点を抽出し、各時刻の画像の各々において各特徴点を追跡し、追跡された各特徴点の、各時刻の画像の各々における位置と、自車両の各時刻の位置、自車両の各時刻の姿勢、及び追跡された各特徴点の3次元位置から求まる、各特徴点の、各時刻の画像の各々における位置との差分、並びに取得された各時刻の衛星情報と、自車両の各時刻の姿勢との整合性を表すコストを最小化するように、自車両の各時刻の位置及び自車両の各時刻の姿勢を推定することにより、車両の周辺の画像情報と車両が取得した衛星情報とを考慮して、自車両の軌跡及び自車両の姿勢を精度良く推定することができる。

0014

また、本発明は、前記取得手段によって取得された各時刻の前記衛星情報に基づいて、各衛星についてのクロックドリフトを算出するクロックドリフト算出手段と、前記取得手段によって取得された各時刻の前記衛星情報に含まれる、各衛星の各時刻のドップラーシフトと、前記クロックドリフト算出手段によって算出された各衛星についてのクロックドリフト、及び前記移動体の各時刻の姿勢とに基づいて、前記ドップラーシフトから求められる前記移動体と各衛星との相対速度の観測値と、前記クロックドリフト及び前記移動体の各時刻の姿勢から求められる前記移動体と各衛星との相対速度の推定値との差分を表すドップラーコストを算出するドップラーコスト演算手段とを更に含み、前記状態推定手段は、前記差分と、前記ドップラーコスト演算手段によって算出された前記ドップラーコストとを含む前記コストを最小化するように、前記移動体の各時刻の位置及び前記移動体の各時刻の姿勢を推定するようにすることができる。

0015

また、本発明は、ピッチ角算出手段を更に含み、前記取得手段は、前記移動体に搭載された速度センサによって検出された各時刻の速度と、前記移動体に搭載された加速度センサによって検出された各時刻の加速度と、前記移動体に搭載された角速度センサによって検出された各時刻の角速度とを前記移動体情報として更に取得し、前記ピッチ角算出手段は、前記取得手段によって取得された各時刻の前記速度及び各時刻の前記加速度に基づいて、前記移動体の各時刻のピッチ角を算出し、前記ピッチ角算出手段によって算出された前記移動体の各時刻のピッチ角と、前記移動体の姿勢のうちのピッチ角との差分を表すピッチ変化コストを算出するピッチ変化コスト演算手段と、前記取得手段によって取得された各時刻の前記速度から求められる移動体の位置の変化と、前記移動体の位置の変化との差分を表す位置変化コストを算出する位置変化コスト演算手段と、前記取得手段によって取得された各時刻の前記角速度から求められる移動体の方位角の変化と、前記移動体の姿勢のうちの方位角の変化との差分を表す方位変化コストを算出する方位変化コスト演算手段と、前記状態推定手段は、前記差分と、前記ドップラーコスト演算手段によって算出された前記ドップラーコストと、前記ピッチ変化コスト演算手段によって算出された前記ピッチ変化コストと、前記位置変化コスト演算手段によって算出された前記位置変化コストと、前記方位変化コスト演算手段によって算出された前記方位変化コストとを含む前記コストを最小化するように、前記移動体の各時刻の位置及び前記移動体の各時刻の姿勢を推定するようにすることができる。

0016

また、本発明の前記状態推定手段は、予め定められた収束条件を満たすまで、前記移動体の各時刻の位置、及び前記移動体の各時刻の姿勢を推定することを繰り返す
ようにすることができる。

0017

また、本発明の前記取得手段は、複数の前記移動体の各々について、前記移動体情報を取得し、前記状態推定手段は、複数の前記移動体の各々についての前記コストの総和を最小化するように、複数の前記移動体の各々について、前記移動体の各時刻の位置及び前記移動体の各時刻の姿勢を推定するようにすることができる。

0018

また、本発明は、測位手段を更に含み、前記取得手段は、定点観測地点において観測された各時刻の衛星情報及び前記定点観測地点に関する情報を更に取得し、前記測位手段は、複数の前記移動体の各々について、前記取得手段によって取得された前記定点観測地点の各時刻の前記衛星情報及び前記定点観測地点に関する情報に基づいて、前記移動体の各時刻の位置を推定し、前記状態推定手段は、複数の前記移動体の各々についての、前記測位手段によって推定された前記移動体の各時刻の位置と、前記移動体の各時刻の位置との差分を含む前記コストの総和を最小化するように、複数の前記移動体の各々について、前記移動体の各時刻の位置及び前記移動体の姿勢を推定するようにすることができる。

0019

また、本発明の情報端末は、移動体に搭載された情報端末であって、前記移動体に搭載された受信手段によって受信された各時刻の衛星情報、前記移動体に搭載された撮像手段によって撮像された各時刻の前記移動体の周辺の画像、前記移動体に搭載された速度センサによって検出された各時刻の速度、前記移動体に搭載された加速度センサによって検出された各時刻の加速度、及び前記移動体に搭載された角速度センサによって検出された各時刻の角速度の少なくとも1つを前記移動体情報として取得する情報取得手段と、前記情報取得手段によって取得された前記移動体情報を、本発明の状態推定装置へ送信する通信手段と、を含んで構成されている。

0020

また、本発明の情報処理システムは、本発明の状態推定装置と、本発明の情報端末とを含んで構成されている。

0021

なお、本発明のプログラムを記憶する記憶媒体は、特に限定されず、ハードディスクであってもよいし、ROMであってもよい。また、CD−ROMDVDディスク光磁気ディスクICカードであってもよい。更にまた、該プログラムを、ネットワークに接続されたサーバ等からダウンロードするようにしてもよい。

発明の効果

0022

以上説明したように、本発明の状態推定装置、情報端末、情報処理システム、及びプログラムによれば、各時刻の移動体の周辺の画像の各々から抽出された各特徴点を追跡し、追跡された各特徴点の、各時刻の画像の各々における位置と、移動体の各時刻の位置、移動体の各時刻の姿勢、及び追跡された各特徴点の3次元位置から求まる、各特徴点の、各時刻の画像の各々における位置との差分、並びに取得された各時刻の衛星情報と、移動体の各時刻の姿勢との整合性を表すコストを最小化するように、移動体の各時刻の位置及び移動体の各時刻の姿勢を推定することにより、移動体の周辺の画像情報と移動体が取得した衛星情報とを考慮して、移動体の軌跡及び移動体の姿勢を精度良く推定することができる、という効果が得られる。

図面の簡単な説明

0023

本発明の第1の実施の形態に係る状態推定装置を示すブロック図である。
本発明の実施の形態に係る処理の流れを説明するための説明図である。
自車両のピッチ角と自車両の方位角と3次元回転行列との関係を説明するための説明図である。
画像における特徴点と自車両の方位との関係を説明するための説明図である。
画像における特徴点と自車両の方位との関係を説明するための説明図である。
本発明の第1の実施の形態の効果を説明するための説明図である。
本発明の第1の実施の形態における車両状態推定処理ルーチンの内容を示すフローチャートである。
本発明の第1の実施の形態におけるコスト演算処理ルーチンの内容を示すフローチャートである。
本発明の第2の実施の形態の概要を説明するための説明図である。
本発明の第2の実施の形態に係る情報処理システムを示すブロック図である。
本発明の第2の実施の形態に係る車載装置を示すブロック図である。
本発明の第2の実施の形態に係る固定点観測装置を示すブロック図である。
本発明の第2の実施の形態に係る状態推定装置を示すブロック図である。
本発明の第2の実施の形態の効果を説明するための説明図である。
本発明の第2の実施の形態の効果を説明するための説明図である。
本発明の第2の実施の形態における車両状態推定処理ルーチンの内容を示すフローチャートである。
本発明の第2の実施の形態におけるコスト演算処理ルーチンの内容を示すフローチャートである。

実施例

0024

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

0025

<本発明の第1の実施の形態に係る状態推定装置10の構成>

0026

図1に示すように、第1の実施の形態に係る状態推定装置10は、自車両の周辺の画像を逐次撮像するカメラ12と、自車両の加速度を逐次検出する加速度センサ14と、自車両の速度を逐次検出する速度センサ16と、自車両の角速度を逐次検出する角速度センサ18と、各測位衛星から送信される衛星情報を逐次受信するGPS(Global Positioning System)装置19と、自車両の各時刻の位置及姿勢を推定するコンピュータ20とを備えている。第1の実施の形態では、状態推定装置10が車両に搭載される場合について説明する。また、第1の実施の形態では、単独の車両がGPS装置内部センサ、及びカメラによって得られた画像の情報を最適化することで自車両の軌跡及び自車両の各時刻の姿勢を精度良く推定する。

0027

カメラ12は、各時刻の自車両の周辺の画像を撮像する。なお、カメラは1台のみでもよいし、複数台でもよい。また、カメラ12の撮像範囲は、自車両の進行方向を向いていてもよいし、後ろ向きまたは全方位でもよい。カメラ12は、撮像手段の一例である。

0028

加速度センサ14は、各時刻の自車両の加速度を検出する。本実施形態では、自車両の並進加速度を取得する。なお、本実施形態において、最低限必要であるのは車両進行方向への加速度であるが、車両横方向や上下方向の加速度も取得し同時に利用してもよい。

0029

速度センサ16は、各時刻の自車両の速度を検出する。なお、本実施形態では、速度センサ16は、自車両の車輪オドメーターで計測される車輪速を取得する。なお、車輪速以外の車速推定手段(例えば、衛星情報、レーザレーダなどの測距情報、加速度センサなど)を利用してもよい。

0030

角速度センサ18は、各時刻の自車両の角速度を検出する。本実施の形態では、角速度センサ18は、ヨーレイトを角速度として検出する。なお、本実施の形態では、角速度センサ18はヨーレイトを検出するが、ピッチ角速度またはロール角速度を併せて検出してもよい。

0031

GPS装置19は、各時刻について、擬似距離ドップラー周波数、衛星番号、衛星位置、衛星速度等を含む衛星情報を、各測位衛星から受信する。GPS装置19は、受信手段の一例である。

0032

コンピュータ20は、CPU、後述する車両状態推定処理ルーチンを実現するためのプログラムを記憶したROM、データを一時的に記憶するRAM、及びHDD等の記憶装置で構成されている。このコンピュータ20を機能ブロックで表すと、図1に示すように、画像取得部22と、加速度取得部24と、車速取得部26と、角速度取得部28と、衛星情報取得部30と、データ蓄積部32と、特徴点追跡部36と、車両ピッチ角算出部38と、クロックドリフト算出部40と、軌跡姿勢推定部42とを含んだ構成で表すことができる。画像取得部22、加速度取得部24、車速取得部26、角速度取得部28、及び衛星情報取得部30は、取得手段の一例である。また、軌跡姿勢推定部42は状態推定手段の一例である。

0033

画像取得部22は、カメラ12によって撮像された各時刻の自車両の周辺の画像を取得する。

0034

加速度取得部24は、加速度センサ14によって検出された自車両の各時刻の加速度を取得する。

0035

車速取得部26は、速度センサ16によって検出された自車両の各時刻の速度を取得する。

0036

角速度取得部28は、角速度センサ18によって検出された自車両の各時刻の角速度を取得する。なお、最低限必要な角速度はヨーレイトであるが、ピッチレイトロールレイトも取得し同時に利用してもよい。

0037

衛星情報取得部30は、GPS装置19によって受信された各時刻の衛星情報を取得する。衛星情報の内容としては、一般的に使われるローデータ(例えば、疑似距離、ドップラーシフト、GPS時刻、SNR、搬送波位相)と衛星軌道情報対流圏又は電離層補正係数とする。なお、GPSだけでなく、GLONASS(Global Navigation Satellite System)、Beidou(BeiDou Navigation Satellite System)、Gallileo、QZSS(Quasi Zenith Satellites System)などを利用してもよい。

0038

データ蓄積部32には、画像取得部22によって取得された各時刻の画像と、加速度取得部24によって取得された各時刻の加速度と、車速取得部26によって取得された各時刻の速度と、角速度取得部28によって取得された各時刻の角速度と、衛星情報取得部30によって取得された各時刻の衛星情報とが、車両データとして時系列蓄積される。本実施形態では、走行距離が1m増える毎にデータ蓄積を行い、蓄積データが100個を超えた場合は新たなデータを蓄積する毎に最古のデータを破棄する。ただし、蓄積のタイミングは任意のタイムステップ毎データ受信毎でもよいし、蓄積データ数増減してもよい。データ蓄積部32に格納される各データは、移動体情報の一例である。

0039

特徴点追跡部36は、データ蓄積部32に格納された各時刻の画像の各々から特徴点を抽出し、各時刻の画像の各々において抽出された各特徴点を追跡する。特徴点は、画像から抽出される特徴的な画素パターンである。

0040

具体的には、特徴点追跡部36は、まず初めに、データ蓄積部32に格納された最古の画像からN個の特徴点を抽出する。次に、特徴点追跡部36は、データ蓄積部32に格納された各画像について、特徴点を抽出した結果に基づいて、順次新しい時刻の画像において各特徴点を追跡する。

0041

なお、特徴点追跡部36は、追跡の過程で、途中で特徴点の追跡が失敗した場合にはその特徴点はそれ以上追跡せず、その代わりとして当該時刻の画像中で新たに別の特徴点を抽出し追跡処理を行う。結果として特徴点追跡部36による処理によって、時刻tにおける特徴点iの画像上座標を格納した配列x(t,i)が算出される。

0042

特徴点の選び方としては多様な手法が提案されているが、ロバスト性の高いSIFT、実装が容易なHarris、計算負荷が低いFASTなど任意の手法でよい。また、点特徴以外にも、線や面などの特徴量を用いてもよい。特徴点の追跡方法はLucas−Kanadeなど一般的な手法でもよいし、その他の手法でもよい。

0043

車両ピッチ角算出部38は、データ蓄積部32に格納された、自車両の各時刻の速度及び各時刻の加速度に基づいて、自車両の各時刻のピッチ角を算出する。以下の式(1)にピッチ角の算出式を示す。

0044

0045

上記式(1)のφは自車両のピッチ角、Aは加速度取得部24で取得された自車両進行方向への加速度、vは車速取得部26で取得された速度、gは重力加速度、Tは現時刻、Δtは任意のタイムステップを示す。なお、ここではΔtを0.5sとするが、他の値でもよい。

0046

クロックドリフト算出部40は、データ蓄積部32に格納された各時刻の衛星情報に基づいて、各衛星についてのクロックドリフトを算出する。クロックドリフトは、衛星受信機時計誤差変化率を表す。以下の式(2)に、一般的なクロックドリフト算出の式を示す。

0047

0048

上記式(2)のインデックスiは衛星iの観測値であることを示し、Diはドップラーシフト[Hz]、f1は衛星信号周波数[Hz]、cは光速[m/s]、Vは自車両の3次元速度ベクトル[m/s]、viは衛星iの3次元速度ベクトル[m/s]、ρは単位時間あたりの受信機内の時計誤差変化率(クロックドリフト)[s/s]を示す。また、Giは自車から衛星iへの視線方向単位ベクトルであり、自車の概算3次元位置X[m]、衛星iの3次元位置xi[m]を用いて表される。

0049

上記式(2)中、Diは受信機から得られる観測値、f1、cは既知の固定値、vi、xiは衛星の軌道情報を用いて一般的により正確に算出できる既知の値、Xは疑似距離観測値を用いて一般的な方法で算出できる値であるため、未知数は自車の3次元速度ベクトルVとクロックドリフトρの4つとなる。

0050

このため、クロックドリフト算出部40は、衛星が4機以上観測された場合に、上記(2)式の観測方程式を解きクロックドリフトを算出する。

0051

以上では衛星情報のみを用いてクロックドリフトを算出する方法を示したが、衛星情報、ジャイロ、車輪速の時系列データを利用することで衛星受信環境に対するロバスト性を向上させたクロックドリフト算出手法(例えば、特開2013‐113789号公報を参照)を用いてもよい。

0052

軌跡姿勢推定部42は、データ蓄積部32に格納された各時刻の衛星情報と、特徴点追跡部36によって各時刻の画像の各々において抽出された各特徴点の追跡結果と、車両ピッチ角算出部38によって算出された自車両の各時刻のピッチ角と、クロックドリフト算出部40によって算出されたクロックドリフトに基づいて、自車両の各時刻の軌跡及び自車両の各時刻の姿勢を推定する。

0053

軌跡姿勢推定部42は、暫定軌跡姿勢演算部44と、特徴点コスト演算部46と、ピッチ変化コスト演算部48と、位置変化コスト演算部50と、方位変化コスト演算部52と、ドップラーコスト演算部54と、最適軌跡姿勢決定部56とを備えている。

0054

図2に、軌跡姿勢推定部42の処理を説明するための図を示す。軌跡姿勢推定部42では、図2に示すように、暫定軌跡姿勢演算部44によって、自車両の軌跡、自車両の姿勢、特徴点追跡部36によって各時刻の画像の各々において抽出された各特徴点の3次元位置の暫定値が算出される。次に、各コスト演算部によって、算出された暫定値に対するコストを演算される。そして、最適軌跡姿勢決定部56によって、予め定められた収束条件が満たされたか否かが判定され、最適な自車両の軌跡及び自車両の姿勢を出力する。以下、各処理について説明する。

0055

暫定軌跡姿勢演算部44は、収束演算が初回である場合、自車両の各時刻の軌跡、自車両の各時刻の姿勢、及び特徴点追跡部36によって追跡された各特徴点の3次元位置として、適当な値を初期値として設定する。

0056

なお、自車両の姿勢の初期値は、データ蓄積部32に蓄積された方位角及びピッチ角をそのまま利用し、ロール角は0として設定してもよい。また、自車両の軌跡の初期値は、自車両の姿勢の初期値と、データ蓄積部32に蓄積された自車両の速度とに基づき単純に算出してもよい。また、自車両の軌跡及び自車両の姿勢の初期値が得られれば、一般的な三角測量の原理に基づき画像中の特徴点位置実世界中の特徴点位置に変換し初期値として利用することができる。

0057

暫定軌跡姿勢演算部44は、収束演算が2回目以降である場合、後述する特徴点コスト演算部46、ピッチ変化コスト演算部48、位置変化コスト演算部50、方位変化コスト演算部52、及びドップラーコスト演算部54によって演算された各コストに基づいて、自車両の各時刻の軌跡及び自車両の各時刻の姿勢を推定する。収束演算の2回目以降では各コスト演算部の結果をフィードバックする形で出力値が逐次正しい値へ修正される。収束演算が2回目以降である場合の処理については、各コスト演算部の処理を説明した後に説明する。

0058

特徴点コスト演算部46は、特徴点追跡部36によって追跡された各特徴点の位置と、暫定軌跡姿勢演算部44によって得られた自車両の各時刻の位置、自車両の各時刻の姿勢、及び特徴点追跡部36によって追跡された各特徴点の3次元位置から求まる、各特徴点の、各時刻の画像の各々における位置との差分を表す特徴点コストを演算する。

0059

特徴点コスト演算部46で演算される特徴点コストは、画像の観測値(特徴点の位置)と、暫定軌跡姿勢演算部44によって暫定的に算出された自車両の軌跡及び自車両の姿勢との整合性を評価する指標であり、整合性が低いほど、特徴点コストは増加するように設定される。特徴点コストとしては、例えば以下の式(3)に示すようにコスト設定を行うことができる。

0060

0061

上記式(3)において、インデックスtは時刻インデックスを示し、mは特徴点インデックスを示す。また、Xt,m’は時刻tの特徴点mの画像中における2次元位置、Rtは自車両の姿勢の暫定値(3次元回転行列)、Iは3次元単位行列、xtは時刻tの自車両の3次元位置の暫定値、Xmは実世界における特徴点mの3次元位置の暫定値、Kはカメラのキャリブレーション行列(3×3)、Mは推定に利用する特徴点の数、Tは推定に利用する時系列データ数、ecamは各観測値残差、Ecamは残差の総和(コスト)を示す。また、自車両の姿勢のうちの時刻tのピッチ角φtと、自車両の姿勢のうちの時刻tの方位角θtと、自車両の姿勢を表す3次元回転行列Rtとは、以下の式によって表される。

0062

0063

なお、上記式では、添え字tを省略して記載している。また、図3に示すように、X軸まわりの回転(ピッチ角)をφ、Y軸まわりの回転(方位角、ヨー角)をθ、Z軸まわりの回転(ロール角)をψで表す。

0064

なお、画像中の2次元位置座標は画像の中心点又は端点(左下端など)などを原点としピクセル単位で付与するものとする。また、実世界の3次元位置座標は任意の位置(t=0の位置など)を原点とした平面座標とし、[南北方向東西方向,高さ方向]を位置座標の要素とする。

0065

ピッチ変化コスト演算部48は、車両ピッチ角算出部38によって算出された自車両の各時刻のピッチ角と、暫定軌跡姿勢演算部44によって得られた自車両の姿勢のうちのピッチ角との差分を表すピッチ変化コストを算出する。

0066

ピッチ変化コスト演算部48で算出するピッチ変化コストは、車両ピッチ角算出部38で算出された車両ピッチ角と、暫定軌跡姿勢演算部44によって暫定的に算出された姿勢のうちのピッチ角との整合性を評価する指標であり、整合性が低い程コストは増加するように設定される。例えば以下の式(4)に示すようにコスト設定を行うことができる。

0067

0068

φt’は車両ピッチ角算出部38で算出された時刻tにおける車両ピッチ角、φtは暫定軌跡姿勢演算部44で算出された時刻tにおけるピッチ角の暫定値、eΔpitchは各観測値の残差、EΔpitchは残差の総和(コスト)を示す。車両ピッチ角算出部38によって算出された車両ピッチ角と、暫定軌跡姿勢演算部44によって算出された暫定のピッチ角は、それぞれ加速度センサのピッチ角と車体のピッチ角であり常に一定の差を有するため、両者ともt=0からの変化量を比較対象として差分をとる。

0069

位置変化コスト演算部50は、データ蓄積部32に格納された自車両の各時刻の速度から求められる自車両の位置の変化と、暫定軌跡姿勢演算部44によって得られた自車両の位置の変化との差分を表す位置変化コストを算出する。

0070

位置変化コスト演算部50では、データ蓄積部32に蓄積された自車両の速度と、暫定軌跡姿勢演算部44によって算出された自車両の軌跡とに基づき、位置変化コストが算出される。ここで算出する位置変化コストは、データ蓄積部32で蓄積された自車両の速度に基づく位置変化と、暫定軌跡姿勢演算部44によって暫定的に算出された自車両の軌跡に基づく変化量との整合性を評価する指標であり、整合性が低い程コストは増加するように設定される。例えば以下の式(5)に示すようにコスト設定を行うことができる。

0071

0072

Vtはデータ蓄積部32に蓄積された時刻tにおける自車両の速度、xtは暫定軌跡姿勢演算部44によって算出された時刻tにおける自車両の軌跡の暫定値、Δtは蓄積データのタイムステップ、eΔposは各観測値の残差、EΔposは残差の総和(コスト)とする。

0073

方位変化コスト演算部52は、データ蓄積部32に蓄積された各時刻の角速度から求められる自車両の方位角の変化と、暫定軌跡姿勢演算部44によって得られた自車両の姿勢のうちの方位角の変化との差分を表す方位変化コストを算出する。

0074

方位変化コスト演算部52では、データ蓄積部32で蓄積されたヨーレイトと、暫定軌跡姿勢演算部44によって算出された自車両の姿勢のうちの方位角とに基づき、方位変化コストを算出する。ここで算出する方位変化コストは、データ蓄積部32で蓄積されたヨーレイトと、暫定的に算出した自車両の姿勢のうちの方位角の変化量との整合性を評価する指標であり、整合性が低い程コストは増加するように設定する。例えば以下の式(6)に示すようにコスト設定を行うことができる。

0075

0076

ωtはデータ蓄積部32で蓄積された時刻tにおけるヨーレイト、θtは暫定軌跡姿勢演算部44によって算出された時刻tにおける方位角の暫定値、Δtは蓄積データのタイムステップ、eΔheadingは各観測値の残差、EΔheadingは残差の総和(コスト)とする。

0077

図4及び図5に、画像における特徴点と自車両の位置との関係を説明するための図を示す。

0078

図4に示すように、自車両の方位角において推定誤差が発生すると、画像における特徴点の位置と自車両の方位角の誤差とが相互に影響を与え、相乗的に推定誤差が増加していく。その結果、自車両の軌跡の推定誤差も増大する。

0079

そこで、本実施の形態では、図5に示すように、後述するドップラーコスト演算部54で演算されるドップラーコストによって衛星情報を考慮する。衛星情報を考慮することにより、自車両の姿勢のうちの方位角の推定誤差が増加しなくなるため、特徴点の位置の推定誤差も増加しにくくなり、相乗的な推定誤差の低減の効果が得られる。その結果、自車両の軌跡の推定誤差も増加しにくくなる。

0080

ドップラーコスト演算部54は、データ蓄積部32に格納された各時刻の衛星情報と、暫定軌跡姿勢演算部44によって算出された自車両の各時刻の姿勢のうちの方位角との整合性を表すドップラーコストを演算する。

0081

具体的には、ドップラーコスト演算部54は、データ蓄積部32に格納された各時刻の衛星情報に含まれる、各衛星の各時刻のドップラーシフトと、クロックドリフト算出部40によって算出された各衛星についてのクロックドリフト、及び暫定軌跡姿勢演算部44によって算出された自車両の各時刻の姿勢のうちの方位角とに基づいて、ドップラーシフトから求められる自車両と各衛星との相対速度の観測値と、クロックドリフト算出部40によって算出されたクロックドリフト及び自車両の各時刻の姿勢のうちの方位角から求められる自車両と各衛星との相対速度の推定値との差分を表すドップラーコストを算出する。

0082

ドップラーコスト演算部54では、データ蓄積部32で蓄積された衛星情報のドップラーシフトと、暫定軌跡姿勢演算部44によって算出された自車両の姿勢のうちの方位角とに基づき、ドップラーコストを算出する。ここで算出するドップラーコストはデータ蓄積部32で蓄積した衛星情報のドップラーシフトと、暫定軌跡姿勢演算部44によって暫定的に算出された姿勢のうちの方位角との整合性を評価する指標であり、整合性が低い程コストは増加するように設定する。

0083

0084

上記式(7)中のインデックスiは衛星iの観測値であることを示し、Dt,iは時刻tにおける衛星iのドップラーシフト、f1は衛星信号の周波数、cは光速、Vtはデータ蓄積部32に蓄積された自車両の時刻tの速度、vt,iは時刻tにおける衛星iの3次元速度ベクトル[m/s]、ρtはクロックドリフト算出部40で算出された時刻tのクロックドリフト、θtは暫定軌跡姿勢演算部44によって算出された時刻tの自車両の姿勢のうちの方位角の暫定値を示す。Gt,iは時刻tにおける自車両から衛星iへの視線方向単位ベクトルであり、自車両の概算3次元位置Xtと衛星iの3次元位置xt,iを用いて表される。また、Edopplerは残差の総和(コスト)とする。

0085

暫定軌跡姿勢演算部44は、収束演算が2回目以降である場合、特徴点コスト演算部46、ピッチ変化コスト演算部48、位置変化コスト演算部50、方位変化コスト演算部52、及びドップラーコスト演算部54によって演算された各コストに基づいて、自車両の各時刻の軌跡及び自車両の各時刻の姿勢を推定する。

0086

具体的には、暫定軌跡姿勢演算部44は、各コスト演算部の結果に基づいて、自車両の各時刻の位置、自車両の各時刻の姿勢、及び特徴点の3次元位置についての、前回値をからの修正量δpを、以下の式(8)に従い算出する。

0087

0088

そして、暫定軌跡姿勢演算部44は、前回の出力値pと上記で算出した修正量δpとを用いて以下の式(9)に示すように自車両の各時刻の位置、自車両の各時刻の姿勢、及び特徴点の3次元位置を更新する。これにより、各コスト演算部によって演算されたコストを最小化するように、自車両の各時刻の位置及び自車両の各時刻の姿勢が推定される。

0089

0090

なお、本実施形態では、一般的で簡単な最適化手法であるニュートン法の例を示したが、よりロバスト性の高いLevenberg-Marquardt法などを用いてもよい。

0091

最適軌跡姿勢決定部56は、暫定軌跡姿勢演算部44で算出した結果が最適値であるか否かを判断し、最適値であると判断した場合はその値を最適な自車両の軌跡及び自車両の姿勢として出力する。最適値の決定条件は残差の総和であるEtotalの値が一定以下になった場合、またはEtotalの値が更新された時に前回値との差分が一定以下になった場合とする。また、それ以外の条件を設定してもよい。

0092

図6に本実施の形態の効果を説明するための図を示す。図6に示すように、本実施形態では、各コスト演算部によって演算されるコストを最小化するように、自車両の軌跡及び自車両の姿勢が推定されるため、全ての観測値のつじつまが合うような自車両の軌跡及び自車両の姿勢が推定される。また、画像の特徴点と衛星情報から得られる自車両の姿勢とを用いることにより、自車両の軌跡及び自車両の姿勢が精度よく推定される。

0093

<第1の実施の形態に係る状態推定装置10の作用>

0094

次に、第1の実施の形態に係る状態推定装置10の作用について説明する。

0095

各時刻において、GPS装置19が各測位衛星から衛星情報を受信し、各センサが自車両のデータを検出し、各取得部によって各センサによって検出された車両データがデータ蓄積部32に格納されているときに、コンピュータ20において、図7に示す車両状態推定処理ルーチンが実行される。

0096

テップS100において、データ蓄積部32に蓄積された各データを取得する。

0097

ステップS102において、特徴点追跡部36は、上記ステップS100で取得された各時刻の画像の各々から特徴点を抽出する。

0098

ステップS104において、特徴点追跡部36は、上記ステップS102で各時刻の画像の各々から抽出された特徴点に基づいて、各時刻の画像の各々における各特徴点を追跡する。

0099

ステップS106において、車両ピッチ角算出部38は、上記ステップS100で取得された、自車両の各時刻の速度及び各時刻の加速度に基づいて、上記式(1)に従って、自車両の各時刻のピッチ角を算出する。

0100

ステップS108において、クロックドリフト算出部40は、上記ステップS100で取得された各時刻の衛星情報に基づいて、上記式(2)に従って、各衛星についてのクロックドリフトを算出する。

0101

ステップS110において、暫定軌跡姿勢演算部44は、収束演算が初回である場合、自車両の各時刻の軌跡、自車両の各時刻の姿勢、及び特徴点追跡部36によって追跡された各特徴点の3次元位置として、適当な値を初期値として設定する。または、暫定軌跡姿勢演算部44は、収束演算が2回目以降である場合、前回のステップS112で演算された各コストに基づいて、上記式(8)〜(9)に従って、自車両の各時刻の軌跡及び自車両の各時刻の姿勢を推定する。

0102

ステップS112において、各コスト演算部は、ステップS110で得られた自車両の各時刻の軌跡、自車両の各時刻の姿勢、及び特徴点追跡部36によって追跡された各特徴点の3次元位置に基づいて、各コストを演算する。ステップS112は、図8に示すコスト演算処理ルーチンによって実現される。

0103

<コスト演算処理ルーチン>

0104

ステップS200において、特徴点コスト演算部46は、上記式(3)に従って、上記ステップS104で得られた追跡された各特徴点の位置と、上記ステップS110で得られた自車両の各時刻の位置、自車両の各時刻の姿勢、及び上記ステップS104で追跡された各特徴点の3次元位置から求まる、各特徴点の、各時刻の画像の各々における位置との差分を表す特徴点コストを演算する。

0105

ステップS202において、ピッチ変化コスト演算部48は、上記式(4)に従って、上記ステップS106で算出された自車両の各時刻のピッチ角と、上記ステップS110で得られた自車両の姿勢のうちのピッチ角との差分との差分を表すピッチ変化コストを算出する。

0106

ステップS204において、位置変化コスト演算部50は、上記式(5)に従って、上記ステップS100で取得された自車両の各時刻の速度から求められる自車両の位置の変化と、上記ステップS110で得られた自車両の位置の変化との差分を表す位置変化コストを算出する。

0107

ステップS206において、方位変化コスト演算部52は、上記式(6)に従って、上記ステップS100で取得された各時刻の角速度から求められる自車両の方位角の変化と、上記ステップS110で得られた自車両の姿勢のうちの方位角の変化との差分を表す方位変化コストを算出する。

0108

ステップS208において、ドップラーコスト演算部54は、上記式(7)に従って、上記ステップS100で取得された各時刻の衛星情報と、上記ステップS110で得られた自車両の各時刻の姿勢のうちの方位角との整合性を表すドップラーコストを演算する。

0109

ステップS210において、上記ステップS200〜ステップS208で演算された各コストを結果として出力し、コスト演算処理ルーチンを終了する。

0110

次に、図7に示す車両状態推定処理ルーチンのステップS114へ戻り、最適軌跡姿勢決定部56は、上記ステップS110で算出された結果が最適値であるか否かを判断する。上記ステップS110で算出された結果が最適値であると判断された場合には、ステップS116へ進む。一方、上記ステップS110で算出された結果が最適値でないと判断された場合には、ステップS110へ戻る。

0111

ステップS116において、最適軌跡姿勢決定部56は、最適値であると判断した自車両の軌跡及び自車両の各時刻の姿勢を、結果として出力して、車両状態推定処理ルーチンを終了する。

0112

以上説明したように、本発明の実施の形態に係る車両状態推定装置によれば、各時刻の自車両の周辺の画像の各々から特徴点を抽出し、各時刻の画像の各々において各特徴点を追跡し、追跡された各特徴点の、各時刻の画像の各々における位置と、自車両の各時刻の位置、自車両の各時刻の姿勢、及び追跡された各特徴点の3次元位置から求まる、各特徴点の、各時刻の画像の各々における位置との差分、並びに取得された各時刻の衛星情報と、自車両の各時刻の姿勢との整合性を表すコストを最小化するように、自車両の各時刻の位置及び自車両の各時刻の姿勢を推定することにより、車両の周辺の画像情報と車両が取得した衛星情報とを考慮して、自車両の軌跡及び自車両の姿勢を精度良く推定することができる。

0113

また、画像情報以外の情報(ジャイロ、車輪速、GPS)の観測値との乖離度合を、最尤推定収束演算のコスト関数とすることで、誤った局所解への収束を回避できるため、自車両の軌跡及び自車両の姿勢の推定精度を向上させることができる。

0114

<第2の実施の形態>
次に、本発明の第2の実施の形態に係る状態推定装置210について説明する。なお、第1の実施の形態と同様の構成及び同様の処理については、同一の符号を付して説明を省略する。

0115

第2の実施の形態では、状態推定装置210がサーバとして構成され、車載装置の各センサによって取得された情報を受信する点が、第1の実施の形態と異なる。第2の実施の形態では、複数の車両の車載装置が備えたGPS装置、内部センサ、及びカメラによって得られた各種情報及び画像が、外部のデータサーバである状態推定装置210に送信される。そして、状態推定装置210は、車両の車載装置から送信された車両データを蓄積する。そして、状態推定装置210は、図9に示すように、固定観測地点で観測された衛星情報に基づいて、各車両についてRTK(Real Time Kinematic)測位を行い、各車両の測位結果を組み合わせて全データを最適化することで、各車両の軌跡及び各車両の姿勢を精度良く推定する。

0116

<第2の実施の形態に係る情報処理システム100の構成>
図10に示すように、第2の実施の形態に係る情報処理システム100は、複数の車載装置102と、固定点観測装置110と、状態推定装置210とを備えている。複数の車載装置102と固定点観測装置110と状態推定装置210とは、例えばインターネットなどのネットワーク150を介して接続されている。通信手段は、WiFi(登録商標)や携帯電話回線でもよいし、その他の手段でもよい。

0117

<車載装置>
車載装置102は、図11に示すように、自車両の周辺の画像を逐次撮像するカメラ12と、自車両の加速度を逐次検出する加速度センサ14と、自車両の速度を逐次検出する速度センサ16と、自車両の角速度を逐次検出する角速度センサ18と、各測位衛星から送信される衛星情報を逐次受信するGPS装置19と、各センサによって得られた情報を状態推定装置210へ送信するコンピュータ104とを備えている。車載装置102は、情報端末の一例である。

0118

コンピュータ104は、CPU、各処理ルーチンを実現するためのプログラムを記憶したROM、データを一時的に記憶するRAM、及びHDD等の記憶装置で構成されている。このコンピュータ104を機能ブロックで表すと、図11に示すように、画像取得部22と、加速度取得部24と、車速取得部26と、角速度取得部28と、衛星情報取得部30と、車両データ送信部106とを含んだ構成で表すことができる。画像取得部22、加速度取得部24、車速取得部26、角速度取得部28、及び衛星情報取得部30は、情報取得手段の一例である。

0119

車両データ送信部106は、画像取得部22によって取得された各時刻の画像と、加速度取得部24によって取得された各時刻の加速度と、車速取得部26によって取得された各時刻の速度と、角速度取得部28によって取得された各時刻の角速度と、衛星情報取得部30によって取得された各時刻の衛星情報とを、車両データとして状態推定装置210へ逐次送信する。

0120

<固定点観測装置>
固定点観測装置110は、図12に示すように、各測位衛星から送信される衛星情報を逐次受信するGPS装置114と、GPS装置114によって受信された衛星情報と固定点観測地点に関する情報とを状態推定装置210へ送信するコンピュータ112とを備えている。本実施の形態では、固定点観測装置110は、基準局に設置される。固定点観測地点は、定点観測地点の一例である。

0121

コンピュータ112は、CPU、各処理ルーチンを実現するためのプログラムを記憶したROM、データを一時的に記憶するRAM、及びHDD等の記憶装置で構成されている。このコンピュータ112を機能ブロックで表すと、図12に示すように、GPS装置114によって受信された各時刻の衛星情報を取得する衛星情報取得部116と、衛星情報取得部116によって取得された固定点観測地点において観測された衛星情報及び固定点観測地点に関する情報とを状態推定装置210へ逐次送信する固定点観測データ送信部118とを含んだ構成で表すことができる。

0122

衛星情報取得部116は、電子基準点などの固定点観測地点で観測した衛星のローデータ(例えば、疑似距離、ドップラーシフト、GPS時刻、SNR(Signal to Noise Ratio)、及び搬送波位相)を取得する。

0123

固定点観測データ送信部118は、衛星情報取得部116によって取得された衛星情報と固定点観測地点に関する情報とを、固定点観測データとして状態推定装置210へ送信する。また、基準局データはある都市などの地域内の固定点観測地点の観測データのみに限定してもよいし、全国または全世界などより広範囲の固定点観測地点の観測データを送信してもよい。

0124

<状態推定装置>
図13に示すように、第2の実施の形態に係る状態推定装置210は、CPU、後述する各処理ルーチンを実現するためのプログラム等を記憶したROM、データを一時的に記憶するRAM、記憶手段としてのメモリネットワークインタフェース等を含むサーバで構成されており、機能的には、車両データ受信部230と、固定点観測データ受信部231と、データ蓄積部232と、特徴点追跡部36と、車両ピッチ角算出部38と、クロックドリフト算出部40と、高精度測位部241と、軌跡姿勢推定部242とを含んだ構成で表すことができる。なお、第2の実施の形態では、車両データ受信部230、及び固定点観測データ受信部231が、取得手段の一例である。また、軌跡姿勢推定部242は車両状態推定手段の一例である。

0125

車両データ受信部230は、複数の車載装置102によって送信された車両データを受信する。

0126

固定点観測データ受信部231は、固定点観測装置110から送信された固定点観測データを受信する。

0127

データ蓄積部232には、車両データ受信部230によって受信された複数の車両の各々についての車両データと、固定点観測データ受信部231によって受信された固定点観測データとが、時系列に蓄積される。

0128

特徴点追跡部236は、データ蓄積部232に格納された複数の車両データの各々について、当該車両データの各時刻の画像の各々から特徴点を抽出し、各時刻の画像の各々において抽出された各特徴点を追跡する。

0129

車両ピッチ角算出部238は、データ蓄積部232に格納された複数の車両データの各々について、当該車両データの車両の各時刻の速度及び各時刻の加速度に基づいて、上記式(1)に従って、当該車両の各時刻のピッチ角を算出する。

0130

クロックドリフト算出部240は、データ蓄積部232に格納された複数の車両データの各々について、当該車両データの各時刻の衛星情報に基づいて、各衛星についてのクロックドリフトを算出する。

0131

高精度測位部241は、データ蓄積部232に格納された各時刻の固定点観測地点の衛星情報及び固定点観測地点に関する情報に基づいて、複数の車両の各時刻の位置を高精度に推定する。本実施形態では、一般的なRTK(Realtime Kinematic)測位の方法を用いて、複数の車両の各時刻の位置を高精度に推定する。高精度測位部241は、測位手段の一例である。

0132

軌跡姿勢推定部242は、暫定軌跡姿勢演算部244と、特徴点コスト演算部246と、ピッチ変化コスト演算部248と、位置変化コスト演算部250と、方位変化コスト演算部252と、ドップラーコスト演算部254と、位置コスト演算部255と、最適軌跡姿勢決定部56とを備えている。

0133

暫定軌跡姿勢演算部244は、収束演算が初回である場合、自車両の各時刻の軌跡、自車両の各時刻の姿勢、及び特徴点追跡部236によって追跡された各特徴点の3次元位置として、適当な値を初期値として設定する。

0134

暫定軌跡姿勢演算部244は、収束演算が2回目以降である場合、特徴点コスト演算部246、ピッチ変化コスト演算部248、位置変化コスト演算部250、方位変化コスト演算部252、ドップラーコスト演算部254、及び後述する位置コスト演算部255によって演算された各コストに基づいて、各車両の各時刻の軌跡及び各車両の各時刻の姿勢を推定する。収束演算が2回目以降である場合の処理については、各コスト演算部の処理を説明した後に説明する。

0135

特徴点コスト演算部246は、複数の車両の各々について、特徴点追跡部236によって追跡された各特徴点に対し、異なる車両における特徴点同士対応付ける。
そして、特徴点コスト演算部246は、複数の車両の各々について、特徴点追跡部236によって追跡された各特徴点の位置と、暫定軌跡姿勢演算部244によって得られた各車両の各時刻の位置、各車両の各時刻の姿勢、及び特徴点追跡部236によって追跡された各特徴点の3次元位置から求まる、各特徴点の、各時刻の画像の各々における位置との差分を表す特徴点コストを演算する。

0136

ピッチ変化コスト演算部248は、複数の車両の各々について、車両ピッチ角算出部238によって算出された各車両の各時刻のピッチ角と、暫定軌跡姿勢演算部244によって得られた各車両の姿勢のうちのピッチ角との差分との差分を表すピッチ変化コストを算出する。

0137

位置変化コスト演算部250は、複数の車両の各々について、データ蓄積部232に格納された各車両の各時刻の速度から求められる各車両の位置の変化と、暫定軌跡姿勢演算部244によって得られた各車両の位置の変化との差分を表す位置変化コストを算出する。

0138

方位変化コスト演算部252は、複数の車両の各々について、データ蓄積部232に蓄積された各時刻の角速度から求められる各車両の方位角の変化と、暫定軌跡姿勢演算部244によって得られた各車両の姿勢のうちの方位角の変化との差分を表す方位変化コストを算出する。

0139

ドップラーコスト演算部254は、複数の車両の各々について、データ蓄積部232に格納された各時刻の衛星情報と、暫定軌跡姿勢演算部244によって算出された各車両の各時刻の姿勢のうちの方位角との整合性を表すドップラーコストを演算する。

0140

位置コスト演算部255は、データ蓄積部232に格納された複数の車両データに対応する車両の各々について、高精度測位部241によって推定された車両の各時刻の位置から得られる車両の軌跡と、暫定軌跡姿勢演算部244によって算出された車両の軌跡との差分を表す位置コストを算出する。

0141

位置コスト演算部255で算出される位置コストは、複数の車両の各々について、高精度測位部241によって算出された車両の各時刻の位置と、暫定軌跡姿勢演算部244によって暫定的に算出された車両の軌跡との整合性を評価する指標であり、整合性が低い程コストは増加するように設定される。例えば、以下の式(10)に示すようにコスト設定を行うことができる。

0142

0143

xt’は高精度測位部241によって算出された時刻tにおける車両の位置、xtは暫定軌跡姿勢演算部244によって算出された時刻tにおける車両の軌跡の暫定値、eposは各観測値の残差、Eposは残差の総和(コスト)とする。

0144

暫定軌跡姿勢演算部244は、収束演算が2回目以降である場合、特徴点コスト演算部46、ピッチ変化コスト演算部48、位置変化コスト演算部50、方位変化コスト演算部52、ドップラーコスト演算部54、及び位置コスト演算部255によって演算された各コストに基づいて、各車両の各時刻の軌跡及び各車両の各時刻の姿勢を推定する。

0145

具体的には、暫定軌跡姿勢演算部244は、各コスト演算部の結果に基づいて、各車両の各時刻の位置、各車両の各時刻の姿勢、及び特徴点の3次元位置についての、前回値をからの修正量δpを、以下の式(11)に従い算出する。

0146

0147

上記式(11)において、nは車両のインデックス、mは特徴点のインデックス、tは時刻のインデックスを示す。

0148

暫定軌跡姿勢演算部44は、上記式(11)に従って、各コスト演算部によって演算されたコストを最小化するように、各車両の各時刻の位置及び各車両の各時刻の姿勢が推定する。

0149

そして、暫定軌跡姿勢演算部44は、前回の出力値pは上記で算出した修正量δpを用いて以下の式(12)に示すように各車両の各時刻の位置、各車両の各時刻の姿勢、及び特徴点の3次元位置を更新する。これにより、各コスト演算部によって演算されたコストを最小化するように、複数の車両の各々について、車両の各時刻の位置及び車両の各時刻の姿勢が推定される。

0150

0151

なお、本実施形態では、一般的で簡単な最適化手法であるニュートン法の例を示したが、よりロバスト性の高いLevenberg-Marquardt法などを用いてもよい。

0152

図14及び図15に第2の実施の形態の効果を説明するための図を示す。図14に示すように、単独の車両の軌跡を推定する場合、車両の走行と共にすこしずつ軌跡の誤差が累積する。この場合、特徴点の位置にも誤差が生じ、軌跡の補正が困難となる。

0153

一方、図15に示すように、第2の実施の形態では、複数の車両の情報を収集するため、画像における特徴点の位置の誤差は、車両の台数分だけ平均化され、単独の車両の軌跡の推定に比べ、画像における特徴点の位置の誤差は低減される。さらに、第1の実施の形態と同様に、画像における特徴点の位置とつじつまが合うように、各車両の軌跡が最適化されるため、各車両の軌跡の推定誤差も低減される。

0154

<第2の実施の形態に係る状態推定装置210の作用>

0155

次に、第2の実施の形態に係る状態推定装置210の作用について説明する。

0156

各時刻において、各車載装置102が各測位衛星から衛星情報を受信し、各センサが車両のデータを検出し、取得した車両データを状態推定装置210へ送信する。また、各時刻において、固定点観測装置110が各測位衛星から衛星情報を受信し、取得した固定点観測データを状態推定装置210へ送信する。そして、各時刻において、状態推定装置210が各車載装置102から送信された車両データと、固定点観測装置110から送信された固定点観測データとを受信し、車両データと固定点観測データとがデータ蓄積部232に格納されているときに、コンピュータ220において、図16に示す車両状態推定処理ルーチンが実行される。

0157

ステップS300において、データ蓄積部232に蓄積された各車両の車両データと固定点観測データとを取得する。

0158

ステップS302において、特徴点追跡部236は、複数の車両の各々について、上記ステップS300で取得された車両データに含まれる各時刻の画像の各々から特徴点を抽出する。

0159

ステップS304において、特徴点追跡部236は、複数の車両の各々について、上記ステップS302で各時刻の画像の各々から抽出された特徴点に基づいて、各時刻の画像の各々における各特徴点を追跡する。

0160

ステップS306において、車両ピッチ角算出部238は、複数の車両の各々について、上記ステップS300で取得された車両データに含まれる車両の各時刻の速度及び各時刻の加速度に基づいて、上記式(1)に従って、車両の各時刻のピッチ角を算出する。

0161

ステップS308において、クロックドリフト算出部240は、複数の車両の各々について、上記ステップS300で取得された車両データに含まれる各時刻の衛星情報に基づいて、上記式(2)に従って、各衛星についてのクロックドリフトを算出する。

0162

ステップS309において、高精度測位部241は、上記ステップS300で取得された固定点観測データに基づいて、複数の車両の各時刻の位置を高精度に推定する。

0163

ステップS310において、暫定軌跡姿勢演算部44は、収束演算が初回である場合、複数の車両の各々について、車両の各時刻の軌跡、車両の各時刻の姿勢、及び特徴点追跡部36によって追跡された各特徴点の3次元位置として、適当な値を初期値として設定する。または、暫定軌跡姿勢演算部44は、収束演算が2回目以降である場合、複数の車両の各々について、前回のステップS312で演算された各コストに基づいて、上記式(8)〜(9)に従って、車両の各時刻の軌跡及び車両の各時刻の姿勢を推定する。

0164

ステップS312において、各コスト演算部は、複数の車両の各々について、ステップS310で得られた車両の各時刻の軌跡、車両の各時刻の姿勢、及び特徴点追跡部36によって追跡された各特徴点の3次元位置に基づいて、各コストを演算する。ステップS312は、図17に示すコスト演算処理ルーチンによって実現される。

0165

<コスト演算処理ルーチン>

0166

ステップS400において、特徴点コスト演算部246は、複数の車両の各々について、上記ステップS304で得られた各特徴点に対し、異なる車両における特徴点同士を対応付ける。そして、特徴点コスト演算部246は、複数の車両の各々について、上記式(3)に従って、上記ステップS304で得られた追跡された各特徴点の位置と、上記ステップS310で得られた車両の各時刻の位置、車両の各時刻の姿勢、及び上記ステップS304で追跡された各特徴点の3次元位置から求まる、各特徴点の、各時刻の画像の各々における位置との差分を表す特徴点コストを演算する。

0167

ステップS402において、ピッチ変化コスト演算部248は、複数の車両の各々について、上記式(4)に従って、上記ステップS306で算出された車両の各時刻のピッチ角と、上記ステップS310で得られた車両の姿勢のうちのピッチ角との差分との差分を表すピッチ変化コストを算出する。

0168

ステップS404において、位置変化コスト演算部250は、複数の車両の各々について、上記式(5)に従って、上記ステップS300で取得された車両の各時刻の速度から求められる車両の位置の変化と、上記ステップS310で得られた車両の位置の変化との差分を表す位置変化コストを算出する。

0169

ステップS406において、方位変化コスト演算部252は、複数の車両の各々について、上記式(6)に従って、上記ステップS300で取得された各時刻の角速度から求められる車両の方位角の変化と、上記ステップS310で得られた車両の姿勢のうちの方位角の変化との差分を表す方位変化コストを算出する。

0170

ステップS408において、ドップラーコスト演算部254は、複数の車両の各々について、上記式(7)に従って、上記ステップS300で取得された各時刻の衛星情報と、上記ステップS310で得られた車両の各時刻の姿勢のうちの方位角との整合性を表すドップラーコストを演算する。

0171

ステップS409において、位置コスト演算部255は、複数の車両の各々について、上記式(10)に従って、上記ステップS309で推定された車両の各時刻の位置から得られる車両の各時刻の軌跡と、上記ステップS310で得られた車両の軌跡との差分を表す位置コストを算出する。

0172

ステップS410において、上記ステップS400〜ステップS409で演算された各コストを結果として出力し、コスト演算処理ルーチンを終了する。

0173

次に、図16に示す車両状態推定処理ルーチンのステップS114へ戻り、最適軌跡姿勢決定部56は、上記ステップS310で算出された結果が最適値であるか否かを判断する。上記ステップS310で算出された結果が最適値であると判断された場合には、ステップS316へ進む。一方、上記ステップS310で算出された結果が最適値でないと判断された場合には、ステップS310へ戻る。

0174

ステップS316において、最適軌跡姿勢決定部56は、最適値であると判断した車両の軌跡及び車両の各時刻の姿勢を、結果として出力して、車両状態推定処理ルーチンを終了する。

0175

以上説明したように、第2の実施の形態に係る状態推定装置によれば、固定点観測地点の各時刻の衛星情報及び固定点観測地点に関する情報に基づいて、複数の車両の各々について、車両の各時刻の位置を推定し、車両の各時刻の位置から求められる車両の各時刻の軌跡と、車両の軌跡との差分を表す位置コストを算出し、複数の車両の各々についての、位置コストを含むコストを最小化するように、車両の軌跡及び車両の姿勢を推定することにより、複数の車両の各々について、車両の軌跡及び車両の姿勢を精度良く推定することができる。

0176

複数の車両で計測されたセンサ情報(ジャイロ、車輪速、GPS)をサーバである状態推定装置に蓄積し、それら全体を最適化する各車両の軌跡及び各車両の姿勢と特徴点位置を求めることで各車センサ誤差平均化効果が得られるため、複数の車両の各々について、車両の軌跡及び車両の姿勢を精度良く推定することができる。

0177

なお、上記第1の実施の形態では、特徴点コスト演算部46、ピッチ変化コスト演算部48、位置変化コスト演算部50、方位変化コスト演算部52、及びドップラーコスト演算部54によって演算された各コストに基づいて、車両の軌跡及び車両の姿勢を推定する場合を例に説明し、上記第2の実施の形態では、特徴点コスト演算部246、ピッチ変化コスト演算部248、位置変化コスト演算部250、方位変化コスト演算部252、ドップラーコスト演算部254、及び位置コスト演算部255によって演算された各コストに基づいて、車両の軌跡及び車両の姿勢を推定する場合を例に説明したが、これに限定されるものではない。各コストの少なくとも1つを最小化するように、車両の軌跡及び車両の姿勢を推定してもよい。例えば、特徴点コスト演算部によって演算された特徴点コストと、ドップラーコスト演算部によって演算されたドップラーコストとを最小化するように、車両の軌跡及び車両の姿勢を推定してもよい。

0178

また、上記第1の実施の形態では、状態推定装置が車両に搭載される場合について説明したが、これに限定されるものではなく、上記第2の実施の形態のように、状態推定装置をサーバとし、車両の車載装置から送信された車両データに基づいて、車両の軌跡及び姿勢を推定しても良い。

0179

また、上記実施の形態では、車両に搭載される状態推定装置について説明したが、本発明の状態推定装置が搭載される移動体は車両に限定されない。例えば、状態推定装置をロボットに搭載してもよいし、歩行者携帯できるように状態推定装置をポータブル端末として構成するようにしてもよい。

0180

10,210状態推定装置
12カメラ
14加速度センサ
16速度センサ
18角速度センサ
18 速度センサ
19,114GPS装置
20,104,112,220コンピュータ
22画像取得部
24加速度取得部
26車速取得部
28 角速度取得部
30衛星情報取得部
32,232データ蓄積部
36,236特徴点追跡部
38,238車両ピッチ角算出部
40,240クロックドリフト算出部
42,242軌跡姿勢推定部
44,244暫定軌跡姿勢演算部
46,246特徴点コスト演算部
48,248ピッチ変化コスト演算部
50,250位置変化コスト演算部
52,252方位変化コスト演算部
54,254ドップラーコスト演算部
56最適軌跡姿勢決定部
100情報処理システム
102車載装置
106車両データ送信部
110固定点観測装置
116 衛星情報取得部
118 固定点観測データ送信部
150ネットワーク
230 車両データ受信部
231 固定点観測データ受信部
241高精度測位部
255位置コスト演算部

ページトップへ

この技術を出願した法人

この技術を発明した人物

ページトップへ

関連する挑戦したい社会課題

関連する公募課題

ページトップへ

技術視点だけで見ていませんか?

この技術の活用可能性がある分野

分野別動向を把握したい方- 事業化視点で見る -

(分野番号表示ON)※整理標準化データをもとに当社作成

ページトップへ

おススメ サービス

おススメ astavisionコンテンツ

新着 最近 公開された関連が強い技術

この 技術と関連性が強い人物

関連性が強い人物一覧

この 技術と関連する社会課題

関連する挑戦したい社会課題一覧

この 技術と関連する公募課題

関連する公募課題一覧

astavision 新着記事

サイト情報について

本サービスは、国が公開している情報(公開特許公報、特許整理標準化データ等)を元に構成されています。出典元のデータには一部間違いやノイズがあり、情報の正確さについては保証致しかねます。また一時的に、各データの収録範囲や更新周期によって、一部の情報が正しく表示されないことがございます。当サイトの情報を元にした諸問題、不利益等について当方は何ら責任を負いかねることを予めご承知おきのほど宜しくお願い申し上げます。

主たる情報の出典

特許情報…特許整理標準化データ(XML編)、公開特許公報、特許公報、審決公報、Patent Map Guidance System データ