(57)【特許請求の範囲】
【請求項１】
移動体に搭載された受信手段によって受信された各時刻の衛星情報、及び移動体に搭載された撮像手段によって撮像された各時刻の前記移動体の周辺の画像を移動体情報として取得する取得手段と、
前記取得手段によって取得された各時刻の前記画像の各々から特徴点を抽出し、各時刻の前記画像の各々において各特徴点を追跡する特徴点追跡手段と、
前記特徴点追跡手段によって追跡された各特徴点の、各時刻の前記画像の各々における位置と、前記移動体の各時刻の位置、前記移動体の各時刻の姿勢、及び前記特徴点追跡手段によって追跡された各特徴点の３次元位置から求まる、各特徴点の、各時刻の前記画像の各々における位置との差分、並びに前記取得手段によって取得された各時刻の前記衛星情報と、前記移動体の各時刻の姿勢との整合性を表すコストを最小化するように、前記移動体の各時刻の位置及び前記移動体の各時刻の姿勢を推定する状態推定手段と、
を含む状態推定装置。
【請求項２】
前記取得手段によって取得された各時刻の前記衛星情報に基づいて、各衛星についてのクロックドリフトを算出するクロックドリフト算出手段と、
前記取得手段によって取得された各時刻の前記衛星情報に含まれる、各衛星の各時刻のドップラーシフトと、前記クロックドリフト算出手段によって算出された各衛星についてのクロックドリフト、及び前記移動体の各時刻の姿勢とに基づいて、前記ドップラーシフトから求められる前記移動体と各衛星との相対速度の観測値と、前記クロックドリフト及び前記移動体の各時刻の姿勢から求められる前記移動体と各衛星との相対速度の推定値との差分を表すドップラーコストを算出するドップラーコスト演算手段とを更に含み、
前記状態推定手段は、
前記差分と、前記ドップラーコスト演算手段によって算出された前記ドップラーコストとを含む前記コストを最小化するように、前記移動体の各時刻の位置及び前記移動体の各時刻の姿勢を推定する
請求項１に記載の状態推定装置。
【請求項３】
ピッチ角算出手段を更に含み、
前記取得手段は、前記移動体に搭載された速度センサによって検出された各時刻の速度と、前記移動体に搭載された加速度センサによって検出された各時刻の加速度と、前記移動体に搭載された角速度センサによって検出された各時刻の角速度とを前記移動体情報として更に取得し、
前記ピッチ角算出手段は、前記取得手段によって取得された各時刻の前記速度及び各時刻の前記加速度に基づいて、前記移動体の各時刻のピッチ角を算出し、
前記ピッチ角算出手段によって算出された前記移動体の各時刻のピッチ角と、前記移動体の姿勢のうちのピッチ角との差分を表すピッチ変化コストを算出するピッチ変化コスト演算手段と、
前記取得手段によって取得された各時刻の前記速度から求められる移動体の位置の変化と、前記移動体の位置の変化との差分を表す位置変化コストを算出する位置変化コスト演算手段と、
前記取得手段によって取得された各時刻の前記角速度から求められる移動体の方位角の変化と、前記移動体の姿勢のうちの方位角の変化との差分を表す方位変化コストを算出する方位変化コスト演算手段と、
前記状態推定手段は、
前記差分と、前記ドップラーコスト演算手段によって算出された前記ドップラーコストと、前記ピッチ変化コスト演算手段によって算出された前記ピッチ変化コストと、前記位置変化コスト演算手段によって算出された前記位置変化コストと、前記方位変化コスト演算手段によって算出された前記方位変化コストとを含む前記コストを最小化するように、前記移動体の各時刻の位置及び前記移動体の各時刻の姿勢を推定する
請求項２に記載の状態推定装置。
【請求項４】
前記状態推定手段は、
予め定められた収束条件を満たすまで、前記移動体の各時刻の位置、及び前記移動体の各時刻の姿勢を推定することを繰り返す
請求項１〜請求項３の何れか１項に記載の状態推定装置。
【請求項５】
前記取得手段は、複数の前記移動体の各々について、前記移動体情報を取得し、
前記状態推定手段は、複数の前記移動体の各々についての前記コストの総和を最小化するように、複数の前記移動体の各々について、前記移動体の各時刻の位置及び前記移動体の各時刻の姿勢を推定する
請求項１〜請求項４の何れか１項に記載の状態推定装置。
【請求項６】
測位手段を更に含み、
前記取得手段は、定点観測地点において観測された各時刻の衛星情報及び前記定点観測地点に関する情報を更に取得し、
前記測位手段は、複数の前記移動体の各々について、前記取得手段によって取得された前記定点観測地点の各時刻の前記衛星情報及び前記定点観測地点に関する情報に基づいて、前記移動体の各時刻の位置を推定し、
前記状態推定手段は、
複数の前記移動体の各々についての、前記測位手段によって推定された前記移動体の各時刻の位置と、前記移動体の各時刻の位置との差分を含む前記コストの総和を最小化するように、複数の前記移動体の各々について、前記移動体の各時刻の位置及び前記移動体の姿勢を推定する
請求項５に記載の状態推定装置。
【請求項７】
移動体に搭載された情報端末であって、
前記移動体に搭載された受信手段によって受信された各時刻の衛星情報、前記移動体に搭載された撮像手段によって撮像された各時刻の前記移動体の周辺の画像、前記移動体に搭載された速度センサによって検出された各時刻の速度、前記移動体に搭載された加速度センサによって検出された各時刻の加速度、及び前記移動体に搭載された角速度センサによって検出された各時刻の角速度の少なくとも１つを前記移動体情報として取得する情報取得手段と、
前記情報取得手段によって取得された前記移動体情報を、請求項１〜請求項６の何れか１項に記載の状態推定装置へ送信する通信手段と、
を含む情報端末。
【請求項８】
請求項１〜請求項６の何れか１項に記載の状態推定装置と、
請求項７に記載の情報端末とを含む情報処理システム。
【請求項９】
コンピュータを、
移動体に搭載された受信手段によって受信された各時刻の衛星情報、及び移動体に搭載された撮像手段によって撮像された各時刻の前記移動体の周辺の画像を移動体情報として取得する取得手段、
前記取得手段によって取得された各時刻の前記画像の各々から特徴点を抽出し、各時刻の前記画像の各々において各特徴点を追跡する特徴点追跡手段、及び
前記特徴点追跡手段によって追跡された各特徴点の、各時刻の前記画像の各々における位置と、前記移動体の各時刻の位置、前記移動体の各時刻の姿勢、及び前記特徴点追跡手段によって追跡された各特徴点の３次元位置から求まる、各特徴点の、各時刻の前記画像の各々における位置との差分、並びに前記取得手段によって取得された各時刻の前記衛星情報と、前記移動体の各時刻の姿勢との整合性を表すコストを最小化するように、前記移動体の各時刻の位置及び前記移動体の各時刻の姿勢を推定する状態推定手段
として機能させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、状態推定装置、情報端末、情報処理システム、及びプログラムに関する。
【背景技術】
【０００２】
従来、２時刻以上の連続するカメラ画像上で対応し合う特徴点の情報を利用することで自車両の姿勢角（３軸）と並進方向速度（３軸）とをそれぞれ求める技術が知られている。しかし、画像の特徴点の対応に誤りがある場合、推定結果に大きな誤差を生じる可能性がある。
【０００３】
これに対し、特許文献１に記載の従来手法では、まず地磁気センサ・加速度センサなどを用いて姿勢角を既知とすることで、特徴点の誤差の影響を受けにくい状態で並進速度を推定する。次に、並進成分を既知とすることで誤対応を含む特徴点を検出・除外する。そして、最後に残った正しい特徴点を用いて移動体の姿勢及び並進方向速度を再計算することで、特徴点の対応に誤りがある場合でも精度の良い姿勢角および並進方向速度を得る。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２００４−７８３１５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかし、一般的に、複数画像を用いて姿勢・並進速度の推定を行う際には収束演算などを用いて観測方程式を解く必要があるが、もし特徴点の誤対応が少ない場合であったとしても、収束演算の初期値などの影響によって局所解に陥り大きな推定誤差が発生することがある。
【０００６】
従来技術では外部センサを利用することで特徴点の誤対応の影響を軽減しているが、最終的に画像の特徴点情報のみを用いて姿勢・並進成分を推定するため、その際の初期値の設定などにより上記問題が生じ、最終的に大きく誤った推定結果が得られる可能性がある、という問題がある。
【０００７】
本発明は、上記の事情に鑑みてなされたもので、移動体の周辺の画像情報と移動体が取得した衛星情報とを考慮して、移動体の軌跡及び移動体の姿勢を精度良く推定することができる状態推定装置、情報端末、情報処理システム、及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【０００８】
上記目的を達成するために、本発明の状態推定装置は、移動体に搭載された受信手段によって受信された各時刻の衛星情報、及び移動体に搭載された撮像手段によって撮像された各時刻の前記移動体の周辺の画像を移動体情報として取得する取得手段と、前記取得手段によって取得された各時刻の前記画像の各々から特徴点を抽出し、各時刻の前記画像の各々において各特徴点を追跡する特徴点追跡手段と、前記特徴点追跡手段によって追跡された各特徴点の、各時刻の前記画像の各々における位置と、前記移動体の各時刻の位置、前記移動体の各時刻の姿勢、及び前記特徴点追跡手段によって追跡された各特徴点の３次元位置から求まる、各特徴点の、各時刻の前記画像の各々における位置との差分、並びに前記取得手段によって取得された各時刻の前記衛星情報と、前記移動体の各時刻の姿勢との整合性を表すコストを最小化するように、前記移動体の各時刻の位置及び前記移動体の各時刻の姿勢を推定する状態推定手段と、を含んで構成されている。
【０００９】
また、本発明のプログラムは、コンピュータを、移動体に搭載された受信手段によって受信された各時刻の衛星情報、及び移動体に搭載された撮像手段によって撮像された各時刻の前記移動体の周辺の画像を移動体情報として取得する取得手段、前記取得手段によって取得された各時刻の前記画像の各々から特徴点を抽出し、各時刻の前記画像の各々において各特徴点を追跡する特徴点追跡手段、及び前記特徴点追跡手段によって追跡された各特徴点の、各時刻の前記画像の各々における位置と、前記移動体の各時刻の位置、前記移動体の各時刻の姿勢、及び前記特徴点追跡手段によって追跡された各特徴点の３次元位置から求まる、各特徴点の、各時刻の前記画像の各々における位置との差分、並びに前記取得手段によって取得された各時刻の前記衛星情報と、前記移動体の各時刻の姿勢との整合性を表すコストを最小化するように、前記移動体の各時刻の位置及び前記移動体の各時刻の姿勢を推定する状態推定手段として機能させるためのプログラムである。
【００１０】
本発明によれば、取得手段が、移動体に搭載された受信手段によって受信された各時刻の衛星情報、及び移動体に搭載された撮像手段によって撮像された各時刻の移動体の周辺の画像を移動体情報として取得する。
【００１１】
そして、特徴点追跡手段が、取得手段によって取得された各時刻の画像の各々から特徴点を抽出し、各時刻の画像の各々において各特徴点を追跡する。
【００１２】
そして、状態推定手段が、特徴点追跡手段によって追跡された各特徴点の、各時刻の画像の各々における位置と、移動体の各時刻の位置、移動体の各時刻の姿勢、及び特徴点追跡手段によって追跡された各特徴点の３次元位置から求まる、各特徴点の、各時刻の画像の各々における位置との差分、並びに取得手段によって取得された各時刻の衛星情報と、移動体の各時刻の姿勢との整合性を表すコストを最小化するように、移動体の各時刻の位置及び移動体の各時刻の姿勢を推定する。
【００１３】
このように、各時刻の自車両の周辺の画像の各々から特徴点を抽出し、各時刻の画像の各々において各特徴点を追跡し、追跡された各特徴点の、各時刻の画像の各々における位置と、自車両の各時刻の位置、自車両の各時刻の姿勢、及び追跡された各特徴点の３次元位置から求まる、各特徴点の、各時刻の画像の各々における位置との差分、並びに取得された各時刻の衛星情報と、自車両の各時刻の姿勢との整合性を表すコストを最小化するように、自車両の各時刻の位置及び自車両の各時刻の姿勢を推定することにより、車両の周辺の画像情報と車両が取得した衛星情報とを考慮して、自車両の軌跡及び自車両の姿勢を精度良く推定することができる。
【００１４】
また、本発明は、前記取得手段によって取得された各時刻の前記衛星情報に基づいて、各衛星についてのクロックドリフトを算出するクロックドリフト算出手段と、前記取得手段によって取得された各時刻の前記衛星情報に含まれる、各衛星の各時刻のドップラーシフトと、前記クロックドリフト算出手段によって算出された各衛星についてのクロックドリフト、及び前記移動体の各時刻の姿勢とに基づいて、前記ドップラーシフトから求められる前記移動体と各衛星との相対速度の観測値と、前記クロックドリフト及び前記移動体の各時刻の姿勢から求められる前記移動体と各衛星との相対速度の推定値との差分を表すドップラーコストを算出するドップラーコスト演算手段とを更に含み、前記状態推定手段は、前記差分と、前記ドップラーコスト演算手段によって算出された前記ドップラーコストとを含む前記コストを最小化するように、前記移動体の各時刻の位置及び前記移動体の各時刻の姿勢を推定するようにすることができる。
【００１５】
また、本発明は、ピッチ角算出手段を更に含み、前記取得手段は、前記移動体に搭載された速度センサによって検出された各時刻の速度と、前記移動体に搭載された加速度センサによって検出された各時刻の加速度と、前記移動体に搭載された角速度センサによって検出された各時刻の角速度とを前記移動体情報として更に取得し、前記ピッチ角算出手段は、前記取得手段によって取得された各時刻の前記速度及び各時刻の前記加速度に基づいて、前記移動体の各時刻のピッチ角を算出し、前記ピッチ角算出手段によって算出された前記移動体の各時刻のピッチ角と、前記移動体の姿勢のうちのピッチ角との差分を表すピッチ変化コストを算出するピッチ変化コスト演算手段と、前記取得手段によって取得された各時刻の前記速度から求められる移動体の位置の変化と、前記移動体の位置の変化との差分を表す位置変化コストを算出する位置変化コスト演算手段と、前記取得手段によって取得された各時刻の前記角速度から求められる移動体の方位角の変化と、前記移動体の姿勢のうちの方位角の変化との差分を表す方位変化コストを算出する方位変化コスト演算手段と、前記状態推定手段は、前記差分と、前記ドップラーコスト演算手段によって算出された前記ドップラーコストと、前記ピッチ変化コスト演算手段によって算出された前記ピッチ変化コストと、前記位置変化コスト演算手段によって算出された前記位置変化コストと、前記方位変化コスト演算手段によって算出された前記方位変化コストとを含む前記コストを最小化するように、前記移動体の各時刻の位置及び前記移動体の各時刻の姿勢を推定するようにすることができる。
【００１６】
また、本発明の前記状態推定手段は、予め定められた収束条件を満たすまで、前記移動体の各時刻の位置、及び前記移動体の各時刻の姿勢を推定することを繰り返す
ようにすることができる。
【００１７】
また、本発明の前記取得手段は、複数の前記移動体の各々について、前記移動体情報を取得し、前記状態推定手段は、複数の前記移動体の各々についての前記コストの総和を最小化するように、複数の前記移動体の各々について、前記移動体の各時刻の位置及び前記移動体の各時刻の姿勢を推定するようにすることができる。
【００１８】
また、本発明は、測位手段を更に含み、前記取得手段は、定点観測地点において観測された各時刻の衛星情報及び前記定点観測地点に関する情報を更に取得し、前記測位手段は、複数の前記移動体の各々について、前記取得手段によって取得された前記定点観測地点の各時刻の前記衛星情報及び前記定点観測地点に関する情報に基づいて、前記移動体の各時刻の位置を推定し、前記状態推定手段は、複数の前記移動体の各々についての、前記測位手段によって推定された前記移動体の各時刻の位置と、前記移動体の各時刻の位置との差分を含む前記コストの総和を最小化するように、複数の前記移動体の各々について、前記移動体の各時刻の位置及び前記移動体の姿勢を推定するようにすることができる。
【００１９】
また、本発明の情報端末は、移動体に搭載された情報端末であって、前記移動体に搭載された受信手段によって受信された各時刻の衛星情報、前記移動体に搭載された撮像手段によって撮像された各時刻の前記移動体の周辺の画像、前記移動体に搭載された速度センサによって検出された各時刻の速度、前記移動体に搭載された加速度センサによって検出された各時刻の加速度、及び前記移動体に搭載された角速度センサによって検出された各時刻の角速度の少なくとも１つを前記移動体情報として取得する情報取得手段と、前記情報取得手段によって取得された前記移動体情報を、本発明の状態推定装置へ送信する通信手段と、を含んで構成されている。
【００２０】
また、本発明の情報処理システムは、本発明の状態推定装置と、本発明の情報端末とを含んで構成されている。
【００２１】
なお、本発明のプログラムを記憶する記憶媒体は、特に限定されず、ハードディスクであってもよいし、ＲＯＭであってもよい。また、ＣＤ−ＲＯＭやＤＶＤディスク、光磁気ディスクやＩＣカードであってもよい。更にまた、該プログラムを、ネットワークに接続されたサーバ等からダウンロードするようにしてもよい。
【発明の効果】
【００２２】
以上説明したように、本発明の状態推定装置、情報端末、情報処理システム、及びプログラムによれば、各時刻の移動体の周辺の画像の各々から抽出された各特徴点を追跡し、追跡された各特徴点の、各時刻の画像の各々における位置と、移動体の各時刻の位置、移動体の各時刻の姿勢、及び追跡された各特徴点の３次元位置から求まる、各特徴点の、各時刻の画像の各々における位置との差分、並びに取得された各時刻の衛星情報と、移動体の各時刻の姿勢との整合性を表すコストを最小化するように、移動体の各時刻の位置及び移動体の各時刻の姿勢を推定することにより、移動体の周辺の画像情報と移動体が取得した衛星情報とを考慮して、移動体の軌跡及び移動体の姿勢を精度良く推定することができる、という効果が得られる。
【図面の簡単な説明】
【００２３】
【図１】本発明の第１の実施の形態に係る状態推定装置を示すブロック図である。
【図２】本発明の実施の形態に係る処理の流れを説明するための説明図である。
【図３】自車両のピッチ角と自車両の方位角と３次元回転行列との関係を説明するための説明図である。
【図４】画像における特徴点と自車両の方位との関係を説明するための説明図である。
【図５】画像における特徴点と自車両の方位との関係を説明するための説明図である。
【図６】本発明の第１の実施の形態の効果を説明するための説明図である。
【図７】本発明の第１の実施の形態における車両状態推定処理ルーチンの内容を示すフローチャートである。
【図８】本発明の第１の実施の形態におけるコスト演算処理ルーチンの内容を示すフローチャートである。
【図９】本発明の第２の実施の形態の概要を説明するための説明図である。
【図１０】本発明の第２の実施の形態に係る情報処理システムを示すブロック図である。
【図１１】本発明の第２の実施の形態に係る車載装置を示すブロック図である。
【図１２】本発明の第２の実施の形態に係る固定点観測装置を示すブロック図である。
【図１３】本発明の第２の実施の形態に係る状態推定装置を示すブロック図である。
【図１４】本発明の第２の実施の形態の効果を説明するための説明図である。
【図１５】本発明の第２の実施の形態の効果を説明するための説明図である。
【図１６】本発明の第２の実施の形態における車両状態推定処理ルーチンの内容を示すフローチャートである。
【図１７】本発明の第２の実施の形態におけるコスト演算処理ルーチンの内容を示すフローチャートである。
【発明を実施するための形態】
【００２４】
以下、図面を参照して本発明の実施の形態を詳細に説明する。
【００２５】
＜本発明の第１の実施の形態に係る状態推定装置１０の構成＞
【００２６】
図１に示すように、第１の実施の形態に係る状態推定装置１０は、自車両の周辺の画像を逐次撮像するカメラ１２と、自車両の加速度を逐次検出する加速度センサ１４と、自車両の速度を逐次検出する速度センサ１６と、自車両の角速度を逐次検出する角速度センサ１８と、各測位衛星から送信される衛星情報を逐次受信するＧＰＳ（Global Positioning System）装置１９と、自車両の各時刻の位置及姿勢を推定するコンピュータ２０とを備えている。第１の実施の形態では、状態推定装置１０が車両に搭載される場合について説明する。また、第１の実施の形態では、単独の車両がＧＰＳ装置、内部センサ、及びカメラによって得られた画像の情報を最適化することで自車両の軌跡及び自車両の各時刻の姿勢を精度良く推定する。
【００２７】
カメラ１２は、各時刻の自車両の周辺の画像を撮像する。なお、カメラは１台のみでもよいし、複数台でもよい。また、カメラ１２の撮像範囲は、自車両の進行方向を向いていてもよいし、後ろ向きまたは全方位でもよい。カメラ１２は、撮像手段の一例である。
【００２８】
加速度センサ１４は、各時刻の自車両の加速度を検出する。本実施形態では、自車両の並進加速度を取得する。なお、本実施形態において、最低限必要であるのは車両進行方向への加速度であるが、車両横方向や上下方向の加速度も取得し同時に利用してもよい。
【００２９】
速度センサ１６は、各時刻の自車両の速度を検出する。なお、本実施形態では、速度センサ１６は、自車両の車輪のオドメーターで計測される車輪速を取得する。なお、車輪速以外の車速推定手段（例えば、衛星情報、レーザレーダなどの測距情報、加速度センサなど）を利用してもよい。
【００３０】
角速度センサ１８は、各時刻の自車両の角速度を検出する。本実施の形態では、角速度センサ１８は、ヨーレイトを角速度として検出する。なお、本実施の形態では、角速度センサ１８はヨーレイトを検出するが、ピッチ角速度またはロール角速度を併せて検出してもよい。
【００３１】
ＧＰＳ装置１９は、各時刻について、擬似距離、ドップラー周波数、衛星番号、衛星位置、衛星速度等を含む衛星情報を、各測位衛星から受信する。ＧＰＳ装置１９は、受信手段の一例である。
【００３２】
コンピュータ２０は、ＣＰＵ、後述する車両状態推定処理ルーチンを実現するためのプログラムを記憶したＲＯＭ、データを一時的に記憶するＲＡＭ、及びＨＤＤ等の記憶装置で構成されている。このコンピュータ２０を機能ブロックで表すと、図１に示すように、画像取得部２２と、加速度取得部２４と、車速取得部２６と、角速度取得部２８と、衛星情報取得部３０と、データ蓄積部３２と、特徴点追跡部３６と、車両ピッチ角算出部３８と、クロックドリフト算出部４０と、軌跡姿勢推定部４２とを含んだ構成で表すことができる。画像取得部２２、加速度取得部２４、車速取得部２６、角速度取得部２８、及び衛星情報取得部３０は、取得手段の一例である。また、軌跡姿勢推定部４２は状態推定手段の一例である。
【００３３】
画像取得部２２は、カメラ１２によって撮像された各時刻の自車両の周辺の画像を取得する。
【００３４】
加速度取得部２４は、加速度センサ１４によって検出された自車両の各時刻の加速度を取得する。
【００３５】
車速取得部２６は、速度センサ１６によって検出された自車両の各時刻の速度を取得する。
【００３６】
角速度取得部２８は、角速度センサ１８によって検出された自車両の各時刻の角速度を取得する。なお、最低限必要な角速度はヨーレイトであるが、ピッチレイトやロールレイトも取得し同時に利用してもよい。
【００３７】
衛星情報取得部３０は、ＧＰＳ装置１９によって受信された各時刻の衛星情報を取得する。衛星情報の内容としては、一般的に使われるローデータ（例えば、疑似距離、ドップラーシフト、ＧＰＳ時刻、ＳＮＲ、搬送波位相）と衛星軌道情報、対流圏又は電離層補正係数とする。なお、ＧＰＳだけでなく、ＧＬＯＮＡＳＳ（Global Navigation Satellite System）、Ｂｅｉｄｏｕ（BeiDou Navigation Satellite System）、Ｇａｌｌｉｌｅｏ、ＱＺＳＳ（Quasi Zenith Satellites System）などを利用してもよい。
【００３８】
データ蓄積部３２には、画像取得部２２によって取得された各時刻の画像と、加速度取得部２４によって取得された各時刻の加速度と、車速取得部２６によって取得された各時刻の速度と、角速度取得部２８によって取得された各時刻の角速度と、衛星情報取得部３０によって取得された各時刻の衛星情報とが、車両データとして時系列に蓄積される。本実施形態では、走行距離が１ｍ増える毎にデータ蓄積を行い、蓄積データが１００個を超えた場合は新たなデータを蓄積する毎に最古のデータを破棄する。ただし、蓄積のタイミングは任意のタイムステップ毎やデータ受信毎でもよいし、蓄積データ数は増減してもよい。データ蓄積部３２に格納される各データは、移動体情報の一例である。
【００３９】
特徴点追跡部３６は、データ蓄積部３２に格納された各時刻の画像の各々から特徴点を抽出し、各時刻の画像の各々において抽出された各特徴点を追跡する。特徴点は、画像から抽出される特徴的な画素パターンである。
【００４０】
具体的には、特徴点追跡部３６は、まず初めに、データ蓄積部３２に格納された最古の画像からＮ個の特徴点を抽出する。次に、特徴点追跡部３６は、データ蓄積部３２に格納された各画像について、特徴点を抽出した結果に基づいて、順次新しい時刻の画像において各特徴点を追跡する。
【００４１】
なお、特徴点追跡部３６は、追跡の過程で、途中で特徴点の追跡が失敗した場合にはその特徴点はそれ以上追跡せず、その代わりとして当該時刻の画像中で新たに別の特徴点を抽出し追跡処理を行う。結果として特徴点追跡部３６による処理によって、時刻ｔにおける特徴点ｉの画像上座標を格納した配列ｘ（ｔ，ｉ）が算出される。
【００４２】
特徴点の選び方としては多様な手法が提案されているが、ロバスト性の高いＳＩＦＴ、実装が容易なＨａｒｒｉｓ、計算負荷が低いＦＡＳＴなど任意の手法でよい。また、点特徴以外にも、線や面などの特徴量を用いてもよい。特徴点の追跡方法はＬｕｃａｓ−Ｋａｎａｄｅなど一般的な手法でもよいし、その他の手法でもよい。
【００４３】
車両ピッチ角算出部３８は、データ蓄積部３２に格納された、自車両の各時刻の速度及び各時刻の加速度に基づいて、自車両の各時刻のピッチ角を算出する。以下の式（１）にピッチ角の算出式を示す。
【００４４】
【数１】
0006613961.tif 000002
【００４５】
上記式（１）のφは自車両のピッチ角、Ａは加速度取得部２４で取得された自車両進行方向への加速度、ｖは車速取得部２６で取得された速度、ｇは重力加速度、Ｔは現時刻、Δｔは任意のタイムステップを示す。なお、ここではΔｔを０．５ｓとするが、他の値でもよい。
【００４６】
クロックドリフト算出部４０は、データ蓄積部３２に格納された各時刻の衛星情報に基づいて、各衛星についてのクロックドリフトを算出する。クロックドリフトは、衛星受信機の時計誤差変化率を表す。以下の式（２）に、一般的なクロックドリフト算出の式を示す。
【００４７】
【数２】
0006613961.tif 000003
【００４８】
上記式（２）のインデックスｉは衛星ｉの観測値であることを示し、Ｄｉはドップラーシフト［Ｈｚ］、ｆ１は衛星信号の周波数［Ｈｚ］、ｃは光速［ｍ／ｓ］、Ｖは自車両の３次元速度ベクトル［ｍ／ｓ］、ｖｉは衛星ｉの３次元速度ベクトル［ｍ／ｓ］、ρは単位時間あたりの受信機内の時計誤差変化率（クロックドリフト）［ｓ／ｓ］を示す。また、Ｇｉは自車から衛星ｉへの視線方向単位ベクトルであり、自車の概算３次元位置Ｘ［ｍ］、衛星ｉの３次元位置ｘｉ［ｍ］を用いて表される。
【００４９】
上記式（２）中、Ｄｉは受信機から得られる観測値、ｆ１、ｃは既知の固定値、ｖｉ、ｘｉは衛星の軌道情報を用いて一般的により正確に算出できる既知の値、Ｘは疑似距離観測値を用いて一般的な方法で算出できる値であるため、未知数は自車の３次元速度ベクトルＶとクロックドリフトρの４つとなる。
【００５０】
このため、クロックドリフト算出部４０は、衛星が４機以上観測された場合に、上記（２）式の観測方程式を解きクロックドリフトを算出する。
【００５１】
以上では衛星情報のみを用いてクロックドリフトを算出する方法を示したが、衛星情報、ジャイロ、車輪速の時系列データを利用することで衛星受信環境に対するロバスト性を向上させたクロックドリフト算出手法（例えば、特開２０１３‐１１３７８９号公報を参照）を用いてもよい。
【００５２】
軌跡姿勢推定部４２は、データ蓄積部３２に格納された各時刻の衛星情報と、特徴点追跡部３６によって各時刻の画像の各々において抽出された各特徴点の追跡結果と、車両ピッチ角算出部３８によって算出された自車両の各時刻のピッチ角と、クロックドリフト算出部４０によって算出されたクロックドリフトに基づいて、自車両の各時刻の軌跡及び自車両の各時刻の姿勢を推定する。
【００５３】
軌跡姿勢推定部４２は、暫定軌跡姿勢演算部４４と、特徴点コスト演算部４６と、ピッチ変化コスト演算部４８と、位置変化コスト演算部５０と、方位変化コスト演算部５２と、ドップラーコスト演算部５４と、最適軌跡姿勢決定部５６とを備えている。
【００５４】
図２に、軌跡姿勢推定部４２の処理を説明するための図を示す。軌跡姿勢推定部４２では、図２に示すように、暫定軌跡姿勢演算部４４によって、自車両の軌跡、自車両の姿勢、特徴点追跡部３６によって各時刻の画像の各々において抽出された各特徴点の３次元位置の暫定値が算出される。次に、各コスト演算部によって、算出された暫定値に対するコストを演算される。そして、最適軌跡姿勢決定部５６によって、予め定められた収束条件が満たされたか否かが判定され、最適な自車両の軌跡及び自車両の姿勢を出力する。以下、各処理について説明する。
【００５５】
暫定軌跡姿勢演算部４４は、収束演算が初回である場合、自車両の各時刻の軌跡、自車両の各時刻の姿勢、及び特徴点追跡部３６によって追跡された各特徴点の３次元位置として、適当な値を初期値として設定する。
【００５６】
なお、自車両の姿勢の初期値は、データ蓄積部３２に蓄積された方位角及びピッチ角をそのまま利用し、ロール角は０として設定してもよい。また、自車両の軌跡の初期値は、自車両の姿勢の初期値と、データ蓄積部３２に蓄積された自車両の速度とに基づき単純に算出してもよい。また、自車両の軌跡及び自車両の姿勢の初期値が得られれば、一般的な三角測量の原理に基づき画像中の特徴点位置を実世界中の特徴点位置に変換し初期値として利用することができる。
【００５７】
暫定軌跡姿勢演算部４４は、収束演算が２回目以降である場合、後述する特徴点コスト演算部４６、ピッチ変化コスト演算部４８、位置変化コスト演算部５０、方位変化コスト演算部５２、及びドップラーコスト演算部５４によって演算された各コストに基づいて、自車両の各時刻の軌跡及び自車両の各時刻の姿勢を推定する。収束演算の２回目以降では各コスト演算部の結果をフィードバックする形で出力値が逐次正しい値へ修正される。収束演算が２回目以降である場合の処理については、各コスト演算部の処理を説明した後に説明する。
【００５８】
特徴点コスト演算部４６は、特徴点追跡部３６によって追跡された各特徴点の位置と、暫定軌跡姿勢演算部４４によって得られた自車両の各時刻の位置、自車両の各時刻の姿勢、及び特徴点追跡部３６によって追跡された各特徴点の３次元位置から求まる、各特徴点の、各時刻の画像の各々における位置との差分を表す特徴点コストを演算する。
【００５９】
特徴点コスト演算部４６で演算される特徴点コストは、画像の観測値（特徴点の位置）と、暫定軌跡姿勢演算部４４によって暫定的に算出された自車両の軌跡及び自車両の姿勢との整合性を評価する指標であり、整合性が低いほど、特徴点コストは増加するように設定される。特徴点コストとしては、例えば以下の式（３）に示すようにコスト設定を行うことができる。
【００６０】
【数３】
0006613961.tif 000004
【００６１】
上記式（３）において、インデックスｔは時刻インデックスを示し、ｍは特徴点インデックスを示す。また、Ｘｔ，ｍ’は時刻ｔの特徴点ｍの画像中における２次元位置、Ｒｔは自車両の姿勢の暫定値（３次元回転行列）、Ｉは３次元単位行列、ｘｔは時刻ｔの自車両の３次元位置の暫定値、Ｘｍは実世界における特徴点ｍの３次元位置の暫定値、Ｋはカメラのキャリブレーション行列（３×３）、Ｍは推定に利用する特徴点の数、Ｔは推定に利用する時系列データ数、ｅｃａｍは各観測値の残差、Ｅｃａｍは残差の総和（コスト）を示す。また、自車両の姿勢のうちの時刻ｔのピッチ角φｔと、自車両の姿勢のうちの時刻ｔの方位角θｔと、自車両の姿勢を表す３次元回転行列Ｒｔとは、以下の式によって表される。
【００６２】
【数４】
0006613961.tif 000005
【００６３】
なお、上記式では、添え字ｔを省略して記載している。また、図３に示すように、Ｘ軸まわりの回転（ピッチ角）をφ、Ｙ軸まわりの回転（方位角、ヨー角）をθ、Ｚ軸まわりの回転（ロール角）をψで表す。
【００６４】
なお、画像中の２次元位置座標は画像の中心点又は端点（左下端など）などを原点としピクセル単位で付与するものとする。また、実世界の３次元位置座標は任意の位置（ｔ＝０の位置など）を原点とした平面座標とし、［南北方向，東西方向，高さ方向］を位置座標の要素とする。
【００６５】
ピッチ変化コスト演算部４８は、車両ピッチ角算出部３８によって算出された自車両の各時刻のピッチ角と、暫定軌跡姿勢演算部４４によって得られた自車両の姿勢のうちのピッチ角との差分を表すピッチ変化コストを算出する。
【００６６】
ピッチ変化コスト演算部４８で算出するピッチ変化コストは、車両ピッチ角算出部３８で算出された車両ピッチ角と、暫定軌跡姿勢演算部４４によって暫定的に算出された姿勢のうちのピッチ角との整合性を評価する指標であり、整合性が低い程コストは増加するように設定される。例えば以下の式（４）に示すようにコスト設定を行うことができる。
【００６７】
【数５】
0006613961.tif 000006
【００６８】
φｔ’は車両ピッチ角算出部３８で算出された時刻ｔにおける車両ピッチ角、φｔは暫定軌跡姿勢演算部４４で算出された時刻ｔにおけるピッチ角の暫定値、ｅΔｐｉｔｃｈは各観測値の残差、ＥΔｐｉｔｃｈは残差の総和（コスト）を示す。車両ピッチ角算出部３８によって算出された車両ピッチ角と、暫定軌跡姿勢演算部４４によって算出された暫定のピッチ角は、それぞれ加速度センサのピッチ角と車体のピッチ角であり常に一定の差を有するため、両者ともｔ＝０からの変化量を比較対象として差分をとる。
【００６９】
位置変化コスト演算部５０は、データ蓄積部３２に格納された自車両の各時刻の速度から求められる自車両の位置の変化と、暫定軌跡姿勢演算部４４によって得られた自車両の位置の変化との差分を表す位置変化コストを算出する。
【００７０】
位置変化コスト演算部５０では、データ蓄積部３２に蓄積された自車両の速度と、暫定軌跡姿勢演算部４４によって算出された自車両の軌跡とに基づき、位置変化コストが算出される。ここで算出する位置変化コストは、データ蓄積部３２で蓄積された自車両の速度に基づく位置変化と、暫定軌跡姿勢演算部４４によって暫定的に算出された自車両の軌跡に基づく変化量との整合性を評価する指標であり、整合性が低い程コストは増加するように設定される。例えば以下の式（５）に示すようにコスト設定を行うことができる。
【００７１】
【数６】
0006613961.tif 000007
【００７２】
Ｖｔはデータ蓄積部３２に蓄積された時刻ｔにおける自車両の速度、ｘｔは暫定軌跡姿勢演算部４４によって算出された時刻ｔにおける自車両の軌跡の暫定値、Δｔは蓄積データのタイムステップ、ｅΔｐｏｓは各観測値の残差、ＥΔｐｏｓは残差の総和（コスト）とする。
【００７３】
方位変化コスト演算部５２は、データ蓄積部３２に蓄積された各時刻の角速度から求められる自車両の方位角の変化と、暫定軌跡姿勢演算部４４によって得られた自車両の姿勢のうちの方位角の変化との差分を表す方位変化コストを算出する。
【００７４】
方位変化コスト演算部５２では、データ蓄積部３２で蓄積されたヨーレイトと、暫定軌跡姿勢演算部４４によって算出された自車両の姿勢のうちの方位角とに基づき、方位変化コストを算出する。ここで算出する方位変化コストは、データ蓄積部３２で蓄積されたヨーレイトと、暫定的に算出した自車両の姿勢のうちの方位角の変化量との整合性を評価する指標であり、整合性が低い程コストは増加するように設定する。例えば以下の式（６）に示すようにコスト設定を行うことができる。
【００７５】
【数７】
0006613961.tif 000008
【００７６】
ωｔはデータ蓄積部３２で蓄積された時刻ｔにおけるヨーレイト、θｔは暫定軌跡姿勢演算部４４によって算出された時刻ｔにおける方位角の暫定値、Δｔは蓄積データのタイムステップ、ｅΔｈｅａｄｉｎｇは各観測値の残差、ＥΔｈｅａｄｉｎｇは残差の総和（コスト）とする。
【００７７】
図４及び図５に、画像における特徴点と自車両の位置との関係を説明するための図を示す。
【００７８】
図４に示すように、自車両の方位角において推定誤差が発生すると、画像における特徴点の位置と自車両の方位角の誤差とが相互に影響を与え、相乗的に推定誤差が増加していく。その結果、自車両の軌跡の推定誤差も増大する。
【００７９】
そこで、本実施の形態では、図５に示すように、後述するドップラーコスト演算部５４で演算されるドップラーコストによって衛星情報を考慮する。衛星情報を考慮することにより、自車両の姿勢のうちの方位角の推定誤差が増加しなくなるため、特徴点の位置の推定誤差も増加しにくくなり、相乗的な推定誤差の低減の効果が得られる。その結果、自車両の軌跡の推定誤差も増加しにくくなる。
【００８０】
ドップラーコスト演算部５４は、データ蓄積部３２に格納された各時刻の衛星情報と、暫定軌跡姿勢演算部４４によって算出された自車両の各時刻の姿勢のうちの方位角との整合性を表すドップラーコストを演算する。
【００８１】
具体的には、ドップラーコスト演算部５４は、データ蓄積部３２に格納された各時刻の衛星情報に含まれる、各衛星の各時刻のドップラーシフトと、クロックドリフト算出部４０によって算出された各衛星についてのクロックドリフト、及び暫定軌跡姿勢演算部４４によって算出された自車両の各時刻の姿勢のうちの方位角とに基づいて、ドップラーシフトから求められる自車両と各衛星との相対速度の観測値と、クロックドリフト算出部４０によって算出されたクロックドリフト及び自車両の各時刻の姿勢のうちの方位角から求められる自車両と各衛星との相対速度の推定値との差分を表すドップラーコストを算出する。
【００８２】
ドップラーコスト演算部５４では、データ蓄積部３２で蓄積された衛星情報のドップラーシフトと、暫定軌跡姿勢演算部４４によって算出された自車両の姿勢のうちの方位角とに基づき、ドップラーコストを算出する。ここで算出するドップラーコストはデータ蓄積部３２で蓄積した衛星情報のドップラーシフトと、暫定軌跡姿勢演算部４４によって暫定的に算出された姿勢のうちの方位角との整合性を評価する指標であり、整合性が低い程コストは増加するように設定する。
【００８３】
【数８】
0006613961.tif 000009
【００８４】
上記式（７）中のインデックスｉは衛星ｉの観測値であることを示し、Ｄｔ，ｉは時刻ｔにおける衛星ｉのドップラーシフト、ｆ１は衛星信号の周波数、ｃは光速、Ｖｔはデータ蓄積部３２に蓄積された自車両の時刻ｔの速度、ｖｔ，ｉは時刻ｔにおける衛星ｉの３次元速度ベクトル［ｍ／ｓ］、ρｔはクロックドリフト算出部４０で算出された時刻ｔのクロックドリフト、θｔは暫定軌跡姿勢演算部４４によって算出された時刻ｔの自車両の姿勢のうちの方位角の暫定値を示す。Ｇｔ，ｉは時刻ｔにおける自車両から衛星ｉへの視線方向単位ベクトルであり、自車両の概算３次元位置Ｘｔと衛星ｉの３次元位置ｘｔ，ｉを用いて表される。また、Ｅｄｏｐｐｌｅｒは残差の総和（コスト）とする。
【００８５】
暫定軌跡姿勢演算部４４は、収束演算が２回目以降である場合、特徴点コスト演算部４６、ピッチ変化コスト演算部４８、位置変化コスト演算部５０、方位変化コスト演算部５２、及びドップラーコスト演算部５４によって演算された各コストに基づいて、自車両の各時刻の軌跡及び自車両の各時刻の姿勢を推定する。
【００８６】
具体的には、暫定軌跡姿勢演算部４４は、各コスト演算部の結果に基づいて、自車両の各時刻の位置、自車両の各時刻の姿勢、及び特徴点の３次元位置についての、前回値をからの修正量δｐを、以下の式（８）に従い算出する。
【００８７】
【数９】
0006613961.tif 000010
【００８８】
そして、暫定軌跡姿勢演算部４４は、前回の出力値ｐと上記で算出した修正量δｐとを用いて以下の式（９）に示すように自車両の各時刻の位置、自車両の各時刻の姿勢、及び特徴点の３次元位置を更新する。これにより、各コスト演算部によって演算されたコストを最小化するように、自車両の各時刻の位置及び自車両の各時刻の姿勢が推定される。
【００８９】
【数１０】
0006613961.tif 000011
【００９０】
なお、本実施形態では、一般的で簡単な最適化手法であるニュートン法の例を示したが、よりロバスト性の高いLevenberg-Marquardt法などを用いてもよい。
【００９１】
最適軌跡姿勢決定部５６は、暫定軌跡姿勢演算部４４で算出した結果が最適値であるか否かを判断し、最適値であると判断した場合はその値を最適な自車両の軌跡及び自車両の姿勢として出力する。最適値の決定条件は残差の総和であるＥｔｏｔａｌの値が一定以下になった場合、またはＥｔｏｔａｌの値が更新された時に前回値との差分が一定以下になった場合とする。また、それ以外の条件を設定してもよい。
【００９２】
図６に本実施の形態の効果を説明するための図を示す。図６に示すように、本実施形態では、各コスト演算部によって演算されるコストを最小化するように、自車両の軌跡及び自車両の姿勢が推定されるため、全ての観測値のつじつまが合うような自車両の軌跡及び自車両の姿勢が推定される。また、画像の特徴点と衛星情報から得られる自車両の姿勢とを用いることにより、自車両の軌跡及び自車両の姿勢が精度よく推定される。
【００９３】
＜第１の実施の形態に係る状態推定装置１０の作用＞
【００９４】
次に、第１の実施の形態に係る状態推定装置１０の作用について説明する。
【００９５】
各時刻において、ＧＰＳ装置１９が各測位衛星から衛星情報を受信し、各センサが自車両のデータを検出し、各取得部によって各センサによって検出された車両データがデータ蓄積部３２に格納されているときに、コンピュータ２０において、図７に示す車両状態推定処理ルーチンが実行される。
【００９６】
ステップＳ１００において、データ蓄積部３２に蓄積された各データを取得する。
【００９７】
ステップＳ１０２において、特徴点追跡部３６は、上記ステップＳ１００で取得された各時刻の画像の各々から特徴点を抽出する。
【００９８】
ステップＳ１０４において、特徴点追跡部３６は、上記ステップＳ１０２で各時刻の画像の各々から抽出された特徴点に基づいて、各時刻の画像の各々における各特徴点を追跡する。
【００９９】
ステップＳ１０６において、車両ピッチ角算出部３８は、上記ステップＳ１００で取得された、自車両の各時刻の速度及び各時刻の加速度に基づいて、上記式（１）に従って、自車両の各時刻のピッチ角を算出する。
【０１００】
ステップＳ１０８において、クロックドリフト算出部４０は、上記ステップＳ１００で取得された各時刻の衛星情報に基づいて、上記式（２）に従って、各衛星についてのクロックドリフトを算出する。
【０１０１】
ステップＳ１１０において、暫定軌跡姿勢演算部４４は、収束演算が初回である場合、自車両の各時刻の軌跡、自車両の各時刻の姿勢、及び特徴点追跡部３６によって追跡された各特徴点の３次元位置として、適当な値を初期値として設定する。または、暫定軌跡姿勢演算部４４は、収束演算が２回目以降である場合、前回のステップＳ１１２で演算された各コストに基づいて、上記式（８）〜（９）に従って、自車両の各時刻の軌跡及び自車両の各時刻の姿勢を推定する。
【０１０２】
ステップＳ１１２において、各コスト演算部は、ステップＳ１１０で得られた自車両の各時刻の軌跡、自車両の各時刻の姿勢、及び特徴点追跡部３６によって追跡された各特徴点の３次元位置に基づいて、各コストを演算する。ステップＳ１１２は、図８に示すコスト演算処理ルーチンによって実現される。
【０１０３】
＜コスト演算処理ルーチン＞
【０１０４】
ステップＳ２００において、特徴点コスト演算部４６は、上記式（３）に従って、上記ステップＳ１０４で得られた追跡された各特徴点の位置と、上記ステップＳ１１０で得られた自車両の各時刻の位置、自車両の各時刻の姿勢、及び上記ステップＳ１０４で追跡された各特徴点の３次元位置から求まる、各特徴点の、各時刻の画像の各々における位置との差分を表す特徴点コストを演算する。
【０１０５】
ステップＳ２０２において、ピッチ変化コスト演算部４８は、上記式（４）に従って、上記ステップＳ１０６で算出された自車両の各時刻のピッチ角と、上記ステップＳ１１０で得られた自車両の姿勢のうちのピッチ角との差分との差分を表すピッチ変化コストを算出する。
【０１０６】
ステップＳ２０４において、位置変化コスト演算部５０は、上記式（５）に従って、上記ステップＳ１００で取得された自車両の各時刻の速度から求められる自車両の位置の変化と、上記ステップＳ１１０で得られた自車両の位置の変化との差分を表す位置変化コストを算出する。
【０１０７】
ステップＳ２０６において、方位変化コスト演算部５２は、上記式（６）に従って、上記ステップＳ１００で取得された各時刻の角速度から求められる自車両の方位角の変化と、上記ステップＳ１１０で得られた自車両の姿勢のうちの方位角の変化との差分を表す方位変化コストを算出する。
【０１０８】
ステップＳ２０８において、ドップラーコスト演算部５４は、上記式（７）に従って、上記ステップＳ１００で取得された各時刻の衛星情報と、上記ステップＳ１１０で得られた自車両の各時刻の姿勢のうちの方位角との整合性を表すドップラーコストを演算する。
【０１０９】
ステップＳ２１０において、上記ステップＳ２００〜ステップＳ２０８で演算された各コストを結果として出力し、コスト演算処理ルーチンを終了する。
【０１１０】
次に、図７に示す車両状態推定処理ルーチンのステップＳ１１４へ戻り、最適軌跡姿勢決定部５６は、上記ステップＳ１１０で算出された結果が最適値であるか否かを判断する。上記ステップＳ１１０で算出された結果が最適値であると判断された場合には、ステップＳ１１６へ進む。一方、上記ステップＳ１１０で算出された結果が最適値でないと判断された場合には、ステップＳ１１０へ戻る。
【０１１１】
ステップＳ１１６において、最適軌跡姿勢決定部５６は、最適値であると判断した自車両の軌跡及び自車両の各時刻の姿勢を、結果として出力して、車両状態推定処理ルーチンを終了する。
【０１１２】
以上説明したように、本発明の実施の形態に係る車両状態推定装置によれば、各時刻の自車両の周辺の画像の各々から特徴点を抽出し、各時刻の画像の各々において各特徴点を追跡し、追跡された各特徴点の、各時刻の画像の各々における位置と、自車両の各時刻の位置、自車両の各時刻の姿勢、及び追跡された各特徴点の３次元位置から求まる、各特徴点の、各時刻の画像の各々における位置との差分、並びに取得された各時刻の衛星情報と、自車両の各時刻の姿勢との整合性を表すコストを最小化するように、自車両の各時刻の位置及び自車両の各時刻の姿勢を推定することにより、車両の周辺の画像情報と車両が取得した衛星情報とを考慮して、自車両の軌跡及び自車両の姿勢を精度良く推定することができる。
【０１１３】
また、画像情報以外の情報（ジャイロ、車輪速、ＧＰＳ）の観測値との乖離度合を、最尤推定収束演算のコスト関数とすることで、誤った局所解への収束を回避できるため、自車両の軌跡及び自車両の姿勢の推定精度を向上させることができる。
【０１１４】
＜第２の実施の形態＞
次に、本発明の第２の実施の形態に係る状態推定装置２１０について説明する。なお、第１の実施の形態と同様の構成及び同様の処理については、同一の符号を付して説明を省略する。
【０１１５】
第２の実施の形態では、状態推定装置２１０がサーバとして構成され、車載装置の各センサによって取得された情報を受信する点が、第１の実施の形態と異なる。第２の実施の形態では、複数の車両の車載装置が備えたＧＰＳ装置、内部センサ、及びカメラによって得られた各種情報及び画像が、外部のデータサーバである状態推定装置２１０に送信される。そして、状態推定装置２１０は、車両の車載装置から送信された車両データを蓄積する。そして、状態推定装置２１０は、図９に示すように、固定観測地点で観測された衛星情報に基づいて、各車両についてＲＴＫ（Real Time Kinematic）測位を行い、各車両の測位結果を組み合わせて全データを最適化することで、各車両の軌跡及び各車両の姿勢を精度良く推定する。
【０１１６】
＜第２の実施の形態に係る情報処理システム１００の構成＞
図１０に示すように、第２の実施の形態に係る情報処理システム１００は、複数の車載装置１０２と、固定点観測装置１１０と、状態推定装置２１０とを備えている。複数の車載装置１０２と固定点観測装置１１０と状態推定装置２１０とは、例えばインターネットなどのネットワーク１５０を介して接続されている。通信手段は、ＷｉＦｉ（登録商標）や携帯電話の回線でもよいし、その他の手段でもよい。
【０１１７】
＜車載装置＞
車載装置１０２は、図１１に示すように、自車両の周辺の画像を逐次撮像するカメラ１２と、自車両の加速度を逐次検出する加速度センサ１４と、自車両の速度を逐次検出する速度センサ１６と、自車両の角速度を逐次検出する角速度センサ１８と、各測位衛星から送信される衛星情報を逐次受信するＧＰＳ装置１９と、各センサによって得られた情報を状態推定装置２１０へ送信するコンピュータ１０４とを備えている。車載装置１０２は、情報端末の一例である。
【０１１８】
コンピュータ１０４は、ＣＰＵ、各処理ルーチンを実現するためのプログラムを記憶したＲＯＭ、データを一時的に記憶するＲＡＭ、及びＨＤＤ等の記憶装置で構成されている。このコンピュータ１０４を機能ブロックで表すと、図１１に示すように、画像取得部２２と、加速度取得部２４と、車速取得部２６と、角速度取得部２８と、衛星情報取得部３０と、車両データ送信部１０６とを含んだ構成で表すことができる。画像取得部２２、加速度取得部２４、車速取得部２６、角速度取得部２８、及び衛星情報取得部３０は、情報取得手段の一例である。
【０１１９】
車両データ送信部１０６は、画像取得部２２によって取得された各時刻の画像と、加速度取得部２４によって取得された各時刻の加速度と、車速取得部２６によって取得された各時刻の速度と、角速度取得部２８によって取得された各時刻の角速度と、衛星情報取得部３０によって取得された各時刻の衛星情報とを、車両データとして状態推定装置２１０へ逐次送信する。
【０１２０】
＜固定点観測装置＞
固定点観測装置１１０は、図１２に示すように、各測位衛星から送信される衛星情報を逐次受信するＧＰＳ装置１１４と、ＧＰＳ装置１１４によって受信された衛星情報と固定点観測地点に関する情報とを状態推定装置２１０へ送信するコンピュータ１１２とを備えている。本実施の形態では、固定点観測装置１１０は、基準局に設置される。固定点観測地点は、定点観測地点の一例である。
【０１２１】
コンピュータ１１２は、ＣＰＵ、各処理ルーチンを実現するためのプログラムを記憶したＲＯＭ、データを一時的に記憶するＲＡＭ、及びＨＤＤ等の記憶装置で構成されている。このコンピュータ１１２を機能ブロックで表すと、図１２に示すように、ＧＰＳ装置１１４によって受信された各時刻の衛星情報を取得する衛星情報取得部１１６と、衛星情報取得部１１６によって取得された固定点観測地点において観測された衛星情報及び固定点観測地点に関する情報とを状態推定装置２１０へ逐次送信する固定点観測データ送信部１１８とを含んだ構成で表すことができる。
【０１２２】
衛星情報取得部１１６は、電子基準点などの固定点観測地点で観測した衛星のローデータ（例えば、疑似距離、ドップラーシフト、ＧＰＳ時刻、ＳＮＲ（Signal to Noise Ratio）、及び搬送波位相）を取得する。
【０１２３】
固定点観測データ送信部１１８は、衛星情報取得部１１６によって取得された衛星情報と固定点観測地点に関する情報とを、固定点観測データとして状態推定装置２１０へ送信する。また、基準局データはある都市などの地域内の固定点観測地点の観測データのみに限定してもよいし、全国または全世界などより広範囲の固定点観測地点の観測データを送信してもよい。
【０１２４】
＜状態推定装置＞
図１３に示すように、第２の実施の形態に係る状態推定装置２１０は、ＣＰＵ、後述する各処理ルーチンを実現するためのプログラム等を記憶したＲＯＭ、データを一時的に記憶するＲＡＭ、記憶手段としてのメモリ、ネットワークインタフェース等を含むサーバで構成されており、機能的には、車両データ受信部２３０と、固定点観測データ受信部２３１と、データ蓄積部２３２と、特徴点追跡部３６と、車両ピッチ角算出部３８と、クロックドリフト算出部４０と、高精度測位部２４１と、軌跡姿勢推定部２４２とを含んだ構成で表すことができる。なお、第２の実施の形態では、車両データ受信部２３０、及び固定点観測データ受信部２３１が、取得手段の一例である。また、軌跡姿勢推定部２４２は車両状態推定手段の一例である。
【０１２５】
車両データ受信部２３０は、複数の車載装置１０２によって送信された車両データを受信する。
【０１２６】
固定点観測データ受信部２３１は、固定点観測装置１１０から送信された固定点観測データを受信する。
【０１２７】
データ蓄積部２３２には、車両データ受信部２３０によって受信された複数の車両の各々についての車両データと、固定点観測データ受信部２３１によって受信された固定点観測データとが、時系列に蓄積される。
【０１２８】
特徴点追跡部２３６は、データ蓄積部２３２に格納された複数の車両データの各々について、当該車両データの各時刻の画像の各々から特徴点を抽出し、各時刻の画像の各々において抽出された各特徴点を追跡する。
【０１２９】
車両ピッチ角算出部２３８は、データ蓄積部２３２に格納された複数の車両データの各々について、当該車両データの車両の各時刻の速度及び各時刻の加速度に基づいて、上記式（１）に従って、当該車両の各時刻のピッチ角を算出する。
【０１３０】
クロックドリフト算出部２４０は、データ蓄積部２３２に格納された複数の車両データの各々について、当該車両データの各時刻の衛星情報に基づいて、各衛星についてのクロックドリフトを算出する。
【０１３１】
高精度測位部２４１は、データ蓄積部２３２に格納された各時刻の固定点観測地点の衛星情報及び固定点観測地点に関する情報に基づいて、複数の車両の各時刻の位置を高精度に推定する。本実施形態では、一般的なＲＴＫ（Realtime Kinematic）測位の方法を用いて、複数の車両の各時刻の位置を高精度に推定する。高精度測位部２４１は、測位手段の一例である。
【０１３２】
軌跡姿勢推定部２４２は、暫定軌跡姿勢演算部２４４と、特徴点コスト演算部２４６と、ピッチ変化コスト演算部２４８と、位置変化コスト演算部２５０と、方位変化コスト演算部２５２と、ドップラーコスト演算部２５４と、位置コスト演算部２５５と、最適軌跡姿勢決定部５６とを備えている。
【０１３３】
暫定軌跡姿勢演算部２４４は、収束演算が初回である場合、自車両の各時刻の軌跡、自車両の各時刻の姿勢、及び特徴点追跡部２３６によって追跡された各特徴点の３次元位置として、適当な値を初期値として設定する。
【０１３４】
暫定軌跡姿勢演算部２４４は、収束演算が２回目以降である場合、特徴点コスト演算部２４６、ピッチ変化コスト演算部２４８、位置変化コスト演算部２５０、方位変化コスト演算部２５２、ドップラーコスト演算部２５４、及び後述する位置コスト演算部２５５によって演算された各コストに基づいて、各車両の各時刻の軌跡及び各車両の各時刻の姿勢を推定する。収束演算が２回目以降である場合の処理については、各コスト演算部の処理を説明した後に説明する。
【０１３５】
特徴点コスト演算部２４６は、複数の車両の各々について、特徴点追跡部２３６によって追跡された各特徴点に対し、異なる車両における特徴点同士を対応付ける。
そして、特徴点コスト演算部２４６は、複数の車両の各々について、特徴点追跡部２３６によって追跡された各特徴点の位置と、暫定軌跡姿勢演算部２４４によって得られた各車両の各時刻の位置、各車両の各時刻の姿勢、及び特徴点追跡部２３６によって追跡された各特徴点の３次元位置から求まる、各特徴点の、各時刻の画像の各々における位置との差分を表す特徴点コストを演算する。
【０１３６】
ピッチ変化コスト演算部２４８は、複数の車両の各々について、車両ピッチ角算出部２３８によって算出された各車両の各時刻のピッチ角と、暫定軌跡姿勢演算部２４４によって得られた各車両の姿勢のうちのピッチ角との差分との差分を表すピッチ変化コストを算出する。
【０１３７】
位置変化コスト演算部２５０は、複数の車両の各々について、データ蓄積部２３２に格納された各車両の各時刻の速度から求められる各車両の位置の変化と、暫定軌跡姿勢演算部２４４によって得られた各車両の位置の変化との差分を表す位置変化コストを算出する。
【０１３８】
方位変化コスト演算部２５２は、複数の車両の各々について、データ蓄積部２３２に蓄積された各時刻の角速度から求められる各車両の方位角の変化と、暫定軌跡姿勢演算部２４４によって得られた各車両の姿勢のうちの方位角の変化との差分を表す方位変化コストを算出する。
【０１３９】
ドップラーコスト演算部２５４は、複数の車両の各々について、データ蓄積部２３２に格納された各時刻の衛星情報と、暫定軌跡姿勢演算部２４４によって算出された各車両の各時刻の姿勢のうちの方位角との整合性を表すドップラーコストを演算する。
【０１４０】
位置コスト演算部２５５は、データ蓄積部２３２に格納された複数の車両データに対応する車両の各々について、高精度測位部２４１によって推定された車両の各時刻の位置から得られる車両の軌跡と、暫定軌跡姿勢演算部２４４によって算出された車両の軌跡との差分を表す位置コストを算出する。
【０１４１】
位置コスト演算部２５５で算出される位置コストは、複数の車両の各々について、高精度測位部２４１によって算出された車両の各時刻の位置と、暫定軌跡姿勢演算部２４４によって暫定的に算出された車両の軌跡との整合性を評価する指標であり、整合性が低い程コストは増加するように設定される。例えば、以下の式（１０）に示すようにコスト設定を行うことができる。
【０１４２】
【数１１】
0006613961.tif 000012
【０１４３】
ｘｔ’は高精度測位部２４１によって算出された時刻ｔにおける車両の位置、ｘｔは暫定軌跡姿勢演算部２４４によって算出された時刻ｔにおける車両の軌跡の暫定値、ｅｐｏｓは各観測値の残差、Ｅｐｏｓは残差の総和（コスト）とする。
【０１４４】
暫定軌跡姿勢演算部２４４は、収束演算が２回目以降である場合、特徴点コスト演算部４６、ピッチ変化コスト演算部４８、位置変化コスト演算部５０、方位変化コスト演算部５２、ドップラーコスト演算部５４、及び位置コスト演算部２５５によって演算された各コストに基づいて、各車両の各時刻の軌跡及び各車両の各時刻の姿勢を推定する。
【０１４５】
具体的には、暫定軌跡姿勢演算部２４４は、各コスト演算部の結果に基づいて、各車両の各時刻の位置、各車両の各時刻の姿勢、及び特徴点の３次元位置についての、前回値をからの修正量δｐを、以下の式（１１）に従い算出する。
【０１４６】
【数１２】
0006613961.tif 000013
【０１４７】
上記式（１１）において、ｎは車両のインデックス、ｍは特徴点のインデックス、ｔは時刻のインデックスを示す。
【０１４８】
暫定軌跡姿勢演算部４４は、上記式（１１）に従って、各コスト演算部によって演算されたコストを最小化するように、各車両の各時刻の位置及び各車両の各時刻の姿勢が推定する。
【０１４９】
そして、暫定軌跡姿勢演算部４４は、前回の出力値ｐは上記で算出した修正量δｐを用いて以下の式（１２）に示すように各車両の各時刻の位置、各車両の各時刻の姿勢、及び特徴点の３次元位置を更新する。これにより、各コスト演算部によって演算されたコストを最小化するように、複数の車両の各々について、車両の各時刻の位置及び車両の各時刻の姿勢が推定される。
【０１５０】
【数１３】
0006613961.tif 000014
【０１５１】
なお、本実施形態では、一般的で簡単な最適化手法であるニュートン法の例を示したが、よりロバスト性の高いLevenberg-Marquardt法などを用いてもよい。
【０１５２】
図１４及び図１５に第２の実施の形態の効果を説明するための図を示す。図１４に示すように、単独の車両の軌跡を推定する場合、車両の走行と共にすこしずつ軌跡の誤差が累積する。この場合、特徴点の位置にも誤差が生じ、軌跡の補正が困難となる。
【０１５３】
一方、図１５に示すように、第２の実施の形態では、複数の車両の情報を収集するため、画像における特徴点の位置の誤差は、車両の台数分だけ平均化され、単独の車両の軌跡の推定に比べ、画像における特徴点の位置の誤差は低減される。さらに、第１の実施の形態と同様に、画像における特徴点の位置とつじつまが合うように、各車両の軌跡が最適化されるため、各車両の軌跡の推定誤差も低減される。
【０１５４】
＜第２の実施の形態に係る状態推定装置２１０の作用＞
【０１５５】
次に、第２の実施の形態に係る状態推定装置２１０の作用について説明する。
【０１５６】
各時刻において、各車載装置１０２が各測位衛星から衛星情報を受信し、各センサが車両のデータを検出し、取得した車両データを状態推定装置２１０へ送信する。また、各時刻において、固定点観測装置１１０が各測位衛星から衛星情報を受信し、取得した固定点観測データを状態推定装置２１０へ送信する。そして、各時刻において、状態推定装置２１０が各車載装置１０２から送信された車両データと、固定点観測装置１１０から送信された固定点観測データとを受信し、車両データと固定点観測データとがデータ蓄積部２３２に格納されているときに、コンピュータ２２０において、図１６に示す車両状態推定処理ルーチンが実行される。
【０１５７】
ステップＳ３００において、データ蓄積部２３２に蓄積された各車両の車両データと固定点観測データとを取得する。
【０１５８】
ステップＳ３０２において、特徴点追跡部２３６は、複数の車両の各々について、上記ステップＳ３００で取得された車両データに含まれる各時刻の画像の各々から特徴点を抽出する。
【０１５９】
ステップＳ３０４において、特徴点追跡部２３６は、複数の車両の各々について、上記ステップＳ３０２で各時刻の画像の各々から抽出された特徴点に基づいて、各時刻の画像の各々における各特徴点を追跡する。
【０１６０】
ステップＳ３０６において、車両ピッチ角算出部２３８は、複数の車両の各々について、上記ステップＳ３００で取得された車両データに含まれる車両の各時刻の速度及び各時刻の加速度に基づいて、上記式（１）に従って、車両の各時刻のピッチ角を算出する。
【０１６１】
ステップＳ３０８において、クロックドリフト算出部２４０は、複数の車両の各々について、上記ステップＳ３００で取得された車両データに含まれる各時刻の衛星情報に基づいて、上記式（２）に従って、各衛星についてのクロックドリフトを算出する。
【０１６２】
ステップＳ３０９において、高精度測位部２４１は、上記ステップＳ３００で取得された固定点観測データに基づいて、複数の車両の各時刻の位置を高精度に推定する。
【０１６３】
ステップＳ３１０において、暫定軌跡姿勢演算部４４は、収束演算が初回である場合、複数の車両の各々について、車両の各時刻の軌跡、車両の各時刻の姿勢、及び特徴点追跡部３６によって追跡された各特徴点の３次元位置として、適当な値を初期値として設定する。または、暫定軌跡姿勢演算部４４は、収束演算が２回目以降である場合、複数の車両の各々について、前回のステップＳ３１２で演算された各コストに基づいて、上記式（８）〜（９）に従って、車両の各時刻の軌跡及び車両の各時刻の姿勢を推定する。
【０１６４】
ステップＳ３１２において、各コスト演算部は、複数の車両の各々について、ステップＳ３１０で得られた車両の各時刻の軌跡、車両の各時刻の姿勢、及び特徴点追跡部３６によって追跡された各特徴点の３次元位置に基づいて、各コストを演算する。ステップＳ３１２は、図１７に示すコスト演算処理ルーチンによって実現される。
【０１６５】
＜コスト演算処理ルーチン＞
【０１６６】
ステップＳ４００において、特徴点コスト演算部２４６は、複数の車両の各々について、上記ステップＳ３０４で得られた各特徴点に対し、異なる車両における特徴点同士を対応付ける。そして、特徴点コスト演算部２４６は、複数の車両の各々について、上記式（３）に従って、上記ステップＳ３０４で得られた追跡された各特徴点の位置と、上記ステップＳ３１０で得られた車両の各時刻の位置、車両の各時刻の姿勢、及び上記ステップＳ３０４で追跡された各特徴点の３次元位置から求まる、各特徴点の、各時刻の画像の各々における位置との差分を表す特徴点コストを演算する。
【０１６７】
ステップＳ４０２において、ピッチ変化コスト演算部２４８は、複数の車両の各々について、上記式（４）に従って、上記ステップＳ３０６で算出された車両の各時刻のピッチ角と、上記ステップＳ３１０で得られた車両の姿勢のうちのピッチ角との差分との差分を表すピッチ変化コストを算出する。
【０１６８】
ステップＳ４０４において、位置変化コスト演算部２５０は、複数の車両の各々について、上記式（５）に従って、上記ステップＳ３００で取得された車両の各時刻の速度から求められる車両の位置の変化と、上記ステップＳ３１０で得られた車両の位置の変化との差分を表す位置変化コストを算出する。
【０１６９】
ステップＳ４０６において、方位変化コスト演算部２５２は、複数の車両の各々について、上記式（６）に従って、上記ステップＳ３００で取得された各時刻の角速度から求められる車両の方位角の変化と、上記ステップＳ３１０で得られた車両の姿勢のうちの方位角の変化との差分を表す方位変化コストを算出する。
【０１７０】
ステップＳ４０８において、ドップラーコスト演算部２５４は、複数の車両の各々について、上記式（７）に従って、上記ステップＳ３００で取得された各時刻の衛星情報と、上記ステップＳ３１０で得られた車両の各時刻の姿勢のうちの方位角との整合性を表すドップラーコストを演算する。
【０１７１】
ステップＳ４０９において、位置コスト演算部２５５は、複数の車両の各々について、上記式（１０）に従って、上記ステップＳ３０９で推定された車両の各時刻の位置から得られる車両の各時刻の軌跡と、上記ステップＳ３１０で得られた車両の軌跡との差分を表す位置コストを算出する。
【０１７２】
ステップＳ４１０において、上記ステップＳ４００〜ステップＳ４０９で演算された各コストを結果として出力し、コスト演算処理ルーチンを終了する。
【０１７３】
次に、図１６に示す車両状態推定処理ルーチンのステップＳ１１４へ戻り、最適軌跡姿勢決定部５６は、上記ステップＳ３１０で算出された結果が最適値であるか否かを判断する。上記ステップＳ３１０で算出された結果が最適値であると判断された場合には、ステップＳ３１６へ進む。一方、上記ステップＳ３１０で算出された結果が最適値でないと判断された場合には、ステップＳ３１０へ戻る。
【０１７４】
ステップＳ３１６において、最適軌跡姿勢決定部５６は、最適値であると判断した車両の軌跡及び車両の各時刻の姿勢を、結果として出力して、車両状態推定処理ルーチンを終了する。
【０１７５】
以上説明したように、第２の実施の形態に係る状態推定装置によれば、固定点観測地点の各時刻の衛星情報及び固定点観測地点に関する情報に基づいて、複数の車両の各々について、車両の各時刻の位置を推定し、車両の各時刻の位置から求められる車両の各時刻の軌跡と、車両の軌跡との差分を表す位置コストを算出し、複数の車両の各々についての、位置コストを含むコストを最小化するように、車両の軌跡及び車両の姿勢を推定することにより、複数の車両の各々について、車両の軌跡及び車両の姿勢を精度良く推定することができる。
【０１７６】
複数の車両で計測されたセンサ情報（ジャイロ、車輪速、ＧＰＳ）をサーバである状態推定装置に蓄積し、それら全体を最適化する各車両の軌跡及び各車両の姿勢と特徴点位置を求めることで各車センサ誤差の平均化効果が得られるため、複数の車両の各々について、車両の軌跡及び車両の姿勢を精度良く推定することができる。
【０１７７】
なお、上記第１の実施の形態では、特徴点コスト演算部４６、ピッチ変化コスト演算部４８、位置変化コスト演算部５０、方位変化コスト演算部５２、及びドップラーコスト演算部５４によって演算された各コストに基づいて、車両の軌跡及び車両の姿勢を推定する場合を例に説明し、上記第２の実施の形態では、特徴点コスト演算部２４６、ピッチ変化コスト演算部２４８、位置変化コスト演算部２５０、方位変化コスト演算部２５２、ドップラーコスト演算部２５４、及び位置コスト演算部２５５によって演算された各コストに基づいて、車両の軌跡及び車両の姿勢を推定する場合を例に説明したが、これに限定されるものではない。各コストの少なくとも１つを最小化するように、車両の軌跡及び車両の姿勢を推定してもよい。例えば、特徴点コスト演算部によって演算された特徴点コストと、ドップラーコスト演算部によって演算されたドップラーコストとを最小化するように、車両の軌跡及び車両の姿勢を推定してもよい。
【０１７８】
また、上記第１の実施の形態では、状態推定装置が車両に搭載される場合について説明したが、これに限定されるものではなく、上記第２の実施の形態のように、状態推定装置をサーバとし、車両の車載装置から送信された車両データに基づいて、車両の軌跡及び姿勢を推定しても良い。
【０１７９】
また、上記実施の形態では、車両に搭載される状態推定装置について説明したが、本発明の状態推定装置が搭載される移動体は車両に限定されない。例えば、状態推定装置をロボットに搭載してもよいし、歩行者が携帯できるように状態推定装置をポータブル端末として構成するようにしてもよい。
【符号の説明】
【０１８０】
１０，２１０  状態推定装置
１２   カメラ
１４   加速度センサ
１６   速度センサ
１８   角速度センサ
１８   速度センサ
１９，１１４  ＧＰＳ装置
２０，１０４，１１２，２２０  コンピュータ
２２   画像取得部
２４   加速度取得部
２６   車速取得部
２８   角速度取得部
３０   衛星情報取得部
３２，２３２  データ蓄積部
３６，２３６  特徴点追跡部
３８，２３８  車両ピッチ角算出部
４０，２４０  クロックドリフト算出部
４２，２４２  軌跡姿勢推定部
４４，２４４  暫定軌跡姿勢演算部
４６，２４６  特徴点コスト演算部
４８，２４８  ピッチ変化コスト演算部
５０，２５０  位置変化コスト演算部
５２，２５２  方位変化コスト演算部
５４，２５４  ドップラーコスト演算部
５６   最適軌跡姿勢決定部
１００ 情報処理システム
１０２ 車載装置
１０６ 車両データ送信部
１１０ 固定点観測装置
１１６ 衛星情報取得部
１１８ 固定点観測データ送信部
１５０ ネットワーク
２３０ 車両データ受信部
２３１ 固定点観測データ受信部
２４１ 高精度測位部
２５５ 位置コスト演算部
【図１】
0006613961.tif 000015
【図２】
0006613961.tif 000016
【図３】
0006613961.tif 000017
【図４】
0006613961.tif 000018
【図５】
0006613961.tif 000019
【図６】
0006613961.tif 000020
【図７】
0006613961.tif 000021
【図８】
0006613961.tif 000022
【図９】
0006613961.tif 000023
【図１０】
0006613961.tif 000024
【図１１】
0006613961.tif 000025
【図１２】
0006613961.tif 000026
【図１３】
0006613961.tif 000027
【図１４】
0006613961.tif 000028
【図１５】
0006613961.tif 000029
【図１６】
0006613961.tif 000030
【図１７】
0006613961.tif 000031