JP2003075172A - 車両挙動計測装置 - Google Patents

Abstract

(57)【要約】 【課題】 加速度センサの加速度信号に重畳されている
車両の移動に伴う前後方向加速度成分と、車両の傾斜に
伴う重力の前後方向加速度成分を分離し、移動時や停車
時などに係わらず、カルマンフィルタ処理で精度の高い
姿勢角を求めることのできる車両挙動計測装置を得る。 【解決手段】 車両の前後方向加速度に対応した加速度
信号を出力する加速度センサと、車両の移動距離に対応
した距離信号を出力する距離センサと、加速度信号およ
び距離信号に基づいてカルマンフィルタ処理を行い、車
両の姿勢角を離散時刻毎に算出する信号処理器とを備え
た。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】この発明は、例えば、自動車
などに適用される車両挙動計測装置に関するものであ
る。

【０００２】

【従来の技術】従来より、車両（自動車）走行において
は、快適さが求められており、自動変速機による制御に
ついても、例えば道路の勾配などの走行条件に応じて木
目細やかな制御が必要になってきた。このような走行条
件に応じた制御を行うために、車内に車両挙動計測装置
を設置し、車両の姿勢角（道路の勾配）などを計測して
いる。

【０００３】例えば、特開２００１−０８２９５６号公
報に示された従来の車両挙動計測装置においては、加速
度センサにより直接検出される加速度実測値（車両の走
行方向に現れる前後方向加速度）と、距離センサにより
得られるパルス信号（車両の走行速度に応じて周期が変
化する）を微分して算出される加速度計算値（走行速度
の微分値）との２種の加速度値に基づいて、車両の姿勢
角（走行中の道路の勾配）を算出している。

【０００４】ここで、加速度実測値をＧｓｅｎ、加速度
計算値をＤｖｓｐとしたとき、加速度実測値Ｇｓｅｎお
よび加速度計算値Ｄｖｓｐと車両の姿勢角βとの関係
は、以下の（１）式で表される。

【０００５】

【数１】

【０００６】（１）式において、ｇは重力加速度であ
る。また、加速度実測値Ｇｓｅｎから加速度計算値Ｄｖ
ｓｐを減算した値を、ノイズ低減用のローパスフィルタ
（低域通過フィルタ）にかけて、車両の姿勢角βの算出
を行っている。

【０００７】また、他の従来装置として、例えば、特開
平９-０４２９７９号公報に示された車両挙動計測装置
がある。この車両挙動計測装置は、例えばカーナビゲー
ションシステムに搭載されており、車両挙動計測装置か
ら出力される値に基づいて車両の現在位置をカーナビゲ
ーションシステムの表示部の地図上に表示している。

【０００８】この車両挙動計測装置の距離センサは、車
輪の回転数を計測して車両の移動距離を算出している。
しかし、車両が勾配の急な坂道を登る場合、実際の移動
距離と地図上に表されている水平方向の移動距離とを比
較すると、両者間に誤差が生じるので、地図上に正確な
現在位置を表示することはできない。

【０００９】そこで、この誤差を小さくするために、前
述のように、車両挙動計測装置の加速度センサが検出し
たＸ軸、Ｙ軸およびＺ軸方向の加速度と、距離センサが
検出した加速度とに基づいて車両の姿勢角βを演算して
いる。

【００１０】そして、以下の（２）式のように距離セン
サが算出した車両の速度Ｖ_iから水平方向の速度Ｖ'_iを
演算し、水平面（地図上）の距離の誤差を補正した後
に、水平面（地図上）における車両の現在位置を表示し
ている。

【００１１】

【数２】

【００１２】

【発明が解決しようとする課題】従来の車両挙動計測装
置は以下のように、姿勢角βを高精度にるためには、加
速度実測値を高精度に検出して、加速度実測値Ｇｓｅｎ
と加速度計算値Ｄｖｓｐとの誤差を抑制する必要があっ
た。

【００１３】また、加速度センサは、出力信号（電圧）
から加速度を得るために、加速度信号のバイアスと加速
度の関係を示すスケールファクタとを利用しているの
で、加速度実測値Ｇｓｅｎの検出精度を高くするために
は、加速度信号のバイアスとスケールファクタとを補正
する必要があった。

【００１４】特に、加速度信号のバイアスは、車両の移
動中において変動量が大きいので、車両が移動中にも補
正する必要があり、仮に補正演算が実行されなければ、
高精度に姿勢角βを検出できる時間が短くなり、実用に
向かないという問題点があった。

【００１５】また、従来の車両挙動計測装置は、単に水
平方向の加速度を補正するのみであり、車両の３次元位
置（緯度、経度、高度）の運動を補正することができな
いという問題点があった。

【００１６】さらに、従来の車両挙動計測装置は、水平
面上の２次元位置（緯度、経度）を算出するために、距
離および方位による移動ベクトルを前回の推定位置に加
算して車両の現在位置を更新しているので、移動ベクト
ルの累積誤差を補正することができないという問題点が
あった。

【００１７】この発明は、上記問題点を解決するために
なされたもので、車両の挙動によらず、高精度に車両の
姿勢角を算出することのできる車両挙動計測装置を得る
ことを目的とする。

【００１８】また、この発明は、車両の挙動によらず、
加速度信号のバイアスとスケールファクタとを補正する
ことのできる車両挙動計測装置を得ることを目的とす
る。

【００１９】また、この発明は、車両の挙動によらず、
高精度に車両の高度を算出することのできる車両挙動計
測装置を得ることを目的とする。

【００２０】また、この発明は、離散時刻毎に車両の３
次元位置を補正することのできる車両挙動計測装置を得
ることを目的とする。

【００２１】さらに、この発明は、離散時刻毎に車両の
２次元位置を補正することのできる車両挙動計測装置を
得ることを目的とする。

【００２２】

【課題を解決するための手段】この発明に係る車両挙動
計測装置は、車両の前後方向加速度に対応した加速度信
号を出力する加速度センサと、車両の移動距離に対応し
た距離信号を出力する距離センサと、加速度信号および
距離信号に基づいてカルマンフィルタ処理を行い、車両
の姿勢角を離散時刻毎に算出する信号処理器とを備えた
ものである。

【００２３】この発明に係る車両挙動計測装置の信号処
理器は、加速度信号および距離信号から算出された車両
の加速度および車両の姿勢角と加速度センサに含まれる
スケールファクタとに基づいて離散時刻毎に加速度信号
のバイアスを補正するものである。

【００２４】この発明に係る車両挙動計測装置は、車両
の高度に対応した高度信号を出力する高度計を備え、信
号処理器は、加速度信号、距離信号および高度信号に基
づいてカルマンフィルタ処理を行い、離散時刻毎に姿勢
角および高度の少なくとも一方を算出するものである。

【００２５】この発明に係る車両挙動計測装置の信号処
理器は、高度信号と車両の加速度と加速度センサに含ま
れるスケールファクタとに基づいて離散時刻毎に加速度
信号のバイアスを補正するものである。

【００２６】この発明に係る車両挙動計測装置の信号処
理器は、距離信号および姿勢角から算出された車両の高
度方向の移動量と、高度信号の変化量とを比較して、カ
ルマンフィルタ処理による高度の観測誤差を算出するも
のである。

【００２７】この発明に係る車両挙動計測装置は、高度
計は、大気圧に対応した気圧信号を出力する気圧高度セ
ンサからなり、信号処理器は、距離信号から算出された
車両の速度と気圧信号から算出された高度差とに基づい
て離散時刻毎に加速度信号のバイアスを補正し、加速度
信号、距離信号および気圧信号に基づいてカルマンフィ
ルタ処理を行い、離散時刻毎に車両の姿勢角および高度
の少なくとも一方を算出するものである。

【００２８】この発明に係る車両挙動計測装置は、高度
計は、車両の高度を含む３次元位置に対応した位置信号
を出力するＧＰＳ受信機からなり、信号処理器は、加速
度信号、距離信号および位置信号に基づいてカルマンフ
ィルタ処理を行い、離散時刻毎に車両の姿勢角および高
度の少なくとも一方を算出するものである。

【００２９】この発明に係る車両挙動計測装置は、大気
圧に対応した気圧信号を出力する気圧高度センサを備
え、信号処理器は、位置信号と気圧信号とを比較してカ
ルマンフィルタ処理による高度の観測誤差を算出し、加
速度信号、距離信号、位置信号および気圧信号に基づい
てカルマンフィルタ処理を行い、離散時刻毎に車両の姿
勢角および高度の少なくとも一方を算出するものであ
る。

【００３０】この発明に係る車両挙動計測装置は、車両
の角速度に対応した角速度信号を出力する相対方位セン
サを備え、信号処理器は、加速度信号、距離信号、位置
信号および角速度信号に基づいてカルマンフィルタ処理
を行い、離散時刻毎に車両の３次元位置および姿勢角を
算出するものである。

【００３１】この発明に係る車両挙動計測装置は、車両
の角速度に対応した角速度信号を出力する相対方位セン
サを備え、信号処理器は、車両の姿勢角および移動距離
から算出される水平面の移動距離と、車両の移動距離と
を比較してシステム誤差を算出し、加速度信号、距離信
号、位置信号および角速度信号に基づいてカルマンフィ
ルタ処理を行い、離散時刻毎に車両の２次元位置および
進行方位を算出するものである。

【００３２】

【発明の実施の形態】実施の形態１．以下、図面を参照
しながら、この発明の実施の形態１について説明する。
この発明による車両挙動計測装置は、例えば、車両がど
の道路のどの位置にいるかという情報を搭乗者に知らせ
るカーナビゲーションシステムに設置されており、その
中で車両の姿勢角などを計測し、これらの計測情報に基
づき、車両の現在位置を推定する。

【００３３】図１は、この発明の実施の形態１の構成を
示すブロック図である。

【００３４】図１において、１は加速度センサであり、
車両の走行方向に現れる前後方向加速度を計測し、前後
方向加速度と加速度信号のバイアスとの関係を示す傾き
（スケールファクタ）に基づいて前後方向加速度に応じ
た加速度信号（電圧）を出力する。

【００３５】２は距離センサであり、タイヤの回転数か
ら車両の走行距離を計測し、その走行距離に応じて距離
信号（パルス信号）を出力する。６はコンピュータを含
む信号処理器であり、あらかじめメモリに記憶した制御
プログラムに従って車両の挙動を推定する。

【００３６】信号処理器６は、距離信号から車両の速度
および加速度を算出する速度計算手段６１と、加速度信
号、速度計算手段６１で算出する速度および加速度から
車両の姿勢角などを算出する挙動推定手段６４とで構成
されている。

【００３７】信号処理器６内の制御プログラムは、図２
〜図４のフローチャートに示すように第１の割込処理、
第２の割込処理およびメインルーチンにより構成されて
いる。図４のメインルーチンが処理を開始すると、第１
の割込処理（図２参照）は、所定時間△Ｔ毎に実行され
る。第２の割込処理（図３参照）は、距離センサ２から
距離信号が出力されたときに実行される。

【００３８】次に、図１とともに、図２〜図４を参照し
ながら、この発明の実施の形態１による動作について説
明する。

【００３９】まず、図２に示す第１の割込処理におい
て、割込処理を開始するタイミングを示す処理開始フラ
グをセットし（ステップＳ２０１）、図２の処理を終了
する。

【００４０】また、図３に示す第２の割込処理におい
て、距離センサ２からのパルス信号に対応した車速パル
スカウンタに「１」を加算し（ステップＳ３０１）、図
３の処理を終了する。

【００４１】一方、図４に示すメインルーチンにおいて
は、速度計算手段６１および挙動推定手段６４の動作を
管理している。まず、全ての処理を初期化し（ステップ
Ｓ４０１）、以下の処理を実行する。

【００４２】すなわち、第１の割込処理（図２）でセッ
トされた処理開始フラグを参照して、メインルーチンの
処理を開始するタイミングか否かを判定する（ステップ
Ｓ４０２）。ステップＳ４０２において、処理開始フラ
グがセットされている（すなわち、ＹＥＳ）と判定され
れば、処理を開始するタイミングであると見なして、次
回の処理のために処理開始フラグをクリアする（ステッ
プＳ４０３）。

【００４３】一方、ステップＳ４０２において、処理開
始フラグがセットされていない（すなわち、ＮＯ）と判
定されれば、メインルーチンの処理を開始するタイミン
グではないと見なし、第１の割込処理内のステップＳ２
０１で処理開始フラグがセット（ステップＳ４０２でＹ
ＥＳと判定）されるまで待機する。

【００４４】ステップＳ４０３に続いて、速度計算手段
６１の処理（ステップＳ４０４）を実行する。すなわ
ち、速度計算手段６１は、以下の（３）式、（４）式に
より、車速パルスカウンタのカウント値△Ｎ_iから速度
Ｖ_Riおよび加速度Ａ_Riを算出する。

【００４５】

【数３】

【００４６】また、次回の処理のために、車速パルスカ
ウンタのカウント値△Ｎ_iを０にする。なお、（３）式
中のＳＦ_SPDiは距離センサ２の距離信号（パルス信号）
を距離に変換する係数（スケールファクタ）である。

【００４７】次に、加速度信号（電圧）Ｓ_iをＡ／Ｄ変
換し（ステップＳ４０５）、挙動推定手段６４の処理
（ステップＳ４０６）を実行する。ここでは、加速度信
号および距離信号から車両の姿勢角β_iなどを算出す
る。

【００４８】次に、挙動推定手段６４で算出した車両の
速度Ｖ_iと姿勢角β_iと加速度センサ１のスケールファク
タＳＦ_ACCiとを用いて、以下の（５）式により、加速度
センサ１の加速度信号のバイアスＳ_Oiを算出する（ステ
ップＳ４０７）。

【００４９】

【数４】

【００５０】（５）式のように算出（補正）された値
は、次回の加速度センサ１での計測に使用される。最後
に、挙動推定手段６４で算出した車両の速度Ｖ_iおよび
姿勢角β_iを出力して（ステップＳ４０８）、ステップ
Ｓ４０２に戻る。

【００５１】以上のように、離散時刻ｉ毎に車両の速度
Ｖ_iおよび姿勢角β_iの算出と、加速度信号のバイアスＳ
_Oiの補正とを実行する。

【００５２】次に、挙動推定手段６４で適用されている
車両挙動の推定方法について、さらに具体的に説明す
る。この推定方法は、各時刻における予測値と観測値と
の誤差（ノイズ）を修正（除去）しながら、各時刻にお
ける最適な推定値を逐次求める方法である。

【００５３】まず、ある値を予測するための算出式を設
定し、この算出式を用いて、観測値を得ることができる
時刻ｎまで予測を繰り返す。その後、観測値が取得され
れば、観測値と時刻ｎでの予測値とのそれぞれの誤差を
算出し、観測値の誤差を差し引いた上で、時刻ｎでの予
測値を観測値に所定割合だけ漸近させるように推定値を
算出する。また、両誤差を用いて所定割合を算出し、こ
の所定割合を次回の推定値の算出に用いる。

【００５４】このように、誤差の算出と算出式の修正を
繰り返すことによって、上記算出式は、次第に最適な推
定値を算出することができる形に収まっていくことにな
る。この処理方法は、カルマンフィルタ処理と呼ばれて
いる。

【００５５】以下、このカルマンフィルタ処理を用い
て、加速度信号Ｓ_iと車両挙動を表す速度Ｖ_iと姿勢角β
_iとの関係と、距離センサ２からの観測値とその観測値
の誤差との関係とから、現在の車両挙動を推定する。ま
た、算出された車両挙動を用いて、加速度センサ１の加
速度信号のバイアスＳ_Oiを補正する。

【００５６】以上のような処理を離散時刻ｉ毎に繰り返
すことにより、繰り返し推定された推定値は、誤差が小
さくなり、真の値に近づくことになる。また、加速度信
号のバイアスＳ_Oiの補正を繰り返すことにより、加速度
信号のバイアスＳ_Oiを利用した加速度センサ１の加速度
実測値も精度が高くなる。

【００５７】まず、加速度信号Ｓ_iから車両挙動を表す
速度Ｖ_iおよび姿勢角β_iを離散時刻ｉ毎に算出する関係
式（システムモデル）を導く。加速度センサ１におい
て、加速度信号Ｓ_iと車両の移動に伴う前後加速度成分
Ｓ_(move)iと、道路傾斜時に重力加速度の影響で生じる
加速度成分Ｓ_(tilt)iと、加速度信号のバイアスＳ_Oiと
は、以下の（６）式の関係にある。

【００５８】

【数５】

【００５９】また、前後加速度成分Ｓ_(move)iと、道路
傾斜時に重力加速度の影響で生じる加速度成分Ｓ
_(tilt)iと、速度Ｖ_i、車両の姿勢角β_iは、それぞれ、
以下の（７）式、（８）式のように表わされる。

【００６０】

【数６】

【００６１】上記（６）式に（７）式および（８）式を
代入すると、車両の姿勢角β_iに関する以下の（９）式
〜（１２）式が導出される。

【００６２】

【数７】

【００６３】ここで、（９）式の姿勢角β_iが２０°以
下ならば、２％以下の誤差内で（１０）式の近似式を使
えることから、（１０）式を変形して（１１）式と（１
２）式のシステムモデルが得られる。

【００６４】なお、（１１）式および（１２）式中のＳ
Ｆ_ACCiは、加速度信号Ｓ_iを加速度に変換するスケール
ファクタであり、ｇは重力加速度、Ａ_iは車両挙動を表
す加速度である。

【００６５】次に、距離信号から算出された速度Ｖ_Riと
加速度Ａ_Ri、およびシステムモデルの算出結果の関係
は、以下の（１３）式、（１４）式の観測モデルのよう
に表される。

【００６６】

【数８】

【００６７】なお、（１３）式、（１４）式において、
δＶ_Ri、δＡ_Riは、距離信号（パルス信号）の量子化で
速度および加速度の取り得る最大値との差異を、それぞ
れ、距離信号から算出した速度Ｖ_Riおよび加速度Ａ_Riの
誤差とした値である。

【００６８】そして、状態方程式は、以下の（１５）式
〜（１９）式のように表される。

【００６９】

【数９】

【００７０】（１５）式〜（１９）式において、ｘ_iは
離散時刻ｉにおける速度、加速度、姿勢角およびスケー
ルファクタの状態ベクトルを表す。次に、Ｆ_iは離散時
刻ｉにおける状態遷移行列、Ｇ_iは離散時刻ｉにおける
駆動行列を表す。さらに、ｗ_iは離散時刻ｉにおけるシ
ステム誤差、ｙ_iは離散時刻ｉにおける観測値、ｖ_iは離
散時刻ｉにおける観測誤差（観測値と真の値との誤差）
を表す。Ｈは観測行列を表す。

【００７１】さらに、観測方程式は、以下の（２０）式
〜（２３）式のように表される。

【００７２】

【数１０】

【００７３】なお、観測誤差ｖ_iは、速度および加速度
の各誤差δＶ_Ri、δＡ_Riとする。システム誤差ｗ_iは、
状態ベクトルｘ_iの要素ＳＦ_ACCiと、（９）式で算出し
た加速度センサ１の加速度信号のバイアスＳ_Oiとのそれ
ぞれの標準偏差を誤差としたδＳＦ_ACCi、δＳ_Oiとす
る。

【００７４】以上の関係式と、以下の（２４）式〜（２
８）式に示すカルマンフィルタ方程式によって、車両の
速度と加速度と姿勢角とスケールファクタの予測をす
る。

【００７５】

【数１１】

【００７６】そして、離散時刻ｉ毎に車両の速度Ｖ_iと
加速度Ａ_iと姿勢角β_iとスケールファクタＳＦ_ACCiとの
推定値を算出する。さらに、それぞれの誤差、カルマン
方程式の係数となるカルマンゲインの算出を行う。

【００７７】なお、（２４）式〜（２８）式において、
ｘ_i|iは離散時刻ｉにおける状態ベクトルの推定量、ｘ
_i+1|iは離散時刻ｉにおける離散時刻ｉ＋１の状態ベク
トルの予測量、Ｋ_iはカルマンゲイン、Σ_i|iは離散時刻
ｉにおける状態ベクトルの共分散行列の推定量、Σ
_i+1|iは離散時刻ｉにおける離散時刻ｉ＋１の共分散行
列の予測量、Σ_viは観測誤差の共分散行列、Σ_wiはシス
テム誤差の共分散行列を示す。

【００７８】このように、カルマンフィルタ処理によっ
て、離散時刻ｉ毎に姿勢角β_iなどの推定を行い、さら
に、加速度センサ１の加速度信号のバイアスＳ_Oiの補正
を行うことによって、車両挙動の推定値と加速度センサ
１による加速度実測値との精度が高くなる。

【００７９】なお、補正された加速度信号のバイアスＳ
_Oiと距離信号から算出された速度Ｖ _Riとを（１０）式に
直接代入して計算するのみであっても、精度の高い車両
の姿勢角を得ることができる。

【００８０】また、ここでは、（１３）式および（１
４）式との観測モデルにおいて、距離センサ２が出力す
る距離信号から速度および加速度を算出するものとして
説明したが、距離センサ２以外にも、車両の速度および
加速度を計測する手段（例えばＧＰＳ受信機など）を距
離センサ２の代わりに用いても同様の効果がある。

【００８１】また、加速度信号のバイアスＳ_Oiの補正に
おいては、（５）式の算出値を加速度センサ１の加速度
信号のバイアスＳ_Oiの補正値として用いたが、さらにロ
ーパスフィルタ処理したものを加速度信号のバイアスＳ
_Oiの補正値として用いれば、加速度センサ１の加速度信
号から算出する車両の姿勢角をより正確に、かつ誤差変
動を少なくすることができる。

【００８２】実施の形態２．なお、上記実施の形態１で
は、加速度センサ１および距離センサ２のみを用いた
が、さらに高度計（ＧＰＳ受信機）を用いてもよい。

【００８３】以下、図面を参照しながら、この発明の実
施の形態２について説明する。図５は、この発明の実施
の形態２の構成を示すブロック図である。

【００８４】図５において、前述（図１参照）と同様の
ものについては、同一符号を付して、または符号の後に
「Ａ」を付して詳述を省略する。

【００８５】３は高度計として用いられるＧＰＳ受信機
であり、複数のＧＰＳ衛星から電波を受信するＧＰＳア
ンテナが接続されている。ＧＰＳ受信機３は、周知のよ
うに複数のＧＰＳ衛星からの電波により位置（緯度、経
度、高度）を計測して位置信号（高度信号を含む）出力
する。

【００８６】信号処理器６Ａは、前述の速度計算手段６
１に加えて、挙動推定手段６４Ａを備えている。挙動推
定手段６４Ａは、加速度センサ１からの加速度信号と、
速度計算手段６１により求められた速度および加速度
と、ＧＰＳ受信機３により計測された高度を含む３次元
位置とから、車両の姿勢角や高度などを算出する。

【００８７】信号処理器６Ａ内の制御プログラムは、前
述（図４参照）のメインルーチンと、前述（図２参照）
の所定時間△Ｔ毎に実行される第１の割込処理と、前述
（図３参照）の距離信号の出力毎に実行される第２の割
込処理と、図６に示した第３の割込処理とにより構成さ
れている。第３の割込処理においては、ＧＰＳ受信機３
が所定の周期毎に出力する位置データを受信する。

【００８８】次に、図５とともに、図６および図２〜図
４のフローチャートを参照しながら、この発明の実施の
形態２による動作について説明する。

【００８９】なお、第１および第２の割込処理について
は、図２および図３とともに前述したとおりである。同
様にメインルーチンについては、図４とともに前述した
とおりである。ただし、図４内のステップＳ４０６〜Ｓ
４０８の一部動作のみが異なる。

【００９０】図６において、まず、ＧＰＳ受信機３が出
力した位置信号を受信して車両挙動計測装置内のメモリ
に記憶させる（ステップＳ６０１）。続いて、全ての位
置信号の受信が完了したか判定し（ステップＳ６０
２）、全ての位置データを受信完了した（すなわち、Ｙ
ＥＳ）と判定されれば、ＧＰＳ観測位置信号受信フラグ
をセットし（ステップＳ６０３）、受信が完了していな
い（すなわち、ＮＯ）と判定されれば、ステップＳ６０
３を実行せずに図６の第３の割込処理を終了する。

【００９１】なお、メインルーチン（図４参照）におい
て、前述と同様に、速度計算手段６１および挙動推定手
段６４Ａの動作を管理する。

【００９２】すなわち、図４内のステップＳ４０１〜Ｓ
４０５においては、前述と同様に動作し、ステップＳ４
０６（挙動推定手段６４Ａ）においては、加速度信号、
距離信号および位置信号（高度信号）とから、姿勢角β
_iや高度Ｈ_iなどを算出する。

【００９３】

【数１２】

【００９４】次に、ステップＳ４０７において、以下の
（２９）式〜（３１）式を求める。すなわち、前述の
（１０）式に（３０）式を代入して導出した（３１）式
に、挙動推定手段６４Ａで算出した車両の速度Ｖ_i、高
度Ｈ_iおよび加速度センサ１のスケールファクタＳＦ
_ACCiを代入して、加速度センサ１の加速度信号のバイア
スＳ _Oiを算出する。この算出（補正）値は、次回の加速
度センサ１での計測に使用される。

【００９５】最後に、ステップＳ４０８において、車両
の速度Ｖ_iと姿勢角β_iおよび高度Ｈ _iの少なくとも一方
とを出力して、ステップＳ４０２に戻る。

【００９６】以上のように、離散時刻ｉ毎に、車両の速
度Ｖ_iと姿勢角β_iおよび高度Ｈ_iの少なくとも一方との
算出と、加速度信号のバイアスＳ_Oiおよびスケールファ
クタＳＦ_ACCiの補正とを行う。

【００９７】次に、挙動推定手段６４Ａで適用されてい
る車両挙動の推定方法について説明する。この推定方法
においても前述と同様に、カルマンフィルタ処理が用い
られる。

【００９８】まず、離散時刻ｉにおける車両の移動距離
Ｌ_i、姿勢角β_iが既知であれば、以下の（３２）式に従
って高低差ΔＨ_iが求めることができ、高度Ｈ_iは以下の
（３３）式で算出することができる。

【００９９】

【数１３】

【０１００】（３２）式において、移動距離Ｌ_iは、以
下の（３４）式により算出される。

【０１０１】

【数１４】

【０１０２】上記（３３）式に（３２）式を代入する
と、以下の（３５）式が導出される。

【０１０３】

【数１５】

【０１０４】この（３５）式と前述の（１１）式および
（１２）式とは、加速度信号Ｓ_iから車両挙動に関する
速度Ｖ_iと姿勢角β_iおよび高度Ｈ_iの少なくとも一方と
を離散時刻ｉ毎に算出するシステムモデルである。

【０１０５】次に、速度計算手段６１で算出した速度Ｖ
_Riおよび加速度Ａ_Riと、（１１）式、（１２）式および
（３５）式のシステムモデルの算出結果との関係は、
（１３）式、（１４）式および（３６）式の観測モデル
のように表される。

【０１０６】

【数１６】

【０１０７】なお、この場合、δＶ_RiおよびδＡ_Riは、
距離信号から算出された速度および加速度の誤差であ
り、距離信号から算出した速度および加速度と、ＧＰＳ
受信機３で計測した速度および加速度とを比較して求め
られる。

【０１０８】また、δＨ_Riは、離散時刻ｉ毎の車両の移
動距離Ｌ_iおよび姿勢角β_iから（３２）式で算出された
高度方向移動量ΔＨ_iと、離散時刻ｉにおけるＧＰＳ受
信機３が計測した高度の変化量（Ｈ_Ri−Ｈ_Ri-1）とを比
較して求めたＧＰＳ受信機３が計測した高度の誤差であ
る。

【０１０９】そして、状態方程式は、（１５）式および
（３７）式〜（４０）式のように表される。

【０１１０】

【数１７】

【０１１１】さらに、観測方程式は、（２０）式および
（４０）式〜（４２）式のように表される。

【０１１２】

【数１８】

【０１１３】なお、観測誤差ｖ_iについては、速度の誤
差、加速度の誤差および高度の誤差δＶ_Ri、δＡ_Riおよ
びδＨ_Riとする。また、システム誤差ｗ_iは、状態ベク
トルｘ_iの要素ＳＦ_ACCiと、（３１）式で算出した加速
度センサ１の加速度信号のバイアスＳ_Oiとのそれぞれの
標準偏差を誤差としたδＳＦ_ACCiおよびδＳ_Oiとする。

【０１１４】以上の関係式と、（２４）式〜（２８）式
に示すカルマンフィルタ方程式によって、車両の速度と
加速度と高度および姿勢角の少なくとも一方とスケール
ファクタとの予測をする。そして、離散時刻ｉ毎に車両
の速度Ｖ_iと加速度Ａ_iと高度Ｈ_iおよび姿勢角β_iの少な
くとも一方とスケールファクタＳＦ_ACCiとの推定値を算
出する。さらに、それぞれの誤差、カルマン方程式の係
数となるカルマンゲインの算出を行う。

【０１１５】このように、カルマンフィルタ処理によっ
て、離散時刻ｉ毎に速度Ｖ_iと姿勢角β_iおよび高度Ｈ_i
の少なくとも一方とスケールファクタＳＦ_ACCiとなどの
推定を行い、さらに、加速度センサ１の加速度信号のバ
イアスＳ_Oiの補正を行うことによって、車両挙動と加速
度センサ１による加速度実測値との精度が高くなる。

【０１１６】また、ここでは、（１３）式および（１
４）式の観測モデルにおいて、距離センサ２が出力する
距離信号から速度Ｖ_Riおよび加速度Ａ_Riを算出するもの
として説明したが、距離センサ２以外で車両の速度、加
速度および高度を計測する手段（例えば、ＧＰＳ受信機
３）を距離センサ２の代わりに用いて、カルマンフィル
タ処理を実行することにより、最適な車両の速度Ｖ_i、
姿勢角β_iおよび高度Ｈ_iとを得ることができる。また、
距離センサ２を省くことができ、装置を簡略化すること
ができる。

【０１１７】また、ＧＰＳ受信機３にＤｉｆｆｅｒｅｎ
ｔｉａｌ ＧＰＳ（以下、ＤＧＰＳ）受信機、もしくは
測量用にも用いられるＲｅａｌ Ｔｉｍｅ Ｋｉｎｅｍ
ａｔｉｃ ＧＰＳ(以下、ＲＴＫＧＰＳ)受信機を用いれ
ば、より高精度に算出することができる。

【０１１８】また、移動距離Ｌ_iは（３４）式で算出す
るが、別の運動方程式で求めても同様な効果を得ること
ができる。

【０１１９】実施の形態３．なお、上記実施の形態２で
は、加速度センサ１および距離センサ２に加えて、高度
計としてＧＰＳ受信機３を用いたが、ＧＰＳ受信機３の
代わりに気圧高度センサを用いてもよい。

【０１２０】以下、図面を参照しながら、この発明の実
施の形態３について説明する。図７は、この発明の実施
の形態３の構成を示すブロック図である。

【０１２１】図７において、前述（図１および図５参
照）と同様のものについては、同一符号を付して、また
は符号の後に「Ｂ」を付して詳述を省略する。

【０１２２】４は高度計として用いられる気圧高度セン
サであり、大気圧に応じた気圧信号（電圧）を出力す
る。

【０１２３】信号処理器６Ｂは、前述の速度計算手段６
１に加えて、距離推定手段６４Ｂを備えている。挙動推
定手段６４Ｂは、加速度センサ１からの加速度信号と、
速度計算手段６１により求められた速度および加速度
と、気圧高度センサ４からの気圧信号とから、姿勢角や
高度などを算出する。

【０１２４】信号処理器６Ｂ内の制御プログラムは、前
述（図４参照）のメインルーチンと、前述（図２参照）
の所定時間△Ｔ毎に実行される第１の割込処理と、前述
（図３参照）の距離信号の出力毎に実行される第２の割
込処理とにより構成されている。

【０１２５】次に、図７とともに、図２〜図４のフロー
チャートを参照しながら、この発明の実施の形態３によ
る動作について説明する。

【０１２６】なお、第１および第２の割込処理につい
て、図２および図３とともに前述したとおりである。同
様に、メインルーチンについては、図４とともに前述し
たとおりである。ただし、図４内のステップＳ４０６〜
Ｓ４０８の一部動作のみが異なる。

【０１２７】なお、メインルーチン（図４参照）におい
ては、前述と同様に、速度計算手段６１および挙動推定
処理６４Ｂの動作を管理する。

【０１２８】すなわち、図４内のステップＳ４０１〜Ｓ
４０５においては、前述と同様に動作し、ステップＳ４
０６（挙動推定手段６４Ｂ）においては、気圧信号をＡ
／Ｄ変換し、図８に従って大気圧に換算して、さらに図
９に従って高度（信号）に変換する。そして、加速度信
号、距離信号および高度信号から、車両の姿勢角β_iや
高度Ｈ_iなどを算出する。

【０１２９】次に、ステップＳ４０７において、加速度
センサ１の加速度信号のバイアスを補正する。すなわ
ち、離散時刻ｉ毎に気圧変動から高度差（Ｈ_i−Ｈ_i-1）
を算出し、同様に距離信号から算出した速度から速度変
動（Ｖ_i−Ｖ_i-1）と、（３４）式により移動距離Ｌ_iを
算出して、（３１）式により、加速度センサ１の加速度
信号のバイアスＳ_Oiを算出する。この算出（補正）値は
次回の加速度センサ１での計測に使用される。

【０１３０】最後に、ステップＳ４０８において、車両
の速度Ｖ_iと姿勢角β_iおよび高度Ｈ _iの少なくとも一方
とを出力して、ステップＳ４０２に戻る。

【０１３１】以上のように、離散時刻ｉ毎に車両の速度
Ｖ_iと姿勢角β_iおよび高度Ｈ_iの少なくとも一方との算
出、加速度信号のバイアスＳ_Oiの補正を行う。

【０１３２】なお、ここでは、実施の形態２で用いたＧ
ＰＳ受信機に代えて、気圧高度センサを高度計として用
いた。信号処理器６４Ｂは、この気圧高度センサから出
力された気圧信号を高度信号に変換して、前述のＧＰＳ
受信機からの高度信号と同様に使用する。よって、挙動
推定手段６４Ｂで適用されているカルマンフィルタ処理
の推定方法については、実施の形態２と同様であるので
詳述を省略する。

【０１３３】ただし、観測誤差ｖ_iについては、速度の
誤差、加速度の誤差および高度の誤差δＶ_Ri、δＡ_Riお
よびδＨ_Riとする。速度の誤差δＶ_Riおよび加速度の誤
差δＡ_Riについては、実施の形態１で前述したとおりで
ある。さらに、高度の誤差δＨ_Riについては、実施の形
態２で前述したとおりである。

【０１３４】また、ここでは、高度計として気圧高度セ
ンサを用いたが、高度を検出できるものであれば他のも
のでもよく、カルマンフィルタ処理により同様な効果を
得ることができる。

【０１３５】さらに、精度の高いＤＧＰＳ受信機、ＲＴ
ＫＧＰＳ受信機であれば、なお一層精度の高い姿勢角を
得ることができる。

【０１３６】実施の形態４．なお、前述の実施の形態２
および３では、高度計としてＧＰＳ受信機または気圧高
度センサのどちらか一方を用いたが、ＧＰＳ受信機と気
圧高度センサとの両方を用いてもよい。

【０１３７】以下、図面を参照しながら、この発明の実
施の形態４について説明する。図１０は、この発明の実
施の形態４の構成を示すブロック図である。

【０１３８】図１０において、前述（図１、図５および
図７参照）と同様のものについては、同一符号を付し
て、または符号の後に「Ｃ」を付して詳述を省略する。

【０１３９】信号処理器６Ｃは、前述の速度計算手段６
１に加えて、挙動推定手段６４Ｃを備えている。挙動推
定手段６４Ｃは、加速度センサ１からの加速度信号と、
速度計算手段６１により求められた速度および加速度
と、ＧＰＳ受信機３からの位置信号と、気圧高度センサ
４からの気圧信号とから、姿勢角や高度などを算出す
る。

【０１４０】信号処理器６Ｃ内の制御プログラムは、図
１１で示したメインルーチンと、前述（図２参照）の第
１の割込処理と、前述（図３参照）の第２の割込処理
と、前述（図６参照）の第３の割込処理とにより構成さ
れている。

【０１４１】次に、図１０とともに、図２、図３、図６
および図１１を参照しながら、この発明の動作について
説明する。

【０１４２】なお、第１、第２および第３の割込処理に
ついては、図２、図３および図６とともに前述したとお
りである。

【０１４３】図１１に示すメインルーチンにおいては、
速度計算手段６１および挙動推定手段６４Ｃの動作を管
理している。まず、全ての処理を初期化し（ステップＳ
１１０１）、以下の処理を実行する。

【０１４４】第１の割込処理（図２）でセットされた処
理開始フラグを参照して、すなわち、メインルーチンの
処理を開始するタイミングか否か判定する（ステップＳ
１１０２）。ステップＳ１１０２において、処理開始フ
ラグがセットされている（すなわち、ＹＥＳ）と判定さ
れれば、処理を開始するタイミングであると見なして、
次回の処理のために処理開始フラグをクリアする（ステ
ップＳ１１０３）。

【０１４５】一方、ステップＳ１１０２において、処理
開始フラグがセットされていない（すなわち、ＮＯ）と
判定されれば、メインルーチンの処理を開始するタイミ
ングではないと見なし、第１の割込処理内のステップＳ
２０１で処理開始フラグがセット（ステップＳ１１０２
でＹＥＳと判定）されるまで待機する。

【０１４６】ステップＳ１１０３に続いて、速度計算手
段６１の処理（ステップＳ１１０４）を実行する。すな
わち、速度計算手段は、前述の（３）式、（４）式によ
り、距離センサ２の車速パルスカウンタのカウント値△
Ｎ_iから速度Ｖ_Riおよび加速度Ａ_Riを算出する。また、
次回の処理のために、車速パルスカウンタのカウント値
△Ｎ_iを０にする。

【０１４７】次に、加速度信号Ｓ_iをＡ／Ｄ変換する
（ステップＳ１１０５）。

【０１４８】また、気圧信号をＡ／Ｄ変換して、図８に
従って大気圧に換算し、さらに図９に従って高度（高度
信号）に換算する（ステップＳ１１０６）。

【０１４９】次に、挙動推定手段６４Ｃの処理（ステッ
プＳ１１０７）を実行する。ここでは、加速度信号、距
離信号、位置信号および気圧信号から、車両の姿勢角β
_iや高度Ｈ_iなどを算出する。

【０１５０】挙動推定手段６４Ｃで算出した車両の速度
Ｖ_i、高度Ｈ_i、姿勢角β_iおよびスケールファクタＳＦ
_ACCiを用いて（３１）式により、加速度センサ１の加速
度信号のバイアスＳ_Oiを算出する（ステップＳ１１０
８）。

【０１５１】（３１）式のように算出（補正）された値
は、次回の加速度センサ１での計測に使用される。最後
に、挙動推定手段６４Ｃで算出した車両の速度Ｖ_iと姿
勢角β_iおよび高度Ｈ_iの少なくとも一方とを出力して
（ステップＳ１１０９）、ステップＳ４０２に戻る。

【０１５２】以上のように、離散時刻ｉ毎に、車両の速
度Ｖ_iと姿勢角β_iおよび高度Ｈ_iの少なくとも一方との
算出、加速度信号のバイアスＳ_Oiの補正を実行する。

【０１５３】次に、挙動推定手段６４Ｃで適用されてい
る推定方法は、前述の実施の形態３と同様に気圧高度セ
ンサ４で検出した高度を観測モデルとして用いてカルマ
ンフィルタ処理を用いる。よって、前述の実施の形態２
および３と同様であるので、詳述は省略する。

【０１５４】ただし、観測誤差ｖ_iについては、前述の
距離信号から求めた速度および加速度と、ＧＰＳ受信機
３で計測した速度および加速度との誤差δＶ_Riおよびδ
Ａ_Riとする。さらに、ＧＰＳ受信機３および気圧高度セ
ンサ４でそれぞれ検出した高度の差にローパスフィルタ
処理した値を高度の誤差δＨ_Riとする。

【０１５５】また、ここでは、前述の実施の形態３のよ
うに気圧高度センサ４で検出した高度を観測モデルにお
ける高度Ｈ_Riとしたが、ＧＰＳ受信機３で検出した高度
を用いても、カルマンフィルタ処理により、最適な車両
の速度Ｖ_i、姿勢角β_iおよび高度Ｈ_iを算出することが
できる。

【０１５６】さらに、ＧＰＳ受信機３に、精度の高いＤ
ＧＰＳ受信機やＲＴＫＧＰＳ受信機を用いれば、なお一
層精度の高い速度Ｖ_i、姿勢角β_iおよび高度Ｈ_iを算出
することができる。

【０１５７】実施の形態５．なお、上記実施の形態２で
は、加速度センサ１、距離センサ２およびＧＰＳ受信機
３のみを用いたが、さらに相対方位センサを用いてもよ
い。

【０１５８】以下、図面を参照しながら、この発明の実
施の形態５について説明する。図１２は、この発明の実
施の形態５の構成を示すブロック図である。

【０１５９】図１２において、前述（図１、図５、図７
および図１０）と同様のものについては、同一符号を付
して、または符号の後に「Ｄ」を付して詳述を省略す
る。

【０１６０】５はジャイロを用いて車両のヨーレート
（車が左右に向きを変えようとする動きの速度）に応じ
た角速度信号を出力する相対方位センサである。

【０１６１】信号処理器６Ｄは、前述の速度計算手段６
１に加えて、相対方位センサ５が出力する信号のオフセ
ットを検出するオフセット検出手段６２と、車両の方位
変化を計算する方位変化計算手段６３と、車両の位置
（λ_i，φ_i，Ｈ_i）および姿勢角β_iを算出する挙動推定
手段６４Ｄとを備えている。

【０１６２】信号処理器６Ｄ内の制御プログラムは、図
１３に示すメインルーチンと、前述（図２参照）の第１
の割込処理と、前述（図３参照）の第２の割込処理と、
前述（図６参照）の第３の割込処理とにより構成されて
いる。

【０１６３】次に、図１２とともに、図２、図３、図６
および図１３のフローチャートを参照しながら、この発
明の実施の形態５による動作について説明する。

【０１６４】なお、第１、第２および第３の割込処理に
ついては、図２、図３および図６とともに前述したとお
りである。

【０１６５】一方、図１３に示すメインルーチンにおい
ては、速度計算手段６１、オフセット検出手段６２、方
位変化計算手段６３および挙動推定手段６４Ｄの動作を
管理している。まず、全ての処理を初期化し（ステップ
Ｓ１３０１）、以下の処理を実行する。

【０１６６】第１の割込処理（図２）でセットされた処
理開始フラグを参照して、すなわち、メインルーチンの
処理を開始するタイミングか否かを判定する（ステップ
Ｓ１３０２）。ステップＳ１３０２において、処理開始
フラグがセットされている（すなわち、ＹＥＳ）と判定
されれば、処理を開始するタイミングであると見なし
て、次回の処理のために処理開始フラグをクリアする
（ステップＳ１３０３）。

【０１６７】一方、ステップＳ１３０２において、処理
開始フラグがセットされていない（すなわち、ＮＯ）と
判定されれば、メインルーチンの処理を開始するタイミ
ングではないと見なし、第１の割込処理内のステップＳ
２０１で処理開始フラグがセット（ステップＳ１３０２
でＹＥＳと判定）されるまで待機する。

【０１６８】ステップＳ１３０３に続いて、速度計算手
段６１の処理（ステップＳ１３０４）を実行する。すな
わち、速度計算手段６１は、以下の（４３）式、（４
４）式により、所定時間△Ｔ毎にカウントした距離セン
サ２の車速パルスカウンタのカウント値△Ｎ_iから、移
動距離Ｌ_iおよび速度Ｖ_iを算出する。

【０１６９】

【数１９】

【０１７０】また、次回の処理のために車速パルスカウ
ンタのカウント値△Ｎ_iを０にする。

【０１７１】次に、（１２）式により、加速度信号Ｓ_i
から車両の姿勢角β_iを算出する（ステップＳ１３０
５）。オフセット検出手段６２の処理は、ステップＳ１
３０４で計算した速度Ｖ_iが０（即ち、停車中）の時、
停車中における所定時間分の角速度信号ω_sgを平均して
角速度信号のオフセットω_0fiを算出する（ステップＳ
１３０６）。

【０１７２】次に、方位変化計算手段６３の処理（ステ
ップＳ１３０７）を実行する。すなわち、方位変化計算
手段６３は、以下の（４５）式により、所定時間△Ｔ毎
に計測した角速度信号ω_sgから方位変化△α_iを算出す
る。

【０１７３】

【数２０】

【０１７４】なお、（４５）式中のＳＦ_{( ω→α )i}は、
角速度信号を方位に変換するスケールファクタである。

【０１７５】次に、第３の割込処理（図６）でセットさ
れたＧＰＳ観測位置信号受信フラグを参照して、すなわ
ち、ＧＰＳ受信機３から位置信号を受信完了したか否か
を判定する（ステップＳ１３０８）。ステップＳ１３０
８において、ＧＰＳ観測位置信号受信フラグがセットさ
れている（すなわち、ＹＥＳ）と判定されれば、ＧＰＳ
受信機３から位置信号を受信完了したと見なして、次回
の処理のためにＧＰＳ観測位置信号受信フラグをクリア
する（ステップＳ１３０９）。

【０１７６】一方、ステップＳ１３０８において、ＧＰ
Ｓ観測位置信号受信フラグがセットされていない（すな
わち、ＮＯ）と判定されれば、ＧＰＳ受信機３から位置
信号を受信完了していないと見なして、ステップＳ１３
０２に戻る。

【０１７７】ステップＳ１３０９に続いて、挙動推定手
段６４Ｄの処理（ステップＳ１３１０）を実行する。こ
こでは、加速度信号、距離信号、位置信号および角速度
信号から、車両の位置（λ_i,φ_i,Ｈ_i）および姿勢角β_i
を算出する。

【０１７８】そして、車両の位置（λ_i,φ_i,Ｈ_i）、姿
勢角β_iおよび速度Ｖ_iを出力する（ステップＳ１３１
１）。最後に、ＧＰＳ受信機３から位置信号を受信する
間の移動距離Ｌ_iと方位変化△α_iとを０にして（ステッ
プＳ１３１２）、ステップＳ１３０２に戻る。

【０１７９】以上のように、離散時刻ｉ毎に車両の位置
（λ_i,φ_i,Ｈ_i）、姿勢角β_iおよび速度Ｖ_iの算出を行
う。

【０１８０】次に、挙動推定手段６４Ｄで適用されてい
る車両挙動の推定方法についてさらに具体的に説明す
る。この推定方法も、前述したカルマンフィルタ処理を
用いる。

【０１８１】まず、車両の移動距離Ｌ_i、進行方位α_i、
算出する位置（λ_i,φ_i,Ｈ_i）および姿勢角β_iの関係
は、以下の（４６）式〜（４８）式のシステムモデルの
ように表される。

【０１８２】

【数２１】

【０１８３】また、ＧＰＳ受信機３で観測した位置情報
（λ_Ri,φ_Ri,Ｈ_Ri）およびシステムモデルの車両位置
（λ_i,φ_i,Ｈ_i）の関係は、以下の（４９）式〜（５
１）式の観測モデルのように表される。

【０１８４】

【数２２】

【０１８５】なお、（４６）式〜（５１）式において、
ｉは離散時刻であり、λ_{i-1 、}φ_i-1およびＨ_i-1は離散時
刻ｉ−１における車両位置の経度、緯度および高度であ
る。また、δΔα_iはΔα_iの誤差である。さらに、δλ
_Ri、δφ_RiおよびδＨ_Riは、ＧＰＳ受信機３の観測位置
λ_Ri、φ_RiおよびＨ_Riの誤差である。

【０１８６】そして、状態方程式は、以下の（１５）式
および（５２）式〜（５５）式のように表される。

【０１８７】

【数２３】

【０１８８】さらに、観測方程式は、以下の（５６）式
〜（５９）式のように表される。

【０１８９】

【数２４】

【０１９０】なお、観測誤差ｖ_iについては、ＧＰＳ受
信機３で観測した位置信号とカルマンフィルタ処理で算
出した位置データ（緯度、経度、高度）との誤差をそれ
ぞれ求め、その最大の誤差（δλ_Ri、δφ_Ri、δＨ_Ri）
とする。また、所定区間におけるＧＰＳ航跡と車両位置
の走行軌跡との移動方向の誤差を方位誤差（システム誤
差ｗ_i）δΔα_iとして算出する。

【０１９１】以上の関係式と、前述の（２４）式〜（２
８）式に示すカルマンフィルタ方程式によって、車両の
位置および姿勢角の予測をする。

【０１９２】そして、離散時刻ｉ毎に車両の位置（λ_i,
φ_i,Ｈ_i）と姿勢角β_iとの推定値を算出する。さらに、
それぞれの誤差やカルマン方程式の係数となるカルマン
ゲインの算出を行う。

【０１９３】このように、カルマンフィルタ処理によっ
て、離散時刻ｉ毎に車両の位置（λ _i,φ_i,Ｈ_i）および
姿勢角β_iの推定を行うことによって、車両の３次元位
置と姿勢角の推定値の精度が高くなる。

【０１９４】なお、前述の（４６）式〜（４８）式のシ
ステムモデルにおいて、既知の進行方位α_iを用いて緯
度λ_iおよび経度φ_iを算出した。この他に、離散時刻ｉ
−１における進行方位α_i-1に方位変化Δα_iを加算し
て、離散時刻ｉにおける進行方位α_iを算出するような
モデルに対応した状態方程式および観測方程式であって
も、カルマンフィルタ処理により、最適な車両の３次元
位置および姿勢角を得ることができる。

【０１９５】実施の形態６．以下、図面を参照しなが
ら、この発明の実施の形態６について説明する。図１４
は、この発明の実施の形態６の構成を示す図である。

【０１９６】図１４において、前述（図１、図５、図
７、図１０および図１２）と同様のものについては、同
一符号を付して、または符号の後に「Ｅ」を付して詳述
を省略する。

【０１９７】信号処理器６Ｅは、前述の速度計算手段６
１と、前述のオフセット検出手段６２と、前述の方位変
化計算手段６３と、車両の位置（λ_i，φ_i）および方位
α_iを算出する挙動推定手段６４Ｅとから構成される。

【０１９８】信号処理器６Ｅ内の制御プログラムは、前
述（図１３参照）のメインルーチンと、前述（図２参
照）の第１の割込処理と、前述（図３参照）の第２の割
込処理と、前述（図６参照）の第３の割込処理とにより
構成されている。

【０１９９】次に、図１４とともに図２、図３、図６お
よび図１３のフローチャートを参照しながら、この発明
の実施の形態６による動作について説明する。

【０２００】なお、第１、第２および第３の割込処理に
ついては、図２、図３および図６とともに前述したとお
りである。同様にメインルーチンについては、図１３と
ともに前述したとおりである。ただし、図１３内のＳ１
３１０〜Ｓ１３１２の一部動作のみが異なる。

【０２０１】なお、メインルーチン（図１３参照）にお
いては、前述と同様に、信号処理器６Ｅの速度計算手段
６１、オフセット検出手段６２、方位変化計算手段６３
および挙動推定手段６４Ｅの動作を管理する。

【０２０２】すなわち、図１３内のステップＳ１３０１
〜Ｓ１３０９においては、前述と同様に動作し、ステッ
プＳ１３１０（挙動推定手段６４Ｅ）においては、加速
度信号、距離信号、位置信号および角速度信号から、車
両の位置（λ_i,φ_i）と進行方位α_iとを算出する。

【０２０３】次に、ステップＳ１３１１において、車両
の位置（λ_i,φ_i）、進行方位α_iおよび速度Ｖ_iを出力
する。最後に、ステップＳ１３１２において、ＧＰＳ受
信機３から位置信号を受信する間の移動距離Ｌ_iと方位
変化△α_iとを０にしてステップＳ１３０２に戻る。

【０２０４】次に、挙動推定手段６４Ｅで適用されてい
る挙動推定方法について説明する。この推定方法におい
ても前述と同様に、カルマンフィルタ処理が用いられ
る。

【０２０５】まず、車両の移動距離Ｌ_iと方位変化△α_i
と、算出する位置（λ_i,φ_i）と進行方位α_iとの関係
は、以下の（６０）式〜（６５）式のシステムモデルで
表される。

【０２０６】

【数２５】

【０２０７】さらに、ＧＰＳ受信機３で観測した位置情
報（λ_Ri,φ_Ri）と、システムモデルの車両位置（λ_i,
φ_i）との関係は、以下の（６６）式、（６７）式の観
測モデルで表される。

【０２０８】

【数２６】

【０２０９】なお、（６０）式〜（６７）式において、
ｉは離散時刻であり、λ_i-1およびφ_i-1は離散時刻ｉ−
１における車両位置の経度および緯度である。また、α
_i-1は離散時刻ｉ−１における車両の進行方位である。
ＳＦ_{d →λ}およびＳＦ_{d →φ}は経度方向の移動量の単位を
距離から経度および緯度へ変換する係数である。また、
δλ_iおよびδφ_iは車両位置の経度および緯度の各誤差
である。さらに、δΔα_iはΔα_iの誤差であり、δＬ_i
はＬ_iの誤差である。また、δλ_Riおよびδφ_RiはＧＰ
Ｓ受信機３の観測位置λ_Riおよびφ_Riの誤差である。

【０２１０】状態方程式は、以下の（１５）式および
（６８）式〜（７１）式のように表される。

【０２１１】

【数２７】

【０２１２】さらに、観測方程式は、以下の（２０）式
および（７２）式〜（７４）式のように表される。

【０２１３】

【数２８】

【０２１４】なお、観測誤差ｖ_iについては、ＧＰＳ受
信機３から出力される位置信号とカルマンフィルタ処理
で算出した位置データ（緯度、経度）との誤差を求め、
その最大の誤差をＧＰＳ受信機３から出力される位置信
号の誤差（δλ_Ri、δφ_Ri）とし、この誤差（δλ_Ri、
δφ_Ri）を観測誤差ｖ_iとする。

【０２１５】距離信号の距離の誤差δＬ_iは、加速度セ
ンサ１で検出した車両の姿勢角をβ_iとすると、実際の
地図水平面上の移動距離はＬｃｏｓβ_iであるので、Ｌ_i
(１−ｃｏｓβ_i）となる。また、所定区間におけるＧ
ＰＳ航跡と車両位置の走行軌跡との移動方向の差を算出
して、方位誤差δΔα_iとする。以上のδＬ_iとδΔα_i
とをシステム誤差ｗ_iとする。

【０２１６】以上の関係式と、前述の（２４）式〜（２
８）式に示すのカルマンフィルタ方程式によって、車両
の位置（λ_i,φ_i）および進行方位α_iを算出（推定）す
る。

【０２１７】このように、カルマンフィルタ処理で離散
時刻ｉ毎に車両の位置（λ_i,φ_i）および進行方位α_iの
推定を行うことによって、車両の位置および進行方位の
推定値の精度が高くなる。

【０２１８】また、ここでは、車両の姿勢角β_iにより
推定される地図水平面上の距離誤差δＬ_iを（１５）式
および（６８）式〜（７１）式の状態方程式で用いた
が、（６０）式〜（６５）式のシステムモデルにおい
て、車両の姿勢角β_iにより緯度φ_iおよび経度λ_iを更
新する差分方程式で状態方程式を導出するようにして
も、カルマンフィルタ処理により最適な車両の２次元位
置および進行方位を得ることができる。

【０２１９】

【発明の効果】以上のように、この発明によれば、車両
の前後方向加速度に対応した加速度信号を出力する加速
度センサと、車両の移動距離に対応した距離信号を出力
する距離センサと、加速度信号および距離信号に基づい
てカルマンフィルタ処理を行い、車両の姿勢角を離散時
刻毎に算出する信号処理器とを備えたので、加速度セン
サの出力電圧に重畳されている車両の移動に伴う前後方
向加速度成分と、車両の傾斜に伴う重力の前後方向加速
度成分を分離し、移動時や停車時などに係わらず、カル
マンフィルタ処理で精度の高い姿勢角を求めることので
きる車両挙動計測装置が得られる効果がある。

【０２２０】また、この発明によれば、信号処理器は、
加速度信号および距離信号から算出された車両の加速度
および姿勢角と加速度センサに含まれるスケールファク
タとに基づいて離散時刻毎に加速度信号のバイアスを補
正するので、車両の移動時や停車時などに係わらず、加
速度センサの加速度信号のバイアスを補正することがで
き、精度の高い車両の姿勢角を求め続けることのできる
車両挙動計測装置が得られる効果がある。

【０２２１】また、この発明によれば、車両の高度に対
応した高度信号を出力する高度計を備え、信号処理器
は、加速度信号、距離信号および高度信号に基づいてカ
ルマンフィルタ処理を行い、離散時刻毎に姿勢角および
高度の少なくとも一方を算出するので、加速度センサの
出力電圧に重畳されている車両の移動に伴う前後方向加
速度成分と、車両の傾斜に伴う重力の前後方向加速度成
分を分離して、車両の移動時や停車時に係わらずカルマ
ンフィルタ処理により精度の高い車両の姿勢角と高度と
を求めることのできる車両挙動計測装置が得られる効果
がある。

【０２２２】また、この発明によれば、信号処理器は、
高度信号と車両の加速度と加速度センサに含まれるスケ
ールファクタとに基づいて離散時刻毎に加速度信号のバ
イアスを補正するので、車両の移動時や停車時に係わら
ず加速度センサの加速度信号のバイアスを補正でき、精
度の高い車両の姿勢角と高度とを求め続けることのでき
る車両挙動計測装置が得られる効果がある。

【０２２３】また、この発明によれば、信号処理器は、
距離信号および姿勢角から算出された車両の高度方向の
移動量と、高度信号の変化量とを比較して、カルマンフ
ィルタ処理による高度の観測誤差を算出するので、高度
計が計測した高度の誤差を精度高く算出することで、カ
ルマンフィルタ処理により、さらに最適な車両の姿勢角
と高度とを求めることのできる車両挙動計測装置が得ら
れる効果がある。

【０２２４】また、この発明によれば、高度計は、大気
圧に対応した気圧信号を出力する気圧高度センサからな
り、信号処理器は、距離信号から算出された車両の速度
と気圧信号から算出された高度差とに基づいて離散時刻
毎に加速度信号のバイアスを補正し、加速度信号、距離
信号および気圧信号に基づいてカルマンフィルタ処理を
行い、離散時刻毎に車両の姿勢角および高度の少なくと
も一方を算出するので、加速度センサの加速度信号のバ
イアスを車両の移動時や停車時に係わらず補正でき、精
度の高い車両の姿勢角、高度および速度を求め続けるこ
とのできる車両挙動計測装置が得られる効果がある。さ
らに、高度計として気圧高度センサを用いたので、ＧＰ
Ｓ衛星が放射した電波を受信できない構造物内もしくは
上空が閉ざされている道路、例えばトンネルや高架下で
あっても加速度センサの加速度信号のバイアスを補正で
き、どこでも精度の高い車両の姿勢角と高度とを求め続
けることのできる車両挙動計測装置が得られる効果があ
る。

【０２２５】また、この発明によれば、高度計は、車両
の高度を含む３次元位置に対応した位置信号を出力する
ＧＰＳ受信機からなり、信号処理器は、加速度信号、距
離信号および位置信号に基づいてカルマンフィルタ処理
を行い、離散時刻毎に車両の姿勢角および高度の少なく
とも一方を算出するので、加速度センサの出力電圧に重
畳されている車両の移動に伴う前後方向加速度成分と車
両の傾斜に伴う重力の前後方向加速度成分とを分離し
て、車両の移動時や停車時に係わらずカルマンフィルタ
処理により精度の高い車両の姿勢角と高度とを求めるの
できる車両挙動計測装置が得られる効果がある。

【０２２６】また、この発明によれば、大気圧に対応し
た気圧信号を出力する気圧高度センサを備え、信号処理
器は、位置信号と気圧信号とを比較してカルマンフィル
タ処理による高度の観測誤差を算出し、加速度信号、距
離信号、位置信号および気圧信号に基づいてカルマンフ
ィルタ処理を行い、離散時刻毎に車両の姿勢角および高
度の少なくとも一方を算出するので、気圧高度センサが
計測した高度に含まれる大気圧変動に伴う高度の誤差、
ＧＰＳ受信機が計測した高度に含まれる誤差、および加
速度センサと距離センサの出力信号から算出した高度方
向移動量の積算誤差の、それぞれの誤差に対応した最適
な車両の姿勢角と高度とを求めることのできる車両挙動
計測装置が得られる効果がある。

【０２２７】また、この発明によれば、車両の角速度に
対応した角速度信号を出力する相対方位センサを備え、
信号処理器は、加速度信号、距離信号、位置信号および
角速度信号に基づいてカルマンフィルタ処理を行い、離
散時刻毎に車両の３次元位置および姿勢角を算出するの
で、坂道走行中の移動距離を水平面に投影したときの位
置ずれを、緯度、経度、および高度方向で移動の整合が
図れる様に最適に修正して、より精度の高い車両の３次
元位置を求めることのできる車両挙動計測装置が得られ
る効果がある。

【０２２８】また、この発明によれば、車両の角速度に
対応した角速度信号を出力する相対方位センサを備え、
信号処理器は、車両の姿勢角および移動距離から算出さ
れる水平面の移動距離と、車両の移動距離とを比較して
システム誤差を算出し、加速度信号、距離信号、位置信
号および角速度信号に基づいてカルマンフィルタ処理を
行い、離散時刻毎に車両の２次元位置および進行方位を
算出するので、坂道走行中の移動距離を水平面に投影し
たときの位置ずれを、緯度、経度方向で移動の整合が図
れる様に最適に修正して、より精度の高い車両の２次元
位置を求めることのできる車両挙動計測装置が得られる
効果がある。

【図面の簡単な説明】

【図１】 この発明の実施の形態１の構成を示すブロッ
ク図である。

【図２】 この発明の実施の形態１による動作の一部を
示すフローチャートである。

【図３】 この発明の実施の形態１による動作の一部を
示すフローチャートである。

【図４】 この発明の実施の形態１による動作の一部を
示すフローチャートである。

【図５】 この発明の実施の形態２の構成を示すブロッ
ク図である。

【図６】 この発明の実施の形態２による動作の一部を
示すフローチャートである。

【図７】 この発明の実施の形態３の構成を示すブロッ
ク図である。

【図８】 この発明の実施の形態３に係る大気圧と電圧
との関係を示す特性図である。

【図９】 この発明の実施の形態３に係る大気圧と高度
との関係として示す説明図（大気圧・高度換算表）であ
る。

【図１０】 この発明の実施の形態４の構成を示すブロ
ック図である。

【図１１】 この発明の実施の形態４による動作の一部
を示すフローチャートである。

【図１２】 この発明の実施の形態５の構成を示すブロ
ック図である。

【図１３】 この発明の実施の形態５による動作の一部
を示すフローチャートである。

【図１４】 この発明の実施の形態６の構成を示すブロ
ック図である。

【符号の説明】

１ 加速度センサ、２ 距離センサ、３ ＧＰＳ受信
機、４ 気圧高度センサ、５ 相対方位センサ、６、６
Ａ〜６Ｅ 信号処理器、６１ 速度計算手段、６２ オ
フセット検出手段、６３ 方位変化計算手段、６４、６
４Ａ〜６４Ｅ 挙動推定手段。

───────────────────────────────────────────────────── フロントページの続き (72)発明者 下谷 光生 東京都千代田区丸の内二丁目２番３号 三 菱電機株式会社内 Ｆターム(参考） 2F029 AA02 AB01 AB07 AC03 AC04 AC14 AD03 3D037 FA15 FA21 FA23 5H180 AA01 CC27 CC30 FF04 FF07 FF22 FF27 FF32

Claims (10)

【特許請求の範囲】
1. 【請求項１】 車両の前後方向加速度に対応した加速度
   信号を出力する加速度センサと、 前記車両の移動距離に対応した距離信号を出力する距離
   センサと、 前記加速度信号および前記距離信号に基づいてカルマン
   フィルタ処理を行い、前記車両の姿勢角を離散時刻毎に
   算出する信号処理器とを備えた車両挙動計測装置。
2. 【請求項２】 前記信号処理器は、 前記加速度信号および前記距離信号から算出された前記
   車両の加速度および姿勢角と前記加速度センサに含まれ
   るスケールファクタとに基づいて前記離散時刻毎に前記
   加速度信号のバイアスを補正することを特徴とする請求
   項１に記載の車両挙動計測装置。
3. 【請求項３】 前記車両の高度に対応した高度信号を出
   力する高度計を備え、 前記信号処理器は、前記加速度信号、前記距離信号およ
   び前記高度信号に基づいてカルマンフィルタ処理を行
   い、前記離散時刻毎に姿勢角および高度の少なくとも一
   方を算出することを特徴とする請求項１に記載の車両挙
   動計測装置。
4. 【請求項４】 前記信号処理器は、 前記高度信号と前記車両の加速度と前記加速度センサに
   含まれるスケールファクタとに基づいて前記離散時刻毎
   に前記加速度信号のバイアスを補正することを特徴とす
   る請求項３に記載の車両挙動計測装置。
5. 【請求項５】 前記信号処理器は、 前記距離信号および前記姿勢角から算出された前記車両
   の高度方向の移動量と、前記高度信号の変化量とを比較
   して、前記カルマンフィルタ処理による高度の観測誤差
   を算出することを特徴とする請求項３に記載の車両挙動
   計測装置。
6. 【請求項６】 前記高度計は、大気圧に対応した気圧信
   号を出力する気圧高度センサからなり、 前記信号処理器は、前記距離信号から算出された前記車
   両の速度と前記気圧信号から算出された高度差とに基づ
   いて離散時刻毎に前記加速度信号のバイアスを補正し、
   前記加速度信号、前記距離信号および前記気圧信号に基
   づいてカルマンフィルタ処理を行い、前記離散時刻毎に
   前記車両の姿勢角および高度の少なくとも一方を算出す
   ることを特徴とする請求項３から請求項５までのいずれ
   かに記載の車両挙動計測装置。
7. 【請求項７】 前記高度計は、前記車両の高度を含む３
   次元位置に対応した位置信号を出力するＧＰＳ受信機か
   らなり、 前記信号処理器は、前記加速度信号、前記距離信号およ
   び前記位置信号に基づいてカルマンフィルタ処理を行
   い、前記離散時刻毎に前記車両の姿勢角および高度の少
   なくとも一方を算出することを特徴とする請求項３から
   請求項５までのいずれかに記載の車両挙動計測装置。
8. 【請求項８】 大気圧に対応した気圧信号を出力する気
   圧高度センサを備え、 前記信号処理器は、前記位置信号と前記気圧信号とを比
   較して前記カルマンフィルタ処理による高度の観測誤差
   を算出し、前記加速度信号、前記距離信号、前記位置信
   号および前記気圧信号に基づいてカルマンフィルタ処理
   を行い、前記離散時刻毎に前記車両の姿勢角および高度
   の少なくとも一方を算出することを特徴とする請求項７
   に記載の車両挙動計測装置。
9. 【請求項９】 前記車両の角速度に対応した角速度信号
   を出力する相対方位センサを備え、 前記信号処理器は、前記加速度信号、前記距離信号、前
   記位置信号および前記角速度信号に基づいてカルマンフ
   ィルタ処理を行い、前記離散時刻毎に前記車両の３次元
   位置および姿勢角を算出することを特徴とする請求項７
   に記載の車両挙動計測装置。
10. 【請求項１０】 車両の角速度に対応した角速度信号を
    出力する相対方位センサを備え、 前記信号処理器は、前記車両の姿勢角および移動距離か
    ら算出される水平面の移動距離と、車両の移動距離とを
    比較してシステム誤差を算出し、前記加速度信号、前記
    距離信号、前記位置信号および前記角速度信号に基づい
    てカルマンフィルタ処理を行い、前記離散時刻毎に前記
    車両の２次元位置および進行方位を算出することを特徴
    とする請求項７に記載の車両挙動計測装置。