JP4846784B2 - ビークル走行軌跡計測装置 - Google Patents

Description

本発明は、自動車や船舶等のようなビークル（vehicle）に搭載された移動局と、地上に固定された基準局とを有し、ＧＰＳ（Global Positioning System；汎地球測位システム）から発信されるＧＰＳ信号等に基づいてビークルの走行軌跡を計測するビークル走行軌跡計測装置に関する。

従来では、ＧＰＳ信号等に基づいて自動車の走行軌跡を計測する自動車走行軌跡計測装置に関する技術の一例が開示されている（例えば非特許文献１を参照）。この技術では、自動車の走行中は各種データ（例えばＧＰＳ信号や慣性データ等）を蓄積するのみとし、走行終了後に当該自動車の走行軌跡を求める。
飯沼裕美他：自動車走行軌跡計測装置の開発および実用性確認試験結果、自動車技術会学術講演会前刷集，No.1-99，9931891，p17-20

非特許文献１に記載された自動車走行軌跡計測装置では、走行終了後にしか自動車の走行軌跡を求めることができないという問題点がある。仮に走行中にＧＰＳ信号や慣性データ等に基づいて自動車の走行軌跡を求めようとしても、慣性データの積分誤差等の影響を受けたり、計算量が多くて処理が追いつかない等の理由により、実際の走行軌跡とは異なる走行軌跡になってしまっていた。
本発明はこのような点に鑑みてなしたものであり、実時間で実際の走行軌跡と同等の走行軌跡が得られるビークル走行軌跡計測装置を提供することを目的とする。

（１）課題を解決するための手段（以下では単に「解決手段」と呼ぶ。）１は、自動車や船舶等のようなビークルに搭載された移動局と、地上に固定された基準局とを有し、ＧＰＳ衛星群から発信されるＧＰＳ信号等に基づいてビークルの走行軌跡を計測するビークル走行軌跡計測装置であって、
前記移動局には、前記ＧＰＳ信号に基づいてビークルの位置を測位するビークル位置測位手段と、ビークルの姿勢角や加速度を検出するＩＭＵ（Inertial Measurement Unit；慣性姿勢計測装置）とを備え、
前記基準局には、前記ＧＰＳ信号に基づいて基準局の位置を測位する基準局位置測位手段を備え、
前記移動局で観測されたビークルの位置や姿勢角，加速度を含む第１データと、前記基準局で観測された基準局の位置を含む第２データとを受けて、次に示す式（１）および式（２）からなる運動方程式に基づいて実時間でビークルの走行軌跡を計測する実時間融合処理装置を有することを要旨とする。

式（１）および式（２）に記載した変数や定数は、次の通りである。すなわち、Ｒ_ｇは位置ベクトル（position vector）を表し、上記第１データと第２データの両方のデータを用いてキネマティック測位計算して求める。Ｖは速度ベクトル（velocity vector）を表し、上記位置ベクトルＲ _ｇ と後述する加速度計測定ベクトルＡとから推定して求める。Ａは第１データに含まれる加速度から地球座標へ変換した形で求められる加速度計測定ベクトル（accelerometer measurement vector）を表す。ε_ｄａはドライバの運転の揺らぎによる加速度変化率（fluctuation of acceleration vector{jerk}）を表し、白色雑音をローパスフィルタで有色化して求める。ｗ_ｄａは駆動雑音（system noise）を表し、上記加速度計測定ベクトルＡから加速度のパワースペクトルのＤＣ成分として求める。Ｉは単位行列を表す。ν_ＲｇはＧＰＳ観測データの観測雑音（observation noise）を表し、ビークルの走行状態によらず同一であるものとみなして計算を行う。ν_ａは加速度データの観測雑音を表し、ビークルの走行状態によらず同一であるものとみなして計算を行う。ΔＴはＩＭＵ測定装置２６の離散時間サンプリングタイム間隔時間（例えば0.01秒間）を表す定数値である。αは所定の角周波数（＝１／時定数）を表す定数値である。ｙ _ｔ は観測ベクトルを表し、式（１）および式（２）を用いて求める。
また、上記各変数および定数の定義に用いた変数や関数は、次の通りである。すなわち、Ｑは駆動雑音の共分散を表し、ビークルの走行状態によらず同一であるものとみなして計算を行う。ｑは観測雑音の共分散を表し、ビークルの走行状態によらず同一であるものとみなして計算を行う。δはクロネッカーのデルタである。Ｅは期待値を表す関数である。

第１データには、例えばＧＰＳ信号のデータ（以下では単に「ＧＰＳデータ」と呼ぶ。）や、ビークルの位置，加速度，角速度，姿勢角などが該当する。第２データには、例えばＧＰＳデータなどが該当する。なお、ビークルの位置は第１データおよび第２データの双方に含まれるＧＰＳデータを用いてキネマティック測位計算することで求める。解決手段１では、次の四つの方法を用いて上記の式（１）および式（２）を得る。

（ａ）回転系の状態量である姿勢角はカルマンフィルタで推定せず、ＩＭＵからの姿勢角をそのまま使用することで、状態方程式は並進系のみを扱い、カルマンフィルタの推定の対象とする。
（ｂ）加速度の座標変換をカルマンフィルタの外で計算することにより、状態方程式のシステム行列内の座標変換を除くことができるので、座標系の慣性系（地球座標系）に統一し、かつ状態方程式を線形化する。
（ｃ）位置は測位精度が良好なＧＰＳ測位値で補正するので、センサバイアスの長周期モデルであるセンサバイアスモデルを除く。
（ｄ）加速度データのパワースペクトルのＤＣ成分（直流成分およびそれに近い成分を指す）をドライバの運転の揺らぎとして、システムの駆動源（システム雑音；以下では「駆動雑音」と呼ぶ。）とみなす。

上述した四つの方法によって得られた式（１）および式（２）によれば、状態方程式が線形で極めて簡単な形で表されるので、慣性データの積分誤差等の影響を排除し、計算量が少なくなって処理も迅速に行える。したがって、ビークルの走行中において、実時間で実際の走行軌跡と同等の走行軌跡を得ることができる。

（２）解決手段２は、自動車や船舶等のようなビークルに搭載された移動局と、地上に固定された基準局とを有し、ＧＰＳ衛星群から発信されるＧＰＳ信号等に基づいてビークルの走行軌跡を計測するビークル走行軌跡計測装置であって、
前記移動局には、前記ＧＰＳ信号に基づいてビークルの位置を測位するビークル位置測位手段と、ビークルの姿勢角を検出するＩＭＵとを備え、
前記基準局には、前記ＧＰＳ信号に基づいて基準局の位置を測位する基準局位置測位手段を備え、
前記移動局で観測されたビークルの位置や姿勢角を含む第１データと、前記基準局で観測された基準局の位置を含む第２データとを受けて、次に示す式（３）および式（４）からなる運動方程式に基づいて実時間でビークルの走行軌跡を計測する実時間融合処理装置を有することを要旨とする。

解決手段２によれば、上述した四つの方法を用いて式（３）および式（４）を得る。解決手段１がドライバの運転の揺らぎを加速度変化率（ジャーク）εとして解釈したモデルを採用したのに対して、解決手段２ではドライバの運転の揺らぎを加速度Ａとして解釈したモデルを採用した点で異なる。

（３）解決手段３は、解決手段１に記載したビークル走行軌跡計測装置であって、ＩＭＵで観測して更新されるデータの周波数をビークル位置測位手段で更新されるデータの周波数よりも高くし、
実時間融合処理装置は、前記ビークル位置測位手段で得られるデータの相互間におけるデータを前記ＩＭＵで得られるデータを用いて補間するカルマンフィルタを有することを要旨とする。

解決手段３によれば、ビークル位置測位手段の更新周波数が低く、これのみでは滑らかでない走行軌跡であって、さらにＩＭＵとビークル位置測位手段とで処理する周波数が異なっていても、カルマンフィルタがＩＭＵで得られる高い更新周波数のデータを用いて補間を行うので、滑らかな走行軌跡を得ることができる。

（４）解決手段４は、解決手段３に記載したビークル走行軌跡計測システムであって、カルマンフィルタは、第１データおよび第２データの双方に含まれるＧＰＳデータを用いてキネマティック測位計算して得られたビークルの位置と、ＩＭＵによって得られたビークルの加速度とを用いて、ビークルの横滑角を推定するように構成したことを要旨とする。

解決手段４によれば、カルマンフィルタはビークルの横滑角を推定するので、走行軌跡だけでなく、走行中の状態（ビークルの横滑角）をきめ細かく得ることができる。

本発明によれば、慣性データの積分誤差等の影響を排除し、計算量が少なくなって処理も迅速に行うので、ビークルの走行中において実時間で実際の走行軌跡と同等の走行軌跡を得ることができる。

本発明の構成例を模式的に表す図である。 第１ドライバモデルを模式的に表す図である。 第２ドライバモデルを模式的に表す図である。 カルマンフィルタの構成を模式的に表す図である。 カルマンフィルタの時間更新および観測更新を説明する図である。 観測雑音を有色化したシステムモデルを表す図である。 観測雑音を有色化したカルマンフィルタを表す図である。 走行軌跡の一例を表す図である。 走行軌跡の一例を表す図である。 走行中の速度を（Ａ）に表し、横滑角を（Ｂ）に表した図である。

符号の説明

１０ ＧＰＳ衛星群
２０ 移動局
２２，３２ ＧＰＳ受信部
２４，３４，４２ データ処理部
２６ ＩＭＵ測定装置
２８ 操作端末
３０ 基準局
４０ 実時間融合処理装置
５０ ローパスフィルタ
５２ 積分器
Ｄ１ 第１データ
Ｄ２ 第２データ
Ｆ_ｔ 遷移行列
Ｈ_ｔ 観測行列
Ｋ_ｔ カルマンゲイン
Ｐ 推定誤差共分散行列
Ｑ 駆動雑音の共分散
Ｒ_ｇ 位置ベクトル
Ｖ 速度
Ｗ_ｔ 駆動雑音
Ａ 加速度
ｕ 横速度
ｖ 縦速度
β 横滑角
ν 観測雑音行列
ν_Ｒｇ ＧＰＳ観測データの観測雑音
ν_ａ 加速度データの観測雑音

本発明を実施するための最良の形態について、図１〜図１０を参照しながら説明する。

図１には、ビークル走行軌跡計測装置の構成例を模式図で表す。ビークル走行軌跡計測装置は、移動局２０や、基準局３０、実時間融合処理装置４０などを有する。移動局２０は自動車や船舶，飛行機等のようなビークル（すなわち移動体）に搭載されるのに対し、基準局３０は地上に固定される。

移動局２０は、ＧＰＳ受信部２２や、データ処理部２４、ＩＭＵ測定装置２６、操作端末２８などを有する。ＧＰＳ受信部２２は、ＧＰＳ衛星群１０（すなわち４つ以上のＧＰＳ衛星）からそれぞれ発信されるＧＰＳ信号を受信する。ＩＭＵ測定装置２６は、ビークルの姿勢角や加速度等を検出する。データ処理部２４はビークル位置測位手段に相当し、ＧＰＳ受信部２２によって受信されたＧＰＳ信号を含めてＧＰＳデータと時系列同期を取ってデータ処理部４２に送信する。操作端末２８は、データ処理部２４でビークルの位置を測位するに当たって必要な所定値を入力する。

基準局３０は、ＧＰＳ受信部３２やデータ処理部３４などを有する。ＧＰＳ受信部３２は、上述したＧＰＳ受信部２２と同様にして、ＧＰＳ衛星群１０からそれぞれ発信されるＧＰＳ信号を受信する。データ処理部３４は基準局位置測位手段に相当し、ＧＰＳ受信部３２によって受信されたＧＰＳ信号に基づいて基準局３０の位置を測位する。

実時間融合処理装置４０はデータ処理部４２などを有する。このデータ処理部４２は、移動局２０で観測されたビークルの位置や姿勢角，加速度を含む第１データＤ１と、基準局３０で観測された基準局３０の位置を含む第２データＤ２とを受けて、実時間でビークルの走行軌跡を計測する。第１データＤ１には、例えばＧＰＳデータや、ビークルの位置，加速度（Ａ），角速度（ω），姿勢角（ｗ）などが該当する。第２データＤ２には、例えばＧＰＳデータなどが該当する。本例ではデータ処理部２４，３４に送信器を備え、データ処理部４２に受信器を備えている。すなわちデータ処理部４２は、データ処理部２４から第１データＤ１を受け、データ処理部３４から第２データＤ２を受けてビークルの走行軌跡を計測する。なお、ビークルの現在位置（後述する位置ベクトルＲ_ｇに相当する）は、第１データＤ１および第２データＤ２の双方に含まれるＧＰＳデータを用いてキネマティック測位計算して求める。

次に、データ処理部４２でビークルの走行軌跡を計測するにあたっての計算式（運動方程式）について説明する。まず、ドライバの運転の揺らぎを加速度変化率（ジャーク）εとして解釈したドライバモデル（以下では「第１ドライバモデル」と呼ぶ。）を用いる。「ドライバの運転の揺らぎ」は、運転という不確定性のある駆動雑音を意味する。
第１ドライバモデルは、駆動雑音Ｗ_ｔを入力とし、加速度Ａを出力とするモデルであって図２に示すようなブロック図になる。このモデルでは、白色雑音を駆動雑音Ｗ_ｔとして、ローパスフィルタ５０（整形フィルタ）を通過させることで有色化したドライバの運転の揺らぎε_ｄａをさらに、積分器５２で積分して加速度Ａの揺らぎとする。

また、簡略化を図るための四つの方法を以下に示す。
（ａ）回転系の状態量である姿勢角はカルマンフィルタで推定せず、ＩＭＵからの姿勢角をそのまま使用する。こうすることで従来の状態方程式に含まれていた回転系の状態変数[ｗ，ω，ε_ｄω]を削除できるので、状態方程式は並進系のみを扱うことができ、カルマンフィルタの推定の対象とする。
（ｂ）加速度の座標変換をカルマンフィルタの外で計算することにより、状態方程式のシステム行列内の座標変換行列を除く。こうすることで座標系の慣性系（地球座標系）に統一し、かつ状態方程式を線形化する。
（ｃ）位置は測位精度が良好なＧＰＳ測位値で補正する。こうすることで、センサバイアスの長周期モデルであるセンサバイアスモデルを除く。
（ｄ）加速度データのパワースペクトルのＤＣ成分（直流成分およびそれに近い成分を指す）をドライバの運転の揺らぎとして、駆動雑音とみなす。すなわち従来では何らかの駆動雑音源としていたε_ｄａを「ドライバの運転の揺らぎ」として定義した。

従来におけるビークルの運動を並進系と回転系とを併せて記述した運動方程式（以下では「状態方程式」と呼ぶ。）に対して、上述した第１ドライバモデルおよび４つの簡略法を適用すると、次のような式（１）および式（２）になる。

式（１）を集約した行列表現にすると、次の（ｉ）式になる。同様にして式（２）を集約した行列表現にすると、次の（ii）式になる。また、式中のｙ_ｔは観測ベクトル（observation vector）である。なお、以下では便宜上括弧内の表記を用いて説明する。

次に、上述した第１ドライバモデルとは別個に、ドライバの運転の揺らぎを加速度Ａとして解釈したモデル（以下では「第２ドライバモデル」と呼ぶ。）を用いる。第２ドライバモデルは、駆動雑音Ｗ_ｔを入力とし、加速度Ａを出力とするモデルであって図３に示すようなブロック図になる。このモデルでは、白色雑音を駆動雑音Ｗ_ｔとして、ローパスフィルタ５０（整形フィルタ）を通過させることで有色化した加速度Ａの揺らぎとする。

第１ドライバモデルの場合と同様にして、従来の状態方程式に対して第２ドライバモデルおよび４つの簡略法を適用すると、次のような式（３）および式（４）になる。

通常の離散時間形のカルマンフィルタにおいては、フィルタの状態更新レート（時間更新周波数）と観測値を反映するレート（観測周波数）が同じである。本発明では、これらのレート（すなわち周波数）が異なるカルマンフィルタ、具体的には時間更新周波数が観測周波数よりも高くなるように構成したカルマンフィルタを用いた。このカルマンフィルタをブロック図で表すと図４のようになる。

図４に記載した変数や定数のうち、遷移行列Ｆ_ｔと観測行列Ｈ_ｔについて上述した通りである。Ｘは状態ベクトル（state vector）を表すが、推定値であることを明示するために「＾」を冠している。ｚ^−１は前回のサンプリングで得られた離散系伝達特性を表す。カルマンゲインＫ_ｔは次に示す式（５）のように、Ｐ（推定誤差共分散），Ｈ_ｔ（観測行列）およびＲ_ｔ（観測誤差共分散）を用いて導出される。初期条件や、フィルタ方程式、推定誤差共分散などについては次の通りである。

図４に表したカルマンフィルタにおいて、例えば観測更新ループＬ１における観測周波数を５[Hz]に設定し、時間更新ループＬ２における時間更新周波数を１００[Hz]に設定した場合には、図５のようにしてデータの更新を行う。

〔観測周波数が５[Hz]のアルゴリズム〕
ｉ）観測更新については、ＧＰＳデータを用いてキネマティック測位計算して求めた位置ベクトルＲ_ｇの観測値と、ＩＭＵ測定装置２６によって検出されたビークルの加速度Ａの観測値とを入力し、前回までの推定状態量（加速度Ａ，ドライバの運転の揺らぎε_ｄａ，位置ベクトルＲ_ｇの各推定値）を観測更新し、推定状態量の推定誤差共分散行列Ｐを観測更新する。
ii）時間更新については、観測更新した上記推定状態量に遷移行列Ｆ_ｔを掛けて時間更新するとともに、観測更新した推定誤差共分散行列Ｐに遷移行列Ｆ_ｔを掛けて駆動雑音の共分散Ｑを加える。

〔時間更新周波数が１００[Hz]のアルゴリズム〕
ｉ）観測更新については、ＩＭＵ測定装置２６によって検出されたビークルの加速度Ａの観測値を入力し、前回までの推定状態量（加速度Ａ，ドライバの運転の揺らぎε_ｄａの各推定値）を観測更新し、推定状態量の推定誤差共分散行列Ｐを観測更新する。
ii）時間更新については、観測更新した推定状態量に遷移行列Ｆ_ｔを掛けて時間更新し、観測更新した推定誤差共分散行列に遷移行列Ｆ_ｔを掛けて駆動雑音の共分散Ｑを加える。
iii）運動方程式の時間更新については、上記の推定状態量の加速度Ａに対して遷移行列Ｆ_ｔを掛けて積分するととも、速度Ｖおよび位置ベクトルＲ_ｇの各推定値を時間更新し、推定状態量（Ｖ，Ｒ_ｇ）を時間更新するごとに推定誤差共分散行列Ｐに遷移行列Ｆ_ｔを掛け、駆動雑音の共分散Ｑを加える。

上述したカルマンフィルタによれば、加速度Ａ，ドライバの運転の揺らぎε_ｄａ，位置ベクトルＲ_ｇなどを推定（濾波）して観測データの補間を行うとともに、存在する観測データの範囲内で観測データがない状態量である速度（縦速度，横速度）を誤差の二乗平均が最小という意味で最適な推定ができる。さらには、これらの縦速度ｖおよび横速度ｕを用いて、次式により横滑角βを推定することができる。
β＝tan^−１（ｕ／ｖ）…（６）

上述した駆動雑音Ｗ_ｔの有色化は、システムの方程式と整形フィルタの組み合わせにより実現され、結局は白色雑音を含む状態方程式が得られた（図２，図３を参照）。ここで、さらに有色な観測雑音モデルを追加してみる。

第１には、観測雑音を有色化したシステムモデルであって、ブロック図で表すと図６のようになる。当該図６に表したブロック図は、上述した式（１）および式（２）で表した状態方程式をカルマンフィルタ方程式に変換したものである。
通常のカルマンフィルタでは、観測雑音は白色であるが、ここでは図６のループ６０で観測雑音を有色化している。この有色化により、ｙ_ｔ（観測データ）のパワースペクトル表現の自由度が増加する。

第２には、観測雑音を有色化したカルマンフィルタであって、ブロック図で表すと図７のようになる。当該図６に表したブロック図は、上述した式（３）および式（４）で表した状態方程式をカルマンフィルタ方程式に変換したものである。

上述した式（１）および式（２）にかかる状態方程式と、式（３）および式（４）にかかる状態方程式はいずれも線形で極めて簡単な形で表されるので、慣性データの積分誤差等の影響を排除し、計算量が少なくなって処理も迅速に行える。したがって、ビークルの走行中において、実時間で実際の走行軌跡と同等の走行軌跡を得ることができる。

また、時間更新周波数が観測周波数よりも高くなるように構成したカルマンフィルタを用いたので、ＩＭＵ測定装置２６とデータ処理部２４とで処理する周波数が異なっていても、カルマンフィルタがＩＭＵ測定装置２６で得られるデータを用いて補間を行うので、滑らかな走行軌跡を得ることができる。

さらに、カルマンフィルタは、キネマティック測位計算して求めたビークルの位置ベクトルＲ_ｇと、ＩＭＵ測定装置２６によって得られた加速度Ａとを用いて、ビークルの速度Ｖ（詳細には縦速度ｖや横速度ｕ）や横滑角βを推定するので、走行軌跡だけでなく、走行中の状態（ビークルの速度Ｖや横滑角β）をきめ細かく得ることができる。

ビークルとして自動車を用いて２種類のテストコースを走行させ、この走行中に得られた走行軌跡を図８と図９に表す。図８には「Ｊ」字状を周回するコースの例を示し、図９には「８」字状を周回するコースの例を示す。いずれのコースを走行させた場合でも、推定誤差共分散行列Ｐ、駆動雑音の共分散Ｑ、観測雑音行列ν（観測雑音ν_Ｒｇ，観測雑音ν_ａ）およびそれらの雑音比（ν／Ｑ）は走行条件によらずほぼ同一であった。したがって、どのような走行路であってもビークル（本例は自動車）の走行中において、実時間で実際の走行軌跡と同等の走行軌跡を得ることができる。

また、走行中に得られた速度（縦速度ｖ，横速度ｕ）の変化を図１０（Ａ）に表し、上記式（６）によって得られた横滑角βの変化を図１０（Ｂ）に表す。したがって、どのような走行路であっても、走行中の状態（ビークルの速度や横滑角）をきめ細かく得ることができる。

〔他の実施の形態〕
以上では本発明を実施するための最良の形態について説明したが、本発明は当該形態に何ら限定されるものではない。言い換えれば、本発明の要旨を逸脱しない範囲内において、種々なる形態で実施することができる。例えば、次に示す各形態を実現してもよい。

上述した実施の形態では、ビークルとして自動車を適用した。この形態に代えて、航空機（ミサイルを含む）や、船舶、屋外用の搬送車などにも同様に適用することが可能である。これらの移動体であっても、走行中の状態（速度や横滑角等）をきめ細かく得ることができる。

Claims (3)

1. 自動車や船舶等のようなビークルに搭載された移動局と、地上に固定された基準局とを有し、ＧＰＳ衛星群から発信されるＧＰＳ信号等に基づいてビークルの走行軌跡を計測するビークル走行軌跡計測装置であって、
   前記移動局には、前記ＧＰＳ信号に基づいてビークルの位置を測位するビークル位置測位手段と、ビークルの姿勢角や加速度を検出するＩＭＵとを備え、
   前記基準局には、前記ＧＰＳ信号に基づいて基準局の位置を測位する基準局位置測位手段を備え、
   前記移動局で観測されたビークルの位置や姿勢角，加速度を含む第１データと、前記基準局で観測された基準局の位置を含む第２データとを受けて、次に示す式（１）および式（２）からなる運動方程式に基づいて実時間でビークルの走行軌跡を計測する実時間融合処理装置を有するビークル走行軌跡計測装置。
   

   
2. 請求項１に記載したビークル走行軌跡計測装置であって、
   ＩＭＵで観測して更新されるデータの周波数をビークル位置測位手段で更新されるデータの周波数よりも高くし、
   実時間融合処理装置は、前記ビークル位置測位手段で得られるデータの相互間におけるデータを前記ＩＭＵで得られるデータを用いて補間するカルマンフィルタを有するビークル走行軌跡計測装置。
3. 請求項２に記載したビークル走行軌跡計測装置であって、
   カルマンフィルタは、第１データおよび第２データの双方に含まれるＧＰＳ信号のデータを用いてキネマティック測位計算して得られたビークルの位置と、ＩＭＵによって得られたビークルの加速度とを用いて、ビークルの横滑角を推定するように構成したビークル走行軌跡計測装置。

Images