JP2013044562A

JP2013044562A - 路面勾配算出装置及び方法

Info

Publication number: JP2013044562A
Application number: JP2011180670A
Authority: JP
Inventors: Kazuhiro Ichikawa; 和宏市川
Original assignee: Ono Sokki Co Ltd
Current assignee: Ono Sokki Co Ltd
Priority date: 2011-08-22
Filing date: 2011-08-22
Publication date: 2013-03-04

Abstract

【課題】走行する移動体上でリアルタイムに、かつ、高精度に路面勾配を算出できる路面勾配算出装置及び方法を提供すること。
【解決手段】路面勾配算出装置１０は、移動体に設置され、ＧＰＳ衛星からＧＰＳ搬送波を受信するＧＰＳ受信機１０１と、移動体の略重心位置に設置され、３次元の角速度と加速度とを求めるためのデータを出力するＩＭＵ１０２とを備える。そして、路面勾配算出装置１０は、ＧＰＳ受信機１０１が受信したＧＰＳ搬送波と、ＩＭＵ１０２が出力したデータとをカルマンフィルタにより処理することで速度を算出し、算出した速度の水平方向の速度を積算し、水平方向相対距離を算出し、算出した速度の垂直方向の速度を積算し、垂直方向相対距離を算出し、算出した水平方向相対距離が所定の水平方向距離になる区間ごとに、算出した水平方向相対距離と、算出した垂直方向相対距離とに基づいて、路面勾配を算出する。
【選択図】図１

Description

本発明は、路面勾配算出装置及び方法に関する。

従来より、路面勾配を算出する方法には、ＧＰＳ（ＧｌｏｂａｌＰｏｓｉｔｉｏｎｉｎｇＳｙｓｔｅｍ）を使用して路面勾配を算出する手段や、その他センサを使用して路面勾配を算出する手段等があるが、これらの技術を開示する特許文献１から特許文献４が知られている。

特許文献１の勾配検出装置は、ＧＰＳ受信機から得られる緯度、経度、標高から、路面勾配を算出する。この勾配検出装置は、衛星から地球の絶対座標及び標高に関する情報を受信するＧＰＳレシーバーと、道路上の交差点ごとに標高値を記憶する標高記憶手段とを備え、道路上において自車位置及びその標高値を検出すると、現在地と次の交差点までの間の道路の勾配値を算出する。

特許文献２の道路勾配計測システムは、ＧＰＳから得られる緯度、経度、標高と、大気圧計を使用して得られる高さとに基づいて、特許文献１による勾配値よりも高精度な路面勾配を計測しようとする。この道路勾配計測システムは、大気圧に基づく仮標高を演算し、仮標高をＧＰＳデータの標高データにより補正して道路標高を演算し、ＧＰＳデータの緯度データと経度データとに基づいて移動距離を演算し、道路標高と移動距離とに基づいて道路勾配を演算する。

特許文献３の装置は、加速度センサと車輪の回転速度から算出した加速度の偏差から路面勾配を推定する。この装置は、所定質量のウェイトと、ウェイトに作用している重力の力に応じてウェイトによって生じる第１パラメータを表す第１信号を発生する第１センサと、ウェイトに作用している重力及び車両の加速度の合成力に応じてウェイトによって生じる第２パラメータを表す第２信号を発生する第２センサとを備え、第１及び第２信号に応じて、車両が走行中の勾配の角度に対応した値の勾配角度信号を発生する。

特許文献４の道路勾配計測装置は、ジャイロセンサを用いて検出した車両傾き角と、車両と路面との距離を２点で計測することで得られる車両傾き角との偏差から路面勾配を検出する。この道路勾配計測装置は、ジャイロセンサを用いて、車両の走行中のピッチ角を検出し、車両の前後方向に位置を異ならせた複数箇所で、車両の車体と路面との距離を検出することによって、車両傾き角を検出する。そして、ピッチ角と車両傾き角との偏差に基づいて、走行中の道路の道路勾配を検出する。ピッチ角は、車両傾き角及び道路勾配のみによって決定され、ジャイロセンサを用いれば精度よくピッチ角を検出することができる。したがって、車両の前後で路面との距離を検出して車両傾き角を求めてやれば、ピッチ角と車両傾き角との偏差から、精度よく道路勾配を算出することが可能となる。

特開平８−３０４０６９号公報特開２００４−１１０５９０号公報特開平６−２０１３８０号公報特開２００９−２７６１０９号公報

しかしながら、特許文献１及び特許文献２の方法では，ＧＰＳの単独測位時の精度が数ｍオーダーの精度のため、また、標高の精度がさらに悪くなるため、路面勾配を求めるには十分ではない。また、一定の距離区間が必要なため、路面勾配を離散的にしか求めることができない。特許文献３の発明では、連続的に計測することが可能であるが、車両には様々な外乱が生じるため、安定して精度よく勾配を計測できるものではない。特許文献４の装置は、特許文献１、特許文献２及び特許文献３と比較して精度よく計測できるが、車高センサの取り付けが煩雑になる。

そこで、公道を走行しながら、走行する移動体上でリアルタイムに、かつ、高精度に路面勾配を算出できる装置が求められている。

本発明は、走行する移動体上でリアルタイムに、かつ、高精度に路面勾配を算出できる路面勾配算出装置及び方法を提供することを目的とする。

本発明では、以下のような解決手段を提供する。
（１）移動体に設置され、ＧＰＳ衛星からＧＰＳ搬送波を受信するＧＰＳ受信機と、前記移動体の略重心位置に設置され、３次元の角速度と加速度とを求めるためのデータを出力するＩＭＵとを備え、前記移動体が移動する路面の勾配を算出する路面勾配算出装置であって、前記ＧＰＳ受信機が受信したＧＰＳ搬送波と、前記ＩＭＵが出力したデータとをカルマンフィルタにより処理することで速度を算出する移動体高精度速度算出手段と、前記移動体高精度速度算出手段により算出された速度の水平方向の速度を積算し、水平方向相対距離を算出する水平方向相対距離算出手段と、前記移動体高精度速度算出手段により算出された速度の垂直方向の速度を積算し、垂直方向相対距離を算出する垂直方向相対距離算出手段と、前記水平方向相対距離算出手段により算出された水平方向相対距離が所定の水平方向距離になる区間ごとに、算出された前記水平方向相対距離と、算出された前記垂直方向相対距離とに基づいて、路面勾配を算出する路面勾配算出手段と、を備える路面勾配算出装置。

（１）の構成によれば、本発明に係る路面勾配算出装置は、移動体に設置され、ＧＰＳ衛星からＧＰＳ搬送波を受信するＧＰＳ受信機と、移動体の略重心位置に設置され、３次元の角速度と加速度とを求めるためのデータを出力するＩＭＵ（ＩｎｅｒｔｉａｌＭｅａｓｕｒｅｍｅｎｔＵｎｉｔ：慣性計測装置）とを備える。そして、路面勾配算出装置は、ＧＰＳ受信機が受信したＧＰＳ搬送波と、ＩＭＵが出力したデータとをカルマンフィルタにより処理することで速度を算出し、算出した速度の水平方向の速度を積算し、水平方向相対距離を算出し、算出した速度の垂直方向の速度を積算し、垂直方向相対距離を算出し、算出した水平方向相対距離が所定の水平方向距離になる区間ごとに、算出した水平方向相対距離と、算出した垂直方向相対距離とに基づいて、路面勾配を算出する。

すなわち、本発明に係る路面勾配算出装置は、移動体に設置されたＧＰＳ受信機と、移動体の略重心位置に設置されたＩＭＵとを用いて算出された移動体の高精度の速度に基づいて、水平方向相対距離と、垂直方向相対距離とを算出し、算出した水平方向相対距離と垂直方向相対距離とから路面勾配を算出する。
したがって、本発明に係る路面勾配算出装置は、走行する移動体上でリアルタイムに、かつ、高精度に路面勾配を算出できることができる。

（２）前記区間をオーバーラップさせるための間隔であって、前記所定の水平方向距離よりも小さい間隔を指定するオーバーラップ間隔指定手段をさらに備え、前記路面勾配算出手段は、前記オーバーラップ間隔指定手段によって指定された間隔ごとに、前記水平方向相対距離及び前記垂直方向相対距離を積算し始め、前記水平方向相対距離が所定の水平方向距離になると、前記路面勾配を算出する、（１）に記載の路面勾配算出装置。

したがって、（２）に係る路面勾配算出装置は、走行する移動体上でリアルタイムに路面勾配を算出し、さらに連続的かつ滑らかに路面勾配を算出することができる。

（３）前記移動体高精度速度算出手段は、前記ＩＭＵからデータを受信して、前記移動体の加速度及び角速度を計測する加速度計測手段と、前記加速度計測手段によって計測された前記移動体の加速度及び角速度からストラップダウン演算により速度を算出し、算出した速度に基づいてリアルタイム補間速度を算出するストラップダウンナビゲータ手段と、前記ＧＰＳ受信機が受信したＧＰＳ搬送波のドップラーシフト量から前記移動体のドップラー速度を測定する速度測定手段と、前記速度測定手段によって測定された前記ドップラー速度から算出した加速度と、前記加速度計測手段によって計測された加速度との差分を算出し、算出した差分に基づいて係数を算出する良否係数算出手段と、前記良否係数算出手段によって前記係数が算出されるための時間だけ、前記速度測定手段によって測定された前記ドップラー速度を演算に用いる時間を遅延させる遅延処理手段と、前記遅延処理手段によって遅延させた前記ドップラー速度と、前記ストラップダウンナビゲータ手段からフィードバックさせた前記リアルタイム補間速度との同期化を行い、同期化された前記ドップラー速度と前記リアルタイム補間速度との誤差量を算出する同期化処理手段と、前記同期化処理手段によって算出された前記誤差量に、前記良否係数算出手段によって算出された前記係数を乗算する良否係数乗算手段と、前記良否係数乗算手段によって前記係数が乗算された誤差量から前記リアルタイム補間速度に対する調整量を、カルマンフィルタによって推定演算するカルマンフィルタ手段と、を備え、前記ストラップダウンナビゲータ手段は、前記ストラップダウン演算により算出された速度に、前記カルマンフィルタ手段によって推定演算された前記調整量を融合して前記リアルタイム補間速度を算出する、（１）又は（２）に記載の路面勾配算出装置。

したがって、（３）に係る路面勾配算出装置は、走行する移動体上でリアルタイムに、かつ、高精度に路面勾配を算出することができる。

（４）移動体に設置され、ＧＰＳ衛星からＧＰＳ搬送波を受信するＧＰＳ受信機と、前記移動体の略重心位置に設置され、３次元の角速度と加速度とを求めるためのデータを出力するＩＭＵとを備え、前記移動体が移動する路面の勾配を算出する路面勾配算出装置が実行する方法であって、前記ＧＰＳ受信機が受信したＧＰＳ搬送波と、前記ＩＭＵが出力したデータとをカルマンフィルタにより処理することで速度を算出する移動体高精度速度算出ステップと、前記移動体高精度速度算出ステップにより算出された速度の水平方向の速度を積算し、水平方向相対距離を算出する水平方向相対距離算出ステップと、前記移動体高精度速度算出ステップにより算出された速度の垂直方向の速度を積算し、垂直方向相対距離を算出する垂直方向相対距離算出ステップと、前記水平方向相対距離算出ステップにより算出された水平方向相対距離が所定の水平方向距離になる区間ごとに、算出された前記水平方向相対距離と、算出された前記垂直方向相対距離とに基づいて、路面勾配を算出する路面勾配算出ステップと、を備える方法。

したがって、（４）に係る方法は、（１）と同様に、走行する移動体上でリアルタイムに、かつ、高精度に路面勾配を算出することができる。

本発明によれば、走行する移動体上でリアルタイムに、かつ、高精度に路面勾配を算出することができる。本発明によれば、公道を走行しながら、走行する移動体上でリアルタイムに、連続的かつ滑らかに路面勾配を算出することができる。
さらに、本発明によれば、車速や走行距離等と同時に路面勾配を計測が可能となるため、実車両走行試験、特に動力性能や燃費性能、登板性能等を簡易に計測することが可能である。

本発明の一実施形態に係る路面勾配算出装置の使用状態を示す図である。本発明の一実施形態である路面勾配算出装置の構成を示すブロック図である。本発明の一実施形態である路面勾配算出装置の移動体高精度速度算出部１１の構成を示すブロック図である。本発明の一実施形態に係る路面勾配算出装置のオーバーラップ処理を説明するための図である。本発明の一実施形態に係る路面勾配算出装置のハードウェア構成の一例を示す図である。本発明の一実施形態に係る路面勾配算出装置の処理内容を示すフローチャートである。本発明の一実施形態に係る路面勾配算出装置の移動体高精度速度算出処理の処理内容を示すフローチャートである。本発明の一実施形態に係る路面勾配算出装置がオーバーラップ処理を行わない場合に算出した路面勾配を示す図である。本発明の一実施形態に係る路面勾配算出装置がオーバーラップ処理を行った場合に算出した路面勾配を示す図である。本発明の一実施形態に係る路面勾配算出装置によって公道の路面勾配を算出した場合を示す図である。

以下、本発明の実施形態について図を参照しながら説明する。図１は、本発明の一実施形態に係る路面勾配算出装置１０の使用状態を示す図である。図１は、路面勾配算出装置１０が、移動体５１（例えば、自動車等）にＧＰＳ信号を受信しやすいように設置されたＧＰＳ受信機１０１と、移動体５１の略重心位置に設置されたＩＭＵ１０２とに接続されていることを示している。ＧＰＳ受信機１０１は、ＧＰＳ衛星からＧＰＳ搬送波を受信する。ＩＭＵ１０２は、３次元の角速度と加速度とを求めるためのデータを出力する。

図２は、本発明の一実施形態である路面勾配算出装置１０の構成を示すブロック図である。路面勾配算出装置１０は、移動体高精度速度算出手段としての移動体高精度速度算出部１１と、水平方向相対距離算出手段としての水平方向相対距離算出部１２と、垂直方向相対距離算出手段としての垂直方向相対距離算出部１３と、路面勾配算出手段としての路面勾配算出部１４と、オーバーラップ指定手段としてのオーバーラップ間隔指定部１４１とを備えている。

移動体高精度速度算出部１１は、ＧＰＳ受信機が受信したＧＰＳ搬送波と、ＩＭＵが出力したデータとをカルマンフィルタにより処理することで速度を算出する。移動体高精度速度算出部１１は、ＧＰＳ受信機１０１及びＩＭＵ１０２のデータを処理して得られるドップラー速度、３軸加速度、３軸角速度を用いて、ＮＥＤ（Ｎｏｒｔｈ−Ｅａｓｔ−Ｄｏｗｎ）座標系における高精度な移動体速度を算出する。

水平方向相対距離算出部１２は、移動体高精度速度算出部１１により算出された速度の水平方向の速度を積算し、水平方向相対距離を算出する。水平方向相対距離算出部１２は、高精度なＮＥＤ座標系の速度から水平方向の速度を求め、それらを積算し水平距離を算出する。水平方向速度は、式（１）より得られる。
水平方向速度＝√（北速度）^２＋（東速度）^２・・・式（１）

垂直方向相対距離算出部１３は、移動体高精度速度算出部１１により算出された速度の垂直方向の速度を積算し、垂直方向相対距離を算出する。垂直方向相対距離算出部１３は、高精度なＮＥＤ座標系の速度から垂直方向の速度を求め、それらを積算し垂直距離を算出する。垂直方向速度は、ＮＥＤ座標系のＤとして直接、移動体高精度速度算出部１１より得られる。

路面勾配算出部１４は、水平方向相対距離算出部１２により算出された水平方向相対距離が所定の水平方向距離になる区間ごとに、算出された水平方向相対距離と、算出された垂直方向相対距離とに基づいて、路面勾配を算出する。路面勾配は、水平距離、垂直距離から、所定の水平方向距離（指定水平距離の区間、例えば、５０ｍ）ごとに式（２）を演算することで勾配（％）を算出する。
路面勾配（％）＝垂直距離／水平距離＊１００・・・式（２）
本方式は、積算処理を行うことで移動体の振動を除去できるため、移動体に搭載していながらも高精度に路面勾配を算出することが可能である。

さらに、オーバーラップ間隔指定部１４１は、所定の水平方向距離の区間をオーバーラップさせるための間隔であって、所定の水平方向距離よりも小さい間隔を指定する。具体的には、オーバーラップ間隔指定部１４１は、路面勾配を算出するための区間をオーバーラップさせるための間隔を入力装置１１００から入力し、メモリ１０５０に記憶させる。路面勾配算出部１４は、オーバーラップ間隔指定部１４１によって指定された間隔ごとに、水平方向相対距離及び垂直方向相対距離を積算し始め、水平方向相対距離が所定の水平方向距離になると、路面勾配を算出する（オーバーラップ処理）。

図３は、本発明の一実施形態である路面勾配算出装置１０の移動体高精度速度算出部１１の構成を示すブロック図である。移動体高精度速度算出部１１は、加速度計測部１１１と、ストラップダウンナビゲータ部１１２と、速度測定部１１３と、良否係数算出部１１４と、遅延処理部１１５と、同期化処理部１１６と、良否係数乗算部１１７と、カルマンフィルタ部１１８とを備える。

加速度計測部１１１は、ＩＭＵ１０２を使用して移動体の加速度及び角速度を計測する。ＩＭＵ１０２は、３軸のジャイロと３方向の加速度計から構成され、３次元の角速度と加速度とを求める。なお、ＩＭＵ１０２は、計測の信頼性向上のために、さらに複数のセンサを搭載してもよい。

ストラップダウンナビゲータ部１１２（自律航法アルゴリズム）は、加速度計測部１１１によって計測された移動体の加速度及び角速度からストラップダウン演算により速度を算出し、算出した速度に、後述するカルマンフィルタ部１１８によって推定演算された調整量を融合してリアルタイム補間速度を算出し、算出したリアルタイム補間速度を出力すると共に、カルマンフィルタ部１１８へフィードバックさせる。

速度測定部１１３は、ＧＰＳ受信機１０１を使用してＧＰＳ搬送波のドップラーシフト量から移動体のドップラー速度を測定する。例えば、ＧＰＳ受信機１０１とＧＰＳ衛星との距離が、遠ざかる又は近づくと、ＧＰＳ受信機１０１が受信する搬送波の位相は、連続的に変化し、周波数が低くなったり高くなったりする。速度測定部１１３は、この周波数の変化量からＧＰＳ受信機１０１が出力するドップラー速度を取得する。

良否係数算出部１１４は、速度測定部１１３によって測定されたドップラー速度から算出した加速度と、加速度計測部１１１によって計測された加速度との差分を算出し、算出した差分に基づいて係数（図３におけるβ）を算出する。例えば、良否係数算出部１１４は、座標変換処理部（図示せず）によって、ＩＭＵ１０２によって計測された加速度を、ストラップダウンナビゲータ部１１２により演算された現在の姿勢角に基づき座標変換し、ＧＰＳ受信機１０１が出力したドップラー速度と同座標系の加速度に変換する。そして、良否係数算出部１１４は、ドップラー速度を微分した加速度と、座標変換した加速度との差分を算出し、算出した差分を二乗し、二乗した差分についてエンベロープ処理を行う。次に、良否係数算出部１１４は、エンベロープ処理を行った後の差分を示す関数の逆関数を求め、求めた逆関数に基づいて、係数を算出する。すなわち、良否係数算出部１１４は、求めた逆関数に基づいて、ドップラー速度がノイズを含んでいないと判断した場合に良判定を行い、ノイズを含んでいると判断した場合に否判定を行い、判定を数値化した良否係数を算出する。例えば、良否係数＝１−速度変動判定−α×ドップラー精度指標、により良否係数を算出してもよい。ここで、速度変動判定はドップラー速度がノイズを含むと判定された割合、αは所定の値、ドップラー速度精度指標はＧＰＳ衛星の配置に関する指標や、観測衛星数等のＧＰＳ受信機から得られる情報に基づいて統計的に算出した指標である。このように算出された良否係数は、カルマンフィルタ部１１８に入力する誤差量を調整する係数である。

遅延処理部１１５は、良否係数算出部１１４によって係数が算出されるための時間だけ、速度測定部１１３によって測定されたドップラー速度を演算に用いる時間を遅延させる。すなわち、遅延処理部１１５は、後述する、ドップラー速度による誤差量を算出するための演算を、良否係数算出部１１４によって係数が算出される時間だけ遅延させる。

同期化処理部１１６は、遅延処理部１１５によって遅延させたドップラー速度と、ストラップダウンナビゲータ部１１２からフィードバックさせたリアルタイム補間速度との同期化を行い、同期化されたドップラー速度とリアルタイム補間速度との誤差量（図３におけるδｘ）を算出する。

良否係数乗算部１１７は、同期化処理部１１６によって算出された誤差量に、良否係数算出部１１４によって算出された係数を乗算する。

カルマンフィルタ部１１８は、良否係数乗算部１１７によって係数が乗算された誤差量からリアルタイム補間速度に対する調整量を、カルマンフィルタによって推定演算する。例えば、カルマンフィルタ部１１８は、入力された誤差量と、１ステップ前の推定演算し記憶した調整量とから、今回の調整量（ろ波推定値）を算出する反復推定器（反復推定型フィルタ）である。

このように、移動体高精度速度算出部１１は、速度測定部１１３によって測定されたドップラー速度とフィードバックされたリアルタイム補間速度とを同期させて算出した誤差量にドップラー速度の良否係数を乗算し、乗算して調整した誤差量からカルマンフィルタ部１１８によって推定演算された調整量と、加速度計測部１１１によって計測された加速度及び角速度からストラップダウン演算により算出された速度と、を融合してリアルタイム補間速度を算出し、算出したリアルタイム補間速度を出力すると共にフィードバックさせる。

図４は、本発明の一実施形態に係る路面勾配算出装置１０のオーバーラップ処理を説明するための図である。図４（ａ）は、路面勾配算出装置１０が、所定の水平方向距離として基準走行区間距離ごとに路面勾配を算出する場合を説明する図である。路面勾配算出装置１０は、基準走行区間距離ごとに、積算の開始値を０にし、積算した水平方向相対距離と、積算した垂直方向相対距離とに基づいて路面勾配を算出する。

図４（ｂ）は、路面勾配算出装置１０が、路面勾配算出処理のオーバーラップ処理を説明するための図である。路面勾配算出装置１０は、オーバーラップをさせるための間隔ごとに積算の開始値を０にする開始点を設け、開始点から所定の水平方向距離（基準走行区間距離）ごとに、積算した水平方向相対距離と、積算した垂直方向相対距離とに基づいて路面勾配を算出する。具体的には、路面勾配算出装置１０は、最初の開始から基準走行区間距離の地点までの路面勾配を、積算した水平方向相対距離と、積算した垂直方向相対距離とに基づいて算出する。次に、路面勾配算出装置１０は、積算した水平方向相対距離及び垂直方向相対距離から、オーバーラップをさせるための間隔の水平方向相対距離及び垂直方向相対距離を取り除き、その後に新たに算出した水平方向の速度及び垂直方向の速度を積算し、積算した水平方向相対距離が基準走行区間距離に到達すると、積算した水平方向相対距離と、積算した垂直方向相対距離とに基づいて路面勾配を算出する。
オーバーラップをさせるための間隔がサンプリング時間である場合、オーバーラップ処理は、サンプリング時間間隔で路面勾配を算出する。よって、路面勾配算出装置１０は、連続的かつ滑らかに路面勾配を算出する。

オーバーラップ処理をＣ言語で記述すると、次の様な例になる。ここでは、指定水平距離区間を１０ｍとしている．
ＨＤｉｓｔａｎｃｅ＝ＨＤｉｓｔａｎｃｅ＋ＨＶｅｌｏｃｉｔｙ＿ｎｅｗ＊ｓａｍｐｌｉｎｇ＿ｔｉｍｅ；
ＶＤｉｓｔａｎｃｅ＝ＶＤｉｓｔａｎｃｅ＋ＶＶｅｌｏｃｉｔｙ＿ｎｅｗ＊ｓａｍｐｌｉｎｇ＿ｔｉｍｅ；
ＥｎｑｕｅｕｅＨ（ＨＶｅｌｏｃｉｔｙ＿ｎｅｗ）；／／得られた水平速度をキューに入れる
ＥｎｑｕｅｕｅＶ（ＶＶｅｌｏｃｉｔｙ＿ｎｅｗ）；／／得られた垂直速度をキューに入れる
ｗｈｉｌｅ（ＨＤｉｓｔａｎｃｅ＞１０）｛
／／水平方向が指定距離未満になるまで続ける（ここでは１０ｍとする）
／／古い水平、垂直距離を指定距離になるまで引いていく
ＨＤｉｓｔａｎｃｅ＝ＨＤｉｓｔａｎｃｅ−ＤｅｑｕｅｕｅＨ（）＊ｓａｍｐｌｉｎｇ＿ｔｉｍｅ；
ＶＤｉｓｔａｎｃｅ＝ＶＤｉｓｔａｎｃｅ−ＤｅｑｕｅｕｅＶ（）＊ｓａｍｐｌｉｎｇ＿ｔｉｍｅ；
｝
Ｇｒａｄｉｅｎｔ＝ＶＤｉｓｔａｎｃｅ／ＨＤｉｓｔａｎｃｅ＊１００；／／勾配を算出する（％）

ただし、ＨＤｉｓｔａｎｃｅ：水平距離演算変数
ＨＶｅｌｏｃｉｔｙ＿ｎｅｗ：水平速度変数
ＶＶｅｌｏｃｉｔｙ＿ｎｅｗ：垂直速度変数
ｓａｍｐｌｉｎｇ＿ｔｉｍｅ：サンプリング時間変数
Ｇｒａｄｉｅｎｔ：勾配変数
ＥｎｑｕｅｕｅＨ（），ＥｎｑｕｅｕｅＶ（）：エンキュー関数
ＤｅｑｕｅｕｅＨ（），ＤｅｑｕｅｕｅＶ（）：デキュー関数

図５は、本発明の一実施形態に係る路面勾配算出装置１０のハードウェア構成の一例を示す図である。路面勾配算出装置１０は、ＣＰＵ（ＣｅｎｔｒａｌＰｒｏｃｅｓｓｉｎｇＵｎｉｔ）１０１０、バスライン１００５、通信Ｉ／Ｆ１０４０、メモリ１０５０、表示器１０２２、入力装置１１００、ＧＰＳ受信機１０１及びＩＭＵ１０２を備える。

ＣＰＵ１０１０は、路面勾配算出装置１０を統括的に制御する部分であり、メモリ１０５０に記憶された各種プログラムを適宜読み出して実行することにより、上述したハードウェアと協働し、本発明に係る各種機能を実現している。

メモリ１０５０は、適宜読み出して実行されるプログラムを記憶し、プログラムの実行によって作成される種々の情報を記憶する。例えば、メモリ１０５０は、ストラップダウンナビゲータ部１１２によってフィードバックされるリアルタイム補間速度や、カルマンフィルタ部１１８によって推定演算される調整量等を記憶する。

ここで、表示器１０２２は、路面勾配算出装置１０による演算処理結果、例えば、路面勾配や、算出したリアルタイム補間速度や、算出過程のドップラー速度等を表示したりするものであり、液晶表示装置（ＬＣＤ）等のディスプレイ装置を含む。

また、通信Ｉ／Ｆ１０４０は、路面勾配算出装置１０を専用ネットワークを介してホストコンピュータ等と接続できるようにするためのネットワーク・アダプタである。通信Ｉ／Ｆ１０４０は、ホストコンピュータ等に、算出された路面勾配等を出力するための接続インターフェースである。

入力装置１１００は、路面勾配算出装置１０の利用者による入力の受け付けを行うものであり、キーボード及びマウス等で構成される。例えば、入力装置１１００は、オーバーラップ処理のための間隔を入力する際に、使用される。

ＧＰＳ受信機１０１は、無線通信に必要なアンテナ及びアンテナ信号処理回路等を含んで構成される。ＧＰＳ受信機１０１は、複数のＧＰＳ衛星から電波を受信し、受信したＧＰＳ搬送波のドップラーシフト量からドップラー速度を算出し、出力する。

ＩＭＵ１０２は、移動体の加速度及び角速度を計測するためのデータを出力する。ＩＭＵ１０２は、慣性計測装置（ＩＭＵ：ＩｎｅｒｔｉａｌＭｅａｓｕｒｅｍｅｎｔＵｎｉｔ）であって、３軸のジャイロと３方向の加速度計によって、運動を司る３軸の角度（又は角速度）と加速度とを検出する。

図６は、本発明の一実施形態に係る路面勾配算出装置１０の処理内容を示すフローチャートである。なお、本処理は、例えば、プログラム開始指令を受け付けて開始し、プログラム終了指令により終了する。

ステップＳ１０１において、ＣＰＵ１０１０は、オーバーラップさせるための間隔を指定し、ストラップダウンナビゲータ部１１２の初期設定（初期速度、初期位置、初期姿勢角）、カルマンフィルタ部１１８の初期設定（誤差共分散行列の初期化）を行う。その後、ＣＰＵ１０１０は、処理をステップＳ１０２に移す。

ステップＳ１０２において、ＣＰＵ１０１０（移動体高精度速度算出部１１）は、移動体高精度速度算出処理を行う。その後、ＣＰＵ１０１０は、処理をステップＳ１０３に移す。

ステップＳ１０３において、ＣＰＵ１０１０（水平方向相対距離算出部１２）は、水平方向相対距離を算出する。より具体的には、ＣＰＵ１０１０は、ステップＳ１０２において算出された速度の水平方向の速度を積算し、水平方向相対距離を算出する。その後、ＣＰＵ１０１０は、処理をステップＳ１０４に移す。

ステップＳ１０４において、ＣＰＵ１０１０（垂直方向相対距離算出部１３）は、垂直方向相対距離を算出する。より具体的には、ＣＰＵ１０１０は、ステップＳ１０２において算出された速度の垂直方向の速度を積算し、垂直方向相対距離を算出する。その後、ＣＰＵ１０１０は、処理をステップＳ１０５に移す。

ステップＳ１０５において、ＣＰＵ１０１０（路面勾配算出部１４）は、算出した水平方向相対距離が所定の水平方向距離に到達したか否かを判断する。この判断がＹＥＳの場合、ＣＰＵ１０１０は、処理をステップＳ１０６に移し、この判断がＮＯの場合、ＣＰＵ１０１０は、処理をステップＳ１０２に移す。

ステップＳ１０６において、ＣＰＵ１０１０（路面勾配算出部１４）は、路面勾配を算出する。より具体的には、ＣＰＵ１０１０は、ステップＳ１０３で算出した水平方向相対距離と、ステップＳ１０４で算出した垂直方向相対距離とに基づいて、路面勾配を算出する。その後、ＣＰＵ１０１０は、処理をステップＳ１０７に移す。

ステップＳ１０７において、ＣＰＵ１０１０（路面勾配算出部１４）は、水平方向のオーバーラップ処理を行う。より具体的には、ＣＰＵ１０１０は、算出した水平方向相対距離から、オーバーラップさせるための間隔に相当する積算開始からの水平方向相対距離だけ除く。その後、ＣＰＵ１０１０は、処理をステップＳ１０８に移す。

ステップＳ１０８において、（路面勾配算出部１４）は、垂直方向のオーバーラップ処理を行う。より具体的には、ＣＰＵ１０１０は、算出した垂直方向相対距離から、オーバーラップさせるための間隔に相当する積算開始からの垂直方向相対距離だけ除く。その後、ＣＰＵ１０１０は、処理をステップＳ１０２に移す。

図７は、本発明の一実施形態に係る路面勾配算出装置１０の移動体高精度速度算出処理の処理内容を示すフローチャートである。

ステップＳ２０１において、ＣＰＵ１０１０（加速度計測部１１１）は、ＩＭＵ１０２によって加速度及び角速度を計測する。その後、ＣＰＵ１０１０は、処理をステップＳ２０２に移す。

ステップＳ２０２において、ＣＰＵ１０１０（ストラップダウンナビゲータ部１１２）は、ステップＳ２０１において計測された加速度及び角速度からストラップダウン演算により速度を算出し、カルマンフィルタ部１１８からの調整量を融合させて、リアルタイム補間速度を算出し、出力すると共にフィードバックさせる。その後、ＣＰＵ１０１０は、処理をステップＳ２０３に移す。

ステップＳ２０３において、ＣＰＵ１０１０（速度測定部１１３）は、ＧＰＳ受信機１０１によって移動体のドップラー速度を取得する。より具体的には、ＣＰＵ１０１０は、ＧＰＳ受信機１０１が受信したＧＰＳ搬送波のドップラーシフト量から算出したドップラー速度を取得する。その後、ＣＰＵ１０１０は、処理をステップＳ２０４に移す。

ステップＳ２０４において、ＣＰＵ１０１０（良否係数算出部１１４）は、ドップラー速度から良否係数を算出する。より具体的には、ＣＰＵ１０１０は、ステップＳ２０２において算出したドップラー速度を微分した加速度と、ＩＭＵ１０２が計測した加速度との差分を算出し、算出した差分を二乗し、二乗した差分についてエンベロープ処理を行う。そして、ＣＰＵ１０１０は、エンベロープ処理を行った後の差分を示す関数の逆関数を求め、求めた逆関数に基づいて、ドップラー速度の信頼性（ドップラー速度がノイズを含んでいないと判定した場合に良、ノイズを含んでいると判定した場合に否）に対する判定の結果を示す良否係数（β）を算出する。その後、ＣＰＵ１０１０は、処理をステップＳ２０５に移す。

ステップＳ２０５において、ＣＰＵ１０１０（遅延処理部１１５、同期化処理部１１６）は、フィードバックされたリアルタイム補間速度と、ドップラー速度とを同期させ、誤差量を算出する。より具体的には、ＣＰＵ１０１０は、ステップＳ２０３において算出されたドップラー速度による誤差量を算出するための演算を、ステップＳ２０４において良否係数が算出される時間だけ遅延させ、遅延させたドップラー速度とフィードバックされたリアルタイム補間速度とを同期させて誤差量（δｘ）を算出する。その後、ＣＰＵ１０１０は、処理をステップＳ２０６に移す。

ステップＳ２０６において、ＣＰＵ１０１０（良否係数乗算部１１７）は、誤差量に良否係数を乗算する。より具体的には、ＣＰＵ１０１０は、ステップＳ２０５において算出した誤差量（δｘ）に、ステップＳ２０４において算出した良否係数（β）を乗算して、調整された誤差量（βδｘ）を算出する。その後、ＣＰＵ１０１０は、処理をステップＳ２０７に移す。

ステップＳ２０７において、ＣＰＵ１０１０（カルマンフィルタ部１１８）は、調整された誤差量（βδｘ）からリアルタイム補間速度に対する調整量を、カルマンフィルタによって推定演算する。その後、ＣＰＵ１０１０は、移動体高精度速度算出処理に移行したステップの次のステップに、処理を戻す。

図８は、本発明の一実施形態に係る路面勾配算出装置１０がオーバーラップ処理を行わない場合に算出した路面勾配を示す図である。オーバーラップ処理を行なわずに、２０ｍ区間ごとに勾配を計算しているため、非連続な計測値が示されている。路面勾配算出装置１０は、移動体高精度速度算出部１１が算出する高精度なＮＥＤ座標系の速度（水平速度精度：０．１ｋｍ／ｈ３ｄｒｍｓ、垂直速度精度：０．２ｋｍ／ｈ３ｄｒｍｓ）に基づいて、勾配を算出しているため、精度がよい勾配演算値が得られる。また、走行速度が速ければ移動体高精度速度算出部１１によるＳＮがさらによくなるため、勾配精度はより向上する。

図９は、本発明の一実施形態に係る路面勾配算出装置１０がオーバーラップ処理を行った場合に算出した路面勾配を示す図である。路面勾配算出装置１０は、オーバーラップ処理により、図９のように、より滑らかで連続的な勾配計測値を算出し、出力することが可能である。

図１０は、本発明の一実施形態に係る路面勾配算出装置１０によって公道の路面勾配を算出した場合を示す図である。公道にて同じ場所を繰り返し走行した場合、路面勾配算出装置１０は、１回目と２回目と３回目とで、非常に再現性が高い路面勾配を算出している。

本実施形態によれば、路面勾配算出装置１０は、移動体に設置され、ＧＰＳ衛星からＧＰＳ搬送波を受信するＧＰＳ受信機１０１と、移動体の略重心位置に設置され、３次元の角速度と加速度とを求めるためのデータを出力するＩＭＵ１０２とを備える。そして、路面勾配算出装置１０は、ＧＰＳ受信機１０１が受信したＧＰＳ搬送波と、ＩＭＵ１０２が出力したデータとをカルマンフィルタにより処理することで速度を算出し、算出した速度の水平方向の速度を積算し、水平方向相対距離を算出し、算出した速度の垂直方向の速度を積算し、垂直方向相対距離を算出し、算出した水平方向相対距離が所定の水平方向距離になる区間ごとに、算出した水平方向相対距離と、算出した垂直方向相対距離とに基づいて、路面勾配を算出する。さらに、路面勾配算出装置１０は、オーバーラップ処理を行い、走行する移動体上でリアルタイムに路面勾配を算出し、連続的かつ滑らかに路面勾配を算出する。

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

１０路面勾配算出装置
１１移動体高精度速度算出部
１１１加速度計測部
１１２ストラップダウンナビゲータ部
１１３速度測定部
１１４良否係数算出部
１１５遅延処理部
１１６同期化処理部
１１７良否係数乗算部
１１８カルマンフィルタ部
１２水平方向相対距離算出部
１３垂直方向相対距離算出部
１４路面勾配算出部
１４１オーバーラップ間隔指定部
５１移動体
１０１ＧＰＳ受信機
１０２ＩＭＵ

Claims

移動体に設置され、ＧＰＳ衛星からＧＰＳ搬送波を受信するＧＰＳ受信機と、前記移動体の略重心位置に設置され、３次元の角速度と加速度とを求めるためのデータを出力するＩＭＵとを備え、前記移動体が移動する路面の勾配を算出する路面勾配算出装置であって、
前記ＧＰＳ受信機が受信したＧＰＳ搬送波と、前記ＩＭＵが出力したデータとをカルマンフィルタにより処理することで速度を算出する移動体高精度速度算出手段と、
前記移動体高精度速度算出手段により算出された速度の水平方向の速度を積算し、水平方向相対距離を算出する水平方向相対距離算出手段と、
前記移動体高精度速度算出手段により算出された速度の垂直方向の速度を積算し、垂直方向相対距離を算出する垂直方向相対距離算出手段と、
前記水平方向相対距離算出手段により算出された水平方向相対距離が所定の水平方向距離になる区間ごとに、算出された前記水平方向相対距離と、算出された前記垂直方向相対距離とに基づいて、路面勾配を算出する路面勾配算出手段と、
を備える路面勾配算出装置。
前記区間をオーバーラップさせるための間隔であって、前記所定の水平方向距離よりも小さい間隔を指定するオーバーラップ間隔指定手段をさらに備え、
前記路面勾配算出手段は、前記オーバーラップ間隔指定手段によって指定された間隔ごとに、前記水平方向相対距離及び前記垂直方向相対距離を積算し始め、前記水平方向相対距離が所定の水平方向距離になると、前記路面勾配を算出する、
請求項１に記載の路面勾配算出装置。
前記移動体高精度速度算出手段は、
前記ＩＭＵからデータを受信して、前記移動体の加速度及び角速度を計測する加速度計測手段と、
前記加速度計測手段によって計測された前記移動体の加速度及び角速度からストラップダウン演算により速度を算出し、算出した速度に基づいてリアルタイム補間速度を算出するストラップダウンナビゲータ手段と、
前記ＧＰＳ受信機が受信したＧＰＳ搬送波のドップラーシフト量から前記移動体のドップラー速度を測定する速度測定手段と、
前記速度測定手段によって測定された前記ドップラー速度から算出した加速度と、前記加速度計測手段によって計測された加速度との差分を算出し、算出した差分に基づいて係数を算出する良否係数算出手段と、
前記良否係数算出手段によって前記係数が算出されるための時間だけ、前記速度測定手段によって測定された前記ドップラー速度を演算に用いる時間を遅延させる遅延処理手段と、
前記遅延処理手段によって遅延させた前記ドップラー速度と、前記ストラップダウンナビゲータ手段からフィードバックさせた前記リアルタイム補間速度との同期化を行い、同期化された前記ドップラー速度と前記リアルタイム補間速度との誤差量を算出する同期化処理手段と、
前記同期化処理手段によって算出された前記誤差量に、前記良否係数算出手段によって算出された前記係数を乗算する良否係数乗算手段と、
前記良否係数乗算手段によって前記係数が乗算された誤差量から前記リアルタイム補間速度に対する調整量を、カルマンフィルタによって推定演算するカルマンフィルタ手段と、
を備え、
前記ストラップダウンナビゲータ手段は、前記ストラップダウン演算により算出された速度に、前記カルマンフィルタ手段によって推定演算された前記調整量を融合して前記リアルタイム補間速度を算出する、
請求項１又は２に記載の路面勾配算出装置。
移動体に設置され、ＧＰＳ衛星からＧＰＳ搬送波を受信するＧＰＳ受信機と、前記移動体の略重心位置に設置され、３次元の角速度と加速度とを求めるためのデータを出力するＩＭＵとを備え、前記移動体が移動する路面の勾配を算出する路面勾配算出装置が実行する方法であって、
前記ＧＰＳ受信機が受信したＧＰＳ搬送波と、前記ＩＭＵが出力したデータとをカルマンフィルタにより処理することで速度を算出する移動体高精度速度算出ステップと、
前記移動体高精度速度算出ステップにより算出された速度の水平方向の速度を積算し、水平方向相対距離を算出する水平方向相対距離算出ステップと、
前記移動体高精度速度算出ステップにより算出された速度の垂直方向の速度を積算し、垂直方向相対距離を算出する垂直方向相対距離算出ステップと、
前記水平方向相対距離算出ステップにより算出された水平方向相対距離が所定の水平方向距離になる区間ごとに、算出された前記水平方向相対距離と、算出された前記垂直方向相対距離とに基づいて、路面勾配を算出する路面勾配算出ステップと、
を備える方法。