特許 6008510 計測装置及び計測方法

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

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

(11)【特許番号】6008510

(24)【登録日】2016年9月23日

(45)【発行日】2016年10月19日

(54)【発明の名称】計測装置及び計測方法

(51)【国際特許分類】

   G01S 19/52 20100101AFI20161006BHJP

   G01S 19/40 20100101ALI20161006BHJP

   G01S 19/54 20100101ALI20161006BHJP

   G01C 21/28 20060101ALI20161006BHJP

【ＦＩ】

   G01S19/52

   G01S19/40

   G01S19/54

   G01C21/28

【請求項の数】2

【全頁数】20

(21)【出願番号】特願2012-34439(P2012-34439)

(22)【出願日】2012年2月20日

(65)【公開番号】特開2013-170904(P2013-170904A)

(43)【公開日】2013年9月2日

【審査請求日】2014年12月4日

(73)【特許権者】

【識別番号】000145806

【氏名又は名称】株式会社小野測器

(74)【代理人】

【識別番号】100106002

【弁理士】

【氏名又は名称】正林 真之

(74)【代理人】

【識別番号】100120891

【弁理士】

【氏名又は名称】林 一好

(72)【発明者】

【氏名】市川 和宏

(72)【発明者】

【氏名】五十嵐 英昭

【審査官】 中村 説志

(56)【参考文献】

【文献】 特開平０７−２３９２３６（ＪＰ，Ａ）

【文献】 特開平０８−０６８６２４（ＪＰ，Ａ）

【文献】 特開平０７−０３５５５８（ＪＰ，Ａ）

【文献】 特開２００４−２３９６４３（ＪＰ，Ａ）

【文献】 特開２０１０−０９６６４７（ＪＰ，Ａ）

【文献】 特開２００１−１９４４４２（ＪＰ，Ａ）

【文献】 特許第５１１８１７７（ＪＰ，Ｂ２）

【文献】 五十嵐英昭，“ＧＰＳとＩＭＵを融合した高精度車速計”，自動車技術，日本，公益社団法人自動車技術会，２０１１年 ７月 １日，Vol.65,No.7，p.108-112

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

Ｇ０１Ｓ ５／００− ５／１４

Ｇ０１Ｓ１９／００−１９／５５

Ｇ０１Ｃ２１／００−２５／００

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

【請求項1】

第１のアンテナ及び第２のアンテナにより構成されるＧＰＳ受信機により受信した信号に基づいて、ＧＰＳ衛星から出力される搬送波の周波数のドップラーシフト量から地球座標上における移動体の速度（ドップラー速度）を計測する移動体速度計測部と、前記移動体の進行方向の軸線上に、ＧＰＳ衛星から出力される信号を受信する前記第１のアンテナと前記第２のアンテナが所定の距離を置いて配置されており、前記第１のアンテナ及び前記第２のアンテナにより受信した信号に基づいてそれぞれのアンテナ地点における搬送波位相を計測し、計測した搬送波位相に基づいて、前記第１のアンテナと前記第２のアンテナの相対位置を算出する相対位置算出部と、前記相対位置算出部により算出した相対位置に基づいて、前記移動体の絶対姿勢角を計測する姿勢角計測部と、ＩＭＵの検出値に基づいて、加速度及び角速度を計測する加速度・角速度計測部と、前記移動体の速度と、前記加速度及び角速度に基づいて、ストラップダウン演算を行うことにより、リアルタイムな前記移動体の姿勢角及び速度を算出する速度・姿勢角算出部を備える計測装置において、
前記速度・姿勢角算出部は、
前記加速度及び角速度に基づいて、ストラップダウン演算を行い、算出値として補正速度、補正位置、補正姿勢角、補正３軸加速度及び補正３軸角速度を算出するストラップダウンナビゲータ部と、
前記移動体速度計測部及び前記姿勢角計測部のサンプリング周波数を前記加速度・角速度計測部のサンプリング周波数に合わせるため、前記移動体速度計測部及び前記姿勢角計測部の現在の計測値と過去のサンプリングから線形補間処理を行い、前記加速度・角速度計測部と同じサンプリング周波数にする処理を行う補間処理部と、
前記補間処理部により線形補間処理された前記移動体の速度（ドップラー速度）に基づいて、加速度を算出し、算出した加速度と、前記加速度・角速度計測部により計測された加速度との差分を算出し、当該差分に基づく係数αを算出する良否判定部と、
前記ストラップダウンナビゲータ部の出力と、前記補間処理部により線形補間処理された速度及び姿勢角との間の同期ずれを、線形補間処理された速度及び姿勢角の予測処理を行うことで同期させ、前記良否判定部によって係数αが算出されるための時間だけ、移動体速度計測部によって測定された移動体の速度（ドップラー速度）の演算に用いる時間を遅延させる同期ずれ補正部と、
前記ストラップダウンナビゲータ部によるストラップダウン演算により得られた補正速度、補正位置、補正姿勢角を一定時間遅延させる同期化処理部と、
前記同期ずれ補正部により同期補正及び遅延させた移動体の速度（ドップラー速度）、姿勢角、位置と、前記同期化処理部で同期遅延させた補正速度、補正位置、補正姿勢角とを減算して誤差量δｘを算出する減算部と、
前記良否判定部により算出された係数αと、前記減算部の誤差量δｘを乗算する乗算部と、
前記乗算部によって前記係数αが乗算された誤差量δｘから前記補正速度、補正位置、補正姿勢角、補正３軸加速度及び補正３軸角速度に対する調整量を推定演算する状態推定部と、を備え、
前記ストラップダウンナビゲータ部は、前記加速度・角速度計測部により計測された加速度及び角速度に基づいて、ストラップダウン演算を行い位置、速度、姿勢角を算出し、さらに前記状態推定部によって推定演算された調整量により当該算出した値を補正して、補正速度、補正位置、補正姿勢角、補正３軸加速度及び補正３軸角速度を算出し、
前記状態推定部は、前記ＩＭＵの座標軸に対する前記第１のアンテナと第２のアンテナとの並びの軸のずれである取り付け角度オフセット誤差変数と前記ＩＭＵと前記ＧＰＳ受信機の第１のアンテナとの取り付け位置誤差変数が加えられた状態方程式を用いて、前記乗算部によって前記係数αが乗算された誤差量δｘから前記補正速度、補正位置、補正姿勢角、補正３軸加速度及び補正３軸角速度に対する調整量をリアルタイムに正確に推定演算する計測装置。

【請求項2】

第１のアンテナ及び第２のアンテナにより構成されるＧＰＳ受信機により受信した信号に基づいて、ＧＰＳ衛星から出力される搬送波の周波数のドップラーシフト量から地球座標上における移動体の速度（ドップラー速度）を計測する移動体速度計測工程と、前記移動体の進行方向の軸線上に、ＧＰＳ衛星から出力される信号を受信する前記第１のアンテナと前記第２のアンテナが所定の距離を置いて配置されており、前記第１のアンテナ及び前記第２のアンテナにより受信した信号に基づいてそれぞれのアンテナ地点における搬送波位相を計測し、計測した搬送波位相に基づいて、前記第１のアンテナと前記第２のアンテナの相対位置を算出する相対位置算出工程と、前記相対位置算出工程により算出した相対位置に基づいて、前記移動体の絶対姿勢角を計測する姿勢角計測工程と、ＩＭＵの検出値に基づいて、加速度及び角速度を計測する加速度・角速度計測工程と、前記移動体の速度と、前記加速度及び角速度に基づいて、ストラップダウン演算を行うことにより、リアルタイムな前記移動体の姿勢角及び速度を算出する速度・姿勢角算出工程を備える計測方法において、
前記速度・姿勢角算出工程は、
前記加速度及び角速度に基づいて、ストラップダウン演算を行い、算出値として補正速度、補正位置、補正姿勢角、補正３軸加速度及び補正３軸角速度を算出するストラップダウンナビゲータ工程と、
前記移動体速度計測工程及び前記姿勢角計測工程のサンプリング周波数を前記加速度・角速度計測工程のサンプリング周波数に合わせるため、前記移動体速度計測工程及び前記姿勢角計測工程の現在の計測値と過去のサンプリングから線形補間処理を行い、前記加速度・角速度計測工程と同じサンプリング周波数にする処理を行う補間処理工程と、
前記補間処理工程により線形補間処理された前記移動体の速度（ドップラー速度）に基づいて、加速度を算出し、算出した加速度と、前記加速度・角速度計測工程により計測された加速度との差分を算出し、当該差分に基づく係数αを算出する良否判定工程と、
前記ストラップダウンナビゲータ工程の出力と、前記補間処理工程により線形補間処理された速度及び姿勢角との間の同期ずれを、線形補間処理された速度及び姿勢角の予測処理を行うことで同期させ、前記良否判定工程によって係数αが算出されるための時間だけ、移動体速度計測工程によって測定された移動体の速度（ドップラー速度）の演算に用いる時間を遅延させる同期ずれ補正工程と、
前記ストラップダウンナビゲータ工程によるストラップダウン演算により得られた補正速度、補正位置、補正姿勢角を一定時間遅延させる同期化処理工程と、
前記同期ずれ補正工程により同期補正及び遅延させた移動体の速度（ドップラー速度）、姿勢角、位置と、前記同期化処理工程で同期遅延させた補正速度、補正位置、補正姿勢角とを減算して誤差量δｘを算出する減算工程と、
前記良否判定工程により算出された係数αと、前記減算工程の誤差量δｘを乗算する乗算工程と、
前記乗算工程によって前記係数αが乗算された誤差量δｘから前記補正速度、補正位置、補正姿勢角、補正３軸加速度及び補正３軸角速度に対する調整量を推定演算する状態推定工程と、を備え、
前記ストラップダウンナビゲータ工程は、前記加速度・角速度計測工程により計測された加速度及び角速度に基づいて、ストラップダウン演算を行い位置、速度、姿勢角を算出し、さらに前記状態推定工程によって推定演算された調整量により当該算出した値を補正して、補正速度、補正位置、補正姿勢角、補正３軸加速度及び補正３軸角速度を算出し、
前記状態推定工程は、前記ＩＭＵの座標軸に対する前記第１のアンテナと第２のアンテナとの並びの軸のずれである取り付け角度オフセット誤差変数と前記ＩＭＵと前記ＧＰＳ受信機の第１のアンテナとの取り付け位置誤差変数が加えられた状態方程式を用いて、前記乗算工程によって前記係数αが乗算された誤差量δｘから前記補正速度、補正位置、補正姿勢角、補正３軸加速度及び補正３軸角速度に対する調整量をリアルタイムに正確に推定演算する計測方法。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、移動体の速度等を計測する計測装置及び計測方法に関する。

【背景技術】

【0002】

従来から、光学式センサを移動体の前進方向と横方向に２個取り付けて、移動体の横速度や横滑り角を検出する技術がある。また、慣性センサから得られる横滑り角速度とヨー角速度を加算した値の積分値とＧＰＳ受信機による相対方位角との差分から横滑り角を算出する技術も提案されている（例えば、特許文献１を参照）。さらに、２個のＧＰＳアンテナと、１軸加速度センサと、１軸角速度センサを使用して横滑り角を測定する技術も提案されている（例えば、特許文献２を参照）。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開２００５−１７１９１号公報

【特許文献2】特開２００７−３４６１号公報

【発明の概要】

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

【0004】

ところで、ＧＰＳ受信機と、３軸加速度等を検出するＩＭＵを併用して移動体の横滑り角等を検出する場合、ＧＰＳ受信機側とＩＭＵ側との間で時間同期ができていないと、精度が向上せず、正確な応答時間にならない問題がある。
例えば、ＧＰＳ受信機側は、実時間を基準にデータを２０［Ｈｚ］でサンプリングする。この２０［Ｈｚ］という周期は、絶対時間に対して正確なものである。しかし、ＧＰＳ受信機側の出力Ｉ／ＦがＵＡＲＴを採用しているため、メッセージ長の違いにより、５０［ｍｓ］±２〜３［ｍｓ］程度のジッタが生じてしまう。

【0005】

一方、ＩＭＵ側は、水晶発振器の精度を基準に１００［Ｈｚ］でサンプリングする。この１００［Ｈｚ］という周期は、水晶発振器に依存している（６０［ｐｐｍ］程度）。よって、周期は、絶対時間に対して正確ではない。具体的には、数百秒で数［ｍｓ］程度の累積誤差が生じる。
よって、単に従来技術を利用して、同期あわせを行っても、ＧＰＳ受信機側とＩＭＵ側を同期させることが困難になる。

【0006】

本発明は、上述した問題点を解決し、同期のための特別なハードウェア等を使用せずに、簡易なアルゴリズムでＧＰＳ受信機側とＩＭＵ側の同期をとることができる計測装置及び計測方法を提供することを１つの目的とする。

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

【0007】

本発明では、以下のような解決手段を提供する。
（１）計測装置は、ＧＰＳ受信機により受信した信号に基づいて、移動体の速度を計測する移動体速度計測部と、ＩＭＵの検出値に基づいて、加速度及び角速度を計測する加速度・角速度計測部と、移動体の速度と、加速度及び角速度に基づいて、ストラップダウン演算を行うことにより、リアルタイムな移動体の姿勢角及び速度を算出する速度・姿勢角算出部を備える計測装置において、速度・姿勢角算出部は、加速度及び角速度に基づいて、ストラップダウン演算を行い、算出値として補正速度、補正位置、補正姿勢角、補正加速度及び補正角速度を算出するストラップダウンナビゲータ部と、移動体速度計測部のサンプリング周波数が加速度・角速度計測部のサンプリング周波数に一致するように、移動体速度計測部により計測した移動体の速度の波形を線形補間処理する補間処理部と、補間処理部により線形補間処理された移動体の速度に基づいて、加速度を算出し、算出した加速度と、加速度・角速度計測部により計測された加速度との差分を算出し、当該差分に基づく係数を算出する良否判定部と、移動体速度計測部と加速度・角速度計測部の計測タイミングのずれ時間の分だけ、補間処理部により線形補間処理された移動体の速度の位相を進ませる補正を行う同期ずれ補正部と、ストラップダウンナビゲータ部によるストラップダウン演算により得られた補正速度を一定時間遅延させる同期化処理部と、同期ずれ補正部から得られる速度と同期化処理部より得られる補正速度の誤差量を算出する減算部と、良否判定部により算出された係数と、減算部の誤差量を乗算する乗算部と、乗算部によって係数が乗算された誤差量から補正速度に対する調整量を推定演算する状態推定部と、を備え、ストラップダウンナビゲータ部は、加速度・角速度計測部により計測された加速度及び角速度に対してストラップダウン演算を行い位置、速度、姿勢角を算出し、さらに状態推定部によって推定演算された調整量により当該算出した値を補正し、状態推定部は、ＧＰＳ受信機とＩＭＵの角度オフセット誤差変数と、取り付け位置誤差変数が加えられた状態方程式を用いて、乗算部によって係数が乗算された誤差量から補正速度、補正位置、補正姿勢角、補正加速度及び補正角速度に対する調整量をリアルタイムに正確に推定演算する構成である。

【0008】

このようにして、計測装置は、同期のための特別なハードウェア等を使用せずに、簡易なアルゴリズムでＧＰＳ受信機側とＩＭＵ側の同期をとることができる。

【0009】

（２）計測方法は、ＧＰＳ受信機により受信した信号に基づいて、移動体の速度を計測する移動体速度計測工程と、ＩＭＵの検出値に基づいて、加速度及び角速度を計測する加速度・角速度計測工程と、移動体の速度と、加速度及び角速度に基づいて、ストラップダウン演算を行うことにより、リアルタイムな移動体の姿勢角及び速度を算出する速度・姿勢角算出工程を備える計測方法において、速度・姿勢角算出工程は、加速度及び角速度に基づいて、ストラップダウン演算を行い、算出値として補正速度、補正位置、補正姿勢角、補正加速度及び補正角速度を算出するストラップダウンナビゲータ工程と、移動体速度計測工程のサンプリング周波数が加速度・角速度計測工程のサンプリング周波数に一致するように、移動体速度計測工程により計測した移動体の速度の波形を線形補間処理する補間処理工程と、補間処理工程により線形補間処理された移動体の速度に基づいて、加速度を算出し、算出した加速度と、加速度・角速度計測工程により計測された加速度との差分を算出し、当該差分に基づく係数を算出する良否判定工程と、移動体速度計測工程と加速度・角速度計測工程の計測タイミングのずれ時間の分だけ、補間処理工程により線形補間処理された移動体の速度の位相を進ませる補正を行う同期ずれ補正工程と、ストラップダウンナビゲータ工程によるストラップダウン演算により得られた補正速度を一定時間遅延させる同期化処理工程と、同期ずれ補正工程から得られる速度と同期化処理工程より得られる補正速度の誤差量を算出する減算工程と、良否判定工程により算出された係数と、減算工程の誤差量を乗算する乗算工程と、乗算工程によって係数が乗算された誤差量から補正速度に対する調整量を推定演算する状態推定工程と、を備え、ストラップダウンナビゲータ工程は、加速度・角速度計測工程により計測された加速度及び角速度に対してストラップダウン演算を行い位置、速度、姿勢角を算出し、さらに状態推定工程によって推定演算された調整量により当該算出した値を補正し、状態推定工程は、ＧＰＳ受信機とＩＭＵの角度オフセット誤差変数と、取り付け位置誤差変数が加えられた状態方程式を用いて、乗算工程によって係数が乗算された誤差量から補正速度、補正位置、補正姿勢角、補正加速度及び補正角速度に対する調整量をリアルタイムに正確に推定演算する構成である。

【0010】

このようにして、計測方法は、同期のための特別なハードウェア等を使用せずに、簡易なアルゴリズムでＧＰＳ受信機側とＩＭＵ側の同期をとることができる。

【発明の効果】

【0011】

同期のための特別なハードウェア等を使用せずに、簡易なアルゴリズムでＧＰＳ受信機側とＩＭＵ側の同期をとることができる。

【図面の簡単な説明】

【0012】

【図1】計測装置の構成を示すブロック図である。

【図2】第１のアンテナと第２のアンテナとＩＭＵを移動体の屋根に配置する際の説明に供する図である。

【図3】キャリブレーション部の具体的な処理についての説明に供する図である。

【図4】速度・姿勢角算出部の第１の構成を示すブロック図である。

【図5】第１のアンテナ及び第２のアンテナと、ＩＭＵの位置関係についての説明に供する図である。

【図6】横滑り角の精度評価結果を示す図である。

【図7】速度・姿勢角算出部の第２の構成を示すブロック図である。

【図8】移動体速度計測部及び姿勢角計測部から入力された信号の波形を示す図である。

【図9】リサンプル処理についての説明に供する図である。

【図10】サンプリングタイミングとずれ時間の計測についての説明に供する図である。

【図11】同期ずれ補正部の構成を示すブロック図である。

【図12】２０［Ｈｚ］ＲＡＷデータと、１００［Ｈｚ］線形補間データと、１００［Ｈｚ］同期ずれ補正データを比較したときの図である。

【図13】リアルタイム同期補正の有無についての説明に供する図である。

【図14】計測装置によって出力されるリアルタイム補間速度と、ＧＰＳ受信機によるドップラー速度とを示す図である。

【図15】計測装置によって出力されるリアルタイム補間速度の立ち上がりと、ＧＰＳ受信機から出力されるドップラー速度の立ち上がりとを示す図である。

【図16】計測装置によって出力されるリアルタイム補間速度と、ＧＰＳ受信機から出力されるドップラー速度と、光学式速度計測装置から出力される速度とを示す図である。

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

【0013】

以下、本発明の実施形態について図を参照しながら説明する。
図１は、本発明の一実施形態である計測装置１の構成を示すブロック図である。計測装置１は、相対位置算出部１２と、姿勢角計測部１３と、キャリブレーション部１４と、移動体速度計測部１５と、加速度・角速度計測部１６と、速度・姿勢角算出部１７と、指定位置座標速度算出部１８と、横速度・横滑り角算出部１９とを備える。

【0014】

相対位置算出部１２は、移動体２の進行方向の軸線上に、ＧＰＳ衛星から出力される信号を受信する第１のアンテナ１０と第２のアンテナ１１が所定の距離置いて配置されており、第１のアンテナ１０及び第２のアンテナ１１により受信した信号に基づいてそれぞれのアンテナ地点における搬送波位相を計測し、計測した搬送波位相に基づいて、第１のアンテナ１０と第２のアンテナ１１の相対位置を算出する。なお、移動体２とは、例えば、車両のことである。

【0015】

具体的には、相対位置算出部１２は、第１のアンテナ１０及び第２のアンテナ１１によってＧＰＳ衛星から出力される電波を受信し、搬送波位相をそれぞれ計測する。相対位置算出部１２は、計測したそれぞれの搬送波位相に基づいて、リアルタイムキネマティクス（ＲＴＫ）測位を実施する。これにより、相対位置算出部１２は、第１のアンテナ１０と第２のアンテナ１１の間の相対位置関係を計測することができる。

【0016】

また、本実施例では、第１のアンテナ１０（マスター側アンテナ）は、移動体２の進行方向において、第２のアンテナ１１（スレーブ側アンテナ）よりも後方に配置されているものとする。また、第１のアンテナ１０と第２のアンテナ１１は、図２に示すように、移動体２の屋根の上等に配置されるものとするが、ＧＰＳ衛星からの電波を受信できる位置であれば、外側に限られない。

【0017】

姿勢角計測部１３は、相対位置算出部１２により算出した相対位置に基づいて、移動体２の絶対姿勢角を計測する。
また、姿勢角計測部１３は、最終的に第１のアンテナ１０と第２のアンテナ１１との間の基線ベクトルが決まるので、地球座標系に対する姿勢角φ_ｎＧＰＳ［ｒａｄ］を求めることができる。姿勢角は、アンテナ間の距離を大きくとることで精度は向上する。参考値として、１ｍで０．１５°ＲＭＳ、２ｍで０．１°ＲＭＳの精度を得ることができる。姿勢角計測部１３は、ＧＰＳ衛星の搬送波を使用して姿勢角を計測するため、移動体速度計測部１５と同期して、５〜１００Ｈｚ程度のサンプリング周波数で計測する。

【0018】

キャリブレーション部１４は、姿勢角計測部１３で計測した絶対姿勢角と、移動体２の進行方向とのずれ量を計算し、当該ずれ量に基づいて絶対姿勢角を調整する。

【0019】

以下に、キャリブレーション部１４の具体的な処理について図３を参照しながら説明する。
移動体２の正確な姿勢角を計測するためには、第１のアンテナ１０と第２のアンテナ１１を移動体２の車両センターラインにあわせること（軸あわせ）が必要である。
手作業であわせることが精度上難しいので、キャリブレーション部１４により、以下の手順で、キャリブレーション（校正）を計測前に行って、軸あわせを行う。

【0020】

【数1】

【0021】

このようにして、キャリブレーション部１４は、平均取り付け角度誤差を使用することで、姿勢角を車両センターラインに合わせることができる。なお、第１のアンテナ１０と第２のアンテナ１１が移動体２の車両センターラインにあっていれば、キャリブレーション部１４による処理は不要となる。この場合には、姿勢角計測部１３により計測された絶対姿勢角は、調整されずに速度・姿勢角算出部１７に供給される。

【0022】

移動体速度計測部１５は、第１のアンテナ１０により受信した信号に基づいて、ＧＰＳ衛星から出力される搬送波の周波数のドップラーシフト量から地球座標上における移動体２の速度（ドップラー速度）を算出する。具体的には、移動体速度計測部１５は、第１のアンテナ１０により受信した信号に基づいて、ＧＰＳ衛星からの搬送波のドップラーシフト周波数を使用して移動体２の速度（ＮＥＤ座標系速度：北、東、下座標）を高精度に計測する。また、移動体速度計測部１５は、５〜１００Ｈｚ程度のサンプリング周波数で移動体２の速度を計測する。また、移動体速度計測部１５は、緯度、経度、標高についても、５〜１００Ｈｚ程度のサンプリング周波数で同期して計測する。なお、第１のアンテナ１０とＧＰＳ衛星との距離が、遠ざかる又は近づくと、第１のアンテナ１０が受信する搬送波の位相は、連続的に変化し、周波数が低くなったり高くなったりする。移動体速度計測部１５は、この周波数の変化量から第１のアンテナ１０が出力する速度を取得する。

【0023】

加速度・角速度計測部１６は、第１のアンテナ１０から所定の距離置いて配置されているモーションセンサ２０により３軸方向の加速度と角速度を計測する。また、加速度・角速度計測部１６は、モーションセンサ２０の設置地点の３軸方向の加速度・角速度を１００〜１ｋＨｚ程度のサンプリング周波数で計測する。なお、モーションセンサ２０は、例えば、３軸方向の加速度及び角速度を計測することができるＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ：慣性計測装置）により構成される。以下では、モーションセンサ２０をＩＭＵ２０という。

【0024】

また、ＩＭＵ２０は、図２に示すように、移動体２の進行方向（図２中のＸ軸方向）に対して、第２のアンテナが配置されている方向に向けて、ルーフ上にマグネット等で取り付ける。また、第１のアンテナ１０及び第２のアンテナ１１も同様に、マグネット等でルーフ上に取り付ける。また、第１のアンテナ１０と第２のアンテナ間の距離を大きくするほど、精度は向上するが、運用上の観点から１ｍ又は２ｍ程度で使用するものとする。

【0025】

速度・姿勢角算出部１７は、キャリブレーション部１４により調整された絶対姿勢角と、移動体速度計測部１５により算出された移動体２の速度と、加速度・角速度計測部１６により計測された加速度及び角速度を利用して、ストラップダウン演算を行うことにより、リアルタイムな移動体２の姿勢角及び速度を算出する。

【0026】

指定位置座標速度算出部１８は、速度・姿勢角算出部１７により算出されたリアルタイムな移動体２の姿勢角及び速度に基づいて、移動体２内において指定された位置座標における速度を算出する。ここで、指定された位置座標とは、例えば、図２に示すように、移動体２の重心（Ｃ．Ｏ．Ｇ、Ｃｅｎｔｅｒ Ｏｆ Ｇｒａｖｉｔｙ）のことである。

【0027】

横速度・横滑り角算出部１９は、速度・姿勢角算出部１７で算出した姿勢角と、指定位置座標速度算出部１８により算出された速度に基づいて、移動体２内において指定された位置座標における横速度及び横滑り角をリアルタイムに算出する。

【0028】

このようにして、計測装置１は、移動体２内において任意に指定された位置における横速度と横滑り角をリアルタイムに正確に算出することができる。

【0029】

＜第１の実施例＞
つぎに、速度・姿勢角算出部１７の構成について、図４を参照しながら説明する。
速度・姿勢角算出部１７は、図４に示すように、ストラップダウンナビゲータ部１００と、良否判定部１０１と、同期化処理部１０２と、乗算部１０３と、状態推定部１０４と、遅延処理部１０５を備え、リアルタイムな高精度の速度と姿勢角を演算する。

【0030】

ストラップダウンナビゲータ部１００（自律航法アルゴリズム）は、加速度・角速度計測部１６により計測された加速度及び角速度に基づいて、ストラップダウン演算を行い、算出値として補正速度（ＮＥＤ方向）、補正位置（緯度、経度、標高）、補正姿勢角、補正３軸加速度及び補正３軸角速度を算出する。詳細には、ストラップダウンナビゲータ部１００は、加速度・角速度計測部１６により計測された加速度及び角速度に対してストラップダウン演算を行い位置、速度、姿勢角等を算出し、さらに状態推定部１０４によって推定演算された調整量でストラップダウン演算によって得られた位置、速度、姿勢角等を補正して出力する。

【0031】

良否判定部１０１は、移動体速度計測部１５により算出された移動体２の速度に基づく加速度と、加速度・角速度計測部１６により計測された加速度との差分を算出し、当該差分に基づく係数を算出する。

【0032】

ここで、良否判定部１０１の詳細について説明する。良否判定部１０１は、移動体速度計測部１５によって測定されたドップラー速度から算出した加速度と、加速度・角速度計測部１６によって計測された加速度との差分を算出し、算出した差分に基づいて係数αを算出する。

【0033】

例えば、良否判定部１０１は、ドップラー速度を微分した加速度と、ＩＭＵ２０が計測した加速度との差分を算出し、算出した差分を二乗し、二乗した差分についてエンベロープ処理を行う。そして、良否判定部１０１は、エンベロープ処理を行った後の差分を示す関数の逆関数を求め、求めた逆関数に基づいて、係数αを算出する。

【0034】

すなわち、良否判定部１０１は、求めた逆関数に基づいて、ドップラー速度がノイズを含んでいないと判断した場合に良判定を行い、ノイズを含んでいると判断した場合に否判定を行って、それぞれの判定を数値化した係数αを算出する。

【0035】

また、良否判定部１０１は、姿勢角計測部１３より計測された（絶対）姿勢角と、ストラップダウンナビゲータ部１００により演算される補正姿勢角の差分を算出し、上述した良否判定方法を適用することにより、姿勢角計測部１３より計測された姿勢角の良否をリアルタイムに良否判定することができる。

【0036】

同期化処理部１０２は、ストラップダウンナビゲータ部１００によるストラップダウン演算により得られた補正速度（リアルタイム補間速度）、補正位置、補正姿勢角と、移動体速度計測部１５から得られたＮＥＤ方向の速度（ドップラー速度）、位置（緯度、経度、標高）、キャリブレーション部１４から得られた姿勢角の同期化処理を行うために、一定時間分これらを遅延させる。

【0037】

遅延処理部１０５は、移動体速度計測部１５により算出されたＧＰＳ衛星のドップラー速度、位置及びキャリブレーション部１４により調整された絶対姿勢角を遅延処理する。
具体的には、遅延処理部１０５は、良否判定部１０１によって係数αが算出されるための時間だけ、移動体速度計測部１５から得られたＮＥＤ方向の速度（ドップラー速度）、位置（緯度、経度、標高）、キャリブレーション部１４から得られた絶対姿勢角を良否判定演算に用いる時間分だけ遅延させる。

【0038】

減算部１０８は、遅延処理部１０５で遅延させた移動体２の速度、姿勢角、位置と、同期化処理部１０２で遅延させたリアルタイム補間速度、補正位置、補正姿勢角とを減算して誤差量δｘを求める。
乗算部１０３は、良否判定部１０１により算出された係数αと、減算部１０８により遅延処理部１０５と同期化処理部１０２とを減算して得られた誤差量δｘを乗算する。

【0039】

状態推定部１０４は、乗算部１０３によって係数αが乗算された誤差量δｘから補正速度、補正位置、補正姿勢角、補正３軸加速度、補正３軸角速度に対する調整量を推定演算する。詳細には、状態推定部１０４は、第１のアンテナ１０とＩＭＵ２０の角度オフセット誤差変数と、取り付け位置誤差変数が加えられた状態方程式を用いて、乗算部１０３によって係数αが乗算された誤差量δｘから補正速度、補正位置、補正姿勢角、補正３軸加速度、補正３軸角速度に対する調整量を正確に推定演算する。

【0040】

また、以下に、状態推定部１０４の具体的な処理について説明する。
状態推定部１０４は、いわゆる拡張カルマンフィルタにより構成されている。
拡張カルマンフィルタは、状態方程式（（３）式）と観測方程式（（４）式）を離散化した（５）式と（６）式により導くことができる。

【数2】

【数3】

【数4】

【数5】

【数6】

なお、Δｔは、サンプリング時間であり、δｘは、状態変数であり、（８）式で表わされる１９×１の状態変数行列である。

【数7】

【数8】

【数9】

【0041】

これにより、状態推定部１０４は、高精度にＩＭＵ２０の位置地点の速度・姿勢角を算出することができる。取り付け位置関係は、例えば、０．５０［ｍ］、取り付け角度誤差は、例えば、５°程度まで推定して補正可能である。

【0042】

【数10】

【0043】

つぎに、指定位置座標速度算出部１８による指定位置における速度の算出について説明する。
ＩＭＵ２０が設置されている場所から、指定位置（例えば、移動体２の重心点）までの距離をζ_ｂ［ｒａｄ］とすると、指定位置座標速度算出部１８は、重心点のＮＥＤ座標系速度ベクトルは、（９）式により算出する。

【数11】

【0044】

また、横速度・横滑り角算出部１９による演算について説明する。
横速度・横滑り角算出部１９は、横滑り角β［ｒａｄ］を（１０）式により算出する。

【数12】

【0045】

また、横速度・横滑り角算出部１９は、横速度Ｖ_Ｌ［ｍ／ｓ］を（１１）式により算出する。

【数13】

【0046】

ここで、図６は、横滑り角の精度評価結果を示したものである。横軸に直進速度、縦軸に横滑り角精度を示している。また、図６は、直進速度が１０，２０，４０，６０，８０，１００［ｋｍ／ｈ］の時の横滑り角の精度を各５回計測した結果である。

【0047】

また、比較対象として光学式検出器の精度も同試験において検証した。計測装置１は、横滑り角を直進速度３０［ｋｍ／ｈ］以上において、０．１５°ＲＭＳ以内という高い精度を達成している。また、出力遅れ時間は、５［ｍｓ］以内であり、高い応答性も有していることも確認している。これらの結果から、光学式検出器と比較して、本実施例に係る計測装置１の方がより精度が高いことが分かる。

【0048】

よって、計測装置１は、ＥＳＣ（横滑り防止装置）評価用計測器として使用できる利点がある。また、計測装置１は、リアルタイム出力（５ｍｓ以下）を行えるため、ユーザに後処理を強いることがなく使用することができるという利点がある。

【0049】

＜第２の実施例＞
つぎに、加速度・角速度計測部１６で使用するサンプリング周波数と、移動体速度計測部１５及び姿勢角計測部１３で使用するサンプリング周波数が異なる場合について説明する。具体的には、加速度・角速度計測部１６で使用するサンプリング周波数を１００［Ｈｚ］とし、移動体速度計測部１５及び姿勢角計測部１３で使用するサンプリング周波数を２０［Ｈｚ］とする。

【0050】

ここで、加速度・角速度計測部１６と、移動体速度計測部１５及び姿勢角計測部１３で時間同期ができていないと、精度が向上せず、正確な応答時間にならない問題がある。
移動体速度計測部１５及び姿勢角計測部１３は、実時間を基準にデータを２０［Ｈｚ］でサンプリングする。この２０［Ｈｚ］という周期は、絶対時間に対して正確なものである。しかし、第１のアンテナ１０及び第２のアンテナ１１で構成されるＧＰＳ受信機の出力Ｉ／ＦがＵＡＲＴを採用しているため、メッセージ長の違いにより、５０［ｍｓ］±２〜３［ｍｓ］程度のジッタが生じてしまう。

【0051】

一方、加速度・角速度計測部１６は、水晶発振器の精度を基準に１００［Ｈｚ］でサンプリングする。この１００［Ｈｚ］という周期は、水晶発振器に依存している（６０［ｐｐｍ］程度）。よって、周期は、絶対時間に対して正確ではない。具体的には、数百秒で数［ｍｓ］程度の累積誤差が生じる。

【0052】

よって、単に従来技術を利用して、同期あわせを行っても、加速度・角速度計測部１６と、移動体速度計測部１５及び姿勢角計測部１３を同期させることが困難になる。
本実施例に係る計測装置１は、同期のための特別なハードウェア等を使用せずに、簡易なアルゴリズムで解決することを１つの目的にしている。

【0053】

具体的には、計測装置１の速度・姿勢角算出部１７は、図７に示すように、ストラップダウンナビゲータ部１００と、良否判定部１０１と、同期化処理部１０２と、乗算部１０３と、状態推定部１０４と、補間処理部１０６と、同期ずれ補正部１０７を備えることにより、異なるサンプリングレートを有する加速度・角速度計測部１６と姿勢角計測部１３の速度計測値を時間同期させ、速度精度と、積算距離精度と、実時間応答性を向上させる。
なお、ストラップダウンナビゲータ部１００と、良否判定部１０１と、同期化処理部１０２と、乗算部１０３と、状態推定部１０４の基本的な動作については、第１の実施例と同様である。また、以下では、第１のアンテナ１０と第２のアンテナ１１で構成される受信機をＧＰＳ受信機と呼ぶ。

【0054】

ストラップダウンナビゲータ部１００（自律航法アルゴリズム）は、加速度・角速度計測部１６により計測された加速度及び角速度に基づいて、ストラップダウン演算を行い、算出値として補正速度（ＮＥＤ方向）、補正位置（緯度、経度、標高）、補正姿勢角、補正３軸加速度及び補正３軸角速度を算出する。詳細には、ストラップダウンナビゲータ部１００は、加速度・角速度計測部１６により計測された加速度及び角速度に状態推定部１０４によって推定演算された調整量を融合して、ストラップダウン演算を行う。

【0055】

補間処理部１０６は、ＧＰＳ受信機側のサンプリング周波数がＩＭＵ２０側のサンプリング周波数に一致するように、移動体速度計測部１５により計測した移動体の速度の波形を線形補間処理する。具体的には、補間処理部１０６は、加速度・角速度計測部１６のサンプリング周波数と、移動体速度計測部１５及び姿勢角計測部１３のサンプリング周波数を合わせるため、移動体速度計測部１５及び姿勢角計測部１３の現在の計測値と過去のサンプリングから線形補間処理を行い、加速度・角速度計測部１６と同じサンプリング周波数にする処理を行う。

【0056】

なお、本実施例では、移動体速度計測部１５及び姿勢角計測部１３のサンプリング周波数を、加速度・角速度計測部１６の高いサンプリング周波数に合わせる、いわゆるアップサンプリングを行うものとして説明するが、これに限られない。例えば、加速度・角速度計測部１６のサンプリング周波数を、移動体速度計測部１５及び姿勢角計測部１３の低いサンプリング周波数に合わせる、いわゆるダウンサンプリングを行っても良い。この構成の場合には、補間処理部１０６は、ストラップダウンナビゲータ部１００の前段に配置される。

【0057】

ここで、移動体速度計測部１５及び姿勢角計測部１３から入力された信号（デジタル信号）の波形を、図８に示す。サンプリング周波数が２０［Ｈｚ］なので、１サンプルあたり５０［ｍｓ］になる。

【0058】

補間処理部１０６は、移動体速度計測部１５及び姿勢角計測部１３から入力された信号（２０［Ｈｚ］ＲＡＷデータ）を１００［Ｈｚ］でリサンプル処理を行う（図９を参照）。なお、実時間で線形補間処理を行う場合は、因果性により必ず処理による時間遅れ（２５［ｍｓ］）が生じるが（図９中に示すＡ）、この時間遅れは一定値なので、同期化処理部１０２で補正を行う。

【0059】

また、加速度・角速度計測部１６のサンプリング時間は、１０［ｍｓ］である。このサンプリング時間の精度は、水晶発振器に依存する。また、移動体速度計測部１５のサンプリング時間は、５０［ｍｓ］±２〜３［ｍｓ］である。このサンプリング時間は、絶対時間に対して正確である。

【0060】

このようにして、加速度・角速度計測部１６の計測時間と、移動体速度計測部１５（及び姿勢角計測部１３）の計測時間との間には、０〜１０［ｍｓ］のずれが生じてしまう。また、このずれ量は、加速度・角速度計測部１６の計測タイミングの精度と、移動体速度計測部１５（及び姿勢角計測部１３）のジッタにより変動してしまう（図１０を参照）。
なお、補間処理部１０６は、補間精度をさらに向上させるために、ｎ次関数補間、スプライン補間、ラグランジュ補間、又はマルチレートフィルタを用いても良い。

【0061】

良否判定部１０１は、補間処理部１０６により線形補間処理された移動体２の速度に基づいて、加速度を算出し、算出した加速度と、加速度・角速度計測部１６により計測された加速度との差分を算出し、当該差分に基づく係数αを算出する。

【0062】

同期ずれ補正部１０７は、ＧＰＳ受信機側とＩＭＵ２０側の計測タイミングのずれ時間の分だけ、補間処理部１０６により線形補間処理された移動体２の速度の位相を進ませる補正を行う。具体的には、同期ずれ補正部１０７は、ストラップダウンナビゲータ部１００の出力と、補間処理部１０６により補間処理された速度及び姿勢角との間の同期ずれ（数［ｍｓ］）を、線形補間処理された速度及び姿勢角の予測処理を行うことで、１［ｍｓ］以内のレベルまで同期させる。これにより、同期ずれ補正部１０７は、状態推定部１０４（拡張カルマンフィルタ）に入力する誤差量δｘを同じタイミングの計測値で実施することができる。

【0063】

また、同期ずれ補正部１０７は、図１１に示すように、微分処理部１０７ａと、ＬＰＦ１０７ｂと、ずれ時間調整部１０７ｃと、加算器１０７ｄとを備える。
微分処理部１０７ａは、入力されたＧＰＳ速度ｖ（ＮＥＤ方向の速度）を微分して、加速度ａを算出する。
ＬＰＦ１０７ｂは、微分処理部１０７ａで算出した加速度ａに対してフィルタリング処理を行う。
ずれ時間調整部１０７ｃは、ＬＰＦ１０７ｂでフィルタリング処理された加速度にずれ時間を加算処理する。
加算器１０７ｄは、ＧＰＳ速度にずれ時間調整部１０７ｃで調整された値を加算する（（１２）式を参照）。

【数14】

【0064】

このようにして、同期ずれ補正部１０７は、ずれ時間Δｔ分、ＧＰＳ受信機の速度の位相を進ませる処理を行う。
なお、同期ずれ補正部１０７は、遅延処理部１０５と同様に、良否判定部１０１によって係数αが算出されるための時間だけ、移動体速度計測部１５によって測定されたドップラー速度を演算に用いる時間を遅延させる処理も行っている。すなわち、同期ずれ補正部１０７は、ドップラー速度による誤差量を算出するための演算を、良否判定部１０１によって係数αが算出される時間だけ遅延させる。

【0065】

ここで、図１２に、移動体速度計測部１５及び姿勢角計測部１３から入力された信号（２０［Ｈｚ］ＲＡＷデータ）と、補間処理部１０６による処理後の信号（１００［Ｈｚ］線形補間データ）と、同期ずれ補正部１０７による処理後の信号（１００［Ｈｚ］同期ずれ補正データ）を比較して示す。
同期ずれ補正部１０７は、図１２に示すように、１サンプルごとにずれ量が変動する移動体速度計測部１５（及び姿勢角計測部１３）の計測時間と加速度・角速度計測部１６の計測時間の差を確実に補正することができる。

【0066】

同期化処理部１０２は、ストラップダウンナビゲータ部１００によるストラップダウン演算により得られた補正速度（リアルタイム補間速度）、補正位置、補正姿勢角と、移動体速度計測部１５から得られたＮＥＤ方向の速度（ドップラー速度）、位置（緯度、経度、標高）、キャリブレーション部１４から得られた姿勢角の同期化処理を行うために、一定時間分これらを遅延させる。
減算部１０８は、同期ずれ補正部１０７で同期補正及び遅延させた移動体２の速度、姿勢角、位置と、同期化処理部１０２で同期遅延させたリアルタイム補間速度、補正位置、補正姿勢角とを減算して誤差量δｘを算出する。
乗算部１０３は、良否判定部１０１により算出された係数αと、減算部１０８により同期ずれ補正部１０７と同期化処理部１０２との減算処理により得られた誤差量δｘを乗算する。

【0067】

状態推定部１０４は、乗算部１０３によって係数が乗算された誤差量から補正速度、補正位置、補正姿勢角、補正３軸加速度、補正３軸角速度に対する調整量を推定演算する。具体的には、状態推定部１０４は、ＧＰＳ受信機とＩＭＵ２０の角度オフセット誤差変数と、取り付け位置誤差変数が加えられた状態方程式を用いて、乗算部１０３によって係数αが乗算された誤差量δｘから補正速度、補正位置、補正姿勢角、補正３軸加速度、補正３軸角速度に対する調整量（−ｋαδｘ）を推定演算する。

【0068】

また、移動体２を正確に校正された１００ｍ間を１０［ｋｍ／ｈ］から４０［ｋｍ／ｈ］まで加速し、４０［ｋｍ／ｈ］から１０［ｋｍ／ｈ］に減速した場合（加減速試験）における、従来技術（リアルタイム同期補正がないもの）と、本実施例に係る計測装置１（リアルタイム同期補正があるもの）を比較した精度結果を図１３に示す。本実施例に係る計測装置１の方が、明らかに精度が高いことが分かる。

【0069】

また、図１４は、計測装置１によって出力されるリアルタイム補間速度と、第１のアンテナ１０及び第２のアンテナ１１により構成されるＧＰＳ受信機によるドップラー速度とを示す図である。図１４（ａ）は、縦軸を速度とし横軸を時間としたグラフに、ＧＰＳ受信機によるドップラー速度を表した図である。同様に、図１４（ｂ）は、縦軸を速度とし横軸を時間としたグラフに、計測装置１によって出力されるリアルタイム補間速度を表した図である。

【0070】

図１４（ａ）に示すように、木々やビル群等によってＧＰＳ衛星からの信号が反射されてマルチパスが発生する場合、ＧＰＳ受信機によるドップラー速度は、マルチパスによるノイズによる速度を含んで出力される。一方、図１４（ｂ）に示すように、ＧＰＳ受信機によるドップラー速度がマルチパスによるノイズを含んでいても、計測装置１は、ノイズの影響を小さくし、非常に滑らかな波形によって移動体２の速度を出力する。

【0071】

図１５は、計測装置１によって出力されるリアルタイム補間速度の立ち上がりと、ＧＰＳ受信機から出力されるドップラー速度の立ち上がりとを示す図である。図１５（ａ）は、計測装置１が出力した速度と、ＧＰＳ受信機によるドップラー速度と、ドップラー速度を１Ｈｚのローパス処理をした速度との比較を示す図である。図１５（ｂ）は、図１５（ａ）の一部（破線による円によって囲まれた部分）を拡大した図である。

【0072】

図１５（ｂ）に示すように、マルチパスによるノイズを含むドップラー速度と、マルチパスによるノイズを含むドップラー速度をローパス処理によってノイズ除去した速度とに比較して、計測装置１が出力する速度は、ノイズ除去のための演算による出力遅れを小さくして、リアルタイム性を実現している。

【0073】

図１６は、計測装置１によって出力されるリアルタイム補間速度と、ＧＰＳ受信機から出力されるドップラー速度と、光学式速度計測装置から出力される速度とを示す図である。ここで、光学式速度計測装置は、路面の不規則な模様から、特定の間隔（例えば、２．３［ｍｍ］）によるクシ型構造の特殊受光素子によって特定の間隔の反射ムラだけを抽出し、抽出した反射ムラの計数値に特定の間隔をかけて算出した高精度の速度を出力する。

【0074】

図１６が示すように、計測装置１は、マルチパスによるノイズを含むドップラー速度におけるノイズの影響を小さくし、リアルタイムに移動体２の速度を出力し、出力した速度は、光学式速度計測装置による高精度の速度と同様である。

【0075】

以上、本発明の実施形態について説明したが、本発明は上述した実施形態に限るものではない。また、本発明の実施形態に記載された効果は、本発明から生じる最も好適な効果を列挙したに過ぎず、本発明による効果は、本発明の実施形態に記載されたものに限定されるものではない。

【符号の説明】

【0076】

１ 計測装置
１０ 第１のアンテナ
１１ 第２のアンテナ
１２ 相対位置算出部
１３ 姿勢角計測部
１４ キャリブレーション部
１５ 移動体速度計測部
１６ 加速度・角速度計測部
１７ 速度・姿勢角算出部
１８ 指定位置座標速度算出部
１９ 横速度・横滑り角算出部
１００ ストラップダウンナビゲータ部
１０１ 良否判定部
１０２ 同期化処理部
１０３ 乗算部
１０４ 状態推定部
１０５ 遅延処理部
１０６ 補間処理部
１０７ 同期ずれ補正部
１０８ 減算部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】

【図16】